3063 files changed, 207315 insertions, 127050 deletions

diff --git a/drivers/Kconfig b/drivers/Kconfig
index 0a0a90f52d2..0e87a34b647 100644
--- a/drivers/Kconfig
+++ b/drivers/Kconfig
@@ -132,6 +132,8 @@ source "drivers/staging/Kconfig"
 
 source "drivers/platform/Kconfig"
 
+source "drivers/soc/Kconfig"
+
 source "drivers/clk/Kconfig"
 
 source "drivers/hwspinlock/Kconfig"
diff --git a/drivers/Makefile b/drivers/Makefile
index d05d81b19b5..f98b50d8251 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -33,6 +33,9 @@ obj-y				+= amba/
 # really early.
 obj-$(CONFIG_DMADEVICES)	+= dma/
 
+# SOC specific infrastructure drivers.
+obj-y				+= soc/
+
 obj-$(CONFIG_VIRTIO)		+= virtio/
 obj-$(CONFIG_XEN)		+= xen/
 
@@ -80,7 +83,6 @@ obj-$(CONFIG_PCCARD)		+= pcmcia/
 obj-$(CONFIG_DIO)		+= dio/
 obj-$(CONFIG_SBUS)		+= sbus/
 obj-$(CONFIG_ZORRO)		+= zorro/
-obj-$(CONFIG_MAC)		+= macintosh/
 obj-$(CONFIG_ATA_OVER_ETH)	+= block/aoe/
 obj-$(CONFIG_PARIDE) 		+= block/paride/
 obj-$(CONFIG_TC)		+= tc/
@@ -119,7 +121,7 @@ obj-$(CONFIG_SGI_SN)		+= sn/
 obj-y				+= firmware/
 obj-$(CONFIG_CRYPTO)		+= crypto/
 obj-$(CONFIG_SUPERH)		+= sh/
-obj-$(CONFIG_ARCH_SHMOBILE_LEGACY)	+= sh/
+obj-$(CONFIG_ARCH_SHMOBILE)	+= sh/
 ifndef CONFIG_ARCH_USES_GETTIMEOFFSET
 obj-y				+= clocksource/
 endif
@@ -138,7 +140,6 @@ obj-y				+= clk/
 
 obj-$(CONFIG_MAILBOX)		+= mailbox/
 obj-$(CONFIG_HWSPINLOCK)	+= hwspinlock/
-obj-$(CONFIG_NFC)		+= nfc/
 obj-$(CONFIG_IOMMU_SUPPORT)	+= iommu/
 obj-$(CONFIG_REMOTEPROC)	+= remoteproc/
 obj-$(CONFIG_RPMSG)		+= rpmsg/
diff --git a/drivers/acpi/Kconfig b/drivers/acpi/Kconfig
index ab686b31010..a34a2284100 100644
--- a/drivers/acpi/Kconfig
+++ b/drivers/acpi/Kconfig
@@ -47,6 +47,23 @@ config ACPI_SLEEP
 	depends on SUSPEND || HIBERNATION
 	default y
 
+config ACPI_PROCFS_POWER
+	bool "Deprecated power /proc/acpi directories"
+	depends on PROC_FS
+	help
+	  For backwards compatibility, this option allows
+          deprecated power /proc/acpi/ directories to exist, even when
+          they have been replaced by functions in /sys.
+          The deprecated directories (and their replacements) include:
+	  /proc/acpi/battery/* (/sys/class/power_supply/*)
+	  /proc/acpi/ac_adapter/* (sys/class/power_supply/*)
+	  This option has no effect on /proc/acpi/ directories
+	  and functions, which do not yet exist in /sys
+	  This option, together with the proc directories, will be
+	  deleted in the future.
+
+	  Say N to delete power /proc/acpi/ directories that have moved to /sys/
+
 config ACPI_EC_DEBUGFS
 	tristate "EC read/write access through /sys/kernel/debug/ec"
 	default n
diff --git a/drivers/acpi/Makefile b/drivers/acpi/Makefile
index 0331f91d56e..ea55e0179f8 100644
--- a/drivers/acpi/Makefile
+++ b/drivers/acpi/Makefile
@@ -39,14 +39,16 @@ acpi-y				+= processor_core.o
 acpi-y				+= ec.o
 acpi-$(CONFIG_ACPI_DOCK)	+= dock.o
 acpi-y				+= pci_root.o pci_link.o pci_irq.o
-acpi-$(CONFIG_X86_INTEL_LPSS)	+= acpi_lpss.o
+acpi-y				+= acpi_lpss.o
 acpi-y				+= acpi_platform.o
+acpi-y				+= acpi_pnp.o
 acpi-y				+= power.o
 acpi-y				+= event.o
 acpi-y				+= sysfs.o
 acpi-$(CONFIG_X86)		+= acpi_cmos_rtc.o
 acpi-$(CONFIG_DEBUG_FS)		+= debugfs.o
 acpi-$(CONFIG_ACPI_NUMA)	+= numa.o
+acpi-$(CONFIG_ACPI_PROCFS_POWER) += cm_sbs.o
 ifdef CONFIG_ACPI_VIDEO
 acpi-y				+= video_detect.o
 endif
@@ -62,9 +64,9 @@ obj-$(CONFIG_ACPI_FAN)		+= fan.o
 obj-$(CONFIG_ACPI_VIDEO)	+= video.o
 obj-$(CONFIG_ACPI_PCI_SLOT)	+= pci_slot.o
 obj-$(CONFIG_ACPI_PROCESSOR)	+= processor.o
-obj-$(CONFIG_ACPI_CONTAINER)	+= container.o
+obj-y				+= container.o
 obj-$(CONFIG_ACPI_THERMAL)	+= thermal.o
-obj-$(CONFIG_ACPI_HOTPLUG_MEMORY) += acpi_memhotplug.o
+obj-y				+= acpi_memhotplug.o
 obj-$(CONFIG_ACPI_BATTERY)	+= battery.o
 obj-$(CONFIG_ACPI_SBS)		+= sbshc.o
 obj-$(CONFIG_ACPI_SBS)		+= sbs.o
diff --git a/drivers/acpi/ac.c b/drivers/acpi/ac.c
index 2c01c1da29c..c67f6f5ad61 100644
--- a/drivers/acpi/ac.c
+++ b/drivers/acpi/ac.c
@@ -52,11 +52,39 @@ MODULE_AUTHOR("Paul Diefenbaugh");
 MODULE_DESCRIPTION("ACPI AC Adapter Driver");
 MODULE_LICENSE("GPL");
 
+static int acpi_ac_add(struct acpi_device *device);
+static int acpi_ac_remove(struct acpi_device *device);
+static void acpi_ac_notify(struct acpi_device *device, u32 event);
+
+static const struct acpi_device_id ac_device_ids[] = {
+	{"ACPI0003", 0},
+	{"", 0},
+};
+MODULE_DEVICE_TABLE(acpi, ac_device_ids);
+
+#ifdef CONFIG_PM_SLEEP
+static int acpi_ac_resume(struct device *dev);
+#endif
+static SIMPLE_DEV_PM_OPS(acpi_ac_pm, NULL, acpi_ac_resume);
+
 static int ac_sleep_before_get_state_ms;
 
+static struct acpi_driver acpi_ac_driver = {
+	.name = "ac",
+	.class = ACPI_AC_CLASS,
+	.ids = ac_device_ids,
+	.flags = ACPI_DRIVER_ALL_NOTIFY_EVENTS,
+	.ops = {
+		.add = acpi_ac_add,
+		.remove = acpi_ac_remove,
+		.notify = acpi_ac_notify,
+		},
+	.drv.pm = &acpi_ac_pm,
+};
+
 struct acpi_ac {
 	struct power_supply charger;
-	struct platform_device *pdev;
+	struct acpi_device * device;
 	unsigned long long state;
 	struct notifier_block battery_nb;
 };
@@ -69,10 +97,12 @@ struct acpi_ac {
 
 static int acpi_ac_get_state(struct acpi_ac *ac)
 {
-	acpi_status status;
-	acpi_handle handle = ACPI_HANDLE(&ac->pdev->dev);
+	acpi_status status = AE_OK;
+
+	if (!ac)
+		return -EINVAL;
 
-	status = acpi_evaluate_integer(handle, "_PSR", NULL,
+	status = acpi_evaluate_integer(ac->device->handle, "_PSR", NULL,
 				       &ac->state);
 	if (ACPI_FAILURE(status)) {
 		ACPI_EXCEPTION((AE_INFO, status,
@@ -117,10 +147,9 @@ static enum power_supply_property ac_props[] = {
                                    Driver Model
    -------------------------------------------------------------------------- */
 
-static void acpi_ac_notify_handler(acpi_handle handle, u32 event, void *data)
+static void acpi_ac_notify(struct acpi_device *device, u32 event)
 {
-	struct acpi_ac *ac = data;
-	struct acpi_device *adev;
+	struct acpi_ac *ac = acpi_driver_data(device);
 
 	if (!ac)
 		return;
@@ -143,11 +172,10 @@ static void acpi_ac_notify_handler(acpi_handle handle, u32 event, void *data)
 			msleep(ac_sleep_before_get_state_ms);
 
 		acpi_ac_get_state(ac);
-		adev = ACPI_COMPANION(&ac->pdev->dev);
-		acpi_bus_generate_netlink_event(adev->pnp.device_class,
-						dev_name(&ac->pdev->dev),
-						event, (u32) ac->state);
-		acpi_notifier_call_chain(adev, event, (u32) ac->state);
+		acpi_bus_generate_netlink_event(device->pnp.device_class,
+						  dev_name(&device->dev), event,
+						  (u32) ac->state);
+		acpi_notifier_call_chain(device, event, (u32) ac->state);
 		kobject_uevent(&ac->charger.dev->kobj, KOBJ_CHANGE);
 	}
 
@@ -192,49 +220,39 @@ static struct dmi_system_id ac_dmi_table[] = {
 	{},
 };
 
-static int acpi_ac_probe(struct platform_device *pdev)
+static int acpi_ac_add(struct acpi_device *device)
 {
 	int result = 0;
 	struct acpi_ac *ac = NULL;
-	struct acpi_device *adev;
 
-	if (!pdev)
-		return -EINVAL;
 
-	adev = ACPI_COMPANION(&pdev->dev);
-	if (!adev)
-		return -ENODEV;
+	if (!device)
+		return -EINVAL;
 
 	ac = kzalloc(sizeof(struct acpi_ac), GFP_KERNEL);
 	if (!ac)
 		return -ENOMEM;
 
-	strcpy(acpi_device_name(adev), ACPI_AC_DEVICE_NAME);
-	strcpy(acpi_device_class(adev), ACPI_AC_CLASS);
-	ac->pdev = pdev;
-	platform_set_drvdata(pdev, ac);
+	ac->device = device;
+	strcpy(acpi_device_name(device), ACPI_AC_DEVICE_NAME);
+	strcpy(acpi_device_class(device), ACPI_AC_CLASS);
+	device->driver_data = ac;
 
 	result = acpi_ac_get_state(ac);
 	if (result)
 		goto end;
 
-	ac->charger.name = acpi_device_bid(adev);
+	ac->charger.name = acpi_device_bid(device);
 	ac->charger.type = POWER_SUPPLY_TYPE_MAINS;
 	ac->charger.properties = ac_props;
 	ac->charger.num_properties = ARRAY_SIZE(ac_props);
 	ac->charger.get_property = get_ac_property;
-	result = power_supply_register(&pdev->dev, &ac->charger);
+	result = power_supply_register(&ac->device->dev, &ac->charger);
 	if (result)
 		goto end;
 
-	result = acpi_install_notify_handler(ACPI_HANDLE(&pdev->dev),
-			ACPI_ALL_NOTIFY, acpi_ac_notify_handler, ac);
-	if (result) {
-		power_supply_unregister(&ac->charger);
-		goto end;
-	}
 	printk(KERN_INFO PREFIX "%s [%s] (%s)\n",
-	       acpi_device_name(adev), acpi_device_bid(adev),
+	       acpi_device_name(device), acpi_device_bid(device),
 	       ac->state ? "on-line" : "off-line");
 
 	ac->battery_nb.notifier_call = acpi_ac_battery_notify;
@@ -256,7 +274,7 @@ static int acpi_ac_resume(struct device *dev)
 	if (!dev)
 		return -EINVAL;
 
-	ac = platform_get_drvdata(to_platform_device(dev));
+	ac = acpi_driver_data(to_acpi_device(dev));
 	if (!ac)
 		return -EINVAL;
 
@@ -270,19 +288,17 @@ static int acpi_ac_resume(struct device *dev)
 #else
 #define acpi_ac_resume NULL
 #endif
-static SIMPLE_DEV_PM_OPS(acpi_ac_pm_ops, NULL, acpi_ac_resume);
 
-static int acpi_ac_remove(struct platform_device *pdev)
+static int acpi_ac_remove(struct acpi_device *device)
 {
-	struct acpi_ac *ac;
+	struct acpi_ac *ac = NULL;
+
 
-	if (!pdev)
+	if (!device || !acpi_driver_data(device))
 		return -EINVAL;
 
-	acpi_remove_notify_handler(ACPI_HANDLE(&pdev->dev),
-			ACPI_ALL_NOTIFY, acpi_ac_notify_handler);
+	ac = acpi_driver_data(device);
 
-	ac = platform_get_drvdata(pdev);
 	if (ac->charger.dev)
 		power_supply_unregister(&ac->charger);
 	unregister_acpi_notifier(&ac->battery_nb);
@@ -292,23 +308,6 @@ static int acpi_ac_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static const struct acpi_device_id acpi_ac_match[] = {
-	{ "ACPI0003", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(acpi, acpi_ac_match);
-
-static struct platform_driver acpi_ac_driver = {
-	.probe          = acpi_ac_probe,
-	.remove         = acpi_ac_remove,
-	.driver         = {
-		.name   = "acpi-ac",
-		.owner  = THIS_MODULE,
-		.pm     = &acpi_ac_pm_ops,
-		.acpi_match_table = ACPI_PTR(acpi_ac_match),
-	},
-};
-
 static int __init acpi_ac_init(void)
 {
 	int result;
@@ -316,7 +315,7 @@ static int __init acpi_ac_init(void)
 	if (acpi_disabled)
 		return -ENODEV;
 
-	result = platform_driver_register(&acpi_ac_driver);
+	result = acpi_bus_register_driver(&acpi_ac_driver);
 	if (result < 0)
 		return -ENODEV;
 
@@ -325,7 +324,7 @@ static int __init acpi_ac_init(void)
 
 static void __exit acpi_ac_exit(void)
 {
-	platform_driver_unregister(&acpi_ac_driver);
+	acpi_bus_unregister_driver(&acpi_ac_driver);
 }
 module_init(acpi_ac_init);
 module_exit(acpi_ac_exit);
diff --git a/drivers/acpi/acpi_cmos_rtc.c b/drivers/acpi/acpi_cmos_rtc.c
index 961b45d18a5..2da8660262e 100644
--- a/drivers/acpi/acpi_cmos_rtc.c
+++ b/drivers/acpi/acpi_cmos_rtc.c
@@ -68,7 +68,7 @@ static int acpi_install_cmos_rtc_space_handler(struct acpi_device *adev,
 		return -ENODEV;
 	}
 
-	return 0;
+	return 1;
 }
 
 static void acpi_remove_cmos_rtc_space_handler(struct acpi_device *adev)
diff --git a/drivers/acpi/acpi_extlog.c b/drivers/acpi/acpi_extlog.c
index c4a5d87ede7..185334114d7 100644
--- a/drivers/acpi/acpi_extlog.c
+++ b/drivers/acpi/acpi_extlog.c
@@ -220,13 +220,13 @@ static int __init extlog_init(void)
 		goto err;
 	}
 
-	extlog_l1_hdr = acpi_os_map_memory(l1_dirbase, l1_hdr_size);
+	extlog_l1_hdr = acpi_os_map_iomem(l1_dirbase, l1_hdr_size);
 	l1_head = (struct extlog_l1_head *)extlog_l1_hdr;
 	l1_size = l1_head->total_len;
 	l1_percpu_entry = l1_head->entries;
 	elog_base = l1_head->elog_base;
 	elog_size = l1_head->elog_len;
-	acpi_os_unmap_memory(extlog_l1_hdr, l1_hdr_size);
+	acpi_os_unmap_iomem(extlog_l1_hdr, l1_hdr_size);
 	release_mem_region(l1_dirbase, l1_hdr_size);
 
 	/* remap L1 header again based on completed information */
@@ -237,7 +237,7 @@ static int __init extlog_init(void)
 			(unsigned long long)l1_dirbase + l1_size);
 		goto err;
 	}
-	extlog_l1_addr = acpi_os_map_memory(l1_dirbase, l1_size);
+	extlog_l1_addr = acpi_os_map_iomem(l1_dirbase, l1_size);
 	l1_entry_base = (u64 *)((u8 *)extlog_l1_addr + l1_hdr_size);
 
 	/* remap elog table */
@@ -248,7 +248,7 @@ static int __init extlog_init(void)
 			(unsigned long long)elog_base + elog_size);
 		goto err_release_l1_dir;
 	}
-	elog_addr = acpi_os_map_memory(elog_base, elog_size);
+	elog_addr = acpi_os_map_iomem(elog_base, elog_size);
 
 	rc = -ENOMEM;
 	/* allocate buffer to save elog record */
@@ -270,11 +270,11 @@ static int __init extlog_init(void)
 
 err_release_elog:
 	if (elog_addr)
-		acpi_os_unmap_memory(elog_addr, elog_size);
+		acpi_os_unmap_iomem(elog_addr, elog_size);
 	release_mem_region(elog_base, elog_size);
 err_release_l1_dir:
 	if (extlog_l1_addr)
-		acpi_os_unmap_memory(extlog_l1_addr, l1_size);
+		acpi_os_unmap_iomem(extlog_l1_addr, l1_size);
 	release_mem_region(l1_dirbase, l1_size);
 err:
 	pr_warn(FW_BUG "Extended error log disabled because of problems parsing f/w tables\n");
@@ -287,9 +287,9 @@ static void __exit extlog_exit(void)
 	mce_unregister_decode_chain(&extlog_mce_dec);
 	((struct extlog_l1_head *)extlog_l1_addr)->flags &= ~FLAG_OS_OPTIN;
 	if (extlog_l1_addr)
-		acpi_os_unmap_memory(extlog_l1_addr, l1_size);
+		acpi_os_unmap_iomem(extlog_l1_addr, l1_size);
 	if (elog_addr)
-		acpi_os_unmap_memory(elog_addr, elog_size);
+		acpi_os_unmap_iomem(elog_addr, elog_size);
 	release_mem_region(elog_base, elog_size);
 	release_mem_region(l1_dirbase, l1_size);
 	kfree(elog_buf);
diff --git a/drivers/acpi/acpi_lpss.c b/drivers/acpi/acpi_lpss.c
index 69e29f409d4..63407d26488 100644
--- a/drivers/acpi/acpi_lpss.c
+++ b/drivers/acpi/acpi_lpss.c
@@ -19,15 +19,21 @@
 #include <linux/platform_device.h>
 #include <linux/platform_data/clk-lpss.h>
 #include <linux/pm_runtime.h>
+#include <linux/delay.h>
 
 #include "internal.h"
 
 ACPI_MODULE_NAME("acpi_lpss");
 
+#ifdef CONFIG_X86_INTEL_LPSS
+
+#define LPSS_ADDR(desc) ((unsigned long)&desc)
+
 #define LPSS_CLK_SIZE	0x04
 #define LPSS_LTR_SIZE	0x18
 
 /* Offsets relative to LPSS_PRIVATE_OFFSET */
+#define LPSS_CLK_DIVIDER_DEF_MASK	(BIT(1) | BIT(16))
 #define LPSS_GENERAL			0x08
 #define LPSS_GENERAL_LTR_MODE_SW	BIT(2)
 #define LPSS_GENERAL_UART_RTS_OVRD	BIT(3)
@@ -43,6 +49,8 @@ ACPI_MODULE_NAME("acpi_lpss");
 #define LPSS_TX_INT			0x20
 #define LPSS_TX_INT_MASK		BIT(1)
 
+#define LPSS_PRV_REG_COUNT		9
+
 struct lpss_shared_clock {
 	const char *name;
 	unsigned long rate;
@@ -57,7 +65,9 @@ struct lpss_device_desc {
 	bool ltr_required;
 	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);
 };
@@ -72,6 +82,7 @@ struct lpss_private_data {
 	resource_size_t mmio_size;
 	struct clk *clk;
 	const struct lpss_device_desc *dev_desc;
+	u32 prv_reg_ctx[LPSS_PRV_REG_COUNT];
 };
 
 static void lpss_uart_setup(struct lpss_private_data *pdata)
@@ -92,6 +103,14 @@ static struct lpss_device_desc lpt_dev_desc = {
 	.clk_required = true,
 	.prv_offset = 0x800,
 	.ltr_required = true,
+	.clk_divider = true,
+	.clk_gate = true,
+};
+
+static struct lpss_device_desc lpt_i2c_dev_desc = {
+	.clk_required = true,
+	.prv_offset = 0x800,
+	.ltr_required = true,
 	.clk_gate = true,
 };
 
@@ -99,6 +118,7 @@ static struct lpss_device_desc lpt_uart_dev_desc = {
 	.clk_required = true,
 	.prv_offset = 0x800,
 	.ltr_required = true,
+	.clk_divider = true,
 	.clk_gate = true,
 	.setup = lpss_uart_setup,
 };
@@ -116,32 +136,25 @@ static struct lpss_shared_clock pwm_clock = {
 
 static struct lpss_device_desc byt_pwm_dev_desc = {
 	.clk_required = true,
+	.save_ctx = true,
 	.shared_clock = &pwm_clock,
 };
 
-static struct lpss_shared_clock uart_clock = {
-	.name = "uart_clk",
-	.rate = 44236800,
-};
-
 static struct lpss_device_desc byt_uart_dev_desc = {
 	.clk_required = true,
 	.prv_offset = 0x800,
+	.clk_divider = true,
 	.clk_gate = true,
-	.shared_clock = &uart_clock,
+	.save_ctx = true,
 	.setup = lpss_uart_setup,
 };
 
-static struct lpss_shared_clock spi_clock = {
-	.name = "spi_clk",
-	.rate = 50000000,
-};
-
 static struct lpss_device_desc byt_spi_dev_desc = {
 	.clk_required = true,
 	.prv_offset = 0x400,
+	.clk_divider = true,
 	.clk_gate = true,
-	.shared_clock = &spi_clock,
+	.save_ctx = true,
 };
 
 static struct lpss_device_desc byt_sdio_dev_desc = {
@@ -156,43 +169,53 @@ static struct lpss_shared_clock i2c_clock = {
 static struct lpss_device_desc byt_i2c_dev_desc = {
 	.clk_required = true,
 	.prv_offset = 0x800,
+	.save_ctx = true,
 	.shared_clock = &i2c_clock,
 };
 
+#else
+
+#define LPSS_ADDR(desc) (0UL)
+
+#endif /* CONFIG_X86_INTEL_LPSS */
+
 static const struct acpi_device_id acpi_lpss_device_ids[] = {
 	/* Generic LPSS devices */
-	{ "INTL9C60", (unsigned long)&lpss_dma_desc },
+	{ "INTL9C60", LPSS_ADDR(lpss_dma_desc) },
 
 	/* Lynxpoint LPSS devices */
-	{ "INT33C0", (unsigned long)&lpt_dev_desc },
-	{ "INT33C1", (unsigned long)&lpt_dev_desc },
-	{ "INT33C2", (unsigned long)&lpt_dev_desc },
-	{ "INT33C3", (unsigned long)&lpt_dev_desc },
-	{ "INT33C4", (unsigned long)&lpt_uart_dev_desc },
-	{ "INT33C5", (unsigned long)&lpt_uart_dev_desc },
-	{ "INT33C6", (unsigned long)&lpt_sdio_dev_desc },
+	{ "INT33C0", LPSS_ADDR(lpt_dev_desc) },
+	{ "INT33C1", LPSS_ADDR(lpt_dev_desc) },
+	{ "INT33C2", LPSS_ADDR(lpt_i2c_dev_desc) },
+	{ "INT33C3", LPSS_ADDR(lpt_i2c_dev_desc) },
+	{ "INT33C4", LPSS_ADDR(lpt_uart_dev_desc) },
+	{ "INT33C5", LPSS_ADDR(lpt_uart_dev_desc) },
+	{ "INT33C6", LPSS_ADDR(lpt_sdio_dev_desc) },
 	{ "INT33C7", },
 
 	/* BayTrail LPSS devices */
-	{ "80860F09", (unsigned long)&byt_pwm_dev_desc },
-	{ "80860F0A", (unsigned long)&byt_uart_dev_desc },
-	{ "80860F0E", (unsigned long)&byt_spi_dev_desc },
-	{ "80860F14", (unsigned long)&byt_sdio_dev_desc },
-	{ "80860F41", (unsigned long)&byt_i2c_dev_desc },
+	{ "80860F09", LPSS_ADDR(byt_pwm_dev_desc) },
+	{ "80860F0A", LPSS_ADDR(byt_uart_dev_desc) },
+	{ "80860F0E", LPSS_ADDR(byt_spi_dev_desc) },
+	{ "80860F14", LPSS_ADDR(byt_sdio_dev_desc) },
+	{ "80860F41", LPSS_ADDR(byt_i2c_dev_desc) },
 	{ "INT33B2", },
-
-	{ "INT3430", (unsigned long)&lpt_dev_desc },
-	{ "INT3431", (unsigned long)&lpt_dev_desc },
-	{ "INT3432", (unsigned long)&lpt_dev_desc },
-	{ "INT3433", (unsigned long)&lpt_dev_desc },
-	{ "INT3434", (unsigned long)&lpt_uart_dev_desc },
-	{ "INT3435", (unsigned long)&lpt_uart_dev_desc },
-	{ "INT3436", (unsigned long)&lpt_sdio_dev_desc },
+	{ "INT33FC", },
+
+	{ "INT3430", LPSS_ADDR(lpt_dev_desc) },
+	{ "INT3431", LPSS_ADDR(lpt_dev_desc) },
+	{ "INT3432", LPSS_ADDR(lpt_i2c_dev_desc) },
+	{ "INT3433", LPSS_ADDR(lpt_i2c_dev_desc) },
+	{ "INT3434", LPSS_ADDR(lpt_uart_dev_desc) },
+	{ "INT3435", LPSS_ADDR(lpt_uart_dev_desc) },
+	{ "INT3436", LPSS_ADDR(lpt_sdio_dev_desc) },
 	{ "INT3437", },
 
 	{ }
 };
 
+#ifdef CONFIG_X86_INTEL_LPSS
+
 static int is_memory(struct acpi_resource *res, void *not_used)
 {
 	struct resource r;
@@ -212,9 +235,11 @@ static int register_device_clock(struct acpi_device *adev,
 {
 	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;
-	const char *parent;
+	const char *parent, *clk_name;
+	void __iomem *prv_base;
 
 	if (!lpss_clk_dev)
 		lpt_register_clock_device();
@@ -225,7 +250,7 @@ static int register_device_clock(struct acpi_device *adev,
 
 	if (dev_desc->clkdev_name) {
 		clk_register_clkdev(clk_data->clk, dev_desc->clkdev_name,
-				    dev_name(&adev->dev));
+				    devname);
 		return 0;
 	}
 
@@ -234,6 +259,7 @@ static int register_device_clock(struct acpi_device *adev,
 		return -ENODATA;
 
 	parent = clk_data->name;
+	prv_base = pdata->mmio_base + dev_desc->prv_offset;
 
 	if (shared_clock) {
 		clk = shared_clock->clk;
@@ -247,16 +273,41 @@ static int register_device_clock(struct acpi_device *adev,
 	}
 
 	if (dev_desc->clk_gate) {
-		clk = clk_register_gate(NULL, dev_name(&adev->dev), parent, 0,
-					pdata->mmio_base + dev_desc->prv_offset,
-					0, 0, NULL);
-		pdata->clk = clk;
+		clk = clk_register_gate(NULL, devname, parent, 0,
+					prv_base, 0, 0, NULL);
+		parent = devname;
+	}
+
+	if (dev_desc->clk_divider) {
+		/* Prevent division by zero */
+		if (!readl(prv_base))
+			writel(LPSS_CLK_DIVIDER_DEF_MASK, prv_base);
+
+		clk_name = kasprintf(GFP_KERNEL, "%s-div", devname);
+		if (!clk_name)
+			return -ENOMEM;
+		clk = clk_register_fractional_divider(NULL, clk_name, parent,
+						      0, prv_base,
+						      1, 15, 16, 15, 0, NULL);
+		parent = clk_name;
+
+		clk_name = kasprintf(GFP_KERNEL, "%s-update", devname);
+		if (!clk_name) {
+			kfree(parent);
+			return -ENOMEM;
+		}
+		clk = clk_register_gate(NULL, clk_name, parent,
+					CLK_SET_RATE_PARENT | CLK_SET_RATE_GATE,
+					prv_base, 31, 0, NULL);
+		kfree(parent);
+		kfree(clk_name);
 	}
 
 	if (IS_ERR(clk))
 		return PTR_ERR(clk);
 
-	clk_register_clkdev(clk, NULL, dev_name(&adev->dev));
+	pdata->clk = clk;
+	clk_register_clkdev(clk, NULL, devname);
 	return 0;
 }
 
@@ -267,12 +318,14 @@ static int acpi_lpss_create_device(struct acpi_device *adev,
 	struct lpss_private_data *pdata;
 	struct resource_list_entry *rentry;
 	struct list_head resource_list;
+	struct platform_device *pdev;
 	int ret;
 
 	dev_desc = (struct lpss_device_desc *)id->driver_data;
-	if (!dev_desc)
-		return acpi_create_platform_device(adev, id);
-
+	if (!dev_desc) {
+		pdev = acpi_create_platform_device(adev);
+		return IS_ERR_OR_NULL(pdev) ? PTR_ERR(pdev) : 1;
+	}
 	pdata = kzalloc(sizeof(*pdata), GFP_KERNEL);
 	if (!pdata)
 		return -ENOMEM;
@@ -322,10 +375,13 @@ static int acpi_lpss_create_device(struct acpi_device *adev,
 		dev_desc->setup(pdata);
 
 	adev->driver_data = pdata;
-	ret = acpi_create_platform_device(adev, id);
-	if (ret > 0)
-		return ret;
+	pdev = acpi_create_platform_device(adev);
+	if (!IS_ERR_OR_NULL(pdev)) {
+		device_enable_async_suspend(&pdev->dev);
+		return 1;
+	}
 
+	ret = PTR_ERR(pdev);
 	adev->driver_data = NULL;
 
  err_out:
@@ -449,6 +505,126 @@ static void acpi_lpss_set_ltr(struct device *dev, s32 val)
 	}
 }
 
+#ifdef CONFIG_PM
+/**
+ * acpi_lpss_save_ctx() - Save the private registers of LPSS device
+ * @dev: LPSS device
+ *
+ * Most LPSS devices have private registers which may loose their context when
+ * the device is powered down. acpi_lpss_save_ctx() saves those registers into
+ * prv_reg_ctx array.
+ */
+static void acpi_lpss_save_ctx(struct device *dev)
+{
+	struct lpss_private_data *pdata = acpi_driver_data(ACPI_COMPANION(dev));
+	unsigned int i;
+
+	for (i = 0; i < LPSS_PRV_REG_COUNT; i++) {
+		unsigned long offset = i * sizeof(u32);
+
+		pdata->prv_reg_ctx[i] = __lpss_reg_read(pdata, offset);
+		dev_dbg(dev, "saving 0x%08x from LPSS reg at offset 0x%02lx\n",
+			pdata->prv_reg_ctx[i], offset);
+	}
+}
+
+/**
+ * acpi_lpss_restore_ctx() - Restore the private registers of LPSS device
+ * @dev: LPSS device
+ *
+ * Restores the registers that were previously stored with acpi_lpss_save_ctx().
+ */
+static void acpi_lpss_restore_ctx(struct device *dev)
+{
+	struct lpss_private_data *pdata = acpi_driver_data(ACPI_COMPANION(dev));
+	unsigned int i;
+
+	/*
+	 * The following delay is needed or the subsequent write operations may
+	 * fail. The LPSS devices are actually PCI devices and the PCI spec
+	 * expects 10ms delay before the device can be accessed after D3 to D0
+	 * transition.
+	 */
+	msleep(10);
+
+	for (i = 0; i < LPSS_PRV_REG_COUNT; i++) {
+		unsigned long offset = i * sizeof(u32);
+
+		__lpss_reg_write(pdata->prv_reg_ctx[i], pdata, offset);
+		dev_dbg(dev, "restoring 0x%08x to LPSS reg at offset 0x%02lx\n",
+			pdata->prv_reg_ctx[i], offset);
+	}
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int acpi_lpss_suspend_late(struct device *dev)
+{
+	int ret = pm_generic_suspend_late(dev);
+
+	if (ret)
+		return ret;
+
+	acpi_lpss_save_ctx(dev);
+	return acpi_dev_suspend_late(dev);
+}
+
+static int acpi_lpss_restore_early(struct device *dev)
+{
+	int ret = acpi_dev_resume_early(dev);
+
+	if (ret)
+		return ret;
+
+	acpi_lpss_restore_ctx(dev);
+	return pm_generic_resume_early(dev);
+}
+#endif /* CONFIG_PM_SLEEP */
+
+#ifdef CONFIG_PM_RUNTIME
+static int acpi_lpss_runtime_suspend(struct device *dev)
+{
+	int ret = pm_generic_runtime_suspend(dev);
+
+	if (ret)
+		return ret;
+
+	acpi_lpss_save_ctx(dev);
+	return acpi_dev_runtime_suspend(dev);
+}
+
+static int acpi_lpss_runtime_resume(struct device *dev)
+{
+	int ret = acpi_dev_runtime_resume(dev);
+
+	if (ret)
+		return ret;
+
+	acpi_lpss_restore_ctx(dev);
+	return pm_generic_runtime_resume(dev);
+}
+#endif /* CONFIG_PM_RUNTIME */
+#endif /* CONFIG_PM */
+
+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,
+		.freeze = acpi_subsys_freeze,
+		.poweroff = acpi_subsys_suspend,
+		.poweroff_late = acpi_subsys_suspend_late,
+#endif
+#ifdef CONFIG_PM_RUNTIME
+		.runtime_suspend = acpi_lpss_runtime_suspend,
+		.runtime_resume = acpi_lpss_runtime_resume,
+#endif
+	},
+};
+
 static int acpi_lpss_platform_notify(struct notifier_block *nb,
 				     unsigned long action, void *data)
 {
@@ -456,7 +632,6 @@ static int acpi_lpss_platform_notify(struct notifier_block *nb,
 	struct lpss_private_data *pdata;
 	struct acpi_device *adev;
 	const struct acpi_device_id *id;
-	int ret = 0;
 
 	id = acpi_match_device(acpi_lpss_device_ids, &pdev->dev);
 	if (!id || !id->driver_data)
@@ -466,7 +641,7 @@ static int acpi_lpss_platform_notify(struct notifier_block *nb,
 		return 0;
 
 	pdata = acpi_driver_data(adev);
-	if (!pdata || !pdata->mmio_base || !pdata->dev_desc->ltr_required)
+	if (!pdata || !pdata->mmio_base)
 		return 0;
 
 	if (pdata->mmio_size < pdata->dev_desc->prv_offset + LPSS_LTR_SIZE) {
@@ -474,12 +649,27 @@ static int acpi_lpss_platform_notify(struct notifier_block *nb,
 		return 0;
 	}
 
-	if (action == BUS_NOTIFY_ADD_DEVICE)
-		ret = sysfs_create_group(&pdev->dev.kobj, &lpss_attr_group);
-	else if (action == BUS_NOTIFY_DEL_DEVICE)
-		sysfs_remove_group(&pdev->dev.kobj, &lpss_attr_group);
+	switch (action) {
+	case BUS_NOTIFY_BOUND_DRIVER:
+		if (pdata->dev_desc->save_ctx)
+			pdev->dev.pm_domain = &acpi_lpss_pm_domain;
+		break;
+	case BUS_NOTIFY_UNBOUND_DRIVER:
+		if (pdata->dev_desc->save_ctx)
+			pdev->dev.pm_domain = NULL;
+		break;
+	case BUS_NOTIFY_ADD_DEVICE:
+		if (pdata->dev_desc->ltr_required)
+			return sysfs_create_group(&pdev->dev.kobj,
+						  &lpss_attr_group);
+	case BUS_NOTIFY_DEL_DEVICE:
+		if (pdata->dev_desc->ltr_required)
+			sysfs_remove_group(&pdev->dev.kobj, &lpss_attr_group);
+	default:
+		break;
+	}
 
-	return ret;
+	return 0;
 }
 
 static struct notifier_block acpi_lpss_nb = {
@@ -518,3 +708,16 @@ void __init acpi_lpss_init(void)
 		acpi_scan_add_handler(&lpss_handler);
 	}
 }
+
+#else
+
+static struct acpi_scan_handler lpss_handler = {
+	.ids = acpi_lpss_device_ids,
+};
+
+void __init acpi_lpss_init(void)
+{
+	acpi_scan_add_handler(&lpss_handler);
+}
+
+#endif /* CONFIG_X86_INTEL_LPSS */
diff --git a/drivers/acpi/acpi_memhotplug.c b/drivers/acpi/acpi_memhotplug.c
index b67be85ff0f..23e2319ead4 100644
--- a/drivers/acpi/acpi_memhotplug.c
+++ b/drivers/acpi/acpi_memhotplug.c
@@ -44,6 +44,13 @@
 
 ACPI_MODULE_NAME("acpi_memhotplug");
 
+static const struct acpi_device_id memory_device_ids[] = {
+	{ACPI_MEMORY_DEVICE_HID, 0},
+	{"", 0},
+};
+
+#ifdef CONFIG_ACPI_HOTPLUG_MEMORY
+
 /* Memory Device States */
 #define MEMORY_INVALID_STATE	0
 #define MEMORY_POWER_ON_STATE	1
@@ -53,11 +60,6 @@ static int acpi_memory_device_add(struct acpi_device *device,
 				  const struct acpi_device_id *not_used);
 static void acpi_memory_device_remove(struct acpi_device *device);
 
-static const struct acpi_device_id memory_device_ids[] = {
-	{ACPI_MEMORY_DEVICE_HID, 0},
-	{"", 0},
-};
-
 static struct acpi_scan_handler memory_device_handler = {
 	.ids = memory_device_ids,
 	.attach = acpi_memory_device_add,
@@ -364,9 +366,11 @@ static bool __initdata acpi_no_memhotplug;
 
 void __init acpi_memory_hotplug_init(void)
 {
-	if (acpi_no_memhotplug)
+	if (acpi_no_memhotplug) {
+		memory_device_handler.attach = NULL;
+		acpi_scan_add_handler(&memory_device_handler);
 		return;
-
+	}
 	acpi_scan_add_handler_with_hotplug(&memory_device_handler, "memory");
 }
 
@@ -376,3 +380,16 @@ static int __init disable_acpi_memory_hotplug(char *str)
 	return 1;
 }
 __setup("acpi_no_memhotplug", disable_acpi_memory_hotplug);
+
+#else
+
+static struct acpi_scan_handler memory_device_handler = {
+	.ids = memory_device_ids,
+};
+
+void __init acpi_memory_hotplug_init(void)
+{
+	acpi_scan_add_handler(&memory_device_handler);
+}
+
+#endif /* CONFIG_ACPI_HOTPLUG_MEMORY */
diff --git a/drivers/acpi/acpi_pad.c b/drivers/acpi/acpi_pad.c
index 37d73024b82..f148a0580e0 100644
--- a/drivers/acpi/acpi_pad.c
+++ b/drivers/acpi/acpi_pad.c
@@ -156,12 +156,13 @@ static int power_saving_thread(void *data)
 
 	while (!kthread_should_stop()) {
 		int cpu;
-		u64 expire_time;
+		unsigned long expire_time;
 
 		try_to_freeze();
 
 		/* round robin to cpus */
-		if (last_jiffies + round_robin_time * HZ < jiffies) {
+		expire_time = last_jiffies + round_robin_time * HZ;
+		if (time_before(expire_time, jiffies)) {
 			last_jiffies = jiffies;
 			round_robin_cpu(tsk_index);
 		}
@@ -200,7 +201,7 @@ static int power_saving_thread(void *data)
 					CLOCK_EVT_NOTIFY_BROADCAST_EXIT, &cpu);
 			local_irq_enable();
 
-			if (jiffies > expire_time) {
+			if (time_before(expire_time, jiffies)) {
 				do_sleep = 1;
 				break;
 			}
@@ -215,8 +216,15 @@ static int power_saving_thread(void *data)
 		 * borrow CPU time from this CPU and cause RT task use > 95%
 		 * CPU time. To make 'avoid starvation' work, takes a nap here.
 		 */
-		if (do_sleep)
+		if (unlikely(do_sleep))
 			schedule_timeout_killable(HZ * idle_pct / 100);
+
+		/* If an external event has set the need_resched flag, then
+		 * we need to deal with it, or this loop will continue to
+		 * spin without calling __mwait().
+		 */
+		if (unlikely(need_resched()))
+			schedule();
 	}
 
 	exit_round_robin(tsk_index);
diff --git a/drivers/acpi/acpi_platform.c b/drivers/acpi/acpi_platform.c
index dbfe49e5fd6..2bf9082f752 100644
--- a/drivers/acpi/acpi_platform.c
+++ b/drivers/acpi/acpi_platform.c
@@ -22,28 +22,16 @@
 
 ACPI_MODULE_NAME("platform");
 
-/*
- * The following ACPI IDs are known to be suitable for representing as
- * platform devices.
- */
-static const struct acpi_device_id acpi_platform_device_ids[] = {
-
-	{ "PNP0D40" },
-	{ "ACPI0003" },
-	{ "VPC2004" },
-	{ "BCM4752" },
-
-	/* Intel Smart Sound Technology */
-	{ "INT33C8" },
-	{ "80860F28" },
-
-	{ }
+static const struct acpi_device_id forbidden_id_list[] = {
+	{"PNP0000", 0},	/* PIC */
+	{"PNP0100", 0},	/* Timer */
+	{"PNP0200", 0},	/* AT DMA Controller */
+	{"", 0},
 };
 
 /**
  * acpi_create_platform_device - Create platform device for ACPI device node
  * @adev: ACPI device node to create a platform device for.
- * @id: ACPI device ID used to match @adev.
  *
  * Check if the given @adev can be represented as a platform device and, if
  * that's the case, create and register a platform device, populate its common
@@ -51,8 +39,7 @@ static const struct acpi_device_id acpi_platform_device_ids[] = {
  *
  * Name of the platform device will be the same as @adev's.
  */
-int acpi_create_platform_device(struct acpi_device *adev,
-				const struct acpi_device_id *id)
+struct platform_device *acpi_create_platform_device(struct acpi_device *adev)
 {
 	struct platform_device *pdev = NULL;
 	struct acpi_device *acpi_parent;
@@ -64,19 +51,22 @@ int acpi_create_platform_device(struct acpi_device *adev,
 
 	/* If the ACPI node already has a physical device attached, skip it. */
 	if (adev->physical_node_count)
-		return 0;
+		return NULL;
+
+	if (!acpi_match_device_ids(adev, forbidden_id_list))
+		return ERR_PTR(-EINVAL);
 
 	INIT_LIST_HEAD(&resource_list);
 	count = acpi_dev_get_resources(adev, &resource_list, NULL, NULL);
 	if (count < 0) {
-		return 0;
+		return NULL;
 	} else if (count > 0) {
 		resources = kmalloc(count * sizeof(struct resource),
 				    GFP_KERNEL);
 		if (!resources) {
 			dev_err(&adev->dev, "No memory for resources\n");
 			acpi_dev_free_resource_list(&resource_list);
-			return -ENOMEM;
+			return ERR_PTR(-ENOMEM);
 		}
 		count = 0;
 		list_for_each_entry(rentry, &resource_list, node)
@@ -113,25 +103,13 @@ int acpi_create_platform_device(struct acpi_device *adev,
 	pdevinfo.num_res = count;
 	pdevinfo.acpi_node.companion = adev;
 	pdev = platform_device_register_full(&pdevinfo);
-	if (IS_ERR(pdev)) {
+	if (IS_ERR(pdev))
 		dev_err(&adev->dev, "platform device creation failed: %ld\n",
 			PTR_ERR(pdev));
-		pdev = NULL;
-	} else {
+	else
 		dev_dbg(&adev->dev, "created platform device %s\n",
 			dev_name(&pdev->dev));
-	}
 
 	kfree(resources);
-	return 1;
-}
-
-static struct acpi_scan_handler platform_handler = {
-	.ids = acpi_platform_device_ids,
-	.attach = acpi_create_platform_device,
-};
-
-void __init acpi_platform_init(void)
-{
-	acpi_scan_add_handler(&platform_handler);
+	return pdev;
 }
diff --git a/drivers/acpi/acpi_pnp.c b/drivers/acpi/acpi_pnp.c
new file mode 100644
index 00000000000..6703c1fd993
--- /dev/null
+++ b/drivers/acpi/acpi_pnp.c
@@ -0,0 +1,395 @@
+/*
+ * ACPI support for PNP bus type
+ *
+ * Copyright (C) 2014, Intel Corporation
+ * Authors: Zhang Rui <rui.zhang@intel.com>
+ *          Rafael J. Wysocki <rafael.j.wysocki@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>
+
+static const struct acpi_device_id acpi_pnp_device_ids[] = {
+	/* pata_isapnp */
+	{"PNP0600"},		/* Generic ESDI/IDE/ATA compatible hard disk controller */
+	/* floppy */
+	{"PNP0700"},
+	/* ipmi_si */
+	{"IPI0001"},
+	/* tpm_inf_pnp */
+	{"IFX0101"},		/* Infineon TPMs */
+	{"IFX0102"},		/* Infineon TPMs */
+	/*tpm_tis */
+	{"PNP0C31"},		/* TPM */
+	{"ATM1200"},		/* Atmel */
+	{"IFX0102"},		/* Infineon */
+	{"BCM0101"},		/* Broadcom */
+	{"BCM0102"},		/* Broadcom */
+	{"NSC1200"},		/* National */
+	{"ICO0102"},		/* Intel */
+	/* ide   */
+	{"PNP0600"},		/* Generic ESDI/IDE/ATA compatible hard disk controller */
+	/* ns558 */
+	{"ASB16fd"},		/* AdLib NSC16 */
+	{"AZT3001"},		/* AZT1008 */
+	{"CDC0001"},		/* Opl3-SAx */
+	{"CSC0001"},		/* CS4232 */
+	{"CSC000f"},		/* CS4236 */
+	{"CSC0101"},		/* CS4327 */
+	{"CTL7001"},		/* SB16 */
+	{"CTL7002"},		/* AWE64 */
+	{"CTL7005"},		/* Vibra16 */
+	{"ENS2020"},		/* SoundscapeVIVO */
+	{"ESS0001"},		/* ES1869 */
+	{"ESS0005"},		/* ES1878 */
+	{"ESS6880"},		/* ES688 */
+	{"IBM0012"},		/* CS4232 */
+	{"OPT0001"},		/* OPTi Audio16 */
+	{"YMH0006"},		/* Opl3-SA */
+	{"YMH0022"},		/* Opl3-SAx */
+	{"PNPb02f"},		/* Generic */
+	/* i8042 kbd */
+	{"PNP0300"},
+	{"PNP0301"},
+	{"PNP0302"},
+	{"PNP0303"},
+	{"PNP0304"},
+	{"PNP0305"},
+	{"PNP0306"},
+	{"PNP0309"},
+	{"PNP030a"},
+	{"PNP030b"},
+	{"PNP0320"},
+	{"PNP0343"},
+	{"PNP0344"},
+	{"PNP0345"},
+	{"CPQA0D7"},
+	/* i8042 aux */
+	{"AUI0200"},
+	{"FJC6000"},
+	{"FJC6001"},
+	{"PNP0f03"},
+	{"PNP0f0b"},
+	{"PNP0f0e"},
+	{"PNP0f12"},
+	{"PNP0f13"},
+	{"PNP0f19"},
+	{"PNP0f1c"},
+	{"SYN0801"},
+	/* fcpnp */
+	{"AVM0900"},
+	/* radio-cadet */
+	{"MSM0c24"},		/* ADS Cadet AM/FM Radio Card */
+	/* radio-gemtek */
+	{"ADS7183"},		/* AOpen FX-3D/Pro Radio */
+	/* radio-sf16fmr2 */
+	{"MFRad13"},		/* tuner subdevice of SF16-FMD2 */
+	/* ene_ir */
+	{"ENE0100"},
+	{"ENE0200"},
+	{"ENE0201"},
+	{"ENE0202"},
+	/* fintek-cir */
+	{"FIT0002"},		/* CIR */
+	/* ite-cir */
+	{"ITE8704"},		/* Default model */
+	{"ITE8713"},		/* CIR found in EEEBox 1501U */
+	{"ITE8708"},		/* Bridged IT8512 */
+	{"ITE8709"},		/* SRAM-Bridged IT8512 */
+	/* nuvoton-cir */
+	{"WEC0530"},		/* CIR */
+	{"NTN0530"},		/* CIR for new chip's pnp id */
+	/* Winbond CIR */
+	{"WEC1022"},
+	/* wbsd */
+	{"WEC0517"},
+	{"WEC0518"},
+	/* Winbond CIR */
+	{"TCM5090"},		/* 3Com Etherlink III (TP) */
+	{"TCM5091"},		/* 3Com Etherlink III */
+	{"TCM5094"},		/* 3Com Etherlink III (combo) */
+	{"TCM5095"},		/* 3Com Etherlink III (TPO) */
+	{"TCM5098"},		/* 3Com Etherlink III (TPC) */
+	{"PNP80f7"},		/* 3Com Etherlink III compatible */
+	{"PNP80f8"},		/* 3Com Etherlink III compatible */
+	/* nsc-ircc */
+	{"NSC6001"},
+	{"HWPC224"},
+	{"IBM0071"},
+	/* smsc-ircc2 */
+	{"SMCf010"},
+	/* sb1000 */
+	{"GIC1000"},
+	/* parport_pc */
+	{"PNP0400"},		/* Standard LPT Printer Port */
+	{"PNP0401"},		/* ECP Printer Port */
+	/* apple-gmux */
+	{"APP000B"},
+	/* fujitsu-laptop.c */
+	{"FUJ02bf"},
+	{"FUJ02B1"},
+	{"FUJ02E3"},
+	/* system */
+	{"PNP0c02"},		/* General ID for reserving resources */
+	{"PNP0c01"},		/* memory controller */
+	/* rtc_cmos */
+	{"PNP0b00"},
+	{"PNP0b01"},
+	{"PNP0b02"},
+	/* c6xdigio */
+	{"PNP0400"},		/* Standard LPT Printer Port */
+	{"PNP0401"},		/* ECP Printer Port */
+	/* ni_atmio.c */
+	{"NIC1900"},
+	{"NIC2400"},
+	{"NIC2500"},
+	{"NIC2600"},
+	{"NIC2700"},
+	/* serial */
+	{"AAC000F"},		/* Archtek America Corp. Archtek SmartLink Modem 3334BT Plug & Play */
+	{"ADC0001"},		/* Anchor Datacomm BV. SXPro 144 External Data Fax Modem Plug & Play */
+	{"ADC0002"},		/* SXPro 288 External Data Fax Modem Plug & Play */
+	{"AEI0250"},		/* PROLiNK 1456VH ISA PnP K56flex Fax Modem */
+	{"AEI1240"},		/* Actiontec ISA PNP 56K X2 Fax Modem */
+	{"AKY1021"},		/* Rockwell 56K ACF II Fax+Data+Voice Modem */
+	{"AZT4001"},		/* AZT3005 PnP SOUND DEVICE */
+	{"BDP3336"},		/* Best Data Products Inc. Smart One 336F PnP Modem */
+	{"BRI0A49"},		/* Boca Complete Ofc Communicator 14.4 Data-FAX */
+	{"BRI1400"},		/* Boca Research 33,600 ACF Modem */
+	{"BRI3400"},		/* Boca 33.6 Kbps Internal FD34FSVD */
+	{"BRI0A49"},		/* Boca 33.6 Kbps Internal FD34FSVD */
+	{"BDP3336"},		/* Best Data Products Inc. Smart One 336F PnP Modem */
+	{"CPI4050"},		/* Computer Peripherals Inc. EuroViVa CommCenter-33.6 SP PnP */
+	{"CTL3001"},		/* Creative Labs Phone Blaster 28.8 DSVD PnP Voice */
+	{"CTL3011"},		/* Creative Labs Modem Blaster 28.8 DSVD PnP Voice */
+	{"DAV0336"},		/* Davicom ISA 33.6K Modem */
+	{"DMB1032"},		/* Creative Modem Blaster Flash56 DI5601-1 */
+	{"DMB2001"},		/* Creative Modem Blaster V.90 DI5660 */
+	{"ETT0002"},		/* E-Tech CyberBULLET PC56RVP */
+	{"FUJ0202"},		/* Fujitsu 33600 PnP-I2 R Plug & Play */
+	{"FUJ0205"},		/* Fujitsu FMV-FX431 Plug & Play */
+	{"FUJ0206"},		/* Fujitsu 33600 PnP-I4 R Plug & Play */
+	{"FUJ0209"},		/* Fujitsu Fax Voice 33600 PNP-I5 R Plug & Play */
+	{"GVC000F"},		/* Archtek SmartLink Modem 3334BT Plug & Play */
+	{"GVC0303"},		/* Archtek SmartLink Modem 3334BRV 33.6K Data Fax Voice */
+	{"HAY0001"},		/* Hayes Optima 288 V.34-V.FC + FAX + Voice Plug & Play */
+	{"HAY000C"},		/* Hayes Optima 336 V.34 + FAX + Voice PnP */
+	{"HAY000D"},		/* Hayes Optima 336B V.34 + FAX + Voice PnP */
+	{"HAY5670"},		/* Hayes Accura 56K Ext Fax Modem PnP */
+	{"HAY5674"},		/* Hayes Accura 56K Ext Fax Modem PnP */
+	{"HAY5675"},		/* Hayes Accura 56K Fax Modem PnP */
+	{"HAYF000"},		/* Hayes 288, V.34 + FAX */
+	{"HAYF001"},		/* Hayes Optima 288 V.34 + FAX + Voice, Plug & Play */
+	{"IBM0033"},		/* IBM Thinkpad 701 Internal Modem Voice */
+	{"PNP4972"},		/* Intermec CV60 touchscreen port */
+	{"IXDC801"},		/* Intertex 28k8 33k6 Voice EXT PnP */
+	{"IXDC901"},		/* Intertex 33k6 56k Voice EXT PnP */
+	{"IXDD801"},		/* Intertex 28k8 33k6 Voice SP EXT PnP */
+	{"IXDD901"},		/* Intertex 33k6 56k Voice SP EXT PnP */
+	{"IXDF401"},		/* Intertex 28k8 33k6 Voice SP INT PnP */
+	{"IXDF801"},		/* Intertex 28k8 33k6 Voice SP EXT PnP */
+	{"IXDF901"},		/* Intertex 33k6 56k Voice SP EXT PnP */
+	{"KOR4522"},		/* KORTEX 28800 Externe PnP */
+	{"KORF661"},		/* KXPro 33.6 Vocal ASVD PnP */
+	{"LAS4040"},		/* LASAT Internet 33600 PnP */
+	{"LAS4540"},		/* Lasat Safire 560 PnP */
+	{"LAS5440"},		/* Lasat Safire 336  PnP */
+	{"MNP0281"},		/* Microcom TravelPorte FAST V.34 Plug & Play */
+	{"MNP0336"},		/* Microcom DeskPorte V.34 FAST or FAST+ Plug & Play */
+	{"MNP0339"},		/* Microcom DeskPorte FAST EP 28.8 Plug & Play */
+	{"MNP0342"},		/* Microcom DeskPorte 28.8P Plug & Play */
+	{"MNP0500"},		/* Microcom DeskPorte FAST ES 28.8 Plug & Play */
+	{"MNP0501"},		/* Microcom DeskPorte FAST ES 28.8 Plug & Play */
+	{"MNP0502"},		/* Microcom DeskPorte 28.8S Internal Plug & Play */
+	{"MOT1105"},		/* Motorola BitSURFR Plug & Play */
+	{"MOT1111"},		/* Motorola TA210 Plug & Play */
+	{"MOT1114"},		/* Motorola HMTA 200 (ISDN) Plug & Play */
+	{"MOT1115"},		/* Motorola BitSURFR Plug & Play */
+	{"MOT1190"},		/* Motorola Lifestyle 28.8 Internal */
+	{"MOT1501"},		/* Motorola V.3400 Plug & Play */
+	{"MOT1502"},		/* Motorola Lifestyle 28.8 V.34 Plug & Play */
+	{"MOT1505"},		/* Motorola Power 28.8 V.34 Plug & Play */
+	{"MOT1509"},		/* Motorola ModemSURFR External 28.8 Plug & Play */
+	{"MOT150A"},		/* Motorola Premier 33.6 Desktop Plug & Play */
+	{"MOT150F"},		/* Motorola VoiceSURFR 56K External PnP */
+	{"MOT1510"},		/* Motorola ModemSURFR 56K External PnP */
+	{"MOT1550"},		/* Motorola ModemSURFR 56K Internal PnP */
+	{"MOT1560"},		/* Motorola ModemSURFR Internal 28.8 Plug & Play */
+	{"MOT1580"},		/* Motorola Premier 33.6 Internal Plug & Play */
+	{"MOT15B0"},		/* Motorola OnlineSURFR 28.8 Internal Plug & Play */
+	{"MOT15F0"},		/* Motorola VoiceSURFR 56K Internal PnP */
+	{"MVX00A1"},		/*  Deskline K56 Phone System PnP */
+	{"MVX00F2"},		/* PC Rider K56 Phone System PnP */
+	{"nEC8241"},		/* NEC 98NOTE SPEAKER PHONE FAX MODEM(33600bps) */
+	{"PMC2430"},		/* Pace 56 Voice Internal Plug & Play Modem */
+	{"PNP0500"},		/* Generic standard PC COM port     */
+	{"PNP0501"},		/* Generic 16550A-compatible COM port */
+	{"PNPC000"},		/* Compaq 14400 Modem */
+	{"PNPC001"},		/* Compaq 2400/9600 Modem */
+	{"PNPC031"},		/* Dial-Up Networking Serial Cable between 2 PCs */
+	{"PNPC032"},		/* Dial-Up Networking Parallel Cable between 2 PCs */
+	{"PNPC100"},		/* Standard 9600 bps Modem */
+	{"PNPC101"},		/* Standard 14400 bps Modem */
+	{"PNPC102"},		/*  Standard 28800 bps Modem */
+	{"PNPC103"},		/*  Standard Modem */
+	{"PNPC104"},		/*  Standard 9600 bps Modem */
+	{"PNPC105"},		/*  Standard 14400 bps Modem */
+	{"PNPC106"},		/*  Standard 28800 bps Modem */
+	{"PNPC107"},		/*  Standard Modem */
+	{"PNPC108"},		/* Standard 9600 bps Modem */
+	{"PNPC109"},		/* Standard 14400 bps Modem */
+	{"PNPC10A"},		/* Standard 28800 bps Modem */
+	{"PNPC10B"},		/* Standard Modem */
+	{"PNPC10C"},		/* Standard 9600 bps Modem */
+	{"PNPC10D"},		/* Standard 14400 bps Modem */
+	{"PNPC10E"},		/* Standard 28800 bps Modem */
+	{"PNPC10F"},		/* Standard Modem */
+	{"PNP2000"},		/* Standard PCMCIA Card Modem */
+	{"ROK0030"},		/* Rockwell 33.6 DPF Internal PnP, Modular Technology 33.6 Internal PnP */
+	{"ROK0100"},		/* KORTEX 14400 Externe PnP */
+	{"ROK4120"},		/* Rockwell 28.8 */
+	{"ROK4920"},		/* Viking 28.8 INTERNAL Fax+Data+Voice PnP */
+	{"RSS00A0"},		/* Rockwell 33.6 DPF External PnP, BT Prologue 33.6 External PnP, Modular Technology 33.6 External PnP */
+	{"RSS0262"},		/* Viking 56K FAX INT */
+	{"RSS0250"},		/* K56 par,VV,Voice,Speakphone,AudioSpan,PnP */
+	{"SUP1310"},		/* SupraExpress 28.8 Data/Fax PnP modem */
+	{"SUP1381"},		/* SupraExpress 336i PnP Voice Modem */
+	{"SUP1421"},		/* SupraExpress 33.6 Data/Fax PnP modem */
+	{"SUP1590"},		/* SupraExpress 33.6 Data/Fax PnP modem */
+	{"SUP1620"},		/* SupraExpress 336i Sp ASVD */
+	{"SUP1760"},		/* SupraExpress 33.6 Data/Fax PnP modem */
+	{"SUP2171"},		/* SupraExpress 56i Sp Intl */
+	{"TEX0011"},		/* Phoebe Micro 33.6 Data Fax 1433VQH Plug & Play */
+	{"UAC000F"},		/* Archtek SmartLink Modem 3334BT Plug & Play */
+	{"USR0000"},		/* 3Com Corp. Gateway Telepath IIvi 33.6 */
+	{"USR0002"},		/* U.S. Robotics Sporster 33.6K Fax INT PnP */
+	{"USR0004"},		/*  Sportster Vi 14.4 PnP FAX Voicemail */
+	{"USR0006"},		/* U.S. Robotics 33.6K Voice INT PnP */
+	{"USR0007"},		/* U.S. Robotics 33.6K Voice EXT PnP */
+	{"USR0009"},		/* U.S. Robotics Courier V.Everything INT PnP */
+	{"USR2002"},		/* U.S. Robotics 33.6K Voice INT PnP */
+	{"USR2070"},		/* U.S. Robotics 56K Voice INT PnP */
+	{"USR2080"},		/* U.S. Robotics 56K Voice EXT PnP */
+	{"USR3031"},		/* U.S. Robotics 56K FAX INT */
+	{"USR3050"},		/* U.S. Robotics 56K FAX INT */
+	{"USR3070"},		/* U.S. Robotics 56K Voice INT PnP */
+	{"USR3080"},		/* U.S. Robotics 56K Voice EXT PnP */
+	{"USR3090"},		/* U.S. Robotics 56K Voice INT PnP */
+	{"USR9100"},		/* U.S. Robotics 56K Message  */
+	{"USR9160"},		/* U.S. Robotics 56K FAX EXT PnP */
+	{"USR9170"},		/* U.S. Robotics 56K FAX INT PnP */
+	{"USR9180"},		/* U.S. Robotics 56K Voice EXT PnP */
+	{"USR9190"},		/* U.S. Robotics 56K Voice INT PnP */
+	{"WACFXXX"},		/* Wacom tablets */
+	{"FPI2002"},		/* Compaq touchscreen */
+	{"FUJ02B2"},		/* Fujitsu Stylistic touchscreens */
+	{"FUJ02B3"},
+	{"FUJ02B4"},		/* Fujitsu Stylistic LT touchscreens */
+	{"FUJ02B6"},		/* Passive Fujitsu Stylistic touchscreens */
+	{"FUJ02B7"},
+	{"FUJ02B8"},
+	{"FUJ02B9"},
+	{"FUJ02BC"},
+	{"FUJ02E5"},		/* Fujitsu Wacom Tablet PC device */
+	{"FUJ02E6"},		/* Fujitsu P-series tablet PC device */
+	{"FUJ02E7"},		/* Fujitsu Wacom 2FGT Tablet PC device */
+	{"FUJ02E9"},		/* Fujitsu Wacom 1FGT Tablet PC device */
+	{"LTS0001"},		/* LG C1 EXPRESS DUAL (C1-PB11A3) touch screen (actually a FUJ02E6 in disguise) */
+	{"WCI0003"},		/* Rockwell's (PORALiNK) 33600 INT PNP */
+	{"WEC1022"},		/* Winbond CIR port, should not be probed. We should keep track of it to prevent the legacy serial driver from probing it */
+	/* scl200wdt */
+	{"NSC0800"},		/* National Semiconductor PC87307/PC97307 watchdog component */
+	/* mpu401 */
+	{"PNPb006"},
+	/* cs423x-pnpbios */
+	{"CSC0100"},
+	{"CSC0000"},
+	{"GIM0100"},		/* Guillemot Turtlebeach something appears to be cs4232 compatible */
+	/* es18xx-pnpbios */
+	{"ESS1869"},
+	{"ESS1879"},
+	/* snd-opl3sa2-pnpbios */
+	{"YMH0021"},
+	{"NMX2210"},		/* Gateway Solo 2500 */
+	{""},
+};
+
+static bool is_hex_digit(char c)
+{
+	return (c >= 0 && c <= '9') || (c >= 'A' && c <= 'F');
+}
+
+static bool matching_id(char *idstr, char *list_id)
+{
+	int i;
+
+	if (memcmp(idstr, list_id, 3))
+		return false;
+
+	for (i = 3; i < 7; i++) {
+		char c = toupper(idstr[i]);
+
+		if (!is_hex_digit(c)
+		    || (list_id[i] != 'X' && c != toupper(list_id[i])))
+			return false;
+	}
+	return true;
+}
+
+static bool acpi_pnp_match(char *idstr, const struct acpi_device_id **matchid)
+{
+	const struct acpi_device_id *devid;
+
+	for (devid = acpi_pnp_device_ids; devid->id[0]; devid++)
+		if (matching_id(idstr, (char *)devid->id)) {
+			if (matchid)
+				*matchid = devid;
+
+			return true;
+		}
+
+	return false;
+}
+
+static int acpi_pnp_attach(struct acpi_device *adev,
+			   const struct acpi_device_id *id)
+{
+	return 1;
+}
+
+static struct acpi_scan_handler acpi_pnp_handler = {
+	.ids = acpi_pnp_device_ids,
+	.match = acpi_pnp_match,
+	.attach = acpi_pnp_attach,
+};
+
+/*
+ * For CMOS RTC devices, the PNP ACPI scan handler does not work, because
+ * there is a CMOS RTC ACPI scan handler installed already, so we need to
+ * check those devices and enumerate them to the PNP bus directly.
+ */
+static int is_cmos_rtc_device(struct acpi_device *adev)
+{
+	struct acpi_device_id ids[] = {
+		{ "PNP0B00" },
+		{ "PNP0B01" },
+		{ "PNP0B02" },
+		{""},
+	};
+	return !acpi_match_device_ids(adev, ids);
+}
+
+bool acpi_is_pnp_device(struct acpi_device *adev)
+{
+	return adev->handler == &acpi_pnp_handler || is_cmos_rtc_device(adev);
+}
+EXPORT_SYMBOL_GPL(acpi_is_pnp_device);
+
+void __init acpi_pnp_init(void)
+{
+	acpi_scan_add_handler(&acpi_pnp_handler);
+}
diff --git a/drivers/acpi/acpi_processor.c b/drivers/acpi/acpi_processor.c
index b06f5f55ada..1c085742644 100644
--- a/drivers/acpi/acpi_processor.c
+++ b/drivers/acpi/acpi_processor.c
@@ -268,7 +268,7 @@ static int acpi_processor_get_info(struct acpi_device *device)
 	pr->apic_id = apic_id;
 
 	cpu_index = acpi_map_cpuid(pr->apic_id, pr->acpi_id);
-	if (!cpu0_initialized) {
+	if (!cpu0_initialized && !acpi_lapic) {
 		cpu0_initialized = 1;
 		/* Handle UP system running SMP kernel, with no LAPIC in MADT */
 		if ((cpu_index == -1) && (num_online_cpus() == 1))
@@ -405,7 +405,6 @@ static int acpi_processor_add(struct acpi_device *device,
 		goto err;
 
 	pr->dev = dev;
-	dev->offline = pr->flags.need_hotplug_init;
 
 	/* Trigger the processor driver's .probe() if present. */
 	if (device_attach(dev) >= 0)
diff --git a/drivers/acpi/acpica/Makefile b/drivers/acpi/acpica/Makefile
index b7ed86a2042..8bb43f06e11 100644
--- a/drivers/acpi/acpica/Makefile
+++ b/drivers/acpi/acpica/Makefile
@@ -135,6 +135,7 @@ acpi-y +=		\
 	rsxface.o
 
 acpi-y +=		\
+	tbdata.o	\
 	tbfadt.o	\
 	tbfind.o	\
 	tbinstal.o	\
diff --git a/drivers/acpi/acpica/acapps.h b/drivers/acpi/acpica/acapps.h
new file mode 100644
index 00000000000..8698ffba6f3
--- /dev/null
+++ b/drivers/acpi/acpica/acapps.h
@@ -0,0 +1,170 @@
+/******************************************************************************
+ *
+ * Module Name: acapps - common include for ACPI applications/tools
+ *
+ *****************************************************************************/
+
+/*
+ * Copyright (C) 2000 - 2014, Intel Corp.
+ * All rights reserved.
+ *
+ * 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. Redistributions in binary form must reproduce at minimum a disclaimer
+ *    substantially similar to the "NO WARRANTY" disclaimer below
+ *    ("Disclaimer") and any redistribution must be conditioned upon
+ *    including a substantially similar Disclaimer requirement for further
+ *    binary redistribution.
+ * 3. Neither the names of the above-listed copyright holders nor the names
+ *    of any contributors may 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") version 2 as published by the Free
+ * Software Foundation.
+ *
+ * NO WARRANTY
+ * 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 MERCHANTIBILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDERS OR CONTRIBUTORS BE LIABLE FOR 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 DAMAGES.
+ */
+
+#ifndef _ACAPPS
+#define _ACAPPS
+
+/* Common info for tool signons */
+
+#define ACPICA_NAME                 "Intel ACPI Component Architecture"
+#define ACPICA_COPYRIGHT            "Copyright (c) 2000 - 2014 Intel Corporation"
+
+#if ACPI_MACHINE_WIDTH == 64
+#define ACPI_WIDTH          "-64"
+
+#elif ACPI_MACHINE_WIDTH == 32
+#define ACPI_WIDTH          "-32"
+
+#else
+#error unknown ACPI_MACHINE_WIDTH
+#define ACPI_WIDTH          "-??"
+
+#endif
+
+/* Macros for signons and file headers */
+
+#define ACPI_COMMON_SIGNON(utility_name) \
+	"\n%s\n%s version %8.8X%s [%s]\n%s\n\n", \
+	ACPICA_NAME, \
+	utility_name, ((u32) ACPI_CA_VERSION), ACPI_WIDTH, __DATE__, \
+	ACPICA_COPYRIGHT
+
+#define ACPI_COMMON_HEADER(utility_name, prefix) \
+	"%s%s\n%s%s version %8.8X%s [%s]\n%s%s\n%s\n", \
+	prefix, ACPICA_NAME, \
+	prefix, utility_name, ((u32) ACPI_CA_VERSION), ACPI_WIDTH, __DATE__, \
+	prefix, ACPICA_COPYRIGHT, \
+	prefix
+
+/* Macros for usage messages */
+
+#define ACPI_USAGE_HEADER(usage) \
+	printf ("Usage: %s\nOptions:\n", usage);
+
+#define ACPI_OPTION(name, description) \
+	printf ("  %-18s%s\n", name, description);
+
+#define FILE_SUFFIX_DISASSEMBLY     "dsl"
+#define ACPI_TABLE_FILE_SUFFIX      ".dat"
+
+/*
+ * getopt
+ */
+int acpi_getopt(int argc, char **argv, char *opts);
+
+int acpi_getopt_argument(int argc, char **argv);
+
+extern int acpi_gbl_optind;
+extern int acpi_gbl_opterr;
+extern int acpi_gbl_sub_opt_char;
+extern char *acpi_gbl_optarg;
+
+/*
+ * cmfsize - Common get file size function
+ */
+u32 cm_get_file_size(FILE * file);
+
+#ifndef ACPI_DUMP_APP
+/*
+ * adisasm
+ */
+acpi_status
+ad_aml_disassemble(u8 out_to_file,
+		   char *filename, char *prefix, char **out_filename);
+
+void ad_print_statistics(void);
+
+acpi_status ad_find_dsdt(u8 **dsdt_ptr, u32 *dsdt_length);
+
+void ad_dump_tables(void);
+
+acpi_status ad_get_local_tables(void);
+
+acpi_status
+ad_parse_table(struct acpi_table_header *table,
+	       acpi_owner_id * owner_id, u8 load_table, u8 external);
+
+acpi_status ad_display_tables(char *filename, struct acpi_table_header *table);
+
+acpi_status ad_display_statistics(void);
+
+/*
+ * adwalk
+ */
+void
+acpi_dm_cross_reference_namespace(union acpi_parse_object *parse_tree_root,
+				  struct acpi_namespace_node *namespace_root,
+				  acpi_owner_id owner_id);
+
+void acpi_dm_dump_tree(union acpi_parse_object *origin);
+
+void acpi_dm_find_orphan_methods(union acpi_parse_object *origin);
+
+void
+acpi_dm_finish_namespace_load(union acpi_parse_object *parse_tree_root,
+			      struct acpi_namespace_node *namespace_root,
+			      acpi_owner_id owner_id);
+
+void
+acpi_dm_convert_resource_indexes(union acpi_parse_object *parse_tree_root,
+				 struct acpi_namespace_node *namespace_root);
+
+/*
+ * adfile
+ */
+acpi_status ad_initialize(void);
+
+char *fl_generate_filename(char *input_filename, char *suffix);
+
+acpi_status
+fl_split_input_pathname(char *input_path,
+			char **out_directory_path, char **out_filename);
+
+char *ad_generate_filename(char *prefix, char *table_id);
+
+void
+ad_write_table(struct acpi_table_header *table,
+	       u32 length, char *table_name, char *oem_table_id);
+#endif
+
+#endif				/* _ACAPPS */
diff --git a/drivers/acpi/acpica/acevents.h b/drivers/acpi/acpica/acevents.h
index 68ec61fff18..7a7811a9fc2 100644
--- a/drivers/acpi/acpica/acevents.h
+++ b/drivers/acpi/acpica/acevents.h
@@ -104,9 +104,10 @@ acpi_status acpi_ev_finish_gpe(struct acpi_gpe_event_info *gpe_event_info);
  */
 acpi_status
 acpi_ev_create_gpe_block(struct acpi_namespace_node *gpe_device,
-			 struct acpi_generic_address *gpe_block_address,
+			 u64 address,
+			 u8 space_id,
 			 u32 register_count,
-			 u8 gpe_block_base_number,
+			 u16 gpe_block_base_number,
 			 u32 interrupt_number,
 			 struct acpi_gpe_block_info **return_gpe_block);
 
diff --git a/drivers/acpi/acpica/acglobal.h b/drivers/acpi/acpica/acglobal.h
index 49bbc71fad5..115eedcade1 100644
--- a/drivers/acpi/acpica/acglobal.h
+++ b/drivers/acpi/acpica/acglobal.h
@@ -44,144 +44,14 @@
 #ifndef __ACGLOBAL_H__
 #define __ACGLOBAL_H__
 
-/*
- * Ensure that the globals are actually defined and initialized only once.
- *
- * The use of these macros allows a single list of globals (here) in order
- * to simplify maintenance of the code.
- */
-#ifdef DEFINE_ACPI_GLOBALS
-#define ACPI_GLOBAL(type,name) \
-	extern type name; \
-	type name
-
-#define ACPI_INIT_GLOBAL(type,name,value) \
-	type name=value
-
-#else
-#define ACPI_GLOBAL(type,name) \
-	extern type name
-
-#define ACPI_INIT_GLOBAL(type,name,value) \
-	extern type name
-#endif
-
-#ifdef DEFINE_ACPI_GLOBALS
-
-/* Public globals, available from outside ACPICA subsystem */
-
 /*****************************************************************************
  *
- * Runtime configuration (static defaults that can be overriden at runtime)
+ * Globals related to the ACPI tables
  *
  ****************************************************************************/
 
-/*
- * Enable "slack" in the AML interpreter?  Default is FALSE, and the
- * interpreter strictly follows the ACPI specification. Setting to TRUE
- * allows the interpreter to ignore certain errors and/or bad AML constructs.
- *
- * Currently, these features are enabled by this flag:
- *
- * 1) Allow "implicit return" of last value in a control method
- * 2) Allow access beyond the end of an operation region
- * 3) Allow access to uninitialized locals/args (auto-init to integer 0)
- * 4) Allow ANY object type to be a source operand for the Store() operator
- * 5) Allow unresolved references (invalid target name) in package objects
- * 6) Enable warning messages for behavior that is not ACPI spec compliant
- */
-ACPI_INIT_GLOBAL(u8, acpi_gbl_enable_interpreter_slack, FALSE);
+/* Master list of all ACPI tables that were found in the RSDT/XSDT */
 
-/*
- * Automatically serialize all methods that create named objects? Default
- * is TRUE, meaning that all non_serialized methods are scanned once at
- * table load time to determine those that create named objects. Methods
- * that create named objects are marked Serialized in order to prevent
- * possible run-time problems if they are entered by more than one thread.
- */
-ACPI_INIT_GLOBAL(u8, acpi_gbl_auto_serialize_methods, TRUE);
-
-/*
- * Create the predefined _OSI method in the namespace? Default is TRUE
- * because ACPI CA is fully compatible with other ACPI implementations.
- * Changing this will revert ACPI CA (and machine ASL) to pre-OSI behavior.
- */
-ACPI_INIT_GLOBAL(u8, acpi_gbl_create_osi_method, TRUE);
-
-/*
- * Optionally use default values for the ACPI register widths. Set this to
- * TRUE to use the defaults, if an FADT contains incorrect widths/lengths.
- */
-ACPI_INIT_GLOBAL(u8, acpi_gbl_use_default_register_widths, TRUE);
-
-/*
- * Optionally enable output from the AML Debug Object.
- */
-ACPI_INIT_GLOBAL(u8, acpi_gbl_enable_aml_debug_object, FALSE);
-
-/*
- * Optionally copy the entire DSDT to local memory (instead of simply
- * mapping it.) There are some BIOSs that corrupt or replace the original
- * DSDT, creating the need for this option. Default is FALSE, do not copy
- * the DSDT.
- */
-ACPI_INIT_GLOBAL(u8, acpi_gbl_copy_dsdt_locally, FALSE);
-
-/*
- * Optionally ignore an XSDT if present and use the RSDT instead.
- * Although the ACPI specification requires that an XSDT be used instead
- * of the RSDT, the XSDT has been found to be corrupt or ill-formed on
- * some machines. Default behavior is to use the XSDT if present.
- */
-ACPI_INIT_GLOBAL(u8, acpi_gbl_do_not_use_xsdt, FALSE);
-
-/*
- * Optionally use 32-bit FADT addresses if and when there is a conflict
- * (address mismatch) between the 32-bit and 64-bit versions of the
- * address. Although ACPICA adheres to the ACPI specification which
- * requires the use of the corresponding 64-bit address if it is non-zero,
- * some machines have been found to have a corrupted non-zero 64-bit
- * address. Default is FALSE, do not favor the 32-bit addresses.
- */
-ACPI_INIT_GLOBAL(u8, acpi_gbl_use32_bit_fadt_addresses, FALSE);
-
-/*
- * Optionally truncate I/O addresses to 16 bits. Provides compatibility
- * with other ACPI implementations. NOTE: During ACPICA initialization,
- * this value is set to TRUE if any Windows OSI strings have been
- * requested by the BIOS.
- */
-ACPI_INIT_GLOBAL(u8, acpi_gbl_truncate_io_addresses, FALSE);
-
-/*
- * Disable runtime checking and repair of values returned by control methods.
- * Use only if the repair is causing a problem on a particular machine.
- */
-ACPI_INIT_GLOBAL(u8, acpi_gbl_disable_auto_repair, FALSE);
-
-/*
- * Optionally do not load any SSDTs from the RSDT/XSDT during initialization.
- * This can be useful for debugging ACPI problems on some machines.
- */
-ACPI_INIT_GLOBAL(u8, acpi_gbl_disable_ssdt_table_load, FALSE);
-
-/*
- * We keep track of the latest version of Windows that has been requested by
- * the BIOS.
- */
-ACPI_INIT_GLOBAL(u8, acpi_gbl_osi_data, 0);
-
-#endif				/* DEFINE_ACPI_GLOBALS */
-
-/*****************************************************************************
- *
- * ACPI Table globals
- *
- ****************************************************************************/
-
-/*
- * Master list of all ACPI tables that were found in the RSDT/XSDT.
- */
 ACPI_GLOBAL(struct acpi_table_list, acpi_gbl_root_table_list);
 
 /* DSDT information. Used to check for DSDT corruption */
@@ -279,7 +149,6 @@ ACPI_GLOBAL(acpi_exception_handler, acpi_gbl_exception_handler);
 ACPI_GLOBAL(acpi_init_handler, acpi_gbl_init_handler);
 ACPI_GLOBAL(acpi_table_handler, acpi_gbl_table_handler);
 ACPI_GLOBAL(void *, acpi_gbl_table_handler_context);
-ACPI_GLOBAL(struct acpi_walk_state *, acpi_gbl_breakpoint_walk);
 ACPI_GLOBAL(acpi_interface_handler, acpi_gbl_interface_handler);
 ACPI_GLOBAL(struct acpi_sci_handler_info *, acpi_gbl_sci_handler_list);
 
@@ -296,7 +165,6 @@ ACPI_GLOBAL(u8, acpi_gbl_reg_methods_executed);
 /* Misc */
 
 ACPI_GLOBAL(u32, acpi_gbl_original_mode);
-ACPI_GLOBAL(u32, acpi_gbl_rsdp_original_location);
 ACPI_GLOBAL(u32, acpi_gbl_ns_lookup_count);
 ACPI_GLOBAL(u32, acpi_gbl_ps_find_count);
 ACPI_GLOBAL(u16, acpi_gbl_pm1_enable_register_save);
@@ -483,11 +351,6 @@ ACPI_GLOBAL(u16, acpi_gbl_node_type_count_misc);
 ACPI_GLOBAL(u32, acpi_gbl_num_nodes);
 ACPI_GLOBAL(u32, acpi_gbl_num_objects);
 
-ACPI_GLOBAL(u32, acpi_gbl_size_of_parse_tree);
-ACPI_GLOBAL(u32, acpi_gbl_size_of_method_trees);
-ACPI_GLOBAL(u32, acpi_gbl_size_of_node_entries);
-ACPI_GLOBAL(u32, acpi_gbl_size_of_acpi_objects);
-
 #endif				/* ACPI_DEBUGGER */
 
 /*****************************************************************************
@@ -509,5 +372,6 @@ ACPI_INIT_GLOBAL(ACPI_FILE, acpi_gbl_debug_file, NULL);
  ****************************************************************************/
 
 extern const struct ah_predefined_name asl_predefined_info[];
+extern const struct ah_device_id asl_device_ids[];
 
 #endif				/* __ACGLOBAL_H__ */
diff --git a/drivers/acpi/acpica/aclocal.h b/drivers/acpi/acpica/aclocal.h
index 52a21dafb54..91f801a2e68 100644
--- a/drivers/acpi/acpica/aclocal.h
+++ b/drivers/acpi/acpica/aclocal.h
@@ -450,9 +450,9 @@ struct acpi_gpe_event_info {
 struct acpi_gpe_register_info {
 	struct acpi_generic_address status_address;	/* Address of status reg */
 	struct acpi_generic_address enable_address;	/* Address of enable reg */
+	u16 base_gpe_number;	/* Base GPE number for this register */
 	u8 enable_for_wake;	/* GPEs to keep enabled when sleeping */
 	u8 enable_for_run;	/* GPEs to keep enabled when running */
-	u8 base_gpe_number;	/* Base GPE number for this register */
 };
 
 /*
@@ -466,11 +466,12 @@ struct acpi_gpe_block_info {
 	struct acpi_gpe_xrupt_info *xrupt_block;	/* Backpointer to interrupt block */
 	struct acpi_gpe_register_info *register_info;	/* One per GPE register pair */
 	struct acpi_gpe_event_info *event_info;	/* One for each GPE */
-	struct acpi_generic_address block_address;	/* Base address of the block */
+	u64 address;		/* Base address of the block */
 	u32 register_count;	/* Number of register pairs in block */
 	u16 gpe_count;		/* Number of individual GPEs in block */
-	u8 block_base_number;	/* Base GPE number for this block */
-	u8 initialized;         /* TRUE if this block is initialized */
+	u16 block_base_number;	/* Base GPE number for this block */
+	u8 space_id;
+	u8 initialized;		/* TRUE if this block is initialized */
 };
 
 /* Information about GPE interrupt handlers, one per each interrupt level used for GPEs */
@@ -733,7 +734,8 @@ union acpi_parse_value {
 #define ACPI_DASM_MATCHOP               0x06	/* Parent opcode is a Match() operator */
 #define ACPI_DASM_LNOT_PREFIX           0x07	/* Start of a Lnot_equal (etc.) pair of opcodes */
 #define ACPI_DASM_LNOT_SUFFIX           0x08	/* End  of a Lnot_equal (etc.) pair of opcodes */
-#define ACPI_DASM_IGNORE                0x09	/* Not used at this time */
+#define ACPI_DASM_HID_STRING            0x09	/* String is a _HID or _CID */
+#define ACPI_DASM_IGNORE                0x0A	/* Not used at this time */
 
 /*
  * Generic operation (for example:  If, While, Store)
@@ -1147,4 +1149,9 @@ struct ah_predefined_name {
 #endif
 };
 
+struct ah_device_id {
+	char *name;
+	char *description;
+};
+
 #endif				/* __ACLOCAL_H__ */
diff --git a/drivers/acpi/acpica/acpredef.h b/drivers/acpi/acpica/acpredef.h
index a48d713e959..bd08817cafd 100644
--- a/drivers/acpi/acpica/acpredef.h
+++ b/drivers/acpi/acpica/acpredef.h
@@ -586,6 +586,10 @@ const union acpi_predefined_info acpi_gbl_predefined_methods[] = {
 	{{"_LID", METHOD_0ARGS,
 	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
 
+	{{"_LPD", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (1 Int(rev), n Pkg (2 Int) */
+	PACKAGE_INFO(ACPI_PTYPE2_REV_FIXED, ACPI_RTYPE_INTEGER, 2, 0, 0, 0),
+
 	{{"_MAT", METHOD_0ARGS,
 	  METHOD_RETURNS(ACPI_RTYPE_BUFFER)}},
 
@@ -698,12 +702,6 @@ const union acpi_predefined_info acpi_gbl_predefined_methods[] = {
 	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
 	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
 
-	{{"_PRP", METHOD_0ARGS,
-	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Pkgs) each: 1 Str, 1 Int/Str/Pkg */
-	PACKAGE_INFO(ACPI_PTYPE2, ACPI_RTYPE_STRING, 1,
-		     ACPI_RTYPE_INTEGER | ACPI_RTYPE_STRING |
-		     ACPI_RTYPE_PACKAGE | ACPI_RTYPE_REFERENCE, 1, 0),
-
 	{{"_PRS", METHOD_0ARGS,
 	  METHOD_RETURNS(ACPI_RTYPE_BUFFER)}},
 
diff --git a/drivers/acpi/acpica/actables.h b/drivers/acpi/acpica/actables.h
index 5fa4b202769..f14882788ee 100644
--- a/drivers/acpi/acpica/actables.h
+++ b/drivers/acpi/acpica/actables.h
@@ -54,6 +54,31 @@ acpi_status acpi_tb_validate_rsdp(struct acpi_table_rsdp *rsdp);
 u8 *acpi_tb_scan_memory_for_rsdp(u8 *start_address, u32 length);
 
 /*
+ * tbdata - table data structure management
+ */
+acpi_status acpi_tb_get_next_root_index(u32 *table_index);
+
+void
+acpi_tb_init_table_descriptor(struct acpi_table_desc *table_desc,
+			      acpi_physical_address address,
+			      u8 flags, struct acpi_table_header *table);
+
+acpi_status
+acpi_tb_acquire_temp_table(struct acpi_table_desc *table_desc,
+			   acpi_physical_address address, u8 flags);
+
+void acpi_tb_release_temp_table(struct acpi_table_desc *table_desc);
+
+acpi_status acpi_tb_validate_temp_table(struct acpi_table_desc *table_desc);
+
+acpi_status
+acpi_tb_verify_temp_table(struct acpi_table_desc *table_desc, char *signature);
+
+u8 acpi_tb_is_table_loaded(u32 table_index);
+
+void acpi_tb_set_table_loaded_flag(u32 table_index, u8 is_loaded);
+
+/*
  * tbfadt - FADT parse/convert/validate
  */
 void acpi_tb_parse_fadt(u32 table_index);
@@ -72,22 +97,32 @@ acpi_tb_find_table(char *signature,
  */
 acpi_status acpi_tb_resize_root_table_list(void);
 
-acpi_status acpi_tb_verify_table(struct acpi_table_desc *table_desc);
+acpi_status acpi_tb_validate_table(struct acpi_table_desc *table_desc);
+
+void acpi_tb_invalidate_table(struct acpi_table_desc *table_desc);
+
+void acpi_tb_override_table(struct acpi_table_desc *old_table_desc);
 
-struct acpi_table_header *acpi_tb_table_override(struct acpi_table_header
-						 *table_header,
-						 struct acpi_table_desc
-						 *table_desc);
+acpi_status
+acpi_tb_acquire_table(struct acpi_table_desc *table_desc,
+		      struct acpi_table_header **table_ptr,
+		      u32 *table_length, u8 *table_flags);
+
+void
+acpi_tb_release_table(struct acpi_table_header *table,
+		      u32 table_length, u8 table_flags);
 
 acpi_status
-acpi_tb_add_table(struct acpi_table_desc *table_desc, u32 *table_index);
+acpi_tb_install_standard_table(acpi_physical_address address,
+			       u8 flags,
+			       u8 reload, u8 override, u32 *table_index);
 
 acpi_status
 acpi_tb_store_table(acpi_physical_address address,
 		    struct acpi_table_header *table,
 		    u32 length, u8 flags, u32 *table_index);
 
-void acpi_tb_delete_table(struct acpi_table_desc *table_desc);
+void acpi_tb_uninstall_table(struct acpi_table_desc *table_desc);
 
 void acpi_tb_terminate(void);
 
@@ -99,10 +134,6 @@ acpi_status acpi_tb_release_owner_id(u32 table_index);
 
 acpi_status acpi_tb_get_owner_id(u32 table_index, acpi_owner_id *owner_id);
 
-u8 acpi_tb_is_table_loaded(u32 table_index);
-
-void acpi_tb_set_table_loaded_flag(u32 table_index, u8 is_loaded);
-
 /*
  * tbutils - table manager utilities
  */
@@ -124,8 +155,13 @@ void acpi_tb_check_dsdt_header(void);
 struct acpi_table_header *acpi_tb_copy_dsdt(u32 table_index);
 
 void
-acpi_tb_install_table(acpi_physical_address address,
-		      char *signature, u32 table_index);
+acpi_tb_install_table_with_override(u32 table_index,
+				    struct acpi_table_desc *new_table_desc,
+				    u8 override);
+
+acpi_status
+acpi_tb_install_fixed_table(acpi_physical_address address,
+			    char *signature, u32 table_index);
 
 acpi_status acpi_tb_parse_root_table(acpi_physical_address rsdp_address);
 
diff --git a/drivers/acpi/acpica/acutils.h b/drivers/acpi/acpica/acutils.h
index ceeec0b7ccb..1e256c5bda2 100644
--- a/drivers/acpi/acpica/acutils.h
+++ b/drivers/acpi/acpica/acutils.h
@@ -176,8 +176,7 @@ acpi_status acpi_ut_init_globals(void);
 
 char *acpi_ut_get_mutex_name(u32 mutex_id);
 
-const char *acpi_ut_get_notify_name(u32 notify_value);
-
+const char *acpi_ut_get_notify_name(u32 notify_value, acpi_object_type type);
 #endif
 
 char *acpi_ut_get_type_name(acpi_object_type type);
@@ -737,4 +736,11 @@ acpi_ut_method_error(const char *module_name,
 		     struct acpi_namespace_node *node,
 		     const char *path, acpi_status lookup_status);
 
+/*
+ * Utility functions for ACPI names and IDs
+ */
+const struct ah_predefined_name *acpi_ah_match_predefined_name(char *nameseg);
+
+const struct ah_device_id *acpi_ah_match_hardware_id(char *hid);
+
 #endif				/* _ACUTILS_H */
diff --git a/drivers/acpi/acpica/evgpe.c b/drivers/acpi/acpica/evgpe.c
index 955f83da68a..48f70013b48 100644
--- a/drivers/acpi/acpica/evgpe.c
+++ b/drivers/acpi/acpica/evgpe.c
@@ -383,7 +383,7 @@ u32 acpi_ev_gpe_detect(struct acpi_gpe_xrupt_info * gpe_xrupt_list)
 			if (!(gpe_register_info->enable_for_run |
 			      gpe_register_info->enable_for_wake)) {
 				ACPI_DEBUG_PRINT((ACPI_DB_INTERRUPTS,
-						  "Ignore disabled registers for GPE%02X-GPE%02X: "
+						  "Ignore disabled registers for GPE %02X-%02X: "
 						  "RunEnable=%02X, WakeEnable=%02X\n",
 						  gpe_register_info->
 						  base_gpe_number,
@@ -416,7 +416,7 @@ u32 acpi_ev_gpe_detect(struct acpi_gpe_xrupt_info * gpe_xrupt_list)
 			}
 
 			ACPI_DEBUG_PRINT((ACPI_DB_INTERRUPTS,
-					  "Read registers for GPE%02X-GPE%02X: Status=%02X, Enable=%02X, "
+					  "Read registers for GPE %02X-%02X: Status=%02X, Enable=%02X, "
 					  "RunEnable=%02X, WakeEnable=%02X\n",
 					  gpe_register_info->base_gpe_number,
 					  gpe_register_info->base_gpe_number +
@@ -706,7 +706,8 @@ acpi_ev_gpe_dispatch(struct acpi_namespace_node *gpe_device,
 		status = acpi_hw_clear_gpe(gpe_event_info);
 		if (ACPI_FAILURE(status)) {
 			ACPI_EXCEPTION((AE_INFO, status,
-					"Unable to clear GPE%02X", gpe_number));
+					"Unable to clear GPE %02X",
+					gpe_number));
 			return_UINT32(ACPI_INTERRUPT_NOT_HANDLED);
 		}
 	}
@@ -723,7 +724,7 @@ acpi_ev_gpe_dispatch(struct acpi_namespace_node *gpe_device,
 	status = acpi_hw_low_set_gpe(gpe_event_info, ACPI_GPE_DISABLE);
 	if (ACPI_FAILURE(status)) {
 		ACPI_EXCEPTION((AE_INFO, status,
-				"Unable to disable GPE%02X", gpe_number));
+				"Unable to disable GPE %02X", gpe_number));
 		return_UINT32(ACPI_INTERRUPT_NOT_HANDLED);
 	}
 
@@ -764,7 +765,7 @@ acpi_ev_gpe_dispatch(struct acpi_namespace_node *gpe_device,
 					 gpe_event_info);
 		if (ACPI_FAILURE(status)) {
 			ACPI_EXCEPTION((AE_INFO, status,
-					"Unable to queue handler for GPE%02X - event disabled",
+					"Unable to queue handler for GPE %02X - event disabled",
 					gpe_number));
 		}
 		break;
@@ -776,7 +777,7 @@ acpi_ev_gpe_dispatch(struct acpi_namespace_node *gpe_device,
 		 * a GPE to be enabled if it has no handler or method.
 		 */
 		ACPI_ERROR((AE_INFO,
-			    "No handler or method for GPE%02X, disabling event",
+			    "No handler or method for GPE %02X, disabling event",
 			    gpe_number));
 
 		break;
diff --git a/drivers/acpi/acpica/evgpeblk.c b/drivers/acpi/acpica/evgpeblk.c
index caaed3c673f..d86699eea33 100644
--- a/drivers/acpi/acpica/evgpeblk.c
+++ b/drivers/acpi/acpica/evgpeblk.c
@@ -252,21 +252,17 @@ acpi_ev_create_gpe_info_blocks(struct acpi_gpe_block_info *gpe_block)
 
 		/* Init the register_info for this GPE register (8 GPEs) */
 
-		this_register->base_gpe_number =
-		    (u8) (gpe_block->block_base_number +
-			  (i * ACPI_GPE_REGISTER_WIDTH));
+		this_register->base_gpe_number = (u16)
+		    (gpe_block->block_base_number +
+		     (i * ACPI_GPE_REGISTER_WIDTH));
 
-		this_register->status_address.address =
-		    gpe_block->block_address.address + i;
+		this_register->status_address.address = gpe_block->address + i;
 
 		this_register->enable_address.address =
-		    gpe_block->block_address.address + i +
-		    gpe_block->register_count;
+		    gpe_block->address + i + gpe_block->register_count;
 
-		this_register->status_address.space_id =
-		    gpe_block->block_address.space_id;
-		this_register->enable_address.space_id =
-		    gpe_block->block_address.space_id;
+		this_register->status_address.space_id = gpe_block->space_id;
+		this_register->enable_address.space_id = gpe_block->space_id;
 		this_register->status_address.bit_width =
 		    ACPI_GPE_REGISTER_WIDTH;
 		this_register->enable_address.bit_width =
@@ -334,9 +330,10 @@ error_exit:
 
 acpi_status
 acpi_ev_create_gpe_block(struct acpi_namespace_node *gpe_device,
-			 struct acpi_generic_address *gpe_block_address,
+			 u64 address,
+			 u8 space_id,
 			 u32 register_count,
-			 u8 gpe_block_base_number,
+			 u16 gpe_block_base_number,
 			 u32 interrupt_number,
 			 struct acpi_gpe_block_info **return_gpe_block)
 {
@@ -359,15 +356,14 @@ acpi_ev_create_gpe_block(struct acpi_namespace_node *gpe_device,
 
 	/* Initialize the new GPE block */
 
+	gpe_block->address = address;
+	gpe_block->space_id = space_id;
 	gpe_block->node = gpe_device;
 	gpe_block->gpe_count = (u16)(register_count * ACPI_GPE_REGISTER_WIDTH);
 	gpe_block->initialized = FALSE;
 	gpe_block->register_count = register_count;
 	gpe_block->block_base_number = gpe_block_base_number;
 
-	ACPI_MEMCPY(&gpe_block->block_address, gpe_block_address,
-		    sizeof(struct acpi_generic_address));
-
 	/*
 	 * Create the register_info and event_info sub-structures
 	 * Note: disables and clears all GPEs in the block
@@ -408,12 +404,14 @@ acpi_ev_create_gpe_block(struct acpi_namespace_node *gpe_device,
 	}
 
 	ACPI_DEBUG_PRINT_RAW((ACPI_DB_INIT,
-			      "    Initialized GPE %02X to %02X [%4.4s] %u regs on interrupt 0x%X\n",
+			      "    Initialized GPE %02X to %02X [%4.4s] %u regs on interrupt 0x%X%s\n",
 			      (u32)gpe_block->block_base_number,
 			      (u32)(gpe_block->block_base_number +
 				    (gpe_block->gpe_count - 1)),
 			      gpe_device->name.ascii, gpe_block->register_count,
-			      interrupt_number));
+			      interrupt_number,
+			      interrupt_number ==
+			      acpi_gbl_FADT.sci_interrupt ? " (SCI)" : ""));
 
 	/* Update global count of currently available GPEs */
 
diff --git a/drivers/acpi/acpica/evgpeinit.c b/drivers/acpi/acpica/evgpeinit.c
index ae779c1e871..49fc7effd96 100644
--- a/drivers/acpi/acpica/evgpeinit.c
+++ b/drivers/acpi/acpica/evgpeinit.c
@@ -131,8 +131,10 @@ acpi_status acpi_ev_gpe_initialize(void)
 		/* Install GPE Block 0 */
 
 		status = acpi_ev_create_gpe_block(acpi_gbl_fadt_gpe_device,
-						  &acpi_gbl_FADT.xgpe0_block,
-						  register_count0, 0,
+						  acpi_gbl_FADT.xgpe0_block.
+						  address,
+						  acpi_gbl_FADT.xgpe0_block.
+						  space_id, register_count0, 0,
 						  acpi_gbl_FADT.sci_interrupt,
 						  &acpi_gbl_gpe_fadt_blocks[0]);
 
@@ -169,8 +171,10 @@ acpi_status acpi_ev_gpe_initialize(void)
 
 			status =
 			    acpi_ev_create_gpe_block(acpi_gbl_fadt_gpe_device,
-						     &acpi_gbl_FADT.xgpe1_block,
-						     register_count1,
+						     acpi_gbl_FADT.xgpe1_block.
+						     address,
+						     acpi_gbl_FADT.xgpe1_block.
+						     space_id, register_count1,
 						     acpi_gbl_FADT.gpe1_base,
 						     acpi_gbl_FADT.
 						     sci_interrupt,
diff --git a/drivers/acpi/acpica/evmisc.c b/drivers/acpi/acpica/evmisc.c
index 5d594eb2e5e..24ea3424981 100644
--- a/drivers/acpi/acpica/evmisc.c
+++ b/drivers/acpi/acpica/evmisc.c
@@ -167,7 +167,8 @@ acpi_ev_queue_notify_request(struct acpi_namespace_node * node,
 			  "Dispatching Notify on [%4.4s] (%s) Value 0x%2.2X (%s) Node %p\n",
 			  acpi_ut_get_node_name(node),
 			  acpi_ut_get_type_name(node->type), notify_value,
-			  acpi_ut_get_notify_name(notify_value), node));
+			  acpi_ut_get_notify_name(notify_value, ACPI_TYPE_ANY),
+			  node));
 
 	status = acpi_os_execute(OSL_NOTIFY_HANDLER, acpi_ev_notify_dispatch,
 				 info);
diff --git a/drivers/acpi/acpica/evsci.c b/drivers/acpi/acpica/evsci.c
index 4d8a709c1fc..29630e30382 100644
--- a/drivers/acpi/acpica/evsci.c
+++ b/drivers/acpi/acpica/evsci.c
@@ -117,7 +117,7 @@ static u32 ACPI_SYSTEM_XFACE acpi_ev_sci_xrupt_handler(void *context)
 	ACPI_FUNCTION_TRACE(ev_sci_xrupt_handler);
 
 	/*
-	 * We are guaranteed by the ACPI CA initialization/shutdown code that
+	 * We are guaranteed by the ACPICA initialization/shutdown code that
 	 * if this interrupt handler is installed, ACPI is enabled.
 	 */
 
diff --git a/drivers/acpi/acpica/evxface.c b/drivers/acpi/acpica/evxface.c
index a734b27da06..11e5803b8b4 100644
--- a/drivers/acpi/acpica/evxface.c
+++ b/drivers/acpi/acpica/evxface.c
@@ -239,7 +239,7 @@ acpi_remove_notify_handler(acpi_handle device,
 	union acpi_operand_object *obj_desc;
 	union acpi_operand_object *handler_obj;
 	union acpi_operand_object *previous_handler_obj;
-	acpi_status status;
+	acpi_status status = AE_OK;
 	u32 i;
 
 	ACPI_FUNCTION_TRACE(acpi_remove_notify_handler);
@@ -251,20 +251,17 @@ acpi_remove_notify_handler(acpi_handle device,
 		return_ACPI_STATUS(AE_BAD_PARAMETER);
 	}
 
-	/* Make sure all deferred notify tasks are completed */
-
-	acpi_os_wait_events_complete();
-
-	status = acpi_ut_acquire_mutex(ACPI_MTX_NAMESPACE);
-	if (ACPI_FAILURE(status)) {
-		return_ACPI_STATUS(status);
-	}
-
 	/* Root Object. Global handlers are removed here */
 
 	if (device == ACPI_ROOT_OBJECT) {
 		for (i = 0; i < ACPI_NUM_NOTIFY_TYPES; i++) {
 			if (handler_type & (i + 1)) {
+				status =
+				    acpi_ut_acquire_mutex(ACPI_MTX_NAMESPACE);
+				if (ACPI_FAILURE(status)) {
+					return_ACPI_STATUS(status);
+				}
+
 				if (!acpi_gbl_global_notify[i].handler ||
 				    (acpi_gbl_global_notify[i].handler !=
 				     handler)) {
@@ -277,31 +274,40 @@ acpi_remove_notify_handler(acpi_handle device,
 
 				acpi_gbl_global_notify[i].handler = NULL;
 				acpi_gbl_global_notify[i].context = NULL;
+
+				(void)acpi_ut_release_mutex(ACPI_MTX_NAMESPACE);
+
+				/* Make sure all deferred notify tasks are completed */
+
+				acpi_os_wait_events_complete();
 			}
 		}
 
-		goto unlock_and_exit;
+		return_ACPI_STATUS(AE_OK);
 	}
 
 	/* All other objects: Are Notifies allowed on this object? */
 
 	if (!acpi_ev_is_notify_object(node)) {
-		status = AE_TYPE;
-		goto unlock_and_exit;
+		return_ACPI_STATUS(AE_TYPE);
 	}
 
 	/* Must have an existing internal object */
 
 	obj_desc = acpi_ns_get_attached_object(node);
 	if (!obj_desc) {
-		status = AE_NOT_EXIST;
-		goto unlock_and_exit;
+		return_ACPI_STATUS(AE_NOT_EXIST);
 	}
 
 	/* Internal object exists. Find the handler and remove it */
 
 	for (i = 0; i < ACPI_NUM_NOTIFY_TYPES; i++) {
 		if (handler_type & (i + 1)) {
+			status = acpi_ut_acquire_mutex(ACPI_MTX_NAMESPACE);
+			if (ACPI_FAILURE(status)) {
+				return_ACPI_STATUS(status);
+			}
+
 			handler_obj = obj_desc->common_notify.notify_list[i];
 			previous_handler_obj = NULL;
 
@@ -329,10 +335,17 @@ acpi_remove_notify_handler(acpi_handle device,
 				    handler_obj->notify.next[i];
 			}
 
+			(void)acpi_ut_release_mutex(ACPI_MTX_NAMESPACE);
+
+			/* Make sure all deferred notify tasks are completed */
+
+			acpi_os_wait_events_complete();
 			acpi_ut_remove_reference(handler_obj);
 		}
 	}
 
+	return_ACPI_STATUS(status);
+
 unlock_and_exit:
 	(void)acpi_ut_release_mutex(ACPI_MTX_NAMESPACE);
 	return_ACPI_STATUS(status);
@@ -457,6 +470,8 @@ exit:
 	return_ACPI_STATUS(status);
 }
 
+ACPI_EXPORT_SYMBOL(acpi_install_sci_handler)
+
 /*******************************************************************************
  *
  * FUNCTION:    acpi_remove_sci_handler
@@ -468,7 +483,6 @@ exit:
  * DESCRIPTION: Remove a handler for a System Control Interrupt.
  *
  ******************************************************************************/
-
 acpi_status acpi_remove_sci_handler(acpi_sci_handler address)
 {
 	struct acpi_sci_handler_info *prev_sci_handler;
@@ -522,6 +536,8 @@ unlock_and_exit:
 	return_ACPI_STATUS(status);
 }
 
+ACPI_EXPORT_SYMBOL(acpi_remove_sci_handler)
+
 /*******************************************************************************
  *
  * FUNCTION:    acpi_install_global_event_handler
@@ -537,7 +553,6 @@ unlock_and_exit:
  *              Can be used to update event counters, etc.
  *
  ******************************************************************************/
-
 acpi_status
 acpi_install_global_event_handler(acpi_gbl_event_handler handler, void *context)
 {
@@ -840,10 +855,6 @@ acpi_remove_gpe_handler(acpi_handle gpe_device,
 		return_ACPI_STATUS(AE_BAD_PARAMETER);
 	}
 
-	/* Make sure all deferred GPE tasks are completed */
-
-	acpi_os_wait_events_complete();
-
 	status = acpi_ut_acquire_mutex(ACPI_MTX_EVENTS);
 	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
@@ -895,9 +906,17 @@ acpi_remove_gpe_handler(acpi_handle gpe_device,
 		(void)acpi_ev_add_gpe_reference(gpe_event_info);
 	}
 
+	acpi_os_release_lock(acpi_gbl_gpe_lock, flags);
+	(void)acpi_ut_release_mutex(ACPI_MTX_EVENTS);
+
+	/* Make sure all deferred GPE tasks are completed */
+
+	acpi_os_wait_events_complete();
+
 	/* Now we can free the handler object */
 
 	ACPI_FREE(handler);
+	return_ACPI_STATUS(status);
 
 unlock_and_exit:
 	acpi_os_release_lock(acpi_gbl_gpe_lock, flags);
diff --git a/drivers/acpi/acpica/evxfgpe.c b/drivers/acpi/acpica/evxfgpe.c
index 20a1392ffe0..cb534faf536 100644
--- a/drivers/acpi/acpica/evxfgpe.c
+++ b/drivers/acpi/acpica/evxfgpe.c
@@ -599,9 +599,10 @@ acpi_install_gpe_block(acpi_handle gpe_device,
 	 * For user-installed GPE Block Devices, the gpe_block_base_number
 	 * is always zero
 	 */
-	status =
-	    acpi_ev_create_gpe_block(node, gpe_block_address, register_count, 0,
-				     interrupt_number, &gpe_block);
+	status = acpi_ev_create_gpe_block(node, gpe_block_address->address,
+					  gpe_block_address->space_id,
+					  register_count, 0, interrupt_number,
+					  &gpe_block);
 	if (ACPI_FAILURE(status)) {
 		goto unlock_and_exit;
 	}
diff --git a/drivers/acpi/acpica/exconfig.c b/drivers/acpi/acpica/exconfig.c
index 8ba1464efd1..7d2949420db 100644
--- a/drivers/acpi/acpica/exconfig.c
+++ b/drivers/acpi/acpica/exconfig.c
@@ -343,16 +343,14 @@ acpi_ex_load_op(union acpi_operand_object *obj_desc,
 		struct acpi_walk_state *walk_state)
 {
 	union acpi_operand_object *ddb_handle;
+	struct acpi_table_header *table_header;
 	struct acpi_table_header *table;
-	struct acpi_table_desc table_desc;
 	u32 table_index;
 	acpi_status status;
 	u32 length;
 
 	ACPI_FUNCTION_TRACE(ex_load_op);
 
-	ACPI_MEMSET(&table_desc, 0, sizeof(struct acpi_table_desc));
-
 	/* Source Object can be either an op_region or a Buffer/Field */
 
 	switch (obj_desc->common.type) {
@@ -380,17 +378,17 @@ acpi_ex_load_op(union acpi_operand_object *obj_desc,
 
 		/* Get the table header first so we can get the table length */
 
-		table = ACPI_ALLOCATE(sizeof(struct acpi_table_header));
-		if (!table) {
+		table_header = ACPI_ALLOCATE(sizeof(struct acpi_table_header));
+		if (!table_header) {
 			return_ACPI_STATUS(AE_NO_MEMORY);
 		}
 
 		status =
 		    acpi_ex_region_read(obj_desc,
 					sizeof(struct acpi_table_header),
-					ACPI_CAST_PTR(u8, table));
-		length = table->length;
-		ACPI_FREE(table);
+					ACPI_CAST_PTR(u8, table_header));
+		length = table_header->length;
+		ACPI_FREE(table_header);
 
 		if (ACPI_FAILURE(status)) {
 			return_ACPI_STATUS(status);
@@ -420,22 +418,19 @@ acpi_ex_load_op(union acpi_operand_object *obj_desc,
 
 		/* Allocate a buffer for the table */
 
-		table_desc.pointer = ACPI_ALLOCATE(length);
-		if (!table_desc.pointer) {
+		table = ACPI_ALLOCATE(length);
+		if (!table) {
 			return_ACPI_STATUS(AE_NO_MEMORY);
 		}
 
 		/* Read the entire table */
 
 		status = acpi_ex_region_read(obj_desc, length,
-					     ACPI_CAST_PTR(u8,
-							   table_desc.pointer));
+					     ACPI_CAST_PTR(u8, table));
 		if (ACPI_FAILURE(status)) {
-			ACPI_FREE(table_desc.pointer);
+			ACPI_FREE(table);
 			return_ACPI_STATUS(status);
 		}
-
-		table_desc.address = obj_desc->region.address;
 		break;
 
 	case ACPI_TYPE_BUFFER:	/* Buffer or resolved region_field */
@@ -452,10 +447,10 @@ acpi_ex_load_op(union acpi_operand_object *obj_desc,
 
 		/* Get the actual table length from the table header */
 
-		table =
+		table_header =
 		    ACPI_CAST_PTR(struct acpi_table_header,
 				  obj_desc->buffer.pointer);
-		length = table->length;
+		length = table_header->length;
 
 		/* Table cannot extend beyond the buffer */
 
@@ -470,13 +465,12 @@ acpi_ex_load_op(union acpi_operand_object *obj_desc,
 		 * Copy the table from the buffer because the buffer could be modified
 		 * or even deleted in the future
 		 */
-		table_desc.pointer = ACPI_ALLOCATE(length);
-		if (!table_desc.pointer) {
+		table = ACPI_ALLOCATE(length);
+		if (!table) {
 			return_ACPI_STATUS(AE_NO_MEMORY);
 		}
 
-		ACPI_MEMCPY(table_desc.pointer, table, length);
-		table_desc.address = ACPI_TO_INTEGER(table_desc.pointer);
+		ACPI_MEMCPY(table, table_header, length);
 		break;
 
 	default:
@@ -484,27 +478,32 @@ acpi_ex_load_op(union acpi_operand_object *obj_desc,
 		return_ACPI_STATUS(AE_AML_OPERAND_TYPE);
 	}
 
-	/* Validate table checksum (will not get validated in tb_add_table) */
-
-	status = acpi_tb_verify_checksum(table_desc.pointer, length);
-	if (ACPI_FAILURE(status)) {
-		ACPI_FREE(table_desc.pointer);
-		return_ACPI_STATUS(status);
-	}
-
-	/* Complete the table descriptor */
+	/* Install the new table into the local data structures */
 
-	table_desc.length = length;
-	table_desc.flags = ACPI_TABLE_ORIGIN_ALLOCATED;
+	ACPI_INFO((AE_INFO, "Dynamic OEM Table Load:"));
+	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
 
-	/* Install the new table into the local data structures */
+	status = acpi_tb_install_standard_table(ACPI_PTR_TO_PHYSADDR(table),
+						ACPI_TABLE_ORIGIN_INTERNAL_VIRTUAL,
+						TRUE, TRUE, &table_index);
 
-	status = acpi_tb_add_table(&table_desc, &table_index);
+	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
 	if (ACPI_FAILURE(status)) {
 
 		/* Delete allocated table buffer */
 
-		acpi_tb_delete_table(&table_desc);
+		ACPI_FREE(table);
+		return_ACPI_STATUS(status);
+	}
+
+	/*
+	 * Note: Now table is "INSTALLED", it must be validated before
+	 * loading.
+	 */
+	status =
+	    acpi_tb_validate_table(&acpi_gbl_root_table_list.
+				   tables[table_index]);
+	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
 	}
 
@@ -536,9 +535,6 @@ acpi_ex_load_op(union acpi_operand_object *obj_desc,
 		return_ACPI_STATUS(status);
 	}
 
-	ACPI_INFO((AE_INFO, "Dynamic OEM Table Load:"));
-	acpi_tb_print_table_header(0, table_desc.pointer);
-
 	/* Remove the reference by added by acpi_ex_store above */
 
 	acpi_ut_remove_reference(ddb_handle);
@@ -546,8 +542,7 @@ acpi_ex_load_op(union acpi_operand_object *obj_desc,
 	/* Invoke table handler if present */
 
 	if (acpi_gbl_table_handler) {
-		(void)acpi_gbl_table_handler(ACPI_TABLE_EVENT_LOAD,
-					     table_desc.pointer,
+		(void)acpi_gbl_table_handler(ACPI_TABLE_EVENT_LOAD, table,
 					     acpi_gbl_table_handler_context);
 	}
 
@@ -576,6 +571,13 @@ acpi_status acpi_ex_unload_table(union acpi_operand_object *ddb_handle)
 	ACPI_FUNCTION_TRACE(ex_unload_table);
 
 	/*
+	 * Temporarily emit a warning so that the ASL for the machine can be
+	 * hopefully obtained. This is to say that the Unload() operator is
+	 * extremely rare if not completely unused.
+	 */
+	ACPI_WARNING((AE_INFO, "Received request to unload an ACPI table"));
+
+	/*
 	 * Validate the handle
 	 * Although the handle is partially validated in acpi_ex_reconfiguration()
 	 * when it calls acpi_ex_resolve_operands(), the handle is more completely
diff --git a/drivers/acpi/acpica/exdump.c b/drivers/acpi/acpica/exdump.c
index 973fdae00f9..925202acc3e 100644
--- a/drivers/acpi/acpica/exdump.c
+++ b/drivers/acpi/acpica/exdump.c
@@ -134,9 +134,11 @@ static struct acpi_exdump_info acpi_ex_dump_method[9] = {
 	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(method.aml_start), "Aml Start"}
 };
 
-static struct acpi_exdump_info acpi_ex_dump_mutex[5] = {
+static struct acpi_exdump_info acpi_ex_dump_mutex[6] = {
 	{ACPI_EXD_INIT, ACPI_EXD_TABLE_SIZE(acpi_ex_dump_mutex), NULL},
 	{ACPI_EXD_UINT8, ACPI_EXD_OFFSET(mutex.sync_level), "Sync Level"},
+	{ACPI_EXD_UINT8, ACPI_EXD_OFFSET(mutex.original_sync_level),
+	 "Original Sync Level"},
 	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(mutex.owner_thread), "Owner Thread"},
 	{ACPI_EXD_UINT16, ACPI_EXD_OFFSET(mutex.acquisition_depth),
 	 "Acquire Depth"},
diff --git a/drivers/acpi/acpica/hwpci.c b/drivers/acpi/acpica/hwpci.c
index e701d8c33db..6aade8e1d2a 100644
--- a/drivers/acpi/acpica/hwpci.c
+++ b/drivers/acpi/acpica/hwpci.c
@@ -140,11 +140,12 @@ acpi_hw_derive_pci_id(struct acpi_pci_id *pci_id,
 		/* Walk the list, updating the PCI device/function/bus numbers */
 
 		status = acpi_hw_process_pci_list(pci_id, list_head);
-	}
 
-	/* Always delete the list */
+		/* Delete the list */
+
+		acpi_hw_delete_pci_list(list_head);
+	}
 
-	acpi_hw_delete_pci_list(list_head);
 	return_ACPI_STATUS(status);
 }
 
@@ -187,6 +188,10 @@ acpi_hw_build_pci_list(acpi_handle root_pci_device,
 	while (1) {
 		status = acpi_get_parent(current_device, &parent_device);
 		if (ACPI_FAILURE(status)) {
+
+			/* Must delete the list before exit */
+
+			acpi_hw_delete_pci_list(*return_list_head);
 			return (status);
 		}
 
@@ -199,6 +204,10 @@ acpi_hw_build_pci_list(acpi_handle root_pci_device,
 
 		list_element = ACPI_ALLOCATE(sizeof(struct acpi_pci_device));
 		if (!list_element) {
+
+			/* Must delete the list before exit */
+
+			acpi_hw_delete_pci_list(*return_list_head);
 			return (AE_NO_MEMORY);
 		}
 
diff --git a/drivers/acpi/acpica/rscreate.c b/drivers/acpi/acpica/rscreate.c
index 75d36905065..049d9c22a0f 100644
--- a/drivers/acpi/acpica/rscreate.c
+++ b/drivers/acpi/acpica/rscreate.c
@@ -72,6 +72,8 @@ acpi_buffer_to_resource(u8 *aml_buffer,
 	void *resource;
 	void *current_resource_ptr;
 
+	ACPI_FUNCTION_TRACE(acpi_buffer_to_resource);
+
 	/*
 	 * Note: we allow AE_AML_NO_RESOURCE_END_TAG, since an end tag
 	 * is not required here.
@@ -85,7 +87,7 @@ acpi_buffer_to_resource(u8 *aml_buffer,
 		status = AE_OK;
 	}
 	if (ACPI_FAILURE(status)) {
-		return (status);
+		return_ACPI_STATUS(status);
 	}
 
 	/* Allocate a buffer for the converted resource */
@@ -93,7 +95,7 @@ acpi_buffer_to_resource(u8 *aml_buffer,
 	resource = ACPI_ALLOCATE_ZEROED(list_size_needed);
 	current_resource_ptr = resource;
 	if (!resource) {
-		return (AE_NO_MEMORY);
+		return_ACPI_STATUS(AE_NO_MEMORY);
 	}
 
 	/* Perform the AML-to-Resource conversion */
@@ -110,9 +112,11 @@ acpi_buffer_to_resource(u8 *aml_buffer,
 		*resource_ptr = resource;
 	}
 
-	return (status);
+	return_ACPI_STATUS(status);
 }
 
+ACPI_EXPORT_SYMBOL(acpi_buffer_to_resource)
+
 /*******************************************************************************
  *
  * FUNCTION:    acpi_rs_create_resource_list
@@ -130,10 +134,9 @@ acpi_buffer_to_resource(u8 *aml_buffer,
  *              of device resources.
  *
  ******************************************************************************/
-
 acpi_status
 acpi_rs_create_resource_list(union acpi_operand_object *aml_buffer,
-			     struct acpi_buffer * output_buffer)
+			     struct acpi_buffer *output_buffer)
 {
 
 	acpi_status status;
diff --git a/drivers/acpi/acpica/tbdata.c b/drivers/acpi/acpica/tbdata.c
new file mode 100644
index 00000000000..f499c10ceb4
--- /dev/null
+++ b/drivers/acpi/acpica/tbdata.c
@@ -0,0 +1,760 @@
+/******************************************************************************
+ *
+ * Module Name: tbdata - Table manager data structure functions
+ *
+ *****************************************************************************/
+
+/*
+ * Copyright (C) 2000 - 2014, Intel Corp.
+ * All rights reserved.
+ *
+ * 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. Redistributions in binary form must reproduce at minimum a disclaimer
+ *    substantially similar to the "NO WARRANTY" disclaimer below
+ *    ("Disclaimer") and any redistribution must be conditioned upon
+ *    including a substantially similar Disclaimer requirement for further
+ *    binary redistribution.
+ * 3. Neither the names of the above-listed copyright holders nor the names
+ *    of any contributors may 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") version 2 as published by the Free
+ * Software Foundation.
+ *
+ * NO WARRANTY
+ * 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 MERCHANTIBILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDERS OR CONTRIBUTORS BE LIABLE FOR 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 DAMAGES.
+ */
+
+#include <acpi/acpi.h>
+#include "accommon.h"
+#include "acnamesp.h"
+#include "actables.h"
+
+#define _COMPONENT          ACPI_TABLES
+ACPI_MODULE_NAME("tbdata")
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_tb_init_table_descriptor
+ *
+ * PARAMETERS:  table_desc              - Table descriptor
+ *              address                 - Physical address of the table
+ *              flags                   - Allocation flags of the table
+ *              table                   - Pointer to the table
+ *
+ * RETURN:      None
+ *
+ * DESCRIPTION: Initialize a new table descriptor
+ *
+ ******************************************************************************/
+void
+acpi_tb_init_table_descriptor(struct acpi_table_desc *table_desc,
+			      acpi_physical_address address,
+			      u8 flags, struct acpi_table_header *table)
+{
+
+	/*
+	 * Initialize the table descriptor. Set the pointer to NULL, since the
+	 * table is not fully mapped at this time.
+	 */
+	ACPI_MEMSET(table_desc, 0, sizeof(struct acpi_table_desc));
+	table_desc->address = address;
+	table_desc->length = table->length;
+	table_desc->flags = flags;
+	ACPI_MOVE_32_TO_32(table_desc->signature.ascii, table->signature);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_tb_acquire_table
+ *
+ * PARAMETERS:  table_desc          - Table descriptor
+ *              table_ptr           - Where table is returned
+ *              table_length        - Where table length is returned
+ *              table_flags         - Where table allocation flags are returned
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Acquire an ACPI table. It can be used for tables not
+ *              maintained in the acpi_gbl_root_table_list.
+ *
+ ******************************************************************************/
+
+acpi_status
+acpi_tb_acquire_table(struct acpi_table_desc *table_desc,
+		      struct acpi_table_header **table_ptr,
+		      u32 *table_length, u8 *table_flags)
+{
+	struct acpi_table_header *table = NULL;
+
+	switch (table_desc->flags & ACPI_TABLE_ORIGIN_MASK) {
+	case ACPI_TABLE_ORIGIN_INTERNAL_PHYSICAL:
+
+		table =
+		    acpi_os_map_memory(table_desc->address, table_desc->length);
+		break;
+
+	case ACPI_TABLE_ORIGIN_INTERNAL_VIRTUAL:
+	case ACPI_TABLE_ORIGIN_EXTERNAL_VIRTUAL:
+
+		table =
+		    ACPI_CAST_PTR(struct acpi_table_header,
+				  table_desc->address);
+		break;
+
+	default:
+
+		break;
+	}
+
+	/* Table is not valid yet */
+
+	if (!table) {
+		return (AE_NO_MEMORY);
+	}
+
+	/* Fill the return values */
+
+	*table_ptr = table;
+	*table_length = table_desc->length;
+	*table_flags = table_desc->flags;
+	return (AE_OK);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_tb_release_table
+ *
+ * PARAMETERS:  table               - Pointer for the table
+ *              table_length        - Length for the table
+ *              table_flags         - Allocation flags for the table
+ *
+ * RETURN:      None
+ *
+ * DESCRIPTION: Release a table. The inverse of acpi_tb_acquire_table().
+ *
+ ******************************************************************************/
+
+void
+acpi_tb_release_table(struct acpi_table_header *table,
+		      u32 table_length, u8 table_flags)
+{
+
+	switch (table_flags & ACPI_TABLE_ORIGIN_MASK) {
+	case ACPI_TABLE_ORIGIN_INTERNAL_PHYSICAL:
+
+		acpi_os_unmap_memory(table, table_length);
+		break;
+
+	case ACPI_TABLE_ORIGIN_INTERNAL_VIRTUAL:
+	case ACPI_TABLE_ORIGIN_EXTERNAL_VIRTUAL:
+	default:
+
+		break;
+	}
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_tb_acquire_temp_table
+ *
+ * PARAMETERS:  table_desc          - Table descriptor to be acquired
+ *              address             - Address of the table
+ *              flags               - Allocation flags of the table
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: This function validates the table header to obtain the length
+ *              of a table and fills the table descriptor to make its state as
+ *              "INSTALLED". Such a table descriptor is only used for verified
+ *              installation.
+ *
+ ******************************************************************************/
+
+acpi_status
+acpi_tb_acquire_temp_table(struct acpi_table_desc *table_desc,
+			   acpi_physical_address address, u8 flags)
+{
+	struct acpi_table_header *table_header;
+
+	switch (flags & ACPI_TABLE_ORIGIN_MASK) {
+	case ACPI_TABLE_ORIGIN_INTERNAL_PHYSICAL:
+
+		/* Get the length of the full table from the header */
+
+		table_header =
+		    acpi_os_map_memory(address,
+				       sizeof(struct acpi_table_header));
+		if (!table_header) {
+			return (AE_NO_MEMORY);
+		}
+
+		acpi_tb_init_table_descriptor(table_desc, address, flags,
+					      table_header);
+		acpi_os_unmap_memory(table_header,
+				     sizeof(struct acpi_table_header));
+		return (AE_OK);
+
+	case ACPI_TABLE_ORIGIN_INTERNAL_VIRTUAL:
+	case ACPI_TABLE_ORIGIN_EXTERNAL_VIRTUAL:
+
+		table_header = ACPI_CAST_PTR(struct acpi_table_header, address);
+		if (!table_header) {
+			return (AE_NO_MEMORY);
+		}
+
+		acpi_tb_init_table_descriptor(table_desc, address, flags,
+					      table_header);
+		return (AE_OK);
+
+	default:
+
+		break;
+	}
+
+	/* Table is not valid yet */
+
+	return (AE_NO_MEMORY);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_tb_release_temp_table
+ *
+ * PARAMETERS:  table_desc          - Table descriptor to be released
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: The inverse of acpi_tb_acquire_temp_table().
+ *
+ *****************************************************************************/
+
+void acpi_tb_release_temp_table(struct acpi_table_desc *table_desc)
+{
+
+	/*
+	 * Note that the .Address is maintained by the callers of
+	 * acpi_tb_acquire_temp_table(), thus do not invoke acpi_tb_uninstall_table()
+	 * where .Address will be freed.
+	 */
+	acpi_tb_invalidate_table(table_desc);
+}
+
+/******************************************************************************
+ *
+ * FUNCTION:    acpi_tb_validate_table
+ *
+ * PARAMETERS:  table_desc          - Table descriptor
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: This function is called to validate the table, the returned
+ *              table descriptor is in "VALIDATED" state.
+ *
+ *****************************************************************************/
+
+acpi_status acpi_tb_validate_table(struct acpi_table_desc *table_desc)
+{
+	acpi_status status = AE_OK;
+
+	ACPI_FUNCTION_TRACE(tb_validate_table);
+
+	/* Validate the table if necessary */
+
+	if (!table_desc->pointer) {
+		status = acpi_tb_acquire_table(table_desc, &table_desc->pointer,
+					       &table_desc->length,
+					       &table_desc->flags);
+		if (!table_desc->pointer) {
+			status = AE_NO_MEMORY;
+		}
+	}
+
+	return_ACPI_STATUS(status);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_tb_invalidate_table
+ *
+ * PARAMETERS:  table_desc          - Table descriptor
+ *
+ * RETURN:      None
+ *
+ * DESCRIPTION: Invalidate one internal ACPI table, this is the inverse of
+ *              acpi_tb_validate_table().
+ *
+ ******************************************************************************/
+
+void acpi_tb_invalidate_table(struct acpi_table_desc *table_desc)
+{
+
+	ACPI_FUNCTION_TRACE(tb_invalidate_table);
+
+	/* Table must be validated */
+
+	if (!table_desc->pointer) {
+		return_VOID;
+	}
+
+	acpi_tb_release_table(table_desc->pointer, table_desc->length,
+			      table_desc->flags);
+	table_desc->pointer = NULL;
+
+	return_VOID;
+}
+
+/******************************************************************************
+ *
+ * FUNCTION:    acpi_tb_validate_temp_table
+ *
+ * PARAMETERS:  table_desc          - Table descriptor
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: This function is called to validate the table, the returned
+ *              table descriptor is in "VALIDATED" state.
+ *
+ *****************************************************************************/
+
+acpi_status acpi_tb_validate_temp_table(struct acpi_table_desc *table_desc)
+{
+
+	if (!table_desc->pointer && !acpi_gbl_verify_table_checksum) {
+		/*
+		 * Only validates the header of the table.
+		 * Note that Length contains the size of the mapping after invoking
+		 * this work around, this value is required by
+		 * acpi_tb_release_temp_table().
+		 * We can do this because in acpi_init_table_descriptor(), the Length
+		 * field of the installed descriptor is filled with the actual
+		 * table length obtaining from the table header.
+		 */
+		table_desc->length = sizeof(struct acpi_table_header);
+	}
+
+	return (acpi_tb_validate_table(table_desc));
+}
+
+/******************************************************************************
+ *
+ * FUNCTION:    acpi_tb_verify_temp_table
+ *
+ * PARAMETERS:  table_desc          - Table descriptor
+ *              signature           - Table signature to verify
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: This function is called to validate and verify the table, the
+ *              returned table descriptor is in "VALIDATED" state.
+ *
+ *****************************************************************************/
+
+acpi_status
+acpi_tb_verify_temp_table(struct acpi_table_desc * table_desc, char *signature)
+{
+	acpi_status status = AE_OK;
+
+	ACPI_FUNCTION_TRACE(tb_verify_temp_table);
+
+	/* Validate the table */
+
+	status = acpi_tb_validate_temp_table(table_desc);
+	if (ACPI_FAILURE(status)) {
+		return_ACPI_STATUS(AE_NO_MEMORY);
+	}
+
+	/* If a particular signature is expected (DSDT/FACS), it must match */
+
+	if (signature && !ACPI_COMPARE_NAME(&table_desc->signature, signature)) {
+		ACPI_BIOS_ERROR((AE_INFO,
+				 "Invalid signature 0x%X for ACPI table, expected [%s]",
+				 table_desc->signature.integer, signature));
+		status = AE_BAD_SIGNATURE;
+		goto invalidate_and_exit;
+	}
+
+	/* Verify the checksum */
+
+	if (acpi_gbl_verify_table_checksum) {
+		status =
+		    acpi_tb_verify_checksum(table_desc->pointer,
+					    table_desc->length);
+		if (ACPI_FAILURE(status)) {
+			ACPI_EXCEPTION((AE_INFO, AE_NO_MEMORY,
+					"%4.4s " ACPI_PRINTF_UINT
+					" Attempted table install failed",
+					acpi_ut_valid_acpi_name(table_desc->
+								signature.
+								ascii) ?
+					table_desc->signature.ascii : "????",
+					ACPI_FORMAT_TO_UINT(table_desc->
+							    address)));
+			goto invalidate_and_exit;
+		}
+	}
+
+	return_ACPI_STATUS(AE_OK);
+
+invalidate_and_exit:
+	acpi_tb_invalidate_table(table_desc);
+	return_ACPI_STATUS(status);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_tb_resize_root_table_list
+ *
+ * PARAMETERS:  None
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Expand the size of global table array
+ *
+ ******************************************************************************/
+
+acpi_status acpi_tb_resize_root_table_list(void)
+{
+	struct acpi_table_desc *tables;
+	u32 table_count;
+
+	ACPI_FUNCTION_TRACE(tb_resize_root_table_list);
+
+	/* allow_resize flag is a parameter to acpi_initialize_tables */
+
+	if (!(acpi_gbl_root_table_list.flags & ACPI_ROOT_ALLOW_RESIZE)) {
+		ACPI_ERROR((AE_INFO,
+			    "Resize of Root Table Array is not allowed"));
+		return_ACPI_STATUS(AE_SUPPORT);
+	}
+
+	/* Increase the Table Array size */
+
+	if (acpi_gbl_root_table_list.flags & ACPI_ROOT_ORIGIN_ALLOCATED) {
+		table_count = acpi_gbl_root_table_list.max_table_count;
+	} else {
+		table_count = acpi_gbl_root_table_list.current_table_count;
+	}
+
+	tables = ACPI_ALLOCATE_ZEROED(((acpi_size) table_count +
+				       ACPI_ROOT_TABLE_SIZE_INCREMENT) *
+				      sizeof(struct acpi_table_desc));
+	if (!tables) {
+		ACPI_ERROR((AE_INFO,
+			    "Could not allocate new root table array"));
+		return_ACPI_STATUS(AE_NO_MEMORY);
+	}
+
+	/* Copy and free the previous table array */
+
+	if (acpi_gbl_root_table_list.tables) {
+		ACPI_MEMCPY(tables, acpi_gbl_root_table_list.tables,
+			    (acpi_size) table_count *
+			    sizeof(struct acpi_table_desc));
+
+		if (acpi_gbl_root_table_list.flags & ACPI_ROOT_ORIGIN_ALLOCATED) {
+			ACPI_FREE(acpi_gbl_root_table_list.tables);
+		}
+	}
+
+	acpi_gbl_root_table_list.tables = tables;
+	acpi_gbl_root_table_list.max_table_count =
+	    table_count + ACPI_ROOT_TABLE_SIZE_INCREMENT;
+	acpi_gbl_root_table_list.flags |= ACPI_ROOT_ORIGIN_ALLOCATED;
+
+	return_ACPI_STATUS(AE_OK);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_tb_get_next_root_index
+ *
+ * PARAMETERS:  table_index         - Where table index is returned
+ *
+ * RETURN:      Status and table index.
+ *
+ * DESCRIPTION: Allocate a new ACPI table entry to the global table list
+ *
+ ******************************************************************************/
+
+acpi_status acpi_tb_get_next_root_index(u32 *table_index)
+{
+	acpi_status status;
+
+	/* Ensure that there is room for the table in the Root Table List */
+
+	if (acpi_gbl_root_table_list.current_table_count >=
+	    acpi_gbl_root_table_list.max_table_count) {
+		status = acpi_tb_resize_root_table_list();
+		if (ACPI_FAILURE(status)) {
+			return (status);
+		}
+	}
+
+	*table_index = acpi_gbl_root_table_list.current_table_count;
+	acpi_gbl_root_table_list.current_table_count++;
+	return (AE_OK);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_tb_terminate
+ *
+ * PARAMETERS:  None
+ *
+ * RETURN:      None
+ *
+ * DESCRIPTION: Delete all internal ACPI tables
+ *
+ ******************************************************************************/
+
+void acpi_tb_terminate(void)
+{
+	u32 i;
+
+	ACPI_FUNCTION_TRACE(tb_terminate);
+
+	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
+
+	/* Delete the individual tables */
+
+	for (i = 0; i < acpi_gbl_root_table_list.current_table_count; i++) {
+		acpi_tb_uninstall_table(&acpi_gbl_root_table_list.tables[i]);
+	}
+
+	/*
+	 * Delete the root table array if allocated locally. Array cannot be
+	 * mapped, so we don't need to check for that flag.
+	 */
+	if (acpi_gbl_root_table_list.flags & ACPI_ROOT_ORIGIN_ALLOCATED) {
+		ACPI_FREE(acpi_gbl_root_table_list.tables);
+	}
+
+	acpi_gbl_root_table_list.tables = NULL;
+	acpi_gbl_root_table_list.flags = 0;
+	acpi_gbl_root_table_list.current_table_count = 0;
+
+	ACPI_DEBUG_PRINT((ACPI_DB_INFO, "ACPI Tables freed\n"));
+
+	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
+	return_VOID;
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_tb_delete_namespace_by_owner
+ *
+ * PARAMETERS:  table_index         - Table index
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Delete all namespace objects created when this table was loaded.
+ *
+ ******************************************************************************/
+
+acpi_status acpi_tb_delete_namespace_by_owner(u32 table_index)
+{
+	acpi_owner_id owner_id;
+	acpi_status status;
+
+	ACPI_FUNCTION_TRACE(tb_delete_namespace_by_owner);
+
+	status = acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
+	if (ACPI_FAILURE(status)) {
+		return_ACPI_STATUS(status);
+	}
+
+	if (table_index >= acpi_gbl_root_table_list.current_table_count) {
+
+		/* The table index does not exist */
+
+		(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
+		return_ACPI_STATUS(AE_NOT_EXIST);
+	}
+
+	/* Get the owner ID for this table, used to delete namespace nodes */
+
+	owner_id = acpi_gbl_root_table_list.tables[table_index].owner_id;
+	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
+
+	/*
+	 * Need to acquire the namespace writer lock to prevent interference
+	 * with any concurrent namespace walks. The interpreter must be
+	 * released during the deletion since the acquisition of the deletion
+	 * lock may block, and also since the execution of a namespace walk
+	 * must be allowed to use the interpreter.
+	 */
+	(void)acpi_ut_release_mutex(ACPI_MTX_INTERPRETER);
+	status = acpi_ut_acquire_write_lock(&acpi_gbl_namespace_rw_lock);
+
+	acpi_ns_delete_namespace_by_owner(owner_id);
+	if (ACPI_FAILURE(status)) {
+		return_ACPI_STATUS(status);
+	}
+
+	acpi_ut_release_write_lock(&acpi_gbl_namespace_rw_lock);
+
+	status = acpi_ut_acquire_mutex(ACPI_MTX_INTERPRETER);
+	return_ACPI_STATUS(status);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_tb_allocate_owner_id
+ *
+ * PARAMETERS:  table_index         - Table index
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Allocates owner_id in table_desc
+ *
+ ******************************************************************************/
+
+acpi_status acpi_tb_allocate_owner_id(u32 table_index)
+{
+	acpi_status status = AE_BAD_PARAMETER;
+
+	ACPI_FUNCTION_TRACE(tb_allocate_owner_id);
+
+	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
+	if (table_index < acpi_gbl_root_table_list.current_table_count) {
+		status =
+		    acpi_ut_allocate_owner_id(&
+					      (acpi_gbl_root_table_list.
+					       tables[table_index].owner_id));
+	}
+
+	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
+	return_ACPI_STATUS(status);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_tb_release_owner_id
+ *
+ * PARAMETERS:  table_index         - Table index
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Releases owner_id in table_desc
+ *
+ ******************************************************************************/
+
+acpi_status acpi_tb_release_owner_id(u32 table_index)
+{
+	acpi_status status = AE_BAD_PARAMETER;
+
+	ACPI_FUNCTION_TRACE(tb_release_owner_id);
+
+	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
+	if (table_index < acpi_gbl_root_table_list.current_table_count) {
+		acpi_ut_release_owner_id(&
+					 (acpi_gbl_root_table_list.
+					  tables[table_index].owner_id));
+		status = AE_OK;
+	}
+
+	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
+	return_ACPI_STATUS(status);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_tb_get_owner_id
+ *
+ * PARAMETERS:  table_index         - Table index
+ *              owner_id            - Where the table owner_id is returned
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: returns owner_id for the ACPI table
+ *
+ ******************************************************************************/
+
+acpi_status acpi_tb_get_owner_id(u32 table_index, acpi_owner_id * owner_id)
+{
+	acpi_status status = AE_BAD_PARAMETER;
+
+	ACPI_FUNCTION_TRACE(tb_get_owner_id);
+
+	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
+	if (table_index < acpi_gbl_root_table_list.current_table_count) {
+		*owner_id =
+		    acpi_gbl_root_table_list.tables[table_index].owner_id;
+		status = AE_OK;
+	}
+
+	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
+	return_ACPI_STATUS(status);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_tb_is_table_loaded
+ *
+ * PARAMETERS:  table_index         - Index into the root table
+ *
+ * RETURN:      Table Loaded Flag
+ *
+ ******************************************************************************/
+
+u8 acpi_tb_is_table_loaded(u32 table_index)
+{
+	u8 is_loaded = FALSE;
+
+	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
+	if (table_index < acpi_gbl_root_table_list.current_table_count) {
+		is_loaded = (u8)
+		    (acpi_gbl_root_table_list.tables[table_index].flags &
+		     ACPI_TABLE_IS_LOADED);
+	}
+
+	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
+	return (is_loaded);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_tb_set_table_loaded_flag
+ *
+ * PARAMETERS:  table_index         - Table index
+ *              is_loaded           - TRUE if table is loaded, FALSE otherwise
+ *
+ * RETURN:      None
+ *
+ * DESCRIPTION: Sets the table loaded flag to either TRUE or FALSE.
+ *
+ ******************************************************************************/
+
+void acpi_tb_set_table_loaded_flag(u32 table_index, u8 is_loaded)
+{
+
+	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
+	if (table_index < acpi_gbl_root_table_list.current_table_count) {
+		if (is_loaded) {
+			acpi_gbl_root_table_list.tables[table_index].flags |=
+			    ACPI_TABLE_IS_LOADED;
+		} else {
+			acpi_gbl_root_table_list.tables[table_index].flags &=
+			    ~ACPI_TABLE_IS_LOADED;
+		}
+	}
+
+	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
+}
diff --git a/drivers/acpi/acpica/tbfadt.c b/drivers/acpi/acpica/tbfadt.c
index ec14588254d..41519a95808 100644
--- a/drivers/acpi/acpica/tbfadt.c
+++ b/drivers/acpi/acpica/tbfadt.c
@@ -52,7 +52,8 @@ ACPI_MODULE_NAME("tbfadt")
 static void
 acpi_tb_init_generic_address(struct acpi_generic_address *generic_address,
 			     u8 space_id,
-			     u8 byte_width, u64 address, char *register_name);
+			     u8 byte_width,
+			     u64 address, char *register_name, u8 flags);
 
 static void acpi_tb_convert_fadt(void);
 
@@ -69,13 +70,14 @@ typedef struct acpi_fadt_info {
 	u16 address32;
 	u16 length;
 	u8 default_length;
-	u8 type;
+	u8 flags;
 
 } acpi_fadt_info;
 
 #define ACPI_FADT_OPTIONAL          0
 #define ACPI_FADT_REQUIRED          1
 #define ACPI_FADT_SEPARATE_LENGTH   2
+#define ACPI_FADT_GPE_REGISTER      4
 
 static struct acpi_fadt_info fadt_info_table[] = {
 	{"Pm1aEventBlock",
@@ -125,14 +127,14 @@ static struct acpi_fadt_info fadt_info_table[] = {
 	 ACPI_FADT_OFFSET(gpe0_block),
 	 ACPI_FADT_OFFSET(gpe0_block_length),
 	 0,
-	 ACPI_FADT_SEPARATE_LENGTH},
+	 ACPI_FADT_SEPARATE_LENGTH | ACPI_FADT_GPE_REGISTER},
 
 	{"Gpe1Block",
 	 ACPI_FADT_OFFSET(xgpe1_block),
 	 ACPI_FADT_OFFSET(gpe1_block),
 	 ACPI_FADT_OFFSET(gpe1_block_length),
 	 0,
-	 ACPI_FADT_SEPARATE_LENGTH}
+	 ACPI_FADT_SEPARATE_LENGTH | ACPI_FADT_GPE_REGISTER}
 };
 
 #define ACPI_FADT_INFO_ENTRIES \
@@ -189,19 +191,29 @@ static struct acpi_fadt_pm_info fadt_pm_info_table[] = {
 static void
 acpi_tb_init_generic_address(struct acpi_generic_address *generic_address,
 			     u8 space_id,
-			     u8 byte_width, u64 address, char *register_name)
+			     u8 byte_width,
+			     u64 address, char *register_name, u8 flags)
 {
 	u8 bit_width;
 
-	/* Bit width field in the GAS is only one byte long, 255 max */
-
+	/*
+	 * Bit width field in the GAS is only one byte long, 255 max.
+	 * Check for bit_width overflow in GAS.
+	 */
 	bit_width = (u8)(byte_width * 8);
-
-	if (byte_width > 31) {	/* (31*8)=248 */
-		ACPI_ERROR((AE_INFO,
-			    "%s - 32-bit FADT register is too long (%u bytes, %u bits) "
-			    "to convert to GAS struct - 255 bits max, truncating",
-			    register_name, byte_width, (byte_width * 8)));
+	if (byte_width > 31) {	/* (31*8)=248, (32*8)=256 */
+		/*
+		 * No error for GPE blocks, because we do not use the bit_width
+		 * for GPEs, the legacy length (byte_width) is used instead to
+		 * allow for a large number of GPEs.
+		 */
+		if (!(flags & ACPI_FADT_GPE_REGISTER)) {
+			ACPI_ERROR((AE_INFO,
+				    "%s - 32-bit FADT register is too long (%u bytes, %u bits) "
+				    "to convert to GAS struct - 255 bits max, truncating",
+				    register_name, byte_width,
+				    (byte_width * 8)));
+		}
 
 		bit_width = 255;
 	}
@@ -332,15 +344,15 @@ void acpi_tb_parse_fadt(u32 table_index)
 
 	/* Obtain the DSDT and FACS tables via their addresses within the FADT */
 
-	acpi_tb_install_table((acpi_physical_address) acpi_gbl_FADT.Xdsdt,
-			      ACPI_SIG_DSDT, ACPI_TABLE_INDEX_DSDT);
+	acpi_tb_install_fixed_table((acpi_physical_address) acpi_gbl_FADT.Xdsdt,
+				    ACPI_SIG_DSDT, ACPI_TABLE_INDEX_DSDT);
 
 	/* If Hardware Reduced flag is set, there is no FACS */
 
 	if (!acpi_gbl_reduced_hardware) {
-		acpi_tb_install_table((acpi_physical_address) acpi_gbl_FADT.
-				      Xfacs, ACPI_SIG_FACS,
-				      ACPI_TABLE_INDEX_FACS);
+		acpi_tb_install_fixed_table((acpi_physical_address)
+					    acpi_gbl_FADT.Xfacs, ACPI_SIG_FACS,
+					    ACPI_TABLE_INDEX_FACS);
 	}
 }
 
@@ -450,6 +462,7 @@ static void acpi_tb_convert_fadt(void)
 	struct acpi_generic_address *address64;
 	u32 address32;
 	u8 length;
+	u8 flags;
 	u32 i;
 
 	/*
@@ -515,6 +528,7 @@ static void acpi_tb_convert_fadt(void)
 				       fadt_info_table[i].length);
 
 		name = fadt_info_table[i].name;
+		flags = fadt_info_table[i].flags;
 
 		/*
 		 * Expand the ACPI 1.0 32-bit addresses to the ACPI 2.0 64-bit "X"
@@ -554,7 +568,7 @@ static void acpi_tb_convert_fadt(void)
 									   [i].
 									   length),
 							     (u64)address32,
-							     name);
+							     name, flags);
 			} else if (address64->address != (u64)address32) {
 
 				/* Address mismatch */
@@ -582,7 +596,8 @@ static void acpi_tb_convert_fadt(void)
 								      length),
 								     (u64)
 								     address32,
-								     name);
+								     name,
+								     flags);
 				}
 			}
 		}
@@ -603,7 +618,7 @@ static void acpi_tb_convert_fadt(void)
 					   address64->bit_width));
 		}
 
-		if (fadt_info_table[i].type & ACPI_FADT_REQUIRED) {
+		if (fadt_info_table[i].flags & ACPI_FADT_REQUIRED) {
 			/*
 			 * Field is required (Pm1a_event, Pm1a_control).
 			 * Both the address and length must be non-zero.
@@ -617,7 +632,7 @@ static void acpi_tb_convert_fadt(void)
 								    address),
 						 length));
 			}
-		} else if (fadt_info_table[i].type & ACPI_FADT_SEPARATE_LENGTH) {
+		} else if (fadt_info_table[i].flags & ACPI_FADT_SEPARATE_LENGTH) {
 			/*
 			 * Field is optional (Pm2_control, GPE0, GPE1) AND has its own
 			 * length field. If present, both the address and length must
@@ -726,7 +741,7 @@ static void acpi_tb_setup_fadt_registers(void)
 						     (fadt_pm_info_table[i].
 						      register_num *
 						      pm1_register_byte_width),
-						     "PmRegisters");
+						     "PmRegisters", 0);
 		}
 	}
 }
diff --git a/drivers/acpi/acpica/tbfind.c b/drivers/acpi/acpica/tbfind.c
index c12003947bd..cb947700206 100644
--- a/drivers/acpi/acpica/tbfind.c
+++ b/drivers/acpi/acpica/tbfind.c
@@ -99,8 +99,8 @@ acpi_tb_find_table(char *signature,
 			/* Table is not currently mapped, map it */
 
 			status =
-			    acpi_tb_verify_table(&acpi_gbl_root_table_list.
-						 tables[i]);
+			    acpi_tb_validate_table(&acpi_gbl_root_table_list.
+						   tables[i]);
 			if (ACPI_FAILURE(status)) {
 				return_ACPI_STATUS(status);
 			}
diff --git a/drivers/acpi/acpica/tbinstal.c b/drivers/acpi/acpica/tbinstal.c
index e3040947e9a..755b90c40dd 100644
--- a/drivers/acpi/acpica/tbinstal.c
+++ b/drivers/acpi/acpica/tbinstal.c
@@ -43,688 +43,483 @@
 
 #include <acpi/acpi.h>
 #include "accommon.h"
-#include "acnamesp.h"
 #include "actables.h"
 
 #define _COMPONENT          ACPI_TABLES
 ACPI_MODULE_NAME("tbinstal")
 
-/******************************************************************************
+/* Local prototypes */
+static u8
+acpi_tb_compare_tables(struct acpi_table_desc *table_desc, u32 table_index);
+
+/*******************************************************************************
  *
- * FUNCTION:    acpi_tb_verify_table
+ * FUNCTION:    acpi_tb_compare_tables
  *
- * PARAMETERS:  table_desc          - table
+ * PARAMETERS:  table_desc          - Table 1 descriptor to be compared
+ *              table_index         - Index of table 2 to be compared
  *
- * RETURN:      Status
+ * RETURN:      TRUE if both tables are identical.
  *
- * DESCRIPTION: this function is called to verify and map table
+ * DESCRIPTION: This function compares a table with another table that has
+ *              already been installed in the root table list.
  *
- *****************************************************************************/
-acpi_status acpi_tb_verify_table(struct acpi_table_desc *table_desc)
+ ******************************************************************************/
+
+static u8
+acpi_tb_compare_tables(struct acpi_table_desc *table_desc, u32 table_index)
 {
 	acpi_status status = AE_OK;
+	u8 is_identical;
+	struct acpi_table_header *table;
+	u32 table_length;
+	u8 table_flags;
 
-	ACPI_FUNCTION_TRACE(tb_verify_table);
-
-	/* Map the table if necessary */
-
-	if (!table_desc->pointer) {
-		if ((table_desc->flags & ACPI_TABLE_ORIGIN_MASK) ==
-		    ACPI_TABLE_ORIGIN_MAPPED) {
-			table_desc->pointer =
-			    acpi_os_map_memory(table_desc->address,
-					       table_desc->length);
-		}
-		if (!table_desc->pointer) {
-			return_ACPI_STATUS(AE_NO_MEMORY);
-		}
+	status =
+	    acpi_tb_acquire_table(&acpi_gbl_root_table_list.tables[table_index],
+				  &table, &table_length, &table_flags);
+	if (ACPI_FAILURE(status)) {
+		return (FALSE);
 	}
 
-	/* Always calculate checksum, ignore bad checksum if requested */
+	/*
+	 * Check for a table match on the entire table length,
+	 * not just the header.
+	 */
+	is_identical = (u8)((table_desc->length != table_length ||
+			     ACPI_MEMCMP(table_desc->pointer, table,
+					 table_length)) ? FALSE : TRUE);
 
-	status =
-	    acpi_tb_verify_checksum(table_desc->pointer, table_desc->length);
+	/* Release the acquired table */
 
-	return_ACPI_STATUS(status);
+	acpi_tb_release_table(table, table_length, table_flags);
+	return (is_identical);
 }
 
 /*******************************************************************************
  *
- * FUNCTION:    acpi_tb_add_table
+ * FUNCTION:    acpi_tb_install_table_with_override
  *
- * PARAMETERS:  table_desc          - Table descriptor
- *              table_index         - Where the table index is returned
+ * PARAMETERS:  table_index             - Index into root table array
+ *              new_table_desc          - New table descriptor to install
+ *              override                - Whether override should be performed
  *
- * RETURN:      Status
+ * RETURN:      None
  *
- * DESCRIPTION: This function is called to add an ACPI table. It is used to
- *              dynamically load tables via the Load and load_table AML
- *              operators.
+ * DESCRIPTION: Install an ACPI table into the global data structure. The
+ *              table override mechanism is called to allow the host
+ *              OS to replace any table before it is installed in the root
+ *              table array.
  *
  ******************************************************************************/
 
-acpi_status
-acpi_tb_add_table(struct acpi_table_desc *table_desc, u32 *table_index)
+void
+acpi_tb_install_table_with_override(u32 table_index,
+				    struct acpi_table_desc *new_table_desc,
+				    u8 override)
 {
-	u32 i;
-	acpi_status status = AE_OK;
 
-	ACPI_FUNCTION_TRACE(tb_add_table);
-
-	if (!table_desc->pointer) {
-		status = acpi_tb_verify_table(table_desc);
-		if (ACPI_FAILURE(status) || !table_desc->pointer) {
-			return_ACPI_STATUS(status);
-		}
+	if (table_index >= acpi_gbl_root_table_list.current_table_count) {
+		return;
 	}
 
 	/*
-	 * Validate the incoming table signature.
+	 * ACPI Table Override:
 	 *
-	 * 1) Originally, we checked the table signature for "SSDT" or "PSDT".
-	 * 2) We added support for OEMx tables, signature "OEM".
-	 * 3) Valid tables were encountered with a null signature, so we just
-	 *    gave up on validating the signature, (05/2008).
-	 * 4) We encountered non-AML tables such as the MADT, which caused
-	 *    interpreter errors and kernel faults. So now, we once again allow
-	 *    only "SSDT", "OEMx", and now, also a null signature. (05/2011).
+	 * Before we install the table, let the host OS override it with a new
+	 * one if desired. Any table within the RSDT/XSDT can be replaced,
+	 * including the DSDT which is pointed to by the FADT.
 	 */
-	if ((table_desc->pointer->signature[0] != 0x00) &&
-	    (!ACPI_COMPARE_NAME(table_desc->pointer->signature, ACPI_SIG_SSDT))
-	    && (ACPI_STRNCMP(table_desc->pointer->signature, "OEM", 3))) {
-		ACPI_BIOS_ERROR((AE_INFO,
-				 "Table has invalid signature [%4.4s] (0x%8.8X), "
-				 "must be SSDT or OEMx",
-				 acpi_ut_valid_acpi_name(table_desc->pointer->
-							 signature) ?
-				 table_desc->pointer->signature : "????",
-				 *(u32 *)table_desc->pointer->signature));
-
-		return_ACPI_STATUS(AE_BAD_SIGNATURE);
+	if (override) {
+		acpi_tb_override_table(new_table_desc);
 	}
 
-	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
+	acpi_tb_init_table_descriptor(&acpi_gbl_root_table_list.
+				      tables[table_index],
+				      new_table_desc->address,
+				      new_table_desc->flags,
+				      new_table_desc->pointer);
 
-	/* Check if table is already registered */
+	acpi_tb_print_table_header(new_table_desc->address,
+				   new_table_desc->pointer);
 
-	for (i = 0; i < acpi_gbl_root_table_list.current_table_count; ++i) {
-		if (!acpi_gbl_root_table_list.tables[i].pointer) {
-			status =
-			    acpi_tb_verify_table(&acpi_gbl_root_table_list.
-						 tables[i]);
-			if (ACPI_FAILURE(status)
-			    || !acpi_gbl_root_table_list.tables[i].pointer) {
-				continue;
-			}
-		}
+	/* Set the global integer width (based upon revision of the DSDT) */
 
-		/*
-		 * Check for a table match on the entire table length,
-		 * not just the header.
-		 */
-		if (table_desc->length !=
-		    acpi_gbl_root_table_list.tables[i].length) {
-			continue;
-		}
-
-		if (ACPI_MEMCMP(table_desc->pointer,
-				acpi_gbl_root_table_list.tables[i].pointer,
-				acpi_gbl_root_table_list.tables[i].length)) {
-			continue;
-		}
-
-		/*
-		 * Note: the current mechanism does not unregister a table if it is
-		 * dynamically unloaded. The related namespace entries are deleted,
-		 * but the table remains in the root table list.
-		 *
-		 * The assumption here is that the number of different tables that
-		 * will be loaded is actually small, and there is minimal overhead
-		 * in just keeping the table in case it is needed again.
-		 *
-		 * If this assumption changes in the future (perhaps on large
-		 * machines with many table load/unload operations), tables will
-		 * need to be unregistered when they are unloaded, and slots in the
-		 * root table list should be reused when empty.
-		 */
-
-		/*
-		 * Table is already registered.
-		 * We can delete the table that was passed as a parameter.
-		 */
-		acpi_tb_delete_table(table_desc);
-		*table_index = i;
-
-		if (acpi_gbl_root_table_list.tables[i].
-		    flags & ACPI_TABLE_IS_LOADED) {
-
-			/* Table is still loaded, this is an error */
-
-			status = AE_ALREADY_EXISTS;
-			goto release;
-		} else {
-			/* Table was unloaded, allow it to be reloaded */
-
-			table_desc->pointer =
-			    acpi_gbl_root_table_list.tables[i].pointer;
-			table_desc->address =
-			    acpi_gbl_root_table_list.tables[i].address;
-			status = AE_OK;
-			goto print_header;
-		}
+	if (table_index == ACPI_TABLE_INDEX_DSDT) {
+		acpi_ut_set_integer_width(new_table_desc->pointer->revision);
 	}
-
-	/*
-	 * ACPI Table Override:
-	 * Allow the host to override dynamically loaded tables.
-	 * NOTE: the table is fully mapped at this point, and the mapping will
-	 * be deleted by tb_table_override if the table is actually overridden.
-	 */
-	(void)acpi_tb_table_override(table_desc->pointer, table_desc);
-
-	/* Add the table to the global root table list */
-
-	status = acpi_tb_store_table(table_desc->address, table_desc->pointer,
-				     table_desc->length, table_desc->flags,
-				     table_index);
-	if (ACPI_FAILURE(status)) {
-		goto release;
-	}
-
-print_header:
-	acpi_tb_print_table_header(table_desc->address, table_desc->pointer);
-
-release:
-	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
-	return_ACPI_STATUS(status);
 }
 
 /*******************************************************************************
  *
- * FUNCTION:    acpi_tb_table_override
+ * FUNCTION:    acpi_tb_install_fixed_table
  *
- * PARAMETERS:  table_header        - Header for the original table
- *              table_desc          - Table descriptor initialized for the
- *                                    original table. May or may not be mapped.
+ * PARAMETERS:  address                 - Physical address of DSDT or FACS
+ *              signature               - Table signature, NULL if no need to
+ *                                        match
+ *              table_index             - Index into root table array
  *
- * RETURN:      Pointer to the entire new table. NULL if table not overridden.
- *              If overridden, installs the new table within the input table
- *              descriptor.
+ * RETURN:      Status
  *
- * DESCRIPTION: Attempt table override by calling the OSL override functions.
- *              Note: If the table is overridden, then the entire new table
- *              is mapped and returned by this function.
+ * DESCRIPTION: Install a fixed ACPI table (DSDT/FACS) into the global data
+ *              structure.
  *
  ******************************************************************************/
 
-struct acpi_table_header *acpi_tb_table_override(struct acpi_table_header
-						 *table_header,
-						 struct acpi_table_desc
-						 *table_desc)
+acpi_status
+acpi_tb_install_fixed_table(acpi_physical_address address,
+			    char *signature, u32 table_index)
 {
+	struct acpi_table_desc new_table_desc;
 	acpi_status status;
-	struct acpi_table_header *new_table = NULL;
-	acpi_physical_address new_address = 0;
-	u32 new_table_length = 0;
-	u8 new_flags;
-	char *override_type;
 
-	/* (1) Attempt logical override (returns a logical address) */
+	ACPI_FUNCTION_TRACE(tb_install_fixed_table);
 
-	status = acpi_os_table_override(table_header, &new_table);
-	if (ACPI_SUCCESS(status) && new_table) {
-		new_address = ACPI_PTR_TO_PHYSADDR(new_table);
-		new_table_length = new_table->length;
-		new_flags = ACPI_TABLE_ORIGIN_OVERRIDE;
-		override_type = "Logical";
-		goto finish_override;
+	if (!address) {
+		ACPI_ERROR((AE_INFO,
+			    "Null physical address for ACPI table [%s]",
+			    signature));
+		return (AE_NO_MEMORY);
 	}
 
-	/* (2) Attempt physical override (returns a physical address) */
+	/* Fill a table descriptor for validation */
 
-	status = acpi_os_physical_table_override(table_header,
-						 &new_address,
-						 &new_table_length);
-	if (ACPI_SUCCESS(status) && new_address && new_table_length) {
-
-		/* Map the entire new table */
-
-		new_table = acpi_os_map_memory(new_address, new_table_length);
-		if (!new_table) {
-			ACPI_EXCEPTION((AE_INFO, AE_NO_MEMORY,
-					"%4.4s " ACPI_PRINTF_UINT
-					" Attempted physical table override failed",
-					table_header->signature,
-					ACPI_FORMAT_TO_UINT(table_desc->
-							    address)));
-			return (NULL);
-		}
-
-		override_type = "Physical";
-		new_flags = ACPI_TABLE_ORIGIN_MAPPED;
-		goto finish_override;
+	status = acpi_tb_acquire_temp_table(&new_table_desc, address,
+					    ACPI_TABLE_ORIGIN_INTERNAL_PHYSICAL);
+	if (ACPI_FAILURE(status)) {
+		ACPI_ERROR((AE_INFO, "Could not acquire table length at %p",
+			    ACPI_CAST_PTR(void, address)));
+		return_ACPI_STATUS(status);
 	}
 
-	return (NULL);		/* There was no override */
-
-finish_override:
-
-	ACPI_INFO((AE_INFO, "%4.4s " ACPI_PRINTF_UINT
-		   " %s table override, new table: " ACPI_PRINTF_UINT,
-		   table_header->signature,
-		   ACPI_FORMAT_TO_UINT(table_desc->address),
-		   override_type, ACPI_FORMAT_TO_UINT(new_table)));
+	/* Validate and verify a table before installation */
 
-	/* We can now unmap/delete the original table (if fully mapped) */
+	status = acpi_tb_verify_temp_table(&new_table_desc, signature);
+	if (ACPI_FAILURE(status)) {
+		goto release_and_exit;
+	}
 
-	acpi_tb_delete_table(table_desc);
+	acpi_tb_install_table_with_override(table_index, &new_table_desc, TRUE);
 
-	/* Setup descriptor for the new table */
+release_and_exit:
 
-	table_desc->address = new_address;
-	table_desc->pointer = new_table;
-	table_desc->length = new_table_length;
-	table_desc->flags = new_flags;
+	/* Release the temporary table descriptor */
 
-	return (new_table);
+	acpi_tb_release_temp_table(&new_table_desc);
+	return_ACPI_STATUS(status);
 }
 
 /*******************************************************************************
  *
- * FUNCTION:    acpi_tb_resize_root_table_list
+ * FUNCTION:    acpi_tb_install_standard_table
  *
- * PARAMETERS:  None
+ * PARAMETERS:  address             - Address of the table (might be a virtual
+ *                                    address depending on the table_flags)
+ *              flags               - Flags for the table
+ *              reload              - Whether reload should be performed
+ *              override            - Whether override should be performed
+ *              table_index         - Where the table index is returned
  *
  * RETURN:      Status
  *
- * DESCRIPTION: Expand the size of global table array
+ * DESCRIPTION: This function is called to install an ACPI table that is
+ *              neither DSDT nor FACS (a "standard" table.)
+ *              When this function is called by "Load" or "LoadTable" opcodes,
+ *              or by acpi_load_table() API, the "Reload" parameter is set.
+ *              After sucessfully returning from this function, table is
+ *              "INSTALLED" but not "VALIDATED".
  *
  ******************************************************************************/
 
-acpi_status acpi_tb_resize_root_table_list(void)
+acpi_status
+acpi_tb_install_standard_table(acpi_physical_address address,
+			       u8 flags,
+			       u8 reload, u8 override, u32 *table_index)
 {
-	struct acpi_table_desc *tables;
-	u32 table_count;
-
-	ACPI_FUNCTION_TRACE(tb_resize_root_table_list);
-
-	/* allow_resize flag is a parameter to acpi_initialize_tables */
+	u32 i;
+	acpi_status status = AE_OK;
+	struct acpi_table_desc new_table_desc;
 
-	if (!(acpi_gbl_root_table_list.flags & ACPI_ROOT_ALLOW_RESIZE)) {
-		ACPI_ERROR((AE_INFO,
-			    "Resize of Root Table Array is not allowed"));
-		return_ACPI_STATUS(AE_SUPPORT);
-	}
+	ACPI_FUNCTION_TRACE(tb_install_standard_table);
 
-	/* Increase the Table Array size */
+	/* Acquire a temporary table descriptor for validation */
 
-	if (acpi_gbl_root_table_list.flags & ACPI_ROOT_ORIGIN_ALLOCATED) {
-		table_count = acpi_gbl_root_table_list.max_table_count;
-	} else {
-		table_count = acpi_gbl_root_table_list.current_table_count;
+	status = acpi_tb_acquire_temp_table(&new_table_desc, address, flags);
+	if (ACPI_FAILURE(status)) {
+		ACPI_ERROR((AE_INFO, "Could not acquire table length at %p",
+			    ACPI_CAST_PTR(void, address)));
+		return_ACPI_STATUS(status);
 	}
 
-	tables = ACPI_ALLOCATE_ZEROED(((acpi_size) table_count +
-				       ACPI_ROOT_TABLE_SIZE_INCREMENT) *
-				      sizeof(struct acpi_table_desc));
-	if (!tables) {
-		ACPI_ERROR((AE_INFO,
-			    "Could not allocate new root table array"));
-		return_ACPI_STATUS(AE_NO_MEMORY);
+	/*
+	 * Optionally do not load any SSDTs from the RSDT/XSDT. This can
+	 * be useful for debugging ACPI problems on some machines.
+	 */
+	if (!reload &&
+	    acpi_gbl_disable_ssdt_table_install &&
+	    ACPI_COMPARE_NAME(&new_table_desc.signature, ACPI_SIG_SSDT)) {
+		ACPI_INFO((AE_INFO, "Ignoring installation of %4.4s at %p",
+			   new_table_desc.signature.ascii, ACPI_CAST_PTR(void,
+									 address)));
+		goto release_and_exit;
 	}
 
-	/* Copy and free the previous table array */
-
-	if (acpi_gbl_root_table_list.tables) {
-		ACPI_MEMCPY(tables, acpi_gbl_root_table_list.tables,
-			    (acpi_size) table_count *
-			    sizeof(struct acpi_table_desc));
+	/* Validate and verify a table before installation */
 
-		if (acpi_gbl_root_table_list.flags & ACPI_ROOT_ORIGIN_ALLOCATED) {
-			ACPI_FREE(acpi_gbl_root_table_list.tables);
-		}
+	status = acpi_tb_verify_temp_table(&new_table_desc, NULL);
+	if (ACPI_FAILURE(status)) {
+		goto release_and_exit;
 	}
 
-	acpi_gbl_root_table_list.tables = tables;
-	acpi_gbl_root_table_list.max_table_count =
-	    table_count + ACPI_ROOT_TABLE_SIZE_INCREMENT;
-	acpi_gbl_root_table_list.flags |= ACPI_ROOT_ORIGIN_ALLOCATED;
-
-	return_ACPI_STATUS(AE_OK);
-}
-
-/*******************************************************************************
- *
- * FUNCTION:    acpi_tb_store_table
- *
- * PARAMETERS:  address             - Table address
- *              table               - Table header
- *              length              - Table length
- *              flags               - flags
- *
- * RETURN:      Status and table index.
- *
- * DESCRIPTION: Add an ACPI table to the global table list
- *
- ******************************************************************************/
+	if (reload) {
+		/*
+		 * Validate the incoming table signature.
+		 *
+		 * 1) Originally, we checked the table signature for "SSDT" or "PSDT".
+		 * 2) We added support for OEMx tables, signature "OEM".
+		 * 3) Valid tables were encountered with a null signature, so we just
+		 *    gave up on validating the signature, (05/2008).
+		 * 4) We encountered non-AML tables such as the MADT, which caused
+		 *    interpreter errors and kernel faults. So now, we once again allow
+		 *    only "SSDT", "OEMx", and now, also a null signature. (05/2011).
+		 */
+		if ((new_table_desc.signature.ascii[0] != 0x00) &&
+		    (!ACPI_COMPARE_NAME
+		     (&new_table_desc.signature, ACPI_SIG_SSDT))
+		    && (ACPI_STRNCMP(new_table_desc.signature.ascii, "OEM", 3)))
+		{
+			ACPI_BIOS_ERROR((AE_INFO,
+					 "Table has invalid signature [%4.4s] (0x%8.8X), "
+					 "must be SSDT or OEMx",
+					 acpi_ut_valid_acpi_name(new_table_desc.
+								 signature.
+								 ascii) ?
+					 new_table_desc.signature.
+					 ascii : "????",
+					 new_table_desc.signature.integer));
+
+			status = AE_BAD_SIGNATURE;
+			goto release_and_exit;
+		}
 
-acpi_status
-acpi_tb_store_table(acpi_physical_address address,
-		    struct acpi_table_header *table,
-		    u32 length, u8 flags, u32 *table_index)
-{
-	acpi_status status;
-	struct acpi_table_desc *new_table;
+		/* Check if table is already registered */
 
-	/* Ensure that there is room for the table in the Root Table List */
+		for (i = 0; i < acpi_gbl_root_table_list.current_table_count;
+		     ++i) {
+			/*
+			 * Check for a table match on the entire table length,
+			 * not just the header.
+			 */
+			if (!acpi_tb_compare_tables(&new_table_desc, i)) {
+				continue;
+			}
 
-	if (acpi_gbl_root_table_list.current_table_count >=
-	    acpi_gbl_root_table_list.max_table_count) {
-		status = acpi_tb_resize_root_table_list();
-		if (ACPI_FAILURE(status)) {
-			return (status);
+			/*
+			 * Note: the current mechanism does not unregister a table if it is
+			 * dynamically unloaded. The related namespace entries are deleted,
+			 * but the table remains in the root table list.
+			 *
+			 * The assumption here is that the number of different tables that
+			 * will be loaded is actually small, and there is minimal overhead
+			 * in just keeping the table in case it is needed again.
+			 *
+			 * If this assumption changes in the future (perhaps on large
+			 * machines with many table load/unload operations), tables will
+			 * need to be unregistered when they are unloaded, and slots in the
+			 * root table list should be reused when empty.
+			 */
+			if (acpi_gbl_root_table_list.tables[i].
+			    flags & ACPI_TABLE_IS_LOADED) {
+
+				/* Table is still loaded, this is an error */
+
+				status = AE_ALREADY_EXISTS;
+				goto release_and_exit;
+			} else {
+				/*
+				 * Table was unloaded, allow it to be reloaded.
+				 * As we are going to return AE_OK to the caller, we should
+				 * take the responsibility of freeing the input descriptor.
+				 * Refill the input descriptor to ensure
+				 * acpi_tb_install_table_with_override() can be called again to
+				 * indicate the re-installation.
+				 */
+				acpi_tb_uninstall_table(&new_table_desc);
+				*table_index = i;
+				(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
+				return_ACPI_STATUS(AE_OK);
+			}
 		}
 	}
 
-	new_table =
-	    &acpi_gbl_root_table_list.tables[acpi_gbl_root_table_list.
-					     current_table_count];
-
-	/* Initialize added table */
-
-	new_table->address = address;
-	new_table->pointer = table;
-	new_table->length = length;
-	new_table->owner_id = 0;
-	new_table->flags = flags;
-
-	ACPI_MOVE_32_TO_32(&new_table->signature, table->signature);
-
-	*table_index = acpi_gbl_root_table_list.current_table_count;
-	acpi_gbl_root_table_list.current_table_count++;
-	return (AE_OK);
-}
-
-/*******************************************************************************
- *
- * FUNCTION:    acpi_tb_delete_table
- *
- * PARAMETERS:  table_index         - Table index
- *
- * RETURN:      None
- *
- * DESCRIPTION: Delete one internal ACPI table
- *
- ******************************************************************************/
+	/* Add the table to the global root table list */
 
-void acpi_tb_delete_table(struct acpi_table_desc *table_desc)
-{
-	/* Table must be mapped or allocated */
-	if (!table_desc->pointer) {
-		return;
+	status = acpi_tb_get_next_root_index(&i);
+	if (ACPI_FAILURE(status)) {
+		goto release_and_exit;
 	}
-	switch (table_desc->flags & ACPI_TABLE_ORIGIN_MASK) {
-	case ACPI_TABLE_ORIGIN_MAPPED:
-
-		acpi_os_unmap_memory(table_desc->pointer, table_desc->length);
-		break;
-
-	case ACPI_TABLE_ORIGIN_ALLOCATED:
 
-		ACPI_FREE(table_desc->pointer);
-		break;
+	*table_index = i;
+	acpi_tb_install_table_with_override(i, &new_table_desc, override);
 
-		/* Not mapped or allocated, there is nothing we can do */
+release_and_exit:
 
-	default:
+	/* Release the temporary table descriptor */
 
-		return;
-	}
-
-	table_desc->pointer = NULL;
+	acpi_tb_release_temp_table(&new_table_desc);
+	return_ACPI_STATUS(status);
 }
 
 /*******************************************************************************
  *
- * FUNCTION:    acpi_tb_terminate
+ * FUNCTION:    acpi_tb_override_table
  *
- * PARAMETERS:  None
+ * PARAMETERS:  old_table_desc      - Validated table descriptor to be
+ *                                    overridden
  *
  * RETURN:      None
  *
- * DESCRIPTION: Delete all internal ACPI tables
+ * DESCRIPTION: Attempt table override by calling the OSL override functions.
+ *              Note: If the table is overridden, then the entire new table
+ *              is acquired and returned by this function.
+ *              Before/after invocation, the table descriptor is in a state
+ *              that is "VALIDATED".
  *
  ******************************************************************************/
 
-void acpi_tb_terminate(void)
+void acpi_tb_override_table(struct acpi_table_desc *old_table_desc)
 {
-	u32 i;
-
-	ACPI_FUNCTION_TRACE(tb_terminate);
-
-	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
-
-	/* Delete the individual tables */
+	acpi_status status;
+	char *override_type;
+	struct acpi_table_desc new_table_desc;
+	struct acpi_table_header *table;
+	acpi_physical_address address;
+	u32 length;
 
-	for (i = 0; i < acpi_gbl_root_table_list.current_table_count; i++) {
-		acpi_tb_delete_table(&acpi_gbl_root_table_list.tables[i]);
-	}
+	/* (1) Attempt logical override (returns a logical address) */
 
-	/*
-	 * Delete the root table array if allocated locally. Array cannot be
-	 * mapped, so we don't need to check for that flag.
-	 */
-	if (acpi_gbl_root_table_list.flags & ACPI_ROOT_ORIGIN_ALLOCATED) {
-		ACPI_FREE(acpi_gbl_root_table_list.tables);
+	status = acpi_os_table_override(old_table_desc->pointer, &table);
+	if (ACPI_SUCCESS(status) && table) {
+		acpi_tb_acquire_temp_table(&new_table_desc,
+					   ACPI_PTR_TO_PHYSADDR(table),
+					   ACPI_TABLE_ORIGIN_EXTERNAL_VIRTUAL);
+		override_type = "Logical";
+		goto finish_override;
 	}
 
-	acpi_gbl_root_table_list.tables = NULL;
-	acpi_gbl_root_table_list.flags = 0;
-	acpi_gbl_root_table_list.current_table_count = 0;
+	/* (2) Attempt physical override (returns a physical address) */
 
-	ACPI_DEBUG_PRINT((ACPI_DB_INFO, "ACPI Tables freed\n"));
-	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
+	status = acpi_os_physical_table_override(old_table_desc->pointer,
+						 &address, &length);
+	if (ACPI_SUCCESS(status) && address && length) {
+		acpi_tb_acquire_temp_table(&new_table_desc, address,
+					   ACPI_TABLE_ORIGIN_INTERNAL_PHYSICAL);
+		override_type = "Physical";
+		goto finish_override;
+	}
 
-	return_VOID;
-}
+	return;			/* There was no override */
 
-/*******************************************************************************
- *
- * FUNCTION:    acpi_tb_delete_namespace_by_owner
- *
- * PARAMETERS:  table_index         - Table index
- *
- * RETURN:      Status
- *
- * DESCRIPTION: Delete all namespace objects created when this table was loaded.
- *
- ******************************************************************************/
-
-acpi_status acpi_tb_delete_namespace_by_owner(u32 table_index)
-{
-	acpi_owner_id owner_id;
-	acpi_status status;
+finish_override:
 
-	ACPI_FUNCTION_TRACE(tb_delete_namespace_by_owner);
+	/* Validate and verify a table before overriding */
 
-	status = acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
+	status = acpi_tb_verify_temp_table(&new_table_desc, NULL);
 	if (ACPI_FAILURE(status)) {
-		return_ACPI_STATUS(status);
+		return;
 	}
 
-	if (table_index >= acpi_gbl_root_table_list.current_table_count) {
-
-		/* The table index does not exist */
-
-		(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
-		return_ACPI_STATUS(AE_NOT_EXIST);
-	}
+	ACPI_INFO((AE_INFO, "%4.4s " ACPI_PRINTF_UINT
+		   " %s table override, new table: " ACPI_PRINTF_UINT,
+		   old_table_desc->signature.ascii,
+		   ACPI_FORMAT_TO_UINT(old_table_desc->address),
+		   override_type, ACPI_FORMAT_TO_UINT(new_table_desc.address)));
 
-	/* Get the owner ID for this table, used to delete namespace nodes */
+	/* We can now uninstall the original table */
 
-	owner_id = acpi_gbl_root_table_list.tables[table_index].owner_id;
-	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
+	acpi_tb_uninstall_table(old_table_desc);
 
 	/*
-	 * Need to acquire the namespace writer lock to prevent interference
-	 * with any concurrent namespace walks. The interpreter must be
-	 * released during the deletion since the acquisition of the deletion
-	 * lock may block, and also since the execution of a namespace walk
-	 * must be allowed to use the interpreter.
+	 * Replace the original table descriptor and keep its state as
+	 * "VALIDATED".
 	 */
-	(void)acpi_ut_release_mutex(ACPI_MTX_INTERPRETER);
-	status = acpi_ut_acquire_write_lock(&acpi_gbl_namespace_rw_lock);
+	acpi_tb_init_table_descriptor(old_table_desc, new_table_desc.address,
+				      new_table_desc.flags,
+				      new_table_desc.pointer);
+	acpi_tb_validate_temp_table(old_table_desc);
 
-	acpi_ns_delete_namespace_by_owner(owner_id);
-	if (ACPI_FAILURE(status)) {
-		return_ACPI_STATUS(status);
-	}
+	/* Release the temporary table descriptor */
 
-	acpi_ut_release_write_lock(&acpi_gbl_namespace_rw_lock);
-
-	status = acpi_ut_acquire_mutex(ACPI_MTX_INTERPRETER);
-	return_ACPI_STATUS(status);
+	acpi_tb_release_temp_table(&new_table_desc);
 }
 
 /*******************************************************************************
  *
- * FUNCTION:    acpi_tb_allocate_owner_id
+ * FUNCTION:    acpi_tb_store_table
  *
- * PARAMETERS:  table_index         - Table index
+ * PARAMETERS:  address             - Table address
+ *              table               - Table header
+ *              length              - Table length
+ *              flags               - Install flags
+ *              table_index         - Where the table index is returned
  *
- * RETURN:      Status
+ * RETURN:      Status and table index.
  *
- * DESCRIPTION: Allocates owner_id in table_desc
+ * DESCRIPTION: Add an ACPI table to the global table list
  *
  ******************************************************************************/
 
-acpi_status acpi_tb_allocate_owner_id(u32 table_index)
+acpi_status
+acpi_tb_store_table(acpi_physical_address address,
+		    struct acpi_table_header * table,
+		    u32 length, u8 flags, u32 *table_index)
 {
-	acpi_status status = AE_BAD_PARAMETER;
-
-	ACPI_FUNCTION_TRACE(tb_allocate_owner_id);
+	acpi_status status;
+	struct acpi_table_desc *table_desc;
 
-	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
-	if (table_index < acpi_gbl_root_table_list.current_table_count) {
-		status = acpi_ut_allocate_owner_id
-		    (&(acpi_gbl_root_table_list.tables[table_index].owner_id));
+	status = acpi_tb_get_next_root_index(table_index);
+	if (ACPI_FAILURE(status)) {
+		return (status);
 	}
 
-	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
-	return_ACPI_STATUS(status);
-}
-
-/*******************************************************************************
- *
- * FUNCTION:    acpi_tb_release_owner_id
- *
- * PARAMETERS:  table_index         - Table index
- *
- * RETURN:      Status
- *
- * DESCRIPTION: Releases owner_id in table_desc
- *
- ******************************************************************************/
-
-acpi_status acpi_tb_release_owner_id(u32 table_index)
-{
-	acpi_status status = AE_BAD_PARAMETER;
-
-	ACPI_FUNCTION_TRACE(tb_release_owner_id);
-
-	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
-	if (table_index < acpi_gbl_root_table_list.current_table_count) {
-		acpi_ut_release_owner_id(&
-					 (acpi_gbl_root_table_list.
-					  tables[table_index].owner_id));
-		status = AE_OK;
-	}
+	/* Initialize added table */
 
-	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
-	return_ACPI_STATUS(status);
+	table_desc = &acpi_gbl_root_table_list.tables[*table_index];
+	acpi_tb_init_table_descriptor(table_desc, address, flags, table);
+	table_desc->pointer = table;
+	return (AE_OK);
 }
 
 /*******************************************************************************
  *
- * FUNCTION:    acpi_tb_get_owner_id
+ * FUNCTION:    acpi_tb_uninstall_table
  *
- * PARAMETERS:  table_index         - Table index
- *              owner_id            - Where the table owner_id is returned
+ * PARAMETERS:  table_desc          - Table descriptor
  *
- * RETURN:      Status
+ * RETURN:      None
  *
- * DESCRIPTION: returns owner_id for the ACPI table
+ * DESCRIPTION: Delete one internal ACPI table
  *
  ******************************************************************************/
 
-acpi_status acpi_tb_get_owner_id(u32 table_index, acpi_owner_id *owner_id)
+void acpi_tb_uninstall_table(struct acpi_table_desc *table_desc)
 {
-	acpi_status status = AE_BAD_PARAMETER;
-
-	ACPI_FUNCTION_TRACE(tb_get_owner_id);
 
-	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
-	if (table_index < acpi_gbl_root_table_list.current_table_count) {
-		*owner_id =
-		    acpi_gbl_root_table_list.tables[table_index].owner_id;
-		status = AE_OK;
-	}
+	ACPI_FUNCTION_TRACE(tb_uninstall_table);
 
-	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
-	return_ACPI_STATUS(status);
-}
-
-/*******************************************************************************
- *
- * FUNCTION:    acpi_tb_is_table_loaded
- *
- * PARAMETERS:  table_index         - Table index
- *
- * RETURN:      Table Loaded Flag
- *
- ******************************************************************************/
+	/* Table must be installed */
 
-u8 acpi_tb_is_table_loaded(u32 table_index)
-{
-	u8 is_loaded = FALSE;
-
-	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
-	if (table_index < acpi_gbl_root_table_list.current_table_count) {
-		is_loaded = (u8)
-		    (acpi_gbl_root_table_list.tables[table_index].flags &
-		     ACPI_TABLE_IS_LOADED);
+	if (!table_desc->address) {
+		return_VOID;
 	}
 
-	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
-	return (is_loaded);
-}
-
-/*******************************************************************************
- *
- * FUNCTION:    acpi_tb_set_table_loaded_flag
- *
- * PARAMETERS:  table_index         - Table index
- *              is_loaded           - TRUE if table is loaded, FALSE otherwise
- *
- * RETURN:      None
- *
- * DESCRIPTION: Sets the table loaded flag to either TRUE or FALSE.
- *
- ******************************************************************************/
-
-void acpi_tb_set_table_loaded_flag(u32 table_index, u8 is_loaded)
-{
+	acpi_tb_invalidate_table(table_desc);
 
-	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
-	if (table_index < acpi_gbl_root_table_list.current_table_count) {
-		if (is_loaded) {
-			acpi_gbl_root_table_list.tables[table_index].flags |=
-			    ACPI_TABLE_IS_LOADED;
-		} else {
-			acpi_gbl_root_table_list.tables[table_index].flags &=
-			    ~ACPI_TABLE_IS_LOADED;
-		}
+	if ((table_desc->flags & ACPI_TABLE_ORIGIN_MASK) ==
+	    ACPI_TABLE_ORIGIN_INTERNAL_VIRTUAL) {
+		ACPI_FREE(ACPI_CAST_PTR(void, table_desc->address));
 	}
 
-	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
+	table_desc->address = ACPI_PTR_TO_PHYSADDR(NULL);
+	return_VOID;
 }
diff --git a/drivers/acpi/acpica/tbutils.c b/drivers/acpi/acpica/tbutils.c
index a4702eee91a..6b1ca9991b9 100644
--- a/drivers/acpi/acpica/tbutils.c
+++ b/drivers/acpi/acpica/tbutils.c
@@ -49,8 +49,6 @@
 ACPI_MODULE_NAME("tbutils")
 
 /* Local prototypes */
-static acpi_status acpi_tb_validate_xsdt(acpi_physical_address address);
-
 static acpi_physical_address
 acpi_tb_get_root_table_entry(u8 *table_entry, u32 table_entry_size);
 
@@ -178,9 +176,13 @@ struct acpi_table_header *acpi_tb_copy_dsdt(u32 table_index)
 	}
 
 	ACPI_MEMCPY(new_table, table_desc->pointer, table_desc->length);
-	acpi_tb_delete_table(table_desc);
-	table_desc->pointer = new_table;
-	table_desc->flags = ACPI_TABLE_ORIGIN_ALLOCATED;
+	acpi_tb_uninstall_table(table_desc);
+
+	acpi_tb_init_table_descriptor(&acpi_gbl_root_table_list.
+				      tables[ACPI_TABLE_INDEX_DSDT],
+				      ACPI_PTR_TO_PHYSADDR(new_table),
+				      ACPI_TABLE_ORIGIN_INTERNAL_VIRTUAL,
+				      new_table);
 
 	ACPI_INFO((AE_INFO,
 		   "Forced DSDT copy: length 0x%05X copied locally, original unmapped",
@@ -191,116 +193,6 @@ struct acpi_table_header *acpi_tb_copy_dsdt(u32 table_index)
 
 /*******************************************************************************
  *
- * FUNCTION:    acpi_tb_install_table
- *
- * PARAMETERS:  address                 - Physical address of DSDT or FACS
- *              signature               - Table signature, NULL if no need to
- *                                        match
- *              table_index             - Index into root table array
- *
- * RETURN:      None
- *
- * DESCRIPTION: Install an ACPI table into the global data structure. The
- *              table override mechanism is called to allow the host
- *              OS to replace any table before it is installed in the root
- *              table array.
- *
- ******************************************************************************/
-
-void
-acpi_tb_install_table(acpi_physical_address address,
-		      char *signature, u32 table_index)
-{
-	struct acpi_table_header *table;
-	struct acpi_table_header *final_table;
-	struct acpi_table_desc *table_desc;
-
-	if (!address) {
-		ACPI_ERROR((AE_INFO,
-			    "Null physical address for ACPI table [%s]",
-			    signature));
-		return;
-	}
-
-	/* Map just the table header */
-
-	table = acpi_os_map_memory(address, sizeof(struct acpi_table_header));
-	if (!table) {
-		ACPI_ERROR((AE_INFO,
-			    "Could not map memory for table [%s] at %p",
-			    signature, ACPI_CAST_PTR(void, address)));
-		return;
-	}
-
-	/* If a particular signature is expected (DSDT/FACS), it must match */
-
-	if (signature && !ACPI_COMPARE_NAME(table->signature, signature)) {
-		ACPI_BIOS_ERROR((AE_INFO,
-				 "Invalid signature 0x%X for ACPI table, expected [%s]",
-				 *ACPI_CAST_PTR(u32, table->signature),
-				 signature));
-		goto unmap_and_exit;
-	}
-
-	/*
-	 * Initialize the table entry. Set the pointer to NULL, since the
-	 * table is not fully mapped at this time.
-	 */
-	table_desc = &acpi_gbl_root_table_list.tables[table_index];
-
-	table_desc->address = address;
-	table_desc->pointer = NULL;
-	table_desc->length = table->length;
-	table_desc->flags = ACPI_TABLE_ORIGIN_MAPPED;
-	ACPI_MOVE_32_TO_32(table_desc->signature.ascii, table->signature);
-
-	/*
-	 * ACPI Table Override:
-	 *
-	 * Before we install the table, let the host OS override it with a new
-	 * one if desired. Any table within the RSDT/XSDT can be replaced,
-	 * including the DSDT which is pointed to by the FADT.
-	 *
-	 * NOTE: If the table is overridden, then final_table will contain a
-	 * mapped pointer to the full new table. If the table is not overridden,
-	 * or if there has been a physical override, then the table will be
-	 * fully mapped later (in verify table). In any case, we must
-	 * unmap the header that was mapped above.
-	 */
-	final_table = acpi_tb_table_override(table, table_desc);
-	if (!final_table) {
-		final_table = table;	/* There was no override */
-	}
-
-	acpi_tb_print_table_header(table_desc->address, final_table);
-
-	/* Set the global integer width (based upon revision of the DSDT) */
-
-	if (table_index == ACPI_TABLE_INDEX_DSDT) {
-		acpi_ut_set_integer_width(final_table->revision);
-	}
-
-	/*
-	 * If we have a physical override during this early loading of the ACPI
-	 * tables, unmap the table for now. It will be mapped again later when
-	 * it is actually used. This supports very early loading of ACPI tables,
-	 * before virtual memory is fully initialized and running within the
-	 * host OS. Note: A logical override has the ACPI_TABLE_ORIGIN_OVERRIDE
-	 * flag set and will not be deleted below.
-	 */
-	if (final_table != table) {
-		acpi_tb_delete_table(table_desc);
-	}
-
-unmap_and_exit:
-
-	/* Always unmap the table header that we mapped above */
-
-	acpi_os_unmap_memory(table, sizeof(struct acpi_table_header));
-}
-
-/*******************************************************************************
- *
  * FUNCTION:    acpi_tb_get_root_table_entry
  *
  * PARAMETERS:  table_entry         - Pointer to the RSDT/XSDT table entry
@@ -357,87 +249,6 @@ acpi_tb_get_root_table_entry(u8 *table_entry, u32 table_entry_size)
 
 /*******************************************************************************
  *
- * FUNCTION:    acpi_tb_validate_xsdt
- *
- * PARAMETERS:  address             - Physical address of the XSDT (from RSDP)
- *
- * RETURN:      Status. AE_OK if the table appears to be valid.
- *
- * DESCRIPTION: Validate an XSDT to ensure that it is of minimum size and does
- *              not contain any NULL entries. A problem that is seen in the
- *              field is that the XSDT exists, but is actually useless because
- *              of one or more (or all) NULL entries.
- *
- ******************************************************************************/
-
-static acpi_status acpi_tb_validate_xsdt(acpi_physical_address xsdt_address)
-{
-	struct acpi_table_header *table;
-	u8 *next_entry;
-	acpi_physical_address address;
-	u32 length;
-	u32 entry_count;
-	acpi_status status;
-	u32 i;
-
-	/* Get the XSDT length */
-
-	table =
-	    acpi_os_map_memory(xsdt_address, sizeof(struct acpi_table_header));
-	if (!table) {
-		return (AE_NO_MEMORY);
-	}
-
-	length = table->length;
-	acpi_os_unmap_memory(table, sizeof(struct acpi_table_header));
-
-	/*
-	 * Minimum XSDT length is the size of the standard ACPI header
-	 * plus one physical address entry
-	 */
-	if (length < (sizeof(struct acpi_table_header) + ACPI_XSDT_ENTRY_SIZE)) {
-		return (AE_INVALID_TABLE_LENGTH);
-	}
-
-	/* Map the entire XSDT */
-
-	table = acpi_os_map_memory(xsdt_address, length);
-	if (!table) {
-		return (AE_NO_MEMORY);
-	}
-
-	/* Get the number of entries and pointer to first entry */
-
-	status = AE_OK;
-	next_entry = ACPI_ADD_PTR(u8, table, sizeof(struct acpi_table_header));
-	entry_count = (u32)((table->length - sizeof(struct acpi_table_header)) /
-			    ACPI_XSDT_ENTRY_SIZE);
-
-	/* Validate each entry (physical address) within the XSDT */
-
-	for (i = 0; i < entry_count; i++) {
-		address =
-		    acpi_tb_get_root_table_entry(next_entry,
-						 ACPI_XSDT_ENTRY_SIZE);
-		if (!address) {
-
-			/* Detected a NULL entry, XSDT is invalid */
-
-			status = AE_NULL_ENTRY;
-			break;
-		}
-
-		next_entry += ACPI_XSDT_ENTRY_SIZE;
-	}
-
-	/* Unmap table */
-
-	acpi_os_unmap_memory(table, length);
-	return (status);
-}
-
-/*******************************************************************************
- *
  * FUNCTION:    acpi_tb_parse_root_table
  *
  * PARAMETERS:  rsdp                    - Pointer to the RSDP
@@ -464,6 +275,7 @@ acpi_status __init acpi_tb_parse_root_table(acpi_physical_address rsdp_address)
 	u32 length;
 	u8 *table_entry;
 	acpi_status status;
+	u32 table_index;
 
 	ACPI_FUNCTION_TRACE(tb_parse_root_table);
 
@@ -502,25 +314,6 @@ acpi_status __init acpi_tb_parse_root_table(acpi_physical_address rsdp_address)
 	 */
 	acpi_os_unmap_memory(rsdp, sizeof(struct acpi_table_rsdp));
 
-	/*
-	 * If it is present and used, validate the XSDT for access/size
-	 * and ensure that all table entries are at least non-NULL
-	 */
-	if (table_entry_size == ACPI_XSDT_ENTRY_SIZE) {
-		status = acpi_tb_validate_xsdt(address);
-		if (ACPI_FAILURE(status)) {
-			ACPI_BIOS_WARNING((AE_INFO,
-					   "XSDT is invalid (%s), using RSDT",
-					   acpi_format_exception(status)));
-
-			/* Fall back to the RSDT */
-
-			address =
-			    (acpi_physical_address) rsdp->rsdt_physical_address;
-			table_entry_size = ACPI_RSDT_ENTRY_SIZE;
-		}
-	}
-
 	/* Map the RSDT/XSDT table header to get the full table length */
 
 	table = acpi_os_map_memory(address, sizeof(struct acpi_table_header));
@@ -573,55 +366,36 @@ acpi_status __init acpi_tb_parse_root_table(acpi_physical_address rsdp_address)
 	/* Initialize the root table array from the RSDT/XSDT */
 
 	for (i = 0; i < table_count; i++) {
-		if (acpi_gbl_root_table_list.current_table_count >=
-		    acpi_gbl_root_table_list.max_table_count) {
-
-			/* There is no more room in the root table array, attempt resize */
-
-			status = acpi_tb_resize_root_table_list();
-			if (ACPI_FAILURE(status)) {
-				ACPI_WARNING((AE_INFO,
-					      "Truncating %u table entries!",
-					      (unsigned) (table_count -
-					       (acpi_gbl_root_table_list.
-							  current_table_count -
-							  2))));
-				break;
-			}
-		}
 
 		/* Get the table physical address (32-bit for RSDT, 64-bit for XSDT) */
 
-		acpi_gbl_root_table_list.tables[acpi_gbl_root_table_list.
-						current_table_count].address =
+		address =
 		    acpi_tb_get_root_table_entry(table_entry, table_entry_size);
 
-		table_entry += table_entry_size;
-		acpi_gbl_root_table_list.current_table_count++;
-	}
-
-	/*
-	 * It is not possible to map more than one entry in some environments,
-	 * so unmap the root table here before mapping other tables
-	 */
-	acpi_os_unmap_memory(table, length);
+		/* Skip NULL entries in RSDT/XSDT */
 
-	/*
-	 * Complete the initialization of the root table array by examining
-	 * the header of each table
-	 */
-	for (i = 2; i < acpi_gbl_root_table_list.current_table_count; i++) {
-		acpi_tb_install_table(acpi_gbl_root_table_list.tables[i].
-				      address, NULL, i);
+		if (!address) {
+			goto next_table;
+		}
 
-		/* Special case for FADT - validate it then get the DSDT and FACS */
+		status = acpi_tb_install_standard_table(address,
+							ACPI_TABLE_ORIGIN_INTERNAL_PHYSICAL,
+							FALSE, TRUE,
+							&table_index);
 
-		if (ACPI_COMPARE_NAME
-		    (&acpi_gbl_root_table_list.tables[i].signature,
-		     ACPI_SIG_FADT)) {
-			acpi_tb_parse_fadt(i);
+		if (ACPI_SUCCESS(status) &&
+		    ACPI_COMPARE_NAME(&acpi_gbl_root_table_list.
+				      tables[table_index].signature,
+				      ACPI_SIG_FADT)) {
+			acpi_tb_parse_fadt(table_index);
 		}
+
+next_table:
+
+		table_entry += table_entry_size;
 	}
 
+	acpi_os_unmap_memory(table, length);
+
 	return_ACPI_STATUS(AE_OK);
 }
diff --git a/drivers/acpi/acpica/tbxface.c b/drivers/acpi/acpica/tbxface.c
index a1593159d9e..6482b0ded65 100644
--- a/drivers/acpi/acpica/tbxface.c
+++ b/drivers/acpi/acpica/tbxface.c
@@ -206,8 +206,8 @@ acpi_status
 acpi_get_table_header(char *signature,
 		      u32 instance, struct acpi_table_header *out_table_header)
 {
-       u32 i;
-       u32 j;
+	u32 i;
+	u32 j;
 	struct acpi_table_header *header;
 
 	/* Parameter validation */
@@ -233,7 +233,7 @@ acpi_get_table_header(char *signature,
 		if (!acpi_gbl_root_table_list.tables[i].pointer) {
 			if ((acpi_gbl_root_table_list.tables[i].flags &
 			     ACPI_TABLE_ORIGIN_MASK) ==
-			    ACPI_TABLE_ORIGIN_MAPPED) {
+			    ACPI_TABLE_ORIGIN_INTERNAL_PHYSICAL) {
 				header =
 				    acpi_os_map_memory(acpi_gbl_root_table_list.
 						       tables[i].address,
@@ -321,8 +321,8 @@ acpi_get_table_with_size(char *signature,
 	       u32 instance, struct acpi_table_header **out_table,
 	       acpi_size *tbl_size)
 {
-       u32 i;
-       u32 j;
+	u32 i;
+	u32 j;
 	acpi_status status;
 
 	/* Parameter validation */
@@ -346,7 +346,7 @@ acpi_get_table_with_size(char *signature,
 		}
 
 		status =
-		    acpi_tb_verify_table(&acpi_gbl_root_table_list.tables[i]);
+		    acpi_tb_validate_table(&acpi_gbl_root_table_list.tables[i]);
 		if (ACPI_SUCCESS(status)) {
 			*out_table = acpi_gbl_root_table_list.tables[i].pointer;
 			*tbl_size = acpi_gbl_root_table_list.tables[i].length;
@@ -390,7 +390,7 @@ ACPI_EXPORT_SYMBOL(acpi_get_table)
  *
  ******************************************************************************/
 acpi_status
-acpi_get_table_by_index(u32 table_index, struct acpi_table_header **table)
+acpi_get_table_by_index(u32 table_index, struct acpi_table_header ** table)
 {
 	acpi_status status;
 
@@ -416,8 +416,8 @@ acpi_get_table_by_index(u32 table_index, struct acpi_table_header **table)
 		/* Table is not mapped, map it */
 
 		status =
-		    acpi_tb_verify_table(&acpi_gbl_root_table_list.
-					 tables[table_index]);
+		    acpi_tb_validate_table(&acpi_gbl_root_table_list.
+					   tables[table_index]);
 		if (ACPI_FAILURE(status)) {
 			(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
 			return_ACPI_STATUS(status);
diff --git a/drivers/acpi/acpica/tbxfload.c b/drivers/acpi/acpica/tbxfload.c
index 0909420fc77..ab5308b81aa 100644
--- a/drivers/acpi/acpica/tbxfload.c
+++ b/drivers/acpi/acpica/tbxfload.c
@@ -117,7 +117,7 @@ static acpi_status acpi_tb_load_namespace(void)
 				tables[ACPI_TABLE_INDEX_DSDT].signature),
 			       ACPI_SIG_DSDT)
 	    ||
-	    ACPI_FAILURE(acpi_tb_verify_table
+	    ACPI_FAILURE(acpi_tb_validate_table
 			 (&acpi_gbl_root_table_list.
 			  tables[ACPI_TABLE_INDEX_DSDT]))) {
 		status = AE_NO_ACPI_TABLES;
@@ -128,7 +128,7 @@ static acpi_status acpi_tb_load_namespace(void)
 	 * Save the DSDT pointer for simple access. This is the mapped memory
 	 * address. We must take care here because the address of the .Tables
 	 * array can change dynamically as tables are loaded at run-time. Note:
-	 * .Pointer field is not validated until after call to acpi_tb_verify_table.
+	 * .Pointer field is not validated until after call to acpi_tb_validate_table.
 	 */
 	acpi_gbl_DSDT =
 	    acpi_gbl_root_table_list.tables[ACPI_TABLE_INDEX_DSDT].pointer;
@@ -174,24 +174,11 @@ static acpi_status acpi_tb_load_namespace(void)
 					(acpi_gbl_root_table_list.tables[i].
 					 signature), ACPI_SIG_PSDT))
 		    ||
-		    ACPI_FAILURE(acpi_tb_verify_table
+		    ACPI_FAILURE(acpi_tb_validate_table
 				 (&acpi_gbl_root_table_list.tables[i]))) {
 			continue;
 		}
 
-		/*
-		 * Optionally do not load any SSDTs from the RSDT/XSDT. This can
-		 * be useful for debugging ACPI problems on some machines.
-		 */
-		if (acpi_gbl_disable_ssdt_table_load) {
-			ACPI_INFO((AE_INFO, "Ignoring %4.4s at %p",
-				   acpi_gbl_root_table_list.tables[i].signature.
-				   ascii, ACPI_CAST_PTR(void,
-							acpi_gbl_root_table_list.
-							tables[i].address)));
-			continue;
-		}
-
 		/* Ignore errors while loading tables, get as many as possible */
 
 		(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
@@ -208,6 +195,45 @@ unlock_and_exit:
 
 /*******************************************************************************
  *
+ * FUNCTION:    acpi_install_table
+ *
+ * PARAMETERS:  address             - Address of the ACPI table to be installed.
+ *              physical            - Whether the address is a physical table
+ *                                    address or not
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Dynamically install an ACPI table.
+ *              Note: This function should only be invoked after
+ *                    acpi_initialize_tables() and before acpi_load_tables().
+ *
+ ******************************************************************************/
+
+acpi_status __init
+acpi_install_table(acpi_physical_address address, u8 physical)
+{
+	acpi_status status;
+	u8 flags;
+	u32 table_index;
+
+	ACPI_FUNCTION_TRACE(acpi_install_table);
+
+	if (physical) {
+		flags = ACPI_TABLE_ORIGIN_EXTERNAL_VIRTUAL;
+	} else {
+		flags = ACPI_TABLE_ORIGIN_INTERNAL_PHYSICAL;
+	}
+
+	status = acpi_tb_install_standard_table(address, flags,
+						FALSE, FALSE, &table_index);
+
+	return_ACPI_STATUS(status);
+}
+
+ACPI_EXPORT_SYMBOL_INIT(acpi_install_table)
+
+/*******************************************************************************
+ *
  * FUNCTION:    acpi_load_table
  *
  * PARAMETERS:  table               - Pointer to a buffer containing the ACPI
@@ -222,11 +248,9 @@ unlock_and_exit:
  *              to ensure that the table is not deleted or unmapped.
  *
  ******************************************************************************/
-
 acpi_status acpi_load_table(struct acpi_table_header *table)
 {
 	acpi_status status;
-	struct acpi_table_desc table_desc;
 	u32 table_index;
 
 	ACPI_FUNCTION_TRACE(acpi_load_table);
@@ -237,14 +261,6 @@ acpi_status acpi_load_table(struct acpi_table_header *table)
 		return_ACPI_STATUS(AE_BAD_PARAMETER);
 	}
 
-	/* Init local table descriptor */
-
-	ACPI_MEMSET(&table_desc, 0, sizeof(struct acpi_table_desc));
-	table_desc.address = ACPI_PTR_TO_PHYSADDR(table);
-	table_desc.pointer = table;
-	table_desc.length = table->length;
-	table_desc.flags = ACPI_TABLE_ORIGIN_UNKNOWN;
-
 	/* Must acquire the interpreter lock during this operation */
 
 	status = acpi_ut_acquire_mutex(ACPI_MTX_INTERPRETER);
@@ -255,7 +271,24 @@ acpi_status acpi_load_table(struct acpi_table_header *table)
 	/* Install the table and load it into the namespace */
 
 	ACPI_INFO((AE_INFO, "Host-directed Dynamic ACPI Table Load:"));
-	status = acpi_tb_add_table(&table_desc, &table_index);
+	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
+
+	status = acpi_tb_install_standard_table(ACPI_PTR_TO_PHYSADDR(table),
+						ACPI_TABLE_ORIGIN_EXTERNAL_VIRTUAL,
+						TRUE, FALSE, &table_index);
+
+	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
+	if (ACPI_FAILURE(status)) {
+		goto unlock_and_exit;
+	}
+
+	/*
+	 * Note: Now table is "INSTALLED", it must be validated before
+	 * using.
+	 */
+	status =
+	    acpi_tb_validate_table(&acpi_gbl_root_table_list.
+				   tables[table_index]);
 	if (ACPI_FAILURE(status)) {
 		goto unlock_and_exit;
 	}
diff --git a/drivers/acpi/acpica/utdecode.c b/drivers/acpi/acpica/utdecode.c
index fbfa9eca011..90ec37c473c 100644
--- a/drivers/acpi/acpica/utdecode.c
+++ b/drivers/acpi/acpica/utdecode.c
@@ -462,7 +462,7 @@ char *acpi_ut_get_mutex_name(u32 mutex_id)
 
 /* Names for Notify() values, used for debug output */
 
-static const char *acpi_gbl_notify_value_names[ACPI_NOTIFY_MAX + 1] = {
+static const char *acpi_gbl_generic_notify[ACPI_NOTIFY_MAX + 1] = {
 	/* 00 */ "Bus Check",
 	/* 01 */ "Device Check",
 	/* 02 */ "Device Wake",
@@ -473,23 +473,75 @@ static const char *acpi_gbl_notify_value_names[ACPI_NOTIFY_MAX + 1] = {
 	/* 07 */ "Power Fault",
 	/* 08 */ "Capabilities Check",
 	/* 09 */ "Device PLD Check",
-	/* 10 */ "Reserved",
-	/* 11 */ "System Locality Update",
-	/* 12 */ "Shutdown Request"
+	/* 0A */ "Reserved",
+	/* 0B */ "System Locality Update",
+	/* 0C */ "Shutdown Request"
 };
 
-const char *acpi_ut_get_notify_name(u32 notify_value)
+static const char *acpi_gbl_device_notify[4] = {
+	/* 80 */ "Status Change",
+	/* 81 */ "Information Change",
+	/* 82 */ "Device-Specific Change",
+	/* 83 */ "Device-Specific Change"
+};
+
+static const char *acpi_gbl_processor_notify[4] = {
+	/* 80 */ "Performance Capability Change",
+	/* 81 */ "C-State Change",
+	/* 82 */ "Throttling Capability Change",
+	/* 83 */ "Device-Specific Change"
+};
+
+static const char *acpi_gbl_thermal_notify[4] = {
+	/* 80 */ "Thermal Status Change",
+	/* 81 */ "Thermal Trip Point Change",
+	/* 82 */ "Thermal Device List Change",
+	/* 83 */ "Thermal Relationship Change"
+};
+
+const char *acpi_ut_get_notify_name(u32 notify_value, acpi_object_type type)
 {
 
+	/* 00 - 0C are common to all object types */
+
 	if (notify_value <= ACPI_NOTIFY_MAX) {
-		return (acpi_gbl_notify_value_names[notify_value]);
-	} else if (notify_value <= ACPI_MAX_SYS_NOTIFY) {
+		return (acpi_gbl_generic_notify[notify_value]);
+	}
+
+	/* 0D - 7F are reserved */
+
+	if (notify_value <= ACPI_MAX_SYS_NOTIFY) {
 		return ("Reserved");
-	} else if (notify_value <= ACPI_MAX_DEVICE_SPECIFIC_NOTIFY) {
-		return ("Device Specific");
-	} else {
-		return ("Hardware Specific");
 	}
+
+	/* 80 - 83 are per-object-type */
+
+	if (notify_value <= 0x83) {
+		switch (type) {
+		case ACPI_TYPE_ANY:
+		case ACPI_TYPE_DEVICE:
+			return (acpi_gbl_device_notify[notify_value - 0x80]);
+
+		case ACPI_TYPE_PROCESSOR:
+			return (acpi_gbl_processor_notify[notify_value - 0x80]);
+
+		case ACPI_TYPE_THERMAL:
+			return (acpi_gbl_thermal_notify[notify_value - 0x80]);
+
+		default:
+			return ("Target object type does not support notifies");
+		}
+	}
+
+	/* 84 - BF are device-specific */
+
+	if (notify_value <= ACPI_MAX_DEVICE_SPECIFIC_NOTIFY) {
+		return ("Device-Specific");
+	}
+
+	/* C0 and above are hardware-specific */
+
+	return ("Hardware-Specific");
 }
 #endif
 
diff --git a/drivers/acpi/acpica/utglobal.c b/drivers/acpi/acpica/utglobal.c
index f3abeae9d2f..d69be3cb3fa 100644
--- a/drivers/acpi/acpica/utglobal.c
+++ b/drivers/acpi/acpica/utglobal.c
@@ -55,28 +55,7 @@ ACPI_MODULE_NAME("utglobal")
  * Static global variable initialization.
  *
  ******************************************************************************/
-/* Debug output control masks */
-u32 acpi_dbg_level = ACPI_DEBUG_DEFAULT;
-
-u32 acpi_dbg_layer = 0;
-
-/* acpi_gbl_FADT is a local copy of the FADT, converted to a common format. */
-
-struct acpi_table_fadt acpi_gbl_FADT;
-u32 acpi_gbl_trace_flags;
-acpi_name acpi_gbl_trace_method_name;
-u8 acpi_gbl_system_awake_and_running;
-u32 acpi_current_gpe_count;
-
-/*
- * ACPI 5.0 introduces the concept of a "reduced hardware platform", meaning
- * that the ACPI hardware is no longer required. A flag in the FADT indicates
- * a reduced HW machine, and that flag is duplicated here for convenience.
- */
-u8 acpi_gbl_reduced_hardware;
-
 /* Various state name strings */
-
 const char *acpi_gbl_sleep_state_names[ACPI_S_STATE_COUNT] = {
 	"\\_S0_",
 	"\\_S1_",
@@ -337,7 +316,6 @@ acpi_status acpi_ut_init_globals(void)
 	acpi_gbl_acpi_hardware_present = TRUE;
 	acpi_gbl_last_owner_id_index = 0;
 	acpi_gbl_next_owner_id_offset = 0;
-	acpi_gbl_trace_method_name = 0;
 	acpi_gbl_trace_dbg_level = 0;
 	acpi_gbl_trace_dbg_layer = 0;
 	acpi_gbl_debugger_configuration = DEBUGGER_THREADING;
@@ -377,9 +355,7 @@ acpi_status acpi_ut_init_globals(void)
 	acpi_gbl_disable_mem_tracking = FALSE;
 #endif
 
-#ifdef ACPI_DEBUGGER
-	acpi_gbl_db_terminate_threads = FALSE;
-#endif
+	ACPI_DEBUGGER_EXEC(acpi_gbl_db_terminate_threads = FALSE);
 
 	return_ACPI_STATUS(AE_OK);
 }
diff --git a/drivers/acpi/acpica/utstring.c b/drivers/acpi/acpica/utstring.c
index 77219336c7e..6dc54b3c28b 100644
--- a/drivers/acpi/acpica/utstring.c
+++ b/drivers/acpi/acpica/utstring.c
@@ -353,7 +353,7 @@ void acpi_ut_print_string(char *string, u16 max_length)
 	}
 
 	acpi_os_printf("\"");
-	for (i = 0; string[i] && (i < max_length); i++) {
+	for (i = 0; (i < max_length) && string[i]; i++) {
 
 		/* Escape sequences */
 
diff --git a/drivers/acpi/acpica/utxferror.c b/drivers/acpi/acpica/utxferror.c
index edd861102f1..88ef77f3cf8 100644
--- a/drivers/acpi/acpica/utxferror.c
+++ b/drivers/acpi/acpica/utxferror.c
@@ -53,6 +53,7 @@ ACPI_MODULE_NAME("utxferror")
  * This module is used for the in-kernel ACPICA as well as the ACPICA
  * tools/applications.
  */
+#ifndef ACPI_NO_ERROR_MESSAGES	/* Entire module */
 /*******************************************************************************
  *
  * FUNCTION:    acpi_error
@@ -249,3 +250,4 @@ acpi_bios_warning(const char *module_name,
 }
 
 ACPI_EXPORT_SYMBOL(acpi_bios_warning)
+#endif				/* ACPI_NO_ERROR_MESSAGES */
diff --git a/drivers/acpi/apei/einj.c b/drivers/acpi/apei/einj.c
index 1be6f556448..a095d4f858d 100644
--- a/drivers/acpi/apei/einj.c
+++ b/drivers/acpi/apei/einj.c
@@ -202,7 +202,7 @@ static void check_vendor_extension(u64 paddr,
 
 	if (!offset)
 		return;
-	v = acpi_os_map_memory(paddr + offset, sizeof(*v));
+	v = acpi_os_map_iomem(paddr + offset, sizeof(*v));
 	if (!v)
 		return;
 	sbdf = v->pcie_sbdf;
@@ -210,7 +210,7 @@ static void check_vendor_extension(u64 paddr,
 		sbdf >> 24, (sbdf >> 16) & 0xff,
 		(sbdf >> 11) & 0x1f, (sbdf >> 8) & 0x7,
 		 v->vendor_id, v->device_id, v->rev_id);
-	acpi_os_unmap_memory(v, sizeof(*v));
+	acpi_os_unmap_iomem(v, sizeof(*v));
 }
 
 static void *einj_get_parameter_address(void)
@@ -236,7 +236,7 @@ static void *einj_get_parameter_address(void)
 	if (pa_v5) {
 		struct set_error_type_with_address *v5param;
 
-		v5param = acpi_os_map_memory(pa_v5, sizeof(*v5param));
+		v5param = acpi_os_map_iomem(pa_v5, sizeof(*v5param));
 		if (v5param) {
 			acpi5 = 1;
 			check_vendor_extension(pa_v5, v5param);
@@ -246,11 +246,11 @@ static void *einj_get_parameter_address(void)
 	if (param_extension && pa_v4) {
 		struct einj_parameter *v4param;
 
-		v4param = acpi_os_map_memory(pa_v4, sizeof(*v4param));
+		v4param = acpi_os_map_iomem(pa_v4, sizeof(*v4param));
 		if (!v4param)
 			return NULL;
 		if (v4param->reserved1 || v4param->reserved2) {
-			acpi_os_unmap_memory(v4param, sizeof(*v4param));
+			acpi_os_unmap_iomem(v4param, sizeof(*v4param));
 			return NULL;
 		}
 		return v4param;
@@ -794,7 +794,7 @@ err_unmap:
 			sizeof(struct set_error_type_with_address) :
 			sizeof(struct einj_parameter);
 
-		acpi_os_unmap_memory(einj_param, size);
+		acpi_os_unmap_iomem(einj_param, size);
 	}
 	apei_exec_post_unmap_gars(&ctx);
 err_release:
@@ -816,7 +816,7 @@ static void __exit einj_exit(void)
 			sizeof(struct set_error_type_with_address) :
 			sizeof(struct einj_parameter);
 
-		acpi_os_unmap_memory(einj_param, size);
+		acpi_os_unmap_iomem(einj_param, size);
 	}
 	einj_exec_ctx_init(&ctx);
 	apei_exec_post_unmap_gars(&ctx);
diff --git a/drivers/acpi/battery.c b/drivers/acpi/battery.c
index 9a2c63b2005..e48fc98e71c 100644
--- a/drivers/acpi/battery.c
+++ b/drivers/acpi/battery.c
@@ -36,6 +36,12 @@
 #include <linux/suspend.h>
 #include <asm/unaligned.h>
 
+#ifdef CONFIG_ACPI_PROCFS_POWER
+#include <linux/proc_fs.h>
+#include <linux/seq_file.h>
+#include <asm/uaccess.h>
+#endif
+
 #include <linux/acpi.h>
 #include <linux/power_supply.h>
 
@@ -50,6 +56,10 @@
 /* Battery power unit: 0 means mW, 1 means mA */
 #define ACPI_BATTERY_POWER_UNIT_MA	1
 
+#define ACPI_BATTERY_STATE_DISCHARGING	0x1
+#define ACPI_BATTERY_STATE_CHARGING	0x2
+#define ACPI_BATTERY_STATE_CRITICAL	0x4
+
 #define _COMPONENT		ACPI_BATTERY_COMPONENT
 
 ACPI_MODULE_NAME("battery");
@@ -64,6 +74,19 @@ static unsigned int cache_time = 1000;
 module_param(cache_time, uint, 0644);
 MODULE_PARM_DESC(cache_time, "cache time in milliseconds");
 
+#ifdef CONFIG_ACPI_PROCFS_POWER
+extern struct proc_dir_entry *acpi_lock_battery_dir(void);
+extern void *acpi_unlock_battery_dir(struct proc_dir_entry *acpi_battery_dir);
+
+enum acpi_battery_files {
+	info_tag = 0,
+	state_tag,
+	alarm_tag,
+	ACPI_BATTERY_NUMFILES,
+};
+
+#endif
+
 static const struct acpi_device_id battery_device_ids[] = {
 	{"PNP0C0A", 0},
 	{"", 0},
@@ -150,7 +173,7 @@ static int acpi_battery_get_state(struct acpi_battery *battery);
 
 static int acpi_battery_is_charged(struct acpi_battery *battery)
 {
-	/* either charging or discharging */
+	/* charging, discharging or critical low */
 	if (battery->state != 0)
 		return 0;
 
@@ -185,9 +208,9 @@ static int acpi_battery_get_property(struct power_supply *psy,
 		return -ENODEV;
 	switch (psp) {
 	case POWER_SUPPLY_PROP_STATUS:
-		if (battery->state & 0x01)
+		if (battery->state & ACPI_BATTERY_STATE_DISCHARGING)
 			val->intval = POWER_SUPPLY_STATUS_DISCHARGING;
-		else if (battery->state & 0x02)
+		else if (battery->state & ACPI_BATTERY_STATE_CHARGING)
 			val->intval = POWER_SUPPLY_STATUS_CHARGING;
 		else if (acpi_battery_is_charged(battery))
 			val->intval = POWER_SUPPLY_STATUS_FULL;
@@ -250,6 +273,17 @@ static int acpi_battery_get_property(struct power_supply *psy,
 		else
 			val->intval = 0;
 		break;
+	case POWER_SUPPLY_PROP_CAPACITY_LEVEL:
+		if (battery->state & ACPI_BATTERY_STATE_CRITICAL)
+			val->intval = POWER_SUPPLY_CAPACITY_LEVEL_CRITICAL;
+		else if (test_bit(ACPI_BATTERY_ALARM_PRESENT, &battery->flags) &&
+			(battery->capacity_now <= battery->alarm))
+			val->intval = POWER_SUPPLY_CAPACITY_LEVEL_LOW;
+		else if (acpi_battery_is_charged(battery))
+			val->intval = POWER_SUPPLY_CAPACITY_LEVEL_FULL;
+		else
+			val->intval = POWER_SUPPLY_CAPACITY_LEVEL_NORMAL;
+		break;
 	case POWER_SUPPLY_PROP_MODEL_NAME:
 		val->strval = battery->model_number;
 		break;
@@ -277,6 +311,7 @@ static enum power_supply_property charge_battery_props[] = {
 	POWER_SUPPLY_PROP_CHARGE_FULL,
 	POWER_SUPPLY_PROP_CHARGE_NOW,
 	POWER_SUPPLY_PROP_CAPACITY,
+	POWER_SUPPLY_PROP_CAPACITY_LEVEL,
 	POWER_SUPPLY_PROP_MODEL_NAME,
 	POWER_SUPPLY_PROP_MANUFACTURER,
 	POWER_SUPPLY_PROP_SERIAL_NUMBER,
@@ -294,11 +329,20 @@ static enum power_supply_property energy_battery_props[] = {
 	POWER_SUPPLY_PROP_ENERGY_FULL,
 	POWER_SUPPLY_PROP_ENERGY_NOW,
 	POWER_SUPPLY_PROP_CAPACITY,
+	POWER_SUPPLY_PROP_CAPACITY_LEVEL,
 	POWER_SUPPLY_PROP_MODEL_NAME,
 	POWER_SUPPLY_PROP_MANUFACTURER,
 	POWER_SUPPLY_PROP_SERIAL_NUMBER,
 };
 
+#ifdef CONFIG_ACPI_PROCFS_POWER
+inline char *acpi_battery_units(struct acpi_battery *battery)
+{
+	return (battery->power_unit == ACPI_BATTERY_POWER_UNIT_MA) ?
+		"mA" : "mW";
+}
+#endif
+
 /* --------------------------------------------------------------------------
                                Battery Management
    -------------------------------------------------------------------------- */
@@ -578,7 +622,8 @@ static int sysfs_add_battery(struct acpi_battery *battery)
 	battery->bat.type = POWER_SUPPLY_TYPE_BATTERY;
 	battery->bat.get_property = acpi_battery_get_property;
 
-	result = power_supply_register(&battery->device->dev, &battery->bat);
+	result = power_supply_register_no_ws(&battery->device->dev, &battery->bat);
+
 	if (result)
 		return result;
 	return device_create_file(battery->bat.dev, &alarm_attr);
@@ -669,7 +714,7 @@ static void acpi_battery_quirks(struct acpi_battery *battery)
 	}
 }
 
-static int acpi_battery_update(struct acpi_battery *battery)
+static int acpi_battery_update(struct acpi_battery *battery, bool resume)
 {
 	int result, old_present = acpi_battery_present(battery);
 	result = acpi_battery_get_status(battery);
@@ -680,6 +725,10 @@ static int acpi_battery_update(struct acpi_battery *battery)
 		battery->update_time = 0;
 		return 0;
 	}
+
+	if (resume)
+		return 0;
+
 	if (!battery->update_time ||
 	    old_present != acpi_battery_present(battery)) {
 		result = acpi_battery_get_info(battery);
@@ -693,7 +742,19 @@ static int acpi_battery_update(struct acpi_battery *battery)
 			return result;
 	}
 	result = acpi_battery_get_state(battery);
+	if (result)
+		return result;
 	acpi_battery_quirks(battery);
+
+	/*
+	 * Wakeup the system if battery is critical low
+	 * or lower than the alarm level
+	 */
+	if ((battery->state & ACPI_BATTERY_STATE_CRITICAL) ||
+	    (test_bit(ACPI_BATTERY_ALARM_PRESENT, &battery->flags) &&
+            (battery->capacity_now <= battery->alarm)))
+		pm_wakeup_event(&battery->device->dev, 0);
+
 	return result;
 }
 
@@ -717,6 +778,279 @@ static void acpi_battery_refresh(struct acpi_battery *battery)
 }
 
 /* --------------------------------------------------------------------------
+                              FS Interface (/proc)
+   -------------------------------------------------------------------------- */
+
+#ifdef CONFIG_ACPI_PROCFS_POWER
+static struct proc_dir_entry *acpi_battery_dir;
+
+static int acpi_battery_print_info(struct seq_file *seq, int result)
+{
+	struct acpi_battery *battery = seq->private;
+
+	if (result)
+		goto end;
+
+	seq_printf(seq, "present:                 %s\n",
+		   acpi_battery_present(battery) ? "yes" : "no");
+	if (!acpi_battery_present(battery))
+		goto end;
+	if (battery->design_capacity == ACPI_BATTERY_VALUE_UNKNOWN)
+		seq_printf(seq, "design capacity:         unknown\n");
+	else
+		seq_printf(seq, "design capacity:         %d %sh\n",
+			   battery->design_capacity,
+			   acpi_battery_units(battery));
+
+	if (battery->full_charge_capacity == ACPI_BATTERY_VALUE_UNKNOWN)
+		seq_printf(seq, "last full capacity:      unknown\n");
+	else
+		seq_printf(seq, "last full capacity:      %d %sh\n",
+			   battery->full_charge_capacity,
+			   acpi_battery_units(battery));
+
+	seq_printf(seq, "battery technology:      %srechargeable\n",
+		   (!battery->technology)?"non-":"");
+
+	if (battery->design_voltage == ACPI_BATTERY_VALUE_UNKNOWN)
+		seq_printf(seq, "design voltage:          unknown\n");
+	else
+		seq_printf(seq, "design voltage:          %d mV\n",
+			   battery->design_voltage);
+	seq_printf(seq, "design capacity warning: %d %sh\n",
+		   battery->design_capacity_warning,
+		   acpi_battery_units(battery));
+	seq_printf(seq, "design capacity low:     %d %sh\n",
+		   battery->design_capacity_low,
+		   acpi_battery_units(battery));
+	seq_printf(seq, "cycle count:		  %i\n", battery->cycle_count);
+	seq_printf(seq, "capacity granularity 1:  %d %sh\n",
+		   battery->capacity_granularity_1,
+		   acpi_battery_units(battery));
+	seq_printf(seq, "capacity granularity 2:  %d %sh\n",
+		   battery->capacity_granularity_2,
+		   acpi_battery_units(battery));
+	seq_printf(seq, "model number:            %s\n", battery->model_number);
+	seq_printf(seq, "serial number:           %s\n", battery->serial_number);
+	seq_printf(seq, "battery type:            %s\n", battery->type);
+	seq_printf(seq, "OEM info:                %s\n", battery->oem_info);
+      end:
+	if (result)
+		seq_printf(seq, "ERROR: Unable to read battery info\n");
+	return result;
+}
+
+static int acpi_battery_print_state(struct seq_file *seq, int result)
+{
+	struct acpi_battery *battery = seq->private;
+
+	if (result)
+		goto end;
+
+	seq_printf(seq, "present:                 %s\n",
+		   acpi_battery_present(battery) ? "yes" : "no");
+	if (!acpi_battery_present(battery))
+		goto end;
+
+	seq_printf(seq, "capacity state:          %s\n",
+			(battery->state & 0x04) ? "critical" : "ok");
+	if ((battery->state & 0x01) && (battery->state & 0x02))
+		seq_printf(seq,
+			   "charging state:          charging/discharging\n");
+	else if (battery->state & 0x01)
+		seq_printf(seq, "charging state:          discharging\n");
+	else if (battery->state & 0x02)
+		seq_printf(seq, "charging state:          charging\n");
+	else
+		seq_printf(seq, "charging state:          charged\n");
+
+	if (battery->rate_now == ACPI_BATTERY_VALUE_UNKNOWN)
+		seq_printf(seq, "present rate:            unknown\n");
+	else
+		seq_printf(seq, "present rate:            %d %s\n",
+			   battery->rate_now, acpi_battery_units(battery));
+
+	if (battery->capacity_now == ACPI_BATTERY_VALUE_UNKNOWN)
+		seq_printf(seq, "remaining capacity:      unknown\n");
+	else
+		seq_printf(seq, "remaining capacity:      %d %sh\n",
+			   battery->capacity_now, acpi_battery_units(battery));
+	if (battery->voltage_now == ACPI_BATTERY_VALUE_UNKNOWN)
+		seq_printf(seq, "present voltage:         unknown\n");
+	else
+		seq_printf(seq, "present voltage:         %d mV\n",
+			   battery->voltage_now);
+      end:
+	if (result)
+		seq_printf(seq, "ERROR: Unable to read battery state\n");
+
+	return result;
+}
+
+static int acpi_battery_print_alarm(struct seq_file *seq, int result)
+{
+	struct acpi_battery *battery = seq->private;
+
+	if (result)
+		goto end;
+
+	if (!acpi_battery_present(battery)) {
+		seq_printf(seq, "present:                 no\n");
+		goto end;
+	}
+	seq_printf(seq, "alarm:                   ");
+	if (!battery->alarm)
+		seq_printf(seq, "unsupported\n");
+	else
+		seq_printf(seq, "%u %sh\n", battery->alarm,
+				acpi_battery_units(battery));
+      end:
+	if (result)
+		seq_printf(seq, "ERROR: Unable to read battery alarm\n");
+	return result;
+}
+
+static ssize_t acpi_battery_write_alarm(struct file *file,
+					const char __user * buffer,
+					size_t count, loff_t * ppos)
+{
+	int result = 0;
+	char alarm_string[12] = { '\0' };
+	struct seq_file *m = file->private_data;
+	struct acpi_battery *battery = m->private;
+
+	if (!battery || (count > sizeof(alarm_string) - 1))
+		return -EINVAL;
+	if (!acpi_battery_present(battery)) {
+		result = -ENODEV;
+		goto end;
+	}
+	if (copy_from_user(alarm_string, buffer, count)) {
+		result = -EFAULT;
+		goto end;
+	}
+	alarm_string[count] = '\0';
+	battery->alarm = simple_strtol(alarm_string, NULL, 0);
+	result = acpi_battery_set_alarm(battery);
+      end:
+	if (!result)
+		return count;
+	return result;
+}
+
+typedef int(*print_func)(struct seq_file *seq, int result);
+
+static print_func acpi_print_funcs[ACPI_BATTERY_NUMFILES] = {
+	acpi_battery_print_info,
+	acpi_battery_print_state,
+	acpi_battery_print_alarm,
+};
+
+static int acpi_battery_read(int fid, struct seq_file *seq)
+{
+	struct acpi_battery *battery = seq->private;
+	int result = acpi_battery_update(battery, false);
+	return acpi_print_funcs[fid](seq, result);
+}
+
+#define DECLARE_FILE_FUNCTIONS(_name) \
+static int acpi_battery_read_##_name(struct seq_file *seq, void *offset) \
+{ \
+	return acpi_battery_read(_name##_tag, seq); \
+} \
+static int acpi_battery_##_name##_open_fs(struct inode *inode, struct file *file) \
+{ \
+	return single_open(file, acpi_battery_read_##_name, PDE_DATA(inode)); \
+}
+
+DECLARE_FILE_FUNCTIONS(info);
+DECLARE_FILE_FUNCTIONS(state);
+DECLARE_FILE_FUNCTIONS(alarm);
+
+#undef DECLARE_FILE_FUNCTIONS
+
+#define FILE_DESCRIPTION_RO(_name) \
+	{ \
+	.name = __stringify(_name), \
+	.mode = S_IRUGO, \
+	.ops = { \
+		.open = acpi_battery_##_name##_open_fs, \
+		.read = seq_read, \
+		.llseek = seq_lseek, \
+		.release = single_release, \
+		.owner = THIS_MODULE, \
+		}, \
+	}
+
+#define FILE_DESCRIPTION_RW(_name) \
+	{ \
+	.name = __stringify(_name), \
+	.mode = S_IFREG | S_IRUGO | S_IWUSR, \
+	.ops = { \
+		.open = acpi_battery_##_name##_open_fs, \
+		.read = seq_read, \
+		.llseek = seq_lseek, \
+		.write = acpi_battery_write_##_name, \
+		.release = single_release, \
+		.owner = THIS_MODULE, \
+		}, \
+	}
+
+static const struct battery_file {
+	struct file_operations ops;
+	umode_t mode;
+	const char *name;
+} acpi_battery_file[] = {
+	FILE_DESCRIPTION_RO(info),
+	FILE_DESCRIPTION_RO(state),
+	FILE_DESCRIPTION_RW(alarm),
+};
+
+#undef FILE_DESCRIPTION_RO
+#undef FILE_DESCRIPTION_RW
+
+static int acpi_battery_add_fs(struct acpi_device *device)
+{
+	struct proc_dir_entry *entry = NULL;
+	int i;
+
+	printk(KERN_WARNING PREFIX "Deprecated procfs I/F for battery is loaded,"
+			" please retry with CONFIG_ACPI_PROCFS_POWER cleared\n");
+	if (!acpi_device_dir(device)) {
+		acpi_device_dir(device) = proc_mkdir(acpi_device_bid(device),
+						     acpi_battery_dir);
+		if (!acpi_device_dir(device))
+			return -ENODEV;
+	}
+
+	for (i = 0; i < ACPI_BATTERY_NUMFILES; ++i) {
+		entry = proc_create_data(acpi_battery_file[i].name,
+					 acpi_battery_file[i].mode,
+					 acpi_device_dir(device),
+					 &acpi_battery_file[i].ops,
+					 acpi_driver_data(device));
+		if (!entry)
+			return -ENODEV;
+	}
+	return 0;
+}
+
+static void acpi_battery_remove_fs(struct acpi_device *device)
+{
+	int i;
+	if (!acpi_device_dir(device))
+		return;
+	for (i = 0; i < ACPI_BATTERY_NUMFILES; ++i)
+		remove_proc_entry(acpi_battery_file[i].name,
+				  acpi_device_dir(device));
+
+	remove_proc_entry(acpi_device_bid(device), acpi_battery_dir);
+	acpi_device_dir(device) = NULL;
+}
+
+#endif
+
+/* --------------------------------------------------------------------------
                                  Driver Interface
    -------------------------------------------------------------------------- */
 
@@ -730,7 +1064,7 @@ static void acpi_battery_notify(struct acpi_device *device, u32 event)
 	old = battery->bat.dev;
 	if (event == ACPI_BATTERY_NOTIFY_INFO)
 		acpi_battery_refresh(battery);
-	acpi_battery_update(battery);
+	acpi_battery_update(battery, false);
 	acpi_bus_generate_netlink_event(device->pnp.device_class,
 					dev_name(&device->dev), event,
 					acpi_battery_present(battery));
@@ -745,13 +1079,27 @@ static int battery_notify(struct notifier_block *nb,
 {
 	struct acpi_battery *battery = container_of(nb, struct acpi_battery,
 						    pm_nb);
+	int result;
+
 	switch (mode) {
 	case PM_POST_HIBERNATION:
 	case PM_POST_SUSPEND:
-		if (battery->bat.dev) {
-			sysfs_remove_battery(battery);
-			sysfs_add_battery(battery);
-		}
+		if (!acpi_battery_present(battery))
+			return 0;
+
+		if (!battery->bat.dev) {
+			result = acpi_battery_get_info(battery);
+			if (result)
+				return result;
+
+			result = sysfs_add_battery(battery);
+			if (result)
+				return result;
+		} else
+			acpi_battery_refresh(battery);
+
+		acpi_battery_init_alarm(battery);
+		acpi_battery_get_state(battery);
 		break;
 	}
 
@@ -787,9 +1135,18 @@ static int acpi_battery_add(struct acpi_device *device)
 	mutex_init(&battery->sysfs_lock);
 	if (acpi_has_method(battery->device->handle, "_BIX"))
 		set_bit(ACPI_BATTERY_XINFO_PRESENT, &battery->flags);
-	result = acpi_battery_update(battery);
+	result = acpi_battery_update(battery, false);
 	if (result)
 		goto fail;
+#ifdef CONFIG_ACPI_PROCFS_POWER
+	result = acpi_battery_add_fs(device);
+#endif
+	if (result) {
+#ifdef CONFIG_ACPI_PROCFS_POWER
+		acpi_battery_remove_fs(device);
+#endif
+		goto fail;
+	}
 
 	printk(KERN_INFO PREFIX "%s Slot [%s] (battery %s)\n",
 		ACPI_BATTERY_DEVICE_NAME, acpi_device_bid(device),
@@ -798,6 +1155,8 @@ static int acpi_battery_add(struct acpi_device *device)
 	battery->pm_nb.notifier_call = battery_notify;
 	register_pm_notifier(&battery->pm_nb);
 
+	device_init_wakeup(&device->dev, 1);
+
 	return result;
 
 fail:
@@ -814,8 +1173,12 @@ static int acpi_battery_remove(struct acpi_device *device)
 
 	if (!device || !acpi_driver_data(device))
 		return -EINVAL;
+	device_init_wakeup(&device->dev, 0);
 	battery = acpi_driver_data(device);
 	unregister_pm_notifier(&battery->pm_nb);
+#ifdef CONFIG_ACPI_PROCFS_POWER
+	acpi_battery_remove_fs(device);
+#endif
 	sysfs_remove_battery(battery);
 	mutex_destroy(&battery->lock);
 	mutex_destroy(&battery->sysfs_lock);
@@ -837,7 +1200,7 @@ static int acpi_battery_resume(struct device *dev)
 		return -EINVAL;
 
 	battery->update_time = 0;
-	acpi_battery_update(battery);
+	acpi_battery_update(battery, true);
 	return 0;
 }
 #else
@@ -866,7 +1229,19 @@ static void __init acpi_battery_init_async(void *unused, async_cookie_t cookie)
 
 	if (dmi_check_system(bat_dmi_table))
 		battery_bix_broken_package = 1;
-	acpi_bus_register_driver(&acpi_battery_driver);
+	
+#ifdef CONFIG_ACPI_PROCFS_POWER
+	acpi_battery_dir = acpi_lock_battery_dir();
+	if (!acpi_battery_dir)
+		return;
+#endif
+	if (acpi_bus_register_driver(&acpi_battery_driver) < 0) {
+#ifdef CONFIG_ACPI_PROCFS_POWER
+		acpi_unlock_battery_dir(acpi_battery_dir);
+#endif
+		return;
+	}
+	return;
 }
 
 static int __init acpi_battery_init(void)
@@ -878,6 +1253,9 @@ static int __init acpi_battery_init(void)
 static void __exit acpi_battery_exit(void)
 {
 	acpi_bus_unregister_driver(&acpi_battery_driver);
+#ifdef CONFIG_ACPI_PROCFS_POWER
+	acpi_unlock_battery_dir(acpi_battery_dir);
+#endif
 }
 
 module_init(acpi_battery_init);
diff --git a/drivers/acpi/blacklist.c b/drivers/acpi/blacklist.c
index afec4526c48..3d8413d02a9 100644
--- a/drivers/acpi/blacklist.c
+++ b/drivers/acpi/blacklist.c
@@ -314,6 +314,14 @@ static struct dmi_system_id acpi_osi_dmi_table[] __initdata = {
 		     DMI_MATCH(DMI_PRODUCT_VERSION, "2349D15"),
 		},
 	},
+	{
+	.callback = dmi_disable_osi_win8,
+	.ident = "Dell Inspiron 7737",
+	.matches = {
+		    DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
+		    DMI_MATCH(DMI_PRODUCT_NAME, "Inspiron 7737"),
+		},
+	},
 
 	/*
 	 * BIOS invocation of _OSI(Linux) is almost always a BIOS bug.
@@ -374,6 +382,19 @@ static struct dmi_system_id acpi_osi_dmi_table[] __initdata = {
 		     DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T500"),
 		},
 	},
+	/*
+	 * Without this this EEEpc exports a non working WMI interface, with
+	 * this it exports a working "good old" eeepc_laptop interface, fixing
+	 * both brightness control, and rfkill not working.
+	 */
+	{
+	.callback = dmi_enable_osi_linux,
+	.ident = "Asus EEE PC 1015PX",
+	.matches = {
+		     DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK Computer INC."),
+		     DMI_MATCH(DMI_PRODUCT_NAME, "1015PX"),
+		},
+	},
 	{}
 };
 
diff --git a/drivers/acpi/bus.c b/drivers/acpi/bus.c
index cf925c4f36b..c5bc8cfe09f 100644
--- a/drivers/acpi/bus.c
+++ b/drivers/acpi/bus.c
@@ -52,6 +52,12 @@ struct proc_dir_entry *acpi_root_dir;
 EXPORT_SYMBOL(acpi_root_dir);
 
 #ifdef CONFIG_X86
+#ifdef CONFIG_ACPI_CUSTOM_DSDT
+static inline int set_copy_dsdt(const struct dmi_system_id *id)
+{
+	return 0;
+}
+#else
 static int set_copy_dsdt(const struct dmi_system_id *id)
 {
 	printk(KERN_NOTICE "%s detected - "
@@ -59,6 +65,7 @@ static int set_copy_dsdt(const struct dmi_system_id *id)
 	acpi_gbl_copy_dsdt_locally = 1;
 	return 0;
 }
+#endif
 
 static struct dmi_system_id dsdt_dmi_table[] __initdata = {
 	/*
@@ -132,6 +139,21 @@ void acpi_bus_private_data_handler(acpi_handle handle,
 }
 EXPORT_SYMBOL(acpi_bus_private_data_handler);
 
+int acpi_bus_attach_private_data(acpi_handle handle, void *data)
+{
+	acpi_status status;
+
+	status = acpi_attach_data(handle,
+			acpi_bus_private_data_handler, data);
+	if (ACPI_FAILURE(status)) {
+		acpi_handle_debug(handle, "Error attaching device data\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(acpi_bus_attach_private_data);
+
 int acpi_bus_get_private_data(acpi_handle handle, void **data)
 {
 	acpi_status status;
@@ -140,15 +162,20 @@ int acpi_bus_get_private_data(acpi_handle handle, void **data)
 		return -EINVAL;
 
 	status = acpi_get_data(handle, acpi_bus_private_data_handler, data);
-	if (ACPI_FAILURE(status) || !*data) {
-		ACPI_DEBUG_PRINT((ACPI_DB_INFO, "No context for object [%p]\n",
-				handle));
+	if (ACPI_FAILURE(status)) {
+		acpi_handle_debug(handle, "No context for object\n");
 		return -ENODEV;
 	}
 
 	return 0;
 }
-EXPORT_SYMBOL(acpi_bus_get_private_data);
+EXPORT_SYMBOL_GPL(acpi_bus_get_private_data);
+
+void acpi_bus_detach_private_data(acpi_handle handle)
+{
+	acpi_detach_data(handle, acpi_bus_private_data_handler);
+}
+EXPORT_SYMBOL_GPL(acpi_bus_detach_private_data);
 
 void acpi_bus_no_hotplug(acpi_handle handle)
 {
@@ -340,16 +367,18 @@ static void acpi_bus_notify(acpi_handle handle, u32 type, void *data)
 {
 	struct acpi_device *adev;
 	struct acpi_driver *driver;
-	acpi_status status;
 	u32 ost_code = ACPI_OST_SC_NON_SPECIFIC_FAILURE;
+	bool hotplug_event = false;
 
 	switch (type) {
 	case ACPI_NOTIFY_BUS_CHECK:
 		acpi_handle_debug(handle, "ACPI_NOTIFY_BUS_CHECK event\n");
+		hotplug_event = true;
 		break;
 
 	case ACPI_NOTIFY_DEVICE_CHECK:
 		acpi_handle_debug(handle, "ACPI_NOTIFY_DEVICE_CHECK event\n");
+		hotplug_event = true;
 		break;
 
 	case ACPI_NOTIFY_DEVICE_WAKE:
@@ -358,6 +387,7 @@ static void acpi_bus_notify(acpi_handle handle, u32 type, void *data)
 
 	case ACPI_NOTIFY_EJECT_REQUEST:
 		acpi_handle_debug(handle, "ACPI_NOTIFY_EJECT_REQUEST event\n");
+		hotplug_event = true;
 		break;
 
 	case ACPI_NOTIFY_DEVICE_CHECK_LIGHT:
@@ -393,16 +423,9 @@ static void acpi_bus_notify(acpi_handle handle, u32 type, void *data)
 	    (driver->flags & ACPI_DRIVER_ALL_NOTIFY_EVENTS))
 		driver->ops.notify(adev, type);
 
-	switch (type) {
-	case ACPI_NOTIFY_BUS_CHECK:
-	case ACPI_NOTIFY_DEVICE_CHECK:
-	case ACPI_NOTIFY_EJECT_REQUEST:
-		status = acpi_hotplug_schedule(adev, type);
-		if (ACPI_SUCCESS(status))
-			return;
-	default:
-		break;
-	}
+	if (hotplug_event && ACPI_SUCCESS(acpi_hotplug_schedule(adev, type)))
+		return;
+
 	acpi_bus_put_acpi_device(adev);
 	return;
 
@@ -466,6 +489,9 @@ void __init acpi_early_init(void)
 
 	printk(KERN_INFO PREFIX "Core revision %08x\n", ACPI_CA_VERSION);
 
+	/* It's safe to verify table checksums during late stage */
+	acpi_gbl_verify_table_checksum = TRUE;
+
 	/* enable workarounds, unless strict ACPI spec. compliance */
 	if (!acpi_strict)
 		acpi_gbl_enable_interpreter_slack = TRUE;
diff --git a/drivers/acpi/cm_sbs.c b/drivers/acpi/cm_sbs.c
new file mode 100644
index 00000000000..6c9ee68e46f
--- /dev/null
+++ b/drivers/acpi/cm_sbs.c
@@ -0,0 +1,105 @@
+/*
+ * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or (at
+ *  your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful, but
+ *  WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with this program; if not, write to the Free Software Foundation, Inc.,
+ *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ *
+ * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/acpi.h>
+#include <linux/types.h>
+#include <linux/proc_fs.h>
+#include <linux/seq_file.h>
+#include <acpi/acpi_bus.h>
+#include <acpi/acpi_drivers.h>
+
+#define PREFIX "ACPI: "
+
+ACPI_MODULE_NAME("cm_sbs");
+#define ACPI_AC_CLASS		"ac_adapter"
+#define ACPI_BATTERY_CLASS	"battery"
+#define _COMPONENT		ACPI_SBS_COMPONENT
+static struct proc_dir_entry *acpi_ac_dir;
+static struct proc_dir_entry *acpi_battery_dir;
+
+static DEFINE_MUTEX(cm_sbs_mutex);
+
+static int lock_ac_dir_cnt;
+static int lock_battery_dir_cnt;
+
+struct proc_dir_entry *acpi_lock_ac_dir(void)
+{
+	mutex_lock(&cm_sbs_mutex);
+	if (!acpi_ac_dir)
+		acpi_ac_dir = proc_mkdir(ACPI_AC_CLASS, acpi_root_dir);
+	if (acpi_ac_dir) {
+		lock_ac_dir_cnt++;
+	} else {
+		printk(KERN_ERR PREFIX
+				  "Cannot create %s\n", ACPI_AC_CLASS);
+	}
+	mutex_unlock(&cm_sbs_mutex);
+	return acpi_ac_dir;
+}
+EXPORT_SYMBOL(acpi_lock_ac_dir);
+
+void acpi_unlock_ac_dir(struct proc_dir_entry *acpi_ac_dir_param)
+{
+	mutex_lock(&cm_sbs_mutex);
+	if (acpi_ac_dir_param)
+		lock_ac_dir_cnt--;
+	if (lock_ac_dir_cnt == 0 && acpi_ac_dir_param && acpi_ac_dir) {
+		remove_proc_entry(ACPI_AC_CLASS, acpi_root_dir);
+		acpi_ac_dir = NULL;
+	}
+	mutex_unlock(&cm_sbs_mutex);
+}
+EXPORT_SYMBOL(acpi_unlock_ac_dir);
+
+struct proc_dir_entry *acpi_lock_battery_dir(void)
+{
+	mutex_lock(&cm_sbs_mutex);
+	if (!acpi_battery_dir) {
+		acpi_battery_dir =
+		    proc_mkdir(ACPI_BATTERY_CLASS, acpi_root_dir);
+	}
+	if (acpi_battery_dir) {
+		lock_battery_dir_cnt++;
+	} else {
+		printk(KERN_ERR PREFIX
+				  "Cannot create %s\n", ACPI_BATTERY_CLASS);
+	}
+	mutex_unlock(&cm_sbs_mutex);
+	return acpi_battery_dir;
+}
+EXPORT_SYMBOL(acpi_lock_battery_dir);
+
+void acpi_unlock_battery_dir(struct proc_dir_entry *acpi_battery_dir_param)
+{
+	mutex_lock(&cm_sbs_mutex);
+	if (acpi_battery_dir_param)
+		lock_battery_dir_cnt--;
+	if (lock_battery_dir_cnt == 0 && acpi_battery_dir_param
+	    && acpi_battery_dir) {
+		remove_proc_entry(ACPI_BATTERY_CLASS, acpi_root_dir);
+		acpi_battery_dir = NULL;
+	}
+	mutex_unlock(&cm_sbs_mutex);
+	return;
+}
+EXPORT_SYMBOL(acpi_unlock_battery_dir);
diff --git a/drivers/acpi/container.c b/drivers/acpi/container.c
index 63119d09b35..76f7cff6459 100644
--- a/drivers/acpi/container.c
+++ b/drivers/acpi/container.c
@@ -41,6 +41,8 @@ static const struct acpi_device_id container_device_ids[] = {
 	{"", 0},
 };
 
+#ifdef CONFIG_ACPI_CONTAINER
+
 static int acpi_container_offline(struct container_dev *cdev)
 {
 	struct acpi_device *adev = ACPI_COMPANION(&cdev->dev);
@@ -109,5 +111,18 @@ static struct acpi_scan_handler container_handler = {
 
 void __init acpi_container_init(void)
 {
+	acpi_scan_add_handler(&container_handler);
+}
+
+#else
+
+static struct acpi_scan_handler container_handler = {
+	.ids = container_device_ids,
+};
+
+void __init acpi_container_init(void)
+{
 	acpi_scan_add_handler_with_hotplug(&container_handler, "container");
 }
+
+#endif /* CONFIG_ACPI_CONTAINER */
diff --git a/drivers/acpi/device_pm.c b/drivers/acpi/device_pm.c
index d047739f338..49a51277f81 100644
--- a/drivers/acpi/device_pm.c
+++ b/drivers/acpi/device_pm.c
@@ -900,18 +900,47 @@ EXPORT_SYMBOL_GPL(acpi_dev_resume_early);
  */
 int acpi_subsys_prepare(struct device *dev)
 {
-	/*
-	 * Devices having power.ignore_children set may still be necessary for
-	 * suspending their children in the next phase of device suspend.
-	 */
-	if (dev->power.ignore_children)
-		pm_runtime_resume(dev);
+	struct acpi_device *adev = ACPI_COMPANION(dev);
+	u32 sys_target;
+	int ret, state;
+
+	ret = pm_generic_prepare(dev);
+	if (ret < 0)
+		return ret;
+
+	if (!adev || !pm_runtime_suspended(dev)
+	    || device_may_wakeup(dev) != !!adev->wakeup.prepare_count)
+		return 0;
+
+	sys_target = acpi_target_system_state();
+	if (sys_target == ACPI_STATE_S0)
+		return 1;
 
-	return pm_generic_prepare(dev);
+	if (adev->power.flags.dsw_present)
+		return 0;
+
+	ret = acpi_dev_pm_get_state(dev, adev, sys_target, NULL, &state);
+	return !ret && state == adev->power.state;
 }
 EXPORT_SYMBOL_GPL(acpi_subsys_prepare);
 
 /**
+ * acpi_subsys_complete - Finalize device's resume during system resume.
+ * @dev: Device to handle.
+ */
+void acpi_subsys_complete(struct device *dev)
+{
+	/*
+	 * If the device had been runtime-suspended before the system went into
+	 * the sleep state it is going out of and it has never been resumed till
+	 * now, resume it in case the firmware powered it up.
+	 */
+	if (dev->power.direct_complete)
+		pm_request_resume(dev);
+}
+EXPORT_SYMBOL_GPL(acpi_subsys_complete);
+
+/**
  * acpi_subsys_suspend - Run the device driver's suspend callback.
  * @dev: Device to handle.
  *
@@ -923,6 +952,7 @@ int acpi_subsys_suspend(struct device *dev)
 	pm_runtime_resume(dev);
 	return pm_generic_suspend(dev);
 }
+EXPORT_SYMBOL_GPL(acpi_subsys_suspend);
 
 /**
  * acpi_subsys_suspend_late - Suspend device using ACPI.
@@ -968,6 +998,7 @@ int acpi_subsys_freeze(struct device *dev)
 	pm_runtime_resume(dev);
 	return pm_generic_freeze(dev);
 }
+EXPORT_SYMBOL_GPL(acpi_subsys_freeze);
 
 #endif /* CONFIG_PM_SLEEP */
 
@@ -979,6 +1010,7 @@ static struct dev_pm_domain acpi_general_pm_domain = {
 #endif
 #ifdef CONFIG_PM_SLEEP
 		.prepare = acpi_subsys_prepare,
+		.complete = acpi_subsys_complete,
 		.suspend = acpi_subsys_suspend,
 		.suspend_late = acpi_subsys_suspend_late,
 		.resume_early = acpi_subsys_resume_early,
diff --git a/drivers/acpi/internal.h b/drivers/acpi/internal.h
index 957391306cb..7de5b603f27 100644
--- a/drivers/acpi/internal.h
+++ b/drivers/acpi/internal.h
@@ -30,12 +30,10 @@ void acpi_pci_root_init(void);
 void acpi_pci_link_init(void);
 void acpi_processor_init(void);
 void acpi_platform_init(void);
+void acpi_pnp_init(void);
 int acpi_sysfs_init(void);
-#ifdef CONFIG_ACPI_CONTAINER
 void acpi_container_init(void);
-#else
-static inline void acpi_container_init(void) {}
-#endif
+void acpi_memory_hotplug_init(void);
 #ifdef CONFIG_ACPI_DOCK
 void register_dock_dependent_device(struct acpi_device *adev,
 				    acpi_handle dshandle);
@@ -47,11 +45,6 @@ static inline void register_dock_dependent_device(struct acpi_device *adev,
 static inline int dock_notify(struct acpi_device *adev, u32 event) { return -ENODEV; }
 static inline void acpi_dock_add(struct acpi_device *adev) {}
 #endif
-#ifdef CONFIG_ACPI_HOTPLUG_MEMORY
-void acpi_memory_hotplug_init(void);
-#else
-static inline void acpi_memory_hotplug_init(void) {}
-#endif
 #ifdef CONFIG_X86
 void acpi_cmos_rtc_init(void);
 #else
@@ -72,11 +65,7 @@ int acpi_debugfs_init(void);
 #else
 static inline void acpi_debugfs_init(void) { return; }
 #endif
-#ifdef CONFIG_X86_INTEL_LPSS
 void acpi_lpss_init(void);
-#else
-static inline void acpi_lpss_init(void) {}
-#endif
 
 acpi_status acpi_hotplug_schedule(struct acpi_device *adev, u32 src);
 bool acpi_queue_hotplug_work(struct work_struct *work);
@@ -180,8 +169,7 @@ static inline void suspend_nvs_restore(void) {}
   -------------------------------------------------------------------------- */
 struct platform_device;
 
-int acpi_create_platform_device(struct acpi_device *adev,
-				const struct acpi_device_id *id);
+struct platform_device *acpi_create_platform_device(struct acpi_device *adev);
 
 /*--------------------------------------------------------------------------
 					Video
diff --git a/drivers/acpi/nvs.c b/drivers/acpi/nvs.c
index de4fe03873c..85287b8fe3a 100644
--- a/drivers/acpi/nvs.c
+++ b/drivers/acpi/nvs.c
@@ -139,8 +139,8 @@ void suspend_nvs_free(void)
 					iounmap(entry->kaddr);
 					entry->unmap = false;
 				} else {
-					acpi_os_unmap_memory(entry->kaddr,
-							     entry->size);
+					acpi_os_unmap_iomem(entry->kaddr,
+							    entry->size);
 				}
 				entry->kaddr = NULL;
 			}
diff --git a/drivers/acpi/osl.c b/drivers/acpi/osl.c
index 6776c599816..147bc1b91b4 100644
--- a/drivers/acpi/osl.c
+++ b/drivers/acpi/osl.c
@@ -355,7 +355,7 @@ static void acpi_unmap(acpi_physical_address pg_off, void __iomem *vaddr)
 }
 
 void __iomem *__init_refok
-acpi_os_map_memory(acpi_physical_address phys, acpi_size size)
+acpi_os_map_iomem(acpi_physical_address phys, acpi_size size)
 {
 	struct acpi_ioremap *map;
 	void __iomem *virt;
@@ -401,10 +401,17 @@ acpi_os_map_memory(acpi_physical_address phys, acpi_size size)
 
 	list_add_tail_rcu(&map->list, &acpi_ioremaps);
 
- out:
+out:
 	mutex_unlock(&acpi_ioremap_lock);
 	return map->virt + (phys - map->phys);
 }
+EXPORT_SYMBOL_GPL(acpi_os_map_iomem);
+
+void *__init_refok
+acpi_os_map_memory(acpi_physical_address phys, acpi_size size)
+{
+	return (void *)acpi_os_map_iomem(phys, size);
+}
 EXPORT_SYMBOL_GPL(acpi_os_map_memory);
 
 static void acpi_os_drop_map_ref(struct acpi_ioremap *map)
@@ -422,7 +429,7 @@ static void acpi_os_map_cleanup(struct acpi_ioremap *map)
 	}
 }
 
-void __ref acpi_os_unmap_memory(void __iomem *virt, acpi_size size)
+void __ref acpi_os_unmap_iomem(void __iomem *virt, acpi_size size)
 {
 	struct acpi_ioremap *map;
 
@@ -443,6 +450,12 @@ void __ref acpi_os_unmap_memory(void __iomem *virt, acpi_size size)
 
 	acpi_os_map_cleanup(map);
 }
+EXPORT_SYMBOL_GPL(acpi_os_unmap_iomem);
+
+void __ref acpi_os_unmap_memory(void *virt, acpi_size size)
+{
+	return acpi_os_unmap_iomem((void __iomem *)virt, size);
+}
 EXPORT_SYMBOL_GPL(acpi_os_unmap_memory);
 
 void __init early_acpi_os_unmap_memory(void __iomem *virt, acpi_size size)
@@ -464,7 +477,7 @@ int acpi_os_map_generic_address(struct acpi_generic_address *gas)
 	if (!addr || !gas->bit_width)
 		return -EINVAL;
 
-	virt = acpi_os_map_memory(addr, gas->bit_width / 8);
+	virt = acpi_os_map_iomem(addr, gas->bit_width / 8);
 	if (!virt)
 		return -EIO;
 
@@ -1770,16 +1783,15 @@ acpi_status acpi_os_release_object(acpi_cache_t * cache, void *object)
 }
 #endif
 
-static int __init acpi_no_auto_ssdt_setup(char *s)
+static int __init acpi_no_static_ssdt_setup(char *s)
 {
-        printk(KERN_NOTICE PREFIX "SSDT auto-load disabled\n");
+	acpi_gbl_disable_ssdt_table_install = TRUE;
+	pr_info("ACPI: static SSDT installation disabled\n");
 
-        acpi_gbl_disable_ssdt_table_load = TRUE;
-
-        return 1;
+	return 0;
 }
 
-__setup("acpi_no_auto_ssdt", acpi_no_auto_ssdt_setup);
+early_param("acpi_no_static_ssdt", acpi_no_static_ssdt_setup);
 
 static int __init acpi_disable_return_repair(char *s)
 {
diff --git a/drivers/acpi/processor_driver.c b/drivers/acpi/processor_driver.c
index 7f70f3182d5..4fcbd670415 100644
--- a/drivers/acpi/processor_driver.c
+++ b/drivers/acpi/processor_driver.c
@@ -121,6 +121,13 @@ static int acpi_cpu_soft_notify(struct notifier_block *nfb,
 	struct acpi_processor *pr = per_cpu(processors, cpu);
 	struct acpi_device *device;
 
+	/*
+	 * CPU_STARTING and CPU_DYING must not sleep. Return here since
+	 * acpi_bus_get_device() may sleep.
+	 */
+	if (action == CPU_STARTING || action == CPU_DYING)
+		return NOTIFY_DONE;
+
 	if (!pr || acpi_bus_get_device(pr->handle, &device))
 		return NOTIFY_DONE;
 
diff --git a/drivers/acpi/scan.c b/drivers/acpi/scan.c
index 7efe546a8c4..f775fa0d850 100644
--- a/drivers/acpi/scan.c
+++ b/drivers/acpi/scan.c
@@ -84,7 +84,7 @@ EXPORT_SYMBOL_GPL(acpi_initialize_hp_context);
 
 int acpi_scan_add_handler(struct acpi_scan_handler *handler)
 {
-	if (!handler || !handler->attach)
+	if (!handler)
 		return -EINVAL;
 
 	list_add_tail(&handler->list_node, &acpi_scan_handlers_list);
@@ -1551,9 +1551,13 @@ static void acpi_bus_get_power_flags(struct acpi_device *device)
 	 */
 	if (acpi_has_method(device->handle, "_PSC"))
 		device->power.flags.explicit_get = 1;
+
 	if (acpi_has_method(device->handle, "_IRC"))
 		device->power.flags.inrush_current = 1;
 
+	if (acpi_has_method(device->handle, "_DSW"))
+		device->power.flags.dsw_present = 1;
+
 	/*
 	 * Enumerate supported power management states
 	 */
@@ -1793,8 +1797,10 @@ static void acpi_set_pnp_ids(acpi_handle handle, struct acpi_device_pnp *pnp,
 			return;
 		}
 
-		if (info->valid & ACPI_VALID_HID)
+		if (info->valid & ACPI_VALID_HID) {
 			acpi_add_id(pnp, info->hardware_id.string);
+			pnp->type.platform_id = 1;
+		}
 		if (info->valid & ACPI_VALID_CID) {
 			cid_list = &info->compatible_id_list;
 			for (i = 0; i < cid_list->count; i++)
@@ -1973,6 +1979,9 @@ static bool acpi_scan_handler_matching(struct acpi_scan_handler *handler,
 {
 	const struct acpi_device_id *devid;
 
+	if (handler->match)
+		return handler->match(idstr, matchid);
+
 	for (devid = handler->ids; devid->id[0]; devid++)
 		if (!strcmp((char *)devid->id, idstr)) {
 			if (matchid)
@@ -2061,6 +2070,44 @@ static acpi_status acpi_bus_check_add(acpi_handle handle, u32 lvl_not_used,
 	return AE_OK;
 }
 
+static int acpi_check_spi_i2c_slave(struct acpi_resource *ares, void *data)
+{
+	bool *is_spi_i2c_slave_p = data;
+
+	if (ares->type != ACPI_RESOURCE_TYPE_SERIAL_BUS)
+		return 1;
+
+	/*
+	 * devices that are connected to UART still need to be enumerated to
+	 * platform bus
+	 */
+	if (ares->data.common_serial_bus.type != ACPI_RESOURCE_SERIAL_TYPE_UART)
+		*is_spi_i2c_slave_p = true;
+
+	 /* no need to do more checking */
+	return -1;
+}
+
+static void acpi_default_enumeration(struct acpi_device *device)
+{
+	struct list_head resource_list;
+	bool is_spi_i2c_slave = false;
+
+	if (!device->pnp.type.platform_id || device->handler)
+		return;
+
+	/*
+	 * Do not enemerate SPI/I2C slaves as they will be enuerated by their
+	 * respective parents.
+	 */
+	INIT_LIST_HEAD(&resource_list);
+	acpi_dev_get_resources(device, &resource_list, acpi_check_spi_i2c_slave,
+			       &is_spi_i2c_slave);
+	acpi_dev_free_resource_list(&resource_list);
+	if (!is_spi_i2c_slave)
+		acpi_create_platform_device(device);
+}
+
 static int acpi_scan_attach_handler(struct acpi_device *device)
 {
 	struct acpi_hardware_id *hwid;
@@ -2072,6 +2119,10 @@ static int acpi_scan_attach_handler(struct acpi_device *device)
 
 		handler = acpi_scan_match_handler(hwid->id, &devid);
 		if (handler) {
+			if (!handler->attach) {
+				device->pnp.type.platform_id = 0;
+				continue;
+			}
 			device->handler = handler;
 			ret = handler->attach(device, devid);
 			if (ret > 0)
@@ -2082,6 +2133,9 @@ static int acpi_scan_attach_handler(struct acpi_device *device)
 				break;
 		}
 	}
+	if (!ret)
+		acpi_default_enumeration(device);
+
 	return ret;
 }
 
@@ -2241,11 +2295,11 @@ int __init acpi_scan_init(void)
 	acpi_pci_root_init();
 	acpi_pci_link_init();
 	acpi_processor_init();
-	acpi_platform_init();
 	acpi_lpss_init();
 	acpi_cmos_rtc_init();
 	acpi_container_init();
 	acpi_memory_hotplug_init();
+	acpi_pnp_init();
 
 	mutex_lock(&acpi_scan_lock);
 	/*
@@ -2259,12 +2313,16 @@ int __init acpi_scan_init(void)
 	if (result)
 		goto out;
 
-	result = acpi_bus_scan_fixed();
-	if (result) {
-		acpi_detach_data(acpi_root->handle, acpi_scan_drop_device);
-		acpi_device_del(acpi_root);
-		put_device(&acpi_root->dev);
-		goto out;
+	/* Fixed feature devices do not exist on HW-reduced platform */
+	if (!acpi_gbl_reduced_hardware) {
+		result = acpi_bus_scan_fixed();
+		if (result) {
+			acpi_detach_data(acpi_root->handle,
+					 acpi_scan_drop_device);
+			acpi_device_del(acpi_root);
+			put_device(&acpi_root->dev);
+			goto out;
+		}
 	}
 
 	acpi_update_all_gpes();
diff --git a/drivers/acpi/sleep.c b/drivers/acpi/sleep.c
index c40fb2e81bb..c11e3795431 100644
--- a/drivers/acpi/sleep.c
+++ b/drivers/acpi/sleep.c
@@ -89,6 +89,7 @@ u32 acpi_target_system_state(void)
 {
 	return acpi_target_sleep_state;
 }
+EXPORT_SYMBOL_GPL(acpi_target_system_state);
 
 static bool pwr_btn_event_pending;
 
@@ -611,6 +612,22 @@ static const struct platform_suspend_ops acpi_suspend_ops_old = {
 	.recover = acpi_pm_finish,
 };
 
+static int acpi_freeze_begin(void)
+{
+	acpi_scan_lock_acquire();
+	return 0;
+}
+
+static void acpi_freeze_end(void)
+{
+	acpi_scan_lock_release();
+}
+
+static const struct platform_freeze_ops acpi_freeze_ops = {
+	.begin = acpi_freeze_begin,
+	.end = acpi_freeze_end,
+};
+
 static void acpi_sleep_suspend_setup(void)
 {
 	int i;
@@ -621,7 +638,9 @@ static void acpi_sleep_suspend_setup(void)
 
 	suspend_set_ops(old_suspend_ordering ?
 		&acpi_suspend_ops_old : &acpi_suspend_ops);
+	freeze_set_ops(&acpi_freeze_ops);
 }
+
 #else /* !CONFIG_SUSPEND */
 static inline void acpi_sleep_suspend_setup(void) {}
 #endif /* !CONFIG_SUSPEND */
diff --git a/drivers/acpi/tables.c b/drivers/acpi/tables.c
index 21782290df4..05550ba44d3 100644
--- a/drivers/acpi/tables.c
+++ b/drivers/acpi/tables.c
@@ -44,6 +44,12 @@ static struct acpi_table_desc initial_tables[ACPI_MAX_TABLES] __initdata;
 
 static int acpi_apic_instance __initdata;
 
+/*
+ * Disable table checksum verification for the early stage due to the size
+ * limitation of the current x86 early mapping implementation.
+ */
+static bool acpi_verify_table_checksum __initdata = false;
+
 void acpi_table_print_madt_entry(struct acpi_subtable_header *header)
 {
 	if (!header)
@@ -333,6 +339,14 @@ int __init acpi_table_init(void)
 {
 	acpi_status status;
 
+	if (acpi_verify_table_checksum) {
+		pr_info("Early table checksum verification enabled\n");
+		acpi_gbl_verify_table_checksum = TRUE;
+	} else {
+		pr_info("Early table checksum verification disabled\n");
+		acpi_gbl_verify_table_checksum = FALSE;
+	}
+
 	status = acpi_initialize_tables(initial_tables, ACPI_MAX_TABLES, 0);
 	if (ACPI_FAILURE(status))
 		return -EINVAL;
@@ -354,3 +368,12 @@ static int __init acpi_parse_apic_instance(char *str)
 }
 
 early_param("acpi_apic_instance", acpi_parse_apic_instance);
+
+static int __init acpi_force_table_verification_setup(char *s)
+{
+	acpi_verify_table_checksum = true;
+
+	return 0;
+}
+
+early_param("acpi_force_table_verification", acpi_force_table_verification_setup);
diff --git a/drivers/acpi/thermal.c b/drivers/acpi/thermal.c
index c1e31a41f94..112817e963e 100644
--- a/drivers/acpi/thermal.c
+++ b/drivers/acpi/thermal.c
@@ -925,13 +925,10 @@ static int acpi_thermal_register_thermal_zone(struct acpi_thermal *tz)
 	if (result)
 		return result;
 
-	status = acpi_attach_data(tz->device->handle,
-				  acpi_bus_private_data_handler,
-				  tz->thermal_zone);
-	if (ACPI_FAILURE(status)) {
-		pr_err(PREFIX "Error attaching device data\n");
+	status =  acpi_bus_attach_private_data(tz->device->handle,
+					       tz->thermal_zone);
+	if (ACPI_FAILURE(status))
 		return -ENODEV;
-	}
 
 	tz->tz_enabled = 1;
 
@@ -946,7 +943,7 @@ static void acpi_thermal_unregister_thermal_zone(struct acpi_thermal *tz)
 	sysfs_remove_link(&tz->thermal_zone->device.kobj, "device");
 	thermal_zone_device_unregister(tz->thermal_zone);
 	tz->thermal_zone = NULL;
-	acpi_detach_data(tz->device->handle, acpi_bus_private_data_handler);
+	acpi_bus_detach_private_data(tz->device->handle);
 }
 
 
@@ -1278,8 +1275,8 @@ static int __init acpi_thermal_init(void)
 
 static void __exit acpi_thermal_exit(void)
 {
-	destroy_workqueue(acpi_thermal_pm_queue);
 	acpi_bus_unregister_driver(&acpi_thermal_driver);
+	destroy_workqueue(acpi_thermal_pm_queue);
 
 	return;
 }
diff --git a/drivers/acpi/utils.c b/drivers/acpi/utils.c
index bba52614858..07c8c5a5ee9 100644
--- a/drivers/acpi/utils.c
+++ b/drivers/acpi/utils.c
@@ -30,6 +30,7 @@
 #include <linux/types.h>
 #include <linux/hardirq.h>
 #include <linux/acpi.h>
+#include <linux/dynamic_debug.h>
 
 #include "internal.h"
 
@@ -457,6 +458,24 @@ acpi_evaluate_ost(acpi_handle handle, u32 source_event, u32 status_code,
 EXPORT_SYMBOL(acpi_evaluate_ost);
 
 /**
+ * acpi_handle_path: Return the object path of handle
+ *
+ * Caller must free the returned buffer
+ */
+static char *acpi_handle_path(acpi_handle handle)
+{
+	struct acpi_buffer buffer = {
+		.length = ACPI_ALLOCATE_BUFFER,
+		.pointer = NULL
+	};
+
+	if (in_interrupt() ||
+	    acpi_get_name(handle, ACPI_FULL_PATHNAME, &buffer) != AE_OK)
+		return NULL;
+	return buffer.pointer;
+}
+
+/**
  * acpi_handle_printk: Print message with ACPI prefix and object path
  *
  * This function is called through acpi_handle_<level> macros and prints
@@ -469,29 +488,50 @@ acpi_handle_printk(const char *level, acpi_handle handle, const char *fmt, ...)
 {
 	struct va_format vaf;
 	va_list args;
-	struct acpi_buffer buffer = {
-		.length = ACPI_ALLOCATE_BUFFER,
-		.pointer = NULL
-	};
 	const char *path;
 
 	va_start(args, fmt);
 	vaf.fmt = fmt;
 	vaf.va = &args;
 
-	if (in_interrupt() ||
-	    acpi_get_name(handle, ACPI_FULL_PATHNAME, &buffer) != AE_OK)
-		path = "<n/a>";
-	else
-		path = buffer.pointer;
-
-	printk("%sACPI: %s: %pV", level, path, &vaf);
+	path = acpi_handle_path(handle);
+	printk("%sACPI: %s: %pV", level, path ? path : "<n/a>" , &vaf);
 
 	va_end(args);
-	kfree(buffer.pointer);
+	kfree(path);
 }
 EXPORT_SYMBOL(acpi_handle_printk);
 
+#if defined(CONFIG_DYNAMIC_DEBUG)
+/**
+ * __acpi_handle_debug: pr_debug with ACPI prefix and object path
+ *
+ * This function is called through acpi_handle_debug macro and debug
+ * prints a message with ACPI prefix and object path. This function
+ * acquires the global namespace mutex to obtain an object path.  In
+ * interrupt context, it shows the object path as <n/a>.
+ */
+void
+__acpi_handle_debug(struct _ddebug *descriptor, acpi_handle handle,
+		    const char *fmt, ...)
+{
+	struct va_format vaf;
+	va_list args;
+	const char *path;
+
+	va_start(args, fmt);
+	vaf.fmt = fmt;
+	vaf.va = &args;
+
+	path = acpi_handle_path(handle);
+	__dynamic_pr_debug(descriptor, "ACPI: %s: %pV", path ? path : "<n/a>", &vaf);
+
+	va_end(args);
+	kfree(path);
+}
+EXPORT_SYMBOL(__acpi_handle_debug);
+#endif
+
 /**
  * acpi_has_method: Check whether @handle has a method named @name
  * @handle: ACPI device handle
diff --git a/drivers/acpi/video.c b/drivers/acpi/video.c
index 8b6990e417e..101fb090dcb 100644
--- a/drivers/acpi/video.c
+++ b/drivers/acpi/video.c
@@ -68,7 +68,7 @@ MODULE_AUTHOR("Bruno Ducrot");
 MODULE_DESCRIPTION("ACPI Video Driver");
 MODULE_LICENSE("GPL");
 
-static bool brightness_switch_enabled = 1;
+static bool brightness_switch_enabled;
 module_param(brightness_switch_enabled, bool, 0644);
 
 /*
@@ -150,6 +150,8 @@ struct acpi_video_enumerated_device {
 
 struct acpi_video_bus {
 	struct acpi_device *device;
+	bool backlight_registered;
+	bool backlight_notifier_registered;
 	u8 dos_setting;
 	struct acpi_video_enumerated_device *attached_array;
 	u8 attached_count;
@@ -161,6 +163,7 @@ struct acpi_video_bus {
 	struct input_dev *input;
 	char phys[32];	/* for input device */
 	struct notifier_block pm_nb;
+	struct notifier_block backlight_nb;
 };
 
 struct acpi_video_device_flags {
@@ -457,10 +460,10 @@ static struct dmi_system_id video_dmi_table[] __initdata = {
 	},
 	{
 	 .callback = video_set_use_native_backlight,
-	 .ident = "ThinkPad T430s",
+	 .ident = "ThinkPad T430 and T430s",
 	 .matches = {
 		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T430s"),
+		DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T430"),
 		},
 	},
 	{
@@ -472,7 +475,15 @@ static struct dmi_system_id video_dmi_table[] __initdata = {
 		},
 	},
 	{
-	.callback = video_set_use_native_backlight,
+	 .callback = video_set_use_native_backlight,
+	 .ident = "ThinkPad W530",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
+		DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad W530"),
+		},
+	},
+	{
+	 .callback = video_set_use_native_backlight,
 	.ident = "ThinkPad X1 Carbon",
 	.matches = {
 		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
@@ -488,6 +499,14 @@ static struct dmi_system_id video_dmi_table[] __initdata = {
 		},
 	},
 	{
+	 .callback = video_set_use_native_backlight,
+	 .ident = "Lenovo Yoga 2 11",
+	 .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 = {
@@ -500,7 +519,7 @@ static struct dmi_system_id video_dmi_table[] __initdata = {
 	 .ident = "Dell Inspiron 7520",
 	 .matches = {
 		DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "Inspiron 7520"),
+		DMI_MATCH(DMI_PRODUCT_NAME, "Inspiron 7520"),
 		},
 	},
 	{
@@ -513,6 +532,22 @@ static struct dmi_system_id video_dmi_table[] __initdata = {
 	},
 	{
 	 .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"),
@@ -520,6 +555,14 @@ static struct dmi_system_id video_dmi_table[] __initdata = {
 		},
 	},
 	{
+	 .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 = "HP ProBook 4340s",
 	.matches = {
@@ -571,6 +614,14 @@ static struct dmi_system_id video_dmi_table[] __initdata = {
 	},
 	{
 	.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"),
@@ -1650,88 +1701,92 @@ acpi_video_bus_match(acpi_handle handle, u32 level, void *context,
 
 static void acpi_video_dev_register_backlight(struct acpi_video_device *device)
 {
-	if (acpi_video_verify_backlight_support()) {
-		struct backlight_properties props;
-		struct pci_dev *pdev;
-		acpi_handle acpi_parent;
-		struct device *parent = NULL;
-		int result;
-		static int count;
-		char *name;
-
-		result = acpi_video_init_brightness(device);
-		if (result)
-			return;
-		name = kasprintf(GFP_KERNEL, "acpi_video%d", count);
-		if (!name)
-			return;
-		count++;
+	struct backlight_properties props;
+	struct pci_dev *pdev;
+	acpi_handle acpi_parent;
+	struct device *parent = NULL;
+	int result;
+	static int count;
+	char *name;
 
-		acpi_get_parent(device->dev->handle, &acpi_parent);
+	result = acpi_video_init_brightness(device);
+	if (result)
+		return;
+	name = kasprintf(GFP_KERNEL, "acpi_video%d", count);
+	if (!name)
+		return;
+	count++;
 
-		pdev = acpi_get_pci_dev(acpi_parent);
-		if (pdev) {
-			parent = &pdev->dev;
-			pci_dev_put(pdev);
-		}
+	acpi_get_parent(device->dev->handle, &acpi_parent);
 
-		memset(&props, 0, sizeof(struct backlight_properties));
-		props.type = BACKLIGHT_FIRMWARE;
-		props.max_brightness = device->brightness->count - 3;
-		device->backlight = backlight_device_register(name,
-							      parent,
-							      device,
-							      &acpi_backlight_ops,
-							      &props);
-		kfree(name);
-		if (IS_ERR(device->backlight))
-			return;
+	pdev = acpi_get_pci_dev(acpi_parent);
+	if (pdev) {
+		parent = &pdev->dev;
+		pci_dev_put(pdev);
+	}
 
-		/*
-		 * Save current brightness level in case we have to restore it
-		 * before acpi_video_device_lcd_set_level() is called next time.
-		 */
-		device->backlight->props.brightness =
-				acpi_video_get_brightness(device->backlight);
+	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_FIRMWARE;
+	props.max_brightness = device->brightness->count - 3;
+	device->backlight = backlight_device_register(name,
+						      parent,
+						      device,
+						      &acpi_backlight_ops,
+						      &props);
+	kfree(name);
+	if (IS_ERR(device->backlight))
+		return;
 
-		device->cooling_dev = thermal_cooling_device_register("LCD",
-					device->dev, &video_cooling_ops);
-		if (IS_ERR(device->cooling_dev)) {
-			/*
-			 * Set cooling_dev to NULL so we don't crash trying to
-			 * free it.
-			 * Also, why the hell we are returning early and
-			 * not attempt to register video output if cooling
-			 * device registration failed?
-			 * -- dtor
-			 */
-			device->cooling_dev = NULL;
-			return;
-		}
+	/*
+	 * Save current brightness level in case we have to restore it
+	 * before acpi_video_device_lcd_set_level() is called next time.
+	 */
+	device->backlight->props.brightness =
+			acpi_video_get_brightness(device->backlight);
 
-		dev_info(&device->dev->dev, "registered as cooling_device%d\n",
-			 device->cooling_dev->id);
-		result = sysfs_create_link(&device->dev->dev.kobj,
-				&device->cooling_dev->device.kobj,
-				"thermal_cooling");
-		if (result)
-			printk(KERN_ERR PREFIX "Create sysfs link\n");
-		result = sysfs_create_link(&device->cooling_dev->device.kobj,
-				&device->dev->dev.kobj, "device");
-		if (result)
-			printk(KERN_ERR PREFIX "Create sysfs link\n");
+	device->cooling_dev = thermal_cooling_device_register("LCD",
+				device->dev, &video_cooling_ops);
+	if (IS_ERR(device->cooling_dev)) {
+		/*
+		 * Set cooling_dev to NULL so we don't crash trying to free it.
+		 * Also, why the hell we are returning early and not attempt to
+		 * register video output if cooling device registration failed?
+		 * -- dtor
+		 */
+		device->cooling_dev = NULL;
+		return;
 	}
+
+	dev_info(&device->dev->dev, "registered as cooling_device%d\n",
+		 device->cooling_dev->id);
+	result = sysfs_create_link(&device->dev->dev.kobj,
+			&device->cooling_dev->device.kobj,
+			"thermal_cooling");
+	if (result)
+		printk(KERN_ERR PREFIX "Create sysfs link\n");
+	result = sysfs_create_link(&device->cooling_dev->device.kobj,
+			&device->dev->dev.kobj, "device");
+	if (result)
+		printk(KERN_ERR PREFIX "Create sysfs link\n");
 }
 
 static int acpi_video_bus_register_backlight(struct acpi_video_bus *video)
 {
 	struct acpi_video_device *dev;
 
+	if (video->backlight_registered)
+		return 0;
+
+	if (!acpi_video_verify_backlight_support())
+		return 0;
+
 	mutex_lock(&video->device_list_lock);
 	list_for_each_entry(dev, &video->video_device_list, entry)
 		acpi_video_dev_register_backlight(dev);
 	mutex_unlock(&video->device_list_lock);
 
+	video->backlight_registered = true;
+
 	video->pm_nb.notifier_call = acpi_video_resume;
 	video->pm_nb.priority = 0;
 	return register_pm_notifier(&video->pm_nb);
@@ -1759,13 +1814,20 @@ static void acpi_video_dev_unregister_backlight(struct acpi_video_device *device
 static int acpi_video_bus_unregister_backlight(struct acpi_video_bus *video)
 {
 	struct acpi_video_device *dev;
-	int error = unregister_pm_notifier(&video->pm_nb);
+	int error;
+
+	if (!video->backlight_registered)
+		return 0;
+
+	error = unregister_pm_notifier(&video->pm_nb);
 
 	mutex_lock(&video->device_list_lock);
 	list_for_each_entry(dev, &video->video_device_list, entry)
 		acpi_video_dev_unregister_backlight(dev);
 	mutex_unlock(&video->device_list_lock);
 
+	video->backlight_registered = false;
+
 	return error;
 }
 
@@ -1859,6 +1921,56 @@ static void acpi_video_bus_remove_notify_handler(struct acpi_video_bus *video)
 	video->input = NULL;
 }
 
+static int acpi_video_backlight_notify(struct notifier_block *nb,
+					unsigned long val, void *bd)
+{
+	struct backlight_device *backlight = bd;
+	struct acpi_video_bus *video;
+
+	/* acpi_video_verify_backlight_support only cares about raw devices */
+	if (backlight->props.type != BACKLIGHT_RAW)
+		return NOTIFY_DONE;
+
+	video = container_of(nb, struct acpi_video_bus, backlight_nb);
+
+	switch (val) {
+	case BACKLIGHT_REGISTERED:
+		if (!acpi_video_verify_backlight_support())
+			acpi_video_bus_unregister_backlight(video);
+		break;
+	case BACKLIGHT_UNREGISTERED:
+		acpi_video_bus_register_backlight(video);
+		break;
+	}
+
+	return NOTIFY_OK;
+}
+
+static int acpi_video_bus_add_backlight_notify_handler(
+						struct acpi_video_bus *video)
+{
+	int error;
+
+	video->backlight_nb.notifier_call = acpi_video_backlight_notify;
+	video->backlight_nb.priority = 0;
+	error = backlight_register_notifier(&video->backlight_nb);
+	if (error == 0)
+		video->backlight_notifier_registered = true;
+
+	return error;
+}
+
+static int acpi_video_bus_remove_backlight_notify_handler(
+						struct acpi_video_bus *video)
+{
+	if (!video->backlight_notifier_registered)
+		return 0;
+
+	video->backlight_notifier_registered = false;
+
+	return backlight_unregister_notifier(&video->backlight_nb);
+}
+
 static int acpi_video_bus_put_devices(struct acpi_video_bus *video)
 {
 	struct acpi_video_device *dev, *next;
@@ -1940,6 +2052,7 @@ static int acpi_video_bus_add(struct acpi_device *device)
 
 	acpi_video_bus_register_backlight(video);
 	acpi_video_bus_add_notify_handler(video);
+	acpi_video_bus_add_backlight_notify_handler(video);
 
 	return 0;
 
@@ -1963,6 +2076,7 @@ static int acpi_video_bus_remove(struct acpi_device *device)
 
 	video = acpi_driver_data(device);
 
+	acpi_video_bus_remove_backlight_notify_handler(video);
 	acpi_video_bus_remove_notify_handler(video);
 	acpi_video_bus_unregister_backlight(video);
 	acpi_video_bus_put_devices(video);
@@ -2053,6 +2167,20 @@ void acpi_video_unregister(void)
 }
 EXPORT_SYMBOL(acpi_video_unregister);
 
+void acpi_video_unregister_backlight(void)
+{
+	struct acpi_video_bus *video;
+
+	if (!register_count)
+		return;
+
+	mutex_lock(&video_list_lock);
+	list_for_each_entry(video, &video_bus_head, entry)
+		acpi_video_bus_unregister_backlight(video);
+	mutex_unlock(&video_list_lock);
+}
+EXPORT_SYMBOL(acpi_video_unregister_backlight);
+
 /*
  * This is kind of nasty. Hardware using Intel chipsets may require
  * the video opregion code to be run first in order to initialise
diff --git a/drivers/ata/Kconfig b/drivers/ata/Kconfig
index c2706047337..7671dbac601 100644
--- a/drivers/ata/Kconfig
+++ b/drivers/ata/Kconfig
@@ -123,6 +123,15 @@ config AHCI_IMX
 
 	  If unsure, say N.
 
+config AHCI_MVEBU
+	tristate "Marvell EBU AHCI SATA support"
+	depends on ARCH_MVEBU
+	help
+	  This option enables support for the Marvebu EBU SoC's
+	  onboard AHCI SATA.
+
+	  If unsure, say N.
+
 config AHCI_SUNXI
 	tristate "Allwinner sunxi AHCI SATA support"
 	depends on ARCH_SUNXI
@@ -815,7 +824,7 @@ config PATA_AT32
 
 config PATA_AT91
 	tristate "PATA support for AT91SAM9260"
-	depends on ARM && ARCH_AT91
+	depends on ARM && SOC_AT91SAM9
 	help
 	  This option enables support for IDE devices on the Atmel AT91SAM9260 SoC.
 
diff --git a/drivers/ata/Makefile b/drivers/ata/Makefile
index 44c8016e565..5a02aeecef5 100644
--- a/drivers/ata/Makefile
+++ b/drivers/ata/Makefile
@@ -12,6 +12,7 @@ obj-$(CONFIG_SATA_DWC)		+= sata_dwc_460ex.o
 obj-$(CONFIG_SATA_HIGHBANK)	+= sata_highbank.o libahci.o
 obj-$(CONFIG_AHCI_DA850)	+= ahci_da850.o libahci.o libahci_platform.o
 obj-$(CONFIG_AHCI_IMX)		+= ahci_imx.o libahci.o libahci_platform.o
+obj-$(CONFIG_AHCI_MVEBU)	+= ahci_mvebu.o libahci.o libahci_platform.o
 obj-$(CONFIG_AHCI_SUNXI)	+= ahci_sunxi.o libahci.o libahci_platform.o
 obj-$(CONFIG_AHCI_ST)		+= ahci_st.o libahci.o libahci_platform.o
 obj-$(CONFIG_AHCI_XGENE)	+= ahci_xgene.o libahci.o libahci_platform.o
diff --git a/drivers/ata/acard-ahci.c b/drivers/ata/acard-ahci.c
index b51605ac597..0cd7c7a39e5 100644
--- a/drivers/ata/acard-ahci.c
+++ b/drivers/ata/acard-ahci.c
@@ -77,7 +77,7 @@ static bool acard_ahci_qc_fill_rtf(struct ata_queued_cmd *qc);
 static int acard_ahci_port_start(struct ata_port *ap);
 static int acard_ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent);
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int acard_ahci_pci_device_suspend(struct pci_dev *pdev, pm_message_t mesg);
 static int acard_ahci_pci_device_resume(struct pci_dev *pdev);
 #endif
@@ -118,13 +118,13 @@ static struct pci_driver acard_ahci_pci_driver = {
 	.id_table		= acard_ahci_pci_tbl,
 	.probe			= acard_ahci_init_one,
 	.remove			= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend		= acard_ahci_pci_device_suspend,
 	.resume			= acard_ahci_pci_device_resume,
 #endif
 };
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int acard_ahci_pci_device_suspend(struct pci_dev *pdev, pm_message_t mesg)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
diff --git a/drivers/ata/ahci.c b/drivers/ata/ahci.c
index 71e15b73513..dae5607e111 100644
--- a/drivers/ata/ahci.c
+++ b/drivers/ata/ahci.c
@@ -445,10 +445,14 @@ static const struct pci_device_id ahci_pci_tbl[] = {
 	  .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 */
+	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL_EXT, 0x91a0),
+	  .driver_data = board_ahci_yes_fbs },
 	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL_EXT, 0x91a3),
 	  .driver_data = board_ahci_yes_fbs },
 	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL_EXT, 0x9230),
 	  .driver_data = board_ahci_yes_fbs },
+	{ PCI_DEVICE(PCI_VENDOR_ID_TTI, 0x0642),
+	  .driver_data = board_ahci_yes_fbs },
 
 	/* Promise */
 	{ PCI_VDEVICE(PROMISE, 0x3f20), board_ahci },	/* PDC42819 */
@@ -1115,6 +1119,17 @@ static bool ahci_broken_online(struct pci_dev *pdev)
 	return pdev->bus->number == (val >> 8) && pdev->devfn == (val & 0xff);
 }
 
+static bool ahci_broken_devslp(struct pci_dev *pdev)
+{
+	/* device with broken DEVSLP but still showing SDS capability */
+	static const struct pci_device_id ids[] = {
+		{ PCI_VDEVICE(INTEL, 0x0f23)}, /* Valleyview SoC */
+		{}
+	};
+
+	return pci_match_id(ids, pdev);
+}
+
 #ifdef CONFIG_ATA_ACPI
 static void ahci_gtf_filter_workaround(struct ata_host *host)
 {
@@ -1364,6 +1379,10 @@ static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	hpriv->mmio = pcim_iomap_table(pdev)[ahci_pci_bar];
 
+	/* must set flag prior to save config in order to take effect */
+	if (ahci_broken_devslp(pdev))
+		hpriv->flags |= AHCI_HFLAG_NO_DEVSLP;
+
 	/* save initial config */
 	ahci_pci_save_initial_config(pdev, hpriv);
 
diff --git a/drivers/ata/ahci.h b/drivers/ata/ahci.h
index b5eb886da22..05882e4445a 100644
--- a/drivers/ata/ahci.h
+++ b/drivers/ata/ahci.h
@@ -236,6 +236,8 @@ enum {
 						        port start (wait until
 						        error-handling stage) */
 	AHCI_HFLAG_MULTI_MSI		= (1 << 16), /* multiple PCI MSIs */
+	AHCI_HFLAG_NO_DEVSLP		= (1 << 17), /* no device sleep */
+	AHCI_HFLAG_NO_FBS		= (1 << 18), /* no FBS */
 
 	/* ap->flags bits */
 
diff --git a/drivers/ata/ahci_da850.c b/drivers/ata/ahci_da850.c
index 2c83613ce2d..2b77d53bccf 100644
--- a/drivers/ata/ahci_da850.c
+++ b/drivers/ata/ahci_da850.c
@@ -85,7 +85,8 @@ static int ahci_da850_probe(struct platform_device *pdev)
 
 	da850_sata_init(dev, pwrdn_reg, hpriv->mmio);
 
-	rc = ahci_platform_init_host(pdev, hpriv, &ahci_da850_port_info, 0, 0);
+	rc = ahci_platform_init_host(pdev, hpriv, &ahci_da850_port_info,
+				     0, 0, 0);
 	if (rc)
 		goto disable_resources;
 
diff --git a/drivers/ata/ahci_imx.c b/drivers/ata/ahci_imx.c
index 497c7abe1c7..3a901520c62 100644
--- a/drivers/ata/ahci_imx.c
+++ b/drivers/ata/ahci_imx.c
@@ -29,9 +29,25 @@
 #include "ahci.h"
 
 enum {
-	PORT_PHY_CTL = 0x178,			/* Port0 PHY Control */
-	PORT_PHY_CTL_PDDQ_LOC = 0x100000,	/* PORT_PHY_CTL bits */
-	HOST_TIMER1MS = 0xe0,			/* Timer 1-ms */
+	/* Timer 1-ms Register */
+	IMX_TIMER1MS				= 0x00e0,
+	/* Port0 PHY Control Register */
+	IMX_P0PHYCR				= 0x0178,
+	IMX_P0PHYCR_TEST_PDDQ			= 1 << 20,
+	IMX_P0PHYCR_CR_READ			= 1 << 19,
+	IMX_P0PHYCR_CR_WRITE			= 1 << 18,
+	IMX_P0PHYCR_CR_CAP_DATA			= 1 << 17,
+	IMX_P0PHYCR_CR_CAP_ADDR			= 1 << 16,
+	/* Port0 PHY Status Register */
+	IMX_P0PHYSR				= 0x017c,
+	IMX_P0PHYSR_CR_ACK			= 1 << 18,
+	IMX_P0PHYSR_CR_DATA_OUT			= 0xffff << 0,
+	/* Lane0 Output Status Register */
+	IMX_LANE0_OUT_STAT			= 0x2003,
+	IMX_LANE0_OUT_STAT_RX_PLL_STATE		= 1 << 1,
+	/* Clock Reset Register */
+	IMX_CLOCK_RESET				= 0x7f3f,
+	IMX_CLOCK_RESET_RESET			= 1 << 0,
 };
 
 enum ahci_imx_type {
@@ -54,9 +70,149 @@ MODULE_PARM_DESC(hotplug, "AHCI IMX hot-plug support (0=Don't support, 1=support
 
 static void ahci_imx_host_stop(struct ata_host *host);
 
+static int imx_phy_crbit_assert(void __iomem *mmio, u32 bit, bool assert)
+{
+	int timeout = 10;
+	u32 crval;
+	u32 srval;
+
+	/* Assert or deassert the bit */
+	crval = readl(mmio + IMX_P0PHYCR);
+	if (assert)
+		crval |= bit;
+	else
+		crval &= ~bit;
+	writel(crval, mmio + IMX_P0PHYCR);
+
+	/* Wait for the cr_ack signal */
+	do {
+		srval = readl(mmio + IMX_P0PHYSR);
+		if ((assert ? srval : ~srval) & IMX_P0PHYSR_CR_ACK)
+			break;
+		usleep_range(100, 200);
+	} while (--timeout);
+
+	return timeout ? 0 : -ETIMEDOUT;
+}
+
+static int imx_phy_reg_addressing(u16 addr, void __iomem *mmio)
+{
+	u32 crval = addr;
+	int ret;
+
+	/* Supply the address on cr_data_in */
+	writel(crval, mmio + IMX_P0PHYCR);
+
+	/* Assert the cr_cap_addr signal */
+	ret = imx_phy_crbit_assert(mmio, IMX_P0PHYCR_CR_CAP_ADDR, true);
+	if (ret)
+		return ret;
+
+	/* Deassert cr_cap_addr */
+	ret = imx_phy_crbit_assert(mmio, IMX_P0PHYCR_CR_CAP_ADDR, false);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int imx_phy_reg_write(u16 val, void __iomem *mmio)
+{
+	u32 crval = val;
+	int ret;
+
+	/* Supply the data on cr_data_in */
+	writel(crval, mmio + IMX_P0PHYCR);
+
+	/* Assert the cr_cap_data signal */
+	ret = imx_phy_crbit_assert(mmio, IMX_P0PHYCR_CR_CAP_DATA, true);
+	if (ret)
+		return ret;
+
+	/* Deassert cr_cap_data */
+	ret = imx_phy_crbit_assert(mmio, IMX_P0PHYCR_CR_CAP_DATA, false);
+	if (ret)
+		return ret;
+
+	if (val & IMX_CLOCK_RESET_RESET) {
+		/*
+		 * In case we're resetting the phy, it's unable to acknowledge,
+		 * so we return immediately here.
+		 */
+		crval |= IMX_P0PHYCR_CR_WRITE;
+		writel(crval, mmio + IMX_P0PHYCR);
+		goto out;
+	}
+
+	/* Assert the cr_write signal */
+	ret = imx_phy_crbit_assert(mmio, IMX_P0PHYCR_CR_WRITE, true);
+	if (ret)
+		return ret;
+
+	/* Deassert cr_write */
+	ret = imx_phy_crbit_assert(mmio, IMX_P0PHYCR_CR_WRITE, false);
+	if (ret)
+		return ret;
+
+out:
+	return 0;
+}
+
+static int imx_phy_reg_read(u16 *val, void __iomem *mmio)
+{
+	int ret;
+
+	/* Assert the cr_read signal */
+	ret = imx_phy_crbit_assert(mmio, IMX_P0PHYCR_CR_READ, true);
+	if (ret)
+		return ret;
+
+	/* Capture the data from cr_data_out[] */
+	*val = readl(mmio + IMX_P0PHYSR) & IMX_P0PHYSR_CR_DATA_OUT;
+
+	/* Deassert cr_read */
+	ret = imx_phy_crbit_assert(mmio, IMX_P0PHYCR_CR_READ, false);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int imx_sata_phy_reset(struct ahci_host_priv *hpriv)
+{
+	void __iomem *mmio = hpriv->mmio;
+	int timeout = 10;
+	u16 val;
+	int ret;
+
+	/* Reset SATA PHY by setting RESET bit of PHY register CLOCK_RESET */
+	ret = imx_phy_reg_addressing(IMX_CLOCK_RESET, mmio);
+	if (ret)
+		return ret;
+	ret = imx_phy_reg_write(IMX_CLOCK_RESET_RESET, mmio);
+	if (ret)
+		return ret;
+
+	/* Wait for PHY RX_PLL to be stable */
+	do {
+		usleep_range(100, 200);
+		ret = imx_phy_reg_addressing(IMX_LANE0_OUT_STAT, mmio);
+		if (ret)
+			return ret;
+		ret = imx_phy_reg_read(&val, mmio);
+		if (ret)
+			return ret;
+		if (val & IMX_LANE0_OUT_STAT_RX_PLL_STATE)
+			break;
+	} while (--timeout);
+
+	return timeout ? 0 : -ETIMEDOUT;
+}
+
 static int imx_sata_enable(struct ahci_host_priv *hpriv)
 {
 	struct imx_ahci_priv *imxpriv = hpriv->plat_data;
+	struct device *dev = &imxpriv->ahci_pdev->dev;
 	int ret;
 
 	if (imxpriv->no_device)
@@ -101,6 +257,14 @@ static int imx_sata_enable(struct ahci_host_priv *hpriv)
 		regmap_update_bits(imxpriv->gpr, IOMUXC_GPR13,
 				   IMX6Q_GPR13_SATA_MPLL_CLK_EN,
 				   IMX6Q_GPR13_SATA_MPLL_CLK_EN);
+
+		usleep_range(100, 200);
+
+		ret = imx_sata_phy_reset(hpriv);
+		if (ret) {
+			dev_err(dev, "failed to reset phy: %d\n", ret);
+			goto disable_regulator;
+		}
 	}
 
 	usleep_range(1000, 2000);
@@ -156,8 +320,8 @@ static void ahci_imx_error_handler(struct ata_port *ap)
 	 * without full reset once the pddq mode is enabled making it
 	 * impossible to use as part of libata LPM.
 	 */
-	reg_val = readl(mmio + PORT_PHY_CTL);
-	writel(reg_val | PORT_PHY_CTL_PDDQ_LOC, mmio + PORT_PHY_CTL);
+	reg_val = readl(mmio + IMX_P0PHYCR);
+	writel(reg_val | IMX_P0PHYCR_TEST_PDDQ, mmio + IMX_P0PHYCR);
 	imx_sata_disable(hpriv);
 	imxpriv->no_device = true;
 }
@@ -217,6 +381,7 @@ static int imx_ahci_probe(struct platform_device *pdev)
 	if (!imxpriv)
 		return -ENOMEM;
 
+	imxpriv->ahci_pdev = pdev;
 	imxpriv->no_device = false;
 	imxpriv->first_time = true;
 	imxpriv->type = (enum ahci_imx_type)of_id->data;
@@ -248,7 +413,7 @@ static int imx_ahci_probe(struct platform_device *pdev)
 
 	/*
 	 * Configure the HWINIT bits of the HOST_CAP and HOST_PORTS_IMPL,
-	 * and IP vendor specific register HOST_TIMER1MS.
+	 * and IP vendor specific register IMX_TIMER1MS.
 	 * Configure CAP_SSS (support stagered spin up).
 	 * Implement the port0.
 	 * Get the ahb clock rate, and configure the TIMER1MS register.
@@ -265,9 +430,10 @@ static int imx_ahci_probe(struct platform_device *pdev)
 	}
 
 	reg_val = clk_get_rate(imxpriv->ahb_clk) / 1000;
-	writel(reg_val, hpriv->mmio + HOST_TIMER1MS);
+	writel(reg_val, hpriv->mmio + IMX_TIMER1MS);
 
-	ret = ahci_platform_init_host(pdev, hpriv, &ahci_imx_port_info, 0, 0);
+	ret = ahci_platform_init_host(pdev, hpriv, &ahci_imx_port_info,
+				      0, 0, 0);
 	if (ret)
 		imx_sata_disable(hpriv);
 
diff --git a/drivers/ata/ahci_mvebu.c b/drivers/ata/ahci_mvebu.c
new file mode 100644
index 00000000000..fd3dfd733b8
--- /dev/null
+++ b/drivers/ata/ahci_mvebu.c
@@ -0,0 +1,128 @@
+/*
+ * AHCI glue platform driver for Marvell EBU SOCs
+ *
+ * Copyright (C) 2014 Marvell
+ *
+ * Thomas Petazzoni <thomas.petazzoni@free-electrons.com>
+ * Marcin Wojtas <mw@semihalf.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/ahci_platform.h>
+#include <linux/kernel.h>
+#include <linux/mbus.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include "ahci.h"
+
+#define AHCI_VENDOR_SPECIFIC_0_ADDR  0xa0
+#define AHCI_VENDOR_SPECIFIC_0_DATA  0xa4
+
+#define AHCI_WINDOW_CTRL(win)	(0x60 + ((win) << 4))
+#define AHCI_WINDOW_BASE(win)	(0x64 + ((win) << 4))
+#define AHCI_WINDOW_SIZE(win)	(0x68 + ((win) << 4))
+
+static void ahci_mvebu_mbus_config(struct ahci_host_priv *hpriv,
+				   const struct mbus_dram_target_info *dram)
+{
+	int i;
+
+	for (i = 0; i < 4; i++) {
+		writel(0, hpriv->mmio + AHCI_WINDOW_CTRL(i));
+		writel(0, hpriv->mmio + AHCI_WINDOW_BASE(i));
+		writel(0, hpriv->mmio + AHCI_WINDOW_SIZE(i));
+	}
+
+	for (i = 0; i < dram->num_cs; i++) {
+		const struct mbus_dram_window *cs = dram->cs + i;
+
+		writel((cs->mbus_attr << 8) |
+		       (dram->mbus_dram_target_id << 4) | 1,
+		       hpriv->mmio + AHCI_WINDOW_CTRL(i));
+		writel(cs->base, hpriv->mmio + AHCI_WINDOW_BASE(i));
+		writel(((cs->size - 1) & 0xffff0000),
+		       hpriv->mmio + AHCI_WINDOW_SIZE(i));
+	}
+}
+
+static void ahci_mvebu_regret_option(struct ahci_host_priv *hpriv)
+{
+	/*
+	 * Enable the regret bit to allow the SATA unit to regret a
+	 * request that didn't receive an acknowlegde and avoid a
+	 * deadlock
+	 */
+	writel(0x4, hpriv->mmio + AHCI_VENDOR_SPECIFIC_0_ADDR);
+	writel(0x80, hpriv->mmio + AHCI_VENDOR_SPECIFIC_0_DATA);
+}
+
+static const struct ata_port_info ahci_mvebu_port_info = {
+	.flags	   = AHCI_FLAG_COMMON,
+	.pio_mask  = ATA_PIO4,
+	.udma_mask = ATA_UDMA6,
+	.port_ops  = &ahci_platform_ops,
+};
+
+static int ahci_mvebu_probe(struct platform_device *pdev)
+{
+	struct ahci_host_priv *hpriv;
+	const struct mbus_dram_target_info *dram;
+	int rc;
+
+	hpriv = ahci_platform_get_resources(pdev);
+	if (IS_ERR(hpriv))
+		return PTR_ERR(hpriv);
+
+	rc = ahci_platform_enable_resources(hpriv);
+	if (rc)
+		return rc;
+
+	dram = mv_mbus_dram_info();
+	if (!dram)
+		return -ENODEV;
+
+	ahci_mvebu_mbus_config(hpriv, dram);
+	ahci_mvebu_regret_option(hpriv);
+
+	rc = ahci_platform_init_host(pdev, hpriv, &ahci_mvebu_port_info,
+				     0, 0, 0);
+	if (rc)
+		goto disable_resources;
+
+	return 0;
+
+disable_resources:
+	ahci_platform_disable_resources(hpriv);
+	return rc;
+}
+
+static const struct of_device_id ahci_mvebu_of_match[] = {
+	{ .compatible = "marvell,armada-380-ahci", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, ahci_mvebu_of_match);
+
+/*
+ * We currently don't provide power management related operations,
+ * since there is no suspend/resume support at the platform level for
+ * Armada 38x for the moment.
+ */
+static struct platform_driver ahci_mvebu_driver = {
+	.probe = ahci_mvebu_probe,
+	.remove = ata_platform_remove_one,
+	.driver = {
+		.name = "ahci-mvebu",
+		.owner = THIS_MODULE,
+		.of_match_table = ahci_mvebu_of_match,
+	},
+};
+module_platform_driver(ahci_mvebu_driver);
+
+MODULE_DESCRIPTION("Marvell EBU AHCI SATA driver");
+MODULE_AUTHOR("Thomas Petazzoni <thomas.petazzoni@free-electrons.com>, Marcin Wojtas <mw@semihalf.com>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:ahci_mvebu");
diff --git a/drivers/ata/ahci_platform.c b/drivers/ata/ahci_platform.c
index ef67e79944f..ebe505c1776 100644
--- a/drivers/ata/ahci_platform.c
+++ b/drivers/ata/ahci_platform.c
@@ -16,6 +16,7 @@
 #include <linux/module.h>
 #include <linux/pm.h>
 #include <linux/device.h>
+#include <linux/of_device.h>
 #include <linux/platform_device.h>
 #include <linux/libata.h>
 #include <linux/ahci_platform.h>
@@ -33,6 +34,7 @@ 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;
+	unsigned long hflags = 0;
 	int rc;
 
 	hpriv = ahci_platform_get_resources(pdev);
@@ -55,7 +57,11 @@ static int ahci_probe(struct platform_device *pdev)
 			goto disable_resources;
 	}
 
-	rc = ahci_platform_init_host(pdev, hpriv, &ahci_port_info, 0, 0);
+	if (of_device_is_compatible(dev->of_node, "hisilicon,hisi-ahci"))
+		hflags |= AHCI_HFLAG_NO_FBS;
+
+	rc = ahci_platform_init_host(pdev, hpriv, &ahci_port_info,
+				     hflags, 0, 0);
 	if (rc)
 		goto pdata_exit;
 
@@ -76,6 +82,7 @@ static const struct of_device_id ahci_of_match[] = {
 	{ .compatible = "snps,exynos5440-ahci", },
 	{ .compatible = "ibm,476gtr-ahci", },
 	{ .compatible = "snps,dwc-ahci", },
+	{ .compatible = "hisilicon,hisi-ahci", },
 	{},
 };
 MODULE_DEVICE_TABLE(of, ahci_of_match);
diff --git a/drivers/ata/ahci_st.c b/drivers/ata/ahci_st.c
index 633222226c1..2595598df9c 100644
--- a/drivers/ata/ahci_st.c
+++ b/drivers/ata/ahci_st.c
@@ -166,7 +166,7 @@ static int st_ahci_probe(struct platform_device *pdev)
 	if (err)
 		return err;
 
-	err = ahci_platform_init_host(pdev, hpriv, &st_ahci_port_info, 0, 0);
+	err = ahci_platform_init_host(pdev, hpriv, &st_ahci_port_info, 0, 0, 0);
 	if (err) {
 		ahci_platform_disable_resources(hpriv);
 		return err;
diff --git a/drivers/ata/ahci_sunxi.c b/drivers/ata/ahci_sunxi.c
index 42d3f64e74b..02002f125bd 100644
--- a/drivers/ata/ahci_sunxi.c
+++ b/drivers/ata/ahci_sunxi.c
@@ -157,8 +157,6 @@ static void ahci_sunxi_start_engine(struct ata_port *ap)
 }
 
 static const struct ata_port_info ahci_sunxi_port_info = {
-	AHCI_HFLAGS(AHCI_HFLAG_32BIT_ONLY | AHCI_HFLAG_NO_MSI |
-			  AHCI_HFLAG_NO_PMP | AHCI_HFLAG_YES_NCQ),
 	.flags		= AHCI_FLAG_COMMON | ATA_FLAG_NCQ,
 	.pio_mask	= ATA_PIO4,
 	.udma_mask	= ATA_UDMA6,
@@ -169,6 +167,7 @@ static int ahci_sunxi_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
 	struct ahci_host_priv *hpriv;
+	unsigned long hflags;
 	int rc;
 
 	hpriv = ahci_platform_get_resources(pdev);
@@ -185,7 +184,11 @@ static int ahci_sunxi_probe(struct platform_device *pdev)
 	if (rc)
 		goto disable_resources;
 
-	rc = ahci_platform_init_host(pdev, hpriv, &ahci_sunxi_port_info, 0, 0);
+	hflags = AHCI_HFLAG_32BIT_ONLY | AHCI_HFLAG_NO_MSI |
+		 AHCI_HFLAG_NO_PMP | AHCI_HFLAG_YES_NCQ;
+
+	rc = ahci_platform_init_host(pdev, hpriv, &ahci_sunxi_port_info,
+				     hflags, 0, 0);
 	if (rc)
 		goto disable_resources;
 
diff --git a/drivers/ata/ahci_xgene.c b/drivers/ata/ahci_xgene.c
index 77c89bf171f..042a9bb45c8 100644
--- a/drivers/ata/ahci_xgene.c
+++ b/drivers/ata/ahci_xgene.c
@@ -303,7 +303,6 @@ static struct ata_port_operations xgene_ahci_ops = {
 };
 
 static const struct ata_port_info xgene_ahci_port_info = {
-	AHCI_HFLAGS(AHCI_HFLAG_NO_PMP | AHCI_HFLAG_YES_NCQ),
 	.flags = AHCI_FLAG_COMMON | ATA_FLAG_NCQ,
 	.pio_mask = ATA_PIO4,
 	.udma_mask = ATA_UDMA6,
@@ -382,6 +381,7 @@ static int xgene_ahci_probe(struct platform_device *pdev)
 	struct ahci_host_priv *hpriv;
 	struct xgene_ahci_context *ctx;
 	struct resource *res;
+	unsigned long hflags;
 	int rc;
 
 	hpriv = ahci_platform_get_resources(pdev);
@@ -450,7 +450,10 @@ static int xgene_ahci_probe(struct platform_device *pdev)
 		goto disable_resources;
 	}
 
-	rc = ahci_platform_init_host(pdev, hpriv, &xgene_ahci_port_info, 0, 0);
+	hflags = AHCI_HFLAG_NO_PMP | AHCI_HFLAG_YES_NCQ;
+
+	rc = ahci_platform_init_host(pdev, hpriv, &xgene_ahci_port_info,
+				     hflags, 0, 0);
 	if (rc)
 		goto disable_resources;
 
diff --git a/drivers/ata/ata_generic.c b/drivers/ata/ata_generic.c
index 9498a7d3846..9ff545ce8da 100644
--- a/drivers/ata/ata_generic.c
+++ b/drivers/ata/ata_generic.c
@@ -241,7 +241,7 @@ static struct pci_driver ata_generic_pci_driver = {
 	.id_table	= ata_generic,
 	.probe 		= ata_generic_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/ata_piix.c b/drivers/ata/ata_piix.c
index 6334c8d7c3f..893e30e9a9e 100644
--- a/drivers/ata/ata_piix.c
+++ b/drivers/ata/ata_piix.c
@@ -830,7 +830,7 @@ static bool piix_irq_check(struct ata_port *ap)
 	return ap->ops->bmdma_status(ap) & ATA_DMA_INTR;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int piix_broken_suspend(void)
 {
 	static const struct dmi_system_id sysids[] = {
@@ -1767,7 +1767,7 @@ static struct pci_driver piix_pci_driver = {
 	.id_table		= piix_pci_tbl,
 	.probe			= piix_init_one,
 	.remove			= piix_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend		= piix_pci_device_suspend,
 	.resume			= piix_pci_device_resume,
 #endif
diff --git a/drivers/ata/libahci.c b/drivers/ata/libahci.c
index 6bd4f660b4e..40ea583d361 100644
--- a/drivers/ata/libahci.c
+++ b/drivers/ata/libahci.c
@@ -452,11 +452,23 @@ void ahci_save_initial_config(struct device *dev,
 		cap &= ~HOST_CAP_SNTF;
 	}
 
+	if ((cap2 & HOST_CAP2_SDS) && (hpriv->flags & AHCI_HFLAG_NO_DEVSLP)) {
+		dev_info(dev,
+			 "controller can't do DEVSLP, turning off\n");
+		cap2 &= ~HOST_CAP2_SDS;
+		cap2 &= ~HOST_CAP2_SADM;
+	}
+
 	if (!(cap & HOST_CAP_FBS) && (hpriv->flags & AHCI_HFLAG_YES_FBS)) {
 		dev_info(dev, "controller can do FBS, turning on CAP_FBS\n");
 		cap |= HOST_CAP_FBS;
 	}
 
+	if ((cap & HOST_CAP_FBS) && (hpriv->flags & AHCI_HFLAG_NO_FBS)) {
+		dev_info(dev, "controller can't do FBS, turning off CAP_FBS\n");
+		cap &= ~HOST_CAP_FBS;
+	}
+
 	if (force_port_map && port_map != force_port_map) {
 		dev_info(dev, "forcing port_map 0x%x -> 0x%x\n",
 			 port_map, force_port_map);
diff --git a/drivers/ata/libahci_platform.c b/drivers/ata/libahci_platform.c
index 7cb3a85719c..3a5b4ed25a4 100644
--- a/drivers/ata/libahci_platform.c
+++ b/drivers/ata/libahci_platform.c
@@ -283,6 +283,7 @@ EXPORT_SYMBOL_GPL(ahci_platform_get_resources);
  * @pdev: platform device pointer for the host
  * @hpriv: ahci-host private data for the host
  * @pi_template: template for the ata_port_info to use
+ * @host_flags: ahci host flags used in ahci_host_priv
  * @force_port_map: param passed to ahci_save_initial_config
  * @mask_port_map: param passed to ahci_save_initial_config
  *
@@ -296,6 +297,7 @@ EXPORT_SYMBOL_GPL(ahci_platform_get_resources);
 int ahci_platform_init_host(struct platform_device *pdev,
 			    struct ahci_host_priv *hpriv,
 			    const struct ata_port_info *pi_template,
+			    unsigned long host_flags,
 			    unsigned int force_port_map,
 			    unsigned int mask_port_map)
 {
@@ -312,7 +314,8 @@ int ahci_platform_init_host(struct platform_device *pdev,
 	}
 
 	/* prepare host */
-	hpriv->flags |= (unsigned long)pi.private_data;
+	pi.private_data = (void *)host_flags;
+	hpriv->flags |= host_flags;
 
 	ahci_save_initial_config(dev, hpriv, force_port_map, mask_port_map);
 
diff --git a/drivers/ata/libata-core.c b/drivers/ata/libata-core.c
index 943cc8b83e5..18d97d5c7d9 100644
--- a/drivers/ata/libata-core.c
+++ b/drivers/ata/libata-core.c
@@ -4224,10 +4224,10 @@ static const struct ata_blacklist_entry ata_device_blacklist [] = {
 	{ "PIONEER DVD-RW  DVR-216D",	NULL,	ATA_HORKAGE_NOSETXFER },
 
 	/* devices that don't properly handle queued TRIM commands */
-	{ "Micron_M500*",		"MU0[1-4]*",	ATA_HORKAGE_NO_NCQ_TRIM, },
-	{ "Crucial_CT???M500SSD*",	"MU0[1-4]*",	ATA_HORKAGE_NO_NCQ_TRIM, },
-	{ "Micron_M550*",		NULL,		ATA_HORKAGE_NO_NCQ_TRIM, },
-	{ "Crucial_CT???M550SSD*",	NULL,		ATA_HORKAGE_NO_NCQ_TRIM, },
+	{ "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, },
 
 	/*
 	 * Some WD SATA-I drives spin up and down erratically when the link
@@ -6314,6 +6314,8 @@ int ata_host_activate(struct ata_host *host, int irq,
 static void ata_port_detach(struct ata_port *ap)
 {
 	unsigned long flags;
+	struct ata_link *link;
+	struct ata_device *dev;
 
 	if (!ap->ops->error_handler)
 		goto skip_eh;
@@ -6333,6 +6335,13 @@ static void ata_port_detach(struct ata_port *ap)
 	cancel_delayed_work_sync(&ap->hotplug_task);
 
  skip_eh:
+	/* clean up zpodd on port removal */
+	ata_for_each_link(link, ap, HOST_FIRST) {
+		ata_for_each_dev(dev, link, ALL) {
+			if (zpodd_dev_enabled(dev))
+				zpodd_exit(dev);
+		}
+	}
 	if (ap->pmp_link) {
 		int i;
 		for (i = 0; i < SATA_PMP_MAX_PORTS; i++)
diff --git a/drivers/ata/libata-scsi.c b/drivers/ata/libata-scsi.c
index ef8567de6a7..72691fd9394 100644
--- a/drivers/ata/libata-scsi.c
+++ b/drivers/ata/libata-scsi.c
@@ -1993,7 +1993,11 @@ static unsigned int ata_scsiop_inq_std(struct ata_scsi_args *args, u8 *rbuf)
 	memcpy(rbuf, hdr, sizeof(hdr));
 	memcpy(&rbuf[8], "ATA     ", 8);
 	ata_id_string(args->id, &rbuf[16], ATA_ID_PROD, 16);
-	ata_id_string(args->id, &rbuf[32], ATA_ID_FW_REV, 4);
+
+	/* From SAT, use last 2 words from fw rev unless they are spaces */
+	ata_id_string(args->id, &rbuf[32], ATA_ID_FW_REV + 2, 4);
+	if (strncmp(&rbuf[32], "    ", 4) == 0)
+		ata_id_string(args->id, &rbuf[32], ATA_ID_FW_REV, 4);
 
 	if (rbuf[32] == 0 || rbuf[32] == ' ')
 		memcpy(&rbuf[32], "n/a ", 4);
diff --git a/drivers/ata/libata-sff.c b/drivers/ata/libata-sff.c
index b603720b877..1121153f1ec 100644
--- a/drivers/ata/libata-sff.c
+++ b/drivers/ata/libata-sff.c
@@ -2433,15 +2433,6 @@ int ata_pci_sff_activate_host(struct ata_host *host,
 		mask = (1 << 2) | (1 << 0);
 		if ((tmp8 & mask) != mask)
 			legacy_mode = 1;
-#if defined(CONFIG_NO_ATA_LEGACY)
-		/* Some platforms with PCI limits cannot address compat
-		   port space. In that case we punt if their firmware has
-		   left a device in compatibility mode */
-		if (legacy_mode) {
-			printk(KERN_ERR "ata: Compatibility mode ATA is not supported on this platform, skipping.\n");
-			return -EOPNOTSUPP;
-		}
-#endif
 	}
 
 	if (!devres_open_group(dev, NULL, GFP_KERNEL))
diff --git a/drivers/ata/pata_acpi.c b/drivers/ata/pata_acpi.c
index 5108b8744dc..b70fce2a38e 100644
--- a/drivers/ata/pata_acpi.c
+++ b/drivers/ata/pata_acpi.c
@@ -265,7 +265,7 @@ static struct pci_driver pacpi_pci_driver = {
 	.id_table		= pacpi_pci_tbl,
 	.probe			= pacpi_init_one,
 	.remove			= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend		= ata_pci_device_suspend,
 	.resume			= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/pata_ali.c b/drivers/ata/pata_ali.c
index 1b7b2ccabcf..d19cd88ed2d 100644
--- a/drivers/ata/pata_ali.c
+++ b/drivers/ata/pata_ali.c
@@ -589,7 +589,7 @@ static int ali_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
 		return ata_pci_bmdma_init_one(pdev, ppi, &ali_sht, NULL, 0);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int ali_reinit_one(struct pci_dev *pdev)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
@@ -616,7 +616,7 @@ static struct pci_driver ali_pci_driver = {
 	.id_table	= ali,
 	.probe 		= ali_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= ali_reinit_one,
 #endif
diff --git a/drivers/ata/pata_amd.c b/drivers/ata/pata_amd.c
index 1206fa6b62c..8d4d959a821 100644
--- a/drivers/ata/pata_amd.c
+++ b/drivers/ata/pata_amd.c
@@ -574,7 +574,7 @@ static int amd_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
 	return ata_pci_bmdma_init_one(pdev, ppi, &amd_sht, hpriv, 0);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int amd_reinit_one(struct pci_dev *pdev)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
@@ -625,7 +625,7 @@ static struct pci_driver amd_pci_driver = {
 	.id_table	= amd,
 	.probe 		= amd_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= amd_reinit_one,
 #endif
diff --git a/drivers/ata/pata_artop.c b/drivers/ata/pata_artop.c
index 3aa4e655e3c..96c05c9494f 100644
--- a/drivers/ata/pata_artop.c
+++ b/drivers/ata/pata_artop.c
@@ -422,7 +422,7 @@ static const struct pci_device_id artop_pci_tbl[] = {
 	{ }	/* terminate list */
 };
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int atp8xx_reinit_one(struct pci_dev *pdev)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
@@ -444,7 +444,7 @@ static struct pci_driver artop_pci_driver = {
 	.id_table		= artop_pci_tbl,
 	.probe			= artop_init_one,
 	.remove			= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend		= ata_pci_device_suspend,
 	.resume			= atp8xx_reinit_one,
 #endif
diff --git a/drivers/ata/pata_atiixp.c b/drivers/ata/pata_atiixp.c
index 30fa4ca4cef..970f7767e5f 100644
--- a/drivers/ata/pata_atiixp.c
+++ b/drivers/ata/pata_atiixp.c
@@ -298,7 +298,7 @@ static struct pci_driver atiixp_pci_driver = {
 	.id_table	= atiixp,
 	.probe 		= atiixp_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.resume		= ata_pci_device_resume,
 	.suspend	= ata_pci_device_suspend,
 #endif
diff --git a/drivers/ata/pata_atp867x.c b/drivers/ata/pata_atp867x.c
index 7e73a0f1e32..a705cfca90f 100644
--- a/drivers/ata/pata_atp867x.c
+++ b/drivers/ata/pata_atp867x.c
@@ -530,7 +530,7 @@ err_out:
 	return rc;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int atp867x_reinit_one(struct pci_dev *pdev)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
@@ -558,7 +558,7 @@ static struct pci_driver atp867x_driver = {
 	.id_table 	= atp867x_pci_tbl,
 	.probe 		= atp867x_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= atp867x_reinit_one,
 #endif
diff --git a/drivers/ata/pata_bf54x.c b/drivers/ata/pata_bf54x.c
index ba0d8a29dc2..03f2f2bc83b 100644
--- a/drivers/ata/pata_bf54x.c
+++ b/drivers/ata/pata_bf54x.c
@@ -1619,7 +1619,7 @@ static int bfin_atapi_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int bfin_atapi_suspend(struct platform_device *pdev, pm_message_t state)
 {
 	struct ata_host *host = platform_get_drvdata(pdev);
diff --git a/drivers/ata/pata_cmd640.c b/drivers/ata/pata_cmd640.c
index 57f1be64dbf..c47caa807fa 100644
--- a/drivers/ata/pata_cmd640.c
+++ b/drivers/ata/pata_cmd640.c
@@ -231,7 +231,7 @@ static int cmd640_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
 	return ata_pci_sff_init_one(pdev, ppi, &cmd640_sht, NULL, 0);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int cmd640_reinit_one(struct pci_dev *pdev)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
@@ -256,7 +256,7 @@ static struct pci_driver cmd640_pci_driver = {
 	.id_table	= cmd640,
 	.probe 		= cmd640_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= cmd640_reinit_one,
 #endif
diff --git a/drivers/ata/pata_cmd64x.c b/drivers/ata/pata_cmd64x.c
index 6bca3505b9e..13ca5883285 100644
--- a/drivers/ata/pata_cmd64x.c
+++ b/drivers/ata/pata_cmd64x.c
@@ -487,7 +487,7 @@ static int cmd64x_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
 	return ata_pci_bmdma_init_one(pdev, ppi, &cmd64x_sht, NULL, 0);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int cmd64x_reinit_one(struct pci_dev *pdev)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
@@ -518,7 +518,7 @@ static struct pci_driver cmd64x_pci_driver = {
 	.id_table	= cmd64x,
 	.probe 		= cmd64x_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= cmd64x_reinit_one,
 #endif
diff --git a/drivers/ata/pata_cs5520.c b/drivers/ata/pata_cs5520.c
index bcde4b78680..d65cb9d2fa8 100644
--- a/drivers/ata/pata_cs5520.c
+++ b/drivers/ata/pata_cs5520.c
@@ -229,7 +229,7 @@ static int cs5520_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
 	return ata_host_register(host, &cs5520_sht);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 /**
  *	cs5520_reinit_one	-	device resume
  *	@pdev: PCI device
@@ -278,7 +278,7 @@ static int cs5520_pci_device_suspend(struct pci_dev *pdev, pm_message_t mesg)
 	pci_save_state(pdev);
 	return 0;
 }
-#endif /* CONFIG_PM */
+#endif /* CONFIG_PM_SLEEP */
 
 /* For now keep DMA off. We can set it for all but A rev CS5510 once the
    core ATA code can handle it */
@@ -295,7 +295,7 @@ static struct pci_driver cs5520_pci_driver = {
 	.id_table	= pata_cs5520,
 	.probe 		= cs5520_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= cs5520_pci_device_suspend,
 	.resume		= cs5520_reinit_one,
 #endif
diff --git a/drivers/ata/pata_cs5530.c b/drivers/ata/pata_cs5530.c
index 8afe854a5a5..48ae4b43447 100644
--- a/drivers/ata/pata_cs5530.c
+++ b/drivers/ata/pata_cs5530.c
@@ -326,7 +326,7 @@ static int cs5530_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
 	return ata_pci_bmdma_init_one(pdev, ppi, &cs5530_sht, NULL, 0);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int cs5530_reinit_one(struct pci_dev *pdev)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
@@ -343,7 +343,7 @@ static int cs5530_reinit_one(struct pci_dev *pdev)
 	ata_host_resume(host);
 	return 0;
 }
-#endif /* CONFIG_PM */
+#endif /* CONFIG_PM_SLEEP */
 
 static const struct pci_device_id cs5530[] = {
 	{ PCI_VDEVICE(CYRIX, PCI_DEVICE_ID_CYRIX_5530_IDE), },
@@ -356,7 +356,7 @@ static struct pci_driver cs5530_pci_driver = {
 	.id_table	= cs5530,
 	.probe 		= cs5530_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= cs5530_reinit_one,
 #endif
diff --git a/drivers/ata/pata_cs5535.c b/drivers/ata/pata_cs5535.c
index 2c0986fa4bb..97584e8456d 100644
--- a/drivers/ata/pata_cs5535.c
+++ b/drivers/ata/pata_cs5535.c
@@ -200,7 +200,7 @@ static struct pci_driver cs5535_pci_driver = {
 	.id_table	= cs5535,
 	.probe 		= cs5535_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/pata_cs5536.c b/drivers/ata/pata_cs5536.c
index 32ddcae5a36..6c15a554efb 100644
--- a/drivers/ata/pata_cs5536.c
+++ b/drivers/ata/pata_cs5536.c
@@ -297,7 +297,7 @@ static struct pci_driver cs5536_pci_driver = {
 	.id_table	= cs5536,
 	.probe		= cs5536_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/pata_cypress.c b/drivers/ata/pata_cypress.c
index 3435bd6a5cc..793018460d8 100644
--- a/drivers/ata/pata_cypress.c
+++ b/drivers/ata/pata_cypress.c
@@ -151,7 +151,7 @@ static struct pci_driver cy82c693_pci_driver = {
 	.id_table	= cy82c693,
 	.probe 		= cy82c693_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/pata_efar.c b/drivers/ata/pata_efar.c
index f440892225f..4a57a6f032d 100644
--- a/drivers/ata/pata_efar.c
+++ b/drivers/ata/pata_efar.c
@@ -288,7 +288,7 @@ static struct pci_driver efar_pci_driver = {
 	.id_table		= efar_pci_tbl,
 	.probe			= efar_init_one,
 	.remove			= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend		= ata_pci_device_suspend,
 	.resume			= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/pata_ep93xx.c b/drivers/ata/pata_ep93xx.c
index cad9d45749c..6ad5c072ce3 100644
--- a/drivers/ata/pata_ep93xx.c
+++ b/drivers/ata/pata_ep93xx.c
@@ -708,8 +708,8 @@ static void ep93xx_pata_dma_start(struct ata_queued_cmd *qc)
 	struct dma_chan *channel = qc->dma_dir == DMA_TO_DEVICE
 		? drv_data->dma_tx_channel : drv_data->dma_rx_channel;
 
-	txd = channel->device->device_prep_slave_sg(channel, qc->sg,
-		 qc->n_elem, qc->dma_dir, DMA_CTRL_ACK, NULL);
+	txd = dmaengine_prep_slave_sg(channel, qc->sg, qc->n_elem, qc->dma_dir,
+		DMA_CTRL_ACK);
 	if (!txd) {
 		dev_err(qc->ap->dev, "failed to prepare slave for sg dma\n");
 		return;
diff --git a/drivers/ata/pata_hpt366.c b/drivers/ata/pata_hpt366.c
index 8e76f79689d..cbc3de793d1 100644
--- a/drivers/ata/pata_hpt366.c
+++ b/drivers/ata/pata_hpt366.c
@@ -386,7 +386,7 @@ static int hpt36x_init_one(struct pci_dev *dev, const struct pci_device_id *id)
 	return ata_pci_bmdma_init_one(dev, ppi, &hpt36x_sht, hpriv, 0);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int hpt36x_reinit_one(struct pci_dev *dev)
 {
 	struct ata_host *host = pci_get_drvdata(dev);
@@ -411,7 +411,7 @@ static struct pci_driver hpt36x_pci_driver = {
 	.id_table	= hpt36x,
 	.probe		= hpt36x_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= hpt36x_reinit_one,
 #endif
diff --git a/drivers/ata/pata_hpt3x3.c b/drivers/ata/pata_hpt3x3.c
index 255c5aaff3a..d019cdd5bc9 100644
--- a/drivers/ata/pata_hpt3x3.c
+++ b/drivers/ata/pata_hpt3x3.c
@@ -249,7 +249,7 @@ static int hpt3x3_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
 				 IRQF_SHARED, &hpt3x3_sht);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int hpt3x3_reinit_one(struct pci_dev *dev)
 {
 	struct ata_host *host = pci_get_drvdata(dev);
@@ -277,7 +277,7 @@ static struct pci_driver hpt3x3_pci_driver = {
 	.id_table	= hpt3x3,
 	.probe 		= hpt3x3_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= hpt3x3_reinit_one,
 #endif
diff --git a/drivers/ata/pata_imx.c b/drivers/ata/pata_imx.c
index e0872db913d..af424573c2f 100644
--- a/drivers/ata/pata_imx.c
+++ b/drivers/ata/pata_imx.c
@@ -185,7 +185,7 @@ static int pata_imx_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int pata_imx_suspend(struct device *dev)
 {
 	struct ata_host *host = dev_get_drvdata(dev);
@@ -244,7 +244,7 @@ static struct platform_driver pata_imx_driver = {
 		.name		= DRV_NAME,
 		.of_match_table	= imx_pata_dt_ids,
 		.owner		= THIS_MODULE,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 		.pm		= &pata_imx_pm_ops,
 #endif
 	},
diff --git a/drivers/ata/pata_it8213.c b/drivers/ata/pata_it8213.c
index 81369d187a5..4f97d1e52f8 100644
--- a/drivers/ata/pata_it8213.c
+++ b/drivers/ata/pata_it8213.c
@@ -283,7 +283,7 @@ static struct pci_driver it8213_pci_driver = {
 	.id_table		= it8213_pci_tbl,
 	.probe			= it8213_init_one,
 	.remove			= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend		= ata_pci_device_suspend,
 	.resume			= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/pata_it821x.c b/drivers/ata/pata_it821x.c
index dc3d7877f29..a5088ecb349 100644
--- a/drivers/ata/pata_it821x.c
+++ b/drivers/ata/pata_it821x.c
@@ -935,7 +935,7 @@ static int it821x_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
 	return ata_pci_bmdma_init_one(pdev, ppi, &it821x_sht, NULL, 0);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int it821x_reinit_one(struct pci_dev *pdev)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
@@ -965,7 +965,7 @@ static struct pci_driver it821x_pci_driver = {
 	.id_table	= it821x,
 	.probe 		= it821x_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= it821x_reinit_one,
 #endif
diff --git a/drivers/ata/pata_jmicron.c b/drivers/ata/pata_jmicron.c
index b1cfa0258fd..4d1a5d2c428 100644
--- a/drivers/ata/pata_jmicron.c
+++ b/drivers/ata/pata_jmicron.c
@@ -157,7 +157,7 @@ static struct pci_driver jmicron_pci_driver = {
 	.id_table		= jmicron_pci_tbl,
 	.probe			= jmicron_init_one,
 	.remove			= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend		= ata_pci_device_suspend,
 	.resume			= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/pata_macio.c b/drivers/ata/pata_macio.c
index c28d0645e85..a02f76fdcfc 100644
--- a/drivers/ata/pata_macio.c
+++ b/drivers/ata/pata_macio.c
@@ -845,8 +845,7 @@ static int pata_macio_slave_config(struct scsi_device *sdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-
+#ifdef CONFIG_PM_SLEEP
 static int pata_macio_do_suspend(struct pata_macio_priv *priv, pm_message_t mesg)
 {
 	int rc;
@@ -907,8 +906,7 @@ static int pata_macio_do_resume(struct pata_macio_priv *priv)
 
 	return 0;
 }
-
-#endif /* CONFIG_PM */
+#endif /* CONFIG_PM_SLEEP */
 
 static struct scsi_host_template pata_macio_sht = {
 	ATA_BASE_SHT(DRV_NAME),
@@ -1208,8 +1206,7 @@ static int pata_macio_detach(struct macio_dev *mdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-
+#ifdef CONFIG_PM_SLEEP
 static int pata_macio_suspend(struct macio_dev *mdev, pm_message_t mesg)
 {
 	struct ata_host *host = macio_get_drvdata(mdev);
@@ -1223,8 +1220,7 @@ static int pata_macio_resume(struct macio_dev *mdev)
 
 	return pata_macio_do_resume(host->private_data);
 }
-
-#endif /* CONFIG_PM */
+#endif /* CONFIG_PM_SLEEP */
 
 #ifdef CONFIG_PMAC_MEDIABAY
 static void pata_macio_mb_event(struct macio_dev* mdev, int mb_state)
@@ -1316,8 +1312,7 @@ static void pata_macio_pci_detach(struct pci_dev *pdev)
 	ata_host_detach(host);
 }
 
-#ifdef CONFIG_PM
-
+#ifdef CONFIG_PM_SLEEP
 static int pata_macio_pci_suspend(struct pci_dev *pdev, pm_message_t mesg)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
@@ -1331,8 +1326,7 @@ static int pata_macio_pci_resume(struct pci_dev *pdev)
 
 	return pata_macio_do_resume(host->private_data);
 }
-
-#endif /* CONFIG_PM */
+#endif /* CONFIG_PM_SLEEP */
 
 static struct of_device_id pata_macio_match[] =
 {
@@ -1360,7 +1354,7 @@ static struct macio_driver pata_macio_driver =
 	},
 	.probe		= pata_macio_attach,
 	.remove		= pata_macio_detach,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= pata_macio_suspend,
 	.resume		= pata_macio_resume,
 #endif
@@ -1383,7 +1377,7 @@ static struct pci_driver pata_macio_pci_driver = {
 	.id_table	= pata_macio_pci_match,
 	.probe		= pata_macio_pci_attach,
 	.remove		= pata_macio_pci_detach,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= pata_macio_pci_suspend,
 	.resume		= pata_macio_pci_resume,
 #endif
diff --git a/drivers/ata/pata_marvell.c b/drivers/ata/pata_marvell.c
index 6bad3df3a13..ae9feb1ba8f 100644
--- a/drivers/ata/pata_marvell.c
+++ b/drivers/ata/pata_marvell.c
@@ -171,7 +171,7 @@ static struct pci_driver marvell_pci_driver = {
 	.id_table		= marvell_pci_tbl,
 	.probe			= marvell_init_one,
 	.remove			= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend		= ata_pci_device_suspend,
 	.resume			= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/pata_mpc52xx.c b/drivers/ata/pata_mpc52xx.c
index 0024ced3e20..ccd1c83a05c 100644
--- a/drivers/ata/pata_mpc52xx.c
+++ b/drivers/ata/pata_mpc52xx.c
@@ -819,9 +819,7 @@ mpc52xx_ata_remove(struct platform_device *op)
 	return 0;
 }
 
-
-#ifdef CONFIG_PM
-
+#ifdef CONFIG_PM_SLEEP
 static int
 mpc52xx_ata_suspend(struct platform_device *op, pm_message_t state)
 {
@@ -847,10 +845,8 @@ mpc52xx_ata_resume(struct platform_device *op)
 
 	return 0;
 }
-
 #endif
 
-
 static struct of_device_id mpc52xx_ata_of_match[] = {
 	{ .compatible = "fsl,mpc5200-ata", },
 	{ .compatible = "mpc5200-ata", },
@@ -861,7 +857,7 @@ static struct of_device_id mpc52xx_ata_of_match[] = {
 static struct platform_driver mpc52xx_ata_of_platform_driver = {
 	.probe		= mpc52xx_ata_probe,
 	.remove		= mpc52xx_ata_remove,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= mpc52xx_ata_suspend,
 	.resume		= mpc52xx_ata_resume,
 #endif
diff --git a/drivers/ata/pata_mpiix.c b/drivers/ata/pata_mpiix.c
index f39a5379e81..202b4d60139 100644
--- a/drivers/ata/pata_mpiix.c
+++ b/drivers/ata/pata_mpiix.c
@@ -223,7 +223,7 @@ static struct pci_driver mpiix_pci_driver = {
 	.id_table	= mpiix,
 	.probe 		= mpiix_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/pata_netcell.c b/drivers/ata/pata_netcell.c
index e3b97093ef9..0ea18331d46 100644
--- a/drivers/ata/pata_netcell.c
+++ b/drivers/ata/pata_netcell.c
@@ -92,7 +92,7 @@ static struct pci_driver netcell_pci_driver = {
 	.id_table		= netcell_pci_tbl,
 	.probe			= netcell_init_one,
 	.remove			= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend		= ata_pci_device_suspend,
 	.resume			= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/pata_ninja32.c b/drivers/ata/pata_ninja32.c
index 56201a69af1..efb272da856 100644
--- a/drivers/ata/pata_ninja32.c
+++ b/drivers/ata/pata_ninja32.c
@@ -152,8 +152,7 @@ static int ninja32_init_one(struct pci_dev *dev, const struct pci_device_id *id)
 				 IRQF_SHARED, &ninja32_sht);
 }
 
-#ifdef CONFIG_PM
-
+#ifdef CONFIG_PM_SLEEP
 static int ninja32_reinit_one(struct pci_dev *pdev)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
@@ -183,7 +182,7 @@ static struct pci_driver ninja32_pci_driver = {
 	.id_table	= ninja32,
 	.probe 		= ninja32_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= ninja32_reinit_one,
 #endif
diff --git a/drivers/ata/pata_ns87410.c b/drivers/ata/pata_ns87410.c
index 6154c3ee11a..200e1eb23a2 100644
--- a/drivers/ata/pata_ns87410.c
+++ b/drivers/ata/pata_ns87410.c
@@ -161,7 +161,7 @@ static struct pci_driver ns87410_pci_driver = {
 	.id_table	= ns87410,
 	.probe 		= ns87410_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/pata_ns87415.c b/drivers/ata/pata_ns87415.c
index d44df7ccfe4..84c6b225b56 100644
--- a/drivers/ata/pata_ns87415.c
+++ b/drivers/ata/pata_ns87415.c
@@ -385,7 +385,7 @@ static const struct pci_device_id ns87415_pci_tbl[] = {
 	{ }	/* terminate list */
 };
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int ns87415_reinit_one(struct pci_dev *pdev)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
@@ -407,7 +407,7 @@ static struct pci_driver ns87415_pci_driver = {
 	.id_table		= ns87415_pci_tbl,
 	.probe			= ns87415_init_one,
 	.remove			= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend		= ata_pci_device_suspend,
 	.resume			= ns87415_reinit_one,
 #endif
diff --git a/drivers/ata/pata_octeon_cf.c b/drivers/ata/pata_octeon_cf.c
index 83c4ddb1bc7..2a97d3a531e 100644
--- a/drivers/ata/pata_octeon_cf.c
+++ b/drivers/ata/pata_octeon_cf.c
@@ -865,7 +865,7 @@ static int octeon_cf_probe(struct platform_device *pdev)
 	if (node == NULL)
 		return -EINVAL;
 
-	cf_port = kzalloc(sizeof(*cf_port), GFP_KERNEL);
+	cf_port = devm_kzalloc(&pdev->dev, sizeof(*cf_port), GFP_KERNEL);
 	if (!cf_port)
 		return -ENOMEM;
 
@@ -881,10 +881,9 @@ static int octeon_cf_probe(struct platform_device *pdev)
 	n_size = of_n_size_cells(node);
 
 	reg_prop = of_find_property(node, "reg", &reg_len);
-	if (!reg_prop || reg_len < sizeof(__be32)) {
-		rv = -EINVAL;
-		goto free_cf_port;
-	}
+	if (!reg_prop || reg_len < sizeof(__be32))
+		return -EINVAL;
+
 	cs_num = reg_prop->value;
 	cf_port->cs0 = be32_to_cpup(cs_num);
 
@@ -901,16 +900,13 @@ static int octeon_cf_probe(struct platform_device *pdev)
 				res_dma = platform_get_resource(dma_dev, IORESOURCE_MEM, 0);
 				if (!res_dma) {
 					of_node_put(dma_node);
-					rv = -EINVAL;
-					goto free_cf_port;
+					return -EINVAL;
 				}
 				cf_port->dma_base = (u64)devm_ioremap_nocache(&pdev->dev, res_dma->start,
 									 resource_size(res_dma));
-
 				if (!cf_port->dma_base) {
 					of_node_put(dma_node);
-					rv = -EINVAL;
-					goto free_cf_port;
+					return -EINVAL;
 				}
 
 				irq_handler = octeon_cf_interrupt;
@@ -921,41 +917,34 @@ static int octeon_cf_probe(struct platform_device *pdev)
 			of_node_put(dma_node);
 		}
 		res_cs1 = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-		if (!res_cs1) {
-			rv = -EINVAL;
-			goto free_cf_port;
-		}
+		if (!res_cs1)
+			return -EINVAL;
+
 		cs1 = devm_ioremap_nocache(&pdev->dev, res_cs1->start,
 					   resource_size(res_cs1));
-
 		if (!cs1)
-			goto free_cf_port;
+			return rv;
+
+		if (reg_len < (n_addr + n_size + 1) * sizeof(__be32))
+			return -EINVAL;
 
-		if (reg_len < (n_addr + n_size + 1) * sizeof(__be32)) {
-			rv = -EINVAL;
-			goto free_cf_port;
-		}
 		cs_num += n_addr + n_size;
 		cf_port->cs1 = be32_to_cpup(cs_num);
 	}
 
 	res_cs0 = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-
-	if (!res_cs0) {
-		rv = -EINVAL;
-		goto free_cf_port;
-	}
+	if (!res_cs0)
+		return -EINVAL;
 
 	cs0 = devm_ioremap_nocache(&pdev->dev, res_cs0->start,
 				   resource_size(res_cs0));
-
 	if (!cs0)
-		goto free_cf_port;
+		return rv;
 
 	/* allocate host */
 	host = ata_host_alloc(&pdev->dev, 1);
 	if (!host)
-		goto free_cf_port;
+		return rv;
 
 	ap = host->ports[0];
 	ap->private_data = cf_port;
@@ -1020,17 +1009,12 @@ static int octeon_cf_probe(struct platform_device *pdev)
 
 	ata_port_desc(ap, "cmd %p ctl %p", base, ap->ioaddr.ctl_addr);
 
-
 	dev_info(&pdev->dev, "version " DRV_VERSION" %d bit%s.\n",
 		 is_16bit ? 16 : 8,
 		 cf_port->is_true_ide ? ", True IDE" : "");
 
 	return ata_host_activate(host, irq, irq_handler,
 				 IRQF_SHARED, &octeon_cf_sht);
-
-free_cf_port:
-	kfree(cf_port);
-	return rv;
 }
 
 static void octeon_cf_shutdown(struct device *dev)
diff --git a/drivers/ata/pata_oldpiix.c b/drivers/ata/pata_oldpiix.c
index 319b64491b7..b9bf78b7d48 100644
--- a/drivers/ata/pata_oldpiix.c
+++ b/drivers/ata/pata_oldpiix.c
@@ -258,7 +258,7 @@ static struct pci_driver oldpiix_pci_driver = {
 	.id_table		= oldpiix_pci_tbl,
 	.probe			= oldpiix_init_one,
 	.remove			= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend		= ata_pci_device_suspend,
 	.resume			= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/pata_opti.c b/drivers/ata/pata_opti.c
index fb042e0519d..3a944a02926 100644
--- a/drivers/ata/pata_opti.c
+++ b/drivers/ata/pata_opti.c
@@ -184,7 +184,7 @@ static struct pci_driver opti_pci_driver = {
 	.id_table	= opti,
 	.probe 		= opti_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/pata_optidma.c b/drivers/ata/pata_optidma.c
index bb71ea214b9..bdec7efa464 100644
--- a/drivers/ata/pata_optidma.c
+++ b/drivers/ata/pata_optidma.c
@@ -440,7 +440,7 @@ static struct pci_driver optidma_pci_driver = {
 	.id_table	= optidma,
 	.probe 		= optidma_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/pata_pdc2027x.c b/drivers/ata/pata_pdc2027x.c
index 1151f23177b..4d06a5cda98 100644
--- a/drivers/ata/pata_pdc2027x.c
+++ b/drivers/ata/pata_pdc2027x.c
@@ -62,7 +62,7 @@ enum {
 };
 
 static int pdc2027x_init_one(struct pci_dev *pdev, const struct pci_device_id *ent);
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int pdc2027x_reinit_one(struct pci_dev *pdev);
 #endif
 static int pdc2027x_prereset(struct ata_link *link, unsigned long deadline);
@@ -128,7 +128,7 @@ static struct pci_driver pdc2027x_pci_driver = {
 	.id_table		= pdc2027x_pci_tbl,
 	.probe			= pdc2027x_init_one,
 	.remove			= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend		= ata_pci_device_suspend,
 	.resume			= pdc2027x_reinit_one,
 #endif
@@ -761,7 +761,7 @@ static int pdc2027x_init_one(struct pci_dev *pdev,
 				 IRQF_SHARED, &pdc2027x_sht);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int pdc2027x_reinit_one(struct pci_dev *pdev)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
diff --git a/drivers/ata/pata_pdc202xx_old.c b/drivers/ata/pata_pdc202xx_old.c
index defa050e178..9001991d283 100644
--- a/drivers/ata/pata_pdc202xx_old.c
+++ b/drivers/ata/pata_pdc202xx_old.c
@@ -377,7 +377,7 @@ static struct pci_driver pdc202xx_pci_driver = {
 	.id_table	= pdc202xx,
 	.probe 		= pdc202xx_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/pata_piccolo.c b/drivers/ata/pata_piccolo.c
index 0b46be11705..35cb0e26323 100644
--- a/drivers/ata/pata_piccolo.c
+++ b/drivers/ata/pata_piccolo.c
@@ -110,7 +110,7 @@ static struct pci_driver ata_tosh_pci_driver = {
 	.id_table	= ata_tosh,
 	.probe 		= ata_tosh_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/pata_radisys.c b/drivers/ata/pata_radisys.c
index be3f10240dc..a3f1123d17a 100644
--- a/drivers/ata/pata_radisys.c
+++ b/drivers/ata/pata_radisys.c
@@ -237,7 +237,7 @@ static struct pci_driver radisys_pci_driver = {
 	.id_table		= radisys_pci_tbl,
 	.probe			= radisys_init_one,
 	.remove			= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend		= ata_pci_device_suspend,
 	.resume			= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/pata_rdc.c b/drivers/ata/pata_rdc.c
index 521b2137ea3..9ce5952216b 100644
--- a/drivers/ata/pata_rdc.c
+++ b/drivers/ata/pata_rdc.c
@@ -382,7 +382,7 @@ static struct pci_driver rdc_pci_driver = {
 	.id_table		= rdc_pci_tbl,
 	.probe			= rdc_init_one,
 	.remove			= rdc_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend		= ata_pci_device_suspend,
 	.resume			= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/pata_rz1000.c b/drivers/ata/pata_rz1000.c
index caedc90855b..b3ec18c6bcc 100644
--- a/drivers/ata/pata_rz1000.c
+++ b/drivers/ata/pata_rz1000.c
@@ -101,7 +101,7 @@ static int rz1000_init_one (struct pci_dev *pdev, const struct pci_device_id *en
 	return -ENODEV;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int rz1000_reinit_one(struct pci_dev *pdev)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
@@ -133,7 +133,7 @@ static struct pci_driver rz1000_pci_driver = {
 	.id_table	= pata_rz1000,
 	.probe 		= rz1000_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= rz1000_reinit_one,
 #endif
diff --git a/drivers/ata/pata_samsung_cf.c b/drivers/ata/pata_samsung_cf.c
index 0610e78c8a2..fb528831fb9 100644
--- a/drivers/ata/pata_samsung_cf.c
+++ b/drivers/ata/pata_samsung_cf.c
@@ -619,7 +619,7 @@ static int __exit pata_s3c_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int pata_s3c_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
@@ -670,7 +670,7 @@ static struct platform_driver pata_s3c_driver = {
 	.driver		= {
 		.name	= DRV_NAME,
 		.owner	= THIS_MODULE,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 		.pm	= &pata_s3c_pm_ops,
 #endif
 	},
diff --git a/drivers/ata/pata_sc1200.c b/drivers/ata/pata_sc1200.c
index 96a232fffae..c71de5d680d 100644
--- a/drivers/ata/pata_sc1200.c
+++ b/drivers/ata/pata_sc1200.c
@@ -254,7 +254,7 @@ static struct pci_driver sc1200_pci_driver = {
 	.id_table	= sc1200,
 	.probe 		= sc1200_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/pata_scc.c b/drivers/ata/pata_scc.c
index f1f5b5ae338..4e006d74bef 100644
--- a/drivers/ata/pata_scc.c
+++ b/drivers/ata/pata_scc.c
@@ -1096,7 +1096,7 @@ static struct pci_driver scc_pci_driver = {
 	.id_table		= scc_pci_tbl,
 	.probe			= scc_init_one,
 	.remove			= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend		= ata_pci_device_suspend,
 	.resume			= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/pata_sch.c b/drivers/ata/pata_sch.c
index 5a1cde0ea36..b920c3407f8 100644
--- a/drivers/ata/pata_sch.c
+++ b/drivers/ata/pata_sch.c
@@ -64,7 +64,7 @@ static struct pci_driver sch_pci_driver = {
 	.id_table		= sch_pci_tbl,
 	.probe			= sch_init_one,
 	.remove			= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend		= ata_pci_device_suspend,
 	.resume			= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/pata_serverworks.c b/drivers/ata/pata_serverworks.c
index e27f31fe1b6..fc5f31d4828 100644
--- a/drivers/ata/pata_serverworks.c
+++ b/drivers/ata/pata_serverworks.c
@@ -436,7 +436,7 @@ static int serverworks_init_one(struct pci_dev *pdev, const struct pci_device_id
 	return ata_pci_bmdma_init_one(pdev, ppi, &serverworks_sht, NULL, 0);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int serverworks_reinit_one(struct pci_dev *pdev)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
@@ -468,7 +468,7 @@ static struct pci_driver serverworks_pci_driver = {
 	.id_table	= serverworks,
 	.probe 		= serverworks_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= serverworks_reinit_one,
 #endif
diff --git a/drivers/ata/pata_sil680.c b/drivers/ata/pata_sil680.c
index 73fe362d971..f597edccede 100644
--- a/drivers/ata/pata_sil680.c
+++ b/drivers/ata/pata_sil680.c
@@ -403,7 +403,7 @@ use_ioports:
 	return ata_pci_bmdma_init_one(pdev, ppi, &sil680_sht, NULL, 0);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int sil680_reinit_one(struct pci_dev *pdev)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
@@ -429,7 +429,7 @@ static struct pci_driver sil680_pci_driver = {
 	.id_table	= sil680,
 	.probe 		= sil680_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= sil680_reinit_one,
 #endif
diff --git a/drivers/ata/pata_sis.c b/drivers/ata/pata_sis.c
index 78d913aa93c..626f989d5c6 100644
--- a/drivers/ata/pata_sis.c
+++ b/drivers/ata/pata_sis.c
@@ -869,7 +869,7 @@ static int sis_init_one (struct pci_dev *pdev, const struct pci_device_id *ent)
 	return ata_pci_bmdma_init_one(pdev, ppi, &sis_sht, chipset, 0);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int sis_reinit_one(struct pci_dev *pdev)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
@@ -899,7 +899,7 @@ static struct pci_driver sis_pci_driver = {
 	.id_table		= sis_pci_tbl,
 	.probe			= sis_init_one,
 	.remove			= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend		= ata_pci_device_suspend,
 	.resume			= sis_reinit_one,
 #endif
diff --git a/drivers/ata/pata_sl82c105.c b/drivers/ata/pata_sl82c105.c
index 900f0e4a1fa..4935f61f629 100644
--- a/drivers/ata/pata_sl82c105.c
+++ b/drivers/ata/pata_sl82c105.c
@@ -337,7 +337,7 @@ static int sl82c105_init_one(struct pci_dev *dev, const struct pci_device_id *id
 	return ata_pci_bmdma_init_one(dev, ppi, &sl82c105_sht, NULL, 0);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int sl82c105_reinit_one(struct pci_dev *pdev)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
@@ -365,7 +365,7 @@ static struct pci_driver sl82c105_pci_driver = {
 	.id_table	= sl82c105,
 	.probe 		= sl82c105_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= sl82c105_reinit_one,
 #endif
diff --git a/drivers/ata/pata_triflex.c b/drivers/ata/pata_triflex.c
index 7bc78e264f9..d9364af8eb6 100644
--- a/drivers/ata/pata_triflex.c
+++ b/drivers/ata/pata_triflex.c
@@ -207,7 +207,7 @@ static const struct pci_device_id triflex[] = {
 	{ },
 };
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int triflex_ata_pci_device_suspend(struct pci_dev *pdev, pm_message_t mesg)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
@@ -233,7 +233,7 @@ static struct pci_driver triflex_pci_driver = {
 	.id_table	= triflex,
 	.probe 		= triflex_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= triflex_ata_pci_device_suspend,
 	.resume		= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/pata_via.c b/drivers/ata/pata_via.c
index f6c9632bdff..1ca6bcab369 100644
--- a/drivers/ata/pata_via.c
+++ b/drivers/ata/pata_via.c
@@ -659,7 +659,7 @@ static int via_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
 	return ata_pci_bmdma_init_one(pdev, ppi, &via_sht, (void *)config, 0);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 /**
  *	via_reinit_one		-	reinit after resume
  *	@pdev; PCI device
@@ -704,7 +704,7 @@ static struct pci_driver via_pci_driver = {
 	.id_table	= via,
 	.probe 		= via_init_one,
 	.remove		= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= via_reinit_one,
 #endif
diff --git a/drivers/ata/sata_fsl.c b/drivers/ata/sata_fsl.c
index fb0b40a191c..616a6d2ac20 100644
--- a/drivers/ata/sata_fsl.c
+++ b/drivers/ata/sata_fsl.c
@@ -774,20 +774,6 @@ static int sata_fsl_port_start(struct ata_port *ap)
 	VPRINTK("HControl = 0x%x\n", ioread32(hcr_base + HCONTROL));
 	VPRINTK("CHBA  = 0x%x\n", ioread32(hcr_base + CHBA));
 
-#ifdef CONFIG_MPC8315_DS
-	/*
-	 * Workaround for 8315DS board 3gbps link-up issue,
-	 * currently limit SATA port to GEN1 speed
-	 */
-	sata_fsl_scr_read(&ap->link, SCR_CONTROL, &temp);
-	temp &= ~(0xF << 4);
-	temp |= (0x1 << 4);
-	sata_fsl_scr_write(&ap->link, SCR_CONTROL, temp);
-
-	sata_fsl_scr_read(&ap->link, SCR_CONTROL, &temp);
-	dev_warn(dev, "scr_control, speed limited to %x\n", temp);
-#endif
-
 	return 0;
 }
 
@@ -1588,7 +1574,7 @@ static int sata_fsl_remove(struct platform_device *ofdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int sata_fsl_suspend(struct platform_device *op, pm_message_t state)
 {
 	struct ata_host *host = platform_get_drvdata(op);
@@ -1644,7 +1630,7 @@ static struct platform_driver fsl_sata_driver = {
 	},
 	.probe		= sata_fsl_probe,
 	.remove		= sata_fsl_remove,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= sata_fsl_suspend,
 	.resume		= sata_fsl_resume,
 #endif
diff --git a/drivers/ata/sata_inic162x.c b/drivers/ata/sata_inic162x.c
index 5c54d957370..069827826b2 100644
--- a/drivers/ata/sata_inic162x.c
+++ b/drivers/ata/sata_inic162x.c
@@ -785,7 +785,7 @@ static int init_controller(void __iomem *mmio_base, u16 hctl)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int inic_pci_device_resume(struct pci_dev *pdev)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
@@ -898,7 +898,7 @@ static const struct pci_device_id inic_pci_tbl[] = {
 static struct pci_driver inic_pci_driver = {
 	.name 		= DRV_NAME,
 	.id_table	= inic_pci_tbl,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend	= ata_pci_device_suspend,
 	.resume		= inic_pci_device_resume,
 #endif
diff --git a/drivers/ata/sata_mv.c b/drivers/ata/sata_mv.c
index 05c8a44adf8..391cfda1e83 100644
--- a/drivers/ata/sata_mv.c
+++ b/drivers/ata/sata_mv.c
@@ -4222,7 +4222,7 @@ static int mv_platform_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int mv_platform_suspend(struct platform_device *pdev, pm_message_t state)
 {
 	struct ata_host *host = platform_get_drvdata(pdev);
@@ -4289,7 +4289,7 @@ static struct platform_driver mv_platform_driver = {
 #ifdef CONFIG_PCI
 static int mv_pci_init_one(struct pci_dev *pdev,
 			   const struct pci_device_id *ent);
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int mv_pci_device_resume(struct pci_dev *pdev);
 #endif
 
@@ -4299,7 +4299,7 @@ static struct pci_driver mv_pci_driver = {
 	.id_table		= mv_pci_tbl,
 	.probe			= mv_pci_init_one,
 	.remove			= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend		= ata_pci_device_suspend,
 	.resume			= mv_pci_device_resume,
 #endif
@@ -4457,7 +4457,7 @@ static int mv_pci_init_one(struct pci_dev *pdev,
 				 IS_GEN_I(hpriv) ? &mv5_sht : &mv6_sht);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int mv_pci_device_resume(struct pci_dev *pdev)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
diff --git a/drivers/ata/sata_nv.c b/drivers/ata/sata_nv.c
index ba5f2712033..cdf99fac139 100644
--- a/drivers/ata/sata_nv.c
+++ b/drivers/ata/sata_nv.c
@@ -295,7 +295,7 @@ struct nv_swncq_port_priv {
 #define NV_ADMA_CHECK_INTR(GCTL, PORT) ((GCTL) & (1 << (19 + (12 * (PORT)))))
 
 static int nv_init_one(struct pci_dev *pdev, const struct pci_device_id *ent);
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int nv_pci_device_resume(struct pci_dev *pdev);
 #endif
 static void nv_ck804_host_stop(struct ata_host *host);
@@ -379,7 +379,7 @@ static struct pci_driver nv_pci_driver = {
 	.name			= DRV_NAME,
 	.id_table		= nv_pci_tbl,
 	.probe			= nv_init_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend		= ata_pci_device_suspend,
 	.resume			= nv_pci_device_resume,
 #endif
@@ -2431,7 +2431,7 @@ static int nv_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	return ata_pci_sff_activate_host(host, ipriv->irq_handler, ipriv->sht);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int nv_pci_device_resume(struct pci_dev *pdev)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
diff --git a/drivers/ata/sata_rcar.c b/drivers/ata/sata_rcar.c
index 2b25bd83fc9..61eb6d77dac 100644
--- a/drivers/ata/sata_rcar.c
+++ b/drivers/ata/sata_rcar.c
@@ -937,7 +937,7 @@ static int sata_rcar_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int sata_rcar_suspend(struct device *dev)
 {
 	struct ata_host *host = dev_get_drvdata(dev);
@@ -991,7 +991,7 @@ static struct platform_driver sata_rcar_driver = {
 		.name		= DRV_NAME,
 		.owner		= THIS_MODULE,
 		.of_match_table	= sata_rcar_match,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 		.pm		= &sata_rcar_pm_ops,
 #endif
 	},
diff --git a/drivers/ata/sata_sil.c b/drivers/ata/sata_sil.c
index 3062f8605b2..40b76b2d18c 100644
--- a/drivers/ata/sata_sil.c
+++ b/drivers/ata/sata_sil.c
@@ -112,7 +112,7 @@ enum {
 };
 
 static int sil_init_one(struct pci_dev *pdev, const struct pci_device_id *ent);
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int sil_pci_device_resume(struct pci_dev *pdev);
 #endif
 static void sil_dev_config(struct ata_device *dev);
@@ -166,7 +166,7 @@ static struct pci_driver sil_pci_driver = {
 	.id_table		= sil_pci_tbl,
 	.probe			= sil_init_one,
 	.remove			= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend		= ata_pci_device_suspend,
 	.resume			= sil_pci_device_resume,
 #endif
@@ -802,7 +802,7 @@ static int sil_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 				 &sil_sht);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int sil_pci_device_resume(struct pci_dev *pdev)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
diff --git a/drivers/ata/sata_sil24.c b/drivers/ata/sata_sil24.c
index aa1051ba6d1..0534890f118 100644
--- a/drivers/ata/sata_sil24.c
+++ b/drivers/ata/sata_sil24.c
@@ -353,8 +353,10 @@ static void sil24_error_handler(struct ata_port *ap);
 static void sil24_post_internal_cmd(struct ata_queued_cmd *qc);
 static int sil24_port_start(struct ata_port *ap);
 static int sil24_init_one(struct pci_dev *pdev, const struct pci_device_id *ent);
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int sil24_pci_device_resume(struct pci_dev *pdev);
+#endif
+#ifdef CONFIG_PM
 static int sil24_port_resume(struct ata_port *ap);
 #endif
 
@@ -375,7 +377,7 @@ static struct pci_driver sil24_pci_driver = {
 	.id_table		= sil24_pci_tbl,
 	.probe			= sil24_init_one,
 	.remove			= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend		= ata_pci_device_suspend,
 	.resume			= sil24_pci_device_resume,
 #endif
@@ -1350,7 +1352,7 @@ static int sil24_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 				 &sil24_sht);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int sil24_pci_device_resume(struct pci_dev *pdev)
 {
 	struct ata_host *host = pci_get_drvdata(pdev);
@@ -1370,7 +1372,9 @@ static int sil24_pci_device_resume(struct pci_dev *pdev)
 
 	return 0;
 }
+#endif
 
+#ifdef CONFIG_PM
 static int sil24_port_resume(struct ata_port *ap)
 {
 	sil24_config_pmp(ap, ap->nr_pmp_links);
diff --git a/drivers/ata/sata_sis.c b/drivers/ata/sata_sis.c
index b513428171b..d1637ac40a7 100644
--- a/drivers/ata/sata_sis.c
+++ b/drivers/ata/sata_sis.c
@@ -82,7 +82,7 @@ static struct pci_driver sis_pci_driver = {
 	.id_table		= sis_pci_tbl,
 	.probe			= sis_init_one,
 	.remove			= ata_pci_remove_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend		= ata_pci_device_suspend,
 	.resume			= ata_pci_device_resume,
 #endif
diff --git a/drivers/ata/sata_via.c b/drivers/ata/sata_via.c
index f72e84228c5..47bf89464ce 100644
--- a/drivers/ata/sata_via.c
+++ b/drivers/ata/sata_via.c
@@ -103,7 +103,7 @@ static struct pci_driver svia_pci_driver = {
 	.name			= DRV_NAME,
 	.id_table		= svia_pci_tbl,
 	.probe			= svia_init_one,
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	.suspend		= ata_pci_device_suspend,
 	.resume			= ata_pci_device_resume,
 #endif
diff --git a/drivers/base/Kconfig b/drivers/base/Kconfig
index 8fa8deab644..23b8726962a 100644
--- a/drivers/base/Kconfig
+++ b/drivers/base/Kconfig
@@ -1,10 +1,10 @@
 menu "Generic Driver Options"
 
-config UEVENT_HELPER_PATH
-	string "path to uevent helper"
-	default ""
+config UEVENT_HELPER
+	bool "Support for uevent helper"
+	default y
 	help
-	  Path to uevent helper program forked by the kernel for
+	  The uevent helper program is forked by the kernel for
 	  every uevent.
 	  Before the switch to the netlink-based uevent source, this was
 	  used to hook hotplug scripts into kernel device events. It
@@ -15,8 +15,13 @@ config UEVENT_HELPER_PATH
 	  that it creates a high system load, or on smaller systems
 	  it is known to create out-of-memory situations during bootup.
 
-	  To disable user space helper program execution at early boot
-	  time specify an empty string here. This setting can be altered
+config UEVENT_HELPER_PATH
+	string "path to uevent helper"
+	depends on UEVENT_HELPER
+	default ""
+	help
+	  To disable user space helper program execution at by default
+	  specify an empty string here. This setting can still be altered
 	  via /proc/sys/kernel/hotplug or via /sys/kernel/uevent_helper
 	  later at runtime.
 
@@ -253,7 +258,7 @@ endchoice
 
 config CMA_ALIGNMENT
 	int "Maximum PAGE_SIZE order of alignment for contiguous buffers"
-	range 4 9
+	range 4 12
 	default 8
 	help
 	  DMA mapping framework by default aligns all buffers to the smallest
diff --git a/drivers/base/base.h b/drivers/base/base.h
index 24f424249d9..251c5d30f96 100644
--- a/drivers/base/base.h
+++ b/drivers/base/base.h
@@ -63,8 +63,6 @@ struct driver_private {
  *	binding of drivers which were unable to get all the resources needed by
  *	the device; typically because it depends on another driver getting
  *	probed first.
- * @driver_data - private pointer for driver specific info.  Will turn into a
- * list soon.
  * @device - pointer back to the struct class that this structure is
  * associated with.
  *
@@ -76,7 +74,6 @@ struct device_private {
 	struct klist_node knode_driver;
 	struct klist_node knode_bus;
 	struct list_head deferred_probe;
-	void *driver_data;
 	struct device *device;
 };
 #define to_device_private_parent(obj)	\
diff --git a/drivers/base/dd.c b/drivers/base/dd.c
index 62ec61e8f84..e4ffbcf2f51 100644
--- a/drivers/base/dd.c
+++ b/drivers/base/dd.c
@@ -587,29 +587,3 @@ void driver_detach(struct device_driver *drv)
 		put_device(dev);
 	}
 }
-
-/*
- * These exports can't be _GPL due to .h files using this within them, and it
- * might break something that was previously working...
- */
-void *dev_get_drvdata(const struct device *dev)
-{
-	if (dev && dev->p)
-		return dev->p->driver_data;
-	return NULL;
-}
-EXPORT_SYMBOL(dev_get_drvdata);
-
-int dev_set_drvdata(struct device *dev, void *data)
-{
-	int error;
-
-	if (!dev->p) {
-		error = device_private_init(dev);
-		if (error)
-			return error;
-	}
-	dev->p->driver_data = data;
-	return 0;
-}
-EXPORT_SYMBOL(dev_set_drvdata);
diff --git a/drivers/base/devres.c b/drivers/base/devres.c
index db4e264eecb..52302946770 100644
--- a/drivers/base/devres.c
+++ b/drivers/base/devres.c
@@ -831,3 +831,100 @@ void devm_kfree(struct device *dev, void *p)
 	WARN_ON(rc);
 }
 EXPORT_SYMBOL_GPL(devm_kfree);
+
+/**
+ * devm_kmemdup - Resource-managed kmemdup
+ * @dev: Device this memory belongs to
+ * @src: Memory region to duplicate
+ * @len: Memory region length
+ * @gfp: GFP mask to use
+ *
+ * Duplicate region of a memory using resource managed kmalloc
+ */
+void *devm_kmemdup(struct device *dev, const void *src, size_t len, gfp_t gfp)
+{
+	void *p;
+
+	p = devm_kmalloc(dev, len, gfp);
+	if (p)
+		memcpy(p, src, len);
+
+	return p;
+}
+EXPORT_SYMBOL_GPL(devm_kmemdup);
+
+struct pages_devres {
+	unsigned long addr;
+	unsigned int order;
+};
+
+static int devm_pages_match(struct device *dev, void *res, void *p)
+{
+	struct pages_devres *devres = res;
+	struct pages_devres *target = p;
+
+	return devres->addr == target->addr;
+}
+
+static void devm_pages_release(struct device *dev, void *res)
+{
+	struct pages_devres *devres = res;
+
+	free_pages(devres->addr, devres->order);
+}
+
+/**
+ * devm_get_free_pages - Resource-managed __get_free_pages
+ * @dev: Device to allocate memory for
+ * @gfp_mask: Allocation gfp flags
+ * @order: Allocation size is (1 << order) pages
+ *
+ * Managed get_free_pages.  Memory allocated with this function is
+ * automatically freed on driver detach.
+ *
+ * RETURNS:
+ * Address of allocated memory on success, 0 on failure.
+ */
+
+unsigned long devm_get_free_pages(struct device *dev,
+				  gfp_t gfp_mask, unsigned int order)
+{
+	struct pages_devres *devres;
+	unsigned long addr;
+
+	addr = __get_free_pages(gfp_mask, order);
+
+	if (unlikely(!addr))
+		return 0;
+
+	devres = devres_alloc(devm_pages_release,
+			      sizeof(struct pages_devres), GFP_KERNEL);
+	if (unlikely(!devres)) {
+		free_pages(addr, order);
+		return 0;
+	}
+
+	devres->addr = addr;
+	devres->order = order;
+
+	devres_add(dev, devres);
+	return addr;
+}
+EXPORT_SYMBOL_GPL(devm_get_free_pages);
+
+/**
+ * devm_free_pages - Resource-managed free_pages
+ * @dev: Device this memory belongs to
+ * @addr: Memory to free
+ *
+ * Free memory allocated with devm_get_free_pages(). Unlike free_pages,
+ * there is no need to supply the @order.
+ */
+void devm_free_pages(struct device *dev, unsigned long addr)
+{
+	struct pages_devres devres = { .addr = addr };
+
+	WARN_ON(devres_release(dev, devm_pages_release, devm_pages_match,
+			       &devres));
+}
+EXPORT_SYMBOL_GPL(devm_free_pages);
diff --git a/drivers/base/dma-buf.c b/drivers/base/dma-buf.c
index ea77701deda..840c7fa8098 100644
--- a/drivers/base/dma-buf.c
+++ b/drivers/base/dma-buf.c
@@ -491,7 +491,7 @@ EXPORT_SYMBOL_GPL(dma_buf_kunmap);
  * 			dma-buf buffer.
  *
  * This function adjusts the passed in vma so that it points at the file of the
- * dma_buf operation. It alsog adjusts the starting pgoff and does bounds
+ * dma_buf operation. It also adjusts the starting pgoff and does bounds
  * checking on the size of the vma. Then it calls the exporters mmap function to
  * set up the mapping.
  *
diff --git a/drivers/base/dma-coherent.c b/drivers/base/dma-coherent.c
index bc256b64102..7d6e84a5142 100644
--- a/drivers/base/dma-coherent.c
+++ b/drivers/base/dma-coherent.c
@@ -10,13 +10,13 @@
 struct dma_coherent_mem {
 	void		*virt_base;
 	dma_addr_t	device_base;
-	phys_addr_t	pfn_base;
+	unsigned long	pfn_base;
 	int		size;
 	int		flags;
 	unsigned long	*bitmap;
 };
 
-int dma_declare_coherent_memory(struct device *dev, dma_addr_t bus_addr,
+int dma_declare_coherent_memory(struct device *dev, phys_addr_t phys_addr,
 				dma_addr_t device_addr, size_t size, int flags)
 {
 	void __iomem *mem_base = NULL;
@@ -32,7 +32,7 @@ int dma_declare_coherent_memory(struct device *dev, dma_addr_t bus_addr,
 
 	/* FIXME: this routine just ignores DMA_MEMORY_INCLUDES_CHILDREN */
 
-	mem_base = ioremap(bus_addr, size);
+	mem_base = ioremap(phys_addr, size);
 	if (!mem_base)
 		goto out;
 
@@ -45,7 +45,7 @@ int dma_declare_coherent_memory(struct device *dev, dma_addr_t bus_addr,
 
 	dev->dma_mem->virt_base = mem_base;
 	dev->dma_mem->device_base = device_addr;
-	dev->dma_mem->pfn_base = PFN_DOWN(bus_addr);
+	dev->dma_mem->pfn_base = PFN_DOWN(phys_addr);
 	dev->dma_mem->size = pages;
 	dev->dma_mem->flags = flags;
 
@@ -208,7 +208,7 @@ int dma_mmap_from_coherent(struct device *dev, struct vm_area_struct *vma,
 
 		*ret = -ENXIO;
 		if (off < count && user_count <= count - off) {
-			unsigned pfn = mem->pfn_base + start + off;
+			unsigned long pfn = mem->pfn_base + start + off;
 			*ret = remap_pfn_range(vma, vma->vm_start, pfn,
 					       user_count << PAGE_SHIFT,
 					       vma->vm_page_prot);
diff --git a/drivers/base/dma-contiguous.c b/drivers/base/dma-contiguous.c
index 165c2c299e5..83969f8c572 100644
--- a/drivers/base/dma-contiguous.c
+++ b/drivers/base/dma-contiguous.c
@@ -37,6 +37,7 @@ struct cma {
 	unsigned long	base_pfn;
 	unsigned long	count;
 	unsigned long	*bitmap;
+	struct mutex	lock;
 };
 
 struct cma *dma_contiguous_default_area;
@@ -59,11 +60,22 @@ struct cma *dma_contiguous_default_area;
  */
 static const phys_addr_t size_bytes = CMA_SIZE_MBYTES * SZ_1M;
 static phys_addr_t size_cmdline = -1;
+static phys_addr_t base_cmdline;
+static phys_addr_t limit_cmdline;
 
 static int __init early_cma(char *p)
 {
 	pr_debug("%s(%s)\n", __func__, p);
 	size_cmdline = memparse(p, &p);
+	if (*p != '@')
+		return 0;
+	base_cmdline = memparse(p + 1, &p);
+	if (*p != '-') {
+		limit_cmdline = base_cmdline + size_cmdline;
+		return 0;
+	}
+	limit_cmdline = memparse(p + 1, &p);
+
 	return 0;
 }
 early_param("cma", early_cma);
@@ -107,11 +119,18 @@ static inline __maybe_unused phys_addr_t cma_early_percent_memory(void)
 void __init dma_contiguous_reserve(phys_addr_t limit)
 {
 	phys_addr_t selected_size = 0;
+	phys_addr_t selected_base = 0;
+	phys_addr_t selected_limit = limit;
+	bool fixed = false;
 
 	pr_debug("%s(limit %08lx)\n", __func__, (unsigned long)limit);
 
 	if (size_cmdline != -1) {
 		selected_size = size_cmdline;
+		selected_base = base_cmdline;
+		selected_limit = min_not_zero(limit_cmdline, limit);
+		if (base_cmdline + size_cmdline == limit_cmdline)
+			fixed = true;
 	} else {
 #ifdef CONFIG_CMA_SIZE_SEL_MBYTES
 		selected_size = size_bytes;
@@ -128,10 +147,12 @@ void __init dma_contiguous_reserve(phys_addr_t limit)
 		pr_debug("%s: reserving %ld MiB for global area\n", __func__,
 			 (unsigned long)selected_size / SZ_1M);
 
-		dma_contiguous_reserve_area(selected_size, 0, limit,
-					    &dma_contiguous_default_area);
+		dma_contiguous_reserve_area(selected_size, selected_base,
+					    selected_limit,
+					    &dma_contiguous_default_area,
+					    fixed);
 	}
-};
+}
 
 static DEFINE_MUTEX(cma_mutex);
 
@@ -161,6 +182,7 @@ static int __init cma_activate_area(struct cma *cma)
 		init_cma_reserved_pageblock(pfn_to_page(base_pfn));
 	} while (--i);
 
+	mutex_init(&cma->lock);
 	return 0;
 }
 
@@ -187,15 +209,20 @@ core_initcall(cma_init_reserved_areas);
  * @base: Base address of the reserved area optional, use 0 for any
  * @limit: End address of the reserved memory (optional, 0 for any).
  * @res_cma: Pointer to store the created cma region.
+ * @fixed: hint about where to place the reserved area
  *
  * This function reserves memory from early allocator. It should be
  * called by arch specific code once the early allocator (memblock or bootmem)
  * has been activated and all other subsystems have already allocated/reserved
  * memory. This function allows to create custom reserved areas for specific
  * devices.
+ *
+ * If @fixed is true, reserve contiguous area at exactly @base.  If false,
+ * reserve in range from @base to @limit.
  */
 int __init dma_contiguous_reserve_area(phys_addr_t size, phys_addr_t base,
-				       phys_addr_t limit, struct cma **res_cma)
+				       phys_addr_t limit, struct cma **res_cma,
+				       bool fixed)
 {
 	struct cma *cma = &cma_areas[cma_area_count];
 	phys_addr_t alignment;
@@ -221,18 +248,15 @@ int __init dma_contiguous_reserve_area(phys_addr_t size, phys_addr_t base,
 	limit &= ~(alignment - 1);
 
 	/* Reserve memory */
-	if (base) {
+	if (base && fixed) {
 		if (memblock_is_region_reserved(base, size) ||
 		    memblock_reserve(base, size) < 0) {
 			ret = -EBUSY;
 			goto err;
 		}
 	} else {
-		/*
-		 * Use __memblock_alloc_base() since
-		 * memblock_alloc_base() panic()s.
-		 */
-		phys_addr_t addr = __memblock_alloc_base(size, alignment, limit);
+		phys_addr_t addr = memblock_alloc_range(size, alignment, base,
+							limit);
 		if (!addr) {
 			ret = -ENOMEM;
 			goto err;
@@ -261,6 +285,13 @@ err:
 	return ret;
 }
 
+static void clear_cma_bitmap(struct cma *cma, unsigned long pfn, int count)
+{
+	mutex_lock(&cma->lock);
+	bitmap_clear(cma->bitmap, pfn - cma->base_pfn, count);
+	mutex_unlock(&cma->lock);
+}
+
 /**
  * dma_alloc_from_contiguous() - allocate pages from contiguous area
  * @dev:   Pointer to device for which the allocation is performed.
@@ -269,7 +300,7 @@ err:
  *
  * This function allocates memory buffer for specified device. It uses
  * device specific contiguous memory area if available or the default
- * global one. Requires architecture specific get_dev_cma_area() helper
+ * global one. Requires architecture specific dev_get_cma_area() helper
  * function.
  */
 struct page *dma_alloc_from_contiguous(struct device *dev, int count,
@@ -294,30 +325,41 @@ struct page *dma_alloc_from_contiguous(struct device *dev, int count,
 
 	mask = (1 << align) - 1;
 
-	mutex_lock(&cma_mutex);
 
 	for (;;) {
+		mutex_lock(&cma->lock);
 		pageno = bitmap_find_next_zero_area(cma->bitmap, cma->count,
 						    start, count, mask);
-		if (pageno >= cma->count)
+		if (pageno >= cma->count) {
+			mutex_unlock(&cma->lock);
 			break;
+		}
+		bitmap_set(cma->bitmap, pageno, count);
+		/*
+		 * It's safe to drop the lock here. We've marked this region for
+		 * our exclusive use. If the migration fails we will take the
+		 * lock again and unmark it.
+		 */
+		mutex_unlock(&cma->lock);
 
 		pfn = cma->base_pfn + pageno;
+		mutex_lock(&cma_mutex);
 		ret = alloc_contig_range(pfn, pfn + count, MIGRATE_CMA);
+		mutex_unlock(&cma_mutex);
 		if (ret == 0) {
-			bitmap_set(cma->bitmap, pageno, count);
 			page = pfn_to_page(pfn);
 			break;
 		} else if (ret != -EBUSY) {
+			clear_cma_bitmap(cma, pfn, count);
 			break;
 		}
+		clear_cma_bitmap(cma, pfn, count);
 		pr_debug("%s(): memory range at %p is busy, retrying\n",
 			 __func__, pfn_to_page(pfn));
 		/* try again with a bit different memory target */
 		start = pageno + mask + 1;
 	}
 
-	mutex_unlock(&cma_mutex);
 	pr_debug("%s(): returned %p\n", __func__, page);
 	return page;
 }
@@ -350,10 +392,8 @@ bool dma_release_from_contiguous(struct device *dev, struct page *pages,
 
 	VM_BUG_ON(pfn + count > cma->base_pfn + cma->count);
 
-	mutex_lock(&cma_mutex);
-	bitmap_clear(cma->bitmap, pfn - cma->base_pfn, count);
 	free_contig_range(pfn, count);
-	mutex_unlock(&cma_mutex);
+	clear_cma_bitmap(cma, pfn, count);
 
 	return true;
 }
diff --git a/drivers/base/dma-mapping.c b/drivers/base/dma-mapping.c
index 0ce39a33b3c..6cd08e145bf 100644
--- a/drivers/base/dma-mapping.c
+++ b/drivers/base/dma-mapping.c
@@ -175,7 +175,7 @@ static void dmam_coherent_decl_release(struct device *dev, void *res)
 /**
  * dmam_declare_coherent_memory - Managed dma_declare_coherent_memory()
  * @dev: Device to declare coherent memory for
- * @bus_addr: Bus address of coherent memory to be declared
+ * @phys_addr: Physical address of coherent memory to be declared
  * @device_addr: Device address of coherent memory to be declared
  * @size: Size of coherent memory to be declared
  * @flags: Flags
@@ -185,7 +185,7 @@ static void dmam_coherent_decl_release(struct device *dev, void *res)
  * RETURNS:
  * 0 on success, -errno on failure.
  */
-int dmam_declare_coherent_memory(struct device *dev, dma_addr_t bus_addr,
+int dmam_declare_coherent_memory(struct device *dev, phys_addr_t phys_addr,
 				 dma_addr_t device_addr, size_t size, int flags)
 {
 	void *res;
@@ -195,7 +195,7 @@ int dmam_declare_coherent_memory(struct device *dev, dma_addr_t bus_addr,
 	if (!res)
 		return -ENOMEM;
 
-	rc = dma_declare_coherent_memory(dev, bus_addr, device_addr, size,
+	rc = dma_declare_coherent_memory(dev, phys_addr, device_addr, size,
 					 flags);
 	if (rc == 0)
 		devres_add(dev, res);
diff --git a/drivers/base/memory.c b/drivers/base/memory.c
index bece691cb5d..89f752dd846 100644
--- a/drivers/base/memory.c
+++ b/drivers/base/memory.c
@@ -118,16 +118,6 @@ static ssize_t show_mem_start_phys_index(struct device *dev,
 	return sprintf(buf, "%08lx\n", phys_index);
 }
 
-static ssize_t show_mem_end_phys_index(struct device *dev,
-			struct device_attribute *attr, char *buf)
-{
-	struct memory_block *mem = to_memory_block(dev);
-	unsigned long phys_index;
-
-	phys_index = mem->end_section_nr / sections_per_block;
-	return sprintf(buf, "%08lx\n", phys_index);
-}
-
 /*
  * Show whether the section of memory is likely to be hot-removable
  */
@@ -384,7 +374,6 @@ static ssize_t show_phys_device(struct device *dev,
 }
 
 static DEVICE_ATTR(phys_index, 0444, show_mem_start_phys_index, NULL);
-static DEVICE_ATTR(end_phys_index, 0444, show_mem_end_phys_index, NULL);
 static DEVICE_ATTR(state, 0644, show_mem_state, store_mem_state);
 static DEVICE_ATTR(phys_device, 0444, show_phys_device, NULL);
 static DEVICE_ATTR(removable, 0444, show_mem_removable, NULL);
@@ -529,7 +518,6 @@ struct memory_block *find_memory_block(struct mem_section *section)
 
 static struct attribute *memory_memblk_attrs[] = {
 	&dev_attr_phys_index.attr,
-	&dev_attr_end_phys_index.attr,
 	&dev_attr_state.attr,
 	&dev_attr_phys_device.attr,
 	&dev_attr_removable.attr,
diff --git a/drivers/base/platform.c b/drivers/base/platform.c
index 5b47210889e..9e9227e1762 100644
--- a/drivers/base/platform.c
+++ b/drivers/base/platform.c
@@ -131,9 +131,12 @@ EXPORT_SYMBOL_GPL(platform_get_resource_byname);
  */
 int platform_get_irq_byname(struct platform_device *dev, const char *name)
 {
-	struct resource *r = platform_get_resource_byname(dev, IORESOURCE_IRQ,
-							  name);
+	struct resource *r;
+
+	if (IS_ENABLED(CONFIG_OF_IRQ) && dev->dev.of_node)
+		return of_irq_get_byname(dev->dev.of_node, name);
 
+	r = platform_get_resource_byname(dev, IORESOURCE_IRQ, name);
 	return r ? r->start : -ENXIO;
 }
 EXPORT_SYMBOL_GPL(platform_get_irq_byname);
diff --git a/drivers/base/power/domain.c b/drivers/base/power/domain.c
index ae098a261fc..eee55c1e5fd 100644
--- a/drivers/base/power/domain.c
+++ b/drivers/base/power/domain.c
@@ -105,7 +105,7 @@ static bool genpd_sd_counter_dec(struct generic_pm_domain *genpd)
 static void genpd_sd_counter_inc(struct generic_pm_domain *genpd)
 {
 	atomic_inc(&genpd->sd_count);
-	smp_mb__after_atomic_inc();
+	smp_mb__after_atomic();
 }
 
 static void genpd_acquire_lock(struct generic_pm_domain *genpd)
diff --git a/drivers/base/power/main.c b/drivers/base/power/main.c
index 86d5e4fb5b9..343ffad5937 100644
--- a/drivers/base/power/main.c
+++ b/drivers/base/power/main.c
@@ -479,7 +479,7 @@ static int device_resume_noirq(struct device *dev, pm_message_t state, bool asyn
 	TRACE_DEVICE(dev);
 	TRACE_RESUME(0);
 
-	if (dev->power.syscore)
+	if (dev->power.syscore || dev->power.direct_complete)
 		goto Out;
 
 	if (!dev->power.is_noirq_suspended)
@@ -605,7 +605,7 @@ static int device_resume_early(struct device *dev, pm_message_t state, bool asyn
 	TRACE_DEVICE(dev);
 	TRACE_RESUME(0);
 
-	if (dev->power.syscore)
+	if (dev->power.syscore || dev->power.direct_complete)
 		goto Out;
 
 	if (!dev->power.is_late_suspended)
@@ -735,6 +735,12 @@ static int device_resume(struct device *dev, pm_message_t state, bool async)
 	if (dev->power.syscore)
 		goto Complete;
 
+	if (dev->power.direct_complete) {
+		/* Match the pm_runtime_disable() in __device_suspend(). */
+		pm_runtime_enable(dev);
+		goto Complete;
+	}
+
 	dpm_wait(dev->parent, async);
 	dpm_watchdog_set(&wd, dev);
 	device_lock(dev);
@@ -1007,7 +1013,7 @@ static int __device_suspend_noirq(struct device *dev, pm_message_t state, bool a
 		goto Complete;
 	}
 
-	if (dev->power.syscore)
+	if (dev->power.syscore || dev->power.direct_complete)
 		goto Complete;
 
 	dpm_wait_for_children(dev, async);
@@ -1146,7 +1152,7 @@ static int __device_suspend_late(struct device *dev, pm_message_t state, bool as
 		goto Complete;
 	}
 
-	if (dev->power.syscore)
+	if (dev->power.syscore || dev->power.direct_complete)
 		goto Complete;
 
 	dpm_wait_for_children(dev, async);
@@ -1332,6 +1338,17 @@ static int __device_suspend(struct device *dev, pm_message_t state, bool async)
 	if (dev->power.syscore)
 		goto Complete;
 
+	if (dev->power.direct_complete) {
+		if (pm_runtime_status_suspended(dev)) {
+			pm_runtime_disable(dev);
+			if (pm_runtime_suspended_if_enabled(dev))
+				goto Complete;
+
+			pm_runtime_enable(dev);
+		}
+		dev->power.direct_complete = false;
+	}
+
 	dpm_watchdog_set(&wd, dev);
 	device_lock(dev);
 
@@ -1382,10 +1399,19 @@ static int __device_suspend(struct device *dev, pm_message_t state, bool async)
 
  End:
 	if (!error) {
+		struct device *parent = dev->parent;
+
 		dev->power.is_suspended = true;
-		if (dev->power.wakeup_path
-		    && dev->parent && !dev->parent->power.ignore_children)
-			dev->parent->power.wakeup_path = true;
+		if (parent) {
+			spin_lock_irq(&parent->power.lock);
+
+			dev->parent->power.direct_complete = false;
+			if (dev->power.wakeup_path
+			    && !dev->parent->power.ignore_children)
+				dev->parent->power.wakeup_path = true;
+
+			spin_unlock_irq(&parent->power.lock);
+		}
 	}
 
 	device_unlock(dev);
@@ -1487,7 +1513,7 @@ static int device_prepare(struct device *dev, pm_message_t state)
 {
 	int (*callback)(struct device *) = NULL;
 	char *info = NULL;
-	int error = 0;
+	int ret = 0;
 
 	if (dev->power.syscore)
 		return 0;
@@ -1523,17 +1549,27 @@ static int device_prepare(struct device *dev, pm_message_t state)
 		callback = dev->driver->pm->prepare;
 	}
 
-	if (callback) {
-		error = callback(dev);
-		suspend_report_result(callback, error);
-	}
+	if (callback)
+		ret = callback(dev);
 
 	device_unlock(dev);
 
-	if (error)
+	if (ret < 0) {
+		suspend_report_result(callback, ret);
 		pm_runtime_put(dev);
-
-	return error;
+		return ret;
+	}
+	/*
+	 * A positive return value from ->prepare() means "this device appears
+	 * to be runtime-suspended and its state is fine, so if it really is
+	 * runtime-suspended, you can leave it in that state provided that you
+	 * will do the same thing with all of its descendants".  This only
+	 * applies to suspend transitions, however.
+	 */
+	spin_lock_irq(&dev->power.lock);
+	dev->power.direct_complete = ret > 0 && state.event == PM_EVENT_SUSPEND;
+	spin_unlock_irq(&dev->power.lock);
+	return 0;
 }
 
 /**
diff --git a/drivers/base/power/opp.c b/drivers/base/power/opp.c
index 25538675d59..89ced955faf 100644
--- a/drivers/base/power/opp.c
+++ b/drivers/base/power/opp.c
@@ -15,7 +15,6 @@
 #include <linux/errno.h>
 #include <linux/err.h>
 #include <linux/slab.h>
-#include <linux/cpufreq.h>
 #include <linux/device.h>
 #include <linux/list.h>
 #include <linux/rculist.h>
@@ -394,6 +393,13 @@ EXPORT_SYMBOL_GPL(dev_pm_opp_find_freq_floor);
  * to keep the integrity of the internal data structures. Callers should ensure
  * that this function is *NOT* called under RCU protection or in contexts where
  * mutex cannot be locked.
+ *
+ * Return:
+ * 0:		On success OR
+ *		Duplicate OPPs (both freq and volt are same) and opp->available
+ * -EEXIST:	Freq are same and volt are different OR
+ *		Duplicate OPPs (both freq and volt are same) and !opp->available
+ * -ENOMEM:	Memory allocation failure
  */
 int dev_pm_opp_add(struct device *dev, unsigned long freq, unsigned long u_volt)
 {
@@ -443,15 +449,31 @@ int dev_pm_opp_add(struct device *dev, unsigned long freq, unsigned long u_volt)
 	new_opp->u_volt = u_volt;
 	new_opp->available = true;
 
-	/* Insert new OPP in order of increasing frequency */
+	/*
+	 * Insert new OPP in order of increasing frequency
+	 * and discard if already present
+	 */
 	head = &dev_opp->opp_list;
 	list_for_each_entry_rcu(opp, &dev_opp->opp_list, node) {
-		if (new_opp->rate < opp->rate)
+		if (new_opp->rate <= opp->rate)
 			break;
 		else
 			head = &opp->node;
 	}
 
+	/* Duplicate OPPs ? */
+	if (new_opp->rate == opp->rate) {
+		int ret = opp->available && new_opp->u_volt == opp->u_volt ?
+			0 : -EEXIST;
+
+		dev_warn(dev, "%s: duplicate OPPs detected. Existing: freq: %lu, volt: %lu, enabled: %d. New: freq: %lu, volt: %lu, enabled: %d\n",
+			 __func__, opp->rate, opp->u_volt, opp->available,
+			 new_opp->rate, new_opp->u_volt, new_opp->available);
+		mutex_unlock(&dev_opp_list_lock);
+		kfree(new_opp);
+		return ret;
+	}
+
 	list_add_rcu(&new_opp->node, head);
 	mutex_unlock(&dev_opp_list_lock);
 
@@ -596,96 +618,6 @@ int dev_pm_opp_disable(struct device *dev, unsigned long freq)
 }
 EXPORT_SYMBOL_GPL(dev_pm_opp_disable);
 
-#ifdef CONFIG_CPU_FREQ
-/**
- * dev_pm_opp_init_cpufreq_table() - create a cpufreq table for a device
- * @dev:	device for which we do this operation
- * @table:	Cpufreq table returned back to caller
- *
- * Generate a cpufreq table for a provided device- this assumes that the
- * opp list is already initialized and ready for usage.
- *
- * This function allocates required memory for the cpufreq table. It is
- * expected that the caller does the required maintenance such as freeing
- * the table as required.
- *
- * Returns -EINVAL for bad pointers, -ENODEV if the device is not found, -ENOMEM
- * if no memory available for the operation (table is not populated), returns 0
- * if successful and table is populated.
- *
- * WARNING: It is  important for the callers to ensure refreshing their copy of
- * the table if any of the mentioned functions have been invoked in the interim.
- *
- * Locking: The internal device_opp and opp structures are RCU protected.
- * To simplify the logic, we pretend we are updater and hold relevant mutex here
- * Callers should ensure that this function is *NOT* called under RCU protection
- * or in contexts where mutex locking cannot be used.
- */
-int dev_pm_opp_init_cpufreq_table(struct device *dev,
-			    struct cpufreq_frequency_table **table)
-{
-	struct device_opp *dev_opp;
-	struct dev_pm_opp *opp;
-	struct cpufreq_frequency_table *freq_table;
-	int i = 0;
-
-	/* Pretend as if I am an updater */
-	mutex_lock(&dev_opp_list_lock);
-
-	dev_opp = find_device_opp(dev);
-	if (IS_ERR(dev_opp)) {
-		int r = PTR_ERR(dev_opp);
-		mutex_unlock(&dev_opp_list_lock);
-		dev_err(dev, "%s: Device OPP not found (%d)\n", __func__, r);
-		return r;
-	}
-
-	freq_table = kzalloc(sizeof(struct cpufreq_frequency_table) *
-			     (dev_pm_opp_get_opp_count(dev) + 1), GFP_KERNEL);
-	if (!freq_table) {
-		mutex_unlock(&dev_opp_list_lock);
-		dev_warn(dev, "%s: Unable to allocate frequency table\n",
-			__func__);
-		return -ENOMEM;
-	}
-
-	list_for_each_entry(opp, &dev_opp->opp_list, node) {
-		if (opp->available) {
-			freq_table[i].driver_data = i;
-			freq_table[i].frequency = opp->rate / 1000;
-			i++;
-		}
-	}
-	mutex_unlock(&dev_opp_list_lock);
-
-	freq_table[i].driver_data = i;
-	freq_table[i].frequency = CPUFREQ_TABLE_END;
-
-	*table = &freq_table[0];
-
-	return 0;
-}
-EXPORT_SYMBOL_GPL(dev_pm_opp_init_cpufreq_table);
-
-/**
- * dev_pm_opp_free_cpufreq_table() - free the cpufreq table
- * @dev:	device for which we do this operation
- * @table:	table to free
- *
- * Free up the table allocated by dev_pm_opp_init_cpufreq_table
- */
-void dev_pm_opp_free_cpufreq_table(struct device *dev,
-				struct cpufreq_frequency_table **table)
-{
-	if (!table)
-		return;
-
-	kfree(*table);
-	*table = NULL;
-}
-EXPORT_SYMBOL_GPL(dev_pm_opp_free_cpufreq_table);
-#endif		/* CONFIG_CPU_FREQ */
-
 /**
  * dev_pm_opp_get_notifier() - find notifier_head of the device with opp
  * @dev:	device pointer used to lookup device OPPs.
@@ -734,11 +666,9 @@ int of_init_opp_table(struct device *dev)
 		unsigned long freq = be32_to_cpup(val++) * 1000;
 		unsigned long volt = be32_to_cpup(val++);
 
-		if (dev_pm_opp_add(dev, freq, volt)) {
+		if (dev_pm_opp_add(dev, freq, volt))
 			dev_warn(dev, "%s: Failed to add OPP %ld\n",
 				 __func__, freq);
-			continue;
-		}
 		nr -= 2;
 	}
 
diff --git a/drivers/base/power/wakeup.c b/drivers/base/power/wakeup.c
index 2d56f4113ae..eb1bd2ecad8 100644
--- a/drivers/base/power/wakeup.c
+++ b/drivers/base/power/wakeup.c
@@ -318,10 +318,16 @@ int device_init_wakeup(struct device *dev, bool enable)
 {
 	int ret = 0;
 
+	if (!dev)
+		return -EINVAL;
+
 	if (enable) {
 		device_set_wakeup_capable(dev, true);
 		ret = device_wakeup_enable(dev);
 	} else {
+		if (dev->power.can_wakeup)
+			device_wakeup_disable(dev);
+
 		device_set_wakeup_capable(dev, false);
 	}
 
diff --git a/drivers/base/regmap/regcache-rbtree.c b/drivers/base/regmap/regcache-rbtree.c
index 930cad4e5df..6a7e4fa1285 100644
--- a/drivers/base/regmap/regcache-rbtree.c
+++ b/drivers/base/regmap/regcache-rbtree.c
@@ -23,16 +23,16 @@ static int regcache_rbtree_write(struct regmap *map, unsigned int reg,
 static int regcache_rbtree_exit(struct regmap *map);
 
 struct regcache_rbtree_node {
-	/* the actual rbtree node holding this block */
-	struct rb_node node;
-	/* base register handled by this block */
-	unsigned int base_reg;
 	/* block of adjacent registers */
 	void *block;
 	/* Which registers are present */
 	long *cache_present;
+	/* base register handled by this block */
+	unsigned int base_reg;
 	/* number of registers available in the block */
 	unsigned int blklen;
+	/* the actual rbtree node holding this block */
+	struct rb_node node;
 } __attribute__ ((packed));
 
 struct regcache_rbtree_ctx {
diff --git a/drivers/base/regmap/regmap-i2c.c b/drivers/base/regmap/regmap-i2c.c
index ebd18952976..ca193d1ef47 100644
--- a/drivers/base/regmap/regmap-i2c.c
+++ b/drivers/base/regmap/regmap-i2c.c
@@ -14,6 +14,79 @@
 #include <linux/i2c.h>
 #include <linux/module.h>
 
+
+static int regmap_smbus_byte_reg_read(void *context, unsigned int reg,
+				      unsigned int *val)
+{
+	struct device *dev = context;
+	struct i2c_client *i2c = to_i2c_client(dev);
+	int ret;
+
+	if (reg > 0xff)
+		return -EINVAL;
+
+	ret = i2c_smbus_read_byte_data(i2c, reg);
+	if (ret < 0)
+		return ret;
+
+	*val = ret;
+
+	return 0;
+}
+
+static int regmap_smbus_byte_reg_write(void *context, unsigned int reg,
+				       unsigned int val)
+{
+	struct device *dev = context;
+	struct i2c_client *i2c = to_i2c_client(dev);
+
+	if (val > 0xff || reg > 0xff)
+		return -EINVAL;
+
+	return i2c_smbus_write_byte_data(i2c, reg, val);
+}
+
+static struct regmap_bus regmap_smbus_byte = {
+	.reg_write = regmap_smbus_byte_reg_write,
+	.reg_read = regmap_smbus_byte_reg_read,
+};
+
+static int regmap_smbus_word_reg_read(void *context, unsigned int reg,
+				      unsigned int *val)
+{
+	struct device *dev = context;
+	struct i2c_client *i2c = to_i2c_client(dev);
+	int ret;
+
+	if (reg > 0xff)
+		return -EINVAL;
+
+	ret = i2c_smbus_read_word_data(i2c, reg);
+	if (ret < 0)
+		return ret;
+
+	*val = ret;
+
+	return 0;
+}
+
+static int regmap_smbus_word_reg_write(void *context, unsigned int reg,
+				       unsigned int val)
+{
+	struct device *dev = context;
+	struct i2c_client *i2c = to_i2c_client(dev);
+
+	if (val > 0xffff || reg > 0xff)
+		return -EINVAL;
+
+	return i2c_smbus_write_word_data(i2c, reg, val);
+}
+
+static struct regmap_bus regmap_smbus_word = {
+	.reg_write = regmap_smbus_word_reg_write,
+	.reg_read = regmap_smbus_word_reg_read,
+};
+
 static int regmap_i2c_write(void *context, const void *data, size_t count)
 {
 	struct device *dev = context;
@@ -97,6 +170,23 @@ static struct regmap_bus regmap_i2c = {
 	.read = regmap_i2c_read,
 };
 
+static const struct regmap_bus *regmap_get_i2c_bus(struct i2c_client *i2c,
+					const struct regmap_config *config)
+{
+	if (i2c_check_functionality(i2c->adapter, I2C_FUNC_I2C))
+		return &regmap_i2c;
+	else if (config->val_bits == 16 && config->reg_bits == 8 &&
+		 i2c_check_functionality(i2c->adapter,
+					 I2C_FUNC_SMBUS_WORD_DATA))
+		return &regmap_smbus_word;
+	else if (config->val_bits == 8 && config->reg_bits == 8 &&
+		 i2c_check_functionality(i2c->adapter,
+					 I2C_FUNC_SMBUS_BYTE_DATA))
+		return &regmap_smbus_byte;
+
+	return ERR_PTR(-ENOTSUPP);
+}
+
 /**
  * regmap_init_i2c(): Initialise register map
  *
@@ -109,7 +199,12 @@ static struct regmap_bus regmap_i2c = {
 struct regmap *regmap_init_i2c(struct i2c_client *i2c,
 			       const struct regmap_config *config)
 {
-	return regmap_init(&i2c->dev, &regmap_i2c, &i2c->dev, config);
+	const struct regmap_bus *bus = regmap_get_i2c_bus(i2c, config);
+
+	if (IS_ERR(bus))
+		return ERR_CAST(bus);
+
+	return regmap_init(&i2c->dev, bus, &i2c->dev, config);
 }
 EXPORT_SYMBOL_GPL(regmap_init_i2c);
 
@@ -126,7 +221,12 @@ EXPORT_SYMBOL_GPL(regmap_init_i2c);
 struct regmap *devm_regmap_init_i2c(struct i2c_client *i2c,
 				    const struct regmap_config *config)
 {
-	return devm_regmap_init(&i2c->dev, &regmap_i2c, &i2c->dev, config);
+	const struct regmap_bus *bus = regmap_get_i2c_bus(i2c, config);
+
+	if (IS_ERR(bus))
+		return ERR_CAST(bus);
+
+	return devm_regmap_init(&i2c->dev, bus, &i2c->dev, config);
 }
 EXPORT_SYMBOL_GPL(devm_regmap_init_i2c);
 
diff --git a/drivers/base/regmap/regmap-irq.c b/drivers/base/regmap/regmap-irq.c
index edf88f20cbc..6299a50a596 100644
--- a/drivers/base/regmap/regmap-irq.c
+++ b/drivers/base/regmap/regmap-irq.c
@@ -10,13 +10,13 @@
  * published by the Free Software Foundation.
  */
 
-#include <linux/export.h>
 #include <linux/device.h>
-#include <linux/regmap.h>
-#include <linux/irq.h>
+#include <linux/export.h>
 #include <linux/interrupt.h>
+#include <linux/irq.h>
 #include <linux/irqdomain.h>
 #include <linux/pm_runtime.h>
+#include <linux/regmap.h>
 #include <linux/slab.h>
 
 #include "internal.h"
@@ -347,6 +347,9 @@ int regmap_add_irq_chip(struct regmap *map, int irq, int irq_flags,
 	int ret = -ENOMEM;
 	u32 reg;
 
+	if (chip->num_regs <= 0)
+		return -EINVAL;
+
 	for (i = 0; i < chip->num_irqs; i++) {
 		if (chip->irqs[i].reg_offset % map->reg_stride)
 			return -EINVAL;
diff --git a/drivers/base/regmap/regmap-mmio.c b/drivers/base/regmap/regmap-mmio.c
index 1e03e7f8bac..04a329a377e 100644
--- a/drivers/base/regmap/regmap-mmio.c
+++ b/drivers/base/regmap/regmap-mmio.c
@@ -61,9 +61,28 @@ static int regmap_mmio_regbits_check(size_t reg_bits)
 	}
 }
 
-static inline void regmap_mmio_count_check(size_t count)
+static inline void regmap_mmio_count_check(size_t count, u32 offset)
 {
-	BUG_ON(count % 2 != 0);
+	BUG_ON(count <= offset);
+}
+
+static inline unsigned int
+regmap_mmio_get_offset(const void *reg, size_t reg_size)
+{
+	switch (reg_size) {
+	case 1:
+		return *(u8 *)reg;
+	case 2:
+		return *(u16 *)reg;
+	case 4:
+		return *(u32 *)reg;
+#ifdef CONFIG_64BIT
+	case 8:
+		return *(u64 *)reg;
+#endif
+	default:
+		BUG();
+	}
 }
 
 static int regmap_mmio_gather_write(void *context,
@@ -71,7 +90,7 @@ static int regmap_mmio_gather_write(void *context,
 				    const void *val, size_t val_size)
 {
 	struct regmap_mmio_context *ctx = context;
-	u32 offset;
+	unsigned int offset;
 	int ret;
 
 	regmap_mmio_regsize_check(reg_size);
@@ -82,7 +101,7 @@ static int regmap_mmio_gather_write(void *context,
 			return ret;
 	}
 
-	offset = *(u32 *)reg;
+	offset = regmap_mmio_get_offset(reg, reg_size);
 
 	while (val_size) {
 		switch (ctx->val_bytes) {
@@ -118,9 +137,9 @@ static int regmap_mmio_gather_write(void *context,
 static int regmap_mmio_write(void *context, const void *data, size_t count)
 {
 	struct regmap_mmio_context *ctx = context;
-	u32 offset = ctx->reg_bytes + ctx->pad_bytes;
+	unsigned int offset = ctx->reg_bytes + ctx->pad_bytes;
 
-	regmap_mmio_count_check(count);
+	regmap_mmio_count_check(count, offset);
 
 	return regmap_mmio_gather_write(context, data, ctx->reg_bytes,
 					data + offset, count - offset);
@@ -131,7 +150,7 @@ static int regmap_mmio_read(void *context,
 			    void *val, size_t val_size)
 {
 	struct regmap_mmio_context *ctx = context;
-	u32 offset;
+	unsigned int offset;
 	int ret;
 
 	regmap_mmio_regsize_check(reg_size);
@@ -142,7 +161,7 @@ static int regmap_mmio_read(void *context,
 			return ret;
 	}
 
-	offset = *(u32 *)reg;
+	offset = regmap_mmio_get_offset(reg, reg_size);
 
 	while (val_size) {
 		switch (ctx->val_bytes) {
diff --git a/drivers/base/regmap/regmap.c b/drivers/base/regmap/regmap.c
index 63e30ef096e..74d8c0672cf 100644
--- a/drivers/base/regmap/regmap.c
+++ b/drivers/base/regmap/regmap.c
@@ -35,10 +35,14 @@ static int _regmap_update_bits(struct regmap *map, unsigned int reg,
 			       unsigned int mask, unsigned int val,
 			       bool *change);
 
+static int _regmap_bus_reg_read(void *context, unsigned int reg,
+				unsigned int *val);
 static int _regmap_bus_read(void *context, unsigned int reg,
 			    unsigned int *val);
 static int _regmap_bus_formatted_write(void *context, unsigned int reg,
 				       unsigned int val);
+static int _regmap_bus_reg_write(void *context, unsigned int reg,
+				 unsigned int val);
 static int _regmap_bus_raw_write(void *context, unsigned int reg,
 				 unsigned int val);
 
@@ -192,6 +196,13 @@ static void regmap_format_16_be(void *buf, unsigned int val, unsigned int shift)
 	b[0] = cpu_to_be16(val << shift);
 }
 
+static void regmap_format_16_le(void *buf, unsigned int val, unsigned int shift)
+{
+	__le16 *b = buf;
+
+	b[0] = cpu_to_le16(val << shift);
+}
+
 static void regmap_format_16_native(void *buf, unsigned int val,
 				    unsigned int shift)
 {
@@ -216,6 +227,13 @@ static void regmap_format_32_be(void *buf, unsigned int val, unsigned int shift)
 	b[0] = cpu_to_be32(val << shift);
 }
 
+static void regmap_format_32_le(void *buf, unsigned int val, unsigned int shift)
+{
+	__le32 *b = buf;
+
+	b[0] = cpu_to_le32(val << shift);
+}
+
 static void regmap_format_32_native(void *buf, unsigned int val,
 				    unsigned int shift)
 {
@@ -240,6 +258,13 @@ static unsigned int regmap_parse_16_be(const void *buf)
 	return be16_to_cpu(b[0]);
 }
 
+static unsigned int regmap_parse_16_le(const void *buf)
+{
+	const __le16 *b = buf;
+
+	return le16_to_cpu(b[0]);
+}
+
 static void regmap_parse_16_be_inplace(void *buf)
 {
 	__be16 *b = buf;
@@ -247,6 +272,13 @@ static void regmap_parse_16_be_inplace(void *buf)
 	b[0] = be16_to_cpu(b[0]);
 }
 
+static void regmap_parse_16_le_inplace(void *buf)
+{
+	__le16 *b = buf;
+
+	b[0] = le16_to_cpu(b[0]);
+}
+
 static unsigned int regmap_parse_16_native(const void *buf)
 {
 	return *(u16 *)buf;
@@ -269,6 +301,13 @@ static unsigned int regmap_parse_32_be(const void *buf)
 	return be32_to_cpu(b[0]);
 }
 
+static unsigned int regmap_parse_32_le(const void *buf)
+{
+	const __le32 *b = buf;
+
+	return le32_to_cpu(b[0]);
+}
+
 static void regmap_parse_32_be_inplace(void *buf)
 {
 	__be32 *b = buf;
@@ -276,6 +315,13 @@ static void regmap_parse_32_be_inplace(void *buf)
 	b[0] = be32_to_cpu(b[0]);
 }
 
+static void regmap_parse_32_le_inplace(void *buf)
+{
+	__le32 *b = buf;
+
+	b[0] = le32_to_cpu(b[0]);
+}
+
 static unsigned int regmap_parse_32_native(const void *buf)
 {
 	return *(u32 *)buf;
@@ -495,6 +541,12 @@ struct regmap *regmap_init(struct device *dev,
 
 		map->defer_caching = false;
 		goto skip_format_initialization;
+	} else if (!bus->read || !bus->write) {
+		map->reg_read = _regmap_bus_reg_read;
+		map->reg_write = _regmap_bus_reg_write;
+
+		map->defer_caching = false;
+		goto skip_format_initialization;
 	} else {
 		map->reg_read  = _regmap_bus_read;
 	}
@@ -608,6 +660,11 @@ struct regmap *regmap_init(struct device *dev,
 			map->format.parse_val = regmap_parse_16_be;
 			map->format.parse_inplace = regmap_parse_16_be_inplace;
 			break;
+		case REGMAP_ENDIAN_LITTLE:
+			map->format.format_val = regmap_format_16_le;
+			map->format.parse_val = regmap_parse_16_le;
+			map->format.parse_inplace = regmap_parse_16_le_inplace;
+			break;
 		case REGMAP_ENDIAN_NATIVE:
 			map->format.format_val = regmap_format_16_native;
 			map->format.parse_val = regmap_parse_16_native;
@@ -629,6 +686,11 @@ struct regmap *regmap_init(struct device *dev,
 			map->format.parse_val = regmap_parse_32_be;
 			map->format.parse_inplace = regmap_parse_32_be_inplace;
 			break;
+		case REGMAP_ENDIAN_LITTLE:
+			map->format.format_val = regmap_format_32_le;
+			map->format.parse_val = regmap_parse_32_le;
+			map->format.parse_inplace = regmap_parse_32_le_inplace;
+			break;
 		case REGMAP_ENDIAN_NATIVE:
 			map->format.format_val = regmap_format_32_native;
 			map->format.parse_val = regmap_parse_32_native;
@@ -1284,6 +1346,14 @@ static int _regmap_bus_formatted_write(void *context, unsigned int reg,
 	return ret;
 }
 
+static int _regmap_bus_reg_write(void *context, unsigned int reg,
+				 unsigned int val)
+{
+	struct regmap *map = context;
+
+	return map->bus->reg_write(map->bus_context, reg, val);
+}
+
 static int _regmap_bus_raw_write(void *context, unsigned int reg,
 				 unsigned int val)
 {
@@ -1615,6 +1685,9 @@ static int _regmap_raw_multi_reg_write(struct regmap *map,
 	size_t pair_size = reg_bytes + pad_bytes + val_bytes;
 	size_t len = pair_size * num_regs;
 
+	if (!len)
+		return -EINVAL;
+
 	buf = kzalloc(len, GFP_KERNEL);
 	if (!buf)
 		return -ENOMEM;
@@ -1662,7 +1735,7 @@ static int _regmap_range_multi_paged_reg_write(struct regmap *map,
 	int ret;
 	int i, n;
 	struct reg_default *base;
-	unsigned int this_page;
+	unsigned int this_page = 0;
 	/*
 	 * the set of registers are not neccessarily in order, but
 	 * since the order of write must be preserved this algorithm
@@ -1925,6 +1998,14 @@ static int _regmap_raw_read(struct regmap *map, unsigned int reg, void *val,
 	return ret;
 }
 
+static int _regmap_bus_reg_read(void *context, unsigned int reg,
+				unsigned int *val)
+{
+	struct regmap *map = context;
+
+	return map->bus->reg_read(map->bus_context, reg, val);
+}
+
 static int _regmap_bus_read(void *context, unsigned int reg,
 			    unsigned int *val)
 {
diff --git a/drivers/block/amiflop.c b/drivers/block/amiflop.c
index 748dea4f34d..758da2287d9 100644
--- a/drivers/block/amiflop.c
+++ b/drivers/block/amiflop.c
@@ -1406,7 +1406,7 @@ next_segment:
 
 		track = block / (floppy->dtype->sects * floppy->type->sect_mult);
 		sector = block % (floppy->dtype->sects * floppy->type->sect_mult);
-		data = rq->buffer + 512 * cnt;
+		data = bio_data(rq->bio) + 512 * cnt;
 #ifdef DEBUG
 		printk("access to track %d, sector %d, with buffer at "
 		       "0x%08lx\n", track, sector, data);
diff --git a/drivers/block/ataflop.c b/drivers/block/ataflop.c
index 96b629e1f0c..2104b1b4ccd 100644
--- a/drivers/block/ataflop.c
+++ b/drivers/block/ataflop.c
@@ -1484,7 +1484,7 @@ repeat:
 	ReqCnt = 0;
 	ReqCmd = rq_data_dir(fd_request);
 	ReqBlock = blk_rq_pos(fd_request);
-	ReqBuffer = fd_request->buffer;
+	ReqBuffer = bio_data(fd_request->bio);
 	setup_req_params( drive );
 	do_fd_action( drive );
 
@@ -1952,7 +1952,7 @@ static int __init atari_floppy_init (void)
 		goto Enomem;
 	}
 	TrackBuffer = DMABuffer + 512;
-	PhysDMABuffer = virt_to_phys(DMABuffer);
+	PhysDMABuffer = atari_stram_to_phys(DMABuffer);
 	PhysTrackBuffer = virt_to_phys(TrackBuffer);
 	BufferDrive = BufferSide = BufferTrack = -1;
 
diff --git a/drivers/block/brd.c b/drivers/block/brd.c
index e73b85cf075..c7d138eca73 100644
--- a/drivers/block/brd.c
+++ b/drivers/block/brd.c
@@ -200,11 +200,11 @@ static int copy_to_brd_setup(struct brd_device *brd, sector_t sector, size_t n)
 
 	copy = min_t(size_t, n, PAGE_SIZE - offset);
 	if (!brd_insert_page(brd, sector))
-		return -ENOMEM;
+		return -ENOSPC;
 	if (copy < n) {
 		sector += copy >> SECTOR_SHIFT;
 		if (!brd_insert_page(brd, sector))
-			return -ENOMEM;
+			return -ENOSPC;
 	}
 	return 0;
 }
@@ -360,6 +360,15 @@ out:
 	bio_endio(bio, err);
 }
 
+static int brd_rw_page(struct block_device *bdev, sector_t sector,
+		       struct page *page, int rw)
+{
+	struct brd_device *brd = bdev->bd_disk->private_data;
+	int err = brd_do_bvec(brd, page, PAGE_CACHE_SIZE, 0, rw, sector);
+	page_endio(page, rw & WRITE, err);
+	return err;
+}
+
 #ifdef CONFIG_BLK_DEV_XIP
 static int brd_direct_access(struct block_device *bdev, sector_t sector,
 			void **kaddr, unsigned long *pfn)
@@ -375,7 +384,7 @@ static int brd_direct_access(struct block_device *bdev, sector_t sector,
 		return -ERANGE;
 	page = brd_insert_page(brd, sector);
 	if (!page)
-		return -ENOMEM;
+		return -ENOSPC;
 	*kaddr = page_address(page);
 	*pfn = page_to_pfn(page);
 
@@ -419,6 +428,7 @@ static int brd_ioctl(struct block_device *bdev, fmode_t mode,
 
 static const struct block_device_operations brd_fops = {
 	.owner =		THIS_MODULE,
+	.rw_page =		brd_rw_page,
 	.ioctl =		brd_ioctl,
 #ifdef CONFIG_BLK_DEV_XIP
 	.direct_access =	brd_direct_access,
diff --git a/drivers/block/cciss.c b/drivers/block/cciss.c
index 73894ca3395..4595c22f33f 100644
--- a/drivers/block/cciss.c
+++ b/drivers/block/cciss.c
@@ -4080,7 +4080,7 @@ static void cciss_interrupt_mode(ctlr_info_t *h)
 		goto default_int_mode;
 
 	if (pci_find_capability(h->pdev, PCI_CAP_ID_MSIX)) {
-		err = pci_enable_msix(h->pdev, cciss_msix_entries, 4);
+		err = pci_enable_msix_exact(h->pdev, cciss_msix_entries, 4);
 		if (!err) {
 			h->intr[0] = cciss_msix_entries[0].vector;
 			h->intr[1] = cciss_msix_entries[1].vector;
@@ -4088,10 +4088,6 @@ static void cciss_interrupt_mode(ctlr_info_t *h)
 			h->intr[3] = cciss_msix_entries[3].vector;
 			h->msix_vector = 1;
 			return;
-		}
-		if (err > 0) {
-			dev_warn(&h->pdev->dev,
-				"only %d MSI-X vectors available\n", err);
 		} else {
 			dev_warn(&h->pdev->dev,
 				"MSI-X init failed %d\n", err);
diff --git a/drivers/block/drbd/drbd_actlog.c b/drivers/block/drbd/drbd_actlog.c
index 90ae4ba8f9e..05a1780ffa8 100644
--- a/drivers/block/drbd/drbd_actlog.c
+++ b/drivers/block/drbd/drbd_actlog.c
@@ -29,7 +29,6 @@
 #include <linux/drbd_limits.h>
 #include <linux/dynamic_debug.h>
 #include "drbd_int.h"
-#include "drbd_wrappers.h"
 
 
 enum al_transaction_types {
@@ -204,7 +203,7 @@ int drbd_md_sync_page_io(struct drbd_device *device, struct drbd_backing_dev *bd
 
 	BUG_ON(!bdev->md_bdev);
 
-	drbd_dbg(device, "meta_data io: %s [%d]:%s(,%llus,%s) %pS\n",
+	dynamic_drbd_dbg(device, "meta_data io: %s [%d]:%s(,%llus,%s) %pS\n",
 	     current->comm, current->pid, __func__,
 	     (unsigned long long)sector, (rw & WRITE) ? "WRITE" : "READ",
 	     (void*)_RET_IP_ );
@@ -276,7 +275,6 @@ bool drbd_al_begin_io_fastpath(struct drbd_device *device, struct drbd_interval
 	return _al_get(device, first, true);
 }
 
-static
 bool drbd_al_begin_io_prepare(struct drbd_device *device, struct drbd_interval *i)
 {
 	/* for bios crossing activity log extent boundaries,
@@ -846,7 +844,7 @@ void __drbd_set_in_sync(struct drbd_device *device, sector_t sector, int size,
 	int wake_up = 0;
 	unsigned long flags;
 
-	if (size <= 0 || !IS_ALIGNED(size, 512) || size > DRBD_MAX_BIO_SIZE) {
+	if (size <= 0 || !IS_ALIGNED(size, 512) || size > DRBD_MAX_DISCARD_SIZE) {
 		drbd_err(device, "drbd_set_in_sync: sector=%llus size=%d nonsense!\n",
 				(unsigned long long)sector, size);
 		return;
@@ -920,7 +918,7 @@ int __drbd_set_out_of_sync(struct drbd_device *device, sector_t sector, int size
 	if (size == 0)
 		return 0;
 
-	if (size < 0 || !IS_ALIGNED(size, 512) || size > DRBD_MAX_BIO_SIZE) {
+	if (size < 0 || !IS_ALIGNED(size, 512) || size > DRBD_MAX_DISCARD_SIZE) {
 		drbd_err(device, "sector: %llus, size: %d\n",
 			(unsigned long long)sector, size);
 		return 0;
@@ -1023,8 +1021,7 @@ int drbd_rs_begin_io(struct drbd_device *device, sector_t sector)
 	unsigned int enr = BM_SECT_TO_EXT(sector);
 	struct bm_extent *bm_ext;
 	int i, sig;
-	int sa = 200; /* Step aside 200 times, then grab the extent and let app-IO wait.
-			 200 times -> 20 seconds. */
+	bool sa;
 
 retry:
 	sig = wait_event_interruptible(device->al_wait,
@@ -1035,12 +1032,15 @@ retry:
 	if (test_bit(BME_LOCKED, &bm_ext->flags))
 		return 0;
 
+	/* step aside only while we are above c-min-rate; unless disabled. */
+	sa = drbd_rs_c_min_rate_throttle(device);
+
 	for (i = 0; i < AL_EXT_PER_BM_SECT; i++) {
 		sig = wait_event_interruptible(device->al_wait,
 					       !_is_in_al(device, enr * AL_EXT_PER_BM_SECT + i) ||
-					       test_bit(BME_PRIORITY, &bm_ext->flags));
+					       (sa && test_bit(BME_PRIORITY, &bm_ext->flags)));
 
-		if (sig || (test_bit(BME_PRIORITY, &bm_ext->flags) && sa)) {
+		if (sig || (sa && test_bit(BME_PRIORITY, &bm_ext->flags))) {
 			spin_lock_irq(&device->al_lock);
 			if (lc_put(device->resync, &bm_ext->lce) == 0) {
 				bm_ext->flags = 0; /* clears BME_NO_WRITES and eventually BME_PRIORITY */
@@ -1052,9 +1052,6 @@ retry:
 				return -EINTR;
 			if (schedule_timeout_interruptible(HZ/10))
 				return -EINTR;
-			if (sa && --sa == 0)
-				drbd_warn(device, "drbd_rs_begin_io() stepped aside for 20sec."
-					 "Resync stalled?\n");
 			goto retry;
 		}
 	}
@@ -1288,7 +1285,7 @@ void drbd_rs_failed_io(struct drbd_device *device, sector_t sector, int size)
 	sector_t esector, nr_sectors;
 	int wake_up = 0;
 
-	if (size <= 0 || !IS_ALIGNED(size, 512) || size > DRBD_MAX_BIO_SIZE) {
+	if (size <= 0 || !IS_ALIGNED(size, 512) || size > DRBD_MAX_DISCARD_SIZE) {
 		drbd_err(device, "drbd_rs_failed_io: sector=%llus size=%d nonsense!\n",
 				(unsigned long long)sector, size);
 		return;
diff --git a/drivers/block/drbd/drbd_int.h b/drivers/block/drbd/drbd_int.h
index e7093d4291f..a76ceb344d6 100644
--- a/drivers/block/drbd/drbd_int.h
+++ b/drivers/block/drbd/drbd_int.h
@@ -382,6 +382,12 @@ enum {
 	__EE_CALL_AL_COMPLETE_IO,
 	__EE_MAY_SET_IN_SYNC,
 
+	/* is this a TRIM aka REQ_DISCARD? */
+	__EE_IS_TRIM,
+	/* our lower level cannot handle trim,
+	 * and we want to fall back to zeroout instead */
+	__EE_IS_TRIM_USE_ZEROOUT,
+
 	/* In case a barrier failed,
 	 * we need to resubmit without the barrier flag. */
 	__EE_RESUBMITTED,
@@ -405,7 +411,9 @@ enum {
 };
 #define EE_CALL_AL_COMPLETE_IO (1<<__EE_CALL_AL_COMPLETE_IO)
 #define EE_MAY_SET_IN_SYNC     (1<<__EE_MAY_SET_IN_SYNC)
-#define	EE_RESUBMITTED         (1<<__EE_RESUBMITTED)
+#define EE_IS_TRIM             (1<<__EE_IS_TRIM)
+#define EE_IS_TRIM_USE_ZEROOUT (1<<__EE_IS_TRIM_USE_ZEROOUT)
+#define EE_RESUBMITTED         (1<<__EE_RESUBMITTED)
 #define EE_WAS_ERROR           (1<<__EE_WAS_ERROR)
 #define EE_HAS_DIGEST          (1<<__EE_HAS_DIGEST)
 #define EE_RESTART_REQUESTS	(1<<__EE_RESTART_REQUESTS)
@@ -579,6 +587,7 @@ struct drbd_resource {
 	struct list_head resources;
 	struct res_opts res_opts;
 	struct mutex conf_update;	/* mutex for ready-copy-update of net_conf and disk_conf */
+	struct mutex adm_mutex;		/* mutex to serialize administrative requests */
 	spinlock_t req_lock;
 
 	unsigned susp:1;		/* IO suspended by user */
@@ -609,6 +618,7 @@ struct drbd_connection {
 	struct drbd_socket data;	/* data/barrier/cstate/parameter packets */
 	struct drbd_socket meta;	/* ping/ack (metadata) packets */
 	int agreed_pro_version;		/* actually used protocol version */
+	u32 agreed_features;
 	unsigned long last_received;	/* in jiffies, either socket */
 	unsigned int ko_count;
 
@@ -814,6 +824,28 @@ struct drbd_device {
 	struct submit_worker submit;
 };
 
+struct drbd_config_context {
+	/* assigned from drbd_genlmsghdr */
+	unsigned int minor;
+	/* assigned from request attributes, if present */
+	unsigned int volume;
+#define VOLUME_UNSPECIFIED		(-1U)
+	/* pointer into the request skb,
+	 * limited lifetime! */
+	char *resource_name;
+	struct nlattr *my_addr;
+	struct nlattr *peer_addr;
+
+	/* reply buffer */
+	struct sk_buff *reply_skb;
+	/* pointer into reply buffer */
+	struct drbd_genlmsghdr *reply_dh;
+	/* resolved from attributes, if possible */
+	struct drbd_device *device;
+	struct drbd_resource *resource;
+	struct drbd_connection *connection;
+};
+
 static inline struct drbd_device *minor_to_device(unsigned int minor)
 {
 	return (struct drbd_device *)idr_find(&drbd_devices, minor);
@@ -821,7 +853,7 @@ static inline struct drbd_device *minor_to_device(unsigned int minor)
 
 static inline struct drbd_peer_device *first_peer_device(struct drbd_device *device)
 {
-	return list_first_entry(&device->peer_devices, struct drbd_peer_device, peer_devices);
+	return list_first_entry_or_null(&device->peer_devices, struct drbd_peer_device, peer_devices);
 }
 
 #define for_each_resource(resource, _resources) \
@@ -1139,6 +1171,12 @@ struct bm_extent {
 #define DRBD_MAX_SIZE_H80_PACKET (1U << 15) /* Header 80 only allows packets up to 32KiB data */
 #define DRBD_MAX_BIO_SIZE_P95    (1U << 17) /* Protocol 95 to 99 allows bios up to 128KiB */
 
+/* For now, don't allow more than one activity log extent worth of data
+ * to be discarded in one go. We may need to rework drbd_al_begin_io()
+ * to allow for even larger discard ranges */
+#define DRBD_MAX_DISCARD_SIZE	AL_EXTENT_SIZE
+#define DRBD_MAX_DISCARD_SECTORS (DRBD_MAX_DISCARD_SIZE >> 9)
+
 extern int  drbd_bm_init(struct drbd_device *device);
 extern int  drbd_bm_resize(struct drbd_device *device, sector_t sectors, int set_new_bits);
 extern void drbd_bm_cleanup(struct drbd_device *device);
@@ -1229,9 +1267,9 @@ extern struct bio *bio_alloc_drbd(gfp_t gfp_mask);
 extern rwlock_t global_state_lock;
 
 extern int conn_lowest_minor(struct drbd_connection *connection);
-enum drbd_ret_code drbd_create_device(struct drbd_resource *resource, unsigned int minor, int vnr);
+extern enum drbd_ret_code drbd_create_device(struct drbd_config_context *adm_ctx, unsigned int minor);
 extern void drbd_destroy_device(struct kref *kref);
-extern void drbd_delete_device(struct drbd_device *mdev);
+extern void drbd_delete_device(struct drbd_device *device);
 
 extern struct drbd_resource *drbd_create_resource(const char *name);
 extern void drbd_free_resource(struct drbd_resource *resource);
@@ -1257,7 +1295,7 @@ extern int is_valid_ar_handle(struct drbd_request *, sector_t);
 
 
 /* drbd_nl.c */
-extern int drbd_msg_put_info(const char *info);
+extern int drbd_msg_put_info(struct sk_buff *skb, const char *info);
 extern void drbd_suspend_io(struct drbd_device *device);
 extern void drbd_resume_io(struct drbd_device *device);
 extern char *ppsize(char *buf, unsigned long long size);
@@ -1283,6 +1321,10 @@ extern void conn_try_outdate_peer_async(struct drbd_connection *connection);
 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_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);
 enum drbd_ret_code drbd_resync_after_valid(struct drbd_device *device, int o_minor);
 void drbd_resync_after_changed(struct drbd_device *device);
@@ -1332,16 +1374,20 @@ extern int w_start_resync(struct drbd_work *, int);
 extern void resync_timer_fn(unsigned long data);
 extern void start_resync_timer_fn(unsigned long data);
 
+extern void drbd_endio_write_sec_final(struct drbd_peer_request *peer_req);
+
 /* drbd_receiver.c */
 extern int drbd_receiver(struct drbd_thread *thi);
 extern int drbd_asender(struct drbd_thread *thi);
-extern int drbd_rs_should_slow_down(struct drbd_device *device, sector_t sector);
+extern bool drbd_rs_c_min_rate_throttle(struct drbd_device *device);
+extern bool drbd_rs_should_slow_down(struct drbd_device *device, sector_t sector);
 extern int drbd_submit_peer_request(struct drbd_device *,
 				    struct drbd_peer_request *, const unsigned,
 				    const int);
 extern int drbd_free_peer_reqs(struct drbd_device *, struct list_head *);
 extern struct drbd_peer_request *drbd_alloc_peer_req(struct drbd_peer_device *, u64,
 						     sector_t, unsigned int,
+						     bool,
 						     gfp_t) __must_hold(local);
 extern void __drbd_free_peer_req(struct drbd_device *, struct drbd_peer_request *,
 				 int);
@@ -1401,6 +1447,37 @@ static inline void drbd_tcp_quickack(struct socket *sock)
 			(char*)&val, sizeof(val));
 }
 
+/* sets the number of 512 byte sectors of our virtual device */
+static inline void drbd_set_my_capacity(struct drbd_device *device,
+					sector_t size)
+{
+	/* set_capacity(device->this_bdev->bd_disk, size); */
+	set_capacity(device->vdisk, size);
+	device->this_bdev->bd_inode->i_size = (loff_t)size << 9;
+}
+
+/*
+ * used to submit our private bio
+ */
+static inline void drbd_generic_make_request(struct drbd_device *device,
+					     int fault_type, struct bio *bio)
+{
+	__release(local);
+	if (!bio->bi_bdev) {
+		printk(KERN_ERR "drbd%d: drbd_generic_make_request: "
+				"bio->bi_bdev == NULL\n",
+		       device_to_minor(device));
+		dump_stack();
+		bio_endio(bio, -ENODEV);
+		return;
+	}
+
+	if (drbd_insert_fault(device, fault_type))
+		bio_endio(bio, -EIO);
+	else
+		generic_make_request(bio);
+}
+
 void drbd_bump_write_ordering(struct drbd_connection *connection, enum write_ordering_e wo);
 
 /* drbd_proc.c */
@@ -1410,6 +1487,7 @@ extern const char *drbd_conn_str(enum drbd_conns s);
 extern const char *drbd_role_str(enum drbd_role s);
 
 /* drbd_actlog.c */
+extern bool drbd_al_begin_io_prepare(struct drbd_device *device, struct drbd_interval *i);
 extern int drbd_al_begin_io_nonblock(struct drbd_device *device, struct drbd_interval *i);
 extern void drbd_al_begin_io_commit(struct drbd_device *device, bool delegate);
 extern bool drbd_al_begin_io_fastpath(struct drbd_device *device, struct drbd_interval *i);
@@ -2144,7 +2222,7 @@ static inline void drbd_md_flush(struct drbd_device *device)
 
 static inline struct drbd_connection *first_connection(struct drbd_resource *resource)
 {
-	return list_first_entry(&resource->connections,
+	return list_first_entry_or_null(&resource->connections,
 				struct drbd_connection, connections);
 }
 
diff --git a/drivers/block/drbd/drbd_main.c b/drivers/block/drbd/drbd_main.c
index 331e5cc1227..960645c26e6 100644
--- a/drivers/block/drbd/drbd_main.c
+++ b/drivers/block/drbd/drbd_main.c
@@ -1607,8 +1607,8 @@ static u32 bio_flags_to_wire(struct drbd_connection *connection, unsigned long b
 		return bi_rw & REQ_SYNC ? DP_RW_SYNC : 0;
 }
 
-/* Used to send write requests
- * R_PRIMARY -> Peer	(P_DATA)
+/* Used to send write or TRIM aka REQ_DISCARD requests
+ * R_PRIMARY -> Peer	(P_DATA, P_TRIM)
  */
 int drbd_send_dblock(struct drbd_peer_device *peer_device, struct drbd_request *req)
 {
@@ -1640,6 +1640,16 @@ int drbd_send_dblock(struct drbd_peer_device *peer_device, struct drbd_request *
 			dp_flags |= DP_SEND_WRITE_ACK;
 	}
 	p->dp_flags = cpu_to_be32(dp_flags);
+
+	if (dp_flags & DP_DISCARD) {
+		struct p_trim *t = (struct p_trim*)p;
+		t->size = cpu_to_be32(req->i.size);
+		err = __send_command(peer_device->connection, device->vnr, sock, P_TRIM, sizeof(*t), NULL, 0);
+		goto out;
+	}
+
+	/* our digest is still only over the payload.
+	 * TRIM does not carry any payload. */
 	if (dgs)
 		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);
@@ -1675,6 +1685,7 @@ int drbd_send_dblock(struct drbd_peer_device *peer_device, struct drbd_request *
 		     ... Be noisy about digest too large ...
 		} */
 	}
+out:
 	mutex_unlock(&sock->mutex);  /* locked by drbd_prepare_command() */
 
 	return err;
@@ -2570,6 +2581,7 @@ struct drbd_resource *drbd_create_resource(const char *name)
 	INIT_LIST_HEAD(&resource->connections);
 	list_add_tail_rcu(&resource->resources, &drbd_resources);
 	mutex_init(&resource->conf_update);
+	mutex_init(&resource->adm_mutex);
 	spin_lock_init(&resource->req_lock);
 	return resource;
 
@@ -2687,14 +2699,16 @@ static int init_submitter(struct drbd_device *device)
 	return 0;
 }
 
-enum drbd_ret_code drbd_create_device(struct drbd_resource *resource, unsigned int minor, int vnr)
+enum drbd_ret_code drbd_create_device(struct drbd_config_context *adm_ctx, unsigned int minor)
 {
+	struct drbd_resource *resource = adm_ctx->resource;
 	struct drbd_connection *connection;
 	struct drbd_device *device;
 	struct drbd_peer_device *peer_device, *tmp_peer_device;
 	struct gendisk *disk;
 	struct request_queue *q;
 	int id;
+	int vnr = adm_ctx->volume;
 	enum drbd_ret_code err = ERR_NOMEM;
 
 	device = minor_to_device(minor);
@@ -2763,7 +2777,7 @@ enum drbd_ret_code drbd_create_device(struct drbd_resource *resource, unsigned i
 	if (id < 0) {
 		if (id == -ENOSPC) {
 			err = ERR_MINOR_EXISTS;
-			drbd_msg_put_info("requested minor exists already");
+			drbd_msg_put_info(adm_ctx->reply_skb, "requested minor exists already");
 		}
 		goto out_no_minor_idr;
 	}
@@ -2773,7 +2787,7 @@ enum drbd_ret_code drbd_create_device(struct drbd_resource *resource, unsigned i
 	if (id < 0) {
 		if (id == -ENOSPC) {
 			err = ERR_MINOR_EXISTS;
-			drbd_msg_put_info("requested minor exists already");
+			drbd_msg_put_info(adm_ctx->reply_skb, "requested minor exists already");
 		}
 		goto out_idr_remove_minor;
 	}
@@ -2794,7 +2808,7 @@ enum drbd_ret_code drbd_create_device(struct drbd_resource *resource, unsigned i
 		if (id < 0) {
 			if (id == -ENOSPC) {
 				err = ERR_INVALID_REQUEST;
-				drbd_msg_put_info("requested volume exists already");
+				drbd_msg_put_info(adm_ctx->reply_skb, "requested volume exists already");
 			}
 			goto out_idr_remove_from_resource;
 		}
@@ -2803,7 +2817,7 @@ enum drbd_ret_code drbd_create_device(struct drbd_resource *resource, unsigned i
 
 	if (init_submitter(device)) {
 		err = ERR_NOMEM;
-		drbd_msg_put_info("unable to create submit workqueue");
+		drbd_msg_put_info(adm_ctx->reply_skb, "unable to create submit workqueue");
 		goto out_idr_remove_vol;
 	}
 
diff --git a/drivers/block/drbd/drbd_nl.c b/drivers/block/drbd/drbd_nl.c
index 526414bc2ca..1b35c45c92b 100644
--- a/drivers/block/drbd/drbd_nl.c
+++ b/drivers/block/drbd/drbd_nl.c
@@ -34,7 +34,6 @@
 #include "drbd_int.h"
 #include "drbd_protocol.h"
 #include "drbd_req.h"
-#include "drbd_wrappers.h"
 #include <asm/unaligned.h>
 #include <linux/drbd_limits.h>
 #include <linux/kthread.h>
@@ -82,32 +81,6 @@ int drbd_adm_get_status_all(struct sk_buff *skb, struct netlink_callback *cb);
 /* used blkdev_get_by_path, to claim our meta data device(s) */
 static char *drbd_m_holder = "Hands off! this is DRBD's meta data device.";
 
-/* Configuration is strictly serialized, because generic netlink message
- * processing is strictly serialized by the genl_lock().
- * Which means we can use one static global drbd_config_context struct.
- */
-static struct drbd_config_context {
-	/* assigned from drbd_genlmsghdr */
-	unsigned int minor;
-	/* assigned from request attributes, if present */
-	unsigned int volume;
-#define VOLUME_UNSPECIFIED		(-1U)
-	/* pointer into the request skb,
-	 * limited lifetime! */
-	char *resource_name;
-	struct nlattr *my_addr;
-	struct nlattr *peer_addr;
-
-	/* reply buffer */
-	struct sk_buff *reply_skb;
-	/* pointer into reply buffer */
-	struct drbd_genlmsghdr *reply_dh;
-	/* resolved from attributes, if possible */
-	struct drbd_device *device;
-	struct drbd_resource *resource;
-	struct drbd_connection *connection;
-} adm_ctx;
-
 static void drbd_adm_send_reply(struct sk_buff *skb, struct genl_info *info)
 {
 	genlmsg_end(skb, genlmsg_data(nlmsg_data(nlmsg_hdr(skb))));
@@ -117,9 +90,8 @@ static void drbd_adm_send_reply(struct sk_buff *skb, struct genl_info *info)
 
 /* Used on a fresh "drbd_adm_prepare"d reply_skb, this cannot fail: The only
  * reason it could fail was no space in skb, and there are 4k available. */
-int drbd_msg_put_info(const char *info)
+int drbd_msg_put_info(struct sk_buff *skb, const char *info)
 {
-	struct sk_buff *skb = adm_ctx.reply_skb;
 	struct nlattr *nla;
 	int err = -EMSGSIZE;
 
@@ -143,42 +115,46 @@ int drbd_msg_put_info(const char *info)
  * and per-family private info->pointers.
  * But we need to stay compatible with older kernels.
  * If it returns successfully, adm_ctx members are valid.
+ *
+ * At this point, we still rely on the global genl_lock().
+ * If we want to avoid that, and allow "genl_family.parallel_ops", we may need
+ * to add additional synchronization against object destruction/modification.
  */
 #define DRBD_ADM_NEED_MINOR	1
 #define DRBD_ADM_NEED_RESOURCE	2
 #define DRBD_ADM_NEED_CONNECTION 4
-static int drbd_adm_prepare(struct sk_buff *skb, struct genl_info *info,
-		unsigned flags)
+static int drbd_adm_prepare(struct drbd_config_context *adm_ctx,
+	struct sk_buff *skb, struct genl_info *info, unsigned flags)
 {
 	struct drbd_genlmsghdr *d_in = info->userhdr;
 	const u8 cmd = info->genlhdr->cmd;
 	int err;
 
-	memset(&adm_ctx, 0, sizeof(adm_ctx));
+	memset(adm_ctx, 0, sizeof(*adm_ctx));
 
 	/* genl_rcv_msg only checks for CAP_NET_ADMIN on "GENL_ADMIN_PERM" :( */
 	if (cmd != DRBD_ADM_GET_STATUS && !capable(CAP_NET_ADMIN))
 	       return -EPERM;
 
-	adm_ctx.reply_skb = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
-	if (!adm_ctx.reply_skb) {
+	adm_ctx->reply_skb = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
+	if (!adm_ctx->reply_skb) {
 		err = -ENOMEM;
 		goto fail;
 	}
 
-	adm_ctx.reply_dh = genlmsg_put_reply(adm_ctx.reply_skb,
+	adm_ctx->reply_dh = genlmsg_put_reply(adm_ctx->reply_skb,
 					info, &drbd_genl_family, 0, cmd);
 	/* put of a few bytes into a fresh skb of >= 4k will always succeed.
 	 * but anyways */
-	if (!adm_ctx.reply_dh) {
+	if (!adm_ctx->reply_dh) {
 		err = -ENOMEM;
 		goto fail;
 	}
 
-	adm_ctx.reply_dh->minor = d_in->minor;
-	adm_ctx.reply_dh->ret_code = NO_ERROR;
+	adm_ctx->reply_dh->minor = d_in->minor;
+	adm_ctx->reply_dh->ret_code = NO_ERROR;
 
-	adm_ctx.volume = VOLUME_UNSPECIFIED;
+	adm_ctx->volume = VOLUME_UNSPECIFIED;
 	if (info->attrs[DRBD_NLA_CFG_CONTEXT]) {
 		struct nlattr *nla;
 		/* parse and validate only */
@@ -188,111 +164,131 @@ static int drbd_adm_prepare(struct sk_buff *skb, struct genl_info *info,
 
 		/* It was present, and valid,
 		 * copy it over to the reply skb. */
-		err = nla_put_nohdr(adm_ctx.reply_skb,
+		err = nla_put_nohdr(adm_ctx->reply_skb,
 				info->attrs[DRBD_NLA_CFG_CONTEXT]->nla_len,
 				info->attrs[DRBD_NLA_CFG_CONTEXT]);
 		if (err)
 			goto fail;
 
-		/* and assign stuff to the global adm_ctx */
+		/* and assign stuff to the adm_ctx */
 		nla = nested_attr_tb[__nla_type(T_ctx_volume)];
 		if (nla)
-			adm_ctx.volume = nla_get_u32(nla);
+			adm_ctx->volume = nla_get_u32(nla);
 		nla = nested_attr_tb[__nla_type(T_ctx_resource_name)];
 		if (nla)
-			adm_ctx.resource_name = nla_data(nla);
-		adm_ctx.my_addr = nested_attr_tb[__nla_type(T_ctx_my_addr)];
-		adm_ctx.peer_addr = nested_attr_tb[__nla_type(T_ctx_peer_addr)];
-		if ((adm_ctx.my_addr &&
-		     nla_len(adm_ctx.my_addr) > sizeof(adm_ctx.connection->my_addr)) ||
-		    (adm_ctx.peer_addr &&
-		     nla_len(adm_ctx.peer_addr) > sizeof(adm_ctx.connection->peer_addr))) {
+			adm_ctx->resource_name = nla_data(nla);
+		adm_ctx->my_addr = nested_attr_tb[__nla_type(T_ctx_my_addr)];
+		adm_ctx->peer_addr = nested_attr_tb[__nla_type(T_ctx_peer_addr)];
+		if ((adm_ctx->my_addr &&
+		     nla_len(adm_ctx->my_addr) > sizeof(adm_ctx->connection->my_addr)) ||
+		    (adm_ctx->peer_addr &&
+		     nla_len(adm_ctx->peer_addr) > sizeof(adm_ctx->connection->peer_addr))) {
 			err = -EINVAL;
 			goto fail;
 		}
 	}
 
-	adm_ctx.minor = d_in->minor;
-	adm_ctx.device = minor_to_device(d_in->minor);
-	if (adm_ctx.resource_name) {
-		adm_ctx.resource = drbd_find_resource(adm_ctx.resource_name);
+	adm_ctx->minor = d_in->minor;
+	adm_ctx->device = minor_to_device(d_in->minor);
+
+	/* We are protected by the global genl_lock().
+	 * But we may explicitly drop it/retake it in drbd_adm_set_role(),
+	 * so make sure this object stays around. */
+	if (adm_ctx->device)
+		kref_get(&adm_ctx->device->kref);
+
+	if (adm_ctx->resource_name) {
+		adm_ctx->resource = drbd_find_resource(adm_ctx->resource_name);
 	}
 
-	if (!adm_ctx.device && (flags & DRBD_ADM_NEED_MINOR)) {
-		drbd_msg_put_info("unknown minor");
+	if (!adm_ctx->device && (flags & DRBD_ADM_NEED_MINOR)) {
+		drbd_msg_put_info(adm_ctx->reply_skb, "unknown minor");
 		return ERR_MINOR_INVALID;
 	}
-	if (!adm_ctx.resource && (flags & DRBD_ADM_NEED_RESOURCE)) {
-		drbd_msg_put_info("unknown resource");
-		if (adm_ctx.resource_name)
+	if (!adm_ctx->resource && (flags & DRBD_ADM_NEED_RESOURCE)) {
+		drbd_msg_put_info(adm_ctx->reply_skb, "unknown resource");
+		if (adm_ctx->resource_name)
 			return ERR_RES_NOT_KNOWN;
 		return ERR_INVALID_REQUEST;
 	}
 
 	if (flags & DRBD_ADM_NEED_CONNECTION) {
-		if (adm_ctx.resource) {
-			drbd_msg_put_info("no resource name expected");
+		if (adm_ctx->resource) {
+			drbd_msg_put_info(adm_ctx->reply_skb, "no resource name expected");
 			return ERR_INVALID_REQUEST;
 		}
-		if (adm_ctx.device) {
-			drbd_msg_put_info("no minor number expected");
+		if (adm_ctx->device) {
+			drbd_msg_put_info(adm_ctx->reply_skb, "no minor number expected");
 			return ERR_INVALID_REQUEST;
 		}
-		if (adm_ctx.my_addr && adm_ctx.peer_addr)
-			adm_ctx.connection = conn_get_by_addrs(nla_data(adm_ctx.my_addr),
-							  nla_len(adm_ctx.my_addr),
-							  nla_data(adm_ctx.peer_addr),
-							  nla_len(adm_ctx.peer_addr));
-		if (!adm_ctx.connection) {
-			drbd_msg_put_info("unknown connection");
+		if (adm_ctx->my_addr && adm_ctx->peer_addr)
+			adm_ctx->connection = conn_get_by_addrs(nla_data(adm_ctx->my_addr),
+							  nla_len(adm_ctx->my_addr),
+							  nla_data(adm_ctx->peer_addr),
+							  nla_len(adm_ctx->peer_addr));
+		if (!adm_ctx->connection) {
+			drbd_msg_put_info(adm_ctx->reply_skb, "unknown connection");
 			return ERR_INVALID_REQUEST;
 		}
 	}
 
 	/* some more paranoia, if the request was over-determined */
-	if (adm_ctx.device && adm_ctx.resource &&
-	    adm_ctx.device->resource != adm_ctx.resource) {
+	if (adm_ctx->device && adm_ctx->resource &&
+	    adm_ctx->device->resource != adm_ctx->resource) {
 		pr_warning("request: minor=%u, resource=%s; but that minor belongs to resource %s\n",
-				adm_ctx.minor, adm_ctx.resource->name,
-				adm_ctx.device->resource->name);
-		drbd_msg_put_info("minor exists in different resource");
+				adm_ctx->minor, adm_ctx->resource->name,
+				adm_ctx->device->resource->name);
+		drbd_msg_put_info(adm_ctx->reply_skb, "minor exists in different resource");
 		return ERR_INVALID_REQUEST;
 	}
-	if (adm_ctx.device &&
-	    adm_ctx.volume != VOLUME_UNSPECIFIED &&
-	    adm_ctx.volume != adm_ctx.device->vnr) {
+	if (adm_ctx->device &&
+	    adm_ctx->volume != VOLUME_UNSPECIFIED &&
+	    adm_ctx->volume != adm_ctx->device->vnr) {
 		pr_warning("request: minor=%u, volume=%u; but that minor is volume %u in %s\n",
-				adm_ctx.minor, adm_ctx.volume,
-				adm_ctx.device->vnr,
-				adm_ctx.device->resource->name);
-		drbd_msg_put_info("minor exists as different volume");
+				adm_ctx->minor, adm_ctx->volume,
+				adm_ctx->device->vnr,
+				adm_ctx->device->resource->name);
+		drbd_msg_put_info(adm_ctx->reply_skb, "minor exists as different volume");
 		return ERR_INVALID_REQUEST;
 	}
 
+	/* still, provide adm_ctx->resource always, if possible. */
+	if (!adm_ctx->resource) {
+		adm_ctx->resource = adm_ctx->device ? adm_ctx->device->resource
+			: adm_ctx->connection ? adm_ctx->connection->resource : NULL;
+		if (adm_ctx->resource)
+			kref_get(&adm_ctx->resource->kref);
+	}
+
 	return NO_ERROR;
 
 fail:
-	nlmsg_free(adm_ctx.reply_skb);
-	adm_ctx.reply_skb = NULL;
+	nlmsg_free(adm_ctx->reply_skb);
+	adm_ctx->reply_skb = NULL;
 	return err;
 }
 
-static int drbd_adm_finish(struct genl_info *info, int retcode)
+static int drbd_adm_finish(struct drbd_config_context *adm_ctx,
+	struct genl_info *info, int retcode)
 {
-	if (adm_ctx.connection) {
-		kref_put(&adm_ctx.connection->kref, drbd_destroy_connection);
-		adm_ctx.connection = NULL;
+	if (adm_ctx->device) {
+		kref_put(&adm_ctx->device->kref, drbd_destroy_device);
+		adm_ctx->device = NULL;
 	}
-	if (adm_ctx.resource) {
-		kref_put(&adm_ctx.resource->kref, drbd_destroy_resource);
-		adm_ctx.resource = NULL;
+	if (adm_ctx->connection) {
+		kref_put(&adm_ctx->connection->kref, &drbd_destroy_connection);
+		adm_ctx->connection = NULL;
+	}
+	if (adm_ctx->resource) {
+		kref_put(&adm_ctx->resource->kref, drbd_destroy_resource);
+		adm_ctx->resource = NULL;
 	}
 
-	if (!adm_ctx.reply_skb)
+	if (!adm_ctx->reply_skb)
 		return -ENOMEM;
 
-	adm_ctx.reply_dh->ret_code = retcode;
-	drbd_adm_send_reply(adm_ctx.reply_skb, info);
+	adm_ctx->reply_dh->ret_code = retcode;
+	drbd_adm_send_reply(adm_ctx->reply_skb, info);
 	return 0;
 }
 
@@ -426,6 +422,14 @@ static enum drbd_fencing_p highest_fencing_policy(struct drbd_connection *connec
 	}
 	rcu_read_unlock();
 
+	if (fp == FP_NOT_AVAIL) {
+		/* IO Suspending works on the whole resource.
+		   Do it only for one device. */
+		vnr = 0;
+		peer_device = idr_get_next(&connection->peer_devices, &vnr);
+		drbd_change_state(peer_device->device, CS_VERBOSE | CS_HARD, NS(susp_fen, 0));
+	}
+
 	return fp;
 }
 
@@ -438,12 +442,13 @@ bool conn_try_outdate_peer(struct drbd_connection *connection)
 	char *ex_to_string;
 	int r;
 
+	spin_lock_irq(&connection->resource->req_lock);
 	if (connection->cstate >= C_WF_REPORT_PARAMS) {
 		drbd_err(connection, "Expected cstate < C_WF_REPORT_PARAMS\n");
+		spin_unlock_irq(&connection->resource->req_lock);
 		return false;
 	}
 
-	spin_lock_irq(&connection->resource->req_lock);
 	connect_cnt = connection->connect_cnt;
 	spin_unlock_irq(&connection->resource->req_lock);
 
@@ -654,11 +659,11 @@ drbd_set_role(struct drbd_device *device, enum drbd_role new_role, int force)
 			put_ldev(device);
 		}
 	} else {
-		mutex_lock(&device->resource->conf_update);
+		/* Called from drbd_adm_set_role only.
+		 * We are still holding the conf_update mutex. */
 		nc = first_peer_device(device)->connection->net_conf;
 		if (nc)
 			nc->discard_my_data = 0; /* without copy; single bit op is atomic */
-		mutex_unlock(&device->resource->conf_update);
 
 		set_disk_ro(device->vdisk, false);
 		if (get_ldev(device)) {
@@ -700,11 +705,12 @@ static const char *from_attrs_err_to_txt(int err)
 
 int drbd_adm_set_role(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct set_role_parms parms;
 	int err;
 	enum drbd_ret_code retcode;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
@@ -715,17 +721,22 @@ int drbd_adm_set_role(struct sk_buff *skb, struct genl_info *info)
 		err = set_role_parms_from_attrs(&parms, info);
 		if (err) {
 			retcode = ERR_MANDATORY_TAG;
-			drbd_msg_put_info(from_attrs_err_to_txt(err));
+			drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 			goto out;
 		}
 	}
+	genl_unlock();
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 
 	if (info->genlhdr->cmd == DRBD_ADM_PRIMARY)
 		retcode = drbd_set_role(adm_ctx.device, R_PRIMARY, parms.assume_uptodate);
 	else
 		retcode = drbd_set_role(adm_ctx.device, R_SECONDARY, 0);
+
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
+	genl_lock();
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
@@ -1104,15 +1115,18 @@ static void drbd_setup_queue_param(struct drbd_device *device, unsigned int max_
 	struct request_queue * const q = device->rq_queue;
 	unsigned int max_hw_sectors = max_bio_size >> 9;
 	unsigned int max_segments = 0;
+	struct request_queue *b = NULL;
 
 	if (get_ldev_if_state(device, D_ATTACHING)) {
-		struct request_queue * const b = device->ldev->backing_bdev->bd_disk->queue;
+		b = device->ldev->backing_bdev->bd_disk->queue;
 
 		max_hw_sectors = min(queue_max_hw_sectors(b), max_bio_size >> 9);
 		rcu_read_lock();
 		max_segments = rcu_dereference(device->ldev->disk_conf)->max_bio_bvecs;
 		rcu_read_unlock();
-		put_ldev(device);
+
+		blk_set_stacking_limits(&q->limits);
+		blk_queue_max_write_same_sectors(q, 0);
 	}
 
 	blk_queue_logical_block_size(q, 512);
@@ -1121,8 +1135,25 @@ static void drbd_setup_queue_param(struct drbd_device *device, unsigned int max_
 	blk_queue_max_segments(q, max_segments ? max_segments : BLK_MAX_SEGMENTS);
 	blk_queue_segment_boundary(q, PAGE_CACHE_SIZE-1);
 
-	if (get_ldev_if_state(device, D_ATTACHING)) {
-		struct request_queue * const b = device->ldev->backing_bdev->bd_disk->queue;
+	if (b) {
+		struct drbd_connection *connection = first_peer_device(device)->connection;
+
+		if (blk_queue_discard(b) &&
+		    (connection->cstate < C_CONNECTED || connection->agreed_features & FF_TRIM)) {
+			/* For now, don't allow more than one activity log extent worth of data
+			 * to be discarded in one go. We may need to rework drbd_al_begin_io()
+			 * to allow for even larger discard ranges */
+			q->limits.max_discard_sectors = DRBD_MAX_DISCARD_SECTORS;
+
+			queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, q);
+			/* REALLY? Is stacking secdiscard "legal"? */
+			if (blk_queue_secdiscard(b))
+				queue_flag_set_unlocked(QUEUE_FLAG_SECDISCARD, q);
+		} else {
+			q->limits.max_discard_sectors = 0;
+			queue_flag_clear_unlocked(QUEUE_FLAG_DISCARD, q);
+			queue_flag_clear_unlocked(QUEUE_FLAG_SECDISCARD, q);
+		}
 
 		blk_queue_stack_limits(q, b);
 
@@ -1164,8 +1195,14 @@ void drbd_reconsider_max_bio_size(struct drbd_device *device)
 			peer = DRBD_MAX_BIO_SIZE_P95;  /* drbd 8.3.8 onwards, before 8.4.0 */
 		else
 			peer = DRBD_MAX_BIO_SIZE;
-	}
 
+		/* We may later detach and re-attach on a disconnected Primary.
+		 * Avoid this setting to jump back in that case.
+		 * We want to store what we know the peer DRBD can handle,
+		 * not what the peer IO backend can handle. */
+		if (peer > device->peer_max_bio_size)
+			device->peer_max_bio_size = peer;
+	}
 	new = min(local, peer);
 
 	if (device->state.role == R_PRIMARY && new < now)
@@ -1258,19 +1295,21 @@ static unsigned int drbd_al_extents_max(struct drbd_backing_dev *bdev)
 
 int drbd_adm_disk_opts(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	enum drbd_ret_code retcode;
 	struct drbd_device *device;
 	struct disk_conf *new_disk_conf, *old_disk_conf;
 	struct fifo_buffer *old_plan = NULL, *new_plan = NULL;
 	int err, fifo_size;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
-		goto out;
+		goto finish;
 
 	device = adm_ctx.device;
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 
 	/* we also need a disk
 	 * to change the options on */
@@ -1294,7 +1333,7 @@ int drbd_adm_disk_opts(struct sk_buff *skb, struct genl_info *info)
 	err = disk_conf_from_attrs_for_change(new_disk_conf, info);
 	if (err && err != -ENOMSG) {
 		retcode = ERR_MANDATORY_TAG;
-		drbd_msg_put_info(from_attrs_err_to_txt(err));
+		drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 		goto fail_unlock;
 	}
 
@@ -1385,12 +1424,15 @@ fail_unlock:
 success:
 	put_ldev(device);
  out:
-	drbd_adm_finish(info, retcode);
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
+ finish:
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct drbd_device *device;
 	int err;
 	enum drbd_ret_code retcode;
@@ -1406,13 +1448,14 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 	enum drbd_state_rv rv;
 	struct net_conf *nc;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
 		goto finish;
 
 	device = adm_ctx.device;
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	conn_reconfig_start(first_peer_device(device)->connection);
 
 	/* if you want to reconfigure, please tear down first */
@@ -1455,7 +1498,7 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 	err = disk_conf_from_attrs(new_disk_conf, info);
 	if (err) {
 		retcode = ERR_MANDATORY_TAG;
-		drbd_msg_put_info(from_attrs_err_to_txt(err));
+		drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 		goto fail;
 	}
 
@@ -1619,7 +1662,7 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 	}
 
 	if (device->state.conn < C_CONNECTED &&
-	    device->state.role == R_PRIMARY &&
+	    device->state.role == R_PRIMARY && device->ed_uuid &&
 	    (device->ed_uuid & ~((u64)1)) != (nbc->md.uuid[UI_CURRENT] & ~((u64)1))) {
 		drbd_err(device, "Can only attach to data with current UUID=%016llX\n",
 		    (unsigned long long)device->ed_uuid);
@@ -1797,7 +1840,8 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 	kobject_uevent(&disk_to_dev(device->vdisk)->kobj, KOBJ_CHANGE);
 	put_ldev(device);
 	conn_reconfig_done(first_peer_device(device)->connection);
-	drbd_adm_finish(info, retcode);
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 
  force_diskless_dec:
@@ -1819,9 +1863,9 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 	kfree(new_disk_conf);
 	lc_destroy(resync_lru);
 	kfree(new_plan);
-
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
  finish:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
@@ -1860,11 +1904,12 @@ out:
  * Only then we have finally detached. */
 int drbd_adm_detach(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	enum drbd_ret_code retcode;
 	struct detach_parms parms = { };
 	int err;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
@@ -1874,14 +1919,16 @@ int drbd_adm_detach(struct sk_buff *skb, struct genl_info *info)
 		err = detach_parms_from_attrs(&parms, info);
 		if (err) {
 			retcode = ERR_MANDATORY_TAG;
-			drbd_msg_put_info(from_attrs_err_to_txt(err));
+			drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 			goto out;
 		}
 	}
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	retcode = adm_detach(adm_ctx.device, parms.force_detach);
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
@@ -2055,6 +2102,7 @@ static void free_crypto(struct crypto *crypto)
 
 int drbd_adm_net_opts(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	enum drbd_ret_code retcode;
 	struct drbd_connection *connection;
 	struct net_conf *old_net_conf, *new_net_conf = NULL;
@@ -2063,13 +2111,14 @@ int drbd_adm_net_opts(struct sk_buff *skb, struct genl_info *info)
 	int rsr; /* re-sync running */
 	struct crypto crypto = { };
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_CONNECTION);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_CONNECTION);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
-		goto out;
+		goto finish;
 
 	connection = adm_ctx.connection;
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 
 	new_net_conf = kzalloc(sizeof(struct net_conf), GFP_KERNEL);
 	if (!new_net_conf) {
@@ -2084,7 +2133,7 @@ int drbd_adm_net_opts(struct sk_buff *skb, struct genl_info *info)
 	old_net_conf = connection->net_conf;
 
 	if (!old_net_conf) {
-		drbd_msg_put_info("net conf missing, try connect");
+		drbd_msg_put_info(adm_ctx.reply_skb, "net conf missing, try connect");
 		retcode = ERR_INVALID_REQUEST;
 		goto fail;
 	}
@@ -2096,7 +2145,7 @@ int drbd_adm_net_opts(struct sk_buff *skb, struct genl_info *info)
 	err = net_conf_from_attrs_for_change(new_net_conf, info);
 	if (err && err != -ENOMSG) {
 		retcode = ERR_MANDATORY_TAG;
-		drbd_msg_put_info(from_attrs_err_to_txt(err));
+		drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 		goto fail;
 	}
 
@@ -2167,12 +2216,15 @@ int drbd_adm_net_opts(struct sk_buff *skb, struct genl_info *info)
  done:
 	conn_reconfig_done(connection);
  out:
-	drbd_adm_finish(info, retcode);
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
+ finish:
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 int drbd_adm_connect(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct drbd_peer_device *peer_device;
 	struct net_conf *old_net_conf, *new_net_conf = NULL;
 	struct crypto crypto = { };
@@ -2182,14 +2234,14 @@ int drbd_adm_connect(struct sk_buff *skb, struct genl_info *info)
 	int i;
 	int err;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_RESOURCE);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_RESOURCE);
 
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
 		goto out;
 	if (!(adm_ctx.my_addr && adm_ctx.peer_addr)) {
-		drbd_msg_put_info("connection endpoint(s) missing");
+		drbd_msg_put_info(adm_ctx.reply_skb, "connection endpoint(s) missing");
 		retcode = ERR_INVALID_REQUEST;
 		goto out;
 	}
@@ -2215,6 +2267,7 @@ int drbd_adm_connect(struct sk_buff *skb, struct genl_info *info)
 		}
 	}
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	connection = first_connection(adm_ctx.resource);
 	conn_reconfig_start(connection);
 
@@ -2235,7 +2288,7 @@ int drbd_adm_connect(struct sk_buff *skb, struct genl_info *info)
 	err = net_conf_from_attrs(new_net_conf, info);
 	if (err && err != -ENOMSG) {
 		retcode = ERR_MANDATORY_TAG;
-		drbd_msg_put_info(from_attrs_err_to_txt(err));
+		drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 		goto fail;
 	}
 
@@ -2284,7 +2337,8 @@ int drbd_adm_connect(struct sk_buff *skb, struct genl_info *info)
 	retcode = conn_request_state(connection, NS(conn, C_UNCONNECTED), CS_VERBOSE);
 
 	conn_reconfig_done(connection);
-	drbd_adm_finish(info, retcode);
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 
 fail:
@@ -2292,8 +2346,9 @@ fail:
 	kfree(new_net_conf);
 
 	conn_reconfig_done(connection);
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
@@ -2356,13 +2411,14 @@ static enum drbd_state_rv conn_try_disconnect(struct drbd_connection *connection
 
 int drbd_adm_disconnect(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct disconnect_parms parms;
 	struct drbd_connection *connection;
 	enum drbd_state_rv rv;
 	enum drbd_ret_code retcode;
 	int err;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_CONNECTION);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_CONNECTION);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
@@ -2374,18 +2430,20 @@ int drbd_adm_disconnect(struct sk_buff *skb, struct genl_info *info)
 		err = disconnect_parms_from_attrs(&parms, info);
 		if (err) {
 			retcode = ERR_MANDATORY_TAG;
-			drbd_msg_put_info(from_attrs_err_to_txt(err));
+			drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 			goto fail;
 		}
 	}
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	rv = conn_try_disconnect(connection, parms.force_disconnect);
 	if (rv < SS_SUCCESS)
 		retcode = rv;  /* FIXME: Type mismatch. */
 	else
 		retcode = NO_ERROR;
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
  fail:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
@@ -2407,6 +2465,7 @@ void resync_after_online_grow(struct drbd_device *device)
 
 int drbd_adm_resize(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct disk_conf *old_disk_conf, *new_disk_conf = NULL;
 	struct resize_parms rs;
 	struct drbd_device *device;
@@ -2417,12 +2476,13 @@ int drbd_adm_resize(struct sk_buff *skb, struct genl_info *info)
 	sector_t u_size;
 	int err;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
-		goto fail;
+		goto finish;
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	device = adm_ctx.device;
 	if (!get_ldev(device)) {
 		retcode = ERR_NO_DISK;
@@ -2436,7 +2496,7 @@ int drbd_adm_resize(struct sk_buff *skb, struct genl_info *info)
 		err = resize_parms_from_attrs(&rs, info);
 		if (err) {
 			retcode = ERR_MANDATORY_TAG;
-			drbd_msg_put_info(from_attrs_err_to_txt(err));
+			drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 			goto fail_ldev;
 		}
 	}
@@ -2482,7 +2542,7 @@ int drbd_adm_resize(struct sk_buff *skb, struct genl_info *info)
 			goto fail_ldev;
 		}
 
-		if (device->state.conn != C_CONNECTED) {
+		if (device->state.conn != C_CONNECTED && !rs.resize_force) {
 			retcode = ERR_MD_LAYOUT_CONNECTED;
 			goto fail_ldev;
 		}
@@ -2528,7 +2588,9 @@ int drbd_adm_resize(struct sk_buff *skb, struct genl_info *info)
 	}
 
  fail:
-	drbd_adm_finish(info, retcode);
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
+ finish:
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 
  fail_ldev:
@@ -2538,11 +2600,12 @@ int drbd_adm_resize(struct sk_buff *skb, struct genl_info *info)
 
 int drbd_adm_resource_opts(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	enum drbd_ret_code retcode;
 	struct res_opts res_opts;
 	int err;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_RESOURCE);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_RESOURCE);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
@@ -2555,33 +2618,37 @@ int drbd_adm_resource_opts(struct sk_buff *skb, struct genl_info *info)
 	err = res_opts_from_attrs(&res_opts, info);
 	if (err && err != -ENOMSG) {
 		retcode = ERR_MANDATORY_TAG;
-		drbd_msg_put_info(from_attrs_err_to_txt(err));
+		drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 		goto fail;
 	}
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	err = set_resource_options(adm_ctx.resource, &res_opts);
 	if (err) {
 		retcode = ERR_INVALID_REQUEST;
 		if (err == -ENOMEM)
 			retcode = ERR_NOMEM;
 	}
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 
 fail:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 int drbd_adm_invalidate(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct drbd_device *device;
 	int retcode; /* enum drbd_ret_code rsp. enum drbd_state_rv */
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
 		goto out;
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	device = adm_ctx.device;
 
 	/* If there is still bitmap IO pending, probably because of a previous
@@ -2605,26 +2672,29 @@ int drbd_adm_invalidate(struct sk_buff *skb, struct genl_info *info)
 	} else
 		retcode = drbd_request_state(device, NS(conn, C_STARTING_SYNC_T));
 	drbd_resume_io(device);
-
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 static int drbd_adm_simple_request_state(struct sk_buff *skb, struct genl_info *info,
 		union drbd_state mask, union drbd_state val)
 {
+	struct drbd_config_context adm_ctx;
 	enum drbd_ret_code retcode;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
 		goto out;
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	retcode = drbd_request_state(adm_ctx.device, mask, val);
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
@@ -2639,15 +2709,17 @@ static int drbd_bmio_set_susp_al(struct drbd_device *device)
 
 int drbd_adm_invalidate_peer(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	int retcode; /* drbd_ret_code, drbd_state_rv */
 	struct drbd_device *device;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
 		goto out;
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	device = adm_ctx.device;
 
 	/* If there is still bitmap IO pending, probably because of a previous
@@ -2674,40 +2746,45 @@ int drbd_adm_invalidate_peer(struct sk_buff *skb, struct genl_info *info)
 	} else
 		retcode = drbd_request_state(device, NS(conn, C_STARTING_SYNC_S));
 	drbd_resume_io(device);
-
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 int drbd_adm_pause_sync(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	enum drbd_ret_code retcode;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
 		goto out;
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	if (drbd_request_state(adm_ctx.device, NS(user_isp, 1)) == SS_NOTHING_TO_DO)
 		retcode = ERR_PAUSE_IS_SET;
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 int drbd_adm_resume_sync(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	union drbd_dev_state s;
 	enum drbd_ret_code retcode;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
 		goto out;
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	if (drbd_request_state(adm_ctx.device, NS(user_isp, 0)) == SS_NOTHING_TO_DO) {
 		s = adm_ctx.device->state;
 		if (s.conn == C_PAUSED_SYNC_S || s.conn == C_PAUSED_SYNC_T) {
@@ -2717,9 +2794,9 @@ int drbd_adm_resume_sync(struct sk_buff *skb, struct genl_info *info)
 			retcode = ERR_PAUSE_IS_CLEAR;
 		}
 	}
-
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
@@ -2730,15 +2807,17 @@ int drbd_adm_suspend_io(struct sk_buff *skb, struct genl_info *info)
 
 int drbd_adm_resume_io(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct drbd_device *device;
 	int retcode; /* enum drbd_ret_code rsp. enum drbd_state_rv */
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
 		goto out;
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	device = adm_ctx.device;
 	if (test_bit(NEW_CUR_UUID, &device->flags)) {
 		drbd_uuid_new_current(device);
@@ -2753,9 +2832,9 @@ int drbd_adm_resume_io(struct sk_buff *skb, struct genl_info *info)
 			tl_restart(first_peer_device(device)->connection, FAIL_FROZEN_DISK_IO);
 	}
 	drbd_resume_io(device);
-
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
@@ -2931,10 +3010,11 @@ nla_put_failure:
 
 int drbd_adm_get_status(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	enum drbd_ret_code retcode;
 	int err;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
@@ -2946,7 +3026,7 @@ int drbd_adm_get_status(struct sk_buff *skb, struct genl_info *info)
 		return err;
 	}
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
@@ -3133,11 +3213,12 @@ dump:
 
 int drbd_adm_get_timeout_type(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	enum drbd_ret_code retcode;
 	struct timeout_parms tp;
 	int err;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
@@ -3154,17 +3235,18 @@ int drbd_adm_get_timeout_type(struct sk_buff *skb, struct genl_info *info)
 		return err;
 	}
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 int drbd_adm_start_ov(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct drbd_device *device;
 	enum drbd_ret_code retcode;
 	struct start_ov_parms parms;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
@@ -3179,10 +3261,12 @@ int drbd_adm_start_ov(struct sk_buff *skb, struct genl_info *info)
 		int err = start_ov_parms_from_attrs(&parms, info);
 		if (err) {
 			retcode = ERR_MANDATORY_TAG;
-			drbd_msg_put_info(from_attrs_err_to_txt(err));
+			drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 			goto out;
 		}
 	}
+	mutex_lock(&adm_ctx.resource->adm_mutex);
+
 	/* w_make_ov_request expects position to be aligned */
 	device->ov_start_sector = parms.ov_start_sector & ~(BM_SECT_PER_BIT-1);
 	device->ov_stop_sector = parms.ov_stop_sector;
@@ -3193,21 +3277,24 @@ int drbd_adm_start_ov(struct sk_buff *skb, struct genl_info *info)
 	wait_event(device->misc_wait, !test_bit(BITMAP_IO, &device->flags));
 	retcode = drbd_request_state(device, NS(conn, C_VERIFY_S));
 	drbd_resume_io(device);
+
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 
 int drbd_adm_new_c_uuid(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct drbd_device *device;
 	enum drbd_ret_code retcode;
 	int skip_initial_sync = 0;
 	int err;
 	struct new_c_uuid_parms args;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
@@ -3219,11 +3306,12 @@ int drbd_adm_new_c_uuid(struct sk_buff *skb, struct genl_info *info)
 		err = new_c_uuid_parms_from_attrs(&args, info);
 		if (err) {
 			retcode = ERR_MANDATORY_TAG;
-			drbd_msg_put_info(from_attrs_err_to_txt(err));
+			drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 			goto out_nolock;
 		}
 	}
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	mutex_lock(device->state_mutex); /* Protects us against serialized state changes. */
 
 	if (!get_ldev(device)) {
@@ -3268,22 +3356,24 @@ out_dec:
 	put_ldev(device);
 out:
 	mutex_unlock(device->state_mutex);
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out_nolock:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 static enum drbd_ret_code
-drbd_check_resource_name(const char *name)
+drbd_check_resource_name(struct drbd_config_context *adm_ctx)
 {
+	const char *name = adm_ctx->resource_name;
 	if (!name || !name[0]) {
-		drbd_msg_put_info("resource name missing");
+		drbd_msg_put_info(adm_ctx->reply_skb, "resource name missing");
 		return ERR_MANDATORY_TAG;
 	}
 	/* if we want to use these in sysfs/configfs/debugfs some day,
 	 * we must not allow slashes */
 	if (strchr(name, '/')) {
-		drbd_msg_put_info("invalid resource name");
+		drbd_msg_put_info(adm_ctx->reply_skb, "invalid resource name");
 		return ERR_INVALID_REQUEST;
 	}
 	return NO_ERROR;
@@ -3291,11 +3381,12 @@ drbd_check_resource_name(const char *name)
 
 int drbd_adm_new_resource(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	enum drbd_ret_code retcode;
 	struct res_opts res_opts;
 	int err;
 
-	retcode = drbd_adm_prepare(skb, info, 0);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, 0);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
@@ -3305,48 +3396,50 @@ int drbd_adm_new_resource(struct sk_buff *skb, struct genl_info *info)
 	err = res_opts_from_attrs(&res_opts, info);
 	if (err && err != -ENOMSG) {
 		retcode = ERR_MANDATORY_TAG;
-		drbd_msg_put_info(from_attrs_err_to_txt(err));
+		drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 		goto out;
 	}
 
-	retcode = drbd_check_resource_name(adm_ctx.resource_name);
+	retcode = drbd_check_resource_name(&adm_ctx);
 	if (retcode != NO_ERROR)
 		goto out;
 
 	if (adm_ctx.resource) {
 		if (info->nlhdr->nlmsg_flags & NLM_F_EXCL) {
 			retcode = ERR_INVALID_REQUEST;
-			drbd_msg_put_info("resource exists");
+			drbd_msg_put_info(adm_ctx.reply_skb, "resource exists");
 		}
 		/* else: still NO_ERROR */
 		goto out;
 	}
 
+	/* not yet safe for genl_family.parallel_ops */
 	if (!conn_create(adm_ctx.resource_name, &res_opts))
 		retcode = ERR_NOMEM;
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 int drbd_adm_new_minor(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct drbd_genlmsghdr *dh = info->userhdr;
 	enum drbd_ret_code retcode;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_RESOURCE);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_RESOURCE);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
 		goto out;
 
 	if (dh->minor > MINORMASK) {
-		drbd_msg_put_info("requested minor out of range");
+		drbd_msg_put_info(adm_ctx.reply_skb, "requested minor out of range");
 		retcode = ERR_INVALID_REQUEST;
 		goto out;
 	}
 	if (adm_ctx.volume > DRBD_VOLUME_MAX) {
-		drbd_msg_put_info("requested volume id out of range");
+		drbd_msg_put_info(adm_ctx.reply_skb, "requested volume id out of range");
 		retcode = ERR_INVALID_REQUEST;
 		goto out;
 	}
@@ -3360,9 +3453,11 @@ int drbd_adm_new_minor(struct sk_buff *skb, struct genl_info *info)
 		goto out;
 	}
 
-	retcode = drbd_create_device(adm_ctx.resource, dh->minor, adm_ctx.volume);
+	mutex_lock(&adm_ctx.resource->adm_mutex);
+	retcode = drbd_create_device(&adm_ctx, dh->minor);
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
@@ -3383,35 +3478,40 @@ static enum drbd_ret_code adm_del_minor(struct drbd_device *device)
 
 int drbd_adm_del_minor(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	enum drbd_ret_code retcode;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
 		goto out;
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	retcode = adm_del_minor(adm_ctx.device);
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 int drbd_adm_down(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct drbd_resource *resource;
 	struct drbd_connection *connection;
 	struct drbd_device *device;
 	int retcode; /* enum drbd_ret_code rsp. enum drbd_state_rv */
 	unsigned i;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_RESOURCE);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_RESOURCE);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
-		goto out;
+		goto finish;
 
 	resource = adm_ctx.resource;
+	mutex_lock(&resource->adm_mutex);
 	/* demote */
 	for_each_connection(connection, resource) {
 		struct drbd_peer_device *peer_device;
@@ -3419,14 +3519,14 @@ int drbd_adm_down(struct sk_buff *skb, struct genl_info *info)
 		idr_for_each_entry(&connection->peer_devices, peer_device, i) {
 			retcode = drbd_set_role(peer_device->device, R_SECONDARY, 0);
 			if (retcode < SS_SUCCESS) {
-				drbd_msg_put_info("failed to demote");
+				drbd_msg_put_info(adm_ctx.reply_skb, "failed to demote");
 				goto out;
 			}
 		}
 
 		retcode = conn_try_disconnect(connection, 0);
 		if (retcode < SS_SUCCESS) {
-			drbd_msg_put_info("failed to disconnect");
+			drbd_msg_put_info(adm_ctx.reply_skb, "failed to disconnect");
 			goto out;
 		}
 	}
@@ -3435,7 +3535,7 @@ int drbd_adm_down(struct sk_buff *skb, struct genl_info *info)
 	idr_for_each_entry(&resource->devices, device, i) {
 		retcode = adm_detach(device, 0);
 		if (retcode < SS_SUCCESS || retcode > NO_ERROR) {
-			drbd_msg_put_info("failed to detach");
+			drbd_msg_put_info(adm_ctx.reply_skb, "failed to detach");
 			goto out;
 		}
 	}
@@ -3453,7 +3553,7 @@ int drbd_adm_down(struct sk_buff *skb, struct genl_info *info)
 		retcode = adm_del_minor(device);
 		if (retcode != NO_ERROR) {
 			/* "can not happen" */
-			drbd_msg_put_info("failed to delete volume");
+			drbd_msg_put_info(adm_ctx.reply_skb, "failed to delete volume");
 			goto out;
 		}
 	}
@@ -3462,25 +3562,28 @@ int drbd_adm_down(struct sk_buff *skb, struct genl_info *info)
 	synchronize_rcu();
 	drbd_free_resource(resource);
 	retcode = NO_ERROR;
-
 out:
-	drbd_adm_finish(info, retcode);
+	mutex_unlock(&resource->adm_mutex);
+finish:
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 int drbd_adm_del_resource(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct drbd_resource *resource;
 	struct drbd_connection *connection;
 	enum drbd_ret_code retcode;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_RESOURCE);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_RESOURCE);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
-		goto out;
+		goto finish;
 
 	resource = adm_ctx.resource;
+	mutex_lock(&resource->adm_mutex);
 	for_each_connection(connection, resource) {
 		if (connection->cstate > C_STANDALONE) {
 			retcode = ERR_NET_CONFIGURED;
@@ -3499,7 +3602,9 @@ int drbd_adm_del_resource(struct sk_buff *skb, struct genl_info *info)
 	drbd_free_resource(resource);
 	retcode = NO_ERROR;
 out:
-	drbd_adm_finish(info, retcode);
+	mutex_unlock(&resource->adm_mutex);
+finish:
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
diff --git a/drivers/block/drbd/drbd_nla.c b/drivers/block/drbd/drbd_nla.c
index fa672b6df8d..b2d4791498a 100644
--- a/drivers/block/drbd/drbd_nla.c
+++ b/drivers/block/drbd/drbd_nla.c
@@ -1,4 +1,3 @@
-#include "drbd_wrappers.h"
 #include <linux/kernel.h>
 #include <net/netlink.h>
 #include <linux/drbd_genl_api.h>
diff --git a/drivers/block/drbd/drbd_proc.c b/drivers/block/drbd/drbd_proc.c
index 2f26e8ffa45..89736bdbbc7 100644
--- a/drivers/block/drbd/drbd_proc.c
+++ b/drivers/block/drbd/drbd_proc.c
@@ -116,7 +116,7 @@ static void drbd_syncer_progress(struct drbd_device *device, struct seq_file *se
 	/* ------------------------ ~18s average ------------------------ */
 	i = (device->rs_last_mark + 2) % DRBD_SYNC_MARKS;
 	dt = (jiffies - device->rs_mark_time[i]) / HZ;
-	if (dt > (DRBD_SYNC_MARK_STEP * DRBD_SYNC_MARKS))
+	if (dt > 180)
 		stalled = 1;
 
 	if (!dt)
diff --git a/drivers/block/drbd/drbd_protocol.h b/drivers/block/drbd/drbd_protocol.h
index 3c04ec0ea33..2da9104a385 100644
--- a/drivers/block/drbd/drbd_protocol.h
+++ b/drivers/block/drbd/drbd_protocol.h
@@ -54,6 +54,11 @@ enum drbd_packet {
 	P_CONN_ST_CHG_REPLY   = 0x2b, /* meta sock: Connection side state req reply */
 	P_RETRY_WRITE	      = 0x2c, /* Protocol C: retry conflicting write request */
 	P_PROTOCOL_UPDATE     = 0x2d, /* data sock: is used in established connections */
+        /* 0x2e to 0x30 reserved, used in drbd 9 */
+
+	/* REQ_DISCARD. We used "discard" in different contexts before,
+	 * which is why I chose TRIM here, to disambiguate. */
+	P_TRIM                = 0x31,
 
 	P_MAY_IGNORE	      = 0x100, /* Flag to test if (cmd > P_MAY_IGNORE) ... */
 	P_MAX_OPT_CMD	      = 0x101,
@@ -119,6 +124,11 @@ struct p_data {
 	u32	    dp_flags;
 } __packed;
 
+struct p_trim {
+	struct p_data p_data;
+	u32	    size;	/* == bio->bi_size */
+} __packed;
+
 /*
  * commands which share a struct:
  *  p_block_ack:
@@ -150,6 +160,8 @@ struct p_block_req {
  *   ReportParams
  */
 
+#define FF_TRIM      1
+
 struct p_connection_features {
 	u32 protocol_min;
 	u32 feature_flags;
diff --git a/drivers/block/drbd/drbd_receiver.c b/drivers/block/drbd/drbd_receiver.c
index 68e3992e883..b6c8aaf4931 100644
--- a/drivers/block/drbd/drbd_receiver.c
+++ b/drivers/block/drbd/drbd_receiver.c
@@ -46,9 +46,10 @@
 #include "drbd_int.h"
 #include "drbd_protocol.h"
 #include "drbd_req.h"
-
 #include "drbd_vli.h"
 
+#define PRO_FEATURES (FF_TRIM)
+
 struct packet_info {
 	enum drbd_packet cmd;
 	unsigned int size;
@@ -65,7 +66,7 @@ enum finish_epoch {
 static int drbd_do_features(struct drbd_connection *connection);
 static int drbd_do_auth(struct drbd_connection *connection);
 static int drbd_disconnected(struct drbd_peer_device *);
-
+static void conn_wait_active_ee_empty(struct drbd_connection *connection);
 static enum finish_epoch drbd_may_finish_epoch(struct drbd_connection *, struct drbd_epoch *, enum epoch_event);
 static int e_end_block(struct drbd_work *, int);
 
@@ -234,9 +235,17 @@ static void drbd_kick_lo_and_reclaim_net(struct drbd_device *device)
  * @retry:	whether to retry, if not enough pages are available right now
  *
  * Tries to allocate number pages, first from our own page pool, then from
- * the kernel, unless this allocation would exceed the max_buffers setting.
+ * the kernel.
  * Possibly retry until DRBD frees sufficient pages somewhere else.
  *
+ * If this allocation would exceed the max_buffers setting, we throttle
+ * allocation (schedule_timeout) to give the system some room to breathe.
+ *
+ * We do not use max-buffers as hard limit, because it could lead to
+ * congestion and further to a distributed deadlock during online-verify or
+ * (checksum based) resync, if the max-buffers, socket buffer sizes and
+ * resync-rate settings are mis-configured.
+ *
  * Returns a page chain linked via page->private.
  */
 struct page *drbd_alloc_pages(struct drbd_peer_device *peer_device, unsigned int number,
@@ -246,10 +255,8 @@ struct page *drbd_alloc_pages(struct drbd_peer_device *peer_device, unsigned int
 	struct page *page = NULL;
 	struct net_conf *nc;
 	DEFINE_WAIT(wait);
-	int mxb;
+	unsigned int mxb;
 
-	/* Yes, we may run up to @number over max_buffers. If we
-	 * follow it strictly, the admin will get it wrong anyways. */
 	rcu_read_lock();
 	nc = rcu_dereference(peer_device->connection->net_conf);
 	mxb = nc ? nc->max_buffers : 1000000;
@@ -277,7 +284,8 @@ struct page *drbd_alloc_pages(struct drbd_peer_device *peer_device, unsigned int
 			break;
 		}
 
-		schedule();
+		if (schedule_timeout(HZ/10) == 0)
+			mxb = UINT_MAX;
 	}
 	finish_wait(&drbd_pp_wait, &wait);
 
@@ -331,7 +339,7 @@ You must not have the req_lock:
 
 struct drbd_peer_request *
 drbd_alloc_peer_req(struct drbd_peer_device *peer_device, u64 id, sector_t sector,
-		    unsigned int data_size, gfp_t gfp_mask) __must_hold(local)
+		    unsigned int data_size, bool has_payload, gfp_t gfp_mask) __must_hold(local)
 {
 	struct drbd_device *device = peer_device->device;
 	struct drbd_peer_request *peer_req;
@@ -348,7 +356,7 @@ drbd_alloc_peer_req(struct drbd_peer_device *peer_device, u64 id, sector_t secto
 		return NULL;
 	}
 
-	if (data_size) {
+	if (has_payload && data_size) {
 		page = drbd_alloc_pages(peer_device, nr_pages, (gfp_mask & __GFP_WAIT));
 		if (!page)
 			goto fail;
@@ -1026,24 +1034,27 @@ randomize:
 	if (drbd_send_protocol(connection) == -EOPNOTSUPP)
 		return -1;
 
+	/* Prevent a race between resync-handshake and
+	 * being promoted to Primary.
+	 *
+	 * Grab and release the state mutex, so we know that any current
+	 * drbd_set_role() is finished, and any incoming drbd_set_role
+	 * will see the STATE_SENT flag, and wait for it to be cleared.
+	 */
+	idr_for_each_entry(&connection->peer_devices, peer_device, vnr)
+		mutex_lock(peer_device->device->state_mutex);
+
 	set_bit(STATE_SENT, &connection->flags);
 
+	idr_for_each_entry(&connection->peer_devices, peer_device, vnr)
+		mutex_unlock(peer_device->device->state_mutex);
+
 	rcu_read_lock();
 	idr_for_each_entry(&connection->peer_devices, peer_device, vnr) {
 		struct drbd_device *device = peer_device->device;
 		kref_get(&device->kref);
 		rcu_read_unlock();
 
-		/* Prevent a race between resync-handshake and
-		 * being promoted to Primary.
-		 *
-		 * Grab and release the state mutex, so we know that any current
-		 * drbd_set_role() is finished, and any incoming drbd_set_role
-		 * will see the STATE_SENT flag, and wait for it to be cleared.
-		 */
-		mutex_lock(device->state_mutex);
-		mutex_unlock(device->state_mutex);
-
 		if (discard_my_data)
 			set_bit(DISCARD_MY_DATA, &device->flags);
 		else
@@ -1315,6 +1326,20 @@ int drbd_submit_peer_request(struct drbd_device *device,
 	unsigned nr_pages = (ds + PAGE_SIZE -1) >> PAGE_SHIFT;
 	int err = -ENOMEM;
 
+	if (peer_req->flags & EE_IS_TRIM_USE_ZEROOUT) {
+		/* wait for all pending IO completions, before we start
+		 * zeroing things out. */
+		conn_wait_active_ee_empty(first_peer_device(device)->connection);
+		if (blkdev_issue_zeroout(device->ldev->backing_bdev,
+			sector, ds >> 9, GFP_NOIO))
+			peer_req->flags |= EE_WAS_ERROR;
+		drbd_endio_write_sec_final(peer_req);
+		return 0;
+	}
+
+	if (peer_req->flags & EE_IS_TRIM)
+		nr_pages = 0; /* discards don't have any payload. */
+
 	/* In most cases, we will only need one bio.  But in case the lower
 	 * level restrictions happen to be different at this offset on this
 	 * side than those of the sending peer, we may need to submit the
@@ -1326,7 +1351,7 @@ int drbd_submit_peer_request(struct drbd_device *device,
 next_bio:
 	bio = bio_alloc(GFP_NOIO, nr_pages);
 	if (!bio) {
-		drbd_err(device, "submit_ee: Allocation of a bio failed\n");
+		drbd_err(device, "submit_ee: Allocation of a bio failed (nr_pages=%u)\n", nr_pages);
 		goto fail;
 	}
 	/* > peer_req->i.sector, unless this is the first bio */
@@ -1340,6 +1365,11 @@ next_bio:
 	bios = bio;
 	++n_bios;
 
+	if (rw & REQ_DISCARD) {
+		bio->bi_iter.bi_size = ds;
+		goto submit;
+	}
+
 	page_chain_for_each(page) {
 		unsigned len = min_t(unsigned, ds, PAGE_SIZE);
 		if (!bio_add_page(bio, page, len, 0)) {
@@ -1360,8 +1390,9 @@ next_bio:
 		sector += len >> 9;
 		--nr_pages;
 	}
-	D_ASSERT(device, page == NULL);
 	D_ASSERT(device, ds == 0);
+submit:
+	D_ASSERT(device, page == NULL);
 
 	atomic_set(&peer_req->pending_bios, n_bios);
 	do {
@@ -1490,19 +1521,21 @@ static int receive_Barrier(struct drbd_connection *connection, struct packet_inf
  * and from receive_Data */
 static struct drbd_peer_request *
 read_in_block(struct drbd_peer_device *peer_device, u64 id, sector_t sector,
-	      int data_size) __must_hold(local)
+	      struct packet_info *pi) __must_hold(local)
 {
 	struct drbd_device *device = peer_device->device;
 	const sector_t capacity = drbd_get_capacity(device->this_bdev);
 	struct drbd_peer_request *peer_req;
 	struct page *page;
 	int dgs, ds, err;
+	int data_size = pi->size;
 	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;
-	if (peer_device->connection->peer_integrity_tfm) {
+	if (!trim && peer_device->connection->peer_integrity_tfm) {
 		dgs = crypto_hash_digestsize(peer_device->connection->peer_integrity_tfm);
 		/*
 		 * FIXME: Receive the incoming digest into the receive buffer
@@ -1514,9 +1547,15 @@ read_in_block(struct drbd_peer_device *peer_device, u64 id, sector_t sector,
 		data_size -= dgs;
 	}
 
+	if (trim) {
+		D_ASSERT(peer_device, data_size == 0);
+		data_size = be32_to_cpu(trim->size);
+	}
+
 	if (!expect(IS_ALIGNED(data_size, 512)))
 		return NULL;
-	if (!expect(data_size <= DRBD_MAX_BIO_SIZE))
+	/* prepare for larger trim requests. */
+	if (!trim && !expect(data_size <= DRBD_MAX_BIO_SIZE))
 		return NULL;
 
 	/* even though we trust out peer,
@@ -1532,11 +1571,11 @@ read_in_block(struct drbd_peer_device *peer_device, u64 id, sector_t sector,
 	/* GFP_NOIO, because we must not cause arbitrary write-out: in a DRBD
 	 * "criss-cross" setup, that might cause write-out on some other DRBD,
 	 * which in turn might block on the other node at this very place.  */
-	peer_req = drbd_alloc_peer_req(peer_device, id, sector, data_size, GFP_NOIO);
+	peer_req = drbd_alloc_peer_req(peer_device, id, sector, data_size, trim == NULL, GFP_NOIO);
 	if (!peer_req)
 		return NULL;
 
-	if (!data_size)
+	if (trim)
 		return peer_req;
 
 	ds = data_size;
@@ -1676,12 +1715,12 @@ static int e_end_resync_block(struct drbd_work *w, int unused)
 }
 
 static int recv_resync_read(struct drbd_peer_device *peer_device, sector_t sector,
-			    int data_size) __releases(local)
+			    struct packet_info *pi) __releases(local)
 {
 	struct drbd_device *device = peer_device->device;
 	struct drbd_peer_request *peer_req;
 
-	peer_req = read_in_block(peer_device, ID_SYNCER, sector, data_size);
+	peer_req = read_in_block(peer_device, ID_SYNCER, sector, pi);
 	if (!peer_req)
 		goto fail;
 
@@ -1697,7 +1736,7 @@ static int recv_resync_read(struct drbd_peer_device *peer_device, sector_t secto
 	list_add(&peer_req->w.list, &device->sync_ee);
 	spin_unlock_irq(&device->resource->req_lock);
 
-	atomic_add(data_size >> 9, &device->rs_sect_ev);
+	atomic_add(pi->size >> 9, &device->rs_sect_ev);
 	if (drbd_submit_peer_request(device, peer_req, WRITE, DRBD_FAULT_RS_WR) == 0)
 		return 0;
 
@@ -1785,7 +1824,7 @@ static int receive_RSDataReply(struct drbd_connection *connection, struct packet
 		/* data is submitted to disk within recv_resync_read.
 		 * corresponding put_ldev done below on error,
 		 * or in drbd_peer_request_endio. */
-		err = recv_resync_read(peer_device, sector, pi->size);
+		err = recv_resync_read(peer_device, sector, pi);
 	} else {
 		if (__ratelimit(&drbd_ratelimit_state))
 			drbd_err(device, "Can not write resync data to local disk.\n");
@@ -2196,7 +2235,7 @@ static int receive_Data(struct drbd_connection *connection, struct packet_info *
 	 */
 
 	sector = be64_to_cpu(p->sector);
-	peer_req = read_in_block(peer_device, p->block_id, sector, pi->size);
+	peer_req = read_in_block(peer_device, p->block_id, sector, pi);
 	if (!peer_req) {
 		put_ldev(device);
 		return -EIO;
@@ -2206,7 +2245,15 @@ static int receive_Data(struct drbd_connection *connection, struct packet_info *
 
 	dp_flags = be32_to_cpu(p->dp_flags);
 	rw |= wire_flags_to_bio(dp_flags);
-	if (peer_req->pages == NULL) {
+	if (pi->cmd == P_TRIM) {
+		struct request_queue *q = bdev_get_queue(device->ldev->backing_bdev);
+		peer_req->flags |= EE_IS_TRIM;
+		if (!blk_queue_discard(q))
+			peer_req->flags |= EE_IS_TRIM_USE_ZEROOUT;
+		D_ASSERT(peer_device, peer_req->i.size > 0);
+		D_ASSERT(peer_device, rw & REQ_DISCARD);
+		D_ASSERT(peer_device, peer_req->pages == NULL);
+	} else if (peer_req->pages == NULL) {
 		D_ASSERT(device, peer_req->i.size == 0);
 		D_ASSERT(device, dp_flags & DP_FLUSH);
 	}
@@ -2242,7 +2289,12 @@ static int receive_Data(struct drbd_connection *connection, struct packet_info *
 		update_peer_seq(peer_device, peer_seq);
 		spin_lock_irq(&device->resource->req_lock);
 	}
-	list_add(&peer_req->w.list, &device->active_ee);
+	/* if we use the zeroout fallback code, we process synchronously
+	 * and we wait for all pending requests, respectively wait for
+	 * active_ee to become empty in drbd_submit_peer_request();
+	 * better not add ourselves here. */
+	if ((peer_req->flags & EE_IS_TRIM_USE_ZEROOUT) == 0)
+		list_add(&peer_req->w.list, &device->active_ee);
 	spin_unlock_irq(&device->resource->req_lock);
 
 	if (device->state.conn == C_SYNC_TARGET)
@@ -2313,39 +2365,45 @@ out_interrupted:
  * The current sync rate used here uses only the most recent two step marks,
  * to have a short time average so we can react faster.
  */
-int drbd_rs_should_slow_down(struct drbd_device *device, sector_t sector)
+bool drbd_rs_should_slow_down(struct drbd_device *device, sector_t sector)
 {
-	struct gendisk *disk = device->ldev->backing_bdev->bd_contains->bd_disk;
-	unsigned long db, dt, dbdt;
 	struct lc_element *tmp;
-	int curr_events;
-	int throttle = 0;
-	unsigned int c_min_rate;
-
-	rcu_read_lock();
-	c_min_rate = rcu_dereference(device->ldev->disk_conf)->c_min_rate;
-	rcu_read_unlock();
+	bool throttle = true;
 
-	/* feature disabled? */
-	if (c_min_rate == 0)
-		return 0;
+	if (!drbd_rs_c_min_rate_throttle(device))
+		return false;
 
 	spin_lock_irq(&device->al_lock);
 	tmp = lc_find(device->resync, BM_SECT_TO_EXT(sector));
 	if (tmp) {
 		struct bm_extent *bm_ext = lc_entry(tmp, struct bm_extent, lce);
-		if (test_bit(BME_PRIORITY, &bm_ext->flags)) {
-			spin_unlock_irq(&device->al_lock);
-			return 0;
-		}
+		if (test_bit(BME_PRIORITY, &bm_ext->flags))
+			throttle = false;
 		/* Do not slow down if app IO is already waiting for this extent */
 	}
 	spin_unlock_irq(&device->al_lock);
 
+	return throttle;
+}
+
+bool drbd_rs_c_min_rate_throttle(struct drbd_device *device)
+{
+	struct gendisk *disk = device->ldev->backing_bdev->bd_contains->bd_disk;
+	unsigned long db, dt, dbdt;
+	unsigned int c_min_rate;
+	int curr_events;
+
+	rcu_read_lock();
+	c_min_rate = rcu_dereference(device->ldev->disk_conf)->c_min_rate;
+	rcu_read_unlock();
+
+	/* feature disabled? */
+	if (c_min_rate == 0)
+		return false;
+
 	curr_events = (int)part_stat_read(&disk->part0, sectors[0]) +
 		      (int)part_stat_read(&disk->part0, sectors[1]) -
 			atomic_read(&device->rs_sect_ev);
-
 	if (!device->rs_last_events || curr_events - device->rs_last_events > 64) {
 		unsigned long rs_left;
 		int i;
@@ -2368,12 +2426,11 @@ int drbd_rs_should_slow_down(struct drbd_device *device, sector_t sector)
 		dbdt = Bit2KB(db/dt);
 
 		if (dbdt > c_min_rate)
-			throttle = 1;
+			return true;
 	}
-	return throttle;
+	return false;
 }
 
-
 static int receive_DataRequest(struct drbd_connection *connection, struct packet_info *pi)
 {
 	struct drbd_peer_device *peer_device;
@@ -2436,7 +2493,8 @@ static int receive_DataRequest(struct drbd_connection *connection, struct packet
 	/* GFP_NOIO, because we must not cause arbitrary write-out: in a DRBD
 	 * "criss-cross" setup, that might cause write-out on some other DRBD,
 	 * which in turn might block on the other node at this very place.  */
-	peer_req = drbd_alloc_peer_req(peer_device, p->block_id, sector, size, GFP_NOIO);
+	peer_req = drbd_alloc_peer_req(peer_device, p->block_id, sector, size,
+			true /* has real payload */, GFP_NOIO);
 	if (!peer_req) {
 		put_ldev(device);
 		return -ENOMEM;
@@ -3648,6 +3706,13 @@ static int receive_sizes(struct drbd_connection *connection, struct packet_info
 		put_ldev(device);
 	}
 
+	device->peer_max_bio_size = be32_to_cpu(p->max_bio_size);
+	drbd_reconsider_max_bio_size(device);
+	/* Leave drbd_reconsider_max_bio_size() before drbd_determine_dev_size().
+	   In case we cleared the QUEUE_FLAG_DISCARD from our queue in
+	   drbd_reconsider_max_bio_size(), we can be sure that after
+	   drbd_determine_dev_size() no REQ_DISCARDs are in the queue. */
+
 	ddsf = be16_to_cpu(p->dds_flags);
 	if (get_ldev(device)) {
 		dd = drbd_determine_dev_size(device, ddsf, NULL);
@@ -3660,9 +3725,6 @@ static int receive_sizes(struct drbd_connection *connection, struct packet_info
 		drbd_set_my_capacity(device, p_size);
 	}
 
-	device->peer_max_bio_size = be32_to_cpu(p->max_bio_size);
-	drbd_reconsider_max_bio_size(device);
-
 	if (get_ldev(device)) {
 		if (device->ldev->known_size != drbd_get_capacity(device->ldev->backing_bdev)) {
 			device->ldev->known_size = drbd_get_capacity(device->ldev->backing_bdev);
@@ -4423,6 +4485,7 @@ static struct data_cmd drbd_cmd_handler[] = {
 	[P_OUT_OF_SYNC]     = { 0, sizeof(struct p_block_desc), receive_out_of_sync },
 	[P_CONN_ST_CHG_REQ] = { 0, sizeof(struct p_req_state), receive_req_conn_state },
 	[P_PROTOCOL_UPDATE] = { 1, sizeof(struct p_protocol), receive_protocol },
+	[P_TRIM]	    = { 0, sizeof(struct p_trim), receive_Data },
 };
 
 static void drbdd(struct drbd_connection *connection)
@@ -4630,6 +4693,7 @@ static int drbd_send_features(struct drbd_connection *connection)
 	memset(p, 0, sizeof(*p));
 	p->protocol_min = cpu_to_be32(PRO_VERSION_MIN);
 	p->protocol_max = cpu_to_be32(PRO_VERSION_MAX);
+	p->feature_flags = cpu_to_be32(PRO_FEATURES);
 	return conn_send_command(connection, sock, P_CONNECTION_FEATURES, sizeof(*p), NULL, 0);
 }
 
@@ -4683,10 +4747,14 @@ static int drbd_do_features(struct drbd_connection *connection)
 		goto incompat;
 
 	connection->agreed_pro_version = min_t(int, PRO_VERSION_MAX, p->protocol_max);
+	connection->agreed_features = PRO_FEATURES & be32_to_cpu(p->feature_flags);
 
 	drbd_info(connection, "Handshake successful: "
 	     "Agreed network protocol version %d\n", connection->agreed_pro_version);
 
+	drbd_info(connection, "Agreed to%ssupport TRIM on protocol level\n",
+		  connection->agreed_features & FF_TRIM ? " " : " not ");
+
 	return 1;
 
  incompat:
@@ -4778,6 +4846,12 @@ static int drbd_do_auth(struct drbd_connection *connection)
 		goto fail;
 	}
 
+	if (pi.size < CHALLENGE_LEN) {
+		drbd_err(connection, "AuthChallenge payload too small.\n");
+		rv = -1;
+		goto fail;
+	}
+
 	peers_ch = kmalloc(pi.size, GFP_NOIO);
 	if (peers_ch == NULL) {
 		drbd_err(connection, "kmalloc of peers_ch failed\n");
@@ -4791,6 +4865,12 @@ static int drbd_do_auth(struct drbd_connection *connection)
 		goto fail;
 	}
 
+	if (!memcmp(my_challenge, peers_ch, CHALLENGE_LEN)) {
+		drbd_err(connection, "Peer presented the same challenge!\n");
+		rv = -1;
+		goto fail;
+	}
+
 	resp_size = crypto_hash_digestsize(connection->cram_hmac_tfm);
 	response = kmalloc(resp_size, GFP_NOIO);
 	if (response == NULL) {
diff --git a/drivers/block/drbd/drbd_req.c b/drivers/block/drbd/drbd_req.c
index 3779c8d2875..09803d0d520 100644
--- a/drivers/block/drbd/drbd_req.c
+++ b/drivers/block/drbd/drbd_req.c
@@ -522,6 +522,13 @@ int __req_mod(struct drbd_request *req, enum drbd_req_event what,
 		mod_rq_state(req, m, RQ_LOCAL_PENDING, RQ_LOCAL_COMPLETED);
 		break;
 
+	case DISCARD_COMPLETED_NOTSUPP:
+	case DISCARD_COMPLETED_WITH_ERROR:
+		/* I'd rather not detach from local disk just because it
+		 * failed a REQ_DISCARD. */
+		mod_rq_state(req, m, RQ_LOCAL_PENDING, RQ_LOCAL_COMPLETED);
+		break;
+
 	case QUEUE_FOR_NET_READ:
 		/* READ or READA, and
 		 * no local disk,
@@ -1235,6 +1242,7 @@ void do_submit(struct work_struct *ws)
 		if (list_empty(&incoming))
 			break;
 
+skip_fast_path:
 		wait_event(device->al_wait, prepare_al_transaction_nonblock(device, &incoming, &pending));
 		/* Maybe more was queued, while we prepared the transaction?
 		 * Try to stuff them into this transaction as well.
@@ -1273,6 +1281,25 @@ void do_submit(struct work_struct *ws)
 			list_del_init(&req->tl_requests);
 			drbd_send_and_submit(device, req);
 		}
+
+		/* If all currently hot activity log extents are kept busy by
+		 * incoming requests, we still must not totally starve new
+		 * requests to cold extents. In that case, prepare one request
+		 * in blocking mode. */
+		list_for_each_entry_safe(req, tmp, &incoming, tl_requests) {
+			list_del_init(&req->tl_requests);
+			req->rq_state |= RQ_IN_ACT_LOG;
+			if (!drbd_al_begin_io_prepare(device, &req->i)) {
+				/* Corresponding extent was hot after all? */
+				drbd_send_and_submit(device, req);
+			} else {
+				/* Found a request to a cold extent.
+				 * Put on "pending" list,
+				 * and try to cumulate with more. */
+				list_add(&req->tl_requests, &pending);
+				goto skip_fast_path;
+			}
+		}
 	}
 }
 
@@ -1326,23 +1353,35 @@ int drbd_merge_bvec(struct request_queue *q, struct bvec_merge_data *bvm, struct
 	return limit;
 }
 
-static struct drbd_request *find_oldest_request(struct drbd_connection *connection)
+static void find_oldest_requests(
+		struct drbd_connection *connection,
+		struct drbd_device *device,
+		struct drbd_request **oldest_req_waiting_for_peer,
+		struct drbd_request **oldest_req_waiting_for_disk)
 {
-	/* Walk the transfer log,
-	 * and find the oldest not yet completed request */
 	struct drbd_request *r;
+	*oldest_req_waiting_for_peer = NULL;
+	*oldest_req_waiting_for_disk = NULL;
 	list_for_each_entry(r, &connection->transfer_log, tl_requests) {
-		if (atomic_read(&r->completion_ref))
-			return r;
+		const unsigned s = r->rq_state;
+		if (!*oldest_req_waiting_for_peer
+		&& ((s & RQ_NET_MASK) && !(s & RQ_NET_DONE)))
+			*oldest_req_waiting_for_peer = r;
+
+		if (!*oldest_req_waiting_for_disk
+		&& (s & RQ_LOCAL_PENDING) && r->device == device)
+			*oldest_req_waiting_for_disk = r;
+
+		if (*oldest_req_waiting_for_peer && *oldest_req_waiting_for_disk)
+			break;
 	}
-	return NULL;
 }
 
 void request_timer_fn(unsigned long data)
 {
 	struct drbd_device *device = (struct drbd_device *) data;
 	struct drbd_connection *connection = first_peer_device(device)->connection;
-	struct drbd_request *req; /* oldest request */
+	struct drbd_request *req_disk, *req_peer; /* oldest request */
 	struct net_conf *nc;
 	unsigned long ent = 0, dt = 0, et, nt; /* effective timeout = ko_count * timeout */
 	unsigned long now;
@@ -1366,8 +1405,8 @@ void request_timer_fn(unsigned long data)
 	now = jiffies;
 
 	spin_lock_irq(&device->resource->req_lock);
-	req = find_oldest_request(connection);
-	if (!req) {
+	find_oldest_requests(connection, device, &req_peer, &req_disk);
+	if (req_peer == NULL && req_disk == NULL) {
 		spin_unlock_irq(&device->resource->req_lock);
 		mod_timer(&device->request_timer, now + et);
 		return;
@@ -1389,19 +1428,26 @@ void request_timer_fn(unsigned long data)
 	 * ~198 days with 250 HZ, we have a window where the timeout would need
 	 * to expire twice (worst case) to become effective. Good enough.
 	 */
-	if (ent && req->rq_state & RQ_NET_PENDING &&
-		 time_after(now, req->start_time + ent) &&
+	if (ent && req_peer &&
+		 time_after(now, req_peer->start_time + ent) &&
 		!time_in_range(now, connection->last_reconnect_jif, connection->last_reconnect_jif + ent)) {
 		drbd_warn(device, "Remote failed to finish a request within ko-count * timeout\n");
 		_drbd_set_state(_NS(device, conn, C_TIMEOUT), CS_VERBOSE | CS_HARD, NULL);
 	}
-	if (dt && req->rq_state & RQ_LOCAL_PENDING && req->device == device &&
-		 time_after(now, req->start_time + dt) &&
+	if (dt && req_disk &&
+		 time_after(now, req_disk->start_time + dt) &&
 		!time_in_range(now, device->last_reattach_jif, device->last_reattach_jif + dt)) {
 		drbd_warn(device, "Local backing device failed to meet the disk-timeout\n");
 		__drbd_chk_io_error(device, DRBD_FORCE_DETACH);
 	}
-	nt = (time_after(now, req->start_time + et) ? now : req->start_time) + et;
+
+	/* Reschedule timer for the nearest not already expired timeout.
+	 * Fallback to now + min(effective network timeout, disk timeout). */
+	ent = (ent && req_peer && time_before(now, req_peer->start_time + ent))
+		? req_peer->start_time + ent : now + et;
+	dt = (dt && req_disk && time_before(now, req_disk->start_time + dt))
+		? req_disk->start_time + dt : now + et;
+	nt = time_before(ent, dt) ? ent : dt;
 	spin_unlock_irq(&connection->resource->req_lock);
 	mod_timer(&device->request_timer, nt);
 }
diff --git a/drivers/block/drbd/drbd_req.h b/drivers/block/drbd/drbd_req.h
index c684c963538..8566cd5866b 100644
--- a/drivers/block/drbd/drbd_req.h
+++ b/drivers/block/drbd/drbd_req.h
@@ -30,7 +30,6 @@
 #include <linux/slab.h>
 #include <linux/drbd.h>
 #include "drbd_int.h"
-#include "drbd_wrappers.h"
 
 /* The request callbacks will be called in irq context by the IDE drivers,
    and in Softirqs/Tasklets/BH context by the SCSI drivers,
@@ -111,11 +110,14 @@ enum drbd_req_event {
 	BARRIER_ACKED, /* in protocol A and B */
 	DATA_RECEIVED, /* (remote read) */
 
+	COMPLETED_OK,
 	READ_COMPLETED_WITH_ERROR,
 	READ_AHEAD_COMPLETED_WITH_ERROR,
 	WRITE_COMPLETED_WITH_ERROR,
+	DISCARD_COMPLETED_NOTSUPP,
+	DISCARD_COMPLETED_WITH_ERROR,
+
 	ABORT_DISK_IO,
-	COMPLETED_OK,
 	RESEND,
 	FAIL_FROZEN_DISK_IO,
 	RESTART_FROZEN_DISK_IO,
diff --git a/drivers/block/drbd/drbd_state.c b/drivers/block/drbd/drbd_state.c
index 1a84345a386..a5d8aae00e0 100644
--- a/drivers/block/drbd/drbd_state.c
+++ b/drivers/block/drbd/drbd_state.c
@@ -54,8 +54,8 @@ static void after_state_ch(struct drbd_device *device, union drbd_state os,
 static enum drbd_state_rv is_valid_state(struct drbd_device *, union drbd_state);
 static enum drbd_state_rv is_valid_soft_transition(union drbd_state, union drbd_state, struct drbd_connection *);
 static enum drbd_state_rv is_valid_transition(union drbd_state os, union drbd_state ns);
-static union drbd_state sanitize_state(struct drbd_device *device, union drbd_state ns,
-				       enum sanitize_state_warnings *warn);
+static union drbd_state sanitize_state(struct drbd_device *device, union drbd_state os,
+				       union drbd_state ns, enum sanitize_state_warnings *warn);
 
 static inline bool is_susp(union drbd_state s)
 {
@@ -287,7 +287,7 @@ _req_st_cond(struct drbd_device *device, union drbd_state mask,
 
 	spin_lock_irqsave(&device->resource->req_lock, flags);
 	os = drbd_read_state(device);
-	ns = sanitize_state(device, apply_mask_val(os, mask, val), NULL);
+	ns = sanitize_state(device, os, apply_mask_val(os, mask, val), NULL);
 	rv = is_valid_transition(os, ns);
 	if (rv >= SS_SUCCESS)
 		rv = SS_UNKNOWN_ERROR;  /* cont waiting, otherwise fail. */
@@ -333,7 +333,7 @@ drbd_req_state(struct drbd_device *device, union drbd_state mask,
 
 	spin_lock_irqsave(&device->resource->req_lock, flags);
 	os = drbd_read_state(device);
-	ns = sanitize_state(device, apply_mask_val(os, mask, val), NULL);
+	ns = sanitize_state(device, os, apply_mask_val(os, mask, val), NULL);
 	rv = is_valid_transition(os, ns);
 	if (rv < SS_SUCCESS) {
 		spin_unlock_irqrestore(&device->resource->req_lock, flags);
@@ -740,8 +740,8 @@ static void print_sanitize_warnings(struct drbd_device *device, enum sanitize_st
  * When we loose connection, we have to set the state of the peers disk (pdsk)
  * to D_UNKNOWN. This rule and many more along those lines are in this function.
  */
-static union drbd_state sanitize_state(struct drbd_device *device, union drbd_state ns,
-				       enum sanitize_state_warnings *warn)
+static union drbd_state sanitize_state(struct drbd_device *device, union drbd_state os,
+				       union drbd_state ns, enum sanitize_state_warnings *warn)
 {
 	enum drbd_fencing_p fp;
 	enum drbd_disk_state disk_min, disk_max, pdsk_min, pdsk_max;
@@ -882,11 +882,13 @@ static union drbd_state sanitize_state(struct drbd_device *device, union drbd_st
 	}
 
 	if (fp == FP_STONITH &&
-	    (ns.role == R_PRIMARY && ns.conn < C_CONNECTED && ns.pdsk > D_OUTDATED))
+	    (ns.role == R_PRIMARY && ns.conn < C_CONNECTED && ns.pdsk > D_OUTDATED) &&
+	    !(os.role == R_PRIMARY && os.conn < C_CONNECTED && os.pdsk > D_OUTDATED))
 		ns.susp_fen = 1; /* Suspend IO while fence-peer handler runs (peer lost) */
 
 	if (device->resource->res_opts.on_no_data == OND_SUSPEND_IO &&
-	    (ns.role == R_PRIMARY && ns.disk < D_UP_TO_DATE && ns.pdsk < D_UP_TO_DATE))
+	    (ns.role == R_PRIMARY && ns.disk < D_UP_TO_DATE && ns.pdsk < D_UP_TO_DATE) &&
+	    !(os.role == R_PRIMARY && os.disk < D_UP_TO_DATE && os.pdsk < D_UP_TO_DATE))
 		ns.susp_nod = 1; /* Suspend IO while no data available (no accessible data available) */
 
 	if (ns.aftr_isp || ns.peer_isp || ns.user_isp) {
@@ -958,7 +960,7 @@ __drbd_set_state(struct drbd_device *device, union drbd_state ns,
 
 	os = drbd_read_state(device);
 
-	ns = sanitize_state(device, ns, &ssw);
+	ns = sanitize_state(device, os, ns, &ssw);
 	if (ns.i == os.i)
 		return SS_NOTHING_TO_DO;
 
@@ -1656,7 +1658,7 @@ conn_is_valid_transition(struct drbd_connection *connection, union drbd_state ma
 	idr_for_each_entry(&connection->peer_devices, peer_device, vnr) {
 		struct drbd_device *device = peer_device->device;
 		os = drbd_read_state(device);
-		ns = sanitize_state(device, apply_mask_val(os, mask, val), NULL);
+		ns = sanitize_state(device, os, apply_mask_val(os, mask, val), NULL);
 
 		if (flags & CS_IGN_OUTD_FAIL && ns.disk == D_OUTDATED && os.disk < D_OUTDATED)
 			ns.disk = os.disk;
@@ -1718,7 +1720,7 @@ conn_set_state(struct drbd_connection *connection, union drbd_state mask, union
 		number_of_volumes++;
 		os = drbd_read_state(device);
 		ns = apply_mask_val(os, mask, val);
-		ns = sanitize_state(device, ns, NULL);
+		ns = sanitize_state(device, os, ns, NULL);
 
 		if (flags & CS_IGN_OUTD_FAIL && ns.disk == D_OUTDATED && os.disk < D_OUTDATED)
 			ns.disk = os.disk;
@@ -1763,19 +1765,19 @@ conn_set_state(struct drbd_connection *connection, union drbd_state mask, union
 static enum drbd_state_rv
 _conn_rq_cond(struct drbd_connection *connection, union drbd_state mask, union drbd_state val)
 {
-	enum drbd_state_rv rv;
+	enum drbd_state_rv err, rv = SS_UNKNOWN_ERROR; /* continue waiting */;
 
 	if (test_and_clear_bit(CONN_WD_ST_CHG_OKAY, &connection->flags))
-		return SS_CW_SUCCESS;
+		rv = SS_CW_SUCCESS;
 
 	if (test_and_clear_bit(CONN_WD_ST_CHG_FAIL, &connection->flags))
-		return SS_CW_FAILED_BY_PEER;
+		rv = SS_CW_FAILED_BY_PEER;
 
-	rv = conn_is_valid_transition(connection, mask, val, 0);
-	if (rv == SS_SUCCESS && connection->cstate == C_WF_REPORT_PARAMS)
-		rv = SS_UNKNOWN_ERROR; /* continue waiting */
+	err = conn_is_valid_transition(connection, mask, val, 0);
+	if (err == SS_SUCCESS && connection->cstate == C_WF_REPORT_PARAMS)
+		return rv;
 
-	return rv;
+	return err;
 }
 
 enum drbd_state_rv
diff --git a/drivers/block/drbd/drbd_worker.c b/drivers/block/drbd/drbd_worker.c
index 2c4ce42c365..d8f57b6305c 100644
--- a/drivers/block/drbd/drbd_worker.c
+++ b/drivers/block/drbd/drbd_worker.c
@@ -118,7 +118,7 @@ static void drbd_endio_read_sec_final(struct drbd_peer_request *peer_req) __rele
 
 /* writes on behalf of the partner, or resync writes,
  * "submitted" by the receiver, final stage.  */
-static void drbd_endio_write_sec_final(struct drbd_peer_request *peer_req) __releases(local)
+void drbd_endio_write_sec_final(struct drbd_peer_request *peer_req) __releases(local)
 {
 	unsigned long flags = 0;
 	struct drbd_peer_device *peer_device = peer_req->peer_device;
@@ -150,7 +150,9 @@ static void drbd_endio_write_sec_final(struct drbd_peer_request *peer_req) __rel
 
 	do_wake = list_empty(block_id == ID_SYNCER ? &device->sync_ee : &device->active_ee);
 
-	if (test_bit(__EE_WAS_ERROR, &peer_req->flags))
+	/* FIXME do we want to detach for failed REQ_DISCARD?
+	 * ((peer_req->flags & (EE_WAS_ERROR|EE_IS_TRIM)) == EE_WAS_ERROR) */
+	if (peer_req->flags & EE_WAS_ERROR)
 		__drbd_chk_io_error(device, DRBD_WRITE_ERROR);
 	spin_unlock_irqrestore(&device->resource->req_lock, flags);
 
@@ -176,10 +178,12 @@ void drbd_peer_request_endio(struct bio *bio, int error)
 	struct drbd_device *device = peer_req->peer_device->device;
 	int uptodate = bio_flagged(bio, BIO_UPTODATE);
 	int is_write = bio_data_dir(bio) == WRITE;
+	int is_discard = !!(bio->bi_rw & REQ_DISCARD);
 
 	if (error && __ratelimit(&drbd_ratelimit_state))
 		drbd_warn(device, "%s: error=%d s=%llus\n",
-				is_write ? "write" : "read", error,
+				is_write ? (is_discard ? "discard" : "write")
+					: "read", error,
 				(unsigned long long)peer_req->i.sector);
 	if (!error && !uptodate) {
 		if (__ratelimit(&drbd_ratelimit_state))
@@ -263,7 +267,12 @@ void drbd_request_endio(struct bio *bio, int error)
 
 	/* to avoid recursion in __req_mod */
 	if (unlikely(error)) {
-		what = (bio_data_dir(bio) == WRITE)
+		if (bio->bi_rw & REQ_DISCARD)
+			what = (error == -EOPNOTSUPP)
+				? DISCARD_COMPLETED_NOTSUPP
+				: DISCARD_COMPLETED_WITH_ERROR;
+		else
+			what = (bio_data_dir(bio) == WRITE)
 			? WRITE_COMPLETED_WITH_ERROR
 			: (bio_rw(bio) == READ)
 			  ? READ_COMPLETED_WITH_ERROR
@@ -395,7 +404,7 @@ static int read_for_csum(struct drbd_peer_device *peer_device, sector_t sector,
 	/* GFP_TRY, because if there is no memory available right now, this may
 	 * be rescheduled for later. It is "only" background resync, after all. */
 	peer_req = drbd_alloc_peer_req(peer_device, ID_SYNCER /* unused */, sector,
-				       size, GFP_TRY);
+				       size, true /* has real payload */, GFP_TRY);
 	if (!peer_req)
 		goto defer;
 
@@ -492,10 +501,9 @@ struct fifo_buffer *fifo_alloc(int fifo_size)
 	return fb;
 }
 
-static int drbd_rs_controller(struct drbd_device *device)
+static int drbd_rs_controller(struct drbd_device *device, unsigned int sect_in)
 {
 	struct disk_conf *dc;
-	unsigned int sect_in;  /* Number of sectors that came in since the last turn */
 	unsigned int want;     /* The number of sectors we want in the proxy */
 	int req_sect; /* Number of sectors to request in this turn */
 	int correction; /* Number of sectors more we need in the proxy*/
@@ -505,9 +513,6 @@ static int drbd_rs_controller(struct drbd_device *device)
 	int max_sect;
 	struct fifo_buffer *plan;
 
-	sect_in = atomic_xchg(&device->rs_sect_in, 0); /* Number of sectors that came in */
-	device->rs_in_flight -= sect_in;
-
 	dc = rcu_dereference(device->ldev->disk_conf);
 	plan = rcu_dereference(device->rs_plan_s);
 
@@ -550,11 +555,16 @@ static int drbd_rs_controller(struct drbd_device *device)
 
 static int drbd_rs_number_requests(struct drbd_device *device)
 {
-	int number;
+	unsigned int sect_in;  /* Number of sectors that came in since the last turn */
+	int number, mxb;
+
+	sect_in = atomic_xchg(&device->rs_sect_in, 0);
+	device->rs_in_flight -= sect_in;
 
 	rcu_read_lock();
+	mxb = drbd_get_max_buffers(device) / 2;
 	if (rcu_dereference(device->rs_plan_s)->size) {
-		number = drbd_rs_controller(device) >> (BM_BLOCK_SHIFT - 9);
+		number = drbd_rs_controller(device, sect_in) >> (BM_BLOCK_SHIFT - 9);
 		device->c_sync_rate = number * HZ * (BM_BLOCK_SIZE / 1024) / SLEEP_TIME;
 	} else {
 		device->c_sync_rate = rcu_dereference(device->ldev->disk_conf)->resync_rate;
@@ -562,8 +572,14 @@ static int drbd_rs_number_requests(struct drbd_device *device)
 	}
 	rcu_read_unlock();
 
-	/* ignore the amount of pending requests, the resync controller should
-	 * throttle down to incoming reply rate soon enough anyways. */
+	/* Don't have more than "max-buffers"/2 in-flight.
+	 * Otherwise we may cause the remote site to stall on drbd_alloc_pages(),
+	 * potentially causing a distributed deadlock on congestion during
+	 * online-verify or (checksum-based) resync, if max-buffers,
+	 * socket buffer sizes and resync rate settings are mis-configured. */
+	if (mxb - device->rs_in_flight < number)
+		number = mxb - device->rs_in_flight;
+
 	return number;
 }
 
@@ -597,7 +613,7 @@ static int make_resync_request(struct drbd_device *device, int cancel)
 
 	max_bio_size = queue_max_hw_sectors(device->rq_queue) << 9;
 	number = drbd_rs_number_requests(device);
-	if (number == 0)
+	if (number <= 0)
 		goto requeue;
 
 	for (i = 0; i < number; i++) {
@@ -647,7 +663,7 @@ next_sector:
 		 */
 		align = 1;
 		rollback_i = i;
-		for (;;) {
+		while (i < number) {
 			if (size + BM_BLOCK_SIZE > max_bio_size)
 				break;
 
@@ -1670,11 +1686,15 @@ void drbd_start_resync(struct drbd_device *device, enum drbd_conns side)
 	}
 	clear_bit(B_RS_H_DONE, &device->flags);
 
-	write_lock_irq(&global_state_lock);
+	/* req_lock: serialize with drbd_send_and_submit() and others
+	 * global_state_lock: for stable sync-after dependencies */
+	spin_lock_irq(&device->resource->req_lock);
+	write_lock(&global_state_lock);
 	/* Did some connection breakage or IO error race with us? */
 	if (device->state.conn < C_CONNECTED
 	|| !get_ldev_if_state(device, D_NEGOTIATING)) {
-		write_unlock_irq(&global_state_lock);
+		write_unlock(&global_state_lock);
+		spin_unlock_irq(&device->resource->req_lock);
 		mutex_unlock(device->state_mutex);
 		return;
 	}
@@ -1714,7 +1734,8 @@ void drbd_start_resync(struct drbd_device *device, enum drbd_conns side)
 		}
 		_drbd_pause_after(device);
 	}
-	write_unlock_irq(&global_state_lock);
+	write_unlock(&global_state_lock);
+	spin_unlock_irq(&device->resource->req_lock);
 
 	if (r == SS_SUCCESS) {
 		/* reset rs_last_bcast when a resync or verify is started,
@@ -1778,34 +1799,6 @@ void drbd_start_resync(struct drbd_device *device, enum drbd_conns side)
 	mutex_unlock(device->state_mutex);
 }
 
-/* If the resource already closed the current epoch, but we did not
- * (because we have not yet seen new requests), we should send the
- * corresponding barrier now.  Must be checked within the same spinlock
- * that is used to check for new requests. */
-static bool need_to_send_barrier(struct drbd_connection *connection)
-{
-	if (!connection->send.seen_any_write_yet)
-		return false;
-
-	/* Skip barriers that do not contain any writes.
-	 * This may happen during AHEAD mode. */
-	if (!connection->send.current_epoch_writes)
-		return false;
-
-	/* ->req_lock is held when requests are queued on
-	 * connection->sender_work, and put into ->transfer_log.
-	 * It is also held when ->current_tle_nr is increased.
-	 * So either there are already new requests queued,
-	 * and corresponding barriers will be send there.
-	 * Or nothing new is queued yet, so the difference will be 1.
-	 */
-	if (atomic_read(&connection->current_tle_nr) !=
-	    connection->send.current_epoch_nr + 1)
-		return false;
-
-	return true;
-}
-
 static bool dequeue_work_batch(struct drbd_work_queue *queue, struct list_head *work_list)
 {
 	spin_lock_irq(&queue->q_lock);
@@ -1864,12 +1857,22 @@ static void wait_for_work(struct drbd_connection *connection, struct list_head *
 			spin_unlock_irq(&connection->resource->req_lock);
 			break;
 		}
-		send_barrier = need_to_send_barrier(connection);
+
+		/* We found nothing new to do, no to-be-communicated request,
+		 * no other work item.  We may still need to close the last
+		 * epoch.  Next incoming request epoch will be connection ->
+		 * current transfer log epoch number.  If that is different
+		 * from the epoch of the last request we communicated, it is
+		 * safe to send the epoch separating barrier now.
+		 */
+		send_barrier =
+			atomic_read(&connection->current_tle_nr) !=
+			connection->send.current_epoch_nr;
 		spin_unlock_irq(&connection->resource->req_lock);
-		if (send_barrier) {
-			drbd_send_barrier(connection);
-			connection->send.current_epoch_nr++;
-		}
+
+		if (send_barrier)
+			maybe_send_barrier(connection,
+					connection->send.current_epoch_nr + 1);
 		schedule();
 		/* may be woken up for other things but new work, too,
 		 * e.g. if the current epoch got closed.
diff --git a/drivers/block/drbd/drbd_wrappers.h b/drivers/block/drbd/drbd_wrappers.h
deleted file mode 100644
index 3db9ebaf64f..00000000000
--- a/drivers/block/drbd/drbd_wrappers.h
+++ /dev/null
@@ -1,54 +0,0 @@
-#ifndef _DRBD_WRAPPERS_H
-#define _DRBD_WRAPPERS_H
-
-#include <linux/ctype.h>
-#include <linux/mm.h>
-#include "drbd_int.h"
-
-/* see get_sb_bdev and bd_claim */
-extern char *drbd_sec_holder;
-
-/* sets the number of 512 byte sectors of our virtual device */
-static inline void drbd_set_my_capacity(struct drbd_device *device,
-					sector_t size)
-{
-	/* set_capacity(device->this_bdev->bd_disk, size); */
-	set_capacity(device->vdisk, size);
-	device->this_bdev->bd_inode->i_size = (loff_t)size << 9;
-}
-
-#define drbd_bio_uptodate(bio) bio_flagged(bio, BIO_UPTODATE)
-
-/* bi_end_io handlers */
-extern void drbd_md_io_complete(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);
-
-/*
- * used to submit our private bio
- */
-static inline void drbd_generic_make_request(struct drbd_device *device,
-					     int fault_type, struct bio *bio)
-{
-	__release(local);
-	if (!bio->bi_bdev) {
-		printk(KERN_ERR "drbd%d: drbd_generic_make_request: "
-				"bio->bi_bdev == NULL\n",
-		       device_to_minor(device));
-		dump_stack();
-		bio_endio(bio, -ENODEV);
-		return;
-	}
-
-	if (drbd_insert_fault(device, fault_type))
-		bio_endio(bio, -EIO);
-	else
-		generic_make_request(bio);
-}
-
-#ifndef __CHECKER__
-# undef __cond_lock
-# define __cond_lock(x,c) (c)
-#endif
-
-#endif
diff --git a/drivers/block/floppy.c b/drivers/block/floppy.c
index fa9bb742df6..677db049f55 100644
--- a/drivers/block/floppy.c
+++ b/drivers/block/floppy.c
@@ -2351,7 +2351,7 @@ static void rw_interrupt(void)
 	}
 
 	if (CT(COMMAND) != FD_READ ||
-	    raw_cmd->kernel_data == current_req->buffer) {
+	    raw_cmd->kernel_data == bio_data(current_req->bio)) {
 		/* transfer directly from buffer */
 		cont->done(1);
 	} else if (CT(COMMAND) == FD_READ) {
@@ -2640,7 +2640,7 @@ static int make_raw_rw_request(void)
 		raw_cmd->flags &= ~FD_RAW_WRITE;
 		raw_cmd->flags |= FD_RAW_READ;
 		COMMAND = FM_MODE(_floppy, FD_READ);
-	} else if ((unsigned long)current_req->buffer < MAX_DMA_ADDRESS) {
+	} else if ((unsigned long)bio_data(current_req->bio) < MAX_DMA_ADDRESS) {
 		unsigned long dma_limit;
 		int direct, indirect;
 
@@ -2654,13 +2654,13 @@ static int make_raw_rw_request(void)
 		 */
 		max_size = buffer_chain_size();
 		dma_limit = (MAX_DMA_ADDRESS -
-			     ((unsigned long)current_req->buffer)) >> 9;
+			     ((unsigned long)bio_data(current_req->bio))) >> 9;
 		if ((unsigned long)max_size > dma_limit)
 			max_size = dma_limit;
 		/* 64 kb boundaries */
-		if (CROSS_64KB(current_req->buffer, max_size << 9))
+		if (CROSS_64KB(bio_data(current_req->bio), max_size << 9))
 			max_size = (K_64 -
-				    ((unsigned long)current_req->buffer) %
+				    ((unsigned long)bio_data(current_req->bio)) %
 				    K_64) >> 9;
 		direct = transfer_size(ssize, max_sector, max_size) - fsector_t;
 		/*
@@ -2677,7 +2677,7 @@ static int make_raw_rw_request(void)
 		       (DP->read_track & (1 << DRS->probed_format)))))) {
 			max_size = blk_rq_sectors(current_req);
 		} else {
-			raw_cmd->kernel_data = current_req->buffer;
+			raw_cmd->kernel_data = bio_data(current_req->bio);
 			raw_cmd->length = current_count_sectors << 9;
 			if (raw_cmd->length == 0) {
 				DPRINT("%s: zero dma transfer attempted\n", __func__);
@@ -2731,7 +2731,7 @@ static int make_raw_rw_request(void)
 	raw_cmd->length = ((raw_cmd->length - 1) | (ssize - 1)) + 1;
 	raw_cmd->length <<= 9;
 	if ((raw_cmd->length < current_count_sectors << 9) ||
-	    (raw_cmd->kernel_data != current_req->buffer &&
+	    (raw_cmd->kernel_data != bio_data(current_req->bio) &&
 	     CT(COMMAND) == FD_WRITE &&
 	     (aligned_sector_t + (raw_cmd->length >> 9) > buffer_max ||
 	      aligned_sector_t < buffer_min)) ||
@@ -2739,7 +2739,7 @@ static int make_raw_rw_request(void)
 	    raw_cmd->length <= 0 || current_count_sectors <= 0) {
 		DPRINT("fractionary current count b=%lx s=%lx\n",
 		       raw_cmd->length, current_count_sectors);
-		if (raw_cmd->kernel_data != current_req->buffer)
+		if (raw_cmd->kernel_data != bio_data(current_req->bio))
 			pr_info("addr=%d, length=%ld\n",
 				(int)((raw_cmd->kernel_data -
 				       floppy_track_buffer) >> 9),
@@ -2756,7 +2756,7 @@ static int make_raw_rw_request(void)
 		return 0;
 	}
 
-	if (raw_cmd->kernel_data != current_req->buffer) {
+	if (raw_cmd->kernel_data != bio_data(current_req->bio)) {
 		if (raw_cmd->kernel_data < floppy_track_buffer ||
 		    current_count_sectors < 0 ||
 		    raw_cmd->length < 0 ||
@@ -3812,7 +3812,7 @@ static int __floppy_read_block_0(struct block_device *bdev, int drive)
 	bio.bi_iter.bi_size = size;
 	bio.bi_bdev = bdev;
 	bio.bi_iter.bi_sector = 0;
-	bio.bi_flags = (1 << BIO_QUIET);
+	bio.bi_flags |= (1 << BIO_QUIET);
 	bio.bi_private = &cbdata;
 	bio.bi_end_io = floppy_rb0_cb;
 
diff --git a/drivers/block/hd.c b/drivers/block/hd.c
index bf397bf108b..8a290c08262 100644
--- a/drivers/block/hd.c
+++ b/drivers/block/hd.c
@@ -464,11 +464,11 @@ static void read_intr(void)
 
 ok_to_read:
 	req = hd_req;
-	insw(HD_DATA, req->buffer, 256);
+	insw(HD_DATA, bio_data(req->bio), 256);
 #ifdef DEBUG
 	printk("%s: read: sector %ld, remaining = %u, buffer=%p\n",
 	       req->rq_disk->disk_name, blk_rq_pos(req) + 1,
-	       blk_rq_sectors(req) - 1, req->buffer+512);
+	       blk_rq_sectors(req) - 1, bio_data(req->bio)+512);
 #endif
 	if (hd_end_request(0, 512)) {
 		SET_HANDLER(&read_intr);
@@ -505,7 +505,7 @@ static void write_intr(void)
 ok_to_write:
 	if (hd_end_request(0, 512)) {
 		SET_HANDLER(&write_intr);
-		outsw(HD_DATA, req->buffer, 256);
+		outsw(HD_DATA, bio_data(req->bio), 256);
 		return;
 	}
 
@@ -624,7 +624,7 @@ repeat:
 	printk("%s: %sing: CHS=%d/%d/%d, sectors=%d, buffer=%p\n",
 		req->rq_disk->disk_name,
 		req_data_dir(req) == READ ? "read" : "writ",
-		cyl, head, sec, nsect, req->buffer);
+		cyl, head, sec, nsect, bio_data(req->bio));
 #endif
 	if (req->cmd_type == REQ_TYPE_FS) {
 		switch (rq_data_dir(req)) {
@@ -643,7 +643,7 @@ repeat:
 				bad_rw_intr();
 				goto repeat;
 			}
-			outsw(HD_DATA, req->buffer, 256);
+			outsw(HD_DATA, bio_data(req->bio), 256);
 			break;
 		default:
 			printk("unknown hd-command\n");
diff --git a/drivers/block/loop.c b/drivers/block/loop.c
index f70a230a294..6cb1beb47c2 100644
--- a/drivers/block/loop.c
+++ b/drivers/block/loop.c
@@ -548,7 +548,7 @@ static int loop_thread(void *data)
 	struct loop_device *lo = data;
 	struct bio *bio;
 
-	set_user_nice(current, -20);
+	set_user_nice(current, MIN_NICE);
 
 	while (!kthread_should_stop() || !bio_list_empty(&lo->lo_bio_list)) {
 
diff --git a/drivers/block/mg_disk.c b/drivers/block/mg_disk.c
index eb59b124136..e352cac707e 100644
--- a/drivers/block/mg_disk.c
+++ b/drivers/block/mg_disk.c
@@ -479,7 +479,7 @@ static unsigned int mg_out(struct mg_host *host,
 
 static void mg_read_one(struct mg_host *host, struct request *req)
 {
-	u16 *buff = (u16 *)req->buffer;
+	u16 *buff = (u16 *)bio_data(req->bio);
 	u32 i;
 
 	for (i = 0; i < MG_SECTOR_SIZE >> 1; i++)
@@ -496,7 +496,7 @@ static void mg_read(struct request *req)
 		mg_bad_rw_intr(host);
 
 	MG_DBG("requested %d sects (from %ld), buffer=0x%p\n",
-	       blk_rq_sectors(req), blk_rq_pos(req), req->buffer);
+	       blk_rq_sectors(req), blk_rq_pos(req), bio_data(req->bio));
 
 	do {
 		if (mg_wait(host, ATA_DRQ,
@@ -514,7 +514,7 @@ static void mg_read(struct request *req)
 
 static void mg_write_one(struct mg_host *host, struct request *req)
 {
-	u16 *buff = (u16 *)req->buffer;
+	u16 *buff = (u16 *)bio_data(req->bio);
 	u32 i;
 
 	for (i = 0; i < MG_SECTOR_SIZE >> 1; i++)
@@ -534,7 +534,7 @@ static void mg_write(struct request *req)
 	}
 
 	MG_DBG("requested %d sects (from %ld), buffer=0x%p\n",
-	       rem, blk_rq_pos(req), req->buffer);
+	       rem, blk_rq_pos(req), bio_data(req->bio));
 
 	if (mg_wait(host, ATA_DRQ,
 		    MG_TMAX_WAIT_WR_DRQ) != MG_ERR_NONE) {
@@ -585,7 +585,7 @@ ok_to_read:
 	mg_read_one(host, req);
 
 	MG_DBG("sector %ld, remaining=%ld, buffer=0x%p\n",
-	       blk_rq_pos(req), blk_rq_sectors(req) - 1, req->buffer);
+	       blk_rq_pos(req), blk_rq_sectors(req) - 1, bio_data(req->bio));
 
 	/* send read confirm */
 	outb(MG_CMD_RD_CONF, (unsigned long)host->dev_base + MG_REG_COMMAND);
@@ -624,7 +624,7 @@ ok_to_write:
 		/* write 1 sector and set handler if remains */
 		mg_write_one(host, req);
 		MG_DBG("sector %ld, remaining=%ld, buffer=0x%p\n",
-		       blk_rq_pos(req), blk_rq_sectors(req), req->buffer);
+		       blk_rq_pos(req), blk_rq_sectors(req), bio_data(req->bio));
 		host->mg_do_intr = mg_write_intr;
 		mod_timer(&host->timer, jiffies + 3 * HZ);
 	}
diff --git a/drivers/block/mtip32xx/mtip32xx.c b/drivers/block/mtip32xx/mtip32xx.c
index 59c5abe32f0..295f3afbbef 100644
--- a/drivers/block/mtip32xx/mtip32xx.c
+++ b/drivers/block/mtip32xx/mtip32xx.c
@@ -31,6 +31,7 @@
 #include <linux/module.h>
 #include <linux/genhd.h>
 #include <linux/blkdev.h>
+#include <linux/blk-mq.h>
 #include <linux/bio.h>
 #include <linux/dma-mapping.h>
 #include <linux/idr.h>
@@ -38,6 +39,7 @@
 #include <../drivers/ata/ahci.h>
 #include <linux/export.h>
 #include <linux/debugfs.h>
+#include <linux/prefetch.h>
 #include "mtip32xx.h"
 
 #define HW_CMD_SLOT_SZ		(MTIP_MAX_COMMAND_SLOTS * 32)
@@ -173,60 +175,36 @@ static bool mtip_check_surprise_removal(struct pci_dev *pdev)
 	return false; /* device present */
 }
 
-/*
- * Obtain an empty command slot.
- *
- * This function needs to be reentrant since it could be called
- * at the same time on multiple CPUs. The allocation of the
- * command slot must be atomic.
- *
- * @port Pointer to the port data structure.
- *
- * return value
- *	>= 0	Index of command slot obtained.
- *	-1	No command slots available.
- */
-static int get_slot(struct mtip_port *port)
+static struct mtip_cmd *mtip_get_int_command(struct driver_data *dd)
 {
-	int slot, i;
-	unsigned int num_command_slots = port->dd->slot_groups * 32;
+	struct request *rq;
 
-	/*
-	 * Try 10 times, because there is a small race here.
-	 *  that's ok, because it's still cheaper than a lock.
-	 *
-	 * Race: Since this section is not protected by lock, same bit
-	 * could be chosen by different process contexts running in
-	 * different processor. So instead of costly lock, we are going
-	 * with loop.
-	 */
-	for (i = 0; i < 10; i++) {
-		slot = find_next_zero_bit(port->allocated,
-					 num_command_slots, 1);
-		if ((slot < num_command_slots) &&
-		    (!test_and_set_bit(slot, port->allocated)))
-			return slot;
-	}
-	dev_warn(&port->dd->pdev->dev, "Failed to get a tag.\n");
+	rq = blk_mq_alloc_request(dd->queue, 0, __GFP_WAIT, true);
+	return blk_mq_rq_to_pdu(rq);
+}
 
-	mtip_check_surprise_removal(port->dd->pdev);
-	return -1;
+static void mtip_put_int_command(struct driver_data *dd, struct mtip_cmd *cmd)
+{
+	blk_put_request(blk_mq_rq_from_pdu(cmd));
 }
 
 /*
- * Release a command slot.
- *
- * @port Pointer to the port data structure.
- * @tag  Tag of command to release
- *
- * return value
- *	None
+ * Once we add support for one hctx per mtip group, this will change a bit
  */
-static inline void release_slot(struct mtip_port *port, int tag)
+static struct request *mtip_rq_from_tag(struct driver_data *dd,
+					unsigned int tag)
+{
+	struct blk_mq_hw_ctx *hctx = dd->queue->queue_hw_ctx[0];
+
+	return blk_mq_tag_to_rq(hctx->tags, tag);
+}
+
+static struct mtip_cmd *mtip_cmd_from_tag(struct driver_data *dd,
+					  unsigned int tag)
 {
-	smp_mb__before_clear_bit();
-	clear_bit(tag, port->allocated);
-	smp_mb__after_clear_bit();
+	struct request *rq = mtip_rq_from_tag(dd, tag);
+
+	return blk_mq_rq_to_pdu(rq);
 }
 
 /*
@@ -248,93 +226,28 @@ static inline void release_slot(struct mtip_port *port, int tag)
  *	None
  */
 static void mtip_async_complete(struct mtip_port *port,
-				int tag,
-				void *data,
-				int status)
+				int tag, struct mtip_cmd *cmd, int status)
 {
-	struct mtip_cmd *cmd;
-	struct driver_data *dd = data;
-	int unaligned, cb_status = status ? -EIO : 0;
-	void (*func)(void *, int);
+	struct driver_data *dd = port->dd;
+	struct request *rq;
 
 	if (unlikely(!dd) || unlikely(!port))
 		return;
 
-	cmd = &port->commands[tag];
-
 	if (unlikely(status == PORT_IRQ_TF_ERR)) {
 		dev_warn(&port->dd->pdev->dev,
 			"Command tag %d failed due to TFE\n", tag);
 	}
 
-	/* Clear the active flag */
-	atomic_set(&port->commands[tag].active, 0);
-
-	/* Upper layer callback */
-	func = cmd->async_callback;
-	if (likely(func && cmpxchg(&cmd->async_callback, func, 0) == func)) {
+	/* Unmap the DMA scatter list entries */
+	dma_unmap_sg(&dd->pdev->dev, cmd->sg, cmd->scatter_ents, cmd->direction);
 
-		/* Unmap the DMA scatter list entries */
-		dma_unmap_sg(&dd->pdev->dev,
-			cmd->sg,
-			cmd->scatter_ents,
-			cmd->direction);
+	rq = mtip_rq_from_tag(dd, tag);
 
-		func(cmd->async_data, cb_status);
-		unaligned = cmd->unaligned;
+	if (unlikely(cmd->unaligned))
+		up(&port->cmd_slot_unal);
 
-		/* Clear the allocated bit for the command */
-		release_slot(port, tag);
-
-		if (unlikely(unaligned))
-			up(&port->cmd_slot_unal);
-		else
-			up(&port->cmd_slot);
-	}
-}
-
-/*
- * This function is called for clean the pending command in the
- * command slot during the surprise removal of device and return
- * error to the upper layer.
- *
- * @dd Pointer to the DRIVER_DATA structure.
- *
- * return value
- *	None
- */
-static void mtip_command_cleanup(struct driver_data *dd)
-{
-	int tag = 0;
-	struct mtip_cmd *cmd;
-	struct mtip_port *port = dd->port;
-	unsigned int num_cmd_slots = dd->slot_groups * 32;
-
-	if (!test_bit(MTIP_DDF_INIT_DONE_BIT, &dd->dd_flag))
-		return;
-
-	if (!port)
-		return;
-
-	cmd = &port->commands[MTIP_TAG_INTERNAL];
-	if (atomic_read(&cmd->active))
-		if (readl(port->cmd_issue[MTIP_TAG_INTERNAL]) &
-					(1 << MTIP_TAG_INTERNAL))
-			if (cmd->comp_func)
-				cmd->comp_func(port, MTIP_TAG_INTERNAL,
-					 cmd->comp_data, -ENODEV);
-
-	while (1) {
-		tag = find_next_bit(port->allocated, num_cmd_slots, tag);
-		if (tag >= num_cmd_slots)
-			break;
-
-		cmd = &port->commands[tag];
-		if (atomic_read(&cmd->active))
-			mtip_async_complete(port, tag, dd, -ENODEV);
-	}
-
-	set_bit(MTIP_DDF_CLEANUP_BIT, &dd->dd_flag);
+	blk_mq_end_io(rq, status ? -EIO : 0);
 }
 
 /*
@@ -388,8 +301,6 @@ static inline void mtip_issue_ncq_command(struct mtip_port *port, int tag)
 {
 	int group = tag >> 5;
 
-	atomic_set(&port->commands[tag].active, 1);
-
 	/* guard SACT and CI registers */
 	spin_lock(&port->cmd_issue_lock[group]);
 	writel((1 << MTIP_TAG_BIT(tag)),
@@ -397,10 +308,6 @@ static inline void mtip_issue_ncq_command(struct mtip_port *port, int tag)
 	writel((1 << MTIP_TAG_BIT(tag)),
 			port->cmd_issue[MTIP_TAG_INDEX(tag)]);
 	spin_unlock(&port->cmd_issue_lock[group]);
-
-	/* Set the command's timeout value.*/
-	port->commands[tag].comp_time = jiffies + msecs_to_jiffies(
-					MTIP_NCQ_COMMAND_TIMEOUT_MS);
 }
 
 /*
@@ -648,132 +555,13 @@ static void print_tags(struct driver_data *dd,
 
 	memset(tagmap, 0, sizeof(tagmap));
 	for (group = SLOTBITS_IN_LONGS; group > 0; group--)
-		tagmap_len = sprintf(tagmap + tagmap_len, "%016lX ",
+		tagmap_len += sprintf(tagmap + tagmap_len, "%016lX ",
 						tagbits[group-1]);
 	dev_warn(&dd->pdev->dev,
 			"%d command(s) %s: tagmap [%s]", cnt, msg, tagmap);
 }
 
 /*
- * Called periodically to see if any read/write commands are
- * taking too long to complete.
- *
- * @data Pointer to the PORT data structure.
- *
- * return value
- *	None
- */
-static void mtip_timeout_function(unsigned long int data)
-{
-	struct mtip_port *port = (struct mtip_port *) data;
-	struct host_to_dev_fis *fis;
-	struct mtip_cmd *cmd;
-	int unaligned, tag, cmdto_cnt = 0;
-	unsigned int bit, group;
-	unsigned int num_command_slots;
-	unsigned long to, tagaccum[SLOTBITS_IN_LONGS];
-	void (*func)(void *, int);
-
-	if (unlikely(!port))
-		return;
-
-	if (unlikely(port->dd->sr))
-		return;
-
-	if (test_bit(MTIP_DDF_RESUME_BIT, &port->dd->dd_flag)) {
-		mod_timer(&port->cmd_timer,
-			jiffies + msecs_to_jiffies(30000));
-		return;
-	}
-	/* clear the tag accumulator */
-	memset(tagaccum, 0, SLOTBITS_IN_LONGS * sizeof(long));
-	num_command_slots = port->dd->slot_groups * 32;
-
-	for (tag = 0; tag < num_command_slots; tag++) {
-		/*
-		 * Skip internal command slot as it has
-		 * its own timeout mechanism
-		 */
-		if (tag == MTIP_TAG_INTERNAL)
-			continue;
-
-		if (atomic_read(&port->commands[tag].active) &&
-		   (time_after(jiffies, port->commands[tag].comp_time))) {
-			group = tag >> 5;
-			bit = tag & 0x1F;
-
-			cmd = &port->commands[tag];
-			fis = (struct host_to_dev_fis *) cmd->command;
-
-			set_bit(tag, tagaccum);
-			cmdto_cnt++;
-			if (cmdto_cnt == 1)
-				set_bit(MTIP_PF_EH_ACTIVE_BIT, &port->flags);
-
-			/*
-			 * Clear the completed bit. This should prevent
-			 *  any interrupt handlers from trying to retire
-			 *  the command.
-			 */
-			writel(1 << bit, port->completed[group]);
-
-			/* Clear the active flag for the command */
-			atomic_set(&port->commands[tag].active, 0);
-
-			func = cmd->async_callback;
-			if (func &&
-			    cmpxchg(&cmd->async_callback, func, 0) == func) {
-
-				/* Unmap the DMA scatter list entries */
-				dma_unmap_sg(&port->dd->pdev->dev,
-						cmd->sg,
-						cmd->scatter_ents,
-						cmd->direction);
-
-				func(cmd->async_data, -EIO);
-				unaligned = cmd->unaligned;
-
-				/* Clear the allocated bit for the command. */
-				release_slot(port, tag);
-
-				if (unaligned)
-					up(&port->cmd_slot_unal);
-				else
-					up(&port->cmd_slot);
-			}
-		}
-	}
-
-	if (cmdto_cnt) {
-		print_tags(port->dd, "timed out", tagaccum, cmdto_cnt);
-		if (!test_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags)) {
-			mtip_device_reset(port->dd);
-			wake_up_interruptible(&port->svc_wait);
-		}
-		clear_bit(MTIP_PF_EH_ACTIVE_BIT, &port->flags);
-	}
-
-	if (port->ic_pause_timer) {
-		to  = port->ic_pause_timer + msecs_to_jiffies(1000);
-		if (time_after(jiffies, to)) {
-			if (!test_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags)) {
-				port->ic_pause_timer = 0;
-				clear_bit(MTIP_PF_SE_ACTIVE_BIT, &port->flags);
-				clear_bit(MTIP_PF_DM_ACTIVE_BIT, &port->flags);
-				clear_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags);
-				wake_up_interruptible(&port->svc_wait);
-			}
-
-
-		}
-	}
-
-	/* Restart the timer */
-	mod_timer(&port->cmd_timer,
-		jiffies + msecs_to_jiffies(MTIP_TIMEOUT_CHECK_PERIOD));
-}
-
-/*
  * Internal command completion callback function.
  *
  * This function is normally called by the driver ISR when an internal
@@ -789,28 +577,19 @@ static void mtip_timeout_function(unsigned long int data)
  *	None
  */
 static void mtip_completion(struct mtip_port *port,
-			    int tag,
-			    void *data,
-			    int status)
+			    int tag, struct mtip_cmd *command, int status)
 {
-	struct mtip_cmd *command = &port->commands[tag];
-	struct completion *waiting = data;
+	struct completion *waiting = command->comp_data;
 	if (unlikely(status == PORT_IRQ_TF_ERR))
 		dev_warn(&port->dd->pdev->dev,
 			"Internal command %d completed with TFE\n", tag);
 
-	command->async_callback = NULL;
-	command->comp_func = NULL;
-
 	complete(waiting);
 }
 
 static void mtip_null_completion(struct mtip_port *port,
-			    int tag,
-			    void *data,
-			    int status)
+			    int tag, struct mtip_cmd *command, int status)
 {
-	return;
 }
 
 static int mtip_read_log_page(struct mtip_port *port, u8 page, u16 *buffer,
@@ -842,19 +621,16 @@ static void mtip_handle_tfe(struct driver_data *dd)
 
 	port = dd->port;
 
-	/* Stop the timer to prevent command timeouts. */
-	del_timer(&port->cmd_timer);
 	set_bit(MTIP_PF_EH_ACTIVE_BIT, &port->flags);
 
 	if (test_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags) &&
 			test_bit(MTIP_TAG_INTERNAL, port->allocated)) {
-		cmd = &port->commands[MTIP_TAG_INTERNAL];
+		cmd = mtip_cmd_from_tag(dd, MTIP_TAG_INTERNAL);
 		dbg_printk(MTIP_DRV_NAME " TFE for the internal command\n");
 
-		atomic_inc(&cmd->active); /* active > 1 indicates error */
 		if (cmd->comp_data && cmd->comp_func) {
 			cmd->comp_func(port, MTIP_TAG_INTERNAL,
-					cmd->comp_data, PORT_IRQ_TF_ERR);
+					cmd, PORT_IRQ_TF_ERR);
 		}
 		goto handle_tfe_exit;
 	}
@@ -866,6 +642,8 @@ static void mtip_handle_tfe(struct driver_data *dd)
 	for (group = 0; group < dd->slot_groups; group++) {
 		completed = readl(port->completed[group]);
 
+		dev_warn(&dd->pdev->dev, "g=%u, comp=%x\n", group, completed);
+
 		/* clear completed status register in the hardware.*/
 		writel(completed, port->completed[group]);
 
@@ -879,15 +657,11 @@ static void mtip_handle_tfe(struct driver_data *dd)
 			if (tag == MTIP_TAG_INTERNAL)
 				continue;
 
-			cmd = &port->commands[tag];
+			cmd = mtip_cmd_from_tag(dd, tag);
 			if (likely(cmd->comp_func)) {
 				set_bit(tag, tagaccum);
 				cmd_cnt++;
-				atomic_set(&cmd->active, 0);
-				cmd->comp_func(port,
-					 tag,
-					 cmd->comp_data,
-					 0);
+				cmd->comp_func(port, tag, cmd, 0);
 			} else {
 				dev_err(&port->dd->pdev->dev,
 					"Missing completion func for tag %d",
@@ -947,11 +721,7 @@ static void mtip_handle_tfe(struct driver_data *dd)
 		for (bit = 0; bit < 32; bit++) {
 			reissue = 1;
 			tag = (group << 5) + bit;
-			cmd = &port->commands[tag];
-
-			/* If the active bit is set re-issue the command */
-			if (atomic_read(&cmd->active) == 0)
-				continue;
+			cmd = mtip_cmd_from_tag(dd, tag);
 
 			fis = (struct host_to_dev_fis *)cmd->command;
 
@@ -970,11 +740,9 @@ static void mtip_handle_tfe(struct driver_data *dd)
 					tag,
 					fail_reason != NULL ?
 						fail_reason : "unknown");
-					atomic_set(&cmd->active, 0);
 					if (cmd->comp_func) {
 						cmd->comp_func(port, tag,
-							cmd->comp_data,
-							-ENODATA);
+							cmd, -ENODATA);
 					}
 					continue;
 				}
@@ -997,14 +765,9 @@ static void mtip_handle_tfe(struct driver_data *dd)
 			/* Retire a command that will not be reissued */
 			dev_warn(&port->dd->pdev->dev,
 				"retiring tag %d\n", tag);
-			atomic_set(&cmd->active, 0);
 
 			if (cmd->comp_func)
-				cmd->comp_func(
-					port,
-					tag,
-					cmd->comp_data,
-					PORT_IRQ_TF_ERR);
+				cmd->comp_func(port, tag, cmd, PORT_IRQ_TF_ERR);
 			else
 				dev_warn(&port->dd->pdev->dev,
 					"Bad completion for tag %d\n",
@@ -1017,9 +780,6 @@ handle_tfe_exit:
 	/* clear eh_active */
 	clear_bit(MTIP_PF_EH_ACTIVE_BIT, &port->flags);
 	wake_up_interruptible(&port->svc_wait);
-
-	mod_timer(&port->cmd_timer,
-		 jiffies + msecs_to_jiffies(MTIP_TIMEOUT_CHECK_PERIOD));
 }
 
 /*
@@ -1048,15 +808,10 @@ static inline void mtip_workq_sdbfx(struct mtip_port *port, int group,
 			if (unlikely(tag == MTIP_TAG_INTERNAL))
 				continue;
 
-			command = &port->commands[tag];
-			/* make internal callback */
-			if (likely(command->comp_func)) {
-				command->comp_func(
-					port,
-					tag,
-					command->comp_data,
-					0);
-			} else {
+			command = mtip_cmd_from_tag(dd, tag);
+			if (likely(command->comp_func))
+				command->comp_func(port, tag, command, 0);
+			else {
 				dev_dbg(&dd->pdev->dev,
 					"Null completion for tag %d",
 					tag);
@@ -1081,16 +836,13 @@ static inline void mtip_workq_sdbfx(struct mtip_port *port, int group,
 static inline void mtip_process_legacy(struct driver_data *dd, u32 port_stat)
 {
 	struct mtip_port *port = dd->port;
-	struct mtip_cmd *cmd = &port->commands[MTIP_TAG_INTERNAL];
+	struct mtip_cmd *cmd = mtip_cmd_from_tag(dd, MTIP_TAG_INTERNAL);
 
 	if (test_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags) &&
 	    (cmd != NULL) && !(readl(port->cmd_issue[MTIP_TAG_INTERNAL])
 		& (1 << MTIP_TAG_INTERNAL))) {
 		if (cmd->comp_func) {
-			cmd->comp_func(port,
-				MTIP_TAG_INTERNAL,
-				cmd->comp_data,
-				0);
+			cmd->comp_func(port, MTIP_TAG_INTERNAL, cmd, 0);
 			return;
 		}
 	}
@@ -1103,8 +855,6 @@ static inline void mtip_process_legacy(struct driver_data *dd, u32 port_stat)
  */
 static inline void mtip_process_errors(struct driver_data *dd, u32 port_stat)
 {
-	if (likely(port_stat & (PORT_IRQ_TF_ERR | PORT_IRQ_IF_ERR)))
-		mtip_handle_tfe(dd);
 
 	if (unlikely(port_stat & PORT_IRQ_CONNECT)) {
 		dev_warn(&dd->pdev->dev,
@@ -1122,6 +872,12 @@ static inline void mtip_process_errors(struct driver_data *dd, u32 port_stat)
 		dev_warn(&dd->pdev->dev,
 			"Port stat errors %x unhandled\n",
 			(port_stat & ~PORT_IRQ_HANDLED));
+		if (mtip_check_surprise_removal(dd->pdev))
+			return;
+	}
+	if (likely(port_stat & (PORT_IRQ_TF_ERR | PORT_IRQ_IF_ERR))) {
+		set_bit(MTIP_PF_EH_ACTIVE_BIT, &dd->port->flags);
+		wake_up_interruptible(&dd->port->svc_wait);
 	}
 }
 
@@ -1222,7 +978,6 @@ static irqreturn_t mtip_irq_handler(int irq, void *instance)
 
 static void mtip_issue_non_ncq_command(struct mtip_port *port, int tag)
 {
-	atomic_set(&port->commands[tag].active, 1);
 	writel(1 << MTIP_TAG_BIT(tag),
 		port->cmd_issue[MTIP_TAG_INDEX(tag)]);
 }
@@ -1280,6 +1035,8 @@ static int mtip_quiesce_io(struct mtip_port *port, unsigned long timeout)
 	unsigned int n;
 	unsigned int active = 1;
 
+	blk_mq_stop_hw_queues(port->dd->queue);
+
 	to = jiffies + msecs_to_jiffies(timeout);
 	do {
 		if (test_bit(MTIP_PF_SVC_THD_ACTIVE_BIT, &port->flags) &&
@@ -1287,8 +1044,13 @@ static int mtip_quiesce_io(struct mtip_port *port, unsigned long timeout)
 			msleep(20);
 			continue; /* svc thd is actively issuing commands */
 		}
+
+		msleep(100);
+		if (mtip_check_surprise_removal(port->dd->pdev))
+			goto err_fault;
 		if (test_bit(MTIP_DDF_REMOVE_PENDING_BIT, &port->dd->dd_flag))
-			return -EFAULT;
+			goto err_fault;
+
 		/*
 		 * Ignore s_active bit 0 of array element 0.
 		 * This bit will always be set
@@ -1299,11 +1061,13 @@ static int mtip_quiesce_io(struct mtip_port *port, unsigned long timeout)
 
 		if (!active)
 			break;
-
-		msleep(20);
 	} while (time_before(jiffies, to));
 
+	blk_mq_start_stopped_hw_queues(port->dd->queue, true);
 	return active ? -EBUSY : 0;
+err_fault:
+	blk_mq_start_stopped_hw_queues(port->dd->queue, true);
+	return -EFAULT;
 }
 
 /*
@@ -1335,10 +1099,9 @@ static int mtip_exec_internal_command(struct mtip_port *port,
 {
 	struct mtip_cmd_sg *command_sg;
 	DECLARE_COMPLETION_ONSTACK(wait);
-	int rv = 0, ready2go = 1;
-	struct mtip_cmd *int_cmd = &port->commands[MTIP_TAG_INTERNAL];
-	unsigned long to;
+	struct mtip_cmd *int_cmd;
 	struct driver_data *dd = port->dd;
+	int rv = 0;
 
 	/* Make sure the buffer is 8 byte aligned. This is asic specific. */
 	if (buffer & 0x00000007) {
@@ -1346,19 +1109,8 @@ static int mtip_exec_internal_command(struct mtip_port *port,
 		return -EFAULT;
 	}
 
-	to = jiffies + msecs_to_jiffies(timeout);
-	do {
-		ready2go = !test_and_set_bit(MTIP_TAG_INTERNAL,
-						port->allocated);
-		if (ready2go)
-			break;
-		mdelay(100);
-	} while (time_before(jiffies, to));
-	if (!ready2go) {
-		dev_warn(&dd->pdev->dev,
-			"Internal cmd active. new cmd [%02X]\n", fis->command);
-		return -EBUSY;
-	}
+	int_cmd = mtip_get_int_command(dd);
+
 	set_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags);
 	port->ic_pause_timer = 0;
 
@@ -1368,10 +1120,11 @@ static int mtip_exec_internal_command(struct mtip_port *port,
 	if (atomic == GFP_KERNEL) {
 		if (fis->command != ATA_CMD_STANDBYNOW1) {
 			/* wait for io to complete if non atomic */
-			if (mtip_quiesce_io(port, 5000) < 0) {
+			if (mtip_quiesce_io(port,
+					MTIP_QUIESCE_IO_TIMEOUT_MS) < 0) {
 				dev_warn(&dd->pdev->dev,
 					"Failed to quiesce IO\n");
-				release_slot(port, MTIP_TAG_INTERNAL);
+				mtip_put_int_command(dd, int_cmd);
 				clear_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags);
 				wake_up_interruptible(&port->svc_wait);
 				return -EBUSY;
@@ -1416,9 +1169,9 @@ static int mtip_exec_internal_command(struct mtip_port *port,
 
 	if (atomic == GFP_KERNEL) {
 		/* Wait for the command to complete or timeout. */
-		if (wait_for_completion_interruptible_timeout(
+		if ((rv = wait_for_completion_interruptible_timeout(
 				&wait,
-				msecs_to_jiffies(timeout)) <= 0) {
+				msecs_to_jiffies(timeout))) <= 0) {
 			if (rv == -ERESTARTSYS) { /* interrupted */
 				dev_err(&dd->pdev->dev,
 					"Internal command [%02X] was interrupted after %lu ms\n",
@@ -1497,8 +1250,7 @@ static int mtip_exec_internal_command(struct mtip_port *port,
 	}
 exec_ic_exit:
 	/* Clear the allocated and active bits for the internal command. */
-	atomic_set(&int_cmd->active, 0);
-	release_slot(port, MTIP_TAG_INTERNAL);
+	mtip_put_int_command(dd, int_cmd);
 	if (rv >= 0 && mtip_pause_ncq(port, fis)) {
 		/* NCQ paused */
 		return rv;
@@ -1529,6 +1281,37 @@ static inline void ata_swap_string(u16 *buf, unsigned int len)
 		be16_to_cpus(&buf[i]);
 }
 
+static void mtip_set_timeout(struct driver_data *dd,
+					struct host_to_dev_fis *fis,
+					unsigned int *timeout, u8 erasemode)
+{
+	switch (fis->command) {
+	case ATA_CMD_DOWNLOAD_MICRO:
+		*timeout = 120000; /* 2 minutes */
+		break;
+	case ATA_CMD_SEC_ERASE_UNIT:
+	case 0xFC:
+		if (erasemode)
+			*timeout = ((*(dd->port->identify + 90) * 2) * 60000);
+		else
+			*timeout = ((*(dd->port->identify + 89) * 2) * 60000);
+		break;
+	case ATA_CMD_STANDBYNOW1:
+		*timeout = 120000;  /* 2 minutes */
+		break;
+	case 0xF7:
+	case 0xFA:
+		*timeout = 60000;  /* 60 seconds */
+		break;
+	case ATA_CMD_SMART:
+		*timeout = 15000;  /* 15 seconds */
+		break;
+	default:
+		*timeout = MTIP_IOCTL_CMD_TIMEOUT_MS;
+		break;
+	}
+}
+
 /*
  * Request the device identity information.
  *
@@ -1576,7 +1359,7 @@ static int mtip_get_identify(struct mtip_port *port, void __user *user_buffer)
 				sizeof(u16) * ATA_ID_WORDS,
 				0,
 				GFP_KERNEL,
-				MTIP_INTERNAL_COMMAND_TIMEOUT_MS)
+				MTIP_INT_CMD_TIMEOUT_MS)
 				< 0) {
 		rv = -1;
 		goto out;
@@ -1644,6 +1427,7 @@ static int mtip_standby_immediate(struct mtip_port *port)
 	int rv;
 	struct host_to_dev_fis	fis;
 	unsigned long start;
+	unsigned int timeout;
 
 	/* Build the FIS. */
 	memset(&fis, 0, sizeof(struct host_to_dev_fis));
@@ -1651,6 +1435,8 @@ static int mtip_standby_immediate(struct mtip_port *port)
 	fis.opts	= 1 << 7;
 	fis.command	= ATA_CMD_STANDBYNOW1;
 
+	mtip_set_timeout(port->dd, &fis, &timeout, 0);
+
 	start = jiffies;
 	rv = mtip_exec_internal_command(port,
 					&fis,
@@ -1659,7 +1445,7 @@ static int mtip_standby_immediate(struct mtip_port *port)
 					0,
 					0,
 					GFP_ATOMIC,
-					15000);
+					timeout);
 	dbg_printk(MTIP_DRV_NAME "Time taken to complete standby cmd: %d ms\n",
 			jiffies_to_msecs(jiffies - start));
 	if (rv)
@@ -1705,7 +1491,7 @@ static int mtip_read_log_page(struct mtip_port *port, u8 page, u16 *buffer,
 					sectors * ATA_SECT_SIZE,
 					0,
 					GFP_ATOMIC,
-					MTIP_INTERNAL_COMMAND_TIMEOUT_MS);
+					MTIP_INT_CMD_TIMEOUT_MS);
 }
 
 /*
@@ -1998,6 +1784,7 @@ static int exec_drive_task(struct mtip_port *port, u8 *command)
 {
 	struct host_to_dev_fis	fis;
 	struct host_to_dev_fis *reply = (port->rxfis + RX_FIS_D2H_REG);
+	unsigned int to;
 
 	/* Build the FIS. */
 	memset(&fis, 0, sizeof(struct host_to_dev_fis));
@@ -2011,6 +1798,8 @@ static int exec_drive_task(struct mtip_port *port, u8 *command)
 	fis.cyl_hi	= command[5];
 	fis.device	= command[6] & ~0x10; /* Clear the dev bit*/
 
+	mtip_set_timeout(port->dd, &fis, &to, 0);
+
 	dbg_printk(MTIP_DRV_NAME " %s: User Command: cmd %x, feat %x, nsect %x, sect %x, lcyl %x, hcyl %x, sel %x\n",
 		__func__,
 		command[0],
@@ -2029,7 +1818,7 @@ static int exec_drive_task(struct mtip_port *port, u8 *command)
 				 0,
 				 0,
 				 GFP_KERNEL,
-				 MTIP_IOCTL_COMMAND_TIMEOUT_MS) < 0) {
+				 to) < 0) {
 		return -1;
 	}
 
@@ -2069,6 +1858,7 @@ static int exec_drive_command(struct mtip_port *port, u8 *command,
 	u8 *buf = NULL;
 	dma_addr_t dma_addr = 0;
 	int rv = 0, xfer_sz = command[3];
+	unsigned int to;
 
 	if (xfer_sz) {
 		if (!user_buffer)
@@ -2100,6 +1890,8 @@ static int exec_drive_command(struct mtip_port *port, u8 *command,
 		fis.cyl_hi	= 0xC2;
 	}
 
+	mtip_set_timeout(port->dd, &fis, &to, 0);
+
 	if (xfer_sz)
 		reply = (port->rxfis + RX_FIS_PIO_SETUP);
 	else
@@ -2122,7 +1914,7 @@ static int exec_drive_command(struct mtip_port *port, u8 *command,
 				 (xfer_sz ? ATA_SECT_SIZE * xfer_sz : 0),
 				 0,
 				 GFP_KERNEL,
-				 MTIP_IOCTL_COMMAND_TIMEOUT_MS)
+				 to)
 				 < 0) {
 		rv = -EFAULT;
 		goto exit_drive_command;
@@ -2202,36 +1994,6 @@ static unsigned int implicit_sector(unsigned char command,
 	}
 	return rv;
 }
-static void mtip_set_timeout(struct driver_data *dd,
-					struct host_to_dev_fis *fis,
-					unsigned int *timeout, u8 erasemode)
-{
-	switch (fis->command) {
-	case ATA_CMD_DOWNLOAD_MICRO:
-		*timeout = 120000; /* 2 minutes */
-		break;
-	case ATA_CMD_SEC_ERASE_UNIT:
-	case 0xFC:
-		if (erasemode)
-			*timeout = ((*(dd->port->identify + 90) * 2) * 60000);
-		else
-			*timeout = ((*(dd->port->identify + 89) * 2) * 60000);
-		break;
-	case ATA_CMD_STANDBYNOW1:
-		*timeout = 120000;  /* 2 minutes */
-		break;
-	case 0xF7:
-	case 0xFA:
-		*timeout = 60000;  /* 60 seconds */
-		break;
-	case ATA_CMD_SMART:
-		*timeout = 15000;  /* 15 seconds */
-		break;
-	default:
-		*timeout = MTIP_IOCTL_COMMAND_TIMEOUT_MS;
-		break;
-	}
-}
 
 /*
  * Executes a taskfile
@@ -2606,22 +2368,23 @@ static int mtip_hw_ioctl(struct driver_data *dd, unsigned int cmd,
  * return value
  *	None
  */
-static void mtip_hw_submit_io(struct driver_data *dd, sector_t sector,
-			      int nsect, int nents, int tag, void *callback,
-			      void *data, int dir, int unaligned)
+static void mtip_hw_submit_io(struct driver_data *dd, struct request *rq,
+			      struct mtip_cmd *command, int nents,
+			      struct blk_mq_hw_ctx *hctx)
 {
 	struct host_to_dev_fis	*fis;
 	struct mtip_port *port = dd->port;
-	struct mtip_cmd *command = &port->commands[tag];
-	int dma_dir = (dir == READ) ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
-	u64 start = sector;
+	int dma_dir = rq_data_dir(rq) == READ ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
+	u64 start = blk_rq_pos(rq);
+	unsigned int nsect = blk_rq_sectors(rq);
 
 	/* Map the scatter list for DMA access */
 	nents = dma_map_sg(&dd->pdev->dev, command->sg, nents, dma_dir);
 
+	prefetch(&port->flags);
+
 	command->scatter_ents = nents;
 
-	command->unaligned = unaligned;
 	/*
 	 * The number of retries for this command before it is
 	 * reported as a failure to the upper layers.
@@ -2632,8 +2395,10 @@ static void mtip_hw_submit_io(struct driver_data *dd, sector_t sector,
 	fis = command->command;
 	fis->type        = 0x27;
 	fis->opts        = 1 << 7;
-	fis->command     =
-		(dir == READ ? ATA_CMD_FPDMA_READ : ATA_CMD_FPDMA_WRITE);
+	if (dma_dir == DMA_FROM_DEVICE)
+		fis->command = ATA_CMD_FPDMA_READ;
+	else
+		fis->command = ATA_CMD_FPDMA_WRITE;
 	fis->lba_low     = start & 0xFF;
 	fis->lba_mid     = (start >> 8) & 0xFF;
 	fis->lba_hi      = (start >> 16) & 0xFF;
@@ -2643,14 +2408,14 @@ static void mtip_hw_submit_io(struct driver_data *dd, sector_t sector,
 	fis->device	 = 1 << 6;
 	fis->features    = nsect & 0xFF;
 	fis->features_ex = (nsect >> 8) & 0xFF;
-	fis->sect_count  = ((tag << 3) | (tag >> 5));
+	fis->sect_count  = ((rq->tag << 3) | (rq->tag >> 5));
 	fis->sect_cnt_ex = 0;
 	fis->control     = 0;
 	fis->res2        = 0;
 	fis->res3        = 0;
 	fill_command_sg(dd, command, nents);
 
-	if (unaligned)
+	if (unlikely(command->unaligned))
 		fis->device |= 1 << 7;
 
 	/* Populate the command header */
@@ -2668,81 +2433,17 @@ static void mtip_hw_submit_io(struct driver_data *dd, sector_t sector,
 	command->direction = dma_dir;
 
 	/*
-	 * Set the completion function and data for the command passed
-	 * from the upper layer.
-	 */
-	command->async_data = data;
-	command->async_callback = callback;
-
-	/*
 	 * To prevent this command from being issued
 	 * if an internal command is in progress or error handling is active.
 	 */
-	if (port->flags & MTIP_PF_PAUSE_IO) {
-		set_bit(tag, port->cmds_to_issue);
+	if (unlikely(port->flags & MTIP_PF_PAUSE_IO)) {
+		set_bit(rq->tag, port->cmds_to_issue);
 		set_bit(MTIP_PF_ISSUE_CMDS_BIT, &port->flags);
 		return;
 	}
 
 	/* Issue the command to the hardware */
-	mtip_issue_ncq_command(port, tag);
-
-	return;
-}
-
-/*
- * Release a command slot.
- *
- * @dd  Pointer to the driver data structure.
- * @tag Slot tag
- *
- * return value
- *      None
- */
-static void mtip_hw_release_scatterlist(struct driver_data *dd, int tag,
-								int unaligned)
-{
-	struct semaphore *sem = unaligned ? &dd->port->cmd_slot_unal :
-							&dd->port->cmd_slot;
-	release_slot(dd->port, tag);
-	up(sem);
-}
-
-/*
- * Obtain a command slot and return its associated scatter list.
- *
- * @dd  Pointer to the driver data structure.
- * @tag Pointer to an int that will receive the allocated command
- *            slot tag.
- *
- * return value
- *	Pointer to the scatter list for the allocated command slot
- *	or NULL if no command slots are available.
- */
-static struct scatterlist *mtip_hw_get_scatterlist(struct driver_data *dd,
-						   int *tag, int unaligned)
-{
-	struct semaphore *sem = unaligned ? &dd->port->cmd_slot_unal :
-							&dd->port->cmd_slot;
-
-	/*
-	 * It is possible that, even with this semaphore, a thread
-	 * may think that no command slots are available. Therefore, we
-	 * need to make an attempt to get_slot().
-	 */
-	down(sem);
-	*tag = get_slot(dd->port);
-
-	if (unlikely(test_bit(MTIP_DDF_REMOVE_PENDING_BIT, &dd->dd_flag))) {
-		up(sem);
-		return NULL;
-	}
-	if (unlikely(*tag < 0)) {
-		up(sem);
-		return NULL;
-	}
-
-	return dd->port->commands[*tag].sg;
+	mtip_issue_ncq_command(port, rq->tag);
 }
 
 /*
@@ -3113,6 +2814,7 @@ static int mtip_free_orphan(struct driver_data *dd)
 		if (dd->queue) {
 			dd->queue->queuedata = NULL;
 			blk_cleanup_queue(dd->queue);
+			blk_mq_free_tag_set(&dd->tags);
 			dd->queue = NULL;
 		}
 	}
@@ -3270,6 +2972,11 @@ static int mtip_service_thread(void *data)
 	int ret;
 
 	while (1) {
+		if (kthread_should_stop() ||
+			test_bit(MTIP_PF_SVC_THD_STOP_BIT, &port->flags))
+			goto st_out;
+		clear_bit(MTIP_PF_SVC_THD_ACTIVE_BIT, &port->flags);
+
 		/*
 		 * the condition is to check neither an internal command is
 		 * is in progress nor error handling is active
@@ -3277,11 +2984,12 @@ static int mtip_service_thread(void *data)
 		wait_event_interruptible(port->svc_wait, (port->flags) &&
 			!(port->flags & MTIP_PF_PAUSE_IO));
 
-		if (kthread_should_stop())
-			goto st_out;
-
 		set_bit(MTIP_PF_SVC_THD_ACTIVE_BIT, &port->flags);
 
+		if (kthread_should_stop() ||
+			test_bit(MTIP_PF_SVC_THD_STOP_BIT, &port->flags))
+			goto st_out;
+
 		/* If I am an orphan, start self cleanup */
 		if (test_bit(MTIP_PF_SR_CLEANUP_BIT, &port->flags))
 			break;
@@ -3290,6 +2998,16 @@ static int mtip_service_thread(void *data)
 				&dd->dd_flag)))
 			goto st_out;
 
+restart_eh:
+		/* Demux bits: start with error handling */
+		if (test_bit(MTIP_PF_EH_ACTIVE_BIT, &port->flags)) {
+			mtip_handle_tfe(dd);
+			clear_bit(MTIP_PF_EH_ACTIVE_BIT, &port->flags);
+		}
+
+		if (test_bit(MTIP_PF_EH_ACTIVE_BIT, &port->flags))
+			goto restart_eh;
+
 		if (test_bit(MTIP_PF_ISSUE_CMDS_BIT, &port->flags)) {
 			slot = 1;
 			/* used to restrict the loop to one iteration */
@@ -3319,16 +3037,14 @@ static int mtip_service_thread(void *data)
 			}
 
 			clear_bit(MTIP_PF_ISSUE_CMDS_BIT, &port->flags);
-		} else if (test_bit(MTIP_PF_REBUILD_BIT, &port->flags)) {
+		}
+
+		if (test_bit(MTIP_PF_REBUILD_BIT, &port->flags)) {
 			if (mtip_ftl_rebuild_poll(dd) < 0)
 				set_bit(MTIP_DDF_REBUILD_FAILED_BIT,
 							&dd->dd_flag);
 			clear_bit(MTIP_PF_REBUILD_BIT, &port->flags);
 		}
-		clear_bit(MTIP_PF_SVC_THD_ACTIVE_BIT, &port->flags);
-
-		if (test_bit(MTIP_PF_SVC_THD_STOP_BIT, &port->flags))
-			goto st_out;
 	}
 
 	/* wait for pci remove to exit */
@@ -3365,7 +3081,6 @@ st_out:
  */
 static void mtip_dma_free(struct driver_data *dd)
 {
-	int i;
 	struct mtip_port *port = dd->port;
 
 	if (port->block1)
@@ -3376,13 +3091,6 @@ static void mtip_dma_free(struct driver_data *dd)
 		dmam_free_coherent(&dd->pdev->dev, AHCI_CMD_TBL_SZ,
 				port->command_list, port->command_list_dma);
 	}
-
-	for (i = 0; i < MTIP_MAX_COMMAND_SLOTS; i++) {
-		if (port->commands[i].command)
-			dmam_free_coherent(&dd->pdev->dev, CMD_DMA_ALLOC_SZ,
-				port->commands[i].command,
-				port->commands[i].command_dma);
-	}
 }
 
 /*
@@ -3396,8 +3104,6 @@ static void mtip_dma_free(struct driver_data *dd)
 static int mtip_dma_alloc(struct driver_data *dd)
 {
 	struct mtip_port *port = dd->port;
-	int i, rv = 0;
-	u32 host_cap_64 = readl(dd->mmio + HOST_CAP) & HOST_CAP_64;
 
 	/* Allocate dma memory for RX Fis, Identify, and Sector Bufffer */
 	port->block1 =
@@ -3430,41 +3136,63 @@ static int mtip_dma_alloc(struct driver_data *dd)
 	port->smart_buf     = port->block1 + AHCI_SMARTBUF_OFFSET;
 	port->smart_buf_dma = port->block1_dma + AHCI_SMARTBUF_OFFSET;
 
-	/* Setup per command SGL DMA region */
-
-	/* Point the command headers at the command tables */
-	for (i = 0; i < MTIP_MAX_COMMAND_SLOTS; i++) {
-		port->commands[i].command =
-			dmam_alloc_coherent(&dd->pdev->dev, CMD_DMA_ALLOC_SZ,
-				&port->commands[i].command_dma, GFP_KERNEL);
-		if (!port->commands[i].command) {
-			rv = -ENOMEM;
-			mtip_dma_free(dd);
-			return rv;
-		}
-		memset(port->commands[i].command, 0, CMD_DMA_ALLOC_SZ);
-
-		port->commands[i].command_header = port->command_list +
-					(sizeof(struct mtip_cmd_hdr) * i);
-		port->commands[i].command_header_dma =
-					dd->port->command_list_dma +
-					(sizeof(struct mtip_cmd_hdr) * i);
+	return 0;
+}
 
-		if (host_cap_64)
-			port->commands[i].command_header->ctbau =
-				__force_bit2int cpu_to_le32(
-				(port->commands[i].command_dma >> 16) >> 16);
+static int mtip_hw_get_identify(struct driver_data *dd)
+{
+	struct smart_attr attr242;
+	unsigned char *buf;
+	int rv;
 
-		port->commands[i].command_header->ctba =
-				__force_bit2int cpu_to_le32(
-				port->commands[i].command_dma & 0xFFFFFFFF);
+	if (mtip_get_identify(dd->port, NULL) < 0)
+		return -EFAULT;
 
-		sg_init_table(port->commands[i].sg, MTIP_MAX_SG);
+	if (*(dd->port->identify + MTIP_FTL_REBUILD_OFFSET) ==
+		MTIP_FTL_REBUILD_MAGIC) {
+		set_bit(MTIP_PF_REBUILD_BIT, &dd->port->flags);
+		return MTIP_FTL_REBUILD_MAGIC;
+	}
+	mtip_dump_identify(dd->port);
 
-		/* Mark command as currently inactive */
-		atomic_set(&dd->port->commands[i].active, 0);
+	/* check write protect, over temp and rebuild statuses */
+	rv = mtip_read_log_page(dd->port, ATA_LOG_SATA_NCQ,
+				dd->port->log_buf,
+				dd->port->log_buf_dma, 1);
+	if (rv) {
+		dev_warn(&dd->pdev->dev,
+			"Error in READ LOG EXT (10h) command\n");
+		/* non-critical error, don't fail the load */
+	} else {
+		buf = (unsigned char *)dd->port->log_buf;
+		if (buf[259] & 0x1) {
+			dev_info(&dd->pdev->dev,
+				"Write protect bit is set.\n");
+			set_bit(MTIP_DDF_WRITE_PROTECT_BIT, &dd->dd_flag);
+		}
+		if (buf[288] == 0xF7) {
+			dev_info(&dd->pdev->dev,
+				"Exceeded Tmax, drive in thermal shutdown.\n");
+			set_bit(MTIP_DDF_OVER_TEMP_BIT, &dd->dd_flag);
+		}
+		if (buf[288] == 0xBF) {
+			dev_info(&dd->pdev->dev,
+				"Drive indicates rebuild has failed.\n");
+			/* TODO */
+		}
 	}
-	return 0;
+
+	/* get write protect progess */
+	memset(&attr242, 0, sizeof(struct smart_attr));
+	if (mtip_get_smart_attr(dd->port, 242, &attr242))
+		dev_warn(&dd->pdev->dev,
+				"Unable to check write protect progress\n");
+	else
+		dev_info(&dd->pdev->dev,
+				"Write protect progress: %u%% (%u blocks)\n",
+				attr242.cur, le32_to_cpu(attr242.data));
+
+	return rv;
 }
 
 /*
@@ -3481,8 +3209,6 @@ static int mtip_hw_init(struct driver_data *dd)
 	int rv;
 	unsigned int num_command_slots;
 	unsigned long timeout, timetaken;
-	unsigned char *buf;
-	struct smart_attr attr242;
 
 	dd->mmio = pcim_iomap_table(dd->pdev)[MTIP_ABAR];
 
@@ -3513,8 +3239,6 @@ static int mtip_hw_init(struct driver_data *dd)
 	else
 		dd->unal_qdepth = 0;
 
-	/* Counting semaphore to track command slot usage */
-	sema_init(&dd->port->cmd_slot, num_command_slots - 1 - dd->unal_qdepth);
 	sema_init(&dd->port->cmd_slot_unal, dd->unal_qdepth);
 
 	/* Spinlock to prevent concurrent issue */
@@ -3599,73 +3323,16 @@ static int mtip_hw_init(struct driver_data *dd)
 	writel(readl(dd->mmio + HOST_CTL) | HOST_IRQ_EN,
 					dd->mmio + HOST_CTL);
 
-	init_timer(&dd->port->cmd_timer);
 	init_waitqueue_head(&dd->port->svc_wait);
 
-	dd->port->cmd_timer.data = (unsigned long int) dd->port;
-	dd->port->cmd_timer.function = mtip_timeout_function;
-	mod_timer(&dd->port->cmd_timer,
-		jiffies + msecs_to_jiffies(MTIP_TIMEOUT_CHECK_PERIOD));
-
-
 	if (test_bit(MTIP_DDF_REMOVE_PENDING_BIT, &dd->dd_flag)) {
 		rv = -EFAULT;
 		goto out3;
 	}
 
-	if (mtip_get_identify(dd->port, NULL) < 0) {
-		rv = -EFAULT;
-		goto out3;
-	}
-	mtip_dump_identify(dd->port);
-
-	if (*(dd->port->identify + MTIP_FTL_REBUILD_OFFSET) ==
-		MTIP_FTL_REBUILD_MAGIC) {
-		set_bit(MTIP_PF_REBUILD_BIT, &dd->port->flags);
-		return MTIP_FTL_REBUILD_MAGIC;
-	}
-
-	/* check write protect, over temp and rebuild statuses */
-	rv = mtip_read_log_page(dd->port, ATA_LOG_SATA_NCQ,
-				dd->port->log_buf,
-				dd->port->log_buf_dma, 1);
-	if (rv) {
-		dev_warn(&dd->pdev->dev,
-			"Error in READ LOG EXT (10h) command\n");
-		/* non-critical error, don't fail the load */
-	} else {
-		buf = (unsigned char *)dd->port->log_buf;
-		if (buf[259] & 0x1) {
-			dev_info(&dd->pdev->dev,
-				"Write protect bit is set.\n");
-			set_bit(MTIP_DDF_WRITE_PROTECT_BIT, &dd->dd_flag);
-		}
-		if (buf[288] == 0xF7) {
-			dev_info(&dd->pdev->dev,
-				"Exceeded Tmax, drive in thermal shutdown.\n");
-			set_bit(MTIP_DDF_OVER_TEMP_BIT, &dd->dd_flag);
-		}
-		if (buf[288] == 0xBF) {
-			dev_info(&dd->pdev->dev,
-				"Drive is in security locked state.\n");
-			set_bit(MTIP_DDF_SEC_LOCK_BIT, &dd->dd_flag);
-		}
-	}
-
-	/* get write protect progess */
-	memset(&attr242, 0, sizeof(struct smart_attr));
-	if (mtip_get_smart_attr(dd->port, 242, &attr242))
-		dev_warn(&dd->pdev->dev,
-				"Unable to check write protect progress\n");
-	else
-		dev_info(&dd->pdev->dev,
-				"Write protect progress: %u%% (%u blocks)\n",
-				attr242.cur, le32_to_cpu(attr242.data));
 	return rv;
 
 out3:
-	del_timer_sync(&dd->port->cmd_timer);
-
 	/* Disable interrupts on the HBA. */
 	writel(readl(dd->mmio + HOST_CTL) & ~HOST_IRQ_EN,
 			dd->mmio + HOST_CTL);
@@ -3685,6 +3352,22 @@ out1:
 	return rv;
 }
 
+static void mtip_standby_drive(struct driver_data *dd)
+{
+	if (dd->sr)
+		return;
+
+	/*
+	 * Send standby immediate (E0h) to the drive so that it
+	 * saves its state.
+	 */
+	if (!test_bit(MTIP_PF_REBUILD_BIT, &dd->port->flags) &&
+	    !test_bit(MTIP_DDF_SEC_LOCK_BIT, &dd->dd_flag))
+		if (mtip_standby_immediate(dd->port))
+			dev_warn(&dd->pdev->dev,
+				"STANDBY IMMEDIATE failed\n");
+}
+
 /*
  * Called to deinitialize an interface.
  *
@@ -3700,12 +3383,6 @@ static int mtip_hw_exit(struct driver_data *dd)
 	 * saves its state.
 	 */
 	if (!dd->sr) {
-		if (!test_bit(MTIP_PF_REBUILD_BIT, &dd->port->flags) &&
-		    !test_bit(MTIP_DDF_SEC_LOCK_BIT, &dd->dd_flag))
-			if (mtip_standby_immediate(dd->port))
-				dev_warn(&dd->pdev->dev,
-					"STANDBY IMMEDIATE failed\n");
-
 		/* de-initialize the port. */
 		mtip_deinit_port(dd->port);
 
@@ -3714,8 +3391,6 @@ static int mtip_hw_exit(struct driver_data *dd)
 				dd->mmio + HOST_CTL);
 	}
 
-	del_timer_sync(&dd->port->cmd_timer);
-
 	/* Release the IRQ. */
 	irq_set_affinity_hint(dd->pdev->irq, NULL);
 	devm_free_irq(&dd->pdev->dev, dd->pdev->irq, dd);
@@ -4032,100 +3707,138 @@ static const struct block_device_operations mtip_block_ops = {
  *
  * @queue Pointer to the request queue. Unused other than to obtain
  *              the driver data structure.
- * @bio   Pointer to the BIO.
+ * @rq    Pointer to the request.
  *
  */
-static void mtip_make_request(struct request_queue *queue, struct bio *bio)
+static int mtip_submit_request(struct blk_mq_hw_ctx *hctx, struct request *rq)
 {
-	struct driver_data *dd = queue->queuedata;
-	struct scatterlist *sg;
-	struct bio_vec bvec;
-	struct bvec_iter iter;
-	int nents = 0;
-	int tag = 0, unaligned = 0;
+	struct driver_data *dd = hctx->queue->queuedata;
+	struct mtip_cmd *cmd = blk_mq_rq_to_pdu(rq);
+	unsigned int nents;
 
 	if (unlikely(dd->dd_flag & MTIP_DDF_STOP_IO)) {
 		if (unlikely(test_bit(MTIP_DDF_REMOVE_PENDING_BIT,
 							&dd->dd_flag))) {
-			bio_endio(bio, -ENXIO);
-			return;
+			return -ENXIO;
 		}
 		if (unlikely(test_bit(MTIP_DDF_OVER_TEMP_BIT, &dd->dd_flag))) {
-			bio_endio(bio, -ENODATA);
-			return;
+			return -ENODATA;
 		}
 		if (unlikely(test_bit(MTIP_DDF_WRITE_PROTECT_BIT,
 							&dd->dd_flag) &&
-				bio_data_dir(bio))) {
-			bio_endio(bio, -ENODATA);
-			return;
-		}
-		if (unlikely(test_bit(MTIP_DDF_SEC_LOCK_BIT, &dd->dd_flag))) {
-			bio_endio(bio, -ENODATA);
-			return;
-		}
-		if (test_bit(MTIP_DDF_REBUILD_FAILED_BIT, &dd->dd_flag)) {
-			bio_endio(bio, -ENXIO);
-			return;
+				rq_data_dir(rq))) {
+			return -ENODATA;
 		}
+		if (unlikely(test_bit(MTIP_DDF_SEC_LOCK_BIT, &dd->dd_flag)))
+			return -ENODATA;
+		if (test_bit(MTIP_DDF_REBUILD_FAILED_BIT, &dd->dd_flag))
+			return -ENXIO;
 	}
 
-	if (unlikely(bio->bi_rw & REQ_DISCARD)) {
-		bio_endio(bio, mtip_send_trim(dd, bio->bi_iter.bi_sector,
-						bio_sectors(bio)));
-		return;
-	}
+	if (rq->cmd_flags & REQ_DISCARD) {
+		int err;
 
-	if (unlikely(!bio_has_data(bio))) {
-		blk_queue_flush(queue, 0);
-		bio_endio(bio, 0);
-		return;
+		err = mtip_send_trim(dd, blk_rq_pos(rq), blk_rq_sectors(rq));
+		blk_mq_end_io(rq, err);
+		return 0;
 	}
 
-	if (bio_data_dir(bio) == WRITE && bio_sectors(bio) <= 64 &&
-							dd->unal_qdepth) {
-		if (bio->bi_iter.bi_sector % 8 != 0)
-			/* Unaligned on 4k boundaries */
-			unaligned = 1;
-		else if (bio_sectors(bio) % 8 != 0) /* Aligned but not 4k/8k */
-			unaligned = 1;
+	/* Create the scatter list for this request. */
+	nents = blk_rq_map_sg(hctx->queue, rq, cmd->sg);
+
+	/* Issue the read/write. */
+	mtip_hw_submit_io(dd, rq, cmd, nents, hctx);
+	return 0;
+}
+
+static bool mtip_check_unal_depth(struct blk_mq_hw_ctx *hctx,
+				  struct request *rq)
+{
+	struct driver_data *dd = hctx->queue->queuedata;
+	struct mtip_cmd *cmd = blk_mq_rq_to_pdu(rq);
+
+	if (rq_data_dir(rq) == READ || !dd->unal_qdepth)
+		return false;
+
+	/*
+	 * If unaligned depth must be limited on this controller, mark it
+	 * as unaligned if the IO isn't on a 4k boundary (start of length).
+	 */
+	if (blk_rq_sectors(rq) <= 64) {
+		if ((blk_rq_pos(rq) & 7) || (blk_rq_sectors(rq) & 7))
+			cmd->unaligned = 1;
 	}
 
-	sg = mtip_hw_get_scatterlist(dd, &tag, unaligned);
-	if (likely(sg != NULL)) {
-		blk_queue_bounce(queue, &bio);
+	if (cmd->unaligned && down_trylock(&dd->port->cmd_slot_unal))
+		return true;
 
-		if (unlikely((bio)->bi_vcnt > MTIP_MAX_SG)) {
-			dev_warn(&dd->pdev->dev,
-				"Maximum number of SGL entries exceeded\n");
-			bio_io_error(bio);
-			mtip_hw_release_scatterlist(dd, tag, unaligned);
-			return;
-		}
+	return false;
+}
 
-		/* Create the scatter list for this bio. */
-		bio_for_each_segment(bvec, bio, iter) {
-			sg_set_page(&sg[nents],
-					bvec.bv_page,
-					bvec.bv_len,
-					bvec.bv_offset);
-			nents++;
-		}
+static int mtip_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *rq)
+{
+	int ret;
 
-		/* Issue the read/write. */
-		mtip_hw_submit_io(dd,
-				bio->bi_iter.bi_sector,
-				bio_sectors(bio),
-				nents,
-				tag,
-				bio_endio,
-				bio,
-				bio_data_dir(bio),
-				unaligned);
-	} else
-		bio_io_error(bio);
+	if (unlikely(mtip_check_unal_depth(hctx, rq)))
+		return BLK_MQ_RQ_QUEUE_BUSY;
+
+	ret = mtip_submit_request(hctx, rq);
+	if (likely(!ret))
+		return BLK_MQ_RQ_QUEUE_OK;
+
+	rq->errors = ret;
+	return BLK_MQ_RQ_QUEUE_ERROR;
+}
+
+static void mtip_free_cmd(void *data, struct request *rq,
+			  unsigned int hctx_idx, unsigned int request_idx)
+{
+	struct driver_data *dd = data;
+	struct mtip_cmd *cmd = blk_mq_rq_to_pdu(rq);
+
+	if (!cmd->command)
+		return;
+
+	dmam_free_coherent(&dd->pdev->dev, CMD_DMA_ALLOC_SZ,
+				cmd->command, cmd->command_dma);
+}
+
+static int mtip_init_cmd(void *data, struct request *rq, unsigned int hctx_idx,
+			 unsigned int request_idx, unsigned int numa_node)
+{
+	struct driver_data *dd = data;
+	struct mtip_cmd *cmd = blk_mq_rq_to_pdu(rq);
+	u32 host_cap_64 = readl(dd->mmio + HOST_CAP) & HOST_CAP_64;
+
+	cmd->command = dmam_alloc_coherent(&dd->pdev->dev, CMD_DMA_ALLOC_SZ,
+			&cmd->command_dma, GFP_KERNEL);
+	if (!cmd->command)
+		return -ENOMEM;
+
+	memset(cmd->command, 0, CMD_DMA_ALLOC_SZ);
+
+	/* Point the command headers at the command tables. */
+	cmd->command_header = dd->port->command_list +
+				(sizeof(struct mtip_cmd_hdr) * request_idx);
+	cmd->command_header_dma = dd->port->command_list_dma +
+				(sizeof(struct mtip_cmd_hdr) * request_idx);
+
+	if (host_cap_64)
+		cmd->command_header->ctbau = __force_bit2int cpu_to_le32((cmd->command_dma >> 16) >> 16);
+
+	cmd->command_header->ctba = __force_bit2int cpu_to_le32(cmd->command_dma & 0xFFFFFFFF);
+
+	sg_init_table(cmd->sg, MTIP_MAX_SG);
+	return 0;
 }
 
+static struct blk_mq_ops mtip_mq_ops = {
+	.queue_rq	= mtip_queue_rq,
+	.map_queue	= blk_mq_map_queue,
+	.init_request	= mtip_init_cmd,
+	.exit_request	= mtip_free_cmd,
+};
+
 /*
  * Block layer initialization function.
  *
@@ -4148,11 +3861,7 @@ static int mtip_block_initialize(struct driver_data *dd)
 	if (dd->disk)
 		goto skip_create_disk; /* hw init done, before rebuild */
 
-	/* Initialize the protocol layer. */
-	wait_for_rebuild = mtip_hw_init(dd);
-	if (wait_for_rebuild < 0) {
-		dev_err(&dd->pdev->dev,
-			"Protocol layer initialization failed\n");
+	if (mtip_hw_init(dd)) {
 		rv = -EINVAL;
 		goto protocol_init_error;
 	}
@@ -4194,29 +3903,53 @@ static int mtip_block_initialize(struct driver_data *dd)
 
 	mtip_hw_debugfs_init(dd);
 
-	/*
-	 * if rebuild pending, start the service thread, and delay the block
-	 * queue creation and add_disk()
-	 */
-	if (wait_for_rebuild == MTIP_FTL_REBUILD_MAGIC)
-		goto start_service_thread;
-
 skip_create_disk:
-	/* Allocate the request queue. */
-	dd->queue = blk_alloc_queue_node(GFP_KERNEL, dd->numa_node);
-	if (dd->queue == NULL) {
+	memset(&dd->tags, 0, sizeof(dd->tags));
+	dd->tags.ops = &mtip_mq_ops;
+	dd->tags.nr_hw_queues = 1;
+	dd->tags.queue_depth = MTIP_MAX_COMMAND_SLOTS;
+	dd->tags.reserved_tags = 1;
+	dd->tags.cmd_size = sizeof(struct mtip_cmd);
+	dd->tags.numa_node = dd->numa_node;
+	dd->tags.flags = BLK_MQ_F_SHOULD_MERGE;
+	dd->tags.driver_data = dd;
+
+	rv = blk_mq_alloc_tag_set(&dd->tags);
+	if (rv) {
 		dev_err(&dd->pdev->dev,
 			"Unable to allocate request queue\n");
 		rv = -ENOMEM;
 		goto block_queue_alloc_init_error;
 	}
 
-	/* Attach our request function to the request queue. */
-	blk_queue_make_request(dd->queue, mtip_make_request);
+	/* Allocate the request queue. */
+	dd->queue = blk_mq_init_queue(&dd->tags);
+	if (IS_ERR(dd->queue)) {
+		dev_err(&dd->pdev->dev,
+			"Unable to allocate request queue\n");
+		rv = -ENOMEM;
+		goto block_queue_alloc_init_error;
+	}
 
 	dd->disk->queue		= dd->queue;
 	dd->queue->queuedata	= dd;
 
+	/* Initialize the protocol layer. */
+	wait_for_rebuild = mtip_hw_get_identify(dd);
+	if (wait_for_rebuild < 0) {
+		dev_err(&dd->pdev->dev,
+			"Protocol layer initialization failed\n");
+		rv = -EINVAL;
+		goto init_hw_cmds_error;
+	}
+
+	/*
+	 * if rebuild pending, start the service thread, and delay the block
+	 * queue creation and add_disk()
+	 */
+	if (wait_for_rebuild == MTIP_FTL_REBUILD_MAGIC)
+		goto start_service_thread;
+
 	/* Set device limits. */
 	set_bit(QUEUE_FLAG_NONROT, &dd->queue->queue_flags);
 	blk_queue_max_segments(dd->queue, MTIP_MAX_SG);
@@ -4295,8 +4028,9 @@ kthread_run_error:
 	del_gendisk(dd->disk);
 
 read_capacity_error:
+init_hw_cmds_error:
 	blk_cleanup_queue(dd->queue);
-
+	blk_mq_free_tag_set(&dd->tags);
 block_queue_alloc_init_error:
 	mtip_hw_debugfs_exit(dd);
 disk_index_error:
@@ -4345,6 +4079,9 @@ static int mtip_block_remove(struct driver_data *dd)
 				kobject_put(kobj);
 			}
 		}
+
+		mtip_standby_drive(dd);
+
 		/*
 		 * Delete our gendisk structure. This also removes the device
 		 * from /dev
@@ -4357,6 +4094,7 @@ static int mtip_block_remove(struct driver_data *dd)
 			if (dd->disk->queue) {
 				del_gendisk(dd->disk);
 				blk_cleanup_queue(dd->queue);
+				blk_mq_free_tag_set(&dd->tags);
 				dd->queue = NULL;
 			} else
 				put_disk(dd->disk);
@@ -4391,6 +4129,8 @@ static int mtip_block_remove(struct driver_data *dd)
  */
 static int mtip_block_shutdown(struct driver_data *dd)
 {
+	mtip_hw_shutdown(dd);
+
 	/* Delete our gendisk structure, and cleanup the blk queue. */
 	if (dd->disk) {
 		dev_info(&dd->pdev->dev,
@@ -4399,6 +4139,7 @@ static int mtip_block_shutdown(struct driver_data *dd)
 		if (dd->disk->queue) {
 			del_gendisk(dd->disk);
 			blk_cleanup_queue(dd->queue);
+			blk_mq_free_tag_set(&dd->tags);
 		} else
 			put_disk(dd->disk);
 		dd->disk  = NULL;
@@ -4408,8 +4149,6 @@ static int mtip_block_shutdown(struct driver_data *dd)
 	spin_lock(&rssd_index_lock);
 	ida_remove(&rssd_index_ida, dd->index);
 	spin_unlock(&rssd_index_lock);
-
-	mtip_hw_shutdown(dd);
 	return 0;
 }
 
@@ -4479,6 +4218,57 @@ static DEFINE_HANDLER(5);
 static DEFINE_HANDLER(6);
 static DEFINE_HANDLER(7);
 
+static void mtip_disable_link_opts(struct driver_data *dd, struct pci_dev *pdev)
+{
+	int pos;
+	unsigned short pcie_dev_ctrl;
+
+	pos = pci_find_capability(pdev, PCI_CAP_ID_EXP);
+	if (pos) {
+		pci_read_config_word(pdev,
+			pos + PCI_EXP_DEVCTL,
+			&pcie_dev_ctrl);
+		if (pcie_dev_ctrl & (1 << 11) ||
+		    pcie_dev_ctrl & (1 << 4)) {
+			dev_info(&dd->pdev->dev,
+				"Disabling ERO/No-Snoop on bridge device %04x:%04x\n",
+					pdev->vendor, pdev->device);
+			pcie_dev_ctrl &= ~(PCI_EXP_DEVCTL_NOSNOOP_EN |
+						PCI_EXP_DEVCTL_RELAX_EN);
+			pci_write_config_word(pdev,
+				pos + PCI_EXP_DEVCTL,
+				pcie_dev_ctrl);
+		}
+	}
+}
+
+static void mtip_fix_ero_nosnoop(struct driver_data *dd, struct pci_dev *pdev)
+{
+	/*
+	 * This workaround is specific to AMD/ATI chipset with a PCI upstream
+	 * device with device id 0x5aXX
+	 */
+	if (pdev->bus && pdev->bus->self) {
+		if (pdev->bus->self->vendor == PCI_VENDOR_ID_ATI &&
+		    ((pdev->bus->self->device & 0xff00) == 0x5a00)) {
+			mtip_disable_link_opts(dd, pdev->bus->self);
+		} else {
+			/* Check further up the topology */
+			struct pci_dev *parent_dev = pdev->bus->self;
+			if (parent_dev->bus &&
+				parent_dev->bus->parent &&
+				parent_dev->bus->parent->self &&
+				parent_dev->bus->parent->self->vendor ==
+					 PCI_VENDOR_ID_ATI &&
+				(parent_dev->bus->parent->self->device &
+					0xff00) == 0x5a00) {
+				mtip_disable_link_opts(dd,
+					parent_dev->bus->parent->self);
+			}
+		}
+	}
+}
+
 /*
  * Called for each supported PCI device detected.
  *
@@ -4630,6 +4420,8 @@ static int mtip_pci_probe(struct pci_dev *pdev,
 		goto msi_initialize_err;
 	}
 
+	mtip_fix_ero_nosnoop(dd, pdev);
+
 	/* Initialize the block layer. */
 	rv = mtip_block_initialize(dd);
 	if (rv < 0) {
@@ -4710,8 +4502,6 @@ static void mtip_pci_remove(struct pci_dev *pdev)
 		dev_warn(&dd->pdev->dev,
 			"Completion workers still active!\n");
 	}
-	/* Cleanup the outstanding commands */
-	mtip_command_cleanup(dd);
 
 	/* Clean up the block layer. */
 	mtip_block_remove(dd);
@@ -4737,8 +4527,6 @@ static void mtip_pci_remove(struct pci_dev *pdev)
 
 	pcim_iounmap_regions(pdev, 1 << MTIP_ABAR);
 	pci_set_drvdata(pdev, NULL);
-	pci_dev_put(pdev);
-
 }
 
 /*
@@ -4935,13 +4723,13 @@ static int __init mtip_init(void)
  */
 static void __exit mtip_exit(void)
 {
-	debugfs_remove_recursive(dfs_parent);
-
 	/* Release the allocated major block device number. */
 	unregister_blkdev(mtip_major, MTIP_DRV_NAME);
 
 	/* Unregister the PCI driver. */
 	pci_unregister_driver(&mtip_pci_driver);
+
+	debugfs_remove_recursive(dfs_parent);
 }
 
 MODULE_AUTHOR("Micron Technology, Inc");
diff --git a/drivers/block/mtip32xx/mtip32xx.h b/drivers/block/mtip32xx/mtip32xx.h
index ffb955e7ccb..ba1b31ee22e 100644
--- a/drivers/block/mtip32xx/mtip32xx.h
+++ b/drivers/block/mtip32xx/mtip32xx.h
@@ -40,9 +40,11 @@
 #define MTIP_MAX_RETRIES	2
 
 /* Various timeout values in ms */
-#define MTIP_NCQ_COMMAND_TIMEOUT_MS       5000
-#define MTIP_IOCTL_COMMAND_TIMEOUT_MS     5000
-#define MTIP_INTERNAL_COMMAND_TIMEOUT_MS  5000
+#define MTIP_NCQ_CMD_TIMEOUT_MS      15000
+#define MTIP_IOCTL_CMD_TIMEOUT_MS    5000
+#define MTIP_INT_CMD_TIMEOUT_MS      5000
+#define MTIP_QUIESCE_IO_TIMEOUT_MS   (MTIP_NCQ_CMD_TIMEOUT_MS * \
+				     (MTIP_MAX_RETRIES + 1))
 
 /* check for timeouts every 500ms */
 #define MTIP_TIMEOUT_CHECK_PERIOD	500
@@ -331,12 +333,8 @@ struct mtip_cmd {
 	 */
 	void (*comp_func)(struct mtip_port *port,
 				int tag,
-				void *data,
+				struct mtip_cmd *cmd,
 				int status);
-	/* Additional callback function that may be called by comp_func() */
-	void (*async_callback)(void *data, int status);
-
-	void *async_data; /* Addl. data passed to async_callback() */
 
 	int scatter_ents; /* Number of scatter list entries used */
 
@@ -347,10 +345,6 @@ struct mtip_cmd {
 	int retries; /* The number of retries left for this command. */
 
 	int direction; /* Data transfer direction */
-
-	unsigned long comp_time; /* command completion time, in jiffies */
-
-	atomic_t active; /* declares if this command sent to the drive. */
 };
 
 /* Structure used to describe a port. */
@@ -436,12 +430,6 @@ struct mtip_port {
 	 * or error handling is active
 	 */
 	unsigned long cmds_to_issue[SLOTBITS_IN_LONGS];
-	/*
-	 * Array of command slots. Structure includes pointers to the
-	 * command header and command table, and completion function and data
-	 * pointers.
-	 */
-	struct mtip_cmd commands[MTIP_MAX_COMMAND_SLOTS];
 	/* Used by mtip_service_thread to wait for an event */
 	wait_queue_head_t svc_wait;
 	/*
@@ -452,13 +440,7 @@ struct mtip_port {
 	/*
 	 * Timer used to complete commands that have been active for too long.
 	 */
-	struct timer_list cmd_timer;
 	unsigned long ic_pause_timer;
-	/*
-	 * Semaphore used to block threads if there are no
-	 * command slots available.
-	 */
-	struct semaphore cmd_slot;
 
 	/* Semaphore to control queue depth of unaligned IOs */
 	struct semaphore cmd_slot_unal;
@@ -485,6 +467,8 @@ struct driver_data {
 
 	struct request_queue *queue; /* Our request queue. */
 
+	struct blk_mq_tag_set tags; /* blk_mq tags */
+
 	struct mtip_port *port; /* Pointer to the port data structure. */
 
 	unsigned product_type; /* magic value declaring the product type */
@@ -509,19 +493,19 @@ struct driver_data {
 
 	struct workqueue_struct *isr_workq;
 
-	struct mtip_work work[MTIP_MAX_SLOT_GROUPS];
-
 	atomic_t irq_workers_active;
 
+	struct mtip_work work[MTIP_MAX_SLOT_GROUPS];
+
 	int isr_binding;
 
 	struct block_device *bdev;
 
-	int unal_qdepth; /* qdepth of unaligned IO queue */
-
 	struct list_head online_list; /* linkage for online list */
 
 	struct list_head remove_list; /* linkage for removing list */
+
+	int unal_qdepth; /* qdepth of unaligned IO queue */
 };
 
 #endif
diff --git a/drivers/block/nbd.c b/drivers/block/nbd.c
index 3a70ea2f7cd..fb31b8ee437 100644
--- a/drivers/block/nbd.c
+++ b/drivers/block/nbd.c
@@ -243,14 +243,11 @@ static int nbd_send_req(struct nbd_device *nbd, struct request *req)
 	struct nbd_request request;
 	unsigned long size = blk_rq_bytes(req);
 
+	memset(&request, 0, sizeof(request));
 	request.magic = htonl(NBD_REQUEST_MAGIC);
 	request.type = htonl(nbd_cmd(req));
 
-	if (nbd_cmd(req) == NBD_CMD_FLUSH) {
-		/* Other values are reserved for FLUSH requests.  */
-		request.from = 0;
-		request.len = 0;
-	} else {
+	if (nbd_cmd(req) != NBD_CMD_FLUSH && nbd_cmd(req) != NBD_CMD_DISC) {
 		request.from = cpu_to_be64((u64)blk_rq_pos(req) << 9);
 		request.len = htonl(size);
 	}
@@ -533,7 +530,7 @@ static int nbd_thread(void *data)
 	struct nbd_device *nbd = data;
 	struct request *req;
 
-	set_user_nice(current, -20);
+	set_user_nice(current, MIN_NICE);
 	while (!kthread_should_stop() || !list_empty(&nbd->waiting_queue)) {
 		/* wait for something to do */
 		wait_event_interruptible(nbd->waiting_wq,
diff --git a/drivers/block/null_blk.c b/drivers/block/null_blk.c
index 091b9ea14fe..77087a29b12 100644
--- a/drivers/block/null_blk.c
+++ b/drivers/block/null_blk.c
@@ -32,6 +32,7 @@ struct nullb {
 	unsigned int index;
 	struct request_queue *q;
 	struct gendisk *disk;
+	struct blk_mq_tag_set tag_set;
 	struct hrtimer timer;
 	unsigned int queue_depth;
 	spinlock_t lock;
@@ -202,8 +203,8 @@ static enum hrtimer_restart null_cmd_timer_expired(struct hrtimer *timer)
 		entry = llist_reverse_order(entry);
 		do {
 			cmd = container_of(entry, struct nullb_cmd, ll_list);
-			end_cmd(cmd);
 			entry = entry->next;
+			end_cmd(cmd);
 		} while (entry);
 	}
 
@@ -226,7 +227,7 @@ static void null_cmd_end_timer(struct nullb_cmd *cmd)
 
 static void null_softirq_done_fn(struct request *rq)
 {
-	end_cmd(rq->special);
+	end_cmd(blk_mq_rq_to_pdu(rq));
 }
 
 static inline void null_handle_cmd(struct nullb_cmd *cmd)
@@ -311,7 +312,7 @@ static void null_request_fn(struct request_queue *q)
 
 static int null_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *rq)
 {
-	struct nullb_cmd *cmd = rq->special;
+	struct nullb_cmd *cmd = blk_mq_rq_to_pdu(rq);
 
 	cmd->rq = rq;
 	cmd->nq = hctx->driver_data;
@@ -320,46 +321,6 @@ static int null_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *rq)
 	return BLK_MQ_RQ_QUEUE_OK;
 }
 
-static struct blk_mq_hw_ctx *null_alloc_hctx(struct blk_mq_reg *reg, unsigned int hctx_index)
-{
-	int b_size = DIV_ROUND_UP(reg->nr_hw_queues, nr_online_nodes);
-	int tip = (reg->nr_hw_queues % nr_online_nodes);
-	int node = 0, i, n;
-
-	/*
-	 * Split submit queues evenly wrt to the number of nodes. If uneven,
-	 * fill the first buckets with one extra, until the rest is filled with
-	 * no extra.
-	 */
-	for (i = 0, n = 1; i < hctx_index; i++, n++) {
-		if (n % b_size == 0) {
-			n = 0;
-			node++;
-
-			tip--;
-			if (!tip)
-				b_size = reg->nr_hw_queues / nr_online_nodes;
-		}
-	}
-
-	/*
-	 * A node might not be online, therefore map the relative node id to the
-	 * real node id.
-	 */
-	for_each_online_node(n) {
-		if (!node)
-			break;
-		node--;
-	}
-
-	return kzalloc_node(sizeof(struct blk_mq_hw_ctx), GFP_KERNEL, n);
-}
-
-static void null_free_hctx(struct blk_mq_hw_ctx *hctx, unsigned int hctx_index)
-{
-	kfree(hctx);
-}
-
 static void null_init_queue(struct nullb *nullb, struct nullb_queue *nq)
 {
 	BUG_ON(!nullb);
@@ -389,19 +350,14 @@ static struct blk_mq_ops null_mq_ops = {
 	.complete	= null_softirq_done_fn,
 };
 
-static struct blk_mq_reg null_mq_reg = {
-	.ops		= &null_mq_ops,
-	.queue_depth	= 64,
-	.cmd_size	= sizeof(struct nullb_cmd),
-	.flags		= BLK_MQ_F_SHOULD_MERGE,
-};
-
 static void null_del_dev(struct nullb *nullb)
 {
 	list_del_init(&nullb->list);
 
 	del_gendisk(nullb->disk);
 	blk_cleanup_queue(nullb->q);
+	if (queue_mode == NULL_Q_MQ)
+		blk_mq_free_tag_set(&nullb->tag_set);
 	put_disk(nullb->disk);
 	kfree(nullb);
 }
@@ -506,7 +462,7 @@ static int null_add_dev(void)
 
 	nullb = kzalloc_node(sizeof(*nullb), GFP_KERNEL, home_node);
 	if (!nullb)
-		return -ENOMEM;
+		goto out;
 
 	spin_lock_init(&nullb->lock);
 
@@ -514,49 +470,44 @@ static int null_add_dev(void)
 		submit_queues = nr_online_nodes;
 
 	if (setup_queues(nullb))
-		goto err;
+		goto out_free_nullb;
 
 	if (queue_mode == NULL_Q_MQ) {
-		null_mq_reg.numa_node = home_node;
-		null_mq_reg.queue_depth = hw_queue_depth;
-		null_mq_reg.nr_hw_queues = submit_queues;
-
-		if (use_per_node_hctx) {
-			null_mq_reg.ops->alloc_hctx = null_alloc_hctx;
-			null_mq_reg.ops->free_hctx = null_free_hctx;
-		} else {
-			null_mq_reg.ops->alloc_hctx = blk_mq_alloc_single_hw_queue;
-			null_mq_reg.ops->free_hctx = blk_mq_free_single_hw_queue;
-		}
-
-		nullb->q = blk_mq_init_queue(&null_mq_reg, nullb);
+		nullb->tag_set.ops = &null_mq_ops;
+		nullb->tag_set.nr_hw_queues = submit_queues;
+		nullb->tag_set.queue_depth = hw_queue_depth;
+		nullb->tag_set.numa_node = home_node;
+		nullb->tag_set.cmd_size	= sizeof(struct nullb_cmd);
+		nullb->tag_set.flags = BLK_MQ_F_SHOULD_MERGE;
+		nullb->tag_set.driver_data = nullb;
+
+		if (blk_mq_alloc_tag_set(&nullb->tag_set))
+			goto out_cleanup_queues;
+
+		nullb->q = blk_mq_init_queue(&nullb->tag_set);
+		if (!nullb->q)
+			goto out_cleanup_tags;
 	} else if (queue_mode == NULL_Q_BIO) {
 		nullb->q = blk_alloc_queue_node(GFP_KERNEL, home_node);
+		if (!nullb->q)
+			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)
+			goto out_cleanup_queues;
 		blk_queue_prep_rq(nullb->q, null_rq_prep_fn);
-		if (nullb->q)
-			blk_queue_softirq_done(nullb->q, null_softirq_done_fn);
+		blk_queue_softirq_done(nullb->q, null_softirq_done_fn);
 		init_driver_queues(nullb);
 	}
 
-	if (!nullb->q)
-		goto queue_fail;
-
 	nullb->q->queuedata = nullb;
 	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, nullb->q);
 
 	disk = nullb->disk = alloc_disk_node(1, home_node);
-	if (!disk) {
-queue_fail:
-		blk_cleanup_queue(nullb->q);
-		cleanup_queues(nullb);
-err:
-		kfree(nullb);
-		return -ENOMEM;
-	}
+	if (!disk)
+		goto out_cleanup_blk_queue;
 
 	mutex_lock(&lock);
 	list_add_tail(&nullb->list, &nullb_list);
@@ -579,6 +530,18 @@ err:
 	sprintf(disk->disk_name, "nullb%d", nullb->index);
 	add_disk(disk);
 	return 0;
+
+out_cleanup_blk_queue:
+	blk_cleanup_queue(nullb->q);
+out_cleanup_tags:
+	if (queue_mode == NULL_Q_MQ)
+		blk_mq_free_tag_set(&nullb->tag_set);
+out_cleanup_queues:
+	cleanup_queues(nullb);
+out_free_nullb:
+	kfree(nullb);
+out:
+	return -ENOMEM;
 }
 
 static int __init null_init(void)
diff --git a/drivers/block/nvme-core.c b/drivers/block/nvme-core.c
index 7c64fa756cc..a842c71dcc2 100644
--- a/drivers/block/nvme-core.c
+++ b/drivers/block/nvme-core.c
@@ -2775,6 +2775,16 @@ static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	return result;
 }
 
+static void nvme_reset_notify(struct pci_dev *pdev, bool prepare)
+{
+       struct nvme_dev *dev = pci_get_drvdata(pdev);
+
+       if (prepare)
+               nvme_dev_shutdown(dev);
+       else
+               nvme_dev_resume(dev);
+}
+
 static void nvme_shutdown(struct pci_dev *pdev)
 {
 	struct nvme_dev *dev = pci_get_drvdata(pdev);
@@ -2839,6 +2849,7 @@ static const struct pci_error_handlers nvme_err_handler = {
 	.link_reset	= nvme_link_reset,
 	.slot_reset	= nvme_slot_reset,
 	.resume		= nvme_error_resume,
+	.reset_notify	= nvme_reset_notify,
 };
 
 /* Move to pci_ids.h later */
diff --git a/drivers/block/paride/pcd.c b/drivers/block/paride/pcd.c
index e76bdc074db..719cb1bc164 100644
--- a/drivers/block/paride/pcd.c
+++ b/drivers/block/paride/pcd.c
@@ -747,7 +747,7 @@ static void do_pcd_request(struct request_queue * q)
 			pcd_current = cd;
 			pcd_sector = blk_rq_pos(pcd_req);
 			pcd_count = blk_rq_cur_sectors(pcd_req);
-			pcd_buf = pcd_req->buffer;
+			pcd_buf = bio_data(pcd_req->bio);
 			pcd_busy = 1;
 			ps_set_intr(do_pcd_read, NULL, 0, nice);
 			return;
diff --git a/drivers/block/paride/pd.c b/drivers/block/paride/pd.c
index 19ad8f0c83e..fea7e76a00d 100644
--- a/drivers/block/paride/pd.c
+++ b/drivers/block/paride/pd.c
@@ -454,7 +454,7 @@ static enum action do_pd_io_start(void)
 		if (pd_block + pd_count > get_capacity(pd_req->rq_disk))
 			return Fail;
 		pd_run = blk_rq_sectors(pd_req);
-		pd_buf = pd_req->buffer;
+		pd_buf = bio_data(pd_req->bio);
 		pd_retries = 0;
 		if (pd_cmd == READ)
 			return do_pd_read_start();
@@ -485,7 +485,7 @@ static int pd_next_buf(void)
 	spin_lock_irqsave(&pd_lock, saved_flags);
 	__blk_end_request_cur(pd_req, 0);
 	pd_count = blk_rq_cur_sectors(pd_req);
-	pd_buf = pd_req->buffer;
+	pd_buf = bio_data(pd_req->bio);
 	spin_unlock_irqrestore(&pd_lock, saved_flags);
 	return 0;
 }
diff --git a/drivers/block/paride/pf.c b/drivers/block/paride/pf.c
index f5c86d523ba..9a15fd3c934 100644
--- a/drivers/block/paride/pf.c
+++ b/drivers/block/paride/pf.c
@@ -795,7 +795,7 @@ repeat:
 	}
 
 	pf_cmd = rq_data_dir(pf_req);
-	pf_buf = pf_req->buffer;
+	pf_buf = bio_data(pf_req->bio);
 	pf_retries = 0;
 
 	pf_busy = 1;
@@ -827,7 +827,7 @@ static int pf_next_buf(void)
 		if (!pf_req)
 			return 1;
 		pf_count = blk_rq_cur_sectors(pf_req);
-		pf_buf = pf_req->buffer;
+		pf_buf = bio_data(pf_req->bio);
 	}
 	return 0;
 }
diff --git a/drivers/block/pktcdvd.c b/drivers/block/pktcdvd.c
index a2af73db187..758ac442c5b 100644
--- a/drivers/block/pktcdvd.c
+++ b/drivers/block/pktcdvd.c
@@ -704,6 +704,7 @@ 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);
+	blk_rq_set_block_pc(rq);
 
 	if (cgc->buflen) {
 		ret = blk_rq_map_kern(q, rq, cgc->buffer, cgc->buflen,
@@ -716,7 +717,6 @@ static int pkt_generic_packet(struct pktcdvd_device *pd, struct packet_command *
 	memcpy(rq->cmd, cgc->cmd, CDROM_PACKET_SIZE);
 
 	rq->timeout = 60*HZ;
-	rq->cmd_type = REQ_TYPE_BLOCK_PC;
 	if (cgc->quiet)
 		rq->cmd_flags |= REQ_QUIET;
 
@@ -1463,7 +1463,7 @@ static int kcdrwd(void *foobar)
 	struct packet_data *pkt;
 	long min_sleep_time, residue;
 
-	set_user_nice(current, -20);
+	set_user_nice(current, MIN_NICE);
 	set_freezable();
 
 	for (;;) {
diff --git a/drivers/block/skd_main.c b/drivers/block/skd_main.c
index a69dd93d1bd..608532d3f8c 100644
--- a/drivers/block/skd_main.c
+++ b/drivers/block/skd_main.c
@@ -563,7 +563,6 @@ skd_prep_discard_cdb(struct skd_scsi_request *scsi_req,
 
 	req = skreq->req;
 	blk_add_request_payload(req, page, len);
-	req->buffer = buf;
 }
 
 static void skd_request_fn_not_online(struct request_queue *q);
@@ -744,6 +743,7 @@ static void skd_request_fn(struct request_queue *q)
 				break;
 			}
 			skreq->discard_page = 1;
+			req->completion_data = page;
 			skd_prep_discard_cdb(scsi_req, skreq, page, lba, count);
 
 		} else if (flush == SKD_FLUSH_ZERO_SIZE_FIRST) {
@@ -858,8 +858,7 @@ static void skd_end_request(struct skd_device *skdev,
 		(skreq->discard_page == 1)) {
 		pr_debug("%s:%s:%d, free the page!",
 			 skdev->name, __func__, __LINE__);
-		free_page((unsigned long)req->buffer);
-		req->buffer = NULL;
+		__free_page(req->completion_data);
 	}
 
 	if (unlikely(error)) {
@@ -3945,15 +3944,14 @@ static int skd_acquire_msix(struct skd_device *skdev)
 	for (i = 0; i < SKD_MAX_MSIX_COUNT; i++)
 		entries[i].entry = i;
 
-	rc = pci_enable_msix_range(pdev, entries,
-				   SKD_MIN_MSIX_COUNT, SKD_MAX_MSIX_COUNT);
-	if (rc < 0) {
+	rc = pci_enable_msix_exact(pdev, entries, SKD_MAX_MSIX_COUNT);
+	if (rc) {
 		pr_err("(%s): failed to enable MSI-X %d\n",
 		       skd_name(skdev), rc);
 		goto msix_out;
 	}
 
-	skdev->msix_count = rc;
+	skdev->msix_count = SKD_MAX_MSIX_COUNT;
 	skdev->msix_entries = kzalloc(sizeof(struct skd_msix_entry) *
 				      skdev->msix_count, GFP_KERNEL);
 	if (!skdev->msix_entries) {
diff --git a/drivers/block/swim.c b/drivers/block/swim.c
index b02d53a399f..6b44bbe528b 100644
--- a/drivers/block/swim.c
+++ b/drivers/block/swim.c
@@ -549,7 +549,7 @@ static void redo_fd_request(struct request_queue *q)
 		case READ:
 			err = floppy_read_sectors(fs, blk_rq_pos(req),
 						  blk_rq_cur_sectors(req),
-						  req->buffer);
+						  bio_data(req->bio));
 			break;
 		}
 	done:
diff --git a/drivers/block/swim3.c b/drivers/block/swim3.c
index c74f7b56e7c..523ee8fd4c1 100644
--- a/drivers/block/swim3.c
+++ b/drivers/block/swim3.c
@@ -342,7 +342,7 @@ static void start_request(struct floppy_state *fs)
 		swim3_dbg("do_fd_req: dev=%s cmd=%d sec=%ld nr_sec=%u buf=%p\n",
 			  req->rq_disk->disk_name, req->cmd,
 			  (long)blk_rq_pos(req), blk_rq_sectors(req),
-			  req->buffer);
+			  bio_data(req->bio));
 		swim3_dbg("           errors=%d current_nr_sectors=%u\n",
 			  req->errors, blk_rq_cur_sectors(req));
 #endif
@@ -479,11 +479,11 @@ static inline void setup_transfer(struct floppy_state *fs)
 		/* Set up 3 dma commands: write preamble, data, postamble */
 		init_dma(cp, OUTPUT_MORE, write_preamble, sizeof(write_preamble));
 		++cp;
-		init_dma(cp, OUTPUT_MORE, req->buffer, 512);
+		init_dma(cp, OUTPUT_MORE, bio_data(req->bio), 512);
 		++cp;
 		init_dma(cp, OUTPUT_LAST, write_postamble, sizeof(write_postamble));
 	} else {
-		init_dma(cp, INPUT_LAST, req->buffer, n * 512);
+		init_dma(cp, INPUT_LAST, bio_data(req->bio), n * 512);
 	}
 	++cp;
 	out_le16(&cp->command, DBDMA_STOP);
diff --git a/drivers/block/virtio_blk.c b/drivers/block/virtio_blk.c
index 6d8a87f252d..f63d358f3d9 100644
--- a/drivers/block/virtio_blk.c
+++ b/drivers/block/virtio_blk.c
@@ -30,6 +30,9 @@ struct virtio_blk
 	/* The disk structure for the kernel. */
 	struct gendisk *disk;
 
+	/* Block layer tags. */
+	struct blk_mq_tag_set tag_set;
+
 	/* Process context for config space updates */
 	struct work_struct config_work;
 
@@ -112,7 +115,7 @@ static int __virtblk_add_req(struct virtqueue *vq,
 
 static inline void virtblk_request_done(struct request *req)
 {
-	struct virtblk_req *vbr = req->special;
+	struct virtblk_req *vbr = blk_mq_rq_to_pdu(req);
 	int error = virtblk_result(vbr);
 
 	if (req->cmd_type == REQ_TYPE_BLOCK_PC) {
@@ -144,21 +147,22 @@ static void virtblk_done(struct virtqueue *vq)
 		if (unlikely(virtqueue_is_broken(vq)))
 			break;
 	} while (!virtqueue_enable_cb(vq));
-	spin_unlock_irqrestore(&vblk->vq_lock, flags);
 
 	/* In case queue is stopped waiting for more buffers. */
 	if (req_done)
-		blk_mq_start_stopped_hw_queues(vblk->disk->queue);
+		blk_mq_start_stopped_hw_queues(vblk->disk->queue, true);
+	spin_unlock_irqrestore(&vblk->vq_lock, flags);
 }
 
 static int virtio_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *req)
 {
 	struct virtio_blk *vblk = hctx->queue->queuedata;
-	struct virtblk_req *vbr = req->special;
+	struct virtblk_req *vbr = blk_mq_rq_to_pdu(req);
 	unsigned long flags;
 	unsigned int num;
 	const bool last = (req->cmd_flags & REQ_END) != 0;
 	int err;
+	bool notify = false;
 
 	BUG_ON(req->nr_phys_segments + 2 > vblk->sg_elems);
 
@@ -202,8 +206,8 @@ static int virtio_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *req)
 	err = __virtblk_add_req(vblk->vq, vbr, vbr->sg, num);
 	if (err) {
 		virtqueue_kick(vblk->vq);
-		spin_unlock_irqrestore(&vblk->vq_lock, flags);
 		blk_mq_stop_hw_queue(hctx);
+		spin_unlock_irqrestore(&vblk->vq_lock, flags);
 		/* Out of mem doesn't actually happen, since we fall back
 		 * to direct descriptors */
 		if (err == -ENOMEM || err == -ENOSPC)
@@ -211,10 +215,12 @@ static int virtio_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *req)
 		return BLK_MQ_RQ_QUEUE_ERROR;
 	}
 
-	if (last)
-		virtqueue_kick(vblk->vq);
-
+	if (last && virtqueue_kick_prepare(vblk->vq))
+		notify = true;
 	spin_unlock_irqrestore(&vblk->vq_lock, flags);
+
+	if (notify)
+		virtqueue_notify(vblk->vq);
 	return BLK_MQ_RQ_QUEUE_OK;
 }
 
@@ -480,33 +486,27 @@ static const struct device_attribute dev_attr_cache_type_rw =
 	__ATTR(cache_type, S_IRUGO|S_IWUSR,
 	       virtblk_cache_type_show, virtblk_cache_type_store);
 
-static struct blk_mq_ops virtio_mq_ops = {
-	.queue_rq	= virtio_queue_rq,
-	.map_queue	= blk_mq_map_queue,
-	.alloc_hctx	= blk_mq_alloc_single_hw_queue,
-	.free_hctx	= blk_mq_free_single_hw_queue,
-	.complete	= virtblk_request_done,
-};
-
-static struct blk_mq_reg virtio_mq_reg = {
-	.ops		= &virtio_mq_ops,
-	.nr_hw_queues	= 1,
-	.queue_depth	= 0, /* Set in virtblk_probe */
-	.numa_node	= NUMA_NO_NODE,
-	.flags		= BLK_MQ_F_SHOULD_MERGE,
-};
-module_param_named(queue_depth, virtio_mq_reg.queue_depth, uint, 0444);
-
-static int virtblk_init_vbr(void *data, struct blk_mq_hw_ctx *hctx,
-			     struct request *rq, unsigned int nr)
+static int virtblk_init_request(void *data, struct request *rq,
+		unsigned int hctx_idx, unsigned int request_idx,
+		unsigned int numa_node)
 {
 	struct virtio_blk *vblk = data;
-	struct virtblk_req *vbr = rq->special;
+	struct virtblk_req *vbr = blk_mq_rq_to_pdu(rq);
 
 	sg_init_table(vbr->sg, vblk->sg_elems);
 	return 0;
 }
 
+static struct blk_mq_ops virtio_mq_ops = {
+	.queue_rq	= virtio_queue_rq,
+	.map_queue	= blk_mq_map_queue,
+	.complete	= virtblk_request_done,
+	.init_request	= virtblk_init_request,
+};
+
+static unsigned int virtblk_queue_depth;
+module_param_named(queue_depth, virtblk_queue_depth, uint, 0444);
+
 static int virtblk_probe(struct virtio_device *vdev)
 {
 	struct virtio_blk *vblk;
@@ -561,24 +561,34 @@ static int virtblk_probe(struct virtio_device *vdev)
 	}
 
 	/* Default queue sizing is to fill the ring. */
-	if (!virtio_mq_reg.queue_depth) {
-		virtio_mq_reg.queue_depth = vblk->vq->num_free;
+	if (!virtblk_queue_depth) {
+		virtblk_queue_depth = vblk->vq->num_free;
 		/* ... but without indirect descs, we use 2 descs per req */
 		if (!virtio_has_feature(vdev, VIRTIO_RING_F_INDIRECT_DESC))
-			virtio_mq_reg.queue_depth /= 2;
+			virtblk_queue_depth /= 2;
 	}
-	virtio_mq_reg.cmd_size =
+
+	memset(&vblk->tag_set, 0, sizeof(vblk->tag_set));
+	vblk->tag_set.ops = &virtio_mq_ops;
+	vblk->tag_set.nr_hw_queues = 1;
+	vblk->tag_set.queue_depth = virtblk_queue_depth;
+	vblk->tag_set.numa_node = NUMA_NO_NODE;
+	vblk->tag_set.flags = BLK_MQ_F_SHOULD_MERGE;
+	vblk->tag_set.cmd_size =
 		sizeof(struct virtblk_req) +
 		sizeof(struct scatterlist) * sg_elems;
+	vblk->tag_set.driver_data = vblk;
+
+	err = blk_mq_alloc_tag_set(&vblk->tag_set);
+	if (err)
+		goto out_put_disk;
 
-	q = vblk->disk->queue = blk_mq_init_queue(&virtio_mq_reg, vblk);
+	q = vblk->disk->queue = blk_mq_init_queue(&vblk->tag_set);
 	if (!q) {
 		err = -ENOMEM;
-		goto out_put_disk;
+		goto out_free_tags;
 	}
 
-	blk_mq_init_commands(q, virtblk_init_vbr, vblk);
-
 	q->queuedata = vblk;
 
 	virtblk_name_format("vd", index, vblk->disk->disk_name, DISK_NAME_LEN);
@@ -679,6 +689,8 @@ static int virtblk_probe(struct virtio_device *vdev)
 out_del_disk:
 	del_gendisk(vblk->disk);
 	blk_cleanup_queue(vblk->disk->queue);
+out_free_tags:
+	blk_mq_free_tag_set(&vblk->tag_set);
 out_put_disk:
 	put_disk(vblk->disk);
 out_free_vq:
@@ -705,6 +717,8 @@ static void virtblk_remove(struct virtio_device *vdev)
 	del_gendisk(vblk->disk);
 	blk_cleanup_queue(vblk->disk->queue);
 
+	blk_mq_free_tag_set(&vblk->tag_set);
+
 	/* Stop all the virtqueues. */
 	vdev->config->reset(vdev);
 
@@ -749,7 +763,7 @@ static int virtblk_restore(struct virtio_device *vdev)
 	vblk->config_enable = true;
 	ret = init_vq(vdev->priv);
 	if (!ret)
-		blk_mq_start_stopped_hw_queues(vblk->disk->queue);
+		blk_mq_start_stopped_hw_queues(vblk->disk->queue, true);
 
 	return ret;
 }
diff --git a/drivers/block/xen-blkback/common.h b/drivers/block/xen-blkback/common.h
index be052773ad0..f65b807e323 100644
--- a/drivers/block/xen-blkback/common.h
+++ b/drivers/block/xen-blkback/common.h
@@ -314,7 +314,7 @@ struct xen_blkif {
 	unsigned long long			st_rd_sect;
 	unsigned long long			st_wr_sect;
 
-	wait_queue_head_t	waiting_to_free;
+	struct work_struct	free_work;
 	/* Thread shutdown wait queue. */
 	wait_queue_head_t	shutdown_wq;
 };
@@ -361,7 +361,7 @@ struct pending_req {
 #define xen_blkif_put(_b)				\
 	do {						\
 		if (atomic_dec_and_test(&(_b)->refcnt))	\
-			wake_up(&(_b)->waiting_to_free);\
+			schedule_work(&(_b)->free_work);\
 	} while (0)
 
 struct phys_req {
diff --git a/drivers/block/xen-blkback/xenbus.c b/drivers/block/xen-blkback/xenbus.c
index 9a547e6b6eb..3a8b810b498 100644
--- a/drivers/block/xen-blkback/xenbus.c
+++ b/drivers/block/xen-blkback/xenbus.c
@@ -35,12 +35,26 @@ static void connect(struct backend_info *);
 static int connect_ring(struct backend_info *);
 static void backend_changed(struct xenbus_watch *, const char **,
 			    unsigned int);
+static void xen_blkif_free(struct xen_blkif *blkif);
+static void xen_vbd_free(struct xen_vbd *vbd);
 
 struct xenbus_device *xen_blkbk_xenbus(struct backend_info *be)
 {
 	return be->dev;
 }
 
+/*
+ * The last request could free the device from softirq context and
+ * xen_blkif_free() can sleep.
+ */
+static void xen_blkif_deferred_free(struct work_struct *work)
+{
+	struct xen_blkif *blkif;
+
+	blkif = container_of(work, struct xen_blkif, free_work);
+	xen_blkif_free(blkif);
+}
+
 static int blkback_name(struct xen_blkif *blkif, char *buf)
 {
 	char *devpath, *devname;
@@ -121,7 +135,6 @@ static struct xen_blkif *xen_blkif_alloc(domid_t domid)
 	init_completion(&blkif->drain_complete);
 	atomic_set(&blkif->drain, 0);
 	blkif->st_print = jiffies;
-	init_waitqueue_head(&blkif->waiting_to_free);
 	blkif->persistent_gnts.rb_node = NULL;
 	spin_lock_init(&blkif->free_pages_lock);
 	INIT_LIST_HEAD(&blkif->free_pages);
@@ -132,6 +145,7 @@ static struct xen_blkif *xen_blkif_alloc(domid_t domid)
 	INIT_WORK(&blkif->persistent_purge_work, xen_blkbk_unmap_purged_grants);
 
 	INIT_LIST_HEAD(&blkif->pending_free);
+	INIT_WORK(&blkif->free_work, xen_blkif_deferred_free);
 
 	for (i = 0; i < XEN_BLKIF_REQS; i++) {
 		req = kzalloc(sizeof(*req), GFP_KERNEL);
@@ -231,7 +245,7 @@ static int xen_blkif_map(struct xen_blkif *blkif, unsigned long shared_page,
 	return 0;
 }
 
-static void xen_blkif_disconnect(struct xen_blkif *blkif)
+static int xen_blkif_disconnect(struct xen_blkif *blkif)
 {
 	if (blkif->xenblkd) {
 		kthread_stop(blkif->xenblkd);
@@ -239,9 +253,12 @@ static void xen_blkif_disconnect(struct xen_blkif *blkif)
 		blkif->xenblkd = NULL;
 	}
 
-	atomic_dec(&blkif->refcnt);
-	wait_event(blkif->waiting_to_free, atomic_read(&blkif->refcnt) == 0);
-	atomic_inc(&blkif->refcnt);
+	/* The above kthread_stop() guarantees that at this point we
+	 * don't have any discard_io or other_io requests. So, checking
+	 * for inflight IO is enough.
+	 */
+	if (atomic_read(&blkif->inflight) > 0)
+		return -EBUSY;
 
 	if (blkif->irq) {
 		unbind_from_irqhandler(blkif->irq, blkif);
@@ -252,6 +269,8 @@ static void xen_blkif_disconnect(struct xen_blkif *blkif)
 		xenbus_unmap_ring_vfree(blkif->be->dev, blkif->blk_ring);
 		blkif->blk_rings.common.sring = NULL;
 	}
+
+	return 0;
 }
 
 static void xen_blkif_free(struct xen_blkif *blkif)
@@ -259,8 +278,8 @@ static void xen_blkif_free(struct xen_blkif *blkif)
 	struct pending_req *req, *n;
 	int i = 0, j;
 
-	if (!atomic_dec_and_test(&blkif->refcnt))
-		BUG();
+	xen_blkif_disconnect(blkif);
+	xen_vbd_free(&blkif->vbd);
 
 	/* Remove all persistent grants and the cache of ballooned pages. */
 	xen_blkbk_free_caches(blkif);
@@ -449,16 +468,15 @@ static int xen_blkbk_remove(struct xenbus_device *dev)
 		be->backend_watch.node = NULL;
 	}
 
+	dev_set_drvdata(&dev->dev, NULL);
+
 	if (be->blkif) {
 		xen_blkif_disconnect(be->blkif);
-		xen_vbd_free(&be->blkif->vbd);
-		xen_blkif_free(be->blkif);
-		be->blkif = NULL;
+		xen_blkif_put(be->blkif);
 	}
 
 	kfree(be->mode);
 	kfree(be);
-	dev_set_drvdata(&dev->dev, NULL);
 	return 0;
 }
 
@@ -481,10 +499,15 @@ static void xen_blkbk_discard(struct xenbus_transaction xbt, struct backend_info
 	struct xenbus_device *dev = be->dev;
 	struct xen_blkif *blkif = be->blkif;
 	int err;
-	int state = 0;
+	int state = 0, discard_enable;
 	struct block_device *bdev = be->blkif->vbd.bdev;
 	struct request_queue *q = bdev_get_queue(bdev);
 
+	err = xenbus_scanf(XBT_NIL, dev->nodename, "discard-enable", "%d",
+			   &discard_enable);
+	if (err == 1 && !discard_enable)
+		return;
+
 	if (blk_queue_discard(q)) {
 		err = xenbus_printf(xbt, dev->nodename,
 			"discard-granularity", "%u",
@@ -700,7 +723,11 @@ static void frontend_changed(struct xenbus_device *dev,
 		 * Enforce precondition before potential leak point.
 		 * xen_blkif_disconnect() is idempotent.
 		 */
-		xen_blkif_disconnect(be->blkif);
+		err = xen_blkif_disconnect(be->blkif);
+		if (err) {
+			xenbus_dev_fatal(dev, err, "pending I/O");
+			break;
+		}
 
 		err = connect_ring(be);
 		if (err)
diff --git a/drivers/block/xen-blkfront.c b/drivers/block/xen-blkfront.c
index efe1b476173..5deb235bd18 100644
--- a/drivers/block/xen-blkfront.c
+++ b/drivers/block/xen-blkfront.c
@@ -612,10 +612,10 @@ static void do_blkif_request(struct request_queue *rq)
 		}
 
 		pr_debug("do_blk_req %p: cmd %p, sec %lx, "
-			 "(%u/%u) buffer:%p [%s]\n",
+			 "(%u/%u) [%s]\n",
 			 req, req->cmd, (unsigned long)blk_rq_pos(req),
 			 blk_rq_cur_sectors(req), blk_rq_sectors(req),
-			 req->buffer, rq_data_dir(req) ? "write" : "read");
+			 rq_data_dir(req) ? "write" : "read");
 
 		if (blkif_queue_request(req)) {
 			blk_requeue_request(rq, req);
@@ -1635,36 +1635,24 @@ blkfront_closing(struct blkfront_info *info)
 static void blkfront_setup_discard(struct blkfront_info *info)
 {
 	int err;
-	char *type;
 	unsigned int discard_granularity;
 	unsigned int discard_alignment;
 	unsigned int discard_secure;
 
-	type = xenbus_read(XBT_NIL, info->xbdev->otherend, "type", NULL);
-	if (IS_ERR(type))
-		return;
-
-	info->feature_secdiscard = 0;
-	if (strncmp(type, "phy", 3) == 0) {
-		err = xenbus_gather(XBT_NIL, info->xbdev->otherend,
-			"discard-granularity", "%u", &discard_granularity,
-			"discard-alignment", "%u", &discard_alignment,
-			NULL);
-		if (!err) {
-			info->feature_discard = 1;
-			info->discard_granularity = discard_granularity;
-			info->discard_alignment = discard_alignment;
-		}
-		err = xenbus_gather(XBT_NIL, info->xbdev->otherend,
-			    "discard-secure", "%d", &discard_secure,
-			    NULL);
-		if (!err)
-			info->feature_secdiscard = discard_secure;
-
-	} else if (strncmp(type, "file", 4) == 0)
-		info->feature_discard = 1;
-
-	kfree(type);
+	info->feature_discard = 1;
+	err = xenbus_gather(XBT_NIL, info->xbdev->otherend,
+		"discard-granularity", "%u", &discard_granularity,
+		"discard-alignment", "%u", &discard_alignment,
+		NULL);
+	if (!err) {
+		info->discard_granularity = discard_granularity;
+		info->discard_alignment = discard_alignment;
+	}
+	err = xenbus_gather(XBT_NIL, info->xbdev->otherend,
+		    "discard-secure", "%d", &discard_secure,
+		    NULL);
+	if (!err)
+		info->feature_secdiscard = !!discard_secure;
 }
 
 static int blkfront_setup_indirect(struct blkfront_info *info)
diff --git a/drivers/block/xsysace.c b/drivers/block/xsysace.c
index 1393b8871a2..ab3ea62e5df 100644
--- a/drivers/block/xsysace.c
+++ b/drivers/block/xsysace.c
@@ -661,7 +661,7 @@ static void ace_fsm_dostate(struct ace_device *ace)
 			rq_data_dir(req));
 
 		ace->req = req;
-		ace->data_ptr = req->buffer;
+		ace->data_ptr = bio_data(req->bio);
 		ace->data_count = blk_rq_cur_sectors(req) * ACE_BUF_PER_SECTOR;
 		ace_out32(ace, ACE_MPULBA, blk_rq_pos(req) & 0x0FFFFFFF);
 
@@ -733,7 +733,7 @@ static void ace_fsm_dostate(struct ace_device *ace)
 			 *      blk_rq_sectors(ace->req),
 			 *      blk_rq_cur_sectors(ace->req));
 			 */
-			ace->data_ptr = ace->req->buffer;
+			ace->data_ptr = bio_data(ace->req->bio);
 			ace->data_count = blk_rq_cur_sectors(ace->req) * 16;
 			ace_fsm_yieldirq(ace);
 			break;
diff --git a/drivers/block/z2ram.c b/drivers/block/z2ram.c
index 27de5046708..968f9e52eff 100644
--- a/drivers/block/z2ram.c
+++ b/drivers/block/z2ram.c
@@ -87,13 +87,15 @@ static void do_z2_request(struct request_queue *q)
 		while (len) {
 			unsigned long addr = start & Z2RAM_CHUNKMASK;
 			unsigned long size = Z2RAM_CHUNKSIZE - addr;
+			void *buffer = bio_data(req->bio);
+
 			if (len < size)
 				size = len;
 			addr += z2ram_map[ start >> Z2RAM_CHUNKSHIFT ];
 			if (rq_data_dir(req) == READ)
-				memcpy(req->buffer, (char *)addr, size);
+				memcpy(buffer, (char *)addr, size);
 			else
-				memcpy((char *)addr, req->buffer, size);
+				memcpy((char *)addr, buffer, size);
 			start += size;
 			len -= size;
 		}
diff --git a/drivers/block/zram/zram_drv.c b/drivers/block/zram/zram_drv.c
index 9849b5233bf..48eccb35018 100644
--- a/drivers/block/zram/zram_drv.c
+++ b/drivers/block/zram/zram_drv.c
@@ -572,10 +572,10 @@ static void zram_bio_discard(struct zram *zram, u32 index,
 	 * skipping this logical block is appropriate here.
 	 */
 	if (offset) {
-		if (n < offset)
+		if (n <= (PAGE_SIZE - offset))
 			return;
 
-		n -= offset;
+		n -= (PAGE_SIZE - offset);
 		index++;
 	}
 
diff --git a/drivers/bluetooth/hci_ldisc.c b/drivers/bluetooth/hci_ldisc.c
index f1fbf4f1e5b..e00f8f5b5c8 100644
--- a/drivers/bluetooth/hci_ldisc.c
+++ b/drivers/bluetooth/hci_ldisc.c
@@ -118,10 +118,6 @@ static inline struct sk_buff *hci_uart_dequeue(struct hci_uart *hu)
 
 int hci_uart_tx_wakeup(struct hci_uart *hu)
 {
-	struct tty_struct *tty = hu->tty;
-	struct hci_dev *hdev = hu->hdev;
-	struct sk_buff *skb;
-
 	if (test_and_set_bit(HCI_UART_SENDING, &hu->tx_state)) {
 		set_bit(HCI_UART_TX_WAKEUP, &hu->tx_state);
 		return 0;
@@ -129,6 +125,22 @@ int hci_uart_tx_wakeup(struct hci_uart *hu)
 
 	BT_DBG("");
 
+	schedule_work(&hu->write_work);
+
+	return 0;
+}
+
+static void hci_uart_write_work(struct work_struct *work)
+{
+	struct hci_uart *hu = container_of(work, struct hci_uart, write_work);
+	struct tty_struct *tty = hu->tty;
+	struct hci_dev *hdev = hu->hdev;
+	struct sk_buff *skb;
+
+	/* REVISIT: should we cope with bad skbs or ->write() returning
+	 * and error value ?
+	 */
+
 restart:
 	clear_bit(HCI_UART_TX_WAKEUP, &hu->tx_state);
 
@@ -153,7 +165,6 @@ restart:
 		goto restart;
 
 	clear_bit(HCI_UART_SENDING, &hu->tx_state);
-	return 0;
 }
 
 static void hci_uart_init_work(struct work_struct *work)
@@ -282,6 +293,7 @@ static int hci_uart_tty_open(struct tty_struct *tty)
 	tty->receive_room = 65536;
 
 	INIT_WORK(&hu->init_ready, hci_uart_init_work);
+	INIT_WORK(&hu->write_work, hci_uart_write_work);
 
 	spin_lock_init(&hu->rx_lock);
 
@@ -319,6 +331,8 @@ static void hci_uart_tty_close(struct tty_struct *tty)
 	if (hdev)
 		hci_uart_close(hdev);
 
+	cancel_work_sync(&hu->write_work);
+
 	if (test_and_clear_bit(HCI_UART_PROTO_SET, &hu->flags)) {
 		if (hdev) {
 			if (test_bit(HCI_UART_REGISTERED, &hu->flags))
diff --git a/drivers/bluetooth/hci_uart.h b/drivers/bluetooth/hci_uart.h
index fffa61ff5cb..12df101ca94 100644
--- a/drivers/bluetooth/hci_uart.h
+++ b/drivers/bluetooth/hci_uart.h
@@ -68,6 +68,7 @@ struct hci_uart {
 	unsigned long		hdev_flags;
 
 	struct work_struct	init_ready;
+	struct work_struct	write_work;
 
 	struct hci_uart_proto	*proto;
 	void			*priv;
diff --git a/drivers/bus/Kconfig b/drivers/bus/Kconfig
index 552373c4e36..a118ec1650f 100644
--- a/drivers/bus/Kconfig
+++ b/drivers/bus/Kconfig
@@ -4,6 +4,14 @@
 
 menu "Bus devices"
 
+config BRCMSTB_GISB_ARB
+	bool "Broadcom STB GISB bus arbiter"
+	depends on ARM
+	help
+	  Driver for the Broadcom Set Top Box System-on-a-chip internal bus
+	  arbiter. This driver provides timeout and target abort error handling
+	  and internal bus master decoding.
+
 config IMX_WEIM
 	bool "Freescale EIM DRIVER"
 	depends on ARCH_MXC
@@ -41,4 +49,14 @@ config ARM_CCI
 	help
 	  Driver supporting the CCI cache coherent interconnect for ARM
 	  platforms.
+
+config VEXPRESS_CONFIG
+	bool "Versatile Express configuration bus"
+	default y if ARCH_VEXPRESS
+	depends on ARM || ARM64
+	depends on OF
+	select REGMAP
+	help
+	  Platform configuration infrastructure for the ARM Ltd.
+	  Versatile Express.
 endmenu
diff --git a/drivers/bus/Makefile b/drivers/bus/Makefile
index 8947bdd0de8..6a4ea7e4af1 100644
--- a/drivers/bus/Makefile
+++ b/drivers/bus/Makefile
@@ -2,6 +2,7 @@
 # Makefile for the bus drivers.
 #
 
+obj-$(CONFIG_BRCMSTB_GISB_ARB)	+= brcmstb_gisb.o
 obj-$(CONFIG_IMX_WEIM)	+= imx-weim.o
 obj-$(CONFIG_MVEBU_MBUS) += mvebu-mbus.o
 obj-$(CONFIG_OMAP_OCP2SCP)	+= omap-ocp2scp.o
@@ -10,3 +11,5 @@ obj-$(CONFIG_OMAP_OCP2SCP)	+= omap-ocp2scp.o
 obj-$(CONFIG_OMAP_INTERCONNECT)	+= omap_l3_smx.o omap_l3_noc.o
 # CCI cache coherent interconnect for ARM platforms
 obj-$(CONFIG_ARM_CCI)		+= arm-cci.o
+
+obj-$(CONFIG_VEXPRESS_CONFIG)	+= vexpress-config.o
diff --git a/drivers/bus/brcmstb_gisb.c b/drivers/bus/brcmstb_gisb.c
new file mode 100644
index 00000000000..6159b7752a6
--- /dev/null
+++ b/drivers/bus/brcmstb_gisb.c
@@ -0,0 +1,289 @@
+/*
+ * 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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/types.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/sysfs.h>
+#include <linux/io.h>
+#include <linux/string.h>
+#include <linux/device.h>
+#include <linux/list.h>
+#include <linux/of.h>
+#include <linux/bitops.h>
+
+#include <asm/bug.h>
+#include <asm/signal.h>
+
+#define ARB_TIMER			0x008
+#define ARB_ERR_CAP_CLR			0x7e4
+#define  ARB_ERR_CAP_CLEAR		(1 << 0)
+#define ARB_ERR_CAP_HI_ADDR		0x7e8
+#define ARB_ERR_CAP_ADDR		0x7ec
+#define ARB_ERR_CAP_DATA		0x7f0
+#define ARB_ERR_CAP_STATUS		0x7f4
+#define  ARB_ERR_CAP_STATUS_TIMEOUT	(1 << 12)
+#define  ARB_ERR_CAP_STATUS_TEA		(1 << 11)
+#define  ARB_ERR_CAP_STATUS_BS_SHIFT	(1 << 2)
+#define  ARB_ERR_CAP_STATUS_BS_MASK	0x3c
+#define  ARB_ERR_CAP_STATUS_WRITE	(1 << 1)
+#define  ARB_ERR_CAP_STATUS_VALID	(1 << 0)
+#define ARB_ERR_CAP_MASTER		0x7f8
+
+struct brcmstb_gisb_arb_device {
+	void __iomem	*base;
+	struct mutex	lock;
+	struct list_head next;
+	u32 valid_mask;
+	const char *master_names[sizeof(u32) * BITS_PER_BYTE];
+};
+
+static LIST_HEAD(brcmstb_gisb_arb_device_list);
+
+static ssize_t gisb_arb_get_timeout(struct device *dev,
+				    struct device_attribute *attr,
+				    char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct brcmstb_gisb_arb_device *gdev = platform_get_drvdata(pdev);
+	u32 timeout;
+
+	mutex_lock(&gdev->lock);
+	timeout = ioread32(gdev->base + ARB_TIMER);
+	mutex_unlock(&gdev->lock);
+
+	return sprintf(buf, "%d", timeout);
+}
+
+static ssize_t gisb_arb_set_timeout(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct brcmstb_gisb_arb_device *gdev = platform_get_drvdata(pdev);
+	int val, ret;
+
+	ret = kstrtoint(buf, 10, &val);
+	if (ret < 0)
+		return ret;
+
+	if (val == 0 || val >= 0xffffffff)
+		return -EINVAL;
+
+	mutex_lock(&gdev->lock);
+	iowrite32(val, gdev->base + ARB_TIMER);
+	mutex_unlock(&gdev->lock);
+
+	return count;
+}
+
+static const char *
+brcmstb_gisb_master_to_str(struct brcmstb_gisb_arb_device *gdev,
+						u32 masters)
+{
+	u32 mask = gdev->valid_mask & masters;
+
+	if (hweight_long(mask) != 1)
+		return NULL;
+
+	return gdev->master_names[ffs(mask) - 1];
+}
+
+static int brcmstb_gisb_arb_decode_addr(struct brcmstb_gisb_arb_device *gdev,
+					const char *reason)
+{
+	u32 cap_status;
+	unsigned long arb_addr;
+	u32 master;
+	const char *m_name;
+	char m_fmt[11];
+
+	cap_status = ioread32(gdev->base + ARB_ERR_CAP_STATUS);
+
+	/* Invalid captured address, bail out */
+	if (!(cap_status & ARB_ERR_CAP_STATUS_VALID))
+		return 1;
+
+	/* Read the address and master */
+	arb_addr = ioread32(gdev->base + ARB_ERR_CAP_ADDR) & 0xffffffff;
+#if (IS_ENABLED(CONFIG_PHYS_ADDR_T_64BIT))
+	arb_addr |= (u64)ioread32(gdev->base + ARB_ERR_CAP_HI_ADDR) << 32;
+#endif
+	master = ioread32(gdev->base + ARB_ERR_CAP_MASTER);
+
+	m_name = brcmstb_gisb_master_to_str(gdev, master);
+	if (!m_name) {
+		snprintf(m_fmt, sizeof(m_fmt), "0x%08x", master);
+		m_name = m_fmt;
+	}
+
+	pr_crit("%s: %s at 0x%lx [%c %s], core: %s\n",
+		__func__, reason, arb_addr,
+		cap_status & ARB_ERR_CAP_STATUS_WRITE ? 'W' : 'R',
+		cap_status & ARB_ERR_CAP_STATUS_TIMEOUT ? "timeout" : "",
+		m_name);
+
+	/* clear the GISB error */
+	iowrite32(ARB_ERR_CAP_CLEAR, gdev->base + ARB_ERR_CAP_CLR);
+
+	return 0;
+}
+
+static int brcmstb_bus_error_handler(unsigned long addr, unsigned int fsr,
+				     struct pt_regs *regs)
+{
+	int ret = 0;
+	struct brcmstb_gisb_arb_device *gdev;
+
+	/* iterate over each GISB arb registered handlers */
+	list_for_each_entry(gdev, &brcmstb_gisb_arb_device_list, next)
+		ret |= brcmstb_gisb_arb_decode_addr(gdev, "bus error");
+	/*
+	 * If it was an imprecise abort, then we need to correct the
+	 * return address to be _after_ the instruction.
+	*/
+	if (fsr & (1 << 10))
+		regs->ARM_pc += 4;
+
+	return ret;
+}
+
+void __init brcmstb_hook_fault_code(void)
+{
+	hook_fault_code(22, brcmstb_bus_error_handler, SIGBUS, 0,
+			"imprecise external abort");
+}
+
+static irqreturn_t brcmstb_gisb_timeout_handler(int irq, void *dev_id)
+{
+	brcmstb_gisb_arb_decode_addr(dev_id, "timeout");
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t brcmstb_gisb_tea_handler(int irq, void *dev_id)
+{
+	brcmstb_gisb_arb_decode_addr(dev_id, "target abort");
+
+	return IRQ_HANDLED;
+}
+
+static DEVICE_ATTR(gisb_arb_timeout, S_IWUSR | S_IRUGO,
+		gisb_arb_get_timeout, gisb_arb_set_timeout);
+
+static struct attribute *gisb_arb_sysfs_attrs[] = {
+	&dev_attr_gisb_arb_timeout.attr,
+	NULL,
+};
+
+static struct attribute_group gisb_arb_sysfs_attr_group = {
+	.attrs = gisb_arb_sysfs_attrs,
+};
+
+static int brcmstb_gisb_arb_probe(struct platform_device *pdev)
+{
+	struct device_node *dn = pdev->dev.of_node;
+	struct brcmstb_gisb_arb_device *gdev;
+	struct resource *r;
+	int err, timeout_irq, tea_irq;
+	unsigned int num_masters, j = 0;
+	int i, first, last;
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	timeout_irq = platform_get_irq(pdev, 0);
+	tea_irq = platform_get_irq(pdev, 1);
+
+	gdev = devm_kzalloc(&pdev->dev, sizeof(*gdev), GFP_KERNEL);
+	if (!gdev)
+		return -ENOMEM;
+
+	mutex_init(&gdev->lock);
+	INIT_LIST_HEAD(&gdev->next);
+
+	gdev->base = devm_request_and_ioremap(&pdev->dev, r);
+	if (!gdev->base)
+		return -ENOMEM;
+
+	err = devm_request_irq(&pdev->dev, timeout_irq,
+				brcmstb_gisb_timeout_handler, 0, pdev->name,
+				gdev);
+	if (err < 0)
+		return err;
+
+	err = devm_request_irq(&pdev->dev, tea_irq,
+				brcmstb_gisb_tea_handler, 0, pdev->name,
+				gdev);
+	if (err < 0)
+		return err;
+
+	/* If we do not have a valid mask, assume all masters are enabled */
+	if (of_property_read_u32(dn, "brcm,gisb-arb-master-mask",
+				&gdev->valid_mask))
+		gdev->valid_mask = 0xffffffff;
+
+	/* Proceed with reading the litteral names if we agree on the
+	 * number of masters
+	 */
+	num_masters = of_property_count_strings(dn,
+			"brcm,gisb-arb-master-names");
+	if (hweight_long(gdev->valid_mask) == num_masters) {
+		first = ffs(gdev->valid_mask) - 1;
+		last = fls(gdev->valid_mask) - 1;
+
+		for (i = first; i < last; i++) {
+			if (!(gdev->valid_mask & BIT(i)))
+				continue;
+
+			of_property_read_string_index(dn,
+					"brcm,gisb-arb-master-names", j,
+					&gdev->master_names[i]);
+			j++;
+		}
+	}
+
+	err = sysfs_create_group(&pdev->dev.kobj, &gisb_arb_sysfs_attr_group);
+	if (err)
+		return err;
+
+	platform_set_drvdata(pdev, gdev);
+
+	list_add_tail(&gdev->next, &brcmstb_gisb_arb_device_list);
+
+	dev_info(&pdev->dev, "registered mem: %p, irqs: %d, %d\n",
+			gdev->base, timeout_irq, tea_irq);
+
+	return 0;
+}
+
+static const struct of_device_id brcmstb_gisb_arb_of_match[] = {
+	{ .compatible = "brcm,gisb-arb" },
+	{ },
+};
+
+static struct platform_driver brcmstb_gisb_arb_driver = {
+	.probe	= brcmstb_gisb_arb_probe,
+	.driver = {
+		.name	= "brcm-gisb-arb",
+		.owner	= THIS_MODULE,
+		.of_match_table = brcmstb_gisb_arb_of_match,
+	},
+};
+
+static int __init brcm_gisb_driver_init(void)
+{
+	return platform_driver_register(&brcmstb_gisb_arb_driver);
+}
+
+module_init(brcm_gisb_driver_init);
diff --git a/drivers/bus/mvebu-mbus.c b/drivers/bus/mvebu-mbus.c
index 293e2e0a0a8..26c3779d871 100644
--- a/drivers/bus/mvebu-mbus.c
+++ b/drivers/bus/mvebu-mbus.c
@@ -56,6 +56,7 @@
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/debugfs.h>
+#include <linux/log2.h>
 
 /*
  * DDR target is the same on all platforms.
@@ -222,12 +223,6 @@ static int mvebu_mbus_window_conflicts(struct mvebu_mbus_state *mbus,
 		 */
 		if ((u64)base < wend && end > wbase)
 			return 0;
-
-		/*
-		 * Check if target/attribute conflicts
-		 */
-		if (target == wtarget && attr == wattr)
-			return 0;
 	}
 
 	return 1;
@@ -266,6 +261,17 @@ static int mvebu_mbus_setup_window(struct mvebu_mbus_state *mbus,
 		mbus->soc->win_cfg_offset(win);
 	u32 ctrl, remap_addr;
 
+	if (!is_power_of_2(size)) {
+		WARN(true, "Invalid MBus window size: 0x%zx\n", size);
+		return -EINVAL;
+	}
+
+	if ((base & (phys_addr_t)(size - 1)) != 0) {
+		WARN(true, "Invalid MBus base/size: %pa len 0x%zx\n", &base,
+		     size);
+		return -EINVAL;
+	}
+
 	ctrl = ((size - 1) & WIN_CTRL_SIZE_MASK) |
 		(attr << WIN_CTRL_ATTR_SHIFT)    |
 		(target << WIN_CTRL_TGT_SHIFT)   |
@@ -413,6 +419,10 @@ static int mvebu_devs_debug_show(struct seq_file *seq, void *v)
 			   win, (unsigned long long)wbase,
 			   (unsigned long long)(wbase + wsize), wtarget, wattr);
 
+		if (!is_power_of_2(wsize) ||
+		    ((wbase & (u64)(wsize - 1)) != 0))
+			seq_puts(seq, " (Invalid base/size!!)");
+
 		if (win < mbus->soc->num_remappable_wins) {
 			seq_printf(seq, " (remap %016llx)\n",
 				   (unsigned long long)wremap);
@@ -694,7 +704,6 @@ static int __init mvebu_mbus_common_init(struct mvebu_mbus_state *mbus,
 					 phys_addr_t sdramwins_phys_base,
 					 size_t sdramwins_size)
 {
-	struct device_node *np;
 	int win;
 
 	mbus->mbuswins_base = ioremap(mbuswins_phys_base, mbuswins_size);
@@ -707,12 +716,6 @@ static int __init mvebu_mbus_common_init(struct mvebu_mbus_state *mbus,
 		return -ENOMEM;
 	}
 
-	np = of_find_compatible_node(NULL, NULL, "marvell,coherency-fabric");
-	if (np) {
-		mbus->hw_io_coherency = 1;
-		of_node_put(np);
-	}
-
 	for (win = 0; win < mbus->soc->num_wins; win++)
 		mvebu_mbus_disable_window(mbus, win);
 
@@ -882,7 +885,7 @@ static void __init mvebu_mbus_get_pcie_resources(struct device_node *np,
 	}
 }
 
-int __init mvebu_mbus_dt_init(void)
+int __init mvebu_mbus_dt_init(bool is_coherent)
 {
 	struct resource mbuswins_res, sdramwins_res;
 	struct device_node *np, *controller;
@@ -920,6 +923,8 @@ int __init mvebu_mbus_dt_init(void)
 		return -EINVAL;
 	}
 
+	mbus_state.hw_io_coherency = is_coherent;
+
 	/* Get optional pcie-{mem,io}-aperture properties */
 	mvebu_mbus_get_pcie_resources(np, &mbus_state.pcie_mem_aperture,
 					  &mbus_state.pcie_io_aperture);
diff --git a/drivers/bus/omap_l3_noc.c b/drivers/bus/omap_l3_noc.c
index feeecae623f..531ae591783 100644
--- a/drivers/bus/omap_l3_noc.c
+++ b/drivers/bus/omap_l3_noc.c
@@ -1,43 +1,45 @@
 /*
- * OMAP4XXX L3 Interconnect error handling driver
+ * OMAP L3 Interconnect error handling driver
  *
- * Copyright (C) 2011 Texas Corporation
+ * Copyright (C) 2011-2014 Texas Instruments Incorporated - http://www.ti.com/
  *	Santosh Shilimkar <santosh.shilimkar@ti.com>
  *	Sricharan <r.sricharan@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; either version 2 of the License, or
- * (at your option) any later version.
+ * 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
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
- * USA
  */
-#include <linux/module.h>
 #include <linux/init.h>
-#include <linux/io.h>
-#include <linux/platform_device.h>
 #include <linux/interrupt.h>
+#include <linux/io.h>
 #include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
 #include <linux/slab.h>
 
 #include "omap_l3_noc.h"
 
-/*
- * Interrupt Handler for L3 error detection.
- *	1) Identify the L3 clockdomain partition to which the error belongs to.
- *	2) Identify the slave where the error information is logged
- *	3) Print the logged information.
- *	4) Add dump stack to provide kernel trace.
+/**
+ * l3_handle_target() - Handle Target specific parse and reporting
+ * @l3:		pointer to l3 struct
+ * @base:	base address of clkdm
+ * @flag_mux:	flagmux corresponding to the event
+ * @err_src:	error source index of the slave (target)
  *
- * Two Types of errors :
+ * This does the second part of the error interrupt handling:
+ *	3) Parse in the slave information
+ *	4) Print the logged information.
+ *	5) Add dump stack to provide kernel trace.
+ *	6) Clear the source if known.
+ *
+ * This handles two types of errors:
  *	1) Custom errors in L3 :
  *		Target like DMM/FW/EMIF generates SRESP=ERR error
  *	2) Standard L3 error:
@@ -53,214 +55,264 @@
  *	can be trapped as well. But the trapping is implemented as part
  *	secure software and hence need not be implemented here.
  */
-static irqreturn_t l3_interrupt_handler(int irq, void *_l3)
+static int l3_handle_target(struct omap_l3 *l3, void __iomem *base,
+			    struct l3_flagmux_data *flag_mux, int err_src)
 {
+	int k;
+	u32 std_err_main, clear, masterid;
+	u8 op_code, m_req_info;
+	void __iomem *l3_targ_base;
+	void __iomem *l3_targ_stderr, *l3_targ_slvofslsb, *l3_targ_mstaddr;
+	void __iomem *l3_targ_hdr, *l3_targ_info;
+	struct l3_target_data *l3_targ_inst;
+	struct l3_masters_data *master;
+	char *target_name, *master_name = "UN IDENTIFIED";
+	char *err_description;
+	char err_string[30] = { 0 };
+	char info_string[60] = { 0 };
+
+	/* We DONOT expect err_src to go out of bounds */
+	BUG_ON(err_src > MAX_CLKDM_TARGETS);
+
+	if (err_src < flag_mux->num_targ_data) {
+		l3_targ_inst = &flag_mux->l3_targ[err_src];
+		target_name = l3_targ_inst->name;
+		l3_targ_base = base + l3_targ_inst->offset;
+	} else {
+		target_name = L3_TARGET_NOT_SUPPORTED;
+	}
 
-	struct omap4_l3 *l3 = _l3;
-	int inttype, i, k;
+	if (target_name == L3_TARGET_NOT_SUPPORTED)
+		return -ENODEV;
+
+	/* Read the stderrlog_main_source from clk domain */
+	l3_targ_stderr = l3_targ_base + L3_TARG_STDERRLOG_MAIN;
+	l3_targ_slvofslsb = l3_targ_base + L3_TARG_STDERRLOG_SLVOFSLSB;
+
+	std_err_main = readl_relaxed(l3_targ_stderr);
+
+	switch (std_err_main & CUSTOM_ERROR) {
+	case STANDARD_ERROR:
+		err_description = "Standard";
+		snprintf(err_string, sizeof(err_string),
+			 ": At Address: 0x%08X ",
+			 readl_relaxed(l3_targ_slvofslsb));
+
+		l3_targ_mstaddr = l3_targ_base + L3_TARG_STDERRLOG_MSTADDR;
+		l3_targ_hdr = l3_targ_base + L3_TARG_STDERRLOG_HDR;
+		l3_targ_info = l3_targ_base + L3_TARG_STDERRLOG_INFO;
+		break;
+
+	case CUSTOM_ERROR:
+		err_description = "Custom";
+
+		l3_targ_mstaddr = l3_targ_base +
+				  L3_TARG_STDERRLOG_CINFO_MSTADDR;
+		l3_targ_hdr = l3_targ_base + L3_TARG_STDERRLOG_CINFO_OPCODE;
+		l3_targ_info = l3_targ_base + L3_TARG_STDERRLOG_CINFO_INFO;
+		break;
+
+	default:
+		/* Nothing to be handled here as of now */
+		return 0;
+	}
+
+	/* STDERRLOG_MSTADDR Stores the NTTP master address. */
+	masterid = (readl_relaxed(l3_targ_mstaddr) &
+		    l3->mst_addr_mask) >> __ffs(l3->mst_addr_mask);
+
+	for (k = 0, master = l3->l3_masters; k < l3->num_masters;
+	     k++, master++) {
+		if (masterid == master->id) {
+			master_name = master->name;
+			break;
+		}
+	}
+
+	op_code = readl_relaxed(l3_targ_hdr) & 0x7;
+
+	m_req_info = readl_relaxed(l3_targ_info) & 0xF;
+	snprintf(info_string, sizeof(info_string),
+		 ": %s in %s mode during %s access",
+		 (m_req_info & BIT(0)) ? "Opcode Fetch" : "Data Access",
+		 (m_req_info & BIT(1)) ? "Supervisor" : "User",
+		 (m_req_info & BIT(3)) ? "Debug" : "Functional");
+
+	WARN(true,
+	     "%s:L3 %s Error: MASTER %s TARGET %s (%s)%s%s\n",
+	     dev_name(l3->dev),
+	     err_description,
+	     master_name, target_name,
+	     l3_transaction_type[op_code],
+	     err_string, info_string);
+
+	/* clear the std error log*/
+	clear = std_err_main | CLEAR_STDERR_LOG;
+	writel_relaxed(clear, l3_targ_stderr);
+
+	return 0;
+}
+
+/**
+ * l3_interrupt_handler() - interrupt handler for l3 events
+ * @irq:	irq number
+ * @_l3:	pointer to l3 structure
+ *
+ * Interrupt Handler for L3 error detection.
+ *	1) Identify the L3 clockdomain partition to which the error belongs to.
+ *	2) Identify the slave where the error information is logged
+ *	... handle the slave event..
+ *	7) if the slave is unknown, mask out the slave.
+ */
+static irqreturn_t l3_interrupt_handler(int irq, void *_l3)
+{
+	struct omap_l3 *l3 = _l3;
+	int inttype, i, ret;
 	int err_src = 0;
-	u32 std_err_main, err_reg, clear, masterid;
-	void __iomem *base, *l3_targ_base;
-	char *target_name, *master_name = "UN IDENTIFIED";
+	u32 err_reg, mask_val;
+	void __iomem *base, *mask_reg;
+	struct l3_flagmux_data *flag_mux;
 
 	/* Get the Type of interrupt */
 	inttype = irq == l3->app_irq ? L3_APPLICATION_ERROR : L3_DEBUG_ERROR;
 
-	for (i = 0; i < L3_MODULES; i++) {
+	for (i = 0; i < l3->num_modules; i++) {
 		/*
 		 * Read the regerr register of the clock domain
 		 * to determine the source
 		 */
 		base = l3->l3_base[i];
-		err_reg = __raw_readl(base + l3_flagmux[i] +
-					+ L3_FLAGMUX_REGERR0 + (inttype << 3));
+		flag_mux = l3->l3_flagmux[i];
+		err_reg = readl_relaxed(base + flag_mux->offset +
+					L3_FLAGMUX_REGERR0 + (inttype << 3));
+
+		err_reg &= ~(inttype ? flag_mux->mask_app_bits :
+				flag_mux->mask_dbg_bits);
 
 		/* Get the corresponding error and analyse */
 		if (err_reg) {
 			/* Identify the source from control status register */
 			err_src = __ffs(err_reg);
 
-			/* Read the stderrlog_main_source from clk domain */
-			l3_targ_base = base + *(l3_targ[i] + err_src);
-			std_err_main =  __raw_readl(l3_targ_base +
-					L3_TARG_STDERRLOG_MAIN);
-			masterid = __raw_readl(l3_targ_base +
-					L3_TARG_STDERRLOG_MSTADDR);
-
-			switch (std_err_main & CUSTOM_ERROR) {
-			case STANDARD_ERROR:
-				target_name =
-					l3_targ_inst_name[i][err_src];
-				WARN(true, "L3 standard error: TARGET:%s at address 0x%x\n",
-					target_name,
-					__raw_readl(l3_targ_base +
-						L3_TARG_STDERRLOG_SLVOFSLSB));
-				/* clear the std error log*/
-				clear = std_err_main | CLEAR_STDERR_LOG;
-				writel(clear, l3_targ_base +
-					L3_TARG_STDERRLOG_MAIN);
-				break;
-
-			case CUSTOM_ERROR:
-				target_name =
-					l3_targ_inst_name[i][err_src];
-				for (k = 0; k < NUM_OF_L3_MASTERS; k++) {
-					if (masterid == l3_masters[k].id)
-						master_name =
-							l3_masters[k].name;
-				}
-				WARN(true, "L3 custom error: MASTER:%s TARGET:%s\n",
-					master_name, target_name);
-				/* clear the std error log*/
-				clear = std_err_main | CLEAR_STDERR_LOG;
-				writel(clear, l3_targ_base +
-					L3_TARG_STDERRLOG_MAIN);
-				break;
-
-			default:
-				/* Nothing to be handled here as of now */
-				break;
+			ret = l3_handle_target(l3, base, flag_mux, err_src);
+
+			/*
+			 * Certain plaforms may have "undocumented" status
+			 * pending on boot. So dont generate a severe warning
+			 * here. Just mask it off to prevent the error from
+			 * reoccuring and locking up the system.
+			 */
+			if (ret) {
+				dev_err(l3->dev,
+					"L3 %s error: target %d mod:%d %s\n",
+					inttype ? "debug" : "application",
+					err_src, i, "(unclearable)");
+
+				mask_reg = base + flag_mux->offset +
+					   L3_FLAGMUX_MASK0 + (inttype << 3);
+				mask_val = readl_relaxed(mask_reg);
+				mask_val &= ~(1 << err_src);
+				writel_relaxed(mask_val, mask_reg);
+
+				/* Mark these bits as to be ignored */
+				if (inttype)
+					flag_mux->mask_app_bits |= 1 << err_src;
+				else
+					flag_mux->mask_dbg_bits |= 1 << err_src;
 			}
-		/* Error found so break the for loop */
-		break;
+
+			/* Error found so break the for loop */
+			break;
 		}
 	}
 	return IRQ_HANDLED;
 }
 
-static int omap4_l3_probe(struct platform_device *pdev)
+static const struct of_device_id l3_noc_match[] = {
+	{.compatible = "ti,omap4-l3-noc", .data = &omap_l3_data},
+	{.compatible = "ti,dra7-l3-noc", .data = &dra_l3_data},
+	{.compatible = "ti,am4372-l3-noc", .data = &am4372_l3_data},
+	{},
+};
+MODULE_DEVICE_TABLE(of, l3_noc_match);
+
+static int omap_l3_probe(struct platform_device *pdev)
 {
-	static struct omap4_l3 *l3;
-	struct resource	*res;
-	int ret;
+	const struct of_device_id *of_id;
+	static struct omap_l3 *l3;
+	int ret, i, res_idx;
+
+	of_id = of_match_device(l3_noc_match, &pdev->dev);
+	if (!of_id) {
+		dev_err(&pdev->dev, "OF data missing\n");
+		return -EINVAL;
+	}
 
-	l3 = kzalloc(sizeof(*l3), GFP_KERNEL);
+	l3 = devm_kzalloc(&pdev->dev, sizeof(*l3), GFP_KERNEL);
 	if (!l3)
 		return -ENOMEM;
 
+	memcpy(l3, of_id->data, sizeof(*l3));
+	l3->dev = &pdev->dev;
 	platform_set_drvdata(pdev, l3);
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "couldn't find resource 0\n");
-		ret = -ENODEV;
-		goto err0;
-	}
-
-	l3->l3_base[0] = ioremap(res->start, resource_size(res));
-	if (!l3->l3_base[0]) {
-		dev_err(&pdev->dev, "ioremap failed\n");
-		ret = -ENOMEM;
-		goto err0;
-	}
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-	if (!res) {
-		dev_err(&pdev->dev, "couldn't find resource 1\n");
-		ret = -ENODEV;
-		goto err1;
-	}
-
-	l3->l3_base[1] = ioremap(res->start, resource_size(res));
-	if (!l3->l3_base[1]) {
-		dev_err(&pdev->dev, "ioremap failed\n");
-		ret = -ENOMEM;
-		goto err1;
-	}
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 2);
-	if (!res) {
-		dev_err(&pdev->dev, "couldn't find resource 2\n");
-		ret = -ENODEV;
-		goto err2;
-	}
+	/* Get mem resources */
+	for (i = 0, res_idx = 0; i < l3->num_modules; i++) {
+		struct resource	*res;
 
-	l3->l3_base[2] = ioremap(res->start, resource_size(res));
-	if (!l3->l3_base[2]) {
-		dev_err(&pdev->dev, "ioremap failed\n");
-		ret = -ENOMEM;
-		goto err2;
+		if (l3->l3_base[i] == L3_BASE_IS_SUBMODULE) {
+			/* First entry cannot be submodule */
+			BUG_ON(i == 0);
+			l3->l3_base[i] = l3->l3_base[i - 1];
+			continue;
+		}
+		res = platform_get_resource(pdev, IORESOURCE_MEM, res_idx);
+		l3->l3_base[i] = devm_ioremap_resource(&pdev->dev, res);
+		if (IS_ERR(l3->l3_base[i])) {
+			dev_err(l3->dev, "ioremap %d failed\n", i);
+			return PTR_ERR(l3->l3_base[i]);
+		}
+		res_idx++;
 	}
 
 	/*
 	 * Setup interrupt Handlers
 	 */
 	l3->debug_irq = platform_get_irq(pdev, 0);
-	ret = request_irq(l3->debug_irq,
-			l3_interrupt_handler,
-			IRQF_DISABLED, "l3-dbg-irq", l3);
+	ret = devm_request_irq(l3->dev, l3->debug_irq, l3_interrupt_handler,
+			       IRQF_DISABLED, "l3-dbg-irq", l3);
 	if (ret) {
-		pr_crit("L3: request_irq failed to register for 0x%x\n",
-						l3->debug_irq);
-		goto err3;
+		dev_err(l3->dev, "request_irq failed for %d\n",
+			l3->debug_irq);
+		return ret;
 	}
 
 	l3->app_irq = platform_get_irq(pdev, 1);
-	ret = request_irq(l3->app_irq,
-			l3_interrupt_handler,
-			IRQF_DISABLED, "l3-app-irq", l3);
-	if (ret) {
-		pr_crit("L3: request_irq failed to register for 0x%x\n",
-						l3->app_irq);
-		goto err4;
-	}
+	ret = devm_request_irq(l3->dev, l3->app_irq, l3_interrupt_handler,
+			       IRQF_DISABLED, "l3-app-irq", l3);
+	if (ret)
+		dev_err(l3->dev, "request_irq failed for %d\n", l3->app_irq);
 
-	return 0;
-
-err4:
-	free_irq(l3->debug_irq, l3);
-err3:
-	iounmap(l3->l3_base[2]);
-err2:
-	iounmap(l3->l3_base[1]);
-err1:
-	iounmap(l3->l3_base[0]);
-err0:
-	kfree(l3);
 	return ret;
 }
 
-static int omap4_l3_remove(struct platform_device *pdev)
-{
-	struct omap4_l3 *l3 = platform_get_drvdata(pdev);
-
-	free_irq(l3->app_irq, l3);
-	free_irq(l3->debug_irq, l3);
-	iounmap(l3->l3_base[0]);
-	iounmap(l3->l3_base[1]);
-	iounmap(l3->l3_base[2]);
-	kfree(l3);
-
-	return 0;
-}
-
-#if defined(CONFIG_OF)
-static const struct of_device_id l3_noc_match[] = {
-	{.compatible = "ti,omap4-l3-noc", },
-	{},
-};
-MODULE_DEVICE_TABLE(of, l3_noc_match);
-#else
-#define l3_noc_match NULL
-#endif
-
-static struct platform_driver omap4_l3_driver = {
-	.probe		= omap4_l3_probe,
-	.remove		= omap4_l3_remove,
+static struct platform_driver omap_l3_driver = {
+	.probe		= omap_l3_probe,
 	.driver		= {
 		.name		= "omap_l3_noc",
 		.owner		= THIS_MODULE,
-		.of_match_table = l3_noc_match,
+		.of_match_table = of_match_ptr(l3_noc_match),
 	},
 };
 
-static int __init omap4_l3_init(void)
+static int __init omap_l3_init(void)
 {
-	return platform_driver_register(&omap4_l3_driver);
+	return platform_driver_register(&omap_l3_driver);
 }
-postcore_initcall_sync(omap4_l3_init);
+postcore_initcall_sync(omap_l3_init);
 
-static void __exit omap4_l3_exit(void)
+static void __exit omap_l3_exit(void)
 {
-	platform_driver_unregister(&omap4_l3_driver);
+	platform_driver_unregister(&omap_l3_driver);
 }
-module_exit(omap4_l3_exit);
+module_exit(omap_l3_exit);
diff --git a/drivers/bus/omap_l3_noc.h b/drivers/bus/omap_l3_noc.h
index a6ce34dc481..551e0106143 100644
--- a/drivers/bus/omap_l3_noc.h
+++ b/drivers/bus/omap_l3_noc.h
@@ -1,29 +1,25 @@
 /*
- * OMAP4XXX L3 Interconnect  error handling driver header
+ * OMAP L3 Interconnect  error handling driver header
  *
- * Copyright (C) 2011 Texas Corporation
+ * Copyright (C) 2011-2014 Texas Instruments Incorporated - http://www.ti.com/
  *	Santosh Shilimkar <santosh.shilimkar@ti.com>
  *	sricharan <r.sricharan@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; either version 2 of the License, or
- * (at your option) any later version.
+ * 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
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
- * USA
  */
-#ifndef __ARCH_ARM_MACH_OMAP2_L3_INTERCONNECT_3XXX_H
-#define __ARCH_ARM_MACH_OMAP2_L3_INTERCONNECT_3XXX_H
+#ifndef __OMAP_L3_NOC_H
+#define __OMAP_L3_NOC_H
+
+#define MAX_L3_MODULES			3
+#define MAX_CLKDM_TARGETS		31
 
-#define L3_MODULES			3
 #define CLEAR_STDERR_LOG		(1 << 31)
 #define CUSTOM_ERROR			0x2
 #define STANDARD_ERROR			0x0
@@ -33,63 +29,165 @@
 
 /* L3 TARG register offsets */
 #define L3_TARG_STDERRLOG_MAIN		0x48
+#define L3_TARG_STDERRLOG_HDR		0x4c
+#define L3_TARG_STDERRLOG_MSTADDR	0x50
+#define L3_TARG_STDERRLOG_INFO		0x58
 #define L3_TARG_STDERRLOG_SLVOFSLSB	0x5c
-#define L3_TARG_STDERRLOG_MSTADDR	0x68
+#define L3_TARG_STDERRLOG_CINFO_INFO	0x64
+#define L3_TARG_STDERRLOG_CINFO_MSTADDR	0x68
+#define L3_TARG_STDERRLOG_CINFO_OPCODE	0x6c
 #define L3_FLAGMUX_REGERR0		0xc
+#define L3_FLAGMUX_MASK0		0x8
+
+#define L3_TARGET_NOT_SUPPORTED		NULL
+
+#define L3_BASE_IS_SUBMODULE		((void __iomem *)(1 << 0))
+
+static const char * const l3_transaction_type[] = {
+	/* 0 0 0 */ "Idle",
+	/* 0 0 1 */ "Write",
+	/* 0 1 0 */ "Read",
+	/* 0 1 1 */ "ReadEx",
+	/* 1 0 0 */ "Read Link",
+	/* 1 0 1 */ "Write Non-Posted",
+	/* 1 1 0 */ "Write Conditional",
+	/* 1 1 1 */ "Write Broadcast",
+};
 
-#define NUM_OF_L3_MASTERS	(sizeof(l3_masters)/sizeof(l3_masters[0]))
-
-static u32 l3_flagmux[L3_MODULES] = {
-	0x500,
-	0x1000,
-	0X0200
-};
-
-/* L3 Target standard Error register offsets */
-static u32 l3_targ_inst_clk1[] = {
-	0x100, /* DMM1 */
-	0x200, /* DMM2 */
-	0x300, /* ABE */
-	0x400, /* L4CFG */
-	0x600,  /* CLK2 PWR DISC */
-	0x0,	/* Host CLK1 */
-	0x900	/* L4 Wakeup */
-};
-
-static u32 l3_targ_inst_clk2[] = {
-	0x500, /* CORTEX M3 */
-	0x300, /* DSS */
-	0x100, /* GPMC */
-	0x400, /* ISS */
-	0x700, /* IVAHD */
-	0xD00, /* missing in TRM  corresponds to AES1*/
-	0x900, /* L4 PER0*/
-	0x200, /* OCMRAM */
-	0x100, /* missing in TRM corresponds to GPMC sERROR*/
-	0x600, /* SGX */
-	0x800, /* SL2 */
-	0x1600, /* C2C */
-	0x1100,	/* missing in TRM corresponds PWR DISC CLK1*/
-	0xF00, /* missing in TRM corrsponds to SHA1*/
-	0xE00, /* missing in TRM corresponds to AES2*/
-	0xC00, /* L4 PER3 */
-	0xA00, /* L4 PER1*/
-	0xB00, /* L4 PER2*/
-	0x0, /* HOST CLK2 */
-	0x1800, /* CAL */
-	0x1700 /* LLI */
-};
-
-static u32 l3_targ_inst_clk3[] = {
-	0x0100	/* EMUSS */,
-	0x0300, /* DEBUGSS_CT_TBR */
-	0x0 /* HOST CLK3 */
-};
-
-static struct l3_masters_data {
+/**
+ * struct l3_masters_data - L3 Master information
+ * @id:		ID of the L3 Master
+ * @name:	master name
+ */
+struct l3_masters_data {
 	u32 id;
-	char name[10];
-} l3_masters[] = {
+	char *name;
+};
+
+/**
+ * struct l3_target_data - L3 Target information
+ * @offset:	Offset from base for L3 Target
+ * @name:	Target name
+ *
+ * Target information is organized indexed by bit field definitions.
+ */
+struct l3_target_data {
+	u32 offset;
+	char *name;
+};
+
+/**
+ * struct l3_flagmux_data - Flag Mux information
+ * @offset:	offset from base for flagmux register
+ * @l3_targ:	array indexed by flagmux index (bit offset) pointing to the
+ *		target data. unsupported ones are marked with
+ *		L3_TARGET_NOT_SUPPORTED
+ * @num_targ_data: number of entries in target data
+ * @mask_app_bits: ignore these from raw application irq status
+ * @mask_dbg_bits: ignore these from raw debug irq status
+ */
+struct l3_flagmux_data {
+	u32 offset;
+	struct l3_target_data *l3_targ;
+	u8 num_targ_data;
+	u32 mask_app_bits;
+	u32 mask_dbg_bits;
+};
+
+
+/**
+ * struct omap_l3 - Description of data relevant for L3 bus.
+ * @dev:	device representing the bus (populated runtime)
+ * @l3_base:	base addresses of modules (populated runtime if 0)
+ *		if set to L3_BASE_IS_SUBMODULE, then uses previous
+ *		module index as the base address
+ * @l3_flag_mux: array containing flag mux data per module
+ *		 offset from corresponding module base indexed per
+ *		 module.
+ * @num_modules: number of clock domains / modules.
+ * @l3_masters:	array pointing to master data containing name and register
+ *		offset for the master.
+ * @num_master: number of masters
+ * @mst_addr_mask: Mask representing MSTADDR information of NTTP packet
+ * @debug_irq:	irq number of the debug interrupt (populated runtime)
+ * @app_irq:	irq number of the application interrupt (populated runtime)
+ */
+struct omap_l3 {
+	struct device *dev;
+
+	void __iomem *l3_base[MAX_L3_MODULES];
+	struct l3_flagmux_data **l3_flagmux;
+	int num_modules;
+
+	struct l3_masters_data *l3_masters;
+	int num_masters;
+	u32 mst_addr_mask;
+
+	int debug_irq;
+	int app_irq;
+};
+
+static struct l3_target_data omap_l3_target_data_clk1[] = {
+	{0x100,	"DMM1",},
+	{0x200,	"DMM2",},
+	{0x300,	"ABE",},
+	{0x400,	"L4CFG",},
+	{0x600,	"CLK2PWRDISC",},
+	{0x0,	"HOSTCLK1",},
+	{0x900,	"L4WAKEUP",},
+};
+
+static struct l3_flagmux_data omap_l3_flagmux_clk1 = {
+	.offset = 0x500,
+	.l3_targ = omap_l3_target_data_clk1,
+	.num_targ_data = ARRAY_SIZE(omap_l3_target_data_clk1),
+};
+
+
+static struct l3_target_data omap_l3_target_data_clk2[] = {
+	{0x500,	"CORTEXM3",},
+	{0x300,	"DSS",},
+	{0x100,	"GPMC",},
+	{0x400,	"ISS",},
+	{0x700,	"IVAHD",},
+	{0xD00,	"AES1",},
+	{0x900,	"L4PER0",},
+	{0x200,	"OCMRAM",},
+	{0x100,	"GPMCsERROR",},
+	{0x600,	"SGX",},
+	{0x800,	"SL2",},
+	{0x1600, "C2C",},
+	{0x1100, "PWRDISCCLK1",},
+	{0xF00,	"SHA1",},
+	{0xE00,	"AES2",},
+	{0xC00,	"L4PER3",},
+	{0xA00,	"L4PER1",},
+	{0xB00,	"L4PER2",},
+	{0x0,	"HOSTCLK2",},
+	{0x1800, "CAL",},
+	{0x1700, "LLI",},
+};
+
+static struct l3_flagmux_data omap_l3_flagmux_clk2 = {
+	.offset = 0x1000,
+	.l3_targ = omap_l3_target_data_clk2,
+	.num_targ_data = ARRAY_SIZE(omap_l3_target_data_clk2),
+};
+
+
+static struct l3_target_data omap_l3_target_data_clk3[] = {
+	{0x0100, "EMUSS",},
+	{0x0300, "DEBUG SOURCE",},
+	{0x0,	"HOST CLK3",},
+};
+
+static struct l3_flagmux_data omap_l3_flagmux_clk3 = {
+	.offset = 0x0200,
+	.l3_targ = omap_l3_target_data_clk3,
+	.num_targ_data = ARRAY_SIZE(omap_l3_target_data_clk3),
+};
+
+static struct l3_masters_data omap_l3_masters[] = {
 	{ 0x0 , "MPU"},
 	{ 0x10, "CS_ADP"},
 	{ 0x14, "xxx"},
@@ -117,60 +215,261 @@ static struct l3_masters_data {
 	{ 0xC8, "USBHOSTFS"}
 };
 
-static char *l3_targ_inst_name[L3_MODULES][21] = {
-	{
-		"DMM1",
-		"DMM2",
-		"ABE",
-		"L4CFG",
-		"CLK2 PWR DISC",
-		"HOST CLK1",
-		"L4 WAKEUP"
-	},
-	{
-		"CORTEX M3" ,
-		"DSS ",
-		"GPMC ",
-		"ISS ",
-		"IVAHD ",
-		"AES1",
-		"L4 PER0",
-		"OCMRAM ",
-		"GPMC sERROR",
-		"SGX ",
-		"SL2 ",
-		"C2C ",
-		"PWR DISC CLK1",
-		"SHA1",
-		"AES2",
-		"L4 PER3",
-		"L4 PER1",
-		"L4 PER2",
-		"HOST CLK2",
-		"CAL",
-		"LLI"
-	},
-	{
-		"EMUSS",
-		"DEBUG SOURCE",
-		"HOST CLK3"
-	},
-};
-
-static u32 *l3_targ[L3_MODULES] = {
-	l3_targ_inst_clk1,
-	l3_targ_inst_clk2,
-	l3_targ_inst_clk3,
-};
-
-struct omap4_l3 {
-	struct device *dev;
-	struct clk *ick;
+static struct l3_flagmux_data *omap_l3_flagmux[] = {
+	&omap_l3_flagmux_clk1,
+	&omap_l3_flagmux_clk2,
+	&omap_l3_flagmux_clk3,
+};
+
+static const struct omap_l3 omap_l3_data = {
+	.l3_flagmux = omap_l3_flagmux,
+	.num_modules = ARRAY_SIZE(omap_l3_flagmux),
+	.l3_masters = omap_l3_masters,
+	.num_masters = ARRAY_SIZE(omap_l3_masters),
+	/* The 6 MSBs of register field used to distinguish initiator */
+	.mst_addr_mask = 0xFC,
+};
 
-	/* memory base */
-	void __iomem *l3_base[L3_MODULES];
+/* DRA7 data */
+static struct l3_target_data dra_l3_target_data_clk1[] = {
+	{0x2a00, "AES1",},
+	{0x0200, "DMM_P1",},
+	{0x0600, "DSP2_SDMA",},
+	{0x0b00, "EVE2",},
+	{0x1300, "DMM_P2",},
+	{0x2c00, "AES2",},
+	{0x0300, "DSP1_SDMA",},
+	{0x0a00, "EVE1",},
+	{0x0c00, "EVE3",},
+	{0x0d00, "EVE4",},
+	{0x2900, "DSS",},
+	{0x0100, "GPMC",},
+	{0x3700, "PCIE1",},
+	{0x1600, "IVA_CONFIG",},
+	{0x1800, "IVA_SL2IF",},
+	{0x0500, "L4_CFG",},
+	{0x1d00, "L4_WKUP",},
+	{0x3800, "PCIE2",},
+	{0x3300, "SHA2_1",},
+	{0x1200, "GPU",},
+	{0x1000, "IPU1",},
+	{0x1100, "IPU2",},
+	{0x2000, "TPCC_EDMA",},
+	{0x2e00, "TPTC1_EDMA",},
+	{0x2b00, "TPTC2_EDMA",},
+	{0x0700, "VCP1",},
+	{0x2500, "L4_PER2_P3",},
+	{0x0e00, "L4_PER3_P3",},
+	{0x2200, "MMU1",},
+	{0x1400, "PRUSS1",},
+	{0x1500, "PRUSS2"},
+	{0x0800, "VCP1",},
+};
 
-	int debug_irq;
-	int app_irq;
+static struct l3_flagmux_data dra_l3_flagmux_clk1 = {
+	.offset = 0x803500,
+	.l3_targ = dra_l3_target_data_clk1,
+	.num_targ_data = ARRAY_SIZE(dra_l3_target_data_clk1),
+};
+
+static struct l3_target_data dra_l3_target_data_clk2[] = {
+	{0x0,	"HOST CLK1",},
+	{0x0,	"HOST CLK2",},
+	{0xdead, L3_TARGET_NOT_SUPPORTED,},
+	{0x3400, "SHA2_2",},
+	{0x0900, "BB2D",},
+	{0xdead, L3_TARGET_NOT_SUPPORTED,},
+	{0x2100, "L4_PER1_P3",},
+	{0x1c00, "L4_PER1_P1",},
+	{0x1f00, "L4_PER1_P2",},
+	{0x2300, "L4_PER2_P1",},
+	{0x2400, "L4_PER2_P2",},
+	{0x2600, "L4_PER3_P1",},
+	{0x2700, "L4_PER3_P2",},
+	{0x2f00, "MCASP1",},
+	{0x3000, "MCASP2",},
+	{0x3100, "MCASP3",},
+	{0x2800, "MMU2",},
+	{0x0f00, "OCMC_RAM1",},
+	{0x1700, "OCMC_RAM2",},
+	{0x1900, "OCMC_RAM3",},
+	{0x1e00, "OCMC_ROM",},
+	{0x3900, "QSPI",},
+};
+
+static struct l3_flagmux_data dra_l3_flagmux_clk2 = {
+	.offset = 0x803600,
+	.l3_targ = dra_l3_target_data_clk2,
+	.num_targ_data = ARRAY_SIZE(dra_l3_target_data_clk2),
+};
+
+static struct l3_target_data dra_l3_target_data_clk3[] = {
+	{0x0100, "L3_INSTR"},
+	{0x0300, "DEBUGSS_CT_TBR"},
+	{0x0,	 "HOST CLK3"},
+};
+
+static struct l3_flagmux_data dra_l3_flagmux_clk3 = {
+	.offset = 0x200,
+	.l3_targ = dra_l3_target_data_clk3,
+	.num_targ_data = ARRAY_SIZE(dra_l3_target_data_clk3),
+};
+
+static struct l3_masters_data dra_l3_masters[] = {
+	{ 0x0, "MPU" },
+	{ 0x4, "CS_DAP" },
+	{ 0x5, "IEEE1500_2_OCP" },
+	{ 0x8, "DSP1_MDMA" },
+	{ 0x9, "DSP1_CFG" },
+	{ 0xA, "DSP1_DMA" },
+	{ 0xB, "DSP2_MDMA" },
+	{ 0xC, "DSP2_CFG" },
+	{ 0xD, "DSP2_DMA" },
+	{ 0xE, "IVA" },
+	{ 0x10, "EVE1_P1" },
+	{ 0x11, "EVE2_P1" },
+	{ 0x12, "EVE3_P1" },
+	{ 0x13, "EVE4_P1" },
+	{ 0x14, "PRUSS1 PRU1" },
+	{ 0x15, "PRUSS1 PRU2" },
+	{ 0x16, "PRUSS2 PRU1" },
+	{ 0x17, "PRUSS2 PRU2" },
+	{ 0x18, "IPU1" },
+	{ 0x19, "IPU2" },
+	{ 0x1A, "SDMA" },
+	{ 0x1B, "CDMA" },
+	{ 0x1C, "TC1_EDMA" },
+	{ 0x1D, "TC2_EDMA" },
+	{ 0x20, "DSS" },
+	{ 0x21, "MMU1" },
+	{ 0x22, "PCIE1" },
+	{ 0x23, "MMU2" },
+	{ 0x24, "VIP1" },
+	{ 0x25, "VIP2" },
+	{ 0x26, "VIP3" },
+	{ 0x27, "VPE" },
+	{ 0x28, "GPU_P1" },
+	{ 0x29, "BB2D" },
+	{ 0x29, "GPU_P2" },
+	{ 0x2B, "GMAC_SW" },
+	{ 0x2C, "USB3" },
+	{ 0x2D, "USB2_SS" },
+	{ 0x2E, "USB2_ULPI_SS1" },
+	{ 0x2F, "USB2_ULPI_SS2" },
+	{ 0x30, "CSI2_1" },
+	{ 0x31, "CSI2_2" },
+	{ 0x33, "SATA" },
+	{ 0x34, "EVE1_P2" },
+	{ 0x35, "EVE2_P2" },
+	{ 0x36, "EVE3_P2" },
+	{ 0x37, "EVE4_P2" }
 };
-#endif
+
+static struct l3_flagmux_data *dra_l3_flagmux[] = {
+	&dra_l3_flagmux_clk1,
+	&dra_l3_flagmux_clk2,
+	&dra_l3_flagmux_clk3,
+};
+
+static const struct omap_l3 dra_l3_data = {
+	.l3_base = { [1] = L3_BASE_IS_SUBMODULE },
+	.l3_flagmux = dra_l3_flagmux,
+	.num_modules = ARRAY_SIZE(dra_l3_flagmux),
+	.l3_masters = dra_l3_masters,
+	.num_masters = ARRAY_SIZE(dra_l3_masters),
+	/* The 6 MSBs of register field used to distinguish initiator */
+	.mst_addr_mask = 0xFC,
+};
+
+/* AM4372 data */
+static struct l3_target_data am4372_l3_target_data_200f[] = {
+	{0xf00,  "EMIF",},
+	{0x1200, "DES",},
+	{0x400,  "OCMCRAM",},
+	{0x700,  "TPTC0",},
+	{0x800,  "TPTC1",},
+	{0x900,  "TPTC2"},
+	{0xb00,  "TPCC",},
+	{0xd00,  "DEBUGSS",},
+	{0xdead, L3_TARGET_NOT_SUPPORTED,},
+	{0x200,  "SHA",},
+	{0xc00,  "SGX530",},
+	{0x500,  "AES0",},
+	{0xa00,  "L4_FAST",},
+	{0x300,  "MPUSS_L2_RAM",},
+	{0x100,  "ICSS",},
+};
+
+static struct l3_flagmux_data am4372_l3_flagmux_200f = {
+	.offset = 0x1000,
+	.l3_targ = am4372_l3_target_data_200f,
+	.num_targ_data = ARRAY_SIZE(am4372_l3_target_data_200f),
+};
+
+static struct l3_target_data am4372_l3_target_data_100s[] = {
+	{0x100, "L4_PER_0",},
+	{0x200, "L4_PER_1",},
+	{0x300, "L4_PER_2",},
+	{0x400, "L4_PER_3",},
+	{0x800, "McASP0",},
+	{0x900, "McASP1",},
+	{0xC00, "MMCHS2",},
+	{0x700, "GPMC",},
+	{0xD00, "L4_FW",},
+	{0xdead, L3_TARGET_NOT_SUPPORTED,},
+	{0x500, "ADCTSC",},
+	{0xE00, "L4_WKUP",},
+	{0xA00, "MAG_CARD",},
+};
+
+static struct l3_flagmux_data am4372_l3_flagmux_100s = {
+	.offset = 0x600,
+	.l3_targ = am4372_l3_target_data_100s,
+	.num_targ_data = ARRAY_SIZE(am4372_l3_target_data_100s),
+};
+
+static struct l3_masters_data am4372_l3_masters[] = {
+	{ 0x0, "M1 (128-bit)"},
+	{ 0x1, "M2 (64-bit)"},
+	{ 0x4, "DAP"},
+	{ 0x5, "P1500"},
+	{ 0xC, "ICSS0"},
+	{ 0xD, "ICSS1"},
+	{ 0x14, "Wakeup Processor"},
+	{ 0x18, "TPTC0 Read"},
+	{ 0x19, "TPTC0 Write"},
+	{ 0x1A, "TPTC1 Read"},
+	{ 0x1B, "TPTC1 Write"},
+	{ 0x1C, "TPTC2 Read"},
+	{ 0x1D, "TPTC2 Write"},
+	{ 0x20, "SGX530"},
+	{ 0x21, "OCP WP Traffic Probe"},
+	{ 0x22, "OCP WP DMA Profiling"},
+	{ 0x23, "OCP WP Event Trace"},
+	{ 0x25, "DSS"},
+	{ 0x28, "Crypto DMA RD"},
+	{ 0x29, "Crypto DMA WR"},
+	{ 0x2C, "VPFE0"},
+	{ 0x2D, "VPFE1"},
+	{ 0x30, "GEMAC"},
+	{ 0x34, "USB0 RD"},
+	{ 0x35, "USB0 WR"},
+	{ 0x36, "USB1 RD"},
+	{ 0x37, "USB1 WR"},
+};
+
+static struct l3_flagmux_data *am4372_l3_flagmux[] = {
+	&am4372_l3_flagmux_200f,
+	&am4372_l3_flagmux_100s,
+};
+
+static const struct omap_l3 am4372_l3_data = {
+	.l3_flagmux = am4372_l3_flagmux,
+	.num_modules = ARRAY_SIZE(am4372_l3_flagmux),
+	.l3_masters = am4372_l3_masters,
+	.num_masters = ARRAY_SIZE(am4372_l3_masters),
+	/* All 6 bits of register field used to distinguish initiator */
+	.mst_addr_mask = 0x3F,
+};
+
+#endif	/* __OMAP_L3_NOC_H */
diff --git a/drivers/bus/vexpress-config.c b/drivers/bus/vexpress-config.c
new file mode 100644
index 00000000000..a64763b6b5f
--- /dev/null
+++ b/drivers/bus/vexpress-config.c
@@ -0,0 +1,202 @@
+/*
+ * 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.
+ *
+ * Copyright (C) 2014 ARM Limited
+ */
+
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/vexpress.h>
+
+
+struct vexpress_config_bridge {
+	struct vexpress_config_bridge_ops *ops;
+	void *context;
+};
+
+
+static DEFINE_MUTEX(vexpress_config_mutex);
+static struct class *vexpress_config_class;
+static u32 vexpress_config_site_master = VEXPRESS_SITE_MASTER;
+
+
+void vexpress_config_set_master(u32 site)
+{
+	vexpress_config_site_master = site;
+}
+
+u32 vexpress_config_get_master(void)
+{
+	return vexpress_config_site_master;
+}
+
+void vexpress_config_lock(void *arg)
+{
+	mutex_lock(&vexpress_config_mutex);
+}
+
+void vexpress_config_unlock(void *arg)
+{
+	mutex_unlock(&vexpress_config_mutex);
+}
+
+
+static void vexpress_config_find_prop(struct device_node *node,
+		const char *name, u32 *val)
+{
+	/* Default value */
+	*val = 0;
+
+	of_node_get(node);
+	while (node) {
+		if (of_property_read_u32(node, name, val) == 0) {
+			of_node_put(node);
+			return;
+		}
+		node = of_get_next_parent(node);
+	}
+}
+
+int vexpress_config_get_topo(struct device_node *node, u32 *site,
+		u32 *position, u32 *dcc)
+{
+	vexpress_config_find_prop(node, "arm,vexpress,site", site);
+	if (*site == VEXPRESS_SITE_MASTER)
+		*site = vexpress_config_site_master;
+	if (WARN_ON(vexpress_config_site_master == VEXPRESS_SITE_MASTER))
+		return -EINVAL;
+	vexpress_config_find_prop(node, "arm,vexpress,position", position);
+	vexpress_config_find_prop(node, "arm,vexpress,dcc", dcc);
+
+	return 0;
+}
+
+
+static void vexpress_config_devres_release(struct device *dev, void *res)
+{
+	struct vexpress_config_bridge *bridge = dev_get_drvdata(dev->parent);
+	struct regmap *regmap = res;
+
+	bridge->ops->regmap_exit(regmap, bridge->context);
+}
+
+struct regmap *devm_regmap_init_vexpress_config(struct device *dev)
+{
+	struct vexpress_config_bridge *bridge;
+	struct regmap *regmap;
+	struct regmap **res;
+
+	if (WARN_ON(dev->parent->class != vexpress_config_class))
+		return ERR_PTR(-ENODEV);
+
+	bridge = dev_get_drvdata(dev->parent);
+	if (WARN_ON(!bridge))
+		return ERR_PTR(-EINVAL);
+
+	res = devres_alloc(vexpress_config_devres_release, sizeof(*res),
+			GFP_KERNEL);
+	if (!res)
+		return ERR_PTR(-ENOMEM);
+
+	regmap = bridge->ops->regmap_init(dev, bridge->context);
+	if (IS_ERR(regmap)) {
+		devres_free(res);
+		return regmap;
+	}
+
+	*res = regmap;
+	devres_add(dev, res);
+
+	return regmap;
+}
+EXPORT_SYMBOL_GPL(devm_regmap_init_vexpress_config);
+
+struct device *vexpress_config_bridge_register(struct device *parent,
+		struct vexpress_config_bridge_ops *ops, void *context)
+{
+	struct device *dev;
+	struct vexpress_config_bridge *bridge;
+
+	if (!vexpress_config_class) {
+		vexpress_config_class = class_create(THIS_MODULE,
+				"vexpress-config");
+		if (IS_ERR(vexpress_config_class))
+			return (void *)vexpress_config_class;
+	}
+
+	dev = device_create(vexpress_config_class, parent, 0,
+			NULL, "%s.bridge", dev_name(parent));
+
+	if (IS_ERR(dev))
+		return dev;
+
+	bridge = devm_kmalloc(dev, sizeof(*bridge), GFP_KERNEL);
+	if (!bridge) {
+		put_device(dev);
+		device_unregister(dev);
+		return ERR_PTR(-ENOMEM);
+	}
+	bridge->ops = ops;
+	bridge->context = context;
+
+	dev_set_drvdata(dev, bridge);
+
+	dev_dbg(parent, "Registered bridge '%s', parent node %p\n",
+			dev_name(dev), parent->of_node);
+
+	return dev;
+}
+
+
+static int vexpress_config_node_match(struct device *dev, const void *data)
+{
+	const struct device_node *node = data;
+
+	dev_dbg(dev, "Parent node %p, looking for %p\n",
+			dev->parent->of_node, node);
+
+	return dev->parent->of_node == node;
+}
+
+static int vexpress_config_populate(struct device_node *node)
+{
+	struct device_node *bridge;
+	struct device *parent;
+
+	bridge = of_parse_phandle(node, "arm,vexpress,config-bridge", 0);
+	if (!bridge)
+		return -EINVAL;
+
+	parent = class_find_device(vexpress_config_class, NULL, bridge,
+			vexpress_config_node_match);
+	if (WARN_ON(!parent))
+		return -ENODEV;
+
+	return of_platform_populate(node, NULL, NULL, parent);
+}
+
+static int __init vexpress_config_init(void)
+{
+	int err = 0;
+	struct device_node *node;
+
+	/* Need the config devices early, before the "normal" devices... */
+	for_each_compatible_node(node, NULL, "arm,vexpress,config-bus") {
+		err = vexpress_config_populate(node);
+		if (err)
+			break;
+	}
+
+	return err;
+}
+postcore_initcall(vexpress_config_init);
+
diff --git a/drivers/cdrom/cdrom.c b/drivers/cdrom/cdrom.c
index 8a3aff724d9..898b84bba28 100644
--- a/drivers/cdrom/cdrom.c
+++ b/drivers/cdrom/cdrom.c
@@ -312,36 +312,24 @@ static const char *mrw_format_status[] = {
 
 static const char *mrw_address_space[] = { "DMA", "GAA" };
 
-#if (ERRLOGMASK!=CD_NOTHING)
-#define cdinfo(type, fmt, args...)			\
+#if (ERRLOGMASK != CD_NOTHING)
+#define cd_dbg(type, fmt, ...)				\
 do {							\
 	if ((ERRLOGMASK & type) || debug == 1)		\
-		pr_info(fmt, ##args);			\
+		pr_debug(fmt, ##__VA_ARGS__);		\
 } while (0)
 #else
-#define cdinfo(type, fmt, args...)			\
+#define cd_dbg(type, fmt, ...)				\
 do {							\
 	if (0 && (ERRLOGMASK & type) || debug == 1)	\
-		pr_info(fmt, ##args);			\
+		pr_debug(fmt, ##__VA_ARGS__);		\
 } while (0)
 #endif
 
-/* These are used to simplify getting data in from and back to user land */
-#define IOCTL_IN(arg, type, in)					\
-	if (copy_from_user(&(in), (type __user *) (arg), sizeof (in)))	\
-		return -EFAULT;
-
-#define IOCTL_OUT(arg, type, out) \
-	if (copy_to_user((type __user *) (arg), &(out), sizeof (out)))	\
-		return -EFAULT;
-
 /* The (cdo->capability & ~cdi->mask & CDC_XXX) construct was used in
    a lot of places. This macro makes the code more clear. */
 #define CDROM_CAN(type) (cdi->ops->capability & ~cdi->mask & (type))
 
-/* used in the audio ioctls */
-#define CHECKAUDIO if ((ret=check_for_audio_disc(cdi, cdo))) return ret
-
 /*
  * Another popular OS uses 7 seconds as the hard timeout for default
  * commands, so it is a good choice for us as well.
@@ -349,21 +337,6 @@ do {							\
 #define CDROM_DEF_TIMEOUT	(7 * HZ)
 
 /* Not-exported routines. */
-static int open_for_data(struct cdrom_device_info * cdi);
-static int check_for_audio_disc(struct cdrom_device_info * cdi,
-			 struct cdrom_device_ops * cdo);
-static void sanitize_format(union cdrom_addr *addr, 
-		u_char * curr, u_char requested);
-static int mmc_ioctl(struct cdrom_device_info *cdi, unsigned int cmd,
-		     unsigned long arg);
-
-int cdrom_get_last_written(struct cdrom_device_info *, long *);
-static int cdrom_get_next_writable(struct cdrom_device_info *, long *);
-static void cdrom_count_tracks(struct cdrom_device_info *, tracktype*);
-
-static int cdrom_mrw_exit(struct cdrom_device_info *cdi);
-
-static int cdrom_get_disc_info(struct cdrom_device_info *cdi, disc_information *di);
 
 static void cdrom_sysctl_register(void);
 
@@ -382,113 +355,65 @@ static int cdrom_dummy_generic_packet(struct cdrom_device_info *cdi,
 	return -EIO;
 }
 
-/* This macro makes sure we don't have to check on cdrom_device_ops
- * existence in the run-time routines below. Change_capability is a
- * hack to have the capability flags defined const, while we can still
- * change it here without gcc complaining at every line.
- */
-#define ENSURE(call, bits) if (cdo->call == NULL) *change_capability &= ~(bits)
-
-int register_cdrom(struct cdrom_device_info *cdi)
-{
-	static char banner_printed;
-        struct cdrom_device_ops *cdo = cdi->ops;
-        int *change_capability = (int *)&cdo->capability; /* hack */
-
-	cdinfo(CD_OPEN, "entering register_cdrom\n"); 
-
-	if (cdo->open == NULL || cdo->release == NULL)
-		return -EINVAL;
-	if (!banner_printed) {
-		pr_info("Uniform CD-ROM driver " REVISION "\n");
-		banner_printed = 1;
-		cdrom_sysctl_register();
-	}
-
-	ENSURE(drive_status, CDC_DRIVE_STATUS );
-	if (cdo->check_events == NULL && cdo->media_changed == NULL)
-		*change_capability = ~(CDC_MEDIA_CHANGED | CDC_SELECT_DISC);
-	ENSURE(tray_move, CDC_CLOSE_TRAY | CDC_OPEN_TRAY);
-	ENSURE(lock_door, CDC_LOCK);
-	ENSURE(select_speed, CDC_SELECT_SPEED);
-	ENSURE(get_last_session, CDC_MULTI_SESSION);
-	ENSURE(get_mcn, CDC_MCN);
-	ENSURE(reset, CDC_RESET);
-	ENSURE(generic_packet, CDC_GENERIC_PACKET);
-	cdi->mc_flags = 0;
-	cdo->n_minors = 0;
-        cdi->options = CDO_USE_FFLAGS;
-	
-	if (autoclose==1 && CDROM_CAN(CDC_CLOSE_TRAY))
-		cdi->options |= (int) CDO_AUTO_CLOSE;
-	if (autoeject==1 && CDROM_CAN(CDC_OPEN_TRAY))
-		cdi->options |= (int) CDO_AUTO_EJECT;
-	if (lockdoor==1)
-		cdi->options |= (int) CDO_LOCK;
-	if (check_media_type==1)
-		cdi->options |= (int) CDO_CHECK_TYPE;
-
-	if (CDROM_CAN(CDC_MRW_W))
-		cdi->exit = cdrom_mrw_exit;
-
-	if (cdi->disk)
-		cdi->cdda_method = CDDA_BPC_FULL;
-	else
-		cdi->cdda_method = CDDA_OLD;
-
-	if (!cdo->generic_packet)
-		cdo->generic_packet = cdrom_dummy_generic_packet;
-
-	cdinfo(CD_REG_UNREG, "drive \"/dev/%s\" registered\n", cdi->name);
-	mutex_lock(&cdrom_mutex);
-	list_add(&cdi->list, &cdrom_list);
-	mutex_unlock(&cdrom_mutex);
-	return 0;
-}
-#undef ENSURE
-
-void unregister_cdrom(struct cdrom_device_info *cdi)
+static int cdrom_flush_cache(struct cdrom_device_info *cdi)
 {
-	cdinfo(CD_OPEN, "entering unregister_cdrom\n"); 
+	struct packet_command cgc;
 
-	mutex_lock(&cdrom_mutex);
-	list_del(&cdi->list);
-	mutex_unlock(&cdrom_mutex);
+	init_cdrom_command(&cgc, NULL, 0, CGC_DATA_NONE);
+	cgc.cmd[0] = GPCMD_FLUSH_CACHE;
 
-	if (cdi->exit)
-		cdi->exit(cdi);
+	cgc.timeout = 5 * 60 * HZ;
 
-	cdi->ops->n_minors--;
-	cdinfo(CD_REG_UNREG, "drive \"/dev/%s\" unregistered\n", cdi->name);
+	return cdi->ops->generic_packet(cdi, &cgc);
 }
 
-int cdrom_get_media_event(struct cdrom_device_info *cdi,
-			  struct media_event_desc *med)
+/* requires CD R/RW */
+static int cdrom_get_disc_info(struct cdrom_device_info *cdi,
+			       disc_information *di)
 {
+	struct cdrom_device_ops *cdo = cdi->ops;
 	struct packet_command cgc;
-	unsigned char buffer[8];
-	struct event_header *eh = (struct event_header *) buffer;
+	int ret, buflen;
 
-	init_cdrom_command(&cgc, buffer, sizeof(buffer), CGC_DATA_READ);
-	cgc.cmd[0] = GPCMD_GET_EVENT_STATUS_NOTIFICATION;
-	cgc.cmd[1] = 1;		/* IMMED */
-	cgc.cmd[4] = 1 << 4;	/* media event */
-	cgc.cmd[8] = sizeof(buffer);
+	/* set up command and get the disc info */
+	init_cdrom_command(&cgc, di, sizeof(*di), CGC_DATA_READ);
+	cgc.cmd[0] = GPCMD_READ_DISC_INFO;
+	cgc.cmd[8] = cgc.buflen = 2;
 	cgc.quiet = 1;
 
-	if (cdi->ops->generic_packet(cdi, &cgc))
-		return 1;
+	ret = cdo->generic_packet(cdi, &cgc);
+	if (ret)
+		return ret;
 
-	if (be16_to_cpu(eh->data_len) < sizeof(*med))
-		return 1;
+	/* not all drives have the same disc_info length, so requeue
+	 * packet with the length the drive tells us it can supply
+	 */
+	buflen = be16_to_cpu(di->disc_information_length) +
+		sizeof(di->disc_information_length);
 
-	if (eh->nea || eh->notification_class != 0x4)
-		return 1;
+	if (buflen > sizeof(disc_information))
+		buflen = sizeof(disc_information);
 
-	memcpy(med, &buffer[sizeof(*eh)], sizeof(*med));
-	return 0;
+	cgc.cmd[8] = cgc.buflen = buflen;
+	ret = cdo->generic_packet(cdi, &cgc);
+	if (ret)
+		return ret;
+
+	/* return actual fill size */
+	return buflen;
 }
 
+/* This macro makes sure we don't have to check on cdrom_device_ops
+ * existence in the run-time routines below. Change_capability is a
+ * hack to have the capability flags defined const, while we can still
+ * change it here without gcc complaining at every line.
+ */
+#define ENSURE(call, bits)			\
+do {						\
+	if (cdo->call == NULL)			\
+		*change_capability &= ~(bits);	\
+} while (0)
+
 /*
  * the first prototypes used 0x2c as the page code for the mrw mode page,
  * subsequently this was changed to 0x03. probe the one used by this drive
@@ -605,18 +530,6 @@ static int cdrom_mrw_bgformat_susp(struct cdrom_device_info *cdi, int immed)
 	return cdi->ops->generic_packet(cdi, &cgc);
 }
 
-static int cdrom_flush_cache(struct cdrom_device_info *cdi)
-{
-	struct packet_command cgc;
-
-	init_cdrom_command(&cgc, NULL, 0, CGC_DATA_NONE);
-	cgc.cmd[0] = GPCMD_FLUSH_CACHE;
-
-	cgc.timeout = 5 * 60 * HZ;
-
-	return cdi->ops->generic_packet(cdi, &cgc);
-}
-
 static int cdrom_mrw_exit(struct cdrom_device_info *cdi)
 {
 	disc_information di;
@@ -650,17 +563,19 @@ static int cdrom_mrw_set_lba_space(struct cdrom_device_info *cdi, int space)
 	cgc.buffer = buffer;
 	cgc.buflen = sizeof(buffer);
 
-	if ((ret = cdrom_mode_sense(cdi, &cgc, cdi->mrw_mode_page, 0)))
+	ret = cdrom_mode_sense(cdi, &cgc, cdi->mrw_mode_page, 0);
+	if (ret)
 		return ret;
 
-	mph = (struct mode_page_header *) buffer;
+	mph = (struct mode_page_header *)buffer;
 	offset = be16_to_cpu(mph->desc_length);
 	size = be16_to_cpu(mph->mode_data_length) + 2;
 
 	buffer[offset + 3] = space;
 	cgc.buflen = size;
 
-	if ((ret = cdrom_mode_select(cdi, &cgc)))
+	ret = cdrom_mode_select(cdi, &cgc);
+	if (ret)
 		return ret;
 
 	pr_info("%s: mrw address space %s selected\n",
@@ -668,6 +583,106 @@ static int cdrom_mrw_set_lba_space(struct cdrom_device_info *cdi, int space)
 	return 0;
 }
 
+int register_cdrom(struct cdrom_device_info *cdi)
+{
+	static char banner_printed;
+	struct cdrom_device_ops *cdo = cdi->ops;
+	int *change_capability = (int *)&cdo->capability; /* hack */
+
+	cd_dbg(CD_OPEN, "entering register_cdrom\n");
+
+	if (cdo->open == NULL || cdo->release == NULL)
+		return -EINVAL;
+	if (!banner_printed) {
+		pr_info("Uniform CD-ROM driver " REVISION "\n");
+		banner_printed = 1;
+		cdrom_sysctl_register();
+	}
+
+	ENSURE(drive_status, CDC_DRIVE_STATUS);
+	if (cdo->check_events == NULL && cdo->media_changed == NULL)
+		*change_capability = ~(CDC_MEDIA_CHANGED | CDC_SELECT_DISC);
+	ENSURE(tray_move, CDC_CLOSE_TRAY | CDC_OPEN_TRAY);
+	ENSURE(lock_door, CDC_LOCK);
+	ENSURE(select_speed, CDC_SELECT_SPEED);
+	ENSURE(get_last_session, CDC_MULTI_SESSION);
+	ENSURE(get_mcn, CDC_MCN);
+	ENSURE(reset, CDC_RESET);
+	ENSURE(generic_packet, CDC_GENERIC_PACKET);
+	cdi->mc_flags = 0;
+	cdo->n_minors = 0;
+	cdi->options = CDO_USE_FFLAGS;
+
+	if (autoclose == 1 && CDROM_CAN(CDC_CLOSE_TRAY))
+		cdi->options |= (int) CDO_AUTO_CLOSE;
+	if (autoeject == 1 && CDROM_CAN(CDC_OPEN_TRAY))
+		cdi->options |= (int) CDO_AUTO_EJECT;
+	if (lockdoor == 1)
+		cdi->options |= (int) CDO_LOCK;
+	if (check_media_type == 1)
+		cdi->options |= (int) CDO_CHECK_TYPE;
+
+	if (CDROM_CAN(CDC_MRW_W))
+		cdi->exit = cdrom_mrw_exit;
+
+	if (cdi->disk)
+		cdi->cdda_method = CDDA_BPC_FULL;
+	else
+		cdi->cdda_method = CDDA_OLD;
+
+	if (!cdo->generic_packet)
+		cdo->generic_packet = cdrom_dummy_generic_packet;
+
+	cd_dbg(CD_REG_UNREG, "drive \"/dev/%s\" registered\n", cdi->name);
+	mutex_lock(&cdrom_mutex);
+	list_add(&cdi->list, &cdrom_list);
+	mutex_unlock(&cdrom_mutex);
+	return 0;
+}
+#undef ENSURE
+
+void unregister_cdrom(struct cdrom_device_info *cdi)
+{
+	cd_dbg(CD_OPEN, "entering unregister_cdrom\n");
+
+	mutex_lock(&cdrom_mutex);
+	list_del(&cdi->list);
+	mutex_unlock(&cdrom_mutex);
+
+	if (cdi->exit)
+		cdi->exit(cdi);
+
+	cdi->ops->n_minors--;
+	cd_dbg(CD_REG_UNREG, "drive \"/dev/%s\" unregistered\n", cdi->name);
+}
+
+int cdrom_get_media_event(struct cdrom_device_info *cdi,
+			  struct media_event_desc *med)
+{
+	struct packet_command cgc;
+	unsigned char buffer[8];
+	struct event_header *eh = (struct event_header *)buffer;
+
+	init_cdrom_command(&cgc, buffer, sizeof(buffer), CGC_DATA_READ);
+	cgc.cmd[0] = GPCMD_GET_EVENT_STATUS_NOTIFICATION;
+	cgc.cmd[1] = 1;		/* IMMED */
+	cgc.cmd[4] = 1 << 4;	/* media event */
+	cgc.cmd[8] = sizeof(buffer);
+	cgc.quiet = 1;
+
+	if (cdi->ops->generic_packet(cdi, &cgc))
+		return 1;
+
+	if (be16_to_cpu(eh->data_len) < sizeof(*med))
+		return 1;
+
+	if (eh->nea || eh->notification_class != 0x4)
+		return 1;
+
+	memcpy(med, &buffer[sizeof(*eh)], sizeof(*med));
+	return 0;
+}
+
 static int cdrom_get_random_writable(struct cdrom_device_info *cdi,
 			      struct rwrt_feature_desc *rfd)
 {
@@ -839,7 +854,7 @@ static int cdrom_ram_open_write(struct cdrom_device_info *cdi)
 	else if (CDF_RWRT == be16_to_cpu(rfd.feature_code))
 		ret = !rfd.curr;
 
-	cdinfo(CD_OPEN, "can open for random write\n");
+	cd_dbg(CD_OPEN, "can open for random write\n");
 	return ret;
 }
 
@@ -928,12 +943,12 @@ static void cdrom_dvd_rw_close_write(struct cdrom_device_info *cdi)
 	struct packet_command cgc;
 
 	if (cdi->mmc3_profile != 0x1a) {
-		cdinfo(CD_CLOSE, "%s: No DVD+RW\n", cdi->name);
+		cd_dbg(CD_CLOSE, "%s: No DVD+RW\n", cdi->name);
 		return;
 	}
 
 	if (!cdi->media_written) {
-		cdinfo(CD_CLOSE, "%s: DVD+RW media clean\n", cdi->name);
+		cd_dbg(CD_CLOSE, "%s: DVD+RW media clean\n", cdi->name);
 		return;
 	}
 
@@ -969,82 +984,74 @@ static int cdrom_close_write(struct cdrom_device_info *cdi)
 #endif
 }
 
-/* We use the open-option O_NONBLOCK to indicate that the
- * purpose of opening is only for subsequent ioctl() calls; no device
- * integrity checks are performed.
- *
- * We hope that all cd-player programs will adopt this convention. It
- * is in their own interest: device control becomes a lot easier
- * this way.
- */
-int cdrom_open(struct cdrom_device_info *cdi, struct block_device *bdev, fmode_t mode)
+/* badly broken, I know. Is due for a fixup anytime. */
+static void cdrom_count_tracks(struct cdrom_device_info *cdi, tracktype *tracks)
 {
-	int ret;
-
-	cdinfo(CD_OPEN, "entering cdrom_open\n"); 
-
-	/* open is event synchronization point, check events first */
-	check_disk_change(bdev);
-
-	/* if this was a O_NONBLOCK open and we should honor the flags,
-	 * do a quick open without drive/disc integrity checks. */
-	cdi->use_count++;
-	if ((mode & FMODE_NDELAY) && (cdi->options & CDO_USE_FFLAGS)) {
-		ret = cdi->ops->open(cdi, 1);
-	} else {
-		ret = open_for_data(cdi);
-		if (ret)
-			goto err;
-		cdrom_mmc3_profile(cdi);
-		if (mode & FMODE_WRITE) {
-			ret = -EROFS;
-			if (cdrom_open_write(cdi))
-				goto err_release;
-			if (!CDROM_CAN(CDC_RAM))
-				goto err_release;
-			ret = 0;
-			cdi->media_written = 0;
-		}
+	struct cdrom_tochdr header;
+	struct cdrom_tocentry entry;
+	int ret, i;
+	tracks->data = 0;
+	tracks->audio = 0;
+	tracks->cdi = 0;
+	tracks->xa = 0;
+	tracks->error = 0;
+	cd_dbg(CD_COUNT_TRACKS, "entering cdrom_count_tracks\n");
+	/* Grab the TOC header so we can see how many tracks there are */
+	ret = cdi->ops->audio_ioctl(cdi, CDROMREADTOCHDR, &header);
+	if (ret) {
+		if (ret == -ENOMEDIUM)
+			tracks->error = CDS_NO_DISC;
+		else
+			tracks->error = CDS_NO_INFO;
+		return;
 	}
-
-	if (ret)
-		goto err;
-
-	cdinfo(CD_OPEN, "Use count for \"/dev/%s\" now %d\n",
-			cdi->name, cdi->use_count);
-	return 0;
-err_release:
-	if (CDROM_CAN(CDC_LOCK) && cdi->options & CDO_LOCK) {
-		cdi->ops->lock_door(cdi, 0);
-		cdinfo(CD_OPEN, "door unlocked.\n");
+	/* check what type of tracks are on this disc */
+	entry.cdte_format = CDROM_MSF;
+	for (i = header.cdth_trk0; i <= header.cdth_trk1; i++) {
+		entry.cdte_track = i;
+		if (cdi->ops->audio_ioctl(cdi, CDROMREADTOCENTRY, &entry)) {
+			tracks->error = CDS_NO_INFO;
+			return;
+		}
+		if (entry.cdte_ctrl & CDROM_DATA_TRACK) {
+			if (entry.cdte_format == 0x10)
+				tracks->cdi++;
+			else if (entry.cdte_format == 0x20)
+				tracks->xa++;
+			else
+				tracks->data++;
+		} else {
+			tracks->audio++;
+		}
+		cd_dbg(CD_COUNT_TRACKS, "track %d: format=%d, ctrl=%d\n",
+		       i, entry.cdte_format, entry.cdte_ctrl);
 	}
-	cdi->ops->release(cdi);
-err:
-	cdi->use_count--;
-	return ret;
+	cd_dbg(CD_COUNT_TRACKS, "disc has %d tracks: %d=audio %d=data %d=Cd-I %d=XA\n",
+	       header.cdth_trk1, tracks->audio, tracks->data,
+	       tracks->cdi, tracks->xa);
 }
 
 static
-int open_for_data(struct cdrom_device_info * cdi)
+int open_for_data(struct cdrom_device_info *cdi)
 {
 	int ret;
 	struct cdrom_device_ops *cdo = cdi->ops;
 	tracktype tracks;
-	cdinfo(CD_OPEN, "entering open_for_data\n");
+	cd_dbg(CD_OPEN, "entering open_for_data\n");
 	/* Check if the driver can report drive status.  If it can, we
 	   can do clever things.  If it can't, well, we at least tried! */
 	if (cdo->drive_status != NULL) {
 		ret = cdo->drive_status(cdi, CDSL_CURRENT);
-		cdinfo(CD_OPEN, "drive_status=%d\n", ret); 
+		cd_dbg(CD_OPEN, "drive_status=%d\n", ret);
 		if (ret == CDS_TRAY_OPEN) {
-			cdinfo(CD_OPEN, "the tray is open...\n"); 
+			cd_dbg(CD_OPEN, "the tray is open...\n");
 			/* can/may i close it? */
 			if (CDROM_CAN(CDC_CLOSE_TRAY) &&
 			    cdi->options & CDO_AUTO_CLOSE) {
-				cdinfo(CD_OPEN, "trying to close the tray.\n"); 
+				cd_dbg(CD_OPEN, "trying to close the tray\n");
 				ret=cdo->tray_move(cdi,0);
 				if (ret) {
-					cdinfo(CD_OPEN, "bummer. tried to close the tray but failed.\n"); 
+					cd_dbg(CD_OPEN, "bummer. tried to close the tray but failed.\n");
 					/* Ignore the error from the low
 					level driver.  We don't care why it
 					couldn't close the tray.  We only care 
@@ -1054,19 +1061,19 @@ int open_for_data(struct cdrom_device_info * cdi)
 					goto clean_up_and_return;
 				}
 			} else {
-				cdinfo(CD_OPEN, "bummer. this drive can't close the tray.\n"); 
+				cd_dbg(CD_OPEN, "bummer. this drive can't close the tray.\n");
 				ret=-ENOMEDIUM;
 				goto clean_up_and_return;
 			}
 			/* Ok, the door should be closed now.. Check again */
 			ret = cdo->drive_status(cdi, CDSL_CURRENT);
 			if ((ret == CDS_NO_DISC) || (ret==CDS_TRAY_OPEN)) {
-				cdinfo(CD_OPEN, "bummer. the tray is still not closed.\n"); 
-				cdinfo(CD_OPEN, "tray might not contain a medium.\n");
+				cd_dbg(CD_OPEN, "bummer. the tray is still not closed.\n");
+				cd_dbg(CD_OPEN, "tray might not contain a medium\n");
 				ret=-ENOMEDIUM;
 				goto clean_up_and_return;
 			}
-			cdinfo(CD_OPEN, "the tray is now closed.\n"); 
+			cd_dbg(CD_OPEN, "the tray is now closed\n");
 		}
 		/* the door should be closed now, check for the disc */
 		ret = cdo->drive_status(cdi, CDSL_CURRENT);
@@ -1077,7 +1084,7 @@ int open_for_data(struct cdrom_device_info * cdi)
 	}
 	cdrom_count_tracks(cdi, &tracks);
 	if (tracks.error == CDS_NO_DISC) {
-		cdinfo(CD_OPEN, "bummer. no disc.\n");
+		cd_dbg(CD_OPEN, "bummer. no disc.\n");
 		ret=-ENOMEDIUM;
 		goto clean_up_and_return;
 	}
@@ -1087,34 +1094,34 @@ int open_for_data(struct cdrom_device_info * cdi)
 		if (cdi->options & CDO_CHECK_TYPE) {
 		    /* give people a warning shot, now that CDO_CHECK_TYPE
 		       is the default case! */
-		    cdinfo(CD_OPEN, "bummer. wrong media type.\n"); 
-		    cdinfo(CD_WARNING, "pid %d must open device O_NONBLOCK!\n",
-					(unsigned int)task_pid_nr(current));
+		    cd_dbg(CD_OPEN, "bummer. wrong media type.\n");
+		    cd_dbg(CD_WARNING, "pid %d must open device O_NONBLOCK!\n",
+			   (unsigned int)task_pid_nr(current));
 		    ret=-EMEDIUMTYPE;
 		    goto clean_up_and_return;
 		}
 		else {
-		    cdinfo(CD_OPEN, "wrong media type, but CDO_CHECK_TYPE not set.\n");
+		    cd_dbg(CD_OPEN, "wrong media type, but CDO_CHECK_TYPE not set\n");
 		}
 	}
 
-	cdinfo(CD_OPEN, "all seems well, opening the device.\n"); 
+	cd_dbg(CD_OPEN, "all seems well, opening the devicen");
 
 	/* all seems well, we can open the device */
 	ret = cdo->open(cdi, 0); /* open for data */
-	cdinfo(CD_OPEN, "opening the device gave me %d.\n", ret); 
+	cd_dbg(CD_OPEN, "opening the device gave me %d\n", ret);
 	/* After all this careful checking, we shouldn't have problems
 	   opening the device, but we don't want the device locked if 
 	   this somehow fails... */
 	if (ret) {
-		cdinfo(CD_OPEN, "open device failed.\n"); 
+		cd_dbg(CD_OPEN, "open device failed\n");
 		goto clean_up_and_return;
 	}
 	if (CDROM_CAN(CDC_LOCK) && (cdi->options & CDO_LOCK)) {
 			cdo->lock_door(cdi, 1);
-			cdinfo(CD_OPEN, "door locked.\n");
+			cd_dbg(CD_OPEN, "door locked\n");
 	}
-	cdinfo(CD_OPEN, "device opened successfully.\n"); 
+	cd_dbg(CD_OPEN, "device opened successfully\n");
 	return ret;
 
 	/* Something failed.  Try to unlock the drive, because some drivers
@@ -1123,14 +1130,70 @@ int open_for_data(struct cdrom_device_info * cdi)
 	This ensures that the drive gets unlocked after a mount fails.  This 
 	is a goto to avoid bloating the driver with redundant code. */ 
 clean_up_and_return:
-	cdinfo(CD_OPEN, "open failed.\n"); 
+	cd_dbg(CD_OPEN, "open failed\n");
 	if (CDROM_CAN(CDC_LOCK) && cdi->options & CDO_LOCK) {
 			cdo->lock_door(cdi, 0);
-			cdinfo(CD_OPEN, "door unlocked.\n");
+			cd_dbg(CD_OPEN, "door unlocked\n");
 	}
 	return ret;
 }
 
+/* We use the open-option O_NONBLOCK to indicate that the
+ * purpose of opening is only for subsequent ioctl() calls; no device
+ * integrity checks are performed.
+ *
+ * We hope that all cd-player programs will adopt this convention. It
+ * is in their own interest: device control becomes a lot easier
+ * this way.
+ */
+int cdrom_open(struct cdrom_device_info *cdi, struct block_device *bdev,
+	       fmode_t mode)
+{
+	int ret;
+
+	cd_dbg(CD_OPEN, "entering cdrom_open\n");
+
+	/* open is event synchronization point, check events first */
+	check_disk_change(bdev);
+
+	/* if this was a O_NONBLOCK open and we should honor the flags,
+	 * do a quick open without drive/disc integrity checks. */
+	cdi->use_count++;
+	if ((mode & FMODE_NDELAY) && (cdi->options & CDO_USE_FFLAGS)) {
+		ret = cdi->ops->open(cdi, 1);
+	} else {
+		ret = open_for_data(cdi);
+		if (ret)
+			goto err;
+		cdrom_mmc3_profile(cdi);
+		if (mode & FMODE_WRITE) {
+			ret = -EROFS;
+			if (cdrom_open_write(cdi))
+				goto err_release;
+			if (!CDROM_CAN(CDC_RAM))
+				goto err_release;
+			ret = 0;
+			cdi->media_written = 0;
+		}
+	}
+
+	if (ret)
+		goto err;
+
+	cd_dbg(CD_OPEN, "Use count for \"/dev/%s\" now %d\n",
+	       cdi->name, cdi->use_count);
+	return 0;
+err_release:
+	if (CDROM_CAN(CDC_LOCK) && cdi->options & CDO_LOCK) {
+		cdi->ops->lock_door(cdi, 0);
+		cd_dbg(CD_OPEN, "door unlocked\n");
+	}
+	cdi->ops->release(cdi);
+err:
+	cdi->use_count--;
+	return ret;
+}
+
 /* This code is similar to that in open_for_data. The routine is called
    whenever an audio play operation is requested.
 */
@@ -1139,21 +1202,21 @@ static int check_for_audio_disc(struct cdrom_device_info * cdi,
 {
         int ret;
 	tracktype tracks;
-	cdinfo(CD_OPEN, "entering check_for_audio_disc\n");
+	cd_dbg(CD_OPEN, "entering check_for_audio_disc\n");
 	if (!(cdi->options & CDO_CHECK_TYPE))
 		return 0;
 	if (cdo->drive_status != NULL) {
 		ret = cdo->drive_status(cdi, CDSL_CURRENT);
-		cdinfo(CD_OPEN, "drive_status=%d\n", ret); 
+		cd_dbg(CD_OPEN, "drive_status=%d\n", ret);
 		if (ret == CDS_TRAY_OPEN) {
-			cdinfo(CD_OPEN, "the tray is open...\n"); 
+			cd_dbg(CD_OPEN, "the tray is open...\n");
 			/* can/may i close it? */
 			if (CDROM_CAN(CDC_CLOSE_TRAY) &&
 			    cdi->options & CDO_AUTO_CLOSE) {
-				cdinfo(CD_OPEN, "trying to close the tray.\n"); 
+				cd_dbg(CD_OPEN, "trying to close the tray\n");
 				ret=cdo->tray_move(cdi,0);
 				if (ret) {
-					cdinfo(CD_OPEN, "bummer. tried to close tray but failed.\n"); 
+					cd_dbg(CD_OPEN, "bummer. tried to close tray but failed.\n");
 					/* Ignore the error from the low
 					level driver.  We don't care why it
 					couldn't close the tray.  We only care 
@@ -1162,20 +1225,20 @@ static int check_for_audio_disc(struct cdrom_device_info * cdi,
 					return -ENOMEDIUM;
 				}
 			} else {
-				cdinfo(CD_OPEN, "bummer. this driver can't close the tray.\n"); 
+				cd_dbg(CD_OPEN, "bummer. this driver can't close the tray.\n");
 				return -ENOMEDIUM;
 			}
 			/* Ok, the door should be closed now.. Check again */
 			ret = cdo->drive_status(cdi, CDSL_CURRENT);
 			if ((ret == CDS_NO_DISC) || (ret==CDS_TRAY_OPEN)) {
-				cdinfo(CD_OPEN, "bummer. the tray is still not closed.\n"); 
+				cd_dbg(CD_OPEN, "bummer. the tray is still not closed.\n");
 				return -ENOMEDIUM;
 			}	
 			if (ret!=CDS_DISC_OK) {
-				cdinfo(CD_OPEN, "bummer. disc isn't ready.\n"); 
+				cd_dbg(CD_OPEN, "bummer. disc isn't ready.\n");
 				return -EIO;
 			}	
-			cdinfo(CD_OPEN, "the tray is now closed.\n"); 
+			cd_dbg(CD_OPEN, "the tray is now closed\n");
 		}	
 	}
 	cdrom_count_tracks(cdi, &tracks);
@@ -1193,17 +1256,18 @@ void cdrom_release(struct cdrom_device_info *cdi, fmode_t mode)
 	struct cdrom_device_ops *cdo = cdi->ops;
 	int opened_for_data;
 
-	cdinfo(CD_CLOSE, "entering cdrom_release\n");
+	cd_dbg(CD_CLOSE, "entering cdrom_release\n");
 
 	if (cdi->use_count > 0)
 		cdi->use_count--;
 
 	if (cdi->use_count == 0) {
-		cdinfo(CD_CLOSE, "Use count for \"/dev/%s\" now zero\n", cdi->name);
+		cd_dbg(CD_CLOSE, "Use count for \"/dev/%s\" now zero\n",
+		       cdi->name);
 		cdrom_dvd_rw_close_write(cdi);
 
 		if ((cdo->capability & CDC_LOCK) && !cdi->keeplocked) {
-			cdinfo(CD_CLOSE, "Unlocking door!\n");
+			cd_dbg(CD_CLOSE, "Unlocking door!\n");
 			cdo->lock_door(cdi, 0);
 		}
 	}
@@ -1262,7 +1326,7 @@ static int cdrom_slot_status(struct cdrom_device_info *cdi, int slot)
 	struct cdrom_changer_info *info;
 	int ret;
 
-	cdinfo(CD_CHANGER, "entering cdrom_slot_status()\n"); 
+	cd_dbg(CD_CHANGER, "entering cdrom_slot_status()\n");
 	if (cdi->sanyo_slot)
 		return CDS_NO_INFO;
 	
@@ -1292,7 +1356,7 @@ int cdrom_number_of_slots(struct cdrom_device_info *cdi)
 	int nslots = 1;
 	struct cdrom_changer_info *info;
 
-	cdinfo(CD_CHANGER, "entering cdrom_number_of_slots()\n"); 
+	cd_dbg(CD_CHANGER, "entering cdrom_number_of_slots()\n");
 	/* cdrom_read_mech_status requires a valid value for capacity: */
 	cdi->capacity = 0; 
 
@@ -1313,7 +1377,7 @@ static int cdrom_load_unload(struct cdrom_device_info *cdi, int slot)
 {
 	struct packet_command cgc;
 
-	cdinfo(CD_CHANGER, "entering cdrom_load_unload()\n"); 
+	cd_dbg(CD_CHANGER, "entering cdrom_load_unload()\n");
 	if (cdi->sanyo_slot && slot < 0)
 		return 0;
 
@@ -1342,7 +1406,7 @@ static int cdrom_select_disc(struct cdrom_device_info *cdi, int slot)
 	int curslot;
 	int ret;
 
-	cdinfo(CD_CHANGER, "entering cdrom_select_disc()\n"); 
+	cd_dbg(CD_CHANGER, "entering cdrom_select_disc()\n");
 	if (!CDROM_CAN(CDC_SELECT_DISC))
 		return -EDRIVE_CANT_DO_THIS;
 
@@ -1476,51 +1540,6 @@ int cdrom_media_changed(struct cdrom_device_info *cdi)
 	return media_changed(cdi, 0);
 }
 
-/* badly broken, I know. Is due for a fixup anytime. */
-static void cdrom_count_tracks(struct cdrom_device_info *cdi, tracktype* tracks)
-{
-	struct cdrom_tochdr header;
-	struct cdrom_tocentry entry;
-	int ret, i;
-	tracks->data=0;
-	tracks->audio=0;
-	tracks->cdi=0;
-	tracks->xa=0;
-	tracks->error=0;
-	cdinfo(CD_COUNT_TRACKS, "entering cdrom_count_tracks\n"); 
-	/* Grab the TOC header so we can see how many tracks there are */
-	if ((ret = cdi->ops->audio_ioctl(cdi, CDROMREADTOCHDR, &header))) {
-		if (ret == -ENOMEDIUM)
-			tracks->error = CDS_NO_DISC;
-		else
-			tracks->error = CDS_NO_INFO;
-		return;
-	}	
-	/* check what type of tracks are on this disc */
-	entry.cdte_format = CDROM_MSF;
-	for (i = header.cdth_trk0; i <= header.cdth_trk1; i++) {
-		entry.cdte_track  = i;
-		if (cdi->ops->audio_ioctl(cdi, CDROMREADTOCENTRY, &entry)) {
-			tracks->error=CDS_NO_INFO;
-			return;
-		}	
-		if (entry.cdte_ctrl & CDROM_DATA_TRACK) {
-		    if (entry.cdte_format == 0x10)
-			tracks->cdi++;
-		    else if (entry.cdte_format == 0x20) 
-			tracks->xa++;
-		    else
-			tracks->data++;
-		} else
-		    tracks->audio++;
-		cdinfo(CD_COUNT_TRACKS, "track %d: format=%d, ctrl=%d\n",
-		       i, entry.cdte_format, entry.cdte_ctrl);
-	}	
-	cdinfo(CD_COUNT_TRACKS, "disc has %d tracks: %d=audio %d=data %d=Cd-I %d=XA\n", 
-		header.cdth_trk1, tracks->audio, tracks->data, 
-		tracks->cdi, tracks->xa);
-}	
-
 /* Requests to the low-level drivers will /always/ be done in the
    following format convention:
 
@@ -1632,7 +1651,7 @@ static int dvd_do_auth(struct cdrom_device_info *cdi, dvd_authinfo *ai)
 	switch (ai->type) {
 	/* LU data send */
 	case DVD_LU_SEND_AGID:
-		cdinfo(CD_DVD, "entering DVD_LU_SEND_AGID\n"); 
+		cd_dbg(CD_DVD, "entering DVD_LU_SEND_AGID\n");
 		cgc.quiet = 1;
 		setup_report_key(&cgc, ai->lsa.agid, 0);
 
@@ -1644,7 +1663,7 @@ static int dvd_do_auth(struct cdrom_device_info *cdi, dvd_authinfo *ai)
 		break;
 
 	case DVD_LU_SEND_KEY1:
-		cdinfo(CD_DVD, "entering DVD_LU_SEND_KEY1\n"); 
+		cd_dbg(CD_DVD, "entering DVD_LU_SEND_KEY1\n");
 		setup_report_key(&cgc, ai->lsk.agid, 2);
 
 		if ((ret = cdo->generic_packet(cdi, &cgc)))
@@ -1655,7 +1674,7 @@ static int dvd_do_auth(struct cdrom_device_info *cdi, dvd_authinfo *ai)
 		break;
 
 	case DVD_LU_SEND_CHALLENGE:
-		cdinfo(CD_DVD, "entering DVD_LU_SEND_CHALLENGE\n"); 
+		cd_dbg(CD_DVD, "entering DVD_LU_SEND_CHALLENGE\n");
 		setup_report_key(&cgc, ai->lsc.agid, 1);
 
 		if ((ret = cdo->generic_packet(cdi, &cgc)))
@@ -1667,7 +1686,7 @@ static int dvd_do_auth(struct cdrom_device_info *cdi, dvd_authinfo *ai)
 
 	/* Post-auth key */
 	case DVD_LU_SEND_TITLE_KEY:
-		cdinfo(CD_DVD, "entering DVD_LU_SEND_TITLE_KEY\n"); 
+		cd_dbg(CD_DVD, "entering DVD_LU_SEND_TITLE_KEY\n");
 		cgc.quiet = 1;
 		setup_report_key(&cgc, ai->lstk.agid, 4);
 		cgc.cmd[5] = ai->lstk.lba;
@@ -1686,7 +1705,7 @@ static int dvd_do_auth(struct cdrom_device_info *cdi, dvd_authinfo *ai)
 		break;
 
 	case DVD_LU_SEND_ASF:
-		cdinfo(CD_DVD, "entering DVD_LU_SEND_ASF\n"); 
+		cd_dbg(CD_DVD, "entering DVD_LU_SEND_ASF\n");
 		setup_report_key(&cgc, ai->lsasf.agid, 5);
 		
 		if ((ret = cdo->generic_packet(cdi, &cgc)))
@@ -1697,7 +1716,7 @@ static int dvd_do_auth(struct cdrom_device_info *cdi, dvd_authinfo *ai)
 
 	/* LU data receive (LU changes state) */
 	case DVD_HOST_SEND_CHALLENGE:
-		cdinfo(CD_DVD, "entering DVD_HOST_SEND_CHALLENGE\n"); 
+		cd_dbg(CD_DVD, "entering DVD_HOST_SEND_CHALLENGE\n");
 		setup_send_key(&cgc, ai->hsc.agid, 1);
 		buf[1] = 0xe;
 		copy_chal(&buf[4], ai->hsc.chal);
@@ -1709,7 +1728,7 @@ static int dvd_do_auth(struct cdrom_device_info *cdi, dvd_authinfo *ai)
 		break;
 
 	case DVD_HOST_SEND_KEY2:
-		cdinfo(CD_DVD, "entering DVD_HOST_SEND_KEY2\n"); 
+		cd_dbg(CD_DVD, "entering DVD_HOST_SEND_KEY2\n");
 		setup_send_key(&cgc, ai->hsk.agid, 3);
 		buf[1] = 0xa;
 		copy_key(&buf[4], ai->hsk.key);
@@ -1724,7 +1743,7 @@ static int dvd_do_auth(struct cdrom_device_info *cdi, dvd_authinfo *ai)
 	/* Misc */
 	case DVD_INVALIDATE_AGID:
 		cgc.quiet = 1;
-		cdinfo(CD_DVD, "entering DVD_INVALIDATE_AGID\n"); 
+		cd_dbg(CD_DVD, "entering DVD_INVALIDATE_AGID\n");
 		setup_report_key(&cgc, ai->lsa.agid, 0x3f);
 		if ((ret = cdo->generic_packet(cdi, &cgc)))
 			return ret;
@@ -1732,7 +1751,7 @@ static int dvd_do_auth(struct cdrom_device_info *cdi, dvd_authinfo *ai)
 
 	/* Get region settings */
 	case DVD_LU_SEND_RPC_STATE:
-		cdinfo(CD_DVD, "entering DVD_LU_SEND_RPC_STATE\n");
+		cd_dbg(CD_DVD, "entering DVD_LU_SEND_RPC_STATE\n");
 		setup_report_key(&cgc, 0, 8);
 		memset(&rpc_state, 0, sizeof(rpc_state_t));
 		cgc.buffer = (char *) &rpc_state;
@@ -1749,7 +1768,7 @@ static int dvd_do_auth(struct cdrom_device_info *cdi, dvd_authinfo *ai)
 
 	/* Set region settings */
 	case DVD_HOST_SEND_RPC_STATE:
-		cdinfo(CD_DVD, "entering DVD_HOST_SEND_RPC_STATE\n");
+		cd_dbg(CD_DVD, "entering DVD_HOST_SEND_RPC_STATE\n");
 		setup_send_key(&cgc, 0, 6);
 		buf[1] = 6;
 		buf[4] = ai->hrpcs.pdrc;
@@ -1759,7 +1778,7 @@ static int dvd_do_auth(struct cdrom_device_info *cdi, dvd_authinfo *ai)
 		break;
 
 	default:
-		cdinfo(CD_WARNING, "Invalid DVD key ioctl (%d)\n", ai->type);
+		cd_dbg(CD_WARNING, "Invalid DVD key ioctl (%d)\n", ai->type);
 		return -ENOTTY;
 	}
 
@@ -1891,7 +1910,8 @@ static int dvd_read_bca(struct cdrom_device_info *cdi, dvd_struct *s,
 
 	s->bca.len = buf[0] << 8 | buf[1];
 	if (s->bca.len < 12 || s->bca.len > 188) {
-		cdinfo(CD_WARNING, "Received invalid BCA length (%d)\n", s->bca.len);
+		cd_dbg(CD_WARNING, "Received invalid BCA length (%d)\n",
+		       s->bca.len);
 		ret = -EIO;
 		goto out;
 	}
@@ -1927,14 +1947,13 @@ static int dvd_read_manufact(struct cdrom_device_info *cdi, dvd_struct *s,
 
 	s->manufact.len = buf[0] << 8 | buf[1];
 	if (s->manufact.len < 0) {
-		cdinfo(CD_WARNING, "Received invalid manufacture info length"
-				   " (%d)\n", s->manufact.len);
+		cd_dbg(CD_WARNING, "Received invalid manufacture info length (%d)\n",
+		       s->manufact.len);
 		ret = -EIO;
 	} else {
 		if (s->manufact.len > 2048) {
-			cdinfo(CD_WARNING, "Received invalid manufacture info "
-					"length (%d): truncating to 2048\n",
-					s->manufact.len);
+			cd_dbg(CD_WARNING, "Received invalid manufacture info length (%d): truncating to 2048\n",
+			       s->manufact.len);
 			s->manufact.len = 2048;
 		}
 		memcpy(s->manufact.value, &buf[4], s->manufact.len);
@@ -1965,8 +1984,8 @@ static int dvd_read_struct(struct cdrom_device_info *cdi, dvd_struct *s,
 		return dvd_read_manufact(cdi, s, cgc);
 		
 	default:
-		cdinfo(CD_WARNING, ": Invalid DVD structure read requested (%d)\n",
-					s->type);
+		cd_dbg(CD_WARNING, ": Invalid DVD structure read requested (%d)\n",
+		       s->type);
 		return -EINVAL;
 	}
 }
@@ -2165,6 +2184,7 @@ static int cdrom_read_cdda_bpc(struct cdrom_device_info *cdi, __u8 __user *ubuf,
 			ret = -ENOMEM;
 			break;
 		}
+		blk_rq_set_block_pc(rq);
 
 		ret = blk_rq_map_user(q, rq, NULL, ubuf, len, GFP_KERNEL);
 		if (ret) {
@@ -2184,7 +2204,6 @@ static int cdrom_read_cdda_bpc(struct cdrom_device_info *cdi, __u8 __user *ubuf,
 		rq->cmd[9] = 0xf8;
 
 		rq->cmd_len = 12;
-		rq->cmd_type = REQ_TYPE_BLOCK_PC;
 		rq->timeout = 60 * HZ;
 		bio = rq->bio;
 
@@ -2255,7 +2274,7 @@ static int cdrom_ioctl_multisession(struct cdrom_device_info *cdi,
 	u8 requested_format;
 	int ret;
 
-	cdinfo(CD_DO_IOCTL, "entering CDROMMULTISESSION\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROMMULTISESSION\n");
 
 	if (!(cdi->ops->capability & CDC_MULTI_SESSION))
 		return -ENOSYS;
@@ -2277,13 +2296,13 @@ static int cdrom_ioctl_multisession(struct cdrom_device_info *cdi,
 	if (copy_to_user(argp, &ms_info, sizeof(ms_info)))
 		return -EFAULT;
 
-	cdinfo(CD_DO_IOCTL, "CDROMMULTISESSION successful\n");
+	cd_dbg(CD_DO_IOCTL, "CDROMMULTISESSION successful\n");
 	return 0;
 }
 
 static int cdrom_ioctl_eject(struct cdrom_device_info *cdi)
 {
-	cdinfo(CD_DO_IOCTL, "entering CDROMEJECT\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROMEJECT\n");
 
 	if (!CDROM_CAN(CDC_OPEN_TRAY))
 		return -ENOSYS;
@@ -2300,7 +2319,7 @@ static int cdrom_ioctl_eject(struct cdrom_device_info *cdi)
 
 static int cdrom_ioctl_closetray(struct cdrom_device_info *cdi)
 {
-	cdinfo(CD_DO_IOCTL, "entering CDROMCLOSETRAY\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROMCLOSETRAY\n");
 
 	if (!CDROM_CAN(CDC_CLOSE_TRAY))
 		return -ENOSYS;
@@ -2310,7 +2329,7 @@ static int cdrom_ioctl_closetray(struct cdrom_device_info *cdi)
 static int cdrom_ioctl_eject_sw(struct cdrom_device_info *cdi,
 		unsigned long arg)
 {
-	cdinfo(CD_DO_IOCTL, "entering CDROMEJECT_SW\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROMEJECT_SW\n");
 
 	if (!CDROM_CAN(CDC_OPEN_TRAY))
 		return -ENOSYS;
@@ -2329,7 +2348,7 @@ static int cdrom_ioctl_media_changed(struct cdrom_device_info *cdi,
 	struct cdrom_changer_info *info;
 	int ret;
 
-	cdinfo(CD_DO_IOCTL, "entering CDROM_MEDIA_CHANGED\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_MEDIA_CHANGED\n");
 
 	if (!CDROM_CAN(CDC_MEDIA_CHANGED))
 		return -ENOSYS;
@@ -2355,7 +2374,7 @@ static int cdrom_ioctl_media_changed(struct cdrom_device_info *cdi,
 static int cdrom_ioctl_set_options(struct cdrom_device_info *cdi,
 		unsigned long arg)
 {
-	cdinfo(CD_DO_IOCTL, "entering CDROM_SET_OPTIONS\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_SET_OPTIONS\n");
 
 	/*
 	 * Options need to be in sync with capability.
@@ -2383,7 +2402,7 @@ static int cdrom_ioctl_set_options(struct cdrom_device_info *cdi,
 static int cdrom_ioctl_clear_options(struct cdrom_device_info *cdi,
 		unsigned long arg)
 {
-	cdinfo(CD_DO_IOCTL, "entering CDROM_CLEAR_OPTIONS\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_CLEAR_OPTIONS\n");
 
 	cdi->options &= ~(int) arg;
 	return cdi->options;
@@ -2392,7 +2411,7 @@ static int cdrom_ioctl_clear_options(struct cdrom_device_info *cdi,
 static int cdrom_ioctl_select_speed(struct cdrom_device_info *cdi,
 		unsigned long arg)
 {
-	cdinfo(CD_DO_IOCTL, "entering CDROM_SELECT_SPEED\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_SELECT_SPEED\n");
 
 	if (!CDROM_CAN(CDC_SELECT_SPEED))
 		return -ENOSYS;
@@ -2402,7 +2421,7 @@ static int cdrom_ioctl_select_speed(struct cdrom_device_info *cdi,
 static int cdrom_ioctl_select_disc(struct cdrom_device_info *cdi,
 		unsigned long arg)
 {
-	cdinfo(CD_DO_IOCTL, "entering CDROM_SELECT_DISC\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_SELECT_DISC\n");
 
 	if (!CDROM_CAN(CDC_SELECT_DISC))
 		return -ENOSYS;
@@ -2420,14 +2439,14 @@ static int cdrom_ioctl_select_disc(struct cdrom_device_info *cdi,
 	if (cdi->ops->select_disc)
 		return cdi->ops->select_disc(cdi, arg);
 
-	cdinfo(CD_CHANGER, "Using generic cdrom_select_disc()\n");
+	cd_dbg(CD_CHANGER, "Using generic cdrom_select_disc()\n");
 	return cdrom_select_disc(cdi, arg);
 }
 
 static int cdrom_ioctl_reset(struct cdrom_device_info *cdi,
 		struct block_device *bdev)
 {
-	cdinfo(CD_DO_IOCTL, "entering CDROM_RESET\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_RESET\n");
 
 	if (!capable(CAP_SYS_ADMIN))
 		return -EACCES;
@@ -2440,7 +2459,7 @@ static int cdrom_ioctl_reset(struct cdrom_device_info *cdi,
 static int cdrom_ioctl_lock_door(struct cdrom_device_info *cdi,
 		unsigned long arg)
 {
-	cdinfo(CD_DO_IOCTL, "%socking door.\n", arg ? "L" : "Unl");
+	cd_dbg(CD_DO_IOCTL, "%socking door\n", arg ? "L" : "Unl");
 
 	if (!CDROM_CAN(CDC_LOCK))
 		return -EDRIVE_CANT_DO_THIS;
@@ -2459,7 +2478,7 @@ static int cdrom_ioctl_lock_door(struct cdrom_device_info *cdi,
 static int cdrom_ioctl_debug(struct cdrom_device_info *cdi,
 		unsigned long arg)
 {
-	cdinfo(CD_DO_IOCTL, "%sabling debug.\n", arg ? "En" : "Dis");
+	cd_dbg(CD_DO_IOCTL, "%sabling debug\n", arg ? "En" : "Dis");
 
 	if (!capable(CAP_SYS_ADMIN))
 		return -EACCES;
@@ -2469,7 +2488,7 @@ static int cdrom_ioctl_debug(struct cdrom_device_info *cdi,
 
 static int cdrom_ioctl_get_capability(struct cdrom_device_info *cdi)
 {
-	cdinfo(CD_DO_IOCTL, "entering CDROM_GET_CAPABILITY\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_GET_CAPABILITY\n");
 	return (cdi->ops->capability & ~cdi->mask);
 }
 
@@ -2485,7 +2504,7 @@ static int cdrom_ioctl_get_mcn(struct cdrom_device_info *cdi,
 	struct cdrom_mcn mcn;
 	int ret;
 
-	cdinfo(CD_DO_IOCTL, "entering CDROM_GET_MCN\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_GET_MCN\n");
 
 	if (!(cdi->ops->capability & CDC_MCN))
 		return -ENOSYS;
@@ -2495,14 +2514,14 @@ static int cdrom_ioctl_get_mcn(struct cdrom_device_info *cdi,
 
 	if (copy_to_user(argp, &mcn, sizeof(mcn)))
 		return -EFAULT;
-	cdinfo(CD_DO_IOCTL, "CDROM_GET_MCN successful\n");
+	cd_dbg(CD_DO_IOCTL, "CDROM_GET_MCN successful\n");
 	return 0;
 }
 
 static int cdrom_ioctl_drive_status(struct cdrom_device_info *cdi,
 		unsigned long arg)
 {
-	cdinfo(CD_DO_IOCTL, "entering CDROM_DRIVE_STATUS\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_DRIVE_STATUS\n");
 
 	if (!(cdi->ops->capability & CDC_DRIVE_STATUS))
 		return -ENOSYS;
@@ -2535,7 +2554,7 @@ static int cdrom_ioctl_disc_status(struct cdrom_device_info *cdi)
 {
 	tracktype tracks;
 
-	cdinfo(CD_DO_IOCTL, "entering CDROM_DISC_STATUS\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_DISC_STATUS\n");
 
 	cdrom_count_tracks(cdi, &tracks);
 	if (tracks.error)
@@ -2557,13 +2576,13 @@ static int cdrom_ioctl_disc_status(struct cdrom_device_info *cdi)
 		return CDS_DATA_1;
 	/* Policy mode off */
 
-	cdinfo(CD_WARNING,"This disc doesn't have any tracks I recognize!\n");
+	cd_dbg(CD_WARNING, "This disc doesn't have any tracks I recognize!\n");
 	return CDS_NO_INFO;
 }
 
 static int cdrom_ioctl_changer_nslots(struct cdrom_device_info *cdi)
 {
-	cdinfo(CD_DO_IOCTL, "entering CDROM_CHANGER_NSLOTS\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_CHANGER_NSLOTS\n");
 	return cdi->capacity;
 }
 
@@ -2574,7 +2593,7 @@ static int cdrom_ioctl_get_subchnl(struct cdrom_device_info *cdi,
 	u8 requested, back;
 	int ret;
 
-	/* cdinfo(CD_DO_IOCTL,"entering CDROMSUBCHNL\n");*/
+	/* cd_dbg(CD_DO_IOCTL,"entering CDROMSUBCHNL\n");*/
 
 	if (copy_from_user(&q, argp, sizeof(q)))
 		return -EFAULT;
@@ -2594,7 +2613,7 @@ static int cdrom_ioctl_get_subchnl(struct cdrom_device_info *cdi,
 
 	if (copy_to_user(argp, &q, sizeof(q)))
 		return -EFAULT;
-	/* cdinfo(CD_DO_IOCTL, "CDROMSUBCHNL successful\n"); */
+	/* cd_dbg(CD_DO_IOCTL, "CDROMSUBCHNL successful\n"); */
 	return 0;
 }
 
@@ -2604,7 +2623,7 @@ static int cdrom_ioctl_read_tochdr(struct cdrom_device_info *cdi,
 	struct cdrom_tochdr header;
 	int ret;
 
-	/* cdinfo(CD_DO_IOCTL, "entering CDROMREADTOCHDR\n"); */
+	/* cd_dbg(CD_DO_IOCTL, "entering CDROMREADTOCHDR\n"); */
 
 	if (copy_from_user(&header, argp, sizeof(header)))
 		return -EFAULT;
@@ -2615,7 +2634,7 @@ static int cdrom_ioctl_read_tochdr(struct cdrom_device_info *cdi,
 
 	if (copy_to_user(argp, &header, sizeof(header)))
 		return -EFAULT;
-	/* cdinfo(CD_DO_IOCTL, "CDROMREADTOCHDR successful\n"); */
+	/* cd_dbg(CD_DO_IOCTL, "CDROMREADTOCHDR successful\n"); */
 	return 0;
 }
 
@@ -2626,7 +2645,7 @@ static int cdrom_ioctl_read_tocentry(struct cdrom_device_info *cdi,
 	u8 requested_format;
 	int ret;
 
-	/* cdinfo(CD_DO_IOCTL, "entering CDROMREADTOCENTRY\n"); */
+	/* cd_dbg(CD_DO_IOCTL, "entering CDROMREADTOCENTRY\n"); */
 
 	if (copy_from_user(&entry, argp, sizeof(entry)))
 		return -EFAULT;
@@ -2643,7 +2662,7 @@ static int cdrom_ioctl_read_tocentry(struct cdrom_device_info *cdi,
 
 	if (copy_to_user(argp, &entry, sizeof(entry)))
 		return -EFAULT;
-	/* cdinfo(CD_DO_IOCTL, "CDROMREADTOCENTRY successful\n"); */
+	/* cd_dbg(CD_DO_IOCTL, "CDROMREADTOCENTRY successful\n"); */
 	return 0;
 }
 
@@ -2652,7 +2671,7 @@ static int cdrom_ioctl_play_msf(struct cdrom_device_info *cdi,
 {
 	struct cdrom_msf msf;
 
-	cdinfo(CD_DO_IOCTL, "entering CDROMPLAYMSF\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROMPLAYMSF\n");
 
 	if (!CDROM_CAN(CDC_PLAY_AUDIO))
 		return -ENOSYS;
@@ -2667,7 +2686,7 @@ static int cdrom_ioctl_play_trkind(struct cdrom_device_info *cdi,
 	struct cdrom_ti ti;
 	int ret;
 
-	cdinfo(CD_DO_IOCTL, "entering CDROMPLAYTRKIND\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROMPLAYTRKIND\n");
 
 	if (!CDROM_CAN(CDC_PLAY_AUDIO))
 		return -ENOSYS;
@@ -2684,7 +2703,7 @@ static int cdrom_ioctl_volctrl(struct cdrom_device_info *cdi,
 {
 	struct cdrom_volctrl volume;
 
-	cdinfo(CD_DO_IOCTL, "entering CDROMVOLCTRL\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROMVOLCTRL\n");
 
 	if (!CDROM_CAN(CDC_PLAY_AUDIO))
 		return -ENOSYS;
@@ -2699,7 +2718,7 @@ static int cdrom_ioctl_volread(struct cdrom_device_info *cdi,
 	struct cdrom_volctrl volume;
 	int ret;
 
-	cdinfo(CD_DO_IOCTL, "entering CDROMVOLREAD\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROMVOLREAD\n");
 
 	if (!CDROM_CAN(CDC_PLAY_AUDIO))
 		return -ENOSYS;
@@ -2718,7 +2737,7 @@ static int cdrom_ioctl_audioctl(struct cdrom_device_info *cdi,
 {
 	int ret;
 
-	cdinfo(CD_DO_IOCTL, "doing audio ioctl (start/stop/pause/resume)\n");
+	cd_dbg(CD_DO_IOCTL, "doing audio ioctl (start/stop/pause/resume)\n");
 
 	if (!CDROM_CAN(CDC_PLAY_AUDIO))
 		return -ENOSYS;
@@ -2729,103 +2748,6 @@ static int cdrom_ioctl_audioctl(struct cdrom_device_info *cdi,
 }
 
 /*
- * Just about every imaginable ioctl is supported in the Uniform layer
- * these days.
- * ATAPI / SCSI specific code now mainly resides in mmc_ioctl().
- */
-int cdrom_ioctl(struct cdrom_device_info *cdi, struct block_device *bdev,
-		fmode_t mode, unsigned int cmd, unsigned long arg)
-{
-	void __user *argp = (void __user *)arg;
-	int ret;
-
-	/*
-	 * Try the generic SCSI command ioctl's first.
-	 */
-	ret = scsi_cmd_blk_ioctl(bdev, mode, cmd, argp);
-	if (ret != -ENOTTY)
-		return ret;
-
-	switch (cmd) {
-	case CDROMMULTISESSION:
-		return cdrom_ioctl_multisession(cdi, argp);
-	case CDROMEJECT:
-		return cdrom_ioctl_eject(cdi);
-	case CDROMCLOSETRAY:
-		return cdrom_ioctl_closetray(cdi);
-	case CDROMEJECT_SW:
-		return cdrom_ioctl_eject_sw(cdi, arg);
-	case CDROM_MEDIA_CHANGED:
-		return cdrom_ioctl_media_changed(cdi, arg);
-	case CDROM_SET_OPTIONS:
-		return cdrom_ioctl_set_options(cdi, arg);
-	case CDROM_CLEAR_OPTIONS:
-		return cdrom_ioctl_clear_options(cdi, arg);
-	case CDROM_SELECT_SPEED:
-		return cdrom_ioctl_select_speed(cdi, arg);
-	case CDROM_SELECT_DISC:
-		return cdrom_ioctl_select_disc(cdi, arg);
-	case CDROMRESET:
-		return cdrom_ioctl_reset(cdi, bdev);
-	case CDROM_LOCKDOOR:
-		return cdrom_ioctl_lock_door(cdi, arg);
-	case CDROM_DEBUG:
-		return cdrom_ioctl_debug(cdi, arg);
-	case CDROM_GET_CAPABILITY:
-		return cdrom_ioctl_get_capability(cdi);
-	case CDROM_GET_MCN:
-		return cdrom_ioctl_get_mcn(cdi, argp);
-	case CDROM_DRIVE_STATUS:
-		return cdrom_ioctl_drive_status(cdi, arg);
-	case CDROM_DISC_STATUS:
-		return cdrom_ioctl_disc_status(cdi);
-	case CDROM_CHANGER_NSLOTS:
-		return cdrom_ioctl_changer_nslots(cdi);
-	}
-
-	/*
-	 * Use the ioctls that are implemented through the generic_packet()
-	 * interface. this may look at bit funny, but if -ENOTTY is
-	 * returned that particular ioctl is not implemented and we
-	 * let it go through the device specific ones.
-	 */
-	if (CDROM_CAN(CDC_GENERIC_PACKET)) {
-		ret = mmc_ioctl(cdi, cmd, arg);
-		if (ret != -ENOTTY)
-			return ret;
-	}
-
-	/*
-	 * Note: most of the cdinfo() calls are commented out here,
-	 * because they fill up the sys log when CD players poll
-	 * the drive.
-	 */
-	switch (cmd) {
-	case CDROMSUBCHNL:
-		return cdrom_ioctl_get_subchnl(cdi, argp);
-	case CDROMREADTOCHDR:
-		return cdrom_ioctl_read_tochdr(cdi, argp);
-	case CDROMREADTOCENTRY:
-		return cdrom_ioctl_read_tocentry(cdi, argp);
-	case CDROMPLAYMSF:
-		return cdrom_ioctl_play_msf(cdi, argp);
-	case CDROMPLAYTRKIND:
-		return cdrom_ioctl_play_trkind(cdi, argp);
-	case CDROMVOLCTRL:
-		return cdrom_ioctl_volctrl(cdi, argp);
-	case CDROMVOLREAD:
-		return cdrom_ioctl_volread(cdi, argp);
-	case CDROMSTART:
-	case CDROMSTOP:
-	case CDROMPAUSE:
-	case CDROMRESUME:
-		return cdrom_ioctl_audioctl(cdi, cmd);
-	}
-
-	return -ENOSYS;
-}
-
-/*
  * Required when we need to use READ_10 to issue other than 2048 block
  * reads
  */
@@ -2854,10 +2776,158 @@ static int cdrom_switch_blocksize(struct cdrom_device_info *cdi, int size)
 	return cdo->generic_packet(cdi, &cgc);
 }
 
+static int cdrom_get_track_info(struct cdrom_device_info *cdi,
+				__u16 track, __u8 type, track_information *ti)
+{
+	struct cdrom_device_ops *cdo = cdi->ops;
+	struct packet_command cgc;
+	int ret, buflen;
+
+	init_cdrom_command(&cgc, ti, 8, CGC_DATA_READ);
+	cgc.cmd[0] = GPCMD_READ_TRACK_RZONE_INFO;
+	cgc.cmd[1] = type & 3;
+	cgc.cmd[4] = (track & 0xff00) >> 8;
+	cgc.cmd[5] = track & 0xff;
+	cgc.cmd[8] = 8;
+	cgc.quiet = 1;
+
+	ret = cdo->generic_packet(cdi, &cgc);
+	if (ret)
+		return ret;
+
+	buflen = be16_to_cpu(ti->track_information_length) +
+		sizeof(ti->track_information_length);
+
+	if (buflen > sizeof(track_information))
+		buflen = sizeof(track_information);
+
+	cgc.cmd[8] = cgc.buflen = buflen;
+	ret = cdo->generic_packet(cdi, &cgc);
+	if (ret)
+		return ret;
+
+	/* return actual fill size */
+	return buflen;
+}
+
+/* return the last written block on the CD-R media. this is for the udf
+   file system. */
+int cdrom_get_last_written(struct cdrom_device_info *cdi, long *last_written)
+{
+	struct cdrom_tocentry toc;
+	disc_information di;
+	track_information ti;
+	__u32 last_track;
+	int ret = -1, ti_size;
+
+	if (!CDROM_CAN(CDC_GENERIC_PACKET))
+		goto use_toc;
+
+	ret = cdrom_get_disc_info(cdi, &di);
+	if (ret < (int)(offsetof(typeof(di), last_track_lsb)
+			+ sizeof(di.last_track_lsb)))
+		goto use_toc;
+
+	/* if unit didn't return msb, it's zeroed by cdrom_get_disc_info */
+	last_track = (di.last_track_msb << 8) | di.last_track_lsb;
+	ti_size = cdrom_get_track_info(cdi, last_track, 1, &ti);
+	if (ti_size < (int)offsetof(typeof(ti), track_start))
+		goto use_toc;
+
+	/* if this track is blank, try the previous. */
+	if (ti.blank) {
+		if (last_track == 1)
+			goto use_toc;
+		last_track--;
+		ti_size = cdrom_get_track_info(cdi, last_track, 1, &ti);
+	}
+
+	if (ti_size < (int)(offsetof(typeof(ti), track_size)
+				+ sizeof(ti.track_size)))
+		goto use_toc;
+
+	/* if last recorded field is valid, return it. */
+	if (ti.lra_v && ti_size >= (int)(offsetof(typeof(ti), last_rec_address)
+				+ sizeof(ti.last_rec_address))) {
+		*last_written = be32_to_cpu(ti.last_rec_address);
+	} else {
+		/* make it up instead */
+		*last_written = be32_to_cpu(ti.track_start) +
+				be32_to_cpu(ti.track_size);
+		if (ti.free_blocks)
+			*last_written -= (be32_to_cpu(ti.free_blocks) + 7);
+	}
+	return 0;
+
+	/* this is where we end up if the drive either can't do a
+	   GPCMD_READ_DISC_INFO or GPCMD_READ_TRACK_RZONE_INFO or if
+	   it doesn't give enough information or fails. then we return
+	   the toc contents. */
+use_toc:
+	toc.cdte_format = CDROM_MSF;
+	toc.cdte_track = CDROM_LEADOUT;
+	if ((ret = cdi->ops->audio_ioctl(cdi, CDROMREADTOCENTRY, &toc)))
+		return ret;
+	sanitize_format(&toc.cdte_addr, &toc.cdte_format, CDROM_LBA);
+	*last_written = toc.cdte_addr.lba;
+	return 0;
+}
+
+/* return the next writable block. also for udf file system. */
+static int cdrom_get_next_writable(struct cdrom_device_info *cdi,
+				   long *next_writable)
+{
+	disc_information di;
+	track_information ti;
+	__u16 last_track;
+	int ret, ti_size;
+
+	if (!CDROM_CAN(CDC_GENERIC_PACKET))
+		goto use_last_written;
+
+	ret = cdrom_get_disc_info(cdi, &di);
+	if (ret < 0 || ret < offsetof(typeof(di), last_track_lsb)
+				+ sizeof(di.last_track_lsb))
+		goto use_last_written;
+
+	/* if unit didn't return msb, it's zeroed by cdrom_get_disc_info */
+	last_track = (di.last_track_msb << 8) | di.last_track_lsb;
+	ti_size = cdrom_get_track_info(cdi, last_track, 1, &ti);
+	if (ti_size < 0 || ti_size < offsetof(typeof(ti), track_start))
+		goto use_last_written;
+
+	/* if this track is blank, try the previous. */
+	if (ti.blank) {
+		if (last_track == 1)
+			goto use_last_written;
+		last_track--;
+		ti_size = cdrom_get_track_info(cdi, last_track, 1, &ti);
+		if (ti_size < 0)
+			goto use_last_written;
+	}
+
+	/* if next recordable address field is valid, use it. */
+	if (ti.nwa_v && ti_size >= offsetof(typeof(ti), next_writable)
+				+ sizeof(ti.next_writable)) {
+		*next_writable = be32_to_cpu(ti.next_writable);
+		return 0;
+	}
+
+use_last_written:
+	ret = cdrom_get_last_written(cdi, next_writable);
+	if (ret) {
+		*next_writable = 0;
+		return ret;
+	} else {
+		*next_writable += 7;
+		return 0;
+	}
+}
+
 static noinline int mmc_ioctl_cdrom_read_data(struct cdrom_device_info *cdi,
-					void __user *arg,
-					struct packet_command *cgc,
-					int cmd)
+					      void __user *arg,
+					      struct packet_command *cgc,
+					      int cmd)
 {
 	struct request_sense sense;
 	struct cdrom_msf msf;
@@ -2876,7 +2946,8 @@ static noinline int mmc_ioctl_cdrom_read_data(struct cdrom_device_info *cdi,
 		blocksize = CD_FRAMESIZE_RAW0;
 		break;
 	}
-	IOCTL_IN(arg, struct cdrom_msf, msf);
+	if (copy_from_user(&msf, (struct cdrom_msf __user *)arg, sizeof(msf)))
+		return -EFAULT;
 	lba = msf_to_lba(msf.cdmsf_min0, msf.cdmsf_sec0, msf.cdmsf_frame0);
 	/* FIXME: we need upper bound checking, too!! */
 	if (lba < 0)
@@ -2891,8 +2962,8 @@ static noinline int mmc_ioctl_cdrom_read_data(struct cdrom_device_info *cdi,
 	cgc->data_direction = CGC_DATA_READ;
 	ret = cdrom_read_block(cdi, cgc, lba, 1, format, blocksize);
 	if (ret && sense.sense_key == 0x05 &&
-		   sense.asc == 0x20 &&
-		   sense.ascq == 0x00) {
+	    sense.asc == 0x20 &&
+	    sense.ascq == 0x00) {
 		/*
 		 * SCSI-II devices are not required to support
 		 * READ_CD, so let's try switching block size
@@ -2913,12 +2984,14 @@ out:
 }
 
 static noinline int mmc_ioctl_cdrom_read_audio(struct cdrom_device_info *cdi,
-					void __user *arg)
+					       void __user *arg)
 {
 	struct cdrom_read_audio ra;
 	int lba;
 
-	IOCTL_IN(arg, struct cdrom_read_audio, ra);
+	if (copy_from_user(&ra, (struct cdrom_read_audio __user *)arg,
+			   sizeof(ra)))
+		return -EFAULT;
 
 	if (ra.addr_format == CDROM_MSF)
 		lba = msf_to_lba(ra.addr.msf.minute,
@@ -2937,12 +3010,13 @@ static noinline int mmc_ioctl_cdrom_read_audio(struct cdrom_device_info *cdi,
 }
 
 static noinline int mmc_ioctl_cdrom_subchannel(struct cdrom_device_info *cdi,
-					void __user *arg)
+					       void __user *arg)
 {
 	int ret;
 	struct cdrom_subchnl q;
 	u_char requested, back;
-	IOCTL_IN(arg, struct cdrom_subchnl, q);
+	if (copy_from_user(&q, (struct cdrom_subchnl __user *)arg, sizeof(q)))
+		return -EFAULT;
 	requested = q.cdsc_format;
 	if (!((requested == CDROM_MSF) ||
 	      (requested == CDROM_LBA)))
@@ -2954,19 +3028,21 @@ static noinline int mmc_ioctl_cdrom_subchannel(struct cdrom_device_info *cdi,
 	back = q.cdsc_format; /* local copy */
 	sanitize_format(&q.cdsc_absaddr, &back, requested);
 	sanitize_format(&q.cdsc_reladdr, &q.cdsc_format, requested);
-	IOCTL_OUT(arg, struct cdrom_subchnl, q);
-	/* cdinfo(CD_DO_IOCTL, "CDROMSUBCHNL successful\n"); */
+	if (copy_to_user((struct cdrom_subchnl __user *)arg, &q, sizeof(q)))
+		return -EFAULT;
+	/* cd_dbg(CD_DO_IOCTL, "CDROMSUBCHNL successful\n"); */
 	return 0;
 }
 
 static noinline int mmc_ioctl_cdrom_play_msf(struct cdrom_device_info *cdi,
-					void __user *arg,
-					struct packet_command *cgc)
+					     void __user *arg,
+					     struct packet_command *cgc)
 {
 	struct cdrom_device_ops *cdo = cdi->ops;
 	struct cdrom_msf msf;
-	cdinfo(CD_DO_IOCTL, "entering CDROMPLAYMSF\n");
-	IOCTL_IN(arg, struct cdrom_msf, msf);
+	cd_dbg(CD_DO_IOCTL, "entering CDROMPLAYMSF\n");
+	if (copy_from_user(&msf, (struct cdrom_msf __user *)arg, sizeof(msf)))
+		return -EFAULT;
 	cgc->cmd[0] = GPCMD_PLAY_AUDIO_MSF;
 	cgc->cmd[3] = msf.cdmsf_min0;
 	cgc->cmd[4] = msf.cdmsf_sec0;
@@ -2979,13 +3055,14 @@ static noinline int mmc_ioctl_cdrom_play_msf(struct cdrom_device_info *cdi,
 }
 
 static noinline int mmc_ioctl_cdrom_play_blk(struct cdrom_device_info *cdi,
-					void __user *arg,
-					struct packet_command *cgc)
+					     void __user *arg,
+					     struct packet_command *cgc)
 {
 	struct cdrom_device_ops *cdo = cdi->ops;
 	struct cdrom_blk blk;
-	cdinfo(CD_DO_IOCTL, "entering CDROMPLAYBLK\n");
-	IOCTL_IN(arg, struct cdrom_blk, blk);
+	cd_dbg(CD_DO_IOCTL, "entering CDROMPLAYBLK\n");
+	if (copy_from_user(&blk, (struct cdrom_blk __user *)arg, sizeof(blk)))
+		return -EFAULT;
 	cgc->cmd[0] = GPCMD_PLAY_AUDIO_10;
 	cgc->cmd[2] = (blk.from >> 24) & 0xff;
 	cgc->cmd[3] = (blk.from >> 16) & 0xff;
@@ -2998,9 +3075,9 @@ static noinline int mmc_ioctl_cdrom_play_blk(struct cdrom_device_info *cdi,
 }
 
 static noinline int mmc_ioctl_cdrom_volume(struct cdrom_device_info *cdi,
-					void __user *arg,
-					struct packet_command *cgc,
-					unsigned int cmd)
+					   void __user *arg,
+					   struct packet_command *cgc,
+					   unsigned int cmd)
 {
 	struct cdrom_volctrl volctrl;
 	unsigned char buffer[32];
@@ -3008,9 +3085,11 @@ static noinline int mmc_ioctl_cdrom_volume(struct cdrom_device_info *cdi,
 	unsigned short offset;
 	int ret;
 
-	cdinfo(CD_DO_IOCTL, "entering CDROMVOLUME\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROMVOLUME\n");
 
-	IOCTL_IN(arg, struct cdrom_volctrl, volctrl);
+	if (copy_from_user(&volctrl, (struct cdrom_volctrl __user *)arg,
+			   sizeof(volctrl)))
+		return -EFAULT;
 
 	cgc->buffer = buffer;
 	cgc->buflen = 24;
@@ -3030,14 +3109,14 @@ static noinline int mmc_ioctl_cdrom_volume(struct cdrom_device_info *cdi,
 	if (offset + 16 > cgc->buflen) {
 		cgc->buflen = offset + 16;
 		ret = cdrom_mode_sense(cdi, cgc,
-					GPMODE_AUDIO_CTL_PAGE, 0);
+				       GPMODE_AUDIO_CTL_PAGE, 0);
 		if (ret)
 			return ret;
 	}
 
 	/* sanity check */
 	if ((buffer[offset] & 0x3f) != GPMODE_AUDIO_CTL_PAGE ||
-			buffer[offset + 1] < 14)
+	    buffer[offset + 1] < 14)
 		return -EINVAL;
 
 	/* now we have the current volume settings. if it was only
@@ -3047,7 +3126,9 @@ static noinline int mmc_ioctl_cdrom_volume(struct cdrom_device_info *cdi,
 		volctrl.channel1 = buffer[offset+11];
 		volctrl.channel2 = buffer[offset+13];
 		volctrl.channel3 = buffer[offset+15];
-		IOCTL_OUT(arg, struct cdrom_volctrl, volctrl);
+		if (copy_to_user((struct cdrom_volctrl __user *)arg, &volctrl,
+				 sizeof(volctrl)))
+			return -EFAULT;
 		return 0;
 	}
 		
@@ -3069,11 +3150,11 @@ static noinline int mmc_ioctl_cdrom_volume(struct cdrom_device_info *cdi,
 }
 
 static noinline int mmc_ioctl_cdrom_start_stop(struct cdrom_device_info *cdi,
-					struct packet_command *cgc,
-					int cmd)
+					       struct packet_command *cgc,
+					       int cmd)
 {
 	struct cdrom_device_ops *cdo = cdi->ops;
-	cdinfo(CD_DO_IOCTL, "entering CDROMSTART/CDROMSTOP\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROMSTART/CDROMSTOP\n");
 	cgc->cmd[0] = GPCMD_START_STOP_UNIT;
 	cgc->cmd[1] = 1;
 	cgc->cmd[4] = (cmd == CDROMSTART) ? 1 : 0;
@@ -3082,11 +3163,11 @@ static noinline int mmc_ioctl_cdrom_start_stop(struct cdrom_device_info *cdi,
 }
 
 static noinline int mmc_ioctl_cdrom_pause_resume(struct cdrom_device_info *cdi,
-					struct packet_command *cgc,
-					int cmd)
+						 struct packet_command *cgc,
+						 int cmd)
 {
 	struct cdrom_device_ops *cdo = cdi->ops;
-	cdinfo(CD_DO_IOCTL, "entering CDROMPAUSE/CDROMRESUME\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROMPAUSE/CDROMRESUME\n");
 	cgc->cmd[0] = GPCMD_PAUSE_RESUME;
 	cgc->cmd[8] = (cmd == CDROMRESUME) ? 1 : 0;
 	cgc->data_direction = CGC_DATA_NONE;
@@ -3094,8 +3175,8 @@ static noinline int mmc_ioctl_cdrom_pause_resume(struct cdrom_device_info *cdi,
 }
 
 static noinline int mmc_ioctl_dvd_read_struct(struct cdrom_device_info *cdi,
-						void __user *arg,
-						struct packet_command *cgc)
+					      void __user *arg,
+					      struct packet_command *cgc)
 {
 	int ret;
 	dvd_struct *s;
@@ -3108,7 +3189,7 @@ static noinline int mmc_ioctl_dvd_read_struct(struct cdrom_device_info *cdi,
 	if (!s)
 		return -ENOMEM;
 
-	cdinfo(CD_DO_IOCTL, "entering DVD_READ_STRUCT\n");
+	cd_dbg(CD_DO_IOCTL, "entering DVD_READ_STRUCT\n");
 	if (copy_from_user(s, arg, size)) {
 		kfree(s);
 		return -EFAULT;
@@ -3126,44 +3207,48 @@ out:
 }
 
 static noinline int mmc_ioctl_dvd_auth(struct cdrom_device_info *cdi,
-					void __user *arg)
+				       void __user *arg)
 {
 	int ret;
 	dvd_authinfo ai;
 	if (!CDROM_CAN(CDC_DVD))
 		return -ENOSYS;
-	cdinfo(CD_DO_IOCTL, "entering DVD_AUTH\n");
-	IOCTL_IN(arg, dvd_authinfo, ai);
+	cd_dbg(CD_DO_IOCTL, "entering DVD_AUTH\n");
+	if (copy_from_user(&ai, (dvd_authinfo __user *)arg, sizeof(ai)))
+		return -EFAULT;
 	ret = dvd_do_auth(cdi, &ai);
 	if (ret)
 		return ret;
-	IOCTL_OUT(arg, dvd_authinfo, ai);
+	if (copy_to_user((dvd_authinfo __user *)arg, &ai, sizeof(ai)))
+		return -EFAULT;
 	return 0;
 }
 
 static noinline int mmc_ioctl_cdrom_next_writable(struct cdrom_device_info *cdi,
-						void __user *arg)
+						  void __user *arg)
 {
 	int ret;
 	long next = 0;
-	cdinfo(CD_DO_IOCTL, "entering CDROM_NEXT_WRITABLE\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_NEXT_WRITABLE\n");
 	ret = cdrom_get_next_writable(cdi, &next);
 	if (ret)
 		return ret;
-	IOCTL_OUT(arg, long, next);
+	if (copy_to_user((long __user *)arg, &next, sizeof(next)))
+		return -EFAULT;
 	return 0;
 }
 
 static noinline int mmc_ioctl_cdrom_last_written(struct cdrom_device_info *cdi,
-						void __user *arg)
+						 void __user *arg)
 {
 	int ret;
 	long last = 0;
-	cdinfo(CD_DO_IOCTL, "entering CDROM_LAST_WRITTEN\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_LAST_WRITTEN\n");
 	ret = cdrom_get_last_written(cdi, &last);
 	if (ret)
 		return ret;
-	IOCTL_OUT(arg, long, last);
+	if (copy_to_user((long __user *)arg, &last, sizeof(last)))
+		return -EFAULT;
 	return 0;
 }
 
@@ -3212,181 +3297,101 @@ static int mmc_ioctl(struct cdrom_device_info *cdi, unsigned int cmd,
 	return -ENOTTY;
 }
 
-static int cdrom_get_track_info(struct cdrom_device_info *cdi, __u16 track, __u8 type,
-			 track_information *ti)
-{
-	struct cdrom_device_ops *cdo = cdi->ops;
-	struct packet_command cgc;
-	int ret, buflen;
-
-	init_cdrom_command(&cgc, ti, 8, CGC_DATA_READ);
-	cgc.cmd[0] = GPCMD_READ_TRACK_RZONE_INFO;
-	cgc.cmd[1] = type & 3;
-	cgc.cmd[4] = (track & 0xff00) >> 8;
-	cgc.cmd[5] = track & 0xff;
-	cgc.cmd[8] = 8;
-	cgc.quiet = 1;
-
-	if ((ret = cdo->generic_packet(cdi, &cgc)))
-		return ret;
-	
-	buflen = be16_to_cpu(ti->track_information_length) +
-		     sizeof(ti->track_information_length);
-
-	if (buflen > sizeof(track_information))
-		buflen = sizeof(track_information);
-
-	cgc.cmd[8] = cgc.buflen = buflen;
-	if ((ret = cdo->generic_packet(cdi, &cgc)))
-		return ret;
-
-	/* return actual fill size */
-	return buflen;
-}
-
-/* requires CD R/RW */
-static int cdrom_get_disc_info(struct cdrom_device_info *cdi, disc_information *di)
+/*
+ * Just about every imaginable ioctl is supported in the Uniform layer
+ * these days.
+ * ATAPI / SCSI specific code now mainly resides in mmc_ioctl().
+ */
+int cdrom_ioctl(struct cdrom_device_info *cdi, struct block_device *bdev,
+		fmode_t mode, unsigned int cmd, unsigned long arg)
 {
-	struct cdrom_device_ops *cdo = cdi->ops;
-	struct packet_command cgc;
-	int ret, buflen;
-
-	/* set up command and get the disc info */
-	init_cdrom_command(&cgc, di, sizeof(*di), CGC_DATA_READ);
-	cgc.cmd[0] = GPCMD_READ_DISC_INFO;
-	cgc.cmd[8] = cgc.buflen = 2;
-	cgc.quiet = 1;
-
-	if ((ret = cdo->generic_packet(cdi, &cgc)))
-		return ret;
+	void __user *argp = (void __user *)arg;
+	int ret;
 
-	/* not all drives have the same disc_info length, so requeue
-	 * packet with the length the drive tells us it can supply
+	/*
+	 * Try the generic SCSI command ioctl's first.
 	 */
-	buflen = be16_to_cpu(di->disc_information_length) +
-		     sizeof(di->disc_information_length);
-
-	if (buflen > sizeof(disc_information))
-		buflen = sizeof(disc_information);
-
-	cgc.cmd[8] = cgc.buflen = buflen;
-	if ((ret = cdo->generic_packet(cdi, &cgc)))
+	ret = scsi_cmd_blk_ioctl(bdev, mode, cmd, argp);
+	if (ret != -ENOTTY)
 		return ret;
 
-	/* return actual fill size */
-	return buflen;
-}
-
-/* return the last written block on the CD-R media. this is for the udf
-   file system. */
-int cdrom_get_last_written(struct cdrom_device_info *cdi, long *last_written)
-{
-	struct cdrom_tocentry toc;
-	disc_information di;
-	track_information ti;
-	__u32 last_track;
-	int ret = -1, ti_size;
-
-	if (!CDROM_CAN(CDC_GENERIC_PACKET))
-		goto use_toc;
-
-	ret = cdrom_get_disc_info(cdi, &di);
-	if (ret < (int)(offsetof(typeof(di), last_track_lsb)
-			+ sizeof(di.last_track_lsb)))
-		goto use_toc;
-
-	/* if unit didn't return msb, it's zeroed by cdrom_get_disc_info */
-	last_track = (di.last_track_msb << 8) | di.last_track_lsb;
-	ti_size = cdrom_get_track_info(cdi, last_track, 1, &ti);
-	if (ti_size < (int)offsetof(typeof(ti), track_start))
-		goto use_toc;
-
-	/* if this track is blank, try the previous. */
-	if (ti.blank) {
-		if (last_track==1)
-			goto use_toc;
-		last_track--;
-		ti_size = cdrom_get_track_info(cdi, last_track, 1, &ti);
-	}
-
-	if (ti_size < (int)(offsetof(typeof(ti), track_size)
-				+ sizeof(ti.track_size)))
-		goto use_toc;
-
-	/* if last recorded field is valid, return it. */
-	if (ti.lra_v && ti_size >= (int)(offsetof(typeof(ti), last_rec_address)
-				+ sizeof(ti.last_rec_address))) {
-		*last_written = be32_to_cpu(ti.last_rec_address);
-	} else {
-		/* make it up instead */
-		*last_written = be32_to_cpu(ti.track_start) +
-				be32_to_cpu(ti.track_size);
-		if (ti.free_blocks)
-			*last_written -= (be32_to_cpu(ti.free_blocks) + 7);
+	switch (cmd) {
+	case CDROMMULTISESSION:
+		return cdrom_ioctl_multisession(cdi, argp);
+	case CDROMEJECT:
+		return cdrom_ioctl_eject(cdi);
+	case CDROMCLOSETRAY:
+		return cdrom_ioctl_closetray(cdi);
+	case CDROMEJECT_SW:
+		return cdrom_ioctl_eject_sw(cdi, arg);
+	case CDROM_MEDIA_CHANGED:
+		return cdrom_ioctl_media_changed(cdi, arg);
+	case CDROM_SET_OPTIONS:
+		return cdrom_ioctl_set_options(cdi, arg);
+	case CDROM_CLEAR_OPTIONS:
+		return cdrom_ioctl_clear_options(cdi, arg);
+	case CDROM_SELECT_SPEED:
+		return cdrom_ioctl_select_speed(cdi, arg);
+	case CDROM_SELECT_DISC:
+		return cdrom_ioctl_select_disc(cdi, arg);
+	case CDROMRESET:
+		return cdrom_ioctl_reset(cdi, bdev);
+	case CDROM_LOCKDOOR:
+		return cdrom_ioctl_lock_door(cdi, arg);
+	case CDROM_DEBUG:
+		return cdrom_ioctl_debug(cdi, arg);
+	case CDROM_GET_CAPABILITY:
+		return cdrom_ioctl_get_capability(cdi);
+	case CDROM_GET_MCN:
+		return cdrom_ioctl_get_mcn(cdi, argp);
+	case CDROM_DRIVE_STATUS:
+		return cdrom_ioctl_drive_status(cdi, arg);
+	case CDROM_DISC_STATUS:
+		return cdrom_ioctl_disc_status(cdi);
+	case CDROM_CHANGER_NSLOTS:
+		return cdrom_ioctl_changer_nslots(cdi);
 	}
-	return 0;
 
-	/* this is where we end up if the drive either can't do a
-	   GPCMD_READ_DISC_INFO or GPCMD_READ_TRACK_RZONE_INFO or if
-	   it doesn't give enough information or fails. then we return
-	   the toc contents. */
-use_toc:
-	toc.cdte_format = CDROM_MSF;
-	toc.cdte_track = CDROM_LEADOUT;
-	if ((ret = cdi->ops->audio_ioctl(cdi, CDROMREADTOCENTRY, &toc)))
-		return ret;
-	sanitize_format(&toc.cdte_addr, &toc.cdte_format, CDROM_LBA);
-	*last_written = toc.cdte_addr.lba;
-	return 0;
-}
-
-/* return the next writable block. also for udf file system. */
-static int cdrom_get_next_writable(struct cdrom_device_info *cdi, long *next_writable)
-{
-	disc_information di;
-	track_information ti;
-	__u16 last_track;
-	int ret, ti_size;
-
-	if (!CDROM_CAN(CDC_GENERIC_PACKET))
-		goto use_last_written;
-
-	ret = cdrom_get_disc_info(cdi, &di);
-	if (ret < 0 || ret < offsetof(typeof(di), last_track_lsb)
-				+ sizeof(di.last_track_lsb))
-		goto use_last_written;
-
-	/* if unit didn't return msb, it's zeroed by cdrom_get_disc_info */
-	last_track = (di.last_track_msb << 8) | di.last_track_lsb;
-	ti_size = cdrom_get_track_info(cdi, last_track, 1, &ti);
-	if (ti_size < 0 || ti_size < offsetof(typeof(ti), track_start))
-		goto use_last_written;
-
-        /* if this track is blank, try the previous. */
-	if (ti.blank) {
-		if (last_track == 1)
-			goto use_last_written;
-		last_track--;
-		ti_size = cdrom_get_track_info(cdi, last_track, 1, &ti);
-		if (ti_size < 0)
-			goto use_last_written;
+	/*
+	 * Use the ioctls that are implemented through the generic_packet()
+	 * interface. this may look at bit funny, but if -ENOTTY is
+	 * returned that particular ioctl is not implemented and we
+	 * let it go through the device specific ones.
+	 */
+	if (CDROM_CAN(CDC_GENERIC_PACKET)) {
+		ret = mmc_ioctl(cdi, cmd, arg);
+		if (ret != -ENOTTY)
+			return ret;
 	}
 
-	/* if next recordable address field is valid, use it. */
-	if (ti.nwa_v && ti_size >= offsetof(typeof(ti), next_writable)
-				+ sizeof(ti.next_writable)) {
-		*next_writable = be32_to_cpu(ti.next_writable);
-		return 0;
+	/*
+	 * Note: most of the cd_dbg() calls are commented out here,
+	 * because they fill up the sys log when CD players poll
+	 * the drive.
+	 */
+	switch (cmd) {
+	case CDROMSUBCHNL:
+		return cdrom_ioctl_get_subchnl(cdi, argp);
+	case CDROMREADTOCHDR:
+		return cdrom_ioctl_read_tochdr(cdi, argp);
+	case CDROMREADTOCENTRY:
+		return cdrom_ioctl_read_tocentry(cdi, argp);
+	case CDROMPLAYMSF:
+		return cdrom_ioctl_play_msf(cdi, argp);
+	case CDROMPLAYTRKIND:
+		return cdrom_ioctl_play_trkind(cdi, argp);
+	case CDROMVOLCTRL:
+		return cdrom_ioctl_volctrl(cdi, argp);
+	case CDROMVOLREAD:
+		return cdrom_ioctl_volread(cdi, argp);
+	case CDROMSTART:
+	case CDROMSTOP:
+	case CDROMPAUSE:
+	case CDROMRESUME:
+		return cdrom_ioctl_audioctl(cdi, cmd);
 	}
 
-use_last_written:
-	if ((ret = cdrom_get_last_written(cdi, next_writable))) {
-		*next_writable = 0;
-		return ret;
-	} else {
-		*next_writable += 7;
-		return 0;
-	}
+	return -ENOSYS;
 }
 
 EXPORT_SYMBOL(cdrom_get_last_written);
@@ -3465,7 +3470,7 @@ static int cdrom_print_info(const char *header, int val, char *info,
 	return 0;
 }
 
-static int cdrom_sysctl_info(ctl_table *ctl, int write,
+static int cdrom_sysctl_info(struct ctl_table *ctl, int write,
                            void __user *buffer, size_t *lenp, loff_t *ppos)
 {
 	int pos;
@@ -3578,7 +3583,7 @@ static void cdrom_update_settings(void)
 	mutex_unlock(&cdrom_mutex);
 }
 
-static int cdrom_sysctl_handler(ctl_table *ctl, int write,
+static int cdrom_sysctl_handler(struct ctl_table *ctl, int write,
 				void __user *buffer, size_t *lenp, loff_t *ppos)
 {
 	int ret;
@@ -3604,7 +3609,7 @@ static int cdrom_sysctl_handler(ctl_table *ctl, int write,
 }
 
 /* Place files in /proc/sys/dev/cdrom */
-static ctl_table cdrom_table[] = {
+static struct ctl_table cdrom_table[] = {
 	{
 		.procname	= "info",
 		.data		= &cdrom_sysctl_settings.info, 
@@ -3650,7 +3655,7 @@ static ctl_table cdrom_table[] = {
 	{ }
 };
 
-static ctl_table cdrom_cdrom_table[] = {
+static struct ctl_table cdrom_cdrom_table[] = {
 	{
 		.procname	= "cdrom",
 		.maxlen		= 0,
@@ -3661,7 +3666,7 @@ static ctl_table cdrom_cdrom_table[] = {
 };
 
 /* Make sure that /proc/sys/dev is there */
-static ctl_table cdrom_root_table[] = {
+static struct ctl_table cdrom_root_table[] = {
 	{
 		.procname	= "dev",
 		.maxlen		= 0,
diff --git a/drivers/cdrom/gdrom.c b/drivers/cdrom/gdrom.c
index 51e75ad9642..584bc312640 100644
--- a/drivers/cdrom/gdrom.c
+++ b/drivers/cdrom/gdrom.c
@@ -602,7 +602,7 @@ static void gdrom_readdisk_dma(struct work_struct *work)
 		spin_unlock(&gdrom_lock);
 		block = blk_rq_pos(req)/GD_TO_BLK + GD_SESSION_OFFSET;
 		block_cnt = blk_rq_sectors(req)/GD_TO_BLK;
-		__raw_writel(virt_to_phys(req->buffer), GDROM_DMA_STARTADDR_REG);
+		__raw_writel(virt_to_phys(bio_data(req->bio)), GDROM_DMA_STARTADDR_REG);
 		__raw_writel(block_cnt * GDROM_HARD_SECTOR, GDROM_DMA_LENGTH_REG);
 		__raw_writel(1, GDROM_DMA_DIRECTION_REG);
 		__raw_writel(1, GDROM_DMA_ENABLE_REG);
diff --git a/drivers/char/applicom.c b/drivers/char/applicom.c
index 974321a2508..14790304b84 100644
--- a/drivers/char/applicom.c
+++ b/drivers/char/applicom.c
@@ -345,7 +345,6 @@ out:
 			free_irq(apbs[i].irq, &dummy);
 		iounmap(apbs[i].RamIO);
 	}
-	pci_disable_device(dev);
 	return ret;
 }
 
diff --git a/drivers/char/hw_random/Kconfig b/drivers/char/hw_random/Kconfig
index 244759bbd7b..836b061ced3 100644
--- a/drivers/char/hw_random/Kconfig
+++ b/drivers/char/hw_random/Kconfig
@@ -2,7 +2,7 @@
 # Hardware Random Number Generator (RNG) configuration
 #
 
-config HW_RANDOM
+menuconfig HW_RANDOM
 	tristate "Hardware Random Number Generator Core support"
 	default m
 	---help---
@@ -20,9 +20,11 @@ config HW_RANDOM
 
 	  If unsure, say Y.
 
+if HW_RANDOM
+
 config HW_RANDOM_TIMERIOMEM
 	tristate "Timer IOMEM HW Random Number Generator support"
-	depends on HW_RANDOM && HAS_IOMEM
+	depends on HAS_IOMEM
 	---help---
 	  This driver provides kernel-side support for a generic Random
 	  Number Generator used by reading a 'dumb' iomem address that
@@ -36,7 +38,7 @@ config HW_RANDOM_TIMERIOMEM
 
 config HW_RANDOM_INTEL
 	tristate "Intel HW Random Number Generator support"
-	depends on HW_RANDOM && (X86 || IA64) && PCI
+	depends on (X86 || IA64) && PCI
 	default HW_RANDOM
 	---help---
 	  This driver provides kernel-side support for the Random Number
@@ -49,7 +51,7 @@ config HW_RANDOM_INTEL
 
 config HW_RANDOM_AMD
 	tristate "AMD HW Random Number Generator support"
-	depends on HW_RANDOM && (X86 || PPC_MAPLE) && PCI
+	depends on (X86 || PPC_MAPLE) && PCI
 	default HW_RANDOM
 	---help---
 	  This driver provides kernel-side support for the Random Number
@@ -62,8 +64,8 @@ config HW_RANDOM_AMD
 
 config HW_RANDOM_ATMEL
 	tristate "Atmel Random Number Generator support"
-	depends on HW_RANDOM && HAVE_CLK
-	default (HW_RANDOM && ARCH_AT91)
+	depends on ARCH_AT91 && HAVE_CLK
+	default HW_RANDOM
 	---help---
 	  This driver provides kernel-side support for the Random Number
 	  Generator hardware found on Atmel AT91 devices.
@@ -75,7 +77,7 @@ config HW_RANDOM_ATMEL
 
 config HW_RANDOM_BCM63XX
 	tristate "Broadcom BCM63xx Random Number Generator support"
-	depends on HW_RANDOM && BCM63XX
+	depends on BCM63XX
 	default HW_RANDOM
 	---help---
 	  This driver provides kernel-side support for the Random Number
@@ -88,7 +90,7 @@ config HW_RANDOM_BCM63XX
 
 config HW_RANDOM_BCM2835
 	tristate "Broadcom BCM2835 Random Number Generator support"
-	depends on HW_RANDOM && ARCH_BCM2835
+	depends on ARCH_BCM2835
 	default HW_RANDOM
 	---help---
 	  This driver provides kernel-side support for the Random Number
@@ -101,7 +103,7 @@ config HW_RANDOM_BCM2835
 
 config HW_RANDOM_GEODE
 	tristate "AMD Geode HW Random Number Generator support"
-	depends on HW_RANDOM && X86_32 && PCI
+	depends on X86_32 && PCI
 	default HW_RANDOM
 	---help---
 	  This driver provides kernel-side support for the Random Number
@@ -114,7 +116,7 @@ config HW_RANDOM_GEODE
 
 config HW_RANDOM_N2RNG
 	tristate "Niagara2 Random Number Generator support"
-	depends on HW_RANDOM && SPARC64
+	depends on SPARC64
 	default HW_RANDOM
 	---help---
 	  This driver provides kernel-side support for the Random Number
@@ -127,7 +129,7 @@ config HW_RANDOM_N2RNG
 
 config HW_RANDOM_VIA
 	tristate "VIA HW Random Number Generator support"
-	depends on HW_RANDOM && X86
+	depends on X86
 	default HW_RANDOM
 	---help---
 	  This driver provides kernel-side support for the Random Number
@@ -140,7 +142,7 @@ config HW_RANDOM_VIA
 
 config HW_RANDOM_IXP4XX
 	tristate "Intel IXP4xx NPU HW Pseudo-Random Number Generator support"
-	depends on HW_RANDOM && ARCH_IXP4XX
+	depends on ARCH_IXP4XX
 	default HW_RANDOM
 	---help---
 	  This driver provides kernel-side support for the Pseudo-Random
@@ -153,7 +155,7 @@ config HW_RANDOM_IXP4XX
 
 config HW_RANDOM_OMAP
 	tristate "OMAP Random Number Generator support"
-	depends on HW_RANDOM && (ARCH_OMAP16XX || ARCH_OMAP2PLUS)
+	depends on ARCH_OMAP16XX || ARCH_OMAP2PLUS
 	default HW_RANDOM
  	---help---
  	  This driver provides kernel-side support for the Random Number
@@ -167,7 +169,7 @@ config HW_RANDOM_OMAP
 
 config HW_RANDOM_OMAP3_ROM
 	tristate "OMAP3 ROM Random Number Generator support"
-	depends on HW_RANDOM && ARCH_OMAP3
+	depends on ARCH_OMAP3
 	default HW_RANDOM
 	---help---
 	  This driver provides kernel-side support for the Random Number
@@ -180,7 +182,7 @@ config HW_RANDOM_OMAP3_ROM
 
 config HW_RANDOM_OCTEON
 	tristate "Octeon Random Number Generator support"
-	depends on HW_RANDOM && CAVIUM_OCTEON_SOC
+	depends on CAVIUM_OCTEON_SOC
 	default HW_RANDOM
 	---help---
 	  This driver provides kernel-side support for the Random Number
@@ -193,7 +195,7 @@ config HW_RANDOM_OCTEON
 
 config HW_RANDOM_PASEMI
 	tristate "PA Semi HW Random Number Generator support"
-	depends on HW_RANDOM && PPC_PASEMI
+	depends on PPC_PASEMI
 	default HW_RANDOM
 	---help---
 	  This driver provides kernel-side support for the Random Number
@@ -206,7 +208,7 @@ config HW_RANDOM_PASEMI
 
 config HW_RANDOM_VIRTIO
 	tristate "VirtIO Random Number Generator support"
-	depends on HW_RANDOM && VIRTIO
+	depends on VIRTIO
 	---help---
 	  This driver provides kernel-side support for the virtual Random Number
 	  Generator hardware.
@@ -216,7 +218,7 @@ config HW_RANDOM_VIRTIO
 
 config HW_RANDOM_TX4939
 	tristate "TX4939 Random Number Generator support"
-	depends on HW_RANDOM && SOC_TX4939
+	depends on SOC_TX4939
 	default HW_RANDOM
 	---help---
 	  This driver provides kernel-side support for the Random Number
@@ -229,7 +231,8 @@ config HW_RANDOM_TX4939
 
 config HW_RANDOM_MXC_RNGA
 	tristate "Freescale i.MX RNGA Random Number Generator"
-	depends on HW_RANDOM && ARCH_HAS_RNGA
+	depends on ARCH_HAS_RNGA
+	default HW_RANDOM
 	---help---
 	  This driver provides kernel-side support for the Random Number
 	  Generator hardware found on Freescale i.MX processors.
@@ -241,7 +244,8 @@ config HW_RANDOM_MXC_RNGA
 
 config HW_RANDOM_NOMADIK
 	tristate "ST-Ericsson Nomadik Random Number Generator support"
-	depends on HW_RANDOM && ARCH_NOMADIK
+	depends on ARCH_NOMADIK
+	default HW_RANDOM
 	---help---
 	  This driver provides kernel-side support for the Random Number
 	  Generator hardware found on ST-Ericsson SoCs (8815 and 8500).
@@ -251,21 +255,10 @@ config HW_RANDOM_NOMADIK
 
 	  If unsure, say Y.
 
-config HW_RANDOM_PICOXCELL
-	tristate "Picochip picoXcell true random number generator support"
-	depends on HW_RANDOM && ARCH_PICOXCELL && PICOXCELL_PC3X3
-	---help---
-	  This driver provides kernel-side support for the Random Number
-	  Generator hardware found on Picochip PC3x3 and later devices.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called picoxcell-rng.
-
-	  If unsure, say Y.
-
 config HW_RANDOM_PPC4XX
 	tristate "PowerPC 4xx generic true random number generator support"
-	depends on HW_RANDOM && PPC && 4xx
+	depends on PPC && 4xx
+	default HW_RANDOM
 	---help---
 	 This driver provides the kernel-side support for the TRNG hardware
 	 found in the security function of some PowerPC 4xx SoCs.
@@ -275,24 +268,9 @@ config HW_RANDOM_PPC4XX
 
 	 If unsure, say N.
 
-config UML_RANDOM
-	depends on UML
-	tristate "Hardware random number generator"
-	help
-	  This option enables UML's "hardware" random number generator.  It
-	  attaches itself to the host's /dev/random, supplying as much entropy
-	  as the host has, rather than the small amount the UML gets from its
-	  own drivers.  It registers itself as a standard hardware random number
-	  generator, major 10, minor 183, and the canonical device name is
-	  /dev/hwrng.
-	  The way to make use of this is to install the rng-tools package
-	  (check your distro, or download from
-	  http://sourceforge.net/projects/gkernel/).  rngd periodically reads
-	  /dev/hwrng and injects the entropy into /dev/random.
-
 config HW_RANDOM_PSERIES
 	tristate "pSeries HW Random Number Generator support"
-	depends on HW_RANDOM && PPC64 && IBMVIO
+	depends on PPC64 && IBMVIO
 	default HW_RANDOM
 	---help---
 	  This driver provides kernel-side support for the Random Number
@@ -305,7 +283,7 @@ config HW_RANDOM_PSERIES
 
 config HW_RANDOM_POWERNV
 	tristate "PowerNV Random Number Generator support"
-	depends on HW_RANDOM && PPC_POWERNV
+	depends on PPC_POWERNV
 	default HW_RANDOM
 	---help---
 	  This is the driver for Random Number Generator hardware found
@@ -318,7 +296,8 @@ config HW_RANDOM_POWERNV
 
 config HW_RANDOM_EXYNOS
 	tristate "EXYNOS HW random number generator support"
-	depends on HW_RANDOM && HAS_IOMEM && HAVE_CLK
+	depends on ARCH_EXYNOS
+	default HW_RANDOM
 	---help---
 	  This driver provides kernel-side support for the Random Number
 	  Generator hardware found on EXYNOS SOCs.
@@ -330,7 +309,7 @@ config HW_RANDOM_EXYNOS
 
 config HW_RANDOM_TPM
 	tristate "TPM HW Random Number Generator support"
-	depends on HW_RANDOM && TCG_TPM
+	depends on TCG_TPM
 	default HW_RANDOM
 	---help---
 	  This driver provides kernel-side support for the Random Number
@@ -344,6 +323,7 @@ config HW_RANDOM_TPM
 config HW_RANDOM_MSM
 	tristate "Qualcomm SoCs Random Number Generator support"
 	depends on HW_RANDOM && ARCH_QCOM
+	default HW_RANDOM
 	---help---
 	  This driver provides kernel-side support for the Random Number
 	  Generator hardware found on Qualcomm SoCs.
@@ -352,3 +332,20 @@ config HW_RANDOM_MSM
 	  module will be called msm-rng.
 
 	  If unsure, say Y.
+
+endif # HW_RANDOM
+
+config UML_RANDOM
+	depends on UML
+	tristate "Hardware random number generator"
+	help
+	  This option enables UML's "hardware" random number generator.  It
+	  attaches itself to the host's /dev/random, supplying as much entropy
+	  as the host has, rather than the small amount the UML gets from its
+	  own drivers.  It registers itself as a standard hardware random number
+	  generator, major 10, minor 183, and the canonical device name is
+	  /dev/hwrng.
+	  The way to make use of this is to install the rng-tools package
+	  (check your distro, or download from
+	  http://sourceforge.net/projects/gkernel/).  rngd periodically reads
+	  /dev/hwrng and injects the entropy into /dev/random.
diff --git a/drivers/char/hw_random/Makefile b/drivers/char/hw_random/Makefile
index 3ae7755a52e..199ed283e14 100644
--- a/drivers/char/hw_random/Makefile
+++ b/drivers/char/hw_random/Makefile
@@ -22,7 +22,6 @@ obj-$(CONFIG_HW_RANDOM_TX4939) += tx4939-rng.o
 obj-$(CONFIG_HW_RANDOM_MXC_RNGA) += mxc-rnga.o
 obj-$(CONFIG_HW_RANDOM_OCTEON) += octeon-rng.o
 obj-$(CONFIG_HW_RANDOM_NOMADIK) += nomadik-rng.o
-obj-$(CONFIG_HW_RANDOM_PICOXCELL) += picoxcell-rng.o
 obj-$(CONFIG_HW_RANDOM_PPC4XX) += ppc4xx-rng.o
 obj-$(CONFIG_HW_RANDOM_PSERIES) += pseries-rng.o
 obj-$(CONFIG_HW_RANDOM_POWERNV) += powernv-rng.o
diff --git a/drivers/char/hw_random/n2-drv.c b/drivers/char/hw_random/n2-drv.c
index 432232eefe0..292a5889f67 100644
--- a/drivers/char/hw_random/n2-drv.c
+++ b/drivers/char/hw_random/n2-drv.c
@@ -632,7 +632,7 @@ static int n2rng_probe(struct platform_device *op)
 	multi_capable = (match->data != NULL);
 
 	n2rng_driver_version();
-	np = kzalloc(sizeof(*np), GFP_KERNEL);
+	np = devm_kzalloc(&op->dev, sizeof(*np), GFP_KERNEL);
 	if (!np)
 		goto out;
 	np->op = op;
@@ -653,7 +653,7 @@ static int n2rng_probe(struct platform_device *op)
 					 &np->hvapi_minor)) {
 			dev_err(&op->dev, "Cannot register suitable "
 				"HVAPI version.\n");
-			goto out_free;
+			goto out;
 		}
 	}
 
@@ -676,15 +676,16 @@ static int n2rng_probe(struct platform_device *op)
 	dev_info(&op->dev, "Registered RNG HVAPI major %lu minor %lu\n",
 		 np->hvapi_major, np->hvapi_minor);
 
-	np->units = kzalloc(sizeof(struct n2rng_unit) * np->num_units,
-			    GFP_KERNEL);
+	np->units = devm_kzalloc(&op->dev,
+				 sizeof(struct n2rng_unit) * np->num_units,
+				 GFP_KERNEL);
 	err = -ENOMEM;
 	if (!np->units)
 		goto out_hvapi_unregister;
 
 	err = n2rng_init_control(np);
 	if (err)
-		goto out_free_units;
+		goto out_hvapi_unregister;
 
 	dev_info(&op->dev, "Found %s RNG, units: %d\n",
 		 ((np->flags & N2RNG_FLAG_MULTI) ?
@@ -697,7 +698,7 @@ static int n2rng_probe(struct platform_device *op)
 
 	err = hwrng_register(&np->hwrng);
 	if (err)
-		goto out_free_units;
+		goto out_hvapi_unregister;
 
 	platform_set_drvdata(op, np);
 
@@ -705,15 +706,9 @@ static int n2rng_probe(struct platform_device *op)
 
 	return 0;
 
-out_free_units:
-	kfree(np->units);
-	np->units = NULL;
-
 out_hvapi_unregister:
 	sun4v_hvapi_unregister(HV_GRP_RNG);
 
-out_free:
-	kfree(np);
 out:
 	return err;
 }
@@ -730,11 +725,6 @@ static int n2rng_remove(struct platform_device *op)
 
 	sun4v_hvapi_unregister(HV_GRP_RNG);
 
-	kfree(np->units);
-	np->units = NULL;
-
-	kfree(np);
-
 	return 0;
 }
 
diff --git a/drivers/char/hw_random/omap-rng.c b/drivers/char/hw_random/omap-rng.c
index 9b89ff4881d..f66ea258382 100644
--- a/drivers/char/hw_random/omap-rng.c
+++ b/drivers/char/hw_random/omap-rng.c
@@ -369,10 +369,8 @@ static int omap_rng_probe(struct platform_device *pdev)
 	int ret;
 
 	priv = devm_kzalloc(dev, sizeof(struct omap_rng_dev), GFP_KERNEL);
-	if (!priv) {
-		dev_err(&pdev->dev, "could not allocate memory\n");
+	if (!priv)
 		return -ENOMEM;
-	};
 
 	omap_rng_ops.priv = (unsigned long)priv;
 	platform_set_drvdata(pdev, priv);
diff --git a/drivers/char/hw_random/picoxcell-rng.c b/drivers/char/hw_random/picoxcell-rng.c
deleted file mode 100644
index eab5448ad56..00000000000
--- a/drivers/char/hw_random/picoxcell-rng.c
+++ /dev/null
@@ -1,181 +0,0 @@
-/*
- * Copyright (c) 2010-2011 Picochip Ltd., Jamie Iles
- *
- * 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.
- *
- * All enquiries to support@picochip.com
- */
-#include <linux/clk.h>
-#include <linux/delay.h>
-#include <linux/err.h>
-#include <linux/hw_random.h>
-#include <linux/io.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/platform_device.h>
-
-#define DATA_REG_OFFSET		0x0200
-#define CSR_REG_OFFSET		0x0278
-#define CSR_OUT_EMPTY_MASK	(1 << 24)
-#define CSR_FAULT_MASK		(1 << 1)
-#define TRNG_BLOCK_RESET_MASK	(1 << 0)
-#define TAI_REG_OFFSET		0x0380
-
-/*
- * The maximum amount of time in microseconds to spend waiting for data if the
- * core wants us to wait.  The TRNG should generate 32 bits every 320ns so a
- * timeout of 20us seems reasonable.  The TRNG does builtin tests of the data
- * for randomness so we can't always assume there is data present.
- */
-#define PICO_TRNG_TIMEOUT		20
-
-static void __iomem *rng_base;
-static struct clk *rng_clk;
-static struct device *rng_dev;
-
-static inline u32 picoxcell_trng_read_csr(void)
-{
-	return __raw_readl(rng_base + CSR_REG_OFFSET);
-}
-
-static inline bool picoxcell_trng_is_empty(void)
-{
-	return picoxcell_trng_read_csr() & CSR_OUT_EMPTY_MASK;
-}
-
-/*
- * Take the random number generator out of reset and make sure the interrupts
- * are masked. We shouldn't need to get large amounts of random bytes so just
- * poll the status register. The hardware generates 32 bits every 320ns so we
- * shouldn't have to wait long enough to warrant waiting for an IRQ.
- */
-static void picoxcell_trng_start(void)
-{
-	__raw_writel(0, rng_base + TAI_REG_OFFSET);
-	__raw_writel(0, rng_base + CSR_REG_OFFSET);
-}
-
-static void picoxcell_trng_reset(void)
-{
-	__raw_writel(TRNG_BLOCK_RESET_MASK, rng_base + CSR_REG_OFFSET);
-	__raw_writel(TRNG_BLOCK_RESET_MASK, rng_base + TAI_REG_OFFSET);
-	picoxcell_trng_start();
-}
-
-/*
- * Get some random data from the random number generator. The hw_random core
- * layer provides us with locking.
- */
-static int picoxcell_trng_read(struct hwrng *rng, void *buf, size_t max,
-			       bool wait)
-{
-	int i;
-
-	/* Wait for some data to become available. */
-	for (i = 0; i < PICO_TRNG_TIMEOUT && picoxcell_trng_is_empty(); ++i) {
-		if (!wait)
-			return 0;
-
-		udelay(1);
-	}
-
-	if (picoxcell_trng_read_csr() & CSR_FAULT_MASK) {
-		dev_err(rng_dev, "fault detected, resetting TRNG\n");
-		picoxcell_trng_reset();
-		return -EIO;
-	}
-
-	if (i == PICO_TRNG_TIMEOUT)
-		return 0;
-
-	*(u32 *)buf = __raw_readl(rng_base + DATA_REG_OFFSET);
-	return sizeof(u32);
-}
-
-static struct hwrng picoxcell_trng = {
-	.name		= "picoxcell",
-	.read		= picoxcell_trng_read,
-};
-
-static int picoxcell_trng_probe(struct platform_device *pdev)
-{
-	int ret;
-	struct resource *mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-
-	rng_base = devm_ioremap_resource(&pdev->dev, mem);
-	if (IS_ERR(rng_base))
-		return PTR_ERR(rng_base);
-
-	rng_clk = devm_clk_get(&pdev->dev, NULL);
-	if (IS_ERR(rng_clk)) {
-		dev_warn(&pdev->dev, "no clk\n");
-		return PTR_ERR(rng_clk);
-	}
-
-	ret = clk_enable(rng_clk);
-	if (ret) {
-		dev_warn(&pdev->dev, "unable to enable clk\n");
-		return ret;
-	}
-
-	picoxcell_trng_start();
-	ret = hwrng_register(&picoxcell_trng);
-	if (ret)
-		goto err_register;
-
-	rng_dev = &pdev->dev;
-	dev_info(&pdev->dev, "pixoxcell random number generator active\n");
-
-	return 0;
-
-err_register:
-	clk_disable(rng_clk);
-	return ret;
-}
-
-static int picoxcell_trng_remove(struct platform_device *pdev)
-{
-	hwrng_unregister(&picoxcell_trng);
-	clk_disable(rng_clk);
-
-	return 0;
-}
-
-#ifdef CONFIG_PM
-static int picoxcell_trng_suspend(struct device *dev)
-{
-	clk_disable(rng_clk);
-
-	return 0;
-}
-
-static int picoxcell_trng_resume(struct device *dev)
-{
-	return clk_enable(rng_clk);
-}
-
-static const struct dev_pm_ops picoxcell_trng_pm_ops = {
-	.suspend	= picoxcell_trng_suspend,
-	.resume		= picoxcell_trng_resume,
-};
-#endif /* CONFIG_PM */
-
-static struct platform_driver picoxcell_trng_driver = {
-	.probe		= picoxcell_trng_probe,
-	.remove		= picoxcell_trng_remove,
-	.driver		= {
-		.name	= "picoxcell-trng",
-		.owner	= THIS_MODULE,
-#ifdef CONFIG_PM
-		.pm	= &picoxcell_trng_pm_ops,
-#endif /* CONFIG_PM */
-	},
-};
-
-module_platform_driver(picoxcell_trng_driver);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Jamie Iles");
-MODULE_DESCRIPTION("Picochip picoXcell TRNG driver");
diff --git a/drivers/char/hw_random/timeriomem-rng.c b/drivers/char/hw_random/timeriomem-rng.c
index 439ff8b28c4..b6ab9ac3f34 100644
--- a/drivers/char/hw_random/timeriomem-rng.c
+++ b/drivers/char/hw_random/timeriomem-rng.c
@@ -120,10 +120,8 @@ static int timeriomem_rng_probe(struct platform_device *pdev)
 	/* Allocate memory for the device structure (and zero it) */
 	priv = devm_kzalloc(&pdev->dev,
 			sizeof(struct timeriomem_rng_private_data), GFP_KERNEL);
-	if (!priv) {
-		dev_err(&pdev->dev, "failed to allocate device structure.\n");
+	if (!priv)
 		return -ENOMEM;
-	}
 
 	platform_set_drvdata(pdev, priv);
 
diff --git a/drivers/char/ipmi/ipmi_si_intf.c b/drivers/char/ipmi/ipmi_si_intf.c
index 1c4bb4f6ce9..5d665680ae3 100644
--- a/drivers/char/ipmi/ipmi_si_intf.c
+++ b/drivers/char/ipmi/ipmi_si_intf.c
@@ -1007,7 +1007,7 @@ static int ipmi_thread(void *data)
 	struct timespec busy_until;
 
 	ipmi_si_set_not_busy(&busy_until);
-	set_user_nice(current, 19);
+	set_user_nice(current, MAX_NICE);
 	while (!kthread_should_stop()) {
 		int busy_wait;
 
diff --git a/drivers/char/random.c b/drivers/char/random.c
index 6b75713d953..4ad71ef2cd5 100644
--- a/drivers/char/random.c
+++ b/drivers/char/random.c
@@ -902,6 +902,7 @@ void add_disk_randomness(struct gendisk *disk)
 	add_timer_randomness(disk->random, 0x100 + disk_devt(disk));
 	trace_add_disk_randomness(disk_devt(disk), ENTROPY_BITS(&input_pool));
 }
+EXPORT_SYMBOL_GPL(add_disk_randomness);
 #endif
 
 /*********************************************************************
@@ -995,8 +996,11 @@ retry:
 		ibytes = min_t(size_t, ibytes, have_bytes - reserved);
 	if (ibytes < min)
 		ibytes = 0;
-	entropy_count = max_t(int, 0,
-			      entropy_count - (ibytes << (ENTROPY_SHIFT + 3)));
+	if (have_bytes >= ibytes + reserved)
+		entropy_count -= ibytes << (ENTROPY_SHIFT + 3);
+	else
+		entropy_count = reserved << (ENTROPY_SHIFT + 3);
+
 	if (cmpxchg(&r->entropy_count, orig, entropy_count) != orig)
 		goto retry;
 
@@ -1578,10 +1582,10 @@ static int proc_do_uuid(struct ctl_table *table, int write,
 /*
  * Return entropy available scaled to integral bits
  */
-static int proc_do_entropy(ctl_table *table, int write,
+static int proc_do_entropy(struct ctl_table *table, int write,
 			   void __user *buffer, size_t *lenp, loff_t *ppos)
 {
-	ctl_table fake_table;
+	struct ctl_table fake_table;
 	int entropy_count;
 
 	entropy_count = *(int *)table->data >> ENTROPY_SHIFT;
diff --git a/drivers/char/tpm/tpm_acpi.c b/drivers/char/tpm/tpm_acpi.c
index b9a57fa4b71..565a9478cb9 100644
--- a/drivers/char/tpm/tpm_acpi.c
+++ b/drivers/char/tpm/tpm_acpi.c
@@ -95,7 +95,7 @@ int read_log(struct tpm_bios_log *log)
 
 	log->bios_event_log_end = log->bios_event_log + len;
 
-	virt = acpi_os_map_memory(start, len);
+	virt = acpi_os_map_iomem(start, len);
 	if (!virt) {
 		kfree(log->bios_event_log);
 		printk("%s: ERROR - Unable to map memory\n", __func__);
@@ -104,6 +104,6 @@ int read_log(struct tpm_bios_log *log)
 
 	memcpy_fromio(log->bios_event_log, virt, len);
 
-	acpi_os_unmap_memory(virt, len);
+	acpi_os_unmap_iomem(virt, len);
 	return 0;
 }
diff --git a/drivers/char/tpm/tpm_ppi.c b/drivers/char/tpm/tpm_ppi.c
index b3ea223585b..61dcc8011ec 100644
--- a/drivers/char/tpm/tpm_ppi.c
+++ b/drivers/char/tpm/tpm_ppi.c
@@ -328,13 +328,11 @@ int tpm_add_ppi(struct kobject *parent)
 	/* Cache TPM ACPI handle and version string */
 	acpi_walk_namespace(ACPI_TYPE_DEVICE, ACPI_ROOT_OBJECT, ACPI_UINT32_MAX,
 			    ppi_callback, NULL, NULL, &tpm_ppi_handle);
-	if (tpm_ppi_handle == NULL)
-		return -ENODEV;
-
-	return sysfs_create_group(parent, &ppi_attr_grp);
+	return tpm_ppi_handle ? sysfs_create_group(parent, &ppi_attr_grp) : 0;
 }
 
 void tpm_remove_ppi(struct kobject *parent)
 {
-	sysfs_remove_group(parent, &ppi_attr_grp);
+	if (tpm_ppi_handle)
+		sysfs_remove_group(parent, &ppi_attr_grp);
 }
diff --git a/drivers/clk/Kconfig b/drivers/clk/Kconfig
index 6f56d3a4f01..9f9c5ae5359 100644
--- a/drivers/clk/Kconfig
+++ b/drivers/clk/Kconfig
@@ -30,14 +30,7 @@ config COMMON_CLK_WM831X
           Supports the clocking subsystem of the WM831x/2x series of
 	  PMICs from Wolfson Microlectronics.
 
-config COMMON_CLK_VERSATILE
-	bool "Clock driver for ARM Reference designs"
-	depends on ARCH_INTEGRATOR || ARCH_REALVIEW || ARCH_VEXPRESS || ARM64
-	---help---
-          Supports clocking on ARM Reference designs:
-	  - Integrator/AP and Integrator/CP
-	  - RealView PB1176, EB, PB11MP and PBX
-	  - Versatile Express
+source "drivers/clk/versatile/Kconfig"
 
 config COMMON_CLK_MAX77686
 	tristate "Clock driver for Maxim 77686 MFD"
@@ -65,12 +58,12 @@ config COMMON_CLK_SI570
 	  clock generators.
 
 config COMMON_CLK_S2MPS11
-	tristate "Clock driver for S2MPS11/S5M8767 MFD"
+	tristate "Clock driver for S2MPS1X/S5M8767 MFD"
 	depends on MFD_SEC_CORE
 	---help---
-	  This driver supports S2MPS11/S5M8767 crystal oscillator clock. These
-	  multi-function devices have 3 fixed-rate oscillators, clocked at
-	  32KHz each.
+	  This driver supports S2MPS11/S2MPS14/S5M8767 crystal oscillator
+	  clock. These multi-function devices have two (S2MPS14) or three
+	  (S2MPS11, S5M8767) fixed-rate oscillators, clocked at 32KHz each.
 
 config CLK_TWL6040
 	tristate "External McPDM functional clock from twl6040"
@@ -115,3 +108,5 @@ endmenu
 
 source "drivers/clk/bcm/Kconfig"
 source "drivers/clk/mvebu/Kconfig"
+
+source "drivers/clk/samsung/Kconfig"
diff --git a/drivers/clk/Makefile b/drivers/clk/Makefile
index 5f8a28735c9..567f1025902 100644
--- a/drivers/clk/Makefile
+++ b/drivers/clk/Makefile
@@ -8,10 +8,12 @@ obj-$(CONFIG_COMMON_CLK)	+= clk-fixed-rate.o
 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
 
 # hardware specific clock types
 # please keep this section sorted lexicographically by file/directory path name
 obj-$(CONFIG_COMMON_CLK_AXI_CLKGEN)	+= clk-axi-clkgen.o
+obj-$(CONFIG_ARCH_AXXIA)		+= clk-axm5516.o
 obj-$(CONFIG_ARCH_BCM2835)		+= clk-bcm2835.o
 obj-$(CONFIG_ARCH_EFM32)		+= clk-efm32gg.o
 obj-$(CONFIG_ARCH_HIGHBANK)		+= clk-highbank.o
@@ -31,8 +33,10 @@ obj-$(CONFIG_COMMON_CLK_WM831X)		+= clk-wm831x.o
 obj-$(CONFIG_COMMON_CLK_XGENE)		+= clk-xgene.o
 obj-$(CONFIG_COMMON_CLK_AT91)		+= at91/
 obj-$(CONFIG_ARCH_BCM_MOBILE)		+= bcm/
+obj-$(CONFIG_ARCH_BERLIN)		+= berlin/
 obj-$(CONFIG_ARCH_HI3xxx)		+= hisilicon/
 obj-$(CONFIG_ARCH_HIP04)		+= hisilicon/
+obj-$(CONFIG_ARCH_HIX5HD2)		+= hisilicon/
 obj-$(CONFIG_COMMON_CLK_KEYSTONE)	+= keystone/
 ifeq ($(CONFIG_COMMON_CLK), y)
 obj-$(CONFIG_ARCH_MMP)			+= mmp/
@@ -41,7 +45,7 @@ obj-$(CONFIG_PLAT_ORION)		+= mvebu/
 obj-$(CONFIG_ARCH_MXS)			+= mxs/
 obj-$(CONFIG_COMMON_CLK_QCOM)		+= qcom/
 obj-$(CONFIG_ARCH_ROCKCHIP)		+= rockchip/
-obj-$(CONFIG_PLAT_SAMSUNG)		+= samsung/
+obj-$(CONFIG_COMMON_CLK_SAMSUNG)	+= samsung/
 obj-$(CONFIG_ARCH_SHMOBILE_MULTI)	+= shmobile/
 obj-$(CONFIG_ARCH_SIRF)			+= sirf/
 obj-$(CONFIG_ARCH_SOCFPGA)		+= socfpga/
diff --git a/drivers/clk/at91/Makefile b/drivers/clk/at91/Makefile
index 46c1d3d0d66..4998aee5926 100644
--- a/drivers/clk/at91/Makefile
+++ b/drivers/clk/at91/Makefile
@@ -2,8 +2,8 @@
 # Makefile for at91 specific clk
 #
 
-obj-y += pmc.o
-obj-y += clk-main.o clk-pll.o clk-plldiv.o clk-master.o
+obj-y += pmc.o sckc.o
+obj-y += clk-slow.o clk-main.o clk-pll.o clk-plldiv.o clk-master.o
 obj-y += clk-system.o clk-peripheral.o clk-programmable.o
 
 obj-$(CONFIG_HAVE_AT91_UTMI)		+= clk-utmi.o
diff --git a/drivers/clk/at91/clk-main.c b/drivers/clk/at91/clk-main.c
index 8e9e8cc0412..733306131b9 100644
--- a/drivers/clk/at91/clk-main.c
+++ b/drivers/clk/at91/clk-main.c
@@ -30,99 +30,546 @@
 #define MAINF_LOOP_MIN_WAIT	(USEC_PER_SEC / SLOW_CLOCK_FREQ)
 #define MAINF_LOOP_MAX_WAIT	MAINFRDY_TIMEOUT
 
-struct clk_main {
+#define MOR_KEY_MASK		(0xff << 16)
+
+struct clk_main_osc {
 	struct clk_hw hw;
 	struct at91_pmc *pmc;
-	unsigned long rate;
 	unsigned int irq;
 	wait_queue_head_t wait;
 };
 
-#define to_clk_main(hw) container_of(hw, struct clk_main, hw)
+#define to_clk_main_osc(hw) container_of(hw, struct clk_main_osc, hw)
+
+struct clk_main_rc_osc {
+	struct clk_hw hw;
+	struct at91_pmc *pmc;
+	unsigned int irq;
+	wait_queue_head_t wait;
+	unsigned long frequency;
+	unsigned long accuracy;
+};
+
+#define to_clk_main_rc_osc(hw) container_of(hw, struct clk_main_rc_osc, hw)
+
+struct clk_rm9200_main {
+	struct clk_hw hw;
+	struct at91_pmc *pmc;
+};
+
+#define to_clk_rm9200_main(hw) container_of(hw, struct clk_rm9200_main, hw)
 
-static irqreturn_t clk_main_irq_handler(int irq, void *dev_id)
+struct clk_sam9x5_main {
+	struct clk_hw hw;
+	struct at91_pmc *pmc;
+	unsigned int irq;
+	wait_queue_head_t wait;
+	u8 parent;
+};
+
+#define to_clk_sam9x5_main(hw) container_of(hw, struct clk_sam9x5_main, hw)
+
+static irqreturn_t clk_main_osc_irq_handler(int irq, void *dev_id)
 {
-	struct clk_main *clkmain = (struct clk_main *)dev_id;
+	struct clk_main_osc *osc = dev_id;
 
-	wake_up(&clkmain->wait);
-	disable_irq_nosync(clkmain->irq);
+	wake_up(&osc->wait);
+	disable_irq_nosync(osc->irq);
 
 	return IRQ_HANDLED;
 }
 
-static int clk_main_prepare(struct clk_hw *hw)
+static int clk_main_osc_prepare(struct clk_hw *hw)
 {
-	struct clk_main *clkmain = to_clk_main(hw);
-	struct at91_pmc *pmc = clkmain->pmc;
-	unsigned long halt_time, timeout;
+	struct clk_main_osc *osc = to_clk_main_osc(hw);
+	struct at91_pmc *pmc = osc->pmc;
 	u32 tmp;
 
+	tmp = pmc_read(pmc, AT91_CKGR_MOR) & ~MOR_KEY_MASK;
+	if (tmp & AT91_PMC_OSCBYPASS)
+		return 0;
+
+	if (!(tmp & AT91_PMC_MOSCEN)) {
+		tmp |= AT91_PMC_MOSCEN | AT91_PMC_KEY;
+		pmc_write(pmc, AT91_CKGR_MOR, tmp);
+	}
+
 	while (!(pmc_read(pmc, AT91_PMC_SR) & AT91_PMC_MOSCS)) {
-		enable_irq(clkmain->irq);
-		wait_event(clkmain->wait,
+		enable_irq(osc->irq);
+		wait_event(osc->wait,
 			   pmc_read(pmc, AT91_PMC_SR) & AT91_PMC_MOSCS);
 	}
 
-	if (clkmain->rate)
-		return 0;
+	return 0;
+}
+
+static void clk_main_osc_unprepare(struct clk_hw *hw)
+{
+	struct clk_main_osc *osc = to_clk_main_osc(hw);
+	struct at91_pmc *pmc = osc->pmc;
+	u32 tmp = pmc_read(pmc, AT91_CKGR_MOR);
+
+	if (tmp & AT91_PMC_OSCBYPASS)
+		return;
+
+	if (!(tmp & AT91_PMC_MOSCEN))
+		return;
+
+	tmp &= ~(AT91_PMC_KEY | AT91_PMC_MOSCEN);
+	pmc_write(pmc, AT91_CKGR_MOR, tmp | AT91_PMC_KEY);
+}
+
+static int clk_main_osc_is_prepared(struct clk_hw *hw)
+{
+	struct clk_main_osc *osc = to_clk_main_osc(hw);
+	struct at91_pmc *pmc = osc->pmc;
+	u32 tmp = pmc_read(pmc, AT91_CKGR_MOR);
+
+	if (tmp & AT91_PMC_OSCBYPASS)
+		return 1;
+
+	return !!((pmc_read(pmc, AT91_PMC_SR) & AT91_PMC_MOSCS) &&
+		  (pmc_read(pmc, AT91_CKGR_MOR) & AT91_PMC_MOSCEN));
+}
+
+static const struct clk_ops main_osc_ops = {
+	.prepare = clk_main_osc_prepare,
+	.unprepare = clk_main_osc_unprepare,
+	.is_prepared = clk_main_osc_is_prepared,
+};
+
+static struct clk * __init
+at91_clk_register_main_osc(struct at91_pmc *pmc,
+			   unsigned int irq,
+			   const char *name,
+			   const char *parent_name,
+			   bool bypass)
+{
+	int ret;
+	struct clk_main_osc *osc;
+	struct clk *clk = NULL;
+	struct clk_init_data init;
+
+	if (!pmc || !irq || !name || !parent_name)
+		return ERR_PTR(-EINVAL);
+
+	osc = kzalloc(sizeof(*osc), GFP_KERNEL);
+	if (!osc)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &main_osc_ops;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+	init.flags = CLK_IGNORE_UNUSED;
+
+	osc->hw.init = &init;
+	osc->pmc = pmc;
+	osc->irq = irq;
+
+	init_waitqueue_head(&osc->wait);
+	irq_set_status_flags(osc->irq, IRQ_NOAUTOEN);
+	ret = request_irq(osc->irq, clk_main_osc_irq_handler,
+			  IRQF_TRIGGER_HIGH, name, osc);
+	if (ret)
+		return ERR_PTR(ret);
+
+	if (bypass)
+		pmc_write(pmc, AT91_CKGR_MOR,
+			  (pmc_read(pmc, AT91_CKGR_MOR) &
+			   ~(MOR_KEY_MASK | AT91_PMC_MOSCEN)) |
+			  AT91_PMC_OSCBYPASS | AT91_PMC_KEY);
+
+	clk = clk_register(NULL, &osc->hw);
+	if (IS_ERR(clk)) {
+		free_irq(irq, osc);
+		kfree(osc);
+	}
+
+	return clk;
+}
+
+void __init of_at91rm9200_clk_main_osc_setup(struct device_node *np,
+					     struct at91_pmc *pmc)
+{
+	struct clk *clk;
+	unsigned int irq;
+	const char *name = np->name;
+	const char *parent_name;
+	bool bypass;
+
+	of_property_read_string(np, "clock-output-names", &name);
+	bypass = of_property_read_bool(np, "atmel,osc-bypass");
+	parent_name = of_clk_get_parent_name(np, 0);
+
+	irq = irq_of_parse_and_map(np, 0);
+	if (!irq)
+		return;
+
+	clk = at91_clk_register_main_osc(pmc, irq, name, parent_name, bypass);
+	if (IS_ERR(clk))
+		return;
+
+	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+}
+
+static irqreturn_t clk_main_rc_osc_irq_handler(int irq, void *dev_id)
+{
+	struct clk_main_rc_osc *osc = dev_id;
+
+	wake_up(&osc->wait);
+	disable_irq_nosync(osc->irq);
+
+	return IRQ_HANDLED;
+}
+
+static int clk_main_rc_osc_prepare(struct clk_hw *hw)
+{
+	struct clk_main_rc_osc *osc = to_clk_main_rc_osc(hw);
+	struct at91_pmc *pmc = osc->pmc;
+	u32 tmp;
+
+	tmp = pmc_read(pmc, AT91_CKGR_MOR) & ~MOR_KEY_MASK;
+
+	if (!(tmp & AT91_PMC_MOSCRCEN)) {
+		tmp |= AT91_PMC_MOSCRCEN | AT91_PMC_KEY;
+		pmc_write(pmc, AT91_CKGR_MOR, tmp);
+	}
+
+	while (!(pmc_read(pmc, AT91_PMC_SR) & AT91_PMC_MOSCRCS)) {
+		enable_irq(osc->irq);
+		wait_event(osc->wait,
+			   pmc_read(pmc, AT91_PMC_SR) & AT91_PMC_MOSCRCS);
+	}
+
+	return 0;
+}
+
+static void clk_main_rc_osc_unprepare(struct clk_hw *hw)
+{
+	struct clk_main_rc_osc *osc = to_clk_main_rc_osc(hw);
+	struct at91_pmc *pmc = osc->pmc;
+	u32 tmp = pmc_read(pmc, AT91_CKGR_MOR);
+
+	if (!(tmp & AT91_PMC_MOSCRCEN))
+		return;
+
+	tmp &= ~(MOR_KEY_MASK | AT91_PMC_MOSCRCEN);
+	pmc_write(pmc, AT91_CKGR_MOR, tmp | AT91_PMC_KEY);
+}
+
+static int clk_main_rc_osc_is_prepared(struct clk_hw *hw)
+{
+	struct clk_main_rc_osc *osc = to_clk_main_rc_osc(hw);
+	struct at91_pmc *pmc = osc->pmc;
+
+	return !!((pmc_read(pmc, AT91_PMC_SR) & AT91_PMC_MOSCRCS) &&
+		  (pmc_read(pmc, AT91_CKGR_MOR) & AT91_PMC_MOSCRCEN));
+}
+
+static unsigned long clk_main_rc_osc_recalc_rate(struct clk_hw *hw,
+						 unsigned long parent_rate)
+{
+	struct clk_main_rc_osc *osc = to_clk_main_rc_osc(hw);
+
+	return osc->frequency;
+}
+
+static unsigned long clk_main_rc_osc_recalc_accuracy(struct clk_hw *hw,
+						     unsigned long parent_acc)
+{
+	struct clk_main_rc_osc *osc = to_clk_main_rc_osc(hw);
+
+	return osc->accuracy;
+}
+
+static const struct clk_ops main_rc_osc_ops = {
+	.prepare = clk_main_rc_osc_prepare,
+	.unprepare = clk_main_rc_osc_unprepare,
+	.is_prepared = clk_main_rc_osc_is_prepared,
+	.recalc_rate = clk_main_rc_osc_recalc_rate,
+	.recalc_accuracy = clk_main_rc_osc_recalc_accuracy,
+};
+
+static struct clk * __init
+at91_clk_register_main_rc_osc(struct at91_pmc *pmc,
+			      unsigned int irq,
+			      const char *name,
+			      u32 frequency, u32 accuracy)
+{
+	int ret;
+	struct clk_main_rc_osc *osc;
+	struct clk *clk = NULL;
+	struct clk_init_data init;
+
+	if (!pmc || !irq || !name || !frequency)
+		return ERR_PTR(-EINVAL);
+
+	osc = kzalloc(sizeof(*osc), GFP_KERNEL);
+	if (!osc)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &main_rc_osc_ops;
+	init.parent_names = NULL;
+	init.num_parents = 0;
+	init.flags = CLK_IS_ROOT | CLK_IGNORE_UNUSED;
+
+	osc->hw.init = &init;
+	osc->pmc = pmc;
+	osc->irq = irq;
+	osc->frequency = frequency;
+	osc->accuracy = accuracy;
+
+	init_waitqueue_head(&osc->wait);
+	irq_set_status_flags(osc->irq, IRQ_NOAUTOEN);
+	ret = request_irq(osc->irq, clk_main_rc_osc_irq_handler,
+			  IRQF_TRIGGER_HIGH, name, osc);
+	if (ret)
+		return ERR_PTR(ret);
+
+	clk = clk_register(NULL, &osc->hw);
+	if (IS_ERR(clk)) {
+		free_irq(irq, osc);
+		kfree(osc);
+	}
+
+	return clk;
+}
+
+void __init of_at91sam9x5_clk_main_rc_osc_setup(struct device_node *np,
+						struct at91_pmc *pmc)
+{
+	struct clk *clk;
+	unsigned int irq;
+	u32 frequency = 0;
+	u32 accuracy = 0;
+	const char *name = np->name;
+
+	of_property_read_string(np, "clock-output-names", &name);
+	of_property_read_u32(np, "clock-frequency", &frequency);
+	of_property_read_u32(np, "clock-accuracy", &accuracy);
+
+	irq = irq_of_parse_and_map(np, 0);
+	if (!irq)
+		return;
+
+	clk = at91_clk_register_main_rc_osc(pmc, irq, name, frequency,
+					    accuracy);
+	if (IS_ERR(clk))
+		return;
+
+	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+}
+
+
+static int clk_main_probe_frequency(struct at91_pmc *pmc)
+{
+	unsigned long prep_time, timeout;
+	u32 tmp;
 
 	timeout = jiffies + usecs_to_jiffies(MAINFRDY_TIMEOUT);
 	do {
-		halt_time = jiffies;
+		prep_time = jiffies;
 		tmp = pmc_read(pmc, AT91_CKGR_MCFR);
 		if (tmp & AT91_PMC_MAINRDY)
 			return 0;
 		usleep_range(MAINF_LOOP_MIN_WAIT, MAINF_LOOP_MAX_WAIT);
-	} while (time_before(halt_time, timeout));
+	} while (time_before(prep_time, timeout));
 
-	return 0;
+	return -ETIMEDOUT;
 }
 
-static int clk_main_is_prepared(struct clk_hw *hw)
+static unsigned long clk_main_recalc_rate(struct at91_pmc *pmc,
+					  unsigned long parent_rate)
 {
-	struct clk_main *clkmain = to_clk_main(hw);
+	u32 tmp;
+
+	if (parent_rate)
+		return parent_rate;
+
+	tmp = pmc_read(pmc, AT91_CKGR_MCFR);
+	if (!(tmp & AT91_PMC_MAINRDY))
+		return 0;
 
-	return !!(pmc_read(clkmain->pmc, AT91_PMC_SR) & AT91_PMC_MOSCS);
+	return ((tmp & AT91_PMC_MAINF) * SLOW_CLOCK_FREQ) / MAINF_DIV;
 }
 
-static unsigned long clk_main_recalc_rate(struct clk_hw *hw,
-					  unsigned long parent_rate)
+static int clk_rm9200_main_prepare(struct clk_hw *hw)
 {
-	u32 tmp;
-	struct clk_main *clkmain = to_clk_main(hw);
+	struct clk_rm9200_main *clkmain = to_clk_rm9200_main(hw);
+
+	return clk_main_probe_frequency(clkmain->pmc);
+}
+
+static int clk_rm9200_main_is_prepared(struct clk_hw *hw)
+{
+	struct clk_rm9200_main *clkmain = to_clk_rm9200_main(hw);
+
+	return !!(pmc_read(clkmain->pmc, AT91_CKGR_MCFR) & AT91_PMC_MAINRDY);
+}
+
+static unsigned long clk_rm9200_main_recalc_rate(struct clk_hw *hw,
+						 unsigned long parent_rate)
+{
+	struct clk_rm9200_main *clkmain = to_clk_rm9200_main(hw);
+
+	return clk_main_recalc_rate(clkmain->pmc, parent_rate);
+}
+
+static const struct clk_ops rm9200_main_ops = {
+	.prepare = clk_rm9200_main_prepare,
+	.is_prepared = clk_rm9200_main_is_prepared,
+	.recalc_rate = clk_rm9200_main_recalc_rate,
+};
+
+static struct clk * __init
+at91_clk_register_rm9200_main(struct at91_pmc *pmc,
+			      const char *name,
+			      const char *parent_name)
+{
+	struct clk_rm9200_main *clkmain;
+	struct clk *clk = NULL;
+	struct clk_init_data init;
+
+	if (!pmc || !name)
+		return ERR_PTR(-EINVAL);
+
+	if (!parent_name)
+		return ERR_PTR(-EINVAL);
+
+	clkmain = kzalloc(sizeof(*clkmain), GFP_KERNEL);
+	if (!clkmain)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &rm9200_main_ops;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+	init.flags = 0;
+
+	clkmain->hw.init = &init;
+	clkmain->pmc = pmc;
+
+	clk = clk_register(NULL, &clkmain->hw);
+	if (IS_ERR(clk))
+		kfree(clkmain);
+
+	return clk;
+}
+
+void __init of_at91rm9200_clk_main_setup(struct device_node *np,
+					 struct at91_pmc *pmc)
+{
+	struct clk *clk;
+	const char *parent_name;
+	const char *name = np->name;
+
+	parent_name = of_clk_get_parent_name(np, 0);
+	of_property_read_string(np, "clock-output-names", &name);
+
+	clk = at91_clk_register_rm9200_main(pmc, name, parent_name);
+	if (IS_ERR(clk))
+		return;
+
+	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+}
+
+static irqreturn_t clk_sam9x5_main_irq_handler(int irq, void *dev_id)
+{
+	struct clk_sam9x5_main *clkmain = dev_id;
+
+	wake_up(&clkmain->wait);
+	disable_irq_nosync(clkmain->irq);
+
+	return IRQ_HANDLED;
+}
+
+static int clk_sam9x5_main_prepare(struct clk_hw *hw)
+{
+	struct clk_sam9x5_main *clkmain = to_clk_sam9x5_main(hw);
 	struct at91_pmc *pmc = clkmain->pmc;
 
-	if (clkmain->rate)
-		return clkmain->rate;
+	while (!(pmc_read(pmc, AT91_PMC_SR) & AT91_PMC_MOSCSELS)) {
+		enable_irq(clkmain->irq);
+		wait_event(clkmain->wait,
+			   pmc_read(pmc, AT91_PMC_SR) & AT91_PMC_MOSCSELS);
+	}
+
+	return clk_main_probe_frequency(pmc);
+}
 
-	tmp = pmc_read(pmc, AT91_CKGR_MCFR) & AT91_PMC_MAINF;
-	clkmain->rate = (tmp * parent_rate) / MAINF_DIV;
+static int clk_sam9x5_main_is_prepared(struct clk_hw *hw)
+{
+	struct clk_sam9x5_main *clkmain = to_clk_sam9x5_main(hw);
 
-	return clkmain->rate;
+	return !!(pmc_read(clkmain->pmc, AT91_PMC_SR) & AT91_PMC_MOSCSELS);
 }
 
-static const struct clk_ops main_ops = {
-	.prepare = clk_main_prepare,
-	.is_prepared = clk_main_is_prepared,
-	.recalc_rate = clk_main_recalc_rate,
+static unsigned long clk_sam9x5_main_recalc_rate(struct clk_hw *hw,
+						 unsigned long parent_rate)
+{
+	struct clk_sam9x5_main *clkmain = to_clk_sam9x5_main(hw);
+
+	return clk_main_recalc_rate(clkmain->pmc, parent_rate);
+}
+
+static int clk_sam9x5_main_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct clk_sam9x5_main *clkmain = to_clk_sam9x5_main(hw);
+	struct at91_pmc *pmc = clkmain->pmc;
+	u32 tmp;
+
+	if (index > 1)
+		return -EINVAL;
+
+	tmp = pmc_read(pmc, AT91_CKGR_MOR) & ~MOR_KEY_MASK;
+
+	if (index && !(tmp & AT91_PMC_MOSCSEL))
+		pmc_write(pmc, AT91_CKGR_MOR, tmp | AT91_PMC_MOSCSEL);
+	else if (!index && (tmp & AT91_PMC_MOSCSEL))
+		pmc_write(pmc, AT91_CKGR_MOR, tmp & ~AT91_PMC_MOSCSEL);
+
+	while (!(pmc_read(pmc, AT91_PMC_SR) & AT91_PMC_MOSCSELS)) {
+		enable_irq(clkmain->irq);
+		wait_event(clkmain->wait,
+			   pmc_read(pmc, AT91_PMC_SR) & AT91_PMC_MOSCSELS);
+	}
+
+	return 0;
+}
+
+static u8 clk_sam9x5_main_get_parent(struct clk_hw *hw)
+{
+	struct clk_sam9x5_main *clkmain = to_clk_sam9x5_main(hw);
+
+	return !!(pmc_read(clkmain->pmc, AT91_CKGR_MOR) & AT91_PMC_MOSCEN);
+}
+
+static const struct clk_ops sam9x5_main_ops = {
+	.prepare = clk_sam9x5_main_prepare,
+	.is_prepared = clk_sam9x5_main_is_prepared,
+	.recalc_rate = clk_sam9x5_main_recalc_rate,
+	.set_parent = clk_sam9x5_main_set_parent,
+	.get_parent = clk_sam9x5_main_get_parent,
 };
 
 static struct clk * __init
-at91_clk_register_main(struct at91_pmc *pmc,
-		       unsigned int irq,
-		       const char *name,
-		       const char *parent_name,
-		       unsigned long rate)
+at91_clk_register_sam9x5_main(struct at91_pmc *pmc,
+			      unsigned int irq,
+			      const char *name,
+			      const char **parent_names,
+			      int num_parents)
 {
 	int ret;
-	struct clk_main *clkmain;
+	struct clk_sam9x5_main *clkmain;
 	struct clk *clk = NULL;
 	struct clk_init_data init;
 
 	if (!pmc || !irq || !name)
 		return ERR_PTR(-EINVAL);
 
-	if (!rate && !parent_name)
+	if (!parent_names || !num_parents)
 		return ERR_PTR(-EINVAL);
 
 	clkmain = kzalloc(sizeof(*clkmain), GFP_KERNEL);
@@ -130,19 +577,20 @@ at91_clk_register_main(struct at91_pmc *pmc,
 		return ERR_PTR(-ENOMEM);
 
 	init.name = name;
-	init.ops = &main_ops;
-	init.parent_names = parent_name ? &parent_name : NULL;
-	init.num_parents = parent_name ? 1 : 0;
-	init.flags = parent_name ? 0 : CLK_IS_ROOT;
+	init.ops = &sam9x5_main_ops;
+	init.parent_names = parent_names;
+	init.num_parents = num_parents;
+	init.flags = CLK_SET_PARENT_GATE;
 
 	clkmain->hw.init = &init;
-	clkmain->rate = rate;
 	clkmain->pmc = pmc;
 	clkmain->irq = irq;
+	clkmain->parent = !!(pmc_read(clkmain->pmc, AT91_CKGR_MOR) &
+			     AT91_PMC_MOSCEN);
 	init_waitqueue_head(&clkmain->wait);
 	irq_set_status_flags(clkmain->irq, IRQ_NOAUTOEN);
-	ret = request_irq(clkmain->irq, clk_main_irq_handler,
-			  IRQF_TRIGGER_HIGH, "clk-main", clkmain);
+	ret = request_irq(clkmain->irq, clk_sam9x5_main_irq_handler,
+			  IRQF_TRIGGER_HIGH, name, clkmain);
 	if (ret)
 		return ERR_PTR(ret);
 
@@ -155,33 +603,36 @@ at91_clk_register_main(struct at91_pmc *pmc,
 	return clk;
 }
 
-
-
-static void __init
-of_at91_clk_main_setup(struct device_node *np, struct at91_pmc *pmc)
+void __init of_at91sam9x5_clk_main_setup(struct device_node *np,
+					 struct at91_pmc *pmc)
 {
 	struct clk *clk;
+	const char *parent_names[2];
+	int num_parents;
 	unsigned int irq;
-	const char *parent_name;
 	const char *name = np->name;
-	u32 rate = 0;
+	int i;
+
+	num_parents = of_count_phandle_with_args(np, "clocks", "#clock-cells");
+	if (num_parents <= 0 || num_parents > 2)
+		return;
+
+	for (i = 0; i < num_parents; ++i) {
+		parent_names[i] = of_clk_get_parent_name(np, i);
+		if (!parent_names[i])
+			return;
+	}
 
-	parent_name = of_clk_get_parent_name(np, 0);
 	of_property_read_string(np, "clock-output-names", &name);
-	of_property_read_u32(np, "clock-frequency", &rate);
+
 	irq = irq_of_parse_and_map(np, 0);
 	if (!irq)
 		return;
 
-	clk = at91_clk_register_main(pmc, irq, name, parent_name, rate);
+	clk = at91_clk_register_sam9x5_main(pmc, irq, name, parent_names,
+					    num_parents);
 	if (IS_ERR(clk))
 		return;
 
 	of_clk_add_provider(np, of_clk_src_simple_get, clk);
 }
-
-void __init of_at91rm9200_clk_main_setup(struct device_node *np,
-					 struct at91_pmc *pmc)
-{
-	of_at91_clk_main_setup(np, pmc);
-}
diff --git a/drivers/clk/at91/clk-slow.c b/drivers/clk/at91/clk-slow.c
new file mode 100644
index 00000000000..0300c46ee24
--- /dev/null
+++ b/drivers/clk/at91/clk-slow.c
@@ -0,0 +1,467 @@
+/*
+ * drivers/clk/at91/clk-slow.c
+ *
+ *  Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.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"
+#include "sckc.h"
+
+#define SLOW_CLOCK_FREQ		32768
+#define SLOWCK_SW_CYCLES	5
+#define SLOWCK_SW_TIME_USEC	((SLOWCK_SW_CYCLES * USEC_PER_SEC) / \
+				 SLOW_CLOCK_FREQ)
+
+#define	AT91_SCKC_CR			0x00
+#define		AT91_SCKC_RCEN		(1 << 0)
+#define		AT91_SCKC_OSC32EN	(1 << 1)
+#define		AT91_SCKC_OSC32BYP	(1 << 2)
+#define		AT91_SCKC_OSCSEL	(1 << 3)
+
+struct clk_slow_osc {
+	struct clk_hw hw;
+	void __iomem *sckcr;
+	unsigned long startup_usec;
+};
+
+#define to_clk_slow_osc(hw) container_of(hw, struct clk_slow_osc, hw)
+
+struct clk_slow_rc_osc {
+	struct clk_hw hw;
+	void __iomem *sckcr;
+	unsigned long frequency;
+	unsigned long accuracy;
+	unsigned long startup_usec;
+};
+
+#define to_clk_slow_rc_osc(hw) container_of(hw, struct clk_slow_rc_osc, hw)
+
+struct clk_sam9260_slow {
+	struct clk_hw hw;
+	struct at91_pmc *pmc;
+};
+
+#define to_clk_sam9260_slow(hw) container_of(hw, struct clk_sam9260_slow, hw)
+
+struct clk_sam9x5_slow {
+	struct clk_hw hw;
+	void __iomem *sckcr;
+	u8 parent;
+};
+
+#define to_clk_sam9x5_slow(hw) container_of(hw, struct clk_sam9x5_slow, hw)
+
+
+static int clk_slow_osc_prepare(struct clk_hw *hw)
+{
+	struct clk_slow_osc *osc = to_clk_slow_osc(hw);
+	void __iomem *sckcr = osc->sckcr;
+	u32 tmp = readl(sckcr);
+
+	if (tmp & AT91_SCKC_OSC32BYP)
+		return 0;
+
+	writel(tmp | AT91_SCKC_OSC32EN, sckcr);
+
+	usleep_range(osc->startup_usec, osc->startup_usec + 1);
+
+	return 0;
+}
+
+static void clk_slow_osc_unprepare(struct clk_hw *hw)
+{
+	struct clk_slow_osc *osc = to_clk_slow_osc(hw);
+	void __iomem *sckcr = osc->sckcr;
+	u32 tmp = readl(sckcr);
+
+	if (tmp & AT91_SCKC_OSC32BYP)
+		return;
+
+	writel(tmp & ~AT91_SCKC_OSC32EN, sckcr);
+}
+
+static int clk_slow_osc_is_prepared(struct clk_hw *hw)
+{
+	struct clk_slow_osc *osc = to_clk_slow_osc(hw);
+	void __iomem *sckcr = osc->sckcr;
+	u32 tmp = readl(sckcr);
+
+	if (tmp & AT91_SCKC_OSC32BYP)
+		return 1;
+
+	return !!(tmp & AT91_SCKC_OSC32EN);
+}
+
+static const struct clk_ops slow_osc_ops = {
+	.prepare = clk_slow_osc_prepare,
+	.unprepare = clk_slow_osc_unprepare,
+	.is_prepared = clk_slow_osc_is_prepared,
+};
+
+static struct clk * __init
+at91_clk_register_slow_osc(void __iomem *sckcr,
+			   const char *name,
+			   const char *parent_name,
+			   unsigned long startup,
+			   bool bypass)
+{
+	struct clk_slow_osc *osc;
+	struct clk *clk = NULL;
+	struct clk_init_data init;
+
+	if (!sckcr || !name || !parent_name)
+		return ERR_PTR(-EINVAL);
+
+	osc = kzalloc(sizeof(*osc), GFP_KERNEL);
+	if (!osc)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &slow_osc_ops;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+	init.flags = CLK_IGNORE_UNUSED;
+
+	osc->hw.init = &init;
+	osc->sckcr = sckcr;
+	osc->startup_usec = startup;
+
+	if (bypass)
+		writel((readl(sckcr) & ~AT91_SCKC_OSC32EN) | AT91_SCKC_OSC32BYP,
+		       sckcr);
+
+	clk = clk_register(NULL, &osc->hw);
+	if (IS_ERR(clk))
+		kfree(osc);
+
+	return clk;
+}
+
+void __init of_at91sam9x5_clk_slow_osc_setup(struct device_node *np,
+					     void __iomem *sckcr)
+{
+	struct clk *clk;
+	const char *parent_name;
+	const char *name = np->name;
+	u32 startup;
+	bool bypass;
+
+	parent_name = of_clk_get_parent_name(np, 0);
+	of_property_read_string(np, "clock-output-names", &name);
+	of_property_read_u32(np, "atmel,startup-time-usec", &startup);
+	bypass = of_property_read_bool(np, "atmel,osc-bypass");
+
+	clk = at91_clk_register_slow_osc(sckcr, name, parent_name, startup,
+					 bypass);
+	if (IS_ERR(clk))
+		return;
+
+	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+}
+
+static unsigned long clk_slow_rc_osc_recalc_rate(struct clk_hw *hw,
+						 unsigned long parent_rate)
+{
+	struct clk_slow_rc_osc *osc = to_clk_slow_rc_osc(hw);
+
+	return osc->frequency;
+}
+
+static unsigned long clk_slow_rc_osc_recalc_accuracy(struct clk_hw *hw,
+						     unsigned long parent_acc)
+{
+	struct clk_slow_rc_osc *osc = to_clk_slow_rc_osc(hw);
+
+	return osc->accuracy;
+}
+
+static int clk_slow_rc_osc_prepare(struct clk_hw *hw)
+{
+	struct clk_slow_rc_osc *osc = to_clk_slow_rc_osc(hw);
+	void __iomem *sckcr = osc->sckcr;
+
+	writel(readl(sckcr) | AT91_SCKC_RCEN, sckcr);
+
+	usleep_range(osc->startup_usec, osc->startup_usec + 1);
+
+	return 0;
+}
+
+static void clk_slow_rc_osc_unprepare(struct clk_hw *hw)
+{
+	struct clk_slow_rc_osc *osc = to_clk_slow_rc_osc(hw);
+	void __iomem *sckcr = osc->sckcr;
+
+	writel(readl(sckcr) & ~AT91_SCKC_RCEN, sckcr);
+}
+
+static int clk_slow_rc_osc_is_prepared(struct clk_hw *hw)
+{
+	struct clk_slow_rc_osc *osc = to_clk_slow_rc_osc(hw);
+
+	return !!(readl(osc->sckcr) & AT91_SCKC_RCEN);
+}
+
+static const struct clk_ops slow_rc_osc_ops = {
+	.prepare = clk_slow_rc_osc_prepare,
+	.unprepare = clk_slow_rc_osc_unprepare,
+	.is_prepared = clk_slow_rc_osc_is_prepared,
+	.recalc_rate = clk_slow_rc_osc_recalc_rate,
+	.recalc_accuracy = clk_slow_rc_osc_recalc_accuracy,
+};
+
+static struct clk * __init
+at91_clk_register_slow_rc_osc(void __iomem *sckcr,
+			      const char *name,
+			      unsigned long frequency,
+			      unsigned long accuracy,
+			      unsigned long startup)
+{
+	struct clk_slow_rc_osc *osc;
+	struct clk *clk = NULL;
+	struct clk_init_data init;
+
+	if (!sckcr || !name)
+		return ERR_PTR(-EINVAL);
+
+	osc = kzalloc(sizeof(*osc), GFP_KERNEL);
+	if (!osc)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &slow_rc_osc_ops;
+	init.parent_names = NULL;
+	init.num_parents = 0;
+	init.flags = CLK_IS_ROOT | CLK_IGNORE_UNUSED;
+
+	osc->hw.init = &init;
+	osc->sckcr = sckcr;
+	osc->frequency = frequency;
+	osc->accuracy = accuracy;
+	osc->startup_usec = startup;
+
+	clk = clk_register(NULL, &osc->hw);
+	if (IS_ERR(clk))
+		kfree(osc);
+
+	return clk;
+}
+
+void __init of_at91sam9x5_clk_slow_rc_osc_setup(struct device_node *np,
+						void __iomem *sckcr)
+{
+	struct clk *clk;
+	u32 frequency = 0;
+	u32 accuracy = 0;
+	u32 startup = 0;
+	const char *name = np->name;
+
+	of_property_read_string(np, "clock-output-names", &name);
+	of_property_read_u32(np, "clock-frequency", &frequency);
+	of_property_read_u32(np, "clock-accuracy", &accuracy);
+	of_property_read_u32(np, "atmel,startup-time-usec", &startup);
+
+	clk = at91_clk_register_slow_rc_osc(sckcr, name, frequency, accuracy,
+					    startup);
+	if (IS_ERR(clk))
+		return;
+
+	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+}
+
+static int clk_sam9x5_slow_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct clk_sam9x5_slow *slowck = to_clk_sam9x5_slow(hw);
+	void __iomem *sckcr = slowck->sckcr;
+	u32 tmp;
+
+	if (index > 1)
+		return -EINVAL;
+
+	tmp = readl(sckcr);
+
+	if ((!index && !(tmp & AT91_SCKC_OSCSEL)) ||
+	    (index && (tmp & AT91_SCKC_OSCSEL)))
+		return 0;
+
+	if (index)
+		tmp |= AT91_SCKC_OSCSEL;
+	else
+		tmp &= ~AT91_SCKC_OSCSEL;
+
+	writel(tmp, sckcr);
+
+	usleep_range(SLOWCK_SW_TIME_USEC, SLOWCK_SW_TIME_USEC + 1);
+
+	return 0;
+}
+
+static u8 clk_sam9x5_slow_get_parent(struct clk_hw *hw)
+{
+	struct clk_sam9x5_slow *slowck = to_clk_sam9x5_slow(hw);
+
+	return !!(readl(slowck->sckcr) & AT91_SCKC_OSCSEL);
+}
+
+static const struct clk_ops sam9x5_slow_ops = {
+	.set_parent = clk_sam9x5_slow_set_parent,
+	.get_parent = clk_sam9x5_slow_get_parent,
+};
+
+static struct clk * __init
+at91_clk_register_sam9x5_slow(void __iomem *sckcr,
+			      const char *name,
+			      const char **parent_names,
+			      int num_parents)
+{
+	struct clk_sam9x5_slow *slowck;
+	struct clk *clk = NULL;
+	struct clk_init_data init;
+
+	if (!sckcr || !name || !parent_names || !num_parents)
+		return ERR_PTR(-EINVAL);
+
+	slowck = kzalloc(sizeof(*slowck), GFP_KERNEL);
+	if (!slowck)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &sam9x5_slow_ops;
+	init.parent_names = parent_names;
+	init.num_parents = num_parents;
+	init.flags = 0;
+
+	slowck->hw.init = &init;
+	slowck->sckcr = sckcr;
+	slowck->parent = !!(readl(sckcr) & AT91_SCKC_OSCSEL);
+
+	clk = clk_register(NULL, &slowck->hw);
+	if (IS_ERR(clk))
+		kfree(slowck);
+
+	return clk;
+}
+
+void __init of_at91sam9x5_clk_slow_setup(struct device_node *np,
+					 void __iomem *sckcr)
+{
+	struct clk *clk;
+	const char *parent_names[2];
+	int num_parents;
+	const char *name = np->name;
+	int i;
+
+	num_parents = of_count_phandle_with_args(np, "clocks", "#clock-cells");
+	if (num_parents <= 0 || num_parents > 2)
+		return;
+
+	for (i = 0; i < num_parents; ++i) {
+		parent_names[i] = of_clk_get_parent_name(np, i);
+		if (!parent_names[i])
+			return;
+	}
+
+	of_property_read_string(np, "clock-output-names", &name);
+
+	clk = at91_clk_register_sam9x5_slow(sckcr, name, parent_names,
+					    num_parents);
+	if (IS_ERR(clk))
+		return;
+
+	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+}
+
+static u8 clk_sam9260_slow_get_parent(struct clk_hw *hw)
+{
+	struct clk_sam9260_slow *slowck = to_clk_sam9260_slow(hw);
+
+	return !!(pmc_read(slowck->pmc, AT91_PMC_SR) & AT91_PMC_OSCSEL);
+}
+
+static const struct clk_ops sam9260_slow_ops = {
+	.get_parent = clk_sam9260_slow_get_parent,
+};
+
+static struct clk * __init
+at91_clk_register_sam9260_slow(struct at91_pmc *pmc,
+			       const char *name,
+			       const char **parent_names,
+			       int num_parents)
+{
+	struct clk_sam9260_slow *slowck;
+	struct clk *clk = NULL;
+	struct clk_init_data init;
+
+	if (!pmc || !name)
+		return ERR_PTR(-EINVAL);
+
+	if (!parent_names || !num_parents)
+		return ERR_PTR(-EINVAL);
+
+	slowck = kzalloc(sizeof(*slowck), GFP_KERNEL);
+	if (!slowck)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &sam9260_slow_ops;
+	init.parent_names = parent_names;
+	init.num_parents = num_parents;
+	init.flags = 0;
+
+	slowck->hw.init = &init;
+	slowck->pmc = pmc;
+
+	clk = clk_register(NULL, &slowck->hw);
+	if (IS_ERR(clk))
+		kfree(slowck);
+
+	return clk;
+}
+
+void __init of_at91sam9260_clk_slow_setup(struct device_node *np,
+					  struct at91_pmc *pmc)
+{
+	struct clk *clk;
+	const char *parent_names[2];
+	int num_parents;
+	const char *name = np->name;
+	int i;
+
+	num_parents = of_count_phandle_with_args(np, "clocks", "#clock-cells");
+	if (num_parents <= 0 || num_parents > 1)
+		return;
+
+	for (i = 0; i < num_parents; ++i) {
+		parent_names[i] = of_clk_get_parent_name(np, i);
+		if (!parent_names[i])
+			return;
+	}
+
+	of_property_read_string(np, "clock-output-names", &name);
+
+	clk = at91_clk_register_sam9260_slow(pmc, name, parent_names,
+					     num_parents);
+	if (IS_ERR(clk))
+		return;
+
+	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+}
diff --git a/drivers/clk/at91/pmc.c b/drivers/clk/at91/pmc.c
index 6a61477a57e..524196bb35a 100644
--- a/drivers/clk/at91/pmc.c
+++ b/drivers/clk/at91/pmc.c
@@ -229,11 +229,28 @@ out_free_pmc:
 }
 
 static const struct of_device_id pmc_clk_ids[] __initconst = {
+	/* Slow oscillator */
+	{
+		.compatible = "atmel,at91sam9260-clk-slow",
+		.data = of_at91sam9260_clk_slow_setup,
+	},
 	/* Main clock */
 	{
+		.compatible = "atmel,at91rm9200-clk-main-osc",
+		.data = of_at91rm9200_clk_main_osc_setup,
+	},
+	{
+		.compatible = "atmel,at91sam9x5-clk-main-rc-osc",
+		.data = of_at91sam9x5_clk_main_rc_osc_setup,
+	},
+	{
 		.compatible = "atmel,at91rm9200-clk-main",
 		.data = of_at91rm9200_clk_main_setup,
 	},
+	{
+		.compatible = "atmel,at91sam9x5-clk-main",
+		.data = of_at91sam9x5_clk_main_setup,
+	},
 	/* PLL clocks */
 	{
 		.compatible = "atmel,at91rm9200-clk-pll",
diff --git a/drivers/clk/at91/pmc.h b/drivers/clk/at91/pmc.h
index 441350983cc..6c762597611 100644
--- a/drivers/clk/at91/pmc.h
+++ b/drivers/clk/at91/pmc.h
@@ -58,8 +58,17 @@ static inline void pmc_write(struct at91_pmc *pmc, int offset, u32 value)
 int of_at91_get_clk_range(struct device_node *np, const char *propname,
 			  struct clk_range *range);
 
+extern void __init of_at91sam9260_clk_slow_setup(struct device_node *np,
+						 struct at91_pmc *pmc);
+
+extern void __init of_at91rm9200_clk_main_osc_setup(struct device_node *np,
+						    struct at91_pmc *pmc);
+extern void __init of_at91sam9x5_clk_main_rc_osc_setup(struct device_node *np,
+						       struct at91_pmc *pmc);
 extern void __init of_at91rm9200_clk_main_setup(struct device_node *np,
 						struct at91_pmc *pmc);
+extern void __init of_at91sam9x5_clk_main_setup(struct device_node *np,
+						struct at91_pmc *pmc);
 
 extern void __init of_at91rm9200_clk_pll_setup(struct device_node *np,
 					       struct at91_pmc *pmc);
diff --git a/drivers/clk/at91/sckc.c b/drivers/clk/at91/sckc.c
new file mode 100644
index 00000000000..1184d76a7ab
--- /dev/null
+++ b/drivers/clk/at91/sckc.c
@@ -0,0 +1,57 @@
+/*
+ * drivers/clk/at91/sckc.c
+ *
+ *  Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.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/of.h>
+#include <linux/of_address.h>
+#include <linux/io.h>
+
+#include "sckc.h"
+
+static const struct of_device_id sckc_clk_ids[] __initconst = {
+	/* Slow clock */
+	{
+		.compatible = "atmel,at91sam9x5-clk-slow-osc",
+		.data = of_at91sam9x5_clk_slow_osc_setup,
+	},
+	{
+		.compatible = "atmel,at91sam9x5-clk-slow-rc-osc",
+		.data = of_at91sam9x5_clk_slow_rc_osc_setup,
+	},
+	{
+		.compatible = "atmel,at91sam9x5-clk-slow",
+		.data = of_at91sam9x5_clk_slow_setup,
+	},
+	{ /*sentinel*/ }
+};
+
+static void __init of_at91sam9x5_sckc_setup(struct device_node *np)
+{
+	struct device_node *childnp;
+	void (*clk_setup)(struct device_node *, void __iomem *);
+	const struct of_device_id *clk_id;
+	void __iomem *regbase = of_iomap(np, 0);
+
+	if (!regbase)
+		return;
+
+	for_each_child_of_node(np, childnp) {
+		clk_id = of_match_node(sckc_clk_ids, childnp);
+		if (!clk_id)
+			continue;
+		clk_setup = clk_id->data;
+		clk_setup(childnp, regbase);
+	}
+}
+CLK_OF_DECLARE(at91sam9x5_clk_sckc, "atmel,at91sam9x5-sckc",
+	       of_at91sam9x5_sckc_setup);
diff --git a/drivers/clk/at91/sckc.h b/drivers/clk/at91/sckc.h
new file mode 100644
index 00000000000..836fcf59820
--- /dev/null
+++ b/drivers/clk/at91/sckc.h
@@ -0,0 +1,22 @@
+/*
+ * drivers/clk/at91/sckc.h
+ *
+ *  Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.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.
+ */
+
+#ifndef __AT91_SCKC_H_
+#define __AT91_SCKC_H_
+
+extern void __init of_at91sam9x5_clk_slow_osc_setup(struct device_node *np,
+						    void __iomem *sckcr);
+extern void __init of_at91sam9x5_clk_slow_rc_osc_setup(struct device_node *np,
+						       void __iomem *sckcr);
+extern void __init of_at91sam9x5_clk_slow_setup(struct device_node *np,
+						void __iomem *sckcr);
+
+#endif /* __AT91_SCKC_H_ */
diff --git a/drivers/clk/bcm/Kconfig b/drivers/clk/bcm/Kconfig
index a7262fb8ce5..75506e53075 100644
--- a/drivers/clk/bcm/Kconfig
+++ b/drivers/clk/bcm/Kconfig
@@ -6,4 +6,4 @@ config CLK_BCM_KONA
 	help
 	  Enable common clock framework support for Broadcom SoCs
 	  using "Kona" style clock control units, including those
-	  in the BCM281xx family.
+	  in the BCM281xx and BCM21664 families.
diff --git a/drivers/clk/bcm/Makefile b/drivers/clk/bcm/Makefile
index cf93359aa86..6297d05a9a1 100644
--- a/drivers/clk/bcm/Makefile
+++ b/drivers/clk/bcm/Makefile
@@ -1,3 +1,4 @@
 obj-$(CONFIG_CLK_BCM_KONA)	+= clk-kona.o
 obj-$(CONFIG_CLK_BCM_KONA)	+= clk-kona-setup.o
 obj-$(CONFIG_CLK_BCM_KONA)	+= clk-bcm281xx.o
+obj-$(CONFIG_CLK_BCM_KONA)	+= clk-bcm21664.o
diff --git a/drivers/clk/bcm/clk-bcm21664.c b/drivers/clk/bcm/clk-bcm21664.c
new file mode 100644
index 00000000000..eeae4cad228
--- /dev/null
+++ b/drivers/clk/bcm/clk-bcm21664.c
@@ -0,0 +1,290 @@
+/*
+ * Copyright (C) 2014 Broadcom Corporation
+ * Copyright 2014 Linaro 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 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 "clk-kona.h"
+#include "dt-bindings/clock/bcm21664.h"
+
+#define BCM21664_CCU_COMMON(_name, _capname) \
+	KONA_CCU_COMMON(BCM21664, _name, _capname)
+
+/* Root CCU */
+
+static struct peri_clk_data frac_1m_data = {
+	.gate		= HW_SW_GATE(0x214, 16, 0, 1),
+	.clocks		= CLOCKS("ref_crystal"),
+};
+
+static struct ccu_data root_ccu_data = {
+	BCM21664_CCU_COMMON(root, ROOT),
+	/* no policy control */
+	.kona_clks	= {
+		[BCM21664_ROOT_CCU_FRAC_1M] =
+			KONA_CLK(root, frac_1m, peri),
+		[BCM21664_ROOT_CCU_CLOCK_COUNT] = LAST_KONA_CLK,
+	},
+};
+
+/* AON CCU */
+
+static struct peri_clk_data hub_timer_data = {
+	.gate		= HW_SW_GATE(0x0414, 16, 0, 1),
+	.hyst		= HYST(0x0414, 8, 9),
+	.clocks		= CLOCKS("bbl_32k",
+				 "frac_1m",
+				 "dft_19_5m"),
+	.sel		= SELECTOR(0x0a10, 0, 2),
+	.trig		= TRIGGER(0x0a40, 4),
+};
+
+static struct ccu_data aon_ccu_data = {
+	BCM21664_CCU_COMMON(aon, AON),
+	.policy		= {
+		.enable		= CCU_LVM_EN(0x0034, 0),
+		.control	= CCU_POLICY_CTL(0x000c, 0, 1, 2),
+	},
+	.kona_clks	= {
+		[BCM21664_AON_CCU_HUB_TIMER] =
+			KONA_CLK(aon, hub_timer, peri),
+		[BCM21664_AON_CCU_CLOCK_COUNT] = LAST_KONA_CLK,
+	},
+};
+
+/* Master CCU */
+
+static struct peri_clk_data sdio1_data = {
+	.gate		= HW_SW_GATE(0x0358, 18, 2, 3),
+	.clocks		= CLOCKS("ref_crystal",
+				 "var_52m",
+				 "ref_52m",
+				 "var_96m",
+				 "ref_96m"),
+	.sel		= SELECTOR(0x0a28, 0, 3),
+	.div		= DIVIDER(0x0a28, 4, 14),
+	.trig		= TRIGGER(0x0afc, 9),
+};
+
+static struct peri_clk_data sdio2_data = {
+	.gate		= HW_SW_GATE(0x035c, 18, 2, 3),
+	.clocks		= CLOCKS("ref_crystal",
+				 "var_52m",
+				 "ref_52m",
+				 "var_96m",
+				 "ref_96m"),
+	.sel		= SELECTOR(0x0a2c, 0, 3),
+	.div		= DIVIDER(0x0a2c, 4, 14),
+	.trig		= TRIGGER(0x0afc, 10),
+};
+
+static struct peri_clk_data sdio3_data = {
+	.gate		= HW_SW_GATE(0x0364, 18, 2, 3),
+	.clocks		= CLOCKS("ref_crystal",
+				 "var_52m",
+				 "ref_52m",
+				 "var_96m",
+				 "ref_96m"),
+	.sel		= SELECTOR(0x0a34, 0, 3),
+	.div		= DIVIDER(0x0a34, 4, 14),
+	.trig		= TRIGGER(0x0afc, 12),
+};
+
+static struct peri_clk_data sdio4_data = {
+	.gate		= HW_SW_GATE(0x0360, 18, 2, 3),
+	.clocks		= CLOCKS("ref_crystal",
+				 "var_52m",
+				 "ref_52m",
+				 "var_96m",
+				 "ref_96m"),
+	.sel		= SELECTOR(0x0a30, 0, 3),
+	.div		= DIVIDER(0x0a30, 4, 14),
+	.trig		= TRIGGER(0x0afc, 11),
+};
+
+static struct peri_clk_data sdio1_sleep_data = {
+	.clocks		= CLOCKS("ref_32k"),	/* Verify */
+	.gate		= HW_SW_GATE(0x0358, 18, 2, 3),
+};
+
+static struct peri_clk_data sdio2_sleep_data = {
+	.clocks		= CLOCKS("ref_32k"),	/* Verify */
+	.gate		= HW_SW_GATE(0x035c, 18, 2, 3),
+};
+
+static struct peri_clk_data sdio3_sleep_data = {
+	.clocks		= CLOCKS("ref_32k"),	/* Verify */
+	.gate		= HW_SW_GATE(0x0364, 18, 2, 3),
+};
+
+static struct peri_clk_data sdio4_sleep_data = {
+	.clocks		= CLOCKS("ref_32k"),	/* Verify */
+	.gate		= HW_SW_GATE(0x0360, 18, 2, 3),
+};
+
+static struct ccu_data master_ccu_data = {
+	BCM21664_CCU_COMMON(master, MASTER),
+	.policy		= {
+		.enable		= CCU_LVM_EN(0x0034, 0),
+		.control	= CCU_POLICY_CTL(0x000c, 0, 1, 2),
+	},
+	.kona_clks	= {
+		[BCM21664_MASTER_CCU_SDIO1] =
+			KONA_CLK(master, sdio1, peri),
+		[BCM21664_MASTER_CCU_SDIO2] =
+			KONA_CLK(master, sdio2, peri),
+		[BCM21664_MASTER_CCU_SDIO3] =
+			KONA_CLK(master, sdio3, peri),
+		[BCM21664_MASTER_CCU_SDIO4] =
+			KONA_CLK(master, sdio4, peri),
+		[BCM21664_MASTER_CCU_SDIO1_SLEEP] =
+			KONA_CLK(master, sdio1_sleep, peri),
+		[BCM21664_MASTER_CCU_SDIO2_SLEEP] =
+			KONA_CLK(master, sdio2_sleep, peri),
+		[BCM21664_MASTER_CCU_SDIO3_SLEEP] =
+			KONA_CLK(master, sdio3_sleep, peri),
+		[BCM21664_MASTER_CCU_SDIO4_SLEEP] =
+			KONA_CLK(master, sdio4_sleep, peri),
+		[BCM21664_MASTER_CCU_CLOCK_COUNT] = LAST_KONA_CLK,
+	},
+};
+
+/* Slave CCU */
+
+static struct peri_clk_data uartb_data = {
+	.gate		= HW_SW_GATE(0x0400, 18, 2, 3),
+	.clocks		= CLOCKS("ref_crystal",
+				 "var_156m",
+				 "ref_156m"),
+	.sel		= SELECTOR(0x0a10, 0, 2),
+	.div		= FRAC_DIVIDER(0x0a10, 4, 12, 8),
+	.trig		= TRIGGER(0x0afc, 2),
+};
+
+static struct peri_clk_data uartb2_data = {
+	.gate		= HW_SW_GATE(0x0404, 18, 2, 3),
+	.clocks		= CLOCKS("ref_crystal",
+				 "var_156m",
+				 "ref_156m"),
+	.sel		= SELECTOR(0x0a14, 0, 2),
+	.div		= FRAC_DIVIDER(0x0a14, 4, 12, 8),
+	.trig		= TRIGGER(0x0afc, 3),
+};
+
+static struct peri_clk_data uartb3_data = {
+	.gate		= HW_SW_GATE(0x0408, 18, 2, 3),
+	.clocks		= CLOCKS("ref_crystal",
+				 "var_156m",
+				 "ref_156m"),
+	.sel		= SELECTOR(0x0a18, 0, 2),
+	.div		= FRAC_DIVIDER(0x0a18, 4, 12, 8),
+	.trig		= TRIGGER(0x0afc, 4),
+};
+
+static struct peri_clk_data bsc1_data = {
+	.gate		= HW_SW_GATE(0x0458, 18, 2, 3),
+	.clocks		= CLOCKS("ref_crystal",
+				 "var_104m",
+				 "ref_104m",
+				 "var_13m",
+				 "ref_13m"),
+	.sel		= SELECTOR(0x0a64, 0, 3),
+	.trig		= TRIGGER(0x0afc, 23),
+};
+
+static struct peri_clk_data bsc2_data = {
+	.gate		= HW_SW_GATE(0x045c, 18, 2, 3),
+	.clocks		= CLOCKS("ref_crystal",
+				 "var_104m",
+				 "ref_104m",
+				 "var_13m",
+				 "ref_13m"),
+	.sel		= SELECTOR(0x0a68, 0, 3),
+	.trig		= TRIGGER(0x0afc, 24),
+};
+
+static struct peri_clk_data bsc3_data = {
+	.gate		= HW_SW_GATE(0x0470, 18, 2, 3),
+	.clocks		= CLOCKS("ref_crystal",
+				 "var_104m",
+				 "ref_104m",
+				 "var_13m",
+				 "ref_13m"),
+	.sel		= SELECTOR(0x0a7c, 0, 3),
+	.trig		= TRIGGER(0x0afc, 18),
+};
+
+static struct peri_clk_data bsc4_data = {
+	.gate		= HW_SW_GATE(0x0474, 18, 2, 3),
+	.clocks		= CLOCKS("ref_crystal",
+				 "var_104m",
+				 "ref_104m",
+				 "var_13m",
+				 "ref_13m"),
+	.sel		= SELECTOR(0x0a80, 0, 3),
+	.trig		= TRIGGER(0x0afc, 19),
+};
+
+static struct ccu_data slave_ccu_data = {
+	BCM21664_CCU_COMMON(slave, SLAVE),
+       .policy		= {
+		.enable		= CCU_LVM_EN(0x0034, 0),
+		.control	= CCU_POLICY_CTL(0x000c, 0, 1, 2),
+	},
+	.kona_clks	= {
+		[BCM21664_SLAVE_CCU_UARTB] =
+			KONA_CLK(slave, uartb, peri),
+		[BCM21664_SLAVE_CCU_UARTB2] =
+			KONA_CLK(slave, uartb2, peri),
+		[BCM21664_SLAVE_CCU_UARTB3] =
+			KONA_CLK(slave, uartb3, peri),
+		[BCM21664_SLAVE_CCU_BSC1] =
+			KONA_CLK(slave, bsc1, peri),
+		[BCM21664_SLAVE_CCU_BSC2] =
+			KONA_CLK(slave, bsc2, peri),
+		[BCM21664_SLAVE_CCU_BSC3] =
+			KONA_CLK(slave, bsc3, peri),
+		[BCM21664_SLAVE_CCU_BSC4] =
+			KONA_CLK(slave, bsc4, peri),
+		[BCM21664_SLAVE_CCU_CLOCK_COUNT] = LAST_KONA_CLK,
+	},
+};
+
+/* Device tree match table callback functions */
+
+static void __init kona_dt_root_ccu_setup(struct device_node *node)
+{
+	kona_dt_ccu_setup(&root_ccu_data, node);
+}
+
+static void __init kona_dt_aon_ccu_setup(struct device_node *node)
+{
+	kona_dt_ccu_setup(&aon_ccu_data, node);
+}
+
+static void __init kona_dt_master_ccu_setup(struct device_node *node)
+{
+	kona_dt_ccu_setup(&master_ccu_data, node);
+}
+
+static void __init kona_dt_slave_ccu_setup(struct device_node *node)
+{
+	kona_dt_ccu_setup(&slave_ccu_data, node);
+}
+
+CLK_OF_DECLARE(bcm21664_root_ccu, BCM21664_DT_ROOT_CCU_COMPAT,
+			kona_dt_root_ccu_setup);
+CLK_OF_DECLARE(bcm21664_aon_ccu, BCM21664_DT_AON_CCU_COMPAT,
+			kona_dt_aon_ccu_setup);
+CLK_OF_DECLARE(bcm21664_master_ccu, BCM21664_DT_MASTER_CCU_COMPAT,
+			kona_dt_master_ccu_setup);
+CLK_OF_DECLARE(bcm21664_slave_ccu, BCM21664_DT_SLAVE_CCU_COMPAT,
+			kona_dt_slave_ccu_setup);
diff --git a/drivers/clk/bcm/clk-bcm281xx.c b/drivers/clk/bcm/clk-bcm281xx.c
index 3c66de696ae..502a487d62c 100644
--- a/drivers/clk/bcm/clk-bcm281xx.c
+++ b/drivers/clk/bcm/clk-bcm281xx.c
@@ -15,14 +15,10 @@
 #include "clk-kona.h"
 #include "dt-bindings/clock/bcm281xx.h"
 
-/* bcm11351 CCU device tree "compatible" strings */
-#define BCM11351_DT_ROOT_CCU_COMPAT	"brcm,bcm11351-root-ccu"
-#define BCM11351_DT_AON_CCU_COMPAT	"brcm,bcm11351-aon-ccu"
-#define BCM11351_DT_HUB_CCU_COMPAT	"brcm,bcm11351-hub-ccu"
-#define BCM11351_DT_MASTER_CCU_COMPAT	"brcm,bcm11351-master-ccu"
-#define BCM11351_DT_SLAVE_CCU_COMPAT	"brcm,bcm11351-slave-ccu"
+#define BCM281XX_CCU_COMMON(_name, _ucase_name) \
+	KONA_CCU_COMMON(BCM281XX, _name, _ucase_name)
 
-/* Root CCU clocks */
+/* Root CCU */
 
 static struct peri_clk_data frac_1m_data = {
 	.gate		= HW_SW_GATE(0x214, 16, 0, 1),
@@ -31,7 +27,16 @@ static struct peri_clk_data frac_1m_data = {
 	.clocks		= CLOCKS("ref_crystal"),
 };
 
-/* AON CCU clocks */
+static struct ccu_data root_ccu_data = {
+	BCM281XX_CCU_COMMON(root, ROOT),
+	.kona_clks	= {
+		[BCM281XX_ROOT_CCU_FRAC_1M] =
+			KONA_CLK(root, frac_1m, peri),
+		[BCM281XX_ROOT_CCU_CLOCK_COUNT] = LAST_KONA_CLK,
+	},
+};
+
+/* AON CCU */
 
 static struct peri_clk_data hub_timer_data = {
 	.gate		= HW_SW_GATE(0x0414, 16, 0, 1),
@@ -60,7 +65,20 @@ static struct peri_clk_data pmu_bsc_var_data = {
 	.trig		= TRIGGER(0x0a40, 2),
 };
 
-/* Hub CCU clocks */
+static struct ccu_data aon_ccu_data = {
+	BCM281XX_CCU_COMMON(aon, AON),
+	.kona_clks	= {
+		[BCM281XX_AON_CCU_HUB_TIMER] =
+			KONA_CLK(aon, hub_timer, peri),
+		[BCM281XX_AON_CCU_PMU_BSC] =
+			KONA_CLK(aon, pmu_bsc, peri),
+		[BCM281XX_AON_CCU_PMU_BSC_VAR] =
+			KONA_CLK(aon, pmu_bsc_var, peri),
+		[BCM281XX_AON_CCU_CLOCK_COUNT] = LAST_KONA_CLK,
+	},
+};
+
+/* Hub CCU */
 
 static struct peri_clk_data tmon_1m_data = {
 	.gate		= HW_SW_GATE(0x04a4, 18, 2, 3),
@@ -70,7 +88,16 @@ static struct peri_clk_data tmon_1m_data = {
 	.trig		= TRIGGER(0x0e84, 1),
 };
 
-/* Master CCU clocks */
+static struct ccu_data hub_ccu_data = {
+	BCM281XX_CCU_COMMON(hub, HUB),
+	.kona_clks	= {
+		[BCM281XX_HUB_CCU_TMON_1M] =
+			KONA_CLK(hub, tmon_1m, peri),
+		[BCM281XX_HUB_CCU_CLOCK_COUNT] = LAST_KONA_CLK,
+	},
+};
+
+/* Master CCU */
 
 static struct peri_clk_data sdio1_data = {
 	.gate		= HW_SW_GATE(0x0358, 18, 2, 3),
@@ -153,7 +180,28 @@ static struct peri_clk_data hsic2_12m_data = {
 	.trig		= TRIGGER(0x0afc, 5),
 };
 
-/* Slave CCU clocks */
+static struct ccu_data master_ccu_data = {
+	BCM281XX_CCU_COMMON(master, MASTER),
+	.kona_clks	= {
+		[BCM281XX_MASTER_CCU_SDIO1] =
+			KONA_CLK(master, sdio1, peri),
+		[BCM281XX_MASTER_CCU_SDIO2] =
+			KONA_CLK(master, sdio2, peri),
+		[BCM281XX_MASTER_CCU_SDIO3] =
+			KONA_CLK(master, sdio3, peri),
+		[BCM281XX_MASTER_CCU_SDIO4] =
+			KONA_CLK(master, sdio4, peri),
+		[BCM281XX_MASTER_CCU_USB_IC] =
+			KONA_CLK(master, usb_ic, peri),
+		[BCM281XX_MASTER_CCU_HSIC2_48M] =
+			KONA_CLK(master, hsic2_48m, peri),
+		[BCM281XX_MASTER_CCU_HSIC2_12M] =
+			KONA_CLK(master, hsic2_12m, peri),
+		[BCM281XX_MASTER_CCU_CLOCK_COUNT] = LAST_KONA_CLK,
+	},
+};
+
+/* Slave CCU */
 
 static struct peri_clk_data uartb_data = {
 	.gate		= HW_SW_GATE(0x0400, 18, 2, 3),
@@ -261,156 +309,67 @@ static struct peri_clk_data pwm_data = {
 	.trig		= TRIGGER(0x0afc, 15),
 };
 
-/*
- * CCU setup routines
- *
- * These are called from kona_dt_ccu_setup() to initialize the array
- * of clocks provided by the CCU.  Once allocated, the entries in
- * the array are initialized by calling kona_clk_setup() with the
- * initialization data for each clock.  They return 0 if successful
- * or an error code otherwise.
- */
-static int __init bcm281xx_root_ccu_clks_setup(struct ccu_data *ccu)
-{
-	struct clk **clks;
-	size_t count = BCM281XX_ROOT_CCU_CLOCK_COUNT;
-
-	clks = kzalloc(count * sizeof(*clks), GFP_KERNEL);
-	if (!clks) {
-		pr_err("%s: failed to allocate root clocks\n", __func__);
-		return -ENOMEM;
-	}
-	ccu->data.clks = clks;
-	ccu->data.clk_num = count;
-
-	PERI_CLK_SETUP(clks, ccu, BCM281XX_ROOT_CCU_FRAC_1M, frac_1m);
-
-	return 0;
-}
-
-static int __init bcm281xx_aon_ccu_clks_setup(struct ccu_data *ccu)
-{
-	struct clk **clks;
-	size_t count = BCM281XX_AON_CCU_CLOCK_COUNT;
-
-	clks = kzalloc(count * sizeof(*clks), GFP_KERNEL);
-	if (!clks) {
-		pr_err("%s: failed to allocate aon clocks\n", __func__);
-		return -ENOMEM;
-	}
-	ccu->data.clks = clks;
-	ccu->data.clk_num = count;
-
-	PERI_CLK_SETUP(clks, ccu, BCM281XX_AON_CCU_HUB_TIMER, hub_timer);
-	PERI_CLK_SETUP(clks, ccu, BCM281XX_AON_CCU_PMU_BSC, pmu_bsc);
-	PERI_CLK_SETUP(clks, ccu, BCM281XX_AON_CCU_PMU_BSC_VAR, pmu_bsc_var);
-
-	return 0;
-}
-
-static int __init bcm281xx_hub_ccu_clks_setup(struct ccu_data *ccu)
-{
-	struct clk **clks;
-	size_t count = BCM281XX_HUB_CCU_CLOCK_COUNT;
-
-	clks = kzalloc(count * sizeof(*clks), GFP_KERNEL);
-	if (!clks) {
-		pr_err("%s: failed to allocate hub clocks\n", __func__);
-		return -ENOMEM;
-	}
-	ccu->data.clks = clks;
-	ccu->data.clk_num = count;
-
-	PERI_CLK_SETUP(clks, ccu, BCM281XX_HUB_CCU_TMON_1M, tmon_1m);
-
-	return 0;
-}
-
-static int __init bcm281xx_master_ccu_clks_setup(struct ccu_data *ccu)
-{
-	struct clk **clks;
-	size_t count = BCM281XX_MASTER_CCU_CLOCK_COUNT;
-
-	clks = kzalloc(count * sizeof(*clks), GFP_KERNEL);
-	if (!clks) {
-		pr_err("%s: failed to allocate master clocks\n", __func__);
-		return -ENOMEM;
-	}
-	ccu->data.clks = clks;
-	ccu->data.clk_num = count;
-
-	PERI_CLK_SETUP(clks, ccu, BCM281XX_MASTER_CCU_SDIO1, sdio1);
-	PERI_CLK_SETUP(clks, ccu, BCM281XX_MASTER_CCU_SDIO2, sdio2);
-	PERI_CLK_SETUP(clks, ccu, BCM281XX_MASTER_CCU_SDIO3, sdio3);
-	PERI_CLK_SETUP(clks, ccu, BCM281XX_MASTER_CCU_SDIO4, sdio4);
-	PERI_CLK_SETUP(clks, ccu, BCM281XX_MASTER_CCU_USB_IC, usb_ic);
-	PERI_CLK_SETUP(clks, ccu, BCM281XX_MASTER_CCU_HSIC2_48M, hsic2_48m);
-	PERI_CLK_SETUP(clks, ccu, BCM281XX_MASTER_CCU_HSIC2_12M, hsic2_12m);
-
-	return 0;
-}
-
-static int __init bcm281xx_slave_ccu_clks_setup(struct ccu_data *ccu)
-{
-	struct clk **clks;
-	size_t count = BCM281XX_SLAVE_CCU_CLOCK_COUNT;
-
-	clks = kzalloc(count * sizeof(*clks), GFP_KERNEL);
-	if (!clks) {
-		pr_err("%s: failed to allocate slave clocks\n", __func__);
-		return -ENOMEM;
-	}
-	ccu->data.clks = clks;
-	ccu->data.clk_num = count;
-
-	PERI_CLK_SETUP(clks, ccu, BCM281XX_SLAVE_CCU_UARTB, uartb);
-	PERI_CLK_SETUP(clks, ccu, BCM281XX_SLAVE_CCU_UARTB2, uartb2);
-	PERI_CLK_SETUP(clks, ccu, BCM281XX_SLAVE_CCU_UARTB3, uartb3);
-	PERI_CLK_SETUP(clks, ccu, BCM281XX_SLAVE_CCU_UARTB4, uartb4);
-	PERI_CLK_SETUP(clks, ccu, BCM281XX_SLAVE_CCU_SSP0, ssp0);
-	PERI_CLK_SETUP(clks, ccu, BCM281XX_SLAVE_CCU_SSP2, ssp2);
-	PERI_CLK_SETUP(clks, ccu, BCM281XX_SLAVE_CCU_BSC1, bsc1);
-	PERI_CLK_SETUP(clks, ccu, BCM281XX_SLAVE_CCU_BSC2, bsc2);
-	PERI_CLK_SETUP(clks, ccu, BCM281XX_SLAVE_CCU_BSC3, bsc3);
-	PERI_CLK_SETUP(clks, ccu, BCM281XX_SLAVE_CCU_PWM, pwm);
-
-	return 0;
-}
+static struct ccu_data slave_ccu_data = {
+	BCM281XX_CCU_COMMON(slave, SLAVE),
+	.kona_clks	= {
+		[BCM281XX_SLAVE_CCU_UARTB] =
+			KONA_CLK(slave, uartb, peri),
+		[BCM281XX_SLAVE_CCU_UARTB2] =
+			KONA_CLK(slave, uartb2, peri),
+		[BCM281XX_SLAVE_CCU_UARTB3] =
+			KONA_CLK(slave, uartb3, peri),
+		[BCM281XX_SLAVE_CCU_UARTB4] =
+			KONA_CLK(slave, uartb4, peri),
+		[BCM281XX_SLAVE_CCU_SSP0] =
+			KONA_CLK(slave, ssp0, peri),
+		[BCM281XX_SLAVE_CCU_SSP2] =
+			KONA_CLK(slave, ssp2, peri),
+		[BCM281XX_SLAVE_CCU_BSC1] =
+			KONA_CLK(slave, bsc1, peri),
+		[BCM281XX_SLAVE_CCU_BSC2] =
+			KONA_CLK(slave, bsc2, peri),
+		[BCM281XX_SLAVE_CCU_BSC3] =
+			KONA_CLK(slave, bsc3, peri),
+		[BCM281XX_SLAVE_CCU_PWM] =
+			KONA_CLK(slave, pwm, peri),
+		[BCM281XX_SLAVE_CCU_CLOCK_COUNT] = LAST_KONA_CLK,
+	},
+};
 
 /* Device tree match table callback functions */
 
 static void __init kona_dt_root_ccu_setup(struct device_node *node)
 {
-	kona_dt_ccu_setup(node, bcm281xx_root_ccu_clks_setup);
+	kona_dt_ccu_setup(&root_ccu_data, node);
 }
 
 static void __init kona_dt_aon_ccu_setup(struct device_node *node)
 {
-	kona_dt_ccu_setup(node, bcm281xx_aon_ccu_clks_setup);
+	kona_dt_ccu_setup(&aon_ccu_data, node);
 }
 
 static void __init kona_dt_hub_ccu_setup(struct device_node *node)
 {
-	kona_dt_ccu_setup(node, bcm281xx_hub_ccu_clks_setup);
+	kona_dt_ccu_setup(&hub_ccu_data, node);
 }
 
 static void __init kona_dt_master_ccu_setup(struct device_node *node)
 {
-	kona_dt_ccu_setup(node, bcm281xx_master_ccu_clks_setup);
+	kona_dt_ccu_setup(&master_ccu_data, node);
 }
 
 static void __init kona_dt_slave_ccu_setup(struct device_node *node)
 {
-	kona_dt_ccu_setup(node, bcm281xx_slave_ccu_clks_setup);
+	kona_dt_ccu_setup(&slave_ccu_data, node);
 }
 
-CLK_OF_DECLARE(bcm11351_root_ccu, BCM11351_DT_ROOT_CCU_COMPAT,
+CLK_OF_DECLARE(bcm281xx_root_ccu, BCM281XX_DT_ROOT_CCU_COMPAT,
 			kona_dt_root_ccu_setup);
-CLK_OF_DECLARE(bcm11351_aon_ccu, BCM11351_DT_AON_CCU_COMPAT,
+CLK_OF_DECLARE(bcm281xx_aon_ccu, BCM281XX_DT_AON_CCU_COMPAT,
 			kona_dt_aon_ccu_setup);
-CLK_OF_DECLARE(bcm11351_hub_ccu, BCM11351_DT_HUB_CCU_COMPAT,
+CLK_OF_DECLARE(bcm281xx_hub_ccu, BCM281XX_DT_HUB_CCU_COMPAT,
 			kona_dt_hub_ccu_setup);
-CLK_OF_DECLARE(bcm11351_master_ccu, BCM11351_DT_MASTER_CCU_COMPAT,
+CLK_OF_DECLARE(bcm281xx_master_ccu, BCM281XX_DT_MASTER_CCU_COMPAT,
 			kona_dt_master_ccu_setup);
-CLK_OF_DECLARE(bcm11351_slave_ccu, BCM11351_DT_SLAVE_CCU_COMPAT,
+CLK_OF_DECLARE(bcm281xx_slave_ccu, BCM281XX_DT_SLAVE_CCU_COMPAT,
 			kona_dt_slave_ccu_setup);
diff --git a/drivers/clk/bcm/clk-kona-setup.c b/drivers/clk/bcm/clk-kona-setup.c
index c7607feb18d..e5aededdd32 100644
--- a/drivers/clk/bcm/clk-kona-setup.c
+++ b/drivers/clk/bcm/clk-kona-setup.c
@@ -25,9 +25,34 @@ LIST_HEAD(ccu_list);	/* The list of set up CCUs */
 
 /* Validity checking */
 
+static bool ccu_data_offsets_valid(struct ccu_data *ccu)
+{
+	struct ccu_policy *ccu_policy = &ccu->policy;
+	u32 limit;
+
+	limit = ccu->range - sizeof(u32);
+	limit = round_down(limit, sizeof(u32));
+	if (ccu_policy_exists(ccu_policy)) {
+		if (ccu_policy->enable.offset > limit) {
+			pr_err("%s: bad policy enable offset for %s "
+					"(%u > %u)\n", __func__,
+				ccu->name, ccu_policy->enable.offset, limit);
+			return false;
+		}
+		if (ccu_policy->control.offset > limit) {
+			pr_err("%s: bad policy control offset for %s "
+					"(%u > %u)\n", __func__,
+				ccu->name, ccu_policy->control.offset, limit);
+			return false;
+		}
+	}
+
+	return true;
+}
+
 static bool clk_requires_trigger(struct kona_clk *bcm_clk)
 {
-	struct peri_clk_data *peri = bcm_clk->peri;
+	struct peri_clk_data *peri = bcm_clk->u.peri;
 	struct bcm_clk_sel *sel;
 	struct bcm_clk_div *div;
 
@@ -54,7 +79,9 @@ static bool clk_requires_trigger(struct kona_clk *bcm_clk)
 static bool peri_clk_data_offsets_valid(struct kona_clk *bcm_clk)
 {
 	struct peri_clk_data *peri;
+	struct bcm_clk_policy *policy;
 	struct bcm_clk_gate *gate;
+	struct bcm_clk_hyst *hyst;
 	struct bcm_clk_div *div;
 	struct bcm_clk_sel *sel;
 	struct bcm_clk_trig *trig;
@@ -63,37 +90,59 @@ static bool peri_clk_data_offsets_valid(struct kona_clk *bcm_clk)
 	u32 limit;
 
 	BUG_ON(bcm_clk->type != bcm_clk_peri);
-	peri = bcm_clk->peri;
-	name = bcm_clk->name;
+	peri = bcm_clk->u.peri;
+	name = bcm_clk->init_data.name;
 	range = bcm_clk->ccu->range;
 
 	limit = range - sizeof(u32);
 	limit = round_down(limit, sizeof(u32));
 
+	policy = &peri->policy;
+	if (policy_exists(policy)) {
+		if (policy->offset > limit) {
+			pr_err("%s: bad policy offset for %s (%u > %u)\n",
+				__func__, name, policy->offset, limit);
+			return false;
+		}
+	}
+
 	gate = &peri->gate;
+	hyst = &peri->hyst;
 	if (gate_exists(gate)) {
 		if (gate->offset > limit) {
 			pr_err("%s: bad gate offset for %s (%u > %u)\n",
 				__func__, name, gate->offset, limit);
 			return false;
 		}
+
+		if (hyst_exists(hyst)) {
+			if (hyst->offset > limit) {
+				pr_err("%s: bad hysteresis offset for %s "
+					"(%u > %u)\n", __func__,
+					name, hyst->offset, limit);
+				return false;
+			}
+		}
+	} else if (hyst_exists(hyst)) {
+		pr_err("%s: hysteresis but no gate for %s\n", __func__, name);
+		return false;
 	}
 
 	div = &peri->div;
 	if (divider_exists(div)) {
-		if (div->offset > limit) {
+		if (div->u.s.offset > limit) {
 			pr_err("%s: bad divider offset for %s (%u > %u)\n",
-				__func__, name, div->offset, limit);
+				__func__, name, div->u.s.offset, limit);
 			return false;
 		}
 	}
 
 	div = &peri->pre_div;
 	if (divider_exists(div)) {
-		if (div->offset > limit) {
+		if (div->u.s.offset > limit) {
 			pr_err("%s: bad pre-divider offset for %s "
 					"(%u > %u)\n",
-				__func__, name, div->offset, limit);
+				__func__, name, div->u.s.offset, limit);
 			return false;
 		}
 	}
@@ -167,6 +216,36 @@ static bool bitfield_valid(u32 shift, u32 width, const char *field_name,
 	return true;
 }
 
+static bool
+ccu_policy_valid(struct ccu_policy *ccu_policy, const char *ccu_name)
+{
+	struct bcm_lvm_en *enable = &ccu_policy->enable;
+	struct bcm_policy_ctl *control;
+
+	if (!bit_posn_valid(enable->bit, "policy enable", ccu_name))
+		return false;
+
+	control = &ccu_policy->control;
+	if (!bit_posn_valid(control->go_bit, "policy control GO", ccu_name))
+		return false;
+
+	if (!bit_posn_valid(control->atl_bit, "policy control ATL", ccu_name))
+		return false;
+
+	if (!bit_posn_valid(control->ac_bit, "policy control AC", ccu_name))
+		return false;
+
+	return true;
+}
+
+static bool policy_valid(struct bcm_clk_policy *policy, const char *clock_name)
+{
+	if (!bit_posn_valid(policy->bit, "policy", clock_name))
+		return false;
+
+	return true;
+}
+
 /*
  * All gates, if defined, have a status bit, and for hardware-only
  * gates, that's it.  Gates that can be software controlled also
@@ -196,6 +275,17 @@ static bool gate_valid(struct bcm_clk_gate *gate, const char *field_name,
 	return true;
 }
 
+static bool hyst_valid(struct bcm_clk_hyst *hyst, const char *clock_name)
+{
+	if (!bit_posn_valid(hyst->en_bit, "hysteresis enable", clock_name))
+		return false;
+
+	if (!bit_posn_valid(hyst->val_bit, "hysteresis value", clock_name))
+		return false;
+
+	return true;
+}
+
 /*
  * A selector bitfield must be valid.  Its parent_sel array must
  * also be reasonable for the field.
@@ -249,21 +339,22 @@ static bool div_valid(struct bcm_clk_div *div, const char *field_name,
 {
 	if (divider_is_fixed(div)) {
 		/* Any fixed divider value but 0 is OK */
-		if (div->fixed == 0) {
+		if (div->u.fixed == 0) {
 			pr_err("%s: bad %s fixed value 0 for %s\n", __func__,
 				field_name, clock_name);
 			return false;
 		}
 		return true;
 	}
-	if (!bitfield_valid(div->shift, div->width, field_name, clock_name))
+	if (!bitfield_valid(div->u.s.shift, div->u.s.width,
+				field_name, clock_name))
 		return false;
 
 	if (divider_has_fraction(div))
-		if (div->frac_width > div->width) {
+		if (div->u.s.frac_width > div->u.s.width) {
 			pr_warn("%s: bad %s fraction width for %s (%u > %u)\n",
 				__func__, field_name, clock_name,
-				div->frac_width, div->width);
+				div->u.s.frac_width, div->u.s.width);
 			return false;
 		}
 
@@ -278,7 +369,7 @@ static bool div_valid(struct bcm_clk_div *div, const char *field_name,
  */
 static bool kona_dividers_valid(struct kona_clk *bcm_clk)
 {
-	struct peri_clk_data *peri = bcm_clk->peri;
+	struct peri_clk_data *peri = bcm_clk->u.peri;
 	struct bcm_clk_div *div;
 	struct bcm_clk_div *pre_div;
 	u32 limit;
@@ -295,7 +386,7 @@ static bool kona_dividers_valid(struct kona_clk *bcm_clk)
 
 	limit = BITS_PER_BYTE * sizeof(u32);
 
-	return div->frac_width + pre_div->frac_width <= limit;
+	return div->u.s.frac_width + pre_div->u.s.frac_width <= limit;
 }
 
 
@@ -311,7 +402,9 @@ static bool
 peri_clk_data_valid(struct kona_clk *bcm_clk)
 {
 	struct peri_clk_data *peri;
+	struct bcm_clk_policy *policy;
 	struct bcm_clk_gate *gate;
+	struct bcm_clk_hyst *hyst;
 	struct bcm_clk_sel *sel;
 	struct bcm_clk_div *div;
 	struct bcm_clk_div *pre_div;
@@ -328,12 +421,21 @@ peri_clk_data_valid(struct kona_clk *bcm_clk)
 	if (!peri_clk_data_offsets_valid(bcm_clk))
 		return false;
 
-	peri = bcm_clk->peri;
-	name = bcm_clk->name;
+	peri = bcm_clk->u.peri;
+	name = bcm_clk->init_data.name;
+
+	policy = &peri->policy;
+	if (policy_exists(policy) && !policy_valid(policy, name))
+		return false;
+
 	gate = &peri->gate;
 	if (gate_exists(gate) && !gate_valid(gate, "gate", name))
 		return false;
 
+	hyst = &peri->hyst;
+	if (hyst_exists(hyst) && !hyst_valid(hyst, name))
+		return false;
+
 	sel = &peri->sel;
 	if (selector_exists(sel)) {
 		if (!sel_valid(sel, "selector", name))
@@ -566,7 +668,6 @@ static void peri_clk_teardown(struct peri_clk_data *data,
 				struct clk_init_data *init_data)
 {
 	clk_sel_teardown(&data->sel, init_data);
-	init_data->ops = NULL;
 }
 
 /*
@@ -575,10 +676,9 @@ static void peri_clk_teardown(struct peri_clk_data *data,
  * that can be assigned if the clock has one or more parent clocks
  * associated with it.
  */
-static int peri_clk_setup(struct ccu_data *ccu, struct peri_clk_data *data,
-			struct clk_init_data *init_data)
+static int
+peri_clk_setup(struct peri_clk_data *data, struct clk_init_data *init_data)
 {
-	init_data->ops = &kona_peri_clk_ops;
 	init_data->flags = CLK_IGNORE_UNUSED;
 
 	return clk_sel_setup(data->clocks, &data->sel, init_data);
@@ -588,12 +688,12 @@ static void bcm_clk_teardown(struct kona_clk *bcm_clk)
 {
 	switch (bcm_clk->type) {
 	case bcm_clk_peri:
-		peri_clk_teardown(bcm_clk->data, &bcm_clk->init_data);
+		peri_clk_teardown(bcm_clk->u.data, &bcm_clk->init_data);
 		break;
 	default:
 		break;
 	}
-	bcm_clk->data = NULL;
+	bcm_clk->u.data = NULL;
 	bcm_clk->type = bcm_clk_none;
 }
 
@@ -616,39 +716,26 @@ static void kona_clk_teardown(struct clk *clk)
 	bcm_clk_teardown(bcm_clk);
 }
 
-struct clk *kona_clk_setup(struct ccu_data *ccu, const char *name,
-			enum bcm_clk_type type, void *data)
+struct clk *kona_clk_setup(struct kona_clk *bcm_clk)
 {
-	struct kona_clk *bcm_clk;
-	struct clk_init_data *init_data;
+	struct clk_init_data *init_data = &bcm_clk->init_data;
 	struct clk *clk = NULL;
 
-	bcm_clk = kzalloc(sizeof(*bcm_clk), GFP_KERNEL);
-	if (!bcm_clk) {
-		pr_err("%s: failed to allocate bcm_clk for %s\n", __func__,
-			name);
-		return NULL;
-	}
-	bcm_clk->ccu = ccu;
-	bcm_clk->name = name;
-
-	init_data = &bcm_clk->init_data;
-	init_data->name = name;
-	switch (type) {
+	switch (bcm_clk->type) {
 	case bcm_clk_peri:
-		if (peri_clk_setup(ccu, data, init_data))
-			goto out_free;
+		if (peri_clk_setup(bcm_clk->u.data, init_data))
+			return NULL;
 		break;
 	default:
-		data = NULL;
-		break;
+		pr_err("%s: clock type %d invalid for %s\n", __func__,
+			(int)bcm_clk->type, init_data->name);
+		return NULL;
 	}
-	bcm_clk->type = type;
-	bcm_clk->data = data;
 
 	/* Make sure everything makes sense before we set it up */
 	if (!kona_clk_valid(bcm_clk)) {
-		pr_err("%s: clock data invalid for %s\n", __func__, name);
+		pr_err("%s: clock data invalid for %s\n", __func__,
+			init_data->name);
 		goto out_teardown;
 	}
 
@@ -656,7 +743,7 @@ struct clk *kona_clk_setup(struct ccu_data *ccu, const char *name,
 	clk = clk_register(NULL, &bcm_clk->hw);
 	if (IS_ERR(clk)) {
 		pr_err("%s: error registering clock %s (%ld)\n", __func__,
-				name, PTR_ERR(clk));
+			init_data->name, PTR_ERR(clk));
 		goto out_teardown;
 	}
 	BUG_ON(!clk);
@@ -664,8 +751,6 @@ struct clk *kona_clk_setup(struct ccu_data *ccu, const char *name,
 	return clk;
 out_teardown:
 	bcm_clk_teardown(bcm_clk);
-out_free:
-	kfree(bcm_clk);
 
 	return NULL;
 }
@@ -674,50 +759,64 @@ static void ccu_clks_teardown(struct ccu_data *ccu)
 {
 	u32 i;
 
-	for (i = 0; i < ccu->data.clk_num; i++)
-		kona_clk_teardown(ccu->data.clks[i]);
-	kfree(ccu->data.clks);
+	for (i = 0; i < ccu->clk_data.clk_num; i++)
+		kona_clk_teardown(ccu->clk_data.clks[i]);
+	kfree(ccu->clk_data.clks);
 }
 
 static void kona_ccu_teardown(struct ccu_data *ccu)
 {
-	if (!ccu)
-		return;
-
+	kfree(ccu->clk_data.clks);
+	ccu->clk_data.clks = NULL;
 	if (!ccu->base)
-		goto done;
+		return;
 
 	of_clk_del_provider(ccu->node);	/* safe if never added */
 	ccu_clks_teardown(ccu);
 	list_del(&ccu->links);
 	of_node_put(ccu->node);
+	ccu->node = NULL;
 	iounmap(ccu->base);
-done:
-	kfree(ccu->name);
-	kfree(ccu);
+	ccu->base = NULL;
+}
+
+static bool ccu_data_valid(struct ccu_data *ccu)
+{
+	struct ccu_policy *ccu_policy;
+
+	if (!ccu_data_offsets_valid(ccu))
+		return false;
+
+	ccu_policy = &ccu->policy;
+	if (ccu_policy_exists(ccu_policy))
+		if (!ccu_policy_valid(ccu_policy, ccu->name))
+			return false;
+
+	return true;
 }
 
 /*
  * Set up a CCU.  Call the provided ccu_clks_setup callback to
  * initialize the array of clocks provided by the CCU.
  */
-void __init kona_dt_ccu_setup(struct device_node *node,
-			int (*ccu_clks_setup)(struct ccu_data *))
+void __init kona_dt_ccu_setup(struct ccu_data *ccu,
+			struct device_node *node)
 {
-	struct ccu_data *ccu;
 	struct resource res = { 0 };
 	resource_size_t range;
+	unsigned int i;
 	int ret;
 
-	ccu = kzalloc(sizeof(*ccu), GFP_KERNEL);
-	if (ccu)
-		ccu->name = kstrdup(node->name, GFP_KERNEL);
-	if (!ccu || !ccu->name) {
-		pr_err("%s: unable to allocate CCU struct for %s\n",
-			__func__, node->name);
-		kfree(ccu);
+	if (ccu->clk_data.clk_num) {
+		size_t size;
 
-		return;
+		size = ccu->clk_data.clk_num * sizeof(*ccu->clk_data.clks);
+		ccu->clk_data.clks = kzalloc(size, GFP_KERNEL);
+		if (!ccu->clk_data.clks) {
+			pr_err("%s: unable to allocate %u clocks for %s\n",
+				__func__, ccu->clk_data.clk_num, node->name);
+			return;
+		}
 	}
 
 	ret = of_address_to_resource(node, 0, &res);
@@ -735,24 +834,33 @@ void __init kona_dt_ccu_setup(struct device_node *node,
 	}
 
 	ccu->range = (u32)range;
+
+	if (!ccu_data_valid(ccu)) {
+		pr_err("%s: ccu data not valid for %s\n", __func__, node->name);
+		goto out_err;
+	}
+
 	ccu->base = ioremap(res.start, ccu->range);
 	if (!ccu->base) {
 		pr_err("%s: unable to map CCU registers for %s\n", __func__,
 			node->name);
 		goto out_err;
 	}
-
-	spin_lock_init(&ccu->lock);
-	INIT_LIST_HEAD(&ccu->links);
 	ccu->node = of_node_get(node);
-
 	list_add_tail(&ccu->links, &ccu_list);
 
-	/* Set up clocks array (in ccu->data) */
-	if (ccu_clks_setup(ccu))
-		goto out_err;
+	/*
+	 * Set up each defined kona clock and save the result in
+	 * the clock framework clock array (in ccu->data).  Then
+	 * register as a provider for these clocks.
+	 */
+	for (i = 0; i < ccu->clk_data.clk_num; i++) {
+		if (!ccu->kona_clks[i].ccu)
+			continue;
+		ccu->clk_data.clks[i] = kona_clk_setup(&ccu->kona_clks[i]);
+	}
 
-	ret = of_clk_add_provider(node, of_clk_src_onecell_get, &ccu->data);
+	ret = of_clk_add_provider(node, of_clk_src_onecell_get, &ccu->clk_data);
 	if (ret) {
 		pr_err("%s: error adding ccu %s as provider (%d)\n", __func__,
 				node->name, ret);
diff --git a/drivers/clk/bcm/clk-kona.c b/drivers/clk/bcm/clk-kona.c
index e3d339e0830..95af2e665dd 100644
--- a/drivers/clk/bcm/clk-kona.c
+++ b/drivers/clk/bcm/clk-kona.c
@@ -16,6 +16,14 @@
 
 #include <linux/delay.h>
 
+/*
+ * "Policies" affect the frequencies of bus clocks provided by a
+ * CCU.  (I believe these polices are named "Deep Sleep", "Economy",
+ * "Normal", and "Turbo".)  A lower policy number has lower power
+ * consumption, and policy 2 is the default.
+ */
+#define CCU_POLICY_COUNT	4
+
 #define CCU_ACCESS_PASSWORD      0xA5A500
 #define CLK_GATE_DELAY_LOOP      2000
 
@@ -61,7 +69,7 @@ u64 do_div_round_closest(u64 dividend, unsigned long divisor)
 /* Convert a divider into the scaled divisor value it represents. */
 static inline u64 scaled_div_value(struct bcm_clk_div *div, u32 reg_div)
 {
-	return (u64)reg_div + ((u64)1 << div->frac_width);
+	return (u64)reg_div + ((u64)1 << div->u.s.frac_width);
 }
 
 /*
@@ -77,7 +85,7 @@ u64 scaled_div_build(struct bcm_clk_div *div, u32 div_value, u32 billionths)
 	BUG_ON(billionths >= BILLION);
 
 	combined = (u64)div_value * BILLION + billionths;
-	combined <<= div->frac_width;
+	combined <<= div->u.s.frac_width;
 
 	return do_div_round_closest(combined, BILLION);
 }
@@ -87,7 +95,7 @@ static inline u64
 scaled_div_min(struct bcm_clk_div *div)
 {
 	if (divider_is_fixed(div))
-		return (u64)div->fixed;
+		return (u64)div->u.fixed;
 
 	return scaled_div_value(div, 0);
 }
@@ -98,9 +106,9 @@ u64 scaled_div_max(struct bcm_clk_div *div)
 	u32 reg_div;
 
 	if (divider_is_fixed(div))
-		return (u64)div->fixed;
+		return (u64)div->u.fixed;
 
-	reg_div = ((u32)1 << div->width) - 1;
+	reg_div = ((u32)1 << div->u.s.width) - 1;
 
 	return scaled_div_value(div, reg_div);
 }
@@ -115,7 +123,7 @@ divider(struct bcm_clk_div *div, u64 scaled_div)
 	BUG_ON(scaled_div < scaled_div_min(div));
 	BUG_ON(scaled_div > scaled_div_max(div));
 
-	return (u32)(scaled_div - ((u64)1 << div->frac_width));
+	return (u32)(scaled_div - ((u64)1 << div->u.s.frac_width));
 }
 
 /* Return a rate scaled for use when dividing by a scaled divisor. */
@@ -125,7 +133,7 @@ scale_rate(struct bcm_clk_div *div, u32 rate)
 	if (divider_is_fixed(div))
 		return (u64)rate;
 
-	return (u64)rate << div->frac_width;
+	return (u64)rate << div->u.s.frac_width;
 }
 
 /* CCU access */
@@ -207,9 +215,154 @@ __ccu_wait_bit(struct ccu_data *ccu, u32 reg_offset, u32 bit, bool want)
 			return true;
 		udelay(1);
 	}
+	pr_warn("%s: %s/0x%04x bit %u was never %s\n", __func__,
+		ccu->name, reg_offset, bit, want ? "set" : "clear");
+
 	return false;
 }
 
+/* Policy operations */
+
+static bool __ccu_policy_engine_start(struct ccu_data *ccu, bool sync)
+{
+	struct bcm_policy_ctl *control = &ccu->policy.control;
+	u32 offset;
+	u32 go_bit;
+	u32 mask;
+	bool ret;
+
+	/* If we don't need to control policy for this CCU, we're done. */
+	if (!policy_ctl_exists(control))
+		return true;
+
+	offset = control->offset;
+	go_bit = control->go_bit;
+
+	/* Ensure we're not busy before we start */
+	ret = __ccu_wait_bit(ccu, offset, go_bit, false);
+	if (!ret) {
+		pr_err("%s: ccu %s policy engine wouldn't go idle\n",
+			__func__, ccu->name);
+		return false;
+	}
+
+	/*
+	 * If it's a synchronous request, we'll wait for the voltage
+	 * and frequency of the active load to stabilize before
+	 * returning.  To do this we select the active load by
+	 * setting the ATL bit.
+	 *
+	 * An asynchronous request instead ramps the voltage in the
+	 * background, and when that process stabilizes, the target
+	 * load is copied to the active load and the CCU frequency
+	 * is switched.  We do this by selecting the target load
+	 * (ATL bit clear) and setting the request auto-copy (AC bit
+	 * set).
+	 *
+	 * Note, we do NOT read-modify-write this register.
+	 */
+	mask = (u32)1 << go_bit;
+	if (sync)
+		mask |= 1 << control->atl_bit;
+	else
+		mask |= 1 << control->ac_bit;
+	__ccu_write(ccu, offset, mask);
+
+	/* Wait for indication that operation is complete. */
+	ret = __ccu_wait_bit(ccu, offset, go_bit, false);
+	if (!ret)
+		pr_err("%s: ccu %s policy engine never started\n",
+			__func__, ccu->name);
+
+	return ret;
+}
+
+static bool __ccu_policy_engine_stop(struct ccu_data *ccu)
+{
+	struct bcm_lvm_en *enable = &ccu->policy.enable;
+	u32 offset;
+	u32 enable_bit;
+	bool ret;
+
+	/* If we don't need to control policy for this CCU, we're done. */
+	if (!policy_lvm_en_exists(enable))
+		return true;
+
+	/* Ensure we're not busy before we start */
+	offset = enable->offset;
+	enable_bit = enable->bit;
+	ret = __ccu_wait_bit(ccu, offset, enable_bit, false);
+	if (!ret) {
+		pr_err("%s: ccu %s policy engine already stopped\n",
+			__func__, ccu->name);
+		return false;
+	}
+
+	/* Now set the bit to stop the engine (NO read-modify-write) */
+	__ccu_write(ccu, offset, (u32)1 << enable_bit);
+
+	/* Wait for indication that it has stopped. */
+	ret = __ccu_wait_bit(ccu, offset, enable_bit, false);
+	if (!ret)
+		pr_err("%s: ccu %s policy engine never stopped\n",
+			__func__, ccu->name);
+
+	return ret;
+}
+
+/*
+ * A CCU has four operating conditions ("policies"), and some clocks
+ * can be disabled or enabled based on which policy is currently in
+ * effect.  Such clocks have a bit in a "policy mask" register for
+ * each policy indicating whether the clock is enabled for that
+ * policy or not.  The bit position for a clock is the same for all
+ * four registers, and the 32-bit registers are at consecutive
+ * addresses.
+ */
+static bool policy_init(struct ccu_data *ccu, struct bcm_clk_policy *policy)
+{
+	u32 offset;
+	u32 mask;
+	int i;
+	bool ret;
+
+	if (!policy_exists(policy))
+		return true;
+
+	/*
+	 * We need to stop the CCU policy engine to allow update
+	 * of our policy bits.
+	 */
+	if (!__ccu_policy_engine_stop(ccu)) {
+		pr_err("%s: unable to stop CCU %s policy engine\n",
+			__func__, ccu->name);
+		return false;
+	}
+
+	/*
+	 * For now, if a clock defines its policy bit we just mark
+	 * it "enabled" for all four policies.
+	 */
+	offset = policy->offset;
+	mask = (u32)1 << policy->bit;
+	for (i = 0; i < CCU_POLICY_COUNT; i++) {
+		u32 reg_val;
+
+		reg_val = __ccu_read(ccu, offset);
+		reg_val |= mask;
+		__ccu_write(ccu, offset, reg_val);
+		offset += sizeof(u32);
+	}
+
+	/* We're done updating; fire up the policy engine again. */
+	ret = __ccu_policy_engine_start(ccu, true);
+	if (!ret)
+		pr_err("%s: unable to restart CCU %s policy engine\n",
+			__func__, ccu->name);
+
+	return ret;
+}
+
 /* Gate operations */
 
 /* Determine whether a clock is gated.  CCU lock must be held.  */
@@ -374,6 +527,35 @@ static int clk_gate(struct ccu_data *ccu, const char *name,
 	return -EIO;
 }
 
+/* Hysteresis operations */
+
+/*
+ * If a clock gate requires a turn-off delay it will have
+ * "hysteresis" register bits defined.  The first, if set, enables
+ * the delay; and if enabled, the second bit determines whether the
+ * delay is "low" or "high" (1 means high).  For now, if it's
+ * defined for a clock, we set it.
+ */
+static bool hyst_init(struct ccu_data *ccu, struct bcm_clk_hyst *hyst)
+{
+	u32 offset;
+	u32 reg_val;
+	u32 mask;
+
+	if (!hyst_exists(hyst))
+		return true;
+
+	offset = hyst->offset;
+	mask = (u32)1 << hyst->en_bit;
+	mask |= (u32)1 << hyst->val_bit;
+
+	reg_val = __ccu_read(ccu, offset);
+	reg_val |= mask;
+	__ccu_write(ccu, offset, reg_val);
+
+	return true;
+}
+
 /* Trigger operations */
 
 /*
@@ -398,14 +580,14 @@ static u64 divider_read_scaled(struct ccu_data *ccu, struct bcm_clk_div *div)
 	u32 reg_div;
 
 	if (divider_is_fixed(div))
-		return (u64)div->fixed;
+		return (u64)div->u.fixed;
 
 	flags = ccu_lock(ccu);
-	reg_val = __ccu_read(ccu, div->offset);
+	reg_val = __ccu_read(ccu, div->u.s.offset);
 	ccu_unlock(ccu, flags);
 
 	/* Extract the full divider field from the register value */
-	reg_div = bitfield_extract(reg_val, div->shift, div->width);
+	reg_div = bitfield_extract(reg_val, div->u.s.shift, div->u.s.width);
 
 	/* Return the scaled divisor value it represents */
 	return scaled_div_value(div, reg_div);
@@ -433,16 +615,17 @@ static int __div_commit(struct ccu_data *ccu, struct bcm_clk_gate *gate,
 	 * state was defined in the device tree, we just find out
 	 * what its current value is rather than updating it.
 	 */
-	if (div->scaled_div == BAD_SCALED_DIV_VALUE) {
-		reg_val = __ccu_read(ccu, div->offset);
-		reg_div = bitfield_extract(reg_val, div->shift, div->width);
-		div->scaled_div = scaled_div_value(div, reg_div);
+	if (div->u.s.scaled_div == BAD_SCALED_DIV_VALUE) {
+		reg_val = __ccu_read(ccu, div->u.s.offset);
+		reg_div = bitfield_extract(reg_val, div->u.s.shift,
+						div->u.s.width);
+		div->u.s.scaled_div = scaled_div_value(div, reg_div);
 
 		return 0;
 	}
 
 	/* Convert the scaled divisor to the value we need to record */
-	reg_div = divider(div, div->scaled_div);
+	reg_div = divider(div, div->u.s.scaled_div);
 
 	/* Clock needs to be enabled before changing the rate */
 	enabled = __is_clk_gate_enabled(ccu, gate);
@@ -452,9 +635,10 @@ static int __div_commit(struct ccu_data *ccu, struct bcm_clk_gate *gate,
 	}
 
 	/* Replace the divider value and record the result */
-	reg_val = __ccu_read(ccu, div->offset);
-	reg_val = bitfield_replace(reg_val, div->shift, div->width, reg_div);
-	__ccu_write(ccu, div->offset, reg_val);
+	reg_val = __ccu_read(ccu, div->u.s.offset);
+	reg_val = bitfield_replace(reg_val, div->u.s.shift, div->u.s.width,
+					reg_div);
+	__ccu_write(ccu, div->u.s.offset, reg_val);
 
 	/* If the trigger fails we still want to disable the gate */
 	if (!__clk_trigger(ccu, trig))
@@ -490,11 +674,11 @@ static int divider_write(struct ccu_data *ccu, struct bcm_clk_gate *gate,
 
 	BUG_ON(divider_is_fixed(div));
 
-	previous = div->scaled_div;
+	previous = div->u.s.scaled_div;
 	if (previous == scaled_div)
 		return 0;	/* No change */
 
-	div->scaled_div = scaled_div;
+	div->u.s.scaled_div = scaled_div;
 
 	flags = ccu_lock(ccu);
 	__ccu_write_enable(ccu);
@@ -505,7 +689,7 @@ static int divider_write(struct ccu_data *ccu, struct bcm_clk_gate *gate,
 	ccu_unlock(ccu, flags);
 
 	if (ret)
-		div->scaled_div = previous;		/* Revert the change */
+		div->u.s.scaled_div = previous;		/* Revert the change */
 
 	return ret;
 
@@ -802,23 +986,23 @@ static int selector_write(struct ccu_data *ccu, struct bcm_clk_gate *gate,
 static int kona_peri_clk_enable(struct clk_hw *hw)
 {
 	struct kona_clk *bcm_clk = to_kona_clk(hw);
-	struct bcm_clk_gate *gate = &bcm_clk->peri->gate;
+	struct bcm_clk_gate *gate = &bcm_clk->u.peri->gate;
 
-	return clk_gate(bcm_clk->ccu, bcm_clk->name, gate, true);
+	return clk_gate(bcm_clk->ccu, bcm_clk->init_data.name, gate, true);
 }
 
 static void kona_peri_clk_disable(struct clk_hw *hw)
 {
 	struct kona_clk *bcm_clk = to_kona_clk(hw);
-	struct bcm_clk_gate *gate = &bcm_clk->peri->gate;
+	struct bcm_clk_gate *gate = &bcm_clk->u.peri->gate;
 
-	(void)clk_gate(bcm_clk->ccu, bcm_clk->name, gate, false);
+	(void)clk_gate(bcm_clk->ccu, bcm_clk->init_data.name, gate, false);
 }
 
 static int kona_peri_clk_is_enabled(struct clk_hw *hw)
 {
 	struct kona_clk *bcm_clk = to_kona_clk(hw);
-	struct bcm_clk_gate *gate = &bcm_clk->peri->gate;
+	struct bcm_clk_gate *gate = &bcm_clk->u.peri->gate;
 
 	return is_clk_gate_enabled(bcm_clk->ccu, gate) ? 1 : 0;
 }
@@ -827,7 +1011,7 @@ static unsigned long kona_peri_clk_recalc_rate(struct clk_hw *hw,
 			unsigned long parent_rate)
 {
 	struct kona_clk *bcm_clk = to_kona_clk(hw);
-	struct peri_clk_data *data = bcm_clk->peri;
+	struct peri_clk_data *data = bcm_clk->u.peri;
 
 	return clk_recalc_rate(bcm_clk->ccu, &data->div, &data->pre_div,
 				parent_rate);
@@ -837,20 +1021,72 @@ static long kona_peri_clk_round_rate(struct clk_hw *hw, unsigned long rate,
 			unsigned long *parent_rate)
 {
 	struct kona_clk *bcm_clk = to_kona_clk(hw);
-	struct bcm_clk_div *div = &bcm_clk->peri->div;
+	struct bcm_clk_div *div = &bcm_clk->u.peri->div;
 
 	if (!divider_exists(div))
 		return __clk_get_rate(hw->clk);
 
 	/* Quietly avoid a zero rate */
-	return round_rate(bcm_clk->ccu, div, &bcm_clk->peri->pre_div,
+	return round_rate(bcm_clk->ccu, div, &bcm_clk->u.peri->pre_div,
 				rate ? rate : 1, *parent_rate, NULL);
 }
 
+static long kona_peri_clk_determine_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long *best_parent_rate, struct clk **best_parent)
+{
+	struct kona_clk *bcm_clk = to_kona_clk(hw);
+	struct clk *clk = hw->clk;
+	struct clk *current_parent;
+	unsigned long parent_rate;
+	unsigned long best_delta;
+	unsigned long best_rate;
+	u32 parent_count;
+	u32 which;
+
+	/*
+	 * If there is no other parent to choose, use the current one.
+	 * Note:  We don't honor (or use) CLK_SET_RATE_NO_REPARENT.
+	 */
+	WARN_ON_ONCE(bcm_clk->init_data.flags & CLK_SET_RATE_NO_REPARENT);
+	parent_count = (u32)bcm_clk->init_data.num_parents;
+	if (parent_count < 2)
+		return kona_peri_clk_round_rate(hw, rate, best_parent_rate);
+
+	/* Unless we can do better, stick with current parent */
+	current_parent = clk_get_parent(clk);
+	parent_rate = __clk_get_rate(current_parent);
+	best_rate = kona_peri_clk_round_rate(hw, rate, &parent_rate);
+	best_delta = abs(best_rate - rate);
+
+	/* Check whether any other parent clock can produce a better result */
+	for (which = 0; which < parent_count; which++) {
+		struct clk *parent = clk_get_parent_by_index(clk, which);
+		unsigned long delta;
+		unsigned long other_rate;
+
+		BUG_ON(!parent);
+		if (parent == current_parent)
+			continue;
+
+		/* We don't support CLK_SET_RATE_PARENT */
+		parent_rate = __clk_get_rate(parent);
+		other_rate = kona_peri_clk_round_rate(hw, rate, &parent_rate);
+		delta = abs(other_rate - rate);
+		if (delta < best_delta) {
+			best_delta = delta;
+			best_rate = other_rate;
+			*best_parent = parent;
+			*best_parent_rate = parent_rate;
+		}
+	}
+
+	return best_rate;
+}
+
 static int kona_peri_clk_set_parent(struct clk_hw *hw, u8 index)
 {
 	struct kona_clk *bcm_clk = to_kona_clk(hw);
-	struct peri_clk_data *data = bcm_clk->peri;
+	struct peri_clk_data *data = bcm_clk->u.peri;
 	struct bcm_clk_sel *sel = &data->sel;
 	struct bcm_clk_trig *trig;
 	int ret;
@@ -870,12 +1106,13 @@ static int kona_peri_clk_set_parent(struct clk_hw *hw, u8 index)
 
 	ret = selector_write(bcm_clk->ccu, &data->gate, sel, trig, index);
 	if (ret == -ENXIO) {
-		pr_err("%s: gating failure for %s\n", __func__, bcm_clk->name);
+		pr_err("%s: gating failure for %s\n", __func__,
+			bcm_clk->init_data.name);
 		ret = -EIO;	/* Don't proliferate weird errors */
 	} else if (ret == -EIO) {
 		pr_err("%s: %strigger failed for %s\n", __func__,
 			trig == &data->pre_trig ? "pre-" : "",
-			bcm_clk->name);
+			bcm_clk->init_data.name);
 	}
 
 	return ret;
@@ -884,7 +1121,7 @@ static int kona_peri_clk_set_parent(struct clk_hw *hw, u8 index)
 static u8 kona_peri_clk_get_parent(struct clk_hw *hw)
 {
 	struct kona_clk *bcm_clk = to_kona_clk(hw);
-	struct peri_clk_data *data = bcm_clk->peri;
+	struct peri_clk_data *data = bcm_clk->u.peri;
 	u8 index;
 
 	index = selector_read_index(bcm_clk->ccu, &data->sel);
@@ -897,7 +1134,7 @@ static int kona_peri_clk_set_rate(struct clk_hw *hw, unsigned long rate,
 			unsigned long parent_rate)
 {
 	struct kona_clk *bcm_clk = to_kona_clk(hw);
-	struct peri_clk_data *data = bcm_clk->peri;
+	struct peri_clk_data *data = bcm_clk->u.peri;
 	struct bcm_clk_div *div = &data->div;
 	u64 scaled_div = 0;
 	int ret;
@@ -934,10 +1171,12 @@ static int kona_peri_clk_set_rate(struct clk_hw *hw, unsigned long rate,
 	ret = divider_write(bcm_clk->ccu, &data->gate, &data->div,
 				&data->trig, scaled_div);
 	if (ret == -ENXIO) {
-		pr_err("%s: gating failure for %s\n", __func__, bcm_clk->name);
+		pr_err("%s: gating failure for %s\n", __func__,
+			bcm_clk->init_data.name);
 		ret = -EIO;	/* Don't proliferate weird errors */
 	} else if (ret == -EIO) {
-		pr_err("%s: trigger failed for %s\n", __func__, bcm_clk->name);
+		pr_err("%s: trigger failed for %s\n", __func__,
+			bcm_clk->init_data.name);
 	}
 
 	return ret;
@@ -948,7 +1187,7 @@ struct clk_ops kona_peri_clk_ops = {
 	.disable = kona_peri_clk_disable,
 	.is_enabled = kona_peri_clk_is_enabled,
 	.recalc_rate = kona_peri_clk_recalc_rate,
-	.round_rate = kona_peri_clk_round_rate,
+	.determine_rate = kona_peri_clk_determine_rate,
 	.set_parent = kona_peri_clk_set_parent,
 	.get_parent = kona_peri_clk_get_parent,
 	.set_rate = kona_peri_clk_set_rate,
@@ -958,16 +1197,25 @@ struct clk_ops kona_peri_clk_ops = {
 static bool __peri_clk_init(struct kona_clk *bcm_clk)
 {
 	struct ccu_data *ccu = bcm_clk->ccu;
-	struct peri_clk_data *peri = bcm_clk->peri;
-	const char *name = bcm_clk->name;
+	struct peri_clk_data *peri = bcm_clk->u.peri;
+	const char *name = bcm_clk->init_data.name;
 	struct bcm_clk_trig *trig;
 
 	BUG_ON(bcm_clk->type != bcm_clk_peri);
 
+	if (!policy_init(ccu, &peri->policy)) {
+		pr_err("%s: error initializing policy for %s\n",
+			__func__, name);
+		return false;
+	}
 	if (!gate_init(ccu, &peri->gate)) {
 		pr_err("%s: error initializing gate for %s\n", __func__, name);
 		return false;
 	}
+	if (!hyst_init(ccu, &peri->hyst)) {
+		pr_err("%s: error initializing hyst for %s\n", __func__, name);
+		return false;
+	}
 	if (!div_init(ccu, &peri->gate, &peri->div, &peri->trig)) {
 		pr_err("%s: error initializing divider for %s\n", __func__,
 			name);
@@ -1012,13 +1260,13 @@ bool __init kona_ccu_init(struct ccu_data *ccu)
 {
 	unsigned long flags;
 	unsigned int which;
-	struct clk **clks = ccu->data.clks;
+	struct clk **clks = ccu->clk_data.clks;
 	bool success = true;
 
 	flags = ccu_lock(ccu);
 	__ccu_write_enable(ccu);
 
-	for (which = 0; which < ccu->data.clk_num; which++) {
+	for (which = 0; which < ccu->clk_data.clk_num; which++) {
 		struct kona_clk *bcm_clk;
 
 		if (!clks[which])
diff --git a/drivers/clk/bcm/clk-kona.h b/drivers/clk/bcm/clk-kona.h
index 5e139adc3dc..2537b307291 100644
--- a/drivers/clk/bcm/clk-kona.h
+++ b/drivers/clk/bcm/clk-kona.h
@@ -43,8 +43,14 @@
 #define FLAG_FLIP(obj, type, flag)	((obj)->flags ^= FLAG(type, flag))
 #define FLAG_TEST(obj, type, flag)	(!!((obj)->flags & FLAG(type, flag)))
 
+/* CCU field state tests */
+
+#define ccu_policy_exists(ccu_policy)	((ccu_policy)->enable.offset != 0)
+
 /* Clock field state tests */
 
+#define policy_exists(policy)		((policy)->offset != 0)
+
 #define gate_exists(gate)		FLAG_TEST(gate, GATE, EXISTS)
 #define gate_is_enabled(gate)		FLAG_TEST(gate, GATE, ENABLED)
 #define gate_is_hw_controllable(gate)	FLAG_TEST(gate, GATE, HW)
@@ -54,14 +60,19 @@
 
 #define gate_flip_enabled(gate)		FLAG_FLIP(gate, GATE, ENABLED)
 
+#define hyst_exists(hyst)		((hyst)->offset != 0)
+
 #define divider_exists(div)		FLAG_TEST(div, DIV, EXISTS)
 #define divider_is_fixed(div)		FLAG_TEST(div, DIV, FIXED)
 #define divider_has_fraction(div)	(!divider_is_fixed(div) && \
-						(div)->frac_width > 0)
+						(div)->u.s.frac_width > 0)
 
 #define selector_exists(sel)		((sel)->width != 0)
 #define trigger_exists(trig)		FLAG_TEST(trig, TRIG, EXISTS)
 
+#define policy_lvm_en_exists(enable)	((enable)->offset != 0)
+#define policy_ctl_exists(control)	((control)->offset != 0)
+
 /* Clock type, used to tell common block what it's part of */
 enum bcm_clk_type {
 	bcm_clk_none,		/* undefined clock type */
@@ -71,25 +82,26 @@ enum bcm_clk_type {
 };
 
 /*
- * Each CCU defines a mapped area of memory containing registers
- * used to manage clocks implemented by the CCU.  Access to memory
- * within the CCU's space is serialized by a spinlock.  Before any
- * (other) address can be written, a special access "password" value
- * must be written to its WR_ACCESS register (located at the base
- * address of the range).  We keep track of the name of each CCU as
- * it is set up, and maintain them in a list.
+ * CCU policy control for clocks.  Clocks can be enabled or disabled
+ * based on the CCU policy in effect.  One bit in each policy mask
+ * register (one per CCU policy) represents whether the clock is
+ * enabled when that policy is effect or not.  The CCU policy engine
+ * must be stopped to update these bits, and must be restarted again
+ * afterward.
  */
-struct ccu_data {
-	void __iomem *base;	/* base of mapped address space */
-	spinlock_t lock;	/* serialization lock */
-	bool write_enabled;	/* write access is currently enabled */
-	struct list_head links;	/* for ccu_list */
-	struct device_node *node;
-	struct clk_onecell_data data;
-	const char *name;
-	u32 range;		/* byte range of address space */
+struct bcm_clk_policy {
+	u32 offset;		/* first policy mask register offset */
+	u32 bit;		/* bit used in all mask registers */
 };
 
+/* Policy initialization macro */
+
+#define POLICY(_offset, _bit)						\
+	{								\
+		.offset = (_offset),					\
+		.bit = (_bit),						\
+	}
+
 /*
  * Gating control and status is managed by a 32-bit gate register.
  *
@@ -195,6 +207,22 @@ struct bcm_clk_gate {
 		.flags = FLAG(GATE, HW)|FLAG(GATE, EXISTS),		\
 	}
 
+/* Gate hysteresis for clocks */
+struct bcm_clk_hyst {
+	u32 offset;		/* hyst register offset (normally CLKGATE) */
+	u32 en_bit;		/* bit used to enable hysteresis */
+	u32 val_bit;		/* if enabled: 0 = low delay; 1 = high delay */
+};
+
+/* Hysteresis initialization macro */
+
+#define HYST(_offset, _en_bit, _val_bit)				\
+	{								\
+		.offset = (_offset),					\
+		.en_bit = (_en_bit),					\
+		.val_bit = (_val_bit),					\
+	}
+
 /*
  * Each clock can have zero, one, or two dividers which change the
  * output rate of the clock.  Each divider can be either fixed or
@@ -244,9 +272,9 @@ struct bcm_clk_div {
 			u32 frac_width;	/* field fraction width */
 
 			u64 scaled_div;	/* scaled divider value */
-		};
+		} s;
 		u32 fixed;	/* non-zero fixed divider value */
-	};
+	} u;
 	u32 flags;		/* BCM_CLK_DIV_FLAGS_* below */
 };
 
@@ -263,28 +291,28 @@ struct bcm_clk_div {
 /* A fixed (non-zero) divider */
 #define FIXED_DIVIDER(_value)						\
 	{								\
-		.fixed = (_value),					\
+		.u.fixed = (_value),					\
 		.flags = FLAG(DIV, EXISTS)|FLAG(DIV, FIXED),		\
 	}
 
 /* A divider with an integral divisor */
 #define DIVIDER(_offset, _shift, _width)				\
 	{								\
-		.offset = (_offset),					\
-		.shift = (_shift),					\
-		.width = (_width),					\
-		.scaled_div = BAD_SCALED_DIV_VALUE,			\
+		.u.s.offset = (_offset),				\
+		.u.s.shift = (_shift),					\
+		.u.s.width = (_width),					\
+		.u.s.scaled_div = BAD_SCALED_DIV_VALUE,			\
 		.flags = FLAG(DIV, EXISTS),				\
 	}
 
 /* A divider whose divisor has an integer and fractional part */
 #define FRAC_DIVIDER(_offset, _shift, _width, _frac_width)		\
 	{								\
-		.offset = (_offset),					\
-		.shift = (_shift),					\
-		.width = (_width),					\
-		.frac_width = (_frac_width),				\
-		.scaled_div = BAD_SCALED_DIV_VALUE,			\
+		.u.s.offset = (_offset),				\
+		.u.s.shift = (_shift),					\
+		.u.s.width = (_width),					\
+		.u.s.frac_width = (_frac_width),			\
+		.u.s.scaled_div = BAD_SCALED_DIV_VALUE,			\
 		.flags = FLAG(DIV, EXISTS),				\
 	}
 
@@ -360,7 +388,9 @@ struct bcm_clk_trig {
 	}
 
 struct peri_clk_data {
+	struct bcm_clk_policy policy;
 	struct bcm_clk_gate gate;
+	struct bcm_clk_hyst hyst;
 	struct bcm_clk_trig pre_trig;
 	struct bcm_clk_div pre_div;
 	struct bcm_clk_trig trig;
@@ -373,26 +403,103 @@ struct peri_clk_data {
 
 struct kona_clk {
 	struct clk_hw hw;
-	struct clk_init_data init_data;
-	const char *name;	/* name of this clock */
+	struct clk_init_data init_data;	/* includes name of this clock */
 	struct ccu_data *ccu;	/* ccu this clock is associated with */
 	enum bcm_clk_type type;
 	union {
 		void *data;
 		struct peri_clk_data *peri;
-	};
+	} u;
 };
 #define to_kona_clk(_hw) \
 	container_of(_hw, struct kona_clk, hw)
 
-/* Exported globals */
+/* Initialization macro for an entry in a CCU's kona_clks[] array. */
+#define KONA_CLK(_ccu_name, _clk_name, _type)				\
+	{								\
+		.init_data	= {					\
+			.name = #_clk_name,				\
+			.ops = &kona_ ## _type ## _clk_ops,		\
+		},							\
+		.ccu		= &_ccu_name ## _ccu_data,		\
+		.type		= bcm_clk_ ## _type,			\
+		.u.data		= &_clk_name ## _data,			\
+	}
+#define LAST_KONA_CLK	{ .type = bcm_clk_none }
 
-extern struct clk_ops kona_peri_clk_ops;
+/*
+ * CCU policy control.  To enable software update of the policy
+ * tables the CCU policy engine must be stopped by setting the
+ * software update enable bit (LVM_EN).  After an update the engine
+ * is restarted using the GO bit and either the GO_ATL or GO_AC bit.
+ */
+struct bcm_lvm_en {
+	u32 offset;		/* LVM_EN register offset */
+	u32 bit;		/* POLICY_CONFIG_EN bit in register */
+};
 
-/* Help functions */
+/* Policy enable initialization macro */
+#define CCU_LVM_EN(_offset, _bit)					\
+	{								\
+		.offset = (_offset),					\
+		.bit = (_bit),						\
+	}
+
+struct bcm_policy_ctl {
+	u32 offset;		/* POLICY_CTL register offset */
+	u32 go_bit;
+	u32 atl_bit;		/* GO, GO_ATL, and GO_AC bits */
+	u32 ac_bit;
+};
 
-#define PERI_CLK_SETUP(clks, ccu, id, name) \
-	clks[id] = kona_clk_setup(ccu, #name, bcm_clk_peri, &name ## _data)
+/* Policy control initialization macro */
+#define CCU_POLICY_CTL(_offset, _go_bit, _ac_bit, _atl_bit)		\
+	{								\
+		.offset = (_offset),					\
+		.go_bit = (_go_bit),					\
+		.ac_bit = (_ac_bit),					\
+		.atl_bit = (_atl_bit),					\
+	}
+
+struct ccu_policy {
+	struct bcm_lvm_en enable;
+	struct bcm_policy_ctl control;
+};
+
+/*
+ * Each CCU defines a mapped area of memory containing registers
+ * used to manage clocks implemented by the CCU.  Access to memory
+ * within the CCU's space is serialized by a spinlock.  Before any
+ * (other) address can be written, a special access "password" value
+ * must be written to its WR_ACCESS register (located at the base
+ * address of the range).  We keep track of the name of each CCU as
+ * it is set up, and maintain them in a list.
+ */
+struct ccu_data {
+	void __iomem *base;	/* base of mapped address space */
+	spinlock_t lock;	/* serialization lock */
+	bool write_enabled;	/* write access is currently enabled */
+	struct ccu_policy policy;
+	struct list_head links;	/* for ccu_list */
+	struct device_node *node;
+	struct clk_onecell_data clk_data;
+	const char *name;
+	u32 range;		/* byte range of address space */
+	struct kona_clk kona_clks[];	/* must be last */
+};
+
+/* Initialization for common fields in a Kona ccu_data structure */
+#define KONA_CCU_COMMON(_prefix, _name, _ccuname)			    \
+	.name		= #_name "_ccu",				    \
+	.lock		= __SPIN_LOCK_UNLOCKED(_name ## _ccu_data.lock),    \
+	.links		= LIST_HEAD_INIT(_name ## _ccu_data.links),	    \
+	.clk_data	= {						    \
+		.clk_num = _prefix ## _ ## _ccuname ## _CCU_CLOCK_COUNT,    \
+	}
+
+/* Exported globals */
+
+extern struct clk_ops kona_peri_clk_ops;
 
 /* Externally visible functions */
 
@@ -401,10 +508,9 @@ extern u64 scaled_div_max(struct bcm_clk_div *div);
 extern u64 scaled_div_build(struct bcm_clk_div *div, u32 div_value,
 				u32 billionths);
 
-extern struct clk *kona_clk_setup(struct ccu_data *ccu, const char *name,
-			enum bcm_clk_type type, void *data);
-extern void __init kona_dt_ccu_setup(struct device_node *node,
-			int (*ccu_clks_setup)(struct ccu_data *));
+extern struct clk *kona_clk_setup(struct kona_clk *bcm_clk);
+extern void __init kona_dt_ccu_setup(struct ccu_data *ccu,
+				struct device_node *node);
 extern bool __init kona_ccu_init(struct ccu_data *ccu);
 
 #endif /* _CLK_KONA_H */
diff --git a/drivers/clk/berlin/Makefile b/drivers/clk/berlin/Makefile
new file mode 100644
index 00000000000..2a36ab710a0
--- /dev/null
+++ b/drivers/clk/berlin/Makefile
@@ -0,0 +1,4 @@
+obj-y += berlin2-avpll.o berlin2-pll.o berlin2-div.o
+obj-$(CONFIG_MACH_BERLIN_BG2)	+= bg2.o
+obj-$(CONFIG_MACH_BERLIN_BG2CD)	+= bg2.o
+obj-$(CONFIG_MACH_BERLIN_BG2Q)	+= bg2q.o
diff --git a/drivers/clk/berlin/berlin2-avpll.c b/drivers/clk/berlin/berlin2-avpll.c
new file mode 100644
index 00000000000..fd0f26c3846
--- /dev/null
+++ b/drivers/clk/berlin/berlin2-avpll.c
@@ -0,0 +1,393 @@
+/*
+ * Copyright (c) 2014 Marvell Technology Group Ltd.
+ *
+ * Sebastian Hesselbarth <sebastian.hesselbarth@gmail.com>
+ * Alexandre Belloni <alexandre.belloni@free-electrons.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <linux/clk-provider.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/slab.h>
+
+#include "berlin2-avpll.h"
+
+/*
+ * Berlin2 SoCs comprise up to two PLLs called AVPLL built upon a
+ * VCO with 8 channels each, channel 8 is the odd-one-out and does
+ * not provide mul/div.
+ *
+ * Unfortunately, its registers are not named but just numbered. To
+ * get in at least some kind of structure, we split each AVPLL into
+ * the VCOs and each channel into separate clock drivers.
+ *
+ * Also, here and there the VCO registers are a bit different with
+ * respect to bit shifts. Make sure to add a comment for those.
+ */
+#define NUM_CHANNELS	8
+
+#define AVPLL_CTRL(x)		((x) * 0x4)
+
+#define VCO_CTRL0		AVPLL_CTRL(0)
+/* BG2/BG2CDs VCO_B has an additional shift of 4 for its VCO_CTRL0 reg */
+#define  VCO_RESET		BIT(0)
+#define  VCO_POWERUP		BIT(1)
+#define  VCO_INTERPOL_SHIFT	2
+#define  VCO_INTERPOL_MASK	(0xf << VCO_INTERPOL_SHIFT)
+#define  VCO_REG1V45_SEL_SHIFT	6
+#define  VCO_REG1V45_SEL(x)	((x) << VCO_REG1V45_SEL_SHIFT)
+#define  VCO_REG1V45_SEL_1V40	VCO_REG1V45_SEL(0)
+#define  VCO_REG1V45_SEL_1V45	VCO_REG1V45_SEL(1)
+#define  VCO_REG1V45_SEL_1V50	VCO_REG1V45_SEL(2)
+#define  VCO_REG1V45_SEL_1V55	VCO_REG1V45_SEL(3)
+#define  VCO_REG1V45_SEL_MASK	VCO_REG1V45_SEL(3)
+#define  VCO_REG0V9_SEL_SHIFT	8
+#define  VCO_REG0V9_SEL_MASK	(0xf << VCO_REG0V9_SEL_SHIFT)
+#define  VCO_VTHCAL_SHIFT	12
+#define  VCO_VTHCAL(x)		((x) << VCO_VTHCAL_SHIFT)
+#define  VCO_VTHCAL_0V90	VCO_VTHCAL(0)
+#define  VCO_VTHCAL_0V95	VCO_VTHCAL(1)
+#define  VCO_VTHCAL_1V00	VCO_VTHCAL(2)
+#define  VCO_VTHCAL_1V05	VCO_VTHCAL(3)
+#define  VCO_VTHCAL_MASK	VCO_VTHCAL(3)
+#define  VCO_KVCOEXT_SHIFT	14
+#define  VCO_KVCOEXT_MASK	(0x3 << VCO_KVCOEXT_SHIFT)
+#define  VCO_KVCOEXT_ENABLE	BIT(17)
+#define  VCO_V2IEXT_SHIFT	18
+#define  VCO_V2IEXT_MASK	(0xf << VCO_V2IEXT_SHIFT)
+#define  VCO_V2IEXT_ENABLE	BIT(22)
+#define  VCO_SPEED_SHIFT	23
+#define  VCO_SPEED(x)		((x) << VCO_SPEED_SHIFT)
+#define  VCO_SPEED_1G08_1G21	VCO_SPEED(0)
+#define  VCO_SPEED_1G21_1G40	VCO_SPEED(1)
+#define  VCO_SPEED_1G40_1G61	VCO_SPEED(2)
+#define  VCO_SPEED_1G61_1G86	VCO_SPEED(3)
+#define  VCO_SPEED_1G86_2G00	VCO_SPEED(4)
+#define  VCO_SPEED_2G00_2G22	VCO_SPEED(5)
+#define  VCO_SPEED_2G22		VCO_SPEED(6)
+#define  VCO_SPEED_MASK		VCO_SPEED(0x7)
+#define  VCO_CLKDET_ENABLE	BIT(26)
+#define VCO_CTRL1		AVPLL_CTRL(1)
+#define  VCO_REFDIV_SHIFT	0
+#define  VCO_REFDIV(x)		((x) << VCO_REFDIV_SHIFT)
+#define  VCO_REFDIV_1		VCO_REFDIV(0)
+#define  VCO_REFDIV_2		VCO_REFDIV(1)
+#define  VCO_REFDIV_4		VCO_REFDIV(2)
+#define  VCO_REFDIV_3		VCO_REFDIV(3)
+#define  VCO_REFDIV_MASK	VCO_REFDIV(0x3f)
+#define  VCO_FBDIV_SHIFT	6
+#define  VCO_FBDIV(x)		((x) << VCO_FBDIV_SHIFT)
+#define  VCO_FBDIV_MASK		VCO_FBDIV(0xff)
+#define  VCO_ICP_SHIFT		14
+/* PLL Charge Pump Current = 10uA * (x + 1) */
+#define  VCO_ICP(x)		((x) << VCO_ICP_SHIFT)
+#define  VCO_ICP_MASK		VCO_ICP(0xf)
+#define  VCO_LOAD_CAP		BIT(18)
+#define  VCO_CALIBRATION_START	BIT(19)
+#define VCO_FREQOFFSETn(x)	AVPLL_CTRL(3 + (x))
+#define  VCO_FREQOFFSET_MASK	0x7ffff
+#define VCO_CTRL10		AVPLL_CTRL(10)
+#define  VCO_POWERUP_CH1	BIT(20)
+#define VCO_CTRL11		AVPLL_CTRL(11)
+#define VCO_CTRL12		AVPLL_CTRL(12)
+#define VCO_CTRL13		AVPLL_CTRL(13)
+#define VCO_CTRL14		AVPLL_CTRL(14)
+#define VCO_CTRL15		AVPLL_CTRL(15)
+#define VCO_SYNC1n(x)		AVPLL_CTRL(15 + (x))
+#define  VCO_SYNC1_MASK		0x1ffff
+#define VCO_SYNC2n(x)		AVPLL_CTRL(23 + (x))
+#define  VCO_SYNC2_MASK		0x1ffff
+#define VCO_CTRL30		AVPLL_CTRL(30)
+#define  VCO_DPLL_CH1_ENABLE	BIT(17)
+
+struct berlin2_avpll_vco {
+	struct clk_hw hw;
+	void __iomem *base;
+	u8 flags;
+};
+
+#define to_avpll_vco(hw) container_of(hw, struct berlin2_avpll_vco, hw)
+
+static int berlin2_avpll_vco_is_enabled(struct clk_hw *hw)
+{
+	struct berlin2_avpll_vco *vco = to_avpll_vco(hw);
+	u32 reg;
+
+	reg = readl_relaxed(vco->base + VCO_CTRL0);
+	if (vco->flags & BERLIN2_AVPLL_BIT_QUIRK)
+		reg >>= 4;
+
+	return !!(reg & VCO_POWERUP);
+}
+
+static int berlin2_avpll_vco_enable(struct clk_hw *hw)
+{
+	struct berlin2_avpll_vco *vco = to_avpll_vco(hw);
+	u32 reg;
+
+	reg = readl_relaxed(vco->base + VCO_CTRL0);
+	if (vco->flags & BERLIN2_AVPLL_BIT_QUIRK)
+		reg |= VCO_POWERUP << 4;
+	else
+		reg |= VCO_POWERUP;
+	writel_relaxed(reg, vco->base + VCO_CTRL0);
+
+	return 0;
+}
+
+static void berlin2_avpll_vco_disable(struct clk_hw *hw)
+{
+	struct berlin2_avpll_vco *vco = to_avpll_vco(hw);
+	u32 reg;
+
+	reg = readl_relaxed(vco->base + VCO_CTRL0);
+	if (vco->flags & BERLIN2_AVPLL_BIT_QUIRK)
+		reg &= ~(VCO_POWERUP << 4);
+	else
+		reg &= ~VCO_POWERUP;
+	writel_relaxed(reg, vco->base + VCO_CTRL0);
+}
+
+static u8 vco_refdiv[] = { 1, 2, 4, 3 };
+
+static unsigned long
+berlin2_avpll_vco_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
+{
+	struct berlin2_avpll_vco *vco = to_avpll_vco(hw);
+	u32 reg, refdiv, fbdiv;
+	u64 freq = parent_rate;
+
+	/* AVPLL VCO frequency: Fvco = (Fref / refdiv) * fbdiv */
+	reg = readl_relaxed(vco->base + VCO_CTRL1);
+	refdiv = (reg & VCO_REFDIV_MASK) >> VCO_REFDIV_SHIFT;
+	refdiv = vco_refdiv[refdiv];
+	fbdiv = (reg & VCO_FBDIV_MASK) >> VCO_FBDIV_SHIFT;
+	freq *= fbdiv;
+	do_div(freq, refdiv);
+
+	return (unsigned long)freq;
+}
+
+static const struct clk_ops berlin2_avpll_vco_ops = {
+	.is_enabled	= berlin2_avpll_vco_is_enabled,
+	.enable		= berlin2_avpll_vco_enable,
+	.disable	= berlin2_avpll_vco_disable,
+	.recalc_rate	= berlin2_avpll_vco_recalc_rate,
+};
+
+struct clk * __init berlin2_avpll_vco_register(void __iomem *base,
+			       const char *name, const char *parent_name,
+			       u8 vco_flags, unsigned long flags)
+{
+	struct berlin2_avpll_vco *vco;
+	struct clk_init_data init;
+
+	vco = kzalloc(sizeof(*vco), GFP_KERNEL);
+	if (!vco)
+		return ERR_PTR(-ENOMEM);
+
+	vco->base = base;
+	vco->flags = vco_flags;
+	vco->hw.init = &init;
+	init.name = name;
+	init.ops = &berlin2_avpll_vco_ops;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+	init.flags = flags;
+
+	return clk_register(NULL, &vco->hw);
+}
+
+struct berlin2_avpll_channel {
+	struct clk_hw hw;
+	void __iomem *base;
+	u8 flags;
+	u8 index;
+};
+
+#define to_avpll_channel(hw) container_of(hw, struct berlin2_avpll_channel, hw)
+
+static int berlin2_avpll_channel_is_enabled(struct clk_hw *hw)
+{
+	struct berlin2_avpll_channel *ch = to_avpll_channel(hw);
+	u32 reg;
+
+	if (ch->index == 7)
+		return 1;
+
+	reg = readl_relaxed(ch->base + VCO_CTRL10);
+	reg &= VCO_POWERUP_CH1 << ch->index;
+
+	return !!reg;
+}
+
+static int berlin2_avpll_channel_enable(struct clk_hw *hw)
+{
+	struct berlin2_avpll_channel *ch = to_avpll_channel(hw);
+	u32 reg;
+
+	reg = readl_relaxed(ch->base + VCO_CTRL10);
+	reg |= VCO_POWERUP_CH1 << ch->index;
+	writel_relaxed(reg, ch->base + VCO_CTRL10);
+
+	return 0;
+}
+
+static void berlin2_avpll_channel_disable(struct clk_hw *hw)
+{
+	struct berlin2_avpll_channel *ch = to_avpll_channel(hw);
+	u32 reg;
+
+	reg = readl_relaxed(ch->base + VCO_CTRL10);
+	reg &= ~(VCO_POWERUP_CH1 << ch->index);
+	writel_relaxed(reg, ch->base + VCO_CTRL10);
+}
+
+static const u8 div_hdmi[] = { 1, 2, 4, 6 };
+static const u8 div_av1[] = { 1, 2, 5, 5 };
+
+static unsigned long
+berlin2_avpll_channel_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
+{
+	struct berlin2_avpll_channel *ch = to_avpll_channel(hw);
+	u32 reg, div_av2, div_av3, divider = 1;
+	u64 freq = parent_rate;
+
+	reg = readl_relaxed(ch->base + VCO_CTRL30);
+	if ((reg & (VCO_DPLL_CH1_ENABLE << ch->index)) == 0)
+		goto skip_div;
+
+	/*
+	 * Fch = (Fref * sync2) /
+	 *    (sync1 * div_hdmi * div_av1 * div_av2 * div_av3)
+	 */
+
+	reg = readl_relaxed(ch->base + VCO_SYNC1n(ch->index));
+	/* BG2/BG2CDs SYNC1 reg on AVPLL_B channel 1 is shifted by 4 */
+	if (ch->flags & BERLIN2_AVPLL_BIT_QUIRK && ch->index == 0)
+		reg >>= 4;
+	divider = reg & VCO_SYNC1_MASK;
+
+	reg = readl_relaxed(ch->base + VCO_SYNC2n(ch->index));
+	freq *= reg & VCO_SYNC2_MASK;
+
+	/* Channel 8 has no dividers */
+	if (ch->index == 7)
+		goto skip_div;
+
+	/*
+	 * HDMI divider start at VCO_CTRL11, bit 7; MSB is enable, lower 2 bit
+	 * determine divider.
+	 */
+	reg = readl_relaxed(ch->base + VCO_CTRL11) >> 7;
+	reg = (reg >> (ch->index * 3));
+	if (reg & BIT(2))
+		divider *= div_hdmi[reg & 0x3];
+
+	/*
+	 * AV1 divider start at VCO_CTRL11, bit 28; MSB is enable, lower 2 bit
+	 * determine divider.
+	 */
+	if (ch->index == 0) {
+		reg = readl_relaxed(ch->base + VCO_CTRL11);
+		reg >>= 28;
+	} else {
+		reg = readl_relaxed(ch->base + VCO_CTRL12);
+		reg >>= (ch->index-1) * 3;
+	}
+	if (reg & BIT(2))
+		divider *= div_av1[reg & 0x3];
+
+	/*
+	 * AV2 divider start at VCO_CTRL12, bit 18; each 7 bits wide,
+	 * zero is not a valid value.
+	 */
+	if (ch->index < 2) {
+		reg = readl_relaxed(ch->base + VCO_CTRL12);
+		reg >>= 18 + (ch->index * 7);
+	} else if (ch->index < 7) {
+		reg = readl_relaxed(ch->base + VCO_CTRL13);
+		reg >>= (ch->index - 2) * 7;
+	} else {
+		reg = readl_relaxed(ch->base + VCO_CTRL14);
+	}
+	div_av2 = reg & 0x7f;
+	if (div_av2)
+		divider *= div_av2;
+
+	/*
+	 * AV3 divider start at VCO_CTRL14, bit 7; each 4 bits wide.
+	 * AV2/AV3 form a fractional divider, where only specfic values for AV3
+	 * are allowed. AV3 != 0 divides by AV2/2, AV3=0 is bypass.
+	 */
+	if (ch->index < 6) {
+		reg = readl_relaxed(ch->base + VCO_CTRL14);
+		reg >>= 7 + (ch->index * 4);
+	} else {
+		reg = readl_relaxed(ch->base + VCO_CTRL15);
+	}
+	div_av3 = reg & 0xf;
+	if (div_av2 && div_av3)
+		freq *= 2;
+
+skip_div:
+	do_div(freq, divider);
+	return (unsigned long)freq;
+}
+
+static const struct clk_ops berlin2_avpll_channel_ops = {
+	.is_enabled	= berlin2_avpll_channel_is_enabled,
+	.enable		= berlin2_avpll_channel_enable,
+	.disable	= berlin2_avpll_channel_disable,
+	.recalc_rate	= berlin2_avpll_channel_recalc_rate,
+};
+
+/*
+ * Another nice quirk:
+ * On some production SoCs, AVPLL channels are scrambled with respect
+ * to the channel numbering in the registers but still referenced by
+ * their original channel numbers. We deal with it by having a flag
+ * and a translation table for the index.
+ */
+static const u8 quirk_index[] __initconst = { 0, 6, 5, 4, 3, 2, 1, 7 };
+
+struct clk * __init berlin2_avpll_channel_register(void __iomem *base,
+			   const char *name, u8 index, const char *parent_name,
+			   u8 ch_flags, unsigned long flags)
+{
+	struct berlin2_avpll_channel *ch;
+	struct clk_init_data init;
+
+	ch = kzalloc(sizeof(*ch), GFP_KERNEL);
+	if (!ch)
+		return ERR_PTR(-ENOMEM);
+
+	ch->base = base;
+	if (ch_flags & BERLIN2_AVPLL_SCRAMBLE_QUIRK)
+		ch->index = quirk_index[index];
+	else
+		ch->index = index;
+
+	ch->flags = ch_flags;
+	ch->hw.init = &init;
+	init.name = name;
+	init.ops = &berlin2_avpll_channel_ops;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+	init.flags = flags;
+
+	return clk_register(NULL, &ch->hw);
+}
diff --git a/drivers/clk/berlin/berlin2-avpll.h b/drivers/clk/berlin/berlin2-avpll.h
new file mode 100644
index 00000000000..a37f5068d29
--- /dev/null
+++ b/drivers/clk/berlin/berlin2-avpll.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (c) 2014 Marvell Technology Group Ltd.
+ *
+ * Sebastian Hesselbarth <sebastian.hesselbarth@gmail.com>
+ * Alexandre Belloni <alexandre.belloni@free-electrons.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#ifndef __BERLIN2_AVPLL_H
+#define __BERLIN2_AVPLL_H
+
+struct clk;
+
+#define BERLIN2_AVPLL_BIT_QUIRK		BIT(0)
+#define BERLIN2_AVPLL_SCRAMBLE_QUIRK	BIT(1)
+
+struct clk * __init
+berlin2_avpll_vco_register(void __iomem *base, const char *name,
+	   const char *parent_name, u8 vco_flags, unsigned long flags);
+
+struct clk * __init
+berlin2_avpll_channel_register(void __iomem *base, const char *name,
+		       u8 index, const char *parent_name, u8 ch_flags,
+		       unsigned long flags);
+
+#endif /* __BERLIN2_AVPLL_H */
diff --git a/drivers/clk/berlin/berlin2-div.c b/drivers/clk/berlin/berlin2-div.c
new file mode 100644
index 00000000000..81ff97f8aa0
--- /dev/null
+++ b/drivers/clk/berlin/berlin2-div.c
@@ -0,0 +1,265 @@
+/*
+ * Copyright (c) 2014 Marvell Technology Group Ltd.
+ *
+ * Alexandre Belloni <alexandre.belloni@free-electrons.com>
+ * Sebastian Hesselbarth <sebastian.hesselbarth@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <linux/bitops.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+#include "berlin2-div.h"
+
+/*
+ * Clock dividers in Berlin2 SoCs comprise a complex cell to select
+ * input pll and divider. The virtual structure as it is used in Marvell
+ * BSP code can be seen as:
+ *
+ *                      +---+
+ * pll0 --------------->| 0 |                   +---+
+ *           +---+      |(B)|--+--------------->| 0 |      +---+
+ * pll1.0 -->| 0 |  +-->| 1 |  |   +--------+   |(E)|----->| 0 |   +---+
+ * pll1.1 -->| 1 |  |   +---+  +-->|(C) 1:M |-->| 1 |      |(F)|-->|(G)|->
+ * ...    -->|(A)|--+          |   +--------+   +---+  +-->| 1 |   +---+
+ * ...    -->|   |             +-->|(D) 1:3 |----------+   +---+
+ * pll1.N -->| N |                 +---------
+ *           +---+
+ *
+ * (A) input pll clock mux controlled by               <PllSelect[1:n]>
+ * (B) input pll bypass mux controlled by              <PllSwitch>
+ * (C) programmable clock divider controlled by        <Select[1:n]>
+ * (D) constant div-by-3 clock divider
+ * (E) programmable clock divider bypass controlled by <Switch>
+ * (F) constant div-by-3 clock mux controlled by       <D3Switch>
+ * (G) clock gate controlled by                        <Enable>
+ *
+ * For whatever reason, above control signals come in two flavors:
+ * - single register dividers with all bits in one register
+ * - shared register dividers with bits spread over multiple registers
+ *   (including signals for the same cell spread over consecutive registers)
+ *
+ * Also, clock gate and pll mux is not available on every div cell, so
+ * we have to deal with those, too. We reuse common clock composite driver
+ * for it.
+ */
+
+#define PLL_SELECT_MASK	0x7
+#define DIV_SELECT_MASK	0x7
+
+struct berlin2_div {
+	struct clk_hw hw;
+	void __iomem *base;
+	struct berlin2_div_map map;
+	spinlock_t *lock;
+};
+
+#define to_berlin2_div(hw) container_of(hw, struct berlin2_div, hw)
+
+static u8 clk_div[] = { 1, 2, 4, 6, 8, 12, 1, 1 };
+
+static int berlin2_div_is_enabled(struct clk_hw *hw)
+{
+	struct berlin2_div *div = to_berlin2_div(hw);
+	struct berlin2_div_map *map = &div->map;
+	u32 reg;
+
+	if (div->lock)
+		spin_lock(div->lock);
+
+	reg = readl_relaxed(div->base + map->gate_offs);
+	reg >>= map->gate_shift;
+
+	if (div->lock)
+		spin_unlock(div->lock);
+
+	return (reg & 0x1);
+}
+
+static int berlin2_div_enable(struct clk_hw *hw)
+{
+	struct berlin2_div *div = to_berlin2_div(hw);
+	struct berlin2_div_map *map = &div->map;
+	u32 reg;
+
+	if (div->lock)
+		spin_lock(div->lock);
+
+	reg = readl_relaxed(div->base + map->gate_offs);
+	reg |= BIT(map->gate_shift);
+	writel_relaxed(reg, div->base + map->gate_offs);
+
+	if (div->lock)
+		spin_unlock(div->lock);
+
+	return 0;
+}
+
+static void berlin2_div_disable(struct clk_hw *hw)
+{
+	struct berlin2_div *div = to_berlin2_div(hw);
+	struct berlin2_div_map *map = &div->map;
+	u32 reg;
+
+	if (div->lock)
+		spin_lock(div->lock);
+
+	reg = readl_relaxed(div->base + map->gate_offs);
+	reg &= ~BIT(map->gate_shift);
+	writel_relaxed(reg, div->base + map->gate_offs);
+
+	if (div->lock)
+		spin_unlock(div->lock);
+}
+
+static int berlin2_div_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct berlin2_div *div = to_berlin2_div(hw);
+	struct berlin2_div_map *map = &div->map;
+	u32 reg;
+
+	if (div->lock)
+		spin_lock(div->lock);
+
+	/* index == 0 is PLL_SWITCH */
+	reg = readl_relaxed(div->base + map->pll_switch_offs);
+	if (index == 0)
+		reg &= ~BIT(map->pll_switch_shift);
+	else
+		reg |= BIT(map->pll_switch_shift);
+	writel_relaxed(reg, div->base + map->pll_switch_offs);
+
+	/* index > 0 is PLL_SELECT */
+	if (index > 0) {
+		reg = readl_relaxed(div->base + map->pll_select_offs);
+		reg &= ~(PLL_SELECT_MASK << map->pll_select_shift);
+		reg |= (index - 1) << map->pll_select_shift;
+		writel_relaxed(reg, div->base + map->pll_select_offs);
+	}
+
+	if (div->lock)
+		spin_unlock(div->lock);
+
+	return 0;
+}
+
+static u8 berlin2_div_get_parent(struct clk_hw *hw)
+{
+	struct berlin2_div *div = to_berlin2_div(hw);
+	struct berlin2_div_map *map = &div->map;
+	u32 reg;
+	u8 index = 0;
+
+	if (div->lock)
+		spin_lock(div->lock);
+
+	/* PLL_SWITCH == 0 is index 0 */
+	reg = readl_relaxed(div->base + map->pll_switch_offs);
+	reg &= BIT(map->pll_switch_shift);
+	if (reg) {
+		reg = readl_relaxed(div->base + map->pll_select_offs);
+		reg >>= map->pll_select_shift;
+		reg &= PLL_SELECT_MASK;
+		index = 1 + reg;
+	}
+
+	if (div->lock)
+		spin_unlock(div->lock);
+
+	return index;
+}
+
+static unsigned long berlin2_div_recalc_rate(struct clk_hw *hw,
+					     unsigned long parent_rate)
+{
+	struct berlin2_div *div = to_berlin2_div(hw);
+	struct berlin2_div_map *map = &div->map;
+	u32 divsw, div3sw, divider = 1;
+
+	if (div->lock)
+		spin_lock(div->lock);
+
+	divsw = readl_relaxed(div->base + map->div_switch_offs) &
+		(1 << map->div_switch_shift);
+	div3sw = readl_relaxed(div->base + map->div3_switch_offs) &
+		(1 << map->div3_switch_shift);
+
+	/* constant divide-by-3 (dominant) */
+	if (div3sw != 0) {
+		divider = 3;
+	/* divider can be bypassed with DIV_SWITCH == 0 */
+	} else if (divsw == 0) {
+		divider = 1;
+	/* clock divider determined by DIV_SELECT */
+	} else {
+		u32 reg;
+		reg = readl_relaxed(div->base + map->div_select_offs);
+		reg >>= map->div_select_shift;
+		reg &= DIV_SELECT_MASK;
+		divider = clk_div[reg];
+	}
+
+	if (div->lock)
+		spin_unlock(div->lock);
+
+	return parent_rate / divider;
+}
+
+static const struct clk_ops berlin2_div_rate_ops = {
+	.recalc_rate	= berlin2_div_recalc_rate,
+};
+
+static const struct clk_ops berlin2_div_gate_ops = {
+	.is_enabled	= berlin2_div_is_enabled,
+	.enable		= berlin2_div_enable,
+	.disable	= berlin2_div_disable,
+};
+
+static const struct clk_ops berlin2_div_mux_ops = {
+	.set_parent	= berlin2_div_set_parent,
+	.get_parent	= berlin2_div_get_parent,
+};
+
+struct clk * __init
+berlin2_div_register(const struct berlin2_div_map *map,
+		     void __iomem *base, const char *name, u8 div_flags,
+		     const char **parent_names, int num_parents,
+		     unsigned long flags, spinlock_t *lock)
+{
+	const struct clk_ops *mux_ops = &berlin2_div_mux_ops;
+	const struct clk_ops *rate_ops = &berlin2_div_rate_ops;
+	const struct clk_ops *gate_ops = &berlin2_div_gate_ops;
+	struct berlin2_div *div;
+
+	div = kzalloc(sizeof(*div), GFP_KERNEL);
+	if (!div)
+		return ERR_PTR(-ENOMEM);
+
+	/* copy div_map to allow __initconst */
+	memcpy(&div->map, map, sizeof(*map));
+	div->base = base;
+	div->lock = lock;
+
+	if ((div_flags & BERLIN2_DIV_HAS_GATE) == 0)
+		gate_ops = NULL;
+	if ((div_flags & BERLIN2_DIV_HAS_MUX) == 0)
+		mux_ops = NULL;
+
+	return clk_register_composite(NULL, name, parent_names, num_parents,
+				      &div->hw, mux_ops, &div->hw, rate_ops,
+				      &div->hw, gate_ops, flags);
+}
diff --git a/drivers/clk/berlin/berlin2-div.h b/drivers/clk/berlin/berlin2-div.h
new file mode 100644
index 00000000000..15e3384f311
--- /dev/null
+++ b/drivers/clk/berlin/berlin2-div.h
@@ -0,0 +1,89 @@
+/*
+ * Copyright (c) 2014 Marvell Technology Group Ltd.
+ *
+ * Alexandre Belloni <alexandre.belloni@free-electrons.com>
+ * Sebastian Hesselbarth <sebastian.hesselbarth@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#ifndef __BERLIN2_DIV_H
+#define __BERLIN2_DIV_H
+
+struct clk;
+
+#define BERLIN2_DIV_HAS_GATE		BIT(0)
+#define BERLIN2_DIV_HAS_MUX		BIT(1)
+
+#define BERLIN2_PLL_SELECT(_off, _sh)	\
+	.pll_select_offs = _off,	\
+	.pll_select_shift = _sh
+
+#define BERLIN2_PLL_SWITCH(_off, _sh)	\
+	.pll_switch_offs = _off,	\
+	.pll_switch_shift = _sh
+
+#define BERLIN2_DIV_SELECT(_off, _sh)	\
+	.div_select_offs = _off,	\
+	.div_select_shift = _sh
+
+#define BERLIN2_DIV_SWITCH(_off, _sh)	\
+	.div_switch_offs = _off,	\
+	.div_switch_shift = _sh
+
+#define BERLIN2_DIV_D3SWITCH(_off, _sh)	\
+	.div3_switch_offs = _off,	\
+	.div3_switch_shift = _sh
+
+#define BERLIN2_DIV_GATE(_off, _sh)	\
+	.gate_offs = _off,		\
+	.gate_shift = _sh
+
+#define BERLIN2_SINGLE_DIV(_off)	\
+	BERLIN2_DIV_GATE(_off, 0),	\
+	BERLIN2_PLL_SELECT(_off, 1),	\
+	BERLIN2_PLL_SWITCH(_off, 4),	\
+	BERLIN2_DIV_SWITCH(_off, 5),	\
+	BERLIN2_DIV_D3SWITCH(_off, 6),	\
+	BERLIN2_DIV_SELECT(_off, 7)
+
+struct berlin2_div_map {
+	u16 pll_select_offs;
+	u16 pll_switch_offs;
+	u16 div_select_offs;
+	u16 div_switch_offs;
+	u16 div3_switch_offs;
+	u16 gate_offs;
+	u8 pll_select_shift;
+	u8 pll_switch_shift;
+	u8 div_select_shift;
+	u8 div_switch_shift;
+	u8 div3_switch_shift;
+	u8 gate_shift;
+};
+
+struct berlin2_div_data {
+	const char *name;
+	const u8 *parent_ids;
+	int num_parents;
+	unsigned long flags;
+	struct berlin2_div_map map;
+	u8 div_flags;
+};
+
+struct clk * __init
+berlin2_div_register(const struct berlin2_div_map *map,
+	     void __iomem *base,  const char *name, u8 div_flags,
+	     const char **parent_names, int num_parents,
+	     unsigned long flags,  spinlock_t *lock);
+
+#endif /* __BERLIN2_DIV_H */
diff --git a/drivers/clk/berlin/berlin2-pll.c b/drivers/clk/berlin/berlin2-pll.c
new file mode 100644
index 00000000000..bdc506b0382
--- /dev/null
+++ b/drivers/clk/berlin/berlin2-pll.c
@@ -0,0 +1,117 @@
+/*
+ * Copyright (c) 2014 Marvell Technology Group Ltd.
+ *
+ * Alexandre Belloni <alexandre.belloni@free-electrons.com>
+ * Sebastian Hesselbarth <sebastian.hesselbarth@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <linux/clk-provider.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/slab.h>
+#include <asm/div64.h>
+
+#include "berlin2-div.h"
+
+struct berlin2_pll_map {
+	const u8 vcodiv[16];
+	u8 mult;
+	u8 fbdiv_shift;
+	u8 rfdiv_shift;
+	u8 divsel_shift;
+};
+
+struct berlin2_pll {
+	struct clk_hw hw;
+	void __iomem *base;
+	struct berlin2_pll_map map;
+};
+
+#define to_berlin2_pll(hw) container_of(hw, struct berlin2_pll, hw)
+
+#define SPLL_CTRL0	0x00
+#define SPLL_CTRL1	0x04
+#define SPLL_CTRL2	0x08
+#define SPLL_CTRL3	0x0c
+#define SPLL_CTRL4	0x10
+
+#define FBDIV_MASK	0x1ff
+#define RFDIV_MASK	0x1f
+#define DIVSEL_MASK	0xf
+
+/*
+ * The output frequency formula for the pll is:
+ * clkout = fbdiv / refdiv * parent / vcodiv
+ */
+static unsigned long
+berlin2_pll_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
+{
+	struct berlin2_pll *pll = to_berlin2_pll(hw);
+	struct berlin2_pll_map *map = &pll->map;
+	u32 val, fbdiv, rfdiv, vcodivsel, vcodiv;
+	u64 rate = parent_rate;
+
+	val = readl_relaxed(pll->base + SPLL_CTRL0);
+	fbdiv = (val >> map->fbdiv_shift) & FBDIV_MASK;
+	rfdiv = (val >> map->rfdiv_shift) & RFDIV_MASK;
+	if (rfdiv == 0) {
+		pr_warn("%s has zero rfdiv\n", __clk_get_name(hw->clk));
+		rfdiv = 1;
+	}
+
+	val = readl_relaxed(pll->base + SPLL_CTRL1);
+	vcodivsel = (val >> map->divsel_shift) & DIVSEL_MASK;
+	vcodiv = map->vcodiv[vcodivsel];
+	if (vcodiv == 0) {
+		pr_warn("%s has zero vcodiv (index %d)\n",
+			__clk_get_name(hw->clk), vcodivsel);
+		vcodiv = 1;
+	}
+
+	rate *= fbdiv * map->mult;
+	do_div(rate, rfdiv * vcodiv);
+
+	return (unsigned long)rate;
+}
+
+static const struct clk_ops berlin2_pll_ops = {
+	.recalc_rate	= berlin2_pll_recalc_rate,
+};
+
+struct clk * __init
+berlin2_pll_register(const struct berlin2_pll_map *map,
+		     void __iomem *base, const char *name,
+		     const char *parent_name, unsigned long flags)
+{
+	struct clk_init_data init;
+	struct berlin2_pll *pll;
+
+	pll = kzalloc(sizeof(*pll), GFP_KERNEL);
+	if (!pll)
+		return ERR_PTR(-ENOMEM);
+
+	/* copy pll_map to allow __initconst */
+	memcpy(&pll->map, map, sizeof(*map));
+	pll->base = base;
+	pll->hw.init = &init;
+	init.name = name;
+	init.ops = &berlin2_pll_ops;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+	init.flags = flags;
+
+	return clk_register(NULL, &pll->hw);
+}
diff --git a/drivers/clk/berlin/berlin2-pll.h b/drivers/clk/berlin/berlin2-pll.h
new file mode 100644
index 00000000000..8831ce27ac1
--- /dev/null
+++ b/drivers/clk/berlin/berlin2-pll.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright (c) 2014 Marvell Technology Group Ltd.
+ *
+ * Alexandre Belloni <alexandre.belloni@free-electrons.com>
+ * Sebastian Hesselbarth <sebastian.hesselbarth@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#ifndef __BERLIN2_PLL_H
+#define __BERLIN2_PLL_H
+
+struct clk;
+
+struct berlin2_pll_map {
+	const u8 vcodiv[16];
+	u8 mult;
+	u8 fbdiv_shift;
+	u8 rfdiv_shift;
+	u8 divsel_shift;
+};
+
+struct clk * __init
+berlin2_pll_register(const struct berlin2_pll_map *map,
+		     void __iomem *base, const char *name,
+		     const char *parent_name, unsigned long flags);
+
+#endif /* __BERLIN2_PLL_H */
diff --git a/drivers/clk/berlin/bg2.c b/drivers/clk/berlin/bg2.c
new file mode 100644
index 00000000000..515fb133495
--- /dev/null
+++ b/drivers/clk/berlin/bg2.c
@@ -0,0 +1,691 @@
+/*
+ * Copyright (c) 2014 Marvell Technology Group Ltd.
+ *
+ * Sebastian Hesselbarth <sebastian.hesselbarth@gmail.com>
+ * Alexandre Belloni <alexandre.belloni@free-electrons.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/slab.h>
+
+#include <dt-bindings/clock/berlin2.h>
+
+#include "berlin2-avpll.h"
+#include "berlin2-div.h"
+#include "berlin2-pll.h"
+#include "common.h"
+
+#define REG_PINMUX0		0x0000
+#define REG_PINMUX1		0x0004
+#define REG_SYSPLLCTL0		0x0014
+#define REG_SYSPLLCTL4		0x0024
+#define REG_MEMPLLCTL0		0x0028
+#define REG_MEMPLLCTL4		0x0038
+#define REG_CPUPLLCTL0		0x003c
+#define REG_CPUPLLCTL4		0x004c
+#define REG_AVPLLCTL0		0x0050
+#define REG_AVPLLCTL31		0x00cc
+#define REG_AVPLLCTL62		0x0148
+#define REG_PLLSTATUS		0x014c
+#define REG_CLKENABLE		0x0150
+#define REG_CLKSELECT0		0x0154
+#define REG_CLKSELECT1		0x0158
+#define REG_CLKSELECT2		0x015c
+#define REG_CLKSELECT3		0x0160
+#define REG_CLKSWITCH0		0x0164
+#define REG_CLKSWITCH1		0x0168
+#define REG_RESET_TRIGGER	0x0178
+#define REG_RESET_STATUS0	0x017c
+#define REG_RESET_STATUS1	0x0180
+#define REG_SW_GENERIC0		0x0184
+#define REG_SW_GENERIC3		0x0190
+#define REG_PRODUCTID		0x01cc
+#define REG_PRODUCTID_EXT	0x01d0
+#define REG_GFX3DCORE_CLKCTL	0x022c
+#define REG_GFX3DSYS_CLKCTL	0x0230
+#define REG_ARC_CLKCTL		0x0234
+#define REG_VIP_CLKCTL		0x0238
+#define REG_SDIO0XIN_CLKCTL	0x023c
+#define REG_SDIO1XIN_CLKCTL	0x0240
+#define REG_GFX3DEXTRA_CLKCTL	0x0244
+#define REG_GFX3D_RESET		0x0248
+#define REG_GC360_CLKCTL	0x024c
+#define REG_SDIO_DLLMST_CLKCTL	0x0250
+
+/*
+ * BG2/BG2CD SoCs have the following audio/video I/O units:
+ *
+ * audiohd: HDMI TX audio
+ * audio0:  7.1ch TX
+ * audio1:  2ch TX
+ * audio2:  2ch RX
+ * audio3:  SPDIF TX
+ * video0:  HDMI video
+ * video1:  Secondary video
+ * video2:  SD auxiliary video
+ *
+ * There are no external audio clocks (ACLKI0, ACLKI1) and
+ * only one external video clock (VCLKI0).
+ *
+ * Currently missing bits and pieces:
+ * - audio_fast_pll is unknown
+ * - audiohd_pll is unknown
+ * - video0_pll is unknown
+ * - audio[023], audiohd parent pll is assumed to be audio_fast_pll
+ *
+ */
+
+#define	MAX_CLKS 41
+static struct clk *clks[MAX_CLKS];
+static struct clk_onecell_data clk_data;
+static DEFINE_SPINLOCK(lock);
+static void __iomem *gbase;
+
+enum {
+	REFCLK, VIDEO_EXT0,
+	SYSPLL, MEMPLL, CPUPLL,
+	AVPLL_A1, AVPLL_A2, AVPLL_A3, AVPLL_A4,
+	AVPLL_A5, AVPLL_A6, AVPLL_A7, AVPLL_A8,
+	AVPLL_B1, AVPLL_B2, AVPLL_B3, AVPLL_B4,
+	AVPLL_B5, AVPLL_B6, AVPLL_B7, AVPLL_B8,
+	AUDIO1_PLL, AUDIO_FAST_PLL,
+	VIDEO0_PLL, VIDEO0_IN,
+	VIDEO1_PLL, VIDEO1_IN,
+	VIDEO2_PLL, VIDEO2_IN,
+};
+
+static const char *clk_names[] = {
+	[REFCLK]		= "refclk",
+	[VIDEO_EXT0]		= "video_ext0",
+	[SYSPLL]		= "syspll",
+	[MEMPLL]		= "mempll",
+	[CPUPLL]		= "cpupll",
+	[AVPLL_A1]		= "avpll_a1",
+	[AVPLL_A2]		= "avpll_a2",
+	[AVPLL_A3]		= "avpll_a3",
+	[AVPLL_A4]		= "avpll_a4",
+	[AVPLL_A5]		= "avpll_a5",
+	[AVPLL_A6]		= "avpll_a6",
+	[AVPLL_A7]		= "avpll_a7",
+	[AVPLL_A8]		= "avpll_a8",
+	[AVPLL_B1]		= "avpll_b1",
+	[AVPLL_B2]		= "avpll_b2",
+	[AVPLL_B3]		= "avpll_b3",
+	[AVPLL_B4]		= "avpll_b4",
+	[AVPLL_B5]		= "avpll_b5",
+	[AVPLL_B6]		= "avpll_b6",
+	[AVPLL_B7]		= "avpll_b7",
+	[AVPLL_B8]		= "avpll_b8",
+	[AUDIO1_PLL]		= "audio1_pll",
+	[AUDIO_FAST_PLL]	= "audio_fast_pll",
+	[VIDEO0_PLL]		= "video0_pll",
+	[VIDEO0_IN]		= "video0_in",
+	[VIDEO1_PLL]		= "video1_pll",
+	[VIDEO1_IN]		= "video1_in",
+	[VIDEO2_PLL]		= "video2_pll",
+	[VIDEO2_IN]		= "video2_in",
+};
+
+static const struct berlin2_pll_map bg2_pll_map __initconst = {
+	.vcodiv		= {10, 15, 20, 25, 30, 40, 50, 60, 80},
+	.mult		= 10,
+	.fbdiv_shift	= 6,
+	.rfdiv_shift	= 1,
+	.divsel_shift	= 7,
+};
+
+static const u8 default_parent_ids[] = {
+	SYSPLL, AVPLL_B4, AVPLL_A5, AVPLL_B6, AVPLL_B7, SYSPLL
+};
+
+static const struct berlin2_div_data bg2_divs[] __initconst = {
+	{
+		.name = "sys",
+		.parent_ids = (const u8 []){
+			SYSPLL, AVPLL_B4, AVPLL_B5, AVPLL_B6, AVPLL_B7, SYSPLL
+		},
+		.num_parents = 6,
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 0),
+			BERLIN2_PLL_SELECT(REG_CLKSELECT0, 0),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT0, 3),
+			BERLIN2_PLL_SWITCH(REG_CLKSWITCH0, 3),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH0, 4),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH0, 5),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = CLK_IGNORE_UNUSED,
+	},
+	{
+		.name = "cpu",
+		.parent_ids = (const u8 []){
+			CPUPLL, MEMPLL, MEMPLL, MEMPLL, MEMPLL
+		},
+		.num_parents = 5,
+		.map = {
+			BERLIN2_PLL_SELECT(REG_CLKSELECT0, 6),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT0, 9),
+			BERLIN2_PLL_SWITCH(REG_CLKSWITCH0, 6),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH0, 7),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH0, 8),
+		},
+		.div_flags = BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "drmfigo",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 16),
+			BERLIN2_PLL_SELECT(REG_CLKSELECT0, 17),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT0, 20),
+			BERLIN2_PLL_SWITCH(REG_CLKSWITCH0, 12),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH0, 13),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH0, 14),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "cfg",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 1),
+			BERLIN2_PLL_SELECT(REG_CLKSELECT0, 23),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT0, 26),
+			BERLIN2_PLL_SWITCH(REG_CLKSWITCH0, 15),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH0, 16),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH0, 17),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "gfx",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 4),
+			BERLIN2_PLL_SELECT(REG_CLKSELECT0, 29),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT1, 0),
+			BERLIN2_PLL_SWITCH(REG_CLKSWITCH0, 18),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH0, 19),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH0, 20),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "zsp",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 5),
+			BERLIN2_PLL_SELECT(REG_CLKSELECT1, 3),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT1, 6),
+			BERLIN2_PLL_SWITCH(REG_CLKSWITCH0, 21),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH0, 22),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH0, 23),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "perif",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 6),
+			BERLIN2_PLL_SELECT(REG_CLKSELECT1, 9),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT1, 12),
+			BERLIN2_PLL_SWITCH(REG_CLKSWITCH0, 24),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH0, 25),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH0, 26),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = CLK_IGNORE_UNUSED,
+	},
+	{
+		.name = "pcube",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 2),
+			BERLIN2_PLL_SELECT(REG_CLKSELECT1, 15),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT1, 18),
+			BERLIN2_PLL_SWITCH(REG_CLKSWITCH0, 27),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH0, 28),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH0, 29),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "vscope",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 3),
+			BERLIN2_PLL_SELECT(REG_CLKSELECT1, 21),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT1, 24),
+			BERLIN2_PLL_SWITCH(REG_CLKSWITCH0, 30),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH0, 31),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH1, 0),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "nfc_ecc",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 18),
+			BERLIN2_PLL_SELECT(REG_CLKSELECT1, 27),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT2, 0),
+			BERLIN2_PLL_SWITCH(REG_CLKSWITCH1, 1),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH1, 2),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH1, 3),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "vpp",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 21),
+			BERLIN2_PLL_SELECT(REG_CLKSELECT2, 3),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT2, 6),
+			BERLIN2_PLL_SWITCH(REG_CLKSWITCH1, 4),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH1, 5),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH1, 6),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "app",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 20),
+			BERLIN2_PLL_SELECT(REG_CLKSELECT2, 9),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT2, 12),
+			BERLIN2_PLL_SWITCH(REG_CLKSWITCH1, 7),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH1, 8),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH1, 9),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "audio0",
+		.parent_ids = (const u8 []){ AUDIO_FAST_PLL },
+		.num_parents = 1,
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 22),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT2, 17),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH1, 10),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH1, 11),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE,
+		.flags = 0,
+	},
+	{
+		.name = "audio2",
+		.parent_ids = (const u8 []){ AUDIO_FAST_PLL },
+		.num_parents = 1,
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 24),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT2, 20),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH1, 14),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH1, 15),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE,
+		.flags = 0,
+	},
+	{
+		.name = "audio3",
+		.parent_ids = (const u8 []){ AUDIO_FAST_PLL },
+		.num_parents = 1,
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 25),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT2, 23),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH1, 16),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH1, 17),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE,
+		.flags = 0,
+	},
+	{
+		.name = "audio1",
+		.parent_ids = (const u8 []){ AUDIO1_PLL },
+		.num_parents = 1,
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 23),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT3, 0),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH1, 12),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH1, 13),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE,
+		.flags = 0,
+	},
+	{
+		.name = "gfx3d_core",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_SINGLE_DIV(REG_GFX3DCORE_CLKCTL),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "gfx3d_sys",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_SINGLE_DIV(REG_GFX3DSYS_CLKCTL),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "arc",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_SINGLE_DIV(REG_ARC_CLKCTL),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "vip",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_SINGLE_DIV(REG_VIP_CLKCTL),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "sdio0xin",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_SINGLE_DIV(REG_SDIO0XIN_CLKCTL),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "sdio1xin",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_SINGLE_DIV(REG_SDIO1XIN_CLKCTL),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "gfx3d_extra",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_SINGLE_DIV(REG_GFX3DEXTRA_CLKCTL),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "gc360",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_SINGLE_DIV(REG_GC360_CLKCTL),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "sdio_dllmst",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_SINGLE_DIV(REG_SDIO_DLLMST_CLKCTL),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+};
+
+static const struct berlin2_gate_data bg2_gates[] __initconst = {
+	{ "geth0",	"perif",	7 },
+	{ "geth1",	"perif",	8 },
+	{ "sata",	"perif",	9 },
+	{ "ahbapb",	"perif",	10, CLK_IGNORE_UNUSED },
+	{ "usb0",	"perif",	11 },
+	{ "usb1",	"perif",	12 },
+	{ "pbridge",	"perif",	13, CLK_IGNORE_UNUSED },
+	{ "sdio0",	"perif",	14, CLK_IGNORE_UNUSED },
+	{ "sdio1",	"perif",	15, CLK_IGNORE_UNUSED },
+	{ "nfc",	"perif",	17 },
+	{ "smemc",	"perif",	19 },
+	{ "audiohd",	"audiohd_pll",	26 },
+	{ "video0",	"video0_in",	27 },
+	{ "video1",	"video1_in",	28 },
+	{ "video2",	"video2_in",	29 },
+};
+
+static void __init berlin2_clock_setup(struct device_node *np)
+{
+	const char *parent_names[9];
+	struct clk *clk;
+	u8 avpll_flags = 0;
+	int n;
+
+	gbase = of_iomap(np, 0);
+	if (!gbase)
+		return;
+
+	/* overwrite default clock names with DT provided ones */
+	clk = of_clk_get_by_name(np, clk_names[REFCLK]);
+	if (!IS_ERR(clk)) {
+		clk_names[REFCLK] = __clk_get_name(clk);
+		clk_put(clk);
+	}
+
+	clk = of_clk_get_by_name(np, clk_names[VIDEO_EXT0]);
+	if (!IS_ERR(clk)) {
+		clk_names[VIDEO_EXT0] = __clk_get_name(clk);
+		clk_put(clk);
+	}
+
+	/* simple register PLLs */
+	clk = berlin2_pll_register(&bg2_pll_map, gbase + REG_SYSPLLCTL0,
+				   clk_names[SYSPLL], clk_names[REFCLK], 0);
+	if (IS_ERR(clk))
+		goto bg2_fail;
+
+	clk = berlin2_pll_register(&bg2_pll_map, gbase + REG_MEMPLLCTL0,
+				   clk_names[MEMPLL], clk_names[REFCLK], 0);
+	if (IS_ERR(clk))
+		goto bg2_fail;
+
+	clk = berlin2_pll_register(&bg2_pll_map, gbase + REG_CPUPLLCTL0,
+				   clk_names[CPUPLL], clk_names[REFCLK], 0);
+	if (IS_ERR(clk))
+		goto bg2_fail;
+
+	if (of_device_is_compatible(np, "marvell,berlin2-global-register"))
+		avpll_flags |= BERLIN2_AVPLL_SCRAMBLE_QUIRK;
+
+	/* audio/video VCOs */
+	clk = berlin2_avpll_vco_register(gbase + REG_AVPLLCTL0, "avpll_vcoA",
+			 clk_names[REFCLK], avpll_flags, 0);
+	if (IS_ERR(clk))
+		goto bg2_fail;
+
+	for (n = 0; n < 8; n++) {
+		clk = berlin2_avpll_channel_register(gbase + REG_AVPLLCTL0,
+			     clk_names[AVPLL_A1 + n], n, "avpll_vcoA",
+			     avpll_flags, 0);
+		if (IS_ERR(clk))
+			goto bg2_fail;
+	}
+
+	clk = berlin2_avpll_vco_register(gbase + REG_AVPLLCTL31, "avpll_vcoB",
+				 clk_names[REFCLK], BERLIN2_AVPLL_BIT_QUIRK |
+				 avpll_flags, 0);
+	if (IS_ERR(clk))
+		goto bg2_fail;
+
+	for (n = 0; n < 8; n++) {
+		clk = berlin2_avpll_channel_register(gbase + REG_AVPLLCTL31,
+			     clk_names[AVPLL_B1 + n], n, "avpll_vcoB",
+			     BERLIN2_AVPLL_BIT_QUIRK | avpll_flags, 0);
+		if (IS_ERR(clk))
+			goto bg2_fail;
+	}
+
+	/* reference clock bypass switches */
+	parent_names[0] = clk_names[SYSPLL];
+	parent_names[1] = clk_names[REFCLK];
+	clk = clk_register_mux(NULL, "syspll_byp", parent_names, 2,
+			       0, gbase + REG_CLKSWITCH0, 0, 1, 0, &lock);
+	if (IS_ERR(clk))
+		goto bg2_fail;
+	clk_names[SYSPLL] = __clk_get_name(clk);
+
+	parent_names[0] = clk_names[MEMPLL];
+	parent_names[1] = clk_names[REFCLK];
+	clk = clk_register_mux(NULL, "mempll_byp", parent_names, 2,
+			       0, gbase + REG_CLKSWITCH0, 1, 1, 0, &lock);
+	if (IS_ERR(clk))
+		goto bg2_fail;
+	clk_names[MEMPLL] = __clk_get_name(clk);
+
+	parent_names[0] = clk_names[CPUPLL];
+	parent_names[1] = clk_names[REFCLK];
+	clk = clk_register_mux(NULL, "cpupll_byp", parent_names, 2,
+			       0, gbase + REG_CLKSWITCH0, 2, 1, 0, &lock);
+	if (IS_ERR(clk))
+		goto bg2_fail;
+	clk_names[CPUPLL] = __clk_get_name(clk);
+
+	/* clock muxes */
+	parent_names[0] = clk_names[AVPLL_B3];
+	parent_names[1] = clk_names[AVPLL_A3];
+	clk = clk_register_mux(NULL, clk_names[AUDIO1_PLL], parent_names, 2,
+			       0, gbase + REG_CLKSELECT2, 29, 1, 0, &lock);
+	if (IS_ERR(clk))
+		goto bg2_fail;
+
+	parent_names[0] = clk_names[VIDEO0_PLL];
+	parent_names[1] = clk_names[VIDEO_EXT0];
+	clk = clk_register_mux(NULL, clk_names[VIDEO0_IN], parent_names, 2,
+			       0, gbase + REG_CLKSELECT3, 4, 1, 0, &lock);
+	if (IS_ERR(clk))
+		goto bg2_fail;
+
+	parent_names[0] = clk_names[VIDEO1_PLL];
+	parent_names[1] = clk_names[VIDEO_EXT0];
+	clk = clk_register_mux(NULL, clk_names[VIDEO1_IN], parent_names, 2,
+			       0, gbase + REG_CLKSELECT3, 6, 1, 0, &lock);
+	if (IS_ERR(clk))
+		goto bg2_fail;
+
+	parent_names[0] = clk_names[AVPLL_A2];
+	parent_names[1] = clk_names[AVPLL_B2];
+	clk = clk_register_mux(NULL, clk_names[VIDEO1_PLL], parent_names, 2,
+			       0, gbase + REG_CLKSELECT3, 7, 1, 0, &lock);
+	if (IS_ERR(clk))
+		goto bg2_fail;
+
+	parent_names[0] = clk_names[VIDEO2_PLL];
+	parent_names[1] = clk_names[VIDEO_EXT0];
+	clk = clk_register_mux(NULL, clk_names[VIDEO2_IN], parent_names, 2,
+			       0, gbase + REG_CLKSELECT3, 9, 1, 0, &lock);
+	if (IS_ERR(clk))
+		goto bg2_fail;
+
+	parent_names[0] = clk_names[AVPLL_B1];
+	parent_names[1] = clk_names[AVPLL_A5];
+	clk = clk_register_mux(NULL, clk_names[VIDEO2_PLL], parent_names, 2,
+			       0, gbase + REG_CLKSELECT3, 10, 1, 0, &lock);
+	if (IS_ERR(clk))
+		goto bg2_fail;
+
+	/* clock divider cells */
+	for (n = 0; n < ARRAY_SIZE(bg2_divs); n++) {
+		const struct berlin2_div_data *dd = &bg2_divs[n];
+		int k;
+
+		for (k = 0; k < dd->num_parents; k++)
+			parent_names[k] = clk_names[dd->parent_ids[k]];
+
+		clks[CLKID_SYS + n] = berlin2_div_register(&dd->map, gbase,
+				dd->name, dd->div_flags, parent_names,
+				dd->num_parents, dd->flags, &lock);
+	}
+
+	/* clock gate cells */
+	for (n = 0; n < ARRAY_SIZE(bg2_gates); n++) {
+		const struct berlin2_gate_data *gd = &bg2_gates[n];
+
+		clks[CLKID_GETH0 + n] = clk_register_gate(NULL, gd->name,
+			    gd->parent_name, gd->flags, gbase + REG_CLKENABLE,
+			    gd->bit_idx, 0, &lock);
+	}
+
+	/* twdclk is derived from cpu/3 */
+	clks[CLKID_TWD] =
+		clk_register_fixed_factor(NULL, "twd", "cpu", 0, 1, 3);
+
+	/* check for errors on leaf clocks */
+	for (n = 0; n < MAX_CLKS; n++) {
+		if (!IS_ERR(clks[n]))
+			continue;
+
+		pr_err("%s: Unable to register leaf clock %d\n",
+		       np->full_name, n);
+		goto bg2_fail;
+	}
+
+	/* register clk-provider */
+	clk_data.clks = clks;
+	clk_data.clk_num = MAX_CLKS;
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+
+	return;
+
+bg2_fail:
+	iounmap(gbase);
+}
+CLK_OF_DECLARE(berlin2_clock, "marvell,berlin2-chip-ctrl",
+	       berlin2_clock_setup);
+CLK_OF_DECLARE(berlin2cd_clock, "marvell,berlin2cd-chip-ctrl",
+	       berlin2_clock_setup);
diff --git a/drivers/clk/berlin/bg2q.c b/drivers/clk/berlin/bg2q.c
new file mode 100644
index 00000000000..21784e4eb3f
--- /dev/null
+++ b/drivers/clk/berlin/bg2q.c
@@ -0,0 +1,389 @@
+/*
+ * Copyright (c) 2014 Marvell Technology Group Ltd.
+ *
+ * Alexandre Belloni <alexandre.belloni@free-electrons.com>
+ * Sebastian Hesselbarth <sebastian.hesselbarth@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/slab.h>
+
+#include <dt-bindings/clock/berlin2q.h>
+
+#include "berlin2-div.h"
+#include "berlin2-pll.h"
+#include "common.h"
+
+#define REG_PINMUX0		0x0018
+#define REG_PINMUX5		0x002c
+#define REG_SYSPLLCTL0		0x0030
+#define REG_SYSPLLCTL4		0x0040
+#define REG_CLKENABLE		0x00e8
+#define REG_CLKSELECT0		0x00ec
+#define REG_CLKSELECT1		0x00f0
+#define REG_CLKSELECT2		0x00f4
+#define REG_CLKSWITCH0		0x00f8
+#define REG_CLKSWITCH1		0x00fc
+#define REG_SW_GENERIC0		0x0110
+#define REG_SW_GENERIC3		0x011c
+#define REG_SDIO0XIN_CLKCTL	0x0158
+#define REG_SDIO1XIN_CLKCTL	0x015c
+
+#define	MAX_CLKS 27
+static struct clk *clks[MAX_CLKS];
+static struct clk_onecell_data clk_data;
+static DEFINE_SPINLOCK(lock);
+static void __iomem *gbase;
+static void __iomem *cpupll_base;
+
+enum {
+	REFCLK,
+	SYSPLL, CPUPLL,
+	AVPLL_B1, AVPLL_B2, AVPLL_B3, AVPLL_B4,
+	AVPLL_B5, AVPLL_B6, AVPLL_B7, AVPLL_B8,
+};
+
+static const char *clk_names[] = {
+	[REFCLK]		= "refclk",
+	[SYSPLL]		= "syspll",
+	[CPUPLL]		= "cpupll",
+	[AVPLL_B1]		= "avpll_b1",
+	[AVPLL_B2]		= "avpll_b2",
+	[AVPLL_B3]		= "avpll_b3",
+	[AVPLL_B4]		= "avpll_b4",
+	[AVPLL_B5]		= "avpll_b5",
+	[AVPLL_B6]		= "avpll_b6",
+	[AVPLL_B7]		= "avpll_b7",
+	[AVPLL_B8]		= "avpll_b8",
+};
+
+static const struct berlin2_pll_map bg2q_pll_map __initconst = {
+	.vcodiv		= {1, 0, 2, 0, 3, 4, 0, 6, 8},
+	.mult		= 1,
+	.fbdiv_shift	= 7,
+	.rfdiv_shift	= 2,
+	.divsel_shift	= 9,
+};
+
+static const u8 default_parent_ids[] = {
+	SYSPLL, AVPLL_B4, AVPLL_B5, AVPLL_B6, AVPLL_B7, SYSPLL
+};
+
+static const struct berlin2_div_data bg2q_divs[] __initconst = {
+	{
+		.name = "sys",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 0),
+			BERLIN2_PLL_SELECT(REG_CLKSELECT0, 0),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT0, 3),
+			BERLIN2_PLL_SWITCH(REG_CLKSWITCH0, 3),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH0, 4),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH0, 5),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = CLK_IGNORE_UNUSED,
+	},
+	{
+		.name = "drmfigo",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 17),
+			BERLIN2_PLL_SELECT(REG_CLKSELECT0, 6),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT0, 9),
+			BERLIN2_PLL_SWITCH(REG_CLKSWITCH0, 6),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH0, 7),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH0, 8),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "cfg",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 1),
+			BERLIN2_PLL_SELECT(REG_CLKSELECT0, 12),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT0, 15),
+			BERLIN2_PLL_SWITCH(REG_CLKSWITCH0, 9),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH0, 10),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH0, 11),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "gfx2d",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 4),
+			BERLIN2_PLL_SELECT(REG_CLKSELECT0, 18),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT0, 21),
+			BERLIN2_PLL_SWITCH(REG_CLKSWITCH0, 12),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH0, 13),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH0, 14),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "zsp",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 6),
+			BERLIN2_PLL_SELECT(REG_CLKSELECT0, 24),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT0, 27),
+			BERLIN2_PLL_SWITCH(REG_CLKSWITCH0, 15),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH0, 16),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH0, 17),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "perif",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 7),
+			BERLIN2_PLL_SELECT(REG_CLKSELECT1, 0),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT1, 3),
+			BERLIN2_PLL_SWITCH(REG_CLKSWITCH0, 18),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH0, 19),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH0, 20),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = CLK_IGNORE_UNUSED,
+	},
+	{
+		.name = "pcube",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 2),
+			BERLIN2_PLL_SELECT(REG_CLKSELECT1, 6),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT1, 9),
+			BERLIN2_PLL_SWITCH(REG_CLKSWITCH0, 21),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH0, 22),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH0, 23),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "vscope",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 3),
+			BERLIN2_PLL_SELECT(REG_CLKSELECT1, 12),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT1, 15),
+			BERLIN2_PLL_SWITCH(REG_CLKSWITCH0, 24),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH0, 25),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH0, 26),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "nfc_ecc",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 19),
+			BERLIN2_PLL_SELECT(REG_CLKSELECT1, 18),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT1, 21),
+			BERLIN2_PLL_SWITCH(REG_CLKSWITCH0, 27),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH0, 28),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH0, 29),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "vpp",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 21),
+			BERLIN2_PLL_SELECT(REG_CLKSELECT1, 24),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT1, 27),
+			BERLIN2_PLL_SWITCH(REG_CLKSWITCH0, 30),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH0, 31),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH1, 0),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "app",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_DIV_GATE(REG_CLKENABLE, 20),
+			BERLIN2_PLL_SELECT(REG_CLKSELECT2, 0),
+			BERLIN2_DIV_SELECT(REG_CLKSELECT2, 3),
+			BERLIN2_PLL_SWITCH(REG_CLKSWITCH1, 1),
+			BERLIN2_DIV_SWITCH(REG_CLKSWITCH1, 2),
+			BERLIN2_DIV_D3SWITCH(REG_CLKSWITCH1, 3),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "sdio0xin",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_SINGLE_DIV(REG_SDIO0XIN_CLKCTL),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+	{
+		.name = "sdio1xin",
+		.parent_ids = default_parent_ids,
+		.num_parents = ARRAY_SIZE(default_parent_ids),
+		.map = {
+			BERLIN2_SINGLE_DIV(REG_SDIO1XIN_CLKCTL),
+		},
+		.div_flags = BERLIN2_DIV_HAS_GATE | BERLIN2_DIV_HAS_MUX,
+		.flags = 0,
+	},
+};
+
+static const struct berlin2_gate_data bg2q_gates[] __initconst = {
+	{ "gfx2daxi",	"perif",	5 },
+	{ "geth0",	"perif",	8 },
+	{ "sata",	"perif",	9 },
+	{ "ahbapb",	"perif",	10, CLK_IGNORE_UNUSED },
+	{ "usb0",	"perif",	11 },
+	{ "usb1",	"perif",	12 },
+	{ "usb2",	"perif",	13 },
+	{ "usb3",	"perif",	14 },
+	{ "pbridge",	"perif",	15, CLK_IGNORE_UNUSED },
+	{ "sdio",	"perif",	16, CLK_IGNORE_UNUSED },
+	{ "nfc",	"perif",	18 },
+	{ "smemc",	"perif",	19 },
+	{ "pcie",	"perif",	22 },
+};
+
+static void __init berlin2q_clock_setup(struct device_node *np)
+{
+	const char *parent_names[9];
+	struct clk *clk;
+	int n;
+
+	gbase = of_iomap(np, 0);
+	if (!gbase) {
+		pr_err("%s: Unable to map global base\n", np->full_name);
+		return;
+	}
+
+	/* BG2Q CPU PLL is not part of global registers */
+	cpupll_base = of_iomap(np, 1);
+	if (!cpupll_base) {
+		pr_err("%s: Unable to map cpupll base\n", np->full_name);
+		iounmap(gbase);
+		return;
+	}
+
+	/* overwrite default clock names with DT provided ones */
+	clk = of_clk_get_by_name(np, clk_names[REFCLK]);
+	if (!IS_ERR(clk)) {
+		clk_names[REFCLK] = __clk_get_name(clk);
+		clk_put(clk);
+	}
+
+	/* simple register PLLs */
+	clk = berlin2_pll_register(&bg2q_pll_map, gbase + REG_SYSPLLCTL0,
+				   clk_names[SYSPLL], clk_names[REFCLK], 0);
+	if (IS_ERR(clk))
+		goto bg2q_fail;
+
+	clk = berlin2_pll_register(&bg2q_pll_map, cpupll_base,
+				   clk_names[CPUPLL], clk_names[REFCLK], 0);
+	if (IS_ERR(clk))
+		goto bg2q_fail;
+
+	/* TODO: add BG2Q AVPLL */
+
+	/*
+	 * TODO: add reference clock bypass switches:
+	 * memPLLSWBypass, cpuPLLSWBypass, and sysPLLSWBypass
+	 */
+
+	/* clock divider cells */
+	for (n = 0; n < ARRAY_SIZE(bg2q_divs); n++) {
+		const struct berlin2_div_data *dd = &bg2q_divs[n];
+		int k;
+
+		for (k = 0; k < dd->num_parents; k++)
+			parent_names[k] = clk_names[dd->parent_ids[k]];
+
+		clks[CLKID_SYS + n] = berlin2_div_register(&dd->map, gbase,
+				dd->name, dd->div_flags, parent_names,
+				dd->num_parents, dd->flags, &lock);
+	}
+
+	/* clock gate cells */
+	for (n = 0; n < ARRAY_SIZE(bg2q_gates); n++) {
+		const struct berlin2_gate_data *gd = &bg2q_gates[n];
+
+		clks[CLKID_GFX2DAXI + n] = clk_register_gate(NULL, gd->name,
+			    gd->parent_name, gd->flags, gbase + REG_CLKENABLE,
+			    gd->bit_idx, 0, &lock);
+	}
+
+	/*
+	 * twdclk is derived from cpu/3
+	 * TODO: use cpupll until cpuclk is not available
+	 */
+	clks[CLKID_TWD] =
+		clk_register_fixed_factor(NULL, "twd", clk_names[CPUPLL],
+					  0, 1, 3);
+
+	/* check for errors on leaf clocks */
+	for (n = 0; n < MAX_CLKS; n++) {
+		if (!IS_ERR(clks[n]))
+			continue;
+
+		pr_err("%s: Unable to register leaf clock %d\n",
+		       np->full_name, n);
+		goto bg2q_fail;
+	}
+
+	/* register clk-provider */
+	clk_data.clks = clks;
+	clk_data.clk_num = MAX_CLKS;
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+
+	return;
+
+bg2q_fail:
+	iounmap(cpupll_base);
+	iounmap(gbase);
+}
+CLK_OF_DECLARE(berlin2q_clock, "marvell,berlin2q-chip-ctrl",
+	       berlin2q_clock_setup);
diff --git a/drivers/clk/berlin/common.h b/drivers/clk/berlin/common.h
new file mode 100644
index 00000000000..bc68a14c455
--- /dev/null
+++ b/drivers/clk/berlin/common.h
@@ -0,0 +1,29 @@
+/*
+ * Copyright (c) 2014 Marvell Technology Group Ltd.
+ *
+ * Sebastian Hesselbarth <sebastian.hesselbarth@gmail.com>
+ * Alexandre Belloni <alexandre.belloni@free-electrons.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#ifndef __BERLIN2_COMMON_H
+#define __BERLIN2_COMMON_H
+
+struct berlin2_gate_data {
+	const char *name;
+	const char *parent_name;
+	u8 bit_idx;
+	unsigned long flags;
+};
+
+#endif /* BERLIN2_COMMON_H */
diff --git a/drivers/clk/clk-axm5516.c b/drivers/clk/clk-axm5516.c
new file mode 100644
index 00000000000..d2f1e119b45
--- /dev/null
+++ b/drivers/clk/clk-axm5516.c
@@ -0,0 +1,615 @@
+/*
+ * drivers/clk/clk-axm5516.c
+ *
+ * Provides clock implementations for three different types of clock devices on
+ * the Axxia device: PLL clock, a clock divider and a clock mux.
+ *
+ * Copyright (C) 2014 LSI 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.
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/clk-provider.h>
+#include <linux/regmap.h>
+#include <dt-bindings/clock/lsi,axm5516-clks.h>
+
+
+/**
+ * struct axxia_clk - Common struct to all Axxia clocks.
+ * @hw: clk_hw for the common clk framework
+ * @regmap: Regmap for the clock control registers
+ */
+struct axxia_clk {
+	struct clk_hw hw;
+	struct regmap *regmap;
+};
+#define to_axxia_clk(_hw) container_of(_hw, struct axxia_clk, hw)
+
+/**
+ * struct axxia_pllclk - Axxia PLL generated clock.
+ * @aclk: Common struct
+ * @reg: Offset into regmap for PLL control register
+ */
+struct axxia_pllclk {
+	struct axxia_clk aclk;
+	u32 reg;
+};
+#define to_axxia_pllclk(_aclk) container_of(_aclk, struct axxia_pllclk, aclk)
+
+/**
+ * axxia_pllclk_recalc - Calculate the PLL generated clock rate given the
+ * parent clock rate.
+ */
+static unsigned long
+axxia_pllclk_recalc(struct clk_hw *hw, unsigned long parent_rate)
+{
+	struct axxia_clk *aclk = to_axxia_clk(hw);
+	struct axxia_pllclk *pll = to_axxia_pllclk(aclk);
+	unsigned long rate, fbdiv, refdiv, postdiv;
+	u32 control;
+
+	regmap_read(aclk->regmap, pll->reg, &control);
+	postdiv = ((control >> 0) & 0xf) + 1;
+	fbdiv   = ((control >> 4) & 0xfff) + 3;
+	refdiv  = ((control >> 16) & 0x1f) + 1;
+	rate = (parent_rate / (refdiv * postdiv)) * fbdiv;
+
+	return rate;
+}
+
+static const struct clk_ops axxia_pllclk_ops = {
+	.recalc_rate = axxia_pllclk_recalc,
+};
+
+/**
+ * struct axxia_divclk - Axxia clock divider
+ * @aclk: Common struct
+ * @reg: Offset into regmap for PLL control register
+ * @shift: Bit position for divider value
+ * @width: Number of bits in divider value
+ */
+struct axxia_divclk {
+	struct axxia_clk aclk;
+	u32 reg;
+	u32 shift;
+	u32 width;
+};
+#define to_axxia_divclk(_aclk) container_of(_aclk, struct axxia_divclk, aclk)
+
+/**
+ * axxia_divclk_recalc_rate - Calculate clock divider output rage
+ */
+static unsigned long
+axxia_divclk_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
+{
+	struct axxia_clk *aclk = to_axxia_clk(hw);
+	struct axxia_divclk *divclk = to_axxia_divclk(aclk);
+	u32 ctrl, div;
+
+	regmap_read(aclk->regmap, divclk->reg, &ctrl);
+	div = 1 + ((ctrl >> divclk->shift) & ((1 << divclk->width)-1));
+
+	return parent_rate / div;
+}
+
+static const struct clk_ops axxia_divclk_ops = {
+	.recalc_rate = axxia_divclk_recalc_rate,
+};
+
+/**
+ * struct axxia_clkmux - Axxia clock mux
+ * @aclk: Common struct
+ * @reg: Offset into regmap for PLL control register
+ * @shift: Bit position for selection value
+ * @width: Number of bits in selection value
+ */
+struct axxia_clkmux {
+	struct axxia_clk aclk;
+	u32 reg;
+	u32 shift;
+	u32 width;
+};
+#define to_axxia_clkmux(_aclk) container_of(_aclk, struct axxia_clkmux, aclk)
+
+/**
+ * axxia_clkmux_get_parent - Return the index of selected parent clock
+ */
+static u8 axxia_clkmux_get_parent(struct clk_hw *hw)
+{
+	struct axxia_clk *aclk = to_axxia_clk(hw);
+	struct axxia_clkmux *mux = to_axxia_clkmux(aclk);
+	u32 ctrl, parent;
+
+	regmap_read(aclk->regmap, mux->reg, &ctrl);
+	parent = (ctrl >> mux->shift) & ((1 << mux->width) - 1);
+
+	return (u8) parent;
+}
+
+static const struct clk_ops axxia_clkmux_ops = {
+	.get_parent = axxia_clkmux_get_parent,
+};
+
+
+/*
+ * PLLs
+ */
+
+static struct axxia_pllclk clk_fab_pll = {
+	.aclk.hw.init = &(struct clk_init_data){
+		.name = "clk_fab_pll",
+		.parent_names = (const char *[]){
+			"clk_ref0"
+		},
+		.num_parents = 1,
+		.ops = &axxia_pllclk_ops,
+	},
+	.reg   = 0x01800,
+};
+
+static struct axxia_pllclk clk_cpu_pll = {
+	.aclk.hw.init = &(struct clk_init_data){
+		.name = "clk_cpu_pll",
+		.parent_names = (const char *[]){
+			"clk_ref0"
+		},
+		.num_parents = 1,
+		.ops = &axxia_pllclk_ops,
+	},
+	.reg   = 0x02000,
+};
+
+static struct axxia_pllclk clk_sys_pll = {
+	.aclk.hw.init = &(struct clk_init_data){
+		.name = "clk_sys_pll",
+		.parent_names = (const char *[]){
+			"clk_ref0"
+		},
+		.num_parents = 1,
+		.ops = &axxia_pllclk_ops,
+	},
+	.reg   = 0x02800,
+};
+
+static struct axxia_pllclk clk_sm0_pll = {
+	.aclk.hw.init = &(struct clk_init_data){
+		.name = "clk_sm0_pll",
+		.parent_names = (const char *[]){
+			"clk_ref2"
+		},
+		.num_parents = 1,
+		.ops = &axxia_pllclk_ops,
+	},
+	.reg   = 0x03000,
+};
+
+static struct axxia_pllclk clk_sm1_pll = {
+	.aclk.hw.init = &(struct clk_init_data){
+		.name = "clk_sm1_pll",
+		.parent_names = (const char *[]){
+			"clk_ref1"
+		},
+		.num_parents = 1,
+		.ops = &axxia_pllclk_ops,
+	},
+	.reg   = 0x03800,
+};
+
+/*
+ * Clock dividers
+ */
+
+static struct axxia_divclk clk_cpu0_div = {
+	.aclk.hw.init = &(struct clk_init_data){
+		.name = "clk_cpu0_div",
+		.parent_names = (const char *[]){
+			"clk_cpu_pll"
+		},
+		.num_parents = 1,
+		.ops = &axxia_divclk_ops,
+	},
+	.reg   = 0x10008,
+	.shift = 0,
+	.width = 4,
+};
+
+static struct axxia_divclk clk_cpu1_div = {
+	.aclk.hw.init = &(struct clk_init_data){
+		.name = "clk_cpu1_div",
+		.parent_names = (const char *[]){
+			"clk_cpu_pll"
+		},
+		.num_parents = 1,
+		.ops = &axxia_divclk_ops,
+	},
+	.reg   = 0x10008,
+	.shift = 4,
+	.width = 4,
+};
+
+static struct axxia_divclk clk_cpu2_div = {
+	.aclk.hw.init = &(struct clk_init_data){
+		.name = "clk_cpu2_div",
+		.parent_names = (const char *[]){
+			"clk_cpu_pll"
+		},
+		.num_parents = 1,
+		.ops = &axxia_divclk_ops,
+	},
+	.reg   = 0x10008,
+	.shift = 8,
+	.width = 4,
+};
+
+static struct axxia_divclk clk_cpu3_div = {
+	.aclk.hw.init = &(struct clk_init_data){
+		.name = "clk_cpu3_div",
+		.parent_names = (const char *[]){
+			"clk_cpu_pll"
+		},
+		.num_parents = 1,
+		.ops = &axxia_divclk_ops,
+	},
+	.reg   = 0x10008,
+	.shift = 12,
+	.width = 4,
+};
+
+static struct axxia_divclk clk_nrcp_div = {
+	.aclk.hw.init = &(struct clk_init_data){
+		.name = "clk_nrcp_div",
+		.parent_names = (const char *[]){
+			"clk_sys_pll"
+		},
+		.num_parents = 1,
+		.ops = &axxia_divclk_ops,
+	},
+	.reg   = 0x1000c,
+	.shift = 0,
+	.width = 4,
+};
+
+static struct axxia_divclk clk_sys_div = {
+	.aclk.hw.init = &(struct clk_init_data){
+		.name = "clk_sys_div",
+		.parent_names = (const char *[]){
+			"clk_sys_pll"
+		},
+		.num_parents = 1,
+		.ops = &axxia_divclk_ops,
+	},
+	.reg   = 0x1000c,
+	.shift = 4,
+	.width = 4,
+};
+
+static struct axxia_divclk clk_fab_div = {
+	.aclk.hw.init = &(struct clk_init_data){
+		.name = "clk_fab_div",
+		.parent_names = (const char *[]){
+			"clk_fab_pll"
+		},
+		.num_parents = 1,
+		.ops = &axxia_divclk_ops,
+	},
+	.reg   = 0x1000c,
+	.shift = 8,
+	.width = 4,
+};
+
+static struct axxia_divclk clk_per_div = {
+	.aclk.hw.init = &(struct clk_init_data){
+		.name = "clk_per_div",
+		.parent_names = (const char *[]){
+			"clk_sm1_pll"
+		},
+		.num_parents = 1,
+		.flags = CLK_IS_BASIC,
+		.ops = &axxia_divclk_ops,
+	},
+	.reg   = 0x1000c,
+	.shift = 12,
+	.width = 4,
+};
+
+static struct axxia_divclk clk_mmc_div = {
+	.aclk.hw.init = &(struct clk_init_data){
+		.name = "clk_mmc_div",
+		.parent_names = (const char *[]){
+			"clk_sm1_pll"
+		},
+		.num_parents = 1,
+		.flags = CLK_IS_BASIC,
+		.ops = &axxia_divclk_ops,
+	},
+	.reg   = 0x1000c,
+	.shift = 16,
+	.width = 4,
+};
+
+/*
+ * Clock MUXes
+ */
+
+static struct axxia_clkmux clk_cpu0_mux = {
+	.aclk.hw.init = &(struct clk_init_data){
+		.name = "clk_cpu0",
+		.parent_names = (const char *[]){
+			"clk_ref0",
+			"clk_cpu_pll",
+			"clk_cpu0_div",
+			"clk_cpu0_div"
+		},
+		.num_parents = 4,
+		.ops = &axxia_clkmux_ops,
+	},
+	.reg   = 0x10000,
+	.shift = 0,
+	.width = 2,
+};
+
+static struct axxia_clkmux clk_cpu1_mux = {
+	.aclk.hw.init = &(struct clk_init_data){
+		.name = "clk_cpu1",
+		.parent_names = (const char *[]){
+			"clk_ref0",
+			"clk_cpu_pll",
+			"clk_cpu1_div",
+			"clk_cpu1_div"
+		},
+		.num_parents = 4,
+		.ops = &axxia_clkmux_ops,
+	},
+	.reg   = 0x10000,
+	.shift = 2,
+	.width = 2,
+};
+
+static struct axxia_clkmux clk_cpu2_mux = {
+	.aclk.hw.init = &(struct clk_init_data){
+		.name = "clk_cpu2",
+		.parent_names = (const char *[]){
+			"clk_ref0",
+			"clk_cpu_pll",
+			"clk_cpu2_div",
+			"clk_cpu2_div"
+		},
+		.num_parents = 4,
+		.ops = &axxia_clkmux_ops,
+	},
+	.reg   = 0x10000,
+	.shift = 4,
+	.width = 2,
+};
+
+static struct axxia_clkmux clk_cpu3_mux = {
+	.aclk.hw.init = &(struct clk_init_data){
+		.name = "clk_cpu3",
+		.parent_names = (const char *[]){
+			"clk_ref0",
+			"clk_cpu_pll",
+			"clk_cpu3_div",
+			"clk_cpu3_div"
+		},
+		.num_parents = 4,
+		.ops = &axxia_clkmux_ops,
+	},
+	.reg   = 0x10000,
+	.shift = 6,
+	.width = 2,
+};
+
+static struct axxia_clkmux clk_nrcp_mux = {
+	.aclk.hw.init = &(struct clk_init_data){
+		.name = "clk_nrcp",
+		.parent_names = (const char *[]){
+			"clk_ref0",
+			"clk_sys_pll",
+			"clk_nrcp_div",
+			"clk_nrcp_div"
+		},
+		.num_parents = 4,
+		.ops = &axxia_clkmux_ops,
+	},
+	.reg   = 0x10004,
+	.shift = 0,
+	.width = 2,
+};
+
+static struct axxia_clkmux clk_sys_mux = {
+	.aclk.hw.init = &(struct clk_init_data){
+		.name = "clk_sys",
+		.parent_names = (const char *[]){
+			"clk_ref0",
+			"clk_sys_pll",
+			"clk_sys_div",
+			"clk_sys_div"
+		},
+		.num_parents = 4,
+		.ops = &axxia_clkmux_ops,
+	},
+	.reg   = 0x10004,
+	.shift = 2,
+	.width = 2,
+};
+
+static struct axxia_clkmux clk_fab_mux = {
+	.aclk.hw.init = &(struct clk_init_data){
+		.name = "clk_fab",
+		.parent_names = (const char *[]){
+			"clk_ref0",
+			"clk_fab_pll",
+			"clk_fab_div",
+			"clk_fab_div"
+		},
+		.num_parents = 4,
+		.ops = &axxia_clkmux_ops,
+	},
+	.reg   = 0x10004,
+	.shift = 4,
+	.width = 2,
+};
+
+static struct axxia_clkmux clk_per_mux = {
+	.aclk.hw.init = &(struct clk_init_data){
+		.name = "clk_per",
+		.parent_names = (const char *[]){
+			"clk_ref1",
+			"clk_per_div"
+		},
+		.num_parents = 2,
+		.ops = &axxia_clkmux_ops,
+	},
+	.reg   = 0x10004,
+	.shift = 6,
+	.width = 1,
+};
+
+static struct axxia_clkmux clk_mmc_mux = {
+	.aclk.hw.init = &(struct clk_init_data){
+		.name = "clk_mmc",
+		.parent_names = (const char *[]){
+			"clk_ref1",
+			"clk_mmc_div"
+		},
+		.num_parents = 2,
+		.ops = &axxia_clkmux_ops,
+	},
+	.reg   = 0x10004,
+	.shift = 9,
+	.width = 1,
+};
+
+/* Table of all supported clocks indexed by the clock identifiers from the
+ * device tree binding
+ */
+static struct axxia_clk *axmclk_clocks[] = {
+	[AXXIA_CLK_FAB_PLL]  = &clk_fab_pll.aclk,
+	[AXXIA_CLK_CPU_PLL]  = &clk_cpu_pll.aclk,
+	[AXXIA_CLK_SYS_PLL]  = &clk_sys_pll.aclk,
+	[AXXIA_CLK_SM0_PLL]  = &clk_sm0_pll.aclk,
+	[AXXIA_CLK_SM1_PLL]  = &clk_sm1_pll.aclk,
+	[AXXIA_CLK_FAB_DIV]  = &clk_fab_div.aclk,
+	[AXXIA_CLK_SYS_DIV]  = &clk_sys_div.aclk,
+	[AXXIA_CLK_NRCP_DIV] = &clk_nrcp_div.aclk,
+	[AXXIA_CLK_CPU0_DIV] = &clk_cpu0_div.aclk,
+	[AXXIA_CLK_CPU1_DIV] = &clk_cpu1_div.aclk,
+	[AXXIA_CLK_CPU2_DIV] = &clk_cpu2_div.aclk,
+	[AXXIA_CLK_CPU3_DIV] = &clk_cpu3_div.aclk,
+	[AXXIA_CLK_PER_DIV]  = &clk_per_div.aclk,
+	[AXXIA_CLK_MMC_DIV]  = &clk_mmc_div.aclk,
+	[AXXIA_CLK_FAB]      = &clk_fab_mux.aclk,
+	[AXXIA_CLK_SYS]      = &clk_sys_mux.aclk,
+	[AXXIA_CLK_NRCP]     = &clk_nrcp_mux.aclk,
+	[AXXIA_CLK_CPU0]     = &clk_cpu0_mux.aclk,
+	[AXXIA_CLK_CPU1]     = &clk_cpu1_mux.aclk,
+	[AXXIA_CLK_CPU2]     = &clk_cpu2_mux.aclk,
+	[AXXIA_CLK_CPU3]     = &clk_cpu3_mux.aclk,
+	[AXXIA_CLK_PER]      = &clk_per_mux.aclk,
+	[AXXIA_CLK_MMC]      = &clk_mmc_mux.aclk,
+};
+
+static const struct regmap_config axmclk_regmap_config = {
+	.reg_bits	= 32,
+	.reg_stride	= 4,
+	.val_bits	= 32,
+	.max_register	= 0x1fffc,
+	.fast_io	= true,
+};
+
+static const struct of_device_id axmclk_match_table[] = {
+	{ .compatible = "lsi,axm5516-clks" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, axmclk_match_table);
+
+struct axmclk_priv {
+	struct clk_onecell_data onecell;
+	struct clk *clks[];
+};
+
+static int axmclk_probe(struct platform_device *pdev)
+{
+	void __iomem *base;
+	struct resource *res;
+	int i, ret;
+	struct device *dev = &pdev->dev;
+	struct clk *clk;
+	struct regmap *regmap;
+	size_t num_clks;
+	struct axmclk_priv *priv;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	regmap = devm_regmap_init_mmio(dev, base, &axmclk_regmap_config);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	num_clks = ARRAY_SIZE(axmclk_clocks);
+	pr_info("axmclk: supporting %u clocks\n", num_clks);
+	priv = devm_kzalloc(dev, sizeof(*priv) + sizeof(*priv->clks) * num_clks,
+			    GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->onecell.clks = priv->clks;
+	priv->onecell.clk_num = num_clks;
+
+	/* Update each entry with the allocated regmap and register the clock
+	 * with the common clock framework
+	 */
+	for (i = 0; i < num_clks; i++) {
+		axmclk_clocks[i]->regmap = regmap;
+		clk = devm_clk_register(dev, &axmclk_clocks[i]->hw);
+		if (IS_ERR(clk))
+			return PTR_ERR(clk);
+		priv->clks[i] = clk;
+	}
+
+	ret = of_clk_add_provider(dev->of_node,
+				  of_clk_src_onecell_get, &priv->onecell);
+
+	return ret;
+}
+
+static int axmclk_remove(struct platform_device *pdev)
+{
+	of_clk_del_provider(pdev->dev.of_node);
+	return 0;
+}
+
+static struct platform_driver axmclk_driver = {
+	.probe		= axmclk_probe,
+	.remove		= axmclk_remove,
+	.driver		= {
+		.name	= "clk-axm5516",
+		.owner	= THIS_MODULE,
+		.of_match_table = axmclk_match_table,
+	},
+};
+
+static int __init axmclk_init(void)
+{
+	return platform_driver_register(&axmclk_driver);
+}
+core_initcall(axmclk_init);
+
+static void __exit axmclk_exit(void)
+{
+	platform_driver_unregister(&axmclk_driver);
+}
+module_exit(axmclk_exit);
+
+MODULE_DESCRIPTION("AXM5516 clock driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:clk-axm5516");
diff --git a/drivers/clk/clk-divider.c b/drivers/clk/clk-divider.c
index ec22112e569..18a9de29df0 100644
--- a/drivers/clk/clk-divider.c
+++ b/drivers/clk/clk-divider.c
@@ -43,6 +43,17 @@ static unsigned int _get_table_maxdiv(const struct clk_div_table *table)
 	return maxdiv;
 }
 
+static unsigned int _get_table_mindiv(const struct clk_div_table *table)
+{
+	unsigned int mindiv = UINT_MAX;
+	const struct clk_div_table *clkt;
+
+	for (clkt = table; clkt->div; clkt++)
+		if (clkt->div < mindiv)
+			mindiv = clkt->div;
+	return mindiv;
+}
+
 static unsigned int _get_maxdiv(struct clk_divider *divider)
 {
 	if (divider->flags & CLK_DIVIDER_ONE_BASED)
@@ -144,6 +155,103 @@ static bool _is_valid_div(struct clk_divider *divider, unsigned int div)
 	return true;
 }
 
+static int _round_up_table(const struct clk_div_table *table, int div)
+{
+	const struct clk_div_table *clkt;
+	int up = INT_MAX;
+
+	for (clkt = table; clkt->div; clkt++) {
+		if (clkt->div == div)
+			return clkt->div;
+		else if (clkt->div < div)
+			continue;
+
+		if ((clkt->div - div) < (up - div))
+			up = clkt->div;
+	}
+
+	return up;
+}
+
+static int _round_down_table(const struct clk_div_table *table, int div)
+{
+	const struct clk_div_table *clkt;
+	int down = _get_table_mindiv(table);
+
+	for (clkt = table; clkt->div; clkt++) {
+		if (clkt->div == div)
+			return clkt->div;
+		else if (clkt->div > div)
+			continue;
+
+		if ((div - clkt->div) < (div - down))
+			down = clkt->div;
+	}
+
+	return down;
+}
+
+static int _div_round_up(struct clk_divider *divider,
+		unsigned long parent_rate, unsigned long rate)
+{
+	int div = DIV_ROUND_UP(parent_rate, rate);
+
+	if (divider->flags & CLK_DIVIDER_POWER_OF_TWO)
+		div = __roundup_pow_of_two(div);
+	if (divider->table)
+		div = _round_up_table(divider->table, div);
+
+	return div;
+}
+
+static int _div_round_closest(struct clk_divider *divider,
+		unsigned long parent_rate, unsigned long rate)
+{
+	int up, down, div;
+
+	up = down = div = DIV_ROUND_CLOSEST(parent_rate, rate);
+
+	if (divider->flags & CLK_DIVIDER_POWER_OF_TWO) {
+		up = __roundup_pow_of_two(div);
+		down = __rounddown_pow_of_two(div);
+	} else if (divider->table) {
+		up = _round_up_table(divider->table, div);
+		down = _round_down_table(divider->table, div);
+	}
+
+	return (up - div) <= (div - down) ? up : down;
+}
+
+static int _div_round(struct clk_divider *divider, unsigned long parent_rate,
+		unsigned long rate)
+{
+	if (divider->flags & CLK_DIVIDER_ROUND_CLOSEST)
+		return _div_round_closest(divider, parent_rate, rate);
+
+	return _div_round_up(divider, parent_rate, rate);
+}
+
+static bool _is_best_div(struct clk_divider *divider,
+		unsigned long rate, unsigned long now, unsigned long best)
+{
+	if (divider->flags & CLK_DIVIDER_ROUND_CLOSEST)
+		return abs(rate - now) < abs(rate - best);
+
+	return now <= rate && now > best;
+}
+
+static int _next_div(struct clk_divider *divider, int div)
+{
+	div++;
+
+	if (divider->flags & CLK_DIVIDER_POWER_OF_TWO)
+		return __roundup_pow_of_two(div);
+	if (divider->table)
+		return _round_up_table(divider->table, div);
+
+	return div;
+}
+
 static int clk_divider_bestdiv(struct clk_hw *hw, unsigned long rate,
 		unsigned long *best_parent_rate)
 {
@@ -159,7 +267,7 @@ static int clk_divider_bestdiv(struct clk_hw *hw, unsigned long rate,
 
 	if (!(__clk_get_flags(hw->clk) & CLK_SET_RATE_PARENT)) {
 		parent_rate = *best_parent_rate;
-		bestdiv = DIV_ROUND_UP(parent_rate, rate);
+		bestdiv = _div_round(divider, parent_rate, rate);
 		bestdiv = bestdiv == 0 ? 1 : bestdiv;
 		bestdiv = bestdiv > maxdiv ? maxdiv : bestdiv;
 		return bestdiv;
@@ -171,7 +279,7 @@ static int clk_divider_bestdiv(struct clk_hw *hw, unsigned long rate,
 	 */
 	maxdiv = min(ULONG_MAX / rate, maxdiv);
 
-	for (i = 1; i <= maxdiv; i++) {
+	for (i = 1; i <= maxdiv; i = _next_div(divider, i)) {
 		if (!_is_valid_div(divider, i))
 			continue;
 		if (rate * i == parent_rate_saved) {
@@ -186,7 +294,7 @@ static int clk_divider_bestdiv(struct clk_hw *hw, unsigned long rate,
 		parent_rate = __clk_round_rate(__clk_get_parent(hw->clk),
 				MULT_ROUND_UP(rate, i));
 		now = DIV_ROUND_UP(parent_rate, i);
-		if (now <= rate && now > best) {
+		if (_is_best_div(divider, rate, now, best)) {
 			bestdiv = i;
 			best = now;
 			*best_parent_rate = parent_rate;
@@ -219,6 +327,10 @@ static int clk_divider_set_rate(struct clk_hw *hw, unsigned long rate,
 	u32 val;
 
 	div = DIV_ROUND_UP(parent_rate, rate);
+
+	if (!_is_valid_div(divider, div))
+		return -EINVAL;
+
 	value = _get_val(divider, div);
 
 	if (value > div_mask(divider))
@@ -249,6 +361,11 @@ const struct clk_ops clk_divider_ops = {
 };
 EXPORT_SYMBOL_GPL(clk_divider_ops);
 
+const struct clk_ops clk_divider_ro_ops = {
+	.recalc_rate = clk_divider_recalc_rate,
+};
+EXPORT_SYMBOL_GPL(clk_divider_ro_ops);
+
 static struct clk *_register_divider(struct device *dev, const char *name,
 		const char *parent_name, unsigned long flags,
 		void __iomem *reg, u8 shift, u8 width,
@@ -274,7 +391,10 @@ static struct clk *_register_divider(struct device *dev, const char *name,
 	}
 
 	init.name = name;
-	init.ops = &clk_divider_ops;
+	if (clk_divider_flags & CLK_DIVIDER_READ_ONLY)
+		init.ops = &clk_divider_ro_ops;
+	else
+		init.ops = &clk_divider_ops;
 	init.flags = flags | CLK_IS_BASIC;
 	init.parent_names = (parent_name ? &parent_name: NULL);
 	init.num_parents = (parent_name ? 1 : 0);
diff --git a/drivers/clk/clk-fractional-divider.c b/drivers/clk/clk-fractional-divider.c
new file mode 100644
index 00000000000..ede685ca0d2
--- /dev/null
+++ b/drivers/clk/clk-fractional-divider.c
@@ -0,0 +1,135 @@
+/*
+ * Copyright (C) 2014 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Adjustable fractional divider clock implementation.
+ * Output rate = (m / n) * parent_rate.
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/slab.h>
+#include <linux/gcd.h>
+
+#define to_clk_fd(_hw) container_of(_hw, struct clk_fractional_divider, hw)
+
+static unsigned long clk_fd_recalc_rate(struct clk_hw *hw,
+					unsigned long parent_rate)
+{
+	struct clk_fractional_divider *fd = to_clk_fd(hw);
+	unsigned long flags = 0;
+	u32 val, m, n;
+	u64 ret;
+
+	if (fd->lock)
+		spin_lock_irqsave(fd->lock, flags);
+
+	val = clk_readl(fd->reg);
+
+	if (fd->lock)
+		spin_unlock_irqrestore(fd->lock, flags);
+
+	m = (val & fd->mmask) >> fd->mshift;
+	n = (val & fd->nmask) >> fd->nshift;
+
+	ret = parent_rate * m;
+	do_div(ret, n);
+
+	return ret;
+}
+
+static long clk_fd_round_rate(struct clk_hw *hw, unsigned long rate,
+			      unsigned long *prate)
+{
+	struct clk_fractional_divider *fd = to_clk_fd(hw);
+	unsigned maxn = (fd->nmask >> fd->nshift) + 1;
+	unsigned div;
+
+	if (!rate || rate >= *prate)
+		return *prate;
+
+	div = gcd(*prate, rate);
+
+	while ((*prate / div) > maxn) {
+		div <<= 1;
+		rate <<= 1;
+	}
+
+	return rate;
+}
+
+static int clk_fd_set_rate(struct clk_hw *hw, unsigned long rate,
+			   unsigned long parent_rate)
+{
+	struct clk_fractional_divider *fd = to_clk_fd(hw);
+	unsigned long flags = 0;
+	unsigned long div;
+	unsigned n, m;
+	u32 val;
+
+	div = gcd(parent_rate, rate);
+	m = rate / div;
+	n = parent_rate / div;
+
+	if (fd->lock)
+		spin_lock_irqsave(fd->lock, flags);
+
+	val = clk_readl(fd->reg);
+	val &= ~(fd->mmask | fd->nmask);
+	val |= (m << fd->mshift) | (n << fd->nshift);
+	clk_writel(val, fd->reg);
+
+	if (fd->lock)
+		spin_unlock_irqrestore(fd->lock, flags);
+
+	return 0;
+}
+
+const struct clk_ops clk_fractional_divider_ops = {
+	.recalc_rate = clk_fd_recalc_rate,
+	.round_rate = clk_fd_round_rate,
+	.set_rate = clk_fd_set_rate,
+};
+EXPORT_SYMBOL_GPL(clk_fractional_divider_ops);
+
+struct clk *clk_register_fractional_divider(struct device *dev,
+		const char *name, const char *parent_name, unsigned long flags,
+		void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
+		u8 clk_divider_flags, spinlock_t *lock)
+{
+	struct clk_fractional_divider *fd;
+	struct clk_init_data init;
+	struct clk *clk;
+
+	fd = kzalloc(sizeof(*fd), GFP_KERNEL);
+	if (!fd) {
+		dev_err(dev, "could not allocate fractional divider clk\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	init.name = name;
+	init.ops = &clk_fractional_divider_ops;
+	init.flags = flags | CLK_IS_BASIC;
+	init.parent_names = parent_name ? &parent_name : NULL;
+	init.num_parents = parent_name ? 1 : 0;
+
+	fd->reg = reg;
+	fd->mshift = mshift;
+	fd->mmask = (BIT(mwidth) - 1) << mshift;
+	fd->nshift = nshift;
+	fd->nmask = (BIT(nwidth) - 1) << nshift;
+	fd->flags = clk_divider_flags;
+	fd->lock = lock;
+	fd->hw.init = &init;
+
+	clk = clk_register(dev, &fd->hw);
+	if (IS_ERR(clk))
+		kfree(fd);
+
+	return clk;
+}
+EXPORT_SYMBOL_GPL(clk_register_fractional_divider);
diff --git a/drivers/clk/clk-s2mps11.c b/drivers/clk/clk-s2mps11.c
index f2f62a1bf61..9b7b5859a42 100644
--- a/drivers/clk/clk-s2mps11.c
+++ b/drivers/clk/clk-s2mps11.c
@@ -1,7 +1,7 @@
 /*
  * clk-s2mps11.c - Clock driver for S2MPS11.
  *
- * Copyright (C) 2013 Samsung Electornics
+ * Copyright (C) 2013,2014 Samsung Electornics
  *
  * This program is free software; you can redistribute  it and/or modify it
  * under  the terms of  the GNU General  Public License as published by the
@@ -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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- *
  */
 
 #include <linux/module.h>
@@ -27,6 +23,7 @@
 #include <linux/clk-provider.h>
 #include <linux/platform_device.h>
 #include <linux/mfd/samsung/s2mps11.h>
+#include <linux/mfd/samsung/s2mps14.h>
 #include <linux/mfd/samsung/s5m8767.h>
 #include <linux/mfd/samsung/core.h>
 
@@ -44,6 +41,7 @@ enum {
 
 struct s2mps11_clk {
 	struct sec_pmic_dev *iodev;
+	struct device_node *clk_np;
 	struct clk_hw hw;
 	struct clk *clk;
 	struct clk_lookup *lookup;
@@ -125,7 +123,21 @@ static struct clk_init_data s2mps11_clks_init[S2MPS11_CLKS_NUM] = {
 	},
 };
 
-static struct device_node *s2mps11_clk_parse_dt(struct platform_device *pdev)
+static struct clk_init_data s2mps14_clks_init[S2MPS11_CLKS_NUM] = {
+	[S2MPS11_CLK_AP] = {
+		.name = "s2mps14_ap",
+		.ops = &s2mps11_clk_ops,
+		.flags = CLK_IS_ROOT,
+	},
+	[S2MPS11_CLK_BT] = {
+		.name = "s2mps14_bt",
+		.ops = &s2mps11_clk_ops,
+		.flags = CLK_IS_ROOT,
+	},
+};
+
+static struct device_node *s2mps11_clk_parse_dt(struct platform_device *pdev,
+		struct clk_init_data *clks_init)
 {
 	struct sec_pmic_dev *iodev = dev_get_drvdata(pdev->dev.parent);
 	struct device_node *clk_np;
@@ -140,14 +152,12 @@ static struct device_node *s2mps11_clk_parse_dt(struct platform_device *pdev)
 		return ERR_PTR(-EINVAL);
 	}
 
-	clk_table = devm_kzalloc(&pdev->dev, sizeof(struct clk *) *
-				 S2MPS11_CLKS_NUM, GFP_KERNEL);
-	if (!clk_table)
-		return ERR_PTR(-ENOMEM);
-
-	for (i = 0; i < S2MPS11_CLKS_NUM; i++)
+	for (i = 0; i < S2MPS11_CLKS_NUM; i++) {
+		if (!clks_init[i].name)
+			continue; /* Skip clocks not present in some devices */
 		of_property_read_string_index(clk_np, "clock-output-names", i,
-				&s2mps11_clks_init[i].name);
+				&clks_init[i].name);
+	}
 
 	return clk_np;
 }
@@ -156,8 +166,8 @@ static int s2mps11_clk_probe(struct platform_device *pdev)
 {
 	struct sec_pmic_dev *iodev = dev_get_drvdata(pdev->dev.parent);
 	struct s2mps11_clk *s2mps11_clks, *s2mps11_clk;
-	struct device_node *clk_np = NULL;
 	unsigned int s2mps11_reg;
+	struct clk_init_data *clks_init;
 	int i, ret = 0;
 	u32 val;
 
@@ -168,25 +178,39 @@ static int s2mps11_clk_probe(struct platform_device *pdev)
 
 	s2mps11_clk = s2mps11_clks;
 
-	clk_np = s2mps11_clk_parse_dt(pdev);
-	if (IS_ERR(clk_np))
-		return PTR_ERR(clk_np);
+	clk_table = devm_kzalloc(&pdev->dev, sizeof(struct clk *) *
+				 S2MPS11_CLKS_NUM, GFP_KERNEL);
+	if (!clk_table)
+		return -ENOMEM;
 
 	switch(platform_get_device_id(pdev)->driver_data) {
 	case S2MPS11X:
 		s2mps11_reg = S2MPS11_REG_RTC_CTRL;
+		clks_init = s2mps11_clks_init;
+		break;
+	case S2MPS14X:
+		s2mps11_reg = S2MPS14_REG_RTCCTRL;
+		clks_init = s2mps14_clks_init;
 		break;
 	case S5M8767X:
 		s2mps11_reg = S5M8767_REG_CTRL1;
+		clks_init = s2mps11_clks_init;
 		break;
 	default:
 		dev_err(&pdev->dev, "Invalid device type\n");
 		return -EINVAL;
 	};
 
+	/* Store clocks of_node in first element of s2mps11_clks array */
+	s2mps11_clks->clk_np = s2mps11_clk_parse_dt(pdev, clks_init);
+	if (IS_ERR(s2mps11_clks->clk_np))
+		return PTR_ERR(s2mps11_clks->clk_np);
+
 	for (i = 0; i < S2MPS11_CLKS_NUM; i++, s2mps11_clk++) {
+		if (!clks_init[i].name)
+			continue; /* Skip clocks not present in some devices */
 		s2mps11_clk->iodev = iodev;
-		s2mps11_clk->hw.init = &s2mps11_clks_init[i];
+		s2mps11_clk->hw.init = &clks_init[i];
 		s2mps11_clk->mask = 1 << i;
 		s2mps11_clk->reg = s2mps11_reg;
 
@@ -219,15 +243,18 @@ static int s2mps11_clk_probe(struct platform_device *pdev)
 		clkdev_add(s2mps11_clk->lookup);
 	}
 
-	if (clk_table) {
-		for (i = 0; i < S2MPS11_CLKS_NUM; i++)
-			clk_table[i] = s2mps11_clks[i].clk;
-
-		clk_data.clks = clk_table;
-		clk_data.clk_num = S2MPS11_CLKS_NUM;
-		of_clk_add_provider(clk_np, of_clk_src_onecell_get, &clk_data);
+	for (i = 0; i < S2MPS11_CLKS_NUM; i++) {
+		/* Skip clocks not present on S2MPS14 */
+		if (!clks_init[i].name)
+			continue;
+		clk_table[i] = s2mps11_clks[i].clk;
 	}
 
+	clk_data.clks = clk_table;
+	clk_data.clk_num = S2MPS11_CLKS_NUM;
+	of_clk_add_provider(s2mps11_clks->clk_np, of_clk_src_onecell_get,
+			&clk_data);
+
 	platform_set_drvdata(pdev, s2mps11_clks);
 
 	return ret;
@@ -250,14 +277,23 @@ static int s2mps11_clk_remove(struct platform_device *pdev)
 	struct s2mps11_clk *s2mps11_clks = platform_get_drvdata(pdev);
 	int i;
 
-	for (i = 0; i < S2MPS11_CLKS_NUM; i++)
+	of_clk_del_provider(s2mps11_clks[0].clk_np);
+	/* Drop the reference obtained in s2mps11_clk_parse_dt */
+	of_node_put(s2mps11_clks[0].clk_np);
+
+	for (i = 0; i < S2MPS11_CLKS_NUM; i++) {
+		/* Skip clocks not present on S2MPS14 */
+		if (!s2mps11_clks[i].lookup)
+			continue;
 		clkdev_drop(s2mps11_clks[i].lookup);
+	}
 
 	return 0;
 }
 
 static const struct platform_device_id s2mps11_clk_id[] = {
 	{ "s2mps11-clk", S2MPS11X},
+	{ "s2mps14-clk", S2MPS14X},
 	{ "s5m8767-clk", S5M8767X},
 	{ },
 };
diff --git a/drivers/clk/clk-si570.c b/drivers/clk/clk-si570.c
index 4bbbe32585e..fc167b3f891 100644
--- a/drivers/clk/clk-si570.c
+++ b/drivers/clk/clk-si570.c
@@ -526,6 +526,6 @@ static struct i2c_driver si570_driver = {
 module_i2c_driver(si570_driver);
 
 MODULE_AUTHOR("Guenter Roeck <guenter.roeck@ericsson.com>");
-MODULE_AUTHOR("Soeren Brinkmann <soren.brinkmann@xilinx.com");
+MODULE_AUTHOR("Soeren Brinkmann <soren.brinkmann@xilinx.com>");
 MODULE_DESCRIPTION("Si570 driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/clk/clk-u300.c b/drivers/clk/clk-u300.c
index 3efbdd078d1..406bfc1375b 100644
--- a/drivers/clk/clk-u300.c
+++ b/drivers/clk/clk-u300.c
@@ -1168,6 +1168,7 @@ static const struct of_device_id u300_clk_match[] __initconst = {
 		.compatible = "stericsson,u300-syscon-mclk",
 		.data = of_u300_syscon_mclk_init,
 	},
+	{}
 };
 
 
diff --git a/drivers/clk/clk.c b/drivers/clk/clk.c
index dff0373f53c..8b73edef151 100644
--- a/drivers/clk/clk.c
+++ b/drivers/clk/clk.c
@@ -106,12 +106,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 %-10lu %-11lu",
+	seq_printf(s, "%*s%-*s %11d %12d %11lu %10lu\n",
 		   level * 3 + 1, "",
 		   30 - level * 3, c->name,
 		   c->enable_count, c->prepare_count, clk_get_rate(c),
 		   clk_get_accuracy(c));
-	seq_printf(s, "\n");
 }
 
 static void clk_summary_show_subtree(struct seq_file *s, struct clk *c,
@@ -132,8 +131,8 @@ static int clk_summary_show(struct seq_file *s, void *data)
 {
 	struct clk *c;
 
-	seq_printf(s, "   clock                        enable_cnt  prepare_cnt  rate        accuracy\n");
-	seq_printf(s, "---------------------------------------------------------------------------------\n");
+	seq_puts(s, "   clock                         enable_cnt  prepare_cnt        rate   accuracy\n");
+	seq_puts(s, "--------------------------------------------------------------------------------\n");
 
 	clk_prepare_lock();
 
@@ -822,6 +821,9 @@ void __clk_unprepare(struct clk *clk)
  */
 void clk_unprepare(struct clk *clk)
 {
+	if (IS_ERR_OR_NULL(clk))
+		return;
+
 	clk_prepare_lock();
 	__clk_unprepare(clk);
 	clk_prepare_unlock();
@@ -883,9 +885,6 @@ static void __clk_disable(struct clk *clk)
 	if (!clk)
 		return;
 
-	if (WARN_ON(IS_ERR(clk)))
-		return;
-
 	if (WARN_ON(clk->enable_count == 0))
 		return;
 
@@ -914,6 +913,9 @@ void clk_disable(struct clk *clk)
 {
 	unsigned long flags;
 
+	if (IS_ERR_OR_NULL(clk))
+		return;
+
 	flags = clk_enable_lock();
 	__clk_disable(clk);
 	clk_enable_unlock(flags);
@@ -1004,6 +1006,7 @@ unsigned long __clk_round_rate(struct clk *clk, unsigned long rate)
 	else
 		return clk->rate;
 }
+EXPORT_SYMBOL_GPL(__clk_round_rate);
 
 /**
  * clk_round_rate - round the given rate for a clk
@@ -1115,6 +1118,13 @@ long clk_get_accuracy(struct clk *clk)
 }
 EXPORT_SYMBOL_GPL(clk_get_accuracy);
 
+static unsigned long clk_recalc(struct clk *clk, unsigned long parent_rate)
+{
+	if (clk->ops->recalc_rate)
+		return clk->ops->recalc_rate(clk->hw, parent_rate);
+	return parent_rate;
+}
+
 /**
  * __clk_recalc_rates
  * @clk: first clk in the subtree
@@ -1140,10 +1150,7 @@ static void __clk_recalc_rates(struct clk *clk, unsigned long msg)
 	if (clk->parent)
 		parent_rate = clk->parent->rate;
 
-	if (clk->ops->recalc_rate)
-		clk->rate = clk->ops->recalc_rate(clk->hw, parent_rate);
-	else
-		clk->rate = parent_rate;
+	clk->rate = clk_recalc(clk, parent_rate);
 
 	/*
 	 * ignore NOTIFY_STOP and NOTIFY_BAD return values for POST_RATE_CHANGE
@@ -1334,10 +1341,7 @@ static int __clk_speculate_rates(struct clk *clk, unsigned long parent_rate)
 	unsigned long new_rate;
 	int ret = NOTIFY_DONE;
 
-	if (clk->ops->recalc_rate)
-		new_rate = clk->ops->recalc_rate(clk->hw, parent_rate);
-	else
-		new_rate = parent_rate;
+	new_rate = clk_recalc(clk, parent_rate);
 
 	/* abort rate change if a driver returns NOTIFY_BAD or NOTIFY_STOP */
 	if (clk->notifier_count)
@@ -1373,10 +1377,7 @@ static void clk_calc_subtree(struct clk *clk, unsigned long new_rate,
 		new_parent->new_child = clk;
 
 	hlist_for_each_entry(child, &clk->children, child_node) {
-		if (child->ops->recalc_rate)
-			child->new_rate = child->ops->recalc_rate(child->hw, new_rate);
-		else
-			child->new_rate = new_rate;
+		child->new_rate = clk_recalc(child, new_rate);
 		clk_calc_subtree(child, child->new_rate, NULL, 0);
 	}
 }
@@ -1524,10 +1525,7 @@ static void clk_change_rate(struct clk *clk)
 	if (!skip_set_rate && clk->ops->set_rate)
 		clk->ops->set_rate(clk->hw, clk->new_rate, best_parent_rate);
 
-	if (clk->ops->recalc_rate)
-		clk->rate = clk->ops->recalc_rate(clk->hw, best_parent_rate);
-	else
-		clk->rate = best_parent_rate;
+	clk->rate = clk_recalc(clk, best_parent_rate);
 
 	if (clk->notifier_count && old_rate != clk->rate)
 		__clk_notify(clk, POST_RATE_CHANGE, old_rate, clk->rate);
@@ -1716,9 +1714,6 @@ int clk_set_parent(struct clk *clk, struct clk *parent)
 	if (!clk)
 		return 0;
 
-	if (!clk->ops)
-		return -EINVAL;
-
 	/* verify ops for for multi-parent clks */
 	if ((clk->num_parents > 1) && (!clk->ops->set_parent))
 		return -ENOSYS;
@@ -1984,9 +1979,28 @@ struct clk *__clk_register(struct device *dev, struct clk_hw *hw)
 }
 EXPORT_SYMBOL_GPL(__clk_register);
 
-static int _clk_register(struct device *dev, struct clk_hw *hw, struct clk *clk)
+/**
+ * clk_register - allocate a new clock, register it and return an opaque cookie
+ * @dev: device that is registering this clock
+ * @hw: link to hardware-specific clock data
+ *
+ * clk_register is the primary interface for populating the clock tree with new
+ * clock nodes.  It returns a pointer to the newly allocated struct clk which
+ * cannot be dereferenced by driver code but may be used in conjuction with the
+ * rest of the clock API.  In the event of an error clk_register will return an
+ * error code; drivers must test for an error code after calling clk_register.
+ */
+struct clk *clk_register(struct device *dev, struct clk_hw *hw)
 {
 	int i, ret;
+	struct clk *clk;
+
+	clk = kzalloc(sizeof(*clk), GFP_KERNEL);
+	if (!clk) {
+		pr_err("%s: could not allocate clk\n", __func__);
+		ret = -ENOMEM;
+		goto fail_out;
+	}
 
 	clk->name = kstrdup(hw->init->name, GFP_KERNEL);
 	if (!clk->name) {
@@ -2026,7 +2040,7 @@ static int _clk_register(struct device *dev, struct clk_hw *hw, struct clk *clk)
 
 	ret = __clk_init(dev, clk);
 	if (!ret)
-		return 0;
+		return clk;
 
 fail_parent_names_copy:
 	while (--i >= 0)
@@ -2035,36 +2049,6 @@ fail_parent_names_copy:
 fail_parent_names:
 	kfree(clk->name);
 fail_name:
-	return ret;
-}
-
-/**
- * clk_register - allocate a new clock, register it and return an opaque cookie
- * @dev: device that is registering this clock
- * @hw: link to hardware-specific clock data
- *
- * clk_register is the primary interface for populating the clock tree with new
- * clock nodes.  It returns a pointer to the newly allocated struct clk which
- * cannot be dereferenced by driver code but may be used in conjuction with the
- * rest of the clock API.  In the event of an error clk_register will return an
- * error code; drivers must test for an error code after calling clk_register.
- */
-struct clk *clk_register(struct device *dev, struct clk_hw *hw)
-{
-	int ret;
-	struct clk *clk;
-
-	clk = kzalloc(sizeof(*clk), GFP_KERNEL);
-	if (!clk) {
-		pr_err("%s: could not allocate clk\n", __func__);
-		ret = -ENOMEM;
-		goto fail_out;
-	}
-
-	ret = _clk_register(dev, hw, clk);
-	if (!ret)
-		return clk;
-
 	kfree(clk);
 fail_out:
 	return ERR_PTR(ret);
@@ -2151,9 +2135,10 @@ void clk_unregister(struct clk *clk)
 
 	if (!hlist_empty(&clk->children)) {
 		struct clk *child;
+		struct hlist_node *t;
 
 		/* Reparent all children to the orphan list. */
-		hlist_for_each_entry(child, &clk->children, child_node)
+		hlist_for_each_entry_safe(child, t, &clk->children, child_node)
 			clk_set_parent(child, NULL);
 	}
 
@@ -2173,7 +2158,7 @@ EXPORT_SYMBOL_GPL(clk_unregister);
 
 static void devm_clk_release(struct device *dev, void *res)
 {
-	clk_unregister(res);
+	clk_unregister(*(struct clk **)res);
 }
 
 /**
@@ -2188,18 +2173,18 @@ static void devm_clk_release(struct device *dev, void *res)
 struct clk *devm_clk_register(struct device *dev, struct clk_hw *hw)
 {
 	struct clk *clk;
-	int ret;
+	struct clk **clkp;
 
-	clk = devres_alloc(devm_clk_release, sizeof(*clk), GFP_KERNEL);
-	if (!clk)
+	clkp = devres_alloc(devm_clk_release, sizeof(*clkp), GFP_KERNEL);
+	if (!clkp)
 		return ERR_PTR(-ENOMEM);
 
-	ret = _clk_register(dev, hw, clk);
-	if (!ret) {
-		devres_add(dev, clk);
+	clk = clk_register(dev, hw);
+	if (!IS_ERR(clk)) {
+		*clkp = clk;
+		devres_add(dev, clkp);
 	} else {
-		devres_free(clk);
-		clk = ERR_PTR(ret);
+		devres_free(clkp);
 	}
 
 	return clk;
diff --git a/drivers/clk/clk.h b/drivers/clk/clk.h
index 795cc9f0dac..c798138f023 100644
--- a/drivers/clk/clk.h
+++ b/drivers/clk/clk.h
@@ -10,6 +10,7 @@
  */
 
 #if defined(CONFIG_OF) && defined(CONFIG_COMMON_CLK)
+struct clk *of_clk_get_by_clkspec(struct of_phandle_args *clkspec);
 struct clk *__of_clk_get_from_provider(struct of_phandle_args *clkspec);
 void of_clk_lock(void);
 void of_clk_unlock(void);
diff --git a/drivers/clk/clkdev.c b/drivers/clk/clkdev.c
index a360b2eca5c..f890b901c6b 100644
--- a/drivers/clk/clkdev.c
+++ b/drivers/clk/clkdev.c
@@ -27,6 +27,32 @@ static LIST_HEAD(clocks);
 static DEFINE_MUTEX(clocks_mutex);
 
 #if defined(CONFIG_OF) && defined(CONFIG_COMMON_CLK)
+
+/**
+ * of_clk_get_by_clkspec() - Lookup a clock form a clock provider
+ * @clkspec: pointer to a clock specifier data structure
+ *
+ * This function looks up a struct clk from the registered list of clock
+ * providers, an input is a clock specifier data structure as returned
+ * from the of_parse_phandle_with_args() function call.
+ */
+struct clk *of_clk_get_by_clkspec(struct of_phandle_args *clkspec)
+{
+	struct clk *clk;
+
+	if (!clkspec)
+		return ERR_PTR(-EINVAL);
+
+	of_clk_lock();
+	clk = __of_clk_get_from_provider(clkspec);
+
+	if (!IS_ERR(clk) && !__clk_get(clk))
+		clk = ERR_PTR(-ENOENT);
+
+	of_clk_unlock();
+	return clk;
+}
+
 struct clk *of_clk_get(struct device_node *np, int index)
 {
 	struct of_phandle_args clkspec;
@@ -41,13 +67,7 @@ struct clk *of_clk_get(struct device_node *np, int index)
 	if (rc)
 		return ERR_PTR(rc);
 
-	of_clk_lock();
-	clk = __of_clk_get_from_provider(&clkspec);
-
-	if (!IS_ERR(clk) && !__clk_get(clk))
-		clk = ERR_PTR(-ENOENT);
-
-	of_clk_unlock();
+	clk = of_clk_get_by_clkspec(&clkspec);
 	of_node_put(clkspec.np);
 	return clk;
 }
diff --git a/drivers/clk/hisilicon/Makefile b/drivers/clk/hisilicon/Makefile
index 40b33c6a825..038c02f4d0e 100644
--- a/drivers/clk/hisilicon/Makefile
+++ b/drivers/clk/hisilicon/Makefile
@@ -6,3 +6,4 @@ obj-y	+= clk.o clkgate-separated.o
 
 obj-$(CONFIG_ARCH_HI3xxx)	+= clk-hi3620.o
 obj-$(CONFIG_ARCH_HIP04)	+= clk-hip04.o
+obj-$(CONFIG_ARCH_HIX5HD2)	+= clk-hix5hd2.o
diff --git a/drivers/clk/hisilicon/clk-hix5hd2.c b/drivers/clk/hisilicon/clk-hix5hd2.c
new file mode 100644
index 00000000000..e5fcfb4e32e
--- /dev/null
+++ b/drivers/clk/hisilicon/clk-hix5hd2.c
@@ -0,0 +1,101 @@
+/*
+ * 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/of_address.h>
+#include <dt-bindings/clock/hix5hd2-clock.h>
+#include "clk.h"
+
+static struct hisi_fixed_rate_clock hix5hd2_fixed_rate_clks[] __initdata = {
+	{ HIX5HD2_FIXED_1200M, "1200m", NULL, CLK_IS_ROOT, 1200000000, },
+	{ HIX5HD2_FIXED_400M, "400m", NULL, CLK_IS_ROOT, 400000000, },
+	{ HIX5HD2_FIXED_48M, "48m", NULL, CLK_IS_ROOT, 48000000, },
+	{ HIX5HD2_FIXED_24M, "24m", NULL, CLK_IS_ROOT, 24000000, },
+	{ HIX5HD2_FIXED_600M, "600m", NULL, CLK_IS_ROOT, 600000000, },
+	{ HIX5HD2_FIXED_300M, "300m", NULL, CLK_IS_ROOT, 300000000, },
+	{ HIX5HD2_FIXED_75M, "75m", NULL, CLK_IS_ROOT, 75000000, },
+	{ HIX5HD2_FIXED_200M, "200m", NULL, CLK_IS_ROOT, 200000000, },
+	{ HIX5HD2_FIXED_100M, "100m", NULL, CLK_IS_ROOT, 100000000, },
+	{ HIX5HD2_FIXED_40M, "40m", NULL, CLK_IS_ROOT, 40000000, },
+	{ HIX5HD2_FIXED_150M, "150m", NULL, CLK_IS_ROOT, 150000000, },
+	{ HIX5HD2_FIXED_1728M, "1728m", NULL, CLK_IS_ROOT, 1728000000, },
+	{ HIX5HD2_FIXED_28P8M, "28p8m", NULL, CLK_IS_ROOT, 28000000, },
+	{ HIX5HD2_FIXED_432M, "432m", NULL, CLK_IS_ROOT, 432000000, },
+	{ HIX5HD2_FIXED_345P6M, "345p6m", NULL, CLK_IS_ROOT, 345000000, },
+	{ HIX5HD2_FIXED_288M, "288m", NULL, CLK_IS_ROOT, 288000000, },
+	{ HIX5HD2_FIXED_60M,	"60m", NULL, CLK_IS_ROOT, 60000000, },
+	{ HIX5HD2_FIXED_750M, "750m", NULL, CLK_IS_ROOT, 750000000, },
+	{ HIX5HD2_FIXED_500M, "500m", NULL, CLK_IS_ROOT, 500000000, },
+	{ HIX5HD2_FIXED_54M,	"54m", NULL, CLK_IS_ROOT, 54000000, },
+	{ HIX5HD2_FIXED_27M, "27m", NULL, CLK_IS_ROOT, 27000000, },
+	{ HIX5HD2_FIXED_1500M, "1500m", NULL, CLK_IS_ROOT, 1500000000, },
+	{ HIX5HD2_FIXED_375M, "375m", NULL, CLK_IS_ROOT, 375000000, },
+	{ HIX5HD2_FIXED_187M, "187m", NULL, CLK_IS_ROOT, 187000000, },
+	{ HIX5HD2_FIXED_250M, "250m", NULL, CLK_IS_ROOT, 250000000, },
+	{ HIX5HD2_FIXED_125M, "125m", NULL, CLK_IS_ROOT, 125000000, },
+	{ HIX5HD2_FIXED_2P02M, "2m", NULL, CLK_IS_ROOT, 2000000, },
+	{ HIX5HD2_FIXED_50M, "50m", NULL, CLK_IS_ROOT, 50000000, },
+	{ HIX5HD2_FIXED_25M, "25m", NULL, CLK_IS_ROOT, 25000000, },
+	{ HIX5HD2_FIXED_83M, "83m", NULL, CLK_IS_ROOT, 83333333, },
+};
+
+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 = {
+		"75m", "100m", "50m", "15m", };
+static u32 sdio1_mux_table[] = {0, 1, 2, 3};
+
+static const char *fephy_mux_p[] __initconst = { "25m", "125m"};
+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_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*/
+	{ 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*/
+	{ 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, },
+};
+
+static void __init hix5hd2_clk_init(struct device_node *np)
+{
+	struct hisi_clock_data *clk_data;
+
+	clk_data = hisi_clk_init(np, HIX5HD2_NR_CLKS);
+	if (!clk_data)
+		return;
+
+	hisi_clk_register_fixed_rate(hix5hd2_fixed_rate_clks,
+				     ARRAY_SIZE(hix5hd2_fixed_rate_clks),
+				     clk_data);
+	hisi_clk_register_mux(hix5hd2_mux_clks, ARRAY_SIZE(hix5hd2_mux_clks),
+					clk_data);
+	hisi_clk_register_gate(hix5hd2_gate_clks,
+			ARRAY_SIZE(hix5hd2_gate_clks), clk_data);
+}
+
+CLK_OF_DECLARE(hix5hd2_clk, "hisilicon,hix5hd2-clock", hix5hd2_clk_init);
diff --git a/drivers/clk/hisilicon/clk.c b/drivers/clk/hisilicon/clk.c
index 276f672e7b1..a078e84f7b0 100644
--- a/drivers/clk/hisilicon/clk.c
+++ b/drivers/clk/hisilicon/clk.c
@@ -127,11 +127,14 @@ void __init hisi_clk_register_mux(struct hisi_mux_clock *clks,
 	int i;
 
 	for (i = 0; i < nums; i++) {
-		clk = clk_register_mux(NULL, clks[i].name, clks[i].parent_names,
-				       clks[i].num_parents, clks[i].flags,
-				       base + clks[i].offset, clks[i].shift,
-				       clks[i].width, clks[i].mux_flags,
-				       &hisi_clk_lock);
+		u32 mask = BIT(clks[i].width) - 1;
+
+		clk = clk_register_mux_table(NULL, clks[i].name,
+					clks[i].parent_names,
+					clks[i].num_parents, clks[i].flags,
+					base + clks[i].offset, clks[i].shift,
+					mask, clks[i].mux_flags,
+					clks[i].table, &hisi_clk_lock);
 		if (IS_ERR(clk)) {
 			pr_err("%s: failed to register clock %s\n",
 			       __func__, clks[i].name);
@@ -174,6 +177,34 @@ void __init hisi_clk_register_divider(struct hisi_divider_clock *clks,
 	}
 }
 
+void __init hisi_clk_register_gate(struct hisi_gate_clock *clks,
+				       int nums, struct hisi_clock_data *data)
+{
+	struct clk *clk;
+	void __iomem *base = data->base;
+	int i;
+
+	for (i = 0; i < nums; i++) {
+		clk = clk_register_gate(NULL, clks[i].name,
+						clks[i].parent_name,
+						clks[i].flags,
+						base + clks[i].offset,
+						clks[i].bit_idx,
+						clks[i].gate_flags,
+						&hisi_clk_lock);
+		if (IS_ERR(clk)) {
+			pr_err("%s: failed to register clock %s\n",
+			       __func__, clks[i].name);
+			continue;
+		}
+
+		if (clks[i].alias)
+			clk_register_clkdev(clk, clks[i].alias, NULL);
+
+		data->clk_data.clks[clks[i].id] = clk;
+	}
+}
+
 void __init hisi_clk_register_gate_sep(struct hisi_gate_clock *clks,
 				       int nums, struct hisi_clock_data *data)
 {
diff --git a/drivers/clk/hisilicon/clk.h b/drivers/clk/hisilicon/clk.h
index 43fa5da88f0..31083ffc065 100644
--- a/drivers/clk/hisilicon/clk.h
+++ b/drivers/clk/hisilicon/clk.h
@@ -62,6 +62,7 @@ struct hisi_mux_clock {
 	u8			shift;
 	u8			width;
 	u8			mux_flags;
+	u32			*table;
 	const char		*alias;
 };
 
@@ -103,6 +104,8 @@ void __init hisi_clk_register_mux(struct hisi_mux_clock *, int,
 				struct hisi_clock_data *);
 void __init hisi_clk_register_divider(struct hisi_divider_clock *,
 				int, struct hisi_clock_data *);
+void __init hisi_clk_register_gate(struct hisi_gate_clock *,
+					int, struct hisi_clock_data *);
 void __init hisi_clk_register_gate_sep(struct hisi_gate_clock *,
 					int, struct hisi_clock_data *);
 #endif	/* __HISI_CLK_H */
diff --git a/drivers/clk/mvebu/Kconfig b/drivers/clk/mvebu/Kconfig
index 693f7be129f..3b34dba9178 100644
--- a/drivers/clk/mvebu/Kconfig
+++ b/drivers/clk/mvebu/Kconfig
@@ -34,3 +34,7 @@ config DOVE_CLK
 config KIRKWOOD_CLK
 	bool
 	select MVEBU_CLK_COMMON
+
+config ORION_CLK
+	bool
+	select MVEBU_CLK_COMMON
diff --git a/drivers/clk/mvebu/Makefile b/drivers/clk/mvebu/Makefile
index 4c66162fb0b..a9a56fc0190 100644
--- a/drivers/clk/mvebu/Makefile
+++ b/drivers/clk/mvebu/Makefile
@@ -8,3 +8,4 @@ obj-$(CONFIG_ARMADA_38X_CLK)	+= armada-38x.o
 obj-$(CONFIG_ARMADA_XP_CLK)	+= armada-xp.o
 obj-$(CONFIG_DOVE_CLK)		+= dove.o
 obj-$(CONFIG_KIRKWOOD_CLK)	+= kirkwood.o
+obj-$(CONFIG_ORION_CLK)		+= orion.o
diff --git a/drivers/clk/mvebu/orion.c b/drivers/clk/mvebu/orion.c
new file mode 100644
index 00000000000..fd129566c1c
--- /dev/null
+++ b/drivers/clk/mvebu/orion.c
@@ -0,0 +1,210 @@
+/*
+ * Marvell Orion SoC clocks
+ *
+ * Copyright (C) 2014 Thomas Petazzoni
+ *
+ * Thomas Petazzoni <thomas.petazzoni@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/kernel.h>
+#include <linux/clk-provider.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include "common.h"
+
+static const struct coreclk_ratio orion_coreclk_ratios[] __initconst = {
+	{ .id = 0, .name = "ddrclk", }
+};
+
+/*
+ * Orion 5182
+ */
+
+#define SAR_MV88F5182_TCLK_FREQ      8
+#define SAR_MV88F5182_TCLK_FREQ_MASK 0x3
+
+static u32 __init mv88f5182_get_tclk_freq(void __iomem *sar)
+{
+	u32 opt = (readl(sar) >> SAR_MV88F5182_TCLK_FREQ) &
+		SAR_MV88F5182_TCLK_FREQ_MASK;
+	if (opt == 1)
+		return 150000000;
+	else if (opt == 2)
+		return 166666667;
+	else
+		return 0;
+}
+
+#define SAR_MV88F5182_CPU_FREQ       4
+#define SAR_MV88F5182_CPU_FREQ_MASK  0xf
+
+static u32 __init mv88f5182_get_cpu_freq(void __iomem *sar)
+{
+	u32 opt = (readl(sar) >> SAR_MV88F5182_CPU_FREQ) &
+		SAR_MV88F5182_CPU_FREQ_MASK;
+	if (opt == 0)
+		return 333333333;
+	else if (opt == 1 || opt == 2)
+		return 400000000;
+	else if (opt == 3)
+		return 500000000;
+	else
+		return 0;
+}
+
+static void __init mv88f5182_get_clk_ratio(void __iomem *sar, int id,
+					   int *mult, int *div)
+{
+	u32 opt = (readl(sar) >> SAR_MV88F5182_CPU_FREQ) &
+		SAR_MV88F5182_CPU_FREQ_MASK;
+	if (opt == 0 || opt == 1) {
+		*mult = 1;
+		*div  = 2;
+	} else if (opt == 2 || opt == 3) {
+		*mult = 1;
+		*div  = 3;
+	} else {
+		*mult = 0;
+		*div  = 1;
+	}
+}
+
+static const struct coreclk_soc_desc mv88f5182_coreclks = {
+	.get_tclk_freq = mv88f5182_get_tclk_freq,
+	.get_cpu_freq = mv88f5182_get_cpu_freq,
+	.get_clk_ratio = mv88f5182_get_clk_ratio,
+	.ratios = orion_coreclk_ratios,
+	.num_ratios = ARRAY_SIZE(orion_coreclk_ratios),
+};
+
+static void __init mv88f5182_clk_init(struct device_node *np)
+{
+	return mvebu_coreclk_setup(np, &mv88f5182_coreclks);
+}
+
+CLK_OF_DECLARE(mv88f5182_clk, "marvell,mv88f5182-core-clock", mv88f5182_clk_init);
+
+/*
+ * Orion 5281
+ */
+
+static u32 __init mv88f5281_get_tclk_freq(void __iomem *sar)
+{
+	/* On 5281, tclk is always 166 Mhz */
+	return 166666667;
+}
+
+#define SAR_MV88F5281_CPU_FREQ       4
+#define SAR_MV88F5281_CPU_FREQ_MASK  0xf
+
+static u32 __init mv88f5281_get_cpu_freq(void __iomem *sar)
+{
+	u32 opt = (readl(sar) >> SAR_MV88F5281_CPU_FREQ) &
+		SAR_MV88F5281_CPU_FREQ_MASK;
+	if (opt == 1 || opt == 2)
+		return 400000000;
+	else if (opt == 3)
+		return 500000000;
+	else
+		return 0;
+}
+
+static void __init mv88f5281_get_clk_ratio(void __iomem *sar, int id,
+					   int *mult, int *div)
+{
+	u32 opt = (readl(sar) >> SAR_MV88F5281_CPU_FREQ) &
+		SAR_MV88F5281_CPU_FREQ_MASK;
+	if (opt == 1) {
+		*mult = 1;
+		*div = 2;
+	} else if (opt == 2 || opt == 3) {
+		*mult = 1;
+		*div = 3;
+	} else {
+		*mult = 0;
+		*div = 1;
+	}
+}
+
+static const struct coreclk_soc_desc mv88f5281_coreclks = {
+	.get_tclk_freq = mv88f5281_get_tclk_freq,
+	.get_cpu_freq = mv88f5281_get_cpu_freq,
+	.get_clk_ratio = mv88f5281_get_clk_ratio,
+	.ratios = orion_coreclk_ratios,
+	.num_ratios = ARRAY_SIZE(orion_coreclk_ratios),
+};
+
+static void __init mv88f5281_clk_init(struct device_node *np)
+{
+	return mvebu_coreclk_setup(np, &mv88f5281_coreclks);
+}
+
+CLK_OF_DECLARE(mv88f5281_clk, "marvell,mv88f5281-core-clock", mv88f5281_clk_init);
+
+/*
+ * Orion 6183
+ */
+
+#define SAR_MV88F6183_TCLK_FREQ      9
+#define SAR_MV88F6183_TCLK_FREQ_MASK 0x1
+
+static u32 __init mv88f6183_get_tclk_freq(void __iomem *sar)
+{
+	u32 opt = (readl(sar) >> SAR_MV88F6183_TCLK_FREQ) &
+		SAR_MV88F6183_TCLK_FREQ_MASK;
+	if (opt == 0)
+		return 133333333;
+	else if (opt == 1)
+		return 166666667;
+	else
+		return 0;
+}
+
+#define SAR_MV88F6183_CPU_FREQ       1
+#define SAR_MV88F6183_CPU_FREQ_MASK  0x3f
+
+static u32 __init mv88f6183_get_cpu_freq(void __iomem *sar)
+{
+	u32 opt = (readl(sar) >> SAR_MV88F6183_CPU_FREQ) &
+		SAR_MV88F6183_CPU_FREQ_MASK;
+	if (opt == 9)
+		return 333333333;
+	else if (opt == 17)
+		return 400000000;
+	else
+		return 0;
+}
+
+static void __init mv88f6183_get_clk_ratio(void __iomem *sar, int id,
+					   int *mult, int *div)
+{
+	u32 opt = (readl(sar) >> SAR_MV88F6183_CPU_FREQ) &
+		SAR_MV88F6183_CPU_FREQ_MASK;
+	if (opt == 9 || opt == 17) {
+		*mult = 1;
+		*div  = 2;
+	} else {
+		*mult = 0;
+		*div  = 1;
+	}
+}
+
+static const struct coreclk_soc_desc mv88f6183_coreclks = {
+	.get_tclk_freq = mv88f6183_get_tclk_freq,
+	.get_cpu_freq = mv88f6183_get_cpu_freq,
+	.get_clk_ratio = mv88f6183_get_clk_ratio,
+	.ratios = orion_coreclk_ratios,
+	.num_ratios = ARRAY_SIZE(orion_coreclk_ratios),
+};
+
+
+static void __init mv88f6183_clk_init(struct device_node *np)
+{
+	return mvebu_coreclk_setup(np, &mv88f6183_coreclks);
+}
+
+CLK_OF_DECLARE(mv88f6183_clk, "marvell,mv88f6183-core-clock", mv88f6183_clk_init);
diff --git a/drivers/clk/qcom/Kconfig b/drivers/clk/qcom/Kconfig
index 995bcfa021a..7f696b7d442 100644
--- a/drivers/clk/qcom/Kconfig
+++ b/drivers/clk/qcom/Kconfig
@@ -13,10 +13,10 @@ config MSM_GCC_8660
 	  i2c, USB, SD/eMMC, etc.
 
 config MSM_GCC_8960
-	tristate "MSM8960 Global Clock Controller"
+	tristate "APQ8064/MSM8960 Global Clock Controller"
 	depends on COMMON_CLK_QCOM
 	help
-	  Support for the global clock controller on msm8960 devices.
+	  Support for the global clock controller on apq8064/msm8960 devices.
 	  Say Y if you want to use peripheral devices such as UART, SPI,
 	  i2c, USB, SD/eMMC, SATA, PCIe, etc.
 
diff --git a/drivers/clk/qcom/Makefile b/drivers/clk/qcom/Makefile
index f60db2ef1ae..689e05bf4f9 100644
--- a/drivers/clk/qcom/Makefile
+++ b/drivers/clk/qcom/Makefile
@@ -1,5 +1,6 @@
 obj-$(CONFIG_COMMON_CLK_QCOM) += clk-qcom.o
 
+clk-qcom-y += common.o
 clk-qcom-y += clk-regmap.o
 clk-qcom-y += clk-pll.o
 clk-qcom-y += clk-rcg.o
diff --git a/drivers/clk/qcom/clk-rcg.h b/drivers/clk/qcom/clk-rcg.h
index 1d6b6dece32..b9ec11dfd1b 100644
--- a/drivers/clk/qcom/clk-rcg.h
+++ b/drivers/clk/qcom/clk-rcg.h
@@ -155,5 +155,8 @@ struct clk_rcg2 {
 #define to_clk_rcg2(_hw) container_of(to_clk_regmap(_hw), struct clk_rcg2, clkr)
 
 extern const struct clk_ops clk_rcg2_ops;
+extern const struct clk_ops clk_edp_pixel_ops;
+extern const struct clk_ops clk_byte_ops;
+extern const struct clk_ops clk_pixel_ops;
 
 #endif
diff --git a/drivers/clk/qcom/clk-rcg2.c b/drivers/clk/qcom/clk-rcg2.c
index 00f878a04d3..cd185d5cc67 100644
--- a/drivers/clk/qcom/clk-rcg2.c
+++ b/drivers/clk/qcom/clk-rcg2.c
@@ -19,6 +19,7 @@
 #include <linux/clk-provider.h>
 #include <linux/delay.h>
 #include <linux/regmap.h>
+#include <linux/math64.h>
 
 #include <asm/div64.h>
 
@@ -55,7 +56,7 @@ static int clk_rcg2_is_enabled(struct clk_hw *hw)
 	if (ret)
 		return ret;
 
-	return (cmd & CMD_ROOT_OFF) != 0;
+	return (cmd & CMD_ROOT_OFF) == 0;
 }
 
 static u8 clk_rcg2_get_parent(struct clk_hw *hw)
@@ -181,7 +182,8 @@ struct freq_tbl *find_freq(const struct freq_tbl *f, unsigned long rate)
 		if (rate <= f->freq)
 			return f;
 
-	return NULL;
+	/* Default to our fastest rate */
+	return f - 1;
 }
 
 static long _freq_tbl_determine_rate(struct clk_hw *hw,
@@ -224,31 +226,25 @@ static long clk_rcg2_determine_rate(struct clk_hw *hw, unsigned long rate,
 	return _freq_tbl_determine_rate(hw, rcg->freq_tbl, rate, p_rate, p);
 }
 
-static int __clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate)
+static int clk_rcg2_configure(struct clk_rcg2 *rcg, const struct freq_tbl *f)
 {
-	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
-	const struct freq_tbl *f;
 	u32 cfg, mask;
 	int ret;
 
-	f = find_freq(rcg->freq_tbl, rate);
-	if (!f)
-		return -EINVAL;
-
 	if (rcg->mnd_width && f->n) {
 		mask = BIT(rcg->mnd_width) - 1;
-		ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + M_REG,
-					 mask, f->m);
+		ret = regmap_update_bits(rcg->clkr.regmap,
+				rcg->cmd_rcgr + M_REG, mask, f->m);
 		if (ret)
 			return ret;
 
-		ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + N_REG,
-					 mask, ~(f->n - f->m));
+		ret = regmap_update_bits(rcg->clkr.regmap,
+				rcg->cmd_rcgr + N_REG, mask, ~(f->n - f->m));
 		if (ret)
 			return ret;
 
-		ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + D_REG,
-					 mask, ~f->n);
+		ret = regmap_update_bits(rcg->clkr.regmap,
+				rcg->cmd_rcgr + D_REG, mask, ~f->n);
 		if (ret)
 			return ret;
 	}
@@ -259,14 +255,26 @@ static int __clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate)
 	cfg |= rcg->parent_map[f->src] << CFG_SRC_SEL_SHIFT;
 	if (rcg->mnd_width && f->n)
 		cfg |= CFG_MODE_DUAL_EDGE;
-	ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, mask,
-			cfg);
+	ret = regmap_update_bits(rcg->clkr.regmap,
+			rcg->cmd_rcgr + CFG_REG, mask, cfg);
 	if (ret)
 		return ret;
 
 	return update_config(rcg);
 }
 
+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);
+	if (!f)
+		return -EINVAL;
+
+	return clk_rcg2_configure(rcg, f);
+}
+
 static int clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate,
 			    unsigned long parent_rate)
 {
@@ -289,3 +297,265 @@ const struct clk_ops clk_rcg2_ops = {
 	.set_rate_and_parent = clk_rcg2_set_rate_and_parent,
 };
 EXPORT_SYMBOL_GPL(clk_rcg2_ops);
+
+struct frac_entry {
+	int num;
+	int den;
+};
+
+static const struct frac_entry frac_table_675m[] = {	/* link rate of 270M */
+	{ 52, 295 },	/* 119 M */
+	{ 11, 57 },	/* 130.25 M */
+	{ 63, 307 },	/* 138.50 M */
+	{ 11, 50 },	/* 148.50 M */
+	{ 47, 206 },	/* 154 M */
+	{ 31, 100 },	/* 205.25 M */
+	{ 107, 269 },	/* 268.50 M */
+	{ },
+};
+
+static struct frac_entry frac_table_810m[] = { /* Link rate of 162M */
+	{ 31, 211 },	/* 119 M */
+	{ 32, 199 },	/* 130.25 M */
+	{ 63, 307 },	/* 138.50 M */
+	{ 11, 60 },	/* 148.50 M */
+	{ 50, 263 },	/* 154 M */
+	{ 31, 120 },	/* 205.25 M */
+	{ 119, 359 },	/* 268.50 M */
+	{ },
+};
+
+static int clk_edp_pixel_set_rate(struct clk_hw *hw, unsigned long rate,
+			      unsigned long parent_rate)
+{
+	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
+	struct freq_tbl f = *rcg->freq_tbl;
+	const struct frac_entry *frac;
+	int delta = 100000;
+	s64 src_rate = parent_rate;
+	s64 request;
+	u32 mask = BIT(rcg->hid_width) - 1;
+	u32 hid_div;
+
+	if (src_rate == 810000000)
+		frac = frac_table_810m;
+	else
+		frac = frac_table_675m;
+
+	for (; frac->num; frac++) {
+		request = rate;
+		request *= frac->den;
+		request = div_s64(request, frac->num);
+		if ((src_rate < (request - delta)) ||
+		    (src_rate > (request + delta)))
+			continue;
+
+		regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
+				&hid_div);
+		f.pre_div = hid_div;
+		f.pre_div >>= CFG_SRC_DIV_SHIFT;
+		f.pre_div &= mask;
+		f.m = frac->num;
+		f.n = frac->den;
+
+		return clk_rcg2_configure(rcg, &f);
+	}
+
+	return -EINVAL;
+}
+
+static int clk_edp_pixel_set_rate_and_parent(struct clk_hw *hw,
+		unsigned long rate, unsigned long parent_rate, u8 index)
+{
+	/* Parent index is set statically in frequency table */
+	return clk_edp_pixel_set_rate(hw, rate, parent_rate);
+}
+
+static long clk_edp_pixel_determine_rate(struct clk_hw *hw, unsigned long rate,
+				 unsigned long *p_rate, struct clk **p)
+{
+	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
+	const struct freq_tbl *f = rcg->freq_tbl;
+	const struct frac_entry *frac;
+	int delta = 100000;
+	s64 src_rate = *p_rate;
+	s64 request;
+	u32 mask = BIT(rcg->hid_width) - 1;
+	u32 hid_div;
+
+	/* Force the correct parent */
+	*p = clk_get_parent_by_index(hw->clk, f->src);
+
+	if (src_rate == 810000000)
+		frac = frac_table_810m;
+	else
+		frac = frac_table_675m;
+
+	for (; frac->num; frac++) {
+		request = rate;
+		request *= frac->den;
+		request = div_s64(request, frac->num);
+		if ((src_rate < (request - delta)) ||
+		    (src_rate > (request + delta)))
+			continue;
+
+		regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
+				&hid_div);
+		hid_div >>= CFG_SRC_DIV_SHIFT;
+		hid_div &= mask;
+
+		return calc_rate(src_rate, frac->num, frac->den, !!frac->den,
+				 hid_div);
+	}
+
+	return -EINVAL;
+}
+
+const struct clk_ops clk_edp_pixel_ops = {
+	.is_enabled = clk_rcg2_is_enabled,
+	.get_parent = clk_rcg2_get_parent,
+	.set_parent = clk_rcg2_set_parent,
+	.recalc_rate = clk_rcg2_recalc_rate,
+	.set_rate = clk_edp_pixel_set_rate,
+	.set_rate_and_parent = clk_edp_pixel_set_rate_and_parent,
+	.determine_rate = clk_edp_pixel_determine_rate,
+};
+EXPORT_SYMBOL_GPL(clk_edp_pixel_ops);
+
+static long clk_byte_determine_rate(struct clk_hw *hw, unsigned long rate,
+			 unsigned long *p_rate, struct clk **p)
+{
+	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
+	const struct freq_tbl *f = rcg->freq_tbl;
+	unsigned long parent_rate, div;
+	u32 mask = BIT(rcg->hid_width) - 1;
+
+	if (rate == 0)
+		return -EINVAL;
+
+	*p = clk_get_parent_by_index(hw->clk, f->src);
+	*p_rate = parent_rate = __clk_round_rate(*p, rate);
+
+	div = DIV_ROUND_UP((2 * parent_rate), rate) - 1;
+	div = min_t(u32, div, mask);
+
+	return calc_rate(parent_rate, 0, 0, 0, div);
+}
+
+static int clk_byte_set_rate(struct clk_hw *hw, unsigned long rate,
+			 unsigned long parent_rate)
+{
+	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
+	struct freq_tbl f = *rcg->freq_tbl;
+	unsigned long div;
+	u32 mask = BIT(rcg->hid_width) - 1;
+
+	div = DIV_ROUND_UP((2 * parent_rate), rate) - 1;
+	div = min_t(u32, div, mask);
+
+	f.pre_div = div;
+
+	return clk_rcg2_configure(rcg, &f);
+}
+
+static int clk_byte_set_rate_and_parent(struct clk_hw *hw,
+		unsigned long rate, unsigned long parent_rate, u8 index)
+{
+	/* Parent index is set statically in frequency table */
+	return clk_byte_set_rate(hw, rate, parent_rate);
+}
+
+const struct clk_ops clk_byte_ops = {
+	.is_enabled = clk_rcg2_is_enabled,
+	.get_parent = clk_rcg2_get_parent,
+	.set_parent = clk_rcg2_set_parent,
+	.recalc_rate = clk_rcg2_recalc_rate,
+	.set_rate = clk_byte_set_rate,
+	.set_rate_and_parent = clk_byte_set_rate_and_parent,
+	.determine_rate = clk_byte_determine_rate,
+};
+EXPORT_SYMBOL_GPL(clk_byte_ops);
+
+static const struct frac_entry frac_table_pixel[] = {
+	{ 3, 8 },
+	{ 2, 9 },
+	{ 4, 9 },
+	{ 1, 1 },
+	{ }
+};
+
+static long clk_pixel_determine_rate(struct clk_hw *hw, unsigned long rate,
+				 unsigned long *p_rate, struct clk **p)
+{
+	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
+	unsigned long request, src_rate;
+	int delta = 100000;
+	const struct freq_tbl *f = rcg->freq_tbl;
+	const struct frac_entry *frac = frac_table_pixel;
+	struct clk *parent = *p = clk_get_parent_by_index(hw->clk, f->src);
+
+	for (; frac->num; frac++) {
+		request = (rate * frac->den) / frac->num;
+
+		src_rate = __clk_round_rate(parent, request);
+		if ((src_rate < (request - delta)) ||
+			(src_rate > (request + delta)))
+			continue;
+
+		*p_rate = src_rate;
+		return (src_rate * frac->num) / frac->den;
+	}
+
+	return -EINVAL;
+}
+
+static int clk_pixel_set_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long parent_rate)
+{
+	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
+	struct freq_tbl f = *rcg->freq_tbl;
+	const struct frac_entry *frac = frac_table_pixel;
+	unsigned long request, src_rate;
+	int delta = 100000;
+	u32 mask = BIT(rcg->hid_width) - 1;
+	u32 hid_div;
+	struct clk *parent = clk_get_parent_by_index(hw->clk, f.src);
+
+	for (; frac->num; frac++) {
+		request = (rate * frac->den) / frac->num;
+
+		src_rate = __clk_round_rate(parent, request);
+		if ((src_rate < (request - delta)) ||
+			(src_rate > (request + delta)))
+			continue;
+
+		regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
+				&hid_div);
+		f.pre_div = hid_div;
+		f.pre_div >>= CFG_SRC_DIV_SHIFT;
+		f.pre_div &= mask;
+		f.m = frac->num;
+		f.n = frac->den;
+
+		return clk_rcg2_configure(rcg, &f);
+	}
+	return -EINVAL;
+}
+
+static int clk_pixel_set_rate_and_parent(struct clk_hw *hw, unsigned long rate,
+		unsigned long parent_rate, u8 index)
+{
+	/* Parent index is set statically in frequency table */
+	return clk_pixel_set_rate(hw, rate, parent_rate);
+}
+
+const struct clk_ops clk_pixel_ops = {
+	.is_enabled = clk_rcg2_is_enabled,
+	.get_parent = clk_rcg2_get_parent,
+	.set_parent = clk_rcg2_set_parent,
+	.recalc_rate = clk_rcg2_recalc_rate,
+	.set_rate = clk_pixel_set_rate,
+	.set_rate_and_parent = clk_pixel_set_rate_and_parent,
+	.determine_rate = clk_pixel_determine_rate,
+};
+EXPORT_SYMBOL_GPL(clk_pixel_ops);
diff --git a/drivers/clk/qcom/common.c b/drivers/clk/qcom/common.c
new file mode 100644
index 00000000000..9b5a1cfc6b9
--- /dev/null
+++ b/drivers/clk/qcom/common.c
@@ -0,0 +1,101 @@
+/*
+ * Copyright (c) 2013-2014, The Linux Foundation. All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/export.h>
+#include <linux/regmap.h>
+#include <linux/platform_device.h>
+#include <linux/clk-provider.h>
+#include <linux/reset-controller.h>
+
+#include "common.h"
+#include "clk-regmap.h"
+#include "reset.h"
+
+struct qcom_cc {
+	struct qcom_reset_controller reset;
+	struct clk_onecell_data data;
+	struct clk *clks[];
+};
+
+int qcom_cc_probe(struct platform_device *pdev, const struct qcom_cc_desc *desc)
+{
+	void __iomem *base;
+	struct resource *res;
+	int i, ret;
+	struct device *dev = &pdev->dev;
+	struct clk *clk;
+	struct clk_onecell_data *data;
+	struct clk **clks;
+	struct regmap *regmap;
+	struct qcom_reset_controller *reset;
+	struct qcom_cc *cc;
+	size_t num_clks = desc->num_clks;
+	struct clk_regmap **rclks = desc->clks;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	regmap = devm_regmap_init_mmio(dev, base, desc->config);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	cc = devm_kzalloc(dev, sizeof(*cc) + sizeof(*clks) * num_clks,
+			  GFP_KERNEL);
+	if (!cc)
+		return -ENOMEM;
+
+	clks = cc->clks;
+	data = &cc->data;
+	data->clks = clks;
+	data->clk_num = num_clks;
+
+	for (i = 0; i < num_clks; i++) {
+		if (!rclks[i]) {
+			clks[i] = ERR_PTR(-ENOENT);
+			continue;
+		}
+		clk = devm_clk_register_regmap(dev, rclks[i]);
+		if (IS_ERR(clk))
+			return PTR_ERR(clk);
+		clks[i] = clk;
+	}
+
+	ret = of_clk_add_provider(dev->of_node, of_clk_src_onecell_get, data);
+	if (ret)
+		return ret;
+
+	reset = &cc->reset;
+	reset->rcdev.of_node = dev->of_node;
+	reset->rcdev.ops = &qcom_reset_ops;
+	reset->rcdev.owner = dev->driver->owner;
+	reset->rcdev.nr_resets = desc->num_resets;
+	reset->regmap = regmap;
+	reset->reset_map = desc->resets;
+	platform_set_drvdata(pdev, &reset->rcdev);
+
+	ret = reset_controller_register(&reset->rcdev);
+	if (ret)
+		of_clk_del_provider(dev->of_node);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(qcom_cc_probe);
+
+void qcom_cc_remove(struct platform_device *pdev)
+{
+	of_clk_del_provider(pdev->dev.of_node);
+	reset_controller_unregister(platform_get_drvdata(pdev));
+}
+EXPORT_SYMBOL_GPL(qcom_cc_remove);
diff --git a/drivers/clk/qcom/common.h b/drivers/clk/qcom/common.h
new file mode 100644
index 00000000000..2c3cfc86034
--- /dev/null
+++ b/drivers/clk/qcom/common.h
@@ -0,0 +1,34 @@
+/*
+ * Copyright (c) 2014, The Linux Foundation. All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#ifndef __QCOM_CLK_COMMON_H__
+#define __QCOM_CLK_COMMON_H__
+
+struct platform_device;
+struct regmap_config;
+struct clk_regmap;
+struct qcom_reset_map;
+
+struct qcom_cc_desc {
+	const struct regmap_config *config;
+	struct clk_regmap **clks;
+	size_t num_clks;
+	const struct qcom_reset_map *resets;
+	size_t num_resets;
+};
+
+extern int qcom_cc_probe(struct platform_device *pdev,
+			 const struct qcom_cc_desc *desc);
+
+extern void qcom_cc_remove(struct platform_device *pdev);
+
+#endif
diff --git a/drivers/clk/qcom/gcc-msm8660.c b/drivers/clk/qcom/gcc-msm8660.c
index bc0b7f1fcfb..0c4b727ae42 100644
--- a/drivers/clk/qcom/gcc-msm8660.c
+++ b/drivers/clk/qcom/gcc-msm8660.c
@@ -25,6 +25,7 @@
 #include <dt-bindings/clock/qcom,gcc-msm8660.h>
 #include <dt-bindings/reset/qcom,gcc-msm8660.h>
 
+#include "common.h"
 #include "clk-regmap.h"
 #include "clk-pll.h"
 #include "clk-rcg.h"
@@ -2701,51 +2702,24 @@ static const struct regmap_config gcc_msm8660_regmap_config = {
 	.fast_io	= true,
 };
 
+static const struct qcom_cc_desc gcc_msm8660_desc = {
+	.config = &gcc_msm8660_regmap_config,
+	.clks = gcc_msm8660_clks,
+	.num_clks = ARRAY_SIZE(gcc_msm8660_clks),
+	.resets = gcc_msm8660_resets,
+	.num_resets = ARRAY_SIZE(gcc_msm8660_resets),
+};
+
 static const struct of_device_id gcc_msm8660_match_table[] = {
 	{ .compatible = "qcom,gcc-msm8660" },
 	{ }
 };
 MODULE_DEVICE_TABLE(of, gcc_msm8660_match_table);
 
-struct qcom_cc {
-	struct qcom_reset_controller reset;
-	struct clk_onecell_data data;
-	struct clk *clks[];
-};
-
 static int gcc_msm8660_probe(struct platform_device *pdev)
 {
-	void __iomem *base;
-	struct resource *res;
-	int i, ret;
-	struct device *dev = &pdev->dev;
 	struct clk *clk;
-	struct clk_onecell_data *data;
-	struct clk **clks;
-	struct regmap *regmap;
-	size_t num_clks;
-	struct qcom_reset_controller *reset;
-	struct qcom_cc *cc;
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	base = devm_ioremap_resource(dev, res);
-	if (IS_ERR(base))
-		return PTR_ERR(base);
-
-	regmap = devm_regmap_init_mmio(dev, base, &gcc_msm8660_regmap_config);
-	if (IS_ERR(regmap))
-		return PTR_ERR(regmap);
-
-	num_clks = ARRAY_SIZE(gcc_msm8660_clks);
-	cc = devm_kzalloc(dev, sizeof(*cc) + sizeof(*clks) * num_clks,
-			  GFP_KERNEL);
-	if (!cc)
-		return -ENOMEM;
-
-	clks = cc->clks;
-	data = &cc->data;
-	data->clks = clks;
-	data->clk_num = num_clks;
+	struct device *dev = &pdev->dev;
 
 	/* Temporary until RPM clocks supported */
 	clk = clk_register_fixed_rate(dev, "cxo", NULL, CLK_IS_ROOT, 19200000);
@@ -2756,39 +2730,12 @@ static int gcc_msm8660_probe(struct platform_device *pdev)
 	if (IS_ERR(clk))
 		return PTR_ERR(clk);
 
-	for (i = 0; i < num_clks; i++) {
-		if (!gcc_msm8660_clks[i])
-			continue;
-		clk = devm_clk_register_regmap(dev, gcc_msm8660_clks[i]);
-		if (IS_ERR(clk))
-			return PTR_ERR(clk);
-		clks[i] = clk;
-	}
-
-	ret = of_clk_add_provider(dev->of_node, of_clk_src_onecell_get, data);
-	if (ret)
-		return ret;
-
-	reset = &cc->reset;
-	reset->rcdev.of_node = dev->of_node;
-	reset->rcdev.ops = &qcom_reset_ops,
-	reset->rcdev.owner = THIS_MODULE,
-	reset->rcdev.nr_resets = ARRAY_SIZE(gcc_msm8660_resets),
-	reset->regmap = regmap;
-	reset->reset_map = gcc_msm8660_resets,
-	platform_set_drvdata(pdev, &reset->rcdev);
-
-	ret = reset_controller_register(&reset->rcdev);
-	if (ret)
-		of_clk_del_provider(dev->of_node);
-
-	return ret;
+	return qcom_cc_probe(pdev, &gcc_msm8660_desc);
 }
 
 static int gcc_msm8660_remove(struct platform_device *pdev)
 {
-	of_clk_del_provider(pdev->dev.of_node);
-	reset_controller_unregister(platform_get_drvdata(pdev));
+	qcom_cc_remove(pdev);
 	return 0;
 }
 
diff --git a/drivers/clk/qcom/gcc-msm8960.c b/drivers/clk/qcom/gcc-msm8960.c
index fd446ab2fd9..f4ffd91901f 100644
--- a/drivers/clk/qcom/gcc-msm8960.c
+++ b/drivers/clk/qcom/gcc-msm8960.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2013-2014, The Linux Foundation. All rights reserved.
  *
  * This software is licensed under the terms of the GNU General Public
  * License version 2, as published by the Free Software Foundation, and
@@ -25,6 +25,7 @@
 #include <dt-bindings/clock/qcom,gcc-msm8960.h>
 #include <dt-bindings/reset/qcom,gcc-msm8960.h>
 
+#include "common.h"
 #include "clk-regmap.h"
 #include "clk-pll.h"
 #include "clk-rcg.h"
@@ -2809,7 +2810,7 @@ static const struct qcom_reset_map gcc_msm8960_resets[] = {
 	[PPSS_PROC_RESET] = { 0x2594, 1 },
 	[PPSS_RESET] = { 0x2594},
 	[DMA_BAM_RESET] = { 0x25c0, 7 },
-	[SIC_TIC_RESET] = { 0x2600, 7 },
+	[SPS_TIC_H_RESET] = { 0x2600, 7 },
 	[SLIMBUS_H_RESET] = { 0x2620, 7 },
 	[SFAB_CFPB_M_RESET] = { 0x2680, 7 },
 	[SFAB_CFPB_S_RESET] = { 0x26c0, 7 },
@@ -2822,7 +2823,7 @@ static const struct qcom_reset_map gcc_msm8960_resets[] = {
 	[SFAB_SFPB_M_RESET] = { 0x2780, 7 },
 	[SFAB_SFPB_S_RESET] = { 0x27a0, 7 },
 	[RPM_PROC_RESET] = { 0x27c0, 7 },
-	[PMIC_SSBI2_RESET] = { 0x270c, 12 },
+	[PMIC_SSBI2_RESET] = { 0x280c, 12 },
 	[SDC1_RESET] = { 0x2830 },
 	[SDC2_RESET] = { 0x2850 },
 	[SDC3_RESET] = { 0x2870 },
@@ -2867,6 +2868,16 @@ static const struct qcom_reset_map gcc_msm8960_resets[] = {
 	[RIVA_RESET] = { 0x35e0 },
 };
 
+static struct clk_regmap *gcc_apq8064_clks[] = {
+	[PLL8] = &pll8.clkr,
+	[PLL8_VOTE] = &pll8_vote,
+	[GSBI7_UART_SRC] = &gsbi7_uart_src.clkr,
+	[GSBI7_UART_CLK] = &gsbi7_uart_clk.clkr,
+	[GSBI7_QUP_SRC] = &gsbi7_qup_src.clkr,
+	[GSBI7_QUP_CLK] = &gsbi7_qup_clk.clkr,
+	[GSBI7_H_CLK] = &gsbi7_h_clk.clkr,
+};
+
 static const struct regmap_config gcc_msm8960_regmap_config = {
 	.reg_bits	= 32,
 	.reg_stride	= 4,
@@ -2875,51 +2886,38 @@ static const struct regmap_config gcc_msm8960_regmap_config = {
 	.fast_io	= true,
 };
 
+static const struct qcom_cc_desc gcc_msm8960_desc = {
+	.config = &gcc_msm8960_regmap_config,
+	.clks = gcc_msm8960_clks,
+	.num_clks = ARRAY_SIZE(gcc_msm8960_clks),
+	.resets = gcc_msm8960_resets,
+	.num_resets = ARRAY_SIZE(gcc_msm8960_resets),
+};
+
+static const struct qcom_cc_desc gcc_apq8064_desc = {
+	.config = &gcc_msm8960_regmap_config,
+	.clks = gcc_apq8064_clks,
+	.num_clks = ARRAY_SIZE(gcc_apq8064_clks),
+	.resets = gcc_msm8960_resets,
+	.num_resets = ARRAY_SIZE(gcc_msm8960_resets),
+};
+
 static const struct of_device_id gcc_msm8960_match_table[] = {
-	{ .compatible = "qcom,gcc-msm8960" },
+	{ .compatible = "qcom,gcc-msm8960", .data = &gcc_msm8960_desc },
+	{ .compatible = "qcom,gcc-apq8064", .data = &gcc_apq8064_desc },
 	{ }
 };
 MODULE_DEVICE_TABLE(of, gcc_msm8960_match_table);
 
-struct qcom_cc {
-	struct qcom_reset_controller reset;
-	struct clk_onecell_data data;
-	struct clk *clks[];
-};
-
 static int gcc_msm8960_probe(struct platform_device *pdev)
 {
-	void __iomem *base;
-	struct resource *res;
-	int i, ret;
-	struct device *dev = &pdev->dev;
 	struct clk *clk;
-	struct clk_onecell_data *data;
-	struct clk **clks;
-	struct regmap *regmap;
-	size_t num_clks;
-	struct qcom_reset_controller *reset;
-	struct qcom_cc *cc;
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	base = devm_ioremap_resource(dev, res);
-	if (IS_ERR(base))
-		return PTR_ERR(base);
-
-	regmap = devm_regmap_init_mmio(dev, base, &gcc_msm8960_regmap_config);
-	if (IS_ERR(regmap))
-		return PTR_ERR(regmap);
-
-	num_clks = ARRAY_SIZE(gcc_msm8960_clks);
-	cc = devm_kzalloc(dev, sizeof(*cc) + sizeof(*clks) * num_clks,
-			  GFP_KERNEL);
-	if (!cc)
-		return -ENOMEM;
-
-	clks = cc->clks;
-	data = &cc->data;
-	data->clks = clks;
-	data->clk_num = num_clks;
+	struct device *dev = &pdev->dev;
+	const struct of_device_id *match;
+
+	match = of_match_device(gcc_msm8960_match_table, &pdev->dev);
+	if (!match)
+		return -EINVAL;
 
 	/* Temporary until RPM clocks supported */
 	clk = clk_register_fixed_rate(dev, "cxo", NULL, CLK_IS_ROOT, 19200000);
@@ -2930,39 +2928,12 @@ static int gcc_msm8960_probe(struct platform_device *pdev)
 	if (IS_ERR(clk))
 		return PTR_ERR(clk);
 
-	for (i = 0; i < num_clks; i++) {
-		if (!gcc_msm8960_clks[i])
-			continue;
-		clk = devm_clk_register_regmap(dev, gcc_msm8960_clks[i]);
-		if (IS_ERR(clk))
-			return PTR_ERR(clk);
-		clks[i] = clk;
-	}
-
-	ret = of_clk_add_provider(dev->of_node, of_clk_src_onecell_get, data);
-	if (ret)
-		return ret;
-
-	reset = &cc->reset;
-	reset->rcdev.of_node = dev->of_node;
-	reset->rcdev.ops = &qcom_reset_ops,
-	reset->rcdev.owner = THIS_MODULE,
-	reset->rcdev.nr_resets = ARRAY_SIZE(gcc_msm8960_resets),
-	reset->regmap = regmap;
-	reset->reset_map = gcc_msm8960_resets,
-	platform_set_drvdata(pdev, &reset->rcdev);
-
-	ret = reset_controller_register(&reset->rcdev);
-	if (ret)
-		of_clk_del_provider(dev->of_node);
-
-	return ret;
+	return qcom_cc_probe(pdev, match->data);
 }
 
 static int gcc_msm8960_remove(struct platform_device *pdev)
 {
-	of_clk_del_provider(pdev->dev.of_node);
-	reset_controller_unregister(platform_get_drvdata(pdev));
+	qcom_cc_remove(pdev);
 	return 0;
 }
 
diff --git a/drivers/clk/qcom/gcc-msm8974.c b/drivers/clk/qcom/gcc-msm8974.c
index 51d457e2b95..7af7c18d214 100644
--- a/drivers/clk/qcom/gcc-msm8974.c
+++ b/drivers/clk/qcom/gcc-msm8974.c
@@ -25,6 +25,7 @@
 #include <dt-bindings/clock/qcom,gcc-msm8974.h>
 #include <dt-bindings/reset/qcom,gcc-msm8974.h>
 
+#include "common.h"
 #include "clk-regmap.h"
 #include "clk-pll.h"
 #include "clk-rcg.h"
@@ -34,6 +35,7 @@
 #define P_XO	0
 #define P_GPLL0	1
 #define P_GPLL1	1
+#define P_GPLL4	2
 
 static const u8 gcc_xo_gpll0_map[] = {
 	[P_XO]		= 0,
@@ -45,6 +47,18 @@ static const char *gcc_xo_gpll0[] = {
 	"gpll0_vote",
 };
 
+static const u8 gcc_xo_gpll0_gpll4_map[] = {
+	[P_XO]		= 0,
+	[P_GPLL0]	= 1,
+	[P_GPLL4]	= 5,
+};
+
+static const char *gcc_xo_gpll0_gpll4[] = {
+	"xo",
+	"gpll0_vote",
+	"gpll4_vote",
+};
+
 #define F(f, s, h, m, n) { (f), (s), (2 * (h) - 1), (m), (n) }
 
 static struct clk_pll gpll0 = {
@@ -137,6 +151,33 @@ static struct clk_regmap gpll1_vote = {
 	},
 };
 
+static struct clk_pll gpll4 = {
+	.l_reg = 0x1dc4,
+	.m_reg = 0x1dc8,
+	.n_reg = 0x1dcc,
+	.config_reg = 0x1dd4,
+	.mode_reg = 0x1dc0,
+	.status_reg = 0x1ddc,
+	.status_bit = 17,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "gpll4",
+		.parent_names = (const char *[]){ "xo" },
+		.num_parents = 1,
+		.ops = &clk_pll_ops,
+	},
+};
+
+static struct clk_regmap gpll4_vote = {
+	.enable_reg = 0x1480,
+	.enable_mask = BIT(4),
+	.hw.init = &(struct clk_init_data){
+		.name = "gpll4_vote",
+		.parent_names = (const char *[]){ "gpll4" },
+		.num_parents = 1,
+		.ops = &clk_pll_vote_ops,
+	},
+};
+
 static const struct freq_tbl ftbl_gcc_usb30_master_clk[] = {
 	F(125000000, P_GPLL0, 1, 5, 24),
 	{ }
@@ -811,18 +852,33 @@ static const struct freq_tbl ftbl_gcc_sdcc1_4_apps_clk[] = {
 	{ }
 };
 
+static const struct freq_tbl ftbl_gcc_sdcc1_apps_clk_pro[] = {
+	F(144000, P_XO, 16, 3, 25),
+	F(400000, P_XO, 12, 1, 4),
+	F(20000000, P_GPLL0, 15, 1, 2),
+	F(25000000, P_GPLL0, 12, 1, 2),
+	F(50000000, P_GPLL0, 12, 0, 0),
+	F(100000000, P_GPLL0, 6, 0, 0),
+	F(192000000, P_GPLL4, 4, 0, 0),
+	F(200000000, P_GPLL0, 3, 0, 0),
+	F(384000000, P_GPLL4, 2, 0, 0),
+	{ }
+};
+
+static struct clk_init_data sdcc1_apps_clk_src_init = {
+	.name = "sdcc1_apps_clk_src",
+	.parent_names = gcc_xo_gpll0,
+	.num_parents = 2,
+	.ops = &clk_rcg2_ops,
+};
+
 static struct clk_rcg2 sdcc1_apps_clk_src = {
 	.cmd_rcgr = 0x04d0,
 	.mnd_width = 8,
 	.hid_width = 5,
 	.parent_map = gcc_xo_gpll0_map,
 	.freq_tbl = ftbl_gcc_sdcc1_4_apps_clk,
-	.clkr.hw.init = &(struct clk_init_data){
-		.name = "sdcc1_apps_clk_src",
-		.parent_names = gcc_xo_gpll0,
-		.num_parents = 2,
-		.ops = &clk_rcg2_ops,
-	},
+	.clkr.hw.init = &sdcc1_apps_clk_src_init,
 };
 
 static struct clk_rcg2 sdcc2_apps_clk_src = {
@@ -1340,7 +1396,7 @@ static struct clk_branch gcc_blsp1_uart6_apps_clk = {
 };
 
 static struct clk_branch gcc_blsp2_ahb_clk = {
-	.halt_reg = 0x05c4,
+	.halt_reg = 0x0944,
 	.halt_check = BRANCH_HALT_VOTED,
 	.clkr = {
 		.enable_reg = 0x1484,
@@ -1994,6 +2050,38 @@ static struct clk_branch gcc_sdcc1_apps_clk = {
 	},
 };
 
+static struct clk_branch gcc_sdcc1_cdccal_ff_clk = {
+	.halt_reg = 0x04e8,
+	.clkr = {
+		.enable_reg = 0x04e8,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_sdcc1_cdccal_ff_clk",
+			.parent_names = (const char *[]){
+				"xo"
+			},
+			.num_parents = 1,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_sdcc1_cdccal_sleep_clk = {
+	.halt_reg = 0x04e4,
+	.clkr = {
+		.enable_reg = 0x04e4,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_sdcc1_cdccal_sleep_clk",
+			.parent_names = (const char *[]){
+				"sleep_clk_src"
+			},
+			.num_parents = 1,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
 static struct clk_branch gcc_sdcc2_ahb_clk = {
 	.halt_reg = 0x0508,
 	.clkr = {
@@ -2483,6 +2571,10 @@ static struct clk_regmap *gcc_msm8974_clocks[] = {
 	[GCC_USB_HSIC_IO_CAL_SLEEP_CLK] = &gcc_usb_hsic_io_cal_sleep_clk.clkr,
 	[GCC_USB_HSIC_SYSTEM_CLK] = &gcc_usb_hsic_system_clk.clkr,
 	[GCC_MMSS_GPLL0_CLK_SRC] = &gcc_mmss_gpll0_clk_src,
+	[GPLL4] = NULL,
+	[GPLL4_VOTE] = NULL,
+	[GCC_SDCC1_CDCCAL_SLEEP_CLK] = NULL,
+	[GCC_SDCC1_CDCCAL_FF_CLK] = NULL,
 };
 
 static const struct qcom_reset_map gcc_msm8974_resets[] = {
@@ -2574,51 +2666,51 @@ static const struct regmap_config gcc_msm8974_regmap_config = {
 	.fast_io	= true,
 };
 
+static const struct qcom_cc_desc gcc_msm8974_desc = {
+	.config = &gcc_msm8974_regmap_config,
+	.clks = gcc_msm8974_clocks,
+	.num_clks = ARRAY_SIZE(gcc_msm8974_clocks),
+	.resets = gcc_msm8974_resets,
+	.num_resets = ARRAY_SIZE(gcc_msm8974_resets),
+};
+
 static const struct of_device_id gcc_msm8974_match_table[] = {
 	{ .compatible = "qcom,gcc-msm8974" },
+	{ .compatible = "qcom,gcc-msm8974pro" , .data = (void *)1UL },
+	{ .compatible = "qcom,gcc-msm8974pro-ac", .data = (void *)1UL },
 	{ }
 };
 MODULE_DEVICE_TABLE(of, gcc_msm8974_match_table);
 
-struct qcom_cc {
-	struct qcom_reset_controller reset;
-	struct clk_onecell_data data;
-	struct clk *clks[];
-};
+static void msm8974_pro_clock_override(void)
+{
+	sdcc1_apps_clk_src_init.parent_names = gcc_xo_gpll0_gpll4;
+	sdcc1_apps_clk_src_init.num_parents = 3;
+	sdcc1_apps_clk_src.freq_tbl = ftbl_gcc_sdcc1_apps_clk_pro;
+	sdcc1_apps_clk_src.parent_map = gcc_xo_gpll0_gpll4_map;
+
+	gcc_msm8974_clocks[GPLL4] = &gpll4.clkr;
+	gcc_msm8974_clocks[GPLL4_VOTE] = &gpll4_vote;
+	gcc_msm8974_clocks[GCC_SDCC1_CDCCAL_SLEEP_CLK] =
+		&gcc_sdcc1_cdccal_sleep_clk.clkr;
+	gcc_msm8974_clocks[GCC_SDCC1_CDCCAL_FF_CLK] =
+		&gcc_sdcc1_cdccal_ff_clk.clkr;
+}
 
 static int gcc_msm8974_probe(struct platform_device *pdev)
 {
-	void __iomem *base;
-	struct resource *res;
-	int i, ret;
-	struct device *dev = &pdev->dev;
 	struct clk *clk;
-	struct clk_onecell_data *data;
-	struct clk **clks;
-	struct regmap *regmap;
-	size_t num_clks;
-	struct qcom_reset_controller *reset;
-	struct qcom_cc *cc;
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	base = devm_ioremap_resource(dev, res);
-	if (IS_ERR(base))
-		return PTR_ERR(base);
-
-	regmap = devm_regmap_init_mmio(dev, base, &gcc_msm8974_regmap_config);
-	if (IS_ERR(regmap))
-		return PTR_ERR(regmap);
-
-	num_clks = ARRAY_SIZE(gcc_msm8974_clocks);
-	cc = devm_kzalloc(dev, sizeof(*cc) + sizeof(*clks) * num_clks,
-			  GFP_KERNEL);
-	if (!cc)
-		return -ENOMEM;
-
-	clks = cc->clks;
-	data = &cc->data;
-	data->clks = clks;
-	data->clk_num = num_clks;
+	struct device *dev = &pdev->dev;
+	bool pro;
+	const struct of_device_id *id;
+
+	id = of_match_device(gcc_msm8974_match_table, dev);
+	if (!id)
+		return -ENODEV;
+	pro = !!(id->data);
+
+	if (pro)
+		msm8974_pro_clock_override();
 
 	/* Temporary until RPM clocks supported */
 	clk = clk_register_fixed_rate(dev, "xo", NULL, CLK_IS_ROOT, 19200000);
@@ -2631,39 +2723,12 @@ static int gcc_msm8974_probe(struct platform_device *pdev)
 	if (IS_ERR(clk))
 		return PTR_ERR(clk);
 
-	for (i = 0; i < num_clks; i++) {
-		if (!gcc_msm8974_clocks[i])
-			continue;
-		clk = devm_clk_register_regmap(dev, gcc_msm8974_clocks[i]);
-		if (IS_ERR(clk))
-			return PTR_ERR(clk);
-		clks[i] = clk;
-	}
-
-	ret = of_clk_add_provider(dev->of_node, of_clk_src_onecell_get, data);
-	if (ret)
-		return ret;
-
-	reset = &cc->reset;
-	reset->rcdev.of_node = dev->of_node;
-	reset->rcdev.ops = &qcom_reset_ops,
-	reset->rcdev.owner = THIS_MODULE,
-	reset->rcdev.nr_resets = ARRAY_SIZE(gcc_msm8974_resets),
-	reset->regmap = regmap;
-	reset->reset_map = gcc_msm8974_resets,
-	platform_set_drvdata(pdev, &reset->rcdev);
-
-	ret = reset_controller_register(&reset->rcdev);
-	if (ret)
-		of_clk_del_provider(dev->of_node);
-
-	return ret;
+	return qcom_cc_probe(pdev, &gcc_msm8974_desc);
 }
 
 static int gcc_msm8974_remove(struct platform_device *pdev)
 {
-	of_clk_del_provider(pdev->dev.of_node);
-	reset_controller_unregister(platform_get_drvdata(pdev));
+	qcom_cc_remove(pdev);
 	return 0;
 }
 
diff --git a/drivers/clk/qcom/mmcc-msm8960.c b/drivers/clk/qcom/mmcc-msm8960.c
index f9b59c7e48e..12f3c0b64fc 100644
--- a/drivers/clk/qcom/mmcc-msm8960.c
+++ b/drivers/clk/qcom/mmcc-msm8960.c
@@ -26,6 +26,7 @@
 #include <dt-bindings/clock/qcom,mmcc-msm8960.h>
 #include <dt-bindings/reset/qcom,mmcc-msm8960.h>
 
+#include "common.h"
 #include "clk-regmap.h"
 #include "clk-pll.h"
 #include "clk-rcg.h"
@@ -2222,85 +2223,28 @@ static const struct regmap_config mmcc_msm8960_regmap_config = {
 	.fast_io	= true,
 };
 
+static const struct qcom_cc_desc mmcc_msm8960_desc = {
+	.config = &mmcc_msm8960_regmap_config,
+	.clks = mmcc_msm8960_clks,
+	.num_clks = ARRAY_SIZE(mmcc_msm8960_clks),
+	.resets = mmcc_msm8960_resets,
+	.num_resets = ARRAY_SIZE(mmcc_msm8960_resets),
+};
+
 static const struct of_device_id mmcc_msm8960_match_table[] = {
 	{ .compatible = "qcom,mmcc-msm8960" },
 	{ }
 };
 MODULE_DEVICE_TABLE(of, mmcc_msm8960_match_table);
 
-struct qcom_cc {
-	struct qcom_reset_controller reset;
-	struct clk_onecell_data data;
-	struct clk *clks[];
-};
-
 static int mmcc_msm8960_probe(struct platform_device *pdev)
 {
-	void __iomem *base;
-	struct resource *res;
-	int i, ret;
-	struct device *dev = &pdev->dev;
-	struct clk *clk;
-	struct clk_onecell_data *data;
-	struct clk **clks;
-	struct regmap *regmap;
-	size_t num_clks;
-	struct qcom_reset_controller *reset;
-	struct qcom_cc *cc;
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	base = devm_ioremap_resource(dev, res);
-	if (IS_ERR(base))
-		return PTR_ERR(base);
-
-	regmap = devm_regmap_init_mmio(dev, base, &mmcc_msm8960_regmap_config);
-	if (IS_ERR(regmap))
-		return PTR_ERR(regmap);
-
-	num_clks = ARRAY_SIZE(mmcc_msm8960_clks);
-	cc = devm_kzalloc(dev, sizeof(*cc) + sizeof(*clks) * num_clks,
-			  GFP_KERNEL);
-	if (!cc)
-		return -ENOMEM;
-
-	clks = cc->clks;
-	data = &cc->data;
-	data->clks = clks;
-	data->clk_num = num_clks;
-
-	for (i = 0; i < num_clks; i++) {
-		if (!mmcc_msm8960_clks[i])
-			continue;
-		clk = devm_clk_register_regmap(dev, mmcc_msm8960_clks[i]);
-		if (IS_ERR(clk))
-			return PTR_ERR(clk);
-		clks[i] = clk;
-	}
-
-	ret = of_clk_add_provider(dev->of_node, of_clk_src_onecell_get, data);
-	if (ret)
-		return ret;
-
-	reset = &cc->reset;
-	reset->rcdev.of_node = dev->of_node;
-	reset->rcdev.ops = &qcom_reset_ops,
-	reset->rcdev.owner = THIS_MODULE,
-	reset->rcdev.nr_resets = ARRAY_SIZE(mmcc_msm8960_resets),
-	reset->regmap = regmap;
-	reset->reset_map = mmcc_msm8960_resets,
-	platform_set_drvdata(pdev, &reset->rcdev);
-
-	ret = reset_controller_register(&reset->rcdev);
-	if (ret)
-		of_clk_del_provider(dev->of_node);
-
-	return ret;
+	return qcom_cc_probe(pdev, &mmcc_msm8960_desc);
 }
 
 static int mmcc_msm8960_remove(struct platform_device *pdev)
 {
-	of_clk_del_provider(pdev->dev.of_node);
-	reset_controller_unregister(platform_get_drvdata(pdev));
+	qcom_cc_remove(pdev);
 	return 0;
 }
 
diff --git a/drivers/clk/qcom/mmcc-msm8974.c b/drivers/clk/qcom/mmcc-msm8974.c
index c95774514b8..c65b9051587 100644
--- a/drivers/clk/qcom/mmcc-msm8974.c
+++ b/drivers/clk/qcom/mmcc-msm8974.c
@@ -25,6 +25,7 @@
 #include <dt-bindings/clock/qcom,mmcc-msm8974.h>
 #include <dt-bindings/reset/qcom,mmcc-msm8974.h>
 
+#include "common.h"
 #include "clk-regmap.h"
 #include "clk-pll.h"
 #include "clk-rcg.h"
@@ -40,9 +41,11 @@
 #define P_EDPVCO	3
 #define P_GPLL1		4
 #define P_DSI0PLL	4
+#define P_DSI0PLL_BYTE	4
 #define P_MMPLL2	4
 #define P_MMPLL3	4
 #define P_DSI1PLL	5
+#define P_DSI1PLL_BYTE	5
 
 static const u8 mmcc_xo_mmpll0_mmpll1_gpll0_map[] = {
 	[P_XO]		= 0,
@@ -160,6 +163,24 @@ static const char *mmcc_xo_dsi_hdmi_edp_gpll0[] = {
 	"dsi1pll",
 };
 
+static const u8 mmcc_xo_dsibyte_hdmi_edp_gpll0_map[] = {
+	[P_XO]			= 0,
+	[P_EDPLINK]		= 4,
+	[P_HDMIPLL]		= 3,
+	[P_GPLL0]		= 5,
+	[P_DSI0PLL_BYTE]	= 1,
+	[P_DSI1PLL_BYTE]	= 2,
+};
+
+static const char *mmcc_xo_dsibyte_hdmi_edp_gpll0[] = {
+	"xo",
+	"edp_link_clk",
+	"hdmipll",
+	"gpll0_vote",
+	"dsi0pllbyte",
+	"dsi1pllbyte",
+};
+
 #define F(f, s, h, m, n) { (f), (s), (2 * (h) - 1), (m), (n) }
 
 static struct clk_pll mmpll0 = {
@@ -169,6 +190,7 @@ static struct clk_pll mmpll0 = {
 	.config_reg = 0x0014,
 	.mode_reg = 0x0000,
 	.status_reg = 0x001c,
+	.status_bit = 17,
         .clkr.hw.init = &(struct clk_init_data){
                 .name = "mmpll0",
                 .parent_names = (const char *[]){ "xo" },
@@ -192,9 +214,10 @@ static struct clk_pll mmpll1 = {
 	.l_reg = 0x0044,
 	.m_reg = 0x0048,
 	.n_reg = 0x004c,
-	.config_reg = 0x0054,
+	.config_reg = 0x0050,
 	.mode_reg = 0x0040,
 	.status_reg = 0x005c,
+	.status_bit = 17,
         .clkr.hw.init = &(struct clk_init_data){
                 .name = "mmpll1",
                 .parent_names = (const char *[]){ "xo" },
@@ -218,7 +241,7 @@ static struct clk_pll mmpll2 = {
 	.l_reg = 0x4104,
 	.m_reg = 0x4108,
 	.n_reg = 0x410c,
-	.config_reg = 0x4114,
+	.config_reg = 0x4110,
 	.mode_reg = 0x4100,
 	.status_reg = 0x411c,
         .clkr.hw.init = &(struct clk_init_data){
@@ -233,9 +256,10 @@ static struct clk_pll mmpll3 = {
 	.l_reg = 0x0084,
 	.m_reg = 0x0088,
 	.n_reg = 0x008c,
-	.config_reg = 0x0094,
+	.config_reg = 0x0090,
 	.mode_reg = 0x0080,
 	.status_reg = 0x009c,
+	.status_bit = 17,
         .clkr.hw.init = &(struct clk_init_data){
                 .name = "mmpll3",
                 .parent_names = (const char *[]){ "xo" },
@@ -496,15 +520,8 @@ static struct clk_rcg2 jpeg2_clk_src = {
 	},
 };
 
-static struct freq_tbl ftbl_mdss_pclk0_clk[] = {
-	F(125000000, P_DSI0PLL, 2, 0, 0),
-	F(250000000, P_DSI0PLL, 1, 0, 0),
-	{ }
-};
-
-static struct freq_tbl ftbl_mdss_pclk1_clk[] = {
-	F(125000000, P_DSI1PLL, 2, 0, 0),
-	F(250000000, P_DSI1PLL, 1, 0, 0),
+static struct freq_tbl pixel_freq_tbl[] = {
+	{ .src = P_DSI0PLL },
 	{ }
 };
 
@@ -513,12 +530,13 @@ static struct clk_rcg2 pclk0_clk_src = {
 	.mnd_width = 8,
 	.hid_width = 5,
 	.parent_map = mmcc_xo_dsi_hdmi_edp_gpll0_map,
-	.freq_tbl = ftbl_mdss_pclk0_clk,
+	.freq_tbl = pixel_freq_tbl,
 	.clkr.hw.init = &(struct clk_init_data){
 		.name = "pclk0_clk_src",
 		.parent_names = mmcc_xo_dsi_hdmi_edp_gpll0,
 		.num_parents = 6,
-		.ops = &clk_rcg2_ops,
+		.ops = &clk_pixel_ops,
+		.flags = CLK_SET_RATE_PARENT,
 	},
 };
 
@@ -527,12 +545,13 @@ static struct clk_rcg2 pclk1_clk_src = {
 	.mnd_width = 8,
 	.hid_width = 5,
 	.parent_map = mmcc_xo_dsi_hdmi_edp_gpll0_map,
-	.freq_tbl = ftbl_mdss_pclk1_clk,
+	.freq_tbl = pixel_freq_tbl,
 	.clkr.hw.init = &(struct clk_init_data){
 		.name = "pclk1_clk_src",
 		.parent_names = mmcc_xo_dsi_hdmi_edp_gpll0,
 		.num_parents = 6,
-		.ops = &clk_rcg2_ops,
+		.ops = &clk_pixel_ops,
+		.flags = CLK_SET_RATE_PARENT,
 	},
 };
 
@@ -750,41 +769,36 @@ static struct clk_rcg2 cpp_clk_src = {
 	},
 };
 
-static struct freq_tbl ftbl_mdss_byte0_clk[] = {
-	F(93750000, P_DSI0PLL, 8, 0, 0),
-	F(187500000, P_DSI0PLL, 4, 0, 0),
-	{ }
-};
-
-static struct freq_tbl ftbl_mdss_byte1_clk[] = {
-	F(93750000, P_DSI1PLL, 8, 0, 0),
-	F(187500000, P_DSI1PLL, 4, 0, 0),
+static struct freq_tbl byte_freq_tbl[] = {
+	{ .src = P_DSI0PLL_BYTE },
 	{ }
 };
 
 static struct clk_rcg2 byte0_clk_src = {
 	.cmd_rcgr = 0x2120,
 	.hid_width = 5,
-	.parent_map = mmcc_xo_dsi_hdmi_edp_gpll0_map,
-	.freq_tbl = ftbl_mdss_byte0_clk,
+	.parent_map = mmcc_xo_dsibyte_hdmi_edp_gpll0_map,
+	.freq_tbl = byte_freq_tbl,
 	.clkr.hw.init = &(struct clk_init_data){
 		.name = "byte0_clk_src",
-		.parent_names = mmcc_xo_dsi_hdmi_edp_gpll0,
+		.parent_names = mmcc_xo_dsibyte_hdmi_edp_gpll0,
 		.num_parents = 6,
-		.ops = &clk_rcg2_ops,
+		.ops = &clk_byte_ops,
+		.flags = CLK_SET_RATE_PARENT,
 	},
 };
 
 static struct clk_rcg2 byte1_clk_src = {
 	.cmd_rcgr = 0x2140,
 	.hid_width = 5,
-	.parent_map = mmcc_xo_dsi_hdmi_edp_gpll0_map,
-	.freq_tbl = ftbl_mdss_byte1_clk,
+	.parent_map = mmcc_xo_dsibyte_hdmi_edp_gpll0_map,
+	.freq_tbl = byte_freq_tbl,
 	.clkr.hw.init = &(struct clk_init_data){
 		.name = "byte1_clk_src",
-		.parent_names = mmcc_xo_dsi_hdmi_edp_gpll0,
+		.parent_names = mmcc_xo_dsibyte_hdmi_edp_gpll0,
 		.num_parents = 6,
-		.ops = &clk_rcg2_ops,
+		.ops = &clk_byte_ops,
+		.flags = CLK_SET_RATE_PARENT,
 	},
 };
 
@@ -822,12 +836,12 @@ static struct clk_rcg2 edplink_clk_src = {
 		.parent_names = mmcc_xo_dsi_hdmi_edp_gpll0,
 		.num_parents = 6,
 		.ops = &clk_rcg2_ops,
+		.flags = CLK_SET_RATE_PARENT,
 	},
 };
 
-static struct freq_tbl ftbl_mdss_edppixel_clk[] = {
-	F(175000000, P_EDPVCO, 2, 0, 0),
-	F(350000000, P_EDPVCO, 11, 0, 0),
+static struct freq_tbl edp_pixel_freq_tbl[] = {
+	{ .src = P_EDPVCO },
 	{ }
 };
 
@@ -836,12 +850,12 @@ static struct clk_rcg2 edppixel_clk_src = {
 	.mnd_width = 8,
 	.hid_width = 5,
 	.parent_map = mmcc_xo_dsi_hdmi_edp_map,
-	.freq_tbl = ftbl_mdss_edppixel_clk,
+	.freq_tbl = edp_pixel_freq_tbl,
 	.clkr.hw.init = &(struct clk_init_data){
 		.name = "edppixel_clk_src",
 		.parent_names = mmcc_xo_dsi_hdmi_edp,
 		.num_parents = 6,
-		.ops = &clk_rcg2_ops,
+		.ops = &clk_edp_pixel_ops,
 	},
 };
 
@@ -853,11 +867,11 @@ static struct freq_tbl ftbl_mdss_esc0_1_clk[] = {
 static struct clk_rcg2 esc0_clk_src = {
 	.cmd_rcgr = 0x2160,
 	.hid_width = 5,
-	.parent_map = mmcc_xo_dsi_hdmi_edp_gpll0_map,
+	.parent_map = mmcc_xo_dsibyte_hdmi_edp_gpll0_map,
 	.freq_tbl = ftbl_mdss_esc0_1_clk,
 	.clkr.hw.init = &(struct clk_init_data){
 		.name = "esc0_clk_src",
-		.parent_names = mmcc_xo_dsi_hdmi_edp_gpll0,
+		.parent_names = mmcc_xo_dsibyte_hdmi_edp_gpll0,
 		.num_parents = 6,
 		.ops = &clk_rcg2_ops,
 	},
@@ -866,26 +880,18 @@ static struct clk_rcg2 esc0_clk_src = {
 static struct clk_rcg2 esc1_clk_src = {
 	.cmd_rcgr = 0x2180,
 	.hid_width = 5,
-	.parent_map = mmcc_xo_dsi_hdmi_edp_gpll0_map,
+	.parent_map = mmcc_xo_dsibyte_hdmi_edp_gpll0_map,
 	.freq_tbl = ftbl_mdss_esc0_1_clk,
 	.clkr.hw.init = &(struct clk_init_data){
 		.name = "esc1_clk_src",
-		.parent_names = mmcc_xo_dsi_hdmi_edp_gpll0,
+		.parent_names = mmcc_xo_dsibyte_hdmi_edp_gpll0,
 		.num_parents = 6,
 		.ops = &clk_rcg2_ops,
 	},
 };
 
-static struct freq_tbl ftbl_mdss_extpclk_clk[] = {
-	F(25200000, P_HDMIPLL, 1, 0, 0),
-	F(27000000, P_HDMIPLL, 1, 0, 0),
-	F(27030000, P_HDMIPLL, 1, 0, 0),
-	F(65000000, P_HDMIPLL, 1, 0, 0),
-	F(74250000, P_HDMIPLL, 1, 0, 0),
-	F(108000000, P_HDMIPLL, 1, 0, 0),
-	F(148500000, P_HDMIPLL, 1, 0, 0),
-	F(268500000, P_HDMIPLL, 1, 0, 0),
-	F(297000000, P_HDMIPLL, 1, 0, 0),
+static struct freq_tbl extpclk_freq_tbl[] = {
+	{ .src = P_HDMIPLL },
 	{ }
 };
 
@@ -893,12 +899,13 @@ static struct clk_rcg2 extpclk_clk_src = {
 	.cmd_rcgr = 0x2060,
 	.hid_width = 5,
 	.parent_map = mmcc_xo_dsi_hdmi_edp_gpll0_map,
-	.freq_tbl = ftbl_mdss_extpclk_clk,
+	.freq_tbl = extpclk_freq_tbl,
 	.clkr.hw.init = &(struct clk_init_data){
 		.name = "extpclk_clk_src",
 		.parent_names = mmcc_xo_dsi_hdmi_edp_gpll0,
 		.num_parents = 6,
-		.ops = &clk_rcg2_ops,
+		.ops = &clk_byte_ops,
+		.flags = CLK_SET_RATE_PARENT,
 	},
 };
 
@@ -2318,7 +2325,7 @@ static const struct pll_config mmpll1_config = {
 	.vco_val = 0x0,
 	.vco_mask = 0x3 << 20,
 	.pre_div_val = 0x0,
-	.pre_div_mask = 0x3 << 12,
+	.pre_div_mask = 0x7 << 12,
 	.post_div_val = 0x0,
 	.post_div_mask = 0x3 << 8,
 	.mn_ena_mask = BIT(24),
@@ -2332,7 +2339,7 @@ static struct pll_config mmpll3_config = {
 	.vco_val = 0x0,
 	.vco_mask = 0x3 << 20,
 	.pre_div_val = 0x0,
-	.pre_div_mask = 0x3 << 12,
+	.pre_div_mask = 0x7 << 12,
 	.post_div_val = 0x0,
 	.post_div_mask = 0x3 << 8,
 	.mn_ena_mask = BIT(24),
@@ -2524,88 +2531,39 @@ static const struct regmap_config mmcc_msm8974_regmap_config = {
 	.fast_io	= true,
 };
 
+static const struct qcom_cc_desc mmcc_msm8974_desc = {
+	.config = &mmcc_msm8974_regmap_config,
+	.clks = mmcc_msm8974_clocks,
+	.num_clks = ARRAY_SIZE(mmcc_msm8974_clocks),
+	.resets = mmcc_msm8974_resets,
+	.num_resets = ARRAY_SIZE(mmcc_msm8974_resets),
+};
+
 static const struct of_device_id mmcc_msm8974_match_table[] = {
 	{ .compatible = "qcom,mmcc-msm8974" },
 	{ }
 };
 MODULE_DEVICE_TABLE(of, mmcc_msm8974_match_table);
 
-struct qcom_cc {
-	struct qcom_reset_controller reset;
-	struct clk_onecell_data data;
-	struct clk *clks[];
-};
-
 static int mmcc_msm8974_probe(struct platform_device *pdev)
 {
-	void __iomem *base;
-	struct resource *res;
-	int i, ret;
-	struct device *dev = &pdev->dev;
-	struct clk *clk;
-	struct clk_onecell_data *data;
-	struct clk **clks;
+	int ret;
 	struct regmap *regmap;
-	size_t num_clks;
-	struct qcom_reset_controller *reset;
-	struct qcom_cc *cc;
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	base = devm_ioremap_resource(dev, res);
-	if (IS_ERR(base))
-		return PTR_ERR(base);
-
-	regmap = devm_regmap_init_mmio(dev, base, &mmcc_msm8974_regmap_config);
-	if (IS_ERR(regmap))
-		return PTR_ERR(regmap);
-
-	num_clks = ARRAY_SIZE(mmcc_msm8974_clocks);
-	cc = devm_kzalloc(dev, sizeof(*cc) + sizeof(*clks) * num_clks,
-			  GFP_KERNEL);
-	if (!cc)
-		return -ENOMEM;
-
-	clks = cc->clks;
-	data = &cc->data;
-	data->clks = clks;
-	data->clk_num = num_clks;
-
-	clk_pll_configure_sr_hpm_lp(&mmpll1, regmap, &mmpll1_config, true);
-	clk_pll_configure_sr_hpm_lp(&mmpll3, regmap, &mmpll3_config, false);
-
-	for (i = 0; i < num_clks; i++) {
-		if (!mmcc_msm8974_clocks[i])
-			continue;
-		clk = devm_clk_register_regmap(dev, mmcc_msm8974_clocks[i]);
-		if (IS_ERR(clk))
-			return PTR_ERR(clk);
-		clks[i] = clk;
-	}
 
-	ret = of_clk_add_provider(dev->of_node, of_clk_src_onecell_get, data);
+	ret = qcom_cc_probe(pdev, &mmcc_msm8974_desc);
 	if (ret)
 		return ret;
 
-	reset = &cc->reset;
-	reset->rcdev.of_node = dev->of_node;
-	reset->rcdev.ops = &qcom_reset_ops,
-	reset->rcdev.owner = THIS_MODULE,
-	reset->rcdev.nr_resets = ARRAY_SIZE(mmcc_msm8974_resets),
-	reset->regmap = regmap;
-	reset->reset_map = mmcc_msm8974_resets,
-	platform_set_drvdata(pdev, &reset->rcdev);
-
-	ret = reset_controller_register(&reset->rcdev);
-	if (ret)
-		of_clk_del_provider(dev->of_node);
+	regmap = dev_get_regmap(&pdev->dev, NULL);
+	clk_pll_configure_sr_hpm_lp(&mmpll1, regmap, &mmpll1_config, true);
+	clk_pll_configure_sr_hpm_lp(&mmpll3, regmap, &mmpll3_config, false);
 
-	return ret;
+	return 0;
 }
 
 static int mmcc_msm8974_remove(struct platform_device *pdev)
 {
-	of_clk_del_provider(pdev->dev.of_node);
-	reset_controller_unregister(platform_get_drvdata(pdev));
+	qcom_cc_remove(pdev);
 	return 0;
 }
 
diff --git a/drivers/clk/rockchip/clk-rockchip.c b/drivers/clk/rockchip/clk-rockchip.c
index 967c141b1a2..4cf838d52ef 100644
--- a/drivers/clk/rockchip/clk-rockchip.c
+++ b/drivers/clk/rockchip/clk-rockchip.c
@@ -24,8 +24,7 @@ static DEFINE_SPINLOCK(clk_lock);
  * Gate clocks
  */
 
-static void __init rk2928_gate_clk_init(struct device_node *node,
-					 void *data)
+static void __init rk2928_gate_clk_init(struct device_node *node)
 {
 	struct clk_onecell_data *clk_data;
 	const char *clk_parent;
diff --git a/drivers/clk/samsung/Kconfig b/drivers/clk/samsung/Kconfig
new file mode 100644
index 00000000000..84196ecdaa1
--- /dev/null
+++ b/drivers/clk/samsung/Kconfig
@@ -0,0 +1,26 @@
+config COMMON_CLK_SAMSUNG
+	bool
+	select COMMON_CLK
+
+config S3C2410_COMMON_CLK
+	bool
+	select COMMON_CLK_SAMSUNG
+	help
+	  Build the s3c2410 clock driver based on the common clock framework.
+
+config S3C2410_COMMON_DCLK
+	bool
+	select COMMON_CLK_SAMSUNG
+	select REGMAP_MMIO
+	help
+	  Temporary symbol to build the dclk driver based on the common clock
+	  framework.
+
+config S3C2412_COMMON_CLK
+	bool
+	select COMMON_CLK_SAMSUNG
+
+config S3C2443_COMMON_CLK
+	bool
+	select COMMON_CLK_SAMSUNG
+
diff --git a/drivers/clk/samsung/Makefile b/drivers/clk/samsung/Makefile
index 8eb4799237f..69e81773164 100644
--- a/drivers/clk/samsung/Makefile
+++ b/drivers/clk/samsung/Makefile
@@ -3,9 +3,16 @@
 #
 
 obj-$(CONFIG_COMMON_CLK)	+= clk.o clk-pll.o
+obj-$(CONFIG_SOC_EXYNOS3250)	+= clk-exynos3250.o
 obj-$(CONFIG_ARCH_EXYNOS4)	+= clk-exynos4.o
 obj-$(CONFIG_SOC_EXYNOS5250)	+= clk-exynos5250.o
+obj-$(CONFIG_SOC_EXYNOS5260)	+= clk-exynos5260.o
+obj-$(CONFIG_SOC_EXYNOS5410)	+= clk-exynos5410.o
 obj-$(CONFIG_SOC_EXYNOS5420)	+= clk-exynos5420.o
 obj-$(CONFIG_SOC_EXYNOS5440)	+= clk-exynos5440.o
 obj-$(CONFIG_ARCH_EXYNOS)	+= clk-exynos-audss.o
+obj-$(CONFIG_S3C2410_COMMON_CLK)+= clk-s3c2410.o
+obj-$(CONFIG_S3C2410_COMMON_DCLK)+= clk-s3c2410-dclk.o
+obj-$(CONFIG_S3C2412_COMMON_CLK)+= clk-s3c2412.o
+obj-$(CONFIG_S3C2443_COMMON_CLK)+= clk-s3c2443.o
 obj-$(CONFIG_ARCH_S3C64XX)	+= clk-s3c64xx.o
diff --git a/drivers/clk/samsung/clk-exynos3250.c b/drivers/clk/samsung/clk-exynos3250.c
new file mode 100644
index 00000000000..7a17bd40d1d
--- /dev/null
+++ b/drivers/clk/samsung/clk-exynos3250.c
@@ -0,0 +1,780 @@
+/*
+ * 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 version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Common Clock Framework support for Exynos3250 SoC.
+ */
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/platform_device.h>
+#include <linux/syscore_ops.h>
+
+#include <dt-bindings/clock/exynos3250.h>
+
+#include "clk.h"
+#include "clk-pll.h"
+
+#define SRC_LEFTBUS		0x4200
+#define DIV_LEFTBUS		0x4500
+#define GATE_IP_LEFTBUS		0x4800
+#define SRC_RIGHTBUS		0x8200
+#define DIV_RIGHTBUS		0x8500
+#define GATE_IP_RIGHTBUS	0x8800
+#define GATE_IP_PERIR		0x8960
+#define MPLL_LOCK		0xc010
+#define MPLL_CON0		0xc110
+#define VPLL_LOCK		0xc020
+#define VPLL_CON0		0xc120
+#define UPLL_LOCK		0xc030
+#define UPLL_CON0		0xc130
+#define SRC_TOP0		0xc210
+#define SRC_TOP1		0xc214
+#define SRC_CAM			0xc220
+#define SRC_MFC			0xc228
+#define SRC_G3D			0xc22c
+#define SRC_LCD			0xc234
+#define SRC_ISP			0xc238
+#define SRC_FSYS		0xc240
+#define SRC_PERIL0		0xc250
+#define SRC_PERIL1		0xc254
+#define SRC_MASK_TOP		0xc310
+#define SRC_MASK_CAM		0xc320
+#define SRC_MASK_LCD		0xc334
+#define SRC_MASK_ISP		0xc338
+#define SRC_MASK_FSYS		0xc340
+#define SRC_MASK_PERIL0		0xc350
+#define SRC_MASK_PERIL1		0xc354
+#define DIV_TOP			0xc510
+#define DIV_CAM			0xc520
+#define DIV_MFC			0xc528
+#define DIV_G3D			0xc52c
+#define DIV_LCD			0xc534
+#define DIV_ISP			0xc538
+#define DIV_FSYS0		0xc540
+#define DIV_FSYS1		0xc544
+#define DIV_FSYS2		0xc548
+#define DIV_PERIL0		0xc550
+#define DIV_PERIL1		0xc554
+#define DIV_PERIL3		0xc55c
+#define DIV_PERIL4		0xc560
+#define DIV_PERIL5		0xc564
+#define DIV_CAM1		0xc568
+#define CLKDIV2_RATIO		0xc580
+#define GATE_SCLK_CAM		0xc820
+#define GATE_SCLK_MFC		0xc828
+#define GATE_SCLK_G3D		0xc82c
+#define GATE_SCLK_LCD		0xc834
+#define GATE_SCLK_ISP_TOP	0xc838
+#define GATE_SCLK_FSYS		0xc840
+#define GATE_SCLK_PERIL		0xc850
+#define GATE_IP_CAM		0xc920
+#define GATE_IP_MFC		0xc928
+#define GATE_IP_G3D		0xc92c
+#define GATE_IP_LCD		0xc934
+#define GATE_IP_ISP		0xc938
+#define GATE_IP_FSYS		0xc940
+#define GATE_IP_PERIL		0xc950
+#define GATE_BLOCK		0xc970
+#define APLL_LOCK		0x14000
+#define APLL_CON0		0x14100
+#define SRC_CPU			0x14200
+#define DIV_CPU0		0x14500
+#define DIV_CPU1		0x14504
+
+/* list of PLLs to be registered */
+enum exynos3250_plls {
+	apll, mpll, vpll, upll,
+	nr_plls
+};
+
+static void __iomem *reg_base;
+
+/*
+ * Support for CMU save/restore across system suspends
+ */
+#ifdef CONFIG_PM_SLEEP
+static struct samsung_clk_reg_dump *exynos3250_clk_regs;
+
+static unsigned long exynos3250_cmu_clk_regs[] __initdata = {
+	SRC_LEFTBUS,
+	DIV_LEFTBUS,
+	GATE_IP_LEFTBUS,
+	SRC_RIGHTBUS,
+	DIV_RIGHTBUS,
+	GATE_IP_RIGHTBUS,
+	GATE_IP_PERIR,
+	MPLL_LOCK,
+	MPLL_CON0,
+	VPLL_LOCK,
+	VPLL_CON0,
+	UPLL_LOCK,
+	UPLL_CON0,
+	SRC_TOP0,
+	SRC_TOP1,
+	SRC_CAM,
+	SRC_MFC,
+	SRC_G3D,
+	SRC_LCD,
+	SRC_ISP,
+	SRC_FSYS,
+	SRC_PERIL0,
+	SRC_PERIL1,
+	SRC_MASK_TOP,
+	SRC_MASK_CAM,
+	SRC_MASK_LCD,
+	SRC_MASK_ISP,
+	SRC_MASK_FSYS,
+	SRC_MASK_PERIL0,
+	SRC_MASK_PERIL1,
+	DIV_TOP,
+	DIV_CAM,
+	DIV_MFC,
+	DIV_G3D,
+	DIV_LCD,
+	DIV_ISP,
+	DIV_FSYS0,
+	DIV_FSYS1,
+	DIV_FSYS2,
+	DIV_PERIL0,
+	DIV_PERIL1,
+	DIV_PERIL3,
+	DIV_PERIL4,
+	DIV_PERIL5,
+	DIV_CAM1,
+	CLKDIV2_RATIO,
+	GATE_SCLK_CAM,
+	GATE_SCLK_MFC,
+	GATE_SCLK_G3D,
+	GATE_SCLK_LCD,
+	GATE_SCLK_ISP_TOP,
+	GATE_SCLK_FSYS,
+	GATE_SCLK_PERIL,
+	GATE_IP_CAM,
+	GATE_IP_MFC,
+	GATE_IP_G3D,
+	GATE_IP_LCD,
+	GATE_IP_ISP,
+	GATE_IP_FSYS,
+	GATE_IP_PERIL,
+	GATE_BLOCK,
+	APLL_LOCK,
+	SRC_CPU,
+	DIV_CPU0,
+	DIV_CPU1,
+};
+
+static int exynos3250_clk_suspend(void)
+{
+	samsung_clk_save(reg_base, exynos3250_clk_regs,
+				ARRAY_SIZE(exynos3250_cmu_clk_regs));
+	return 0;
+}
+
+static void exynos3250_clk_resume(void)
+{
+	samsung_clk_restore(reg_base, exynos3250_clk_regs,
+				ARRAY_SIZE(exynos3250_cmu_clk_regs));
+}
+
+static struct syscore_ops exynos3250_clk_syscore_ops = {
+	.suspend = exynos3250_clk_suspend,
+	.resume = exynos3250_clk_resume,
+};
+
+static void exynos3250_clk_sleep_init(void)
+{
+	exynos3250_clk_regs =
+		samsung_clk_alloc_reg_dump(exynos3250_cmu_clk_regs,
+					   ARRAY_SIZE(exynos3250_cmu_clk_regs));
+	if (!exynos3250_clk_regs) {
+		pr_warn("%s: Failed to allocate sleep save data\n", __func__);
+		goto err;
+	}
+
+	register_syscore_ops(&exynos3250_clk_syscore_ops);
+	return;
+err:
+	kfree(exynos3250_clk_regs);
+}
+#else
+static inline void exynos3250_clk_sleep_init(void) { }
+#endif
+
+/* list of all parent clock list */
+PNAME(mout_vpllsrc_p)		= { "fin_pll", };
+
+PNAME(mout_apll_p)		= { "fin_pll", "fout_apll", };
+PNAME(mout_mpll_p)		= { "fin_pll", "fout_mpll", };
+PNAME(mout_vpll_p)		= { "fin_pll", "fout_vpll", };
+PNAME(mout_upll_p)		= { "fin_pll", "fout_upll", };
+
+PNAME(mout_mpll_user_p)		= { "fin_pll", "div_mpll_pre", };
+PNAME(mout_epll_user_p)		= { "fin_pll", "mout_epll", };
+PNAME(mout_core_p)		= { "mout_apll", "mout_mpll_user_c", };
+PNAME(mout_hpm_p)		= { "mout_apll", "mout_mpll_user_c", };
+
+PNAME(mout_ebi_p)		= { "div_aclk_200", "div_aclk_160", };
+PNAME(mout_ebi_1_p)		= { "mout_ebi", "mout_vpll", };
+
+PNAME(mout_gdl_p)		= { "mout_mpll_user_l", };
+PNAME(mout_gdr_p)		= { "mout_mpll_user_r", };
+
+PNAME(mout_aclk_400_mcuisp_sub_p)
+				= { "fin_pll", "div_aclk_400_mcuisp", };
+PNAME(mout_aclk_266_0_p)	= { "div_mpll_pre", "mout_vpll", };
+PNAME(mout_aclk_266_1_p)	= { "mout_epll_user", };
+PNAME(mout_aclk_266_p)		= { "mout_aclk_266_0", "mout_aclk_266_1", };
+PNAME(mout_aclk_266_sub_p)	= { "fin_pll", "div_aclk_266", };
+
+PNAME(group_div_mpll_pre_p)	= { "div_mpll_pre", };
+PNAME(group_epll_vpll_p)	= { "mout_epll_user", "mout_vpll" };
+PNAME(group_sclk_p)		= { "xxti", "xusbxti",
+				    "none", "none",
+				    "none", "none", "div_mpll_pre",
+				    "mout_epll_user", "mout_vpll", };
+PNAME(group_sclk_audio_p)	= { "audiocdclk", "none",
+				    "none", "none",
+				    "xxti", "xusbxti",
+				    "div_mpll_pre", "mout_epll_user",
+				    "mout_vpll", };
+PNAME(group_sclk_cam_blk_p)	= { "xxti", "xusbxti",
+				    "none", "none", "none",
+				    "none", "div_mpll_pre",
+				    "mout_epll_user", "mout_vpll",
+				    "div_cam_blk_320", };
+PNAME(group_sclk_fimd0_p)	= { "xxti", "xusbxti",
+				    "m_bitclkhsdiv4_2l", "none",
+				    "none", "none", "div_mpll_pre",
+				    "mout_epll_user", "mout_vpll",
+				    "none", "none", "none",
+				    "div_lcd_blk_145", };
+
+PNAME(mout_mfc_p)		= { "mout_mfc_0", "mout_mfc_1" };
+PNAME(mout_g3d_p)		= { "mout_g3d_0", "mout_g3d_1" };
+
+static struct samsung_fixed_factor_clock fixed_factor_clks[] __initdata = {
+	FFACTOR(0, "sclk_mpll_1600", "mout_mpll", 1, 1, 0),
+	FFACTOR(0, "sclk_mpll_mif", "mout_mpll", 1, 2, 0),
+	FFACTOR(0, "sclk_bpll", "fout_bpll", 1, 2, 0),
+	FFACTOR(0, "div_cam_blk_320", "sclk_mpll_1600", 1, 5, 0),
+	FFACTOR(0, "div_lcd_blk_145", "sclk_mpll_1600", 1, 11, 0),
+
+	/* HACK: fin_pll hardcoded to xusbxti until detection is implemented. */
+	FFACTOR(CLK_FIN_PLL, "fin_pll", "xusbxti", 1, 1, 0),
+};
+
+static struct samsung_mux_clock 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_LEFTBUS */
+	MUX(CLK_MOUT_MPLL_USER_L, "mout_mpll_user_l", mout_mpll_user_p,
+	    SRC_LEFTBUS, 4, 1),
+	MUX(CLK_MOUT_GDL, "mout_gdl", mout_gdl_p, SRC_LEFTBUS, 0, 1),
+
+	/* SRC_RIGHTBUS */
+	MUX(CLK_MOUT_MPLL_USER_R, "mout_mpll_user_r", mout_mpll_user_p,
+	    SRC_RIGHTBUS, 4, 1),
+	MUX(CLK_MOUT_GDR, "mout_gdr", mout_gdr_p, SRC_RIGHTBUS, 0, 1),
+
+	/* SRC_TOP0 */
+	MUX(CLK_MOUT_EBI, "mout_ebi", mout_ebi_p, SRC_TOP0, 28, 1),
+	MUX(CLK_MOUT_ACLK_200, "mout_aclk_200", group_div_mpll_pre_p,SRC_TOP0, 24, 1),
+	MUX(CLK_MOUT_ACLK_160, "mout_aclk_160", group_div_mpll_pre_p, SRC_TOP0, 20, 1),
+	MUX(CLK_MOUT_ACLK_100, "mout_aclk_100", group_div_mpll_pre_p, SRC_TOP0, 16, 1),
+	MUX(CLK_MOUT_ACLK_266_1, "mout_aclk_266_1", mout_aclk_266_1_p, SRC_TOP0, 14, 1),
+	MUX(CLK_MOUT_ACLK_266_0, "mout_aclk_266_0", mout_aclk_266_0_p, SRC_TOP0, 13, 1),
+	MUX(CLK_MOUT_ACLK_266, "mout_aclk_266", mout_aclk_266_p, SRC_TOP0, 12, 1),
+	MUX(CLK_MOUT_VPLL, "mout_vpll", mout_vpll_p, SRC_TOP0, 8, 1),
+	MUX(CLK_MOUT_EPLL_USER, "mout_epll_user", mout_epll_user_p, SRC_TOP0, 4, 1),
+	MUX(CLK_MOUT_EBI_1, "mout_ebi_1", mout_ebi_1_p, SRC_TOP0, 0, 1),
+
+	/* SRC_TOP1 */
+	MUX(CLK_MOUT_UPLL, "mout_upll", mout_upll_p, SRC_TOP1, 28, 1),
+	MUX(CLK_MOUT_ACLK_400_MCUISP_SUB, "mout_aclk_400_mcuisp_sub", mout_aclk_400_mcuisp_sub_p,
+		SRC_TOP1, 24, 1),
+	MUX(CLK_MOUT_ACLK_266_SUB, "mout_aclk_266_sub", mout_aclk_266_sub_p, SRC_TOP1, 20, 1),
+	MUX(CLK_MOUT_MPLL, "mout_mpll", mout_mpll_p, SRC_TOP1, 12, 1),
+	MUX(CLK_MOUT_ACLK_400_MCUISP, "mout_aclk_400_mcuisp", group_div_mpll_pre_p, SRC_TOP1, 8, 1),
+	MUX(CLK_MOUT_VPLLSRC, "mout_vpllsrc", mout_vpllsrc_p, SRC_TOP1, 0, 1),
+
+	/* SRC_CAM */
+	MUX(CLK_MOUT_CAM1, "mout_cam1", group_sclk_p, SRC_CAM, 20, 4),
+	MUX(CLK_MOUT_CAM_BLK, "mout_cam_blk", group_sclk_cam_blk_p, SRC_CAM, 0, 4),
+
+	/* SRC_MFC */
+	MUX(CLK_MOUT_MFC, "mout_mfc", mout_mfc_p, SRC_MFC, 8, 1),
+	MUX(CLK_MOUT_MFC_1, "mout_mfc_1", group_epll_vpll_p, SRC_MFC, 4, 1),
+	MUX(CLK_MOUT_MFC_0, "mout_mfc_0", group_div_mpll_pre_p, SRC_MFC, 0, 1),
+
+	/* SRC_G3D */
+	MUX(CLK_MOUT_G3D, "mout_g3d", mout_g3d_p, SRC_G3D, 8, 1),
+	MUX(CLK_MOUT_G3D_1, "mout_g3d_1", group_epll_vpll_p, SRC_G3D, 4, 1),
+	MUX(CLK_MOUT_G3D_0, "mout_g3d_0", group_div_mpll_pre_p, SRC_G3D, 0, 1),
+
+	/* SRC_LCD */
+	MUX(CLK_MOUT_MIPI0, "mout_mipi0", group_sclk_p, SRC_LCD, 12, 4),
+	MUX(CLK_MOUT_FIMD0, "mout_fimd0", group_sclk_fimd0_p, SRC_LCD, 0, 4),
+
+	/* SRC_ISP */
+	MUX(CLK_MOUT_UART_ISP, "mout_uart_isp", group_sclk_p, SRC_ISP, 12, 4),
+	MUX(CLK_MOUT_SPI1_ISP, "mout_spi1_isp", group_sclk_p, SRC_ISP, 8, 4),
+	MUX(CLK_MOUT_SPI0_ISP, "mout_spi0_isp", group_sclk_p, SRC_ISP, 4, 4),
+
+	/* 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),
+
+	/* SRC_PERIL0 */
+	MUX(CLK_MOUT_UART1, "mout_uart1", group_sclk_p, SRC_PERIL0, 4, 4),
+	MUX(CLK_MOUT_UART0, "mout_uart0", group_sclk_p, SRC_PERIL0, 0, 4),
+
+	/* SRC_PERIL1 */
+	MUX(CLK_MOUT_SPI1, "mout_spi1", group_sclk_p, SRC_PERIL1, 20, 4),
+	MUX(CLK_MOUT_SPI0, "mout_spi0", group_sclk_p, SRC_PERIL1, 16, 4),
+	MUX(CLK_MOUT_AUDIO, "mout_audio", group_sclk_audio_p, SRC_PERIL1, 4, 4),
+
+	/* SRC_CPU */
+	MUX(CLK_MOUT_MPLL_USER_C, "mout_mpll_user_c", mout_mpll_user_p,
+	    SRC_CPU, 24, 1),
+	MUX(CLK_MOUT_HPM, "mout_hpm", mout_hpm_p, SRC_CPU, 20, 1),
+	MUX(CLK_MOUT_CORE, "mout_core", mout_core_p, SRC_CPU, 16, 1),
+	MUX(CLK_MOUT_APLL, "mout_apll", mout_apll_p, SRC_CPU, 0, 1),
+};
+
+static struct samsung_div_clock 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_LEFTBUS */
+	DIV(CLK_DIV_GPL, "div_gpl", "div_gdl", DIV_LEFTBUS, 4, 3),
+	DIV(CLK_DIV_GDL, "div_gdl", "mout_gdl", DIV_LEFTBUS, 0, 4),
+
+	/* DIV_RIGHTBUS */
+	DIV(CLK_DIV_GPR, "div_gpr", "div_gdr", DIV_RIGHTBUS, 4, 3),
+	DIV(CLK_DIV_GDR, "div_gdr", "mout_gdr", DIV_RIGHTBUS, 0, 4),
+
+	/* DIV_TOP */
+	DIV(CLK_DIV_MPLL_PRE, "div_mpll_pre", "sclk_mpll_mif", DIV_TOP, 28, 2),
+	DIV(CLK_DIV_ACLK_400_MCUISP, "div_aclk_400_mcuisp",
+	    "mout_aclk_400_mcuisp", DIV_TOP, 24, 3),
+	DIV(CLK_DIV_EBI, "div_ebi", "mout_ebi_1", DIV_TOP, 16, 3),
+	DIV(CLK_DIV_ACLK_200, "div_aclk_200", "mout_aclk_200", DIV_TOP, 12, 3),
+	DIV(CLK_DIV_ACLK_160, "div_aclk_160", "mout_aclk_160", DIV_TOP, 8, 3),
+	DIV(CLK_DIV_ACLK_100, "div_aclk_100", "mout_aclk_100", DIV_TOP, 4, 4),
+	DIV(CLK_DIV_ACLK_266, "div_aclk_266", "mout_aclk_266", DIV_TOP, 0, 3),
+
+	/* DIV_CAM */
+	DIV(CLK_DIV_CAM1, "div_cam1", "mout_cam1", DIV_CAM, 20, 4),
+	DIV(CLK_DIV_CAM_BLK, "div_cam_blk", "mout_cam_blk", DIV_CAM, 0, 4),
+
+	/* DIV_MFC */
+	DIV(CLK_DIV_MFC, "div_mfc", "mout_mfc", DIV_MFC, 0, 4),
+
+	/* DIV_G3D */
+	DIV(CLK_DIV_G3D, "div_g3d", "mout_g3d", DIV_G3D, 0, 4),
+
+	/* DIV_LCD */
+	DIV_F(CLK_DIV_MIPI0_PRE, "div_mipi0_pre", "div_mipi0", DIV_LCD, 20, 4,
+		CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DIV_MIPI0, "div_mipi0", "mout_mipi0", DIV_LCD, 16, 4),
+	DIV(CLK_DIV_FIMD0, "div_fimd0", "mout_fimd0", DIV_LCD, 0, 4),
+
+	/* DIV_ISP */
+	DIV(CLK_DIV_UART_ISP, "div_uart_isp", "mout_uart_isp", DIV_ISP, 28, 4),
+	DIV_F(CLK_DIV_SPI1_ISP_PRE, "div_spi1_isp_pre", "div_spi1_isp",
+		DIV_ISP, 20, 8, CLK_SET_RATE_PARENT, 0),
+	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_FSYS0 */
+	DIV_F(CLK_DIV_TSADC_PRE, "div_tsadc_pre", "div_tsadc", DIV_FSYS0, 8, 8,
+		CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DIV_TSADC, "div_tsadc", "mout_tsadc", DIV_FSYS0, 0, 4),
+
+	/* DIV_FSYS1 */
+	DIV_F(CLK_DIV_MMC1_PRE, "div_mmc1_pre", "div_mmc1", DIV_FSYS1, 24, 8,
+		CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DIV_MMC1, "div_mmc1", "mout_mmc1", DIV_FSYS1, 16, 4),
+	DIV_F(CLK_DIV_MMC0_PRE, "div_mmc0_pre", "div_mmc0", DIV_FSYS1, 8, 8,
+		CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DIV_MMC0, "div_mmc0", "mout_mmc0", DIV_FSYS1, 0, 4),
+
+	/* DIV_PERIL0 */
+	DIV(CLK_DIV_UART1, "div_uart1", "mout_uart1", DIV_PERIL0, 4, 4),
+	DIV(CLK_DIV_UART0, "div_uart0", "mout_uart0", DIV_PERIL0, 0, 4),
+
+	/* DIV_PERIL1 */
+	DIV_F(CLK_DIV_SPI1_PRE, "div_spi1_pre", "div_spi1", DIV_PERIL1, 24, 8,
+		CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DIV_SPI1, "div_spi1", "mout_spi1", DIV_PERIL1, 16, 4),
+	DIV_F(CLK_DIV_SPI0_PRE, "div_spi0_pre", "div_spi0", DIV_PERIL1, 8, 8,
+		CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DIV_SPI0, "div_spi0", "mout_spi0", DIV_PERIL1, 0, 4),
+
+	/* DIV_PERIL4 */
+	DIV(CLK_DIV_PCM, "div_pcm", "div_audio", DIV_PERIL4, 20, 8),
+	DIV(CLK_DIV_AUDIO, "div_audio", "mout_audio", DIV_PERIL4, 16, 4),
+
+	/* DIV_PERIL5 */
+	DIV(CLK_DIV_I2S, "div_i2s", "div_audio", DIV_PERIL5, 8, 6),
+
+	/* DIV_CPU0 */
+	DIV(CLK_DIV_CORE2, "div_core2", "div_core", DIV_CPU0, 28, 3),
+	DIV(CLK_DIV_APLL, "div_apll", "mout_apll", DIV_CPU0, 24, 3),
+	DIV(CLK_DIV_PCLK_DBG, "div_pclk_dbg", "div_core2", DIV_CPU0, 20, 3),
+	DIV(CLK_DIV_ATB, "div_atb", "div_core2", DIV_CPU0, 16, 3),
+	DIV(CLK_DIV_COREM, "div_corem", "div_core2", DIV_CPU0, 4, 3),
+	DIV(CLK_DIV_CORE, "div_core", "mout_core", DIV_CPU0, 0, 3),
+
+	/* DIV_CPU1 */
+	DIV(CLK_DIV_HPM, "div_hpm", "div_copy", DIV_CPU1, 4, 3),
+	DIV(CLK_DIV_COPY, "div_copy", "mout_hpm", DIV_CPU1, 0, 3),
+};
+
+static struct samsung_gate_clock gate_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.
+	 */
+
+	/* GATE_IP_LEFTBUS */
+	GATE(CLK_ASYNC_G3D, "async_g3d", "div_aclk_100", GATE_IP_LEFTBUS, 6,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_ASYNC_MFCL, "async_mfcl", "div_aclk_100", GATE_IP_LEFTBUS, 4,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PPMULEFT, "ppmuleft", "div_aclk_100", GATE_IP_LEFTBUS, 1,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_GPIO_LEFT, "gpio_left", "div_aclk_100", GATE_IP_LEFTBUS, 0,
+		CLK_IGNORE_UNUSED, 0),
+
+	/* GATE_IP_RIGHTBUS */
+	GATE(CLK_ASYNC_ISPMX, "async_ispmx", "div_aclk_100",
+		GATE_IP_RIGHTBUS, 9, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_ASYNC_FSYSD, "async_fsysd", "div_aclk_100",
+		GATE_IP_RIGHTBUS, 5, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_ASYNC_LCD0X, "async_lcd0x", "div_aclk_100",
+		GATE_IP_RIGHTBUS, 3, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_ASYNC_CAMX, "async_camx", "div_aclk_100", GATE_IP_RIGHTBUS, 2,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PPMURIGHT, "ppmuright", "div_aclk_100", GATE_IP_RIGHTBUS, 1,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_GPIO_RIGHT, "gpio_right", "div_aclk_100", GATE_IP_RIGHTBUS, 0,
+		CLK_IGNORE_UNUSED, 0),
+
+	/* GATE_IP_PERIR */
+	GATE(CLK_MONOCNT, "monocnt", "div_aclk_100", GATE_IP_PERIR, 22,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TZPC6, "tzpc6", "div_aclk_100", GATE_IP_PERIR, 21,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PROVISIONKEY1, "provisionkey1", "div_aclk_100",
+		GATE_IP_PERIR, 20, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PROVISIONKEY0, "provisionkey0", "div_aclk_100",
+		GATE_IP_PERIR, 19, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_CMU_ISPPART, "cmu_isppart", "div_aclk_100", GATE_IP_PERIR, 18,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TMU_APBIF, "tmu_apbif", "div_aclk_100",
+		GATE_IP_PERIR, 17, 0, 0),
+	GATE(CLK_KEYIF, "keyif", "div_aclk_100", GATE_IP_PERIR, 16, 0, 0),
+	GATE(CLK_RTC, "rtc", "div_aclk_100", GATE_IP_PERIR, 15, 0, 0),
+	GATE(CLK_WDT, "wdt", "div_aclk_100", GATE_IP_PERIR, 14, 0, 0),
+	GATE(CLK_MCT, "mct", "div_aclk_100", GATE_IP_PERIR, 13, 0, 0),
+	GATE(CLK_SECKEY, "seckey", "div_aclk_100", GATE_IP_PERIR, 12,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TZPC5, "tzpc5", "div_aclk_100", GATE_IP_PERIR, 10,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TZPC4, "tzpc4", "div_aclk_100", GATE_IP_PERIR, 9,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TZPC3, "tzpc3", "div_aclk_100", GATE_IP_PERIR, 8,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TZPC2, "tzpc2", "div_aclk_100", GATE_IP_PERIR, 7,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TZPC1, "tzpc1", "div_aclk_100", GATE_IP_PERIR, 6,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TZPC0, "tzpc0", "div_aclk_100", GATE_IP_PERIR, 5,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_CMU_COREPART, "cmu_corepart", "div_aclk_100", GATE_IP_PERIR, 4,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_CMU_TOPPART, "cmu_toppart", "div_aclk_100", GATE_IP_PERIR, 3,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PMU_APBIF, "pmu_apbif", "div_aclk_100", GATE_IP_PERIR, 2,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_SYSREG, "sysreg", "div_aclk_100", GATE_IP_PERIR, 1,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_CHIP_ID, "chip_id", "div_aclk_100", GATE_IP_PERIR, 0,
+		CLK_IGNORE_UNUSED, 0),
+
+	/* GATE_SCLK_CAM */
+	GATE(CLK_SCLK_JPEG, "sclk_jpeg", "div_cam_blk",
+		GATE_SCLK_CAM, 8, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_M2MSCALER, "sclk_m2mscaler", "div_cam_blk",
+		GATE_SCLK_CAM, 2, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_GSCALER1, "sclk_gscaler1", "div_cam_blk",
+		GATE_SCLK_CAM, 1, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_GSCALER0, "sclk_gscaler0", "div_cam_blk",
+		GATE_SCLK_CAM, 0, CLK_SET_RATE_PARENT, 0),
+
+	/* GATE_SCLK_MFC */
+	GATE(CLK_SCLK_MFC, "sclk_mfc", "div_mfc",
+		GATE_SCLK_MFC, 0, CLK_SET_RATE_PARENT, 0),
+
+	/* GATE_SCLK_G3D */
+	GATE(CLK_SCLK_G3D, "sclk_g3d", "div_g3d",
+		GATE_SCLK_G3D, 0, CLK_SET_RATE_PARENT, 0),
+
+	/* GATE_SCLK_LCD */
+	GATE(CLK_SCLK_MIPIDPHY2L, "sclk_mipidphy2l", "div_mipi0",
+		GATE_SCLK_LCD, 4, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_MIPI0, "sclk_mipi0", "div_mipi0_pre",
+		GATE_SCLK_LCD, 3, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_FIMD0, "sclk_fimd0", "div_fimd0",
+		GATE_SCLK_LCD, 0, CLK_SET_RATE_PARENT, 0),
+
+	/* GATE_SCLK_ISP_TOP */
+	GATE(CLK_SCLK_CAM1, "sclk_cam1", "div_cam1",
+		GATE_SCLK_ISP_TOP, 4, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_UART_ISP, "sclk_uart_isp", "div_uart_isp",
+		GATE_SCLK_ISP_TOP, 3, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_SPI1_ISP, "sclk_spi1_isp", "div_spi1_isp",
+		GATE_SCLK_ISP_TOP, 2, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_SPI0_ISP, "sclk_spi0_isp", "div_spi0_isp",
+		GATE_SCLK_ISP_TOP, 1, CLK_SET_RATE_PARENT, 0),
+
+	/* GATE_SCLK_FSYS */
+	GATE(CLK_SCLK_UPLL, "sclk_upll", "mout_upll", GATE_SCLK_FSYS, 10, 0, 0),
+	GATE(CLK_SCLK_TSADC, "sclk_tsadc", "div_tsadc_pre",
+		GATE_SCLK_FSYS, 9, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_EBI, "sclk_ebi", "div_ebi",
+		GATE_SCLK_FSYS, 6, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_MMC1, "sclk_mmc1", "div_mmc1_pre",
+		GATE_SCLK_FSYS, 1, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_MMC0, "sclk_mmc0", "div_mmc0_pre",
+		GATE_SCLK_FSYS, 0, CLK_SET_RATE_PARENT, 0),
+
+	/* GATE_SCLK_PERIL */
+	GATE(CLK_SCLK_I2S, "sclk_i2s", "div_i2s",
+		GATE_SCLK_PERIL, 18, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_PCM, "sclk_pcm", "div_pcm",
+		GATE_SCLK_PERIL, 16, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_SPI1, "sclk_spi1", "div_spi1_pre",
+		GATE_SCLK_PERIL, 7, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_SPI0, "sclk_spi0", "div_spi0_pre",
+		GATE_SCLK_PERIL, 6, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_UART1, "sclk_uart1", "div_uart1",
+		GATE_SCLK_PERIL, 1, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_UART0, "sclk_uart0", "div_uart0",
+		GATE_SCLK_PERIL, 0, CLK_SET_RATE_PARENT, 0),
+
+	/* GATE_IP_CAM */
+	GATE(CLK_QEJPEG, "qejpeg", "div_cam_blk_320", GATE_IP_CAM, 19,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PIXELASYNCM1, "pixelasyncm1", "div_cam_blk_320",
+		GATE_IP_CAM, 18, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PIXELASYNCM0, "pixelasyncm0", "div_cam_blk_320",
+		GATE_IP_CAM, 17, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PPMUCAMIF, "ppmucamif", "div_cam_blk_320",
+		GATE_IP_CAM, 16, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_QEM2MSCALER, "qem2mscaler", "div_cam_blk_320",
+		GATE_IP_CAM, 14, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_QEGSCALER1, "qegscaler1", "div_cam_blk_320",
+		GATE_IP_CAM, 13, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_QEGSCALER0, "qegscaler0", "div_cam_blk_320",
+		GATE_IP_CAM, 12, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_SMMUJPEG, "smmujpeg", "div_cam_blk_320",
+		GATE_IP_CAM, 11, 0, 0),
+	GATE(CLK_SMMUM2M2SCALER, "smmum2m2scaler", "div_cam_blk_320",
+		GATE_IP_CAM, 9, 0, 0),
+	GATE(CLK_SMMUGSCALER1, "smmugscaler1", "div_cam_blk_320",
+		GATE_IP_CAM, 8, 0, 0),
+	GATE(CLK_SMMUGSCALER0, "smmugscaler0", "div_cam_blk_320",
+		GATE_IP_CAM, 7, 0, 0),
+	GATE(CLK_JPEG, "jpeg", "div_cam_blk_320", GATE_IP_CAM, 6, 0, 0),
+	GATE(CLK_M2MSCALER, "m2mscaler", "div_cam_blk_320",
+		GATE_IP_CAM, 2, 0, 0),
+	GATE(CLK_GSCALER1, "gscaler1", "div_cam_blk_320", GATE_IP_CAM, 1, 0, 0),
+	GATE(CLK_GSCALER0, "gscaler0", "div_cam_blk_320", GATE_IP_CAM, 0, 0, 0),
+
+	/* GATE_IP_MFC */
+	GATE(CLK_QEMFC, "qemfc", "div_aclk_200", GATE_IP_MFC, 5,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PPMUMFC_L, "ppmumfc_l", "div_aclk_200", GATE_IP_MFC, 3,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_SMMUMFC_L, "smmumfc_l", "div_aclk_200", GATE_IP_MFC, 1, 0, 0),
+	GATE(CLK_MFC, "mfc", "div_aclk_200", GATE_IP_MFC, 0, 0, 0),
+
+	/* GATE_IP_G3D */
+	GATE(CLK_SMMUG3D, "smmug3d", "div_aclk_200", GATE_IP_G3D, 3, 0, 0),
+	GATE(CLK_QEG3D, "qeg3d", "div_aclk_200", GATE_IP_G3D, 2,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PPMUG3D, "ppmug3d", "div_aclk_200", GATE_IP_G3D, 1,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_G3D, "g3d", "div_aclk_200", GATE_IP_G3D, 0, 0, 0),
+
+	/* GATE_IP_LCD */
+	GATE(CLK_QE_CH1_LCD, "qe_ch1_lcd", "div_aclk_160", GATE_IP_LCD, 7,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_QE_CH0_LCD, "qe_ch0_lcd", "div_aclk_160", GATE_IP_LCD, 6,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PPMULCD0, "ppmulcd0", "div_aclk_160", GATE_IP_LCD, 5,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_SMMUFIMD0, "smmufimd0", "div_aclk_160", GATE_IP_LCD, 4, 0, 0),
+	GATE(CLK_DSIM0, "dsim0", "div_aclk_160", GATE_IP_LCD, 3, 0, 0),
+	GATE(CLK_SMIES, "smies", "div_aclk_160", GATE_IP_LCD, 2, 0, 0),
+	GATE(CLK_FIMD0, "fimd0", "div_aclk_160", GATE_IP_LCD, 0, 0, 0),
+
+	/* GATE_IP_ISP */
+	GATE(CLK_CAM1, "cam1", "mout_aclk_266_sub", GATE_IP_ISP, 5, 0, 0),
+	GATE(CLK_UART_ISP_TOP, "uart_isp_top", "mout_aclk_266_sub",
+		GATE_IP_ISP, 3, 0, 0),
+	GATE(CLK_SPI1_ISP_TOP, "spi1_isp_top", "mout_aclk_266_sub",
+		GATE_IP_ISP, 2, 0, 0),
+	GATE(CLK_SPI0_ISP_TOP, "spi0_isp_top", "mout_aclk_266_sub",
+		GATE_IP_ISP, 1, 0, 0),
+
+	/* GATE_IP_FSYS */
+	GATE(CLK_TSADC, "tsadc", "div_aclk_200", GATE_IP_FSYS, 20, 0, 0),
+	GATE(CLK_PPMUFILE, "ppmufile", "div_aclk_200", GATE_IP_FSYS, 17,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_USBOTG, "usbotg", "div_aclk_200", GATE_IP_FSYS, 13, 0, 0),
+	GATE(CLK_USBHOST, "usbhost", "div_aclk_200", GATE_IP_FSYS, 12, 0, 0),
+	GATE(CLK_SROMC, "sromc", "div_aclk_200", GATE_IP_FSYS, 11, 0, 0),
+	GATE(CLK_SDMMC1, "sdmmc1", "div_aclk_200", GATE_IP_FSYS, 6, 0, 0),
+	GATE(CLK_SDMMC0, "sdmmc0", "div_aclk_200", GATE_IP_FSYS, 5, 0, 0),
+	GATE(CLK_PDMA1, "pdma1", "div_aclk_200", GATE_IP_FSYS, 1, 0, 0),
+	GATE(CLK_PDMA0, "pdma0", "div_aclk_200", GATE_IP_FSYS, 0, 0, 0),
+
+	/* GATE_IP_PERIL */
+	GATE(CLK_PWM, "pwm", "div_aclk_100", GATE_IP_PERIL, 24, 0, 0),
+	GATE(CLK_PCM, "pcm", "div_aclk_100", GATE_IP_PERIL, 23, 0, 0),
+	GATE(CLK_I2S, "i2s", "div_aclk_100", GATE_IP_PERIL, 21, 0, 0),
+	GATE(CLK_SPI1, "spi1", "div_aclk_100", GATE_IP_PERIL, 17, 0, 0),
+	GATE(CLK_SPI0, "spi0", "div_aclk_100", GATE_IP_PERIL, 16, 0, 0),
+	GATE(CLK_I2C7, "i2c7", "div_aclk_100", GATE_IP_PERIL, 13, 0, 0),
+	GATE(CLK_I2C6, "i2c6", "div_aclk_100", GATE_IP_PERIL, 12, 0, 0),
+	GATE(CLK_I2C5, "i2c5", "div_aclk_100", GATE_IP_PERIL, 11, 0, 0),
+	GATE(CLK_I2C4, "i2c4", "div_aclk_100", GATE_IP_PERIL, 10, 0, 0),
+	GATE(CLK_I2C3, "i2c3", "div_aclk_100", GATE_IP_PERIL, 9, 0, 0),
+	GATE(CLK_I2C2, "i2c2", "div_aclk_100", GATE_IP_PERIL, 8, 0, 0),
+	GATE(CLK_I2C1, "i2c1", "div_aclk_100", GATE_IP_PERIL, 7, 0, 0),
+	GATE(CLK_I2C0, "i2c0", "div_aclk_100", GATE_IP_PERIL, 6, 0, 0),
+	GATE(CLK_UART1, "uart1", "div_aclk_100", GATE_IP_PERIL, 1, 0, 0),
+	GATE(CLK_UART0, "uart0", "div_aclk_100", GATE_IP_PERIL, 0, 0, 0),
+};
+
+/* APLL & MPLL & BPLL & UPLL */
+static struct samsung_pll_rate_table exynos3250_pll_rates[] = {
+	PLL_35XX_RATE(1200000000, 400, 4, 1),
+	PLL_35XX_RATE(1100000000, 275, 3, 1),
+	PLL_35XX_RATE(1066000000, 533, 6, 1),
+	PLL_35XX_RATE(1000000000, 250, 3, 1),
+	PLL_35XX_RATE( 960000000, 320, 4, 1),
+	PLL_35XX_RATE( 900000000, 300, 4, 1),
+	PLL_35XX_RATE( 850000000, 425, 6, 1),
+	PLL_35XX_RATE( 800000000, 200, 3, 1),
+	PLL_35XX_RATE( 700000000, 175, 3, 1),
+	PLL_35XX_RATE( 667000000, 667, 12, 1),
+	PLL_35XX_RATE( 600000000, 400, 4, 2),
+	PLL_35XX_RATE( 533000000, 533, 6, 2),
+	PLL_35XX_RATE( 520000000, 260, 3, 2),
+	PLL_35XX_RATE( 500000000, 250, 3, 2),
+	PLL_35XX_RATE( 400000000, 200, 3, 2),
+	PLL_35XX_RATE( 200000000, 200, 3, 3),
+	PLL_35XX_RATE( 100000000, 200, 3, 4),
+	{ /* sentinel */ }
+};
+
+/* VPLL */
+static struct samsung_pll_rate_table exynos3250_vpll_rates[] = {
+	PLL_36XX_RATE(600000000, 100, 2, 1,     0),
+	PLL_36XX_RATE(533000000, 266, 3, 2, 32768),
+	PLL_36XX_RATE(519230987, 173, 2, 2,  5046),
+	PLL_36XX_RATE(500000000, 250, 3, 2,     0),
+	PLL_36XX_RATE(445500000, 148, 2, 2, 32768),
+	PLL_36XX_RATE(445055007, 148, 2, 2, 23047),
+	PLL_36XX_RATE(400000000, 200, 3, 2,     0),
+	PLL_36XX_RATE(371250000, 123, 2, 2, 49152),
+	PLL_36XX_RATE(370878997, 185, 3, 2, 28803),
+	PLL_36XX_RATE(340000000, 170, 3, 2,     0),
+	PLL_36XX_RATE(335000015, 111, 2, 2, 43691),
+	PLL_36XX_RATE(333000000, 111, 2, 2,     0),
+	PLL_36XX_RATE(330000000, 110, 2, 2,     0),
+	PLL_36XX_RATE(320000015, 106, 2, 2, 43691),
+	PLL_36XX_RATE(300000000, 100, 2, 2,     0),
+	PLL_36XX_RATE(275000000, 275, 3, 3,     0),
+	PLL_36XX_RATE(222750000, 148, 2, 3, 32768),
+	PLL_36XX_RATE(222528007, 148, 2, 3, 23069),
+	PLL_36XX_RATE(160000000, 160, 3, 3,     0),
+	PLL_36XX_RATE(148500000,  99, 2, 3,     0),
+	PLL_36XX_RATE(148352005,  98, 2, 3, 59070),
+	PLL_36XX_RATE(108000000, 144, 2, 4,     0),
+	PLL_36XX_RATE( 74250000,  99, 2, 4,     0),
+	PLL_36XX_RATE( 74176002,  98, 3, 4, 59070),
+	PLL_36XX_RATE( 54054000, 216, 3, 5, 14156),
+	PLL_36XX_RATE( 54000000, 144, 2, 5,     0),
+	{ /* sentinel */ }
+};
+
+static struct samsung_pll_clock exynos3250_plls[nr_plls] __initdata = {
+	[apll] = PLL(pll_35xx, CLK_FOUT_APLL, "fout_apll", "fin_pll",
+			APLL_LOCK, APLL_CON0, NULL),
+	[mpll] = PLL(pll_35xx, CLK_FOUT_MPLL, "fout_mpll", "fin_pll",
+			MPLL_LOCK, MPLL_CON0, NULL),
+	[vpll] = PLL(pll_36xx, CLK_FOUT_VPLL, "fout_vpll", "fin_pll",
+			VPLL_LOCK, VPLL_CON0, NULL),
+	[upll] = PLL(pll_35xx, CLK_FOUT_UPLL, "fout_upll", "fin_pll",
+			UPLL_LOCK, UPLL_CON0, NULL),
+};
+
+static void __init exynos3250_cmu_init(struct device_node *np)
+{
+	struct samsung_clk_provider *ctx;
+
+	reg_base = of_iomap(np, 0);
+	if (!reg_base)
+		panic("%s: failed to map registers\n", __func__);
+
+	ctx = samsung_clk_init(np, reg_base, CLK_NR_CLKS);
+	if (!ctx)
+		panic("%s: unable to allocate context.\n", __func__);
+
+	samsung_clk_register_fixed_factor(ctx, fixed_factor_clks,
+					  ARRAY_SIZE(fixed_factor_clks));
+
+	exynos3250_plls[apll].rate_table = exynos3250_pll_rates;
+	exynos3250_plls[mpll].rate_table = exynos3250_pll_rates;
+	exynos3250_plls[vpll].rate_table = exynos3250_vpll_rates;
+	exynos3250_plls[upll].rate_table = exynos3250_pll_rates;
+
+	samsung_clk_register_pll(ctx, exynos3250_plls,
+					ARRAY_SIZE(exynos3250_plls), reg_base);
+
+	samsung_clk_register_mux(ctx, mux_clks, ARRAY_SIZE(mux_clks));
+	samsung_clk_register_div(ctx, div_clks, ARRAY_SIZE(div_clks));
+	samsung_clk_register_gate(ctx, gate_clks, ARRAY_SIZE(gate_clks));
+
+	exynos3250_clk_sleep_init();
+}
+CLK_OF_DECLARE(exynos3250_cmu, "samsung,exynos3250-cmu", exynos3250_cmu_init);
diff --git a/drivers/clk/samsung/clk-exynos4.c b/drivers/clk/samsung/clk-exynos4.c
index b4f96721017..4f150c9dd38 100644
--- a/drivers/clk/samsung/clk-exynos4.c
+++ b/drivers/clk/samsung/clk-exynos4.c
@@ -324,7 +324,7 @@ static struct syscore_ops exynos4_clk_syscore_ops = {
 	.resume = exynos4_clk_resume,
 };
 
-static void exynos4_clk_sleep_init(void)
+static void __init exynos4_clk_sleep_init(void)
 {
 	exynos4_save_common = samsung_clk_alloc_reg_dump(exynos4_clk_regs,
 					ARRAY_SIZE(exynos4_clk_regs));
@@ -359,7 +359,7 @@ err_warn:
 		__func__);
 }
 #else
-static void exynos4_clk_sleep_init(void) {}
+static void __init exynos4_clk_sleep_init(void) {}
 #endif
 
 /* list of all parent clock list */
@@ -428,7 +428,7 @@ static struct samsung_fixed_rate_clock exynos4_fixed_rate_ext_clks[] __initdata
 /* fixed rate clocks generated inside the soc */
 static struct samsung_fixed_rate_clock exynos4_fixed_rate_clks[] __initdata = {
 	FRATE(0, "sclk_hdmi24m", NULL, CLK_IS_ROOT, 24000000),
-	FRATE(0, "sclk_hdmiphy", NULL, CLK_IS_ROOT, 27000000),
+	FRATE(CLK_SCLK_HDMIPHY, "sclk_hdmiphy", NULL, CLK_IS_ROOT, 27000000),
 	FRATE(0, "sclk_usbphy0", NULL, CLK_IS_ROOT, 48000000),
 };
 
@@ -903,7 +903,7 @@ static struct samsung_gate_clock exynos4x12_gate_clks[] __initdata = {
 	GATE(CLK_AUDSS, "audss", "sclk_epll", E4X12_GATE_IP_MAUDIO, 0, 0, 0),
 	GATE(CLK_MDNIE0, "mdnie0", "aclk160", GATE_IP_LCD0, 2, 0, 0),
 	GATE(CLK_ROTATOR, "rotator", "aclk200", E4X12_GATE_IP_IMAGE, 1, 0, 0),
-	GATE(CLK_MDMA2, "mdma2", "aclk200", E4X12_GATE_IP_IMAGE, 2, 0, 0),
+	GATE(CLK_MDMA, "mdma", "aclk200", E4X12_GATE_IP_IMAGE, 2, 0, 0),
 	GATE(CLK_SMMU_MDMA, "smmu_mdma", "aclk200", E4X12_GATE_IP_IMAGE, 5, 0,
 		0),
 	GATE(CLK_MIPI_HSI, "mipi_hsi", "aclk133", GATE_IP_FSYS, 10, 0, 0),
@@ -1043,7 +1043,7 @@ static unsigned long exynos4_get_xom(void)
 	return xom;
 }
 
-static void __init exynos4_clk_register_finpll(void)
+static void __init exynos4_clk_register_finpll(struct samsung_clk_provider *ctx)
 {
 	struct samsung_fixed_rate_clock fclk;
 	struct clk *clk;
@@ -1066,7 +1066,7 @@ static void __init exynos4_clk_register_finpll(void)
 	fclk.parent_name = NULL;
 	fclk.flags = CLK_IS_ROOT;
 	fclk.fixed_rate = finpll_f;
-	samsung_clk_register_fixed_rate(&fclk, 1);
+	samsung_clk_register_fixed_rate(ctx, &fclk, 1);
 
 }
 
@@ -1176,22 +1176,25 @@ static struct samsung_pll_clock exynos4x12_plls[nr_plls] __initdata = {
 static void __init exynos4_clk_init(struct device_node *np,
 				    enum exynos4_soc soc)
 {
+	struct samsung_clk_provider *ctx;
 	exynos4_soc = soc;
 
 	reg_base = of_iomap(np, 0);
 	if (!reg_base)
 		panic("%s: failed to map registers\n", __func__);
 
-	samsung_clk_init(np, reg_base, CLK_NR_CLKS);
+	ctx = samsung_clk_init(np, reg_base, CLK_NR_CLKS);
+	if (!ctx)
+		panic("%s: unable to allocate context.\n", __func__);
 
-	samsung_clk_of_register_fixed_ext(exynos4_fixed_rate_ext_clks,
+	samsung_clk_of_register_fixed_ext(ctx, exynos4_fixed_rate_ext_clks,
 			ARRAY_SIZE(exynos4_fixed_rate_ext_clks),
 			ext_clk_match);
 
-	exynos4_clk_register_finpll();
+	exynos4_clk_register_finpll(ctx);
 
 	if (exynos4_soc == EXYNOS4210) {
-		samsung_clk_register_mux(exynos4210_mux_early,
+		samsung_clk_register_mux(ctx, exynos4210_mux_early,
 					ARRAY_SIZE(exynos4210_mux_early));
 
 		if (_get_rate("fin_pll") == 24000000) {
@@ -1205,7 +1208,7 @@ static void __init exynos4_clk_init(struct device_node *np,
 			exynos4210_plls[vpll].rate_table =
 							exynos4210_vpll_rates;
 
-		samsung_clk_register_pll(exynos4210_plls,
+		samsung_clk_register_pll(ctx, exynos4210_plls,
 					ARRAY_SIZE(exynos4210_plls), reg_base);
 	} else {
 		if (_get_rate("fin_pll") == 24000000) {
@@ -1217,42 +1220,42 @@ static void __init exynos4_clk_init(struct device_node *np,
 							exynos4x12_vpll_rates;
 		}
 
-		samsung_clk_register_pll(exynos4x12_plls,
+		samsung_clk_register_pll(ctx, exynos4x12_plls,
 					ARRAY_SIZE(exynos4x12_plls), reg_base);
 	}
 
-	samsung_clk_register_fixed_rate(exynos4_fixed_rate_clks,
+	samsung_clk_register_fixed_rate(ctx, exynos4_fixed_rate_clks,
 			ARRAY_SIZE(exynos4_fixed_rate_clks));
-	samsung_clk_register_mux(exynos4_mux_clks,
+	samsung_clk_register_mux(ctx, exynos4_mux_clks,
 			ARRAY_SIZE(exynos4_mux_clks));
-	samsung_clk_register_div(exynos4_div_clks,
+	samsung_clk_register_div(ctx, exynos4_div_clks,
 			ARRAY_SIZE(exynos4_div_clks));
-	samsung_clk_register_gate(exynos4_gate_clks,
+	samsung_clk_register_gate(ctx, exynos4_gate_clks,
 			ARRAY_SIZE(exynos4_gate_clks));
 
 	if (exynos4_soc == EXYNOS4210) {
-		samsung_clk_register_fixed_rate(exynos4210_fixed_rate_clks,
+		samsung_clk_register_fixed_rate(ctx, exynos4210_fixed_rate_clks,
 			ARRAY_SIZE(exynos4210_fixed_rate_clks));
-		samsung_clk_register_mux(exynos4210_mux_clks,
+		samsung_clk_register_mux(ctx, exynos4210_mux_clks,
 			ARRAY_SIZE(exynos4210_mux_clks));
-		samsung_clk_register_div(exynos4210_div_clks,
+		samsung_clk_register_div(ctx, exynos4210_div_clks,
 			ARRAY_SIZE(exynos4210_div_clks));
-		samsung_clk_register_gate(exynos4210_gate_clks,
+		samsung_clk_register_gate(ctx, exynos4210_gate_clks,
 			ARRAY_SIZE(exynos4210_gate_clks));
-		samsung_clk_register_alias(exynos4210_aliases,
+		samsung_clk_register_alias(ctx, exynos4210_aliases,
 			ARRAY_SIZE(exynos4210_aliases));
 	} else {
-		samsung_clk_register_mux(exynos4x12_mux_clks,
+		samsung_clk_register_mux(ctx, exynos4x12_mux_clks,
 			ARRAY_SIZE(exynos4x12_mux_clks));
-		samsung_clk_register_div(exynos4x12_div_clks,
+		samsung_clk_register_div(ctx, exynos4x12_div_clks,
 			ARRAY_SIZE(exynos4x12_div_clks));
-		samsung_clk_register_gate(exynos4x12_gate_clks,
+		samsung_clk_register_gate(ctx, exynos4x12_gate_clks,
 			ARRAY_SIZE(exynos4x12_gate_clks));
-		samsung_clk_register_alias(exynos4x12_aliases,
+		samsung_clk_register_alias(ctx, exynos4x12_aliases,
 			ARRAY_SIZE(exynos4x12_aliases));
 	}
 
-	samsung_clk_register_alias(exynos4_aliases,
+	samsung_clk_register_alias(ctx, exynos4_aliases,
 			ARRAY_SIZE(exynos4_aliases));
 
 	exynos4_clk_sleep_init();
diff --git a/drivers/clk/samsung/clk-exynos5250.c b/drivers/clk/samsung/clk-exynos5250.c
index e7ee4420da8..1fad4c5e3f5 100644
--- a/drivers/clk/samsung/clk-exynos5250.c
+++ b/drivers/clk/samsung/clk-exynos5250.c
@@ -24,10 +24,14 @@
 #define APLL_CON0		0x100
 #define SRC_CPU			0x200
 #define DIV_CPU0		0x500
+#define PWR_CTRL1		0x1020
+#define PWR_CTRL2		0x1024
 #define MPLL_LOCK		0x4000
 #define MPLL_CON0		0x4100
 #define SRC_CORE1		0x4204
 #define GATE_IP_ACP		0x8800
+#define GATE_IP_ISP0		0xc800
+#define GATE_IP_ISP1		0xc804
 #define CPLL_LOCK		0x10020
 #define EPLL_LOCK		0x10030
 #define VPLL_LOCK		0x10040
@@ -37,6 +41,7 @@
 #define VPLL_CON0		0x10140
 #define GPLL_CON0		0x10150
 #define SRC_TOP0		0x10210
+#define SRC_TOP1		0x10214
 #define SRC_TOP2		0x10218
 #define SRC_TOP3		0x1021c
 #define SRC_GSCL		0x10220
@@ -71,6 +76,7 @@
 #define GATE_IP_GSCL		0x10920
 #define GATE_IP_DISP1		0x10928
 #define GATE_IP_MFC		0x1092c
+#define GATE_IP_G3D		0x10930
 #define GATE_IP_GEN		0x10934
 #define GATE_IP_FSYS		0x10944
 #define GATE_IP_PERIC		0x10950
@@ -80,6 +86,23 @@
 #define SRC_CDREX		0x20200
 #define PLL_DIV2_SEL		0x20a24
 
+/*Below definitions are used for PWR_CTRL settings*/
+#define PWR_CTRL1_CORE2_DOWN_RATIO		(7 << 28)
+#define PWR_CTRL1_CORE1_DOWN_RATIO		(7 << 16)
+#define PWR_CTRL1_DIV2_DOWN_EN			(1 << 9)
+#define PWR_CTRL1_DIV1_DOWN_EN			(1 << 8)
+#define PWR_CTRL1_USE_CORE1_WFE			(1 << 5)
+#define PWR_CTRL1_USE_CORE0_WFE			(1 << 4)
+#define PWR_CTRL1_USE_CORE1_WFI			(1 << 1)
+#define PWR_CTRL1_USE_CORE0_WFI			(1 << 0)
+
+#define PWR_CTRL2_DIV2_UP_EN			(1 << 25)
+#define PWR_CTRL2_DIV1_UP_EN			(1 << 24)
+#define PWR_CTRL2_DUR_STANDBY2_VAL		(1 << 16)
+#define PWR_CTRL2_DUR_STANDBY1_VAL		(1 << 8)
+#define PWR_CTRL2_CORE2_UP_RATIO		(1 << 4)
+#define PWR_CTRL2_CORE1_UP_RATIO		(1 << 0)
+
 /* list of PLLs to be registered */
 enum exynos5250_plls {
 	apll, mpll, cpll, epll, vpll, gpll, bpll,
@@ -98,8 +121,11 @@ static struct samsung_clk_reg_dump *exynos5250_save;
 static unsigned long exynos5250_clk_regs[] __initdata = {
 	SRC_CPU,
 	DIV_CPU0,
+	PWR_CTRL1,
+	PWR_CTRL2,
 	SRC_CORE1,
 	SRC_TOP0,
+	SRC_TOP1,
 	SRC_TOP2,
 	SRC_TOP3,
 	SRC_GSCL,
@@ -133,6 +159,7 @@ static unsigned long exynos5250_clk_regs[] __initdata = {
 	DIV_PERIC5,
 	GATE_IP_GSCL,
 	GATE_IP_MFC,
+	GATE_IP_G3D,
 	GATE_IP_GEN,
 	GATE_IP_FSYS,
 	GATE_IP_PERIC,
@@ -141,6 +168,8 @@ static unsigned long exynos5250_clk_regs[] __initdata = {
 	PLL_DIV2_SEL,
 	GATE_IP_DISP1,
 	GATE_IP_ACP,
+	GATE_IP_ISP0,
+	GATE_IP_ISP1,
 };
 
 static int exynos5250_clk_suspend(void)
@@ -189,13 +218,16 @@ PNAME(mout_vpllsrc_p)	= { "fin_pll", "sclk_hdmi27m" };
 PNAME(mout_vpll_p)	= { "mout_vpllsrc", "fout_vpll" };
 PNAME(mout_cpll_p)	= { "fin_pll", "fout_cpll" };
 PNAME(mout_epll_p)	= { "fin_pll", "fout_epll" };
+PNAME(mout_gpll_p)	= { "fin_pll", "fout_gpll" };
 PNAME(mout_mpll_user_p)	= { "fin_pll", "mout_mpll" };
 PNAME(mout_bpll_user_p)	= { "fin_pll", "mout_bpll" };
 PNAME(mout_aclk166_p)	= { "mout_cpll", "mout_mpll_user" };
 PNAME(mout_aclk200_p)	= { "mout_mpll_user", "mout_bpll_user" };
+PNAME(mout_aclk400_p)	= { "mout_aclk400_g3d_mid", "mout_gpll" };
 PNAME(mout_aclk200_sub_p) = { "fin_pll", "div_aclk200" };
 PNAME(mout_aclk266_sub_p) = { "fin_pll", "div_aclk266" };
 PNAME(mout_aclk333_sub_p) = { "fin_pll", "div_aclk333" };
+PNAME(mout_aclk400_isp_sub_p) = { "fin_pll", "div_aclk400_isp" };
 PNAME(mout_hdmi_p)	= { "div_hdmi_pixel", "sclk_hdmiphy" };
 PNAME(mout_usb3_p)	= { "mout_mpll_user", "mout_cpll" };
 PNAME(mout_group1_p)	= { "fin_pll", "fin_pll", "sclk_hdmi27m",
@@ -273,15 +305,23 @@ static struct samsung_mux_clock exynos5250_mux_clks[] __initdata = {
 	MUX(0, "mout_aclk166", mout_aclk166_p, SRC_TOP0, 8, 1),
 	MUX(0, "mout_aclk200", mout_aclk200_p, SRC_TOP0, 12, 1),
 	MUX(0, "mout_aclk333", mout_aclk166_p, SRC_TOP0, 16, 1),
+	MUX(0, "mout_aclk400_g3d_mid", mout_aclk200_p, SRC_TOP0, 20, 1),
+
+	MUX(0, "mout_aclk400_isp", mout_aclk200_p, SRC_TOP1, 24, 1),
+	MUX(0, "mout_aclk400_g3d", mout_aclk400_p, SRC_TOP1, 28, 1),
 
 	MUX(0, "mout_cpll", mout_cpll_p, SRC_TOP2, 8, 1),
 	MUX(0, "mout_epll", mout_epll_p, SRC_TOP2, 12, 1),
 	MUX(0, "mout_vpll", mout_vpll_p, SRC_TOP2, 16, 1),
 	MUX(0, "mout_mpll_user", mout_mpll_user_p, SRC_TOP2, 20, 1),
 	MUX(0, "mout_bpll_user", mout_bpll_user_p, SRC_TOP2, 24, 1),
+	MUX(CLK_MOUT_GPLL, "mout_gpll", mout_gpll_p, SRC_TOP2, 28, 1),
 
 	MUX(0, "mout_aclk200_disp1_sub", mout_aclk200_sub_p, SRC_TOP3, 4, 1),
 	MUX(0, "mout_aclk266_gscl_sub", mout_aclk266_sub_p, SRC_TOP3, 8, 1),
+	MUX(0, "mout_aclk_266_isp_sub", mout_aclk266_sub_p, SRC_TOP3, 16, 1),
+	MUX(0, "mout_aclk_400_isp_sub", mout_aclk400_isp_sub_p,
+			SRC_TOP3, 20, 1),
 	MUX(0, "mout_aclk333_sub", mout_aclk333_sub_p, SRC_TOP3, 24, 1),
 
 	MUX(0, "mout_cam_bayer", mout_group1_p, SRC_GSCL, 12, 4),
@@ -351,7 +391,10 @@ static struct samsung_div_clock exynos5250_div_clks[] __initdata = {
 	DIV(0, "div_aclk200", "mout_aclk200", DIV_TOP0, 12, 3),
 	DIV(0, "div_aclk266", "mout_mpll_user", DIV_TOP0, 16, 3),
 	DIV(0, "div_aclk333", "mout_aclk333", DIV_TOP0, 20, 3),
+	DIV(0, "div_aclk400_g3d", "mout_aclk400_g3d", DIV_TOP0,
+							24, 3),
 
+	DIV(0, "div_aclk400_isp", "mout_aclk400_isp", DIV_TOP1, 20, 3),
 	DIV(0, "div_aclk66_pre", "mout_mpll_user", DIV_TOP1, 24, 3),
 
 	DIV(0, "div_cam_bayer", "mout_cam_bayer", DIV_GSCL, 12, 4),
@@ -428,6 +471,7 @@ static struct samsung_gate_clock exynos5250_gate_clks[] __initdata = {
 	 * CMU_ACP
 	 */
 	GATE(CLK_MDMA0, "mdma0", "div_aclk266", GATE_IP_ACP, 1, 0, 0),
+	GATE(CLK_SSS, "sss", "div_aclk266", GATE_IP_ACP, 2, 0, 0),
 	GATE(CLK_G2D, "g2d", "div_aclk200", GATE_IP_ACP, 3, 0, 0),
 	GATE(CLK_SMMU_MDMA0, "smmu_mdma0", "div_aclk266", GATE_IP_ACP, 5, 0, 0),
 
@@ -533,7 +577,8 @@ static struct samsung_gate_clock exynos5250_gate_clks[] __initdata = {
 		0),
 	GATE(CLK_SMMU_MFCL, "smmu_mfcl", "mout_aclk333_sub", GATE_IP_MFC, 2, 0,
 		0),
-
+	GATE(CLK_G3D, "g3d", "div_aclk400_g3d", GATE_IP_G3D, 0,
+					CLK_SET_RATE_PARENT, 0),
 	GATE(CLK_ROTATOR, "rotator", "div_aclk266", GATE_IP_GEN, 1, 0, 0),
 	GATE(CLK_JPEG, "jpeg", "div_aclk166", GATE_IP_GEN, 2, 0, 0),
 	GATE(CLK_MDMA1, "mdma1", "div_aclk266", GATE_IP_GEN, 4, 0, 0),
@@ -615,6 +660,31 @@ static struct samsung_gate_clock exynos5250_gate_clks[] __initdata = {
 	GATE(CLK_WDT, "wdt", "div_aclk66", GATE_IP_PERIS, 19, 0, 0),
 	GATE(CLK_RTC, "rtc", "div_aclk66", GATE_IP_PERIS, 20, 0, 0),
 	GATE(CLK_TMU, "tmu", "div_aclk66", GATE_IP_PERIS, 21, 0, 0),
+	GATE(CLK_SMMU_TV, "smmu_tv", "mout_aclk200_disp1_sub",
+			GATE_IP_DISP1, 2, 0, 0),
+	GATE(CLK_SMMU_FIMD1, "smmu_fimd1", "mout_aclk200_disp1_sub",
+			GATE_IP_DISP1, 8, 0, 0),
+	GATE(CLK_SMMU_2D, "smmu_2d", "div_aclk200", GATE_IP_ACP, 7, 0, 0),
+	GATE(CLK_SMMU_FIMC_ISP, "smmu_fimc_isp", "mout_aclk_266_isp_sub",
+			GATE_IP_ISP0, 8, 0, 0),
+	GATE(CLK_SMMU_FIMC_DRC, "smmu_fimc_drc", "mout_aclk_266_isp_sub",
+			GATE_IP_ISP0, 9, 0, 0),
+	GATE(CLK_SMMU_FIMC_FD, "smmu_fimc_fd", "mout_aclk_266_isp_sub",
+			GATE_IP_ISP0, 10, 0, 0),
+	GATE(CLK_SMMU_FIMC_SCC, "smmu_fimc_scc", "mout_aclk_266_isp_sub",
+			GATE_IP_ISP0, 11, 0, 0),
+	GATE(CLK_SMMU_FIMC_SCP, "smmu_fimc_scp", "mout_aclk_266_isp_sub",
+			GATE_IP_ISP0, 12, 0, 0),
+	GATE(CLK_SMMU_FIMC_MCU, "smmu_fimc_mcu", "mout_aclk_400_isp_sub",
+			GATE_IP_ISP0, 13, 0, 0),
+	GATE(CLK_SMMU_FIMC_ODC, "smmu_fimc_odc", "mout_aclk_266_isp_sub",
+			GATE_IP_ISP1, 4, 0, 0),
+	GATE(CLK_SMMU_FIMC_DIS0, "smmu_fimc_dis0", "mout_aclk_266_isp_sub",
+			GATE_IP_ISP1, 5, 0, 0),
+	GATE(CLK_SMMU_FIMC_DIS1, "smmu_fimc_dis1", "mout_aclk_266_isp_sub",
+			GATE_IP_ISP1, 6, 0, 0),
+	GATE(CLK_SMMU_FIMC_3DNR, "smmu_fimc_3dnr", "mout_aclk_266_isp_sub",
+			GATE_IP_ISP1, 7, 0, 0),
 };
 
 static struct samsung_pll_rate_table vpll_24mhz_tbl[] __initdata = {
@@ -686,6 +756,9 @@ static struct of_device_id ext_clk_match[] __initdata = {
 /* register exynox5250 clocks */
 static void __init exynos5250_clk_init(struct device_node *np)
 {
+	struct samsung_clk_provider *ctx;
+	unsigned int tmp;
+
 	if (np) {
 		reg_base = of_iomap(np, 0);
 		if (!reg_base)
@@ -694,11 +767,13 @@ static void __init exynos5250_clk_init(struct device_node *np)
 		panic("%s: unable to determine soc\n", __func__);
 	}
 
-	samsung_clk_init(np, reg_base, CLK_NR_CLKS);
-	samsung_clk_of_register_fixed_ext(exynos5250_fixed_rate_ext_clks,
+	ctx = samsung_clk_init(np, reg_base, CLK_NR_CLKS);
+	if (!ctx)
+		panic("%s: unable to allocate context.\n", __func__);
+	samsung_clk_of_register_fixed_ext(ctx, exynos5250_fixed_rate_ext_clks,
 			ARRAY_SIZE(exynos5250_fixed_rate_ext_clks),
 			ext_clk_match);
-	samsung_clk_register_mux(exynos5250_pll_pmux_clks,
+	samsung_clk_register_mux(ctx, exynos5250_pll_pmux_clks,
 				ARRAY_SIZE(exynos5250_pll_pmux_clks));
 
 	if (_get_rate("fin_pll") == 24 * MHZ) {
@@ -709,19 +784,40 @@ static void __init exynos5250_clk_init(struct device_node *np)
 	if (_get_rate("mout_vpllsrc") == 24 * MHZ)
 		exynos5250_plls[vpll].rate_table =  vpll_24mhz_tbl;
 
-	samsung_clk_register_pll(exynos5250_plls, ARRAY_SIZE(exynos5250_plls),
-					reg_base);
-	samsung_clk_register_fixed_rate(exynos5250_fixed_rate_clks,
+	samsung_clk_register_pll(ctx, exynos5250_plls,
+			ARRAY_SIZE(exynos5250_plls),
+			reg_base);
+	samsung_clk_register_fixed_rate(ctx, exynos5250_fixed_rate_clks,
 			ARRAY_SIZE(exynos5250_fixed_rate_clks));
-	samsung_clk_register_fixed_factor(exynos5250_fixed_factor_clks,
+	samsung_clk_register_fixed_factor(ctx, exynos5250_fixed_factor_clks,
 			ARRAY_SIZE(exynos5250_fixed_factor_clks));
-	samsung_clk_register_mux(exynos5250_mux_clks,
+	samsung_clk_register_mux(ctx, exynos5250_mux_clks,
 			ARRAY_SIZE(exynos5250_mux_clks));
-	samsung_clk_register_div(exynos5250_div_clks,
+	samsung_clk_register_div(ctx, exynos5250_div_clks,
 			ARRAY_SIZE(exynos5250_div_clks));
-	samsung_clk_register_gate(exynos5250_gate_clks,
+	samsung_clk_register_gate(ctx, exynos5250_gate_clks,
 			ARRAY_SIZE(exynos5250_gate_clks));
 
+	/*
+	 * Enable arm clock down (in idle) and set arm divider
+	 * ratios in WFI/WFE state.
+	 */
+	tmp = (PWR_CTRL1_CORE2_DOWN_RATIO | PWR_CTRL1_CORE1_DOWN_RATIO |
+		PWR_CTRL1_DIV2_DOWN_EN | PWR_CTRL1_DIV1_DOWN_EN |
+		PWR_CTRL1_USE_CORE1_WFE | PWR_CTRL1_USE_CORE0_WFE |
+		PWR_CTRL1_USE_CORE1_WFI | PWR_CTRL1_USE_CORE0_WFI);
+	__raw_writel(tmp, reg_base + PWR_CTRL1);
+
+	/*
+	 * Enable arm clock up (on exiting idle). Set arm divider
+	 * ratios when not in idle along with the standby duration
+	 * ratios.
+	 */
+	tmp = (PWR_CTRL2_DIV2_UP_EN | PWR_CTRL2_DIV1_UP_EN |
+		PWR_CTRL2_DUR_STANDBY2_VAL | PWR_CTRL2_DUR_STANDBY1_VAL |
+		PWR_CTRL2_CORE2_UP_RATIO | PWR_CTRL2_CORE1_UP_RATIO);
+	__raw_writel(tmp, reg_base + PWR_CTRL2);
+
 	exynos5250_clk_sleep_init();
 
 	pr_info("Exynos5250: clock setup completed, armclk=%ld\n",
diff --git a/drivers/clk/samsung/clk-exynos5260.c b/drivers/clk/samsung/clk-exynos5260.c
new file mode 100644
index 00000000000..64596ba58df
--- /dev/null
+++ b/drivers/clk/samsung/clk-exynos5260.c
@@ -0,0 +1,1980 @@
+/*
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ * Author: Rahul Sharma <rahul.sharma@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.
+ *
+ * Common Clock Framework support for Exynos5260 SoC.
+ */
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/syscore_ops.h>
+
+#include "clk-exynos5260.h"
+#include "clk.h"
+#include "clk-pll.h"
+
+#include <dt-bindings/clock/exynos5260-clk.h>
+
+static LIST_HEAD(clock_reg_cache_list);
+
+struct exynos5260_clock_reg_cache {
+	struct list_head node;
+	void __iomem *reg_base;
+	struct samsung_clk_reg_dump *rdump;
+	unsigned int rd_num;
+};
+
+struct exynos5260_cmu_info {
+	/* list of pll clocks and respective count */
+	struct samsung_pll_clock *pll_clks;
+	unsigned int nr_pll_clks;
+	/* list of mux clocks and respective count */
+	struct samsung_mux_clock *mux_clks;
+	unsigned int nr_mux_clks;
+	/* list of div clocks and respective count */
+	struct samsung_div_clock *div_clks;
+	unsigned int nr_div_clks;
+	/* list of gate clocks and respective count */
+	struct samsung_gate_clock *gate_clks;
+	unsigned int nr_gate_clks;
+	/* list of fixed clocks and respective count */
+	struct samsung_fixed_rate_clock *fixed_clks;
+	unsigned int nr_fixed_clks;
+	/* total number of clocks with IDs assigned*/
+	unsigned int nr_clk_ids;
+
+	/* list and number of clocks registers */
+	unsigned long *clk_regs;
+	unsigned int nr_clk_regs;
+};
+
+/*
+ * Applicable for all 2550 Type PLLS for Exynos5260, listed below
+ * DISP_PLL, EGL_PLL, KFC_PLL, MEM_PLL, BUS_PLL, MEDIA_PLL, G3D_PLL.
+ */
+static struct samsung_pll_rate_table pll2550_24mhz_tbl[] __initdata = {
+	PLL_35XX_RATE(1700000000, 425, 6, 0),
+	PLL_35XX_RATE(1600000000, 200, 3, 0),
+	PLL_35XX_RATE(1500000000, 250, 4, 0),
+	PLL_35XX_RATE(1400000000, 175, 3, 0),
+	PLL_35XX_RATE(1300000000, 325, 6, 0),
+	PLL_35XX_RATE(1200000000, 400, 4, 1),
+	PLL_35XX_RATE(1100000000, 275, 3, 1),
+	PLL_35XX_RATE(1000000000, 250, 3, 1),
+	PLL_35XX_RATE(933000000, 311, 4, 1),
+	PLL_35XX_RATE(900000000, 300, 4, 1),
+	PLL_35XX_RATE(800000000, 200, 3, 1),
+	PLL_35XX_RATE(733000000, 733, 12, 1),
+	PLL_35XX_RATE(700000000, 175, 3, 1),
+	PLL_35XX_RATE(667000000, 667, 12, 1),
+	PLL_35XX_RATE(633000000, 211, 4, 1),
+	PLL_35XX_RATE(620000000, 310, 3, 2),
+	PLL_35XX_RATE(600000000, 400, 4, 2),
+	PLL_35XX_RATE(543000000, 362, 4, 2),
+	PLL_35XX_RATE(533000000, 533, 6, 2),
+	PLL_35XX_RATE(500000000, 250, 3, 2),
+	PLL_35XX_RATE(450000000, 300, 4, 2),
+	PLL_35XX_RATE(400000000, 200, 3, 2),
+	PLL_35XX_RATE(350000000, 175, 3, 2),
+	PLL_35XX_RATE(300000000, 400, 4, 3),
+	PLL_35XX_RATE(266000000, 266, 3, 3),
+	PLL_35XX_RATE(200000000, 200, 3, 3),
+	PLL_35XX_RATE(160000000, 160, 3, 3),
+};
+
+/*
+ * Applicable for 2650 Type PLL for AUD_PLL.
+ */
+static struct samsung_pll_rate_table pll2650_24mhz_tbl[] __initdata = {
+	PLL_36XX_RATE(1600000000, 200, 3, 0, 0),
+	PLL_36XX_RATE(1200000000, 100, 2, 0, 0),
+	PLL_36XX_RATE(1000000000, 250, 3, 1, 0),
+	PLL_36XX_RATE(800000000, 200, 3, 1, 0),
+	PLL_36XX_RATE(600000000, 100, 2, 1, 0),
+	PLL_36XX_RATE(532000000, 266, 3, 2, 0),
+	PLL_36XX_RATE(480000000, 160, 2, 2, 0),
+	PLL_36XX_RATE(432000000, 144, 2, 2, 0),
+	PLL_36XX_RATE(400000000, 200, 3, 2, 0),
+	PLL_36XX_RATE(394073130, 459, 7, 2, 49282),
+	PLL_36XX_RATE(333000000, 111, 2, 2, 0),
+	PLL_36XX_RATE(300000000, 100, 2, 2, 0),
+	PLL_36XX_RATE(266000000, 266, 3, 3, 0),
+	PLL_36XX_RATE(200000000, 200, 3, 3, 0),
+	PLL_36XX_RATE(166000000, 166, 3, 3, 0),
+	PLL_36XX_RATE(133000000, 266, 3, 4, 0),
+	PLL_36XX_RATE(100000000, 200, 3, 4, 0),
+	PLL_36XX_RATE(66000000, 176, 2, 5, 0),
+};
+
+#ifdef CONFIG_PM_SLEEP
+
+static int exynos5260_clk_suspend(void)
+{
+	struct exynos5260_clock_reg_cache *cache;
+
+	list_for_each_entry(cache, &clock_reg_cache_list, node)
+		samsung_clk_save(cache->reg_base, cache->rdump,
+				cache->rd_num);
+
+	return 0;
+}
+
+static void exynos5260_clk_resume(void)
+{
+	struct exynos5260_clock_reg_cache *cache;
+
+	list_for_each_entry(cache, &clock_reg_cache_list, node)
+		samsung_clk_restore(cache->reg_base, cache->rdump,
+				cache->rd_num);
+}
+
+static struct syscore_ops exynos5260_clk_syscore_ops = {
+	.suspend = exynos5260_clk_suspend,
+	.resume = exynos5260_clk_resume,
+};
+
+static void exynos5260_clk_sleep_init(void __iomem *reg_base,
+			unsigned long *rdump,
+			unsigned long nr_rdump)
+{
+	struct exynos5260_clock_reg_cache *reg_cache;
+
+	reg_cache = kzalloc(sizeof(struct exynos5260_clock_reg_cache),
+			GFP_KERNEL);
+	if (!reg_cache)
+		panic("could not allocate register cache.\n");
+
+	reg_cache->rdump = samsung_clk_alloc_reg_dump(rdump, nr_rdump);
+
+	if (!reg_cache->rdump)
+		panic("could not allocate register dump storage.\n");
+
+	if (list_empty(&clock_reg_cache_list))
+		register_syscore_ops(&exynos5260_clk_syscore_ops);
+
+	reg_cache->rd_num = nr_rdump;
+	reg_cache->reg_base = reg_base;
+	list_add_tail(&reg_cache->node, &clock_reg_cache_list);
+}
+
+#else
+static void exynos5260_clk_sleep_init(void __iomem *reg_base,
+			unsigned long *rdump,
+			unsigned long nr_rdump){}
+#endif
+
+/*
+ * Common function which registers plls, muxes, dividers and gates
+ * for each CMU. It also add CMU register list to register cache.
+ */
+
+void __init exynos5260_cmu_register_one(struct device_node *np,
+			struct exynos5260_cmu_info *cmu)
+{
+	void __iomem *reg_base;
+	struct samsung_clk_provider *ctx;
+
+	reg_base = of_iomap(np, 0);
+	if (!reg_base)
+		panic("%s: failed to map registers\n", __func__);
+
+	ctx = samsung_clk_init(np, reg_base, cmu->nr_clk_ids);
+	if (!ctx)
+		panic("%s: unable to alllocate ctx\n", __func__);
+
+	if (cmu->pll_clks)
+		samsung_clk_register_pll(ctx, cmu->pll_clks, cmu->nr_pll_clks,
+			reg_base);
+	if (cmu->mux_clks)
+		samsung_clk_register_mux(ctx,  cmu->mux_clks,
+			cmu->nr_mux_clks);
+	if (cmu->div_clks)
+		samsung_clk_register_div(ctx, cmu->div_clks, cmu->nr_div_clks);
+	if (cmu->gate_clks)
+		samsung_clk_register_gate(ctx, cmu->gate_clks,
+			cmu->nr_gate_clks);
+	if (cmu->fixed_clks)
+		samsung_clk_register_fixed_rate(ctx, cmu->fixed_clks,
+			cmu->nr_fixed_clks);
+	if (cmu->clk_regs)
+		exynos5260_clk_sleep_init(reg_base, cmu->clk_regs,
+			cmu->nr_clk_regs);
+}
+
+
+/* CMU_AUD */
+
+static unsigned long aud_clk_regs[] __initdata = {
+	MUX_SEL_AUD,
+	DIV_AUD0,
+	DIV_AUD1,
+	EN_ACLK_AUD,
+	EN_PCLK_AUD,
+	EN_SCLK_AUD,
+	EN_IP_AUD,
+};
+
+PNAME(mout_aud_pll_user_p) = {"fin_pll", "fout_aud_pll"};
+PNAME(mout_sclk_aud_i2s_p) = {"mout_aud_pll_user", "ioclk_i2s_cdclk"};
+PNAME(mout_sclk_aud_pcm_p) = {"mout_aud_pll_user", "ioclk_pcm_extclk"};
+
+struct samsung_mux_clock aud_mux_clks[] __initdata = {
+	MUX(AUD_MOUT_AUD_PLL_USER, "mout_aud_pll_user", mout_aud_pll_user_p,
+			MUX_SEL_AUD, 0, 1),
+	MUX(AUD_MOUT_SCLK_AUD_I2S, "mout_sclk_aud_i2s", mout_sclk_aud_i2s_p,
+			MUX_SEL_AUD, 4, 1),
+	MUX(AUD_MOUT_SCLK_AUD_PCM, "mout_sclk_aud_pcm", mout_sclk_aud_pcm_p,
+			MUX_SEL_AUD, 8, 1),
+};
+
+struct samsung_div_clock aud_div_clks[] __initdata = {
+	DIV(AUD_DOUT_ACLK_AUD_131, "dout_aclk_aud_131", "mout_aud_pll_user",
+			DIV_AUD0, 0, 4),
+
+	DIV(AUD_DOUT_SCLK_AUD_I2S, "dout_sclk_aud_i2s", "mout_sclk_aud_i2s",
+			DIV_AUD1, 0, 4),
+	DIV(AUD_DOUT_SCLK_AUD_PCM, "dout_sclk_aud_pcm", "mout_sclk_aud_pcm",
+			DIV_AUD1, 4, 8),
+	DIV(AUD_DOUT_SCLK_AUD_UART, "dout_sclk_aud_uart", "mout_aud_pll_user",
+			DIV_AUD1, 12, 4),
+};
+
+struct samsung_gate_clock aud_gate_clks[] __initdata = {
+	GATE(AUD_SCLK_I2S, "sclk_aud_i2s", "dout_sclk_aud_i2s",
+			EN_SCLK_AUD, 0, CLK_SET_RATE_PARENT, 0),
+	GATE(AUD_SCLK_PCM, "sclk_aud_pcm", "dout_sclk_aud_pcm",
+			EN_SCLK_AUD, 1, CLK_SET_RATE_PARENT, 0),
+	GATE(AUD_SCLK_AUD_UART, "sclk_aud_uart", "dout_sclk_aud_uart",
+			EN_SCLK_AUD, 2, CLK_SET_RATE_PARENT, 0),
+
+	GATE(AUD_CLK_SRAMC, "clk_sramc", "dout_aclk_aud_131", EN_IP_AUD,
+			0, 0, 0),
+	GATE(AUD_CLK_DMAC, "clk_dmac", "dout_aclk_aud_131",
+			EN_IP_AUD, 1, 0, 0),
+	GATE(AUD_CLK_I2S, "clk_i2s", "dout_aclk_aud_131", EN_IP_AUD, 2, 0, 0),
+	GATE(AUD_CLK_PCM, "clk_pcm", "dout_aclk_aud_131", EN_IP_AUD, 3, 0, 0),
+	GATE(AUD_CLK_AUD_UART, "clk_aud_uart", "dout_aclk_aud_131",
+			EN_IP_AUD, 4, 0, 0),
+};
+
+static void __init exynos5260_clk_aud_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.mux_clks = aud_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(aud_mux_clks);
+	cmu.div_clks = aud_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(aud_div_clks);
+	cmu.gate_clks = aud_gate_clks;
+	cmu.nr_gate_clks = ARRAY_SIZE(aud_gate_clks);
+	cmu.nr_clk_ids = AUD_NR_CLK;
+	cmu.clk_regs = aud_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(aud_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_aud, "samsung,exynos5260-clock-aud",
+		exynos5260_clk_aud_init);
+
+
+/* CMU_DISP */
+
+static unsigned long disp_clk_regs[] __initdata = {
+	MUX_SEL_DISP0,
+	MUX_SEL_DISP1,
+	MUX_SEL_DISP2,
+	MUX_SEL_DISP3,
+	MUX_SEL_DISP4,
+	DIV_DISP,
+	EN_ACLK_DISP,
+	EN_PCLK_DISP,
+	EN_SCLK_DISP0,
+	EN_SCLK_DISP1,
+	EN_IP_DISP,
+	EN_IP_DISP_BUS,
+};
+
+PNAME(mout_phyclk_dptx_phy_ch3_txd_clk_user_p) = {"fin_pll",
+			"phyclk_dptx_phy_ch3_txd_clk"};
+PNAME(mout_phyclk_dptx_phy_ch2_txd_clk_user_p) = {"fin_pll",
+			"phyclk_dptx_phy_ch2_txd_clk"};
+PNAME(mout_phyclk_dptx_phy_ch1_txd_clk_user_p) = {"fin_pll",
+			"phyclk_dptx_phy_ch1_txd_clk"};
+PNAME(mout_phyclk_dptx_phy_ch0_txd_clk_user_p) = {"fin_pll",
+			"phyclk_dptx_phy_ch0_txd_clk"};
+PNAME(mout_aclk_disp_222_user_p) = {"fin_pll", "dout_aclk_disp_222"};
+PNAME(mout_sclk_disp_pixel_user_p) = {"fin_pll", "dout_sclk_disp_pixel"};
+PNAME(mout_aclk_disp_333_user_p) = {"fin_pll", "dout_aclk_disp_333"};
+PNAME(mout_phyclk_hdmi_phy_tmds_clko_user_p) = {"fin_pll",
+			"phyclk_hdmi_phy_tmds_clko"};
+PNAME(mout_phyclk_hdmi_phy_ref_clko_user_p) = {"fin_pll",
+			"phyclk_hdmi_phy_ref_clko"};
+PNAME(mout_phyclk_hdmi_phy_pixel_clko_user_p) = {"fin_pll",
+			"phyclk_hdmi_phy_pixel_clko"};
+PNAME(mout_phyclk_hdmi_link_o_tmds_clkhi_user_p) = {"fin_pll",
+			"phyclk_hdmi_link_o_tmds_clkhi"};
+PNAME(mout_phyclk_mipi_dphy_4l_m_txbyte_clkhs_p) = {"fin_pll",
+			"phyclk_mipi_dphy_4l_m_txbyte_clkhs"};
+PNAME(mout_phyclk_dptx_phy_o_ref_clk_24m_user_p) = {"fin_pll",
+			"phyclk_dptx_phy_o_ref_clk_24m"};
+PNAME(mout_phyclk_dptx_phy_clk_div2_user_p) = {"fin_pll",
+			"phyclk_dptx_phy_clk_div2"};
+PNAME(mout_sclk_hdmi_pixel_p) = {"mout_sclk_disp_pixel_user",
+			"mout_aclk_disp_222_user"};
+PNAME(mout_phyclk_mipi_dphy_4lmrxclk_esc0_user_p) = {"fin_pll",
+			"phyclk_mipi_dphy_4l_m_rxclkesc0"};
+PNAME(mout_sclk_hdmi_spdif_p) = {"fin_pll", "ioclk_spdif_extclk",
+			"dout_aclk_peri_aud", "phyclk_hdmi_phy_ref_cko"};
+
+struct samsung_mux_clock disp_mux_clks[] __initdata = {
+	MUX(DISP_MOUT_ACLK_DISP_333_USER, "mout_aclk_disp_333_user",
+			mout_aclk_disp_333_user_p,
+			MUX_SEL_DISP0, 0, 1),
+	MUX(DISP_MOUT_SCLK_DISP_PIXEL_USER, "mout_sclk_disp_pixel_user",
+			mout_sclk_disp_pixel_user_p,
+			MUX_SEL_DISP0, 4, 1),
+	MUX(DISP_MOUT_ACLK_DISP_222_USER, "mout_aclk_disp_222_user",
+			mout_aclk_disp_222_user_p,
+			MUX_SEL_DISP0, 8, 1),
+	MUX(DISP_MOUT_PHYCLK_DPTX_PHY_CH0_TXD_CLK_USER,
+			"mout_phyclk_dptx_phy_ch0_txd_clk_user",
+			mout_phyclk_dptx_phy_ch0_txd_clk_user_p,
+			MUX_SEL_DISP0, 16, 1),
+	MUX(DISP_MOUT_PHYCLK_DPTX_PHY_CH1_TXD_CLK_USER,
+			"mout_phyclk_dptx_phy_ch1_txd_clk_user",
+			mout_phyclk_dptx_phy_ch1_txd_clk_user_p,
+			MUX_SEL_DISP0, 20, 1),
+	MUX(DISP_MOUT_PHYCLK_DPTX_PHY_CH2_TXD_CLK_USER,
+			"mout_phyclk_dptx_phy_ch2_txd_clk_user",
+			mout_phyclk_dptx_phy_ch2_txd_clk_user_p,
+			MUX_SEL_DISP0, 24, 1),
+	MUX(DISP_MOUT_PHYCLK_DPTX_PHY_CH3_TXD_CLK_USER,
+			"mout_phyclk_dptx_phy_ch3_txd_clk_user",
+			mout_phyclk_dptx_phy_ch3_txd_clk_user_p,
+			MUX_SEL_DISP0, 28, 1),
+
+	MUX(DISP_MOUT_PHYCLK_DPTX_PHY_CLK_DIV2_USER,
+			"mout_phyclk_dptx_phy_clk_div2_user",
+			mout_phyclk_dptx_phy_clk_div2_user_p,
+			MUX_SEL_DISP1, 0, 1),
+	MUX(DISP_MOUT_PHYCLK_DPTX_PHY_O_REF_CLK_24M_USER,
+			"mout_phyclk_dptx_phy_o_ref_clk_24m_user",
+			mout_phyclk_dptx_phy_o_ref_clk_24m_user_p,
+			MUX_SEL_DISP1, 4, 1),
+	MUX(DISP_MOUT_PHYCLK_MIPI_DPHY_4L_M_TXBYTE_CLKHS,
+			"mout_phyclk_mipi_dphy_4l_m_txbyte_clkhs",
+			mout_phyclk_mipi_dphy_4l_m_txbyte_clkhs_p,
+			MUX_SEL_DISP1, 8, 1),
+	MUX(DISP_MOUT_PHYCLK_HDMI_LINK_O_TMDS_CLKHI_USER,
+			"mout_phyclk_hdmi_link_o_tmds_clkhi_user",
+			mout_phyclk_hdmi_link_o_tmds_clkhi_user_p,
+			MUX_SEL_DISP1, 16, 1),
+	MUX(DISP_MOUT_HDMI_PHY_PIXEL,
+			"mout_phyclk_hdmi_phy_pixel_clko_user",
+			mout_phyclk_hdmi_phy_pixel_clko_user_p,
+			MUX_SEL_DISP1, 20, 1),
+	MUX(DISP_MOUT_PHYCLK_HDMI_PHY_REF_CLKO_USER,
+			"mout_phyclk_hdmi_phy_ref_clko_user",
+			mout_phyclk_hdmi_phy_ref_clko_user_p,
+			MUX_SEL_DISP1, 24, 1),
+	MUX(DISP_MOUT_PHYCLK_HDMI_PHY_TMDS_CLKO_USER,
+			"mout_phyclk_hdmi_phy_tmds_clko_user",
+			mout_phyclk_hdmi_phy_tmds_clko_user_p,
+			MUX_SEL_DISP1, 28, 1),
+
+	MUX(DISP_MOUT_PHYCLK_MIPI_DPHY_4LMRXCLK_ESC0_USER,
+			"mout_phyclk_mipi_dphy_4lmrxclk_esc0_user",
+			mout_phyclk_mipi_dphy_4lmrxclk_esc0_user_p,
+			MUX_SEL_DISP2, 0, 1),
+	MUX(DISP_MOUT_SCLK_HDMI_PIXEL, "mout_sclk_hdmi_pixel",
+			mout_sclk_hdmi_pixel_p,
+			MUX_SEL_DISP2, 4, 1),
+
+	MUX(DISP_MOUT_SCLK_HDMI_SPDIF, "mout_sclk_hdmi_spdif",
+			mout_sclk_hdmi_spdif_p,
+			MUX_SEL_DISP4, 4, 2),
+};
+
+struct samsung_div_clock disp_div_clks[] __initdata = {
+	DIV(DISP_DOUT_PCLK_DISP_111, "dout_pclk_disp_111",
+			"mout_aclk_disp_222_user",
+			DIV_DISP, 8, 4),
+	DIV(DISP_DOUT_SCLK_FIMD1_EXTCLKPLL, "dout_sclk_fimd1_extclkpll",
+			"mout_sclk_disp_pixel_user",
+			DIV_DISP, 12, 4),
+	DIV(DISP_DOUT_SCLK_HDMI_PHY_PIXEL_CLKI,
+			"dout_sclk_hdmi_phy_pixel_clki",
+			"mout_sclk_hdmi_pixel",
+			DIV_DISP, 16, 4),
+};
+
+struct samsung_gate_clock disp_gate_clks[] __initdata = {
+	GATE(DISP_MOUT_HDMI_PHY_PIXEL_USER, "sclk_hdmi_link_i_pixel",
+			"mout_phyclk_hdmi_phy_pixel_clko_user",
+			EN_SCLK_DISP0, 26, CLK_SET_RATE_PARENT, 0),
+	GATE(DISP_SCLK_PIXEL, "sclk_hdmi_phy_pixel_clki",
+			"dout_sclk_hdmi_phy_pixel_clki",
+			EN_SCLK_DISP0, 29, CLK_SET_RATE_PARENT, 0),
+
+	GATE(DISP_CLK_DP, "clk_dptx_link", "mout_aclk_disp_222_user",
+			EN_IP_DISP, 4, 0, 0),
+	GATE(DISP_CLK_DPPHY, "clk_dptx_phy", "mout_aclk_disp_222_user",
+			EN_IP_DISP, 5, 0, 0),
+	GATE(DISP_CLK_DSIM1, "clk_dsim1", "mout_aclk_disp_222_user",
+			EN_IP_DISP, 6, 0, 0),
+	GATE(DISP_CLK_FIMD1, "clk_fimd1", "mout_aclk_disp_222_user",
+			EN_IP_DISP, 7, 0, 0),
+	GATE(DISP_CLK_HDMI, "clk_hdmi", "mout_aclk_disp_222_user",
+			EN_IP_DISP, 8, 0, 0),
+	GATE(DISP_CLK_HDMIPHY, "clk_hdmiphy", "mout_aclk_disp_222_user",
+			EN_IP_DISP, 9, 0, 0),
+	GATE(DISP_CLK_MIPIPHY, "clk_mipi_dphy", "mout_aclk_disp_222_user",
+			EN_IP_DISP, 10, 0, 0),
+	GATE(DISP_CLK_MIXER, "clk_mixer", "mout_aclk_disp_222_user",
+			EN_IP_DISP, 11, 0, 0),
+	GATE(DISP_CLK_PIXEL_DISP, "clk_pixel_disp", "mout_aclk_disp_222_user",
+			EN_IP_DISP, 12, CLK_IGNORE_UNUSED, 0),
+	GATE(DISP_CLK_PIXEL_MIXER, "clk_pixel_mixer", "mout_aclk_disp_222_user",
+			EN_IP_DISP, 13, CLK_IGNORE_UNUSED, 0),
+	GATE(DISP_CLK_SMMU_FIMD1M0, "clk_smmu3_fimd1m0",
+			"mout_aclk_disp_222_user",
+			EN_IP_DISP, 22, 0, 0),
+	GATE(DISP_CLK_SMMU_FIMD1M1, "clk_smmu3_fimd1m1",
+			"mout_aclk_disp_222_user",
+			EN_IP_DISP, 23, 0, 0),
+	GATE(DISP_CLK_SMMU_TV, "clk_smmu3_tv", "mout_aclk_disp_222_user",
+			EN_IP_DISP, 25, 0, 0),
+};
+
+static void __init exynos5260_clk_disp_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.mux_clks = disp_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(disp_mux_clks);
+	cmu.div_clks = disp_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(disp_div_clks);
+	cmu.gate_clks = disp_gate_clks;
+	cmu.nr_gate_clks = ARRAY_SIZE(disp_gate_clks);
+	cmu.nr_clk_ids = DISP_NR_CLK;
+	cmu.clk_regs = disp_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(disp_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_disp, "samsung,exynos5260-clock-disp",
+		exynos5260_clk_disp_init);
+
+
+/* CMU_EGL */
+
+static unsigned long egl_clk_regs[] __initdata = {
+	EGL_PLL_LOCK,
+	EGL_PLL_CON0,
+	EGL_PLL_CON1,
+	EGL_PLL_FREQ_DET,
+	MUX_SEL_EGL,
+	MUX_ENABLE_EGL,
+	DIV_EGL,
+	DIV_EGL_PLL_FDET,
+	EN_ACLK_EGL,
+	EN_PCLK_EGL,
+	EN_SCLK_EGL,
+};
+
+PNAME(mout_egl_b_p) = {"mout_egl_pll", "dout_bus_pll"};
+PNAME(mout_egl_pll_p) = {"fin_pll", "fout_egl_pll"};
+
+struct samsung_mux_clock egl_mux_clks[] __initdata = {
+	MUX(EGL_MOUT_EGL_PLL, "mout_egl_pll", mout_egl_pll_p,
+			MUX_SEL_EGL, 4, 1),
+	MUX(EGL_MOUT_EGL_B, "mout_egl_b", mout_egl_b_p, MUX_SEL_EGL, 16, 1),
+};
+
+struct samsung_div_clock egl_div_clks[] __initdata = {
+	DIV(EGL_DOUT_EGL1, "dout_egl1", "mout_egl_b", DIV_EGL, 0, 3),
+	DIV(EGL_DOUT_EGL2, "dout_egl2", "dout_egl1", DIV_EGL, 4, 3),
+	DIV(EGL_DOUT_ACLK_EGL, "dout_aclk_egl", "dout_egl2", DIV_EGL, 8, 3),
+	DIV(EGL_DOUT_PCLK_EGL, "dout_pclk_egl", "dout_egl_atclk",
+			DIV_EGL, 12, 3),
+	DIV(EGL_DOUT_EGL_ATCLK, "dout_egl_atclk", "dout_egl2", DIV_EGL, 16, 3),
+	DIV(EGL_DOUT_EGL_PCLK_DBG, "dout_egl_pclk_dbg", "dout_egl_atclk",
+			DIV_EGL, 20, 3),
+	DIV(EGL_DOUT_EGL_PLL, "dout_egl_pll", "mout_egl_b", DIV_EGL, 24, 3),
+};
+
+static struct samsung_pll_clock egl_pll_clks[] __initdata = {
+	PLL(pll_2550xx, EGL_FOUT_EGL_PLL, "fout_egl_pll", "fin_pll",
+		EGL_PLL_LOCK, EGL_PLL_CON0,
+		pll2550_24mhz_tbl),
+};
+
+static void __init exynos5260_clk_egl_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.pll_clks = egl_pll_clks;
+	cmu.nr_pll_clks =  ARRAY_SIZE(egl_pll_clks);
+	cmu.mux_clks = egl_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(egl_mux_clks);
+	cmu.div_clks = egl_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(egl_div_clks);
+	cmu.nr_clk_ids = EGL_NR_CLK;
+	cmu.clk_regs = egl_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(egl_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_egl, "samsung,exynos5260-clock-egl",
+		exynos5260_clk_egl_init);
+
+
+/* CMU_FSYS */
+
+static unsigned long fsys_clk_regs[] __initdata = {
+	MUX_SEL_FSYS0,
+	MUX_SEL_FSYS1,
+	EN_ACLK_FSYS,
+	EN_ACLK_FSYS_SECURE_RTIC,
+	EN_ACLK_FSYS_SECURE_SMMU_RTIC,
+	EN_SCLK_FSYS,
+	EN_IP_FSYS,
+	EN_IP_FSYS_SECURE_RTIC,
+	EN_IP_FSYS_SECURE_SMMU_RTIC,
+};
+
+PNAME(mout_phyclk_usbhost20_phyclk_user_p) = {"fin_pll",
+			"phyclk_usbhost20_phy_phyclock"};
+PNAME(mout_phyclk_usbhost20_freeclk_user_p) = {"fin_pll",
+			"phyclk_usbhost20_phy_freeclk"};
+PNAME(mout_phyclk_usbhost20_clk48mohci_user_p) = {"fin_pll",
+			"phyclk_usbhost20_phy_clk48mohci"};
+PNAME(mout_phyclk_usbdrd30_pipe_pclk_user_p) = {"fin_pll",
+			"phyclk_usbdrd30_udrd30_pipe_pclk"};
+PNAME(mout_phyclk_usbdrd30_phyclock_user_p) = {"fin_pll",
+			"phyclk_usbdrd30_udrd30_phyclock"};
+
+struct samsung_mux_clock fsys_mux_clks[] __initdata = {
+	MUX(FSYS_MOUT_PHYCLK_USBDRD30_PHYCLOCK_USER,
+			"mout_phyclk_usbdrd30_phyclock_user",
+			mout_phyclk_usbdrd30_phyclock_user_p,
+			MUX_SEL_FSYS1, 0, 1),
+	MUX(FSYS_MOUT_PHYCLK_USBDRD30_PIPE_PCLK_USER,
+			"mout_phyclk_usbdrd30_pipe_pclk_user",
+			mout_phyclk_usbdrd30_pipe_pclk_user_p,
+			MUX_SEL_FSYS1, 4, 1),
+	MUX(FSYS_MOUT_PHYCLK_USBHOST20_CLK48MOHCI_USER,
+			"mout_phyclk_usbhost20_clk48mohci_user",
+			mout_phyclk_usbhost20_clk48mohci_user_p,
+			MUX_SEL_FSYS1, 8, 1),
+	MUX(FSYS_MOUT_PHYCLK_USBHOST20_FREECLK_USER,
+			"mout_phyclk_usbhost20_freeclk_user",
+			mout_phyclk_usbhost20_freeclk_user_p,
+			MUX_SEL_FSYS1, 12, 1),
+	MUX(FSYS_MOUT_PHYCLK_USBHOST20_PHYCLK_USER,
+			"mout_phyclk_usbhost20_phyclk_user",
+			mout_phyclk_usbhost20_phyclk_user_p,
+			MUX_SEL_FSYS1, 16, 1),
+};
+
+struct samsung_gate_clock fsys_gate_clks[] __initdata = {
+	GATE(FSYS_PHYCLK_USBHOST20, "phyclk_usbhost20_phyclock",
+			"mout_phyclk_usbdrd30_phyclock_user",
+			EN_SCLK_FSYS, 1, 0, 0),
+	GATE(FSYS_PHYCLK_USBDRD30, "phyclk_usbdrd30_udrd30_phyclock_g",
+			"mout_phyclk_usbdrd30_phyclock_user",
+			EN_SCLK_FSYS, 7, 0, 0),
+
+	GATE(FSYS_CLK_MMC0, "clk_mmc0", "dout_aclk_fsys_200",
+			EN_IP_FSYS, 6, 0, 0),
+	GATE(FSYS_CLK_MMC1, "clk_mmc1", "dout_aclk_fsys_200",
+			EN_IP_FSYS, 7, 0, 0),
+	GATE(FSYS_CLK_MMC2, "clk_mmc2", "dout_aclk_fsys_200",
+			EN_IP_FSYS, 8, 0, 0),
+	GATE(FSYS_CLK_PDMA, "clk_pdma", "dout_aclk_fsys_200",
+			EN_IP_FSYS, 9, 0, 0),
+	GATE(FSYS_CLK_SROMC, "clk_sromc", "dout_aclk_fsys_200",
+			EN_IP_FSYS, 13, 0, 0),
+	GATE(FSYS_CLK_USBDRD30, "clk_usbdrd30", "dout_aclk_fsys_200",
+			EN_IP_FSYS, 14, 0, 0),
+	GATE(FSYS_CLK_USBHOST20, "clk_usbhost20", "dout_aclk_fsys_200",
+			EN_IP_FSYS, 15, 0, 0),
+	GATE(FSYS_CLK_USBLINK, "clk_usblink", "dout_aclk_fsys_200",
+			EN_IP_FSYS, 18, 0, 0),
+	GATE(FSYS_CLK_TSI, "clk_tsi", "dout_aclk_fsys_200",
+			EN_IP_FSYS, 20, 0, 0),
+
+	GATE(FSYS_CLK_RTIC, "clk_rtic", "dout_aclk_fsys_200",
+			EN_IP_FSYS_SECURE_RTIC, 11, 0, 0),
+	GATE(FSYS_CLK_SMMU_RTIC, "clk_smmu_rtic", "dout_aclk_fsys_200",
+			EN_IP_FSYS_SECURE_SMMU_RTIC, 12, 0, 0),
+};
+
+static void __init exynos5260_clk_fsys_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.mux_clks = fsys_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(fsys_mux_clks);
+	cmu.gate_clks = fsys_gate_clks;
+	cmu.nr_gate_clks = ARRAY_SIZE(fsys_gate_clks);
+	cmu.nr_clk_ids = FSYS_NR_CLK;
+	cmu.clk_regs = fsys_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(fsys_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_fsys, "samsung,exynos5260-clock-fsys",
+		exynos5260_clk_fsys_init);
+
+
+/* CMU_G2D */
+
+static unsigned long g2d_clk_regs[] __initdata = {
+	MUX_SEL_G2D,
+	MUX_STAT_G2D,
+	DIV_G2D,
+	EN_ACLK_G2D,
+	EN_ACLK_G2D_SECURE_SSS,
+	EN_ACLK_G2D_SECURE_SLIM_SSS,
+	EN_ACLK_G2D_SECURE_SMMU_SLIM_SSS,
+	EN_ACLK_G2D_SECURE_SMMU_SSS,
+	EN_ACLK_G2D_SECURE_SMMU_MDMA,
+	EN_ACLK_G2D_SECURE_SMMU_G2D,
+	EN_PCLK_G2D,
+	EN_PCLK_G2D_SECURE_SMMU_SLIM_SSS,
+	EN_PCLK_G2D_SECURE_SMMU_SSS,
+	EN_PCLK_G2D_SECURE_SMMU_MDMA,
+	EN_PCLK_G2D_SECURE_SMMU_G2D,
+	EN_IP_G2D,
+	EN_IP_G2D_SECURE_SSS,
+	EN_IP_G2D_SECURE_SLIM_SSS,
+	EN_IP_G2D_SECURE_SMMU_SLIM_SSS,
+	EN_IP_G2D_SECURE_SMMU_SSS,
+	EN_IP_G2D_SECURE_SMMU_MDMA,
+	EN_IP_G2D_SECURE_SMMU_G2D,
+};
+
+PNAME(mout_aclk_g2d_333_user_p) = {"fin_pll", "dout_aclk_g2d_333"};
+
+struct samsung_mux_clock g2d_mux_clks[] __initdata = {
+	MUX(G2D_MOUT_ACLK_G2D_333_USER, "mout_aclk_g2d_333_user",
+			mout_aclk_g2d_333_user_p,
+			MUX_SEL_G2D, 0, 1),
+};
+
+struct samsung_div_clock g2d_div_clks[] __initdata = {
+	DIV(G2D_DOUT_PCLK_G2D_83, "dout_pclk_g2d_83", "mout_aclk_g2d_333_user",
+			DIV_G2D, 0, 3),
+};
+
+struct samsung_gate_clock g2d_gate_clks[] __initdata = {
+	GATE(G2D_CLK_G2D, "clk_g2d", "mout_aclk_g2d_333_user",
+			EN_IP_G2D, 4, 0, 0),
+	GATE(G2D_CLK_JPEG, "clk_jpeg", "mout_aclk_g2d_333_user",
+			EN_IP_G2D, 5, 0, 0),
+	GATE(G2D_CLK_MDMA, "clk_mdma", "mout_aclk_g2d_333_user",
+			EN_IP_G2D, 6, 0, 0),
+	GATE(G2D_CLK_SMMU3_JPEG, "clk_smmu3_jpeg", "mout_aclk_g2d_333_user",
+			EN_IP_G2D, 16, 0, 0),
+
+	GATE(G2D_CLK_SSS, "clk_sss", "mout_aclk_g2d_333_user",
+			EN_IP_G2D_SECURE_SSS, 17, 0, 0),
+
+	GATE(G2D_CLK_SLIM_SSS, "clk_slim_sss", "mout_aclk_g2d_333_user",
+			EN_IP_G2D_SECURE_SLIM_SSS, 11, 0, 0),
+
+	GATE(G2D_CLK_SMMU_SLIM_SSS, "clk_smmu_slim_sss",
+			"mout_aclk_g2d_333_user",
+			EN_IP_G2D_SECURE_SMMU_SLIM_SSS, 13, 0, 0),
+
+	GATE(G2D_CLK_SMMU_SSS, "clk_smmu_sss", "mout_aclk_g2d_333_user",
+			EN_IP_G2D_SECURE_SMMU_SSS, 14, 0, 0),
+
+	GATE(G2D_CLK_SMMU_MDMA, "clk_smmu_mdma", "mout_aclk_g2d_333_user",
+			EN_IP_G2D_SECURE_SMMU_MDMA, 12, 0, 0),
+
+	GATE(G2D_CLK_SMMU3_G2D, "clk_smmu3_g2d", "mout_aclk_g2d_333_user",
+			EN_IP_G2D_SECURE_SMMU_G2D, 15, 0, 0),
+};
+
+static void __init exynos5260_clk_g2d_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.mux_clks = g2d_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(g2d_mux_clks);
+	cmu.div_clks = g2d_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(g2d_div_clks);
+	cmu.gate_clks = g2d_gate_clks;
+	cmu.nr_gate_clks = ARRAY_SIZE(g2d_gate_clks);
+	cmu.nr_clk_ids = G2D_NR_CLK;
+	cmu.clk_regs = g2d_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(g2d_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_g2d, "samsung,exynos5260-clock-g2d",
+		exynos5260_clk_g2d_init);
+
+
+/* CMU_G3D */
+
+static unsigned long g3d_clk_regs[] __initdata = {
+	G3D_PLL_LOCK,
+	G3D_PLL_CON0,
+	G3D_PLL_CON1,
+	G3D_PLL_FDET,
+	MUX_SEL_G3D,
+	DIV_G3D,
+	DIV_G3D_PLL_FDET,
+	EN_ACLK_G3D,
+	EN_PCLK_G3D,
+	EN_SCLK_G3D,
+	EN_IP_G3D,
+};
+
+PNAME(mout_g3d_pll_p) = {"fin_pll", "fout_g3d_pll"};
+
+struct samsung_mux_clock g3d_mux_clks[] __initdata = {
+	MUX(G3D_MOUT_G3D_PLL, "mout_g3d_pll", mout_g3d_pll_p,
+			MUX_SEL_G3D, 0, 1),
+};
+
+struct samsung_div_clock g3d_div_clks[] __initdata = {
+	DIV(G3D_DOUT_PCLK_G3D, "dout_pclk_g3d", "dout_aclk_g3d", DIV_G3D, 0, 3),
+	DIV(G3D_DOUT_ACLK_G3D, "dout_aclk_g3d", "mout_g3d_pll", DIV_G3D, 4, 3),
+};
+
+struct samsung_gate_clock g3d_gate_clks[] __initdata = {
+	GATE(G3D_CLK_G3D, "clk_g3d", "dout_aclk_g3d", EN_IP_G3D, 2, 0, 0),
+	GATE(G3D_CLK_G3D_HPM, "clk_g3d_hpm", "dout_aclk_g3d",
+			EN_IP_G3D, 3, 0, 0),
+};
+
+static struct samsung_pll_clock g3d_pll_clks[] __initdata = {
+	PLL(pll_2550, G3D_FOUT_G3D_PLL, "fout_g3d_pll", "fin_pll",
+		G3D_PLL_LOCK, G3D_PLL_CON0,
+		pll2550_24mhz_tbl),
+};
+
+static void __init exynos5260_clk_g3d_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.pll_clks = g3d_pll_clks;
+	cmu.nr_pll_clks =  ARRAY_SIZE(g3d_pll_clks);
+	cmu.mux_clks = g3d_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(g3d_mux_clks);
+	cmu.div_clks = g3d_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(g3d_div_clks);
+	cmu.gate_clks = g3d_gate_clks;
+	cmu.nr_gate_clks = ARRAY_SIZE(g3d_gate_clks);
+	cmu.nr_clk_ids = G3D_NR_CLK;
+	cmu.clk_regs = g3d_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(g3d_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_g3d, "samsung,exynos5260-clock-g3d",
+		exynos5260_clk_g3d_init);
+
+
+/* CMU_GSCL */
+
+static unsigned long gscl_clk_regs[] __initdata = {
+	MUX_SEL_GSCL,
+	DIV_GSCL,
+	EN_ACLK_GSCL,
+	EN_ACLK_GSCL_FIMC,
+	EN_ACLK_GSCL_SECURE_SMMU_GSCL0,
+	EN_ACLK_GSCL_SECURE_SMMU_GSCL1,
+	EN_ACLK_GSCL_SECURE_SMMU_MSCL0,
+	EN_ACLK_GSCL_SECURE_SMMU_MSCL1,
+	EN_PCLK_GSCL,
+	EN_PCLK_GSCL_FIMC,
+	EN_PCLK_GSCL_SECURE_SMMU_GSCL0,
+	EN_PCLK_GSCL_SECURE_SMMU_GSCL1,
+	EN_PCLK_GSCL_SECURE_SMMU_MSCL0,
+	EN_PCLK_GSCL_SECURE_SMMU_MSCL1,
+	EN_SCLK_GSCL,
+	EN_SCLK_GSCL_FIMC,
+	EN_IP_GSCL,
+	EN_IP_GSCL_FIMC,
+	EN_IP_GSCL_SECURE_SMMU_GSCL0,
+	EN_IP_GSCL_SECURE_SMMU_GSCL1,
+	EN_IP_GSCL_SECURE_SMMU_MSCL0,
+	EN_IP_GSCL_SECURE_SMMU_MSCL1,
+};
+
+PNAME(mout_aclk_gscl_333_user_p) = {"fin_pll", "dout_aclk_gscl_333"};
+PNAME(mout_aclk_m2m_400_user_p) = {"fin_pll", "dout_aclk_gscl_400"};
+PNAME(mout_aclk_gscl_fimc_user_p) = {"fin_pll", "dout_aclk_gscl_400"};
+PNAME(mout_aclk_csis_p) = {"dout_aclk_csis_200", "mout_aclk_gscl_fimc_user"};
+
+struct samsung_mux_clock gscl_mux_clks[] __initdata = {
+	MUX(GSCL_MOUT_ACLK_GSCL_333_USER, "mout_aclk_gscl_333_user",
+			mout_aclk_gscl_333_user_p,
+			MUX_SEL_GSCL, 0, 1),
+	MUX(GSCL_MOUT_ACLK_M2M_400_USER, "mout_aclk_m2m_400_user",
+			mout_aclk_m2m_400_user_p,
+			MUX_SEL_GSCL, 4, 1),
+	MUX(GSCL_MOUT_ACLK_GSCL_FIMC_USER, "mout_aclk_gscl_fimc_user",
+			mout_aclk_gscl_fimc_user_p,
+			MUX_SEL_GSCL, 8, 1),
+	MUX(GSCL_MOUT_ACLK_CSIS, "mout_aclk_csis", mout_aclk_csis_p,
+			MUX_SEL_GSCL, 24, 1),
+};
+
+struct samsung_div_clock gscl_div_clks[] __initdata = {
+	DIV(GSCL_DOUT_PCLK_M2M_100, "dout_pclk_m2m_100",
+			"mout_aclk_m2m_400_user",
+			DIV_GSCL, 0, 3),
+	DIV(GSCL_DOUT_ACLK_CSIS_200, "dout_aclk_csis_200",
+			"mout_aclk_m2m_400_user",
+			DIV_GSCL, 4, 3),
+};
+
+struct samsung_gate_clock gscl_gate_clks[] __initdata = {
+	GATE(GSCL_SCLK_CSIS0_WRAP, "sclk_csis0_wrap", "dout_aclk_csis_200",
+			EN_SCLK_GSCL_FIMC, 0, CLK_SET_RATE_PARENT, 0),
+	GATE(GSCL_SCLK_CSIS1_WRAP, "sclk_csis1_wrap", "dout_aclk_csis_200",
+			EN_SCLK_GSCL_FIMC, 1, CLK_SET_RATE_PARENT, 0),
+
+	GATE(GSCL_CLK_GSCL0, "clk_gscl0", "mout_aclk_gscl_333_user",
+			EN_IP_GSCL, 2, 0, 0),
+	GATE(GSCL_CLK_GSCL1, "clk_gscl1", "mout_aclk_gscl_333_user",
+			EN_IP_GSCL, 3, 0, 0),
+	GATE(GSCL_CLK_MSCL0, "clk_mscl0", "mout_aclk_gscl_333_user",
+			EN_IP_GSCL, 4, 0, 0),
+	GATE(GSCL_CLK_MSCL1, "clk_mscl1", "mout_aclk_gscl_333_user",
+			EN_IP_GSCL, 5, 0, 0),
+	GATE(GSCL_CLK_PIXEL_GSCL0, "clk_pixel_gscl0",
+			"mout_aclk_gscl_333_user",
+			EN_IP_GSCL, 8, 0, 0),
+	GATE(GSCL_CLK_PIXEL_GSCL1, "clk_pixel_gscl1",
+			"mout_aclk_gscl_333_user",
+			EN_IP_GSCL, 9, 0, 0),
+
+	GATE(GSCL_CLK_SMMU3_LITE_A, "clk_smmu3_lite_a",
+			"mout_aclk_gscl_fimc_user",
+			EN_IP_GSCL_FIMC, 5, 0, 0),
+	GATE(GSCL_CLK_SMMU3_LITE_B, "clk_smmu3_lite_b",
+			"mout_aclk_gscl_fimc_user",
+			EN_IP_GSCL_FIMC, 6, 0, 0),
+	GATE(GSCL_CLK_SMMU3_LITE_D, "clk_smmu3_lite_d",
+			"mout_aclk_gscl_fimc_user",
+			EN_IP_GSCL_FIMC, 7, 0, 0),
+	GATE(GSCL_CLK_CSIS0, "clk_csis0", "mout_aclk_gscl_fimc_user",
+			EN_IP_GSCL_FIMC, 8, 0, 0),
+	GATE(GSCL_CLK_CSIS1, "clk_csis1", "mout_aclk_gscl_fimc_user",
+			EN_IP_GSCL_FIMC, 9, 0, 0),
+	GATE(GSCL_CLK_FIMC_LITE_A, "clk_fimc_lite_a",
+			"mout_aclk_gscl_fimc_user",
+			EN_IP_GSCL_FIMC, 10, 0, 0),
+	GATE(GSCL_CLK_FIMC_LITE_B, "clk_fimc_lite_b",
+			"mout_aclk_gscl_fimc_user",
+			EN_IP_GSCL_FIMC, 11, 0, 0),
+	GATE(GSCL_CLK_FIMC_LITE_D, "clk_fimc_lite_d",
+			"mout_aclk_gscl_fimc_user",
+			EN_IP_GSCL_FIMC, 12, 0, 0),
+
+	GATE(GSCL_CLK_SMMU3_GSCL0, "clk_smmu3_gscl0",
+			"mout_aclk_gscl_333_user",
+			EN_IP_GSCL_SECURE_SMMU_GSCL0, 17, 0, 0),
+	GATE(GSCL_CLK_SMMU3_GSCL1, "clk_smmu3_gscl1", "mout_aclk_gscl_333_user",
+			EN_IP_GSCL_SECURE_SMMU_GSCL1, 18, 0, 0),
+	GATE(GSCL_CLK_SMMU3_MSCL0, "clk_smmu3_mscl0",
+			"mout_aclk_m2m_400_user",
+			EN_IP_GSCL_SECURE_SMMU_MSCL0, 19, 0, 0),
+	GATE(GSCL_CLK_SMMU3_MSCL1, "clk_smmu3_mscl1",
+			"mout_aclk_m2m_400_user",
+			EN_IP_GSCL_SECURE_SMMU_MSCL1, 20, 0, 0),
+};
+
+static void __init exynos5260_clk_gscl_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.mux_clks = gscl_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(gscl_mux_clks);
+	cmu.div_clks = gscl_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(gscl_div_clks);
+	cmu.gate_clks = gscl_gate_clks;
+	cmu.nr_gate_clks = ARRAY_SIZE(gscl_gate_clks);
+	cmu.nr_clk_ids = GSCL_NR_CLK;
+	cmu.clk_regs = gscl_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(gscl_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_gscl, "samsung,exynos5260-clock-gscl",
+		exynos5260_clk_gscl_init);
+
+
+/* CMU_ISP */
+
+static unsigned long isp_clk_regs[] __initdata = {
+	MUX_SEL_ISP0,
+	MUX_SEL_ISP1,
+	DIV_ISP,
+	EN_ACLK_ISP0,
+	EN_ACLK_ISP1,
+	EN_PCLK_ISP0,
+	EN_PCLK_ISP1,
+	EN_SCLK_ISP,
+	EN_IP_ISP0,
+	EN_IP_ISP1,
+};
+
+PNAME(mout_isp_400_user_p) = {"fin_pll", "dout_aclk_isp1_400"};
+PNAME(mout_isp_266_user_p)	 = {"fin_pll", "dout_aclk_isp1_266"};
+
+struct samsung_mux_clock isp_mux_clks[] __initdata = {
+	MUX(ISP_MOUT_ISP_266_USER, "mout_isp_266_user", mout_isp_266_user_p,
+			MUX_SEL_ISP0, 0, 1),
+	MUX(ISP_MOUT_ISP_400_USER, "mout_isp_400_user", mout_isp_400_user_p,
+			MUX_SEL_ISP0, 4, 1),
+};
+
+struct samsung_div_clock isp_div_clks[] __initdata = {
+	DIV(ISP_DOUT_PCLK_ISP_66, "dout_pclk_isp_66", "mout_kfc",
+			DIV_ISP, 0, 3),
+	DIV(ISP_DOUT_PCLK_ISP_133, "dout_pclk_isp_133", "mout_kfc",
+			DIV_ISP, 4, 4),
+	DIV(ISP_DOUT_CA5_ATCLKIN, "dout_ca5_atclkin", "mout_kfc",
+			DIV_ISP, 12, 3),
+	DIV(ISP_DOUT_CA5_PCLKDBG, "dout_ca5_pclkdbg", "mout_kfc",
+			DIV_ISP, 16, 4),
+	DIV(ISP_DOUT_SCLK_MPWM, "dout_sclk_mpwm", "mout_kfc", DIV_ISP, 20, 2),
+};
+
+struct samsung_gate_clock isp_gate_clks[] __initdata = {
+	GATE(ISP_CLK_GIC, "clk_isp_gic", "mout_aclk_isp1_266",
+			EN_IP_ISP0, 15, 0, 0),
+
+	GATE(ISP_CLK_CA5, "clk_isp_ca5", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 1, 0, 0),
+	GATE(ISP_CLK_FIMC_DRC, "clk_isp_fimc_drc", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 2, 0, 0),
+	GATE(ISP_CLK_FIMC_FD, "clk_isp_fimc_fd", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 3, 0, 0),
+	GATE(ISP_CLK_FIMC, "clk_isp_fimc", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 4, 0, 0),
+	GATE(ISP_CLK_FIMC_SCALERC, "clk_isp_fimc_scalerc",
+			"mout_aclk_isp1_266",
+			EN_IP_ISP1, 5, 0, 0),
+	GATE(ISP_CLK_FIMC_SCALERP, "clk_isp_fimc_scalerp",
+			"mout_aclk_isp1_266",
+			EN_IP_ISP1, 6, 0, 0),
+	GATE(ISP_CLK_I2C0, "clk_isp_i2c0", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 7, 0, 0),
+	GATE(ISP_CLK_I2C1, "clk_isp_i2c1", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 8, 0, 0),
+	GATE(ISP_CLK_MCUCTL, "clk_isp_mcuctl", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 9, 0, 0),
+	GATE(ISP_CLK_MPWM, "clk_isp_mpwm", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 10, 0, 0),
+	GATE(ISP_CLK_MTCADC, "clk_isp_mtcadc", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 11, 0, 0),
+	GATE(ISP_CLK_PWM, "clk_isp_pwm", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 14, 0, 0),
+	GATE(ISP_CLK_SMMU_DRC, "clk_smmu_drc", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 21, 0, 0),
+	GATE(ISP_CLK_SMMU_FD, "clk_smmu_fd", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 22, 0, 0),
+	GATE(ISP_CLK_SMMU_ISP, "clk_smmu_isp", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 23, 0, 0),
+	GATE(ISP_CLK_SMMU_ISPCX, "clk_smmu_ispcx", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 24, 0, 0),
+	GATE(ISP_CLK_SMMU_SCALERC, "clk_isp_smmu_scalerc",
+			"mout_aclk_isp1_266",
+			EN_IP_ISP1, 25, 0, 0),
+	GATE(ISP_CLK_SMMU_SCALERP, "clk_isp_smmu_scalerp",
+			"mout_aclk_isp1_266",
+			EN_IP_ISP1, 26, 0, 0),
+	GATE(ISP_CLK_SPI0, "clk_isp_spi0", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 27, 0, 0),
+	GATE(ISP_CLK_SPI1, "clk_isp_spi1", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 28, 0, 0),
+	GATE(ISP_CLK_WDT, "clk_isp_wdt", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 31, 0, 0),
+	GATE(ISP_CLK_UART, "clk_isp_uart", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 30, 0, 0),
+
+	GATE(ISP_SCLK_UART_EXT, "sclk_isp_uart_ext", "fin_pll",
+			EN_SCLK_ISP, 7, CLK_SET_RATE_PARENT, 0),
+	GATE(ISP_SCLK_SPI1_EXT, "sclk_isp_spi1_ext", "fin_pll",
+			EN_SCLK_ISP, 8, CLK_SET_RATE_PARENT, 0),
+	GATE(ISP_SCLK_SPI0_EXT, "sclk_isp_spi0_ext", "fin_pll",
+			EN_SCLK_ISP, 9, CLK_SET_RATE_PARENT, 0),
+};
+
+static void __init exynos5260_clk_isp_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.mux_clks = isp_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(isp_mux_clks);
+	cmu.div_clks = isp_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(isp_div_clks);
+	cmu.gate_clks = isp_gate_clks;
+	cmu.nr_gate_clks = ARRAY_SIZE(isp_gate_clks);
+	cmu.nr_clk_ids = ISP_NR_CLK;
+	cmu.clk_regs = isp_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(isp_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_isp, "samsung,exynos5260-clock-isp",
+		exynos5260_clk_isp_init);
+
+
+/* CMU_KFC */
+
+static unsigned long kfc_clk_regs[] __initdata = {
+	KFC_PLL_LOCK,
+	KFC_PLL_CON0,
+	KFC_PLL_CON1,
+	KFC_PLL_FDET,
+	MUX_SEL_KFC0,
+	MUX_SEL_KFC2,
+	DIV_KFC,
+	DIV_KFC_PLL_FDET,
+	EN_ACLK_KFC,
+	EN_PCLK_KFC,
+	EN_SCLK_KFC,
+	EN_IP_KFC,
+};
+
+PNAME(mout_kfc_pll_p) = {"fin_pll", "fout_kfc_pll"};
+PNAME(mout_kfc_p)	 = {"mout_kfc_pll", "dout_media_pll"};
+
+struct samsung_mux_clock kfc_mux_clks[] __initdata = {
+	MUX(KFC_MOUT_KFC_PLL, "mout_kfc_pll", mout_kfc_pll_p,
+			MUX_SEL_KFC0, 0, 1),
+	MUX(KFC_MOUT_KFC, "mout_kfc", mout_kfc_p, MUX_SEL_KFC2, 0, 1),
+};
+
+struct samsung_div_clock kfc_div_clks[] __initdata = {
+	DIV(KFC_DOUT_KFC1, "dout_kfc1", "mout_kfc", DIV_KFC, 0, 3),
+	DIV(KFC_DOUT_KFC2, "dout_kfc2", "dout_kfc1", DIV_KFC, 4, 3),
+	DIV(KFC_DOUT_KFC_ATCLK, "dout_kfc_atclk", "dout_kfc2", DIV_KFC, 8, 3),
+	DIV(KFC_DOUT_KFC_PCLK_DBG, "dout_kfc_pclk_dbg", "dout_kfc2",
+			DIV_KFC, 12, 3),
+	DIV(KFC_DOUT_ACLK_KFC, "dout_aclk_kfc", "dout_kfc2", DIV_KFC, 16, 3),
+	DIV(KFC_DOUT_PCLK_KFC, "dout_pclk_kfc", "dout_kfc2", DIV_KFC, 20, 3),
+	DIV(KFC_DOUT_KFC_PLL, "dout_kfc_pll", "mout_kfc", DIV_KFC, 24, 3),
+};
+
+static struct samsung_pll_clock kfc_pll_clks[] __initdata = {
+	PLL(pll_2550xx, KFC_FOUT_KFC_PLL, "fout_kfc_pll", "fin_pll",
+		KFC_PLL_LOCK, KFC_PLL_CON0,
+		pll2550_24mhz_tbl),
+};
+
+static void __init exynos5260_clk_kfc_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.pll_clks = kfc_pll_clks;
+	cmu.nr_pll_clks =  ARRAY_SIZE(kfc_pll_clks);
+	cmu.mux_clks = kfc_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(kfc_mux_clks);
+	cmu.div_clks = kfc_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(kfc_div_clks);
+	cmu.nr_clk_ids = KFC_NR_CLK;
+	cmu.clk_regs = kfc_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(kfc_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_kfc, "samsung,exynos5260-clock-kfc",
+		exynos5260_clk_kfc_init);
+
+
+/* CMU_MFC */
+
+static unsigned long mfc_clk_regs[] __initdata = {
+	MUX_SEL_MFC,
+	DIV_MFC,
+	EN_ACLK_MFC,
+	EN_ACLK_SECURE_SMMU2_MFC,
+	EN_PCLK_MFC,
+	EN_PCLK_SECURE_SMMU2_MFC,
+	EN_IP_MFC,
+	EN_IP_MFC_SECURE_SMMU2_MFC,
+};
+
+PNAME(mout_aclk_mfc_333_user_p) = {"fin_pll", "dout_aclk_mfc_333"};
+
+struct samsung_mux_clock mfc_mux_clks[] __initdata = {
+	MUX(MFC_MOUT_ACLK_MFC_333_USER, "mout_aclk_mfc_333_user",
+			mout_aclk_mfc_333_user_p,
+			MUX_SEL_MFC, 0, 1),
+};
+
+struct samsung_div_clock mfc_div_clks[] __initdata = {
+	DIV(MFC_DOUT_PCLK_MFC_83, "dout_pclk_mfc_83", "mout_aclk_mfc_333_user",
+			DIV_MFC, 0, 3),
+};
+
+struct samsung_gate_clock mfc_gate_clks[] __initdata = {
+	GATE(MFC_CLK_MFC, "clk_mfc", "mout_aclk_mfc_333_user",
+			EN_IP_MFC, 1, 0, 0),
+	GATE(MFC_CLK_SMMU2_MFCM0, "clk_smmu2_mfcm0", "mout_aclk_mfc_333_user",
+			EN_IP_MFC_SECURE_SMMU2_MFC, 6, 0, 0),
+	GATE(MFC_CLK_SMMU2_MFCM1, "clk_smmu2_mfcm1", "mout_aclk_mfc_333_user",
+			EN_IP_MFC_SECURE_SMMU2_MFC, 7, 0, 0),
+};
+
+static void __init exynos5260_clk_mfc_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.mux_clks = mfc_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(mfc_mux_clks);
+	cmu.div_clks = mfc_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(mfc_div_clks);
+	cmu.gate_clks = mfc_gate_clks;
+	cmu.nr_gate_clks = ARRAY_SIZE(mfc_gate_clks);
+	cmu.nr_clk_ids = MFC_NR_CLK;
+	cmu.clk_regs = mfc_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(mfc_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_mfc, "samsung,exynos5260-clock-mfc",
+		exynos5260_clk_mfc_init);
+
+
+/* CMU_MIF */
+
+static unsigned long mif_clk_regs[] __initdata = {
+	MEM_PLL_LOCK,
+	BUS_PLL_LOCK,
+	MEDIA_PLL_LOCK,
+	MEM_PLL_CON0,
+	MEM_PLL_CON1,
+	MEM_PLL_FDET,
+	BUS_PLL_CON0,
+	BUS_PLL_CON1,
+	BUS_PLL_FDET,
+	MEDIA_PLL_CON0,
+	MEDIA_PLL_CON1,
+	MEDIA_PLL_FDET,
+	MUX_SEL_MIF,
+	DIV_MIF,
+	DIV_MIF_PLL_FDET,
+	EN_ACLK_MIF,
+	EN_ACLK_MIF_SECURE_DREX1_TZ,
+	EN_ACLK_MIF_SECURE_DREX0_TZ,
+	EN_ACLK_MIF_SECURE_INTMEM,
+	EN_PCLK_MIF,
+	EN_PCLK_MIF_SECURE_MONOCNT,
+	EN_PCLK_MIF_SECURE_RTC_APBIF,
+	EN_PCLK_MIF_SECURE_DREX1_TZ,
+	EN_PCLK_MIF_SECURE_DREX0_TZ,
+	EN_SCLK_MIF,
+	EN_IP_MIF,
+	EN_IP_MIF_SECURE_MONOCNT,
+	EN_IP_MIF_SECURE_RTC_APBIF,
+	EN_IP_MIF_SECURE_DREX1_TZ,
+	EN_IP_MIF_SECURE_DREX0_TZ,
+	EN_IP_MIF_SECURE_INTEMEM,
+};
+
+PNAME(mout_mem_pll_p) = {"fin_pll", "fout_mem_pll"};
+PNAME(mout_bus_pll_p) = {"fin_pll", "fout_bus_pll"};
+PNAME(mout_media_pll_p) = {"fin_pll", "fout_media_pll"};
+PNAME(mout_mif_drex_p) = {"dout_mem_pll", "dout_bus_pll"};
+PNAME(mout_mif_drex2x_p) = {"dout_mem_pll", "dout_bus_pll"};
+PNAME(mout_clkm_phy_p) = {"mout_mif_drex", "dout_media_pll"};
+PNAME(mout_clk2x_phy_p) = {"mout_mif_drex2x", "dout_media_pll"};
+
+struct samsung_mux_clock mif_mux_clks[] __initdata = {
+	MUX(MIF_MOUT_MEM_PLL, "mout_mem_pll", mout_mem_pll_p,
+			MUX_SEL_MIF, 0, 1),
+	MUX(MIF_MOUT_BUS_PLL, "mout_bus_pll", mout_bus_pll_p,
+			MUX_SEL_MIF, 4, 1),
+	MUX(MIF_MOUT_MEDIA_PLL, "mout_media_pll", mout_media_pll_p,
+			MUX_SEL_MIF, 8, 1),
+	MUX(MIF_MOUT_MIF_DREX, "mout_mif_drex", mout_mif_drex_p,
+			MUX_SEL_MIF, 12, 1),
+	MUX(MIF_MOUT_CLKM_PHY, "mout_clkm_phy", mout_clkm_phy_p,
+			MUX_SEL_MIF, 16, 1),
+	MUX(MIF_MOUT_MIF_DREX2X, "mout_mif_drex2x", mout_mif_drex2x_p,
+			MUX_SEL_MIF, 20, 1),
+	MUX(MIF_MOUT_CLK2X_PHY, "mout_clk2x_phy", mout_clk2x_phy_p,
+			MUX_SEL_MIF, 24, 1),
+};
+
+struct samsung_div_clock mif_div_clks[] __initdata = {
+	DIV(MIF_DOUT_MEDIA_PLL, "dout_media_pll", "mout_media_pll",
+			DIV_MIF, 0, 3),
+	DIV(MIF_DOUT_MEM_PLL, "dout_mem_pll", "mout_mem_pll",
+			DIV_MIF, 4, 3),
+	DIV(MIF_DOUT_BUS_PLL, "dout_bus_pll", "mout_bus_pll",
+			DIV_MIF, 8, 3),
+	DIV(MIF_DOUT_CLKM_PHY, "dout_clkm_phy", "mout_clkm_phy",
+			DIV_MIF, 12, 3),
+	DIV(MIF_DOUT_CLK2X_PHY, "dout_clk2x_phy", "mout_clk2x_phy",
+			DIV_MIF, 16, 4),
+	DIV(MIF_DOUT_ACLK_MIF_466, "dout_aclk_mif_466", "dout_clk2x_phy",
+			DIV_MIF, 20, 3),
+	DIV(MIF_DOUT_ACLK_BUS_200, "dout_aclk_bus_200", "dout_bus_pll",
+			DIV_MIF, 24, 3),
+	DIV(MIF_DOUT_ACLK_BUS_100, "dout_aclk_bus_100", "dout_bus_pll",
+			DIV_MIF, 28, 4),
+};
+
+struct samsung_gate_clock mif_gate_clks[] __initdata = {
+	GATE(MIF_CLK_LPDDR3PHY_WRAP0, "clk_lpddr3phy_wrap0", "dout_clk2x_phy",
+			EN_IP_MIF, 12, CLK_IGNORE_UNUSED, 0),
+	GATE(MIF_CLK_LPDDR3PHY_WRAP1, "clk_lpddr3phy_wrap1", "dout_clk2x_phy",
+			EN_IP_MIF, 13, CLK_IGNORE_UNUSED, 0),
+
+	GATE(MIF_CLK_MONOCNT, "clk_monocnt", "dout_aclk_bus_100",
+			EN_IP_MIF_SECURE_MONOCNT, 22,
+			CLK_IGNORE_UNUSED, 0),
+
+	GATE(MIF_CLK_MIF_RTC, "clk_mif_rtc", "dout_aclk_bus_100",
+			EN_IP_MIF_SECURE_RTC_APBIF, 23,
+			CLK_IGNORE_UNUSED, 0),
+
+	GATE(MIF_CLK_DREX1, "clk_drex1", "dout_aclk_mif_466",
+			EN_IP_MIF_SECURE_DREX1_TZ, 9,
+			CLK_IGNORE_UNUSED, 0),
+
+	GATE(MIF_CLK_DREX0, "clk_drex0", "dout_aclk_mif_466",
+			EN_IP_MIF_SECURE_DREX0_TZ, 9,
+			CLK_IGNORE_UNUSED, 0),
+
+	GATE(MIF_CLK_INTMEM, "clk_intmem", "dout_aclk_bus_200",
+			EN_IP_MIF_SECURE_INTEMEM, 11,
+			CLK_IGNORE_UNUSED, 0),
+
+	GATE(MIF_SCLK_LPDDR3PHY_WRAP_U0, "sclk_lpddr3phy_wrap_u0",
+			"dout_clkm_phy", EN_SCLK_MIF, 0,
+			CLK_IGNORE_UNUSED | CLK_SET_RATE_PARENT, 0),
+	GATE(MIF_SCLK_LPDDR3PHY_WRAP_U1, "sclk_lpddr3phy_wrap_u1",
+			"dout_clkm_phy", EN_SCLK_MIF, 1,
+			CLK_IGNORE_UNUSED | CLK_SET_RATE_PARENT, 0),
+};
+
+static struct samsung_pll_clock mif_pll_clks[] __initdata = {
+	PLL(pll_2550xx, MIF_FOUT_MEM_PLL, "fout_mem_pll", "fin_pll",
+		MEM_PLL_LOCK, MEM_PLL_CON0,
+		pll2550_24mhz_tbl),
+	PLL(pll_2550xx, MIF_FOUT_BUS_PLL, "fout_bus_pll", "fin_pll",
+		BUS_PLL_LOCK, BUS_PLL_CON0,
+		pll2550_24mhz_tbl),
+	PLL(pll_2550xx, MIF_FOUT_MEDIA_PLL, "fout_media_pll", "fin_pll",
+		MEDIA_PLL_LOCK, MEDIA_PLL_CON0,
+		pll2550_24mhz_tbl),
+};
+
+static void __init exynos5260_clk_mif_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.pll_clks = mif_pll_clks;
+	cmu.nr_pll_clks =  ARRAY_SIZE(mif_pll_clks);
+	cmu.mux_clks = mif_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(mif_mux_clks);
+	cmu.div_clks = mif_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(mif_div_clks);
+	cmu.gate_clks = mif_gate_clks;
+	cmu.nr_gate_clks = ARRAY_SIZE(mif_gate_clks);
+	cmu.nr_clk_ids = MIF_NR_CLK;
+	cmu.clk_regs = mif_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(mif_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_mif, "samsung,exynos5260-clock-mif",
+		exynos5260_clk_mif_init);
+
+
+/* CMU_PERI */
+
+static unsigned long peri_clk_regs[] __initdata = {
+	MUX_SEL_PERI,
+	MUX_SEL_PERI1,
+	DIV_PERI,
+	EN_PCLK_PERI0,
+	EN_PCLK_PERI1,
+	EN_PCLK_PERI2,
+	EN_PCLK_PERI3,
+	EN_PCLK_PERI_SECURE_CHIPID,
+	EN_PCLK_PERI_SECURE_PROVKEY0,
+	EN_PCLK_PERI_SECURE_PROVKEY1,
+	EN_PCLK_PERI_SECURE_SECKEY,
+	EN_PCLK_PERI_SECURE_ANTIRBKCNT,
+	EN_PCLK_PERI_SECURE_TOP_RTC,
+	EN_PCLK_PERI_SECURE_TZPC,
+	EN_SCLK_PERI,
+	EN_SCLK_PERI_SECURE_TOP_RTC,
+	EN_IP_PERI0,
+	EN_IP_PERI1,
+	EN_IP_PERI2,
+	EN_IP_PERI_SECURE_CHIPID,
+	EN_IP_PERI_SECURE_PROVKEY0,
+	EN_IP_PERI_SECURE_PROVKEY1,
+	EN_IP_PERI_SECURE_SECKEY,
+	EN_IP_PERI_SECURE_ANTIRBKCNT,
+	EN_IP_PERI_SECURE_TOP_RTC,
+	EN_IP_PERI_SECURE_TZPC,
+};
+
+PNAME(mout_sclk_pcm_p) = {"ioclk_pcm_extclk", "fin_pll", "dout_aclk_peri_aud",
+			"phyclk_hdmi_phy_ref_cko"};
+PNAME(mout_sclk_i2scod_p) = {"ioclk_i2s_cdclk", "fin_pll", "dout_aclk_peri_aud",
+			"phyclk_hdmi_phy_ref_cko"};
+PNAME(mout_sclk_spdif_p) = {"ioclk_spdif_extclk", "fin_pll",
+			"dout_aclk_peri_aud", "phyclk_hdmi_phy_ref_cko"};
+
+struct samsung_mux_clock peri_mux_clks[] __initdata = {
+	MUX(PERI_MOUT_SCLK_PCM, "mout_sclk_pcm", mout_sclk_pcm_p,
+			MUX_SEL_PERI1, 4, 2),
+	MUX(PERI_MOUT_SCLK_I2SCOD, "mout_sclk_i2scod", mout_sclk_i2scod_p,
+			MUX_SEL_PERI1, 12, 2),
+	MUX(PERI_MOUT_SCLK_SPDIF, "mout_sclk_spdif", mout_sclk_spdif_p,
+			MUX_SEL_PERI1, 20, 2),
+};
+
+struct samsung_div_clock peri_div_clks[] __initdata = {
+	DIV(PERI_DOUT_PCM, "dout_pcm", "mout_sclk_pcm", DIV_PERI, 0, 8),
+	DIV(PERI_DOUT_I2S, "dout_i2s", "mout_sclk_i2scod", DIV_PERI, 8, 6),
+};
+
+struct samsung_gate_clock peri_gate_clks[] __initdata = {
+	GATE(PERI_SCLK_PCM1, "sclk_pcm1", "dout_pcm", EN_SCLK_PERI, 0,
+			CLK_SET_RATE_PARENT, 0),
+	GATE(PERI_SCLK_I2S, "sclk_i2s", "dout_i2s", EN_SCLK_PERI, 1,
+			CLK_SET_RATE_PARENT, 0),
+	GATE(PERI_SCLK_SPDIF, "sclk_spdif", "dout_sclk_peri_spi0_b",
+			EN_SCLK_PERI, 2, CLK_SET_RATE_PARENT, 0),
+	GATE(PERI_SCLK_SPI0, "sclk_spi0", "dout_sclk_peri_spi0_b",
+			EN_SCLK_PERI, 7, CLK_SET_RATE_PARENT, 0),
+	GATE(PERI_SCLK_SPI1, "sclk_spi1", "dout_sclk_peri_spi1_b",
+			EN_SCLK_PERI, 8, CLK_SET_RATE_PARENT, 0),
+	GATE(PERI_SCLK_SPI2, "sclk_spi2", "dout_sclk_peri_spi2_b",
+			EN_SCLK_PERI, 9, CLK_SET_RATE_PARENT, 0),
+	GATE(PERI_SCLK_UART0, "sclk_uart0", "dout_sclk_peri_uart0",
+			EN_SCLK_PERI, 10, CLK_SET_RATE_PARENT, 0),
+	GATE(PERI_SCLK_UART1, "sclk_uart1", "dout_sclk_peri_uart1",
+			EN_SCLK_PERI, 11, CLK_SET_RATE_PARENT, 0),
+	GATE(PERI_SCLK_UART2, "sclk_uart2", "dout_sclk_peri_uart2",
+			EN_SCLK_PERI, 12, CLK_SET_RATE_PARENT, 0),
+
+	GATE(PERI_CLK_ABB, "clk_abb", "dout_aclk_peri_66",
+		EN_IP_PERI0, 1, 0, 0),
+	GATE(PERI_CLK_EFUSE_WRITER, "clk_efuse_writer", "dout_aclk_peri_66",
+		EN_IP_PERI0, 5, 0, 0),
+	GATE(PERI_CLK_HDMICEC, "clk_hdmicec", "dout_aclk_peri_66",
+		EN_IP_PERI0, 6, 0, 0),
+	GATE(PERI_CLK_I2C10, "clk_i2c10", "dout_aclk_peri_66",
+		EN_IP_PERI0, 7, 0, 0),
+	GATE(PERI_CLK_I2C11, "clk_i2c11", "dout_aclk_peri_66",
+		EN_IP_PERI0, 8, 0, 0),
+	GATE(PERI_CLK_I2C8, "clk_i2c8", "dout_aclk_peri_66",
+		EN_IP_PERI0, 9, 0, 0),
+	GATE(PERI_CLK_I2C9, "clk_i2c9", "dout_aclk_peri_66",
+		EN_IP_PERI0, 10, 0, 0),
+	GATE(PERI_CLK_I2C4, "clk_i2c4", "dout_aclk_peri_66",
+		EN_IP_PERI0, 11, 0, 0),
+	GATE(PERI_CLK_I2C5, "clk_i2c5", "dout_aclk_peri_66",
+		EN_IP_PERI0, 12, 0, 0),
+	GATE(PERI_CLK_I2C6, "clk_i2c6", "dout_aclk_peri_66",
+		EN_IP_PERI0, 13, 0, 0),
+	GATE(PERI_CLK_I2C7, "clk_i2c7", "dout_aclk_peri_66",
+		EN_IP_PERI0, 14, 0, 0),
+	GATE(PERI_CLK_I2CHDMI, "clk_i2chdmi", "dout_aclk_peri_66",
+		EN_IP_PERI0, 15, 0, 0),
+	GATE(PERI_CLK_I2S, "clk_peri_i2s", "dout_aclk_peri_66",
+		EN_IP_PERI0, 16, 0, 0),
+	GATE(PERI_CLK_MCT, "clk_mct", "dout_aclk_peri_66",
+		EN_IP_PERI0, 17, 0, 0),
+	GATE(PERI_CLK_PCM, "clk_peri_pcm", "dout_aclk_peri_66",
+		EN_IP_PERI0, 18, 0, 0),
+	GATE(PERI_CLK_HSIC0, "clk_hsic0", "dout_aclk_peri_66",
+		EN_IP_PERI0, 20, 0, 0),
+	GATE(PERI_CLK_HSIC1, "clk_hsic1", "dout_aclk_peri_66",
+		EN_IP_PERI0, 21, 0, 0),
+	GATE(PERI_CLK_HSIC2, "clk_hsic2", "dout_aclk_peri_66",
+		EN_IP_PERI0, 22, 0, 0),
+	GATE(PERI_CLK_HSIC3, "clk_hsic3", "dout_aclk_peri_66",
+		EN_IP_PERI0, 23, 0, 0),
+	GATE(PERI_CLK_WDT_EGL, "clk_wdt_egl", "dout_aclk_peri_66",
+		EN_IP_PERI0, 24, 0, 0),
+	GATE(PERI_CLK_WDT_KFC, "clk_wdt_kfc", "dout_aclk_peri_66",
+		EN_IP_PERI0, 25, 0, 0),
+
+	GATE(PERI_CLK_UART4, "clk_uart4", "dout_aclk_peri_66",
+		EN_IP_PERI2, 0, 0, 0),
+	GATE(PERI_CLK_PWM, "clk_pwm", "dout_aclk_peri_66",
+		EN_IP_PERI2, 3, 0, 0),
+	GATE(PERI_CLK_SPDIF, "clk_spdif", "dout_aclk_peri_66",
+		EN_IP_PERI2, 6, 0, 0),
+	GATE(PERI_CLK_SPI0, "clk_spi0", "dout_aclk_peri_66",
+		EN_IP_PERI2, 7, 0, 0),
+	GATE(PERI_CLK_SPI1, "clk_spi1", "dout_aclk_peri_66",
+		EN_IP_PERI2, 8, 0, 0),
+	GATE(PERI_CLK_SPI2, "clk_spi2", "dout_aclk_peri_66",
+		EN_IP_PERI2, 9, 0, 0),
+	GATE(PERI_CLK_TMU0, "clk_tmu0", "dout_aclk_peri_66",
+		EN_IP_PERI2, 10, 0, 0),
+	GATE(PERI_CLK_TMU1, "clk_tmu1", "dout_aclk_peri_66",
+		EN_IP_PERI2, 11, 0, 0),
+	GATE(PERI_CLK_TMU2, "clk_tmu2", "dout_aclk_peri_66",
+		EN_IP_PERI2, 12, 0, 0),
+	GATE(PERI_CLK_TMU3, "clk_tmu3", "dout_aclk_peri_66",
+		EN_IP_PERI2, 13, 0, 0),
+	GATE(PERI_CLK_TMU4, "clk_tmu4", "dout_aclk_peri_66",
+		EN_IP_PERI2, 14, 0, 0),
+	GATE(PERI_CLK_ADC, "clk_adc", "dout_aclk_peri_66",
+		EN_IP_PERI2, 18, 0, 0),
+	GATE(PERI_CLK_UART0, "clk_uart0", "dout_aclk_peri_66",
+		EN_IP_PERI2, 19, 0, 0),
+	GATE(PERI_CLK_UART1, "clk_uart1", "dout_aclk_peri_66",
+		EN_IP_PERI2, 20, 0, 0),
+	GATE(PERI_CLK_UART2, "clk_uart2", "dout_aclk_peri_66",
+		EN_IP_PERI2, 21, 0, 0),
+
+	GATE(PERI_CLK_CHIPID, "clk_chipid", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_CHIPID, 2, 0, 0),
+
+	GATE(PERI_CLK_PROVKEY0, "clk_provkey0", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_PROVKEY0, 1, 0, 0),
+
+	GATE(PERI_CLK_PROVKEY1, "clk_provkey1", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_PROVKEY1, 2, 0, 0),
+
+	GATE(PERI_CLK_SECKEY, "clk_seckey", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_SECKEY, 5, 0, 0),
+
+	GATE(PERI_CLK_TOP_RTC, "clk_top_rtc", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TOP_RTC, 5, 0, 0),
+
+	GATE(PERI_CLK_TZPC0, "clk_tzpc0", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TZPC, 10, 0, 0),
+	GATE(PERI_CLK_TZPC1, "clk_tzpc1", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TZPC, 11, 0, 0),
+	GATE(PERI_CLK_TZPC2, "clk_tzpc2", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TZPC, 12, 0, 0),
+	GATE(PERI_CLK_TZPC3, "clk_tzpc3", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TZPC, 13, 0, 0),
+	GATE(PERI_CLK_TZPC4, "clk_tzpc4", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TZPC, 14, 0, 0),
+	GATE(PERI_CLK_TZPC5, "clk_tzpc5", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TZPC, 15, 0, 0),
+	GATE(PERI_CLK_TZPC6, "clk_tzpc6", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TZPC, 16, 0, 0),
+	GATE(PERI_CLK_TZPC7, "clk_tzpc7", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TZPC, 17, 0, 0),
+	GATE(PERI_CLK_TZPC8, "clk_tzpc8", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TZPC, 18, 0, 0),
+	GATE(PERI_CLK_TZPC9, "clk_tzpc9", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TZPC, 19, 0, 0),
+	GATE(PERI_CLK_TZPC10, "clk_tzpc10", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TZPC, 20, 0, 0),
+};
+
+static void __init exynos5260_clk_peri_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.mux_clks = peri_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(peri_mux_clks);
+	cmu.div_clks = peri_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(peri_div_clks);
+	cmu.gate_clks = peri_gate_clks;
+	cmu.nr_gate_clks = ARRAY_SIZE(peri_gate_clks);
+	cmu.nr_clk_ids = PERI_NR_CLK;
+	cmu.clk_regs = peri_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(peri_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_peri, "samsung,exynos5260-clock-peri",
+		exynos5260_clk_peri_init);
+
+
+/* CMU_TOP */
+
+static unsigned long top_clk_regs[] __initdata = {
+	DISP_PLL_LOCK,
+	AUD_PLL_LOCK,
+	DISP_PLL_CON0,
+	DISP_PLL_CON1,
+	DISP_PLL_FDET,
+	AUD_PLL_CON0,
+	AUD_PLL_CON1,
+	AUD_PLL_CON2,
+	AUD_PLL_FDET,
+	MUX_SEL_TOP_PLL0,
+	MUX_SEL_TOP_MFC,
+	MUX_SEL_TOP_G2D,
+	MUX_SEL_TOP_GSCL,
+	MUX_SEL_TOP_ISP10,
+	MUX_SEL_TOP_ISP11,
+	MUX_SEL_TOP_DISP0,
+	MUX_SEL_TOP_DISP1,
+	MUX_SEL_TOP_BUS,
+	MUX_SEL_TOP_PERI0,
+	MUX_SEL_TOP_PERI1,
+	MUX_SEL_TOP_FSYS,
+	DIV_TOP_G2D_MFC,
+	DIV_TOP_GSCL_ISP0,
+	DIV_TOP_ISP10,
+	DIV_TOP_ISP11,
+	DIV_TOP_DISP,
+	DIV_TOP_BUS,
+	DIV_TOP_PERI0,
+	DIV_TOP_PERI1,
+	DIV_TOP_PERI2,
+	DIV_TOP_FSYS0,
+	DIV_TOP_FSYS1,
+	DIV_TOP_HPM,
+	DIV_TOP_PLL_FDET,
+	EN_ACLK_TOP,
+	EN_SCLK_TOP,
+	EN_IP_TOP,
+};
+
+/* fixed rate clocks generated inside the soc */
+struct samsung_fixed_rate_clock fixed_rate_clks[] __initdata = {
+	FRATE(PHYCLK_DPTX_PHY_CH3_TXD_CLK, "phyclk_dptx_phy_ch3_txd_clk", NULL,
+			CLK_IS_ROOT, 270000000),
+	FRATE(PHYCLK_DPTX_PHY_CH2_TXD_CLK, "phyclk_dptx_phy_ch2_txd_clk", NULL,
+			CLK_IS_ROOT, 270000000),
+	FRATE(PHYCLK_DPTX_PHY_CH1_TXD_CLK, "phyclk_dptx_phy_ch1_txd_clk", NULL,
+			CLK_IS_ROOT, 270000000),
+	FRATE(PHYCLK_DPTX_PHY_CH0_TXD_CLK, "phyclk_dptx_phy_ch0_txd_clk", NULL,
+			CLK_IS_ROOT, 270000000),
+	FRATE(phyclk_hdmi_phy_tmds_clko, "phyclk_hdmi_phy_tmds_clko", NULL,
+			CLK_IS_ROOT, 250000000),
+	FRATE(PHYCLK_HDMI_PHY_PIXEL_CLKO, "phyclk_hdmi_phy_pixel_clko", NULL,
+			CLK_IS_ROOT, 1660000000),
+	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,
+			CLK_IS_ROOT, 187500000),
+	FRATE(PHYCLK_DPTX_PHY_O_REF_CLK_24M, "phyclk_dptx_phy_o_ref_clk_24m",
+			NULL, CLK_IS_ROOT, 24000000),
+	FRATE(PHYCLK_DPTX_PHY_CLK_DIV2, "phyclk_dptx_phy_clk_div2", NULL,
+			CLK_IS_ROOT, 135000000),
+	FRATE(PHYCLK_MIPI_DPHY_4L_M_RXCLKESC0,
+			"phyclk_mipi_dphy_4l_m_rxclkesc0", NULL,
+			CLK_IS_ROOT, 20000000),
+	FRATE(PHYCLK_USBHOST20_PHY_PHYCLOCK, "phyclk_usbhost20_phy_phyclock",
+			NULL, CLK_IS_ROOT, 60000000),
+	FRATE(PHYCLK_USBHOST20_PHY_FREECLK, "phyclk_usbhost20_phy_freeclk",
+			NULL, CLK_IS_ROOT, 60000000),
+	FRATE(PHYCLK_USBHOST20_PHY_CLK48MOHCI,
+			"phyclk_usbhost20_phy_clk48mohci",
+			NULL, CLK_IS_ROOT, 48000000),
+	FRATE(PHYCLK_USBDRD30_UDRD30_PIPE_PCLK,
+			"phyclk_usbdrd30_udrd30_pipe_pclk", NULL,
+			CLK_IS_ROOT, 125000000),
+	FRATE(PHYCLK_USBDRD30_UDRD30_PHYCLOCK,
+			"phyclk_usbdrd30_udrd30_phyclock", NULL,
+			CLK_IS_ROOT, 60000000),
+};
+
+PNAME(mout_memtop_pll_user_p) = {"fin_pll", "dout_mem_pll"};
+PNAME(mout_bustop_pll_user_p) = {"fin_pll", "dout_bus_pll"};
+PNAME(mout_mediatop_pll_user_p) = {"fin_pll", "dout_media_pll"};
+PNAME(mout_audtop_pll_user_p) = {"fin_pll", "mout_aud_pll"};
+PNAME(mout_aud_pll_p) = {"fin_pll", "fout_aud_pll"};
+PNAME(mout_disp_pll_p) = {"fin_pll", "fout_disp_pll"};
+PNAME(mout_mfc_bustop_333_p) = {"mout_bustop_pll_user", "mout_disp_pll"};
+PNAME(mout_aclk_mfc_333_p) = {"mout_mediatop_pll_user", "mout_mfc_bustop_333"};
+PNAME(mout_g2d_bustop_333_p) = {"mout_bustop_pll_user", "mout_disp_pll"};
+PNAME(mout_aclk_g2d_333_p) = {"mout_mediatop_pll_user", "mout_g2d_bustop_333"};
+PNAME(mout_gscl_bustop_333_p) = {"mout_bustop_pll_user", "mout_disp_pll"};
+PNAME(mout_aclk_gscl_333_p) = {"mout_mediatop_pll_user",
+			"mout_gscl_bustop_333"};
+PNAME(mout_m2m_mediatop_400_p) = {"mout_mediatop_pll_user", "mout_disp_pll"};
+PNAME(mout_aclk_gscl_400_p) = {"mout_bustop_pll_user",
+			"mout_m2m_mediatop_400"};
+PNAME(mout_gscl_bustop_fimc_p) = {"mout_bustop_pll_user", "mout_disp_pll"};
+PNAME(mout_aclk_gscl_fimc_p) = {"mout_mediatop_pll_user",
+			"mout_gscl_bustop_fimc"};
+PNAME(mout_isp1_media_266_p) = {"mout_mediatop_pll_user",
+			"mout_memtop_pll_user"};
+PNAME(mout_aclk_isp1_266_p) = {"mout_bustop_pll_user", "mout_isp1_media_266"};
+PNAME(mout_isp1_media_400_p) = {"mout_mediatop_pll_user", "mout_disp_pll"};
+PNAME(mout_aclk_isp1_400_p) = {"mout_bustop_pll_user", "mout_isp1_media_400"};
+PNAME(mout_sclk_isp_spi_p) = {"fin_pll", "mout_bustop_pll_user"};
+PNAME(mout_sclk_isp_uart_p) = {"fin_pll", "mout_bustop_pll_user"};
+PNAME(mout_sclk_isp_sensor_p) = {"fin_pll", "mout_bustop_pll_user"};
+PNAME(mout_disp_disp_333_p) = {"mout_disp_pll", "mout_bustop_pll_user"};
+PNAME(mout_aclk_disp_333_p) = {"mout_mediatop_pll_user", "mout_disp_disp_333"};
+PNAME(mout_disp_disp_222_p) = {"mout_disp_pll", "mout_bustop_pll_user"};
+PNAME(mout_aclk_disp_222_p) = {"mout_mediatop_pll_user", "mout_disp_disp_222"};
+PNAME(mout_disp_media_pixel_p) = {"mout_mediatop_pll_user",
+			"mout_bustop_pll_user"};
+PNAME(mout_sclk_disp_pixel_p) = {"mout_disp_pll", "mout_disp_media_pixel"};
+PNAME(mout_bus_bustop_400_p) = {"mout_bustop_pll_user", "mout_memtop_pll_user"};
+PNAME(mout_bus_bustop_100_p) = {"mout_bustop_pll_user", "mout_memtop_pll_user"};
+PNAME(mout_sclk_peri_spi_clk_p) = {"fin_pll", "mout_bustop_pll_user"};
+PNAME(mout_sclk_peri_uart_uclk_p) = {"fin_pll", "mout_bustop_pll_user"};
+PNAME(mout_sclk_fsys_usb_p) = {"fin_pll", "mout_bustop_pll_user"};
+PNAME(mout_sclk_fsys_mmc_sdclkin_a_p) = {"fin_pll", "mout_bustop_pll_user"};
+PNAME(mout_sclk_fsys_mmc0_sdclkin_b_p) = {"mout_sclk_fsys_mmc0_sdclkin_a",
+			"mout_mediatop_pll_user"};
+PNAME(mout_sclk_fsys_mmc1_sdclkin_b_p) = {"mout_sclk_fsys_mmc1_sdclkin_a",
+			"mout_mediatop_pll_user"};
+PNAME(mout_sclk_fsys_mmc2_sdclkin_b_p) = {"mout_sclk_fsys_mmc2_sdclkin_a",
+			"mout_mediatop_pll_user"};
+
+struct samsung_mux_clock top_mux_clks[] __initdata = {
+	MUX(TOP_MOUT_MEDIATOP_PLL_USER, "mout_mediatop_pll_user",
+			mout_mediatop_pll_user_p,
+			MUX_SEL_TOP_PLL0, 0, 1),
+	MUX(TOP_MOUT_MEMTOP_PLL_USER, "mout_memtop_pll_user",
+			mout_memtop_pll_user_p,
+			MUX_SEL_TOP_PLL0, 4, 1),
+	MUX(TOP_MOUT_BUSTOP_PLL_USER, "mout_bustop_pll_user",
+			mout_bustop_pll_user_p,
+			MUX_SEL_TOP_PLL0, 8, 1),
+	MUX(TOP_MOUT_DISP_PLL, "mout_disp_pll", mout_disp_pll_p,
+			MUX_SEL_TOP_PLL0, 12, 1),
+	MUX(TOP_MOUT_AUD_PLL, "mout_aud_pll", mout_aud_pll_p,
+			MUX_SEL_TOP_PLL0, 16, 1),
+	MUX(TOP_MOUT_AUDTOP_PLL_USER, "mout_audtop_pll_user",
+			mout_audtop_pll_user_p,
+			MUX_SEL_TOP_PLL0, 24, 1),
+
+	MUX(TOP_MOUT_DISP_DISP_333, "mout_disp_disp_333", mout_disp_disp_333_p,
+			MUX_SEL_TOP_DISP0, 0, 1),
+	MUX(TOP_MOUT_ACLK_DISP_333, "mout_aclk_disp_333", mout_aclk_disp_333_p,
+			MUX_SEL_TOP_DISP0, 8, 1),
+	MUX(TOP_MOUT_DISP_DISP_222, "mout_disp_disp_222", mout_disp_disp_222_p,
+			MUX_SEL_TOP_DISP0, 12, 1),
+	MUX(TOP_MOUT_ACLK_DISP_222, "mout_aclk_disp_222", mout_aclk_disp_222_p,
+			MUX_SEL_TOP_DISP0, 20, 1),
+
+	MUX(TOP_MOUT_FIMD1, "mout_sclk_disp_pixel", mout_sclk_disp_pixel_p,
+			MUX_SEL_TOP_DISP1, 0, 1),
+	MUX(TOP_MOUT_DISP_MEDIA_PIXEL, "mout_disp_media_pixel",
+			mout_disp_media_pixel_p,
+			MUX_SEL_TOP_DISP1, 8, 1),
+
+	MUX(TOP_MOUT_SCLK_PERI_SPI2_CLK, "mout_sclk_peri_spi2_clk",
+			mout_sclk_peri_spi_clk_p,
+			MUX_SEL_TOP_PERI1, 0, 1),
+	MUX(TOP_MOUT_SCLK_PERI_SPI1_CLK, "mout_sclk_peri_spi1_clk",
+			mout_sclk_peri_spi_clk_p,
+			MUX_SEL_TOP_PERI1, 4, 1),
+	MUX(TOP_MOUT_SCLK_PERI_SPI0_CLK, "mout_sclk_peri_spi0_clk",
+			mout_sclk_peri_spi_clk_p,
+			MUX_SEL_TOP_PERI1, 8, 1),
+	MUX(TOP_MOUT_SCLK_PERI_UART1_UCLK, "mout_sclk_peri_uart1_uclk",
+			mout_sclk_peri_uart_uclk_p,
+			MUX_SEL_TOP_PERI1, 12, 1),
+	MUX(TOP_MOUT_SCLK_PERI_UART2_UCLK, "mout_sclk_peri_uart2_uclk",
+			mout_sclk_peri_uart_uclk_p,
+			MUX_SEL_TOP_PERI1, 16, 1),
+	MUX(TOP_MOUT_SCLK_PERI_UART0_UCLK, "mout_sclk_peri_uart0_uclk",
+			mout_sclk_peri_uart_uclk_p,
+			MUX_SEL_TOP_PERI1, 20, 1),
+
+
+	MUX(TOP_MOUT_BUS1_BUSTOP_400, "mout_bus1_bustop_400",
+			mout_bus_bustop_400_p,
+			MUX_SEL_TOP_BUS, 0, 1),
+	MUX(TOP_MOUT_BUS1_BUSTOP_100, "mout_bus1_bustop_100",
+			mout_bus_bustop_100_p,
+			MUX_SEL_TOP_BUS, 4, 1),
+	MUX(TOP_MOUT_BUS2_BUSTOP_100, "mout_bus2_bustop_100",
+			mout_bus_bustop_100_p,
+			MUX_SEL_TOP_BUS, 8, 1),
+	MUX(TOP_MOUT_BUS2_BUSTOP_400, "mout_bus2_bustop_400",
+			mout_bus_bustop_400_p,
+			MUX_SEL_TOP_BUS, 12, 1),
+	MUX(TOP_MOUT_BUS3_BUSTOP_400, "mout_bus3_bustop_400",
+			mout_bus_bustop_400_p,
+			MUX_SEL_TOP_BUS, 16, 1),
+	MUX(TOP_MOUT_BUS3_BUSTOP_100, "mout_bus3_bustop_100",
+			mout_bus_bustop_100_p,
+			MUX_SEL_TOP_BUS, 20, 1),
+	MUX(TOP_MOUT_BUS4_BUSTOP_400, "mout_bus4_bustop_400",
+			mout_bus_bustop_400_p,
+			MUX_SEL_TOP_BUS, 24, 1),
+	MUX(TOP_MOUT_BUS4_BUSTOP_100, "mout_bus4_bustop_100",
+			mout_bus_bustop_100_p,
+			MUX_SEL_TOP_BUS, 28, 1),
+
+	MUX(TOP_MOUT_SCLK_FSYS_USB, "mout_sclk_fsys_usb",
+			mout_sclk_fsys_usb_p,
+			MUX_SEL_TOP_FSYS, 0, 1),
+	MUX(TOP_MOUT_SCLK_FSYS_MMC2_SDCLKIN_A, "mout_sclk_fsys_mmc2_sdclkin_a",
+			mout_sclk_fsys_mmc_sdclkin_a_p,
+			MUX_SEL_TOP_FSYS, 4, 1),
+	MUX(TOP_MOUT_SCLK_FSYS_MMC2_SDCLKIN_B, "mout_sclk_fsys_mmc2_sdclkin_b",
+			mout_sclk_fsys_mmc2_sdclkin_b_p,
+			MUX_SEL_TOP_FSYS, 8, 1),
+	MUX(TOP_MOUT_SCLK_FSYS_MMC1_SDCLKIN_A, "mout_sclk_fsys_mmc1_sdclkin_a",
+			mout_sclk_fsys_mmc_sdclkin_a_p,
+			MUX_SEL_TOP_FSYS, 12, 1),
+	MUX(TOP_MOUT_SCLK_FSYS_MMC1_SDCLKIN_B, "mout_sclk_fsys_mmc1_sdclkin_b",
+			mout_sclk_fsys_mmc1_sdclkin_b_p,
+			MUX_SEL_TOP_FSYS, 16, 1),
+	MUX(TOP_MOUT_SCLK_FSYS_MMC0_SDCLKIN_A, "mout_sclk_fsys_mmc0_sdclkin_a",
+			mout_sclk_fsys_mmc_sdclkin_a_p,
+			MUX_SEL_TOP_FSYS, 20, 1),
+	MUX(TOP_MOUT_SCLK_FSYS_MMC0_SDCLKIN_B, "mout_sclk_fsys_mmc0_sdclkin_b",
+			mout_sclk_fsys_mmc0_sdclkin_b_p,
+			MUX_SEL_TOP_FSYS, 24, 1),
+
+	MUX(TOP_MOUT_ISP1_MEDIA_400, "mout_isp1_media_400",
+			mout_isp1_media_400_p,
+			MUX_SEL_TOP_ISP10, 4, 1),
+	MUX(TOP_MOUT_ACLK_ISP1_400, "mout_aclk_isp1_400", mout_aclk_isp1_400_p,
+			MUX_SEL_TOP_ISP10, 8 , 1),
+	MUX(TOP_MOUT_ISP1_MEDIA_266, "mout_isp1_media_266",
+			mout_isp1_media_266_p,
+			MUX_SEL_TOP_ISP10, 16, 1),
+	MUX(TOP_MOUT_ACLK_ISP1_266, "mout_aclk_isp1_266", mout_aclk_isp1_266_p,
+			MUX_SEL_TOP_ISP10, 20, 1),
+
+	MUX(TOP_MOUT_SCLK_ISP1_SPI0, "mout_sclk_isp1_spi0", mout_sclk_isp_spi_p,
+			MUX_SEL_TOP_ISP11, 4, 1),
+	MUX(TOP_MOUT_SCLK_ISP1_SPI1, "mout_sclk_isp1_spi1", mout_sclk_isp_spi_p,
+			MUX_SEL_TOP_ISP11, 8, 1),
+	MUX(TOP_MOUT_SCLK_ISP1_UART, "mout_sclk_isp1_uart",
+			mout_sclk_isp_uart_p,
+			MUX_SEL_TOP_ISP11, 12, 1),
+	MUX(TOP_MOUT_SCLK_ISP1_SENSOR0, "mout_sclk_isp1_sensor0",
+			mout_sclk_isp_sensor_p,
+			MUX_SEL_TOP_ISP11, 16, 1),
+	MUX(TOP_MOUT_SCLK_ISP1_SENSOR1, "mout_sclk_isp1_sensor1",
+			mout_sclk_isp_sensor_p,
+			MUX_SEL_TOP_ISP11, 20, 1),
+	MUX(TOP_MOUT_SCLK_ISP1_SENSOR2, "mout_sclk_isp1_sensor2",
+			mout_sclk_isp_sensor_p,
+			MUX_SEL_TOP_ISP11, 24, 1),
+
+	MUX(TOP_MOUT_MFC_BUSTOP_333, "mout_mfc_bustop_333",
+			mout_mfc_bustop_333_p,
+			MUX_SEL_TOP_MFC, 4, 1),
+	MUX(TOP_MOUT_ACLK_MFC_333, "mout_aclk_mfc_333", mout_aclk_mfc_333_p,
+			MUX_SEL_TOP_MFC, 8, 1),
+
+	MUX(TOP_MOUT_G2D_BUSTOP_333, "mout_g2d_bustop_333",
+			mout_g2d_bustop_333_p,
+			MUX_SEL_TOP_G2D, 4, 1),
+	MUX(TOP_MOUT_ACLK_G2D_333, "mout_aclk_g2d_333", mout_aclk_g2d_333_p,
+			MUX_SEL_TOP_G2D, 8, 1),
+
+	MUX(TOP_MOUT_M2M_MEDIATOP_400, "mout_m2m_mediatop_400",
+			mout_m2m_mediatop_400_p,
+			MUX_SEL_TOP_GSCL, 0, 1),
+	MUX(TOP_MOUT_ACLK_GSCL_400, "mout_aclk_gscl_400",
+			mout_aclk_gscl_400_p,
+			MUX_SEL_TOP_GSCL, 4, 1),
+	MUX(TOP_MOUT_GSCL_BUSTOP_333, "mout_gscl_bustop_333",
+			mout_gscl_bustop_333_p,
+			MUX_SEL_TOP_GSCL, 8, 1),
+	MUX(TOP_MOUT_ACLK_GSCL_333, "mout_aclk_gscl_333",
+			mout_aclk_gscl_333_p,
+			MUX_SEL_TOP_GSCL, 12, 1),
+	MUX(TOP_MOUT_GSCL_BUSTOP_FIMC, "mout_gscl_bustop_fimc",
+			mout_gscl_bustop_fimc_p,
+			MUX_SEL_TOP_GSCL, 16, 1),
+	MUX(TOP_MOUT_ACLK_GSCL_FIMC, "mout_aclk_gscl_fimc",
+			mout_aclk_gscl_fimc_p,
+			MUX_SEL_TOP_GSCL, 20, 1),
+};
+
+struct samsung_div_clock top_div_clks[] __initdata = {
+	DIV(TOP_DOUT_ACLK_G2D_333, "dout_aclk_g2d_333", "mout_aclk_g2d_333",
+			DIV_TOP_G2D_MFC, 0, 3),
+	DIV(TOP_DOUT_ACLK_MFC_333, "dout_aclk_mfc_333", "mout_aclk_mfc_333",
+			DIV_TOP_G2D_MFC, 4, 3),
+
+	DIV(TOP_DOUT_ACLK_GSCL_333, "dout_aclk_gscl_333", "mout_aclk_gscl_333",
+			DIV_TOP_GSCL_ISP0, 0, 3),
+	DIV(TOP_DOUT_ACLK_GSCL_400, "dout_aclk_gscl_400", "mout_aclk_gscl_400",
+			DIV_TOP_GSCL_ISP0, 4, 3),
+	DIV(TOP_DOUT_ACLK_GSCL_FIMC, "dout_aclk_gscl_fimc",
+			"mout_aclk_gscl_fimc", DIV_TOP_GSCL_ISP0, 8, 3),
+	DIV(TOP_DOUT_SCLK_ISP1_SENSOR0_A, "dout_sclk_isp1_sensor0_a",
+			"mout_aclk_gscl_fimc", DIV_TOP_GSCL_ISP0, 16, 4),
+	DIV(TOP_DOUT_SCLK_ISP1_SENSOR1_A, "dout_sclk_isp1_sensor1_a",
+			"mout_aclk_gscl_400", DIV_TOP_GSCL_ISP0, 20, 4),
+	DIV(TOP_DOUT_SCLK_ISP1_SENSOR2_A, "dout_sclk_isp1_sensor2_a",
+			"mout_aclk_gscl_fimc", DIV_TOP_GSCL_ISP0, 24, 4),
+
+	DIV(TOP_DOUT_ACLK_ISP1_266, "dout_aclk_isp1_266", "mout_aclk_isp1_266",
+			DIV_TOP_ISP10, 0, 3),
+	DIV(TOP_DOUT_ACLK_ISP1_400, "dout_aclk_isp1_400", "mout_aclk_isp1_400",
+			DIV_TOP_ISP10, 4, 3),
+	DIV(TOP_DOUT_SCLK_ISP1_SPI0_A, "dout_sclk_isp1_spi0_a",
+			"mout_sclk_isp1_spi0", DIV_TOP_ISP10, 12, 4),
+	DIV(TOP_DOUT_SCLK_ISP1_SPI0_B, "dout_sclk_isp1_spi0_b",
+			"dout_sclk_isp1_spi0_a", DIV_TOP_ISP10, 16, 8),
+
+	DIV(TOP_DOUT_SCLK_ISP1_SPI1_A, "dout_sclk_isp1_spi1_a",
+			"mout_sclk_isp1_spi1", DIV_TOP_ISP11, 0, 4),
+	DIV(TOP_DOUT_SCLK_ISP1_SPI1_B, "dout_sclk_isp1_spi1_b",
+			"dout_sclk_isp1_spi1_a", DIV_TOP_ISP11, 4, 8),
+	DIV(TOP_DOUT_SCLK_ISP1_UART, "dout_sclk_isp1_uart",
+			"mout_sclk_isp1_uart", DIV_TOP_ISP11, 12, 4),
+	DIV(TOP_DOUT_SCLK_ISP1_SENSOR0_B, "dout_sclk_isp1_sensor0_b",
+			"dout_sclk_isp1_sensor0_a", DIV_TOP_ISP11, 16, 4),
+	DIV(TOP_DOUT_SCLK_ISP1_SENSOR1_B, "dout_sclk_isp1_sensor1_b",
+			"dout_sclk_isp1_sensor1_a", DIV_TOP_ISP11, 20, 4),
+	DIV(TOP_DOUT_SCLK_ISP1_SENSOR2_B, "dout_sclk_isp1_sensor2_b",
+			"dout_sclk_isp1_sensor2_a", DIV_TOP_ISP11, 24, 4),
+
+	DIV(TOP_DOUTTOP__SCLK_HPM_TARGETCLK, "dout_sclk_hpm_targetclk",
+			"mout_bustop_pll_user", DIV_TOP_HPM, 0, 3),
+
+	DIV(TOP_DOUT_ACLK_DISP_333, "dout_aclk_disp_333", "mout_aclk_disp_333",
+			DIV_TOP_DISP, 0, 3),
+	DIV(TOP_DOUT_ACLK_DISP_222, "dout_aclk_disp_222", "mout_aclk_disp_222",
+			DIV_TOP_DISP, 4, 3),
+	DIV(TOP_DOUT_SCLK_DISP_PIXEL, "dout_sclk_disp_pixel",
+			"mout_sclk_disp_pixel",	DIV_TOP_DISP, 8, 3),
+
+	DIV(TOP_DOUT_ACLK_BUS1_400, "dout_aclk_bus1_400",
+			"mout_bus1_bustop_400",	DIV_TOP_BUS, 0, 3),
+	DIV(TOP_DOUT_ACLK_BUS1_100, "dout_aclk_bus1_100",
+			"mout_bus1_bustop_100",	DIV_TOP_BUS, 4, 4),
+	DIV(TOP_DOUT_ACLK_BUS2_400, "dout_aclk_bus2_400",
+			"mout_bus2_bustop_400",	DIV_TOP_BUS, 8, 3),
+	DIV(TOP_DOUT_ACLK_BUS2_100, "dout_aclk_bus2_100",
+			"mout_bus2_bustop_100",	DIV_TOP_BUS, 12, 4),
+	DIV(TOP_DOUT_ACLK_BUS3_400, "dout_aclk_bus3_400",
+			"mout_bus3_bustop_400", DIV_TOP_BUS, 16, 3),
+	DIV(TOP_DOUT_ACLK_BUS3_100, "dout_aclk_bus3_100",
+			"mout_bus3_bustop_100",	DIV_TOP_BUS, 20, 4),
+	DIV(TOP_DOUT_ACLK_BUS4_400, "dout_aclk_bus4_400",
+			"mout_bus4_bustop_400",	DIV_TOP_BUS, 24, 3),
+	DIV(TOP_DOUT_ACLK_BUS4_100, "dout_aclk_bus4_100",
+			"mout_bus4_bustop_100",	DIV_TOP_BUS, 28, 4),
+
+	DIV(TOP_DOUT_SCLK_PERI_SPI0_A, "dout_sclk_peri_spi0_a",
+			"mout_sclk_peri_spi0_clk", DIV_TOP_PERI0, 4, 4),
+	DIV(TOP_DOUT_SCLK_PERI_SPI0_B, "dout_sclk_peri_spi0_b",
+			"dout_sclk_peri_spi0_a", DIV_TOP_PERI0, 8, 8),
+	DIV(TOP_DOUT_SCLK_PERI_SPI1_A, "dout_sclk_peri_spi1_a",
+			"mout_sclk_peri_spi1_clk", DIV_TOP_PERI0, 16, 4),
+	DIV(TOP_DOUT_SCLK_PERI_SPI1_B, "dout_sclk_peri_spi1_b",
+			"dout_sclk_peri_spi1_a", DIV_TOP_PERI0, 20, 8),
+
+	DIV(TOP_DOUT_SCLK_PERI_SPI2_A, "dout_sclk_peri_spi2_a",
+			"mout_sclk_peri_spi2_clk", DIV_TOP_PERI1, 0, 4),
+	DIV(TOP_DOUT_SCLK_PERI_SPI2_B, "dout_sclk_peri_spi2_b",
+			"dout_sclk_peri_spi2_a", DIV_TOP_PERI1, 4, 8),
+	DIV(TOP_DOUT_SCLK_PERI_UART1, "dout_sclk_peri_uart1",
+			"mout_sclk_peri_uart1_uclk", DIV_TOP_PERI1, 16, 4),
+	DIV(TOP_DOUT_SCLK_PERI_UART2, "dout_sclk_peri_uart2",
+			"mout_sclk_peri_uart2_uclk", DIV_TOP_PERI1, 20, 4),
+	DIV(TOP_DOUT_SCLK_PERI_UART0, "dout_sclk_peri_uart0",
+			"mout_sclk_peri_uart0_uclk", DIV_TOP_PERI1, 24, 4),
+
+	DIV(TOP_DOUT_ACLK_PERI_66, "dout_aclk_peri_66", "mout_bustop_pll_user",
+			DIV_TOP_PERI2, 20, 4),
+	DIV(TOP_DOUT_ACLK_PERI_AUD, "dout_aclk_peri_aud",
+			"mout_audtop_pll_user", DIV_TOP_PERI2, 24, 3),
+
+	DIV(TOP_DOUT_ACLK_FSYS_200, "dout_aclk_fsys_200",
+			"mout_bustop_pll_user", DIV_TOP_FSYS0, 0, 3),
+	DIV(TOP_DOUT_SCLK_FSYS_USBDRD30_SUSPEND_CLK,
+			"dout_sclk_fsys_usbdrd30_suspend_clk",
+			"mout_sclk_fsys_usb", DIV_TOP_FSYS0, 4, 4),
+	DIV(TOP_DOUT_SCLK_FSYS_MMC0_SDCLKIN_A, "dout_sclk_fsys_mmc0_sdclkin_a",
+			"mout_sclk_fsys_mmc0_sdclkin_b",
+			DIV_TOP_FSYS0, 12, 4),
+	DIV(TOP_DOUT_SCLK_FSYS_MMC0_SDCLKIN_B, "dout_sclk_fsys_mmc0_sdclkin_b",
+			"dout_sclk_fsys_mmc0_sdclkin_a",
+			DIV_TOP_FSYS0, 16, 8),
+
+
+	DIV(TOP_DOUT_SCLK_FSYS_MMC1_SDCLKIN_A, "dout_sclk_fsys_mmc1_sdclkin_a",
+			"mout_sclk_fsys_mmc1_sdclkin_b",
+			DIV_TOP_FSYS1, 0, 4),
+	DIV(TOP_DOUT_SCLK_FSYS_MMC1_SDCLKIN_B, "dout_sclk_fsys_mmc1_sdclkin_b",
+			"dout_sclk_fsys_mmc1_sdclkin_a",
+			DIV_TOP_FSYS1, 4, 8),
+	DIV(TOP_DOUT_SCLK_FSYS_MMC2_SDCLKIN_A, "dout_sclk_fsys_mmc2_sdclkin_a",
+			"mout_sclk_fsys_mmc2_sdclkin_b",
+			DIV_TOP_FSYS1, 12, 4),
+	DIV(TOP_DOUT_SCLK_FSYS_MMC2_SDCLKIN_B, "dout_sclk_fsys_mmc2_sdclkin_b",
+			"dout_sclk_fsys_mmc2_sdclkin_a",
+			DIV_TOP_FSYS1, 16, 8),
+
+};
+
+struct samsung_gate_clock top_gate_clks[] __initdata = {
+	GATE(TOP_SCLK_MMC0, "sclk_fsys_mmc0_sdclkin",
+			"dout_sclk_fsys_mmc0_sdclkin_b",
+			EN_SCLK_TOP, 7, CLK_SET_RATE_PARENT, 0),
+	GATE(TOP_SCLK_MMC1, "sclk_fsys_mmc1_sdclkin",
+			"dout_sclk_fsys_mmc1_sdclkin_b",
+			EN_SCLK_TOP, 8, CLK_SET_RATE_PARENT, 0),
+	GATE(TOP_SCLK_MMC2, "sclk_fsys_mmc2_sdclkin",
+			"dout_sclk_fsys_mmc2_sdclkin_b",
+			EN_SCLK_TOP, 9, CLK_SET_RATE_PARENT, 0),
+	GATE(TOP_SCLK_FIMD1, "sclk_disp_pixel", "dout_sclk_disp_pixel",
+			EN_ACLK_TOP, 10, CLK_IGNORE_UNUSED |
+			CLK_SET_RATE_PARENT, 0),
+};
+
+static struct samsung_pll_clock top_pll_clks[] __initdata = {
+	PLL(pll_2550xx, TOP_FOUT_DISP_PLL, "fout_disp_pll", "fin_pll",
+		DISP_PLL_LOCK, DISP_PLL_CON0,
+		pll2550_24mhz_tbl),
+	PLL(pll_2650xx, TOP_FOUT_AUD_PLL, "fout_aud_pll", "fin_pll",
+		AUD_PLL_LOCK, AUD_PLL_CON0,
+		pll2650_24mhz_tbl),
+};
+
+static void __init exynos5260_clk_top_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.pll_clks = top_pll_clks;
+	cmu.nr_pll_clks =  ARRAY_SIZE(top_pll_clks);
+	cmu.mux_clks = top_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(top_mux_clks);
+	cmu.div_clks = top_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(top_div_clks);
+	cmu.gate_clks = top_gate_clks;
+	cmu.nr_gate_clks = ARRAY_SIZE(top_gate_clks);
+	cmu.fixed_clks = fixed_rate_clks;
+	cmu.nr_fixed_clks = ARRAY_SIZE(fixed_rate_clks);
+	cmu.nr_clk_ids = TOP_NR_CLK;
+	cmu.clk_regs = top_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(top_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_top, "samsung,exynos5260-clock-top",
+		exynos5260_clk_top_init);
diff --git a/drivers/clk/samsung/clk-exynos5260.h b/drivers/clk/samsung/clk-exynos5260.h
new file mode 100644
index 00000000000..d739716d6ea
--- /dev/null
+++ b/drivers/clk/samsung/clk-exynos5260.h
@@ -0,0 +1,459 @@
+/*
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ * Author: Rahul Sharma <rahul.sharma@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.
+ *
+ * Common Clock Framework support for Exynos5260 SoC.
+ */
+
+#ifndef __CLK_EXYNOS5260_H
+#define __CLK_EXYNOS5260_H
+
+/*
+*Registers for CMU_AUD
+*/
+#define MUX_SEL_AUD				0x0200
+#define MUX_ENABLE_AUD				0x0300
+#define MUX_STAT_AUD				0x0400
+#define MUX_IGNORE_AUD				0x0500
+#define DIV_AUD0				0x0600
+#define DIV_AUD1				0x0604
+#define DIV_STAT_AUD0				0x0700
+#define DIV_STAT_AUD1				0x0704
+#define EN_ACLK_AUD				0x0800
+#define EN_PCLK_AUD				0x0900
+#define EN_SCLK_AUD				0x0a00
+#define EN_IP_AUD				0x0b00
+
+/*
+*Registers for CMU_DISP
+*/
+#define MUX_SEL_DISP0				0x0200
+#define MUX_SEL_DISP1				0x0204
+#define MUX_SEL_DISP2				0x0208
+#define MUX_SEL_DISP3				0x020C
+#define MUX_SEL_DISP4				0x0210
+#define MUX_ENABLE_DISP0			0x0300
+#define MUX_ENABLE_DISP1			0x0304
+#define MUX_ENABLE_DISP2			0x0308
+#define MUX_ENABLE_DISP3			0x030c
+#define MUX_ENABLE_DISP4			0x0310
+#define MUX_STAT_DISP0				0x0400
+#define MUX_STAT_DISP1				0x0404
+#define MUX_STAT_DISP2				0x0408
+#define MUX_STAT_DISP3				0x040c
+#define MUX_STAT_DISP4				0x0410
+#define MUX_IGNORE_DISP0			0x0500
+#define MUX_IGNORE_DISP1			0x0504
+#define MUX_IGNORE_DISP2			0x0508
+#define MUX_IGNORE_DISP3			0x050c
+#define MUX_IGNORE_DISP4			0x0510
+#define DIV_DISP				0x0600
+#define DIV_STAT_DISP				0x0700
+#define EN_ACLK_DISP				0x0800
+#define EN_PCLK_DISP				0x0900
+#define EN_SCLK_DISP0				0x0a00
+#define EN_SCLK_DISP1				0x0a04
+#define EN_IP_DISP				0x0b00
+#define EN_IP_DISP_BUS				0x0b04
+
+
+/*
+*Registers for CMU_EGL
+*/
+#define EGL_PLL_LOCK				0x0000
+#define EGL_DPLL_LOCK				0x0004
+#define EGL_PLL_CON0				0x0100
+#define EGL_PLL_CON1				0x0104
+#define EGL_PLL_FREQ_DET			0x010c
+#define EGL_DPLL_CON0				0x0110
+#define EGL_DPLL_CON1				0x0114
+#define EGL_DPLL_FREQ_DET			0x011c
+#define MUX_SEL_EGL				0x0200
+#define MUX_ENABLE_EGL				0x0300
+#define MUX_STAT_EGL				0x0400
+#define DIV_EGL					0x0600
+#define DIV_EGL_PLL_FDET			0x0604
+#define DIV_STAT_EGL				0x0700
+#define DIV_STAT_EGL_PLL_FDET			0x0704
+#define EN_ACLK_EGL				0x0800
+#define EN_PCLK_EGL				0x0900
+#define EN_SCLK_EGL				0x0a00
+#define EN_IP_EGL				0x0b00
+#define CLKOUT_CMU_EGL				0x0c00
+#define CLKOUT_CMU_EGL_DIV_STAT			0x0c04
+#define ARMCLK_STOPCTRL				0x1000
+#define EAGLE_EMA_CTRL				0x1008
+#define EAGLE_EMA_STATUS			0x100c
+#define PWR_CTRL				0x1020
+#define PWR_CTRL2				0x1024
+#define CLKSTOP_CTRL				0x1028
+#define INTR_SPREAD_EN				0x1080
+#define INTR_SPREAD_USE_STANDBYWFI		0x1084
+#define INTR_SPREAD_BLOCKING_DURATION		0x1088
+#define CMU_EGL_SPARE0				0x2000
+#define CMU_EGL_SPARE1				0x2004
+#define CMU_EGL_SPARE2				0x2008
+#define CMU_EGL_SPARE3				0x200c
+#define CMU_EGL_SPARE4				0x2010
+
+/*
+*Registers for CMU_FSYS
+*/
+
+#define MUX_SEL_FSYS0				0x0200
+#define MUX_SEL_FSYS1				0x0204
+#define MUX_ENABLE_FSYS0			0x0300
+#define MUX_ENABLE_FSYS1			0x0304
+#define MUX_STAT_FSYS0				0x0400
+#define MUX_STAT_FSYS1				0x0404
+#define MUX_IGNORE_FSYS0			0x0500
+#define MUX_IGNORE_FSYS1			0x0504
+#define EN_ACLK_FSYS				0x0800
+#define EN_ACLK_FSYS_SECURE_RTIC		0x0804
+#define EN_ACLK_FSYS_SECURE_SMMU_RTIC		0x0808
+#define EN_PCLK_FSYS				0x0900
+#define EN_SCLK_FSYS				0x0a00
+#define EN_IP_FSYS				0x0b00
+#define EN_IP_FSYS_SECURE_RTIC			0x0b04
+#define EN_IP_FSYS_SECURE_SMMU_RTIC		0x0b08
+
+/*
+*Registers for CMU_G2D
+*/
+
+#define MUX_SEL_G2D				0x0200
+#define MUX_ENABLE_G2D				0x0300
+#define MUX_STAT_G2D				0x0400
+#define DIV_G2D					0x0600
+#define DIV_STAT_G2D				0x0700
+#define EN_ACLK_G2D				0x0800
+#define EN_ACLK_G2D_SECURE_SSS			0x0804
+#define EN_ACLK_G2D_SECURE_SLIM_SSS		0x0808
+#define EN_ACLK_G2D_SECURE_SMMU_SLIM_SSS	0x080c
+#define EN_ACLK_G2D_SECURE_SMMU_SSS		0x0810
+#define EN_ACLK_G2D_SECURE_SMMU_MDMA		0x0814
+#define EN_ACLK_G2D_SECURE_SMMU_G2D		0x0818
+#define EN_PCLK_G2D				0x0900
+#define EN_PCLK_G2D_SECURE_SMMU_SLIM_SSS	0x0904
+#define EN_PCLK_G2D_SECURE_SMMU_SSS		0x0908
+#define EN_PCLK_G2D_SECURE_SMMU_MDMA		0x090c
+#define EN_PCLK_G2D_SECURE_SMMU_G2D		0x0910
+#define EN_IP_G2D				0x0b00
+#define EN_IP_G2D_SECURE_SSS			0x0b04
+#define EN_IP_G2D_SECURE_SLIM_SSS		0x0b08
+#define EN_IP_G2D_SECURE_SMMU_SLIM_SSS		0x0b0c
+#define EN_IP_G2D_SECURE_SMMU_SSS		0x0b10
+#define EN_IP_G2D_SECURE_SMMU_MDMA		0x0b14
+#define EN_IP_G2D_SECURE_SMMU_G2D		0x0b18
+
+/*
+*Registers for CMU_G3D
+*/
+
+#define G3D_PLL_LOCK				0x0000
+#define G3D_PLL_CON0				0x0100
+#define G3D_PLL_CON1				0x0104
+#define G3D_PLL_FDET				0x010c
+#define MUX_SEL_G3D				0x0200
+#define MUX_EN_G3D				0x0300
+#define MUX_STAT_G3D				0x0400
+#define MUX_IGNORE_G3D				0x0500
+#define DIV_G3D					0x0600
+#define DIV_G3D_PLL_FDET			0x0604
+#define DIV_STAT_G3D				0x0700
+#define DIV_STAT_G3D_PLL_FDET			0x0704
+#define EN_ACLK_G3D				0x0800
+#define EN_PCLK_G3D				0x0900
+#define EN_SCLK_G3D				0x0a00
+#define EN_IP_G3D				0x0b00
+#define CLKOUT_CMU_G3D				0x0c00
+#define CLKOUT_CMU_G3D_DIV_STAT			0x0c04
+#define G3DCLK_STOPCTRL				0x1000
+#define G3D_EMA_CTRL				0x1008
+#define G3D_EMA_STATUS				0x100c
+
+/*
+*Registers for CMU_GSCL
+*/
+
+#define MUX_SEL_GSCL				0x0200
+#define MUX_EN_GSCL				0x0300
+#define MUX_STAT_GSCL				0x0400
+#define MUX_IGNORE_GSCL				0x0500
+#define DIV_GSCL				0x0600
+#define DIV_STAT_GSCL				0x0700
+#define EN_ACLK_GSCL				0x0800
+#define EN_ACLK_GSCL_FIMC			0x0804
+#define EN_ACLK_GSCL_SECURE_SMMU_GSCL0		0x0808
+#define EN_ACLK_GSCL_SECURE_SMMU_GSCL1		0x080c
+#define EN_ACLK_GSCL_SECURE_SMMU_MSCL0		0x0810
+#define EN_ACLK_GSCL_SECURE_SMMU_MSCL1		0x0814
+#define EN_PCLK_GSCL				0x0900
+#define EN_PCLK_GSCL_FIMC			0x0904
+#define EN_PCLK_GSCL_SECURE_SMMU_GSCL0		0x0908
+#define EN_PCLK_GSCL_SECURE_SMMU_GSCL1		0x090c
+#define EN_PCLK_GSCL_SECURE_SMMU_MSCL0		0x0910
+#define EN_PCLK_GSCL_SECURE_SMMU_MSCL1		0x0914
+#define EN_SCLK_GSCL				0x0a00
+#define EN_SCLK_GSCL_FIMC			0x0a04
+#define EN_IP_GSCL				0x0b00
+#define EN_IP_GSCL_FIMC				0x0b04
+#define EN_IP_GSCL_SECURE_SMMU_GSCL0		0x0b08
+#define EN_IP_GSCL_SECURE_SMMU_GSCL1		0x0b0c
+#define EN_IP_GSCL_SECURE_SMMU_MSCL0		0x0b10
+#define EN_IP_GSCL_SECURE_SMMU_MSCL1		0x0b14
+
+/*
+*Registers for CMU_ISP
+*/
+#define MUX_SEL_ISP0				0x0200
+#define MUX_SEL_ISP1				0x0204
+#define MUX_ENABLE_ISP0				0x0300
+#define MUX_ENABLE_ISP1				0x0304
+#define MUX_STAT_ISP0				0x0400
+#define MUX_STAT_ISP1				0x0404
+#define MUX_IGNORE_ISP0				0x0500
+#define MUX_IGNORE_ISP1				0x0504
+#define DIV_ISP					0x0600
+#define DIV_STAT_ISP				0x0700
+#define EN_ACLK_ISP0				0x0800
+#define EN_ACLK_ISP1				0x0804
+#define EN_PCLK_ISP0				0x0900
+#define EN_PCLK_ISP1				0x0904
+#define EN_SCLK_ISP				0x0a00
+#define EN_IP_ISP0				0x0b00
+#define EN_IP_ISP1				0x0b04
+
+/*
+*Registers for CMU_KFC
+*/
+#define KFC_PLL_LOCK				0x0000
+#define KFC_PLL_CON0				0x0100
+#define KFC_PLL_CON1				0x0104
+#define KFC_PLL_FDET				0x010c
+#define MUX_SEL_KFC0				0x0200
+#define MUX_SEL_KFC2				0x0208
+#define MUX_ENABLE_KFC0				0x0300
+#define MUX_ENABLE_KFC2				0x0308
+#define MUX_STAT_KFC0				0x0400
+#define MUX_STAT_KFC2				0x0408
+#define DIV_KFC					0x0600
+#define DIV_KFC_PLL_FDET			0x0604
+#define DIV_STAT_KFC				0x0700
+#define DIV_STAT_KFC_PLL_FDET			0x0704
+#define EN_ACLK_KFC				0x0800
+#define EN_PCLK_KFC				0x0900
+#define EN_SCLK_KFC				0x0a00
+#define EN_IP_KFC				0x0b00
+#define CLKOUT_CMU_KFC				0x0c00
+#define CLKOUT_CMU_KFC_DIV_STAT			0x0c04
+#define ARMCLK_STOPCTRL_KFC			0x1000
+#define ARM_EMA_CTRL				0x1008
+#define ARM_EMA_STATUS				0x100c
+#define PWR_CTRL_KFC				0x1020
+#define PWR_CTRL2_KFC				0x1024
+#define CLKSTOP_CTRL_KFC			0x1028
+#define INTR_SPREAD_ENABLE_KFC			0x1080
+#define INTR_SPREAD_USE_STANDBYWFI_KFC		0x1084
+#define INTR_SPREAD_BLOCKING_DURATION_KFC	0x1088
+#define CMU_KFC_SPARE0				0x2000
+#define CMU_KFC_SPARE1				0x2004
+#define CMU_KFC_SPARE2				0x2008
+#define CMU_KFC_SPARE3				0x200c
+#define CMU_KFC_SPARE4				0x2010
+
+/*
+*Registers for CMU_MFC
+*/
+#define MUX_SEL_MFC				0x0200
+#define MUX_ENABLE_MFC				0x0300
+#define MUX_STAT_MFC				0x0400
+#define DIV_MFC					0x0600
+#define DIV_STAT_MFC				0x0700
+#define EN_ACLK_MFC				0x0800
+#define EN_ACLK_SECURE_SMMU2_MFC		0x0804
+#define EN_PCLK_MFC				0x0900
+#define EN_PCLK_SECURE_SMMU2_MFC		0x0904
+#define EN_IP_MFC				0x0b00
+#define EN_IP_MFC_SECURE_SMMU2_MFC		0x0b04
+
+/*
+*Registers for CMU_MIF
+*/
+#define MEM_PLL_LOCK				0x0000
+#define BUS_PLL_LOCK				0x0004
+#define MEDIA_PLL_LOCK				0x0008
+#define MEM_PLL_CON0				0x0100
+#define MEM_PLL_CON1				0x0104
+#define MEM_PLL_FDET				0x010c
+#define BUS_PLL_CON0				0x0110
+#define BUS_PLL_CON1				0x0114
+#define BUS_PLL_FDET				0x011c
+#define MEDIA_PLL_CON0				0x0120
+#define MEDIA_PLL_CON1				0x0124
+#define MEDIA_PLL_FDET				0x012c
+#define MUX_SEL_MIF				0x0200
+#define MUX_ENABLE_MIF				0x0300
+#define MUX_STAT_MIF				0x0400
+#define MUX_IGNORE_MIF				0x0500
+#define DIV_MIF					0x0600
+#define DIV_MIF_PLL_FDET			0x0604
+#define DIV_STAT_MIF				0x0700
+#define DIV_STAT_MIF_PLL_FDET			0x0704
+#define EN_ACLK_MIF				0x0800
+#define EN_ACLK_MIF_SECURE_DREX1_TZ		0x0804
+#define EN_ACLK_MIF_SECURE_DREX0_TZ		0x0808
+#define EN_ACLK_MIF_SECURE_INTMEM		0x080c
+#define EN_PCLK_MIF				0x0900
+#define EN_PCLK_MIF_SECURE_MONOCNT		0x0904
+#define EN_PCLK_MIF_SECURE_RTC_APBIF		0x0908
+#define EN_PCLK_MIF_SECURE_DREX1_TZ		0x090c
+#define EN_PCLK_MIF_SECURE_DREX0_TZ		0x0910
+#define EN_SCLK_MIF				0x0a00
+#define EN_IP_MIF				0x0b00
+#define EN_IP_MIF_SECURE_MONOCNT		0x0b04
+#define EN_IP_MIF_SECURE_RTC_APBIF		0x0b08
+#define EN_IP_MIF_SECURE_DREX1_TZ		0x0b0c
+#define EN_IP_MIF_SECURE_DREX0_TZ		0x0b10
+#define EN_IP_MIF_SECURE_INTEMEM		0x0b14
+#define CLKOUT_CMU_MIF_DIV_STAT			0x0c04
+#define DREX_FREQ_CTRL				0x1000
+#define PAUSE					0x1004
+#define DDRPHY_LOCK_CTRL			0x1008
+#define CLKOUT_CMU_MIF				0xcb00
+
+/*
+*Registers for CMU_PERI
+*/
+#define MUX_SEL_PERI				0x0200
+#define MUX_SEL_PERI1				0x0204
+#define MUX_ENABLE_PERI				0x0300
+#define MUX_ENABLE_PERI1			0x0304
+#define MUX_STAT_PERI				0x0400
+#define MUX_STAT_PERI1				0x0404
+#define MUX_IGNORE_PERI				0x0500
+#define MUX_IGNORE_PERI1			0x0504
+#define DIV_PERI				0x0600
+#define DIV_STAT_PERI				0x0700
+#define EN_PCLK_PERI0				0x0800
+#define EN_PCLK_PERI1				0x0804
+#define EN_PCLK_PERI2				0x0808
+#define EN_PCLK_PERI3				0x080c
+#define EN_PCLK_PERI_SECURE_CHIPID		0x0810
+#define EN_PCLK_PERI_SECURE_PROVKEY0		0x0814
+#define EN_PCLK_PERI_SECURE_PROVKEY1		0x0818
+#define EN_PCLK_PERI_SECURE_SECKEY		0x081c
+#define EN_PCLK_PERI_SECURE_ANTIRBKCNT		0x0820
+#define EN_PCLK_PERI_SECURE_TOP_RTC		0x0824
+#define EN_PCLK_PERI_SECURE_TZPC		0x0828
+#define EN_SCLK_PERI				0x0a00
+#define EN_SCLK_PERI_SECURE_TOP_RTC		0x0a04
+#define EN_IP_PERI0				0x0b00
+#define EN_IP_PERI1				0x0b04
+#define EN_IP_PERI2				0x0b08
+#define EN_IP_PERI_SECURE_CHIPID		0x0b0c
+#define EN_IP_PERI_SECURE_PROVKEY0		0x0b10
+#define EN_IP_PERI_SECURE_PROVKEY1		0x0b14
+#define EN_IP_PERI_SECURE_SECKEY		0x0b18
+#define EN_IP_PERI_SECURE_ANTIRBKCNT		0x0b1c
+#define EN_IP_PERI_SECURE_TOP_RTC		0x0b20
+#define EN_IP_PERI_SECURE_TZPC			0x0b24
+
+/*
+*Registers for CMU_TOP
+*/
+#define DISP_PLL_LOCK				0x0000
+#define AUD_PLL_LOCK				0x0004
+#define DISP_PLL_CON0				0x0100
+#define DISP_PLL_CON1				0x0104
+#define DISP_PLL_FDET				0x0108
+#define AUD_PLL_CON0				0x0110
+#define AUD_PLL_CON1				0x0114
+#define AUD_PLL_CON2				0x0118
+#define AUD_PLL_FDET				0x011c
+#define MUX_SEL_TOP_PLL0			0x0200
+#define MUX_SEL_TOP_MFC				0x0204
+#define MUX_SEL_TOP_G2D				0x0208
+#define MUX_SEL_TOP_GSCL			0x020c
+#define MUX_SEL_TOP_ISP10			0x0214
+#define MUX_SEL_TOP_ISP11			0x0218
+#define MUX_SEL_TOP_DISP0			0x021c
+#define MUX_SEL_TOP_DISP1			0x0220
+#define MUX_SEL_TOP_BUS				0x0224
+#define MUX_SEL_TOP_PERI0			0x0228
+#define MUX_SEL_TOP_PERI1			0x022c
+#define MUX_SEL_TOP_FSYS			0x0230
+#define MUX_ENABLE_TOP_PLL0			0x0300
+#define MUX_ENABLE_TOP_MFC			0x0304
+#define MUX_ENABLE_TOP_G2D			0x0308
+#define MUX_ENABLE_TOP_GSCL			0x030c
+#define MUX_ENABLE_TOP_ISP10			0x0314
+#define MUX_ENABLE_TOP_ISP11			0x0318
+#define MUX_ENABLE_TOP_DISP0			0x031c
+#define MUX_ENABLE_TOP_DISP1			0x0320
+#define MUX_ENABLE_TOP_BUS			0x0324
+#define MUX_ENABLE_TOP_PERI0			0x0328
+#define MUX_ENABLE_TOP_PERI1			0x032c
+#define MUX_ENABLE_TOP_FSYS			0x0330
+#define MUX_STAT_TOP_PLL0			0x0400
+#define MUX_STAT_TOP_MFC			0x0404
+#define MUX_STAT_TOP_G2D			0x0408
+#define MUX_STAT_TOP_GSCL			0x040c
+#define MUX_STAT_TOP_ISP10			0x0414
+#define MUX_STAT_TOP_ISP11			0x0418
+#define MUX_STAT_TOP_DISP0			0x041c
+#define MUX_STAT_TOP_DISP1			0x0420
+#define MUX_STAT_TOP_BUS			0x0424
+#define MUX_STAT_TOP_PERI0			0x0428
+#define MUX_STAT_TOP_PERI1			0x042c
+#define MUX_STAT_TOP_FSYS			0x0430
+#define MUX_IGNORE_TOP_PLL0			0x0500
+#define MUX_IGNORE_TOP_MFC			0x0504
+#define MUX_IGNORE_TOP_G2D			0x0508
+#define MUX_IGNORE_TOP_GSCL			0x050c
+#define MUX_IGNORE_TOP_ISP10			0x0514
+#define MUX_IGNORE_TOP_ISP11			0x0518
+#define MUX_IGNORE_TOP_DISP0			0x051c
+#define MUX_IGNORE_TOP_DISP1			0x0520
+#define MUX_IGNORE_TOP_BUS			0x0524
+#define MUX_IGNORE_TOP_PERI0			0x0528
+#define MUX_IGNORE_TOP_PERI1			0x052c
+#define MUX_IGNORE_TOP_FSYS			0x0530
+#define DIV_TOP_G2D_MFC				0x0600
+#define DIV_TOP_GSCL_ISP0			0x0604
+#define DIV_TOP_ISP10				0x0608
+#define DIV_TOP_ISP11				0x060c
+#define DIV_TOP_DISP				0x0610
+#define DIV_TOP_BUS				0x0614
+#define DIV_TOP_PERI0				0x0618
+#define DIV_TOP_PERI1				0x061c
+#define DIV_TOP_PERI2				0x0620
+#define DIV_TOP_FSYS0				0x0624
+#define DIV_TOP_FSYS1				0x0628
+#define DIV_TOP_HPM				0x062c
+#define DIV_TOP_PLL_FDET			0x0630
+#define DIV_STAT_TOP_G2D_MFC			0x0700
+#define DIV_STAT_TOP_GSCL_ISP0			0x0704
+#define DIV_STAT_TOP_ISP10			0x0708
+#define DIV_STAT_TOP_ISP11			0x070c
+#define DIV_STAT_TOP_DISP			0x0710
+#define DIV_STAT_TOP_BUS			0x0714
+#define DIV_STAT_TOP_PERI0			0x0718
+#define DIV_STAT_TOP_PERI1			0x071c
+#define DIV_STAT_TOP_PERI2			0x0720
+#define DIV_STAT_TOP_FSYS0			0x0724
+#define DIV_STAT_TOP_FSYS1			0x0728
+#define DIV_STAT_TOP_HPM			0x072c
+#define DIV_STAT_TOP_PLL_FDET			0x0730
+#define EN_ACLK_TOP				0x0800
+#define EN_SCLK_TOP				0x0a00
+#define EN_IP_TOP				0x0b00
+#define CLKOUT_CMU_TOP				0x0c00
+#define CLKOUT_CMU_TOP_DIV_STAT			0x0c04
+
+#endif /*__CLK_EXYNOS5260_H */
+
diff --git a/drivers/clk/samsung/clk-exynos5410.c b/drivers/clk/samsung/clk-exynos5410.c
new file mode 100644
index 00000000000..c9505ab9ee7
--- /dev/null
+++ b/drivers/clk/samsung/clk-exynos5410.c
@@ -0,0 +1,209 @@
+/*
+ * Copyright (c) 2013 Samsung Electronics Co., Ltd.
+ * Author: Tarek Dakhran <t.dakhran@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.
+ *
+ * Common Clock Framework support for Exynos5410 SoC.
+*/
+
+#include <dt-bindings/clock/exynos5410.h>
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+
+#include "clk.h"
+
+#define APLL_LOCK               0x0
+#define APLL_CON0               0x100
+#define CPLL_LOCK               0x10020
+#define CPLL_CON0               0x10120
+#define MPLL_LOCK               0x4000
+#define MPLL_CON0               0x4100
+#define BPLL_LOCK               0x20010
+#define BPLL_CON0               0x20110
+#define KPLL_LOCK               0x28000
+#define KPLL_CON0               0x28100
+
+#define SRC_CPU			0x200
+#define DIV_CPU0		0x500
+#define SRC_CPERI1		0x4204
+#define DIV_TOP0		0x10510
+#define DIV_TOP1		0x10514
+#define DIV_FSYS1		0x1054c
+#define DIV_FSYS2		0x10550
+#define DIV_PERIC0		0x10558
+#define SRC_TOP0		0x10210
+#define SRC_TOP1		0x10214
+#define SRC_TOP2		0x10218
+#define SRC_FSYS		0x10244
+#define SRC_PERIC0		0x10250
+#define SRC_MASK_FSYS		0x10340
+#define SRC_MASK_PERIC0		0x10350
+#define GATE_BUS_FSYS0		0x10740
+#define GATE_IP_FSYS		0x10944
+#define GATE_IP_PERIC		0x10950
+#define GATE_IP_PERIS		0x10960
+#define SRC_CDREX		0x20200
+#define SRC_KFC			0x28200
+#define DIV_KFC0		0x28500
+
+/* list of PLLs */
+enum exynos5410_plls {
+	apll, cpll, mpll,
+	bpll, kpll,
+	nr_plls                 /* number of PLLs */
+};
+
+/* list of all parent clocks */
+PNAME(apll_p)		= { "fin_pll", "fout_apll", };
+PNAME(bpll_p)		= { "fin_pll", "fout_bpll", };
+PNAME(cpll_p)		= { "fin_pll", "fout_cpll" };
+PNAME(mpll_p)		= { "fin_pll", "fout_mpll", };
+PNAME(kpll_p)		= { "fin_pll", "fout_kpll", };
+
+PNAME(mout_cpu_p)	= { "mout_apll", "sclk_mpll", };
+PNAME(mout_kfc_p)	= { "mout_kpll", "sclk_mpll", };
+
+PNAME(mpll_user_p)	= { "fin_pll", "sclk_mpll", };
+PNAME(bpll_user_p)	= { "fin_pll", "sclk_bpll", };
+PNAME(mpll_bpll_p)	= { "sclk_mpll_muxed", "sclk_bpll_muxed", };
+
+PNAME(group2_p)		= { "fin_pll", "fin_pll", "none", "none",
+			"none", "none", "sclk_mpll_bpll",
+			 "none", "none", "sclk_cpll" };
+
+static struct samsung_mux_clock exynos5410_mux_clks[] __initdata = {
+	MUX(0, "mout_apll", apll_p, SRC_CPU, 0, 1),
+	MUX(0, "mout_cpu", mout_cpu_p, SRC_CPU, 16, 1),
+
+	MUX(0, "mout_kpll", kpll_p, SRC_KFC, 0, 1),
+	MUX(0, "mout_kfc", mout_kfc_p, SRC_KFC, 16, 1),
+
+	MUX(0, "sclk_mpll", mpll_p, SRC_CPERI1, 8, 1),
+	MUX(0, "sclk_mpll_muxed", mpll_user_p, SRC_TOP2, 20, 1),
+
+	MUX(0, "sclk_bpll", bpll_p, SRC_CDREX, 0, 1),
+	MUX(0, "sclk_bpll_muxed", bpll_user_p, SRC_TOP2, 24, 1),
+
+	MUX(0, "sclk_cpll", cpll_p, SRC_TOP2, 8, 1),
+
+	MUX(0, "sclk_mpll_bpll", mpll_bpll_p, SRC_TOP1, 20, 1),
+
+	MUX(0, "mout_mmc0", group2_p, SRC_FSYS, 0, 4),
+	MUX(0, "mout_mmc1", group2_p, SRC_FSYS, 4, 4),
+	MUX(0, "mout_mmc2", group2_p, SRC_FSYS, 8, 4),
+
+	MUX(0, "mout_uart0", group2_p, SRC_PERIC0, 0, 4),
+	MUX(0, "mout_uart1", group2_p, SRC_PERIC0, 4, 4),
+	MUX(0, "mout_uart2", group2_p, SRC_PERIC0, 8, 4),
+
+	MUX(0, "mout_aclk200", mpll_bpll_p, SRC_TOP0, 12, 1),
+	MUX(0, "mout_aclk400", mpll_bpll_p, SRC_TOP0, 20, 1),
+};
+
+static struct samsung_div_clock exynos5410_div_clks[] __initdata = {
+	DIV(0, "div_arm", "mout_cpu", DIV_CPU0, 0, 3),
+	DIV(0, "div_arm2", "div_arm", DIV_CPU0, 28, 3),
+
+	DIV(0, "div_acp", "div_arm2", DIV_CPU0, 8, 3),
+	DIV(0, "div_cpud", "div_arm2", DIV_CPU0, 4, 3),
+	DIV(0, "div_atb", "div_arm2", DIV_CPU0, 16, 3),
+	DIV(0, "pclk_dbg", "div_arm2", DIV_CPU0, 20, 3),
+
+	DIV(0, "div_kfc", "mout_kfc", DIV_KFC0, 0, 3),
+	DIV(0, "div_aclk", "div_kfc", DIV_KFC0, 4, 3),
+	DIV(0, "div_pclk", "div_kfc", DIV_KFC0, 20, 3),
+
+	DIV(0, "aclk66_pre", "sclk_mpll_muxed", DIV_TOP1, 24, 3),
+	DIV(0, "aclk66", "aclk66_pre", DIV_TOP0, 0, 3),
+
+	DIV(0, "div_mmc0", "mout_mmc0", DIV_FSYS1, 0, 4),
+	DIV(0, "div_mmc1", "mout_mmc1", DIV_FSYS1, 16, 4),
+	DIV(0, "div_mmc2", "mout_mmc2", DIV_FSYS2, 0, 4),
+
+	DIV_F(0, "div_mmc_pre0", "div_mmc0",
+			DIV_FSYS1, 8, 8, CLK_SET_RATE_PARENT, 0),
+	DIV_F(0, "div_mmc_pre1", "div_mmc1",
+			DIV_FSYS1, 24, 8, CLK_SET_RATE_PARENT, 0),
+	DIV_F(0, "div_mmc_pre2", "div_mmc2",
+			DIV_FSYS2, 8, 8, CLK_SET_RATE_PARENT, 0),
+
+	DIV(0, "div_uart0", "mout_uart0", DIV_PERIC0, 0, 4),
+	DIV(0, "div_uart1", "mout_uart1", DIV_PERIC0, 4, 4),
+	DIV(0, "div_uart2", "mout_uart2", DIV_PERIC0, 8, 4),
+	DIV(0, "div_uart3", "mout_uart3", DIV_PERIC0, 12, 4),
+
+	DIV(0, "aclk200", "mout_aclk200", DIV_TOP0, 12, 3),
+	DIV(0, "aclk400", "mout_aclk400", DIV_TOP0, 24, 3),
+};
+
+static struct samsung_gate_clock exynos5410_gate_clks[] __initdata = {
+	GATE(CLK_MCT, "mct", "aclk66", GATE_IP_PERIS, 18, 0, 0),
+
+	GATE(CLK_SCLK_MMC0, "sclk_mmc0", "div_mmc_pre0",
+			SRC_MASK_FSYS, 0, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_MMC1, "sclk_mmc1", "div_mmc_pre1",
+			SRC_MASK_FSYS, 4, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_MMC2, "sclk_mmc2", "div_mmc_pre2",
+			SRC_MASK_FSYS, 8, CLK_SET_RATE_PARENT, 0),
+
+	GATE(CLK_MMC0, "sdmmc0", "aclk200", GATE_BUS_FSYS0, 12, 0, 0),
+	GATE(CLK_MMC1, "sdmmc1", "aclk200", GATE_BUS_FSYS0, 13, 0, 0),
+	GATE(CLK_MMC2, "sdmmc2", "aclk200", GATE_BUS_FSYS0, 14, 0, 0),
+
+	GATE(CLK_UART0, "uart0", "aclk66", GATE_IP_PERIC, 0, 0, 0),
+	GATE(CLK_UART1, "uart1", "aclk66", GATE_IP_PERIC, 1, 0, 0),
+	GATE(CLK_UART2, "uart2", "aclk66", GATE_IP_PERIC, 2, 0, 0),
+
+	GATE(CLK_SCLK_UART0, "sclk_uart0", "div_uart0",
+			SRC_MASK_PERIC0, 0, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_UART1, "sclk_uart1", "div_uart1",
+			SRC_MASK_PERIC0, 4, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_UART2, "sclk_uart2", "div_uart2",
+			SRC_MASK_PERIC0, 8, CLK_SET_RATE_PARENT, 0),
+};
+
+static struct samsung_pll_clock exynos5410_plls[nr_plls] __initdata = {
+	[apll] = PLL(pll_35xx, CLK_FOUT_APLL, "fout_apll", "fin_pll", APLL_LOCK,
+		APLL_CON0, NULL),
+	[cpll] = PLL(pll_35xx, CLK_FOUT_CPLL, "fout_cpll", "fin_pll", CPLL_LOCK,
+		CPLL_CON0, NULL),
+	[mpll] = PLL(pll_35xx, CLK_FOUT_MPLL, "fout_mpll", "fin_pll", MPLL_LOCK,
+		MPLL_CON0, NULL),
+	[bpll] = PLL(pll_35xx, CLK_FOUT_BPLL, "fout_bpll", "fin_pll", BPLL_LOCK,
+		BPLL_CON0, NULL),
+	[kpll] = PLL(pll_35xx, CLK_FOUT_KPLL, "fout_kpll", "fin_pll", KPLL_LOCK,
+		KPLL_CON0, NULL),
+};
+
+/* register exynos5410 clocks */
+static void __init exynos5410_clk_init(struct device_node *np)
+{
+	struct samsung_clk_provider *ctx;
+	void __iomem *reg_base;
+
+	reg_base = of_iomap(np, 0);
+	if (!reg_base)
+		panic("%s: failed to map registers\n", __func__);
+
+	ctx = samsung_clk_init(np, reg_base, CLK_NR_CLKS);
+
+	samsung_clk_register_pll(ctx, exynos5410_plls,
+			ARRAY_SIZE(exynos5410_plls), reg_base);
+
+	samsung_clk_register_mux(ctx, exynos5410_mux_clks,
+			ARRAY_SIZE(exynos5410_mux_clks));
+	samsung_clk_register_div(ctx, exynos5410_div_clks,
+			ARRAY_SIZE(exynos5410_div_clks));
+	samsung_clk_register_gate(ctx, exynos5410_gate_clks,
+			ARRAY_SIZE(exynos5410_gate_clks));
+
+	pr_debug("Exynos5410: clock setup completed.\n");
+}
+CLK_OF_DECLARE(exynos5410_clk, "samsung,exynos5410-clock", exynos5410_clk_init);
diff --git a/drivers/clk/samsung/clk-exynos5420.c b/drivers/clk/samsung/clk-exynos5420.c
index 60b26819bed..9d7d7eed03f 100644
--- a/drivers/clk/samsung/clk-exynos5420.c
+++ b/drivers/clk/samsung/clk-exynos5420.c
@@ -27,18 +27,24 @@
 #define DIV_CPU1		0x504
 #define GATE_BUS_CPU		0x700
 #define GATE_SCLK_CPU		0x800
+#define CLKOUT_CMU_CPU		0xa00
+#define GATE_IP_G2D		0x8800
 #define CPLL_LOCK		0x10020
 #define DPLL_LOCK		0x10030
 #define EPLL_LOCK		0x10040
 #define RPLL_LOCK		0x10050
 #define IPLL_LOCK		0x10060
 #define SPLL_LOCK		0x10070
-#define VPLL_LOCK		0x10070
+#define VPLL_LOCK		0x10080
 #define MPLL_LOCK		0x10090
 #define CPLL_CON0		0x10120
 #define DPLL_CON0		0x10128
 #define EPLL_CON0		0x10130
+#define EPLL_CON1		0x10134
+#define EPLL_CON2		0x10138
 #define RPLL_CON0		0x10140
+#define RPLL_CON1		0x10144
+#define RPLL_CON2		0x10148
 #define IPLL_CON0		0x10150
 #define SPLL_CON0		0x10160
 #define VPLL_CON0		0x10170
@@ -51,21 +57,31 @@
 #define SRC_TOP5		0x10214
 #define SRC_TOP6		0x10218
 #define SRC_TOP7		0x1021c
+#define SRC_TOP8		0x10220 /* 5800 specific */
+#define SRC_TOP9		0x10224 /* 5800 specific */
 #define SRC_DISP10		0x1022c
 #define SRC_MAU			0x10240
 #define SRC_FSYS		0x10244
 #define SRC_PERIC0		0x10250
 #define SRC_PERIC1		0x10254
+#define SRC_ISP			0x10270
+#define SRC_CAM			0x10274 /* 5800 specific */
 #define SRC_TOP10		0x10280
 #define SRC_TOP11		0x10284
 #define SRC_TOP12		0x10288
-#define	SRC_MASK_DISP10		0x1032c
+#define SRC_TOP13		0x1028c /* 5800 specific */
+#define SRC_MASK_TOP2		0x10308
+#define SRC_MASK_TOP7		0x1031c
+#define SRC_MASK_DISP10		0x1032c
+#define SRC_MASK_MAU		0x10334
 #define SRC_MASK_FSYS		0x10340
 #define SRC_MASK_PERIC0		0x10350
 #define SRC_MASK_PERIC1		0x10354
 #define DIV_TOP0		0x10500
 #define DIV_TOP1		0x10504
 #define DIV_TOP2		0x10508
+#define DIV_TOP8		0x10520 /* 5800 specific */
+#define DIV_TOP9		0x10524 /* 5800 specific */
 #define DIV_DISP10		0x1052c
 #define DIV_MAU			0x10544
 #define DIV_FSYS0		0x10548
@@ -76,54 +92,82 @@
 #define DIV_PERIC2		0x10560
 #define DIV_PERIC3		0x10564
 #define DIV_PERIC4		0x10568
+#define DIV_CAM			0x10574 /* 5800 specific */
+#define SCLK_DIV_ISP0		0x10580
+#define SCLK_DIV_ISP1		0x10584
+#define DIV2_RATIO0		0x10590
+#define DIV4_RATIO		0x105a0
 #define GATE_BUS_TOP		0x10700
+#define GATE_BUS_GEN		0x1073c
 #define GATE_BUS_FSYS0		0x10740
+#define GATE_BUS_FSYS2		0x10748
 #define GATE_BUS_PERIC		0x10750
 #define GATE_BUS_PERIC1		0x10754
 #define GATE_BUS_PERIS0		0x10760
 #define GATE_BUS_PERIS1		0x10764
+#define GATE_BUS_NOC		0x10770
+#define GATE_TOP_SCLK_ISP	0x10870
 #define GATE_IP_GSCL0		0x10910
 #define GATE_IP_GSCL1		0x10920
+#define GATE_IP_CAM		0x10924 /* 5800 specific */
 #define GATE_IP_MFC		0x1092c
 #define GATE_IP_DISP1		0x10928
 #define GATE_IP_G3D		0x10930
 #define GATE_IP_GEN		0x10934
+#define GATE_IP_FSYS		0x10944
+#define GATE_IP_PERIC		0x10950
+#define GATE_IP_PERIS		0x10960
 #define GATE_IP_MSCL		0x10970
 #define GATE_TOP_SCLK_GSCL	0x10820
 #define GATE_TOP_SCLK_DISP1	0x10828
 #define GATE_TOP_SCLK_MAU	0x1083c
 #define GATE_TOP_SCLK_FSYS	0x10840
 #define GATE_TOP_SCLK_PERIC	0x10850
+#define TOP_SPARE2		0x10b08
 #define BPLL_LOCK		0x20010
 #define BPLL_CON0		0x20110
-#define SRC_CDREX		0x20200
 #define KPLL_LOCK		0x28000
 #define KPLL_CON0		0x28100
 #define SRC_KFC			0x28200
 #define DIV_KFC0		0x28500
 
+/* Exynos5x SoC type */
+enum exynos5x_soc {
+	EXYNOS5420,
+	EXYNOS5800,
+};
+
 /* list of PLLs */
-enum exynos5420_plls {
+enum exynos5x_plls {
 	apll, cpll, dpll, epll, rpll, ipll, spll, vpll, mpll,
 	bpll, kpll,
 	nr_plls			/* number of PLLs */
 };
 
 static void __iomem *reg_base;
+static enum exynos5x_soc exynos5x_soc;
 
 #ifdef CONFIG_PM_SLEEP
-static struct samsung_clk_reg_dump *exynos5420_save;
+static struct samsung_clk_reg_dump *exynos5x_save;
+static struct samsung_clk_reg_dump *exynos5800_save;
 
 /*
  * list of controller registers to be saved and restored during a
  * suspend/resume cycle.
  */
-static unsigned long exynos5420_clk_regs[] __initdata = {
+static unsigned long exynos5x_clk_regs[] __initdata = {
 	SRC_CPU,
 	DIV_CPU0,
 	DIV_CPU1,
 	GATE_BUS_CPU,
 	GATE_SCLK_CPU,
+	CLKOUT_CMU_CPU,
+	EPLL_CON0,
+	EPLL_CON1,
+	EPLL_CON2,
+	RPLL_CON0,
+	RPLL_CON1,
+	RPLL_CON2,
 	SRC_TOP0,
 	SRC_TOP1,
 	SRC_TOP2,
@@ -140,10 +184,13 @@ static unsigned long exynos5420_clk_regs[] __initdata = {
 	SRC_TOP10,
 	SRC_TOP11,
 	SRC_TOP12,
+	SRC_MASK_TOP2,
+	SRC_MASK_TOP7,
 	SRC_MASK_DISP10,
 	SRC_MASK_FSYS,
 	SRC_MASK_PERIC0,
 	SRC_MASK_PERIC1,
+	SRC_ISP,
 	DIV_TOP0,
 	DIV_TOP1,
 	DIV_TOP2,
@@ -157,41 +204,71 @@ static unsigned long exynos5420_clk_regs[] __initdata = {
 	DIV_PERIC2,
 	DIV_PERIC3,
 	DIV_PERIC4,
+	SCLK_DIV_ISP0,
+	SCLK_DIV_ISP1,
+	DIV2_RATIO0,
+	DIV4_RATIO,
 	GATE_BUS_TOP,
+	GATE_BUS_GEN,
 	GATE_BUS_FSYS0,
+	GATE_BUS_FSYS2,
 	GATE_BUS_PERIC,
 	GATE_BUS_PERIC1,
 	GATE_BUS_PERIS0,
 	GATE_BUS_PERIS1,
+	GATE_BUS_NOC,
+	GATE_TOP_SCLK_ISP,
 	GATE_IP_GSCL0,
 	GATE_IP_GSCL1,
 	GATE_IP_MFC,
 	GATE_IP_DISP1,
 	GATE_IP_G3D,
 	GATE_IP_GEN,
+	GATE_IP_FSYS,
+	GATE_IP_PERIC,
+	GATE_IP_PERIS,
 	GATE_IP_MSCL,
 	GATE_TOP_SCLK_GSCL,
 	GATE_TOP_SCLK_DISP1,
 	GATE_TOP_SCLK_MAU,
 	GATE_TOP_SCLK_FSYS,
 	GATE_TOP_SCLK_PERIC,
-	SRC_CDREX,
+	TOP_SPARE2,
 	SRC_KFC,
 	DIV_KFC0,
 };
 
+static unsigned long exynos5800_clk_regs[] __initdata = {
+	SRC_TOP8,
+	SRC_TOP9,
+	SRC_CAM,
+	SRC_TOP1,
+	DIV_TOP8,
+	DIV_TOP9,
+	DIV_CAM,
+	GATE_IP_CAM,
+};
+
 static int exynos5420_clk_suspend(void)
 {
-	samsung_clk_save(reg_base, exynos5420_save,
-				ARRAY_SIZE(exynos5420_clk_regs));
+	samsung_clk_save(reg_base, exynos5x_save,
+				ARRAY_SIZE(exynos5x_clk_regs));
+
+	if (exynos5x_soc == EXYNOS5800)
+		samsung_clk_save(reg_base, exynos5800_save,
+				ARRAY_SIZE(exynos5800_clk_regs));
 
 	return 0;
 }
 
 static void exynos5420_clk_resume(void)
 {
-	samsung_clk_restore(reg_base, exynos5420_save,
-				ARRAY_SIZE(exynos5420_clk_regs));
+	samsung_clk_restore(reg_base, exynos5x_save,
+				ARRAY_SIZE(exynos5x_clk_regs));
+
+	if (exynos5x_soc == EXYNOS5800)
+		samsung_clk_restore(reg_base, exynos5800_save,
+				ARRAY_SIZE(exynos5800_clk_regs));
 }
 
 static struct syscore_ops exynos5420_clk_syscore_ops = {
@@ -201,108 +278,183 @@ static struct syscore_ops exynos5420_clk_syscore_ops = {
 
 static void exynos5420_clk_sleep_init(void)
 {
-	exynos5420_save = samsung_clk_alloc_reg_dump(exynos5420_clk_regs,
-					ARRAY_SIZE(exynos5420_clk_regs));
-	if (!exynos5420_save) {
+	exynos5x_save = samsung_clk_alloc_reg_dump(exynos5x_clk_regs,
+					ARRAY_SIZE(exynos5x_clk_regs));
+	if (!exynos5x_save) {
 		pr_warn("%s: failed to allocate sleep save data, no sleep support!\n",
 			__func__);
 		return;
 	}
 
+	if (exynos5x_soc == EXYNOS5800) {
+		exynos5800_save =
+			samsung_clk_alloc_reg_dump(exynos5800_clk_regs,
+					ARRAY_SIZE(exynos5800_clk_regs));
+		if (!exynos5800_save)
+			goto err_soc;
+	}
+
 	register_syscore_ops(&exynos5420_clk_syscore_ops);
+	return;
+err_soc:
+	kfree(exynos5x_save);
+	pr_warn("%s: failed to allocate sleep save data, no sleep support!\n",
+		__func__);
+	return;
 }
 #else
 static void exynos5420_clk_sleep_init(void) {}
 #endif
 
 /* list of all parent clocks */
-PNAME(mspll_cpu_p)	= { "sclk_cpll", "sclk_dpll",
-				"sclk_mpll", "sclk_spll" };
-PNAME(cpu_p)		= { "mout_apll" , "mout_mspll_cpu" };
-PNAME(kfc_p)		= { "mout_kpll" , "mout_mspll_kfc" };
-PNAME(apll_p)		= { "fin_pll", "fout_apll", };
-PNAME(bpll_p)		= { "fin_pll", "fout_bpll", };
-PNAME(cpll_p)		= { "fin_pll", "fout_cpll", };
-PNAME(dpll_p)		= { "fin_pll", "fout_dpll", };
-PNAME(epll_p)		= { "fin_pll", "fout_epll", };
-PNAME(ipll_p)		= { "fin_pll", "fout_ipll", };
-PNAME(kpll_p)		= { "fin_pll", "fout_kpll", };
-PNAME(mpll_p)		= { "fin_pll", "fout_mpll", };
-PNAME(rpll_p)		= { "fin_pll", "fout_rpll", };
-PNAME(spll_p)		= { "fin_pll", "fout_spll", };
-PNAME(vpll_p)		= { "fin_pll", "fout_vpll", };
-
-PNAME(group1_p)		= { "sclk_cpll", "sclk_dpll", "sclk_mpll" };
-PNAME(group2_p)		= { "fin_pll", "sclk_cpll", "sclk_dpll", "sclk_mpll",
-			  "sclk_spll", "sclk_ipll", "sclk_epll", "sclk_rpll" };
-PNAME(group3_p)		= { "sclk_rpll", "sclk_spll" };
-PNAME(group4_p)		= { "sclk_ipll", "sclk_dpll", "sclk_mpll" };
-PNAME(group5_p)		= { "sclk_vpll", "sclk_dpll" };
-
-PNAME(sw_aclk66_p)	= { "dout_aclk66", "sclk_spll" };
-PNAME(aclk66_peric_p)	= { "fin_pll", "mout_sw_aclk66" };
-
-PNAME(sw_aclk200_fsys_p) = { "dout_aclk200_fsys", "sclk_spll"};
-PNAME(user_aclk200_fsys_p)	= { "fin_pll", "mout_sw_aclk200_fsys" };
-
-PNAME(sw_aclk200_fsys2_p) = { "dout_aclk200_fsys2", "sclk_spll"};
-PNAME(user_aclk200_fsys2_p)	= { "fin_pll", "mout_sw_aclk200_fsys2" };
-
-PNAME(sw_aclk200_p) = { "dout_aclk200", "sclk_spll"};
-PNAME(aclk200_disp1_p)	= { "fin_pll", "mout_sw_aclk200" };
-
-PNAME(sw_aclk400_mscl_p) = { "dout_aclk400_mscl", "sclk_spll"};
-PNAME(user_aclk400_mscl_p)	= { "fin_pll", "mout_sw_aclk400_mscl" };
-
-PNAME(sw_aclk333_p) = { "dout_aclk333", "sclk_spll"};
-PNAME(user_aclk333_p)	= { "fin_pll", "mout_sw_aclk333" };
-
-PNAME(sw_aclk166_p) = { "dout_aclk166", "sclk_spll"};
-PNAME(user_aclk166_p)	= { "fin_pll", "mout_sw_aclk166" };
-
-PNAME(sw_aclk266_p) = { "dout_aclk266", "sclk_spll"};
-PNAME(user_aclk266_p)	= { "fin_pll", "mout_sw_aclk266" };
-
-PNAME(sw_aclk333_432_gscl_p) = { "dout_aclk333_432_gscl", "sclk_spll"};
-PNAME(user_aclk333_432_gscl_p)	= { "fin_pll", "mout_sw_aclk333_432_gscl" };
-
-PNAME(sw_aclk300_gscl_p) = { "dout_aclk300_gscl", "sclk_spll"};
-PNAME(user_aclk300_gscl_p)	= { "fin_pll", "mout_sw_aclk300_gscl" };
-
-PNAME(sw_aclk300_disp1_p) = { "dout_aclk300_disp1", "sclk_spll"};
-PNAME(user_aclk300_disp1_p)	= { "fin_pll", "mout_sw_aclk300_disp1" };
-
-PNAME(sw_aclk300_jpeg_p) = { "dout_aclk300_jpeg", "sclk_spll"};
-PNAME(user_aclk300_jpeg_p)	= { "fin_pll", "mout_sw_aclk300_jpeg" };
-
-PNAME(sw_aclk_g3d_p) = { "dout_aclk_g3d", "sclk_spll"};
-PNAME(user_aclk_g3d_p)	= { "fin_pll", "mout_sw_aclk_g3d" };
-
-PNAME(sw_aclk266_g2d_p) = { "dout_aclk266_g2d", "sclk_spll"};
-PNAME(user_aclk266_g2d_p)	= { "fin_pll", "mout_sw_aclk266_g2d" };
-
-PNAME(sw_aclk333_g2d_p) = { "dout_aclk333_g2d", "sclk_spll"};
-PNAME(user_aclk333_g2d_p)	= { "fin_pll", "mout_sw_aclk333_g2d" };
-
-PNAME(audio0_p)	= { "fin_pll", "cdclk0", "sclk_dpll", "sclk_mpll",
-		  "sclk_spll", "sclk_ipll", "sclk_epll", "sclk_rpll" };
-PNAME(audio1_p)	= { "fin_pll", "cdclk1", "sclk_dpll", "sclk_mpll",
-		  "sclk_spll", "sclk_ipll", "sclk_epll", "sclk_rpll" };
-PNAME(audio2_p)	= { "fin_pll", "cdclk2", "sclk_dpll", "sclk_mpll",
-		  "sclk_spll", "sclk_ipll", "sclk_epll", "sclk_rpll" };
-PNAME(spdif_p)	= { "fin_pll", "dout_audio0", "dout_audio1", "dout_audio2",
-		  "spdif_extclk", "sclk_ipll", "sclk_epll", "sclk_rpll" };
-PNAME(hdmi_p)	= { "dout_hdmi_pixel", "sclk_hdmiphy" };
-PNAME(maudio0_p)	= { "fin_pll", "maudio_clk", "sclk_dpll", "sclk_mpll",
-			  "sclk_spll", "sclk_ipll", "sclk_epll", "sclk_rpll" };
+PNAME(mout_mspll_cpu_p) = {"mout_sclk_cpll", "mout_sclk_dpll",
+				"mout_sclk_mpll", "mout_sclk_spll"};
+PNAME(mout_cpu_p) = {"mout_apll" , "mout_mspll_cpu"};
+PNAME(mout_kfc_p) = {"mout_kpll" , "mout_mspll_kfc"};
+PNAME(mout_apll_p) = {"fin_pll", "fout_apll"};
+PNAME(mout_bpll_p) = {"fin_pll", "fout_bpll"};
+PNAME(mout_cpll_p) = {"fin_pll", "fout_cpll"};
+PNAME(mout_dpll_p) = {"fin_pll", "fout_dpll"};
+PNAME(mout_epll_p) = {"fin_pll", "fout_epll"};
+PNAME(mout_ipll_p) = {"fin_pll", "fout_ipll"};
+PNAME(mout_kpll_p) = {"fin_pll", "fout_kpll"};
+PNAME(mout_mpll_p) = {"fin_pll", "fout_mpll"};
+PNAME(mout_rpll_p) = {"fin_pll", "fout_rpll"};
+PNAME(mout_spll_p) = {"fin_pll", "fout_spll"};
+PNAME(mout_vpll_p) = {"fin_pll", "fout_vpll"};
+
+PNAME(mout_group1_p) = {"mout_sclk_cpll", "mout_sclk_dpll",
+					"mout_sclk_mpll"};
+PNAME(mout_group2_p) = {"fin_pll", "mout_sclk_cpll",
+			"mout_sclk_dpll", "mout_sclk_mpll", "mout_sclk_spll",
+			"mout_sclk_ipll", "mout_sclk_epll", "mout_sclk_rpll"};
+PNAME(mout_group3_p) = {"mout_sclk_rpll", "mout_sclk_spll"};
+PNAME(mout_group4_p) = {"mout_sclk_ipll", "mout_sclk_dpll", "mout_sclk_mpll"};
+PNAME(mout_group5_p) = {"mout_sclk_vpll", "mout_sclk_dpll"};
+
+PNAME(mout_fimd1_final_p) = {"mout_fimd1", "mout_fimd1_opt"};
+PNAME(mout_sw_aclk66_p)	= {"dout_aclk66", "mout_sclk_spll"};
+PNAME(mout_user_aclk66_peric_p)	= { "fin_pll", "mout_sw_aclk66"};
+PNAME(mout_user_pclk66_gpio_p) = {"mout_sw_aclk66", "ff_sw_aclk66"};
+
+PNAME(mout_sw_aclk200_fsys_p) = {"dout_aclk200_fsys", "mout_sclk_spll"};
+PNAME(mout_sw_pclk200_fsys_p) = {"dout_pclk200_fsys", "mout_sclk_spll"};
+PNAME(mout_user_pclk200_fsys_p)	= {"fin_pll", "mout_sw_pclk200_fsys"};
+PNAME(mout_user_aclk200_fsys_p)	= {"fin_pll", "mout_sw_aclk200_fsys"};
+
+PNAME(mout_sw_aclk200_fsys2_p) = {"dout_aclk200_fsys2", "mout_sclk_spll"};
+PNAME(mout_user_aclk200_fsys2_p) = {"fin_pll", "mout_sw_aclk200_fsys2"};
+PNAME(mout_sw_aclk100_noc_p) = {"dout_aclk100_noc", "mout_sclk_spll"};
+PNAME(mout_user_aclk100_noc_p) = {"fin_pll", "mout_sw_aclk100_noc"};
+
+PNAME(mout_sw_aclk400_wcore_p) = {"dout_aclk400_wcore", "mout_sclk_spll"};
+PNAME(mout_aclk400_wcore_bpll_p) = {"mout_aclk400_wcore", "sclk_bpll"};
+PNAME(mout_user_aclk400_wcore_p) = {"fin_pll", "mout_sw_aclk400_wcore"};
+
+PNAME(mout_sw_aclk400_isp_p) = {"dout_aclk400_isp", "mout_sclk_spll"};
+PNAME(mout_user_aclk400_isp_p) = {"fin_pll", "mout_sw_aclk400_isp"};
+
+PNAME(mout_sw_aclk333_432_isp0_p) = {"dout_aclk333_432_isp0",
+					"mout_sclk_spll"};
+PNAME(mout_user_aclk333_432_isp0_p) = {"fin_pll", "mout_sw_aclk333_432_isp0"};
+
+PNAME(mout_sw_aclk333_432_isp_p) = {"dout_aclk333_432_isp", "mout_sclk_spll"};
+PNAME(mout_user_aclk333_432_isp_p) = {"fin_pll", "mout_sw_aclk333_432_isp"};
+
+PNAME(mout_sw_aclk200_p) = {"dout_aclk200", "mout_sclk_spll"};
+PNAME(mout_user_aclk200_disp1_p) = {"fin_pll", "mout_sw_aclk200"};
+
+PNAME(mout_sw_aclk400_mscl_p) = {"dout_aclk400_mscl", "mout_sclk_spll"};
+PNAME(mout_user_aclk400_mscl_p)	= {"fin_pll", "mout_sw_aclk400_mscl"};
+
+PNAME(mout_sw_aclk333_p) = {"dout_aclk333", "mout_sclk_spll"};
+PNAME(mout_user_aclk333_p) = {"fin_pll", "mout_sw_aclk333"};
+
+PNAME(mout_sw_aclk166_p) = {"dout_aclk166", "mout_sclk_spll"};
+PNAME(mout_user_aclk166_p) = {"fin_pll", "mout_sw_aclk166"};
+
+PNAME(mout_sw_aclk266_p) = {"dout_aclk266", "mout_sclk_spll"};
+PNAME(mout_user_aclk266_p) = {"fin_pll", "mout_sw_aclk266"};
+PNAME(mout_user_aclk266_isp_p) = {"fin_pll", "mout_sw_aclk266"};
+
+PNAME(mout_sw_aclk333_432_gscl_p) = {"dout_aclk333_432_gscl", "mout_sclk_spll"};
+PNAME(mout_user_aclk333_432_gscl_p) = {"fin_pll", "mout_sw_aclk333_432_gscl"};
+
+PNAME(mout_sw_aclk300_gscl_p) = {"dout_aclk300_gscl", "mout_sclk_spll"};
+PNAME(mout_user_aclk300_gscl_p)	= {"fin_pll", "mout_sw_aclk300_gscl"};
+
+PNAME(mout_sw_aclk300_disp1_p) = {"dout_aclk300_disp1", "mout_sclk_spll"};
+PNAME(mout_sw_aclk400_disp1_p) = {"dout_aclk400_disp1", "mout_sclk_spll"};
+PNAME(mout_user_aclk300_disp1_p) = {"fin_pll", "mout_sw_aclk300_disp1"};
+PNAME(mout_user_aclk400_disp1_p) = {"fin_pll", "mout_sw_aclk400_disp1"};
+
+PNAME(mout_sw_aclk300_jpeg_p) = {"dout_aclk300_jpeg", "mout_sclk_spll"};
+PNAME(mout_user_aclk300_jpeg_p) = {"fin_pll", "mout_sw_aclk300_jpeg"};
+
+PNAME(mout_sw_aclk_g3d_p) = {"dout_aclk_g3d", "mout_sclk_spll"};
+PNAME(mout_user_aclk_g3d_p) = {"fin_pll", "mout_sw_aclk_g3d"};
+
+PNAME(mout_sw_aclk266_g2d_p) = {"dout_aclk266_g2d", "mout_sclk_spll"};
+PNAME(mout_user_aclk266_g2d_p) = {"fin_pll", "mout_sw_aclk266_g2d"};
+
+PNAME(mout_sw_aclk333_g2d_p) = {"dout_aclk333_g2d", "mout_sclk_spll"};
+PNAME(mout_user_aclk333_g2d_p) = {"fin_pll", "mout_sw_aclk333_g2d"};
+
+PNAME(mout_audio0_p) = {"fin_pll", "cdclk0", "mout_sclk_dpll",
+			"mout_sclk_mpll", "mout_sclk_spll", "mout_sclk_ipll",
+			"mout_sclk_epll", "mout_sclk_rpll"};
+PNAME(mout_audio1_p) = {"fin_pll", "cdclk1", "mout_sclk_dpll",
+			"mout_sclk_mpll", "mout_sclk_spll", "mout_sclk_ipll",
+			"mout_sclk_epll", "mout_sclk_rpll"};
+PNAME(mout_audio2_p) = {"fin_pll", "cdclk2", "mout_sclk_dpll",
+			"mout_sclk_mpll", "mout_sclk_spll", "mout_sclk_ipll",
+			"mout_sclk_epll", "mout_sclk_rpll"};
+PNAME(mout_spdif_p) = {"fin_pll", "dout_audio0", "dout_audio1",
+			"dout_audio2", "spdif_extclk", "mout_sclk_ipll",
+			"mout_sclk_epll", "mout_sclk_rpll"};
+PNAME(mout_hdmi_p) = {"dout_hdmi_pixel", "sclk_hdmiphy"};
+PNAME(mout_maudio0_p) = {"fin_pll", "maudio_clk", "mout_sclk_dpll",
+			 "mout_sclk_mpll", "mout_sclk_spll", "mout_sclk_ipll",
+			 "mout_sclk_epll", "mout_sclk_rpll"};
+PNAME(mout_mau_epll_clk_p) = {"mout_sclk_epll", "mout_sclk_dpll",
+				"mout_sclk_mpll", "mout_sclk_spll"};
+/* List of parents specific to exynos5800 */
+PNAME(mout_epll2_5800_p)	= { "mout_sclk_epll", "ff_dout_epll2" };
+PNAME(mout_group1_5800_p)	= { "mout_sclk_cpll", "mout_sclk_dpll",
+				"mout_sclk_mpll", "ff_dout_spll2" };
+PNAME(mout_group2_5800_p)	= { "mout_sclk_cpll", "mout_sclk_dpll",
+					"mout_sclk_mpll", "ff_dout_spll2",
+					"mout_epll2", "mout_sclk_ipll" };
+PNAME(mout_group3_5800_p)	= { "mout_sclk_cpll", "mout_sclk_dpll",
+					"mout_sclk_mpll", "ff_dout_spll2",
+					"mout_epll2" };
+PNAME(mout_group5_5800_p)	= { "mout_sclk_cpll", "mout_sclk_dpll",
+					"mout_sclk_mpll", "mout_sclk_spll" };
+PNAME(mout_group6_5800_p)	= { "mout_sclk_ipll", "mout_sclk_dpll",
+				"mout_sclk_mpll", "ff_dout_spll2" };
+PNAME(mout_group7_5800_p)	= { "mout_sclk_cpll", "mout_sclk_dpll",
+					"mout_sclk_mpll", "mout_sclk_spll",
+					"mout_epll2", "mout_sclk_ipll" };
+PNAME(mout_mau_epll_clk_5800_p)	= { "mout_sclk_epll", "mout_sclk_dpll",
+					"mout_sclk_mpll",
+					"ff_dout_spll2" };
+PNAME(mout_group8_5800_p)	= { "dout_aclk432_scaler", "dout_sclk_sw" };
+PNAME(mout_group9_5800_p)	= { "dout_osc_div", "mout_sw_aclk432_scaler" };
+PNAME(mout_group10_5800_p)	= { "dout_aclk432_cam", "dout_sclk_sw" };
+PNAME(mout_group11_5800_p)	= { "dout_osc_div", "mout_sw_aclk432_cam" };
+PNAME(mout_group12_5800_p)	= { "dout_aclkfl1_550_cam", "dout_sclk_sw" };
+PNAME(mout_group13_5800_p)	= { "dout_osc_div", "mout_sw_aclkfl1_550_cam" };
+PNAME(mout_group14_5800_p)	= { "dout_aclk550_cam", "dout_sclk_sw" };
+PNAME(mout_group15_5800_p)	= { "dout_osc_div", "mout_sw_aclk550_cam" };
 
 /* fixed rate clocks generated outside the soc */
-static struct samsung_fixed_rate_clock exynos5420_fixed_rate_ext_clks[] __initdata = {
+static struct samsung_fixed_rate_clock
+		exynos5x_fixed_rate_ext_clks[] __initdata = {
 	FRATE(CLK_FIN_PLL, "fin_pll", NULL, CLK_IS_ROOT, 0),
 };
 
 /* fixed rate clocks generated inside the soc */
-static struct samsung_fixed_rate_clock exynos5420_fixed_rate_clks[] __initdata = {
+static struct samsung_fixed_rate_clock exynos5x_fixed_rate_clks[] __initdata = {
 	FRATE(CLK_SCLK_HDMIPHY, "sclk_hdmiphy", NULL, CLK_IS_ROOT, 24000000),
 	FRATE(0, "sclk_pwi", NULL, CLK_IS_ROOT, 24000000),
 	FRATE(0, "sclk_usbh20", NULL, CLK_IS_ROOT, 48000000),
@@ -310,146 +462,309 @@ static struct samsung_fixed_rate_clock exynos5420_fixed_rate_clks[] __initdata =
 	FRATE(0, "sclk_usbh20_scan_clk", NULL, CLK_IS_ROOT, 480000000),
 };
 
-static struct samsung_fixed_factor_clock exynos5420_fixed_factor_clks[] __initdata = {
-	FFACTOR(0, "sclk_hsic_12m", "fin_pll", 1, 2, 0),
+static struct samsung_fixed_factor_clock
+		exynos5x_fixed_factor_clks[] __initdata = {
+	FFACTOR(0, "ff_hsic_12m", "fin_pll", 1, 2, 0),
+	FFACTOR(0, "ff_sw_aclk66", "mout_sw_aclk66", 1, 2, 0),
 };
 
-static struct samsung_mux_clock exynos5420_mux_clks[] __initdata = {
-	MUX(0, "mout_mspll_kfc", mspll_cpu_p, SRC_TOP7, 8, 2),
-	MUX(0, "mout_mspll_cpu", mspll_cpu_p, SRC_TOP7, 12, 2),
-	MUX(0, "mout_apll", apll_p, SRC_CPU, 0, 1),
-	MUX(0, "mout_cpu", cpu_p, SRC_CPU, 16, 1),
-	MUX(0, "mout_kpll", kpll_p, SRC_KFC, 0, 1),
-	MUX(0, "mout_cpu_kfc", kfc_p, SRC_KFC, 16, 1),
-
-	MUX(0, "sclk_bpll", bpll_p, SRC_CDREX, 0, 1),
-
-	MUX_A(0, "mout_aclk400_mscl", group1_p,
-			SRC_TOP0, 4, 2, "aclk400_mscl"),
-	MUX(0, "mout_aclk200", group1_p, SRC_TOP0, 8, 2),
-	MUX(0, "mout_aclk200_fsys2", group1_p, SRC_TOP0, 12, 2),
-	MUX(0, "mout_aclk200_fsys", group1_p, SRC_TOP0, 28, 2),
-
-	MUX(0, "mout_aclk333_432_gscl", group4_p, SRC_TOP1, 0, 2),
-	MUX(0, "mout_aclk66", group1_p, SRC_TOP1, 8, 2),
-	MUX(0, "mout_aclk266", group1_p, SRC_TOP1, 20, 2),
-	MUX(0, "mout_aclk166", group1_p, SRC_TOP1, 24, 2),
-	MUX(0, "mout_aclk333", group1_p, SRC_TOP1, 28, 2),
-
-	MUX(0, "mout_aclk333_g2d", group1_p, SRC_TOP2, 8, 2),
-	MUX(0, "mout_aclk266_g2d", group1_p, SRC_TOP2, 12, 2),
-	MUX(0, "mout_aclk_g3d", group5_p, SRC_TOP2, 16, 1),
-	MUX(0, "mout_aclk300_jpeg", group1_p, SRC_TOP2, 20, 2),
-	MUX(0, "mout_aclk300_disp1", group1_p, SRC_TOP2, 24, 2),
-	MUX(0, "mout_aclk300_gscl", group1_p, SRC_TOP2, 28, 2),
-
-	MUX(0, "mout_user_aclk400_mscl", user_aclk400_mscl_p,
+static struct samsung_fixed_factor_clock
+		exynos5800_fixed_factor_clks[] __initdata = {
+	FFACTOR(0, "ff_dout_epll2", "mout_sclk_epll", 1, 2, 0),
+	FFACTOR(0, "ff_dout_spll2", "mout_sclk_spll", 1, 2, 0),
+};
+
+struct samsung_mux_clock exynos5800_mux_clks[] __initdata = {
+	MUX(0, "mout_aclk400_isp", mout_group3_5800_p, SRC_TOP0, 0, 3),
+	MUX(0, "mout_aclk400_mscl", mout_group3_5800_p, SRC_TOP0, 4, 3),
+	MUX(0, "mout_aclk400_wcore", mout_group2_5800_p, SRC_TOP0, 16, 3),
+	MUX(0, "mout_aclk100_noc", mout_group1_5800_p, SRC_TOP0, 20, 2),
+
+	MUX(0, "mout_aclk333_432_gscl", mout_group6_5800_p, SRC_TOP1, 0, 2),
+	MUX(0, "mout_aclk333_432_isp", mout_group6_5800_p, SRC_TOP1, 4, 2),
+	MUX(0, "mout_aclk333_432_isp0", mout_group6_5800_p, SRC_TOP1, 12, 2),
+	MUX(0, "mout_aclk266", mout_group5_5800_p, SRC_TOP1, 20, 2),
+	MUX(0, "mout_aclk333", mout_group1_5800_p, SRC_TOP1, 28, 2),
+
+	MUX(0, "mout_aclk400_disp1", mout_group7_5800_p, SRC_TOP2, 4, 3),
+	MUX(0, "mout_aclk333_g2d", mout_group5_5800_p, SRC_TOP2, 8, 2),
+	MUX(0, "mout_aclk266_g2d", mout_group5_5800_p, SRC_TOP2, 12, 2),
+	MUX(0, "mout_aclk300_jpeg", mout_group5_5800_p, SRC_TOP2, 20, 2),
+	MUX(0, "mout_aclk300_disp1", mout_group5_5800_p, SRC_TOP2, 24, 2),
+	MUX(0, "mout_aclk300_gscl", mout_group5_5800_p, SRC_TOP2, 28, 2),
+
+	MUX(0, "mout_mau_epll_clk", mout_mau_epll_clk_5800_p, SRC_TOP7,
+			20, 2),
+	MUX(0, "sclk_bpll", mout_bpll_p, SRC_TOP7, 24, 1),
+	MUX(0, "mout_epll2", mout_epll2_5800_p, SRC_TOP7, 28, 1),
+
+	MUX(0, "mout_aclk550_cam", mout_group3_5800_p, SRC_TOP8, 16, 3),
+	MUX(0, "mout_aclkfl1_550_cam", mout_group3_5800_p, SRC_TOP8, 20, 3),
+	MUX(0, "mout_aclk432_cam", mout_group6_5800_p, SRC_TOP8, 24, 2),
+	MUX(0, "mout_aclk432_scaler", mout_group6_5800_p, SRC_TOP8, 28, 2),
+
+	MUX(0, "mout_user_aclk550_cam", mout_group15_5800_p,
+							SRC_TOP9, 16, 1),
+	MUX(0, "mout_user_aclkfl1_550_cam", mout_group13_5800_p,
+							SRC_TOP9, 20, 1),
+	MUX(0, "mout_user_aclk432_cam", mout_group11_5800_p,
+							SRC_TOP9, 24, 1),
+	MUX(0, "mout_user_aclk432_scaler", mout_group9_5800_p,
+							SRC_TOP9, 28, 1),
+
+	MUX(0, "mout_sw_aclk550_cam", mout_group14_5800_p, SRC_TOP13, 16, 1),
+	MUX(0, "mout_sw_aclkfl1_550_cam", mout_group12_5800_p,
+							SRC_TOP13, 20, 1),
+	MUX(0, "mout_sw_aclk432_cam", mout_group10_5800_p,
+							SRC_TOP13, 24, 1),
+	MUX(0, "mout_sw_aclk432_scaler", mout_group8_5800_p,
+							SRC_TOP13, 28, 1),
+
+	MUX(0, "mout_fimd1", mout_group2_p, SRC_DISP10, 4, 3),
+};
+
+struct samsung_div_clock exynos5800_div_clks[] __initdata = {
+	DIV(0, "dout_aclk400_wcore", "mout_aclk400_wcore", DIV_TOP0, 16, 3),
+
+	DIV(0, "dout_aclk550_cam", "mout_aclk550_cam",
+				DIV_TOP8, 16, 3),
+	DIV(0, "dout_aclkfl1_550_cam", "mout_aclkfl1_550_cam",
+				DIV_TOP8, 20, 3),
+	DIV(0, "dout_aclk432_cam", "mout_aclk432_cam",
+				DIV_TOP8, 24, 3),
+	DIV(0, "dout_aclk432_scaler", "mout_aclk432_scaler",
+				DIV_TOP8, 28, 3),
+
+	DIV(0, "dout_osc_div", "fin_pll", DIV_TOP9, 20, 3),
+	DIV(0, "dout_sclk_sw", "sclk_spll", DIV_TOP9, 24, 6),
+};
+
+struct samsung_gate_clock exynos5800_gate_clks[] __initdata = {
+	GATE(CLK_ACLK550_CAM, "aclk550_cam", "mout_user_aclk550_cam",
+				GATE_BUS_TOP, 24, 0, 0),
+	GATE(CLK_ACLK432_SCALER, "aclk432_scaler", "mout_user_aclk432_scaler",
+				GATE_BUS_TOP, 27, 0, 0),
+};
+
+struct samsung_mux_clock exynos5420_mux_clks[] __initdata = {
+	MUX(0, "sclk_bpll", mout_bpll_p, TOP_SPARE2, 0, 1),
+	MUX(0, "mout_aclk400_wcore_bpll", mout_aclk400_wcore_bpll_p,
+				TOP_SPARE2, 4, 1),
+
+	MUX(0, "mout_aclk400_isp", mout_group1_p, SRC_TOP0, 0, 2),
+	MUX_A(0, "mout_aclk400_mscl", mout_group1_p,
+				SRC_TOP0, 4, 2, "aclk400_mscl"),
+	MUX(0, "mout_aclk400_wcore", mout_group1_p, SRC_TOP0, 16, 2),
+	MUX(0, "mout_aclk100_noc", mout_group1_p, SRC_TOP0, 20, 2),
+
+	MUX(0, "mout_aclk333_432_gscl", mout_group4_p, SRC_TOP1, 0, 2),
+	MUX(0, "mout_aclk333_432_isp", mout_group4_p,
+				SRC_TOP1, 4, 2),
+	MUX(0, "mout_aclk333_432_isp0", mout_group4_p, SRC_TOP1, 12, 2),
+	MUX(0, "mout_aclk266", mout_group1_p, SRC_TOP1, 20, 2),
+	MUX(0, "mout_aclk333", mout_group1_p, SRC_TOP1, 28, 2),
+
+	MUX(0, "mout_aclk400_disp1", mout_group1_p, SRC_TOP2, 4, 2),
+	MUX(0, "mout_aclk333_g2d", mout_group1_p, SRC_TOP2, 8, 2),
+	MUX(0, "mout_aclk266_g2d", mout_group1_p, SRC_TOP2, 12, 2),
+	MUX(0, "mout_aclk300_jpeg", mout_group1_p, SRC_TOP2, 20, 2),
+	MUX(0, "mout_aclk300_disp1", mout_group1_p, SRC_TOP2, 24, 2),
+	MUX(0, "mout_aclk300_gscl", mout_group1_p, SRC_TOP2, 28, 2),
+
+	MUX(0, "mout_mau_epll_clk", mout_mau_epll_clk_p, SRC_TOP7, 20, 2),
+
+	MUX(0, "mout_fimd1", mout_group3_p, SRC_DISP10, 4, 1),
+};
+
+struct samsung_div_clock exynos5420_div_clks[] __initdata = {
+	DIV(0, "dout_aclk400_wcore", "mout_aclk400_wcore_bpll",
+			DIV_TOP0, 16, 3),
+};
+
+static struct samsung_mux_clock exynos5x_mux_clks[] __initdata = {
+	MUX(0, "mout_user_pclk66_gpio", mout_user_pclk66_gpio_p,
+			SRC_TOP7, 4, 1),
+	MUX(0, "mout_mspll_kfc", mout_mspll_cpu_p, SRC_TOP7, 8, 2),
+	MUX(0, "mout_mspll_cpu", mout_mspll_cpu_p, SRC_TOP7, 12, 2),
+
+	MUX(0, "mout_apll", mout_apll_p, SRC_CPU, 0, 1),
+	MUX(0, "mout_cpu", mout_cpu_p, SRC_CPU, 16, 1),
+	MUX(0, "mout_kpll", mout_kpll_p, SRC_KFC, 0, 1),
+	MUX(0, "mout_kfc", mout_kfc_p, SRC_KFC, 16, 1),
+
+	MUX(0, "mout_aclk200", mout_group1_p, SRC_TOP0, 8, 2),
+	MUX(0, "mout_aclk200_fsys2", mout_group1_p, SRC_TOP0, 12, 2),
+	MUX(0, "mout_pclk200_fsys", mout_group1_p, SRC_TOP0, 24, 2),
+	MUX(0, "mout_aclk200_fsys", mout_group1_p, SRC_TOP0, 28, 2),
+
+	MUX(0, "mout_aclk66", mout_group1_p, SRC_TOP1, 8, 2),
+	MUX(0, "mout_aclk166", mout_group1_p, SRC_TOP1, 24, 2),
+
+	MUX(0, "mout_aclk_g3d", mout_group5_p, SRC_TOP2, 16, 1),
+
+	MUX(0, "mout_user_aclk400_isp", mout_user_aclk400_isp_p,
+			SRC_TOP3, 0, 1),
+	MUX(0, "mout_user_aclk400_mscl", mout_user_aclk400_mscl_p,
 			SRC_TOP3, 4, 1),
-	MUX_A(0, "mout_aclk200_disp1", aclk200_disp1_p,
-			SRC_TOP3, 8, 1, "aclk200_disp1"),
-	MUX(0, "mout_user_aclk200_fsys2", user_aclk200_fsys2_p,
+	MUX(0, "mout_user_aclk200_disp1", mout_user_aclk200_disp1_p,
+			SRC_TOP3, 8, 1),
+	MUX(0, "mout_user_aclk200_fsys2", mout_user_aclk200_fsys2_p,
 			SRC_TOP3, 12, 1),
-	MUX(0, "mout_user_aclk200_fsys", user_aclk200_fsys_p,
+	MUX(0, "mout_user_aclk400_wcore", mout_user_aclk400_wcore_p,
+			SRC_TOP3, 16, 1),
+	MUX(0, "mout_user_aclk100_noc", mout_user_aclk100_noc_p,
+			SRC_TOP3, 20, 1),
+	MUX(0, "mout_user_pclk200_fsys", mout_user_pclk200_fsys_p,
+			SRC_TOP3, 24, 1),
+	MUX(0, "mout_user_aclk200_fsys", mout_user_aclk200_fsys_p,
 			SRC_TOP3, 28, 1),
 
-	MUX(0, "mout_user_aclk333_432_gscl", user_aclk333_432_gscl_p,
+	MUX(0, "mout_user_aclk333_432_gscl", mout_user_aclk333_432_gscl_p,
 			SRC_TOP4, 0, 1),
-	MUX(0, "mout_aclk66_peric", aclk66_peric_p, SRC_TOP4, 8, 1),
-	MUX(0, "mout_user_aclk266", user_aclk266_p, SRC_TOP4, 20, 1),
-	MUX(0, "mout_user_aclk166", user_aclk166_p, SRC_TOP4, 24, 1),
-	MUX(0, "mout_user_aclk333", user_aclk333_p, SRC_TOP4, 28, 1),
-
-	MUX(0, "mout_aclk66_psgen", aclk66_peric_p, SRC_TOP5, 4, 1),
-	MUX(0, "mout_user_aclk333_g2d", user_aclk333_g2d_p, SRC_TOP5, 8, 1),
-	MUX(0, "mout_user_aclk266_g2d", user_aclk266_g2d_p, SRC_TOP5, 12, 1),
-	MUX_A(0, "mout_user_aclk_g3d", user_aclk_g3d_p,
-			SRC_TOP5, 16, 1, "aclkg3d"),
-	MUX(0, "mout_user_aclk300_jpeg", user_aclk300_jpeg_p,
+	MUX(0, "mout_user_aclk333_432_isp", mout_user_aclk333_432_isp_p,
+			SRC_TOP4, 4, 1),
+	MUX(0, "mout_user_aclk66_peric", mout_user_aclk66_peric_p,
+			SRC_TOP4, 8, 1),
+	MUX(0, "mout_user_aclk333_432_isp0", mout_user_aclk333_432_isp0_p,
+			SRC_TOP4, 12, 1),
+	MUX(0, "mout_user_aclk266_isp", mout_user_aclk266_isp_p,
+			SRC_TOP4, 16, 1),
+	MUX(0, "mout_user_aclk266", mout_user_aclk266_p, SRC_TOP4, 20, 1),
+	MUX(0, "mout_user_aclk166", mout_user_aclk166_p, SRC_TOP4, 24, 1),
+	MUX(0, "mout_user_aclk333", mout_user_aclk333_p, SRC_TOP4, 28, 1),
+
+	MUX(0, "mout_user_aclk400_disp1", mout_user_aclk400_disp1_p,
+			SRC_TOP5, 0, 1),
+	MUX(0, "mout_user_aclk66_psgen", mout_user_aclk66_peric_p,
+			SRC_TOP5, 4, 1),
+	MUX(0, "mout_user_aclk333_g2d", mout_user_aclk333_g2d_p,
+			SRC_TOP5, 8, 1),
+	MUX(0, "mout_user_aclk266_g2d", mout_user_aclk266_g2d_p,
+			SRC_TOP5, 12, 1),
+	MUX(CLK_MOUT_G3D, "mout_user_aclk_g3d", mout_user_aclk_g3d_p,
+			SRC_TOP5, 16, 1),
+	MUX(0, "mout_user_aclk300_jpeg", mout_user_aclk300_jpeg_p,
 			SRC_TOP5, 20, 1),
-	MUX(0, "mout_user_aclk300_disp1", user_aclk300_disp1_p,
+	MUX(0, "mout_user_aclk300_disp1", mout_user_aclk300_disp1_p,
 			SRC_TOP5, 24, 1),
-	MUX(0, "mout_user_aclk300_gscl", user_aclk300_gscl_p,
+	MUX(0, "mout_user_aclk300_gscl", mout_user_aclk300_gscl_p,
 			SRC_TOP5, 28, 1),
 
-	MUX(0, "sclk_mpll", mpll_p, SRC_TOP6, 0, 1),
-	MUX(0, "sclk_vpll", vpll_p, SRC_TOP6, 4, 1),
-	MUX(0, "sclk_spll", spll_p, SRC_TOP6, 8, 1),
-	MUX(0, "sclk_ipll", ipll_p, SRC_TOP6, 12, 1),
-	MUX(0, "sclk_rpll", rpll_p, SRC_TOP6, 16, 1),
-	MUX(0, "sclk_epll", epll_p, SRC_TOP6, 20, 1),
-	MUX(0, "sclk_dpll", dpll_p, SRC_TOP6, 24, 1),
-	MUX(0, "sclk_cpll", cpll_p, SRC_TOP6, 28, 1),
-
-	MUX(0, "mout_sw_aclk400_mscl", sw_aclk400_mscl_p, SRC_TOP10, 4, 1),
-	MUX(0, "mout_sw_aclk200", sw_aclk200_p, SRC_TOP10, 8, 1),
-	MUX(0, "mout_sw_aclk200_fsys2", sw_aclk200_fsys2_p,
+	MUX(0, "mout_sclk_mpll", mout_mpll_p, SRC_TOP6, 0, 1),
+	MUX(CLK_MOUT_VPLL, "mout_sclk_vpll", mout_vpll_p, SRC_TOP6, 4, 1),
+	MUX(0, "mout_sclk_spll", mout_spll_p, SRC_TOP6, 8, 1),
+	MUX(0, "mout_sclk_ipll", mout_ipll_p, SRC_TOP6, 12, 1),
+	MUX(0, "mout_sclk_rpll", mout_rpll_p, SRC_TOP6, 16, 1),
+	MUX(0, "mout_sclk_epll", mout_epll_p, SRC_TOP6, 20, 1),
+	MUX(0, "mout_sclk_dpll", mout_dpll_p, SRC_TOP6, 24, 1),
+	MUX(0, "mout_sclk_cpll", mout_cpll_p, SRC_TOP6, 28, 1),
+
+	MUX(0, "mout_sw_aclk400_isp", mout_sw_aclk400_isp_p,
+			SRC_TOP10, 0, 1),
+	MUX(0, "mout_sw_aclk400_mscl", mout_sw_aclk400_mscl_p,
+			SRC_TOP10, 4, 1),
+	MUX(0, "mout_sw_aclk200", mout_sw_aclk200_p, SRC_TOP10, 8, 1),
+	MUX(0, "mout_sw_aclk200_fsys2", mout_sw_aclk200_fsys2_p,
 			SRC_TOP10, 12, 1),
-	MUX(0, "mout_sw_aclk200_fsys", sw_aclk200_fsys_p, SRC_TOP10, 28, 1),
-
-	MUX(0, "mout_sw_aclk333_432_gscl", sw_aclk333_432_gscl_p,
+	MUX(0, "mout_sw_aclk400_wcore", mout_sw_aclk400_wcore_p,
+			SRC_TOP10, 16, 1),
+	MUX(0, "mout_sw_aclk100_noc", mout_sw_aclk100_noc_p,
+			SRC_TOP10, 20, 1),
+	MUX(0, "mout_sw_pclk200_fsys", mout_sw_pclk200_fsys_p,
+			SRC_TOP10, 24, 1),
+	MUX(0, "mout_sw_aclk200_fsys", mout_sw_aclk200_fsys_p,
+			SRC_TOP10, 28, 1),
+
+	MUX(0, "mout_sw_aclk333_432_gscl", mout_sw_aclk333_432_gscl_p,
 			SRC_TOP11, 0, 1),
-	MUX(0, "mout_sw_aclk66", sw_aclk66_p, SRC_TOP11, 8, 1),
-	MUX(0, "mout_sw_aclk266", sw_aclk266_p, SRC_TOP11, 20, 1),
-	MUX(0, "mout_sw_aclk166", sw_aclk166_p, SRC_TOP11, 24, 1),
-	MUX(0, "mout_sw_aclk333", sw_aclk333_p, SRC_TOP11, 28, 1),
-
-	MUX(0, "mout_sw_aclk333_g2d", sw_aclk333_g2d_p, SRC_TOP12, 8, 1),
-	MUX(0, "mout_sw_aclk266_g2d", sw_aclk266_g2d_p, SRC_TOP12, 12, 1),
-	MUX(0, "mout_sw_aclk_g3d", sw_aclk_g3d_p, SRC_TOP12, 16, 1),
-	MUX(0, "mout_sw_aclk300_jpeg", sw_aclk300_jpeg_p, SRC_TOP12, 20, 1),
-	MUX(0, "mout_sw_aclk300_disp1", sw_aclk300_disp1_p,
+	MUX(0, "mout_sw_aclk333_432_isp", mout_sw_aclk333_432_isp_p,
+			SRC_TOP11, 4, 1),
+	MUX(0, "mout_sw_aclk66", mout_sw_aclk66_p, SRC_TOP11, 8, 1),
+	MUX(0, "mout_sw_aclk333_432_isp0", mout_sw_aclk333_432_isp0_p,
+			SRC_TOP11, 12, 1),
+	MUX(0, "mout_sw_aclk266", mout_sw_aclk266_p, SRC_TOP11, 20, 1),
+	MUX(0, "mout_sw_aclk166", mout_sw_aclk166_p, SRC_TOP11, 24, 1),
+	MUX(0, "mout_sw_aclk333", mout_sw_aclk333_p, SRC_TOP11, 28, 1),
+
+	MUX(0, "mout_sw_aclk400_disp1", mout_sw_aclk400_disp1_p,
+			SRC_TOP12, 4, 1),
+	MUX(0, "mout_sw_aclk333_g2d", mout_sw_aclk333_g2d_p,
+			SRC_TOP12, 8, 1),
+	MUX(0, "mout_sw_aclk266_g2d", mout_sw_aclk266_g2d_p,
+			SRC_TOP12, 12, 1),
+	MUX(0, "mout_sw_aclk_g3d", mout_sw_aclk_g3d_p, SRC_TOP12, 16, 1),
+	MUX(0, "mout_sw_aclk300_jpeg", mout_sw_aclk300_jpeg_p,
+			SRC_TOP12, 20, 1),
+	MUX(0, "mout_sw_aclk300_disp1", mout_sw_aclk300_disp1_p,
 			SRC_TOP12, 24, 1),
-	MUX(0, "mout_sw_aclk300_gscl", sw_aclk300_gscl_p, SRC_TOP12, 28, 1),
+	MUX(0, "mout_sw_aclk300_gscl", mout_sw_aclk300_gscl_p,
+			SRC_TOP12, 28, 1),
 
 	/* DISP1 Block */
-	MUX(0, "mout_fimd1", group3_p, SRC_DISP10, 4, 1),
-	MUX(0, "mout_mipi1", group2_p, SRC_DISP10, 16, 3),
-	MUX(0, "mout_dp1", group2_p, SRC_DISP10, 20, 3),
-	MUX(0, "mout_pixel", group2_p, SRC_DISP10, 24, 3),
-	MUX(CLK_MOUT_HDMI, "mout_hdmi", hdmi_p, SRC_DISP10, 28, 1),
+	MUX(0, "mout_mipi1", mout_group2_p, SRC_DISP10, 16, 3),
+	MUX(0, "mout_dp1", mout_group2_p, SRC_DISP10, 20, 3),
+	MUX(0, "mout_pixel", mout_group2_p, SRC_DISP10, 24, 3),
+	MUX(CLK_MOUT_HDMI, "mout_hdmi", mout_hdmi_p, SRC_DISP10, 28, 1),
+	MUX(0, "mout_fimd1_opt", mout_group2_p, SRC_DISP10, 8, 3),
+
+	MUX(0, "mout_fimd1_final", mout_fimd1_final_p, TOP_SPARE2, 8, 1),
 
 	/* MAU Block */
-	MUX(0, "mout_maudio0", maudio0_p, SRC_MAU, 28, 3),
+	MUX(CLK_MOUT_MAUDIO0, "mout_maudio0", mout_maudio0_p, SRC_MAU, 28, 3),
 
 	/* FSYS Block */
-	MUX(0, "mout_usbd301", group2_p, SRC_FSYS, 4, 3),
-	MUX(0, "mout_mmc0", group2_p, SRC_FSYS, 8, 3),
-	MUX(0, "mout_mmc1", group2_p, SRC_FSYS, 12, 3),
-	MUX(0, "mout_mmc2", group2_p, SRC_FSYS, 16, 3),
-	MUX(0, "mout_usbd300", group2_p, SRC_FSYS, 20, 3),
-	MUX(0, "mout_unipro", group2_p, SRC_FSYS, 24, 3),
+	MUX(0, "mout_usbd301", mout_group2_p, SRC_FSYS, 4, 3),
+	MUX(0, "mout_mmc0", mout_group2_p, SRC_FSYS, 8, 3),
+	MUX(0, "mout_mmc1", mout_group2_p, SRC_FSYS, 12, 3),
+	MUX(0, "mout_mmc2", mout_group2_p, SRC_FSYS, 16, 3),
+	MUX(0, "mout_usbd300", mout_group2_p, SRC_FSYS, 20, 3),
+	MUX(0, "mout_unipro", mout_group2_p, SRC_FSYS, 24, 3),
+	MUX(0, "mout_mphy_refclk", mout_group2_p, SRC_FSYS, 28, 3),
 
 	/* PERIC Block */
-	MUX(0, "mout_uart0", group2_p, SRC_PERIC0, 4, 3),
-	MUX(0, "mout_uart1", group2_p, SRC_PERIC0, 8, 3),
-	MUX(0, "mout_uart2", group2_p, SRC_PERIC0, 12, 3),
-	MUX(0, "mout_uart3", group2_p, SRC_PERIC0, 16, 3),
-	MUX(0, "mout_pwm", group2_p, SRC_PERIC0, 24, 3),
-	MUX(0, "mout_spdif", spdif_p, SRC_PERIC0, 28, 3),
-	MUX(0, "mout_audio0", audio0_p, SRC_PERIC1, 8, 3),
-	MUX(0, "mout_audio1", audio1_p, SRC_PERIC1, 12, 3),
-	MUX(0, "mout_audio2", audio2_p, SRC_PERIC1, 16, 3),
-	MUX(0, "mout_spi0", group2_p, SRC_PERIC1, 20, 3),
-	MUX(0, "mout_spi1", group2_p, SRC_PERIC1, 24, 3),
-	MUX(0, "mout_spi2", group2_p, SRC_PERIC1, 28, 3),
+	MUX(0, "mout_uart0", mout_group2_p, SRC_PERIC0, 4, 3),
+	MUX(0, "mout_uart1", mout_group2_p, SRC_PERIC0, 8, 3),
+	MUX(0, "mout_uart2", mout_group2_p, SRC_PERIC0, 12, 3),
+	MUX(0, "mout_uart3", mout_group2_p, SRC_PERIC0, 16, 3),
+	MUX(0, "mout_pwm", mout_group2_p, SRC_PERIC0, 24, 3),
+	MUX(0, "mout_spdif", mout_spdif_p, SRC_PERIC0, 28, 3),
+	MUX(0, "mout_audio0", mout_audio0_p, SRC_PERIC1, 8, 3),
+	MUX(0, "mout_audio1", mout_audio1_p, SRC_PERIC1, 12, 3),
+	MUX(0, "mout_audio2", mout_audio2_p, SRC_PERIC1, 16, 3),
+	MUX(0, "mout_spi0", mout_group2_p, SRC_PERIC1, 20, 3),
+	MUX(0, "mout_spi1", mout_group2_p, SRC_PERIC1, 24, 3),
+	MUX(0, "mout_spi2", mout_group2_p, SRC_PERIC1, 28, 3),
+
+	/* ISP Block */
+	MUX(0, "mout_pwm_isp", mout_group2_p, SRC_ISP, 24, 3),
+	MUX(0, "mout_uart_isp", mout_group2_p, SRC_ISP, 20, 3),
+	MUX(0, "mout_spi0_isp", mout_group2_p, SRC_ISP, 12, 3),
+	MUX(0, "mout_spi1_isp", mout_group2_p, SRC_ISP, 16, 3),
+	MUX(0, "mout_isp_sensor", mout_group2_p, SRC_ISP, 28, 3),
 };
 
-static struct samsung_div_clock exynos5420_div_clks[] __initdata = {
+static struct samsung_div_clock exynos5x_div_clks[] __initdata = {
 	DIV(0, "div_arm", "mout_cpu", DIV_CPU0, 0, 3),
 	DIV(0, "sclk_apll", "mout_apll", DIV_CPU0, 24, 3),
 	DIV(0, "armclk2", "div_arm", DIV_CPU0, 28, 3),
-	DIV(0, "div_kfc", "mout_cpu_kfc", DIV_KFC0, 0, 3),
+	DIV(0, "div_kfc", "mout_kfc", DIV_KFC0, 0, 3),
 	DIV(0, "sclk_kpll", "mout_kpll", DIV_KFC0, 24, 3),
 
+	DIV(0, "dout_aclk400_isp", "mout_aclk400_isp", DIV_TOP0, 0, 3),
 	DIV(0, "dout_aclk400_mscl", "mout_aclk400_mscl", DIV_TOP0, 4, 3),
 	DIV(0, "dout_aclk200", "mout_aclk200", DIV_TOP0, 8, 3),
 	DIV(0, "dout_aclk200_fsys2", "mout_aclk200_fsys2", DIV_TOP0, 12, 3),
+	DIV(0, "dout_aclk100_noc", "mout_aclk100_noc", DIV_TOP0, 20, 3),
 	DIV(0, "dout_pclk200_fsys", "mout_pclk200_fsys", DIV_TOP0, 24, 3),
 	DIV(0, "dout_aclk200_fsys", "mout_aclk200_fsys", DIV_TOP0, 28, 3),
 
 	DIV(0, "dout_aclk333_432_gscl", "mout_aclk333_432_gscl",
 			DIV_TOP1, 0, 3),
+	DIV(0, "dout_aclk333_432_isp", "mout_aclk333_432_isp",
+			DIV_TOP1, 4, 3),
 	DIV(0, "dout_aclk66", "mout_aclk66", DIV_TOP1, 8, 6),
+	DIV(0, "dout_aclk333_432_isp0", "mout_aclk333_432_isp0",
+			DIV_TOP1, 16, 3),
 	DIV(0, "dout_aclk266", "mout_aclk266", DIV_TOP1, 20, 3),
 	DIV(0, "dout_aclk166", "mout_aclk166", DIV_TOP1, 24, 3),
 	DIV(0, "dout_aclk333", "mout_aclk333", DIV_TOP1, 28, 3),
@@ -458,15 +773,16 @@ static struct samsung_div_clock exynos5420_div_clks[] __initdata = {
 	DIV(0, "dout_aclk266_g2d", "mout_aclk266_g2d", DIV_TOP2, 12, 3),
 	DIV(0, "dout_aclk_g3d", "mout_aclk_g3d", DIV_TOP2, 16, 3),
 	DIV(0, "dout_aclk300_jpeg", "mout_aclk300_jpeg", DIV_TOP2, 20, 3),
-	DIV_A(0, "dout_aclk300_disp1", "mout_aclk300_disp1",
-			DIV_TOP2, 24, 3, "aclk300_disp1"),
+	DIV(0, "dout_aclk300_disp1", "mout_aclk300_disp1", DIV_TOP2, 24, 3),
 	DIV(0, "dout_aclk300_gscl", "mout_aclk300_gscl", DIV_TOP2, 28, 3),
 
 	/* DISP1 Block */
-	DIV(0, "dout_fimd1", "mout_fimd1", DIV_DISP10, 0, 4),
+	DIV(0, "dout_fimd1", "mout_fimd1_final", DIV_DISP10, 0, 4),
 	DIV(0, "dout_mipi1", "mout_mipi1", DIV_DISP10, 16, 8),
 	DIV(0, "dout_dp1", "mout_dp1", DIV_DISP10, 24, 4),
 	DIV(CLK_DOUT_PIXEL, "dout_hdmi_pixel", "mout_pixel", DIV_DISP10, 28, 4),
+	DIV(0, "dout_disp1_blk", "aclk200_disp1", DIV2_RATIO0, 16, 2),
+	DIV(0, "dout_aclk400_disp1", "mout_aclk400_disp1", DIV_TOP2, 4, 3),
 
 	/* Audio Block */
 	DIV(0, "dout_maudio0", "mout_maudio0", DIV_MAU, 20, 4),
@@ -484,6 +800,7 @@ static struct samsung_div_clock exynos5420_div_clks[] __initdata = {
 	DIV(0, "dout_mmc2", "mout_mmc2", DIV_FSYS1, 20, 10),
 
 	DIV(0, "dout_unipro", "mout_unipro", DIV_FSYS2, 24, 8),
+	DIV(0, "dout_mphy_refclk", "mout_mphy_refclk", DIV_FSYS2, 16, 8),
 
 	/* UART and PWM */
 	DIV(0, "dout_uart0", "mout_uart0", DIV_PERIC0, 8, 4),
@@ -497,6 +814,9 @@ static struct samsung_div_clock exynos5420_div_clks[] __initdata = {
 	DIV(0, "dout_spi1", "mout_spi1", DIV_PERIC1, 24, 4),
 	DIV(0, "dout_spi2", "mout_spi2", DIV_PERIC1, 28, 4),
 
+	/* Mfc Block */
+	DIV(0, "dout_mfc_blk", "mout_user_aclk333", DIV4_RATIO, 0, 2),
+
 	/* PCM */
 	DIV(0, "dout_pcm1", "dout_audio1", DIV_PERIC2, 16, 8),
 	DIV(0, "dout_pcm2", "dout_audio2", DIV_PERIC2, 24, 8),
@@ -509,15 +829,43 @@ static struct samsung_div_clock exynos5420_div_clks[] __initdata = {
 	DIV(0, "dout_audio2", "mout_audio2", DIV_PERIC3, 28, 4),
 
 	/* SPI Pre-Ratio */
-	DIV(0, "dout_pre_spi0", "dout_spi0", DIV_PERIC4, 8, 8),
-	DIV(0, "dout_pre_spi1", "dout_spi1", DIV_PERIC4, 16, 8),
-	DIV(0, "dout_pre_spi2", "dout_spi2", DIV_PERIC4, 24, 8),
+	DIV(0, "dout_spi0_pre", "dout_spi0", DIV_PERIC4, 8, 8),
+	DIV(0, "dout_spi1_pre", "dout_spi1", DIV_PERIC4, 16, 8),
+	DIV(0, "dout_spi2_pre", "dout_spi2", DIV_PERIC4, 24, 8),
+
+	/* GSCL Block */
+	DIV(0, "dout_gscl_blk_300", "mout_user_aclk300_gscl",
+			DIV2_RATIO0, 4, 2),
+	DIV(0, "dout_gscl_blk_333", "aclk333_432_gscl", DIV2_RATIO0, 6, 2),
+
+	/* MSCL Block */
+	DIV(0, "dout_mscl_blk", "aclk400_mscl", DIV2_RATIO0, 28, 2),
+
+	/* PSGEN */
+	DIV(0, "dout_gen_blk", "mout_user_aclk266", DIV2_RATIO0, 8, 1),
+	DIV(0, "dout_jpg_blk", "aclk166", DIV2_RATIO0, 20, 1),
+
+	/* ISP Block */
+	DIV(0, "dout_isp_sensor0", "mout_isp_sensor", SCLK_DIV_ISP0, 8, 8),
+	DIV(0, "dout_isp_sensor1", "mout_isp_sensor", SCLK_DIV_ISP0, 16, 8),
+	DIV(0, "dout_isp_sensor2", "mout_isp_sensor", SCLK_DIV_ISP0, 24, 8),
+	DIV(0, "dout_pwm_isp", "mout_pwm_isp", SCLK_DIV_ISP1, 28, 4),
+	DIV(0, "dout_uart_isp", "mout_uart_isp", SCLK_DIV_ISP1, 24, 4),
+	DIV(0, "dout_spi0_isp", "mout_spi0_isp", SCLK_DIV_ISP1, 16, 4),
+	DIV(0, "dout_spi1_isp", "mout_spi1_isp", SCLK_DIV_ISP1, 20, 4),
+	DIV_F(0, "dout_spi0_isp_pre", "dout_spi0_isp", SCLK_DIV_ISP1, 0, 8,
+			CLK_SET_RATE_PARENT, 0),
+	DIV_F(0, "dout_spi1_isp_pre", "dout_spi1_isp", SCLK_DIV_ISP1, 8, 8,
+			CLK_SET_RATE_PARENT, 0),
 };
 
-static struct samsung_gate_clock exynos5420_gate_clks[] __initdata = {
-	/* TODO: Re-verify the CG bits for all the gate clocks */
-	GATE_A(CLK_MCT, "pclk_st", "aclk66_psgen", GATE_BUS_PERIS1, 2, 0, 0,
-		"mct"),
+static struct samsung_gate_clock exynos5x_gate_clks[] __initdata = {
+	/* G2D */
+	GATE(CLK_MDMA0, "mdma0", "aclk266_g2d", GATE_IP_G2D, 1, 0, 0),
+	GATE(CLK_SSS, "sss", "aclk266_g2d", GATE_IP_G2D, 2, 0, 0),
+	GATE(CLK_G2D, "g2d", "aclk333_g2d", GATE_IP_G2D, 3, 0, 0),
+	GATE(CLK_SMMU_MDMA0, "smmu_mdma0", "aclk266_g2d", GATE_IP_G2D, 5, 0, 0),
+	GATE(CLK_SMMU_G2D, "smmu_g2d", "aclk333_g2d", GATE_IP_G2D, 7, 0, 0),
 
 	GATE(0, "aclk200_fsys", "mout_user_aclk200_fsys",
 			GATE_BUS_FSYS0, 9, CLK_IGNORE_UNUSED, 0),
@@ -530,20 +878,42 @@ static struct samsung_gate_clock exynos5420_gate_clks[] __initdata = {
 			GATE_BUS_TOP, 1, CLK_IGNORE_UNUSED, 0),
 	GATE(0, "aclk300_jpeg", "mout_user_aclk300_jpeg",
 			GATE_BUS_TOP, 4, CLK_IGNORE_UNUSED, 0),
+	GATE(0, "aclk333_432_isp0", "mout_user_aclk333_432_isp0",
+			GATE_BUS_TOP, 5, 0, 0),
 	GATE(0, "aclk300_gscl", "mout_user_aclk300_gscl",
 			GATE_BUS_TOP, 6, CLK_IGNORE_UNUSED, 0),
 	GATE(0, "aclk333_432_gscl", "mout_user_aclk333_432_gscl",
 			GATE_BUS_TOP, 7, CLK_IGNORE_UNUSED, 0),
-	GATE(0, "pclk66_gpio", "mout_sw_aclk66",
+	GATE(0, "aclk333_432_isp", "mout_user_aclk333_432_isp",
+			GATE_BUS_TOP, 8, 0, 0),
+	GATE(CLK_PCLK66_GPIO, "pclk66_gpio", "mout_user_pclk66_gpio",
 			GATE_BUS_TOP, 9, CLK_IGNORE_UNUSED, 0),
-	GATE(0, "aclk66_psgen", "mout_aclk66_psgen",
+	GATE(0, "aclk66_psgen", "mout_user_aclk66_psgen",
 			GATE_BUS_TOP, 10, CLK_IGNORE_UNUSED, 0),
-	GATE(0, "aclk66_peric", "mout_aclk66_peric",
-			GATE_BUS_TOP, 11, 0, 0),
+	GATE(CLK_ACLK66_PERIC, "aclk66_peric", "mout_user_aclk66_peric",
+			GATE_BUS_TOP, 11, CLK_IGNORE_UNUSED, 0),
+	GATE(0, "aclk266_isp", "mout_user_aclk266_isp",
+			GATE_BUS_TOP, 13, 0, 0),
 	GATE(0, "aclk166", "mout_user_aclk166",
 			GATE_BUS_TOP, 14, CLK_IGNORE_UNUSED, 0),
 	GATE(0, "aclk333", "mout_aclk333",
 			GATE_BUS_TOP, 15, CLK_IGNORE_UNUSED, 0),
+	GATE(0, "aclk400_isp", "mout_user_aclk400_isp",
+			GATE_BUS_TOP, 16, 0, 0),
+	GATE(0, "aclk400_mscl", "mout_user_aclk400_mscl",
+			GATE_BUS_TOP, 17, 0, 0),
+	GATE(0, "aclk200_disp1", "mout_user_aclk200_disp1",
+			GATE_BUS_TOP, 18, 0, 0),
+	GATE(CLK_SCLK_MPHY_IXTAL24, "sclk_mphy_ixtal24", "mphy_refclk_ixtal24",
+			GATE_BUS_TOP, 28, 0, 0),
+	GATE(CLK_SCLK_HSIC_12M, "sclk_hsic_12m", "ff_hsic_12m",
+			GATE_BUS_TOP, 29, 0, 0),
+
+	GATE(0, "aclk300_disp1", "mout_user_aclk300_disp1",
+			SRC_MASK_TOP2, 24, 0, 0),
+
+	GATE(CLK_MAU_EPLL, "mau_epll", "mout_mau_epll_clk",
+			SRC_MASK_TOP7, 20, 0, 0),
 
 	/* sclk */
 	GATE(CLK_SCLK_UART0, "sclk_uart0", "dout_uart0",
@@ -554,11 +924,11 @@ static struct samsung_gate_clock exynos5420_gate_clks[] __initdata = {
 		GATE_TOP_SCLK_PERIC, 2, CLK_SET_RATE_PARENT, 0),
 	GATE(CLK_SCLK_UART3, "sclk_uart3", "dout_uart3",
 		GATE_TOP_SCLK_PERIC, 3, CLK_SET_RATE_PARENT, 0),
-	GATE(CLK_SCLK_SPI0, "sclk_spi0", "dout_pre_spi0",
+	GATE(CLK_SCLK_SPI0, "sclk_spi0", "dout_spi0_pre",
 		GATE_TOP_SCLK_PERIC, 6, CLK_SET_RATE_PARENT, 0),
-	GATE(CLK_SCLK_SPI1, "sclk_spi1", "dout_pre_spi1",
+	GATE(CLK_SCLK_SPI1, "sclk_spi1", "dout_spi1_pre",
 		GATE_TOP_SCLK_PERIC, 7, CLK_SET_RATE_PARENT, 0),
-	GATE(CLK_SCLK_SPI2, "sclk_spi2", "dout_pre_spi2",
+	GATE(CLK_SCLK_SPI2, "sclk_spi2", "dout_spi2_pre",
 		GATE_TOP_SCLK_PERIC, 8, CLK_SET_RATE_PARENT, 0),
 	GATE(CLK_SCLK_SPDIF, "sclk_spdif", "mout_spdif",
 		GATE_TOP_SCLK_PERIC, 9, CLK_SET_RATE_PARENT, 0),
@@ -588,164 +958,191 @@ static struct samsung_gate_clock exynos5420_gate_clks[] __initdata = {
 	GATE(CLK_SCLK_USBD301, "sclk_usbd301", "dout_usbd301",
 		GATE_TOP_SCLK_FSYS, 10, CLK_SET_RATE_PARENT, 0),
 
-	GATE(CLK_SCLK_USBD301, "sclk_unipro", "dout_unipro",
-		SRC_MASK_FSYS, 24, CLK_SET_RATE_PARENT, 0),
-
-	GATE(CLK_SCLK_GSCL_WA, "sclk_gscl_wa", "aclK333_432_gscl",
-		GATE_TOP_SCLK_GSCL, 6, CLK_SET_RATE_PARENT, 0),
-	GATE(CLK_SCLK_GSCL_WB, "sclk_gscl_wb", "aclk333_432_gscl",
-		GATE_TOP_SCLK_GSCL, 7, CLK_SET_RATE_PARENT, 0),
-
 	/* Display */
 	GATE(CLK_SCLK_FIMD1, "sclk_fimd1", "dout_fimd1",
-		GATE_TOP_SCLK_DISP1, 0, CLK_SET_RATE_PARENT, 0),
+			GATE_TOP_SCLK_DISP1, 0, CLK_SET_RATE_PARENT, 0),
 	GATE(CLK_SCLK_MIPI1, "sclk_mipi1", "dout_mipi1",
-		GATE_TOP_SCLK_DISP1, 3, CLK_SET_RATE_PARENT, 0),
+			GATE_TOP_SCLK_DISP1, 3, CLK_SET_RATE_PARENT, 0),
 	GATE(CLK_SCLK_HDMI, "sclk_hdmi", "mout_hdmi",
-		GATE_TOP_SCLK_DISP1, 9, CLK_SET_RATE_PARENT, 0),
+			GATE_TOP_SCLK_DISP1, 9, 0, 0),
 	GATE(CLK_SCLK_PIXEL, "sclk_pixel", "dout_hdmi_pixel",
-		GATE_TOP_SCLK_DISP1, 10, CLK_SET_RATE_PARENT, 0),
+			GATE_TOP_SCLK_DISP1, 10, CLK_SET_RATE_PARENT, 0),
 	GATE(CLK_SCLK_DP1, "sclk_dp1", "dout_dp1",
-		GATE_TOP_SCLK_DISP1, 20, CLK_SET_RATE_PARENT, 0),
+			GATE_TOP_SCLK_DISP1, 20, CLK_SET_RATE_PARENT, 0),
 
 	/* Maudio Block */
 	GATE(CLK_SCLK_MAUDIO0, "sclk_maudio0", "dout_maudio0",
 		GATE_TOP_SCLK_MAU, 0, CLK_SET_RATE_PARENT, 0),
 	GATE(CLK_SCLK_MAUPCM0, "sclk_maupcm0", "dout_maupcm0",
 		GATE_TOP_SCLK_MAU, 1, CLK_SET_RATE_PARENT, 0),
-	/* FSYS */
+
+	/* FSYS Block */
 	GATE(CLK_TSI, "tsi", "aclk200_fsys", GATE_BUS_FSYS0, 0, 0, 0),
 	GATE(CLK_PDMA0, "pdma0", "aclk200_fsys", GATE_BUS_FSYS0, 1, 0, 0),
 	GATE(CLK_PDMA1, "pdma1", "aclk200_fsys", GATE_BUS_FSYS0, 2, 0, 0),
 	GATE(CLK_UFS, "ufs", "aclk200_fsys2", GATE_BUS_FSYS0, 3, 0, 0),
-	GATE(CLK_RTIC, "rtic", "aclk200_fsys", GATE_BUS_FSYS0, 5, 0, 0),
-	GATE(CLK_MMC0, "mmc0", "aclk200_fsys2", GATE_BUS_FSYS0, 12, 0, 0),
-	GATE(CLK_MMC1, "mmc1", "aclk200_fsys2", GATE_BUS_FSYS0, 13, 0, 0),
-	GATE(CLK_MMC2, "mmc2", "aclk200_fsys2", GATE_BUS_FSYS0, 14, 0, 0),
+	GATE(CLK_RTIC, "rtic", "aclk200_fsys", GATE_IP_FSYS, 9, 0, 0),
+	GATE(CLK_MMC0, "mmc0", "aclk200_fsys2", GATE_IP_FSYS, 12, 0, 0),
+	GATE(CLK_MMC1, "mmc1", "aclk200_fsys2", GATE_IP_FSYS, 13, 0, 0),
+	GATE(CLK_MMC2, "mmc2", "aclk200_fsys2", GATE_IP_FSYS, 14, 0, 0),
 	GATE(CLK_SROMC, "sromc", "aclk200_fsys2",
-			GATE_BUS_FSYS0, 19, CLK_IGNORE_UNUSED, 0),
-	GATE(CLK_USBH20, "usbh20", "aclk200_fsys", GATE_BUS_FSYS0, 20, 0, 0),
-	GATE(CLK_USBD300, "usbd300", "aclk200_fsys", GATE_BUS_FSYS0, 21, 0, 0),
-	GATE(CLK_USBD301, "usbd301", "aclk200_fsys", GATE_BUS_FSYS0, 28, 0, 0),
-
-	/* UART */
-	GATE(CLK_UART0, "uart0", "aclk66_peric", GATE_BUS_PERIC, 4, 0, 0),
-	GATE(CLK_UART1, "uart1", "aclk66_peric", GATE_BUS_PERIC, 5, 0, 0),
-	GATE_A(CLK_UART2, "uart2", "aclk66_peric",
-		GATE_BUS_PERIC, 6, CLK_IGNORE_UNUSED, 0, "uart2"),
-	GATE(CLK_UART3, "uart3", "aclk66_peric", GATE_BUS_PERIC, 7, 0, 0),
-	/* I2C */
-	GATE(CLK_I2C0, "i2c0", "aclk66_peric", GATE_BUS_PERIC, 9, 0, 0),
-	GATE(CLK_I2C1, "i2c1", "aclk66_peric", GATE_BUS_PERIC, 10, 0, 0),
-	GATE(CLK_I2C2, "i2c2", "aclk66_peric", GATE_BUS_PERIC, 11, 0, 0),
-	GATE(CLK_I2C3, "i2c3", "aclk66_peric", GATE_BUS_PERIC, 12, 0, 0),
-	GATE(CLK_I2C4, "i2c4", "aclk66_peric", GATE_BUS_PERIC, 13, 0, 0),
-	GATE(CLK_I2C5, "i2c5", "aclk66_peric", GATE_BUS_PERIC, 14, 0, 0),
-	GATE(CLK_I2C6, "i2c6", "aclk66_peric", GATE_BUS_PERIC, 15, 0, 0),
-	GATE(CLK_I2C7, "i2c7", "aclk66_peric", GATE_BUS_PERIC, 16, 0, 0),
-	GATE(CLK_I2C_HDMI, "i2c_hdmi", "aclk66_peric", GATE_BUS_PERIC, 17, 0,
-		0),
-	GATE(CLK_TSADC, "tsadc", "aclk66_peric", GATE_BUS_PERIC, 18, 0, 0),
-	/* SPI */
-	GATE(CLK_SPI0, "spi0", "aclk66_peric", GATE_BUS_PERIC, 19, 0, 0),
-	GATE(CLK_SPI1, "spi1", "aclk66_peric", GATE_BUS_PERIC, 20, 0, 0),
-	GATE(CLK_SPI2, "spi2", "aclk66_peric", GATE_BUS_PERIC, 21, 0, 0),
-	GATE(CLK_KEYIF, "keyif", "aclk66_peric", GATE_BUS_PERIC, 22, 0, 0),
-	/* I2S */
-	GATE(CLK_I2S1, "i2s1", "aclk66_peric", GATE_BUS_PERIC, 23, 0, 0),
-	GATE(CLK_I2S2, "i2s2", "aclk66_peric", GATE_BUS_PERIC, 24, 0, 0),
-	/* PCM */
-	GATE(CLK_PCM1, "pcm1", "aclk66_peric", GATE_BUS_PERIC, 25, 0, 0),
-	GATE(CLK_PCM2, "pcm2", "aclk66_peric", GATE_BUS_PERIC, 26, 0, 0),
-	/* PWM */
-	GATE(CLK_PWM, "pwm", "aclk66_peric", GATE_BUS_PERIC, 27, 0, 0),
-	/* SPDIF */
-	GATE(CLK_SPDIF, "spdif", "aclk66_peric", GATE_BUS_PERIC, 29, 0, 0),
+			GATE_IP_FSYS, 17, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_USBH20, "usbh20", "aclk200_fsys", GATE_IP_FSYS, 18, 0, 0),
+	GATE(CLK_USBD300, "usbd300", "aclk200_fsys", GATE_IP_FSYS, 19, 0, 0),
+	GATE(CLK_USBD301, "usbd301", "aclk200_fsys", GATE_IP_FSYS, 20, 0, 0),
+	GATE(CLK_SCLK_UNIPRO, "sclk_unipro", "dout_unipro",
+			SRC_MASK_FSYS, 24, CLK_SET_RATE_PARENT, 0),
 
-	GATE(CLK_I2C8, "i2c8", "aclk66_peric", GATE_BUS_PERIC1, 0, 0, 0),
-	GATE(CLK_I2C9, "i2c9", "aclk66_peric", GATE_BUS_PERIC1, 1, 0, 0),
-	GATE(CLK_I2C10, "i2c10", "aclk66_peric", GATE_BUS_PERIC1, 2, 0, 0),
+	/* PERIC Block */
+	GATE(CLK_UART0, "uart0", "aclk66_peric", GATE_IP_PERIC, 0, 0, 0),
+	GATE(CLK_UART1, "uart1", "aclk66_peric", GATE_IP_PERIC, 1, 0, 0),
+	GATE(CLK_UART2, "uart2", "aclk66_peric", GATE_IP_PERIC, 2, 0, 0),
+	GATE(CLK_UART3, "uart3", "aclk66_peric", GATE_IP_PERIC, 3, 0, 0),
+	GATE(CLK_I2C0, "i2c0", "aclk66_peric", GATE_IP_PERIC, 6, 0, 0),
+	GATE(CLK_I2C1, "i2c1", "aclk66_peric", GATE_IP_PERIC, 7, 0, 0),
+	GATE(CLK_I2C2, "i2c2", "aclk66_peric", GATE_IP_PERIC, 8, 0, 0),
+	GATE(CLK_I2C3, "i2c3", "aclk66_peric", GATE_IP_PERIC, 9, 0, 0),
+	GATE(CLK_USI0, "usi0", "aclk66_peric", GATE_IP_PERIC, 10, 0, 0),
+	GATE(CLK_USI1, "usi1", "aclk66_peric", GATE_IP_PERIC, 11, 0, 0),
+	GATE(CLK_USI2, "usi2", "aclk66_peric", GATE_IP_PERIC, 12, 0, 0),
+	GATE(CLK_USI3, "usi3", "aclk66_peric", GATE_IP_PERIC, 13, 0, 0),
+	GATE(CLK_I2C_HDMI, "i2c_hdmi", "aclk66_peric", GATE_IP_PERIC, 14, 0, 0),
+	GATE(CLK_TSADC, "tsadc", "aclk66_peric", GATE_IP_PERIC, 15, 0, 0),
+	GATE(CLK_SPI0, "spi0", "aclk66_peric", GATE_IP_PERIC, 16, 0, 0),
+	GATE(CLK_SPI1, "spi1", "aclk66_peric", GATE_IP_PERIC, 17, 0, 0),
+	GATE(CLK_SPI2, "spi2", "aclk66_peric", GATE_IP_PERIC, 18, 0, 0),
+	GATE(CLK_I2S1, "i2s1", "aclk66_peric", GATE_IP_PERIC, 20, 0, 0),
+	GATE(CLK_I2S2, "i2s2", "aclk66_peric", GATE_IP_PERIC, 21, 0, 0),
+	GATE(CLK_PCM1, "pcm1", "aclk66_peric", GATE_IP_PERIC, 22, 0, 0),
+	GATE(CLK_PCM2, "pcm2", "aclk66_peric", GATE_IP_PERIC, 23, 0, 0),
+	GATE(CLK_PWM, "pwm", "aclk66_peric", GATE_IP_PERIC, 24, 0, 0),
+	GATE(CLK_SPDIF, "spdif", "aclk66_peric", GATE_IP_PERIC, 26, 0, 0),
+	GATE(CLK_USI4, "usi4", "aclk66_peric", GATE_IP_PERIC, 28, 0, 0),
+	GATE(CLK_USI5, "usi5", "aclk66_peric", GATE_IP_PERIC, 30, 0, 0),
+	GATE(CLK_USI6, "usi6", "aclk66_peric", GATE_IP_PERIC, 31, 0, 0),
 
+	GATE(CLK_KEYIF, "keyif", "aclk66_peric", GATE_BUS_PERIC, 22, 0, 0),
+
+	/* PERIS Block */
 	GATE(CLK_CHIPID, "chipid", "aclk66_psgen",
-			GATE_BUS_PERIS0, 12, CLK_IGNORE_UNUSED, 0),
+			GATE_IP_PERIS, 0, CLK_IGNORE_UNUSED, 0),
 	GATE(CLK_SYSREG, "sysreg", "aclk66_psgen",
-			GATE_BUS_PERIS0, 13, CLK_IGNORE_UNUSED, 0),
-	GATE(CLK_TZPC0, "tzpc0", "aclk66_psgen", GATE_BUS_PERIS0, 18, 0, 0),
-	GATE(CLK_TZPC1, "tzpc1", "aclk66_psgen", GATE_BUS_PERIS0, 19, 0, 0),
-	GATE(CLK_TZPC2, "tzpc2", "aclk66_psgen", GATE_BUS_PERIS0, 20, 0, 0),
-	GATE(CLK_TZPC3, "tzpc3", "aclk66_psgen", GATE_BUS_PERIS0, 21, 0, 0),
-	GATE(CLK_TZPC4, "tzpc4", "aclk66_psgen", GATE_BUS_PERIS0, 22, 0, 0),
-	GATE(CLK_TZPC5, "tzpc5", "aclk66_psgen", GATE_BUS_PERIS0, 23, 0, 0),
-	GATE(CLK_TZPC6, "tzpc6", "aclk66_psgen", GATE_BUS_PERIS0, 24, 0, 0),
-	GATE(CLK_TZPC7, "tzpc7", "aclk66_psgen", GATE_BUS_PERIS0, 25, 0, 0),
-	GATE(CLK_TZPC8, "tzpc8", "aclk66_psgen", GATE_BUS_PERIS0, 26, 0, 0),
-	GATE(CLK_TZPC9, "tzpc9", "aclk66_psgen", GATE_BUS_PERIS0, 27, 0, 0),
-
-	GATE(CLK_HDMI_CEC, "hdmi_cec", "aclk66_psgen", GATE_BUS_PERIS1, 0, 0,
-		0),
+			GATE_IP_PERIS, 1, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TZPC0, "tzpc0", "aclk66_psgen", GATE_IP_PERIS, 6, 0, 0),
+	GATE(CLK_TZPC1, "tzpc1", "aclk66_psgen", GATE_IP_PERIS, 7, 0, 0),
+	GATE(CLK_TZPC2, "tzpc2", "aclk66_psgen", GATE_IP_PERIS, 8, 0, 0),
+	GATE(CLK_TZPC3, "tzpc3", "aclk66_psgen", GATE_IP_PERIS, 9, 0, 0),
+	GATE(CLK_TZPC4, "tzpc4", "aclk66_psgen", GATE_IP_PERIS, 10, 0, 0),
+	GATE(CLK_TZPC5, "tzpc5", "aclk66_psgen", GATE_IP_PERIS, 11, 0, 0),
+	GATE(CLK_TZPC6, "tzpc6", "aclk66_psgen", GATE_IP_PERIS, 12, 0, 0),
+	GATE(CLK_TZPC7, "tzpc7", "aclk66_psgen", GATE_IP_PERIS, 13, 0, 0),
+	GATE(CLK_TZPC8, "tzpc8", "aclk66_psgen", GATE_IP_PERIS, 14, 0, 0),
+	GATE(CLK_TZPC9, "tzpc9", "aclk66_psgen", GATE_IP_PERIS, 15, 0, 0),
+	GATE(CLK_HDMI_CEC, "hdmi_cec", "aclk66_psgen", GATE_IP_PERIS, 16, 0, 0),
+	GATE(CLK_MCT, "mct", "aclk66_psgen", GATE_IP_PERIS, 18, 0, 0),
+	GATE(CLK_WDT, "wdt", "aclk66_psgen", GATE_IP_PERIS, 19, 0, 0),
+	GATE(CLK_RTC, "rtc", "aclk66_psgen", GATE_IP_PERIS, 20, 0, 0),
+	GATE(CLK_TMU, "tmu", "aclk66_psgen", GATE_IP_PERIS, 21, 0, 0),
+	GATE(CLK_TMU_GPU, "tmu_gpu", "aclk66_psgen", GATE_IP_PERIS, 22, 0, 0),
+
 	GATE(CLK_SECKEY, "seckey", "aclk66_psgen", GATE_BUS_PERIS1, 1, 0, 0),
-	GATE(CLK_WDT, "wdt", "aclk66_psgen", GATE_BUS_PERIS1, 3, 0, 0),
-	GATE(CLK_RTC, "rtc", "aclk66_psgen", GATE_BUS_PERIS1, 4, 0, 0),
-	GATE(CLK_TMU, "tmu", "aclk66_psgen", GATE_BUS_PERIS1, 5, 0, 0),
-	GATE(CLK_TMU_GPU, "tmu_gpu", "aclk66_psgen", GATE_BUS_PERIS1, 6, 0, 0),
+
+	/* GEN Block */
+	GATE(CLK_ROTATOR, "rotator", "mout_user_aclk266", GATE_IP_GEN, 1, 0, 0),
+	GATE(CLK_JPEG, "jpeg", "aclk300_jpeg", GATE_IP_GEN, 2, 0, 0),
+	GATE(CLK_JPEG2, "jpeg2", "aclk300_jpeg", GATE_IP_GEN, 3, 0, 0),
+	GATE(CLK_MDMA1, "mdma1", "mout_user_aclk266", GATE_IP_GEN, 4, 0, 0),
+	GATE(CLK_TOP_RTC, "top_rtc", "aclk66_psgen", GATE_IP_GEN, 5, 0, 0),
+	GATE(CLK_SMMU_ROTATOR, "smmu_rotator", "dout_gen_blk",
+			GATE_IP_GEN, 6, 0, 0),
+	GATE(CLK_SMMU_JPEG, "smmu_jpeg", "dout_jpg_blk", GATE_IP_GEN, 7, 0, 0),
+	GATE(CLK_SMMU_MDMA1, "smmu_mdma1", "dout_gen_blk",
+			GATE_IP_GEN, 9, 0, 0),
+
+	/* GATE_IP_GEN doesn't list gates for smmu_jpeg2 and mc */
+	GATE(CLK_SMMU_JPEG2, "smmu_jpeg2", "dout_jpg_blk",
+			GATE_BUS_GEN, 28, 0, 0),
+	GATE(CLK_MC, "mc", "aclk66_psgen", GATE_BUS_GEN, 12, 0, 0),
+
+	/* GSCL Block */
+	GATE(CLK_SCLK_GSCL_WA, "sclk_gscl_wa", "mout_user_aclk333_432_gscl",
+			GATE_TOP_SCLK_GSCL, 6, 0, 0),
+	GATE(CLK_SCLK_GSCL_WB, "sclk_gscl_wb", "mout_user_aclk333_432_gscl",
+			GATE_TOP_SCLK_GSCL, 7, 0, 0),
 
 	GATE(CLK_GSCL0, "gscl0", "aclk300_gscl", GATE_IP_GSCL0, 0, 0, 0),
 	GATE(CLK_GSCL1, "gscl1", "aclk300_gscl", GATE_IP_GSCL0, 1, 0, 0),
-	GATE(CLK_CLK_3AA, "clk_3aa", "aclk300_gscl", GATE_IP_GSCL0, 4, 0, 0),
-
-	GATE(CLK_SMMU_3AA, "smmu_3aa", "aclk333_432_gscl", GATE_IP_GSCL1, 2, 0,
-		0),
-	GATE(CLK_SMMU_FIMCL0, "smmu_fimcl0", "aclk333_432_gscl",
+	GATE(CLK_FIMC_3AA, "fimc_3aa", "aclk333_432_gscl",
+			GATE_IP_GSCL0, 4, 0, 0),
+	GATE(CLK_FIMC_LITE0, "fimc_lite0", "aclk333_432_gscl",
+			GATE_IP_GSCL0, 5, 0, 0),
+	GATE(CLK_FIMC_LITE1, "fimc_lite1", "aclk333_432_gscl",
+			GATE_IP_GSCL0, 6, 0, 0),
+
+	GATE(CLK_SMMU_3AA, "smmu_3aa", "dout_gscl_blk_333",
+			GATE_IP_GSCL1, 2, 0, 0),
+	GATE(CLK_SMMU_FIMCL0, "smmu_fimcl0", "dout_gscl_blk_333",
 			GATE_IP_GSCL1, 3, 0, 0),
-	GATE(CLK_SMMU_FIMCL1, "smmu_fimcl1", "aclk333_432_gscl",
+	GATE(CLK_SMMU_FIMCL1, "smmu_fimcl1", "dout_gscl_blk_333",
 			GATE_IP_GSCL1, 4, 0, 0),
-	GATE(CLK_SMMU_GSCL0, "smmu_gscl0", "aclk300_gscl", GATE_IP_GSCL1, 6, 0,
-		0),
-	GATE(CLK_SMMU_GSCL1, "smmu_gscl1", "aclk300_gscl", GATE_IP_GSCL1, 7, 0,
-		0),
-	GATE(CLK_GSCL_WA, "gscl_wa", "aclk300_gscl", GATE_IP_GSCL1, 12, 0, 0),
-	GATE(CLK_GSCL_WB, "gscl_wb", "aclk300_gscl", GATE_IP_GSCL1, 13, 0, 0),
-	GATE(CLK_SMMU_FIMCL3, "smmu_fimcl3,", "aclk333_432_gscl",
+	GATE(CLK_SMMU_GSCL0, "smmu_gscl0", "dout_gscl_blk_300",
+			GATE_IP_GSCL1, 6, 0, 0),
+	GATE(CLK_SMMU_GSCL1, "smmu_gscl1", "dout_gscl_blk_300",
+			GATE_IP_GSCL1, 7, 0, 0),
+	GATE(CLK_GSCL_WA, "gscl_wa", "sclk_gscl_wa", GATE_IP_GSCL1, 12, 0, 0),
+	GATE(CLK_GSCL_WB, "gscl_wb", "sclk_gscl_wb", GATE_IP_GSCL1, 13, 0, 0),
+	GATE(CLK_SMMU_FIMCL3, "smmu_fimcl3,", "dout_gscl_blk_333",
 			GATE_IP_GSCL1, 16, 0, 0),
 	GATE(CLK_FIMC_LITE3, "fimc_lite3", "aclk333_432_gscl",
 			GATE_IP_GSCL1, 17, 0, 0),
 
+	/* MSCL Block */
+	GATE(CLK_MSCL0, "mscl0", "aclk400_mscl", GATE_IP_MSCL, 0, 0, 0),
+	GATE(CLK_MSCL1, "mscl1", "aclk400_mscl", GATE_IP_MSCL, 1, 0, 0),
+	GATE(CLK_MSCL2, "mscl2", "aclk400_mscl", GATE_IP_MSCL, 2, 0, 0),
+	GATE(CLK_SMMU_MSCL0, "smmu_mscl0", "dout_mscl_blk",
+			GATE_IP_MSCL, 8, 0, 0),
+	GATE(CLK_SMMU_MSCL1, "smmu_mscl1", "dout_mscl_blk",
+			GATE_IP_MSCL, 9, 0, 0),
+	GATE(CLK_SMMU_MSCL2, "smmu_mscl2", "dout_mscl_blk",
+			GATE_IP_MSCL, 10, 0, 0),
+
 	GATE(CLK_FIMD1, "fimd1", "aclk300_disp1", GATE_IP_DISP1, 0, 0, 0),
 	GATE(CLK_DSIM1, "dsim1", "aclk200_disp1", GATE_IP_DISP1, 3, 0, 0),
 	GATE(CLK_DP1, "dp1", "aclk200_disp1", GATE_IP_DISP1, 4, 0, 0),
-	GATE(CLK_MIXER, "mixer", "aclk166", GATE_IP_DISP1, 5, 0, 0),
+	GATE(CLK_MIXER, "mixer", "aclk200_disp1", GATE_IP_DISP1, 5, 0, 0),
 	GATE(CLK_HDMI, "hdmi", "aclk200_disp1", GATE_IP_DISP1, 6, 0, 0),
-	GATE(CLK_SMMU_FIMD1, "smmu_fimd1", "aclk300_disp1", GATE_IP_DISP1, 8, 0,
-		0),
+	GATE(CLK_SMMU_FIMD1M0, "smmu_fimd1m0", "dout_disp1_blk",
+			GATE_IP_DISP1, 7, 0, 0),
+	GATE(CLK_SMMU_FIMD1M1, "smmu_fimd1m1", "dout_disp1_blk",
+			GATE_IP_DISP1, 8, 0, 0),
+	GATE(CLK_SMMU_MIXER, "smmu_mixer", "aclk200_disp1",
+			GATE_IP_DISP1, 9, 0, 0),
+
+	/* ISP */
+	GATE(CLK_SCLK_UART_ISP, "sclk_uart_isp", "dout_uart_isp",
+			GATE_TOP_SCLK_ISP, 0, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_SPI0_ISP, "sclk_spi0_isp", "dout_spi0_isp_pre",
+			GATE_TOP_SCLK_ISP, 1, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_SPI1_ISP, "sclk_spi1_isp", "dout_spi1_isp_pre",
+			GATE_TOP_SCLK_ISP, 2, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_PWM_ISP, "sclk_pwm_isp", "dout_pwm_isp",
+			GATE_TOP_SCLK_ISP, 3, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_ISP_SENSOR0, "sclk_isp_sensor0", "dout_isp_sensor0",
+			GATE_TOP_SCLK_ISP, 4, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_ISP_SENSOR1, "sclk_isp_sensor1", "dout_isp_sensor1",
+			GATE_TOP_SCLK_ISP, 8, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_ISP_SENSOR2, "sclk_isp_sensor2", "dout_isp_sensor2",
+			GATE_TOP_SCLK_ISP, 12, CLK_SET_RATE_PARENT, 0),
 
 	GATE(CLK_MFC, "mfc", "aclk333", GATE_IP_MFC, 0, 0, 0),
-	GATE(CLK_SMMU_MFCL, "smmu_mfcl", "aclk333", GATE_IP_MFC, 1, 0, 0),
-	GATE(CLK_SMMU_MFCR, "smmu_mfcr", "aclk333", GATE_IP_MFC, 2, 0, 0),
-
-	GATE(CLK_G3D, "g3d", "aclkg3d", GATE_IP_G3D, 9, 0, 0),
+	GATE(CLK_SMMU_MFCL, "smmu_mfcl", "dout_mfc_blk", GATE_IP_MFC, 1, 0, 0),
+	GATE(CLK_SMMU_MFCR, "smmu_mfcr", "dout_mfc_blk", GATE_IP_MFC, 2, 0, 0),
 
-	GATE(CLK_ROTATOR, "rotator", "aclk266", GATE_IP_GEN, 1, 0, 0),
-	GATE(CLK_JPEG, "jpeg", "aclk300_jpeg", GATE_IP_GEN, 2, 0, 0),
-	GATE(CLK_JPEG2, "jpeg2", "aclk300_jpeg", GATE_IP_GEN, 3, 0, 0),
-	GATE(CLK_MDMA1, "mdma1", "aclk266", GATE_IP_GEN, 4, 0, 0),
-	GATE(CLK_SMMU_ROTATOR, "smmu_rotator", "aclk266", GATE_IP_GEN, 6, 0, 0),
-	GATE(CLK_SMMU_JPEG, "smmu_jpeg", "aclk300_jpeg", GATE_IP_GEN, 7, 0, 0),
-	GATE(CLK_SMMU_MDMA1, "smmu_mdma1", "aclk266", GATE_IP_GEN, 9, 0, 0),
-
-	GATE(CLK_MSCL0, "mscl0", "aclk400_mscl", GATE_IP_MSCL, 0, 0, 0),
-	GATE(CLK_MSCL1, "mscl1", "aclk400_mscl", GATE_IP_MSCL, 1, 0, 0),
-	GATE(CLK_MSCL2, "mscl2", "aclk400_mscl", GATE_IP_MSCL, 2, 0, 0),
-	GATE(CLK_SMMU_MSCL0, "smmu_mscl0", "aclk400_mscl", GATE_IP_MSCL, 8, 0,
-		0),
-	GATE(CLK_SMMU_MSCL1, "smmu_mscl1", "aclk400_mscl", GATE_IP_MSCL, 9, 0,
-		0),
-	GATE(CLK_SMMU_MSCL2, "smmu_mscl2", "aclk400_mscl", GATE_IP_MSCL, 10, 0,
-		0),
-	GATE(CLK_SMMU_MIXER, "smmu_mixer", "aclk200_disp1", GATE_IP_DISP1, 9, 0,
-		0),
+	GATE(CLK_G3D, "g3d", "mout_user_aclk_g3d", GATE_IP_G3D, 9, 0, 0),
 };
 
-static struct samsung_pll_clock exynos5420_plls[nr_plls] __initdata = {
+static struct samsung_pll_clock exynos5x_plls[nr_plls] __initdata = {
 	[apll] = PLL(pll_2550, CLK_FOUT_APLL, "fout_apll", "fin_pll", APLL_LOCK,
 		APLL_CON0, NULL),
 	[cpll] = PLL(pll_2550, CLK_FOUT_CPLL, "fout_cpll", "fin_pll", CPLL_LOCK,
@@ -776,8 +1173,11 @@ static struct of_device_id ext_clk_match[] __initdata = {
 };
 
 /* register exynos5420 clocks */
-static void __init exynos5420_clk_init(struct device_node *np)
+static void __init exynos5x_clk_init(struct device_node *np,
+		enum exynos5x_soc soc)
 {
+	struct samsung_clk_provider *ctx;
+
 	if (np) {
 		reg_base = of_iomap(np, 0);
 		if (!reg_base)
@@ -786,23 +1186,56 @@ static void __init exynos5420_clk_init(struct device_node *np)
 		panic("%s: unable to determine soc\n", __func__);
 	}
 
-	samsung_clk_init(np, reg_base, CLK_NR_CLKS);
-	samsung_clk_of_register_fixed_ext(exynos5420_fixed_rate_ext_clks,
-			ARRAY_SIZE(exynos5420_fixed_rate_ext_clks),
+	exynos5x_soc = soc;
+
+	ctx = samsung_clk_init(np, reg_base, CLK_NR_CLKS);
+	if (!ctx)
+		panic("%s: unable to allocate context.\n", __func__);
+
+	samsung_clk_of_register_fixed_ext(ctx, exynos5x_fixed_rate_ext_clks,
+			ARRAY_SIZE(exynos5x_fixed_rate_ext_clks),
 			ext_clk_match);
-	samsung_clk_register_pll(exynos5420_plls, ARRAY_SIZE(exynos5420_plls),
+	samsung_clk_register_pll(ctx, exynos5x_plls, ARRAY_SIZE(exynos5x_plls),
 					reg_base);
-	samsung_clk_register_fixed_rate(exynos5420_fixed_rate_clks,
-			ARRAY_SIZE(exynos5420_fixed_rate_clks));
-	samsung_clk_register_fixed_factor(exynos5420_fixed_factor_clks,
-			ARRAY_SIZE(exynos5420_fixed_factor_clks));
-	samsung_clk_register_mux(exynos5420_mux_clks,
-			ARRAY_SIZE(exynos5420_mux_clks));
-	samsung_clk_register_div(exynos5420_div_clks,
-			ARRAY_SIZE(exynos5420_div_clks));
-	samsung_clk_register_gate(exynos5420_gate_clks,
-			ARRAY_SIZE(exynos5420_gate_clks));
+	samsung_clk_register_fixed_rate(ctx, exynos5x_fixed_rate_clks,
+			ARRAY_SIZE(exynos5x_fixed_rate_clks));
+	samsung_clk_register_fixed_factor(ctx, exynos5x_fixed_factor_clks,
+			ARRAY_SIZE(exynos5x_fixed_factor_clks));
+	samsung_clk_register_mux(ctx, exynos5x_mux_clks,
+			ARRAY_SIZE(exynos5x_mux_clks));
+	samsung_clk_register_div(ctx, exynos5x_div_clks,
+			ARRAY_SIZE(exynos5x_div_clks));
+	samsung_clk_register_gate(ctx, exynos5x_gate_clks,
+			ARRAY_SIZE(exynos5x_gate_clks));
+
+	if (soc == EXYNOS5420) {
+		samsung_clk_register_mux(ctx, exynos5420_mux_clks,
+				ARRAY_SIZE(exynos5420_mux_clks));
+		samsung_clk_register_div(ctx, exynos5420_div_clks,
+				ARRAY_SIZE(exynos5420_div_clks));
+	} else {
+		samsung_clk_register_fixed_factor(
+				ctx, exynos5800_fixed_factor_clks,
+				ARRAY_SIZE(exynos5800_fixed_factor_clks));
+		samsung_clk_register_mux(ctx, exynos5800_mux_clks,
+				ARRAY_SIZE(exynos5800_mux_clks));
+		samsung_clk_register_div(ctx, exynos5800_div_clks,
+				ARRAY_SIZE(exynos5800_div_clks));
+		samsung_clk_register_gate(ctx, exynos5800_gate_clks,
+				ARRAY_SIZE(exynos5800_gate_clks));
+	}
 
 	exynos5420_clk_sleep_init();
 }
+
+static void __init exynos5420_clk_init(struct device_node *np)
+{
+	exynos5x_clk_init(np, EXYNOS5420);
+}
 CLK_OF_DECLARE(exynos5420_clk, "samsung,exynos5420-clock", exynos5420_clk_init);
+
+static void __init exynos5800_clk_init(struct device_node *np)
+{
+	exynos5x_clk_init(np, EXYNOS5800);
+}
+CLK_OF_DECLARE(exynos5800_clk, "samsung,exynos5800-clock", exynos5800_clk_init);
diff --git a/drivers/clk/samsung/clk-exynos5440.c b/drivers/clk/samsung/clk-exynos5440.c
index 2bfad5a993d..647f1440aa6 100644
--- a/drivers/clk/samsung/clk-exynos5440.c
+++ b/drivers/clk/samsung/clk-exynos5440.c
@@ -93,6 +93,7 @@ static struct of_device_id ext_clk_match[] __initdata = {
 static void __init exynos5440_clk_init(struct device_node *np)
 {
 	void __iomem *reg_base;
+	struct samsung_clk_provider *ctx;
 
 	reg_base = of_iomap(np, 0);
 	if (!reg_base) {
@@ -101,22 +102,25 @@ static void __init exynos5440_clk_init(struct device_node *np)
 		return;
 	}
 
-	samsung_clk_init(np, reg_base, CLK_NR_CLKS);
-	samsung_clk_of_register_fixed_ext(exynos5440_fixed_rate_ext_clks,
+	ctx = samsung_clk_init(np, reg_base, CLK_NR_CLKS);
+	if (!ctx)
+		panic("%s: unable to allocate context.\n", __func__);
+
+	samsung_clk_of_register_fixed_ext(ctx, exynos5440_fixed_rate_ext_clks,
 		ARRAY_SIZE(exynos5440_fixed_rate_ext_clks), ext_clk_match);
 
 	samsung_clk_register_pll2550x("cplla", "xtal", reg_base + 0x1c, 0x10);
 	samsung_clk_register_pll2550x("cpllb", "xtal", reg_base + 0x20, 0x10);
 
-	samsung_clk_register_fixed_rate(exynos5440_fixed_rate_clks,
+	samsung_clk_register_fixed_rate(ctx, exynos5440_fixed_rate_clks,
 			ARRAY_SIZE(exynos5440_fixed_rate_clks));
-	samsung_clk_register_fixed_factor(exynos5440_fixed_factor_clks,
+	samsung_clk_register_fixed_factor(ctx, exynos5440_fixed_factor_clks,
 			ARRAY_SIZE(exynos5440_fixed_factor_clks));
-	samsung_clk_register_mux(exynos5440_mux_clks,
+	samsung_clk_register_mux(ctx, exynos5440_mux_clks,
 			ARRAY_SIZE(exynos5440_mux_clks));
-	samsung_clk_register_div(exynos5440_div_clks,
+	samsung_clk_register_div(ctx, exynos5440_div_clks,
 			ARRAY_SIZE(exynos5440_div_clks));
-	samsung_clk_register_gate(exynos5440_gate_clks,
+	samsung_clk_register_gate(ctx, exynos5440_gate_clks,
 			ARRAY_SIZE(exynos5440_gate_clks));
 
 	pr_info("Exynos5440: arm_clk = %ldHz\n", _get_rate("arm_clk"));
diff --git a/drivers/clk/samsung/clk-pll.c b/drivers/clk/samsung/clk-pll.c
index 81e6d2f49aa..b07fad2a916 100644
--- a/drivers/clk/samsung/clk-pll.c
+++ b/drivers/clk/samsung/clk-pll.c
@@ -11,6 +11,7 @@
 
 #include <linux/errno.h>
 #include <linux/hrtimer.h>
+#include <linux/delay.h>
 #include "clk.h"
 #include "clk-pll.h"
 
@@ -59,6 +60,72 @@ static long samsung_pll_round_rate(struct clk_hw *hw,
 }
 
 /*
+ * PLL2126 Clock Type
+ */
+
+#define PLL2126_MDIV_MASK	(0xff)
+#define PLL2126_PDIV_MASK	(0x3f)
+#define PLL2126_SDIV_MASK	(0x3)
+#define PLL2126_MDIV_SHIFT	(16)
+#define PLL2126_PDIV_SHIFT	(8)
+#define PLL2126_SDIV_SHIFT	(0)
+
+static unsigned long samsung_pll2126_recalc_rate(struct clk_hw *hw,
+				unsigned long parent_rate)
+{
+	struct samsung_clk_pll *pll = to_clk_pll(hw);
+	u32 pll_con, mdiv, pdiv, sdiv;
+	u64 fvco = parent_rate;
+
+	pll_con = __raw_readl(pll->con_reg);
+	mdiv = (pll_con >> PLL2126_MDIV_SHIFT) & PLL2126_MDIV_MASK;
+	pdiv = (pll_con >> PLL2126_PDIV_SHIFT) & PLL2126_PDIV_MASK;
+	sdiv = (pll_con >> PLL2126_SDIV_SHIFT) & PLL2126_SDIV_MASK;
+
+	fvco *= (mdiv + 8);
+	do_div(fvco, (pdiv + 2) << sdiv);
+
+	return (unsigned long)fvco;
+}
+
+static const struct clk_ops samsung_pll2126_clk_ops = {
+	.recalc_rate = samsung_pll2126_recalc_rate,
+};
+
+/*
+ * PLL3000 Clock Type
+ */
+
+#define PLL3000_MDIV_MASK	(0xff)
+#define PLL3000_PDIV_MASK	(0x3)
+#define PLL3000_SDIV_MASK	(0x3)
+#define PLL3000_MDIV_SHIFT	(16)
+#define PLL3000_PDIV_SHIFT	(8)
+#define PLL3000_SDIV_SHIFT	(0)
+
+static unsigned long samsung_pll3000_recalc_rate(struct clk_hw *hw,
+				unsigned long parent_rate)
+{
+	struct samsung_clk_pll *pll = to_clk_pll(hw);
+	u32 pll_con, mdiv, pdiv, sdiv;
+	u64 fvco = parent_rate;
+
+	pll_con = __raw_readl(pll->con_reg);
+	mdiv = (pll_con >> PLL3000_MDIV_SHIFT) & PLL3000_MDIV_MASK;
+	pdiv = (pll_con >> PLL3000_PDIV_SHIFT) & PLL3000_PDIV_MASK;
+	sdiv = (pll_con >> PLL3000_SDIV_SHIFT) & PLL3000_SDIV_MASK;
+
+	fvco *= (2 * (mdiv + 8));
+	do_div(fvco, pdiv << sdiv);
+
+	return (unsigned long)fvco;
+}
+
+static const struct clk_ops samsung_pll3000_clk_ops = {
+	.recalc_rate = samsung_pll3000_recalc_rate,
+};
+
+/*
  * PLL35xx Clock Type
  */
 /* Maximum lock time can be 270 * PDIV cycles */
@@ -564,7 +631,9 @@ static const struct clk_ops samsung_pll46xx_clk_min_ops = {
 #define PLL6552_PDIV_MASK	0x3f
 #define PLL6552_SDIV_MASK	0x7
 #define PLL6552_MDIV_SHIFT	16
+#define PLL6552_MDIV_SHIFT_2416	14
 #define PLL6552_PDIV_SHIFT	8
+#define PLL6552_PDIV_SHIFT_2416	5
 #define PLL6552_SDIV_SHIFT	0
 
 static unsigned long samsung_pll6552_recalc_rate(struct clk_hw *hw,
@@ -575,8 +644,13 @@ static unsigned long samsung_pll6552_recalc_rate(struct clk_hw *hw,
 	u64 fvco = parent_rate;
 
 	pll_con = __raw_readl(pll->con_reg);
-	mdiv = (pll_con >> PLL6552_MDIV_SHIFT) & PLL6552_MDIV_MASK;
-	pdiv = (pll_con >> PLL6552_PDIV_SHIFT) & PLL6552_PDIV_MASK;
+	if (pll->type == pll_6552_s3c2416) {
+		mdiv = (pll_con >> PLL6552_MDIV_SHIFT_2416) & PLL6552_MDIV_MASK;
+		pdiv = (pll_con >> PLL6552_PDIV_SHIFT_2416) & PLL6552_PDIV_MASK;
+	} else {
+		mdiv = (pll_con >> PLL6552_MDIV_SHIFT) & PLL6552_MDIV_MASK;
+		pdiv = (pll_con >> PLL6552_PDIV_SHIFT) & PLL6552_PDIV_MASK;
+	}
 	sdiv = (pll_con >> PLL6552_SDIV_SHIFT) & PLL6552_SDIV_MASK;
 
 	fvco *= mdiv;
@@ -628,6 +702,169 @@ static const struct clk_ops samsung_pll6553_clk_ops = {
 };
 
 /*
+ * PLL Clock Type of S3C24XX before S3C2443
+ */
+
+#define PLLS3C2410_MDIV_MASK		(0xff)
+#define PLLS3C2410_PDIV_MASK		(0x1f)
+#define PLLS3C2410_SDIV_MASK		(0x3)
+#define PLLS3C2410_MDIV_SHIFT		(12)
+#define PLLS3C2410_PDIV_SHIFT		(4)
+#define PLLS3C2410_SDIV_SHIFT		(0)
+
+#define PLLS3C2410_ENABLE_REG_OFFSET	0x10
+
+static unsigned long samsung_s3c2410_pll_recalc_rate(struct clk_hw *hw,
+					unsigned long parent_rate)
+{
+	struct samsung_clk_pll *pll = to_clk_pll(hw);
+	u32 pll_con, mdiv, pdiv, sdiv;
+	u64 fvco = parent_rate;
+
+	pll_con = __raw_readl(pll->con_reg);
+	mdiv = (pll_con >> PLLS3C2410_MDIV_SHIFT) & PLLS3C2410_MDIV_MASK;
+	pdiv = (pll_con >> PLLS3C2410_PDIV_SHIFT) & PLLS3C2410_PDIV_MASK;
+	sdiv = (pll_con >> PLLS3C2410_SDIV_SHIFT) & PLLS3C2410_SDIV_MASK;
+
+	fvco *= (mdiv + 8);
+	do_div(fvco, (pdiv + 2) << sdiv);
+
+	return (unsigned int)fvco;
+}
+
+static unsigned long samsung_s3c2440_mpll_recalc_rate(struct clk_hw *hw,
+					unsigned long parent_rate)
+{
+	struct samsung_clk_pll *pll = to_clk_pll(hw);
+	u32 pll_con, mdiv, pdiv, sdiv;
+	u64 fvco = parent_rate;
+
+	pll_con = __raw_readl(pll->con_reg);
+	mdiv = (pll_con >> PLLS3C2410_MDIV_SHIFT) & PLLS3C2410_MDIV_MASK;
+	pdiv = (pll_con >> PLLS3C2410_PDIV_SHIFT) & PLLS3C2410_PDIV_MASK;
+	sdiv = (pll_con >> PLLS3C2410_SDIV_SHIFT) & PLLS3C2410_SDIV_MASK;
+
+	fvco *= (2 * (mdiv + 8));
+	do_div(fvco, (pdiv + 2) << sdiv);
+
+	return (unsigned int)fvco;
+}
+
+static int samsung_s3c2410_pll_set_rate(struct clk_hw *hw, unsigned long drate,
+					unsigned long prate)
+{
+	struct samsung_clk_pll *pll = to_clk_pll(hw);
+	const struct samsung_pll_rate_table *rate;
+	u32 tmp;
+
+	/* Get required rate settings from table */
+	rate = samsung_get_pll_settings(pll, drate);
+	if (!rate) {
+		pr_err("%s: Invalid rate : %lu for pll clk %s\n", __func__,
+			drate, __clk_get_name(hw->clk));
+		return -EINVAL;
+	}
+
+	tmp = __raw_readl(pll->con_reg);
+
+	/* Change PLL PMS values */
+	tmp &= ~((PLLS3C2410_MDIV_MASK << PLLS3C2410_MDIV_SHIFT) |
+			(PLLS3C2410_PDIV_MASK << PLLS3C2410_PDIV_SHIFT) |
+			(PLLS3C2410_SDIV_MASK << PLLS3C2410_SDIV_SHIFT));
+	tmp |= (rate->mdiv << PLLS3C2410_MDIV_SHIFT) |
+			(rate->pdiv << PLLS3C2410_PDIV_SHIFT) |
+			(rate->sdiv << PLLS3C2410_SDIV_SHIFT);
+	__raw_writel(tmp, pll->con_reg);
+
+	/* Time to settle according to the manual */
+	udelay(300);
+
+	return 0;
+}
+
+static int samsung_s3c2410_pll_enable(struct clk_hw *hw, int bit, bool enable)
+{
+	struct samsung_clk_pll *pll = to_clk_pll(hw);
+	u32 pll_en = __raw_readl(pll->lock_reg + PLLS3C2410_ENABLE_REG_OFFSET);
+	u32 pll_en_orig = pll_en;
+
+	if (enable)
+		pll_en &= ~BIT(bit);
+	else
+		pll_en |= BIT(bit);
+
+	__raw_writel(pll_en, pll->lock_reg + PLLS3C2410_ENABLE_REG_OFFSET);
+
+	/* if we started the UPLL, then allow to settle */
+	if (enable && (pll_en_orig & BIT(bit)))
+		udelay(300);
+
+	return 0;
+}
+
+static int samsung_s3c2410_mpll_enable(struct clk_hw *hw)
+{
+	return samsung_s3c2410_pll_enable(hw, 5, true);
+}
+
+static void samsung_s3c2410_mpll_disable(struct clk_hw *hw)
+{
+	samsung_s3c2410_pll_enable(hw, 5, false);
+}
+
+static int samsung_s3c2410_upll_enable(struct clk_hw *hw)
+{
+	return samsung_s3c2410_pll_enable(hw, 7, true);
+}
+
+static void samsung_s3c2410_upll_disable(struct clk_hw *hw)
+{
+	samsung_s3c2410_pll_enable(hw, 7, false);
+}
+
+static const struct clk_ops samsung_s3c2410_mpll_clk_min_ops = {
+	.recalc_rate = samsung_s3c2410_pll_recalc_rate,
+	.enable = samsung_s3c2410_mpll_enable,
+	.disable = samsung_s3c2410_mpll_disable,
+};
+
+static const struct clk_ops samsung_s3c2410_upll_clk_min_ops = {
+	.recalc_rate = samsung_s3c2410_pll_recalc_rate,
+	.enable = samsung_s3c2410_upll_enable,
+	.disable = samsung_s3c2410_upll_disable,
+};
+
+static const struct clk_ops samsung_s3c2440_mpll_clk_min_ops = {
+	.recalc_rate = samsung_s3c2440_mpll_recalc_rate,
+	.enable = samsung_s3c2410_mpll_enable,
+	.disable = samsung_s3c2410_mpll_disable,
+};
+
+static const struct clk_ops samsung_s3c2410_mpll_clk_ops = {
+	.recalc_rate = samsung_s3c2410_pll_recalc_rate,
+	.enable = samsung_s3c2410_mpll_enable,
+	.disable = samsung_s3c2410_mpll_disable,
+	.round_rate = samsung_pll_round_rate,
+	.set_rate = samsung_s3c2410_pll_set_rate,
+};
+
+static const struct clk_ops samsung_s3c2410_upll_clk_ops = {
+	.recalc_rate = samsung_s3c2410_pll_recalc_rate,
+	.enable = samsung_s3c2410_upll_enable,
+	.disable = samsung_s3c2410_upll_disable,
+	.round_rate = samsung_pll_round_rate,
+	.set_rate = samsung_s3c2410_pll_set_rate,
+};
+
+static const struct clk_ops samsung_s3c2440_mpll_clk_ops = {
+	.recalc_rate = samsung_s3c2440_mpll_recalc_rate,
+	.enable = samsung_s3c2410_mpll_enable,
+	.disable = samsung_s3c2410_mpll_disable,
+	.round_rate = samsung_pll_round_rate,
+	.set_rate = samsung_s3c2410_pll_set_rate,
+};
+
+/*
  * PLL2550x Clock Type
  */
 
@@ -710,8 +947,206 @@ struct clk * __init samsung_clk_register_pll2550x(const char *name,
 	return clk;
 }
 
-static void __init _samsung_clk_register_pll(struct samsung_pll_clock *pll_clk,
-						void __iomem *base)
+/*
+ * PLL2550xx Clock Type
+ */
+
+/* Maximum lock time can be 270 * PDIV cycles */
+#define PLL2550XX_LOCK_FACTOR 270
+
+#define PLL2550XX_M_MASK		0x3FF
+#define PLL2550XX_P_MASK		0x3F
+#define PLL2550XX_S_MASK		0x7
+#define PLL2550XX_LOCK_STAT_MASK	0x1
+#define PLL2550XX_M_SHIFT		9
+#define PLL2550XX_P_SHIFT		3
+#define PLL2550XX_S_SHIFT		0
+#define PLL2550XX_LOCK_STAT_SHIFT	21
+
+static unsigned long samsung_pll2550xx_recalc_rate(struct clk_hw *hw,
+				unsigned long parent_rate)
+{
+	struct samsung_clk_pll *pll = to_clk_pll(hw);
+	u32 mdiv, pdiv, sdiv, pll_con;
+	u64 fvco = parent_rate;
+
+	pll_con = __raw_readl(pll->con_reg);
+	mdiv = (pll_con >> PLL2550XX_M_SHIFT) & PLL2550XX_M_MASK;
+	pdiv = (pll_con >> PLL2550XX_P_SHIFT) & PLL2550XX_P_MASK;
+	sdiv = (pll_con >> PLL2550XX_S_SHIFT) & PLL2550XX_S_MASK;
+
+	fvco *= mdiv;
+	do_div(fvco, (pdiv << sdiv));
+
+	return (unsigned long)fvco;
+}
+
+static inline bool samsung_pll2550xx_mp_change(u32 mdiv, u32 pdiv, u32 pll_con)
+{
+	u32 old_mdiv, old_pdiv;
+
+	old_mdiv = (pll_con >> PLL2550XX_M_SHIFT) & PLL2550XX_M_MASK;
+	old_pdiv = (pll_con >> PLL2550XX_P_SHIFT) & PLL2550XX_P_MASK;
+
+	return mdiv != old_mdiv || pdiv != old_pdiv;
+}
+
+static int samsung_pll2550xx_set_rate(struct clk_hw *hw, unsigned long drate,
+					unsigned long prate)
+{
+	struct samsung_clk_pll *pll = to_clk_pll(hw);
+	const struct samsung_pll_rate_table *rate;
+	u32 tmp;
+
+	/* Get required rate settings from table */
+	rate = samsung_get_pll_settings(pll, drate);
+	if (!rate) {
+		pr_err("%s: Invalid rate : %lu for pll clk %s\n", __func__,
+			drate, __clk_get_name(hw->clk));
+		return -EINVAL;
+	}
+
+	tmp = __raw_readl(pll->con_reg);
+
+	if (!(samsung_pll2550xx_mp_change(rate->mdiv, rate->pdiv, tmp))) {
+		/* If only s change, change just s value only*/
+		tmp &= ~(PLL2550XX_S_MASK << PLL2550XX_S_SHIFT);
+		tmp |= rate->sdiv << PLL2550XX_S_SHIFT;
+		__raw_writel(tmp, pll->con_reg);
+
+		return 0;
+	}
+
+	/* Set PLL lock time. */
+	__raw_writel(rate->pdiv * PLL2550XX_LOCK_FACTOR, pll->lock_reg);
+
+	/* Change PLL PMS values */
+	tmp &= ~((PLL2550XX_M_MASK << PLL2550XX_M_SHIFT) |
+			(PLL2550XX_P_MASK << PLL2550XX_P_SHIFT) |
+			(PLL2550XX_S_MASK << PLL2550XX_S_SHIFT));
+	tmp |= (rate->mdiv << PLL2550XX_M_SHIFT) |
+			(rate->pdiv << PLL2550XX_P_SHIFT) |
+			(rate->sdiv << PLL2550XX_S_SHIFT);
+	__raw_writel(tmp, pll->con_reg);
+
+	/* wait_lock_time */
+	do {
+		cpu_relax();
+		tmp = __raw_readl(pll->con_reg);
+	} while (!(tmp & (PLL2550XX_LOCK_STAT_MASK
+			<< PLL2550XX_LOCK_STAT_SHIFT)));
+
+	return 0;
+}
+
+static const struct clk_ops samsung_pll2550xx_clk_ops = {
+	.recalc_rate = samsung_pll2550xx_recalc_rate,
+	.round_rate = samsung_pll_round_rate,
+	.set_rate = samsung_pll2550xx_set_rate,
+};
+
+static const struct clk_ops samsung_pll2550xx_clk_min_ops = {
+	.recalc_rate = samsung_pll2550xx_recalc_rate,
+};
+
+/*
+ * PLL2650XX Clock Type
+ */
+
+/* Maximum lock time can be 3000 * PDIV cycles */
+#define PLL2650XX_LOCK_FACTOR 3000
+
+#define PLL2650XX_MDIV_SHIFT		9
+#define PLL2650XX_PDIV_SHIFT		3
+#define PLL2650XX_SDIV_SHIFT		0
+#define PLL2650XX_KDIV_SHIFT		0
+#define PLL2650XX_MDIV_MASK		0x1ff
+#define PLL2650XX_PDIV_MASK		0x3f
+#define PLL2650XX_SDIV_MASK		0x7
+#define PLL2650XX_KDIV_MASK		0xffff
+#define PLL2650XX_PLL_ENABLE_SHIFT	23
+#define PLL2650XX_PLL_LOCKTIME_SHIFT	21
+#define PLL2650XX_PLL_FOUTMASK_SHIFT	31
+
+static unsigned long samsung_pll2650xx_recalc_rate(struct clk_hw *hw,
+				unsigned long parent_rate)
+{
+	struct samsung_clk_pll *pll = to_clk_pll(hw);
+	u32 mdiv, pdiv, sdiv, pll_con0, pll_con2;
+	s16 kdiv;
+	u64 fvco = parent_rate;
+
+	pll_con0 = __raw_readl(pll->con_reg);
+	pll_con2 = __raw_readl(pll->con_reg + 8);
+	mdiv = (pll_con0 >> PLL2650XX_MDIV_SHIFT) & PLL2650XX_MDIV_MASK;
+	pdiv = (pll_con0 >> PLL2650XX_PDIV_SHIFT) & PLL2650XX_PDIV_MASK;
+	sdiv = (pll_con0 >> PLL2650XX_SDIV_SHIFT) & PLL2650XX_SDIV_MASK;
+	kdiv = (s16)(pll_con2 & PLL2650XX_KDIV_MASK);
+
+	fvco *= (mdiv << 16) + kdiv;
+	do_div(fvco, (pdiv << sdiv));
+	fvco >>= 16;
+
+	return (unsigned long)fvco;
+}
+
+static int samsung_pll2650xx_set_rate(struct clk_hw *hw, unsigned long drate,
+					unsigned long parent_rate)
+{
+	struct samsung_clk_pll *pll = to_clk_pll(hw);
+	u32 tmp, pll_con0, pll_con2;
+	const struct samsung_pll_rate_table *rate;
+
+	rate = samsung_get_pll_settings(pll, drate);
+	if (!rate) {
+		pr_err("%s: Invalid rate : %lu for pll clk %s\n", __func__,
+			drate, __clk_get_name(hw->clk));
+		return -EINVAL;
+	}
+
+	pll_con0 = __raw_readl(pll->con_reg);
+	pll_con2 = __raw_readl(pll->con_reg + 8);
+
+	 /* Change PLL PMS values */
+	pll_con0 &= ~(PLL2650XX_MDIV_MASK << PLL2650XX_MDIV_SHIFT |
+			PLL2650XX_PDIV_MASK << PLL2650XX_PDIV_SHIFT |
+			PLL2650XX_SDIV_MASK << PLL2650XX_SDIV_SHIFT);
+	pll_con0 |= rate->mdiv << PLL2650XX_MDIV_SHIFT;
+	pll_con0 |= rate->pdiv << PLL2650XX_PDIV_SHIFT;
+	pll_con0 |= rate->sdiv << PLL2650XX_SDIV_SHIFT;
+	pll_con0 |= 1 << PLL2650XX_PLL_ENABLE_SHIFT;
+	pll_con0 |= 1 << PLL2650XX_PLL_FOUTMASK_SHIFT;
+
+	pll_con2 &= ~(PLL2650XX_KDIV_MASK << PLL2650XX_KDIV_SHIFT);
+	pll_con2 |= ((~(rate->kdiv) + 1) & PLL2650XX_KDIV_MASK)
+			<< PLL2650XX_KDIV_SHIFT;
+
+	/* Set PLL lock time. */
+	__raw_writel(PLL2650XX_LOCK_FACTOR * rate->pdiv, pll->lock_reg);
+
+	__raw_writel(pll_con0, pll->con_reg);
+	__raw_writel(pll_con2, pll->con_reg + 8);
+
+	do {
+		tmp = __raw_readl(pll->con_reg);
+	} while (!(tmp & (0x1 << PLL2650XX_PLL_LOCKTIME_SHIFT)));
+
+	return 0;
+}
+
+static const struct clk_ops samsung_pll2650xx_clk_ops = {
+	.recalc_rate = samsung_pll2650xx_recalc_rate,
+	.set_rate = samsung_pll2650xx_set_rate,
+	.round_rate = samsung_pll_round_rate,
+};
+
+static const struct clk_ops samsung_pll2650xx_clk_min_ops = {
+	.recalc_rate = samsung_pll2650xx_recalc_rate,
+};
+
+static void __init _samsung_clk_register_pll(struct samsung_clk_provider *ctx,
+				struct samsung_pll_clock *pll_clk,
+				void __iomem *base)
 {
 	struct samsung_clk_pll *pll;
 	struct clk *clk;
@@ -746,6 +1181,12 @@ static void __init _samsung_clk_register_pll(struct samsung_pll_clock *pll_clk,
 	}
 
 	switch (pll_clk->type) {
+	case pll_2126:
+		init.ops = &samsung_pll2126_clk_ops;
+		break;
+	case pll_3000:
+		init.ops = &samsung_pll3000_clk_ops;
+		break;
 	/* clk_ops for 35xx and 2550 are similar */
 	case pll_35xx:
 	case pll_2550:
@@ -773,6 +1214,7 @@ static void __init _samsung_clk_register_pll(struct samsung_pll_clock *pll_clk,
 			init.ops = &samsung_pll36xx_clk_ops;
 		break;
 	case pll_6552:
+	case pll_6552_s3c2416:
 		init.ops = &samsung_pll6552_clk_ops;
 		break;
 	case pll_6553:
@@ -786,6 +1228,36 @@ static void __init _samsung_clk_register_pll(struct samsung_pll_clock *pll_clk,
 		else
 			init.ops = &samsung_pll46xx_clk_ops;
 		break;
+	case pll_s3c2410_mpll:
+		if (!pll->rate_table)
+			init.ops = &samsung_s3c2410_mpll_clk_min_ops;
+		else
+			init.ops = &samsung_s3c2410_mpll_clk_ops;
+		break;
+	case pll_s3c2410_upll:
+		if (!pll->rate_table)
+			init.ops = &samsung_s3c2410_upll_clk_min_ops;
+		else
+			init.ops = &samsung_s3c2410_upll_clk_ops;
+		break;
+	case pll_s3c2440_mpll:
+		if (!pll->rate_table)
+			init.ops = &samsung_s3c2440_mpll_clk_min_ops;
+		else
+			init.ops = &samsung_s3c2440_mpll_clk_ops;
+		break;
+	case pll_2550xx:
+		if (!pll->rate_table)
+			init.ops = &samsung_pll2550xx_clk_min_ops;
+		else
+			init.ops = &samsung_pll2550xx_clk_ops;
+		break;
+	case pll_2650xx:
+		if (!pll->rate_table)
+			init.ops = &samsung_pll2650xx_clk_min_ops;
+		else
+			init.ops = &samsung_pll2650xx_clk_ops;
+		break;
 	default:
 		pr_warn("%s: Unknown pll type for pll clk %s\n",
 			__func__, pll_clk->name);
@@ -804,7 +1276,7 @@ static void __init _samsung_clk_register_pll(struct samsung_pll_clock *pll_clk,
 		return;
 	}
 
-	samsung_clk_add_lookup(clk, pll_clk->id);
+	samsung_clk_add_lookup(ctx, clk, pll_clk->id);
 
 	if (!pll_clk->alias)
 		return;
@@ -815,11 +1287,12 @@ static void __init _samsung_clk_register_pll(struct samsung_pll_clock *pll_clk,
 			__func__, pll_clk->name, ret);
 }
 
-void __init samsung_clk_register_pll(struct samsung_pll_clock *pll_list,
-				unsigned int nr_pll, void __iomem *base)
+void __init samsung_clk_register_pll(struct samsung_clk_provider *ctx,
+			struct samsung_pll_clock *pll_list,
+			unsigned int nr_pll, void __iomem *base)
 {
 	int cnt;
 
 	for (cnt = 0; cnt < nr_pll; cnt++)
-		_samsung_clk_register_pll(&pll_list[cnt], base);
+		_samsung_clk_register_pll(ctx, &pll_list[cnt], base);
 }
diff --git a/drivers/clk/samsung/clk-pll.h b/drivers/clk/samsung/clk-pll.h
index 6c39030080f..c0ed4d41fd9 100644
--- a/drivers/clk/samsung/clk-pll.h
+++ b/drivers/clk/samsung/clk-pll.h
@@ -13,6 +13,8 @@
 #define __SAMSUNG_CLK_PLL_H
 
 enum samsung_pll_type {
+	pll_2126,
+	pll_3000,
 	pll_35xx,
 	pll_36xx,
 	pll_2550,
@@ -24,7 +26,13 @@ enum samsung_pll_type {
 	pll_4650,
 	pll_4650c,
 	pll_6552,
+	pll_6552_s3c2416,
 	pll_6553,
+	pll_s3c2410_mpll,
+	pll_s3c2410_upll,
+	pll_s3c2440_mpll,
+	pll_2550xx,
+	pll_2650xx,
 };
 
 #define PLL_35XX_RATE(_rate, _m, _p, _s)			\
diff --git a/drivers/clk/samsung/clk-s3c2410-dclk.c b/drivers/clk/samsung/clk-s3c2410-dclk.c
new file mode 100644
index 00000000000..0449cc0458e
--- /dev/null
+++ b/drivers/clk/samsung/clk-s3c2410-dclk.c
@@ -0,0 +1,440 @@
+/*
+ * Copyright (c) 2013 Heiko Stuebner <heiko@sntech.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.
+ *
+ * Common Clock Framework support for s3c24xx external clock output.
+ */
+
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include "clk.h"
+
+/* legacy access to misccr, until dt conversion is finished */
+#include <mach/hardware.h>
+#include <mach/regs-gpio.h>
+
+#define MUX_DCLK0	0
+#define MUX_DCLK1	1
+#define DIV_DCLK0	2
+#define DIV_DCLK1	3
+#define GATE_DCLK0	4
+#define GATE_DCLK1	5
+#define MUX_CLKOUT0	6
+#define MUX_CLKOUT1	7
+#define DCLK_MAX_CLKS	(MUX_CLKOUT1 + 1)
+
+enum supported_socs {
+	S3C2410,
+	S3C2412,
+	S3C2440,
+	S3C2443,
+};
+
+struct s3c24xx_dclk_drv_data {
+	const char **clkout0_parent_names;
+	int clkout0_num_parents;
+	const char **clkout1_parent_names;
+	int clkout1_num_parents;
+	const char **mux_parent_names;
+	int mux_num_parents;
+};
+
+/*
+ * Clock for output-parent selection in misccr
+ */
+
+struct s3c24xx_clkout {
+	struct clk_hw		hw;
+	u32			mask;
+	u8			shift;
+};
+
+#define to_s3c24xx_clkout(_hw) container_of(_hw, struct s3c24xx_clkout, hw)
+
+static u8 s3c24xx_clkout_get_parent(struct clk_hw *hw)
+{
+	struct s3c24xx_clkout *clkout = to_s3c24xx_clkout(hw);
+	int num_parents = __clk_get_num_parents(hw->clk);
+	u32 val;
+
+	val = readl_relaxed(S3C24XX_MISCCR) >> clkout->shift;
+	val >>= clkout->shift;
+	val &= clkout->mask;
+
+	if (val >= num_parents)
+		return -EINVAL;
+
+	return val;
+}
+
+static int s3c24xx_clkout_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct s3c24xx_clkout *clkout = to_s3c24xx_clkout(hw);
+	int ret = 0;
+
+	s3c2410_modify_misccr((clkout->mask << clkout->shift),
+			      (index << clkout->shift));
+
+	return ret;
+}
+
+const struct clk_ops s3c24xx_clkout_ops = {
+	.get_parent = s3c24xx_clkout_get_parent,
+	.set_parent = s3c24xx_clkout_set_parent,
+	.determine_rate = __clk_mux_determine_rate,
+};
+
+struct clk *s3c24xx_register_clkout(struct device *dev, const char *name,
+		const char **parent_names, u8 num_parents,
+		u8 shift, u32 mask)
+{
+	struct s3c24xx_clkout *clkout;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	/* allocate the clkout */
+	clkout = kzalloc(sizeof(*clkout), GFP_KERNEL);
+	if (!clkout)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &s3c24xx_clkout_ops;
+	init.flags = CLK_IS_BASIC;
+	init.parent_names = parent_names;
+	init.num_parents = num_parents;
+
+	clkout->shift = shift;
+	clkout->mask = mask;
+	clkout->hw.init = &init;
+
+	clk = clk_register(dev, &clkout->hw);
+
+	return clk;
+}
+
+/*
+ * dclk and clkout init
+ */
+
+struct s3c24xx_dclk {
+	struct device *dev;
+	void __iomem *base;
+	struct clk_onecell_data clk_data;
+	struct notifier_block dclk0_div_change_nb;
+	struct notifier_block dclk1_div_change_nb;
+	spinlock_t dclk_lock;
+	unsigned long reg_save;
+};
+
+#define to_s3c24xx_dclk0(x) \
+		container_of(x, struct s3c24xx_dclk, dclk0_div_change_nb)
+
+#define to_s3c24xx_dclk1(x) \
+		container_of(x, struct s3c24xx_dclk, dclk1_div_change_nb)
+
+static const char *dclk_s3c2410_p[] = { "pclk", "uclk" };
+static const char *clkout0_s3c2410_p[] = { "mpll", "upll", "fclk", "hclk", "pclk",
+			     "gate_dclk0" };
+static const char *clkout1_s3c2410_p[] = { "mpll", "upll", "fclk", "hclk", "pclk",
+			     "gate_dclk1" };
+
+static const char *clkout0_s3c2412_p[] = { "mpll", "upll", "rtc_clkout",
+			     "hclk", "pclk", "gate_dclk0" };
+static const char *clkout1_s3c2412_p[] = { "xti", "upll", "fclk", "hclk", "pclk",
+			     "gate_dclk1" };
+
+static const char *clkout0_s3c2440_p[] = { "xti", "upll", "fclk", "hclk", "pclk",
+			     "gate_dclk0" };
+static const char *clkout1_s3c2440_p[] = { "mpll", "upll", "rtc_clkout",
+			     "hclk", "pclk", "gate_dclk1" };
+
+static const char *dclk_s3c2443_p[] = { "pclk", "epll" };
+static const char *clkout0_s3c2443_p[] = { "xti", "epll", "armclk", "hclk", "pclk",
+			     "gate_dclk0" };
+static const char *clkout1_s3c2443_p[] = { "dummy", "epll", "rtc_clkout",
+			     "hclk", "pclk", "gate_dclk1" };
+
+#define DCLKCON_DCLK_DIV_MASK		0xf
+#define DCLKCON_DCLK0_DIV_SHIFT		4
+#define DCLKCON_DCLK0_CMP_SHIFT		8
+#define DCLKCON_DCLK1_DIV_SHIFT		20
+#define DCLKCON_DCLK1_CMP_SHIFT		24
+
+static void s3c24xx_dclk_update_cmp(struct s3c24xx_dclk *s3c24xx_dclk,
+				    int div_shift, int cmp_shift)
+{
+	unsigned long flags = 0;
+	u32 dclk_con, div, cmp;
+
+	spin_lock_irqsave(&s3c24xx_dclk->dclk_lock, flags);
+
+	dclk_con = readl_relaxed(s3c24xx_dclk->base);
+
+	div = ((dclk_con >> div_shift) & DCLKCON_DCLK_DIV_MASK) + 1;
+	cmp = ((div + 1) / 2) - 1;
+
+	dclk_con &= ~(DCLKCON_DCLK_DIV_MASK << cmp_shift);
+	dclk_con |= (cmp << cmp_shift);
+
+	writel_relaxed(dclk_con, s3c24xx_dclk->base);
+
+	spin_unlock_irqrestore(&s3c24xx_dclk->dclk_lock, flags);
+}
+
+static int s3c24xx_dclk0_div_notify(struct notifier_block *nb,
+			       unsigned long event, void *data)
+{
+	struct s3c24xx_dclk *s3c24xx_dclk = to_s3c24xx_dclk0(nb);
+
+	if (event == POST_RATE_CHANGE) {
+		s3c24xx_dclk_update_cmp(s3c24xx_dclk,
+			DCLKCON_DCLK0_DIV_SHIFT, DCLKCON_DCLK0_CMP_SHIFT);
+	}
+
+	return NOTIFY_DONE;
+}
+
+static int s3c24xx_dclk1_div_notify(struct notifier_block *nb,
+			       unsigned long event, void *data)
+{
+	struct s3c24xx_dclk *s3c24xx_dclk = to_s3c24xx_dclk1(nb);
+
+	if (event == POST_RATE_CHANGE) {
+		s3c24xx_dclk_update_cmp(s3c24xx_dclk,
+			DCLKCON_DCLK1_DIV_SHIFT, DCLKCON_DCLK1_CMP_SHIFT);
+	}
+
+	return NOTIFY_DONE;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int s3c24xx_dclk_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct s3c24xx_dclk *s3c24xx_dclk = platform_get_drvdata(pdev);
+
+	s3c24xx_dclk->reg_save = readl_relaxed(s3c24xx_dclk->base);
+	return 0;
+}
+
+static int s3c24xx_dclk_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct s3c24xx_dclk *s3c24xx_dclk = platform_get_drvdata(pdev);
+
+	writel_relaxed(s3c24xx_dclk->reg_save, s3c24xx_dclk->base);
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(s3c24xx_dclk_pm_ops,
+			 s3c24xx_dclk_suspend, s3c24xx_dclk_resume);
+
+static int s3c24xx_dclk_probe(struct platform_device *pdev)
+{
+	struct s3c24xx_dclk *s3c24xx_dclk;
+	struct resource *mem;
+	struct clk **clk_table;
+	struct s3c24xx_dclk_drv_data *dclk_variant;
+	int ret, i;
+
+	s3c24xx_dclk = devm_kzalloc(&pdev->dev, sizeof(*s3c24xx_dclk),
+				    GFP_KERNEL);
+	if (!s3c24xx_dclk)
+		return -ENOMEM;
+
+	s3c24xx_dclk->dev = &pdev->dev;
+	platform_set_drvdata(pdev, s3c24xx_dclk);
+	spin_lock_init(&s3c24xx_dclk->dclk_lock);
+
+	clk_table = devm_kzalloc(&pdev->dev,
+				 sizeof(struct clk *) * DCLK_MAX_CLKS,
+				 GFP_KERNEL);
+	if (!clk_table)
+		return -ENOMEM;
+
+	s3c24xx_dclk->clk_data.clks = clk_table;
+	s3c24xx_dclk->clk_data.clk_num = DCLK_MAX_CLKS;
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	s3c24xx_dclk->base = devm_ioremap_resource(&pdev->dev, mem);
+	if (IS_ERR(s3c24xx_dclk->base))
+		return PTR_ERR(s3c24xx_dclk->base);
+
+	dclk_variant = (struct s3c24xx_dclk_drv_data *)
+				platform_get_device_id(pdev)->driver_data;
+
+
+	clk_table[MUX_DCLK0] = clk_register_mux(&pdev->dev, "mux_dclk0",
+				dclk_variant->mux_parent_names,
+				dclk_variant->mux_num_parents, 0,
+				s3c24xx_dclk->base, 1, 1, 0,
+				&s3c24xx_dclk->dclk_lock);
+	clk_table[MUX_DCLK1] = clk_register_mux(&pdev->dev, "mux_dclk1",
+				dclk_variant->mux_parent_names,
+				dclk_variant->mux_num_parents, 0,
+				s3c24xx_dclk->base, 17, 1, 0,
+				&s3c24xx_dclk->dclk_lock);
+
+	clk_table[DIV_DCLK0] = clk_register_divider(&pdev->dev, "div_dclk0",
+				"mux_dclk0", 0, s3c24xx_dclk->base,
+				4, 4, 0, &s3c24xx_dclk->dclk_lock);
+	clk_table[DIV_DCLK1] = clk_register_divider(&pdev->dev, "div_dclk1",
+				"mux_dclk1", 0, s3c24xx_dclk->base,
+				20, 4, 0, &s3c24xx_dclk->dclk_lock);
+
+	clk_table[GATE_DCLK0] = clk_register_gate(&pdev->dev, "gate_dclk0",
+				"div_dclk0", CLK_SET_RATE_PARENT,
+				s3c24xx_dclk->base, 0, 0,
+				&s3c24xx_dclk->dclk_lock);
+	clk_table[GATE_DCLK1] = clk_register_gate(&pdev->dev, "gate_dclk1",
+				"div_dclk1", CLK_SET_RATE_PARENT,
+				s3c24xx_dclk->base, 16, 0,
+				&s3c24xx_dclk->dclk_lock);
+
+	clk_table[MUX_CLKOUT0] = s3c24xx_register_clkout(&pdev->dev,
+				"clkout0", dclk_variant->clkout0_parent_names,
+				dclk_variant->clkout0_num_parents, 4, 7);
+	clk_table[MUX_CLKOUT1] = s3c24xx_register_clkout(&pdev->dev,
+				"clkout1", dclk_variant->clkout1_parent_names,
+				dclk_variant->clkout1_num_parents, 8, 7);
+
+	for (i = 0; i < DCLK_MAX_CLKS; i++)
+		if (IS_ERR(clk_table[i])) {
+			dev_err(&pdev->dev, "clock %d failed to register\n", i);
+			ret = PTR_ERR(clk_table[i]);
+			goto err_clk_register;
+		}
+
+	ret = clk_register_clkdev(clk_table[MUX_DCLK0], "dclk0", NULL);
+	if (!ret)
+		ret = clk_register_clkdev(clk_table[MUX_DCLK1], "dclk1", NULL);
+	if (!ret)
+		ret = clk_register_clkdev(clk_table[MUX_CLKOUT0],
+					  "clkout0", NULL);
+	if (!ret)
+		ret = clk_register_clkdev(clk_table[MUX_CLKOUT1],
+					  "clkout1", NULL);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to register aliases, %d\n", ret);
+		goto err_clk_register;
+	}
+
+	s3c24xx_dclk->dclk0_div_change_nb.notifier_call =
+						s3c24xx_dclk0_div_notify;
+
+	s3c24xx_dclk->dclk1_div_change_nb.notifier_call =
+						s3c24xx_dclk1_div_notify;
+
+	ret = clk_notifier_register(clk_table[DIV_DCLK0],
+				    &s3c24xx_dclk->dclk0_div_change_nb);
+	if (ret)
+		goto err_clk_register;
+
+	ret = clk_notifier_register(clk_table[DIV_DCLK1],
+				    &s3c24xx_dclk->dclk1_div_change_nb);
+	if (ret)
+		goto err_dclk_notify;
+
+	return 0;
+
+err_dclk_notify:
+	clk_notifier_unregister(clk_table[DIV_DCLK0],
+				&s3c24xx_dclk->dclk0_div_change_nb);
+err_clk_register:
+	for (i = 0; i < DCLK_MAX_CLKS; i++)
+		if (clk_table[i] && !IS_ERR(clk_table[i]))
+			clk_unregister(clk_table[i]);
+
+	return ret;
+}
+
+static int s3c24xx_dclk_remove(struct platform_device *pdev)
+{
+	struct s3c24xx_dclk *s3c24xx_dclk = platform_get_drvdata(pdev);
+	struct clk **clk_table = s3c24xx_dclk->clk_data.clks;
+	int i;
+
+	clk_notifier_unregister(clk_table[DIV_DCLK1],
+				&s3c24xx_dclk->dclk1_div_change_nb);
+	clk_notifier_unregister(clk_table[DIV_DCLK0],
+				&s3c24xx_dclk->dclk0_div_change_nb);
+
+	for (i = 0; i < DCLK_MAX_CLKS; i++)
+		clk_unregister(clk_table[i]);
+
+	return 0;
+}
+
+static struct s3c24xx_dclk_drv_data dclk_variants[] = {
+	[S3C2410] = {
+		.clkout0_parent_names = clkout0_s3c2410_p,
+		.clkout0_num_parents = ARRAY_SIZE(clkout0_s3c2410_p),
+		.clkout1_parent_names = clkout1_s3c2410_p,
+		.clkout1_num_parents = ARRAY_SIZE(clkout1_s3c2410_p),
+		.mux_parent_names = dclk_s3c2410_p,
+		.mux_num_parents = ARRAY_SIZE(dclk_s3c2410_p),
+	},
+	[S3C2412] = {
+		.clkout0_parent_names = clkout0_s3c2412_p,
+		.clkout0_num_parents = ARRAY_SIZE(clkout0_s3c2412_p),
+		.clkout1_parent_names = clkout1_s3c2412_p,
+		.clkout1_num_parents = ARRAY_SIZE(clkout1_s3c2412_p),
+		.mux_parent_names = dclk_s3c2410_p,
+		.mux_num_parents = ARRAY_SIZE(dclk_s3c2410_p),
+	},
+	[S3C2440] = {
+		.clkout0_parent_names = clkout0_s3c2440_p,
+		.clkout0_num_parents = ARRAY_SIZE(clkout0_s3c2440_p),
+		.clkout1_parent_names = clkout1_s3c2440_p,
+		.clkout1_num_parents = ARRAY_SIZE(clkout1_s3c2440_p),
+		.mux_parent_names = dclk_s3c2410_p,
+		.mux_num_parents = ARRAY_SIZE(dclk_s3c2410_p),
+	},
+	[S3C2443] = {
+		.clkout0_parent_names = clkout0_s3c2443_p,
+		.clkout0_num_parents = ARRAY_SIZE(clkout0_s3c2443_p),
+		.clkout1_parent_names = clkout1_s3c2443_p,
+		.clkout1_num_parents = ARRAY_SIZE(clkout1_s3c2443_p),
+		.mux_parent_names = dclk_s3c2443_p,
+		.mux_num_parents = ARRAY_SIZE(dclk_s3c2443_p),
+	},
+};
+
+static struct platform_device_id s3c24xx_dclk_driver_ids[] = {
+	{
+		.name		= "s3c2410-dclk",
+		.driver_data	= (kernel_ulong_t)&dclk_variants[S3C2410],
+	}, {
+		.name		= "s3c2412-dclk",
+		.driver_data	= (kernel_ulong_t)&dclk_variants[S3C2412],
+	}, {
+		.name		= "s3c2440-dclk",
+		.driver_data	= (kernel_ulong_t)&dclk_variants[S3C2440],
+	}, {
+		.name		= "s3c2443-dclk",
+		.driver_data	= (kernel_ulong_t)&dclk_variants[S3C2443],
+	},
+	{ }
+};
+
+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,
+	.remove = s3c24xx_dclk_remove,
+	.id_table = s3c24xx_dclk_driver_ids,
+};
+module_platform_driver(s3c24xx_dclk_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Heiko Stuebner <heiko@sntech.de>");
+MODULE_DESCRIPTION("Driver for the S3C24XX external clock outputs");
diff --git a/drivers/clk/samsung/clk-s3c2410.c b/drivers/clk/samsung/clk-s3c2410.c
new file mode 100644
index 00000000000..ba0716801db
--- /dev/null
+++ b/drivers/clk/samsung/clk-s3c2410.c
@@ -0,0 +1,482 @@
+/*
+ * Copyright (c) 2013 Heiko Stuebner <heiko@sntech.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.
+ *
+ * Common Clock Framework support for S3C2410 and following SoCs.
+ */
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/syscore_ops.h>
+
+#include <dt-bindings/clock/s3c2410.h>
+
+#include "clk.h"
+#include "clk-pll.h"
+
+#define LOCKTIME	0x00
+#define MPLLCON		0x04
+#define UPLLCON		0x08
+#define CLKCON		0x0c
+#define CLKSLOW		0x10
+#define CLKDIVN		0x14
+#define CAMDIVN		0x18
+
+/* the soc types */
+enum supported_socs {
+	S3C2410,
+	S3C2440,
+	S3C2442,
+};
+
+/* list of PLLs to be registered */
+enum s3c2410_plls {
+	mpll, upll,
+};
+
+static void __iomem *reg_base;
+
+#ifdef CONFIG_PM_SLEEP
+static struct samsung_clk_reg_dump *s3c2410_save;
+
+/*
+ * list of controller registers to be saved and restored during a
+ * suspend/resume cycle.
+ */
+static unsigned long s3c2410_clk_regs[] __initdata = {
+	LOCKTIME,
+	MPLLCON,
+	UPLLCON,
+	CLKCON,
+	CLKSLOW,
+	CLKDIVN,
+	CAMDIVN,
+};
+
+static int s3c2410_clk_suspend(void)
+{
+	samsung_clk_save(reg_base, s3c2410_save,
+				ARRAY_SIZE(s3c2410_clk_regs));
+
+	return 0;
+}
+
+static void s3c2410_clk_resume(void)
+{
+	samsung_clk_restore(reg_base, s3c2410_save,
+				ARRAY_SIZE(s3c2410_clk_regs));
+}
+
+static struct syscore_ops s3c2410_clk_syscore_ops = {
+	.suspend = s3c2410_clk_suspend,
+	.resume = s3c2410_clk_resume,
+};
+
+static void s3c2410_clk_sleep_init(void)
+{
+	s3c2410_save = samsung_clk_alloc_reg_dump(s3c2410_clk_regs,
+						ARRAY_SIZE(s3c2410_clk_regs));
+	if (!s3c2410_save) {
+		pr_warn("%s: failed to allocate sleep save data, no sleep support!\n",
+			__func__);
+		return;
+	}
+
+	register_syscore_ops(&s3c2410_clk_syscore_ops);
+	return;
+}
+#else
+static void s3c2410_clk_sleep_init(void) {}
+#endif
+
+PNAME(fclk_p) = { "mpll", "div_slow" };
+
+struct samsung_mux_clock s3c2410_common_muxes[] __initdata = {
+	MUX(FCLK, "fclk", fclk_p, CLKSLOW, 4, 1),
+};
+
+static struct clk_div_table divslow_d[] = {
+	{ .val = 0, .div = 1 },
+	{ .val = 1, .div = 2 },
+	{ .val = 2, .div = 4 },
+	{ .val = 3, .div = 6 },
+	{ .val = 4, .div = 8 },
+	{ .val = 5, .div = 10 },
+	{ .val = 6, .div = 12 },
+	{ .val = 7, .div = 14 },
+	{ /* sentinel */ },
+};
+
+struct samsung_div_clock s3c2410_common_dividers[] __initdata = {
+	DIV_T(0, "div_slow", "xti", CLKSLOW, 0, 3, divslow_d),
+	DIV(PCLK, "pclk", "hclk", CLKDIVN, 0, 1),
+};
+
+struct samsung_gate_clock s3c2410_common_gates[] __initdata = {
+	GATE(PCLK_SPI, "spi", "pclk", CLKCON, 18, 0, 0),
+	GATE(PCLK_I2S, "i2s", "pclk", CLKCON, 17, 0, 0),
+	GATE(PCLK_I2C, "i2c", "pclk", CLKCON, 16, 0, 0),
+	GATE(PCLK_ADC, "adc", "pclk", CLKCON, 15, 0, 0),
+	GATE(PCLK_RTC, "rtc", "pclk", CLKCON, 14, 0, 0),
+	GATE(PCLK_GPIO, "gpio", "pclk", CLKCON, 13, CLK_IGNORE_UNUSED, 0),
+	GATE(PCLK_UART2, "uart2", "pclk", CLKCON, 12, 0, 0),
+	GATE(PCLK_UART1, "uart1", "pclk", CLKCON, 11, 0, 0),
+	GATE(PCLK_UART0, "uart0", "pclk", CLKCON, 10, 0, 0),
+	GATE(PCLK_SDI, "sdi", "pclk", CLKCON, 9, 0, 0),
+	GATE(PCLK_PWM, "pwm", "pclk", CLKCON, 8, 0, 0),
+	GATE(HCLK_USBD, "usb-device", "hclk", CLKCON, 7, 0, 0),
+	GATE(HCLK_USBH, "usb-host", "hclk", CLKCON, 6, 0, 0),
+	GATE(HCLK_LCD, "lcd", "hclk", CLKCON, 5, 0, 0),
+	GATE(HCLK_NAND, "nand", "hclk", CLKCON, 4, 0, 0),
+};
+
+/* should be added _after_ the soc-specific clocks are created */
+struct samsung_clock_alias s3c2410_common_aliases[] __initdata = {
+	ALIAS(PCLK_I2C, "s3c2410-i2c.0", "i2c"),
+	ALIAS(PCLK_ADC, NULL, "adc"),
+	ALIAS(PCLK_RTC, NULL, "rtc"),
+	ALIAS(PCLK_PWM, NULL, "timers"),
+	ALIAS(HCLK_LCD, NULL, "lcd"),
+	ALIAS(HCLK_USBD, NULL, "usb-device"),
+	ALIAS(HCLK_USBH, NULL, "usb-host"),
+	ALIAS(UCLK, NULL, "usb-bus-host"),
+	ALIAS(UCLK, NULL, "usb-bus-gadget"),
+	ALIAS(ARMCLK, NULL, "armclk"),
+	ALIAS(UCLK, NULL, "uclk"),
+	ALIAS(HCLK, NULL, "hclk"),
+	ALIAS(MPLL, NULL, "mpll"),
+	ALIAS(FCLK, NULL, "fclk"),
+};
+
+/* S3C2410 specific clocks */
+
+static struct samsung_pll_rate_table pll_s3c2410_12mhz_tbl[] __initdata = {
+	/* sorted in descending order */
+	/* 2410A extras */
+	PLL_35XX_RATE(270000000, 127, 1, 1),
+	PLL_35XX_RATE(268000000, 126, 1, 1),
+	PLL_35XX_RATE(266000000, 125, 1, 1),
+	PLL_35XX_RATE(226000000, 105, 1, 1),
+	PLL_35XX_RATE(210000000, 132, 2, 1),
+	/* 2410 common */
+	PLL_35XX_RATE(203000000, 161, 3, 1),
+	PLL_35XX_RATE(192000000, 88, 1, 1),
+	PLL_35XX_RATE(186000000, 85, 1, 1),
+	PLL_35XX_RATE(180000000, 82, 1, 1),
+	PLL_35XX_RATE(170000000, 77, 1, 1),
+	PLL_35XX_RATE(158000000, 71, 1, 1),
+	PLL_35XX_RATE(152000000, 68, 1, 1),
+	PLL_35XX_RATE(147000000, 90, 2, 1),
+	PLL_35XX_RATE(135000000, 82, 2, 1),
+	PLL_35XX_RATE(124000000, 116, 1, 2),
+	PLL_35XX_RATE(118000000, 150, 2, 2),
+	PLL_35XX_RATE(113000000, 105, 1, 2),
+	PLL_35XX_RATE(101000000, 127, 2, 2),
+	PLL_35XX_RATE(90000000, 112, 2, 2),
+	PLL_35XX_RATE(85000000, 105, 2, 2),
+	PLL_35XX_RATE(79000000, 71, 1, 2),
+	PLL_35XX_RATE(68000000, 82, 2, 2),
+	PLL_35XX_RATE(56000000, 142, 2, 3),
+	PLL_35XX_RATE(48000000, 120, 2, 3),
+	PLL_35XX_RATE(51000000, 161, 3, 3),
+	PLL_35XX_RATE(45000000, 82, 1, 3),
+	PLL_35XX_RATE(34000000, 82, 2, 3),
+	{ /* sentinel */ },
+};
+
+static struct samsung_pll_clock s3c2410_plls[] __initdata = {
+	[mpll] = PLL(pll_s3c2410_mpll, MPLL, "mpll", "xti",
+						LOCKTIME, MPLLCON, NULL),
+	[upll] = PLL(pll_s3c2410_upll, UPLL, "upll", "xti",
+						LOCKTIME, UPLLCON, NULL),
+};
+
+struct samsung_div_clock s3c2410_dividers[] __initdata = {
+	DIV(HCLK, "hclk", "mpll", CLKDIVN, 1, 1),
+};
+
+struct samsung_fixed_factor_clock s3c2410_ffactor[] __initdata = {
+	/*
+	 * armclk is directly supplied by the fclk, without
+	 * switching possibility like on the s3c244x below.
+	 */
+	FFACTOR(ARMCLK, "armclk", "fclk", 1, 1, 0),
+
+	/* uclk is fed from the unmodified upll */
+	FFACTOR(UCLK, "uclk", "upll", 1, 1, 0),
+};
+
+struct samsung_clock_alias s3c2410_aliases[] __initdata = {
+	ALIAS(PCLK_UART0, "s3c2410-uart.0", "uart"),
+	ALIAS(PCLK_UART1, "s3c2410-uart.1", "uart"),
+	ALIAS(PCLK_UART2, "s3c2410-uart.2", "uart"),
+	ALIAS(PCLK_UART0, "s3c2410-uart.0", "clk_uart_baud0"),
+	ALIAS(PCLK_UART1, "s3c2410-uart.1", "clk_uart_baud0"),
+	ALIAS(PCLK_UART2, "s3c2410-uart.2", "clk_uart_baud0"),
+	ALIAS(UCLK, NULL, "clk_uart_baud1"),
+};
+
+/* S3C244x specific clocks */
+
+static struct samsung_pll_rate_table pll_s3c244x_12mhz_tbl[] __initdata = {
+	/* sorted in descending order */
+	PLL_35XX_RATE(400000000, 0x5c, 1, 1),
+	PLL_35XX_RATE(390000000, 0x7a, 2, 1),
+	PLL_35XX_RATE(380000000, 0x57, 1, 1),
+	PLL_35XX_RATE(370000000, 0xb1, 4, 1),
+	PLL_35XX_RATE(360000000, 0x70, 2, 1),
+	PLL_35XX_RATE(350000000, 0xa7, 4, 1),
+	PLL_35XX_RATE(340000000, 0x4d, 1, 1),
+	PLL_35XX_RATE(330000000, 0x66, 2, 1),
+	PLL_35XX_RATE(320000000, 0x98, 4, 1),
+	PLL_35XX_RATE(310000000, 0x93, 4, 1),
+	PLL_35XX_RATE(300000000, 0x75, 3, 1),
+	PLL_35XX_RATE(240000000, 0x70, 1, 2),
+	PLL_35XX_RATE(230000000, 0x6b, 1, 2),
+	PLL_35XX_RATE(220000000, 0x66, 1, 2),
+	PLL_35XX_RATE(210000000, 0x84, 2, 2),
+	PLL_35XX_RATE(200000000, 0x5c, 1, 2),
+	PLL_35XX_RATE(190000000, 0x57, 1, 2),
+	PLL_35XX_RATE(180000000, 0x70, 2, 2),
+	PLL_35XX_RATE(170000000, 0x4d, 1, 2),
+	PLL_35XX_RATE(160000000, 0x98, 4, 2),
+	PLL_35XX_RATE(150000000, 0x75, 3, 2),
+	PLL_35XX_RATE(120000000, 0x70, 1, 3),
+	PLL_35XX_RATE(110000000, 0x66, 1, 3),
+	PLL_35XX_RATE(100000000, 0x5c, 1, 3),
+	PLL_35XX_RATE(90000000, 0x70, 2, 3),
+	PLL_35XX_RATE(80000000, 0x98, 4, 3),
+	PLL_35XX_RATE(75000000, 0x75, 3, 3),
+	{ /* sentinel */ },
+};
+
+static struct samsung_pll_clock s3c244x_common_plls[] __initdata = {
+	[mpll] = PLL(pll_s3c2440_mpll, MPLL, "mpll", "xti",
+						LOCKTIME, MPLLCON, NULL),
+	[upll] = PLL(pll_s3c2410_upll, UPLL, "upll", "xti",
+						LOCKTIME, UPLLCON, NULL),
+};
+
+PNAME(hclk_p) = { "fclk", "div_hclk_2", "div_hclk_4", "div_hclk_3" };
+PNAME(armclk_p) = { "fclk", "hclk" };
+
+struct samsung_mux_clock s3c244x_common_muxes[] __initdata = {
+	MUX(HCLK, "hclk", hclk_p, CLKDIVN, 1, 2),
+	MUX(ARMCLK, "armclk", armclk_p, CAMDIVN, 12, 1),
+};
+
+struct samsung_fixed_factor_clock s3c244x_common_ffactor[] __initdata = {
+	FFACTOR(0, "div_hclk_2", "fclk", 1, 2, 0),
+	FFACTOR(0, "ff_cam", "div_cam", 2, 1, CLK_SET_RATE_PARENT),
+};
+
+static struct clk_div_table div_hclk_4_d[] = {
+	{ .val = 0, .div = 4 },
+	{ .val = 1, .div = 8 },
+	{ /* sentinel */ },
+};
+
+static struct clk_div_table div_hclk_3_d[] = {
+	{ .val = 0, .div = 3 },
+	{ .val = 1, .div = 6 },
+	{ /* sentinel */ },
+};
+
+struct samsung_div_clock s3c244x_common_dividers[] __initdata = {
+	DIV(UCLK, "uclk", "upll", CLKDIVN, 3, 1),
+	DIV(0, "div_hclk", "fclk", CLKDIVN, 1, 1),
+	DIV_T(0, "div_hclk_4", "fclk", CAMDIVN, 9, 1, div_hclk_4_d),
+	DIV_T(0, "div_hclk_3", "fclk", CAMDIVN, 8, 1, div_hclk_3_d),
+	DIV(0, "div_cam", "upll", CAMDIVN, 0, 3),
+};
+
+struct samsung_gate_clock s3c244x_common_gates[] __initdata = {
+	GATE(HCLK_CAM, "cam", "hclk", CLKCON, 19, 0, 0),
+};
+
+struct samsung_clock_alias s3c244x_common_aliases[] __initdata = {
+	ALIAS(PCLK_UART0, "s3c2440-uart.0", "uart"),
+	ALIAS(PCLK_UART1, "s3c2440-uart.1", "uart"),
+	ALIAS(PCLK_UART2, "s3c2440-uart.2", "uart"),
+	ALIAS(PCLK_UART0, "s3c2440-uart.0", "clk_uart_baud2"),
+	ALIAS(PCLK_UART1, "s3c2440-uart.1", "clk_uart_baud2"),
+	ALIAS(PCLK_UART2, "s3c2440-uart.2", "clk_uart_baud2"),
+	ALIAS(HCLK_CAM, NULL, "camif"),
+	ALIAS(CAMIF, NULL, "camif-upll"),
+};
+
+/* S3C2440 specific clocks */
+
+PNAME(s3c2440_camif_p) = { "upll", "ff_cam" };
+
+struct samsung_mux_clock s3c2440_muxes[] __initdata = {
+	MUX(CAMIF, "camif", s3c2440_camif_p, CAMDIVN, 4, 1),
+};
+
+struct samsung_gate_clock s3c2440_gates[] __initdata = {
+	GATE(PCLK_AC97, "ac97", "pclk", CLKCON, 20, 0, 0),
+};
+
+/* S3C2442 specific clocks */
+
+struct samsung_fixed_factor_clock s3c2442_ffactor[] __initdata = {
+	FFACTOR(0, "upll_3", "upll", 1, 3, 0),
+};
+
+PNAME(s3c2442_camif_p) = { "upll", "ff_cam", "upll", "upll_3" };
+
+struct samsung_mux_clock s3c2442_muxes[] __initdata = {
+	MUX(CAMIF, "camif", s3c2442_camif_p, CAMDIVN, 4, 2),
+};
+
+/*
+ * fixed rate clocks generated outside the soc
+ * Only necessary until the devicetree-move is complete
+ */
+#define XTI	1
+struct samsung_fixed_rate_clock s3c2410_common_frate_clks[] __initdata = {
+	FRATE(XTI, "xti", NULL, CLK_IS_ROOT, 0),
+};
+
+static void __init s3c2410_common_clk_register_fixed_ext(
+		struct samsung_clk_provider *ctx,
+		unsigned long xti_f)
+{
+	struct samsung_clock_alias xti_alias = ALIAS(XTI, NULL, "xtal");
+
+	s3c2410_common_frate_clks[0].fixed_rate = xti_f;
+	samsung_clk_register_fixed_rate(ctx, s3c2410_common_frate_clks,
+				ARRAY_SIZE(s3c2410_common_frate_clks));
+
+	samsung_clk_register_alias(ctx, &xti_alias, 1);
+}
+
+void __init s3c2410_common_clk_init(struct device_node *np, unsigned long xti_f,
+				    int current_soc,
+				    void __iomem *base)
+{
+	struct samsung_clk_provider *ctx;
+	reg_base = base;
+
+	if (np) {
+		reg_base = of_iomap(np, 0);
+		if (!reg_base)
+			panic("%s: failed to map registers\n", __func__);
+	}
+
+	ctx = samsung_clk_init(np, reg_base, NR_CLKS);
+	if (!ctx)
+		panic("%s: unable to allocate context.\n", __func__);
+
+	/* Register external clocks only in non-dt cases */
+	if (!np)
+		s3c2410_common_clk_register_fixed_ext(ctx, xti_f);
+
+	if (current_soc == 2410) {
+		if (_get_rate("xti") == 12 * MHZ) {
+			s3c2410_plls[mpll].rate_table = pll_s3c2410_12mhz_tbl;
+			s3c2410_plls[upll].rate_table = pll_s3c2410_12mhz_tbl;
+		}
+
+		/* Register PLLs. */
+		samsung_clk_register_pll(ctx, s3c2410_plls,
+				ARRAY_SIZE(s3c2410_plls), reg_base);
+
+	} else { /* S3C2440, S3C2442 */
+		if (_get_rate("xti") == 12 * MHZ) {
+			/*
+			 * plls follow different calculation schemes, with the
+			 * upll following the same scheme as the s3c2410 plls
+			 */
+			s3c244x_common_plls[mpll].rate_table =
+							pll_s3c244x_12mhz_tbl;
+			s3c244x_common_plls[upll].rate_table =
+							pll_s3c2410_12mhz_tbl;
+		}
+
+		/* Register PLLs. */
+		samsung_clk_register_pll(ctx, s3c244x_common_plls,
+				ARRAY_SIZE(s3c244x_common_plls), reg_base);
+	}
+
+	/* Register common internal clocks. */
+	samsung_clk_register_mux(ctx, s3c2410_common_muxes,
+			ARRAY_SIZE(s3c2410_common_muxes));
+	samsung_clk_register_div(ctx, s3c2410_common_dividers,
+			ARRAY_SIZE(s3c2410_common_dividers));
+	samsung_clk_register_gate(ctx, s3c2410_common_gates,
+		ARRAY_SIZE(s3c2410_common_gates));
+
+	if (current_soc == S3C2440 || current_soc == S3C2442) {
+		samsung_clk_register_div(ctx, s3c244x_common_dividers,
+				ARRAY_SIZE(s3c244x_common_dividers));
+		samsung_clk_register_gate(ctx, s3c244x_common_gates,
+				ARRAY_SIZE(s3c244x_common_gates));
+		samsung_clk_register_mux(ctx, s3c244x_common_muxes,
+				ARRAY_SIZE(s3c244x_common_muxes));
+		samsung_clk_register_fixed_factor(ctx, s3c244x_common_ffactor,
+				ARRAY_SIZE(s3c244x_common_ffactor));
+	}
+
+	/* Register SoC-specific clocks. */
+	switch (current_soc) {
+	case S3C2410:
+		samsung_clk_register_div(ctx, s3c2410_dividers,
+				ARRAY_SIZE(s3c2410_dividers));
+		samsung_clk_register_fixed_factor(ctx, s3c2410_ffactor,
+				ARRAY_SIZE(s3c2410_ffactor));
+		samsung_clk_register_alias(ctx, s3c2410_aliases,
+			ARRAY_SIZE(s3c2410_common_aliases));
+		break;
+	case S3C2440:
+		samsung_clk_register_mux(ctx, s3c2440_muxes,
+				ARRAY_SIZE(s3c2440_muxes));
+		samsung_clk_register_gate(ctx, s3c2440_gates,
+				ARRAY_SIZE(s3c2440_gates));
+		break;
+	case S3C2442:
+		samsung_clk_register_mux(ctx, s3c2442_muxes,
+				ARRAY_SIZE(s3c2442_muxes));
+		samsung_clk_register_fixed_factor(ctx, s3c2442_ffactor,
+				ARRAY_SIZE(s3c2442_ffactor));
+		break;
+	}
+
+	/*
+	 * Register common aliases at the end, as some of the aliased clocks
+	 * are SoC specific.
+	 */
+	samsung_clk_register_alias(ctx, s3c2410_common_aliases,
+		ARRAY_SIZE(s3c2410_common_aliases));
+
+	if (current_soc == S3C2440 || current_soc == S3C2442) {
+		samsung_clk_register_alias(ctx, s3c244x_common_aliases,
+			ARRAY_SIZE(s3c244x_common_aliases));
+	}
+
+	s3c2410_clk_sleep_init();
+}
+
+static void __init s3c2410_clk_init(struct device_node *np)
+{
+	s3c2410_common_clk_init(np, 0, S3C2410, 0);
+}
+CLK_OF_DECLARE(s3c2410_clk, "samsung,s3c2410-clock", s3c2410_clk_init);
+
+static void __init s3c2440_clk_init(struct device_node *np)
+{
+	s3c2410_common_clk_init(np, 0, S3C2440, 0);
+}
+CLK_OF_DECLARE(s3c2440_clk, "samsung,s3c2440-clock", s3c2440_clk_init);
+
+static void __init s3c2442_clk_init(struct device_node *np)
+{
+	s3c2410_common_clk_init(np, 0, S3C2442, 0);
+}
+CLK_OF_DECLARE(s3c2442_clk, "samsung,s3c2442-clock", s3c2442_clk_init);
diff --git a/drivers/clk/samsung/clk-s3c2412.c b/drivers/clk/samsung/clk-s3c2412.c
new file mode 100644
index 00000000000..23e4313f625
--- /dev/null
+++ b/drivers/clk/samsung/clk-s3c2412.c
@@ -0,0 +1,274 @@
+/*
+ * Copyright (c) 2013 Heiko Stuebner <heiko@sntech.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.
+ *
+ * Common Clock Framework support for S3C2412 and S3C2413.
+ */
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/syscore_ops.h>
+
+#include <dt-bindings/clock/s3c2412.h>
+
+#include "clk.h"
+#include "clk-pll.h"
+
+#define LOCKTIME	0x00
+#define MPLLCON		0x04
+#define UPLLCON		0x08
+#define CLKCON		0x0c
+#define CLKDIVN		0x14
+#define CLKSRC		0x1c
+
+/* list of PLLs to be registered */
+enum s3c2412_plls {
+	mpll, upll,
+};
+
+static void __iomem *reg_base;
+
+#ifdef CONFIG_PM_SLEEP
+static struct samsung_clk_reg_dump *s3c2412_save;
+
+/*
+ * list of controller registers to be saved and restored during a
+ * suspend/resume cycle.
+ */
+static unsigned long s3c2412_clk_regs[] __initdata = {
+	LOCKTIME,
+	MPLLCON,
+	UPLLCON,
+	CLKCON,
+	CLKDIVN,
+	CLKSRC,
+};
+
+static int s3c2412_clk_suspend(void)
+{
+	samsung_clk_save(reg_base, s3c2412_save,
+				ARRAY_SIZE(s3c2412_clk_regs));
+
+	return 0;
+}
+
+static void s3c2412_clk_resume(void)
+{
+	samsung_clk_restore(reg_base, s3c2412_save,
+				ARRAY_SIZE(s3c2412_clk_regs));
+}
+
+static struct syscore_ops s3c2412_clk_syscore_ops = {
+	.suspend = s3c2412_clk_suspend,
+	.resume = s3c2412_clk_resume,
+};
+
+static void s3c2412_clk_sleep_init(void)
+{
+	s3c2412_save = samsung_clk_alloc_reg_dump(s3c2412_clk_regs,
+						ARRAY_SIZE(s3c2412_clk_regs));
+	if (!s3c2412_save) {
+		pr_warn("%s: failed to allocate sleep save data, no sleep support!\n",
+			__func__);
+		return;
+	}
+
+	register_syscore_ops(&s3c2412_clk_syscore_ops);
+	return;
+}
+#else
+static void s3c2412_clk_sleep_init(void) {}
+#endif
+
+static struct clk_div_table divxti_d[] = {
+	{ .val = 0, .div = 1 },
+	{ .val = 1, .div = 2 },
+	{ .val = 2, .div = 4 },
+	{ .val = 3, .div = 6 },
+	{ .val = 4, .div = 8 },
+	{ .val = 5, .div = 10 },
+	{ .val = 6, .div = 12 },
+	{ .val = 7, .div = 14 },
+	{ /* sentinel */ },
+};
+
+struct samsung_div_clock s3c2412_dividers[] __initdata = {
+	DIV_T(0, "div_xti", "xti", CLKSRC, 0, 3, divxti_d),
+	DIV(0, "div_cam", "mux_cam", CLKDIVN, 16, 4),
+	DIV(0, "div_i2s", "mux_i2s", CLKDIVN, 12, 4),
+	DIV(0, "div_uart", "mux_uart", CLKDIVN, 8, 4),
+	DIV(0, "div_usb", "mux_usb", CLKDIVN, 6, 1),
+	DIV(0, "div_hclk_half", "hclk", CLKDIVN, 5, 1),
+	DIV(ARMDIV, "armdiv", "msysclk", CLKDIVN, 3, 1),
+	DIV(PCLK, "pclk", "hclk", CLKDIVN, 2, 1),
+	DIV(HCLK, "hclk", "armdiv", CLKDIVN, 0, 2),
+};
+
+struct samsung_fixed_factor_clock s3c2412_ffactor[] __initdata = {
+	FFACTOR(0, "ff_hclk", "hclk", 2, 1, CLK_SET_RATE_PARENT),
+};
+
+/*
+ * The first two use the OM[4] setting, which is not readable from
+ * software, so assume it is set to xti.
+ */
+PNAME(erefclk_p) = { "xti", "xti", "xti", "ext" };
+PNAME(urefclk_p) = { "xti", "xti", "xti", "ext" };
+
+PNAME(camclk_p) = { "usysclk", "hclk" };
+PNAME(usbclk_p) = { "usysclk", "hclk" };
+PNAME(i2sclk_p) = { "erefclk", "mpll" };
+PNAME(uartclk_p) = { "erefclk", "mpll" };
+PNAME(usysclk_p) = { "urefclk", "upll" };
+PNAME(msysclk_p) = { "mdivclk", "mpll" };
+PNAME(mdivclk_p) = { "xti", "div_xti" };
+PNAME(armclk_p) = { "armdiv", "hclk" };
+
+struct samsung_mux_clock s3c2412_muxes[] __initdata = {
+	MUX(0, "erefclk", erefclk_p, CLKSRC, 14, 2),
+	MUX(0, "urefclk", urefclk_p, CLKSRC, 12, 2),
+	MUX(0, "mux_cam", camclk_p, CLKSRC, 11, 1),
+	MUX(0, "mux_usb", usbclk_p, CLKSRC, 10, 1),
+	MUX(0, "mux_i2s", i2sclk_p, CLKSRC, 9, 1),
+	MUX(0, "mux_uart", uartclk_p, CLKSRC, 8, 1),
+	MUX(USYSCLK, "usysclk", usysclk_p, CLKSRC, 5, 1),
+	MUX(MSYSCLK, "msysclk", msysclk_p, CLKSRC, 4, 1),
+	MUX(MDIVCLK, "mdivclk", mdivclk_p, CLKSRC, 3, 1),
+	MUX(ARMCLK, "armclk", armclk_p, CLKDIVN, 4, 1),
+};
+
+static struct samsung_pll_clock s3c2412_plls[] __initdata = {
+	[mpll] = PLL(pll_s3c2440_mpll, MPLL, "mpll", "xti",
+						LOCKTIME, MPLLCON, NULL),
+	[upll] = PLL(pll_s3c2410_upll, UPLL, "upll", "urefclk",
+						LOCKTIME, UPLLCON, NULL),
+};
+
+struct samsung_gate_clock s3c2412_gates[] __initdata = {
+	GATE(PCLK_WDT, "wdt", "pclk", CLKCON, 28, 0, 0),
+	GATE(PCLK_SPI, "spi", "pclk", CLKCON, 27, 0, 0),
+	GATE(PCLK_I2S, "i2s", "pclk", CLKCON, 26, 0, 0),
+	GATE(PCLK_I2C, "i2c", "pclk", CLKCON, 25, 0, 0),
+	GATE(PCLK_ADC, "adc", "pclk", CLKCON, 24, 0, 0),
+	GATE(PCLK_RTC, "rtc", "pclk", CLKCON, 23, 0, 0),
+	GATE(PCLK_GPIO, "gpio", "pclk", CLKCON, 22, CLK_IGNORE_UNUSED, 0),
+	GATE(PCLK_UART2, "uart2", "pclk", CLKCON, 21, 0, 0),
+	GATE(PCLK_UART1, "uart1", "pclk", CLKCON, 20, 0, 0),
+	GATE(PCLK_UART0, "uart0", "pclk", CLKCON, 19, 0, 0),
+	GATE(PCLK_SDI, "sdi", "pclk", CLKCON, 18, 0, 0),
+	GATE(PCLK_PWM, "pwm", "pclk", CLKCON, 17, 0, 0),
+	GATE(PCLK_USBD, "usb-device", "pclk", CLKCON, 16, 0, 0),
+	GATE(SCLK_CAM, "sclk_cam", "div_cam", CLKCON, 15, 0, 0),
+	GATE(SCLK_UART, "sclk_uart", "div_uart", CLKCON, 14, 0, 0),
+	GATE(SCLK_I2S, "sclk_i2s", "div_i2s", CLKCON, 13, 0, 0),
+	GATE(SCLK_USBH, "sclk_usbh", "div_usb", CLKCON, 12, 0, 0),
+	GATE(SCLK_USBD, "sclk_usbd", "div_usb", CLKCON, 11, 0, 0),
+	GATE(HCLK_HALF, "hclk_half", "div_hclk_half", CLKCON, 10, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_X2, "hclkx2", "ff_hclk", CLKCON, 9, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_SDRAM, "sdram", "hclk", CLKCON, 8, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_USBH, "usb-host", "hclk", CLKCON, 6, 0, 0),
+	GATE(HCLK_LCD, "lcd", "hclk", CLKCON, 5, 0, 0),
+	GATE(HCLK_NAND, "nand", "hclk", CLKCON, 4, 0, 0),
+	GATE(HCLK_DMA3, "dma3", "hclk", CLKCON, 3, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_DMA2, "dma2", "hclk", CLKCON, 2, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_DMA1, "dma1", "hclk", CLKCON, 1, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_DMA0, "dma0", "hclk", CLKCON, 0, CLK_IGNORE_UNUSED, 0),
+};
+
+struct samsung_clock_alias s3c2412_aliases[] __initdata = {
+	ALIAS(PCLK_UART0, "s3c2412-uart.0", "uart"),
+	ALIAS(PCLK_UART1, "s3c2412-uart.1", "uart"),
+	ALIAS(PCLK_UART2, "s3c2412-uart.2", "uart"),
+	ALIAS(PCLK_UART0, "s3c2412-uart.0", "clk_uart_baud2"),
+	ALIAS(PCLK_UART1, "s3c2412-uart.1", "clk_uart_baud2"),
+	ALIAS(PCLK_UART2, "s3c2412-uart.2", "clk_uart_baud2"),
+	ALIAS(SCLK_UART, NULL, "clk_uart_baud3"),
+	ALIAS(PCLK_I2C, "s3c2410-i2c.0", "i2c"),
+	ALIAS(PCLK_ADC, NULL, "adc"),
+	ALIAS(PCLK_RTC, NULL, "rtc"),
+	ALIAS(PCLK_PWM, NULL, "timers"),
+	ALIAS(HCLK_LCD, NULL, "lcd"),
+	ALIAS(PCLK_USBD, NULL, "usb-device"),
+	ALIAS(SCLK_USBD, NULL, "usb-bus-gadget"),
+	ALIAS(HCLK_USBH, NULL, "usb-host"),
+	ALIAS(SCLK_USBH, NULL, "usb-bus-host"),
+	ALIAS(ARMCLK, NULL, "armclk"),
+	ALIAS(HCLK, NULL, "hclk"),
+	ALIAS(MPLL, NULL, "mpll"),
+	ALIAS(MSYSCLK, NULL, "fclk"),
+};
+
+/*
+ * fixed rate clocks generated outside the soc
+ * Only necessary until the devicetree-move is complete
+ */
+#define XTI	1
+struct samsung_fixed_rate_clock s3c2412_common_frate_clks[] __initdata = {
+	FRATE(XTI, "xti", NULL, CLK_IS_ROOT, 0),
+	FRATE(0, "ext", NULL, CLK_IS_ROOT, 0),
+};
+
+static void __init s3c2412_common_clk_register_fixed_ext(
+		struct samsung_clk_provider *ctx,
+		unsigned long xti_f, unsigned long ext_f)
+{
+	/* xtal alias is necessary for the current cpufreq driver */
+	struct samsung_clock_alias xti_alias = ALIAS(XTI, NULL, "xtal");
+
+	s3c2412_common_frate_clks[0].fixed_rate = xti_f;
+	s3c2412_common_frate_clks[1].fixed_rate = ext_f;
+	samsung_clk_register_fixed_rate(ctx, s3c2412_common_frate_clks,
+				ARRAY_SIZE(s3c2412_common_frate_clks));
+
+	samsung_clk_register_alias(ctx, &xti_alias, 1);
+}
+
+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;
+	reg_base = base;
+
+	if (np) {
+		reg_base = of_iomap(np, 0);
+		if (!reg_base)
+			panic("%s: failed to map registers\n", __func__);
+	}
+
+	ctx = samsung_clk_init(np, reg_base, NR_CLKS);
+	if (!ctx)
+		panic("%s: unable to allocate context.\n", __func__);
+
+	/* Register external clocks only in non-dt cases */
+	if (!np)
+		s3c2412_common_clk_register_fixed_ext(ctx, xti_f, ext_f);
+
+	/* Register PLLs. */
+	samsung_clk_register_pll(ctx, s3c2412_plls, ARRAY_SIZE(s3c2412_plls),
+				 reg_base);
+
+	/* Register common internal clocks. */
+	samsung_clk_register_mux(ctx, s3c2412_muxes, ARRAY_SIZE(s3c2412_muxes));
+	samsung_clk_register_div(ctx, s3c2412_dividers,
+					  ARRAY_SIZE(s3c2412_dividers));
+	samsung_clk_register_gate(ctx, s3c2412_gates,
+					ARRAY_SIZE(s3c2412_gates));
+	samsung_clk_register_fixed_factor(ctx, s3c2412_ffactor,
+					  ARRAY_SIZE(s3c2412_ffactor));
+	samsung_clk_register_alias(ctx, s3c2412_aliases,
+				   ARRAY_SIZE(s3c2412_aliases));
+
+	s3c2412_clk_sleep_init();
+}
+
+static void __init s3c2412_clk_init(struct device_node *np)
+{
+	s3c2412_common_clk_init(np, 0, 0, 0);
+}
+CLK_OF_DECLARE(s3c2412_clk, "samsung,s3c2412-clock", s3c2412_clk_init);
diff --git a/drivers/clk/samsung/clk-s3c2443.c b/drivers/clk/samsung/clk-s3c2443.c
new file mode 100644
index 00000000000..c4bbdabebaa
--- /dev/null
+++ b/drivers/clk/samsung/clk-s3c2443.c
@@ -0,0 +1,466 @@
+/*
+ * Copyright (c) 2013 Heiko Stuebner <heiko@sntech.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.
+ *
+ * Common Clock Framework support for S3C2443 and following SoCs.
+ */
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/syscore_ops.h>
+
+#include <dt-bindings/clock/s3c2443.h>
+
+#include "clk.h"
+#include "clk-pll.h"
+
+/* S3C2416 clock controller register offsets */
+#define LOCKCON0	0x00
+#define LOCKCON1	0x04
+#define MPLLCON		0x10
+#define EPLLCON		0x18
+#define EPLLCON_K	0x1C
+#define CLKSRC		0x20
+#define CLKDIV0		0x24
+#define CLKDIV1		0x28
+#define CLKDIV2		0x2C
+#define HCLKCON		0x30
+#define PCLKCON		0x34
+#define SCLKCON		0x38
+
+/* the soc types */
+enum supported_socs {
+	S3C2416,
+	S3C2443,
+	S3C2450,
+};
+
+/* list of PLLs to be registered */
+enum s3c2443_plls {
+	mpll, epll,
+};
+
+static void __iomem *reg_base;
+
+#ifdef CONFIG_PM_SLEEP
+static struct samsung_clk_reg_dump *s3c2443_save;
+
+/*
+ * list of controller registers to be saved and restored during a
+ * suspend/resume cycle.
+ */
+static unsigned long s3c2443_clk_regs[] __initdata = {
+	LOCKCON0,
+	LOCKCON1,
+	MPLLCON,
+	EPLLCON,
+	EPLLCON_K,
+	CLKSRC,
+	CLKDIV0,
+	CLKDIV1,
+	CLKDIV2,
+	PCLKCON,
+	HCLKCON,
+	SCLKCON,
+};
+
+static int s3c2443_clk_suspend(void)
+{
+	samsung_clk_save(reg_base, s3c2443_save,
+				ARRAY_SIZE(s3c2443_clk_regs));
+
+	return 0;
+}
+
+static void s3c2443_clk_resume(void)
+{
+	samsung_clk_restore(reg_base, s3c2443_save,
+				ARRAY_SIZE(s3c2443_clk_regs));
+}
+
+static struct syscore_ops s3c2443_clk_syscore_ops = {
+	.suspend = s3c2443_clk_suspend,
+	.resume = s3c2443_clk_resume,
+};
+
+static void s3c2443_clk_sleep_init(void)
+{
+	s3c2443_save = samsung_clk_alloc_reg_dump(s3c2443_clk_regs,
+						ARRAY_SIZE(s3c2443_clk_regs));
+	if (!s3c2443_save) {
+		pr_warn("%s: failed to allocate sleep save data, no sleep support!\n",
+			__func__);
+		return;
+	}
+
+	register_syscore_ops(&s3c2443_clk_syscore_ops);
+	return;
+}
+#else
+static void s3c2443_clk_sleep_init(void) {}
+#endif
+
+PNAME(epllref_p) = { "mpllref", "mpllref", "xti", "ext" };
+PNAME(esysclk_p) = { "epllref", "epll" };
+PNAME(mpllref_p) = { "xti", "mdivclk" };
+PNAME(msysclk_p) = { "mpllref", "mpll" };
+PNAME(armclk_p) = { "armdiv" , "hclk" };
+PNAME(i2s0_p) = { "div_i2s0", "ext_i2s", "epllref", "epllref" };
+
+struct samsung_mux_clock s3c2443_common_muxes[] __initdata = {
+	MUX(0, "epllref", epllref_p, CLKSRC, 7, 2),
+	MUX(ESYSCLK, "esysclk", esysclk_p, CLKSRC, 6, 1),
+	MUX(0, "mpllref", mpllref_p, CLKSRC, 3, 1),
+	MUX_A(MSYSCLK, "msysclk", msysclk_p, CLKSRC, 4, 1, "msysclk"),
+	MUX_A(ARMCLK, "armclk", armclk_p, CLKDIV0, 13, 1, "armclk"),
+	MUX(0, "mux_i2s0", i2s0_p, CLKSRC, 14, 2),
+};
+
+static struct clk_div_table hclk_d[] = {
+	{ .val = 0, .div = 1 },
+	{ .val = 1, .div = 2 },
+	{ .val = 3, .div = 4 },
+	{ /* sentinel */ },
+};
+
+static struct clk_div_table mdivclk_d[] = {
+	{ .val = 0, .div = 1 },
+	{ .val = 1, .div = 3 },
+	{ .val = 2, .div = 5 },
+	{ .val = 3, .div = 7 },
+	{ .val = 4, .div = 9 },
+	{ .val = 5, .div = 11 },
+	{ .val = 6, .div = 13 },
+	{ .val = 7, .div = 15 },
+	{ /* sentinel */ },
+};
+
+struct samsung_div_clock s3c2443_common_dividers[] __initdata = {
+	DIV_T(0, "mdivclk", "xti", CLKDIV0, 6, 3, mdivclk_d),
+	DIV(0, "prediv", "msysclk", CLKDIV0, 4, 2),
+	DIV_T(HCLK, "hclk", "prediv", CLKDIV0, 0, 2, hclk_d),
+	DIV(PCLK, "pclk", "hclk", CLKDIV0, 2, 1),
+	DIV(0, "div_hsspi0_epll", "esysclk", CLKDIV1, 24, 2),
+	DIV(0, "div_fimd", "esysclk", CLKDIV1, 16, 8),
+	DIV(0, "div_i2s0", "esysclk", CLKDIV1, 12, 4),
+	DIV(0, "div_uart", "esysclk", CLKDIV1, 8, 4),
+	DIV(0, "div_hsmmc1", "esysclk", CLKDIV1, 6, 2),
+	DIV(0, "div_usbhost", "esysclk", CLKDIV1, 4, 2),
+};
+
+struct samsung_gate_clock s3c2443_common_gates[] __initdata = {
+	GATE(SCLK_HSMMC_EXT, "sclk_hsmmcext", "ext", SCLKCON, 13, 0, 0),
+	GATE(SCLK_HSMMC1, "sclk_hsmmc1", "div_hsmmc1", SCLKCON, 12, 0, 0),
+	GATE(SCLK_FIMD, "sclk_fimd", "div_fimd", SCLKCON, 10, 0, 0),
+	GATE(SCLK_I2S0, "sclk_i2s0", "mux_i2s0", SCLKCON, 9, 0, 0),
+	GATE(SCLK_UART, "sclk_uart", "div_uart", SCLKCON, 8, 0, 0),
+	GATE(SCLK_USBH, "sclk_usbhost", "div_usbhost", SCLKCON, 1, 0, 0),
+	GATE(HCLK_DRAM, "dram", "hclk", HCLKCON, 19, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_SSMC, "ssmc", "hclk", HCLKCON, 18, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_HSMMC1, "hsmmc1", "hclk", HCLKCON, 16, 0, 0),
+	GATE(HCLK_USBD, "usb-device", "hclk", HCLKCON, 12, 0, 0),
+	GATE(HCLK_USBH, "usb-host", "hclk", HCLKCON, 11, 0, 0),
+	GATE(HCLK_LCD, "lcd", "hclk", HCLKCON, 9, 0, 0),
+	GATE(HCLK_DMA5, "dma5", "hclk", HCLKCON, 5, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_DMA4, "dma4", "hclk", HCLKCON, 4, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_DMA3, "dma3", "hclk", HCLKCON, 3, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_DMA2, "dma2", "hclk", HCLKCON, 2, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_DMA1, "dma1", "hclk", HCLKCON, 1, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_DMA0, "dma0", "hclk", HCLKCON, 0, CLK_IGNORE_UNUSED, 0),
+	GATE(PCLK_GPIO, "gpio", "pclk", PCLKCON, 13, CLK_IGNORE_UNUSED, 0),
+	GATE(PCLK_RTC, "rtc", "pclk", PCLKCON, 12, 0, 0),
+	GATE(PCLK_WDT, "wdt", "pclk", PCLKCON, 11, 0, 0),
+	GATE(PCLK_PWM, "pwm", "pclk", PCLKCON, 10, 0, 0),
+	GATE(PCLK_I2S0, "i2s0", "pclk", PCLKCON, 9, 0, 0),
+	GATE(PCLK_AC97, "ac97", "pclk", PCLKCON, 8, 0, 0),
+	GATE(PCLK_ADC, "adc", "pclk", PCLKCON, 7, 0, 0),
+	GATE(PCLK_SPI0, "spi0", "pclk", PCLKCON, 6, 0, 0),
+	GATE(PCLK_I2C0, "i2c0", "pclk", PCLKCON, 4, 0, 0),
+	GATE(PCLK_UART3, "uart3", "pclk", PCLKCON, 3, 0, 0),
+	GATE(PCLK_UART2, "uart2", "pclk", PCLKCON, 2, 0, 0),
+	GATE(PCLK_UART1, "uart1", "pclk", PCLKCON, 1, 0, 0),
+	GATE(PCLK_UART0, "uart0", "pclk", PCLKCON, 0, 0, 0),
+};
+
+struct samsung_clock_alias s3c2443_common_aliases[] __initdata = {
+	ALIAS(HCLK, NULL, "hclk"),
+	ALIAS(HCLK_SSMC, NULL, "nand"),
+	ALIAS(PCLK_UART0, "s3c2440-uart.0", "uart"),
+	ALIAS(PCLK_UART1, "s3c2440-uart.1", "uart"),
+	ALIAS(PCLK_UART2, "s3c2440-uart.2", "uart"),
+	ALIAS(PCLK_UART3, "s3c2440-uart.3", "uart"),
+	ALIAS(PCLK_UART0, "s3c2440-uart.0", "clk_uart_baud2"),
+	ALIAS(PCLK_UART1, "s3c2440-uart.1", "clk_uart_baud2"),
+	ALIAS(PCLK_UART2, "s3c2440-uart.2", "clk_uart_baud2"),
+	ALIAS(PCLK_UART3, "s3c2440-uart.3", "clk_uart_baud2"),
+	ALIAS(SCLK_UART, NULL, "clk_uart_baud3"),
+	ALIAS(PCLK_PWM, NULL, "timers"),
+	ALIAS(PCLK_RTC, NULL, "rtc"),
+	ALIAS(PCLK_WDT, NULL, "watchdog"),
+	ALIAS(PCLK_ADC, NULL, "adc"),
+	ALIAS(PCLK_I2C0, "s3c2410-i2c.0", "i2c"),
+	ALIAS(HCLK_USBD, NULL, "usb-device"),
+	ALIAS(HCLK_USBH, NULL, "usb-host"),
+	ALIAS(SCLK_USBH, NULL, "usb-bus-host"),
+	ALIAS(PCLK_SPI0, "s3c2443-spi.0", "spi"),
+	ALIAS(PCLK_SPI0, "s3c2443-spi.0", "spi_busclk0"),
+	ALIAS(HCLK_HSMMC1, "s3c-sdhci.1", "hsmmc"),
+	ALIAS(HCLK_HSMMC1, "s3c-sdhci.1", "mmc_busclk.0"),
+	ALIAS(PCLK_I2S0, "samsung-i2s.0", "iis"),
+	ALIAS(SCLK_I2S0, NULL, "i2s-if"),
+	ALIAS(HCLK_LCD, NULL, "lcd"),
+	ALIAS(SCLK_FIMD, NULL, "sclk_fimd"),
+};
+
+/* S3C2416 specific clocks */
+
+static struct samsung_pll_clock s3c2416_pll_clks[] __initdata = {
+	[mpll] = PLL(pll_6552_s3c2416, 0, "mpll", "mpllref",
+						LOCKCON0, MPLLCON, NULL),
+	[epll] = PLL(pll_6553, 0, "epll", "epllref",
+						LOCKCON1, EPLLCON, NULL),
+};
+
+PNAME(s3c2416_hsmmc0_p) = { "sclk_hsmmc0", "sclk_hsmmcext" };
+PNAME(s3c2416_hsmmc1_p) = { "sclk_hsmmc1", "sclk_hsmmcext" };
+PNAME(s3c2416_hsspi0_p) = { "hsspi0_epll", "hsspi0_mpll" };
+
+static struct clk_div_table armdiv_s3c2416_d[] = {
+	{ .val = 0, .div = 1 },
+	{ .val = 1, .div = 2 },
+	{ .val = 2, .div = 3 },
+	{ .val = 3, .div = 4 },
+	{ .val = 5, .div = 6 },
+	{ .val = 7, .div = 8 },
+	{ /* sentinel */ },
+};
+
+struct samsung_div_clock s3c2416_dividers[] __initdata = {
+	DIV_T(ARMDIV, "armdiv", "msysclk", CLKDIV0, 9, 3, armdiv_s3c2416_d),
+	DIV(0, "div_hsspi0_mpll", "msysclk", CLKDIV2, 0, 4),
+	DIV(0, "div_hsmmc0", "esysclk", CLKDIV2, 6, 2),
+};
+
+struct samsung_mux_clock s3c2416_muxes[] __initdata = {
+	MUX(MUX_HSMMC0, "mux_hsmmc0", s3c2416_hsmmc0_p, CLKSRC, 16, 1),
+	MUX(MUX_HSMMC1, "mux_hsmmc1", s3c2416_hsmmc1_p, CLKSRC, 17, 1),
+	MUX(MUX_HSSPI0, "mux_hsspi0", s3c2416_hsspi0_p, CLKSRC, 18, 1),
+};
+
+struct samsung_gate_clock s3c2416_gates[] __initdata = {
+	GATE(0, "hsspi0_mpll", "div_hsspi0_mpll", SCLKCON, 19, 0, 0),
+	GATE(0, "hsspi0_epll", "div_hsspi0_epll", SCLKCON, 14, 0, 0),
+	GATE(0, "sclk_hsmmc0", "div_hsmmc0", SCLKCON, 6, 0, 0),
+	GATE(HCLK_2D, "2d", "hclk", HCLKCON, 20, 0, 0),
+	GATE(HCLK_HSMMC0, "hsmmc0", "hclk", HCLKCON, 15, 0, 0),
+	GATE(HCLK_IROM, "irom", "hclk", HCLKCON, 13, CLK_IGNORE_UNUSED, 0),
+	GATE(PCLK_PCM, "pcm", "pclk", PCLKCON, 19, 0, 0),
+};
+
+struct samsung_clock_alias s3c2416_aliases[] __initdata = {
+	ALIAS(HCLK_HSMMC0, "s3c-sdhci.0", "hsmmc"),
+	ALIAS(HCLK_HSMMC0, "s3c-sdhci.0", "mmc_busclk.0"),
+	ALIAS(MUX_HSMMC0, "s3c-sdhci.0", "mmc_busclk.2"),
+	ALIAS(MUX_HSMMC1, "s3c-sdhci.1", "mmc_busclk.2"),
+	ALIAS(MUX_HSSPI0, "s3c2443-spi.0", "spi_busclk2"),
+	ALIAS(ARMDIV, NULL, "armdiv"),
+};
+
+/* S3C2443 specific clocks */
+
+static struct samsung_pll_clock s3c2443_pll_clks[] __initdata = {
+	[mpll] = PLL(pll_3000, 0, "mpll", "mpllref",
+						LOCKCON0, MPLLCON, NULL),
+	[epll] = PLL(pll_2126, 0, "epll", "epllref",
+						LOCKCON1, EPLLCON, NULL),
+};
+
+static struct clk_div_table armdiv_s3c2443_d[] = {
+	{ .val = 0, .div = 1 },
+	{ .val = 8, .div = 2 },
+	{ .val = 2, .div = 3 },
+	{ .val = 9, .div = 4 },
+	{ .val = 10, .div = 6 },
+	{ .val = 11, .div = 8 },
+	{ .val = 13, .div = 12 },
+	{ .val = 15, .div = 16 },
+	{ /* sentinel */ },
+};
+
+struct samsung_div_clock s3c2443_dividers[] __initdata = {
+	DIV_T(ARMDIV, "armdiv", "msysclk", CLKDIV0, 9, 4, armdiv_s3c2443_d),
+	DIV(0, "div_cam", "esysclk", CLKDIV1, 26, 4),
+};
+
+struct samsung_gate_clock s3c2443_gates[] __initdata = {
+	GATE(SCLK_HSSPI0, "sclk_hsspi0", "div_hsspi0_epll", SCLKCON, 14, 0, 0),
+	GATE(SCLK_CAM, "sclk_cam", "div_cam", SCLKCON, 11, 0, 0),
+	GATE(HCLK_CFC, "cfc", "hclk", HCLKCON, 17, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_CAM, "cam", "hclk", HCLKCON, 8, 0, 0),
+	GATE(PCLK_SPI1, "spi1", "pclk", PCLKCON, 15, 0, 0),
+	GATE(PCLK_SDI, "sdi", "pclk", PCLKCON, 5, 0, 0),
+};
+
+struct samsung_clock_alias s3c2443_aliases[] __initdata = {
+	ALIAS(SCLK_HSSPI0, "s3c2443-spi.0", "spi_busclk2"),
+	ALIAS(SCLK_HSMMC1, "s3c-sdhci.1", "mmc_busclk.2"),
+	ALIAS(SCLK_CAM, NULL, "camif-upll"),
+	ALIAS(PCLK_SPI1, "s3c2410-spi.0", "spi"),
+	ALIAS(PCLK_SDI, NULL, "sdi"),
+	ALIAS(HCLK_CFC, NULL, "cfc"),
+	ALIAS(ARMDIV, NULL, "armdiv"),
+};
+
+/* S3C2450 specific clocks */
+
+PNAME(s3c2450_cam_p) = { "div_cam", "hclk" };
+PNAME(s3c2450_hsspi1_p) = { "hsspi1_epll", "hsspi1_mpll" };
+PNAME(i2s1_p) = { "div_i2s1", "ext_i2s", "epllref", "epllref" };
+
+struct samsung_div_clock s3c2450_dividers[] __initdata = {
+	DIV(0, "div_cam", "esysclk", CLKDIV1, 26, 4),
+	DIV(0, "div_hsspi1_epll", "esysclk", CLKDIV2, 24, 2),
+	DIV(0, "div_hsspi1_mpll", "msysclk", CLKDIV2, 16, 4),
+	DIV(0, "div_i2s1", "esysclk", CLKDIV2, 12, 4),
+};
+
+struct samsung_mux_clock s3c2450_muxes[] __initdata = {
+	MUX(0, "mux_cam", s3c2450_cam_p, CLKSRC, 20, 1),
+	MUX(MUX_HSSPI1, "mux_hsspi1", s3c2450_hsspi1_p, CLKSRC, 19, 1),
+	MUX(0, "mux_i2s1", i2s1_p, CLKSRC, 12, 2),
+};
+
+struct samsung_gate_clock s3c2450_gates[] __initdata = {
+	GATE(SCLK_I2S1, "sclk_i2s1", "div_i2s1", SCLKCON, 5, 0, 0),
+	GATE(HCLK_CFC, "cfc", "hclk", HCLKCON, 17, 0, 0),
+	GATE(HCLK_CAM, "cam", "hclk", HCLKCON, 8, 0, 0),
+	GATE(HCLK_DMA7, "dma7", "hclk", HCLKCON, 7, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_DMA6, "dma6", "hclk", HCLKCON, 6, CLK_IGNORE_UNUSED, 0),
+	GATE(PCLK_I2S1, "i2s1", "pclk", PCLKCON, 17, 0, 0),
+	GATE(PCLK_I2C1, "i2c1", "pclk", PCLKCON, 16, 0, 0),
+	GATE(PCLK_SPI1, "spi1", "pclk", PCLKCON, 14, 0, 0),
+};
+
+struct samsung_clock_alias s3c2450_aliases[] __initdata = {
+	ALIAS(PCLK_SPI1, "s3c2443-spi.1", "spi"),
+	ALIAS(PCLK_SPI1, "s3c2443-spi.1", "spi_busclk0"),
+	ALIAS(MUX_HSSPI1, "s3c2443-spi.1", "spi_busclk2"),
+	ALIAS(PCLK_I2C1, "s3c2410-i2c.1", "i2c"),
+};
+
+/*
+ * fixed rate clocks generated outside the soc
+ * Only necessary until the devicetree-move is complete
+ */
+struct samsung_fixed_rate_clock s3c2443_common_frate_clks[] __initdata = {
+	FRATE(0, "xti", NULL, CLK_IS_ROOT, 0),
+	FRATE(0, "ext", NULL, CLK_IS_ROOT, 0),
+	FRATE(0, "ext_i2s", NULL, CLK_IS_ROOT, 0),
+	FRATE(0, "ext_uart", NULL, CLK_IS_ROOT, 0),
+};
+
+static void __init s3c2443_common_clk_register_fixed_ext(
+		struct samsung_clk_provider *ctx, unsigned long xti_f)
+{
+	s3c2443_common_frate_clks[0].fixed_rate = xti_f;
+	samsung_clk_register_fixed_rate(ctx, s3c2443_common_frate_clks,
+				ARRAY_SIZE(s3c2443_common_frate_clks));
+}
+
+void __init s3c2443_common_clk_init(struct device_node *np, unsigned long xti_f,
+				    int current_soc,
+				    void __iomem *base)
+{
+	struct samsung_clk_provider *ctx;
+	reg_base = base;
+
+	if (np) {
+		reg_base = of_iomap(np, 0);
+		if (!reg_base)
+			panic("%s: failed to map registers\n", __func__);
+	}
+
+	ctx = samsung_clk_init(np, reg_base, NR_CLKS);
+	if (!ctx)
+		panic("%s: unable to allocate context.\n", __func__);
+
+	/* Register external clocks only in non-dt cases */
+	if (!np)
+		s3c2443_common_clk_register_fixed_ext(ctx, xti_f);
+
+	/* Register PLLs. */
+	if (current_soc == S3C2416 || current_soc == S3C2450)
+		samsung_clk_register_pll(ctx, s3c2416_pll_clks,
+				ARRAY_SIZE(s3c2416_pll_clks), reg_base);
+	else
+		samsung_clk_register_pll(ctx, s3c2443_pll_clks,
+				ARRAY_SIZE(s3c2443_pll_clks), reg_base);
+
+	/* Register common internal clocks. */
+	samsung_clk_register_mux(ctx, s3c2443_common_muxes,
+			ARRAY_SIZE(s3c2443_common_muxes));
+	samsung_clk_register_div(ctx, s3c2443_common_dividers,
+			ARRAY_SIZE(s3c2443_common_dividers));
+	samsung_clk_register_gate(ctx, s3c2443_common_gates,
+		ARRAY_SIZE(s3c2443_common_gates));
+	samsung_clk_register_alias(ctx, s3c2443_common_aliases,
+		ARRAY_SIZE(s3c2443_common_aliases));
+
+	/* Register SoC-specific clocks. */
+	switch (current_soc) {
+	case S3C2450:
+		samsung_clk_register_div(ctx, s3c2450_dividers,
+				ARRAY_SIZE(s3c2450_dividers));
+		samsung_clk_register_mux(ctx, s3c2450_muxes,
+				ARRAY_SIZE(s3c2450_muxes));
+		samsung_clk_register_gate(ctx, s3c2450_gates,
+				ARRAY_SIZE(s3c2450_gates));
+		samsung_clk_register_alias(ctx, s3c2450_aliases,
+				ARRAY_SIZE(s3c2450_aliases));
+		/* fall through, as s3c2450 extends the s3c2416 clocks */
+	case S3C2416:
+		samsung_clk_register_div(ctx, s3c2416_dividers,
+				ARRAY_SIZE(s3c2416_dividers));
+		samsung_clk_register_mux(ctx, s3c2416_muxes,
+				ARRAY_SIZE(s3c2416_muxes));
+		samsung_clk_register_gate(ctx, s3c2416_gates,
+				ARRAY_SIZE(s3c2416_gates));
+		samsung_clk_register_alias(ctx, s3c2416_aliases,
+				ARRAY_SIZE(s3c2416_aliases));
+		break;
+	case S3C2443:
+		samsung_clk_register_div(ctx, s3c2443_dividers,
+				ARRAY_SIZE(s3c2443_dividers));
+		samsung_clk_register_gate(ctx, s3c2443_gates,
+				ARRAY_SIZE(s3c2443_gates));
+		samsung_clk_register_alias(ctx, s3c2443_aliases,
+				ARRAY_SIZE(s3c2443_aliases));
+		break;
+	}
+
+	s3c2443_clk_sleep_init();
+}
+
+static void __init s3c2416_clk_init(struct device_node *np)
+{
+	s3c2443_common_clk_init(np, 0, S3C2416, 0);
+}
+CLK_OF_DECLARE(s3c2416_clk, "samsung,s3c2416-clock", s3c2416_clk_init);
+
+static void __init s3c2443_clk_init(struct device_node *np)
+{
+	s3c2443_common_clk_init(np, 0, S3C2443, 0);
+}
+CLK_OF_DECLARE(s3c2443_clk, "samsung,s3c2443-clock", s3c2443_clk_init);
+
+static void __init s3c2450_clk_init(struct device_node *np)
+{
+	s3c2443_common_clk_init(np, 0, S3C2450, 0);
+}
+CLK_OF_DECLARE(s3c2450_clk, "samsung,s3c2450-clock", s3c2450_clk_init);
diff --git a/drivers/clk/samsung/clk-s3c64xx.c b/drivers/clk/samsung/clk-s3c64xx.c
index 8bda658137a..efa16ee592c 100644
--- a/drivers/clk/samsung/clk-s3c64xx.c
+++ b/drivers/clk/samsung/clk-s3c64xx.c
@@ -442,12 +442,14 @@ static struct samsung_clock_alias s3c6410_clock_aliases[] = {
 	ALIAS(MEM0_SROM, NULL, "srom"),
 };
 
-static void __init s3c64xx_clk_register_fixed_ext(unsigned long fin_pll_f,
-							unsigned long xusbxti_f)
+static void __init s3c64xx_clk_register_fixed_ext(
+				struct samsung_clk_provider *ctx,
+				unsigned long fin_pll_f,
+				unsigned long xusbxti_f)
 {
 	s3c64xx_fixed_rate_ext_clks[0].fixed_rate = fin_pll_f;
 	s3c64xx_fixed_rate_ext_clks[1].fixed_rate = xusbxti_f;
-	samsung_clk_register_fixed_rate(s3c64xx_fixed_rate_ext_clks,
+	samsung_clk_register_fixed_rate(ctx, s3c64xx_fixed_rate_ext_clks,
 				ARRAY_SIZE(s3c64xx_fixed_rate_ext_clks));
 }
 
@@ -456,6 +458,8 @@ void __init s3c64xx_clk_init(struct device_node *np, unsigned long xtal_f,
 			     unsigned long xusbxti_f, bool s3c6400,
 			     void __iomem *base)
 {
+	struct samsung_clk_provider *ctx;
+
 	reg_base = base;
 	is_s3c6400 = s3c6400;
 
@@ -465,48 +469,50 @@ void __init s3c64xx_clk_init(struct device_node *np, unsigned long xtal_f,
 			panic("%s: failed to map registers\n", __func__);
 	}
 
-	samsung_clk_init(np, reg_base, NR_CLKS);
+	ctx = samsung_clk_init(np, reg_base, NR_CLKS);
+	if (!ctx)
+		panic("%s: unable to allocate context.\n", __func__);
 
 	/* Register external clocks. */
 	if (!np)
-		s3c64xx_clk_register_fixed_ext(xtal_f, xusbxti_f);
+		s3c64xx_clk_register_fixed_ext(ctx, xtal_f, xusbxti_f);
 
 	/* Register PLLs. */
-	samsung_clk_register_pll(s3c64xx_pll_clks,
+	samsung_clk_register_pll(ctx, s3c64xx_pll_clks,
 				ARRAY_SIZE(s3c64xx_pll_clks), reg_base);
 
 	/* Register common internal clocks. */
-	samsung_clk_register_fixed_rate(s3c64xx_fixed_rate_clks,
+	samsung_clk_register_fixed_rate(ctx, s3c64xx_fixed_rate_clks,
 					ARRAY_SIZE(s3c64xx_fixed_rate_clks));
-	samsung_clk_register_mux(s3c64xx_mux_clks,
+	samsung_clk_register_mux(ctx, s3c64xx_mux_clks,
 					ARRAY_SIZE(s3c64xx_mux_clks));
-	samsung_clk_register_div(s3c64xx_div_clks,
+	samsung_clk_register_div(ctx, s3c64xx_div_clks,
 					ARRAY_SIZE(s3c64xx_div_clks));
-	samsung_clk_register_gate(s3c64xx_gate_clks,
+	samsung_clk_register_gate(ctx, s3c64xx_gate_clks,
 					ARRAY_SIZE(s3c64xx_gate_clks));
 
 	/* Register SoC-specific clocks. */
 	if (is_s3c6400) {
-		samsung_clk_register_mux(s3c6400_mux_clks,
+		samsung_clk_register_mux(ctx, s3c6400_mux_clks,
 					ARRAY_SIZE(s3c6400_mux_clks));
-		samsung_clk_register_div(s3c6400_div_clks,
+		samsung_clk_register_div(ctx, s3c6400_div_clks,
 					ARRAY_SIZE(s3c6400_div_clks));
-		samsung_clk_register_gate(s3c6400_gate_clks,
+		samsung_clk_register_gate(ctx, s3c6400_gate_clks,
 					ARRAY_SIZE(s3c6400_gate_clks));
-		samsung_clk_register_alias(s3c6400_clock_aliases,
+		samsung_clk_register_alias(ctx, s3c6400_clock_aliases,
 					ARRAY_SIZE(s3c6400_clock_aliases));
 	} else {
-		samsung_clk_register_mux(s3c6410_mux_clks,
+		samsung_clk_register_mux(ctx, s3c6410_mux_clks,
 					ARRAY_SIZE(s3c6410_mux_clks));
-		samsung_clk_register_div(s3c6410_div_clks,
+		samsung_clk_register_div(ctx, s3c6410_div_clks,
 					ARRAY_SIZE(s3c6410_div_clks));
-		samsung_clk_register_gate(s3c6410_gate_clks,
+		samsung_clk_register_gate(ctx, s3c6410_gate_clks,
 					ARRAY_SIZE(s3c6410_gate_clks));
-		samsung_clk_register_alias(s3c6410_clock_aliases,
+		samsung_clk_register_alias(ctx, s3c6410_clock_aliases,
 					ARRAY_SIZE(s3c6410_clock_aliases));
 	}
 
-	samsung_clk_register_alias(s3c64xx_clock_aliases,
+	samsung_clk_register_alias(ctx, s3c64xx_clock_aliases,
 					ARRAY_SIZE(s3c64xx_clock_aliases));
 	s3c64xx_clk_sleep_init();
 
diff --git a/drivers/clk/samsung/clk.c b/drivers/clk/samsung/clk.c
index 91bec3ebdc8..49629c71c9e 100644
--- a/drivers/clk/samsung/clk.c
+++ b/drivers/clk/samsung/clk.c
@@ -14,13 +14,6 @@
 #include <linux/syscore_ops.h>
 #include "clk.h"
 
-static DEFINE_SPINLOCK(lock);
-static struct clk **clk_table;
-static void __iomem *reg_base;
-#ifdef CONFIG_OF
-static struct clk_onecell_data clk_data;
-#endif
-
 void samsung_clk_save(void __iomem *base,
 				    struct samsung_clk_reg_dump *rd,
 				    unsigned int num_regs)
@@ -55,40 +48,58 @@ struct samsung_clk_reg_dump *samsung_clk_alloc_reg_dump(
 }
 
 /* setup the essentials required to support clock lookup using ccf */
-void __init samsung_clk_init(struct device_node *np, void __iomem *base,
-			     unsigned long nr_clks)
+struct samsung_clk_provider *__init samsung_clk_init(struct device_node *np,
+			void __iomem *base, unsigned long nr_clks)
 {
-	reg_base = base;
+	struct samsung_clk_provider *ctx;
+	struct clk **clk_table;
+	int ret;
+	int i;
 
-	clk_table = kzalloc(sizeof(struct clk *) * nr_clks, GFP_KERNEL);
+	ctx = kzalloc(sizeof(struct samsung_clk_provider), GFP_KERNEL);
+	if (!ctx)
+		panic("could not allocate clock provider context.\n");
+
+	clk_table = kcalloc(nr_clks, sizeof(struct clk *), GFP_KERNEL);
 	if (!clk_table)
 		panic("could not allocate clock lookup table\n");
 
+	for (i = 0; i < nr_clks; ++i)
+		clk_table[i] = ERR_PTR(-ENOENT);
+
+	ctx->reg_base = base;
+	ctx->clk_data.clks = clk_table;
+	ctx->clk_data.clk_num = nr_clks;
+	spin_lock_init(&ctx->lock);
+
 	if (!np)
-		return;
+		return ctx;
 
-#ifdef CONFIG_OF
-	clk_data.clks = clk_table;
-	clk_data.clk_num = nr_clks;
-	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
-#endif
+	ret = of_clk_add_provider(np, of_clk_src_onecell_get,
+			&ctx->clk_data);
+	if (ret)
+		panic("could not register clock provide\n");
+
+	return ctx;
 }
 
 /* add a clock instance to the clock lookup table used for dt based lookup */
-void samsung_clk_add_lookup(struct clk *clk, unsigned int id)
+void samsung_clk_add_lookup(struct samsung_clk_provider *ctx, struct clk *clk,
+				unsigned int id)
 {
-	if (clk_table && id)
-		clk_table[id] = clk;
+	if (ctx->clk_data.clks && id)
+		ctx->clk_data.clks[id] = clk;
 }
 
 /* register a list of aliases */
-void __init samsung_clk_register_alias(struct samsung_clock_alias *list,
-					unsigned int nr_clk)
+void __init samsung_clk_register_alias(struct samsung_clk_provider *ctx,
+				struct samsung_clock_alias *list,
+				unsigned int nr_clk)
 {
 	struct clk *clk;
 	unsigned int idx, ret;
 
-	if (!clk_table) {
+	if (!ctx->clk_data.clks) {
 		pr_err("%s: clock table missing\n", __func__);
 		return;
 	}
@@ -100,7 +111,7 @@ void __init samsung_clk_register_alias(struct samsung_clock_alias *list,
 			continue;
 		}
 
-		clk = clk_table[list->id];
+		clk = ctx->clk_data.clks[list->id];
 		if (!clk) {
 			pr_err("%s: failed to find clock %d\n", __func__,
 				list->id);
@@ -115,7 +126,7 @@ void __init samsung_clk_register_alias(struct samsung_clock_alias *list,
 }
 
 /* register a list of fixed clocks */
-void __init samsung_clk_register_fixed_rate(
+void __init samsung_clk_register_fixed_rate(struct samsung_clk_provider *ctx,
 		struct samsung_fixed_rate_clock *list, unsigned int nr_clk)
 {
 	struct clk *clk;
@@ -130,7 +141,7 @@ void __init samsung_clk_register_fixed_rate(
 			continue;
 		}
 
-		samsung_clk_add_lookup(clk, list->id);
+		samsung_clk_add_lookup(ctx, clk, list->id);
 
 		/*
 		 * Unconditionally add a clock lookup for the fixed rate clocks.
@@ -144,7 +155,7 @@ void __init samsung_clk_register_fixed_rate(
 }
 
 /* register a list of fixed factor clocks */
-void __init samsung_clk_register_fixed_factor(
+void __init samsung_clk_register_fixed_factor(struct samsung_clk_provider *ctx,
 		struct samsung_fixed_factor_clock *list, unsigned int nr_clk)
 {
 	struct clk *clk;
@@ -159,28 +170,30 @@ void __init samsung_clk_register_fixed_factor(
 			continue;
 		}
 
-		samsung_clk_add_lookup(clk, list->id);
+		samsung_clk_add_lookup(ctx, clk, list->id);
 	}
 }
 
 /* register a list of mux clocks */
-void __init samsung_clk_register_mux(struct samsung_mux_clock *list,
-					unsigned int nr_clk)
+void __init samsung_clk_register_mux(struct samsung_clk_provider *ctx,
+				struct samsung_mux_clock *list,
+				unsigned int nr_clk)
 {
 	struct clk *clk;
 	unsigned int idx, ret;
 
 	for (idx = 0; idx < nr_clk; idx++, list++) {
 		clk = clk_register_mux(NULL, list->name, list->parent_names,
-			list->num_parents, list->flags, reg_base + list->offset,
-			list->shift, list->width, list->mux_flags, &lock);
+			list->num_parents, list->flags,
+			ctx->reg_base + list->offset,
+			list->shift, list->width, list->mux_flags, &ctx->lock);
 		if (IS_ERR(clk)) {
 			pr_err("%s: failed to register clock %s\n", __func__,
 				list->name);
 			continue;
 		}
 
-		samsung_clk_add_lookup(clk, list->id);
+		samsung_clk_add_lookup(ctx, clk, list->id);
 
 		/* register a clock lookup only if a clock alias is specified */
 		if (list->alias) {
@@ -194,8 +207,9 @@ void __init samsung_clk_register_mux(struct samsung_mux_clock *list,
 }
 
 /* register a list of div clocks */
-void __init samsung_clk_register_div(struct samsung_div_clock *list,
-					unsigned int nr_clk)
+void __init samsung_clk_register_div(struct samsung_clk_provider *ctx,
+				struct samsung_div_clock *list,
+				unsigned int nr_clk)
 {
 	struct clk *clk;
 	unsigned int idx, ret;
@@ -203,22 +217,22 @@ void __init samsung_clk_register_div(struct samsung_div_clock *list,
 	for (idx = 0; idx < nr_clk; idx++, list++) {
 		if (list->table)
 			clk = clk_register_divider_table(NULL, list->name,
-					list->parent_name, list->flags,
-					reg_base + list->offset, list->shift,
-					list->width, list->div_flags,
-					list->table, &lock);
+				list->parent_name, list->flags,
+				ctx->reg_base + list->offset,
+				list->shift, list->width, list->div_flags,
+				list->table, &ctx->lock);
 		else
 			clk = clk_register_divider(NULL, list->name,
-					list->parent_name, list->flags,
-					reg_base + list->offset, list->shift,
-					list->width, list->div_flags, &lock);
+				list->parent_name, list->flags,
+				ctx->reg_base + list->offset, list->shift,
+				list->width, list->div_flags, &ctx->lock);
 		if (IS_ERR(clk)) {
 			pr_err("%s: failed to register clock %s\n", __func__,
 				list->name);
 			continue;
 		}
 
-		samsung_clk_add_lookup(clk, list->id);
+		samsung_clk_add_lookup(ctx, clk, list->id);
 
 		/* register a clock lookup only if a clock alias is specified */
 		if (list->alias) {
@@ -232,16 +246,17 @@ void __init samsung_clk_register_div(struct samsung_div_clock *list,
 }
 
 /* register a list of gate clocks */
-void __init samsung_clk_register_gate(struct samsung_gate_clock *list,
-						unsigned int nr_clk)
+void __init samsung_clk_register_gate(struct samsung_clk_provider *ctx,
+				struct samsung_gate_clock *list,
+				unsigned int nr_clk)
 {
 	struct clk *clk;
 	unsigned int idx, ret;
 
 	for (idx = 0; idx < nr_clk; idx++, list++) {
 		clk = clk_register_gate(NULL, list->name, list->parent_name,
-				list->flags, reg_base + list->offset,
-				list->bit_idx, list->gate_flags, &lock);
+				list->flags, ctx->reg_base + list->offset,
+				list->bit_idx, list->gate_flags, &ctx->lock);
 		if (IS_ERR(clk)) {
 			pr_err("%s: failed to register clock %s\n", __func__,
 				list->name);
@@ -257,7 +272,7 @@ void __init samsung_clk_register_gate(struct samsung_gate_clock *list,
 					__func__, list->alias);
 		}
 
-		samsung_clk_add_lookup(clk, list->id);
+		samsung_clk_add_lookup(ctx, clk, list->id);
 	}
 }
 
@@ -266,21 +281,21 @@ void __init samsung_clk_register_gate(struct samsung_gate_clock *list,
  * tree and register it
  */
 #ifdef CONFIG_OF
-void __init samsung_clk_of_register_fixed_ext(
+void __init samsung_clk_of_register_fixed_ext(struct samsung_clk_provider *ctx,
 			struct samsung_fixed_rate_clock *fixed_rate_clk,
 			unsigned int nr_fixed_rate_clk,
 			struct of_device_id *clk_matches)
 {
 	const struct of_device_id *match;
-	struct device_node *np;
+	struct device_node *clk_np;
 	u32 freq;
 
-	for_each_matching_node_and_match(np, clk_matches, &match) {
-		if (of_property_read_u32(np, "clock-frequency", &freq))
+	for_each_matching_node_and_match(clk_np, clk_matches, &match) {
+		if (of_property_read_u32(clk_np, "clock-frequency", &freq))
 			continue;
-		fixed_rate_clk[(u32)match->data].fixed_rate = freq;
+		fixed_rate_clk[(unsigned long)match->data].fixed_rate = freq;
 	}
-	samsung_clk_register_fixed_rate(fixed_rate_clk, nr_fixed_rate_clk);
+	samsung_clk_register_fixed_rate(ctx, fixed_rate_clk, nr_fixed_rate_clk);
 }
 #endif
 
diff --git a/drivers/clk/samsung/clk.h b/drivers/clk/samsung/clk.h
index c7141ba826e..9693b80d924 100644
--- a/drivers/clk/samsung/clk.h
+++ b/drivers/clk/samsung/clk.h
@@ -22,6 +22,18 @@
 #include "clk-pll.h"
 
 /**
+ * struct samsung_clk_provider: information about clock provider
+ * @reg_base: virtual address for the register base.
+ * @clk_data: holds clock related data like clk* and number of clocks.
+ * @lock: maintains exclusion bwtween callbacks for a given clock-provider.
+ */
+struct samsung_clk_provider {
+	void __iomem *reg_base;
+	struct clk_onecell_data clk_data;
+	spinlock_t lock;
+};
+
+/**
  * struct samsung_clock_alias: information about mux clock
  * @id: platform specific id of the clock.
  * @dev_name: name of the device to which this clock belongs.
@@ -312,40 +324,52 @@ struct samsung_pll_clock {
 	__PLL(_typ, _id, NULL, _name, _pname, CLK_GET_RATE_NOCACHE,	\
 		_lock, _con, _rtable, _alias)
 
-extern void __init samsung_clk_init(struct device_node *np, void __iomem *base,
-				    unsigned long nr_clks);
+extern struct samsung_clk_provider *__init samsung_clk_init(
+			struct device_node *np, void __iomem *base,
+			unsigned long nr_clks);
 extern void __init samsung_clk_of_register_fixed_ext(
-		struct samsung_fixed_rate_clock *fixed_rate_clk,
-		unsigned int nr_fixed_rate_clk,
-		struct of_device_id *clk_matches);
+			struct samsung_clk_provider *ctx,
+			struct samsung_fixed_rate_clock *fixed_rate_clk,
+			unsigned int nr_fixed_rate_clk,
+			struct of_device_id *clk_matches);
 
-extern void samsung_clk_add_lookup(struct clk *clk, unsigned int id);
+extern void samsung_clk_add_lookup(struct samsung_clk_provider *ctx,
+			struct clk *clk, unsigned int id);
 
-extern void samsung_clk_register_alias(struct samsung_clock_alias *list,
-		unsigned int nr_clk);
+extern void samsung_clk_register_alias(struct samsung_clk_provider *ctx,
+			struct samsung_clock_alias *list,
+			unsigned int nr_clk);
 extern void __init samsung_clk_register_fixed_rate(
-		struct samsung_fixed_rate_clock *clk_list, unsigned int nr_clk);
+			struct samsung_clk_provider *ctx,
+			struct samsung_fixed_rate_clock *clk_list,
+			unsigned int nr_clk);
 extern void __init samsung_clk_register_fixed_factor(
-		struct samsung_fixed_factor_clock *list, unsigned int nr_clk);
-extern void __init samsung_clk_register_mux(struct samsung_mux_clock *clk_list,
-		unsigned int nr_clk);
-extern void __init samsung_clk_register_div(struct samsung_div_clock *clk_list,
-		unsigned int nr_clk);
-extern void __init samsung_clk_register_gate(
-		struct samsung_gate_clock *clk_list, unsigned int nr_clk);
-extern void __init samsung_clk_register_pll(struct samsung_pll_clock *pll_list,
-		unsigned int nr_clk, void __iomem *base);
+			struct samsung_clk_provider *ctx,
+			struct samsung_fixed_factor_clock *list,
+			unsigned int nr_clk);
+extern void __init samsung_clk_register_mux(struct samsung_clk_provider *ctx,
+			struct samsung_mux_clock *clk_list,
+			unsigned int nr_clk);
+extern void __init samsung_clk_register_div(struct samsung_clk_provider *ctx,
+			struct samsung_div_clock *clk_list,
+			unsigned int nr_clk);
+extern void __init samsung_clk_register_gate(struct samsung_clk_provider *ctx,
+			struct samsung_gate_clock *clk_list,
+			unsigned int nr_clk);
+extern void __init samsung_clk_register_pll(struct samsung_clk_provider *ctx,
+			struct samsung_pll_clock *pll_list,
+			unsigned int nr_clk, void __iomem *base);
 
 extern unsigned long _get_rate(const char *clk_name);
 
 extern void samsung_clk_save(void __iomem *base,
-			     struct samsung_clk_reg_dump *rd,
-			     unsigned int num_regs);
+			struct samsung_clk_reg_dump *rd,
+			unsigned int num_regs);
 extern void samsung_clk_restore(void __iomem *base,
-				const struct samsung_clk_reg_dump *rd,
-				unsigned int num_regs);
+			const struct samsung_clk_reg_dump *rd,
+			unsigned int num_regs);
 extern struct samsung_clk_reg_dump *samsung_clk_alloc_reg_dump(
-						const unsigned long *rdump,
-						unsigned long nr_rdump);
+			const unsigned long *rdump,
+			unsigned long nr_rdump);
 
 #endif /* __SAMSUNG_CLK_H */
diff --git a/drivers/clk/shmobile/Makefile b/drivers/clk/shmobile/Makefile
index 5404cb931eb..e0029237827 100644
--- a/drivers/clk/shmobile/Makefile
+++ b/drivers/clk/shmobile/Makefile
@@ -1,5 +1,7 @@
 obj-$(CONFIG_ARCH_EMEV2)		+= clk-emev2.o
 obj-$(CONFIG_ARCH_R7S72100)		+= clk-rz.o
+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_SHMOBILE_MULTI)	+= clk-div6.o
diff --git a/drivers/clk/shmobile/clk-mstp.c b/drivers/clk/shmobile/clk-mstp.c
index 2e5810c88d1..2d2fe773ac8 100644
--- a/drivers/clk/shmobile/clk-mstp.c
+++ b/drivers/clk/shmobile/clk-mstp.c
@@ -112,7 +112,7 @@ static int cpg_mstp_clock_is_enabled(struct clk_hw *hw)
 	else
 		value = clk_readl(group->smstpcr);
 
-	return !!(value & BIT(clock->bit_index));
+	return !(value & BIT(clock->bit_index));
 }
 
 static const struct clk_ops cpg_mstp_clock_ops = {
@@ -156,6 +156,7 @@ cpg_mstp_clock_register(const char *name, const char *parent_name,
 static void __init cpg_mstp_clocks_init(struct device_node *np)
 {
 	struct mstp_clock_group *group;
+	const char *idxname;
 	struct clk **clks;
 	unsigned int i;
 
@@ -184,6 +185,11 @@ static void __init cpg_mstp_clocks_init(struct device_node *np)
 	for (i = 0; i < MSTP_MAX_CLOCKS; ++i)
 		clks[i] = ERR_PTR(-ENOENT);
 
+	if (of_find_property(np, "clock-indices", &i))
+		idxname = "clock-indices";
+	else
+		idxname = "renesas,clock-indices";
+
 	for (i = 0; i < MSTP_MAX_CLOCKS; ++i) {
 		const char *parent_name;
 		const char *name;
@@ -197,8 +203,7 @@ static void __init cpg_mstp_clocks_init(struct device_node *np)
 			continue;
 
 		parent_name = of_clk_get_parent_name(np, i);
-		ret = of_property_read_u32_index(np, "renesas,clock-indices", i,
-						 &clkidx);
+		ret = of_property_read_u32_index(np, idxname, i, &clkidx);
 		if (parent_name == NULL || ret < 0)
 			break;
 
diff --git a/drivers/clk/shmobile/clk-r8a7740.c b/drivers/clk/shmobile/clk-r8a7740.c
new file mode 100644
index 00000000000..1e2eaae21e0
--- /dev/null
+++ b/drivers/clk/shmobile/clk-r8a7740.c
@@ -0,0 +1,199 @@
+/*
+ * r8a7740 Core CPG Clocks
+ *
+ * Copyright (C) 2014  Ulrich Hecht
+ *
+ * This program is free software; 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 <linux/clkdev.h>
+#include <linux/clk/shmobile.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/spinlock.h>
+
+struct r8a7740_cpg {
+	struct clk_onecell_data data;
+	spinlock_t lock;
+	void __iomem *reg;
+};
+
+#define CPG_FRQCRA	0x00
+#define CPG_FRQCRB	0x04
+#define CPG_PLLC2CR	0x2c
+#define CPG_USBCKCR	0x8c
+#define CPG_FRQCRC	0xe0
+
+#define CLK_ENABLE_ON_INIT BIT(0)
+
+struct div4_clk {
+	const char *name;
+	unsigned int reg;
+	unsigned int shift;
+	int flags;
+};
+
+static struct div4_clk div4_clks[] = {
+	{ "i", CPG_FRQCRA, 20, CLK_ENABLE_ON_INIT },
+	{ "zg", CPG_FRQCRA, 16, CLK_ENABLE_ON_INIT },
+	{ "b", CPG_FRQCRA,  8, CLK_ENABLE_ON_INIT },
+	{ "m1", CPG_FRQCRA,  4, CLK_ENABLE_ON_INIT },
+	{ "hp", CPG_FRQCRB,  4, 0 },
+	{ "hpp", CPG_FRQCRC, 20, 0 },
+	{ "usbp", CPG_FRQCRC, 16, 0 },
+	{ "s", CPG_FRQCRC, 12, 0 },
+	{ "zb", CPG_FRQCRC,  8, 0 },
+	{ "m3", CPG_FRQCRC,  4, 0 },
+	{ "cp", CPG_FRQCRC,  0, 0 },
+	{ NULL, 0, 0, 0 },
+};
+
+static const struct clk_div_table div4_div_table[] = {
+	{ 0, 2 }, { 1, 3 }, { 2, 4 }, { 3, 6 }, { 4, 8 }, { 5, 12 },
+	{ 6, 16 }, { 7, 18 }, { 8, 24 }, { 9, 32 }, { 10, 36 }, { 11, 48 },
+	{ 13, 72 }, { 14, 96 }, { 0, 0 }
+};
+
+static u32 cpg_mode __initdata;
+
+static struct clk * __init
+r8a7740_cpg_register_clock(struct device_node *np, struct r8a7740_cpg *cpg,
+			     const char *name)
+{
+	const struct clk_div_table *table = NULL;
+	const char *parent_name;
+	unsigned int shift, reg;
+	unsigned int mult = 1;
+	unsigned int div = 1;
+
+	if (!strcmp(name, "r")) {
+		switch (cpg_mode & (BIT(2) | BIT(1))) {
+		case BIT(1) | BIT(2):
+			/* extal1 */
+			parent_name = of_clk_get_parent_name(np, 0);
+			div = 2048;
+			break;
+		case BIT(2):
+			/* extal1 */
+			parent_name = of_clk_get_parent_name(np, 0);
+			div = 1024;
+			break;
+		default:
+			/* extalr */
+			parent_name = of_clk_get_parent_name(np, 2);
+			break;
+		}
+	} else if (!strcmp(name, "system")) {
+		parent_name = of_clk_get_parent_name(np, 0);
+		if (cpg_mode & BIT(1))
+			div = 2;
+	} else if (!strcmp(name, "pllc0")) {
+		/* PLLC0/1 are configurable multiplier clocks. Register them as
+		 * fixed factor clocks for now as there's no generic multiplier
+		 * clock implementation and we currently have no need to change
+		 * the multiplier value.
+		 */
+		u32 value = clk_readl(cpg->reg + CPG_FRQCRC);
+		parent_name = "system";
+		mult = ((value >> 24) & 0x7f) + 1;
+	} else if (!strcmp(name, "pllc1")) {
+		u32 value = clk_readl(cpg->reg + CPG_FRQCRA);
+		parent_name = "system";
+		mult = ((value >> 24) & 0x7f) + 1;
+		div = 2;
+	} else if (!strcmp(name, "pllc2")) {
+		u32 value = clk_readl(cpg->reg + CPG_PLLC2CR);
+		parent_name = "system";
+		mult = ((value >> 24) & 0x3f) + 1;
+	} else if (!strcmp(name, "usb24s")) {
+		u32 value = clk_readl(cpg->reg + CPG_USBCKCR);
+		if (value & BIT(7))
+			/* extal2 */
+			parent_name = of_clk_get_parent_name(np, 1);
+		else
+			parent_name = "system";
+		if (!(value & BIT(6)))
+			div = 2;
+	} else {
+		struct div4_clk *c;
+		for (c = div4_clks; c->name; c++) {
+			if (!strcmp(name, c->name)) {
+				parent_name = "pllc1";
+				table = div4_div_table;
+				reg = c->reg;
+				shift = c->shift;
+				break;
+			}
+		}
+		if (!c->name)
+			return ERR_PTR(-EINVAL);
+	}
+
+	if (!table) {
+		return clk_register_fixed_factor(NULL, name, parent_name, 0,
+						 mult, div);
+	} else {
+		return clk_register_divider_table(NULL, name, parent_name, 0,
+						  cpg->reg + reg, shift, 4, 0,
+						  table, &cpg->lock);
+	}
+}
+
+static void __init r8a7740_cpg_clocks_init(struct device_node *np)
+{
+	struct r8a7740_cpg *cpg;
+	struct clk **clks;
+	unsigned int i;
+	int num_clks;
+
+	if (of_property_read_u32(np, "renesas,mode", &cpg_mode))
+		pr_warn("%s: missing renesas,mode property\n", __func__);
+
+	num_clks = of_property_count_strings(np, "clock-output-names");
+	if (num_clks < 0) {
+		pr_err("%s: failed to count clocks\n", __func__);
+		return;
+	}
+
+	cpg = kzalloc(sizeof(*cpg), GFP_KERNEL);
+	clks = kzalloc(num_clks * sizeof(*clks), GFP_KERNEL);
+	if (cpg == NULL || clks == NULL) {
+		/* We're leaking memory on purpose, there's no point in cleaning
+		 * up as the system won't boot anyway.
+		 */
+		return;
+	}
+
+	spin_lock_init(&cpg->lock);
+
+	cpg->data.clks = clks;
+	cpg->data.clk_num = num_clks;
+
+	cpg->reg = of_iomap(np, 0);
+	if (WARN_ON(cpg->reg == NULL))
+		return;
+
+	for (i = 0; i < num_clks; ++i) {
+		const char *name;
+		struct clk *clk;
+
+		of_property_read_string_index(np, "clock-output-names", i,
+					      &name);
+
+		clk = r8a7740_cpg_register_clock(np, cpg, name);
+		if (IS_ERR(clk))
+			pr_err("%s: failed to register %s %s clock (%ld)\n",
+			       __func__, np->name, name, PTR_ERR(clk));
+		else
+			cpg->data.clks[i] = clk;
+	}
+
+	of_clk_add_provider(np, of_clk_src_onecell_get, &cpg->data);
+}
+CLK_OF_DECLARE(r8a7740_cpg_clks, "renesas,r8a7740-cpg-clocks",
+	       r8a7740_cpg_clocks_init);
diff --git a/drivers/clk/shmobile/clk-r8a7779.c b/drivers/clk/shmobile/clk-r8a7779.c
new file mode 100644
index 00000000000..652ecacb6da
--- /dev/null
+++ b/drivers/clk/shmobile/clk-r8a7779.c
@@ -0,0 +1,180 @@
+/*
+ * r8a7779 Core CPG Clocks
+ *
+ * Copyright (C) 2013, 2014 Horms Solutions Ltd.
+ *
+ * Contact: Simon Horman <horms@verge.net.au>
+ *
+ * This program is free software; 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 <linux/clkdev.h>
+#include <linux/clk/shmobile.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/spinlock.h>
+
+#include <dt-bindings/clock/r8a7779-clock.h>
+
+#define CPG_NUM_CLOCKS			(R8A7779_CLK_OUT + 1)
+
+struct r8a7779_cpg {
+	struct clk_onecell_data data;
+	spinlock_t lock;
+	void __iomem *reg;
+};
+
+/* -----------------------------------------------------------------------------
+ * CPG Clock Data
+ */
+
+/*
+ *		MD1 = 1			MD1 = 0
+ *		(PLLA = 1500)		(PLLA = 1600)
+ *		(MHz)			(MHz)
+ *------------------------------------------------+--------------------
+ * clkz		1000   (2/3)		800   (1/2)
+ * clkzs	 250   (1/6)		200   (1/8)
+ * clki		 750   (1/2)		800   (1/2)
+ * clks		 250   (1/6)		200   (1/8)
+ * clks1	 125   (1/12)		100   (1/16)
+ * clks3	 187.5 (1/8)		200   (1/8)
+ * clks4	  93.7 (1/16)		100   (1/16)
+ * clkp		  62.5 (1/24)		 50   (1/32)
+ * clkg		  62.5 (1/24)		 66.6 (1/24)
+ * clkb, CLKOUT
+ * (MD2 = 0)	  62.5 (1/24)		 66.6 (1/24)
+ * (MD2 = 1)	  41.6 (1/36)		 50   (1/32)
+ */
+
+#define CPG_CLK_CONFIG_INDEX(md)	(((md) & (BIT(2)|BIT(1))) >> 1)
+
+struct cpg_clk_config {
+	unsigned int z_mult;
+	unsigned int z_div;
+	unsigned int zs_and_s_div;
+	unsigned int s1_div;
+	unsigned int p_div;
+	unsigned int b_and_out_div;
+};
+
+static const struct cpg_clk_config cpg_clk_configs[4] __initconst = {
+	{ 1, 2, 8, 16, 32, 24 },
+	{ 2, 3, 6, 12, 24, 24 },
+	{ 1, 2, 8, 16, 32, 32 },
+	{ 2, 3, 6, 12, 24, 36 },
+};
+
+/*
+ *   MD		PLLA Ratio
+ * 12 11
+ *------------------------
+ * 0  0		x42
+ * 0  1		x48
+ * 1  0		x56
+ * 1  1		x64
+ */
+
+#define CPG_PLLA_MULT_INDEX(md)	(((md) & (BIT(12)|BIT(11))) >> 11)
+
+static const unsigned int cpg_plla_mult[4] __initconst = { 42, 48, 56, 64 };
+
+/* -----------------------------------------------------------------------------
+ * Initialization
+ */
+
+static u32 cpg_mode __initdata;
+
+static struct clk * __init
+r8a7779_cpg_register_clock(struct device_node *np, struct r8a7779_cpg *cpg,
+			   const struct cpg_clk_config *config,
+			   unsigned int plla_mult, const char *name)
+{
+	const char *parent_name = "plla";
+	unsigned int mult = 1;
+	unsigned int div = 1;
+
+	if (!strcmp(name, "plla")) {
+		parent_name = of_clk_get_parent_name(np, 0);
+		mult = plla_mult;
+	} else if (!strcmp(name, "z")) {
+		div = config->z_div;
+		mult = config->z_mult;
+	} else if (!strcmp(name, "zs") || !strcmp(name, "s")) {
+		div = config->zs_and_s_div;
+	} else if (!strcmp(name, "s1")) {
+		div = config->s1_div;
+	} else if (!strcmp(name, "p")) {
+		div = config->p_div;
+	} else if (!strcmp(name, "b") || !strcmp(name, "out")) {
+		div = config->b_and_out_div;
+	} else {
+		return ERR_PTR(-EINVAL);
+	}
+
+	return clk_register_fixed_factor(NULL, name, parent_name, 0, mult, div);
+}
+
+static void __init r8a7779_cpg_clocks_init(struct device_node *np)
+{
+	const struct cpg_clk_config *config;
+	struct r8a7779_cpg *cpg;
+	struct clk **clks;
+	unsigned int i, plla_mult;
+	int num_clks;
+
+	num_clks = of_property_count_strings(np, "clock-output-names");
+	if (num_clks < 0) {
+		pr_err("%s: failed to count clocks\n", __func__);
+		return;
+	}
+
+	cpg = kzalloc(sizeof(*cpg), GFP_KERNEL);
+	clks = kzalloc(CPG_NUM_CLOCKS * sizeof(*clks), GFP_KERNEL);
+	if (cpg == NULL || clks == NULL) {
+		/* We're leaking memory on purpose, there's no point in cleaning
+		 * up as the system won't boot anyway.
+		 */
+		return;
+	}
+
+	spin_lock_init(&cpg->lock);
+
+	cpg->data.clks = clks;
+	cpg->data.clk_num = num_clks;
+
+	config = &cpg_clk_configs[CPG_CLK_CONFIG_INDEX(cpg_mode)];
+	plla_mult = cpg_plla_mult[CPG_PLLA_MULT_INDEX(cpg_mode)];
+
+	for (i = 0; i < num_clks; ++i) {
+		const char *name;
+		struct clk *clk;
+
+		of_property_read_string_index(np, "clock-output-names", i,
+					      &name);
+
+		clk = r8a7779_cpg_register_clock(np, cpg, config,
+						 plla_mult, name);
+		if (IS_ERR(clk))
+			pr_err("%s: failed to register %s %s clock (%ld)\n",
+			       __func__, np->name, name, PTR_ERR(clk));
+		else
+			cpg->data.clks[i] = clk;
+	}
+
+	of_clk_add_provider(np, of_clk_src_onecell_get, &cpg->data);
+}
+CLK_OF_DECLARE(r8a7779_cpg_clks, "renesas,r8a7779-cpg-clocks",
+	       r8a7779_cpg_clocks_init);
+
+void __init r8a7779_clocks_init(u32 mode)
+{
+	cpg_mode = mode;
+
+	of_clk_init(NULL);
+}
diff --git a/drivers/clk/socfpga/clk-gate.c b/drivers/clk/socfpga/clk-gate.c
index 501d513bf89..dd3a78c6479 100644
--- a/drivers/clk/socfpga/clk-gate.c
+++ b/drivers/clk/socfpga/clk-gate.c
@@ -32,7 +32,6 @@
 #define SOCFPGA_MMC_CLK			"sdmmc_clk"
 #define SOCFPGA_GPIO_DB_CLK_OFFSET	0xA8
 
-#define div_mask(width)	((1 << (width)) - 1)
 #define streq(a, b) (strcmp((a), (b)) == 0)
 
 #define to_socfpga_gate_clk(p) container_of(p, struct socfpga_gate_clk, hw.hw)
diff --git a/drivers/clk/socfpga/clk-periph.c b/drivers/clk/socfpga/clk-periph.c
index 81623a3736f..46531c34ec9 100644
--- a/drivers/clk/socfpga/clk-periph.c
+++ b/drivers/clk/socfpga/clk-periph.c
@@ -29,12 +29,18 @@ static unsigned long clk_periclk_recalc_rate(struct clk_hw *hwclk,
 					     unsigned long parent_rate)
 {
 	struct socfpga_periph_clk *socfpgaclk = to_socfpga_periph_clk(hwclk);
-	u32 div;
+	u32 div, val;
 
-	if (socfpgaclk->fixed_div)
+	if (socfpgaclk->fixed_div) {
 		div = socfpgaclk->fixed_div;
-	else
+	} else {
+		if (socfpgaclk->div_reg) {
+			val = readl(socfpgaclk->div_reg) >> socfpgaclk->shift;
+			val &= div_mask(socfpgaclk->width);
+			parent_rate /= (val + 1);
+		}
 		div = ((readl(socfpgaclk->hw.reg) & 0x1ff) + 1);
+	}
 
 	return parent_rate / div;
 }
@@ -54,6 +60,7 @@ static __init void __socfpga_periph_init(struct device_node *node,
 	struct clk_init_data init;
 	int rc;
 	u32 fixed_div;
+	u32 div_reg[3];
 
 	of_property_read_u32(node, "reg", &reg);
 
@@ -63,6 +70,15 @@ static __init void __socfpga_periph_init(struct device_node *node,
 
 	periph_clk->hw.reg = clk_mgr_base_addr + reg;
 
+	rc = of_property_read_u32_array(node, "div-reg", div_reg, 3);
+	if (!rc) {
+		periph_clk->div_reg = clk_mgr_base_addr + div_reg[0];
+		periph_clk->shift = div_reg[1];
+		periph_clk->width = div_reg[2];
+	} else {
+		periph_clk->div_reg = 0;
+	}
+
 	rc = of_property_read_u32(node, "fixed-divider", &fixed_div);
 	if (rc)
 		periph_clk->fixed_div = 0;
diff --git a/drivers/clk/socfpga/clk-pll.c b/drivers/clk/socfpga/clk-pll.c
index 88dafb5e962..de6da957a09 100644
--- a/drivers/clk/socfpga/clk-pll.c
+++ b/drivers/clk/socfpga/clk-pll.c
@@ -20,6 +20,7 @@
 #include <linux/clk-provider.h>
 #include <linux/io.h>
 #include <linux/of.h>
+#include <linux/of_address.h>
 
 #include "clk.h"
 
@@ -43,6 +44,8 @@
 
 #define to_socfpga_clk(p) container_of(p, struct socfpga_pll, hw.hw)
 
+void __iomem *clk_mgr_base_addr;
+
 static unsigned long clk_pll_recalc_rate(struct clk_hw *hwclk,
 					 unsigned long parent_rate)
 {
@@ -87,6 +90,7 @@ static __init struct clk *__socfpga_pll_init(struct device_node *node,
 	const char *clk_name = node->name;
 	const char *parent_name[SOCFPGA_MAX_PARENTS];
 	struct clk_init_data init;
+	struct device_node *clkmgr_np;
 	int rc;
 	int i = 0;
 
@@ -96,6 +100,9 @@ static __init struct clk *__socfpga_pll_init(struct device_node *node,
 	if (WARN_ON(!pll_clk))
 		return NULL;
 
+	clkmgr_np = of_find_compatible_node(NULL, NULL, "altr,clk-mgr");
+	clk_mgr_base_addr = of_iomap(clkmgr_np, 0);
+	BUG_ON(!clk_mgr_base_addr);
 	pll_clk->hw.reg = clk_mgr_base_addr + reg;
 
 	of_property_read_string(node, "clock-output-names", &clk_name);
diff --git a/drivers/clk/socfpga/clk.c b/drivers/clk/socfpga/clk.c
index 35a960a993f..43db947e5f0 100644
--- a/drivers/clk/socfpga/clk.c
+++ b/drivers/clk/socfpga/clk.c
@@ -17,28 +17,11 @@
  * 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/clkdev.h>
-#include <linux/clk-provider.h>
-#include <linux/io.h>
 #include <linux/of.h>
-#include <linux/of_address.h>
 
 #include "clk.h"
 
-void __iomem *clk_mgr_base_addr;
-
-static const struct of_device_id socfpga_child_clocks[] __initconst = {
-	{ .compatible = "altr,socfpga-pll-clock", socfpga_pll_init, },
-	{ .compatible = "altr,socfpga-perip-clk", socfpga_periph_init, },
-	{ .compatible = "altr,socfpga-gate-clk", socfpga_gate_init, },
-	{},
-};
-
-static void __init socfpga_clkmgr_init(struct device_node *node)
-{
-	clk_mgr_base_addr = of_iomap(node, 0);
-	of_clk_init(socfpga_child_clocks);
-}
-CLK_OF_DECLARE(socfpga_mgr, "altr,clk-mgr", socfpga_clkmgr_init);
+CLK_OF_DECLARE(socfpga_pll_clk, "altr,socfpga-pll-clock", socfpga_pll_init);
+CLK_OF_DECLARE(socfpga_perip_clk, "altr,socfpga-perip-clk", socfpga_periph_init);
+CLK_OF_DECLARE(socfpga_gate_clk, "altr,socfpga-gate-clk", socfpga_gate_init);
 
diff --git a/drivers/clk/socfpga/clk.h b/drivers/clk/socfpga/clk.h
index d2e54019c94..d291f60c46e 100644
--- a/drivers/clk/socfpga/clk.h
+++ b/drivers/clk/socfpga/clk.h
@@ -27,6 +27,7 @@
 #define CLKMGR_PERPLL_SRC	0xAC
 
 #define SOCFPGA_MAX_PARENTS		3
+#define div_mask(width) ((1 << (width)) - 1)
 
 extern void __iomem *clk_mgr_base_addr;
 
@@ -52,6 +53,9 @@ struct socfpga_periph_clk {
 	struct clk_gate hw;
 	char *parent_name;
 	u32 fixed_div;
+	void __iomem *div_reg;
+	u32 width;      /* only valid if div_reg != 0 */
+	u32 shift;      /* only valid if div_reg != 0 */
 };
 
 #endif /* SOCFPGA_CLK_H */
diff --git a/drivers/clk/st/clkgen-pll.c b/drivers/clk/st/clkgen-pll.c
index bca0a0badbf..d8b9b1a2aed 100644
--- a/drivers/clk/st/clkgen-pll.c
+++ b/drivers/clk/st/clkgen-pll.c
@@ -521,8 +521,10 @@ static struct clk * __init clkgen_odf_register(const char *parent_name,
 	gate->lock = odf_lock;
 
 	div = kzalloc(sizeof(*div), GFP_KERNEL);
-	if (!div)
+	if (!div) {
+		kfree(gate);
 		return ERR_PTR(-ENOMEM);
+	}
 
 	div->flags = CLK_DIVIDER_ONE_BASED | CLK_DIVIDER_ALLOW_ZERO;
 	div->reg = reg + pll_data->odf[odf].offset;
@@ -653,6 +655,7 @@ static struct of_device_id c32_gpu_pll_of_match[] = {
 		.compatible = "st,stih416-gpu-pll-c32",
 		.data = &st_pll1200c32_gpu_416,
 	},
+	{}
 };
 
 static void __init clkgengpu_c32_pll_setup(struct device_node *np)
diff --git a/drivers/clk/sunxi/clk-factors.c b/drivers/clk/sunxi/clk-factors.c
index 9e232644f07..3806d97e529 100644
--- a/drivers/clk/sunxi/clk-factors.c
+++ b/drivers/clk/sunxi/clk-factors.c
@@ -77,6 +77,41 @@ static long clk_factors_round_rate(struct clk_hw *hw, unsigned long rate,
 	return rate;
 }
 
+static long clk_factors_determine_rate(struct clk_hw *hw, unsigned long rate,
+				       unsigned long *best_parent_rate,
+				       struct clk **best_parent_p)
+{
+	struct clk *clk = hw->clk, *parent, *best_parent = NULL;
+	int i, num_parents;
+	unsigned long parent_rate, best = 0, child_rate, best_child_rate = 0;
+
+	/* find the parent that can help provide the fastest rate <= rate */
+	num_parents = __clk_get_num_parents(clk);
+	for (i = 0; i < num_parents; i++) {
+		parent = clk_get_parent_by_index(clk, i);
+		if (!parent)
+			continue;
+		if (__clk_get_flags(clk) & CLK_SET_RATE_PARENT)
+			parent_rate = __clk_round_rate(parent, rate);
+		else
+			parent_rate = __clk_get_rate(parent);
+
+		child_rate = clk_factors_round_rate(hw, rate, &parent_rate);
+
+		if (child_rate <= rate && child_rate > best_child_rate) {
+			best_parent = parent;
+			best = parent_rate;
+			best_child_rate = child_rate;
+		}
+	}
+
+	if (best_parent)
+		*best_parent_p = best_parent;
+	*best_parent_rate = best;
+
+	return best_child_rate;
+}
+
 static int clk_factors_set_rate(struct clk_hw *hw, unsigned long rate,
 				unsigned long parent_rate)
 {
@@ -113,6 +148,7 @@ static int clk_factors_set_rate(struct clk_hw *hw, unsigned long rate,
 }
 
 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,
diff --git a/drivers/clk/sunxi/clk-sunxi.c b/drivers/clk/sunxi/clk-sunxi.c
index bd7dc733c1c..426483422d3 100644
--- a/drivers/clk/sunxi/clk-sunxi.c
+++ b/drivers/clk/sunxi/clk-sunxi.c
@@ -507,6 +507,43 @@ CLK_OF_DECLARE(sun7i_a20_gmac, "allwinner,sun7i-a20-gmac-clk",
 
 
 /**
+ * clk_sunxi_mmc_phase_control() - configures MMC clock phase control
+ */
+
+void clk_sunxi_mmc_phase_control(struct clk *clk, u8 sample, u8 output)
+{
+	#define to_clk_composite(_hw) container_of(_hw, struct clk_composite, hw)
+	#define to_clk_factors(_hw) container_of(_hw, struct clk_factors, hw)
+
+	struct clk_hw *hw = __clk_get_hw(clk);
+	struct clk_composite *composite = to_clk_composite(hw);
+	struct clk_hw *rate_hw = composite->rate_hw;
+	struct clk_factors *factors = to_clk_factors(rate_hw);
+	unsigned long flags = 0;
+	u32 reg;
+
+	if (factors->lock)
+		spin_lock_irqsave(factors->lock, flags);
+
+	reg = readl(factors->reg);
+
+	/* set sample clock phase control */
+	reg &= ~(0x7 << 20);
+	reg |= ((sample & 0x7) << 20);
+
+	/* set output clock phase control */
+	reg &= ~(0x7 << 8);
+	reg |= ((output & 0x7) << 8);
+
+	writel(reg, factors->reg);
+
+	if (factors->lock)
+		spin_unlock_irqrestore(factors->lock, flags);
+}
+EXPORT_SYMBOL(clk_sunxi_mmc_phase_control);
+
+
+/**
  * sunxi_factors_clk_setup() - Setup function for factor clocks
  */
 
@@ -1278,8 +1315,7 @@ static void __init of_sunxi_table_clock_setup(const struct of_device_id *clk_mat
 	const struct of_device_id *match;
 	void (*setup_function)(struct device_node *, const void *) = function;
 
-	for_each_matching_node(np, clk_match) {
-		match = of_match_node(clk_match, np);
+	for_each_matching_node_and_match(np, clk_match, &match) {
 		data = match->data;
 		setup_function(np, data);
 	}
@@ -1310,7 +1346,7 @@ static void __init sunxi_clock_protect(void)
 	}
 }
 
-static void __init sunxi_init_clocks(void)
+static void __init sunxi_init_clocks(struct device_node *np)
 {
 	/* Register factor clocks */
 	of_sunxi_table_clock_setup(clk_factors_match, sunxi_factors_clk_setup);
diff --git a/drivers/clk/tegra/clk-id.h b/drivers/clk/tegra/clk-id.h
index c39613c519a..0011d547a9f 100644
--- a/drivers/clk/tegra/clk-id.h
+++ b/drivers/clk/tegra/clk-id.h
@@ -233,6 +233,7 @@ enum clk_id {
 	tegra_clk_xusb_hs_src,
 	tegra_clk_xusb_ss,
 	tegra_clk_xusb_ss_src,
+	tegra_clk_xusb_ss_div2,
 	tegra_clk_max,
 };
 
diff --git a/drivers/clk/tegra/clk-pll.c b/drivers/clk/tegra/clk-pll.c
index 0d20241e077..637b62ccc91 100644
--- a/drivers/clk/tegra/clk-pll.c
+++ b/drivers/clk/tegra/clk-pll.c
@@ -58,9 +58,9 @@
 #define PLLDU_LFCON_SET_DIVN 600
 
 #define PLLE_BASE_DIVCML_SHIFT 24
-#define PLLE_BASE_DIVCML_WIDTH 4
+#define PLLE_BASE_DIVCML_MASK 0xf
 #define PLLE_BASE_DIVP_SHIFT 16
-#define PLLE_BASE_DIVP_WIDTH 7
+#define PLLE_BASE_DIVP_WIDTH 6
 #define PLLE_BASE_DIVN_SHIFT 8
 #define PLLE_BASE_DIVN_WIDTH 8
 #define PLLE_BASE_DIVM_SHIFT 0
@@ -96,10 +96,20 @@
 	(PLLE_SS_MAX_VAL | PLLE_SS_INC_VAL | PLLE_SS_INCINTRV_VAL)
 
 #define PLLE_AUX_PLLP_SEL	BIT(2)
+#define PLLE_AUX_USE_LOCKDET	BIT(3)
 #define PLLE_AUX_ENABLE_SWCTL	BIT(4)
+#define PLLE_AUX_SS_SWCTL	BIT(6)
 #define PLLE_AUX_SEQ_ENABLE	BIT(24)
+#define PLLE_AUX_SEQ_START_STATE BIT(25)
 #define PLLE_AUX_PLLRE_SEL	BIT(28)
 
+#define XUSBIO_PLL_CFG0		0x51c
+#define XUSBIO_PLL_CFG0_PADPLL_RESET_SWCTL	BIT(0)
+#define XUSBIO_PLL_CFG0_CLK_ENABLE_SWCTL	BIT(2)
+#define XUSBIO_PLL_CFG0_PADPLL_USE_LOCKDET	BIT(6)
+#define XUSBIO_PLL_CFG0_SEQ_ENABLE		BIT(24)
+#define XUSBIO_PLL_CFG0_SEQ_START_STATE		BIT(25)
+
 #define PLLE_MISC_PLLE_PTS	BIT(8)
 #define PLLE_MISC_IDDQ_SW_VALUE	BIT(13)
 #define PLLE_MISC_IDDQ_SW_CTRL	BIT(14)
@@ -183,6 +193,14 @@
 #define divp_mask(p) (p->params->flags & TEGRA_PLLU ? PLLU_POST_DIVP_MASK :\
 		      mask(p->params->div_nmp->divp_width))
 
+#define divm_shift(p) (p)->params->div_nmp->divm_shift
+#define divn_shift(p) (p)->params->div_nmp->divn_shift
+#define divp_shift(p) (p)->params->div_nmp->divp_shift
+
+#define divm_mask_shifted(p) (divm_mask(p) << divm_shift(p))
+#define divn_mask_shifted(p) (divn_mask(p) << divn_shift(p))
+#define divp_mask_shifted(p) (divp_mask(p) << divp_shift(p))
+
 #define divm_max(p) (divm_mask(p))
 #define divn_max(p) (divn_mask(p))
 #define divp_max(p) (1 << (divp_mask(p)))
@@ -476,13 +494,12 @@ static void _update_pll_mnp(struct tegra_clk_pll *pll,
 	} else {
 		val = pll_readl_base(pll);
 
-		val &= ~((divm_mask(pll) << div_nmp->divm_shift) |
-		 (divn_mask(pll) << div_nmp->divn_shift) |
-		 (divp_mask(pll) << div_nmp->divp_shift));
+		val &= ~(divm_mask_shifted(pll) | divn_mask_shifted(pll) |
+			 divp_mask_shifted(pll));
 
-		val |= ((cfg->m << div_nmp->divm_shift) |
-			(cfg->n << div_nmp->divn_shift) |
-			(cfg->p << div_nmp->divp_shift));
+		val |= (cfg->m << divm_shift(pll)) |
+		       (cfg->n << divn_shift(pll)) |
+		       (cfg->p << divp_shift(pll));
 
 		pll_writel_base(val, pll);
 	}
@@ -730,11 +747,12 @@ static int clk_plle_enable(struct clk_hw *hw)
 	if (pll->params->flags & TEGRA_PLLE_CONFIGURE) {
 		/* configure dividers */
 		val = pll_readl_base(pll);
-		val &= ~(divm_mask(pll) | divn_mask(pll) | divp_mask(pll));
-		val &= ~(PLLE_BASE_DIVCML_WIDTH << PLLE_BASE_DIVCML_SHIFT);
-		val |= sel.m << pll->params->div_nmp->divm_shift;
-		val |= sel.n << pll->params->div_nmp->divn_shift;
-		val |= sel.p << pll->params->div_nmp->divp_shift;
+		val &= ~(divp_mask_shifted(pll) | divn_mask_shifted(pll) |
+			 divm_mask_shifted(pll));
+		val &= ~(PLLE_BASE_DIVCML_MASK << PLLE_BASE_DIVCML_SHIFT);
+		val |= sel.m << divm_shift(pll);
+		val |= sel.n << divn_shift(pll);
+		val |= sel.p << divp_shift(pll);
 		val |= sel.cpcon << PLLE_BASE_DIVCML_SHIFT;
 		pll_writel_base(val, pll);
 	}
@@ -745,10 +763,11 @@ static int clk_plle_enable(struct clk_hw *hw)
 	pll_writel_misc(val, pll);
 
 	val = readl(pll->clk_base + PLLE_SS_CTRL);
+	val &= ~PLLE_SS_COEFFICIENTS_MASK;
 	val |= PLLE_SS_DISABLE;
 	writel(val, pll->clk_base + PLLE_SS_CTRL);
 
-	val |= pll_readl_base(pll);
+	val = pll_readl_base(pll);
 	val |= (PLL_BASE_BYPASS | PLL_BASE_ENABLE);
 	pll_writel_base(val, pll);
 
@@ -1292,10 +1311,11 @@ static int clk_plle_tegra114_enable(struct clk_hw *hw)
 	pll_writel(val, PLLE_SS_CTRL, pll);
 
 	val = pll_readl_base(pll);
-	val &= ~(divm_mask(pll) | divn_mask(pll) | divp_mask(pll));
-	val &= ~(PLLE_BASE_DIVCML_WIDTH << PLLE_BASE_DIVCML_SHIFT);
-	val |= sel.m << pll->params->div_nmp->divm_shift;
-	val |= sel.n << pll->params->div_nmp->divn_shift;
+	val &= ~(divp_mask_shifted(pll) | divn_mask_shifted(pll) |
+		 divm_mask_shifted(pll));
+	val &= ~(PLLE_BASE_DIVCML_MASK << PLLE_BASE_DIVCML_SHIFT);
+	val |= sel.m << divm_shift(pll);
+	val |= sel.n << divn_shift(pll);
 	val |= sel.cpcon << PLLE_BASE_DIVCML_SHIFT;
 	pll_writel_base(val, pll);
 	udelay(1);
@@ -1318,7 +1338,28 @@ static int clk_plle_tegra114_enable(struct clk_hw *hw)
 	pll_writel(val, PLLE_SS_CTRL, pll);
 	udelay(1);
 
-	/* TODO: enable hw control of xusb brick pll */
+	/* Enable hw control of xusb brick pll */
+	val = pll_readl_misc(pll);
+	val &= ~PLLE_MISC_IDDQ_SW_CTRL;
+	pll_writel_misc(val, pll);
+
+	val = pll_readl(pll->params->aux_reg, pll);
+	val |= (PLLE_AUX_USE_LOCKDET | PLLE_AUX_SEQ_START_STATE);
+	val &= ~(PLLE_AUX_ENABLE_SWCTL | PLLE_AUX_SS_SWCTL);
+	pll_writel(val, pll->params->aux_reg, pll);
+	udelay(1);
+	val |= PLLE_AUX_SEQ_ENABLE;
+	pll_writel(val, pll->params->aux_reg, pll);
+
+	val = pll_readl(XUSBIO_PLL_CFG0, pll);
+	val |= (XUSBIO_PLL_CFG0_PADPLL_USE_LOCKDET |
+		XUSBIO_PLL_CFG0_SEQ_START_STATE);
+	val &= ~(XUSBIO_PLL_CFG0_CLK_ENABLE_SWCTL |
+		 XUSBIO_PLL_CFG0_PADPLL_RESET_SWCTL);
+	pll_writel(val, XUSBIO_PLL_CFG0, pll);
+	udelay(1);
+	val |= XUSBIO_PLL_CFG0_SEQ_ENABLE;
+	pll_writel(val, XUSBIO_PLL_CFG0, pll);
 
 out:
 	if (pll->lock)
@@ -1410,6 +1451,15 @@ struct clk *tegra_clk_register_pll(const char *name, const char *parent_name,
 	return clk;
 }
 
+static struct div_nmp pll_e_nmp = {
+	.divn_shift = PLLE_BASE_DIVN_SHIFT,
+	.divn_width = PLLE_BASE_DIVN_WIDTH,
+	.divm_shift = PLLE_BASE_DIVM_SHIFT,
+	.divm_width = PLLE_BASE_DIVM_WIDTH,
+	.divp_shift = PLLE_BASE_DIVP_SHIFT,
+	.divp_width = PLLE_BASE_DIVP_WIDTH,
+};
+
 struct clk *tegra_clk_register_plle(const char *name, const char *parent_name,
 		void __iomem *clk_base, void __iomem *pmc,
 		unsigned long flags, struct tegra_clk_pll_params *pll_params,
@@ -1420,6 +1470,10 @@ struct clk *tegra_clk_register_plle(const char *name, const char *parent_name,
 
 	pll_params->flags |= TEGRA_PLL_LOCK_MISC | TEGRA_PLL_BYPASS;
 	pll_params->flags |= TEGRA_PLL_HAS_LOCK_ENABLE;
+
+	if (!pll_params->div_nmp)
+		pll_params->div_nmp = &pll_e_nmp;
+
 	pll = _tegra_init_pll(clk_base, pmc, pll_params, lock);
 	if (IS_ERR(pll))
 		return ERR_CAST(pll);
@@ -1557,9 +1611,8 @@ struct clk *tegra_clk_register_pllre(const char *name, const char *parent_name,
 		int m;
 
 		m = _pll_fixed_mdiv(pll_params, parent_rate);
-		val = m << PLL_BASE_DIVM_SHIFT;
-		val |= (pll_params->vco_min / parent_rate)
-				<< PLL_BASE_DIVN_SHIFT;
+		val = m << divm_shift(pll);
+		val |= (pll_params->vco_min / parent_rate) << divn_shift(pll);
 		pll_writel_base(val, pll);
 	}
 
@@ -1718,7 +1771,7 @@ struct clk *tegra_clk_register_plle_tegra114(const char *name,
 					"pll_re_vco");
 	} else {
 		val_aux &= ~(PLLE_AUX_PLLRE_SEL | PLLE_AUX_PLLP_SEL);
-		pll_writel(val, pll_params->aux_reg, pll);
+		pll_writel(val_aux, pll_params->aux_reg, pll);
 	}
 
 	clk = _tegra_clk_register_pll(pll, name, parent_name, flags,
diff --git a/drivers/clk/tegra/clk-tegra-periph.c b/drivers/clk/tegra/clk-tegra-periph.c
index 1fa5c3f33b2..adf6b814b5b 100644
--- a/drivers/clk/tegra/clk-tegra-periph.c
+++ b/drivers/clk/tegra/clk-tegra-periph.c
@@ -329,7 +329,9 @@ static u32 mux_clkm_pllp_pllc_pllre_idx[] = {
 static const char *mux_clkm_48M_pllp_480M[] = {
 	"clk_m", "pll_u_48M", "pll_p", "pll_u_480M"
 };
-#define mux_clkm_48M_pllp_480M_idx NULL
+static u32 mux_clkm_48M_pllp_480M_idx[] = {
+	[0] = 0, [1] = 2, [2] = 4, [3] = 6,
+};
 
 static const char *mux_clkm_pllre_clk32_480M_pllc_ref[] = {
 	"clk_m", "pll_re_out", "clk_32k", "pll_u_480M", "pll_c", "pll_ref"
@@ -338,6 +340,11 @@ static u32 mux_clkm_pllre_clk32_480M_pllc_ref_idx[] = {
 	[0] = 0, [1] = 1, [2] = 3, [3] = 3, [4] = 4, [5] = 7,
 };
 
+static const char *mux_ss_60M[] = {
+	"xusb_ss_div2", "pll_u_60M"
+};
+#define mux_ss_60M_idx NULL
+
 static const char *mux_d_audio_clk[] = {
 	"pll_a_out0", "pll_p", "clk_m", "spdif_in_sync", "i2s0_sync",
 	"i2s1_sync", "i2s2_sync", "i2s3_sync", "i2s4_sync", "vimclk_sync",
@@ -499,6 +506,7 @@ static struct tegra_periph_init_data periph_clks[] = {
 	XUSB("xusb_falcon_src", mux_clkm_pllp_pllc_pllre, CLK_SOURCE_XUSB_FALCON_SRC, 143, TEGRA_PERIPH_NO_RESET, tegra_clk_xusb_falcon_src),
 	XUSB("xusb_fs_src", mux_clkm_48M_pllp_480M, CLK_SOURCE_XUSB_FS_SRC, 143, TEGRA_PERIPH_NO_RESET, tegra_clk_xusb_fs_src),
 	XUSB("xusb_ss_src", mux_clkm_pllre_clk32_480M_pllc_ref, CLK_SOURCE_XUSB_SS_SRC, 143, TEGRA_PERIPH_NO_RESET, tegra_clk_xusb_ss_src),
+	NODIV("xusb_hs_src", mux_ss_60M, CLK_SOURCE_XUSB_SS_SRC, 25, MASK(1), 143, TEGRA_PERIPH_NO_RESET, tegra_clk_xusb_hs_src, NULL),
 	XUSB("xusb_dev_src", mux_clkm_pllp_pllc_pllre, CLK_SOURCE_XUSB_DEV_SRC, 95, TEGRA_PERIPH_ON_APB | TEGRA_PERIPH_NO_RESET, tegra_clk_xusb_dev_src),
 };
 
diff --git a/drivers/clk/tegra/clk-tegra114.c b/drivers/clk/tegra/clk-tegra114.c
index 80431f0fb26..b9c8ba258ef 100644
--- a/drivers/clk/tegra/clk-tegra114.c
+++ b/drivers/clk/tegra/clk-tegra114.c
@@ -142,7 +142,6 @@
 #define UTMIPLL_HW_PWRDN_CFG0_IDDQ_SWCTL	BIT(0)
 
 #define CLK_SOURCE_CSITE 0x1d4
-#define CLK_SOURCE_XUSB_SS_SRC 0x610
 #define CLK_SOURCE_EMC 0x19c
 
 /* PLLM override registers */
@@ -834,6 +833,7 @@ static struct tegra_clk tegra114_clks[tegra_clk_max] __initdata = {
 	[tegra_clk_xusb_falcon_src] = { .dt_id = TEGRA114_CLK_XUSB_FALCON_SRC, .present = true },
 	[tegra_clk_xusb_fs_src] = { .dt_id = TEGRA114_CLK_XUSB_FS_SRC, .present = true },
 	[tegra_clk_xusb_ss_src] = { .dt_id = TEGRA114_CLK_XUSB_SS_SRC, .present = true },
+	[tegra_clk_xusb_ss_div2] = { .dt_id = TEGRA114_CLK_XUSB_SS_DIV2, .present = true},
 	[tegra_clk_xusb_dev_src] = { .dt_id = TEGRA114_CLK_XUSB_DEV_SRC, .present = true },
 	[tegra_clk_xusb_dev] = { .dt_id = TEGRA114_CLK_XUSB_DEV, .present = true },
 	[tegra_clk_xusb_hs_src] = { .dt_id = TEGRA114_CLK_XUSB_HS_SRC, .present = true },
@@ -1182,16 +1182,11 @@ static __init void tegra114_periph_clk_init(void __iomem *clk_base,
 					    void __iomem *pmc_base)
 {
 	struct clk *clk;
-	u32 val;
-
-	/* xusb_hs_src */
-	val = readl(clk_base + CLK_SOURCE_XUSB_SS_SRC);
-	val |= BIT(25); /* always select PLLU_60M */
-	writel(val, clk_base + CLK_SOURCE_XUSB_SS_SRC);
 
-	clk = clk_register_fixed_factor(NULL, "xusb_hs_src", "pll_u_60M", 0,
-					1, 1);
-	clks[TEGRA114_CLK_XUSB_HS_SRC] = clk;
+	/* xusb_ss_div2 */
+	clk = clk_register_fixed_factor(NULL, "xusb_ss_div2", "xusb_ss_src", 0,
+					1, 2);
+	clks[TEGRA114_CLK_XUSB_SS_DIV2] = clk;
 
 	/* dsia mux */
 	clk = clk_register_mux(NULL, "dsia_mux", mux_plld_out0_plld2_out0,
@@ -1301,7 +1296,12 @@ static struct tegra_clk_init_table init_table[] __initdata = {
 	{TEGRA114_CLK_GR3D, TEGRA114_CLK_PLL_C2, 300000000, 0},
 	{TEGRA114_CLK_DSIALP, TEGRA114_CLK_PLL_P, 68000000, 0},
 	{TEGRA114_CLK_DSIBLP, TEGRA114_CLK_PLL_P, 68000000, 0},
-
+	{TEGRA114_CLK_PLL_RE_VCO, TEGRA114_CLK_CLK_MAX, 612000000, 0},
+	{TEGRA114_CLK_XUSB_SS_SRC, TEGRA114_CLK_PLL_RE_OUT, 122400000, 0},
+	{TEGRA114_CLK_XUSB_FS_SRC, TEGRA114_CLK_PLL_U_48M, 48000000, 0},
+	{TEGRA114_CLK_XUSB_HS_SRC, TEGRA114_CLK_XUSB_SS_DIV2, 61200000, 0},
+	{TEGRA114_CLK_XUSB_FALCON_SRC, TEGRA114_CLK_PLL_P, 204000000, 0},
+	{TEGRA114_CLK_XUSB_HOST_SRC, TEGRA114_CLK_PLL_P, 102000000, 0},
 	/* This MUST be the last entry. */
 	{TEGRA114_CLK_CLK_MAX, TEGRA114_CLK_CLK_MAX, 0, 0},
 };
diff --git a/drivers/clk/tegra/clk-tegra124.c b/drivers/clk/tegra/clk-tegra124.c
index cc37c342c4c..80efe51fdcd 100644
--- a/drivers/clk/tegra/clk-tegra124.c
+++ b/drivers/clk/tegra/clk-tegra124.c
@@ -30,7 +30,6 @@
 
 #define CLK_SOURCE_CSITE 0x1d4
 #define CLK_SOURCE_EMC 0x19c
-#define CLK_SOURCE_XUSB_SS_SRC 0x610
 
 #define PLLC_BASE 0x80
 #define PLLC_OUT 0x84
@@ -925,6 +924,7 @@ static struct tegra_clk tegra124_clks[tegra_clk_max] __initdata = {
 	[tegra_clk_xusb_falcon_src] = { .dt_id = TEGRA124_CLK_XUSB_FALCON_SRC, .present = true },
 	[tegra_clk_xusb_fs_src] = { .dt_id = TEGRA124_CLK_XUSB_FS_SRC, .present = true },
 	[tegra_clk_xusb_ss_src] = { .dt_id = TEGRA124_CLK_XUSB_SS_SRC, .present = true },
+	[tegra_clk_xusb_ss_div2] = { .dt_id = TEGRA124_CLK_XUSB_SS_DIV2, .present = true },
 	[tegra_clk_xusb_dev_src] = { .dt_id = TEGRA124_CLK_XUSB_DEV_SRC, .present = true },
 	[tegra_clk_xusb_dev] = { .dt_id = TEGRA124_CLK_XUSB_DEV, .present = true },
 	[tegra_clk_xusb_hs_src] = { .dt_id = TEGRA124_CLK_XUSB_HS_SRC, .present = true },
@@ -1105,16 +1105,11 @@ static __init void tegra124_periph_clk_init(void __iomem *clk_base,
 					    void __iomem *pmc_base)
 {
 	struct clk *clk;
-	u32 val;
-
-	/* xusb_hs_src */
-	val = readl(clk_base + CLK_SOURCE_XUSB_SS_SRC);
-	val |= BIT(25); /* always select PLLU_60M */
-	writel(val, clk_base + CLK_SOURCE_XUSB_SS_SRC);
 
-	clk = clk_register_fixed_factor(NULL, "xusb_hs_src", "pll_u_60M", 0,
-					1, 1);
-	clks[TEGRA124_CLK_XUSB_HS_SRC] = clk;
+	/* xusb_ss_div2 */
+	clk = clk_register_fixed_factor(NULL, "xusb_ss_div2", "xusb_ss_src", 0,
+					1, 2);
+	clks[TEGRA124_CLK_XUSB_SS_DIV2] = clk;
 
 	/* dsia mux */
 	clk = clk_register_mux(NULL, "dsia_mux", mux_plld_out0_plld2_out0,
@@ -1368,6 +1363,12 @@ static struct tegra_clk_init_table init_table[] __initdata = {
 	{TEGRA124_CLK_SBC4, TEGRA124_CLK_PLL_P, 12000000, 1},
 	{TEGRA124_CLK_TSEC, TEGRA124_CLK_PLL_C3, 0, 0},
 	{TEGRA124_CLK_MSENC, TEGRA124_CLK_PLL_C3, 0, 0},
+	{TEGRA124_CLK_PLL_RE_VCO, TEGRA124_CLK_CLK_MAX, 672000000, 0},
+	{TEGRA124_CLK_XUSB_SS_SRC, TEGRA124_CLK_PLL_U_480M, 120000000, 0},
+	{TEGRA124_CLK_XUSB_FS_SRC, TEGRA124_CLK_PLL_U_48M, 48000000, 0},
+	{TEGRA124_CLK_XUSB_HS_SRC, TEGRA124_CLK_PLL_U_60M, 60000000, 0},
+	{TEGRA124_CLK_XUSB_FALCON_SRC, TEGRA124_CLK_PLL_RE_OUT, 224000000, 0},
+	{TEGRA124_CLK_XUSB_HOST_SRC, TEGRA124_CLK_PLL_RE_OUT, 112000000, 0},
 	/* This MUST be the last entry. */
 	{TEGRA124_CLK_CLK_MAX, TEGRA124_CLK_CLK_MAX, 0, 0},
 };
diff --git a/drivers/clk/ti/clk-43xx.c b/drivers/clk/ti/clk-43xx.c
index 67c8de572c5..527a43da3d3 100644
--- a/drivers/clk/ti/clk-43xx.c
+++ b/drivers/clk/ti/clk-43xx.c
@@ -105,6 +105,12 @@ static struct ti_dt_clk am43xx_clks[] = {
 	DT_CLK(NULL, "func_12m_clk", "func_12m_clk"),
 	DT_CLK(NULL, "vtp_clk_div", "vtp_clk_div"),
 	DT_CLK(NULL, "usbphy_32khz_clkmux", "usbphy_32khz_clkmux"),
+	DT_CLK("48300200.ehrpwm", "tbclk", "ehrpwm0_tbclk"),
+	DT_CLK("48302200.ehrpwm", "tbclk", "ehrpwm1_tbclk"),
+	DT_CLK("48304200.ehrpwm", "tbclk", "ehrpwm2_tbclk"),
+	DT_CLK("48306200.ehrpwm", "tbclk", "ehrpwm3_tbclk"),
+	DT_CLK("48308200.ehrpwm", "tbclk", "ehrpwm4_tbclk"),
+	DT_CLK("4830a200.ehrpwm", "tbclk", "ehrpwm5_tbclk"),
 	{ .node_name = NULL },
 };
 
diff --git a/drivers/clk/ti/gate.c b/drivers/clk/ti/gate.c
index 3e2999d11d1..58734817d50 100644
--- a/drivers/clk/ti/gate.c
+++ b/drivers/clk/ti/gate.c
@@ -221,7 +221,7 @@ static void __init of_ti_gate_clk_setup(struct device_node *node)
 {
 	_of_ti_gate_clk_setup(node, &omap_gate_clk_ops, NULL);
 }
-CLK_OF_DECLARE(ti_gate_clk, "ti,gate-clock", of_ti_gate_clk_setup)
+CLK_OF_DECLARE(ti_gate_clk, "ti,gate-clock", of_ti_gate_clk_setup);
 
 static void __init of_ti_wait_gate_clk_setup(struct device_node *node)
 {
diff --git a/drivers/clk/versatile/Kconfig b/drivers/clk/versatile/Kconfig
new file mode 100644
index 00000000000..1530c9352a7
--- /dev/null
+++ b/drivers/clk/versatile/Kconfig
@@ -0,0 +1,26 @@
+config COMMON_CLK_VERSATILE
+	bool "Clock driver for ARM Reference designs"
+	depends on ARCH_INTEGRATOR || ARCH_REALVIEW || ARCH_VEXPRESS || ARM64
+	---help---
+          Supports clocking on ARM Reference designs:
+	  - Integrator/AP and Integrator/CP
+	  - RealView PB1176, EB, PB11MP and PBX
+	  - Versatile Express
+
+config CLK_SP810
+	bool "Clock driver for ARM SP810 System Controller"
+	depends on COMMON_CLK_VERSATILE
+	default y if ARCH_VEXPRESS
+	---help---
+	  Supports clock muxing (REFCLK/TIMCLK to TIMERCLKEN0-3) capabilities
+	  of the ARM SP810 System Controller cell.
+
+config CLK_VEXPRESS_OSC
+	bool "Clock driver for Versatile Express OSC clock generators"
+	depends on COMMON_CLK_VERSATILE
+	depends on VEXPRESS_CONFIG
+	default y if ARCH_VEXPRESS
+	---help---
+	  Simple regmap-based driver driving clock generators on Versatile
+	  Express platforms hidden behind its configuration infrastructure,
+	  commonly known as OSCs.
diff --git a/drivers/clk/versatile/Makefile b/drivers/clk/versatile/Makefile
index c16ca787170..fd449f9b006 100644
--- a/drivers/clk/versatile/Makefile
+++ b/drivers/clk/versatile/Makefile
@@ -3,5 +3,6 @@ obj-$(CONFIG_ICST)		+= clk-icst.o
 obj-$(CONFIG_ARCH_INTEGRATOR)	+= clk-integrator.o
 obj-$(CONFIG_INTEGRATOR_IMPD1)	+= clk-impd1.o
 obj-$(CONFIG_ARCH_REALVIEW)	+= clk-realview.o
-obj-$(CONFIG_ARCH_VEXPRESS)	+= clk-vexpress.o clk-sp810.o
-obj-$(CONFIG_VEXPRESS_CONFIG)	+= clk-vexpress-osc.o
+obj-$(CONFIG_ARCH_VEXPRESS)	+= clk-vexpress.o
+obj-$(CONFIG_CLK_SP810)		+= clk-sp810.o
+obj-$(CONFIG_CLK_VEXPRESS_OSC)	+= clk-vexpress-osc.o
diff --git a/drivers/clk/versatile/clk-icst.c b/drivers/clk/versatile/clk-icst.c
index a820b0cfcf5..bc96f103bd7 100644
--- a/drivers/clk/versatile/clk-icst.c
+++ b/drivers/clk/versatile/clk-icst.c
@@ -140,6 +140,7 @@ struct clk *icst_clk_register(struct device *dev,
 
 	pclone = kmemdup(desc->params, sizeof(*pclone), GFP_KERNEL);
 	if (!pclone) {
+		kfree(icst);
 		pr_err("could not clone ICST params\n");
 		return ERR_PTR(-ENOMEM);
 	}
@@ -160,3 +161,4 @@ struct clk *icst_clk_register(struct device *dev,
 
 	return clk;
 }
+EXPORT_SYMBOL_GPL(icst_clk_register);
diff --git a/drivers/clk/versatile/clk-impd1.c b/drivers/clk/versatile/clk-impd1.c
index 31b44f025f9..1cc1330dc57 100644
--- a/drivers/clk/versatile/clk-impd1.c
+++ b/drivers/clk/versatile/clk-impd1.c
@@ -20,6 +20,8 @@
 #define IMPD1_LOCK	0x08
 
 struct impd1_clk {
+	char *pclkname;
+	struct clk *pclk;
 	char *vco1name;
 	struct clk *vco1clk;
 	char *vco2name;
@@ -31,7 +33,7 @@ struct impd1_clk {
 	struct clk *spiclk;
 	char *scname;
 	struct clk *scclk;
-	struct clk_lookup *clks[6];
+	struct clk_lookup *clks[15];
 };
 
 /* One entry for each connected IM-PD1 LM */
@@ -86,6 +88,7 @@ void integrator_impd1_clk_init(void __iomem *base, unsigned int id)
 {
 	struct impd1_clk *imc;
 	struct clk *clk;
+	struct clk *pclk;
 	int i;
 
 	if (id > 3) {
@@ -94,11 +97,18 @@ void integrator_impd1_clk_init(void __iomem *base, unsigned int id)
 	}
 	imc = &impd1_clks[id];
 
+	/* Register the fixed rate PCLK */
+	imc->pclkname = kasprintf(GFP_KERNEL, "lm%x-pclk", id);
+	pclk = clk_register_fixed_rate(NULL, imc->pclkname, NULL,
+				      CLK_IS_ROOT, 0);
+	imc->pclk = pclk;
+
 	imc->vco1name = kasprintf(GFP_KERNEL, "lm%x-vco1", id);
 	clk = icst_clk_register(NULL, &impd1_icst1_desc, imc->vco1name, NULL,
 				base);
 	imc->vco1clk = clk;
-	imc->clks[0] = clkdev_alloc(clk, NULL, "lm%x:01000", id);
+	imc->clks[0] = clkdev_alloc(pclk, "apb_pclk", "lm%x:01000", id);
+	imc->clks[1] = clkdev_alloc(clk, NULL, "lm%x:01000", id);
 
 	/* VCO2 is also called "CLK2" */
 	imc->vco2name = kasprintf(GFP_KERNEL, "lm%x-vco2", id);
@@ -107,32 +117,43 @@ void integrator_impd1_clk_init(void __iomem *base, unsigned int id)
 	imc->vco2clk = clk;
 
 	/* MMCI uses CLK2 right off */
-	imc->clks[1] = clkdev_alloc(clk, NULL, "lm%x:00700", id);
+	imc->clks[2] = clkdev_alloc(pclk, "apb_pclk", "lm%x:00700", id);
+	imc->clks[3] = clkdev_alloc(clk, NULL, "lm%x:00700", id);
 
 	/* UART reference clock divides CLK2 by a fixed factor 4 */
 	imc->uartname = kasprintf(GFP_KERNEL, "lm%x-uartclk", id);
 	clk = clk_register_fixed_factor(NULL, imc->uartname, imc->vco2name,
 				   CLK_IGNORE_UNUSED, 1, 4);
 	imc->uartclk = clk;
-	imc->clks[2] = clkdev_alloc(clk, NULL, "lm%x:00100", id);
-	imc->clks[3] = clkdev_alloc(clk, NULL, "lm%x:00200", id);
+	imc->clks[4] = clkdev_alloc(pclk, "apb_pclk", "lm%x:00100", id);
+	imc->clks[5] = clkdev_alloc(clk, NULL, "lm%x:00100", id);
+	imc->clks[6] = clkdev_alloc(pclk, "apb_pclk", "lm%x:00200", id);
+	imc->clks[7] = clkdev_alloc(clk, NULL, "lm%x:00200", id);
 
 	/* SPI PL022 clock divides CLK2 by a fixed factor 64 */
 	imc->spiname = kasprintf(GFP_KERNEL, "lm%x-spiclk", id);
 	clk = clk_register_fixed_factor(NULL, imc->spiname, imc->vco2name,
 				   CLK_IGNORE_UNUSED, 1, 64);
-	imc->clks[4] = clkdev_alloc(clk, NULL, "lm%x:00300", id);
+	imc->clks[8] = clkdev_alloc(pclk, "apb_pclk", "lm%x:00300", id);
+	imc->clks[9] = clkdev_alloc(clk, NULL, "lm%x:00300", id);
+
+	/* The GPIO blocks and AACI have only PCLK */
+	imc->clks[10] = clkdev_alloc(pclk, "apb_pclk", "lm%x:00400", id);
+	imc->clks[11] = clkdev_alloc(pclk, "apb_pclk", "lm%x:00500", id);
+	imc->clks[12] = clkdev_alloc(pclk, "apb_pclk", "lm%x:00800", id);
 
 	/* Smart Card clock divides CLK2 by a fixed factor 4 */
 	imc->scname = kasprintf(GFP_KERNEL, "lm%x-scclk", id);
 	clk = clk_register_fixed_factor(NULL, imc->scname, imc->vco2name,
 				   CLK_IGNORE_UNUSED, 1, 4);
 	imc->scclk = clk;
-	imc->clks[5] = clkdev_alloc(clk, NULL, "lm%x:00600", id);
+	imc->clks[13] = clkdev_alloc(pclk, "apb_pclk", "lm%x:00600", id);
+	imc->clks[14] = clkdev_alloc(clk, NULL, "lm%x:00600", id);
 
 	for (i = 0; i < ARRAY_SIZE(imc->clks); i++)
 		clkdev_add(imc->clks[i]);
 }
+EXPORT_SYMBOL_GPL(integrator_impd1_clk_init);
 
 void integrator_impd1_clk_exit(unsigned int id)
 {
@@ -149,9 +170,12 @@ void integrator_impd1_clk_exit(unsigned int id)
 	clk_unregister(imc->uartclk);
 	clk_unregister(imc->vco2clk);
 	clk_unregister(imc->vco1clk);
+	clk_unregister(imc->pclk);
 	kfree(imc->scname);
 	kfree(imc->spiname);
 	kfree(imc->uartname);
 	kfree(imc->vco2name);
 	kfree(imc->vco1name);
+	kfree(imc->pclkname);
 }
+EXPORT_SYMBOL_GPL(integrator_impd1_clk_exit);
diff --git a/drivers/clk/versatile/clk-vexpress-osc.c b/drivers/clk/versatile/clk-vexpress-osc.c
index 422391242b3..529a59c0fbf 100644
--- a/drivers/clk/versatile/clk-vexpress-osc.c
+++ b/drivers/clk/versatile/clk-vexpress-osc.c
@@ -11,8 +11,6 @@
  * Copyright (C) 2012 ARM Limited
  */
 
-#define pr_fmt(fmt) "vexpress-osc: " fmt
-
 #include <linux/clkdev.h>
 #include <linux/clk-provider.h>
 #include <linux/err.h>
@@ -22,7 +20,7 @@
 #include <linux/vexpress.h>
 
 struct vexpress_osc {
-	struct vexpress_config_func *func;
+	struct regmap *reg;
 	struct clk_hw hw;
 	unsigned long rate_min;
 	unsigned long rate_max;
@@ -36,7 +34,7 @@ static unsigned long vexpress_osc_recalc_rate(struct clk_hw *hw,
 	struct vexpress_osc *osc = to_vexpress_osc(hw);
 	u32 rate;
 
-	vexpress_config_read(osc->func, 0, &rate);
+	regmap_read(osc->reg, 0, &rate);
 
 	return rate;
 }
@@ -60,7 +58,7 @@ static int vexpress_osc_set_rate(struct clk_hw *hw, unsigned long rate,
 {
 	struct vexpress_osc *osc = to_vexpress_osc(hw);
 
-	return vexpress_config_write(osc->func, 0, rate);
+	return regmap_write(osc->reg, 0, rate);
 }
 
 static struct clk_ops vexpress_osc_ops = {
@@ -70,58 +68,31 @@ static struct clk_ops vexpress_osc_ops = {
 };
 
 
-struct clk * __init vexpress_osc_setup(struct device *dev)
-{
-	struct clk_init_data init;
-	struct vexpress_osc *osc = kzalloc(sizeof(*osc), GFP_KERNEL);
-
-	if (!osc)
-		return NULL;
-
-	osc->func = vexpress_config_func_get_by_dev(dev);
-	if (!osc->func) {
-		kfree(osc);
-		return NULL;
-	}
-
-	init.name = dev_name(dev);
-	init.ops = &vexpress_osc_ops;
-	init.flags = CLK_IS_ROOT;
-	init.num_parents = 0;
-	osc->hw.init = &init;
-
-	return clk_register(NULL, &osc->hw);
-}
-
-void __init vexpress_osc_of_setup(struct device_node *node)
+static int vexpress_osc_probe(struct platform_device *pdev)
 {
+	struct clk_lookup *cl = pdev->dev.platform_data; /* Non-DT lookup */
 	struct clk_init_data init;
 	struct vexpress_osc *osc;
 	struct clk *clk;
 	u32 range[2];
 
-	vexpress_sysreg_of_early_init();
-
-	osc = kzalloc(sizeof(*osc), GFP_KERNEL);
+	osc = devm_kzalloc(&pdev->dev, sizeof(*osc), GFP_KERNEL);
 	if (!osc)
-		return;
+		return -ENOMEM;
 
-	osc->func = vexpress_config_func_get_by_node(node);
-	if (!osc->func) {
-		pr_err("Failed to obtain config func for node '%s'!\n",
-				node->full_name);
-		goto error;
-	}
+	osc->reg = devm_regmap_init_vexpress_config(&pdev->dev);
+	if (IS_ERR(osc->reg))
+		return PTR_ERR(osc->reg);
 
-	if (of_property_read_u32_array(node, "freq-range", range,
+	if (of_property_read_u32_array(pdev->dev.of_node, "freq-range", range,
 			ARRAY_SIZE(range)) == 0) {
 		osc->rate_min = range[0];
 		osc->rate_max = range[1];
 	}
 
-	of_property_read_string(node, "clock-output-names", &init.name);
-	if (!init.name)
-		init.name = node->full_name;
+	if (of_property_read_string(pdev->dev.of_node, "clock-output-names",
+			&init.name) != 0)
+		init.name = dev_name(&pdev->dev);
 
 	init.ops = &vexpress_osc_ops;
 	init.flags = CLK_IS_ROOT;
@@ -130,20 +101,37 @@ void __init vexpress_osc_of_setup(struct device_node *node)
 	osc->hw.init = &init;
 
 	clk = clk_register(NULL, &osc->hw);
-	if (IS_ERR(clk)) {
-		pr_err("Failed to register clock '%s'!\n", init.name);
-		goto error;
+	if (IS_ERR(clk))
+		return PTR_ERR(clk);
+
+	of_clk_add_provider(pdev->dev.of_node, of_clk_src_simple_get, clk);
+
+	/* Only happens for non-DT cases */
+	if (cl) {
+		cl->clk = clk;
+		clkdev_add(cl);
 	}
 
-	of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	dev_dbg(&pdev->dev, "Registered clock '%s'\n", init.name);
+
+	return 0;
+}
 
-	pr_debug("Registered clock '%s'\n", init.name);
+static struct of_device_id vexpress_osc_of_match[] = {
+	{ .compatible = "arm,vexpress-osc", },
+	{}
+};
 
-	return;
+static struct platform_driver vexpress_osc_driver = {
+	.driver	= {
+		.name = "vexpress-osc",
+		.of_match_table = vexpress_osc_of_match,
+	},
+	.probe = vexpress_osc_probe,
+};
 
-error:
-	if (osc->func)
-		vexpress_config_func_put(osc->func);
-	kfree(osc);
+static int __init vexpress_osc_init(void)
+{
+	return platform_driver_register(&vexpress_osc_driver);
 }
-CLK_OF_DECLARE(vexpress_soc, "arm,vexpress-osc", vexpress_osc_of_setup);
+core_initcall(vexpress_osc_init);
diff --git a/drivers/clk/zynq/clkc.c b/drivers/clk/zynq/clkc.c
index 52c09afdcfb..246cf1226ea 100644
--- a/drivers/clk/zynq/clkc.c
+++ b/drivers/clk/zynq/clkc.c
@@ -53,6 +53,9 @@ static void __iomem *zynq_clkc_base;
 
 #define NUM_MIO_PINS	54
 
+#define DBG_CLK_CTRL_CLKACT_TRC		BIT(0)
+#define DBG_CLK_CTRL_CPU_1XCLKACT	BIT(1)
+
 enum zynq_clk {
 	armpll, ddrpll, iopll,
 	cpu_6or4x, cpu_3or2x, cpu_2x, cpu_1x,
@@ -499,6 +502,15 @@ static void __init zynq_clk_setup(struct device_node *np)
 			clk_output_name[cpu_1x], 0, SLCR_DBG_CLK_CTRL, 1, 0,
 			&dbgclk_lock);
 
+	/* leave debug clocks in the state the bootloader set them up to */
+	tmp = clk_readl(SLCR_DBG_CLK_CTRL);
+	if (tmp & DBG_CLK_CTRL_CLKACT_TRC)
+		if (clk_prepare_enable(clks[dbg_trc]))
+			pr_warn("%s: trace clk enable failed\n", __func__);
+	if (tmp & DBG_CLK_CTRL_CPU_1XCLKACT)
+		if (clk_prepare_enable(clks[dbg_apb]))
+			pr_warn("%s: debug APB clk enable failed\n", __func__);
+
 	/* One gated clock for all APER clocks. */
 	clks[dma] = clk_register_gate(NULL, clk_output_name[dma],
 			clk_output_name[cpu_2x], 0, SLCR_APER_CLK_CTRL, 0, 0,
diff --git a/drivers/clocksource/Kconfig b/drivers/clocksource/Kconfig
index 96918e1f26a..065131cbfcc 100644
--- a/drivers/clocksource/Kconfig
+++ b/drivers/clocksource/Kconfig
@@ -136,6 +136,11 @@ config CLKSRC_SAMSUNG_PWM
 	  for all devicetree enabled platforms. This driver will be
 	  needed only on systems that do not have the Exynos MCT available.
 
+config FSL_FTM_TIMER
+	bool
+	help
+	  Support for Freescale FlexTimer Module (FTM) timer.
+
 config VF_PIT_TIMER
 	bool
 	help
@@ -191,3 +196,14 @@ config EM_TIMER_STI
 
 config CLKSRC_QCOM
 	bool
+
+config CLKSRC_VERSATILE
+	bool "ARM Versatile (Express) reference platforms clock source"
+	depends on GENERIC_SCHED_CLOCK && !ARCH_USES_GETTIMEOFFSET
+	select CLKSRC_OF
+	default y if MFD_VEXPRESS_SYSREG
+	help
+	  This option enables clock source based on free running
+	  counter available in the "System Registers" block of
+	  ARM Versatile, RealView and Versatile Express reference
+	  platforms.
diff --git a/drivers/clocksource/Makefile b/drivers/clocksource/Makefile
index 98cb6c51aa8..800b1303c23 100644
--- a/drivers/clocksource/Makefile
+++ b/drivers/clocksource/Makefile
@@ -31,6 +31,7 @@ obj-$(CONFIG_CADENCE_TTC_TIMER)	+= cadence_ttc_timer.o
 obj-$(CONFIG_CLKSRC_EFM32)	+= time-efm32.o
 obj-$(CONFIG_CLKSRC_EXYNOS_MCT)	+= exynos_mct.o
 obj-$(CONFIG_CLKSRC_SAMSUNG_PWM)	+= samsung_pwm_timer.o
+obj-$(CONFIG_FSL_FTM_TIMER)	+= fsl_ftm_timer.o
 obj-$(CONFIG_VF_PIT_TIMER)	+= vf_pit_timer.o
 obj-$(CONFIG_CLKSRC_QCOM)	+= qcom-timer.o
 
@@ -39,3 +40,4 @@ obj-$(CONFIG_ARM_GLOBAL_TIMER)		+= arm_global_timer.o
 obj-$(CONFIG_CLKSRC_METAG_GENERIC)	+= metag_generic.o
 obj-$(CONFIG_ARCH_HAS_TICK_BROADCAST)	+= dummy_timer.o
 obj-$(CONFIG_ARCH_KEYSTONE)		+= timer-keystone.o
+obj-$(CONFIG_CLKSRC_VERSATILE)		+= versatile.o
diff --git a/drivers/clocksource/arm_global_timer.c b/drivers/clocksource/arm_global_timer.c
index 0fc31d029e5..60e5a170c4d 100644
--- a/drivers/clocksource/arm_global_timer.c
+++ b/drivers/clocksource/arm_global_timer.c
@@ -246,11 +246,12 @@ static void __init global_timer_of_register(struct device_node *np)
 	int err = 0;
 
 	/*
-	 * In r2p0 the comparators for each processor with the global timer
+	 * In A9 r2p0 the comparators for each processor with the global timer
 	 * fire when the timer value is greater than or equal to. In previous
 	 * revisions the comparators fired when the timer value was equal to.
 	 */
-	if ((read_cpuid_id() & 0xf0000f) < 0x200000) {
+	if (read_cpuid_part_number() == ARM_CPU_PART_CORTEX_A9
+	    && (read_cpuid_id() & 0xf0000f) < 0x200000) {
 		pr_warn("global-timer: non support for this cpu version.\n");
 		return;
 	}
diff --git a/drivers/clocksource/cadence_ttc_timer.c b/drivers/clocksource/cadence_ttc_timer.c
index 49fbe2847c8..7a08811df9a 100644
--- a/drivers/clocksource/cadence_ttc_timer.c
+++ b/drivers/clocksource/cadence_ttc_timer.c
@@ -118,11 +118,11 @@ static void ttc_set_interval(struct ttc_timer *timer,
 	u32 ctrl_reg;
 
 	/* Disable the counter, set the counter value  and re-enable counter */
-	ctrl_reg = __raw_readl(timer->base_addr + TTC_CNT_CNTRL_OFFSET);
+	ctrl_reg = readl_relaxed(timer->base_addr + TTC_CNT_CNTRL_OFFSET);
 	ctrl_reg |= TTC_CNT_CNTRL_DISABLE_MASK;
-	__raw_writel(ctrl_reg, timer->base_addr + TTC_CNT_CNTRL_OFFSET);
+	writel_relaxed(ctrl_reg, timer->base_addr + TTC_CNT_CNTRL_OFFSET);
 
-	__raw_writel(cycles, timer->base_addr + TTC_INTR_VAL_OFFSET);
+	writel_relaxed(cycles, timer->base_addr + TTC_INTR_VAL_OFFSET);
 
 	/*
 	 * Reset the counter (0x10) so that it starts from 0, one-shot
@@ -130,7 +130,7 @@ static void ttc_set_interval(struct ttc_timer *timer,
 	 */
 	ctrl_reg |= CNT_CNTRL_RESET;
 	ctrl_reg &= ~TTC_CNT_CNTRL_DISABLE_MASK;
-	__raw_writel(ctrl_reg, timer->base_addr + TTC_CNT_CNTRL_OFFSET);
+	writel_relaxed(ctrl_reg, timer->base_addr + TTC_CNT_CNTRL_OFFSET);
 }
 
 /**
@@ -147,7 +147,7 @@ static irqreturn_t ttc_clock_event_interrupt(int irq, void *dev_id)
 	struct ttc_timer *timer = &ttce->ttc;
 
 	/* Acknowledge the interrupt and call event handler */
-	__raw_readl(timer->base_addr + TTC_ISR_OFFSET);
+	readl_relaxed(timer->base_addr + TTC_ISR_OFFSET);
 
 	ttce->ce.event_handler(&ttce->ce);
 
@@ -163,13 +163,13 @@ static cycle_t __ttc_clocksource_read(struct clocksource *cs)
 {
 	struct ttc_timer *timer = &to_ttc_timer_clksrc(cs)->ttc;
 
-	return (cycle_t)__raw_readl(timer->base_addr +
+	return (cycle_t)readl_relaxed(timer->base_addr +
 				TTC_COUNT_VAL_OFFSET);
 }
 
 static u64 notrace ttc_sched_clock_read(void)
 {
-	return __raw_readl(ttc_sched_clock_val_reg);
+	return readl_relaxed(ttc_sched_clock_val_reg);
 }
 
 /**
@@ -211,17 +211,17 @@ static void ttc_set_mode(enum clock_event_mode mode,
 	case CLOCK_EVT_MODE_ONESHOT:
 	case CLOCK_EVT_MODE_UNUSED:
 	case CLOCK_EVT_MODE_SHUTDOWN:
-		ctrl_reg = __raw_readl(timer->base_addr +
+		ctrl_reg = readl_relaxed(timer->base_addr +
 					TTC_CNT_CNTRL_OFFSET);
 		ctrl_reg |= TTC_CNT_CNTRL_DISABLE_MASK;
-		__raw_writel(ctrl_reg,
+		writel_relaxed(ctrl_reg,
 				timer->base_addr + TTC_CNT_CNTRL_OFFSET);
 		break;
 	case CLOCK_EVT_MODE_RESUME:
-		ctrl_reg = __raw_readl(timer->base_addr +
+		ctrl_reg = readl_relaxed(timer->base_addr +
 					TTC_CNT_CNTRL_OFFSET);
 		ctrl_reg &= ~TTC_CNT_CNTRL_DISABLE_MASK;
-		__raw_writel(ctrl_reg,
+		writel_relaxed(ctrl_reg,
 				timer->base_addr + TTC_CNT_CNTRL_OFFSET);
 		break;
 	}
@@ -266,8 +266,8 @@ static int ttc_rate_change_clocksource_cb(struct notifier_block *nb,
 		 * of an abort.
 		 */
 		ttccs->scale_clk_ctrl_reg_old =
-			__raw_readl(ttccs->ttc.base_addr +
-					TTC_CLK_CNTRL_OFFSET);
+			readl_relaxed(ttccs->ttc.base_addr +
+			TTC_CLK_CNTRL_OFFSET);
 
 		psv = (ttccs->scale_clk_ctrl_reg_old &
 				TTC_CLK_CNTRL_PSV_MASK) >>
@@ -291,8 +291,8 @@ static int ttc_rate_change_clocksource_cb(struct notifier_block *nb,
 			return NOTIFY_DONE;
 
 		/* scale up: adjust divider now - before frequency change */
-		__raw_writel(ttccs->scale_clk_ctrl_reg_new,
-				ttccs->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
+		writel_relaxed(ttccs->scale_clk_ctrl_reg_new,
+			       ttccs->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
 		break;
 	}
 	case POST_RATE_CHANGE:
@@ -301,8 +301,8 @@ static int ttc_rate_change_clocksource_cb(struct notifier_block *nb,
 			return NOTIFY_OK;
 
 		/* scale down: adjust divider now - after frequency change */
-		__raw_writel(ttccs->scale_clk_ctrl_reg_new,
-				ttccs->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
+		writel_relaxed(ttccs->scale_clk_ctrl_reg_new,
+			       ttccs->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
 		break;
 
 	case ABORT_RATE_CHANGE:
@@ -311,8 +311,8 @@ static int ttc_rate_change_clocksource_cb(struct notifier_block *nb,
 			return NOTIFY_OK;
 
 		/* restore original register value */
-		__raw_writel(ttccs->scale_clk_ctrl_reg_old,
-				ttccs->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
+		writel_relaxed(ttccs->scale_clk_ctrl_reg_old,
+			       ttccs->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
 		/* fall through */
 	default:
 		return NOTIFY_DONE;
@@ -359,10 +359,10 @@ static void __init ttc_setup_clocksource(struct clk *clk, void __iomem *base)
 	 * with no interrupt and it rolls over at 0xFFFF. Pre-scale
 	 * it by 32 also. Let it start running now.
 	 */
-	__raw_writel(0x0,  ttccs->ttc.base_addr + TTC_IER_OFFSET);
-	__raw_writel(CLK_CNTRL_PRESCALE | CLK_CNTRL_PRESCALE_EN,
+	writel_relaxed(0x0,  ttccs->ttc.base_addr + TTC_IER_OFFSET);
+	writel_relaxed(CLK_CNTRL_PRESCALE | CLK_CNTRL_PRESCALE_EN,
 		     ttccs->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
-	__raw_writel(CNT_CNTRL_RESET,
+	writel_relaxed(CNT_CNTRL_RESET,
 		     ttccs->ttc.base_addr + TTC_CNT_CNTRL_OFFSET);
 
 	err = clocksource_register_hz(&ttccs->cs, ttccs->ttc.freq / PRESCALE);
@@ -438,10 +438,10 @@ static void __init ttc_setup_clockevent(struct clk *clk,
 	 * is prescaled by 32 using the interval interrupt. Leave it
 	 * disabled for now.
 	 */
-	__raw_writel(0x23, ttcce->ttc.base_addr + TTC_CNT_CNTRL_OFFSET);
-	__raw_writel(CLK_CNTRL_PRESCALE | CLK_CNTRL_PRESCALE_EN,
+	writel_relaxed(0x23, ttcce->ttc.base_addr + TTC_CNT_CNTRL_OFFSET);
+	writel_relaxed(CLK_CNTRL_PRESCALE | CLK_CNTRL_PRESCALE_EN,
 		     ttcce->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
-	__raw_writel(0x1,  ttcce->ttc.base_addr + TTC_IER_OFFSET);
+	writel_relaxed(0x1,  ttcce->ttc.base_addr + TTC_IER_OFFSET);
 
 	err = request_irq(irq, ttc_clock_event_interrupt,
 			  IRQF_TIMER, ttcce->ce.name, ttcce);
@@ -490,7 +490,7 @@ static void __init ttc_timer_init(struct device_node *timer)
 		BUG();
 	}
 
-	clksel = __raw_readl(timer_baseaddr + TTC_CLK_CNTRL_OFFSET);
+	clksel = readl_relaxed(timer_baseaddr + TTC_CLK_CNTRL_OFFSET);
 	clksel = !!(clksel & TTC_CLK_CNTRL_CSRC_MASK);
 	clk_cs = of_clk_get(timer, clksel);
 	if (IS_ERR(clk_cs)) {
@@ -498,7 +498,7 @@ static void __init ttc_timer_init(struct device_node *timer)
 		BUG();
 	}
 
-	clksel = __raw_readl(timer_baseaddr + 4 + TTC_CLK_CNTRL_OFFSET);
+	clksel = readl_relaxed(timer_baseaddr + 4 + TTC_CLK_CNTRL_OFFSET);
 	clksel = !!(clksel & TTC_CLK_CNTRL_CSRC_MASK);
 	clk_ce = of_clk_get(timer, clksel);
 	if (IS_ERR(clk_ce)) {
diff --git a/drivers/clocksource/clksrc-of.c b/drivers/clocksource/clksrc-of.c
index ae2e4278c42..0093a8e49e1 100644
--- a/drivers/clocksource/clksrc-of.c
+++ b/drivers/clocksource/clksrc-of.c
@@ -27,7 +27,7 @@ void __init clocksource_of_init(void)
 {
 	struct device_node *np;
 	const struct of_device_id *match;
-	clocksource_of_init_fn init_func;
+	of_init_fn_1 init_func;
 	unsigned clocksources = 0;
 
 	for_each_matching_node_and_match(np, __clksrc_of_table, &match) {
diff --git a/drivers/clocksource/dw_apb_timer_of.c b/drivers/clocksource/dw_apb_timer_of.c
index 2a2ea2717f3..d305fb08976 100644
--- a/drivers/clocksource/dw_apb_timer_of.c
+++ b/drivers/clocksource/dw_apb_timer_of.c
@@ -106,7 +106,7 @@ static void __init add_clocksource(struct device_node *source_timer)
 	sched_rate = rate;
 }
 
-static u64 read_sched_clock(void)
+static u64 notrace read_sched_clock(void)
 {
 	return ~__raw_readl(sched_io_base);
 }
diff --git a/drivers/clocksource/em_sti.c b/drivers/clocksource/em_sti.c
index 9d170834fcf..d0a7bd66b8b 100644
--- a/drivers/clocksource/em_sti.c
+++ b/drivers/clocksource/em_sti.c
@@ -318,10 +318,8 @@ static int em_sti_probe(struct platform_device *pdev)
 	int irq;
 
 	p = devm_kzalloc(&pdev->dev, sizeof(*p), GFP_KERNEL);
-	if (p == NULL) {
-		dev_err(&pdev->dev, "failed to allocate driver data\n");
+	if (p == NULL)
 		return -ENOMEM;
-	}
 
 	p->pdev = pdev;
 	platform_set_drvdata(pdev, p);
diff --git a/drivers/clocksource/exynos_mct.c b/drivers/clocksource/exynos_mct.c
index acf5a329d53..8d6420013a0 100644
--- a/drivers/clocksource/exynos_mct.c
+++ b/drivers/clocksource/exynos_mct.c
@@ -24,6 +24,7 @@
 #include <linux/of_irq.h>
 #include <linux/of_address.h>
 #include <linux/clocksource.h>
+#include <linux/sched_clock.h>
 
 #define EXYNOS4_MCTREG(x)		(x)
 #define EXYNOS4_MCT_G_CNT_L		EXYNOS4_MCTREG(0x100)
@@ -192,12 +193,19 @@ struct clocksource mct_frc = {
 	.resume		= exynos4_frc_resume,
 };
 
+static u64 notrace exynos4_read_sched_clock(void)
+{
+	return exynos4_frc_read(&mct_frc);
+}
+
 static void __init exynos4_clocksource_init(void)
 {
 	exynos4_mct_frc_start(0, 0);
 
 	if (clocksource_register_hz(&mct_frc, clk_rate))
 		panic("%s: can't register clocksource\n", mct_frc.name);
+
+	sched_clock_register(exynos4_read_sched_clock, 64, clk_rate);
 }
 
 static void exynos4_mct_comp0_stop(void)
diff --git a/drivers/clocksource/fsl_ftm_timer.c b/drivers/clocksource/fsl_ftm_timer.c
new file mode 100644
index 00000000000..454227d4f89
--- /dev/null
+++ b/drivers/clocksource/fsl_ftm_timer.c
@@ -0,0 +1,367 @@
+/*
+ * Freescale FlexTimer Module (FTM) timer driver.
+ *
+ * Copyright 2014 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.
+ */
+
+#include <linux/clk.h>
+#include <linux/clockchips.h>
+#include <linux/clocksource.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/sched_clock.h>
+#include <linux/slab.h>
+
+#define FTM_SC		0x00
+#define FTM_SC_CLK_SHIFT	3
+#define FTM_SC_CLK_MASK	(0x3 << FTM_SC_CLK_SHIFT)
+#define FTM_SC_CLK(c)	((c) << FTM_SC_CLK_SHIFT)
+#define FTM_SC_PS_MASK	0x7
+#define FTM_SC_TOIE	BIT(6)
+#define FTM_SC_TOF	BIT(7)
+
+#define FTM_CNT		0x04
+#define FTM_MOD		0x08
+#define FTM_CNTIN	0x4C
+
+#define FTM_PS_MAX	7
+
+struct ftm_clock_device {
+	void __iomem *clksrc_base;
+	void __iomem *clkevt_base;
+	unsigned long periodic_cyc;
+	unsigned long ps;
+	bool big_endian;
+};
+
+static struct ftm_clock_device *priv;
+
+static inline u32 ftm_readl(void __iomem *addr)
+{
+	if (priv->big_endian)
+		return ioread32be(addr);
+	else
+		return ioread32(addr);
+}
+
+static inline void ftm_writel(u32 val, void __iomem *addr)
+{
+	if (priv->big_endian)
+		iowrite32be(val, addr);
+	else
+		iowrite32(val, addr);
+}
+
+static inline void ftm_counter_enable(void __iomem *base)
+{
+	u32 val;
+
+	/* select and enable counter clock source */
+	val = ftm_readl(base + FTM_SC);
+	val &= ~(FTM_SC_PS_MASK | FTM_SC_CLK_MASK);
+	val |= priv->ps | FTM_SC_CLK(1);
+	ftm_writel(val, base + FTM_SC);
+}
+
+static inline void ftm_counter_disable(void __iomem *base)
+{
+	u32 val;
+
+	/* disable counter clock source */
+	val = ftm_readl(base + FTM_SC);
+	val &= ~(FTM_SC_PS_MASK | FTM_SC_CLK_MASK);
+	ftm_writel(val, base + FTM_SC);
+}
+
+static inline void ftm_irq_acknowledge(void __iomem *base)
+{
+	u32 val;
+
+	val = ftm_readl(base + FTM_SC);
+	val &= ~FTM_SC_TOF;
+	ftm_writel(val, base + FTM_SC);
+}
+
+static inline void ftm_irq_enable(void __iomem *base)
+{
+	u32 val;
+
+	val = ftm_readl(base + FTM_SC);
+	val |= FTM_SC_TOIE;
+	ftm_writel(val, base + FTM_SC);
+}
+
+static inline void ftm_irq_disable(void __iomem *base)
+{
+	u32 val;
+
+	val = ftm_readl(base + FTM_SC);
+	val &= ~FTM_SC_TOIE;
+	ftm_writel(val, base + FTM_SC);
+}
+
+static inline void ftm_reset_counter(void __iomem *base)
+{
+	/*
+	 * The CNT register contains the FTM counter value.
+	 * Reset clears the CNT register. Writing any value to COUNT
+	 * updates the counter with its initial value, CNTIN.
+	 */
+	ftm_writel(0x00, base + FTM_CNT);
+}
+
+static u64 ftm_read_sched_clock(void)
+{
+	return ftm_readl(priv->clksrc_base + FTM_CNT);
+}
+
+static int ftm_set_next_event(unsigned long delta,
+				struct clock_event_device *unused)
+{
+	/*
+	 * The CNNIN and MOD are all double buffer registers, writing
+	 * to the MOD register latches the value into a buffer. The MOD
+	 * register is updated with the value of its write buffer with
+	 * the following scenario:
+	 * a, the counter source clock is diabled.
+	 */
+	ftm_counter_disable(priv->clkevt_base);
+
+	/* Force the value of CNTIN to be loaded into the FTM counter */
+	ftm_reset_counter(priv->clkevt_base);
+
+	/*
+	 * The counter increments until the value of MOD is reached,
+	 * at which point the counter is reloaded with the value of CNTIN.
+	 * The TOF (the overflow flag) bit is set when the FTM counter
+	 * changes from MOD to CNTIN. So we should using the delta - 1.
+	 */
+	ftm_writel(delta - 1, priv->clkevt_base + FTM_MOD);
+
+	ftm_counter_enable(priv->clkevt_base);
+
+	ftm_irq_enable(priv->clkevt_base);
+
+	return 0;
+}
+
+static void ftm_set_mode(enum clock_event_mode mode,
+				struct clock_event_device *evt)
+{
+	switch (mode) {
+	case CLOCK_EVT_MODE_PERIODIC:
+		ftm_set_next_event(priv->periodic_cyc, evt);
+		break;
+	case CLOCK_EVT_MODE_ONESHOT:
+		ftm_counter_disable(priv->clkevt_base);
+		break;
+	default:
+		return;
+	}
+}
+
+static irqreturn_t ftm_evt_interrupt(int irq, void *dev_id)
+{
+	struct clock_event_device *evt = dev_id;
+
+	ftm_irq_acknowledge(priv->clkevt_base);
+
+	if (likely(evt->mode == CLOCK_EVT_MODE_ONESHOT)) {
+		ftm_irq_disable(priv->clkevt_base);
+		ftm_counter_disable(priv->clkevt_base);
+	}
+
+	evt->event_handler(evt);
+
+	return IRQ_HANDLED;
+}
+
+static struct clock_event_device ftm_clockevent = {
+	.name		= "Freescale ftm timer",
+	.features	= CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT,
+	.set_mode	= ftm_set_mode,
+	.set_next_event	= ftm_set_next_event,
+	.rating		= 300,
+};
+
+static struct irqaction ftm_timer_irq = {
+	.name		= "Freescale ftm timer",
+	.flags		= IRQF_TIMER | IRQF_IRQPOLL,
+	.handler	= ftm_evt_interrupt,
+	.dev_id		= &ftm_clockevent,
+};
+
+static int __init ftm_clockevent_init(unsigned long freq, int irq)
+{
+	int err;
+
+	ftm_writel(0x00, priv->clkevt_base + FTM_CNTIN);
+	ftm_writel(~0UL, priv->clkevt_base + FTM_MOD);
+
+	ftm_reset_counter(priv->clkevt_base);
+
+	err = setup_irq(irq, &ftm_timer_irq);
+	if (err) {
+		pr_err("ftm: setup irq failed: %d\n", err);
+		return err;
+	}
+
+	ftm_clockevent.cpumask = cpumask_of(0);
+	ftm_clockevent.irq = irq;
+
+	clockevents_config_and_register(&ftm_clockevent,
+					freq / (1 << priv->ps),
+					1, 0xffff);
+
+	ftm_counter_enable(priv->clkevt_base);
+
+	return 0;
+}
+
+static int __init ftm_clocksource_init(unsigned long freq)
+{
+	int err;
+
+	ftm_writel(0x00, priv->clksrc_base + FTM_CNTIN);
+	ftm_writel(~0UL, priv->clksrc_base + FTM_MOD);
+
+	ftm_reset_counter(priv->clksrc_base);
+
+	sched_clock_register(ftm_read_sched_clock, 16, freq / (1 << priv->ps));
+	err = clocksource_mmio_init(priv->clksrc_base + FTM_CNT, "fsl-ftm",
+				    freq / (1 << priv->ps), 300, 16,
+				    clocksource_mmio_readl_up);
+	if (err) {
+		pr_err("ftm: init clock source mmio failed: %d\n", err);
+		return err;
+	}
+
+	ftm_counter_enable(priv->clksrc_base);
+
+	return 0;
+}
+
+static int __init __ftm_clk_init(struct device_node *np, char *cnt_name,
+				 char *ftm_name)
+{
+	struct clk *clk;
+	int err;
+
+	clk = of_clk_get_by_name(np, cnt_name);
+	if (IS_ERR(clk)) {
+		pr_err("ftm: Cannot get \"%s\": %ld\n", cnt_name, PTR_ERR(clk));
+		return PTR_ERR(clk);
+	}
+	err = clk_prepare_enable(clk);
+	if (err) {
+		pr_err("ftm: clock failed to prepare+enable \"%s\": %d\n",
+			cnt_name, err);
+		return err;
+	}
+
+	clk = of_clk_get_by_name(np, ftm_name);
+	if (IS_ERR(clk)) {
+		pr_err("ftm: Cannot get \"%s\": %ld\n", ftm_name, PTR_ERR(clk));
+		return PTR_ERR(clk);
+	}
+	err = clk_prepare_enable(clk);
+	if (err)
+		pr_err("ftm: clock failed to prepare+enable \"%s\": %d\n",
+			ftm_name, err);
+
+	return clk_get_rate(clk);
+}
+
+static unsigned long __init ftm_clk_init(struct device_node *np)
+{
+	unsigned long freq;
+
+	freq = __ftm_clk_init(np, "ftm-evt-counter-en", "ftm-evt");
+	if (freq <= 0)
+		return 0;
+
+	freq = __ftm_clk_init(np, "ftm-src-counter-en", "ftm-src");
+	if (freq <= 0)
+		return 0;
+
+	return freq;
+}
+
+static int __init ftm_calc_closest_round_cyc(unsigned long freq)
+{
+	priv->ps = 0;
+
+	/* The counter register is only using the lower 16 bits, and
+	 * if the 'freq' value is to big here, then the periodic_cyc
+	 * may exceed 0xFFFF.
+	 */
+	do {
+		priv->periodic_cyc = DIV_ROUND_CLOSEST(freq,
+						HZ * (1 << priv->ps++));
+	} while (priv->periodic_cyc > 0xFFFF);
+
+	if (priv->ps > FTM_PS_MAX) {
+		pr_err("ftm: the prescaler is %lu > %d\n",
+				priv->ps, FTM_PS_MAX);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void __init ftm_timer_init(struct device_node *np)
+{
+	unsigned long freq;
+	int irq;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return;
+
+	priv->clkevt_base = of_iomap(np, 0);
+	if (!priv->clkevt_base) {
+		pr_err("ftm: unable to map event timer registers\n");
+		goto err;
+	}
+
+	priv->clksrc_base = of_iomap(np, 1);
+	if (!priv->clksrc_base) {
+		pr_err("ftm: unable to map source timer registers\n");
+		goto err;
+	}
+
+	irq = irq_of_parse_and_map(np, 0);
+	if (irq <= 0) {
+		pr_err("ftm: unable to get IRQ from DT, %d\n", irq);
+		goto err;
+	}
+
+	priv->big_endian = of_property_read_bool(np, "big-endian");
+
+	freq = ftm_clk_init(np);
+	if (!freq)
+		goto err;
+
+	if (ftm_calc_closest_round_cyc(freq))
+		goto err;
+
+	if (ftm_clocksource_init(freq))
+		goto err;
+
+	if (ftm_clockevent_init(freq, irq))
+		goto err;
+
+	return;
+
+err:
+	kfree(priv);
+}
+CLOCKSOURCE_OF_DECLARE(flextimer, "fsl,ftm-timer", ftm_timer_init);
diff --git a/drivers/clocksource/mmio.c b/drivers/clocksource/mmio.c
index c0e25125a55..1593ade2a81 100644
--- a/drivers/clocksource/mmio.c
+++ b/drivers/clocksource/mmio.c
@@ -22,22 +22,22 @@ static inline struct clocksource_mmio *to_mmio_clksrc(struct clocksource *c)
 
 cycle_t clocksource_mmio_readl_up(struct clocksource *c)
 {
-	return readl_relaxed(to_mmio_clksrc(c)->reg);
+	return (cycle_t)readl_relaxed(to_mmio_clksrc(c)->reg);
 }
 
 cycle_t clocksource_mmio_readl_down(struct clocksource *c)
 {
-	return ~readl_relaxed(to_mmio_clksrc(c)->reg);
+	return ~(cycle_t)readl_relaxed(to_mmio_clksrc(c)->reg) & c->mask;
 }
 
 cycle_t clocksource_mmio_readw_up(struct clocksource *c)
 {
-	return readw_relaxed(to_mmio_clksrc(c)->reg);
+	return (cycle_t)readw_relaxed(to_mmio_clksrc(c)->reg);
 }
 
 cycle_t clocksource_mmio_readw_down(struct clocksource *c)
 {
-	return ~(unsigned)readw_relaxed(to_mmio_clksrc(c)->reg);
+	return ~(cycle_t)readw_relaxed(to_mmio_clksrc(c)->reg) & c->mask;
 }
 
 /**
diff --git a/drivers/clocksource/qcom-timer.c b/drivers/clocksource/qcom-timer.c
index e807acf4c66..8d115db1e65 100644
--- a/drivers/clocksource/qcom-timer.c
+++ b/drivers/clocksource/qcom-timer.c
@@ -26,6 +26,8 @@
 #include <linux/of_irq.h>
 #include <linux/sched_clock.h>
 
+#include <asm/delay.h>
+
 #define TIMER_MATCH_VAL			0x0000
 #define TIMER_COUNT_VAL			0x0004
 #define TIMER_ENABLE			0x0008
@@ -179,6 +181,15 @@ static u64 notrace msm_sched_clock_read(void)
 	return msm_clocksource.read(&msm_clocksource);
 }
 
+static unsigned long msm_read_current_timer(void)
+{
+	return msm_clocksource.read(&msm_clocksource);
+}
+
+static struct delay_timer msm_delay_timer = {
+	.read_current_timer = msm_read_current_timer,
+};
+
 static void __init msm_timer_init(u32 dgt_hz, int sched_bits, int irq,
 				  bool percpu)
 {
@@ -217,6 +228,8 @@ err:
 	if (res)
 		pr_err("clocksource_register failed\n");
 	sched_clock_register(msm_sched_clock_read, sched_bits, dgt_hz);
+	msm_delay_timer.freq = dgt_hz;
+	register_current_timer_delay(&msm_delay_timer);
 }
 
 #ifdef CONFIG_ARCH_QCOM
diff --git a/drivers/clocksource/sh_cmt.c b/drivers/clocksource/sh_cmt.c
index 0b1836a6c53..dfa780396b9 100644
--- a/drivers/clocksource/sh_cmt.c
+++ b/drivers/clocksource/sh_cmt.c
@@ -11,40 +11,93 @@
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  */
 
+#include <linux/clk.h>
+#include <linux/clockchips.h>
+#include <linux/clocksource.h>
+#include <linux/delay.h>
+#include <linux/err.h>
 #include <linux/init.h>
-#include <linux/platform_device.h>
-#include <linux/spinlock.h>
 #include <linux/interrupt.h>
-#include <linux/ioport.h>
 #include <linux/io.h>
-#include <linux/clk.h>
+#include <linux/ioport.h>
 #include <linux/irq.h>
-#include <linux/err.h>
-#include <linux/delay.h>
-#include <linux/clocksource.h>
-#include <linux/clockchips.h>
-#include <linux/sh_timer.h>
-#include <linux/slab.h>
 #include <linux/module.h>
+#include <linux/platform_device.h>
 #include <linux/pm_domain.h>
 #include <linux/pm_runtime.h>
+#include <linux/sh_timer.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+struct sh_cmt_device;
+
+/*
+ * The CMT comes in 5 different identified flavours, depending not only on the
+ * SoC but also on the particular instance. The following table lists the main
+ * characteristics of those flavours.
+ *
+ *			16B	32B	32B-F	48B	48B-2
+ * -----------------------------------------------------------------------------
+ * Channels		2	1/4	1	6	2/8
+ * Control Width	16	16	16	16	32
+ * Counter Width	16	32	32	32/48	32/48
+ * Shared Start/Stop	Y	Y	Y	Y	N
+ *
+ * The 48-bit gen2 version has a per-channel start/stop register located in the
+ * channel registers block. All other versions have a shared start/stop register
+ * located in the global space.
+ *
+ * Channels are indexed from 0 to N-1 in the documentation. The channel index
+ * infers the start/stop bit position in the control register and the channel
+ * registers block address. Some CMT instances have a subset of channels
+ * available, in which case the index in the documentation doesn't match the
+ * "real" index as implemented in hardware. This is for instance the case with
+ * CMT0 on r8a7740, which is a 32-bit variant with a single channel numbered 0
+ * in the documentation but using start/stop bit 5 and having its registers
+ * block at 0x60.
+ *
+ * Similarly CMT0 on r8a73a4, r8a7790 and r8a7791, while implementing 32-bit
+ * channels only, is a 48-bit gen2 CMT with the 48-bit channels unavailable.
+ */
+
+enum sh_cmt_model {
+	SH_CMT_16BIT,
+	SH_CMT_32BIT,
+	SH_CMT_32BIT_FAST,
+	SH_CMT_48BIT,
+	SH_CMT_48BIT_GEN2,
+};
+
+struct sh_cmt_info {
+	enum sh_cmt_model model;
 
-struct sh_cmt_priv {
-	void __iomem *mapbase;
-	void __iomem *mapbase_str;
-	struct clk *clk;
 	unsigned long width; /* 16 or 32 bit version of hardware block */
 	unsigned long overflow_bit;
 	unsigned long clear_bits;
-	struct irqaction irqaction;
-	struct platform_device *pdev;
 
+	/* callbacks for CMSTR and CMCSR access */
+	unsigned long (*read_control)(void __iomem *base, unsigned long offs);
+	void (*write_control)(void __iomem *base, unsigned long offs,
+			      unsigned long value);
+
+	/* callbacks for CMCNT and CMCOR access */
+	unsigned long (*read_count)(void __iomem *base, unsigned long offs);
+	void (*write_count)(void __iomem *base, unsigned long offs,
+			    unsigned long value);
+};
+
+struct sh_cmt_channel {
+	struct sh_cmt_device *cmt;
+
+	unsigned int index;	/* Index in the documentation */
+	unsigned int hwidx;	/* Real hardware index */
+
+	void __iomem *iostart;
+	void __iomem *ioctrl;
+
+	unsigned int timer_bit;
 	unsigned long flags;
 	unsigned long match_value;
 	unsigned long next_match_value;
@@ -55,38 +108,52 @@ struct sh_cmt_priv {
 	struct clocksource cs;
 	unsigned long total_cycles;
 	bool cs_enabled;
+};
 
-	/* callbacks for CMSTR and CMCSR access */
-	unsigned long (*read_control)(void __iomem *base, unsigned long offs);
-	void (*write_control)(void __iomem *base, unsigned long offs,
-			      unsigned long value);
+struct sh_cmt_device {
+	struct platform_device *pdev;
 
-	/* callbacks for CMCNT and CMCOR access */
-	unsigned long (*read_count)(void __iomem *base, unsigned long offs);
-	void (*write_count)(void __iomem *base, unsigned long offs,
-			    unsigned long value);
+	const struct sh_cmt_info *info;
+	bool legacy;
+
+	void __iomem *mapbase_ch;
+	void __iomem *mapbase;
+	struct clk *clk;
+
+	struct sh_cmt_channel *channels;
+	unsigned int num_channels;
+
+	bool has_clockevent;
+	bool has_clocksource;
 };
 
-/* Examples of supported CMT timer register layouts and I/O access widths:
- *
- * "16-bit counter and 16-bit control" as found on sh7263:
- * CMSTR 0xfffec000 16-bit
- * CMCSR 0xfffec002 16-bit
- * CMCNT 0xfffec004 16-bit
- * CMCOR 0xfffec006 16-bit
- *
- * "32-bit counter and 16-bit control" as found on sh7372, sh73a0, r8a7740:
- * CMSTR 0xffca0000 16-bit
- * CMCSR 0xffca0060 16-bit
- * CMCNT 0xffca0064 32-bit
- * CMCOR 0xffca0068 32-bit
- *
- * "32-bit counter and 32-bit control" as found on r8a73a4 and r8a7790:
- * CMSTR 0xffca0500 32-bit
- * CMCSR 0xffca0510 32-bit
- * CMCNT 0xffca0514 32-bit
- * CMCOR 0xffca0518 32-bit
- */
+#define SH_CMT16_CMCSR_CMF		(1 << 7)
+#define SH_CMT16_CMCSR_CMIE		(1 << 6)
+#define SH_CMT16_CMCSR_CKS8		(0 << 0)
+#define SH_CMT16_CMCSR_CKS32		(1 << 0)
+#define SH_CMT16_CMCSR_CKS128		(2 << 0)
+#define SH_CMT16_CMCSR_CKS512		(3 << 0)
+#define SH_CMT16_CMCSR_CKS_MASK		(3 << 0)
+
+#define SH_CMT32_CMCSR_CMF		(1 << 15)
+#define SH_CMT32_CMCSR_OVF		(1 << 14)
+#define SH_CMT32_CMCSR_WRFLG		(1 << 13)
+#define SH_CMT32_CMCSR_STTF		(1 << 12)
+#define SH_CMT32_CMCSR_STPF		(1 << 11)
+#define SH_CMT32_CMCSR_SSIE		(1 << 10)
+#define SH_CMT32_CMCSR_CMS		(1 << 9)
+#define SH_CMT32_CMCSR_CMM		(1 << 8)
+#define SH_CMT32_CMCSR_CMTOUT_IE	(1 << 7)
+#define SH_CMT32_CMCSR_CMR_NONE		(0 << 4)
+#define SH_CMT32_CMCSR_CMR_DMA		(1 << 4)
+#define SH_CMT32_CMCSR_CMR_IRQ		(2 << 4)
+#define SH_CMT32_CMCSR_CMR_MASK		(3 << 4)
+#define SH_CMT32_CMCSR_DBGIVD		(1 << 3)
+#define SH_CMT32_CMCSR_CKS_RCLK8	(4 << 0)
+#define SH_CMT32_CMCSR_CKS_RCLK32	(5 << 0)
+#define SH_CMT32_CMCSR_CKS_RCLK128	(6 << 0)
+#define SH_CMT32_CMCSR_CKS_RCLK1	(7 << 0)
+#define SH_CMT32_CMCSR_CKS_MASK		(7 << 0)
 
 static unsigned long sh_cmt_read16(void __iomem *base, unsigned long offs)
 {
@@ -110,64 +177,123 @@ static void sh_cmt_write32(void __iomem *base, unsigned long offs,
 	iowrite32(value, base + (offs << 2));
 }
 
+static const struct sh_cmt_info sh_cmt_info[] = {
+	[SH_CMT_16BIT] = {
+		.model = SH_CMT_16BIT,
+		.width = 16,
+		.overflow_bit = SH_CMT16_CMCSR_CMF,
+		.clear_bits = ~SH_CMT16_CMCSR_CMF,
+		.read_control = sh_cmt_read16,
+		.write_control = sh_cmt_write16,
+		.read_count = sh_cmt_read16,
+		.write_count = sh_cmt_write16,
+	},
+	[SH_CMT_32BIT] = {
+		.model = SH_CMT_32BIT,
+		.width = 32,
+		.overflow_bit = SH_CMT32_CMCSR_CMF,
+		.clear_bits = ~(SH_CMT32_CMCSR_CMF | SH_CMT32_CMCSR_OVF),
+		.read_control = sh_cmt_read16,
+		.write_control = sh_cmt_write16,
+		.read_count = sh_cmt_read32,
+		.write_count = sh_cmt_write32,
+	},
+	[SH_CMT_32BIT_FAST] = {
+		.model = SH_CMT_32BIT_FAST,
+		.width = 32,
+		.overflow_bit = SH_CMT32_CMCSR_CMF,
+		.clear_bits = ~(SH_CMT32_CMCSR_CMF | SH_CMT32_CMCSR_OVF),
+		.read_control = sh_cmt_read16,
+		.write_control = sh_cmt_write16,
+		.read_count = sh_cmt_read32,
+		.write_count = sh_cmt_write32,
+	},
+	[SH_CMT_48BIT] = {
+		.model = SH_CMT_48BIT,
+		.width = 32,
+		.overflow_bit = SH_CMT32_CMCSR_CMF,
+		.clear_bits = ~(SH_CMT32_CMCSR_CMF | SH_CMT32_CMCSR_OVF),
+		.read_control = sh_cmt_read32,
+		.write_control = sh_cmt_write32,
+		.read_count = sh_cmt_read32,
+		.write_count = sh_cmt_write32,
+	},
+	[SH_CMT_48BIT_GEN2] = {
+		.model = SH_CMT_48BIT_GEN2,
+		.width = 32,
+		.overflow_bit = SH_CMT32_CMCSR_CMF,
+		.clear_bits = ~(SH_CMT32_CMCSR_CMF | SH_CMT32_CMCSR_OVF),
+		.read_control = sh_cmt_read32,
+		.write_control = sh_cmt_write32,
+		.read_count = sh_cmt_read32,
+		.write_count = sh_cmt_write32,
+	},
+};
+
 #define CMCSR 0 /* channel register */
 #define CMCNT 1 /* channel register */
 #define CMCOR 2 /* channel register */
 
-static inline unsigned long sh_cmt_read_cmstr(struct sh_cmt_priv *p)
+static inline unsigned long sh_cmt_read_cmstr(struct sh_cmt_channel *ch)
 {
-	return p->read_control(p->mapbase_str, 0);
+	if (ch->iostart)
+		return ch->cmt->info->read_control(ch->iostart, 0);
+	else
+		return ch->cmt->info->read_control(ch->cmt->mapbase, 0);
 }
 
-static inline unsigned long sh_cmt_read_cmcsr(struct sh_cmt_priv *p)
+static inline void sh_cmt_write_cmstr(struct sh_cmt_channel *ch,
+				      unsigned long value)
 {
-	return p->read_control(p->mapbase, CMCSR);
+	if (ch->iostart)
+		ch->cmt->info->write_control(ch->iostart, 0, value);
+	else
+		ch->cmt->info->write_control(ch->cmt->mapbase, 0, value);
 }
 
-static inline unsigned long sh_cmt_read_cmcnt(struct sh_cmt_priv *p)
+static inline unsigned long sh_cmt_read_cmcsr(struct sh_cmt_channel *ch)
 {
-	return p->read_count(p->mapbase, CMCNT);
+	return ch->cmt->info->read_control(ch->ioctrl, CMCSR);
 }
 
-static inline void sh_cmt_write_cmstr(struct sh_cmt_priv *p,
+static inline void sh_cmt_write_cmcsr(struct sh_cmt_channel *ch,
 				      unsigned long value)
 {
-	p->write_control(p->mapbase_str, 0, value);
+	ch->cmt->info->write_control(ch->ioctrl, CMCSR, value);
 }
 
-static inline void sh_cmt_write_cmcsr(struct sh_cmt_priv *p,
-				      unsigned long value)
+static inline unsigned long sh_cmt_read_cmcnt(struct sh_cmt_channel *ch)
 {
-	p->write_control(p->mapbase, CMCSR, value);
+	return ch->cmt->info->read_count(ch->ioctrl, CMCNT);
 }
 
-static inline void sh_cmt_write_cmcnt(struct sh_cmt_priv *p,
+static inline void sh_cmt_write_cmcnt(struct sh_cmt_channel *ch,
 				      unsigned long value)
 {
-	p->write_count(p->mapbase, CMCNT, value);
+	ch->cmt->info->write_count(ch->ioctrl, CMCNT, value);
 }
 
-static inline void sh_cmt_write_cmcor(struct sh_cmt_priv *p,
+static inline void sh_cmt_write_cmcor(struct sh_cmt_channel *ch,
 				      unsigned long value)
 {
-	p->write_count(p->mapbase, CMCOR, value);
+	ch->cmt->info->write_count(ch->ioctrl, CMCOR, value);
 }
 
-static unsigned long sh_cmt_get_counter(struct sh_cmt_priv *p,
+static unsigned long sh_cmt_get_counter(struct sh_cmt_channel *ch,
 					int *has_wrapped)
 {
 	unsigned long v1, v2, v3;
 	int o1, o2;
 
-	o1 = sh_cmt_read_cmcsr(p) & p->overflow_bit;
+	o1 = sh_cmt_read_cmcsr(ch) & ch->cmt->info->overflow_bit;
 
 	/* Make sure the timer value is stable. Stolen from acpi_pm.c */
 	do {
 		o2 = o1;
-		v1 = sh_cmt_read_cmcnt(p);
-		v2 = sh_cmt_read_cmcnt(p);
-		v3 = sh_cmt_read_cmcnt(p);
-		o1 = sh_cmt_read_cmcsr(p) & p->overflow_bit;
+		v1 = sh_cmt_read_cmcnt(ch);
+		v2 = sh_cmt_read_cmcnt(ch);
+		v3 = sh_cmt_read_cmcnt(ch);
+		o1 = sh_cmt_read_cmcsr(ch) & ch->cmt->info->overflow_bit;
 	} while (unlikely((o1 != o2) || (v1 > v2 && v1 < v3)
 			  || (v2 > v3 && v2 < v1) || (v3 > v1 && v3 < v2)));
 
@@ -177,52 +303,56 @@ static unsigned long sh_cmt_get_counter(struct sh_cmt_priv *p,
 
 static DEFINE_RAW_SPINLOCK(sh_cmt_lock);
 
-static void sh_cmt_start_stop_ch(struct sh_cmt_priv *p, int start)
+static void sh_cmt_start_stop_ch(struct sh_cmt_channel *ch, int start)
 {
-	struct sh_timer_config *cfg = p->pdev->dev.platform_data;
 	unsigned long flags, value;
 
 	/* start stop register shared by multiple timer channels */
 	raw_spin_lock_irqsave(&sh_cmt_lock, flags);
-	value = sh_cmt_read_cmstr(p);
+	value = sh_cmt_read_cmstr(ch);
 
 	if (start)
-		value |= 1 << cfg->timer_bit;
+		value |= 1 << ch->timer_bit;
 	else
-		value &= ~(1 << cfg->timer_bit);
+		value &= ~(1 << ch->timer_bit);
 
-	sh_cmt_write_cmstr(p, value);
+	sh_cmt_write_cmstr(ch, value);
 	raw_spin_unlock_irqrestore(&sh_cmt_lock, flags);
 }
 
-static int sh_cmt_enable(struct sh_cmt_priv *p, unsigned long *rate)
+static int sh_cmt_enable(struct sh_cmt_channel *ch, unsigned long *rate)
 {
 	int k, ret;
 
-	pm_runtime_get_sync(&p->pdev->dev);
-	dev_pm_syscore_device(&p->pdev->dev, true);
+	pm_runtime_get_sync(&ch->cmt->pdev->dev);
+	dev_pm_syscore_device(&ch->cmt->pdev->dev, true);
 
 	/* enable clock */
-	ret = clk_enable(p->clk);
+	ret = clk_enable(ch->cmt->clk);
 	if (ret) {
-		dev_err(&p->pdev->dev, "cannot enable clock\n");
+		dev_err(&ch->cmt->pdev->dev, "ch%u: cannot enable clock\n",
+			ch->index);
 		goto err0;
 	}
 
 	/* make sure channel is disabled */
-	sh_cmt_start_stop_ch(p, 0);
+	sh_cmt_start_stop_ch(ch, 0);
 
 	/* configure channel, periodic mode and maximum timeout */
-	if (p->width == 16) {
-		*rate = clk_get_rate(p->clk) / 512;
-		sh_cmt_write_cmcsr(p, 0x43);
+	if (ch->cmt->info->width == 16) {
+		*rate = clk_get_rate(ch->cmt->clk) / 512;
+		sh_cmt_write_cmcsr(ch, SH_CMT16_CMCSR_CMIE |
+				   SH_CMT16_CMCSR_CKS512);
 	} else {
-		*rate = clk_get_rate(p->clk) / 8;
-		sh_cmt_write_cmcsr(p, 0x01a4);
+		*rate = clk_get_rate(ch->cmt->clk) / 8;
+		sh_cmt_write_cmcsr(ch, SH_CMT32_CMCSR_CMM |
+				   SH_CMT32_CMCSR_CMTOUT_IE |
+				   SH_CMT32_CMCSR_CMR_IRQ |
+				   SH_CMT32_CMCSR_CKS_RCLK8);
 	}
 
-	sh_cmt_write_cmcor(p, 0xffffffff);
-	sh_cmt_write_cmcnt(p, 0);
+	sh_cmt_write_cmcor(ch, 0xffffffff);
+	sh_cmt_write_cmcnt(ch, 0);
 
 	/*
 	 * According to the sh73a0 user's manual, as CMCNT can be operated
@@ -236,41 +366,42 @@ static int sh_cmt_enable(struct sh_cmt_priv *p, unsigned long *rate)
 	 * take RCLKx2 at maximum.
 	 */
 	for (k = 0; k < 100; k++) {
-		if (!sh_cmt_read_cmcnt(p))
+		if (!sh_cmt_read_cmcnt(ch))
 			break;
 		udelay(1);
 	}
 
-	if (sh_cmt_read_cmcnt(p)) {
-		dev_err(&p->pdev->dev, "cannot clear CMCNT\n");
+	if (sh_cmt_read_cmcnt(ch)) {
+		dev_err(&ch->cmt->pdev->dev, "ch%u: cannot clear CMCNT\n",
+			ch->index);
 		ret = -ETIMEDOUT;
 		goto err1;
 	}
 
 	/* enable channel */
-	sh_cmt_start_stop_ch(p, 1);
+	sh_cmt_start_stop_ch(ch, 1);
 	return 0;
  err1:
 	/* stop clock */
-	clk_disable(p->clk);
+	clk_disable(ch->cmt->clk);
 
  err0:
 	return ret;
 }
 
-static void sh_cmt_disable(struct sh_cmt_priv *p)
+static void sh_cmt_disable(struct sh_cmt_channel *ch)
 {
 	/* disable channel */
-	sh_cmt_start_stop_ch(p, 0);
+	sh_cmt_start_stop_ch(ch, 0);
 
 	/* disable interrupts in CMT block */
-	sh_cmt_write_cmcsr(p, 0);
+	sh_cmt_write_cmcsr(ch, 0);
 
 	/* stop clock */
-	clk_disable(p->clk);
+	clk_disable(ch->cmt->clk);
 
-	dev_pm_syscore_device(&p->pdev->dev, false);
-	pm_runtime_put(&p->pdev->dev);
+	dev_pm_syscore_device(&ch->cmt->pdev->dev, false);
+	pm_runtime_put(&ch->cmt->pdev->dev);
 }
 
 /* private flags */
@@ -280,24 +411,24 @@ static void sh_cmt_disable(struct sh_cmt_priv *p)
 #define FLAG_SKIPEVENT (1 << 3)
 #define FLAG_IRQCONTEXT (1 << 4)
 
-static void sh_cmt_clock_event_program_verify(struct sh_cmt_priv *p,
+static void sh_cmt_clock_event_program_verify(struct sh_cmt_channel *ch,
 					      int absolute)
 {
 	unsigned long new_match;
-	unsigned long value = p->next_match_value;
+	unsigned long value = ch->next_match_value;
 	unsigned long delay = 0;
 	unsigned long now = 0;
 	int has_wrapped;
 
-	now = sh_cmt_get_counter(p, &has_wrapped);
-	p->flags |= FLAG_REPROGRAM; /* force reprogram */
+	now = sh_cmt_get_counter(ch, &has_wrapped);
+	ch->flags |= FLAG_REPROGRAM; /* force reprogram */
 
 	if (has_wrapped) {
 		/* we're competing with the interrupt handler.
 		 *  -> let the interrupt handler reprogram the timer.
 		 *  -> interrupt number two handles the event.
 		 */
-		p->flags |= FLAG_SKIPEVENT;
+		ch->flags |= FLAG_SKIPEVENT;
 		return;
 	}
 
@@ -309,20 +440,20 @@ static void sh_cmt_clock_event_program_verify(struct sh_cmt_priv *p,
 		 * but don't save the new match value yet.
 		 */
 		new_match = now + value + delay;
-		if (new_match > p->max_match_value)
-			new_match = p->max_match_value;
+		if (new_match > ch->max_match_value)
+			new_match = ch->max_match_value;
 
-		sh_cmt_write_cmcor(p, new_match);
+		sh_cmt_write_cmcor(ch, new_match);
 
-		now = sh_cmt_get_counter(p, &has_wrapped);
-		if (has_wrapped && (new_match > p->match_value)) {
+		now = sh_cmt_get_counter(ch, &has_wrapped);
+		if (has_wrapped && (new_match > ch->match_value)) {
 			/* we are changing to a greater match value,
 			 * so this wrap must be caused by the counter
 			 * matching the old value.
 			 * -> first interrupt reprograms the timer.
 			 * -> interrupt number two handles the event.
 			 */
-			p->flags |= FLAG_SKIPEVENT;
+			ch->flags |= FLAG_SKIPEVENT;
 			break;
 		}
 
@@ -333,7 +464,7 @@ static void sh_cmt_clock_event_program_verify(struct sh_cmt_priv *p,
 			 * -> save programmed match value.
 			 * -> let isr handle the event.
 			 */
-			p->match_value = new_match;
+			ch->match_value = new_match;
 			break;
 		}
 
@@ -344,7 +475,7 @@ static void sh_cmt_clock_event_program_verify(struct sh_cmt_priv *p,
 			 * -> save programmed match value.
 			 * -> let isr handle the event.
 			 */
-			p->match_value = new_match;
+			ch->match_value = new_match;
 			break;
 		}
 
@@ -360,138 +491,141 @@ static void sh_cmt_clock_event_program_verify(struct sh_cmt_priv *p,
 			delay = 1;
 
 		if (!delay)
-			dev_warn(&p->pdev->dev, "too long delay\n");
+			dev_warn(&ch->cmt->pdev->dev, "ch%u: too long delay\n",
+				 ch->index);
 
 	} while (delay);
 }
 
-static void __sh_cmt_set_next(struct sh_cmt_priv *p, unsigned long delta)
+static void __sh_cmt_set_next(struct sh_cmt_channel *ch, unsigned long delta)
 {
-	if (delta > p->max_match_value)
-		dev_warn(&p->pdev->dev, "delta out of range\n");
+	if (delta > ch->max_match_value)
+		dev_warn(&ch->cmt->pdev->dev, "ch%u: delta out of range\n",
+			 ch->index);
 
-	p->next_match_value = delta;
-	sh_cmt_clock_event_program_verify(p, 0);
+	ch->next_match_value = delta;
+	sh_cmt_clock_event_program_verify(ch, 0);
 }
 
-static void sh_cmt_set_next(struct sh_cmt_priv *p, unsigned long delta)
+static void sh_cmt_set_next(struct sh_cmt_channel *ch, unsigned long delta)
 {
 	unsigned long flags;
 
-	raw_spin_lock_irqsave(&p->lock, flags);
-	__sh_cmt_set_next(p, delta);
-	raw_spin_unlock_irqrestore(&p->lock, flags);
+	raw_spin_lock_irqsave(&ch->lock, flags);
+	__sh_cmt_set_next(ch, delta);
+	raw_spin_unlock_irqrestore(&ch->lock, flags);
 }
 
 static irqreturn_t sh_cmt_interrupt(int irq, void *dev_id)
 {
-	struct sh_cmt_priv *p = dev_id;
+	struct sh_cmt_channel *ch = dev_id;
 
 	/* clear flags */
-	sh_cmt_write_cmcsr(p, sh_cmt_read_cmcsr(p) & p->clear_bits);
+	sh_cmt_write_cmcsr(ch, sh_cmt_read_cmcsr(ch) &
+			   ch->cmt->info->clear_bits);
 
 	/* update clock source counter to begin with if enabled
 	 * the wrap flag should be cleared by the timer specific
 	 * isr before we end up here.
 	 */
-	if (p->flags & FLAG_CLOCKSOURCE)
-		p->total_cycles += p->match_value + 1;
+	if (ch->flags & FLAG_CLOCKSOURCE)
+		ch->total_cycles += ch->match_value + 1;
 
-	if (!(p->flags & FLAG_REPROGRAM))
-		p->next_match_value = p->max_match_value;
+	if (!(ch->flags & FLAG_REPROGRAM))
+		ch->next_match_value = ch->max_match_value;
 
-	p->flags |= FLAG_IRQCONTEXT;
+	ch->flags |= FLAG_IRQCONTEXT;
 
-	if (p->flags & FLAG_CLOCKEVENT) {
-		if (!(p->flags & FLAG_SKIPEVENT)) {
-			if (p->ced.mode == CLOCK_EVT_MODE_ONESHOT) {
-				p->next_match_value = p->max_match_value;
-				p->flags |= FLAG_REPROGRAM;
+	if (ch->flags & FLAG_CLOCKEVENT) {
+		if (!(ch->flags & FLAG_SKIPEVENT)) {
+			if (ch->ced.mode == CLOCK_EVT_MODE_ONESHOT) {
+				ch->next_match_value = ch->max_match_value;
+				ch->flags |= FLAG_REPROGRAM;
 			}
 
-			p->ced.event_handler(&p->ced);
+			ch->ced.event_handler(&ch->ced);
 		}
 	}
 
-	p->flags &= ~FLAG_SKIPEVENT;
+	ch->flags &= ~FLAG_SKIPEVENT;
 
-	if (p->flags & FLAG_REPROGRAM) {
-		p->flags &= ~FLAG_REPROGRAM;
-		sh_cmt_clock_event_program_verify(p, 1);
+	if (ch->flags & FLAG_REPROGRAM) {
+		ch->flags &= ~FLAG_REPROGRAM;
+		sh_cmt_clock_event_program_verify(ch, 1);
 
-		if (p->flags & FLAG_CLOCKEVENT)
-			if ((p->ced.mode == CLOCK_EVT_MODE_SHUTDOWN)
-			    || (p->match_value == p->next_match_value))
-				p->flags &= ~FLAG_REPROGRAM;
+		if (ch->flags & FLAG_CLOCKEVENT)
+			if ((ch->ced.mode == CLOCK_EVT_MODE_SHUTDOWN)
+			    || (ch->match_value == ch->next_match_value))
+				ch->flags &= ~FLAG_REPROGRAM;
 	}
 
-	p->flags &= ~FLAG_IRQCONTEXT;
+	ch->flags &= ~FLAG_IRQCONTEXT;
 
 	return IRQ_HANDLED;
 }
 
-static int sh_cmt_start(struct sh_cmt_priv *p, unsigned long flag)
+static int sh_cmt_start(struct sh_cmt_channel *ch, unsigned long flag)
 {
 	int ret = 0;
 	unsigned long flags;
 
-	raw_spin_lock_irqsave(&p->lock, flags);
+	raw_spin_lock_irqsave(&ch->lock, flags);
 
-	if (!(p->flags & (FLAG_CLOCKEVENT | FLAG_CLOCKSOURCE)))
-		ret = sh_cmt_enable(p, &p->rate);
+	if (!(ch->flags & (FLAG_CLOCKEVENT | FLAG_CLOCKSOURCE)))
+		ret = sh_cmt_enable(ch, &ch->rate);
 
 	if (ret)
 		goto out;
-	p->flags |= flag;
+	ch->flags |= flag;
 
 	/* setup timeout if no clockevent */
-	if ((flag == FLAG_CLOCKSOURCE) && (!(p->flags & FLAG_CLOCKEVENT)))
-		__sh_cmt_set_next(p, p->max_match_value);
+	if ((flag == FLAG_CLOCKSOURCE) && (!(ch->flags & FLAG_CLOCKEVENT)))
+		__sh_cmt_set_next(ch, ch->max_match_value);
  out:
-	raw_spin_unlock_irqrestore(&p->lock, flags);
+	raw_spin_unlock_irqrestore(&ch->lock, flags);
 
 	return ret;
 }
 
-static void sh_cmt_stop(struct sh_cmt_priv *p, unsigned long flag)
+static void sh_cmt_stop(struct sh_cmt_channel *ch, unsigned long flag)
 {
 	unsigned long flags;
 	unsigned long f;
 
-	raw_spin_lock_irqsave(&p->lock, flags);
+	raw_spin_lock_irqsave(&ch->lock, flags);
 
-	f = p->flags & (FLAG_CLOCKEVENT | FLAG_CLOCKSOURCE);
-	p->flags &= ~flag;
+	f = ch->flags & (FLAG_CLOCKEVENT | FLAG_CLOCKSOURCE);
+	ch->flags &= ~flag;
 
-	if (f && !(p->flags & (FLAG_CLOCKEVENT | FLAG_CLOCKSOURCE)))
-		sh_cmt_disable(p);
+	if (f && !(ch->flags & (FLAG_CLOCKEVENT | FLAG_CLOCKSOURCE)))
+		sh_cmt_disable(ch);
 
 	/* adjust the timeout to maximum if only clocksource left */
-	if ((flag == FLAG_CLOCKEVENT) && (p->flags & FLAG_CLOCKSOURCE))
-		__sh_cmt_set_next(p, p->max_match_value);
+	if ((flag == FLAG_CLOCKEVENT) && (ch->flags & FLAG_CLOCKSOURCE))
+		__sh_cmt_set_next(ch, ch->max_match_value);
 
-	raw_spin_unlock_irqrestore(&p->lock, flags);
+	raw_spin_unlock_irqrestore(&ch->lock, flags);
 }
 
-static struct sh_cmt_priv *cs_to_sh_cmt(struct clocksource *cs)
+static struct sh_cmt_channel *cs_to_sh_cmt(struct clocksource *cs)
 {
-	return container_of(cs, struct sh_cmt_priv, cs);
+	return container_of(cs, struct sh_cmt_channel, cs);
 }
 
 static cycle_t sh_cmt_clocksource_read(struct clocksource *cs)
 {
-	struct sh_cmt_priv *p = cs_to_sh_cmt(cs);
+	struct sh_cmt_channel *ch = cs_to_sh_cmt(cs);
 	unsigned long flags, raw;
 	unsigned long value;
 	int has_wrapped;
 
-	raw_spin_lock_irqsave(&p->lock, flags);
-	value = p->total_cycles;
-	raw = sh_cmt_get_counter(p, &has_wrapped);
+	raw_spin_lock_irqsave(&ch->lock, flags);
+	value = ch->total_cycles;
+	raw = sh_cmt_get_counter(ch, &has_wrapped);
 
 	if (unlikely(has_wrapped))
-		raw += p->match_value + 1;
-	raw_spin_unlock_irqrestore(&p->lock, flags);
+		raw += ch->match_value + 1;
+	raw_spin_unlock_irqrestore(&ch->lock, flags);
 
 	return value + raw;
 }
@@ -499,53 +633,53 @@ static cycle_t sh_cmt_clocksource_read(struct clocksource *cs)
 static int sh_cmt_clocksource_enable(struct clocksource *cs)
 {
 	int ret;
-	struct sh_cmt_priv *p = cs_to_sh_cmt(cs);
+	struct sh_cmt_channel *ch = cs_to_sh_cmt(cs);
 
-	WARN_ON(p->cs_enabled);
+	WARN_ON(ch->cs_enabled);
 
-	p->total_cycles = 0;
+	ch->total_cycles = 0;
 
-	ret = sh_cmt_start(p, FLAG_CLOCKSOURCE);
+	ret = sh_cmt_start(ch, FLAG_CLOCKSOURCE);
 	if (!ret) {
-		__clocksource_updatefreq_hz(cs, p->rate);
-		p->cs_enabled = true;
+		__clocksource_updatefreq_hz(cs, ch->rate);
+		ch->cs_enabled = true;
 	}
 	return ret;
 }
 
 static void sh_cmt_clocksource_disable(struct clocksource *cs)
 {
-	struct sh_cmt_priv *p = cs_to_sh_cmt(cs);
+	struct sh_cmt_channel *ch = cs_to_sh_cmt(cs);
 
-	WARN_ON(!p->cs_enabled);
+	WARN_ON(!ch->cs_enabled);
 
-	sh_cmt_stop(p, FLAG_CLOCKSOURCE);
-	p->cs_enabled = false;
+	sh_cmt_stop(ch, FLAG_CLOCKSOURCE);
+	ch->cs_enabled = false;
 }
 
 static void sh_cmt_clocksource_suspend(struct clocksource *cs)
 {
-	struct sh_cmt_priv *p = cs_to_sh_cmt(cs);
+	struct sh_cmt_channel *ch = cs_to_sh_cmt(cs);
 
-	sh_cmt_stop(p, FLAG_CLOCKSOURCE);
-	pm_genpd_syscore_poweroff(&p->pdev->dev);
+	sh_cmt_stop(ch, FLAG_CLOCKSOURCE);
+	pm_genpd_syscore_poweroff(&ch->cmt->pdev->dev);
 }
 
 static void sh_cmt_clocksource_resume(struct clocksource *cs)
 {
-	struct sh_cmt_priv *p = cs_to_sh_cmt(cs);
+	struct sh_cmt_channel *ch = cs_to_sh_cmt(cs);
 
-	pm_genpd_syscore_poweron(&p->pdev->dev);
-	sh_cmt_start(p, FLAG_CLOCKSOURCE);
+	pm_genpd_syscore_poweron(&ch->cmt->pdev->dev);
+	sh_cmt_start(ch, FLAG_CLOCKSOURCE);
 }
 
-static int sh_cmt_register_clocksource(struct sh_cmt_priv *p,
-				       char *name, unsigned long rating)
+static int sh_cmt_register_clocksource(struct sh_cmt_channel *ch,
+				       const char *name)
 {
-	struct clocksource *cs = &p->cs;
+	struct clocksource *cs = &ch->cs;
 
 	cs->name = name;
-	cs->rating = rating;
+	cs->rating = 125;
 	cs->read = sh_cmt_clocksource_read;
 	cs->enable = sh_cmt_clocksource_enable;
 	cs->disable = sh_cmt_clocksource_disable;
@@ -554,47 +688,48 @@ static int sh_cmt_register_clocksource(struct sh_cmt_priv *p,
 	cs->mask = CLOCKSOURCE_MASK(sizeof(unsigned long) * 8);
 	cs->flags = CLOCK_SOURCE_IS_CONTINUOUS;
 
-	dev_info(&p->pdev->dev, "used as clock source\n");
+	dev_info(&ch->cmt->pdev->dev, "ch%u: used as clock source\n",
+		 ch->index);
 
 	/* Register with dummy 1 Hz value, gets updated in ->enable() */
 	clocksource_register_hz(cs, 1);
 	return 0;
 }
 
-static struct sh_cmt_priv *ced_to_sh_cmt(struct clock_event_device *ced)
+static struct sh_cmt_channel *ced_to_sh_cmt(struct clock_event_device *ced)
 {
-	return container_of(ced, struct sh_cmt_priv, ced);
+	return container_of(ced, struct sh_cmt_channel, ced);
 }
 
-static void sh_cmt_clock_event_start(struct sh_cmt_priv *p, int periodic)
+static void sh_cmt_clock_event_start(struct sh_cmt_channel *ch, int periodic)
 {
-	struct clock_event_device *ced = &p->ced;
+	struct clock_event_device *ced = &ch->ced;
 
-	sh_cmt_start(p, FLAG_CLOCKEVENT);
+	sh_cmt_start(ch, FLAG_CLOCKEVENT);
 
 	/* TODO: calculate good shift from rate and counter bit width */
 
 	ced->shift = 32;
-	ced->mult = div_sc(p->rate, NSEC_PER_SEC, ced->shift);
-	ced->max_delta_ns = clockevent_delta2ns(p->max_match_value, ced);
+	ced->mult = div_sc(ch->rate, NSEC_PER_SEC, ced->shift);
+	ced->max_delta_ns = clockevent_delta2ns(ch->max_match_value, ced);
 	ced->min_delta_ns = clockevent_delta2ns(0x1f, ced);
 
 	if (periodic)
-		sh_cmt_set_next(p, ((p->rate + HZ/2) / HZ) - 1);
+		sh_cmt_set_next(ch, ((ch->rate + HZ/2) / HZ) - 1);
 	else
-		sh_cmt_set_next(p, p->max_match_value);
+		sh_cmt_set_next(ch, ch->max_match_value);
 }
 
 static void sh_cmt_clock_event_mode(enum clock_event_mode mode,
 				    struct clock_event_device *ced)
 {
-	struct sh_cmt_priv *p = ced_to_sh_cmt(ced);
+	struct sh_cmt_channel *ch = ced_to_sh_cmt(ced);
 
 	/* deal with old setting first */
 	switch (ced->mode) {
 	case CLOCK_EVT_MODE_PERIODIC:
 	case CLOCK_EVT_MODE_ONESHOT:
-		sh_cmt_stop(p, FLAG_CLOCKEVENT);
+		sh_cmt_stop(ch, FLAG_CLOCKEVENT);
 		break;
 	default:
 		break;
@@ -602,16 +737,18 @@ static void sh_cmt_clock_event_mode(enum clock_event_mode mode,
 
 	switch (mode) {
 	case CLOCK_EVT_MODE_PERIODIC:
-		dev_info(&p->pdev->dev, "used for periodic clock events\n");
-		sh_cmt_clock_event_start(p, 1);
+		dev_info(&ch->cmt->pdev->dev,
+			 "ch%u: used for periodic clock events\n", ch->index);
+		sh_cmt_clock_event_start(ch, 1);
 		break;
 	case CLOCK_EVT_MODE_ONESHOT:
-		dev_info(&p->pdev->dev, "used for oneshot clock events\n");
-		sh_cmt_clock_event_start(p, 0);
+		dev_info(&ch->cmt->pdev->dev,
+			 "ch%u: used for oneshot clock events\n", ch->index);
+		sh_cmt_clock_event_start(ch, 0);
 		break;
 	case CLOCK_EVT_MODE_SHUTDOWN:
 	case CLOCK_EVT_MODE_UNUSED:
-		sh_cmt_stop(p, FLAG_CLOCKEVENT);
+		sh_cmt_stop(ch, FLAG_CLOCKEVENT);
 		break;
 	default:
 		break;
@@ -621,196 +758,341 @@ static void sh_cmt_clock_event_mode(enum clock_event_mode mode,
 static int sh_cmt_clock_event_next(unsigned long delta,
 				   struct clock_event_device *ced)
 {
-	struct sh_cmt_priv *p = ced_to_sh_cmt(ced);
+	struct sh_cmt_channel *ch = ced_to_sh_cmt(ced);
 
 	BUG_ON(ced->mode != CLOCK_EVT_MODE_ONESHOT);
-	if (likely(p->flags & FLAG_IRQCONTEXT))
-		p->next_match_value = delta - 1;
+	if (likely(ch->flags & FLAG_IRQCONTEXT))
+		ch->next_match_value = delta - 1;
 	else
-		sh_cmt_set_next(p, delta - 1);
+		sh_cmt_set_next(ch, delta - 1);
 
 	return 0;
 }
 
 static void sh_cmt_clock_event_suspend(struct clock_event_device *ced)
 {
-	struct sh_cmt_priv *p = ced_to_sh_cmt(ced);
+	struct sh_cmt_channel *ch = ced_to_sh_cmt(ced);
 
-	pm_genpd_syscore_poweroff(&p->pdev->dev);
-	clk_unprepare(p->clk);
+	pm_genpd_syscore_poweroff(&ch->cmt->pdev->dev);
+	clk_unprepare(ch->cmt->clk);
 }
 
 static void sh_cmt_clock_event_resume(struct clock_event_device *ced)
 {
-	struct sh_cmt_priv *p = ced_to_sh_cmt(ced);
+	struct sh_cmt_channel *ch = ced_to_sh_cmt(ced);
 
-	clk_prepare(p->clk);
-	pm_genpd_syscore_poweron(&p->pdev->dev);
+	clk_prepare(ch->cmt->clk);
+	pm_genpd_syscore_poweron(&ch->cmt->pdev->dev);
 }
 
-static void sh_cmt_register_clockevent(struct sh_cmt_priv *p,
-				       char *name, unsigned long rating)
+static int sh_cmt_register_clockevent(struct sh_cmt_channel *ch,
+				      const char *name)
 {
-	struct clock_event_device *ced = &p->ced;
+	struct clock_event_device *ced = &ch->ced;
+	int irq;
+	int ret;
 
-	memset(ced, 0, sizeof(*ced));
+	irq = platform_get_irq(ch->cmt->pdev, ch->cmt->legacy ? 0 : ch->index);
+	if (irq < 0) {
+		dev_err(&ch->cmt->pdev->dev, "ch%u: failed to get irq\n",
+			ch->index);
+		return irq;
+	}
+
+	ret = request_irq(irq, sh_cmt_interrupt,
+			  IRQF_TIMER | IRQF_IRQPOLL | IRQF_NOBALANCING,
+			  dev_name(&ch->cmt->pdev->dev), ch);
+	if (ret) {
+		dev_err(&ch->cmt->pdev->dev, "ch%u: failed to request irq %d\n",
+			ch->index, irq);
+		return ret;
+	}
 
 	ced->name = name;
 	ced->features = CLOCK_EVT_FEAT_PERIODIC;
 	ced->features |= CLOCK_EVT_FEAT_ONESHOT;
-	ced->rating = rating;
-	ced->cpumask = cpumask_of(0);
+	ced->rating = 125;
+	ced->cpumask = cpu_possible_mask;
 	ced->set_next_event = sh_cmt_clock_event_next;
 	ced->set_mode = sh_cmt_clock_event_mode;
 	ced->suspend = sh_cmt_clock_event_suspend;
 	ced->resume = sh_cmt_clock_event_resume;
 
-	dev_info(&p->pdev->dev, "used for clock events\n");
+	dev_info(&ch->cmt->pdev->dev, "ch%u: used for clock events\n",
+		 ch->index);
 	clockevents_register_device(ced);
+
+	return 0;
 }
 
-static int sh_cmt_register(struct sh_cmt_priv *p, char *name,
-			   unsigned long clockevent_rating,
-			   unsigned long clocksource_rating)
+static int sh_cmt_register(struct sh_cmt_channel *ch, const char *name,
+			   bool clockevent, bool clocksource)
 {
-	if (clockevent_rating)
-		sh_cmt_register_clockevent(p, name, clockevent_rating);
+	int ret;
 
-	if (clocksource_rating)
-		sh_cmt_register_clocksource(p, name, clocksource_rating);
+	if (clockevent) {
+		ch->cmt->has_clockevent = true;
+		ret = sh_cmt_register_clockevent(ch, name);
+		if (ret < 0)
+			return ret;
+	}
+
+	if (clocksource) {
+		ch->cmt->has_clocksource = true;
+		sh_cmt_register_clocksource(ch, name);
+	}
 
 	return 0;
 }
 
-static int sh_cmt_setup(struct sh_cmt_priv *p, struct platform_device *pdev)
+static int sh_cmt_setup_channel(struct sh_cmt_channel *ch, unsigned int index,
+				unsigned int hwidx, bool clockevent,
+				bool clocksource, struct sh_cmt_device *cmt)
 {
-	struct sh_timer_config *cfg = pdev->dev.platform_data;
-	struct resource *res, *res2;
-	int irq, ret;
-	ret = -ENXIO;
+	int ret;
 
-	memset(p, 0, sizeof(*p));
-	p->pdev = pdev;
+	/* Skip unused channels. */
+	if (!clockevent && !clocksource)
+		return 0;
 
-	if (!cfg) {
-		dev_err(&p->pdev->dev, "missing platform data\n");
-		goto err0;
+	ch->cmt = cmt;
+	ch->index = index;
+	ch->hwidx = hwidx;
+
+	/*
+	 * Compute the address of the channel control register block. For the
+	 * timers with a per-channel start/stop register, compute its address
+	 * as well.
+	 *
+	 * For legacy configuration the address has been mapped explicitly.
+	 */
+	if (cmt->legacy) {
+		ch->ioctrl = cmt->mapbase_ch;
+	} else {
+		switch (cmt->info->model) {
+		case SH_CMT_16BIT:
+			ch->ioctrl = cmt->mapbase + 2 + ch->hwidx * 6;
+			break;
+		case SH_CMT_32BIT:
+		case SH_CMT_48BIT:
+			ch->ioctrl = cmt->mapbase + 0x10 + ch->hwidx * 0x10;
+			break;
+		case SH_CMT_32BIT_FAST:
+			/*
+			 * The 32-bit "fast" timer has a single channel at hwidx
+			 * 5 but is located at offset 0x40 instead of 0x60 for
+			 * some reason.
+			 */
+			ch->ioctrl = cmt->mapbase + 0x40;
+			break;
+		case SH_CMT_48BIT_GEN2:
+			ch->iostart = cmt->mapbase + ch->hwidx * 0x100;
+			ch->ioctrl = ch->iostart + 0x10;
+			break;
+		}
 	}
 
-	res = platform_get_resource(p->pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&p->pdev->dev, "failed to get I/O memory\n");
-		goto err0;
+	if (cmt->info->width == (sizeof(ch->max_match_value) * 8))
+		ch->max_match_value = ~0;
+	else
+		ch->max_match_value = (1 << cmt->info->width) - 1;
+
+	ch->match_value = ch->max_match_value;
+	raw_spin_lock_init(&ch->lock);
+
+	if (cmt->legacy) {
+		ch->timer_bit = ch->hwidx;
+	} else {
+		ch->timer_bit = cmt->info->model == SH_CMT_48BIT_GEN2
+			      ? 0 : ch->hwidx;
 	}
 
-	/* optional resource for the shared timer start/stop register */
-	res2 = platform_get_resource(p->pdev, IORESOURCE_MEM, 1);
+	ret = sh_cmt_register(ch, dev_name(&cmt->pdev->dev),
+			      clockevent, clocksource);
+	if (ret) {
+		dev_err(&cmt->pdev->dev, "ch%u: registration failed\n",
+			ch->index);
+		return ret;
+	}
+	ch->cs_enabled = false;
 
-	irq = platform_get_irq(p->pdev, 0);
-	if (irq < 0) {
-		dev_err(&p->pdev->dev, "failed to get irq\n");
-		goto err0;
+	return 0;
+}
+
+static int sh_cmt_map_memory(struct sh_cmt_device *cmt)
+{
+	struct resource *mem;
+
+	mem = platform_get_resource(cmt->pdev, IORESOURCE_MEM, 0);
+	if (!mem) {
+		dev_err(&cmt->pdev->dev, "failed to get I/O memory\n");
+		return -ENXIO;
 	}
 
-	/* map memory, let mapbase point to our channel */
-	p->mapbase = ioremap_nocache(res->start, resource_size(res));
-	if (p->mapbase == NULL) {
-		dev_err(&p->pdev->dev, "failed to remap I/O memory\n");
-		goto err0;
+	cmt->mapbase = ioremap_nocache(mem->start, resource_size(mem));
+	if (cmt->mapbase == NULL) {
+		dev_err(&cmt->pdev->dev, "failed to remap I/O memory\n");
+		return -ENXIO;
 	}
 
-	/* map second resource for CMSTR */
-	p->mapbase_str = ioremap_nocache(res2 ? res2->start :
-					 res->start - cfg->channel_offset,
-					 res2 ? resource_size(res2) : 2);
-	if (p->mapbase_str == NULL) {
-		dev_err(&p->pdev->dev, "failed to remap I/O second memory\n");
-		goto err1;
+	return 0;
+}
+
+static int sh_cmt_map_memory_legacy(struct sh_cmt_device *cmt)
+{
+	struct sh_timer_config *cfg = cmt->pdev->dev.platform_data;
+	struct resource *res, *res2;
+
+	/* map memory, let mapbase_ch point to our channel */
+	res = platform_get_resource(cmt->pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&cmt->pdev->dev, "failed to get I/O memory\n");
+		return -ENXIO;
 	}
 
-	/* request irq using setup_irq() (too early for request_irq()) */
-	p->irqaction.name = dev_name(&p->pdev->dev);
-	p->irqaction.handler = sh_cmt_interrupt;
-	p->irqaction.dev_id = p;
-	p->irqaction.flags = IRQF_TIMER | IRQF_IRQPOLL | IRQF_NOBALANCING;
-
-	/* get hold of clock */
-	p->clk = clk_get(&p->pdev->dev, "cmt_fck");
-	if (IS_ERR(p->clk)) {
-		dev_err(&p->pdev->dev, "cannot get clock\n");
-		ret = PTR_ERR(p->clk);
-		goto err2;
+	cmt->mapbase_ch = ioremap_nocache(res->start, resource_size(res));
+	if (cmt->mapbase_ch == NULL) {
+		dev_err(&cmt->pdev->dev, "failed to remap I/O memory\n");
+		return -ENXIO;
 	}
 
-	ret = clk_prepare(p->clk);
-	if (ret < 0)
-		goto err3;
+	/* optional resource for the shared timer start/stop register */
+	res2 = platform_get_resource(cmt->pdev, IORESOURCE_MEM, 1);
 
-	if (res2 && (resource_size(res2) == 4)) {
-		/* assume both CMSTR and CMCSR to be 32-bit */
-		p->read_control = sh_cmt_read32;
-		p->write_control = sh_cmt_write32;
-	} else {
-		p->read_control = sh_cmt_read16;
-		p->write_control = sh_cmt_write16;
+	/* map second resource for CMSTR */
+	cmt->mapbase = ioremap_nocache(res2 ? res2->start :
+				       res->start - cfg->channel_offset,
+				       res2 ? resource_size(res2) : 2);
+	if (cmt->mapbase == NULL) {
+		dev_err(&cmt->pdev->dev, "failed to remap I/O second memory\n");
+		iounmap(cmt->mapbase_ch);
+		return -ENXIO;
 	}
 
-	if (resource_size(res) == 6) {
-		p->width = 16;
-		p->read_count = sh_cmt_read16;
-		p->write_count = sh_cmt_write16;
-		p->overflow_bit = 0x80;
-		p->clear_bits = ~0x80;
-	} else {
-		p->width = 32;
-		p->read_count = sh_cmt_read32;
-		p->write_count = sh_cmt_write32;
-		p->overflow_bit = 0x8000;
-		p->clear_bits = ~0xc000;
+	/* identify the model based on the resources */
+	if (resource_size(res) == 6)
+		cmt->info = &sh_cmt_info[SH_CMT_16BIT];
+	else if (res2 && (resource_size(res2) == 4))
+		cmt->info = &sh_cmt_info[SH_CMT_48BIT_GEN2];
+	else
+		cmt->info = &sh_cmt_info[SH_CMT_32BIT];
+
+	return 0;
+}
+
+static void sh_cmt_unmap_memory(struct sh_cmt_device *cmt)
+{
+	iounmap(cmt->mapbase);
+	if (cmt->mapbase_ch)
+		iounmap(cmt->mapbase_ch);
+}
+
+static int sh_cmt_setup(struct sh_cmt_device *cmt, struct platform_device *pdev)
+{
+	struct sh_timer_config *cfg = pdev->dev.platform_data;
+	const struct platform_device_id *id = pdev->id_entry;
+	unsigned int hw_channels;
+	int ret;
+
+	memset(cmt, 0, sizeof(*cmt));
+	cmt->pdev = pdev;
+
+	if (!cfg) {
+		dev_err(&cmt->pdev->dev, "missing platform data\n");
+		return -ENXIO;
 	}
 
-	if (p->width == (sizeof(p->max_match_value) * 8))
-		p->max_match_value = ~0;
+	cmt->info = (const struct sh_cmt_info *)id->driver_data;
+	cmt->legacy = cmt->info ? false : true;
+
+	/* Get hold of clock. */
+	cmt->clk = clk_get(&cmt->pdev->dev, cmt->legacy ? "cmt_fck" : "fck");
+	if (IS_ERR(cmt->clk)) {
+		dev_err(&cmt->pdev->dev, "cannot get clock\n");
+		return PTR_ERR(cmt->clk);
+	}
+
+	ret = clk_prepare(cmt->clk);
+	if (ret < 0)
+		goto err_clk_put;
+
+	/*
+	 * Map the memory resource(s). We need to support both the legacy
+	 * platform device configuration (with one device per channel) and the
+	 * new version (with multiple channels per device).
+	 */
+	if (cmt->legacy)
+		ret = sh_cmt_map_memory_legacy(cmt);
 	else
-		p->max_match_value = (1 << p->width) - 1;
+		ret = sh_cmt_map_memory(cmt);
 
-	p->match_value = p->max_match_value;
-	raw_spin_lock_init(&p->lock);
+	if (ret < 0)
+		goto err_clk_unprepare;
 
-	ret = sh_cmt_register(p, (char *)dev_name(&p->pdev->dev),
-			      cfg->clockevent_rating,
-			      cfg->clocksource_rating);
-	if (ret) {
-		dev_err(&p->pdev->dev, "registration failed\n");
-		goto err4;
+	/* Allocate and setup the channels. */
+	if (cmt->legacy) {
+		cmt->num_channels = 1;
+		hw_channels = 0;
+	} else {
+		cmt->num_channels = hweight8(cfg->channels_mask);
+		hw_channels = cfg->channels_mask;
 	}
-	p->cs_enabled = false;
 
-	ret = setup_irq(irq, &p->irqaction);
-	if (ret) {
-		dev_err(&p->pdev->dev, "failed to request irq %d\n", irq);
-		goto err4;
+	cmt->channels = kzalloc(cmt->num_channels * sizeof(*cmt->channels),
+				GFP_KERNEL);
+	if (cmt->channels == NULL) {
+		ret = -ENOMEM;
+		goto err_unmap;
 	}
 
-	platform_set_drvdata(pdev, p);
+	if (cmt->legacy) {
+		ret = sh_cmt_setup_channel(&cmt->channels[0],
+					   cfg->timer_bit, cfg->timer_bit,
+					   cfg->clockevent_rating != 0,
+					   cfg->clocksource_rating != 0, cmt);
+		if (ret < 0)
+			goto err_unmap;
+	} else {
+		unsigned int mask = hw_channels;
+		unsigned int i;
+
+		/*
+		 * Use the first channel as a clock event device and the second
+		 * channel as a clock source. If only one channel is available
+		 * use it for both.
+		 */
+		for (i = 0; i < cmt->num_channels; ++i) {
+			unsigned int hwidx = ffs(mask) - 1;
+			bool clocksource = i == 1 || cmt->num_channels == 1;
+			bool clockevent = i == 0;
+
+			ret = sh_cmt_setup_channel(&cmt->channels[i], i, hwidx,
+						   clockevent, clocksource,
+						   cmt);
+			if (ret < 0)
+				goto err_unmap;
+
+			mask &= ~(1 << hwidx);
+		}
+	}
+
+	platform_set_drvdata(pdev, cmt);
 
 	return 0;
-err4:
-	clk_unprepare(p->clk);
-err3:
-	clk_put(p->clk);
-err2:
-	iounmap(p->mapbase_str);
-err1:
-	iounmap(p->mapbase);
-err0:
+
+err_unmap:
+	kfree(cmt->channels);
+	sh_cmt_unmap_memory(cmt);
+err_clk_unprepare:
+	clk_unprepare(cmt->clk);
+err_clk_put:
+	clk_put(cmt->clk);
 	return ret;
 }
 
 static int sh_cmt_probe(struct platform_device *pdev)
 {
-	struct sh_cmt_priv *p = platform_get_drvdata(pdev);
-	struct sh_timer_config *cfg = pdev->dev.platform_data;
+	struct sh_cmt_device *cmt = platform_get_drvdata(pdev);
 	int ret;
 
 	if (!is_early_platform_device(pdev)) {
@@ -818,20 +1100,18 @@ static int sh_cmt_probe(struct platform_device *pdev)
 		pm_runtime_enable(&pdev->dev);
 	}
 
-	if (p) {
+	if (cmt) {
 		dev_info(&pdev->dev, "kept as earlytimer\n");
 		goto out;
 	}
 
-	p = kmalloc(sizeof(*p), GFP_KERNEL);
-	if (p == NULL) {
-		dev_err(&pdev->dev, "failed to allocate driver data\n");
+	cmt = kzalloc(sizeof(*cmt), GFP_KERNEL);
+	if (cmt == NULL)
 		return -ENOMEM;
-	}
 
-	ret = sh_cmt_setup(p, pdev);
+	ret = sh_cmt_setup(cmt, pdev);
 	if (ret) {
-		kfree(p);
+		kfree(cmt);
 		pm_runtime_idle(&pdev->dev);
 		return ret;
 	}
@@ -839,7 +1119,7 @@ static int sh_cmt_probe(struct platform_device *pdev)
 		return 0;
 
  out:
-	if (cfg->clockevent_rating || cfg->clocksource_rating)
+	if (cmt->has_clockevent || cmt->has_clocksource)
 		pm_runtime_irq_safe(&pdev->dev);
 	else
 		pm_runtime_idle(&pdev->dev);
@@ -852,12 +1132,24 @@ static int sh_cmt_remove(struct platform_device *pdev)
 	return -EBUSY; /* cannot unregister clockevent and clocksource */
 }
 
+static const struct platform_device_id sh_cmt_id_table[] = {
+	{ "sh_cmt", 0 },
+	{ "sh-cmt-16", (kernel_ulong_t)&sh_cmt_info[SH_CMT_16BIT] },
+	{ "sh-cmt-32", (kernel_ulong_t)&sh_cmt_info[SH_CMT_32BIT] },
+	{ "sh-cmt-32-fast", (kernel_ulong_t)&sh_cmt_info[SH_CMT_32BIT_FAST] },
+	{ "sh-cmt-48", (kernel_ulong_t)&sh_cmt_info[SH_CMT_48BIT] },
+	{ "sh-cmt-48-gen2", (kernel_ulong_t)&sh_cmt_info[SH_CMT_48BIT_GEN2] },
+	{ }
+};
+MODULE_DEVICE_TABLE(platform, sh_cmt_id_table);
+
 static struct platform_driver sh_cmt_device_driver = {
 	.probe		= sh_cmt_probe,
 	.remove		= sh_cmt_remove,
 	.driver		= {
 		.name	= "sh_cmt",
-	}
+	},
+	.id_table	= sh_cmt_id_table,
 };
 
 static int __init sh_cmt_init(void)
diff --git a/drivers/clocksource/sh_mtu2.c b/drivers/clocksource/sh_mtu2.c
index e30d76e0a6f..188d4e092ef 100644
--- a/drivers/clocksource/sh_mtu2.c
+++ b/drivers/clocksource/sh_mtu2.c
@@ -11,37 +11,48 @@
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  */
 
+#include <linux/clk.h>
+#include <linux/clockchips.h>
+#include <linux/delay.h>
+#include <linux/err.h>
 #include <linux/init.h>
-#include <linux/platform_device.h>
-#include <linux/spinlock.h>
 #include <linux/interrupt.h>
-#include <linux/ioport.h>
-#include <linux/delay.h>
 #include <linux/io.h>
-#include <linux/clk.h>
+#include <linux/ioport.h>
 #include <linux/irq.h>
-#include <linux/err.h>
-#include <linux/clockchips.h>
-#include <linux/sh_timer.h>
-#include <linux/slab.h>
 #include <linux/module.h>
+#include <linux/platform_device.h>
 #include <linux/pm_domain.h>
 #include <linux/pm_runtime.h>
+#include <linux/sh_timer.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+struct sh_mtu2_device;
+
+struct sh_mtu2_channel {
+	struct sh_mtu2_device *mtu;
+	unsigned int index;
+
+	void __iomem *base;
+	int irq;
+
+	struct clock_event_device ced;
+};
+
+struct sh_mtu2_device {
+	struct platform_device *pdev;
 
-struct sh_mtu2_priv {
 	void __iomem *mapbase;
 	struct clk *clk;
-	struct irqaction irqaction;
-	struct platform_device *pdev;
-	unsigned long rate;
-	unsigned long periodic;
-	struct clock_event_device ced;
+
+	struct sh_mtu2_channel *channels;
+	unsigned int num_channels;
+
+	bool legacy;
+	bool has_clockevent;
 };
 
 static DEFINE_RAW_SPINLOCK(sh_mtu2_lock);
@@ -55,6 +66,88 @@ static DEFINE_RAW_SPINLOCK(sh_mtu2_lock);
 #define TCNT 5 /* channel register */
 #define TGR  6 /* channel register */
 
+#define TCR_CCLR_NONE		(0 << 5)
+#define TCR_CCLR_TGRA		(1 << 5)
+#define TCR_CCLR_TGRB		(2 << 5)
+#define TCR_CCLR_SYNC		(3 << 5)
+#define TCR_CCLR_TGRC		(5 << 5)
+#define TCR_CCLR_TGRD		(6 << 5)
+#define TCR_CCLR_MASK		(7 << 5)
+#define TCR_CKEG_RISING		(0 << 3)
+#define TCR_CKEG_FALLING	(1 << 3)
+#define TCR_CKEG_BOTH		(2 << 3)
+#define TCR_CKEG_MASK		(3 << 3)
+/* Values 4 to 7 are channel-dependent */
+#define TCR_TPSC_P1		(0 << 0)
+#define TCR_TPSC_P4		(1 << 0)
+#define TCR_TPSC_P16		(2 << 0)
+#define TCR_TPSC_P64		(3 << 0)
+#define TCR_TPSC_CH0_TCLKA	(4 << 0)
+#define TCR_TPSC_CH0_TCLKB	(5 << 0)
+#define TCR_TPSC_CH0_TCLKC	(6 << 0)
+#define TCR_TPSC_CH0_TCLKD	(7 << 0)
+#define TCR_TPSC_CH1_TCLKA	(4 << 0)
+#define TCR_TPSC_CH1_TCLKB	(5 << 0)
+#define TCR_TPSC_CH1_P256	(6 << 0)
+#define TCR_TPSC_CH1_TCNT2	(7 << 0)
+#define TCR_TPSC_CH2_TCLKA	(4 << 0)
+#define TCR_TPSC_CH2_TCLKB	(5 << 0)
+#define TCR_TPSC_CH2_TCLKC	(6 << 0)
+#define TCR_TPSC_CH2_P1024	(7 << 0)
+#define TCR_TPSC_CH34_P256	(4 << 0)
+#define TCR_TPSC_CH34_P1024	(5 << 0)
+#define TCR_TPSC_CH34_TCLKA	(6 << 0)
+#define TCR_TPSC_CH34_TCLKB	(7 << 0)
+#define TCR_TPSC_MASK		(7 << 0)
+
+#define TMDR_BFE		(1 << 6)
+#define TMDR_BFB		(1 << 5)
+#define TMDR_BFA		(1 << 4)
+#define TMDR_MD_NORMAL		(0 << 0)
+#define TMDR_MD_PWM_1		(2 << 0)
+#define TMDR_MD_PWM_2		(3 << 0)
+#define TMDR_MD_PHASE_1		(4 << 0)
+#define TMDR_MD_PHASE_2		(5 << 0)
+#define TMDR_MD_PHASE_3		(6 << 0)
+#define TMDR_MD_PHASE_4		(7 << 0)
+#define TMDR_MD_PWM_SYNC	(8 << 0)
+#define TMDR_MD_PWM_COMP_CREST	(13 << 0)
+#define TMDR_MD_PWM_COMP_TROUGH	(14 << 0)
+#define TMDR_MD_PWM_COMP_BOTH	(15 << 0)
+#define TMDR_MD_MASK		(15 << 0)
+
+#define TIOC_IOCH(n)		((n) << 4)
+#define TIOC_IOCL(n)		((n) << 0)
+#define TIOR_OC_RETAIN		(0 << 0)
+#define TIOR_OC_0_CLEAR		(1 << 0)
+#define TIOR_OC_0_SET		(2 << 0)
+#define TIOR_OC_0_TOGGLE	(3 << 0)
+#define TIOR_OC_1_CLEAR		(5 << 0)
+#define TIOR_OC_1_SET		(6 << 0)
+#define TIOR_OC_1_TOGGLE	(7 << 0)
+#define TIOR_IC_RISING		(8 << 0)
+#define TIOR_IC_FALLING		(9 << 0)
+#define TIOR_IC_BOTH		(10 << 0)
+#define TIOR_IC_TCNT		(12 << 0)
+#define TIOR_MASK		(15 << 0)
+
+#define TIER_TTGE		(1 << 7)
+#define TIER_TTGE2		(1 << 6)
+#define TIER_TCIEU		(1 << 5)
+#define TIER_TCIEV		(1 << 4)
+#define TIER_TGIED		(1 << 3)
+#define TIER_TGIEC		(1 << 2)
+#define TIER_TGIEB		(1 << 1)
+#define TIER_TGIEA		(1 << 0)
+
+#define TSR_TCFD		(1 << 7)
+#define TSR_TCFU		(1 << 5)
+#define TSR_TCFV		(1 << 4)
+#define TSR_TGFD		(1 << 3)
+#define TSR_TGFC		(1 << 2)
+#define TSR_TGFB		(1 << 1)
+#define TSR_TGFA		(1 << 0)
+
 static unsigned long mtu2_reg_offs[] = {
 	[TCR] = 0,
 	[TMDR] = 1,
@@ -65,135 +158,143 @@ static unsigned long mtu2_reg_offs[] = {
 	[TGR] = 8,
 };
 
-static inline unsigned long sh_mtu2_read(struct sh_mtu2_priv *p, int reg_nr)
+static inline unsigned long sh_mtu2_read(struct sh_mtu2_channel *ch, int reg_nr)
 {
-	struct sh_timer_config *cfg = p->pdev->dev.platform_data;
-	void __iomem *base = p->mapbase;
 	unsigned long offs;
 
-	if (reg_nr == TSTR)
-		return ioread8(base + cfg->channel_offset);
+	if (reg_nr == TSTR) {
+		if (ch->mtu->legacy)
+			return ioread8(ch->mtu->mapbase);
+		else
+			return ioread8(ch->mtu->mapbase + 0x280);
+	}
 
 	offs = mtu2_reg_offs[reg_nr];
 
 	if ((reg_nr == TCNT) || (reg_nr == TGR))
-		return ioread16(base + offs);
+		return ioread16(ch->base + offs);
 	else
-		return ioread8(base + offs);
+		return ioread8(ch->base + offs);
 }
 
-static inline void sh_mtu2_write(struct sh_mtu2_priv *p, int reg_nr,
+static inline void sh_mtu2_write(struct sh_mtu2_channel *ch, int reg_nr,
 				unsigned long value)
 {
-	struct sh_timer_config *cfg = p->pdev->dev.platform_data;
-	void __iomem *base = p->mapbase;
 	unsigned long offs;
 
 	if (reg_nr == TSTR) {
-		iowrite8(value, base + cfg->channel_offset);
-		return;
+		if (ch->mtu->legacy)
+			return iowrite8(value, ch->mtu->mapbase);
+		else
+			return iowrite8(value, ch->mtu->mapbase + 0x280);
 	}
 
 	offs = mtu2_reg_offs[reg_nr];
 
 	if ((reg_nr == TCNT) || (reg_nr == TGR))
-		iowrite16(value, base + offs);
+		iowrite16(value, ch->base + offs);
 	else
-		iowrite8(value, base + offs);
+		iowrite8(value, ch->base + offs);
 }
 
-static void sh_mtu2_start_stop_ch(struct sh_mtu2_priv *p, int start)
+static void sh_mtu2_start_stop_ch(struct sh_mtu2_channel *ch, int start)
 {
-	struct sh_timer_config *cfg = p->pdev->dev.platform_data;
 	unsigned long flags, value;
 
 	/* start stop register shared by multiple timer channels */
 	raw_spin_lock_irqsave(&sh_mtu2_lock, flags);
-	value = sh_mtu2_read(p, TSTR);
+	value = sh_mtu2_read(ch, TSTR);
 
 	if (start)
-		value |= 1 << cfg->timer_bit;
+		value |= 1 << ch->index;
 	else
-		value &= ~(1 << cfg->timer_bit);
+		value &= ~(1 << ch->index);
 
-	sh_mtu2_write(p, TSTR, value);
+	sh_mtu2_write(ch, TSTR, value);
 	raw_spin_unlock_irqrestore(&sh_mtu2_lock, flags);
 }
 
-static int sh_mtu2_enable(struct sh_mtu2_priv *p)
+static int sh_mtu2_enable(struct sh_mtu2_channel *ch)
 {
+	unsigned long periodic;
+	unsigned long rate;
 	int ret;
 
-	pm_runtime_get_sync(&p->pdev->dev);
-	dev_pm_syscore_device(&p->pdev->dev, true);
+	pm_runtime_get_sync(&ch->mtu->pdev->dev);
+	dev_pm_syscore_device(&ch->mtu->pdev->dev, true);
 
 	/* enable clock */
-	ret = clk_enable(p->clk);
+	ret = clk_enable(ch->mtu->clk);
 	if (ret) {
-		dev_err(&p->pdev->dev, "cannot enable clock\n");
+		dev_err(&ch->mtu->pdev->dev, "ch%u: cannot enable clock\n",
+			ch->index);
 		return ret;
 	}
 
 	/* make sure channel is disabled */
-	sh_mtu2_start_stop_ch(p, 0);
-
-	p->rate = clk_get_rate(p->clk) / 64;
-	p->periodic = (p->rate + HZ/2) / HZ;
-
-	/* "Periodic Counter Operation" */
-	sh_mtu2_write(p, TCR, 0x23); /* TGRA clear, divide clock by 64 */
-	sh_mtu2_write(p, TIOR, 0);
-	sh_mtu2_write(p, TGR, p->periodic);
-	sh_mtu2_write(p, TCNT, 0);
-	sh_mtu2_write(p, TMDR, 0);
-	sh_mtu2_write(p, TIER, 0x01);
+	sh_mtu2_start_stop_ch(ch, 0);
+
+	rate = clk_get_rate(ch->mtu->clk) / 64;
+	periodic = (rate + HZ/2) / HZ;
+
+	/*
+	 * "Periodic Counter Operation"
+	 * Clear on TGRA compare match, divide clock by 64.
+	 */
+	sh_mtu2_write(ch, TCR, TCR_CCLR_TGRA | TCR_TPSC_P64);
+	sh_mtu2_write(ch, TIOR, TIOC_IOCH(TIOR_OC_0_CLEAR) |
+		      TIOC_IOCL(TIOR_OC_0_CLEAR));
+	sh_mtu2_write(ch, TGR, periodic);
+	sh_mtu2_write(ch, TCNT, 0);
+	sh_mtu2_write(ch, TMDR, TMDR_MD_NORMAL);
+	sh_mtu2_write(ch, TIER, TIER_TGIEA);
 
 	/* enable channel */
-	sh_mtu2_start_stop_ch(p, 1);
+	sh_mtu2_start_stop_ch(ch, 1);
 
 	return 0;
 }
 
-static void sh_mtu2_disable(struct sh_mtu2_priv *p)
+static void sh_mtu2_disable(struct sh_mtu2_channel *ch)
 {
 	/* disable channel */
-	sh_mtu2_start_stop_ch(p, 0);
+	sh_mtu2_start_stop_ch(ch, 0);
 
 	/* stop clock */
-	clk_disable(p->clk);
+	clk_disable(ch->mtu->clk);
 
-	dev_pm_syscore_device(&p->pdev->dev, false);
-	pm_runtime_put(&p->pdev->dev);
+	dev_pm_syscore_device(&ch->mtu->pdev->dev, false);
+	pm_runtime_put(&ch->mtu->pdev->dev);
 }
 
 static irqreturn_t sh_mtu2_interrupt(int irq, void *dev_id)
 {
-	struct sh_mtu2_priv *p = dev_id;
+	struct sh_mtu2_channel *ch = dev_id;
 
 	/* acknowledge interrupt */
-	sh_mtu2_read(p, TSR);
-	sh_mtu2_write(p, TSR, 0xfe);
+	sh_mtu2_read(ch, TSR);
+	sh_mtu2_write(ch, TSR, ~TSR_TGFA);
 
 	/* notify clockevent layer */
-	p->ced.event_handler(&p->ced);
+	ch->ced.event_handler(&ch->ced);
 	return IRQ_HANDLED;
 }
 
-static struct sh_mtu2_priv *ced_to_sh_mtu2(struct clock_event_device *ced)
+static struct sh_mtu2_channel *ced_to_sh_mtu2(struct clock_event_device *ced)
 {
-	return container_of(ced, struct sh_mtu2_priv, ced);
+	return container_of(ced, struct sh_mtu2_channel, ced);
 }
 
 static void sh_mtu2_clock_event_mode(enum clock_event_mode mode,
 				    struct clock_event_device *ced)
 {
-	struct sh_mtu2_priv *p = ced_to_sh_mtu2(ced);
+	struct sh_mtu2_channel *ch = ced_to_sh_mtu2(ced);
 	int disabled = 0;
 
 	/* deal with old setting first */
 	switch (ced->mode) {
 	case CLOCK_EVT_MODE_PERIODIC:
-		sh_mtu2_disable(p);
+		sh_mtu2_disable(ch);
 		disabled = 1;
 		break;
 	default:
@@ -202,12 +303,13 @@ static void sh_mtu2_clock_event_mode(enum clock_event_mode mode,
 
 	switch (mode) {
 	case CLOCK_EVT_MODE_PERIODIC:
-		dev_info(&p->pdev->dev, "used for periodic clock events\n");
-		sh_mtu2_enable(p);
+		dev_info(&ch->mtu->pdev->dev,
+			 "ch%u: used for periodic clock events\n", ch->index);
+		sh_mtu2_enable(ch);
 		break;
 	case CLOCK_EVT_MODE_UNUSED:
 		if (!disabled)
-			sh_mtu2_disable(p);
+			sh_mtu2_disable(ch);
 		break;
 	case CLOCK_EVT_MODE_SHUTDOWN:
 	default:
@@ -217,125 +319,207 @@ static void sh_mtu2_clock_event_mode(enum clock_event_mode mode,
 
 static void sh_mtu2_clock_event_suspend(struct clock_event_device *ced)
 {
-	pm_genpd_syscore_poweroff(&ced_to_sh_mtu2(ced)->pdev->dev);
+	pm_genpd_syscore_poweroff(&ced_to_sh_mtu2(ced)->mtu->pdev->dev);
 }
 
 static void sh_mtu2_clock_event_resume(struct clock_event_device *ced)
 {
-	pm_genpd_syscore_poweron(&ced_to_sh_mtu2(ced)->pdev->dev);
+	pm_genpd_syscore_poweron(&ced_to_sh_mtu2(ced)->mtu->pdev->dev);
 }
 
-static void sh_mtu2_register_clockevent(struct sh_mtu2_priv *p,
-				       char *name, unsigned long rating)
+static void sh_mtu2_register_clockevent(struct sh_mtu2_channel *ch,
+					const char *name)
 {
-	struct clock_event_device *ced = &p->ced;
+	struct clock_event_device *ced = &ch->ced;
 	int ret;
 
-	memset(ced, 0, sizeof(*ced));
-
 	ced->name = name;
 	ced->features = CLOCK_EVT_FEAT_PERIODIC;
-	ced->rating = rating;
-	ced->cpumask = cpumask_of(0);
+	ced->rating = 200;
+	ced->cpumask = cpu_possible_mask;
 	ced->set_mode = sh_mtu2_clock_event_mode;
 	ced->suspend = sh_mtu2_clock_event_suspend;
 	ced->resume = sh_mtu2_clock_event_resume;
 
-	dev_info(&p->pdev->dev, "used for clock events\n");
+	dev_info(&ch->mtu->pdev->dev, "ch%u: used for clock events\n",
+		 ch->index);
 	clockevents_register_device(ced);
 
-	ret = setup_irq(p->irqaction.irq, &p->irqaction);
+	ret = request_irq(ch->irq, sh_mtu2_interrupt,
+			  IRQF_TIMER | IRQF_IRQPOLL | IRQF_NOBALANCING,
+			  dev_name(&ch->mtu->pdev->dev), ch);
 	if (ret) {
-		dev_err(&p->pdev->dev, "failed to request irq %d\n",
-			p->irqaction.irq);
+		dev_err(&ch->mtu->pdev->dev, "ch%u: failed to request irq %d\n",
+			ch->index, ch->irq);
 		return;
 	}
 }
 
-static int sh_mtu2_register(struct sh_mtu2_priv *p, char *name,
-			    unsigned long clockevent_rating)
+static int sh_mtu2_register(struct sh_mtu2_channel *ch, const char *name,
+			    bool clockevent)
 {
-	if (clockevent_rating)
-		sh_mtu2_register_clockevent(p, name, clockevent_rating);
+	if (clockevent) {
+		ch->mtu->has_clockevent = true;
+		sh_mtu2_register_clockevent(ch, name);
+	}
 
 	return 0;
 }
 
-static int sh_mtu2_setup(struct sh_mtu2_priv *p, struct platform_device *pdev)
+static int sh_mtu2_setup_channel(struct sh_mtu2_channel *ch, unsigned int index,
+				 struct sh_mtu2_device *mtu)
 {
-	struct sh_timer_config *cfg = pdev->dev.platform_data;
-	struct resource *res;
-	int irq, ret;
-	ret = -ENXIO;
+	static const unsigned int channel_offsets[] = {
+		0x300, 0x380, 0x000,
+	};
+	bool clockevent;
+
+	ch->mtu = mtu;
+
+	if (mtu->legacy) {
+		struct sh_timer_config *cfg = mtu->pdev->dev.platform_data;
+
+		clockevent = cfg->clockevent_rating != 0;
 
-	memset(p, 0, sizeof(*p));
-	p->pdev = pdev;
+		ch->irq = platform_get_irq(mtu->pdev, 0);
+		ch->base = mtu->mapbase - cfg->channel_offset;
+		ch->index = cfg->timer_bit;
+	} else {
+		char name[6];
 
-	if (!cfg) {
-		dev_err(&p->pdev->dev, "missing platform data\n");
-		goto err0;
+		clockevent = true;
+
+		sprintf(name, "tgi%ua", index);
+		ch->irq = platform_get_irq_byname(mtu->pdev, name);
+		ch->base = mtu->mapbase + channel_offsets[index];
+		ch->index = index;
 	}
 
-	platform_set_drvdata(pdev, p);
+	if (ch->irq < 0) {
+		/* Skip channels with no declared interrupt. */
+		if (!mtu->legacy)
+			return 0;
+
+		dev_err(&mtu->pdev->dev, "ch%u: failed to get irq\n",
+			ch->index);
+		return ch->irq;
+	}
+
+	return sh_mtu2_register(ch, dev_name(&mtu->pdev->dev), clockevent);
+}
 
-	res = platform_get_resource(p->pdev, IORESOURCE_MEM, 0);
+static int sh_mtu2_map_memory(struct sh_mtu2_device *mtu)
+{
+	struct resource *res;
+
+	res = platform_get_resource(mtu->pdev, IORESOURCE_MEM, 0);
 	if (!res) {
-		dev_err(&p->pdev->dev, "failed to get I/O memory\n");
-		goto err0;
+		dev_err(&mtu->pdev->dev, "failed to get I/O memory\n");
+		return -ENXIO;
+	}
+
+	mtu->mapbase = ioremap_nocache(res->start, resource_size(res));
+	if (mtu->mapbase == NULL)
+		return -ENXIO;
+
+	/*
+	 * In legacy platform device configuration (with one device per channel)
+	 * the resource points to the channel base address.
+	 */
+	if (mtu->legacy) {
+		struct sh_timer_config *cfg = mtu->pdev->dev.platform_data;
+		mtu->mapbase += cfg->channel_offset;
 	}
 
-	irq = platform_get_irq(p->pdev, 0);
-	if (irq < 0) {
-		dev_err(&p->pdev->dev, "failed to get irq\n");
-		goto err0;
+	return 0;
+}
+
+static void sh_mtu2_unmap_memory(struct sh_mtu2_device *mtu)
+{
+	if (mtu->legacy) {
+		struct sh_timer_config *cfg = mtu->pdev->dev.platform_data;
+		mtu->mapbase -= cfg->channel_offset;
 	}
 
-	/* map memory, let mapbase point to our channel */
-	p->mapbase = ioremap_nocache(res->start, resource_size(res));
-	if (p->mapbase == NULL) {
-		dev_err(&p->pdev->dev, "failed to remap I/O memory\n");
-		goto err0;
+	iounmap(mtu->mapbase);
+}
+
+static int sh_mtu2_setup(struct sh_mtu2_device *mtu,
+			 struct platform_device *pdev)
+{
+	struct sh_timer_config *cfg = pdev->dev.platform_data;
+	const struct platform_device_id *id = pdev->id_entry;
+	unsigned int i;
+	int ret;
+
+	mtu->pdev = pdev;
+	mtu->legacy = id->driver_data;
+
+	if (mtu->legacy && !cfg) {
+		dev_err(&mtu->pdev->dev, "missing platform data\n");
+		return -ENXIO;
 	}
 
-	/* setup data for setup_irq() (too early for request_irq()) */
-	p->irqaction.name = dev_name(&p->pdev->dev);
-	p->irqaction.handler = sh_mtu2_interrupt;
-	p->irqaction.dev_id = p;
-	p->irqaction.irq = irq;
-	p->irqaction.flags = IRQF_TIMER | IRQF_IRQPOLL | IRQF_NOBALANCING;
-
-	/* get hold of clock */
-	p->clk = clk_get(&p->pdev->dev, "mtu2_fck");
-	if (IS_ERR(p->clk)) {
-		dev_err(&p->pdev->dev, "cannot get clock\n");
-		ret = PTR_ERR(p->clk);
-		goto err1;
+	/* Get hold of clock. */
+	mtu->clk = clk_get(&mtu->pdev->dev, mtu->legacy ? "mtu2_fck" : "fck");
+	if (IS_ERR(mtu->clk)) {
+		dev_err(&mtu->pdev->dev, "cannot get clock\n");
+		return PTR_ERR(mtu->clk);
 	}
 
-	ret = clk_prepare(p->clk);
+	ret = clk_prepare(mtu->clk);
 	if (ret < 0)
-		goto err2;
+		goto err_clk_put;
 
-	ret = sh_mtu2_register(p, (char *)dev_name(&p->pdev->dev),
-			       cfg->clockevent_rating);
-	if (ret < 0)
-		goto err3;
+	/* Map the memory resource. */
+	ret = sh_mtu2_map_memory(mtu);
+	if (ret < 0) {
+		dev_err(&mtu->pdev->dev, "failed to remap I/O memory\n");
+		goto err_clk_unprepare;
+	}
+
+	/* Allocate and setup the channels. */
+	if (mtu->legacy)
+		mtu->num_channels = 1;
+	else
+		mtu->num_channels = 3;
+
+	mtu->channels = kzalloc(sizeof(*mtu->channels) * mtu->num_channels,
+				GFP_KERNEL);
+	if (mtu->channels == NULL) {
+		ret = -ENOMEM;
+		goto err_unmap;
+	}
+
+	if (mtu->legacy) {
+		ret = sh_mtu2_setup_channel(&mtu->channels[0], 0, mtu);
+		if (ret < 0)
+			goto err_unmap;
+	} else {
+		for (i = 0; i < mtu->num_channels; ++i) {
+			ret = sh_mtu2_setup_channel(&mtu->channels[i], i, mtu);
+			if (ret < 0)
+				goto err_unmap;
+		}
+	}
+
+	platform_set_drvdata(pdev, mtu);
 
 	return 0;
- err3:
-	clk_unprepare(p->clk);
- err2:
-	clk_put(p->clk);
- err1:
-	iounmap(p->mapbase);
- err0:
+
+err_unmap:
+	kfree(mtu->channels);
+	sh_mtu2_unmap_memory(mtu);
+err_clk_unprepare:
+	clk_unprepare(mtu->clk);
+err_clk_put:
+	clk_put(mtu->clk);
 	return ret;
 }
 
 static int sh_mtu2_probe(struct platform_device *pdev)
 {
-	struct sh_mtu2_priv *p = platform_get_drvdata(pdev);
-	struct sh_timer_config *cfg = pdev->dev.platform_data;
+	struct sh_mtu2_device *mtu = platform_get_drvdata(pdev);
 	int ret;
 
 	if (!is_early_platform_device(pdev)) {
@@ -343,20 +527,18 @@ static int sh_mtu2_probe(struct platform_device *pdev)
 		pm_runtime_enable(&pdev->dev);
 	}
 
-	if (p) {
+	if (mtu) {
 		dev_info(&pdev->dev, "kept as earlytimer\n");
 		goto out;
 	}
 
-	p = kmalloc(sizeof(*p), GFP_KERNEL);
-	if (p == NULL) {
-		dev_err(&pdev->dev, "failed to allocate driver data\n");
+	mtu = kzalloc(sizeof(*mtu), GFP_KERNEL);
+	if (mtu == NULL)
 		return -ENOMEM;
-	}
 
-	ret = sh_mtu2_setup(p, pdev);
+	ret = sh_mtu2_setup(mtu, pdev);
 	if (ret) {
-		kfree(p);
+		kfree(mtu);
 		pm_runtime_idle(&pdev->dev);
 		return ret;
 	}
@@ -364,7 +546,7 @@ static int sh_mtu2_probe(struct platform_device *pdev)
 		return 0;
 
  out:
-	if (cfg->clockevent_rating)
+	if (mtu->has_clockevent)
 		pm_runtime_irq_safe(&pdev->dev);
 	else
 		pm_runtime_idle(&pdev->dev);
@@ -377,12 +559,20 @@ static int sh_mtu2_remove(struct platform_device *pdev)
 	return -EBUSY; /* cannot unregister clockevent */
 }
 
+static const struct platform_device_id sh_mtu2_id_table[] = {
+	{ "sh_mtu2", 1 },
+	{ "sh-mtu2", 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, sh_mtu2_id_table);
+
 static struct platform_driver sh_mtu2_device_driver = {
 	.probe		= sh_mtu2_probe,
 	.remove		= sh_mtu2_remove,
 	.driver		= {
 		.name	= "sh_mtu2",
-	}
+	},
+	.id_table	= sh_mtu2_id_table,
 };
 
 static int __init sh_mtu2_init(void)
diff --git a/drivers/clocksource/sh_tmu.c b/drivers/clocksource/sh_tmu.c
index ecd7b60bfdf..6bd17a8f3dd 100644
--- a/drivers/clocksource/sh_tmu.c
+++ b/drivers/clocksource/sh_tmu.c
@@ -11,35 +11,41 @@
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  */
 
+#include <linux/clk.h>
+#include <linux/clockchips.h>
+#include <linux/clocksource.h>
+#include <linux/delay.h>
+#include <linux/err.h>
 #include <linux/init.h>
-#include <linux/platform_device.h>
-#include <linux/spinlock.h>
 #include <linux/interrupt.h>
-#include <linux/ioport.h>
-#include <linux/delay.h>
 #include <linux/io.h>
-#include <linux/clk.h>
+#include <linux/ioport.h>
 #include <linux/irq.h>
-#include <linux/err.h>
-#include <linux/clocksource.h>
-#include <linux/clockchips.h>
-#include <linux/sh_timer.h>
-#include <linux/slab.h>
 #include <linux/module.h>
+#include <linux/platform_device.h>
 #include <linux/pm_domain.h>
 #include <linux/pm_runtime.h>
+#include <linux/sh_timer.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+enum sh_tmu_model {
+	SH_TMU_LEGACY,
+	SH_TMU,
+	SH_TMU_SH3,
+};
+
+struct sh_tmu_device;
+
+struct sh_tmu_channel {
+	struct sh_tmu_device *tmu;
+	unsigned int index;
+
+	void __iomem *base;
+	int irq;
 
-struct sh_tmu_priv {
-	void __iomem *mapbase;
-	struct clk *clk;
-	struct irqaction irqaction;
-	struct platform_device *pdev;
 	unsigned long rate;
 	unsigned long periodic;
 	struct clock_event_device ced;
@@ -48,6 +54,21 @@ struct sh_tmu_priv {
 	unsigned int enable_count;
 };
 
+struct sh_tmu_device {
+	struct platform_device *pdev;
+
+	void __iomem *mapbase;
+	struct clk *clk;
+
+	enum sh_tmu_model model;
+
+	struct sh_tmu_channel *channels;
+	unsigned int num_channels;
+
+	bool has_clockevent;
+	bool has_clocksource;
+};
+
 static DEFINE_RAW_SPINLOCK(sh_tmu_lock);
 
 #define TSTR -1 /* shared register */
@@ -55,189 +76,208 @@ static DEFINE_RAW_SPINLOCK(sh_tmu_lock);
 #define TCNT 1 /* channel register */
 #define TCR 2 /* channel register */
 
-static inline unsigned long sh_tmu_read(struct sh_tmu_priv *p, int reg_nr)
+#define TCR_UNF			(1 << 8)
+#define TCR_UNIE		(1 << 5)
+#define TCR_TPSC_CLK4		(0 << 0)
+#define TCR_TPSC_CLK16		(1 << 0)
+#define TCR_TPSC_CLK64		(2 << 0)
+#define TCR_TPSC_CLK256		(3 << 0)
+#define TCR_TPSC_CLK1024	(4 << 0)
+#define TCR_TPSC_MASK		(7 << 0)
+
+static inline unsigned long sh_tmu_read(struct sh_tmu_channel *ch, int reg_nr)
 {
-	struct sh_timer_config *cfg = p->pdev->dev.platform_data;
-	void __iomem *base = p->mapbase;
 	unsigned long offs;
 
-	if (reg_nr == TSTR)
-		return ioread8(base - cfg->channel_offset);
+	if (reg_nr == TSTR) {
+		switch (ch->tmu->model) {
+		case SH_TMU_LEGACY:
+			return ioread8(ch->tmu->mapbase);
+		case SH_TMU_SH3:
+			return ioread8(ch->tmu->mapbase + 2);
+		case SH_TMU:
+			return ioread8(ch->tmu->mapbase + 4);
+		}
+	}
 
 	offs = reg_nr << 2;
 
 	if (reg_nr == TCR)
-		return ioread16(base + offs);
+		return ioread16(ch->base + offs);
 	else
-		return ioread32(base + offs);
+		return ioread32(ch->base + offs);
 }
 
-static inline void sh_tmu_write(struct sh_tmu_priv *p, int reg_nr,
+static inline void sh_tmu_write(struct sh_tmu_channel *ch, int reg_nr,
 				unsigned long value)
 {
-	struct sh_timer_config *cfg = p->pdev->dev.platform_data;
-	void __iomem *base = p->mapbase;
 	unsigned long offs;
 
 	if (reg_nr == TSTR) {
-		iowrite8(value, base - cfg->channel_offset);
-		return;
+		switch (ch->tmu->model) {
+		case SH_TMU_LEGACY:
+			return iowrite8(value, ch->tmu->mapbase);
+		case SH_TMU_SH3:
+			return iowrite8(value, ch->tmu->mapbase + 2);
+		case SH_TMU:
+			return iowrite8(value, ch->tmu->mapbase + 4);
+		}
 	}
 
 	offs = reg_nr << 2;
 
 	if (reg_nr == TCR)
-		iowrite16(value, base + offs);
+		iowrite16(value, ch->base + offs);
 	else
-		iowrite32(value, base + offs);
+		iowrite32(value, ch->base + offs);
 }
 
-static void sh_tmu_start_stop_ch(struct sh_tmu_priv *p, int start)
+static void sh_tmu_start_stop_ch(struct sh_tmu_channel *ch, int start)
 {
-	struct sh_timer_config *cfg = p->pdev->dev.platform_data;
 	unsigned long flags, value;
 
 	/* start stop register shared by multiple timer channels */
 	raw_spin_lock_irqsave(&sh_tmu_lock, flags);
-	value = sh_tmu_read(p, TSTR);
+	value = sh_tmu_read(ch, TSTR);
 
 	if (start)
-		value |= 1 << cfg->timer_bit;
+		value |= 1 << ch->index;
 	else
-		value &= ~(1 << cfg->timer_bit);
+		value &= ~(1 << ch->index);
 
-	sh_tmu_write(p, TSTR, value);
+	sh_tmu_write(ch, TSTR, value);
 	raw_spin_unlock_irqrestore(&sh_tmu_lock, flags);
 }
 
-static int __sh_tmu_enable(struct sh_tmu_priv *p)
+static int __sh_tmu_enable(struct sh_tmu_channel *ch)
 {
 	int ret;
 
 	/* enable clock */
-	ret = clk_enable(p->clk);
+	ret = clk_enable(ch->tmu->clk);
 	if (ret) {
-		dev_err(&p->pdev->dev, "cannot enable clock\n");
+		dev_err(&ch->tmu->pdev->dev, "ch%u: cannot enable clock\n",
+			ch->index);
 		return ret;
 	}
 
 	/* make sure channel is disabled */
-	sh_tmu_start_stop_ch(p, 0);
+	sh_tmu_start_stop_ch(ch, 0);
 
 	/* maximum timeout */
-	sh_tmu_write(p, TCOR, 0xffffffff);
-	sh_tmu_write(p, TCNT, 0xffffffff);
+	sh_tmu_write(ch, TCOR, 0xffffffff);
+	sh_tmu_write(ch, TCNT, 0xffffffff);
 
 	/* configure channel to parent clock / 4, irq off */
-	p->rate = clk_get_rate(p->clk) / 4;
-	sh_tmu_write(p, TCR, 0x0000);
+	ch->rate = clk_get_rate(ch->tmu->clk) / 4;
+	sh_tmu_write(ch, TCR, TCR_TPSC_CLK4);
 
 	/* enable channel */
-	sh_tmu_start_stop_ch(p, 1);
+	sh_tmu_start_stop_ch(ch, 1);
 
 	return 0;
 }
 
-static int sh_tmu_enable(struct sh_tmu_priv *p)
+static int sh_tmu_enable(struct sh_tmu_channel *ch)
 {
-	if (p->enable_count++ > 0)
+	if (ch->enable_count++ > 0)
 		return 0;
 
-	pm_runtime_get_sync(&p->pdev->dev);
-	dev_pm_syscore_device(&p->pdev->dev, true);
+	pm_runtime_get_sync(&ch->tmu->pdev->dev);
+	dev_pm_syscore_device(&ch->tmu->pdev->dev, true);
 
-	return __sh_tmu_enable(p);
+	return __sh_tmu_enable(ch);
 }
 
-static void __sh_tmu_disable(struct sh_tmu_priv *p)
+static void __sh_tmu_disable(struct sh_tmu_channel *ch)
 {
 	/* disable channel */
-	sh_tmu_start_stop_ch(p, 0);
+	sh_tmu_start_stop_ch(ch, 0);
 
 	/* disable interrupts in TMU block */
-	sh_tmu_write(p, TCR, 0x0000);
+	sh_tmu_write(ch, TCR, TCR_TPSC_CLK4);
 
 	/* stop clock */
-	clk_disable(p->clk);
+	clk_disable(ch->tmu->clk);
 }
 
-static void sh_tmu_disable(struct sh_tmu_priv *p)
+static void sh_tmu_disable(struct sh_tmu_channel *ch)
 {
-	if (WARN_ON(p->enable_count == 0))
+	if (WARN_ON(ch->enable_count == 0))
 		return;
 
-	if (--p->enable_count > 0)
+	if (--ch->enable_count > 0)
 		return;
 
-	__sh_tmu_disable(p);
+	__sh_tmu_disable(ch);
 
-	dev_pm_syscore_device(&p->pdev->dev, false);
-	pm_runtime_put(&p->pdev->dev);
+	dev_pm_syscore_device(&ch->tmu->pdev->dev, false);
+	pm_runtime_put(&ch->tmu->pdev->dev);
 }
 
-static void sh_tmu_set_next(struct sh_tmu_priv *p, unsigned long delta,
+static void sh_tmu_set_next(struct sh_tmu_channel *ch, unsigned long delta,
 			    int periodic)
 {
 	/* stop timer */
-	sh_tmu_start_stop_ch(p, 0);
+	sh_tmu_start_stop_ch(ch, 0);
 
 	/* acknowledge interrupt */
-	sh_tmu_read(p, TCR);
+	sh_tmu_read(ch, TCR);
 
 	/* enable interrupt */
-	sh_tmu_write(p, TCR, 0x0020);
+	sh_tmu_write(ch, TCR, TCR_UNIE | TCR_TPSC_CLK4);
 
 	/* reload delta value in case of periodic timer */
 	if (periodic)
-		sh_tmu_write(p, TCOR, delta);
+		sh_tmu_write(ch, TCOR, delta);
 	else
-		sh_tmu_write(p, TCOR, 0xffffffff);
+		sh_tmu_write(ch, TCOR, 0xffffffff);
 
-	sh_tmu_write(p, TCNT, delta);
+	sh_tmu_write(ch, TCNT, delta);
 
 	/* start timer */
-	sh_tmu_start_stop_ch(p, 1);
+	sh_tmu_start_stop_ch(ch, 1);
 }
 
 static irqreturn_t sh_tmu_interrupt(int irq, void *dev_id)
 {
-	struct sh_tmu_priv *p = dev_id;
+	struct sh_tmu_channel *ch = dev_id;
 
 	/* disable or acknowledge interrupt */
-	if (p->ced.mode == CLOCK_EVT_MODE_ONESHOT)
-		sh_tmu_write(p, TCR, 0x0000);
+	if (ch->ced.mode == CLOCK_EVT_MODE_ONESHOT)
+		sh_tmu_write(ch, TCR, TCR_TPSC_CLK4);
 	else
-		sh_tmu_write(p, TCR, 0x0020);
+		sh_tmu_write(ch, TCR, TCR_UNIE | TCR_TPSC_CLK4);
 
 	/* notify clockevent layer */
-	p->ced.event_handler(&p->ced);
+	ch->ced.event_handler(&ch->ced);
 	return IRQ_HANDLED;
 }
 
-static struct sh_tmu_priv *cs_to_sh_tmu(struct clocksource *cs)
+static struct sh_tmu_channel *cs_to_sh_tmu(struct clocksource *cs)
 {
-	return container_of(cs, struct sh_tmu_priv, cs);
+	return container_of(cs, struct sh_tmu_channel, cs);
 }
 
 static cycle_t sh_tmu_clocksource_read(struct clocksource *cs)
 {
-	struct sh_tmu_priv *p = cs_to_sh_tmu(cs);
+	struct sh_tmu_channel *ch = cs_to_sh_tmu(cs);
 
-	return sh_tmu_read(p, TCNT) ^ 0xffffffff;
+	return sh_tmu_read(ch, TCNT) ^ 0xffffffff;
 }
 
 static int sh_tmu_clocksource_enable(struct clocksource *cs)
 {
-	struct sh_tmu_priv *p = cs_to_sh_tmu(cs);
+	struct sh_tmu_channel *ch = cs_to_sh_tmu(cs);
 	int ret;
 
-	if (WARN_ON(p->cs_enabled))
+	if (WARN_ON(ch->cs_enabled))
 		return 0;
 
-	ret = sh_tmu_enable(p);
+	ret = sh_tmu_enable(ch);
 	if (!ret) {
-		__clocksource_updatefreq_hz(cs, p->rate);
-		p->cs_enabled = true;
+		__clocksource_updatefreq_hz(cs, ch->rate);
+		ch->cs_enabled = true;
 	}
 
 	return ret;
@@ -245,48 +285,48 @@ static int sh_tmu_clocksource_enable(struct clocksource *cs)
 
 static void sh_tmu_clocksource_disable(struct clocksource *cs)
 {
-	struct sh_tmu_priv *p = cs_to_sh_tmu(cs);
+	struct sh_tmu_channel *ch = cs_to_sh_tmu(cs);
 
-	if (WARN_ON(!p->cs_enabled))
+	if (WARN_ON(!ch->cs_enabled))
 		return;
 
-	sh_tmu_disable(p);
-	p->cs_enabled = false;
+	sh_tmu_disable(ch);
+	ch->cs_enabled = false;
 }
 
 static void sh_tmu_clocksource_suspend(struct clocksource *cs)
 {
-	struct sh_tmu_priv *p = cs_to_sh_tmu(cs);
+	struct sh_tmu_channel *ch = cs_to_sh_tmu(cs);
 
-	if (!p->cs_enabled)
+	if (!ch->cs_enabled)
 		return;
 
-	if (--p->enable_count == 0) {
-		__sh_tmu_disable(p);
-		pm_genpd_syscore_poweroff(&p->pdev->dev);
+	if (--ch->enable_count == 0) {
+		__sh_tmu_disable(ch);
+		pm_genpd_syscore_poweroff(&ch->tmu->pdev->dev);
 	}
 }
 
 static void sh_tmu_clocksource_resume(struct clocksource *cs)
 {
-	struct sh_tmu_priv *p = cs_to_sh_tmu(cs);
+	struct sh_tmu_channel *ch = cs_to_sh_tmu(cs);
 
-	if (!p->cs_enabled)
+	if (!ch->cs_enabled)
 		return;
 
-	if (p->enable_count++ == 0) {
-		pm_genpd_syscore_poweron(&p->pdev->dev);
-		__sh_tmu_enable(p);
+	if (ch->enable_count++ == 0) {
+		pm_genpd_syscore_poweron(&ch->tmu->pdev->dev);
+		__sh_tmu_enable(ch);
 	}
 }
 
-static int sh_tmu_register_clocksource(struct sh_tmu_priv *p,
-				       char *name, unsigned long rating)
+static int sh_tmu_register_clocksource(struct sh_tmu_channel *ch,
+				       const char *name)
 {
-	struct clocksource *cs = &p->cs;
+	struct clocksource *cs = &ch->cs;
 
 	cs->name = name;
-	cs->rating = rating;
+	cs->rating = 200;
 	cs->read = sh_tmu_clocksource_read;
 	cs->enable = sh_tmu_clocksource_enable;
 	cs->disable = sh_tmu_clocksource_disable;
@@ -295,43 +335,44 @@ static int sh_tmu_register_clocksource(struct sh_tmu_priv *p,
 	cs->mask = CLOCKSOURCE_MASK(32);
 	cs->flags = CLOCK_SOURCE_IS_CONTINUOUS;
 
-	dev_info(&p->pdev->dev, "used as clock source\n");
+	dev_info(&ch->tmu->pdev->dev, "ch%u: used as clock source\n",
+		 ch->index);
 
 	/* Register with dummy 1 Hz value, gets updated in ->enable() */
 	clocksource_register_hz(cs, 1);
 	return 0;
 }
 
-static struct sh_tmu_priv *ced_to_sh_tmu(struct clock_event_device *ced)
+static struct sh_tmu_channel *ced_to_sh_tmu(struct clock_event_device *ced)
 {
-	return container_of(ced, struct sh_tmu_priv, ced);
+	return container_of(ced, struct sh_tmu_channel, ced);
 }
 
-static void sh_tmu_clock_event_start(struct sh_tmu_priv *p, int periodic)
+static void sh_tmu_clock_event_start(struct sh_tmu_channel *ch, int periodic)
 {
-	struct clock_event_device *ced = &p->ced;
+	struct clock_event_device *ced = &ch->ced;
 
-	sh_tmu_enable(p);
+	sh_tmu_enable(ch);
 
-	clockevents_config(ced, p->rate);
+	clockevents_config(ced, ch->rate);
 
 	if (periodic) {
-		p->periodic = (p->rate + HZ/2) / HZ;
-		sh_tmu_set_next(p, p->periodic, 1);
+		ch->periodic = (ch->rate + HZ/2) / HZ;
+		sh_tmu_set_next(ch, ch->periodic, 1);
 	}
 }
 
 static void sh_tmu_clock_event_mode(enum clock_event_mode mode,
 				    struct clock_event_device *ced)
 {
-	struct sh_tmu_priv *p = ced_to_sh_tmu(ced);
+	struct sh_tmu_channel *ch = ced_to_sh_tmu(ced);
 	int disabled = 0;
 
 	/* deal with old setting first */
 	switch (ced->mode) {
 	case CLOCK_EVT_MODE_PERIODIC:
 	case CLOCK_EVT_MODE_ONESHOT:
-		sh_tmu_disable(p);
+		sh_tmu_disable(ch);
 		disabled = 1;
 		break;
 	default:
@@ -340,16 +381,18 @@ static void sh_tmu_clock_event_mode(enum clock_event_mode mode,
 
 	switch (mode) {
 	case CLOCK_EVT_MODE_PERIODIC:
-		dev_info(&p->pdev->dev, "used for periodic clock events\n");
-		sh_tmu_clock_event_start(p, 1);
+		dev_info(&ch->tmu->pdev->dev,
+			 "ch%u: used for periodic clock events\n", ch->index);
+		sh_tmu_clock_event_start(ch, 1);
 		break;
 	case CLOCK_EVT_MODE_ONESHOT:
-		dev_info(&p->pdev->dev, "used for oneshot clock events\n");
-		sh_tmu_clock_event_start(p, 0);
+		dev_info(&ch->tmu->pdev->dev,
+			 "ch%u: used for oneshot clock events\n", ch->index);
+		sh_tmu_clock_event_start(ch, 0);
 		break;
 	case CLOCK_EVT_MODE_UNUSED:
 		if (!disabled)
-			sh_tmu_disable(p);
+			sh_tmu_disable(ch);
 		break;
 	case CLOCK_EVT_MODE_SHUTDOWN:
 	default:
@@ -360,147 +403,234 @@ static void sh_tmu_clock_event_mode(enum clock_event_mode mode,
 static int sh_tmu_clock_event_next(unsigned long delta,
 				   struct clock_event_device *ced)
 {
-	struct sh_tmu_priv *p = ced_to_sh_tmu(ced);
+	struct sh_tmu_channel *ch = ced_to_sh_tmu(ced);
 
 	BUG_ON(ced->mode != CLOCK_EVT_MODE_ONESHOT);
 
 	/* program new delta value */
-	sh_tmu_set_next(p, delta, 0);
+	sh_tmu_set_next(ch, delta, 0);
 	return 0;
 }
 
 static void sh_tmu_clock_event_suspend(struct clock_event_device *ced)
 {
-	pm_genpd_syscore_poweroff(&ced_to_sh_tmu(ced)->pdev->dev);
+	pm_genpd_syscore_poweroff(&ced_to_sh_tmu(ced)->tmu->pdev->dev);
 }
 
 static void sh_tmu_clock_event_resume(struct clock_event_device *ced)
 {
-	pm_genpd_syscore_poweron(&ced_to_sh_tmu(ced)->pdev->dev);
+	pm_genpd_syscore_poweron(&ced_to_sh_tmu(ced)->tmu->pdev->dev);
 }
 
-static void sh_tmu_register_clockevent(struct sh_tmu_priv *p,
-				       char *name, unsigned long rating)
+static void sh_tmu_register_clockevent(struct sh_tmu_channel *ch,
+				       const char *name)
 {
-	struct clock_event_device *ced = &p->ced;
+	struct clock_event_device *ced = &ch->ced;
 	int ret;
 
-	memset(ced, 0, sizeof(*ced));
-
 	ced->name = name;
 	ced->features = CLOCK_EVT_FEAT_PERIODIC;
 	ced->features |= CLOCK_EVT_FEAT_ONESHOT;
-	ced->rating = rating;
+	ced->rating = 200;
 	ced->cpumask = cpumask_of(0);
 	ced->set_next_event = sh_tmu_clock_event_next;
 	ced->set_mode = sh_tmu_clock_event_mode;
 	ced->suspend = sh_tmu_clock_event_suspend;
 	ced->resume = sh_tmu_clock_event_resume;
 
-	dev_info(&p->pdev->dev, "used for clock events\n");
+	dev_info(&ch->tmu->pdev->dev, "ch%u: used for clock events\n",
+		 ch->index);
 
 	clockevents_config_and_register(ced, 1, 0x300, 0xffffffff);
 
-	ret = setup_irq(p->irqaction.irq, &p->irqaction);
+	ret = request_irq(ch->irq, sh_tmu_interrupt,
+			  IRQF_TIMER | IRQF_IRQPOLL | IRQF_NOBALANCING,
+			  dev_name(&ch->tmu->pdev->dev), ch);
 	if (ret) {
-		dev_err(&p->pdev->dev, "failed to request irq %d\n",
-			p->irqaction.irq);
+		dev_err(&ch->tmu->pdev->dev, "ch%u: failed to request irq %d\n",
+			ch->index, ch->irq);
 		return;
 	}
 }
 
-static int sh_tmu_register(struct sh_tmu_priv *p, char *name,
-		    unsigned long clockevent_rating,
-		    unsigned long clocksource_rating)
+static int sh_tmu_register(struct sh_tmu_channel *ch, const char *name,
+			   bool clockevent, bool clocksource)
 {
-	if (clockevent_rating)
-		sh_tmu_register_clockevent(p, name, clockevent_rating);
-	else if (clocksource_rating)
-		sh_tmu_register_clocksource(p, name, clocksource_rating);
+	if (clockevent) {
+		ch->tmu->has_clockevent = true;
+		sh_tmu_register_clockevent(ch, name);
+	} else if (clocksource) {
+		ch->tmu->has_clocksource = true;
+		sh_tmu_register_clocksource(ch, name);
+	}
 
 	return 0;
 }
 
-static int sh_tmu_setup(struct sh_tmu_priv *p, struct platform_device *pdev)
+static int sh_tmu_channel_setup(struct sh_tmu_channel *ch, unsigned int index,
+				bool clockevent, bool clocksource,
+				struct sh_tmu_device *tmu)
 {
-	struct sh_timer_config *cfg = pdev->dev.platform_data;
-	struct resource *res;
-	int irq, ret;
-	ret = -ENXIO;
+	/* Skip unused channels. */
+	if (!clockevent && !clocksource)
+		return 0;
 
-	memset(p, 0, sizeof(*p));
-	p->pdev = pdev;
+	ch->tmu = tmu;
+
+	if (tmu->model == SH_TMU_LEGACY) {
+		struct sh_timer_config *cfg = tmu->pdev->dev.platform_data;
+
+		/*
+		 * The SH3 variant (SH770x, SH7705, SH7710 and SH7720) maps
+		 * channel registers blocks at base + 2 + 12 * index, while all
+		 * other variants map them at base + 4 + 12 * index. We can
+		 * compute the index by just dividing by 12, the 2 bytes or 4
+		 * bytes offset being hidden by the integer division.
+		 */
+		ch->index = cfg->channel_offset / 12;
+		ch->base = tmu->mapbase + cfg->channel_offset;
+	} else {
+		ch->index = index;
+
+		if (tmu->model == SH_TMU_SH3)
+			ch->base = tmu->mapbase + 4 + ch->index * 12;
+		else
+			ch->base = tmu->mapbase + 8 + ch->index * 12;
+	}
 
-	if (!cfg) {
-		dev_err(&p->pdev->dev, "missing platform data\n");
-		goto err0;
+	ch->irq = platform_get_irq(tmu->pdev, index);
+	if (ch->irq < 0) {
+		dev_err(&tmu->pdev->dev, "ch%u: failed to get irq\n",
+			ch->index);
+		return ch->irq;
 	}
 
-	platform_set_drvdata(pdev, p);
+	ch->cs_enabled = false;
+	ch->enable_count = 0;
+
+	return sh_tmu_register(ch, dev_name(&tmu->pdev->dev),
+			       clockevent, clocksource);
+}
+
+static int sh_tmu_map_memory(struct sh_tmu_device *tmu)
+{
+	struct resource *res;
 
-	res = platform_get_resource(p->pdev, IORESOURCE_MEM, 0);
+	res = platform_get_resource(tmu->pdev, IORESOURCE_MEM, 0);
 	if (!res) {
-		dev_err(&p->pdev->dev, "failed to get I/O memory\n");
-		goto err0;
+		dev_err(&tmu->pdev->dev, "failed to get I/O memory\n");
+		return -ENXIO;
+	}
+
+	tmu->mapbase = ioremap_nocache(res->start, resource_size(res));
+	if (tmu->mapbase == NULL)
+		return -ENXIO;
+
+	/*
+	 * In legacy platform device configuration (with one device per channel)
+	 * the resource points to the channel base address.
+	 */
+	if (tmu->model == SH_TMU_LEGACY) {
+		struct sh_timer_config *cfg = tmu->pdev->dev.platform_data;
+		tmu->mapbase -= cfg->channel_offset;
 	}
 
-	irq = platform_get_irq(p->pdev, 0);
-	if (irq < 0) {
-		dev_err(&p->pdev->dev, "failed to get irq\n");
-		goto err0;
+	return 0;
+}
+
+static void sh_tmu_unmap_memory(struct sh_tmu_device *tmu)
+{
+	if (tmu->model == SH_TMU_LEGACY) {
+		struct sh_timer_config *cfg = tmu->pdev->dev.platform_data;
+		tmu->mapbase += cfg->channel_offset;
 	}
 
-	/* map memory, let mapbase point to our channel */
-	p->mapbase = ioremap_nocache(res->start, resource_size(res));
-	if (p->mapbase == NULL) {
-		dev_err(&p->pdev->dev, "failed to remap I/O memory\n");
-		goto err0;
+	iounmap(tmu->mapbase);
+}
+
+static int sh_tmu_setup(struct sh_tmu_device *tmu, struct platform_device *pdev)
+{
+	struct sh_timer_config *cfg = pdev->dev.platform_data;
+	const struct platform_device_id *id = pdev->id_entry;
+	unsigned int i;
+	int ret;
+
+	if (!cfg) {
+		dev_err(&tmu->pdev->dev, "missing platform data\n");
+		return -ENXIO;
 	}
 
-	/* setup data for setup_irq() (too early for request_irq()) */
-	p->irqaction.name = dev_name(&p->pdev->dev);
-	p->irqaction.handler = sh_tmu_interrupt;
-	p->irqaction.dev_id = p;
-	p->irqaction.irq = irq;
-	p->irqaction.flags = IRQF_TIMER | IRQF_IRQPOLL | IRQF_NOBALANCING;
-
-	/* get hold of clock */
-	p->clk = clk_get(&p->pdev->dev, "tmu_fck");
-	if (IS_ERR(p->clk)) {
-		dev_err(&p->pdev->dev, "cannot get clock\n");
-		ret = PTR_ERR(p->clk);
-		goto err1;
+	tmu->pdev = pdev;
+	tmu->model = id->driver_data;
+
+	/* Get hold of clock. */
+	tmu->clk = clk_get(&tmu->pdev->dev,
+			   tmu->model == SH_TMU_LEGACY ? "tmu_fck" : "fck");
+	if (IS_ERR(tmu->clk)) {
+		dev_err(&tmu->pdev->dev, "cannot get clock\n");
+		return PTR_ERR(tmu->clk);
 	}
 
-	ret = clk_prepare(p->clk);
+	ret = clk_prepare(tmu->clk);
 	if (ret < 0)
-		goto err2;
+		goto err_clk_put;
 
-	p->cs_enabled = false;
-	p->enable_count = 0;
+	/* Map the memory resource. */
+	ret = sh_tmu_map_memory(tmu);
+	if (ret < 0) {
+		dev_err(&tmu->pdev->dev, "failed to remap I/O memory\n");
+		goto err_clk_unprepare;
+	}
 
-	ret = sh_tmu_register(p, (char *)dev_name(&p->pdev->dev),
-			      cfg->clockevent_rating,
-			      cfg->clocksource_rating);
-	if (ret < 0)
-		goto err3;
+	/* Allocate and setup the channels. */
+	if (tmu->model == SH_TMU_LEGACY)
+		tmu->num_channels = 1;
+	else
+		tmu->num_channels = hweight8(cfg->channels_mask);
+
+	tmu->channels = kzalloc(sizeof(*tmu->channels) * tmu->num_channels,
+				GFP_KERNEL);
+	if (tmu->channels == NULL) {
+		ret = -ENOMEM;
+		goto err_unmap;
+	}
+
+	if (tmu->model == SH_TMU_LEGACY) {
+		ret = sh_tmu_channel_setup(&tmu->channels[0], 0,
+					   cfg->clockevent_rating != 0,
+					   cfg->clocksource_rating != 0, tmu);
+		if (ret < 0)
+			goto err_unmap;
+	} else {
+		/*
+		 * Use the first channel as a clock event device and the second
+		 * channel as a clock source.
+		 */
+		for (i = 0; i < tmu->num_channels; ++i) {
+			ret = sh_tmu_channel_setup(&tmu->channels[i], i,
+						   i == 0, i == 1, tmu);
+			if (ret < 0)
+				goto err_unmap;
+		}
+	}
+
+	platform_set_drvdata(pdev, tmu);
 
 	return 0;
 
- err3:
-	clk_unprepare(p->clk);
- err2:
-	clk_put(p->clk);
- err1:
-	iounmap(p->mapbase);
- err0:
+err_unmap:
+	kfree(tmu->channels);
+	sh_tmu_unmap_memory(tmu);
+err_clk_unprepare:
+	clk_unprepare(tmu->clk);
+err_clk_put:
+	clk_put(tmu->clk);
 	return ret;
 }
 
 static int sh_tmu_probe(struct platform_device *pdev)
 {
-	struct sh_tmu_priv *p = platform_get_drvdata(pdev);
-	struct sh_timer_config *cfg = pdev->dev.platform_data;
+	struct sh_tmu_device *tmu = platform_get_drvdata(pdev);
 	int ret;
 
 	if (!is_early_platform_device(pdev)) {
@@ -508,20 +638,18 @@ static int sh_tmu_probe(struct platform_device *pdev)
 		pm_runtime_enable(&pdev->dev);
 	}
 
-	if (p) {
+	if (tmu) {
 		dev_info(&pdev->dev, "kept as earlytimer\n");
 		goto out;
 	}
 
-	p = kmalloc(sizeof(*p), GFP_KERNEL);
-	if (p == NULL) {
-		dev_err(&pdev->dev, "failed to allocate driver data\n");
+	tmu = kzalloc(sizeof(*tmu), GFP_KERNEL);
+	if (tmu == NULL)
 		return -ENOMEM;
-	}
 
-	ret = sh_tmu_setup(p, pdev);
+	ret = sh_tmu_setup(tmu, pdev);
 	if (ret) {
-		kfree(p);
+		kfree(tmu);
 		pm_runtime_idle(&pdev->dev);
 		return ret;
 	}
@@ -529,7 +657,7 @@ static int sh_tmu_probe(struct platform_device *pdev)
 		return 0;
 
  out:
-	if (cfg->clockevent_rating || cfg->clocksource_rating)
+	if (tmu->has_clockevent || tmu->has_clocksource)
 		pm_runtime_irq_safe(&pdev->dev);
 	else
 		pm_runtime_idle(&pdev->dev);
@@ -542,12 +670,21 @@ static int sh_tmu_remove(struct platform_device *pdev)
 	return -EBUSY; /* cannot unregister clockevent and clocksource */
 }
 
+static const struct platform_device_id sh_tmu_id_table[] = {
+	{ "sh_tmu", SH_TMU_LEGACY },
+	{ "sh-tmu", SH_TMU },
+	{ "sh-tmu-sh3", SH_TMU_SH3 },
+	{ }
+};
+MODULE_DEVICE_TABLE(platform, sh_tmu_id_table);
+
 static struct platform_driver sh_tmu_device_driver = {
 	.probe		= sh_tmu_probe,
 	.remove		= sh_tmu_remove,
 	.driver		= {
 		.name	= "sh_tmu",
-	}
+	},
+	.id_table	= sh_tmu_id_table,
 };
 
 static int __init sh_tmu_init(void)
diff --git a/drivers/clocksource/tcb_clksrc.c b/drivers/clocksource/tcb_clksrc.c
index 00fdd117028..a8d7ea14f18 100644
--- a/drivers/clocksource/tcb_clksrc.c
+++ b/drivers/clocksource/tcb_clksrc.c
@@ -100,7 +100,7 @@ static void tc_mode(enum clock_event_mode m, struct clock_event_device *d)
 			|| tcd->clkevt.mode == CLOCK_EVT_MODE_ONESHOT) {
 		__raw_writel(0xff, regs + ATMEL_TC_REG(2, IDR));
 		__raw_writel(ATMEL_TC_CLKDIS, regs + ATMEL_TC_REG(2, CCR));
-		clk_disable_unprepare(tcd->clk);
+		clk_disable(tcd->clk);
 	}
 
 	switch (m) {
@@ -109,7 +109,7 @@ static void tc_mode(enum clock_event_mode m, struct clock_event_device *d)
 	 * of oneshot, we get lower overhead and improved accuracy.
 	 */
 	case CLOCK_EVT_MODE_PERIODIC:
-		clk_prepare_enable(tcd->clk);
+		clk_enable(tcd->clk);
 
 		/* slow clock, count up to RC, then irq and restart */
 		__raw_writel(timer_clock
@@ -126,7 +126,7 @@ static void tc_mode(enum clock_event_mode m, struct clock_event_device *d)
 		break;
 
 	case CLOCK_EVT_MODE_ONESHOT:
-		clk_prepare_enable(tcd->clk);
+		clk_enable(tcd->clk);
 
 		/* slow clock, count up to RC, then irq and stop */
 		__raw_writel(timer_clock | ATMEL_TC_CPCSTOP
@@ -194,7 +194,7 @@ static int __init setup_clkevents(struct atmel_tc *tc, int clk32k_divisor_idx)
 	ret = clk_prepare_enable(t2_clk);
 	if (ret)
 		return ret;
-	clk_disable_unprepare(t2_clk);
+	clk_disable(t2_clk);
 
 	clkevt.regs = tc->regs;
 	clkevt.clk = t2_clk;
diff --git a/drivers/clocksource/time-efm32.c b/drivers/clocksource/time-efm32.c
index 1a6205b7bed..bba62f9deef 100644
--- a/drivers/clocksource/time-efm32.c
+++ b/drivers/clocksource/time-efm32.c
@@ -272,4 +272,5 @@ static void __init efm32_timer_init(struct device_node *np)
 		}
 	}
 }
-CLOCKSOURCE_OF_DECLARE(efm32, "efm32,timer", efm32_timer_init);
+CLOCKSOURCE_OF_DECLARE(efm32compat, "efm32,timer", efm32_timer_init);
+CLOCKSOURCE_OF_DECLARE(efm32, "energymicro,efm32-timer", efm32_timer_init);
diff --git a/drivers/clocksource/timer-marco.c b/drivers/clocksource/timer-marco.c
index b52e1c078b9..dbd30398222 100644
--- a/drivers/clocksource/timer-marco.c
+++ b/drivers/clocksource/timer-marco.c
@@ -199,7 +199,7 @@ static int sirfsoc_local_timer_setup(struct clock_event_device *ce)
 
 	action->dev_id = ce;
 	BUG_ON(setup_irq(ce->irq, action));
-	irq_set_affinity(action->irq, cpumask_of(cpu));
+	irq_force_affinity(action->irq, cpumask_of(cpu));
 
 	clockevents_register_device(ce);
 	return 0;
@@ -252,15 +252,13 @@ static void __init sirfsoc_clockevent_init(void)
 }
 
 /* initialize the kernel jiffy timer source */
-static void __init sirfsoc_marco_timer_init(void)
+static void __init sirfsoc_marco_timer_init(struct device_node *np)
 {
 	unsigned long rate;
 	u32 timer_div;
 	struct clk *clk;
 
-	/* timer's input clock is io clock */
-	clk = clk_get_sys("io", NULL);
-
+	clk = of_clk_get(np, 0);
 	BUG_ON(IS_ERR(clk));
 	rate = clk_get_rate(clk);
 
@@ -303,6 +301,6 @@ static void __init sirfsoc_of_timer_init(struct device_node *np)
 	if (!sirfsoc_timer1_irq.irq)
 		panic("No irq passed for timer1 via DT\n");
 
-	sirfsoc_marco_timer_init();
+	sirfsoc_marco_timer_init(np);
 }
 CLOCKSOURCE_OF_DECLARE(sirfsoc_marco_timer, "sirf,marco-tick", sirfsoc_of_timer_init );
diff --git a/drivers/clocksource/timer-prima2.c b/drivers/clocksource/timer-prima2.c
index 1a6b2d6356d..a722aac7ac0 100644
--- a/drivers/clocksource/timer-prima2.c
+++ b/drivers/clocksource/timer-prima2.c
@@ -61,7 +61,8 @@ static irqreturn_t sirfsoc_timer_interrupt(int irq, void *dev_id)
 {
 	struct clock_event_device *ce = dev_id;
 
-	WARN_ON(!(readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_STATUS) & BIT(0)));
+	WARN_ON(!(readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_STATUS) &
+		BIT(0)));
 
 	/* clear timer0 interrupt */
 	writel_relaxed(BIT(0), sirfsoc_timer_base + SIRFSOC_TIMER_STATUS);
@@ -77,9 +78,11 @@ static cycle_t sirfsoc_timer_read(struct clocksource *cs)
 	u64 cycles;
 
 	/* latch the 64-bit timer counter */
-	writel_relaxed(SIRFSOC_TIMER_LATCH_BIT, sirfsoc_timer_base + SIRFSOC_TIMER_LATCH);
+	writel_relaxed(SIRFSOC_TIMER_LATCH_BIT,
+		sirfsoc_timer_base + SIRFSOC_TIMER_LATCH);
 	cycles = readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_LATCHED_HI);
-	cycles = (cycles << 32) | readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_LATCHED_LO);
+	cycles = (cycles << 32) |
+		readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_LATCHED_LO);
 
 	return cycles;
 }
@@ -89,11 +92,13 @@ static int sirfsoc_timer_set_next_event(unsigned long delta,
 {
 	unsigned long now, next;
 
-	writel_relaxed(SIRFSOC_TIMER_LATCH_BIT, sirfsoc_timer_base + SIRFSOC_TIMER_LATCH);
+	writel_relaxed(SIRFSOC_TIMER_LATCH_BIT,
+		sirfsoc_timer_base + SIRFSOC_TIMER_LATCH);
 	now = readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_LATCHED_LO);
 	next = now + delta;
 	writel_relaxed(next, sirfsoc_timer_base + SIRFSOC_TIMER_MATCH_0);
-	writel_relaxed(SIRFSOC_TIMER_LATCH_BIT, sirfsoc_timer_base + SIRFSOC_TIMER_LATCH);
+	writel_relaxed(SIRFSOC_TIMER_LATCH_BIT,
+		sirfsoc_timer_base + SIRFSOC_TIMER_LATCH);
 	now = readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_LATCHED_LO);
 
 	return next - now > delta ? -ETIME : 0;
@@ -108,10 +113,12 @@ static void sirfsoc_timer_set_mode(enum clock_event_mode mode,
 		WARN_ON(1);
 		break;
 	case CLOCK_EVT_MODE_ONESHOT:
-		writel_relaxed(val | BIT(0), sirfsoc_timer_base + SIRFSOC_TIMER_INT_EN);
+		writel_relaxed(val | BIT(0),
+			sirfsoc_timer_base + SIRFSOC_TIMER_INT_EN);
 		break;
 	case CLOCK_EVT_MODE_SHUTDOWN:
-		writel_relaxed(val & ~BIT(0), sirfsoc_timer_base + SIRFSOC_TIMER_INT_EN);
+		writel_relaxed(val & ~BIT(0),
+			sirfsoc_timer_base + SIRFSOC_TIMER_INT_EN);
 		break;
 	case CLOCK_EVT_MODE_UNUSED:
 	case CLOCK_EVT_MODE_RESUME:
@@ -123,10 +130,13 @@ static void sirfsoc_clocksource_suspend(struct clocksource *cs)
 {
 	int i;
 
-	writel_relaxed(SIRFSOC_TIMER_LATCH_BIT, sirfsoc_timer_base + SIRFSOC_TIMER_LATCH);
+	writel_relaxed(SIRFSOC_TIMER_LATCH_BIT,
+		sirfsoc_timer_base + SIRFSOC_TIMER_LATCH);
 
 	for (i = 0; i < SIRFSOC_TIMER_REG_CNT; i++)
-		sirfsoc_timer_reg_val[i] = readl_relaxed(sirfsoc_timer_base + sirfsoc_timer_reg_list[i]);
+		sirfsoc_timer_reg_val[i] =
+			readl_relaxed(sirfsoc_timer_base +
+				sirfsoc_timer_reg_list[i]);
 }
 
 static void sirfsoc_clocksource_resume(struct clocksource *cs)
@@ -134,10 +144,13 @@ static void sirfsoc_clocksource_resume(struct clocksource *cs)
 	int i;
 
 	for (i = 0; i < SIRFSOC_TIMER_REG_CNT - 2; i++)
-		writel_relaxed(sirfsoc_timer_reg_val[i], sirfsoc_timer_base + sirfsoc_timer_reg_list[i]);
+		writel_relaxed(sirfsoc_timer_reg_val[i],
+			sirfsoc_timer_base + sirfsoc_timer_reg_list[i]);
 
-	writel_relaxed(sirfsoc_timer_reg_val[SIRFSOC_TIMER_REG_CNT - 2], sirfsoc_timer_base + SIRFSOC_TIMER_COUNTER_LO);
-	writel_relaxed(sirfsoc_timer_reg_val[SIRFSOC_TIMER_REG_CNT - 1], sirfsoc_timer_base + SIRFSOC_TIMER_COUNTER_HI);
+	writel_relaxed(sirfsoc_timer_reg_val[SIRFSOC_TIMER_REG_CNT - 2],
+		sirfsoc_timer_base + SIRFSOC_TIMER_COUNTER_LO);
+	writel_relaxed(sirfsoc_timer_reg_val[SIRFSOC_TIMER_REG_CNT - 1],
+		sirfsoc_timer_base + SIRFSOC_TIMER_COUNTER_HI);
 }
 
 static struct clock_event_device sirfsoc_clockevent = {
@@ -185,11 +198,8 @@ static void __init sirfsoc_prima2_timer_init(struct device_node *np)
 	unsigned long rate;
 	struct clk *clk;
 
-	/* timer's input clock is io clock */
-	clk = clk_get_sys("io", NULL);
-
+	clk = of_clk_get(np, 0);
 	BUG_ON(IS_ERR(clk));
-
 	rate = clk_get_rate(clk);
 
 	BUG_ON(rate < PRIMA2_CLOCK_FREQ);
@@ -202,7 +212,7 @@ static void __init sirfsoc_prima2_timer_init(struct device_node *np)
 	sirfsoc_timer_irq.irq = irq_of_parse_and_map(np, 0);
 
 	writel_relaxed(rate / PRIMA2_CLOCK_FREQ / 2 - 1,
-		       sirfsoc_timer_base + SIRFSOC_TIMER_DIV);
+		sirfsoc_timer_base + SIRFSOC_TIMER_DIV);
 	writel_relaxed(0, sirfsoc_timer_base + SIRFSOC_TIMER_COUNTER_LO);
 	writel_relaxed(0, sirfsoc_timer_base + SIRFSOC_TIMER_COUNTER_HI);
 	writel_relaxed(BIT(0), sirfsoc_timer_base + SIRFSOC_TIMER_STATUS);
@@ -216,4 +226,5 @@ static void __init sirfsoc_prima2_timer_init(struct device_node *np)
 
 	sirfsoc_clockevent_init();
 }
-CLOCKSOURCE_OF_DECLARE(sirfsoc_prima2_timer, "sirf,prima2-tick", sirfsoc_prima2_timer_init);
+CLOCKSOURCE_OF_DECLARE(sirfsoc_prima2_timer,
+	"sirf,prima2-tick", sirfsoc_prima2_timer_init);
diff --git a/drivers/clocksource/timer-sun5i.c b/drivers/clocksource/timer-sun5i.c
index deebcd6469f..02268448dc8 100644
--- a/drivers/clocksource/timer-sun5i.c
+++ b/drivers/clocksource/timer-sun5i.c
@@ -16,6 +16,7 @@
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/irqreturn.h>
+#include <linux/reset.h>
 #include <linux/sched_clock.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
@@ -143,6 +144,7 @@ static u64 sun5i_timer_sched_read(void)
 
 static void __init sun5i_timer_init(struct device_node *node)
 {
+	struct reset_control *rstc;
 	unsigned long rate;
 	struct clk *clk;
 	int ret, irq;
@@ -162,6 +164,10 @@ static void __init sun5i_timer_init(struct device_node *node)
 	clk_prepare_enable(clk);
 	rate = clk_get_rate(clk);
 
+	rstc = of_reset_control_get(node, NULL);
+	if (!IS_ERR(rstc))
+		reset_control_deassert(rstc);
+
 	writel(~0, timer_base + TIMER_INTVAL_LO_REG(1));
 	writel(TIMER_CTL_ENABLE | TIMER_CTL_RELOAD,
 	       timer_base + TIMER_CTL_REG(1));
diff --git a/drivers/clocksource/versatile.c b/drivers/clocksource/versatile.c
new file mode 100644
index 00000000000..2798e749223
--- /dev/null
+++ b/drivers/clocksource/versatile.c
@@ -0,0 +1,40 @@
+/*
+ * 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.
+ *
+ * Copyright (C) 2014 ARM Limited
+ */
+
+#include <linux/clocksource.h>
+#include <linux/io.h>
+#include <linux/of_address.h>
+#include <linux/sched_clock.h>
+
+#define SYS_24MHZ 0x05c
+
+static void __iomem *versatile_sys_24mhz;
+
+static u64 notrace versatile_sys_24mhz_read(void)
+{
+	return readl(versatile_sys_24mhz);
+}
+
+static void __init versatile_sched_clock_init(struct device_node *node)
+{
+	void __iomem *base = of_iomap(node, 0);
+
+	if (!base)
+		return;
+
+	versatile_sys_24mhz = base + SYS_24MHZ;
+
+	sched_clock_register(versatile_sys_24mhz_read, 32, 24000000);
+}
+CLOCKSOURCE_OF_DECLARE(versatile, "arm,vexpress-sysreg",
+		       versatile_sched_clock_init);
diff --git a/drivers/connector/connector.c b/drivers/connector/connector.c
index b14f1d36f89..f612d68629d 100644
--- a/drivers/connector/connector.c
+++ b/drivers/connector/connector.c
@@ -43,6 +43,8 @@ static struct cn_dev cdev;
 static int cn_already_initialized;
 
 /*
+ * Sends mult (multiple) cn_msg at a time.
+ *
  * msg->seq and msg->ack are used to determine message genealogy.
  * When someone sends message it puts there locally unique sequence
  * and random acknowledge numbers.  Sequence number may be copied into
@@ -62,10 +64,13 @@ static int cn_already_initialized;
  * the acknowledgement number in the original message + 1, then it is
  * a new message.
  *
+ * If msg->len != len, then additional cn_msg messages are expected following
+ * the first msg.
+ *
  * The message is sent to, the portid if given, the group if given, both if
  * both, or if both are zero then the group is looked up and sent there.
  */
-int cn_netlink_send(struct cn_msg *msg, u32 portid, u32 __group,
+int cn_netlink_send_mult(struct cn_msg *msg, u16 len, u32 portid, u32 __group,
 	gfp_t gfp_mask)
 {
 	struct cn_callback_entry *__cbq;
@@ -98,7 +103,7 @@ int cn_netlink_send(struct cn_msg *msg, u32 portid, u32 __group,
 	if (!portid && !netlink_has_listeners(dev->nls, group))
 		return -ESRCH;
 
-	size = sizeof(*msg) + msg->len;
+	size = sizeof(*msg) + len;
 
 	skb = nlmsg_new(size, gfp_mask);
 	if (!skb)
@@ -121,6 +126,14 @@ int cn_netlink_send(struct cn_msg *msg, u32 portid, u32 __group,
 					 gfp_mask);
 	return netlink_unicast(dev->nls, skb, portid, !(gfp_mask&__GFP_WAIT));
 }
+EXPORT_SYMBOL_GPL(cn_netlink_send_mult);
+
+/* same as cn_netlink_send_mult except msg->len is used for len */
+int cn_netlink_send(struct cn_msg *msg, u32 portid, u32 __group,
+	gfp_t gfp_mask)
+{
+	return cn_netlink_send_mult(msg, msg->len, portid, __group, gfp_mask);
+}
 EXPORT_SYMBOL_GPL(cn_netlink_send);
 
 /*
diff --git a/drivers/cpufreq/Kconfig.arm b/drivers/cpufreq/Kconfig.arm
index 580503513f0..36d20d0fce2 100644
--- a/drivers/cpufreq/Kconfig.arm
+++ b/drivers/cpufreq/Kconfig.arm
@@ -5,7 +5,8 @@
 # big LITTLE core layer and glue drivers
 config ARM_BIG_LITTLE_CPUFREQ
 	tristate "Generic ARM big LITTLE CPUfreq driver"
-	depends on ARM && BIG_LITTLE && ARM_CPU_TOPOLOGY && HAVE_CLK
+	depends on (BIG_LITTLE && ARM_CPU_TOPOLOGY) || (ARM64 && SMP)
+	depends on HAVE_CLK
 	select PM_OPP
 	help
 	  This enables the Generic CPUfreq driver for ARM big.LITTLE platforms.
@@ -30,7 +31,7 @@ config ARM_EXYNOS_CPUFREQ
 
 config ARM_EXYNOS4210_CPUFREQ
 	bool "SAMSUNG EXYNOS4210"
-	depends on CPU_EXYNOS4210 && !ARCH_MULTIPLATFORM
+	depends on CPU_EXYNOS4210
 	default y
 	select ARM_EXYNOS_CPUFREQ
 	help
@@ -41,7 +42,7 @@ config ARM_EXYNOS4210_CPUFREQ
 
 config ARM_EXYNOS4X12_CPUFREQ
 	bool "SAMSUNG EXYNOS4x12"
-	depends on (SOC_EXYNOS4212 || SOC_EXYNOS4412) && !ARCH_MULTIPLATFORM
+	depends on SOC_EXYNOS4212 || SOC_EXYNOS4412
 	default y
 	select ARM_EXYNOS_CPUFREQ
 	help
@@ -52,7 +53,7 @@ config ARM_EXYNOS4X12_CPUFREQ
 
 config ARM_EXYNOS5250_CPUFREQ
 	bool "SAMSUNG EXYNOS5250"
-	depends on SOC_EXYNOS5250 && !ARCH_MULTIPLATFORM
+	depends on SOC_EXYNOS5250
 	default y
 	select ARM_EXYNOS_CPUFREQ
 	help
@@ -85,7 +86,7 @@ config ARM_EXYNOS_CPU_FREQ_BOOST_SW
 	  It allows usage of special frequencies for Samsung Exynos
 	  processors if thermal conditions are appropriate.
 
-	  It reguires, for safe operation, thermal framework with properly
+	  It requires, for safe operation, thermal framework with properly
 	  defined trip points.
 
 	  If in doubt, say N.
@@ -186,7 +187,7 @@ config ARM_S3C2416_CPUFREQ
 	  S3C2450 SoC. The S3C2416 supports changing the rate of the
 	  armdiv clock source and also entering a so called dynamic
 	  voltage scaling mode in which it is possible to reduce the
-	  core voltage of the cpu.
+	  core voltage of the CPU.
 
 	  If in doubt, say N.
 
diff --git a/drivers/cpufreq/Kconfig.x86 b/drivers/cpufreq/Kconfig.x86
index d369349eeaa..89ae88f9189 100644
--- a/drivers/cpufreq/Kconfig.x86
+++ b/drivers/cpufreq/Kconfig.x86
@@ -10,7 +10,7 @@ config X86_INTEL_PSTATE
 	  The driver implements an internal governor and will become
           the scaling driver and governor for Sandy bridge processors.
 
-	  When this driver is enabled it will become the perferred
+	  When this driver is enabled it will become the preferred
           scaling driver for Sandy bridge processors.
 
 	  If in doubt, say N.
@@ -52,7 +52,7 @@ config X86_ACPI_CPUFREQ_CPB
 	help
 	  The powernow-k8 driver used to provide a sysfs knob called "cpb"
 	  to disable the Core Performance Boosting feature of AMD CPUs. This
-	  file has now been superseeded by the more generic "boost" entry.
+	  file has now been superseded by the more generic "boost" entry.
 
 	  By enabling this option the acpi_cpufreq driver provides the old
 	  entry in addition to the new boost ones, for compatibility reasons.
diff --git a/drivers/cpufreq/Makefile b/drivers/cpufreq/Makefile
index 0dbb963c1ae..738c8b7b17d 100644
--- a/drivers/cpufreq/Makefile
+++ b/drivers/cpufreq/Makefile
@@ -1,5 +1,7 @@
 # CPUfreq core
 obj-$(CONFIG_CPU_FREQ)			+= cpufreq.o freq_table.o
+obj-$(CONFIG_PM_OPP)			+= cpufreq_opp.o
+
 # CPUfreq stats
 obj-$(CONFIG_CPU_FREQ_STAT)             += cpufreq_stats.o
 
diff --git a/drivers/cpufreq/acpi-cpufreq.c b/drivers/cpufreq/acpi-cpufreq.c
index 000e4e0afd7..b0c18ed8d83 100644
--- a/drivers/cpufreq/acpi-cpufreq.c
+++ b/drivers/cpufreq/acpi-cpufreq.c
@@ -213,7 +213,7 @@ static unsigned extract_io(u32 value, struct acpi_cpufreq_data *data)
 
 static unsigned extract_msr(u32 msr, struct acpi_cpufreq_data *data)
 {
-	int i;
+	struct cpufreq_frequency_table *pos;
 	struct acpi_processor_performance *perf;
 
 	if (boot_cpu_data.x86_vendor == X86_VENDOR_AMD)
@@ -223,10 +223,9 @@ static unsigned extract_msr(u32 msr, struct acpi_cpufreq_data *data)
 
 	perf = data->acpi_data;
 
-	for (i = 0; data->freq_table[i].frequency != CPUFREQ_TABLE_END; i++) {
-		if (msr == perf->states[data->freq_table[i].driver_data].status)
-			return data->freq_table[i].frequency;
-	}
+	cpufreq_for_each_entry(pos, data->freq_table)
+		if (msr == perf->states[pos->driver_data].status)
+			return pos->frequency;
 	return data->freq_table[0].frequency;
 }
 
diff --git a/drivers/cpufreq/arm_big_little.c b/drivers/cpufreq/arm_big_little.c
index bad2ed317ba..1f4d4e31505 100644
--- a/drivers/cpufreq/arm_big_little.c
+++ b/drivers/cpufreq/arm_big_little.c
@@ -226,22 +226,22 @@ static inline u32 get_table_count(struct cpufreq_frequency_table *table)
 /* get the minimum frequency in the cpufreq_frequency_table */
 static inline u32 get_table_min(struct cpufreq_frequency_table *table)
 {
-	int i;
+	struct cpufreq_frequency_table *pos;
 	uint32_t min_freq = ~0;
-	for (i = 0; (table[i].frequency != CPUFREQ_TABLE_END); i++)
-		if (table[i].frequency < min_freq)
-			min_freq = table[i].frequency;
+	cpufreq_for_each_entry(pos, table)
+		if (pos->frequency < min_freq)
+			min_freq = pos->frequency;
 	return min_freq;
 }
 
 /* get the maximum frequency in the cpufreq_frequency_table */
 static inline u32 get_table_max(struct cpufreq_frequency_table *table)
 {
-	int i;
+	struct cpufreq_frequency_table *pos;
 	uint32_t max_freq = 0;
-	for (i = 0; (table[i].frequency != CPUFREQ_TABLE_END); i++)
-		if (table[i].frequency > max_freq)
-			max_freq = table[i].frequency;
+	cpufreq_for_each_entry(pos, table)
+		if (pos->frequency > max_freq)
+			max_freq = pos->frequency;
 	return max_freq;
 }
 
diff --git a/drivers/cpufreq/cpufreq-cpu0.c b/drivers/cpufreq/cpufreq-cpu0.c
index 1bf6bbac3e0..09b9129c7bd 100644
--- a/drivers/cpufreq/cpufreq-cpu0.c
+++ b/drivers/cpufreq/cpufreq-cpu0.c
@@ -130,7 +130,7 @@ static int cpu0_cpufreq_probe(struct platform_device *pdev)
 		return -ENOENT;
 	}
 
-	cpu_reg = devm_regulator_get_optional(cpu_dev, "cpu0");
+	cpu_reg = regulator_get_optional(cpu_dev, "cpu0");
 	if (IS_ERR(cpu_reg)) {
 		/*
 		 * If cpu0 regulator supply node is present, but regulator is
@@ -145,23 +145,23 @@ static int cpu0_cpufreq_probe(struct platform_device *pdev)
 			PTR_ERR(cpu_reg));
 	}
 
-	cpu_clk = devm_clk_get(cpu_dev, NULL);
+	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_node;
+		goto out_put_reg;
 	}
 
 	ret = of_init_opp_table(cpu_dev);
 	if (ret) {
 		pr_err("failed to init OPP table: %d\n", ret);
-		goto out_put_node;
+		goto out_put_clk;
 	}
 
 	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_node;
+		goto out_put_clk;
 	}
 
 	of_property_read_u32(np, "voltage-tolerance", &voltage_tolerance);
@@ -216,6 +216,12 @@ static int cpu0_cpufreq_probe(struct platform_device *pdev)
 
 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;
diff --git a/drivers/cpufreq/cpufreq-nforce2.c b/drivers/cpufreq/cpufreq-nforce2.c
index bc447b9003c..a2258090b58 100644
--- a/drivers/cpufreq/cpufreq-nforce2.c
+++ b/drivers/cpufreq/cpufreq-nforce2.c
@@ -379,7 +379,7 @@ static struct cpufreq_driver nforce2_driver = {
 };
 
 #ifdef MODULE
-static DEFINE_PCI_DEVICE_TABLE(nforce2_ids) = {
+static const struct pci_device_id nforce2_ids[] = {
 	{ PCI_VENDOR_ID_NVIDIA, PCI_DEVICE_ID_NVIDIA_NFORCE2 },
 	{}
 };
diff --git a/drivers/cpufreq/cpufreq.c b/drivers/cpufreq/cpufreq.c
index abda6609d3e..ae11dd51f81 100644
--- a/drivers/cpufreq/cpufreq.c
+++ b/drivers/cpufreq/cpufreq.c
@@ -354,6 +354,18 @@ static void cpufreq_notify_post_transition(struct cpufreq_policy *policy,
 void cpufreq_freq_transition_begin(struct cpufreq_policy *policy,
 		struct cpufreq_freqs *freqs)
 {
+
+	/*
+	 * Catch double invocations of _begin() which lead to self-deadlock.
+	 * ASYNC_NOTIFICATION drivers are left out because the cpufreq core
+	 * doesn't invoke _begin() on their behalf, and hence the chances of
+	 * double invocations are very low. Moreover, there are scenarios
+	 * where these checks can emit false-positive warnings in these
+	 * drivers; so we avoid that by skipping them altogether.
+	 */
+	WARN_ON(!(cpufreq_driver->flags & CPUFREQ_ASYNC_NOTIFICATION)
+				&& current == policy->transition_task);
+
 wait:
 	wait_event(policy->transition_wait, !policy->transition_ongoing);
 
@@ -365,6 +377,7 @@ wait:
 	}
 
 	policy->transition_ongoing = true;
+	policy->transition_task = current;
 
 	spin_unlock(&policy->transition_lock);
 
@@ -381,6 +394,7 @@ void cpufreq_freq_transition_end(struct cpufreq_policy *policy,
 	cpufreq_notify_post_transition(policy, freqs, transition_failed);
 
 	policy->transition_ongoing = false;
+	policy->transition_task = NULL;
 
 	wake_up(&policy->transition_wait);
 }
@@ -1802,12 +1816,43 @@ EXPORT_SYMBOL(cpufreq_unregister_notifier);
  *                              GOVERNORS                            *
  *********************************************************************/
 
+static int __target_index(struct cpufreq_policy *policy,
+			  struct cpufreq_frequency_table *freq_table, int index)
+{
+	struct cpufreq_freqs freqs;
+	int retval = -EINVAL;
+	bool notify;
+
+	notify = !(cpufreq_driver->flags & CPUFREQ_ASYNC_NOTIFICATION);
+
+	if (notify) {
+		freqs.old = policy->cur;
+		freqs.new = freq_table[index].frequency;
+		freqs.flags = 0;
+
+		pr_debug("%s: cpu: %d, oldfreq: %u, new freq: %u\n",
+			 __func__, policy->cpu, freqs.old, freqs.new);
+
+		cpufreq_freq_transition_begin(policy, &freqs);
+	}
+
+	retval = cpufreq_driver->target_index(policy, index);
+	if (retval)
+		pr_err("%s: Failed to change cpu frequency: %d\n", __func__,
+		       retval);
+
+	if (notify)
+		cpufreq_freq_transition_end(policy, &freqs, retval);
+
+	return retval;
+}
+
 int __cpufreq_driver_target(struct cpufreq_policy *policy,
 			    unsigned int target_freq,
 			    unsigned int relation)
 {
-	int retval = -EINVAL;
 	unsigned int old_target_freq = target_freq;
+	int retval = -EINVAL;
 
 	if (cpufreq_disabled())
 		return -ENODEV;
@@ -1834,8 +1879,6 @@ int __cpufreq_driver_target(struct cpufreq_policy *policy,
 		retval = cpufreq_driver->target(policy, target_freq, relation);
 	else if (cpufreq_driver->target_index) {
 		struct cpufreq_frequency_table *freq_table;
-		struct cpufreq_freqs freqs;
-		bool notify;
 		int index;
 
 		freq_table = cpufreq_frequency_get_table(policy->cpu);
@@ -1856,26 +1899,7 @@ int __cpufreq_driver_target(struct cpufreq_policy *policy,
 			goto out;
 		}
 
-		notify = !(cpufreq_driver->flags & CPUFREQ_ASYNC_NOTIFICATION);
-
-		if (notify) {
-			freqs.old = policy->cur;
-			freqs.new = freq_table[index].frequency;
-			freqs.flags = 0;
-
-			pr_debug("%s: cpu: %d, oldfreq: %u, new freq: %u\n",
-				 __func__, policy->cpu, freqs.old, freqs.new);
-
-			cpufreq_freq_transition_begin(policy, &freqs);
-		}
-
-		retval = cpufreq_driver->target_index(policy, index);
-		if (retval)
-			pr_err("%s: Failed to change cpu frequency: %d\n",
-			       __func__, retval);
-
-		if (notify)
-			cpufreq_freq_transition_end(policy, &freqs, retval);
+		retval = __target_index(policy, freq_table, index);
 	}
 
 out:
diff --git a/drivers/cpufreq/cpufreq_governor.c b/drivers/cpufreq/cpufreq_governor.c
index ba43991ba98..e1c6433b16e 100644
--- a/drivers/cpufreq/cpufreq_governor.c
+++ b/drivers/cpufreq/cpufreq_governor.c
@@ -366,6 +366,11 @@ int cpufreq_governor_dbs(struct cpufreq_policy *policy,
 		break;
 
 	case CPUFREQ_GOV_LIMITS:
+		mutex_lock(&dbs_data->mutex);
+		if (!cpu_cdbs->cur_policy) {
+			mutex_unlock(&dbs_data->mutex);
+			break;
+		}
 		mutex_lock(&cpu_cdbs->timer_mutex);
 		if (policy->max < cpu_cdbs->cur_policy->cur)
 			__cpufreq_driver_target(cpu_cdbs->cur_policy,
@@ -375,6 +380,7 @@ int cpufreq_governor_dbs(struct cpufreq_policy *policy,
 					policy->min, CPUFREQ_RELATION_L);
 		dbs_check_cpu(dbs_data, cpu);
 		mutex_unlock(&cpu_cdbs->timer_mutex);
+		mutex_unlock(&dbs_data->mutex);
 		break;
 	}
 	return 0;
diff --git a/drivers/cpufreq/cpufreq_opp.c b/drivers/cpufreq/cpufreq_opp.c
new file mode 100644
index 00000000000..c0c6f4a4ecc
--- /dev/null
+++ b/drivers/cpufreq/cpufreq_opp.c
@@ -0,0 +1,110 @@
+/*
+ * Generic OPP helper interface for CPUFreq drivers
+ *
+ * Copyright (C) 2009-2014 Texas Instruments Incorporated.
+ *	Nishanth Menon
+ *	Romit Dasgupta
+ *	Kevin Hilman
+ *
+ * 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/cpufreq.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/errno.h>
+#include <linux/export.h>
+#include <linux/kernel.h>
+#include <linux/pm_opp.h>
+#include <linux/rcupdate.h>
+#include <linux/slab.h>
+
+/**
+ * dev_pm_opp_init_cpufreq_table() - create a cpufreq table for a device
+ * @dev:	device for which we do this operation
+ * @table:	Cpufreq table returned back to caller
+ *
+ * Generate a cpufreq table for a provided device- this assumes that the
+ * opp list is already initialized and ready for usage.
+ *
+ * This function allocates required memory for the cpufreq table. It is
+ * expected that the caller does the required maintenance such as freeing
+ * the table as required.
+ *
+ * Returns -EINVAL for bad pointers, -ENODEV if the device is not found, -ENOMEM
+ * if no memory available for the operation (table is not populated), returns 0
+ * if successful and table is populated.
+ *
+ * WARNING: It is  important for the callers to ensure refreshing their copy of
+ * the table if any of the mentioned functions have been invoked in the interim.
+ *
+ * Locking: The internal device_opp and opp structures are RCU protected.
+ * Since we just use the regular accessor functions to access the internal data
+ * structures, we use RCU read lock inside this function. As a result, users of
+ * this function DONOT need to use explicit locks for invoking.
+ */
+int dev_pm_opp_init_cpufreq_table(struct device *dev,
+				  struct cpufreq_frequency_table **table)
+{
+	struct dev_pm_opp *opp;
+	struct cpufreq_frequency_table *freq_table = NULL;
+	int i, max_opps, ret = 0;
+	unsigned long rate;
+
+	rcu_read_lock();
+
+	max_opps = dev_pm_opp_get_opp_count(dev);
+	if (max_opps <= 0) {
+		ret = max_opps ? max_opps : -ENODATA;
+		goto out;
+	}
+
+	freq_table = kzalloc(sizeof(*freq_table) * (max_opps + 1), GFP_KERNEL);
+	if (!freq_table) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	for (i = 0, rate = 0; i < max_opps; i++, rate++) {
+		/* find next rate */
+		opp = dev_pm_opp_find_freq_ceil(dev, &rate);
+		if (IS_ERR(opp)) {
+			ret = PTR_ERR(opp);
+			goto out;
+		}
+		freq_table[i].driver_data = i;
+		freq_table[i].frequency = rate / 1000;
+	}
+
+	freq_table[i].driver_data = i;
+	freq_table[i].frequency = CPUFREQ_TABLE_END;
+
+	*table = &freq_table[0];
+
+out:
+	rcu_read_unlock();
+	if (ret)
+		kfree(freq_table);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(dev_pm_opp_init_cpufreq_table);
+
+/**
+ * dev_pm_opp_free_cpufreq_table() - free the cpufreq table
+ * @dev:	device for which we do this operation
+ * @table:	table to free
+ *
+ * Free up the table allocated by dev_pm_opp_init_cpufreq_table
+ */
+void dev_pm_opp_free_cpufreq_table(struct device *dev,
+				   struct cpufreq_frequency_table **table)
+{
+	if (!table)
+		return;
+
+	kfree(*table);
+	*table = NULL;
+}
+EXPORT_SYMBOL_GPL(dev_pm_opp_free_cpufreq_table);
diff --git a/drivers/cpufreq/cpufreq_stats.c b/drivers/cpufreq/cpufreq_stats.c
index ecaaebf969f..0cd9b4dcef9 100644
--- a/drivers/cpufreq/cpufreq_stats.c
+++ b/drivers/cpufreq/cpufreq_stats.c
@@ -182,11 +182,11 @@ static void cpufreq_stats_free_table(unsigned int cpu)
 
 static int __cpufreq_stats_create_table(struct cpufreq_policy *policy)
 {
-	unsigned int i, j, count = 0, ret = 0;
+	unsigned int i, count = 0, ret = 0;
 	struct cpufreq_stats *stat;
 	unsigned int alloc_size;
 	unsigned int cpu = policy->cpu;
-	struct cpufreq_frequency_table *table;
+	struct cpufreq_frequency_table *pos, *table;
 
 	table = cpufreq_frequency_get_table(cpu);
 	if (unlikely(!table))
@@ -205,12 +205,8 @@ static int __cpufreq_stats_create_table(struct cpufreq_policy *policy)
 	stat->cpu = cpu;
 	per_cpu(cpufreq_stats_table, cpu) = stat;
 
-	for (i = 0; table[i].frequency != CPUFREQ_TABLE_END; i++) {
-		unsigned int freq = table[i].frequency;
-		if (freq == CPUFREQ_ENTRY_INVALID)
-			continue;
+	cpufreq_for_each_valid_entry(pos, table)
 		count++;
-	}
 
 	alloc_size = count * sizeof(int) + count * sizeof(u64);
 
@@ -228,15 +224,11 @@ static int __cpufreq_stats_create_table(struct cpufreq_policy *policy)
 #ifdef CONFIG_CPU_FREQ_STAT_DETAILS
 	stat->trans_table = stat->freq_table + count;
 #endif
-	j = 0;
-	for (i = 0; table[i].frequency != CPUFREQ_TABLE_END; i++) {
-		unsigned int freq = table[i].frequency;
-		if (freq == CPUFREQ_ENTRY_INVALID)
-			continue;
-		if (freq_table_get_index(stat, freq) == -1)
-			stat->freq_table[j++] = freq;
-	}
-	stat->state_num = j;
+	i = 0;
+	cpufreq_for_each_valid_entry(pos, table)
+		if (freq_table_get_index(stat, pos->frequency) == -1)
+			stat->freq_table[i++] = pos->frequency;
+	stat->state_num = i;
 	spin_lock(&cpufreq_stats_lock);
 	stat->last_time = get_jiffies_64();
 	stat->last_index = freq_table_get_index(stat, policy->cur);
diff --git a/drivers/cpufreq/dbx500-cpufreq.c b/drivers/cpufreq/dbx500-cpufreq.c
index 412a78bb0c9..4bebc1b5db4 100644
--- a/drivers/cpufreq/dbx500-cpufreq.c
+++ b/drivers/cpufreq/dbx500-cpufreq.c
@@ -45,7 +45,7 @@ static struct cpufreq_driver dbx500_cpufreq_driver = {
 
 static int dbx500_cpufreq_probe(struct platform_device *pdev)
 {
-	int i = 0;
+	struct cpufreq_frequency_table *pos;
 
 	freq_table = dev_get_platdata(&pdev->dev);
 	if (!freq_table) {
@@ -60,10 +60,8 @@ static int dbx500_cpufreq_probe(struct platform_device *pdev)
 	}
 
 	pr_info("dbx500-cpufreq: Available frequencies:\n");
-	while (freq_table[i].frequency != CPUFREQ_TABLE_END) {
-		pr_info("  %d Mhz\n", freq_table[i].frequency/1000);
-		i++;
-	}
+	cpufreq_for_each_entry(pos, freq_table)
+		pr_info("  %d Mhz\n", pos->frequency / 1000);
 
 	return cpufreq_register_driver(&dbx500_cpufreq_driver);
 }
diff --git a/drivers/cpufreq/elanfreq.c b/drivers/cpufreq/elanfreq.c
index 7f5d2a68c35..1c06e786c9b 100644
--- a/drivers/cpufreq/elanfreq.c
+++ b/drivers/cpufreq/elanfreq.c
@@ -147,7 +147,7 @@ static int elanfreq_target(struct cpufreq_policy *policy,
 static int elanfreq_cpu_init(struct cpufreq_policy *policy)
 {
 	struct cpuinfo_x86 *c = &cpu_data(0);
-	unsigned int i;
+	struct cpufreq_frequency_table *pos;
 
 	/* capability check */
 	if ((c->x86_vendor != X86_VENDOR_AMD) ||
@@ -159,10 +159,9 @@ static int elanfreq_cpu_init(struct cpufreq_policy *policy)
 		max_freq = elanfreq_get_cpu_frequency(0);
 
 	/* table init */
-	for (i = 0; (elanfreq_table[i].frequency != CPUFREQ_TABLE_END); i++) {
-		if (elanfreq_table[i].frequency > max_freq)
-			elanfreq_table[i].frequency = CPUFREQ_ENTRY_INVALID;
-	}
+	cpufreq_for_each_entry(pos, elanfreq_table)
+		if (pos->frequency > max_freq)
+			pos->frequency = CPUFREQ_ENTRY_INVALID;
 
 	/* cpuinfo and default policy values */
 	policy->cpuinfo.transition_latency = CPUFREQ_ETERNAL;
diff --git a/drivers/cpufreq/exynos-cpufreq.c b/drivers/cpufreq/exynos-cpufreq.c
index f99cfe24e7b..1e0ec57bf6e 100644
--- a/drivers/cpufreq/exynos-cpufreq.c
+++ b/drivers/cpufreq/exynos-cpufreq.c
@@ -17,8 +17,7 @@
 #include <linux/regulator/consumer.h>
 #include <linux/cpufreq.h>
 #include <linux/platform_device.h>
-
-#include <plat/cpu.h>
+#include <linux/of.h>
 
 #include "exynos-cpufreq.h"
 
@@ -29,17 +28,16 @@ static unsigned int locking_frequency;
 static int exynos_cpufreq_get_index(unsigned int freq)
 {
 	struct cpufreq_frequency_table *freq_table = exynos_info->freq_table;
-	int index;
+	struct cpufreq_frequency_table *pos;
 
-	for (index = 0;
-		freq_table[index].frequency != CPUFREQ_TABLE_END; index++)
-		if (freq_table[index].frequency == freq)
+	cpufreq_for_each_entry(pos, freq_table)
+		if (pos->frequency == freq)
 			break;
 
-	if (freq_table[index].frequency == CPUFREQ_TABLE_END)
+	if (pos->frequency == CPUFREQ_TABLE_END)
 		return -EINVAL;
 
-	return index;
+	return pos - freq_table;
 }
 
 static int exynos_cpufreq_scale(unsigned int target_freq)
@@ -49,6 +47,7 @@ static int exynos_cpufreq_scale(unsigned int target_freq)
 	struct cpufreq_policy *policy = cpufreq_cpu_get(0);
 	unsigned int arm_volt, safe_arm_volt = 0;
 	unsigned int mpll_freq_khz = exynos_info->mpll_freq_khz;
+	struct device *dev = exynos_info->dev;
 	unsigned int old_freq;
 	int index, old_index;
 	int ret = 0;
@@ -90,8 +89,8 @@ static int exynos_cpufreq_scale(unsigned int target_freq)
 		/* Firstly, voltage up to increase frequency */
 		ret = regulator_set_voltage(arm_regulator, arm_volt, arm_volt);
 		if (ret) {
-			pr_err("%s: failed to set cpu voltage to %d\n",
-				__func__, arm_volt);
+			dev_err(dev, "failed to set cpu voltage to %d\n",
+				arm_volt);
 			return ret;
 		}
 	}
@@ -100,8 +99,8 @@ static int exynos_cpufreq_scale(unsigned int target_freq)
 		ret = regulator_set_voltage(arm_regulator, safe_arm_volt,
 				      safe_arm_volt);
 		if (ret) {
-			pr_err("%s: failed to set cpu voltage to %d\n",
-				__func__, safe_arm_volt);
+			dev_err(dev, "failed to set cpu voltage to %d\n",
+				safe_arm_volt);
 			return ret;
 		}
 	}
@@ -115,8 +114,8 @@ static int exynos_cpufreq_scale(unsigned int target_freq)
 		ret = regulator_set_voltage(arm_regulator, arm_volt,
 				arm_volt);
 		if (ret) {
-			pr_err("%s: failed to set cpu voltage to %d\n",
-				__func__, arm_volt);
+			dev_err(dev, "failed to set cpu voltage to %d\n",
+				arm_volt);
 			goto out;
 		}
 	}
@@ -163,26 +162,36 @@ static int exynos_cpufreq_probe(struct platform_device *pdev)
 	if (!exynos_info)
 		return -ENOMEM;
 
-	if (soc_is_exynos4210())
+	exynos_info->dev = &pdev->dev;
+
+	if (of_machine_is_compatible("samsung,exynos4210")) {
+		exynos_info->type = EXYNOS_SOC_4210;
 		ret = exynos4210_cpufreq_init(exynos_info);
-	else if (soc_is_exynos4212() || soc_is_exynos4412())
+	} else if (of_machine_is_compatible("samsung,exynos4212")) {
+		exynos_info->type = EXYNOS_SOC_4212;
 		ret = exynos4x12_cpufreq_init(exynos_info);
-	else if (soc_is_exynos5250())
+	} else if (of_machine_is_compatible("samsung,exynos4412")) {
+		exynos_info->type = EXYNOS_SOC_4412;
+		ret = exynos4x12_cpufreq_init(exynos_info);
+	} else if (of_machine_is_compatible("samsung,exynos5250")) {
+		exynos_info->type = EXYNOS_SOC_5250;
 		ret = exynos5250_cpufreq_init(exynos_info);
-	else
-		return 0;
+	} else {
+		pr_err("%s: Unknown SoC type\n", __func__);
+		return -ENODEV;
+	}
 
 	if (ret)
 		goto err_vdd_arm;
 
 	if (exynos_info->set_freq == NULL) {
-		pr_err("%s: No set_freq function (ERR)\n", __func__);
+		dev_err(&pdev->dev, "No set_freq function (ERR)\n");
 		goto err_vdd_arm;
 	}
 
 	arm_regulator = regulator_get(NULL, "vdd_arm");
 	if (IS_ERR(arm_regulator)) {
-		pr_err("%s: failed to get resource vdd_arm\n", __func__);
+		dev_err(&pdev->dev, "failed to get resource vdd_arm\n");
 		goto err_vdd_arm;
 	}
 
@@ -192,7 +201,7 @@ static int exynos_cpufreq_probe(struct platform_device *pdev)
 	if (!cpufreq_register_driver(&exynos_driver))
 		return 0;
 
-	pr_err("%s: failed to register cpufreq driver\n", __func__);
+	dev_err(&pdev->dev, "failed to register cpufreq driver\n");
 	regulator_put(arm_regulator);
 err_vdd_arm:
 	kfree(exynos_info);
diff --git a/drivers/cpufreq/exynos-cpufreq.h b/drivers/cpufreq/exynos-cpufreq.h
index 3ddade8a512..9f2062a7cc0 100644
--- a/drivers/cpufreq/exynos-cpufreq.h
+++ b/drivers/cpufreq/exynos-cpufreq.h
@@ -17,6 +17,13 @@ enum cpufreq_level_index {
 	L20,
 };
 
+enum exynos_soc_type {
+	EXYNOS_SOC_4210,
+	EXYNOS_SOC_4212,
+	EXYNOS_SOC_4412,
+	EXYNOS_SOC_5250,
+};
+
 #define APLL_FREQ(f, a0, a1, a2, a3, a4, a5, a6, a7, b0, b1, b2, m, p, s) \
 	{ \
 		.freq = (f) * 1000, \
@@ -34,6 +41,8 @@ struct apll_freq {
 };
 
 struct exynos_dvfs_info {
+	enum exynos_soc_type type;
+	struct device	*dev;
 	unsigned long	mpll_freq_khz;
 	unsigned int	pll_safe_idx;
 	struct clk	*cpu_clk;
@@ -41,6 +50,7 @@ struct exynos_dvfs_info {
 	struct cpufreq_frequency_table	*freq_table;
 	void (*set_freq)(unsigned int, unsigned int);
 	bool (*need_apll_change)(unsigned int, unsigned int);
+	void __iomem	*cmu_regs;
 };
 
 #ifdef CONFIG_ARM_EXYNOS4210_CPUFREQ
@@ -68,24 +78,21 @@ static inline int exynos5250_cpufreq_init(struct exynos_dvfs_info *info)
 }
 #endif
 
-#include <plat/cpu.h>
-#include <mach/map.h>
-
-#define EXYNOS4_CLKSRC_CPU			(S5P_VA_CMU + 0x14200)
-#define EXYNOS4_CLKMUX_STATCPU			(S5P_VA_CMU + 0x14400)
+#define EXYNOS4_CLKSRC_CPU			0x14200
+#define EXYNOS4_CLKMUX_STATCPU			0x14400
 
-#define EXYNOS4_CLKDIV_CPU			(S5P_VA_CMU + 0x14500)
-#define EXYNOS4_CLKDIV_CPU1			(S5P_VA_CMU + 0x14504)
-#define EXYNOS4_CLKDIV_STATCPU			(S5P_VA_CMU + 0x14600)
-#define EXYNOS4_CLKDIV_STATCPU1			(S5P_VA_CMU + 0x14604)
+#define EXYNOS4_CLKDIV_CPU			0x14500
+#define EXYNOS4_CLKDIV_CPU1			0x14504
+#define EXYNOS4_CLKDIV_STATCPU			0x14600
+#define EXYNOS4_CLKDIV_STATCPU1			0x14604
 
 #define EXYNOS4_CLKSRC_CPU_MUXCORE_SHIFT	(16)
 #define EXYNOS4_CLKMUX_STATCPU_MUXCORE_MASK	(0x7 << EXYNOS4_CLKSRC_CPU_MUXCORE_SHIFT)
 
-#define EXYNOS5_APLL_LOCK			(S5P_VA_CMU + 0x00000)
-#define EXYNOS5_APLL_CON0			(S5P_VA_CMU + 0x00100)
-#define EXYNOS5_CLKMUX_STATCPU			(S5P_VA_CMU + 0x00400)
-#define EXYNOS5_CLKDIV_CPU0			(S5P_VA_CMU + 0x00500)
-#define EXYNOS5_CLKDIV_CPU1			(S5P_VA_CMU + 0x00504)
-#define EXYNOS5_CLKDIV_STATCPU0			(S5P_VA_CMU + 0x00600)
-#define EXYNOS5_CLKDIV_STATCPU1			(S5P_VA_CMU + 0x00604)
+#define EXYNOS5_APLL_LOCK			0x00000
+#define EXYNOS5_APLL_CON0			0x00100
+#define EXYNOS5_CLKMUX_STATCPU			0x00400
+#define EXYNOS5_CLKDIV_CPU0			0x00500
+#define EXYNOS5_CLKDIV_CPU1			0x00504
+#define EXYNOS5_CLKDIV_STATCPU0			0x00600
+#define EXYNOS5_CLKDIV_STATCPU1			0x00604
diff --git a/drivers/cpufreq/exynos4210-cpufreq.c b/drivers/cpufreq/exynos4210-cpufreq.c
index 6384e5b9a34..61a54310a1b 100644
--- a/drivers/cpufreq/exynos4210-cpufreq.c
+++ b/drivers/cpufreq/exynos4210-cpufreq.c
@@ -16,6 +16,8 @@
 #include <linux/io.h>
 #include <linux/slab.h>
 #include <linux/cpufreq.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
 
 #include "exynos-cpufreq.h"
 
@@ -23,6 +25,7 @@ static struct clk *cpu_clk;
 static struct clk *moutcore;
 static struct clk *mout_mpll;
 static struct clk *mout_apll;
+static struct exynos_dvfs_info *cpufreq;
 
 static unsigned int exynos4210_volt_table[] = {
 	1250000, 1150000, 1050000, 975000, 950000,
@@ -60,20 +63,20 @@ static void exynos4210_set_clkdiv(unsigned int div_index)
 
 	tmp = apll_freq_4210[div_index].clk_div_cpu0;
 
-	__raw_writel(tmp, EXYNOS4_CLKDIV_CPU);
+	__raw_writel(tmp, cpufreq->cmu_regs + EXYNOS4_CLKDIV_CPU);
 
 	do {
-		tmp = __raw_readl(EXYNOS4_CLKDIV_STATCPU);
+		tmp = __raw_readl(cpufreq->cmu_regs + EXYNOS4_CLKDIV_STATCPU);
 	} while (tmp & 0x1111111);
 
 	/* Change Divider - CPU1 */
 
 	tmp = apll_freq_4210[div_index].clk_div_cpu1;
 
-	__raw_writel(tmp, EXYNOS4_CLKDIV_CPU1);
+	__raw_writel(tmp, cpufreq->cmu_regs + EXYNOS4_CLKDIV_CPU1);
 
 	do {
-		tmp = __raw_readl(EXYNOS4_CLKDIV_STATCPU1);
+		tmp = __raw_readl(cpufreq->cmu_regs + EXYNOS4_CLKDIV_STATCPU1);
 	} while (tmp & 0x11);
 }
 
@@ -85,7 +88,7 @@ static void exynos4210_set_apll(unsigned int index)
 	clk_set_parent(moutcore, mout_mpll);
 
 	do {
-		tmp = (__raw_readl(EXYNOS4_CLKMUX_STATCPU)
+		tmp = (__raw_readl(cpufreq->cmu_regs + EXYNOS4_CLKMUX_STATCPU)
 			>> EXYNOS4_CLKSRC_CPU_MUXCORE_SHIFT);
 		tmp &= 0x7;
 	} while (tmp != 0x2);
@@ -96,7 +99,7 @@ static void exynos4210_set_apll(unsigned int index)
 	clk_set_parent(moutcore, mout_apll);
 
 	do {
-		tmp = __raw_readl(EXYNOS4_CLKMUX_STATCPU);
+		tmp = __raw_readl(cpufreq->cmu_regs + EXYNOS4_CLKMUX_STATCPU);
 		tmp &= EXYNOS4_CLKMUX_STATCPU_MUXCORE_MASK;
 	} while (tmp != (0x1 << EXYNOS4_CLKSRC_CPU_MUXCORE_SHIFT));
 }
@@ -115,8 +118,30 @@ static void exynos4210_set_frequency(unsigned int old_index,
 
 int exynos4210_cpufreq_init(struct exynos_dvfs_info *info)
 {
+	struct device_node *np;
 	unsigned long rate;
 
+	/*
+	 * HACK: This is a temporary workaround to get access to clock
+	 * controller registers directly and remove static mappings and
+	 * 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.
+	 */
+	np = of_find_compatible_node(NULL, NULL, "samsung,exynos4210-clock");
+	if (!np) {
+		pr_err("%s: failed to find clock controller DT node\n",
+			__func__);
+		return -ENODEV;
+	}
+
+	info->cmu_regs = of_iomap(np, 0);
+	if (!info->cmu_regs) {
+		pr_err("%s: failed to map CMU registers\n", __func__);
+		return -EFAULT;
+	}
+
 	cpu_clk = clk_get(NULL, "armclk");
 	if (IS_ERR(cpu_clk))
 		return PTR_ERR(cpu_clk);
@@ -143,6 +168,8 @@ int exynos4210_cpufreq_init(struct exynos_dvfs_info *info)
 	info->freq_table = exynos4210_freq_table;
 	info->set_freq = exynos4210_set_frequency;
 
+	cpufreq = info;
+
 	return 0;
 
 err_mout_apll:
diff --git a/drivers/cpufreq/exynos4x12-cpufreq.c b/drivers/cpufreq/exynos4x12-cpufreq.c
index 466c76ad335..351a2074cfe 100644
--- a/drivers/cpufreq/exynos4x12-cpufreq.c
+++ b/drivers/cpufreq/exynos4x12-cpufreq.c
@@ -16,6 +16,8 @@
 #include <linux/io.h>
 #include <linux/slab.h>
 #include <linux/cpufreq.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
 
 #include "exynos-cpufreq.h"
 
@@ -23,6 +25,7 @@ static struct clk *cpu_clk;
 static struct clk *moutcore;
 static struct clk *mout_mpll;
 static struct clk *mout_apll;
+static struct exynos_dvfs_info *cpufreq;
 
 static unsigned int exynos4x12_volt_table[] = {
 	1350000, 1287500, 1250000, 1187500, 1137500, 1087500, 1037500,
@@ -100,28 +103,26 @@ static struct apll_freq apll_freq_4412[] = {
 static void exynos4x12_set_clkdiv(unsigned int div_index)
 {
 	unsigned int tmp;
-	unsigned int stat_cpu1;
 
 	/* Change Divider - CPU0 */
 
 	tmp = apll_freq_4x12[div_index].clk_div_cpu0;
 
-	__raw_writel(tmp, EXYNOS4_CLKDIV_CPU);
+	__raw_writel(tmp, cpufreq->cmu_regs + EXYNOS4_CLKDIV_CPU);
 
-	while (__raw_readl(EXYNOS4_CLKDIV_STATCPU) & 0x11111111)
+	while (__raw_readl(cpufreq->cmu_regs + EXYNOS4_CLKDIV_STATCPU)
+	       & 0x11111111)
 		cpu_relax();
 
 	/* Change Divider - CPU1 */
 	tmp = apll_freq_4x12[div_index].clk_div_cpu1;
 
-	__raw_writel(tmp, EXYNOS4_CLKDIV_CPU1);
-	if (soc_is_exynos4212())
-		stat_cpu1 = 0x11;
-	else
-		stat_cpu1 = 0x111;
+	__raw_writel(tmp, cpufreq->cmu_regs + EXYNOS4_CLKDIV_CPU1);
 
-	while (__raw_readl(EXYNOS4_CLKDIV_STATCPU1) & stat_cpu1)
+	do {
 		cpu_relax();
+		tmp = __raw_readl(cpufreq->cmu_regs + EXYNOS4_CLKDIV_STATCPU1);
+	} while (tmp != 0x0);
 }
 
 static void exynos4x12_set_apll(unsigned int index)
@@ -133,7 +134,7 @@ static void exynos4x12_set_apll(unsigned int index)
 
 	do {
 		cpu_relax();
-		tmp = (__raw_readl(EXYNOS4_CLKMUX_STATCPU)
+		tmp = (__raw_readl(cpufreq->cmu_regs + EXYNOS4_CLKMUX_STATCPU)
 			>> EXYNOS4_CLKSRC_CPU_MUXCORE_SHIFT);
 		tmp &= 0x7;
 	} while (tmp != 0x2);
@@ -145,7 +146,7 @@ static void exynos4x12_set_apll(unsigned int index)
 
 	do {
 		cpu_relax();
-		tmp = __raw_readl(EXYNOS4_CLKMUX_STATCPU);
+		tmp = __raw_readl(cpufreq->cmu_regs + EXYNOS4_CLKMUX_STATCPU);
 		tmp &= EXYNOS4_CLKMUX_STATCPU_MUXCORE_MASK;
 	} while (tmp != (0x1 << EXYNOS4_CLKSRC_CPU_MUXCORE_SHIFT));
 }
@@ -164,8 +165,30 @@ static void exynos4x12_set_frequency(unsigned int old_index,
 
 int exynos4x12_cpufreq_init(struct exynos_dvfs_info *info)
 {
+	struct device_node *np;
 	unsigned long rate;
 
+	/*
+	 * HACK: This is a temporary workaround to get access to clock
+	 * controller registers directly and remove static mappings and
+	 * 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.
+	 */
+	np = of_find_compatible_node(NULL, NULL, "samsung,exynos4412-clock");
+	if (!np) {
+		pr_err("%s: failed to find clock controller DT node\n",
+			__func__);
+		return -ENODEV;
+	}
+
+	info->cmu_regs = of_iomap(np, 0);
+	if (!info->cmu_regs) {
+		pr_err("%s: failed to map CMU registers\n", __func__);
+		return -EFAULT;
+	}
+
 	cpu_clk = clk_get(NULL, "armclk");
 	if (IS_ERR(cpu_clk))
 		return PTR_ERR(cpu_clk);
@@ -184,7 +207,7 @@ int exynos4x12_cpufreq_init(struct exynos_dvfs_info *info)
 	if (IS_ERR(mout_apll))
 		goto err_mout_apll;
 
-	if (soc_is_exynos4212())
+	if (info->type == EXYNOS_SOC_4212)
 		apll_freq_4x12 = apll_freq_4212;
 	else
 		apll_freq_4x12 = apll_freq_4412;
@@ -197,6 +220,8 @@ int exynos4x12_cpufreq_init(struct exynos_dvfs_info *info)
 	info->freq_table = exynos4x12_freq_table;
 	info->set_freq = exynos4x12_set_frequency;
 
+	cpufreq = info;
+
 	return 0;
 
 err_mout_apll:
diff --git a/drivers/cpufreq/exynos5250-cpufreq.c b/drivers/cpufreq/exynos5250-cpufreq.c
index 363a0b3fe1b..c91ce69dc63 100644
--- a/drivers/cpufreq/exynos5250-cpufreq.c
+++ b/drivers/cpufreq/exynos5250-cpufreq.c
@@ -16,8 +16,8 @@
 #include <linux/io.h>
 #include <linux/slab.h>
 #include <linux/cpufreq.h>
-
-#include <mach/map.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
 
 #include "exynos-cpufreq.h"
 
@@ -25,6 +25,7 @@ static struct clk *cpu_clk;
 static struct clk *moutcore;
 static struct clk *mout_mpll;
 static struct clk *mout_apll;
+static struct exynos_dvfs_info *cpufreq;
 
 static unsigned int exynos5250_volt_table[] = {
 	1300000, 1250000, 1225000, 1200000, 1150000,
@@ -87,17 +88,18 @@ static void set_clkdiv(unsigned int div_index)
 
 	tmp = apll_freq_5250[div_index].clk_div_cpu0;
 
-	__raw_writel(tmp, EXYNOS5_CLKDIV_CPU0);
+	__raw_writel(tmp, cpufreq->cmu_regs + EXYNOS5_CLKDIV_CPU0);
 
-	while (__raw_readl(EXYNOS5_CLKDIV_STATCPU0) & 0x11111111)
+	while (__raw_readl(cpufreq->cmu_regs + EXYNOS5_CLKDIV_STATCPU0)
+	       & 0x11111111)
 		cpu_relax();
 
 	/* Change Divider - CPU1 */
 	tmp = apll_freq_5250[div_index].clk_div_cpu1;
 
-	__raw_writel(tmp, EXYNOS5_CLKDIV_CPU1);
+	__raw_writel(tmp, cpufreq->cmu_regs + EXYNOS5_CLKDIV_CPU1);
 
-	while (__raw_readl(EXYNOS5_CLKDIV_STATCPU1) & 0x11)
+	while (__raw_readl(cpufreq->cmu_regs + EXYNOS5_CLKDIV_STATCPU1) & 0x11)
 		cpu_relax();
 }
 
@@ -111,7 +113,8 @@ static void set_apll(unsigned int index)
 
 	do {
 		cpu_relax();
-		tmp = (__raw_readl(EXYNOS5_CLKMUX_STATCPU) >> 16);
+		tmp = (__raw_readl(cpufreq->cmu_regs + EXYNOS5_CLKMUX_STATCPU)
+			>> 16);
 		tmp &= 0x7;
 	} while (tmp != 0x2);
 
@@ -122,7 +125,7 @@ static void set_apll(unsigned int index)
 
 	do {
 		cpu_relax();
-		tmp = __raw_readl(EXYNOS5_CLKMUX_STATCPU);
+		tmp = __raw_readl(cpufreq->cmu_regs + EXYNOS5_CLKMUX_STATCPU);
 		tmp &= (0x7 << 16);
 	} while (tmp != (0x1 << 16));
 }
@@ -141,8 +144,30 @@ static void exynos5250_set_frequency(unsigned int old_index,
 
 int exynos5250_cpufreq_init(struct exynos_dvfs_info *info)
 {
+	struct device_node *np;
 	unsigned long rate;
 
+	/*
+	 * HACK: This is a temporary workaround to get access to clock
+	 * controller registers directly and remove static mappings and
+	 * 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.
+	 */
+	np = of_find_compatible_node(NULL, NULL, "samsung,exynos5250-clock");
+	if (!np) {
+		pr_err("%s: failed to find clock controller DT node\n",
+			__func__);
+		return -ENODEV;
+	}
+
+	info->cmu_regs = of_iomap(np, 0);
+	if (!info->cmu_regs) {
+		pr_err("%s: failed to map CMU registers\n", __func__);
+		return -EFAULT;
+	}
+
 	cpu_clk = clk_get(NULL, "armclk");
 	if (IS_ERR(cpu_clk))
 		return PTR_ERR(cpu_clk);
@@ -169,6 +194,8 @@ int exynos5250_cpufreq_init(struct exynos_dvfs_info *info)
 	info->freq_table = exynos5250_freq_table;
 	info->set_freq = exynos5250_set_frequency;
 
+	cpufreq = info;
+
 	return 0;
 
 err_mout_apll:
diff --git a/drivers/cpufreq/exynos5440-cpufreq.c b/drivers/cpufreq/exynos5440-cpufreq.c
index a6b8214d7b7..f33f25b483c 100644
--- a/drivers/cpufreq/exynos5440-cpufreq.c
+++ b/drivers/cpufreq/exynos5440-cpufreq.c
@@ -114,25 +114,23 @@ static struct cpufreq_freqs freqs;
 
 static int init_div_table(void)
 {
-	struct cpufreq_frequency_table *freq_tbl = dvfs_info->freq_table;
+	struct cpufreq_frequency_table *pos, *freq_tbl = dvfs_info->freq_table;
 	unsigned int tmp, clk_div, ema_div, freq, volt_id;
-	int i = 0;
 	struct dev_pm_opp *opp;
 
 	rcu_read_lock();
-	for (i = 0; freq_tbl[i].frequency != CPUFREQ_TABLE_END; i++) {
-
+	cpufreq_for_each_entry(pos, freq_tbl) {
 		opp = dev_pm_opp_find_freq_exact(dvfs_info->dev,
-					freq_tbl[i].frequency * 1000, true);
+					pos->frequency * 1000, true);
 		if (IS_ERR(opp)) {
 			rcu_read_unlock();
 			dev_err(dvfs_info->dev,
 				"failed to find valid OPP for %u KHZ\n",
-				freq_tbl[i].frequency);
+				pos->frequency);
 			return PTR_ERR(opp);
 		}
 
-		freq = freq_tbl[i].frequency / 1000; /* In MHZ */
+		freq = pos->frequency / 1000; /* In MHZ */
 		clk_div = ((freq / CPU_DIV_FREQ_MAX) & P0_7_CPUCLKDEV_MASK)
 					<< P0_7_CPUCLKDEV_SHIFT;
 		clk_div |= ((freq / CPU_ATB_FREQ_MAX) & P0_7_ATBCLKDEV_MASK)
@@ -157,7 +155,8 @@ static int init_div_table(void)
 		tmp = (clk_div | ema_div | (volt_id << P0_7_VDD_SHIFT)
 			| ((freq / FREQ_UNIT) << P0_7_FREQ_SHIFT));
 
-		__raw_writel(tmp, dvfs_info->base + XMU_PMU_P0_7 + 4 * i);
+		__raw_writel(tmp, dvfs_info->base + XMU_PMU_P0_7 + 4 *
+						(pos - freq_tbl));
 	}
 
 	rcu_read_unlock();
@@ -166,8 +165,9 @@ static int init_div_table(void)
 
 static void exynos_enable_dvfs(unsigned int cur_frequency)
 {
-	unsigned int tmp, i, cpu;
+	unsigned int tmp, cpu;
 	struct cpufreq_frequency_table *freq_table = dvfs_info->freq_table;
+	struct cpufreq_frequency_table *pos;
 	/* Disable DVFS */
 	__raw_writel(0,	dvfs_info->base + XMU_DVFS_CTRL);
 
@@ -182,15 +182,15 @@ static void exynos_enable_dvfs(unsigned int cur_frequency)
 	 __raw_writel(tmp, dvfs_info->base + XMU_PMUIRQEN);
 
 	/* Set initial performance index */
-	for (i = 0; freq_table[i].frequency != CPUFREQ_TABLE_END; i++)
-		if (freq_table[i].frequency == cur_frequency)
+	cpufreq_for_each_entry(pos, freq_table)
+		if (pos->frequency == cur_frequency)
 			break;
 
-	if (freq_table[i].frequency == CPUFREQ_TABLE_END) {
+	if (pos->frequency == CPUFREQ_TABLE_END) {
 		dev_crit(dvfs_info->dev, "Boot up frequency not supported\n");
 		/* Assign the highest frequency */
-		i = 0;
-		cur_frequency = freq_table[i].frequency;
+		pos = freq_table;
+		cur_frequency = pos->frequency;
 	}
 
 	dev_info(dvfs_info->dev, "Setting dvfs initial frequency = %uKHZ",
@@ -199,7 +199,7 @@ static void exynos_enable_dvfs(unsigned int cur_frequency)
 	for (cpu = 0; cpu < CONFIG_NR_CPUS; cpu++) {
 		tmp = __raw_readl(dvfs_info->base + XMU_C0_3_PSTATE + cpu * 4);
 		tmp &= ~(P_VALUE_MASK << C0_3_PSTATE_NEW_SHIFT);
-		tmp |= (i << C0_3_PSTATE_NEW_SHIFT);
+		tmp |= ((pos - freq_table) << C0_3_PSTATE_NEW_SHIFT);
 		__raw_writel(tmp, dvfs_info->base + XMU_C0_3_PSTATE + cpu * 4);
 	}
 
diff --git a/drivers/cpufreq/freq_table.c b/drivers/cpufreq/freq_table.c
index 08e7bbcf6d7..1632981c4b2 100644
--- a/drivers/cpufreq/freq_table.c
+++ b/drivers/cpufreq/freq_table.c
@@ -21,22 +21,19 @@
 int cpufreq_frequency_table_cpuinfo(struct cpufreq_policy *policy,
 				    struct cpufreq_frequency_table *table)
 {
+	struct cpufreq_frequency_table *pos;
 	unsigned int min_freq = ~0;
 	unsigned int max_freq = 0;
-	unsigned int i;
+	unsigned int freq;
 
-	for (i = 0; (table[i].frequency != CPUFREQ_TABLE_END); i++) {
-		unsigned int freq = table[i].frequency;
-		if (freq == CPUFREQ_ENTRY_INVALID) {
-			pr_debug("table entry %u is invalid, skipping\n", i);
+	cpufreq_for_each_valid_entry(pos, table) {
+		freq = pos->frequency;
 
-			continue;
-		}
 		if (!cpufreq_boost_enabled()
-		    && (table[i].flags & CPUFREQ_BOOST_FREQ))
+		    && (pos->flags & CPUFREQ_BOOST_FREQ))
 			continue;
 
-		pr_debug("table entry %u: %u kHz\n", i, freq);
+		pr_debug("table entry %u: %u kHz\n", (int)(pos - table), freq);
 		if (freq < min_freq)
 			min_freq = freq;
 		if (freq > max_freq)
@@ -57,7 +54,8 @@ EXPORT_SYMBOL_GPL(cpufreq_frequency_table_cpuinfo);
 int cpufreq_frequency_table_verify(struct cpufreq_policy *policy,
 				   struct cpufreq_frequency_table *table)
 {
-	unsigned int next_larger = ~0, freq, i = 0;
+	struct cpufreq_frequency_table *pos;
+	unsigned int freq, next_larger = ~0;
 	bool found = false;
 
 	pr_debug("request for verification of policy (%u - %u kHz) for cpu %u\n",
@@ -65,9 +63,9 @@ int cpufreq_frequency_table_verify(struct cpufreq_policy *policy,
 
 	cpufreq_verify_within_cpu_limits(policy);
 
-	for (; freq = table[i].frequency, freq != CPUFREQ_TABLE_END; i++) {
-		if (freq == CPUFREQ_ENTRY_INVALID)
-			continue;
+	cpufreq_for_each_valid_entry(pos, table) {
+		freq = pos->frequency;
+
 		if ((freq >= policy->min) && (freq <= policy->max)) {
 			found = true;
 			break;
@@ -118,7 +116,8 @@ int cpufreq_frequency_table_target(struct cpufreq_policy *policy,
 		.driver_data = ~0,
 		.frequency = 0,
 	};
-	unsigned int i;
+	struct cpufreq_frequency_table *pos;
+	unsigned int freq, i = 0;
 
 	pr_debug("request for target %u kHz (relation: %u) for cpu %u\n",
 					target_freq, relation, policy->cpu);
@@ -132,15 +131,19 @@ int cpufreq_frequency_table_target(struct cpufreq_policy *policy,
 		break;
 	}
 
-	for (i = 0; (table[i].frequency != CPUFREQ_TABLE_END); i++) {
-		unsigned int freq = table[i].frequency;
-		if (freq == CPUFREQ_ENTRY_INVALID)
-			continue;
+	cpufreq_for_each_valid_entry(pos, table) {
+		freq = pos->frequency;
+
+		i = pos - table;
 		if ((freq < policy->min) || (freq > policy->max))
 			continue;
+		if (freq == target_freq) {
+			optimal.driver_data = i;
+			break;
+		}
 		switch (relation) {
 		case CPUFREQ_RELATION_H:
-			if (freq <= target_freq) {
+			if (freq < target_freq) {
 				if (freq >= optimal.frequency) {
 					optimal.frequency = freq;
 					optimal.driver_data = i;
@@ -153,7 +156,7 @@ int cpufreq_frequency_table_target(struct cpufreq_policy *policy,
 			}
 			break;
 		case CPUFREQ_RELATION_L:
-			if (freq >= target_freq) {
+			if (freq > target_freq) {
 				if (freq <= optimal.frequency) {
 					optimal.frequency = freq;
 					optimal.driver_data = i;
@@ -184,8 +187,7 @@ EXPORT_SYMBOL_GPL(cpufreq_frequency_table_target);
 int cpufreq_frequency_table_get_index(struct cpufreq_policy *policy,
 		unsigned int freq)
 {
-	struct cpufreq_frequency_table *table;
-	int i;
+	struct cpufreq_frequency_table *pos, *table;
 
 	table = cpufreq_frequency_get_table(policy->cpu);
 	if (unlikely(!table)) {
@@ -193,10 +195,9 @@ int cpufreq_frequency_table_get_index(struct cpufreq_policy *policy,
 		return -ENOENT;
 	}
 
-	for (i = 0; table[i].frequency != CPUFREQ_TABLE_END; i++) {
-		if (table[i].frequency == freq)
-			return i;
-	}
+	cpufreq_for_each_valid_entry(pos, table)
+		if (pos->frequency == freq)
+			return pos - table;
 
 	return -EINVAL;
 }
@@ -208,16 +209,13 @@ EXPORT_SYMBOL_GPL(cpufreq_frequency_table_get_index);
 static ssize_t show_available_freqs(struct cpufreq_policy *policy, char *buf,
 				    bool show_boost)
 {
-	unsigned int i = 0;
 	ssize_t count = 0;
-	struct cpufreq_frequency_table *table = policy->freq_table;
+	struct cpufreq_frequency_table *pos, *table = policy->freq_table;
 
 	if (!table)
 		return -ENODEV;
 
-	for (i = 0; (table[i].frequency != CPUFREQ_TABLE_END); i++) {
-		if (table[i].frequency == CPUFREQ_ENTRY_INVALID)
-			continue;
+	cpufreq_for_each_valid_entry(pos, table) {
 		/*
 		 * show_boost = true and driver_data = BOOST freq
 		 * display BOOST freqs
@@ -229,10 +227,10 @@ static ssize_t show_available_freqs(struct cpufreq_policy *policy, char *buf,
 		 * show_boost = false and driver_data != BOOST freq
 		 * display NON BOOST freqs
 		 */
-		if (show_boost ^ (table[i].flags & CPUFREQ_BOOST_FREQ))
+		if (show_boost ^ (pos->flags & CPUFREQ_BOOST_FREQ))
 			continue;
 
-		count += sprintf(&buf[count], "%d ", table[i].frequency);
+		count += sprintf(&buf[count], "%d ", pos->frequency);
 	}
 	count += sprintf(&buf[count], "\n");
 
diff --git a/drivers/cpufreq/imx6q-cpufreq.c b/drivers/cpufreq/imx6q-cpufreq.c
index e27fca86fe4..af366c21d4b 100644
--- a/drivers/cpufreq/imx6q-cpufreq.c
+++ b/drivers/cpufreq/imx6q-cpufreq.c
@@ -9,7 +9,6 @@
 #include <linux/clk.h>
 #include <linux/cpu.h>
 #include <linux/cpufreq.h>
-#include <linux/delay.h>
 #include <linux/err.h>
 #include <linux/module.h>
 #include <linux/of.h>
@@ -170,25 +169,25 @@ static int imx6q_cpufreq_probe(struct platform_device *pdev)
 		return -ENOENT;
 	}
 
-	arm_clk = devm_clk_get(cpu_dev, "arm");
-	pll1_sys_clk = devm_clk_get(cpu_dev, "pll1_sys");
-	pll1_sw_clk = devm_clk_get(cpu_dev, "pll1_sw");
-	step_clk = devm_clk_get(cpu_dev, "step");
-	pll2_pfd2_396m_clk = devm_clk_get(cpu_dev, "pll2_pfd2_396m");
+	arm_clk = clk_get(cpu_dev, "arm");
+	pll1_sys_clk = clk_get(cpu_dev, "pll1_sys");
+	pll1_sw_clk = clk_get(cpu_dev, "pll1_sw");
+	step_clk = clk_get(cpu_dev, "step");
+	pll2_pfd2_396m_clk = clk_get(cpu_dev, "pll2_pfd2_396m");
 	if (IS_ERR(arm_clk) || IS_ERR(pll1_sys_clk) || IS_ERR(pll1_sw_clk) ||
 	    IS_ERR(step_clk) || IS_ERR(pll2_pfd2_396m_clk)) {
 		dev_err(cpu_dev, "failed to get clocks\n");
 		ret = -ENOENT;
-		goto put_node;
+		goto put_clk;
 	}
 
-	arm_reg = devm_regulator_get(cpu_dev, "arm");
-	pu_reg = devm_regulator_get(cpu_dev, "pu");
-	soc_reg = devm_regulator_get(cpu_dev, "soc");
+	arm_reg = regulator_get(cpu_dev, "arm");
+	pu_reg = regulator_get(cpu_dev, "pu");
+	soc_reg = regulator_get(cpu_dev, "soc");
 	if (IS_ERR(arm_reg) || IS_ERR(pu_reg) || IS_ERR(soc_reg)) {
 		dev_err(cpu_dev, "failed to get regulators\n");
 		ret = -ENOENT;
-		goto put_node;
+		goto put_reg;
 	}
 
 	/*
@@ -201,21 +200,21 @@ static int imx6q_cpufreq_probe(struct platform_device *pdev)
 		ret = of_init_opp_table(cpu_dev);
 		if (ret < 0) {
 			dev_err(cpu_dev, "failed to init OPP table: %d\n", ret);
-			goto put_node;
+			goto put_reg;
 		}
 
 		num = dev_pm_opp_get_opp_count(cpu_dev);
 		if (num < 0) {
 			ret = num;
 			dev_err(cpu_dev, "no OPP table is found: %d\n", ret);
-			goto put_node;
+			goto put_reg;
 		}
 	}
 
 	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 put_node;
+		goto put_reg;
 	}
 
 	/* Make imx6_soc_volt array's size same as arm opp number */
@@ -301,7 +300,24 @@ soc_opp_out:
 
 free_freq_table:
 	dev_pm_opp_free_cpufreq_table(cpu_dev, &freq_table);
-put_node:
+put_reg:
+	if (!IS_ERR(arm_reg))
+		regulator_put(arm_reg);
+	if (!IS_ERR(pu_reg))
+		regulator_put(pu_reg);
+	if (!IS_ERR(soc_reg))
+		regulator_put(soc_reg);
+put_clk:
+	if (!IS_ERR(arm_clk))
+		clk_put(arm_clk);
+	if (!IS_ERR(pll1_sys_clk))
+		clk_put(pll1_sys_clk);
+	if (!IS_ERR(pll1_sw_clk))
+		clk_put(pll1_sw_clk);
+	if (!IS_ERR(step_clk))
+		clk_put(step_clk);
+	if (!IS_ERR(pll2_pfd2_396m_clk))
+		clk_put(pll2_pfd2_396m_clk);
 	of_node_put(np);
 	return ret;
 }
@@ -310,6 +326,14 @@ static int imx6q_cpufreq_remove(struct platform_device *pdev)
 {
 	cpufreq_unregister_driver(&imx6q_cpufreq_driver);
 	dev_pm_opp_free_cpufreq_table(cpu_dev, &freq_table);
+	regulator_put(arm_reg);
+	regulator_put(pu_reg);
+	regulator_put(soc_reg);
+	clk_put(arm_clk);
+	clk_put(pll1_sys_clk);
+	clk_put(pll1_sw_clk);
+	clk_put(step_clk);
+	clk_put(pll2_pfd2_396m_clk);
 
 	return 0;
 }
diff --git a/drivers/cpufreq/intel_pstate.c b/drivers/cpufreq/intel_pstate.c
index 099967302bf..aebd4572eb6 100644
--- a/drivers/cpufreq/intel_pstate.c
+++ b/drivers/cpufreq/intel_pstate.c
@@ -32,17 +32,16 @@
 #include <asm/msr.h>
 #include <asm/cpu_device_id.h>
 
-#define SAMPLE_COUNT		3
-
 #define BYT_RATIOS		0x66a
 #define BYT_VIDS		0x66b
 #define BYT_TURBO_RATIOS	0x66c
+#define BYT_TURBO_VIDS		0x66d
 
 
-#define FRAC_BITS 6
+#define FRAC_BITS 8
 #define int_tofp(X) ((int64_t)(X) << FRAC_BITS)
 #define fp_toint(X) ((X) >> FRAC_BITS)
-#define FP_ROUNDUP(X) ((X) += 1 << FRAC_BITS)
+
 
 static inline int32_t mul_fp(int32_t x, int32_t y)
 {
@@ -58,8 +57,8 @@ struct sample {
 	int32_t core_pct_busy;
 	u64 aperf;
 	u64 mperf;
-	unsigned long long tsc;
 	int freq;
+	ktime_t time;
 };
 
 struct pstate_data {
@@ -70,8 +69,9 @@ struct pstate_data {
 };
 
 struct vid_data {
-	int32_t min;
-	int32_t max;
+	int min;
+	int max;
+	int turbo;
 	int32_t ratio;
 };
 
@@ -88,17 +88,15 @@ struct _pid {
 struct cpudata {
 	int cpu;
 
-	char name[64];
-
 	struct timer_list timer;
 
 	struct pstate_data pstate;
 	struct vid_data vid;
 	struct _pid pid;
 
+	ktime_t last_sample_time;
 	u64	prev_aperf;
 	u64	prev_mperf;
-	unsigned long long prev_tsc;
 	struct sample sample;
 };
 
@@ -198,7 +196,10 @@ static signed int pid_calc(struct _pid *pid, int32_t busy)
 	pid->last_err = fp_error;
 
 	result = pterm + mul_fp(pid->integral, pid->i_gain) + dterm;
-
+	if (result >= 0)
+		result = result + (1 << (FRAC_BITS-1));
+	else
+		result = result - (1 << (FRAC_BITS-1));
 	return (signed int)fp_toint(result);
 }
 
@@ -359,14 +360,14 @@ static int byt_get_min_pstate(void)
 {
 	u64 value;
 	rdmsrl(BYT_RATIOS, value);
-	return (value >> 8) & 0xFF;
+	return (value >> 8) & 0x3F;
 }
 
 static int byt_get_max_pstate(void)
 {
 	u64 value;
 	rdmsrl(BYT_RATIOS, value);
-	return (value >> 16) & 0xFF;
+	return (value >> 16) & 0x3F;
 }
 
 static int byt_get_turbo_pstate(void)
@@ -393,6 +394,9 @@ static void byt_set_pstate(struct cpudata *cpudata, int pstate)
 	vid_fp = clamp_t(int32_t, vid_fp, cpudata->vid.min, cpudata->vid.max);
 	vid = fp_toint(vid_fp);
 
+	if (pstate > cpudata->pstate.max_pstate)
+		vid = cpudata->vid.turbo;
+
 	val |= vid;
 
 	wrmsrl(MSR_IA32_PERF_CTL, val);
@@ -402,13 +406,17 @@ static void byt_get_vid(struct cpudata *cpudata)
 {
 	u64 value;
 
+
 	rdmsrl(BYT_VIDS, value);
-	cpudata->vid.min = int_tofp((value >> 8) & 0x7f);
-	cpudata->vid.max = int_tofp((value >> 16) & 0x7f);
+	cpudata->vid.min = int_tofp((value >> 8) & 0x3f);
+	cpudata->vid.max = int_tofp((value >> 16) & 0x3f);
 	cpudata->vid.ratio = div_fp(
 		cpudata->vid.max - cpudata->vid.min,
 		int_tofp(cpudata->pstate.max_pstate -
 			cpudata->pstate.min_pstate));
+
+	rdmsrl(BYT_TURBO_VIDS, value);
+	cpudata->vid.turbo = value & 0x7f;
 }
 
 
@@ -537,66 +545,54 @@ static inline void intel_pstate_pstate_decrease(struct cpudata *cpu, int steps)
 
 static void intel_pstate_get_cpu_pstates(struct cpudata *cpu)
 {
-	sprintf(cpu->name, "Intel 2nd generation core");
-
 	cpu->pstate.min_pstate = pstate_funcs.get_min();
 	cpu->pstate.max_pstate = pstate_funcs.get_max();
 	cpu->pstate.turbo_pstate = pstate_funcs.get_turbo();
 
 	if (pstate_funcs.get_vid)
 		pstate_funcs.get_vid(cpu);
-
-	/*
-	 * goto max pstate so we don't slow up boot if we are built-in if we are
-	 * a module we will take care of it during normal operation
-	 */
-	intel_pstate_set_pstate(cpu, cpu->pstate.max_pstate);
+	intel_pstate_set_pstate(cpu, cpu->pstate.min_pstate);
 }
 
-static inline void intel_pstate_calc_busy(struct cpudata *cpu,
-					struct sample *sample)
+static inline void intel_pstate_calc_busy(struct cpudata *cpu)
 {
-	int32_t core_pct;
-	int32_t c0_pct;
+	struct sample *sample = &cpu->sample;
+	int64_t core_pct;
+	int32_t rem;
 
-	core_pct = div_fp(int_tofp((sample->aperf)),
-			int_tofp((sample->mperf)));
-	core_pct = mul_fp(core_pct, int_tofp(100));
-	FP_ROUNDUP(core_pct);
+	core_pct = int_tofp(sample->aperf) * int_tofp(100);
+	core_pct = div_u64_rem(core_pct, int_tofp(sample->mperf), &rem);
 
-	c0_pct = div_fp(int_tofp(sample->mperf), int_tofp(sample->tsc));
+	if ((rem << 1) >= int_tofp(sample->mperf))
+		core_pct += 1;
 
 	sample->freq = fp_toint(
 		mul_fp(int_tofp(cpu->pstate.max_pstate * 1000), core_pct));
 
-	sample->core_pct_busy = mul_fp(core_pct, c0_pct);
+	sample->core_pct_busy = (int32_t)core_pct;
 }
 
 static inline void intel_pstate_sample(struct cpudata *cpu)
 {
 	u64 aperf, mperf;
-	unsigned long long tsc;
 
 	rdmsrl(MSR_IA32_APERF, aperf);
 	rdmsrl(MSR_IA32_MPERF, mperf);
-	tsc = native_read_tsc();
 
 	aperf = aperf >> FRAC_BITS;
 	mperf = mperf >> FRAC_BITS;
-	tsc = tsc >> FRAC_BITS;
 
+	cpu->last_sample_time = cpu->sample.time;
+	cpu->sample.time = ktime_get();
 	cpu->sample.aperf = aperf;
 	cpu->sample.mperf = mperf;
-	cpu->sample.tsc = tsc;
 	cpu->sample.aperf -= cpu->prev_aperf;
 	cpu->sample.mperf -= cpu->prev_mperf;
-	cpu->sample.tsc -= cpu->prev_tsc;
 
-	intel_pstate_calc_busy(cpu, &cpu->sample);
+	intel_pstate_calc_busy(cpu);
 
 	cpu->prev_aperf = aperf;
 	cpu->prev_mperf = mperf;
-	cpu->prev_tsc = tsc;
 }
 
 static inline void intel_pstate_set_sample_time(struct cpudata *cpu)
@@ -610,13 +606,25 @@ static inline void intel_pstate_set_sample_time(struct cpudata *cpu)
 
 static inline int32_t intel_pstate_get_scaled_busy(struct cpudata *cpu)
 {
-	int32_t core_busy, max_pstate, current_pstate;
+	int32_t core_busy, max_pstate, current_pstate, sample_ratio;
+	u32 duration_us;
+	u32 sample_time;
 
 	core_busy = cpu->sample.core_pct_busy;
 	max_pstate = int_tofp(cpu->pstate.max_pstate);
 	current_pstate = int_tofp(cpu->pstate.current_pstate);
 	core_busy = mul_fp(core_busy, div_fp(max_pstate, current_pstate));
-	return FP_ROUNDUP(core_busy);
+
+	sample_time = (pid_params.sample_rate_ms  * USEC_PER_MSEC);
+	duration_us = (u32) ktime_us_delta(cpu->sample.time,
+					cpu->last_sample_time);
+	if (duration_us > sample_time * 3) {
+		sample_ratio = div_fp(int_tofp(sample_time),
+				int_tofp(duration_us));
+		core_busy = mul_fp(core_busy, sample_ratio);
+	}
+
+	return core_busy;
 }
 
 static inline void intel_pstate_adjust_busy_pstate(struct cpudata *cpu)
@@ -670,10 +678,13 @@ static const struct x86_cpu_id intel_pstate_cpu_ids[] = {
 	ICPU(0x37, byt_params),
 	ICPU(0x3a, core_params),
 	ICPU(0x3c, core_params),
+	ICPU(0x3d, core_params),
 	ICPU(0x3e, core_params),
 	ICPU(0x3f, core_params),
 	ICPU(0x45, core_params),
 	ICPU(0x46, core_params),
+	ICPU(0x4f, core_params),
+	ICPU(0x56, core_params),
 	{}
 };
 MODULE_DEVICE_TABLE(x86cpu, intel_pstate_cpu_ids);
@@ -695,11 +706,6 @@ static int intel_pstate_init_cpu(unsigned int cpunum)
 	cpu = all_cpu_data[cpunum];
 
 	intel_pstate_get_cpu_pstates(cpu);
-	if (!cpu->pstate.current_pstate) {
-		all_cpu_data[cpunum] = NULL;
-		kfree(cpu);
-		return -ENODATA;
-	}
 
 	cpu->cpu = cpunum;
 
@@ -710,7 +716,6 @@ static int intel_pstate_init_cpu(unsigned int cpunum)
 	cpu->timer.expires = jiffies + HZ/100;
 	intel_pstate_busy_pid_reset(cpu);
 	intel_pstate_sample(cpu);
-	intel_pstate_set_pstate(cpu, cpu->pstate.max_pstate);
 
 	add_timer_on(&cpu->timer, cpunum);
 
diff --git a/drivers/cpufreq/longhaul.c b/drivers/cpufreq/longhaul.c
index 5c4369b5d83..c913906a719 100644
--- a/drivers/cpufreq/longhaul.c
+++ b/drivers/cpufreq/longhaul.c
@@ -530,6 +530,7 @@ static int longhaul_get_ranges(void)
 
 static void longhaul_setup_voltagescaling(void)
 {
+	struct cpufreq_frequency_table *freq_pos;
 	union msr_longhaul longhaul;
 	struct mV_pos minvid, maxvid, vid;
 	unsigned int j, speed, pos, kHz_step, numvscales;
@@ -608,18 +609,16 @@ static void longhaul_setup_voltagescaling(void)
 	/* Calculate kHz for one voltage step */
 	kHz_step = (highest_speed - min_vid_speed) / numvscales;
 
-	j = 0;
-	while (longhaul_table[j].frequency != CPUFREQ_TABLE_END) {
-		speed = longhaul_table[j].frequency;
+	cpufreq_for_each_entry(freq_pos, longhaul_table) {
+		speed = freq_pos->frequency;
 		if (speed > min_vid_speed)
 			pos = (speed - min_vid_speed) / kHz_step + minvid.pos;
 		else
 			pos = minvid.pos;
-		longhaul_table[j].driver_data |= mV_vrm_table[pos] << 8;
+		freq_pos->driver_data |= mV_vrm_table[pos] << 8;
 		vid = vrm_mV_table[mV_vrm_table[pos]];
 		printk(KERN_INFO PFX "f: %d kHz, index: %d, vid: %d mV\n",
-				speed, j, vid.mV);
-		j++;
+			speed, (int)(freq_pos - longhaul_table), vid.mV);
 	}
 
 	can_scale_voltage = 1;
diff --git a/drivers/cpufreq/loongson2_cpufreq.c b/drivers/cpufreq/loongson2_cpufreq.c
index f0bc31f5db2..d4add862194 100644
--- a/drivers/cpufreq/loongson2_cpufreq.c
+++ b/drivers/cpufreq/loongson2_cpufreq.c
@@ -62,7 +62,7 @@ static int loongson2_cpufreq_target(struct cpufreq_policy *policy,
 	set_cpus_allowed_ptr(current, &cpus_allowed);
 
 	/* setting the cpu frequency */
-	clk_set_rate(policy->clk, freq);
+	clk_set_rate(policy->clk, freq * 1000);
 
 	return 0;
 }
@@ -92,7 +92,7 @@ static int loongson2_cpufreq_cpu_init(struct cpufreq_policy *policy)
 	     i++)
 		loongson2_clockmod_table[i].frequency = (rate * i) / 8;
 
-	ret = clk_set_rate(cpuclk, rate);
+	ret = clk_set_rate(cpuclk, rate * 1000);
 	if (ret) {
 		clk_put(cpuclk);
 		return ret;
diff --git a/drivers/cpufreq/pasemi-cpufreq.c b/drivers/cpufreq/pasemi-cpufreq.c
index 84c84b5f0f3..35dd4d7ffee 100644
--- a/drivers/cpufreq/pasemi-cpufreq.c
+++ b/drivers/cpufreq/pasemi-cpufreq.c
@@ -136,9 +136,10 @@ void restore_astate(int cpu)
 
 static int pas_cpufreq_cpu_init(struct cpufreq_policy *policy)
 {
+	struct cpufreq_frequency_table *pos;
 	const u32 *max_freqp;
 	u32 max_freq;
-	int i, cur_astate;
+	int cur_astate;
 	struct resource res;
 	struct device_node *cpu, *dn;
 	int err = -ENODEV;
@@ -197,10 +198,9 @@ static int pas_cpufreq_cpu_init(struct cpufreq_policy *policy)
 	pr_debug("initializing frequency table\n");
 
 	/* initialize frequency table */
-	for (i=0; pas_freqs[i].frequency!=CPUFREQ_TABLE_END; i++) {
-		pas_freqs[i].frequency =
-			get_astate_freq(pas_freqs[i].driver_data) * 100000;
-		pr_debug("%d: %d\n", i, pas_freqs[i].frequency);
+	cpufreq_for_each_entry(pos, pas_freqs) {
+		pos->frequency = get_astate_freq(pos->driver_data) * 100000;
+		pr_debug("%d: %d\n", (int)(pos - pas_freqs), pos->frequency);
 	}
 
 	cur_astate = get_cur_astate(policy->cpu);
diff --git a/drivers/cpufreq/powernow-k6.c b/drivers/cpufreq/powernow-k6.c
index 78904e6ca4a..c8012bc8691 100644
--- a/drivers/cpufreq/powernow-k6.c
+++ b/drivers/cpufreq/powernow-k6.c
@@ -151,6 +151,7 @@ static int powernow_k6_target(struct cpufreq_policy *policy,
 
 static int powernow_k6_cpu_init(struct cpufreq_policy *policy)
 {
+	struct cpufreq_frequency_table *pos;
 	unsigned int i, f;
 	unsigned khz;
 
@@ -168,12 +169,11 @@ static int powernow_k6_cpu_init(struct cpufreq_policy *policy)
 		}
 	}
 	if (param_max_multiplier) {
-		for (i = 0; (clock_ratio[i].frequency != CPUFREQ_TABLE_END); i++) {
-			if (clock_ratio[i].driver_data == param_max_multiplier) {
+		cpufreq_for_each_entry(pos, clock_ratio)
+			if (pos->driver_data == param_max_multiplier) {
 				max_multiplier = param_max_multiplier;
 				goto have_max_multiplier;
 			}
-		}
 		printk(KERN_ERR "powernow-k6: invalid max_multiplier parameter, valid parameters 20, 30, 35, 40, 45, 50, 55, 60\n");
 		return -EINVAL;
 	}
@@ -201,12 +201,12 @@ have_busfreq:
 	param_busfreq = busfreq * 10;
 
 	/* table init */
-	for (i = 0; (clock_ratio[i].frequency != CPUFREQ_TABLE_END); i++) {
-		f = clock_ratio[i].driver_data;
+	cpufreq_for_each_entry(pos, clock_ratio) {
+		f = pos->driver_data;
 		if (f > max_multiplier)
-			clock_ratio[i].frequency = CPUFREQ_ENTRY_INVALID;
+			pos->frequency = CPUFREQ_ENTRY_INVALID;
 		else
-			clock_ratio[i].frequency = busfreq * f;
+			pos->frequency = busfreq * f;
 	}
 
 	/* cpuinfo and default policy values */
diff --git a/drivers/cpufreq/powernow-k8.c b/drivers/cpufreq/powernow-k8.c
index 1b6ae6b57c1..f9ce7e4bf0f 100644
--- a/drivers/cpufreq/powernow-k8.c
+++ b/drivers/cpufreq/powernow-k8.c
@@ -27,6 +27,8 @@
  *  power and thermal data sheets, (e.g. 30417.pdf, 30430.pdf, 43375.pdf)
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/kernel.h>
 #include <linux/smp.h>
 #include <linux/module.h>
@@ -45,7 +47,6 @@
 #include <linux/mutex.h>
 #include <acpi/processor.h>
 
-#define PFX "powernow-k8: "
 #define VERSION "version 2.20.00"
 #include "powernow-k8.h"
 
@@ -161,7 +162,7 @@ static int write_new_fid(struct powernow_k8_data *data, u32 fid)
 	u32 i = 0;
 
 	if ((fid & INVALID_FID_MASK) || (data->currvid & INVALID_VID_MASK)) {
-		printk(KERN_ERR PFX "internal error - overflow on fid write\n");
+		pr_err("internal error - overflow on fid write\n");
 		return 1;
 	}
 
@@ -175,9 +176,7 @@ static int write_new_fid(struct powernow_k8_data *data, u32 fid)
 	do {
 		wrmsr(MSR_FIDVID_CTL, lo, data->plllock * PLL_LOCK_CONVERSION);
 		if (i++ > 100) {
-			printk(KERN_ERR PFX
-				"Hardware error - pending bit very stuck - "
-				"no further pstate changes possible\n");
+			pr_err("Hardware error - pending bit very stuck - no further pstate changes possible\n");
 			return 1;
 		}
 	} while (query_current_values_with_pending_wait(data));
@@ -185,15 +184,13 @@ static int write_new_fid(struct powernow_k8_data *data, u32 fid)
 	count_off_irt(data);
 
 	if (savevid != data->currvid) {
-		printk(KERN_ERR PFX
-			"vid change on fid trans, old 0x%x, new 0x%x\n",
-			savevid, data->currvid);
+		pr_err("vid change on fid trans, old 0x%x, new 0x%x\n",
+		       savevid, data->currvid);
 		return 1;
 	}
 
 	if (fid != data->currfid) {
-		printk(KERN_ERR PFX
-			"fid trans failed, fid 0x%x, curr 0x%x\n", fid,
+		pr_err("fid trans failed, fid 0x%x, curr 0x%x\n", fid,
 			data->currfid);
 		return 1;
 	}
@@ -209,7 +206,7 @@ static int write_new_vid(struct powernow_k8_data *data, u32 vid)
 	int i = 0;
 
 	if ((data->currfid & INVALID_FID_MASK) || (vid & INVALID_VID_MASK)) {
-		printk(KERN_ERR PFX "internal error - overflow on vid write\n");
+		pr_err("internal error - overflow on vid write\n");
 		return 1;
 	}
 
@@ -223,23 +220,19 @@ static int write_new_vid(struct powernow_k8_data *data, u32 vid)
 	do {
 		wrmsr(MSR_FIDVID_CTL, lo, STOP_GRANT_5NS);
 		if (i++ > 100) {
-			printk(KERN_ERR PFX "internal error - pending bit "
-					"very stuck - no further pstate "
-					"changes possible\n");
+			pr_err("internal error - pending bit very stuck - no further pstate changes possible\n");
 			return 1;
 		}
 	} while (query_current_values_with_pending_wait(data));
 
 	if (savefid != data->currfid) {
-		printk(KERN_ERR PFX "fid changed on vid trans, old "
-			"0x%x new 0x%x\n",
-		       savefid, data->currfid);
+		pr_err("fid changed on vid trans, old 0x%x new 0x%x\n",
+			savefid, data->currfid);
 		return 1;
 	}
 
 	if (vid != data->currvid) {
-		printk(KERN_ERR PFX "vid trans failed, vid 0x%x, "
-				"curr 0x%x\n",
+		pr_err("vid trans failed, vid 0x%x, curr 0x%x\n",
 				vid, data->currvid);
 		return 1;
 	}
@@ -283,8 +276,7 @@ static int transition_fid_vid(struct powernow_k8_data *data,
 		return 1;
 
 	if ((reqfid != data->currfid) || (reqvid != data->currvid)) {
-		printk(KERN_ERR PFX "failed (cpu%d): req 0x%x 0x%x, "
-				"curr 0x%x 0x%x\n",
+		pr_err("failed (cpu%d): req 0x%x 0x%x, curr 0x%x 0x%x\n",
 				smp_processor_id(),
 				reqfid, reqvid, data->currfid, data->currvid);
 		return 1;
@@ -304,8 +296,7 @@ static int core_voltage_pre_transition(struct powernow_k8_data *data,
 	u32 savefid = data->currfid;
 	u32 maxvid, lo, rvomult = 1;
 
-	pr_debug("ph1 (cpu%d): start, currfid 0x%x, currvid 0x%x, "
-		"reqvid 0x%x, rvo 0x%x\n",
+	pr_debug("ph1 (cpu%d): start, currfid 0x%x, currvid 0x%x, reqvid 0x%x, rvo 0x%x\n",
 		smp_processor_id(),
 		data->currfid, data->currvid, reqvid, data->rvo);
 
@@ -342,8 +333,7 @@ static int core_voltage_pre_transition(struct powernow_k8_data *data,
 		return 1;
 
 	if (savefid != data->currfid) {
-		printk(KERN_ERR PFX "ph1 err, currfid changed 0x%x\n",
-				data->currfid);
+		pr_err("ph1 err, currfid changed 0x%x\n", data->currfid);
 		return 1;
 	}
 
@@ -360,13 +350,11 @@ static int core_frequency_transition(struct powernow_k8_data *data, u32 reqfid)
 	u32 fid_interval, savevid = data->currvid;
 
 	if (data->currfid == reqfid) {
-		printk(KERN_ERR PFX "ph2 null fid transition 0x%x\n",
-				data->currfid);
+		pr_err("ph2 null fid transition 0x%x\n", data->currfid);
 		return 0;
 	}
 
-	pr_debug("ph2 (cpu%d): starting, currfid 0x%x, currvid 0x%x, "
-		"reqfid 0x%x\n",
+	pr_debug("ph2 (cpu%d): starting, currfid 0x%x, currvid 0x%x, reqfid 0x%x\n",
 		smp_processor_id(),
 		data->currfid, data->currvid, reqfid);
 
@@ -409,15 +397,13 @@ static int core_frequency_transition(struct powernow_k8_data *data, u32 reqfid)
 		return 1;
 
 	if (data->currfid != reqfid) {
-		printk(KERN_ERR PFX
-			"ph2: mismatch, failed fid transition, "
-			"curr 0x%x, req 0x%x\n",
+		pr_err("ph2: mismatch, failed fid transition, curr 0x%x, req 0x%x\n",
 			data->currfid, reqfid);
 		return 1;
 	}
 
 	if (savevid != data->currvid) {
-		printk(KERN_ERR PFX "ph2: vid changed, save 0x%x, curr 0x%x\n",
+		pr_err("ph2: vid changed, save 0x%x, curr 0x%x\n",
 			savevid, data->currvid);
 		return 1;
 	}
@@ -444,17 +430,14 @@ static int core_voltage_post_transition(struct powernow_k8_data *data,
 			return 1;
 
 		if (savefid != data->currfid) {
-			printk(KERN_ERR PFX
-			       "ph3: bad fid change, save 0x%x, curr 0x%x\n",
-			       savefid, data->currfid);
+			pr_err("ph3: bad fid change, save 0x%x, curr 0x%x\n",
+				savefid, data->currfid);
 			return 1;
 		}
 
 		if (data->currvid != reqvid) {
-			printk(KERN_ERR PFX
-			       "ph3: failed vid transition\n, "
-			       "req 0x%x, curr 0x%x",
-			       reqvid, data->currvid);
+			pr_err("ph3: failed vid transition\n, req 0x%x, curr 0x%x",
+				reqvid, data->currvid);
 			return 1;
 		}
 	}
@@ -498,23 +481,20 @@ static void check_supported_cpu(void *_rc)
 	if ((eax & CPUID_XFAM) == CPUID_XFAM_K8) {
 		if (((eax & CPUID_USE_XFAM_XMOD) != CPUID_USE_XFAM_XMOD) ||
 		    ((eax & CPUID_XMOD) > CPUID_XMOD_REV_MASK)) {
-			printk(KERN_INFO PFX
-				"Processor cpuid %x not supported\n", eax);
+			pr_info("Processor cpuid %x not supported\n", eax);
 			return;
 		}
 
 		eax = cpuid_eax(CPUID_GET_MAX_CAPABILITIES);
 		if (eax < CPUID_FREQ_VOLT_CAPABILITIES) {
-			printk(KERN_INFO PFX
-			       "No frequency change capabilities detected\n");
+			pr_info("No frequency change capabilities detected\n");
 			return;
 		}
 
 		cpuid(CPUID_FREQ_VOLT_CAPABILITIES, &eax, &ebx, &ecx, &edx);
 		if ((edx & P_STATE_TRANSITION_CAPABLE)
 			!= P_STATE_TRANSITION_CAPABLE) {
-			printk(KERN_INFO PFX
-				"Power state transitions not supported\n");
+			pr_info("Power state transitions not supported\n");
 			return;
 		}
 		*rc = 0;
@@ -529,43 +509,39 @@ static int check_pst_table(struct powernow_k8_data *data, struct pst_s *pst,
 
 	for (j = 0; j < data->numps; j++) {
 		if (pst[j].vid > LEAST_VID) {
-			printk(KERN_ERR FW_BUG PFX "vid %d invalid : 0x%x\n",
-			       j, pst[j].vid);
+			pr_err(FW_BUG "vid %d invalid : 0x%x\n", j,
+				pst[j].vid);
 			return -EINVAL;
 		}
 		if (pst[j].vid < data->rvo) {
 			/* vid + rvo >= 0 */
-			printk(KERN_ERR FW_BUG PFX "0 vid exceeded with pstate"
-			       " %d\n", j);
+			pr_err(FW_BUG "0 vid exceeded with pstate %d\n", j);
 			return -ENODEV;
 		}
 		if (pst[j].vid < maxvid + data->rvo) {
 			/* vid + rvo >= maxvid */
-			printk(KERN_ERR FW_BUG PFX "maxvid exceeded with pstate"
-			       " %d\n", j);
+			pr_err(FW_BUG "maxvid exceeded with pstate %d\n", j);
 			return -ENODEV;
 		}
 		if (pst[j].fid > MAX_FID) {
-			printk(KERN_ERR FW_BUG PFX "maxfid exceeded with pstate"
-			       " %d\n", j);
+			pr_err(FW_BUG "maxfid exceeded with pstate %d\n", j);
 			return -ENODEV;
 		}
 		if (j && (pst[j].fid < HI_FID_TABLE_BOTTOM)) {
 			/* Only first fid is allowed to be in "low" range */
-			printk(KERN_ERR FW_BUG PFX "two low fids - %d : "
-			       "0x%x\n", j, pst[j].fid);
+			pr_err(FW_BUG "two low fids - %d : 0x%x\n", j,
+				pst[j].fid);
 			return -EINVAL;
 		}
 		if (pst[j].fid < lastfid)
 			lastfid = pst[j].fid;
 	}
 	if (lastfid & 1) {
-		printk(KERN_ERR FW_BUG PFX "lastfid invalid\n");
+		pr_err(FW_BUG "lastfid invalid\n");
 		return -EINVAL;
 	}
 	if (lastfid > LO_FID_TABLE_TOP)
-		printk(KERN_INFO FW_BUG PFX
-			"first fid not from lo freq table\n");
+		pr_info(FW_BUG "first fid not from lo freq table\n");
 
 	return 0;
 }
@@ -582,16 +558,14 @@ static void print_basics(struct powernow_k8_data *data)
 	for (j = 0; j < data->numps; j++) {
 		if (data->powernow_table[j].frequency !=
 				CPUFREQ_ENTRY_INVALID) {
-				printk(KERN_INFO PFX
-					"fid 0x%x (%d MHz), vid 0x%x\n",
-					data->powernow_table[j].driver_data & 0xff,
-					data->powernow_table[j].frequency/1000,
-					data->powernow_table[j].driver_data >> 8);
+			pr_info("fid 0x%x (%d MHz), vid 0x%x\n",
+				data->powernow_table[j].driver_data & 0xff,
+				data->powernow_table[j].frequency/1000,
+				data->powernow_table[j].driver_data >> 8);
 		}
 	}
 	if (data->batps)
-		printk(KERN_INFO PFX "Only %d pstates on battery\n",
-				data->batps);
+		pr_info("Only %d pstates on battery\n", data->batps);
 }
 
 static int fill_powernow_table(struct powernow_k8_data *data,
@@ -602,21 +576,20 @@ static int fill_powernow_table(struct powernow_k8_data *data,
 
 	if (data->batps) {
 		/* use ACPI support to get full speed on mains power */
-		printk(KERN_WARNING PFX
-			"Only %d pstates usable (use ACPI driver for full "
-			"range\n", data->batps);
+		pr_warn("Only %d pstates usable (use ACPI driver for full range\n",
+			data->batps);
 		data->numps = data->batps;
 	}
 
 	for (j = 1; j < data->numps; j++) {
 		if (pst[j-1].fid >= pst[j].fid) {
-			printk(KERN_ERR PFX "PST out of sequence\n");
+			pr_err("PST out of sequence\n");
 			return -EINVAL;
 		}
 	}
 
 	if (data->numps < 2) {
-		printk(KERN_ERR PFX "no p states to transition\n");
+		pr_err("no p states to transition\n");
 		return -ENODEV;
 	}
 
@@ -626,7 +599,7 @@ static int fill_powernow_table(struct powernow_k8_data *data,
 	powernow_table = kzalloc((sizeof(*powernow_table)
 		* (data->numps + 1)), GFP_KERNEL);
 	if (!powernow_table) {
-		printk(KERN_ERR PFX "powernow_table memory alloc failure\n");
+		pr_err("powernow_table memory alloc failure\n");
 		return -ENOMEM;
 	}
 
@@ -681,13 +654,13 @@ static int find_psb_table(struct powernow_k8_data *data)
 
 		pr_debug("table vers: 0x%x\n", psb->tableversion);
 		if (psb->tableversion != PSB_VERSION_1_4) {
-			printk(KERN_ERR FW_BUG PFX "PSB table is not v1.4\n");
+			pr_err(FW_BUG "PSB table is not v1.4\n");
 			return -ENODEV;
 		}
 
 		pr_debug("flags: 0x%x\n", psb->flags1);
 		if (psb->flags1) {
-			printk(KERN_ERR FW_BUG PFX "unknown flags\n");
+			pr_err(FW_BUG "unknown flags\n");
 			return -ENODEV;
 		}
 
@@ -716,7 +689,7 @@ static int find_psb_table(struct powernow_k8_data *data)
 				cpst = 1;
 		}
 		if (cpst != 1) {
-			printk(KERN_ERR FW_BUG PFX "numpst must be 1\n");
+			pr_err(FW_BUG "numpst must be 1\n");
 			return -ENODEV;
 		}
 
@@ -742,9 +715,8 @@ static int find_psb_table(struct powernow_k8_data *data)
 	 * BIOS and Kernel Developer's Guide, which is available on
 	 * www.amd.com
 	 */
-	printk(KERN_ERR FW_BUG PFX "No PSB or ACPI _PSS objects\n");
-	printk(KERN_ERR PFX "Make sure that your BIOS is up to date"
-		" and Cool'N'Quiet support is enabled in BIOS setup\n");
+	pr_err(FW_BUG "No PSB or ACPI _PSS objects\n");
+	pr_err("Make sure that your BIOS is up to date and Cool'N'Quiet support is enabled in BIOS setup\n");
 	return -ENODEV;
 }
 
@@ -819,8 +791,7 @@ static int powernow_k8_cpu_init_acpi(struct powernow_k8_data *data)
 	acpi_processor_notify_smm(THIS_MODULE);
 
 	if (!zalloc_cpumask_var(&data->acpi_data.shared_cpu_map, GFP_KERNEL)) {
-		printk(KERN_ERR PFX
-				"unable to alloc powernow_k8_data cpumask\n");
+		pr_err("unable to alloc powernow_k8_data cpumask\n");
 		ret_val = -ENOMEM;
 		goto err_out_mem;
 	}
@@ -885,9 +856,8 @@ static int fill_powernow_table_fidvid(struct powernow_k8_data *data,
 		}
 
 		if (freq != (data->acpi_data.states[i].core_frequency * 1000)) {
-			printk(KERN_INFO PFX "invalid freq entries "
-				"%u kHz vs. %u kHz\n", freq,
-				(unsigned int)
+			pr_info("invalid freq entries %u kHz vs. %u kHz\n",
+				freq, (unsigned int)
 				(data->acpi_data.states[i].core_frequency
 				 * 1000));
 			invalidate_entry(powernow_table, i);
@@ -916,7 +886,7 @@ static int get_transition_latency(struct powernow_k8_data *data)
 			max_latency = cur_latency;
 	}
 	if (max_latency == 0) {
-		pr_err(FW_WARN PFX "Invalid zero transition latency\n");
+		pr_err(FW_WARN "Invalid zero transition latency\n");
 		max_latency = 1;
 	}
 	/* value in usecs, needs to be in nanoseconds */
@@ -991,7 +961,7 @@ static long powernowk8_target_fn(void *arg)
 	checkvid = data->currvid;
 
 	if (pending_bit_stuck()) {
-		printk(KERN_ERR PFX "failing targ, change pending bit set\n");
+		pr_err("failing targ, change pending bit set\n");
 		return -EIO;
 	}
 
@@ -1003,12 +973,11 @@ static long powernowk8_target_fn(void *arg)
 		return -EIO;
 
 	pr_debug("targ: curr fid 0x%x, vid 0x%x\n",
-		 data->currfid, data->currvid);
+		data->currfid, data->currvid);
 
 	if ((checkvid != data->currvid) ||
 	    (checkfid != data->currfid)) {
-		pr_info(PFX
-		       "error - out of sync, fix 0x%x 0x%x, vid 0x%x 0x%x\n",
+		pr_info("error - out of sync, fix 0x%x 0x%x, vid 0x%x 0x%x\n",
 		       checkfid, data->currfid,
 		       checkvid, data->currvid);
 	}
@@ -1020,7 +989,7 @@ static long powernowk8_target_fn(void *arg)
 	ret = transition_frequency_fidvid(data, newstate);
 
 	if (ret) {
-		printk(KERN_ERR PFX "transition frequency failed\n");
+		pr_err("transition frequency failed\n");
 		mutex_unlock(&fidvid_mutex);
 		return 1;
 	}
@@ -1049,7 +1018,7 @@ static void powernowk8_cpu_init_on_cpu(void *_init_on_cpu)
 	struct init_on_cpu *init_on_cpu = _init_on_cpu;
 
 	if (pending_bit_stuck()) {
-		printk(KERN_ERR PFX "failing init, change pending bit set\n");
+		pr_err("failing init, change pending bit set\n");
 		init_on_cpu->rc = -ENODEV;
 		return;
 	}
@@ -1064,11 +1033,10 @@ static void powernowk8_cpu_init_on_cpu(void *_init_on_cpu)
 	init_on_cpu->rc = 0;
 }
 
-static const char missing_pss_msg[] =
-	KERN_ERR
-	FW_BUG PFX "No compatible ACPI _PSS objects found.\n"
-	FW_BUG PFX "First, make sure Cool'N'Quiet is enabled in the BIOS.\n"
-	FW_BUG PFX "If that doesn't help, try upgrading your BIOS.\n";
+#define MISSING_PSS_MSG \
+	FW_BUG "No compatible ACPI _PSS objects found.\n" \
+	FW_BUG "First, make sure Cool'N'Quiet is enabled in the BIOS.\n" \
+	FW_BUG "If that doesn't help, try upgrading your BIOS.\n"
 
 /* per CPU init entry point to the driver */
 static int powernowk8_cpu_init(struct cpufreq_policy *pol)
@@ -1083,7 +1051,7 @@ static int powernowk8_cpu_init(struct cpufreq_policy *pol)
 
 	data = kzalloc(sizeof(*data), GFP_KERNEL);
 	if (!data) {
-		printk(KERN_ERR PFX "unable to alloc powernow_k8_data");
+		pr_err("unable to alloc powernow_k8_data");
 		return -ENOMEM;
 	}
 
@@ -1095,13 +1063,11 @@ static int powernowk8_cpu_init(struct cpufreq_policy *pol)
 		 * an UP version, and is deprecated by AMD.
 		 */
 		if (num_online_cpus() != 1) {
-			printk_once(missing_pss_msg);
+			pr_err_once(MISSING_PSS_MSG);
 			goto err_out;
 		}
 		if (pol->cpu != 0) {
-			printk(KERN_ERR FW_BUG PFX "No ACPI _PSS objects for "
-			       "CPU other than CPU0. Complain to your BIOS "
-			       "vendor.\n");
+			pr_err(FW_BUG "No ACPI _PSS objects for CPU other than CPU0. Complain to your BIOS vendor.\n");
 			goto err_out;
 		}
 		rc = find_psb_table(data);
@@ -1129,7 +1095,7 @@ static int powernowk8_cpu_init(struct cpufreq_policy *pol)
 
 	/* min/max the cpu is capable of */
 	if (cpufreq_table_validate_and_show(pol, data->powernow_table)) {
-		printk(KERN_ERR FW_BUG PFX "invalid powernow_table\n");
+		pr_err(FW_BUG "invalid powernow_table\n");
 		powernow_k8_cpu_exit_acpi(data);
 		kfree(data->powernow_table);
 		kfree(data);
@@ -1137,7 +1103,7 @@ static int powernowk8_cpu_init(struct cpufreq_policy *pol)
 	}
 
 	pr_debug("cpu_init done, current fid 0x%x, vid 0x%x\n",
-		 data->currfid, data->currvid);
+		data->currfid, data->currvid);
 
 	/* Point all the CPUs in this policy to the same data */
 	for_each_cpu(cpu, pol->cpus)
@@ -1220,12 +1186,12 @@ static void __request_acpi_cpufreq(void)
 		goto request;
 
 	if (strncmp(cur_drv, drv, min_t(size_t, strlen(cur_drv), strlen(drv))))
-		pr_warn(PFX "WTF driver: %s\n", cur_drv);
+		pr_warn("WTF driver: %s\n", cur_drv);
 
 	return;
 
  request:
-	pr_warn(PFX "This CPU is not supported anymore, using acpi-cpufreq instead.\n");
+	pr_warn("This CPU is not supported anymore, using acpi-cpufreq instead.\n");
 	request_module(drv);
 }
 
@@ -1260,7 +1226,7 @@ static int powernowk8_init(void)
 	if (ret)
 		return ret;
 
-	pr_info(PFX "Found %d %s (%d cpu cores) (" VERSION ")\n",
+	pr_info("Found %d %s (%d cpu cores) (" VERSION ")\n",
 		num_online_nodes(), boot_cpu_data.x86_model_id, supported_cpus);
 
 	return ret;
@@ -1274,8 +1240,8 @@ static void __exit powernowk8_exit(void)
 	cpufreq_unregister_driver(&cpufreq_amd64_driver);
 }
 
-MODULE_AUTHOR("Paul Devriendt <paul.devriendt@amd.com> and "
-		"Mark Langsdorf <mark.langsdorf@amd.com>");
+MODULE_AUTHOR("Paul Devriendt <paul.devriendt@amd.com>");
+MODULE_AUTHOR("Mark Langsdorf <mark.langsdorf@amd.com>");
 MODULE_DESCRIPTION("AMD Athlon 64 and Opteron processor frequency driver.");
 MODULE_LICENSE("GPL");
 
diff --git a/drivers/cpufreq/powernow-k8.h b/drivers/cpufreq/powernow-k8.h
index 79329d4d5ab..45ce11e8662 100644
--- a/drivers/cpufreq/powernow-k8.h
+++ b/drivers/cpufreq/powernow-k8.h
@@ -19,7 +19,7 @@ struct powernow_k8_data {
 	u32 vidmvs;  /* usable value calculated from mvs */
 	u32 vstable; /* voltage stabilization time, units 20 us */
 	u32 plllock; /* pll lock time, units 1 us */
-        u32 exttype; /* extended interface = 1 */
+	u32 exttype; /* extended interface = 1 */
 
 	/* keep track of the current fid / vid or pstate */
 	u32 currvid;
diff --git a/drivers/cpufreq/powernv-cpufreq.c b/drivers/cpufreq/powernv-cpufreq.c
index af4968813e7..bb1d08dc8cc 100644
--- a/drivers/cpufreq/powernv-cpufreq.c
+++ b/drivers/cpufreq/powernv-cpufreq.c
@@ -235,7 +235,7 @@ static void powernv_read_cpu_freq(void *arg)
  * firmware for CPU 'cpu'. This value is reported through the sysfs
  * file cpuinfo_cur_freq.
  */
-unsigned int powernv_cpufreq_get(unsigned int cpu)
+static unsigned int powernv_cpufreq_get(unsigned int cpu)
 {
 	struct powernv_smp_call_data freq_data;
 
diff --git a/drivers/cpufreq/ppc_cbe_cpufreq.c b/drivers/cpufreq/ppc_cbe_cpufreq.c
index 5be8a48dba7..5a4c5a639f6 100644
--- a/drivers/cpufreq/ppc_cbe_cpufreq.c
+++ b/drivers/cpufreq/ppc_cbe_cpufreq.c
@@ -67,9 +67,10 @@ static int set_pmode(unsigned int cpu, unsigned int slow_mode)
 
 static int cbe_cpufreq_cpu_init(struct cpufreq_policy *policy)
 {
+	struct cpufreq_frequency_table *pos;
 	const u32 *max_freqp;
 	u32 max_freq;
-	int i, cur_pmode;
+	int cur_pmode;
 	struct device_node *cpu;
 
 	cpu = of_get_cpu_node(policy->cpu, NULL);
@@ -102,9 +103,9 @@ static int cbe_cpufreq_cpu_init(struct cpufreq_policy *policy)
 	pr_debug("initializing frequency table\n");
 
 	/* initialize frequency table */
-	for (i=0; cbe_freqs[i].frequency!=CPUFREQ_TABLE_END; i++) {
-		cbe_freqs[i].frequency = max_freq / cbe_freqs[i].driver_data;
-		pr_debug("%d: %d\n", i, cbe_freqs[i].frequency);
+	cpufreq_for_each_entry(pos, cbe_freqs) {
+		pos->frequency = max_freq / pos->driver_data;
+		pr_debug("%d: %d\n", (int)(pos - cbe_freqs), pos->frequency);
 	}
 
 	/* if DEBUG is enabled set_pmode() measures the latency
diff --git a/drivers/cpufreq/s3c2416-cpufreq.c b/drivers/cpufreq/s3c2416-cpufreq.c
index 4626f90559b..2fd53eaaec2 100644
--- a/drivers/cpufreq/s3c2416-cpufreq.c
+++ b/drivers/cpufreq/s3c2416-cpufreq.c
@@ -266,7 +266,7 @@ out:
 static void __init s3c2416_cpufreq_cfg_regulator(struct s3c2416_data *s3c_freq)
 {
 	int count, v, i, found;
-	struct cpufreq_frequency_table *freq;
+	struct cpufreq_frequency_table *pos;
 	struct s3c2416_dvfs *dvfs;
 
 	count = regulator_count_voltages(s3c_freq->vddarm);
@@ -275,12 +275,11 @@ static void __init s3c2416_cpufreq_cfg_regulator(struct s3c2416_data *s3c_freq)
 		return;
 	}
 
-	freq = s3c_freq->freq_table;
-	while (count > 0 && freq->frequency != CPUFREQ_TABLE_END) {
-		if (freq->frequency == CPUFREQ_ENTRY_INVALID)
-			continue;
+	if (!count)
+		goto out;
 
-		dvfs = &s3c2416_dvfs_table[freq->driver_data];
+	cpufreq_for_each_valid_entry(pos, s3c_freq->freq_table) {
+		dvfs = &s3c2416_dvfs_table[pos->driver_data];
 		found = 0;
 
 		/* Check only the min-voltage, more is always ok on S3C2416 */
@@ -292,13 +291,12 @@ static void __init s3c2416_cpufreq_cfg_regulator(struct s3c2416_data *s3c_freq)
 
 		if (!found) {
 			pr_debug("cpufreq: %dkHz unsupported by regulator\n",
-				 freq->frequency);
-			freq->frequency = CPUFREQ_ENTRY_INVALID;
+				 pos->frequency);
+			pos->frequency = CPUFREQ_ENTRY_INVALID;
 		}
-
-		freq++;
 	}
 
+out:
 	/* Guessed */
 	s3c_freq->regulator_latency = 1 * 1000 * 1000;
 }
@@ -338,7 +336,7 @@ static struct notifier_block s3c2416_cpufreq_reboot_notifier = {
 static int __init s3c2416_cpufreq_driver_init(struct cpufreq_policy *policy)
 {
 	struct s3c2416_data *s3c_freq = &s3c2416_cpufreq;
-	struct cpufreq_frequency_table *freq;
+	struct cpufreq_frequency_table *pos;
 	struct clk *msysclk;
 	unsigned long rate;
 	int ret;
@@ -427,31 +425,27 @@ static int __init s3c2416_cpufreq_driver_init(struct cpufreq_policy *policy)
 	s3c_freq->regulator_latency = 0;
 #endif
 
-	freq = s3c_freq->freq_table;
-	while (freq->frequency != CPUFREQ_TABLE_END) {
+	cpufreq_for_each_entry(pos, s3c_freq->freq_table) {
 		/* special handling for dvs mode */
-		if (freq->driver_data == 0) {
+		if (pos->driver_data == 0) {
 			if (!s3c_freq->hclk) {
 				pr_debug("cpufreq: %dkHz unsupported as it would need unavailable dvs mode\n",
-					 freq->frequency);
-				freq->frequency = CPUFREQ_ENTRY_INVALID;
+					 pos->frequency);
+				pos->frequency = CPUFREQ_ENTRY_INVALID;
 			} else {
-				freq++;
 				continue;
 			}
 		}
 
 		/* Check for frequencies we can generate */
 		rate = clk_round_rate(s3c_freq->armdiv,
-				      freq->frequency * 1000);
+				      pos->frequency * 1000);
 		rate /= 1000;
-		if (rate != freq->frequency) {
+		if (rate != pos->frequency) {
 			pr_debug("cpufreq: %dkHz unsupported by clock (clk_round_rate return %lu)\n",
-				 freq->frequency, rate);
-			freq->frequency = CPUFREQ_ENTRY_INVALID;
+				pos->frequency, rate);
+			pos->frequency = CPUFREQ_ENTRY_INVALID;
 		}
-
-		freq++;
 	}
 
 	/* Datasheet says PLL stabalisation time must be at least 300us,
diff --git a/drivers/cpufreq/s3c24xx-cpufreq.c b/drivers/cpufreq/s3c24xx-cpufreq.c
index be1b2b5c975..227ebf7c1ee 100644
--- a/drivers/cpufreq/s3c24xx-cpufreq.c
+++ b/drivers/cpufreq/s3c24xx-cpufreq.c
@@ -141,6 +141,7 @@ static int s3c_cpufreq_calcdivs(struct s3c_cpufreq_config *cfg)
 
 static void s3c_cpufreq_setfvco(struct s3c_cpufreq_config *cfg)
 {
+	cfg->mpll = _clk_mpll;
 	(cfg->info->set_fvco)(cfg);
 }
 
diff --git a/drivers/cpufreq/s3c64xx-cpufreq.c b/drivers/cpufreq/s3c64xx-cpufreq.c
index ff7d3ecb85f..176e84cc399 100644
--- a/drivers/cpufreq/s3c64xx-cpufreq.c
+++ b/drivers/cpufreq/s3c64xx-cpufreq.c
@@ -118,11 +118,10 @@ static void __init s3c64xx_cpufreq_config_regulator(void)
 		pr_err("Unable to check supported voltages\n");
 	}
 
-	freq = s3c64xx_freq_table;
-	while (count > 0 && freq->frequency != CPUFREQ_TABLE_END) {
-		if (freq->frequency == CPUFREQ_ENTRY_INVALID)
-			continue;
+	if (!count)
+		goto out;
 
+	cpufreq_for_each_valid_entry(freq, s3c64xx_freq_table) {
 		dvfs = &s3c64xx_dvfs_table[freq->driver_data];
 		found = 0;
 
@@ -137,10 +136,9 @@ static void __init s3c64xx_cpufreq_config_regulator(void)
 				 freq->frequency);
 			freq->frequency = CPUFREQ_ENTRY_INVALID;
 		}
-
-		freq++;
 	}
 
+out:
 	/* Guess based on having to do an I2C/SPI write; in future we
 	 * will be able to query the regulator performance here. */
 	regulator_latency = 1 * 1000 * 1000;
@@ -179,8 +177,7 @@ static int s3c64xx_cpufreq_driver_init(struct cpufreq_policy *policy)
 	}
 #endif
 
-	freq = s3c64xx_freq_table;
-	while (freq->frequency != CPUFREQ_TABLE_END) {
+	cpufreq_for_each_entry(freq, s3c64xx_freq_table) {
 		unsigned long r;
 
 		/* Check for frequencies we can generate */
@@ -196,8 +193,6 @@ static int s3c64xx_cpufreq_driver_init(struct cpufreq_policy *policy)
 		 * frequency is the maximum we can support. */
 		if (!vddarm && freq->frequency > clk_get_rate(policy->clk) / 1000)
 			freq->frequency = CPUFREQ_ENTRY_INVALID;
-
-		freq++;
 	}
 
 	/* Datasheet says PLL stabalisation time (if we were to use
diff --git a/drivers/cpufreq/s5pv210-cpufreq.c b/drivers/cpufreq/s5pv210-cpufreq.c
index ab2c1a40d43..19a10b89fef 100644
--- a/drivers/cpufreq/s5pv210-cpufreq.c
+++ b/drivers/cpufreq/s5pv210-cpufreq.c
@@ -175,10 +175,8 @@ static int s5pv210_target(struct cpufreq_policy *policy, unsigned int index)
 	mutex_lock(&set_freq_lock);
 
 	if (no_cpufreq_access) {
-#ifdef CONFIG_PM_VERBOSE
-		pr_err("%s:%d denied access to %s as it is disabled"
-				"temporarily\n", __FILE__, __LINE__, __func__);
-#endif
+		pr_err("Denied access to %s as it is disabled temporarily\n",
+		       __func__);
 		ret = -EINVAL;
 		goto exit;
 	}
diff --git a/drivers/cpufreq/speedstep-centrino.c b/drivers/cpufreq/speedstep-centrino.c
index 6723f0390f2..7d4a3157160 100644
--- a/drivers/cpufreq/speedstep-centrino.c
+++ b/drivers/cpufreq/speedstep-centrino.c
@@ -28,7 +28,7 @@
 #include <asm/cpu_device_id.h>
 
 #define PFX		"speedstep-centrino: "
-#define MAINTAINER	"cpufreq@vger.kernel.org"
+#define MAINTAINER	"linux-pm@vger.kernel.org"
 
 #define INTEL_MSR_RANGE	(0xffff)
 
diff --git a/drivers/cpufreq/tegra-cpufreq.c b/drivers/cpufreq/tegra-cpufreq.c
index 63f00598a25..6e774c6ac20 100644
--- a/drivers/cpufreq/tegra-cpufreq.c
+++ b/drivers/cpufreq/tegra-cpufreq.c
@@ -82,9 +82,9 @@ out:
 	return ret;
 }
 
-static int tegra_update_cpu_speed(struct cpufreq_policy *policy,
-		unsigned long rate)
+static int tegra_target(struct cpufreq_policy *policy, unsigned int index)
 {
+	unsigned long rate = freq_table[index].frequency;
 	int ret = 0;
 
 	/*
@@ -106,11 +106,6 @@ static int tegra_update_cpu_speed(struct cpufreq_policy *policy,
 	return ret;
 }
 
-static int tegra_target(struct cpufreq_policy *policy, unsigned int index)
-{
-	return tegra_update_cpu_speed(policy, freq_table[index].frequency);
-}
-
 static int tegra_cpu_init(struct cpufreq_policy *policy)
 {
 	int ret;
diff --git a/drivers/cpuidle/Kconfig b/drivers/cpuidle/Kconfig
index f04e25f6c98..1b96fb91d32 100644
--- a/drivers/cpuidle/Kconfig
+++ b/drivers/cpuidle/Kconfig
@@ -35,6 +35,11 @@ depends on ARM
 source "drivers/cpuidle/Kconfig.arm"
 endmenu
 
+menu "MIPS CPU Idle Drivers"
+depends on MIPS
+source "drivers/cpuidle/Kconfig.mips"
+endmenu
+
 menu "POWERPC CPU Idle Drivers"
 depends on PPC
 source "drivers/cpuidle/Kconfig.powerpc"
diff --git a/drivers/cpuidle/Kconfig.arm b/drivers/cpuidle/Kconfig.arm
index 97ccc31dbdd..b6d69e899f5 100644
--- a/drivers/cpuidle/Kconfig.arm
+++ b/drivers/cpuidle/Kconfig.arm
@@ -1,6 +1,11 @@
 #
 # ARM CPU Idle drivers
 #
+config ARM_ARMADA_370_XP_CPUIDLE
+	bool "CPU Idle Driver for Armada 370/XP family processors"
+	depends on ARCH_MVEBU
+	help
+	  Select this to enable cpuidle on Armada 370/XP processors.
 
 config ARM_BIG_LITTLE_CPUIDLE
 	bool "Support for ARM big.LITTLE processors"
@@ -13,6 +18,12 @@ config ARM_BIG_LITTLE_CPUIDLE
 	  define different C-states for little and big cores through the
 	  multiple CPU idle drivers infrastructure.
 
+config ARM_CLPS711X_CPUIDLE
+	bool "CPU Idle Driver for CLPS711X processors"
+	depends on ARCH_CLPS711X || COMPILE_TEST
+	help
+	  Select this to enable cpuidle on Cirrus Logic CLPS711X SOCs.
+
 config ARM_HIGHBANK_CPUIDLE
 	bool "CPU Idle Driver for Calxeda processors"
 	depends on ARM_PSCI
@@ -44,3 +55,9 @@ config ARM_AT91_CPUIDLE
 	depends on ARCH_AT91
 	help
 	  Select this to enable cpuidle for AT91 processors
+
+config ARM_EXYNOS_CPUIDLE
+	bool "Cpu Idle Driver for the Exynos processors"
+	depends on ARCH_EXYNOS
+	help
+	  Select this to enable cpuidle for Exynos processors
diff --git a/drivers/cpuidle/Kconfig.mips b/drivers/cpuidle/Kconfig.mips
new file mode 100644
index 00000000000..0e70ee28a5c
--- /dev/null
+++ b/drivers/cpuidle/Kconfig.mips
@@ -0,0 +1,17 @@
+#
+# MIPS CPU Idle Drivers
+#
+config MIPS_CPS_CPUIDLE
+	bool "CPU Idle driver for MIPS CPS platforms"
+	depends on CPU_IDLE
+	depends on SYS_SUPPORTS_MIPS_CPS
+	select ARCH_NEEDS_CPU_IDLE_COUPLED if MIPS_MT
+	select GENERIC_CLOCKEVENTS_BROADCAST if SMP
+	select MIPS_CPS_PM
+	default y
+	help
+	  Select this option to enable processor idle state management
+	  through cpuidle for systems built around the MIPS Coherent
+	  Processing System (CPS) architecture. In order to make use of
+	  the deepest idle states you will need to ensure that you are
+	  also using the CONFIG_MIPS_CPS SMP implementation.
diff --git a/drivers/cpuidle/Makefile b/drivers/cpuidle/Makefile
index f71ae1b373c..d8bb1ff7256 100644
--- a/drivers/cpuidle/Makefile
+++ b/drivers/cpuidle/Makefile
@@ -7,12 +7,19 @@ obj-$(CONFIG_ARCH_NEEDS_CPU_IDLE_COUPLED) += coupled.o
 
 ##################################################################################
 # ARM SoC drivers
+obj-$(CONFIG_ARM_ARMADA_370_XP_CPUIDLE) += cpuidle-armada-370-xp.o
 obj-$(CONFIG_ARM_BIG_LITTLE_CPUIDLE)	+= cpuidle-big_little.o
+obj-$(CONFIG_ARM_CLPS711X_CPUIDLE)	+= cpuidle-clps711x.o
 obj-$(CONFIG_ARM_HIGHBANK_CPUIDLE)	+= cpuidle-calxeda.o
 obj-$(CONFIG_ARM_KIRKWOOD_CPUIDLE)	+= cpuidle-kirkwood.o
 obj-$(CONFIG_ARM_ZYNQ_CPUIDLE)		+= cpuidle-zynq.o
 obj-$(CONFIG_ARM_U8500_CPUIDLE)         += cpuidle-ux500.o
 obj-$(CONFIG_ARM_AT91_CPUIDLE)          += cpuidle-at91.o
+obj-$(CONFIG_ARM_EXYNOS_CPUIDLE)        += cpuidle-exynos.o
+
+###############################################################################
+# MIPS drivers
+obj-$(CONFIG_MIPS_CPS_CPUIDLE)		+= cpuidle-cps.o
 
 ###############################################################################
 # POWERPC drivers
diff --git a/drivers/cpuidle/coupled.c b/drivers/cpuidle/coupled.c
index cb6654bfad7..73fe2f8d7f9 100644
--- a/drivers/cpuidle/coupled.c
+++ b/drivers/cpuidle/coupled.c
@@ -159,7 +159,7 @@ void cpuidle_coupled_parallel_barrier(struct cpuidle_device *dev, atomic_t *a)
 {
 	int n = dev->coupled->online_count;
 
-	smp_mb__before_atomic_inc();
+	smp_mb__before_atomic();
 	atomic_inc(a);
 
 	while (atomic_read(a) < n)
diff --git a/drivers/cpuidle/cpuidle-armada-370-xp.c b/drivers/cpuidle/cpuidle-armada-370-xp.c
new file mode 100644
index 00000000000..28587d0f394
--- /dev/null
+++ b/drivers/cpuidle/cpuidle-armada-370-xp.c
@@ -0,0 +1,93 @@
+/*
+ * Marvell Armada 370 and Armada XP SoC cpuidle driver
+ *
+ * Copyright (C) 2014 Marvell
+ *
+ * Nadav Haklai <nadavh@marvell.com>
+ * Gregory CLEMENT <gregory.clement@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ *
+ * Maintainer: Gregory CLEMENT <gregory.clement@free-electrons.com>
+ */
+
+#include <linux/cpu_pm.h>
+#include <linux/cpuidle.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/suspend.h>
+#include <linux/platform_device.h>
+#include <asm/cpuidle.h>
+
+#define ARMADA_370_XP_MAX_STATES	3
+#define ARMADA_370_XP_FLAG_DEEP_IDLE	0x10000
+
+static int (*armada_370_xp_cpu_suspend)(int);
+
+static int armada_370_xp_enter_idle(struct cpuidle_device *dev,
+				struct cpuidle_driver *drv,
+				int index)
+{
+	int ret;
+	bool deepidle = false;
+	cpu_pm_enter();
+
+	if (drv->states[index].flags & ARMADA_370_XP_FLAG_DEEP_IDLE)
+		deepidle = true;
+
+	ret = armada_370_xp_cpu_suspend(deepidle);
+	if (ret)
+		return ret;
+
+	cpu_pm_exit();
+
+	return index;
+}
+
+static struct cpuidle_driver armada_370_xp_idle_driver = {
+	.name			= "armada_370_xp_idle",
+	.states[0]		= ARM_CPUIDLE_WFI_STATE,
+	.states[1]		= {
+		.enter			= armada_370_xp_enter_idle,
+		.exit_latency		= 10,
+		.power_usage		= 50,
+		.target_residency	= 100,
+		.flags			= CPUIDLE_FLAG_TIME_VALID,
+		.name			= "MV CPU IDLE",
+		.desc			= "CPU power down",
+	},
+	.states[2]		= {
+		.enter			= armada_370_xp_enter_idle,
+		.exit_latency		= 100,
+		.power_usage		= 5,
+		.target_residency	= 1000,
+		.flags			= CPUIDLE_FLAG_TIME_VALID |
+						ARMADA_370_XP_FLAG_DEEP_IDLE,
+		.name			= "MV CPU DEEP IDLE",
+		.desc			= "CPU and L2 Fabric power down",
+	},
+	.state_count = ARMADA_370_XP_MAX_STATES,
+};
+
+static int armada_370_xp_cpuidle_probe(struct platform_device *pdev)
+{
+
+	armada_370_xp_cpu_suspend = (void *)(pdev->dev.platform_data);
+	return cpuidle_register(&armada_370_xp_idle_driver, NULL);
+}
+
+static struct platform_driver armada_370_xp_cpuidle_plat_driver = {
+	.driver = {
+		.name = "cpuidle-armada-370-xp",
+		.owner = THIS_MODULE,
+	},
+	.probe = armada_370_xp_cpuidle_probe,
+};
+
+module_platform_driver(armada_370_xp_cpuidle_plat_driver);
+
+MODULE_AUTHOR("Gregory CLEMENT <gregory.clement@free-electrons.com>");
+MODULE_DESCRIPTION("Armada 370/XP cpu idle driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/cpuidle/cpuidle-clps711x.c b/drivers/cpuidle/cpuidle-clps711x.c
new file mode 100644
index 00000000000..5243811daa6
--- /dev/null
+++ b/drivers/cpuidle/cpuidle-clps711x.c
@@ -0,0 +1,64 @@
+/*
+ *  CLPS711X CPU idle driver
+ *
+ *  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/cpuidle.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+
+#define CLPS711X_CPUIDLE_NAME	"clps711x-cpuidle"
+
+static void __iomem *clps711x_halt;
+
+static int clps711x_cpuidle_halt(struct cpuidle_device *dev,
+				 struct cpuidle_driver *drv, int index)
+{
+	writel(0xaa, clps711x_halt);
+
+	return index;
+}
+
+static struct cpuidle_driver clps711x_idle_driver = {
+	.name		= CLPS711X_CPUIDLE_NAME,
+	.owner		= THIS_MODULE,
+	.states[0]	= {
+		.name		= "HALT",
+		.desc		= "CLPS711X HALT",
+		.enter		= clps711x_cpuidle_halt,
+		.exit_latency	= 1,
+	},
+	.state_count	= 1,
+};
+
+static int __init clps711x_cpuidle_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	clps711x_halt = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(clps711x_halt))
+		return PTR_ERR(clps711x_halt);
+
+	return cpuidle_register(&clps711x_idle_driver, NULL);
+}
+
+static struct platform_driver clps711x_cpuidle_driver = {
+	.driver	= {
+		.name	= CLPS711X_CPUIDLE_NAME,
+		.owner	= THIS_MODULE,
+	},
+};
+module_platform_driver_probe(clps711x_cpuidle_driver, clps711x_cpuidle_probe);
+
+MODULE_AUTHOR("Alexander Shiyan <shc_work@mail.ru>");
+MODULE_DESCRIPTION("CLPS711X CPU idle driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/cpuidle/cpuidle-cps.c b/drivers/cpuidle/cpuidle-cps.c
new file mode 100644
index 00000000000..fc7b62720de
--- /dev/null
+++ b/drivers/cpuidle/cpuidle-cps.c
@@ -0,0 +1,186 @@
+/*
+ * Copyright (C) 2014 Imagination Technologies
+ * Author: Paul Burton <paul.burton@imgtec.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/cpu_pm.h>
+#include <linux/cpuidle.h>
+#include <linux/init.h>
+
+#include <asm/idle.h>
+#include <asm/pm-cps.h>
+
+/* Enumeration of the various idle states this driver may enter */
+enum cps_idle_state {
+	STATE_WAIT = 0,		/* MIPS wait instruction, coherent */
+	STATE_NC_WAIT,		/* MIPS wait instruction, non-coherent */
+	STATE_CLOCK_GATED,	/* Core clock gated */
+	STATE_POWER_GATED,	/* Core power gated */
+	STATE_COUNT
+};
+
+static int cps_nc_enter(struct cpuidle_device *dev,
+			struct cpuidle_driver *drv, int index)
+{
+	enum cps_pm_state pm_state;
+	int err;
+
+	/*
+	 * At least one core must remain powered up & clocked in order for the
+	 * system to have any hope of functioning.
+	 *
+	 * TODO: don't treat core 0 specially, just prevent the final core
+	 * TODO: remap interrupt affinity temporarily
+	 */
+	if (!cpu_data[dev->cpu].core && (index > STATE_NC_WAIT))
+		index = STATE_NC_WAIT;
+
+	/* Select the appropriate cps_pm_state */
+	switch (index) {
+	case STATE_NC_WAIT:
+		pm_state = CPS_PM_NC_WAIT;
+		break;
+	case STATE_CLOCK_GATED:
+		pm_state = CPS_PM_CLOCK_GATED;
+		break;
+	case STATE_POWER_GATED:
+		pm_state = CPS_PM_POWER_GATED;
+		break;
+	default:
+		BUG();
+		return -EINVAL;
+	}
+
+	/* Notify listeners the CPU is about to power down */
+	if ((pm_state == CPS_PM_POWER_GATED) && cpu_pm_enter())
+		return -EINTR;
+
+	/* Enter that state */
+	err = cps_pm_enter_state(pm_state);
+
+	/* Notify listeners the CPU is back up */
+	if (pm_state == CPS_PM_POWER_GATED)
+		cpu_pm_exit();
+
+	return err ?: index;
+}
+
+static struct cpuidle_driver cps_driver = {
+	.name			= "cpc_cpuidle",
+	.owner			= THIS_MODULE,
+	.states = {
+		[STATE_WAIT] = MIPS_CPUIDLE_WAIT_STATE,
+		[STATE_NC_WAIT] = {
+			.enter	= cps_nc_enter,
+			.exit_latency		= 200,
+			.target_residency	= 450,
+			.flags	= CPUIDLE_FLAG_TIME_VALID,
+			.name	= "nc-wait",
+			.desc	= "non-coherent MIPS wait",
+		},
+		[STATE_CLOCK_GATED] = {
+			.enter	= cps_nc_enter,
+			.exit_latency		= 300,
+			.target_residency	= 700,
+			.flags	= CPUIDLE_FLAG_TIME_VALID |
+				  CPUIDLE_FLAG_TIMER_STOP,
+			.name	= "clock-gated",
+			.desc	= "core clock gated",
+		},
+		[STATE_POWER_GATED] = {
+			.enter	= cps_nc_enter,
+			.exit_latency		= 600,
+			.target_residency	= 1000,
+			.flags	= CPUIDLE_FLAG_TIME_VALID |
+				  CPUIDLE_FLAG_TIMER_STOP,
+			.name	= "power-gated",
+			.desc	= "core power gated",
+		},
+	},
+	.state_count		= STATE_COUNT,
+	.safe_state_index	= 0,
+};
+
+static void __init cps_cpuidle_unregister(void)
+{
+	int cpu;
+	struct cpuidle_device *device;
+
+	for_each_possible_cpu(cpu) {
+		device = &per_cpu(cpuidle_dev, cpu);
+		cpuidle_unregister_device(device);
+	}
+
+	cpuidle_unregister_driver(&cps_driver);
+}
+
+static int __init cps_cpuidle_init(void)
+{
+	int err, cpu, core, i;
+	struct cpuidle_device *device;
+
+	/* Detect supported states */
+	if (!cps_pm_support_state(CPS_PM_POWER_GATED))
+		cps_driver.state_count = STATE_CLOCK_GATED + 1;
+	if (!cps_pm_support_state(CPS_PM_CLOCK_GATED))
+		cps_driver.state_count = STATE_NC_WAIT + 1;
+	if (!cps_pm_support_state(CPS_PM_NC_WAIT))
+		cps_driver.state_count = STATE_WAIT + 1;
+
+	/* Inform the user if some states are unavailable */
+	if (cps_driver.state_count < STATE_COUNT) {
+		pr_info("cpuidle-cps: limited to ");
+		switch (cps_driver.state_count - 1) {
+		case STATE_WAIT:
+			pr_cont("coherent wait\n");
+			break;
+		case STATE_NC_WAIT:
+			pr_cont("non-coherent wait\n");
+			break;
+		case STATE_CLOCK_GATED:
+			pr_cont("clock gating\n");
+			break;
+		}
+	}
+
+	/*
+	 * Set the coupled flag on the appropriate states if this system
+	 * requires it.
+	 */
+	if (coupled_coherence)
+		for (i = STATE_NC_WAIT; i < cps_driver.state_count; i++)
+			cps_driver.states[i].flags |= CPUIDLE_FLAG_COUPLED;
+
+	err = cpuidle_register_driver(&cps_driver);
+	if (err) {
+		pr_err("Failed to register CPS cpuidle driver\n");
+		return err;
+	}
+
+	for_each_possible_cpu(cpu) {
+		core = cpu_data[cpu].core;
+		device = &per_cpu(cpuidle_dev, cpu);
+		device->cpu = cpu;
+#ifdef CONFIG_MIPS_MT
+		cpumask_copy(&device->coupled_cpus, &cpu_sibling_map[cpu]);
+#endif
+
+		err = cpuidle_register_device(device);
+		if (err) {
+			pr_err("Failed to register CPU%d cpuidle device\n",
+			       cpu);
+			goto err_out;
+		}
+	}
+
+	return 0;
+err_out:
+	cps_cpuidle_unregister();
+	return err;
+}
+device_initcall(cps_cpuidle_init);
diff --git a/drivers/cpuidle/cpuidle-exynos.c b/drivers/cpuidle/cpuidle-exynos.c
new file mode 100644
index 00000000000..7c015126382
--- /dev/null
+++ b/drivers/cpuidle/cpuidle-exynos.c
@@ -0,0 +1,99 @@
+/* linux/arch/arm/mach-exynos/cpuidle.c
+ *
+ * Copyright (c) 2011 Samsung Electronics Co., Ltd.
+ *		http://www.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.
+*/
+
+#include <linux/cpuidle.h>
+#include <linux/cpu_pm.h>
+#include <linux/export.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+
+#include <asm/proc-fns.h>
+#include <asm/suspend.h>
+#include <asm/cpuidle.h>
+
+static void (*exynos_enter_aftr)(void);
+
+static int idle_finisher(unsigned long flags)
+{
+	exynos_enter_aftr();
+	cpu_do_idle();
+
+	return 1;
+}
+
+static int exynos_enter_core0_aftr(struct cpuidle_device *dev,
+				struct cpuidle_driver *drv,
+				int index)
+{
+	cpu_pm_enter();
+	cpu_suspend(0, idle_finisher);
+	cpu_pm_exit();
+
+	return index;
+}
+
+static int exynos_enter_lowpower(struct cpuidle_device *dev,
+				struct cpuidle_driver *drv,
+				int index)
+{
+	int new_index = index;
+
+	/* AFTR can only be entered when cores other than CPU0 are offline */
+	if (num_online_cpus() > 1 || dev->cpu != 0)
+		new_index = drv->safe_state_index;
+
+	if (new_index == 0)
+		return arm_cpuidle_simple_enter(dev, drv, new_index);
+	else
+		return exynos_enter_core0_aftr(dev, drv, new_index);
+}
+
+static struct cpuidle_driver exynos_idle_driver = {
+	.name			= "exynos_idle",
+	.owner			= THIS_MODULE,
+	.states = {
+		[0] = ARM_CPUIDLE_WFI_STATE,
+		[1] = {
+			.enter			= exynos_enter_lowpower,
+			.exit_latency		= 300,
+			.target_residency	= 100000,
+			.flags			= CPUIDLE_FLAG_TIME_VALID,
+			.name			= "C1",
+			.desc			= "ARM power down",
+		},
+	},
+	.state_count = 2,
+	.safe_state_index = 0,
+};
+
+static int exynos_cpuidle_probe(struct platform_device *pdev)
+{
+	int ret;
+
+	exynos_enter_aftr = (void *)(pdev->dev.platform_data);
+
+	ret = cpuidle_register(&exynos_idle_driver, NULL);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to register cpuidle driver\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static struct platform_driver exynos_cpuidle_driver = {
+	.probe	= exynos_cpuidle_probe,
+	.driver = {
+		.name = "exynos_cpuidle",
+		.owner = THIS_MODULE,
+	},
+};
+
+module_platform_driver(exynos_cpuidle_driver);
diff --git a/drivers/cpuidle/cpuidle.c b/drivers/cpuidle/cpuidle.c
index 8236746e46b..cb7019977c5 100644
--- a/drivers/cpuidle/cpuidle.c
+++ b/drivers/cpuidle/cpuidle.c
@@ -32,6 +32,7 @@ LIST_HEAD(cpuidle_detected_devices);
 static int enabled_devices;
 static int off __read_mostly;
 static int initialized __read_mostly;
+static bool use_deepest_state __read_mostly;
 
 int cpuidle_disabled(void)
 {
@@ -65,23 +66,42 @@ int cpuidle_play_dead(void)
 }
 
 /**
- * cpuidle_enabled - check if the cpuidle framework is ready
- * @dev: cpuidle device for this cpu
- * @drv: cpuidle driver for this cpu
+ * cpuidle_use_deepest_state - Enable/disable the "deepest idle" mode.
+ * @enable: Whether enable or disable the feature.
+ *
+ * If the "deepest idle" mode is enabled, cpuidle will ignore the governor and
+ * always use the state with the greatest exit latency (out of the states that
+ * are not disabled).
  *
- * Return 0 on success, otherwise:
- * -NODEV : the cpuidle framework is not available
- * -EBUSY : the cpuidle framework is not initialized
+ * This function can only be called after cpuidle_pause() to avoid races.
  */
-int cpuidle_enabled(struct cpuidle_driver *drv, struct cpuidle_device *dev)
+void cpuidle_use_deepest_state(bool enable)
 {
-	if (off || !initialized)
-		return -ENODEV;
+	use_deepest_state = enable;
+}
 
-	if (!drv || !dev || !dev->enabled)
-		return -EBUSY;
+/**
+ * cpuidle_find_deepest_state - Find the state of the greatest exit latency.
+ * @drv: cpuidle driver for a given CPU.
+ * @dev: cpuidle device for a given CPU.
+ */
+static int cpuidle_find_deepest_state(struct cpuidle_driver *drv,
+				      struct cpuidle_device *dev)
+{
+	unsigned int latency_req = 0;
+	int i, ret = CPUIDLE_DRIVER_STATE_START - 1;
 
-	return 0;
+	for (i = CPUIDLE_DRIVER_STATE_START; i < drv->state_count; i++) {
+		struct cpuidle_state *s = &drv->states[i];
+		struct cpuidle_state_usage *su = &dev->states_usage[i];
+
+		if (s->disabled || su->disable || s->exit_latency <= latency_req)
+			continue;
+
+		latency_req = s->exit_latency;
+		ret = i;
+	}
+	return ret;
 }
 
 /**
@@ -138,6 +158,15 @@ int cpuidle_enter_state(struct cpuidle_device *dev, struct cpuidle_driver *drv,
  */
 int cpuidle_select(struct cpuidle_driver *drv, struct cpuidle_device *dev)
 {
+	if (off || !initialized)
+		return -ENODEV;
+
+	if (!drv || !dev || !dev->enabled)
+		return -EBUSY;
+
+	if (unlikely(use_deepest_state))
+		return cpuidle_find_deepest_state(drv, dev);
+
 	return cpuidle_curr_governor->select(drv, dev);
 }
 
@@ -169,7 +198,7 @@ int cpuidle_enter(struct cpuidle_driver *drv, struct cpuidle_device *dev,
  */
 void cpuidle_reflect(struct cpuidle_device *dev, int index)
 {
-	if (cpuidle_curr_governor->reflect)
+	if (cpuidle_curr_governor->reflect && !unlikely(use_deepest_state))
 		cpuidle_curr_governor->reflect(dev, index);
 }
 
diff --git a/drivers/cpuidle/governors/menu.c b/drivers/cpuidle/governors/menu.c
index 71b52329335..c4f80c15a48 100644
--- a/drivers/cpuidle/governors/menu.c
+++ b/drivers/cpuidle/governors/menu.c
@@ -296,7 +296,7 @@ static int menu_select(struct cpuidle_driver *drv, struct cpuidle_device *dev)
 		data->needs_update = 0;
 	}
 
-	data->last_state_idx = 0;
+	data->last_state_idx = CPUIDLE_DRIVER_STATE_START - 1;
 
 	/* Special case when user has set very strict latency requirement */
 	if (unlikely(latency_req == 0))
@@ -311,13 +311,6 @@ static int menu_select(struct cpuidle_driver *drv, struct cpuidle_device *dev)
 	data->bucket = which_bucket(data->next_timer_us);
 
 	/*
-	 * if the correction factor is 0 (eg first time init or cpu hotplug
-	 * etc), we actually want to start out with a unity factor.
-	 */
-	if (data->correction_factor[data->bucket] == 0)
-		data->correction_factor[data->bucket] = RESOLUTION * DECAY;
-
-	/*
 	 * Force the result of multiplication to be 64 bits even if both
 	 * operands are 32 bits.
 	 * Make sure to round up for half microseconds.
@@ -466,9 +459,17 @@ static int menu_enable_device(struct cpuidle_driver *drv,
 				struct cpuidle_device *dev)
 {
 	struct menu_device *data = &per_cpu(menu_devices, dev->cpu);
+	int i;
 
 	memset(data, 0, sizeof(struct menu_device));
 
+	/*
+	 * if the correction factor is 0 (eg first time init or cpu hotplug
+	 * etc), we actually want to start out with a unity factor.
+	 */
+	for(i = 0; i < BUCKETS; i++)
+		data->correction_factor[i] = RESOLUTION * DECAY;
+
 	return 0;
 }
 
diff --git a/drivers/crypto/Kconfig b/drivers/crypto/Kconfig
index 03ccdb0ccf9..f066fa23cc0 100644
--- a/drivers/crypto/Kconfig
+++ b/drivers/crypto/Kconfig
@@ -301,14 +301,14 @@ config CRYPTO_DEV_SAHARA
 	  found in some Freescale i.MX chips.
 
 config CRYPTO_DEV_S5P
-	tristate "Support for Samsung S5PV210 crypto accelerator"
-	depends on ARCH_S5PV210
+	tristate "Support for Samsung S5PV210/Exynos crypto accelerator"
+	depends on ARCH_S5PV210 || ARCH_EXYNOS
 	select CRYPTO_AES
 	select CRYPTO_ALGAPI
 	select CRYPTO_BLKCIPHER
 	help
 	  This option allows you to have support for S5P crypto acceleration.
-	  Select this to offload Samsung S5PV210 or S5PC110 from AES
+	  Select this to offload Samsung S5PV210 or S5PC110, Exynos from AES
 	  algorithms execution.
 
 config CRYPTO_DEV_NX
diff --git a/drivers/crypto/atmel-aes.c b/drivers/crypto/atmel-aes.c
index d7c9e317423..a083474991a 100644
--- a/drivers/crypto/atmel-aes.c
+++ b/drivers/crypto/atmel-aes.c
@@ -716,6 +716,12 @@ static int atmel_aes_crypt(struct ablkcipher_request *req, unsigned long mode)
 			return -EINVAL;
 		}
 		ctx->block_size = CFB32_BLOCK_SIZE;
+	} else if (mode & AES_FLAGS_CFB64) {
+		if (!IS_ALIGNED(req->nbytes, CFB64_BLOCK_SIZE)) {
+			pr_err("request size is not exact amount of CFB64 blocks\n");
+			return -EINVAL;
+		}
+		ctx->block_size = CFB64_BLOCK_SIZE;
 	} else {
 		if (!IS_ALIGNED(req->nbytes, AES_BLOCK_SIZE)) {
 			pr_err("request size is not exact amount of AES blocks\n");
@@ -1069,7 +1075,7 @@ static struct crypto_alg aes_algs[] = {
 	.cra_driver_name	= "atmel-cfb8-aes",
 	.cra_priority		= 100,
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
-	.cra_blocksize		= CFB64_BLOCK_SIZE,
+	.cra_blocksize		= CFB8_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
 	.cra_alignmask		= 0x0,
 	.cra_type		= &crypto_ablkcipher_type,
diff --git a/drivers/crypto/bfin_crc.c b/drivers/crypto/bfin_crc.c
index c9ff298e6d2..b099e33cb07 100644
--- a/drivers/crypto/bfin_crc.c
+++ b/drivers/crypto/bfin_crc.c
@@ -29,10 +29,11 @@
 #include <crypto/hash.h>
 #include <crypto/internal/hash.h>
 
-#include <asm/blackfin.h>
-#include <asm/bfin_crc.h>
 #include <asm/dma.h>
 #include <asm/portmux.h>
+#include <asm/io.h>
+
+#include "bfin_crc.h"
 
 #define CRC_CCRYPTO_QUEUE_LENGTH	5
 
@@ -54,12 +55,13 @@ struct bfin_crypto_crc {
 	int			irq;
 	int			dma_ch;
 	u32			poly;
-	volatile struct crc_register *regs;
+	struct crc_register	*regs;
 
 	struct ahash_request	*req; /* current request in operation */
 	struct dma_desc_array	*sg_cpu; /* virt addr of sg dma descriptors */
 	dma_addr_t		sg_dma; /* phy addr of sg dma descriptors */
 	u8			*sg_mid_buf;
+	dma_addr_t		sg_mid_dma; /* phy addr of sg mid buffer */
 
 	struct tasklet_struct	done_task;
 	struct crypto_queue	queue; /* waiting requests */
@@ -132,13 +134,13 @@ static struct scatterlist *sg_get(struct scatterlist *sg_list, unsigned int nent
 
 static int bfin_crypto_crc_init_hw(struct bfin_crypto_crc *crc, u32 key)
 {
-	crc->regs->datacntrld = 0;
-	crc->regs->control = MODE_CALC_CRC << OPMODE_OFFSET;
-	crc->regs->curresult = key;
+	writel(0, &crc->regs->datacntrld);
+	writel(MODE_CALC_CRC << OPMODE_OFFSET, &crc->regs->control);
+	writel(key, &crc->regs->curresult);
 
 	/* setup CRC interrupts */
-	crc->regs->status = CMPERRI | DCNTEXPI;
-	crc->regs->intrenset = CMPERRI | DCNTEXPI;
+	writel(CMPERRI | DCNTEXPI, &crc->regs->status);
+	writel(CMPERRI | DCNTEXPI, &crc->regs->intrenset);
 
 	return 0;
 }
@@ -194,7 +196,6 @@ static void bfin_crypto_crc_config_dma(struct bfin_crypto_crc *crc)
 	dma_map_sg(crc->dev, ctx->sg, ctx->sg_nents, DMA_TO_DEVICE);
 
 	for_each_sg(ctx->sg, sg, ctx->sg_nents, j) {
-		dma_config = DMAFLOW_ARRAY | RESTART | NDSIZE_3 | DMAEN | PSIZE_32;
 		dma_addr = sg_dma_address(sg);
 		/* deduce extra bytes in last sg */
 		if (sg_is_last(sg))
@@ -207,12 +208,29 @@ static void bfin_crypto_crc_config_dma(struct bfin_crypto_crc *crc)
 			   bytes in current sg buffer. Move addr of current
 			   sg and deduce the length of current sg.
 			 */
-			memcpy(crc->sg_mid_buf +((i-1) << 2) + mid_dma_count,
-				(void *)dma_addr,
+			memcpy(crc->sg_mid_buf +(i << 2) + mid_dma_count,
+				sg_virt(sg),
 				CHKSUM_DIGEST_SIZE - mid_dma_count);
 			dma_addr += CHKSUM_DIGEST_SIZE - mid_dma_count;
 			dma_count -= CHKSUM_DIGEST_SIZE - mid_dma_count;
+
+			dma_config = DMAFLOW_ARRAY | RESTART | NDSIZE_3 |
+				DMAEN | PSIZE_32 | WDSIZE_32;
+
+			/* setup new dma descriptor for next middle dma */
+			crc->sg_cpu[i].start_addr = crc->sg_mid_dma + (i << 2);
+			crc->sg_cpu[i].cfg = dma_config;
+			crc->sg_cpu[i].x_count = 1;
+			crc->sg_cpu[i].x_modify = CHKSUM_DIGEST_SIZE;
+			dev_dbg(crc->dev, "%d: crc_dma: start_addr:0x%lx, "
+				"cfg:0x%lx, x_count:0x%lx, x_modify:0x%lx\n",
+				i, crc->sg_cpu[i].start_addr,
+				crc->sg_cpu[i].cfg, crc->sg_cpu[i].x_count,
+				crc->sg_cpu[i].x_modify);
+			i++;
 		}
+
+		dma_config = DMAFLOW_ARRAY | RESTART | NDSIZE_3 | DMAEN | PSIZE_32;
 		/* chop current sg dma len to multiple of 32 bits */
 		mid_dma_count = dma_count % 4;
 		dma_count &= ~0x3;
@@ -243,24 +261,9 @@ static void bfin_crypto_crc_config_dma(struct bfin_crypto_crc *crc)
 
 		if (mid_dma_count) {
 			/* copy extra bytes to next middle dma buffer */
-			dma_config = DMAFLOW_ARRAY | RESTART | NDSIZE_3 |
-				DMAEN | PSIZE_32 | WDSIZE_32;
 			memcpy(crc->sg_mid_buf + (i << 2),
-				(void *)(dma_addr + (dma_count << 2)),
+				(u8*)sg_virt(sg) + (dma_count << 2),
 				mid_dma_count);
-			/* setup new dma descriptor for next middle dma */
-			crc->sg_cpu[i].start_addr = dma_map_single(crc->dev,
-					crc->sg_mid_buf + (i << 2),
-					CHKSUM_DIGEST_SIZE, DMA_TO_DEVICE);
-			crc->sg_cpu[i].cfg = dma_config;
-			crc->sg_cpu[i].x_count = 1;
-			crc->sg_cpu[i].x_modify = CHKSUM_DIGEST_SIZE;
-			dev_dbg(crc->dev, "%d: crc_dma: start_addr:0x%lx, "
-				"cfg:0x%lx, x_count:0x%lx, x_modify:0x%lx\n",
-				i, crc->sg_cpu[i].start_addr,
-				crc->sg_cpu[i].cfg, crc->sg_cpu[i].x_count,
-				crc->sg_cpu[i].x_modify);
-			i++;
 		}
 	}
 
@@ -303,6 +306,7 @@ static int bfin_crypto_crc_handle_queue(struct bfin_crypto_crc *crc,
 	int nsg, i, j;
 	unsigned int nextlen;
 	unsigned long flags;
+	u32 reg;
 
 	spin_lock_irqsave(&crc->lock, flags);
 	if (req)
@@ -402,13 +406,14 @@ finish_update:
 		ctx->sg_buflen += CHKSUM_DIGEST_SIZE;
 
 	/* set CRC data count before start DMA */
-	crc->regs->datacnt = ctx->sg_buflen >> 2;
+	writel(ctx->sg_buflen >> 2, &crc->regs->datacnt);
 
 	/* setup and enable CRC DMA */
 	bfin_crypto_crc_config_dma(crc);
 
 	/* finally kick off CRC operation */
-	crc->regs->control |= BLKEN;
+	reg = readl(&crc->regs->control);
+	writel(reg | BLKEN, &crc->regs->control);
 
 	return -EINPROGRESS;
 }
@@ -529,14 +534,17 @@ static void bfin_crypto_crc_done_task(unsigned long data)
 static irqreturn_t bfin_crypto_crc_handler(int irq, void *dev_id)
 {
 	struct bfin_crypto_crc *crc = dev_id;
+	u32 reg;
 
-	if (crc->regs->status & DCNTEXP) {
-		crc->regs->status = DCNTEXP;
+	if (readl(&crc->regs->status) & DCNTEXP) {
+		writel(DCNTEXP, &crc->regs->status);
 
 		/* prepare results */
-		put_unaligned_le32(crc->regs->result, crc->req->result);
+		put_unaligned_le32(readl(&crc->regs->result),
+			crc->req->result);
 
-		crc->regs->control &= ~BLKEN;
+		reg = readl(&crc->regs->control);
+		writel(reg & ~BLKEN, &crc->regs->control);
 		crc->busy = 0;
 
 		if (crc->req->base.complete)
@@ -560,7 +568,7 @@ static int bfin_crypto_crc_suspend(struct platform_device *pdev, pm_message_t st
 	struct bfin_crypto_crc *crc = platform_get_drvdata(pdev);
 	int i = 100000;
 
-	while ((crc->regs->control & BLKEN) && --i)
+	while ((readl(&crc->regs->control) & BLKEN) && --i)
 		cpu_relax();
 
 	if (i == 0)
@@ -647,29 +655,32 @@ static int bfin_crypto_crc_probe(struct platform_device *pdev)
 	 * 1 last + 1 next dma descriptors
 	 */
 	crc->sg_mid_buf = (u8 *)(crc->sg_cpu + ((CRC_MAX_DMA_DESC + 1) << 1));
+	crc->sg_mid_dma = crc->sg_dma + sizeof(struct dma_desc_array)
+			* ((CRC_MAX_DMA_DESC + 1) << 1);
 
-	crc->regs->control = 0;
-	crc->regs->poly = crc->poly = (u32)pdev->dev.platform_data;
+	writel(0, &crc->regs->control);
+	crc->poly = (u32)pdev->dev.platform_data;
+	writel(crc->poly, &crc->regs->poly);
 
-	while (!(crc->regs->status & LUTDONE) && (--timeout) > 0)
+	while (!(readl(&crc->regs->status) & LUTDONE) && (--timeout) > 0)
 		cpu_relax();
 
 	if (timeout == 0)
 		dev_info(&pdev->dev, "init crc poly timeout\n");
 
+	platform_set_drvdata(pdev, crc);
+
 	spin_lock(&crc_list.lock);
 	list_add(&crc->list, &crc_list.dev_list);
 	spin_unlock(&crc_list.lock);
 
-	platform_set_drvdata(pdev, crc);
-
-	ret = crypto_register_ahash(&algs);
-	if (ret) {
-		spin_lock(&crc_list.lock);
-		list_del(&crc->list);
-		spin_unlock(&crc_list.lock);
-		dev_err(&pdev->dev, "Cann't register crypto ahash device\n");
-		goto out_error_dma;
+	if (list_is_singular(&crc_list.dev_list)) {
+		ret = crypto_register_ahash(&algs);
+		if (ret) {
+			dev_err(&pdev->dev,
+				"Can't register crypto ahash device\n");
+			goto out_error_dma;
+		}
 	}
 
 	dev_info(&pdev->dev, "initialized\n");
diff --git a/drivers/crypto/bfin_crc.h b/drivers/crypto/bfin_crc.h
new file mode 100644
index 00000000000..75cef4dc85a
--- /dev/null
+++ b/drivers/crypto/bfin_crc.h
@@ -0,0 +1,125 @@
+/*
+ * bfin_crc.h - interface to Blackfin CRC controllers
+ *
+ * Copyright 2012 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#ifndef __BFIN_CRC_H__
+#define __BFIN_CRC_H__
+
+/* Function driver which use hardware crc must initialize the structure */
+struct crc_info {
+	/* Input data address */
+	unsigned char *in_addr;
+	/* Output data address */
+	unsigned char *out_addr;
+	/* Input or output bytes */
+	unsigned long datasize;
+	union {
+	/* CRC to compare with that of input buffer */
+	unsigned long crc_compare;
+	/* Value to compare with input data */
+	unsigned long val_verify;
+	/* Value to fill */
+	unsigned long val_fill;
+	};
+	/* Value to program the 32b CRC Polynomial */
+	unsigned long crc_poly;
+	union {
+	/* CRC calculated from the input data */
+	unsigned long crc_result;
+	/* First failed position to verify input data */
+	unsigned long pos_verify;
+	};
+	/* CRC mirror flags */
+	unsigned int bitmirr:1;
+	unsigned int bytmirr:1;
+	unsigned int w16swp:1;
+	unsigned int fdsel:1;
+	unsigned int rsltmirr:1;
+	unsigned int polymirr:1;
+	unsigned int cmpmirr:1;
+};
+
+/* Userspace interface */
+#define CRC_IOC_MAGIC		'C'
+#define CRC_IOC_CALC_CRC	_IOWR('C', 0x01, unsigned int)
+#define CRC_IOC_MEMCPY_CRC	_IOWR('C', 0x02, unsigned int)
+#define CRC_IOC_VERIFY_VAL	_IOWR('C', 0x03, unsigned int)
+#define CRC_IOC_FILL_VAL	_IOWR('C', 0x04, unsigned int)
+
+
+#ifdef __KERNEL__
+
+#include <linux/types.h>
+#include <linux/spinlock.h>
+#include <linux/miscdevice.h>
+
+struct crc_register {
+	u32 control;
+	u32 datacnt;
+	u32 datacntrld;
+	u32 __pad_1[2];
+	u32 compare;
+	u32 fillval;
+	u32 datafifo;
+	u32 intren;
+	u32 intrenset;
+	u32 intrenclr;
+	u32 poly;
+	u32 __pad_2[4];
+	u32 status;
+	u32 datacntcap;
+	u32 __pad_3;
+	u32 result;
+	u32 curresult;
+	u32 __pad_4[3];
+	u32 revid;
+};
+
+/* CRC_STATUS Masks */
+#define CMPERR			0x00000002	/* Compare error */
+#define DCNTEXP			0x00000010	/* datacnt register expired */
+#define IBR			0x00010000	/* Input buffer ready */
+#define OBR			0x00020000	/* Output buffer ready */
+#define IRR			0x00040000	/* Immediate result readt */
+#define LUTDONE			0x00080000	/* Look-up table generation done */
+#define FSTAT			0x00700000	/* FIFO status */
+#define MAX_FIFO		4		/* Max fifo size */
+
+/* CRC_CONTROL Masks */
+#define BLKEN			0x00000001	/* Block enable */
+#define OPMODE			0x000000F0	/* Operation mode */
+#define OPMODE_OFFSET		4		/* Operation mode mask offset*/
+#define MODE_DMACPY_CRC		1		/* MTM CRC compute and compare */
+#define MODE_DATA_FILL		2		/* MTM data fill */
+#define MODE_CALC_CRC		3		/* MSM CRC compute and compare */
+#define MODE_DATA_VERIFY	4		/* MSM data verify */
+#define AUTOCLRZ		0x00000100	/* Auto clear to zero */
+#define AUTOCLRF		0x00000200	/* Auto clear to one */
+#define OBRSTALL		0x00001000	/* Stall on output buffer ready */
+#define IRRSTALL		0x00002000	/* Stall on immediate result ready */
+#define BITMIRR			0x00010000	/* Mirror bits within each byte of 32-bit input data */
+#define BITMIRR_OFFSET		16		/* Mirror bits offset */
+#define BYTMIRR			0x00020000	/* Mirror bytes of 32-bit input data */
+#define BYTMIRR_OFFSET		17		/* Mirror bytes offset */
+#define W16SWP			0x00040000	/* Mirror uppper and lower 16-bit word of 32-bit input data */
+#define W16SWP_OFFSET		18		/* Mirror 16-bit word offset */
+#define FDSEL			0x00080000	/* FIFO is written after input data is mirrored */
+#define FDSEL_OFFSET		19		/* Mirror FIFO offset */
+#define RSLTMIRR		0x00100000	/* CRC result registers are mirrored. */
+#define RSLTMIRR_OFFSET		20		/* Mirror CRC result offset. */
+#define POLYMIRR		0x00200000	/* CRC poly register is mirrored. */
+#define POLYMIRR_OFFSET		21		/* Mirror CRC poly offset. */
+#define CMPMIRR			0x00400000	/* CRC compare register is mirrored. */
+#define CMPMIRR_OFFSET		22		/* Mirror CRC compare offset. */
+
+/* CRC_INTREN Masks */
+#define CMPERRI 		0x02		/* CRC_ERROR_INTR */
+#define DCNTEXPI 		0x10		/* CRC_STATUS_INTR */
+
+#endif
+
+#endif
diff --git a/drivers/crypto/caam/caamalg.c b/drivers/crypto/caam/caamalg.c
index 5f891254db7..c09ce1f040d 100644
--- a/drivers/crypto/caam/caamalg.c
+++ b/drivers/crypto/caam/caamalg.c
@@ -303,6 +303,7 @@ static int aead_null_set_sh_desc(struct crypto_aead *aead)
 	 * Job Descriptor and Shared Descriptors
 	 * must all fit into the 64-word Descriptor h/w Buffer
 	 */
+	keys_fit_inline = false;
 	if (DESC_AEAD_NULL_DEC_LEN + DESC_JOB_IO_LEN +
 	    ctx->split_key_pad_len <= CAAM_DESC_BYTES_MAX)
 		keys_fit_inline = true;
@@ -472,6 +473,7 @@ static int aead_set_sh_desc(struct crypto_aead *aead)
 	 * Job Descriptor and Shared Descriptors
 	 * must all fit into the 64-word Descriptor h/w Buffer
 	 */
+	keys_fit_inline = false;
 	if (DESC_AEAD_DEC_LEN + DESC_JOB_IO_LEN +
 	    ctx->split_key_pad_len + ctx->enckeylen <=
 	    CAAM_DESC_BYTES_MAX)
@@ -527,6 +529,7 @@ static int aead_set_sh_desc(struct crypto_aead *aead)
 	 * Job Descriptor and Shared Descriptors
 	 * must all fit into the 64-word Descriptor h/w Buffer
 	 */
+	keys_fit_inline = false;
 	if (DESC_AEAD_GIVENC_LEN + DESC_JOB_IO_LEN +
 	    ctx->split_key_pad_len + ctx->enckeylen <=
 	    CAAM_DESC_BYTES_MAX)
@@ -918,11 +921,8 @@ static void aead_encrypt_done(struct device *jrdev, u32 *desc, u32 err,
 	edesc = (struct aead_edesc *)((char *)desc -
 		 offsetof(struct aead_edesc, hw_desc));
 
-	if (err) {
-		char tmp[CAAM_ERROR_STR_MAX];
-
-		dev_err(jrdev, "%08x: %s\n", err, caam_jr_strstatus(tmp, err));
-	}
+	if (err)
+		caam_jr_strstatus(jrdev, err);
 
 	aead_unmap(jrdev, edesc, req);
 
@@ -969,11 +969,8 @@ static void aead_decrypt_done(struct device *jrdev, u32 *desc, u32 err,
 		       req->cryptlen - ctx->authsize, 1);
 #endif
 
-	if (err) {
-		char tmp[CAAM_ERROR_STR_MAX];
-
-		dev_err(jrdev, "%08x: %s\n", err, caam_jr_strstatus(tmp, err));
-	}
+	if (err)
+		caam_jr_strstatus(jrdev, err);
 
 	aead_unmap(jrdev, edesc, req);
 
@@ -1018,11 +1015,8 @@ static void ablkcipher_encrypt_done(struct device *jrdev, u32 *desc, u32 err,
 	edesc = (struct ablkcipher_edesc *)((char *)desc -
 		 offsetof(struct ablkcipher_edesc, hw_desc));
 
-	if (err) {
-		char tmp[CAAM_ERROR_STR_MAX];
-
-		dev_err(jrdev, "%08x: %s\n", err, caam_jr_strstatus(tmp, err));
-	}
+	if (err)
+		caam_jr_strstatus(jrdev, err);
 
 #ifdef DEBUG
 	print_hex_dump(KERN_ERR, "dstiv  @"__stringify(__LINE__)": ",
@@ -1053,11 +1047,8 @@ static void ablkcipher_decrypt_done(struct device *jrdev, u32 *desc, u32 err,
 
 	edesc = (struct ablkcipher_edesc *)((char *)desc -
 		 offsetof(struct ablkcipher_edesc, hw_desc));
-	if (err) {
-		char tmp[CAAM_ERROR_STR_MAX];
-
-		dev_err(jrdev, "%08x: %s\n", err, caam_jr_strstatus(tmp, err));
-	}
+	if (err)
+		caam_jr_strstatus(jrdev, err);
 
 #ifdef DEBUG
 	print_hex_dump(KERN_ERR, "dstiv  @"__stringify(__LINE__)": ",
diff --git a/drivers/crypto/caam/caamhash.c b/drivers/crypto/caam/caamhash.c
index 0378328f47a..0d9284ef96a 100644
--- a/drivers/crypto/caam/caamhash.c
+++ b/drivers/crypto/caam/caamhash.c
@@ -545,7 +545,8 @@ static int ahash_setkey(struct crypto_ahash *ahash,
 				      DMA_TO_DEVICE);
 	if (dma_mapping_error(jrdev, ctx->key_dma)) {
 		dev_err(jrdev, "unable to map key i/o memory\n");
-		return -ENOMEM;
+		ret = -ENOMEM;
+		goto map_err;
 	}
 #ifdef DEBUG
 	print_hex_dump(KERN_ERR, "ctx.key@"__stringify(__LINE__)": ",
@@ -559,6 +560,7 @@ static int ahash_setkey(struct crypto_ahash *ahash,
 				 DMA_TO_DEVICE);
 	}
 
+map_err:
 	kfree(hashed_key);
 	return ret;
 badkey:
@@ -631,11 +633,8 @@ static void ahash_done(struct device *jrdev, u32 *desc, u32 err,
 
 	edesc = (struct ahash_edesc *)((char *)desc -
 		 offsetof(struct ahash_edesc, hw_desc));
-	if (err) {
-		char tmp[CAAM_ERROR_STR_MAX];
-
-		dev_err(jrdev, "%08x: %s\n", err, caam_jr_strstatus(tmp, err));
-	}
+	if (err)
+		caam_jr_strstatus(jrdev, err);
 
 	ahash_unmap(jrdev, edesc, req, digestsize);
 	kfree(edesc);
@@ -669,11 +668,8 @@ static void ahash_done_bi(struct device *jrdev, u32 *desc, u32 err,
 
 	edesc = (struct ahash_edesc *)((char *)desc -
 		 offsetof(struct ahash_edesc, hw_desc));
-	if (err) {
-		char tmp[CAAM_ERROR_STR_MAX];
-
-		dev_err(jrdev, "%08x: %s\n", err, caam_jr_strstatus(tmp, err));
-	}
+	if (err)
+		caam_jr_strstatus(jrdev, err);
 
 	ahash_unmap_ctx(jrdev, edesc, req, ctx->ctx_len, DMA_BIDIRECTIONAL);
 	kfree(edesc);
@@ -707,11 +703,8 @@ static void ahash_done_ctx_src(struct device *jrdev, u32 *desc, u32 err,
 
 	edesc = (struct ahash_edesc *)((char *)desc -
 		 offsetof(struct ahash_edesc, hw_desc));
-	if (err) {
-		char tmp[CAAM_ERROR_STR_MAX];
-
-		dev_err(jrdev, "%08x: %s\n", err, caam_jr_strstatus(tmp, err));
-	}
+	if (err)
+		caam_jr_strstatus(jrdev, err);
 
 	ahash_unmap_ctx(jrdev, edesc, req, digestsize, DMA_FROM_DEVICE);
 	kfree(edesc);
@@ -745,11 +738,8 @@ static void ahash_done_ctx_dst(struct device *jrdev, u32 *desc, u32 err,
 
 	edesc = (struct ahash_edesc *)((char *)desc -
 		 offsetof(struct ahash_edesc, hw_desc));
-	if (err) {
-		char tmp[CAAM_ERROR_STR_MAX];
-
-		dev_err(jrdev, "%08x: %s\n", err, caam_jr_strstatus(tmp, err));
-	}
+	if (err)
+		caam_jr_strstatus(jrdev, err);
 
 	ahash_unmap_ctx(jrdev, edesc, req, ctx->ctx_len, DMA_TO_DEVICE);
 	kfree(edesc);
diff --git a/drivers/crypto/caam/caamrng.c b/drivers/crypto/caam/caamrng.c
index 3529b54048c..8c07d3153f1 100644
--- a/drivers/crypto/caam/caamrng.c
+++ b/drivers/crypto/caam/caamrng.c
@@ -103,11 +103,8 @@ static void rng_done(struct device *jrdev, u32 *desc, u32 err, void *context)
 	bd = (struct buf_data *)((char *)desc -
 	      offsetof(struct buf_data, hw_desc));
 
-	if (err) {
-		char tmp[CAAM_ERROR_STR_MAX];
-
-		dev_err(jrdev, "%08x: %s\n", err, caam_jr_strstatus(tmp, err));
-	}
+	if (err)
+		caam_jr_strstatus(jrdev, err);
 
 	atomic_set(&bd->empty, BUF_NOT_EMPTY);
 	complete(&bd->filled);
diff --git a/drivers/crypto/caam/error.c b/drivers/crypto/caam/error.c
index 9f25f529602..6531054a44c 100644
--- a/drivers/crypto/caam/error.c
+++ b/drivers/crypto/caam/error.c
@@ -11,243 +11,208 @@
 #include "jr.h"
 #include "error.h"
 
-#define SPRINTFCAT(str, format, param, max_alloc)		\
-{								\
-	char *tmp;						\
-								\
-	tmp = kmalloc(sizeof(format) + max_alloc, GFP_ATOMIC);	\
-	sprintf(tmp, format, param);				\
-	strcat(str, tmp);					\
-	kfree(tmp);						\
-}
-
-static void report_jump_idx(u32 status, char *outstr)
+static const struct {
+	u8 value;
+	const char *error_text;
+} desc_error_list[] = {
+	{ 0x00, "No error." },
+	{ 0x01, "SGT Length Error. The descriptor is trying to read more data than is contained in the SGT table." },
+	{ 0x02, "SGT Null Entry Error." },
+	{ 0x03, "Job Ring Control Error. There is a bad value in the Job Ring Control register." },
+	{ 0x04, "Invalid Descriptor Command. The Descriptor Command field is invalid." },
+	{ 0x05, "Reserved." },
+	{ 0x06, "Invalid KEY Command" },
+	{ 0x07, "Invalid LOAD Command" },
+	{ 0x08, "Invalid STORE Command" },
+	{ 0x09, "Invalid OPERATION Command" },
+	{ 0x0A, "Invalid FIFO LOAD Command" },
+	{ 0x0B, "Invalid FIFO STORE Command" },
+	{ 0x0C, "Invalid MOVE/MOVE_LEN Command" },
+	{ 0x0D, "Invalid JUMP Command. A nonlocal JUMP Command is invalid because the target is not a Job Header Command, or the jump is from a Trusted Descriptor to a Job Descriptor, or because the target Descriptor contains a Shared Descriptor." },
+	{ 0x0E, "Invalid MATH Command" },
+	{ 0x0F, "Invalid SIGNATURE Command" },
+	{ 0x10, "Invalid Sequence Command. A SEQ IN PTR OR SEQ OUT PTR Command is invalid or a SEQ KEY, SEQ LOAD, SEQ FIFO LOAD, or SEQ FIFO STORE decremented the input or output sequence length below 0. This error may result if a built-in PROTOCOL Command has encountered a malformed PDU." },
+	{ 0x11, "Skip data type invalid. The type must be 0xE or 0xF."},
+	{ 0x12, "Shared Descriptor Header Error" },
+	{ 0x13, "Header Error. Invalid length or parity, or certain other problems." },
+	{ 0x14, "Burster Error. Burster has gotten to an illegal state" },
+	{ 0x15, "Context Register Length Error. The descriptor is trying to read or write past the end of the Context Register. A SEQ LOAD or SEQ STORE with the VLF bit set was executed with too large a length in the variable length register (VSOL for SEQ STORE or VSIL for SEQ LOAD)." },
+	{ 0x16, "DMA Error" },
+	{ 0x17, "Reserved." },
+	{ 0x1A, "Job failed due to JR reset" },
+	{ 0x1B, "Job failed due to Fail Mode" },
+	{ 0x1C, "DECO Watchdog timer timeout error" },
+	{ 0x1D, "DECO tried to copy a key from another DECO but the other DECO's Key Registers were locked" },
+	{ 0x1E, "DECO attempted to copy data from a DECO that had an unmasked Descriptor error" },
+	{ 0x1F, "LIODN error. DECO was trying to share from itself or from another DECO but the two Non-SEQ LIODN values didn't match or the 'shared from' DECO's Descriptor required that the SEQ LIODNs be the same and they aren't." },
+	{ 0x20, "DECO has completed a reset initiated via the DRR register" },
+	{ 0x21, "Nonce error. When using EKT (CCM) key encryption option in the FIFO STORE Command, the Nonce counter reached its maximum value and this encryption mode can no longer be used." },
+	{ 0x22, "Meta data is too large (> 511 bytes) for TLS decap (input frame; block ciphers) and IPsec decap (output frame, when doing the next header byte update) and DCRC (output frame)." },
+	{ 0x23, "Read Input Frame error" },
+	{ 0x24, "JDKEK, TDKEK or TDSK not loaded error" },
+	{ 0x80, "DNR (do not run) error" },
+	{ 0x81, "undefined protocol command" },
+	{ 0x82, "invalid setting in PDB" },
+	{ 0x83, "Anti-replay LATE error" },
+	{ 0x84, "Anti-replay REPLAY error" },
+	{ 0x85, "Sequence number overflow" },
+	{ 0x86, "Sigver invalid signature" },
+	{ 0x87, "DSA Sign Illegal test descriptor" },
+	{ 0x88, "Protocol Format Error - A protocol has seen an error in the format of data received. When running RSA, this means that formatting with random padding was used, and did not follow the form: 0x00, 0x02, 8-to-N bytes of non-zero pad, 0x00, F data." },
+	{ 0x89, "Protocol Size Error - A protocol has seen an error in size. When running RSA, pdb size N < (size of F) when no formatting is used; or pdb size N < (F + 11) when formatting is used." },
+	{ 0xC1, "Blob Command error: Undefined mode" },
+	{ 0xC2, "Blob Command error: Secure Memory Blob mode error" },
+	{ 0xC4, "Blob Command error: Black Blob key or input size error" },
+	{ 0xC5, "Blob Command error: Invalid key destination" },
+	{ 0xC8, "Blob Command error: Trusted/Secure mode error" },
+	{ 0xF0, "IPsec TTL or hop limit field either came in as 0, or was decremented to 0" },
+	{ 0xF1, "3GPP HFN matches or exceeds the Threshold" },
+};
+
+static const char * const cha_id_list[] = {
+	"",
+	"AES",
+	"DES",
+	"ARC4",
+	"MDHA",
+	"RNG",
+	"SNOW f8",
+	"Kasumi f8/9",
+	"PKHA",
+	"CRCA",
+	"SNOW f9",
+	"ZUCE",
+	"ZUCA",
+};
+
+static const char * const err_id_list[] = {
+	"No error.",
+	"Mode error.",
+	"Data size error.",
+	"Key size error.",
+	"PKHA A memory size error.",
+	"PKHA B memory size error.",
+	"Data arrived out of sequence error.",
+	"PKHA divide-by-zero error.",
+	"PKHA modulus even error.",
+	"DES key parity error.",
+	"ICV check failed.",
+	"Hardware error.",
+	"Unsupported CCM AAD size.",
+	"Class 1 CHA is not reset",
+	"Invalid CHA combination was selected",
+	"Invalid CHA selected.",
+};
+
+static const char * const rng_err_id_list[] = {
+	"",
+	"",
+	"",
+	"Instantiate",
+	"Not instantiated",
+	"Test instantiate",
+	"Prediction resistance",
+	"Prediction resistance and test request",
+	"Uninstantiate",
+	"Secure key generation",
+};
+
+static void report_ccb_status(struct device *jrdev, const u32 status,
+			      const char *error)
 {
+	u8 cha_id = (status & JRSTA_CCBERR_CHAID_MASK) >>
+		    JRSTA_CCBERR_CHAID_SHIFT;
+	u8 err_id = status & JRSTA_CCBERR_ERRID_MASK;
 	u8 idx = (status & JRSTA_DECOERR_INDEX_MASK) >>
 		  JRSTA_DECOERR_INDEX_SHIFT;
+	char *idx_str;
+	const char *cha_str = "unidentified cha_id value 0x";
+	char cha_err_code[3] = { 0 };
+	const char *err_str = "unidentified err_id value 0x";
+	char err_err_code[3] = { 0 };
 
 	if (status & JRSTA_DECOERR_JUMP)
-		strcat(outstr, "jump tgt desc idx ");
+		idx_str = "jump tgt desc idx";
 	else
-		strcat(outstr, "desc idx ");
-
-	SPRINTFCAT(outstr, "%d: ", idx, sizeof("255"));
-}
-
-static void report_ccb_status(u32 status, char *outstr)
-{
-	static const char * const cha_id_list[] = {
-		"",
-		"AES",
-		"DES",
-		"ARC4",
-		"MDHA",
-		"RNG",
-		"SNOW f8",
-		"Kasumi f8/9",
-		"PKHA",
-		"CRCA",
-		"SNOW f9",
-		"ZUCE",
-		"ZUCA",
-	};
-	static const char * const err_id_list[] = {
-		"No error.",
-		"Mode error.",
-		"Data size error.",
-		"Key size error.",
-		"PKHA A memory size error.",
-		"PKHA B memory size error.",
-		"Data arrived out of sequence error.",
-		"PKHA divide-by-zero error.",
-		"PKHA modulus even error.",
-		"DES key parity error.",
-		"ICV check failed.",
-		"Hardware error.",
-		"Unsupported CCM AAD size.",
-		"Class 1 CHA is not reset",
-		"Invalid CHA combination was selected",
-		"Invalid CHA selected.",
-	};
-	static const char * const rng_err_id_list[] = {
-		"",
-		"",
-		"",
-		"Instantiate",
-		"Not instantiated",
-		"Test instantiate",
-		"Prediction resistance",
-		"Prediction resistance and test request",
-		"Uninstantiate",
-		"Secure key generation",
-	};
-	u8 cha_id = (status & JRSTA_CCBERR_CHAID_MASK) >>
-		    JRSTA_CCBERR_CHAID_SHIFT;
-	u8 err_id = status & JRSTA_CCBERR_ERRID_MASK;
+		idx_str = "desc idx";
 
-	report_jump_idx(status, outstr);
-
-	if (cha_id < ARRAY_SIZE(cha_id_list)) {
-		SPRINTFCAT(outstr, "%s: ", cha_id_list[cha_id],
-			   strlen(cha_id_list[cha_id]));
-	} else {
-		SPRINTFCAT(outstr, "unidentified cha_id value 0x%02x: ",
-			   cha_id, sizeof("ff"));
-	}
+	if (cha_id < ARRAY_SIZE(cha_id_list))
+		cha_str = cha_id_list[cha_id];
+	else
+		snprintf(cha_err_code, sizeof(cha_err_code), "%02x", cha_id);
 
 	if ((cha_id << JRSTA_CCBERR_CHAID_SHIFT) == JRSTA_CCBERR_CHAID_RNG &&
 	    err_id < ARRAY_SIZE(rng_err_id_list) &&
 	    strlen(rng_err_id_list[err_id])) {
 		/* RNG-only error */
-		SPRINTFCAT(outstr, "%s", rng_err_id_list[err_id],
-			   strlen(rng_err_id_list[err_id]));
-	} else if (err_id < ARRAY_SIZE(err_id_list)) {
-		SPRINTFCAT(outstr, "%s", err_id_list[err_id],
-			   strlen(err_id_list[err_id]));
-	} else {
-		SPRINTFCAT(outstr, "unidentified err_id value 0x%02x",
-			   err_id, sizeof("ff"));
-	}
+		err_str = rng_err_id_list[err_id];
+	} else if (err_id < ARRAY_SIZE(err_id_list))
+		err_str = err_id_list[err_id];
+	else
+		snprintf(err_err_code, sizeof(err_err_code), "%02x", err_id);
+
+	dev_err(jrdev, "%08x: %s: %s %d: %s%s: %s%s\n",
+		status, error, idx_str, idx,
+		cha_str, cha_err_code,
+		err_str, err_err_code);
 }
 
-static void report_jump_status(u32 status, char *outstr)
+static void report_jump_status(struct device *jrdev, const u32 status,
+			       const char *error)
 {
-	SPRINTFCAT(outstr, "%s() not implemented", __func__, sizeof(__func__));
+	dev_err(jrdev, "%08x: %s: %s() not implemented\n",
+		status, error, __func__);
 }
 
-static void report_deco_status(u32 status, char *outstr)
+static void report_deco_status(struct device *jrdev, const u32 status,
+			       const char *error)
 {
-	static const struct {
-		u8 value;
-		char *error_text;
-	} desc_error_list[] = {
-		{ 0x00, "No error." },
-		{ 0x01, "SGT Length Error. The descriptor is trying to read "
-			"more data than is contained in the SGT table." },
-		{ 0x02, "SGT Null Entry Error." },
-		{ 0x03, "Job Ring Control Error. There is a bad value in the "
-			"Job Ring Control register." },
-		{ 0x04, "Invalid Descriptor Command. The Descriptor Command "
-			"field is invalid." },
-		{ 0x05, "Reserved." },
-		{ 0x06, "Invalid KEY Command" },
-		{ 0x07, "Invalid LOAD Command" },
-		{ 0x08, "Invalid STORE Command" },
-		{ 0x09, "Invalid OPERATION Command" },
-		{ 0x0A, "Invalid FIFO LOAD Command" },
-		{ 0x0B, "Invalid FIFO STORE Command" },
-		{ 0x0C, "Invalid MOVE/MOVE_LEN Command" },
-		{ 0x0D, "Invalid JUMP Command. A nonlocal JUMP Command is "
-			"invalid because the target is not a Job Header "
-			"Command, or the jump is from a Trusted Descriptor to "
-			"a Job Descriptor, or because the target Descriptor "
-			"contains a Shared Descriptor." },
-		{ 0x0E, "Invalid MATH Command" },
-		{ 0x0F, "Invalid SIGNATURE Command" },
-		{ 0x10, "Invalid Sequence Command. A SEQ IN PTR OR SEQ OUT PTR "
-			"Command is invalid or a SEQ KEY, SEQ LOAD, SEQ FIFO "
-			"LOAD, or SEQ FIFO STORE decremented the input or "
-			"output sequence length below 0. This error may result "
-			"if a built-in PROTOCOL Command has encountered a "
-			"malformed PDU." },
-		{ 0x11, "Skip data type invalid. The type must be 0xE or 0xF."},
-		{ 0x12, "Shared Descriptor Header Error" },
-		{ 0x13, "Header Error. Invalid length or parity, or certain "
-			"other problems." },
-		{ 0x14, "Burster Error. Burster has gotten to an illegal "
-			"state" },
-		{ 0x15, "Context Register Length Error. The descriptor is "
-			"trying to read or write past the end of the Context "
-			"Register. A SEQ LOAD or SEQ STORE with the VLF bit "
-			"set was executed with too large a length in the "
-			"variable length register (VSOL for SEQ STORE or VSIL "
-			"for SEQ LOAD)." },
-		{ 0x16, "DMA Error" },
-		{ 0x17, "Reserved." },
-		{ 0x1A, "Job failed due to JR reset" },
-		{ 0x1B, "Job failed due to Fail Mode" },
-		{ 0x1C, "DECO Watchdog timer timeout error" },
-		{ 0x1D, "DECO tried to copy a key from another DECO but the "
-			"other DECO's Key Registers were locked" },
-		{ 0x1E, "DECO attempted to copy data from a DECO that had an "
-			"unmasked Descriptor error" },
-		{ 0x1F, "LIODN error. DECO was trying to share from itself or "
-			"from another DECO but the two Non-SEQ LIODN values "
-			"didn't match or the 'shared from' DECO's Descriptor "
-			"required that the SEQ LIODNs be the same and they "
-			"aren't." },
-		{ 0x20, "DECO has completed a reset initiated via the DRR "
-			"register" },
-		{ 0x21, "Nonce error. When using EKT (CCM) key encryption "
-			"option in the FIFO STORE Command, the Nonce counter "
-			"reached its maximum value and this encryption mode "
-			"can no longer be used." },
-		{ 0x22, "Meta data is too large (> 511 bytes) for TLS decap "
-			"(input frame; block ciphers) and IPsec decap (output "
-			"frame, when doing the next header byte update) and "
-			"DCRC (output frame)." },
-		{ 0x23, "Read Input Frame error" },
-		{ 0x24, "JDKEK, TDKEK or TDSK not loaded error" },
-		{ 0x80, "DNR (do not run) error" },
-		{ 0x81, "undefined protocol command" },
-		{ 0x82, "invalid setting in PDB" },
-		{ 0x83, "Anti-replay LATE error" },
-		{ 0x84, "Anti-replay REPLAY error" },
-		{ 0x85, "Sequence number overflow" },
-		{ 0x86, "Sigver invalid signature" },
-		{ 0x87, "DSA Sign Illegal test descriptor" },
-		{ 0x88, "Protocol Format Error - A protocol has seen an error "
-			"in the format of data received. When running RSA, "
-			"this means that formatting with random padding was "
-			"used, and did not follow the form: 0x00, 0x02, 8-to-N "
-			"bytes of non-zero pad, 0x00, F data." },
-		{ 0x89, "Protocol Size Error - A protocol has seen an error in "
-			"size. When running RSA, pdb size N < (size of F) when "
-			"no formatting is used; or pdb size N < (F + 11) when "
-			"formatting is used." },
-		{ 0xC1, "Blob Command error: Undefined mode" },
-		{ 0xC2, "Blob Command error: Secure Memory Blob mode error" },
-		{ 0xC4, "Blob Command error: Black Blob key or input size "
-			"error" },
-		{ 0xC5, "Blob Command error: Invalid key destination" },
-		{ 0xC8, "Blob Command error: Trusted/Secure mode error" },
-		{ 0xF0, "IPsec TTL or hop limit field either came in as 0, "
-			"or was decremented to 0" },
-		{ 0xF1, "3GPP HFN matches or exceeds the Threshold" },
-	};
-	u8 desc_error = status & JRSTA_DECOERR_ERROR_MASK;
+	u8 err_id = status & JRSTA_DECOERR_ERROR_MASK;
+	u8 idx = (status & JRSTA_DECOERR_INDEX_MASK) >>
+		  JRSTA_DECOERR_INDEX_SHIFT;
+	char *idx_str;
+	const char *err_str = "unidentified error value 0x";
+	char err_err_code[3] = { 0 };
 	int i;
 
-	report_jump_idx(status, outstr);
+	if (status & JRSTA_DECOERR_JUMP)
+		idx_str = "jump tgt desc idx";
+	else
+		idx_str = "desc idx";
 
 	for (i = 0; i < ARRAY_SIZE(desc_error_list); i++)
-		if (desc_error_list[i].value == desc_error)
+		if (desc_error_list[i].value == err_id)
 			break;
 
-	if (i != ARRAY_SIZE(desc_error_list) && desc_error_list[i].error_text) {
-		SPRINTFCAT(outstr, "%s", desc_error_list[i].error_text,
-			   strlen(desc_error_list[i].error_text));
-	} else {
-		SPRINTFCAT(outstr, "unidentified error value 0x%02x",
-			   desc_error, sizeof("ff"));
-	}
+	if (i != ARRAY_SIZE(desc_error_list) && desc_error_list[i].error_text)
+		err_str = desc_error_list[i].error_text;
+	else
+		snprintf(err_err_code, sizeof(err_err_code), "%02x", err_id);
+
+	dev_err(jrdev, "%08x: %s: %s %d: %s%s\n",
+		status, error, idx_str, idx, err_str, err_err_code);
 }
 
-static void report_jr_status(u32 status, char *outstr)
+static void report_jr_status(struct device *jrdev, const u32 status,
+			     const char *error)
 {
-	SPRINTFCAT(outstr, "%s() not implemented", __func__, sizeof(__func__));
+	dev_err(jrdev, "%08x: %s: %s() not implemented\n",
+		status, error, __func__);
 }
 
-static void report_cond_code_status(u32 status, char *outstr)
+static void report_cond_code_status(struct device *jrdev, const u32 status,
+				    const char *error)
 {
-	SPRINTFCAT(outstr, "%s() not implemented", __func__, sizeof(__func__));
+	dev_err(jrdev, "%08x: %s: %s() not implemented\n",
+		status, error, __func__);
 }
 
-char *caam_jr_strstatus(char *outstr, u32 status)
+void caam_jr_strstatus(struct device *jrdev, u32 status)
 {
 	static const struct stat_src {
-		void (*report_ssed)(u32 status, char *outstr);
-		char *error;
+		void (*report_ssed)(struct device *jrdev, const u32 status,
+				    const char *error);
+		const char *error;
 	} status_src[] = {
 		{ NULL, "No error" },
 		{ NULL, NULL },
@@ -259,12 +224,16 @@ char *caam_jr_strstatus(char *outstr, u32 status)
 		{ report_cond_code_status, "Condition Code" },
 	};
 	u32 ssrc = status >> JRSTA_SSRC_SHIFT;
-
-	sprintf(outstr, "%s: ", status_src[ssrc].error);
-
-	if (status_src[ssrc].report_ssed)
-		status_src[ssrc].report_ssed(status, outstr);
-
-	return outstr;
+	const char *error = status_src[ssrc].error;
+
+	/*
+	 * If there is no further error handling function, just
+	 * print the error code, error string and exit. Otherwise
+	 * call the handler function.
+	 */
+	if (!status_src[ssrc].report_ssed)
+		dev_err(jrdev, "%08x: %s: \n", status, status_src[ssrc].error);
+	else
+		status_src[ssrc].report_ssed(jrdev, status, error);
 }
 EXPORT_SYMBOL(caam_jr_strstatus);
diff --git a/drivers/crypto/caam/error.h b/drivers/crypto/caam/error.h
index 02c7baa1748..b6350b0d915 100644
--- a/drivers/crypto/caam/error.h
+++ b/drivers/crypto/caam/error.h
@@ -7,5 +7,5 @@
 #ifndef CAAM_ERROR_H
 #define CAAM_ERROR_H
 #define CAAM_ERROR_STR_MAX 302
-extern char *caam_jr_strstatus(char *outstr, u32 status);
+void caam_jr_strstatus(struct device *jrdev, u32 status);
 #endif /* CAAM_ERROR_H */
diff --git a/drivers/crypto/caam/key_gen.c b/drivers/crypto/caam/key_gen.c
index ea2e406610e..871703c49d2 100644
--- a/drivers/crypto/caam/key_gen.c
+++ b/drivers/crypto/caam/key_gen.c
@@ -19,11 +19,8 @@ void split_key_done(struct device *dev, u32 *desc, u32 err,
 	dev_err(dev, "%s %d: err 0x%x\n", __func__, __LINE__, err);
 #endif
 
-	if (err) {
-		char tmp[CAAM_ERROR_STR_MAX];
-
-		dev_err(dev, "%08x: %s\n", err, caam_jr_strstatus(tmp, err));
-	}
+	if (err)
+		caam_jr_strstatus(dev, err);
 
 	res->err = err;
 
diff --git a/drivers/crypto/ccp/ccp-crypto-aes-xts.c b/drivers/crypto/ccp/ccp-crypto-aes-xts.c
index 0237ab58f24..0cc5594b7de 100644
--- a/drivers/crypto/ccp/ccp-crypto-aes-xts.c
+++ b/drivers/crypto/ccp/ccp-crypto-aes-xts.c
@@ -191,12 +191,12 @@ static int ccp_aes_xts_cra_init(struct crypto_tfm *tfm)
 	ctx->complete = ccp_aes_xts_complete;
 	ctx->u.aes.key_len = 0;
 
-	fallback_tfm = crypto_alloc_ablkcipher(tfm->__crt_alg->cra_name, 0,
+	fallback_tfm = crypto_alloc_ablkcipher(crypto_tfm_alg_name(tfm), 0,
 					       CRYPTO_ALG_ASYNC |
 					       CRYPTO_ALG_NEED_FALLBACK);
 	if (IS_ERR(fallback_tfm)) {
 		pr_warn("could not load fallback driver %s\n",
-			tfm->__crt_alg->cra_name);
+			crypto_tfm_alg_name(tfm));
 		return PTR_ERR(fallback_tfm);
 	}
 	ctx->u.aes.tfm_ablkcipher = fallback_tfm;
diff --git a/drivers/crypto/ccp/ccp-pci.c b/drivers/crypto/ccp/ccp-pci.c
index 93319f9db75..0d746236df5 100644
--- a/drivers/crypto/ccp/ccp-pci.c
+++ b/drivers/crypto/ccp/ccp-pci.c
@@ -48,12 +48,11 @@ static int ccp_get_msix_irqs(struct ccp_device *ccp)
 	for (v = 0; v < ARRAY_SIZE(msix_entry); v++)
 		msix_entry[v].entry = v;
 
-	while ((ret = pci_enable_msix(pdev, msix_entry, v)) > 0)
-		v = ret;
-	if (ret)
+	ret = pci_enable_msix_range(pdev, msix_entry, 1, v);
+	if (ret < 0)
 		return ret;
 
-	ccp_pci->msix_count = v;
+	ccp_pci->msix_count = ret;
 	for (v = 0; v < ccp_pci->msix_count; v++) {
 		/* Set the interrupt names and request the irqs */
 		snprintf(ccp_pci->msix[v].name, name_len, "ccp-%u", v);
diff --git a/drivers/crypto/geode-aes.c b/drivers/crypto/geode-aes.c
index 0c9ff497172..fe538e5287a 100644
--- a/drivers/crypto/geode-aes.c
+++ b/drivers/crypto/geode-aes.c
@@ -226,7 +226,7 @@ geode_encrypt(struct crypto_tfm *tfm, u8 *out, const u8 *in)
 	op->dst = (void *) out;
 	op->mode = AES_MODE_ECB;
 	op->flags = 0;
-	op->len = AES_MIN_BLOCK_SIZE;
+	op->len = AES_BLOCK_SIZE;
 	op->dir = AES_DIR_ENCRYPT;
 
 	geode_aes_crypt(op);
@@ -247,7 +247,7 @@ geode_decrypt(struct crypto_tfm *tfm, u8 *out, const u8 *in)
 	op->dst = (void *) out;
 	op->mode = AES_MODE_ECB;
 	op->flags = 0;
-	op->len = AES_MIN_BLOCK_SIZE;
+	op->len = AES_BLOCK_SIZE;
 	op->dir = AES_DIR_DECRYPT;
 
 	geode_aes_crypt(op);
@@ -255,7 +255,7 @@ geode_decrypt(struct crypto_tfm *tfm, u8 *out, const u8 *in)
 
 static int fallback_init_cip(struct crypto_tfm *tfm)
 {
-	const char *name = tfm->__crt_alg->cra_name;
+	const char *name = crypto_tfm_alg_name(tfm);
 	struct geode_aes_op *op = crypto_tfm_ctx(tfm);
 
 	op->fallback.cip = crypto_alloc_cipher(name, 0,
@@ -286,7 +286,7 @@ static struct crypto_alg geode_alg = {
 							CRYPTO_ALG_NEED_FALLBACK,
 	.cra_init			=	fallback_init_cip,
 	.cra_exit			=	fallback_exit_cip,
-	.cra_blocksize		=	AES_MIN_BLOCK_SIZE,
+	.cra_blocksize		=	AES_BLOCK_SIZE,
 	.cra_ctxsize		=	sizeof(struct geode_aes_op),
 	.cra_module			=	THIS_MODULE,
 	.cra_u				=	{
@@ -320,7 +320,7 @@ geode_cbc_decrypt(struct blkcipher_desc *desc,
 		op->src = walk.src.virt.addr,
 		op->dst = walk.dst.virt.addr;
 		op->mode = AES_MODE_CBC;
-		op->len = nbytes - (nbytes % AES_MIN_BLOCK_SIZE);
+		op->len = nbytes - (nbytes % AES_BLOCK_SIZE);
 		op->dir = AES_DIR_DECRYPT;
 
 		ret = geode_aes_crypt(op);
@@ -352,7 +352,7 @@ geode_cbc_encrypt(struct blkcipher_desc *desc,
 		op->src = walk.src.virt.addr,
 		op->dst = walk.dst.virt.addr;
 		op->mode = AES_MODE_CBC;
-		op->len = nbytes - (nbytes % AES_MIN_BLOCK_SIZE);
+		op->len = nbytes - (nbytes % AES_BLOCK_SIZE);
 		op->dir = AES_DIR_ENCRYPT;
 
 		ret = geode_aes_crypt(op);
@@ -365,7 +365,7 @@ geode_cbc_encrypt(struct blkcipher_desc *desc,
 
 static int fallback_init_blk(struct crypto_tfm *tfm)
 {
-	const char *name = tfm->__crt_alg->cra_name;
+	const char *name = crypto_tfm_alg_name(tfm);
 	struct geode_aes_op *op = crypto_tfm_ctx(tfm);
 
 	op->fallback.blk = crypto_alloc_blkcipher(name, 0,
@@ -396,7 +396,7 @@ static struct crypto_alg geode_cbc_alg = {
 						CRYPTO_ALG_NEED_FALLBACK,
 	.cra_init			=	fallback_init_blk,
 	.cra_exit			=	fallback_exit_blk,
-	.cra_blocksize		=	AES_MIN_BLOCK_SIZE,
+	.cra_blocksize		=	AES_BLOCK_SIZE,
 	.cra_ctxsize		=	sizeof(struct geode_aes_op),
 	.cra_alignmask		=	15,
 	.cra_type			=	&crypto_blkcipher_type,
@@ -408,7 +408,7 @@ static struct crypto_alg geode_cbc_alg = {
 			.setkey			=	geode_setkey_blk,
 			.encrypt		=	geode_cbc_encrypt,
 			.decrypt		=	geode_cbc_decrypt,
-			.ivsize			=	AES_IV_LENGTH,
+			.ivsize			=	AES_BLOCK_SIZE,
 		}
 	}
 };
@@ -432,7 +432,7 @@ geode_ecb_decrypt(struct blkcipher_desc *desc,
 		op->src = walk.src.virt.addr,
 		op->dst = walk.dst.virt.addr;
 		op->mode = AES_MODE_ECB;
-		op->len = nbytes - (nbytes % AES_MIN_BLOCK_SIZE);
+		op->len = nbytes - (nbytes % AES_BLOCK_SIZE);
 		op->dir = AES_DIR_DECRYPT;
 
 		ret = geode_aes_crypt(op);
@@ -462,7 +462,7 @@ geode_ecb_encrypt(struct blkcipher_desc *desc,
 		op->src = walk.src.virt.addr,
 		op->dst = walk.dst.virt.addr;
 		op->mode = AES_MODE_ECB;
-		op->len = nbytes - (nbytes % AES_MIN_BLOCK_SIZE);
+		op->len = nbytes - (nbytes % AES_BLOCK_SIZE);
 		op->dir = AES_DIR_ENCRYPT;
 
 		ret = geode_aes_crypt(op);
@@ -482,7 +482,7 @@ static struct crypto_alg geode_ecb_alg = {
 						CRYPTO_ALG_NEED_FALLBACK,
 	.cra_init			=	fallback_init_blk,
 	.cra_exit			=	fallback_exit_blk,
-	.cra_blocksize		=	AES_MIN_BLOCK_SIZE,
+	.cra_blocksize		=	AES_BLOCK_SIZE,
 	.cra_ctxsize		=	sizeof(struct geode_aes_op),
 	.cra_alignmask		=	15,
 	.cra_type			=	&crypto_blkcipher_type,
@@ -547,7 +547,7 @@ static int geode_aes_probe(struct pci_dev *dev, const struct pci_device_id *id)
 	if (ret)
 		goto eecb;
 
-	printk(KERN_NOTICE "geode-aes: GEODE AES engine enabled.\n");
+	dev_notice(&dev->dev, "GEODE AES engine enabled.\n");
 	return 0;
 
  eecb:
@@ -565,7 +565,7 @@ static int geode_aes_probe(struct pci_dev *dev, const struct pci_device_id *id)
  eenable:
 	pci_disable_device(dev);
 
-	printk(KERN_ERR "geode-aes:  GEODE AES initialization failed.\n");
+	dev_err(&dev->dev, "GEODE AES initialization failed.\n");
 	return ret;
 }
 
diff --git a/drivers/crypto/geode-aes.h b/drivers/crypto/geode-aes.h
index f1855b50da4..f442ca972e3 100644
--- a/drivers/crypto/geode-aes.h
+++ b/drivers/crypto/geode-aes.h
@@ -10,10 +10,6 @@
 #define _GEODE_AES_H_
 
 /* driver logic flags */
-#define AES_IV_LENGTH  16
-#define AES_KEY_LENGTH 16
-#define AES_MIN_BLOCK_SIZE 16
-
 #define AES_MODE_ECB 0
 #define AES_MODE_CBC 1
 
@@ -64,7 +60,7 @@ struct geode_aes_op {
 	u32 flags;
 	int len;
 
-	u8 key[AES_KEY_LENGTH];
+	u8 key[AES_KEYSIZE_128];
 	u8 *iv;
 
 	union {
diff --git a/drivers/crypto/mv_cesa.c b/drivers/crypto/mv_cesa.c
index 8d1e6f8e9e9..29d0ee50490 100644
--- a/drivers/crypto/mv_cesa.c
+++ b/drivers/crypto/mv_cesa.c
@@ -622,8 +622,8 @@ static int queue_manag(void *data)
 		}
 
 		if (async_req) {
-			if (async_req->tfm->__crt_alg->cra_type !=
-			    &crypto_ahash_type) {
+			if (crypto_tfm_alg_type(async_req->tfm) !=
+			    CRYPTO_ALG_TYPE_AHASH) {
 				struct ablkcipher_request *req =
 				    ablkcipher_request_cast(async_req);
 				mv_start_new_crypt_req(req);
@@ -843,7 +843,7 @@ static int mv_hash_setkey(struct crypto_ahash *tfm, const u8 * key,
 static int mv_cra_hash_init(struct crypto_tfm *tfm, const char *base_hash_name,
 			    enum hash_op op, int count_add)
 {
-	const char *fallback_driver_name = tfm->__crt_alg->cra_name;
+	const char *fallback_driver_name = crypto_tfm_alg_name(tfm);
 	struct mv_tfm_hash_ctx *ctx = crypto_tfm_ctx(tfm);
 	struct crypto_shash *fallback_tfm = NULL;
 	struct crypto_shash *base_hash = NULL;
diff --git a/drivers/crypto/mxs-dcp.c b/drivers/crypto/mxs-dcp.c
index 7bbe0ab21ec..b5f7e6db24d 100644
--- a/drivers/crypto/mxs-dcp.c
+++ b/drivers/crypto/mxs-dcp.c
@@ -104,7 +104,6 @@ struct dcp_sha_req_ctx {
  * design of Linux Crypto API.
  */
 static struct dcp *global_sdcp;
-static DEFINE_MUTEX(global_mutex);
 
 /* DCP register layout. */
 #define MXS_DCP_CTRL				0x00
@@ -482,7 +481,7 @@ static int mxs_dcp_aes_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
 
 static int mxs_dcp_aes_fallback_init(struct crypto_tfm *tfm)
 {
-	const char *name = tfm->__crt_alg->cra_name;
+	const char *name = crypto_tfm_alg_name(tfm);
 	const uint32_t flags = CRYPTO_ALG_ASYNC | CRYPTO_ALG_NEED_FALLBACK;
 	struct dcp_async_ctx *actx = crypto_tfm_ctx(tfm);
 	struct crypto_ablkcipher *blk;
@@ -907,60 +906,49 @@ static int mxs_dcp_probe(struct platform_device *pdev)
 	struct resource *iores;
 	int dcp_vmi_irq, dcp_irq;
 
-	mutex_lock(&global_mutex);
 	if (global_sdcp) {
 		dev_err(dev, "Only one DCP instance allowed!\n");
-		ret = -ENODEV;
-		goto err_mutex;
+		return -ENODEV;
 	}
 
 	iores = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	dcp_vmi_irq = platform_get_irq(pdev, 0);
-	if (dcp_vmi_irq < 0) {
-		ret = dcp_vmi_irq;
-		goto err_mutex;
-	}
+	if (dcp_vmi_irq < 0)
+		return dcp_vmi_irq;
 
 	dcp_irq = platform_get_irq(pdev, 1);
-	if (dcp_irq < 0) {
-		ret = dcp_irq;
-		goto err_mutex;
-	}
+	if (dcp_irq < 0)
+		return dcp_irq;
 
 	sdcp = devm_kzalloc(dev, sizeof(*sdcp), GFP_KERNEL);
-	if (!sdcp) {
-		ret = -ENOMEM;
-		goto err_mutex;
-	}
+	if (!sdcp)
+		return -ENOMEM;
 
 	sdcp->dev = dev;
 	sdcp->base = devm_ioremap_resource(dev, iores);
-	if (IS_ERR(sdcp->base)) {
-		ret = PTR_ERR(sdcp->base);
-		goto err_mutex;
-	}
+	if (IS_ERR(sdcp->base))
+		return PTR_ERR(sdcp->base);
+
 
 	ret = devm_request_irq(dev, dcp_vmi_irq, mxs_dcp_irq, 0,
 			       "dcp-vmi-irq", sdcp);
 	if (ret) {
 		dev_err(dev, "Failed to claim DCP VMI IRQ!\n");
-		goto err_mutex;
+		return ret;
 	}
 
 	ret = devm_request_irq(dev, dcp_irq, mxs_dcp_irq, 0,
 			       "dcp-irq", sdcp);
 	if (ret) {
 		dev_err(dev, "Failed to claim DCP IRQ!\n");
-		goto err_mutex;
+		return ret;
 	}
 
 	/* Allocate coherent helper block. */
 	sdcp->coh = devm_kzalloc(dev, sizeof(*sdcp->coh) + DCP_ALIGNMENT,
 				   GFP_KERNEL);
-	if (!sdcp->coh) {
-		ret = -ENOMEM;
-		goto err_mutex;
-	}
+	if (!sdcp->coh)
+		return -ENOMEM;
 
 	/* Re-align the structure so it fits the DCP constraints. */
 	sdcp->coh = PTR_ALIGN(sdcp->coh, DCP_ALIGNMENT);
@@ -968,7 +956,7 @@ static int mxs_dcp_probe(struct platform_device *pdev)
 	/* Restart the DCP block. */
 	ret = stmp_reset_block(sdcp->base);
 	if (ret)
-		goto err_mutex;
+		return ret;
 
 	/* Initialize control register. */
 	writel(MXS_DCP_CTRL_GATHER_RESIDUAL_WRITES |
@@ -1006,8 +994,7 @@ static int mxs_dcp_probe(struct platform_device *pdev)
 						      NULL, "mxs_dcp_chan/sha");
 	if (IS_ERR(sdcp->thread[DCP_CHAN_HASH_SHA])) {
 		dev_err(dev, "Error starting SHA thread!\n");
-		ret = PTR_ERR(sdcp->thread[DCP_CHAN_HASH_SHA]);
-		goto err_mutex;
+		return PTR_ERR(sdcp->thread[DCP_CHAN_HASH_SHA]);
 	}
 
 	sdcp->thread[DCP_CHAN_CRYPTO] = kthread_run(dcp_chan_thread_aes,
@@ -1064,9 +1051,6 @@ err_destroy_aes_thread:
 
 err_destroy_sha_thread:
 	kthread_stop(sdcp->thread[DCP_CHAN_HASH_SHA]);
-
-err_mutex:
-	mutex_unlock(&global_mutex);
 	return ret;
 }
 
@@ -1088,9 +1072,7 @@ static int mxs_dcp_remove(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, NULL);
 
-	mutex_lock(&global_mutex);
 	global_sdcp = NULL;
-	mutex_unlock(&global_mutex);
 
 	return 0;
 }
diff --git a/drivers/crypto/n2_core.c b/drivers/crypto/n2_core.c
index e1f0ab413c3..7263c10a56e 100644
--- a/drivers/crypto/n2_core.c
+++ b/drivers/crypto/n2_core.c
@@ -356,7 +356,7 @@ static int n2_hash_async_finup(struct ahash_request *req)
 
 static int n2_hash_cra_init(struct crypto_tfm *tfm)
 {
-	const char *fallback_driver_name = tfm->__crt_alg->cra_name;
+	const char *fallback_driver_name = crypto_tfm_alg_name(tfm);
 	struct crypto_ahash *ahash = __crypto_ahash_cast(tfm);
 	struct n2_hash_ctx *ctx = crypto_ahash_ctx(ahash);
 	struct crypto_ahash *fallback_tfm;
@@ -391,7 +391,7 @@ static void n2_hash_cra_exit(struct crypto_tfm *tfm)
 
 static int n2_hmac_cra_init(struct crypto_tfm *tfm)
 {
-	const char *fallback_driver_name = tfm->__crt_alg->cra_name;
+	const char *fallback_driver_name = crypto_tfm_alg_name(tfm);
 	struct crypto_ahash *ahash = __crypto_ahash_cast(tfm);
 	struct n2_hmac_ctx *ctx = crypto_ahash_ctx(ahash);
 	struct n2_hmac_alg *n2alg = n2_hmac_alg(tfm);
diff --git a/drivers/crypto/nx/nx-842.c b/drivers/crypto/nx/nx-842.c
index 1e5481d88a2..502edf0a293 100644
--- a/drivers/crypto/nx/nx-842.c
+++ b/drivers/crypto/nx/nx-842.c
@@ -1197,12 +1197,7 @@ static int __init nx842_probe(struct vio_dev *viodev,
 	}
 
 	rcu_read_lock();
-	if (dev_set_drvdata(&viodev->dev, rcu_dereference(devdata))) {
-		rcu_read_unlock();
-		dev_err(&viodev->dev, "failed to set driver data for device\n");
-		ret = -1;
-		goto error;
-	}
+	dev_set_drvdata(&viodev->dev, rcu_dereference(devdata));
 	rcu_read_unlock();
 
 	if (sysfs_create_group(&viodev->dev.kobj, &nx842_attribute_group)) {
@@ -1234,7 +1229,7 @@ static int __exit nx842_remove(struct vio_dev *viodev)
 	old_devdata = rcu_dereference_check(devdata,
 			lockdep_is_held(&devdata_mutex));
 	of_reconfig_notifier_unregister(&nx842_of_nb);
-	rcu_assign_pointer(devdata, NULL);
+	RCU_INIT_POINTER(devdata, NULL);
 	spin_unlock_irqrestore(&devdata_mutex, flags);
 	synchronize_rcu();
 	dev_set_drvdata(&viodev->dev, NULL);
@@ -1285,7 +1280,7 @@ static void __exit nx842_exit(void)
 	spin_lock_irqsave(&devdata_mutex, flags);
 	old_devdata = rcu_dereference_check(devdata,
 			lockdep_is_held(&devdata_mutex));
-	rcu_assign_pointer(devdata, NULL);
+	RCU_INIT_POINTER(devdata, NULL);
 	spin_unlock_irqrestore(&devdata_mutex, flags);
 	synchronize_rcu();
 	if (old_devdata)
diff --git a/drivers/crypto/omap-des.c b/drivers/crypto/omap-des.c
index ec5f13162b7..b8bc84be874 100644
--- a/drivers/crypto/omap-des.c
+++ b/drivers/crypto/omap-des.c
@@ -223,12 +223,19 @@ static void omap_des_write_n(struct omap_des_dev *dd, u32 offset,
 
 static int omap_des_hw_init(struct omap_des_dev *dd)
 {
+	int err;
+
 	/*
 	 * clocks are enabled when request starts and disabled when finished.
 	 * It may be long delays between requests.
 	 * Device might go to off mode to save power.
 	 */
-	pm_runtime_get_sync(dd->dev);
+	err = pm_runtime_get_sync(dd->dev);
+	if (err < 0) {
+		pm_runtime_put_noidle(dd->dev);
+		dev_err(dd->dev, "%s: failed to get_sync(%d)\n", __func__, err);
+		return err;
+	}
 
 	if (!(dd->flags & FLAGS_INIT)) {
 		dd->flags |= FLAGS_INIT;
@@ -1074,16 +1081,20 @@ static int omap_des_probe(struct platform_device *pdev)
 	if (err)
 		goto err_res;
 
-	dd->io_base = devm_request_and_ioremap(dev, res);
-	if (!dd->io_base) {
-		dev_err(dev, "can't ioremap\n");
-		err = -ENOMEM;
+	dd->io_base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(dd->io_base)) {
+		err = PTR_ERR(dd->io_base);
 		goto err_res;
 	}
 	dd->phys_base = res->start;
 
 	pm_runtime_enable(dev);
-	pm_runtime_get_sync(dev);
+	err = pm_runtime_get_sync(dev);
+	if (err < 0) {
+		pm_runtime_put_noidle(dev);
+		dev_err(dd->dev, "%s: failed to get_sync(%d)\n", __func__, err);
+		goto err_get;
+	}
 
 	omap_des_dma_stop(dd);
 
@@ -1148,6 +1159,7 @@ err_algs:
 err_irq:
 	tasklet_kill(&dd->done_task);
 	tasklet_kill(&dd->queue_task);
+err_get:
 	pm_runtime_disable(dev);
 err_res:
 	dd = NULL;
@@ -1191,7 +1203,14 @@ static int omap_des_suspend(struct device *dev)
 
 static int omap_des_resume(struct device *dev)
 {
-	pm_runtime_get_sync(dev);
+	int err;
+
+	err = pm_runtime_get_sync(dev);
+	if (err < 0) {
+		pm_runtime_put_noidle(dev);
+		dev_err(dev, "%s: failed to get_sync(%d)\n", __func__, err);
+		return err;
+	}
 	return 0;
 }
 #endif
diff --git a/drivers/crypto/padlock-sha.c b/drivers/crypto/padlock-sha.c
index 9266c0e2549..bace885634f 100644
--- a/drivers/crypto/padlock-sha.c
+++ b/drivers/crypto/padlock-sha.c
@@ -211,7 +211,7 @@ static int padlock_sha256_final(struct shash_desc *desc, u8 *out)
 static int padlock_cra_init(struct crypto_tfm *tfm)
 {
 	struct crypto_shash *hash = __crypto_shash_cast(tfm);
-	const char *fallback_driver_name = tfm->__crt_alg->cra_name;
+	const char *fallback_driver_name = crypto_tfm_alg_name(tfm);
 	struct padlock_sha_ctx *ctx = crypto_tfm_ctx(tfm);
 	struct crypto_shash *fallback_tfm;
 	int err = -ENOMEM;
diff --git a/drivers/crypto/s5p-sss.c b/drivers/crypto/s5p-sss.c
index be45762f390..4197ad9a711 100644
--- a/drivers/crypto/s5p-sss.c
+++ b/drivers/crypto/s5p-sss.c
@@ -22,6 +22,7 @@
 #include <linux/scatterlist.h>
 #include <linux/dma-mapping.h>
 #include <linux/io.h>
+#include <linux/of.h>
 #include <linux/crypto.h>
 #include <linux/interrupt.h>
 
@@ -29,9 +30,6 @@
 #include <crypto/aes.h>
 #include <crypto/ctr.h>
 
-#include <plat/cpu.h>
-#include <mach/dma.h>
-
 #define _SBF(s, v)                      ((v) << (s))
 #define _BIT(b)                         _SBF(b, 1)
 
@@ -105,7 +103,7 @@
 #define SSS_REG_FCPKDMAO                0x005C
 
 /* AES registers */
-#define SSS_REG_AES_CONTROL             0x4000
+#define SSS_REG_AES_CONTROL		0x00
 #define SSS_AES_BYTESWAP_DI             _BIT(11)
 #define SSS_AES_BYTESWAP_DO             _BIT(10)
 #define SSS_AES_BYTESWAP_IV             _BIT(9)
@@ -121,21 +119,25 @@
 #define SSS_AES_CHAIN_MODE_CTR          _SBF(1, 0x02)
 #define SSS_AES_MODE_DECRYPT            _BIT(0)
 
-#define SSS_REG_AES_STATUS              0x4004
+#define SSS_REG_AES_STATUS		0x04
 #define SSS_AES_BUSY                    _BIT(2)
 #define SSS_AES_INPUT_READY             _BIT(1)
 #define SSS_AES_OUTPUT_READY            _BIT(0)
 
-#define SSS_REG_AES_IN_DATA(s)          (0x4010 + (s << 2))
-#define SSS_REG_AES_OUT_DATA(s)         (0x4020 + (s << 2))
-#define SSS_REG_AES_IV_DATA(s)          (0x4030 + (s << 2))
-#define SSS_REG_AES_CNT_DATA(s)         (0x4040 + (s << 2))
-#define SSS_REG_AES_KEY_DATA(s)         (0x4080 + (s << 2))
+#define SSS_REG_AES_IN_DATA(s)		(0x10 + (s << 2))
+#define SSS_REG_AES_OUT_DATA(s)		(0x20 + (s << 2))
+#define SSS_REG_AES_IV_DATA(s)		(0x30 + (s << 2))
+#define SSS_REG_AES_CNT_DATA(s)		(0x40 + (s << 2))
+#define SSS_REG_AES_KEY_DATA(s)		(0x80 + (s << 2))
 
 #define SSS_REG(dev, reg)               ((dev)->ioaddr + (SSS_REG_##reg))
 #define SSS_READ(dev, reg)              __raw_readl(SSS_REG(dev, reg))
 #define SSS_WRITE(dev, reg, val)        __raw_writel((val), SSS_REG(dev, reg))
 
+#define SSS_AES_REG(dev, reg)           ((dev)->aes_ioaddr + SSS_REG_##reg)
+#define SSS_AES_WRITE(dev, reg, val)    __raw_writel((val), \
+						SSS_AES_REG(dev, reg))
+
 /* HW engine modes */
 #define FLAGS_AES_DECRYPT               _BIT(0)
 #define FLAGS_AES_MODE_MASK             _SBF(1, 0x03)
@@ -145,6 +147,20 @@
 #define AES_KEY_LEN         16
 #define CRYPTO_QUEUE_LEN    1
 
+/**
+ * struct samsung_aes_variant - platform specific SSS driver data
+ * @has_hash_irq: true if SSS module uses hash interrupt, false otherwise
+ * @aes_offset: AES register offset from SSS module's base.
+ *
+ * Specifies platform specific configuration of SSS module.
+ * Note: A structure for driver specific platform data is used for future
+ * expansion of its usage.
+ */
+struct samsung_aes_variant {
+	bool			    has_hash_irq;
+	unsigned int		    aes_offset;
+};
+
 struct s5p_aes_reqctx {
 	unsigned long mode;
 };
@@ -161,6 +177,7 @@ struct s5p_aes_dev {
 	struct device              *dev;
 	struct clk                 *clk;
 	void __iomem               *ioaddr;
+	void __iomem               *aes_ioaddr;
 	int                         irq_hash;
 	int                         irq_fc;
 
@@ -173,10 +190,48 @@ struct s5p_aes_dev {
 	struct crypto_queue         queue;
 	bool                        busy;
 	spinlock_t                  lock;
+
+	struct samsung_aes_variant *variant;
 };
 
 static struct s5p_aes_dev *s5p_dev;
 
+static const struct samsung_aes_variant s5p_aes_data = {
+	.has_hash_irq	= true,
+	.aes_offset	= 0x4000,
+};
+
+static const struct samsung_aes_variant exynos_aes_data = {
+	.has_hash_irq	= false,
+	.aes_offset	= 0x200,
+};
+
+static const struct of_device_id s5p_sss_dt_match[] = {
+	{
+		.compatible = "samsung,s5pv210-secss",
+		.data = &s5p_aes_data,
+	},
+	{
+		.compatible = "samsung,exynos4210-secss",
+		.data = &exynos_aes_data,
+	},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, s5p_sss_dt_match);
+
+static inline struct samsung_aes_variant *find_s5p_sss_version
+				   (struct platform_device *pdev)
+{
+	if (IS_ENABLED(CONFIG_OF) && (pdev->dev.of_node)) {
+		const struct of_device_id *match;
+		match = of_match_node(s5p_sss_dt_match,
+					pdev->dev.of_node);
+		return (struct samsung_aes_variant *)match->data;
+	}
+	return (struct samsung_aes_variant *)
+			platform_get_device_id(pdev)->driver_data;
+}
+
 static void s5p_set_dma_indata(struct s5p_aes_dev *dev, struct scatterlist *sg)
 {
 	SSS_WRITE(dev, FCBRDMAS, sg_dma_address(sg));
@@ -272,8 +327,12 @@ static void s5p_aes_tx(struct s5p_aes_dev *dev)
 		}
 
 		s5p_set_dma_outdata(dev, dev->sg_dst);
-	} else
+	} else {
 		s5p_aes_complete(dev, err);
+
+		dev->busy = true;
+		tasklet_schedule(&dev->tasklet);
+	}
 }
 
 static void s5p_aes_rx(struct s5p_aes_dev *dev)
@@ -322,14 +381,15 @@ static void s5p_set_aes(struct s5p_aes_dev *dev,
 {
 	void __iomem *keystart;
 
-	memcpy(dev->ioaddr + SSS_REG_AES_IV_DATA(0), iv, 0x10);
+	if (iv)
+		memcpy(dev->aes_ioaddr + SSS_REG_AES_IV_DATA(0), iv, 0x10);
 
 	if (keylen == AES_KEYSIZE_256)
-		keystart = dev->ioaddr + SSS_REG_AES_KEY_DATA(0);
+		keystart = dev->aes_ioaddr + SSS_REG_AES_KEY_DATA(0);
 	else if (keylen == AES_KEYSIZE_192)
-		keystart = dev->ioaddr + SSS_REG_AES_KEY_DATA(2);
+		keystart = dev->aes_ioaddr + SSS_REG_AES_KEY_DATA(2);
 	else
-		keystart = dev->ioaddr + SSS_REG_AES_KEY_DATA(4);
+		keystart = dev->aes_ioaddr + SSS_REG_AES_KEY_DATA(4);
 
 	memcpy(keystart, key, keylen);
 }
@@ -379,7 +439,7 @@ static void s5p_aes_crypt_start(struct s5p_aes_dev *dev, unsigned long mode)
 	if (err)
 		goto outdata_error;
 
-	SSS_WRITE(dev, AES_CONTROL, aes_control);
+	SSS_AES_WRITE(dev, AES_CONTROL, aes_control);
 	s5p_set_aes(dev, dev->ctx->aes_key, req->info, dev->ctx->keylen);
 
 	s5p_set_dma_indata(dev,  req->src);
@@ -410,10 +470,13 @@ static void s5p_tasklet_cb(unsigned long data)
 	spin_lock_irqsave(&dev->lock, flags);
 	backlog   = crypto_get_backlog(&dev->queue);
 	async_req = crypto_dequeue_request(&dev->queue);
-	spin_unlock_irqrestore(&dev->lock, flags);
 
-	if (!async_req)
+	if (!async_req) {
+		dev->busy = false;
+		spin_unlock_irqrestore(&dev->lock, flags);
 		return;
+	}
+	spin_unlock_irqrestore(&dev->lock, flags);
 
 	if (backlog)
 		backlog->complete(backlog, -EINPROGRESS);
@@ -432,14 +495,13 @@ static int s5p_aes_handle_req(struct s5p_aes_dev *dev,
 	int err;
 
 	spin_lock_irqsave(&dev->lock, flags);
+	err = ablkcipher_enqueue_request(&dev->queue, req);
 	if (dev->busy) {
-		err = -EAGAIN;
 		spin_unlock_irqrestore(&dev->lock, flags);
 		goto exit;
 	}
 	dev->busy = true;
 
-	err = ablkcipher_enqueue_request(&dev->queue, req);
 	spin_unlock_irqrestore(&dev->lock, flags);
 
 	tasklet_schedule(&dev->tasklet);
@@ -564,6 +626,7 @@ static int s5p_aes_probe(struct platform_device *pdev)
 	struct s5p_aes_dev *pdata;
 	struct device      *dev = &pdev->dev;
 	struct resource    *res;
+	struct samsung_aes_variant *variant;
 
 	if (s5p_dev)
 		return -EEXIST;
@@ -577,30 +640,25 @@ static int s5p_aes_probe(struct platform_device *pdev)
 	if (IS_ERR(pdata->ioaddr))
 		return PTR_ERR(pdata->ioaddr);
 
+	variant = find_s5p_sss_version(pdev);
+
 	pdata->clk = devm_clk_get(dev, "secss");
 	if (IS_ERR(pdata->clk)) {
 		dev_err(dev, "failed to find secss clock source\n");
 		return -ENOENT;
 	}
 
-	clk_enable(pdata->clk);
+	err = clk_prepare_enable(pdata->clk);
+	if (err < 0) {
+		dev_err(dev, "Enabling SSS clk failed, err %d\n", err);
+		return err;
+	}
 
 	spin_lock_init(&pdata->lock);
 
-	pdata->irq_hash = platform_get_irq_byname(pdev, "hash");
-	if (pdata->irq_hash < 0) {
-		err = pdata->irq_hash;
-		dev_warn(dev, "hash interrupt is not available.\n");
-		goto err_irq;
-	}
-	err = devm_request_irq(dev, pdata->irq_hash, s5p_aes_interrupt,
-			       IRQF_SHARED, pdev->name, pdev);
-	if (err < 0) {
-		dev_warn(dev, "hash interrupt is not available.\n");
-		goto err_irq;
-	}
+	pdata->aes_ioaddr = pdata->ioaddr + variant->aes_offset;
 
-	pdata->irq_fc = platform_get_irq_byname(pdev, "feed control");
+	pdata->irq_fc = platform_get_irq(pdev, 0);
 	if (pdata->irq_fc < 0) {
 		err = pdata->irq_fc;
 		dev_warn(dev, "feed control interrupt is not available.\n");
@@ -613,6 +671,23 @@ static int s5p_aes_probe(struct platform_device *pdev)
 		goto err_irq;
 	}
 
+	if (variant->has_hash_irq) {
+		pdata->irq_hash = platform_get_irq(pdev, 1);
+		if (pdata->irq_hash < 0) {
+			err = pdata->irq_hash;
+			dev_warn(dev, "hash interrupt is not available.\n");
+			goto err_irq;
+		}
+		err = devm_request_irq(dev, pdata->irq_hash, s5p_aes_interrupt,
+				       IRQF_SHARED, pdev->name, pdev);
+		if (err < 0) {
+			dev_warn(dev, "hash interrupt is not available.\n");
+			goto err_irq;
+		}
+	}
+
+	pdata->busy = false;
+	pdata->variant = variant;
 	pdata->dev = dev;
 	platform_set_drvdata(pdev, pdata);
 	s5p_dev = pdata;
@@ -639,7 +714,7 @@ static int s5p_aes_probe(struct platform_device *pdev)
 	tasklet_kill(&pdata->tasklet);
 
  err_irq:
-	clk_disable(pdata->clk);
+	clk_disable_unprepare(pdata->clk);
 
 	s5p_dev = NULL;
 
@@ -659,7 +734,7 @@ static int s5p_aes_remove(struct platform_device *pdev)
 
 	tasklet_kill(&pdata->tasklet);
 
-	clk_disable(pdata->clk);
+	clk_disable_unprepare(pdata->clk);
 
 	s5p_dev = NULL;
 
@@ -672,6 +747,7 @@ static struct platform_driver s5p_aes_crypto = {
 	.driver	= {
 		.owner	= THIS_MODULE,
 		.name	= "s5p-secss",
+		.of_match_table = s5p_sss_dt_match,
 	},
 };
 
diff --git a/drivers/crypto/sahara.c b/drivers/crypto/sahara.c
index 07a5987ce67..164e1ec624e 100644
--- a/drivers/crypto/sahara.c
+++ b/drivers/crypto/sahara.c
@@ -728,7 +728,7 @@ static int sahara_aes_cbc_decrypt(struct ablkcipher_request *req)
 
 static int sahara_aes_cra_init(struct crypto_tfm *tfm)
 {
-	const char *name = tfm->__crt_alg->cra_name;
+	const char *name = crypto_tfm_alg_name(tfm);
 	struct sahara_ctx *ctx = crypto_tfm_ctx(tfm);
 
 	ctx->fallback = crypto_alloc_ablkcipher(name, 0,
diff --git a/drivers/devfreq/Kconfig b/drivers/devfreq/Kconfig
index 7d2f4355070..49e74c1fc63 100644
--- a/drivers/devfreq/Kconfig
+++ b/drivers/devfreq/Kconfig
@@ -70,19 +70,20 @@ config ARM_EXYNOS4_BUS_DEVFREQ
 	depends on (CPU_EXYNOS4210 || SOC_EXYNOS4212 || SOC_EXYNOS4412) && !ARCH_MULTIPLATFORM
 	select ARCH_HAS_OPP
 	select DEVFREQ_GOV_SIMPLE_ONDEMAND
+	select PM_OPP
 	help
 	  This adds the DEVFREQ driver for Exynos4210 memory bus (vdd_int)
 	  and Exynos4212/4412 memory interface and bus (vdd_mif + vdd_int).
 	  It reads PPMU counters of memory controllers and adjusts
 	  the operating frequencies and voltages with OPP support.
-	  To operate with optimal voltages, ASV support is required
-	  (CONFIG_EXYNOS_ASV).
+	  This does not yet operate with optimal voltages.
 
 config ARM_EXYNOS5_BUS_DEVFREQ
 	bool "ARM Exynos5250 Bus DEVFREQ Driver"
 	depends on SOC_EXYNOS5250
 	select ARCH_HAS_OPP
 	select DEVFREQ_GOV_SIMPLE_ONDEMAND
+	select PM_OPP
 	help
 	  This adds the DEVFREQ driver for Exynos5250 bus interface (vdd_int).
 	  It reads PPMU counters of memory controllers and adjusts the
diff --git a/drivers/devfreq/devfreq.c b/drivers/devfreq/devfreq.c
index 2042ec3656b..9f90369dd6b 100644
--- a/drivers/devfreq/devfreq.c
+++ b/drivers/devfreq/devfreq.c
@@ -394,7 +394,7 @@ static int devfreq_notifier_call(struct notifier_block *nb, unsigned long type,
  * @devfreq:	the devfreq struct
  * @skip:	skip calling device_unregister().
  */
-static void _remove_devfreq(struct devfreq *devfreq, bool skip)
+static void _remove_devfreq(struct devfreq *devfreq)
 {
 	mutex_lock(&devfreq_list_lock);
 	if (IS_ERR(find_device_devfreq(devfreq->dev.parent))) {
@@ -412,11 +412,6 @@ static void _remove_devfreq(struct devfreq *devfreq, bool skip)
 	if (devfreq->profile->exit)
 		devfreq->profile->exit(devfreq->dev.parent);
 
-	if (!skip && get_device(&devfreq->dev)) {
-		device_unregister(&devfreq->dev);
-		put_device(&devfreq->dev);
-	}
-
 	mutex_destroy(&devfreq->lock);
 	kfree(devfreq);
 }
@@ -426,14 +421,12 @@ static void _remove_devfreq(struct devfreq *devfreq, bool skip)
  * @dev:	the devfreq device
  *
  * This calls _remove_devfreq() if _remove_devfreq() is not called.
- * Note that devfreq_dev_release() could be called by _remove_devfreq() as
- * well as by others unregistering the device.
  */
 static void devfreq_dev_release(struct device *dev)
 {
 	struct devfreq *devfreq = to_devfreq(dev);
 
-	_remove_devfreq(devfreq, true);
+	_remove_devfreq(devfreq);
 }
 
 /**
@@ -544,12 +537,76 @@ int devfreq_remove_device(struct devfreq *devfreq)
 	if (!devfreq)
 		return -EINVAL;
 
-	_remove_devfreq(devfreq, false);
+	device_unregister(&devfreq->dev);
+	put_device(&devfreq->dev);
 
 	return 0;
 }
 EXPORT_SYMBOL(devfreq_remove_device);
 
+static int devm_devfreq_dev_match(struct device *dev, void *res, void *data)
+{
+	struct devfreq **r = res;
+
+	if (WARN_ON(!r || !*r))
+		return 0;
+
+	return *r == data;
+}
+
+static void devm_devfreq_dev_release(struct device *dev, void *res)
+{
+	devfreq_remove_device(*(struct devfreq **)res);
+}
+
+/**
+ * devm_devfreq_add_device() - Resource-managed devfreq_add_device()
+ * @dev:	the device to add devfreq feature.
+ * @profile:	device-specific profile to run devfreq.
+ * @governor_name:	name of the policy to choose frequency.
+ * @data:	private data for the governor. The devfreq framework does not
+ *		touch this value.
+ *
+ * This function manages automatically the memory of devfreq device using device
+ * resource management and simplify the free operation for memory of devfreq
+ * device.
+ */
+struct devfreq *devm_devfreq_add_device(struct device *dev,
+					struct devfreq_dev_profile *profile,
+					const char *governor_name,
+					void *data)
+{
+	struct devfreq **ptr, *devfreq;
+
+	ptr = devres_alloc(devm_devfreq_dev_release, sizeof(*ptr), GFP_KERNEL);
+	if (!ptr)
+		return ERR_PTR(-ENOMEM);
+
+	devfreq = devfreq_add_device(dev, profile, governor_name, data);
+	if (IS_ERR(devfreq)) {
+		devres_free(ptr);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	*ptr = devfreq;
+	devres_add(dev, ptr);
+
+	return devfreq;
+}
+EXPORT_SYMBOL(devm_devfreq_add_device);
+
+/**
+ * devm_devfreq_remove_device() - Resource-managed devfreq_remove_device()
+ * @dev:	the device to add devfreq feature.
+ * @devfreq:	the devfreq instance to be removed
+ */
+void devm_devfreq_remove_device(struct device *dev, struct devfreq *devfreq)
+{
+	WARN_ON(devres_release(dev, devm_devfreq_dev_release,
+			       devm_devfreq_dev_match, devfreq));
+}
+EXPORT_SYMBOL(devm_devfreq_remove_device);
+
 /**
  * devfreq_suspend_device() - Suspend devfreq of a device.
  * @devfreq: the devfreq instance to be suspended
@@ -1112,6 +1169,54 @@ int devfreq_unregister_opp_notifier(struct device *dev, struct devfreq *devfreq)
 	return ret;
 }
 
+static void devm_devfreq_opp_release(struct device *dev, void *res)
+{
+	devfreq_unregister_opp_notifier(dev, *(struct devfreq **)res);
+}
+
+/**
+ * devm_ devfreq_register_opp_notifier()
+ *		- Resource-managed devfreq_register_opp_notifier()
+ * @dev:	The devfreq user device. (parent of devfreq)
+ * @devfreq:	The devfreq object.
+ */
+int devm_devfreq_register_opp_notifier(struct device *dev,
+				       struct devfreq *devfreq)
+{
+	struct devfreq **ptr;
+	int ret;
+
+	ptr = devres_alloc(devm_devfreq_opp_release, sizeof(*ptr), GFP_KERNEL);
+	if (!ptr)
+		return -ENOMEM;
+
+	ret = devfreq_register_opp_notifier(dev, devfreq);
+	if (ret) {
+		devres_free(ptr);
+		return ret;
+	}
+
+	*ptr = devfreq;
+	devres_add(dev, ptr);
+
+	return 0;
+}
+EXPORT_SYMBOL(devm_devfreq_register_opp_notifier);
+
+/**
+ * devm_devfreq_unregister_opp_notifier()
+ *		- Resource-managed devfreq_unregister_opp_notifier()
+ * @dev:	The devfreq user device. (parent of devfreq)
+ * @devfreq:	The devfreq object.
+ */
+void devm_devfreq_unregister_opp_notifier(struct device *dev,
+					 struct devfreq *devfreq)
+{
+	WARN_ON(devres_release(dev, devm_devfreq_opp_release,
+			       devm_devfreq_dev_match, devfreq));
+}
+EXPORT_SYMBOL(devm_devfreq_unregister_opp_notifier);
+
 MODULE_AUTHOR("MyungJoo Ham <myungjoo.ham@samsung.com>");
 MODULE_DESCRIPTION("devfreq class support");
 MODULE_LICENSE("GPL");
diff --git a/drivers/devfreq/exynos/Makefile b/drivers/devfreq/exynos/Makefile
index bfaaf5b0d61..49bc9175f92 100644
--- a/drivers/devfreq/exynos/Makefile
+++ b/drivers/devfreq/exynos/Makefile
@@ -1,3 +1,3 @@
 # Exynos DEVFREQ Drivers
-obj-$(CONFIG_ARM_EXYNOS4_BUS_DEVFREQ)	+= exynos4_bus.o
+obj-$(CONFIG_ARM_EXYNOS4_BUS_DEVFREQ)	+= exynos_ppmu.o exynos4_bus.o
 obj-$(CONFIG_ARM_EXYNOS5_BUS_DEVFREQ)	+= exynos_ppmu.o exynos5_bus.o
diff --git a/drivers/devfreq/exynos/exynos4_bus.c b/drivers/devfreq/exynos/exynos4_bus.c
index e07b0c68c71..d9b08d3b683 100644
--- a/drivers/devfreq/exynos/exynos4_bus.c
+++ b/drivers/devfreq/exynos/exynos4_bus.c
@@ -25,13 +25,9 @@
 #include <linux/regulator/consumer.h>
 #include <linux/module.h>
 
-/* Exynos4 ASV has been in the mailing list, but not upstreamed, yet. */
-#ifdef CONFIG_EXYNOS_ASV
-extern unsigned int exynos_result_of_asv;
-#endif
-
 #include <mach/map.h>
 
+#include "exynos_ppmu.h"
 #include "exynos4_bus.h"
 
 #define MAX_SAFEVOLT	1200000 /* 1.2V */
@@ -44,22 +40,6 @@ enum exynos4_busf_type {
 /* Assume that the bus is saturated if the utilization is 40% */
 #define BUS_SATURATION_RATIO	40
 
-enum ppmu_counter {
-	PPMU_PMNCNT0 = 0,
-	PPMU_PMCCNT1,
-	PPMU_PMNCNT2,
-	PPMU_PMNCNT3,
-	PPMU_PMNCNT_MAX,
-};
-struct exynos4_ppmu {
-	void __iomem *hw_base;
-	unsigned int ccnt;
-	unsigned int event;
-	unsigned int count[PPMU_PMNCNT_MAX];
-	bool ccnt_overflow;
-	bool count_overflow[PPMU_PMNCNT_MAX];
-};
-
 enum busclk_level_idx {
 	LV_0 = 0,
 	LV_1,
@@ -68,6 +48,13 @@ enum busclk_level_idx {
 	LV_4,
 	_LV_END
 };
+
+enum exynos_ppmu_idx {
+	PPMU_DMC0,
+	PPMU_DMC1,
+	PPMU_END,
+};
+
 #define EX4210_LV_MAX	LV_2
 #define EX4x12_LV_MAX	LV_4
 #define EX4210_LV_NUM	(LV_2 + 1)
@@ -91,7 +78,7 @@ struct busfreq_data {
 	struct regulator *vdd_int;
 	struct regulator *vdd_mif; /* Exynos4412/4212 only */
 	struct busfreq_opp_info curr_oppinfo;
-	struct exynos4_ppmu dmc[2];
+	struct busfreq_ppmu_data ppmu_data;
 
 	struct notifier_block pm_notifier;
 	struct mutex lock;
@@ -101,12 +88,6 @@ struct busfreq_data {
 	unsigned int top_divtable[_LV_END];
 };
 
-struct bus_opp_table {
-	unsigned int idx;
-	unsigned long clk;
-	unsigned long volt;
-};
-
 /* 4210 controls clock of mif and voltage of int */
 static struct bus_opp_table exynos4210_busclk_table[] = {
 	{LV_0, 400000, 1150000},
@@ -524,57 +505,6 @@ static int exynos4x12_set_busclk(struct busfreq_data *data,
 	return 0;
 }
 
-
-static void busfreq_mon_reset(struct busfreq_data *data)
-{
-	unsigned int i;
-
-	for (i = 0; i < 2; i++) {
-		void __iomem *ppmu_base = data->dmc[i].hw_base;
-
-		/* Reset PPMU */
-		__raw_writel(0x8000000f, ppmu_base + 0xf010);
-		__raw_writel(0x8000000f, ppmu_base + 0xf050);
-		__raw_writel(0x6, ppmu_base + 0xf000);
-		__raw_writel(0x0, ppmu_base + 0xf100);
-
-		/* Set PPMU Event */
-		data->dmc[i].event = 0x6;
-		__raw_writel(((data->dmc[i].event << 12) | 0x1),
-			     ppmu_base + 0xfc);
-
-		/* Start PPMU */
-		__raw_writel(0x1, ppmu_base + 0xf000);
-	}
-}
-
-static void exynos4_read_ppmu(struct busfreq_data *data)
-{
-	int i, j;
-
-	for (i = 0; i < 2; i++) {
-		void __iomem *ppmu_base = data->dmc[i].hw_base;
-		u32 overflow;
-
-		/* Stop PPMU */
-		__raw_writel(0x0, ppmu_base + 0xf000);
-
-		/* Update local data from PPMU */
-		overflow = __raw_readl(ppmu_base + 0xf050);
-
-		data->dmc[i].ccnt = __raw_readl(ppmu_base + 0xf100);
-		data->dmc[i].ccnt_overflow = overflow & (1 << 31);
-
-		for (j = 0; j < PPMU_PMNCNT_MAX; j++) {
-			data->dmc[i].count[j] = __raw_readl(
-					ppmu_base + (0xf110 + (0x10 * j)));
-			data->dmc[i].count_overflow[j] = overflow & (1 << j);
-		}
-	}
-
-	busfreq_mon_reset(data);
-}
-
 static int exynos4x12_get_intspec(unsigned long mifclk)
 {
 	int i = 0;
@@ -698,84 +628,35 @@ out:
 	return err;
 }
 
-static int exynos4_get_busier_dmc(struct busfreq_data *data)
-{
-	u64 p0 = data->dmc[0].count[0];
-	u64 p1 = data->dmc[1].count[0];
-
-	p0 *= data->dmc[1].ccnt;
-	p1 *= data->dmc[0].ccnt;
-
-	if (data->dmc[1].ccnt == 0)
-		return 0;
-
-	if (p0 > p1)
-		return 0;
-	return 1;
-}
-
 static int exynos4_bus_get_dev_status(struct device *dev,
 				      struct devfreq_dev_status *stat)
 {
 	struct busfreq_data *data = dev_get_drvdata(dev);
-	int busier_dmc;
-	int cycles_x2 = 2; /* 2 x cycles */
-	void __iomem *addr;
-	u32 timing;
-	u32 memctrl;
-
-	exynos4_read_ppmu(data);
-	busier_dmc = exynos4_get_busier_dmc(data);
-	stat->current_frequency = data->curr_oppinfo.rate;
+	struct busfreq_ppmu_data *ppmu_data = &data->ppmu_data;
+	int busier;
 
-	if (busier_dmc)
-		addr = S5P_VA_DMC1;
-	else
-		addr = S5P_VA_DMC0;
-
-	memctrl = __raw_readl(addr + 0x04); /* one of DDR2/3/LPDDR2 */
-	timing = __raw_readl(addr + 0x38); /* CL or WL/RL values */
-
-	switch ((memctrl >> 8) & 0xf) {
-	case 0x4: /* DDR2 */
-		cycles_x2 = ((timing >> 16) & 0xf) * 2;
-		break;
-	case 0x5: /* LPDDR2 */
-	case 0x6: /* DDR3 */
-		cycles_x2 = ((timing >> 8) & 0xf) + ((timing >> 0) & 0xf);
-		break;
-	default:
-		pr_err("%s: Unknown Memory Type(%d).\n", __func__,
-		       (memctrl >> 8) & 0xf);
-		return -EINVAL;
-	}
+	exynos_read_ppmu(ppmu_data);
+	busier = exynos_get_busier_ppmu(ppmu_data);
+	stat->current_frequency = data->curr_oppinfo.rate;
 
 	/* Number of cycles spent on memory access */
-	stat->busy_time = data->dmc[busier_dmc].count[0] / 2 * (cycles_x2 + 2);
+	stat->busy_time = ppmu_data->ppmu[busier].count[PPMU_PMNCNT3];
 	stat->busy_time *= 100 / BUS_SATURATION_RATIO;
-	stat->total_time = data->dmc[busier_dmc].ccnt;
+	stat->total_time = ppmu_data->ppmu[busier].ccnt;
 
 	/* If the counters have overflown, retry */
-	if (data->dmc[busier_dmc].ccnt_overflow ||
-	    data->dmc[busier_dmc].count_overflow[0])
+	if (ppmu_data->ppmu[busier].ccnt_overflow ||
+	    ppmu_data->ppmu[busier].count_overflow[0])
 		return -EAGAIN;
 
 	return 0;
 }
 
-static void exynos4_bus_exit(struct device *dev)
-{
-	struct busfreq_data *data = dev_get_drvdata(dev);
-
-	devfreq_unregister_opp_notifier(dev, data->devfreq);
-}
-
 static struct devfreq_dev_profile exynos4_devfreq_profile = {
 	.initial_freq	= 400000,
 	.polling_ms	= 50,
 	.target		= exynos4_bus_target,
 	.get_dev_status	= exynos4_bus_get_dev_status,
-	.exit		= exynos4_bus_exit,
 };
 
 static int exynos4210_init_tables(struct busfreq_data *data)
@@ -837,11 +718,11 @@ static int exynos4210_init_tables(struct busfreq_data *data)
 		data->top_divtable[i] = tmp;
 	}
 
-#ifdef CONFIG_EXYNOS_ASV
-	tmp = exynos4_result_of_asv;
-#else
+	/*
+	 * TODO: init tmp based on busfreq_data
+	 * (device-tree or platform-data)
+	 */
 	tmp = 0; /* Max voltages for the reliability of the unknown */
-#endif
 
 	pr_debug("ASV Group of Exynos4 is %d\n", tmp);
 	/* Use merged grouping for voltage */
@@ -922,11 +803,7 @@ static int exynos4x12_init_tables(struct busfreq_data *data)
 		data->dmc_divtable[i] = tmp;
 	}
 
-#ifdef CONFIG_EXYNOS_ASV
-	tmp = exynos4_result_of_asv;
-#else
 	tmp = 0; /* Max voltages for the reliability of the unknown */
-#endif
 
 	if (tmp > 8)
 		tmp = 0;
@@ -1020,6 +897,7 @@ unlock:
 static int exynos4_busfreq_probe(struct platform_device *pdev)
 {
 	struct busfreq_data *data;
+	struct busfreq_ppmu_data *ppmu_data;
 	struct dev_pm_opp *opp;
 	struct device *dev = &pdev->dev;
 	int err = 0;
@@ -1030,9 +908,19 @@ static int exynos4_busfreq_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
+	ppmu_data = &data->ppmu_data;
+	ppmu_data->ppmu_end = PPMU_END;
+	ppmu_data->ppmu = devm_kzalloc(dev,
+				       sizeof(struct exynos_ppmu) * PPMU_END,
+				       GFP_KERNEL);
+	if (!ppmu_data->ppmu) {
+		dev_err(dev, "Failed to allocate memory for exynos_ppmu\n");
+		return -ENOMEM;
+	}
+
 	data->type = pdev->id_entry->driver_data;
-	data->dmc[0].hw_base = S5P_VA_DMC0;
-	data->dmc[1].hw_base = S5P_VA_DMC1;
+	ppmu_data->ppmu[PPMU_DMC0].hw_base = S5P_VA_DMC0;
+	ppmu_data->ppmu[PPMU_DMC1].hw_base = S5P_VA_DMC1;
 	data->pm_notifier.notifier_call = exynos4_busfreq_pm_notifier_event;
 	data->dev = dev;
 	mutex_init(&data->lock);
@@ -1048,8 +936,11 @@ static int exynos4_busfreq_probe(struct platform_device *pdev)
 		dev_err(dev, "Cannot determine the device id %d\n", data->type);
 		err = -EINVAL;
 	}
-	if (err)
+	if (err) {
+		dev_err(dev, "Cannot initialize busfreq table %d\n",
+			     data->type);
 		return err;
+	}
 
 	data->vdd_int = devm_regulator_get(dev, "vdd_int");
 	if (IS_ERR(data->vdd_int)) {
@@ -1079,19 +970,28 @@ static int exynos4_busfreq_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, data);
 
-	busfreq_mon_reset(data);
-
-	data->devfreq = devfreq_add_device(dev, &exynos4_devfreq_profile,
+	data->devfreq = devm_devfreq_add_device(dev, &exynos4_devfreq_profile,
 					   "simple_ondemand", NULL);
 	if (IS_ERR(data->devfreq))
 		return PTR_ERR(data->devfreq);
 
-	devfreq_register_opp_notifier(dev, data->devfreq);
+	/*
+	 * Start PPMU (Performance Profiling Monitoring Unit) to check
+	 * utilization of each IP in the Exynos4 SoC.
+	 */
+	busfreq_mon_reset(ppmu_data);
 
+	/* Register opp_notifier for Exynos4 busfreq */
+	err = devm_devfreq_register_opp_notifier(dev, data->devfreq);
+	if (err < 0) {
+		dev_err(dev, "Failed to register opp notifier\n");
+		return err;
+	}
+
+	/* Register pm_notifier for Exynos4 busfreq */
 	err = register_pm_notifier(&data->pm_notifier);
 	if (err) {
 		dev_err(dev, "Failed to setup pm notifier\n");
-		devfreq_remove_device(data->devfreq);
 		return err;
 	}
 
@@ -1102,23 +1002,24 @@ static int exynos4_busfreq_remove(struct platform_device *pdev)
 {
 	struct busfreq_data *data = platform_get_drvdata(pdev);
 
+	/* Unregister all of notifier chain */
 	unregister_pm_notifier(&data->pm_notifier);
-	devfreq_remove_device(data->devfreq);
 
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
 static int exynos4_busfreq_resume(struct device *dev)
 {
 	struct busfreq_data *data = dev_get_drvdata(dev);
+	struct busfreq_ppmu_data *ppmu_data = &data->ppmu_data;
 
-	busfreq_mon_reset(data);
+	busfreq_mon_reset(ppmu_data);
 	return 0;
 }
+#endif
 
-static const struct dev_pm_ops exynos4_busfreq_pm = {
-	.resume	= exynos4_busfreq_resume,
-};
+static SIMPLE_DEV_PM_OPS(exynos4_busfreq_pm_ops, NULL, exynos4_busfreq_resume);
 
 static const struct platform_device_id exynos4_busfreq_id[] = {
 	{ "exynos4210-busfreq", TYPE_BUSF_EXYNOS4210 },
@@ -1134,7 +1035,7 @@ static struct platform_driver exynos4_busfreq_driver = {
 	.driver = {
 		.name	= "exynos4-busfreq",
 		.owner	= THIS_MODULE,
-		.pm	= &exynos4_busfreq_pm,
+		.pm	= &exynos4_busfreq_pm_ops,
 	},
 };
 
diff --git a/drivers/devfreq/exynos/exynos5_bus.c b/drivers/devfreq/exynos/exynos5_bus.c
index 6eef1f7397c..6cd0392e279 100644
--- a/drivers/devfreq/exynos/exynos5_bus.c
+++ b/drivers/devfreq/exynos/exynos5_bus.c
@@ -50,7 +50,7 @@ struct busfreq_data_int {
 	struct device *dev;
 	struct devfreq *devfreq;
 	struct regulator *vdd_int;
-	struct exynos_ppmu ppmu[PPMU_END];
+	struct busfreq_ppmu_data ppmu_data;
 	unsigned long curr_freq;
 	bool disabled;
 
@@ -75,49 +75,6 @@ static struct int_bus_opp_table exynos5_int_opp_table[] = {
 	{0, 0, 0},
 };
 
-static void busfreq_mon_reset(struct busfreq_data_int *data)
-{
-	unsigned int i;
-
-	for (i = PPMU_RIGHT; i < PPMU_END; i++) {
-		void __iomem *ppmu_base = data->ppmu[i].hw_base;
-
-		/* Reset the performance and cycle counters */
-		exynos_ppmu_reset(ppmu_base);
-
-		/* Setup count registers to monitor read/write transactions */
-		data->ppmu[i].event[PPMU_PMNCNT3] = RDWR_DATA_COUNT;
-		exynos_ppmu_setevent(ppmu_base, PPMU_PMNCNT3,
-					data->ppmu[i].event[PPMU_PMNCNT3]);
-
-		exynos_ppmu_start(ppmu_base);
-	}
-}
-
-static void exynos5_read_ppmu(struct busfreq_data_int *data)
-{
-	int i, j;
-
-	for (i = PPMU_RIGHT; i < PPMU_END; i++) {
-		void __iomem *ppmu_base = data->ppmu[i].hw_base;
-
-		exynos_ppmu_stop(ppmu_base);
-
-		/* Update local data from PPMU */
-		data->ppmu[i].ccnt = __raw_readl(ppmu_base + PPMU_CCNT);
-
-		for (j = PPMU_PMNCNT0; j < PPMU_PMNCNT_MAX; j++) {
-			if (data->ppmu[i].event[j] == 0)
-				data->ppmu[i].count[j] = 0;
-			else
-				data->ppmu[i].count[j] =
-					exynos_ppmu_read(ppmu_base, j);
-		}
-	}
-
-	busfreq_mon_reset(data);
-}
-
 static int exynos5_int_setvolt(struct busfreq_data_int *data,
 				unsigned long volt)
 {
@@ -185,59 +142,33 @@ out:
 	return err;
 }
 
-static int exynos5_get_busier_dmc(struct busfreq_data_int *data)
-{
-	int i, j;
-	int busy = 0;
-	unsigned int temp = 0;
-
-	for (i = PPMU_RIGHT; i < PPMU_END; i++) {
-		for (j = PPMU_PMNCNT0; j < PPMU_PMNCNT_MAX; j++) {
-			if (data->ppmu[i].count[j] > temp) {
-				temp = data->ppmu[i].count[j];
-				busy = i;
-			}
-		}
-	}
-
-	return busy;
-}
-
 static int exynos5_int_get_dev_status(struct device *dev,
 				      struct devfreq_dev_status *stat)
 {
 	struct platform_device *pdev = container_of(dev, struct platform_device,
 						    dev);
 	struct busfreq_data_int *data = platform_get_drvdata(pdev);
+	struct busfreq_ppmu_data *ppmu_data = &data->ppmu_data;
 	int busier_dmc;
 
-	exynos5_read_ppmu(data);
-	busier_dmc = exynos5_get_busier_dmc(data);
+	exynos_read_ppmu(ppmu_data);
+	busier_dmc = exynos_get_busier_ppmu(ppmu_data);
 
 	stat->current_frequency = data->curr_freq;
 
 	/* Number of cycles spent on memory access */
-	stat->busy_time = data->ppmu[busier_dmc].count[PPMU_PMNCNT3];
+	stat->busy_time = ppmu_data->ppmu[busier_dmc].count[PPMU_PMNCNT3];
 	stat->busy_time *= 100 / INT_BUS_SATURATION_RATIO;
-	stat->total_time = data->ppmu[busier_dmc].ccnt;
+	stat->total_time = ppmu_data->ppmu[busier_dmc].ccnt;
 
 	return 0;
 }
-static void exynos5_int_exit(struct device *dev)
-{
-	struct platform_device *pdev = container_of(dev, struct platform_device,
-						    dev);
-	struct busfreq_data_int *data = platform_get_drvdata(pdev);
-
-	devfreq_unregister_opp_notifier(dev, data->devfreq);
-}
 
 static struct devfreq_dev_profile exynos5_devfreq_int_profile = {
 	.initial_freq		= 160000,
 	.polling_ms		= 100,
 	.target			= exynos5_busfreq_int_target,
 	.get_dev_status		= exynos5_int_get_dev_status,
-	.exit			= exynos5_int_exit,
 };
 
 static int exynos5250_init_int_tables(struct busfreq_data_int *data)
@@ -315,6 +246,7 @@ unlock:
 static int exynos5_busfreq_int_probe(struct platform_device *pdev)
 {
 	struct busfreq_data_int *data;
+	struct busfreq_ppmu_data *ppmu_data;
 	struct dev_pm_opp *opp;
 	struct device *dev = &pdev->dev;
 	struct device_node *np;
@@ -330,16 +262,26 @@ static int exynos5_busfreq_int_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
+	ppmu_data = &data->ppmu_data;
+	ppmu_data->ppmu_end = PPMU_END;
+	ppmu_data->ppmu = devm_kzalloc(dev,
+				       sizeof(struct exynos_ppmu) * PPMU_END,
+				       GFP_KERNEL);
+	if (!ppmu_data->ppmu) {
+		dev_err(dev, "Failed to allocate memory for exynos_ppmu\n");
+		return -ENOMEM;
+	}
+
 	np = of_find_compatible_node(NULL, NULL, "samsung,exynos5250-ppmu");
 	if (np == NULL) {
 		pr_err("Unable to find PPMU node\n");
 		return -ENOENT;
 	}
 
-	for (i = PPMU_RIGHT; i < PPMU_END; i++) {
+	for (i = 0; i < ppmu_data->ppmu_end; i++) {
 		/* map PPMU memory region */
-		data->ppmu[i].hw_base = of_iomap(np, i);
-		if (data->ppmu[i].hw_base == NULL) {
+		ppmu_data->ppmu[i].hw_base = of_iomap(np, i);
+		if (ppmu_data->ppmu[i].hw_base == NULL) {
 			dev_err(&pdev->dev, "failed to map memory region\n");
 			return -ENOMEM;
 		}
@@ -390,32 +332,29 @@ static int exynos5_busfreq_int_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, data);
 
-	busfreq_mon_reset(data);
+	busfreq_mon_reset(ppmu_data);
 
-	data->devfreq = devfreq_add_device(dev, &exynos5_devfreq_int_profile,
+	data->devfreq = devm_devfreq_add_device(dev, &exynos5_devfreq_int_profile,
 					   "simple_ondemand", NULL);
+	if (IS_ERR(data->devfreq))
+		return PTR_ERR(data->devfreq);
 
-	if (IS_ERR(data->devfreq)) {
-		err = PTR_ERR(data->devfreq);
-		goto err_devfreq_add;
+	err = devm_devfreq_register_opp_notifier(dev, data->devfreq);
+	if (err < 0) {
+		dev_err(dev, "Failed to register opp notifier\n");
+		return err;
 	}
 
-	devfreq_register_opp_notifier(dev, data->devfreq);
-
 	err = register_pm_notifier(&data->pm_notifier);
 	if (err) {
 		dev_err(dev, "Failed to setup pm notifier\n");
-		goto err_devfreq_add;
+		return err;
 	}
 
 	/* TODO: Add a new QOS class for int/mif bus */
 	pm_qos_add_request(&data->int_req, PM_QOS_NETWORK_THROUGHPUT, -1);
 
 	return 0;
-
-err_devfreq_add:
-	devfreq_remove_device(data->devfreq);
-	return err;
 }
 
 static int exynos5_busfreq_int_remove(struct platform_device *pdev)
@@ -424,24 +363,27 @@ static int exynos5_busfreq_int_remove(struct platform_device *pdev)
 
 	pm_qos_remove_request(&data->int_req);
 	unregister_pm_notifier(&data->pm_notifier);
-	devfreq_remove_device(data->devfreq);
 
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
 static int exynos5_busfreq_int_resume(struct device *dev)
 {
 	struct platform_device *pdev = container_of(dev, struct platform_device,
 						    dev);
 	struct busfreq_data_int *data = platform_get_drvdata(pdev);
+	struct busfreq_ppmu_data *ppmu_data = &data->ppmu_data;
 
-	busfreq_mon_reset(data);
+	busfreq_mon_reset(ppmu_data);
 	return 0;
 }
-
 static const struct dev_pm_ops exynos5_busfreq_int_pm = {
 	.resume	= exynos5_busfreq_int_resume,
 };
+#endif
+static SIMPLE_DEV_PM_OPS(exynos5_busfreq_int_pm_ops, NULL,
+			 exynos5_busfreq_int_resume);
 
 /* platform device pointer for exynos5 devfreq device. */
 static struct platform_device *exynos5_devfreq_pdev;
@@ -452,7 +394,7 @@ static struct platform_driver exynos5_busfreq_int_driver = {
 	.driver		= {
 		.name		= "exynos5-bus-int",
 		.owner		= THIS_MODULE,
-		.pm		= &exynos5_busfreq_int_pm,
+		.pm		= &exynos5_busfreq_int_pm_ops,
 	},
 };
 
diff --git a/drivers/devfreq/exynos/exynos_ppmu.c b/drivers/devfreq/exynos/exynos_ppmu.c
index 85fc5ac1036..75fcc5140ff 100644
--- a/drivers/devfreq/exynos/exynos_ppmu.c
+++ b/drivers/devfreq/exynos/exynos_ppmu.c
@@ -54,3 +54,63 @@ unsigned int exynos_ppmu_read(void __iomem *ppmu_base, unsigned int ch)
 
 	return total;
 }
+
+void busfreq_mon_reset(struct busfreq_ppmu_data *ppmu_data)
+{
+	unsigned int i;
+
+	for (i = 0; i < ppmu_data->ppmu_end; i++) {
+		void __iomem *ppmu_base = ppmu_data->ppmu[i].hw_base;
+
+		/* Reset the performance and cycle counters */
+		exynos_ppmu_reset(ppmu_base);
+
+		/* Setup count registers to monitor read/write transactions */
+		ppmu_data->ppmu[i].event[PPMU_PMNCNT3] = RDWR_DATA_COUNT;
+		exynos_ppmu_setevent(ppmu_base, PPMU_PMNCNT3,
+					ppmu_data->ppmu[i].event[PPMU_PMNCNT3]);
+
+		exynos_ppmu_start(ppmu_base);
+	}
+}
+
+void exynos_read_ppmu(struct busfreq_ppmu_data *ppmu_data)
+{
+	int i, j;
+
+	for (i = 0; i < ppmu_data->ppmu_end; i++) {
+		void __iomem *ppmu_base = ppmu_data->ppmu[i].hw_base;
+
+		exynos_ppmu_stop(ppmu_base);
+
+		/* Update local data from PPMU */
+		ppmu_data->ppmu[i].ccnt = __raw_readl(ppmu_base + PPMU_CCNT);
+
+		for (j = PPMU_PMNCNT0; j < PPMU_PMNCNT_MAX; j++) {
+			if (ppmu_data->ppmu[i].event[j] == 0)
+				ppmu_data->ppmu[i].count[j] = 0;
+			else
+				ppmu_data->ppmu[i].count[j] =
+					exynos_ppmu_read(ppmu_base, j);
+		}
+	}
+
+	busfreq_mon_reset(ppmu_data);
+}
+
+int exynos_get_busier_ppmu(struct busfreq_ppmu_data *ppmu_data)
+{
+	unsigned int count = 0;
+	int i, j, busy = 0;
+
+	for (i = 0; i < ppmu_data->ppmu_end; i++) {
+		for (j = PPMU_PMNCNT0; j < PPMU_PMNCNT_MAX; j++) {
+			if (ppmu_data->ppmu[i].count[j] > count) {
+				count = ppmu_data->ppmu[i].count[j];
+				busy = i;
+			}
+		}
+	}
+
+	return busy;
+}
diff --git a/drivers/devfreq/exynos/exynos_ppmu.h b/drivers/devfreq/exynos/exynos_ppmu.h
index 7dfb221eacc..71f17ba3563 100644
--- a/drivers/devfreq/exynos/exynos_ppmu.h
+++ b/drivers/devfreq/exynos/exynos_ppmu.h
@@ -69,10 +69,18 @@ struct exynos_ppmu {
 	bool count_overflow[PPMU_PMNCNT_MAX];
 };
 
+struct busfreq_ppmu_data {
+	struct exynos_ppmu *ppmu;
+	int ppmu_end;
+};
+
 void exynos_ppmu_reset(void __iomem *ppmu_base);
 void exynos_ppmu_setevent(void __iomem *ppmu_base, unsigned int ch,
 			unsigned int evt);
 void exynos_ppmu_start(void __iomem *ppmu_base);
 void exynos_ppmu_stop(void __iomem *ppmu_base);
 unsigned int exynos_ppmu_read(void __iomem *ppmu_base, unsigned int ch);
+void busfreq_mon_reset(struct busfreq_ppmu_data *ppmu_data);
+void exynos_read_ppmu(struct busfreq_ppmu_data *ppmu_data);
+int exynos_get_busier_ppmu(struct busfreq_ppmu_data *ppmu_data);
 #endif /* __DEVFREQ_EXYNOS_PPMU_H */
diff --git a/drivers/dma/Kconfig b/drivers/dma/Kconfig
index 5c5863842de..1eca7b9760e 100644
--- a/drivers/dma/Kconfig
+++ b/drivers/dma/Kconfig
@@ -234,7 +234,7 @@ config PL330_DMA
 
 config PCH_DMA
 	tristate "Intel EG20T PCH / LAPIS Semicon IOH(ML7213/ML7223/ML7831) DMA"
-	depends on PCI && X86
+	depends on PCI && (X86_32 || COMPILE_TEST)
 	select DMA_ENGINE
 	help
 	  Enable support for Intel EG20T PCH DMA engine.
@@ -269,7 +269,7 @@ config MXS_DMA
 	select DMA_ENGINE
 	help
 	  Support the MXS DMA engine. This engine including APBH-DMA
-	  and APBX-DMA is integrated into Freescale i.MX23/28 chips.
+	  and APBX-DMA is integrated into Freescale i.MX23/28/MX6Q/MX6DL chips.
 
 config EP93XX_DMA
 	bool "Cirrus Logic EP93xx DMA support"
@@ -361,6 +361,20 @@ config FSL_EDMA
 	  multiplexing capability for DMA request sources(slot).
 	  This module can be found on Freescale Vybrid and LS-1 SoCs.
 
+config XILINX_VDMA
+	tristate "Xilinx AXI VDMA Engine"
+	depends on (ARCH_ZYNQ || MICROBLAZE)
+	select DMA_ENGINE
+	help
+	  Enable support for Xilinx AXI VDMA Soft IP.
+
+	  This engine provides high-bandwidth direct memory access
+	  between memory and AXI4-Stream video type target
+	  peripherals including peripherals which support AXI4-
+	  Stream Video Protocol.  It has two stream interfaces/
+	  channels, Memory Mapped to Stream (MM2S) and Stream to
+	  Memory Mapped (S2MM) for the data transfers.
+
 config DMA_ENGINE
 	bool
 
diff --git a/drivers/dma/Makefile b/drivers/dma/Makefile
index 5150c82c9ca..c779e1eb2db 100644
--- a/drivers/dma/Makefile
+++ b/drivers/dma/Makefile
@@ -46,3 +46,4 @@ obj-$(CONFIG_K3_DMA) += k3dma.o
 obj-$(CONFIG_MOXART_DMA) += moxart-dma.o
 obj-$(CONFIG_FSL_EDMA) += fsl-edma.o
 obj-$(CONFIG_QCOM_BAM_DMA) += qcom_bam_dma.o
+obj-y += xilinx/
diff --git a/drivers/dma/dmaengine.c b/drivers/dma/dmaengine.c
index a886713937f..d5d30ed863c 100644
--- a/drivers/dma/dmaengine.c
+++ b/drivers/dma/dmaengine.c
@@ -1009,6 +1009,7 @@ static void dmaengine_unmap(struct kref *kref)
 		dma_unmap_page(dev, unmap->addr[i], unmap->len,
 			       DMA_BIDIRECTIONAL);
 	}
+	cnt = unmap->map_cnt;
 	mempool_free(unmap, __get_unmap_pool(cnt)->pool);
 }
 
@@ -1074,6 +1075,7 @@ dmaengine_get_unmap_data(struct device *dev, int nr, gfp_t flags)
 	memset(unmap, 0, sizeof(*unmap));
 	kref_init(&unmap->kref);
 	unmap->dev = dev;
+	unmap->map_cnt = nr;
 
 	return unmap;
 }
diff --git a/drivers/dma/dw/core.c b/drivers/dma/dw/core.c
index cfdbb92aae1..a27ded53ab4 100644
--- a/drivers/dma/dw/core.c
+++ b/drivers/dma/dw/core.c
@@ -1493,6 +1493,13 @@ 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;
 
@@ -1500,15 +1507,19 @@ int dw_dma_probe(struct dw_dma_chip *chip, struct dw_dma_platform_data *pdata)
 
 	if (!pdata && autocfg) {
 		pdata = devm_kzalloc(chip->dev, sizeof(*pdata), GFP_KERNEL);
-		if (!pdata)
-			return -ENOMEM;
+		if (!pdata) {
+			err = -ENOMEM;
+			goto err_pdata;
+		}
 
 		/* Fill platform data with the default values */
 		pdata->is_private = true;
 		pdata->chan_allocation_order = CHAN_ALLOCATION_ASCENDING;
 		pdata->chan_priority = CHAN_PRIORITY_ASCENDING;
-	} else if (!pdata || pdata->nr_channels > DW_DMA_MAX_NR_CHANNELS)
-		return -EINVAL;
+	} else if (!pdata || pdata->nr_channels > DW_DMA_MAX_NR_CHANNELS) {
+		err = -EINVAL;
+		goto err_pdata;
+	}
 
 	if (autocfg)
 		nr_channels = (dw_params >> DW_PARAMS_NR_CHAN & 0x7) + 1;
@@ -1517,13 +1528,10 @@ int dw_dma_probe(struct dw_dma_chip *chip, struct dw_dma_platform_data *pdata)
 
 	dw->chan = devm_kcalloc(chip->dev, nr_channels, sizeof(*dw->chan),
 				GFP_KERNEL);
-	if (!dw->chan)
-		return -ENOMEM;
-
-	dw->clk = devm_clk_get(chip->dev, "hclk");
-	if (IS_ERR(dw->clk))
-		return PTR_ERR(dw->clk);
-	clk_prepare_enable(dw->clk);
+	if (!dw->chan) {
+		err = -ENOMEM;
+		goto err_pdata;
+	}
 
 	/* Get hardware configuration parameters */
 	if (autocfg) {
@@ -1548,21 +1556,22 @@ int dw_dma_probe(struct dw_dma_chip *chip, struct dw_dma_platform_data *pdata)
 	/* Disable BLOCK interrupts as well */
 	channel_clear_bit(dw, MASK.BLOCK, dw->all_chan_mask);
 
-	err = devm_request_irq(chip->dev, chip->irq, dw_dma_interrupt,
-			       IRQF_SHARED, "dw_dmac", dw);
-	if (err)
-		return err;
-
 	/* Create a pool of consistent memory blocks for hardware descriptors */
 	dw->desc_pool = dmam_pool_create("dw_dmac_desc_pool", chip->dev,
 					 sizeof(struct dw_desc), 4, 0);
 	if (!dw->desc_pool) {
 		dev_err(chip->dev, "No memory for descriptors dma pool\n");
-		return -ENOMEM;
+		err = -ENOMEM;
+		goto err_pdata;
 	}
 
 	tasklet_init(&dw->tasklet, dw_dma_tasklet, (unsigned long)dw);
 
+	err = request_irq(chip->irq, dw_dma_interrupt, IRQF_SHARED,
+			  "dw_dmac", dw);
+	if (err)
+		goto err_pdata;
+
 	INIT_LIST_HEAD(&dw->dma.channels);
 	for (i = 0; i < nr_channels; i++) {
 		struct dw_dma_chan	*dwc = &dw->chan[i];
@@ -1650,12 +1659,20 @@ int dw_dma_probe(struct dw_dma_chip *chip, struct dw_dma_platform_data *pdata)
 
 	dma_writel(dw, CFG, DW_CFG_DMA_EN);
 
+	err = dma_async_device_register(&dw->dma);
+	if (err)
+		goto err_dma_register;
+
 	dev_info(chip->dev, "DesignWare DMA Controller, %d channels\n",
 		 nr_channels);
 
-	dma_async_device_register(&dw->dma);
-
 	return 0;
+
+err_dma_register:
+	free_irq(chip->irq, dw);
+err_pdata:
+	clk_disable_unprepare(dw->clk);
+	return err;
 }
 EXPORT_SYMBOL_GPL(dw_dma_probe);
 
@@ -1667,6 +1684,7 @@ int dw_dma_remove(struct dw_dma_chip *chip)
 	dw_dma_off(dw);
 	dma_async_device_unregister(&dw->dma);
 
+	free_irq(chip->irq, dw);
 	tasklet_kill(&dw->tasklet);
 
 	list_for_each_entry_safe(dwc, _dwc, &dw->dma.channels,
@@ -1675,6 +1693,8 @@ 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);
diff --git a/drivers/dma/dw/pci.c b/drivers/dma/dw/pci.c
index fec59f1a77b..39e30c3c7a9 100644
--- a/drivers/dma/dw/pci.c
+++ b/drivers/dma/dw/pci.c
@@ -93,19 +93,13 @@ static int dw_pci_resume_early(struct device *dev)
 	return dw_dma_resume(chip);
 };
 
-#else /* !CONFIG_PM_SLEEP */
-
-#define dw_pci_suspend_late	NULL
-#define dw_pci_resume_early	NULL
-
-#endif /* !CONFIG_PM_SLEEP */
+#endif /* CONFIG_PM_SLEEP */
 
 static const struct dev_pm_ops dw_pci_dev_pm_ops = {
-	.suspend_late = dw_pci_suspend_late,
-	.resume_early = dw_pci_resume_early,
+	SET_LATE_SYSTEM_SLEEP_PM_OPS(dw_pci_suspend_late, dw_pci_resume_early)
 };
 
-static DEFINE_PCI_DEVICE_TABLE(dw_pci_id_table) = {
+static const struct pci_device_id dw_pci_id_table[] = {
 	/* Medfield */
 	{ PCI_VDEVICE(INTEL, 0x0827), (kernel_ulong_t)&dw_pci_pdata },
 	{ PCI_VDEVICE(INTEL, 0x0830), (kernel_ulong_t)&dw_pci_pdata },
diff --git a/drivers/dma/dw/platform.c b/drivers/dma/dw/platform.c
index 453822cc4f9..c5b339af6be 100644
--- a/drivers/dma/dw/platform.c
+++ b/drivers/dma/dw/platform.c
@@ -256,7 +256,7 @@ MODULE_DEVICE_TABLE(acpi, dw_dma_acpi_id_table);
 
 #ifdef CONFIG_PM_SLEEP
 
-static int dw_suspend_noirq(struct device *dev)
+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);
@@ -264,7 +264,7 @@ static int dw_suspend_noirq(struct device *dev)
 	return dw_dma_suspend(chip);
 }
 
-static int dw_resume_noirq(struct device *dev)
+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);
@@ -272,20 +272,10 @@ static int dw_resume_noirq(struct device *dev)
 	return dw_dma_resume(chip);
 }
 
-#else /* !CONFIG_PM_SLEEP */
-
-#define dw_suspend_noirq	NULL
-#define dw_resume_noirq		NULL
-
-#endif /* !CONFIG_PM_SLEEP */
+#endif /* CONFIG_PM_SLEEP */
 
 static const struct dev_pm_ops dw_dev_pm_ops = {
-	.suspend_noirq = dw_suspend_noirq,
-	.resume_noirq = dw_resume_noirq,
-	.freeze_noirq = dw_suspend_noirq,
-	.thaw_noirq = dw_resume_noirq,
-	.restore_noirq = dw_resume_noirq,
-	.poweroff_noirq = dw_suspend_noirq,
+	SET_LATE_SYSTEM_SLEEP_PM_OPS(dw_suspend_late, dw_resume_early)
 };
 
 static struct platform_driver dw_driver = {
diff --git a/drivers/dma/edma.c b/drivers/dma/edma.c
index 926360c2db6..d08c4dedef3 100644
--- a/drivers/dma/edma.c
+++ b/drivers/dma/edma.c
@@ -57,14 +57,48 @@
 #define EDMA_MAX_SLOTS		MAX_NR_SG
 #define EDMA_DESCRIPTORS	16
 
+struct edma_pset {
+	u32				len;
+	dma_addr_t			addr;
+	struct edmacc_param		param;
+};
+
 struct edma_desc {
 	struct virt_dma_desc		vdesc;
 	struct list_head		node;
+	enum dma_transfer_direction	direction;
 	int				cyclic;
 	int				absync;
 	int				pset_nr;
+	struct edma_chan		*echan;
 	int				processed;
-	struct edmacc_param		pset[0];
+
+	/*
+	 * The following 4 elements are used for residue accounting.
+	 *
+	 * - processed_stat: the number of SG elements we have traversed
+	 * so far to cover accounting. This is updated directly to processed
+	 * during edma_callback and is always <= processed, because processed
+	 * refers to the number of pending transfer (programmed to EDMA
+	 * controller), where as processed_stat tracks number of transfers
+	 * accounted for so far.
+	 *
+	 * - residue: The amount of bytes we have left to transfer for this desc
+	 *
+	 * - residue_stat: The residue in bytes of data we have covered
+	 * so far for accounting. This is updated directly to residue
+	 * during callbacks to keep it current.
+	 *
+	 * - sg_len: Tracks the length of the current intermediate transfer,
+	 * this is required to update the residue during intermediate transfer
+	 * completion callback.
+	 */
+	int				processed_stat;
+	u32				sg_len;
+	u32				residue;
+	u32				residue_stat;
+
+	struct edma_pset		pset[0];
 };
 
 struct edma_cc;
@@ -136,12 +170,14 @@ static void edma_execute(struct edma_chan *echan)
 	/* Find out how many left */
 	left = edesc->pset_nr - edesc->processed;
 	nslots = min(MAX_NR_SG, left);
+	edesc->sg_len = 0;
 
 	/* Write descriptor PaRAM set(s) */
 	for (i = 0; i < nslots; i++) {
 		j = i + edesc->processed;
-		edma_write_slot(echan->slot[i], &edesc->pset[j]);
-		dev_dbg(echan->vchan.chan.device->dev,
+		edma_write_slot(echan->slot[i], &edesc->pset[j].param);
+		edesc->sg_len += edesc->pset[j].len;
+		dev_vdbg(echan->vchan.chan.device->dev,
 			"\n pset[%d]:\n"
 			"  chnum\t%d\n"
 			"  slot\t%d\n"
@@ -154,14 +190,14 @@ static void edma_execute(struct edma_chan *echan)
 			"  cidx\t%08x\n"
 			"  lkrld\t%08x\n",
 			j, echan->ch_num, echan->slot[i],
-			edesc->pset[j].opt,
-			edesc->pset[j].src,
-			edesc->pset[j].dst,
-			edesc->pset[j].a_b_cnt,
-			edesc->pset[j].ccnt,
-			edesc->pset[j].src_dst_bidx,
-			edesc->pset[j].src_dst_cidx,
-			edesc->pset[j].link_bcntrld);
+			edesc->pset[j].param.opt,
+			edesc->pset[j].param.src,
+			edesc->pset[j].param.dst,
+			edesc->pset[j].param.a_b_cnt,
+			edesc->pset[j].param.ccnt,
+			edesc->pset[j].param.src_dst_bidx,
+			edesc->pset[j].param.src_dst_cidx,
+			edesc->pset[j].param.link_bcntrld);
 		/* Link to the previous slot if not the last set */
 		if (i != (nslots - 1))
 			edma_link(echan->slot[i], echan->slot[i+1]);
@@ -183,7 +219,8 @@ static void edma_execute(struct edma_chan *echan)
 	}
 
 	if (edesc->processed <= MAX_NR_SG) {
-		dev_dbg(dev, "first transfer starting %d\n", echan->ch_num);
+		dev_dbg(dev, "first transfer starting on channel %d\n",
+			echan->ch_num);
 		edma_start(echan->ch_num);
 	} else {
 		dev_dbg(dev, "chan: %d: completed %d elements, resuming\n",
@@ -197,7 +234,7 @@ static void edma_execute(struct edma_chan *echan)
 	 * MAX_NR_SG
 	 */
 	if (echan->missed) {
-		dev_dbg(dev, "missed event in execute detected\n");
+		dev_dbg(dev, "missed event on channel %d\n", echan->ch_num);
 		edma_clean_channel(echan->ch_num);
 		edma_stop(echan->ch_num);
 		edma_start(echan->ch_num);
@@ -242,6 +279,26 @@ static int edma_slave_config(struct edma_chan *echan,
 	return 0;
 }
 
+static int edma_dma_pause(struct edma_chan *echan)
+{
+	/* Pause/Resume only allowed with cyclic mode */
+	if (!echan->edesc->cyclic)
+		return -EINVAL;
+
+	edma_pause(echan->ch_num);
+	return 0;
+}
+
+static int edma_dma_resume(struct edma_chan *echan)
+{
+	/* Pause/Resume only allowed with cyclic mode */
+	if (!echan->edesc->cyclic)
+		return -EINVAL;
+
+	edma_resume(echan->ch_num);
+	return 0;
+}
+
 static int edma_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
 			unsigned long arg)
 {
@@ -257,6 +314,14 @@ static int edma_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
 		config = (struct dma_slave_config *)arg;
 		ret = edma_slave_config(echan, config);
 		break;
+	case DMA_PAUSE:
+		ret = edma_dma_pause(echan);
+		break;
+
+	case DMA_RESUME:
+		ret = edma_dma_resume(echan);
+		break;
+
 	default:
 		ret = -ENOSYS;
 	}
@@ -275,18 +340,23 @@ static int edma_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
  * @dma_length: Total length of the DMA transfer
  * @direction: Direction of the transfer
  */
-static int edma_config_pset(struct dma_chan *chan, struct edmacc_param *pset,
+static int edma_config_pset(struct dma_chan *chan, struct edma_pset *epset,
 	dma_addr_t src_addr, dma_addr_t dst_addr, u32 burst,
 	enum dma_slave_buswidth dev_width, unsigned int dma_length,
 	enum dma_transfer_direction direction)
 {
 	struct edma_chan *echan = to_edma_chan(chan);
 	struct device *dev = chan->device->dev;
+	struct edmacc_param *param = &epset->param;
 	int acnt, bcnt, ccnt, cidx;
 	int src_bidx, dst_bidx, src_cidx, dst_cidx;
 	int absync;
 
 	acnt = dev_width;
+
+	/* src/dst_maxburst == 0 is the same case as src/dst_maxburst == 1 */
+	if (!burst)
+		burst = 1;
 	/*
 	 * If the maxburst is equal to the fifo width, use
 	 * A-synced transfers. This allows for large contiguous
@@ -337,41 +407,50 @@ static int edma_config_pset(struct dma_chan *chan, struct edmacc_param *pset,
 		cidx = acnt * bcnt;
 	}
 
+	epset->len = dma_length;
+
 	if (direction == DMA_MEM_TO_DEV) {
 		src_bidx = acnt;
 		src_cidx = cidx;
 		dst_bidx = 0;
 		dst_cidx = 0;
+		epset->addr = src_addr;
 	} else if (direction == DMA_DEV_TO_MEM)  {
 		src_bidx = 0;
 		src_cidx = 0;
 		dst_bidx = acnt;
 		dst_cidx = cidx;
+		epset->addr = dst_addr;
+	} else if (direction == DMA_MEM_TO_MEM)  {
+		src_bidx = acnt;
+		src_cidx = cidx;
+		dst_bidx = acnt;
+		dst_cidx = cidx;
 	} else {
 		dev_err(dev, "%s: direction not implemented yet\n", __func__);
 		return -EINVAL;
 	}
 
-	pset->opt = EDMA_TCC(EDMA_CHAN_SLOT(echan->ch_num));
+	param->opt = EDMA_TCC(EDMA_CHAN_SLOT(echan->ch_num));
 	/* Configure A or AB synchronized transfers */
 	if (absync)
-		pset->opt |= SYNCDIM;
+		param->opt |= SYNCDIM;
 
-	pset->src = src_addr;
-	pset->dst = dst_addr;
+	param->src = src_addr;
+	param->dst = dst_addr;
 
-	pset->src_dst_bidx = (dst_bidx << 16) | src_bidx;
-	pset->src_dst_cidx = (dst_cidx << 16) | src_cidx;
+	param->src_dst_bidx = (dst_bidx << 16) | src_bidx;
+	param->src_dst_cidx = (dst_cidx << 16) | src_cidx;
 
-	pset->a_b_cnt = bcnt << 16 | acnt;
-	pset->ccnt = ccnt;
+	param->a_b_cnt = bcnt << 16 | acnt;
+	param->ccnt = ccnt;
 	/*
 	 * Only time when (bcntrld) auto reload is required is for
 	 * A-sync case, and in this case, a requirement of reload value
 	 * of SZ_64K-1 only is assured. 'link' is initially set to NULL
 	 * and then later will be populated by edma_execute.
 	 */
-	pset->link_bcntrld = 0xffffffff;
+	param->link_bcntrld = 0xffffffff;
 	return absync;
 }
 
@@ -401,23 +480,26 @@ static struct dma_async_tx_descriptor *edma_prep_slave_sg(
 		dev_width = echan->cfg.dst_addr_width;
 		burst = echan->cfg.dst_maxburst;
 	} else {
-		dev_err(dev, "%s: bad direction?\n", __func__);
+		dev_err(dev, "%s: bad direction: %d\n", __func__, direction);
 		return NULL;
 	}
 
 	if (dev_width == DMA_SLAVE_BUSWIDTH_UNDEFINED) {
-		dev_err(dev, "Undefined slave buswidth\n");
+		dev_err(dev, "%s: Undefined slave buswidth\n", __func__);
 		return NULL;
 	}
 
 	edesc = kzalloc(sizeof(*edesc) + sg_len *
 		sizeof(edesc->pset[0]), GFP_ATOMIC);
 	if (!edesc) {
-		dev_dbg(dev, "Failed to allocate a descriptor\n");
+		dev_err(dev, "%s: Failed to allocate a descriptor\n", __func__);
 		return NULL;
 	}
 
 	edesc->pset_nr = sg_len;
+	edesc->residue = 0;
+	edesc->direction = direction;
+	edesc->echan = echan;
 
 	/* Allocate a PaRAM slot, if needed */
 	nslots = min_t(unsigned, MAX_NR_SG, sg_len);
@@ -429,7 +511,8 @@ static struct dma_async_tx_descriptor *edma_prep_slave_sg(
 						EDMA_SLOT_ANY);
 			if (echan->slot[i] < 0) {
 				kfree(edesc);
-				dev_err(dev, "Failed to allocate slot\n");
+				dev_err(dev, "%s: Failed to allocate slot\n",
+					__func__);
 				return NULL;
 			}
 		}
@@ -452,16 +535,56 @@ static struct dma_async_tx_descriptor *edma_prep_slave_sg(
 		}
 
 		edesc->absync = ret;
+		edesc->residue += sg_dma_len(sg);
 
 		/* If this is the last in a current SG set of transactions,
 		   enable interrupts so that next set is processed */
 		if (!((i+1) % MAX_NR_SG))
-			edesc->pset[i].opt |= TCINTEN;
+			edesc->pset[i].param.opt |= TCINTEN;
 
 		/* If this is the last set, enable completion interrupt flag */
 		if (i == sg_len - 1)
-			edesc->pset[i].opt |= TCINTEN;
+			edesc->pset[i].param.opt |= TCINTEN;
 	}
+	edesc->residue_stat = edesc->residue;
+
+	return vchan_tx_prep(&echan->vchan, &edesc->vdesc, tx_flags);
+}
+
+struct dma_async_tx_descriptor *edma_prep_dma_memcpy(
+	struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
+	size_t len, unsigned long tx_flags)
+{
+	int ret;
+	struct edma_desc *edesc;
+	struct device *dev = chan->device->dev;
+	struct edma_chan *echan = to_edma_chan(chan);
+
+	if (unlikely(!echan || !len))
+		return NULL;
+
+	edesc = kzalloc(sizeof(*edesc) + sizeof(edesc->pset[0]), GFP_ATOMIC);
+	if (!edesc) {
+		dev_dbg(dev, "Failed to allocate a descriptor\n");
+		return NULL;
+	}
+
+	edesc->pset_nr = 1;
+
+	ret = edma_config_pset(chan, &edesc->pset[0], src, dest, 1,
+			       DMA_SLAVE_BUSWIDTH_4_BYTES, len, DMA_MEM_TO_MEM);
+	if (ret < 0)
+		return NULL;
+
+	edesc->absync = ret;
+
+	/*
+	 * Enable intermediate transfer chaining to re-trigger channel
+	 * on completion of every TR, and enable transfer-completion
+	 * interrupt on completion of the whole transfer.
+	 */
+	edesc->pset[0].param.opt |= ITCCHEN;
+	edesc->pset[0].param.opt |= TCINTEN;
 
 	return vchan_tx_prep(&echan->vchan, &edesc->vdesc, tx_flags);
 }
@@ -493,12 +616,12 @@ static struct dma_async_tx_descriptor *edma_prep_dma_cyclic(
 		dev_width = echan->cfg.dst_addr_width;
 		burst = echan->cfg.dst_maxburst;
 	} else {
-		dev_err(dev, "%s: bad direction?\n", __func__);
+		dev_err(dev, "%s: bad direction: %d\n", __func__, direction);
 		return NULL;
 	}
 
 	if (dev_width == DMA_SLAVE_BUSWIDTH_UNDEFINED) {
-		dev_err(dev, "Undefined slave buswidth\n");
+		dev_err(dev, "%s: Undefined slave buswidth\n", __func__);
 		return NULL;
 	}
 
@@ -523,16 +646,18 @@ static struct dma_async_tx_descriptor *edma_prep_dma_cyclic(
 	edesc = kzalloc(sizeof(*edesc) + nslots *
 		sizeof(edesc->pset[0]), GFP_ATOMIC);
 	if (!edesc) {
-		dev_dbg(dev, "Failed to allocate a descriptor\n");
+		dev_err(dev, "%s: Failed to allocate a descriptor\n", __func__);
 		return NULL;
 	}
 
 	edesc->cyclic = 1;
 	edesc->pset_nr = nslots;
+	edesc->residue = edesc->residue_stat = buf_len;
+	edesc->direction = direction;
+	edesc->echan = echan;
 
-	dev_dbg(dev, "%s: nslots=%d\n", __func__, nslots);
-	dev_dbg(dev, "%s: period_len=%d\n", __func__, period_len);
-	dev_dbg(dev, "%s: buf_len=%d\n", __func__, buf_len);
+	dev_dbg(dev, "%s: channel=%d nslots=%d period_len=%zu buf_len=%zu\n",
+		__func__, echan->ch_num, nslots, period_len, buf_len);
 
 	for (i = 0; i < nslots; i++) {
 		/* Allocate a PaRAM slot, if needed */
@@ -542,7 +667,8 @@ static struct dma_async_tx_descriptor *edma_prep_dma_cyclic(
 						EDMA_SLOT_ANY);
 			if (echan->slot[i] < 0) {
 				kfree(edesc);
-				dev_err(dev, "Failed to allocate slot\n");
+				dev_err(dev, "%s: Failed to allocate slot\n",
+					__func__);
 				return NULL;
 			}
 		}
@@ -566,8 +692,8 @@ static struct dma_async_tx_descriptor *edma_prep_dma_cyclic(
 		else
 			src_addr += period_len;
 
-		dev_dbg(dev, "%s: Configure period %d of buf:\n", __func__, i);
-		dev_dbg(dev,
+		dev_vdbg(dev, "%s: Configure period %d of buf:\n", __func__, i);
+		dev_vdbg(dev,
 			"\n pset[%d]:\n"
 			"  chnum\t%d\n"
 			"  slot\t%d\n"
@@ -580,14 +706,14 @@ static struct dma_async_tx_descriptor *edma_prep_dma_cyclic(
 			"  cidx\t%08x\n"
 			"  lkrld\t%08x\n",
 			i, echan->ch_num, echan->slot[i],
-			edesc->pset[i].opt,
-			edesc->pset[i].src,
-			edesc->pset[i].dst,
-			edesc->pset[i].a_b_cnt,
-			edesc->pset[i].ccnt,
-			edesc->pset[i].src_dst_bidx,
-			edesc->pset[i].src_dst_cidx,
-			edesc->pset[i].link_bcntrld);
+			edesc->pset[i].param.opt,
+			edesc->pset[i].param.src,
+			edesc->pset[i].param.dst,
+			edesc->pset[i].param.a_b_cnt,
+			edesc->pset[i].param.ccnt,
+			edesc->pset[i].param.src_dst_bidx,
+			edesc->pset[i].param.src_dst_cidx,
+			edesc->pset[i].param.link_bcntrld);
 
 		edesc->absync = ret;
 
@@ -595,7 +721,7 @@ static struct dma_async_tx_descriptor *edma_prep_dma_cyclic(
 		 * Enable interrupts for every period because callback
 		 * has to be called for every period.
 		 */
-		edesc->pset[i].opt |= TCINTEN;
+		edesc->pset[i].param.opt |= TCINTEN;
 	}
 
 	return vchan_tx_prep(&echan->vchan, &edesc->vdesc, tx_flags);
@@ -606,7 +732,6 @@ static void edma_callback(unsigned ch_num, u16 ch_status, void *data)
 	struct edma_chan *echan = data;
 	struct device *dev = echan->vchan.chan.device->dev;
 	struct edma_desc *edesc;
-	unsigned long flags;
 	struct edmacc_param p;
 
 	edesc = echan->edesc;
@@ -617,27 +742,34 @@ static void edma_callback(unsigned ch_num, u16 ch_status, void *data)
 
 	switch (ch_status) {
 	case EDMA_DMA_COMPLETE:
-		spin_lock_irqsave(&echan->vchan.lock, flags);
+		spin_lock(&echan->vchan.lock);
 
 		if (edesc) {
 			if (edesc->cyclic) {
 				vchan_cyclic_callback(&edesc->vdesc);
 			} else if (edesc->processed == edesc->pset_nr) {
 				dev_dbg(dev, "Transfer complete, stopping channel %d\n", ch_num);
+				edesc->residue = 0;
 				edma_stop(echan->ch_num);
 				vchan_cookie_complete(&edesc->vdesc);
 				edma_execute(echan);
 			} else {
 				dev_dbg(dev, "Intermediate transfer complete on channel %d\n", ch_num);
+
+				/* Update statistics for tx_status */
+				edesc->residue -= edesc->sg_len;
+				edesc->residue_stat = edesc->residue;
+				edesc->processed_stat = edesc->processed;
+
 				edma_execute(echan);
 			}
 		}
 
-		spin_unlock_irqrestore(&echan->vchan.lock, flags);
+		spin_unlock(&echan->vchan.lock);
 
 		break;
 	case EDMA_DMA_CC_ERROR:
-		spin_lock_irqsave(&echan->vchan.lock, flags);
+		spin_lock(&echan->vchan.lock);
 
 		edma_read_slot(EDMA_CHAN_SLOT(echan->slot[0]), &p);
 
@@ -668,7 +800,7 @@ static void edma_callback(unsigned ch_num, u16 ch_status, void *data)
 			edma_trigger_channel(echan->ch_num);
 		}
 
-		spin_unlock_irqrestore(&echan->vchan.lock, flags);
+		spin_unlock(&echan->vchan.lock);
 
 		break;
 	default:
@@ -704,7 +836,7 @@ static int edma_alloc_chan_resources(struct dma_chan *chan)
 	echan->alloced = true;
 	echan->slot[0] = echan->ch_num;
 
-	dev_dbg(dev, "allocated channel for %u:%u\n",
+	dev_dbg(dev, "allocated channel %d for %u:%u\n", echan->ch_num,
 		EDMA_CTLR(echan->ch_num), EDMA_CHAN_SLOT(echan->ch_num));
 
 	return 0;
@@ -756,23 +888,52 @@ static void edma_issue_pending(struct dma_chan *chan)
 	spin_unlock_irqrestore(&echan->vchan.lock, flags);
 }
 
-static size_t edma_desc_size(struct edma_desc *edesc)
+static u32 edma_residue(struct edma_desc *edesc)
 {
+	bool dst = edesc->direction == DMA_DEV_TO_MEM;
+	struct edma_pset *pset = edesc->pset;
+	dma_addr_t done, pos;
 	int i;
-	size_t size;
-
-	if (edesc->absync)
-		for (size = i = 0; i < edesc->pset_nr; i++)
-			size += (edesc->pset[i].a_b_cnt & 0xffff) *
-				(edesc->pset[i].a_b_cnt >> 16) *
-				 edesc->pset[i].ccnt;
-	else
-		size = (edesc->pset[0].a_b_cnt & 0xffff) *
-			(edesc->pset[0].a_b_cnt >> 16) +
-			(edesc->pset[0].a_b_cnt & 0xffff) *
-			(SZ_64K - 1) * edesc->pset[0].ccnt;
-
-	return size;
+
+	/*
+	 * We always read the dst/src position from the first RamPar
+	 * pset. That's the one which is active now.
+	 */
+	pos = edma_get_position(edesc->echan->slot[0], dst);
+
+	/*
+	 * Cyclic is simple. Just subtract pset[0].addr from pos.
+	 *
+	 * We never update edesc->residue in the cyclic case, so we
+	 * can tell the remaining room to the end of the circular
+	 * buffer.
+	 */
+	if (edesc->cyclic) {
+		done = pos - pset->addr;
+		edesc->residue_stat = edesc->residue - done;
+		return edesc->residue_stat;
+	}
+
+	/*
+	 * For SG operation we catch up with the last processed
+	 * status.
+	 */
+	pset += edesc->processed_stat;
+
+	for (i = edesc->processed_stat; i < edesc->processed; i++, pset++) {
+		/*
+		 * If we are inside this pset address range, we know
+		 * this is the active one. Get the current delta and
+		 * stop walking the psets.
+		 */
+		if (pos >= pset->addr && pos < pset->addr + pset->len)
+			return edesc->residue_stat - (pos - pset->addr);
+
+		/* Otherwise mark it done and update residue_stat. */
+		edesc->processed_stat++;
+		edesc->residue_stat -= pset->len;
+	}
+	return edesc->residue_stat;
 }
 
 /* Check request completion status */
@@ -790,13 +951,10 @@ static enum dma_status edma_tx_status(struct dma_chan *chan,
 		return ret;
 
 	spin_lock_irqsave(&echan->vchan.lock, flags);
-	vdesc = vchan_find_desc(&echan->vchan, cookie);
-	if (vdesc) {
-		txstate->residue = edma_desc_size(to_edma_desc(&vdesc->tx));
-	} else if (echan->edesc && echan->edesc->vdesc.tx.cookie == cookie) {
-		struct edma_desc *edesc = echan->edesc;
-		txstate->residue = edma_desc_size(edesc);
-	}
+	if (echan->edesc && echan->edesc->vdesc.tx.cookie == cookie)
+		txstate->residue = edma_residue(echan->edesc);
+	else if ((vdesc = vchan_find_desc(&echan->vchan, cookie)))
+		txstate->residue = to_edma_desc(&vdesc->tx)->residue;
 	spin_unlock_irqrestore(&echan->vchan.lock, flags);
 
 	return ret;
@@ -822,18 +980,43 @@ static void __init edma_chan_init(struct edma_cc *ecc,
 	}
 }
 
+#define EDMA_DMA_BUSWIDTHS	(BIT(DMA_SLAVE_BUSWIDTH_1_BYTE) | \
+				 BIT(DMA_SLAVE_BUSWIDTH_2_BYTES) | \
+				 BIT(DMA_SLAVE_BUSWIDTH_4_BYTES))
+
+static int edma_dma_device_slave_caps(struct dma_chan *dchan,
+				      struct dma_slave_caps *caps)
+{
+	caps->src_addr_widths = EDMA_DMA_BUSWIDTHS;
+	caps->dstn_addr_widths = EDMA_DMA_BUSWIDTHS;
+	caps->directions = BIT(DMA_DEV_TO_MEM) | BIT(DMA_MEM_TO_DEV);
+	caps->cmd_pause = true;
+	caps->cmd_terminate = true;
+	caps->residue_granularity = DMA_RESIDUE_GRANULARITY_DESCRIPTOR;
+
+	return 0;
+}
+
 static void edma_dma_init(struct edma_cc *ecc, struct dma_device *dma,
 			  struct device *dev)
 {
 	dma->device_prep_slave_sg = edma_prep_slave_sg;
 	dma->device_prep_dma_cyclic = edma_prep_dma_cyclic;
+	dma->device_prep_dma_memcpy = edma_prep_dma_memcpy;
 	dma->device_alloc_chan_resources = edma_alloc_chan_resources;
 	dma->device_free_chan_resources = edma_free_chan_resources;
 	dma->device_issue_pending = edma_issue_pending;
 	dma->device_tx_status = edma_tx_status;
 	dma->device_control = edma_control;
+	dma->device_slave_caps = edma_dma_device_slave_caps;
 	dma->dev = dev;
 
+	/*
+	 * code using dma memcpy must make sure alignment of
+	 * length is at dma->copy_align boundary.
+	 */
+	dma->copy_align = DMA_SLAVE_BUSWIDTH_4_BYTES;
+
 	INIT_LIST_HEAD(&dma->channels);
 }
 
@@ -861,6 +1044,8 @@ static int edma_probe(struct platform_device *pdev)
 
 	dma_cap_zero(ecc->dma_slave.cap_mask);
 	dma_cap_set(DMA_SLAVE, ecc->dma_slave.cap_mask);
+	dma_cap_set(DMA_CYCLIC, ecc->dma_slave.cap_mask);
+	dma_cap_set(DMA_MEMCPY, ecc->dma_slave.cap_mask);
 
 	edma_dma_init(ecc, &ecc->dma_slave, &pdev->dev);
 
diff --git a/drivers/dma/fsldma.c b/drivers/dma/fsldma.c
index f157c6f76b3..e0fec68aed2 100644
--- a/drivers/dma/fsldma.c
+++ b/drivers/dma/fsldma.c
@@ -61,6 +61,16 @@ static u32 get_sr(struct fsldma_chan *chan)
 	return DMA_IN(chan, &chan->regs->sr, 32);
 }
 
+static void set_mr(struct fsldma_chan *chan, u32 val)
+{
+	DMA_OUT(chan, &chan->regs->mr, val, 32);
+}
+
+static u32 get_mr(struct fsldma_chan *chan)
+{
+	return DMA_IN(chan, &chan->regs->mr, 32);
+}
+
 static void set_cdar(struct fsldma_chan *chan, dma_addr_t addr)
 {
 	DMA_OUT(chan, &chan->regs->cdar, addr | FSL_DMA_SNEN, 64);
@@ -71,6 +81,11 @@ static dma_addr_t get_cdar(struct fsldma_chan *chan)
 	return DMA_IN(chan, &chan->regs->cdar, 64) & ~FSL_DMA_SNEN;
 }
 
+static void set_bcr(struct fsldma_chan *chan, u32 val)
+{
+	DMA_OUT(chan, &chan->regs->bcr, val, 32);
+}
+
 static u32 get_bcr(struct fsldma_chan *chan)
 {
 	return DMA_IN(chan, &chan->regs->bcr, 32);
@@ -135,7 +150,7 @@ static void set_ld_eol(struct fsldma_chan *chan, struct fsl_desc_sw *desc)
 static void dma_init(struct fsldma_chan *chan)
 {
 	/* Reset the channel */
-	DMA_OUT(chan, &chan->regs->mr, 0, 32);
+	set_mr(chan, 0);
 
 	switch (chan->feature & FSL_DMA_IP_MASK) {
 	case FSL_DMA_IP_85XX:
@@ -144,16 +159,15 @@ static void dma_init(struct fsldma_chan *chan)
 		 * EOLNIE - End of links interrupt enable
 		 * BWC - Bandwidth sharing among channels
 		 */
-		DMA_OUT(chan, &chan->regs->mr, FSL_DMA_MR_BWC
-				| FSL_DMA_MR_EIE | FSL_DMA_MR_EOLNIE, 32);
+		set_mr(chan, FSL_DMA_MR_BWC | FSL_DMA_MR_EIE
+			| FSL_DMA_MR_EOLNIE);
 		break;
 	case FSL_DMA_IP_83XX:
 		/* Set the channel to below modes:
 		 * EOTIE - End-of-transfer interrupt enable
 		 * PRC_RM - PCI read multiple
 		 */
-		DMA_OUT(chan, &chan->regs->mr, FSL_DMA_MR_EOTIE
-				| FSL_DMA_MR_PRC_RM, 32);
+		set_mr(chan, FSL_DMA_MR_EOTIE | FSL_DMA_MR_PRC_RM);
 		break;
 	}
 }
@@ -175,10 +189,10 @@ static void dma_start(struct fsldma_chan *chan)
 {
 	u32 mode;
 
-	mode = DMA_IN(chan, &chan->regs->mr, 32);
+	mode = get_mr(chan);
 
 	if (chan->feature & FSL_DMA_CHAN_PAUSE_EXT) {
-		DMA_OUT(chan, &chan->regs->bcr, 0, 32);
+		set_bcr(chan, 0);
 		mode |= FSL_DMA_MR_EMP_EN;
 	} else {
 		mode &= ~FSL_DMA_MR_EMP_EN;
@@ -191,7 +205,7 @@ static void dma_start(struct fsldma_chan *chan)
 		mode |= FSL_DMA_MR_CS;
 	}
 
-	DMA_OUT(chan, &chan->regs->mr, mode, 32);
+	set_mr(chan, mode);
 }
 
 static void dma_halt(struct fsldma_chan *chan)
@@ -200,7 +214,7 @@ static void dma_halt(struct fsldma_chan *chan)
 	int i;
 
 	/* read the mode register */
-	mode = DMA_IN(chan, &chan->regs->mr, 32);
+	mode = get_mr(chan);
 
 	/*
 	 * The 85xx controller supports channel abort, which will stop
@@ -209,14 +223,14 @@ static void dma_halt(struct fsldma_chan *chan)
 	 */
 	if ((chan->feature & FSL_DMA_IP_MASK) == FSL_DMA_IP_85XX) {
 		mode |= FSL_DMA_MR_CA;
-		DMA_OUT(chan, &chan->regs->mr, mode, 32);
+		set_mr(chan, mode);
 
 		mode &= ~FSL_DMA_MR_CA;
 	}
 
 	/* stop the DMA controller */
 	mode &= ~(FSL_DMA_MR_CS | FSL_DMA_MR_EMS_EN);
-	DMA_OUT(chan, &chan->regs->mr, mode, 32);
+	set_mr(chan, mode);
 
 	/* wait for the DMA controller to become idle */
 	for (i = 0; i < 100; i++) {
@@ -245,7 +259,7 @@ static void fsl_chan_set_src_loop_size(struct fsldma_chan *chan, int size)
 {
 	u32 mode;
 
-	mode = DMA_IN(chan, &chan->regs->mr, 32);
+	mode = get_mr(chan);
 
 	switch (size) {
 	case 0:
@@ -259,7 +273,7 @@ static void fsl_chan_set_src_loop_size(struct fsldma_chan *chan, int size)
 		break;
 	}
 
-	DMA_OUT(chan, &chan->regs->mr, mode, 32);
+	set_mr(chan, mode);
 }
 
 /**
@@ -277,7 +291,7 @@ static void fsl_chan_set_dst_loop_size(struct fsldma_chan *chan, int size)
 {
 	u32 mode;
 
-	mode = DMA_IN(chan, &chan->regs->mr, 32);
+	mode = get_mr(chan);
 
 	switch (size) {
 	case 0:
@@ -291,7 +305,7 @@ static void fsl_chan_set_dst_loop_size(struct fsldma_chan *chan, int size)
 		break;
 	}
 
-	DMA_OUT(chan, &chan->regs->mr, mode, 32);
+	set_mr(chan, mode);
 }
 
 /**
@@ -312,10 +326,10 @@ static void fsl_chan_set_request_count(struct fsldma_chan *chan, int size)
 
 	BUG_ON(size > 1024);
 
-	mode = DMA_IN(chan, &chan->regs->mr, 32);
+	mode = get_mr(chan);
 	mode |= (__ilog2(size) << 24) & 0x0f000000;
 
-	DMA_OUT(chan, &chan->regs->mr, mode, 32);
+	set_mr(chan, mode);
 }
 
 /**
@@ -404,6 +418,19 @@ static dma_cookie_t fsl_dma_tx_submit(struct dma_async_tx_descriptor *tx)
 }
 
 /**
+ * fsl_dma_free_descriptor - Free descriptor from channel's DMA pool.
+ * @chan : Freescale DMA channel
+ * @desc: descriptor to be freed
+ */
+static void fsl_dma_free_descriptor(struct fsldma_chan *chan,
+		struct fsl_desc_sw *desc)
+{
+	list_del(&desc->node);
+	chan_dbg(chan, "LD %p free\n", desc);
+	dma_pool_free(chan->desc_pool, desc, desc->async_tx.phys);
+}
+
+/**
  * fsl_dma_alloc_descriptor - Allocate descriptor from channel's DMA pool.
  * @chan : Freescale DMA channel
  *
@@ -426,14 +453,107 @@ static struct fsl_desc_sw *fsl_dma_alloc_descriptor(struct fsldma_chan *chan)
 	desc->async_tx.tx_submit = fsl_dma_tx_submit;
 	desc->async_tx.phys = pdesc;
 
-#ifdef FSL_DMA_LD_DEBUG
 	chan_dbg(chan, "LD %p allocated\n", desc);
-#endif
 
 	return desc;
 }
 
 /**
+ * fsl_chan_xfer_ld_queue - transfer any pending transactions
+ * @chan : Freescale DMA channel
+ *
+ * HARDWARE STATE: idle
+ * LOCKING: must hold chan->desc_lock
+ */
+static void fsl_chan_xfer_ld_queue(struct fsldma_chan *chan)
+{
+	struct fsl_desc_sw *desc;
+
+	/*
+	 * If the list of pending descriptors is empty, then we
+	 * don't need to do any work at all
+	 */
+	if (list_empty(&chan->ld_pending)) {
+		chan_dbg(chan, "no pending LDs\n");
+		return;
+	}
+
+	/*
+	 * The DMA controller is not idle, which means that the interrupt
+	 * handler will start any queued transactions when it runs after
+	 * this transaction finishes
+	 */
+	if (!chan->idle) {
+		chan_dbg(chan, "DMA controller still busy\n");
+		return;
+	}
+
+	/*
+	 * If there are some link descriptors which have not been
+	 * transferred, we need to start the controller
+	 */
+
+	/*
+	 * Move all elements from the queue of pending transactions
+	 * onto the list of running transactions
+	 */
+	chan_dbg(chan, "idle, starting controller\n");
+	desc = list_first_entry(&chan->ld_pending, struct fsl_desc_sw, node);
+	list_splice_tail_init(&chan->ld_pending, &chan->ld_running);
+
+	/*
+	 * The 85xx DMA controller doesn't clear the channel start bit
+	 * automatically at the end of a transfer. Therefore we must clear
+	 * it in software before starting the transfer.
+	 */
+	if ((chan->feature & FSL_DMA_IP_MASK) == FSL_DMA_IP_85XX) {
+		u32 mode;
+
+		mode = get_mr(chan);
+		mode &= ~FSL_DMA_MR_CS;
+		set_mr(chan, mode);
+	}
+
+	/*
+	 * Program the descriptor's address into the DMA controller,
+	 * then start the DMA transaction
+	 */
+	set_cdar(chan, desc->async_tx.phys);
+	get_cdar(chan);
+
+	dma_start(chan);
+	chan->idle = false;
+}
+
+/**
+ * fsldma_cleanup_descriptor - cleanup and free a single link descriptor
+ * @chan: Freescale DMA channel
+ * @desc: descriptor to cleanup and free
+ *
+ * This function is used on a descriptor which has been executed by the DMA
+ * controller. It will run any callbacks, submit any dependencies, and then
+ * free the descriptor.
+ */
+static void fsldma_cleanup_descriptor(struct fsldma_chan *chan,
+				      struct fsl_desc_sw *desc)
+{
+	struct dma_async_tx_descriptor *txd = &desc->async_tx;
+
+	/* Run the link descriptor callback function */
+	if (txd->callback) {
+		chan_dbg(chan, "LD %p callback\n", desc);
+		txd->callback(txd->callback_param);
+	}
+
+	/* Run any dependencies */
+	dma_run_dependencies(txd);
+
+	dma_descriptor_unmap(txd);
+	chan_dbg(chan, "LD %p free\n", desc);
+	dma_pool_free(chan->desc_pool, desc, txd->phys);
+}
+
+/**
  * fsl_dma_alloc_chan_resources - Allocate resources for DMA channel.
  * @chan : Freescale DMA channel
  *
@@ -477,13 +597,8 @@ static void fsldma_free_desc_list(struct fsldma_chan *chan,
 {
 	struct fsl_desc_sw *desc, *_desc;
 
-	list_for_each_entry_safe(desc, _desc, list, node) {
-		list_del(&desc->node);
-#ifdef FSL_DMA_LD_DEBUG
-		chan_dbg(chan, "LD %p free\n", desc);
-#endif
-		dma_pool_free(chan->desc_pool, desc, desc->async_tx.phys);
-	}
+	list_for_each_entry_safe(desc, _desc, list, node)
+		fsl_dma_free_descriptor(chan, desc);
 }
 
 static void fsldma_free_desc_list_reverse(struct fsldma_chan *chan,
@@ -491,13 +606,8 @@ static void fsldma_free_desc_list_reverse(struct fsldma_chan *chan,
 {
 	struct fsl_desc_sw *desc, *_desc;
 
-	list_for_each_entry_safe_reverse(desc, _desc, list, node) {
-		list_del(&desc->node);
-#ifdef FSL_DMA_LD_DEBUG
-		chan_dbg(chan, "LD %p free\n", desc);
-#endif
-		dma_pool_free(chan->desc_pool, desc, desc->async_tx.phys);
-	}
+	list_for_each_entry_safe_reverse(desc, _desc, list, node)
+		fsl_dma_free_descriptor(chan, desc);
 }
 
 /**
@@ -520,35 +630,6 @@ static void fsl_dma_free_chan_resources(struct dma_chan *dchan)
 }
 
 static struct dma_async_tx_descriptor *
-fsl_dma_prep_interrupt(struct dma_chan *dchan, unsigned long flags)
-{
-	struct fsldma_chan *chan;
-	struct fsl_desc_sw *new;
-
-	if (!dchan)
-		return NULL;
-
-	chan = to_fsl_chan(dchan);
-
-	new = fsl_dma_alloc_descriptor(chan);
-	if (!new) {
-		chan_err(chan, "%s\n", msg_ld_oom);
-		return NULL;
-	}
-
-	new->async_tx.cookie = -EBUSY;
-	new->async_tx.flags = flags;
-
-	/* Insert the link descriptor to the LD ring */
-	list_add_tail(&new->node, &new->tx_list);
-
-	/* Set End-of-link to the last link descriptor of new list */
-	set_ld_eol(chan, new);
-
-	return &new->async_tx;
-}
-
-static struct dma_async_tx_descriptor *
 fsl_dma_prep_memcpy(struct dma_chan *dchan,
 	dma_addr_t dma_dst, dma_addr_t dma_src,
 	size_t len, unsigned long flags)
@@ -817,105 +898,6 @@ static int fsl_dma_device_control(struct dma_chan *dchan,
 }
 
 /**
- * fsldma_cleanup_descriptor - cleanup and free a single link descriptor
- * @chan: Freescale DMA channel
- * @desc: descriptor to cleanup and free
- *
- * This function is used on a descriptor which has been executed by the DMA
- * controller. It will run any callbacks, submit any dependencies, and then
- * free the descriptor.
- */
-static void fsldma_cleanup_descriptor(struct fsldma_chan *chan,
-				      struct fsl_desc_sw *desc)
-{
-	struct dma_async_tx_descriptor *txd = &desc->async_tx;
-
-	/* Run the link descriptor callback function */
-	if (txd->callback) {
-#ifdef FSL_DMA_LD_DEBUG
-		chan_dbg(chan, "LD %p callback\n", desc);
-#endif
-		txd->callback(txd->callback_param);
-	}
-
-	/* Run any dependencies */
-	dma_run_dependencies(txd);
-
-	dma_descriptor_unmap(txd);
-#ifdef FSL_DMA_LD_DEBUG
-	chan_dbg(chan, "LD %p free\n", desc);
-#endif
-	dma_pool_free(chan->desc_pool, desc, txd->phys);
-}
-
-/**
- * fsl_chan_xfer_ld_queue - transfer any pending transactions
- * @chan : Freescale DMA channel
- *
- * HARDWARE STATE: idle
- * LOCKING: must hold chan->desc_lock
- */
-static void fsl_chan_xfer_ld_queue(struct fsldma_chan *chan)
-{
-	struct fsl_desc_sw *desc;
-
-	/*
-	 * If the list of pending descriptors is empty, then we
-	 * don't need to do any work at all
-	 */
-	if (list_empty(&chan->ld_pending)) {
-		chan_dbg(chan, "no pending LDs\n");
-		return;
-	}
-
-	/*
-	 * The DMA controller is not idle, which means that the interrupt
-	 * handler will start any queued transactions when it runs after
-	 * this transaction finishes
-	 */
-	if (!chan->idle) {
-		chan_dbg(chan, "DMA controller still busy\n");
-		return;
-	}
-
-	/*
-	 * If there are some link descriptors which have not been
-	 * transferred, we need to start the controller
-	 */
-
-	/*
-	 * Move all elements from the queue of pending transactions
-	 * onto the list of running transactions
-	 */
-	chan_dbg(chan, "idle, starting controller\n");
-	desc = list_first_entry(&chan->ld_pending, struct fsl_desc_sw, node);
-	list_splice_tail_init(&chan->ld_pending, &chan->ld_running);
-
-	/*
-	 * The 85xx DMA controller doesn't clear the channel start bit
-	 * automatically at the end of a transfer. Therefore we must clear
-	 * it in software before starting the transfer.
-	 */
-	if ((chan->feature & FSL_DMA_IP_MASK) == FSL_DMA_IP_85XX) {
-		u32 mode;
-
-		mode = DMA_IN(chan, &chan->regs->mr, 32);
-		mode &= ~FSL_DMA_MR_CS;
-		DMA_OUT(chan, &chan->regs->mr, mode, 32);
-	}
-
-	/*
-	 * Program the descriptor's address into the DMA controller,
-	 * then start the DMA transaction
-	 */
-	set_cdar(chan, desc->async_tx.phys);
-	get_cdar(chan);
-
-	dma_start(chan);
-	chan->idle = false;
-}
-
-/**
  * fsl_dma_memcpy_issue_pending - Issue the DMA start command
  * @chan : Freescale DMA channel
  */
@@ -1304,12 +1286,10 @@ static int fsldma_of_probe(struct platform_device *op)
 	fdev->irq = irq_of_parse_and_map(op->dev.of_node, 0);
 
 	dma_cap_set(DMA_MEMCPY, fdev->common.cap_mask);
-	dma_cap_set(DMA_INTERRUPT, fdev->common.cap_mask);
 	dma_cap_set(DMA_SG, fdev->common.cap_mask);
 	dma_cap_set(DMA_SLAVE, fdev->common.cap_mask);
 	fdev->common.device_alloc_chan_resources = fsl_dma_alloc_chan_resources;
 	fdev->common.device_free_chan_resources = fsl_dma_free_chan_resources;
-	fdev->common.device_prep_dma_interrupt = fsl_dma_prep_interrupt;
 	fdev->common.device_prep_dma_memcpy = fsl_dma_prep_memcpy;
 	fdev->common.device_prep_dma_sg = fsl_dma_prep_sg;
 	fdev->common.device_tx_status = fsl_tx_status;
diff --git a/drivers/dma/imx-sdma.c b/drivers/dma/imx-sdma.c
index 19041cefabb..128714622bf 100644
--- a/drivers/dma/imx-sdma.c
+++ b/drivers/dma/imx-sdma.c
@@ -607,8 +607,6 @@ static void sdma_handle_channel_loop(struct sdma_channel *sdmac)
 
 		if (bd->mode.status & BD_RROR)
 			sdmac->status = DMA_ERROR;
-		else
-			sdmac->status = DMA_IN_PROGRESS;
 
 		bd->mode.status |= BD_DONE;
 		sdmac->buf_tail++;
diff --git a/drivers/dma/mmp_pdma.c b/drivers/dma/mmp_pdma.c
index bf02e7beb51..a7b186d536b 100644
--- a/drivers/dma/mmp_pdma.c
+++ b/drivers/dma/mmp_pdma.c
@@ -29,8 +29,8 @@
 #define DALGN		0x00a0
 #define DINT		0x00f0
 #define DDADR		0x0200
-#define DSADR		0x0204
-#define DTADR		0x0208
+#define DSADR(n)	(0x0204 + ((n) << 4))
+#define DTADR(n)	(0x0208 + ((n) << 4))
 #define DCMD		0x020c
 
 #define DCSR_RUN	BIT(31)	/* Run Bit (read / write) */
@@ -277,7 +277,7 @@ static void mmp_pdma_free_phy(struct mmp_pdma_chan *pchan)
 		return;
 
 	/* clear the channel mapping in DRCMR */
-	reg = DRCMR(pchan->phy->vchan->drcmr);
+	reg = DRCMR(pchan->drcmr);
 	writel(0, pchan->phy->base + reg);
 
 	spin_lock_irqsave(&pdev->phy_lock, flags);
@@ -748,11 +748,92 @@ static int mmp_pdma_control(struct dma_chan *dchan, enum dma_ctrl_cmd cmd,
 	return 0;
 }
 
+static unsigned int mmp_pdma_residue(struct mmp_pdma_chan *chan,
+				     dma_cookie_t cookie)
+{
+	struct mmp_pdma_desc_sw *sw;
+	u32 curr, residue = 0;
+	bool passed = false;
+	bool cyclic = chan->cyclic_first != NULL;
+
+	/*
+	 * If the channel does not have a phy pointer anymore, it has already
+	 * been completed. Therefore, its residue is 0.
+	 */
+	if (!chan->phy)
+		return 0;
+
+	if (chan->dir == DMA_DEV_TO_MEM)
+		curr = readl(chan->phy->base + DTADR(chan->phy->idx));
+	else
+		curr = readl(chan->phy->base + DSADR(chan->phy->idx));
+
+	list_for_each_entry(sw, &chan->chain_running, node) {
+		u32 start, end, len;
+
+		if (chan->dir == DMA_DEV_TO_MEM)
+			start = sw->desc.dtadr;
+		else
+			start = sw->desc.dsadr;
+
+		len = sw->desc.dcmd & DCMD_LENGTH;
+		end = start + len;
+
+		/*
+		 * 'passed' will be latched once we found the descriptor which
+		 * lies inside the boundaries of the curr pointer. All
+		 * descriptors that occur in the list _after_ we found that
+		 * partially handled descriptor are still to be processed and
+		 * are hence added to the residual bytes counter.
+		 */
+
+		if (passed) {
+			residue += len;
+		} else if (curr >= start && curr <= end) {
+			residue += end - curr;
+			passed = true;
+		}
+
+		/*
+		 * Descriptors that have the ENDIRQEN bit set mark the end of a
+		 * transaction chain, and the cookie assigned with it has been
+		 * returned previously from mmp_pdma_tx_submit().
+		 *
+		 * In case we have multiple transactions in the running chain,
+		 * and the cookie does not match the one the user asked us
+		 * about, reset the state variables and start over.
+		 *
+		 * This logic does not apply to cyclic transactions, where all
+		 * descriptors have the ENDIRQEN bit set, and for which we
+		 * can't have multiple transactions on one channel anyway.
+		 */
+		if (cyclic || !(sw->desc.dcmd & DCMD_ENDIRQEN))
+			continue;
+
+		if (sw->async_tx.cookie == cookie) {
+			return residue;
+		} else {
+			residue = 0;
+			passed = false;
+		}
+	}
+
+	/* We should only get here in case of cyclic transactions */
+	return residue;
+}
+
 static enum dma_status mmp_pdma_tx_status(struct dma_chan *dchan,
 					  dma_cookie_t cookie,
 					  struct dma_tx_state *txstate)
 {
-	return dma_cookie_status(dchan, cookie, txstate);
+	struct mmp_pdma_chan *chan = to_mmp_pdma_chan(dchan);
+	enum dma_status ret;
+
+	ret = dma_cookie_status(dchan, cookie, txstate);
+	if (likely(ret != DMA_ERROR))
+		dma_set_residue(txstate, mmp_pdma_residue(chan, cookie));
+
+	return ret;
 }
 
 /**
@@ -858,8 +939,7 @@ static int mmp_pdma_chan_init(struct mmp_pdma_device *pdev, int idx, int irq)
 	struct mmp_pdma_chan *chan;
 	int ret;
 
-	chan = devm_kzalloc(pdev->dev, sizeof(struct mmp_pdma_chan),
-			    GFP_KERNEL);
+	chan = devm_kzalloc(pdev->dev, sizeof(*chan), GFP_KERNEL);
 	if (chan == NULL)
 		return -ENOMEM;
 
@@ -946,8 +1026,7 @@ static int mmp_pdma_probe(struct platform_device *op)
 			irq_num++;
 	}
 
-	pdev->phy = devm_kcalloc(pdev->dev,
-				 dma_channels, sizeof(struct mmp_pdma_chan),
+	pdev->phy = devm_kcalloc(pdev->dev, dma_channels, sizeof(*pdev->phy),
 				 GFP_KERNEL);
 	if (pdev->phy == NULL)
 		return -ENOMEM;
diff --git a/drivers/dma/mpc512x_dma.c b/drivers/dma/mpc512x_dma.c
index 448750da440..2ad43738ac8 100644
--- a/drivers/dma/mpc512x_dma.c
+++ b/drivers/dma/mpc512x_dma.c
@@ -2,6 +2,7 @@
  * Copyright (C) Freescale Semicondutor, Inc. 2007, 2008.
  * Copyright (C) Semihalf 2009
  * Copyright (C) Ilya Yanok, Emcraft Systems 2010
+ * Copyright (C) Alexander Popov, Promcontroller 2014
  *
  * Written by Piotr Ziecik <kosmo@semihalf.com>. Hardware description
  * (defines, structures and comments) was taken from MPC5121 DMA driver
@@ -29,8 +30,18 @@
  */
 
 /*
- * This is initial version of MPC5121 DMA driver. Only memory to memory
- * transfers are supported (tested using dmatest module).
+ * MPC512x and MPC8308 DMA driver. It supports
+ * memory to memory data transfers (tested using dmatest module) and
+ * data transfers between memory and peripheral I/O memory
+ * by means of slave scatter/gather with these limitations:
+ *  - chunked transfers (described by s/g lists with more than one item)
+ *     are refused as long as proper support for scatter/gather is missing;
+ *  - transfers on MPC8308 always start from software as this SoC appears
+ *     not to have external request lines for peripheral flow control;
+ *  - only peripheral devices with 4-byte FIFO access register are supported;
+ *  - minimal memory <-> I/O memory transfer chunk is 4 bytes and consequently
+ *     source and destination addresses must be 4-byte aligned
+ *     and transfer size must be aligned on (4 * maxburst) boundary;
  */
 
 #include <linux/module.h>
@@ -52,9 +63,17 @@
 #define MPC_DMA_DESCRIPTORS	64
 
 /* Macro definitions */
-#define MPC_DMA_CHANNELS	64
 #define MPC_DMA_TCD_OFFSET	0x1000
 
+/*
+ * Maximum channel counts for individual hardware variants
+ * and the maximum channel count over all supported controllers,
+ * used for data structure size
+ */
+#define MPC8308_DMACHAN_MAX	16
+#define MPC512x_DMACHAN_MAX	64
+#define MPC_DMA_CHANNELS	64
+
 /* Arbitration mode of group and channel */
 #define MPC_DMA_DMACR_EDCG	(1 << 31)
 #define MPC_DMA_DMACR_ERGA	(1 << 3)
@@ -181,6 +200,7 @@ struct mpc_dma_desc {
 	dma_addr_t			tcd_paddr;
 	int				error;
 	struct list_head		node;
+	int				will_access_peripheral;
 };
 
 struct mpc_dma_chan {
@@ -193,6 +213,12 @@ struct mpc_dma_chan {
 	struct mpc_dma_tcd		*tcd;
 	dma_addr_t			tcd_paddr;
 
+	/* Settings for access to peripheral FIFO */
+	dma_addr_t			src_per_paddr;
+	u32				src_tcd_nunits;
+	dma_addr_t			dst_per_paddr;
+	u32				dst_tcd_nunits;
+
 	/* Lock for this structure */
 	spinlock_t			lock;
 };
@@ -243,8 +269,23 @@ static void mpc_dma_execute(struct mpc_dma_chan *mchan)
 	struct mpc_dma_desc *mdesc;
 	int cid = mchan->chan.chan_id;
 
-	/* Move all queued descriptors to active list */
-	list_splice_tail_init(&mchan->queued, &mchan->active);
+	while (!list_empty(&mchan->queued)) {
+		mdesc = list_first_entry(&mchan->queued,
+						struct mpc_dma_desc, node);
+		/*
+		 * Grab either several mem-to-mem transfer descriptors
+		 * or one peripheral transfer descriptor,
+		 * don't mix mem-to-mem and peripheral transfer descriptors
+		 * within the same 'active' list.
+		 */
+		if (mdesc->will_access_peripheral) {
+			if (list_empty(&mchan->active))
+				list_move_tail(&mdesc->node, &mchan->active);
+			break;
+		} else {
+			list_move_tail(&mdesc->node, &mchan->active);
+		}
+	}
 
 	/* Chain descriptors into one transaction */
 	list_for_each_entry(mdesc, &mchan->active, node) {
@@ -270,7 +311,17 @@ static void mpc_dma_execute(struct mpc_dma_chan *mchan)
 
 	if (first != prev)
 		mdma->tcd[cid].e_sg = 1;
-	out_8(&mdma->regs->dmassrt, cid);
+
+	if (mdma->is_mpc8308) {
+		/* MPC8308, no request lines, software initiated start */
+		out_8(&mdma->regs->dmassrt, cid);
+	} else if (first->will_access_peripheral) {
+		/* Peripherals involved, start by external request signal */
+		out_8(&mdma->regs->dmaserq, cid);
+	} else {
+		/* Memory to memory transfer, software initiated start */
+		out_8(&mdma->regs->dmassrt, cid);
+	}
 }
 
 /* Handle interrupt on one half of DMA controller (32 channels) */
@@ -588,6 +639,7 @@ mpc_dma_prep_memcpy(struct dma_chan *chan, dma_addr_t dst, dma_addr_t src,
 	}
 
 	mdesc->error = 0;
+	mdesc->will_access_peripheral = 0;
 	tcd = mdesc->tcd;
 
 	/* Prepare Transfer Control Descriptor for this transaction */
@@ -635,6 +687,193 @@ mpc_dma_prep_memcpy(struct dma_chan *chan, dma_addr_t dst, dma_addr_t src,
 	return &mdesc->desc;
 }
 
+static struct dma_async_tx_descriptor *
+mpc_dma_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
+		unsigned int sg_len, enum dma_transfer_direction direction,
+		unsigned long flags, void *context)
+{
+	struct mpc_dma *mdma = dma_chan_to_mpc_dma(chan);
+	struct mpc_dma_chan *mchan = dma_chan_to_mpc_dma_chan(chan);
+	struct mpc_dma_desc *mdesc = NULL;
+	dma_addr_t per_paddr;
+	u32 tcd_nunits;
+	struct mpc_dma_tcd *tcd;
+	unsigned long iflags;
+	struct scatterlist *sg;
+	size_t len;
+	int iter, i;
+
+	/* Currently there is no proper support for scatter/gather */
+	if (sg_len != 1)
+		return NULL;
+
+	if (!is_slave_direction(direction))
+		return NULL;
+
+	for_each_sg(sgl, sg, sg_len, i) {
+		spin_lock_irqsave(&mchan->lock, iflags);
+
+		mdesc = list_first_entry(&mchan->free,
+						struct mpc_dma_desc, node);
+		if (!mdesc) {
+			spin_unlock_irqrestore(&mchan->lock, iflags);
+			/* Try to free completed descriptors */
+			mpc_dma_process_completed(mdma);
+			return NULL;
+		}
+
+		list_del(&mdesc->node);
+
+		if (direction == DMA_DEV_TO_MEM) {
+			per_paddr = mchan->src_per_paddr;
+			tcd_nunits = mchan->src_tcd_nunits;
+		} else {
+			per_paddr = mchan->dst_per_paddr;
+			tcd_nunits = mchan->dst_tcd_nunits;
+		}
+
+		spin_unlock_irqrestore(&mchan->lock, iflags);
+
+		if (per_paddr == 0 || tcd_nunits == 0)
+			goto err_prep;
+
+		mdesc->error = 0;
+		mdesc->will_access_peripheral = 1;
+
+		/* Prepare Transfer Control Descriptor for this transaction */
+		tcd = mdesc->tcd;
+
+		memset(tcd, 0, sizeof(struct mpc_dma_tcd));
+
+		if (!IS_ALIGNED(sg_dma_address(sg), 4))
+			goto err_prep;
+
+		if (direction == DMA_DEV_TO_MEM) {
+			tcd->saddr = per_paddr;
+			tcd->daddr = sg_dma_address(sg);
+			tcd->soff = 0;
+			tcd->doff = 4;
+		} else {
+			tcd->saddr = sg_dma_address(sg);
+			tcd->daddr = per_paddr;
+			tcd->soff = 4;
+			tcd->doff = 0;
+		}
+
+		tcd->ssize = MPC_DMA_TSIZE_4;
+		tcd->dsize = MPC_DMA_TSIZE_4;
+
+		len = sg_dma_len(sg);
+		tcd->nbytes = tcd_nunits * 4;
+		if (!IS_ALIGNED(len, tcd->nbytes))
+			goto err_prep;
+
+		iter = len / tcd->nbytes;
+		if (iter >= 1 << 15) {
+			/* len is too big */
+			goto err_prep;
+		}
+		/* citer_linkch contains the high bits of iter */
+		tcd->biter = iter & 0x1ff;
+		tcd->biter_linkch = iter >> 9;
+		tcd->citer = tcd->biter;
+		tcd->citer_linkch = tcd->biter_linkch;
+
+		tcd->e_sg = 0;
+		tcd->d_req = 1;
+
+		/* Place descriptor in prepared list */
+		spin_lock_irqsave(&mchan->lock, iflags);
+		list_add_tail(&mdesc->node, &mchan->prepared);
+		spin_unlock_irqrestore(&mchan->lock, iflags);
+	}
+
+	return &mdesc->desc;
+
+err_prep:
+	/* Put the descriptor back */
+	spin_lock_irqsave(&mchan->lock, iflags);
+	list_add_tail(&mdesc->node, &mchan->free);
+	spin_unlock_irqrestore(&mchan->lock, iflags);
+
+	return NULL;
+}
+
+static int mpc_dma_device_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
+							unsigned long arg)
+{
+	struct mpc_dma_chan *mchan;
+	struct mpc_dma *mdma;
+	struct dma_slave_config *cfg;
+	unsigned long flags;
+
+	mchan = dma_chan_to_mpc_dma_chan(chan);
+	switch (cmd) {
+	case DMA_TERMINATE_ALL:
+		/* Disable channel requests */
+		mdma = dma_chan_to_mpc_dma(chan);
+
+		spin_lock_irqsave(&mchan->lock, flags);
+
+		out_8(&mdma->regs->dmacerq, chan->chan_id);
+		list_splice_tail_init(&mchan->prepared, &mchan->free);
+		list_splice_tail_init(&mchan->queued, &mchan->free);
+		list_splice_tail_init(&mchan->active, &mchan->free);
+
+		spin_unlock_irqrestore(&mchan->lock, flags);
+
+		return 0;
+
+	case DMA_SLAVE_CONFIG:
+		/*
+		 * Software constraints:
+		 *  - only transfers between a peripheral device and
+		 *     memory are supported;
+		 *  - only peripheral devices with 4-byte FIFO access register
+		 *     are supported;
+		 *  - minimal transfer chunk is 4 bytes and consequently
+		 *     source and destination addresses must be 4-byte aligned
+		 *     and transfer size must be aligned on (4 * maxburst)
+		 *     boundary;
+		 *  - during the transfer RAM address is being incremented by
+		 *     the size of minimal transfer chunk;
+		 *  - peripheral port's address is constant during the transfer.
+		 */
+
+		cfg = (void *)arg;
+
+		if (cfg->src_addr_width != DMA_SLAVE_BUSWIDTH_4_BYTES ||
+		    cfg->dst_addr_width != DMA_SLAVE_BUSWIDTH_4_BYTES ||
+		    !IS_ALIGNED(cfg->src_addr, 4) ||
+		    !IS_ALIGNED(cfg->dst_addr, 4)) {
+			return -EINVAL;
+		}
+
+		spin_lock_irqsave(&mchan->lock, flags);
+
+		mchan->src_per_paddr = cfg->src_addr;
+		mchan->src_tcd_nunits = cfg->src_maxburst;
+		mchan->dst_per_paddr = cfg->dst_addr;
+		mchan->dst_tcd_nunits = cfg->dst_maxburst;
+
+		/* Apply defaults */
+		if (mchan->src_tcd_nunits == 0)
+			mchan->src_tcd_nunits = 1;
+		if (mchan->dst_tcd_nunits == 0)
+			mchan->dst_tcd_nunits = 1;
+
+		spin_unlock_irqrestore(&mchan->lock, flags);
+
+		return 0;
+
+	default:
+		/* Unknown command */
+		break;
+	}
+
+	return -ENXIO;
+}
+
 static int mpc_dma_probe(struct platform_device *op)
 {
 	struct device_node *dn = op->dev.of_node;
@@ -649,13 +888,15 @@ static int mpc_dma_probe(struct platform_device *op)
 	mdma = devm_kzalloc(dev, sizeof(struct mpc_dma), GFP_KERNEL);
 	if (!mdma) {
 		dev_err(dev, "Memory exhausted!\n");
-		return -ENOMEM;
+		retval = -ENOMEM;
+		goto err;
 	}
 
 	mdma->irq = irq_of_parse_and_map(dn, 0);
 	if (mdma->irq == NO_IRQ) {
 		dev_err(dev, "Error mapping IRQ!\n");
-		return -EINVAL;
+		retval = -EINVAL;
+		goto err;
 	}
 
 	if (of_device_is_compatible(dn, "fsl,mpc8308-dma")) {
@@ -663,14 +904,15 @@ static int mpc_dma_probe(struct platform_device *op)
 		mdma->irq2 = irq_of_parse_and_map(dn, 1);
 		if (mdma->irq2 == NO_IRQ) {
 			dev_err(dev, "Error mapping IRQ!\n");
-			return -EINVAL;
+			retval = -EINVAL;
+			goto err_dispose1;
 		}
 	}
 
 	retval = of_address_to_resource(dn, 0, &res);
 	if (retval) {
 		dev_err(dev, "Error parsing memory region!\n");
-		return retval;
+		goto err_dispose2;
 	}
 
 	regs_start = res.start;
@@ -678,31 +920,34 @@ static int mpc_dma_probe(struct platform_device *op)
 
 	if (!devm_request_mem_region(dev, regs_start, regs_size, DRV_NAME)) {
 		dev_err(dev, "Error requesting memory region!\n");
-		return -EBUSY;
+		retval = -EBUSY;
+		goto err_dispose2;
 	}
 
 	mdma->regs = devm_ioremap(dev, regs_start, regs_size);
 	if (!mdma->regs) {
 		dev_err(dev, "Error mapping memory region!\n");
-		return -ENOMEM;
+		retval = -ENOMEM;
+		goto err_dispose2;
 	}
 
 	mdma->tcd = (struct mpc_dma_tcd *)((u8 *)(mdma->regs)
 							+ MPC_DMA_TCD_OFFSET);
 
-	retval = devm_request_irq(dev, mdma->irq, &mpc_dma_irq, 0, DRV_NAME,
-									mdma);
+	retval = request_irq(mdma->irq, &mpc_dma_irq, 0, DRV_NAME, mdma);
 	if (retval) {
 		dev_err(dev, "Error requesting IRQ!\n");
-		return -EINVAL;
+		retval = -EINVAL;
+		goto err_dispose2;
 	}
 
 	if (mdma->is_mpc8308) {
-		retval = devm_request_irq(dev, mdma->irq2, &mpc_dma_irq, 0,
-				DRV_NAME, mdma);
+		retval = request_irq(mdma->irq2, &mpc_dma_irq, 0,
+							DRV_NAME, mdma);
 		if (retval) {
 			dev_err(dev, "Error requesting IRQ2!\n");
-			return -EINVAL;
+			retval = -EINVAL;
+			goto err_free1;
 		}
 	}
 
@@ -710,18 +955,21 @@ static int mpc_dma_probe(struct platform_device *op)
 
 	dma = &mdma->dma;
 	dma->dev = dev;
-	if (!mdma->is_mpc8308)
-		dma->chancnt = MPC_DMA_CHANNELS;
+	if (mdma->is_mpc8308)
+		dma->chancnt = MPC8308_DMACHAN_MAX;
 	else
-		dma->chancnt = 16; /* MPC8308 DMA has only 16 channels */
+		dma->chancnt = MPC512x_DMACHAN_MAX;
 	dma->device_alloc_chan_resources = mpc_dma_alloc_chan_resources;
 	dma->device_free_chan_resources = mpc_dma_free_chan_resources;
 	dma->device_issue_pending = mpc_dma_issue_pending;
 	dma->device_tx_status = mpc_dma_tx_status;
 	dma->device_prep_dma_memcpy = mpc_dma_prep_memcpy;
+	dma->device_prep_slave_sg = mpc_dma_prep_slave_sg;
+	dma->device_control = mpc_dma_device_control;
 
 	INIT_LIST_HEAD(&dma->channels);
 	dma_cap_set(DMA_MEMCPY, dma->cap_mask);
+	dma_cap_set(DMA_SLAVE, dma->cap_mask);
 
 	for (i = 0; i < dma->chancnt; i++) {
 		mchan = &mdma->channels[i];
@@ -747,7 +995,19 @@ static int mpc_dma_probe(struct platform_device *op)
 	 * - Round-robin group arbitration,
 	 * - Round-robin channel arbitration.
 	 */
-	if (!mdma->is_mpc8308) {
+	if (mdma->is_mpc8308) {
+		/* MPC8308 has 16 channels and lacks some registers */
+		out_be32(&mdma->regs->dmacr, MPC_DMA_DMACR_ERCA);
+
+		/* enable snooping */
+		out_be32(&mdma->regs->dmagpor, MPC_DMA_DMAGPOR_SNOOP_ENABLE);
+		/* Disable error interrupts */
+		out_be32(&mdma->regs->dmaeeil, 0);
+
+		/* Clear interrupts status */
+		out_be32(&mdma->regs->dmaintl, 0xFFFF);
+		out_be32(&mdma->regs->dmaerrl, 0xFFFF);
+	} else {
 		out_be32(&mdma->regs->dmacr, MPC_DMA_DMACR_EDCG |
 					MPC_DMA_DMACR_ERGA | MPC_DMA_DMACR_ERCA);
 
@@ -768,29 +1028,28 @@ static int mpc_dma_probe(struct platform_device *op)
 		/* Route interrupts to IPIC */
 		out_be32(&mdma->regs->dmaihsa, 0);
 		out_be32(&mdma->regs->dmailsa, 0);
-	} else {
-		/* MPC8308 has 16 channels and lacks some registers */
-		out_be32(&mdma->regs->dmacr, MPC_DMA_DMACR_ERCA);
-
-		/* enable snooping */
-		out_be32(&mdma->regs->dmagpor, MPC_DMA_DMAGPOR_SNOOP_ENABLE);
-		/* Disable error interrupts */
-		out_be32(&mdma->regs->dmaeeil, 0);
-
-		/* Clear interrupts status */
-		out_be32(&mdma->regs->dmaintl, 0xFFFF);
-		out_be32(&mdma->regs->dmaerrl, 0xFFFF);
 	}
 
 	/* Register DMA engine */
 	dev_set_drvdata(dev, mdma);
 	retval = dma_async_device_register(dma);
-	if (retval) {
-		devm_free_irq(dev, mdma->irq, mdma);
-		irq_dispose_mapping(mdma->irq);
-	}
+	if (retval)
+		goto err_free2;
 
 	return retval;
+
+err_free2:
+	if (mdma->is_mpc8308)
+		free_irq(mdma->irq2, mdma);
+err_free1:
+	free_irq(mdma->irq, mdma);
+err_dispose2:
+	if (mdma->is_mpc8308)
+		irq_dispose_mapping(mdma->irq2);
+err_dispose1:
+	irq_dispose_mapping(mdma->irq);
+err:
+	return retval;
 }
 
 static int mpc_dma_remove(struct platform_device *op)
@@ -799,7 +1058,11 @@ static int mpc_dma_remove(struct platform_device *op)
 	struct mpc_dma *mdma = dev_get_drvdata(dev);
 
 	dma_async_device_unregister(&mdma->dma);
-	devm_free_irq(dev, mdma->irq, mdma);
+	if (mdma->is_mpc8308) {
+		free_irq(mdma->irq2, mdma);
+		irq_dispose_mapping(mdma->irq2);
+	}
+	free_irq(mdma->irq, mdma);
 	irq_dispose_mapping(mdma->irq);
 
 	return 0;
@@ -807,6 +1070,7 @@ static int mpc_dma_remove(struct platform_device *op)
 
 static struct of_device_id mpc_dma_match[] = {
 	{ .compatible = "fsl,mpc5121-dma", },
+	{ .compatible = "fsl,mpc8308-dma", },
 	{},
 };
 
diff --git a/drivers/dma/mv_xor.c b/drivers/dma/mv_xor.c
index 766b68ed505..394cbc5c93e 100644
--- a/drivers/dma/mv_xor.c
+++ b/drivers/dma/mv_xor.c
@@ -191,12 +191,10 @@ static void mv_set_mode(struct mv_xor_chan *chan,
 
 static void mv_chan_activate(struct mv_xor_chan *chan)
 {
-	u32 activation;
-
 	dev_dbg(mv_chan_to_devp(chan), " activate chan.\n");
-	activation = readl_relaxed(XOR_ACTIVATION(chan));
-	activation |= 0x1;
-	writel_relaxed(activation, XOR_ACTIVATION(chan));
+
+	/* writel ensures all descriptors are flushed before activation */
+	writel(BIT(0), XOR_ACTIVATION(chan));
 }
 
 static char mv_chan_is_busy(struct mv_xor_chan *chan)
diff --git a/drivers/dma/pch_dma.c b/drivers/dma/pch_dma.c
index 05fa548bd65..9f9ca9fe5ce 100644
--- a/drivers/dma/pch_dma.c
+++ b/drivers/dma/pch_dma.c
@@ -21,6 +21,7 @@
 #include <linux/dma-mapping.h>
 #include <linux/init.h>
 #include <linux/pci.h>
+#include <linux/slab.h>
 #include <linux/interrupt.h>
 #include <linux/module.h>
 #include <linux/pch_dma.h>
@@ -996,7 +997,7 @@ static void pch_dma_remove(struct pci_dev *pdev)
 #define PCI_DEVICE_ID_ML7831_DMA1_8CH	0x8810
 #define PCI_DEVICE_ID_ML7831_DMA2_4CH	0x8815
 
-DEFINE_PCI_DEVICE_TABLE(pch_dma_id_table) = {
+const struct pci_device_id pch_dma_id_table[] = {
 	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_EG20T_PCH_DMA_8CH), 8 },
 	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_EG20T_PCH_DMA_4CH), 4 },
 	{ PCI_VDEVICE(ROHM, PCI_DEVICE_ID_ML7213_DMA1_8CH), 8}, /* UART Video */
diff --git a/drivers/dma/s3c24xx-dma.c b/drivers/dma/s3c24xx-dma.c
index b209a0f1734..012520c9fd7 100644
--- a/drivers/dma/s3c24xx-dma.c
+++ b/drivers/dma/s3c24xx-dma.c
@@ -164,6 +164,7 @@ struct s3c24xx_sg {
  * @disrcc: value for source control register
  * @didstc: value for destination control register
  * @dcon: base value for dcon register
+ * @cyclic: indicate cyclic transfer
  */
 struct s3c24xx_txd {
 	struct virt_dma_desc vd;
@@ -173,6 +174,7 @@ struct s3c24xx_txd {
 	u32 disrcc;
 	u32 didstc;
 	u32 dcon;
+	bool cyclic;
 };
 
 struct s3c24xx_dma_chan;
@@ -669,8 +671,10 @@ static irqreturn_t s3c24xx_dma_irq(int irq, void *data)
 		/* when more sg's are in this txd, start the next one */
 		if (!list_is_last(txd->at, &txd->dsg_list)) {
 			txd->at = txd->at->next;
+			if (txd->cyclic)
+				vchan_cyclic_callback(&txd->vd);
 			s3c24xx_dma_start_next_sg(s3cchan, txd);
-		} else {
+		} else if (!txd->cyclic) {
 			s3cchan->at = NULL;
 			vchan_cookie_complete(&txd->vd);
 
@@ -682,6 +686,12 @@ static irqreturn_t s3c24xx_dma_irq(int irq, void *data)
 				s3c24xx_dma_start_next_txd(s3cchan);
 			else
 				s3c24xx_dma_phy_free(s3cchan);
+		} else {
+			vchan_cyclic_callback(&txd->vd);
+
+			/* Cyclic: reset at beginning */
+			txd->at = txd->dsg_list.next;
+			s3c24xx_dma_start_next_sg(s3cchan, txd);
 		}
 	}
 	spin_unlock(&s3cchan->vc.lock);
@@ -877,6 +887,104 @@ static struct dma_async_tx_descriptor *s3c24xx_dma_prep_memcpy(
 	return vchan_tx_prep(&s3cchan->vc, &txd->vd, flags);
 }
 
+static struct dma_async_tx_descriptor *s3c24xx_dma_prep_dma_cyclic(
+	struct dma_chan *chan, dma_addr_t addr, size_t size, size_t period,
+	enum dma_transfer_direction direction, unsigned long flags,
+	void *context)
+{
+	struct s3c24xx_dma_chan *s3cchan = to_s3c24xx_dma_chan(chan);
+	struct s3c24xx_dma_engine *s3cdma = s3cchan->host;
+	const struct s3c24xx_dma_platdata *pdata = s3cdma->pdata;
+	struct s3c24xx_dma_channel *cdata = &pdata->channels[s3cchan->id];
+	struct s3c24xx_txd *txd;
+	struct s3c24xx_sg *dsg;
+	unsigned sg_len;
+	dma_addr_t slave_addr;
+	u32 hwcfg = 0;
+	int i;
+
+	dev_dbg(&s3cdma->pdev->dev,
+		"prepare cyclic transaction of %zu bytes with period %zu from %s\n",
+		size, period, s3cchan->name);
+
+	if (!is_slave_direction(direction)) {
+		dev_err(&s3cdma->pdev->dev,
+			"direction %d unsupported\n", direction);
+		return NULL;
+	}
+
+	txd = s3c24xx_dma_get_txd();
+	if (!txd)
+		return NULL;
+
+	txd->cyclic = 1;
+
+	if (cdata->handshake)
+		txd->dcon |= S3C24XX_DCON_HANDSHAKE;
+
+	switch (cdata->bus) {
+	case S3C24XX_DMA_APB:
+		txd->dcon |= S3C24XX_DCON_SYNC_PCLK;
+		hwcfg |= S3C24XX_DISRCC_LOC_APB;
+		break;
+	case S3C24XX_DMA_AHB:
+		txd->dcon |= S3C24XX_DCON_SYNC_HCLK;
+		hwcfg |= S3C24XX_DISRCC_LOC_AHB;
+		break;
+	}
+
+	/*
+	 * Always assume our peripheral desintation is a fixed
+	 * address in memory.
+	 */
+	hwcfg |= S3C24XX_DISRCC_INC_FIXED;
+
+	/*
+	 * Individual dma operations are requested by the slave,
+	 * so serve only single atomic operations (S3C24XX_DCON_SERV_SINGLE).
+	 */
+	txd->dcon |= S3C24XX_DCON_SERV_SINGLE;
+
+	if (direction == DMA_MEM_TO_DEV) {
+		txd->disrcc = S3C24XX_DISRCC_LOC_AHB |
+			      S3C24XX_DISRCC_INC_INCREMENT;
+		txd->didstc = hwcfg;
+		slave_addr = s3cchan->cfg.dst_addr;
+		txd->width = s3cchan->cfg.dst_addr_width;
+	} else {
+		txd->disrcc = hwcfg;
+		txd->didstc = S3C24XX_DIDSTC_LOC_AHB |
+			      S3C24XX_DIDSTC_INC_INCREMENT;
+		slave_addr = s3cchan->cfg.src_addr;
+		txd->width = s3cchan->cfg.src_addr_width;
+	}
+
+	sg_len = size / period;
+
+	for (i = 0; i < sg_len; i++) {
+		dsg = kzalloc(sizeof(*dsg), GFP_NOWAIT);
+		if (!dsg) {
+			s3c24xx_dma_free_txd(txd);
+			return NULL;
+		}
+		list_add_tail(&dsg->node, &txd->dsg_list);
+
+		dsg->len = period;
+		/* Check last period length */
+		if (i == sg_len - 1)
+			dsg->len = size - period * i;
+		if (direction == DMA_MEM_TO_DEV) {
+			dsg->src_addr = addr + period * i;
+			dsg->dst_addr = slave_addr;
+		} else { /* DMA_DEV_TO_MEM */
+			dsg->src_addr = slave_addr;
+			dsg->dst_addr = addr + period * i;
+		}
+	}
+
+	return vchan_tx_prep(&s3cchan->vc, &txd->vd, flags);
+}
+
 static struct dma_async_tx_descriptor *s3c24xx_dma_prep_slave_sg(
 		struct dma_chan *chan, struct scatterlist *sgl,
 		unsigned int sg_len, enum dma_transfer_direction direction,
@@ -961,7 +1069,6 @@ static struct dma_async_tx_descriptor *s3c24xx_dma_prep_slave_sg(
 			dsg->src_addr = slave_addr;
 			dsg->dst_addr = sg_dma_address(sg);
 		}
-		break;
 	}
 
 	return vchan_tx_prep(&s3cchan->vc, &txd->vd, flags);
@@ -1198,6 +1305,7 @@ static int s3c24xx_dma_probe(struct platform_device *pdev)
 
 	/* Initialize slave engine for SoC internal dedicated peripherals */
 	dma_cap_set(DMA_SLAVE, s3cdma->slave.cap_mask);
+	dma_cap_set(DMA_CYCLIC, s3cdma->slave.cap_mask);
 	dma_cap_set(DMA_PRIVATE, s3cdma->slave.cap_mask);
 	s3cdma->slave.dev = &pdev->dev;
 	s3cdma->slave.device_alloc_chan_resources =
@@ -1207,6 +1315,7 @@ static int s3c24xx_dma_probe(struct platform_device *pdev)
 	s3cdma->slave.device_tx_status = s3c24xx_dma_tx_status;
 	s3cdma->slave.device_issue_pending = s3c24xx_dma_issue_pending;
 	s3cdma->slave.device_prep_slave_sg = s3c24xx_dma_prep_slave_sg;
+	s3cdma->slave.device_prep_dma_cyclic = s3c24xx_dma_prep_dma_cyclic;
 	s3cdma->slave.device_control = s3c24xx_dma_control;
 
 	/* Register as many memcpy channels as there are physical channels */
diff --git a/drivers/dma/sa11x0-dma.c b/drivers/dma/sa11x0-dma.c
index ab26d46bbe1..5ebdfbc1051 100644
--- a/drivers/dma/sa11x0-dma.c
+++ b/drivers/dma/sa11x0-dma.c
@@ -113,11 +113,9 @@ struct sa11x0_dma_phy {
 	struct sa11x0_dma_desc	*txd_load;
 	unsigned		sg_done;
 	struct sa11x0_dma_desc	*txd_done;
-#ifdef CONFIG_PM_SLEEP
 	u32			dbs[2];
 	u32			dbt[2];
 	u32			dcsr;
-#endif
 };
 
 struct sa11x0_dma_dev {
@@ -984,7 +982,6 @@ static int sa11x0_dma_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
 static int sa11x0_dma_suspend(struct device *dev)
 {
 	struct sa11x0_dma_dev *d = dev_get_drvdata(dev);
@@ -1054,7 +1051,6 @@ static int sa11x0_dma_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static const struct dev_pm_ops sa11x0_dma_pm_ops = {
 	.suspend_noirq = sa11x0_dma_suspend,
diff --git a/drivers/dma/sh/Kconfig b/drivers/dma/sh/Kconfig
index b4c81383100..0f719816c91 100644
--- a/drivers/dma/sh/Kconfig
+++ b/drivers/dma/sh/Kconfig
@@ -4,7 +4,7 @@
 
 config SH_DMAE_BASE
 	bool "Renesas SuperH DMA Engine support"
-	depends on (SUPERH && SH_DMA) || (ARM && ARCH_SHMOBILE)
+	depends on (SUPERH && SH_DMA) || ARCH_SHMOBILE || COMPILE_TEST
 	depends on !SH_DMA_API
 	default y
 	select DMA_ENGINE
diff --git a/drivers/dma/sh/rcar-hpbdma.c b/drivers/dma/sh/rcar-hpbdma.c
index 3083d901a41..b212d9471ab 100644
--- a/drivers/dma/sh/rcar-hpbdma.c
+++ b/drivers/dma/sh/rcar-hpbdma.c
@@ -18,6 +18,7 @@
 
 #include <linux/dmaengine.h>
 #include <linux/delay.h>
+#include <linux/err.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
 #include <linux/module.h>
diff --git a/drivers/dma/sh/shdma-base.c b/drivers/dma/sh/shdma-base.c
index 52396771acb..b35007e21e6 100644
--- a/drivers/dma/sh/shdma-base.c
+++ b/drivers/dma/sh/shdma-base.c
@@ -73,8 +73,7 @@ static void shdma_chan_xfer_ld_queue(struct shdma_chan *schan)
 static dma_cookie_t shdma_tx_submit(struct dma_async_tx_descriptor *tx)
 {
 	struct shdma_desc *chunk, *c, *desc =
-		container_of(tx, struct shdma_desc, async_tx),
-		*last = desc;
+		container_of(tx, struct shdma_desc, async_tx);
 	struct shdma_chan *schan = to_shdma_chan(tx->chan);
 	dma_async_tx_callback callback = tx->callback;
 	dma_cookie_t cookie;
@@ -98,19 +97,20 @@ static dma_cookie_t shdma_tx_submit(struct dma_async_tx_descriptor *tx)
 				      &chunk->node == &schan->ld_free))
 			break;
 		chunk->mark = DESC_SUBMITTED;
-		/* Callback goes to the last chunk */
-		chunk->async_tx.callback = NULL;
+		if (chunk->chunks == 1) {
+			chunk->async_tx.callback = callback;
+			chunk->async_tx.callback_param = tx->callback_param;
+		} else {
+			/* Callback goes to the last chunk */
+			chunk->async_tx.callback = NULL;
+		}
 		chunk->cookie = cookie;
 		list_move_tail(&chunk->node, &schan->ld_queue);
-		last = chunk;
 
 		dev_dbg(schan->dev, "submit #%d@%p on %d\n",
-			tx->cookie, &last->async_tx, schan->id);
+			tx->cookie, &chunk->async_tx, schan->id);
 	}
 
-	last->async_tx.callback = callback;
-	last->async_tx.callback_param = tx->callback_param;
-
 	if (power_up) {
 		int ret;
 		schan->pm_state = SHDMA_PM_BUSY;
@@ -304,6 +304,7 @@ static dma_async_tx_callback __ld_cleanup(struct shdma_chan *schan, bool all)
 	dma_async_tx_callback callback = NULL;
 	void *param = NULL;
 	unsigned long flags;
+	LIST_HEAD(cyclic_list);
 
 	spin_lock_irqsave(&schan->chan_lock, flags);
 	list_for_each_entry_safe(desc, _desc, &schan->ld_queue, node) {
@@ -369,10 +370,16 @@ static dma_async_tx_callback __ld_cleanup(struct shdma_chan *schan, bool all)
 		if (((desc->mark == DESC_COMPLETED ||
 		      desc->mark == DESC_WAITING) &&
 		     async_tx_test_ack(&desc->async_tx)) || all) {
-			/* Remove from ld_queue list */
-			desc->mark = DESC_IDLE;
 
-			list_move(&desc->node, &schan->ld_free);
+			if (all || !desc->cyclic) {
+				/* Remove from ld_queue list */
+				desc->mark = DESC_IDLE;
+				list_move(&desc->node, &schan->ld_free);
+			} else {
+				/* reuse as cyclic */
+				desc->mark = DESC_SUBMITTED;
+				list_move_tail(&desc->node, &cyclic_list);
+			}
 
 			if (list_empty(&schan->ld_queue)) {
 				dev_dbg(schan->dev, "Bring down channel %d\n", schan->id);
@@ -389,6 +396,8 @@ static dma_async_tx_callback __ld_cleanup(struct shdma_chan *schan, bool all)
 		 */
 		schan->dma_chan.completed_cookie = schan->dma_chan.cookie;
 
+	list_splice_tail(&cyclic_list, &schan->ld_queue);
+
 	spin_unlock_irqrestore(&schan->chan_lock, flags);
 
 	if (callback)
@@ -521,7 +530,7 @@ static struct shdma_desc *shdma_add_desc(struct shdma_chan *schan,
  */
 static struct dma_async_tx_descriptor *shdma_prep_sg(struct shdma_chan *schan,
 	struct scatterlist *sgl, unsigned int sg_len, dma_addr_t *addr,
-	enum dma_transfer_direction direction, unsigned long flags)
+	enum dma_transfer_direction direction, unsigned long flags, bool cyclic)
 {
 	struct scatterlist *sg;
 	struct shdma_desc *first = NULL, *new = NULL /* compiler... */;
@@ -569,7 +578,11 @@ static struct dma_async_tx_descriptor *shdma_prep_sg(struct shdma_chan *schan,
 			if (!new)
 				goto err_get_desc;
 
-			new->chunks = chunks--;
+			new->cyclic = cyclic;
+			if (cyclic)
+				new->chunks = 1;
+			else
+				new->chunks = chunks--;
 			list_add_tail(&new->node, &tx_list);
 		} while (len);
 	}
@@ -612,7 +625,8 @@ static struct dma_async_tx_descriptor *shdma_prep_memcpy(
 	sg_dma_address(&sg) = dma_src;
 	sg_dma_len(&sg) = len;
 
-	return shdma_prep_sg(schan, &sg, 1, &dma_dest, DMA_MEM_TO_MEM, flags);
+	return shdma_prep_sg(schan, &sg, 1, &dma_dest, DMA_MEM_TO_MEM,
+			     flags, false);
 }
 
 static struct dma_async_tx_descriptor *shdma_prep_slave_sg(
@@ -640,7 +654,58 @@ static struct dma_async_tx_descriptor *shdma_prep_slave_sg(
 	slave_addr = ops->slave_addr(schan);
 
 	return shdma_prep_sg(schan, sgl, sg_len, &slave_addr,
-			      direction, flags);
+			     direction, flags, false);
+}
+
+#define SHDMA_MAX_SG_LEN 32
+
+static struct dma_async_tx_descriptor *shdma_prep_dma_cyclic(
+	struct dma_chan *chan, dma_addr_t buf_addr, size_t buf_len,
+	size_t period_len, enum dma_transfer_direction direction,
+	unsigned long flags, void *context)
+{
+	struct shdma_chan *schan = to_shdma_chan(chan);
+	struct shdma_dev *sdev = to_shdma_dev(schan->dma_chan.device);
+	const struct shdma_ops *ops = sdev->ops;
+	unsigned int sg_len = buf_len / period_len;
+	int slave_id = schan->slave_id;
+	dma_addr_t slave_addr;
+	struct scatterlist sgl[SHDMA_MAX_SG_LEN];
+	int i;
+
+	if (!chan)
+		return NULL;
+
+	BUG_ON(!schan->desc_num);
+
+	if (sg_len > SHDMA_MAX_SG_LEN) {
+		dev_err(schan->dev, "sg length %d exceds limit %d",
+				sg_len, SHDMA_MAX_SG_LEN);
+		return NULL;
+	}
+
+	/* Someone calling slave DMA on a generic channel? */
+	if (slave_id < 0 || (buf_len < period_len)) {
+		dev_warn(schan->dev,
+			"%s: bad parameter: buf_len=%zu, period_len=%zu, id=%d\n",
+			__func__, buf_len, period_len, slave_id);
+		return NULL;
+	}
+
+	slave_addr = ops->slave_addr(schan);
+
+	sg_init_table(sgl, sg_len);
+	for (i = 0; i < sg_len; i++) {
+		dma_addr_t src = buf_addr + (period_len * i);
+
+		sg_set_page(&sgl[i], pfn_to_page(PFN_DOWN(src)), period_len,
+			    offset_in_page(src));
+		sg_dma_address(&sgl[i]) = src;
+		sg_dma_len(&sgl[i]) = period_len;
+	}
+
+	return shdma_prep_sg(schan, sgl, sg_len, &slave_addr,
+			     direction, flags, true);
 }
 
 static int shdma_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
@@ -915,6 +980,7 @@ int shdma_init(struct device *dev, struct shdma_dev *sdev,
 
 	/* Compulsory for DMA_SLAVE fields */
 	dma_dev->device_prep_slave_sg = shdma_prep_slave_sg;
+	dma_dev->device_prep_dma_cyclic = shdma_prep_dma_cyclic;
 	dma_dev->device_control = shdma_control;
 
 	dma_dev->dev = dev;
diff --git a/drivers/dma/sh/shdmac.c b/drivers/dma/sh/shdmac.c
index dda7e7563f5..146d5df926d 100644
--- a/drivers/dma/sh/shdmac.c
+++ b/drivers/dma/sh/shdmac.c
@@ -18,21 +18,22 @@
  *
  */
 
+#include <linux/delay.h>
+#include <linux/dmaengine.h>
+#include <linux/err.h>
 #include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/kdebug.h>
 #include <linux/module.h>
+#include <linux/notifier.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
-#include <linux/slab.h>
-#include <linux/interrupt.h>
-#include <linux/dmaengine.h>
-#include <linux/delay.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
+#include <linux/rculist.h>
 #include <linux/sh_dma.h>
-#include <linux/notifier.h>
-#include <linux/kdebug.h>
+#include <linux/slab.h>
 #include <linux/spinlock.h>
-#include <linux/rculist.h>
 
 #include "../dmaengine.h"
 #include "shdma.h"
diff --git a/drivers/dma/sh/sudmac.c b/drivers/dma/sh/sudmac.c
index 4e7df43b50d..3ce10390989 100644
--- a/drivers/dma/sh/sudmac.c
+++ b/drivers/dma/sh/sudmac.c
@@ -14,12 +14,13 @@
  * published by the Free Software Foundation.
  */
 
+#include <linux/dmaengine.h>
+#include <linux/err.h>
 #include <linux/init.h>
-#include <linux/module.h>
-#include <linux/slab.h>
 #include <linux/interrupt.h>
-#include <linux/dmaengine.h>
+#include <linux/module.h>
 #include <linux/platform_device.h>
+#include <linux/slab.h>
 #include <linux/sudmac.h>
 
 struct sudmac_chan {
diff --git a/drivers/dma/ste_dma40.c b/drivers/dma/ste_dma40.c
index bf18c786ed4..c7984459ede 100644
--- a/drivers/dma/ste_dma40.c
+++ b/drivers/dma/ste_dma40.c
@@ -556,7 +556,6 @@ struct d40_gen_dmac {
  * later
  * @reg_val_backup_chan: Backup data for standard channel parameter registers.
  * @gcc_pwr_off_mask: Mask to maintain the channels that can be turned off.
- * @initialized: true if the dma has been initialized
  * @gen_dmac: the struct for generic registers values to represent u8500/8540
  * DMA controller
  */
@@ -594,7 +593,6 @@ struct d40_base {
 	u32				  reg_val_backup_v4[BACKUP_REGS_SZ_MAX];
 	u32				 *reg_val_backup_chan;
 	u16				  gcc_pwr_off_mask;
-	bool				  initialized;
 	struct d40_gen_dmac		  gen_dmac;
 };
 
@@ -1056,62 +1054,6 @@ static int d40_sg_2_dmalen(struct scatterlist *sgl, int sg_len,
 	return len;
 }
 
-
-#ifdef CONFIG_PM
-static void dma40_backup(void __iomem *baseaddr, u32 *backup,
-			 u32 *regaddr, int num, bool save)
-{
-	int i;
-
-	for (i = 0; i < num; i++) {
-		void __iomem *addr = baseaddr + regaddr[i];
-
-		if (save)
-			backup[i] = readl_relaxed(addr);
-		else
-			writel_relaxed(backup[i], addr);
-	}
-}
-
-static void d40_save_restore_registers(struct d40_base *base, bool save)
-{
-	int i;
-
-	/* Save/Restore channel specific registers */
-	for (i = 0; i < base->num_phy_chans; i++) {
-		void __iomem *addr;
-		int idx;
-
-		if (base->phy_res[i].reserved)
-			continue;
-
-		addr = base->virtbase + D40_DREG_PCBASE + i * D40_DREG_PCDELTA;
-		idx = i * ARRAY_SIZE(d40_backup_regs_chan);
-
-		dma40_backup(addr, &base->reg_val_backup_chan[idx],
-			     d40_backup_regs_chan,
-			     ARRAY_SIZE(d40_backup_regs_chan),
-			     save);
-	}
-
-	/* Save/Restore global registers */
-	dma40_backup(base->virtbase, base->reg_val_backup,
-		     d40_backup_regs, ARRAY_SIZE(d40_backup_regs),
-		     save);
-
-	/* Save/Restore registers only existing on dma40 v3 and later */
-	if (base->gen_dmac.backup)
-		dma40_backup(base->virtbase, base->reg_val_backup_v4,
-			     base->gen_dmac.backup,
-			base->gen_dmac.backup_size,
-			save);
-}
-#else
-static void d40_save_restore_registers(struct d40_base *base, bool save)
-{
-}
-#endif
-
 static int __d40_execute_command_phy(struct d40_chan *d40c,
 				     enum d40_command command)
 {
@@ -1495,8 +1437,8 @@ static int d40_pause(struct d40_chan *d40c)
 	if (!d40c->busy)
 		return 0;
 
-	pm_runtime_get_sync(d40c->base->dev);
 	spin_lock_irqsave(&d40c->lock, flags);
+	pm_runtime_get_sync(d40c->base->dev);
 
 	res = d40_channel_execute_command(d40c, D40_DMA_SUSPEND_REQ);
 
@@ -2998,18 +2940,88 @@ failure1:
 }
 
 /* Suspend resume functionality */
-#ifdef CONFIG_PM
-static int dma40_pm_suspend(struct device *dev)
+#ifdef CONFIG_PM_SLEEP
+static int dma40_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct d40_base *base = platform_get_drvdata(pdev);
-	int ret = 0;
+	int ret;
+
+	ret = pm_runtime_force_suspend(dev);
+	if (ret)
+		return ret;
 
 	if (base->lcpa_regulator)
 		ret = regulator_disable(base->lcpa_regulator);
 	return ret;
 }
 
+static int dma40_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct d40_base *base = platform_get_drvdata(pdev);
+	int ret = 0;
+
+	if (base->lcpa_regulator) {
+		ret = regulator_enable(base->lcpa_regulator);
+		if (ret)
+			return ret;
+	}
+
+	return pm_runtime_force_resume(dev);
+}
+#endif
+
+#ifdef CONFIG_PM
+static void dma40_backup(void __iomem *baseaddr, u32 *backup,
+			 u32 *regaddr, int num, bool save)
+{
+	int i;
+
+	for (i = 0; i < num; i++) {
+		void __iomem *addr = baseaddr + regaddr[i];
+
+		if (save)
+			backup[i] = readl_relaxed(addr);
+		else
+			writel_relaxed(backup[i], addr);
+	}
+}
+
+static void d40_save_restore_registers(struct d40_base *base, bool save)
+{
+	int i;
+
+	/* Save/Restore channel specific registers */
+	for (i = 0; i < base->num_phy_chans; i++) {
+		void __iomem *addr;
+		int idx;
+
+		if (base->phy_res[i].reserved)
+			continue;
+
+		addr = base->virtbase + D40_DREG_PCBASE + i * D40_DREG_PCDELTA;
+		idx = i * ARRAY_SIZE(d40_backup_regs_chan);
+
+		dma40_backup(addr, &base->reg_val_backup_chan[idx],
+			     d40_backup_regs_chan,
+			     ARRAY_SIZE(d40_backup_regs_chan),
+			     save);
+	}
+
+	/* Save/Restore global registers */
+	dma40_backup(base->virtbase, base->reg_val_backup,
+		     d40_backup_regs, ARRAY_SIZE(d40_backup_regs),
+		     save);
+
+	/* Save/Restore registers only existing on dma40 v3 and later */
+	if (base->gen_dmac.backup)
+		dma40_backup(base->virtbase, base->reg_val_backup_v4,
+			     base->gen_dmac.backup,
+			base->gen_dmac.backup_size,
+			save);
+}
+
 static int dma40_runtime_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
@@ -3030,36 +3042,20 @@ static int dma40_runtime_resume(struct device *dev)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct d40_base *base = platform_get_drvdata(pdev);
 
-	if (base->initialized)
-		d40_save_restore_registers(base, false);
+	d40_save_restore_registers(base, false);
 
 	writel_relaxed(D40_DREG_GCC_ENABLE_ALL,
 		       base->virtbase + D40_DREG_GCC);
 	return 0;
 }
-
-static int dma40_resume(struct device *dev)
-{
-	struct platform_device *pdev = to_platform_device(dev);
-	struct d40_base *base = platform_get_drvdata(pdev);
-	int ret = 0;
-
-	if (base->lcpa_regulator)
-		ret = regulator_enable(base->lcpa_regulator);
-
-	return ret;
-}
+#endif
 
 static const struct dev_pm_ops dma40_pm_ops = {
-	.suspend		= dma40_pm_suspend,
-	.runtime_suspend	= dma40_runtime_suspend,
-	.runtime_resume		= dma40_runtime_resume,
-	.resume			= dma40_resume,
+	SET_LATE_SYSTEM_SLEEP_PM_OPS(dma40_suspend, dma40_resume)
+	SET_PM_RUNTIME_PM_OPS(dma40_runtime_suspend,
+				dma40_runtime_resume,
+				NULL)
 };
-#define DMA40_PM_OPS	(&dma40_pm_ops)
-#else
-#define DMA40_PM_OPS	NULL
-#endif
 
 /* Initialization functions. */
 
@@ -3645,12 +3641,6 @@ static int __init d40_probe(struct platform_device *pdev)
 		goto failure;
 	}
 
-	pm_runtime_irq_safe(base->dev);
-	pm_runtime_set_autosuspend_delay(base->dev, DMA40_AUTOSUSPEND_DELAY);
-	pm_runtime_use_autosuspend(base->dev);
-	pm_runtime_enable(base->dev);
-	pm_runtime_resume(base->dev);
-
 	if (base->plat_data->use_esram_lcla) {
 
 		base->lcpa_regulator = regulator_get(base->dev, "lcla_esram");
@@ -3671,7 +3661,15 @@ static int __init d40_probe(struct platform_device *pdev)
 		}
 	}
 
-	base->initialized = true;
+	writel_relaxed(D40_DREG_GCC_ENABLE_ALL, base->virtbase + D40_DREG_GCC);
+
+	pm_runtime_irq_safe(base->dev);
+	pm_runtime_set_autosuspend_delay(base->dev, DMA40_AUTOSUSPEND_DELAY);
+	pm_runtime_use_autosuspend(base->dev);
+	pm_runtime_mark_last_busy(base->dev);
+	pm_runtime_set_active(base->dev);
+	pm_runtime_enable(base->dev);
+
 	ret = d40_dmaengine_init(base, num_reserved_chans);
 	if (ret)
 		goto failure;
@@ -3754,7 +3752,7 @@ static struct platform_driver d40_driver = {
 	.driver = {
 		.owner = THIS_MODULE,
 		.name  = D40_NAME,
-		.pm = DMA40_PM_OPS,
+		.pm = &dma40_pm_ops,
 		.of_match_table = d40_match,
 	},
 };
diff --git a/drivers/dma/xilinx/Makefile b/drivers/dma/xilinx/Makefile
new file mode 100644
index 00000000000..3c4e9f2fea2
--- /dev/null
+++ b/drivers/dma/xilinx/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_XILINX_VDMA) += xilinx_vdma.o
diff --git a/drivers/dma/xilinx/xilinx_vdma.c b/drivers/dma/xilinx/xilinx_vdma.c
new file mode 100644
index 00000000000..42a13e8d460
--- /dev/null
+++ b/drivers/dma/xilinx/xilinx_vdma.c
@@ -0,0 +1,1379 @@
+/*
+ * DMA driver for Xilinx Video DMA Engine
+ *
+ * Copyright (C) 2010-2014 Xilinx, Inc. All rights reserved.
+ *
+ * Based on the Freescale DMA driver.
+ *
+ * Description:
+ * The AXI Video Direct Memory Access (AXI VDMA) core is a soft Xilinx IP
+ * core that provides high-bandwidth direct memory access between memory
+ * and AXI4-Stream type video target peripherals. The core provides efficient
+ * two dimensional DMA operations with independent asynchronous read (S2MM)
+ * and write (MM2S) channel operation. It can be configured to have either
+ * one channel or two channels. If configured as two channels, one is to
+ * transmit to the video device (MM2S) and another is to receive from the
+ * video device (S2MM). Initialization, status, interrupt and management
+ * registers are accessed through an AXI4-Lite slave interface.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the 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/amba/xilinx_dma.h>
+#include <linux/bitops.h>
+#include <linux/dmapool.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/of_dma.h>
+#include <linux/of_platform.h>
+#include <linux/of_irq.h>
+#include <linux/slab.h>
+
+#include "../dmaengine.h"
+
+/* Register/Descriptor Offsets */
+#define XILINX_VDMA_MM2S_CTRL_OFFSET		0x0000
+#define XILINX_VDMA_S2MM_CTRL_OFFSET		0x0030
+#define XILINX_VDMA_MM2S_DESC_OFFSET		0x0050
+#define XILINX_VDMA_S2MM_DESC_OFFSET		0x00a0
+
+/* Control Registers */
+#define XILINX_VDMA_REG_DMACR			0x0000
+#define XILINX_VDMA_DMACR_DELAY_MAX		0xff
+#define XILINX_VDMA_DMACR_DELAY_SHIFT		24
+#define XILINX_VDMA_DMACR_FRAME_COUNT_MAX	0xff
+#define XILINX_VDMA_DMACR_FRAME_COUNT_SHIFT	16
+#define XILINX_VDMA_DMACR_ERR_IRQ		BIT(14)
+#define XILINX_VDMA_DMACR_DLY_CNT_IRQ		BIT(13)
+#define XILINX_VDMA_DMACR_FRM_CNT_IRQ		BIT(12)
+#define XILINX_VDMA_DMACR_MASTER_SHIFT		8
+#define XILINX_VDMA_DMACR_FSYNCSRC_SHIFT	5
+#define XILINX_VDMA_DMACR_FRAMECNT_EN		BIT(4)
+#define XILINX_VDMA_DMACR_GENLOCK_EN		BIT(3)
+#define XILINX_VDMA_DMACR_RESET			BIT(2)
+#define XILINX_VDMA_DMACR_CIRC_EN		BIT(1)
+#define XILINX_VDMA_DMACR_RUNSTOP		BIT(0)
+#define XILINX_VDMA_DMACR_FSYNCSRC_MASK		GENMASK(6, 5)
+
+#define XILINX_VDMA_REG_DMASR			0x0004
+#define XILINX_VDMA_DMASR_EOL_LATE_ERR		BIT(15)
+#define XILINX_VDMA_DMASR_ERR_IRQ		BIT(14)
+#define XILINX_VDMA_DMASR_DLY_CNT_IRQ		BIT(13)
+#define XILINX_VDMA_DMASR_FRM_CNT_IRQ		BIT(12)
+#define XILINX_VDMA_DMASR_SOF_LATE_ERR		BIT(11)
+#define XILINX_VDMA_DMASR_SG_DEC_ERR		BIT(10)
+#define XILINX_VDMA_DMASR_SG_SLV_ERR		BIT(9)
+#define XILINX_VDMA_DMASR_EOF_EARLY_ERR		BIT(8)
+#define XILINX_VDMA_DMASR_SOF_EARLY_ERR		BIT(7)
+#define XILINX_VDMA_DMASR_DMA_DEC_ERR		BIT(6)
+#define XILINX_VDMA_DMASR_DMA_SLAVE_ERR		BIT(5)
+#define XILINX_VDMA_DMASR_DMA_INT_ERR		BIT(4)
+#define XILINX_VDMA_DMASR_IDLE			BIT(1)
+#define XILINX_VDMA_DMASR_HALTED		BIT(0)
+#define XILINX_VDMA_DMASR_DELAY_MASK		GENMASK(31, 24)
+#define XILINX_VDMA_DMASR_FRAME_COUNT_MASK	GENMASK(23, 16)
+
+#define XILINX_VDMA_REG_CURDESC			0x0008
+#define XILINX_VDMA_REG_TAILDESC		0x0010
+#define XILINX_VDMA_REG_REG_INDEX		0x0014
+#define XILINX_VDMA_REG_FRMSTORE		0x0018
+#define XILINX_VDMA_REG_THRESHOLD		0x001c
+#define XILINX_VDMA_REG_FRMPTR_STS		0x0024
+#define XILINX_VDMA_REG_PARK_PTR		0x0028
+#define XILINX_VDMA_PARK_PTR_WR_REF_SHIFT	8
+#define XILINX_VDMA_PARK_PTR_RD_REF_SHIFT	0
+#define XILINX_VDMA_REG_VDMA_VERSION		0x002c
+
+/* Register Direct Mode Registers */
+#define XILINX_VDMA_REG_VSIZE			0x0000
+#define XILINX_VDMA_REG_HSIZE			0x0004
+
+#define XILINX_VDMA_REG_FRMDLY_STRIDE		0x0008
+#define XILINX_VDMA_FRMDLY_STRIDE_FRMDLY_SHIFT	24
+#define XILINX_VDMA_FRMDLY_STRIDE_STRIDE_SHIFT	0
+
+#define XILINX_VDMA_REG_START_ADDRESS(n)	(0x000c + 4 * (n))
+
+/* HW specific definitions */
+#define XILINX_VDMA_MAX_CHANS_PER_DEVICE	0x2
+
+#define XILINX_VDMA_DMAXR_ALL_IRQ_MASK	\
+		(XILINX_VDMA_DMASR_FRM_CNT_IRQ | \
+		 XILINX_VDMA_DMASR_DLY_CNT_IRQ | \
+		 XILINX_VDMA_DMASR_ERR_IRQ)
+
+#define XILINX_VDMA_DMASR_ALL_ERR_MASK	\
+		(XILINX_VDMA_DMASR_EOL_LATE_ERR | \
+		 XILINX_VDMA_DMASR_SOF_LATE_ERR | \
+		 XILINX_VDMA_DMASR_SG_DEC_ERR | \
+		 XILINX_VDMA_DMASR_SG_SLV_ERR | \
+		 XILINX_VDMA_DMASR_EOF_EARLY_ERR | \
+		 XILINX_VDMA_DMASR_SOF_EARLY_ERR | \
+		 XILINX_VDMA_DMASR_DMA_DEC_ERR | \
+		 XILINX_VDMA_DMASR_DMA_SLAVE_ERR | \
+		 XILINX_VDMA_DMASR_DMA_INT_ERR)
+
+/*
+ * Recoverable errors are DMA Internal error, SOF Early, EOF Early
+ * and SOF Late. They are only recoverable when C_FLUSH_ON_FSYNC
+ * is enabled in the h/w system.
+ */
+#define XILINX_VDMA_DMASR_ERR_RECOVER_MASK	\
+		(XILINX_VDMA_DMASR_SOF_LATE_ERR | \
+		 XILINX_VDMA_DMASR_EOF_EARLY_ERR | \
+		 XILINX_VDMA_DMASR_SOF_EARLY_ERR | \
+		 XILINX_VDMA_DMASR_DMA_INT_ERR)
+
+/* Axi VDMA Flush on Fsync bits */
+#define XILINX_VDMA_FLUSH_S2MM		3
+#define XILINX_VDMA_FLUSH_MM2S		2
+#define XILINX_VDMA_FLUSH_BOTH		1
+
+/* Delay loop counter to prevent hardware failure */
+#define XILINX_VDMA_LOOP_COUNT		1000000
+
+/**
+ * struct xilinx_vdma_desc_hw - Hardware Descriptor
+ * @next_desc: Next Descriptor Pointer @0x00
+ * @pad1: Reserved @0x04
+ * @buf_addr: Buffer address @0x08
+ * @pad2: Reserved @0x0C
+ * @vsize: Vertical Size @0x10
+ * @hsize: Horizontal Size @0x14
+ * @stride: Number of bytes between the first
+ *	    pixels of each horizontal line @0x18
+ */
+struct xilinx_vdma_desc_hw {
+	u32 next_desc;
+	u32 pad1;
+	u32 buf_addr;
+	u32 pad2;
+	u32 vsize;
+	u32 hsize;
+	u32 stride;
+} __aligned(64);
+
+/**
+ * struct xilinx_vdma_tx_segment - Descriptor segment
+ * @hw: Hardware descriptor
+ * @node: Node in the descriptor segments list
+ * @phys: Physical address of segment
+ */
+struct xilinx_vdma_tx_segment {
+	struct xilinx_vdma_desc_hw hw;
+	struct list_head node;
+	dma_addr_t phys;
+} __aligned(64);
+
+/**
+ * struct xilinx_vdma_tx_descriptor - Per Transaction structure
+ * @async_tx: Async transaction descriptor
+ * @segments: TX segments list
+ * @node: Node in the channel descriptors list
+ */
+struct xilinx_vdma_tx_descriptor {
+	struct dma_async_tx_descriptor async_tx;
+	struct list_head segments;
+	struct list_head node;
+};
+
+/**
+ * struct xilinx_vdma_chan - Driver specific VDMA channel structure
+ * @xdev: Driver specific device structure
+ * @ctrl_offset: Control registers offset
+ * @desc_offset: TX descriptor registers offset
+ * @lock: Descriptor operation lock
+ * @pending_list: Descriptors waiting
+ * @active_desc: Active descriptor
+ * @allocated_desc: Allocated descriptor
+ * @done_list: Complete descriptors
+ * @common: DMA common channel
+ * @desc_pool: Descriptors pool
+ * @dev: The dma device
+ * @irq: Channel IRQ
+ * @id: Channel ID
+ * @direction: Transfer direction
+ * @num_frms: Number of frames
+ * @has_sg: Support scatter transfers
+ * @genlock: Support genlock mode
+ * @err: Channel has errors
+ * @tasklet: Cleanup work after irq
+ * @config: Device configuration info
+ * @flush_on_fsync: Flush on Frame sync
+ */
+struct xilinx_vdma_chan {
+	struct xilinx_vdma_device *xdev;
+	u32 ctrl_offset;
+	u32 desc_offset;
+	spinlock_t lock;
+	struct list_head pending_list;
+	struct xilinx_vdma_tx_descriptor *active_desc;
+	struct xilinx_vdma_tx_descriptor *allocated_desc;
+	struct list_head done_list;
+	struct dma_chan common;
+	struct dma_pool *desc_pool;
+	struct device *dev;
+	int irq;
+	int id;
+	enum dma_transfer_direction direction;
+	int num_frms;
+	bool has_sg;
+	bool genlock;
+	bool err;
+	struct tasklet_struct tasklet;
+	struct xilinx_vdma_config config;
+	bool flush_on_fsync;
+};
+
+/**
+ * struct xilinx_vdma_device - VDMA device structure
+ * @regs: I/O mapped base address
+ * @dev: Device Structure
+ * @common: DMA device structure
+ * @chan: Driver specific VDMA channel
+ * @has_sg: Specifies whether Scatter-Gather is present or not
+ * @flush_on_fsync: Flush on frame sync
+ */
+struct xilinx_vdma_device {
+	void __iomem *regs;
+	struct device *dev;
+	struct dma_device common;
+	struct xilinx_vdma_chan *chan[XILINX_VDMA_MAX_CHANS_PER_DEVICE];
+	bool has_sg;
+	u32 flush_on_fsync;
+};
+
+/* Macros */
+#define to_xilinx_chan(chan) \
+	container_of(chan, struct xilinx_vdma_chan, common)
+#define to_vdma_tx_descriptor(tx) \
+	container_of(tx, struct xilinx_vdma_tx_descriptor, async_tx)
+
+/* IO accessors */
+static inline u32 vdma_read(struct xilinx_vdma_chan *chan, u32 reg)
+{
+	return ioread32(chan->xdev->regs + reg);
+}
+
+static inline void vdma_write(struct xilinx_vdma_chan *chan, u32 reg, u32 value)
+{
+	iowrite32(value, chan->xdev->regs + reg);
+}
+
+static inline void vdma_desc_write(struct xilinx_vdma_chan *chan, u32 reg,
+				   u32 value)
+{
+	vdma_write(chan, chan->desc_offset + reg, value);
+}
+
+static inline u32 vdma_ctrl_read(struct xilinx_vdma_chan *chan, u32 reg)
+{
+	return vdma_read(chan, chan->ctrl_offset + reg);
+}
+
+static inline void vdma_ctrl_write(struct xilinx_vdma_chan *chan, u32 reg,
+				   u32 value)
+{
+	vdma_write(chan, chan->ctrl_offset + reg, value);
+}
+
+static inline void vdma_ctrl_clr(struct xilinx_vdma_chan *chan, u32 reg,
+				 u32 clr)
+{
+	vdma_ctrl_write(chan, reg, vdma_ctrl_read(chan, reg) & ~clr);
+}
+
+static inline void vdma_ctrl_set(struct xilinx_vdma_chan *chan, u32 reg,
+				 u32 set)
+{
+	vdma_ctrl_write(chan, reg, vdma_ctrl_read(chan, reg) | set);
+}
+
+/* -----------------------------------------------------------------------------
+ * Descriptors and segments alloc and free
+ */
+
+/**
+ * xilinx_vdma_alloc_tx_segment - Allocate transaction segment
+ * @chan: Driver specific VDMA channel
+ *
+ * Return: The allocated segment on success and NULL on failure.
+ */
+static struct xilinx_vdma_tx_segment *
+xilinx_vdma_alloc_tx_segment(struct xilinx_vdma_chan *chan)
+{
+	struct xilinx_vdma_tx_segment *segment;
+	dma_addr_t phys;
+
+	segment = dma_pool_alloc(chan->desc_pool, GFP_ATOMIC, &phys);
+	if (!segment)
+		return NULL;
+
+	memset(segment, 0, sizeof(*segment));
+	segment->phys = phys;
+
+	return segment;
+}
+
+/**
+ * xilinx_vdma_free_tx_segment - Free transaction segment
+ * @chan: Driver specific VDMA channel
+ * @segment: VDMA transaction segment
+ */
+static void xilinx_vdma_free_tx_segment(struct xilinx_vdma_chan *chan,
+					struct xilinx_vdma_tx_segment *segment)
+{
+	dma_pool_free(chan->desc_pool, segment, segment->phys);
+}
+
+/**
+ * xilinx_vdma_tx_descriptor - Allocate transaction descriptor
+ * @chan: Driver specific VDMA channel
+ *
+ * Return: The allocated descriptor on success and NULL on failure.
+ */
+static struct xilinx_vdma_tx_descriptor *
+xilinx_vdma_alloc_tx_descriptor(struct xilinx_vdma_chan *chan)
+{
+	struct xilinx_vdma_tx_descriptor *desc;
+	unsigned long flags;
+
+	if (chan->allocated_desc)
+		return chan->allocated_desc;
+
+	desc = kzalloc(sizeof(*desc), GFP_KERNEL);
+	if (!desc)
+		return NULL;
+
+	spin_lock_irqsave(&chan->lock, flags);
+	chan->allocated_desc = desc;
+	spin_unlock_irqrestore(&chan->lock, flags);
+
+	INIT_LIST_HEAD(&desc->segments);
+
+	return desc;
+}
+
+/**
+ * xilinx_vdma_free_tx_descriptor - Free transaction descriptor
+ * @chan: Driver specific VDMA channel
+ * @desc: VDMA transaction descriptor
+ */
+static void
+xilinx_vdma_free_tx_descriptor(struct xilinx_vdma_chan *chan,
+			       struct xilinx_vdma_tx_descriptor *desc)
+{
+	struct xilinx_vdma_tx_segment *segment, *next;
+
+	if (!desc)
+		return;
+
+	list_for_each_entry_safe(segment, next, &desc->segments, node) {
+		list_del(&segment->node);
+		xilinx_vdma_free_tx_segment(chan, segment);
+	}
+
+	kfree(desc);
+}
+
+/* Required functions */
+
+/**
+ * xilinx_vdma_free_desc_list - Free descriptors list
+ * @chan: Driver specific VDMA channel
+ * @list: List to parse and delete the descriptor
+ */
+static void xilinx_vdma_free_desc_list(struct xilinx_vdma_chan *chan,
+					struct list_head *list)
+{
+	struct xilinx_vdma_tx_descriptor *desc, *next;
+
+	list_for_each_entry_safe(desc, next, list, node) {
+		list_del(&desc->node);
+		xilinx_vdma_free_tx_descriptor(chan, desc);
+	}
+}
+
+/**
+ * xilinx_vdma_free_descriptors - Free channel descriptors
+ * @chan: Driver specific VDMA channel
+ */
+static void xilinx_vdma_free_descriptors(struct xilinx_vdma_chan *chan)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&chan->lock, flags);
+
+	xilinx_vdma_free_desc_list(chan, &chan->pending_list);
+	xilinx_vdma_free_desc_list(chan, &chan->done_list);
+
+	xilinx_vdma_free_tx_descriptor(chan, chan->active_desc);
+	chan->active_desc = NULL;
+
+	spin_unlock_irqrestore(&chan->lock, flags);
+}
+
+/**
+ * xilinx_vdma_free_chan_resources - Free channel resources
+ * @dchan: DMA channel
+ */
+static void xilinx_vdma_free_chan_resources(struct dma_chan *dchan)
+{
+	struct xilinx_vdma_chan *chan = to_xilinx_chan(dchan);
+
+	dev_dbg(chan->dev, "Free all channel resources.\n");
+
+	xilinx_vdma_free_descriptors(chan);
+	dma_pool_destroy(chan->desc_pool);
+	chan->desc_pool = NULL;
+}
+
+/**
+ * xilinx_vdma_chan_desc_cleanup - Clean channel descriptors
+ * @chan: Driver specific VDMA channel
+ */
+static void xilinx_vdma_chan_desc_cleanup(struct xilinx_vdma_chan *chan)
+{
+	struct xilinx_vdma_tx_descriptor *desc, *next;
+	unsigned long flags;
+
+	spin_lock_irqsave(&chan->lock, flags);
+
+	list_for_each_entry_safe(desc, next, &chan->done_list, node) {
+		dma_async_tx_callback callback;
+		void *callback_param;
+
+		/* Remove from the list of running transactions */
+		list_del(&desc->node);
+
+		/* Run the link descriptor callback function */
+		callback = desc->async_tx.callback;
+		callback_param = desc->async_tx.callback_param;
+		if (callback) {
+			spin_unlock_irqrestore(&chan->lock, flags);
+			callback(callback_param);
+			spin_lock_irqsave(&chan->lock, flags);
+		}
+
+		/* Run any dependencies, then free the descriptor */
+		dma_run_dependencies(&desc->async_tx);
+		xilinx_vdma_free_tx_descriptor(chan, desc);
+	}
+
+	spin_unlock_irqrestore(&chan->lock, flags);
+}
+
+/**
+ * xilinx_vdma_do_tasklet - Schedule completion tasklet
+ * @data: Pointer to the Xilinx VDMA channel structure
+ */
+static void xilinx_vdma_do_tasklet(unsigned long data)
+{
+	struct xilinx_vdma_chan *chan = (struct xilinx_vdma_chan *)data;
+
+	xilinx_vdma_chan_desc_cleanup(chan);
+}
+
+/**
+ * xilinx_vdma_alloc_chan_resources - Allocate channel resources
+ * @dchan: DMA channel
+ *
+ * Return: '0' on success and failure value on error
+ */
+static int xilinx_vdma_alloc_chan_resources(struct dma_chan *dchan)
+{
+	struct xilinx_vdma_chan *chan = to_xilinx_chan(dchan);
+
+	/* Has this channel already been allocated? */
+	if (chan->desc_pool)
+		return 0;
+
+	/*
+	 * We need the descriptor to be aligned to 64bytes
+	 * for meeting Xilinx VDMA specification requirement.
+	 */
+	chan->desc_pool = dma_pool_create("xilinx_vdma_desc_pool",
+				chan->dev,
+				sizeof(struct xilinx_vdma_tx_segment),
+				__alignof__(struct xilinx_vdma_tx_segment), 0);
+	if (!chan->desc_pool) {
+		dev_err(chan->dev,
+			"unable to allocate channel %d descriptor pool\n",
+			chan->id);
+		return -ENOMEM;
+	}
+
+	dma_cookie_init(dchan);
+	return 0;
+}
+
+/**
+ * xilinx_vdma_tx_status - Get VDMA transaction status
+ * @dchan: DMA channel
+ * @cookie: Transaction identifier
+ * @txstate: Transaction state
+ *
+ * Return: DMA transaction status
+ */
+static enum dma_status xilinx_vdma_tx_status(struct dma_chan *dchan,
+					dma_cookie_t cookie,
+					struct dma_tx_state *txstate)
+{
+	return dma_cookie_status(dchan, cookie, txstate);
+}
+
+/**
+ * xilinx_vdma_is_running - Check if VDMA channel is running
+ * @chan: Driver specific VDMA channel
+ *
+ * Return: '1' if running, '0' if not.
+ */
+static bool xilinx_vdma_is_running(struct xilinx_vdma_chan *chan)
+{
+	return !(vdma_ctrl_read(chan, XILINX_VDMA_REG_DMASR) &
+		 XILINX_VDMA_DMASR_HALTED) &&
+		(vdma_ctrl_read(chan, XILINX_VDMA_REG_DMACR) &
+		 XILINX_VDMA_DMACR_RUNSTOP);
+}
+
+/**
+ * xilinx_vdma_is_idle - Check if VDMA channel is idle
+ * @chan: Driver specific VDMA channel
+ *
+ * Return: '1' if idle, '0' if not.
+ */
+static bool xilinx_vdma_is_idle(struct xilinx_vdma_chan *chan)
+{
+	return vdma_ctrl_read(chan, XILINX_VDMA_REG_DMASR) &
+		XILINX_VDMA_DMASR_IDLE;
+}
+
+/**
+ * xilinx_vdma_halt - Halt VDMA channel
+ * @chan: Driver specific VDMA channel
+ */
+static void xilinx_vdma_halt(struct xilinx_vdma_chan *chan)
+{
+	int loop = XILINX_VDMA_LOOP_COUNT;
+
+	vdma_ctrl_clr(chan, XILINX_VDMA_REG_DMACR, XILINX_VDMA_DMACR_RUNSTOP);
+
+	/* Wait for the hardware to halt */
+	do {
+		if (vdma_ctrl_read(chan, XILINX_VDMA_REG_DMASR) &
+		    XILINX_VDMA_DMASR_HALTED)
+			break;
+	} while (loop--);
+
+	if (!loop) {
+		dev_err(chan->dev, "Cannot stop channel %p: %x\n",
+			chan, vdma_ctrl_read(chan, XILINX_VDMA_REG_DMASR));
+		chan->err = true;
+	}
+
+	return;
+}
+
+/**
+ * xilinx_vdma_start - Start VDMA channel
+ * @chan: Driver specific VDMA channel
+ */
+static void xilinx_vdma_start(struct xilinx_vdma_chan *chan)
+{
+	int loop = XILINX_VDMA_LOOP_COUNT;
+
+	vdma_ctrl_set(chan, XILINX_VDMA_REG_DMACR, XILINX_VDMA_DMACR_RUNSTOP);
+
+	/* Wait for the hardware to start */
+	do {
+		if (!(vdma_ctrl_read(chan, XILINX_VDMA_REG_DMASR) &
+		      XILINX_VDMA_DMASR_HALTED))
+			break;
+	} while (loop--);
+
+	if (!loop) {
+		dev_err(chan->dev, "Cannot start channel %p: %x\n",
+			chan, vdma_ctrl_read(chan, XILINX_VDMA_REG_DMASR));
+
+		chan->err = true;
+	}
+
+	return;
+}
+
+/**
+ * xilinx_vdma_start_transfer - Starts VDMA transfer
+ * @chan: Driver specific channel struct pointer
+ */
+static void xilinx_vdma_start_transfer(struct xilinx_vdma_chan *chan)
+{
+	struct xilinx_vdma_config *config = &chan->config;
+	struct xilinx_vdma_tx_descriptor *desc;
+	unsigned long flags;
+	u32 reg;
+	struct xilinx_vdma_tx_segment *head, *tail = NULL;
+
+	if (chan->err)
+		return;
+
+	spin_lock_irqsave(&chan->lock, flags);
+
+	/* There's already an active descriptor, bail out. */
+	if (chan->active_desc)
+		goto out_unlock;
+
+	if (list_empty(&chan->pending_list))
+		goto out_unlock;
+
+	desc = list_first_entry(&chan->pending_list,
+				struct xilinx_vdma_tx_descriptor, node);
+
+	/* If it is SG mode and hardware is busy, cannot submit */
+	if (chan->has_sg && xilinx_vdma_is_running(chan) &&
+	    !xilinx_vdma_is_idle(chan)) {
+		dev_dbg(chan->dev, "DMA controller still busy\n");
+		goto out_unlock;
+	}
+
+	/*
+	 * If hardware is idle, then all descriptors on the running lists are
+	 * done, start new transfers
+	 */
+	if (chan->has_sg) {
+		head = list_first_entry(&desc->segments,
+					struct xilinx_vdma_tx_segment, node);
+		tail = list_entry(desc->segments.prev,
+				  struct xilinx_vdma_tx_segment, node);
+
+		vdma_ctrl_write(chan, XILINX_VDMA_REG_CURDESC, head->phys);
+	}
+
+	/* Configure the hardware using info in the config structure */
+	reg = vdma_ctrl_read(chan, XILINX_VDMA_REG_DMACR);
+
+	if (config->frm_cnt_en)
+		reg |= XILINX_VDMA_DMACR_FRAMECNT_EN;
+	else
+		reg &= ~XILINX_VDMA_DMACR_FRAMECNT_EN;
+
+	/*
+	 * With SG, start with circular mode, so that BDs can be fetched.
+	 * In direct register mode, if not parking, enable circular mode
+	 */
+	if (chan->has_sg || !config->park)
+		reg |= XILINX_VDMA_DMACR_CIRC_EN;
+
+	if (config->park)
+		reg &= ~XILINX_VDMA_DMACR_CIRC_EN;
+
+	vdma_ctrl_write(chan, XILINX_VDMA_REG_DMACR, reg);
+
+	if (config->park && (config->park_frm >= 0) &&
+			(config->park_frm < chan->num_frms)) {
+		if (chan->direction == DMA_MEM_TO_DEV)
+			vdma_write(chan, XILINX_VDMA_REG_PARK_PTR,
+				config->park_frm <<
+					XILINX_VDMA_PARK_PTR_RD_REF_SHIFT);
+		else
+			vdma_write(chan, XILINX_VDMA_REG_PARK_PTR,
+				config->park_frm <<
+					XILINX_VDMA_PARK_PTR_WR_REF_SHIFT);
+	}
+
+	/* Start the hardware */
+	xilinx_vdma_start(chan);
+
+	if (chan->err)
+		goto out_unlock;
+
+	/* Start the transfer */
+	if (chan->has_sg) {
+		vdma_ctrl_write(chan, XILINX_VDMA_REG_TAILDESC, tail->phys);
+	} else {
+		struct xilinx_vdma_tx_segment *segment, *last = NULL;
+		int i = 0;
+
+		list_for_each_entry(segment, &desc->segments, node) {
+			vdma_desc_write(chan,
+					XILINX_VDMA_REG_START_ADDRESS(i++),
+					segment->hw.buf_addr);
+			last = segment;
+		}
+
+		if (!last)
+			goto out_unlock;
+
+		/* HW expects these parameters to be same for one transaction */
+		vdma_desc_write(chan, XILINX_VDMA_REG_HSIZE, last->hw.hsize);
+		vdma_desc_write(chan, XILINX_VDMA_REG_FRMDLY_STRIDE,
+				last->hw.stride);
+		vdma_desc_write(chan, XILINX_VDMA_REG_VSIZE, last->hw.vsize);
+	}
+
+	list_del(&desc->node);
+	chan->active_desc = desc;
+
+out_unlock:
+	spin_unlock_irqrestore(&chan->lock, flags);
+}
+
+/**
+ * xilinx_vdma_issue_pending - Issue pending transactions
+ * @dchan: DMA channel
+ */
+static void xilinx_vdma_issue_pending(struct dma_chan *dchan)
+{
+	struct xilinx_vdma_chan *chan = to_xilinx_chan(dchan);
+
+	xilinx_vdma_start_transfer(chan);
+}
+
+/**
+ * xilinx_vdma_complete_descriptor - Mark the active descriptor as complete
+ * @chan : xilinx DMA channel
+ *
+ * CONTEXT: hardirq
+ */
+static void xilinx_vdma_complete_descriptor(struct xilinx_vdma_chan *chan)
+{
+	struct xilinx_vdma_tx_descriptor *desc;
+	unsigned long flags;
+
+	spin_lock_irqsave(&chan->lock, flags);
+
+	desc = chan->active_desc;
+	if (!desc) {
+		dev_dbg(chan->dev, "no running descriptors\n");
+		goto out_unlock;
+	}
+
+	dma_cookie_complete(&desc->async_tx);
+	list_add_tail(&desc->node, &chan->done_list);
+
+	chan->active_desc = NULL;
+
+out_unlock:
+	spin_unlock_irqrestore(&chan->lock, flags);
+}
+
+/**
+ * xilinx_vdma_reset - Reset VDMA channel
+ * @chan: Driver specific VDMA channel
+ *
+ * Return: '0' on success and failure value on error
+ */
+static int xilinx_vdma_reset(struct xilinx_vdma_chan *chan)
+{
+	int loop = XILINX_VDMA_LOOP_COUNT;
+	u32 tmp;
+
+	vdma_ctrl_set(chan, XILINX_VDMA_REG_DMACR, XILINX_VDMA_DMACR_RESET);
+
+	tmp = vdma_ctrl_read(chan, XILINX_VDMA_REG_DMACR) &
+		XILINX_VDMA_DMACR_RESET;
+
+	/* Wait for the hardware to finish reset */
+	do {
+		tmp = vdma_ctrl_read(chan, XILINX_VDMA_REG_DMACR) &
+			XILINX_VDMA_DMACR_RESET;
+	} while (loop-- && tmp);
+
+	if (!loop) {
+		dev_err(chan->dev, "reset timeout, cr %x, sr %x\n",
+			vdma_ctrl_read(chan, XILINX_VDMA_REG_DMACR),
+			vdma_ctrl_read(chan, XILINX_VDMA_REG_DMASR));
+		return -ETIMEDOUT;
+	}
+
+	chan->err = false;
+
+	return 0;
+}
+
+/**
+ * xilinx_vdma_chan_reset - Reset VDMA channel and enable interrupts
+ * @chan: Driver specific VDMA channel
+ *
+ * Return: '0' on success and failure value on error
+ */
+static int xilinx_vdma_chan_reset(struct xilinx_vdma_chan *chan)
+{
+	int err;
+
+	/* Reset VDMA */
+	err = xilinx_vdma_reset(chan);
+	if (err)
+		return err;
+
+	/* Enable interrupts */
+	vdma_ctrl_set(chan, XILINX_VDMA_REG_DMACR,
+		      XILINX_VDMA_DMAXR_ALL_IRQ_MASK);
+
+	return 0;
+}
+
+/**
+ * xilinx_vdma_irq_handler - VDMA Interrupt handler
+ * @irq: IRQ number
+ * @data: Pointer to the Xilinx VDMA channel structure
+ *
+ * Return: IRQ_HANDLED/IRQ_NONE
+ */
+static irqreturn_t xilinx_vdma_irq_handler(int irq, void *data)
+{
+	struct xilinx_vdma_chan *chan = data;
+	u32 status;
+
+	/* Read the status and ack the interrupts. */
+	status = vdma_ctrl_read(chan, XILINX_VDMA_REG_DMASR);
+	if (!(status & XILINX_VDMA_DMAXR_ALL_IRQ_MASK))
+		return IRQ_NONE;
+
+	vdma_ctrl_write(chan, XILINX_VDMA_REG_DMASR,
+			status & XILINX_VDMA_DMAXR_ALL_IRQ_MASK);
+
+	if (status & XILINX_VDMA_DMASR_ERR_IRQ) {
+		/*
+		 * An error occurred. If C_FLUSH_ON_FSYNC is enabled and the
+		 * error is recoverable, ignore it. Otherwise flag the error.
+		 *
+		 * Only recoverable errors can be cleared in the DMASR register,
+		 * make sure not to write to other error bits to 1.
+		 */
+		u32 errors = status & XILINX_VDMA_DMASR_ALL_ERR_MASK;
+		vdma_ctrl_write(chan, XILINX_VDMA_REG_DMASR,
+				errors & XILINX_VDMA_DMASR_ERR_RECOVER_MASK);
+
+		if (!chan->flush_on_fsync ||
+		    (errors & ~XILINX_VDMA_DMASR_ERR_RECOVER_MASK)) {
+			dev_err(chan->dev,
+				"Channel %p has errors %x, cdr %x tdr %x\n",
+				chan, errors,
+				vdma_ctrl_read(chan, XILINX_VDMA_REG_CURDESC),
+				vdma_ctrl_read(chan, XILINX_VDMA_REG_TAILDESC));
+			chan->err = true;
+		}
+	}
+
+	if (status & XILINX_VDMA_DMASR_DLY_CNT_IRQ) {
+		/*
+		 * Device takes too long to do the transfer when user requires
+		 * responsiveness.
+		 */
+		dev_dbg(chan->dev, "Inter-packet latency too long\n");
+	}
+
+	if (status & XILINX_VDMA_DMASR_FRM_CNT_IRQ) {
+		xilinx_vdma_complete_descriptor(chan);
+		xilinx_vdma_start_transfer(chan);
+	}
+
+	tasklet_schedule(&chan->tasklet);
+	return IRQ_HANDLED;
+}
+
+/**
+ * xilinx_vdma_tx_submit - Submit DMA transaction
+ * @tx: Async transaction descriptor
+ *
+ * Return: cookie value on success and failure value on error
+ */
+static dma_cookie_t xilinx_vdma_tx_submit(struct dma_async_tx_descriptor *tx)
+{
+	struct xilinx_vdma_tx_descriptor *desc = to_vdma_tx_descriptor(tx);
+	struct xilinx_vdma_chan *chan = to_xilinx_chan(tx->chan);
+	dma_cookie_t cookie;
+	unsigned long flags;
+	int err;
+
+	if (chan->err) {
+		/*
+		 * If reset fails, need to hard reset the system.
+		 * Channel is no longer functional
+		 */
+		err = xilinx_vdma_chan_reset(chan);
+		if (err < 0)
+			return err;
+	}
+
+	spin_lock_irqsave(&chan->lock, flags);
+
+	cookie = dma_cookie_assign(tx);
+
+	/* Append the transaction to the pending transactions queue. */
+	list_add_tail(&desc->node, &chan->pending_list);
+
+	/* Free the allocated desc */
+	chan->allocated_desc = NULL;
+
+	spin_unlock_irqrestore(&chan->lock, flags);
+
+	return cookie;
+}
+
+/**
+ * xilinx_vdma_dma_prep_interleaved - prepare a descriptor for a
+ *	DMA_SLAVE transaction
+ * @dchan: DMA channel
+ * @xt: Interleaved template pointer
+ * @flags: transfer ack flags
+ *
+ * Return: Async transaction descriptor on success and NULL on failure
+ */
+static struct dma_async_tx_descriptor *
+xilinx_vdma_dma_prep_interleaved(struct dma_chan *dchan,
+				 struct dma_interleaved_template *xt,
+				 unsigned long flags)
+{
+	struct xilinx_vdma_chan *chan = to_xilinx_chan(dchan);
+	struct xilinx_vdma_tx_descriptor *desc;
+	struct xilinx_vdma_tx_segment *segment, *prev = NULL;
+	struct xilinx_vdma_desc_hw *hw;
+
+	if (!is_slave_direction(xt->dir))
+		return NULL;
+
+	if (!xt->numf || !xt->sgl[0].size)
+		return NULL;
+
+	/* Allocate a transaction descriptor. */
+	desc = xilinx_vdma_alloc_tx_descriptor(chan);
+	if (!desc)
+		return NULL;
+
+	dma_async_tx_descriptor_init(&desc->async_tx, &chan->common);
+	desc->async_tx.tx_submit = xilinx_vdma_tx_submit;
+	async_tx_ack(&desc->async_tx);
+
+	/* Allocate the link descriptor from DMA pool */
+	segment = xilinx_vdma_alloc_tx_segment(chan);
+	if (!segment)
+		goto error;
+
+	/* Fill in the hardware descriptor */
+	hw = &segment->hw;
+	hw->vsize = xt->numf;
+	hw->hsize = xt->sgl[0].size;
+	hw->stride = xt->sgl[0].icg <<
+			XILINX_VDMA_FRMDLY_STRIDE_STRIDE_SHIFT;
+	hw->stride |= chan->config.frm_dly <<
+			XILINX_VDMA_FRMDLY_STRIDE_FRMDLY_SHIFT;
+
+	if (xt->dir != DMA_MEM_TO_DEV)
+		hw->buf_addr = xt->dst_start;
+	else
+		hw->buf_addr = xt->src_start;
+
+	/* Link the previous next descriptor to current */
+	prev = list_last_entry(&desc->segments,
+				struct xilinx_vdma_tx_segment, node);
+	prev->hw.next_desc = segment->phys;
+
+	/* Insert the segment into the descriptor segments list. */
+	list_add_tail(&segment->node, &desc->segments);
+
+	prev = segment;
+
+	/* Link the last hardware descriptor with the first. */
+	segment = list_first_entry(&desc->segments,
+				   struct xilinx_vdma_tx_segment, node);
+	prev->hw.next_desc = segment->phys;
+
+	return &desc->async_tx;
+
+error:
+	xilinx_vdma_free_tx_descriptor(chan, desc);
+	return NULL;
+}
+
+/**
+ * xilinx_vdma_terminate_all - Halt the channel and free descriptors
+ * @chan: Driver specific VDMA Channel pointer
+ */
+static void xilinx_vdma_terminate_all(struct xilinx_vdma_chan *chan)
+{
+	/* Halt the DMA engine */
+	xilinx_vdma_halt(chan);
+
+	/* Remove and free all of the descriptors in the lists */
+	xilinx_vdma_free_descriptors(chan);
+}
+
+/**
+ * xilinx_vdma_channel_set_config - Configure VDMA channel
+ * Run-time configuration for Axi VDMA, supports:
+ * . halt the channel
+ * . configure interrupt coalescing and inter-packet delay threshold
+ * . start/stop parking
+ * . enable genlock
+ *
+ * @dchan: DMA channel
+ * @cfg: VDMA device configuration pointer
+ *
+ * Return: '0' on success and failure value on error
+ */
+int xilinx_vdma_channel_set_config(struct dma_chan *dchan,
+					struct xilinx_vdma_config *cfg)
+{
+	struct xilinx_vdma_chan *chan = to_xilinx_chan(dchan);
+	u32 dmacr;
+
+	if (cfg->reset)
+		return xilinx_vdma_chan_reset(chan);
+
+	dmacr = vdma_ctrl_read(chan, XILINX_VDMA_REG_DMACR);
+
+	chan->config.frm_dly = cfg->frm_dly;
+	chan->config.park = cfg->park;
+
+	/* genlock settings */
+	chan->config.gen_lock = cfg->gen_lock;
+	chan->config.master = cfg->master;
+
+	if (cfg->gen_lock && chan->genlock) {
+		dmacr |= XILINX_VDMA_DMACR_GENLOCK_EN;
+		dmacr |= cfg->master << XILINX_VDMA_DMACR_MASTER_SHIFT;
+	}
+
+	chan->config.frm_cnt_en = cfg->frm_cnt_en;
+	if (cfg->park)
+		chan->config.park_frm = cfg->park_frm;
+	else
+		chan->config.park_frm = -1;
+
+	chan->config.coalesc = cfg->coalesc;
+	chan->config.delay = cfg->delay;
+
+	if (cfg->coalesc <= XILINX_VDMA_DMACR_FRAME_COUNT_MAX) {
+		dmacr |= cfg->coalesc << XILINX_VDMA_DMACR_FRAME_COUNT_SHIFT;
+		chan->config.coalesc = cfg->coalesc;
+	}
+
+	if (cfg->delay <= XILINX_VDMA_DMACR_DELAY_MAX) {
+		dmacr |= cfg->delay << XILINX_VDMA_DMACR_DELAY_SHIFT;
+		chan->config.delay = cfg->delay;
+	}
+
+	/* FSync Source selection */
+	dmacr &= ~XILINX_VDMA_DMACR_FSYNCSRC_MASK;
+	dmacr |= cfg->ext_fsync << XILINX_VDMA_DMACR_FSYNCSRC_SHIFT;
+
+	vdma_ctrl_write(chan, XILINX_VDMA_REG_DMACR, dmacr);
+
+	return 0;
+}
+EXPORT_SYMBOL(xilinx_vdma_channel_set_config);
+
+/**
+ * xilinx_vdma_device_control - Configure DMA channel of the device
+ * @dchan: DMA Channel pointer
+ * @cmd: DMA control command
+ * @arg: Channel configuration
+ *
+ * Return: '0' on success and failure value on error
+ */
+static int xilinx_vdma_device_control(struct dma_chan *dchan,
+				      enum dma_ctrl_cmd cmd, unsigned long arg)
+{
+	struct xilinx_vdma_chan *chan = to_xilinx_chan(dchan);
+
+	if (cmd != DMA_TERMINATE_ALL)
+		return -ENXIO;
+
+	xilinx_vdma_terminate_all(chan);
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * Probe and remove
+ */
+
+/**
+ * xilinx_vdma_chan_remove - Per Channel remove function
+ * @chan: Driver specific VDMA channel
+ */
+static void xilinx_vdma_chan_remove(struct xilinx_vdma_chan *chan)
+{
+	/* Disable all interrupts */
+	vdma_ctrl_clr(chan, XILINX_VDMA_REG_DMACR,
+		      XILINX_VDMA_DMAXR_ALL_IRQ_MASK);
+
+	if (chan->irq > 0)
+		free_irq(chan->irq, chan);
+
+	tasklet_kill(&chan->tasklet);
+
+	list_del(&chan->common.device_node);
+}
+
+/**
+ * xilinx_vdma_chan_probe - Per Channel Probing
+ * It get channel features from the device tree entry and
+ * initialize special channel handling routines
+ *
+ * @xdev: Driver specific device structure
+ * @node: Device node
+ *
+ * Return: '0' on success and failure value on error
+ */
+static int xilinx_vdma_chan_probe(struct xilinx_vdma_device *xdev,
+				  struct device_node *node)
+{
+	struct xilinx_vdma_chan *chan;
+	bool has_dre = false;
+	u32 value, width;
+	int err;
+
+	/* Allocate and initialize the channel structure */
+	chan = devm_kzalloc(xdev->dev, sizeof(*chan), GFP_KERNEL);
+	if (!chan)
+		return -ENOMEM;
+
+	chan->dev = xdev->dev;
+	chan->xdev = xdev;
+	chan->has_sg = xdev->has_sg;
+
+	spin_lock_init(&chan->lock);
+	INIT_LIST_HEAD(&chan->pending_list);
+	INIT_LIST_HEAD(&chan->done_list);
+
+	/* Retrieve the channel properties from the device tree */
+	has_dre = of_property_read_bool(node, "xlnx,include-dre");
+
+	chan->genlock = of_property_read_bool(node, "xlnx,genlock-mode");
+
+	err = of_property_read_u32(node, "xlnx,datawidth", &value);
+	if (err) {
+		dev_err(xdev->dev, "missing xlnx,datawidth property\n");
+		return err;
+	}
+	width = value >> 3; /* Convert bits to bytes */
+
+	/* If data width is greater than 8 bytes, DRE is not in hw */
+	if (width > 8)
+		has_dre = false;
+
+	if (!has_dre)
+		xdev->common.copy_align = fls(width - 1);
+
+	if (of_device_is_compatible(node, "xlnx,axi-vdma-mm2s-channel")) {
+		chan->direction = DMA_MEM_TO_DEV;
+		chan->id = 0;
+
+		chan->ctrl_offset = XILINX_VDMA_MM2S_CTRL_OFFSET;
+		chan->desc_offset = XILINX_VDMA_MM2S_DESC_OFFSET;
+
+		if (xdev->flush_on_fsync == XILINX_VDMA_FLUSH_BOTH ||
+		    xdev->flush_on_fsync == XILINX_VDMA_FLUSH_MM2S)
+			chan->flush_on_fsync = true;
+	} else if (of_device_is_compatible(node,
+					    "xlnx,axi-vdma-s2mm-channel")) {
+		chan->direction = DMA_DEV_TO_MEM;
+		chan->id = 1;
+
+		chan->ctrl_offset = XILINX_VDMA_S2MM_CTRL_OFFSET;
+		chan->desc_offset = XILINX_VDMA_S2MM_DESC_OFFSET;
+
+		if (xdev->flush_on_fsync == XILINX_VDMA_FLUSH_BOTH ||
+		    xdev->flush_on_fsync == XILINX_VDMA_FLUSH_S2MM)
+			chan->flush_on_fsync = true;
+	} else {
+		dev_err(xdev->dev, "Invalid channel compatible node\n");
+		return -EINVAL;
+	}
+
+	/* Request the interrupt */
+	chan->irq = irq_of_parse_and_map(node, 0);
+	err = request_irq(chan->irq, xilinx_vdma_irq_handler, IRQF_SHARED,
+			  "xilinx-vdma-controller", chan);
+	if (err) {
+		dev_err(xdev->dev, "unable to request IRQ %d\n", chan->irq);
+		return err;
+	}
+
+	/* Initialize the tasklet */
+	tasklet_init(&chan->tasklet, xilinx_vdma_do_tasklet,
+			(unsigned long)chan);
+
+	/*
+	 * Initialize the DMA channel and add it to the DMA engine channels
+	 * list.
+	 */
+	chan->common.device = &xdev->common;
+
+	list_add_tail(&chan->common.device_node, &xdev->common.channels);
+	xdev->chan[chan->id] = chan;
+
+	/* Reset the channel */
+	err = xilinx_vdma_chan_reset(chan);
+	if (err < 0) {
+		dev_err(xdev->dev, "Reset channel failed\n");
+		return err;
+	}
+
+	return 0;
+}
+
+/**
+ * of_dma_xilinx_xlate - Translation function
+ * @dma_spec: Pointer to DMA specifier as found in the device tree
+ * @ofdma: Pointer to DMA controller data
+ *
+ * Return: DMA channel pointer on success and NULL on error
+ */
+static struct dma_chan *of_dma_xilinx_xlate(struct of_phandle_args *dma_spec,
+						struct of_dma *ofdma)
+{
+	struct xilinx_vdma_device *xdev = ofdma->of_dma_data;
+	int chan_id = dma_spec->args[0];
+
+	if (chan_id >= XILINX_VDMA_MAX_CHANS_PER_DEVICE)
+		return NULL;
+
+	return dma_get_slave_channel(&xdev->chan[chan_id]->common);
+}
+
+/**
+ * xilinx_vdma_probe - Driver probe function
+ * @pdev: Pointer to the platform_device structure
+ *
+ * Return: '0' on success and failure value on error
+ */
+static int xilinx_vdma_probe(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct xilinx_vdma_device *xdev;
+	struct device_node *child;
+	struct resource *io;
+	u32 num_frames;
+	int i, err;
+
+	/* Allocate and initialize the DMA engine structure */
+	xdev = devm_kzalloc(&pdev->dev, sizeof(*xdev), GFP_KERNEL);
+	if (!xdev)
+		return -ENOMEM;
+
+	xdev->dev = &pdev->dev;
+
+	/* Request and map I/O memory */
+	io = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	xdev->regs = devm_ioremap_resource(&pdev->dev, io);
+	if (IS_ERR(xdev->regs))
+		return PTR_ERR(xdev->regs);
+
+	/* Retrieve the DMA engine properties from the device tree */
+	xdev->has_sg = of_property_read_bool(node, "xlnx,include-sg");
+
+	err = of_property_read_u32(node, "xlnx,num-fstores", &num_frames);
+	if (err < 0) {
+		dev_err(xdev->dev, "missing xlnx,num-fstores property\n");
+		return err;
+	}
+
+	err = of_property_read_u32(node, "xlnx,flush-fsync",
+					&xdev->flush_on_fsync);
+	if (err < 0)
+		dev_warn(xdev->dev, "missing xlnx,flush-fsync property\n");
+
+	/* Initialize the DMA engine */
+	xdev->common.dev = &pdev->dev;
+
+	INIT_LIST_HEAD(&xdev->common.channels);
+	dma_cap_set(DMA_SLAVE, xdev->common.cap_mask);
+	dma_cap_set(DMA_PRIVATE, xdev->common.cap_mask);
+
+	xdev->common.device_alloc_chan_resources =
+				xilinx_vdma_alloc_chan_resources;
+	xdev->common.device_free_chan_resources =
+				xilinx_vdma_free_chan_resources;
+	xdev->common.device_prep_interleaved_dma =
+				xilinx_vdma_dma_prep_interleaved;
+	xdev->common.device_control = xilinx_vdma_device_control;
+	xdev->common.device_tx_status = xilinx_vdma_tx_status;
+	xdev->common.device_issue_pending = xilinx_vdma_issue_pending;
+
+	platform_set_drvdata(pdev, xdev);
+
+	/* Initialize the channels */
+	for_each_child_of_node(node, child) {
+		err = xilinx_vdma_chan_probe(xdev, child);
+		if (err < 0)
+			goto error;
+	}
+
+	for (i = 0; i < XILINX_VDMA_MAX_CHANS_PER_DEVICE; i++)
+		if (xdev->chan[i])
+			xdev->chan[i]->num_frms = num_frames;
+
+	/* Register the DMA engine with the core */
+	dma_async_device_register(&xdev->common);
+
+	err = of_dma_controller_register(node, of_dma_xilinx_xlate,
+					 xdev);
+	if (err < 0) {
+		dev_err(&pdev->dev, "Unable to register DMA to DT\n");
+		dma_async_device_unregister(&xdev->common);
+		goto error;
+	}
+
+	dev_info(&pdev->dev, "Xilinx AXI VDMA Engine Driver Probed!!\n");
+
+	return 0;
+
+error:
+	for (i = 0; i < XILINX_VDMA_MAX_CHANS_PER_DEVICE; i++)
+		if (xdev->chan[i])
+			xilinx_vdma_chan_remove(xdev->chan[i]);
+
+	return err;
+}
+
+/**
+ * xilinx_vdma_remove - Driver remove function
+ * @pdev: Pointer to the platform_device structure
+ *
+ * Return: Always '0'
+ */
+static int xilinx_vdma_remove(struct platform_device *pdev)
+{
+	struct xilinx_vdma_device *xdev = platform_get_drvdata(pdev);
+	int i;
+
+	of_dma_controller_free(pdev->dev.of_node);
+
+	dma_async_device_unregister(&xdev->common);
+
+	for (i = 0; i < XILINX_VDMA_MAX_CHANS_PER_DEVICE; i++)
+		if (xdev->chan[i])
+			xilinx_vdma_chan_remove(xdev->chan[i]);
+
+	return 0;
+}
+
+static const struct of_device_id xilinx_vdma_of_ids[] = {
+	{ .compatible = "xlnx,axi-vdma-1.00.a",},
+	{}
+};
+
+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,
+	.remove = xilinx_vdma_remove,
+};
+
+module_platform_driver(xilinx_vdma_driver);
+
+MODULE_AUTHOR("Xilinx, Inc.");
+MODULE_DESCRIPTION("Xilinx VDMA driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/edac/edac_mc.c b/drivers/edac/edac_mc.c
index 33edd676634..2c694b5297c 100644
--- a/drivers/edac/edac_mc.c
+++ b/drivers/edac/edac_mc.c
@@ -1018,7 +1018,7 @@ static void edac_ce_error(struct mem_ctl_info *mci,
 	}
 	edac_inc_ce_error(mci, enable_per_layer_report, pos, error_count);
 
-	if (mci->scrub_mode & SCRUB_SW_SRC) {
+	if (mci->scrub_mode == SCRUB_SW_SRC) {
 		/*
 			* Some memory controllers (called MCs below) can remap
 			* memory so that it is still available at a different
diff --git a/drivers/edac/i82875p_edac.c b/drivers/edac/i82875p_edac.c
index 8d0450b9b9a..64b68320249 100644
--- a/drivers/edac/i82875p_edac.c
+++ b/drivers/edac/i82875p_edac.c
@@ -275,7 +275,6 @@ static int i82875p_setup_overfl_dev(struct pci_dev *pdev,
 {
 	struct pci_dev *dev;
 	void __iomem *window;
-	int err;
 
 	*ovrfl_pdev = NULL;
 	*ovrfl_window = NULL;
@@ -293,13 +292,8 @@ static int i82875p_setup_overfl_dev(struct pci_dev *pdev,
 		if (dev == NULL)
 			return 1;
 
-		err = pci_bus_add_device(dev);
-		if (err) {
-			i82875p_printk(KERN_ERR,
-				"%s(): pci_bus_add_device() Failed\n",
-				__func__);
-		}
 		pci_bus_assign_resources(dev->bus);
+		pci_bus_add_device(dev);
 	}
 
 	*ovrfl_pdev = dev;
diff --git a/drivers/edac/mce_amd.c b/drivers/edac/mce_amd.c
index 51b9caa0b02..5f43620d580 100644
--- a/drivers/edac/mce_amd.c
+++ b/drivers/edac/mce_amd.c
@@ -6,7 +6,6 @@
 static struct amd_decoder_ops *fam_ops;
 
 static u8 xec_mask	 = 0xf;
-static u8 nb_err_cpumask = 0xf;
 
 static bool report_gart_errors;
 static void (*nb_bus_decoder)(int node_id, struct mce *m);
@@ -852,7 +851,6 @@ static int __init mce_amd_init(void)
 		break;
 
 	case 0x14:
-		nb_err_cpumask  = 0x3;
 		fam_ops->mc0_mce = cat_mc0_mce;
 		fam_ops->mc1_mce = cat_mc1_mce;
 		fam_ops->mc2_mce = k8_mc2_mce;
diff --git a/drivers/extcon/Kconfig b/drivers/extcon/Kconfig
index be56e8ac95e..aebde489c29 100644
--- a/drivers/extcon/Kconfig
+++ b/drivers/extcon/Kconfig
@@ -28,13 +28,13 @@ config EXTCON_ADC_JACK
 	  Say Y here to enable extcon device driver based on ADC values.
 
 config EXTCON_MAX14577
-	tristate "MAX14577 EXTCON Support"
+	tristate "MAX14577/77836 EXTCON Support"
 	depends on MFD_MAX14577
 	select IRQ_DOMAIN
 	select REGMAP_I2C
 	help
 	  If you say yes here you get support for the MUIC device of
-	  Maxim MAX14577 PMIC. The MAX14577 MUIC is a USB port accessory
+	  Maxim MAX14577/77836. The MAX14577/77836 MUIC is a USB port accessory
 	  detector and switch.
 
 config EXTCON_MAX77693
diff --git a/drivers/extcon/extcon-adc-jack.c b/drivers/extcon/extcon-adc-jack.c
index e23f1c2e505..e18f95be373 100644
--- a/drivers/extcon/extcon-adc-jack.c
+++ b/drivers/extcon/extcon-adc-jack.c
@@ -39,7 +39,7 @@
  * @chan:		iio channel being queried.
  */
 struct adc_jack_data {
-	struct extcon_dev edev;
+	struct extcon_dev *edev;
 
 	const char **cable_names;
 	int num_cables;
@@ -64,7 +64,7 @@ static void adc_jack_handler(struct work_struct *work)
 
 	ret = iio_read_channel_raw(data->chan, &adc_val);
 	if (ret < 0) {
-		dev_err(&data->edev.dev, "read channel() error: %d\n", ret);
+		dev_err(&data->edev->dev, "read channel() error: %d\n", ret);
 		return;
 	}
 
@@ -80,7 +80,7 @@ static void adc_jack_handler(struct work_struct *work)
 	}
 	/* if no def has met, it means state = 0 (no cables attached) */
 
-	extcon_set_state(&data->edev, state);
+	extcon_set_state(data->edev, state);
 }
 
 static irqreturn_t adc_jack_irq_thread(int irq, void *_data)
@@ -102,33 +102,33 @@ static int adc_jack_probe(struct platform_device *pdev)
 	if (!data)
 		return -ENOMEM;
 
-	data->edev.name = pdata->name;
-
 	if (!pdata->cable_names) {
-		err = -EINVAL;
 		dev_err(&pdev->dev, "error: cable_names not defined.\n");
-		goto out;
+		return -EINVAL;
 	}
 
-	data->edev.dev.parent = &pdev->dev;
-	data->edev.supported_cable = pdata->cable_names;
+	data->edev = devm_extcon_dev_allocate(&pdev->dev, pdata->cable_names);
+	if (IS_ERR(data->edev)) {
+		dev_err(&pdev->dev, "failed to allocate extcon device\n");
+		return -ENOMEM;
+	}
+	data->edev->dev.parent = &pdev->dev;
+	data->edev->name = pdata->name;
 
 	/* Check the length of array and set num_cables */
-	for (i = 0; data->edev.supported_cable[i]; i++)
+	for (i = 0; data->edev->supported_cable[i]; i++)
 		;
 	if (i == 0 || i > SUPPORTED_CABLE_MAX) {
-		err = -EINVAL;
 		dev_err(&pdev->dev, "error: pdata->cable_names size = %d\n",
 				i - 1);
-		goto out;
+		return -EINVAL;
 	}
 	data->num_cables = i;
 
 	if (!pdata->adc_conditions ||
 			!pdata->adc_conditions[0].state) {
-		err = -EINVAL;
 		dev_err(&pdev->dev, "error: adc_conditions not defined.\n");
-		goto out;
+		return -EINVAL;
 	}
 	data->adc_conditions = pdata->adc_conditions;
 
@@ -138,10 +138,8 @@ static int adc_jack_probe(struct platform_device *pdev)
 	data->num_conditions = i;
 
 	data->chan = iio_channel_get(&pdev->dev, pdata->consumer_channel);
-	if (IS_ERR(data->chan)) {
-		err = PTR_ERR(data->chan);
-		goto out;
-	}
+	if (IS_ERR(data->chan))
+		return PTR_ERR(data->chan);
 
 	data->handling_delay = msecs_to_jiffies(pdata->handling_delay_ms);
 
@@ -149,15 +147,14 @@ static int adc_jack_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, data);
 
-	err = extcon_dev_register(&data->edev);
+	err = devm_extcon_dev_register(&pdev->dev, data->edev);
 	if (err)
-		goto out;
+		return err;
 
 	data->irq = platform_get_irq(pdev, 0);
 	if (!data->irq) {
 		dev_err(&pdev->dev, "platform_get_irq failed\n");
-		err = -ENODEV;
-		goto err_irq;
+		return -ENODEV;
 	}
 
 	err = request_any_context_irq(data->irq, adc_jack_irq_thread,
@@ -165,15 +162,10 @@ static int adc_jack_probe(struct platform_device *pdev)
 
 	if (err < 0) {
 		dev_err(&pdev->dev, "error: irq %d\n", data->irq);
-		goto err_irq;
+		return err;
 	}
 
 	return 0;
-
-err_irq:
-	extcon_dev_unregister(&data->edev);
-out:
-	return err;
 }
 
 static int adc_jack_remove(struct platform_device *pdev)
@@ -182,7 +174,6 @@ static int adc_jack_remove(struct platform_device *pdev)
 
 	free_irq(data->irq, data);
 	cancel_work_sync(&data->handler.work);
-	extcon_dev_unregister(&data->edev);
 
 	return 0;
 }
diff --git a/drivers/extcon/extcon-arizona.c b/drivers/extcon/extcon-arizona.c
index 98a14f6143a..6c84e3d1204 100644
--- a/drivers/extcon/extcon-arizona.c
+++ b/drivers/extcon/extcon-arizona.c
@@ -91,7 +91,7 @@ struct arizona_extcon_info {
 
 	int hpdet_ip;
 
-	struct extcon_dev edev;
+	struct extcon_dev *edev;
 };
 
 static const struct arizona_micd_config micd_default_modes[] = {
@@ -546,7 +546,7 @@ static irqreturn_t arizona_hpdet_irq(int irq, void *data)
 	}
 
 	/* If the cable was removed while measuring ignore the result */
-	ret = extcon_get_cable_state_(&info->edev, ARIZONA_CABLE_MECHANICAL);
+	ret = extcon_get_cable_state_(info->edev, ARIZONA_CABLE_MECHANICAL);
 	if (ret < 0) {
 		dev_err(arizona->dev, "Failed to check cable state: %d\n",
 			ret);
@@ -581,7 +581,7 @@ static irqreturn_t arizona_hpdet_irq(int irq, void *data)
 	else
 		report = ARIZONA_CABLE_HEADPHONE;
 
-	ret = extcon_set_cable_state_(&info->edev, report, true);
+	ret = extcon_set_cable_state_(info->edev, report, true);
 	if (ret != 0)
 		dev_err(arizona->dev, "Failed to report HP/line: %d\n",
 			ret);
@@ -664,7 +664,7 @@ err:
 			   ARIZONA_ACCDET_MODE_MASK, ARIZONA_ACCDET_MODE_MIC);
 
 	/* Just report headphone */
-	ret = extcon_update_state(&info->edev,
+	ret = extcon_update_state(info->edev,
 				  1 << ARIZONA_CABLE_HEADPHONE,
 				  1 << ARIZONA_CABLE_HEADPHONE);
 	if (ret != 0)
@@ -723,7 +723,7 @@ err:
 			   ARIZONA_ACCDET_MODE_MASK, ARIZONA_ACCDET_MODE_MIC);
 
 	/* Just report headphone */
-	ret = extcon_update_state(&info->edev,
+	ret = extcon_update_state(info->edev,
 				  1 << ARIZONA_CABLE_HEADPHONE,
 				  1 << ARIZONA_CABLE_HEADPHONE);
 	if (ret != 0)
@@ -764,7 +764,7 @@ static void arizona_micd_detect(struct work_struct *work)
 	mutex_lock(&info->lock);
 
 	/* If the cable was removed while measuring ignore the result */
-	ret = extcon_get_cable_state_(&info->edev, ARIZONA_CABLE_MECHANICAL);
+	ret = extcon_get_cable_state_(info->edev, ARIZONA_CABLE_MECHANICAL);
 	if (ret < 0) {
 		dev_err(arizona->dev, "Failed to check cable state: %d\n",
 				ret);
@@ -812,7 +812,7 @@ static void arizona_micd_detect(struct work_struct *work)
 	if (info->detecting && (val & ARIZONA_MICD_LVL_8)) {
 		arizona_identify_headphone(info);
 
-		ret = extcon_update_state(&info->edev,
+		ret = extcon_update_state(info->edev,
 					  1 << ARIZONA_CABLE_MICROPHONE,
 					  1 << ARIZONA_CABLE_MICROPHONE);
 
@@ -999,7 +999,7 @@ static irqreturn_t arizona_jackdet(int irq, void *data)
 
 	if (info->last_jackdet == present) {
 		dev_dbg(arizona->dev, "Detected jack\n");
-		ret = extcon_set_cable_state_(&info->edev,
+		ret = extcon_set_cable_state_(info->edev,
 					      ARIZONA_CABLE_MECHANICAL, true);
 
 		if (ret != 0)
@@ -1038,7 +1038,7 @@ static irqreturn_t arizona_jackdet(int irq, void *data)
 					 info->micd_ranges[i].key, 0);
 		input_sync(info->input);
 
-		ret = extcon_update_state(&info->edev, 0xffffffff, 0);
+		ret = extcon_update_state(info->edev, 0xffffffff, 0);
 		if (ret != 0)
 			dev_err(arizona->dev, "Removal report failed: %d\n",
 				ret);
@@ -1105,15 +1105,14 @@ static int arizona_extcon_probe(struct platform_device *pdev)
 	info = devm_kzalloc(&pdev->dev, sizeof(*info), GFP_KERNEL);
 	if (!info) {
 		dev_err(&pdev->dev, "Failed to allocate memory\n");
-		ret = -ENOMEM;
-		goto err;
+		return -ENOMEM;
 	}
 
 	info->micvdd = devm_regulator_get(arizona->dev, "MICVDD");
 	if (IS_ERR(info->micvdd)) {
 		ret = PTR_ERR(info->micvdd);
 		dev_err(arizona->dev, "Failed to get MICVDD: %d\n", ret);
-		goto err;
+		return ret;
 	}
 
 	mutex_init(&info->lock);
@@ -1151,15 +1150,19 @@ static int arizona_extcon_probe(struct platform_device *pdev)
 		break;
 	}
 
-	info->edev.name = "Headset Jack";
-	info->edev.dev.parent = arizona->dev;
-	info->edev.supported_cable = arizona_cable;
+	info->edev = devm_extcon_dev_allocate(&pdev->dev, arizona_cable);
+	if (IS_ERR(info->edev)) {
+		dev_err(&pdev->dev, "failed to allocate extcon device\n");
+		return -ENOMEM;
+	}
+	info->edev->name = "Headset Jack";
+	info->edev->dev.parent = arizona->dev;
 
-	ret = extcon_dev_register(&info->edev);
+	ret = devm_extcon_dev_register(&pdev->dev, info->edev);
 	if (ret < 0) {
 		dev_err(arizona->dev, "extcon_dev_register() failed: %d\n",
 			ret);
-		goto err;
+		return ret;
 	}
 
 	info->input = devm_input_allocate_device(&pdev->dev);
@@ -1410,8 +1413,6 @@ err_rise:
 err_input:
 err_register:
 	pm_runtime_disable(&pdev->dev);
-	extcon_dev_unregister(&info->edev);
-err:
 	return ret;
 }
 
@@ -1445,7 +1446,6 @@ static int arizona_extcon_remove(struct platform_device *pdev)
 	regmap_update_bits(arizona->regmap, ARIZONA_JACK_DETECT_ANALOGUE,
 			   ARIZONA_JD1_ENA, 0);
 	arizona_clk32k_disable(arizona);
-	extcon_dev_unregister(&info->edev);
 
 	return 0;
 }
diff --git a/drivers/extcon/extcon-class.c b/drivers/extcon/extcon-class.c
index 7ab21aa6eaa..18d42c0e458 100644
--- a/drivers/extcon/extcon-class.c
+++ b/drivers/extcon/extcon-class.c
@@ -565,6 +565,100 @@ static void dummy_sysfs_dev_release(struct device *dev)
 {
 }
 
+/*
+ * extcon_dev_allocate() - Allocate the memory of extcon device.
+ * @supported_cable:	Array of supported cable names ending with NULL.
+ *			If supported_cable is NULL, cable name related APIs
+ *			are disabled.
+ *
+ * This function allocates the memory for extcon device without allocating
+ * memory in each extcon provider driver and initialize default setting for
+ * extcon device.
+ *
+ * Return the pointer of extcon device if success or ERR_PTR(err) if fail
+ */
+struct extcon_dev *extcon_dev_allocate(const char **supported_cable)
+{
+	struct extcon_dev *edev;
+
+	edev = kzalloc(sizeof(*edev), GFP_KERNEL);
+	if (!edev)
+		return ERR_PTR(-ENOMEM);
+
+	edev->max_supported = 0;
+	edev->supported_cable = supported_cable;
+
+	return edev;
+}
+
+/*
+ * extcon_dev_free() - Free the memory of extcon device.
+ * @edev:	the extcon device to free
+ */
+void extcon_dev_free(struct extcon_dev *edev)
+{
+	kfree(edev);
+}
+EXPORT_SYMBOL_GPL(extcon_dev_free);
+
+static int devm_extcon_dev_match(struct device *dev, void *res, void *data)
+{
+	struct extcon_dev **r = res;
+
+	if (WARN_ON(!r || !*r))
+		return 0;
+
+	return *r == data;
+}
+
+static void devm_extcon_dev_release(struct device *dev, void *res)
+{
+	extcon_dev_free(*(struct extcon_dev **)res);
+}
+
+/**
+ * devm_extcon_dev_allocate - Allocate managed extcon device
+ * @dev:		device owning the extcon device being created
+ * @supported_cable:	Array of supported cable names ending with NULL.
+ *			If supported_cable is NULL, cable name related APIs
+ *			are disabled.
+ *
+ * This function manages automatically the memory of extcon device using device
+ * resource management and simplify the control of freeing the memory of extcon
+ * device.
+ *
+ * Returns the pointer memory of allocated extcon_dev if success
+ * or ERR_PTR(err) if fail
+ */
+struct extcon_dev *devm_extcon_dev_allocate(struct device *dev,
+					    const char **supported_cable)
+{
+	struct extcon_dev **ptr, *edev;
+
+	ptr = devres_alloc(devm_extcon_dev_release, sizeof(*ptr), GFP_KERNEL);
+	if (!ptr)
+		return ERR_PTR(-ENOMEM);
+
+	edev = extcon_dev_allocate(supported_cable);
+	if (IS_ERR(edev)) {
+		devres_free(ptr);
+		return edev;
+	}
+
+	*ptr = edev;
+	devres_add(dev, ptr);
+
+	return edev;
+}
+EXPORT_SYMBOL_GPL(devm_extcon_dev_allocate);
+
+void devm_extcon_dev_free(struct device *dev, struct extcon_dev *edev)
+{
+	WARN_ON(devres_release(dev, devm_extcon_dev_release,
+			       devm_extcon_dev_match, edev));
+}
+EXPORT_SYMBOL_GPL(devm_extcon_dev_free);
+
 /**
  * extcon_dev_register() - Register a new extcon device
  * @edev	: the new extcon device (should be allocated before calling)
@@ -819,6 +913,63 @@ void extcon_dev_unregister(struct extcon_dev *edev)
 }
 EXPORT_SYMBOL_GPL(extcon_dev_unregister);
 
+static void devm_extcon_dev_unreg(struct device *dev, void *res)
+{
+	extcon_dev_unregister(*(struct extcon_dev **)res);
+}
+
+/**
+ * devm_extcon_dev_register() - Resource-managed extcon_dev_register()
+ * @dev:	device to allocate extcon device
+ * @edev:	the new extcon device to register
+ *
+ * Managed extcon_dev_register() function. If extcon device is attached with
+ * this function, that extcon device is automatically unregistered on driver
+ * detach. Internally this function calls extcon_dev_register() function.
+ * To get more information, refer that function.
+ *
+ * If extcon device is registered with this function and the device needs to be
+ * unregistered separately, devm_extcon_dev_unregister() should be used.
+ *
+ * Returns 0 if success or negaive error number if failure.
+ */
+int devm_extcon_dev_register(struct device *dev, struct extcon_dev *edev)
+{
+	struct extcon_dev **ptr;
+	int ret;
+
+	ptr = devres_alloc(devm_extcon_dev_unreg, sizeof(*ptr), GFP_KERNEL);
+	if (!ptr)
+		return -ENOMEM;
+
+	ret = extcon_dev_register(edev);
+	if (ret) {
+		devres_free(ptr);
+		return ret;
+	}
+
+	*ptr = edev;
+	devres_add(dev, ptr);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(devm_extcon_dev_register);
+
+/**
+ * devm_extcon_dev_unregister() - Resource-managed extcon_dev_unregister()
+ * @dev:	device the extcon belongs to
+ * @edev:	the extcon device to unregister
+ *
+ * Unregister extcon device that is registered with devm_extcon_dev_register()
+ * function.
+ */
+void devm_extcon_dev_unregister(struct device *dev, struct extcon_dev *edev)
+{
+	WARN_ON(devres_release(dev, devm_extcon_dev_unreg,
+			       devm_extcon_dev_match, edev));
+}
+EXPORT_SYMBOL_GPL(devm_extcon_dev_unregister);
+
 #ifdef CONFIG_OF
 /*
  * extcon_get_edev_by_phandle - Get the extcon device from devicetree
diff --git a/drivers/extcon/extcon-gpio.c b/drivers/extcon/extcon-gpio.c
index 13d522255d8..645b2835681 100644
--- a/drivers/extcon/extcon-gpio.c
+++ b/drivers/extcon/extcon-gpio.c
@@ -32,7 +32,7 @@
 #include <linux/extcon/extcon-gpio.h>
 
 struct gpio_extcon_data {
-	struct extcon_dev edev;
+	struct extcon_dev *edev;
 	unsigned gpio;
 	bool gpio_active_low;
 	const char *state_on;
@@ -53,7 +53,7 @@ static void gpio_extcon_work(struct work_struct *work)
 	state = gpio_get_value(data->gpio);
 	if (data->gpio_active_low)
 		state = !state;
-	extcon_set_state(&data->edev, state);
+	extcon_set_state(data->edev, state);
 }
 
 static irqreturn_t gpio_irq_handler(int irq, void *dev_id)
@@ -67,9 +67,10 @@ static irqreturn_t gpio_irq_handler(int irq, void *dev_id)
 
 static ssize_t extcon_gpio_print_state(struct extcon_dev *edev, char *buf)
 {
-	struct gpio_extcon_data	*extcon_data =
-		container_of(edev, struct gpio_extcon_data, edev);
+	struct device *dev = edev->dev.parent;
+	struct gpio_extcon_data *extcon_data = dev_get_drvdata(dev);
 	const char *state;
+
 	if (extcon_get_state(edev))
 		state = extcon_data->state_on;
 	else
@@ -98,15 +99,21 @@ static int gpio_extcon_probe(struct platform_device *pdev)
 	if (!extcon_data)
 		return -ENOMEM;
 
-	extcon_data->edev.name = pdata->name;
-	extcon_data->edev.dev.parent = &pdev->dev;
+	extcon_data->edev = devm_extcon_dev_allocate(&pdev->dev, NULL);
+	if (IS_ERR(extcon_data->edev)) {
+		dev_err(&pdev->dev, "failed to allocate extcon device\n");
+		return -ENOMEM;
+	}
+	extcon_data->edev->name = pdata->name;
+	extcon_data->edev->dev.parent = &pdev->dev;
+
 	extcon_data->gpio = pdata->gpio;
 	extcon_data->gpio_active_low = pdata->gpio_active_low;
 	extcon_data->state_on = pdata->state_on;
 	extcon_data->state_off = pdata->state_off;
 	extcon_data->check_on_resume = pdata->check_on_resume;
 	if (pdata->state_on && pdata->state_off)
-		extcon_data->edev.print_state = extcon_gpio_print_state;
+		extcon_data->edev->print_state = extcon_gpio_print_state;
 
 	ret = devm_gpio_request_one(&pdev->dev, extcon_data->gpio, GPIOF_DIR_IN,
 				    pdev->name);
@@ -121,34 +128,27 @@ static int gpio_extcon_probe(struct platform_device *pdev)
 				msecs_to_jiffies(pdata->debounce);
 	}
 
-	ret = extcon_dev_register(&extcon_data->edev);
+	ret = devm_extcon_dev_register(&pdev->dev, extcon_data->edev);
 	if (ret < 0)
 		return ret;
 
 	INIT_DELAYED_WORK(&extcon_data->work, gpio_extcon_work);
 
 	extcon_data->irq = gpio_to_irq(extcon_data->gpio);
-	if (extcon_data->irq < 0) {
-		ret = extcon_data->irq;
-		goto err;
-	}
+	if (extcon_data->irq < 0)
+		return extcon_data->irq;
 
 	ret = request_any_context_irq(extcon_data->irq, gpio_irq_handler,
 				      pdata->irq_flags, pdev->name,
 				      extcon_data);
 	if (ret < 0)
-		goto err;
+		return ret;
 
 	platform_set_drvdata(pdev, extcon_data);
 	/* Perform initial detection */
 	gpio_extcon_work(&extcon_data->work.work);
 
 	return 0;
-
-err:
-	extcon_dev_unregister(&extcon_data->edev);
-
-	return ret;
 }
 
 static int gpio_extcon_remove(struct platform_device *pdev)
@@ -157,7 +157,6 @@ static int gpio_extcon_remove(struct platform_device *pdev)
 
 	cancel_delayed_work_sync(&extcon_data->work);
 	free_irq(extcon_data->irq, extcon_data);
-	extcon_dev_unregister(&extcon_data->edev);
 
 	return 0;
 }
diff --git a/drivers/extcon/extcon-max14577.c b/drivers/extcon/extcon-max14577.c
index 3846941801b..d49e891b567 100644
--- a/drivers/extcon/extcon-max14577.c
+++ b/drivers/extcon/extcon-max14577.c
@@ -1,8 +1,9 @@
 /*
- * extcon-max14577.c - MAX14577 extcon driver to support MAX14577 MUIC
+ * extcon-max14577.c - MAX14577/77836 extcon driver to support MUIC
  *
- * Copyright (C) 2013 Samsung Electrnoics
+ * Copyright (C) 2013,2014 Samsung Electrnoics
  * Chanwoo Choi <cw00.choi@samsung.com>
+ * Krzysztof Kozlowski <k.kozlowski@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
@@ -24,7 +25,6 @@
 #include <linux/mfd/max14577-private.h>
 #include <linux/extcon.h>
 
-#define	DEV_NAME			"max14577-muic"
 #define	DELAY_MS_DEFAULT		17000		/* unit: millisecond */
 
 enum max14577_muic_adc_debounce_time {
@@ -40,6 +40,42 @@ enum max14577_muic_status {
 	MAX14577_MUIC_STATUS_END,
 };
 
+/**
+ * struct max14577_muic_irq
+ * @irq: the index of irq list of MUIC device.
+ * @name: the name of irq.
+ * @virq: the virtual irq to use irq domain
+ */
+struct max14577_muic_irq {
+	unsigned int irq;
+	const char *name;
+	unsigned int virq;
+};
+
+static struct max14577_muic_irq max14577_muic_irqs[] = {
+	{ MAX14577_IRQ_INT1_ADC,	"muic-ADC" },
+	{ MAX14577_IRQ_INT1_ADCLOW,	"muic-ADCLOW" },
+	{ MAX14577_IRQ_INT1_ADCERR,	"muic-ADCError" },
+	{ MAX14577_IRQ_INT2_CHGTYP,	"muic-CHGTYP" },
+	{ MAX14577_IRQ_INT2_CHGDETRUN,	"muic-CHGDETRUN" },
+	{ MAX14577_IRQ_INT2_DCDTMR,	"muic-DCDTMR" },
+	{ MAX14577_IRQ_INT2_DBCHG,	"muic-DBCHG" },
+	{ MAX14577_IRQ_INT2_VBVOLT,	"muic-VBVOLT" },
+};
+
+static struct max14577_muic_irq max77836_muic_irqs[] = {
+	{ MAX14577_IRQ_INT1_ADC,	"muic-ADC" },
+	{ MAX14577_IRQ_INT1_ADCLOW,	"muic-ADCLOW" },
+	{ MAX14577_IRQ_INT1_ADCERR,	"muic-ADCError" },
+	{ MAX77836_IRQ_INT1_ADC1K,	"muic-ADC1K" },
+	{ MAX14577_IRQ_INT2_CHGTYP,	"muic-CHGTYP" },
+	{ MAX14577_IRQ_INT2_CHGDETRUN,	"muic-CHGDETRUN" },
+	{ MAX14577_IRQ_INT2_DCDTMR,	"muic-DCDTMR" },
+	{ MAX14577_IRQ_INT2_DBCHG,	"muic-DBCHG" },
+	{ MAX14577_IRQ_INT2_VBVOLT,	"muic-VBVOLT" },
+	{ MAX77836_IRQ_INT2_VIDRM,	"muic-VIDRM" },
+};
+
 struct max14577_muic_info {
 	struct device *dev;
 	struct max14577 *max14577;
@@ -48,6 +84,8 @@ struct max14577_muic_info {
 	int prev_chg_type;
 	u8 status[MAX14577_MUIC_STATUS_END];
 
+	struct max14577_muic_irq *muic_irqs;
+	unsigned int muic_irqs_num;
 	bool irq_adc;
 	bool irq_chg;
 	struct work_struct irq_work;
@@ -74,29 +112,6 @@ enum max14577_muic_cable_group {
 	MAX14577_CABLE_GROUP_CHG,
 };
 
-/**
- * struct max14577_muic_irq
- * @irq: the index of irq list of MUIC device.
- * @name: the name of irq.
- * @virq: the virtual irq to use irq domain
- */
-struct max14577_muic_irq {
-	unsigned int irq;
-	const char *name;
-	unsigned int virq;
-};
-
-static struct max14577_muic_irq muic_irqs[] = {
-	{ MAX14577_IRQ_INT1_ADC,	"muic-ADC" },
-	{ MAX14577_IRQ_INT1_ADCLOW,	"muic-ADCLOW" },
-	{ MAX14577_IRQ_INT1_ADCERR,	"muic-ADCError" },
-	{ MAX14577_IRQ_INT2_CHGTYP,	"muic-CHGTYP" },
-	{ MAX14577_IRQ_INT2_CHGDETRUN,	"muic-CHGDETRUN" },
-	{ MAX14577_IRQ_INT2_DCDTMR,	"muic-DCDTMR" },
-	{ MAX14577_IRQ_INT2_DBCHG,	"muic-DBCHG" },
-	{ MAX14577_IRQ_INT2_VBVOLT,	"muic-VBVOLT" },
-};
-
 /* Define supported accessory type */
 enum max14577_muic_acc_type {
 	MAX14577_MUIC_ADC_GROUND = 0x0,
@@ -528,21 +543,12 @@ static void max14577_muic_irq_work(struct work_struct *work)
 	return;
 }
 
-static irqreturn_t max14577_muic_irq_handler(int irq, void *data)
+/*
+ * Sets irq_adc or irq_chg in max14577_muic_info and returns 1.
+ * Returns 0 if irq_type does not match registered IRQ for this device type.
+ */
+static int max14577_parse_irq(struct max14577_muic_info *info, int irq_type)
 {
-	struct max14577_muic_info *info = data;
-	int i, irq_type = -1;
-
-	/*
-	 * We may be called multiple times for different nested IRQ-s.
-	 * Including changes in INT1_ADC and INT2_CGHTYP at once.
-	 * However we only need to know whether it was ADC, charger
-	 * or both interrupts so decode IRQ and turn on proper flags.
-	 */
-	for (i = 0; i < ARRAY_SIZE(muic_irqs); i++)
-		if (irq == muic_irqs[i].virq)
-			irq_type = muic_irqs[i].irq;
-
 	switch (irq_type) {
 	case MAX14577_IRQ_INT1_ADC:
 	case MAX14577_IRQ_INT1_ADCLOW:
@@ -550,7 +556,7 @@ static irqreturn_t max14577_muic_irq_handler(int irq, void *data)
 		/* Handle all of accessory except for
 		   type of charger accessory */
 		info->irq_adc = true;
-		break;
+		return 1;
 	case MAX14577_IRQ_INT2_CHGTYP:
 	case MAX14577_IRQ_INT2_CHGDETRUN:
 	case MAX14577_IRQ_INT2_DCDTMR:
@@ -558,8 +564,62 @@ static irqreturn_t max14577_muic_irq_handler(int irq, void *data)
 	case MAX14577_IRQ_INT2_VBVOLT:
 		/* Handle charger accessory */
 		info->irq_chg = true;
+		return 1;
+	default:
+		return 0;
+	}
+}
+
+/*
+ * Sets irq_adc or irq_chg in max14577_muic_info and returns 1.
+ * Returns 0 if irq_type does not match registered IRQ for this device type.
+ */
+static int max77836_parse_irq(struct max14577_muic_info *info, int irq_type)
+{
+	/* First check common max14577 interrupts */
+	if (max14577_parse_irq(info, irq_type))
+		return 1;
+
+	switch (irq_type) {
+	case MAX77836_IRQ_INT1_ADC1K:
+		info->irq_adc = true;
+		return 1;
+	case MAX77836_IRQ_INT2_VIDRM:
+		/* Handle charger accessory */
+		info->irq_chg = true;
+		return 1;
+	default:
+		return 0;
+	}
+}
+
+static irqreturn_t max14577_muic_irq_handler(int irq, void *data)
+{
+	struct max14577_muic_info *info = data;
+	int i, irq_type = -1;
+	bool irq_parsed;
+
+	/*
+	 * We may be called multiple times for different nested IRQ-s.
+	 * Including changes in INT1_ADC and INT2_CGHTYP at once.
+	 * However we only need to know whether it was ADC, charger
+	 * or both interrupts so decode IRQ and turn on proper flags.
+	 */
+	for (i = 0; i < info->muic_irqs_num; i++)
+		if (irq == info->muic_irqs[i].virq)
+			irq_type = info->muic_irqs[i].irq;
+
+	switch (info->max14577->dev_type) {
+	case MAXIM_DEVICE_TYPE_MAX77836:
+		irq_parsed = max77836_parse_irq(info, irq_type);
 		break;
+	case MAXIM_DEVICE_TYPE_MAX14577:
 	default:
+		irq_parsed = max14577_parse_irq(info, irq_type);
+		break;
+	}
+
+	if (!irq_parsed) {
 		dev_err(info->dev, "muic interrupt: irq %d occurred, skipped\n",
 				irq_type);
 		return IRQ_HANDLED;
@@ -644,13 +704,24 @@ static int max14577_muic_probe(struct platform_device *pdev)
 
 	INIT_WORK(&info->irq_work, max14577_muic_irq_work);
 
+	switch (max14577->dev_type) {
+	case MAXIM_DEVICE_TYPE_MAX77836:
+		info->muic_irqs = max77836_muic_irqs;
+		info->muic_irqs_num = ARRAY_SIZE(max77836_muic_irqs);
+		break;
+	case MAXIM_DEVICE_TYPE_MAX14577:
+	default:
+		info->muic_irqs = max14577_muic_irqs;
+		info->muic_irqs_num = ARRAY_SIZE(max14577_muic_irqs);
+	}
+
 	/* Support irq domain for max14577 MUIC device */
-	for (i = 0; i < ARRAY_SIZE(muic_irqs); i++) {
-		struct max14577_muic_irq *muic_irq = &muic_irqs[i];
+	for (i = 0; i < info->muic_irqs_num; i++) {
+		struct max14577_muic_irq *muic_irq = &info->muic_irqs[i];
 		unsigned int virq = 0;
 
 		virq = regmap_irq_get_virq(max14577->irq_data, muic_irq->irq);
-		if (!virq)
+		if (virq <= 0)
 			return -EINVAL;
 		muic_irq->virq = virq;
 
@@ -668,14 +739,16 @@ static int max14577_muic_probe(struct platform_device *pdev)
 	}
 
 	/* Initialize extcon device */
-	info->edev = devm_kzalloc(&pdev->dev, sizeof(*info->edev), GFP_KERNEL);
-	if (!info->edev) {
+	info->edev = devm_extcon_dev_allocate(&pdev->dev,
+					      max14577_extcon_cable);
+	if (IS_ERR(info->edev)) {
 		dev_err(&pdev->dev, "failed to allocate memory for extcon\n");
 		return -ENOMEM;
 	}
-	info->edev->name = DEV_NAME;
-	info->edev->supported_cable = max14577_extcon_cable;
-	ret = extcon_dev_register(info->edev);
+
+	info->edev->name = dev_name(&pdev->dev);
+
+	ret = devm_extcon_dev_register(&pdev->dev, info->edev);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to register extcon device\n");
 		return ret;
@@ -694,7 +767,7 @@ static int max14577_muic_probe(struct platform_device *pdev)
 			MAX14577_REG_DEVICEID, &id);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "failed to read revision number\n");
-		goto err_extcon;
+		return ret;
 	}
 	dev_info(info->dev, "device ID : 0x%x\n", id);
 
@@ -710,19 +783,10 @@ static int max14577_muic_probe(struct platform_device *pdev)
 	 * driver should notify cable state to upper layer.
 	 */
 	INIT_DELAYED_WORK(&info->wq_detcable, max14577_muic_detect_cable_wq);
-	ret = queue_delayed_work(system_power_efficient_wq, &info->wq_detcable,
+	queue_delayed_work(system_power_efficient_wq, &info->wq_detcable,
 			delay_jiffies);
-	if (ret < 0) {
-		dev_err(&pdev->dev,
-			"failed to schedule delayed work for cable detect\n");
-		goto err_extcon;
-	}
 
 	return ret;
-
-err_extcon:
-	extcon_dev_unregister(info->edev);
-	return ret;
 }
 
 static int max14577_muic_remove(struct platform_device *pdev)
@@ -730,23 +794,30 @@ static int max14577_muic_remove(struct platform_device *pdev)
 	struct max14577_muic_info *info = platform_get_drvdata(pdev);
 
 	cancel_work_sync(&info->irq_work);
-	extcon_dev_unregister(info->edev);
 
 	return 0;
 }
 
+static const struct platform_device_id max14577_muic_id[] = {
+	{ "max14577-muic", MAXIM_DEVICE_TYPE_MAX14577, },
+	{ "max77836-muic", MAXIM_DEVICE_TYPE_MAX77836, },
+	{ }
+};
+MODULE_DEVICE_TABLE(platform, max14577_muic_id);
+
 static struct platform_driver max14577_muic_driver = {
 	.driver		= {
-		.name	= DEV_NAME,
+		.name	= "max14577-muic",
 		.owner	= THIS_MODULE,
 	},
 	.probe		= max14577_muic_probe,
 	.remove		= max14577_muic_remove,
+	.id_table	= max14577_muic_id,
 };
 
 module_platform_driver(max14577_muic_driver);
 
-MODULE_DESCRIPTION("MAXIM 14577 Extcon driver");
-MODULE_AUTHOR("Chanwoo Choi <cw00.choi@samsung.com>");
+MODULE_DESCRIPTION("Maxim 14577/77836 Extcon driver");
+MODULE_AUTHOR("Chanwoo Choi <cw00.choi@samsung.com>, Krzysztof Kozlowski <k.kozlowski@samsung.com>");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("platform:extcon-max14577");
diff --git a/drivers/extcon/extcon-max77693.c b/drivers/extcon/extcon-max77693.c
index da268fbc901..2c7c3e19159 100644
--- a/drivers/extcon/extcon-max77693.c
+++ b/drivers/extcon/extcon-max77693.c
@@ -1175,25 +1175,24 @@ static int max77693_muic_probe(struct platform_device *pdev)
 	}
 
 	/* Initialize extcon device */
-	info->edev = devm_kzalloc(&pdev->dev, sizeof(struct extcon_dev),
-				  GFP_KERNEL);
-	if (!info->edev) {
+	info->edev = devm_extcon_dev_allocate(&pdev->dev,
+					      max77693_extcon_cable);
+	if (IS_ERR(info->edev)) {
 		dev_err(&pdev->dev, "failed to allocate memory for extcon\n");
 		ret = -ENOMEM;
 		goto err_irq;
 	}
 	info->edev->name = DEV_NAME;
 	info->edev->dev.parent = &pdev->dev;
-	info->edev->supported_cable = max77693_extcon_cable;
-	ret = extcon_dev_register(info->edev);
+
+	ret = devm_extcon_dev_register(&pdev->dev, info->edev);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to register extcon device\n");
 		goto err_irq;
 	}
 
-
 	/* Initialize MUIC register by using platform data or default data */
-	if (pdata->muic_data) {
+	if (pdata && pdata->muic_data) {
 		init_data = pdata->muic_data->init_data;
 		num_init_data = pdata->muic_data->num_init_data;
 	} else {
@@ -1226,7 +1225,7 @@ static int max77693_muic_probe(struct platform_device *pdev)
 				= init_data[i].data;
 	}
 
-	if (pdata->muic_data) {
+	if (pdata && pdata->muic_data) {
 		struct max77693_muic_platform_data *muic_pdata
 						   = pdata->muic_data;
 
@@ -1267,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_extcon;
+		goto err_irq;
 	}
 	dev_info(info->dev, "device ID : 0x%x\n", id);
 
@@ -1283,12 +1282,11 @@ static int max77693_muic_probe(struct platform_device *pdev)
 	 * driver should notify cable state to upper layer.
 	 */
 	INIT_DELAYED_WORK(&info->wq_detcable, max77693_muic_detect_cable_wq);
-	schedule_delayed_work(&info->wq_detcable, delay_jiffies);
+	queue_delayed_work(system_power_efficient_wq, &info->wq_detcable,
+			delay_jiffies);
 
 	return ret;
 
-err_extcon:
-	extcon_dev_unregister(info->edev);
 err_irq:
 	while (--i >= 0)
 		free_irq(muic_irqs[i].virq, info);
@@ -1304,7 +1302,6 @@ static int max77693_muic_remove(struct platform_device *pdev)
 		free_irq(muic_irqs[i].virq, info);
 	cancel_work_sync(&info->irq_work);
 	input_unregister_device(info->dock);
-	extcon_dev_unregister(info->edev);
 
 	return 0;
 }
diff --git a/drivers/extcon/extcon-max8997.c b/drivers/extcon/extcon-max8997.c
index 6a00464658c..d9f7f1baaa0 100644
--- a/drivers/extcon/extcon-max8997.c
+++ b/drivers/extcon/extcon-max8997.c
@@ -699,23 +699,22 @@ static int max8997_muic_probe(struct platform_device *pdev)
 	}
 
 	/* External connector */
-	info->edev = devm_kzalloc(&pdev->dev, sizeof(struct extcon_dev),
-				  GFP_KERNEL);
-	if (!info->edev) {
+	info->edev = devm_extcon_dev_allocate(&pdev->dev, max8997_extcon_cable);
+	if (IS_ERR(info->edev)) {
 		dev_err(&pdev->dev, "failed to allocate memory for extcon\n");
 		ret = -ENOMEM;
 		goto err_irq;
 	}
 	info->edev->name = DEV_NAME;
 	info->edev->dev.parent = &pdev->dev;
-	info->edev->supported_cable = max8997_extcon_cable;
-	ret = extcon_dev_register(info->edev);
+
+	ret = devm_extcon_dev_register(&pdev->dev, info->edev);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to register extcon device\n");
 		goto err_irq;
 	}
 
-	if (pdata->muic_pdata) {
+	if (pdata && pdata->muic_pdata) {
 		struct max8997_muic_platform_data *muic_pdata
 			= pdata->muic_pdata;
 
@@ -770,7 +769,8 @@ static int max8997_muic_probe(struct platform_device *pdev)
 	 * driver should notify cable state to upper layer.
 	 */
 	INIT_DELAYED_WORK(&info->wq_detcable, max8997_muic_detect_cable_wq);
-	schedule_delayed_work(&info->wq_detcable, delay_jiffies);
+	queue_delayed_work(system_power_efficient_wq, &info->wq_detcable,
+			delay_jiffies);
 
 	return 0;
 
@@ -789,8 +789,6 @@ static int max8997_muic_remove(struct platform_device *pdev)
 		free_irq(muic_irqs[i].virq, info);
 	cancel_work_sync(&info->irq_work);
 
-	extcon_dev_unregister(info->edev);
-
 	return 0;
 }
 
diff --git a/drivers/extcon/extcon-palmas.c b/drivers/extcon/extcon-palmas.c
index ddff2b72f0a..7417ce84eb2 100644
--- a/drivers/extcon/extcon-palmas.c
+++ b/drivers/extcon/extcon-palmas.c
@@ -23,6 +23,7 @@
 #include <linux/module.h>
 #include <linux/interrupt.h>
 #include <linux/platform_device.h>
+#include <linux/slab.h>
 #include <linux/err.h>
 #include <linux/mfd/palmas.h>
 #include <linux/of.h>
@@ -56,7 +57,7 @@ static irqreturn_t palmas_vbus_irq_handler(int irq, void *_palmas_usb)
 	if (vbus_line_state & PALMAS_INT3_LINE_STATE_VBUS) {
 		if (palmas_usb->linkstat != PALMAS_USB_STATE_VBUS) {
 			palmas_usb->linkstat = PALMAS_USB_STATE_VBUS;
-			extcon_set_cable_state(&palmas_usb->edev, "USB", true);
+			extcon_set_cable_state(palmas_usb->edev, "USB", true);
 			dev_info(palmas_usb->dev, "USB cable is attached\n");
 		} else {
 			dev_dbg(palmas_usb->dev,
@@ -65,7 +66,7 @@ static irqreturn_t palmas_vbus_irq_handler(int irq, void *_palmas_usb)
 	} else if (!(vbus_line_state & PALMAS_INT3_LINE_STATE_VBUS)) {
 		if (palmas_usb->linkstat == PALMAS_USB_STATE_VBUS) {
 			palmas_usb->linkstat = PALMAS_USB_STATE_DISCONNECT;
-			extcon_set_cable_state(&palmas_usb->edev, "USB", false);
+			extcon_set_cable_state(palmas_usb->edev, "USB", false);
 			dev_info(palmas_usb->dev, "USB cable is detached\n");
 		} else {
 			dev_dbg(palmas_usb->dev,
@@ -92,7 +93,7 @@ static irqreturn_t palmas_id_irq_handler(int irq, void *_palmas_usb)
 			PALMAS_USB_ID_INT_LATCH_CLR,
 			PALMAS_USB_ID_INT_EN_HI_CLR_ID_GND);
 		palmas_usb->linkstat = PALMAS_USB_STATE_ID;
-		extcon_set_cable_state(&palmas_usb->edev, "USB-HOST", true);
+		extcon_set_cable_state(palmas_usb->edev, "USB-HOST", true);
 		dev_info(palmas_usb->dev, "USB-HOST cable is attached\n");
 	} else if ((set & PALMAS_USB_ID_INT_SRC_ID_FLOAT) &&
 				(id_src & PALMAS_USB_ID_INT_SRC_ID_FLOAT)) {
@@ -100,17 +101,17 @@ static irqreturn_t palmas_id_irq_handler(int irq, void *_palmas_usb)
 			PALMAS_USB_ID_INT_LATCH_CLR,
 			PALMAS_USB_ID_INT_EN_HI_CLR_ID_FLOAT);
 		palmas_usb->linkstat = PALMAS_USB_STATE_DISCONNECT;
-		extcon_set_cable_state(&palmas_usb->edev, "USB-HOST", false);
+		extcon_set_cable_state(palmas_usb->edev, "USB-HOST", false);
 		dev_info(palmas_usb->dev, "USB-HOST cable is detached\n");
 	} else if ((palmas_usb->linkstat == PALMAS_USB_STATE_ID) &&
 				(!(set & PALMAS_USB_ID_INT_SRC_ID_GND))) {
 		palmas_usb->linkstat = PALMAS_USB_STATE_DISCONNECT;
-		extcon_set_cable_state(&palmas_usb->edev, "USB-HOST", false);
+		extcon_set_cable_state(palmas_usb->edev, "USB-HOST", false);
 		dev_info(palmas_usb->dev, "USB-HOST cable is detached\n");
 	} else if ((palmas_usb->linkstat == PALMAS_USB_STATE_DISCONNECT) &&
 				(id_src & PALMAS_USB_ID_INT_SRC_ID_GND)) {
 		palmas_usb->linkstat = PALMAS_USB_STATE_ID;
-		extcon_set_cable_state(&palmas_usb->edev, "USB-HOST", true);
+		extcon_set_cable_state(palmas_usb->edev, "USB-HOST", true);
 		dev_info(palmas_usb->dev, " USB-HOST cable is attached\n");
 	}
 
@@ -186,13 +187,20 @@ static int palmas_usb_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, palmas_usb);
 
-	palmas_usb->edev.supported_cable = palmas_extcon_cable;
-	palmas_usb->edev.dev.parent = palmas_usb->dev;
-	palmas_usb->edev.mutually_exclusive = mutually_exclusive;
+	palmas_usb->edev = devm_extcon_dev_allocate(&pdev->dev,
+						    palmas_extcon_cable);
+	if (IS_ERR(palmas_usb->edev)) {
+		dev_err(&pdev->dev, "failed to allocate extcon device\n");
+		return -ENOMEM;
+	}
+	palmas_usb->edev->name = kstrdup(node->name, GFP_KERNEL);
+	palmas_usb->edev->dev.parent = palmas_usb->dev;
+	palmas_usb->edev->mutually_exclusive = mutually_exclusive;
 
-	status = extcon_dev_register(&palmas_usb->edev);
+	status = devm_extcon_dev_register(&pdev->dev, palmas_usb->edev);
 	if (status) {
 		dev_err(&pdev->dev, "failed to register extcon device\n");
+		kfree(palmas_usb->edev->name);
 		return status;
 	}
 
@@ -206,7 +214,8 @@ static int palmas_usb_probe(struct platform_device *pdev)
 		if (status < 0) {
 			dev_err(&pdev->dev, "can't get IRQ %d, err %d\n",
 					palmas_usb->id_irq, status);
-			goto fail_extcon;
+			kfree(palmas_usb->edev->name);
+			return status;
 		}
 	}
 
@@ -220,25 +229,21 @@ static int palmas_usb_probe(struct platform_device *pdev)
 		if (status < 0) {
 			dev_err(&pdev->dev, "can't get IRQ %d, err %d\n",
 					palmas_usb->vbus_irq, status);
-			goto fail_extcon;
+			kfree(palmas_usb->edev->name);
+			return status;
 		}
 	}
 
 	palmas_enable_irq(palmas_usb);
 	device_set_wakeup_capable(&pdev->dev, true);
 	return 0;
-
-fail_extcon:
-	extcon_dev_unregister(&palmas_usb->edev);
-
-	return status;
 }
 
 static int palmas_usb_remove(struct platform_device *pdev)
 {
 	struct palmas_usb *palmas_usb = platform_get_drvdata(pdev);
 
-	extcon_dev_unregister(&palmas_usb->edev);
+	kfree(palmas_usb->edev->name);
 
 	return 0;
 }
diff --git a/drivers/firewire/Kconfig b/drivers/firewire/Kconfig
index 7a701a58bbf..4199849e375 100644
--- a/drivers/firewire/Kconfig
+++ b/drivers/firewire/Kconfig
@@ -1,5 +1,5 @@
 menu "IEEE 1394 (FireWire) support"
-	depends on PCI || BROKEN
+	depends on PCI || COMPILE_TEST
 	# firewire-core does not depend on PCI but is
 	# not useful without PCI controller driver
 
diff --git a/drivers/firewire/core.h b/drivers/firewire/core.h
index c98764aeeec..e1480ff683d 100644
--- a/drivers/firewire/core.h
+++ b/drivers/firewire/core.h
@@ -118,7 +118,6 @@ int fw_card_add(struct fw_card *card,
 		u32 max_receive, u32 link_speed, u64 guid);
 void fw_core_remove_card(struct fw_card *card);
 int fw_compute_block_crc(__be32 *block);
-void fw_schedule_bus_reset(struct fw_card *card, bool delayed, bool short_reset);
 void fw_schedule_bm_work(struct fw_card *card, unsigned long delay);
 
 /* -cdev */
@@ -237,8 +236,8 @@ static inline bool is_next_generation(int new_generation, int old_generation)
 
 #define LOCAL_BUS 0xffc0
 
-/* arbitrarily chosen maximum range for physical DMA: 128 TB */
-#define FW_MAX_PHYSICAL_RANGE		(128ULL << 40)
+/* OHCI-1394's default upper bound for physical DMA: 4 GB */
+#define FW_MAX_PHYSICAL_RANGE		(1ULL << 32)
 
 void fw_core_handle_request(struct fw_card *card, struct fw_packet *request);
 void fw_core_handle_response(struct fw_card *card, struct fw_packet *packet);
diff --git a/drivers/firewire/net.c b/drivers/firewire/net.c
index 4af0a7bad7f..c3986452194 100644
--- a/drivers/firewire/net.c
+++ b/drivers/firewire/net.c
@@ -1462,8 +1462,8 @@ static int fwnet_probe(struct fw_unit *unit,
 
 	net = alloc_netdev(sizeof(*dev), "firewire%d", fwnet_init_dev);
 	if (net == NULL) {
-		ret = -ENOMEM;
-		goto out;
+		mutex_unlock(&fwnet_device_mutex);
+		return -ENOMEM;
 	}
 
 	allocated_netdev = true;
diff --git a/drivers/firewire/ohci.c b/drivers/firewire/ohci.c
index 8db66321956..57985410f12 100644
--- a/drivers/firewire/ohci.c
+++ b/drivers/firewire/ohci.c
@@ -282,6 +282,7 @@ static char ohci_driver_name[] = KBUILD_MODNAME;
 #define PCI_DEVICE_ID_TI_TSB82AA2	0x8025
 #define PCI_DEVICE_ID_VIA_VT630X	0x3044
 #define PCI_REV_ID_VIA_VT6306		0x46
+#define PCI_DEVICE_ID_VIA_VT6315	0x3403
 
 #define QUIRK_CYCLE_TIMER		0x1
 #define QUIRK_RESET_PACKET		0x2
@@ -334,6 +335,12 @@ static const struct {
 	{PCI_VENDOR_ID_VIA, PCI_DEVICE_ID_VIA_VT630X, PCI_REV_ID_VIA_VT6306,
 		QUIRK_CYCLE_TIMER | QUIRK_IR_WAKE},
 
+	{PCI_VENDOR_ID_VIA, PCI_DEVICE_ID_VIA_VT6315, 0,
+		QUIRK_CYCLE_TIMER | QUIRK_NO_MSI},
+
+	{PCI_VENDOR_ID_VIA, PCI_DEVICE_ID_VIA_VT6315, PCI_ANY_ID,
+		0},
+
 	{PCI_VENDOR_ID_VIA, PCI_ANY_ID, PCI_ANY_ID,
 		QUIRK_CYCLE_TIMER | QUIRK_NO_MSI},
 };
@@ -3498,7 +3505,7 @@ static int ohci_flush_iso_completions(struct fw_iso_context *base)
 		}
 
 		clear_bit_unlock(0, &ctx->flushing_completions);
-		smp_mb__after_clear_bit();
+		smp_mb__after_atomic();
 	}
 
 	tasklet_enable(&ctx->context.tasklet);
@@ -3716,7 +3723,7 @@ static int pci_probe(struct pci_dev *dev,
 		    version >> 16, version & 0xff, ohci->card.index,
 		    ohci->n_ir, ohci->n_it, ohci->quirks,
 		    reg_read(ohci, OHCI1394_PhyUpperBound) ?
-			", >4 GB phys DMA" : "");
+			", physUB" : "");
 
 	return 0;
 
diff --git a/drivers/firmware/efi/Kconfig b/drivers/firmware/efi/Kconfig
index 1e75f48b61f..d420ae2d341 100644
--- a/drivers/firmware/efi/Kconfig
+++ b/drivers/firmware/efi/Kconfig
@@ -47,6 +47,13 @@ config EFI_RUNTIME_MAP
 
 	  See also Documentation/ABI/testing/sysfs-firmware-efi-runtime-map.
 
+config EFI_PARAMS_FROM_FDT
+	bool
+	help
+	  Select this config option from the architecture Kconfig if
+	  the EFI runtime support gets system table address, memory
+          map address, and other parameters from the device tree.
+
 endmenu
 
 config UEFI_CPER
diff --git a/drivers/firmware/efi/arm-stub.c b/drivers/firmware/efi/arm-stub.c
new file mode 100644
index 00000000000..41114ce03b0
--- /dev/null
+++ b/drivers/firmware/efi/arm-stub.c
@@ -0,0 +1,278 @@
+/*
+ * EFI stub implementation that is shared by arm and arm64 architectures.
+ * This should be #included by the EFI stub implementation files.
+ *
+ * Copyright (C) 2013,2014 Linaro Limited
+ *     Roy Franz <roy.franz@linaro.org
+ * Copyright (C) 2013 Red Hat, Inc.
+ *     Mark Salter <msalter@redhat.com>
+ *
+ * This file is part of the Linux kernel, and is made available under the
+ * terms of the GNU General Public License version 2.
+ *
+ */
+
+static int __init efi_secureboot_enabled(efi_system_table_t *sys_table_arg)
+{
+	static efi_guid_t const var_guid __initconst = EFI_GLOBAL_VARIABLE_GUID;
+	static efi_char16_t const var_name[] __initconst = {
+		'S', 'e', 'c', 'u', 'r', 'e', 'B', 'o', 'o', 't', 0 };
+
+	efi_get_variable_t *f_getvar = sys_table_arg->runtime->get_variable;
+	unsigned long size = sizeof(u8);
+	efi_status_t status;
+	u8 val;
+
+	status = f_getvar((efi_char16_t *)var_name, (efi_guid_t *)&var_guid,
+			  NULL, &size, &val);
+
+	switch (status) {
+	case EFI_SUCCESS:
+		return val;
+	case EFI_NOT_FOUND:
+		return 0;
+	default:
+		return 1;
+	}
+}
+
+static efi_status_t efi_open_volume(efi_system_table_t *sys_table_arg,
+				    void *__image, void **__fh)
+{
+	efi_file_io_interface_t *io;
+	efi_loaded_image_t *image = __image;
+	efi_file_handle_t *fh;
+	efi_guid_t fs_proto = EFI_FILE_SYSTEM_GUID;
+	efi_status_t status;
+	void *handle = (void *)(unsigned long)image->device_handle;
+
+	status = sys_table_arg->boottime->handle_protocol(handle,
+				 &fs_proto, (void **)&io);
+	if (status != EFI_SUCCESS) {
+		efi_printk(sys_table_arg, "Failed to handle fs_proto\n");
+		return status;
+	}
+
+	status = io->open_volume(io, &fh);
+	if (status != EFI_SUCCESS)
+		efi_printk(sys_table_arg, "Failed to open volume\n");
+
+	*__fh = fh;
+	return status;
+}
+static efi_status_t efi_file_close(void *handle)
+{
+	efi_file_handle_t *fh = handle;
+
+	return fh->close(handle);
+}
+
+static efi_status_t
+efi_file_read(void *handle, unsigned long *size, void *addr)
+{
+	efi_file_handle_t *fh = handle;
+
+	return fh->read(handle, size, addr);
+}
+
+
+static efi_status_t
+efi_file_size(efi_system_table_t *sys_table_arg, void *__fh,
+	      efi_char16_t *filename_16, void **handle, u64 *file_sz)
+{
+	efi_file_handle_t *h, *fh = __fh;
+	efi_file_info_t *info;
+	efi_status_t status;
+	efi_guid_t info_guid = EFI_FILE_INFO_ID;
+	unsigned long info_sz;
+
+	status = fh->open(fh, &h, filename_16, EFI_FILE_MODE_READ, (u64)0);
+	if (status != EFI_SUCCESS) {
+		efi_printk(sys_table_arg, "Failed to open file: ");
+		efi_char16_printk(sys_table_arg, filename_16);
+		efi_printk(sys_table_arg, "\n");
+		return status;
+	}
+
+	*handle = h;
+
+	info_sz = 0;
+	status = h->get_info(h, &info_guid, &info_sz, NULL);
+	if (status != EFI_BUFFER_TOO_SMALL) {
+		efi_printk(sys_table_arg, "Failed to get file info size\n");
+		return status;
+	}
+
+grow:
+	status = sys_table_arg->boottime->allocate_pool(EFI_LOADER_DATA,
+				 info_sz, (void **)&info);
+	if (status != EFI_SUCCESS) {
+		efi_printk(sys_table_arg, "Failed to alloc mem for file info\n");
+		return status;
+	}
+
+	status = h->get_info(h, &info_guid, &info_sz,
+						   info);
+	if (status == EFI_BUFFER_TOO_SMALL) {
+		sys_table_arg->boottime->free_pool(info);
+		goto grow;
+	}
+
+	*file_sz = info->file_size;
+	sys_table_arg->boottime->free_pool(info);
+
+	if (status != EFI_SUCCESS)
+		efi_printk(sys_table_arg, "Failed to get initrd info\n");
+
+	return status;
+}
+
+
+
+static void efi_char16_printk(efi_system_table_t *sys_table_arg,
+			      efi_char16_t *str)
+{
+	struct efi_simple_text_output_protocol *out;
+
+	out = (struct efi_simple_text_output_protocol *)sys_table_arg->con_out;
+	out->output_string(out, str);
+}
+
+
+/*
+ * This function handles the architcture specific differences between arm and
+ * arm64 regarding where the kernel image must be loaded and any memory that
+ * must be reserved. On failure it is required to free all
+ * all allocations it has made.
+ */
+static efi_status_t handle_kernel_image(efi_system_table_t *sys_table,
+					unsigned long *image_addr,
+					unsigned long *image_size,
+					unsigned long *reserve_addr,
+					unsigned long *reserve_size,
+					unsigned long dram_base,
+					efi_loaded_image_t *image);
+/*
+ * EFI entry point for the arm/arm64 EFI stubs.  This is the entrypoint
+ * that is described in the PE/COFF header.  Most of the code is the same
+ * for both archictectures, with the arch-specific code provided in the
+ * handle_kernel_image() function.
+ */
+unsigned long __init efi_entry(void *handle, efi_system_table_t *sys_table,
+			       unsigned long *image_addr)
+{
+	efi_loaded_image_t *image;
+	efi_status_t status;
+	unsigned long image_size = 0;
+	unsigned long dram_base;
+	/* addr/point and size pairs for memory management*/
+	unsigned long initrd_addr;
+	u64 initrd_size = 0;
+	unsigned long fdt_addr = 0;  /* Original DTB */
+	u64 fdt_size = 0;  /* We don't get size from configuration table */
+	char *cmdline_ptr = NULL;
+	int cmdline_size = 0;
+	unsigned long new_fdt_addr;
+	efi_guid_t loaded_image_proto = LOADED_IMAGE_PROTOCOL_GUID;
+	unsigned long reserve_addr = 0;
+	unsigned long reserve_size = 0;
+
+	/* Check if we were booted by the EFI firmware */
+	if (sys_table->hdr.signature != EFI_SYSTEM_TABLE_SIGNATURE)
+		goto fail;
+
+	pr_efi(sys_table, "Booting Linux Kernel...\n");
+
+	/*
+	 * Get a handle to the loaded image protocol.  This is used to get
+	 * information about the running image, such as size and the command
+	 * line.
+	 */
+	status = sys_table->boottime->handle_protocol(handle,
+					&loaded_image_proto, (void *)&image);
+	if (status != EFI_SUCCESS) {
+		pr_efi_err(sys_table, "Failed to get loaded image protocol\n");
+		goto fail;
+	}
+
+	dram_base = get_dram_base(sys_table);
+	if (dram_base == EFI_ERROR) {
+		pr_efi_err(sys_table, "Failed to find DRAM base\n");
+		goto fail;
+	}
+	status = handle_kernel_image(sys_table, image_addr, &image_size,
+				     &reserve_addr,
+				     &reserve_size,
+				     dram_base, image);
+	if (status != EFI_SUCCESS) {
+		pr_efi_err(sys_table, "Failed to relocate kernel\n");
+		goto fail;
+	}
+
+	/*
+	 * Get the command line from EFI, using the LOADED_IMAGE
+	 * protocol. We are going to copy the command line into the
+	 * device tree, so this can be allocated anywhere.
+	 */
+	cmdline_ptr = efi_convert_cmdline(sys_table, image, &cmdline_size);
+	if (!cmdline_ptr) {
+		pr_efi_err(sys_table, "getting command line via LOADED_IMAGE_PROTOCOL\n");
+		goto fail_free_image;
+	}
+
+	/*
+	 * Unauthenticated device tree data is a security hazard, so
+	 * ignore 'dtb=' unless UEFI Secure Boot is disabled.
+	 */
+	if (efi_secureboot_enabled(sys_table)) {
+		pr_efi(sys_table, "UEFI Secure Boot is enabled.\n");
+	} else {
+		status = handle_cmdline_files(sys_table, image, cmdline_ptr,
+					      "dtb=",
+					      ~0UL, (unsigned long *)&fdt_addr,
+					      (unsigned long *)&fdt_size);
+
+		if (status != EFI_SUCCESS) {
+			pr_efi_err(sys_table, "Failed to load device tree!\n");
+			goto fail_free_cmdline;
+		}
+	}
+	if (!fdt_addr)
+		/* Look for a device tree configuration table entry. */
+		fdt_addr = (uintptr_t)get_fdt(sys_table);
+
+	status = handle_cmdline_files(sys_table, image, cmdline_ptr,
+				      "initrd=", dram_base + SZ_512M,
+				      (unsigned long *)&initrd_addr,
+				      (unsigned long *)&initrd_size);
+	if (status != EFI_SUCCESS)
+		pr_efi_err(sys_table, "Failed initrd from command line!\n");
+
+	new_fdt_addr = fdt_addr;
+	status = allocate_new_fdt_and_exit_boot(sys_table, handle,
+				&new_fdt_addr, dram_base + MAX_FDT_OFFSET,
+				initrd_addr, initrd_size, cmdline_ptr,
+				fdt_addr, fdt_size);
+
+	/*
+	 * If all went well, we need to return the FDT address to the
+	 * calling function so it can be passed to kernel as part of
+	 * the kernel boot protocol.
+	 */
+	if (status == EFI_SUCCESS)
+		return new_fdt_addr;
+
+	pr_efi_err(sys_table, "Failed to update FDT and exit boot services\n");
+
+	efi_free(sys_table, initrd_size, initrd_addr);
+	efi_free(sys_table, fdt_size, fdt_addr);
+
+fail_free_cmdline:
+	efi_free(sys_table, cmdline_size, (unsigned long)cmdline_ptr);
+
+fail_free_image:
+	efi_free(sys_table, image_size, *image_addr);
+	efi_free(sys_table, reserve_size, reserve_addr);
+fail:
+	return EFI_ERROR;
+}
diff --git a/drivers/firmware/efi/efi-stub-helper.c b/drivers/firmware/efi/efi-stub-helper.c
index 2c41eaece2c..eb6d4be9e72 100644
--- a/drivers/firmware/efi/efi-stub-helper.c
+++ b/drivers/firmware/efi/efi-stub-helper.c
@@ -11,6 +11,10 @@
  */
 #define EFI_READ_CHUNK_SIZE	(1024 * 1024)
 
+/* error code which can't be mistaken for valid address */
+#define EFI_ERROR	(~0UL)
+
+
 struct file_info {
 	efi_file_handle_t *handle;
 	u64 size;
@@ -33,6 +37,9 @@ static void efi_printk(efi_system_table_t *sys_table_arg, char *str)
 	}
 }
 
+#define pr_efi(sys_table, msg)     efi_printk(sys_table, "EFI stub: "msg)
+#define pr_efi_err(sys_table, msg) efi_printk(sys_table, "EFI stub: ERROR: "msg)
+
 
 static efi_status_t efi_get_memory_map(efi_system_table_t *sys_table_arg,
 				       efi_memory_desc_t **map,
@@ -80,6 +87,32 @@ fail:
 	return status;
 }
 
+
+static unsigned long __init get_dram_base(efi_system_table_t *sys_table_arg)
+{
+	efi_status_t status;
+	unsigned long map_size;
+	unsigned long membase  = EFI_ERROR;
+	struct efi_memory_map map;
+	efi_memory_desc_t *md;
+
+	status = efi_get_memory_map(sys_table_arg, (efi_memory_desc_t **)&map.map,
+				    &map_size, &map.desc_size, NULL, NULL);
+	if (status != EFI_SUCCESS)
+		return membase;
+
+	map.map_end = map.map + map_size;
+
+	for_each_efi_memory_desc(&map, md)
+		if (md->attribute & EFI_MEMORY_WB)
+			if (membase > md->phys_addr)
+				membase = md->phys_addr;
+
+	efi_call_early(free_pool, map.map);
+
+	return membase;
+}
+
 /*
  * Allocate at the highest possible address that is not above 'max'.
  */
@@ -267,7 +300,7 @@ static efi_status_t handle_cmdline_files(efi_system_table_t *sys_table_arg,
 	struct file_info *files;
 	unsigned long file_addr;
 	u64 file_size_total;
-	efi_file_handle_t *fh;
+	efi_file_handle_t *fh = NULL;
 	efi_status_t status;
 	int nr_files;
 	char *str;
@@ -310,7 +343,7 @@ static efi_status_t handle_cmdline_files(efi_system_table_t *sys_table_arg,
 	status = efi_call_early(allocate_pool, EFI_LOADER_DATA,
 				nr_files * sizeof(*files), (void **)&files);
 	if (status != EFI_SUCCESS) {
-		efi_printk(sys_table_arg, "Failed to alloc mem for file handle list\n");
+		pr_efi_err(sys_table_arg, "Failed to alloc mem for file handle list\n");
 		goto fail;
 	}
 
@@ -374,13 +407,13 @@ static efi_status_t handle_cmdline_files(efi_system_table_t *sys_table_arg,
 		status = efi_high_alloc(sys_table_arg, file_size_total, 0x1000,
 				    &file_addr, max_addr);
 		if (status != EFI_SUCCESS) {
-			efi_printk(sys_table_arg, "Failed to alloc highmem for files\n");
+			pr_efi_err(sys_table_arg, "Failed to alloc highmem for files\n");
 			goto close_handles;
 		}
 
 		/* We've run out of free low memory. */
 		if (file_addr > max_addr) {
-			efi_printk(sys_table_arg, "We've run out of free low memory\n");
+			pr_efi_err(sys_table_arg, "We've run out of free low memory\n");
 			status = EFI_INVALID_PARAMETER;
 			goto free_file_total;
 		}
@@ -401,7 +434,7 @@ static efi_status_t handle_cmdline_files(efi_system_table_t *sys_table_arg,
 						       &chunksize,
 						       (void *)addr);
 				if (status != EFI_SUCCESS) {
-					efi_printk(sys_table_arg, "Failed to read file\n");
+					pr_efi_err(sys_table_arg, "Failed to read file\n");
 					goto free_file_total;
 				}
 				addr += chunksize;
@@ -486,7 +519,7 @@ static efi_status_t efi_relocate_kernel(efi_system_table_t *sys_table_arg,
 				       &new_addr);
 	}
 	if (status != EFI_SUCCESS) {
-		efi_printk(sys_table_arg, "ERROR: Failed to allocate usable memory for kernel.\n");
+		pr_efi_err(sys_table_arg, "Failed to allocate usable memory for kernel.\n");
 		return status;
 	}
 
@@ -503,62 +536,99 @@ static efi_status_t efi_relocate_kernel(efi_system_table_t *sys_table_arg,
 }
 
 /*
+ * Get the number of UTF-8 bytes corresponding to an UTF-16 character.
+ * This overestimates for surrogates, but that is okay.
+ */
+static int efi_utf8_bytes(u16 c)
+{
+	return 1 + (c >= 0x80) + (c >= 0x800);
+}
+
+/*
+ * Convert an UTF-16 string, not necessarily null terminated, to UTF-8.
+ */
+static u8 *efi_utf16_to_utf8(u8 *dst, const u16 *src, int n)
+{
+	unsigned int c;
+
+	while (n--) {
+		c = *src++;
+		if (n && c >= 0xd800 && c <= 0xdbff &&
+		    *src >= 0xdc00 && *src <= 0xdfff) {
+			c = 0x10000 + ((c & 0x3ff) << 10) + (*src & 0x3ff);
+			src++;
+			n--;
+		}
+		if (c >= 0xd800 && c <= 0xdfff)
+			c = 0xfffd; /* Unmatched surrogate */
+		if (c < 0x80) {
+			*dst++ = c;
+			continue;
+		}
+		if (c < 0x800) {
+			*dst++ = 0xc0 + (c >> 6);
+			goto t1;
+		}
+		if (c < 0x10000) {
+			*dst++ = 0xe0 + (c >> 12);
+			goto t2;
+		}
+		*dst++ = 0xf0 + (c >> 18);
+		*dst++ = 0x80 + ((c >> 12) & 0x3f);
+	t2:
+		*dst++ = 0x80 + ((c >> 6) & 0x3f);
+	t1:
+		*dst++ = 0x80 + (c & 0x3f);
+	}
+
+	return dst;
+}
+
+/*
  * Convert the unicode UEFI command line to ASCII to pass to kernel.
  * Size of memory allocated return in *cmd_line_len.
  * Returns NULL on error.
  */
-static char *efi_convert_cmdline_to_ascii(efi_system_table_t *sys_table_arg,
-				      efi_loaded_image_t *image,
-				      int *cmd_line_len)
+static char *efi_convert_cmdline(efi_system_table_t *sys_table_arg,
+				 efi_loaded_image_t *image,
+				 int *cmd_line_len)
 {
-	u16 *s2;
+	const u16 *s2;
 	u8 *s1 = NULL;
 	unsigned long cmdline_addr = 0;
-	int load_options_size = image->load_options_size / 2; /* ASCII */
-	void *options = image->load_options;
-	int options_size = 0;
+	int load_options_chars = image->load_options_size / 2; /* UTF-16 */
+	const u16 *options = image->load_options;
+	int options_bytes = 0;  /* UTF-8 bytes */
+	int options_chars = 0;  /* UTF-16 chars */
 	efi_status_t status;
-	int i;
 	u16 zero = 0;
 
 	if (options) {
 		s2 = options;
-		while (*s2 && *s2 != '\n' && options_size < load_options_size) {
-			s2++;
-			options_size++;
+		while (*s2 && *s2 != '\n'
+		       && options_chars < load_options_chars) {
+			options_bytes += efi_utf8_bytes(*s2++);
+			options_chars++;
 		}
 	}
 
-	if (options_size == 0) {
+	if (!options_chars) {
 		/* No command line options, so return empty string*/
-		options_size = 1;
 		options = &zero;
 	}
 
-	options_size++;  /* NUL termination */
-#ifdef CONFIG_ARM
-	/*
-	 * For ARM, allocate at a high address to avoid reserved
-	 * regions at low addresses that we don't know the specfics of
-	 * at the time we are processing the command line.
-	 */
-	status = efi_high_alloc(sys_table_arg, options_size, 0,
-			    &cmdline_addr, 0xfffff000);
-#else
-	status = efi_low_alloc(sys_table_arg, options_size, 0,
-			    &cmdline_addr);
-#endif
+	options_bytes++;	/* NUL termination */
+
+	status = efi_low_alloc(sys_table_arg, options_bytes, 0, &cmdline_addr);
 	if (status != EFI_SUCCESS)
 		return NULL;
 
 	s1 = (u8 *)cmdline_addr;
-	s2 = (u16 *)options;
-
-	for (i = 0; i < options_size - 1; i++)
-		*s1++ = *s2++;
+	s2 = (const u16 *)options;
 
+	s1 = efi_utf16_to_utf8(s1, s2, options_chars);
 	*s1 = '\0';
 
-	*cmd_line_len = options_size;
+	*cmd_line_len = options_bytes;
 	return (char *)cmdline_addr;
 }
diff --git a/drivers/firmware/efi/efi.c b/drivers/firmware/efi/efi.c
index af20f171233..cd36deb619f 100644
--- a/drivers/firmware/efi/efi.c
+++ b/drivers/firmware/efi/efi.c
@@ -20,6 +20,8 @@
 #include <linux/init.h>
 #include <linux/device.h>
 #include <linux/efi.h>
+#include <linux/of.h>
+#include <linux/of_fdt.h>
 #include <linux/io.h>
 
 struct efi __read_mostly efi = {
@@ -318,3 +320,80 @@ int __init efi_config_init(efi_config_table_type_t *arch_tables)
 
 	return 0;
 }
+
+#ifdef CONFIG_EFI_PARAMS_FROM_FDT
+
+#define UEFI_PARAM(name, prop, field)			   \
+	{						   \
+		{ name },				   \
+		{ prop },				   \
+		offsetof(struct efi_fdt_params, field),    \
+		FIELD_SIZEOF(struct efi_fdt_params, field) \
+	}
+
+static __initdata struct {
+	const char name[32];
+	const char propname[32];
+	int offset;
+	int size;
+} dt_params[] = {
+	UEFI_PARAM("System Table", "linux,uefi-system-table", system_table),
+	UEFI_PARAM("MemMap Address", "linux,uefi-mmap-start", mmap),
+	UEFI_PARAM("MemMap Size", "linux,uefi-mmap-size", mmap_size),
+	UEFI_PARAM("MemMap Desc. Size", "linux,uefi-mmap-desc-size", desc_size),
+	UEFI_PARAM("MemMap Desc. Version", "linux,uefi-mmap-desc-ver", desc_ver)
+};
+
+struct param_info {
+	int verbose;
+	void *params;
+};
+
+static int __init fdt_find_uefi_params(unsigned long node, const char *uname,
+				       int depth, void *data)
+{
+	struct param_info *info = data;
+	void *prop, *dest;
+	unsigned long len;
+	u64 val;
+	int i;
+
+	if (depth != 1 ||
+	    (strcmp(uname, "chosen") != 0 && strcmp(uname, "chosen@0") != 0))
+		return 0;
+
+	pr_info("Getting parameters from FDT:\n");
+
+	for (i = 0; i < ARRAY_SIZE(dt_params); i++) {
+		prop = of_get_flat_dt_prop(node, dt_params[i].propname, &len);
+		if (!prop) {
+			pr_err("Can't find %s in device tree!\n",
+			       dt_params[i].name);
+			return 0;
+		}
+		dest = info->params + dt_params[i].offset;
+
+		val = of_read_number(prop, len / sizeof(u32));
+
+		if (dt_params[i].size == sizeof(u32))
+			*(u32 *)dest = val;
+		else
+			*(u64 *)dest = val;
+
+		if (info->verbose)
+			pr_info("  %s: 0x%0*llx\n", dt_params[i].name,
+				dt_params[i].size * 2, val);
+	}
+	return 1;
+}
+
+int __init efi_get_fdt_params(struct efi_fdt_params *params, int verbose)
+{
+	struct param_info info;
+
+	info.verbose = verbose;
+	info.params = params;
+
+	return of_scan_flat_dt(fdt_find_uefi_params, &info);
+}
+#endif /* CONFIG_EFI_PARAMS_FROM_FDT */
diff --git a/drivers/firmware/efi/efivars.c b/drivers/firmware/efi/efivars.c
index 50ea412a25e..463c56545ae 100644
--- a/drivers/firmware/efi/efivars.c
+++ b/drivers/firmware/efi/efivars.c
@@ -69,6 +69,7 @@
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/ucs2_string.h>
+#include <linux/compat.h>
 
 #define EFIVARS_VERSION "0.08"
 #define EFIVARS_DATE "2004-May-17"
@@ -86,6 +87,15 @@ static struct kset *efivars_kset;
 static struct bin_attribute *efivars_new_var;
 static struct bin_attribute *efivars_del_var;
 
+struct compat_efi_variable {
+	efi_char16_t  VariableName[EFI_VAR_NAME_LEN/sizeof(efi_char16_t)];
+	efi_guid_t    VendorGuid;
+	__u32         DataSize;
+	__u8          Data[1024];
+	__u32         Status;
+	__u32         Attributes;
+} __packed;
+
 struct efivar_attribute {
 	struct attribute attr;
 	ssize_t (*show) (struct efivar_entry *entry, char *buf);
@@ -189,45 +199,107 @@ efivar_data_read(struct efivar_entry *entry, char *buf)
 	memcpy(buf, var->Data, var->DataSize);
 	return var->DataSize;
 }
-/*
- * We allow each variable to be edited via rewriting the
- * entire efi variable structure.
- */
-static ssize_t
-efivar_store_raw(struct efivar_entry *entry, const char *buf, size_t count)
-{
-	struct efi_variable *new_var, *var = &entry->var;
-	int err;
 
-	if (count != sizeof(struct efi_variable))
-		return -EINVAL;
-
-	new_var = (struct efi_variable *)buf;
+static inline int
+sanity_check(struct efi_variable *var, efi_char16_t *name, efi_guid_t vendor,
+	     unsigned long size, u32 attributes, u8 *data)
+{
 	/*
 	 * If only updating the variable data, then the name
 	 * and guid should remain the same
 	 */
-	if (memcmp(new_var->VariableName, var->VariableName, sizeof(var->VariableName)) ||
-		efi_guidcmp(new_var->VendorGuid, var->VendorGuid)) {
+	if (memcmp(name, var->VariableName, sizeof(var->VariableName)) ||
+		efi_guidcmp(vendor, var->VendorGuid)) {
 		printk(KERN_ERR "efivars: Cannot edit the wrong variable!\n");
 		return -EINVAL;
 	}
 
-	if ((new_var->DataSize <= 0) || (new_var->Attributes == 0)){
+	if ((size <= 0) || (attributes == 0)){
 		printk(KERN_ERR "efivars: DataSize & Attributes must be valid!\n");
 		return -EINVAL;
 	}
 
-	if ((new_var->Attributes & ~EFI_VARIABLE_MASK) != 0 ||
-	    efivar_validate(new_var, new_var->Data, new_var->DataSize) == false) {
+	if ((attributes & ~EFI_VARIABLE_MASK) != 0 ||
+	    efivar_validate(name, data, size) == false) {
 		printk(KERN_ERR "efivars: Malformed variable content\n");
 		return -EINVAL;
 	}
 
-	memcpy(&entry->var, new_var, count);
+	return 0;
+}
+
+static inline bool is_compat(void)
+{
+	if (IS_ENABLED(CONFIG_COMPAT) && is_compat_task())
+		return true;
+
+	return false;
+}
+
+static void
+copy_out_compat(struct efi_variable *dst, struct compat_efi_variable *src)
+{
+	memcpy(dst->VariableName, src->VariableName, EFI_VAR_NAME_LEN);
+	memcpy(dst->Data, src->Data, sizeof(src->Data));
+
+	dst->VendorGuid = src->VendorGuid;
+	dst->DataSize = src->DataSize;
+	dst->Attributes = src->Attributes;
+}
+
+/*
+ * We allow each variable to be edited via rewriting the
+ * entire efi variable structure.
+ */
+static ssize_t
+efivar_store_raw(struct efivar_entry *entry, const char *buf, size_t count)
+{
+	struct efi_variable *new_var, *var = &entry->var;
+	efi_char16_t *name;
+	unsigned long size;
+	efi_guid_t vendor;
+	u32 attributes;
+	u8 *data;
+	int err;
+
+	if (is_compat()) {
+		struct compat_efi_variable *compat;
+
+		if (count != sizeof(*compat))
+			return -EINVAL;
+
+		compat = (struct compat_efi_variable *)buf;
+		attributes = compat->Attributes;
+		vendor = compat->VendorGuid;
+		name = compat->VariableName;
+		size = compat->DataSize;
+		data = compat->Data;
+
+		err = sanity_check(var, name, vendor, size, attributes, data);
+		if (err)
+			return err;
+
+		copy_out_compat(&entry->var, compat);
+	} else {
+		if (count != sizeof(struct efi_variable))
+			return -EINVAL;
+
+		new_var = (struct efi_variable *)buf;
 
-	err = efivar_entry_set(entry, new_var->Attributes,
-			       new_var->DataSize, new_var->Data, NULL);
+		attributes = new_var->Attributes;
+		vendor = new_var->VendorGuid;
+		name = new_var->VariableName;
+		size = new_var->DataSize;
+		data = new_var->Data;
+
+		err = sanity_check(var, name, vendor, size, attributes, data);
+		if (err)
+			return err;
+
+		memcpy(&entry->var, new_var, count);
+	}
+
+	err = efivar_entry_set(entry, attributes, size, data, NULL);
 	if (err) {
 		printk(KERN_WARNING "efivars: set_variable() failed: status=%d\n", err);
 		return -EIO;
@@ -240,6 +312,8 @@ static ssize_t
 efivar_show_raw(struct efivar_entry *entry, char *buf)
 {
 	struct efi_variable *var = &entry->var;
+	struct compat_efi_variable *compat;
+	size_t size;
 
 	if (!entry || !buf)
 		return 0;
@@ -249,9 +323,23 @@ efivar_show_raw(struct efivar_entry *entry, char *buf)
 			     &entry->var.DataSize, entry->var.Data))
 		return -EIO;
 
-	memcpy(buf, var, sizeof(*var));
+	if (is_compat()) {
+		compat = (struct compat_efi_variable *)buf;
+
+		size = sizeof(*compat);
+		memcpy(compat->VariableName, var->VariableName,
+			EFI_VAR_NAME_LEN);
+		memcpy(compat->Data, var->Data, sizeof(compat->Data));
+
+		compat->VendorGuid = var->VendorGuid;
+		compat->DataSize = var->DataSize;
+		compat->Attributes = var->Attributes;
+	} else {
+		size = sizeof(*var);
+		memcpy(buf, var, size);
+	}
 
-	return sizeof(*var);
+	return size;
 }
 
 /*
@@ -326,15 +414,39 @@ static ssize_t efivar_create(struct file *filp, struct kobject *kobj,
 			     struct bin_attribute *bin_attr,
 			     char *buf, loff_t pos, size_t count)
 {
+	struct compat_efi_variable *compat = (struct compat_efi_variable *)buf;
 	struct efi_variable *new_var = (struct efi_variable *)buf;
 	struct efivar_entry *new_entry;
+	bool need_compat = is_compat();
+	efi_char16_t *name;
+	unsigned long size;
+	u32 attributes;
+	u8 *data;
 	int err;
 
 	if (!capable(CAP_SYS_ADMIN))
 		return -EACCES;
 
-	if ((new_var->Attributes & ~EFI_VARIABLE_MASK) != 0 ||
-	    efivar_validate(new_var, new_var->Data, new_var->DataSize) == false) {
+	if (need_compat) {
+		if (count != sizeof(*compat))
+			return -EINVAL;
+
+		attributes = compat->Attributes;
+		name = compat->VariableName;
+		size = compat->DataSize;
+		data = compat->Data;
+	} else {
+		if (count != sizeof(*new_var))
+			return -EINVAL;
+
+		attributes = new_var->Attributes;
+		name = new_var->VariableName;
+		size = new_var->DataSize;
+		data = new_var->Data;
+	}
+
+	if ((attributes & ~EFI_VARIABLE_MASK) != 0 ||
+	    efivar_validate(name, data, size) == false) {
 		printk(KERN_ERR "efivars: Malformed variable content\n");
 		return -EINVAL;
 	}
@@ -343,10 +455,13 @@ static ssize_t efivar_create(struct file *filp, struct kobject *kobj,
 	if (!new_entry)
 		return -ENOMEM;
 
-	memcpy(&new_entry->var, new_var, sizeof(*new_var));
+	if (need_compat)
+		copy_out_compat(&new_entry->var, compat);
+	else
+		memcpy(&new_entry->var, new_var, sizeof(*new_var));
 
-	err = efivar_entry_set(new_entry, new_var->Attributes, new_var->DataSize,
-			       new_var->Data, &efivar_sysfs_list);
+	err = efivar_entry_set(new_entry, attributes, size,
+			       data, &efivar_sysfs_list);
 	if (err) {
 		if (err == -EEXIST)
 			err = -EINVAL;
@@ -369,15 +484,32 @@ static ssize_t efivar_delete(struct file *filp, struct kobject *kobj,
 			     char *buf, loff_t pos, size_t count)
 {
 	struct efi_variable *del_var = (struct efi_variable *)buf;
+	struct compat_efi_variable *compat;
 	struct efivar_entry *entry;
+	efi_char16_t *name;
+	efi_guid_t vendor;
 	int err = 0;
 
 	if (!capable(CAP_SYS_ADMIN))
 		return -EACCES;
 
+	if (is_compat()) {
+		if (count != sizeof(*compat))
+			return -EINVAL;
+
+		compat = (struct compat_efi_variable *)buf;
+		name = compat->VariableName;
+		vendor = compat->VendorGuid;
+	} else {
+		if (count != sizeof(*del_var))
+			return -EINVAL;
+
+		name = del_var->VariableName;
+		vendor = del_var->VendorGuid;
+	}
+
 	efivar_entry_iter_begin();
-	entry = efivar_entry_find(del_var->VariableName, del_var->VendorGuid,
-				  &efivar_sysfs_list, true);
+	entry = efivar_entry_find(name, vendor, &efivar_sysfs_list, true);
 	if (!entry)
 		err = -EINVAL;
 	else if (__efivar_entry_delete(entry))
diff --git a/drivers/firmware/efi/fdt.c b/drivers/firmware/efi/fdt.c
new file mode 100644
index 00000000000..5c6a8e8a958
--- /dev/null
+++ b/drivers/firmware/efi/fdt.c
@@ -0,0 +1,285 @@
+/*
+ * FDT related Helper functions used by the EFI stub on multiple
+ * architectures. This should be #included by the EFI stub
+ * implementation files.
+ *
+ * Copyright 2013 Linaro Limited; author Roy Franz
+ *
+ * This file is part of the Linux kernel, and is made available
+ * under the terms of the GNU General Public License version 2.
+ *
+ */
+
+static efi_status_t update_fdt(efi_system_table_t *sys_table, void *orig_fdt,
+			       unsigned long orig_fdt_size,
+			       void *fdt, int new_fdt_size, char *cmdline_ptr,
+			       u64 initrd_addr, u64 initrd_size,
+			       efi_memory_desc_t *memory_map,
+			       unsigned long map_size, unsigned long desc_size,
+			       u32 desc_ver)
+{
+	int node, prev;
+	int status;
+	u32 fdt_val32;
+	u64 fdt_val64;
+
+	/*
+	 * Copy definition of linux_banner here.  Since this code is
+	 * built as part of the decompressor for ARM v7, pulling
+	 * in version.c where linux_banner is defined for the
+	 * kernel brings other kernel dependencies with it.
+	 */
+	const char linux_banner[] =
+	    "Linux version " UTS_RELEASE " (" LINUX_COMPILE_BY "@"
+	    LINUX_COMPILE_HOST ") (" LINUX_COMPILER ") " UTS_VERSION "\n";
+
+	/* Do some checks on provided FDT, if it exists*/
+	if (orig_fdt) {
+		if (fdt_check_header(orig_fdt)) {
+			pr_efi_err(sys_table, "Device Tree header not valid!\n");
+			return EFI_LOAD_ERROR;
+		}
+		/*
+		 * We don't get the size of the FDT if we get if from a
+		 * configuration table.
+		 */
+		if (orig_fdt_size && fdt_totalsize(orig_fdt) > orig_fdt_size) {
+			pr_efi_err(sys_table, "Truncated device tree! foo!\n");
+			return EFI_LOAD_ERROR;
+		}
+	}
+
+	if (orig_fdt)
+		status = fdt_open_into(orig_fdt, fdt, new_fdt_size);
+	else
+		status = fdt_create_empty_tree(fdt, new_fdt_size);
+
+	if (status != 0)
+		goto fdt_set_fail;
+
+	/*
+	 * Delete any memory nodes present. We must delete nodes which
+	 * early_init_dt_scan_memory may try to use.
+	 */
+	prev = 0;
+	for (;;) {
+		const char *type, *name;
+		int len;
+
+		node = fdt_next_node(fdt, prev, NULL);
+		if (node < 0)
+			break;
+
+		type = fdt_getprop(fdt, node, "device_type", &len);
+		if (type && strncmp(type, "memory", len) == 0) {
+			fdt_del_node(fdt, node);
+			continue;
+		}
+
+		prev = node;
+	}
+
+	node = fdt_subnode_offset(fdt, 0, "chosen");
+	if (node < 0) {
+		node = fdt_add_subnode(fdt, 0, "chosen");
+		if (node < 0) {
+			status = node; /* node is error code when negative */
+			goto fdt_set_fail;
+		}
+	}
+
+	if ((cmdline_ptr != NULL) && (strlen(cmdline_ptr) > 0)) {
+		status = fdt_setprop(fdt, node, "bootargs", cmdline_ptr,
+				     strlen(cmdline_ptr) + 1);
+		if (status)
+			goto fdt_set_fail;
+	}
+
+	/* Set initrd address/end in device tree, if present */
+	if (initrd_size != 0) {
+		u64 initrd_image_end;
+		u64 initrd_image_start = cpu_to_fdt64(initrd_addr);
+
+		status = fdt_setprop(fdt, node, "linux,initrd-start",
+				     &initrd_image_start, sizeof(u64));
+		if (status)
+			goto fdt_set_fail;
+		initrd_image_end = cpu_to_fdt64(initrd_addr + initrd_size);
+		status = fdt_setprop(fdt, node, "linux,initrd-end",
+				     &initrd_image_end, sizeof(u64));
+		if (status)
+			goto fdt_set_fail;
+	}
+
+	/* Add FDT entries for EFI runtime services in chosen node. */
+	node = fdt_subnode_offset(fdt, 0, "chosen");
+	fdt_val64 = cpu_to_fdt64((u64)(unsigned long)sys_table);
+	status = fdt_setprop(fdt, node, "linux,uefi-system-table",
+			     &fdt_val64, sizeof(fdt_val64));
+	if (status)
+		goto fdt_set_fail;
+
+	fdt_val64 = cpu_to_fdt64((u64)(unsigned long)memory_map);
+	status = fdt_setprop(fdt, node, "linux,uefi-mmap-start",
+			     &fdt_val64,  sizeof(fdt_val64));
+	if (status)
+		goto fdt_set_fail;
+
+	fdt_val32 = cpu_to_fdt32(map_size);
+	status = fdt_setprop(fdt, node, "linux,uefi-mmap-size",
+			     &fdt_val32,  sizeof(fdt_val32));
+	if (status)
+		goto fdt_set_fail;
+
+	fdt_val32 = cpu_to_fdt32(desc_size);
+	status = fdt_setprop(fdt, node, "linux,uefi-mmap-desc-size",
+			     &fdt_val32, sizeof(fdt_val32));
+	if (status)
+		goto fdt_set_fail;
+
+	fdt_val32 = cpu_to_fdt32(desc_ver);
+	status = fdt_setprop(fdt, node, "linux,uefi-mmap-desc-ver",
+			     &fdt_val32, sizeof(fdt_val32));
+	if (status)
+		goto fdt_set_fail;
+
+	/*
+	 * Add kernel version banner so stub/kernel match can be
+	 * verified.
+	 */
+	status = fdt_setprop_string(fdt, node, "linux,uefi-stub-kern-ver",
+			     linux_banner);
+	if (status)
+		goto fdt_set_fail;
+
+	return EFI_SUCCESS;
+
+fdt_set_fail:
+	if (status == -FDT_ERR_NOSPACE)
+		return EFI_BUFFER_TOO_SMALL;
+
+	return EFI_LOAD_ERROR;
+}
+
+#ifndef EFI_FDT_ALIGN
+#define EFI_FDT_ALIGN EFI_PAGE_SIZE
+#endif
+
+/*
+ * Allocate memory for a new FDT, then add EFI, commandline, and
+ * initrd related fields to the FDT.  This routine increases the
+ * FDT allocation size until the allocated memory is large
+ * enough.  EFI allocations are in EFI_PAGE_SIZE granules,
+ * which are fixed at 4K bytes, so in most cases the first
+ * allocation should succeed.
+ * EFI boot services are exited at the end of this function.
+ * There must be no allocations between the get_memory_map()
+ * call and the exit_boot_services() call, so the exiting of
+ * boot services is very tightly tied to the creation of the FDT
+ * with the final memory map in it.
+ */
+
+efi_status_t allocate_new_fdt_and_exit_boot(efi_system_table_t *sys_table,
+					    void *handle,
+					    unsigned long *new_fdt_addr,
+					    unsigned long max_addr,
+					    u64 initrd_addr, u64 initrd_size,
+					    char *cmdline_ptr,
+					    unsigned long fdt_addr,
+					    unsigned long fdt_size)
+{
+	unsigned long map_size, desc_size;
+	u32 desc_ver;
+	unsigned long mmap_key;
+	efi_memory_desc_t *memory_map;
+	unsigned long new_fdt_size;
+	efi_status_t status;
+
+	/*
+	 * Estimate size of new FDT, and allocate memory for it. We
+	 * will allocate a bigger buffer if this ends up being too
+	 * small, so a rough guess is OK here.
+	 */
+	new_fdt_size = fdt_size + EFI_PAGE_SIZE;
+	while (1) {
+		status = efi_high_alloc(sys_table, new_fdt_size, EFI_FDT_ALIGN,
+					new_fdt_addr, max_addr);
+		if (status != EFI_SUCCESS) {
+			pr_efi_err(sys_table, "Unable to allocate memory for new device tree.\n");
+			goto fail;
+		}
+
+		/*
+		 * Now that we have done our final memory allocation (and free)
+		 * we can get the memory map key  needed for
+		 * exit_boot_services().
+		 */
+		status = efi_get_memory_map(sys_table, &memory_map, &map_size,
+					    &desc_size, &desc_ver, &mmap_key);
+		if (status != EFI_SUCCESS)
+			goto fail_free_new_fdt;
+
+		status = update_fdt(sys_table,
+				    (void *)fdt_addr, fdt_size,
+				    (void *)*new_fdt_addr, new_fdt_size,
+				    cmdline_ptr, initrd_addr, initrd_size,
+				    memory_map, map_size, desc_size, desc_ver);
+
+		/* Succeeding the first time is the expected case. */
+		if (status == EFI_SUCCESS)
+			break;
+
+		if (status == EFI_BUFFER_TOO_SMALL) {
+			/*
+			 * We need to allocate more space for the new
+			 * device tree, so free existing buffer that is
+			 * too small.  Also free memory map, as we will need
+			 * to get new one that reflects the free/alloc we do
+			 * on the device tree buffer.
+			 */
+			efi_free(sys_table, new_fdt_size, *new_fdt_addr);
+			sys_table->boottime->free_pool(memory_map);
+			new_fdt_size += EFI_PAGE_SIZE;
+		} else {
+			pr_efi_err(sys_table, "Unable to constuct new device tree.\n");
+			goto fail_free_mmap;
+		}
+	}
+
+	/* Now we are ready to exit_boot_services.*/
+	status = sys_table->boottime->exit_boot_services(handle, mmap_key);
+
+
+	if (status == EFI_SUCCESS)
+		return status;
+
+	pr_efi_err(sys_table, "Exit boot services failed.\n");
+
+fail_free_mmap:
+	sys_table->boottime->free_pool(memory_map);
+
+fail_free_new_fdt:
+	efi_free(sys_table, new_fdt_size, *new_fdt_addr);
+
+fail:
+	return EFI_LOAD_ERROR;
+}
+
+static void *get_fdt(efi_system_table_t *sys_table)
+{
+	efi_guid_t fdt_guid = DEVICE_TREE_GUID;
+	efi_config_table_t *tables;
+	void *fdt;
+	int i;
+
+	tables = (efi_config_table_t *) sys_table->tables;
+	fdt = NULL;
+
+	for (i = 0; i < sys_table->nr_tables; i++)
+		if (efi_guidcmp(tables[i].guid, fdt_guid) == 0) {
+			fdt = (void *) tables[i].table;
+			break;
+	 }
+
+	return fdt;
+}
diff --git a/drivers/firmware/efi/vars.c b/drivers/firmware/efi/vars.c
index b22659cccca..f0a43646a2f 100644
--- a/drivers/firmware/efi/vars.c
+++ b/drivers/firmware/efi/vars.c
@@ -42,7 +42,7 @@ DECLARE_WORK(efivar_work, NULL);
 EXPORT_SYMBOL_GPL(efivar_work);
 
 static bool
-validate_device_path(struct efi_variable *var, int match, u8 *buffer,
+validate_device_path(efi_char16_t *var_name, int match, u8 *buffer,
 		     unsigned long len)
 {
 	struct efi_generic_dev_path *node;
@@ -75,7 +75,7 @@ validate_device_path(struct efi_variable *var, int match, u8 *buffer,
 }
 
 static bool
-validate_boot_order(struct efi_variable *var, int match, u8 *buffer,
+validate_boot_order(efi_char16_t *var_name, int match, u8 *buffer,
 		    unsigned long len)
 {
 	/* An array of 16-bit integers */
@@ -86,18 +86,18 @@ validate_boot_order(struct efi_variable *var, int match, u8 *buffer,
 }
 
 static bool
-validate_load_option(struct efi_variable *var, int match, u8 *buffer,
+validate_load_option(efi_char16_t *var_name, int match, u8 *buffer,
 		     unsigned long len)
 {
 	u16 filepathlength;
 	int i, desclength = 0, namelen;
 
-	namelen = ucs2_strnlen(var->VariableName, sizeof(var->VariableName));
+	namelen = ucs2_strnlen(var_name, EFI_VAR_NAME_LEN);
 
 	/* Either "Boot" or "Driver" followed by four digits of hex */
 	for (i = match; i < match+4; i++) {
-		if (var->VariableName[i] > 127 ||
-		    hex_to_bin(var->VariableName[i] & 0xff) < 0)
+		if (var_name[i] > 127 ||
+		    hex_to_bin(var_name[i] & 0xff) < 0)
 			return true;
 	}
 
@@ -132,12 +132,12 @@ validate_load_option(struct efi_variable *var, int match, u8 *buffer,
 	/*
 	 * And, finally, check the filepath
 	 */
-	return validate_device_path(var, match, buffer + desclength + 6,
+	return validate_device_path(var_name, match, buffer + desclength + 6,
 				    filepathlength);
 }
 
 static bool
-validate_uint16(struct efi_variable *var, int match, u8 *buffer,
+validate_uint16(efi_char16_t *var_name, int match, u8 *buffer,
 		unsigned long len)
 {
 	/* A single 16-bit integer */
@@ -148,7 +148,7 @@ validate_uint16(struct efi_variable *var, int match, u8 *buffer,
 }
 
 static bool
-validate_ascii_string(struct efi_variable *var, int match, u8 *buffer,
+validate_ascii_string(efi_char16_t *var_name, int match, u8 *buffer,
 		      unsigned long len)
 {
 	int i;
@@ -166,7 +166,7 @@ validate_ascii_string(struct efi_variable *var, int match, u8 *buffer,
 
 struct variable_validate {
 	char *name;
-	bool (*validate)(struct efi_variable *var, int match, u8 *data,
+	bool (*validate)(efi_char16_t *var_name, int match, u8 *data,
 			 unsigned long len);
 };
 
@@ -189,10 +189,10 @@ static const struct variable_validate variable_validate[] = {
 };
 
 bool
-efivar_validate(struct efi_variable *var, u8 *data, unsigned long len)
+efivar_validate(efi_char16_t *var_name, u8 *data, unsigned long len)
 {
 	int i;
-	u16 *unicode_name = var->VariableName;
+	u16 *unicode_name = var_name;
 
 	for (i = 0; variable_validate[i].validate != NULL; i++) {
 		const char *name = variable_validate[i].name;
@@ -208,7 +208,7 @@ efivar_validate(struct efi_variable *var, u8 *data, unsigned long len)
 
 			/* Wildcard in the matching name means we've matched */
 			if (c == '*')
-				return variable_validate[i].validate(var,
+				return variable_validate[i].validate(var_name,
 							     match, data, len);
 
 			/* Case sensitive match */
@@ -217,7 +217,7 @@ efivar_validate(struct efi_variable *var, u8 *data, unsigned long len)
 
 			/* Reached the end of the string while matching */
 			if (!c)
-				return variable_validate[i].validate(var,
+				return variable_validate[i].validate(var_name,
 							     match, data, len);
 		}
 	}
@@ -805,7 +805,7 @@ int efivar_entry_set_get_size(struct efivar_entry *entry, u32 attributes,
 
 	*set = false;
 
-	if (efivar_validate(&entry->var, data, *size) == false)
+	if (efivar_validate(name, data, *size) == false)
 		return -EINVAL;
 
 	/*
diff --git a/drivers/firmware/iscsi_ibft.c b/drivers/firmware/iscsi_ibft.c
index 3ee852c9925..071c2c969ee 100644
--- a/drivers/firmware/iscsi_ibft.c
+++ b/drivers/firmware/iscsi_ibft.c
@@ -756,6 +756,7 @@ static const struct {
 	 */
 	{ ACPI_SIG_IBFT },
 	{ "iBFT" },
+	{ "BIFT" },	/* Broadcom iSCSI Offload */
 };
 
 static void __init acpi_find_ibft_region(void)
diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index a86c49a605c..4a1b5113e52 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -56,6 +56,7 @@ config GPIO_ACPI
 	depends on ACPI
 
 config GPIOLIB_IRQCHIP
+	select IRQ_DOMAIN
 	bool
 
 config DEBUG_GPIO
@@ -243,6 +244,15 @@ config GPIO_OCTEON
 	  Say yes here to support the on-chip GPIO lines on the OCTEON
 	  family of SOCs.
 
+config GPIO_OMAP
+	bool "TI OMAP GPIO support" if COMPILE_TEST && !ARCH_OMAP2PLUS
+	default y if ARCH_OMAP
+	depends on ARM
+	select GENERIC_IRQ_CHIP
+	select GPIOLIB_IRQCHIP
+	help
+	  Say yes here to enable GPIO support for TI OMAP SoCs.
+
 config GPIO_PL061
 	bool "PrimeCell PL061 GPIO support"
 	depends on ARM_AMBA
@@ -259,7 +269,7 @@ config GPIO_PXA
 
 config GPIO_RCAR
 	tristate "Renesas R-Car GPIO"
-	depends on ARM
+	depends on ARM && (ARCH_SHMOBILE || COMPILE_TEST)
 	help
 	  Say yes here to support GPIO on Renesas R-Car SoCs.
 
@@ -510,6 +520,7 @@ config GPIO_PCA953X
 config GPIO_PCA953X_IRQ
 	bool "Interrupt controller support for PCA953x"
 	depends on GPIO_PCA953X=y
+	select GPIOLIB_IRQCHIP
 	help
 	  Say yes here to enable the pca953x to be used as an interrupt
 	  controller. It requires the driver to be built in the kernel.
@@ -579,6 +590,7 @@ config GPIO_STP_XWAY
 config GPIO_TC3589X
 	bool "TC3589X GPIOs"
 	depends on MFD_TC3589X
+	select GPIOLIB_IRQCHIP
 	help
 	  This enables support for the GPIOs found on the TC3589X
 	  I/O Expander.
@@ -699,13 +711,13 @@ config GPIO_AMD8111
 config GPIO_INTEL_MID
 	bool "Intel Mid GPIO support"
 	depends on PCI && X86
-	select IRQ_DOMAIN
+	select GPIOLIB_IRQCHIP
 	help
 	  Say Y here to support Intel Mid GPIO.
 
 config GPIO_PCH
 	tristate "Intel EG20T PCH/LAPIS Semiconductor IOH(ML7223/ML7831) GPIO"
-	depends on PCI && X86
+	depends on PCI && (X86_32 || COMPILE_TEST)
 	select GENERIC_IRQ_CHIP
 	help
 	  This driver is for PCH(Platform controller Hub) GPIO of Intel Topcliff
@@ -739,7 +751,7 @@ config GPIO_SODAVILLE
 
 config GPIO_TIMBERDALE
 	bool "Support for timberdale GPIO IP"
-	depends on MFD_TIMBERDALE && HAS_IOMEM
+	depends on MFD_TIMBERDALE
 	---help---
 	Add support for the GPIO IP in the timberdale FPGA.
 
diff --git a/drivers/gpio/Makefile b/drivers/gpio/Makefile
index 6309aff1d80..d10f6a9d875 100644
--- a/drivers/gpio/Makefile
+++ b/drivers/gpio/Makefile
@@ -58,7 +58,7 @@ obj-$(CONFIG_GPIO_MVEBU)        += gpio-mvebu.o
 obj-$(CONFIG_GPIO_MXC)		+= gpio-mxc.o
 obj-$(CONFIG_GPIO_MXS)		+= gpio-mxs.o
 obj-$(CONFIG_GPIO_OCTEON)	+= gpio-octeon.o
-obj-$(CONFIG_ARCH_OMAP)		+= gpio-omap.o
+obj-$(CONFIG_GPIO_OMAP)		+= gpio-omap.o
 obj-$(CONFIG_GPIO_PCA953X)	+= gpio-pca953x.o
 obj-$(CONFIG_GPIO_PCF857X)	+= gpio-pcf857x.o
 obj-$(CONFIG_GPIO_PCH)		+= gpio-pch.o
diff --git a/drivers/gpio/devres.c b/drivers/gpio/devres.c
index 307464fd015..65978cf85f7 100644
--- a/drivers/gpio/devres.c
+++ b/drivers/gpio/devres.c
@@ -52,6 +52,22 @@ struct gpio_desc *__must_check devm_gpiod_get(struct device *dev,
 EXPORT_SYMBOL(devm_gpiod_get);
 
 /**
+ * devm_gpiod_get_optional - Resource-managed gpiod_get_optional()
+ * @dev: GPIO consumer
+ * @con_id: function within the GPIO consumer
+ *
+ * Managed gpiod_get_optional(). GPIO descriptors returned from this function
+ * are automatically disposed on driver detach. See gpiod_get_optional() for
+ * detailed information about behavior and return values.
+ */
+struct gpio_desc *__must_check devm_gpiod_get_optional(struct device *dev,
+						       const char *con_id)
+{
+	return devm_gpiod_get_index_optional(dev, con_id, 0);
+}
+EXPORT_SYMBOL(devm_gpiod_get_optional);
+
+/**
  * devm_gpiod_get_index - Resource-managed gpiod_get_index()
  * @dev:	GPIO consumer
  * @con_id:	function within the GPIO consumer
@@ -87,6 +103,33 @@ struct gpio_desc *__must_check devm_gpiod_get_index(struct device *dev,
 EXPORT_SYMBOL(devm_gpiod_get_index);
 
 /**
+ * devm_gpiod_get_index_optional - Resource-managed gpiod_get_index_optional()
+ * @dev: GPIO consumer
+ * @con_id: function within the GPIO consumer
+ * @index: index of the GPIO to obtain in the consumer
+ *
+ * Managed gpiod_get_index_optional(). GPIO descriptors returned from this
+ * function are automatically disposed on driver detach. See
+ * gpiod_get_index_optional() for detailed information about behavior and
+ * return values.
+ */
+struct gpio_desc *__must_check devm_gpiod_get_index_optional(struct device *dev,
+							     const char *con_id,
+							     unsigned int index)
+{
+	struct gpio_desc *desc;
+
+	desc = devm_gpiod_get_index(dev, con_id, index);
+	if (IS_ERR(desc)) {
+		if (PTR_ERR(desc) == -ENOENT)
+			return NULL;
+	}
+
+	return desc;
+}
+EXPORT_SYMBOL(devm_gpiod_get_index_optional);
+
+/**
  * devm_gpiod_put - Resource-managed gpiod_put()
  * @desc:	GPIO descriptor to dispose of
  *
diff --git a/drivers/gpio/gpio-adp5520.c b/drivers/gpio/gpio-adp5520.c
index 613265944e2..f1ade8fa321 100644
--- a/drivers/gpio/gpio-adp5520.c
+++ b/drivers/gpio/gpio-adp5520.c
@@ -106,10 +106,8 @@ static int adp5520_gpio_probe(struct platform_device *pdev)
 	}
 
 	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
-	if (dev == NULL) {
-		dev_err(&pdev->dev, "failed to alloc memory\n");
+	if (dev == NULL)
 		return -ENOMEM;
-	}
 
 	dev->master = pdev->dev.parent;
 
diff --git a/drivers/gpio/gpio-adp5588.c b/drivers/gpio/gpio-adp5588.c
index d974020b78b..ef19bc33f2b 100644
--- a/drivers/gpio/gpio-adp5588.c
+++ b/drivers/gpio/gpio-adp5588.c
@@ -379,10 +379,8 @@ static int adp5588_gpio_probe(struct i2c_client *client,
 	}
 
 	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
-	if (dev == NULL) {
-		dev_err(&client->dev, "failed to alloc memory\n");
+	if (dev == NULL)
 		return -ENOMEM;
-	}
 
 	dev->client = client;
 
diff --git a/drivers/gpio/gpio-bt8xx.c b/drivers/gpio/gpio-bt8xx.c
index ecb3ca2d1d1..6557147d933 100644
--- a/drivers/gpio/gpio-bt8xx.c
+++ b/drivers/gpio/gpio-bt8xx.c
@@ -178,7 +178,7 @@ static int bt8xxgpio_probe(struct pci_dev *dev,
 	struct bt8xxgpio *bg;
 	int err;
 
-	bg = kzalloc(sizeof(*bg), GFP_KERNEL);
+	bg = devm_kzalloc(&dev->dev, sizeof(struct bt8xxgpio), GFP_KERNEL);
 	if (!bg)
 		return -ENOMEM;
 
@@ -188,9 +188,9 @@ static int bt8xxgpio_probe(struct pci_dev *dev,
 	err = pci_enable_device(dev);
 	if (err) {
 		printk(KERN_ERR "bt8xxgpio: Can't enable device.\n");
-		goto err_freebg;
+		return err;
 	}
-	if (!request_mem_region(pci_resource_start(dev, 0),
+	if (!devm_request_mem_region(&dev->dev, pci_resource_start(dev, 0),
 				pci_resource_len(dev, 0),
 				"bt8xxgpio")) {
 		printk(KERN_WARNING "bt8xxgpio: Can't request iomem (0x%llx).\n",
@@ -201,11 +201,11 @@ static int bt8xxgpio_probe(struct pci_dev *dev,
 	pci_set_master(dev);
 	pci_set_drvdata(dev, bg);
 
-	bg->mmio = ioremap(pci_resource_start(dev, 0), 0x1000);
+	bg->mmio = devm_ioremap(&dev->dev, pci_resource_start(dev, 0), 0x1000);
 	if (!bg->mmio) {
 		printk(KERN_ERR "bt8xxgpio: ioremap() failed\n");
 		err = -EIO;
-		goto err_release_mem;
+		goto err_disable;
 	}
 
 	/* Disable interrupts */
@@ -220,18 +220,13 @@ static int bt8xxgpio_probe(struct pci_dev *dev,
 	err = gpiochip_add(&bg->gpio);
 	if (err) {
 		printk(KERN_ERR "bt8xxgpio: Failed to register GPIOs\n");
-		goto err_release_mem;
+		goto err_disable;
 	}
 
 	return 0;
 
-err_release_mem:
-	release_mem_region(pci_resource_start(dev, 0),
-			   pci_resource_len(dev, 0));
 err_disable:
 	pci_disable_device(dev);
-err_freebg:
-	kfree(bg);
 
 	return err;
 }
@@ -250,8 +245,6 @@ static void bt8xxgpio_remove(struct pci_dev *pdev)
 	release_mem_region(pci_resource_start(pdev, 0),
 			   pci_resource_len(pdev, 0));
 	pci_disable_device(pdev);
-
-	kfree(bg);
 }
 
 #ifdef CONFIG_PM
diff --git a/drivers/gpio/gpio-davinci.c b/drivers/gpio/gpio-davinci.c
index 339f9dac591..9f0682534e2 100644
--- a/drivers/gpio/gpio-davinci.c
+++ b/drivers/gpio/gpio-davinci.c
@@ -230,10 +230,8 @@ static int davinci_gpio_probe(struct platform_device *pdev)
 	chips = devm_kzalloc(dev,
 			     ngpio * sizeof(struct davinci_gpio_controller),
 			     GFP_KERNEL);
-	if (!chips) {
-		dev_err(dev, "Memory allocation failed\n");
+	if (!chips)
 		return -ENOMEM;
-	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
diff --git a/drivers/gpio/gpio-dwapb.c b/drivers/gpio/gpio-dwapb.c
index ed5711f77e2..cd3b8143527 100644
--- a/drivers/gpio/gpio-dwapb.c
+++ b/drivers/gpio/gpio-dwapb.c
@@ -198,6 +198,8 @@ static int dwapb_irq_set_type(struct irq_data *d, u32 type)
 		break;
 	}
 
+	irq_setup_alt_chip(d, type);
+
 	writel(level, gpio->regs + GPIO_INTTYPE_LEVEL);
 	writel(polarity, gpio->regs + GPIO_INT_POLARITY);
 	spin_unlock_irqrestore(&bgc->lock, flags);
@@ -213,7 +215,7 @@ static void dwapb_configure_irqs(struct dwapb_gpio *gpio,
 	struct irq_chip_generic	*irq_gc;
 	unsigned int hwirq, ngpio = gc->ngpio;
 	struct irq_chip_type *ct;
-	int err, irq;
+	int err, irq, i;
 
 	irq = irq_of_parse_and_map(node, 0);
 	if (!irq) {
@@ -227,7 +229,7 @@ static void dwapb_configure_irqs(struct dwapb_gpio *gpio,
 	if (!gpio->domain)
 		return;
 
-	err = irq_alloc_domain_generic_chips(gpio->domain, ngpio, 1,
+	err = irq_alloc_domain_generic_chips(gpio->domain, ngpio, 2,
 					     "gpio-dwapb", handle_level_irq,
 					     IRQ_NOREQUEST, 0,
 					     IRQ_GC_INIT_NESTED_LOCK);
@@ -248,20 +250,24 @@ static void dwapb_configure_irqs(struct dwapb_gpio *gpio,
 	irq_gc->reg_base = gpio->regs;
 	irq_gc->private = gpio;
 
-	ct = irq_gc->chip_types;
-	ct->chip.irq_ack = irq_gc_ack_set_bit;
-	ct->chip.irq_mask = irq_gc_mask_set_bit;
-	ct->chip.irq_unmask = irq_gc_mask_clr_bit;
-	ct->chip.irq_set_type = dwapb_irq_set_type;
-	ct->chip.irq_enable = dwapb_irq_enable;
-	ct->chip.irq_disable = dwapb_irq_disable;
-	ct->chip.irq_request_resources = dwapb_irq_reqres;
-	ct->chip.irq_release_resources = dwapb_irq_relres;
-	ct->regs.ack = GPIO_PORTA_EOI;
-	ct->regs.mask = GPIO_INTMASK;
-
-	irq_setup_generic_chip(irq_gc, IRQ_MSK(port->bgc.gc.ngpio),
-			IRQ_GC_INIT_NESTED_LOCK, IRQ_NOREQUEST, 0);
+	for (i = 0; i < 2; i++) {
+		ct = &irq_gc->chip_types[i];
+		ct->chip.irq_ack = irq_gc_ack_set_bit;
+		ct->chip.irq_mask = irq_gc_mask_set_bit;
+		ct->chip.irq_unmask = irq_gc_mask_clr_bit;
+		ct->chip.irq_set_type = dwapb_irq_set_type;
+		ct->chip.irq_enable = dwapb_irq_enable;
+		ct->chip.irq_disable = dwapb_irq_disable;
+		ct->chip.irq_request_resources = dwapb_irq_reqres;
+		ct->chip.irq_release_resources = dwapb_irq_relres;
+		ct->regs.ack = GPIO_PORTA_EOI;
+		ct->regs.mask = GPIO_INTMASK;
+		ct->type = IRQ_TYPE_LEVEL_MASK;
+	}
+
+	irq_gc->chip_types[0].type = IRQ_TYPE_LEVEL_MASK;
+	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);
diff --git a/drivers/gpio/gpio-em.c b/drivers/gpio/gpio-em.c
index 8765bd6f48e..cde36054c38 100644
--- a/drivers/gpio/gpio-em.c
+++ b/drivers/gpio/gpio-em.c
@@ -212,7 +212,7 @@ static void __em_gio_set(struct gpio_chip *chip, unsigned int reg,
 {
 	/* upper 16 bits contains mask and lower 16 actual value */
 	em_gio_write(gpio_to_priv(chip), reg,
-		     (1 << (shift + 16)) | (value << shift));
+		     (BIT(shift + 16)) | (value << shift));
 }
 
 static void em_gio_set(struct gpio_chip *chip, unsigned offset, int value)
@@ -284,7 +284,6 @@ static int em_gio_probe(struct platform_device *pdev)
 
 	p = devm_kzalloc(&pdev->dev, sizeof(*p), GFP_KERNEL);
 	if (!p) {
-		dev_err(&pdev->dev, "failed to allocate driver data\n");
 		ret = -ENOMEM;
 		goto err0;
 	}
diff --git a/drivers/gpio/gpio-ep93xx.c b/drivers/gpio/gpio-ep93xx.c
index 80829f3c654..dcc2bb4074e 100644
--- a/drivers/gpio/gpio-ep93xx.c
+++ b/drivers/gpio/gpio-ep93xx.c
@@ -344,37 +344,24 @@ static int ep93xx_gpio_probe(struct platform_device *pdev)
 {
 	struct ep93xx_gpio *ep93xx_gpio;
 	struct resource *res;
-	void __iomem *mmio;
 	int i;
-	int ret;
+	struct device *dev = &pdev->dev;
 
-	ep93xx_gpio = kzalloc(sizeof(*ep93xx_gpio), GFP_KERNEL);
+	ep93xx_gpio = devm_kzalloc(dev, sizeof(struct ep93xx_gpio), GFP_KERNEL);
 	if (!ep93xx_gpio)
 		return -ENOMEM;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		ret = -ENXIO;
-		goto exit_free;
-	}
-
-	if (!request_mem_region(res->start, resource_size(res), pdev->name)) {
-		ret = -EBUSY;
-		goto exit_free;
-	}
-
-	mmio = ioremap(res->start, resource_size(res));
-	if (!mmio) {
-		ret = -ENXIO;
-		goto exit_release;
-	}
-	ep93xx_gpio->mmio_base = mmio;
+	ep93xx_gpio->mmio_base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(ep93xx_gpio->mmio_base))
+		return PTR_ERR(ep93xx_gpio->mmio_base);
 
 	for (i = 0; i < ARRAY_SIZE(ep93xx_gpio_banks); i++) {
 		struct bgpio_chip *bgc = &ep93xx_gpio->bgc[i];
 		struct ep93xx_gpio_bank *bank = &ep93xx_gpio_banks[i];
 
-		if (ep93xx_gpio_add_bank(bgc, &pdev->dev, mmio, bank))
+		if (ep93xx_gpio_add_bank(bgc, &pdev->dev,
+					 ep93xx_gpio->mmio_base, bank))
 			dev_warn(&pdev->dev, "Unable to add gpio bank %s\n",
 				bank->label);
 	}
@@ -382,13 +369,6 @@ static int ep93xx_gpio_probe(struct platform_device *pdev)
 	ep93xx_gpio_init_irq();
 
 	return 0;
-
-exit_release:
-	release_mem_region(res->start, resource_size(res));
-exit_free:
-	kfree(ep93xx_gpio);
-	dev_info(&pdev->dev, "%s failed with errno %d\n", __func__, ret);
-	return ret;
 }
 
 static struct platform_driver ep93xx_gpio_driver = {
diff --git a/drivers/gpio/gpio-ge.c b/drivers/gpio/gpio-ge.c
index 7b95a4a8318..1237a73c3c9 100644
--- a/drivers/gpio/gpio-ge.c
+++ b/drivers/gpio/gpio-ge.c
@@ -18,15 +18,9 @@
  */
 
 #include <linux/kernel.h>
-#include <linux/compiler.h>
-#include <linux/init.h>
 #include <linux/io.h>
-#include <linux/of.h>
 #include <linux/of_device.h>
-#include <linux/of_platform.h>
 #include <linux/of_gpio.h>
-#include <linux/gpio.h>
-#include <linux/slab.h>
 #include <linux/module.h>
 
 #define GEF_GPIO_DIRECT		0x00
@@ -39,28 +33,26 @@
 #define GEF_GPIO_OVERRUN	0x1C
 #define GEF_GPIO_MODE		0x20
 
-static void _gef_gpio_set(void __iomem *reg, unsigned int offset, int value)
+static void gef_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
+	struct of_mm_gpio_chip *mmchip = to_of_mm_gpio_chip(chip);
 	unsigned int data;
 
-	data = ioread32be(reg);
-	/* value: 0=low; 1=high */
-	if (value & 0x1)
-		data = data | (0x1 << offset);
+	data = ioread32be(mmchip->regs + GEF_GPIO_OUT);
+	if (value)
+		data = data | BIT(offset);
 	else
-		data = data & ~(0x1 << offset);
-
-	iowrite32be(data, reg);
+		data = data & ~BIT(offset);
+	iowrite32be(data, mmchip->regs + GEF_GPIO_OUT);
 }
 
-
 static int gef_gpio_dir_in(struct gpio_chip *chip, unsigned offset)
 {
 	unsigned int data;
 	struct of_mm_gpio_chip *mmchip = to_of_mm_gpio_chip(chip);
 
 	data = ioread32be(mmchip->regs + GEF_GPIO_DIRECT);
-	data = data | (0x1 << offset);
+	data = data | BIT(offset);
 	iowrite32be(data, mmchip->regs + GEF_GPIO_DIRECT);
 
 	return 0;
@@ -71,11 +63,11 @@ static int gef_gpio_dir_out(struct gpio_chip *chip, unsigned offset, int value)
 	unsigned int data;
 	struct of_mm_gpio_chip *mmchip = to_of_mm_gpio_chip(chip);
 
-	/* Set direction before switching to input */
-	_gef_gpio_set(mmchip->regs + GEF_GPIO_OUT, offset, value);
+	/* Set value before switching to output */
+	gef_gpio_set(mmchip->regs + GEF_GPIO_OUT, offset, value);
 
 	data = ioread32be(mmchip->regs + GEF_GPIO_DIRECT);
-	data = data & ~(0x1 << offset);
+	data = data & ~BIT(offset);
 	iowrite32be(data, mmchip->regs + GEF_GPIO_DIRECT);
 
 	return 0;
@@ -83,116 +75,56 @@ static int gef_gpio_dir_out(struct gpio_chip *chip, unsigned offset, int value)
 
 static int gef_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	unsigned int data;
-	int state = 0;
 	struct of_mm_gpio_chip *mmchip = to_of_mm_gpio_chip(chip);
 
-	data = ioread32be(mmchip->regs + GEF_GPIO_IN);
-	state = (int)((data >> offset) & 0x1);
-
-	return state;
+	return !!(ioread32be(mmchip->regs + GEF_GPIO_IN) & BIT(offset));
 }
 
-static void gef_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
-{
-	struct of_mm_gpio_chip *mmchip = to_of_mm_gpio_chip(chip);
-
-	_gef_gpio_set(mmchip->regs + GEF_GPIO_OUT, offset, value);
-}
+static const struct of_device_id gef_gpio_ids[] = {
+	{
+		.compatible	= "gef,sbc610-gpio",
+		.data		= (void *)19,
+	}, {
+		.compatible	= "gef,sbc310-gpio",
+		.data		= (void *)6,
+	}, {
+		.compatible	= "ge,imp3a-gpio",
+		.data		= (void *)16,
+	},
+	{ }
+};
+MODULE_DEVICE_TABLE(of, gef_gpio_ids);
 
-static int __init gef_gpio_init(void)
+static int __init gef_gpio_probe(struct platform_device *pdev)
 {
-	struct device_node *np;
-	int retval;
-	struct of_mm_gpio_chip *gef_gpio_chip;
-
-	for_each_compatible_node(np, NULL, "gef,sbc610-gpio") {
-
-		pr_debug("%s: Initialising GEF GPIO\n", np->full_name);
-
-		/* Allocate chip structure */
-		gef_gpio_chip = kzalloc(sizeof(*gef_gpio_chip), GFP_KERNEL);
-		if (!gef_gpio_chip) {
-			pr_err("%s: Unable to allocate structure\n",
-				np->full_name);
-			continue;
-		}
-
-		/* Setup pointers to chip functions */
-		gef_gpio_chip->gc.of_gpio_n_cells = 2;
-		gef_gpio_chip->gc.ngpio = 19;
-		gef_gpio_chip->gc.direction_input = gef_gpio_dir_in;
-		gef_gpio_chip->gc.direction_output = gef_gpio_dir_out;
-		gef_gpio_chip->gc.get = gef_gpio_get;
-		gef_gpio_chip->gc.set = gef_gpio_set;
-
-		/* This function adds a memory mapped GPIO chip */
-		retval = of_mm_gpiochip_add(np, gef_gpio_chip);
-		if (retval) {
-			kfree(gef_gpio_chip);
-			pr_err("%s: Unable to add GPIO\n", np->full_name);
-		}
-	}
-
-	for_each_compatible_node(np, NULL, "gef,sbc310-gpio") {
-
-		pr_debug("%s: Initialising GEF GPIO\n", np->full_name);
-
-		/* Allocate chip structure */
-		gef_gpio_chip = kzalloc(sizeof(*gef_gpio_chip), GFP_KERNEL);
-		if (!gef_gpio_chip) {
-			pr_err("%s: Unable to allocate structure\n",
-				np->full_name);
-			continue;
-		}
-
-		/* Setup pointers to chip functions */
-		gef_gpio_chip->gc.of_gpio_n_cells = 2;
-		gef_gpio_chip->gc.ngpio = 6;
-		gef_gpio_chip->gc.direction_input = gef_gpio_dir_in;
-		gef_gpio_chip->gc.direction_output = gef_gpio_dir_out;
-		gef_gpio_chip->gc.get = gef_gpio_get;
-		gef_gpio_chip->gc.set = gef_gpio_set;
-
-		/* This function adds a memory mapped GPIO chip */
-		retval = of_mm_gpiochip_add(np, gef_gpio_chip);
-		if (retval) {
-			kfree(gef_gpio_chip);
-			pr_err("%s: Unable to add GPIO\n", np->full_name);
-		}
-	}
-
-	for_each_compatible_node(np, NULL, "ge,imp3a-gpio") {
-
-		pr_debug("%s: Initialising GE GPIO\n", np->full_name);
-
-		/* Allocate chip structure */
-		gef_gpio_chip = kzalloc(sizeof(*gef_gpio_chip), GFP_KERNEL);
-		if (!gef_gpio_chip) {
-			pr_err("%s: Unable to allocate structure\n",
-				np->full_name);
-			continue;
-		}
-
-		/* Setup pointers to chip functions */
-		gef_gpio_chip->gc.of_gpio_n_cells = 2;
-		gef_gpio_chip->gc.ngpio = 16;
-		gef_gpio_chip->gc.direction_input = gef_gpio_dir_in;
-		gef_gpio_chip->gc.direction_output = gef_gpio_dir_out;
-		gef_gpio_chip->gc.get = gef_gpio_get;
-		gef_gpio_chip->gc.set = gef_gpio_set;
-
-		/* This function adds a memory mapped GPIO chip */
-		retval = of_mm_gpiochip_add(np, gef_gpio_chip);
-		if (retval) {
-			kfree(gef_gpio_chip);
-			pr_err("%s: Unable to add GPIO\n", np->full_name);
-		}
-	}
+	const struct of_device_id *of_id =
+		of_match_device(gef_gpio_ids, &pdev->dev);
+	struct of_mm_gpio_chip *mmchip;
+
+	mmchip = devm_kzalloc(&pdev->dev, sizeof(*mmchip), GFP_KERNEL);
+	if (!mmchip)
+		return -ENOMEM;
+
+	/* Setup pointers to chip functions */
+	mmchip->gc.ngpio = (u16)(uintptr_t)of_id->data;
+	mmchip->gc.of_gpio_n_cells = 2;
+	mmchip->gc.direction_input = gef_gpio_dir_in;
+	mmchip->gc.direction_output = gef_gpio_dir_out;
+	mmchip->gc.get = gef_gpio_get;
+	mmchip->gc.set = gef_gpio_set;
+
+	/* This function adds a memory mapped GPIO chip */
+	return of_mm_gpiochip_add(pdev->dev.of_node, mmchip);
+};
 
-	return 0;
+static struct platform_driver gef_gpio_driver = {
+	.driver = {
+		.name		= "gef-gpio",
+		.owner		= THIS_MODULE,
+		.of_match_table	= gef_gpio_ids,
+	},
 };
-arch_initcall(gef_gpio_init);
+module_platform_driver_probe(gef_gpio_driver, gef_gpio_probe);
 
 MODULE_DESCRIPTION("GE I/O FPGA GPIO driver");
 MODULE_AUTHOR("Martyn Welch <martyn.welch@ge.com");
diff --git a/drivers/gpio/gpio-generic.c b/drivers/gpio/gpio-generic.c
index b5dff9e742f..fea8c82bb8f 100644
--- a/drivers/gpio/gpio-generic.c
+++ b/drivers/gpio/gpio-generic.c
@@ -388,6 +388,14 @@ static int bgpio_setup_direction(struct bgpio_chip *bgc,
 	return 0;
 }
 
+static int bgpio_request(struct gpio_chip *chip, unsigned gpio_pin)
+{
+	if (gpio_pin < chip->ngpio)
+		return 0;
+
+	return -EINVAL;
+}
+
 int bgpio_remove(struct bgpio_chip *bgc)
 {
 	return gpiochip_remove(&bgc->gc);
@@ -413,6 +421,7 @@ int bgpio_init(struct bgpio_chip *bgc, struct device *dev,
 	bgc->gc.label = dev_name(dev);
 	bgc->gc.base = -1;
 	bgc->gc.ngpio = bgc->bits;
+	bgc->gc.request = bgpio_request;
 
 	ret = bgpio_setup_io(bgc, dat, set, clr);
 	if (ret)
diff --git a/drivers/gpio/gpio-grgpio.c b/drivers/gpio/gpio-grgpio.c
index 84d2478ec29..3c3f515b791 100644
--- a/drivers/gpio/gpio-grgpio.c
+++ b/drivers/gpio/gpio-grgpio.c
@@ -481,7 +481,7 @@ out:
 	return ret;
 }
 
-static struct of_device_id grgpio_match[] = {
+static const struct of_device_id grgpio_match[] = {
 	{.name = "GAISLER_GPIO"},
 	{.name = "01_01a"},
 	{},
diff --git a/drivers/gpio/gpio-ich.c b/drivers/gpio/gpio-ich.c
index e73c6755a5e..70304220a47 100644
--- a/drivers/gpio/gpio-ich.c
+++ b/drivers/gpio/gpio-ich.c
@@ -305,6 +305,8 @@ static struct ichx_desc ich6_desc = {
 
 	.ngpio = 50,
 	.have_blink = true,
+	.regs = ichx_regs,
+	.reglen = ichx_reglen,
 };
 
 /* Intel 3100 */
@@ -324,6 +326,8 @@ static struct ichx_desc i3100_desc = {
 	.uses_gpe0 = true,
 
 	.ngpio = 50,
+	.regs = ichx_regs,
+	.reglen = ichx_reglen,
 };
 
 /* ICH7 and ICH8-based */
diff --git a/drivers/gpio/gpio-janz-ttl.c b/drivers/gpio/gpio-janz-ttl.c
index 2ecd3a09c74..42852eaaf02 100644
--- a/drivers/gpio/gpio-janz-ttl.c
+++ b/drivers/gpio/gpio-janz-ttl.c
@@ -152,34 +152,21 @@ static int ttl_probe(struct platform_device *pdev)
 	pdata = dev_get_platdata(&pdev->dev);
 	if (!pdata) {
 		dev_err(dev, "no platform data\n");
-		ret = -ENXIO;
-		goto out_return;
+		return -ENXIO;
 	}
 
-	mod = kzalloc(sizeof(*mod), GFP_KERNEL);
-	if (!mod) {
-		dev_err(dev, "unable to allocate private data\n");
-		ret = -ENOMEM;
-		goto out_return;
-	}
+	mod = devm_kzalloc(dev, sizeof(*mod), GFP_KERNEL);
+	if (!mod)
+		return -ENOMEM;
 
 	platform_set_drvdata(pdev, mod);
 	spin_lock_init(&mod->lock);
 
 	/* get access to the MODULbus registers for this module */
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(dev, "MODULbus registers not found\n");
-		ret = -ENODEV;
-		goto out_free_mod;
-	}
-
-	mod->regs = ioremap(res->start, resource_size(res));
-	if (!mod->regs) {
-		dev_err(dev, "MODULbus registers not ioremap\n");
-		ret = -ENOMEM;
-		goto out_free_mod;
-	}
+	mod->regs = devm_ioremap_resource(dev, res);
+	if (IS_ERR(mod->regs))
+		return PTR_ERR(mod->regs);
 
 	ttl_setup_device(mod);
 
@@ -198,17 +185,10 @@ static int ttl_probe(struct platform_device *pdev)
 	ret = gpiochip_add(gpio);
 	if (ret) {
 		dev_err(dev, "unable to add GPIO chip\n");
-		goto out_iounmap_regs;
+		return ret;
 	}
 
 	return 0;
-
-out_iounmap_regs:
-	iounmap(mod->regs);
-out_free_mod:
-	kfree(mod);
-out_return:
-	return ret;
 }
 
 static int ttl_remove(struct platform_device *pdev)
@@ -223,8 +203,6 @@ static int ttl_remove(struct platform_device *pdev)
 		return ret;
 	}
 
-	iounmap(mod->regs);
-	kfree(mod);
 	return 0;
 }
 
diff --git a/drivers/gpio/gpio-kempld.c b/drivers/gpio/gpio-kempld.c
index c6d88173f5a..1e5e51987d3 100644
--- a/drivers/gpio/gpio-kempld.c
+++ b/drivers/gpio/gpio-kempld.c
@@ -24,7 +24,7 @@
 #include <linux/mfd/kempld.h>
 
 #define KEMPLD_GPIO_MAX_NUM		16
-#define KEMPLD_GPIO_MASK(x)		(1 << ((x) % 8))
+#define KEMPLD_GPIO_MASK(x)		(BIT((x) % 8))
 #define KEMPLD_GPIO_DIR_NUM(x)		(0x40 + (x) / 8)
 #define KEMPLD_GPIO_LVL_NUM(x)		(0x42 + (x) / 8)
 #define KEMPLD_GPIO_EVT_LVL_EDGE	0x46
@@ -216,4 +216,4 @@ module_platform_driver(kempld_gpio_driver);
 MODULE_DESCRIPTION("KEM PLD GPIO Driver");
 MODULE_AUTHOR("Michael Brunner <michael.brunner@kontron.com>");
 MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:gpio-kempld");
+MODULE_ALIAS("platform:kempld-gpio");
diff --git a/drivers/gpio/gpio-lynxpoint.c b/drivers/gpio/gpio-lynxpoint.c
index 9a82a9074a2..2bea89b7250 100644
--- a/drivers/gpio/gpio-lynxpoint.c
+++ b/drivers/gpio/gpio-lynxpoint.c
@@ -375,10 +375,8 @@ static int lp_gpio_probe(struct platform_device *pdev)
 	int ret = -ENODEV;
 
 	lg = devm_kzalloc(dev, sizeof(struct lp_gpio), GFP_KERNEL);
-	if (!lg) {
-		dev_err(dev, "can't allocate lp_gpio chip data\n");
+	if (!lg)
 		return -ENOMEM;
-	}
 
 	lg->pdev = pdev;
 	platform_set_drvdata(pdev, lg);
diff --git a/drivers/gpio/gpio-max730x.c b/drivers/gpio/gpio-max730x.c
index 8672755f95c..0814584fcdc 100644
--- a/drivers/gpio/gpio-max730x.c
+++ b/drivers/gpio/gpio-max730x.c
@@ -237,10 +237,9 @@ int __max730x_remove(struct device *dev)
 	ts->write(dev, 0x04, 0x00);
 
 	ret = gpiochip_remove(&ts->chip);
-	if (!ret) {
+	if (!ret)
 		mutex_destroy(&ts->lock);
-		kfree(ts);
-	} else
+	else
 		dev_err(dev, "Failed to remove GPIO controller: %d\n", ret);
 
 	return ret;
diff --git a/drivers/gpio/gpio-mcp23s08.c b/drivers/gpio/gpio-mcp23s08.c
index 99a68310e7c..fe7c0e211f9 100644
--- a/drivers/gpio/gpio-mcp23s08.c
+++ b/drivers/gpio/gpio-mcp23s08.c
@@ -714,7 +714,7 @@ fail:
 
 #ifdef CONFIG_OF
 #ifdef CONFIG_SPI_MASTER
-static struct of_device_id mcp23s08_spi_of_match[] = {
+static const struct of_device_id mcp23s08_spi_of_match[] = {
 	{
 		.compatible = "microchip,mcp23s08",
 		.data = (void *) MCP_TYPE_S08,
@@ -738,7 +738,7 @@ MODULE_DEVICE_TABLE(of, mcp23s08_spi_of_match);
 #endif
 
 #if IS_ENABLED(CONFIG_I2C)
-static struct of_device_id mcp23s08_i2c_of_match[] = {
+static const struct of_device_id mcp23s08_i2c_of_match[] = {
 	{
 		.compatible = "microchip,mcp23008",
 		.data = (void *) MCP_TYPE_008,
@@ -867,7 +867,7 @@ static int mcp23s08_probe(struct spi_device *spi)
 {
 	struct mcp23s08_platform_data	*pdata;
 	unsigned			addr;
-	unsigned			chips = 0;
+	int				chips = 0;
 	struct mcp23s08_driver_data	*data;
 	int				status, type;
 	unsigned			base = -1,
@@ -895,8 +895,13 @@ static int mcp23s08_probe(struct spi_device *spi)
 			return -ENODEV;
 		}
 
-		for (addr = 0; addr < ARRAY_SIZE(pdata->chip); addr++)
+		for (addr = 0; addr < ARRAY_SIZE(pdata->chip); addr++) {
 			pullups[addr] = 0;
+			if (spi_present_mask & (1 << addr))
+				chips++;
+		}
+		if (!chips)
+			return -ENODEV;
 	} else {
 		type = spi_get_device_id(spi)->driver_data;
 		pdata = dev_get_platdata(&spi->dev);
@@ -919,12 +924,12 @@ static int mcp23s08_probe(struct spi_device *spi)
 			pullups[addr] = pdata->chip[addr].pullups;
 		}
 
-		if (!chips)
-			return -ENODEV;
-
 		base = pdata->base;
 	}
 
+	if (!chips)
+		return -ENODEV;
+
 	data = kzalloc(sizeof(*data) + chips * sizeof(struct mcp23s08),
 			GFP_KERNEL);
 	if (!data)
@@ -935,6 +940,10 @@ static int mcp23s08_probe(struct spi_device *spi)
 		if (!(spi_present_mask & (1 << addr)))
 			continue;
 		chips--;
+		if (chips < 0) {
+			dev_err(&spi->dev, "FATAL: invalid negative chip id\n");
+			goto fail;
+		}
 		data->mcp[addr] = &data->chip[chips];
 		status = mcp23s08_probe_one(data->mcp[addr], &spi->dev, spi,
 					    0x40 | (addr << 1), type, base,
diff --git a/drivers/gpio/gpio-moxart.c b/drivers/gpio/gpio-moxart.c
index ccd45704e5f..4661e181be0 100644
--- a/drivers/gpio/gpio-moxart.c
+++ b/drivers/gpio/gpio-moxart.c
@@ -113,10 +113,8 @@ static int moxart_gpio_probe(struct platform_device *pdev)
 	int ret;
 
 	mgc = devm_kzalloc(dev, sizeof(*mgc), GFP_KERNEL);
-	if (!mgc) {
-		dev_err(dev, "can't allocate GPIO chip container\n");
+	if (!mgc)
 		return -ENOMEM;
-	}
 	mgc->gpio = moxart_template_chip;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
diff --git a/drivers/gpio/gpio-mvebu.c b/drivers/gpio/gpio-mvebu.c
index d4250942239..418e3865036 100644
--- a/drivers/gpio/gpio-mvebu.c
+++ b/drivers/gpio/gpio-mvebu.c
@@ -535,7 +535,7 @@ static void mvebu_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 #define mvebu_gpio_dbg_show NULL
 #endif
 
-static struct of_device_id mvebu_gpio_of_match[] = {
+static const struct of_device_id mvebu_gpio_of_match[] = {
 	{
 		.compatible = "marvell,orion-gpio",
 		.data       = (void *) MVEBU_GPIO_SOC_VARIANT_ORION,
@@ -574,10 +574,8 @@ static int mvebu_gpio_probe(struct platform_device *pdev)
 		soc_variant = MVEBU_GPIO_SOC_VARIANT_ORION;
 
 	mvchip = devm_kzalloc(&pdev->dev, sizeof(struct mvebu_gpio_chip), GFP_KERNEL);
-	if (!mvchip) {
-		dev_err(&pdev->dev, "Cannot allocate memory\n");
+	if (!mvchip)
 		return -ENOMEM;
-	}
 
 	if (of_property_read_u32(pdev->dev.of_node, "ngpios", &ngpios)) {
 		dev_err(&pdev->dev, "Missing ngpios OF property\n");
@@ -738,9 +736,4 @@ static struct platform_driver mvebu_gpio_driver = {
 	},
 	.probe		= mvebu_gpio_probe,
 };
-
-static int __init mvebu_gpio_init(void)
-{
-	return platform_driver_register(&mvebu_gpio_driver);
-}
-postcore_initcall(mvebu_gpio_init);
+module_platform_driver(mvebu_gpio_driver);
diff --git a/drivers/gpio/gpio-omap.c b/drivers/gpio/gpio-omap.c
index 19b886c21b1..00f29aa1fb9 100644
--- a/drivers/gpio/gpio-omap.c
+++ b/drivers/gpio/gpio-omap.c
@@ -24,9 +24,9 @@
 #include <linux/pm.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
-#include <linux/irqdomain.h>
 #include <linux/irqchip/chained_irq.h>
 #include <linux/gpio.h>
+#include <linux/bitops.h>
 #include <linux/platform_data/gpio-omap.h>
 
 #define OFF_MODE	1
@@ -52,7 +52,6 @@ struct gpio_bank {
 	struct list_head node;
 	void __iomem *base;
 	u16 irq;
-	struct irq_domain *domain;
 	u32 non_wakeup_gpios;
 	u32 enabled_non_wakeup_gpios;
 	struct gpio_regs context;
@@ -84,22 +83,21 @@ struct gpio_bank {
 };
 
 #define GPIO_INDEX(bank, gpio) (gpio % bank->width)
-#define GPIO_BIT(bank, gpio) (1 << GPIO_INDEX(bank, gpio))
+#define GPIO_BIT(bank, gpio) (BIT(GPIO_INDEX(bank, gpio)))
 #define GPIO_MOD_CTRL_BIT	BIT(0)
 
 #define BANK_USED(bank) (bank->mod_usage || bank->irq_usage)
-#define LINE_USED(line, offset) (line & (1 << offset))
+#define LINE_USED(line, offset) (line & (BIT(offset)))
 
 static int irq_to_gpio(struct gpio_bank *bank, unsigned int gpio_irq)
 {
 	return bank->chip.base + gpio_irq;
 }
 
-static int omap_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
+static inline struct gpio_bank *_irq_data_get_bank(struct irq_data *d)
 {
-	struct gpio_bank *bank = container_of(chip, struct gpio_bank, chip);
-
-	return irq_find_mapping(bank->domain, offset);
+	struct gpio_chip *chip = irq_data_get_irq_chip_data(d);
+	return container_of(chip, struct gpio_bank, chip);
 }
 
 static void _set_gpio_direction(struct gpio_bank *bank, int gpio, int is_input)
@@ -110,9 +108,9 @@ static void _set_gpio_direction(struct gpio_bank *bank, int gpio, int is_input)
 	reg += bank->regs->direction;
 	l = readl_relaxed(reg);
 	if (is_input)
-		l |= 1 << gpio;
+		l |= BIT(gpio);
 	else
-		l &= ~(1 << gpio);
+		l &= ~(BIT(gpio));
 	writel_relaxed(l, reg);
 	bank->context.oe = l;
 }
@@ -155,14 +153,14 @@ static int _get_gpio_datain(struct gpio_bank *bank, int offset)
 {
 	void __iomem *reg = bank->base + bank->regs->datain;
 
-	return (readl_relaxed(reg) & (1 << offset)) != 0;
+	return (readl_relaxed(reg) & (BIT(offset))) != 0;
 }
 
 static int _get_gpio_dataout(struct gpio_bank *bank, int offset)
 {
 	void __iomem *reg = bank->base + bank->regs->dataout;
 
-	return (readl_relaxed(reg) & (1 << offset)) != 0;
+	return (readl_relaxed(reg) & (BIT(offset))) != 0;
 }
 
 static inline void _gpio_rmw(void __iomem *base, u32 reg, u32 mask, bool set)
@@ -180,7 +178,7 @@ static inline void _gpio_rmw(void __iomem *base, u32 reg, u32 mask, bool set)
 static inline void _gpio_dbck_enable(struct gpio_bank *bank)
 {
 	if (bank->dbck_enable_mask && !bank->dbck_enabled) {
-		clk_enable(bank->dbck);
+		clk_prepare_enable(bank->dbck);
 		bank->dbck_enabled = true;
 
 		writel_relaxed(bank->dbck_enable_mask,
@@ -198,7 +196,7 @@ static inline void _gpio_dbck_disable(struct gpio_bank *bank)
 		 */
 		writel_relaxed(0, bank->base + bank->regs->debounce_en);
 
-		clk_disable(bank->dbck);
+		clk_disable_unprepare(bank->dbck);
 		bank->dbck_enabled = false;
 	}
 }
@@ -231,7 +229,7 @@ static void _set_gpio_debounce(struct gpio_bank *bank, unsigned gpio,
 
 	l = GPIO_BIT(bank, gpio);
 
-	clk_enable(bank->dbck);
+	clk_prepare_enable(bank->dbck);
 	reg = bank->base + bank->regs->debounce;
 	writel_relaxed(debounce, reg);
 
@@ -245,7 +243,7 @@ static void _set_gpio_debounce(struct gpio_bank *bank, unsigned gpio,
 	bank->dbck_enable_mask = val;
 
 	writel_relaxed(val, reg);
-	clk_disable(bank->dbck);
+	clk_disable_unprepare(bank->dbck);
 	/*
 	 * Enable debounce clock per module.
 	 * This call is mandatory because in omap_gpio_request() when
@@ -290,7 +288,7 @@ static void _clear_gpio_debounce(struct gpio_bank *bank, unsigned gpio)
 		bank->context.debounce = 0;
 		writel_relaxed(bank->context.debounce, bank->base +
 			     bank->regs->debounce);
-		clk_disable(bank->dbck);
+		clk_disable_unprepare(bank->dbck);
 		bank->dbck_enabled = false;
 	}
 }
@@ -299,7 +297,7 @@ static inline void set_gpio_trigger(struct gpio_bank *bank, int gpio,
 						unsigned trigger)
 {
 	void __iomem *base = bank->base;
-	u32 gpio_bit = 1 << gpio;
+	u32 gpio_bit = BIT(gpio);
 
 	_gpio_rmw(base, bank->regs->leveldetect0, gpio_bit,
 		  trigger & IRQ_TYPE_LEVEL_LOW);
@@ -368,9 +366,9 @@ static void _toggle_gpio_edge_triggering(struct gpio_bank *bank, int gpio)
 
 	l = readl_relaxed(reg);
 	if ((l >> gpio) & 1)
-		l &= ~(1 << gpio);
+		l &= ~(BIT(gpio));
 	else
-		l |= 1 << gpio;
+		l |= BIT(gpio);
 
 	writel_relaxed(l, reg);
 }
@@ -392,11 +390,11 @@ static int _set_gpio_triggering(struct gpio_bank *bank, int gpio,
 
 		l = readl_relaxed(reg);
 		if ((trigger & IRQ_TYPE_SENSE_MASK) == IRQ_TYPE_EDGE_BOTH)
-			bank->toggle_mask |= 1 << gpio;
+			bank->toggle_mask |= BIT(gpio);
 		if (trigger & IRQ_TYPE_EDGE_RISING)
-			l |= 1 << gpio;
+			l |= BIT(gpio);
 		else if (trigger & IRQ_TYPE_EDGE_FALLING)
-			l &= ~(1 << gpio);
+			l &= ~(BIT(gpio));
 		else
 			return -EINVAL;
 
@@ -413,10 +411,10 @@ static int _set_gpio_triggering(struct gpio_bank *bank, int gpio,
 		if (trigger & IRQ_TYPE_EDGE_RISING)
 			l |= 2 << (gpio << 1);
 		if (trigger & IRQ_TYPE_EDGE_FALLING)
-			l |= 1 << (gpio << 1);
+			l |= BIT(gpio << 1);
 
 		/* Enable wake-up during idle for dynamic tick */
-		_gpio_rmw(base, bank->regs->wkup_en, 1 << gpio, trigger);
+		_gpio_rmw(base, bank->regs->wkup_en, BIT(gpio), trigger);
 		bank->context.wake_en =
 			readl_relaxed(bank->base + bank->regs->wkup_en);
 		writel_relaxed(l, reg);
@@ -430,7 +428,7 @@ static void _enable_gpio_module(struct gpio_bank *bank, unsigned offset)
 		void __iomem *reg = bank->base + bank->regs->pinctrl;
 
 		/* Claim the pin for MPU */
-		writel_relaxed(readl_relaxed(reg) | (1 << offset), reg);
+		writel_relaxed(readl_relaxed(reg) | (BIT(offset)), reg);
 	}
 
 	if (bank->regs->ctrl && !BANK_USED(bank)) {
@@ -453,7 +451,7 @@ static void _disable_gpio_module(struct gpio_bank *bank, unsigned offset)
 	    !LINE_USED(bank->mod_usage, offset) &&
 	    !LINE_USED(bank->irq_usage, offset)) {
 		/* Disable wake-up during idle for dynamic tick */
-		_gpio_rmw(base, bank->regs->wkup_en, 1 << offset, 0);
+		_gpio_rmw(base, bank->regs->wkup_en, BIT(offset), 0);
 		bank->context.wake_en =
 			readl_relaxed(bank->base + bank->regs->wkup_en);
 	}
@@ -479,7 +477,7 @@ static int gpio_is_input(struct gpio_bank *bank, int mask)
 
 static int gpio_irq_type(struct irq_data *d, unsigned type)
 {
-	struct gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	struct gpio_bank *bank = _irq_data_get_bank(d);
 	unsigned gpio = 0;
 	int retval;
 	unsigned long flags;
@@ -509,20 +507,12 @@ static int gpio_irq_type(struct irq_data *d, unsigned type)
 	if (!LINE_USED(bank->mod_usage, offset)) {
 		_enable_gpio_module(bank, offset);
 		_set_gpio_direction(bank, offset, 1);
-	} else if (!gpio_is_input(bank, 1 << offset)) {
+	} else if (!gpio_is_input(bank, BIT(offset))) {
 		spin_unlock_irqrestore(&bank->lock, flags);
 		return -EINVAL;
 	}
 
-	retval = gpio_lock_as_irq(&bank->chip, offset);
-	if (retval) {
-		dev_err(bank->dev, "unable to lock offset %d for IRQ\n",
-			offset);
-		spin_unlock_irqrestore(&bank->lock, flags);
-		return retval;
-	}
-
-	bank->irq_usage |= 1 << GPIO_INDEX(bank, gpio);
+	bank->irq_usage |= BIT(GPIO_INDEX(bank, gpio));
 	spin_unlock_irqrestore(&bank->lock, flags);
 
 	if (type & (IRQ_TYPE_LEVEL_LOW | IRQ_TYPE_LEVEL_HIGH))
@@ -559,7 +549,7 @@ static u32 _get_gpio_irqbank_mask(struct gpio_bank *bank)
 {
 	void __iomem *reg = bank->base;
 	u32 l;
-	u32 mask = (1 << bank->width) - 1;
+	u32 mask = (BIT(bank->width)) - 1;
 
 	reg += bank->regs->irqenable;
 	l = readl_relaxed(reg);
@@ -664,7 +654,7 @@ static void _reset_gpio(struct gpio_bank *bank, int gpio)
 /* Use disable_irq_wake() and enable_irq_wake() functions from drivers */
 static int gpio_wake_enable(struct irq_data *d, unsigned int enable)
 {
-	struct gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	struct gpio_bank *bank = _irq_data_get_bank(d);
 	unsigned int gpio = irq_to_gpio(bank, d->hwirq);
 
 	return _set_gpio_wakeup(bank, gpio, enable);
@@ -691,7 +681,7 @@ static int omap_gpio_request(struct gpio_chip *chip, unsigned offset)
 		_set_gpio_triggering(bank, offset, IRQ_TYPE_NONE);
 		_enable_gpio_module(bank, offset);
 	}
-	bank->mod_usage |= 1 << offset;
+	bank->mod_usage |= BIT(offset);
 	spin_unlock_irqrestore(&bank->lock, flags);
 
 	return 0;
@@ -703,7 +693,7 @@ static void omap_gpio_free(struct gpio_chip *chip, unsigned offset)
 	unsigned long flags;
 
 	spin_lock_irqsave(&bank->lock, flags);
-	bank->mod_usage &= ~(1 << offset);
+	bank->mod_usage &= ~(BIT(offset));
 	_disable_gpio_module(bank, offset);
 	_reset_gpio(bank, bank->chip.base + offset);
 	spin_unlock_irqrestore(&bank->lock, flags);
@@ -732,11 +722,12 @@ static void gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 	unsigned int bit;
 	struct gpio_bank *bank;
 	int unmasked = 0;
-	struct irq_chip *chip = irq_desc_get_chip(desc);
+	struct irq_chip *irqchip = irq_desc_get_chip(desc);
+	struct gpio_chip *chip = irq_get_handler_data(irq);
 
-	chained_irq_enter(chip, desc);
+	chained_irq_enter(irqchip, desc);
 
-	bank = irq_get_handler_data(irq);
+	bank = container_of(chip, struct gpio_bank, chip);
 	isr_reg = bank->base + bank->regs->irqstatus;
 	pm_runtime_get_sync(bank->dev);
 
@@ -764,7 +755,7 @@ static void gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 		configured, we could unmask GPIO bank interrupt immediately */
 		if (!level_mask && !unmasked) {
 			unmasked = 1;
-			chained_irq_exit(chip, desc);
+			chained_irq_exit(irqchip, desc);
 		}
 
 		if (!isr)
@@ -772,7 +763,7 @@ static void gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 
 		while (isr) {
 			bit = __ffs(isr);
-			isr &= ~(1 << bit);
+			isr &= ~(BIT(bit));
 
 			/*
 			 * Some chips can't respond to both rising and falling
@@ -781,10 +772,11 @@ static void gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 			 * to respond to the IRQ for the opposite direction.
 			 * This will be indicated in the bank toggle_mask.
 			 */
-			if (bank->toggle_mask & (1 << bit))
+			if (bank->toggle_mask & (BIT(bit)))
 				_toggle_gpio_edge_triggering(bank, bit);
 
-			generic_handle_irq(irq_find_mapping(bank->domain, bit));
+			generic_handle_irq(irq_find_mapping(bank->chip.irqdomain,
+							    bit));
 		}
 	}
 	/* if bank has any level sensitive GPIO pin interrupt
@@ -793,20 +785,20 @@ static void gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 	interrupt */
 exit:
 	if (!unmasked)
-		chained_irq_exit(chip, desc);
+		chained_irq_exit(irqchip, desc);
 	pm_runtime_put(bank->dev);
 }
 
 static void gpio_irq_shutdown(struct irq_data *d)
 {
-	struct gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	struct gpio_bank *bank = _irq_data_get_bank(d);
 	unsigned int gpio = irq_to_gpio(bank, d->hwirq);
 	unsigned long flags;
 	unsigned offset = GPIO_INDEX(bank, gpio);
 
 	spin_lock_irqsave(&bank->lock, flags);
 	gpio_unlock_as_irq(&bank->chip, offset);
-	bank->irq_usage &= ~(1 << offset);
+	bank->irq_usage &= ~(BIT(offset));
 	_disable_gpio_module(bank, offset);
 	_reset_gpio(bank, gpio);
 	spin_unlock_irqrestore(&bank->lock, flags);
@@ -821,7 +813,7 @@ static void gpio_irq_shutdown(struct irq_data *d)
 
 static void gpio_ack_irq(struct irq_data *d)
 {
-	struct gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	struct gpio_bank *bank = _irq_data_get_bank(d);
 	unsigned int gpio = irq_to_gpio(bank, d->hwirq);
 
 	_clear_gpio_irqstatus(bank, gpio);
@@ -829,7 +821,7 @@ static void gpio_ack_irq(struct irq_data *d)
 
 static void gpio_mask_irq(struct irq_data *d)
 {
-	struct gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	struct gpio_bank *bank = _irq_data_get_bank(d);
 	unsigned int gpio = irq_to_gpio(bank, d->hwirq);
 	unsigned long flags;
 
@@ -841,7 +833,7 @@ static void gpio_mask_irq(struct irq_data *d)
 
 static void gpio_unmask_irq(struct irq_data *d)
 {
-	struct gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	struct gpio_bank *bank = _irq_data_get_bank(d);
 	unsigned int gpio = irq_to_gpio(bank, d->hwirq);
 	unsigned int irq_mask = GPIO_BIT(bank, gpio);
 	u32 trigger = irqd_get_trigger_type(d);
@@ -936,6 +928,21 @@ static inline void mpuio_init(struct gpio_bank *bank)
 
 /*---------------------------------------------------------------------*/
 
+static int gpio_get_direction(struct gpio_chip *chip, unsigned offset)
+{
+	struct gpio_bank *bank;
+	unsigned long flags;
+	void __iomem *reg;
+	int dir;
+
+	bank = container_of(chip, struct gpio_bank, chip);
+	reg = bank->base + bank->regs->direction;
+	spin_lock_irqsave(&bank->lock, flags);
+	dir = !!(readl_relaxed(reg) & BIT(offset));
+	spin_unlock_irqrestore(&bank->lock, flags);
+	return dir;
+}
+
 static int gpio_input(struct gpio_chip *chip, unsigned offset)
 {
 	struct gpio_bank *bank;
@@ -954,7 +961,7 @@ static int gpio_get(struct gpio_chip *chip, unsigned offset)
 	u32 mask;
 
 	bank = container_of(chip, struct gpio_bank, chip);
-	mask = (1 << offset);
+	mask = (BIT(offset));
 
 	if (gpio_is_input(bank, mask))
 		return _get_gpio_datain(bank, offset);
@@ -1081,10 +1088,12 @@ omap_mpuio_alloc_gc(struct gpio_bank *bank, unsigned int irq_start,
 			       IRQ_NOREQUEST | IRQ_NOPROBE, 0);
 }
 
-static void omap_gpio_chip_init(struct gpio_bank *bank)
+static int omap_gpio_chip_init(struct gpio_bank *bank)
 {
 	int j;
 	static int gpio;
+	int irq_base = 0;
+	int ret;
 
 	/*
 	 * REVISIT eventually switch from OMAP-specific gpio structs
@@ -1092,12 +1101,12 @@ static void omap_gpio_chip_init(struct gpio_bank *bank)
 	 */
 	bank->chip.request = omap_gpio_request;
 	bank->chip.free = omap_gpio_free;
+	bank->chip.get_direction = gpio_get_direction;
 	bank->chip.direction_input = gpio_input;
 	bank->chip.get = gpio_get;
 	bank->chip.direction_output = gpio_output;
 	bank->chip.set_debounce = gpio_debounce;
 	bank->chip.set = gpio_set;
-	bank->chip.to_irq = omap_gpio_to_irq;
 	if (bank->is_mpuio) {
 		bank->chip.label = "mpuio";
 		if (bank->regs->wkup_en)
@@ -1110,22 +1119,48 @@ static void omap_gpio_chip_init(struct gpio_bank *bank)
 	}
 	bank->chip.ngpio = bank->width;
 
-	gpiochip_add(&bank->chip);
+	ret = gpiochip_add(&bank->chip);
+	if (ret) {
+		dev_err(bank->dev, "Could not register gpio chip %d\n", ret);
+		return ret;
+	}
+
+#ifdef CONFIG_ARCH_OMAP1
+	/*
+	 * REVISIT: Once we have OMAP1 supporting SPARSE_IRQ, we can drop
+	 * irq_alloc_descs() since a base IRQ offset will no longer be needed.
+	 */
+	irq_base = irq_alloc_descs(-1, 0, bank->width, 0);
+	if (irq_base < 0) {
+		dev_err(bank->dev, "Couldn't allocate IRQ numbers\n");
+		return -ENODEV;
+	}
+#endif
+
+	ret = gpiochip_irqchip_add(&bank->chip, &gpio_irq_chip,
+				   irq_base, 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);
+		return -ENODEV;
+	}
+
+	gpiochip_set_chained_irqchip(&bank->chip, &gpio_irq_chip,
+				     bank->irq, gpio_irq_handler);
 
 	for (j = 0; j < bank->width; j++) {
-		int irq = irq_create_mapping(bank->domain, j);
+		int irq = irq_find_mapping(bank->chip.irqdomain, j);
 		irq_set_lockdep_class(irq, &gpio_lock_class);
-		irq_set_chip_data(irq, bank);
 		if (bank->is_mpuio) {
 			omap_mpuio_alloc_gc(bank, irq, bank->width);
-		} else {
-			irq_set_chip_and_handler(irq, &gpio_irq_chip,
-						 handle_simple_irq);
-			set_irq_flags(irq, IRQF_VALID);
+			irq_set_chip_and_handler(irq, NULL, NULL);
+			set_irq_flags(irq, 0);
 		}
 	}
-	irq_set_chained_handler(bank->irq, gpio_irq_handler);
-	irq_set_handler_data(bank->irq, bank);
+
+	return 0;
 }
 
 static const struct of_device_id omap_gpio_match[];
@@ -1138,9 +1173,7 @@ static int omap_gpio_probe(struct platform_device *pdev)
 	const struct omap_gpio_platform_data *pdata;
 	struct resource *res;
 	struct gpio_bank *bank;
-#ifdef CONFIG_ARCH_OMAP1
-	int irq_base;
-#endif
+	int ret;
 
 	match = of_match_device(of_match_ptr(omap_gpio_match), dev);
 
@@ -1162,6 +1195,7 @@ static int omap_gpio_probe(struct platform_device *pdev)
 
 	bank->irq = res->start;
 	bank->dev = dev;
+	bank->chip.dev = dev;
 	bank->dbck_flag = pdata->dbck_flag;
 	bank->stride = pdata->bank_stride;
 	bank->width = pdata->bank_width;
@@ -1182,29 +1216,6 @@ static int omap_gpio_probe(struct platform_device *pdev)
 				pdata->get_context_loss_count;
 	}
 
-#ifdef CONFIG_ARCH_OMAP1
-	/*
-	 * REVISIT: Once we have OMAP1 supporting SPARSE_IRQ, we can drop
-	 * irq_alloc_descs() and irq_domain_add_legacy() and just use a
-	 * linear IRQ domain mapping for all OMAP platforms.
-	 */
-	irq_base = irq_alloc_descs(-1, 0, bank->width, 0);
-	if (irq_base < 0) {
-		dev_err(dev, "Couldn't allocate IRQ numbers\n");
-		return -ENODEV;
-	}
-
-	bank->domain = irq_domain_add_legacy(node, bank->width, irq_base,
-					     0, &irq_domain_simple_ops, NULL);
-#else
-	bank->domain = irq_domain_add_linear(node, bank->width,
-					     &irq_domain_simple_ops, NULL);
-#endif
-	if (!bank->domain) {
-		dev_err(dev, "Couldn't register an IRQ domain\n");
-		return -ENODEV;
-	}
-
 	if (bank->regs->set_dataout && bank->regs->clr_dataout)
 		bank->set_dataout = _set_gpio_dataout_reg;
 	else
@@ -1216,7 +1227,7 @@ static int omap_gpio_probe(struct platform_device *pdev)
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	bank->base = devm_ioremap_resource(dev, res);
 	if (IS_ERR(bank->base)) {
-		irq_domain_remove(bank->domain);
+		irq_domain_remove(bank->chip.irqdomain);
 		return PTR_ERR(bank->base);
 	}
 
@@ -1230,7 +1241,11 @@ static int omap_gpio_probe(struct platform_device *pdev)
 		mpuio_init(bank);
 
 	omap_gpio_mod_init(bank);
-	omap_gpio_chip_init(bank);
+
+	ret = omap_gpio_chip_init(bank);
+	if (ret)
+		return ret;
+
 	omap_gpio_show_rev(bank);
 
 	pm_runtime_put(bank->dev);
diff --git a/drivers/gpio/gpio-palmas.c b/drivers/gpio/gpio-palmas.c
index da9d33252e5..86bdbe36206 100644
--- a/drivers/gpio/gpio-palmas.c
+++ b/drivers/gpio/gpio-palmas.c
@@ -148,7 +148,7 @@ static const struct palmas_device_data tps80036_dev_data = {
 	.ngpio = 16,
 };
 
-static struct of_device_id of_palmas_gpio_match[] = {
+static const struct of_device_id of_palmas_gpio_match[] = {
 	{ .compatible = "ti,palmas-gpio", .data = &palmas_dev_data,},
 	{ .compatible = "ti,tps65913-gpio", .data = &palmas_dev_data,},
 	{ .compatible = "ti,tps65914-gpio", .data = &palmas_dev_data,},
@@ -173,10 +173,8 @@ static int palmas_gpio_probe(struct platform_device *pdev)
 
 	palmas_gpio = devm_kzalloc(&pdev->dev,
 				sizeof(*palmas_gpio), GFP_KERNEL);
-	if (!palmas_gpio) {
-		dev_err(&pdev->dev, "Could not allocate palmas_gpio\n");
+	if (!palmas_gpio)
 		return -ENOMEM;
-	}
 
 	palmas_gpio->palmas = palmas;
 	palmas_gpio->gpio_chip.owner = THIS_MODULE;
diff --git a/drivers/gpio/gpio-pca953x.c b/drivers/gpio/gpio-pca953x.c
index d550d8e5870..e721a37c347 100644
--- a/drivers/gpio/gpio-pca953x.c
+++ b/drivers/gpio/gpio-pca953x.c
@@ -15,8 +15,6 @@
 #include <linux/init.h>
 #include <linux/gpio.h>
 #include <linux/interrupt.h>
-#include <linux/irq.h>
-#include <linux/irqdomain.h>
 #include <linux/i2c.h>
 #include <linux/platform_data/pca953x.h>
 #include <linux/slab.h>
@@ -91,7 +89,6 @@ struct pca953x_chip {
 	u8 irq_stat[MAX_BANK];
 	u8 irq_trig_raise[MAX_BANK];
 	u8 irq_trig_fall[MAX_BANK];
-	struct irq_domain *domain;
 #endif
 
 	struct i2c_client *client;
@@ -100,6 +97,11 @@ struct pca953x_chip {
 	int	chip_type;
 };
 
+static inline struct pca953x_chip *to_pca(struct gpio_chip *gc)
+{
+	return container_of(gc, struct pca953x_chip, gpio_chip);
+}
+
 static int pca953x_read_single(struct pca953x_chip *chip, int reg, u32 *val,
 				int off)
 {
@@ -202,12 +204,10 @@ static int pca953x_read_regs(struct pca953x_chip *chip, int reg, u8 *val)
 
 static int pca953x_gpio_direction_input(struct gpio_chip *gc, unsigned off)
 {
-	struct pca953x_chip *chip;
+	struct pca953x_chip *chip = to_pca(gc);
 	u8 reg_val;
 	int ret, offset = 0;
 
-	chip = container_of(gc, struct pca953x_chip, gpio_chip);
-
 	mutex_lock(&chip->i2c_lock);
 	reg_val = chip->reg_direction[off / BANK_SZ] | (1u << (off % BANK_SZ));
 
@@ -233,12 +233,10 @@ exit:
 static int pca953x_gpio_direction_output(struct gpio_chip *gc,
 		unsigned off, int val)
 {
-	struct pca953x_chip *chip;
+	struct pca953x_chip *chip = to_pca(gc);
 	u8 reg_val;
 	int ret, offset = 0;
 
-	chip = container_of(gc, struct pca953x_chip, gpio_chip);
-
 	mutex_lock(&chip->i2c_lock);
 	/* set output level */
 	if (val)
@@ -285,12 +283,10 @@ exit:
 
 static int pca953x_gpio_get_value(struct gpio_chip *gc, unsigned off)
 {
-	struct pca953x_chip *chip;
+	struct pca953x_chip *chip = to_pca(gc);
 	u32 reg_val;
 	int ret, offset = 0;
 
-	chip = container_of(gc, struct pca953x_chip, gpio_chip);
-
 	mutex_lock(&chip->i2c_lock);
 	switch (chip->chip_type) {
 	case PCA953X_TYPE:
@@ -315,12 +311,10 @@ static int pca953x_gpio_get_value(struct gpio_chip *gc, unsigned off)
 
 static void pca953x_gpio_set_value(struct gpio_chip *gc, unsigned off, int val)
 {
-	struct pca953x_chip *chip;
+	struct pca953x_chip *chip = to_pca(gc);
 	u8 reg_val;
 	int ret, offset = 0;
 
-	chip = container_of(gc, struct pca953x_chip, gpio_chip);
-
 	mutex_lock(&chip->i2c_lock);
 	if (val)
 		reg_val = chip->reg_output[off / BANK_SZ]
@@ -367,38 +361,34 @@ static void pca953x_setup_gpio(struct pca953x_chip *chip, int gpios)
 }
 
 #ifdef CONFIG_GPIO_PCA953X_IRQ
-static int pca953x_gpio_to_irq(struct gpio_chip *gc, unsigned off)
-{
-	struct pca953x_chip *chip;
-
-	chip = container_of(gc, struct pca953x_chip, gpio_chip);
-	return irq_create_mapping(chip->domain, off);
-}
-
 static void pca953x_irq_mask(struct irq_data *d)
 {
-	struct pca953x_chip *chip = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct pca953x_chip *chip = to_pca(gc);
 
 	chip->irq_mask[d->hwirq / BANK_SZ] &= ~(1 << (d->hwirq % BANK_SZ));
 }
 
 static void pca953x_irq_unmask(struct irq_data *d)
 {
-	struct pca953x_chip *chip = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct pca953x_chip *chip = to_pca(gc);
 
 	chip->irq_mask[d->hwirq / BANK_SZ] |= 1 << (d->hwirq % BANK_SZ);
 }
 
 static void pca953x_irq_bus_lock(struct irq_data *d)
 {
-	struct pca953x_chip *chip = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct pca953x_chip *chip = to_pca(gc);
 
 	mutex_lock(&chip->irq_lock);
 }
 
 static void pca953x_irq_bus_sync_unlock(struct irq_data *d)
 {
-	struct pca953x_chip *chip = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct pca953x_chip *chip = to_pca(gc);
 	u8 new_irqs;
 	int level, i;
 
@@ -420,7 +410,8 @@ static void pca953x_irq_bus_sync_unlock(struct irq_data *d)
 
 static int pca953x_irq_set_type(struct irq_data *d, unsigned int type)
 {
-	struct pca953x_chip *chip = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct pca953x_chip *chip = to_pca(gc);
 	int bank_nb = d->hwirq / BANK_SZ;
 	u8 mask = 1 << (d->hwirq % BANK_SZ);
 
@@ -503,44 +494,25 @@ static irqreturn_t pca953x_irq_handler(int irq, void *devid)
 	struct pca953x_chip *chip = devid;
 	u8 pending[MAX_BANK];
 	u8 level;
+	unsigned nhandled = 0;
 	int i;
 
 	if (!pca953x_irq_pending(chip, pending))
-		return IRQ_HANDLED;
+		return IRQ_NONE;
 
 	for (i = 0; i < NBANK(chip); i++) {
 		while (pending[i]) {
 			level = __ffs(pending[i]);
-			handle_nested_irq(irq_find_mapping(chip->domain,
+			handle_nested_irq(irq_find_mapping(chip->gpio_chip.irqdomain,
 							level + (BANK_SZ * i)));
 			pending[i] &= ~(1 << level);
+			nhandled++;
 		}
 	}
 
-	return IRQ_HANDLED;
+	return (nhandled > 0) ? IRQ_HANDLED : IRQ_NONE;
 }
 
-static int pca953x_gpio_irq_map(struct irq_domain *d, unsigned int irq,
-		       irq_hw_number_t hwirq)
-{
-	irq_clear_status_flags(irq, IRQ_NOREQUEST);
-	irq_set_chip_data(irq, d->host_data);
-	irq_set_chip(irq, &pca953x_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 pca953x_irq_simple_ops = {
-	.map = pca953x_gpio_irq_map,
-	.xlate = irq_domain_xlate_twocell,
-};
-
 static int pca953x_irq_setup(struct pca953x_chip *chip,
 			     const struct i2c_device_id *id,
 			     int irq_base)
@@ -572,19 +544,12 @@ static int pca953x_irq_setup(struct pca953x_chip *chip,
 			chip->irq_stat[i] &= chip->reg_direction[i];
 		mutex_init(&chip->irq_lock);
 
-		chip->domain = irq_domain_add_simple(client->dev.of_node,
-						chip->gpio_chip.ngpio,
-						irq_base,
-						&pca953x_irq_simple_ops,
-						chip);
-		if (!chip->domain)
-			return -ENODEV;
-
 		ret = devm_request_threaded_irq(&client->dev,
 					client->irq,
 					   NULL,
 					   pca953x_irq_handler,
-					   IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+					   IRQF_TRIGGER_LOW | IRQF_ONESHOT |
+						   IRQF_SHARED,
 					   dev_name(&client->dev), chip);
 		if (ret) {
 			dev_err(&client->dev, "failed to request irq %d\n",
@@ -592,7 +557,16 @@ static int pca953x_irq_setup(struct pca953x_chip *chip,
 			return ret;
 		}
 
-		chip->gpio_chip.to_irq = pca953x_gpio_to_irq;
+		ret =  gpiochip_irqchip_add(&chip->gpio_chip,
+					    &pca953x_irq_chip,
+					    irq_base,
+					    handle_simple_irq,
+					    IRQ_TYPE_NONE);
+		if (ret) {
+			dev_err(&client->dev,
+				"could not connect irqchip to gpiochip\n");
+			return ret;
+		}
 	}
 
 	return 0;
@@ -756,11 +730,11 @@ static int pca953x_probe(struct i2c_client *client,
 	if (ret)
 		return ret;
 
-	ret = pca953x_irq_setup(chip, id, irq_base);
+	ret = gpiochip_add(&chip->gpio_chip);
 	if (ret)
 		return ret;
 
-	ret = gpiochip_add(&chip->gpio_chip);
+	ret = pca953x_irq_setup(chip, id, irq_base);
 	if (ret)
 		return ret;
 
diff --git a/drivers/gpio/gpio-pcf857x.c b/drivers/gpio/gpio-pcf857x.c
index 82735822bc9..27b46751ea7 100644
--- a/drivers/gpio/gpio-pcf857x.c
+++ b/drivers/gpio/gpio-pcf857x.c
@@ -262,7 +262,7 @@ static int pcf857x_irq_domain_init(struct pcf857x *gpio,
 	/* enable real irq */
 	status = devm_request_threaded_irq(&client->dev, client->irq,
 				NULL, pcf857x_irq, IRQF_ONESHOT |
-				IRQF_TRIGGER_FALLING,
+				IRQF_TRIGGER_FALLING | IRQF_SHARED,
 				dev_name(&client->dev), gpio);
 
 	if (status)
@@ -319,7 +319,7 @@ static int pcf857x_probe(struct i2c_client *client,
 		status = pcf857x_irq_domain_init(gpio, client);
 		if (status < 0) {
 			dev_err(&client->dev, "irq_domain init failed\n");
-			goto fail;
+			goto fail_irq_domain;
 		}
 	}
 
@@ -414,12 +414,13 @@ static int pcf857x_probe(struct i2c_client *client,
 	return 0;
 
 fail:
-	dev_dbg(&client->dev, "probe error %d for '%s'\n",
-			status, client->name);
-
 	if (client->irq)
 		pcf857x_irq_domain_cleanup(gpio);
 
+fail_irq_domain:
+	dev_dbg(&client->dev, "probe error %d for '%s'\n",
+		status, client->name);
+
 	return status;
 }
 
diff --git a/drivers/gpio/gpio-pch.c b/drivers/gpio/gpio-pch.c
index 83a15639747..d6eac9b17db 100644
--- a/drivers/gpio/gpio-pch.c
+++ b/drivers/gpio/gpio-pch.c
@@ -20,6 +20,7 @@
 #include <linux/gpio.h>
 #include <linux/interrupt.h>
 #include <linux/irq.h>
+#include <linux/slab.h>
 
 #define PCH_EDGE_FALLING	0
 #define PCH_EDGE_RISING		BIT(0)
diff --git a/drivers/gpio/gpio-pl061.c b/drivers/gpio/gpio-pl061.c
index b0f475243ce..84b49cfb81a 100644
--- a/drivers/gpio/gpio-pl061.c
+++ b/drivers/gpio/gpio-pl061.c
@@ -17,7 +17,6 @@
 #include <linux/irq.h>
 #include <linux/irqchip/chained_irq.h>
 #include <linux/bitops.h>
-#include <linux/workqueue.h>
 #include <linux/gpio.h>
 #include <linux/device.h>
 #include <linux/amba/bus.h>
@@ -88,7 +87,7 @@ static int pl061_direction_input(struct gpio_chip *gc, unsigned offset)
 
 	spin_lock_irqsave(&chip->lock, flags);
 	gpiodir = readb(chip->base + GPIODIR);
-	gpiodir &= ~(1 << offset);
+	gpiodir &= ~(BIT(offset));
 	writeb(gpiodir, chip->base + GPIODIR);
 	spin_unlock_irqrestore(&chip->lock, flags);
 
@@ -106,16 +105,16 @@ static int pl061_direction_output(struct gpio_chip *gc, unsigned offset,
 		return -EINVAL;
 
 	spin_lock_irqsave(&chip->lock, flags);
-	writeb(!!value << offset, chip->base + (1 << (offset + 2)));
+	writeb(!!value << offset, chip->base + (BIT(offset + 2)));
 	gpiodir = readb(chip->base + GPIODIR);
-	gpiodir |= 1 << offset;
+	gpiodir |= BIT(offset);
 	writeb(gpiodir, chip->base + GPIODIR);
 
 	/*
 	 * gpio value is set again, because pl061 doesn't allow to set value of
 	 * a gpio pin before configuring it in OUT mode.
 	 */
-	writeb(!!value << offset, chip->base + (1 << (offset + 2)));
+	writeb(!!value << offset, chip->base + (BIT(offset + 2)));
 	spin_unlock_irqrestore(&chip->lock, flags);
 
 	return 0;
@@ -125,14 +124,14 @@ static int pl061_get_value(struct gpio_chip *gc, unsigned offset)
 {
 	struct pl061_gpio *chip = container_of(gc, struct pl061_gpio, gc);
 
-	return !!readb(chip->base + (1 << (offset + 2)));
+	return !!readb(chip->base + (BIT(offset + 2)));
 }
 
 static void pl061_set_value(struct gpio_chip *gc, unsigned offset, int value)
 {
 	struct pl061_gpio *chip = container_of(gc, struct pl061_gpio, gc);
 
-	writeb(!!value << offset, chip->base + (1 << (offset + 2)));
+	writeb(!!value << offset, chip->base + (BIT(offset + 2)));
 }
 
 static int pl061_irq_type(struct irq_data *d, unsigned trigger)
@@ -207,7 +206,7 @@ static void pl061_irq_mask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
 	struct pl061_gpio *chip = container_of(gc, struct pl061_gpio, gc);
-	u8 mask = 1 << (irqd_to_hwirq(d) % PL061_GPIO_NR);
+	u8 mask = BIT(irqd_to_hwirq(d) % PL061_GPIO_NR);
 	u8 gpioie;
 
 	spin_lock(&chip->lock);
@@ -220,7 +219,7 @@ static void pl061_irq_unmask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
 	struct pl061_gpio *chip = container_of(gc, struct pl061_gpio, gc);
-	u8 mask = 1 << (irqd_to_hwirq(d) % PL061_GPIO_NR);
+	u8 mask = BIT(irqd_to_hwirq(d) % PL061_GPIO_NR);
 	u8 gpioie;
 
 	spin_lock(&chip->lock);
@@ -302,9 +301,9 @@ static int pl061_probe(struct amba_device *adev, const struct amba_id *id)
 
 	for (i = 0; i < PL061_GPIO_NR; i++) {
 		if (pdata) {
-			if (pdata->directions & (1 << i))
+			if (pdata->directions & (BIT(i)))
 				pl061_direction_output(&chip->gc, i,
-						pdata->values & (1 << i));
+						pdata->values & (BIT(i)));
 			else
 				pl061_direction_input(&chip->gc, i);
 		}
@@ -331,7 +330,7 @@ static int pl061_suspend(struct device *dev)
 	chip->csave_regs.gpio_ie = readb(chip->base + GPIOIE);
 
 	for (offset = 0; offset < PL061_GPIO_NR; offset++) {
-		if (chip->csave_regs.gpio_dir & (1 << offset))
+		if (chip->csave_regs.gpio_dir & (BIT(offset)))
 			chip->csave_regs.gpio_data |=
 				pl061_get_value(&chip->gc, offset) << offset;
 	}
@@ -345,10 +344,10 @@ static int pl061_resume(struct device *dev)
 	int offset;
 
 	for (offset = 0; offset < PL061_GPIO_NR; offset++) {
-		if (chip->csave_regs.gpio_dir & (1 << offset))
+		if (chip->csave_regs.gpio_dir & (BIT(offset)))
 			pl061_direction_output(&chip->gc, offset,
 					chip->csave_regs.gpio_data &
-					(1 << offset));
+					(BIT(offset)));
 		else
 			pl061_direction_input(&chip->gc, offset);
 	}
diff --git a/drivers/gpio/gpio-rc5t583.c b/drivers/gpio/gpio-rc5t583.c
index 9b423173ab5..562b0c4d9cc 100644
--- a/drivers/gpio/gpio-rc5t583.c
+++ b/drivers/gpio/gpio-rc5t583.c
@@ -119,10 +119,8 @@ static int rc5t583_gpio_probe(struct platform_device *pdev)
 
 	rc5t583_gpio = devm_kzalloc(&pdev->dev, sizeof(*rc5t583_gpio),
 					GFP_KERNEL);
-	if (!rc5t583_gpio) {
-		dev_warn(&pdev->dev, "Mem allocation for rc5t583_gpio failed");
+	if (!rc5t583_gpio)
 		return -ENOMEM;
-	}
 
 	rc5t583_gpio->gpio_chip.label = "gpio-rc5t583",
 	rc5t583_gpio->gpio_chip.owner = THIS_MODULE,
diff --git a/drivers/gpio/gpio-rcar.c b/drivers/gpio/gpio-rcar.c
index 03c91482432..0c9f803fc1a 100644
--- a/drivers/gpio/gpio-rcar.c
+++ b/drivers/gpio/gpio-rcar.c
@@ -26,6 +26,7 @@
 #include <linux/pinctrl/consumer.h>
 #include <linux/platform_data/gpio-rcar.h>
 #include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
 #include <linux/spinlock.h>
 #include <linux/slab.h>
 
@@ -362,7 +363,6 @@ static int gpio_rcar_probe(struct platform_device *pdev)
 
 	p = devm_kzalloc(dev, sizeof(*p), GFP_KERNEL);
 	if (!p) {
-		dev_err(dev, "failed to allocate driver data\n");
 		ret = -ENOMEM;
 		goto err0;
 	}
@@ -377,6 +377,9 @@ static int gpio_rcar_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, p);
 
+	pm_runtime_enable(dev);
+	pm_runtime_get_sync(dev);
+
 	io = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 
@@ -460,6 +463,8 @@ static int gpio_rcar_probe(struct platform_device *pdev)
 err1:
 	irq_domain_remove(p->irq_domain);
 err0:
+	pm_runtime_put(dev);
+	pm_runtime_disable(dev);
 	return ret;
 }
 
@@ -473,6 +478,8 @@ static int gpio_rcar_remove(struct platform_device *pdev)
 		return ret;
 
 	irq_domain_remove(p->irq_domain);
+	pm_runtime_put(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
 	return 0;
 }
 
diff --git a/drivers/gpio/gpio-rdc321x.c b/drivers/gpio/gpio-rdc321x.c
index 88577c3272a..9fa7e53331c 100644
--- a/drivers/gpio/gpio-rdc321x.c
+++ b/drivers/gpio/gpio-rdc321x.c
@@ -141,17 +141,15 @@ static int rdc321x_gpio_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	rdc321x_gpio_dev = kzalloc(sizeof(struct rdc321x_gpio), GFP_KERNEL);
-	if (!rdc321x_gpio_dev) {
-		dev_err(&pdev->dev, "failed to allocate private data\n");
+	rdc321x_gpio_dev = devm_kzalloc(&pdev->dev, sizeof(struct rdc321x_gpio),
+					GFP_KERNEL);
+	if (!rdc321x_gpio_dev)
 		return -ENOMEM;
-	}
 
 	r = platform_get_resource_byname(pdev, IORESOURCE_IO, "gpio-reg1");
 	if (!r) {
 		dev_err(&pdev->dev, "failed to get gpio-reg1 resource\n");
-		err = -ENODEV;
-		goto out_free;
+		return -ENODEV;
 	}
 
 	spin_lock_init(&rdc321x_gpio_dev->lock);
@@ -162,8 +160,7 @@ static int rdc321x_gpio_probe(struct platform_device *pdev)
 	r = platform_get_resource_byname(pdev, IORESOURCE_IO, "gpio-reg2");
 	if (!r) {
 		dev_err(&pdev->dev, "failed to get gpio-reg2 resource\n");
-		err = -ENODEV;
-		goto out_free;
+		return -ENODEV;
 	}
 
 	rdc321x_gpio_dev->reg2_ctrl_base = r->start;
@@ -187,21 +184,17 @@ static int rdc321x_gpio_probe(struct platform_device *pdev)
 					rdc321x_gpio_dev->reg1_data_base,
 					&rdc321x_gpio_dev->data_reg[0]);
 	if (err)
-		goto out_free;
+		return err;
 
 	err = pci_read_config_dword(rdc321x_gpio_dev->sb_pdev,
 					rdc321x_gpio_dev->reg2_data_base,
 					&rdc321x_gpio_dev->data_reg[1]);
 	if (err)
-		goto out_free;
+		return err;
 
 	dev_info(&pdev->dev, "registering %d GPIOs\n",
 					rdc321x_gpio_dev->chip.ngpio);
 	return gpiochip_add(&rdc321x_gpio_dev->chip);
-
-out_free:
-	kfree(rdc321x_gpio_dev);
-	return err;
 }
 
 static int rdc321x_gpio_remove(struct platform_device *pdev)
@@ -213,8 +206,6 @@ static int rdc321x_gpio_remove(struct platform_device *pdev)
 	if (ret)
 		dev_err(&pdev->dev, "failed to unregister chip\n");
 
-	kfree(rdc321x_gpio_dev);
-
 	return ret;
 }
 
diff --git a/drivers/gpio/gpio-sch.c b/drivers/gpio/gpio-sch.c
index 5af65719b95..a9b1cd16c84 100644
--- a/drivers/gpio/gpio-sch.c
+++ b/drivers/gpio/gpio-sch.c
@@ -97,8 +97,6 @@ static int sch_gpio_core_direction_out(struct gpio_chip *gc,
 	u8 curr_dirs;
 	unsigned short offset, bit;
 
-	sch_gpio_core_set(gc, gpio_num, val);
-
 	spin_lock(&gpio_lock);
 
 	offset = CGIO + gpio_num / 8;
@@ -109,6 +107,17 @@ static int sch_gpio_core_direction_out(struct gpio_chip *gc,
 		outb(curr_dirs & ~(1 << bit), gpio_ba + offset);
 
 	spin_unlock(&gpio_lock);
+
+	/*
+	 * according to the datasheet, writing to the level register has no
+	 * effect when GPIO is programmed as input.
+	 * Actually the the level register is read-only when configured as input.
+	 * Thus presetting the output level before switching to output is _NOT_ possible.
+	 * Hence we set the level after configuring the GPIO as output.
+	 * But we cannot prevent a short low pulse if direction is set to high
+	 * and an external pull-up is connected.
+	 */
+	sch_gpio_core_set(gc, gpio_num, val);
 	return 0;
 }
 
@@ -178,8 +187,6 @@ static int sch_gpio_resume_direction_out(struct gpio_chip *gc,
 	u8 curr_dirs;
 	unsigned short offset, bit;
 
-	sch_gpio_resume_set(gc, gpio_num, val);
-
 	offset = RGIO + gpio_num / 8;
 	bit = gpio_num % 8;
 
@@ -190,6 +197,17 @@ static int sch_gpio_resume_direction_out(struct gpio_chip *gc,
 		outb(curr_dirs & ~(1 << bit), gpio_ba + offset);
 
 	spin_unlock(&gpio_lock);
+
+	/*
+	* according to the datasheet, writing to the level register has no
+	* effect when GPIO is programmed as input.
+	* Actually the the level register is read-only when configured as input.
+	* Thus presetting the output level before switching to output is _NOT_ possible.
+	* Hence we set the level after configuring the GPIO as output.
+	* But we cannot prevent a short low pulse if direction is set to high
+	* and an external pull-up is connected.
+	*/
+	sch_gpio_resume_set(gc, gpio_num, val);
 	return 0;
 }
 
diff --git a/drivers/gpio/gpio-sch311x.c b/drivers/gpio/gpio-sch311x.c
index 0357387b364..f942b80ee40 100644
--- a/drivers/gpio/gpio-sch311x.c
+++ b/drivers/gpio/gpio-sch311x.c
@@ -327,14 +327,22 @@ static int __init sch311x_detect(int sio_config_port, unsigned short *addr)
 	if (err)
 		return err;
 
-	/* Check device ID. We currently know about:
-	 * SCH3112 (0x7c), SCH3114 (0x7d), and SCH3116 (0x7f). */
+	/* Check device ID. */
 	reg = sch311x_sio_inb(sio_config_port, 0x20);
-	if (!(reg == 0x7c || reg == 0x7d || reg == 0x7f)) {
+	switch (reg) {
+	case 0x7c: /* SCH3112 */
+		dev_id = 2;
+		break;
+	case 0x7d: /* SCH3114 */
+		dev_id = 4;
+		break;
+	case 0x7f: /* SCH3116 */
+		dev_id = 6;
+		break;
+	default:
 		err = -ENODEV;
 		goto exit;
 	}
-	dev_id = reg == 0x7c ? 2 : reg == 0x7d ? 4 : 6;
 
 	/* Select logical device A (runtime registers) */
 	sch311x_sio_outb(sio_config_port, 0x07, 0x0a);
diff --git a/drivers/gpio/gpio-spear-spics.c b/drivers/gpio/gpio-spear-spics.c
index 30bcc539425..353263c85d2 100644
--- a/drivers/gpio/gpio-spear-spics.c
+++ b/drivers/gpio/gpio-spear-spics.c
@@ -129,10 +129,8 @@ static int spics_gpio_probe(struct platform_device *pdev)
 	int ret;
 
 	spics = devm_kzalloc(&pdev->dev, sizeof(*spics), GFP_KERNEL);
-	if (!spics) {
-		dev_err(&pdev->dev, "memory allocation fail\n");
+	if (!spics)
 		return -ENOMEM;
-	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	spics->base = devm_ioremap_resource(&pdev->dev, res);
diff --git a/drivers/gpio/gpio-stmpe.c b/drivers/gpio/gpio-stmpe.c
index 2776a09bee5..628b5849429 100644
--- a/drivers/gpio/gpio-stmpe.c
+++ b/drivers/gpio/gpio-stmpe.c
@@ -23,7 +23,8 @@
 enum { REG_RE, REG_FE, REG_IE };
 
 #define CACHE_NR_REGS	3
-#define CACHE_NR_BANKS	(STMPE_NR_GPIOS / 8)
+/* No variant has more than 24 GPIOs */
+#define CACHE_NR_BANKS	(24 / 8)
 
 struct stmpe_gpio {
 	struct gpio_chip chip;
@@ -31,8 +32,6 @@ struct stmpe_gpio {
 	struct device *dev;
 	struct mutex irq_lock;
 	struct irq_domain *domain;
-
-	int irq_base;
 	unsigned norequest_mask;
 
 	/* Caches of interrupt control registers for bus_lock */
@@ -311,13 +310,8 @@ static const struct irq_domain_ops stmpe_gpio_irq_simple_ops = {
 static int stmpe_gpio_irq_init(struct stmpe_gpio *stmpe_gpio,
 		struct device_node *np)
 {
-	int base = 0;
-
-	if (!np)
-		base = stmpe_gpio->irq_base;
-
 	stmpe_gpio->domain = irq_domain_add_simple(np,
-				stmpe_gpio->chip.ngpio, base,
+				stmpe_gpio->chip.ngpio, 0,
 				&stmpe_gpio_irq_simple_ops, stmpe_gpio);
 	if (!stmpe_gpio->domain) {
 		dev_err(stmpe_gpio->dev, "failed to create irqdomain\n");
@@ -354,7 +348,7 @@ static int stmpe_gpio_probe(struct platform_device *pdev)
 #ifdef CONFIG_OF
 	stmpe_gpio->chip.of_node = np;
 #endif
-	stmpe_gpio->chip.base = pdata ? pdata->gpio_base : -1;
+	stmpe_gpio->chip.base = -1;
 
 	if (pdata)
 		stmpe_gpio->norequest_mask = pdata->norequest_mask;
@@ -362,9 +356,7 @@ static int stmpe_gpio_probe(struct platform_device *pdev)
 		of_property_read_u32(np, "st,norequest-mask",
 				&stmpe_gpio->norequest_mask);
 
-	if (irq >= 0)
-		stmpe_gpio->irq_base = stmpe->irq_base + STMPE_INT_GPIO(0);
-	else
+	if (irq < 0)
 		dev_info(&pdev->dev,
 			"device configured in no-irq mode; "
 			"irqs are not available\n");
diff --git a/drivers/gpio/gpio-sx150x.c b/drivers/gpio/gpio-sx150x.c
index 13d73fb2b5e..b51ca9f5c14 100644
--- a/drivers/gpio/gpio-sx150x.c
+++ b/drivers/gpio/gpio-sx150x.c
@@ -22,7 +22,6 @@
 #include <linux/module.h>
 #include <linux/mutex.h>
 #include <linux/slab.h>
-#include <linux/workqueue.h>
 #include <linux/i2c/sx150x.h>
 
 #define NO_UPDATE_PENDING	-1
diff --git a/drivers/gpio/gpio-tc3589x.c b/drivers/gpio/gpio-tc3589x.c
index 1019320984d..51f7cbd9ff7 100644
--- a/drivers/gpio/gpio-tc3589x.c
+++ b/drivers/gpio/gpio-tc3589x.c
@@ -12,8 +12,6 @@
 #include <linux/slab.h>
 #include <linux/gpio.h>
 #include <linux/of.h>
-#include <linux/irq.h>
-#include <linux/irqdomain.h>
 #include <linux/interrupt.h>
 #include <linux/mfd/tc3589x.h>
 
@@ -31,10 +29,6 @@ struct tc3589x_gpio {
 	struct tc3589x *tc3589x;
 	struct device *dev;
 	struct mutex irq_lock;
-	struct irq_domain *domain;
-
-	int irq_base;
-
 	/* Caches of interrupt control registers for bus_lock */
 	u8 regs[CACHE_NR_REGS][CACHE_NR_BANKS];
 	u8 oldregs[CACHE_NR_REGS][CACHE_NR_BANKS];
@@ -95,30 +89,6 @@ static int tc3589x_gpio_direction_input(struct gpio_chip *chip,
 	return tc3589x_set_bits(tc3589x, reg, 1 << pos, 0);
 }
 
-/**
- * tc3589x_gpio_irq_get_irq(): Map a hardware IRQ on a chip to a Linux IRQ
- *
- * @tc3589x_gpio: tc3589x_gpio_irq controller to operate on.
- * @irq: index of the hardware interrupt requested in the chip IRQs
- *
- * Useful for drivers to request their own IRQs.
- */
-static int tc3589x_gpio_irq_get_irq(struct tc3589x_gpio *tc3589x_gpio,
-				     int hwirq)
-{
-	if (!tc3589x_gpio)
-		return -EINVAL;
-
-	return irq_create_mapping(tc3589x_gpio->domain, hwirq);
-}
-
-static int tc3589x_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
-{
-	struct tc3589x_gpio *tc3589x_gpio = to_tc3589x_gpio(chip);
-
-	return tc3589x_gpio_irq_get_irq(tc3589x_gpio, offset);
-}
-
 static struct gpio_chip template_chip = {
 	.label			= "tc3589x",
 	.owner			= THIS_MODULE,
@@ -126,13 +96,13 @@ static struct gpio_chip template_chip = {
 	.get			= tc3589x_gpio_get,
 	.direction_output	= tc3589x_gpio_direction_output,
 	.set			= tc3589x_gpio_set,
-	.to_irq			= tc3589x_gpio_to_irq,
 	.can_sleep		= true,
 };
 
 static int tc3589x_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 {
-	struct tc3589x_gpio *tc3589x_gpio = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct tc3589x_gpio *tc3589x_gpio = container_of(gc, struct tc3589x_gpio, chip);
 	int offset = d->hwirq;
 	int regoffset = offset / 8;
 	int mask = 1 << (offset % 8);
@@ -159,14 +129,16 @@ static int tc3589x_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 
 static void tc3589x_gpio_irq_lock(struct irq_data *d)
 {
-	struct tc3589x_gpio *tc3589x_gpio = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct tc3589x_gpio *tc3589x_gpio = container_of(gc, struct tc3589x_gpio, chip);
 
 	mutex_lock(&tc3589x_gpio->irq_lock);
 }
 
 static void tc3589x_gpio_irq_sync_unlock(struct irq_data *d)
 {
-	struct tc3589x_gpio *tc3589x_gpio = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct tc3589x_gpio *tc3589x_gpio = container_of(gc, struct tc3589x_gpio, chip);
 	struct tc3589x *tc3589x = tc3589x_gpio->tc3589x;
 	static const u8 regmap[] = {
 		[REG_IBE]	= TC3589x_GPIOIBE0,
@@ -194,7 +166,8 @@ static void tc3589x_gpio_irq_sync_unlock(struct irq_data *d)
 
 static void tc3589x_gpio_irq_mask(struct irq_data *d)
 {
-	struct tc3589x_gpio *tc3589x_gpio = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct tc3589x_gpio *tc3589x_gpio = container_of(gc, struct tc3589x_gpio, chip);
 	int offset = d->hwirq;
 	int regoffset = offset / 8;
 	int mask = 1 << (offset % 8);
@@ -204,7 +177,8 @@ static void tc3589x_gpio_irq_mask(struct irq_data *d)
 
 static void tc3589x_gpio_irq_unmask(struct irq_data *d)
 {
-	struct tc3589x_gpio *tc3589x_gpio = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct tc3589x_gpio *tc3589x_gpio = container_of(gc, struct tc3589x_gpio, chip);
 	int offset = d->hwirq;
 	int regoffset = offset / 8;
 	int mask = 1 << (offset % 8);
@@ -242,7 +216,8 @@ static irqreturn_t tc3589x_gpio_irq(int irq, void *dev)
 		while (stat) {
 			int bit = __ffs(stat);
 			int line = i * 8 + bit;
-			int irq = tc3589x_gpio_irq_get_irq(tc3589x_gpio, line);
+			int irq = irq_find_mapping(tc3589x_gpio->chip.irqdomain,
+						   line);
 
 			handle_nested_irq(irq);
 			stat &= ~(1 << bit);
@@ -254,61 +229,6 @@ static irqreturn_t tc3589x_gpio_irq(int irq, void *dev)
 	return IRQ_HANDLED;
 }
 
-static int tc3589x_gpio_irq_map(struct irq_domain *d, unsigned int irq,
-				irq_hw_number_t hwirq)
-{
-	struct tc3589x *tc3589x_gpio = d->host_data;
-
-	irq_set_chip_data(irq, tc3589x_gpio);
-	irq_set_chip_and_handler(irq, &tc3589x_gpio_irq_chip,
-				handle_simple_irq);
-	irq_set_nested_thread(irq, 1);
-#ifdef CONFIG_ARM
-	set_irq_flags(irq, IRQF_VALID);
-#else
-	irq_set_noprobe(irq);
-#endif
-
-	return 0;
-}
-
-static void tc3589x_gpio_irq_unmap(struct irq_domain *d, unsigned int irq)
-{
-#ifdef CONFIG_ARM
-	set_irq_flags(irq, 0);
-#endif
-	irq_set_chip_and_handler(irq, NULL, NULL);
-	irq_set_chip_data(irq, NULL);
-}
-
-static struct irq_domain_ops tc3589x_irq_ops = {
-	.map    = tc3589x_gpio_irq_map,
-	.unmap  = tc3589x_gpio_irq_unmap,
-	.xlate  = irq_domain_xlate_twocell,
-};
-
-static int tc3589x_gpio_irq_init(struct tc3589x_gpio *tc3589x_gpio,
-				struct device_node *np)
-{
-	int base = tc3589x_gpio->irq_base;
-
-	/*
-	 * If this results in a linear domain, irq_create_mapping() will
-	 * take care of allocating IRQ descriptors at runtime. When a base
-	 * is provided, the IRQ descriptors will be allocated when the
-	 * domain is instantiated.
-	 */
-	tc3589x_gpio->domain = irq_domain_add_simple(np,
-			tc3589x_gpio->chip.ngpio, base, &tc3589x_irq_ops,
-			tc3589x_gpio);
-	if (!tc3589x_gpio->domain) {
-		dev_err(tc3589x_gpio->dev, "Failed to create irqdomain\n");
-		return -ENOSYS;
-	}
-
-	return 0;
-}
-
 static int tc3589x_gpio_probe(struct platform_device *pdev)
 {
 	struct tc3589x *tc3589x = dev_get_drvdata(pdev->dev.parent);
@@ -329,7 +249,8 @@ static int tc3589x_gpio_probe(struct platform_device *pdev)
 	if (irq < 0)
 		return irq;
 
-	tc3589x_gpio = kzalloc(sizeof(struct tc3589x_gpio), GFP_KERNEL);
+	tc3589x_gpio = devm_kzalloc(&pdev->dev, sizeof(struct tc3589x_gpio),
+				    GFP_KERNEL);
 	if (!tc3589x_gpio)
 		return -ENOMEM;
 
@@ -347,30 +268,36 @@ static int tc3589x_gpio_probe(struct platform_device *pdev)
 	tc3589x_gpio->chip.of_node = np;
 #endif
 
-	tc3589x_gpio->irq_base = tc3589x->irq_base ?
-		tc3589x->irq_base + TC3589x_INT_GPIO(0) : 0;
-
 	/* Bring the GPIO module out of reset */
 	ret = tc3589x_set_bits(tc3589x, TC3589x_RSTCTRL,
 			       TC3589x_RSTCTRL_GPIRST, 0);
 	if (ret < 0)
-		goto out_free;
-
-	ret = tc3589x_gpio_irq_init(tc3589x_gpio, np);
-	if (ret)
-		goto out_free;
+		return ret;
 
-	ret = request_threaded_irq(irq, NULL, tc3589x_gpio_irq, IRQF_ONESHOT,
-				   "tc3589x-gpio", tc3589x_gpio);
+	ret = devm_request_threaded_irq(&pdev->dev,
+					irq, NULL, tc3589x_gpio_irq,
+					IRQF_ONESHOT, "tc3589x-gpio",
+					tc3589x_gpio);
 	if (ret) {
 		dev_err(&pdev->dev, "unable to get irq: %d\n", ret);
-		goto out_free;
+		return ret;
 	}
 
 	ret = gpiochip_add(&tc3589x_gpio->chip);
 	if (ret) {
 		dev_err(&pdev->dev, "unable to add gpiochip: %d\n", ret);
-		goto out_freeirq;
+		return ret;
+	}
+
+	ret =  gpiochip_irqchip_add(&tc3589x_gpio->chip,
+				    &tc3589x_gpio_irq_chip,
+				    0,
+				    handle_simple_irq,
+				    IRQ_TYPE_NONE);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"could not connect irqchip to gpiochip\n");
+		return ret;
 	}
 
 	if (pdata && pdata->setup)
@@ -379,12 +306,6 @@ static int tc3589x_gpio_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, tc3589x_gpio);
 
 	return 0;
-
-out_freeirq:
-	free_irq(irq, tc3589x_gpio);
-out_free:
-	kfree(tc3589x_gpio);
-	return ret;
 }
 
 static int tc3589x_gpio_remove(struct platform_device *pdev)
@@ -392,7 +313,6 @@ static int tc3589x_gpio_remove(struct platform_device *pdev)
 	struct tc3589x_gpio *tc3589x_gpio = platform_get_drvdata(pdev);
 	struct tc3589x *tc3589x = tc3589x_gpio->tc3589x;
 	struct tc3589x_gpio_platform_data *pdata = tc3589x->pdata->gpio;
-	int irq = platform_get_irq(pdev, 0);
 	int ret;
 
 	if (pdata && pdata->remove)
@@ -405,10 +325,6 @@ static int tc3589x_gpio_remove(struct platform_device *pdev)
 		return ret;
 	}
 
-	free_irq(irq, tc3589x_gpio);
-
-	kfree(tc3589x_gpio);
-
 	return 0;
 }
 
diff --git a/drivers/gpio/gpio-tegra.c b/drivers/gpio/gpio-tegra.c
index 2b49f878b56..4e8fb8261a8 100644
--- a/drivers/gpio/gpio-tegra.c
+++ b/drivers/gpio/gpio-tegra.c
@@ -408,7 +408,7 @@ static struct tegra_gpio_soc_config tegra30_gpio_config = {
 	.upper_offset = 0x80,
 };
 
-static struct of_device_id tegra_gpio_of_match[] = {
+static const struct of_device_id tegra_gpio_of_match[] = {
 	{ .compatible = "nvidia,tegra30-gpio", .data = &tegra30_gpio_config },
 	{ .compatible = "nvidia,tegra20-gpio", .data = &tegra20_gpio_config },
 	{ },
@@ -458,10 +458,8 @@ static int tegra_gpio_probe(struct platform_device *pdev)
 	tegra_gpio_banks = devm_kzalloc(&pdev->dev,
 			tegra_gpio_bank_count * sizeof(*tegra_gpio_banks),
 			GFP_KERNEL);
-	if (!tegra_gpio_banks) {
-		dev_err(&pdev->dev, "Couldn't allocate bank structure\n");
+	if (!tegra_gpio_banks)
 		return -ENODEV;
-	}
 
 	irq_domain = irq_domain_add_linear(pdev->dev.of_node,
 					   tegra_gpio_chip.ngpio,
diff --git a/drivers/gpio/gpio-timberdale.c b/drivers/gpio/gpio-timberdale.c
index f9a8fbde108..efc7c129016 100644
--- a/drivers/gpio/gpio-timberdale.c
+++ b/drivers/gpio/gpio-timberdale.c
@@ -224,6 +224,7 @@ static struct irq_chip timbgpio_irqchip = {
 static int timbgpio_probe(struct platform_device *pdev)
 {
 	int err, i;
+	struct device *dev = &pdev->dev;
 	struct gpio_chip *gc;
 	struct timbgpio *tgpio;
 	struct resource *iomem;
@@ -231,35 +232,35 @@ static int timbgpio_probe(struct platform_device *pdev)
 	int irq = platform_get_irq(pdev, 0);
 
 	if (!pdata || pdata->nr_pins > 32) {
-		err = -EINVAL;
-		goto err_mem;
+		dev_err(dev, "Invalid platform data\n");
+		return -EINVAL;
 	}
 
 	iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!iomem) {
-		err = -EINVAL;
-		goto err_mem;
+		dev_err(dev, "Unable to get resource\n");
+		return -EINVAL;
 	}
 
-	tgpio = kzalloc(sizeof(*tgpio), GFP_KERNEL);
+	tgpio = devm_kzalloc(dev, sizeof(struct timbgpio), GFP_KERNEL);
 	if (!tgpio) {
-		err = -EINVAL;
-		goto err_mem;
+		dev_err(dev, "Memory alloc failed\n");
+		return -EINVAL;
 	}
 	tgpio->irq_base = pdata->irq_base;
 
 	spin_lock_init(&tgpio->lock);
 
-	if (!request_mem_region(iomem->start, resource_size(iomem),
-		DRIVER_NAME)) {
-		err = -EBUSY;
-		goto err_request;
+	if (!devm_request_mem_region(dev, iomem->start, resource_size(iomem),
+				     DRIVER_NAME)) {
+		dev_err(dev, "Region already claimed\n");
+		return -EBUSY;
 	}
 
-	tgpio->membase = ioremap(iomem->start, resource_size(iomem));
+	tgpio->membase = devm_ioremap(dev, iomem->start, resource_size(iomem));
 	if (!tgpio->membase) {
-		err = -ENOMEM;
-		goto err_ioremap;
+		dev_err(dev, "Cannot ioremap\n");
+		return -ENOMEM;
 	}
 
 	gc = &tgpio->gpio;
@@ -279,7 +280,7 @@ static int timbgpio_probe(struct platform_device *pdev)
 
 	err = gpiochip_add(gc);
 	if (err)
-		goto err_chipadd;
+		return err;
 
 	platform_set_drvdata(pdev, tgpio);
 
@@ -302,17 +303,6 @@ static int timbgpio_probe(struct platform_device *pdev)
 	irq_set_chained_handler(irq, timbgpio_irq);
 
 	return 0;
-
-err_chipadd:
-	iounmap(tgpio->membase);
-err_ioremap:
-	release_mem_region(iomem->start, resource_size(iomem));
-err_request:
-	kfree(tgpio);
-err_mem:
-	printk(KERN_ERR DRIVER_NAME": Failed to register GPIOs: %d\n", err);
-
-	return err;
 }
 
 static int timbgpio_remove(struct platform_device *pdev)
@@ -320,7 +310,6 @@ static int timbgpio_remove(struct platform_device *pdev)
 	int err;
 	struct timbgpio_platform_data *pdata = dev_get_platdata(&pdev->dev);
 	struct timbgpio *tgpio = platform_get_drvdata(pdev);
-	struct resource *iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	int irq = platform_get_irq(pdev, 0);
 
 	if (irq >= 0 && tgpio->irq_base > 0) {
@@ -338,10 +327,6 @@ static int timbgpio_remove(struct platform_device *pdev)
 	if (err)
 		printk(KERN_ERR DRIVER_NAME": failed to remove gpio_chip\n");
 
-	iounmap(tgpio->membase);
-	release_mem_region(iomem->start, resource_size(iomem));
-	kfree(tgpio);
-
 	return 0;
 }
 
diff --git a/drivers/gpio/gpio-tps6586x.c b/drivers/gpio/gpio-tps6586x.c
index 8994dfa1349..a69fbea4125 100644
--- a/drivers/gpio/gpio-tps6586x.c
+++ b/drivers/gpio/gpio-tps6586x.c
@@ -97,10 +97,8 @@ static int tps6586x_gpio_probe(struct platform_device *pdev)
 	pdata = dev_get_platdata(pdev->dev.parent);
 	tps6586x_gpio = devm_kzalloc(&pdev->dev,
 				sizeof(*tps6586x_gpio), GFP_KERNEL);
-	if (!tps6586x_gpio) {
-		dev_err(&pdev->dev, "Could not allocate tps6586x_gpio\n");
+	if (!tps6586x_gpio)
 		return -ENOMEM;
-	}
 
 	tps6586x_gpio->parent = pdev->dev.parent;
 
diff --git a/drivers/gpio/gpio-tps65910.c b/drivers/gpio/gpio-tps65910.c
index b6e818e6800..e2f8cda235e 100644
--- a/drivers/gpio/gpio-tps65910.c
+++ b/drivers/gpio/gpio-tps65910.c
@@ -123,10 +123,8 @@ static int tps65910_gpio_probe(struct platform_device *pdev)
 
 	tps65910_gpio = devm_kzalloc(&pdev->dev,
 				sizeof(*tps65910_gpio), GFP_KERNEL);
-	if (!tps65910_gpio) {
-		dev_err(&pdev->dev, "Could not allocate tps65910_gpio\n");
+	if (!tps65910_gpio)
 		return -ENOMEM;
-	}
 
 	tps65910_gpio->tps65910 = tps65910;
 
diff --git a/drivers/gpio/gpio-xilinx.c b/drivers/gpio/gpio-xilinx.c
index 792a05ad464..12481867daf 100644
--- a/drivers/gpio/gpio-xilinx.c
+++ b/drivers/gpio/gpio-xilinx.c
@@ -289,7 +289,7 @@ static int xgpio_of_probe(struct device_node *np)
 	return 0;
 }
 
-static struct of_device_id xgpio_of_match[] = {
+static const struct of_device_id xgpio_of_match[] = {
 	{ .compatible = "xlnx,xps-gpio-1.00.a", },
 	{ /* end of list */ },
 };
diff --git a/drivers/gpio/gpio-zevio.c b/drivers/gpio/gpio-zevio.c
index 9bf5034b6cd..54e54e4cc6c 100644
--- a/drivers/gpio/gpio-zevio.c
+++ b/drivers/gpio/gpio-zevio.c
@@ -81,9 +81,15 @@ static inline void zevio_gpio_port_set(struct zevio_gpio *c, unsigned pin,
 static int zevio_gpio_get(struct gpio_chip *chip, unsigned pin)
 {
 	struct zevio_gpio *controller = to_zevio_gpio(chip);
+	u32 val, dir;
 
-	/* Only reading allowed, so no spinlock needed */
-	u32 val = zevio_gpio_port_get(controller, pin, ZEVIO_GPIO_INPUT);
+	spin_lock(&controller->lock);
+	dir = zevio_gpio_port_get(controller, pin, ZEVIO_GPIO_DIRECTION);
+	if (dir & BIT(ZEVIO_GPIO_BIT(pin)))
+		val = zevio_gpio_port_get(controller, pin, ZEVIO_GPIO_INPUT);
+	else
+		val = zevio_gpio_port_get(controller, pin, ZEVIO_GPIO_OUTPUT);
+	spin_unlock(&controller->lock);
 
 	return (val >> ZEVIO_GPIO_BIT(pin)) & 0x1;
 }
@@ -172,10 +178,8 @@ static int zevio_gpio_probe(struct platform_device *pdev)
 	int status, i;
 
 	controller = devm_kzalloc(&pdev->dev, sizeof(*controller), GFP_KERNEL);
-	if (!controller) {
-		dev_err(&pdev->dev, "not enough free memory\n");
+	if (!controller)
 		return -ENOMEM;
-	}
 
 	/* Copy our reference */
 	controller->chip.gc = zevio_gpio_chip;
@@ -198,7 +202,7 @@ static int zevio_gpio_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id zevio_gpio_of_match[] = {
+static const struct of_device_id zevio_gpio_of_match[] = {
 	{ .compatible = "lsi,zevio-gpio", },
 	{ },
 };
@@ -209,7 +213,7 @@ static struct platform_driver zevio_gpio_driver = {
 	.driver		= {
 		.name	= "gpio-zevio",
 		.owner	= THIS_MODULE,
-		.of_match_table = of_match_ptr(zevio_gpio_of_match),
+		.of_match_table = zevio_gpio_of_match,
 	},
 	.probe		= zevio_gpio_probe,
 };
diff --git a/drivers/gpio/gpiolib-acpi.c b/drivers/gpio/gpiolib-acpi.c
index 401add28933..4a987917c18 100644
--- a/drivers/gpio/gpiolib-acpi.c
+++ b/drivers/gpio/gpiolib-acpi.c
@@ -449,9 +449,10 @@ acpi_gpio_adr_space_handler(u32 function, acpi_physical_address address,
 		mutex_unlock(&achip->conn_lock);
 
 		if (function == ACPI_WRITE)
-			gpiod_set_raw_value(desc, !!((1 << i) & *value));
+			gpiod_set_raw_value_cansleep(desc,
+						     !!((1 << i) & *value));
 		else
-			*value |= (u64)gpiod_get_raw_value(desc) << i;
+			*value |= (u64)gpiod_get_raw_value_cansleep(desc) << i;
 	}
 
 out:
diff --git a/drivers/gpio/gpiolib-of.c b/drivers/gpio/gpiolib-of.c
index 2024d45e550..af7e25c9a9a 100644
--- a/drivers/gpio/gpiolib-of.c
+++ b/drivers/gpio/gpiolib-of.c
@@ -48,7 +48,7 @@ static int of_gpiochip_find_and_xlate(struct gpio_chip *gc, void *data)
 	if (ret < 0)
 		return false;
 
-	gg_data->out_gpio = gpio_to_desc(ret + gc->base);
+	gg_data->out_gpio = gpiochip_get_desc(gc, ret);
 	return true;
 }
 
@@ -96,6 +96,20 @@ struct gpio_desc *of_get_named_gpiod_flags(struct device_node *np,
 }
 EXPORT_SYMBOL(of_get_named_gpiod_flags);
 
+int of_get_named_gpio_flags(struct device_node *np, const char *list_name,
+			    int index, enum of_gpio_flags *flags)
+{
+	struct gpio_desc *desc;
+
+	desc = of_get_named_gpiod_flags(np, list_name, index, flags);
+
+	if (IS_ERR(desc))
+		return PTR_ERR(desc);
+	else
+		return desc_to_gpio(desc);
+}
+EXPORT_SYMBOL(of_get_named_gpio_flags);
+
 /**
  * of_gpio_simple_xlate - translate gpio_spec to the GPIO number and flags
  * @gc:		pointer to the gpio_chip structure
diff --git a/drivers/gpio/gpiolib.c b/drivers/gpio/gpiolib.c
index f48817d9748..d9c9cb4665d 100644
--- a/drivers/gpio/gpiolib.c
+++ b/drivers/gpio/gpiolib.c
@@ -1363,6 +1363,11 @@ void gpiochip_set_chained_irqchip(struct gpio_chip *gpiochip,
 				  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");
+		return;
+	}
+
 	irq_set_chained_handler(parent_irq, parent_handler);
 	/*
 	 * The parent irqchip is already using the chip_data for this
@@ -1372,6 +1377,12 @@ void gpiochip_set_chained_irqchip(struct gpio_chip *gpiochip,
 }
 EXPORT_SYMBOL_GPL(gpiochip_set_chained_irqchip);
 
+/*
+ * This lock class tells lockdep that GPIO irqs are in a different
+ * category than their parents, so it won't report false recursion.
+ */
+static struct lock_class_key gpiochip_irq_lock_class;
+
 /**
  * gpiochip_irq_map() - maps an IRQ into a GPIO irqchip
  * @d: the irqdomain used by this irqchip
@@ -1388,22 +1399,35 @@ static int gpiochip_irq_map(struct irq_domain *d, unsigned int irq,
 	struct gpio_chip *chip = d->host_data;
 
 	irq_set_chip_data(irq, chip);
+	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)
+		irq_set_nested_thread(irq, 1);
 #ifdef CONFIG_ARM
 	set_irq_flags(irq, IRQF_VALID);
 #else
 	irq_set_noprobe(irq);
 #endif
-	irq_set_irq_type(irq, chip->irq_default_type);
+	/*
+	 * No set-up of the hardware will happen if IRQ_TYPE_NONE
+	 * is passed as default type.
+	 */
+	if (chip->irq_default_type != IRQ_TYPE_NONE)
+		irq_set_irq_type(irq, chip->irq_default_type);
 
 	return 0;
 }
 
 static void gpiochip_irq_unmap(struct irq_domain *d, unsigned int irq)
 {
+	struct gpio_chip *chip = d->host_data;
+
 #ifdef CONFIG_ARM
 	set_irq_flags(irq, 0);
 #endif
+	if (chip->can_sleep)
+		irq_set_nested_thread(irq, 0);
 	irq_set_chip_and_handler(irq, NULL, NULL);
 	irq_set_chip_data(irq, NULL);
 }
@@ -1471,7 +1495,8 @@ static void gpiochip_irqchip_remove(struct gpio_chip *gpiochip)
  * @first_irq: if not dynamically assigned, the base (first) IRQ to
  * allocate gpiochip irqs from
  * @handler: the irq handler to use (often a predefined irq core function)
- * @type: the default type for IRQs on this irqchip
+ * @type: the default type for IRQs on this irqchip, pass IRQ_TYPE_NONE
+ * to have the core avoid setting up any default type in the hardware.
  *
  * This function closely associates a certain irqchip with a certain
  * gpiochip, providing an irq domain to translate the local IRQs to
@@ -2571,22 +2596,27 @@ void gpiod_add_lookup_table(struct gpiod_lookup_table *table)
 	mutex_unlock(&gpio_lookup_lock);
 }
 
-#ifdef CONFIG_OF
 static struct gpio_desc *of_find_gpio(struct device *dev, const char *con_id,
 				      unsigned int idx,
 				      enum gpio_lookup_flags *flags)
 {
+	static const char *suffixes[] = { "gpios", "gpio" };
 	char prop_name[32]; /* 32 is max size of property name */
 	enum of_gpio_flags of_flags;
 	struct gpio_desc *desc;
+	unsigned int i;
 
-	if (con_id)
-		snprintf(prop_name, 32, "%s-gpios", con_id);
-	else
-		snprintf(prop_name, 32, "gpios");
+	for (i = 0; i < ARRAY_SIZE(suffixes); i++) {
+		if (con_id)
+			snprintf(prop_name, 32, "%s-%s", con_id, suffixes[i]);
+		else
+			snprintf(prop_name, 32, "%s", suffixes[i]);
 
-	desc = of_get_named_gpiod_flags(dev->of_node, prop_name, idx,
-					&of_flags);
+		desc = of_get_named_gpiod_flags(dev->of_node, prop_name, idx,
+						&of_flags);
+		if (!IS_ERR(desc))
+			break;
+	}
 
 	if (IS_ERR(desc))
 		return desc;
@@ -2596,14 +2626,6 @@ static struct gpio_desc *of_find_gpio(struct device *dev, const char *con_id,
 
 	return desc;
 }
-#else
-static struct gpio_desc *of_find_gpio(struct device *dev, const char *con_id,
-				      unsigned int idx,
-				      enum gpio_lookup_flags *flags)
-{
-	return ERR_PTR(-ENODEV);
-}
-#endif
 
 static struct gpio_desc *acpi_find_gpio(struct device *dev, const char *con_id,
 					unsigned int idx,
@@ -2701,7 +2723,7 @@ static struct gpio_desc *gpiod_find(struct device *dev, const char *con_id,
 }
 
 /**
- * gpio_get - obtain a GPIO for a given GPIO function
+ * gpiod_get - obtain a GPIO for a given GPIO function
  * @dev:	GPIO consumer, can be NULL for system-global GPIOs
  * @con_id:	function within the GPIO consumer
  *
@@ -2716,6 +2738,22 @@ struct gpio_desc *__must_check gpiod_get(struct device *dev, const char *con_id)
 EXPORT_SYMBOL_GPL(gpiod_get);
 
 /**
+ * gpiod_get_optional - obtain an optional GPIO for a given GPIO function
+ * @dev: GPIO consumer, can be NULL for system-global GPIOs
+ * @con_id: function within the GPIO consumer
+ *
+ * This is equivalent to gpiod_get(), except that when no GPIO was assigned to
+ * the requested function it will return NULL. This is convenient for drivers
+ * that need to handle optional GPIOs.
+ */
+struct gpio_desc *__must_check gpiod_get_optional(struct device *dev,
+						  const char *con_id)
+{
+	return gpiod_get_index_optional(dev, con_id, 0);
+}
+EXPORT_SYMBOL_GPL(gpiod_get_optional);
+
+/**
  * gpiod_get_index - obtain a GPIO from a multi-index GPIO function
  * @dev:	GPIO consumer, can be NULL for system-global GPIOs
  * @con_id:	function within the GPIO consumer
@@ -2778,6 +2816,33 @@ struct gpio_desc *__must_check gpiod_get_index(struct device *dev,
 EXPORT_SYMBOL_GPL(gpiod_get_index);
 
 /**
+ * gpiod_get_index_optional - obtain an optional GPIO from a multi-index GPIO
+ *                            function
+ * @dev: GPIO consumer, can be NULL for system-global GPIOs
+ * @con_id: function within the GPIO consumer
+ * @index: index of the GPIO to obtain in the consumer
+ *
+ * This is equivalent to gpiod_get_index(), except that when no GPIO with the
+ * specified index was assigned to the requested function it will return NULL.
+ * This is convenient for drivers that need to handle optional GPIOs.
+ */
+struct gpio_desc *__must_check gpiod_get_index_optional(struct device *dev,
+							const char *con_id,
+							unsigned int index)
+{
+	struct gpio_desc *desc;
+
+	desc = gpiod_get_index(dev, con_id, index);
+	if (IS_ERR(desc)) {
+		if (PTR_ERR(desc) == -ENOENT)
+			return NULL;
+	}
+
+	return desc;
+}
+EXPORT_SYMBOL_GPL(gpiod_get_index_optional);
+
+/**
  * gpiod_put - dispose of a GPIO descriptor
  * @desc:	GPIO descriptor to dispose of
  *
diff --git a/drivers/gpio/gpiolib.h b/drivers/gpio/gpiolib.h
index cf092941a9f..1a4103dd38d 100644
--- a/drivers/gpio/gpiolib.h
+++ b/drivers/gpio/gpiolib.h
@@ -15,6 +15,8 @@
 #include <linux/err.h>
 #include <linux/device.h>
 
+enum of_gpio_flags;
+
 /**
  * struct acpi_gpio_info - ACPI GPIO specific information
  * @gpioint: if %true this GPIO is of type GpioInt otherwise type is GpioIo
@@ -46,4 +48,7 @@ acpi_get_gpiod_by_index(struct device *dev, int index,
 int gpiochip_request_own_desc(struct gpio_desc *desc, const char *label);
 void gpiochip_free_own_desc(struct gpio_desc *desc);
 
+struct gpio_desc *of_get_named_gpiod_flags(struct device_node *np,
+		   const char *list_name, int index, enum of_gpio_flags *flags);
+
 #endif /* GPIOLIB_H */
diff --git a/drivers/gpu/drm/drm_crtc_helper.c b/drivers/gpu/drm/drm_crtc_helper.c
index df281b54db0..872ba11c453 100644
--- a/drivers/gpu/drm/drm_crtc_helper.c
+++ b/drivers/gpu/drm/drm_crtc_helper.c
@@ -29,6 +29,7 @@
  *      Jesse Barnes <jesse.barnes@intel.com>
  */
 
+#include <linux/kernel.h>
 #include <linux/export.h>
 #include <linux/moduleparam.h>
 
@@ -88,7 +89,13 @@ bool drm_helper_encoder_in_use(struct drm_encoder *encoder)
 	struct drm_connector *connector;
 	struct drm_device *dev = encoder->dev;
 
-	WARN_ON(!mutex_is_locked(&dev->mode_config.mutex));
+	/*
+	 * We can expect this mutex to be locked if we are not panicking.
+	 * Locking is currently fubar in the panic handler.
+	 */
+	if (!oops_in_progress)
+		WARN_ON(!mutex_is_locked(&dev->mode_config.mutex));
+
 	list_for_each_entry(connector, &dev->mode_config.connector_list, head)
 		if (connector->encoder == encoder)
 			return true;
@@ -112,7 +119,13 @@ bool drm_helper_crtc_in_use(struct drm_crtc *crtc)
 	struct drm_encoder *encoder;
 	struct drm_device *dev = crtc->dev;
 
-	WARN_ON(!mutex_is_locked(&dev->mode_config.mutex));
+	/*
+	 * We can expect this mutex to be locked if we are not panicking.
+	 * Locking is currently fubar in the panic handler.
+	 */
+	if (!oops_in_progress)
+		WARN_ON(!mutex_is_locked(&dev->mode_config.mutex));
+
 	list_for_each_entry(encoder, &dev->mode_config.encoder_list, head)
 		if (encoder->crtc == crtc && drm_helper_encoder_in_use(encoder))
 			return true;
diff --git a/drivers/gpu/drm/drm_irq.c b/drivers/gpu/drm/drm_irq.c
index c2676b5908d..ec5c3f4cdd0 100644
--- a/drivers/gpu/drm/drm_irq.c
+++ b/drivers/gpu/drm/drm_irq.c
@@ -156,7 +156,7 @@ static void vblank_disable_and_save(struct drm_device *dev, int crtc)
 	 */
 	if ((vblrc > 0) && (abs64(diff_ns) > 1000000)) {
 		atomic_inc(&dev->vblank[crtc].count);
-		smp_mb__after_atomic_inc();
+		smp_mb__after_atomic();
 	}
 
 	/* Invalidate all timestamps while vblank irq's are off. */
@@ -864,9 +864,9 @@ static void drm_update_vblank_count(struct drm_device *dev, int crtc)
 		vblanktimestamp(dev, crtc, tslot) = t_vblank;
 	}
 
-	smp_mb__before_atomic_inc();
+	smp_mb__before_atomic();
 	atomic_add(diff, &dev->vblank[crtc].count);
-	smp_mb__after_atomic_inc();
+	smp_mb__after_atomic();
 }
 
 /**
@@ -1330,9 +1330,9 @@ bool drm_handle_vblank(struct drm_device *dev, int crtc)
 		/* Increment cooked vblank count. This also atomically commits
 		 * the timestamp computed above.
 		 */
-		smp_mb__before_atomic_inc();
+		smp_mb__before_atomic();
 		atomic_inc(&dev->vblank[crtc].count);
-		smp_mb__after_atomic_inc();
+		smp_mb__after_atomic();
 	} else {
 		DRM_DEBUG("crtc %d: Redundant vblirq ignored. diff_ns = %d\n",
 			  crtc, (int) diff_ns);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_g2d.c b/drivers/gpu/drm/exynos/exynos_drm_g2d.c
index 6c1885eedfd..80015871447 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_g2d.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_g2d.c
@@ -467,14 +467,17 @@ static dma_addr_t *g2d_userptr_get_dma_addr(struct drm_device *drm_dev,
 		goto err_free;
 	}
 
+	down_read(&current->mm->mmap_sem);
 	vma = find_vma(current->mm, userptr);
 	if (!vma) {
+		up_read(&current->mm->mmap_sem);
 		DRM_ERROR("failed to get vm region.\n");
 		ret = -EFAULT;
 		goto err_free_pages;
 	}
 
 	if (vma->vm_end < userptr + size) {
+		up_read(&current->mm->mmap_sem);
 		DRM_ERROR("vma is too small.\n");
 		ret = -EFAULT;
 		goto err_free_pages;
@@ -482,6 +485,7 @@ static dma_addr_t *g2d_userptr_get_dma_addr(struct drm_device *drm_dev,
 
 	g2d_userptr->vma = exynos_gem_get_vma(vma);
 	if (!g2d_userptr->vma) {
+		up_read(&current->mm->mmap_sem);
 		DRM_ERROR("failed to copy vma.\n");
 		ret = -ENOMEM;
 		goto err_free_pages;
@@ -492,10 +496,12 @@ static dma_addr_t *g2d_userptr_get_dma_addr(struct drm_device *drm_dev,
 	ret = exynos_gem_get_pages_from_userptr(start & PAGE_MASK,
 						npages, pages, vma);
 	if (ret < 0) {
+		up_read(&current->mm->mmap_sem);
 		DRM_ERROR("failed to get user pages from userptr.\n");
 		goto err_put_vma;
 	}
 
+	up_read(&current->mm->mmap_sem);
 	g2d_userptr->pages = pages;
 
 	sgt = kzalloc(sizeof(*sgt), GFP_KERNEL);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_ipp.c b/drivers/gpu/drm/exynos/exynos_drm_ipp.c
index 09312b87747..3d78144387a 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_ipp.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_ipp.c
@@ -284,7 +284,7 @@ static struct exynos_drm_ippdrv *ipp_find_drv_by_handle(u32 prop_id)
 	/*
 	 * This case is search ipp driver by prop_id handle.
 	 * sometimes, ipp subsystem find driver by prop_id.
-	 * e.g PAUSE state, queue buf, command contro.
+	 * e.g PAUSE state, queue buf, command control.
 	 */
 	list_for_each_entry(ippdrv, &exynos_drm_ippdrv_list, drv_list) {
 		DRM_DEBUG_KMS("count[%d]ippdrv[0x%x]\n", count++, (int)ippdrv);
diff --git a/drivers/gpu/drm/i915/i915_dma.c b/drivers/gpu/drm/i915/i915_dma.c
index 96177eec0a0..eedb023af27 100644
--- a/drivers/gpu/drm/i915/i915_dma.c
+++ b/drivers/gpu/drm/i915/i915_dma.c
@@ -1833,7 +1833,6 @@ int i915_driver_unload(struct drm_device *dev)
 		flush_workqueue(dev_priv->wq);
 
 		mutex_lock(&dev->struct_mutex);
-		i915_gem_free_all_phys_object(dev);
 		i915_gem_cleanup_ringbuffer(dev);
 		i915_gem_context_fini(dev);
 		WARN_ON(dev_priv->mm.aliasing_ppgtt);
diff --git a/drivers/gpu/drm/i915/i915_drv.h b/drivers/gpu/drm/i915/i915_drv.h
index 108e1ec2fa4..388c028e223 100644
--- a/drivers/gpu/drm/i915/i915_drv.h
+++ b/drivers/gpu/drm/i915/i915_drv.h
@@ -242,18 +242,6 @@ struct intel_ddi_plls {
 #define WATCH_LISTS	0
 #define WATCH_GTT	0
 
-#define I915_GEM_PHYS_CURSOR_0 1
-#define I915_GEM_PHYS_CURSOR_1 2
-#define I915_GEM_PHYS_OVERLAY_REGS 3
-#define I915_MAX_PHYS_OBJECT (I915_GEM_PHYS_OVERLAY_REGS)
-
-struct drm_i915_gem_phys_object {
-	int id;
-	struct page **page_list;
-	drm_dma_handle_t *handle;
-	struct drm_i915_gem_object *cur_obj;
-};
-
 struct opregion_header;
 struct opregion_acpi;
 struct opregion_swsci;
@@ -1187,9 +1175,6 @@ struct i915_gem_mm {
 	/** Bit 6 swizzling required for Y tiling */
 	uint32_t bit_6_swizzle_y;
 
-	/* storage for physical objects */
-	struct drm_i915_gem_phys_object *phys_objs[I915_MAX_PHYS_OBJECT];
-
 	/* accounting, useful for userland debugging */
 	spinlock_t object_stat_lock;
 	size_t object_memory;
@@ -1769,7 +1754,7 @@ struct drm_i915_gem_object {
 	struct drm_file *pin_filp;
 
 	/** for phy allocated objects */
-	struct drm_i915_gem_phys_object *phys_obj;
+	drm_dma_handle_t *phys_handle;
 };
 
 #define to_intel_bo(x) container_of(x, struct drm_i915_gem_object, base)
@@ -2204,10 +2189,12 @@ void i915_gem_vma_destroy(struct i915_vma *vma);
 #define PIN_MAPPABLE 0x1
 #define PIN_NONBLOCK 0x2
 #define PIN_GLOBAL 0x4
+#define PIN_OFFSET_BIAS 0x8
+#define PIN_OFFSET_MASK (~4095)
 int __must_check i915_gem_object_pin(struct drm_i915_gem_object *obj,
 				     struct i915_address_space *vm,
 				     uint32_t alignment,
-				     unsigned flags);
+				     uint64_t flags);
 int __must_check i915_vma_unbind(struct i915_vma *vma);
 int i915_gem_object_put_pages(struct drm_i915_gem_object *obj);
 void i915_gem_release_all_mmaps(struct drm_i915_private *dev_priv);
@@ -2334,13 +2321,8 @@ i915_gem_object_pin_to_display_plane(struct drm_i915_gem_object *obj,
 				     u32 alignment,
 				     struct intel_ring_buffer *pipelined);
 void i915_gem_object_unpin_from_display_plane(struct drm_i915_gem_object *obj);
-int i915_gem_attach_phys_object(struct drm_device *dev,
-				struct drm_i915_gem_object *obj,
-				int id,
+int i915_gem_object_attach_phys(struct drm_i915_gem_object *obj,
 				int align);
-void i915_gem_detach_phys_object(struct drm_device *dev,
-				 struct drm_i915_gem_object *obj);
-void i915_gem_free_all_phys_object(struct drm_device *dev);
 int i915_gem_open(struct drm_device *dev, struct drm_file *file);
 void i915_gem_release(struct drm_device *dev, struct drm_file *file);
 
@@ -2465,6 +2447,8 @@ int __must_check i915_gem_evict_something(struct drm_device *dev,
 					  int min_size,
 					  unsigned alignment,
 					  unsigned cache_level,
+					  unsigned long start,
+					  unsigned long end,
 					  unsigned flags);
 int i915_gem_evict_vm(struct i915_address_space *vm, bool do_idle);
 int i915_gem_evict_everything(struct drm_device *dev);
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index 2871ce75f43..3326770c9ed 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -43,10 +43,6 @@ static void i915_gem_object_flush_cpu_write_domain(struct drm_i915_gem_object *o
 static __must_check int
 i915_gem_object_wait_rendering(struct drm_i915_gem_object *obj,
 			       bool readonly);
-static int i915_gem_phys_pwrite(struct drm_device *dev,
-				struct drm_i915_gem_object *obj,
-				struct drm_i915_gem_pwrite *args,
-				struct drm_file *file);
 
 static void i915_gem_write_fence(struct drm_device *dev, int reg,
 				 struct drm_i915_gem_object *obj);
@@ -209,6 +205,128 @@ i915_gem_get_aperture_ioctl(struct drm_device *dev, void *data,
 	return 0;
 }
 
+static void i915_gem_object_detach_phys(struct drm_i915_gem_object *obj)
+{
+	drm_dma_handle_t *phys = obj->phys_handle;
+
+	if (!phys)
+		return;
+
+	if (obj->madv == I915_MADV_WILLNEED) {
+		struct address_space *mapping = file_inode(obj->base.filp)->i_mapping;
+		char *vaddr = phys->vaddr;
+		int i;
+
+		for (i = 0; i < obj->base.size / PAGE_SIZE; i++) {
+			struct page *page = shmem_read_mapping_page(mapping, i);
+			if (!IS_ERR(page)) {
+				char *dst = kmap_atomic(page);
+				memcpy(dst, vaddr, PAGE_SIZE);
+				drm_clflush_virt_range(dst, PAGE_SIZE);
+				kunmap_atomic(dst);
+
+				set_page_dirty(page);
+				mark_page_accessed(page);
+				page_cache_release(page);
+			}
+			vaddr += PAGE_SIZE;
+		}
+		i915_gem_chipset_flush(obj->base.dev);
+	}
+
+#ifdef CONFIG_X86
+	set_memory_wb((unsigned long)phys->vaddr, phys->size / PAGE_SIZE);
+#endif
+	drm_pci_free(obj->base.dev, phys);
+	obj->phys_handle = NULL;
+}
+
+int
+i915_gem_object_attach_phys(struct drm_i915_gem_object *obj,
+			    int align)
+{
+	drm_dma_handle_t *phys;
+	struct address_space *mapping;
+	char *vaddr;
+	int i;
+
+	if (obj->phys_handle) {
+		if ((unsigned long)obj->phys_handle->vaddr & (align -1))
+			return -EBUSY;
+
+		return 0;
+	}
+
+	if (obj->madv != I915_MADV_WILLNEED)
+		return -EFAULT;
+
+	if (obj->base.filp == NULL)
+		return -EINVAL;
+
+	/* create a new object */
+	phys = drm_pci_alloc(obj->base.dev, obj->base.size, align);
+	if (!phys)
+		return -ENOMEM;
+
+	vaddr = phys->vaddr;
+#ifdef CONFIG_X86
+	set_memory_wc((unsigned long)vaddr, phys->size / PAGE_SIZE);
+#endif
+	mapping = file_inode(obj->base.filp)->i_mapping;
+	for (i = 0; i < obj->base.size / PAGE_SIZE; i++) {
+		struct page *page;
+		char *src;
+
+		page = shmem_read_mapping_page(mapping, i);
+		if (IS_ERR(page)) {
+#ifdef CONFIG_X86
+			set_memory_wb((unsigned long)phys->vaddr, phys->size / PAGE_SIZE);
+#endif
+			drm_pci_free(obj->base.dev, phys);
+			return PTR_ERR(page);
+		}
+
+		src = kmap_atomic(page);
+		memcpy(vaddr, src, PAGE_SIZE);
+		kunmap_atomic(src);
+
+		mark_page_accessed(page);
+		page_cache_release(page);
+
+		vaddr += PAGE_SIZE;
+	}
+
+	obj->phys_handle = phys;
+	return 0;
+}
+
+static int
+i915_gem_phys_pwrite(struct drm_i915_gem_object *obj,
+		     struct drm_i915_gem_pwrite *args,
+		     struct drm_file *file_priv)
+{
+	struct drm_device *dev = obj->base.dev;
+	void *vaddr = obj->phys_handle->vaddr + args->offset;
+	char __user *user_data = to_user_ptr(args->data_ptr);
+
+	if (__copy_from_user_inatomic_nocache(vaddr, user_data, args->size)) {
+		unsigned long unwritten;
+
+		/* The physical object once assigned is fixed for the lifetime
+		 * of the obj, so we can safely drop the lock and continue
+		 * to access vaddr.
+		 */
+		mutex_unlock(&dev->struct_mutex);
+		unwritten = copy_from_user(vaddr, user_data, args->size);
+		mutex_lock(&dev->struct_mutex);
+		if (unwritten)
+			return -EFAULT;
+	}
+
+	i915_gem_chipset_flush(dev);
+	return 0;
+}
+
 void *i915_gem_object_alloc(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -921,8 +1039,8 @@ i915_gem_pwrite_ioctl(struct drm_device *dev, void *data,
 	 * pread/pwrite currently are reading and writing from the CPU
 	 * perspective, requiring manual detiling by the client.
 	 */
-	if (obj->phys_obj) {
-		ret = i915_gem_phys_pwrite(dev, obj, args, file);
+	if (obj->phys_handle) {
+		ret = i915_gem_phys_pwrite(obj, args, file);
 		goto out;
 	}
 
@@ -3208,12 +3326,14 @@ static struct i915_vma *
 i915_gem_object_bind_to_vm(struct drm_i915_gem_object *obj,
 			   struct i915_address_space *vm,
 			   unsigned alignment,
-			   unsigned flags)
+			   uint64_t flags)
 {
 	struct drm_device *dev = obj->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 size, fence_size, fence_alignment, unfenced_alignment;
-	size_t gtt_max =
+	unsigned long start =
+		flags & PIN_OFFSET_BIAS ? flags & PIN_OFFSET_MASK : 0;
+	unsigned long end =
 		flags & PIN_MAPPABLE ? dev_priv->gtt.mappable_end : vm->total;
 	struct i915_vma *vma;
 	int ret;
@@ -3242,11 +3362,11 @@ i915_gem_object_bind_to_vm(struct drm_i915_gem_object *obj,
 	/* If the object is bigger than the entire aperture, reject it early
 	 * before evicting everything in a vain attempt to find space.
 	 */
-	if (obj->base.size > gtt_max) {
-		DRM_DEBUG("Attempting to bind an object larger than the aperture: object=%zd > %s aperture=%zu\n",
+	if (obj->base.size > end) {
+		DRM_DEBUG("Attempting to bind an object larger than the aperture: object=%zd > %s aperture=%lu\n",
 			  obj->base.size,
 			  flags & PIN_MAPPABLE ? "mappable" : "total",
-			  gtt_max);
+			  end);
 		return ERR_PTR(-E2BIG);
 	}
 
@@ -3263,12 +3383,15 @@ i915_gem_object_bind_to_vm(struct drm_i915_gem_object *obj,
 search_free:
 	ret = drm_mm_insert_node_in_range_generic(&vm->mm, &vma->node,
 						  size, alignment,
-						  obj->cache_level, 0, gtt_max,
+						  obj->cache_level,
+						  start, end,
 						  DRM_MM_SEARCH_DEFAULT,
 						  DRM_MM_CREATE_DEFAULT);
 	if (ret) {
 		ret = i915_gem_evict_something(dev, vm, size, alignment,
-					       obj->cache_level, flags);
+					       obj->cache_level,
+					       start, end,
+					       flags);
 		if (ret == 0)
 			goto search_free;
 
@@ -3828,11 +3951,30 @@ i915_gem_ring_throttle(struct drm_device *dev, struct drm_file *file)
 	return ret;
 }
 
+static bool
+i915_vma_misplaced(struct i915_vma *vma, uint32_t alignment, uint64_t flags)
+{
+	struct drm_i915_gem_object *obj = vma->obj;
+
+	if (alignment &&
+	    vma->node.start & (alignment - 1))
+		return true;
+
+	if (flags & PIN_MAPPABLE && !obj->map_and_fenceable)
+		return true;
+
+	if (flags & PIN_OFFSET_BIAS &&
+	    vma->node.start < (flags & PIN_OFFSET_MASK))
+		return true;
+
+	return false;
+}
+
 int
 i915_gem_object_pin(struct drm_i915_gem_object *obj,
 		    struct i915_address_space *vm,
 		    uint32_t alignment,
-		    unsigned flags)
+		    uint64_t flags)
 {
 	struct i915_vma *vma;
 	int ret;
@@ -3845,15 +3987,13 @@ i915_gem_object_pin(struct drm_i915_gem_object *obj,
 		if (WARN_ON(vma->pin_count == DRM_I915_GEM_OBJECT_MAX_PIN_COUNT))
 			return -EBUSY;
 
-		if ((alignment &&
-		     vma->node.start & (alignment - 1)) ||
-		    (flags & PIN_MAPPABLE && !obj->map_and_fenceable)) {
+		if (i915_vma_misplaced(vma, alignment, flags)) {
 			WARN(vma->pin_count,
 			     "bo is already pinned with incorrect alignment:"
 			     " offset=%lx, req.alignment=%x, req.map_and_fenceable=%d,"
 			     " obj->map_and_fenceable=%d\n",
 			     i915_gem_obj_offset(obj, vm), alignment,
-			     flags & PIN_MAPPABLE,
+			     !!(flags & PIN_MAPPABLE),
 			     obj->map_and_fenceable);
 			ret = i915_vma_unbind(vma);
 			if (ret)
@@ -4163,9 +4303,6 @@ void i915_gem_free_object(struct drm_gem_object *gem_obj)
 
 	trace_i915_gem_object_destroy(obj);
 
-	if (obj->phys_obj)
-		i915_gem_detach_phys_object(dev, obj);
-
 	list_for_each_entry_safe(vma, next, &obj->vma_list, vma_link) {
 		int ret;
 
@@ -4183,6 +4320,8 @@ void i915_gem_free_object(struct drm_gem_object *gem_obj)
 		}
 	}
 
+	i915_gem_object_detach_phys(obj);
+
 	/* Stolen objects don't hold a ref, but do hold pin count. Fix that up
 	 * before progressing. */
 	if (obj->stolen)
@@ -4646,190 +4785,6 @@ i915_gem_load(struct drm_device *dev)
 	register_shrinker(&dev_priv->mm.inactive_shrinker);
 }
 
-/*
- * Create a physically contiguous memory object for this object
- * e.g. for cursor + overlay regs
- */
-static int i915_gem_init_phys_object(struct drm_device *dev,
-				     int id, int size, int align)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_i915_gem_phys_object *phys_obj;
-	int ret;
-
-	if (dev_priv->mm.phys_objs[id - 1] || !size)
-		return 0;
-
-	phys_obj = kzalloc(sizeof(*phys_obj), GFP_KERNEL);
-	if (!phys_obj)
-		return -ENOMEM;
-
-	phys_obj->id = id;
-
-	phys_obj->handle = drm_pci_alloc(dev, size, align);
-	if (!phys_obj->handle) {
-		ret = -ENOMEM;
-		goto kfree_obj;
-	}
-#ifdef CONFIG_X86
-	set_memory_wc((unsigned long)phys_obj->handle->vaddr, phys_obj->handle->size / PAGE_SIZE);
-#endif
-
-	dev_priv->mm.phys_objs[id - 1] = phys_obj;
-
-	return 0;
-kfree_obj:
-	kfree(phys_obj);
-	return ret;
-}
-
-static void i915_gem_free_phys_object(struct drm_device *dev, int id)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_i915_gem_phys_object *phys_obj;
-
-	if (!dev_priv->mm.phys_objs[id - 1])
-		return;
-
-	phys_obj = dev_priv->mm.phys_objs[id - 1];
-	if (phys_obj->cur_obj) {
-		i915_gem_detach_phys_object(dev, phys_obj->cur_obj);
-	}
-
-#ifdef CONFIG_X86
-	set_memory_wb((unsigned long)phys_obj->handle->vaddr, phys_obj->handle->size / PAGE_SIZE);
-#endif
-	drm_pci_free(dev, phys_obj->handle);
-	kfree(phys_obj);
-	dev_priv->mm.phys_objs[id - 1] = NULL;
-}
-
-void i915_gem_free_all_phys_object(struct drm_device *dev)
-{
-	int i;
-
-	for (i = I915_GEM_PHYS_CURSOR_0; i <= I915_MAX_PHYS_OBJECT; i++)
-		i915_gem_free_phys_object(dev, i);
-}
-
-void i915_gem_detach_phys_object(struct drm_device *dev,
-				 struct drm_i915_gem_object *obj)
-{
-	struct address_space *mapping = file_inode(obj->base.filp)->i_mapping;
-	char *vaddr;
-	int i;
-	int page_count;
-
-	if (!obj->phys_obj)
-		return;
-	vaddr = obj->phys_obj->handle->vaddr;
-
-	page_count = obj->base.size / PAGE_SIZE;
-	for (i = 0; i < page_count; i++) {
-		struct page *page = shmem_read_mapping_page(mapping, i);
-		if (!IS_ERR(page)) {
-			char *dst = kmap_atomic(page);
-			memcpy(dst, vaddr + i*PAGE_SIZE, PAGE_SIZE);
-			kunmap_atomic(dst);
-
-			drm_clflush_pages(&page, 1);
-
-			set_page_dirty(page);
-			mark_page_accessed(page);
-			page_cache_release(page);
-		}
-	}
-	i915_gem_chipset_flush(dev);
-
-	obj->phys_obj->cur_obj = NULL;
-	obj->phys_obj = NULL;
-}
-
-int
-i915_gem_attach_phys_object(struct drm_device *dev,
-			    struct drm_i915_gem_object *obj,
-			    int id,
-			    int align)
-{
-	struct address_space *mapping = file_inode(obj->base.filp)->i_mapping;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int ret = 0;
-	int page_count;
-	int i;
-
-	if (id > I915_MAX_PHYS_OBJECT)
-		return -EINVAL;
-
-	if (obj->phys_obj) {
-		if (obj->phys_obj->id == id)
-			return 0;
-		i915_gem_detach_phys_object(dev, obj);
-	}
-
-	/* create a new object */
-	if (!dev_priv->mm.phys_objs[id - 1]) {
-		ret = i915_gem_init_phys_object(dev, id,
-						obj->base.size, align);
-		if (ret) {
-			DRM_ERROR("failed to init phys object %d size: %zu\n",
-				  id, obj->base.size);
-			return ret;
-		}
-	}
-
-	/* bind to the object */
-	obj->phys_obj = dev_priv->mm.phys_objs[id - 1];
-	obj->phys_obj->cur_obj = obj;
-
-	page_count = obj->base.size / PAGE_SIZE;
-
-	for (i = 0; i < page_count; i++) {
-		struct page *page;
-		char *dst, *src;
-
-		page = shmem_read_mapping_page(mapping, i);
-		if (IS_ERR(page))
-			return PTR_ERR(page);
-
-		src = kmap_atomic(page);
-		dst = obj->phys_obj->handle->vaddr + (i * PAGE_SIZE);
-		memcpy(dst, src, PAGE_SIZE);
-		kunmap_atomic(src);
-
-		mark_page_accessed(page);
-		page_cache_release(page);
-	}
-
-	return 0;
-}
-
-static int
-i915_gem_phys_pwrite(struct drm_device *dev,
-		     struct drm_i915_gem_object *obj,
-		     struct drm_i915_gem_pwrite *args,
-		     struct drm_file *file_priv)
-{
-	void *vaddr = obj->phys_obj->handle->vaddr + args->offset;
-	char __user *user_data = to_user_ptr(args->data_ptr);
-
-	if (__copy_from_user_inatomic_nocache(vaddr, user_data, args->size)) {
-		unsigned long unwritten;
-
-		/* The physical object once assigned is fixed for the lifetime
-		 * of the obj, so we can safely drop the lock and continue
-		 * to access vaddr.
-		 */
-		mutex_unlock(&dev->struct_mutex);
-		unwritten = copy_from_user(vaddr, user_data, args->size);
-		mutex_lock(&dev->struct_mutex);
-		if (unwritten)
-			return -EFAULT;
-	}
-
-	i915_gem_chipset_flush(dev);
-	return 0;
-}
-
 void i915_gem_release(struct drm_device *dev, struct drm_file *file)
 {
 	struct drm_i915_file_private *file_priv = file->driver_priv;
diff --git a/drivers/gpu/drm/i915/i915_gem_evict.c b/drivers/gpu/drm/i915/i915_gem_evict.c
index 75fca63dc8c..bbf4b12d842 100644
--- a/drivers/gpu/drm/i915/i915_gem_evict.c
+++ b/drivers/gpu/drm/i915/i915_gem_evict.c
@@ -68,9 +68,9 @@ mark_free(struct i915_vma *vma, struct list_head *unwind)
 int
 i915_gem_evict_something(struct drm_device *dev, struct i915_address_space *vm,
 			 int min_size, unsigned alignment, unsigned cache_level,
+			 unsigned long start, unsigned long end,
 			 unsigned flags)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct list_head eviction_list, unwind_list;
 	struct i915_vma *vma;
 	int ret = 0;
@@ -102,11 +102,10 @@ i915_gem_evict_something(struct drm_device *dev, struct i915_address_space *vm,
 	 */
 
 	INIT_LIST_HEAD(&unwind_list);
-	if (flags & PIN_MAPPABLE) {
-		BUG_ON(!i915_is_ggtt(vm));
+	if (start != 0 || end != vm->total) {
 		drm_mm_init_scan_with_range(&vm->mm, min_size,
-					    alignment, cache_level, 0,
-					    dev_priv->gtt.mappable_end);
+					    alignment, cache_level,
+					    start, end);
 	} else
 		drm_mm_init_scan(&vm->mm, min_size, alignment, cache_level);
 
diff --git a/drivers/gpu/drm/i915/i915_gem_execbuffer.c b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
index 2c9d9cbaf65..20fef6c5026 100644
--- a/drivers/gpu/drm/i915/i915_gem_execbuffer.c
+++ b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
@@ -35,6 +35,9 @@
 
 #define  __EXEC_OBJECT_HAS_PIN (1<<31)
 #define  __EXEC_OBJECT_HAS_FENCE (1<<30)
+#define  __EXEC_OBJECT_NEEDS_BIAS (1<<28)
+
+#define BATCH_OFFSET_BIAS (256*1024)
 
 struct eb_vmas {
 	struct list_head vmas;
@@ -545,7 +548,7 @@ i915_gem_execbuffer_reserve_vma(struct i915_vma *vma,
 	struct drm_i915_gem_exec_object2 *entry = vma->exec_entry;
 	bool has_fenced_gpu_access = INTEL_INFO(ring->dev)->gen < 4;
 	bool need_fence;
-	unsigned flags;
+	uint64_t flags;
 	int ret;
 
 	flags = 0;
@@ -559,6 +562,8 @@ i915_gem_execbuffer_reserve_vma(struct i915_vma *vma,
 
 	if (entry->flags & EXEC_OBJECT_NEEDS_GTT)
 		flags |= PIN_GLOBAL;
+	if (entry->flags & __EXEC_OBJECT_NEEDS_BIAS)
+		flags |= BATCH_OFFSET_BIAS | PIN_OFFSET_BIAS;
 
 	ret = i915_gem_object_pin(obj, vma->vm, entry->alignment, flags);
 	if (ret)
@@ -592,6 +597,36 @@ i915_gem_execbuffer_reserve_vma(struct i915_vma *vma,
 	return 0;
 }
 
+static bool
+eb_vma_misplaced(struct i915_vma *vma, bool has_fenced_gpu_access)
+{
+	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);
+
+	WARN_ON((need_mappable || need_fence) &&
+	       !i915_is_ggtt(vma->vm));
+
+	if (entry->alignment &&
+	    vma->node.start & (entry->alignment - 1))
+		return true;
+
+	if (need_mappable && !obj->map_and_fenceable)
+		return true;
+
+	if (entry->flags & __EXEC_OBJECT_NEEDS_BIAS &&
+	    vma->node.start < BATCH_OFFSET_BIAS)
+		return true;
+
+	return false;
+}
+
 static int
 i915_gem_execbuffer_reserve(struct intel_ring_buffer *ring,
 			    struct list_head *vmas,
@@ -653,26 +688,10 @@ i915_gem_execbuffer_reserve(struct intel_ring_buffer *ring,
 
 		/* Unbind any ill-fitting objects or pin. */
 		list_for_each_entry(vma, vmas, exec_list) {
-			struct drm_i915_gem_exec_object2 *entry = vma->exec_entry;
-			bool need_fence, need_mappable;
-
-			obj = vma->obj;
-
 			if (!drm_mm_node_allocated(&vma->node))
 				continue;
 
-			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);
-
-			WARN_ON((need_mappable || need_fence) &&
-			       !i915_is_ggtt(vma->vm));
-
-			if ((entry->alignment &&
-			     vma->node.start & (entry->alignment - 1)) ||
-			    (need_mappable && !obj->map_and_fenceable))
+			if (eb_vma_misplaced(vma, has_fenced_gpu_access))
 				ret = i915_vma_unbind(vma);
 			else
 				ret = i915_gem_execbuffer_reserve_vma(vma, ring, need_relocs);
@@ -773,9 +792,9 @@ i915_gem_execbuffer_relocate_slow(struct drm_device *dev,
 		 * relocations were valid.
 		 */
 		for (j = 0; j < exec[i].relocation_count; j++) {
-			if (copy_to_user(&user_relocs[j].presumed_offset,
-					 &invalid_offset,
-					 sizeof(invalid_offset))) {
+			if (__copy_to_user(&user_relocs[j].presumed_offset,
+					   &invalid_offset,
+					   sizeof(invalid_offset))) {
 				ret = -EFAULT;
 				mutex_lock(&dev->struct_mutex);
 				goto err;
@@ -999,6 +1018,25 @@ i915_reset_gen7_sol_offsets(struct drm_device *dev,
 	return 0;
 }
 
+static struct drm_i915_gem_object *
+eb_get_batch(struct eb_vmas *eb)
+{
+	struct i915_vma *vma = list_entry(eb->vmas.prev, typeof(*vma), exec_list);
+
+	/*
+	 * SNA is doing fancy tricks with compressing batch buffers, which leads
+	 * to negative relocation deltas. Usually that works out ok since the
+	 * relocate address is still positive, except when the batch is placed
+	 * very low in the GTT. Ensure this doesn't happen.
+	 *
+	 * Note that actual hangs have only been observed on gen7, but for
+	 * paranoia do it everywhere.
+	 */
+	vma->exec_entry->flags |= __EXEC_OBJECT_NEEDS_BIAS;
+
+	return vma->obj;
+}
+
 static int
 i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 		       struct drm_file *file,
@@ -1153,7 +1191,7 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 		goto err;
 
 	/* take note of the batch buffer before we might reorder the lists */
-	batch_obj = list_entry(eb->vmas.prev, struct i915_vma, exec_list)->obj;
+	batch_obj = eb_get_batch(eb);
 
 	/* Move the objects en-masse into the GTT, evicting if necessary. */
 	need_relocs = (args->flags & I915_EXEC_NO_RELOC) == 0;
@@ -1355,18 +1393,21 @@ i915_gem_execbuffer(struct drm_device *dev, void *data,
 
 	ret = i915_gem_do_execbuffer(dev, data, file, &exec2, exec2_list);
 	if (!ret) {
+		struct drm_i915_gem_exec_object __user *user_exec_list =
+			to_user_ptr(args->buffers_ptr);
+
 		/* Copy the new buffer offsets back to the user's exec list. */
-		for (i = 0; i < args->buffer_count; i++)
-			exec_list[i].offset = exec2_list[i].offset;
-		/* ... and back out to userspace */
-		ret = copy_to_user(to_user_ptr(args->buffers_ptr),
-				   exec_list,
-				   sizeof(*exec_list) * args->buffer_count);
-		if (ret) {
-			ret = -EFAULT;
-			DRM_DEBUG("failed to copy %d exec entries "
-				  "back to user (%d)\n",
-				  args->buffer_count, ret);
+		for (i = 0; i < args->buffer_count; i++) {
+			ret = __copy_to_user(&user_exec_list[i].offset,
+					     &exec2_list[i].offset,
+					     sizeof(user_exec_list[i].offset));
+			if (ret) {
+				ret = -EFAULT;
+				DRM_DEBUG("failed to copy %d exec entries "
+					  "back to user (%d)\n",
+					  args->buffer_count, ret);
+				break;
+			}
 		}
 	}
 
@@ -1412,14 +1453,21 @@ i915_gem_execbuffer2(struct drm_device *dev, void *data,
 	ret = i915_gem_do_execbuffer(dev, data, file, args, exec2_list);
 	if (!ret) {
 		/* Copy the new buffer offsets back to the user's exec list. */
-		ret = copy_to_user(to_user_ptr(args->buffers_ptr),
-				   exec2_list,
-				   sizeof(*exec2_list) * args->buffer_count);
-		if (ret) {
-			ret = -EFAULT;
-			DRM_DEBUG("failed to copy %d exec entries "
-				  "back to user (%d)\n",
-				  args->buffer_count, ret);
+		struct drm_i915_gem_exec_object2 *user_exec_list =
+				   to_user_ptr(args->buffers_ptr);
+		int i;
+
+		for (i = 0; i < args->buffer_count; i++) {
+			ret = __copy_to_user(&user_exec_list[i].offset,
+					     &exec2_list[i].offset,
+					     sizeof(user_exec_list[i].offset));
+			if (ret) {
+				ret = -EFAULT;
+				DRM_DEBUG("failed to copy %d exec entries "
+					  "back to user\n",
+					  args->buffer_count);
+				break;
+			}
 		}
 	}
 
diff --git a/drivers/gpu/drm/i915/i915_gem_gtt.c b/drivers/gpu/drm/i915/i915_gem_gtt.c
index 154b0f8bb88..5deb22864c5 100644
--- a/drivers/gpu/drm/i915/i915_gem_gtt.c
+++ b/drivers/gpu/drm/i915/i915_gem_gtt.c
@@ -1089,7 +1089,9 @@ alloc:
 	if (ret == -ENOSPC && !retried) {
 		ret = i915_gem_evict_something(dev, &dev_priv->gtt.base,
 					       GEN6_PD_SIZE, GEN6_PD_ALIGN,
-					       I915_CACHE_NONE, 0);
+					       I915_CACHE_NONE,
+					       0, dev_priv->gtt.base.total,
+					       0);
 		if (ret)
 			return ret;
 
diff --git a/drivers/gpu/drm/i915/i915_irq.c b/drivers/gpu/drm/i915/i915_irq.c
index f98ba4e6e70..0b99de95593 100644
--- a/drivers/gpu/drm/i915/i915_irq.c
+++ b/drivers/gpu/drm/i915/i915_irq.c
@@ -2157,7 +2157,7 @@ static void i915_error_work_func(struct work_struct *work)
 			 * updates before
 			 * the counter increment.
 			 */
-			smp_mb__before_atomic_inc();
+			smp_mb__before_atomic();
 			atomic_inc(&dev_priv->gpu_error.reset_counter);
 
 			kobject_uevent_env(&dev->primary->kdev->kobj,
diff --git a/drivers/gpu/drm/i915/intel_bios.c b/drivers/gpu/drm/i915/intel_bios.c
index fa486c5fbb0..aff4a113cda 100644
--- a/drivers/gpu/drm/i915/intel_bios.c
+++ b/drivers/gpu/drm/i915/intel_bios.c
@@ -560,47 +560,71 @@ parse_edp(struct drm_i915_private *dev_priv, struct bdb_header *bdb)
 
 	dev_priv->vbt.edp_pps = *edp_pps;
 
-	dev_priv->vbt.edp_rate = edp_link_params->rate ? DP_LINK_BW_2_7 :
-		DP_LINK_BW_1_62;
+	switch (edp_link_params->rate) {
+	case EDP_RATE_1_62:
+		dev_priv->vbt.edp_rate = DP_LINK_BW_1_62;
+		break;
+	case EDP_RATE_2_7:
+		dev_priv->vbt.edp_rate = DP_LINK_BW_2_7;
+		break;
+	default:
+		DRM_DEBUG_KMS("VBT has unknown eDP link rate value %u\n",
+			      edp_link_params->rate);
+		break;
+	}
+
 	switch (edp_link_params->lanes) {
-	case 0:
+	case EDP_LANE_1:
 		dev_priv->vbt.edp_lanes = 1;
 		break;
-	case 1:
+	case EDP_LANE_2:
 		dev_priv->vbt.edp_lanes = 2;
 		break;
-	case 3:
-	default:
+	case EDP_LANE_4:
 		dev_priv->vbt.edp_lanes = 4;
 		break;
+	default:
+		DRM_DEBUG_KMS("VBT has unknown eDP lane count value %u\n",
+			      edp_link_params->lanes);
+		break;
 	}
+
 	switch (edp_link_params->preemphasis) {
-	case 0:
+	case EDP_PREEMPHASIS_NONE:
 		dev_priv->vbt.edp_preemphasis = DP_TRAIN_PRE_EMPHASIS_0;
 		break;
-	case 1:
+	case EDP_PREEMPHASIS_3_5dB:
 		dev_priv->vbt.edp_preemphasis = DP_TRAIN_PRE_EMPHASIS_3_5;
 		break;
-	case 2:
+	case EDP_PREEMPHASIS_6dB:
 		dev_priv->vbt.edp_preemphasis = DP_TRAIN_PRE_EMPHASIS_6;
 		break;
-	case 3:
+	case EDP_PREEMPHASIS_9_5dB:
 		dev_priv->vbt.edp_preemphasis = DP_TRAIN_PRE_EMPHASIS_9_5;
 		break;
+	default:
+		DRM_DEBUG_KMS("VBT has unknown eDP pre-emphasis value %u\n",
+			      edp_link_params->preemphasis);
+		break;
 	}
+
 	switch (edp_link_params->vswing) {
-	case 0:
+	case EDP_VSWING_0_4V:
 		dev_priv->vbt.edp_vswing = DP_TRAIN_VOLTAGE_SWING_400;
 		break;
-	case 1:
+	case EDP_VSWING_0_6V:
 		dev_priv->vbt.edp_vswing = DP_TRAIN_VOLTAGE_SWING_600;
 		break;
-	case 2:
+	case EDP_VSWING_0_8V:
 		dev_priv->vbt.edp_vswing = DP_TRAIN_VOLTAGE_SWING_800;
 		break;
-	case 3:
+	case EDP_VSWING_1_2V:
 		dev_priv->vbt.edp_vswing = DP_TRAIN_VOLTAGE_SWING_1200;
 		break;
+	default:
+		DRM_DEBUG_KMS("VBT has unknown eDP voltage swing value %u\n",
+			      edp_link_params->vswing);
+		break;
 	}
 }
 
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index 48aa516a1ac..5b60e25baa3 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -7825,14 +7825,12 @@ static int intel_crtc_cursor_set(struct drm_crtc *crtc,
 		addr = i915_gem_obj_ggtt_offset(obj);
 	} else {
 		int align = IS_I830(dev) ? 16 * 1024 : 256;
-		ret = i915_gem_attach_phys_object(dev, obj,
-						  (intel_crtc->pipe == 0) ? I915_GEM_PHYS_CURSOR_0 : I915_GEM_PHYS_CURSOR_1,
-						  align);
+		ret = i915_gem_object_attach_phys(obj, align);
 		if (ret) {
 			DRM_DEBUG_KMS("failed to attach phys object\n");
 			goto fail_locked;
 		}
-		addr = obj->phys_obj->handle->busaddr;
+		addr = obj->phys_handle->busaddr;
 	}
 
 	if (IS_GEN2(dev))
@@ -7840,10 +7838,7 @@ static int intel_crtc_cursor_set(struct drm_crtc *crtc,
 
  finish:
 	if (intel_crtc->cursor_bo) {
-		if (INTEL_INFO(dev)->cursor_needs_physical) {
-			if (intel_crtc->cursor_bo != obj)
-				i915_gem_detach_phys_object(dev, intel_crtc->cursor_bo);
-		} else
+		if (!INTEL_INFO(dev)->cursor_needs_physical)
 			i915_gem_object_unpin_from_display_plane(intel_crtc->cursor_bo);
 		drm_gem_object_unreference(&intel_crtc->cursor_bo->base);
 	}
diff --git a/drivers/gpu/drm/i915/intel_dp.c b/drivers/gpu/drm/i915/intel_dp.c
index 5ca68aa9f23..2a00cb828d2 100644
--- a/drivers/gpu/drm/i915/intel_dp.c
+++ b/drivers/gpu/drm/i915/intel_dp.c
@@ -121,6 +121,22 @@ intel_dp_max_link_bw(struct intel_dp *intel_dp)
 	return max_link_bw;
 }
 
+static u8 intel_dp_max_lane_count(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;
+	u8 source_max, sink_max;
+
+	source_max = 4;
+	if (HAS_DDI(dev) && intel_dig_port->port == PORT_A &&
+	    (intel_dig_port->saved_port_bits & DDI_A_4_LANES) == 0)
+		source_max = 2;
+
+	sink_max = drm_dp_max_lane_count(intel_dp->dpcd);
+
+	return min(source_max, sink_max);
+}
+
 /*
  * The units on the numbers in the next two are... bizarre.  Examples will
  * make it clearer; this one parallels an example in the eDP spec.
@@ -171,7 +187,7 @@ intel_dp_mode_valid(struct drm_connector *connector,
 	}
 
 	max_link_clock = drm_dp_bw_code_to_link_rate(intel_dp_max_link_bw(intel_dp));
-	max_lanes = drm_dp_max_lane_count(intel_dp->dpcd);
+	max_lanes = intel_dp_max_lane_count(intel_dp);
 
 	max_rate = intel_dp_max_data_rate(max_link_clock, max_lanes);
 	mode_rate = intel_dp_link_required(target_clock, 18);
@@ -751,8 +767,10 @@ intel_dp_compute_config(struct intel_encoder *encoder,
 	struct intel_crtc *intel_crtc = encoder->new_crtc;
 	struct intel_connector *intel_connector = intel_dp->attached_connector;
 	int lane_count, clock;
-	int max_lane_count = drm_dp_max_lane_count(intel_dp->dpcd);
+	int min_lane_count = 1;
+	int max_lane_count = intel_dp_max_lane_count(intel_dp);
 	/* Conveniently, the link BW constants become indices with a shift...*/
+	int min_clock = 0;
 	int max_clock = intel_dp_max_link_bw(intel_dp) >> 3;
 	int bpp, mode_rate;
 	static int bws[] = { DP_LINK_BW_1_62, DP_LINK_BW_2_7, DP_LINK_BW_5_4 };
@@ -785,19 +803,38 @@ intel_dp_compute_config(struct intel_encoder *encoder,
 	/* Walk through all bpp values. Luckily they're all nicely spaced with 2
 	 * bpc in between. */
 	bpp = pipe_config->pipe_bpp;
-	if (is_edp(intel_dp) && dev_priv->vbt.edp_bpp &&
-	    dev_priv->vbt.edp_bpp < bpp) {
-		DRM_DEBUG_KMS("clamping bpp for eDP panel to BIOS-provided %i\n",
-			      dev_priv->vbt.edp_bpp);
-		bpp = dev_priv->vbt.edp_bpp;
+	if (is_edp(intel_dp)) {
+		if (dev_priv->vbt.edp_bpp && dev_priv->vbt.edp_bpp < bpp) {
+			DRM_DEBUG_KMS("clamping bpp for eDP panel to BIOS-provided %i\n",
+				      dev_priv->vbt.edp_bpp);
+			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]);
+		}
 	}
 
 	for (; bpp >= 6*3; bpp -= 2*3) {
 		mode_rate = intel_dp_link_required(adjusted_mode->crtc_clock,
 						   bpp);
 
-		for (lane_count = 1; lane_count <= max_lane_count; lane_count <<= 1) {
-			for (clock = 0; clock <= max_clock; clock++) {
+		for (lane_count = min_lane_count; lane_count <= max_lane_count; lane_count <<= 1) {
+			for (clock = min_clock; clock <= max_clock; clock++) {
 				link_clock = drm_dp_bw_code_to_link_rate(bws[clock]);
 				link_avail = intel_dp_max_data_rate(link_clock,
 								    lane_count);
diff --git a/drivers/gpu/drm/i915/intel_fbdev.c b/drivers/gpu/drm/i915/intel_fbdev.c
index fce4a0d93c0..f73ba5e6b7a 100644
--- a/drivers/gpu/drm/i915/intel_fbdev.c
+++ b/drivers/gpu/drm/i915/intel_fbdev.c
@@ -387,6 +387,15 @@ static bool intel_fb_initial_config(struct drm_fb_helper *fb_helper,
 							  height);
 		}
 
+		/* No preferred mode marked by the EDID? Are there any modes? */
+		if (!modes[i] && !list_empty(&connector->modes)) {
+			DRM_DEBUG_KMS("using first mode listed on connector %s\n",
+				      drm_get_connector_name(connector));
+			modes[i] = list_first_entry(&connector->modes,
+						    struct drm_display_mode,
+						    head);
+		}
+
 		/* last resort: use current mode */
 		if (!modes[i]) {
 			/*
diff --git a/drivers/gpu/drm/i915/intel_overlay.c b/drivers/gpu/drm/i915/intel_overlay.c
index d8adc9104dc..129db0c7d83 100644
--- a/drivers/gpu/drm/i915/intel_overlay.c
+++ b/drivers/gpu/drm/i915/intel_overlay.c
@@ -193,7 +193,7 @@ intel_overlay_map_regs(struct intel_overlay *overlay)
 	struct overlay_registers __iomem *regs;
 
 	if (OVERLAY_NEEDS_PHYSICAL(overlay->dev))
-		regs = (struct overlay_registers __iomem *)overlay->reg_bo->phys_obj->handle->vaddr;
+		regs = (struct overlay_registers __iomem *)overlay->reg_bo->phys_handle->vaddr;
 	else
 		regs = io_mapping_map_wc(dev_priv->gtt.mappable,
 					 i915_gem_obj_ggtt_offset(overlay->reg_bo));
@@ -1340,14 +1340,12 @@ void intel_setup_overlay(struct drm_device *dev)
 	overlay->reg_bo = reg_bo;
 
 	if (OVERLAY_NEEDS_PHYSICAL(dev)) {
-		ret = i915_gem_attach_phys_object(dev, reg_bo,
-						  I915_GEM_PHYS_OVERLAY_REGS,
-						  PAGE_SIZE);
+		ret = i915_gem_object_attach_phys(reg_bo, PAGE_SIZE);
 		if (ret) {
 			DRM_ERROR("failed to attach phys overlay regs\n");
 			goto out_free_bo;
 		}
-		overlay->flip_addr = reg_bo->phys_obj->handle->busaddr;
+		overlay->flip_addr = reg_bo->phys_handle->busaddr;
 	} else {
 		ret = i915_gem_obj_ggtt_pin(reg_bo, PAGE_SIZE, PIN_MAPPABLE);
 		if (ret) {
@@ -1428,7 +1426,7 @@ intel_overlay_map_regs_atomic(struct intel_overlay *overlay)
 		/* Cast to make sparse happy, but it's wc memory anyway, so
 		 * equivalent to the wc io mapping on X86. */
 		regs = (struct overlay_registers __iomem *)
-			overlay->reg_bo->phys_obj->handle->vaddr;
+			overlay->reg_bo->phys_handle->vaddr;
 	else
 		regs = io_mapping_map_atomic_wc(dev_priv->gtt.mappable,
 						i915_gem_obj_ggtt_offset(overlay->reg_bo));
@@ -1462,7 +1460,7 @@ intel_overlay_capture_error_state(struct drm_device *dev)
 	error->dovsta = I915_READ(DOVSTA);
 	error->isr = I915_READ(ISR);
 	if (OVERLAY_NEEDS_PHYSICAL(overlay->dev))
-		error->base = (__force long)overlay->reg_bo->phys_obj->handle->vaddr;
+		error->base = (__force long)overlay->reg_bo->phys_handle->vaddr;
 	else
 		error->base = i915_gem_obj_ggtt_offset(overlay->reg_bo);
 
diff --git a/drivers/gpu/drm/i915/intel_panel.c b/drivers/gpu/drm/i915/intel_panel.c
index 0eead16aeda..cb8cfb7e097 100644
--- a/drivers/gpu/drm/i915/intel_panel.c
+++ b/drivers/gpu/drm/i915/intel_panel.c
@@ -492,6 +492,7 @@ void intel_panel_set_backlight(struct intel_connector *connector, u32 level,
 	enum pipe pipe = intel_get_pipe_from_connector(connector);
 	u32 freq;
 	unsigned long flags;
+	u64 n;
 
 	if (!panel->backlight.present || pipe == INVALID_PIPE)
 		return;
@@ -502,10 +503,9 @@ void intel_panel_set_backlight(struct intel_connector *connector, u32 level,
 
 	/* scale to hardware max, but be careful to not overflow */
 	freq = panel->backlight.max;
-	if (freq < max)
-		level = level * freq / max;
-	else
-		level = freq / max * level;
+	n = (u64)level * freq;
+	do_div(n, max);
+	level = n;
 
 	panel->backlight.level = level;
 	if (panel->backlight.device)
diff --git a/drivers/gpu/drm/i915/intel_pm.c b/drivers/gpu/drm/i915/intel_pm.c
index 19e94c3edc1..d93dcf683e8 100644
--- a/drivers/gpu/drm/i915/intel_pm.c
+++ b/drivers/gpu/drm/i915/intel_pm.c
@@ -2095,6 +2095,43 @@ static void intel_print_wm_latency(struct drm_device *dev,
 	}
 }
 
+static bool ilk_increase_wm_latency(struct drm_i915_private *dev_priv,
+				    uint16_t wm[5], uint16_t min)
+{
+	int level, max_level = ilk_wm_max_level(dev_priv->dev);
+
+	if (wm[0] >= min)
+		return false;
+
+	wm[0] = max(wm[0], min);
+	for (level = 1; level <= max_level; level++)
+		wm[level] = max_t(uint16_t, wm[level], DIV_ROUND_UP(min, 5));
+
+	return true;
+}
+
+static void snb_wm_latency_quirk(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	bool changed;
+
+	/*
+	 * The BIOS provided WM memory latency values are often
+	 * inadequate for high resolution displays. Adjust them.
+	 */
+	changed = ilk_increase_wm_latency(dev_priv, dev_priv->wm.pri_latency, 12) |
+		ilk_increase_wm_latency(dev_priv, dev_priv->wm.spr_latency, 12) |
+		ilk_increase_wm_latency(dev_priv, dev_priv->wm.cur_latency, 12);
+
+	if (!changed)
+		return;
+
+	DRM_DEBUG_KMS("WM latency values increased to avoid potential underruns\n");
+	intel_print_wm_latency(dev, "Primary", dev_priv->wm.pri_latency);
+	intel_print_wm_latency(dev, "Sprite", dev_priv->wm.spr_latency);
+	intel_print_wm_latency(dev, "Cursor", dev_priv->wm.cur_latency);
+}
+
 static void ilk_setup_wm_latency(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -2112,6 +2149,9 @@ static void ilk_setup_wm_latency(struct drm_device *dev)
 	intel_print_wm_latency(dev, "Primary", dev_priv->wm.pri_latency);
 	intel_print_wm_latency(dev, "Sprite", dev_priv->wm.spr_latency);
 	intel_print_wm_latency(dev, "Cursor", dev_priv->wm.cur_latency);
+
+	if (IS_GEN6(dev))
+		snb_wm_latency_quirk(dev);
 }
 
 static void ilk_compute_wm_parameters(struct drm_crtc *crtc,
diff --git a/drivers/gpu/drm/i915/intel_sdvo.c b/drivers/gpu/drm/i915/intel_sdvo.c
index d27155adf5d..46be00d66df 100644
--- a/drivers/gpu/drm/i915/intel_sdvo.c
+++ b/drivers/gpu/drm/i915/intel_sdvo.c
@@ -2424,8 +2424,8 @@ intel_sdvo_connector_init(struct intel_sdvo_connector *connector,
 	if (ret < 0)
 		goto err1;
 
-	ret = sysfs_create_link(&encoder->ddc.dev.kobj,
-				&drm_connector->kdev->kobj,
+	ret = sysfs_create_link(&drm_connector->kdev->kobj,
+				&encoder->ddc.dev.kobj,
 				encoder->ddc.dev.kobj.name);
 	if (ret < 0)
 		goto err2;
diff --git a/drivers/gpu/drm/i915/intel_uncore.c b/drivers/gpu/drm/i915/intel_uncore.c
index f729dc71d5b..d0c75779d3f 100644
--- a/drivers/gpu/drm/i915/intel_uncore.c
+++ b/drivers/gpu/drm/i915/intel_uncore.c
@@ -185,6 +185,8 @@ static void vlv_force_wake_reset(struct drm_i915_private *dev_priv)
 {
 	__raw_i915_write32(dev_priv, FORCEWAKE_VLV,
 			   _MASKED_BIT_DISABLE(0xffff));
+	__raw_i915_write32(dev_priv, FORCEWAKE_MEDIA_VLV,
+			   _MASKED_BIT_DISABLE(0xffff));
 	/* something from same cacheline, but !FORCEWAKE_VLV */
 	__raw_posting_read(dev_priv, FORCEWAKE_ACK_VLV);
 }
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nvd0.c b/drivers/gpu/drm/nouveau/core/engine/disp/nvd0.c
index 7762665ad8f..876de9ac379 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/nvd0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nvd0.c
@@ -1009,7 +1009,7 @@ exec_clkcmp(struct nv50_disp_priv *priv, int head, int id,
 	}
 
 	if (outp == 8)
-		return false;
+		return conf;
 
 	data = exec_lookup(priv, head, outp, ctrl, dcb, &ver, &hdr, &cnt, &len, &info1);
 	if (data == 0x0000)
diff --git a/drivers/gpu/drm/nouveau/core/subdev/therm/nvd0.c b/drivers/gpu/drm/nouveau/core/subdev/therm/nvd0.c
index 43fec17ea54..bbf117be572 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/therm/nvd0.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/therm/nvd0.c
@@ -40,6 +40,7 @@ pwm_info(struct nouveau_therm *therm, int line)
 		case 0x00: return 2;
 		case 0x19: return 1;
 		case 0x1c: return 0;
+		case 0x1e: return 2;
 		default:
 			break;
 		}
diff --git a/drivers/gpu/drm/nouveau/nouveau_backlight.c b/drivers/gpu/drm/nouveau/nouveau_backlight.c
index 630f6e84fc0..2c1e4aad7da 100644
--- a/drivers/gpu/drm/nouveau/nouveau_backlight.c
+++ b/drivers/gpu/drm/nouveau/nouveau_backlight.c
@@ -31,7 +31,6 @@
  */
 
 #include <linux/backlight.h>
-#include <linux/acpi.h>
 
 #include "nouveau_drm.h"
 #include "nouveau_reg.h"
@@ -222,14 +221,6 @@ nouveau_backlight_init(struct drm_device *dev)
 	struct nouveau_device *device = nv_device(drm->device);
 	struct drm_connector *connector;
 
-#ifdef CONFIG_ACPI
-	if (acpi_video_backlight_support()) {
-		NV_INFO(drm, "ACPI backlight interface available, "
-			     "not registering our own\n");
-		return 0;
-	}
-#endif
-
 	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
 		if (connector->connector_type != DRM_MODE_CONNECTOR_LVDS &&
 		    connector->connector_type != DRM_MODE_CONNECTOR_eDP)
diff --git a/drivers/gpu/drm/radeon/atombios_crtc.c b/drivers/gpu/drm/radeon/atombios_crtc.c
index c31c12b4e66..e911898348f 100644
--- a/drivers/gpu/drm/radeon/atombios_crtc.c
+++ b/drivers/gpu/drm/radeon/atombios_crtc.c
@@ -270,8 +270,6 @@ void atombios_crtc_dpms(struct drm_crtc *crtc, int mode)
 	switch (mode) {
 	case DRM_MODE_DPMS_ON:
 		radeon_crtc->enabled = true;
-		/* adjust pm to dpms changes BEFORE enabling crtcs */
-		radeon_pm_compute_clocks(rdev);
 		atombios_enable_crtc(crtc, ATOM_ENABLE);
 		if (ASIC_IS_DCE3(rdev) && !ASIC_IS_DCE6(rdev))
 			atombios_enable_crtc_memreq(crtc, ATOM_ENABLE);
@@ -289,10 +287,10 @@ void atombios_crtc_dpms(struct drm_crtc *crtc, int mode)
 			atombios_enable_crtc_memreq(crtc, ATOM_DISABLE);
 		atombios_enable_crtc(crtc, ATOM_DISABLE);
 		radeon_crtc->enabled = false;
-		/* adjust pm to dpms changes AFTER disabling crtcs */
-		radeon_pm_compute_clocks(rdev);
 		break;
 	}
+	/* adjust pm to dpms */
+	radeon_pm_compute_clocks(rdev);
 }
 
 static void
diff --git a/drivers/gpu/drm/radeon/radeon.h b/drivers/gpu/drm/radeon/radeon.h
index 68528619834..8149e7cf430 100644
--- a/drivers/gpu/drm/radeon/radeon.h
+++ b/drivers/gpu/drm/radeon/radeon.h
@@ -1642,6 +1642,7 @@ struct radeon_vce {
 	unsigned		fb_version;
 	atomic_t		handles[RADEON_MAX_VCE_HANDLES];
 	struct drm_file		*filp[RADEON_MAX_VCE_HANDLES];
+	unsigned		img_size[RADEON_MAX_VCE_HANDLES];
 	struct delayed_work	idle_work;
 };
 
@@ -1655,7 +1656,7 @@ int radeon_vce_get_destroy_msg(struct radeon_device *rdev, int ring,
 			       uint32_t handle, struct radeon_fence **fence);
 void radeon_vce_free_handles(struct radeon_device *rdev, struct drm_file *filp);
 void radeon_vce_note_usage(struct radeon_device *rdev);
-int radeon_vce_cs_reloc(struct radeon_cs_parser *p, int lo, int hi);
+int radeon_vce_cs_reloc(struct radeon_cs_parser *p, int lo, int hi, unsigned size);
 int radeon_vce_cs_parse(struct radeon_cs_parser *p);
 bool radeon_vce_semaphore_emit(struct radeon_device *rdev,
 			       struct radeon_ring *ring,
@@ -2640,7 +2641,8 @@ void r100_pll_errata_after_index(struct radeon_device *rdev);
 #define ASIC_IS_DCE8(rdev) ((rdev->family >= CHIP_BONAIRE))
 #define ASIC_IS_DCE81(rdev) ((rdev->family == CHIP_KAVERI))
 #define ASIC_IS_DCE82(rdev) ((rdev->family == CHIP_BONAIRE))
-#define ASIC_IS_DCE83(rdev) ((rdev->family == CHIP_KABINI))
+#define ASIC_IS_DCE83(rdev) ((rdev->family == CHIP_KABINI) || \
+			     (rdev->family == CHIP_MULLINS))
 
 #define ASIC_IS_LOMBOK(rdev) ((rdev->ddev->pdev->device == 0x6849) || \
 			      (rdev->ddev->pdev->device == 0x6850) || \
diff --git a/drivers/gpu/drm/radeon/radeon_asic.c b/drivers/gpu/drm/radeon/radeon_asic.c
index be20e62dac8..e5f0177bea1 100644
--- a/drivers/gpu/drm/radeon/radeon_asic.c
+++ b/drivers/gpu/drm/radeon/radeon_asic.c
@@ -2049,8 +2049,8 @@ static struct radeon_asic ci_asic = {
 		.blit_ring_index = RADEON_RING_TYPE_GFX_INDEX,
 		.dma = &cik_copy_dma,
 		.dma_ring_index = R600_RING_TYPE_DMA_INDEX,
-		.copy = &cik_copy_dma,
-		.copy_ring_index = R600_RING_TYPE_DMA_INDEX,
+		.copy = &cik_copy_cpdma,
+		.copy_ring_index = RADEON_RING_TYPE_GFX_INDEX,
 	},
 	.surface = {
 		.set_reg = r600_set_surface_reg,
diff --git a/drivers/gpu/drm/radeon/radeon_bios.c b/drivers/gpu/drm/radeon/radeon_bios.c
index b3633d9a531..9ab30976287 100644
--- a/drivers/gpu/drm/radeon/radeon_bios.c
+++ b/drivers/gpu/drm/radeon/radeon_bios.c
@@ -196,6 +196,20 @@ static bool radeon_atrm_get_bios(struct radeon_device *rdev)
 		}
 	}
 
+	if (!found) {
+		while ((pdev = pci_get_class(PCI_CLASS_DISPLAY_OTHER << 8, pdev)) != NULL) {
+			dhandle = ACPI_HANDLE(&pdev->dev);
+			if (!dhandle)
+				continue;
+
+			status = acpi_get_handle(dhandle, "ATRM", &atrm_handle);
+			if (!ACPI_FAILURE(status)) {
+				found = true;
+				break;
+			}
+		}
+	}
+
 	if (!found)
 		return false;
 
diff --git a/drivers/gpu/drm/radeon/radeon_cs.c b/drivers/gpu/drm/radeon/radeon_cs.c
index 2b6e0ebcc13..41ecf8a6061 100644
--- a/drivers/gpu/drm/radeon/radeon_cs.c
+++ b/drivers/gpu/drm/radeon/radeon_cs.c
@@ -152,6 +152,12 @@ static int radeon_cs_parser_relocs(struct radeon_cs_parser *p)
 			uint32_t domain = r->write_domain ?
 				r->write_domain : r->read_domains;
 
+			if (domain & RADEON_GEM_DOMAIN_CPU) {
+				DRM_ERROR("RADEON_GEM_DOMAIN_CPU is not valid "
+					  "for command submission\n");
+				return -EINVAL;
+			}
+
 			p->relocs[i].domain = domain;
 			if (domain == RADEON_GEM_DOMAIN_VRAM)
 				domain |= RADEON_GEM_DOMAIN_GTT;
@@ -342,10 +348,17 @@ int radeon_cs_parser_init(struct radeon_cs_parser *p, void *data)
 			return -EINVAL;
 
 		/* we only support VM on some SI+ rings */
-		if ((p->rdev->asic->ring[p->ring]->cs_parse == NULL) &&
-		   ((p->cs_flags & RADEON_CS_USE_VM) == 0)) {
-			DRM_ERROR("Ring %d requires VM!\n", p->ring);
-			return -EINVAL;
+		if ((p->cs_flags & RADEON_CS_USE_VM) == 0) {
+			if (p->rdev->asic->ring[p->ring]->cs_parse == NULL) {
+				DRM_ERROR("Ring %d requires VM!\n", p->ring);
+				return -EINVAL;
+			}
+		} else {
+			if (p->rdev->asic->ring[p->ring]->ib_parse == NULL) {
+				DRM_ERROR("VM not supported on ring %d!\n",
+					  p->ring);
+				return -EINVAL;
+			}
 		}
 	}
 
diff --git a/drivers/gpu/drm/radeon/radeon_device.c b/drivers/gpu/drm/radeon/radeon_device.c
index 0e770bbf7e2..2cd144c378d 100644
--- a/drivers/gpu/drm/radeon/radeon_device.c
+++ b/drivers/gpu/drm/radeon/radeon_device.c
@@ -1533,11 +1533,6 @@ int radeon_resume_kms(struct drm_device *dev, bool resume, bool fbcon)
 
 	radeon_restore_bios_scratch_regs(rdev);
 
-	if (fbcon) {
-		radeon_fbdev_set_suspend(rdev, 0);
-		console_unlock();
-	}
-
 	/* init dig PHYs, disp eng pll */
 	if (rdev->is_atom_bios) {
 		radeon_atom_encoder_init(rdev);
@@ -1562,6 +1557,16 @@ int radeon_resume_kms(struct drm_device *dev, bool resume, bool fbcon)
 	}
 
 	drm_kms_helper_poll_enable(dev);
+
+	/* 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 (fbcon) {
+		radeon_fbdev_set_suspend(rdev, 0);
+		console_unlock();
+	}
+
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/radeon/radeon_display.c b/drivers/gpu/drm/radeon/radeon_display.c
index 408b6ac53f0..356b733caaf 100644
--- a/drivers/gpu/drm/radeon/radeon_display.c
+++ b/drivers/gpu/drm/radeon/radeon_display.c
@@ -862,7 +862,7 @@ static void avivo_get_fb_ref_div(unsigned nom, unsigned den, unsigned post_div,
 				 unsigned *fb_div, unsigned *ref_div)
 {
 	/* limit reference * post divider to a maximum */
-	ref_div_max = min(128 / post_div, ref_div_max);
+	ref_div_max = max(min(100 / post_div, ref_div_max), 1u);
 
 	/* get matching reference and feedback divider */
 	*ref_div = min(max(DIV_ROUND_CLOSEST(den, post_div), 1u), ref_div_max);
@@ -999,7 +999,7 @@ void radeon_compute_pll_avivo(struct radeon_pll *pll,
 
 	/* avoid high jitter with small fractional dividers */
 	if (pll->flags & RADEON_PLL_USE_FRAC_FB_DIV && (fb_div % 10)) {
-		fb_div_min = max(fb_div_min, (9 - (fb_div % 10)) * 20 + 60);
+		fb_div_min = max(fb_div_min, (9 - (fb_div % 10)) * 20 + 50);
 		if (fb_div < fb_div_min) {
 			unsigned tmp = DIV_ROUND_UP(fb_div_min, fb_div);
 			fb_div *= tmp;
diff --git a/drivers/gpu/drm/radeon/radeon_kms.c b/drivers/gpu/drm/radeon/radeon_kms.c
index 0cc47f12d99..eaaedba0467 100644
--- a/drivers/gpu/drm/radeon/radeon_kms.c
+++ b/drivers/gpu/drm/radeon/radeon_kms.c
@@ -577,28 +577,29 @@ int radeon_driver_open_kms(struct drm_device *dev, struct drm_file *file_priv)
 			return r;
 		}
 
-		r = radeon_bo_reserve(rdev->ring_tmp_bo.bo, false);
-		if (r) {
-			radeon_vm_fini(rdev, &fpriv->vm);
-			kfree(fpriv);
-			return r;
-		}
+		if (rdev->accel_working) {
+			r = radeon_bo_reserve(rdev->ring_tmp_bo.bo, false);
+			if (r) {
+				radeon_vm_fini(rdev, &fpriv->vm);
+				kfree(fpriv);
+				return r;
+			}
 
-		/* map the ib pool buffer read only into
-		 * virtual address space */
-		bo_va = radeon_vm_bo_add(rdev, &fpriv->vm,
-					 rdev->ring_tmp_bo.bo);
-		r = radeon_vm_bo_set_addr(rdev, bo_va, RADEON_VA_IB_OFFSET,
-					  RADEON_VM_PAGE_READABLE |
-					  RADEON_VM_PAGE_SNOOPED);
+			/* map the ib pool buffer read only into
+			 * virtual address space */
+			bo_va = radeon_vm_bo_add(rdev, &fpriv->vm,
+						 rdev->ring_tmp_bo.bo);
+			r = radeon_vm_bo_set_addr(rdev, bo_va, RADEON_VA_IB_OFFSET,
+						  RADEON_VM_PAGE_READABLE |
+						  RADEON_VM_PAGE_SNOOPED);
 
-		radeon_bo_unreserve(rdev->ring_tmp_bo.bo);
-		if (r) {
-			radeon_vm_fini(rdev, &fpriv->vm);
-			kfree(fpriv);
-			return r;
+			radeon_bo_unreserve(rdev->ring_tmp_bo.bo);
+			if (r) {
+				radeon_vm_fini(rdev, &fpriv->vm);
+				kfree(fpriv);
+				return r;
+			}
 		}
-
 		file_priv->driver_priv = fpriv;
 	}
 
@@ -626,13 +627,15 @@ void radeon_driver_postclose_kms(struct drm_device *dev,
 		struct radeon_bo_va *bo_va;
 		int r;
 
-		r = radeon_bo_reserve(rdev->ring_tmp_bo.bo, false);
-		if (!r) {
-			bo_va = radeon_vm_bo_find(&fpriv->vm,
-						  rdev->ring_tmp_bo.bo);
-			if (bo_va)
-				radeon_vm_bo_rmv(rdev, bo_va);
-			radeon_bo_unreserve(rdev->ring_tmp_bo.bo);
+		if (rdev->accel_working) {
+			r = radeon_bo_reserve(rdev->ring_tmp_bo.bo, false);
+			if (!r) {
+				bo_va = radeon_vm_bo_find(&fpriv->vm,
+							  rdev->ring_tmp_bo.bo);
+				if (bo_va)
+					radeon_vm_bo_rmv(rdev, bo_va);
+				radeon_bo_unreserve(rdev->ring_tmp_bo.bo);
+			}
 		}
 
 		radeon_vm_fini(rdev, &fpriv->vm);
diff --git a/drivers/gpu/drm/radeon/radeon_object.c b/drivers/gpu/drm/radeon/radeon_object.c
index 19bec0dbfa3..4faa4d6f9bb 100644
--- a/drivers/gpu/drm/radeon/radeon_object.c
+++ b/drivers/gpu/drm/radeon/radeon_object.c
@@ -458,7 +458,7 @@ int radeon_bo_list_validate(struct radeon_device *rdev,
 			 * into account. We don't want to disallow buffer moves
 			 * completely.
 			 */
-			if (current_domain != RADEON_GEM_DOMAIN_CPU &&
+			if ((lobj->alt_domain & current_domain) != 0 &&
 			    (domain & current_domain) == 0 && /* will be moved */
 			    bytes_moved > bytes_moved_threshold) {
 				/* don't move it */
@@ -699,22 +699,30 @@ int radeon_bo_fault_reserve_notify(struct ttm_buffer_object *bo)
 	rbo = container_of(bo, struct radeon_bo, tbo);
 	radeon_bo_check_tiling(rbo, 0, 0);
 	rdev = rbo->rdev;
-	if (bo->mem.mem_type == TTM_PL_VRAM) {
-		size = bo->mem.num_pages << PAGE_SHIFT;
-		offset = bo->mem.start << PAGE_SHIFT;
-		if ((offset + size) > rdev->mc.visible_vram_size) {
-			/* 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;
-			r = ttm_bo_validate(bo, &rbo->placement, false, false);
-			if (unlikely(r != 0))
-				return r;
-			offset = bo->mem.start << PAGE_SHIFT;
-			/* this should not happen */
-			if ((offset + size) > rdev->mc.visible_vram_size)
-				return -EINVAL;
-		}
+	if (bo->mem.mem_type != TTM_PL_VRAM)
+		return 0;
+
+	size = bo->mem.num_pages << PAGE_SHIFT;
+	offset = bo->mem.start << PAGE_SHIFT;
+	if ((offset + size) <= rdev->mc.visible_vram_size)
+		return 0;
+
+	/* 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;
+	r = ttm_bo_validate(bo, &rbo->placement, false, false);
+	if (unlikely(r == -ENOMEM)) {
+		radeon_ttm_placement_from_domain(rbo, RADEON_GEM_DOMAIN_GTT);
+		return ttm_bo_validate(bo, &rbo->placement, false, false);
+	} else if (unlikely(r != 0)) {
+		return r;
 	}
+
+	offset = bo->mem.start << PAGE_SHIFT;
+	/* this should never happen */
+	if ((offset + size) > rdev->mc.visible_vram_size)
+		return -EINVAL;
+
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/radeon/radeon_pm.c b/drivers/gpu/drm/radeon/radeon_pm.c
index f30b8426eee..2bdae61c0ac 100644
--- a/drivers/gpu/drm/radeon/radeon_pm.c
+++ b/drivers/gpu/drm/radeon/radeon_pm.c
@@ -361,6 +361,11 @@ static ssize_t radeon_set_pm_profile(struct device *dev,
 	struct drm_device *ddev = dev_get_drvdata(dev);
 	struct radeon_device *rdev = ddev->dev_private;
 
+	/* Can't set profile 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 (rdev->pm.pm_method == PM_METHOD_PROFILE) {
 		if (strncmp("default", buf, strlen("default")) == 0)
@@ -409,6 +414,13 @@ static ssize_t radeon_set_pm_method(struct device *dev,
 	struct drm_device *ddev = dev_get_drvdata(dev);
 	struct radeon_device *rdev = ddev->dev_private;
 
+	/* Can't set method when the card is off */
+	if  ((rdev->flags & RADEON_IS_PX) &&
+	     (ddev->switch_power_state != DRM_SWITCH_POWER_ON)) {
+		count = -EINVAL;
+		goto fail;
+	}
+
 	/* we don't support the legacy modes with dpm */
 	if (rdev->pm.pm_method == PM_METHOD_DPM) {
 		count = -EINVAL;
@@ -446,6 +458,10 @@ 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");
@@ -459,6 +475,11 @@ 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;
@@ -485,6 +506,10 @@ static ssize_t radeon_get_dpm_forced_performance_level(struct device *dev,
 	struct radeon_device *rdev = ddev->dev_private;
 	enum radeon_dpm_forced_level level = rdev->pm.dpm.forced_level;
 
+	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",
 			(level == RADEON_DPM_FORCED_LEVEL_AUTO) ? "auto" :
 			(level == RADEON_DPM_FORCED_LEVEL_LOW) ? "low" : "high");
@@ -500,6 +525,11 @@ static ssize_t radeon_set_dpm_forced_performance_level(struct device *dev,
 	enum radeon_dpm_forced_level level;
 	int ret = 0;
 
+	/* Can't force performance level 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("low", buf, strlen("low")) == 0) {
 		level = RADEON_DPM_FORCED_LEVEL_LOW;
@@ -538,8 +568,14 @@ static ssize_t radeon_hwmon_show_temp(struct device *dev,
 				      char *buf)
 {
 	struct radeon_device *rdev = dev_get_drvdata(dev);
+	struct drm_device *ddev = rdev->ddev;
 	int temp;
 
+	/* Can't get temperature when the card is off */
+	if  ((rdev->flags & RADEON_IS_PX) &&
+	     (ddev->switch_power_state != DRM_SWITCH_POWER_ON))
+		return -EINVAL;
+
 	if (rdev->asic->pm.get_temperature)
 		temp = radeon_get_temperature(rdev);
 	else
@@ -1068,7 +1104,6 @@ static void radeon_pm_resume_dpm(struct radeon_device *rdev)
 	if (ret)
 		goto dpm_resume_fail;
 	rdev->pm.dpm_enabled = true;
-	radeon_pm_compute_clocks(rdev);
 	return;
 
 dpm_resume_fail:
@@ -1614,8 +1649,12 @@ static int radeon_debugfs_pm_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 radeon_device *rdev = dev->dev_private;
+	struct drm_device *ddev = rdev->ddev;
 
-	if (rdev->pm.dpm_enabled) {
+	if  ((rdev->flags & RADEON_IS_PX) &&
+	     (ddev->switch_power_state != DRM_SWITCH_POWER_ON)) {
+		seq_printf(m, "PX asic powered off\n");
+	} else if (rdev->pm.dpm_enabled) {
 		mutex_lock(&rdev->pm.mutex);
 		if (rdev->asic->dpm.debugfs_print_current_performance_level)
 			radeon_dpm_debugfs_print_current_performance_level(rdev, m);
diff --git a/drivers/gpu/drm/radeon/radeon_vce.c b/drivers/gpu/drm/radeon/radeon_vce.c
index f73324c8149..3971d968af6 100644
--- a/drivers/gpu/drm/radeon/radeon_vce.c
+++ b/drivers/gpu/drm/radeon/radeon_vce.c
@@ -443,13 +443,16 @@ int radeon_vce_get_destroy_msg(struct radeon_device *rdev, int ring,
  * @p: parser context
  * @lo: address of lower dword
  * @hi: address of higher dword
+ * @size: size of checker for relocation buffer
  *
  * Patch relocation inside command stream with real buffer address
  */
-int radeon_vce_cs_reloc(struct radeon_cs_parser *p, int lo, int hi)
+int radeon_vce_cs_reloc(struct radeon_cs_parser *p, int lo, int hi,
+			unsigned size)
 {
 	struct radeon_cs_chunk *relocs_chunk;
-	uint64_t offset;
+	struct radeon_cs_reloc *reloc;
+	uint64_t start, end, offset;
 	unsigned idx;
 
 	relocs_chunk = &p->chunks[p->chunk_relocs_idx];
@@ -462,15 +465,60 @@ int radeon_vce_cs_reloc(struct radeon_cs_parser *p, int lo, int hi)
 		return -EINVAL;
 	}
 
-	offset += p->relocs_ptr[(idx / 4)]->gpu_offset;
+	reloc = p->relocs_ptr[(idx / 4)];
+	start = reloc->gpu_offset;
+	end = start + radeon_bo_size(reloc->robj);
+	start += offset;
 
-        p->ib.ptr[lo] = offset & 0xFFFFFFFF;
-        p->ib.ptr[hi] = offset >> 32;
+	p->ib.ptr[lo] = start & 0xFFFFFFFF;
+	p->ib.ptr[hi] = start >> 32;
+
+	if (end <= start) {
+		DRM_ERROR("invalid reloc offset %llX!\n", offset);
+		return -EINVAL;
+	}
+	if ((end - start) < size) {
+		DRM_ERROR("buffer to small (%d / %d)!\n",
+			(unsigned)(end - start), size);
+		return -EINVAL;
+	}
 
 	return 0;
 }
 
 /**
+ * radeon_vce_validate_handle - validate stream handle
+ *
+ * @p: parser context
+ * @handle: handle to validate
+ *
+ * Validates the handle and return the found session index or -EINVAL
+ * we we don't have another free session index.
+ */
+int radeon_vce_validate_handle(struct radeon_cs_parser *p, uint32_t handle)
+{
+	unsigned i;
+
+	/* validate the handle */
+	for (i = 0; i < RADEON_MAX_VCE_HANDLES; ++i) {
+		if (atomic_read(&p->rdev->vce.handles[i]) == handle)
+			return i;
+	}
+
+	/* handle not found try to alloc a new one */
+	for (i = 0; i < RADEON_MAX_VCE_HANDLES; ++i) {
+		if (!atomic_cmpxchg(&p->rdev->vce.handles[i], 0, handle)) {
+			p->rdev->vce.filp[i] = p->filp;
+			p->rdev->vce.img_size[i] = 0;
+			return i;
+		}
+	}
+
+	DRM_ERROR("No more free VCE handles!\n");
+	return -EINVAL;
+}
+
+/**
  * radeon_vce_cs_parse - parse and validate the command stream
  *
  * @p: parser context
@@ -478,8 +526,10 @@ int radeon_vce_cs_reloc(struct radeon_cs_parser *p, int lo, int hi)
  */
 int radeon_vce_cs_parse(struct radeon_cs_parser *p)
 {
-	uint32_t handle = 0;
-	bool destroy = false;
+	int session_idx = -1;
+	bool destroyed = false;
+	uint32_t tmp, handle = 0;
+	uint32_t *size = &tmp;
 	int i, r;
 
 	while (p->idx < p->chunks[p->chunk_ib_idx].length_dw) {
@@ -491,13 +541,29 @@ int radeon_vce_cs_parse(struct radeon_cs_parser *p)
                 	return -EINVAL;
 		}
 
+		if (destroyed) {
+			DRM_ERROR("No other command allowed after destroy!\n");
+			return -EINVAL;
+		}
+
 		switch (cmd) {
 		case 0x00000001: // session
 			handle = radeon_get_ib_value(p, p->idx + 2);
+			session_idx = radeon_vce_validate_handle(p, handle);
+			if (session_idx < 0)
+				return session_idx;
+			size = &p->rdev->vce.img_size[session_idx];
 			break;
 
 		case 0x00000002: // task info
+			break;
+
 		case 0x01000001: // create
+			*size = radeon_get_ib_value(p, p->idx + 8) *
+				radeon_get_ib_value(p, p->idx + 10) *
+				8 * 3 / 2;
+			break;
+
 		case 0x04000001: // config extension
 		case 0x04000002: // pic control
 		case 0x04000005: // rate control
@@ -506,23 +572,39 @@ int radeon_vce_cs_parse(struct radeon_cs_parser *p)
 			break;
 
 		case 0x03000001: // encode
-			r = radeon_vce_cs_reloc(p, p->idx + 10, p->idx + 9);
+			r = radeon_vce_cs_reloc(p, p->idx + 10, p->idx + 9,
+						*size);
 			if (r)
 				return r;
 
-			r = radeon_vce_cs_reloc(p, p->idx + 12, p->idx + 11);
+			r = radeon_vce_cs_reloc(p, p->idx + 12, p->idx + 11,
+						*size / 3);
 			if (r)
 				return r;
 			break;
 
 		case 0x02000001: // destroy
-			destroy = true;
+			destroyed = true;
 			break;
 
 		case 0x05000001: // context buffer
+			r = radeon_vce_cs_reloc(p, p->idx + 3, p->idx + 2,
+						*size * 2);
+			if (r)
+				return r;
+			break;
+
 		case 0x05000004: // video bitstream buffer
+			tmp = radeon_get_ib_value(p, p->idx + 4);
+			r = radeon_vce_cs_reloc(p, p->idx + 3, p->idx + 2,
+						tmp);
+			if (r)
+				return r;
+			break;
+
 		case 0x05000005: // feedback buffer
-			r = radeon_vce_cs_reloc(p, p->idx + 3, p->idx + 2);
+			r = radeon_vce_cs_reloc(p, p->idx + 3, p->idx + 2,
+						4096);
 			if (r)
 				return r;
 			break;
@@ -532,33 +614,21 @@ int radeon_vce_cs_parse(struct radeon_cs_parser *p)
 			return -EINVAL;
 		}
 
+		if (session_idx == -1) {
+			DRM_ERROR("no session command at start of IB\n");
+			return -EINVAL;
+		}
+
 		p->idx += len / 4;
 	}
 
-	if (destroy) {
+	if (destroyed) {
 		/* IB contains a destroy msg, free the handle */
 		for (i = 0; i < RADEON_MAX_VCE_HANDLES; ++i)
 			atomic_cmpxchg(&p->rdev->vce.handles[i], handle, 0);
-
-		return 0;
-        }
-
-	/* create or encode, validate the handle */
-	for (i = 0; i < RADEON_MAX_VCE_HANDLES; ++i) {
-		if (atomic_read(&p->rdev->vce.handles[i]) == handle)
-			return 0;
 	}
 
-	/* handle not found try to alloc a new one */
-	for (i = 0; i < RADEON_MAX_VCE_HANDLES; ++i) {
-		if (!atomic_cmpxchg(&p->rdev->vce.handles[i], 0, handle)) {
-			p->rdev->vce.filp[i] = p->filp;
-			return 0;
-		}
-	}
-
-	DRM_ERROR("No more free VCE handles!\n");
-	return -EINVAL;
+	return 0;
 }
 
 /**
diff --git a/drivers/gpu/drm/radeon/radeon_vm.c b/drivers/gpu/drm/radeon/radeon_vm.c
index 2aae6ce49d3..c11b71d249e 100644
--- a/drivers/gpu/drm/radeon/radeon_vm.c
+++ b/drivers/gpu/drm/radeon/radeon_vm.c
@@ -130,10 +130,10 @@ struct radeon_cs_reloc *radeon_vm_get_bos(struct radeon_device *rdev,
 					  struct list_head *head)
 {
 	struct radeon_cs_reloc *list;
-	unsigned i, idx, size;
+	unsigned i, idx;
 
-	size = (radeon_vm_num_pdes(rdev) + 1) * sizeof(struct radeon_cs_reloc);
-	list = kmalloc(size, GFP_KERNEL);
+	list = kmalloc_array(vm->max_pde_used + 2,
+			     sizeof(struct radeon_cs_reloc), GFP_KERNEL);
 	if (!list)
 		return NULL;
 
@@ -585,7 +585,8 @@ int radeon_vm_update_page_directory(struct radeon_device *rdev,
 {
 	static const uint32_t incr = RADEON_VM_PTE_COUNT * 8;
 
-	uint64_t pd_addr = radeon_bo_gpu_offset(vm->page_directory);
+	struct radeon_bo *pd = vm->page_directory;
+	uint64_t pd_addr = radeon_bo_gpu_offset(pd);
 	uint64_t last_pde = ~0, last_pt = ~0;
 	unsigned count = 0, pt_idx, ndw;
 	struct radeon_ib ib;
@@ -595,7 +596,7 @@ int radeon_vm_update_page_directory(struct radeon_device *rdev,
 	ndw = 64;
 
 	/* assume the worst case */
-	ndw += vm->max_pde_used * 12;
+	ndw += vm->max_pde_used * 16;
 
 	/* update too big for an IB */
 	if (ndw > 0xfffff)
@@ -642,6 +643,7 @@ int radeon_vm_update_page_directory(struct radeon_device *rdev,
 					incr, R600_PTE_VALID);
 
 	if (ib.length_dw != 0) {
+		radeon_semaphore_sync_to(ib.semaphore, pd->tbo.sync_obj);
 		radeon_semaphore_sync_to(ib.semaphore, vm->last_id_use);
 		r = radeon_ib_schedule(rdev, &ib, NULL);
 		if (r) {
@@ -689,15 +691,18 @@ static void radeon_vm_update_ptes(struct radeon_device *rdev,
 	/* walk over the address space and update the page tables */
 	for (addr = start; addr < end; ) {
 		uint64_t pt_idx = addr >> RADEON_VM_BLOCK_SIZE;
+		struct radeon_bo *pt = vm->page_tables[pt_idx].bo;
 		unsigned nptes;
 		uint64_t pte;
 
+		radeon_semaphore_sync_to(ib->semaphore, pt->tbo.sync_obj);
+
 		if ((addr & ~mask) == (end & ~mask))
 			nptes = end - addr;
 		else
 			nptes = RADEON_VM_PTE_COUNT - (addr & mask);
 
-		pte = radeon_bo_gpu_offset(vm->page_tables[pt_idx].bo);
+		pte = radeon_bo_gpu_offset(pt);
 		pte += (addr & mask) * 8;
 
 		if ((last_pte + 8 * count) != pte) {
diff --git a/drivers/gpu/drm/radeon/sid.h b/drivers/gpu/drm/radeon/sid.h
index 683532f8493..7321283602c 100644
--- a/drivers/gpu/drm/radeon/sid.h
+++ b/drivers/gpu/drm/radeon/sid.h
@@ -107,8 +107,8 @@
 #define		SPLL_CHG_STATUS				(1 << 1)
 #define	SPLL_CNTL_MODE					0x618
 #define		SPLL_SW_DIR_CONTROL			(1 << 0)
-#	define SPLL_REFCLK_SEL(x)			((x) << 8)
-#	define SPLL_REFCLK_SEL_MASK			0xFF00
+#	define SPLL_REFCLK_SEL(x)			((x) << 26)
+#	define SPLL_REFCLK_SEL_MASK			(3 << 26)
 
 #define	CG_SPLL_SPREAD_SPECTRUM				0x620
 #define		SSEN					(1 << 0)
diff --git a/drivers/gpu/drm/udl/udl_main.c b/drivers/gpu/drm/udl/udl_main.c
index f5ae57406f3..afdf383f630 100644
--- a/drivers/gpu/drm/udl/udl_main.c
+++ b/drivers/gpu/drm/udl/udl_main.c
@@ -283,7 +283,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 udl_device *udl;
-	int ret;
+	int ret = -ENOMEM;
 
 	DRM_DEBUG("\n");
 	udl = kzalloc(sizeof(struct udl_device), GFP_KERNEL);
@@ -299,7 +299,6 @@ int udl_driver_load(struct drm_device *dev, unsigned long flags)
 	}
 
 	if (!udl_alloc_urb_list(dev, WRITES_IN_FLIGHT, MAX_TRANSFER)) {
-		ret = -ENOMEM;
 		DRM_ERROR("udl_alloc_urb_list failed\n");
 		goto err;
 	}
diff --git a/drivers/hid/Kconfig b/drivers/hid/Kconfig
index 7af9d0b5dea..800c8b60f7a 100644
--- a/drivers/hid/Kconfig
+++ b/drivers/hid/Kconfig
@@ -608,7 +608,10 @@ config HID_SAITEK
 	Support for Saitek devices that are not fully compliant with the
 	HID standard.
 
-	Currently only supports the PS1000 controller.
+	Supported devices:
+	- PS1000 Dual Analog Pad
+	- R.A.T.7 Gaming Mouse
+	- M.M.O.7 Gaming Mouse
 
 config HID_SAMSUNG
 	tristate "Samsung InfraRed remote control or keyboards"
@@ -657,6 +660,14 @@ config HID_SUNPLUS
 	---help---
 	Support for Sunplus wireless desktop.
 
+config HID_RMI
+	tristate "Synaptics RMI4 device support"
+	depends on HID
+	---help---
+	Support for Synaptics RMI4 touchpads.
+	Say Y here if you have a Synaptics RMI4 touchpads over i2c-hid or usbhid
+	and want support for its special functionalities.
+
 config HID_GREENASIA
 	tristate "GreenAsia (Product ID 0x12) game controller support"
 	depends on HID
diff --git a/drivers/hid/Makefile b/drivers/hid/Makefile
index fc712dde02a..a6fa6baf368 100644
--- a/drivers/hid/Makefile
+++ b/drivers/hid/Makefile
@@ -97,6 +97,7 @@ obj-$(CONFIG_HID_ROCCAT)	+= hid-roccat.o hid-roccat-common.o \
 	hid-roccat-arvo.o hid-roccat-isku.o hid-roccat-kone.o \
 	hid-roccat-koneplus.o hid-roccat-konepure.o hid-roccat-kovaplus.o \
 	hid-roccat-lua.o hid-roccat-pyra.o hid-roccat-ryos.o hid-roccat-savu.o
+obj-$(CONFIG_HID_RMI)		+= hid-rmi.o
 obj-$(CONFIG_HID_SAITEK)	+= hid-saitek.o
 obj-$(CONFIG_HID_SAMSUNG)	+= hid-samsung.o
 obj-$(CONFIG_HID_SMARTJOYPLUS)	+= hid-sjoy.o
diff --git a/drivers/hid/hid-core.c b/drivers/hid/hid-core.c
index da52279de93..8ed66fd1ea8 100644
--- a/drivers/hid/hid-core.c
+++ b/drivers/hid/hid-core.c
@@ -779,6 +779,14 @@ static int hid_scan_report(struct hid_device *hid)
 	    (hid->group == HID_GROUP_MULTITOUCH))
 		hid->group = HID_GROUP_MULTITOUCH_WIN_8;
 
+	/*
+	* Vendor specific handlings
+	*/
+	if ((hid->vendor == USB_VENDOR_ID_SYNAPTICS) &&
+	    (hid->group == HID_GROUP_GENERIC))
+		/* hid-rmi should take care of them, not hid-generic */
+		hid->group = HID_GROUP_RMI;
+
 	vfree(parser);
 	return 0;
 }
@@ -842,7 +850,17 @@ struct hid_report *hid_validate_values(struct hid_device *hid,
 	 * ->numbered being checked, which may not always be the case when
 	 * drivers go to access report values.
 	 */
-	report = hid->report_enum[type].report_id_hash[id];
+	if (id == 0) {
+		/*
+		 * Validating on id 0 means we should examine the first
+		 * report in the list.
+		 */
+		report = list_entry(
+				hid->report_enum[type].report_list.next,
+				struct hid_report, list);
+	} else {
+		report = hid->report_enum[type].report_id_hash[id];
+	}
 	if (!report) {
 		hid_err(hid, "missing %s %u\n", hid_report_names[type], id);
 		return NULL;
@@ -1868,7 +1886,11 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_RYOS_MK_PRO) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_SAVU) },
 #endif
+#if IS_ENABLED(CONFIG_HID_SAITEK)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SAITEK, USB_DEVICE_ID_SAITEK_PS1000) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_SAITEK, USB_DEVICE_ID_SAITEK_RAT7) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_SAITEK, USB_DEVICE_ID_SAITEK_MMO7) },
+#endif
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SAMSUNG, USB_DEVICE_ID_SAMSUNG_IR_REMOTE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SAMSUNG, USB_DEVICE_ID_SAMSUNG_WIRELESS_KBD_MOUSE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SKYCABLE, USB_DEVICE_ID_SKYCABLE_WIRELESS_PRESENTER) },
diff --git a/drivers/hid/hid-debug.c b/drivers/hid/hid-debug.c
index 53b771d5683..84c3cb15ccd 100644
--- a/drivers/hid/hid-debug.c
+++ b/drivers/hid/hid-debug.c
@@ -165,6 +165,8 @@ static const struct hid_usage_entry hid_usage_table[] = {
     {0, 0x53, "DeviceIndex"},
     {0, 0x54, "ContactCount"},
     {0, 0x55, "ContactMaximumNumber"},
+    {0, 0x5A, "SecondaryBarrelSwitch"},
+    {0, 0x5B, "TransducerSerialNumber"},
   { 15, 0, "PhysicalInterfaceDevice" },
     {0, 0x00, "Undefined"},
     {0, 0x01, "Physical_Interface_Device"},
@@ -272,6 +274,85 @@ static const struct hid_usage_entry hid_usage_table[] = {
     {0, 0xAA, "Shared_Parameter_Blocks"},
     {0, 0xAB, "Create_New_Effect_Report"},
     {0, 0xAC, "RAM_Pool_Available"},
+  {  0x20, 0, "Sensor" },
+    { 0x20, 0x01, "Sensor" },
+    { 0x20, 0x10, "Biometric" },
+      { 0x20, 0x11, "BiometricHumanPresence" },
+      { 0x20, 0x12, "BiometricHumanProximity" },
+      { 0x20, 0x13, "BiometricHumanTouch" },
+    { 0x20, 0x20, "Electrical" },
+      { 0x20, 0x21, "ElectricalCapacitance" },
+      { 0x20, 0x22, "ElectricalCurrent" },
+      { 0x20, 0x23, "ElectricalPower" },
+      { 0x20, 0x24, "ElectricalInductance" },
+      { 0x20, 0x25, "ElectricalResistance" },
+      { 0x20, 0x26, "ElectricalVoltage" },
+      { 0x20, 0x27, "ElectricalPoteniometer" },
+      { 0x20, 0x28, "ElectricalFrequency" },
+      { 0x20, 0x29, "ElectricalPeriod" },
+    { 0x20, 0x30, "Environmental" },
+      { 0x20, 0x31, "EnvironmentalAtmosphericPressure" },
+      { 0x20, 0x32, "EnvironmentalHumidity" },
+      { 0x20, 0x33, "EnvironmentalTemperature" },
+      { 0x20, 0x34, "EnvironmentalWindDirection" },
+      { 0x20, 0x35, "EnvironmentalWindSpeed" },
+    { 0x20, 0x40, "Light" },
+      { 0x20, 0x41, "LightAmbientLight" },
+      { 0x20, 0x42, "LightConsumerInfrared" },
+    { 0x20, 0x50, "Location" },
+      { 0x20, 0x51, "LocationBroadcast" },
+      { 0x20, 0x52, "LocationDeadReckoning" },
+      { 0x20, 0x53, "LocationGPS" },
+      { 0x20, 0x54, "LocationLookup" },
+      { 0x20, 0x55, "LocationOther" },
+      { 0x20, 0x56, "LocationStatic" },
+      { 0x20, 0x57, "LocationTriangulation" },
+    { 0x20, 0x60, "Mechanical" },
+      { 0x20, 0x61, "MechanicalBooleanSwitch" },
+      { 0x20, 0x62, "MechanicalBooleanSwitchArray" },
+      { 0x20, 0x63, "MechanicalMultivalueSwitch" },
+      { 0x20, 0x64, "MechanicalForce" },
+      { 0x20, 0x65, "MechanicalPressure" },
+      { 0x20, 0x66, "MechanicalStrain" },
+      { 0x20, 0x67, "MechanicalWeight" },
+      { 0x20, 0x68, "MechanicalHapticVibrator" },
+      { 0x20, 0x69, "MechanicalHallEffectSwitch" },
+    { 0x20, 0x70, "Motion" },
+      { 0x20, 0x71, "MotionAccelerometer1D" },
+      { 0x20, 0x72, "MotionAccelerometer2D" },
+      { 0x20, 0x73, "MotionAccelerometer3D" },
+      { 0x20, 0x74, "MotionGyrometer1D" },
+      { 0x20, 0x75, "MotionGyrometer2D" },
+      { 0x20, 0x76, "MotionGyrometer3D" },
+      { 0x20, 0x77, "MotionMotionDetector" },
+      { 0x20, 0x78, "MotionSpeedometer" },
+      { 0x20, 0x79, "MotionAccelerometer" },
+      { 0x20, 0x7A, "MotionGyrometer" },
+    { 0x20, 0x80, "Orientation" },
+      { 0x20, 0x81, "OrientationCompass1D" },
+      { 0x20, 0x82, "OrientationCompass2D" },
+      { 0x20, 0x83, "OrientationCompass3D" },
+      { 0x20, 0x84, "OrientationInclinometer1D" },
+      { 0x20, 0x85, "OrientationInclinometer2D" },
+      { 0x20, 0x86, "OrientationInclinometer3D" },
+      { 0x20, 0x87, "OrientationDistance1D" },
+      { 0x20, 0x88, "OrientationDistance2D" },
+      { 0x20, 0x89, "OrientationDistance3D" },
+      { 0x20, 0x8A, "OrientationDeviceOrientation" },
+      { 0x20, 0x8B, "OrientationCompass" },
+      { 0x20, 0x8C, "OrientationInclinometer" },
+      { 0x20, 0x8D, "OrientationDistance" },
+    { 0x20, 0x90, "Scanner" },
+      { 0x20, 0x91, "ScannerBarcode" },
+      { 0x20, 0x91, "ScannerRFID" },
+      { 0x20, 0x91, "ScannerNFC" },
+    { 0x20, 0xA0, "Time" },
+      { 0x20, 0xA1, "TimeAlarmTimer" },
+      { 0x20, 0xA2, "TimeRealTimeClock" },
+    { 0x20, 0xE0, "Other" },
+      { 0x20, 0xE1, "OtherCustom" },
+      { 0x20, 0xE2, "OtherGeneric" },
+      { 0x20, 0xE3, "OtherGenericEnumerator" },
   { 0x84, 0, "Power Device" },
     { 0x84, 0x02, "PresentStatus" },
     { 0x84, 0x03, "ChangeStatus" },
@@ -855,6 +936,16 @@ static const char *keys[KEY_MAX + 1] = {
 	[KEY_KBDILLUMDOWN] = "KbdIlluminationDown",
 	[KEY_KBDILLUMUP] = "KbdIlluminationUp",
 	[KEY_SWITCHVIDEOMODE] = "SwitchVideoMode",
+	[KEY_BUTTONCONFIG] = "ButtonConfig",
+	[KEY_TASKMANAGER] = "TaskManager",
+	[KEY_JOURNAL] = "Journal",
+	[KEY_CONTROLPANEL] = "ControlPanel",
+	[KEY_APPSELECT] = "AppSelect",
+	[KEY_SCREENSAVER] = "ScreenSaver",
+	[KEY_VOICECOMMAND] = "VoiceCommand",
+	[KEY_BRIGHTNESS_MIN] = "BrightnessMin",
+	[KEY_BRIGHTNESS_MAX] = "BrightnessMax",
+	[KEY_BRIGHTNESS_AUTO] = "BrightnessAuto",
 };
 
 static const char *relatives[REL_MAX + 1] = {
diff --git a/drivers/hid/hid-ids.h b/drivers/hid/hid-ids.h
index 34bb2205d2e..6d00bb9366f 100644
--- a/drivers/hid/hid-ids.h
+++ b/drivers/hid/hid-ids.h
@@ -463,6 +463,7 @@
 
 #define USB_VENDOR_ID_STM_0             0x0483
 #define USB_DEVICE_ID_STM_HID_SENSOR    0x91d1
+#define USB_DEVICE_ID_STM_HID_SENSOR_1  0x9100
 
 #define USB_VENDOR_ID_ION		0x15e4
 #define USB_DEVICE_ID_ICADE		0x0132
@@ -633,6 +634,9 @@
 #define USB_DEVICE_ID_MS_PRESENTER_8K_USB	0x0713
 #define USB_DEVICE_ID_MS_DIGITAL_MEDIA_3K	0x0730
 #define USB_DEVICE_ID_MS_COMFORT_MOUSE_4500	0x076c
+#define USB_DEVICE_ID_MS_SURFACE_PRO_2   0x0799
+#define USB_DEVICE_ID_MS_TOUCH_COVER_2   0x07a7
+#define USB_DEVICE_ID_MS_TYPE_COVER_2    0x07a9
 
 #define USB_VENDOR_ID_MOJO		0x8282
 #define USB_DEVICE_ID_RETRO_ADAPTER	0x3201
@@ -764,11 +768,16 @@
 #define USB_VENDOR_ID_SAITEK		0x06a3
 #define USB_DEVICE_ID_SAITEK_RUMBLEPAD	0xff17
 #define USB_DEVICE_ID_SAITEK_PS1000	0x0621
+#define USB_DEVICE_ID_SAITEK_RAT7	0x0cd7
+#define USB_DEVICE_ID_SAITEK_MMO7	0x0cd0
 
 #define USB_VENDOR_ID_SAMSUNG		0x0419
 #define USB_DEVICE_ID_SAMSUNG_IR_REMOTE	0x0001
 #define USB_DEVICE_ID_SAMSUNG_WIRELESS_KBD_MOUSE	0x0600
 
+#define USB_VENDOR_ID_SEMICO			0x1a2c
+#define USB_DEVICE_ID_SEMICO_USB_KEYKOARD	0x0023
+
 #define USB_VENDOR_ID_SENNHEISER	0x1395
 #define USB_DEVICE_ID_SENNHEISER_BTD500USB	0x002c
 
diff --git a/drivers/hid/hid-input.c b/drivers/hid/hid-input.c
index e7e8b19a928..2619f7f4517 100644
--- a/drivers/hid/hid-input.c
+++ b/drivers/hid/hid-input.c
@@ -684,9 +684,14 @@ static void hidinput_configure_usage(struct hid_input *hidinput, struct hid_fiel
 			break;
 
 		case 0x46: /* TabletPick */
+		case 0x5a: /* SecondaryBarrelSwitch */
 			map_key_clear(BTN_STYLUS2);
 			break;
 
+		case 0x5b: /* TransducerSerialNumber */
+			set_bit(MSC_SERIAL, input->mscbit);
+			break;
+
 		default:  goto unknown;
 		}
 		break;
@@ -721,6 +726,13 @@ static void hidinput_configure_usage(struct hid_input *hidinput, struct hid_fiel
 		case 0x06c: map_key_clear(KEY_YELLOW);		break;
 		case 0x06d: map_key_clear(KEY_ZOOM);		break;
 
+		case 0x06f: map_key_clear(KEY_BRIGHTNESSUP);		break;
+		case 0x070: map_key_clear(KEY_BRIGHTNESSDOWN);		break;
+		case 0x072: map_key_clear(KEY_BRIGHTNESS_TOGGLE);	break;
+		case 0x073: map_key_clear(KEY_BRIGHTNESS_MIN);		break;
+		case 0x074: map_key_clear(KEY_BRIGHTNESS_MAX);		break;
+		case 0x075: map_key_clear(KEY_BRIGHTNESS_AUTO);		break;
+
 		case 0x082: map_key_clear(KEY_VIDEO_NEXT);	break;
 		case 0x083: map_key_clear(KEY_LAST);		break;
 		case 0x084: map_key_clear(KEY_ENTER);		break;
@@ -761,6 +773,7 @@ static void hidinput_configure_usage(struct hid_input *hidinput, struct hid_fiel
 		case 0x0bf: map_key_clear(KEY_SLOW);		break;
 
 		case 0x0cd: map_key_clear(KEY_PLAYPAUSE);	break;
+		case 0x0cf: map_key_clear(KEY_VOICECOMMAND);	break;
 		case 0x0e0: map_abs_clear(ABS_VOLUME);		break;
 		case 0x0e2: map_key_clear(KEY_MUTE);		break;
 		case 0x0e5: map_key_clear(KEY_BASSBOOST);	break;
@@ -768,6 +781,7 @@ static void hidinput_configure_usage(struct hid_input *hidinput, struct hid_fiel
 		case 0x0ea: map_key_clear(KEY_VOLUMEDOWN);	break;
 		case 0x0f5: map_key_clear(KEY_SLOW);		break;
 
+		case 0x181: map_key_clear(KEY_BUTTONCONFIG);	break;
 		case 0x182: map_key_clear(KEY_BOOKMARKS);	break;
 		case 0x183: map_key_clear(KEY_CONFIG);		break;
 		case 0x184: map_key_clear(KEY_WORDPROCESSOR);	break;
@@ -781,6 +795,8 @@ static void hidinput_configure_usage(struct hid_input *hidinput, struct hid_fiel
 		case 0x18c: map_key_clear(KEY_VOICEMAIL);	break;
 		case 0x18d: map_key_clear(KEY_ADDRESSBOOK);	break;
 		case 0x18e: map_key_clear(KEY_CALENDAR);	break;
+		case 0x18f: map_key_clear(KEY_TASKMANAGER);	break;
+		case 0x190: map_key_clear(KEY_JOURNAL);		break;
 		case 0x191: map_key_clear(KEY_FINANCE);		break;
 		case 0x192: map_key_clear(KEY_CALC);		break;
 		case 0x193: map_key_clear(KEY_PLAYER);		break;
@@ -789,12 +805,15 @@ static void hidinput_configure_usage(struct hid_input *hidinput, struct hid_fiel
 		case 0x199: map_key_clear(KEY_CHAT);		break;
 		case 0x19c: map_key_clear(KEY_LOGOFF);		break;
 		case 0x19e: map_key_clear(KEY_COFFEE);		break;
+		case 0x19f: map_key_clear(KEY_CONTROLPANEL);		break;
+		case 0x1a2: map_key_clear(KEY_APPSELECT);		break;
 		case 0x1a3: map_key_clear(KEY_NEXT);		break;
 		case 0x1a4: map_key_clear(KEY_PREVIOUS);	break;
 		case 0x1a6: map_key_clear(KEY_HELP);		break;
 		case 0x1a7: map_key_clear(KEY_DOCUMENTS);	break;
 		case 0x1ab: map_key_clear(KEY_SPELLCHECK);	break;
 		case 0x1ae: map_key_clear(KEY_KEYBOARD);	break;
+		case 0x1b1: map_key_clear(KEY_SCREENSAVER);		break;
 		case 0x1b4: map_key_clear(KEY_FILE);		break;
 		case 0x1b6: map_key_clear(KEY_IMAGES);		break;
 		case 0x1b7: map_key_clear(KEY_AUDIO);		break;
diff --git a/drivers/hid/hid-rmi.c b/drivers/hid/hid-rmi.c
new file mode 100644
index 00000000000..2451c7e5feb
--- /dev/null
+++ b/drivers/hid/hid-rmi.c
@@ -0,0 +1,920 @@
+/*
+ *  Copyright (c) 2013 Andrew Duggan <aduggan@synaptics.com>
+ *  Copyright (c) 2013 Synaptics Incorporated
+ *  Copyright (c) 2014 Benjamin Tissoires <benjamin.tissoires@gmail.com>
+ *  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.
+ */
+
+#include <linux/kernel.h>
+#include <linux/hid.h>
+#include <linux/input.h>
+#include <linux/input/mt.h>
+#include <linux/module.h>
+#include <linux/pm.h>
+#include <linux/slab.h>
+#include <linux/wait.h>
+#include <linux/sched.h>
+#include "hid-ids.h"
+
+#define RMI_MOUSE_REPORT_ID		0x01 /* Mouse emulation Report */
+#define RMI_WRITE_REPORT_ID		0x09 /* Output Report */
+#define RMI_READ_ADDR_REPORT_ID		0x0a /* Output Report */
+#define RMI_READ_DATA_REPORT_ID		0x0b /* Input Report */
+#define RMI_ATTN_REPORT_ID		0x0c /* Input Report */
+#define RMI_SET_RMI_MODE_REPORT_ID	0x0f /* Feature Report */
+
+/* flags */
+#define RMI_READ_REQUEST_PENDING	BIT(0)
+#define RMI_READ_DATA_PENDING		BIT(1)
+#define RMI_STARTED			BIT(2)
+
+enum rmi_mode_type {
+	RMI_MODE_OFF			= 0,
+	RMI_MODE_ATTN_REPORTS		= 1,
+	RMI_MODE_NO_PACKED_ATTN_REPORTS	= 2,
+};
+
+struct rmi_function {
+	unsigned page;			/* page of the function */
+	u16 query_base_addr;		/* base address for queries */
+	u16 command_base_addr;		/* base address for commands */
+	u16 control_base_addr;		/* base address for controls */
+	u16 data_base_addr;		/* base address for datas */
+	unsigned int interrupt_base;	/* cross-function interrupt number
+					 * (uniq in the device)*/
+	unsigned int interrupt_count;	/* number of interrupts */
+	unsigned int report_size;	/* size of a report */
+	unsigned long irq_mask;		/* mask of the interrupts
+					 * (to be applied against ATTN IRQ) */
+};
+
+/**
+ * struct rmi_data - stores information for hid communication
+ *
+ * @page_mutex: Locks current page to avoid changing pages in unexpected ways.
+ * @page: Keeps track of the current virtual page
+ *
+ * @wait: Used for waiting for read data
+ *
+ * @writeReport: output buffer when writing RMI registers
+ * @readReport: input buffer when reading RMI registers
+ *
+ * @input_report_size: size of an input report (advertised by HID)
+ * @output_report_size: size of an output report (advertised by HID)
+ *
+ * @flags: flags for the current device (started, reading, etc...)
+ *
+ * @f11: placeholder of internal RMI function F11 description
+ * @f30: placeholder of internal RMI function F30 description
+ *
+ * @max_fingers: maximum finger count reported by the device
+ * @max_x: maximum x value reported by the device
+ * @max_y: maximum y value reported by the device
+ *
+ * @gpio_led_count: count of GPIOs + LEDs reported by F30
+ * @button_count: actual physical buttons count
+ * @button_mask: button mask used to decode GPIO ATTN reports
+ * @button_state_mask: pull state of the buttons
+ *
+ * @input: pointer to the kernel input device
+ *
+ * @reset_work: worker which will be called in case of a mouse report
+ * @hdev: pointer to the struct hid_device
+ */
+struct rmi_data {
+	struct mutex page_mutex;
+	int page;
+
+	wait_queue_head_t wait;
+
+	u8 *writeReport;
+	u8 *readReport;
+
+	int input_report_size;
+	int output_report_size;
+
+	unsigned long flags;
+
+	struct rmi_function f11;
+	struct rmi_function f30;
+
+	unsigned int max_fingers;
+	unsigned int max_x;
+	unsigned int max_y;
+	unsigned int x_size_mm;
+	unsigned int y_size_mm;
+
+	unsigned int gpio_led_count;
+	unsigned int button_count;
+	unsigned long button_mask;
+	unsigned long button_state_mask;
+
+	struct input_dev *input;
+
+	struct work_struct reset_work;
+	struct hid_device *hdev;
+};
+
+#define RMI_PAGE(addr) (((addr) >> 8) & 0xff)
+
+static int rmi_write_report(struct hid_device *hdev, u8 *report, int len);
+
+/**
+ * rmi_set_page - Set RMI page
+ * @hdev: The pointer to the hid_device struct
+ * @page: The new page address.
+ *
+ * RMI devices have 16-bit addressing, but some of the physical
+ * implementations (like SMBus) only have 8-bit addressing. So RMI implements
+ * a page address at 0xff of every page so we can reliable page addresses
+ * every 256 registers.
+ *
+ * The page_mutex lock must be held when this function is entered.
+ *
+ * Returns zero on success, non-zero on failure.
+ */
+static int rmi_set_page(struct hid_device *hdev, u8 page)
+{
+	struct rmi_data *data = hid_get_drvdata(hdev);
+	int retval;
+
+	data->writeReport[0] = RMI_WRITE_REPORT_ID;
+	data->writeReport[1] = 1;
+	data->writeReport[2] = 0xFF;
+	data->writeReport[4] = page;
+
+	retval = rmi_write_report(hdev, data->writeReport,
+			data->output_report_size);
+	if (retval != data->output_report_size) {
+		dev_err(&hdev->dev,
+			"%s: set page failed: %d.", __func__, retval);
+		return retval;
+	}
+
+	data->page = page;
+	return 0;
+}
+
+static int rmi_set_mode(struct hid_device *hdev, u8 mode)
+{
+	int ret;
+	u8 txbuf[2] = {RMI_SET_RMI_MODE_REPORT_ID, mode};
+
+	ret = hid_hw_raw_request(hdev, RMI_SET_RMI_MODE_REPORT_ID, txbuf,
+			sizeof(txbuf), HID_FEATURE_REPORT, HID_REQ_SET_REPORT);
+	if (ret < 0) {
+		dev_err(&hdev->dev, "unable to set rmi mode to %d (%d)\n", mode,
+			ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int rmi_write_report(struct hid_device *hdev, u8 *report, int len)
+{
+	int ret;
+
+	ret = hid_hw_output_report(hdev, (void *)report, len);
+	if (ret < 0) {
+		dev_err(&hdev->dev, "failed to write hid report (%d)\n", ret);
+		return ret;
+	}
+
+	return ret;
+}
+
+static int rmi_read_block(struct hid_device *hdev, u16 addr, void *buf,
+		const int len)
+{
+	struct rmi_data *data = hid_get_drvdata(hdev);
+	int ret;
+	int bytes_read;
+	int bytes_needed;
+	int retries;
+	int read_input_count;
+
+	mutex_lock(&data->page_mutex);
+
+	if (RMI_PAGE(addr) != data->page) {
+		ret = rmi_set_page(hdev, RMI_PAGE(addr));
+		if (ret < 0)
+			goto exit;
+	}
+
+	for (retries = 5; retries > 0; retries--) {
+		data->writeReport[0] = RMI_READ_ADDR_REPORT_ID;
+		data->writeReport[1] = 0; /* old 1 byte read count */
+		data->writeReport[2] = addr & 0xFF;
+		data->writeReport[3] = (addr >> 8) & 0xFF;
+		data->writeReport[4] = len  & 0xFF;
+		data->writeReport[5] = (len >> 8) & 0xFF;
+
+		set_bit(RMI_READ_REQUEST_PENDING, &data->flags);
+
+		ret = rmi_write_report(hdev, data->writeReport,
+						data->output_report_size);
+		if (ret != data->output_report_size) {
+			clear_bit(RMI_READ_REQUEST_PENDING, &data->flags);
+			dev_err(&hdev->dev,
+				"failed to write request output report (%d)\n",
+				ret);
+			goto exit;
+		}
+
+		bytes_read = 0;
+		bytes_needed = len;
+		while (bytes_read < len) {
+			if (!wait_event_timeout(data->wait,
+				test_bit(RMI_READ_DATA_PENDING, &data->flags),
+					msecs_to_jiffies(1000))) {
+				hid_warn(hdev, "%s: timeout elapsed\n",
+					 __func__);
+				ret = -EAGAIN;
+				break;
+			}
+
+			read_input_count = data->readReport[1];
+			memcpy(buf + bytes_read, &data->readReport[2],
+				read_input_count < bytes_needed ?
+					read_input_count : bytes_needed);
+
+			bytes_read += read_input_count;
+			bytes_needed -= read_input_count;
+			clear_bit(RMI_READ_DATA_PENDING, &data->flags);
+		}
+
+		if (ret >= 0) {
+			ret = 0;
+			break;
+		}
+	}
+
+exit:
+	clear_bit(RMI_READ_REQUEST_PENDING, &data->flags);
+	mutex_unlock(&data->page_mutex);
+	return ret;
+}
+
+static inline int rmi_read(struct hid_device *hdev, u16 addr, void *buf)
+{
+	return rmi_read_block(hdev, addr, buf, 1);
+}
+
+static void rmi_f11_process_touch(struct rmi_data *hdata, int slot,
+		u8 finger_state, u8 *touch_data)
+{
+	int x, y, wx, wy;
+	int wide, major, minor;
+	int z;
+
+	input_mt_slot(hdata->input, slot);
+	input_mt_report_slot_state(hdata->input, MT_TOOL_FINGER,
+			finger_state == 0x01);
+	if (finger_state == 0x01) {
+		x = (touch_data[0] << 4) | (touch_data[2] & 0x0F);
+		y = (touch_data[1] << 4) | (touch_data[2] >> 4);
+		wx = touch_data[3] & 0x0F;
+		wy = touch_data[3] >> 4;
+		wide = (wx > wy);
+		major = max(wx, wy);
+		minor = min(wx, wy);
+		z = touch_data[4];
+
+		/* y is inverted */
+		y = hdata->max_y - y;
+
+		input_event(hdata->input, EV_ABS, ABS_MT_POSITION_X, x);
+		input_event(hdata->input, EV_ABS, ABS_MT_POSITION_Y, y);
+		input_event(hdata->input, EV_ABS, ABS_MT_ORIENTATION, wide);
+		input_event(hdata->input, EV_ABS, ABS_MT_PRESSURE, z);
+		input_event(hdata->input, EV_ABS, ABS_MT_TOUCH_MAJOR, major);
+		input_event(hdata->input, EV_ABS, ABS_MT_TOUCH_MINOR, minor);
+	}
+}
+
+static void rmi_reset_work(struct work_struct *work)
+{
+	struct rmi_data *hdata = container_of(work, struct rmi_data,
+						reset_work);
+
+	/* switch the device to RMI if we receive a generic mouse report */
+	rmi_set_mode(hdata->hdev, RMI_MODE_ATTN_REPORTS);
+}
+
+static inline int rmi_schedule_reset(struct hid_device *hdev)
+{
+	struct rmi_data *hdata = hid_get_drvdata(hdev);
+	return schedule_work(&hdata->reset_work);
+}
+
+static int rmi_f11_input_event(struct hid_device *hdev, u8 irq, u8 *data,
+		int size)
+{
+	struct rmi_data *hdata = hid_get_drvdata(hdev);
+	int offset;
+	int i;
+
+	if (size < hdata->f11.report_size)
+		return 0;
+
+	if (!(irq & hdata->f11.irq_mask))
+		return 0;
+
+	offset = (hdata->max_fingers >> 2) + 1;
+	for (i = 0; i < hdata->max_fingers; i++) {
+		int fs_byte_position = i >> 2;
+		int fs_bit_position = (i & 0x3) << 1;
+		int finger_state = (data[fs_byte_position] >> fs_bit_position) &
+					0x03;
+
+		rmi_f11_process_touch(hdata, i, finger_state,
+				&data[offset + 5 * i]);
+	}
+	input_mt_sync_frame(hdata->input);
+	input_sync(hdata->input);
+	return hdata->f11.report_size;
+}
+
+static int rmi_f30_input_event(struct hid_device *hdev, u8 irq, u8 *data,
+		int size)
+{
+	struct rmi_data *hdata = hid_get_drvdata(hdev);
+	int i;
+	int button = 0;
+	bool value;
+
+	if (!(irq & hdata->f30.irq_mask))
+		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);
+			if (test_bit(i, &hdata->button_state_mask))
+				value = !value;
+			input_event(hdata->input, EV_KEY, BTN_LEFT + button++,
+					value);
+		}
+	}
+	return hdata->f30.report_size;
+}
+
+static int rmi_input_event(struct hid_device *hdev, u8 *data, int size)
+{
+	struct rmi_data *hdata = hid_get_drvdata(hdev);
+	unsigned long irq_mask = 0;
+	unsigned index = 2;
+
+	if (!(test_bit(RMI_STARTED, &hdata->flags)))
+		return 0;
+
+	irq_mask |= hdata->f11.irq_mask;
+	irq_mask |= hdata->f30.irq_mask;
+
+	if (data[1] & ~irq_mask)
+		hid_warn(hdev, "unknown intr source:%02lx %s:%d\n",
+			data[1] & ~irq_mask, __FILE__, __LINE__);
+
+	if (hdata->f11.interrupt_base < hdata->f30.interrupt_base) {
+		index += rmi_f11_input_event(hdev, data[1], &data[index],
+				size - index);
+		index += rmi_f30_input_event(hdev, data[1], &data[index],
+				size - index);
+	} else {
+		index += rmi_f30_input_event(hdev, data[1], &data[index],
+				size - index);
+		index += rmi_f11_input_event(hdev, data[1], &data[index],
+				size - index);
+	}
+
+	return 1;
+}
+
+static int rmi_read_data_event(struct hid_device *hdev, u8 *data, int size)
+{
+	struct rmi_data *hdata = hid_get_drvdata(hdev);
+
+	if (!test_bit(RMI_READ_REQUEST_PENDING, &hdata->flags)) {
+		hid_err(hdev, "no read request pending\n");
+		return 0;
+	}
+
+	memcpy(hdata->readReport, data, size < hdata->input_report_size ?
+			size : hdata->input_report_size);
+	set_bit(RMI_READ_DATA_PENDING, &hdata->flags);
+	wake_up(&hdata->wait);
+
+	return 1;
+}
+
+static int rmi_raw_event(struct hid_device *hdev,
+		struct hid_report *report, u8 *data, int size)
+{
+	switch (data[0]) {
+	case RMI_READ_DATA_REPORT_ID:
+		return rmi_read_data_event(hdev, data, size);
+	case RMI_ATTN_REPORT_ID:
+		return rmi_input_event(hdev, data, size);
+	case RMI_MOUSE_REPORT_ID:
+		rmi_schedule_reset(hdev);
+		break;
+	}
+
+	return 0;
+}
+
+static int rmi_post_reset(struct hid_device *hdev)
+{
+	return rmi_set_mode(hdev, RMI_MODE_ATTN_REPORTS);
+}
+
+static int rmi_post_resume(struct hid_device *hdev)
+{
+	return rmi_set_mode(hdev, RMI_MODE_ATTN_REPORTS);
+}
+
+#define RMI4_MAX_PAGE 0xff
+#define RMI4_PAGE_SIZE 0x0100
+
+#define PDT_START_SCAN_LOCATION 0x00e9
+#define PDT_END_SCAN_LOCATION	0x0005
+#define RMI4_END_OF_PDT(id) ((id) == 0x00 || (id) == 0xff)
+
+struct pdt_entry {
+	u8 query_base_addr:8;
+	u8 command_base_addr:8;
+	u8 control_base_addr:8;
+	u8 data_base_addr:8;
+	u8 interrupt_source_count:3;
+	u8 bits3and4:2;
+	u8 function_version:2;
+	u8 bit7:1;
+	u8 function_number:8;
+} __attribute__((__packed__));
+
+static inline unsigned long rmi_gen_mask(unsigned irq_base, unsigned irq_count)
+{
+	return GENMASK(irq_count + irq_base - 1, irq_base);
+}
+
+static void rmi_register_function(struct rmi_data *data,
+	struct pdt_entry *pdt_entry, int page, unsigned interrupt_count)
+{
+	struct rmi_function *f = NULL;
+	u16 page_base = page << 8;
+
+	switch (pdt_entry->function_number) {
+	case 0x11:
+		f = &data->f11;
+		break;
+	case 0x30:
+		f = &data->f30;
+		break;
+	}
+
+	if (f) {
+		f->page = page;
+		f->query_base_addr = page_base | pdt_entry->query_base_addr;
+		f->command_base_addr = page_base | pdt_entry->command_base_addr;
+		f->control_base_addr = page_base | pdt_entry->control_base_addr;
+		f->data_base_addr = page_base | pdt_entry->data_base_addr;
+		f->interrupt_base = interrupt_count;
+		f->interrupt_count = pdt_entry->interrupt_source_count;
+		f->irq_mask = rmi_gen_mask(f->interrupt_base,
+						f->interrupt_count);
+	}
+}
+
+static int rmi_scan_pdt(struct hid_device *hdev)
+{
+	struct rmi_data *data = hid_get_drvdata(hdev);
+	struct pdt_entry entry;
+	int page;
+	bool page_has_function;
+	int i;
+	int retval;
+	int interrupt = 0;
+	u16 page_start, pdt_start , pdt_end;
+
+	hid_info(hdev, "Scanning PDT...\n");
+
+	for (page = 0; (page <= RMI4_MAX_PAGE); page++) {
+		page_start = RMI4_PAGE_SIZE * page;
+		pdt_start = page_start + PDT_START_SCAN_LOCATION;
+		pdt_end = page_start + PDT_END_SCAN_LOCATION;
+
+		page_has_function = false;
+		for (i = pdt_start; i >= pdt_end; i -= sizeof(entry)) {
+			retval = rmi_read_block(hdev, i, &entry, sizeof(entry));
+			if (retval) {
+				hid_err(hdev,
+					"Read of PDT entry at %#06x failed.\n",
+					i);
+				goto error_exit;
+			}
+
+			if (RMI4_END_OF_PDT(entry.function_number))
+				break;
+
+			page_has_function = true;
+
+			hid_info(hdev, "Found F%02X on page %#04x\n",
+					entry.function_number, page);
+
+			rmi_register_function(data, &entry, page, interrupt);
+			interrupt += entry.interrupt_source_count;
+		}
+
+		if (!page_has_function)
+			break;
+	}
+
+	hid_info(hdev, "%s: Done with PDT scan.\n", __func__);
+	retval = 0;
+
+error_exit:
+	return retval;
+}
+
+static int rmi_populate_f11(struct hid_device *hdev)
+{
+	struct rmi_data *data = hid_get_drvdata(hdev);
+	u8 buf[20];
+	int ret;
+	bool has_query9;
+	bool has_query10;
+	bool has_query11;
+	bool has_query12;
+	bool has_physical_props;
+	unsigned x_size, y_size;
+	u16 query12_offset;
+
+	if (!data->f11.query_base_addr) {
+		hid_err(hdev, "No 2D sensor found, giving up.\n");
+		return -ENODEV;
+	}
+
+	/* query 0 contains some useful information */
+	ret = rmi_read(hdev, data->f11.query_base_addr, buf);
+	if (ret) {
+		hid_err(hdev, "can not get query 0: %d.\n", ret);
+		return ret;
+	}
+	has_query9 = !!(buf[0] & BIT(3));
+	has_query11 = !!(buf[0] & BIT(4));
+	has_query12 = !!(buf[0] & BIT(5));
+
+	/* query 1 to get the max number of fingers */
+	ret = rmi_read(hdev, data->f11.query_base_addr + 1, buf);
+	if (ret) {
+		hid_err(hdev, "can not get NumberOfFingers: %d.\n", ret);
+		return ret;
+	}
+	data->max_fingers = (buf[0] & 0x07) + 1;
+	if (data->max_fingers > 5)
+		data->max_fingers = 10;
+
+	data->f11.report_size = data->max_fingers * 5 +
+				DIV_ROUND_UP(data->max_fingers, 4);
+
+	if (!(buf[0] & BIT(4))) {
+		hid_err(hdev, "No absolute events, giving up.\n");
+		return -ENODEV;
+	}
+
+	/* query 8 to find out if query 10 exists */
+	ret = rmi_read(hdev, data->f11.query_base_addr + 8, buf);
+	if (ret) {
+		hid_err(hdev, "can not read gesture information: %d.\n", ret);
+		return ret;
+	}
+	has_query10 = !!(buf[0] & BIT(2));
+
+	/*
+	 * At least 8 queries are guaranteed to be present in F11
+	 * +1 for query12.
+	 */
+	query12_offset = 9;
+
+	if (has_query9)
+		++query12_offset;
+
+	if (has_query10)
+		++query12_offset;
+
+	if (has_query11)
+		++query12_offset;
+
+	/* query 12 to know if the physical properties are reported */
+	if (has_query12) {
+		ret = rmi_read(hdev, data->f11.query_base_addr
+				+ query12_offset, buf);
+		if (ret) {
+			hid_err(hdev, "can not get query 12: %d.\n", ret);
+			return ret;
+		}
+		has_physical_props = !!(buf[0] & BIT(5));
+
+		if (has_physical_props) {
+			ret = rmi_read_block(hdev,
+					data->f11.query_base_addr
+						+ query12_offset + 1, buf, 4);
+			if (ret) {
+				hid_err(hdev, "can not read query 15-18: %d.\n",
+					ret);
+				return ret;
+			}
+
+			x_size = buf[0] | (buf[1] << 8);
+			y_size = buf[2] | (buf[3] << 8);
+
+			data->x_size_mm = DIV_ROUND_CLOSEST(x_size, 10);
+			data->y_size_mm = DIV_ROUND_CLOSEST(y_size, 10);
+
+			hid_info(hdev, "%s: size in mm: %d x %d\n",
+				 __func__, data->x_size_mm, data->y_size_mm);
+		}
+	}
+
+	/*
+	 * retrieve the ctrl registers
+	 * the ctrl register has a size of 20 but a fw bug split it into 16 + 4,
+	 * and there is no way to know if the first 20 bytes are here or not.
+	 * We use only the first 10 bytes, so get only them.
+	 */
+	ret = rmi_read_block(hdev, data->f11.control_base_addr, buf, 10);
+	if (ret) {
+		hid_err(hdev, "can not read ctrl block of size 10: %d.\n", ret);
+		return ret;
+	}
+
+	data->max_x = buf[6] | (buf[7] << 8);
+	data->max_y = buf[8] | (buf[9] << 8);
+
+	return 0;
+}
+
+static int rmi_populate_f30(struct hid_device *hdev)
+{
+	struct rmi_data *data = hid_get_drvdata(hdev);
+	u8 buf[20];
+	int ret;
+	bool has_gpio, has_led;
+	unsigned bytes_per_ctrl;
+	u8 ctrl2_addr;
+	int ctrl2_3_length;
+	int i;
+
+	/* function F30 is for physical buttons */
+	if (!data->f30.query_base_addr) {
+		hid_err(hdev, "No GPIO/LEDs found, giving up.\n");
+		return -ENODEV;
+	}
+
+	ret = rmi_read_block(hdev, data->f30.query_base_addr, buf, 2);
+	if (ret) {
+		hid_err(hdev, "can not get F30 query registers: %d.\n", ret);
+		return ret;
+	}
+
+	has_gpio = !!(buf[0] & BIT(3));
+	has_led = !!(buf[0] & BIT(2));
+	data->gpio_led_count = buf[1] & 0x1f;
+
+	/* retrieve ctrl 2 & 3 registers */
+	bytes_per_ctrl = (data->gpio_led_count + 7) / 8;
+	/* Ctrl0 is present only if both has_gpio and has_led are set*/
+	ctrl2_addr = (has_gpio && has_led) ? bytes_per_ctrl : 0;
+	/* Ctrl1 is always be present */
+	ctrl2_addr += bytes_per_ctrl;
+	ctrl2_3_length = 2 * bytes_per_ctrl;
+
+	data->f30.report_size = bytes_per_ctrl;
+
+	ret = rmi_read_block(hdev, data->f30.control_base_addr + ctrl2_addr,
+				buf, ctrl2_3_length);
+	if (ret) {
+		hid_err(hdev, "can not read ctrl 2&3 block of size %d: %d.\n",
+			ctrl2_3_length, ret);
+		return ret;
+	}
+
+	for (i = 0; i < data->gpio_led_count; i++) {
+		int byte_position = i >> 3;
+		int bit_position = i & 0x07;
+		u8 dir_byte = buf[byte_position];
+		u8 data_byte = buf[byte_position + bytes_per_ctrl];
+		bool dir = (dir_byte >> bit_position) & BIT(0);
+		bool dat = (data_byte >> bit_position) & BIT(0);
+
+		if (dir == 0) {
+			/* input mode */
+			if (dat) {
+				/* actual buttons have pull up resistor */
+				data->button_count++;
+				set_bit(i, &data->button_mask);
+				set_bit(i, &data->button_state_mask);
+			}
+		}
+
+	}
+
+	return 0;
+}
+
+static int rmi_populate(struct hid_device *hdev)
+{
+	int ret;
+
+	ret = rmi_scan_pdt(hdev);
+	if (ret) {
+		hid_err(hdev, "PDT scan failed with code %d.\n", ret);
+		return ret;
+	}
+
+	ret = rmi_populate_f11(hdev);
+	if (ret) {
+		hid_err(hdev, "Error while initializing F11 (%d).\n", ret);
+		return ret;
+	}
+
+	ret = rmi_populate_f30(hdev);
+	if (ret)
+		hid_warn(hdev, "Error while initializing F30 (%d).\n", ret);
+
+	return 0;
+}
+
+static void rmi_input_configured(struct hid_device *hdev, struct hid_input *hi)
+{
+	struct rmi_data *data = hid_get_drvdata(hdev);
+	struct input_dev *input = hi->input;
+	int ret;
+	int res_x, res_y, i;
+
+	data->input = input;
+
+	hid_dbg(hdev, "Opening low level driver\n");
+	ret = hid_hw_open(hdev);
+	if (ret)
+		return;
+
+	/* Allow incoming hid reports */
+	hid_device_io_start(hdev);
+
+	ret = rmi_set_mode(hdev, RMI_MODE_ATTN_REPORTS);
+	if (ret < 0) {
+		dev_err(&hdev->dev, "failed to set rmi mode\n");
+		goto exit;
+	}
+
+	ret = rmi_set_page(hdev, 0);
+	if (ret < 0) {
+		dev_err(&hdev->dev, "failed to set page select to 0.\n");
+		goto exit;
+	}
+
+	ret = rmi_populate(hdev);
+	if (ret)
+		goto exit;
+
+	__set_bit(EV_ABS, input->evbit);
+	input_set_abs_params(input, ABS_MT_POSITION_X, 1, data->max_x, 0, 0);
+	input_set_abs_params(input, ABS_MT_POSITION_Y, 1, data->max_y, 0, 0);
+
+	if (data->x_size_mm && data->y_size_mm) {
+		res_x = (data->max_x - 1) / data->x_size_mm;
+		res_y = (data->max_y - 1) / data->y_size_mm;
+
+		input_abs_set_res(input, ABS_MT_POSITION_X, res_x);
+		input_abs_set_res(input, ABS_MT_POSITION_Y, res_y);
+	}
+
+	input_set_abs_params(input, ABS_MT_ORIENTATION, 0, 1, 0, 0);
+	input_set_abs_params(input, ABS_MT_PRESSURE, 0, 0xff, 0, 0);
+	input_set_abs_params(input, ABS_MT_TOUCH_MAJOR, 0, 0x0f, 0, 0);
+	input_set_abs_params(input, ABS_MT_TOUCH_MINOR, 0, 0x0f, 0, 0);
+
+	input_mt_init_slots(input, data->max_fingers, INPUT_MT_POINTER);
+
+	if (data->button_count) {
+		__set_bit(EV_KEY, input->evbit);
+		for (i = 0; i < data->button_count; i++)
+			__set_bit(BTN_LEFT + i, input->keybit);
+
+		if (data->button_count == 1)
+			__set_bit(INPUT_PROP_BUTTONPAD, input->propbit);
+	}
+
+	set_bit(RMI_STARTED, &data->flags);
+
+exit:
+	hid_device_io_stop(hdev);
+	hid_hw_close(hdev);
+}
+
+static int rmi_input_mapping(struct hid_device *hdev,
+		struct hid_input *hi, struct hid_field *field,
+		struct hid_usage *usage, unsigned long **bit, int *max)
+{
+	/* we want to make HID ignore the advertised HID collection */
+	return -1;
+}
+
+static int rmi_probe(struct hid_device *hdev, const struct hid_device_id *id)
+{
+	struct rmi_data *data = NULL;
+	int ret;
+	size_t alloc_size;
+
+	data = devm_kzalloc(&hdev->dev, sizeof(struct rmi_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	INIT_WORK(&data->reset_work, rmi_reset_work);
+	data->hdev = hdev;
+
+	hid_set_drvdata(hdev, data);
+
+	hdev->quirks |= HID_QUIRK_NO_INIT_REPORTS;
+
+	ret = hid_parse(hdev);
+	if (ret) {
+		hid_err(hdev, "parse failed\n");
+		return ret;
+	}
+
+	data->input_report_size = (hdev->report_enum[HID_INPUT_REPORT]
+		.report_id_hash[RMI_ATTN_REPORT_ID]->size >> 3)
+		+ 1 /* report id */;
+	data->output_report_size = (hdev->report_enum[HID_OUTPUT_REPORT]
+		.report_id_hash[RMI_WRITE_REPORT_ID]->size >> 3)
+		+ 1 /* report id */;
+
+	alloc_size = data->output_report_size + data->input_report_size;
+
+	data->writeReport = devm_kzalloc(&hdev->dev, alloc_size, GFP_KERNEL);
+	if (!data->writeReport) {
+		ret = -ENOMEM;
+		return ret;
+	}
+
+	data->readReport = data->writeReport + data->output_report_size;
+
+	init_waitqueue_head(&data->wait);
+
+	mutex_init(&data->page_mutex);
+
+	ret = hid_hw_start(hdev, HID_CONNECT_DEFAULT);
+	if (ret) {
+		hid_err(hdev, "hw start failed\n");
+		return ret;
+	}
+
+	if (!test_bit(RMI_STARTED, &data->flags)) {
+		hid_hw_stop(hdev);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static void rmi_remove(struct hid_device *hdev)
+{
+	struct rmi_data *hdata = hid_get_drvdata(hdev);
+
+	clear_bit(RMI_STARTED, &hdata->flags);
+
+	hid_hw_stop(hdev);
+}
+
+static const struct hid_device_id rmi_id[] = {
+	{ HID_DEVICE(HID_BUS_ANY, HID_GROUP_RMI, HID_ANY_ID, HID_ANY_ID) },
+	{ }
+};
+MODULE_DEVICE_TABLE(hid, rmi_id);
+
+static struct hid_driver rmi_driver = {
+	.name = "hid-rmi",
+	.id_table		= rmi_id,
+	.probe			= rmi_probe,
+	.remove			= rmi_remove,
+	.raw_event		= rmi_raw_event,
+	.input_mapping		= rmi_input_mapping,
+	.input_configured	= rmi_input_configured,
+#ifdef CONFIG_PM
+	.resume			= rmi_post_resume,
+	.reset_resume		= rmi_post_reset,
+#endif
+};
+
+module_hid_driver(rmi_driver);
+
+MODULE_AUTHOR("Andrew Duggan <aduggan@synaptics.com>");
+MODULE_DESCRIPTION("RMI HID driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hid/hid-saitek.c b/drivers/hid/hid-saitek.c
index 37961c7e397..69cca1476a0 100644
--- a/drivers/hid/hid-saitek.c
+++ b/drivers/hid/hid-saitek.c
@@ -1,10 +1,17 @@
 /*
- *  HID driver for Saitek devices, currently only the PS1000 (USB gamepad).
+ *  HID driver for Saitek devices.
+ *
+ *  PS1000 (USB gamepad):
  *  Fixes the HID report descriptor by removing a non-existent axis and
  *  clearing the constant bit on the input reports for buttons and d-pad.
  *  (This module is based on "hid-ortek".)
- *
  *  Copyright (c) 2012 Andreas Hübner
+ *
+ *  R.A.T.7, M.M.O.7 (USB gaming mice):
+ *  Fixes the mode button which cycles through three constantly pressed
+ *  buttons. All three press events are mapped to one button and the
+ *  missing release event is generated immediately.
+ *
  */
 
 /*
@@ -21,12 +28,57 @@
 
 #include "hid-ids.h"
 
+#define SAITEK_FIX_PS1000	0x0001
+#define SAITEK_RELEASE_MODE_RAT7	0x0002
+#define SAITEK_RELEASE_MODE_MMO7	0x0004
+
+struct saitek_sc {
+	unsigned long quirks;
+	int mode;
+};
+
+static int saitek_probe(struct hid_device *hdev,
+		const struct hid_device_id *id)
+{
+	unsigned long quirks = id->driver_data;
+	struct saitek_sc *ssc;
+	int ret;
+
+	ssc = devm_kzalloc(&hdev->dev, sizeof(*ssc), GFP_KERNEL);
+	if (ssc == NULL) {
+		hid_err(hdev, "can't alloc saitek descriptor\n");
+		return -ENOMEM;
+	}
+
+	ssc->quirks = quirks;
+	ssc->mode = -1;
+
+	hid_set_drvdata(hdev, ssc);
+
+	ret = hid_parse(hdev);
+	if (ret) {
+		hid_err(hdev, "parse failed\n");
+		return ret;
+	}
+
+	ret = hid_hw_start(hdev, HID_CONNECT_DEFAULT);
+	if (ret) {
+		hid_err(hdev, "hw start failed\n");
+		return ret;
+	}
+
+	return 0;
+}
+
 static __u8 *saitek_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 		unsigned int *rsize)
 {
-	if (*rsize == 137 && rdesc[20] == 0x09 && rdesc[21] == 0x33
-			&& rdesc[94] == 0x81 && rdesc[95] == 0x03
-			&& rdesc[110] == 0x81 && rdesc[111] == 0x03) {
+	struct saitek_sc *ssc = hid_get_drvdata(hdev);
+
+	if ((ssc->quirks & SAITEK_FIX_PS1000) && *rsize == 137 &&
+			rdesc[20] == 0x09 && rdesc[21] == 0x33 &&
+			rdesc[94] == 0x81 && rdesc[95] == 0x03 &&
+			rdesc[110] == 0x81 && rdesc[111] == 0x03) {
 
 		hid_info(hdev, "Fixing up Saitek PS1000 report descriptor\n");
 
@@ -42,8 +94,93 @@ static __u8 *saitek_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 	return rdesc;
 }
 
+static int saitek_raw_event(struct hid_device *hdev,
+		struct hid_report *report, u8 *raw_data, int size)
+{
+	struct saitek_sc *ssc = hid_get_drvdata(hdev);
+
+	if (ssc->quirks & SAITEK_RELEASE_MODE_RAT7 && size == 7) {
+		/* R.A.T.7 uses bits 13, 14, 15 for the mode */
+		int mode = -1;
+		if (raw_data[1] & 0x01)
+			mode = 0;
+		else if (raw_data[1] & 0x02)
+			mode = 1;
+		else if (raw_data[1] & 0x04)
+			mode = 2;
+
+		/* clear mode bits */
+		raw_data[1] &= ~0x07;
+
+		if (mode != ssc->mode) {
+			hid_dbg(hdev, "entered mode %d\n", mode);
+			if (ssc->mode != -1) {
+				/* use bit 13 as the mode button */
+				raw_data[1] |= 0x04;
+			}
+			ssc->mode = mode;
+		}
+	} else if (ssc->quirks & SAITEK_RELEASE_MODE_MMO7 && size == 8) {
+
+		/* M.M.O.7 uses bits 8, 22, 23 for the mode */
+		int mode = -1;
+		if (raw_data[1] & 0x80)
+			mode = 0;
+		else if (raw_data[2] & 0x01)
+			mode = 1;
+		else if (raw_data[2] & 0x02)
+			mode = 2;
+
+		/* clear mode bits */
+		raw_data[1] &= ~0x80;
+		raw_data[2] &= ~0x03;
+
+		if (mode != ssc->mode) {
+			hid_dbg(hdev, "entered mode %d\n", mode);
+			if (ssc->mode != -1) {
+				/* use bit 8 as the mode button, bits 22
+				 * and 23 do not represent buttons
+				 * according to the HID report descriptor
+				 */
+				raw_data[1] |= 0x80;
+			}
+			ssc->mode = mode;
+		}
+	}
+
+	return 0;
+}
+
+static int saitek_event(struct hid_device *hdev, struct hid_field *field,
+		struct hid_usage *usage, __s32 value)
+{
+	struct saitek_sc *ssc = hid_get_drvdata(hdev);
+	struct input_dev *input = field->hidinput->input;
+
+	if (usage->type == EV_KEY && value &&
+			(((ssc->quirks & SAITEK_RELEASE_MODE_RAT7) &&
+			  usage->code - BTN_MOUSE == 10) ||
+			((ssc->quirks & SAITEK_RELEASE_MODE_MMO7) &&
+			 usage->code - BTN_MOUSE == 15))) {
+
+		input_report_key(input, usage->code, 1);
+
+		/* report missing release event */
+		input_report_key(input, usage->code, 0);
+
+		return 1;
+	}
+
+	return 0;
+}
+
 static const struct hid_device_id saitek_devices[] = {
-	{ HID_USB_DEVICE(USB_VENDOR_ID_SAITEK, USB_DEVICE_ID_SAITEK_PS1000)},
+	{ HID_USB_DEVICE(USB_VENDOR_ID_SAITEK, USB_DEVICE_ID_SAITEK_PS1000),
+		.driver_data = SAITEK_FIX_PS1000 },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_SAITEK, USB_DEVICE_ID_SAITEK_RAT7),
+		.driver_data = SAITEK_RELEASE_MODE_RAT7 },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_SAITEK, USB_DEVICE_ID_SAITEK_MMO7),
+		.driver_data = SAITEK_RELEASE_MODE_MMO7 },
 	{ }
 };
 
@@ -52,7 +189,10 @@ MODULE_DEVICE_TABLE(hid, saitek_devices);
 static struct hid_driver saitek_driver = {
 	.name = "saitek",
 	.id_table = saitek_devices,
-	.report_fixup = saitek_report_fixup
+	.probe = saitek_probe,
+	.report_fixup = saitek_report_fixup,
+	.raw_event = saitek_raw_event,
+	.event = saitek_event,
 };
 module_hid_driver(saitek_driver);
 
diff --git a/drivers/hid/hid-sensor-hub.c b/drivers/hid/hid-sensor-hub.c
index be14b5690e9..a8d5c8faf8c 100644
--- a/drivers/hid/hid-sensor-hub.c
+++ b/drivers/hid/hid-sensor-hub.c
@@ -705,8 +705,17 @@ static const struct hid_device_id sensor_hub_devices[] = {
 	{ HID_DEVICE(HID_BUS_ANY, HID_GROUP_SENSOR_HUB, USB_VENDOR_ID_INTEL_1,
 			USB_DEVICE_ID_INTEL_HID_SENSOR_1),
 			.driver_data = HID_SENSOR_HUB_ENUM_QUIRK},
+	{ HID_DEVICE(HID_BUS_ANY, HID_GROUP_SENSOR_HUB, USB_VENDOR_ID_MICROSOFT,
+			USB_DEVICE_ID_MS_SURFACE_PRO_2),
+			.driver_data = HID_SENSOR_HUB_ENUM_QUIRK},
+	{ HID_DEVICE(HID_BUS_ANY, HID_GROUP_SENSOR_HUB, USB_VENDOR_ID_MICROSOFT,
+			USB_DEVICE_ID_MS_TOUCH_COVER_2),
+			.driver_data = HID_SENSOR_HUB_ENUM_QUIRK},
+	{ HID_DEVICE(HID_BUS_ANY, HID_GROUP_SENSOR_HUB, USB_VENDOR_ID_MICROSOFT,
+			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),
+			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,
 			USB_DEVICE_ID_TEXAS_INSTRUMENTS_LENOVO_YOGA),
diff --git a/drivers/hid/hid-sony.c b/drivers/hid/hid-sony.c
index 908de278921..2259eaa8b98 100644
--- a/drivers/hid/hid-sony.c
+++ b/drivers/hid/hid-sony.c
@@ -33,6 +33,7 @@
 #include <linux/power_supply.h>
 #include <linux/spinlock.h>
 #include <linux/list.h>
+#include <linux/idr.h>
 #include <linux/input/mt.h>
 
 #include "hid-ids.h"
@@ -717,8 +718,39 @@ static enum power_supply_property sony_battery_props[] = {
 	POWER_SUPPLY_PROP_STATUS,
 };
 
+struct sixaxis_led {
+	__u8 time_enabled; /* the total time the led is active (0xff means forever) */
+	__u8 duty_length;  /* how long a cycle is in deciseconds (0 means "really fast") */
+	__u8 enabled;
+	__u8 duty_off; /* % of duty_length the led is off (0xff means 100%) */
+	__u8 duty_on;  /* % of duty_length the led is on (0xff mean 100%) */
+} __packed;
+
+struct sixaxis_rumble {
+	__u8 padding;
+	__u8 right_duration; /* Right motor duration (0xff means forever) */
+	__u8 right_motor_on; /* Right (small) motor on/off, only supports values of 0 or 1 (off/on) */
+	__u8 left_duration;    /* Left motor duration (0xff means forever) */
+	__u8 left_motor_force; /* left (large) motor, supports force values from 0 to 255 */
+} __packed;
+
+struct sixaxis_output_report {
+	__u8 report_id;
+	struct sixaxis_rumble rumble;
+	__u8 padding[4];
+	__u8 leds_bitmap; /* bitmap of enabled LEDs: LED_1 = 0x02, LED_2 = 0x04, ... */
+	struct sixaxis_led led[4];    /* LEDx at (4 - x) */
+	struct sixaxis_led _reserved; /* LED5, not actually soldered */
+} __packed;
+
+union sixaxis_output_report_01 {
+	struct sixaxis_output_report data;
+	__u8 buf[36];
+};
+
 static spinlock_t sony_dev_list_lock;
 static LIST_HEAD(sony_device_list);
+static DEFINE_IDA(sony_device_id_allocator);
 
 struct sony_sc {
 	spinlock_t lock;
@@ -728,6 +760,7 @@ struct sony_sc {
 	unsigned long quirks;
 	struct work_struct state_worker;
 	struct power_supply battery;
+	int device_id;
 
 #ifdef CONFIG_SONY_FF
 	__u8 left;
@@ -740,6 +773,8 @@ struct sony_sc {
 	__u8 battery_charging;
 	__u8 battery_capacity;
 	__u8 led_state[MAX_LEDS];
+	__u8 led_delay_on[MAX_LEDS];
+	__u8 led_delay_off[MAX_LEDS];
 	__u8 led_count;
 };
 
@@ -1048,6 +1083,52 @@ static int dualshock4_set_operational_bt(struct hid_device *hdev)
 				HID_FEATURE_REPORT, HID_REQ_GET_REPORT);
 }
 
+static void sixaxis_set_leds_from_id(int id, __u8 values[MAX_LEDS])
+{
+	static const __u8 sixaxis_leds[10][4] = {
+				{ 0x01, 0x00, 0x00, 0x00 },
+				{ 0x00, 0x01, 0x00, 0x00 },
+				{ 0x00, 0x00, 0x01, 0x00 },
+				{ 0x00, 0x00, 0x00, 0x01 },
+				{ 0x01, 0x00, 0x00, 0x01 },
+				{ 0x00, 0x01, 0x00, 0x01 },
+				{ 0x00, 0x00, 0x01, 0x01 },
+				{ 0x01, 0x00, 0x01, 0x01 },
+				{ 0x00, 0x01, 0x01, 0x01 },
+				{ 0x01, 0x01, 0x01, 0x01 }
+	};
+
+	BUG_ON(MAX_LEDS < ARRAY_SIZE(sixaxis_leds[0]));
+
+	if (id < 0)
+		return;
+
+	id %= 10;
+	memcpy(values, sixaxis_leds[id], sizeof(sixaxis_leds[id]));
+}
+
+static void dualshock4_set_leds_from_id(int id, __u8 values[MAX_LEDS])
+{
+	/* The first 4 color/index entries match what the PS4 assigns */
+	static const __u8 color_code[7][3] = {
+			/* Blue   */	{ 0x00, 0x00, 0x01 },
+			/* Red	  */	{ 0x01, 0x00, 0x00 },
+			/* Green  */	{ 0x00, 0x01, 0x00 },
+			/* Pink   */	{ 0x02, 0x00, 0x01 },
+			/* Orange */	{ 0x02, 0x01, 0x00 },
+			/* Teal   */	{ 0x00, 0x01, 0x01 },
+			/* White  */	{ 0x01, 0x01, 0x01 }
+	};
+
+	BUG_ON(MAX_LEDS < ARRAY_SIZE(color_code[0]));
+
+	if (id < 0)
+		return;
+
+	id %= 7;
+	memcpy(values, color_code[id], sizeof(color_code[id]));
+}
+
 static void buzz_set_leds(struct hid_device *hdev, const __u8 *leds)
 {
 	struct list_head *report_list =
@@ -1066,19 +1147,18 @@ static void buzz_set_leds(struct hid_device *hdev, const __u8 *leds)
 	hid_hw_request(hdev, report, HID_REQ_SET_REPORT);
 }
 
-static void sony_set_leds(struct hid_device *hdev, const __u8 *leds, int count)
+static void sony_set_leds(struct sony_sc *sc, const __u8 *leds, int count)
 {
-	struct sony_sc *drv_data = hid_get_drvdata(hdev);
 	int n;
 
 	BUG_ON(count > MAX_LEDS);
 
-	if (drv_data->quirks & BUZZ_CONTROLLER && count == 4) {
-		buzz_set_leds(hdev, leds);
+	if (sc->quirks & BUZZ_CONTROLLER && count == 4) {
+		buzz_set_leds(sc->hdev, leds);
 	} else {
 		for (n = 0; n < count; n++)
-			drv_data->led_state[n] = leds[n];
-		schedule_work(&drv_data->state_worker);
+			sc->led_state[n] = leds[n];
+		schedule_work(&sc->state_worker);
 	}
 }
 
@@ -1090,6 +1170,7 @@ static void sony_led_set_brightness(struct led_classdev *led,
 	struct sony_sc *drv_data;
 
 	int n;
+	int force_update;
 
 	drv_data = hid_get_drvdata(hdev);
 	if (!drv_data) {
@@ -1097,12 +1178,29 @@ static void sony_led_set_brightness(struct led_classdev *led,
 		return;
 	}
 
+	/*
+	 * The Sixaxis on USB will override any LED settings sent to it
+	 * and keep flashing all of the LEDs until the PS button is pressed.
+	 * Updates, even if redundant, must be always be sent to the
+	 * controller to avoid having to toggle the state of an LED just to
+	 * stop the flashing later on.
+	 */
+	force_update = !!(drv_data->quirks & SIXAXIS_CONTROLLER_USB);
+
 	for (n = 0; n < drv_data->led_count; n++) {
-		if (led == drv_data->leds[n]) {
-			if (value != drv_data->led_state[n]) {
-				drv_data->led_state[n] = value;
-				sony_set_leds(hdev, drv_data->led_state, drv_data->led_count);
-			}
+		if (led == drv_data->leds[n] && (force_update ||
+			(value != drv_data->led_state[n] ||
+			drv_data->led_delay_on[n] ||
+			drv_data->led_delay_off[n]))) {
+
+			drv_data->led_state[n] = value;
+
+			/* Setting the brightness stops the blinking */
+			drv_data->led_delay_on[n] = 0;
+			drv_data->led_delay_off[n] = 0;
+
+			sony_set_leds(drv_data, drv_data->led_state,
+					drv_data->led_count);
 			break;
 		}
 	}
@@ -1130,63 +1228,112 @@ static enum led_brightness sony_led_get_brightness(struct led_classdev *led)
 	return LED_OFF;
 }
 
-static void sony_leds_remove(struct hid_device *hdev)
+static int sony_led_blink_set(struct led_classdev *led, unsigned long *delay_on,
+				unsigned long *delay_off)
 {
-	struct sony_sc *drv_data;
-	struct led_classdev *led;
+	struct device *dev = led->dev->parent;
+	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct sony_sc *drv_data = hid_get_drvdata(hdev);
 	int n;
+	__u8 new_on, new_off;
 
-	drv_data = hid_get_drvdata(hdev);
-	BUG_ON(!(drv_data->quirks & SONY_LED_SUPPORT));
+	if (!drv_data) {
+		hid_err(hdev, "No device data\n");
+		return -EINVAL;
+	}
+
+	/* Max delay is 255 deciseconds or 2550 milliseconds */
+	if (*delay_on > 2550)
+		*delay_on = 2550;
+	if (*delay_off > 2550)
+		*delay_off = 2550;
+
+	/* Blink at 1 Hz if both values are zero */
+	if (!*delay_on && !*delay_off)
+		*delay_on = *delay_off = 500;
+
+	new_on = *delay_on / 10;
+	new_off = *delay_off / 10;
 
 	for (n = 0; n < drv_data->led_count; n++) {
-		led = drv_data->leds[n];
-		drv_data->leds[n] = NULL;
+		if (led == drv_data->leds[n])
+			break;
+	}
+
+	/* This LED is not registered on this device */
+	if (n >= drv_data->led_count)
+		return -EINVAL;
+
+	/* Don't schedule work if the values didn't change */
+	if (new_on != drv_data->led_delay_on[n] ||
+		new_off != drv_data->led_delay_off[n]) {
+		drv_data->led_delay_on[n] = new_on;
+		drv_data->led_delay_off[n] = new_off;
+		schedule_work(&drv_data->state_worker);
+	}
+
+	return 0;
+}
+
+static void sony_leds_remove(struct sony_sc *sc)
+{
+	struct led_classdev *led;
+	int n;
+
+	BUG_ON(!(sc->quirks & SONY_LED_SUPPORT));
+
+	for (n = 0; n < sc->led_count; n++) {
+		led = sc->leds[n];
+		sc->leds[n] = NULL;
 		if (!led)
 			continue;
 		led_classdev_unregister(led);
 		kfree(led);
 	}
 
-	drv_data->led_count = 0;
+	sc->led_count = 0;
 }
 
-static int sony_leds_init(struct hid_device *hdev)
+static int sony_leds_init(struct sony_sc *sc)
 {
-	struct sony_sc *drv_data;
+	struct hid_device *hdev = sc->hdev;
 	int n, ret = 0;
-	int max_brightness;
-	int use_colors;
+	int use_ds4_names;
 	struct led_classdev *led;
 	size_t name_sz;
 	char *name;
 	size_t name_len;
 	const char *name_fmt;
-	static const char * const color_str[] = { "red", "green", "blue" };
-	static const __u8 initial_values[MAX_LEDS] = { 0x00, 0x00, 0x00, 0x00 };
+	static const char * const ds4_name_str[] = { "red", "green", "blue",
+						  "global" };
+	__u8 initial_values[MAX_LEDS] = { 0 };
+	__u8 max_brightness[MAX_LEDS] = { 1 };
+	__u8 use_hw_blink[MAX_LEDS] = { 0 };
 
-	drv_data = hid_get_drvdata(hdev);
-	BUG_ON(!(drv_data->quirks & SONY_LED_SUPPORT));
+	BUG_ON(!(sc->quirks & SONY_LED_SUPPORT));
 
-	if (drv_data->quirks & BUZZ_CONTROLLER) {
-		drv_data->led_count = 4;
-		max_brightness = 1;
-		use_colors = 0;
+	if (sc->quirks & BUZZ_CONTROLLER) {
+		sc->led_count = 4;
+		use_ds4_names = 0;
 		name_len = strlen("::buzz#");
 		name_fmt = "%s::buzz%d";
 		/* Validate expected report characteristics. */
 		if (!hid_validate_values(hdev, HID_OUTPUT_REPORT, 0, 0, 7))
 			return -ENODEV;
-	} else if (drv_data->quirks & DUALSHOCK4_CONTROLLER) {
-		drv_data->led_count = 3;
-		max_brightness = 255;
-		use_colors = 1;
+	} else if (sc->quirks & DUALSHOCK4_CONTROLLER) {
+		dualshock4_set_leds_from_id(sc->device_id, initial_values);
+		initial_values[3] = 1;
+		sc->led_count = 4;
+		memset(max_brightness, 255, 3);
+		use_hw_blink[3] = 1;
+		use_ds4_names = 1;
 		name_len = 0;
 		name_fmt = "%s:%s";
 	} else {
-		drv_data->led_count = 4;
-		max_brightness = 1;
-		use_colors = 0;
+		sixaxis_set_leds_from_id(sc->device_id, initial_values);
+		sc->led_count = 4;
+		memset(use_hw_blink, 1, 4);
+		use_ds4_names = 0;
 		name_len = strlen("::sony#");
 		name_fmt = "%s::sony%d";
 	}
@@ -1196,14 +1343,14 @@ static int sony_leds_init(struct hid_device *hdev)
 	 * only relevant if the driver is loaded after somebody actively set the
 	 * LEDs to on
 	 */
-	sony_set_leds(hdev, initial_values, drv_data->led_count);
+	sony_set_leds(sc, initial_values, sc->led_count);
 
 	name_sz = strlen(dev_name(&hdev->dev)) + name_len + 1;
 
-	for (n = 0; n < drv_data->led_count; n++) {
+	for (n = 0; n < sc->led_count; n++) {
 
-		if (use_colors)
-			name_sz = strlen(dev_name(&hdev->dev)) + strlen(color_str[n]) + 2;
+		if (use_ds4_names)
+			name_sz = strlen(dev_name(&hdev->dev)) + strlen(ds4_name_str[n]) + 2;
 
 		led = kzalloc(sizeof(struct led_classdev) + name_sz, GFP_KERNEL);
 		if (!led) {
@@ -1213,30 +1360,35 @@ static int sony_leds_init(struct hid_device *hdev)
 		}
 
 		name = (void *)(&led[1]);
-		if (use_colors)
-			snprintf(name, name_sz, name_fmt, dev_name(&hdev->dev), color_str[n]);
+		if (use_ds4_names)
+			snprintf(name, name_sz, name_fmt, dev_name(&hdev->dev),
+			ds4_name_str[n]);
 		else
 			snprintf(name, name_sz, name_fmt, dev_name(&hdev->dev), n + 1);
 		led->name = name;
-		led->brightness = 0;
-		led->max_brightness = max_brightness;
+		led->brightness = initial_values[n];
+		led->max_brightness = max_brightness[n];
 		led->brightness_get = sony_led_get_brightness;
 		led->brightness_set = sony_led_set_brightness;
 
+		if (use_hw_blink[n])
+			led->blink_set = sony_led_blink_set;
+
+		sc->leds[n] = led;
+
 		ret = led_classdev_register(&hdev->dev, led);
 		if (ret) {
 			hid_err(hdev, "Failed to register LED %d\n", n);
+			sc->leds[n] = NULL;
 			kfree(led);
 			goto error_leds;
 		}
-
-		drv_data->leds[n] = led;
 	}
 
 	return ret;
 
 error_leds:
-	sony_leds_remove(hdev);
+	sony_leds_remove(sc);
 
 	return ret;
 }
@@ -1244,29 +1396,52 @@ error_leds:
 static void sixaxis_state_worker(struct work_struct *work)
 {
 	struct sony_sc *sc = container_of(work, struct sony_sc, state_worker);
-	unsigned char buf[] = {
-		0x01,
-		0x00, 0xff, 0x00, 0xff, 0x00,
-		0x00, 0x00, 0x00, 0x00, 0x00,
-		0xff, 0x27, 0x10, 0x00, 0x32,
-		0xff, 0x27, 0x10, 0x00, 0x32,
-		0xff, 0x27, 0x10, 0x00, 0x32,
-		0xff, 0x27, 0x10, 0x00, 0x32,
-		0x00, 0x00, 0x00, 0x00, 0x00
+	int n;
+	union sixaxis_output_report_01 report = {
+		.buf = {
+			0x01,
+			0x00, 0xff, 0x00, 0xff, 0x00,
+			0x00, 0x00, 0x00, 0x00, 0x00,
+			0xff, 0x27, 0x10, 0x00, 0x32,
+			0xff, 0x27, 0x10, 0x00, 0x32,
+			0xff, 0x27, 0x10, 0x00, 0x32,
+			0xff, 0x27, 0x10, 0x00, 0x32,
+			0x00, 0x00, 0x00, 0x00, 0x00
+		}
 	};
 
 #ifdef CONFIG_SONY_FF
-	buf[3] = sc->right ? 1 : 0;
-	buf[5] = sc->left;
+	report.data.rumble.right_motor_on = sc->right ? 1 : 0;
+	report.data.rumble.left_motor_force = sc->left;
 #endif
 
-	buf[10] |= sc->led_state[0] << 1;
-	buf[10] |= sc->led_state[1] << 2;
-	buf[10] |= sc->led_state[2] << 3;
-	buf[10] |= sc->led_state[3] << 4;
+	report.data.leds_bitmap |= sc->led_state[0] << 1;
+	report.data.leds_bitmap |= sc->led_state[1] << 2;
+	report.data.leds_bitmap |= sc->led_state[2] << 3;
+	report.data.leds_bitmap |= sc->led_state[3] << 4;
+
+	/* Set flag for all leds off, required for 3rd party INTEC controller */
+	if ((report.data.leds_bitmap & 0x1E) == 0)
+		report.data.leds_bitmap |= 0x20;
 
-	hid_hw_raw_request(sc->hdev, 0x01, buf, sizeof(buf), HID_OUTPUT_REPORT,
-			HID_REQ_SET_REPORT);
+	/*
+	 * The LEDs in the report are indexed in reverse order to their
+	 * corresponding light on the controller.
+	 * Index 0 = LED 4, index 1 = LED 3, etc...
+	 *
+	 * In the case of both delay values being zero (blinking disabled) the
+	 * default report values should be used or the controller LED will be
+	 * always off.
+	 */
+	for (n = 0; n < 4; n++) {
+		if (sc->led_delay_on[n] || sc->led_delay_off[n]) {
+			report.data.led[3 - n].duty_off = sc->led_delay_off[n];
+			report.data.led[3 - n].duty_on = sc->led_delay_on[n];
+		}
+	}
+
+	hid_hw_raw_request(sc->hdev, report.data.report_id, report.buf,
+			sizeof(report), HID_OUTPUT_REPORT, HID_REQ_SET_REPORT);
 }
 
 static void dualshock4_state_worker(struct work_struct *work)
@@ -1279,7 +1454,7 @@ static void dualshock4_state_worker(struct work_struct *work)
 
 	if (sc->quirks & DUALSHOCK4_CONTROLLER_USB) {
 		buf[0] = 0x05;
-		buf[1] = 0x03;
+		buf[1] = 0xFF;
 		offset = 4;
 	} else {
 		buf[0] = 0x11;
@@ -1295,9 +1470,18 @@ static void dualshock4_state_worker(struct work_struct *work)
 	offset += 2;
 #endif
 
-	buf[offset++] = sc->led_state[0];
-	buf[offset++] = sc->led_state[1];
-	buf[offset++] = sc->led_state[2];
+	/* LED 3 is the global control */
+	if (sc->led_state[3]) {
+		buf[offset++] = sc->led_state[0];
+		buf[offset++] = sc->led_state[1];
+		buf[offset++] = sc->led_state[2];
+	} else {
+		offset += 3;
+	}
+
+	/* If both delay values are zero the DualShock 4 disables blinking. */
+	buf[offset++] = sc->led_delay_on[3];
+	buf[offset++] = sc->led_delay_off[3];
 
 	if (sc->quirks & DUALSHOCK4_CONTROLLER_USB)
 		hid_hw_output_report(hdev, buf, 32);
@@ -1323,9 +1507,9 @@ static int sony_play_effect(struct input_dev *dev, void *data,
 	return 0;
 }
 
-static int sony_init_ff(struct hid_device *hdev)
+static int sony_init_ff(struct sony_sc *sc)
 {
-	struct hid_input *hidinput = list_entry(hdev->inputs.next,
+	struct hid_input *hidinput = list_entry(sc->hdev->inputs.next,
 						struct hid_input, list);
 	struct input_dev *input_dev = hidinput->input;
 
@@ -1334,7 +1518,7 @@ static int sony_init_ff(struct hid_device *hdev)
 }
 
 #else
-static int sony_init_ff(struct hid_device *hdev)
+static int sony_init_ff(struct sony_sc *sc)
 {
 	return 0;
 }
@@ -1384,8 +1568,6 @@ static int sony_battery_get_property(struct power_supply *psy,
 
 static int sony_battery_probe(struct sony_sc *sc)
 {
-	static atomic_t power_id_seq = ATOMIC_INIT(0);
-	unsigned long power_id;
 	struct hid_device *hdev = sc->hdev;
 	int ret;
 
@@ -1395,15 +1577,13 @@ static int sony_battery_probe(struct sony_sc *sc)
 	 */
 	sc->battery_capacity = 100;
 
-	power_id = (unsigned long)atomic_inc_return(&power_id_seq);
-
 	sc->battery.properties = sony_battery_props;
 	sc->battery.num_properties = ARRAY_SIZE(sony_battery_props);
 	sc->battery.get_property = sony_battery_get_property;
 	sc->battery.type = POWER_SUPPLY_TYPE_BATTERY;
 	sc->battery.use_for_apm = 0;
-	sc->battery.name = kasprintf(GFP_KERNEL, "sony_controller_battery_%lu",
-				     power_id);
+	sc->battery.name = kasprintf(GFP_KERNEL, "sony_controller_battery_%pMR",
+				     sc->mac_address);
 	if (!sc->battery.name)
 		return -ENOMEM;
 
@@ -1578,6 +1758,52 @@ static int sony_check_add(struct sony_sc *sc)
 	return sony_check_add_dev_list(sc);
 }
 
+static int sony_set_device_id(struct sony_sc *sc)
+{
+	int ret;
+
+	/*
+	 * Only DualShock 4 or Sixaxis controllers get an id.
+	 * All others are set to -1.
+	 */
+	if ((sc->quirks & SIXAXIS_CONTROLLER) ||
+	    (sc->quirks & DUALSHOCK4_CONTROLLER)) {
+		ret = ida_simple_get(&sony_device_id_allocator, 0, 0,
+					GFP_KERNEL);
+		if (ret < 0) {
+			sc->device_id = -1;
+			return ret;
+		}
+		sc->device_id = ret;
+	} else {
+		sc->device_id = -1;
+	}
+
+	return 0;
+}
+
+static void sony_release_device_id(struct sony_sc *sc)
+{
+	if (sc->device_id >= 0) {
+		ida_simple_remove(&sony_device_id_allocator, sc->device_id);
+		sc->device_id = -1;
+	}
+}
+
+static inline void sony_init_work(struct sony_sc *sc,
+					void (*worker)(struct work_struct *))
+{
+	if (!sc->worker_initialized)
+		INIT_WORK(&sc->state_worker, worker);
+
+	sc->worker_initialized = 1;
+}
+
+static inline void sony_cancel_work_sync(struct sony_sc *sc)
+{
+	if (sc->worker_initialized)
+		cancel_work_sync(&sc->state_worker);
+}
 
 static int sony_probe(struct hid_device *hdev, const struct hid_device_id *id)
 {
@@ -1615,6 +1841,12 @@ static int sony_probe(struct hid_device *hdev, const struct hid_device_id *id)
 		return ret;
 	}
 
+	ret = sony_set_device_id(sc);
+	if (ret < 0) {
+		hid_err(hdev, "failed to allocate the device id\n");
+		goto err_stop;
+	}
+
 	if (sc->quirks & SIXAXIS_CONTROLLER_USB) {
 		/*
 		 * The Sony Sixaxis does not handle HID Output Reports on the
@@ -1629,8 +1861,7 @@ static int sony_probe(struct hid_device *hdev, const struct hid_device_id *id)
 		hdev->quirks |= HID_QUIRK_NO_OUTPUT_REPORTS_ON_INTR_EP;
 		hdev->quirks |= HID_QUIRK_SKIP_OUTPUT_REPORT_ID;
 		ret = sixaxis_set_operational_usb(hdev);
-		sc->worker_initialized = 1;
-		INIT_WORK(&sc->state_worker, sixaxis_state_worker);
+		sony_init_work(sc, sixaxis_state_worker);
 	} else if (sc->quirks & SIXAXIS_CONTROLLER_BT) {
 		/*
 		 * The Sixaxis wants output reports sent on the ctrl endpoint
@@ -1638,8 +1869,7 @@ static int sony_probe(struct hid_device *hdev, const struct hid_device_id *id)
 		 */
 		hdev->quirks |= HID_QUIRK_NO_OUTPUT_REPORTS_ON_INTR_EP;
 		ret = sixaxis_set_operational_bt(hdev);
-		sc->worker_initialized = 1;
-		INIT_WORK(&sc->state_worker, sixaxis_state_worker);
+		sony_init_work(sc, sixaxis_state_worker);
 	} else if (sc->quirks & DUALSHOCK4_CONTROLLER) {
 		if (sc->quirks & DUALSHOCK4_CONTROLLER_BT) {
 			/*
@@ -1661,8 +1891,7 @@ static int sony_probe(struct hid_device *hdev, const struct hid_device_id *id)
 		if (ret < 0)
 			goto err_stop;
 
-		sc->worker_initialized = 1;
-		INIT_WORK(&sc->state_worker, dualshock4_state_worker);
+		sony_init_work(sc, dualshock4_state_worker);
 	} else {
 		ret = 0;
 	}
@@ -1675,7 +1904,7 @@ static int sony_probe(struct hid_device *hdev, const struct hid_device_id *id)
 		goto err_stop;
 
 	if (sc->quirks & SONY_LED_SUPPORT) {
-		ret = sony_leds_init(hdev);
+		ret = sony_leds_init(sc);
 		if (ret < 0)
 			goto err_stop;
 	}
@@ -1694,7 +1923,7 @@ static int sony_probe(struct hid_device *hdev, const struct hid_device_id *id)
 	}
 
 	if (sc->quirks & SONY_FF_SUPPORT) {
-		ret = sony_init_ff(hdev);
+		ret = sony_init_ff(sc);
 		if (ret < 0)
 			goto err_close;
 	}
@@ -1704,12 +1933,12 @@ err_close:
 	hid_hw_close(hdev);
 err_stop:
 	if (sc->quirks & SONY_LED_SUPPORT)
-		sony_leds_remove(hdev);
+		sony_leds_remove(sc);
 	if (sc->quirks & SONY_BATTERY_SUPPORT)
 		sony_battery_remove(sc);
-	if (sc->worker_initialized)
-		cancel_work_sync(&sc->state_worker);
+	sony_cancel_work_sync(sc);
 	sony_remove_dev_list(sc);
+	sony_release_device_id(sc);
 	hid_hw_stop(hdev);
 	return ret;
 }
@@ -1719,18 +1948,19 @@ static void sony_remove(struct hid_device *hdev)
 	struct sony_sc *sc = hid_get_drvdata(hdev);
 
 	if (sc->quirks & SONY_LED_SUPPORT)
-		sony_leds_remove(hdev);
+		sony_leds_remove(sc);
 
 	if (sc->quirks & SONY_BATTERY_SUPPORT) {
 		hid_hw_close(hdev);
 		sony_battery_remove(sc);
 	}
 
-	if (sc->worker_initialized)
-		cancel_work_sync(&sc->state_worker);
+	sony_cancel_work_sync(sc);
 
 	sony_remove_dev_list(sc);
 
+	sony_release_device_id(sc);
+
 	hid_hw_stop(hdev);
 }
 
@@ -1775,6 +2005,22 @@ static struct hid_driver sony_driver = {
 	.report_fixup  = sony_report_fixup,
 	.raw_event     = sony_raw_event
 };
-module_hid_driver(sony_driver);
+
+static int __init sony_init(void)
+{
+	dbg_hid("Sony:%s\n", __func__);
+
+	return hid_register_driver(&sony_driver);
+}
+
+static void __exit sony_exit(void)
+{
+	dbg_hid("Sony:%s\n", __func__);
+
+	ida_destroy(&sony_device_id_allocator);
+	hid_unregister_driver(&sony_driver);
+}
+module_init(sony_init);
+module_exit(sony_exit);
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/hid/hid-thingm.c b/drivers/hid/hid-thingm.c
index a97c78845f7..134be89b15e 100644
--- a/drivers/hid/hid-thingm.c
+++ b/drivers/hid/hid-thingm.c
@@ -1,7 +1,7 @@
 /*
  * ThingM blink(1) USB RGB LED driver
  *
- * Copyright 2013 Savoir-faire Linux Inc.
+ * Copyright 2013-2014 Savoir-faire Linux Inc.
  *	Vivien Didelot <vivien.didelot@savoirfairelinux.com>
  *
  * This program is free software; you can redistribute it and/or
@@ -10,244 +10,285 @@
  */
 
 #include <linux/hid.h>
+#include <linux/hidraw.h>
 #include <linux/leds.h>
 #include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/workqueue.h>
 
 #include "hid-ids.h"
 
-#define BLINK1_CMD_SIZE		9
+#define REPORT_ID	1
+#define REPORT_SIZE	9
 
-#define blink1_rgb_to_r(rgb)	((rgb & 0xFF0000) >> 16)
-#define blink1_rgb_to_g(rgb)	((rgb & 0x00FF00) >> 8)
-#define blink1_rgb_to_b(rgb)	((rgb & 0x0000FF) >> 0)
+/* Firmware major number of supported devices */
+#define THINGM_MAJOR_MK1	'1'
+#define THINGM_MAJOR_MK2	'2'
 
-/**
- * struct blink1_data - blink(1) device specific data
- * @hdev:		HID device.
- * @led_cdev:		LED class instance.
- * @rgb:		8-bit per channel RGB notation.
- * @fade:		fade time in hundredths of a second.
- * @brightness:		brightness coefficient.
- * @play:		play/pause in-memory patterns.
- */
-struct blink1_data {
+struct thingm_fwinfo {
+	char major;
+	unsigned numrgb;
+	unsigned first;
+};
+
+static const struct thingm_fwinfo thingm_fwinfo[] = {
+	{
+		.major = THINGM_MAJOR_MK1,
+		.numrgb = 1,
+		.first = 0,
+	}, {
+		.major = THINGM_MAJOR_MK2,
+		.numrgb = 2,
+		.first = 1,
+	}
+};
+
+/* A red, green or blue channel, part of an RGB chip */
+struct thingm_led {
+	struct thingm_rgb *rgb;
+	struct led_classdev ldev;
+	char name[32];
+};
+
+/* Basically a WS2812 5050 RGB LED chip */
+struct thingm_rgb {
+	struct thingm_device *tdev;
+	struct thingm_led red;
+	struct thingm_led green;
+	struct thingm_led blue;
+	struct work_struct work;
+	u8 num;
+};
+
+struct thingm_device {
 	struct hid_device *hdev;
-	struct led_classdev led_cdev;
-	u32 rgb;
-	u16 fade;
-	u8 brightness;
-	bool play;
+	struct {
+		char major;
+		char minor;
+	} version;
+	const struct thingm_fwinfo *fwinfo;
+	struct mutex lock;
+	struct thingm_rgb *rgb;
 };
 
-static int blink1_send_command(struct blink1_data *data,
-		u8 buf[BLINK1_CMD_SIZE])
+static int thingm_send(struct thingm_device *tdev, u8 buf[REPORT_SIZE])
 {
 	int ret;
 
-	hid_dbg(data->hdev, "command: %d%c%.2x%.2x%.2x%.2x%.2x%.2x%.2x\n",
+	hid_dbg(tdev->hdev, "-> %d %c %02hhx %02hhx %02hhx %02hhx %02hhx %02hhx %02hhx\n",
 			buf[0], buf[1], buf[2], buf[3], buf[4],
 			buf[5], buf[6], buf[7], buf[8]);
 
-	ret = hid_hw_raw_request(data->hdev, buf[0], buf, BLINK1_CMD_SIZE,
-				 HID_FEATURE_REPORT, HID_REQ_SET_REPORT);
+	ret = hid_hw_raw_request(tdev->hdev, buf[0], buf, REPORT_SIZE,
+			HID_FEATURE_REPORT, HID_REQ_SET_REPORT);
 
 	return ret < 0 ? ret : 0;
 }
 
-static int blink1_update_color(struct blink1_data *data)
+static int thingm_recv(struct thingm_device *tdev, u8 buf[REPORT_SIZE])
 {
-	u8 buf[BLINK1_CMD_SIZE] = { 1, 'n', 0, 0, 0, 0, 0, 0, 0 };
-
-	if (data->brightness) {
-		unsigned int coef = DIV_ROUND_CLOSEST(255, data->brightness);
+	int ret;
 
-		buf[2] = DIV_ROUND_CLOSEST(blink1_rgb_to_r(data->rgb), coef);
-		buf[3] = DIV_ROUND_CLOSEST(blink1_rgb_to_g(data->rgb), coef);
-		buf[4] = DIV_ROUND_CLOSEST(blink1_rgb_to_b(data->rgb), coef);
-	}
+	ret = hid_hw_raw_request(tdev->hdev, buf[0], buf, REPORT_SIZE,
+			HID_FEATURE_REPORT, HID_REQ_GET_REPORT);
+	if (ret < 0)
+		return ret;
 
-	if (data->fade) {
-		buf[1] = 'c';
-		buf[5] = (data->fade & 0xFF00) >> 8;
-		buf[6] = (data->fade & 0x00FF);
-	}
+	hid_dbg(tdev->hdev, "<- %d %c %02hhx %02hhx %02hhx %02hhx %02hhx %02hhx %02hhx\n",
+			buf[0], buf[1], buf[2], buf[3], buf[4],
+			buf[5], buf[6], buf[7], buf[8]);
 
-	return blink1_send_command(data, buf);
+	return 0;
 }
 
-static void blink1_led_set(struct led_classdev *led_cdev,
-		enum led_brightness brightness)
+static int thingm_version(struct thingm_device *tdev)
 {
-	struct blink1_data *data = dev_get_drvdata(led_cdev->dev->parent);
+	u8 buf[REPORT_SIZE] = { REPORT_ID, 'v', 0, 0, 0, 0, 0, 0, 0 };
+	int err;
 
-	data->brightness = brightness;
-	if (blink1_update_color(data))
-		hid_err(data->hdev, "failed to update color\n");
-}
+	err = thingm_send(tdev, buf);
+	if (err)
+		return err;
 
-static enum led_brightness blink1_led_get(struct led_classdev *led_cdev)
-{
-	struct blink1_data *data = dev_get_drvdata(led_cdev->dev->parent);
+	err = thingm_recv(tdev, buf);
+	if (err)
+		return err;
 
-	return data->brightness;
+	tdev->version.major = buf[3];
+	tdev->version.minor = buf[4];
+
+	return 0;
 }
 
-static ssize_t blink1_show_rgb(struct device *dev,
-		struct device_attribute *attr, char *buf)
+static int thingm_write_color(struct thingm_rgb *rgb)
 {
-	struct blink1_data *data = dev_get_drvdata(dev->parent);
+	u8 buf[REPORT_SIZE] = { REPORT_ID, 'c', 0, 0, 0, 0, 0, rgb->num, 0 };
 
-	return sprintf(buf, "%.6X\n", data->rgb);
+	buf[2] = rgb->red.ldev.brightness;
+	buf[3] = rgb->green.ldev.brightness;
+	buf[4] = rgb->blue.ldev.brightness;
+
+	return thingm_send(rgb->tdev, buf);
 }
 
-static ssize_t blink1_store_rgb(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t count)
+static void thingm_work(struct work_struct *work)
 {
-	struct blink1_data *data = dev_get_drvdata(dev->parent);
-	long unsigned int rgb;
-	int ret;
+	struct thingm_rgb *rgb = container_of(work, struct thingm_rgb, work);
 
-	ret = kstrtoul(buf, 16, &rgb);
-	if (ret)
-		return ret;
-
-	/* RGB triplet notation is 24-bit hexadecimal */
-	if (rgb > 0xFFFFFF)
-		return -EINVAL;
+	mutex_lock(&rgb->tdev->lock);
 
-	data->rgb = rgb;
-	ret = blink1_update_color(data);
+	if (thingm_write_color(rgb))
+		hid_err(rgb->tdev->hdev, "failed to write color\n");
 
-	return ret ? ret : count;
+	mutex_unlock(&rgb->tdev->lock);
 }
 
-static DEVICE_ATTR(rgb, S_IRUGO | S_IWUSR, blink1_show_rgb, blink1_store_rgb);
-
-static ssize_t blink1_show_fade(struct device *dev,
-		struct device_attribute *attr, char *buf)
+static void thingm_led_set(struct led_classdev *ldev,
+		enum led_brightness brightness)
 {
-	struct blink1_data *data = dev_get_drvdata(dev->parent);
+	struct thingm_led *led = container_of(ldev, struct thingm_led, ldev);
 
-	return sprintf(buf, "%d\n", data->fade * 10);
+	/* the ledclass has already stored the brightness value */
+	schedule_work(&led->rgb->work);
 }
 
-static ssize_t blink1_store_fade(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t count)
+static int thingm_init_rgb(struct thingm_rgb *rgb)
 {
-	struct blink1_data *data = dev_get_drvdata(dev->parent);
-	long unsigned int fade;
-	int ret;
+	const int minor = ((struct hidraw *) rgb->tdev->hdev->hidraw)->minor;
+	int err;
+
+	/* Register the red diode */
+	snprintf(rgb->red.name, sizeof(rgb->red.name),
+			"thingm%d:red:led%d", minor, rgb->num);
+	rgb->red.ldev.name = rgb->red.name;
+	rgb->red.ldev.max_brightness = 255;
+	rgb->red.ldev.brightness_set = thingm_led_set;
+	rgb->red.rgb = rgb;
+
+	err = led_classdev_register(&rgb->tdev->hdev->dev, &rgb->red.ldev);
+	if (err)
+		return err;
+
+	/* Register the green diode */
+	snprintf(rgb->green.name, sizeof(rgb->green.name),
+			"thingm%d:green:led%d", minor, rgb->num);
+	rgb->green.ldev.name = rgb->green.name;
+	rgb->green.ldev.max_brightness = 255;
+	rgb->green.ldev.brightness_set = thingm_led_set;
+	rgb->green.rgb = rgb;
+
+	err = led_classdev_register(&rgb->tdev->hdev->dev, &rgb->green.ldev);
+	if (err)
+		goto unregister_red;
+
+	/* Register the blue diode */
+	snprintf(rgb->blue.name, sizeof(rgb->blue.name),
+			"thingm%d:blue:led%d", minor, rgb->num);
+	rgb->blue.ldev.name = rgb->blue.name;
+	rgb->blue.ldev.max_brightness = 255;
+	rgb->blue.ldev.brightness_set = thingm_led_set;
+	rgb->blue.rgb = rgb;
+
+	err = led_classdev_register(&rgb->tdev->hdev->dev, &rgb->blue.ldev);
+	if (err)
+		goto unregister_green;
+
+	INIT_WORK(&rgb->work, thingm_work);
 
-	ret = kstrtoul(buf, 10, &fade);
-	if (ret)
-		return ret;
+	return 0;
 
-	/* blink(1) accepts 16-bit fade time, number of 10ms ticks */
-	fade = DIV_ROUND_CLOSEST(fade, 10);
-	if (fade > 65535)
-		return -EINVAL;
+unregister_green:
+	led_classdev_unregister(&rgb->green.ldev);
 
-	data->fade = fade;
+unregister_red:
+	led_classdev_unregister(&rgb->red.ldev);
 
-	return count;
+	return err;
 }
 
-static DEVICE_ATTR(fade, S_IRUGO | S_IWUSR,
-		blink1_show_fade, blink1_store_fade);
-
-static ssize_t blink1_show_play(struct device *dev,
-		struct device_attribute *attr, char *buf)
+static void thingm_remove_rgb(struct thingm_rgb *rgb)
 {
-	struct blink1_data *data = dev_get_drvdata(dev->parent);
-
-	return sprintf(buf, "%d\n", data->play);
+	flush_work(&rgb->work);
+	led_classdev_unregister(&rgb->red.ldev);
+	led_classdev_unregister(&rgb->green.ldev);
+	led_classdev_unregister(&rgb->blue.ldev);
 }
 
-static ssize_t blink1_store_play(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t count)
+static int thingm_probe(struct hid_device *hdev, const struct hid_device_id *id)
 {
-	struct blink1_data *data = dev_get_drvdata(dev->parent);
-	u8 cmd[BLINK1_CMD_SIZE] = { 1, 'p', 0, 0, 0, 0, 0, 0, 0 };
-	long unsigned int play;
-	int ret;
+	struct thingm_device *tdev;
+	int i, err;
 
-	ret = kstrtoul(buf, 10, &play);
-	if (ret)
-		return ret;
+	tdev = devm_kzalloc(&hdev->dev, sizeof(struct thingm_device),
+			GFP_KERNEL);
+	if (!tdev)
+		return -ENOMEM;
 
-	data->play = !!play;
-	cmd[2] = data->play;
-	ret = blink1_send_command(data, cmd);
+	tdev->hdev = hdev;
+	hid_set_drvdata(hdev, tdev);
 
-	return ret ? ret : count;
-}
-
-static DEVICE_ATTR(play, S_IRUGO | S_IWUSR,
-		blink1_show_play, blink1_store_play);
+	err = hid_parse(hdev);
+	if (err)
+		goto error;
 
-static const struct attribute_group blink1_sysfs_group = {
-	.attrs = (struct attribute *[]) {
-		&dev_attr_rgb.attr,
-		&dev_attr_fade.attr,
-		&dev_attr_play.attr,
-		NULL
-	},
-};
+	err = hid_hw_start(hdev, HID_CONNECT_HIDRAW);
+	if (err)
+		goto error;
 
-static int thingm_probe(struct hid_device *hdev, const struct hid_device_id *id)
-{
-	struct blink1_data *data;
-	struct led_classdev *led;
-	char led_name[13];
-	int ret;
+	mutex_init(&tdev->lock);
 
-	data = devm_kzalloc(&hdev->dev, sizeof(struct blink1_data), GFP_KERNEL);
-	if (!data)
-		return -ENOMEM;
+	err = thingm_version(tdev);
+	if (err)
+		goto stop;
 
-	hid_set_drvdata(hdev, data);
-	data->hdev = hdev;
-	data->rgb = 0xFFFFFF; /* set a default white color */
+	hid_dbg(hdev, "firmware version: %c.%c\n",
+			tdev->version.major, tdev->version.minor);
 
-	ret = hid_parse(hdev);
-	if (ret)
-		goto error;
+	for (i = 0; i < ARRAY_SIZE(thingm_fwinfo) && !tdev->fwinfo; ++i)
+		if (thingm_fwinfo[i].major == tdev->version.major)
+			tdev->fwinfo = &thingm_fwinfo[i];
 
-	ret = hid_hw_start(hdev, HID_CONNECT_HIDRAW);
-	if (ret)
-		goto error;
+	if (!tdev->fwinfo) {
+		hid_err(hdev, "unsupported firmware %c\n", tdev->version.major);
+		goto stop;
+	}
 
-	/* blink(1) serial numbers range is 0x1A001000 to 0x1A002FFF */
-	led = &data->led_cdev;
-	snprintf(led_name, sizeof(led_name), "blink1::%s", hdev->uniq + 4);
-	led->name = led_name;
-	led->brightness_set = blink1_led_set;
-	led->brightness_get = blink1_led_get;
-	ret = led_classdev_register(&hdev->dev, led);
-	if (ret)
+	tdev->rgb = devm_kzalloc(&hdev->dev,
+			sizeof(struct thingm_rgb) * tdev->fwinfo->numrgb,
+			GFP_KERNEL);
+	if (!tdev->rgb) {
+		err = -ENOMEM;
 		goto stop;
+	}
 
-	ret = sysfs_create_group(&led->dev->kobj, &blink1_sysfs_group);
-	if (ret)
-		goto remove_led;
+	for (i = 0; i < tdev->fwinfo->numrgb; ++i) {
+		struct thingm_rgb *rgb = tdev->rgb + i;
+
+		rgb->tdev = tdev;
+		rgb->num = tdev->fwinfo->first + i;
+		err = thingm_init_rgb(rgb);
+		if (err) {
+			while (--i >= 0)
+				thingm_remove_rgb(tdev->rgb + i);
+			goto stop;
+		}
+	}
 
 	return 0;
-
-remove_led:
-	led_classdev_unregister(led);
 stop:
 	hid_hw_stop(hdev);
 error:
-	return ret;
+	return err;
 }
 
 static void thingm_remove(struct hid_device *hdev)
 {
-	struct blink1_data *data = hid_get_drvdata(hdev);
-	struct led_classdev *led = &data->led_cdev;
+	struct thingm_device *tdev = hid_get_drvdata(hdev);
+	int i;
+
+	for (i = 0; i < tdev->fwinfo->numrgb; ++i)
+		thingm_remove_rgb(tdev->rgb + i);
 
-	sysfs_remove_group(&led->dev->kobj, &blink1_sysfs_group);
-	led_classdev_unregister(led);
 	hid_hw_stop(hdev);
 }
 
diff --git a/drivers/hid/i2c-hid/i2c-hid.c b/drivers/hid/i2c-hid/i2c-hid.c
index b50860db92f..21aafc8f48c 100644
--- a/drivers/hid/i2c-hid/i2c-hid.c
+++ b/drivers/hid/i2c-hid/i2c-hid.c
@@ -807,34 +807,18 @@ static int i2c_hid_fetch_hid_descriptor(struct i2c_hid *ihid)
 	unsigned int dsize;
 	int ret;
 
-	/* Fetch the length of HID description, retrieve the 4 first bytes:
-	 * bytes 0-1 -> length
-	 * bytes 2-3 -> bcdVersion (has to be 1.00) */
-	ret = i2c_hid_command(client, &hid_descr_cmd, ihid->hdesc_buffer, 4);
-
-	i2c_hid_dbg(ihid, "%s, ihid->hdesc_buffer: %4ph\n", __func__,
-			ihid->hdesc_buffer);
-
+	/* i2c hid fetch using a fixed descriptor size (30 bytes) */
+	i2c_hid_dbg(ihid, "Fetching the HID descriptor\n");
+	ret = i2c_hid_command(client, &hid_descr_cmd, ihid->hdesc_buffer,
+				sizeof(struct i2c_hid_desc));
 	if (ret) {
-		dev_err(&client->dev,
-			"unable to fetch the size of HID descriptor (ret=%d)\n",
-			ret);
-		return -ENODEV;
-	}
-
-	dsize = le16_to_cpu(hdesc->wHIDDescLength);
-	/*
-	 * the size of the HID descriptor should at least contain
-	 * its size and the bcdVersion (4 bytes), and should not be greater
-	 * than sizeof(struct i2c_hid_desc) as we directly fill this struct
-	 * through i2c_hid_command.
-	 */
-	if (dsize < 4 || dsize > sizeof(struct i2c_hid_desc)) {
-		dev_err(&client->dev, "weird size of HID descriptor (%u)\n",
-			dsize);
+		dev_err(&client->dev, "hid_descr_cmd failed\n");
 		return -ENODEV;
 	}
 
+	/* Validate the length of HID descriptor, the 4 first bytes:
+	 * bytes 0-1 -> length
+	 * bytes 2-3 -> bcdVersion (has to be 1.00) */
 	/* check bcdVersion == 1.0 */
 	if (le16_to_cpu(hdesc->bcdVersion) != 0x0100) {
 		dev_err(&client->dev,
@@ -843,17 +827,14 @@ static int i2c_hid_fetch_hid_descriptor(struct i2c_hid *ihid)
 		return -ENODEV;
 	}
 
-	i2c_hid_dbg(ihid, "Fetching the HID descriptor\n");
-
-	ret = i2c_hid_command(client, &hid_descr_cmd, ihid->hdesc_buffer,
-				dsize);
-	if (ret) {
-		dev_err(&client->dev, "hid_descr_cmd Fail\n");
+	/* Descriptor length should be 30 bytes as per the specification */
+	dsize = le16_to_cpu(hdesc->wHIDDescLength);
+	if (dsize != sizeof(struct i2c_hid_desc)) {
+		dev_err(&client->dev, "weird size of HID descriptor (%u)\n",
+			dsize);
 		return -ENODEV;
 	}
-
 	i2c_hid_dbg(ihid, "HID Descriptor: %*ph\n", dsize, ihid->hdesc_buffer);
-
 	return 0;
 }
 
diff --git a/drivers/hid/uhid.c b/drivers/hid/uhid.c
index 0d078c32db4..0cb92e34725 100644
--- a/drivers/hid/uhid.c
+++ b/drivers/hid/uhid.c
@@ -441,12 +441,11 @@ static int uhid_dev_create2(struct uhid_device *uhid,
 	if (uhid->rd_size <= 0 || uhid->rd_size > HID_MAX_DESCRIPTOR_SIZE)
 		return -EINVAL;
 
-	uhid->rd_data = kmalloc(uhid->rd_size, GFP_KERNEL);
+	uhid->rd_data = kmemdup(ev->u.create2.rd_data, uhid->rd_size,
+				GFP_KERNEL);
 	if (!uhid->rd_data)
 		return -ENOMEM;
 
-	memcpy(uhid->rd_data, ev->u.create2.rd_data, uhid->rd_size);
-
 	hid = hid_allocate_device();
 	if (IS_ERR(hid)) {
 		ret = PTR_ERR(hid);
diff --git a/drivers/hid/usbhid/hid-quirks.c b/drivers/hid/usbhid/hid-quirks.c
index 8e4ddb36988..59badc10a08 100644
--- a/drivers/hid/usbhid/hid-quirks.c
+++ b/drivers/hid/usbhid/hid-quirks.c
@@ -115,6 +115,7 @@ static const struct hid_blacklist {
 	{ USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_MOUSEPEN_I608X, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_EASYPEN_M610X, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_NTRIG, USB_DEVICE_ID_NTRIG_DUOSENSE, HID_QUIRK_NO_INIT_REPORTS },
+	{ USB_VENDOR_ID_SEMICO, USB_DEVICE_ID_SEMICO_USB_KEYKOARD, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_SYNAPTICS, USB_DEVICE_ID_SYNAPTICS_LTS1, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_SYNAPTICS, USB_DEVICE_ID_SYNAPTICS_LTS2, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_SYNAPTICS, USB_DEVICE_ID_SYNAPTICS_HD, HID_QUIRK_NO_INIT_REPORTS },
diff --git a/drivers/hsi/Kconfig b/drivers/hsi/Kconfig
index d94e38dd80c..2c76de438eb 100644
--- a/drivers/hsi/Kconfig
+++ b/drivers/hsi/Kconfig
@@ -14,6 +14,7 @@ config HSI_BOARDINFO
 	bool
 	default y
 
+source "drivers/hsi/controllers/Kconfig"
 source "drivers/hsi/clients/Kconfig"
 
 endif # HSI
diff --git a/drivers/hsi/Makefile b/drivers/hsi/Makefile
index 9d5d33f90de..360371e134f 100644
--- a/drivers/hsi/Makefile
+++ b/drivers/hsi/Makefile
@@ -3,4 +3,5 @@
 #
 obj-$(CONFIG_HSI_BOARDINFO)	+= hsi_boardinfo.o
 obj-$(CONFIG_HSI)		+= hsi.o
+obj-y				+= controllers/
 obj-y				+= clients/
diff --git a/drivers/hsi/clients/Kconfig b/drivers/hsi/clients/Kconfig
index 3bacd275f47..71b9f9ab86e 100644
--- a/drivers/hsi/clients/Kconfig
+++ b/drivers/hsi/clients/Kconfig
@@ -4,6 +4,23 @@
 
 comment "HSI clients"
 
+config NOKIA_MODEM
+	tristate "Nokia Modem"
+	depends on HSI && SSI_PROTOCOL
+	help
+	Say Y here if you want to add support for the modem on Nokia
+	N900 (Nokia RX-51) hardware.
+
+	If unsure, say N.
+
+config SSI_PROTOCOL
+	tristate "SSI protocol"
+	depends on HSI && PHONET && (OMAP_SSI=y || OMAP_SSI=m)
+	help
+	If you say Y here, you will enable the SSI protocol aka McSAAB.
+
+	If unsure, say N.
+
 config HSI_CHAR
 	tristate "HSI/SSI character driver"
 	depends on HSI
diff --git a/drivers/hsi/clients/Makefile b/drivers/hsi/clients/Makefile
index 327c0e27c8b..4d5bc0e0b27 100644
--- a/drivers/hsi/clients/Makefile
+++ b/drivers/hsi/clients/Makefile
@@ -2,4 +2,6 @@
 # Makefile for HSI clients
 #
 
-obj-$(CONFIG_HSI_CHAR)	+= hsi_char.o
+obj-$(CONFIG_NOKIA_MODEM)	+= nokia-modem.o
+obj-$(CONFIG_SSI_PROTOCOL)	+= ssi_protocol.o
+obj-$(CONFIG_HSI_CHAR)		+= hsi_char.o
diff --git a/drivers/hsi/clients/hsi_char.c b/drivers/hsi/clients/hsi_char.c
index e61e5f991aa..57f70c28fa3 100644
--- a/drivers/hsi/clients/hsi_char.c
+++ b/drivers/hsi/clients/hsi_char.c
@@ -367,7 +367,7 @@ static int hsc_rx_set(struct hsi_client *cl, struct hsc_rx_config *rxc)
 		return -EINVAL;
 	tmp = cl->rx_cfg;
 	cl->rx_cfg.mode = rxc->mode;
-	cl->rx_cfg.channels = rxc->channels;
+	cl->rx_cfg.num_hw_channels = rxc->channels;
 	cl->rx_cfg.flow = rxc->flow;
 	ret = hsi_setup(cl);
 	if (ret < 0) {
@@ -383,7 +383,7 @@ static int hsc_rx_set(struct hsi_client *cl, struct hsc_rx_config *rxc)
 static inline void hsc_rx_get(struct hsi_client *cl, struct hsc_rx_config *rxc)
 {
 	rxc->mode = cl->rx_cfg.mode;
-	rxc->channels = cl->rx_cfg.channels;
+	rxc->channels = cl->rx_cfg.num_hw_channels;
 	rxc->flow = cl->rx_cfg.flow;
 }
 
@@ -402,7 +402,7 @@ static int hsc_tx_set(struct hsi_client *cl, struct hsc_tx_config *txc)
 		return -EINVAL;
 	tmp = cl->tx_cfg;
 	cl->tx_cfg.mode = txc->mode;
-	cl->tx_cfg.channels = txc->channels;
+	cl->tx_cfg.num_hw_channels = txc->channels;
 	cl->tx_cfg.speed = txc->speed;
 	cl->tx_cfg.arb_mode = txc->arb_mode;
 	ret = hsi_setup(cl);
@@ -417,7 +417,7 @@ static int hsc_tx_set(struct hsi_client *cl, struct hsc_tx_config *txc)
 static inline void hsc_tx_get(struct hsi_client *cl, struct hsc_tx_config *txc)
 {
 	txc->mode = cl->tx_cfg.mode;
-	txc->channels = cl->tx_cfg.channels;
+	txc->channels = cl->tx_cfg.num_hw_channels;
 	txc->speed = cl->tx_cfg.speed;
 	txc->arb_mode = cl->tx_cfg.arb_mode;
 }
@@ -435,7 +435,7 @@ static ssize_t hsc_read(struct file *file, char __user *buf, size_t len,
 		return -EINVAL;
 	if (len > max_data_size)
 		len = max_data_size;
-	if (channel->ch >= channel->cl->rx_cfg.channels)
+	if (channel->ch >= channel->cl->rx_cfg.num_hw_channels)
 		return -ECHRNG;
 	if (test_and_set_bit(HSC_CH_READ, &channel->flags))
 		return -EBUSY;
@@ -492,7 +492,7 @@ static ssize_t hsc_write(struct file *file, const char __user *buf, size_t len,
 		return -EINVAL;
 	if (len > max_data_size)
 		len = max_data_size;
-	if (channel->ch >= channel->cl->tx_cfg.channels)
+	if (channel->ch >= channel->cl->tx_cfg.num_hw_channels)
 		return -ECHRNG;
 	if (test_and_set_bit(HSC_CH_WRITE, &channel->flags))
 		return -EBUSY;
@@ -705,7 +705,7 @@ static int hsc_probe(struct device *dev)
 	if (!hsc_major) {
 		ret = alloc_chrdev_region(&hsc_dev, hsc_baseminor,
 						HSC_DEVS, devname);
-		if (ret > 0)
+		if (ret == 0)
 			hsc_major = MAJOR(hsc_dev);
 	} else {
 		hsc_dev = MKDEV(hsc_major, hsc_baseminor);
diff --git a/drivers/hsi/clients/nokia-modem.c b/drivers/hsi/clients/nokia-modem.c
new file mode 100644
index 00000000000..363b780dace
--- /dev/null
+++ b/drivers/hsi/clients/nokia-modem.c
@@ -0,0 +1,285 @@
+/*
+ * nokia-modem.c
+ *
+ * HSI client driver for Nokia N900 modem.
+ *
+ * Copyright (C) 2014 Sebastian Reichel <sre@kernel.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.
+ *
+ * You should have 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/gpio/consumer.h>
+#include <linux/hsi/hsi.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/of_gpio.h>
+#include <linux/hsi/ssi_protocol.h>
+
+static unsigned int pm;
+module_param(pm, int, 0400);
+MODULE_PARM_DESC(pm,
+	"Enable power management (0=disabled, 1=userland based [default])");
+
+struct nokia_modem_gpio {
+	struct gpio_desc	*gpio;
+	const char		*name;
+};
+
+struct nokia_modem_device {
+	struct tasklet_struct	nokia_modem_rst_ind_tasklet;
+	int			nokia_modem_rst_ind_irq;
+	struct device		*device;
+	struct nokia_modem_gpio	*gpios;
+	int			gpio_amount;
+	struct hsi_client	*ssi_protocol;
+};
+
+static void do_nokia_modem_rst_ind_tasklet(unsigned long data)
+{
+	struct nokia_modem_device *modem = (struct nokia_modem_device *)data;
+
+	if (!modem)
+		return;
+
+	dev_info(modem->device, "CMT rst line change detected\n");
+
+	if (modem->ssi_protocol)
+		ssip_reset_event(modem->ssi_protocol);
+}
+
+static irqreturn_t nokia_modem_rst_ind_isr(int irq, void *data)
+{
+	struct nokia_modem_device *modem = (struct nokia_modem_device *)data;
+
+	tasklet_schedule(&modem->nokia_modem_rst_ind_tasklet);
+
+	return IRQ_HANDLED;
+}
+
+static void nokia_modem_gpio_unexport(struct device *dev)
+{
+	struct nokia_modem_device *modem = dev_get_drvdata(dev);
+	int i;
+
+	for (i = 0; i < modem->gpio_amount; i++) {
+		sysfs_remove_link(&dev->kobj, modem->gpios[i].name);
+		gpiod_unexport(modem->gpios[i].gpio);
+	}
+}
+
+static int nokia_modem_gpio_probe(struct device *dev)
+{
+	struct device_node *np = dev->of_node;
+	struct nokia_modem_device *modem = dev_get_drvdata(dev);
+	int gpio_count, gpio_name_count, i, err;
+
+	gpio_count = of_gpio_count(np);
+
+	if (gpio_count < 0) {
+		dev_err(dev, "missing gpios: %d\n", gpio_count);
+		return gpio_count;
+	}
+
+	gpio_name_count = of_property_count_strings(np, "gpio-names");
+
+	if (gpio_count != gpio_name_count) {
+		dev_err(dev, "number of gpios does not equal number of gpio names\n");
+		return -EINVAL;
+	}
+
+	modem->gpios = devm_kzalloc(dev, gpio_count *
+				sizeof(struct nokia_modem_gpio), GFP_KERNEL);
+	if (!modem->gpios) {
+		dev_err(dev, "Could not allocate memory for gpios\n");
+		return -ENOMEM;
+	}
+
+	modem->gpio_amount = gpio_count;
+
+	for (i = 0; i < gpio_count; i++) {
+		modem->gpios[i].gpio = devm_gpiod_get_index(dev, NULL, i);
+		if (IS_ERR(modem->gpios[i].gpio)) {
+			dev_err(dev, "Could not get gpio %d\n", i);
+			return PTR_ERR(modem->gpios[i].gpio);
+		}
+
+		err = of_property_read_string_index(np, "gpio-names", i,
+						&(modem->gpios[i].name));
+		if (err) {
+			dev_err(dev, "Could not get gpio name %d\n", i);
+			return err;
+		}
+
+		err = gpiod_direction_output(modem->gpios[i].gpio, 0);
+		if (err)
+			return err;
+
+		err = gpiod_export(modem->gpios[i].gpio, 0);
+		if (err)
+			return err;
+
+		err = gpiod_export_link(dev, modem->gpios[i].name,
+							modem->gpios[i].gpio);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+static int nokia_modem_probe(struct device *dev)
+{
+	struct device_node *np;
+	struct nokia_modem_device *modem;
+	struct hsi_client *cl = to_hsi_client(dev);
+	struct hsi_port *port = hsi_get_port(cl);
+	int irq, pflags, err;
+	struct hsi_board_info ssip;
+
+	np = dev->of_node;
+	if (!np) {
+		dev_err(dev, "device tree node not found\n");
+		return -ENXIO;
+	}
+
+	modem = devm_kzalloc(dev, sizeof(*modem), GFP_KERNEL);
+	if (!modem) {
+		dev_err(dev, "Could not allocate memory for nokia_modem_device\n");
+		return -ENOMEM;
+	}
+	dev_set_drvdata(dev, modem);
+
+	irq = irq_of_parse_and_map(np, 0);
+	if (irq < 0) {
+		dev_err(dev, "Invalid rst_ind interrupt (%d)\n", irq);
+		return irq;
+	}
+	modem->nokia_modem_rst_ind_irq = irq;
+	pflags = irq_get_trigger_type(irq);
+
+	tasklet_init(&modem->nokia_modem_rst_ind_tasklet,
+			do_nokia_modem_rst_ind_tasklet, (unsigned long)modem);
+	err = devm_request_irq(dev, irq, nokia_modem_rst_ind_isr,
+				IRQF_DISABLED | pflags, "modem_rst_ind", modem);
+	if (err < 0) {
+		dev_err(dev, "Request rst_ind irq(%d) failed (flags %d)\n",
+								irq, pflags);
+		return err;
+	}
+	enable_irq_wake(irq);
+
+	if(pm) {
+		err = nokia_modem_gpio_probe(dev);
+		if (err < 0) {
+			dev_err(dev, "Could not probe GPIOs\n");
+			goto error1;
+		}
+	}
+
+	ssip.name = "ssi-protocol";
+	ssip.tx_cfg = cl->tx_cfg;
+	ssip.rx_cfg = cl->rx_cfg;
+	ssip.platform_data = NULL;
+	ssip.archdata = NULL;
+
+	modem->ssi_protocol = hsi_new_client(port, &ssip);
+	if (!modem->ssi_protocol) {
+		dev_err(dev, "Could not register ssi-protocol device\n");
+		goto error2;
+	}
+
+	err = device_attach(&modem->ssi_protocol->device);
+	if (err == 0) {
+		dev_err(dev, "Missing ssi-protocol driver\n");
+		err = -EPROBE_DEFER;
+		goto error3;
+	} else if (err < 0) {
+		dev_err(dev, "Could not load ssi-protocol driver (%d)\n", err);
+		goto error3;
+	}
+
+	/* TODO: register cmt-speech hsi client */
+
+	dev_info(dev, "Registered Nokia HSI modem\n");
+
+	return 0;
+
+error3:
+	hsi_remove_client(&modem->ssi_protocol->device, NULL);
+error2:
+	nokia_modem_gpio_unexport(dev);
+error1:
+	disable_irq_wake(modem->nokia_modem_rst_ind_irq);
+	tasklet_kill(&modem->nokia_modem_rst_ind_tasklet);
+
+	return err;
+}
+
+static int nokia_modem_remove(struct device *dev)
+{
+	struct nokia_modem_device *modem = dev_get_drvdata(dev);
+
+	if (!modem)
+		return 0;
+
+	if (modem->ssi_protocol) {
+		hsi_remove_client(&modem->ssi_protocol->device, NULL);
+		modem->ssi_protocol = NULL;
+	}
+
+	nokia_modem_gpio_unexport(dev);
+	dev_set_drvdata(dev, NULL);
+	disable_irq_wake(modem->nokia_modem_rst_ind_irq);
+	tasklet_kill(&modem->nokia_modem_rst_ind_tasklet);
+
+	return 0;
+}
+
+#ifdef CONFIG_OF
+static const struct of_device_id nokia_modem_of_match[] = {
+	{ .compatible = "nokia,n900-modem", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, nokia_modem_of_match);
+#endif
+
+static struct hsi_client_driver nokia_modem_driver = {
+	.driver = {
+		.name	= "nokia-modem",
+		.owner	= THIS_MODULE,
+		.probe	= nokia_modem_probe,
+		.remove	= nokia_modem_remove,
+		.of_match_table = of_match_ptr(nokia_modem_of_match),
+	},
+};
+
+static int __init nokia_modem_init(void)
+{
+	return hsi_register_client_driver(&nokia_modem_driver);
+}
+module_init(nokia_modem_init);
+
+static void __exit nokia_modem_exit(void)
+{
+	hsi_unregister_client_driver(&nokia_modem_driver);
+}
+module_exit(nokia_modem_exit);
+
+MODULE_ALIAS("hsi:nokia-modem");
+MODULE_AUTHOR("Sebastian Reichel <sre@kernel.org>");
+MODULE_DESCRIPTION("HSI driver module for Nokia N900 Modem");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hsi/clients/ssi_protocol.c b/drivers/hsi/clients/ssi_protocol.c
new file mode 100644
index 00000000000..ce4be3738d4
--- /dev/null
+++ b/drivers/hsi/clients/ssi_protocol.c
@@ -0,0 +1,1191 @@
+/*
+ * ssi_protocol.c
+ *
+ * Implementation of the SSI McSAAB improved protocol.
+ *
+ * Copyright (C) 2010 Nokia Corporation. All rights reserved.
+ * Copyright (C) 2013 Sebastian Reichel <sre@kernel.org>
+ *
+ * Contact: Carlos Chinea <carlos.chinea@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/atomic.h>
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/if_ether.h>
+#include <linux/if_arp.h>
+#include <linux/if_phonet.h>
+#include <linux/init.h>
+#include <linux/irq.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/notifier.h>
+#include <linux/scatterlist.h>
+#include <linux/skbuff.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/timer.h>
+#include <linux/hsi/hsi.h>
+#include <linux/hsi/ssi_protocol.h>
+
+void ssi_waketest(struct hsi_client *cl, unsigned int enable);
+
+#define SSIP_TXQUEUE_LEN	100
+#define SSIP_MAX_MTU		65535
+#define SSIP_DEFAULT_MTU	4000
+#define PN_MEDIA_SOS		21
+#define SSIP_MIN_PN_HDR		6	/* FIXME: Revisit */
+#define SSIP_WDTOUT		2000	/* FIXME: has to be 500 msecs */
+#define SSIP_KATOUT		15	/* 15 msecs */
+#define SSIP_MAX_CMDS		5 /* Number of pre-allocated commands buffers */
+#define SSIP_BYTES_TO_FRAMES(x) ((((x) - 1) >> 2) + 1)
+#define SSIP_CMT_LOADER_SYNC	0x11223344
+/*
+ * SSI protocol command definitions
+ */
+#define SSIP_COMMAND(data)	((data) >> 28)
+#define SSIP_PAYLOAD(data)	((data) & 0xfffffff)
+/* Commands */
+#define SSIP_SW_BREAK		0
+#define SSIP_BOOTINFO_REQ	1
+#define SSIP_BOOTINFO_RESP	2
+#define SSIP_WAKETEST_RESULT	3
+#define SSIP_START_TRANS	4
+#define SSIP_READY		5
+/* Payloads */
+#define SSIP_DATA_VERSION(data)	((data) & 0xff)
+#define SSIP_LOCAL_VERID	1
+#define SSIP_WAKETEST_OK	0
+#define SSIP_WAKETEST_FAILED	1
+#define SSIP_PDU_LENGTH(data)	(((data) >> 8) & 0xffff)
+#define SSIP_MSG_ID(data)	((data) & 0xff)
+/* Generic Command */
+#define SSIP_CMD(cmd, payload)	(((cmd) << 28) | ((payload) & 0xfffffff))
+/* Commands for the control channel */
+#define SSIP_BOOTINFO_REQ_CMD(ver) \
+		SSIP_CMD(SSIP_BOOTINFO_REQ, SSIP_DATA_VERSION(ver))
+#define SSIP_BOOTINFO_RESP_CMD(ver) \
+		SSIP_CMD(SSIP_BOOTINFO_RESP, SSIP_DATA_VERSION(ver))
+#define SSIP_START_TRANS_CMD(pdulen, id) \
+		SSIP_CMD(SSIP_START_TRANS, (((pdulen) << 8) | SSIP_MSG_ID(id)))
+#define SSIP_READY_CMD		SSIP_CMD(SSIP_READY, 0)
+#define SSIP_SWBREAK_CMD	SSIP_CMD(SSIP_SW_BREAK, 0)
+
+/* Main state machine states */
+enum {
+	INIT,
+	HANDSHAKE,
+	ACTIVE,
+};
+
+/* Send state machine states */
+enum {
+	SEND_IDLE,
+	WAIT4READY,
+	SEND_READY,
+	SENDING,
+	SENDING_SWBREAK,
+};
+
+/* Receive state machine states */
+enum {
+	RECV_IDLE,
+	RECV_READY,
+	RECEIVING,
+};
+
+/**
+ * struct ssi_protocol - SSI protocol (McSAAB) data
+ * @main_state: Main state machine
+ * @send_state: TX state machine
+ * @recv_state: RX state machine
+ * @waketest: Flag to follow wake line test
+ * @rxid: RX data id
+ * @txid: TX data id
+ * @txqueue_len: TX queue length
+ * @tx_wd: TX watchdog
+ * @rx_wd: RX watchdog
+ * @keep_alive: Workaround for SSI HW bug
+ * @lock: To serialize access to this struct
+ * @netdev: Phonet network device
+ * @txqueue: TX data queue
+ * @cmdqueue: Queue of free commands
+ * @cl: HSI client own reference
+ * @link: Link for ssip_list
+ * @tx_usecount: Refcount to keep track the slaves that use the wake line
+ * @channel_id_cmd: HSI channel id for command stream
+ * @channel_id_data: HSI channel id for data stream
+ */
+struct ssi_protocol {
+	unsigned int		main_state;
+	unsigned int		send_state;
+	unsigned int		recv_state;
+	unsigned int		waketest:1;
+	u8			rxid;
+	u8			txid;
+	unsigned int		txqueue_len;
+	struct timer_list	tx_wd;
+	struct timer_list	rx_wd;
+	struct timer_list	keep_alive; /* wake-up workaround */
+	spinlock_t		lock;
+	struct net_device	*netdev;
+	struct list_head	txqueue;
+	struct list_head	cmdqueue;
+	struct hsi_client	*cl;
+	struct list_head	link;
+	atomic_t		tx_usecnt;
+	int			channel_id_cmd;
+	int			channel_id_data;
+};
+
+/* List of ssi protocol instances */
+static LIST_HEAD(ssip_list);
+
+static void ssip_rxcmd_complete(struct hsi_msg *msg);
+
+static inline void ssip_set_cmd(struct hsi_msg *msg, u32 cmd)
+{
+	u32 *data;
+
+	data = sg_virt(msg->sgt.sgl);
+	*data = cmd;
+}
+
+static inline u32 ssip_get_cmd(struct hsi_msg *msg)
+{
+	u32 *data;
+
+	data = sg_virt(msg->sgt.sgl);
+
+	return *data;
+}
+
+static void ssip_skb_to_msg(struct sk_buff *skb, struct hsi_msg *msg)
+{
+	skb_frag_t *frag;
+	struct scatterlist *sg;
+	int i;
+
+	BUG_ON(msg->sgt.nents != (unsigned int)(skb_shinfo(skb)->nr_frags + 1));
+
+	sg = msg->sgt.sgl;
+	sg_set_buf(sg, skb->data, skb_headlen(skb));
+	for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
+		sg = sg_next(sg);
+		BUG_ON(!sg);
+		frag = &skb_shinfo(skb)->frags[i];
+		sg_set_page(sg, frag->page.p, frag->size, frag->page_offset);
+	}
+}
+
+static void ssip_free_data(struct hsi_msg *msg)
+{
+	struct sk_buff *skb;
+
+	skb = msg->context;
+	pr_debug("free data: msg %p context %p skb %p\n", msg, msg->context,
+								skb);
+	msg->destructor = NULL;
+	dev_kfree_skb(skb);
+	hsi_free_msg(msg);
+}
+
+static struct hsi_msg *ssip_alloc_data(struct ssi_protocol *ssi,
+					struct sk_buff *skb, gfp_t flags)
+{
+	struct hsi_msg *msg;
+
+	msg = hsi_alloc_msg(skb_shinfo(skb)->nr_frags + 1, flags);
+	if (!msg)
+		return NULL;
+	ssip_skb_to_msg(skb, msg);
+	msg->destructor = ssip_free_data;
+	msg->channel = ssi->channel_id_data;
+	msg->context = skb;
+
+	return msg;
+}
+
+static inline void ssip_release_cmd(struct hsi_msg *msg)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(msg->cl);
+
+	dev_dbg(&msg->cl->device, "Release cmd 0x%08x\n", ssip_get_cmd(msg));
+	spin_lock_bh(&ssi->lock);
+	list_add_tail(&msg->link, &ssi->cmdqueue);
+	spin_unlock_bh(&ssi->lock);
+}
+
+static struct hsi_msg *ssip_claim_cmd(struct ssi_protocol *ssi)
+{
+	struct hsi_msg *msg;
+
+	BUG_ON(list_empty(&ssi->cmdqueue));
+
+	spin_lock_bh(&ssi->lock);
+	msg = list_first_entry(&ssi->cmdqueue, struct hsi_msg, link);
+	list_del(&msg->link);
+	spin_unlock_bh(&ssi->lock);
+	msg->destructor = ssip_release_cmd;
+
+	return msg;
+}
+
+static void ssip_free_cmds(struct ssi_protocol *ssi)
+{
+	struct hsi_msg *msg, *tmp;
+
+	list_for_each_entry_safe(msg, tmp, &ssi->cmdqueue, link) {
+		list_del(&msg->link);
+		msg->destructor = NULL;
+		kfree(sg_virt(msg->sgt.sgl));
+		hsi_free_msg(msg);
+	}
+}
+
+static int ssip_alloc_cmds(struct ssi_protocol *ssi)
+{
+	struct hsi_msg *msg;
+	u32 *buf;
+	unsigned int i;
+
+	for (i = 0; i < SSIP_MAX_CMDS; i++) {
+		msg = hsi_alloc_msg(1, GFP_KERNEL);
+		if (!msg)
+			goto out;
+		buf = kmalloc(sizeof(*buf), GFP_KERNEL);
+		if (!buf) {
+			hsi_free_msg(msg);
+			goto out;
+		}
+		sg_init_one(msg->sgt.sgl, buf, sizeof(*buf));
+		msg->channel = ssi->channel_id_cmd;
+		list_add_tail(&msg->link, &ssi->cmdqueue);
+	}
+
+	return 0;
+out:
+	ssip_free_cmds(ssi);
+
+	return -ENOMEM;
+}
+
+static void ssip_set_rxstate(struct ssi_protocol *ssi, unsigned int state)
+{
+	ssi->recv_state = state;
+	switch (state) {
+	case RECV_IDLE:
+		del_timer(&ssi->rx_wd);
+		if (ssi->send_state == SEND_IDLE)
+			del_timer(&ssi->keep_alive);
+		break;
+	case RECV_READY:
+		/* CMT speech workaround */
+		if (atomic_read(&ssi->tx_usecnt))
+			break;
+		/* Otherwise fall through */
+	case RECEIVING:
+		mod_timer(&ssi->keep_alive, jiffies +
+						msecs_to_jiffies(SSIP_KATOUT));
+		mod_timer(&ssi->rx_wd, jiffies + msecs_to_jiffies(SSIP_WDTOUT));
+		break;
+	default:
+		break;
+	}
+}
+
+static void ssip_set_txstate(struct ssi_protocol *ssi, unsigned int state)
+{
+	ssi->send_state = state;
+	switch (state) {
+	case SEND_IDLE:
+	case SEND_READY:
+		del_timer(&ssi->tx_wd);
+		if (ssi->recv_state == RECV_IDLE)
+			del_timer(&ssi->keep_alive);
+		break;
+	case WAIT4READY:
+	case SENDING:
+	case SENDING_SWBREAK:
+		mod_timer(&ssi->keep_alive,
+				jiffies + msecs_to_jiffies(SSIP_KATOUT));
+		mod_timer(&ssi->tx_wd, jiffies + msecs_to_jiffies(SSIP_WDTOUT));
+		break;
+	default:
+		break;
+	}
+}
+
+struct hsi_client *ssip_slave_get_master(struct hsi_client *slave)
+{
+	struct hsi_client *master = ERR_PTR(-ENODEV);
+	struct ssi_protocol *ssi;
+
+	list_for_each_entry(ssi, &ssip_list, link)
+		if (slave->device.parent == ssi->cl->device.parent) {
+			master = ssi->cl;
+			break;
+		}
+
+	return master;
+}
+EXPORT_SYMBOL_GPL(ssip_slave_get_master);
+
+int ssip_slave_start_tx(struct hsi_client *master)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(master);
+
+	dev_dbg(&master->device, "start TX %d\n", atomic_read(&ssi->tx_usecnt));
+	spin_lock_bh(&ssi->lock);
+	if (ssi->send_state == SEND_IDLE) {
+		ssip_set_txstate(ssi, WAIT4READY);
+		hsi_start_tx(master);
+	}
+	spin_unlock_bh(&ssi->lock);
+	atomic_inc(&ssi->tx_usecnt);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ssip_slave_start_tx);
+
+int ssip_slave_stop_tx(struct hsi_client *master)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(master);
+
+	WARN_ON_ONCE(atomic_read(&ssi->tx_usecnt) == 0);
+
+	if (atomic_dec_and_test(&ssi->tx_usecnt)) {
+		spin_lock_bh(&ssi->lock);
+		if ((ssi->send_state == SEND_READY) ||
+			(ssi->send_state == WAIT4READY)) {
+			ssip_set_txstate(ssi, SEND_IDLE);
+			hsi_stop_tx(master);
+		}
+		spin_unlock_bh(&ssi->lock);
+	}
+	dev_dbg(&master->device, "stop TX %d\n", atomic_read(&ssi->tx_usecnt));
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ssip_slave_stop_tx);
+
+int ssip_slave_running(struct hsi_client *master)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(master);
+	return netif_running(ssi->netdev);
+}
+EXPORT_SYMBOL_GPL(ssip_slave_running);
+
+static void ssip_reset(struct hsi_client *cl)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct list_head *head, *tmp;
+	struct hsi_msg *msg;
+
+	if (netif_running(ssi->netdev))
+		netif_carrier_off(ssi->netdev);
+	hsi_flush(cl);
+	spin_lock_bh(&ssi->lock);
+	if (ssi->send_state != SEND_IDLE)
+		hsi_stop_tx(cl);
+	if (ssi->waketest)
+		ssi_waketest(cl, 0);
+	del_timer(&ssi->rx_wd);
+	del_timer(&ssi->tx_wd);
+	del_timer(&ssi->keep_alive);
+	ssi->main_state = 0;
+	ssi->send_state = 0;
+	ssi->recv_state = 0;
+	ssi->waketest = 0;
+	ssi->rxid = 0;
+	ssi->txid = 0;
+	list_for_each_safe(head, tmp, &ssi->txqueue) {
+		msg = list_entry(head, struct hsi_msg, link);
+		dev_dbg(&cl->device, "Pending TX data\n");
+		list_del(head);
+		ssip_free_data(msg);
+	}
+	ssi->txqueue_len = 0;
+	spin_unlock_bh(&ssi->lock);
+}
+
+static void ssip_dump_state(struct hsi_client *cl)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct hsi_msg *msg;
+
+	spin_lock_bh(&ssi->lock);
+	dev_err(&cl->device, "Main state: %d\n", ssi->main_state);
+	dev_err(&cl->device, "Recv state: %d\n", ssi->recv_state);
+	dev_err(&cl->device, "Send state: %d\n", ssi->send_state);
+	dev_err(&cl->device, "CMT %s\n", (ssi->main_state == ACTIVE) ?
+							"Online" : "Offline");
+	dev_err(&cl->device, "Wake test %d\n", ssi->waketest);
+	dev_err(&cl->device, "Data RX id: %d\n", ssi->rxid);
+	dev_err(&cl->device, "Data TX id: %d\n", ssi->txid);
+
+	list_for_each_entry(msg, &ssi->txqueue, link)
+		dev_err(&cl->device, "pending TX data (%p)\n", msg);
+	spin_unlock_bh(&ssi->lock);
+}
+
+static void ssip_error(struct hsi_client *cl)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct hsi_msg *msg;
+
+	ssip_dump_state(cl);
+	ssip_reset(cl);
+	msg = ssip_claim_cmd(ssi);
+	msg->complete = ssip_rxcmd_complete;
+	hsi_async_read(cl, msg);
+}
+
+static void ssip_keep_alive(unsigned long data)
+{
+	struct hsi_client *cl = (struct hsi_client *)data;
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+
+	dev_dbg(&cl->device, "Keep alive kick in: m(%d) r(%d) s(%d)\n",
+		ssi->main_state, ssi->recv_state, ssi->send_state);
+
+	spin_lock(&ssi->lock);
+	if (ssi->recv_state == RECV_IDLE)
+		switch (ssi->send_state) {
+		case SEND_READY:
+			if (atomic_read(&ssi->tx_usecnt) == 0)
+				break;
+			/*
+			 * Fall through. Workaround for cmt-speech
+			 * in that case we relay on audio timers.
+			 */
+		case SEND_IDLE:
+			spin_unlock(&ssi->lock);
+			return;
+		}
+	mod_timer(&ssi->keep_alive, jiffies + msecs_to_jiffies(SSIP_KATOUT));
+	spin_unlock(&ssi->lock);
+}
+
+static void ssip_wd(unsigned long data)
+{
+	struct hsi_client *cl = (struct hsi_client *)data;
+
+	dev_err(&cl->device, "Watchdog trigerred\n");
+	ssip_error(cl);
+}
+
+static void ssip_send_bootinfo_req_cmd(struct hsi_client *cl)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct hsi_msg *msg;
+
+	dev_dbg(&cl->device, "Issuing BOOT INFO REQ command\n");
+	msg = ssip_claim_cmd(ssi);
+	ssip_set_cmd(msg, SSIP_BOOTINFO_REQ_CMD(SSIP_LOCAL_VERID));
+	msg->complete = ssip_release_cmd;
+	hsi_async_write(cl, msg);
+	dev_dbg(&cl->device, "Issuing RX command\n");
+	msg = ssip_claim_cmd(ssi);
+	msg->complete = ssip_rxcmd_complete;
+	hsi_async_read(cl, msg);
+}
+
+static void ssip_start_rx(struct hsi_client *cl)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct hsi_msg *msg;
+
+	dev_dbg(&cl->device, "RX start M(%d) R(%d)\n", ssi->main_state,
+						ssi->recv_state);
+	spin_lock(&ssi->lock);
+	/*
+	 * We can have two UP events in a row due to a short low
+	 * high transition. Therefore we need to ignore the sencond UP event.
+	 */
+	if ((ssi->main_state != ACTIVE) || (ssi->recv_state == RECV_READY)) {
+		if (ssi->main_state == INIT) {
+			ssi->main_state = HANDSHAKE;
+			spin_unlock(&ssi->lock);
+			ssip_send_bootinfo_req_cmd(cl);
+		} else {
+			spin_unlock(&ssi->lock);
+		}
+		return;
+	}
+	ssip_set_rxstate(ssi, RECV_READY);
+	spin_unlock(&ssi->lock);
+
+	msg = ssip_claim_cmd(ssi);
+	ssip_set_cmd(msg, SSIP_READY_CMD);
+	msg->complete = ssip_release_cmd;
+	dev_dbg(&cl->device, "Send READY\n");
+	hsi_async_write(cl, msg);
+}
+
+static void ssip_stop_rx(struct hsi_client *cl)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+
+	dev_dbg(&cl->device, "RX stop M(%d)\n", ssi->main_state);
+	spin_lock(&ssi->lock);
+	if (likely(ssi->main_state == ACTIVE))
+		ssip_set_rxstate(ssi, RECV_IDLE);
+	spin_unlock(&ssi->lock);
+}
+
+static void ssip_free_strans(struct hsi_msg *msg)
+{
+	ssip_free_data(msg->context);
+	ssip_release_cmd(msg);
+}
+
+static void ssip_strans_complete(struct hsi_msg *msg)
+{
+	struct hsi_client *cl = msg->cl;
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct hsi_msg *data;
+
+	data = msg->context;
+	ssip_release_cmd(msg);
+	spin_lock(&ssi->lock);
+	ssip_set_txstate(ssi, SENDING);
+	spin_unlock(&ssi->lock);
+	hsi_async_write(cl, data);
+}
+
+static int ssip_xmit(struct hsi_client *cl)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct hsi_msg *msg, *dmsg;
+	struct sk_buff *skb;
+
+	spin_lock_bh(&ssi->lock);
+	if (list_empty(&ssi->txqueue)) {
+		spin_unlock_bh(&ssi->lock);
+		return 0;
+	}
+	dmsg = list_first_entry(&ssi->txqueue, struct hsi_msg, link);
+	list_del(&dmsg->link);
+	ssi->txqueue_len--;
+	spin_unlock_bh(&ssi->lock);
+
+	msg = ssip_claim_cmd(ssi);
+	skb = dmsg->context;
+	msg->context = dmsg;
+	msg->complete = ssip_strans_complete;
+	msg->destructor = ssip_free_strans;
+
+	spin_lock_bh(&ssi->lock);
+	ssip_set_cmd(msg, SSIP_START_TRANS_CMD(SSIP_BYTES_TO_FRAMES(skb->len),
+								ssi->txid));
+	ssi->txid++;
+	ssip_set_txstate(ssi, SENDING);
+	spin_unlock_bh(&ssi->lock);
+
+	dev_dbg(&cl->device, "Send STRANS (%d frames)\n",
+						SSIP_BYTES_TO_FRAMES(skb->len));
+
+	return hsi_async_write(cl, msg);
+}
+
+/* In soft IRQ context */
+static void ssip_pn_rx(struct sk_buff *skb)
+{
+	struct net_device *dev = skb->dev;
+
+	if (unlikely(!netif_running(dev))) {
+		dev_dbg(&dev->dev, "Drop RX packet\n");
+		dev->stats.rx_dropped++;
+		dev_kfree_skb(skb);
+		return;
+	}
+	if (unlikely(!pskb_may_pull(skb, SSIP_MIN_PN_HDR))) {
+		dev_dbg(&dev->dev, "Error drop RX packet\n");
+		dev->stats.rx_errors++;
+		dev->stats.rx_length_errors++;
+		dev_kfree_skb(skb);
+		return;
+	}
+	dev->stats.rx_packets++;
+	dev->stats.rx_bytes += skb->len;
+
+	/* length field is exchanged in network byte order */
+	((u16 *)skb->data)[2] = ntohs(((u16 *)skb->data)[2]);
+	dev_dbg(&dev->dev, "RX length fixed (%04x -> %u)\n",
+			((u16 *)skb->data)[2], ntohs(((u16 *)skb->data)[2]));
+
+	skb->protocol = htons(ETH_P_PHONET);
+	skb_reset_mac_header(skb);
+	__skb_pull(skb, 1);
+	netif_rx(skb);
+}
+
+static void ssip_rx_data_complete(struct hsi_msg *msg)
+{
+	struct hsi_client *cl = msg->cl;
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct sk_buff *skb;
+
+	if (msg->status == HSI_STATUS_ERROR) {
+		dev_err(&cl->device, "RX data error\n");
+		ssip_free_data(msg);
+		ssip_error(cl);
+		return;
+	}
+	del_timer(&ssi->rx_wd); /* FIXME: Revisit */
+	skb = msg->context;
+	ssip_pn_rx(skb);
+	hsi_free_msg(msg);
+}
+
+static void ssip_rx_bootinforeq(struct hsi_client *cl, u32 cmd)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct hsi_msg *msg;
+
+	/* Workaroud: Ignore CMT Loader message leftover */
+	if (cmd == SSIP_CMT_LOADER_SYNC)
+		return;
+
+	switch (ssi->main_state) {
+	case ACTIVE:
+		dev_err(&cl->device, "Boot info req on active state\n");
+		ssip_error(cl);
+		/* Fall through */
+	case INIT:
+		spin_lock(&ssi->lock);
+		ssi->main_state = HANDSHAKE;
+		if (!ssi->waketest) {
+			ssi->waketest = 1;
+			ssi_waketest(cl, 1); /* FIXME: To be removed */
+		}
+		/* Start boot handshake watchdog */
+		mod_timer(&ssi->tx_wd, jiffies + msecs_to_jiffies(SSIP_WDTOUT));
+		spin_unlock(&ssi->lock);
+		dev_dbg(&cl->device, "Send BOOTINFO_RESP\n");
+		if (SSIP_DATA_VERSION(cmd) != SSIP_LOCAL_VERID)
+			dev_warn(&cl->device, "boot info req verid mismatch\n");
+		msg = ssip_claim_cmd(ssi);
+		ssip_set_cmd(msg, SSIP_BOOTINFO_RESP_CMD(SSIP_LOCAL_VERID));
+		msg->complete = ssip_release_cmd;
+		hsi_async_write(cl, msg);
+		break;
+	case HANDSHAKE:
+		/* Ignore */
+		break;
+	default:
+		dev_dbg(&cl->device, "Wrong state M(%d)\n", ssi->main_state);
+		break;
+	}
+}
+
+static void ssip_rx_bootinforesp(struct hsi_client *cl, u32 cmd)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+
+	if (SSIP_DATA_VERSION(cmd) != SSIP_LOCAL_VERID)
+		dev_warn(&cl->device, "boot info resp verid mismatch\n");
+
+	spin_lock(&ssi->lock);
+	if (ssi->main_state != ACTIVE)
+		/* Use tx_wd as a boot watchdog in non ACTIVE state */
+		mod_timer(&ssi->tx_wd, jiffies + msecs_to_jiffies(SSIP_WDTOUT));
+	else
+		dev_dbg(&cl->device, "boot info resp ignored M(%d)\n",
+							ssi->main_state);
+	spin_unlock(&ssi->lock);
+}
+
+static void ssip_rx_waketest(struct hsi_client *cl, u32 cmd)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	unsigned int wkres = SSIP_PAYLOAD(cmd);
+
+	spin_lock(&ssi->lock);
+	if (ssi->main_state != HANDSHAKE) {
+		dev_dbg(&cl->device, "wake lines test ignored M(%d)\n",
+							ssi->main_state);
+		spin_unlock(&ssi->lock);
+		return;
+	}
+	if (ssi->waketest) {
+		ssi->waketest = 0;
+		ssi_waketest(cl, 0); /* FIXME: To be removed */
+	}
+	ssi->main_state = ACTIVE;
+	del_timer(&ssi->tx_wd); /* Stop boot handshake timer */
+	spin_unlock(&ssi->lock);
+
+	dev_notice(&cl->device, "WAKELINES TEST %s\n",
+				wkres & SSIP_WAKETEST_FAILED ? "FAILED" : "OK");
+	if (wkres & SSIP_WAKETEST_FAILED) {
+		ssip_error(cl);
+		return;
+	}
+	dev_dbg(&cl->device, "CMT is ONLINE\n");
+	netif_wake_queue(ssi->netdev);
+	netif_carrier_on(ssi->netdev);
+}
+
+static void ssip_rx_ready(struct hsi_client *cl)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+
+	spin_lock(&ssi->lock);
+	if (unlikely(ssi->main_state != ACTIVE)) {
+		dev_dbg(&cl->device, "READY on wrong state: S(%d) M(%d)\n",
+					ssi->send_state, ssi->main_state);
+		spin_unlock(&ssi->lock);
+		return;
+	}
+	if (ssi->send_state != WAIT4READY) {
+		dev_dbg(&cl->device, "Ignore spurious READY command\n");
+		spin_unlock(&ssi->lock);
+		return;
+	}
+	ssip_set_txstate(ssi, SEND_READY);
+	spin_unlock(&ssi->lock);
+	ssip_xmit(cl);
+}
+
+static void ssip_rx_strans(struct hsi_client *cl, u32 cmd)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct sk_buff *skb;
+	struct hsi_msg *msg;
+	int len = SSIP_PDU_LENGTH(cmd);
+
+	dev_dbg(&cl->device, "RX strans: %d frames\n", len);
+	spin_lock(&ssi->lock);
+	if (unlikely(ssi->main_state != ACTIVE)) {
+		dev_err(&cl->device, "START TRANS wrong state: S(%d) M(%d)\n",
+					ssi->send_state, ssi->main_state);
+		spin_unlock(&ssi->lock);
+		return;
+	}
+	ssip_set_rxstate(ssi, RECEIVING);
+	if (unlikely(SSIP_MSG_ID(cmd) != ssi->rxid)) {
+		dev_err(&cl->device, "START TRANS id %d expeceted %d\n",
+					SSIP_MSG_ID(cmd), ssi->rxid);
+		spin_unlock(&ssi->lock);
+		goto out1;
+	}
+	ssi->rxid++;
+	spin_unlock(&ssi->lock);
+	skb = netdev_alloc_skb(ssi->netdev, len * 4);
+	if (unlikely(!skb)) {
+		dev_err(&cl->device, "No memory for rx skb\n");
+		goto out1;
+	}
+	skb->dev = ssi->netdev;
+	skb_put(skb, len * 4);
+	msg = ssip_alloc_data(ssi, skb, GFP_ATOMIC);
+	if (unlikely(!msg)) {
+		dev_err(&cl->device, "No memory for RX data msg\n");
+		goto out2;
+	}
+	msg->complete = ssip_rx_data_complete;
+	hsi_async_read(cl, msg);
+
+	return;
+out2:
+	dev_kfree_skb(skb);
+out1:
+	ssip_error(cl);
+}
+
+static void ssip_rxcmd_complete(struct hsi_msg *msg)
+{
+	struct hsi_client *cl = msg->cl;
+	u32 cmd = ssip_get_cmd(msg);
+	unsigned int cmdid = SSIP_COMMAND(cmd);
+
+	if (msg->status == HSI_STATUS_ERROR) {
+		dev_err(&cl->device, "RX error detected\n");
+		ssip_release_cmd(msg);
+		ssip_error(cl);
+		return;
+	}
+	hsi_async_read(cl, msg);
+	dev_dbg(&cl->device, "RX cmd: 0x%08x\n", cmd);
+	switch (cmdid) {
+	case SSIP_SW_BREAK:
+		/* Ignored */
+		break;
+	case SSIP_BOOTINFO_REQ:
+		ssip_rx_bootinforeq(cl, cmd);
+		break;
+	case SSIP_BOOTINFO_RESP:
+		ssip_rx_bootinforesp(cl, cmd);
+		break;
+	case SSIP_WAKETEST_RESULT:
+		ssip_rx_waketest(cl, cmd);
+		break;
+	case SSIP_START_TRANS:
+		ssip_rx_strans(cl, cmd);
+		break;
+	case SSIP_READY:
+		ssip_rx_ready(cl);
+		break;
+	default:
+		dev_warn(&cl->device, "command 0x%08x not supported\n", cmd);
+		break;
+	}
+}
+
+static void ssip_swbreak_complete(struct hsi_msg *msg)
+{
+	struct hsi_client *cl = msg->cl;
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+
+	ssip_release_cmd(msg);
+	spin_lock(&ssi->lock);
+	if (list_empty(&ssi->txqueue)) {
+		if (atomic_read(&ssi->tx_usecnt)) {
+			ssip_set_txstate(ssi, SEND_READY);
+		} else {
+			ssip_set_txstate(ssi, SEND_IDLE);
+			hsi_stop_tx(cl);
+		}
+		spin_unlock(&ssi->lock);
+	} else {
+		spin_unlock(&ssi->lock);
+		ssip_xmit(cl);
+	}
+	netif_wake_queue(ssi->netdev);
+}
+
+static void ssip_tx_data_complete(struct hsi_msg *msg)
+{
+	struct hsi_client *cl = msg->cl;
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct hsi_msg *cmsg;
+
+	if (msg->status == HSI_STATUS_ERROR) {
+		dev_err(&cl->device, "TX data error\n");
+		ssip_error(cl);
+		goto out;
+	}
+	spin_lock(&ssi->lock);
+	if (list_empty(&ssi->txqueue)) {
+		ssip_set_txstate(ssi, SENDING_SWBREAK);
+		spin_unlock(&ssi->lock);
+		cmsg = ssip_claim_cmd(ssi);
+		ssip_set_cmd(cmsg, SSIP_SWBREAK_CMD);
+		cmsg->complete = ssip_swbreak_complete;
+		dev_dbg(&cl->device, "Send SWBREAK\n");
+		hsi_async_write(cl, cmsg);
+	} else {
+		spin_unlock(&ssi->lock);
+		ssip_xmit(cl);
+	}
+out:
+	ssip_free_data(msg);
+}
+
+void ssip_port_event(struct hsi_client *cl, unsigned long event)
+{
+	switch (event) {
+	case HSI_EVENT_START_RX:
+		ssip_start_rx(cl);
+		break;
+	case HSI_EVENT_STOP_RX:
+		ssip_stop_rx(cl);
+		break;
+	default:
+		return;
+	}
+}
+
+static int ssip_pn_open(struct net_device *dev)
+{
+	struct hsi_client *cl = to_hsi_client(dev->dev.parent);
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	int err;
+
+	err = hsi_claim_port(cl, 1);
+	if (err < 0) {
+		dev_err(&cl->device, "SSI port already claimed\n");
+		return err;
+	}
+	err = hsi_register_port_event(cl, ssip_port_event);
+	if (err < 0) {
+		dev_err(&cl->device, "Register HSI port event failed (%d)\n",
+			err);
+		return err;
+	}
+	dev_dbg(&cl->device, "Configuring SSI port\n");
+	hsi_setup(cl);
+	spin_lock_bh(&ssi->lock);
+	if (!ssi->waketest) {
+		ssi->waketest = 1;
+		ssi_waketest(cl, 1); /* FIXME: To be removed */
+	}
+	ssi->main_state = INIT;
+	spin_unlock_bh(&ssi->lock);
+
+	return 0;
+}
+
+static int ssip_pn_stop(struct net_device *dev)
+{
+	struct hsi_client *cl = to_hsi_client(dev->dev.parent);
+
+	ssip_reset(cl);
+	hsi_unregister_port_event(cl);
+	hsi_release_port(cl);
+
+	return 0;
+}
+
+static int ssip_pn_set_mtu(struct net_device *dev, int new_mtu)
+{
+	if (new_mtu > SSIP_MAX_MTU || new_mtu < PHONET_MIN_MTU)
+		return -EINVAL;
+	dev->mtu = new_mtu;
+
+	return 0;
+}
+
+static int ssip_pn_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct hsi_client *cl = to_hsi_client(dev->dev.parent);
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct hsi_msg *msg;
+
+	if ((skb->protocol != htons(ETH_P_PHONET)) ||
+					(skb->len < SSIP_MIN_PN_HDR))
+		goto drop;
+	/* Pad to 32-bits - FIXME: Revisit*/
+	if ((skb->len & 3) && skb_pad(skb, 4 - (skb->len & 3)))
+		goto drop;
+
+	/*
+	 * Modem sends Phonet messages over SSI with its own endianess...
+	 * Assume that modem has the same endianess as we do.
+	 */
+	if (skb_cow_head(skb, 0))
+		goto drop;
+
+	/* length field is exchanged in network byte order */
+	((u16 *)skb->data)[2] = htons(((u16 *)skb->data)[2]);
+
+	msg = ssip_alloc_data(ssi, skb, GFP_ATOMIC);
+	if (!msg) {
+		dev_dbg(&cl->device, "Dropping tx data: No memory\n");
+		goto drop;
+	}
+	msg->complete = ssip_tx_data_complete;
+
+	spin_lock_bh(&ssi->lock);
+	if (unlikely(ssi->main_state != ACTIVE)) {
+		spin_unlock_bh(&ssi->lock);
+		dev_dbg(&cl->device, "Dropping tx data: CMT is OFFLINE\n");
+		goto drop2;
+	}
+	list_add_tail(&msg->link, &ssi->txqueue);
+	ssi->txqueue_len++;
+	if (dev->tx_queue_len < ssi->txqueue_len) {
+		dev_info(&cl->device, "TX queue full %d\n", ssi->txqueue_len);
+		netif_stop_queue(dev);
+	}
+	if (ssi->send_state == SEND_IDLE) {
+		ssip_set_txstate(ssi, WAIT4READY);
+		spin_unlock_bh(&ssi->lock);
+		dev_dbg(&cl->device, "Start TX qlen %d\n", ssi->txqueue_len);
+		hsi_start_tx(cl);
+	} else if (ssi->send_state == SEND_READY) {
+		/* Needed for cmt-speech workaround */
+		dev_dbg(&cl->device, "Start TX on SEND READY qlen %d\n",
+							ssi->txqueue_len);
+		spin_unlock_bh(&ssi->lock);
+		ssip_xmit(cl);
+	} else {
+		spin_unlock_bh(&ssi->lock);
+	}
+	dev->stats.tx_packets++;
+	dev->stats.tx_bytes += skb->len;
+
+	return 0;
+drop2:
+	hsi_free_msg(msg);
+drop:
+	dev->stats.tx_dropped++;
+	dev_kfree_skb(skb);
+
+	return 0;
+}
+
+/* CMT reset event handler */
+void ssip_reset_event(struct hsi_client *master)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(master);
+	dev_err(&ssi->cl->device, "CMT reset detected!\n");
+	ssip_error(ssi->cl);
+}
+EXPORT_SYMBOL_GPL(ssip_reset_event);
+
+static const struct net_device_ops ssip_pn_ops = {
+	.ndo_open	= ssip_pn_open,
+	.ndo_stop	= ssip_pn_stop,
+	.ndo_start_xmit	= ssip_pn_xmit,
+	.ndo_change_mtu	= ssip_pn_set_mtu,
+};
+
+static void ssip_pn_setup(struct net_device *dev)
+{
+	dev->features		= 0;
+	dev->netdev_ops		= &ssip_pn_ops;
+	dev->type		= ARPHRD_PHONET;
+	dev->flags		= IFF_POINTOPOINT | IFF_NOARP;
+	dev->mtu		= SSIP_DEFAULT_MTU;
+	dev->hard_header_len	= 1;
+	dev->dev_addr[0]	= PN_MEDIA_SOS;
+	dev->addr_len		= 1;
+	dev->tx_queue_len	= SSIP_TXQUEUE_LEN;
+
+	dev->destructor		= free_netdev;
+	dev->header_ops		= &phonet_header_ops;
+}
+
+static int ssi_protocol_probe(struct device *dev)
+{
+	static const char ifname[] = "phonet%d";
+	struct hsi_client *cl = to_hsi_client(dev);
+	struct ssi_protocol *ssi;
+	int err;
+
+	ssi = kzalloc(sizeof(*ssi), GFP_KERNEL);
+	if (!ssi) {
+		dev_err(dev, "No memory for ssi protocol\n");
+		return -ENOMEM;
+	}
+
+	spin_lock_init(&ssi->lock);
+	init_timer_deferrable(&ssi->rx_wd);
+	init_timer_deferrable(&ssi->tx_wd);
+	init_timer(&ssi->keep_alive);
+	ssi->rx_wd.data = (unsigned long)cl;
+	ssi->rx_wd.function = ssip_wd;
+	ssi->tx_wd.data = (unsigned long)cl;
+	ssi->tx_wd.function = ssip_wd;
+	ssi->keep_alive.data = (unsigned long)cl;
+	ssi->keep_alive.function = ssip_keep_alive;
+	INIT_LIST_HEAD(&ssi->txqueue);
+	INIT_LIST_HEAD(&ssi->cmdqueue);
+	atomic_set(&ssi->tx_usecnt, 0);
+	hsi_client_set_drvdata(cl, ssi);
+	ssi->cl = cl;
+
+	ssi->channel_id_cmd = hsi_get_channel_id_by_name(cl, "mcsaab-control");
+	if (ssi->channel_id_cmd < 0) {
+		err = ssi->channel_id_cmd;
+		dev_err(dev, "Could not get cmd channel (%d)\n", err);
+		goto out;
+	}
+
+	ssi->channel_id_data = hsi_get_channel_id_by_name(cl, "mcsaab-data");
+	if (ssi->channel_id_data < 0) {
+		err = ssi->channel_id_data;
+		dev_err(dev, "Could not get data channel (%d)\n", err);
+		goto out;
+	}
+
+	err = ssip_alloc_cmds(ssi);
+	if (err < 0) {
+		dev_err(dev, "No memory for commands\n");
+		goto out;
+	}
+
+	ssi->netdev = alloc_netdev(0, ifname, ssip_pn_setup);
+	if (!ssi->netdev) {
+		dev_err(dev, "No memory for netdev\n");
+		err = -ENOMEM;
+		goto out1;
+	}
+
+	SET_NETDEV_DEV(ssi->netdev, dev);
+	netif_carrier_off(ssi->netdev);
+	err = register_netdev(ssi->netdev);
+	if (err < 0) {
+		dev_err(dev, "Register netdev failed (%d)\n", err);
+		goto out2;
+	}
+
+	list_add(&ssi->link, &ssip_list);
+
+	dev_dbg(dev, "channel configuration: cmd=%d, data=%d\n",
+		ssi->channel_id_cmd, ssi->channel_id_data);
+
+	return 0;
+out2:
+	free_netdev(ssi->netdev);
+out1:
+	ssip_free_cmds(ssi);
+out:
+	kfree(ssi);
+
+	return err;
+}
+
+static int ssi_protocol_remove(struct device *dev)
+{
+	struct hsi_client *cl = to_hsi_client(dev);
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+
+	list_del(&ssi->link);
+	unregister_netdev(ssi->netdev);
+	ssip_free_cmds(ssi);
+	hsi_client_set_drvdata(cl, NULL);
+	kfree(ssi);
+
+	return 0;
+}
+
+static struct hsi_client_driver ssip_driver = {
+	.driver = {
+		.name	= "ssi-protocol",
+		.owner	= THIS_MODULE,
+		.probe	= ssi_protocol_probe,
+		.remove	= ssi_protocol_remove,
+	},
+};
+
+static int __init ssip_init(void)
+{
+	pr_info("SSI protocol aka McSAAB added\n");
+
+	return hsi_register_client_driver(&ssip_driver);
+}
+module_init(ssip_init);
+
+static void __exit ssip_exit(void)
+{
+	hsi_unregister_client_driver(&ssip_driver);
+	pr_info("SSI protocol driver removed\n");
+}
+module_exit(ssip_exit);
+
+MODULE_ALIAS("hsi:ssi-protocol");
+MODULE_AUTHOR("Carlos Chinea <carlos.chinea@nokia.com>");
+MODULE_AUTHOR("Remi Denis-Courmont <remi.denis-courmont@nokia.com>");
+MODULE_DESCRIPTION("SSI protocol improved aka McSAAB");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hsi/controllers/Kconfig b/drivers/hsi/controllers/Kconfig
new file mode 100644
index 00000000000..6aba2780817
--- /dev/null
+++ b/drivers/hsi/controllers/Kconfig
@@ -0,0 +1,19 @@
+#
+# HSI controllers configuration
+#
+comment "HSI controllers"
+
+config OMAP_SSI
+	tristate "OMAP SSI hardware driver"
+	depends on HSI && OF && (ARCH_OMAP3 || (ARM && COMPILE_TEST))
+	---help---
+	  SSI is a legacy version of HSI. It is usually used to connect
+	  an application engine with a cellular modem.
+	  If you say Y here, you will enable the OMAP SSI hardware driver.
+
+	  If unsure, say N.
+
+config OMAP_SSI_PORT
+	tristate
+	default m if OMAP_SSI=m
+	default y if OMAP_SSI=y
diff --git a/drivers/hsi/controllers/Makefile b/drivers/hsi/controllers/Makefile
new file mode 100644
index 00000000000..d2665cf9c54
--- /dev/null
+++ b/drivers/hsi/controllers/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for HSI controllers drivers
+#
+
+obj-$(CONFIG_OMAP_SSI)		+= omap_ssi.o
+obj-$(CONFIG_OMAP_SSI_PORT)	+= omap_ssi_port.o
diff --git a/drivers/hsi/controllers/omap_ssi.c b/drivers/hsi/controllers/omap_ssi.c
new file mode 100644
index 00000000000..0fc7a7fd014
--- /dev/null
+++ b/drivers/hsi/controllers/omap_ssi.c
@@ -0,0 +1,625 @@
+/* OMAP SSI driver.
+ *
+ * Copyright (C) 2010 Nokia Corporation. All rights reserved.
+ * Copyright (C) 2014 Sebastian Reichel <sre@kernel.org>
+ *
+ * Contact: Carlos Chinea <carlos.chinea@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/compiler.h>
+#include <linux/err.h>
+#include <linux/ioport.h>
+#include <linux/io.h>
+#include <linux/gpio.h>
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmaengine.h>
+#include <linux/delay.h>
+#include <linux/seq_file.h>
+#include <linux/scatterlist.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/debugfs.h>
+#include <linux/pm_runtime.h>
+#include <linux/of_platform.h>
+#include <linux/hsi/hsi.h>
+#include <linux/idr.h>
+
+#include "omap_ssi_regs.h"
+#include "omap_ssi.h"
+
+/* For automatically allocated device IDs */
+static DEFINE_IDA(platform_omap_ssi_ida);
+
+#ifdef CONFIG_DEBUG_FS
+static int ssi_debug_show(struct seq_file *m, void *p __maybe_unused)
+{
+	struct hsi_controller *ssi = m->private;
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem *sys = omap_ssi->sys;
+
+	pm_runtime_get_sync(ssi->device.parent);
+	seq_printf(m, "REVISION\t: 0x%08x\n",  readl(sys + SSI_REVISION_REG));
+	seq_printf(m, "SYSCONFIG\t: 0x%08x\n", readl(sys + SSI_SYSCONFIG_REG));
+	seq_printf(m, "SYSSTATUS\t: 0x%08x\n", readl(sys + SSI_SYSSTATUS_REG));
+	pm_runtime_put_sync(ssi->device.parent);
+
+	return 0;
+}
+
+static int ssi_debug_gdd_show(struct seq_file *m, void *p __maybe_unused)
+{
+	struct hsi_controller *ssi = m->private;
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem *gdd = omap_ssi->gdd;
+	void __iomem *sys = omap_ssi->sys;
+	int lch;
+
+	pm_runtime_get_sync(ssi->device.parent);
+
+	seq_printf(m, "GDD_MPU_STATUS\t: 0x%08x\n",
+		readl(sys + SSI_GDD_MPU_IRQ_STATUS_REG));
+	seq_printf(m, "GDD_MPU_ENABLE\t: 0x%08x\n\n",
+		readl(sys + SSI_GDD_MPU_IRQ_ENABLE_REG));
+	seq_printf(m, "HW_ID\t\t: 0x%08x\n",
+				readl(gdd + SSI_GDD_HW_ID_REG));
+	seq_printf(m, "PPORT_ID\t: 0x%08x\n",
+				readl(gdd + SSI_GDD_PPORT_ID_REG));
+	seq_printf(m, "MPORT_ID\t: 0x%08x\n",
+				readl(gdd + SSI_GDD_MPORT_ID_REG));
+	seq_printf(m, "TEST\t\t: 0x%08x\n",
+				readl(gdd + SSI_GDD_TEST_REG));
+	seq_printf(m, "GCR\t\t: 0x%08x\n",
+				readl(gdd + SSI_GDD_GCR_REG));
+
+	for (lch = 0; lch < SSI_MAX_GDD_LCH; lch++) {
+		seq_printf(m, "\nGDD LCH %d\n=========\n", lch);
+		seq_printf(m, "CSDP\t\t: 0x%04x\n",
+				readw(gdd + SSI_GDD_CSDP_REG(lch)));
+		seq_printf(m, "CCR\t\t: 0x%04x\n",
+				readw(gdd + SSI_GDD_CCR_REG(lch)));
+		seq_printf(m, "CICR\t\t: 0x%04x\n",
+				readw(gdd + SSI_GDD_CICR_REG(lch)));
+		seq_printf(m, "CSR\t\t: 0x%04x\n",
+				readw(gdd + SSI_GDD_CSR_REG(lch)));
+		seq_printf(m, "CSSA\t\t: 0x%08x\n",
+				readl(gdd + SSI_GDD_CSSA_REG(lch)));
+		seq_printf(m, "CDSA\t\t: 0x%08x\n",
+				readl(gdd + SSI_GDD_CDSA_REG(lch)));
+		seq_printf(m, "CEN\t\t: 0x%04x\n",
+				readw(gdd + SSI_GDD_CEN_REG(lch)));
+		seq_printf(m, "CSAC\t\t: 0x%04x\n",
+				readw(gdd + SSI_GDD_CSAC_REG(lch)));
+		seq_printf(m, "CDAC\t\t: 0x%04x\n",
+				readw(gdd + SSI_GDD_CDAC_REG(lch)));
+		seq_printf(m, "CLNK_CTRL\t: 0x%04x\n",
+				readw(gdd + SSI_GDD_CLNK_CTRL_REG(lch)));
+	}
+
+	pm_runtime_put_sync(ssi->device.parent);
+
+	return 0;
+}
+
+static int ssi_regs_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ssi_debug_show, inode->i_private);
+}
+
+static int ssi_gdd_regs_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ssi_debug_gdd_show, inode->i_private);
+}
+
+static const struct file_operations ssi_regs_fops = {
+	.open		= ssi_regs_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static const struct file_operations ssi_gdd_regs_fops = {
+	.open		= ssi_gdd_regs_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static int __init ssi_debug_add_ctrl(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct dentry *dir;
+
+	/* SSI controller */
+	omap_ssi->dir = debugfs_create_dir(dev_name(&ssi->device), NULL);
+	if (IS_ERR(omap_ssi->dir))
+		return PTR_ERR(omap_ssi->dir);
+
+	debugfs_create_file("regs", S_IRUGO, omap_ssi->dir, ssi,
+								&ssi_regs_fops);
+	/* SSI GDD (DMA) */
+	dir = debugfs_create_dir("gdd", omap_ssi->dir);
+	if (IS_ERR(dir))
+		goto rback;
+	debugfs_create_file("regs", S_IRUGO, dir, ssi, &ssi_gdd_regs_fops);
+
+	return 0;
+rback:
+	debugfs_remove_recursive(omap_ssi->dir);
+
+	return PTR_ERR(dir);
+}
+
+static void ssi_debug_remove_ctrl(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	debugfs_remove_recursive(omap_ssi->dir);
+}
+#endif /* CONFIG_DEBUG_FS */
+
+/*
+ * FIXME: Horrible HACK needed until we remove the useless wakeline test
+ * in the CMT. To be removed !!!!
+ */
+void ssi_waketest(struct hsi_client *cl, unsigned int enable)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	omap_port->wktest = !!enable;
+	if (omap_port->wktest) {
+		pm_runtime_get_sync(ssi->device.parent);
+		writel_relaxed(SSI_WAKE(0),
+				omap_ssi->sys + SSI_SET_WAKE_REG(port->num));
+	} else {
+		writel_relaxed(SSI_WAKE(0),
+				omap_ssi->sys +	SSI_CLEAR_WAKE_REG(port->num));
+		pm_runtime_put_sync(ssi->device.parent);
+	}
+}
+EXPORT_SYMBOL_GPL(ssi_waketest);
+
+static void ssi_gdd_complete(struct hsi_controller *ssi, unsigned int lch)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_msg *msg = omap_ssi->gdd_trn[lch].msg;
+	struct hsi_port *port = to_hsi_port(msg->cl->device.parent);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	unsigned int dir;
+	u32 csr;
+	u32 val;
+
+	spin_lock(&omap_ssi->lock);
+
+	val = readl(omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	val &= ~SSI_GDD_LCH(lch);
+	writel_relaxed(val, omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+
+	if (msg->ttype == HSI_MSG_READ) {
+		dir = DMA_FROM_DEVICE;
+		val = SSI_DATAAVAILABLE(msg->channel);
+		pm_runtime_put_sync(ssi->device.parent);
+	} else {
+		dir = DMA_TO_DEVICE;
+		val = SSI_DATAACCEPT(msg->channel);
+		/* Keep clocks reference for write pio event */
+	}
+	dma_unmap_sg(&ssi->device, msg->sgt.sgl, msg->sgt.nents, dir);
+	csr = readw(omap_ssi->gdd + SSI_GDD_CSR_REG(lch));
+	omap_ssi->gdd_trn[lch].msg = NULL; /* release GDD lch */
+	dev_dbg(&port->device, "DMA completed ch %d ttype %d\n",
+				msg->channel, msg->ttype);
+	spin_unlock(&omap_ssi->lock);
+	if (csr & SSI_CSR_TOUR) { /* Timeout error */
+		msg->status = HSI_STATUS_ERROR;
+		msg->actual_len = 0;
+		spin_lock(&omap_port->lock);
+		list_del(&msg->link); /* Dequeue msg */
+		spin_unlock(&omap_port->lock);
+		msg->complete(msg);
+		return;
+	}
+	spin_lock(&omap_port->lock);
+	val |= readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	writel_relaxed(val, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	spin_unlock(&omap_port->lock);
+
+	msg->status = HSI_STATUS_COMPLETED;
+	msg->actual_len = sg_dma_len(msg->sgt.sgl);
+}
+
+static void ssi_gdd_tasklet(unsigned long dev)
+{
+	struct hsi_controller *ssi = (struct hsi_controller *)dev;
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem *sys = omap_ssi->sys;
+	unsigned int lch;
+	u32 status_reg;
+
+	pm_runtime_get_sync(ssi->device.parent);
+
+	status_reg = readl(sys + SSI_GDD_MPU_IRQ_STATUS_REG);
+	for (lch = 0; lch < SSI_MAX_GDD_LCH; lch++) {
+		if (status_reg & SSI_GDD_LCH(lch))
+			ssi_gdd_complete(ssi, lch);
+	}
+	writel_relaxed(status_reg, sys + SSI_GDD_MPU_IRQ_STATUS_REG);
+	status_reg = readl(sys + SSI_GDD_MPU_IRQ_STATUS_REG);
+
+	pm_runtime_put_sync(ssi->device.parent);
+
+	if (status_reg)
+		tasklet_hi_schedule(&omap_ssi->gdd_tasklet);
+	else
+		enable_irq(omap_ssi->gdd_irq);
+
+}
+
+static irqreturn_t ssi_gdd_isr(int irq, void *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	tasklet_hi_schedule(&omap_ssi->gdd_tasklet);
+	disable_irq_nosync(irq);
+
+	return IRQ_HANDLED;
+}
+
+static unsigned long ssi_get_clk_rate(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	unsigned long rate = clk_get_rate(omap_ssi->fck);
+	return rate;
+}
+
+static int __init ssi_get_iomem(struct platform_device *pd,
+		const char *name, void __iomem **pbase, dma_addr_t *phy)
+{
+	struct resource *mem;
+	struct resource *ioarea;
+	void __iomem *base;
+	struct hsi_controller *ssi = platform_get_drvdata(pd);
+
+	mem = platform_get_resource_byname(pd, IORESOURCE_MEM, name);
+	if (!mem) {
+		dev_err(&pd->dev, "IO memory region missing (%s)\n", name);
+		return -ENXIO;
+	}
+	ioarea = devm_request_mem_region(&ssi->device, mem->start,
+					resource_size(mem), dev_name(&pd->dev));
+	if (!ioarea) {
+		dev_err(&pd->dev, "%s IO memory region request failed\n",
+								mem->name);
+		return -ENXIO;
+	}
+	base = devm_ioremap(&ssi->device, mem->start, resource_size(mem));
+	if (!base) {
+		dev_err(&pd->dev, "%s IO remap failed\n", mem->name);
+		return -ENXIO;
+	}
+	*pbase = base;
+
+	if (phy)
+		*phy = mem->start;
+
+	return 0;
+}
+
+static int __init ssi_add_controller(struct hsi_controller *ssi,
+						struct platform_device *pd)
+{
+	struct omap_ssi_controller *omap_ssi;
+	int err;
+
+	omap_ssi = devm_kzalloc(&ssi->device, sizeof(*omap_ssi), GFP_KERNEL);
+	if (!omap_ssi) {
+		dev_err(&pd->dev, "not enough memory for omap ssi\n");
+		return -ENOMEM;
+	}
+
+	ssi->id = ida_simple_get(&platform_omap_ssi_ida, 0, 0, GFP_KERNEL);
+	if (ssi->id < 0) {
+		err = ssi->id;
+		goto out_err;
+	}
+
+	ssi->owner = THIS_MODULE;
+	ssi->device.parent = &pd->dev;
+	dev_set_name(&ssi->device, "ssi%d", ssi->id);
+	hsi_controller_set_drvdata(ssi, omap_ssi);
+	omap_ssi->dev = &ssi->device;
+	err = ssi_get_iomem(pd, "sys", &omap_ssi->sys, NULL);
+	if (err < 0)
+		goto out_err;
+	err = ssi_get_iomem(pd, "gdd", &omap_ssi->gdd, NULL);
+	if (err < 0)
+		goto out_err;
+	omap_ssi->gdd_irq = platform_get_irq_byname(pd, "gdd_mpu");
+	if (omap_ssi->gdd_irq < 0) {
+		dev_err(&pd->dev, "GDD IRQ resource missing\n");
+		err = omap_ssi->gdd_irq;
+		goto out_err;
+	}
+	tasklet_init(&omap_ssi->gdd_tasklet, ssi_gdd_tasklet,
+							(unsigned long)ssi);
+	err = devm_request_irq(&ssi->device, omap_ssi->gdd_irq, ssi_gdd_isr,
+						0, "gdd_mpu", ssi);
+	if (err < 0) {
+		dev_err(&ssi->device, "Request GDD IRQ %d failed (%d)",
+							omap_ssi->gdd_irq, err);
+		goto out_err;
+	}
+
+	omap_ssi->port = devm_kzalloc(&ssi->device,
+		sizeof(struct omap_ssi_port *) * ssi->num_ports, GFP_KERNEL);
+	if (!omap_ssi->port) {
+		err = -ENOMEM;
+		goto out_err;
+	}
+
+	omap_ssi->fck = devm_clk_get(&ssi->device, "ssi_ssr_fck");
+	if (IS_ERR(omap_ssi->fck)) {
+		dev_err(&pd->dev, "Could not acquire clock \"ssi_ssr_fck\": %li\n",
+			PTR_ERR(omap_ssi->fck));
+		err = -ENODEV;
+		goto out_err;
+	}
+
+	/* TODO: find register, which can be used to detect context loss */
+	omap_ssi->get_loss = NULL;
+
+	omap_ssi->max_speed = UINT_MAX;
+	spin_lock_init(&omap_ssi->lock);
+	err = hsi_register_controller(ssi);
+
+	if (err < 0)
+		goto out_err;
+
+	return 0;
+
+out_err:
+	ida_simple_remove(&platform_omap_ssi_ida, ssi->id);
+	return err;
+}
+
+static int __init ssi_hw_init(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	unsigned int i;
+	u32 val;
+	int err;
+
+	err = pm_runtime_get_sync(ssi->device.parent);
+	if (err < 0) {
+		dev_err(&ssi->device, "runtime PM failed %d\n", err);
+		return err;
+	}
+	/* Reseting SSI controller */
+	writel_relaxed(SSI_SOFTRESET, omap_ssi->sys + SSI_SYSCONFIG_REG);
+	val = readl(omap_ssi->sys + SSI_SYSSTATUS_REG);
+	for (i = 0; ((i < 20) && !(val & SSI_RESETDONE)); i++) {
+		msleep(20);
+		val = readl(omap_ssi->sys + SSI_SYSSTATUS_REG);
+	}
+	if (!(val & SSI_RESETDONE)) {
+		dev_err(&ssi->device, "SSI HW reset failed\n");
+		pm_runtime_put_sync(ssi->device.parent);
+		return -EIO;
+	}
+	/* Reseting GDD */
+	writel_relaxed(SSI_SWRESET, omap_ssi->gdd + SSI_GDD_GRST_REG);
+	/* Get FCK rate in KHz */
+	omap_ssi->fck_rate = DIV_ROUND_CLOSEST(ssi_get_clk_rate(ssi), 1000);
+	dev_dbg(&ssi->device, "SSI fck rate %lu KHz\n", omap_ssi->fck_rate);
+	/* Set default PM settings */
+	val = SSI_AUTOIDLE | SSI_SIDLEMODE_SMART | SSI_MIDLEMODE_SMART;
+	writel_relaxed(val, omap_ssi->sys + SSI_SYSCONFIG_REG);
+	omap_ssi->sysconfig = val;
+	writel_relaxed(SSI_CLK_AUTOGATING_ON, omap_ssi->sys + SSI_GDD_GCR_REG);
+	omap_ssi->gdd_gcr = SSI_CLK_AUTOGATING_ON;
+	pm_runtime_put_sync(ssi->device.parent);
+
+	return 0;
+}
+
+static void ssi_remove_controller(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	int id = ssi->id;
+	tasklet_kill(&omap_ssi->gdd_tasklet);
+	hsi_unregister_controller(ssi);
+	ida_simple_remove(&platform_omap_ssi_ida, id);
+}
+
+static inline int ssi_of_get_available_ports_count(const struct device_node *np)
+{
+	struct device_node *child;
+	int num = 0;
+
+	for_each_available_child_of_node(np, child)
+		if (of_device_is_compatible(child, "ti,omap3-ssi-port"))
+			num++;
+
+	return num;
+}
+
+static int ssi_remove_ports(struct device *dev, void *c)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+
+	of_device_unregister(pdev);
+
+	return 0;
+}
+
+static int __init ssi_probe(struct platform_device *pd)
+{
+	struct platform_device *childpdev;
+	struct device_node *np = pd->dev.of_node;
+	struct device_node *child;
+	struct hsi_controller *ssi;
+	int err;
+	int num_ports;
+
+	if (!np) {
+		dev_err(&pd->dev, "missing device tree data\n");
+		return -EINVAL;
+	}
+
+	num_ports = ssi_of_get_available_ports_count(np);
+
+	ssi = hsi_alloc_controller(num_ports, GFP_KERNEL);
+	if (!ssi) {
+		dev_err(&pd->dev, "No memory for controller\n");
+		return -ENOMEM;
+	}
+
+	platform_set_drvdata(pd, ssi);
+
+	err = ssi_add_controller(ssi, pd);
+	if (err < 0)
+		goto out1;
+
+	pm_runtime_irq_safe(&pd->dev);
+	pm_runtime_enable(&pd->dev);
+
+	err = ssi_hw_init(ssi);
+	if (err < 0)
+		goto out2;
+#ifdef CONFIG_DEBUG_FS
+	err = ssi_debug_add_ctrl(ssi);
+	if (err < 0)
+		goto out2;
+#endif
+
+	for_each_available_child_of_node(np, child) {
+		if (!of_device_is_compatible(child, "ti,omap3-ssi-port"))
+			continue;
+
+		childpdev = of_platform_device_create(child, NULL, &pd->dev);
+		if (!childpdev) {
+			err = -ENODEV;
+			dev_err(&pd->dev, "failed to create ssi controller port\n");
+			goto out3;
+		}
+	}
+
+	dev_info(&pd->dev, "ssi controller %d initialized (%d ports)!\n",
+		ssi->id, num_ports);
+	return err;
+out3:
+	device_for_each_child(&pd->dev, NULL, ssi_remove_ports);
+out2:
+	ssi_remove_controller(ssi);
+out1:
+	platform_set_drvdata(pd, NULL);
+	pm_runtime_disable(&pd->dev);
+
+	return err;
+}
+
+static int __exit ssi_remove(struct platform_device *pd)
+{
+	struct hsi_controller *ssi = platform_get_drvdata(pd);
+
+#ifdef CONFIG_DEBUG_FS
+	ssi_debug_remove_ctrl(ssi);
+#endif
+	ssi_remove_controller(ssi);
+	platform_set_drvdata(pd, NULL);
+
+	pm_runtime_disable(&pd->dev);
+
+	/* cleanup of of_platform_populate() call */
+	device_for_each_child(&pd->dev, NULL, ssi_remove_ports);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_RUNTIME
+static int omap_ssi_runtime_suspend(struct device *dev)
+{
+	struct hsi_controller *ssi = dev_get_drvdata(dev);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	dev_dbg(dev, "runtime suspend!\n");
+
+	if (omap_ssi->get_loss)
+		omap_ssi->loss_count =
+				omap_ssi->get_loss(ssi->device.parent);
+
+	return 0;
+}
+
+static int omap_ssi_runtime_resume(struct device *dev)
+{
+	struct hsi_controller *ssi = dev_get_drvdata(dev);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	dev_dbg(dev, "runtime resume!\n");
+
+	if ((omap_ssi->get_loss) && (omap_ssi->loss_count ==
+				omap_ssi->get_loss(ssi->device.parent)))
+		return 0;
+
+	writel_relaxed(omap_ssi->gdd_gcr, omap_ssi->gdd + SSI_GDD_GCR_REG);
+
+	return 0;
+}
+
+static const struct dev_pm_ops omap_ssi_pm_ops = {
+	SET_RUNTIME_PM_OPS(omap_ssi_runtime_suspend, omap_ssi_runtime_resume,
+		NULL)
+};
+
+#define DEV_PM_OPS     (&omap_ssi_pm_ops)
+#else
+#define DEV_PM_OPS     NULL
+#endif
+
+#ifdef CONFIG_OF
+static const struct of_device_id omap_ssi_of_match[] = {
+	{ .compatible = "ti,omap3-ssi", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, omap_ssi_of_match);
+#else
+#define omap_ssi_of_match NULL
+#endif
+
+static struct platform_driver ssi_pdriver = {
+	.remove	= __exit_p(ssi_remove),
+	.driver	= {
+		.name	= "omap_ssi",
+		.owner	= THIS_MODULE,
+		.pm     = DEV_PM_OPS,
+		.of_match_table = omap_ssi_of_match,
+	},
+};
+
+module_platform_driver_probe(ssi_pdriver, ssi_probe);
+
+MODULE_ALIAS("platform:omap_ssi");
+MODULE_AUTHOR("Carlos Chinea <carlos.chinea@nokia.com>");
+MODULE_AUTHOR("Sebastian Reichel <sre@kernel.org>");
+MODULE_DESCRIPTION("Synchronous Serial Interface Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/hsi/controllers/omap_ssi.h b/drivers/hsi/controllers/omap_ssi.h
new file mode 100644
index 00000000000..9d056417d88
--- /dev/null
+++ b/drivers/hsi/controllers/omap_ssi.h
@@ -0,0 +1,166 @@
+/* OMAP SSI internal interface.
+ *
+ * Copyright (C) 2010 Nokia Corporation. All rights reserved.
+ * Copyright (C) 2013 Sebastian Reichel
+ *
+ * Contact: Carlos Chinea <carlos.chinea@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
+ */
+
+#ifndef __LINUX_HSI_OMAP_SSI_H__
+#define __LINUX_HSI_OMAP_SSI_H__
+
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/hsi/hsi.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+
+#define SSI_MAX_CHANNELS	8
+#define SSI_MAX_GDD_LCH		8
+#define SSI_BYTES_TO_FRAMES(x) ((((x) - 1) >> 2) + 1)
+
+/**
+ * struct omap_ssm_ctx - OMAP synchronous serial module (TX/RX) context
+ * @mode: Bit transmission mode
+ * @channels: Number of channels
+ * @framesize: Frame size in bits
+ * @timeout: RX frame timeout
+ * @divisor: TX divider
+ * @arb_mode: Arbitration mode for TX frame (Round robin, priority)
+ */
+struct omap_ssm_ctx {
+	u32	mode;
+	u32	channels;
+	u32	frame_size;
+	union	{
+			u32	timeout; /* Rx Only */
+			struct	{
+					u32	arb_mode;
+					u32	divisor;
+			}; /* Tx only */
+	};
+};
+
+/**
+ * struct omap_ssi_port - OMAP SSI port data
+ * @dev: device associated to the port (HSI port)
+ * @pdev: platform device associated to the port
+ * @sst_dma: SSI transmitter physical base address
+ * @ssr_dma: SSI receiver physical base address
+ * @sst_base: SSI transmitter base address
+ * @ssr_base: SSI receiver base address
+ * @wk_lock: spin lock to serialize access to the wake lines
+ * @lock: Spin lock to serialize access to the SSI port
+ * @channels: Current number of channels configured (1,2,4 or 8)
+ * @txqueue: TX message queues
+ * @rxqueue: RX message queues
+ * @brkqueue: Queue of incoming HWBREAK requests (FRAME mode)
+ * @irq: IRQ number
+ * @wake_irq: IRQ number for incoming wake line (-1 if none)
+ * @wake_gpio: GPIO number for incoming wake line (-1 if none)
+ * @pio_tasklet: Bottom half for PIO transfers and events
+ * @wake_tasklet: Bottom half for incoming wake events
+ * @wkin_cken: Keep track of clock references due to the incoming wake line
+ * @wk_refcount: Reference count for output wake line
+ * @sys_mpu_enable: Context for the interrupt enable register for irq 0
+ * @sst: Context for the synchronous serial transmitter
+ * @ssr: Context for the synchronous serial receiver
+ */
+struct omap_ssi_port {
+	struct device		*dev;
+	struct device           *pdev;
+	dma_addr_t		sst_dma;
+	dma_addr_t		ssr_dma;
+	void __iomem		*sst_base;
+	void __iomem		*ssr_base;
+	spinlock_t		wk_lock;
+	spinlock_t		lock;
+	unsigned int		channels;
+	struct list_head	txqueue[SSI_MAX_CHANNELS];
+	struct list_head	rxqueue[SSI_MAX_CHANNELS];
+	struct list_head	brkqueue;
+	unsigned int		irq;
+	int			wake_irq;
+	int			wake_gpio;
+	struct tasklet_struct	pio_tasklet;
+	struct tasklet_struct	wake_tasklet;
+	bool			wktest:1; /* FIXME: HACK to be removed */
+	bool			wkin_cken:1; /* Workaround */
+	unsigned int		wk_refcount;
+	/* OMAP SSI port context */
+	u32			sys_mpu_enable; /* We use only one irq */
+	struct omap_ssm_ctx	sst;
+	struct omap_ssm_ctx	ssr;
+	u32			loss_count;
+	u32			port_id;
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *dir;
+#endif
+};
+
+/**
+ * struct gdd_trn - GDD transaction data
+ * @msg: Pointer to the HSI message being served
+ * @sg: Pointer to the current sg entry being served
+ */
+struct gdd_trn {
+	struct hsi_msg		*msg;
+	struct scatterlist	*sg;
+};
+
+/**
+ * struct omap_ssi_controller - OMAP SSI controller data
+ * @dev: device associated to the controller (HSI controller)
+ * @sys: SSI I/O base address
+ * @gdd: GDD I/O base address
+ * @fck: SSI functional clock
+ * @gdd_irq: IRQ line for GDD
+ * @gdd_tasklet: bottom half for DMA transfers
+ * @gdd_trn: Array of GDD transaction data for ongoing GDD transfers
+ * @lock: lock to serialize access to GDD
+ * @loss_count: To follow if we need to restore context or not
+ * @max_speed: Maximum TX speed (Kb/s) set by the clients.
+ * @sysconfig: SSI controller saved context
+ * @gdd_gcr: SSI GDD saved context
+ * @get_loss: Pointer to omap_pm_get_dev_context_loss_count, if any
+ * @port: Array of pointers of the ports of the controller
+ * @dir: Debugfs SSI root directory
+ */
+struct omap_ssi_controller {
+	struct device		*dev;
+	void __iomem		*sys;
+	void __iomem		*gdd;
+	struct clk		*fck;
+	unsigned int		gdd_irq;
+	struct tasklet_struct	gdd_tasklet;
+	struct gdd_trn		gdd_trn[SSI_MAX_GDD_LCH];
+	spinlock_t		lock;
+	unsigned long		fck_rate;
+	u32			loss_count;
+	u32			max_speed;
+	/* OMAP SSI Controller context */
+	u32			sysconfig;
+	u32			gdd_gcr;
+	int			(*get_loss)(struct device *dev);
+	struct omap_ssi_port	**port;
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *dir;
+#endif
+};
+
+#endif /* __LINUX_HSI_OMAP_SSI_H__ */
diff --git a/drivers/hsi/controllers/omap_ssi_port.c b/drivers/hsi/controllers/omap_ssi_port.c
new file mode 100644
index 00000000000..b8693f0b27f
--- /dev/null
+++ b/drivers/hsi/controllers/omap_ssi_port.c
@@ -0,0 +1,1399 @@
+/* OMAP SSI port driver.
+ *
+ * Copyright (C) 2010 Nokia Corporation. All rights reserved.
+ * Copyright (C) 2014 Sebastian Reichel <sre@kernel.org>
+ *
+ * Contact: Carlos Chinea <carlos.chinea@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/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/pm_runtime.h>
+
+#include <linux/of_gpio.h>
+#include <linux/debugfs.h>
+
+#include "omap_ssi_regs.h"
+#include "omap_ssi.h"
+
+static inline int hsi_dummy_msg(struct hsi_msg *msg __maybe_unused)
+{
+	return 0;
+}
+
+static inline int hsi_dummy_cl(struct hsi_client *cl __maybe_unused)
+{
+	return 0;
+}
+
+static inline unsigned int ssi_wakein(struct hsi_port *port)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	return gpio_get_value(omap_port->wake_gpio);
+}
+
+#ifdef CONFIG_DEBUG_FS
+static void ssi_debug_remove_port(struct hsi_port *port)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+
+	debugfs_remove_recursive(omap_port->dir);
+}
+
+static int ssi_debug_port_show(struct seq_file *m, void *p __maybe_unused)
+{
+	struct hsi_port *port = m->private;
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem	*base = omap_ssi->sys;
+	unsigned int ch;
+
+	pm_runtime_get_sync(omap_port->pdev);
+	if (omap_port->wake_irq > 0)
+		seq_printf(m, "CAWAKE\t\t: %d\n", ssi_wakein(port));
+	seq_printf(m, "WAKE\t\t: 0x%08x\n",
+				readl(base + SSI_WAKE_REG(port->num)));
+	seq_printf(m, "MPU_ENABLE_IRQ%d\t: 0x%08x\n", 0,
+			readl(base + SSI_MPU_ENABLE_REG(port->num, 0)));
+	seq_printf(m, "MPU_STATUS_IRQ%d\t: 0x%08x\n", 0,
+			readl(base + SSI_MPU_STATUS_REG(port->num, 0)));
+	/* SST */
+	base = omap_port->sst_base;
+	seq_puts(m, "\nSST\n===\n");
+	seq_printf(m, "ID SST\t\t: 0x%08x\n",
+				readl(base + SSI_SST_ID_REG));
+	seq_printf(m, "MODE\t\t: 0x%08x\n",
+				readl(base + SSI_SST_MODE_REG));
+	seq_printf(m, "FRAMESIZE\t: 0x%08x\n",
+				readl(base + SSI_SST_FRAMESIZE_REG));
+	seq_printf(m, "DIVISOR\t\t: 0x%08x\n",
+				readl(base + SSI_SST_DIVISOR_REG));
+	seq_printf(m, "CHANNELS\t: 0x%08x\n",
+				readl(base + SSI_SST_CHANNELS_REG));
+	seq_printf(m, "ARBMODE\t\t: 0x%08x\n",
+				readl(base + SSI_SST_ARBMODE_REG));
+	seq_printf(m, "TXSTATE\t\t: 0x%08x\n",
+				readl(base + SSI_SST_TXSTATE_REG));
+	seq_printf(m, "BUFSTATE\t: 0x%08x\n",
+				readl(base + SSI_SST_BUFSTATE_REG));
+	seq_printf(m, "BREAK\t\t: 0x%08x\n",
+				readl(base + SSI_SST_BREAK_REG));
+	for (ch = 0; ch < omap_port->channels; ch++) {
+		seq_printf(m, "BUFFER_CH%d\t: 0x%08x\n", ch,
+				readl(base + SSI_SST_BUFFER_CH_REG(ch)));
+	}
+	/* SSR */
+	base = omap_port->ssr_base;
+	seq_puts(m, "\nSSR\n===\n");
+	seq_printf(m, "ID SSR\t\t: 0x%08x\n",
+				readl(base + SSI_SSR_ID_REG));
+	seq_printf(m, "MODE\t\t: 0x%08x\n",
+				readl(base + SSI_SSR_MODE_REG));
+	seq_printf(m, "FRAMESIZE\t: 0x%08x\n",
+				readl(base + SSI_SSR_FRAMESIZE_REG));
+	seq_printf(m, "CHANNELS\t: 0x%08x\n",
+				readl(base + SSI_SSR_CHANNELS_REG));
+	seq_printf(m, "TIMEOUT\t\t: 0x%08x\n",
+				readl(base + SSI_SSR_TIMEOUT_REG));
+	seq_printf(m, "RXSTATE\t\t: 0x%08x\n",
+				readl(base + SSI_SSR_RXSTATE_REG));
+	seq_printf(m, "BUFSTATE\t: 0x%08x\n",
+				readl(base + SSI_SSR_BUFSTATE_REG));
+	seq_printf(m, "BREAK\t\t: 0x%08x\n",
+				readl(base + SSI_SSR_BREAK_REG));
+	seq_printf(m, "ERROR\t\t: 0x%08x\n",
+				readl(base + SSI_SSR_ERROR_REG));
+	seq_printf(m, "ERRORACK\t: 0x%08x\n",
+				readl(base + SSI_SSR_ERRORACK_REG));
+	for (ch = 0; ch < omap_port->channels; ch++) {
+		seq_printf(m, "BUFFER_CH%d\t: 0x%08x\n", ch,
+				readl(base + SSI_SSR_BUFFER_CH_REG(ch)));
+	}
+	pm_runtime_put_sync(omap_port->pdev);
+
+	return 0;
+}
+
+static int ssi_port_regs_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ssi_debug_port_show, inode->i_private);
+}
+
+static const struct file_operations ssi_port_regs_fops = {
+	.open		= ssi_port_regs_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static int ssi_div_get(void *data, u64 *val)
+{
+	struct hsi_port *port = data;
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+
+	pm_runtime_get_sync(omap_port->pdev);
+	*val = readl(omap_port->sst_base + SSI_SST_DIVISOR_REG);
+	pm_runtime_put_sync(omap_port->pdev);
+
+	return 0;
+}
+
+static int ssi_div_set(void *data, u64 val)
+{
+	struct hsi_port *port = data;
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+
+	if (val > 127)
+		return -EINVAL;
+
+	pm_runtime_get_sync(omap_port->pdev);
+	writel(val, omap_port->sst_base + SSI_SST_DIVISOR_REG);
+	omap_port->sst.divisor = val;
+	pm_runtime_put_sync(omap_port->pdev);
+
+	return 0;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(ssi_sst_div_fops, ssi_div_get, ssi_div_set, "%llu\n");
+
+static int __init ssi_debug_add_port(struct omap_ssi_port *omap_port,
+				     struct dentry *dir)
+{
+	struct hsi_port *port = to_hsi_port(omap_port->dev);
+
+	dir = debugfs_create_dir(dev_name(omap_port->dev), dir);
+	if (IS_ERR(dir))
+		return PTR_ERR(dir);
+	omap_port->dir = dir;
+	debugfs_create_file("regs", S_IRUGO, dir, port, &ssi_port_regs_fops);
+	dir = debugfs_create_dir("sst", dir);
+	if (IS_ERR(dir))
+		return PTR_ERR(dir);
+	debugfs_create_file("divisor", S_IRUGO | S_IWUSR, dir, port,
+			    &ssi_sst_div_fops);
+
+	return 0;
+}
+#endif
+
+static int ssi_claim_lch(struct hsi_msg *msg)
+{
+
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	int lch;
+
+	for (lch = 0; lch < SSI_MAX_GDD_LCH; lch++)
+		if (!omap_ssi->gdd_trn[lch].msg) {
+			omap_ssi->gdd_trn[lch].msg = msg;
+			omap_ssi->gdd_trn[lch].sg = msg->sgt.sgl;
+			return lch;
+		}
+
+	return -EBUSY;
+}
+
+static int ssi_start_dma(struct hsi_msg *msg, int lch)
+{
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem *gdd = omap_ssi->gdd;
+	int err;
+	u16 csdp;
+	u16 ccr;
+	u32 s_addr;
+	u32 d_addr;
+	u32 tmp;
+
+	if (msg->ttype == HSI_MSG_READ) {
+		err = dma_map_sg(&ssi->device, msg->sgt.sgl, msg->sgt.nents,
+							DMA_FROM_DEVICE);
+		if (err < 0) {
+			dev_dbg(&ssi->device, "DMA map SG failed !\n");
+			return err;
+		}
+		csdp = SSI_DST_BURST_4x32_BIT | SSI_DST_MEMORY_PORT |
+			SSI_SRC_SINGLE_ACCESS0 | SSI_SRC_PERIPHERAL_PORT |
+			SSI_DATA_TYPE_S32;
+		ccr = msg->channel + 0x10 + (port->num * 8); /* Sync */
+		ccr |= SSI_DST_AMODE_POSTINC | SSI_SRC_AMODE_CONST |
+			SSI_CCR_ENABLE;
+		s_addr = omap_port->ssr_dma +
+					SSI_SSR_BUFFER_CH_REG(msg->channel);
+		d_addr = sg_dma_address(msg->sgt.sgl);
+	} else {
+		err = dma_map_sg(&ssi->device, msg->sgt.sgl, msg->sgt.nents,
+							DMA_TO_DEVICE);
+		if (err < 0) {
+			dev_dbg(&ssi->device, "DMA map SG failed !\n");
+			return err;
+		}
+		csdp = SSI_SRC_BURST_4x32_BIT | SSI_SRC_MEMORY_PORT |
+			SSI_DST_SINGLE_ACCESS0 | SSI_DST_PERIPHERAL_PORT |
+			SSI_DATA_TYPE_S32;
+		ccr = (msg->channel + 1 + (port->num * 8)) & 0xf; /* Sync */
+		ccr |= SSI_SRC_AMODE_POSTINC | SSI_DST_AMODE_CONST |
+			SSI_CCR_ENABLE;
+		s_addr = sg_dma_address(msg->sgt.sgl);
+		d_addr = omap_port->sst_dma +
+					SSI_SST_BUFFER_CH_REG(msg->channel);
+	}
+	dev_dbg(&ssi->device, "lch %d cdsp %08x ccr %04x s_addr %08x d_addr %08x\n",
+		lch, csdp, ccr, s_addr, d_addr);
+
+	/* Hold clocks during the transfer */
+	pm_runtime_get_sync(omap_port->pdev);
+
+	writew_relaxed(csdp, gdd + SSI_GDD_CSDP_REG(lch));
+	writew_relaxed(SSI_BLOCK_IE | SSI_TOUT_IE, gdd + SSI_GDD_CICR_REG(lch));
+	writel_relaxed(d_addr, gdd + SSI_GDD_CDSA_REG(lch));
+	writel_relaxed(s_addr, gdd + SSI_GDD_CSSA_REG(lch));
+	writew_relaxed(SSI_BYTES_TO_FRAMES(msg->sgt.sgl->length),
+						gdd + SSI_GDD_CEN_REG(lch));
+
+	spin_lock_bh(&omap_ssi->lock);
+	tmp = readl(omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	tmp |= SSI_GDD_LCH(lch);
+	writel_relaxed(tmp, omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	spin_unlock_bh(&omap_ssi->lock);
+	writew(ccr, gdd + SSI_GDD_CCR_REG(lch));
+	msg->status = HSI_STATUS_PROCEEDING;
+
+	return 0;
+}
+
+static int ssi_start_pio(struct hsi_msg *msg)
+{
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	u32 val;
+
+	pm_runtime_get_sync(omap_port->pdev);
+	if (msg->ttype == HSI_MSG_WRITE) {
+		val = SSI_DATAACCEPT(msg->channel);
+		/* Hold clocks for pio writes */
+		pm_runtime_get_sync(omap_port->pdev);
+	} else {
+		val = SSI_DATAAVAILABLE(msg->channel) | SSI_ERROROCCURED;
+	}
+	dev_dbg(&port->device, "Single %s transfer\n",
+						msg->ttype ? "write" : "read");
+	val |= readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	writel(val, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	pm_runtime_put_sync(omap_port->pdev);
+	msg->actual_len = 0;
+	msg->status = HSI_STATUS_PROCEEDING;
+
+	return 0;
+}
+
+static int ssi_start_transfer(struct list_head *queue)
+{
+	struct hsi_msg *msg;
+	int lch = -1;
+
+	if (list_empty(queue))
+		return 0;
+	msg = list_first_entry(queue, struct hsi_msg, link);
+	if (msg->status != HSI_STATUS_QUEUED)
+		return 0;
+	if ((msg->sgt.nents) && (msg->sgt.sgl->length > sizeof(u32)))
+		lch = ssi_claim_lch(msg);
+	if (lch >= 0)
+		return ssi_start_dma(msg, lch);
+	else
+		return ssi_start_pio(msg);
+}
+
+static int ssi_async_break(struct hsi_msg *msg)
+{
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	int err = 0;
+	u32 tmp;
+
+	pm_runtime_get_sync(omap_port->pdev);
+	if (msg->ttype == HSI_MSG_WRITE) {
+		if (omap_port->sst.mode != SSI_MODE_FRAME) {
+			err = -EINVAL;
+			goto out;
+		}
+		writel(1, omap_port->sst_base + SSI_SST_BREAK_REG);
+		msg->status = HSI_STATUS_COMPLETED;
+		msg->complete(msg);
+	} else {
+		if (omap_port->ssr.mode != SSI_MODE_FRAME) {
+			err = -EINVAL;
+			goto out;
+		}
+		spin_lock_bh(&omap_port->lock);
+		tmp = readl(omap_ssi->sys +
+					SSI_MPU_ENABLE_REG(port->num, 0));
+		writel(tmp | SSI_BREAKDETECTED,
+			omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+		msg->status = HSI_STATUS_PROCEEDING;
+		list_add_tail(&msg->link, &omap_port->brkqueue);
+		spin_unlock_bh(&omap_port->lock);
+	}
+out:
+	pm_runtime_put_sync(omap_port->pdev);
+
+	return err;
+}
+
+static int ssi_async(struct hsi_msg *msg)
+{
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct list_head *queue;
+	int err = 0;
+
+	BUG_ON(!msg);
+
+	if (msg->sgt.nents > 1)
+		return -ENOSYS; /* TODO: Add sg support */
+
+	if (msg->break_frame)
+		return ssi_async_break(msg);
+
+	if (msg->ttype) {
+		BUG_ON(msg->channel >= omap_port->sst.channels);
+		queue = &omap_port->txqueue[msg->channel];
+	} else {
+		BUG_ON(msg->channel >= omap_port->ssr.channels);
+		queue = &omap_port->rxqueue[msg->channel];
+	}
+	msg->status = HSI_STATUS_QUEUED;
+	spin_lock_bh(&omap_port->lock);
+	list_add_tail(&msg->link, queue);
+	err = ssi_start_transfer(queue);
+	if (err < 0) {
+		list_del(&msg->link);
+		msg->status = HSI_STATUS_ERROR;
+	}
+	spin_unlock_bh(&omap_port->lock);
+	dev_dbg(&port->device, "msg status %d ttype %d ch %d\n",
+				msg->status, msg->ttype, msg->channel);
+
+	return err;
+}
+
+static u32 ssi_calculate_div(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	u32 tx_fckrate = (u32) omap_ssi->fck_rate;
+
+	/* / 2 : SSI TX clock is always half of the SSI functional clock */
+	tx_fckrate >>= 1;
+	/* Round down when tx_fckrate % omap_ssi->max_speed == 0 */
+	tx_fckrate--;
+	dev_dbg(&ssi->device, "TX div %d for fck_rate %lu Khz speed %d Kb/s\n",
+		tx_fckrate / omap_ssi->max_speed, omap_ssi->fck_rate,
+		omap_ssi->max_speed);
+
+	return tx_fckrate / omap_ssi->max_speed;
+}
+
+static void ssi_flush_queue(struct list_head *queue, struct hsi_client *cl)
+{
+	struct list_head *node, *tmp;
+	struct hsi_msg *msg;
+
+	list_for_each_safe(node, tmp, queue) {
+		msg = list_entry(node, struct hsi_msg, link);
+		if ((cl) && (cl != msg->cl))
+			continue;
+		list_del(node);
+		pr_debug("flush queue: ch %d, msg %p len %d type %d ctxt %p\n",
+			msg->channel, msg, msg->sgt.sgl->length,
+					msg->ttype, msg->context);
+		if (msg->destructor)
+			msg->destructor(msg);
+		else
+			hsi_free_msg(msg);
+	}
+}
+
+static int ssi_setup(struct hsi_client *cl)
+{
+	struct hsi_port *port = to_hsi_port(cl->device.parent);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem *sst = omap_port->sst_base;
+	void __iomem *ssr = omap_port->ssr_base;
+	u32 div;
+	u32 val;
+	int err = 0;
+
+	pm_runtime_get_sync(omap_port->pdev);
+	spin_lock_bh(&omap_port->lock);
+	if (cl->tx_cfg.speed)
+		omap_ssi->max_speed = cl->tx_cfg.speed;
+	div = ssi_calculate_div(ssi);
+	if (div > SSI_MAX_DIVISOR) {
+		dev_err(&cl->device, "Invalid TX speed %d Mb/s (div %d)\n",
+						cl->tx_cfg.speed, div);
+		err = -EINVAL;
+		goto out;
+	}
+	/* Set TX/RX module to sleep to stop TX/RX during cfg update */
+	writel_relaxed(SSI_MODE_SLEEP, sst + SSI_SST_MODE_REG);
+	writel_relaxed(SSI_MODE_SLEEP, ssr + SSI_SSR_MODE_REG);
+	/* Flush posted write */
+	val = readl(ssr + SSI_SSR_MODE_REG);
+	/* TX */
+	writel_relaxed(31, sst + SSI_SST_FRAMESIZE_REG);
+	writel_relaxed(div, sst + SSI_SST_DIVISOR_REG);
+	writel_relaxed(cl->tx_cfg.num_hw_channels, sst + SSI_SST_CHANNELS_REG);
+	writel_relaxed(cl->tx_cfg.arb_mode, sst + SSI_SST_ARBMODE_REG);
+	writel_relaxed(cl->tx_cfg.mode, sst + SSI_SST_MODE_REG);
+	/* RX */
+	writel_relaxed(31, ssr + SSI_SSR_FRAMESIZE_REG);
+	writel_relaxed(cl->rx_cfg.num_hw_channels, ssr + SSI_SSR_CHANNELS_REG);
+	writel_relaxed(0, ssr + SSI_SSR_TIMEOUT_REG);
+	/* Cleanup the break queue if we leave FRAME mode */
+	if ((omap_port->ssr.mode == SSI_MODE_FRAME) &&
+		(cl->rx_cfg.mode != SSI_MODE_FRAME))
+		ssi_flush_queue(&omap_port->brkqueue, cl);
+	writel_relaxed(cl->rx_cfg.mode, ssr + SSI_SSR_MODE_REG);
+	omap_port->channels = max(cl->rx_cfg.num_hw_channels,
+				  cl->tx_cfg.num_hw_channels);
+	/* Shadow registering for OFF mode */
+	/* SST */
+	omap_port->sst.divisor = div;
+	omap_port->sst.frame_size = 31;
+	omap_port->sst.channels = cl->tx_cfg.num_hw_channels;
+	omap_port->sst.arb_mode = cl->tx_cfg.arb_mode;
+	omap_port->sst.mode = cl->tx_cfg.mode;
+	/* SSR */
+	omap_port->ssr.frame_size = 31;
+	omap_port->ssr.timeout = 0;
+	omap_port->ssr.channels = cl->rx_cfg.num_hw_channels;
+	omap_port->ssr.mode = cl->rx_cfg.mode;
+out:
+	spin_unlock_bh(&omap_port->lock);
+	pm_runtime_put_sync(omap_port->pdev);
+
+	return err;
+}
+
+static int ssi_flush(struct hsi_client *cl)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_msg *msg;
+	void __iomem *sst = omap_port->sst_base;
+	void __iomem *ssr = omap_port->ssr_base;
+	unsigned int i;
+	u32 err;
+
+	pm_runtime_get_sync(omap_port->pdev);
+	spin_lock_bh(&omap_port->lock);
+	/* Stop all DMA transfers */
+	for (i = 0; i < SSI_MAX_GDD_LCH; i++) {
+		msg = omap_ssi->gdd_trn[i].msg;
+		if (!msg || (port != hsi_get_port(msg->cl)))
+			continue;
+		writew_relaxed(0, omap_ssi->gdd + SSI_GDD_CCR_REG(i));
+		if (msg->ttype == HSI_MSG_READ)
+			pm_runtime_put_sync(omap_port->pdev);
+		omap_ssi->gdd_trn[i].msg = NULL;
+	}
+	/* Flush all SST buffers */
+	writel_relaxed(0, sst + SSI_SST_BUFSTATE_REG);
+	writel_relaxed(0, sst + SSI_SST_TXSTATE_REG);
+	/* Flush all SSR buffers */
+	writel_relaxed(0, ssr + SSI_SSR_RXSTATE_REG);
+	writel_relaxed(0, ssr + SSI_SSR_BUFSTATE_REG);
+	/* Flush all errors */
+	err = readl(ssr + SSI_SSR_ERROR_REG);
+	writel_relaxed(err, ssr + SSI_SSR_ERRORACK_REG);
+	/* Flush break */
+	writel_relaxed(0, ssr + SSI_SSR_BREAK_REG);
+	/* Clear interrupts */
+	writel_relaxed(0, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	writel_relaxed(0xffffff00,
+			omap_ssi->sys + SSI_MPU_STATUS_REG(port->num, 0));
+	writel_relaxed(0, omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	writel(0xff, omap_ssi->sys + SSI_GDD_MPU_IRQ_STATUS_REG);
+	/* Dequeue all pending requests */
+	for (i = 0; i < omap_port->channels; i++) {
+		/* Release write clocks */
+		if (!list_empty(&omap_port->txqueue[i]))
+			pm_runtime_put_sync(omap_port->pdev);
+		ssi_flush_queue(&omap_port->txqueue[i], NULL);
+		ssi_flush_queue(&omap_port->rxqueue[i], NULL);
+	}
+	ssi_flush_queue(&omap_port->brkqueue, NULL);
+	spin_unlock_bh(&omap_port->lock);
+	pm_runtime_put_sync(omap_port->pdev);
+
+	return 0;
+}
+
+static int ssi_start_tx(struct hsi_client *cl)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	dev_dbg(&port->device, "Wake out high %d\n", omap_port->wk_refcount);
+
+	spin_lock_bh(&omap_port->wk_lock);
+	if (omap_port->wk_refcount++) {
+		spin_unlock_bh(&omap_port->wk_lock);
+		return 0;
+	}
+	pm_runtime_get_sync(omap_port->pdev); /* Grab clocks */
+	writel(SSI_WAKE(0), omap_ssi->sys + SSI_SET_WAKE_REG(port->num));
+	spin_unlock_bh(&omap_port->wk_lock);
+
+	return 0;
+}
+
+static int ssi_stop_tx(struct hsi_client *cl)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	dev_dbg(&port->device, "Wake out low %d\n", omap_port->wk_refcount);
+
+	spin_lock_bh(&omap_port->wk_lock);
+	BUG_ON(!omap_port->wk_refcount);
+	if (--omap_port->wk_refcount) {
+		spin_unlock_bh(&omap_port->wk_lock);
+		return 0;
+	}
+	writel(SSI_WAKE(0), omap_ssi->sys + SSI_CLEAR_WAKE_REG(port->num));
+	pm_runtime_put_sync(omap_port->pdev); /* Release clocks */
+	spin_unlock_bh(&omap_port->wk_lock);
+
+	return 0;
+}
+
+static void ssi_transfer(struct omap_ssi_port *omap_port,
+							struct list_head *queue)
+{
+	struct hsi_msg *msg;
+	int err = -1;
+
+	spin_lock_bh(&omap_port->lock);
+	while (err < 0) {
+		err = ssi_start_transfer(queue);
+		if (err < 0) {
+			msg = list_first_entry(queue, struct hsi_msg, link);
+			msg->status = HSI_STATUS_ERROR;
+			msg->actual_len = 0;
+			list_del(&msg->link);
+			spin_unlock_bh(&omap_port->lock);
+			msg->complete(msg);
+			spin_lock_bh(&omap_port->lock);
+		}
+	}
+	spin_unlock_bh(&omap_port->lock);
+}
+
+static void ssi_cleanup_queues(struct hsi_client *cl)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_msg *msg;
+	unsigned int i;
+	u32 rxbufstate = 0;
+	u32 txbufstate = 0;
+	u32 status = SSI_ERROROCCURED;
+	u32 tmp;
+
+	ssi_flush_queue(&omap_port->brkqueue, cl);
+	if (list_empty(&omap_port->brkqueue))
+		status |= SSI_BREAKDETECTED;
+
+	for (i = 0; i < omap_port->channels; i++) {
+		if (list_empty(&omap_port->txqueue[i]))
+			continue;
+		msg = list_first_entry(&omap_port->txqueue[i], struct hsi_msg,
+									link);
+		if ((msg->cl == cl) && (msg->status == HSI_STATUS_PROCEEDING)) {
+			txbufstate |= (1 << i);
+			status |= SSI_DATAACCEPT(i);
+			/* Release the clocks writes, also GDD ones */
+			pm_runtime_put_sync(omap_port->pdev);
+		}
+		ssi_flush_queue(&omap_port->txqueue[i], cl);
+	}
+	for (i = 0; i < omap_port->channels; i++) {
+		if (list_empty(&omap_port->rxqueue[i]))
+			continue;
+		msg = list_first_entry(&omap_port->rxqueue[i], struct hsi_msg,
+									link);
+		if ((msg->cl == cl) && (msg->status == HSI_STATUS_PROCEEDING)) {
+			rxbufstate |= (1 << i);
+			status |= SSI_DATAAVAILABLE(i);
+		}
+		ssi_flush_queue(&omap_port->rxqueue[i], cl);
+		/* Check if we keep the error detection interrupt armed */
+		if (!list_empty(&omap_port->rxqueue[i]))
+			status &= ~SSI_ERROROCCURED;
+	}
+	/* Cleanup write buffers */
+	tmp = readl(omap_port->sst_base + SSI_SST_BUFSTATE_REG);
+	tmp &= ~txbufstate;
+	writel_relaxed(tmp, omap_port->sst_base + SSI_SST_BUFSTATE_REG);
+	/* Cleanup read buffers */
+	tmp = readl(omap_port->ssr_base + SSI_SSR_BUFSTATE_REG);
+	tmp &= ~rxbufstate;
+	writel_relaxed(tmp, omap_port->ssr_base + SSI_SSR_BUFSTATE_REG);
+	/* Disarm and ack pending interrupts */
+	tmp = readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	tmp &= ~status;
+	writel_relaxed(tmp, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	writel_relaxed(status, omap_ssi->sys +
+		SSI_MPU_STATUS_REG(port->num, 0));
+}
+
+static void ssi_cleanup_gdd(struct hsi_controller *ssi, struct hsi_client *cl)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_msg *msg;
+	unsigned int i;
+	u32 val = 0;
+	u32 tmp;
+
+	for (i = 0; i < SSI_MAX_GDD_LCH; i++) {
+		msg = omap_ssi->gdd_trn[i].msg;
+		if ((!msg) || (msg->cl != cl))
+			continue;
+		writew_relaxed(0, omap_ssi->gdd + SSI_GDD_CCR_REG(i));
+		val |= (1 << i);
+		/*
+		 * Clock references for write will be handled in
+		 * ssi_cleanup_queues
+		 */
+		if (msg->ttype == HSI_MSG_READ)
+			pm_runtime_put_sync(omap_port->pdev);
+		omap_ssi->gdd_trn[i].msg = NULL;
+	}
+	tmp = readl_relaxed(omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	tmp &= ~val;
+	writel_relaxed(tmp, omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	writel(val, omap_ssi->sys + SSI_GDD_MPU_IRQ_STATUS_REG);
+}
+
+static int ssi_set_port_mode(struct omap_ssi_port *omap_port, u32 mode)
+{
+	writel(mode, omap_port->sst_base + SSI_SST_MODE_REG);
+	writel(mode, omap_port->ssr_base + SSI_SSR_MODE_REG);
+	/* OCP barrier */
+	mode = readl(omap_port->ssr_base + SSI_SSR_MODE_REG);
+
+	return 0;
+}
+
+static int ssi_release(struct hsi_client *cl)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+
+	spin_lock_bh(&omap_port->lock);
+	pm_runtime_get_sync(omap_port->pdev);
+	/* Stop all the pending DMA requests for that client */
+	ssi_cleanup_gdd(ssi, cl);
+	/* Now cleanup all the queues */
+	ssi_cleanup_queues(cl);
+	pm_runtime_put_sync(omap_port->pdev);
+	/* If it is the last client of the port, do extra checks and cleanup */
+	if (port->claimed <= 1) {
+		/*
+		 * Drop the clock reference for the incoming wake line
+		 * if it is still kept high by the other side.
+		 */
+		if (omap_port->wkin_cken) {
+			pm_runtime_put_sync(omap_port->pdev);
+			omap_port->wkin_cken = 0;
+		}
+		pm_runtime_get_sync(omap_port->pdev);
+		/* Stop any SSI TX/RX without a client */
+		ssi_set_port_mode(omap_port, SSI_MODE_SLEEP);
+		omap_port->sst.mode = SSI_MODE_SLEEP;
+		omap_port->ssr.mode = SSI_MODE_SLEEP;
+		pm_runtime_put_sync(omap_port->pdev);
+		WARN_ON(omap_port->wk_refcount != 0);
+	}
+	spin_unlock_bh(&omap_port->lock);
+
+	return 0;
+}
+
+
+
+static void ssi_error(struct hsi_port *port)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_msg *msg;
+	unsigned int i;
+	u32 err;
+	u32 val;
+	u32 tmp;
+
+	/* ACK error */
+	err = readl(omap_port->ssr_base + SSI_SSR_ERROR_REG);
+	dev_err(&port->device, "SSI error: 0x%02x\n", err);
+	if (!err) {
+		dev_dbg(&port->device, "spurious SSI error ignored!\n");
+		return;
+	}
+	spin_lock(&omap_ssi->lock);
+	/* Cancel all GDD read transfers */
+	for (i = 0, val = 0; i < SSI_MAX_GDD_LCH; i++) {
+		msg = omap_ssi->gdd_trn[i].msg;
+		if ((msg) && (msg->ttype == HSI_MSG_READ)) {
+			writew_relaxed(0, omap_ssi->gdd + SSI_GDD_CCR_REG(i));
+			val |= (1 << i);
+			omap_ssi->gdd_trn[i].msg = NULL;
+		}
+	}
+	tmp = readl(omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	tmp &= ~val;
+	writel_relaxed(tmp, omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	spin_unlock(&omap_ssi->lock);
+	/* Cancel all PIO read transfers */
+	spin_lock(&omap_port->lock);
+	tmp = readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	tmp &= 0xfeff00ff; /* Disable error & all dataavailable interrupts */
+	writel_relaxed(tmp, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	/* ACK error */
+	writel_relaxed(err, omap_port->ssr_base + SSI_SSR_ERRORACK_REG);
+	writel_relaxed(SSI_ERROROCCURED,
+			omap_ssi->sys + SSI_MPU_STATUS_REG(port->num, 0));
+	/* Signal the error all current pending read requests */
+	for (i = 0; i < omap_port->channels; i++) {
+		if (list_empty(&omap_port->rxqueue[i]))
+			continue;
+		msg = list_first_entry(&omap_port->rxqueue[i], struct hsi_msg,
+									link);
+		list_del(&msg->link);
+		msg->status = HSI_STATUS_ERROR;
+		spin_unlock(&omap_port->lock);
+		msg->complete(msg);
+		/* Now restart queued reads if any */
+		ssi_transfer(omap_port, &omap_port->rxqueue[i]);
+		spin_lock(&omap_port->lock);
+	}
+	spin_unlock(&omap_port->lock);
+}
+
+static void ssi_break_complete(struct hsi_port *port)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_msg *msg;
+	struct hsi_msg *tmp;
+	u32 val;
+
+	dev_dbg(&port->device, "HWBREAK received\n");
+
+	spin_lock(&omap_port->lock);
+	val = readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	val &= ~SSI_BREAKDETECTED;
+	writel_relaxed(val, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	writel_relaxed(0, omap_port->ssr_base + SSI_SSR_BREAK_REG);
+	writel(SSI_BREAKDETECTED,
+			omap_ssi->sys + SSI_MPU_STATUS_REG(port->num, 0));
+	spin_unlock(&omap_port->lock);
+
+	list_for_each_entry_safe(msg, tmp, &omap_port->brkqueue, link) {
+		msg->status = HSI_STATUS_COMPLETED;
+		spin_lock(&omap_port->lock);
+		list_del(&msg->link);
+		spin_unlock(&omap_port->lock);
+		msg->complete(msg);
+	}
+
+}
+
+static void ssi_pio_complete(struct hsi_port *port, struct list_head *queue)
+{
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_msg *msg;
+	u32 *buf;
+	u32 reg;
+	u32 val;
+
+	spin_lock(&omap_port->lock);
+	msg = list_first_entry(queue, struct hsi_msg, link);
+	if ((!msg->sgt.nents) || (!msg->sgt.sgl->length)) {
+		msg->actual_len = 0;
+		msg->status = HSI_STATUS_PENDING;
+	}
+	if (msg->ttype == HSI_MSG_WRITE)
+		val = SSI_DATAACCEPT(msg->channel);
+	else
+		val = SSI_DATAAVAILABLE(msg->channel);
+	if (msg->status == HSI_STATUS_PROCEEDING) {
+		buf = sg_virt(msg->sgt.sgl) + msg->actual_len;
+		if (msg->ttype == HSI_MSG_WRITE)
+			writel(*buf, omap_port->sst_base +
+					SSI_SST_BUFFER_CH_REG(msg->channel));
+		 else
+			*buf = readl(omap_port->ssr_base +
+					SSI_SSR_BUFFER_CH_REG(msg->channel));
+		dev_dbg(&port->device, "ch %d ttype %d 0x%08x\n", msg->channel,
+							msg->ttype, *buf);
+		msg->actual_len += sizeof(*buf);
+		if (msg->actual_len >= msg->sgt.sgl->length)
+			msg->status = HSI_STATUS_COMPLETED;
+		/*
+		 * Wait for the last written frame to be really sent before
+		 * we call the complete callback
+		 */
+		if ((msg->status == HSI_STATUS_PROCEEDING) ||
+				((msg->status == HSI_STATUS_COMPLETED) &&
+					(msg->ttype == HSI_MSG_WRITE))) {
+			writel(val, omap_ssi->sys +
+					SSI_MPU_STATUS_REG(port->num, 0));
+			spin_unlock(&omap_port->lock);
+
+			return;
+		}
+
+	}
+	/* Transfer completed at this point */
+	reg = readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	if (msg->ttype == HSI_MSG_WRITE) {
+		/* Release clocks for write transfer */
+		pm_runtime_put_sync(omap_port->pdev);
+	}
+	reg &= ~val;
+	writel_relaxed(reg, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	writel_relaxed(val, omap_ssi->sys + SSI_MPU_STATUS_REG(port->num, 0));
+	list_del(&msg->link);
+	spin_unlock(&omap_port->lock);
+	msg->complete(msg);
+	ssi_transfer(omap_port, queue);
+}
+
+static void ssi_pio_tasklet(unsigned long ssi_port)
+{
+	struct hsi_port *port = (struct hsi_port *)ssi_port;
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem *sys = omap_ssi->sys;
+	unsigned int ch;
+	u32 status_reg;
+
+	pm_runtime_get_sync(omap_port->pdev);
+	status_reg = readl(sys + SSI_MPU_STATUS_REG(port->num, 0));
+	status_reg &= readl(sys + SSI_MPU_ENABLE_REG(port->num, 0));
+
+	for (ch = 0; ch < omap_port->channels; ch++) {
+		if (status_reg & SSI_DATAACCEPT(ch))
+			ssi_pio_complete(port, &omap_port->txqueue[ch]);
+		if (status_reg & SSI_DATAAVAILABLE(ch))
+			ssi_pio_complete(port, &omap_port->rxqueue[ch]);
+	}
+	if (status_reg & SSI_BREAKDETECTED)
+		ssi_break_complete(port);
+	if (status_reg & SSI_ERROROCCURED)
+		ssi_error(port);
+
+	status_reg = readl(sys + SSI_MPU_STATUS_REG(port->num, 0));
+	status_reg &= readl(sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	pm_runtime_put_sync(omap_port->pdev);
+
+	if (status_reg)
+		tasklet_hi_schedule(&omap_port->pio_tasklet);
+	else
+		enable_irq(omap_port->irq);
+}
+
+static irqreturn_t ssi_pio_isr(int irq, void *port)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+
+	tasklet_hi_schedule(&omap_port->pio_tasklet);
+	disable_irq_nosync(irq);
+
+	return IRQ_HANDLED;
+}
+
+static void ssi_wake_tasklet(unsigned long ssi_port)
+{
+	struct hsi_port *port = (struct hsi_port *)ssi_port;
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	if (ssi_wakein(port)) {
+		/**
+		 * We can have a quick High-Low-High transition in the line.
+		 * In such a case if we have long interrupt latencies,
+		 * we can miss the low event or get twice a high event.
+		 * This workaround will avoid breaking the clock reference
+		 * count when such a situation ocurrs.
+		 */
+		spin_lock(&omap_port->lock);
+		if (!omap_port->wkin_cken) {
+			omap_port->wkin_cken = 1;
+			pm_runtime_get_sync(omap_port->pdev);
+		}
+		spin_unlock(&omap_port->lock);
+		dev_dbg(&ssi->device, "Wake in high\n");
+		if (omap_port->wktest) { /* FIXME: HACK ! To be removed */
+			writel(SSI_WAKE(0),
+				omap_ssi->sys + SSI_SET_WAKE_REG(port->num));
+		}
+		hsi_event(port, HSI_EVENT_START_RX);
+	} else {
+		dev_dbg(&ssi->device, "Wake in low\n");
+		if (omap_port->wktest) { /* FIXME: HACK ! To be removed */
+			writel(SSI_WAKE(0),
+				omap_ssi->sys + SSI_CLEAR_WAKE_REG(port->num));
+		}
+		hsi_event(port, HSI_EVENT_STOP_RX);
+		spin_lock(&omap_port->lock);
+		if (omap_port->wkin_cken) {
+			pm_runtime_put_sync(omap_port->pdev);
+			omap_port->wkin_cken = 0;
+		}
+		spin_unlock(&omap_port->lock);
+	}
+}
+
+static irqreturn_t ssi_wake_isr(int irq __maybe_unused, void *ssi_port)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(ssi_port);
+
+	tasklet_hi_schedule(&omap_port->wake_tasklet);
+
+	return IRQ_HANDLED;
+}
+
+static int __init ssi_port_irq(struct hsi_port *port,
+						struct platform_device *pd)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	int err;
+
+	omap_port->irq = platform_get_irq(pd, 0);
+	if (omap_port->irq < 0) {
+		dev_err(&port->device, "Port IRQ resource missing\n");
+		return omap_port->irq;
+	}
+	tasklet_init(&omap_port->pio_tasklet, ssi_pio_tasklet,
+							(unsigned long)port);
+	err = devm_request_irq(&port->device, omap_port->irq, ssi_pio_isr,
+						0, "mpu_irq0", port);
+	if (err < 0)
+		dev_err(&port->device, "Request IRQ %d failed (%d)\n",
+							omap_port->irq, err);
+	return err;
+}
+
+static int __init ssi_wake_irq(struct hsi_port *port,
+						struct platform_device *pd)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	int cawake_irq;
+	int err;
+
+	if (omap_port->wake_gpio == -1) {
+		omap_port->wake_irq = -1;
+		return 0;
+	}
+
+	cawake_irq = gpio_to_irq(omap_port->wake_gpio);
+
+	omap_port->wake_irq = cawake_irq;
+	tasklet_init(&omap_port->wake_tasklet, ssi_wake_tasklet,
+							(unsigned long)port);
+	err = devm_request_irq(&port->device, cawake_irq, ssi_wake_isr,
+		IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+							"cawake", port);
+	if (err < 0)
+		dev_err(&port->device, "Request Wake in IRQ %d failed %d\n",
+						cawake_irq, err);
+	err = enable_irq_wake(cawake_irq);
+	if (err < 0)
+		dev_err(&port->device, "Enable wake on the wakeline in irq %d failed %d\n",
+			cawake_irq, err);
+
+	return err;
+}
+
+static void __init ssi_queues_init(struct omap_ssi_port *omap_port)
+{
+	unsigned int ch;
+
+	for (ch = 0; ch < SSI_MAX_CHANNELS; ch++) {
+		INIT_LIST_HEAD(&omap_port->txqueue[ch]);
+		INIT_LIST_HEAD(&omap_port->rxqueue[ch]);
+	}
+	INIT_LIST_HEAD(&omap_port->brkqueue);
+}
+
+static int __init ssi_port_get_iomem(struct platform_device *pd,
+		const char *name, void __iomem **pbase, dma_addr_t *phy)
+{
+	struct hsi_port *port = platform_get_drvdata(pd);
+	struct resource *mem;
+	struct resource *ioarea;
+	void __iomem *base;
+
+	mem = platform_get_resource_byname(pd, IORESOURCE_MEM, name);
+	if (!mem) {
+		dev_err(&pd->dev, "IO memory region missing (%s)\n", name);
+		return -ENXIO;
+	}
+	ioarea = devm_request_mem_region(&port->device, mem->start,
+					resource_size(mem), dev_name(&pd->dev));
+	if (!ioarea) {
+		dev_err(&pd->dev, "%s IO memory region request failed\n",
+								mem->name);
+		return -ENXIO;
+	}
+	base = devm_ioremap(&port->device, mem->start, resource_size(mem));
+	if (!base) {
+		dev_err(&pd->dev, "%s IO remap failed\n", mem->name);
+		return -ENXIO;
+	}
+	*pbase = base;
+
+	if (phy)
+		*phy = mem->start;
+
+	return 0;
+}
+
+static int __init ssi_port_probe(struct platform_device *pd)
+{
+	struct device_node *np = pd->dev.of_node;
+	struct hsi_port *port;
+	struct omap_ssi_port *omap_port;
+	struct hsi_controller *ssi = dev_get_drvdata(pd->dev.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	u32 cawake_gpio = 0;
+	u32 port_id;
+	int err;
+
+	dev_dbg(&pd->dev, "init ssi port...\n");
+
+	err = ref_module(THIS_MODULE, ssi->owner);
+	if (err) {
+		dev_err(&pd->dev, "could not increment parent module refcount (err=%d)\n",
+			err);
+		return -ENODEV;
+	}
+
+	if (!ssi->port || !omap_ssi->port) {
+		dev_err(&pd->dev, "ssi controller not initialized!\n");
+		err = -ENODEV;
+		goto error;
+	}
+
+	/* get id of first uninitialized port in controller */
+	for (port_id = 0; port_id < ssi->num_ports && omap_ssi->port[port_id];
+		port_id++)
+		;
+
+	if (port_id >= ssi->num_ports) {
+		dev_err(&pd->dev, "port id out of range!\n");
+		err = -ENODEV;
+		goto error;
+	}
+
+	port = ssi->port[port_id];
+
+	if (!np) {
+		dev_err(&pd->dev, "missing device tree data\n");
+		err = -EINVAL;
+		goto error;
+	}
+
+	cawake_gpio = of_get_named_gpio(np, "ti,ssi-cawake-gpio", 0);
+	if (cawake_gpio < 0) {
+		dev_err(&pd->dev, "DT data is missing cawake gpio (err=%d)\n",
+			cawake_gpio);
+		err = -ENODEV;
+		goto error;
+	}
+
+	err = devm_gpio_request_one(&port->device, cawake_gpio, GPIOF_DIR_IN,
+		"cawake");
+	if (err) {
+		dev_err(&pd->dev, "could not request cawake gpio (err=%d)!\n",
+			err);
+		err = -ENXIO;
+		goto error;
+	}
+
+	omap_port = devm_kzalloc(&port->device, sizeof(*omap_port), GFP_KERNEL);
+	if (!omap_port) {
+		err = -ENOMEM;
+		goto error;
+	}
+	omap_port->wake_gpio = cawake_gpio;
+	omap_port->pdev = &pd->dev;
+	omap_port->port_id = port_id;
+
+	/* initialize HSI port */
+	port->async	= ssi_async;
+	port->setup	= ssi_setup;
+	port->flush	= ssi_flush;
+	port->start_tx	= ssi_start_tx;
+	port->stop_tx	= ssi_stop_tx;
+	port->release	= ssi_release;
+	hsi_port_set_drvdata(port, omap_port);
+	omap_ssi->port[port_id] = omap_port;
+
+	platform_set_drvdata(pd, port);
+
+	err = ssi_port_get_iomem(pd, "tx", &omap_port->sst_base,
+		&omap_port->sst_dma);
+	if (err < 0)
+		goto error;
+	err = ssi_port_get_iomem(pd, "rx", &omap_port->ssr_base,
+		&omap_port->ssr_dma);
+	if (err < 0)
+		goto error;
+
+	err = ssi_port_irq(port, pd);
+	if (err < 0)
+		goto error;
+	err = ssi_wake_irq(port, pd);
+	if (err < 0)
+		goto error;
+
+	ssi_queues_init(omap_port);
+	spin_lock_init(&omap_port->lock);
+	spin_lock_init(&omap_port->wk_lock);
+	omap_port->dev = &port->device;
+
+	pm_runtime_irq_safe(omap_port->pdev);
+	pm_runtime_enable(omap_port->pdev);
+
+#ifdef CONFIG_DEBUG_FS
+	err = ssi_debug_add_port(omap_port, omap_ssi->dir);
+	if (err < 0) {
+		pm_runtime_disable(omap_port->pdev);
+		goto error;
+	}
+#endif
+
+	hsi_add_clients_from_dt(port, np);
+
+	dev_info(&pd->dev, "ssi port %u successfully initialized (cawake=%d)\n",
+		port_id, cawake_gpio);
+
+	return 0;
+
+error:
+	return err;
+}
+
+static int __exit ssi_port_remove(struct platform_device *pd)
+{
+	struct hsi_port *port = platform_get_drvdata(pd);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+#ifdef CONFIG_DEBUG_FS
+	ssi_debug_remove_port(port);
+#endif
+
+	hsi_port_unregister_clients(port);
+
+	tasklet_kill(&omap_port->wake_tasklet);
+	tasklet_kill(&omap_port->pio_tasklet);
+
+	port->async	= hsi_dummy_msg;
+	port->setup	= hsi_dummy_cl;
+	port->flush	= hsi_dummy_cl;
+	port->start_tx	= hsi_dummy_cl;
+	port->stop_tx	= hsi_dummy_cl;
+	port->release	= hsi_dummy_cl;
+
+	omap_ssi->port[omap_port->port_id] = NULL;
+	platform_set_drvdata(pd, NULL);
+	pm_runtime_disable(&pd->dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_RUNTIME
+static int ssi_save_port_ctx(struct omap_ssi_port *omap_port)
+{
+	struct hsi_port *port = to_hsi_port(omap_port->dev);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	omap_port->sys_mpu_enable = readl(omap_ssi->sys +
+					SSI_MPU_ENABLE_REG(port->num, 0));
+
+	return 0;
+}
+
+static int ssi_restore_port_ctx(struct omap_ssi_port *omap_port)
+{
+	struct hsi_port *port = to_hsi_port(omap_port->dev);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem	*base;
+
+	writel_relaxed(omap_port->sys_mpu_enable,
+			omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+
+	/* SST context */
+	base = omap_port->sst_base;
+	writel_relaxed(omap_port->sst.frame_size, base + SSI_SST_FRAMESIZE_REG);
+	writel_relaxed(omap_port->sst.channels, base + SSI_SST_CHANNELS_REG);
+	writel_relaxed(omap_port->sst.arb_mode, base + SSI_SST_ARBMODE_REG);
+
+	/* SSR context */
+	base = omap_port->ssr_base;
+	writel_relaxed(omap_port->ssr.frame_size, base + SSI_SSR_FRAMESIZE_REG);
+	writel_relaxed(omap_port->ssr.channels, base + SSI_SSR_CHANNELS_REG);
+	writel_relaxed(omap_port->ssr.timeout, base + SSI_SSR_TIMEOUT_REG);
+
+	return 0;
+}
+
+static int ssi_restore_port_mode(struct omap_ssi_port *omap_port)
+{
+	u32 mode;
+
+	writel_relaxed(omap_port->sst.mode,
+				omap_port->sst_base + SSI_SST_MODE_REG);
+	writel_relaxed(omap_port->ssr.mode,
+				omap_port->ssr_base + SSI_SSR_MODE_REG);
+	/* OCP barrier */
+	mode = readl(omap_port->ssr_base + SSI_SSR_MODE_REG);
+
+	return 0;
+}
+
+static int ssi_restore_divisor(struct omap_ssi_port *omap_port)
+{
+	writel_relaxed(omap_port->sst.divisor,
+				omap_port->sst_base + SSI_SST_DIVISOR_REG);
+
+	return 0;
+}
+
+static int omap_ssi_port_runtime_suspend(struct device *dev)
+{
+	struct hsi_port *port = dev_get_drvdata(dev);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	dev_dbg(dev, "port runtime suspend!\n");
+
+	ssi_set_port_mode(omap_port, SSI_MODE_SLEEP);
+	if (omap_ssi->get_loss)
+		omap_port->loss_count =
+				omap_ssi->get_loss(ssi->device.parent);
+	ssi_save_port_ctx(omap_port);
+
+	return 0;
+}
+
+static int omap_ssi_port_runtime_resume(struct device *dev)
+{
+	struct hsi_port *port = dev_get_drvdata(dev);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	dev_dbg(dev, "port runtime resume!\n");
+
+	if ((omap_ssi->get_loss) && (omap_port->loss_count ==
+				omap_ssi->get_loss(ssi->device.parent)))
+		goto mode; /* We always need to restore the mode & TX divisor */
+
+	ssi_restore_port_ctx(omap_port);
+
+mode:
+	ssi_restore_divisor(omap_port);
+	ssi_restore_port_mode(omap_port);
+
+	return 0;
+}
+
+static const struct dev_pm_ops omap_ssi_port_pm_ops = {
+	SET_RUNTIME_PM_OPS(omap_ssi_port_runtime_suspend,
+		omap_ssi_port_runtime_resume, NULL)
+};
+
+#define DEV_PM_OPS     (&omap_ssi_port_pm_ops)
+#else
+#define DEV_PM_OPS     NULL
+#endif
+
+
+#ifdef CONFIG_OF
+static const struct of_device_id omap_ssi_port_of_match[] = {
+	{ .compatible = "ti,omap3-ssi-port", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, omap_ssi_port_of_match);
+#else
+#define omap_ssi_port_of_match NULL
+#endif
+
+static struct platform_driver ssi_port_pdriver = {
+	.remove	= __exit_p(ssi_port_remove),
+	.driver	= {
+		.name	= "omap_ssi_port",
+		.owner	= THIS_MODULE,
+		.of_match_table = omap_ssi_port_of_match,
+		.pm	= DEV_PM_OPS,
+	},
+};
+
+module_platform_driver_probe(ssi_port_pdriver, ssi_port_probe);
+
+MODULE_ALIAS("platform:omap_ssi_port");
+MODULE_AUTHOR("Carlos Chinea <carlos.chinea@nokia.com>");
+MODULE_AUTHOR("Sebastian Reichel <sre@kernel.org>");
+MODULE_DESCRIPTION("Synchronous Serial Interface Port Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/hsi/controllers/omap_ssi_regs.h b/drivers/hsi/controllers/omap_ssi_regs.h
new file mode 100644
index 00000000000..08f98dd1d01
--- /dev/null
+++ b/drivers/hsi/controllers/omap_ssi_regs.h
@@ -0,0 +1,171 @@
+/* Hardware definitions for SSI.
+ *
+ * Copyright (C) 2010 Nokia Corporation. All rights reserved.
+ *
+ * Contact: Carlos Chinea <carlos.chinea@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
+ */
+
+#ifndef __OMAP_SSI_REGS_H__
+#define __OMAP_SSI_REGS_H__
+
+/*
+ * SSI SYS registers
+ */
+#define SSI_REVISION_REG    0
+#  define SSI_REV_MAJOR    0xf0
+#  define SSI_REV_MINOR    0xf
+#define SSI_SYSCONFIG_REG    0x10
+#  define SSI_AUTOIDLE    (1 << 0)
+#  define SSI_SOFTRESET    (1 << 1)
+#  define SSI_SIDLEMODE_FORCE  0
+#  define SSI_SIDLEMODE_NO    (1 << 3)
+#  define SSI_SIDLEMODE_SMART  (1 << 4)
+#  define SSI_SIDLEMODE_MASK  0x18
+#  define SSI_MIDLEMODE_FORCE  0
+#  define SSI_MIDLEMODE_NO    (1 << 12)
+#  define SSI_MIDLEMODE_SMART  (1 << 13)
+#  define SSI_MIDLEMODE_MASK  0x3000
+#define SSI_SYSSTATUS_REG    0x14
+#  define SSI_RESETDONE    1
+#define SSI_MPU_STATUS_REG(port, irq)  (0x808 + ((port) * 0x10) + ((irq) * 2))
+#define SSI_MPU_ENABLE_REG(port, irq)  (0x80c + ((port) * 0x10) + ((irq) * 8))
+#  define SSI_DATAACCEPT(channel)    (1 << (channel))
+#  define SSI_DATAAVAILABLE(channel)  (1 << ((channel) + 8))
+#  define SSI_DATAOVERRUN(channel)    (1 << ((channel) + 16))
+#  define SSI_ERROROCCURED      (1 << 24)
+#  define SSI_BREAKDETECTED    (1 << 25)
+#define SSI_GDD_MPU_IRQ_STATUS_REG  0x0800
+#define SSI_GDD_MPU_IRQ_ENABLE_REG  0x0804
+#  define SSI_GDD_LCH(channel)  (1 << (channel))
+#define SSI_WAKE_REG(port)    (0xc00 + ((port) * 0x10))
+#define SSI_CLEAR_WAKE_REG(port)  (0xc04 + ((port) * 0x10))
+#define SSI_SET_WAKE_REG(port)    (0xc08 + ((port) * 0x10))
+#  define SSI_WAKE(channel)  (1 << (channel))
+#  define SSI_WAKE_MASK    0xff
+
+/*
+ * SSI SST registers
+ */
+#define SSI_SST_ID_REG      0
+#define SSI_SST_MODE_REG    4
+#  define SSI_MODE_VAL_MASK  3
+#  define SSI_MODE_SLEEP    0
+#  define SSI_MODE_STREAM    1
+#  define SSI_MODE_FRAME    2
+#  define SSI_MODE_MULTIPOINTS  3
+#define SSI_SST_FRAMESIZE_REG    8
+#  define SSI_FRAMESIZE_DEFAULT  31
+#define SSI_SST_TXSTATE_REG    0xc
+#  define  SSI_TXSTATE_IDLE  0
+#define SSI_SST_BUFSTATE_REG    0x10
+#  define  SSI_FULL(channel)  (1 << (channel))
+#define SSI_SST_DIVISOR_REG    0x18
+#  define SSI_MAX_DIVISOR    127
+#define SSI_SST_BREAK_REG    0x20
+#define SSI_SST_CHANNELS_REG    0x24
+#  define SSI_CHANNELS_DEFAULT  4
+#define SSI_SST_ARBMODE_REG    0x28
+#  define SSI_ARBMODE_ROUNDROBIN  0
+#  define SSI_ARBMODE_PRIORITY  1
+#define SSI_SST_BUFFER_CH_REG(channel)  (0x80 + ((channel) * 4))
+#define SSI_SST_SWAPBUF_CH_REG(channel)  (0xc0 + ((channel) * 4))
+
+/*
+ * SSI SSR registers
+ */
+#define SSI_SSR_ID_REG      0
+#define SSI_SSR_MODE_REG    4
+#define SSI_SSR_FRAMESIZE_REG    8
+#define SSI_SSR_RXSTATE_REG    0xc
+#define SSI_SSR_BUFSTATE_REG    0x10
+#  define SSI_NOTEMPTY(channel)  (1 << (channel))
+#define SSI_SSR_BREAK_REG    0x1c
+#define SSI_SSR_ERROR_REG    0x20
+#define SSI_SSR_ERRORACK_REG    0x24
+#define SSI_SSR_OVERRUN_REG    0x2c
+#define SSI_SSR_OVERRUNACK_REG    0x30
+#define SSI_SSR_TIMEOUT_REG    0x34
+#  define SSI_TIMEOUT_DEFAULT  0
+#define SSI_SSR_CHANNELS_REG    0x28
+#define SSI_SSR_BUFFER_CH_REG(channel)  (0x80 + ((channel) * 4))
+#define SSI_SSR_SWAPBUF_CH_REG(channel)  (0xc0 + ((channel) * 4))
+
+/*
+ * SSI GDD registers
+ */
+#define SSI_GDD_HW_ID_REG    0
+#define SSI_GDD_PPORT_ID_REG    0x10
+#define SSI_GDD_MPORT_ID_REG    0x14
+#define SSI_GDD_PPORT_SR_REG    0x20
+#define SSI_GDD_MPORT_SR_REG    0x24
+#  define SSI_ACTIVE_LCH_NUM_MASK  0xff
+#define SSI_GDD_TEST_REG    0x40
+#  define SSI_TEST      1
+#define SSI_GDD_GCR_REG      0x100
+#  define  SSI_CLK_AUTOGATING_ON  (1 << 3)
+#  define  SSI_FREE    (1 << 2)
+#  define  SSI_SWITCH_OFF    (1 << 0)
+#define SSI_GDD_GRST_REG    0x200
+#  define SSI_SWRESET    1
+#define SSI_GDD_CSDP_REG(channel)  (0x800 + ((channel) * 0x40))
+#  define SSI_DST_BURST_EN_MASK  0xc000
+#  define SSI_DST_SINGLE_ACCESS0  0
+#  define SSI_DST_SINGLE_ACCESS  (1 << 14)
+#  define SSI_DST_BURST_4x32_BIT  (2 << 14)
+#  define SSI_DST_BURST_8x32_BIT  (3 << 14)
+#  define SSI_DST_MASK    0x1e00
+#  define SSI_DST_MEMORY_PORT  (8 << 9)
+#  define SSI_DST_PERIPHERAL_PORT  (9 << 9)
+#  define SSI_SRC_BURST_EN_MASK  0x180
+#  define SSI_SRC_SINGLE_ACCESS0  0
+#  define SSI_SRC_SINGLE_ACCESS  (1 << 7)
+#  define SSI_SRC_BURST_4x32_BIT  (2 << 7)
+#  define SSI_SRC_BURST_8x32_BIT  (3 << 7)
+#  define SSI_SRC_MASK    0x3c
+#  define SSI_SRC_MEMORY_PORT  (8 << 2)
+#  define SSI_SRC_PERIPHERAL_PORT  (9 << 2)
+#  define SSI_DATA_TYPE_MASK  3
+#  define SSI_DATA_TYPE_S32  2
+#define SSI_GDD_CCR_REG(channel)  (0x802 + ((channel) * 0x40))
+#  define SSI_DST_AMODE_MASK  (3 << 14)
+#  define SSI_DST_AMODE_CONST  0
+#  define SSI_DST_AMODE_POSTINC  (1 << 12)
+#  define SSI_SRC_AMODE_MASK  (3 << 12)
+#  define SSI_SRC_AMODE_CONST  0
+#  define SSI_SRC_AMODE_POSTINC  (1 << 12)
+#  define SSI_CCR_ENABLE    (1 << 7)
+#  define SSI_CCR_SYNC_MASK  0x1f
+#define SSI_GDD_CICR_REG(channel)  (0x804 + ((channel) * 0x40))
+#  define SSI_BLOCK_IE    (1 << 5)
+#  define SSI_HALF_IE    (1 << 2)
+#  define SSI_TOUT_IE    (1 << 0)
+#define SSI_GDD_CSR_REG(channel)  (0x806 + ((channel) * 0x40))
+#  define SSI_CSR_SYNC    (1 << 6)
+#  define SSI_CSR_BLOCK    (1 << 5)
+#  define SSI_CSR_HALF    (1 << 2)
+#  define SSI_CSR_TOUR    (1 << 0)
+#define SSI_GDD_CSSA_REG(channel)  (0x808 + ((channel) * 0x40))
+#define SSI_GDD_CDSA_REG(channel)  (0x80c + ((channel) * 0x40))
+#define SSI_GDD_CEN_REG(channel)  (0x810 + ((channel) * 0x40))
+#define SSI_GDD_CSAC_REG(channel)  (0x818 + ((channel) * 0x40))
+#define SSI_GDD_CDAC_REG(channel)  (0x81a + ((channel) * 0x40))
+#define SSI_GDD_CLNK_CTRL_REG(channel)  (0x828 + ((channel) * 0x40))
+#  define SSI_ENABLE_LNK    (1 << 15)
+#  define SSI_STOP_LNK    (1 << 14)
+#  define SSI_NEXT_CH_ID_MASK  0xf
+
+#endif /* __OMAP_SSI_REGS_H__ */
diff --git a/drivers/hsi/hsi.c b/drivers/hsi/hsi.c
index 749f7b5c817..fe9371271ce 100644
--- a/drivers/hsi/hsi.c
+++ b/drivers/hsi/hsi.c
@@ -26,6 +26,8 @@
 #include <linux/slab.h>
 #include <linux/string.h>
 #include <linux/notifier.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
 #include "hsi_core.h"
 
 static ssize_t modalias_show(struct device *dev,
@@ -50,7 +52,13 @@ static int hsi_bus_uevent(struct device *dev, struct kobj_uevent_env *env)
 
 static int hsi_bus_match(struct device *dev, struct device_driver *driver)
 {
-	return strcmp(dev_name(dev), driver->name) == 0;
+	if (of_driver_match_device(dev, driver))
+		return true;
+
+	if (strcmp(dev_name(dev), driver->name) == 0)
+		return true;
+
+	return false;
 }
 
 static struct bus_type hsi_bus_type = {
@@ -62,18 +70,37 @@ static struct bus_type hsi_bus_type = {
 
 static void hsi_client_release(struct device *dev)
 {
-	kfree(to_hsi_client(dev));
+	struct hsi_client *cl = to_hsi_client(dev);
+
+	kfree(cl->tx_cfg.channels);
+	kfree(cl->rx_cfg.channels);
+	kfree(cl);
 }
 
-static void hsi_new_client(struct hsi_port *port, struct hsi_board_info *info)
+struct hsi_client *hsi_new_client(struct hsi_port *port,
+						struct hsi_board_info *info)
 {
 	struct hsi_client *cl;
+	size_t size;
 
 	cl = kzalloc(sizeof(*cl), GFP_KERNEL);
 	if (!cl)
-		return;
+		return NULL;
+
 	cl->tx_cfg = info->tx_cfg;
+	if (cl->tx_cfg.channels) {
+		size = cl->tx_cfg.num_channels * sizeof(*cl->tx_cfg.channels);
+		cl->tx_cfg.channels = kzalloc(size , GFP_KERNEL);
+		memcpy(cl->tx_cfg.channels, info->tx_cfg.channels, size);
+	}
+
 	cl->rx_cfg = info->rx_cfg;
+	if (cl->rx_cfg.channels) {
+		size = cl->rx_cfg.num_channels * sizeof(*cl->rx_cfg.channels);
+		cl->rx_cfg.channels = kzalloc(size , GFP_KERNEL);
+		memcpy(cl->rx_cfg.channels, info->rx_cfg.channels, size);
+	}
+
 	cl->device.bus = &hsi_bus_type;
 	cl->device.parent = &port->device;
 	cl->device.release = hsi_client_release;
@@ -85,7 +112,10 @@ static void hsi_new_client(struct hsi_port *port, struct hsi_board_info *info)
 		pr_err("hsi: failed to register client: %s\n", info->name);
 		put_device(&cl->device);
 	}
+
+	return cl;
 }
+EXPORT_SYMBOL_GPL(hsi_new_client);
 
 static void hsi_scan_board_info(struct hsi_controller *hsi)
 {
@@ -101,12 +131,209 @@ static void hsi_scan_board_info(struct hsi_controller *hsi)
 		}
 }
 
-static int hsi_remove_client(struct device *dev, void *data __maybe_unused)
+#ifdef CONFIG_OF
+static struct hsi_board_info hsi_char_dev_info = {
+	.name = "hsi_char",
+};
+
+static int hsi_of_property_parse_mode(struct device_node *client, char *name,
+				      unsigned int *result)
+{
+	const char *mode;
+	int err;
+
+	err = of_property_read_string(client, name, &mode);
+	if (err < 0)
+		return err;
+
+	if (strcmp(mode, "stream") == 0)
+		*result = HSI_MODE_STREAM;
+	else if (strcmp(mode, "frame") == 0)
+		*result = HSI_MODE_FRAME;
+	else
+		return -EINVAL;
+
+	return 0;
+}
+
+static int hsi_of_property_parse_flow(struct device_node *client, char *name,
+				      unsigned int *result)
+{
+	const char *flow;
+	int err;
+
+	err = of_property_read_string(client, name, &flow);
+	if (err < 0)
+		return err;
+
+	if (strcmp(flow, "synchronized") == 0)
+		*result = HSI_FLOW_SYNC;
+	else if (strcmp(flow, "pipeline") == 0)
+		*result = HSI_FLOW_PIPE;
+	else
+		return -EINVAL;
+
+	return 0;
+}
+
+static int hsi_of_property_parse_arb_mode(struct device_node *client,
+					  char *name, unsigned int *result)
+{
+	const char *arb_mode;
+	int err;
+
+	err = of_property_read_string(client, name, &arb_mode);
+	if (err < 0)
+		return err;
+
+	if (strcmp(arb_mode, "round-robin") == 0)
+		*result = HSI_ARB_RR;
+	else if (strcmp(arb_mode, "priority") == 0)
+		*result = HSI_ARB_PRIO;
+	else
+		return -EINVAL;
+
+	return 0;
+}
+
+static void hsi_add_client_from_dt(struct hsi_port *port,
+						struct device_node *client)
+{
+	struct hsi_client *cl;
+	struct hsi_channel channel;
+	struct property *prop;
+	char name[32];
+	int length, cells, err, i, max_chan, mode;
+
+	cl = kzalloc(sizeof(*cl), GFP_KERNEL);
+	if (!cl)
+		return;
+
+	err = of_modalias_node(client, name, sizeof(name));
+	if (err)
+		goto err;
+
+	dev_set_name(&cl->device, "%s", name);
+
+	err = hsi_of_property_parse_mode(client, "hsi-mode", &mode);
+	if (err) {
+		err = hsi_of_property_parse_mode(client, "hsi-rx-mode",
+						 &cl->rx_cfg.mode);
+		if (err)
+			goto err;
+
+		err = hsi_of_property_parse_mode(client, "hsi-tx-mode",
+						 &cl->tx_cfg.mode);
+		if (err)
+			goto err;
+	} else {
+		cl->rx_cfg.mode = mode;
+		cl->tx_cfg.mode = mode;
+	}
+
+	err = of_property_read_u32(client, "hsi-speed-kbps",
+				   &cl->tx_cfg.speed);
+	if (err)
+		goto err;
+	cl->rx_cfg.speed = cl->tx_cfg.speed;
+
+	err = hsi_of_property_parse_flow(client, "hsi-flow",
+					 &cl->rx_cfg.flow);
+	if (err)
+		goto err;
+
+	err = hsi_of_property_parse_arb_mode(client, "hsi-arb-mode",
+					     &cl->rx_cfg.arb_mode);
+	if (err)
+		goto err;
+
+	prop = of_find_property(client, "hsi-channel-ids", &length);
+	if (!prop) {
+		err = -EINVAL;
+		goto err;
+	}
+
+	cells = length / sizeof(u32);
+
+	cl->rx_cfg.num_channels = cells;
+	cl->tx_cfg.num_channels = cells;
+
+	cl->rx_cfg.channels = kzalloc(cells * sizeof(channel), GFP_KERNEL);
+	if (!cl->rx_cfg.channels) {
+		err = -ENOMEM;
+		goto err;
+	}
+
+	cl->tx_cfg.channels = kzalloc(cells * sizeof(channel), GFP_KERNEL);
+	if (!cl->tx_cfg.channels) {
+		err = -ENOMEM;
+		goto err2;
+	}
+
+	max_chan = 0;
+	for (i = 0; i < cells; i++) {
+		err = of_property_read_u32_index(client, "hsi-channel-ids", i,
+						 &channel.id);
+		if (err)
+			goto err3;
+
+		err = of_property_read_string_index(client, "hsi-channel-names",
+						    i, &channel.name);
+		if (err)
+			channel.name = NULL;
+
+		if (channel.id > max_chan)
+			max_chan = channel.id;
+
+		cl->rx_cfg.channels[i] = channel;
+		cl->tx_cfg.channels[i] = channel;
+	}
+
+	cl->rx_cfg.num_hw_channels = max_chan + 1;
+	cl->tx_cfg.num_hw_channels = max_chan + 1;
+
+	cl->device.bus = &hsi_bus_type;
+	cl->device.parent = &port->device;
+	cl->device.release = hsi_client_release;
+	cl->device.of_node = client;
+
+	if (device_register(&cl->device) < 0) {
+		pr_err("hsi: failed to register client: %s\n", name);
+		put_device(&cl->device);
+		goto err3;
+	}
+
+	return;
+
+err3:
+	kfree(cl->tx_cfg.channels);
+err2:
+	kfree(cl->rx_cfg.channels);
+err:
+	kfree(cl);
+	pr_err("hsi client: missing or incorrect of property: err=%d\n", err);
+}
+
+void hsi_add_clients_from_dt(struct hsi_port *port, struct device_node *clients)
+{
+	struct device_node *child;
+
+	/* register hsi-char device */
+	hsi_new_client(port, &hsi_char_dev_info);
+
+	for_each_available_child_of_node(clients, child)
+		hsi_add_client_from_dt(port, child);
+}
+EXPORT_SYMBOL_GPL(hsi_add_clients_from_dt);
+#endif
+
+int hsi_remove_client(struct device *dev, void *data __maybe_unused)
 {
 	device_unregister(dev);
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(hsi_remove_client);
 
 static int hsi_remove_port(struct device *dev, void *data __maybe_unused)
 {
@@ -130,6 +357,16 @@ static void hsi_port_release(struct device *dev)
 }
 
 /**
+ * hsi_unregister_port - Unregister an HSI port
+ * @port: The HSI port to unregister
+ */
+void hsi_port_unregister_clients(struct hsi_port *port)
+{
+	device_for_each_child(&port->device, NULL, hsi_remove_client);
+}
+EXPORT_SYMBOL_GPL(hsi_port_unregister_clients);
+
+/**
  * hsi_unregister_controller - Unregister an HSI controller
  * @hsi: The HSI controller to register
  */
@@ -472,7 +709,7 @@ int hsi_unregister_port_event(struct hsi_client *cl)
 EXPORT_SYMBOL_GPL(hsi_unregister_port_event);
 
 /**
- * hsi_event -Notifies clients about port events
+ * hsi_event - Notifies clients about port events
  * @port: Port where the event occurred
  * @event: The event type
  *
@@ -492,6 +729,32 @@ int hsi_event(struct hsi_port *port, unsigned long event)
 }
 EXPORT_SYMBOL_GPL(hsi_event);
 
+/**
+ * hsi_get_channel_id_by_name - acquire channel id by channel name
+ * @cl: HSI client, which uses the channel
+ * @name: name the channel is known under
+ *
+ * Clients can call this function to get the hsi channel ids similar to
+ * requesting IRQs or GPIOs by name. This function assumes the same
+ * channel configuration is used for RX and TX.
+ *
+ * Returns -errno on error or channel id on success.
+ */
+int hsi_get_channel_id_by_name(struct hsi_client *cl, char *name)
+{
+	int i;
+
+	if (!cl->rx_cfg.channels)
+		return -ENOENT;
+
+	for (i = 0; i < cl->rx_cfg.num_channels; i++)
+		if (!strcmp(cl->rx_cfg.channels[i].name, name))
+			return cl->rx_cfg.channels[i].id;
+
+	return -ENXIO;
+}
+EXPORT_SYMBOL_GPL(hsi_get_channel_id_by_name);
+
 static int __init hsi_init(void)
 {
 	return bus_register(&hsi_bus_type);
diff --git a/drivers/hv/channel.c b/drivers/hv/channel.c
index 602ca86a648..284cf66489f 100644
--- a/drivers/hv/channel.c
+++ b/drivers/hv/channel.c
@@ -471,18 +471,26 @@ int vmbus_teardown_gpadl(struct vmbus_channel *channel, u32 gpadl_handle)
 }
 EXPORT_SYMBOL_GPL(vmbus_teardown_gpadl);
 
+static void reset_channel_cb(void *arg)
+{
+	struct vmbus_channel *channel = arg;
+
+	channel->onchannel_callback = NULL;
+}
+
 static void vmbus_close_internal(struct vmbus_channel *channel)
 {
 	struct vmbus_channel_close_channel *msg;
 	int ret;
-	unsigned long flags;
 
 	channel->state = CHANNEL_OPEN_STATE;
 	channel->sc_creation_callback = NULL;
 	/* Stop callback and cancel the timer asap */
-	spin_lock_irqsave(&channel->inbound_lock, flags);
-	channel->onchannel_callback = NULL;
-	spin_unlock_irqrestore(&channel->inbound_lock, flags);
+	if (channel->target_cpu != smp_processor_id())
+		smp_call_function_single(channel->target_cpu, reset_channel_cb,
+					 channel, true);
+	else
+		reset_channel_cb(channel);
 
 	/* Send a closing message */
 
@@ -674,8 +682,7 @@ int vmbus_sendpacket_multipagebuffer(struct vmbus_channel *channel,
 	u32 pfncount = NUM_PAGES_SPANNED(multi_pagebuffer->offset,
 					 multi_pagebuffer->len);
 
-
-	if ((pfncount < 0) || (pfncount > MAX_MULTIPAGE_BUFFER_COUNT))
+	if (pfncount > MAX_MULTIPAGE_BUFFER_COUNT)
 		return -EINVAL;
 
 	/*
diff --git a/drivers/hv/channel_mgmt.c b/drivers/hv/channel_mgmt.c
index fa920469bf1..6c8b032cacb 100644
--- a/drivers/hv/channel_mgmt.c
+++ b/drivers/hv/channel_mgmt.c
@@ -149,6 +149,7 @@ static struct vmbus_channel *alloc_channel(void)
 	spin_lock_init(&channel->sc_lock);
 
 	INIT_LIST_HEAD(&channel->sc_list);
+	INIT_LIST_HEAD(&channel->percpu_list);
 
 	channel->controlwq = create_workqueue("hv_vmbus_ctl");
 	if (!channel->controlwq) {
@@ -188,7 +189,20 @@ static void free_channel(struct vmbus_channel *channel)
 	queue_work(vmbus_connection.work_queue, &channel->work);
 }
 
+static void percpu_channel_enq(void *arg)
+{
+	struct vmbus_channel *channel = arg;
+	int cpu = smp_processor_id();
+
+	list_add_tail(&channel->percpu_list, &hv_context.percpu_list[cpu]);
+}
 
+static void percpu_channel_deq(void *arg)
+{
+	struct vmbus_channel *channel = arg;
+
+	list_del(&channel->percpu_list);
+}
 
 /*
  * vmbus_process_rescind_offer -
@@ -210,6 +224,12 @@ 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())
+		smp_call_function_single(channel->target_cpu,
+					 percpu_channel_deq, channel, true);
+	else
+		percpu_channel_deq(channel);
+
 	if (channel->primary_channel == NULL) {
 		spin_lock_irqsave(&vmbus_connection.channel_lock, flags);
 		list_del(&channel->listentry);
@@ -245,6 +265,7 @@ static void vmbus_process_offer(struct work_struct *work)
 							work);
 	struct vmbus_channel *channel;
 	bool fnew = true;
+	bool enq = false;
 	int ret;
 	unsigned long flags;
 
@@ -264,12 +285,22 @@ static void vmbus_process_offer(struct work_struct *work)
 		}
 	}
 
-	if (fnew)
+	if (fnew) {
 		list_add_tail(&newchannel->listentry,
 			      &vmbus_connection.chn_list);
+		enq = true;
+	}
 
 	spin_unlock_irqrestore(&vmbus_connection.channel_lock, flags);
 
+	if (enq) {
+		if (newchannel->target_cpu != smp_processor_id())
+			smp_call_function_single(newchannel->target_cpu,
+						 percpu_channel_enq,
+						 newchannel, true);
+		else
+			percpu_channel_enq(newchannel);
+	}
 	if (!fnew) {
 		/*
 		 * Check to see if this is a sub-channel.
@@ -282,6 +313,14 @@ static void vmbus_process_offer(struct work_struct *work)
 			spin_lock_irqsave(&channel->sc_lock, flags);
 			list_add_tail(&newchannel->sc_list, &channel->sc_list);
 			spin_unlock_irqrestore(&channel->sc_lock, flags);
+
+			if (newchannel->target_cpu != smp_processor_id())
+				smp_call_function_single(newchannel->target_cpu,
+							 percpu_channel_enq,
+							 newchannel, true);
+			else
+				percpu_channel_enq(newchannel);
+
 			newchannel->state = CHANNEL_OPEN_STATE;
 			if (channel->sc_creation_callback != NULL)
 				channel->sc_creation_callback(newchannel);
@@ -365,7 +404,7 @@ static u32  next_vp;
  * performance critical channels (IDE, SCSI and Network) will be uniformly
  * distributed across all available CPUs.
  */
-static u32 get_vp_index(uuid_le *type_guid)
+static void init_vp_index(struct vmbus_channel *channel, uuid_le *type_guid)
 {
 	u32 cur_cpu;
 	int i;
@@ -387,10 +426,13 @@ static u32 get_vp_index(uuid_le *type_guid)
 		 * Also if the channel is not a performance critical
 		 * channel, bind it to cpu 0.
 		 */
-		return 0;
+		channel->target_cpu = 0;
+		channel->target_vp = 0;
+		return;
 	}
 	cur_cpu = (++next_vp % max_cpus);
-	return hv_context.vp_index[cur_cpu];
+	channel->target_cpu = cur_cpu;
+	channel->target_vp = hv_context.vp_index[cur_cpu];
 }
 
 /*
@@ -438,7 +480,7 @@ static void vmbus_onoffer(struct vmbus_channel_message_header *hdr)
 				offer->connection_id;
 	}
 
-	newchannel->target_vp = get_vp_index(&offer->offer.if_type);
+	init_vp_index(newchannel, &offer->offer.if_type);
 
 	memcpy(&newchannel->offermsg, offer,
 	       sizeof(struct vmbus_channel_offer_channel));
diff --git a/drivers/hv/connection.c b/drivers/hv/connection.c
index 2e7801af466..e84f4526eb3 100644
--- a/drivers/hv/connection.c
+++ b/drivers/hv/connection.c
@@ -224,8 +224,8 @@ cleanup:
 		vmbus_connection.int_page = NULL;
 	}
 
-	free_pages((unsigned long)vmbus_connection.monitor_pages[0], 1);
-	free_pages((unsigned long)vmbus_connection.monitor_pages[1], 1);
+	free_pages((unsigned long)vmbus_connection.monitor_pages[0], 0);
+	free_pages((unsigned long)vmbus_connection.monitor_pages[1], 0);
 	vmbus_connection.monitor_pages[0] = NULL;
 	vmbus_connection.monitor_pages[1] = NULL;
 
@@ -234,6 +234,28 @@ cleanup:
 	return ret;
 }
 
+/*
+ * Map the given relid to the corresponding channel based on the
+ * per-cpu list of channels that have been affinitized to this CPU.
+ * This will be used in the channel callback path as we can do this
+ * mapping in a lock-free fashion.
+ */
+static struct vmbus_channel *pcpu_relid2channel(u32 relid)
+{
+	struct vmbus_channel *channel;
+	struct vmbus_channel *found_channel  = NULL;
+	int cpu = smp_processor_id();
+	struct list_head *pcpu_head = &hv_context.percpu_list[cpu];
+
+	list_for_each_entry(channel, pcpu_head, percpu_list) {
+		if (channel->offermsg.child_relid == relid) {
+			found_channel = channel;
+			break;
+		}
+	}
+
+	return found_channel;
+}
 
 /*
  * relid2channel - Get the channel object given its
@@ -277,7 +299,6 @@ struct vmbus_channel *relid2channel(u32 relid)
 static void process_chn_event(u32 relid)
 {
 	struct vmbus_channel *channel;
-	unsigned long flags;
 	void *arg;
 	bool read_state;
 	u32 bytes_to_read;
@@ -286,7 +307,7 @@ static void process_chn_event(u32 relid)
 	 * Find the channel based on this relid and invokes the
 	 * channel callback to process the event
 	 */
-	channel = relid2channel(relid);
+	channel = pcpu_relid2channel(relid);
 
 	if (!channel) {
 		pr_err("channel not found for relid - %u\n", relid);
@@ -296,13 +317,12 @@ static void process_chn_event(u32 relid)
 	/*
 	 * A channel once created is persistent even when there
 	 * is no driver handling the device. An unloading driver
-	 * sets the onchannel_callback to NULL under the
-	 * protection of the channel inbound_lock. Thus, checking
-	 * and invoking the driver specific callback takes care of
-	 * orderly unloading of the driver.
+	 * sets the onchannel_callback to NULL on the same CPU
+	 * as where this interrupt is handled (in an interrupt context).
+	 * Thus, checking and invoking the driver specific callback takes
+	 * care of orderly unloading of the driver.
 	 */
 
-	spin_lock_irqsave(&channel->inbound_lock, flags);
 	if (channel->onchannel_callback != NULL) {
 		arg = channel->channel_callback_context;
 		read_state = channel->batched_reading;
@@ -327,7 +347,6 @@ static void process_chn_event(u32 relid)
 		pr_err("no channel callback for relid - %u\n", relid);
 	}
 
-	spin_unlock_irqrestore(&channel->inbound_lock, flags);
 }
 
 /*
diff --git a/drivers/hv/hv.c b/drivers/hv/hv.c
index bcb49502c3b..edfc8488cb0 100644
--- a/drivers/hv/hv.c
+++ b/drivers/hv/hv.c
@@ -383,6 +383,8 @@ void hv_synic_init(void *arg)
 	 */
 	rdmsrl(HV_X64_MSR_VP_INDEX, vp_index);
 	hv_context.vp_index[cpu] = (u32)vp_index;
+
+	INIT_LIST_HEAD(&hv_context.percpu_list[cpu]);
 	return;
 }
 
diff --git a/drivers/hv/hv_balloon.c b/drivers/hv/hv_balloon.c
index 7e6d78dc943..5e90c5d771a 100644
--- a/drivers/hv/hv_balloon.c
+++ b/drivers/hv/hv_balloon.c
@@ -19,6 +19,7 @@
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/kernel.h>
+#include <linux/jiffies.h>
 #include <linux/mman.h>
 #include <linux/delay.h>
 #include <linux/init.h>
@@ -459,6 +460,11 @@ static bool do_hot_add;
  */
 static uint pressure_report_delay = 45;
 
+/*
+ * The last time we posted a pressure report to host.
+ */
+static unsigned long last_post_time;
+
 module_param(hot_add, bool, (S_IRUGO | S_IWUSR));
 MODULE_PARM_DESC(hot_add, "If set attempt memory hot_add");
 
@@ -542,6 +548,7 @@ struct hv_dynmem_device {
 
 static struct hv_dynmem_device dm_device;
 
+static void post_status(struct hv_dynmem_device *dm);
 #ifdef CONFIG_MEMORY_HOTPLUG
 
 static void hv_bring_pgs_online(unsigned long start_pfn, unsigned long size)
@@ -612,7 +619,7 @@ static void hv_mem_hot_add(unsigned long start, unsigned long size,
 		 * have not been "onlined" within the allowed time.
 		 */
 		wait_for_completion_timeout(&dm_device.ol_waitevent, 5*HZ);
-
+		post_status(&dm_device);
 	}
 
 	return;
@@ -951,11 +958,17 @@ static void post_status(struct hv_dynmem_device *dm)
 {
 	struct dm_status status;
 	struct sysinfo val;
+	unsigned long now = jiffies;
+	unsigned long last_post = last_post_time;
 
 	if (pressure_report_delay > 0) {
 		--pressure_report_delay;
 		return;
 	}
+
+	if (!time_after(now, (last_post_time + HZ)))
+		return;
+
 	si_meminfo(&val);
 	memset(&status, 0, sizeof(struct dm_status));
 	status.hdr.type = DM_STATUS_REPORT;
@@ -983,6 +996,14 @@ static void post_status(struct hv_dynmem_device *dm)
 	if (status.hdr.trans_id != atomic_read(&trans_id))
 		return;
 
+	/*
+	 * If the last post time that we sampled has changed,
+	 * we have raced, don't post the status.
+	 */
+	if (last_post != last_post_time)
+		return;
+
+	last_post_time = jiffies;
 	vmbus_sendpacket(dm->dev->channel, &status,
 				sizeof(struct dm_status),
 				(unsigned long)NULL,
@@ -1117,7 +1138,7 @@ static void balloon_up(struct work_struct *dummy)
 
 			if (ret == -EAGAIN)
 				msleep(20);
-
+			post_status(&dm_device);
 		} while (ret == -EAGAIN);
 
 		if (ret) {
@@ -1144,8 +1165,10 @@ static void balloon_down(struct hv_dynmem_device *dm,
 	struct dm_unballoon_response resp;
 	int i;
 
-	for (i = 0; i < range_count; i++)
+	for (i = 0; i < range_count; i++) {
 		free_balloon_pages(dm, &range_array[i]);
+		post_status(&dm_device);
+	}
 
 	if (req->more_pages == 1)
 		return;
diff --git a/drivers/hv/hyperv_vmbus.h b/drivers/hv/hyperv_vmbus.h
index 860134da803..18d1a8404cb 100644
--- a/drivers/hv/hyperv_vmbus.h
+++ b/drivers/hv/hyperv_vmbus.h
@@ -510,6 +510,11 @@ struct hv_context {
 	 * basis.
 	 */
 	struct tasklet_struct *event_dpc[NR_CPUS];
+	/*
+	 * To optimize the mapping of relid to channel, maintain
+	 * per-cpu list of the channels based on their CPU affinity.
+	 */
+	struct list_head percpu_list[NR_CPUS];
 };
 
 extern struct hv_context hv_context;
diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index bc196f49ec5..00343166feb 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -1053,7 +1053,7 @@ config SENSORS_PC87427
 
 config SENSORS_NTC_THERMISTOR
 	tristate "NTC thermistor support"
-	depends on (!OF && !IIO) || (OF && IIO)
+	depends on !OF || IIO=n || IIO
 	help
 	  This driver supports NTC thermistors sensor reading and its
 	  interpretation. The driver can also monitor the temperature and
@@ -1065,6 +1065,16 @@ config SENSORS_NTC_THERMISTOR
 	  This driver can also be built as a module.  If so, the module
 	  will be called ntc-thermistor.
 
+config SENSORS_NCT6683
+	tristate "Nuvoton NCT6683D"
+	depends on !PPC
+	help
+	  If you say yes here you get support for the hardware monitoring
+	  functionality of the Nuvoton NCT6683D eSIO chip.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called nct6683.
+
 config SENSORS_NCT6775
 	tristate "Nuvoton NCT6775F and compatibles"
 	depends on !PPC
diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile
index c48f9873ac7..11798ad7e80 100644
--- a/drivers/hwmon/Makefile
+++ b/drivers/hwmon/Makefile
@@ -114,6 +114,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_NCT6683)	+= nct6683.o
 obj-$(CONFIG_SENSORS_NCT6775)	+= nct6775.o
 obj-$(CONFIG_SENSORS_NTC_THERMISTOR)	+= ntc_thermistor.o
 obj-$(CONFIG_SENSORS_PC87360)	+= pc87360.o
diff --git a/drivers/hwmon/adm1029.c b/drivers/hwmon/adm1029.c
index d19c790e410..78339e880bd 100644
--- a/drivers/hwmon/adm1029.c
+++ b/drivers/hwmon/adm1029.c
@@ -1,7 +1,7 @@
 /*
  * adm1029.c - Part of lm_sensors, Linux kernel modules for hardware monitoring
  *
- * Copyright (C) 2006 Corentin LABBE <corentin.labbe@geomatys.fr>
+ * Copyright (C) 2006 Corentin LABBE <clabbe.montjoie@gmail.com>
  *
  * Based on LM83 Driver by Jean Delvare <jdelvare@suse.de>
  *
@@ -449,6 +449,6 @@ static struct adm1029_data *adm1029_update_device(struct device *dev)
 
 module_i2c_driver(adm1029_driver);
 
-MODULE_AUTHOR("Corentin LABBE <corentin.labbe@geomatys.fr>");
+MODULE_AUTHOR("Corentin LABBE <clabbe.montjoie@gmail.com>");
 MODULE_DESCRIPTION("adm1029 driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/hwmon/emc1403.c b/drivers/hwmon/emc1403.c
index 90ec1173b8a..a37b2204a41 100644
--- a/drivers/hwmon/emc1403.c
+++ b/drivers/hwmon/emc1403.c
@@ -18,9 +18,6 @@
  * with this program; if not, write to the Free Software Foundation, Inc.,
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- * TODO
- *	-	cache alarm and critical limit registers
  */
 
 #include <linux/module.h>
@@ -32,22 +29,18 @@
 #include <linux/err.h>
 #include <linux/sysfs.h>
 #include <linux/mutex.h>
-#include <linux/jiffies.h>
+#include <linux/regmap.h>
 
 #define THERMAL_PID_REG		0xfd
 #define THERMAL_SMSC_ID_REG	0xfe
 #define THERMAL_REVISION_REG	0xff
 
+enum emc1403_chip { emc1402, emc1403, emc1404 };
+
 struct thermal_data {
-	struct i2c_client *client;
-	const struct attribute_group *groups[3];
+	struct regmap *regmap;
 	struct mutex mutex;
-	/*
-	 * Cache the hyst value so we don't keep re-reading it. In theory
-	 * we could cache it forever as nobody else should be writing it.
-	 */
-	u8 cached_hyst;
-	unsigned long hyst_valid;
+	const struct attribute_group *groups[4];
 };
 
 static ssize_t show_temp(struct device *dev,
@@ -55,12 +48,13 @@ static ssize_t show_temp(struct device *dev,
 {
 	struct sensor_device_attribute *sda = to_sensor_dev_attr(attr);
 	struct thermal_data *data = dev_get_drvdata(dev);
+	unsigned int val;
 	int retval;
 
-	retval = i2c_smbus_read_byte_data(data->client, sda->index);
+	retval = regmap_read(data->regmap, sda->index, &val);
 	if (retval < 0)
 		return retval;
-	return sprintf(buf, "%d000\n", retval);
+	return sprintf(buf, "%d000\n", val);
 }
 
 static ssize_t show_bit(struct device *dev,
@@ -68,12 +62,13 @@ static ssize_t show_bit(struct device *dev,
 {
 	struct sensor_device_attribute_2 *sda = to_sensor_dev_attr_2(attr);
 	struct thermal_data *data = dev_get_drvdata(dev);
+	unsigned int val;
 	int retval;
 
-	retval = i2c_smbus_read_byte_data(data->client, sda->nr);
+	retval = regmap_read(data->regmap, sda->nr, &val);
 	if (retval < 0)
 		return retval;
-	return sprintf(buf, "%d\n", !!(retval & sda->index));
+	return sprintf(buf, "%d\n", !!(val & sda->index));
 }
 
 static ssize_t store_temp(struct device *dev,
@@ -86,8 +81,8 @@ static ssize_t store_temp(struct device *dev,
 
 	if (kstrtoul(buf, 10, &val))
 		return -EINVAL;
-	retval = i2c_smbus_write_byte_data(data->client, sda->index,
-					DIV_ROUND_CLOSEST(val, 1000));
+	retval = regmap_write(data->regmap, sda->index,
+			      DIV_ROUND_CLOSEST(val, 1000));
 	if (retval < 0)
 		return retval;
 	return count;
@@ -98,51 +93,51 @@ static ssize_t store_bit(struct device *dev,
 {
 	struct sensor_device_attribute_2 *sda = to_sensor_dev_attr_2(attr);
 	struct thermal_data *data = dev_get_drvdata(dev);
-	struct i2c_client *client = data->client;
 	unsigned long val;
 	int retval;
 
 	if (kstrtoul(buf, 10, &val))
 		return -EINVAL;
 
-	mutex_lock(&data->mutex);
-	retval = i2c_smbus_read_byte_data(client, sda->nr);
+	retval = regmap_update_bits(data->regmap, sda->nr, sda->index,
+				    val ? sda->index : 0);
 	if (retval < 0)
-		goto fail;
-
-	retval &= ~sda->index;
-	if (val)
-		retval |= sda->index;
-
-	retval = i2c_smbus_write_byte_data(client, sda->index, retval);
-	if (retval == 0)
-		retval = count;
-fail:
-	mutex_unlock(&data->mutex);
-	return retval;
+		return retval;
+	return count;
 }
 
-static ssize_t show_hyst(struct device *dev,
-			struct device_attribute *attr, char *buf)
+static ssize_t show_hyst_common(struct device *dev,
+				struct device_attribute *attr, char *buf,
+				bool is_min)
 {
 	struct sensor_device_attribute *sda = to_sensor_dev_attr(attr);
 	struct thermal_data *data = dev_get_drvdata(dev);
-	struct i2c_client *client = data->client;
+	struct regmap *regmap = data->regmap;
+	unsigned int limit;
+	unsigned int hyst;
 	int retval;
-	int hyst;
 
-	retval = i2c_smbus_read_byte_data(client, sda->index);
+	retval = regmap_read(regmap, sda->index, &limit);
 	if (retval < 0)
 		return retval;
 
-	if (time_after(jiffies, data->hyst_valid)) {
-		hyst = i2c_smbus_read_byte_data(client, 0x21);
-		if (hyst < 0)
-			return retval;
-		data->cached_hyst = hyst;
-		data->hyst_valid = jiffies + HZ;
-	}
-	return sprintf(buf, "%d000\n", retval - data->cached_hyst);
+	retval = regmap_read(regmap, 0x21, &hyst);
+	if (retval < 0)
+		return retval;
+
+	return sprintf(buf, "%d000\n", is_min ? limit + hyst : limit - hyst);
+}
+
+static ssize_t show_hyst(struct device *dev,
+			 struct device_attribute *attr, char *buf)
+{
+	return show_hyst_common(dev, attr, buf, false);
+}
+
+static ssize_t show_min_hyst(struct device *dev,
+			     struct device_attribute *attr, char *buf)
+{
+	return show_hyst_common(dev, attr, buf, true);
 }
 
 static ssize_t store_hyst(struct device *dev,
@@ -150,7 +145,8 @@ static ssize_t store_hyst(struct device *dev,
 {
 	struct sensor_device_attribute *sda = to_sensor_dev_attr(attr);
 	struct thermal_data *data = dev_get_drvdata(dev);
-	struct i2c_client *client = data->client;
+	struct regmap *regmap = data->regmap;
+	unsigned int limit;
 	int retval;
 	int hyst;
 	unsigned long val;
@@ -159,23 +155,15 @@ static ssize_t store_hyst(struct device *dev,
 		return -EINVAL;
 
 	mutex_lock(&data->mutex);
-	retval = i2c_smbus_read_byte_data(client, sda->index);
+	retval = regmap_read(regmap, sda->index, &limit);
 	if (retval < 0)
 		goto fail;
 
-	hyst = val - retval * 1000;
-	hyst = DIV_ROUND_CLOSEST(hyst, 1000);
-	if (hyst < 0 || hyst > 255) {
-		retval = -ERANGE;
-		goto fail;
-	}
-
-	retval = i2c_smbus_write_byte_data(client, 0x21, hyst);
-	if (retval == 0) {
+	hyst = limit * 1000 - val;
+	hyst = clamp_val(DIV_ROUND_CLOSEST(hyst, 1000), 0, 255);
+	retval = regmap_write(regmap, 0x21, hyst);
+	if (retval == 0)
 		retval = count;
-		data->cached_hyst = hyst;
-		data->hyst_valid = jiffies + HZ;
-	}
 fail:
 	mutex_unlock(&data->mutex);
 	return retval;
@@ -198,6 +186,8 @@ static SENSOR_DEVICE_ATTR_2(temp1_max_alarm, S_IRUGO,
 	show_bit, NULL, 0x35, 0x01);
 static SENSOR_DEVICE_ATTR_2(temp1_crit_alarm, S_IRUGO,
 	show_bit, NULL, 0x37, 0x01);
+static SENSOR_DEVICE_ATTR(temp1_min_hyst, S_IRUGO, show_min_hyst, NULL, 0x06);
+static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IRUGO, show_hyst, NULL, 0x05);
 static SENSOR_DEVICE_ATTR(temp1_crit_hyst, S_IRUGO | S_IWUSR,
 	show_hyst, store_hyst, 0x20);
 
@@ -208,14 +198,16 @@ static SENSOR_DEVICE_ATTR(temp2_max, S_IRUGO | S_IWUSR,
 static SENSOR_DEVICE_ATTR(temp2_crit, S_IRUGO | S_IWUSR,
 	show_temp, store_temp, 0x19);
 static SENSOR_DEVICE_ATTR(temp2_input, S_IRUGO, show_temp, NULL, 0x01);
+static SENSOR_DEVICE_ATTR_2(temp2_fault, S_IRUGO, show_bit, NULL, 0x1b, 0x02);
 static SENSOR_DEVICE_ATTR_2(temp2_min_alarm, S_IRUGO,
 	show_bit, NULL, 0x36, 0x02);
 static SENSOR_DEVICE_ATTR_2(temp2_max_alarm, S_IRUGO,
 	show_bit, NULL, 0x35, 0x02);
 static SENSOR_DEVICE_ATTR_2(temp2_crit_alarm, S_IRUGO,
 	show_bit, NULL, 0x37, 0x02);
-static SENSOR_DEVICE_ATTR(temp2_crit_hyst, S_IRUGO | S_IWUSR,
-	show_hyst, store_hyst, 0x19);
+static SENSOR_DEVICE_ATTR(temp2_min_hyst, S_IRUGO, show_min_hyst, NULL, 0x08);
+static SENSOR_DEVICE_ATTR(temp2_max_hyst, S_IRUGO, show_hyst, NULL, 0x07);
+static SENSOR_DEVICE_ATTR(temp2_crit_hyst, S_IRUGO, show_hyst, NULL, 0x19);
 
 static SENSOR_DEVICE_ATTR(temp3_min, S_IRUGO | S_IWUSR,
 	show_temp, store_temp, 0x16);
@@ -224,14 +216,16 @@ static SENSOR_DEVICE_ATTR(temp3_max, S_IRUGO | S_IWUSR,
 static SENSOR_DEVICE_ATTR(temp3_crit, S_IRUGO | S_IWUSR,
 	show_temp, store_temp, 0x1A);
 static SENSOR_DEVICE_ATTR(temp3_input, S_IRUGO, show_temp, NULL, 0x23);
+static SENSOR_DEVICE_ATTR_2(temp3_fault, S_IRUGO, show_bit, NULL, 0x1b, 0x04);
 static SENSOR_DEVICE_ATTR_2(temp3_min_alarm, S_IRUGO,
 	show_bit, NULL, 0x36, 0x04);
 static SENSOR_DEVICE_ATTR_2(temp3_max_alarm, S_IRUGO,
 	show_bit, NULL, 0x35, 0x04);
 static SENSOR_DEVICE_ATTR_2(temp3_crit_alarm, S_IRUGO,
 	show_bit, NULL, 0x37, 0x04);
-static SENSOR_DEVICE_ATTR(temp3_crit_hyst, S_IRUGO | S_IWUSR,
-	show_hyst, store_hyst, 0x1A);
+static SENSOR_DEVICE_ATTR(temp3_min_hyst, S_IRUGO, show_min_hyst, NULL, 0x16);
+static SENSOR_DEVICE_ATTR(temp3_max_hyst, S_IRUGO, show_hyst, NULL, 0x15);
+static SENSOR_DEVICE_ATTR(temp3_crit_hyst, S_IRUGO, show_hyst, NULL, 0x1A);
 
 static SENSOR_DEVICE_ATTR(temp4_min, S_IRUGO | S_IWUSR,
 	show_temp, store_temp, 0x2D);
@@ -240,44 +234,66 @@ static SENSOR_DEVICE_ATTR(temp4_max, S_IRUGO | S_IWUSR,
 static SENSOR_DEVICE_ATTR(temp4_crit, S_IRUGO | S_IWUSR,
 	show_temp, store_temp, 0x30);
 static SENSOR_DEVICE_ATTR(temp4_input, S_IRUGO, show_temp, NULL, 0x2A);
+static SENSOR_DEVICE_ATTR_2(temp4_fault, S_IRUGO, show_bit, NULL, 0x1b, 0x08);
 static SENSOR_DEVICE_ATTR_2(temp4_min_alarm, S_IRUGO,
 	show_bit, NULL, 0x36, 0x08);
 static SENSOR_DEVICE_ATTR_2(temp4_max_alarm, S_IRUGO,
 	show_bit, NULL, 0x35, 0x08);
 static SENSOR_DEVICE_ATTR_2(temp4_crit_alarm, S_IRUGO,
 	show_bit, NULL, 0x37, 0x08);
-static SENSOR_DEVICE_ATTR(temp4_crit_hyst, S_IRUGO | S_IWUSR,
-	show_hyst, store_hyst, 0x30);
+static SENSOR_DEVICE_ATTR(temp4_min_hyst, S_IRUGO, show_min_hyst, NULL, 0x2D);
+static SENSOR_DEVICE_ATTR(temp4_max_hyst, S_IRUGO, show_hyst, NULL, 0x2C);
+static SENSOR_DEVICE_ATTR(temp4_crit_hyst, S_IRUGO, show_hyst, NULL, 0x30);
 
 static SENSOR_DEVICE_ATTR_2(power_state, S_IRUGO | S_IWUSR,
 	show_bit, store_bit, 0x03, 0x40);
 
-static struct attribute *emc1403_attrs[] = {
+static struct attribute *emc1402_attrs[] = {
 	&sensor_dev_attr_temp1_min.dev_attr.attr,
 	&sensor_dev_attr_temp1_max.dev_attr.attr,
 	&sensor_dev_attr_temp1_crit.dev_attr.attr,
 	&sensor_dev_attr_temp1_input.dev_attr.attr,
-	&sensor_dev_attr_temp1_min_alarm.dev_attr.attr,
-	&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
-	&sensor_dev_attr_temp1_crit_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp1_min_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
 	&sensor_dev_attr_temp1_crit_hyst.dev_attr.attr,
+
 	&sensor_dev_attr_temp2_min.dev_attr.attr,
 	&sensor_dev_attr_temp2_max.dev_attr.attr,
 	&sensor_dev_attr_temp2_crit.dev_attr.attr,
 	&sensor_dev_attr_temp2_input.dev_attr.attr,
+	&sensor_dev_attr_temp2_min_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp2_max_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp2_crit_hyst.dev_attr.attr,
+
+	&sensor_dev_attr_power_state.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group emc1402_group = {
+		.attrs = emc1402_attrs,
+};
+
+static struct attribute *emc1403_attrs[] = {
+	&sensor_dev_attr_temp1_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit_alarm.dev_attr.attr,
+
+	&sensor_dev_attr_temp2_fault.dev_attr.attr,
 	&sensor_dev_attr_temp2_min_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp2_max_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp2_crit_alarm.dev_attr.attr,
-	&sensor_dev_attr_temp2_crit_hyst.dev_attr.attr,
+
 	&sensor_dev_attr_temp3_min.dev_attr.attr,
 	&sensor_dev_attr_temp3_max.dev_attr.attr,
 	&sensor_dev_attr_temp3_crit.dev_attr.attr,
 	&sensor_dev_attr_temp3_input.dev_attr.attr,
+	&sensor_dev_attr_temp3_fault.dev_attr.attr,
 	&sensor_dev_attr_temp3_min_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp3_max_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp3_crit_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp3_min_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp3_max_hyst.dev_attr.attr,
 	&sensor_dev_attr_temp3_crit_hyst.dev_attr.attr,
-	&sensor_dev_attr_power_state.dev_attr.attr,
 	NULL
 };
 
@@ -290,9 +306,12 @@ static struct attribute *emc1404_attrs[] = {
 	&sensor_dev_attr_temp4_max.dev_attr.attr,
 	&sensor_dev_attr_temp4_crit.dev_attr.attr,
 	&sensor_dev_attr_temp4_input.dev_attr.attr,
+	&sensor_dev_attr_temp4_fault.dev_attr.attr,
 	&sensor_dev_attr_temp4_min_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp4_max_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp4_crit_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp4_min_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp4_max_hyst.dev_attr.attr,
 	&sensor_dev_attr_temp4_crit_hyst.dev_attr.attr,
 	NULL
 };
@@ -301,6 +320,39 @@ static const struct attribute_group emc1404_group = {
 	.attrs = emc1404_attrs,
 };
 
+/*
+ * EMC14x2 uses a different register and different bits to report alarm and
+ * fault status. For simplicity, provide a separate attribute group for this
+ * chip series.
+ * Since we can not re-use the same attribute names, create a separate attribute
+ * array.
+ */
+static struct sensor_device_attribute_2 emc1402_alarms[] = {
+	SENSOR_ATTR_2(temp1_min_alarm, S_IRUGO, show_bit, NULL, 0x02, 0x20),
+	SENSOR_ATTR_2(temp1_max_alarm, S_IRUGO, show_bit, NULL, 0x02, 0x40),
+	SENSOR_ATTR_2(temp1_crit_alarm, S_IRUGO, show_bit, NULL, 0x02, 0x01),
+
+	SENSOR_ATTR_2(temp2_fault, S_IRUGO, show_bit, NULL, 0x02, 0x04),
+	SENSOR_ATTR_2(temp2_min_alarm, S_IRUGO, show_bit, NULL, 0x02, 0x08),
+	SENSOR_ATTR_2(temp2_max_alarm, S_IRUGO, show_bit, NULL, 0x02, 0x10),
+	SENSOR_ATTR_2(temp2_crit_alarm, S_IRUGO, show_bit, NULL, 0x02, 0x02),
+};
+
+static struct attribute *emc1402_alarm_attrs[] = {
+	&emc1402_alarms[0].dev_attr.attr,
+	&emc1402_alarms[1].dev_attr.attr,
+	&emc1402_alarms[2].dev_attr.attr,
+	&emc1402_alarms[3].dev_attr.attr,
+	&emc1402_alarms[4].dev_attr.attr,
+	&emc1402_alarms[5].dev_attr.attr,
+	&emc1402_alarms[6].dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group emc1402_alarm_group = {
+	.attrs = emc1402_alarm_attrs,
+};
+
 static int emc1403_detect(struct i2c_client *client,
 			struct i2c_board_info *info)
 {
@@ -313,9 +365,15 @@ static int emc1403_detect(struct i2c_client *client,
 
 	id = i2c_smbus_read_byte_data(client, THERMAL_PID_REG);
 	switch (id) {
+	case 0x20:
+		strlcpy(info->type, "emc1402", I2C_NAME_SIZE);
+		break;
 	case 0x21:
 		strlcpy(info->type, "emc1403", I2C_NAME_SIZE);
 		break;
+	case 0x22:
+		strlcpy(info->type, "emc1422", I2C_NAME_SIZE);
+		break;
 	case 0x23:
 		strlcpy(info->type, "emc1423", I2C_NAME_SIZE);
 		break;
@@ -330,12 +388,41 @@ static int emc1403_detect(struct i2c_client *client,
 	}
 
 	id = i2c_smbus_read_byte_data(client, THERMAL_REVISION_REG);
-	if (id != 0x01)
+	if (id < 0x01 || id > 0x04)
 		return -ENODEV;
 
 	return 0;
 }
 
+static bool emc1403_regmap_is_volatile(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case 0x00:	/* internal diode high byte */
+	case 0x01:	/* external diode 1 high byte */
+	case 0x02:	/* status */
+	case 0x10:	/* external diode 1 low byte */
+	case 0x1b:	/* external diode fault */
+	case 0x23:	/* external diode 2 high byte */
+	case 0x24:	/* external diode 2 low byte */
+	case 0x29:	/* internal diode low byte */
+	case 0x2a:	/* externl diode 3 high byte */
+	case 0x2b:	/* external diode 3 low byte */
+	case 0x35:	/* high limit status */
+	case 0x36:	/* low limit status */
+	case 0x37:	/* therm limit status */
+		return true;
+	default:
+		return false;
+	}
+}
+
+static struct regmap_config emc1403_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.cache_type = REGCACHE_RBTREE,
+	.volatile_reg = emc1403_regmap_is_volatile,
+};
+
 static int emc1403_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
@@ -347,17 +434,27 @@ static int emc1403_probe(struct i2c_client *client,
 	if (data == NULL)
 		return -ENOMEM;
 
-	data->client = client;
+	data->regmap = devm_regmap_init_i2c(client, &emc1403_regmap_config);
+	if (IS_ERR(data->regmap))
+		return PTR_ERR(data->regmap);
+
 	mutex_init(&data->mutex);
-	data->hyst_valid = jiffies - 1;		/* Expired */
 
-	data->groups[0] = &emc1403_group;
-	if (id->driver_data)
-		data->groups[1] = &emc1404_group;
+	switch (id->driver_data) {
+	case emc1404:
+		data->groups[2] = &emc1404_group;
+	case emc1403:
+		data->groups[1] = &emc1403_group;
+	case emc1402:
+		data->groups[0] = &emc1402_group;
+	}
+
+	if (id->driver_data == emc1402)
+		data->groups[1] = &emc1402_alarm_group;
 
-	hwmon_dev = hwmon_device_register_with_groups(&client->dev,
-						      client->name, data,
-						      data->groups);
+	hwmon_dev = devm_hwmon_device_register_with_groups(&client->dev,
+							   client->name, data,
+							   data->groups);
 	if (IS_ERR(hwmon_dev))
 		return PTR_ERR(hwmon_dev);
 
@@ -366,14 +463,20 @@ static int emc1403_probe(struct i2c_client *client,
 }
 
 static const unsigned short emc1403_address_list[] = {
-	0x18, 0x29, 0x4c, 0x4d, I2C_CLIENT_END
+	0x18, 0x1c, 0x29, 0x4c, 0x4d, 0x5c, I2C_CLIENT_END
 };
 
+/* Last digit of chip name indicates number of channels */
 static const struct i2c_device_id emc1403_idtable[] = {
-	{ "emc1403", 0 },
-	{ "emc1404", 1 },
-	{ "emc1423", 0 },
-	{ "emc1424", 1 },
+	{ "emc1402", emc1402 },
+	{ "emc1403", emc1403 },
+	{ "emc1404", emc1404 },
+	{ "emc1412", emc1402 },
+	{ "emc1413", emc1403 },
+	{ "emc1414", emc1404 },
+	{ "emc1422", emc1402 },
+	{ "emc1423", emc1403 },
+	{ "emc1424", emc1404 },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, emc1403_idtable);
diff --git a/drivers/hwmon/f71805f.c b/drivers/hwmon/f71805f.c
index 32f5132c465..9e57b77ecd3 100644
--- a/drivers/hwmon/f71805f.c
+++ b/drivers/hwmon/f71805f.c
@@ -1387,10 +1387,8 @@ static int f71805f_probe(struct platform_device *pdev)
 
 	data = devm_kzalloc(&pdev->dev, sizeof(struct f71805f_data),
 			    GFP_KERNEL);
-	if (!data) {
-		pr_err("Out of memory\n");
+	if (!data)
 		return -ENOMEM;
-	}
 
 	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
 	if (!devm_request_region(&pdev->dev, res->start + ADDR_REG_OFFSET, 2,
diff --git a/drivers/hwmon/g762.c b/drivers/hwmon/g762.c
index b4b8b5bef71..98a8618d8fb 100644
--- a/drivers/hwmon/g762.c
+++ b/drivers/hwmon/g762.c
@@ -586,7 +586,7 @@ static int do_set_fan_startv(struct device *dev, unsigned long val)
  */
 
 #ifdef CONFIG_OF
-static struct of_device_id g762_dt_match[] = {
+static const struct of_device_id g762_dt_match[] = {
 	{ .compatible = "gmt,g762" },
 	{ .compatible = "gmt,g763" },
 	{ },
diff --git a/drivers/hwmon/gpio-fan.c b/drivers/hwmon/gpio-fan.c
index 73181be5b30..ba35e4d530b 100644
--- a/drivers/hwmon/gpio-fan.c
+++ b/drivers/hwmon/gpio-fan.c
@@ -482,7 +482,7 @@ static int gpio_fan_get_of_pdata(struct device *dev,
 	return 0;
 }
 
-static struct of_device_id of_gpio_fan_match[] = {
+static const struct of_device_id of_gpio_fan_match[] = {
 	{ .compatible = "gpio-fan", },
 	{},
 };
diff --git a/drivers/hwmon/ibmpex.c b/drivers/hwmon/ibmpex.c
index 20ab0fb8539..030e7ff589b 100644
--- a/drivers/hwmon/ibmpex.c
+++ b/drivers/hwmon/ibmpex.c
@@ -463,10 +463,8 @@ static void ibmpex_register_bmc(int iface, struct device *dev)
 	int err;
 
 	data = kzalloc(sizeof(*data), GFP_KERNEL);
-	if (!data) {
-		dev_err(dev, "Insufficient memory for BMC interface.\n");
+	if (!data)
 		return;
-	}
 
 	data->address.addr_type = IPMI_SYSTEM_INTERFACE_ADDR_TYPE;
 	data->address.channel = IPMI_BMC_CHANNEL;
diff --git a/drivers/hwmon/iio_hwmon.c b/drivers/hwmon/iio_hwmon.c
index 9fbb1b1fdff..14c82daab01 100644
--- a/drivers/hwmon/iio_hwmon.c
+++ b/drivers/hwmon/iio_hwmon.c
@@ -163,7 +163,7 @@ static int iio_hwmon_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id iio_hwmon_of_match[] = {
+static const struct of_device_id iio_hwmon_of_match[] = {
 	{ .compatible = "iio-hwmon", },
 	{ }
 };
diff --git a/drivers/hwmon/jc42.c b/drivers/hwmon/jc42.c
index 6013611e4f2..388f8bcd898 100644
--- a/drivers/hwmon/jc42.c
+++ b/drivers/hwmon/jc42.c
@@ -65,6 +65,7 @@ static const unsigned short normal_i2c[] = {
 /* Manufacturer IDs */
 #define ADT_MANID		0x11d4  /* Analog Devices */
 #define ATMEL_MANID		0x001f  /* Atmel */
+#define ATMEL_MANID2		0x1114	/* Atmel */
 #define MAX_MANID		0x004d  /* Maxim */
 #define IDT_MANID		0x00b3  /* IDT */
 #define MCP_MANID		0x0054  /* Microchip */
@@ -82,6 +83,9 @@ static const unsigned short normal_i2c[] = {
 #define AT30TS00_DEVID		0x8201
 #define AT30TS00_DEVID_MASK	0xffff
 
+#define AT30TSE004_DEVID	0x2200
+#define AT30TSE004_DEVID_MASK	0xffff
+
 /* IDT */
 #define TS3000B3_DEVID		0x2903  /* Also matches TSE2002B3 */
 #define TS3000B3_DEVID_MASK	0xffff
@@ -130,6 +134,9 @@ static const unsigned short normal_i2c[] = {
 #define STTS2002_DEVID		0x0300
 #define STTS2002_DEVID_MASK	0xffff
 
+#define STTS2004_DEVID		0x2201
+#define STTS2004_DEVID_MASK	0xffff
+
 #define STTS3000_DEVID		0x0200
 #define STTS3000_DEVID_MASK	0xffff
 
@@ -144,6 +151,7 @@ struct jc42_chips {
 static struct jc42_chips jc42_chips[] = {
 	{ ADT_MANID, ADT7408_DEVID, ADT7408_DEVID_MASK },
 	{ ATMEL_MANID, AT30TS00_DEVID, AT30TS00_DEVID_MASK },
+	{ ATMEL_MANID2, AT30TSE004_DEVID, AT30TSE004_DEVID_MASK },
 	{ IDT_MANID, TS3000B3_DEVID, TS3000B3_DEVID_MASK },
 	{ IDT_MANID, TS3000GB2_DEVID, TS3000GB2_DEVID_MASK },
 	{ MAX_MANID, MAX6604_DEVID, MAX6604_DEVID_MASK },
@@ -158,9 +166,25 @@ static struct jc42_chips jc42_chips[] = {
 	{ STM_MANID, STTS424_DEVID, STTS424_DEVID_MASK },
 	{ STM_MANID, STTS424E_DEVID, STTS424E_DEVID_MASK },
 	{ STM_MANID, STTS2002_DEVID, STTS2002_DEVID_MASK },
+	{ STM_MANID, STTS2004_DEVID, STTS2004_DEVID_MASK },
 	{ STM_MANID, STTS3000_DEVID, STTS3000_DEVID_MASK },
 };
 
+enum temp_index {
+	t_input = 0,
+	t_crit,
+	t_min,
+	t_max,
+	t_num_temp
+};
+
+static const u8 temp_regs[t_num_temp] = {
+	[t_input] = JC42_REG_TEMP,
+	[t_crit] = JC42_REG_TEMP_CRITICAL,
+	[t_min] = JC42_REG_TEMP_LOWER,
+	[t_max] = JC42_REG_TEMP_UPPER,
+};
+
 /* Each client has this additional data */
 struct jc42_data {
 	struct i2c_client *client;
@@ -170,69 +194,7 @@ struct jc42_data {
 	unsigned long	last_updated;	/* In jiffies */
 	u16		orig_config;	/* original configuration */
 	u16		config;		/* current configuration */
-	u16		temp_input;	/* Temperatures */
-	u16		temp_crit;
-	u16		temp_min;
-	u16		temp_max;
-};
-
-static int jc42_probe(struct i2c_client *client,
-		      const struct i2c_device_id *id);
-static int jc42_detect(struct i2c_client *client, struct i2c_board_info *info);
-static int jc42_remove(struct i2c_client *client);
-
-static struct jc42_data *jc42_update_device(struct device *dev);
-
-static const struct i2c_device_id jc42_id[] = {
-	{ "jc42", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, jc42_id);
-
-#ifdef CONFIG_PM
-
-static int jc42_suspend(struct device *dev)
-{
-	struct jc42_data *data = dev_get_drvdata(dev);
-
-	data->config |= JC42_CFG_SHUTDOWN;
-	i2c_smbus_write_word_swapped(data->client, JC42_REG_CONFIG,
-				     data->config);
-	return 0;
-}
-
-static int jc42_resume(struct device *dev)
-{
-	struct jc42_data *data = dev_get_drvdata(dev);
-
-	data->config &= ~JC42_CFG_SHUTDOWN;
-	i2c_smbus_write_word_swapped(data->client, JC42_REG_CONFIG,
-				     data->config);
-	return 0;
-}
-
-static const struct dev_pm_ops jc42_dev_pm_ops = {
-	.suspend = jc42_suspend,
-	.resume = jc42_resume,
-};
-
-#define JC42_DEV_PM_OPS (&jc42_dev_pm_ops)
-#else
-#define JC42_DEV_PM_OPS NULL
-#endif /* CONFIG_PM */
-
-/* This is the driver that will be inserted */
-static struct i2c_driver jc42_driver = {
-	.class		= I2C_CLASS_SPD,
-	.driver = {
-		.name	= "jc42",
-		.pm = JC42_DEV_PM_OPS,
-	},
-	.probe		= jc42_probe,
-	.remove		= jc42_remove,
-	.id_table	= jc42_id,
-	.detect		= jc42_detect,
-	.address_list	= normal_i2c,
+	u16		temp[t_num_temp];/* Temperatures */
 };
 
 #define JC42_TEMP_MIN_EXTENDED	(-40000)
@@ -261,79 +223,81 @@ static int jc42_temp_from_reg(s16 reg)
 	return reg * 125 / 2;
 }
 
-/* sysfs stuff */
-
-/* read routines for temperature limits */
-#define show(value)	\
-static ssize_t show_##value(struct device *dev,				\
-			    struct device_attribute *attr,		\
-			    char *buf)					\
-{									\
-	struct jc42_data *data = jc42_update_device(dev);		\
-	if (IS_ERR(data))						\
-		return PTR_ERR(data);					\
-	return sprintf(buf, "%d\n", jc42_temp_from_reg(data->value));	\
+static struct jc42_data *jc42_update_device(struct device *dev)
+{
+	struct jc42_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	struct jc42_data *ret = data;
+	int i, val;
+
+	mutex_lock(&data->update_lock);
+
+	if (time_after(jiffies, data->last_updated + HZ) || !data->valid) {
+		for (i = 0; i < t_num_temp; i++) {
+			val = i2c_smbus_read_word_swapped(client, temp_regs[i]);
+			if (val < 0) {
+				ret = ERR_PTR(val);
+				goto abort;
+			}
+			data->temp[i] = val;
+		}
+		data->last_updated = jiffies;
+		data->valid = true;
+	}
+abort:
+	mutex_unlock(&data->update_lock);
+	return ret;
 }
 
-show(temp_input);
-show(temp_crit);
-show(temp_min);
-show(temp_max);
+/* sysfs functions */
 
-/* read routines for hysteresis values */
-static ssize_t show_temp_crit_hyst(struct device *dev,
-				   struct device_attribute *attr, char *buf)
+static ssize_t show_temp(struct device *dev, struct device_attribute *devattr,
+			 char *buf)
 {
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
 	struct jc42_data *data = jc42_update_device(dev);
-	int temp, hyst;
-
 	if (IS_ERR(data))
 		return PTR_ERR(data);
-
-	temp = jc42_temp_from_reg(data->temp_crit);
-	hyst = jc42_hysteresis[(data->config & JC42_CFG_HYST_MASK)
-			       >> JC42_CFG_HYST_SHIFT];
-	return sprintf(buf, "%d\n", temp - hyst);
+	return sprintf(buf, "%d\n",
+		       jc42_temp_from_reg(data->temp[attr->index]));
 }
 
-static ssize_t show_temp_max_hyst(struct device *dev,
-				  struct device_attribute *attr, char *buf)
+static ssize_t show_temp_hyst(struct device *dev,
+			      struct device_attribute *devattr, char *buf)
 {
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
 	struct jc42_data *data = jc42_update_device(dev);
 	int temp, hyst;
 
 	if (IS_ERR(data))
 		return PTR_ERR(data);
 
-	temp = jc42_temp_from_reg(data->temp_max);
+	temp = jc42_temp_from_reg(data->temp[attr->index]);
 	hyst = jc42_hysteresis[(data->config & JC42_CFG_HYST_MASK)
 			       >> JC42_CFG_HYST_SHIFT];
 	return sprintf(buf, "%d\n", temp - hyst);
 }
 
-/* write routines */
-#define set(value, reg)	\
-static ssize_t set_##value(struct device *dev,				\
-			   struct device_attribute *attr,		\
-			   const char *buf, size_t count)		\
-{									\
-	struct jc42_data *data = dev_get_drvdata(dev);			\
-	int err, ret = count;						\
-	long val;							\
-	if (kstrtol(buf, 10, &val) < 0)					\
-		return -EINVAL;						\
-	mutex_lock(&data->update_lock);					\
-	data->value = jc42_temp_to_reg(val, data->extended);		\
-	err = i2c_smbus_write_word_swapped(data->client, reg, data->value); \
-	if (err < 0)							\
-		ret = err;						\
-	mutex_unlock(&data->update_lock);				\
-	return ret;							\
-}
+static ssize_t set_temp(struct device *dev, struct device_attribute *devattr,
+			const char *buf, size_t count)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct jc42_data *data = dev_get_drvdata(dev);
+	int err, ret = count;
+	int nr = attr->index;
+	long val;
 
-set(temp_min, JC42_REG_TEMP_LOWER);
-set(temp_max, JC42_REG_TEMP_UPPER);
-set(temp_crit, JC42_REG_TEMP_CRITICAL);
+	if (kstrtol(buf, 10, &val) < 0)
+		return -EINVAL;
+	mutex_lock(&data->update_lock);
+	data->temp[nr] = jc42_temp_to_reg(val, data->extended);
+	err = i2c_smbus_write_word_swapped(data->client, temp_regs[nr],
+					   data->temp[nr]);
+	if (err < 0)
+		ret = err;
+	mutex_unlock(&data->update_lock);
+	return ret;
+}
 
 /*
  * JC42.4 compliant chips only support four hysteresis values.
@@ -352,7 +316,7 @@ static ssize_t set_temp_crit_hyst(struct device *dev,
 	if (kstrtoul(buf, 10, &val) < 0)
 		return -EINVAL;
 
-	diff = jc42_temp_from_reg(data->temp_crit) - val;
+	diff = jc42_temp_from_reg(data->temp[t_crit]) - val;
 	hyst = 0;
 	if (diff > 0) {
 		if (diff < 2250)
@@ -384,25 +348,20 @@ static ssize_t show_alarm(struct device *dev,
 	if (IS_ERR(data))
 		return PTR_ERR(data);
 
-	val = data->temp_input;
+	val = data->temp[t_input];
 	if (bit != JC42_ALARM_CRIT_BIT && (data->config & JC42_CFG_CRIT_ONLY))
 		val = 0;
 	return sprintf(buf, "%u\n", (val >> bit) & 1);
 }
 
-static DEVICE_ATTR(temp1_input, S_IRUGO,
-		   show_temp_input, NULL);
-static DEVICE_ATTR(temp1_crit, S_IRUGO,
-		   show_temp_crit, set_temp_crit);
-static DEVICE_ATTR(temp1_min, S_IRUGO,
-		   show_temp_min, set_temp_min);
-static DEVICE_ATTR(temp1_max, S_IRUGO,
-		   show_temp_max, set_temp_max);
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_temp, NULL, t_input);
+static SENSOR_DEVICE_ATTR(temp1_crit, S_IRUGO, show_temp, set_temp, t_crit);
+static SENSOR_DEVICE_ATTR(temp1_min, S_IRUGO, show_temp, set_temp, t_min);
+static SENSOR_DEVICE_ATTR(temp1_max, S_IRUGO, show_temp, set_temp, t_max);
 
-static DEVICE_ATTR(temp1_crit_hyst, S_IRUGO,
-		   show_temp_crit_hyst, set_temp_crit_hyst);
-static DEVICE_ATTR(temp1_max_hyst, S_IRUGO,
-		   show_temp_max_hyst, NULL);
+static SENSOR_DEVICE_ATTR(temp1_crit_hyst, S_IRUGO, show_temp_hyst,
+			  set_temp_crit_hyst, t_crit);
+static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IRUGO, show_temp_hyst, NULL, t_max);
 
 static SENSOR_DEVICE_ATTR(temp1_crit_alarm, S_IRUGO, show_alarm, NULL,
 			  JC42_ALARM_CRIT_BIT);
@@ -412,12 +371,12 @@ static SENSOR_DEVICE_ATTR(temp1_max_alarm, S_IRUGO, show_alarm, NULL,
 			  JC42_ALARM_MAX_BIT);
 
 static struct attribute *jc42_attributes[] = {
-	&dev_attr_temp1_input.attr,
-	&dev_attr_temp1_crit.attr,
-	&dev_attr_temp1_min.attr,
-	&dev_attr_temp1_max.attr,
-	&dev_attr_temp1_crit_hyst.attr,
-	&dev_attr_temp1_max_hyst.attr,
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit.dev_attr.attr,
+	&sensor_dev_attr_temp1_min.dev_attr.attr,
+	&sensor_dev_attr_temp1_max.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
 	&sensor_dev_attr_temp1_crit_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp1_min_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
@@ -432,12 +391,12 @@ static umode_t jc42_attribute_mode(struct kobject *kobj,
 	unsigned int config = data->config;
 	bool readonly;
 
-	if (attr == &dev_attr_temp1_crit.attr)
+	if (attr == &sensor_dev_attr_temp1_crit.dev_attr.attr)
 		readonly = config & JC42_CFG_TCRIT_LOCK;
-	else if (attr == &dev_attr_temp1_min.attr ||
-		 attr == &dev_attr_temp1_max.attr)
+	else if (attr == &sensor_dev_attr_temp1_min.dev_attr.attr ||
+		 attr == &sensor_dev_attr_temp1_max.dev_attr.attr)
 		readonly = config & JC42_CFG_EVENT_LOCK;
-	else if (attr == &dev_attr_temp1_crit_hyst.attr)
+	else if (attr == &sensor_dev_attr_temp1_crit_hyst.dev_attr.attr)
 		readonly = config & (JC42_CFG_EVENT_LOCK | JC42_CFG_TCRIT_LOCK);
 	else
 		readonly = true;
@@ -537,52 +496,56 @@ static int jc42_remove(struct i2c_client *client)
 	return 0;
 }
 
-static struct jc42_data *jc42_update_device(struct device *dev)
+#ifdef CONFIG_PM
+
+static int jc42_suspend(struct device *dev)
 {
 	struct jc42_data *data = dev_get_drvdata(dev);
-	struct i2c_client *client = data->client;
-	struct jc42_data *ret = data;
-	int val;
 
-	mutex_lock(&data->update_lock);
+	data->config |= JC42_CFG_SHUTDOWN;
+	i2c_smbus_write_word_swapped(data->client, JC42_REG_CONFIG,
+				     data->config);
+	return 0;
+}
 
-	if (time_after(jiffies, data->last_updated + HZ) || !data->valid) {
-		val = i2c_smbus_read_word_swapped(client, JC42_REG_TEMP);
-		if (val < 0) {
-			ret = ERR_PTR(val);
-			goto abort;
-		}
-		data->temp_input = val;
+static int jc42_resume(struct device *dev)
+{
+	struct jc42_data *data = dev_get_drvdata(dev);
 
-		val = i2c_smbus_read_word_swapped(client,
-						  JC42_REG_TEMP_CRITICAL);
-		if (val < 0) {
-			ret = ERR_PTR(val);
-			goto abort;
-		}
-		data->temp_crit = val;
+	data->config &= ~JC42_CFG_SHUTDOWN;
+	i2c_smbus_write_word_swapped(data->client, JC42_REG_CONFIG,
+				     data->config);
+	return 0;
+}
 
-		val = i2c_smbus_read_word_swapped(client, JC42_REG_TEMP_LOWER);
-		if (val < 0) {
-			ret = ERR_PTR(val);
-			goto abort;
-		}
-		data->temp_min = val;
+static const struct dev_pm_ops jc42_dev_pm_ops = {
+	.suspend = jc42_suspend,
+	.resume = jc42_resume,
+};
 
-		val = i2c_smbus_read_word_swapped(client, JC42_REG_TEMP_UPPER);
-		if (val < 0) {
-			ret = ERR_PTR(val);
-			goto abort;
-		}
-		data->temp_max = val;
+#define JC42_DEV_PM_OPS (&jc42_dev_pm_ops)
+#else
+#define JC42_DEV_PM_OPS NULL
+#endif /* CONFIG_PM */
 
-		data->last_updated = jiffies;
-		data->valid = true;
-	}
-abort:
-	mutex_unlock(&data->update_lock);
-	return ret;
-}
+static const struct i2c_device_id jc42_id[] = {
+	{ "jc42", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, jc42_id);
+
+static struct i2c_driver jc42_driver = {
+	.class		= I2C_CLASS_SPD,
+	.driver = {
+		.name	= "jc42",
+		.pm = JC42_DEV_PM_OPS,
+	},
+	.probe		= jc42_probe,
+	.remove		= jc42_remove,
+	.id_table	= jc42_id,
+	.detect		= jc42_detect,
+	.address_list	= normal_i2c,
+};
 
 module_i2c_driver(jc42_driver);
 
diff --git a/drivers/hwmon/lm70.c b/drivers/hwmon/lm70.c
index 505a59e100b..97204dce162 100644
--- a/drivers/hwmon/lm70.c
+++ b/drivers/hwmon/lm70.c
@@ -47,7 +47,7 @@
 #define LM70_CHIP_LM74		3	/* NS LM74 */
 
 struct lm70 {
-	struct device *hwmon_dev;
+	struct spi_device *spi;
 	struct mutex lock;
 	unsigned int chip;
 };
@@ -56,11 +56,11 @@ struct lm70 {
 static ssize_t lm70_sense_temp(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
-	struct spi_device *spi = to_spi_device(dev);
+	struct lm70 *p_lm70 = dev_get_drvdata(dev);
+	struct spi_device *spi = p_lm70->spi;
 	int status, val = 0;
 	u8 rxbuf[2];
 	s16 raw = 0;
-	struct lm70 *p_lm70 = spi_get_drvdata(spi);
 
 	if (mutex_lock_interruptible(&p_lm70->lock))
 		return -ERESTARTSYS;
@@ -121,21 +121,20 @@ out:
 
 static DEVICE_ATTR(temp1_input, S_IRUGO, lm70_sense_temp, NULL);
 
-static ssize_t lm70_show_name(struct device *dev, struct device_attribute
-			      *devattr, char *buf)
-{
-	return sprintf(buf, "%s\n", to_spi_device(dev)->modalias);
-}
+static struct attribute *lm70_attrs[] = {
+	&dev_attr_temp1_input.attr,
+	NULL
+};
 
-static DEVICE_ATTR(name, S_IRUGO, lm70_show_name, NULL);
+ATTRIBUTE_GROUPS(lm70);
 
 /*----------------------------------------------------------------------*/
 
 static int lm70_probe(struct spi_device *spi)
 {
 	int chip = spi_get_device_id(spi)->driver_data;
+	struct device *hwmon_dev;
 	struct lm70 *p_lm70;
-	int status;
 
 	/* signaling is SPI_MODE_0 */
 	if (spi->mode & (SPI_CPOL | SPI_CPHA))
@@ -149,46 +148,14 @@ static int lm70_probe(struct spi_device *spi)
 
 	mutex_init(&p_lm70->lock);
 	p_lm70->chip = chip;
+	p_lm70->spi = spi;
 
-	spi_set_drvdata(spi, p_lm70);
-
-	status = device_create_file(&spi->dev, &dev_attr_temp1_input);
-	if (status)
-		goto out_dev_create_temp_file_failed;
-	status = device_create_file(&spi->dev, &dev_attr_name);
-	if (status)
-		goto out_dev_create_file_failed;
-
-	/* sysfs hook */
-	p_lm70->hwmon_dev = hwmon_device_register(&spi->dev);
-	if (IS_ERR(p_lm70->hwmon_dev)) {
-		dev_dbg(&spi->dev, "hwmon_device_register failed.\n");
-		status = PTR_ERR(p_lm70->hwmon_dev);
-		goto out_dev_reg_failed;
-	}
-
-	return 0;
-
-out_dev_reg_failed:
-	device_remove_file(&spi->dev, &dev_attr_name);
-out_dev_create_file_failed:
-	device_remove_file(&spi->dev, &dev_attr_temp1_input);
-out_dev_create_temp_file_failed:
-	return status;
+	hwmon_dev = devm_hwmon_device_register_with_groups(&spi->dev,
+							   spi->modalias,
+							   p_lm70, lm70_groups);
+	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
 
-static int lm70_remove(struct spi_device *spi)
-{
-	struct lm70 *p_lm70 = spi_get_drvdata(spi);
-
-	hwmon_device_unregister(p_lm70->hwmon_dev);
-	device_remove_file(&spi->dev, &dev_attr_temp1_input);
-	device_remove_file(&spi->dev, &dev_attr_name);
-
-	return 0;
-}
-
-
 static const struct spi_device_id lm70_ids[] = {
 	{ "lm70",   LM70_CHIP_LM70 },
 	{ "tmp121", LM70_CHIP_TMP121 },
@@ -205,7 +172,6 @@ static struct spi_driver lm70_driver = {
 	},
 	.id_table = lm70_ids,
 	.probe	= lm70_probe,
-	.remove	= lm70_remove,
 };
 
 module_spi_driver(lm70_driver);
diff --git a/drivers/hwmon/lm75.c b/drivers/hwmon/lm75.c
index 84a55eacd90..479ffbeed3f 100644
--- a/drivers/hwmon/lm75.c
+++ b/drivers/hwmon/lm75.c
@@ -72,6 +72,7 @@ static const u8 LM75_REG_TEMP[3] = {
 
 /* Each client has this additional data */
 struct lm75_data {
+	struct i2c_client	*client;
 	struct device		*hwmon_dev;
 	struct thermal_zone_device	*tz;
 	struct mutex		update_lock;
@@ -130,8 +131,8 @@ static ssize_t set_temp(struct device *dev, struct device_attribute *da,
 			const char *buf, size_t count)
 {
 	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
-	struct i2c_client *client = to_i2c_client(dev);
-	struct lm75_data *data = i2c_get_clientdata(client);
+	struct lm75_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
 	int nr = attr->index;
 	long temp;
 	int error;
@@ -165,17 +166,14 @@ static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IWUSR | S_IRUGO,
 			show_temp, set_temp, 2);
 static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_temp, NULL, 0);
 
-static struct attribute *lm75_attributes[] = {
+static struct attribute *lm75_attrs[] = {
 	&sensor_dev_attr_temp1_input.dev_attr.attr,
 	&sensor_dev_attr_temp1_max.dev_attr.attr,
 	&sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
 
 	NULL
 };
-
-static const struct attribute_group lm75_group = {
-	.attrs = lm75_attributes,
-};
+ATTRIBUTE_GROUPS(lm75);
 
 /*-----------------------------------------------------------------------*/
 
@@ -184,6 +182,7 @@ static const struct attribute_group lm75_group = {
 static int
 lm75_probe(struct i2c_client *client, const struct i2c_device_id *id)
 {
+	struct device *dev = &client->dev;
 	struct lm75_data *data;
 	int status;
 	u8 set_mask, clr_mask;
@@ -194,10 +193,11 @@ lm75_probe(struct i2c_client *client, const struct i2c_device_id *id)
 			I2C_FUNC_SMBUS_BYTE_DATA | I2C_FUNC_SMBUS_WORD_DATA))
 		return -EIO;
 
-	data = devm_kzalloc(&client->dev, sizeof(struct lm75_data), GFP_KERNEL);
+	data = devm_kzalloc(dev, sizeof(struct lm75_data), GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
+	data->client = client;
 	i2c_set_clientdata(client, data);
 	mutex_init(&data->update_lock);
 
@@ -269,7 +269,7 @@ lm75_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	/* configure as specified */
 	status = lm75_read_value(client, LM75_REG_CONF);
 	if (status < 0) {
-		dev_dbg(&client->dev, "Can't read config? %d\n", status);
+		dev_dbg(dev, "Can't read config? %d\n", status);
 		return status;
 	}
 	data->orig_conf = status;
@@ -277,43 +277,32 @@ lm75_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	new |= set_mask;
 	if (status != new)
 		lm75_write_value(client, LM75_REG_CONF, new);
-	dev_dbg(&client->dev, "Config %02x\n", new);
+	dev_dbg(dev, "Config %02x\n", new);
 
-	/* Register sysfs hooks */
-	status = sysfs_create_group(&client->dev.kobj, &lm75_group);
-	if (status)
-		return status;
+	data->hwmon_dev = hwmon_device_register_with_groups(dev, client->name,
+							    data, lm75_groups);
+	if (IS_ERR(data->hwmon_dev))
+		return PTR_ERR(data->hwmon_dev);
 
-	data->hwmon_dev = hwmon_device_register(&client->dev);
-	if (IS_ERR(data->hwmon_dev)) {
-		status = PTR_ERR(data->hwmon_dev);
-		goto exit_remove;
-	}
-
-	data->tz = thermal_zone_of_sensor_register(&client->dev,
+	data->tz = thermal_zone_of_sensor_register(data->hwmon_dev,
 						   0,
-						   &client->dev,
+						   data->hwmon_dev,
 						   lm75_read_temp, NULL);
 	if (IS_ERR(data->tz))
 		data->tz = NULL;
 
-	dev_info(&client->dev, "%s: sensor '%s'\n",
+	dev_info(dev, "%s: sensor '%s'\n",
 		 dev_name(data->hwmon_dev), client->name);
 
 	return 0;
-
-exit_remove:
-	sysfs_remove_group(&client->dev.kobj, &lm75_group);
-	return status;
 }
 
 static int lm75_remove(struct i2c_client *client)
 {
 	struct lm75_data *data = i2c_get_clientdata(client);
 
-	thermal_zone_of_sensor_unregister(&client->dev, data->tz);
+	thermal_zone_of_sensor_unregister(data->hwmon_dev, data->tz);
 	hwmon_device_unregister(data->hwmon_dev);
-	sysfs_remove_group(&client->dev.kobj, &lm75_group);
 	lm75_write_value(client, LM75_REG_CONF, data->orig_conf);
 	return 0;
 }
@@ -507,8 +496,8 @@ static int lm75_write_value(struct i2c_client *client, u8 reg, u16 value)
 
 static struct lm75_data *lm75_update_device(struct device *dev)
 {
-	struct i2c_client *client = to_i2c_client(dev);
-	struct lm75_data *data = i2c_get_clientdata(client);
+	struct lm75_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
 	struct lm75_data *ret = data;
 
 	mutex_lock(&data->update_lock);
diff --git a/drivers/hwmon/lm77.c b/drivers/hwmon/lm77.c
index 502771c06fd..5ceb443b938 100644
--- a/drivers/hwmon/lm77.c
+++ b/drivers/hwmon/lm77.c
@@ -19,10 +19,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>
@@ -47,50 +43,33 @@ static const unsigned short normal_i2c[] = { 0x48, 0x49, 0x4a, 0x4b,
 #define LM77_REG_TEMP_MIN	0x04
 #define LM77_REG_TEMP_MAX	0x05
 
+enum temp_index {
+	t_input = 0,
+	t_crit,
+	t_min,
+	t_max,
+	t_hyst,
+	t_num_temp
+};
+
+static const u8 temp_regs[t_num_temp] = {
+	[t_input] = LM77_REG_TEMP,
+	[t_min] = LM77_REG_TEMP_MIN,
+	[t_max] = LM77_REG_TEMP_MAX,
+	[t_crit] = LM77_REG_TEMP_CRIT,
+	[t_hyst] = LM77_REG_TEMP_HYST,
+};
+
 /* Each client has this additional data */
 struct lm77_data {
-	struct device		*hwmon_dev;
+	struct i2c_client	*client;
 	struct mutex		update_lock;
 	char			valid;
 	unsigned long		last_updated;	/* In jiffies */
-	int			temp_input;	/* Temperatures */
-	int			temp_crit;
-	int			temp_min;
-	int			temp_max;
-	int			temp_hyst;
+	int			temp[t_num_temp]; /* index using temp_index */
 	u8			alarms;
 };
 
-static int lm77_probe(struct i2c_client *client,
-		      const struct i2c_device_id *id);
-static int lm77_detect(struct i2c_client *client, struct i2c_board_info *info);
-static void lm77_init_client(struct i2c_client *client);
-static int lm77_remove(struct i2c_client *client);
-static u16 lm77_read_value(struct i2c_client *client, u8 reg);
-static int lm77_write_value(struct i2c_client *client, u8 reg, u16 value);
-
-static struct lm77_data *lm77_update_device(struct device *dev);
-
-
-static const struct i2c_device_id lm77_id[] = {
-	{ "lm77", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, lm77_id);
-
-/* This is the driver that will be inserted */
-static struct i2c_driver lm77_driver = {
-	.class		= I2C_CLASS_HWMON,
-	.driver = {
-		.name	= "lm77",
-	},
-	.probe		= lm77_probe,
-	.remove		= lm77_remove,
-	.id_table	= lm77_id,
-	.detect		= lm77_detect,
-	.address_list	= normal_i2c,
-};
-
 /* straight from the datasheet */
 #define LM77_TEMP_MIN (-55000)
 #define LM77_TEMP_MAX 125000
@@ -110,97 +89,109 @@ static inline int LM77_TEMP_FROM_REG(s16 reg)
 	return (reg / 8) * 500;
 }
 
-/* sysfs stuff */
-
-/* read routines for temperature limits */
-#define show(value)	\
-static ssize_t show_##value(struct device *dev,			\
-			    struct device_attribute *attr,	\
-			    char *buf)				\
-{								\
-	struct lm77_data *data = lm77_update_device(dev);	\
-	return sprintf(buf, "%d\n", data->value);		\
+/*
+ * All registers are word-sized, except for the configuration register.
+ * The LM77 uses the high-byte first convention.
+ */
+static u16 lm77_read_value(struct i2c_client *client, u8 reg)
+{
+	if (reg == LM77_REG_CONF)
+		return i2c_smbus_read_byte_data(client, reg);
+	else
+		return i2c_smbus_read_word_swapped(client, reg);
 }
 
-show(temp_input);
-show(temp_crit);
-show(temp_min);
-show(temp_max);
+static int lm77_write_value(struct i2c_client *client, u8 reg, u16 value)
+{
+	if (reg == LM77_REG_CONF)
+		return i2c_smbus_write_byte_data(client, reg, value);
+	else
+		return i2c_smbus_write_word_swapped(client, reg, value);
+}
 
-/* read routines for hysteresis values */
-static ssize_t show_temp_crit_hyst(struct device *dev,
-				   struct device_attribute *attr, char *buf)
+static struct lm77_data *lm77_update_device(struct device *dev)
 {
-	struct lm77_data *data = lm77_update_device(dev);
-	return sprintf(buf, "%d\n", data->temp_crit - data->temp_hyst);
+	struct lm77_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	int i;
+
+	mutex_lock(&data->update_lock);
+
+	if (time_after(jiffies, data->last_updated + HZ + HZ / 2)
+	    || !data->valid) {
+		dev_dbg(&client->dev, "Starting lm77 update\n");
+		for (i = 0; i < t_num_temp; i++) {
+			data->temp[i] =
+			  LM77_TEMP_FROM_REG(lm77_read_value(client,
+							     temp_regs[i]));
+		}
+		data->alarms =
+			lm77_read_value(client, LM77_REG_TEMP) & 0x0007;
+		data->last_updated = jiffies;
+		data->valid = 1;
+	}
+
+	mutex_unlock(&data->update_lock);
+
+	return data;
 }
-static ssize_t show_temp_min_hyst(struct device *dev,
-				  struct device_attribute *attr, char *buf)
+
+/* sysfs stuff */
+
+static ssize_t show_temp(struct device *dev, struct device_attribute *devattr,
+			 char *buf)
 {
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
 	struct lm77_data *data = lm77_update_device(dev);
-	return sprintf(buf, "%d\n", data->temp_min + data->temp_hyst);
+
+	return sprintf(buf, "%d\n", data->temp[attr->index]);
 }
-static ssize_t show_temp_max_hyst(struct device *dev,
-				  struct device_attribute *attr, char *buf)
+
+static ssize_t show_temp_hyst(struct device *dev,
+			      struct device_attribute *devattr, char *buf)
 {
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
 	struct lm77_data *data = lm77_update_device(dev);
-	return sprintf(buf, "%d\n", data->temp_max - data->temp_hyst);
-}
+	int nr = attr->index;
+	int temp;
 
-/* write routines */
-#define set(value, reg)	\
-static ssize_t set_##value(struct device *dev, struct device_attribute *attr, \
-			   const char *buf, size_t count)		\
-{									\
-	struct i2c_client *client = to_i2c_client(dev);			\
-	struct lm77_data *data = i2c_get_clientdata(client);		\
-	long val;							\
-	int err = kstrtol(buf, 10, &val);				\
-	if (err)							\
-		return err;						\
-									\
-	mutex_lock(&data->update_lock);					\
-	data->value = val;						\
-	lm77_write_value(client, reg, LM77_TEMP_TO_REG(data->value));	\
-	mutex_unlock(&data->update_lock);				\
-	return count;							\
-}
+	temp = nr == t_min ? data->temp[nr] + data->temp[t_hyst] :
+			     data->temp[nr] - data->temp[t_hyst];
 
-set(temp_min, LM77_REG_TEMP_MIN);
-set(temp_max, LM77_REG_TEMP_MAX);
+	return sprintf(buf, "%d\n", temp);
+}
 
-/*
- * hysteresis is stored as a relative value on the chip, so it has to be
- * converted first
- */
-static ssize_t set_temp_crit_hyst(struct device *dev,
-				  struct device_attribute *attr,
-				  const char *buf, size_t count)
+static ssize_t set_temp(struct device *dev, struct device_attribute *devattr,
+			const char *buf, size_t count)
 {
-	struct i2c_client *client = to_i2c_client(dev);
-	struct lm77_data *data = i2c_get_clientdata(client);
-	unsigned long val;
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct lm77_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	int nr = attr->index;
+	long val;
 	int err;
 
-	err = kstrtoul(buf, 10, &val);
+	err = kstrtol(buf, 10, &val);
 	if (err)
 		return err;
 
 	mutex_lock(&data->update_lock);
-	data->temp_hyst = data->temp_crit - val;
-	lm77_write_value(client, LM77_REG_TEMP_HYST,
-			 LM77_TEMP_TO_REG(data->temp_hyst));
+	data->temp[nr] = val;
+	lm77_write_value(client, temp_regs[nr], LM77_TEMP_TO_REG(val));
 	mutex_unlock(&data->update_lock);
 	return count;
 }
 
-/* preserve hysteresis when setting T_crit */
-static ssize_t set_temp_crit(struct device *dev, struct device_attribute *attr,
+/*
+ * hysteresis is stored as a relative value on the chip, so it has to be
+ * converted first.
+ */
+static ssize_t set_temp_hyst(struct device *dev,
+			     struct device_attribute *devattr,
 			     const char *buf, size_t count)
 {
-	struct i2c_client *client = to_i2c_client(dev);
-	struct lm77_data *data = i2c_get_clientdata(client);
-	int oldcrithyst;
+	struct lm77_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
 	unsigned long val;
 	int err;
 
@@ -209,13 +200,9 @@ static ssize_t set_temp_crit(struct device *dev, struct device_attribute *attr,
 		return err;
 
 	mutex_lock(&data->update_lock);
-	oldcrithyst = data->temp_crit - data->temp_hyst;
-	data->temp_crit = val;
-	data->temp_hyst = data->temp_crit - oldcrithyst;
-	lm77_write_value(client, LM77_REG_TEMP_CRIT,
-			 LM77_TEMP_TO_REG(data->temp_crit));
+	data->temp[t_hyst] = data->temp[t_crit] - val;
 	lm77_write_value(client, LM77_REG_TEMP_HYST,
-			 LM77_TEMP_TO_REG(data->temp_hyst));
+			 LM77_TEMP_TO_REG(data->temp[t_hyst]));
 	mutex_unlock(&data->update_lock);
 	return count;
 }
@@ -228,43 +215,37 @@ static ssize_t show_alarm(struct device *dev, struct device_attribute *attr,
 	return sprintf(buf, "%u\n", (data->alarms >> bitnr) & 1);
 }
 
-static DEVICE_ATTR(temp1_input, S_IRUGO,
-		   show_temp_input, NULL);
-static DEVICE_ATTR(temp1_crit, S_IWUSR | S_IRUGO,
-		   show_temp_crit, set_temp_crit);
-static DEVICE_ATTR(temp1_min, S_IWUSR | S_IRUGO,
-		   show_temp_min, set_temp_min);
-static DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO,
-		   show_temp_max, set_temp_max);
-
-static DEVICE_ATTR(temp1_crit_hyst, S_IWUSR | S_IRUGO,
-		   show_temp_crit_hyst, set_temp_crit_hyst);
-static DEVICE_ATTR(temp1_min_hyst, S_IRUGO,
-		   show_temp_min_hyst, NULL);
-static DEVICE_ATTR(temp1_max_hyst, S_IRUGO,
-		   show_temp_max_hyst, NULL);
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_temp, NULL, t_input);
+static SENSOR_DEVICE_ATTR(temp1_crit, S_IWUSR | S_IRUGO, show_temp, set_temp,
+			  t_crit);
+static SENSOR_DEVICE_ATTR(temp1_min, S_IWUSR | S_IRUGO, show_temp, set_temp,
+			  t_min);
+static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, show_temp, set_temp,
+			  t_max);
+
+static SENSOR_DEVICE_ATTR(temp1_crit_hyst, S_IWUSR | S_IRUGO, show_temp_hyst,
+			  set_temp_hyst, t_crit);
+static SENSOR_DEVICE_ATTR(temp1_min_hyst, S_IRUGO, show_temp_hyst, NULL, t_min);
+static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IRUGO, show_temp_hyst, NULL, t_max);
 
 static SENSOR_DEVICE_ATTR(temp1_crit_alarm, S_IRUGO, show_alarm, NULL, 2);
 static SENSOR_DEVICE_ATTR(temp1_min_alarm, S_IRUGO, show_alarm, NULL, 0);
 static SENSOR_DEVICE_ATTR(temp1_max_alarm, S_IRUGO, show_alarm, NULL, 1);
 
-static struct attribute *lm77_attributes[] = {
-	&dev_attr_temp1_input.attr,
-	&dev_attr_temp1_crit.attr,
-	&dev_attr_temp1_min.attr,
-	&dev_attr_temp1_max.attr,
-	&dev_attr_temp1_crit_hyst.attr,
-	&dev_attr_temp1_min_hyst.attr,
-	&dev_attr_temp1_max_hyst.attr,
+static struct attribute *lm77_attrs[] = {
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit.dev_attr.attr,
+	&sensor_dev_attr_temp1_min.dev_attr.attr,
+	&sensor_dev_attr_temp1_max.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp1_min_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
 	&sensor_dev_attr_temp1_crit_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp1_min_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
 	NULL
 };
-
-static const struct attribute_group lm77_group = {
-	.attrs = lm77_attributes,
-};
+ATTRIBUTE_GROUPS(lm77);
 
 /* Return 0 if detection is successful, -ENODEV otherwise */
 static int lm77_detect(struct i2c_client *client, struct i2c_board_info *info)
@@ -337,111 +318,52 @@ static int lm77_detect(struct i2c_client *client, struct i2c_board_info *info)
 	return 0;
 }
 
+static void lm77_init_client(struct i2c_client *client)
+{
+	/* Initialize the LM77 chip - turn off shutdown mode */
+	int conf = lm77_read_value(client, LM77_REG_CONF);
+	if (conf & 1)
+		lm77_write_value(client, LM77_REG_CONF, conf & 0xfe);
+}
+
 static int lm77_probe(struct i2c_client *client, const struct i2c_device_id *id)
 {
 	struct device *dev = &client->dev;
+	struct device *hwmon_dev;
 	struct lm77_data *data;
-	int err;
 
 	data = devm_kzalloc(dev, sizeof(struct lm77_data), GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
-	i2c_set_clientdata(client, data);
+	data->client = client;
 	mutex_init(&data->update_lock);
 
 	/* Initialize the LM77 chip */
 	lm77_init_client(client);
 
-	/* Register sysfs hooks */
-	err = sysfs_create_group(&dev->kobj, &lm77_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 exit_remove;
-	}
-
-	return 0;
-
-exit_remove:
-	sysfs_remove_group(&dev->kobj, &lm77_group);
-	return err;
+	hwmon_dev = devm_hwmon_device_register_with_groups(dev, client->name,
+							   data, lm77_groups);
+	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
 
-static int lm77_remove(struct i2c_client *client)
-{
-	struct lm77_data *data = i2c_get_clientdata(client);
-	hwmon_device_unregister(data->hwmon_dev);
-	sysfs_remove_group(&client->dev.kobj, &lm77_group);
-	return 0;
-}
-
-/*
- * All registers are word-sized, except for the configuration register.
- * The LM77 uses the high-byte first convention.
- */
-static u16 lm77_read_value(struct i2c_client *client, u8 reg)
-{
-	if (reg == LM77_REG_CONF)
-		return i2c_smbus_read_byte_data(client, reg);
-	else
-		return i2c_smbus_read_word_swapped(client, reg);
-}
-
-static int lm77_write_value(struct i2c_client *client, u8 reg, u16 value)
-{
-	if (reg == LM77_REG_CONF)
-		return i2c_smbus_write_byte_data(client, reg, value);
-	else
-		return i2c_smbus_write_word_swapped(client, reg, value);
-}
-
-static void lm77_init_client(struct i2c_client *client)
-{
-	/* Initialize the LM77 chip - turn off shutdown mode */
-	int conf = lm77_read_value(client, LM77_REG_CONF);
-	if (conf & 1)
-		lm77_write_value(client, LM77_REG_CONF, conf & 0xfe);
-}
-
-static struct lm77_data *lm77_update_device(struct device *dev)
-{
-	struct i2c_client *client = to_i2c_client(dev);
-	struct lm77_data *data = i2c_get_clientdata(client);
-
-	mutex_lock(&data->update_lock);
-
-	if (time_after(jiffies, data->last_updated + HZ + HZ / 2)
-	    || !data->valid) {
-		dev_dbg(&client->dev, "Starting lm77 update\n");
-		data->temp_input =
-			LM77_TEMP_FROM_REG(lm77_read_value(client,
-							   LM77_REG_TEMP));
-		data->temp_hyst =
-			LM77_TEMP_FROM_REG(lm77_read_value(client,
-							   LM77_REG_TEMP_HYST));
-		data->temp_crit =
-			LM77_TEMP_FROM_REG(lm77_read_value(client,
-							   LM77_REG_TEMP_CRIT));
-		data->temp_min =
-			LM77_TEMP_FROM_REG(lm77_read_value(client,
-							   LM77_REG_TEMP_MIN));
-		data->temp_max =
-			LM77_TEMP_FROM_REG(lm77_read_value(client,
-							   LM77_REG_TEMP_MAX));
-		data->alarms =
-			lm77_read_value(client, LM77_REG_TEMP) & 0x0007;
-		data->last_updated = jiffies;
-		data->valid = 1;
-	}
-
-	mutex_unlock(&data->update_lock);
+static const struct i2c_device_id lm77_id[] = {
+	{ "lm77", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, lm77_id);
 
-	return data;
-}
+/* This is the driver that will be inserted */
+static struct i2c_driver lm77_driver = {
+	.class		= I2C_CLASS_HWMON,
+	.driver = {
+		.name	= "lm77",
+	},
+	.probe		= lm77_probe,
+	.id_table	= lm77_id,
+	.detect		= lm77_detect,
+	.address_list	= normal_i2c,
+};
 
 module_i2c_driver(lm77_driver);
 
diff --git a/drivers/hwmon/lm80.c b/drivers/hwmon/lm80.c
index bd0a1ebbf86..4bcd9b88294 100644
--- a/drivers/hwmon/lm80.c
+++ b/drivers/hwmon/lm80.c
@@ -86,26 +86,41 @@ static inline unsigned char FAN_TO_REG(unsigned rpm, unsigned div)
 #define FAN_FROM_REG(val, div)	((val) == 0 ? -1 : \
 				(val) == 255 ? 0 : 1350000/((div) * (val)))
 
-static inline long TEMP_FROM_REG(u16 temp)
-{
-	long res;
+#define TEMP_FROM_REG(reg)	((reg) * 125 / 32)
+#define TEMP_TO_REG(temp)	(DIV_ROUND_CLOSEST(clamp_val((temp), \
+					-128000, 127000), 1000) << 8)
 
-	temp >>= 4;
-	if (temp < 0x0800)
-		res = 625 * (long) temp;
-	else
-		res = ((long) temp - 0x01000) * 625;
+#define DIV_FROM_REG(val)		(1 << (val))
 
-	return res / 10;
-}
+enum temp_index {
+	t_input = 0,
+	t_hot_max,
+	t_hot_hyst,
+	t_os_max,
+	t_os_hyst,
+	t_num_temp
+};
 
-#define TEMP_LIMIT_FROM_REG(val)	(((val) > 0x80 ? \
-	(val) - 0x100 : (val)) * 1000)
+static const u8 temp_regs[t_num_temp] = {
+	[t_input] = LM80_REG_TEMP,
+	[t_hot_max] = LM80_REG_TEMP_HOT_MAX,
+	[t_hot_hyst] = LM80_REG_TEMP_HOT_HYST,
+	[t_os_max] = LM80_REG_TEMP_OS_MAX,
+	[t_os_hyst] = LM80_REG_TEMP_OS_HYST,
+};
 
-#define TEMP_LIMIT_TO_REG(val)		clamp_val((val) < 0 ? \
-	((val) - 500) / 1000 : ((val) + 500) / 1000, 0, 255)
+enum in_index {
+	i_input = 0,
+	i_max,
+	i_min,
+	i_num_in
+};
 
-#define DIV_FROM_REG(val)		(1 << (val))
+enum fan_index {
+	f_input,
+	f_min,
+	f_num_fan
+};
 
 /*
  * Client data (each client gets its own)
@@ -118,106 +133,187 @@ struct lm80_data {
 	char valid;		/* !=0 if following fields are valid */
 	unsigned long last_updated;	/* In jiffies */
 
-	u8 in[7];		/* Register value */
-	u8 in_max[7];		/* Register value */
-	u8 in_min[7];		/* Register value */
-	u8 fan[2];		/* Register value */
-	u8 fan_min[2];		/* Register value */
+	u8 in[i_num_in][7];	/* Register value, 1st index is enum in_index */
+	u8 fan[f_num_fan][2];	/* Register value, 1st index enum fan_index */
 	u8 fan_div[2];		/* Register encoding, shifted right */
-	u16 temp;		/* Register values, shifted right */
-	u8 temp_hot_max;	/* Register value */
-	u8 temp_hot_hyst;	/* Register value */
-	u8 temp_os_max;		/* Register value */
-	u8 temp_os_hyst;	/* Register value */
+	s16 temp[t_num_temp];	/* Register values, normalized to 16 bit */
 	u16 alarms;		/* Register encoding, combined */
 };
 
-/*
- * Functions declaration
- */
+static int lm80_read_value(struct i2c_client *client, u8 reg)
+{
+	return i2c_smbus_read_byte_data(client, reg);
+}
 
-static int lm80_probe(struct i2c_client *client,
-		      const struct i2c_device_id *id);
-static int lm80_detect(struct i2c_client *client, struct i2c_board_info *info);
-static void lm80_init_client(struct i2c_client *client);
-static struct lm80_data *lm80_update_device(struct device *dev);
-static int lm80_read_value(struct i2c_client *client, u8 reg);
-static int lm80_write_value(struct i2c_client *client, u8 reg, u8 value);
+static int lm80_write_value(struct i2c_client *client, u8 reg, u8 value)
+{
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
 
-/*
- * Driver data (common to all clients)
- */
+/* Called when we have found a new LM80 and after read errors */
+static void lm80_init_client(struct i2c_client *client)
+{
+	/*
+	 * Reset all except Watchdog values and last conversion values
+	 * This sets fan-divs to 2, among others. This makes most other
+	 * initializations unnecessary
+	 */
+	lm80_write_value(client, LM80_REG_CONFIG, 0x80);
+	/* Set 11-bit temperature resolution */
+	lm80_write_value(client, LM80_REG_RES, 0x08);
 
-static const struct i2c_device_id lm80_id[] = {
-	{ "lm80", 0 },
-	{ "lm96080", 1 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, lm80_id);
+	/* Start monitoring */
+	lm80_write_value(client, LM80_REG_CONFIG, 0x01);
+}
 
-static struct i2c_driver lm80_driver = {
-	.class		= I2C_CLASS_HWMON,
-	.driver = {
-		.name	= "lm80",
-	},
-	.probe		= lm80_probe,
-	.id_table	= lm80_id,
-	.detect		= lm80_detect,
-	.address_list	= normal_i2c,
-};
+static struct lm80_data *lm80_update_device(struct device *dev)
+{
+	struct lm80_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	int i;
+	int rv;
+	int prev_rv;
+	struct lm80_data *ret = data;
+
+	mutex_lock(&data->update_lock);
+
+	if (data->error)
+		lm80_init_client(client);
+
+	if (time_after(jiffies, data->last_updated + 2 * HZ) || !data->valid) {
+		dev_dbg(dev, "Starting lm80 update\n");
+		for (i = 0; i <= 6; i++) {
+			rv = lm80_read_value(client, LM80_REG_IN(i));
+			if (rv < 0)
+				goto abort;
+			data->in[i_input][i] = rv;
+
+			rv = lm80_read_value(client, LM80_REG_IN_MIN(i));
+			if (rv < 0)
+				goto abort;
+			data->in[i_min][i] = rv;
+
+			rv = lm80_read_value(client, LM80_REG_IN_MAX(i));
+			if (rv < 0)
+				goto abort;
+			data->in[i_max][i] = rv;
+		}
+
+		rv = lm80_read_value(client, LM80_REG_FAN1);
+		if (rv < 0)
+			goto abort;
+		data->fan[f_input][0] = rv;
+
+		rv = lm80_read_value(client, LM80_REG_FAN_MIN(1));
+		if (rv < 0)
+			goto abort;
+		data->fan[f_min][0] = rv;
+
+		rv = lm80_read_value(client, LM80_REG_FAN2);
+		if (rv < 0)
+			goto abort;
+		data->fan[f_input][1] = rv;
+
+		rv = lm80_read_value(client, LM80_REG_FAN_MIN(2));
+		if (rv < 0)
+			goto abort;
+		data->fan[f_min][1] = rv;
+
+		prev_rv = rv = lm80_read_value(client, LM80_REG_TEMP);
+		if (rv < 0)
+			goto abort;
+		rv = lm80_read_value(client, LM80_REG_RES);
+		if (rv < 0)
+			goto abort;
+		data->temp[t_input] = (prev_rv << 8) | (rv & 0xf0);
+
+		for (i = t_input + 1; i < t_num_temp; i++) {
+			rv = lm80_read_value(client, temp_regs[i]);
+			if (rv < 0)
+				goto abort;
+			data->temp[i] = rv << 8;
+		}
+
+		rv = lm80_read_value(client, LM80_REG_FANDIV);
+		if (rv < 0)
+			goto abort;
+		data->fan_div[0] = (rv >> 2) & 0x03;
+		data->fan_div[1] = (rv >> 4) & 0x03;
+
+		prev_rv = rv = lm80_read_value(client, LM80_REG_ALARM1);
+		if (rv < 0)
+			goto abort;
+		rv = lm80_read_value(client, LM80_REG_ALARM2);
+		if (rv < 0)
+			goto abort;
+		data->alarms = prev_rv + (rv << 8);
+
+		data->last_updated = jiffies;
+		data->valid = 1;
+		data->error = 0;
+	}
+	goto done;
+
+abort:
+	ret = ERR_PTR(rv);
+	data->valid = 0;
+	data->error = 1;
+
+done:
+	mutex_unlock(&data->update_lock);
+
+	return ret;
+}
 
 /*
  * Sysfs stuff
  */
 
-#define show_in(suffix, value) \
-static ssize_t show_in_##suffix(struct device *dev, \
-	struct device_attribute *attr, char *buf) \
-{ \
-	int nr = to_sensor_dev_attr(attr)->index; \
-	struct lm80_data *data = lm80_update_device(dev); \
-	if (IS_ERR(data)) \
-		return PTR_ERR(data); \
-	return sprintf(buf, "%d\n", IN_FROM_REG(data->value[nr])); \
+static ssize_t show_in(struct device *dev, struct device_attribute *attr,
+		       char *buf)
+{
+	struct lm80_data *data = lm80_update_device(dev);
+	int index = to_sensor_dev_attr_2(attr)->index;
+	int nr = to_sensor_dev_attr_2(attr)->nr;
+
+	if (IS_ERR(data))
+		return PTR_ERR(data);
+	return sprintf(buf, "%d\n", IN_FROM_REG(data->in[nr][index]));
 }
-show_in(min, in_min)
-show_in(max, in_max)
-show_in(input, in)
-
-#define set_in(suffix, value, reg) \
-static ssize_t set_in_##suffix(struct device *dev, \
-	struct device_attribute *attr, const char *buf, size_t count) \
-{ \
-	int nr = to_sensor_dev_attr(attr)->index; \
-	struct lm80_data *data = dev_get_drvdata(dev); \
-	struct i2c_client *client = data->client; \
-	long val; \
-	int err = kstrtol(buf, 10, &val); \
-	if (err < 0) \
-		return err; \
-\
-	mutex_lock(&data->update_lock);\
-	data->value[nr] = IN_TO_REG(val); \
-	lm80_write_value(client, reg(nr), data->value[nr]); \
-	mutex_unlock(&data->update_lock);\
-	return count; \
+
+static ssize_t set_in(struct device *dev, struct device_attribute *attr,
+		      const char *buf, size_t count)
+{
+	struct lm80_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	int index = to_sensor_dev_attr_2(attr)->index;
+	int nr = to_sensor_dev_attr_2(attr)->nr;
+	long val;
+	u8 reg;
+	int err = kstrtol(buf, 10, &val);
+	if (err < 0)
+		return err;
+
+	reg = nr == i_min ? LM80_REG_IN_MIN(index) : LM80_REG_IN_MAX(index);
+
+	mutex_lock(&data->update_lock);
+	data->in[nr][index] = IN_TO_REG(val);
+	lm80_write_value(client, reg, data->in[nr][index]);
+	mutex_unlock(&data->update_lock);
+	return count;
 }
-set_in(min, in_min, LM80_REG_IN_MIN)
-set_in(max, in_max, LM80_REG_IN_MAX)
-
-#define show_fan(suffix, value) \
-static ssize_t show_fan_##suffix(struct device *dev, \
-	struct device_attribute *attr, char *buf) \
-{ \
-	int nr = to_sensor_dev_attr(attr)->index; \
-	struct lm80_data *data = lm80_update_device(dev); \
-	if (IS_ERR(data)) \
-		return PTR_ERR(data); \
-	return sprintf(buf, "%d\n", FAN_FROM_REG(data->value[nr], \
-		       DIV_FROM_REG(data->fan_div[nr]))); \
+
+static ssize_t show_fan(struct device *dev, struct device_attribute *attr,
+			char *buf)
+{
+	int index = to_sensor_dev_attr_2(attr)->index;
+	int nr = to_sensor_dev_attr_2(attr)->nr;
+	struct lm80_data *data = lm80_update_device(dev);
+	if (IS_ERR(data))
+		return PTR_ERR(data);
+	return sprintf(buf, "%d\n", FAN_FROM_REG(data->fan[nr][index],
+		       DIV_FROM_REG(data->fan_div[index])));
 }
-show_fan(min, fan_min)
-show_fan(input, fan)
 
 static ssize_t show_fan_div(struct device *dev, struct device_attribute *attr,
 	char *buf)
@@ -232,7 +328,8 @@ static ssize_t show_fan_div(struct device *dev, struct device_attribute *attr,
 static ssize_t set_fan_min(struct device *dev, struct device_attribute *attr,
 	const char *buf, size_t count)
 {
-	int nr = to_sensor_dev_attr(attr)->index;
+	int index = to_sensor_dev_attr_2(attr)->index;
+	int nr = to_sensor_dev_attr_2(attr)->nr;
 	struct lm80_data *data = dev_get_drvdata(dev);
 	struct i2c_client *client = data->client;
 	unsigned long val;
@@ -241,8 +338,10 @@ static ssize_t set_fan_min(struct device *dev, struct device_attribute *attr,
 		return err;
 
 	mutex_lock(&data->update_lock);
-	data->fan_min[nr] = FAN_TO_REG(val, DIV_FROM_REG(data->fan_div[nr]));
-	lm80_write_value(client, LM80_REG_FAN_MIN(nr + 1), data->fan_min[nr]);
+	data->fan[nr][index] = FAN_TO_REG(val,
+					  DIV_FROM_REG(data->fan_div[index]));
+	lm80_write_value(client, LM80_REG_FAN_MIN(index + 1),
+			 data->fan[nr][index]);
 	mutex_unlock(&data->update_lock);
 	return count;
 }
@@ -267,7 +366,7 @@ static ssize_t set_fan_div(struct device *dev, struct device_attribute *attr,
 
 	/* Save fan_min */
 	mutex_lock(&data->update_lock);
-	min = FAN_FROM_REG(data->fan_min[nr],
+	min = FAN_FROM_REG(data->fan[f_min][nr],
 			   DIV_FROM_REG(data->fan_div[nr]));
 
 	switch (val) {
@@ -291,62 +390,47 @@ static ssize_t set_fan_div(struct device *dev, struct device_attribute *attr,
 		return -EINVAL;
 	}
 
-	reg = (lm80_read_value(client, LM80_REG_FANDIV) & ~(3 << (2 * (nr + 1))))
-	    | (data->fan_div[nr] << (2 * (nr + 1)));
+	reg = (lm80_read_value(client, LM80_REG_FANDIV) &
+	       ~(3 << (2 * (nr + 1)))) | (data->fan_div[nr] << (2 * (nr + 1)));
 	lm80_write_value(client, LM80_REG_FANDIV, reg);
 
 	/* Restore fan_min */
-	data->fan_min[nr] = FAN_TO_REG(min, DIV_FROM_REG(data->fan_div[nr]));
-	lm80_write_value(client, LM80_REG_FAN_MIN(nr + 1), data->fan_min[nr]);
+	data->fan[f_min][nr] = FAN_TO_REG(min, DIV_FROM_REG(data->fan_div[nr]));
+	lm80_write_value(client, LM80_REG_FAN_MIN(nr + 1),
+			 data->fan[f_min][nr]);
 	mutex_unlock(&data->update_lock);
 
 	return count;
 }
 
-static ssize_t show_temp_input1(struct device *dev,
-	struct device_attribute *attr, char *buf)
+static ssize_t show_temp(struct device *dev, struct device_attribute *devattr,
+			 char *buf)
 {
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
 	struct lm80_data *data = lm80_update_device(dev);
 	if (IS_ERR(data))
 		return PTR_ERR(data);
-	return sprintf(buf, "%ld\n", TEMP_FROM_REG(data->temp));
+	return sprintf(buf, "%d\n", TEMP_FROM_REG(data->temp[attr->index]));
 }
 
-#define show_temp(suffix, value) \
-static ssize_t show_temp_##suffix(struct device *dev, \
-	struct device_attribute *attr, char *buf) \
-{ \
-	struct lm80_data *data = lm80_update_device(dev); \
-	if (IS_ERR(data)) \
-		return PTR_ERR(data); \
-	return sprintf(buf, "%d\n", TEMP_LIMIT_FROM_REG(data->value)); \
-}
-show_temp(hot_max, temp_hot_max);
-show_temp(hot_hyst, temp_hot_hyst);
-show_temp(os_max, temp_os_max);
-show_temp(os_hyst, temp_os_hyst);
-
-#define set_temp(suffix, value, reg) \
-static ssize_t set_temp_##suffix(struct device *dev, \
-	struct device_attribute *attr, const char *buf, size_t count) \
-{ \
-	struct lm80_data *data = dev_get_drvdata(dev); \
-	struct i2c_client *client = data->client; \
-	long val; \
-	int err = kstrtol(buf, 10, &val); \
-	if (err < 0) \
-		return err; \
-\
-	mutex_lock(&data->update_lock); \
-	data->value = TEMP_LIMIT_TO_REG(val); \
-	lm80_write_value(client, reg, data->value); \
-	mutex_unlock(&data->update_lock); \
-	return count; \
+static ssize_t set_temp(struct device *dev, struct device_attribute *devattr,
+			const char *buf, size_t count)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct lm80_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	int nr = attr->index;
+	long val;
+	int err = kstrtol(buf, 10, &val);
+	if (err < 0)
+		return err;
+
+	mutex_lock(&data->update_lock);
+	data->temp[nr] = TEMP_TO_REG(val);
+	lm80_write_value(client, temp_regs[nr], data->temp[nr] >> 8);
+	mutex_unlock(&data->update_lock);
+	return count;
 }
-set_temp(hot_max, temp_hot_max, LM80_REG_TEMP_HOT_MAX);
-set_temp(hot_hyst, temp_hot_hyst, LM80_REG_TEMP_HOT_HYST);
-set_temp(os_max, temp_os_max, LM80_REG_TEMP_OS_MAX);
-set_temp(os_hyst, temp_os_hyst, LM80_REG_TEMP_OS_HYST);
 
 static ssize_t show_alarms(struct device *dev, struct device_attribute *attr,
 			   char *buf)
@@ -367,60 +451,60 @@ static ssize_t show_alarm(struct device *dev, struct device_attribute *attr,
 	return sprintf(buf, "%u\n", (data->alarms >> bitnr) & 1);
 }
 
-static SENSOR_DEVICE_ATTR(in0_min, S_IWUSR | S_IRUGO,
-		show_in_min, set_in_min, 0);
-static SENSOR_DEVICE_ATTR(in1_min, S_IWUSR | S_IRUGO,
-		show_in_min, set_in_min, 1);
-static SENSOR_DEVICE_ATTR(in2_min, S_IWUSR | S_IRUGO,
-		show_in_min, set_in_min, 2);
-static SENSOR_DEVICE_ATTR(in3_min, S_IWUSR | S_IRUGO,
-		show_in_min, set_in_min, 3);
-static SENSOR_DEVICE_ATTR(in4_min, S_IWUSR | S_IRUGO,
-		show_in_min, set_in_min, 4);
-static SENSOR_DEVICE_ATTR(in5_min, S_IWUSR | S_IRUGO,
-		show_in_min, set_in_min, 5);
-static SENSOR_DEVICE_ATTR(in6_min, S_IWUSR | S_IRUGO,
-		show_in_min, set_in_min, 6);
-static SENSOR_DEVICE_ATTR(in0_max, S_IWUSR | S_IRUGO,
-		show_in_max, set_in_max, 0);
-static SENSOR_DEVICE_ATTR(in1_max, S_IWUSR | S_IRUGO,
-		show_in_max, set_in_max, 1);
-static SENSOR_DEVICE_ATTR(in2_max, S_IWUSR | S_IRUGO,
-		show_in_max, set_in_max, 2);
-static SENSOR_DEVICE_ATTR(in3_max, S_IWUSR | S_IRUGO,
-		show_in_max, set_in_max, 3);
-static SENSOR_DEVICE_ATTR(in4_max, S_IWUSR | S_IRUGO,
-		show_in_max, set_in_max, 4);
-static SENSOR_DEVICE_ATTR(in5_max, S_IWUSR | S_IRUGO,
-		show_in_max, set_in_max, 5);
-static SENSOR_DEVICE_ATTR(in6_max, S_IWUSR | S_IRUGO,
-		show_in_max, set_in_max, 6);
-static SENSOR_DEVICE_ATTR(in0_input, S_IRUGO, show_in_input, NULL, 0);
-static SENSOR_DEVICE_ATTR(in1_input, S_IRUGO, show_in_input, NULL, 1);
-static SENSOR_DEVICE_ATTR(in2_input, S_IRUGO, show_in_input, NULL, 2);
-static SENSOR_DEVICE_ATTR(in3_input, S_IRUGO, show_in_input, NULL, 3);
-static SENSOR_DEVICE_ATTR(in4_input, S_IRUGO, show_in_input, NULL, 4);
-static SENSOR_DEVICE_ATTR(in5_input, S_IRUGO, show_in_input, NULL, 5);
-static SENSOR_DEVICE_ATTR(in6_input, S_IRUGO, show_in_input, NULL, 6);
-static SENSOR_DEVICE_ATTR(fan1_min, S_IWUSR | S_IRUGO,
-		show_fan_min, set_fan_min, 0);
-static SENSOR_DEVICE_ATTR(fan2_min, S_IWUSR | S_IRUGO,
-		show_fan_min, set_fan_min, 1);
-static SENSOR_DEVICE_ATTR(fan1_input, S_IRUGO, show_fan_input, NULL, 0);
-static SENSOR_DEVICE_ATTR(fan2_input, S_IRUGO, show_fan_input, NULL, 1);
+static SENSOR_DEVICE_ATTR_2(in0_min, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_min, 0);
+static SENSOR_DEVICE_ATTR_2(in1_min, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_min, 1);
+static SENSOR_DEVICE_ATTR_2(in2_min, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_min, 2);
+static SENSOR_DEVICE_ATTR_2(in3_min, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_min, 3);
+static SENSOR_DEVICE_ATTR_2(in4_min, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_min, 4);
+static SENSOR_DEVICE_ATTR_2(in5_min, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_min, 5);
+static SENSOR_DEVICE_ATTR_2(in6_min, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_min, 6);
+static SENSOR_DEVICE_ATTR_2(in0_max, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_max, 0);
+static SENSOR_DEVICE_ATTR_2(in1_max, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_max, 1);
+static SENSOR_DEVICE_ATTR_2(in2_max, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_max, 2);
+static SENSOR_DEVICE_ATTR_2(in3_max, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_max, 3);
+static SENSOR_DEVICE_ATTR_2(in4_max, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_max, 4);
+static SENSOR_DEVICE_ATTR_2(in5_max, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_max, 5);
+static SENSOR_DEVICE_ATTR_2(in6_max, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_max, 6);
+static SENSOR_DEVICE_ATTR_2(in0_input, S_IRUGO, show_in, NULL, i_input, 0);
+static SENSOR_DEVICE_ATTR_2(in1_input, S_IRUGO, show_in, NULL, i_input, 1);
+static SENSOR_DEVICE_ATTR_2(in2_input, S_IRUGO, show_in, NULL, i_input, 2);
+static SENSOR_DEVICE_ATTR_2(in3_input, S_IRUGO, show_in, NULL, i_input, 3);
+static SENSOR_DEVICE_ATTR_2(in4_input, S_IRUGO, show_in, NULL, i_input, 4);
+static SENSOR_DEVICE_ATTR_2(in5_input, S_IRUGO, show_in, NULL, i_input, 5);
+static SENSOR_DEVICE_ATTR_2(in6_input, S_IRUGO, show_in, NULL, i_input, 6);
+static SENSOR_DEVICE_ATTR_2(fan1_min, S_IWUSR | S_IRUGO,
+		show_fan, set_fan_min, f_min, 0);
+static SENSOR_DEVICE_ATTR_2(fan2_min, S_IWUSR | S_IRUGO,
+		show_fan, set_fan_min, f_min, 1);
+static SENSOR_DEVICE_ATTR_2(fan1_input, S_IRUGO, show_fan, NULL, f_input, 0);
+static SENSOR_DEVICE_ATTR_2(fan2_input, S_IRUGO, show_fan, NULL, f_input, 1);
 static SENSOR_DEVICE_ATTR(fan1_div, S_IWUSR | S_IRUGO,
 		show_fan_div, set_fan_div, 0);
 static SENSOR_DEVICE_ATTR(fan2_div, S_IWUSR | S_IRUGO,
 		show_fan_div, set_fan_div, 1);
-static DEVICE_ATTR(temp1_input, S_IRUGO, show_temp_input1, NULL);
-static DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, show_temp_hot_max,
-	set_temp_hot_max);
-static DEVICE_ATTR(temp1_max_hyst, S_IWUSR | S_IRUGO, show_temp_hot_hyst,
-	set_temp_hot_hyst);
-static DEVICE_ATTR(temp1_crit, S_IWUSR | S_IRUGO, show_temp_os_max,
-	set_temp_os_max);
-static DEVICE_ATTR(temp1_crit_hyst, S_IWUSR | S_IRUGO, show_temp_os_hyst,
-	set_temp_os_hyst);
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_temp, NULL, t_input);
+static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, show_temp,
+		set_temp, t_hot_max);
+static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IWUSR | S_IRUGO, show_temp,
+		set_temp, t_hot_hyst);
+static SENSOR_DEVICE_ATTR(temp1_crit, S_IWUSR | S_IRUGO, show_temp,
+		set_temp, t_os_max);
+static SENSOR_DEVICE_ATTR(temp1_crit_hyst, S_IWUSR | S_IRUGO, show_temp,
+		set_temp, t_os_hyst);
 static DEVICE_ATTR(alarms, S_IRUGO, show_alarms, NULL);
 static SENSOR_DEVICE_ATTR(in0_alarm, S_IRUGO, show_alarm, NULL, 0);
 static SENSOR_DEVICE_ATTR(in1_alarm, S_IRUGO, show_alarm, NULL, 1);
@@ -466,11 +550,11 @@ static struct attribute *lm80_attrs[] = {
 	&sensor_dev_attr_fan2_input.dev_attr.attr,
 	&sensor_dev_attr_fan1_div.dev_attr.attr,
 	&sensor_dev_attr_fan2_div.dev_attr.attr,
-	&dev_attr_temp1_input.attr,
-	&dev_attr_temp1_max.attr,
-	&dev_attr_temp1_max_hyst.attr,
-	&dev_attr_temp1_crit.attr,
-	&dev_attr_temp1_crit_hyst.attr,
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_temp1_max.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit_hyst.dev_attr.attr,
 	&dev_attr_alarms.attr,
 	&sensor_dev_attr_in0_alarm.dev_attr.attr,
 	&sensor_dev_attr_in1_alarm.dev_attr.attr,
@@ -551,8 +635,8 @@ static int lm80_probe(struct i2c_client *client,
 	lm80_init_client(client);
 
 	/* A few vars need to be filled upon startup */
-	data->fan_min[0] = lm80_read_value(client, LM80_REG_FAN_MIN(1));
-	data->fan_min[1] = lm80_read_value(client, LM80_REG_FAN_MIN(2));
+	data->fan[f_min][0] = lm80_read_value(client, LM80_REG_FAN_MIN(1));
+	data->fan[f_min][1] = lm80_read_value(client, LM80_REG_FAN_MIN(2));
 
 	hwmon_dev = devm_hwmon_device_register_with_groups(dev, client->name,
 							   data, lm80_groups);
@@ -560,143 +644,27 @@ static int lm80_probe(struct i2c_client *client,
 	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
 
-static int lm80_read_value(struct i2c_client *client, u8 reg)
-{
-	return i2c_smbus_read_byte_data(client, reg);
-}
-
-static int lm80_write_value(struct i2c_client *client, u8 reg, u8 value)
-{
-	return i2c_smbus_write_byte_data(client, reg, value);
-}
-
-/* Called when we have found a new LM80. */
-static void lm80_init_client(struct i2c_client *client)
-{
-	/*
-	 * Reset all except Watchdog values and last conversion values
-	 * This sets fan-divs to 2, among others. This makes most other
-	 * initializations unnecessary
-	 */
-	lm80_write_value(client, LM80_REG_CONFIG, 0x80);
-	/* Set 11-bit temperature resolution */
-	lm80_write_value(client, LM80_REG_RES, 0x08);
-
-	/* Start monitoring */
-	lm80_write_value(client, LM80_REG_CONFIG, 0x01);
-}
-
-static struct lm80_data *lm80_update_device(struct device *dev)
-{
-	struct lm80_data *data = dev_get_drvdata(dev);
-	struct i2c_client *client = data->client;
-	int i;
-	int rv;
-	int prev_rv;
-	struct lm80_data *ret = data;
-
-	mutex_lock(&data->update_lock);
-
-	if (data->error)
-		lm80_init_client(client);
-
-	if (time_after(jiffies, data->last_updated + 2 * HZ) || !data->valid) {
-		dev_dbg(dev, "Starting lm80 update\n");
-		for (i = 0; i <= 6; i++) {
-			rv = lm80_read_value(client, LM80_REG_IN(i));
-			if (rv < 0)
-				goto abort;
-			data->in[i] = rv;
-
-			rv = lm80_read_value(client, LM80_REG_IN_MIN(i));
-			if (rv < 0)
-				goto abort;
-			data->in_min[i] = rv;
-
-			rv = lm80_read_value(client, LM80_REG_IN_MAX(i));
-			if (rv < 0)
-				goto abort;
-			data->in_max[i] = rv;
-		}
-
-		rv = lm80_read_value(client, LM80_REG_FAN1);
-		if (rv < 0)
-			goto abort;
-		data->fan[0] = rv;
-
-		rv = lm80_read_value(client, LM80_REG_FAN_MIN(1));
-		if (rv < 0)
-			goto abort;
-		data->fan_min[0] = rv;
-
-		rv = lm80_read_value(client, LM80_REG_FAN2);
-		if (rv < 0)
-			goto abort;
-		data->fan[1] = rv;
-
-		rv = lm80_read_value(client, LM80_REG_FAN_MIN(2));
-		if (rv < 0)
-			goto abort;
-		data->fan_min[1] = rv;
-
-		prev_rv = rv = lm80_read_value(client, LM80_REG_TEMP);
-		if (rv < 0)
-			goto abort;
-		rv = lm80_read_value(client, LM80_REG_RES);
-		if (rv < 0)
-			goto abort;
-		data->temp = (prev_rv << 8) | (rv & 0xf0);
-
-		rv = lm80_read_value(client, LM80_REG_TEMP_OS_MAX);
-		if (rv < 0)
-			goto abort;
-		data->temp_os_max = rv;
-
-		rv = lm80_read_value(client, LM80_REG_TEMP_OS_HYST);
-		if (rv < 0)
-			goto abort;
-		data->temp_os_hyst = rv;
-
-		rv = lm80_read_value(client, LM80_REG_TEMP_HOT_MAX);
-		if (rv < 0)
-			goto abort;
-		data->temp_hot_max = rv;
-
-		rv = lm80_read_value(client, LM80_REG_TEMP_HOT_HYST);
-		if (rv < 0)
-			goto abort;
-		data->temp_hot_hyst = rv;
-
-		rv = lm80_read_value(client, LM80_REG_FANDIV);
-		if (rv < 0)
-			goto abort;
-		data->fan_div[0] = (rv >> 2) & 0x03;
-		data->fan_div[1] = (rv >> 4) & 0x03;
-
-		prev_rv = rv = lm80_read_value(client, LM80_REG_ALARM1);
-		if (rv < 0)
-			goto abort;
-		rv = lm80_read_value(client, LM80_REG_ALARM2);
-		if (rv < 0)
-			goto abort;
-		data->alarms = prev_rv + (rv << 8);
-
-		data->last_updated = jiffies;
-		data->valid = 1;
-		data->error = 0;
-	}
-	goto done;
-
-abort:
-	ret = ERR_PTR(rv);
-	data->valid = 0;
-	data->error = 1;
+/*
+ * Driver data (common to all clients)
+ */
 
-done:
-	mutex_unlock(&data->update_lock);
+static const struct i2c_device_id lm80_id[] = {
+	{ "lm80", 0 },
+	{ "lm96080", 1 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, lm80_id);
 
-	return ret;
-}
+static struct i2c_driver lm80_driver = {
+	.class		= I2C_CLASS_HWMON,
+	.driver = {
+		.name	= "lm80",
+	},
+	.probe		= lm80_probe,
+	.id_table	= lm80_id,
+	.detect		= lm80_detect,
+	.address_list	= normal_i2c,
+};
 
 module_i2c_driver(lm80_driver);
 
diff --git a/drivers/hwmon/lm83.c b/drivers/hwmon/lm83.c
index be02155788c..9e4d0e1d3c4 100644
--- a/drivers/hwmon/lm83.c
+++ b/drivers/hwmon/lm83.c
@@ -25,10 +25,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>
@@ -111,45 +107,12 @@ static const u8 LM83_REG_W_HIGH[] = {
 };
 
 /*
- * Functions declaration
- */
-
-static int lm83_detect(struct i2c_client *new_client,
-		       struct i2c_board_info *info);
-static int lm83_probe(struct i2c_client *client,
-		      const struct i2c_device_id *id);
-static int lm83_remove(struct i2c_client *client);
-static struct lm83_data *lm83_update_device(struct device *dev);
-
-/*
- * Driver data (common to all clients)
- */
-
-static const struct i2c_device_id lm83_id[] = {
-	{ "lm83", lm83 },
-	{ "lm82", lm82 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, lm83_id);
-
-static struct i2c_driver lm83_driver = {
-	.class		= I2C_CLASS_HWMON,
-	.driver = {
-		.name	= "lm83",
-	},
-	.probe		= lm83_probe,
-	.remove		= lm83_remove,
-	.id_table	= lm83_id,
-	.detect		= lm83_detect,
-	.address_list	= normal_i2c,
-};
-
-/*
  * Client data (each client gets its own)
  */
 
 struct lm83_data {
-	struct device *hwmon_dev;
+	struct i2c_client *client;
+	const struct attribute_group *groups[3];
 	struct mutex update_lock;
 	char valid; /* zero until following fields are valid */
 	unsigned long last_updated; /* in jiffies */
@@ -161,6 +124,36 @@ struct lm83_data {
 	u16 alarms; /* bitvector, combined */
 };
 
+static struct lm83_data *lm83_update_device(struct device *dev)
+{
+	struct lm83_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+
+	mutex_lock(&data->update_lock);
+
+	if (time_after(jiffies, data->last_updated + HZ * 2) || !data->valid) {
+		int nr;
+
+		dev_dbg(&client->dev, "Updating lm83 data.\n");
+		for (nr = 0; nr < 9; nr++) {
+			data->temp[nr] =
+			    i2c_smbus_read_byte_data(client,
+			    LM83_REG_R_TEMP[nr]);
+		}
+		data->alarms =
+		    i2c_smbus_read_byte_data(client, LM83_REG_R_STATUS1)
+		    + (i2c_smbus_read_byte_data(client, LM83_REG_R_STATUS2)
+		    << 8);
+
+		data->last_updated = jiffies;
+		data->valid = 1;
+	}
+
+	mutex_unlock(&data->update_lock);
+
+	return data;
+}
+
 /*
  * Sysfs stuff
  */
@@ -177,8 +170,8 @@ static ssize_t set_temp(struct device *dev, struct device_attribute *devattr,
 			const char *buf, size_t count)
 {
 	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
-	struct i2c_client *client = to_i2c_client(dev);
-	struct lm83_data *data = i2c_get_clientdata(client);
+	struct lm83_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
 	long val;
 	int nr = attr->index;
 	int err;
@@ -340,15 +333,15 @@ static int lm83_detect(struct i2c_client *new_client,
 static int lm83_probe(struct i2c_client *new_client,
 		      const struct i2c_device_id *id)
 {
+	struct device *hwmon_dev;
 	struct lm83_data *data;
-	int err;
 
 	data = devm_kzalloc(&new_client->dev, sizeof(struct lm83_data),
 			    GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
-	i2c_set_clientdata(new_client, data);
+	data->client = new_client;
 	mutex_init(&data->update_lock);
 
 	/*
@@ -357,72 +350,37 @@ static int lm83_probe(struct i2c_client *new_client,
 	 * at the same register as the LM83 temp3 entry - so we
 	 * declare 1 and 3 common, and then 2 and 4 only for the LM83.
 	 */
-
-	err = sysfs_create_group(&new_client->dev.kobj, &lm83_group);
-	if (err)
-		return err;
-
-	if (id->driver_data == lm83) {
-		err = sysfs_create_group(&new_client->dev.kobj,
-					 &lm83_group_opt);
-		if (err)
-			goto exit_remove_files;
-	}
-
-	data->hwmon_dev = hwmon_device_register(&new_client->dev);
-	if (IS_ERR(data->hwmon_dev)) {
-		err = PTR_ERR(data->hwmon_dev);
-		goto exit_remove_files;
-	}
-
-	return 0;
-
-exit_remove_files:
-	sysfs_remove_group(&new_client->dev.kobj, &lm83_group);
-	sysfs_remove_group(&new_client->dev.kobj, &lm83_group_opt);
-	return err;
-}
-
-static int lm83_remove(struct i2c_client *client)
-{
-	struct lm83_data *data = i2c_get_clientdata(client);
-
-	hwmon_device_unregister(data->hwmon_dev);
-	sysfs_remove_group(&client->dev.kobj, &lm83_group);
-	sysfs_remove_group(&client->dev.kobj, &lm83_group_opt);
-
-	return 0;
+	data->groups[0] = &lm83_group;
+	if (id->driver_data == lm83)
+		data->groups[1] = &lm83_group_opt;
+
+	hwmon_dev = devm_hwmon_device_register_with_groups(&new_client->dev,
+							   new_client->name,
+							   data, data->groups);
+	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
 
-static struct lm83_data *lm83_update_device(struct device *dev)
-{
-	struct i2c_client *client = to_i2c_client(dev);
-	struct lm83_data *data = i2c_get_clientdata(client);
-
-	mutex_lock(&data->update_lock);
-
-	if (time_after(jiffies, data->last_updated + HZ * 2) || !data->valid) {
-		int nr;
-
-		dev_dbg(&client->dev, "Updating lm83 data.\n");
-		for (nr = 0; nr < 9; nr++) {
-			data->temp[nr] =
-			    i2c_smbus_read_byte_data(client,
-			    LM83_REG_R_TEMP[nr]);
-		}
-		data->alarms =
-		    i2c_smbus_read_byte_data(client, LM83_REG_R_STATUS1)
-		    + (i2c_smbus_read_byte_data(client, LM83_REG_R_STATUS2)
-		    << 8);
-
-		data->last_updated = jiffies;
-		data->valid = 1;
-	}
+/*
+ * Driver data (common to all clients)
+ */
 
-	mutex_unlock(&data->update_lock);
+static const struct i2c_device_id lm83_id[] = {
+	{ "lm83", lm83 },
+	{ "lm82", lm82 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, lm83_id);
 
-	return data;
-}
+static struct i2c_driver lm83_driver = {
+	.class		= I2C_CLASS_HWMON,
+	.driver = {
+		.name	= "lm83",
+	},
+	.probe		= lm83_probe,
+	.id_table	= lm83_id,
+	.detect		= lm83_detect,
+	.address_list	= normal_i2c,
+};
 
 module_i2c_driver(lm83_driver);
 
diff --git a/drivers/hwmon/lm92.c b/drivers/hwmon/lm92.c
index b9022db6511..d2060e245ff 100644
--- a/drivers/hwmon/lm92.c
+++ b/drivers/hwmon/lm92.c
@@ -34,10 +34,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>
@@ -93,46 +89,53 @@ static inline u8 ALARMS_FROM_REG(s16 reg)
 	return reg & 0x0007;
 }
 
-/* Driver data (common to all clients) */
-static struct i2c_driver lm92_driver;
+enum temp_index {
+	t_input,
+	t_crit,
+	t_min,
+	t_max,
+	t_hyst,
+	t_num_regs
+};
+
+static const u8 regs[t_num_regs] = {
+	[t_input] = LM92_REG_TEMP,
+	[t_crit] = LM92_REG_TEMP_CRIT,
+	[t_min] = LM92_REG_TEMP_LOW,
+	[t_max] = LM92_REG_TEMP_HIGH,
+	[t_hyst] = LM92_REG_TEMP_HYST,
+};
 
 /* Client data (each client gets its own) */
 struct lm92_data {
-	struct device *hwmon_dev;
+	struct i2c_client *client;
 	struct mutex update_lock;
 	char valid; /* zero until following fields are valid */
 	unsigned long last_updated; /* in jiffies */
 
 	/* registers values */
-	s16 temp1_input, temp1_crit, temp1_min, temp1_max, temp1_hyst;
+	s16 temp[t_num_regs];	/* index with enum temp_index */
 };
 
-
 /*
  * Sysfs attributes and callback functions
  */
 
 static struct lm92_data *lm92_update_device(struct device *dev)
 {
-	struct i2c_client *client = to_i2c_client(dev);
-	struct lm92_data *data = i2c_get_clientdata(client);
+	struct lm92_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	int i;
 
 	mutex_lock(&data->update_lock);
 
 	if (time_after(jiffies, data->last_updated + HZ)
 	 || !data->valid) {
 		dev_dbg(&client->dev, "Updating lm92 data\n");
-		data->temp1_input = i2c_smbus_read_word_swapped(client,
-				    LM92_REG_TEMP);
-		data->temp1_hyst = i2c_smbus_read_word_swapped(client,
-				    LM92_REG_TEMP_HYST);
-		data->temp1_crit = i2c_smbus_read_word_swapped(client,
-				    LM92_REG_TEMP_CRIT);
-		data->temp1_min = i2c_smbus_read_word_swapped(client,
-				    LM92_REG_TEMP_LOW);
-		data->temp1_max = i2c_smbus_read_word_swapped(client,
-				    LM92_REG_TEMP_HIGH);
-
+		for (i = 0; i < t_num_regs; i++) {
+			data->temp[i] =
+				i2c_smbus_read_word_swapped(client, regs[i]);
+		}
 		data->last_updated = jiffies;
 		data->valid = 1;
 	}
@@ -142,68 +145,60 @@ static struct lm92_data *lm92_update_device(struct device *dev)
 	return data;
 }
 
-#define show_temp(value) \
-static ssize_t show_##value(struct device *dev, struct device_attribute *attr, \
-			    char *buf) \
-{ \
-	struct lm92_data *data = lm92_update_device(dev); \
-	return sprintf(buf, "%d\n", TEMP_FROM_REG(data->value)); \
-}
-show_temp(temp1_input);
-show_temp(temp1_crit);
-show_temp(temp1_min);
-show_temp(temp1_max);
-
-#define set_temp(value, reg) \
-static ssize_t set_##value(struct device *dev, struct device_attribute *attr, \
-			   const char *buf, \
-	size_t count) \
-{ \
-	struct i2c_client *client = to_i2c_client(dev); \
-	struct lm92_data *data = i2c_get_clientdata(client); \
-	long val; \
-	int err = kstrtol(buf, 10, &val); \
-	if (err) \
-		return err; \
-\
-	mutex_lock(&data->update_lock); \
-	data->value = TEMP_TO_REG(val); \
-	i2c_smbus_write_word_swapped(client, reg, data->value); \
-	mutex_unlock(&data->update_lock); \
-	return count; \
+static ssize_t show_temp(struct device *dev, struct device_attribute *devattr,
+			 char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct lm92_data *data = lm92_update_device(dev);
+
+	return sprintf(buf, "%d\n", TEMP_FROM_REG(data->temp[attr->index]));
 }
-set_temp(temp1_crit, LM92_REG_TEMP_CRIT);
-set_temp(temp1_min, LM92_REG_TEMP_LOW);
-set_temp(temp1_max, LM92_REG_TEMP_HIGH);
 
-static ssize_t show_temp1_crit_hyst(struct device *dev,
-				    struct device_attribute *attr, char *buf)
+static ssize_t set_temp(struct device *dev, struct device_attribute *devattr,
+			   const char *buf, size_t count)
 {
-	struct lm92_data *data = lm92_update_device(dev);
-	return sprintf(buf, "%d\n", TEMP_FROM_REG(data->temp1_crit)
-		       - TEMP_FROM_REG(data->temp1_hyst));
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct lm92_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	int nr = attr->index;
+	long val;
+	int err;
+	
+	err = kstrtol(buf, 10, &val);
+	if (err)
+		return err;
+
+	mutex_lock(&data->update_lock);
+	data->temp[nr] = TEMP_TO_REG(val);
+	i2c_smbus_write_word_swapped(client, regs[nr], data->temp[nr]);
+	mutex_unlock(&data->update_lock);
+	return count;
 }
-static ssize_t show_temp1_max_hyst(struct device *dev,
-				   struct device_attribute *attr, char *buf)
+
+static ssize_t show_temp_hyst(struct device *dev,
+			      struct device_attribute *devattr, char *buf)
 {
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
 	struct lm92_data *data = lm92_update_device(dev);
-	return sprintf(buf, "%d\n", TEMP_FROM_REG(data->temp1_max)
-		       - TEMP_FROM_REG(data->temp1_hyst));
+	return sprintf(buf, "%d\n", TEMP_FROM_REG(data->temp[attr->index])
+		       - TEMP_FROM_REG(data->temp[t_hyst]));
 }
-static ssize_t show_temp1_min_hyst(struct device *dev,
-				   struct device_attribute *attr, char *buf)
+
+static ssize_t show_temp_min_hyst(struct device *dev,
+				  struct device_attribute *attr, char *buf)
 {
 	struct lm92_data *data = lm92_update_device(dev);
-	return sprintf(buf, "%d\n", TEMP_FROM_REG(data->temp1_min)
-		       + TEMP_FROM_REG(data->temp1_hyst));
+	return sprintf(buf, "%d\n", TEMP_FROM_REG(data->temp[t_min])
+		       + TEMP_FROM_REG(data->temp[t_hyst]));
 }
 
-static ssize_t set_temp1_crit_hyst(struct device *dev,
-				   struct device_attribute *attr,
-				   const char *buf, size_t count)
+static ssize_t set_temp_hyst(struct device *dev,
+			     struct device_attribute *devattr,
+			     const char *buf, size_t count)
 {
-	struct i2c_client *client = to_i2c_client(dev);
-	struct lm92_data *data = i2c_get_clientdata(client);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct lm92_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
 	long val;
 	int err;
 
@@ -212,9 +207,9 @@ static ssize_t set_temp1_crit_hyst(struct device *dev,
 		return err;
 
 	mutex_lock(&data->update_lock);
-	data->temp1_hyst = TEMP_FROM_REG(data->temp1_crit) - val;
+	data->temp[t_hyst] = TEMP_FROM_REG(data->temp[attr->index]) - val;
 	i2c_smbus_write_word_swapped(client, LM92_REG_TEMP_HYST,
-				     TEMP_TO_REG(data->temp1_hyst));
+				     TEMP_TO_REG(data->temp[t_hyst]));
 	mutex_unlock(&data->update_lock);
 	return count;
 }
@@ -223,7 +218,7 @@ static ssize_t show_alarms(struct device *dev, struct device_attribute *attr,
 			   char *buf)
 {
 	struct lm92_data *data = lm92_update_device(dev);
-	return sprintf(buf, "%d\n", ALARMS_FROM_REG(data->temp1_input));
+	return sprintf(buf, "%d\n", ALARMS_FROM_REG(data->temp[t_input]));
 }
 
 static ssize_t show_alarm(struct device *dev, struct device_attribute *attr,
@@ -231,26 +226,25 @@ static ssize_t show_alarm(struct device *dev, struct device_attribute *attr,
 {
 	int bitnr = to_sensor_dev_attr(attr)->index;
 	struct lm92_data *data = lm92_update_device(dev);
-	return sprintf(buf, "%d\n", (data->temp1_input >> bitnr) & 1);
+	return sprintf(buf, "%d\n", (data->temp[t_input] >> bitnr) & 1);
 }
 
-static DEVICE_ATTR(temp1_input, S_IRUGO, show_temp1_input, NULL);
-static DEVICE_ATTR(temp1_crit, S_IWUSR | S_IRUGO, show_temp1_crit,
-	set_temp1_crit);
-static DEVICE_ATTR(temp1_crit_hyst, S_IWUSR | S_IRUGO, show_temp1_crit_hyst,
-	set_temp1_crit_hyst);
-static DEVICE_ATTR(temp1_min, S_IWUSR | S_IRUGO, show_temp1_min,
-	set_temp1_min);
-static DEVICE_ATTR(temp1_min_hyst, S_IRUGO, show_temp1_min_hyst, NULL);
-static DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, show_temp1_max,
-	set_temp1_max);
-static DEVICE_ATTR(temp1_max_hyst, S_IRUGO, show_temp1_max_hyst, NULL);
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_temp, NULL, t_input);
+static SENSOR_DEVICE_ATTR(temp1_crit, S_IWUSR | S_IRUGO, show_temp, set_temp,
+			  t_crit);
+static SENSOR_DEVICE_ATTR(temp1_crit_hyst, S_IWUSR | S_IRUGO, show_temp_hyst,
+			  set_temp_hyst, t_crit);
+static SENSOR_DEVICE_ATTR(temp1_min, S_IWUSR | S_IRUGO, show_temp, set_temp,
+			  t_min);
+static DEVICE_ATTR(temp1_min_hyst, S_IRUGO, show_temp_min_hyst, NULL);
+static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, show_temp, set_temp,
+			  t_max);
+static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IRUGO, show_temp_hyst, NULL, t_max);
 static DEVICE_ATTR(alarms, S_IRUGO, show_alarms, NULL);
 static SENSOR_DEVICE_ATTR(temp1_crit_alarm, S_IRUGO, show_alarm, NULL, 2);
 static SENSOR_DEVICE_ATTR(temp1_min_alarm, S_IRUGO, show_alarm, NULL, 0);
 static SENSOR_DEVICE_ATTR(temp1_max_alarm, S_IRUGO, show_alarm, NULL, 1);
 
-
 /*
  * Detection and registration
  */
@@ -322,24 +316,21 @@ static int max6635_check(struct i2c_client *client)
 	return 1;
 }
 
-static struct attribute *lm92_attributes[] = {
-	&dev_attr_temp1_input.attr,
-	&dev_attr_temp1_crit.attr,
-	&dev_attr_temp1_crit_hyst.attr,
-	&dev_attr_temp1_min.attr,
+static struct attribute *lm92_attrs[] = {
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp1_min.dev_attr.attr,
 	&dev_attr_temp1_min_hyst.attr,
-	&dev_attr_temp1_max.attr,
-	&dev_attr_temp1_max_hyst.attr,
+	&sensor_dev_attr_temp1_max.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
 	&dev_attr_alarms.attr,
 	&sensor_dev_attr_temp1_crit_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp1_min_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
 	NULL
 };
-
-static const struct attribute_group lm92_group = {
-	.attrs = lm92_attributes,
-};
+ATTRIBUTE_GROUPS(lm92);
 
 /* Return 0 if detection is successful, -ENODEV otherwise */
 static int lm92_detect(struct i2c_client *new_client,
@@ -371,46 +362,24 @@ static int lm92_detect(struct i2c_client *new_client,
 static int lm92_probe(struct i2c_client *new_client,
 		      const struct i2c_device_id *id)
 {
+	struct device *hwmon_dev;
 	struct lm92_data *data;
-	int err;
 
 	data = devm_kzalloc(&new_client->dev, sizeof(struct lm92_data),
 			    GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
-	i2c_set_clientdata(new_client, data);
+	data->client = new_client;
 	mutex_init(&data->update_lock);
 
 	/* Initialize the chipset */
 	lm92_init_client(new_client);
 
-	/* Register sysfs hooks */
-	err = sysfs_create_group(&new_client->dev.kobj, &lm92_group);
-	if (err)
-		return err;
-
-	data->hwmon_dev = hwmon_device_register(&new_client->dev);
-	if (IS_ERR(data->hwmon_dev)) {
-		err = PTR_ERR(data->hwmon_dev);
-		goto exit_remove;
-	}
-
-	return 0;
-
-exit_remove:
-	sysfs_remove_group(&new_client->dev.kobj, &lm92_group);
-	return err;
-}
-
-static int lm92_remove(struct i2c_client *client)
-{
-	struct lm92_data *data = i2c_get_clientdata(client);
-
-	hwmon_device_unregister(data->hwmon_dev);
-	sysfs_remove_group(&client->dev.kobj, &lm92_group);
-
-	return 0;
+	hwmon_dev = devm_hwmon_device_register_with_groups(&new_client->dev,
+							   new_client->name,
+							   data, lm92_groups);
+	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
 
 
@@ -431,7 +400,6 @@ static struct i2c_driver lm92_driver = {
 		.name	= "lm92",
 	},
 	.probe		= lm92_probe,
-	.remove		= lm92_remove,
 	.id_table	= lm92_id,
 	.detect		= lm92_detect,
 	.address_list	= normal_i2c,
diff --git a/drivers/hwmon/lm93.c b/drivers/hwmon/lm93.c
index adf23165a6a..6c2df576f25 100644
--- a/drivers/hwmon/lm93.c
+++ b/drivers/hwmon/lm93.c
@@ -2747,10 +2747,8 @@ static int lm93_probe(struct i2c_client *client,
 	}
 
 	data = devm_kzalloc(&client->dev, sizeof(struct lm93_data), GFP_KERNEL);
-	if (!data) {
-		dev_dbg(&client->dev, "out of memory!\n");
+	if (!data)
 		return -ENOMEM;
-	}
 	i2c_set_clientdata(client, data);
 
 	/* housekeeping */
diff --git a/drivers/hwmon/ltc2945.c b/drivers/hwmon/ltc2945.c
index c9cddf5f056..3701b329b6a 100644
--- a/drivers/hwmon/ltc2945.c
+++ b/drivers/hwmon/ltc2945.c
@@ -1,4 +1,4 @@
-    /*
+/*
  * Driver for Linear Technology LTC2945 I2C Power Monitor
  *
  * Copyright (c) 2014 Guenter Roeck
diff --git a/drivers/hwmon/max1111.c b/drivers/hwmon/max1111.c
index eda077de8a9..f67d71ee838 100644
--- a/drivers/hwmon/max1111.c
+++ b/drivers/hwmon/max1111.c
@@ -192,10 +192,8 @@ static int max1111_probe(struct spi_device *spi)
 		return err;
 
 	data = devm_kzalloc(&spi->dev, sizeof(struct max1111_data), GFP_KERNEL);
-	if (data == NULL) {
-		dev_err(&spi->dev, "failed to allocate memory\n");
+	if (data == NULL)
 		return -ENOMEM;
-	}
 
 	switch (chip) {
 	case max1110:
diff --git a/drivers/hwmon/max1619.c b/drivers/hwmon/max1619.c
index 4c23afe113e..eda9cf59968 100644
--- a/drivers/hwmon/max1619.c
+++ b/drivers/hwmon/max1619.c
@@ -19,13 +19,8 @@
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
-
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/slab.h>
@@ -76,38 +71,14 @@ static int temp_to_reg(int val)
 	return (val < 0 ? val+0x100*1000 : val) / 1000;
 }
 
-/*
- * Functions declaration
- */
-
-static int max1619_probe(struct i2c_client *client,
-			 const struct i2c_device_id *id);
-static int max1619_detect(struct i2c_client *client,
-			  struct i2c_board_info *info);
-static void max1619_init_client(struct i2c_client *client);
-static int max1619_remove(struct i2c_client *client);
-static struct max1619_data *max1619_update_device(struct device *dev);
-
-/*
- * Driver data (common to all clients)
- */
-
-static const struct i2c_device_id max1619_id[] = {
-	{ "max1619", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, max1619_id);
-
-static struct i2c_driver max1619_driver = {
-	.class		= I2C_CLASS_HWMON,
-	.driver = {
-		.name	= "max1619",
-	},
-	.probe		= max1619_probe,
-	.remove		= max1619_remove,
-	.id_table	= max1619_id,
-	.detect		= max1619_detect,
-	.address_list	= normal_i2c,
+enum temp_index {
+	t_input1 = 0,
+	t_input2,
+	t_low2,
+	t_high2,
+	t_crit2,
+	t_hyst2,
+	t_num_regs
 };
 
 /*
@@ -115,60 +86,92 @@ static struct i2c_driver max1619_driver = {
  */
 
 struct max1619_data {
-	struct device *hwmon_dev;
+	struct i2c_client *client;
 	struct mutex update_lock;
 	char valid; /* zero until following fields are valid */
 	unsigned long last_updated; /* in jiffies */
 
 	/* registers values */
-	u8 temp_input1; /* local */
-	u8 temp_input2, temp_low2, temp_high2; /* remote */
-	u8 temp_crit2;
-	u8 temp_hyst2;
+	u8 temp[t_num_regs];	/* index with enum temp_index */
 	u8 alarms;
 };
 
+static const u8 regs_read[t_num_regs] = {
+	[t_input1] = MAX1619_REG_R_LOCAL_TEMP,
+	[t_input2] = MAX1619_REG_R_REMOTE_TEMP,
+	[t_low2] = MAX1619_REG_R_REMOTE_LOW,
+	[t_high2] = MAX1619_REG_R_REMOTE_HIGH,
+	[t_crit2] = MAX1619_REG_R_REMOTE_CRIT,
+	[t_hyst2] = MAX1619_REG_R_TCRIT_HYST,
+};
+
+static const u8 regs_write[t_num_regs] = {
+	[t_low2] = MAX1619_REG_W_REMOTE_LOW,
+	[t_high2] = MAX1619_REG_W_REMOTE_HIGH,
+	[t_crit2] = MAX1619_REG_W_REMOTE_CRIT,
+	[t_hyst2] = MAX1619_REG_W_TCRIT_HYST,
+};
+
+static struct max1619_data *max1619_update_device(struct device *dev)
+{
+	struct max1619_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	int config, i;
+
+	mutex_lock(&data->update_lock);
+
+	if (time_after(jiffies, data->last_updated + HZ * 2) || !data->valid) {
+		dev_dbg(&client->dev, "Updating max1619 data.\n");
+		for (i = 0; i < t_num_regs; i++)
+			data->temp[i] = i2c_smbus_read_byte_data(client,
+					regs_read[i]);
+		data->alarms = i2c_smbus_read_byte_data(client,
+					MAX1619_REG_R_STATUS);
+		/* If OVERT polarity is low, reverse alarm bit */
+		config = i2c_smbus_read_byte_data(client, MAX1619_REG_R_CONFIG);
+		if (!(config & 0x20))
+			data->alarms ^= 0x02;
+
+		data->last_updated = jiffies;
+		data->valid = 1;
+	}
+
+	mutex_unlock(&data->update_lock);
+
+	return data;
+}
+
 /*
  * Sysfs stuff
  */
 
-#define show_temp(value) \
-static ssize_t show_##value(struct device *dev, struct device_attribute *attr, \
-			    char *buf) \
-{ \
-	struct max1619_data *data = max1619_update_device(dev); \
-	return sprintf(buf, "%d\n", temp_from_reg(data->value)); \
-}
-show_temp(temp_input1);
-show_temp(temp_input2);
-show_temp(temp_low2);
-show_temp(temp_high2);
-show_temp(temp_crit2);
-show_temp(temp_hyst2);
-
-#define set_temp2(value, reg) \
-static ssize_t set_##value(struct device *dev, struct device_attribute *attr, \
-			   const char *buf, \
-	size_t count) \
-{ \
-	struct i2c_client *client = to_i2c_client(dev); \
-	struct max1619_data *data = i2c_get_clientdata(client); \
-	long val; \
-	int err = kstrtol(buf, 10, &val); \
-	if (err) \
-		return err; \
-\
-	mutex_lock(&data->update_lock); \
-	data->value = temp_to_reg(val); \
-	i2c_smbus_write_byte_data(client, reg, data->value); \
-	mutex_unlock(&data->update_lock); \
-	return count; \
+static ssize_t show_temp(struct device *dev, struct device_attribute *devattr,
+			 char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct max1619_data *data = max1619_update_device(dev);
+
+	return sprintf(buf, "%d\n", temp_from_reg(data->temp[attr->index]));
 }
 
-set_temp2(temp_low2, MAX1619_REG_W_REMOTE_LOW);
-set_temp2(temp_high2, MAX1619_REG_W_REMOTE_HIGH);
-set_temp2(temp_crit2, MAX1619_REG_W_REMOTE_CRIT);
-set_temp2(temp_hyst2, MAX1619_REG_W_TCRIT_HYST);
+static ssize_t set_temp(struct device *dev, struct device_attribute *devattr,
+			   const char *buf, size_t count)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct max1619_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	long val;
+	int err = kstrtol(buf, 10, &val);
+	if (err)
+		return err;
+
+	mutex_lock(&data->update_lock);
+	data->temp[attr->index] = temp_to_reg(val);
+	i2c_smbus_write_byte_data(client, regs_write[attr->index],
+				  data->temp[attr->index]);
+	mutex_unlock(&data->update_lock);
+	return count;
+}
 
 static ssize_t show_alarms(struct device *dev, struct device_attribute *attr,
 			   char *buf)
@@ -185,29 +188,30 @@ static ssize_t show_alarm(struct device *dev, struct device_attribute *attr,
 	return sprintf(buf, "%d\n", (data->alarms >> bitnr) & 1);
 }
 
-static DEVICE_ATTR(temp1_input, S_IRUGO, show_temp_input1, NULL);
-static DEVICE_ATTR(temp2_input, S_IRUGO, show_temp_input2, NULL);
-static DEVICE_ATTR(temp2_min, S_IWUSR | S_IRUGO, show_temp_low2,
-	set_temp_low2);
-static DEVICE_ATTR(temp2_max, S_IWUSR | S_IRUGO, show_temp_high2,
-	set_temp_high2);
-static DEVICE_ATTR(temp2_crit, S_IWUSR | S_IRUGO, show_temp_crit2,
-	set_temp_crit2);
-static DEVICE_ATTR(temp2_crit_hyst, S_IWUSR | S_IRUGO, show_temp_hyst2,
-	set_temp_hyst2);
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_temp, NULL, t_input1);
+static SENSOR_DEVICE_ATTR(temp2_input, S_IRUGO, show_temp, NULL, t_input2);
+static SENSOR_DEVICE_ATTR(temp2_min, S_IWUSR | S_IRUGO, show_temp, set_temp,
+			  t_low2);
+static SENSOR_DEVICE_ATTR(temp2_max, S_IWUSR | S_IRUGO, show_temp, set_temp,
+			  t_high2);
+static SENSOR_DEVICE_ATTR(temp2_crit, S_IWUSR | S_IRUGO, show_temp, set_temp,
+			  t_crit2);
+static SENSOR_DEVICE_ATTR(temp2_crit_hyst, S_IWUSR | S_IRUGO, show_temp,
+			  set_temp, t_hyst2);
+
 static DEVICE_ATTR(alarms, S_IRUGO, show_alarms, NULL);
 static SENSOR_DEVICE_ATTR(temp2_crit_alarm, S_IRUGO, show_alarm, NULL, 1);
 static SENSOR_DEVICE_ATTR(temp2_fault, S_IRUGO, show_alarm, NULL, 2);
 static SENSOR_DEVICE_ATTR(temp2_min_alarm, S_IRUGO, show_alarm, NULL, 3);
 static SENSOR_DEVICE_ATTR(temp2_max_alarm, S_IRUGO, show_alarm, NULL, 4);
 
-static struct attribute *max1619_attributes[] = {
-	&dev_attr_temp1_input.attr,
-	&dev_attr_temp2_input.attr,
-	&dev_attr_temp2_min.attr,
-	&dev_attr_temp2_max.attr,
-	&dev_attr_temp2_crit.attr,
-	&dev_attr_temp2_crit_hyst.attr,
+static struct attribute *max1619_attrs[] = {
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_temp2_input.dev_attr.attr,
+	&sensor_dev_attr_temp2_min.dev_attr.attr,
+	&sensor_dev_attr_temp2_max.dev_attr.attr,
+	&sensor_dev_attr_temp2_crit.dev_attr.attr,
+	&sensor_dev_attr_temp2_crit_hyst.dev_attr.attr,
 
 	&dev_attr_alarms.attr,
 	&sensor_dev_attr_temp2_crit_alarm.dev_attr.attr,
@@ -216,14 +220,7 @@ static struct attribute *max1619_attributes[] = {
 	&sensor_dev_attr_temp2_max_alarm.dev_attr.attr,
 	NULL
 };
-
-static const struct attribute_group max1619_group = {
-	.attrs = max1619_attributes,
-};
-
-/*
- * Real code
- */
+ATTRIBUTE_GROUPS(max1619);
 
 /* Return 0 if detection is successful, -ENODEV otherwise */
 static int max1619_detect(struct i2c_client *client,
@@ -261,41 +258,6 @@ static int max1619_detect(struct i2c_client *client,
 	return 0;
 }
 
-static int max1619_probe(struct i2c_client *new_client,
-			 const struct i2c_device_id *id)
-{
-	struct max1619_data *data;
-	int err;
-
-	data = devm_kzalloc(&new_client->dev, sizeof(struct max1619_data),
-			    GFP_KERNEL);
-	if (!data)
-		return -ENOMEM;
-
-	i2c_set_clientdata(new_client, data);
-	mutex_init(&data->update_lock);
-
-	/* Initialize the MAX1619 chip */
-	max1619_init_client(new_client);
-
-	/* Register sysfs hooks */
-	err = sysfs_create_group(&new_client->dev.kobj, &max1619_group);
-	if (err)
-		return err;
-
-	data->hwmon_dev = hwmon_device_register(&new_client->dev);
-	if (IS_ERR(data->hwmon_dev)) {
-		err = PTR_ERR(data->hwmon_dev);
-		goto exit_remove_files;
-	}
-
-	return 0;
-
-exit_remove_files:
-	sysfs_remove_group(&new_client->dev.kobj, &max1619_group);
-	return err;
-}
-
 static void max1619_init_client(struct i2c_client *client)
 {
 	u8 config;
@@ -311,48 +273,46 @@ static void max1619_init_client(struct i2c_client *client)
 					  config & 0xBF); /* run */
 }
 
-static int max1619_remove(struct i2c_client *client)
+static int max1619_probe(struct i2c_client *new_client,
+			 const struct i2c_device_id *id)
 {
-	struct max1619_data *data = i2c_get_clientdata(client);
-
-	hwmon_device_unregister(data->hwmon_dev);
-	sysfs_remove_group(&client->dev.kobj, &max1619_group);
-
-	return 0;
-}
+	struct max1619_data *data;
+	struct device *hwmon_dev;
 
-static struct max1619_data *max1619_update_device(struct device *dev)
-{
-	struct i2c_client *client = to_i2c_client(dev);
-	struct max1619_data *data = i2c_get_clientdata(client);
+	data = devm_kzalloc(&new_client->dev, sizeof(struct max1619_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
-	mutex_lock(&data->update_lock);
+	data->client = new_client;
+	mutex_init(&data->update_lock);
 
-	if (time_after(jiffies, data->last_updated + HZ * 2) || !data->valid) {
-		dev_dbg(&client->dev, "Updating max1619 data.\n");
-		data->temp_input1 = i2c_smbus_read_byte_data(client,
-					MAX1619_REG_R_LOCAL_TEMP);
-		data->temp_input2 = i2c_smbus_read_byte_data(client,
-					MAX1619_REG_R_REMOTE_TEMP);
-		data->temp_high2 = i2c_smbus_read_byte_data(client,
-					MAX1619_REG_R_REMOTE_HIGH);
-		data->temp_low2 = i2c_smbus_read_byte_data(client,
-					MAX1619_REG_R_REMOTE_LOW);
-		data->temp_crit2 = i2c_smbus_read_byte_data(client,
-					MAX1619_REG_R_REMOTE_CRIT);
-		data->temp_hyst2 = i2c_smbus_read_byte_data(client,
-					MAX1619_REG_R_TCRIT_HYST);
-		data->alarms = i2c_smbus_read_byte_data(client,
-					MAX1619_REG_R_STATUS);
+	/* Initialize the MAX1619 chip */
+	max1619_init_client(new_client);
 
-		data->last_updated = jiffies;
-		data->valid = 1;
-	}
+	hwmon_dev = devm_hwmon_device_register_with_groups(&new_client->dev,
+							   new_client->name,
+							   data,
+							   max1619_groups);
+	return PTR_ERR_OR_ZERO(hwmon_dev);
+}
 
-	mutex_unlock(&data->update_lock);
+static const struct i2c_device_id max1619_id[] = {
+	{ "max1619", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, max1619_id);
 
-	return data;
-}
+static struct i2c_driver max1619_driver = {
+	.class		= I2C_CLASS_HWMON,
+	.driver = {
+		.name	= "max1619",
+	},
+	.probe		= max1619_probe,
+	.id_table	= max1619_id,
+	.detect		= max1619_detect,
+	.address_list	= normal_i2c,
+};
 
 module_i2c_driver(max1619_driver);
 
diff --git a/drivers/hwmon/max197.c b/drivers/hwmon/max197.c
index 96dccaf919d..82128ad79a9 100644
--- a/drivers/hwmon/max197.c
+++ b/drivers/hwmon/max197.c
@@ -275,10 +275,8 @@ static int max197_probe(struct platform_device *pdev)
 	}
 
 	data = devm_kzalloc(&pdev->dev, sizeof(struct max197_data), GFP_KERNEL);
-	if (!data) {
-		dev_err(&pdev->dev, "devm_kzalloc failed\n");
+	if (!data)
 		return -ENOMEM;
-	}
 
 	data->pdata = pdata;
 	mutex_init(&data->lock);
diff --git a/drivers/hwmon/nct6683.c b/drivers/hwmon/nct6683.c
new file mode 100644
index 00000000000..7710f4694ba
--- /dev/null
+++ b/drivers/hwmon/nct6683.c
@@ -0,0 +1,1457 @@
+/*
+ * nct6683 - Driver for the hardware monitoring functionality of
+ *	     Nuvoton NCT6683D eSIO
+ *
+ * Copyright (C) 2013  Guenter Roeck <linux@roeck-us.net>
+ *
+ * Derived from nct6775 driver
+ * Copyright (C) 2012, 2013  Guenter Roeck <linux@roeck-us.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.
+ *
+ * Supports the following chips:
+ *
+ * Chip        #vin    #fan    #pwm    #temp  chip ID
+ * nct6683d     21(1)   16      8       32(1) 0xc730
+ *
+ * Notes:
+ *	(1) Total number of vin and temp inputs is 32.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/acpi.h>
+#include <linux/dmi.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/jiffies.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+enum kinds { nct6683 };
+
+static bool force;
+module_param(force, bool, 0);
+MODULE_PARM_DESC(force, "Set to one to enable detection on non-Intel boards");
+
+static const char * const nct6683_device_names[] = {
+	"nct6683",
+};
+
+static const char * const nct6683_chip_names[] = {
+	"NCT6683D",
+};
+
+#define DRVNAME "nct6683"
+
+/*
+ * Super-I/O constants and functions
+ */
+
+#define NCT6683_LD_ACPI		0x0a
+#define NCT6683_LD_HWM		0x0b
+#define NCT6683_LD_VID		0x0d
+
+#define SIO_REG_LDSEL		0x07	/* Logical device select */
+#define SIO_REG_DEVID		0x20	/* Device ID (2 bytes) */
+#define SIO_REG_ENABLE		0x30	/* Logical device enable */
+#define SIO_REG_ADDR		0x60	/* Logical device address (2 bytes) */
+
+#define SIO_NCT6681_ID		0xb270	/* for later */
+#define SIO_NCT6683_ID		0xc730
+#define SIO_ID_MASK		0xFFF0
+
+static inline void
+superio_outb(int ioreg, int reg, int val)
+{
+	outb(reg, ioreg);
+	outb(val, ioreg + 1);
+}
+
+static inline int
+superio_inb(int ioreg, int reg)
+{
+	outb(reg, ioreg);
+	return inb(ioreg + 1);
+}
+
+static inline void
+superio_select(int ioreg, int ld)
+{
+	outb(SIO_REG_LDSEL, ioreg);
+	outb(ld, ioreg + 1);
+}
+
+static inline int
+superio_enter(int ioreg)
+{
+	/*
+	 * Try to reserve <ioreg> and <ioreg + 1> for exclusive access.
+	 */
+	if (!request_muxed_region(ioreg, 2, DRVNAME))
+		return -EBUSY;
+
+	outb(0x87, ioreg);
+	outb(0x87, ioreg);
+
+	return 0;
+}
+
+static inline void
+superio_exit(int ioreg)
+{
+	outb(0xaa, ioreg);
+	outb(0x02, ioreg);
+	outb(0x02, ioreg + 1);
+	release_region(ioreg, 2);
+}
+
+/*
+ * ISA constants
+ */
+
+#define IOREGION_ALIGNMENT	(~7)
+#define IOREGION_OFFSET		4	/* Use EC port 1 */
+#define IOREGION_LENGTH		4
+
+#define EC_PAGE_REG		0
+#define EC_INDEX_REG		1
+#define EC_DATA_REG		2
+#define EC_EVENT_REG		3
+
+/* Common and NCT6683 specific data */
+
+#define NCT6683_NUM_REG_MON		32
+#define NCT6683_NUM_REG_FAN		16
+#define NCT6683_NUM_REG_PWM		8
+
+#define NCT6683_REG_MON(x)		(0x100 + (x) * 2)
+#define NCT6683_REG_FAN_RPM(x)		(0x140 + (x) * 2)
+#define NCT6683_REG_PWM(x)		(0x160 + (x))
+
+#define NCT6683_REG_MON_STS(x)		(0x174 + (x))
+#define NCT6683_REG_IDLE(x)		(0x178 + (x))
+
+#define NCT6683_REG_FAN_STS(x)		(0x17c + (x))
+#define NCT6683_REG_FAN_ERRSTS		0x17e
+#define NCT6683_REG_FAN_INITSTS		0x17f
+
+#define NCT6683_HWM_CFG			0x180
+
+#define NCT6683_REG_MON_CFG(x)		(0x1a0 + (x))
+#define NCT6683_REG_FANIN_CFG(x)	(0x1c0 + (x))
+#define NCT6683_REG_FANOUT_CFG(x)	(0x1d0 + (x))
+
+#define NCT6683_REG_INTEL_TEMP_MAX(x)	(0x901 + (x) * 16)
+#define NCT6683_REG_INTEL_TEMP_CRIT(x)	(0x90d + (x) * 16)
+
+#define NCT6683_REG_TEMP_HYST(x)	(0x330 + (x))		/* 8 bit */
+#define NCT6683_REG_TEMP_MAX(x)		(0x350 + (x))		/* 8 bit */
+#define NCT6683_REG_MON_HIGH(x)		(0x370 + (x) * 2)	/* 8 bit */
+#define NCT6683_REG_MON_LOW(x)		(0x371 + (x) * 2)	/* 8 bit */
+
+#define NCT6683_REG_FAN_MIN(x)		(0x3b8 + (x) * 2)	/* 16 bit */
+
+#define NCT6683_REG_CUSTOMER_ID		0x602
+#define NCT6683_CUSTOMER_ID_INTEL	0x805
+
+#define NCT6683_REG_BUILD_YEAR		0x604
+#define NCT6683_REG_BUILD_MONTH		0x605
+#define NCT6683_REG_BUILD_DAY		0x606
+#define NCT6683_REG_SERIAL		0x607
+#define NCT6683_REG_VERSION_HI		0x608
+#define NCT6683_REG_VERSION_LO		0x609
+
+#define NCT6683_REG_CR_CASEOPEN		0xe8
+#define NCT6683_CR_CASEOPEN_MASK	(1 << 7)
+
+#define NCT6683_REG_CR_BEEP		0xe0
+#define NCT6683_CR_BEEP_MASK		(1 << 6)
+
+static const char *const nct6683_mon_label[] = {
+	NULL,	/* disabled */
+	"Local",
+	"Diode 0 (curr)",
+	"Diode 1 (curr)",
+	"Diode 2 (curr)",
+	"Diode 0 (volt)",
+	"Diode 1 (volt)",
+	"Diode 2 (volt)",
+	"Thermistor 14",
+	"Thermistor 15",
+	"Thermistor 16",
+	"Thermistor 0",
+	"Thermistor 1",
+	"Thermistor 2",
+	"Thermistor 3",
+	"Thermistor 4",
+	"Thermistor 5",		/* 0x10 */
+	"Thermistor 6",
+	"Thermistor 7",
+	"Thermistor 8",
+	"Thermistor 9",
+	"Thermistor 10",
+	"Thermistor 11",
+	"Thermistor 12",
+	"Thermistor 13",
+	NULL, NULL, NULL, NULL, NULL, NULL, NULL,
+	"PECI 0.0",		/* 0x20 */
+	"PECI 1.0",
+	"PECI 2.0",
+	"PECI 3.0",
+	"PECI 0.1",
+	"PECI 1.1",
+	"PECI 2.1",
+	"PECI 3.1",
+	"PECI DIMM 0",
+	"PECI DIMM 1",
+	"PECI DIMM 2",
+	"PECI DIMM 3",
+	NULL, NULL, NULL, NULL,
+	"PCH CPU",		/* 0x30 */
+	"PCH CHIP",
+	"PCH CHIP CPU MAX",
+	"PCH MCH",
+	"PCH DIMM 0",
+	"PCH DIMM 1",
+	"PCH DIMM 2",
+	"PCH DIMM 3",
+	"SMBus 0",
+	"SMBus 1",
+	"SMBus 2",
+	"SMBus 3",
+	"SMBus 4",
+	"SMBus 5",
+	"DIMM 0",
+	"DIMM 1",
+	"DIMM 2",		/* 0x40 */
+	"DIMM 3",
+	"AMD TSI Addr 90h",
+	"AMD TSI Addr 92h",
+	"AMD TSI Addr 94h",
+	"AMD TSI Addr 96h",
+	"AMD TSI Addr 98h",
+	"AMD TSI Addr 9ah",
+	"AMD TSI Addr 9ch",
+	"AMD TSI Addr 9dh",
+	NULL, NULL, NULL, NULL, NULL, NULL,
+	"Virtual 0",		/* 0x50 */
+	"Virtual 1",
+	"Virtual 2",
+	"Virtual 3",
+	"Virtual 4",
+	"Virtual 5",
+	"Virtual 6",
+	"Virtual 7",
+	NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL,
+	"VCC",			/* 0x60 voltage sensors */
+	"VSB",
+	"AVSB",
+	"VTT",
+	"VBAT",
+	"VREF",
+	"VIN0",
+	"VIN1",
+	"VIN2",
+	"VIN3",
+	"VIN4",
+	"VIN5",
+	"VIN6",
+	"VIN7",
+	"VIN8",
+	"VIN9",
+	"VIN10",
+	"VIN11",
+	"VIN12",
+	"VIN13",
+	"VIN14",
+	"VIN15",
+	"VIN16",
+};
+
+#define NUM_MON_LABELS		ARRAY_SIZE(nct6683_mon_label)
+#define MON_VOLTAGE_START	0x60
+
+/* ------------------------------------------------------- */
+
+struct nct6683_data {
+	int addr;		/* IO base of EC space */
+	int sioreg;		/* SIO register */
+	enum kinds kind;
+	u16 customer_id;
+
+	struct device *hwmon_dev;
+	const struct attribute_group *groups[6];
+
+	int temp_num;			/* number of temperature attributes */
+	u8 temp_index[NCT6683_NUM_REG_MON];
+	u8 temp_src[NCT6683_NUM_REG_MON];
+
+	u8 in_num;			/* number of voltage attributes */
+	u8 in_index[NCT6683_NUM_REG_MON];
+	u8 in_src[NCT6683_NUM_REG_MON];
+
+	struct mutex update_lock;	/* used to protect sensor updates */
+	bool valid;			/* true if following fields are valid */
+	unsigned long last_updated;	/* In jiffies */
+
+	/* Voltage attribute values */
+	u8 in[3][NCT6683_NUM_REG_MON];	/* [0]=in, [1]=in_max, [2]=in_min */
+
+	/* Temperature attribute values */
+	s16 temp_in[NCT6683_NUM_REG_MON];
+	s8 temp[4][NCT6683_NUM_REG_MON];/* [0]=min, [1]=max, [2]=hyst,
+					 * [3]=crit
+					 */
+
+	/* Fan attribute values */
+	unsigned int rpm[NCT6683_NUM_REG_FAN];
+	u16 fan_min[NCT6683_NUM_REG_FAN];
+	u8 fanin_cfg[NCT6683_NUM_REG_FAN];
+	u8 fanout_cfg[NCT6683_NUM_REG_FAN];
+	u16 have_fan;			/* some fan inputs can be disabled */
+
+	u8 have_pwm;
+	u8 pwm[NCT6683_NUM_REG_PWM];
+
+#ifdef CONFIG_PM
+	/* Remember extra register values over suspend/resume */
+	u8 hwm_cfg;
+#endif
+};
+
+struct nct6683_sio_data {
+	int sioreg;
+	enum kinds kind;
+};
+
+struct sensor_device_template {
+	struct device_attribute dev_attr;
+	union {
+		struct {
+			u8 nr;
+			u8 index;
+		} s;
+		int index;
+	} u;
+	bool s2;	/* true if both index and nr are used */
+};
+
+struct sensor_device_attr_u {
+	union {
+		struct sensor_device_attribute a1;
+		struct sensor_device_attribute_2 a2;
+	} u;
+	char name[32];
+};
+
+#define __TEMPLATE_ATTR(_template, _mode, _show, _store) {	\
+	.attr = {.name = _template, .mode = _mode },		\
+	.show	= _show,					\
+	.store	= _store,					\
+}
+
+#define SENSOR_DEVICE_TEMPLATE(_template, _mode, _show, _store, _index)	\
+	{ .dev_attr = __TEMPLATE_ATTR(_template, _mode, _show, _store),	\
+	  .u.index = _index,						\
+	  .s2 = false }
+
+#define SENSOR_DEVICE_TEMPLATE_2(_template, _mode, _show, _store,	\
+				 _nr, _index)				\
+	{ .dev_attr = __TEMPLATE_ATTR(_template, _mode, _show, _store),	\
+	  .u.s.index = _index,						\
+	  .u.s.nr = _nr,						\
+	  .s2 = true }
+
+#define SENSOR_TEMPLATE(_name, _template, _mode, _show, _store, _index)	\
+static struct sensor_device_template sensor_dev_template_##_name	\
+	= SENSOR_DEVICE_TEMPLATE(_template, _mode, _show, _store,	\
+				 _index)
+
+#define SENSOR_TEMPLATE_2(_name, _template, _mode, _show, _store,	\
+			  _nr, _index)					\
+static struct sensor_device_template sensor_dev_template_##_name	\
+	= SENSOR_DEVICE_TEMPLATE_2(_template, _mode, _show, _store,	\
+				 _nr, _index)
+
+struct sensor_template_group {
+	struct sensor_device_template **templates;
+	umode_t (*is_visible)(struct kobject *, struct attribute *, int);
+	int base;
+};
+
+static struct attribute_group *
+nct6683_create_attr_group(struct device *dev, struct sensor_template_group *tg,
+			  int repeat)
+{
+	struct sensor_device_attribute_2 *a2;
+	struct sensor_device_attribute *a;
+	struct sensor_device_template **t;
+	struct sensor_device_attr_u *su;
+	struct attribute_group *group;
+	struct attribute **attrs;
+	int i, j, count;
+
+	if (repeat <= 0)
+		return ERR_PTR(-EINVAL);
+
+	t = tg->templates;
+	for (count = 0; *t; t++, count++)
+		;
+
+	if (count == 0)
+		return ERR_PTR(-EINVAL);
+
+	group = devm_kzalloc(dev, sizeof(*group), GFP_KERNEL);
+	if (group == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	attrs = devm_kzalloc(dev, sizeof(*attrs) * (repeat * count + 1),
+			     GFP_KERNEL);
+	if (attrs == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	su = devm_kzalloc(dev, sizeof(*su) * repeat * count,
+			  GFP_KERNEL);
+	if (su == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	group->attrs = attrs;
+	group->is_visible = tg->is_visible;
+
+	for (i = 0; i < repeat; i++) {
+		t = tg->templates;
+		for (j = 0; *t != NULL; j++) {
+			snprintf(su->name, sizeof(su->name),
+				 (*t)->dev_attr.attr.name, tg->base + i);
+			if ((*t)->s2) {
+				a2 = &su->u.a2;
+				a2->dev_attr.attr.name = su->name;
+				a2->nr = (*t)->u.s.nr + i;
+				a2->index = (*t)->u.s.index;
+				a2->dev_attr.attr.mode =
+				  (*t)->dev_attr.attr.mode;
+				a2->dev_attr.show = (*t)->dev_attr.show;
+				a2->dev_attr.store = (*t)->dev_attr.store;
+				*attrs = &a2->dev_attr.attr;
+			} else {
+				a = &su->u.a1;
+				a->dev_attr.attr.name = su->name;
+				a->index = (*t)->u.index + i;
+				a->dev_attr.attr.mode =
+				  (*t)->dev_attr.attr.mode;
+				a->dev_attr.show = (*t)->dev_attr.show;
+				a->dev_attr.store = (*t)->dev_attr.store;
+				*attrs = &a->dev_attr.attr;
+			}
+			attrs++;
+			su++;
+			t++;
+		}
+	}
+
+	return group;
+}
+
+/* LSB is 16 mV, except for the following sources, where it is 32 mV */
+#define MON_SRC_VCC	0x60
+#define MON_SRC_VSB	0x61
+#define MON_SRC_AVSB	0x62
+#define MON_SRC_VBAT	0x64
+
+static inline long in_from_reg(u16 reg, u8 src)
+{
+	int scale = 16;
+
+	if (src == MON_SRC_VCC || src == MON_SRC_VSB || src == MON_SRC_AVSB ||
+	    src == MON_SRC_VBAT)
+		scale <<= 1;
+	return reg * scale;
+}
+
+static inline u16 in_to_reg(u32 val, u8 src)
+{
+	int scale = 16;
+
+	if (src == MON_SRC_VCC || src == MON_SRC_VSB || src == MON_SRC_AVSB ||
+	    src == MON_SRC_VBAT)
+		scale <<= 1;
+
+	return clamp_val(DIV_ROUND_CLOSEST(val, scale), 0, 127);
+}
+
+static u16 nct6683_read(struct nct6683_data *data, u16 reg)
+{
+	int res;
+
+	outb_p(0xff, data->addr + EC_PAGE_REG);		/* unlock */
+	outb_p(reg >> 8, data->addr + EC_PAGE_REG);
+	outb_p(reg & 0xff, data->addr + EC_INDEX_REG);
+	res = inb_p(data->addr + EC_DATA_REG);
+	return res;
+}
+
+static u16 nct6683_read16(struct nct6683_data *data, u16 reg)
+{
+	return (nct6683_read(data, reg) << 8) | nct6683_read(data, reg + 1);
+}
+
+static void nct6683_write(struct nct6683_data *data, u16 reg, u16 value)
+{
+	outb_p(0xff, data->addr + EC_PAGE_REG);		/* unlock */
+	outb_p(reg >> 8, data->addr + EC_PAGE_REG);
+	outb_p(reg & 0xff, data->addr + EC_INDEX_REG);
+	outb_p(value & 0xff, data->addr + EC_DATA_REG);
+}
+
+static int get_in_reg(struct nct6683_data *data, int nr, int index)
+{
+	int ch = data->in_index[index];
+	int reg = -EINVAL;
+
+	switch (nr) {
+	case 0:
+		reg = NCT6683_REG_MON(ch);
+		break;
+	case 1:
+		if (data->customer_id != NCT6683_CUSTOMER_ID_INTEL)
+			reg = NCT6683_REG_MON_LOW(ch);
+		break;
+	case 2:
+		if (data->customer_id != NCT6683_CUSTOMER_ID_INTEL)
+			reg = NCT6683_REG_MON_HIGH(ch);
+		break;
+	default:
+		break;
+	}
+	return reg;
+}
+
+static int get_temp_reg(struct nct6683_data *data, int nr, int index)
+{
+	int ch = data->temp_index[index];
+	int reg = -EINVAL;
+
+	switch (data->customer_id) {
+	case NCT6683_CUSTOMER_ID_INTEL:
+		switch (nr) {
+		default:
+		case 1:	/* max */
+			reg = NCT6683_REG_INTEL_TEMP_MAX(ch);
+			break;
+		case 3:	/* crit */
+			reg = NCT6683_REG_INTEL_TEMP_CRIT(ch);
+			break;
+		}
+		break;
+	default:
+		switch (nr) {
+		default:
+		case 0:	/* min */
+			reg = NCT6683_REG_MON_LOW(ch);
+			break;
+		case 1:	/* max */
+			reg = NCT6683_REG_TEMP_MAX(ch);
+			break;
+		case 2:	/* hyst */
+			reg = NCT6683_REG_TEMP_HYST(ch);
+			break;
+		case 3:	/* crit */
+			reg = NCT6683_REG_MON_HIGH(ch);
+			break;
+		}
+		break;
+	}
+	return reg;
+}
+
+static void nct6683_update_pwm(struct device *dev)
+{
+	struct nct6683_data *data = dev_get_drvdata(dev);
+	int i;
+
+	for (i = 0; i < NCT6683_NUM_REG_PWM; i++) {
+		if (!(data->have_pwm & (1 << i)))
+			continue;
+		data->pwm[i] = nct6683_read(data, NCT6683_REG_PWM(i));
+	}
+}
+
+static struct nct6683_data *nct6683_update_device(struct device *dev)
+{
+	struct nct6683_data *data = dev_get_drvdata(dev);
+	int i, j;
+
+	mutex_lock(&data->update_lock);
+
+	if (time_after(jiffies, data->last_updated + HZ) || !data->valid) {
+		/* Measured voltages and limits */
+		for (i = 0; i < data->in_num; i++) {
+			for (j = 0; j < 3; j++) {
+				int reg = get_in_reg(data, j, i);
+
+				if (reg >= 0)
+					data->in[j][i] =
+						nct6683_read(data, reg);
+			}
+		}
+
+		/* Measured temperatures and limits */
+		for (i = 0; i < data->temp_num; i++) {
+			u8 ch = data->temp_index[i];
+
+			data->temp_in[i] = nct6683_read16(data,
+							  NCT6683_REG_MON(ch));
+			for (j = 0; j < 4; j++) {
+				int reg = get_temp_reg(data, j, i);
+
+				if (reg >= 0)
+					data->temp[j][i] =
+						nct6683_read(data, reg);
+			}
+		}
+
+		/* Measured fan speeds and limits */
+		for (i = 0; i < ARRAY_SIZE(data->rpm); i++) {
+			if (!(data->have_fan & (1 << i)))
+				continue;
+
+			data->rpm[i] = nct6683_read16(data,
+						NCT6683_REG_FAN_RPM(i));
+			data->fan_min[i] = nct6683_read16(data,
+						NCT6683_REG_FAN_MIN(i));
+		}
+
+		nct6683_update_pwm(dev);
+
+		data->last_updated = jiffies;
+		data->valid = true;
+	}
+
+	mutex_unlock(&data->update_lock);
+	return data;
+}
+
+/*
+ * Sysfs callback functions
+ */
+static ssize_t
+show_in_label(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	struct nct6683_data *data = nct6683_update_device(dev);
+	int nr = sattr->index;
+
+	return sprintf(buf, "%s\n", nct6683_mon_label[data->in_src[nr]]);
+}
+
+static ssize_t
+show_in_reg(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	struct nct6683_data *data = nct6683_update_device(dev);
+	int index = sattr->index;
+	int nr = sattr->nr;
+
+	return sprintf(buf, "%ld\n",
+		       in_from_reg(data->in[index][nr], data->in_index[index]));
+}
+
+static umode_t nct6683_in_is_visible(struct kobject *kobj,
+				     struct attribute *attr, int index)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct nct6683_data *data = dev_get_drvdata(dev);
+	int nr = index % 4;	/* attribute */
+
+	/*
+	 * Voltage limits exist for Intel boards,
+	 * but register location and encoding is unknown
+	 */
+	if ((nr == 2 || nr == 3) &&
+	    data->customer_id == NCT6683_CUSTOMER_ID_INTEL)
+		return 0;
+
+	return attr->mode;
+}
+
+SENSOR_TEMPLATE(in_label, "in%d_label", S_IRUGO, show_in_label, NULL, 0);
+SENSOR_TEMPLATE_2(in_input, "in%d_input", S_IRUGO, show_in_reg, NULL, 0, 0);
+SENSOR_TEMPLATE_2(in_min, "in%d_min", S_IRUGO, show_in_reg, NULL, 0, 1);
+SENSOR_TEMPLATE_2(in_max, "in%d_max", S_IRUGO, show_in_reg, NULL, 0, 2);
+
+static struct sensor_device_template *nct6683_attributes_in_template[] = {
+	&sensor_dev_template_in_label,
+	&sensor_dev_template_in_input,
+	&sensor_dev_template_in_min,
+	&sensor_dev_template_in_max,
+	NULL
+};
+
+static struct sensor_template_group nct6683_in_template_group = {
+	.templates = nct6683_attributes_in_template,
+	.is_visible = nct6683_in_is_visible,
+};
+
+static ssize_t
+show_fan(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	struct nct6683_data *data = nct6683_update_device(dev);
+
+	return sprintf(buf, "%d\n", data->rpm[sattr->index]);
+}
+
+static ssize_t
+show_fan_min(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6683_data *data = nct6683_update_device(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int nr = sattr->index;
+
+	return sprintf(buf, "%d\n", data->fan_min[nr]);
+}
+
+static ssize_t
+show_fan_pulses(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	struct nct6683_data *data = nct6683_update_device(dev);
+
+	return sprintf(buf, "%d\n",
+		       ((data->fanin_cfg[sattr->index] >> 5) & 0x03) + 1);
+}
+
+static umode_t nct6683_fan_is_visible(struct kobject *kobj,
+				      struct attribute *attr, int index)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct nct6683_data *data = dev_get_drvdata(dev);
+	int fan = index / 3;	/* fan index */
+	int nr = index % 3;	/* attribute index */
+
+	if (!(data->have_fan & (1 << fan)))
+		return 0;
+
+	/*
+	 * Intel may have minimum fan speed limits,
+	 * but register location and encoding are unknown.
+	 */
+	if (nr == 2 && data->customer_id == NCT6683_CUSTOMER_ID_INTEL)
+		return 0;
+
+	return attr->mode;
+}
+
+SENSOR_TEMPLATE(fan_input, "fan%d_input", S_IRUGO, show_fan, NULL, 0);
+SENSOR_TEMPLATE(fan_pulses, "fan%d_pulses", S_IRUGO, show_fan_pulses, NULL, 0);
+SENSOR_TEMPLATE(fan_min, "fan%d_min", S_IRUGO, show_fan_min, NULL, 0);
+
+/*
+ * nct6683_fan_is_visible uses the index into the following array
+ * to determine if attributes should be created or not.
+ * Any change in order or content must be matched.
+ */
+static struct sensor_device_template *nct6683_attributes_fan_template[] = {
+	&sensor_dev_template_fan_input,
+	&sensor_dev_template_fan_pulses,
+	&sensor_dev_template_fan_min,
+	NULL
+};
+
+static struct sensor_template_group nct6683_fan_template_group = {
+	.templates = nct6683_attributes_fan_template,
+	.is_visible = nct6683_fan_is_visible,
+	.base = 1,
+};
+
+static ssize_t
+show_temp_label(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	struct nct6683_data *data = nct6683_update_device(dev);
+	int nr = sattr->index;
+
+	return sprintf(buf, "%s\n", nct6683_mon_label[data->temp_src[nr]]);
+}
+
+static ssize_t
+show_temp8(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	struct nct6683_data *data = nct6683_update_device(dev);
+	int index = sattr->index;
+	int nr = sattr->nr;
+
+	return sprintf(buf, "%d\n", data->temp[index][nr] * 1000);
+}
+
+static ssize_t
+show_temp_hyst(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	struct nct6683_data *data = nct6683_update_device(dev);
+	int nr = sattr->index;
+	int temp = data->temp[1][nr] - data->temp[2][nr];
+
+	return sprintf(buf, "%d\n", temp * 1000);
+}
+
+static ssize_t
+show_temp16(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	struct nct6683_data *data = nct6683_update_device(dev);
+	int index = sattr->index;
+
+	return sprintf(buf, "%d\n", (data->temp_in[index] / 128) * 500);
+}
+
+/*
+ * Temperature sensor type is determined by temperature source
+ * and can not be modified.
+ * 0x02..0x07: Thermal diode
+ * 0x08..0x18: Thermistor
+ * 0x20..0x2b: Intel PECI
+ * 0x42..0x49: AMD TSI
+ * Others are unspecified (not visible)
+ */
+
+static int get_temp_type(u8 src)
+{
+	if (src >= 0x02 && src <= 0x07)
+		return 3;	/* thermal diode */
+	else if (src >= 0x08 && src <= 0x18)
+		return 4;	/* thermistor */
+	else if (src >= 0x20 && src <= 0x2b)
+		return 6;	/* PECI */
+	else if (src >= 0x42 && src <= 0x49)
+		return 5;
+
+	return 0;
+}
+
+static ssize_t
+show_temp_type(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6683_data *data = nct6683_update_device(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int nr = sattr->index;
+	return sprintf(buf, "%d\n", get_temp_type(data->temp_src[nr]));
+}
+
+static umode_t nct6683_temp_is_visible(struct kobject *kobj,
+				       struct attribute *attr, int index)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct nct6683_data *data = dev_get_drvdata(dev);
+	int temp = index / 7;	/* temp index */
+	int nr = index % 7;	/* attribute index */
+
+	/*
+	 * Intel does not have low temperature limits or temperature hysteresis
+	 * registers, or at least register location and encoding is unknown.
+	 */
+	if ((nr == 2 || nr == 4) &&
+	    data->customer_id == NCT6683_CUSTOMER_ID_INTEL)
+		return 0;
+
+	if (nr == 6 && get_temp_type(data->temp_src[temp]) == 0)
+		return 0;				/* type */
+
+	return attr->mode;
+}
+
+SENSOR_TEMPLATE(temp_input, "temp%d_input", S_IRUGO, show_temp16, NULL, 0);
+SENSOR_TEMPLATE(temp_label, "temp%d_label", S_IRUGO, show_temp_label, NULL, 0);
+SENSOR_TEMPLATE_2(temp_min, "temp%d_min", S_IRUGO, show_temp8, NULL, 0, 0);
+SENSOR_TEMPLATE_2(temp_max, "temp%d_max", S_IRUGO, show_temp8, NULL, 0, 1);
+SENSOR_TEMPLATE(temp_max_hyst, "temp%d_max_hyst", S_IRUGO, show_temp_hyst, NULL,
+		0);
+SENSOR_TEMPLATE_2(temp_crit, "temp%d_crit", S_IRUGO, show_temp8, NULL, 0, 3);
+SENSOR_TEMPLATE(temp_type, "temp%d_type", S_IRUGO, show_temp_type, NULL, 0);
+
+/*
+ * nct6683_temp_is_visible uses the index into the following array
+ * to determine if attributes should be created or not.
+ * Any change in order or content must be matched.
+ */
+static struct sensor_device_template *nct6683_attributes_temp_template[] = {
+	&sensor_dev_template_temp_input,
+	&sensor_dev_template_temp_label,
+	&sensor_dev_template_temp_min,		/* 2 */
+	&sensor_dev_template_temp_max,		/* 3 */
+	&sensor_dev_template_temp_max_hyst,	/* 4 */
+	&sensor_dev_template_temp_crit,		/* 5 */
+	&sensor_dev_template_temp_type,		/* 6 */
+	NULL
+};
+
+static struct sensor_template_group nct6683_temp_template_group = {
+	.templates = nct6683_attributes_temp_template,
+	.is_visible = nct6683_temp_is_visible,
+	.base = 1,
+};
+
+static ssize_t
+show_pwm(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6683_data *data = nct6683_update_device(dev);
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	int index = sattr->index;
+
+	return sprintf(buf, "%d\n", data->pwm[index]);
+}
+
+SENSOR_TEMPLATE(pwm, "pwm%d", S_IRUGO, show_pwm, NULL, 0);
+
+static umode_t nct6683_pwm_is_visible(struct kobject *kobj,
+				      struct attribute *attr, int index)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct nct6683_data *data = dev_get_drvdata(dev);
+	int pwm = index;	/* pwm index */
+
+	if (!(data->have_pwm & (1 << pwm)))
+		return 0;
+
+	return attr->mode;
+}
+
+static struct sensor_device_template *nct6683_attributes_pwm_template[] = {
+	&sensor_dev_template_pwm,
+	NULL
+};
+
+static struct sensor_template_group nct6683_pwm_template_group = {
+	.templates = nct6683_attributes_pwm_template,
+	.is_visible = nct6683_pwm_is_visible,
+	.base = 1,
+};
+
+static ssize_t
+show_global_beep(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6683_data *data = dev_get_drvdata(dev);
+	int ret;
+	u8 reg;
+
+	mutex_lock(&data->update_lock);
+
+	ret = superio_enter(data->sioreg);
+	if (ret)
+		goto error;
+	superio_select(data->sioreg, NCT6683_LD_HWM);
+	reg = superio_inb(data->sioreg, NCT6683_REG_CR_BEEP);
+	superio_exit(data->sioreg);
+
+	mutex_unlock(&data->update_lock);
+
+	return sprintf(buf, "%u\n", !!(reg & NCT6683_CR_BEEP_MASK));
+
+error:
+	mutex_unlock(&data->update_lock);
+	return ret;
+}
+
+static ssize_t
+store_global_beep(struct device *dev, struct device_attribute *attr,
+		  const char *buf, size_t count)
+{
+	struct nct6683_data *data = dev_get_drvdata(dev);
+	unsigned long val;
+	u8 reg;
+	int ret;
+
+	if (kstrtoul(buf, 10, &val) || (val != 0 && val != 1))
+		return -EINVAL;
+
+	mutex_lock(&data->update_lock);
+
+	ret = superio_enter(data->sioreg);
+	if (ret) {
+		count = ret;
+		goto error;
+	}
+
+	superio_select(data->sioreg, NCT6683_LD_HWM);
+	reg = superio_inb(data->sioreg, NCT6683_REG_CR_BEEP);
+	if (val)
+		reg |= NCT6683_CR_BEEP_MASK;
+	else
+		reg &= ~NCT6683_CR_BEEP_MASK;
+	superio_outb(data->sioreg, NCT6683_REG_CR_BEEP, reg);
+	superio_exit(data->sioreg);
+error:
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+/* Case open detection */
+
+static ssize_t
+show_caseopen(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6683_data *data = dev_get_drvdata(dev);
+	int ret;
+	u8 reg;
+
+	mutex_lock(&data->update_lock);
+
+	ret = superio_enter(data->sioreg);
+	if (ret)
+		goto error;
+	superio_select(data->sioreg, NCT6683_LD_ACPI);
+	reg = superio_inb(data->sioreg, NCT6683_REG_CR_CASEOPEN);
+	superio_exit(data->sioreg);
+
+	mutex_unlock(&data->update_lock);
+
+	return sprintf(buf, "%u\n", !(reg & NCT6683_CR_CASEOPEN_MASK));
+
+error:
+	mutex_unlock(&data->update_lock);
+	return ret;
+}
+
+static ssize_t
+clear_caseopen(struct device *dev, struct device_attribute *attr,
+	       const char *buf, size_t count)
+{
+	struct nct6683_data *data = dev_get_drvdata(dev);
+	unsigned long val;
+	u8 reg;
+	int ret;
+
+	if (kstrtoul(buf, 10, &val) || val != 0)
+		return -EINVAL;
+
+	mutex_lock(&data->update_lock);
+
+	/*
+	 * Use CR registers to clear caseopen status.
+	 * Caseopen is activ low, clear by writing 1 into the register.
+	 */
+
+	ret = superio_enter(data->sioreg);
+	if (ret) {
+		count = ret;
+		goto error;
+	}
+
+	superio_select(data->sioreg, NCT6683_LD_ACPI);
+	reg = superio_inb(data->sioreg, NCT6683_REG_CR_CASEOPEN);
+	reg |= NCT6683_CR_CASEOPEN_MASK;
+	superio_outb(data->sioreg, NCT6683_REG_CR_CASEOPEN, reg);
+	reg &= ~NCT6683_CR_CASEOPEN_MASK;
+	superio_outb(data->sioreg, NCT6683_REG_CR_CASEOPEN, reg);
+	superio_exit(data->sioreg);
+
+	data->valid = false;	/* Force cache refresh */
+error:
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+static DEVICE_ATTR(intrusion0_alarm, S_IWUSR | S_IRUGO, show_caseopen,
+		   clear_caseopen);
+static DEVICE_ATTR(beep_enable, S_IWUSR | S_IRUGO, show_global_beep,
+		   store_global_beep);
+
+static struct attribute *nct6683_attributes_other[] = {
+	&dev_attr_intrusion0_alarm.attr,
+	&dev_attr_beep_enable.attr,
+	NULL
+};
+
+static const struct attribute_group nct6683_group_other = {
+	.attrs = nct6683_attributes_other,
+};
+
+/* Get the monitoring functions started */
+static inline void nct6683_init_device(struct nct6683_data *data)
+{
+	u8 tmp;
+
+	/* Start hardware monitoring if needed */
+	tmp = nct6683_read(data, NCT6683_HWM_CFG);
+	if (!(tmp & 0x80))
+		nct6683_write(data, NCT6683_HWM_CFG, tmp | 0x80);
+}
+
+/*
+ * There are a total of 24 fan inputs. Each can be configured as input
+ * or as output. A maximum of 16 inputs and 8 outputs is configurable.
+ */
+static void
+nct6683_setup_fans(struct nct6683_data *data)
+{
+	int i;
+	u8 reg;
+
+	for (i = 0; i < NCT6683_NUM_REG_FAN; i++) {
+		reg = nct6683_read(data, NCT6683_REG_FANIN_CFG(i));
+		if (reg & 0x80)
+			data->have_fan |= 1 << i;
+		data->fanin_cfg[i] = reg;
+	}
+	for (i = 0; i < NCT6683_NUM_REG_PWM; i++) {
+		reg = nct6683_read(data, NCT6683_REG_FANOUT_CFG(i));
+		if (reg & 0x80)
+			data->have_pwm |= 1 << i;
+		data->fanout_cfg[i] = reg;
+	}
+}
+
+/*
+ * Translation from monitoring register to temperature and voltage attributes
+ * ==========================================================================
+ *
+ * There are a total of 32 monitoring registers. Each can be assigned to either
+ * a temperature or voltage monitoring source.
+ * NCT6683_REG_MON_CFG(x) defines assignment for each monitoring source.
+ *
+ * Temperature and voltage attribute mapping is determined by walking through
+ * the NCT6683_REG_MON_CFG registers. If the assigned source is
+ * a temperature, temp_index[n] is set to the monitor register index, and
+ * temp_src[n] is set to the temperature source. If the assigned source is
+ * a voltage, the respective values are stored in in_index[] and in_src[],
+ * respectively.
+ */
+
+static void nct6683_setup_sensors(struct nct6683_data *data)
+{
+	u8 reg;
+	int i;
+
+	data->temp_num = 0;
+	data->in_num = 0;
+	for (i = 0; i < NCT6683_NUM_REG_MON; i++) {
+		reg = nct6683_read(data, NCT6683_REG_MON_CFG(i)) & 0x7f;
+		/* Ignore invalid assignments */
+		if (reg >= NUM_MON_LABELS)
+			continue;
+		/* Skip if disabled or reserved */
+		if (nct6683_mon_label[reg] == NULL)
+			continue;
+		if (reg < MON_VOLTAGE_START) {
+			data->temp_index[data->temp_num] = i;
+			data->temp_src[data->temp_num] = reg;
+			data->temp_num++;
+		} else {
+			data->in_index[data->in_num] = i;
+			data->in_src[data->in_num] = reg;
+			data->in_num++;
+		}
+	}
+}
+
+static int nct6683_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct nct6683_sio_data *sio_data = dev->platform_data;
+	struct attribute_group *group;
+	struct nct6683_data *data;
+	struct device *hwmon_dev;
+	struct resource *res;
+	int groups = 0;
+
+	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
+	if (!devm_request_region(dev, res->start, IOREGION_LENGTH, DRVNAME))
+		return -EBUSY;
+
+	data = devm_kzalloc(dev, sizeof(struct nct6683_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->kind = sio_data->kind;
+	data->sioreg = sio_data->sioreg;
+	data->addr = res->start;
+	mutex_init(&data->update_lock);
+	platform_set_drvdata(pdev, data);
+
+	data->customer_id = nct6683_read16(data, NCT6683_REG_CUSTOMER_ID);
+
+	nct6683_init_device(data);
+	nct6683_setup_fans(data);
+	nct6683_setup_sensors(data);
+
+	/* Register sysfs hooks */
+
+	if (data->have_pwm) {
+		group = nct6683_create_attr_group(dev,
+						  &nct6683_pwm_template_group,
+						  fls(data->have_pwm));
+		if (IS_ERR(group))
+			return PTR_ERR(group);
+		data->groups[groups++] = group;
+	}
+
+	if (data->in_num) {
+		group = nct6683_create_attr_group(dev,
+						  &nct6683_in_template_group,
+						  data->in_num);
+		if (IS_ERR(group))
+			return PTR_ERR(group);
+		data->groups[groups++] = group;
+	}
+
+	if (data->have_fan) {
+		group = nct6683_create_attr_group(dev,
+						  &nct6683_fan_template_group,
+						  fls(data->have_fan));
+		if (IS_ERR(group))
+			return PTR_ERR(group);
+		data->groups[groups++] = group;
+	}
+
+	if (data->temp_num) {
+		group = nct6683_create_attr_group(dev,
+						  &nct6683_temp_template_group,
+						  data->temp_num);
+		if (IS_ERR(group))
+			return PTR_ERR(group);
+		data->groups[groups++] = group;
+	}
+	data->groups[groups++] = &nct6683_group_other;
+
+	dev_info(dev, "%s EC firmware version %d.%d build %02x/%02x/%02x\n",
+		 nct6683_chip_names[data->kind],
+		 nct6683_read(data, NCT6683_REG_VERSION_HI),
+		 nct6683_read(data, NCT6683_REG_VERSION_LO),
+		 nct6683_read(data, NCT6683_REG_BUILD_MONTH),
+		 nct6683_read(data, NCT6683_REG_BUILD_DAY),
+		 nct6683_read(data, NCT6683_REG_BUILD_YEAR));
+
+	hwmon_dev = devm_hwmon_device_register_with_groups(dev,
+			nct6683_device_names[data->kind], data, data->groups);
+	return PTR_ERR_OR_ZERO(hwmon_dev);
+}
+
+#ifdef CONFIG_PM
+static int nct6683_suspend(struct device *dev)
+{
+	struct nct6683_data *data = nct6683_update_device(dev);
+
+	mutex_lock(&data->update_lock);
+	data->hwm_cfg = nct6683_read(data, NCT6683_HWM_CFG);
+	mutex_unlock(&data->update_lock);
+
+	return 0;
+}
+
+static int nct6683_resume(struct device *dev)
+{
+	struct nct6683_data *data = dev_get_drvdata(dev);
+
+	mutex_lock(&data->update_lock);
+
+	nct6683_write(data, NCT6683_HWM_CFG, data->hwm_cfg);
+
+	/* Force re-reading all values */
+	data->valid = false;
+	mutex_unlock(&data->update_lock);
+
+	return 0;
+}
+
+static const struct dev_pm_ops nct6683_dev_pm_ops = {
+	.suspend = nct6683_suspend,
+	.resume = nct6683_resume,
+	.freeze = nct6683_suspend,
+	.restore = nct6683_resume,
+};
+
+#define NCT6683_DEV_PM_OPS	(&nct6683_dev_pm_ops)
+#else
+#define NCT6683_DEV_PM_OPS	NULL
+#endif /* CONFIG_PM */
+
+static struct platform_driver nct6683_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= DRVNAME,
+		.pm	= NCT6683_DEV_PM_OPS,
+	},
+	.probe		= nct6683_probe,
+};
+
+static int __init nct6683_find(int sioaddr, struct nct6683_sio_data *sio_data)
+{
+	const char *board_vendor;
+	int addr;
+	u16 val;
+	int err;
+
+	/*
+	 * Only run on Intel boards unless the 'force' module parameter is set
+	 */
+	if (!force) {
+		board_vendor = dmi_get_system_info(DMI_BOARD_VENDOR);
+		if (!board_vendor || strcmp(board_vendor, "Intel Corporation"))
+			return -ENODEV;
+	}
+
+	err = superio_enter(sioaddr);
+	if (err)
+		return err;
+
+	val = (superio_inb(sioaddr, SIO_REG_DEVID) << 8)
+	       | superio_inb(sioaddr, SIO_REG_DEVID + 1);
+
+	switch (val & SIO_ID_MASK) {
+	case SIO_NCT6683_ID:
+		sio_data->kind = nct6683;
+		break;
+	default:
+		if (val != 0xffff)
+			pr_debug("unsupported chip ID: 0x%04x\n", val);
+		goto fail;
+	}
+
+	/* We have a known chip, find the HWM I/O address */
+	superio_select(sioaddr, NCT6683_LD_HWM);
+	val = (superio_inb(sioaddr, SIO_REG_ADDR) << 8)
+	    | superio_inb(sioaddr, SIO_REG_ADDR + 1);
+	addr = val & IOREGION_ALIGNMENT;
+	if (addr == 0) {
+		pr_err("EC base I/O port unconfigured\n");
+		goto fail;
+	}
+
+	/* Activate logical device if needed */
+	val = superio_inb(sioaddr, SIO_REG_ENABLE);
+	if (!(val & 0x01)) {
+		pr_err("EC is disabled\n");
+		goto fail;
+	}
+
+	superio_exit(sioaddr);
+	pr_info("Found %s or compatible chip at %#x:%#x\n",
+		nct6683_chip_names[sio_data->kind], sioaddr, addr);
+	sio_data->sioreg = sioaddr;
+
+	return addr;
+
+fail:
+	superio_exit(sioaddr);
+	return -ENODEV;
+}
+
+/*
+ * when Super-I/O functions move to a separate file, the Super-I/O
+ * bus will manage the lifetime of the device and this module will only keep
+ * track of the nct6683 driver. But since we use platform_device_alloc(), we
+ * must keep track of the device
+ */
+static struct platform_device *pdev[2];
+
+static int __init sensors_nct6683_init(void)
+{
+	struct nct6683_sio_data sio_data;
+	int sioaddr[2] = { 0x2e, 0x4e };
+	struct resource res;
+	bool found = false;
+	int address;
+	int i, err;
+
+	err = platform_driver_register(&nct6683_driver);
+	if (err)
+		return err;
+
+	/*
+	 * initialize sio_data->kind and sio_data->sioreg.
+	 *
+	 * when Super-I/O functions move to a separate file, the Super-I/O
+	 * driver will probe 0x2e and 0x4e and auto-detect the presence of a
+	 * nct6683 hardware monitor, and call probe()
+	 */
+	for (i = 0; i < ARRAY_SIZE(pdev); i++) {
+		address = nct6683_find(sioaddr[i], &sio_data);
+		if (address <= 0)
+			continue;
+
+		found = true;
+
+		pdev[i] = platform_device_alloc(DRVNAME, address);
+		if (!pdev[i]) {
+			err = -ENOMEM;
+			goto exit_device_unregister;
+		}
+
+		err = platform_device_add_data(pdev[i], &sio_data,
+					       sizeof(struct nct6683_sio_data));
+		if (err)
+			goto exit_device_put;
+
+		memset(&res, 0, sizeof(res));
+		res.name = DRVNAME;
+		res.start = address + IOREGION_OFFSET;
+		res.end = address + IOREGION_OFFSET + IOREGION_LENGTH - 1;
+		res.flags = IORESOURCE_IO;
+
+		err = acpi_check_resource_conflict(&res);
+		if (err) {
+			platform_device_put(pdev[i]);
+			pdev[i] = NULL;
+			continue;
+		}
+
+		err = platform_device_add_resources(pdev[i], &res, 1);
+		if (err)
+			goto exit_device_put;
+
+		/* platform_device_add calls probe() */
+		err = platform_device_add(pdev[i]);
+		if (err)
+			goto exit_device_put;
+	}
+	if (!found) {
+		err = -ENODEV;
+		goto exit_unregister;
+	}
+
+	return 0;
+
+exit_device_put:
+	platform_device_put(pdev[i]);
+exit_device_unregister:
+	while (--i >= 0) {
+		if (pdev[i])
+			platform_device_unregister(pdev[i]);
+	}
+exit_unregister:
+	platform_driver_unregister(&nct6683_driver);
+	return err;
+}
+
+static void __exit sensors_nct6683_exit(void)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(pdev); i++) {
+		if (pdev[i])
+			platform_device_unregister(pdev[i]);
+	}
+	platform_driver_unregister(&nct6683_driver);
+}
+
+MODULE_AUTHOR("Guenter Roeck <linux@roeck-us.net>");
+MODULE_DESCRIPTION("NCT6683D driver");
+MODULE_LICENSE("GPL");
+
+module_init(sensors_nct6683_init);
+module_exit(sensors_nct6683_exit);
diff --git a/drivers/hwmon/nct6775.c b/drivers/hwmon/nct6775.c
index 38d5a633405..59d9a3fc96b 100644
--- a/drivers/hwmon/nct6775.c
+++ b/drivers/hwmon/nct6775.c
@@ -4160,7 +4160,7 @@ static int __init sensors_nct6775_init(void)
 		pdev[i] = platform_device_alloc(DRVNAME, address);
 		if (!pdev[i]) {
 			err = -ENOMEM;
-			goto exit_device_put;
+			goto exit_device_unregister;
 		}
 
 		err = platform_device_add_data(pdev[i], &sio_data,
@@ -4198,9 +4198,11 @@ static int __init sensors_nct6775_init(void)
 	return 0;
 
 exit_device_put:
-	for (i = 0; i < ARRAY_SIZE(pdev); i++) {
+	platform_device_put(pdev[i]);
+exit_device_unregister:
+	while (--i >= 0) {
 		if (pdev[i])
-			platform_device_put(pdev[i]);
+			platform_device_unregister(pdev[i]);
 	}
 exit_unregister:
 	platform_driver_unregister(&nct6775_driver);
diff --git a/drivers/hwmon/ntc_thermistor.c b/drivers/hwmon/ntc_thermistor.c
index 8a17f01e867..e76feb86a1d 100644
--- a/drivers/hwmon/ntc_thermistor.c
+++ b/drivers/hwmon/ntc_thermistor.c
@@ -44,6 +44,7 @@ struct ntc_compensation {
 	unsigned int	ohm;
 };
 
+/* Order matters, ntc_match references the entries by index */
 static const struct platform_device_id ntc_thermistor_id[] = {
 	{ "ncp15wb473", TYPE_NCPXXWB473 },
 	{ "ncp18wb473", TYPE_NCPXXWB473 },
@@ -141,7 +142,7 @@ struct ntc_data {
 	char name[PLATFORM_NAME_SIZE];
 };
 
-#ifdef CONFIG_OF
+#if defined(CONFIG_OF) && IS_ENABLED(CONFIG_IIO)
 static int ntc_adc_iio_read(struct ntc_thermistor_platform_data *pdata)
 {
 	struct iio_channel *channel = pdata->chan;
@@ -163,15 +164,15 @@ static int ntc_adc_iio_read(struct ntc_thermistor_platform_data *pdata)
 
 static const struct of_device_id ntc_match[] = {
 	{ .compatible = "ntc,ncp15wb473",
-		.data = &ntc_thermistor_id[TYPE_NCPXXWB473] },
+		.data = &ntc_thermistor_id[0] },
 	{ .compatible = "ntc,ncp18wb473",
-		.data = &ntc_thermistor_id[TYPE_NCPXXWB473] },
+		.data = &ntc_thermistor_id[1] },
 	{ .compatible = "ntc,ncp21wb473",
-		.data = &ntc_thermistor_id[TYPE_NCPXXWB473] },
+		.data = &ntc_thermistor_id[2] },
 	{ .compatible = "ntc,ncp03wb473",
-		.data = &ntc_thermistor_id[TYPE_NCPXXWB473] },
+		.data = &ntc_thermistor_id[3] },
 	{ .compatible = "ntc,ncp15wl333",
-		.data = &ntc_thermistor_id[TYPE_NCPXXWL333] },
+		.data = &ntc_thermistor_id[4] },
 	{ },
 };
 MODULE_DEVICE_TABLE(of, ntc_match);
@@ -223,6 +224,8 @@ ntc_thermistor_parse_dt(struct platform_device *pdev)
 	return NULL;
 }
 
+#define ntc_match	NULL
+
 static void ntc_iio_channel_release(struct ntc_thermistor_platform_data *pdata)
 { }
 #endif
diff --git a/drivers/hwmon/pc87427.c b/drivers/hwmon/pc87427.c
index d847e0a084e..9e4684e747e 100644
--- a/drivers/hwmon/pc87427.c
+++ b/drivers/hwmon/pc87427.c
@@ -1081,10 +1081,8 @@ static int pc87427_probe(struct platform_device *pdev)
 
 	data = devm_kzalloc(&pdev->dev, sizeof(struct pc87427_data),
 			    GFP_KERNEL);
-	if (!data) {
-		pr_err("Out of memory\n");
+	if (!data)
 		return -ENOMEM;
-	}
 
 	data->address[0] = sio_data->address[0];
 	data->address[1] = sio_data->address[1];
diff --git a/drivers/hwmon/s3c-hwmon.c b/drivers/hwmon/s3c-hwmon.c
index 73bd64e8c30..0674c13bbd4 100644
--- a/drivers/hwmon/s3c-hwmon.c
+++ b/drivers/hwmon/s3c-hwmon.c
@@ -285,10 +285,8 @@ static int s3c_hwmon_probe(struct platform_device *dev)
 	}
 
 	hwmon = devm_kzalloc(&dev->dev, sizeof(struct s3c_hwmon), GFP_KERNEL);
-	if (hwmon == NULL) {
-		dev_err(&dev->dev, "no memory\n");
+	if (hwmon == NULL)
 		return -ENOMEM;
-	}
 
 	platform_set_drvdata(dev, hwmon);
 
diff --git a/drivers/hwmon/tmp102.c b/drivers/hwmon/tmp102.c
index 6748b4583e7..51719956cc0 100644
--- a/drivers/hwmon/tmp102.c
+++ b/drivers/hwmon/tmp102.c
@@ -51,6 +51,7 @@
 #define	TMP102_THIGH_REG		0x03
 
 struct tmp102 {
+	struct i2c_client *client;
 	struct device *hwmon_dev;
 	struct thermal_zone_device *tz;
 	struct mutex lock;
@@ -77,9 +78,10 @@ static const u8 tmp102_reg[] = {
 	TMP102_THIGH_REG,
 };
 
-static struct tmp102 *tmp102_update_device(struct i2c_client *client)
+static struct tmp102 *tmp102_update_device(struct device *dev)
 {
-	struct tmp102 *tmp102 = i2c_get_clientdata(client);
+	struct tmp102 *tmp102 = dev_get_drvdata(dev);
+	struct i2c_client *client = tmp102->client;
 
 	mutex_lock(&tmp102->lock);
 	if (time_after(jiffies, tmp102->last_update + HZ / 3)) {
@@ -98,7 +100,7 @@ static struct tmp102 *tmp102_update_device(struct i2c_client *client)
 
 static int tmp102_read_temp(void *dev, long *temp)
 {
-	struct tmp102 *tmp102 = tmp102_update_device(to_i2c_client(dev));
+	struct tmp102 *tmp102 = tmp102_update_device(dev);
 
 	*temp = tmp102->temp[0];
 
@@ -110,7 +112,7 @@ static ssize_t tmp102_show_temp(struct device *dev,
 				char *buf)
 {
 	struct sensor_device_attribute *sda = to_sensor_dev_attr(attr);
-	struct tmp102 *tmp102 = tmp102_update_device(to_i2c_client(dev));
+	struct tmp102 *tmp102 = tmp102_update_device(dev);
 
 	return sprintf(buf, "%d\n", tmp102->temp[sda->index]);
 }
@@ -120,8 +122,8 @@ static ssize_t tmp102_set_temp(struct device *dev,
 			       const char *buf, size_t count)
 {
 	struct sensor_device_attribute *sda = to_sensor_dev_attr(attr);
-	struct i2c_client *client = to_i2c_client(dev);
-	struct tmp102 *tmp102 = i2c_get_clientdata(client);
+	struct tmp102 *tmp102 = dev_get_drvdata(dev);
+	struct i2c_client *client = tmp102->client;
 	long val;
 	int status;
 
@@ -145,16 +147,13 @@ static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IWUSR | S_IRUGO, tmp102_show_temp,
 static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, tmp102_show_temp,
 			  tmp102_set_temp, 2);
 
-static struct attribute *tmp102_attributes[] = {
+static struct attribute *tmp102_attrs[] = {
 	&sensor_dev_attr_temp1_input.dev_attr.attr,
 	&sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
 	&sensor_dev_attr_temp1_max.dev_attr.attr,
 	NULL
 };
-
-static const struct attribute_group tmp102_attr_group = {
-	.attrs = tmp102_attributes,
-};
+ATTRIBUTE_GROUPS(tmp102);
 
 #define TMP102_CONFIG  (TMP102_CONF_TM | TMP102_CONF_EM | TMP102_CONF_CR1)
 #define TMP102_CONFIG_RD_ONLY (TMP102_CONF_R0 | TMP102_CONF_R1 | TMP102_CONF_AL)
@@ -162,72 +161,68 @@ static const struct attribute_group tmp102_attr_group = {
 static int tmp102_probe(struct i2c_client *client,
 				  const struct i2c_device_id *id)
 {
+	struct device *dev = &client->dev;
+	struct device *hwmon_dev;
 	struct tmp102 *tmp102;
 	int status;
 
 	if (!i2c_check_functionality(client->adapter,
 				     I2C_FUNC_SMBUS_WORD_DATA)) {
-		dev_err(&client->dev,
+		dev_err(dev,
 			"adapter doesn't support SMBus word transactions\n");
 		return -ENODEV;
 	}
 
-	tmp102 = devm_kzalloc(&client->dev, sizeof(*tmp102), GFP_KERNEL);
+	tmp102 = devm_kzalloc(dev, sizeof(*tmp102), GFP_KERNEL);
 	if (!tmp102)
 		return -ENOMEM;
 
 	i2c_set_clientdata(client, tmp102);
+	tmp102->client = client;
 
 	status = i2c_smbus_read_word_swapped(client, TMP102_CONF_REG);
 	if (status < 0) {
-		dev_err(&client->dev, "error reading config register\n");
+		dev_err(dev, "error reading config register\n");
 		return status;
 	}
 	tmp102->config_orig = status;
 	status = i2c_smbus_write_word_swapped(client, TMP102_CONF_REG,
 					      TMP102_CONFIG);
 	if (status < 0) {
-		dev_err(&client->dev, "error writing config register\n");
+		dev_err(dev, "error writing config register\n");
 		goto fail_restore_config;
 	}
 	status = i2c_smbus_read_word_swapped(client, TMP102_CONF_REG);
 	if (status < 0) {
-		dev_err(&client->dev, "error reading config register\n");
+		dev_err(dev, "error reading config register\n");
 		goto fail_restore_config;
 	}
 	status &= ~TMP102_CONFIG_RD_ONLY;
 	if (status != TMP102_CONFIG) {
-		dev_err(&client->dev, "config settings did not stick\n");
+		dev_err(dev, "config settings did not stick\n");
 		status = -ENODEV;
 		goto fail_restore_config;
 	}
 	tmp102->last_update = jiffies - HZ;
 	mutex_init(&tmp102->lock);
 
-	status = sysfs_create_group(&client->dev.kobj, &tmp102_attr_group);
-	if (status) {
-		dev_dbg(&client->dev, "could not create sysfs files\n");
+	hwmon_dev = hwmon_device_register_with_groups(dev, client->name,
+						      tmp102, tmp102_groups);
+	if (IS_ERR(hwmon_dev)) {
+		dev_dbg(dev, "unable to register hwmon device\n");
+		status = PTR_ERR(hwmon_dev);
 		goto fail_restore_config;
 	}
-	tmp102->hwmon_dev = hwmon_device_register(&client->dev);
-	if (IS_ERR(tmp102->hwmon_dev)) {
-		dev_dbg(&client->dev, "unable to register hwmon device\n");
-		status = PTR_ERR(tmp102->hwmon_dev);
-		goto fail_remove_sysfs;
-	}
-
-	tmp102->tz = thermal_zone_of_sensor_register(&client->dev, 0,
-						     &client->dev,
+	tmp102->hwmon_dev = hwmon_dev;
+	tmp102->tz = thermal_zone_of_sensor_register(hwmon_dev, 0, hwmon_dev,
 						     tmp102_read_temp, NULL);
 	if (IS_ERR(tmp102->tz))
 		tmp102->tz = NULL;
 
-	dev_info(&client->dev, "initialized\n");
+	dev_info(dev, "initialized\n");
 
 	return 0;
 
-fail_remove_sysfs:
-	sysfs_remove_group(&client->dev.kobj, &tmp102_attr_group);
 fail_restore_config:
 	i2c_smbus_write_word_swapped(client, TMP102_CONF_REG,
 				     tmp102->config_orig);
@@ -238,9 +233,8 @@ static int tmp102_remove(struct i2c_client *client)
 {
 	struct tmp102 *tmp102 = i2c_get_clientdata(client);
 
-	thermal_zone_of_sensor_unregister(&client->dev, tmp102->tz);
+	thermal_zone_of_sensor_unregister(tmp102->hwmon_dev, tmp102->tz);
 	hwmon_device_unregister(tmp102->hwmon_dev);
-	sysfs_remove_group(&client->dev.kobj, &tmp102_attr_group);
 
 	/* Stop monitoring if device was stopped originally */
 	if (tmp102->config_orig & TMP102_CONF_SD) {
diff --git a/drivers/hwmon/tmp421.c b/drivers/hwmon/tmp421.c
index ae26b06fa81..7bab7a9bedc 100644
--- a/drivers/hwmon/tmp421.c
+++ b/drivers/hwmon/tmp421.c
@@ -69,7 +69,7 @@ static const struct i2c_device_id tmp421_id[] = {
 MODULE_DEVICE_TABLE(i2c, tmp421_id);
 
 struct tmp421_data {
-	struct device *hwmon_dev;
+	struct i2c_client *client;
 	struct mutex update_lock;
 	char valid;
 	unsigned long last_updated;
@@ -99,8 +99,8 @@ static int temp_from_u16(u16 reg)
 
 static struct tmp421_data *tmp421_update_device(struct device *dev)
 {
-	struct i2c_client *client = to_i2c_client(dev);
-	struct tmp421_data *data = i2c_get_clientdata(client);
+	struct tmp421_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
 	int i;
 
 	mutex_lock(&data->update_lock);
@@ -198,6 +198,11 @@ static const struct attribute_group tmp421_group = {
 	.is_visible = tmp421_is_visible,
 };
 
+static const struct attribute_group *tmp421_groups[] = {
+	&tmp421_group,
+	NULL
+};
+
 static int tmp421_init_client(struct i2c_client *client)
 {
 	int config, config_orig;
@@ -264,47 +269,26 @@ static int tmp421_detect(struct i2c_client *client,
 static int tmp421_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
+	struct device *dev = &client->dev;
+	struct device *hwmon_dev;
 	struct tmp421_data *data;
 	int err;
 
-	data = devm_kzalloc(&client->dev, sizeof(struct tmp421_data),
-			    GFP_KERNEL);
+	data = devm_kzalloc(dev, sizeof(struct tmp421_data), GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
-	i2c_set_clientdata(client, data);
 	mutex_init(&data->update_lock);
 	data->channels = id->driver_data;
+	data->client = client;
 
 	err = tmp421_init_client(client);
 	if (err)
 		return err;
 
-	err = sysfs_create_group(&client->dev.kobj, &tmp421_group);
-	if (err)
-		return err;
-
-	data->hwmon_dev = hwmon_device_register(&client->dev);
-	if (IS_ERR(data->hwmon_dev)) {
-		err = PTR_ERR(data->hwmon_dev);
-		data->hwmon_dev = NULL;
-		goto exit_remove;
-	}
-	return 0;
-
-exit_remove:
-	sysfs_remove_group(&client->dev.kobj, &tmp421_group);
-	return err;
-}
-
-static int tmp421_remove(struct i2c_client *client)
-{
-	struct tmp421_data *data = i2c_get_clientdata(client);
-
-	hwmon_device_unregister(data->hwmon_dev);
-	sysfs_remove_group(&client->dev.kobj, &tmp421_group);
-
-	return 0;
+	hwmon_dev = devm_hwmon_device_register_with_groups(dev, client->name,
+							   data, tmp421_groups);
+	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
 
 static struct i2c_driver tmp421_driver = {
@@ -313,7 +297,6 @@ static struct i2c_driver tmp421_driver = {
 		.name	= "tmp421",
 	},
 	.probe = tmp421_probe,
-	.remove = tmp421_remove,
 	.id_table = tmp421_id,
 	.detect = tmp421_detect,
 	.address_list = normal_i2c,
diff --git a/drivers/hwmon/ultra45_env.c b/drivers/hwmon/ultra45_env.c
index fb3e69341c1..7d465863606 100644
--- a/drivers/hwmon/ultra45_env.c
+++ b/drivers/hwmon/ultra45_env.c
@@ -252,7 +252,7 @@ static const struct attribute_group env_group = {
 
 static int env_probe(struct platform_device *op)
 {
-	struct env *p = kzalloc(sizeof(*p), GFP_KERNEL);
+	struct env *p = devm_kzalloc(&op->dev, sizeof(*p), GFP_KERNEL);
 	int err = -ENOMEM;
 
 	if (!p)
@@ -262,7 +262,7 @@ static int env_probe(struct platform_device *op)
 
 	p->regs = of_ioremap(&op->resource[0], 0, REG_SIZE, "pic16f747");
 	if (!p->regs)
-		goto out_free;
+		goto out;
 
 	err = sysfs_create_group(&op->dev.kobj, &env_group);
 	if (err)
@@ -286,8 +286,6 @@ out_sysfs_remove_group:
 out_iounmap:
 	of_iounmap(&op->resource[0], p->regs, REG_SIZE);
 
-out_free:
-	kfree(p);
 	goto out;
 }
 
@@ -299,7 +297,6 @@ static int env_remove(struct platform_device *op)
 		sysfs_remove_group(&op->dev.kobj, &env_group);
 		hwmon_device_unregister(p->hwmon_dev);
 		of_iounmap(&op->resource[0], p->regs, REG_SIZE);
-		kfree(p);
 	}
 
 	return 0;
diff --git a/drivers/hwmon/vexpress.c b/drivers/hwmon/vexpress.c
index 8242b75d96c..611f34c7333 100644
--- a/drivers/hwmon/vexpress.c
+++ b/drivers/hwmon/vexpress.c
@@ -26,7 +26,7 @@
 
 struct vexpress_hwmon_data {
 	struct device *hwmon_dev;
-	struct vexpress_config_func *func;
+	struct regmap *reg;
 	const char *name;
 };
 
@@ -53,7 +53,7 @@ static ssize_t vexpress_hwmon_u32_show(struct device *dev,
 	int err;
 	u32 value;
 
-	err = vexpress_config_read(data->func, 0, &value);
+	err = regmap_read(data->reg, 0, &value);
 	if (err)
 		return err;
 
@@ -68,11 +68,11 @@ static ssize_t vexpress_hwmon_u64_show(struct device *dev,
 	int err;
 	u32 value_hi, value_lo;
 
-	err = vexpress_config_read(data->func, 0, &value_lo);
+	err = regmap_read(data->reg, 0, &value_lo);
 	if (err)
 		return err;
 
-	err = vexpress_config_read(data->func, 1, &value_hi);
+	err = regmap_read(data->reg, 1, &value_hi);
 	if (err)
 		return err;
 
@@ -234,9 +234,9 @@ static int vexpress_hwmon_probe(struct platform_device *pdev)
 	type = match->data;
 	data->name = type->name;
 
-	data->func = vexpress_config_func_get_by_dev(&pdev->dev);
-	if (!data->func)
-		return -ENODEV;
+	data->reg = devm_regmap_init_vexpress_config(&pdev->dev);
+	if (IS_ERR(data->reg))
+		return PTR_ERR(data->reg);
 
 	err = sysfs_create_groups(&pdev->dev.kobj, type->attr_groups);
 	if (err)
@@ -252,7 +252,6 @@ static int vexpress_hwmon_probe(struct platform_device *pdev)
 
 error:
 	sysfs_remove_group(&pdev->dev.kobj, match->data);
-	vexpress_config_func_put(data->func);
 	return err;
 }
 
@@ -266,8 +265,6 @@ static int vexpress_hwmon_remove(struct platform_device *pdev)
 	match = of_match_device(vexpress_hwmon_of_match, &pdev->dev);
 	sysfs_remove_group(&pdev->dev.kobj, match->data);
 
-	vexpress_config_func_put(data->func);
-
 	return 0;
 }
 
diff --git a/drivers/hwmon/vt1211.c b/drivers/hwmon/vt1211.c
index 6b2f1a42b3f..344b22ec255 100644
--- a/drivers/hwmon/vt1211.c
+++ b/drivers/hwmon/vt1211.c
@@ -1152,10 +1152,8 @@ static int vt1211_probe(struct platform_device *pdev)
 	int i, err;
 
 	data = devm_kzalloc(dev, sizeof(struct vt1211_data), GFP_KERNEL);
-	if (!data) {
-		dev_err(dev, "Out of memory\n");
+	if (!data)
 		return -ENOMEM;
-	}
 
 	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
 	if (!devm_request_region(dev, res->start, resource_size(res),
diff --git a/drivers/i2c/busses/Kconfig b/drivers/i2c/busses/Kconfig
index c94db1c5e35..620d1004a1e 100644
--- a/drivers/i2c/busses/Kconfig
+++ b/drivers/i2c/busses/Kconfig
@@ -449,7 +449,7 @@ config I2C_EFM32
 
 config I2C_EG20T
 	tristate "Intel EG20T PCH/LAPIS Semicon IOH(ML7213/ML7223/ML7831) I2C"
-	depends on PCI
+	depends on PCI && (X86_32 || COMPILE_TEST)
 	help
 	  This driver is for PCH(Platform controller Hub) I2C of EG20T which
 	  is an IOH(Input/Output Hub) for x86 embedded processor.
@@ -570,13 +570,6 @@ config I2C_NOMADIK
 	  I2C interface from ST-Ericsson's Nomadik and Ux500 architectures,
 	  as well as the STA2X11 PCIe I/O HUB.
 
-config I2C_NUC900
-	tristate "NUC900 I2C Driver"
-	depends on ARCH_W90X900
-	help
-	  Say Y here to include support for I2C controller in the
-	  Winbond/Nuvoton NUC900 based System-on-Chip devices.
-
 config I2C_OCORES
 	tristate "OpenCores I2C Controller"
 	help
@@ -993,6 +986,15 @@ config I2C_SIBYTE
 	help
 	  Supports the SiByte SOC on-chip I2C interfaces (2 channels).
 
+config I2C_CROS_EC_TUNNEL
+	tristate "ChromeOS EC tunnel I2C bus"
+	depends on MFD_CROS_EC
+	help
+	  If you say yes here you get an I2C bus that will tunnel i2c commands
+	  through to the other side of the ChromeOS EC to the i2c bus
+	  connected there. This will work whatever the interface used to
+	  talk to the EC (SPI, I2C or LPC).
+
 config SCx200_I2C
 	tristate "NatSemi SCx200 I2C using GPIO pins (DEPRECATED)"
 	depends on SCx200_GPIO
diff --git a/drivers/i2c/busses/Makefile b/drivers/i2c/busses/Makefile
index 18d18ff9db9..298692cc600 100644
--- a/drivers/i2c/busses/Makefile
+++ b/drivers/i2c/busses/Makefile
@@ -55,7 +55,6 @@ obj-$(CONFIG_I2C_MPC)		+= i2c-mpc.o
 obj-$(CONFIG_I2C_MV64XXX)	+= i2c-mv64xxx.o
 obj-$(CONFIG_I2C_MXS)		+= i2c-mxs.o
 obj-$(CONFIG_I2C_NOMADIK)	+= i2c-nomadik.o
-obj-$(CONFIG_I2C_NUC900)	+= i2c-nuc900.o
 obj-$(CONFIG_I2C_OCORES)	+= i2c-ocores.o
 obj-$(CONFIG_I2C_OMAP)		+= i2c-omap.o
 obj-$(CONFIG_I2C_PASEMI)	+= i2c-pasemi.o
@@ -95,6 +94,7 @@ obj-$(CONFIG_I2C_VIPERBOARD)	+= i2c-viperboard.o
 # Other I2C/SMBus bus drivers
 obj-$(CONFIG_I2C_ACORN)		+= i2c-acorn.o
 obj-$(CONFIG_I2C_BCM_KONA)	+= i2c-bcm-kona.o
+obj-$(CONFIG_I2C_CROS_EC_TUNNEL)	+= i2c-cros-ec-tunnel.o
 obj-$(CONFIG_I2C_ELEKTOR)	+= i2c-elektor.o
 obj-$(CONFIG_I2C_PCA_ISA)	+= i2c-pca-isa.o
 obj-$(CONFIG_I2C_SIBYTE)	+= i2c-sibyte.o
diff --git a/drivers/i2c/busses/i2c-ali1563.c b/drivers/i2c/busses/i2c-ali1563.c
index 98a1c97739b..15517d78d5f 100644
--- a/drivers/i2c/busses/i2c-ali1563.c
+++ b/drivers/i2c/busses/i2c-ali1563.c
@@ -63,7 +63,7 @@
 static struct pci_driver ali1563_pci_driver;
 static unsigned short ali1563_smba;
 
-static int ali1563_transaction(struct i2c_adapter * a, int size)
+static int ali1563_transaction(struct i2c_adapter *a, int size)
 {
 	u32 data;
 	int timeout;
@@ -78,7 +78,7 @@ static int ali1563_transaction(struct i2c_adapter * a, int size)
 	data = inb_p(SMB_HST_STS);
 	if (data & HST_STS_BAD) {
 		dev_err(&a->dev, "ali1563: Trying to reset busy device\n");
-		outb_p(data | HST_STS_BAD,SMB_HST_STS);
+		outb_p(data | HST_STS_BAD, SMB_HST_STS);
 		data = inb_p(SMB_HST_STS);
 		if (data & HST_STS_BAD)
 			return -EBUSY;
@@ -102,10 +102,10 @@ static int ali1563_transaction(struct i2c_adapter * a, int size)
 	if (!timeout) {
 		dev_err(&a->dev, "Timeout - Trying to KILL transaction!\n");
 		/* Issue 'kill' to host controller */
-		outb_p(HST_CNTL2_KILL,SMB_HST_CNTL2);
+		outb_p(HST_CNTL2_KILL, SMB_HST_CNTL2);
 		data = inb_p(SMB_HST_STS);
 		status = -ETIMEDOUT;
- 	}
+	}
 
 	/* device error - no response, ignore the autodetection case */
 	if (data & HST_STS_DEVERR) {
@@ -117,18 +117,18 @@ static int ali1563_transaction(struct i2c_adapter * a, int size)
 	if (data & HST_STS_BUSERR) {
 		dev_err(&a->dev, "Bus collision!\n");
 		/* Issue timeout, hoping it helps */
-		outb_p(HST_CNTL1_TIMEOUT,SMB_HST_CNTL1);
+		outb_p(HST_CNTL1_TIMEOUT, SMB_HST_CNTL1);
 	}
 
 	if (data & HST_STS_FAIL) {
 		dev_err(&a->dev, "Cleaning fail after KILL!\n");
-		outb_p(0x0,SMB_HST_CNTL2);
+		outb_p(0x0, SMB_HST_CNTL2);
 	}
 
 	return status;
 }
 
-static int ali1563_block_start(struct i2c_adapter * a)
+static int ali1563_block_start(struct i2c_adapter *a)
 {
 	u32 data;
 	int timeout;
@@ -142,8 +142,8 @@ static int ali1563_block_start(struct i2c_adapter * a)
 
 	data = inb_p(SMB_HST_STS);
 	if (data & HST_STS_BAD) {
-		dev_warn(&a->dev,"ali1563: Trying to reset busy device\n");
-		outb_p(data | HST_STS_BAD,SMB_HST_STS);
+		dev_warn(&a->dev, "ali1563: Trying to reset busy device\n");
+		outb_p(data | HST_STS_BAD, SMB_HST_STS);
 		data = inb_p(SMB_HST_STS);
 		if (data & HST_STS_BAD)
 			return -EBUSY;
@@ -184,13 +184,14 @@ static int ali1563_block_start(struct i2c_adapter * a)
 	return status;
 }
 
-static int ali1563_block(struct i2c_adapter * a, union i2c_smbus_data * data, u8 rw)
+static int ali1563_block(struct i2c_adapter *a,
+			 union i2c_smbus_data *data, u8 rw)
 {
 	int i, len;
 	int error = 0;
 
 	/* Do we need this? */
-	outb_p(HST_CNTL1_LAST,SMB_HST_CNTL1);
+	outb_p(HST_CNTL1_LAST, SMB_HST_CNTL1);
 
 	if (rw == I2C_SMBUS_WRITE) {
 		len = data->block[0];
@@ -198,8 +199,8 @@ static int ali1563_block(struct i2c_adapter * a, union i2c_smbus_data * data, u8
 			len = 1;
 		else if (len > 32)
 			len = 32;
-		outb_p(len,SMB_HST_DAT0);
-		outb_p(data->block[1],SMB_BLK_DAT);
+		outb_p(len, SMB_HST_DAT0);
+		outb_p(data->block[1], SMB_BLK_DAT);
 	} else
 		len = 32;
 
@@ -208,10 +209,12 @@ static int ali1563_block(struct i2c_adapter * a, union i2c_smbus_data * data, u8
 	for (i = 0; i < len; i++) {
 		if (rw == I2C_SMBUS_WRITE) {
 			outb_p(data->block[i + 1], SMB_BLK_DAT);
-			if ((error = ali1563_block_start(a)))
+			error = ali1563_block_start(a);
+			if (error)
 				break;
 		} else {
-			if ((error = ali1563_block_start(a)))
+			error = ali1563_block_start(a);
+			if (error)
 				break;
 			if (i == 0) {
 				len = inb_p(SMB_HST_DAT0);
@@ -224,25 +227,26 @@ static int ali1563_block(struct i2c_adapter * a, union i2c_smbus_data * data, u8
 		}
 	}
 	/* Do we need this? */
-	outb_p(HST_CNTL1_LAST,SMB_HST_CNTL1);
+	outb_p(HST_CNTL1_LAST, SMB_HST_CNTL1);
 	return error;
 }
 
-static s32 ali1563_access(struct i2c_adapter * a, u16 addr,
+static s32 ali1563_access(struct i2c_adapter *a, u16 addr,
 			  unsigned short flags, char rw, u8 cmd,
-			  int size, union i2c_smbus_data * data)
+			  int size, union i2c_smbus_data *data)
 {
 	int error = 0;
 	int timeout;
 	u32 reg;
 
 	for (timeout = ALI1563_MAX_TIMEOUT; timeout; timeout--) {
-		if (!(reg = inb_p(SMB_HST_STS) & HST_STS_BUSY))
+		reg = inb_p(SMB_HST_STS);
+		if (!(reg & HST_STS_BUSY))
 			break;
 	}
 	if (!timeout)
-		dev_warn(&a->dev,"SMBus not idle. HST_STS = %02x\n",reg);
-	outb_p(0xff,SMB_HST_STS);
+		dev_warn(&a->dev, "SMBus not idle. HST_STS = %02x\n", reg);
+	outb_p(0xff, SMB_HST_STS);
 
 	/* Map the size to what the chip understands */
 	switch (size) {
@@ -268,13 +272,14 @@ static s32 ali1563_access(struct i2c_adapter * a, u16 addr,
 	}
 
 	outb_p(((addr & 0x7f) << 1) | (rw & 0x01), SMB_HST_ADD);
-	outb_p((inb_p(SMB_HST_CNTL2) & ~HST_CNTL2_SIZEMASK) | (size << 3), SMB_HST_CNTL2);
+	outb_p((inb_p(SMB_HST_CNTL2) & ~HST_CNTL2_SIZEMASK) |
+	       (size << 3), SMB_HST_CNTL2);
 
 	/* Write the command register */
 
-	switch(size) {
+	switch (size) {
 	case HST_CNTL2_BYTE:
-		if (rw== I2C_SMBUS_WRITE)
+		if (rw == I2C_SMBUS_WRITE)
 			/* Beware it uses DAT0 register and not CMD! */
 			outb_p(cmd, SMB_HST_DAT0);
 		break;
@@ -292,11 +297,12 @@ static s32 ali1563_access(struct i2c_adapter * a, u16 addr,
 		break;
 	case HST_CNTL2_BLOCK:
 		outb_p(cmd, SMB_HST_CMD);
-		error = ali1563_block(a,data,rw);
+		error = ali1563_block(a, data, rw);
 		goto Done;
 	}
 
-	if ((error = ali1563_transaction(a, size)))
+	error = ali1563_transaction(a, size);
+	if (error)
 		goto Done;
 
 	if ((rw == I2C_SMBUS_WRITE) || (size == HST_CNTL2_QUICK))
@@ -317,7 +323,7 @@ Done:
 	return error;
 }
 
-static u32 ali1563_func(struct i2c_adapter * a)
+static u32 ali1563_func(struct i2c_adapter *a)
 {
 	return I2C_FUNC_SMBUS_QUICK | I2C_FUNC_SMBUS_BYTE |
 	    I2C_FUNC_SMBUS_BYTE_DATA | I2C_FUNC_SMBUS_WORD_DATA |
@@ -329,13 +335,13 @@ static int ali1563_setup(struct pci_dev *dev)
 {
 	u16 ctrl;
 
-	pci_read_config_word(dev,ALI1563_SMBBA,&ctrl);
+	pci_read_config_word(dev, ALI1563_SMBBA, &ctrl);
 
 	/* SMB I/O Base in high 12 bits and must be aligned with the
 	 * size of the I/O space. */
 	ali1563_smba = ctrl & ~(ALI1563_SMB_IOSIZE - 1);
 	if (!ali1563_smba) {
-		dev_warn(&dev->dev,"ali1563_smba Uninitialized\n");
+		dev_warn(&dev->dev, "ali1563_smba Uninitialized\n");
 		goto Err;
 	}
 
@@ -350,8 +356,8 @@ static int ali1563_setup(struct pci_dev *dev)
 				      ctrl | ALI1563_SMB_IOEN);
 		pci_read_config_word(dev, ALI1563_SMBBA, &ctrl);
 		if (!(ctrl & ALI1563_SMB_IOEN)) {
-			dev_err(&dev->dev, "I/O space still not enabled, "
-				"giving up\n");
+			dev_err(&dev->dev,
+				"I/O space still not enabled, giving up\n");
 			goto Err;
 		}
 	}
@@ -375,7 +381,7 @@ Err:
 
 static void ali1563_shutdown(struct pci_dev *dev)
 {
-	release_region(ali1563_smba,ALI1563_SMB_IOSIZE);
+	release_region(ali1563_smba, ALI1563_SMB_IOSIZE);
 }
 
 static const struct i2c_algorithm ali1563_algorithm = {
@@ -394,12 +400,14 @@ static int ali1563_probe(struct pci_dev *dev,
 {
 	int error;
 
-	if ((error = ali1563_setup(dev)))
+	error = ali1563_setup(dev);
+	if (error)
 		goto exit;
 	ali1563_adapter.dev.parent = &dev->dev;
 	snprintf(ali1563_adapter.name, sizeof(ali1563_adapter.name),
 		 "SMBus ALi 1563 Adapter @ %04x", ali1563_smba);
-	if ((error = i2c_add_adapter(&ali1563_adapter)))
+	error = i2c_add_adapter(&ali1563_adapter);
+	if (error)
 		goto exit_shutdown;
 	return 0;
 
@@ -421,12 +429,12 @@ static const struct pci_device_id ali1563_id_table[] = {
 	{},
 };
 
-MODULE_DEVICE_TABLE (pci, ali1563_id_table);
+MODULE_DEVICE_TABLE(pci, ali1563_id_table);
 
 static struct pci_driver ali1563_pci_driver = {
- 	.name		= "ali1563_smbus",
+	.name		= "ali1563_smbus",
 	.id_table	= ali1563_id_table,
- 	.probe		= ali1563_probe,
+	.probe		= ali1563_probe,
 	.remove		= ali1563_remove,
 };
 
diff --git a/drivers/i2c/busses/i2c-bcm2835.c b/drivers/i2c/busses/i2c-bcm2835.c
index c60719577fc..214ff9700ef 100644
--- a/drivers/i2c/busses/i2c-bcm2835.c
+++ b/drivers/i2c/busses/i2c-bcm2835.c
@@ -225,10 +225,8 @@ static int bcm2835_i2c_probe(struct platform_device *pdev)
 	struct i2c_adapter *adap;
 
 	i2c_dev = devm_kzalloc(&pdev->dev, sizeof(*i2c_dev), GFP_KERNEL);
-	if (!i2c_dev) {
-		dev_err(&pdev->dev, "Cannot allocate i2c_dev\n");
+	if (!i2c_dev)
 		return -ENOMEM;
-	}
 	platform_set_drvdata(pdev, i2c_dev);
 	i2c_dev->dev = &pdev->dev;
 	init_completion(&i2c_dev->completion);
diff --git a/drivers/i2c/busses/i2c-bfin-twi.c b/drivers/i2c/busses/i2c-bfin-twi.c
index e6d5162b637..3e271e7558d 100644
--- a/drivers/i2c/busses/i2c-bfin-twi.c
+++ b/drivers/i2c/busses/i2c-bfin-twi.c
@@ -620,35 +620,27 @@ static int i2c_bfin_twi_probe(struct platform_device *pdev)
 	int rc;
 	unsigned int clkhilow;
 
-	iface = kzalloc(sizeof(struct bfin_twi_iface), GFP_KERNEL);
+	iface = devm_kzalloc(&pdev->dev, sizeof(struct bfin_twi_iface),
+			GFP_KERNEL);
 	if (!iface) {
 		dev_err(&pdev->dev, "Cannot allocate memory\n");
-		rc = -ENOMEM;
-		goto out_error_nomem;
+		return -ENOMEM;
 	}
 
 	spin_lock_init(&(iface->lock));
 
 	/* Find and map our resources */
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (res == NULL) {
-		dev_err(&pdev->dev, "Cannot get IORESOURCE_MEM\n");
-		rc = -ENOENT;
-		goto out_error_get_res;
-	}
-
-	iface->regs_base = ioremap(res->start, resource_size(res));
-	if (iface->regs_base == NULL) {
+	iface->regs_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(iface->regs_base)) {
 		dev_err(&pdev->dev, "Cannot map IO\n");
-		rc = -ENXIO;
-		goto out_error_ioremap;
+		return PTR_ERR(iface->regs_base);
 	}
 
 	iface->irq = platform_get_irq(pdev, 0);
 	if (iface->irq < 0) {
 		dev_err(&pdev->dev, "No IRQ specified\n");
-		rc = -ENOENT;
-		goto out_error_no_irq;
+		return -ENOENT;
 	}
 
 	p_adap = &iface->adap;
@@ -666,15 +658,15 @@ static int i2c_bfin_twi_probe(struct platform_device *pdev)
 			"i2c-bfin-twi");
 	if (rc) {
 		dev_err(&pdev->dev, "Can't setup pin mux!\n");
-		goto out_error_pin_mux;
+		return -EBUSY;
 	}
 
-	rc = request_irq(iface->irq, bfin_twi_interrupt_entry,
+	rc = devm_request_irq(&pdev->dev, iface->irq, bfin_twi_interrupt_entry,
 		0, pdev->name, iface);
 	if (rc) {
 		dev_err(&pdev->dev, "Can't get IRQ %d !\n", iface->irq);
 		rc = -ENODEV;
-		goto out_error_req_irq;
+		goto out_error;
 	}
 
 	/* Set TWI internal clock as 10MHz */
@@ -695,7 +687,7 @@ static int i2c_bfin_twi_probe(struct platform_device *pdev)
 	rc = i2c_add_numbered_adapter(p_adap);
 	if (rc < 0) {
 		dev_err(&pdev->dev, "Can't add i2c adapter!\n");
-		goto out_error_add_adapter;
+		goto out_error;
 	}
 
 	platform_set_drvdata(pdev, iface);
@@ -705,17 +697,8 @@ static int i2c_bfin_twi_probe(struct platform_device *pdev)
 
 	return 0;
 
-out_error_add_adapter:
-	free_irq(iface->irq, iface);
-out_error_req_irq:
-out_error_no_irq:
+out_error:
 	peripheral_free_list(dev_get_platdata(&pdev->dev));
-out_error_pin_mux:
-	iounmap(iface->regs_base);
-out_error_ioremap:
-out_error_get_res:
-	kfree(iface);
-out_error_nomem:
 	return rc;
 }
 
@@ -724,10 +707,7 @@ static int i2c_bfin_twi_remove(struct platform_device *pdev)
 	struct bfin_twi_iface *iface = platform_get_drvdata(pdev);
 
 	i2c_del_adapter(&(iface->adap));
-	free_irq(iface->irq, iface);
 	peripheral_free_list(dev_get_platdata(&pdev->dev));
-	iounmap(iface->regs_base);
-	kfree(iface);
 
 	return 0;
 }
diff --git a/drivers/i2c/busses/i2c-cros-ec-tunnel.c b/drivers/i2c/busses/i2c-cros-ec-tunnel.c
new file mode 100644
index 00000000000..8e7a71487bb
--- /dev/null
+++ b/drivers/i2c/busses/i2c-cros-ec-tunnel.c
@@ -0,0 +1,318 @@
+/*
+ *  Copyright (C) 2013 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.
+ *
+ * Expose an I2C passthrough to the ChromeOS EC.
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/mfd/cros_ec.h>
+#include <linux/mfd/cros_ec_commands.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+/**
+ * struct ec_i2c_device - Driver data for I2C tunnel
+ *
+ * @dev: Device node
+ * @adap: I2C adapter
+ * @ec: Pointer to EC device
+ * @remote_bus: The EC bus number we tunnel to on the other side.
+ * @request_buf: Buffer for transmitting data; we expect most transfers to fit.
+ * @response_buf: Buffer for receiving data; we expect most transfers to fit.
+ */
+
+struct ec_i2c_device {
+	struct device *dev;
+	struct i2c_adapter adap;
+	struct cros_ec_device *ec;
+
+	u16 remote_bus;
+
+	u8 request_buf[256];
+	u8 response_buf[256];
+};
+
+/**
+ * ec_i2c_count_message - Count bytes needed for ec_i2c_construct_message
+ *
+ * @i2c_msgs: The i2c messages to read
+ * @num: The number of i2c messages.
+ *
+ * Returns the number of bytes the messages will take up.
+ */
+static int ec_i2c_count_message(const struct i2c_msg i2c_msgs[], int num)
+{
+	int i;
+	int size;
+
+	size = sizeof(struct ec_params_i2c_passthru);
+	size += num * sizeof(struct ec_params_i2c_passthru_msg);
+	for (i = 0; i < num; i++)
+		if (!(i2c_msgs[i].flags & I2C_M_RD))
+			size += i2c_msgs[i].len;
+
+	return size;
+}
+
+/**
+ * ec_i2c_construct_message - construct a message to go to the EC
+ *
+ * This function effectively stuffs the standard i2c_msg format of Linux into
+ * a format that the EC understands.
+ *
+ * @buf: The buffer to fill.  We assume that the buffer is big enough.
+ * @i2c_msgs: The i2c messages to read.
+ * @num: The number of i2c messages.
+ * @bus_num: The remote bus number we want to talk to.
+ *
+ * Returns 0 or a negative error number.
+ */
+static int ec_i2c_construct_message(u8 *buf, const struct i2c_msg i2c_msgs[],
+				    int num, u16 bus_num)
+{
+	struct ec_params_i2c_passthru *params;
+	u8 *out_data;
+	int i;
+
+	out_data = buf + sizeof(struct ec_params_i2c_passthru) +
+		   num * sizeof(struct ec_params_i2c_passthru_msg);
+
+	params = (struct ec_params_i2c_passthru *)buf;
+	params->port = bus_num;
+	params->num_msgs = num;
+	for (i = 0; i < num; i++) {
+		const struct i2c_msg *i2c_msg = &i2c_msgs[i];
+		struct ec_params_i2c_passthru_msg *msg = &params->msg[i];
+
+		msg->len = i2c_msg->len;
+		msg->addr_flags = i2c_msg->addr;
+
+		if (i2c_msg->flags & I2C_M_TEN)
+			msg->addr_flags |= EC_I2C_FLAG_10BIT;
+
+		if (i2c_msg->flags & I2C_M_RD) {
+			msg->addr_flags |= EC_I2C_FLAG_READ;
+		} else {
+			memcpy(out_data, i2c_msg->buf, msg->len);
+			out_data += msg->len;
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * ec_i2c_count_response - Count bytes needed for ec_i2c_parse_response
+ *
+ * @i2c_msgs: The i2c messages to to fill up.
+ * @num: The number of i2c messages expected.
+ *
+ * Returns the number of response bytes expeced.
+ */
+static int ec_i2c_count_response(struct i2c_msg i2c_msgs[], int num)
+{
+	int size;
+	int i;
+
+	size = sizeof(struct ec_response_i2c_passthru);
+	for (i = 0; i < num; i++)
+		if (i2c_msgs[i].flags & I2C_M_RD)
+			size += i2c_msgs[i].len;
+
+	return size;
+}
+
+/**
+ * ec_i2c_parse_response - Parse a response from the EC
+ *
+ * We'll take the EC's response and copy it back into msgs.
+ *
+ * @buf: The buffer to parse.
+ * @i2c_msgs: The i2c messages to to fill up.
+ * @num: The number of i2c messages; will be modified to include the actual
+ *	 number received.
+ *
+ * Returns 0 or a negative error number.
+ */
+static int ec_i2c_parse_response(const u8 *buf, struct i2c_msg i2c_msgs[],
+				 int *num)
+{
+	const struct ec_response_i2c_passthru *resp;
+	const u8 *in_data;
+	int i;
+
+	in_data = buf + sizeof(struct ec_response_i2c_passthru);
+
+	resp = (const struct ec_response_i2c_passthru *)buf;
+	if (resp->i2c_status & EC_I2C_STATUS_TIMEOUT)
+		return -ETIMEDOUT;
+	else if (resp->i2c_status & EC_I2C_STATUS_ERROR)
+		return -EREMOTEIO;
+
+	/* Other side could send us back fewer messages, but not more */
+	if (resp->num_msgs > *num)
+		return -EPROTO;
+	*num = resp->num_msgs;
+
+	for (i = 0; i < *num; i++) {
+		struct i2c_msg *i2c_msg = &i2c_msgs[i];
+
+		if (i2c_msgs[i].flags & I2C_M_RD) {
+			memcpy(i2c_msg->buf, in_data, i2c_msg->len);
+			in_data += i2c_msg->len;
+		}
+	}
+
+	return 0;
+}
+
+static int ec_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg i2c_msgs[],
+		       int num)
+{
+	struct ec_i2c_device *bus = adap->algo_data;
+	struct device *dev = bus->dev;
+	const u16 bus_num = bus->remote_bus;
+	int request_len;
+	int response_len;
+	u8 *request = NULL;
+	u8 *response = NULL;
+	int result;
+
+	request_len = ec_i2c_count_message(i2c_msgs, num);
+	if (request_len < 0) {
+		dev_warn(dev, "Error constructing message %d\n", request_len);
+		result = request_len;
+		goto exit;
+	}
+	response_len = ec_i2c_count_response(i2c_msgs, num);
+	if (response_len < 0) {
+		/* Unexpected; no errors should come when NULL response */
+		dev_warn(dev, "Error preparing response %d\n", response_len);
+		result = response_len;
+		goto exit;
+	}
+
+	if (request_len <= ARRAY_SIZE(bus->request_buf)) {
+		request = bus->request_buf;
+	} else {
+		request = kzalloc(request_len, GFP_KERNEL);
+		if (request == NULL) {
+			result = -ENOMEM;
+			goto exit;
+		}
+	}
+	if (response_len <= ARRAY_SIZE(bus->response_buf)) {
+		response = bus->response_buf;
+	} else {
+		response = kzalloc(response_len, GFP_KERNEL);
+		if (response == NULL) {
+			result = -ENOMEM;
+			goto exit;
+		}
+	}
+
+	ec_i2c_construct_message(request, i2c_msgs, num, bus_num);
+	result = bus->ec->command_sendrecv(bus->ec, EC_CMD_I2C_PASSTHRU,
+					   request, request_len,
+					   response, response_len);
+	if (result)
+		goto exit;
+
+	result = ec_i2c_parse_response(response, i2c_msgs, &num);
+	if (result < 0)
+		goto exit;
+
+	/* Indicate success by saying how many messages were sent */
+	result = num;
+exit:
+	if (request != bus->request_buf)
+		kfree(request);
+	if (response != bus->response_buf)
+		kfree(response);
+
+	return result;
+}
+
+static u32 ec_i2c_functionality(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+}
+
+static const struct i2c_algorithm ec_i2c_algorithm = {
+	.master_xfer	= ec_i2c_xfer,
+	.functionality	= ec_i2c_functionality,
+};
+
+static int ec_i2c_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct cros_ec_device *ec = dev_get_drvdata(pdev->dev.parent);
+	struct device *dev = &pdev->dev;
+	struct ec_i2c_device *bus = NULL;
+	u32 remote_bus;
+	int err;
+
+	if (!ec->command_sendrecv) {
+		dev_err(dev, "Missing sendrecv\n");
+		return -EINVAL;
+	}
+
+	bus = devm_kzalloc(dev, sizeof(*bus), GFP_KERNEL);
+	if (bus == NULL)
+		return -ENOMEM;
+
+	err = of_property_read_u32(np, "google,remote-bus", &remote_bus);
+	if (err) {
+		dev_err(dev, "Couldn't read remote-bus property\n");
+		return err;
+	}
+	bus->remote_bus = remote_bus;
+
+	bus->ec = ec;
+	bus->dev = dev;
+
+	bus->adap.owner = THIS_MODULE;
+	strlcpy(bus->adap.name, "cros-ec-i2c-tunnel", sizeof(bus->adap.name));
+	bus->adap.algo = &ec_i2c_algorithm;
+	bus->adap.algo_data = bus;
+	bus->adap.dev.parent = &pdev->dev;
+	bus->adap.dev.of_node = np;
+
+	err = i2c_add_adapter(&bus->adap);
+	if (err) {
+		dev_err(dev, "cannot register i2c adapter\n");
+		return err;
+	}
+	platform_set_drvdata(pdev, bus);
+
+	return err;
+}
+
+static int ec_i2c_remove(struct platform_device *dev)
+{
+	struct ec_i2c_device *bus = platform_get_drvdata(dev);
+
+	i2c_del_adapter(&bus->adap);
+
+	return 0;
+}
+
+static struct platform_driver ec_i2c_tunnel_driver = {
+	.probe = ec_i2c_probe,
+	.remove = ec_i2c_remove,
+	.driver = {
+		.name = "cros-ec-i2c-tunnel",
+	},
+};
+
+module_platform_driver(ec_i2c_tunnel_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("EC I2C tunnel driver");
+MODULE_ALIAS("platform:cros-ec-i2c-tunnel");
diff --git a/drivers/i2c/busses/i2c-designware-core.c b/drivers/i2c/busses/i2c-designware-core.c
index 22e92c3d3d0..3c20e4bd6dd 100644
--- a/drivers/i2c/busses/i2c-designware-core.c
+++ b/drivers/i2c/busses/i2c-designware-core.c
@@ -422,6 +422,9 @@ static void i2c_dw_xfer_init(struct dw_i2c_dev *dev)
 	 */
 	dw_writel(dev, msgs[dev->msg_write_idx].addr | ic_tar, DW_IC_TAR);
 
+	/* enforce disabled interrupts (due to HW issues) */
+	i2c_dw_disable_int(dev);
+
 	/* Enable the adapter */
 	__i2c_dw_enable(dev, true);
 
diff --git a/drivers/i2c/busses/i2c-designware-pcidrv.c b/drivers/i2c/busses/i2c-designware-pcidrv.c
index 85056c22d21..3356f7ab9f7 100644
--- a/drivers/i2c/busses/i2c-designware-pcidrv.c
+++ b/drivers/i2c/busses/i2c-designware-pcidrv.c
@@ -56,6 +56,7 @@ enum dw_pci_ctl_id_t {
 	medfield_5,
 
 	baytrail,
+	haswell,
 };
 
 struct dw_scl_sda_cfg {
@@ -95,6 +96,15 @@ static struct dw_scl_sda_cfg byt_config = {
 	.sda_hold = 0x6,
 };
 
+/* Haswell HCNT/LCNT/SDA hold time */
+static struct dw_scl_sda_cfg hsw_config = {
+	.ss_hcnt = 0x01b0,
+	.fs_hcnt = 0x48,
+	.ss_lcnt = 0x01fb,
+	.fs_lcnt = 0xa0,
+	.sda_hold = 0x9,
+};
+
 static struct  dw_pci_controller  dw_pci_controllers[] = {
 	[moorestown_0] = {
 		.bus_num     = 0,
@@ -168,6 +178,15 @@ static struct  dw_pci_controller  dw_pci_controllers[] = {
 		.functionality = I2C_FUNC_10BIT_ADDR,
 		.scl_sda_cfg = &byt_config,
 	},
+	[haswell] = {
+		.bus_num = -1,
+		.bus_cfg = INTEL_MID_STD_CFG | DW_IC_CON_SPEED_FAST,
+		.tx_fifo_depth = 32,
+		.rx_fifo_depth = 32,
+		.clk_khz = 100000,
+		.functionality = I2C_FUNC_10BIT_ADDR,
+		.scl_sda_cfg = &hsw_config,
+	},
 };
 static struct i2c_algorithm i2c_dw_algo = {
 	.master_xfer	= i2c_dw_xfer,
@@ -328,6 +347,9 @@ static const struct pci_device_id i2_designware_pci_ids[] = {
 	{ PCI_VDEVICE(INTEL, 0x0F45), baytrail },
 	{ PCI_VDEVICE(INTEL, 0x0F46), baytrail },
 	{ PCI_VDEVICE(INTEL, 0x0F47), baytrail },
+	/* Haswell */
+	{ PCI_VDEVICE(INTEL, 0x9c61), haswell },
+	{ PCI_VDEVICE(INTEL, 0x9c62), haswell },
 	{ 0,}
 };
 MODULE_DEVICE_TABLE(pci, i2_designware_pci_ids);
diff --git a/drivers/i2c/busses/i2c-designware-platdrv.c b/drivers/i2c/busses/i2c-designware-platdrv.c
index 9c780261434..402ec3970fe 100644
--- a/drivers/i2c/busses/i2c-designware-platdrv.c
+++ b/drivers/i2c/busses/i2c-designware-platdrv.c
@@ -247,12 +247,13 @@ static const struct of_device_id dw_i2c_of_match[] = {
 MODULE_DEVICE_TABLE(of, dw_i2c_of_match);
 #endif
 
-#ifdef CONFIG_PM_SLEEP
+#ifdef CONFIG_PM
 static int dw_i2c_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct dw_i2c_dev *i_dev = platform_get_drvdata(pdev);
 
+	i2c_dw_disable(i_dev);
 	clk_disable_unprepare(i_dev->clk);
 
 	return 0;
@@ -268,13 +269,11 @@ static int dw_i2c_resume(struct device *dev)
 
 	return 0;
 }
-
-static SIMPLE_DEV_PM_OPS(dw_i2c_dev_pm_ops, dw_i2c_suspend, dw_i2c_resume);
-#define DW_I2C_DEV_PM_OPS	(&dw_i2c_dev_pm_ops)
-#else
-#define DW_I2C_DEV_PM_OPS	NULL
 #endif
 
+static UNIVERSAL_DEV_PM_OPS(dw_i2c_dev_pm_ops, dw_i2c_suspend,
+			    dw_i2c_resume, NULL);
+
 /* work with hotplug and coldplug */
 MODULE_ALIAS("platform:i2c_designware");
 
@@ -286,7 +285,7 @@ static struct platform_driver dw_i2c_driver = {
 		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(dw_i2c_of_match),
 		.acpi_match_table = ACPI_PTR(dw_i2c_acpi_match),
-		.pm	= DW_I2C_DEV_PM_OPS,
+		.pm	= &dw_i2c_dev_pm_ops,
 	},
 };
 
diff --git a/drivers/i2c/busses/i2c-diolan-u2c.c b/drivers/i2c/busses/i2c-diolan-u2c.c
index 721f7ebf9a3..b19a310bf9b 100644
--- a/drivers/i2c/busses/i2c-diolan-u2c.c
+++ b/drivers/i2c/busses/i2c-diolan-u2c.c
@@ -455,7 +455,6 @@ static int diolan_u2c_probe(struct usb_interface *interface,
 	/* allocate memory for our device state and initialize it */
 	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
 	if (dev == NULL) {
-		dev_err(&interface->dev, "no memory for device state\n");
 		ret = -ENOMEM;
 		goto error;
 	}
diff --git a/drivers/i2c/busses/i2c-efm32.c b/drivers/i2c/busses/i2c-efm32.c
index 777ed409a24..f7eccd682de 100644
--- a/drivers/i2c/busses/i2c-efm32.c
+++ b/drivers/i2c/busses/i2c-efm32.c
@@ -320,10 +320,8 @@ static int efm32_i2c_probe(struct platform_device *pdev)
 		return -EINVAL;
 
 	ddata = devm_kzalloc(&pdev->dev, sizeof(*ddata), GFP_KERNEL);
-	if (!ddata) {
-		dev_dbg(&pdev->dev, "failed to allocate private data\n");
+	if (!ddata)
 		return -ENOMEM;
-	}
 	platform_set_drvdata(pdev, ddata);
 
 	init_completion(&ddata->done);
diff --git a/drivers/i2c/busses/i2c-eg20t.c b/drivers/i2c/busses/i2c-eg20t.c
index ff775ac29e4..a44ea13d143 100644
--- a/drivers/i2c/busses/i2c-eg20t.c
+++ b/drivers/i2c/busses/i2c-eg20t.c
@@ -751,10 +751,8 @@ static int pch_i2c_probe(struct pci_dev *pdev,
 	pch_pci_dbg(pdev, "Entered.\n");
 
 	adap_info = kzalloc((sizeof(struct adapter_info)), GFP_KERNEL);
-	if (adap_info == NULL) {
-		pch_pci_err(pdev, "Memory allocation FAILED\n");
+	if (adap_info == NULL)
 		return -ENOMEM;
-	}
 
 	ret = pci_enable_device(pdev);
 	if (ret) {
diff --git a/drivers/i2c/busses/i2c-exynos5.c b/drivers/i2c/busses/i2c-exynos5.c
index 00af0a0a336..63d22920285 100644
--- a/drivers/i2c/busses/i2c-exynos5.c
+++ b/drivers/i2c/busses/i2c-exynos5.c
@@ -76,12 +76,6 @@
 #define HSI2C_RXFIFO_TRIGGER_LEVEL(x)		((x) << 4)
 #define HSI2C_TXFIFO_TRIGGER_LEVEL(x)		((x) << 16)
 
-/* As per user manual FIFO max depth is 64bytes */
-#define HSI2C_FIFO_MAX				0x40
-/* default trigger levels for Tx and Rx FIFOs */
-#define HSI2C_DEF_TXFIFO_LVL			(HSI2C_FIFO_MAX - 0x30)
-#define HSI2C_DEF_RXFIFO_LVL			(HSI2C_FIFO_MAX - 0x10)
-
 /* I2C_TRAILING_CTL Register bits */
 #define HSI2C_TRAILING_COUNT			(0xf)
 
@@ -183,14 +177,54 @@ struct exynos5_i2c {
 	 * 2. Fast speed upto 1Mbps
 	 */
 	int			speed_mode;
+
+	/* Version of HS-I2C Hardware */
+	struct exynos_hsi2c_variant	*variant;
+};
+
+/**
+ * struct exynos_hsi2c_variant - platform specific HSI2C driver data
+ * @fifo_depth: the fifo depth supported by the HSI2C module
+ *
+ * Specifies platform specific configuration of HSI2C module.
+ * Note: A structure for driver specific platform data is used for future
+ * expansion of its usage.
+ */
+struct exynos_hsi2c_variant {
+	unsigned int	fifo_depth;
+};
+
+static const struct exynos_hsi2c_variant exynos5250_hsi2c_data = {
+	.fifo_depth	= 64,
+};
+
+static const struct exynos_hsi2c_variant exynos5260_hsi2c_data = {
+	.fifo_depth	= 16,
 };
 
 static const struct of_device_id exynos5_i2c_match[] = {
-	{ .compatible = "samsung,exynos5-hsi2c" },
-	{},
+	{
+		.compatible = "samsung,exynos5-hsi2c",
+		.data = &exynos5250_hsi2c_data
+	}, {
+		.compatible = "samsung,exynos5250-hsi2c",
+		.data = &exynos5250_hsi2c_data
+	}, {
+		.compatible = "samsung,exynos5260-hsi2c",
+		.data = &exynos5260_hsi2c_data
+	}, {},
 };
 MODULE_DEVICE_TABLE(of, exynos5_i2c_match);
 
+static inline struct exynos_hsi2c_variant *exynos5_i2c_get_variant
+					(struct platform_device *pdev)
+{
+	const struct of_device_id *match;
+
+	match = of_match_node(exynos5_i2c_match, pdev->dev.of_node);
+	return (struct exynos_hsi2c_variant *)match->data;
+}
+
 static void exynos5_i2c_clr_pend_irq(struct exynos5_i2c *i2c)
 {
 	writel(readl(i2c->regs + HSI2C_INT_STATUS),
@@ -415,7 +449,7 @@ static irqreturn_t exynos5_i2c_irq(int irqno, void *dev_id)
 		fifo_status = readl(i2c->regs + HSI2C_FIFO_STATUS);
 		fifo_level = HSI2C_TX_FIFO_LVL(fifo_status);
 
-		len = HSI2C_FIFO_MAX - fifo_level;
+		len = i2c->variant->fifo_depth - fifo_level;
 		if (len > (i2c->msg->len - i2c->msg_ptr))
 			len = i2c->msg->len - i2c->msg_ptr;
 
@@ -483,6 +517,7 @@ static void exynos5_i2c_message_start(struct exynos5_i2c *i2c, int stop)
 	u32 i2c_auto_conf = 0;
 	u32 fifo_ctl;
 	unsigned long flags;
+	unsigned short trig_lvl;
 
 	i2c_ctl = readl(i2c->regs + HSI2C_CTL);
 	i2c_ctl &= ~(HSI2C_TXCHON | HSI2C_RXCHON);
@@ -493,13 +528,19 @@ static void exynos5_i2c_message_start(struct exynos5_i2c *i2c, int stop)
 
 		i2c_auto_conf = HSI2C_READ_WRITE;
 
-		fifo_ctl |= HSI2C_RXFIFO_TRIGGER_LEVEL(HSI2C_DEF_TXFIFO_LVL);
+		trig_lvl = (i2c->msg->len > i2c->variant->fifo_depth) ?
+			(i2c->variant->fifo_depth * 3 / 4) : i2c->msg->len;
+		fifo_ctl |= HSI2C_RXFIFO_TRIGGER_LEVEL(trig_lvl);
+
 		int_en |= (HSI2C_INT_RX_ALMOSTFULL_EN |
 			HSI2C_INT_TRAILING_EN);
 	} else {
 		i2c_ctl |= HSI2C_TXCHON;
 
-		fifo_ctl |= HSI2C_TXFIFO_TRIGGER_LEVEL(HSI2C_DEF_RXFIFO_LVL);
+		trig_lvl = (i2c->msg->len > i2c->variant->fifo_depth) ?
+			(i2c->variant->fifo_depth * 1 / 4) : i2c->msg->len;
+		fifo_ctl |= HSI2C_TXFIFO_TRIGGER_LEVEL(trig_lvl);
+
 		int_en |= HSI2C_INT_TX_ALMOSTEMPTY_EN;
 	}
 
@@ -621,10 +662,8 @@ static int exynos5_i2c_probe(struct platform_device *pdev)
 	int ret;
 
 	i2c = devm_kzalloc(&pdev->dev, sizeof(struct exynos5_i2c), GFP_KERNEL);
-	if (!i2c) {
-		dev_err(&pdev->dev, "no memory for state\n");
+	if (!i2c)
 		return -ENOMEM;
-	}
 
 	if (of_property_read_u32(np, "clock-frequency", &op_clock)) {
 		i2c->speed_mode = HSI2C_FAST_SPD;
@@ -691,7 +730,9 @@ static int exynos5_i2c_probe(struct platform_device *pdev)
 	if (ret)
 		goto err_clk;
 
-	exynos5_i2c_init(i2c);
+	i2c->variant = exynos5_i2c_get_variant(pdev);
+
+	exynos5_i2c_reset(i2c);
 
 	ret = i2c_add_adapter(&i2c->adap);
 	if (ret < 0) {
diff --git a/drivers/i2c/busses/i2c-gpio.c b/drivers/i2c/busses/i2c-gpio.c
index 02d2d4abb9d..71a45b210a2 100644
--- a/drivers/i2c/busses/i2c-gpio.c
+++ b/drivers/i2c/busses/i2c-gpio.c
@@ -147,24 +147,22 @@ static int i2c_gpio_probe(struct platform_device *pdev)
 		scl_pin = pdata->scl_pin;
 	}
 
-	ret = gpio_request(sda_pin, "sda");
+	ret = devm_gpio_request(&pdev->dev, sda_pin, "sda");
 	if (ret) {
 		if (ret == -EINVAL)
 			ret = -EPROBE_DEFER;	/* Try again later */
-		goto err_request_sda;
+		return ret;
 	}
-	ret = gpio_request(scl_pin, "scl");
+	ret = devm_gpio_request(&pdev->dev, scl_pin, "scl");
 	if (ret) {
 		if (ret == -EINVAL)
 			ret = -EPROBE_DEFER;	/* Try again later */
-		goto err_request_scl;
+		return ret;
 	}
 
 	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
-	if (!priv) {
-		ret = -ENOMEM;
-		goto err_add_bus;
-	}
+	if (!priv)
+		return -ENOMEM;
 	adap = &priv->adap;
 	bit_data = &priv->bit_data;
 	pdata = &priv->pdata;
@@ -225,7 +223,7 @@ static int i2c_gpio_probe(struct platform_device *pdev)
 	adap->nr = pdev->id;
 	ret = i2c_bit_add_numbered_bus(adap);
 	if (ret)
-		goto err_add_bus;
+		return ret;
 
 	platform_set_drvdata(pdev, priv);
 
@@ -235,13 +233,6 @@ static int i2c_gpio_probe(struct platform_device *pdev)
 		 ? ", no clock stretching" : "");
 
 	return 0;
-
-err_add_bus:
-	gpio_free(scl_pin);
-err_request_scl:
-	gpio_free(sda_pin);
-err_request_sda:
-	return ret;
 }
 
 static int i2c_gpio_remove(struct platform_device *pdev)
@@ -255,8 +246,6 @@ static int i2c_gpio_remove(struct platform_device *pdev)
 	pdata = &priv->pdata;
 
 	i2c_del_adapter(adap);
-	gpio_free(pdata->scl_pin);
-	gpio_free(pdata->sda_pin);
 
 	return 0;
 }
diff --git a/drivers/i2c/busses/i2c-imx.c b/drivers/i2c/busses/i2c-imx.c
index db895fb22e6..aa8bc146718 100644
--- a/drivers/i2c/busses/i2c-imx.c
+++ b/drivers/i2c/busses/i2c-imx.c
@@ -183,6 +183,8 @@ struct imx_i2c_struct {
 	unsigned int 		disable_delay;
 	int			stopped;
 	unsigned int		ifdr; /* IMX_I2C_IFDR */
+	unsigned int		cur_clk;
+	unsigned int		bitrate;
 	const struct imx_i2c_hwdata	*hwdata;
 };
 
@@ -305,6 +307,48 @@ static int i2c_imx_acked(struct imx_i2c_struct *i2c_imx)
 	return 0;
 }
 
+static void i2c_imx_set_clk(struct imx_i2c_struct *i2c_imx)
+{
+	struct imx_i2c_clk_pair *i2c_clk_div = i2c_imx->hwdata->clk_div;
+	unsigned int i2c_clk_rate;
+	unsigned int div;
+	int i;
+
+	/* Divider value calculation */
+	i2c_clk_rate = clk_get_rate(i2c_imx->clk);
+	if (i2c_imx->cur_clk == i2c_clk_rate)
+		return;
+	else
+		i2c_imx->cur_clk = i2c_clk_rate;
+
+	div = (i2c_clk_rate + i2c_imx->bitrate - 1) / i2c_imx->bitrate;
+	if (div < i2c_clk_div[0].div)
+		i = 0;
+	else if (div > i2c_clk_div[i2c_imx->hwdata->ndivs - 1].div)
+		i = i2c_imx->hwdata->ndivs - 1;
+	else
+		for (i = 0; i2c_clk_div[i].div < div; i++);
+
+	/* Store divider value */
+	i2c_imx->ifdr = i2c_clk_div[i].val;
+
+	/*
+	 * There dummy delay is calculated.
+	 * It should be about one I2C clock period long.
+	 * This delay is used in I2C bus disable function
+	 * to fix chip hardware bug.
+	 */
+	i2c_imx->disable_delay = (500000U * i2c_clk_div[i].div
+		+ (i2c_clk_rate / 2) - 1) / (i2c_clk_rate / 2);
+
+#ifdef CONFIG_I2C_DEBUG_BUS
+	dev_dbg(&i2c_imx->adapter.dev, "I2C_CLK=%d, REQ DIV=%d\n",
+		i2c_clk_rate, div);
+	dev_dbg(&i2c_imx->adapter.dev, "IFDR[IC]=0x%x, REAL DIV=%d\n",
+		i2c_clk_div[i].val, i2c_clk_div[i].div);
+#endif
+}
+
 static int i2c_imx_start(struct imx_i2c_struct *i2c_imx)
 {
 	unsigned int temp = 0;
@@ -312,6 +356,8 @@ static int i2c_imx_start(struct imx_i2c_struct *i2c_imx)
 
 	dev_dbg(&i2c_imx->adapter.dev, "<%s>\n", __func__);
 
+	i2c_imx_set_clk(i2c_imx);
+
 	result = clk_prepare_enable(i2c_imx->clk);
 	if (result)
 		return result;
@@ -367,45 +413,6 @@ static void i2c_imx_stop(struct imx_i2c_struct *i2c_imx)
 	clk_disable_unprepare(i2c_imx->clk);
 }
 
-static void i2c_imx_set_clk(struct imx_i2c_struct *i2c_imx,
-							unsigned int rate)
-{
-	struct imx_i2c_clk_pair *i2c_clk_div = i2c_imx->hwdata->clk_div;
-	unsigned int i2c_clk_rate;
-	unsigned int div;
-	int i;
-
-	/* Divider value calculation */
-	i2c_clk_rate = clk_get_rate(i2c_imx->clk);
-	div = (i2c_clk_rate + rate - 1) / rate;
-	if (div < i2c_clk_div[0].div)
-		i = 0;
-	else if (div > i2c_clk_div[i2c_imx->hwdata->ndivs - 1].div)
-		i = i2c_imx->hwdata->ndivs - 1;
-	else
-		for (i = 0; i2c_clk_div[i].div < div; i++);
-
-	/* Store divider value */
-	i2c_imx->ifdr = i2c_clk_div[i].val;
-
-	/*
-	 * There dummy delay is calculated.
-	 * It should be about one I2C clock period long.
-	 * This delay is used in I2C bus disable function
-	 * to fix chip hardware bug.
-	 */
-	i2c_imx->disable_delay = (500000U * i2c_clk_div[i].div
-		+ (i2c_clk_rate / 2) - 1) / (i2c_clk_rate / 2);
-
-	/* dev_dbg() can't be used, because adapter is not yet registered */
-#ifdef CONFIG_I2C_DEBUG_BUS
-	dev_dbg(&i2c_imx->adapter.dev, "<%s> I2C_CLK=%d, REQ DIV=%d\n",
-		__func__, i2c_clk_rate, div);
-	dev_dbg(&i2c_imx->adapter.dev, "<%s> IFDR[IC]=0x%x, REAL DIV=%d\n",
-		__func__, i2c_clk_div[i].val, i2c_clk_div[i].div);
-#endif
-}
-
 static irqreturn_t i2c_imx_isr(int irq, void *dev_id)
 {
 	struct imx_i2c_struct *i2c_imx = dev_id;
@@ -458,10 +465,11 @@ static int i2c_imx_write(struct imx_i2c_struct *i2c_imx, struct i2c_msg *msgs)
 	return 0;
 }
 
-static int i2c_imx_read(struct imx_i2c_struct *i2c_imx, struct i2c_msg *msgs)
+static int i2c_imx_read(struct imx_i2c_struct *i2c_imx, struct i2c_msg *msgs, bool is_lastmsg)
 {
 	int i, result;
 	unsigned int temp;
+	int block_data = msgs->flags & I2C_M_RECV_LEN;
 
 	dev_dbg(&i2c_imx->adapter.dev,
 		"<%s> write slave address: addr=0x%x\n",
@@ -481,7 +489,12 @@ static int i2c_imx_read(struct imx_i2c_struct *i2c_imx, struct i2c_msg *msgs)
 	/* setup bus to read data */
 	temp = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2CR);
 	temp &= ~I2CR_MTX;
-	if (msgs->len - 1)
+
+	/*
+	 * Reset the I2CR_TXAK flag initially for SMBus block read since the
+	 * length is unknown
+	 */
+	if ((msgs->len - 1) || block_data)
 		temp &= ~I2CR_TXAK;
 	imx_i2c_write_reg(temp, i2c_imx, IMX_I2C_I2CR);
 	imx_i2c_read_reg(i2c_imx, IMX_I2C_I2DR); /* dummy read */
@@ -490,19 +503,49 @@ static int i2c_imx_read(struct imx_i2c_struct *i2c_imx, struct i2c_msg *msgs)
 
 	/* read data */
 	for (i = 0; i < msgs->len; i++) {
+		u8 len = 0;
 		result = i2c_imx_trx_complete(i2c_imx);
 		if (result)
 			return result;
-		if (i == (msgs->len - 1)) {
-			/* It must generate STOP before read I2DR to prevent
-			   controller from generating another clock cycle */
+		/*
+		 * First byte is the length of remaining packet
+		 * in the SMBus block data read. Add it to
+		 * msgs->len.
+		 */
+		if ((!i) && block_data) {
+			len = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2DR);
+			if ((len == 0) || (len > I2C_SMBUS_BLOCK_MAX))
+				return -EPROTO;
 			dev_dbg(&i2c_imx->adapter.dev,
-				"<%s> clear MSTA\n", __func__);
-			temp = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2CR);
-			temp &= ~(I2CR_MSTA | I2CR_MTX);
-			imx_i2c_write_reg(temp, i2c_imx, IMX_I2C_I2CR);
-			i2c_imx_bus_busy(i2c_imx, 0);
-			i2c_imx->stopped = 1;
+				"<%s> read length: 0x%X\n",
+				__func__, len);
+			msgs->len += len;
+		}
+		if (i == (msgs->len - 1)) {
+			if (is_lastmsg) {
+				/*
+				 * It must generate STOP before read I2DR to prevent
+				 * controller from generating another clock cycle
+				 */
+				dev_dbg(&i2c_imx->adapter.dev,
+					"<%s> clear MSTA\n", __func__);
+				temp = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2CR);
+				temp &= ~(I2CR_MSTA | I2CR_MTX);
+				imx_i2c_write_reg(temp, i2c_imx, IMX_I2C_I2CR);
+				i2c_imx_bus_busy(i2c_imx, 0);
+				i2c_imx->stopped = 1;
+			} else {
+				/*
+				 * For i2c master receiver repeat restart operation like:
+				 * read -> repeat MSTA -> read/write
+				 * The controller must set MTX before read the last byte in
+				 * the first read operation, otherwise the first read cost
+				 * one extra clock cycle.
+				 */
+				temp = readb(i2c_imx->base + IMX_I2C_I2CR);
+				temp |= I2CR_MTX;
+				writeb(temp, i2c_imx->base + IMX_I2C_I2CR);
+			}
 		} else if (i == (msgs->len - 2)) {
 			dev_dbg(&i2c_imx->adapter.dev,
 				"<%s> set TXAK\n", __func__);
@@ -510,7 +553,10 @@ static int i2c_imx_read(struct imx_i2c_struct *i2c_imx, struct i2c_msg *msgs)
 			temp |= I2CR_TXAK;
 			imx_i2c_write_reg(temp, i2c_imx, IMX_I2C_I2CR);
 		}
-		msgs->buf[i] = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2DR);
+		if ((!i) && block_data)
+			msgs->buf[0] = len;
+		else
+			msgs->buf[i] =  imx_i2c_read_reg(i2c_imx, IMX_I2C_I2DR);
 		dev_dbg(&i2c_imx->adapter.dev,
 			"<%s> read byte: B%d=0x%X\n",
 			__func__, i, msgs->buf[i]);
@@ -523,6 +569,7 @@ static int i2c_imx_xfer(struct i2c_adapter *adapter,
 {
 	unsigned int i, temp;
 	int result;
+	bool is_lastmsg = false;
 	struct imx_i2c_struct *i2c_imx = i2c_get_adapdata(adapter);
 
 	dev_dbg(&i2c_imx->adapter.dev, "<%s>\n", __func__);
@@ -534,6 +581,9 @@ static int i2c_imx_xfer(struct i2c_adapter *adapter,
 
 	/* read/write data */
 	for (i = 0; i < num; i++) {
+		if (i == num - 1)
+			is_lastmsg = true;
+
 		if (i) {
 			dev_dbg(&i2c_imx->adapter.dev,
 				"<%s> repeated start\n", __func__);
@@ -564,7 +614,7 @@ static int i2c_imx_xfer(struct i2c_adapter *adapter,
 			(temp & I2SR_RXAK ? 1 : 0));
 #endif
 		if (msgs[i].flags & I2C_M_RD)
-			result = i2c_imx_read(i2c_imx, &msgs[i]);
+			result = i2c_imx_read(i2c_imx, &msgs[i], is_lastmsg);
 		else
 			result = i2c_imx_write(i2c_imx, &msgs[i]);
 		if (result)
@@ -583,7 +633,8 @@ fail0:
 
 static u32 i2c_imx_func(struct i2c_adapter *adapter)
 {
-	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL
+		| I2C_FUNC_SMBUS_READ_BLOCK_DATA;
 }
 
 static struct i2c_algorithm i2c_imx_algo = {
@@ -600,7 +651,6 @@ static int i2c_imx_probe(struct platform_device *pdev)
 	struct imxi2c_platform_data *pdata = dev_get_platdata(&pdev->dev);
 	void __iomem *base;
 	int irq, ret;
-	u32 bitrate;
 
 	dev_dbg(&pdev->dev, "<%s>\n", __func__);
 
@@ -617,10 +667,8 @@ static int i2c_imx_probe(struct platform_device *pdev)
 
 	i2c_imx = devm_kzalloc(&pdev->dev, sizeof(struct imx_i2c_struct),
 				GFP_KERNEL);
-	if (!i2c_imx) {
-		dev_err(&pdev->dev, "can't allocate interface\n");
+	if (!i2c_imx)
 		return -ENOMEM;
-	}
 
 	if (of_id)
 		i2c_imx->hwdata = of_id->data;
@@ -664,12 +712,11 @@ static int i2c_imx_probe(struct platform_device *pdev)
 	i2c_set_adapdata(&i2c_imx->adapter, i2c_imx);
 
 	/* Set up clock divider */
-	bitrate = IMX_I2C_BIT_RATE;
+	i2c_imx->bitrate = IMX_I2C_BIT_RATE;
 	ret = of_property_read_u32(pdev->dev.of_node,
-				   "clock-frequency", &bitrate);
+				   "clock-frequency", &i2c_imx->bitrate);
 	if (ret < 0 && pdata && pdata->bitrate)
-		bitrate = pdata->bitrate;
-	i2c_imx_set_clk(i2c_imx, bitrate);
+		i2c_imx->bitrate = pdata->bitrate;
 
 	/* Set up chip registers to defaults */
 	imx_i2c_write_reg(i2c_imx->hwdata->i2cr_ien_opcode ^ I2CR_IEN,
diff --git a/drivers/i2c/busses/i2c-mpc.c b/drivers/i2c/busses/i2c-mpc.c
index f5391633b53..6a32aa095f8 100644
--- a/drivers/i2c/busses/i2c-mpc.c
+++ b/drivers/i2c/busses/i2c-mpc.c
@@ -115,7 +115,7 @@ static void mpc_i2c_fixup(struct mpc_i2c *i2c)
 	for (k = 9; k; k--) {
 		writeccr(i2c, 0);
 		writeccr(i2c, CCR_MSTA | CCR_MTX | CCR_MEN);
-		udelay(delay_val);
+		readb(i2c->base + MPC_I2C_DR);
 		writeccr(i2c, CCR_MEN);
 		udelay(delay_val << 1);
 	}
diff --git a/drivers/i2c/busses/i2c-mv64xxx.c b/drivers/i2c/busses/i2c-mv64xxx.c
index 540ea692bf7..9f4b775e2e3 100644
--- a/drivers/i2c/busses/i2c-mv64xxx.c
+++ b/drivers/i2c/busses/i2c-mv64xxx.c
@@ -681,7 +681,7 @@ static const struct i2c_algorithm mv64xxx_i2c_algo = {
  *****************************************************************************
  */
 static const struct of_device_id mv64xxx_i2c_of_match_table[] = {
-	{ .compatible = "allwinner,sun4i-i2c", .data = &mv64xxx_i2c_regs_sun4i},
+	{ .compatible = "allwinner,sun4i-a10-i2c", .data = &mv64xxx_i2c_regs_sun4i},
 	{ .compatible = "allwinner,sun6i-a31-i2c", .data = &mv64xxx_i2c_regs_sun4i},
 	{ .compatible = "marvell,mv64xxx-i2c", .data = &mv64xxx_i2c_regs_mv64xxx},
 	{ .compatible = "marvell,mv78230-i2c", .data = &mv64xxx_i2c_regs_mv64xxx},
diff --git a/drivers/i2c/busses/i2c-nomadik.c b/drivers/i2c/busses/i2c-nomadik.c
index 28cbe1b2a2e..0e55d85fd4e 100644
--- a/drivers/i2c/busses/i2c-nomadik.c
+++ b/drivers/i2c/busses/i2c-nomadik.c
@@ -879,19 +879,19 @@ static irqreturn_t i2c_irq_handler(int irq, void *arg)
 #ifdef CONFIG_PM_SLEEP
 static int nmk_i2c_suspend_late(struct device *dev)
 {
-	pinctrl_pm_select_sleep_state(dev);
+	int ret;
 
+	ret = pm_runtime_force_suspend(dev);
+	if (ret)
+		return ret;
+
+	pinctrl_pm_select_sleep_state(dev);
 	return 0;
 }
 
 static int nmk_i2c_resume_early(struct device *dev)
 {
-	/* First go to the default state */
-	pinctrl_pm_select_default_state(dev);
-	/* Then let's idle the pins until the next transfer happens */
-	pinctrl_pm_select_idle_state(dev);
-
-	return 0;
+	return pm_runtime_force_resume(dev);
 }
 #endif
 
@@ -999,7 +999,7 @@ static int nmk_i2c_probe(struct amba_device *adev, const struct amba_id *id)
 
 	dev->virtbase = devm_ioremap(&adev->dev, adev->res.start,
 				resource_size(&adev->res));
-	if (IS_ERR(dev->virtbase)) {
+	if (!dev->virtbase) {
 		ret = -ENOMEM;
 		goto err_no_mem;
 	}
diff --git a/drivers/i2c/busses/i2c-nuc900.c b/drivers/i2c/busses/i2c-nuc900.c
deleted file mode 100644
index 36394d737fa..00000000000
--- a/drivers/i2c/busses/i2c-nuc900.c
+++ /dev/null
@@ -1,709 +0,0 @@
-/*
- * linux/drivers/i2c/busses/i2c-nuc900.c
- *
- * Copyright (c) 2010 Nuvoton technology corporation.
- *
- * This driver based on S3C2410 I2C driver of Ben Dooks <ben-Y5A6D6n0/KfQXOPxS62xeg@public.gmane.org>.
- * Written by Wan ZongShun <mcuos.com-Re5JQEeQqe8AvxtiuMwx3w@public.gmane.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;version 2 of the License.
- *
- */
-
-#include <linux/kernel.h>
-#include <linux/module.h>
-
-#include <linux/i2c.h>
-#include <linux/init.h>
-#include <linux/time.h>
-#include <linux/interrupt.h>
-#include <linux/delay.h>
-#include <linux/errno.h>
-#include <linux/err.h>
-#include <linux/platform_device.h>
-#include <linux/clk.h>
-#include <linux/cpufreq.h>
-#include <linux/slab.h>
-#include <linux/io.h>
-
-#include <mach/mfp.h>
-#include <linux/platform_data/i2c-nuc900.h>
-
-/* nuc900 i2c registers offset */
-
-#define CSR		0x00
-#define DIVIDER		0x04
-#define CMDR		0x08
-#define SWR		0x0C
-#define RXR		0x10
-#define TXR		0x14
-
-/* nuc900 i2c CSR register bits */
-
-#define IRQEN		0x003
-#define I2CBUSY		0x400
-#define I2CSTART	0x018
-#define IRQFLAG		0x004
-#define ARBIT_LOST	0x200
-#define SLAVE_ACK	0x800
-
-/* nuc900 i2c CMDR register bits */
-
-#define I2C_CMD_START	0x10
-#define I2C_CMD_STOP	0x08
-#define I2C_CMD_READ	0x04
-#define I2C_CMD_WRITE	0x02
-#define I2C_CMD_NACK	0x01
-
-/* i2c controller state */
-
-enum nuc900_i2c_state {
-	STATE_IDLE,
-	STATE_START,
-	STATE_READ,
-	STATE_WRITE,
-	STATE_STOP
-};
-
-/* i2c controller private data */
-
-struct nuc900_i2c {
-	spinlock_t		lock;
-	wait_queue_head_t	wait;
-
-	struct i2c_msg		*msg;
-	unsigned int		msg_num;
-	unsigned int		msg_idx;
-	unsigned int		msg_ptr;
-	unsigned int		irq;
-
-	enum nuc900_i2c_state	state;
-
-	void __iomem		*regs;
-	struct clk		*clk;
-	struct device		*dev;
-	struct resource		*ioarea;
-	struct i2c_adapter	adap;
-};
-
-/* nuc900_i2c_master_complete
- *
- * complete the message and wake up the caller, using the given return code,
- * or zero to mean ok.
-*/
-
-static inline void nuc900_i2c_master_complete(struct nuc900_i2c *i2c, int ret)
-{
-	dev_dbg(i2c->dev, "master_complete %d\n", ret);
-
-	i2c->msg_ptr = 0;
-	i2c->msg = NULL;
-	i2c->msg_idx++;
-	i2c->msg_num = 0;
-	if (ret)
-		i2c->msg_idx = ret;
-
-	wake_up(&i2c->wait);
-}
-
-/* irq enable/disable functions */
-
-static inline void nuc900_i2c_disable_irq(struct nuc900_i2c *i2c)
-{
-	unsigned long tmp;
-
-	tmp = readl(i2c->regs + CSR);
-	writel(tmp & ~IRQEN, i2c->regs + CSR);
-}
-
-static inline void nuc900_i2c_enable_irq(struct nuc900_i2c *i2c)
-{
-	unsigned long tmp;
-
-	tmp = readl(i2c->regs + CSR);
-	writel(tmp | IRQEN, i2c->regs + CSR);
-}
-
-
-/* nuc900_i2c_message_start
- *
- * put the start of a message onto the bus
-*/
-
-static void nuc900_i2c_message_start(struct nuc900_i2c *i2c,
-				      struct i2c_msg *msg)
-{
-	unsigned int addr = (msg->addr & 0x7f) << 1;
-
-	if (msg->flags & I2C_M_RD)
-		addr |= 0x1;
-	writel(addr & 0xff, i2c->regs + TXR);
-	writel(I2C_CMD_START | I2C_CMD_WRITE, i2c->regs + CMDR);
-}
-
-static inline void nuc900_i2c_stop(struct nuc900_i2c *i2c, int ret)
-{
-
-	dev_dbg(i2c->dev, "STOP\n");
-
-	/* stop the transfer */
-	i2c->state = STATE_STOP;
-	writel(I2C_CMD_STOP, i2c->regs + CMDR);
-
-	nuc900_i2c_master_complete(i2c, ret);
-	nuc900_i2c_disable_irq(i2c);
-}
-
-/* helper functions to determine the current state in the set of
- * messages we are sending
-*/
-
-/* is_lastmsg()
- *
- * returns TRUE if the current message is the last in the set
-*/
-
-static inline int is_lastmsg(struct nuc900_i2c *i2c)
-{
-	return i2c->msg_idx >= (i2c->msg_num - 1);
-}
-
-/* is_msglast
- *
- * returns TRUE if we this is the last byte in the current message
-*/
-
-static inline int is_msglast(struct nuc900_i2c *i2c)
-{
-	return i2c->msg_ptr == i2c->msg->len-1;
-}
-
-/* is_msgend
- *
- * returns TRUE if we reached the end of the current message
-*/
-
-static inline int is_msgend(struct nuc900_i2c *i2c)
-{
-	return i2c->msg_ptr >= i2c->msg->len;
-}
-
-/* i2c_nuc900_irq_nextbyte
- *
- * process an interrupt and work out what to do
- */
-
-static void i2c_nuc900_irq_nextbyte(struct nuc900_i2c *i2c,
-							unsigned long iicstat)
-{
-	unsigned char byte;
-
-	switch (i2c->state) {
-
-	case STATE_IDLE:
-		dev_err(i2c->dev, "%s: called in STATE_IDLE\n", __func__);
-		break;
-
-	case STATE_STOP:
-		dev_err(i2c->dev, "%s: called in STATE_STOP\n", __func__);
-		nuc900_i2c_disable_irq(i2c);
-		break;
-
-	case STATE_START:
-		/* last thing we did was send a start condition on the
-		 * bus, or started a new i2c message
-		 */
-
-		if (iicstat & SLAVE_ACK &&
-		    !(i2c->msg->flags & I2C_M_IGNORE_NAK)) {
-			/* ack was not received... */
-
-			dev_dbg(i2c->dev, "ack was not received\n");
-			nuc900_i2c_stop(i2c, -ENXIO);
-			break;
-		}
-
-		if (i2c->msg->flags & I2C_M_RD)
-			i2c->state = STATE_READ;
-		else
-			i2c->state = STATE_WRITE;
-
-		/* terminate the transfer if there is nothing to do
-		 * as this is used by the i2c probe to find devices.
-		*/
-
-		if (is_lastmsg(i2c) && i2c->msg->len == 0) {
-			nuc900_i2c_stop(i2c, 0);
-			break;
-		}
-
-		if (i2c->state == STATE_READ)
-			goto prepare_read;
-
-		/* fall through to the write state, as we will need to
-		 * send a byte as well
-		*/
-
-	case STATE_WRITE:
-		/* we are writing data to the device... check for the
-		 * end of the message, and if so, work out what to do
-		 */
-
-		if (!(i2c->msg->flags & I2C_M_IGNORE_NAK)) {
-			if (iicstat & SLAVE_ACK) {
-				dev_dbg(i2c->dev, "WRITE: No Ack\n");
-
-				nuc900_i2c_stop(i2c, -ECONNREFUSED);
-				break;
-			}
-		}
-
-retry_write:
-
-		if (!is_msgend(i2c)) {
-			byte = i2c->msg->buf[i2c->msg_ptr++];
-			writeb(byte, i2c->regs + TXR);
-			writel(I2C_CMD_WRITE, i2c->regs + CMDR);
-
-		} else if (!is_lastmsg(i2c)) {
-			/* we need to go to the next i2c message */
-
-			dev_dbg(i2c->dev, "WRITE: Next Message\n");
-
-			i2c->msg_ptr = 0;
-			i2c->msg_idx++;
-			i2c->msg++;
-
-			/* check to see if we need to do another message */
-			if (i2c->msg->flags & I2C_M_NOSTART) {
-
-				if (i2c->msg->flags & I2C_M_RD) {
-					/* cannot do this, the controller
-					 * forces us to send a new START
-					 * when we change direction
-					*/
-
-					nuc900_i2c_stop(i2c, -EINVAL);
-				}
-
-				goto retry_write;
-			} else {
-				/* send the new start */
-				nuc900_i2c_message_start(i2c, i2c->msg);
-				i2c->state = STATE_START;
-			}
-
-		} else {
-			/* send stop */
-
-			nuc900_i2c_stop(i2c, 0);
-		}
-		break;
-
-	case STATE_READ:
-		/* we have a byte of data in the data register, do
-		 * something with it, and then work out whether we are
-		 * going to do any more read/write
-		 */
-
-		byte = readb(i2c->regs + RXR);
-		i2c->msg->buf[i2c->msg_ptr++] = byte;
-
-prepare_read:
-		if (is_msglast(i2c)) {
-			/* last byte of buffer */
-
-			if (is_lastmsg(i2c))
-				writel(I2C_CMD_READ | I2C_CMD_NACK,
-							i2c->regs + CMDR);
-
-		} else if (is_msgend(i2c)) {
-			/* ok, we've read the entire buffer, see if there
-			 * is anything else we need to do
-			*/
-
-			if (is_lastmsg(i2c)) {
-				/* last message, send stop and complete */
-				dev_dbg(i2c->dev, "READ: Send Stop\n");
-
-				nuc900_i2c_stop(i2c, 0);
-			} else {
-				/* go to the next transfer */
-				dev_dbg(i2c->dev, "READ: Next Transfer\n");
-
-				i2c->msg_ptr = 0;
-				i2c->msg_idx++;
-				i2c->msg++;
-
-				writel(I2C_CMD_READ, i2c->regs + CMDR);
-			}
-
-		} else {
-			writel(I2C_CMD_READ, i2c->regs + CMDR);
-		}
-
-		break;
-	}
-}
-
-/* nuc900_i2c_irq
- *
- * top level IRQ servicing routine
-*/
-
-static irqreturn_t nuc900_i2c_irq(int irqno, void *dev_id)
-{
-	struct nuc900_i2c *i2c = dev_id;
-	unsigned long status;
-
-	status = readl(i2c->regs + CSR);
-	writel(status | IRQFLAG, i2c->regs + CSR);
-
-	if (status & ARBIT_LOST) {
-		/* deal with arbitration loss */
-		dev_err(i2c->dev, "deal with arbitration loss\n");
-		goto out;
-	}
-
-	if (i2c->state == STATE_IDLE) {
-		dev_dbg(i2c->dev, "IRQ: error i2c->state == IDLE\n");
-		goto out;
-	}
-
-	/* pretty much this leaves us with the fact that we've
-	 * transmitted or received whatever byte we last sent
-	*/
-
-	i2c_nuc900_irq_nextbyte(i2c, status);
-
- out:
-	return IRQ_HANDLED;
-}
-
-
-/* nuc900_i2c_set_master
- *
- * get the i2c bus for a master transaction
-*/
-
-static int nuc900_i2c_set_master(struct nuc900_i2c *i2c)
-{
-	int timeout = 400;
-
-	while (timeout-- > 0) {
-		if (((readl(i2c->regs + SWR) & I2CSTART) == I2CSTART) &&
-				((readl(i2c->regs + CSR) & I2CBUSY) == 0)) {
-			return 0;
-		}
-
-		msleep(1);
-	}
-
-	return -ETIMEDOUT;
-}
-
-/* nuc900_i2c_doxfer
- *
- * this starts an i2c transfer
-*/
-
-static int nuc900_i2c_doxfer(struct nuc900_i2c *i2c,
-			      struct i2c_msg *msgs, int num)
-{
-	unsigned long iicstat, timeout;
-	int spins = 20;
-	int ret;
-
-	ret = nuc900_i2c_set_master(i2c);
-	if (ret != 0) {
-		dev_err(i2c->dev, "cannot get bus (error %d)\n", ret);
-		ret = -EAGAIN;
-		goto out;
-	}
-
-	spin_lock_irq(&i2c->lock);
-
-	i2c->msg     = msgs;
-	i2c->msg_num = num;
-	i2c->msg_ptr = 0;
-	i2c->msg_idx = 0;
-	i2c->state   = STATE_START;
-
-	nuc900_i2c_message_start(i2c, msgs);
-	spin_unlock_irq(&i2c->lock);
-
-	timeout = wait_event_timeout(i2c->wait, i2c->msg_num == 0, HZ * 5);
-
-	ret = i2c->msg_idx;
-
-	/* having these next two as dev_err() makes life very
-	 * noisy when doing an i2cdetect
-	*/
-
-	if (timeout == 0)
-		dev_dbg(i2c->dev, "timeout\n");
-	else if (ret != num)
-		dev_dbg(i2c->dev, "incomplete xfer (%d)\n", ret);
-
-	/* ensure the stop has been through the bus */
-
-	dev_dbg(i2c->dev, "waiting for bus idle\n");
-
-	/* first, try busy waiting briefly */
-	do {
-		iicstat = readl(i2c->regs + CSR);
-	} while ((iicstat & I2CBUSY) && --spins);
-
-	/* if that timed out sleep */
-	if (!spins) {
-		msleep(1);
-		iicstat = readl(i2c->regs + CSR);
-	}
-
-	if (iicstat & I2CBUSY)
-		dev_warn(i2c->dev, "timeout waiting for bus idle\n");
-
- out:
-	return ret;
-}
-
-/* nuc900_i2c_xfer
- *
- * first port of call from the i2c bus code when an message needs
- * transferring across the i2c bus.
-*/
-
-static int nuc900_i2c_xfer(struct i2c_adapter *adap,
-			struct i2c_msg *msgs, int num)
-{
-	struct nuc900_i2c *i2c = (struct nuc900_i2c *)adap->algo_data;
-	int retry;
-	int ret;
-
-	nuc900_i2c_enable_irq(i2c);
-
-	for (retry = 0; retry < adap->retries; retry++) {
-
-		ret = nuc900_i2c_doxfer(i2c, msgs, num);
-
-		if (ret != -EAGAIN)
-			return ret;
-
-		dev_dbg(i2c->dev, "Retrying transmission (%d)\n", retry);
-
-		udelay(100);
-	}
-
-	return -EREMOTEIO;
-}
-
-/* declare our i2c functionality */
-static u32 nuc900_i2c_func(struct i2c_adapter *adap)
-{
-	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL | I2C_FUNC_NOSTART |
-		I2C_FUNC_PROTOCOL_MANGLING;
-}
-
-/* i2c bus registration info */
-
-static const struct i2c_algorithm nuc900_i2c_algorithm = {
-	.master_xfer		= nuc900_i2c_xfer,
-	.functionality		= nuc900_i2c_func,
-};
-
-/* nuc900_i2c_probe
- *
- * called by the bus driver when a suitable device is found
-*/
-
-static int nuc900_i2c_probe(struct platform_device *pdev)
-{
-	struct nuc900_i2c *i2c;
-	struct nuc900_platform_i2c *pdata;
-	struct resource *res;
-	int ret;
-
-	pdata = dev_get_platdata(&pdev->dev);
-	if (!pdata) {
-		dev_err(&pdev->dev, "no platform data\n");
-		return -EINVAL;
-	}
-
-	i2c = kzalloc(sizeof(struct nuc900_i2c), GFP_KERNEL);
-	if (!i2c) {
-		dev_err(&pdev->dev, "no memory for state\n");
-		return -ENOMEM;
-	}
-
-	strlcpy(i2c->adap.name, "nuc900-i2c0", sizeof(i2c->adap.name));
-	i2c->adap.owner   = THIS_MODULE;
-	i2c->adap.algo    = &nuc900_i2c_algorithm;
-	i2c->adap.retries = 2;
-	i2c->adap.class   = I2C_CLASS_HWMON | I2C_CLASS_SPD;
-
-	spin_lock_init(&i2c->lock);
-	init_waitqueue_head(&i2c->wait);
-
-	/* find the clock and enable it */
-
-	i2c->dev = &pdev->dev;
-	i2c->clk = clk_get(&pdev->dev, NULL);
-	if (IS_ERR(i2c->clk)) {
-		dev_err(&pdev->dev, "cannot get clock\n");
-		ret = -ENOENT;
-		goto err_noclk;
-	}
-
-	dev_dbg(&pdev->dev, "clock source %p\n", i2c->clk);
-
-	clk_enable(i2c->clk);
-
-	/* map the registers */
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (res == NULL) {
-		dev_err(&pdev->dev, "cannot find IO resource\n");
-		ret = -ENOENT;
-		goto err_clk;
-	}
-
-	i2c->ioarea = request_mem_region(res->start, resource_size(res),
-					 pdev->name);
-
-	if (i2c->ioarea == NULL) {
-		dev_err(&pdev->dev, "cannot request IO\n");
-		ret = -ENXIO;
-		goto err_clk;
-	}
-
-	i2c->regs = ioremap(res->start, resource_size(res));
-
-	if (i2c->regs == NULL) {
-		dev_err(&pdev->dev, "cannot map IO\n");
-		ret = -ENXIO;
-		goto err_ioarea;
-	}
-
-	dev_dbg(&pdev->dev, "registers %p (%p, %p)\n",
-		i2c->regs, i2c->ioarea, res);
-
-	/* setup info block for the i2c core */
-
-	i2c->adap.algo_data = i2c;
-	i2c->adap.dev.parent = &pdev->dev;
-
-	mfp_set_groupg(&pdev->dev, NULL);
-
-	clk_get_rate(i2c->clk);
-
-	ret = (i2c->clk.apbfreq)/(pdata->bus_freq * 5) - 1;
-	writel(ret & 0xffff, i2c->regs + DIVIDER);
-
-	/* find the IRQ for this unit (note, this relies on the init call to
-	 * ensure no current IRQs pending
-	 */
-
-	i2c->irq = ret = platform_get_irq(pdev, 0);
-	if (ret <= 0) {
-		dev_err(&pdev->dev, "cannot find IRQ\n");
-		goto err_iomap;
-	}
-
-	ret = request_irq(i2c->irq, nuc900_i2c_irq, IRQF_SHARED,
-			  dev_name(&pdev->dev), i2c);
-
-	if (ret != 0) {
-		dev_err(&pdev->dev, "cannot claim IRQ %d\n", i2c->irq);
-		goto err_iomap;
-	}
-
-	/* Note, previous versions of the driver used i2c_add_adapter()
-	 * to add the bus at any number. We now pass the bus number via
-	 * the platform data, so if unset it will now default to always
-	 * being bus 0.
-	 */
-
-	i2c->adap.nr = pdata->bus_num;
-
-	ret = i2c_add_numbered_adapter(&i2c->adap);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "failed to add bus to i2c core\n");
-		goto err_irq;
-	}
-
-	platform_set_drvdata(pdev, i2c);
-
-	dev_info(&pdev->dev, "%s: NUC900 I2C adapter\n",
-						dev_name(&i2c->adap.dev));
-	return 0;
-
- err_irq:
-	free_irq(i2c->irq, i2c);
-
- err_iomap:
-	iounmap(i2c->regs);
-
- err_ioarea:
-	release_resource(i2c->ioarea);
-	kfree(i2c->ioarea);
-
- err_clk:
-	clk_disable(i2c->clk);
-	clk_put(i2c->clk);
-
- err_noclk:
-	kfree(i2c);
-	return ret;
-}
-
-/* nuc900_i2c_remove
- *
- * called when device is removed from the bus
-*/
-
-static int nuc900_i2c_remove(struct platform_device *pdev)
-{
-	struct nuc900_i2c *i2c = platform_get_drvdata(pdev);
-
-	i2c_del_adapter(&i2c->adap);
-	free_irq(i2c->irq, i2c);
-
-	clk_disable(i2c->clk);
-	clk_put(i2c->clk);
-
-	iounmap(i2c->regs);
-
-	release_resource(i2c->ioarea);
-	kfree(i2c->ioarea);
-	kfree(i2c);
-
-	return 0;
-}
-
-static struct platform_driver nuc900_i2c_driver = {
-	.probe		= nuc900_i2c_probe,
-	.remove		= nuc900_i2c_remove,
-	.driver		= {
-		.owner	= THIS_MODULE,
-		.name	= "nuc900-i2c0",
-	},
-};
-
-static int __init i2c_adap_nuc900_init(void)
-{
-	return platform_driver_register(&nuc900_i2c_driver);
-}
-
-static void __exit i2c_adap_nuc900_exit(void)
-{
-	platform_driver_unregister(&nuc900_i2c_driver);
-}
-subsys_initcall(i2c_adap_nuc900_init);
-module_exit(i2c_adap_nuc900_exit);
-
-MODULE_DESCRIPTION("NUC900 I2C Bus driver");
-MODULE_AUTHOR("Wan ZongShun, <mcuos.com-Re5JQEeQqe8AvxtiuMwx3w@public.gmane.org>");
-MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:nuc900-i2c0");
diff --git a/drivers/i2c/busses/i2c-ocores.c b/drivers/i2c/busses/i2c-ocores.c
index 1f6369f14fb..0e10cc6182f 100644
--- a/drivers/i2c/busses/i2c-ocores.c
+++ b/drivers/i2c/busses/i2c-ocores.c
@@ -250,7 +250,7 @@ static struct i2c_adapter ocores_adapter = {
 	.algo		= &ocores_algorithm,
 };
 
-static struct of_device_id ocores_i2c_match[] = {
+static const struct of_device_id ocores_i2c_match[] = {
 	{
 		.compatible = "opencores,i2c-ocores",
 		.data = (void *)TYPE_OCORES,
diff --git a/drivers/i2c/busses/i2c-omap.c b/drivers/i2c/busses/i2c-omap.c
index 85f8eac9ba1..b182793a405 100644
--- a/drivers/i2c/busses/i2c-omap.c
+++ b/drivers/i2c/busses/i2c-omap.c
@@ -1114,10 +1114,8 @@ omap_i2c_probe(struct platform_device *pdev)
 	}
 
 	dev = devm_kzalloc(&pdev->dev, sizeof(struct omap_i2c_dev), GFP_KERNEL);
-	if (!dev) {
-		dev_err(&pdev->dev, "Menory allocation failed\n");
+	if (!dev)
 		return -ENOMEM;
-	}
 
 	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	dev->base = devm_ioremap_resource(&pdev->dev, mem);
diff --git a/drivers/i2c/busses/i2c-pxa.c b/drivers/i2c/busses/i2c-pxa.c
index bbe6dfbc5c0..be671f7a0e0 100644
--- a/drivers/i2c/busses/i2c-pxa.c
+++ b/drivers/i2c/busses/i2c-pxa.c
@@ -1084,7 +1084,7 @@ static const struct i2c_algorithm i2c_pxa_pio_algorithm = {
 	.functionality	= i2c_pxa_functionality,
 };
 
-static struct of_device_id i2c_pxa_dt_ids[] = {
+static const struct of_device_id i2c_pxa_dt_ids[] = {
 	{ .compatible = "mrvl,pxa-i2c", .data = (void *)REGS_PXA2XX },
 	{ .compatible = "mrvl,pwri2c", .data = (void *)REGS_PXA3XX },
 	{ .compatible = "mrvl,mmp-twsi", .data = (void *)REGS_PXA2XX },
diff --git a/drivers/i2c/busses/i2c-qup.c b/drivers/i2c/busses/i2c-qup.c
index 1b4cf14f110..2a5efb5b487 100644
--- a/drivers/i2c/busses/i2c-qup.c
+++ b/drivers/i2c/busses/i2c-qup.c
@@ -479,7 +479,7 @@ static int qup_i2c_xfer(struct i2c_adapter *adap,
 	int ret, idx;
 
 	ret = pm_runtime_get_sync(qup->dev);
-	if (ret)
+	if (ret < 0)
 		goto out;
 
 	writel(1, qup->base + QUP_SW_RESET);
diff --git a/drivers/i2c/busses/i2c-rcar.c b/drivers/i2c/busses/i2c-rcar.c
index d4fa8eba6e9..89940592367 100644
--- a/drivers/i2c/busses/i2c-rcar.c
+++ b/drivers/i2c/busses/i2c-rcar.c
@@ -1,7 +1,9 @@
 /*
- *  drivers/i2c/busses/i2c-rcar.c
+ * Driver for the Renesas RCar I2C unit
  *
- * Copyright (C) 2012 Renesas Solutions Corp.
+ * Copyright (C) 2014 Wolfram Sang <wsa@sang-engineering.com>
+ *
+ * Copyright (C) 2012-14 Renesas Solutions Corp.
  * Kuninori Morimoto <kuninori.morimoto.gx@renesas.com>
  *
  * This file is based on the drivers/i2c/busses/i2c-sh7760.c
@@ -12,16 +14,12 @@
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License
+ * the Free Software Foundation; version 2 of the License.
  *
  * This program is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  */
 #include <linux/clk.h>
 #include <linux/delay.h>
@@ -36,7 +34,6 @@
 #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 */
@@ -60,7 +57,7 @@
 #define FSB	(1 << 1)	/* force stop bit */
 #define ESG	(1 << 0)	/* en startbit gen */
 
-/* ICMSR */
+/* ICMSR (also for ICMIE) */
 #define MNR	(1 << 6)	/* nack received */
 #define MAL	(1 << 5)	/* arbitration lost */
 #define MST	(1 << 4)	/* sent a stop */
@@ -69,32 +66,18 @@
 #define MDR	(1 << 1)
 #define MAT	(1 << 0)	/* slave addr xfer done */
 
-/* ICMIE */
-#define MNRE	(1 << 6)	/* nack irq en */
-#define MALE	(1 << 5)	/* arblos irq en */
-#define MSTE	(1 << 4)	/* stop irq en */
-#define MDEE	(1 << 3)
-#define MDTE	(1 << 2)
-#define MDRE	(1 << 1)
-#define MATE	(1 << 0)	/* address sent irq en */
 
+#define RCAR_BUS_PHASE_START	(MDBS | MIE | ESG)
+#define RCAR_BUS_PHASE_DATA	(MDBS | MIE)
+#define RCAR_BUS_PHASE_STOP	(MDBS | MIE | FSB)
 
-enum {
-	RCAR_BUS_PHASE_ADDR,
-	RCAR_BUS_PHASE_DATA,
-	RCAR_BUS_PHASE_STOP,
-};
+#define RCAR_IRQ_SEND	(MNR | MAL | MST | MAT | MDE)
+#define RCAR_IRQ_RECV	(MNR | MAL | MST | MAT | MDR)
+#define RCAR_IRQ_STOP	(MST)
 
-enum {
-	RCAR_IRQ_CLOSE,
-	RCAR_IRQ_OPEN_FOR_SEND,
-	RCAR_IRQ_OPEN_FOR_RECV,
-	RCAR_IRQ_OPEN_FOR_STOP,
-};
+#define RCAR_IRQ_ACK_SEND	(~(MAT | MDE))
+#define RCAR_IRQ_ACK_RECV	(~(MAT | MDR))
 
-/*
- * flags
- */
 #define ID_LAST_MSG	(1 << 0)
 #define ID_IOERROR	(1 << 1)
 #define ID_DONE		(1 << 2)
@@ -112,14 +95,12 @@ struct rcar_i2c_priv {
 	struct i2c_msg	*msg;
 	struct clk *clk;
 
-	spinlock_t lock;
 	wait_queue_head_t wait;
 
 	int pos;
-	int irq;
 	u32 icccr;
 	u32 flags;
-	enum rcar_i2c_type	devtype;
+	enum rcar_i2c_type devtype;
 };
 
 #define rcar_i2c_priv_to_dev(p)		((p)->adap.dev.parent)
@@ -130,9 +111,7 @@ struct rcar_i2c_priv {
 
 #define LOOP_TIMEOUT	1024
 
-/*
- *		basic functions
- */
+
 static void rcar_i2c_write(struct rcar_i2c_priv *priv, int reg, u32 val)
 {
 	writel(val, priv->io + reg);
@@ -161,36 +140,6 @@ static void rcar_i2c_init(struct rcar_i2c_priv *priv)
 	rcar_i2c_write(priv, ICMAR, 0);
 }
 
-static void rcar_i2c_irq_mask(struct rcar_i2c_priv *priv, int open)
-{
-	u32 val = MNRE | MALE | MSTE | MATE; /* default */
-
-	switch (open) {
-	case RCAR_IRQ_OPEN_FOR_SEND:
-		val |= MDEE; /* default + send */
-		break;
-	case RCAR_IRQ_OPEN_FOR_RECV:
-		val |= MDRE; /* default + read */
-		break;
-	case RCAR_IRQ_OPEN_FOR_STOP:
-		val = MSTE; /* stop irq only */
-		break;
-	case RCAR_IRQ_CLOSE:
-	default:
-		val = 0; /* all close */
-		break;
-	}
-	rcar_i2c_write(priv, ICMIER, val);
-}
-
-static void rcar_i2c_set_addr(struct rcar_i2c_priv *priv, u32 recv)
-{
-	rcar_i2c_write(priv, ICMAR, (priv->msg->addr << 1) | recv);
-}
-
-/*
- *		bus control functions
- */
 static int rcar_i2c_bus_barrier(struct rcar_i2c_priv *priv)
 {
 	int i;
@@ -205,24 +154,6 @@ static int rcar_i2c_bus_barrier(struct rcar_i2c_priv *priv)
 	return -EBUSY;
 }
 
-static void rcar_i2c_bus_phase(struct rcar_i2c_priv *priv, int phase)
-{
-	switch (phase) {
-	case RCAR_BUS_PHASE_ADDR:
-		rcar_i2c_write(priv, ICMCR, MDBS | MIE | ESG);
-		break;
-	case RCAR_BUS_PHASE_DATA:
-		rcar_i2c_write(priv, ICMCR, MDBS | MIE);
-		break;
-	case RCAR_BUS_PHASE_STOP:
-		rcar_i2c_write(priv, ICMCR, MDBS | MIE | FSB);
-		break;
-	}
-}
-
-/*
- *		clock function
- */
 static int rcar_i2c_clock_calculate(struct rcar_i2c_priv *priv,
 				    u32 bus_speed,
 				    struct device *dev)
@@ -312,60 +243,18 @@ scgd_find:
 	return 0;
 }
 
-static void rcar_i2c_clock_start(struct rcar_i2c_priv *priv)
+static int rcar_i2c_prepare_msg(struct rcar_i2c_priv *priv)
 {
-	rcar_i2c_write(priv, ICCCR, priv->icccr);
-}
+	int read = !!rcar_i2c_is_recv(priv);
 
-/*
- *		status functions
- */
-static u32 rcar_i2c_status_get(struct rcar_i2c_priv *priv)
-{
-	return rcar_i2c_read(priv, ICMSR);
-}
-
-#define rcar_i2c_status_clear(priv) rcar_i2c_status_bit_clear(priv, 0xffffffff)
-static void rcar_i2c_status_bit_clear(struct rcar_i2c_priv *priv, u32 bit)
-{
-	rcar_i2c_write(priv, ICMSR, ~bit);
-}
-
-/*
- *		recv/send functions
- */
-static int rcar_i2c_recv(struct rcar_i2c_priv *priv)
-{
-	rcar_i2c_set_addr(priv, 1);
-	rcar_i2c_status_clear(priv);
-	rcar_i2c_bus_phase(priv, RCAR_BUS_PHASE_ADDR);
-	rcar_i2c_irq_mask(priv, RCAR_IRQ_OPEN_FOR_RECV);
-
-	return 0;
-}
-
-static int rcar_i2c_send(struct rcar_i2c_priv *priv)
-{
-	int ret;
-
-	/*
-	 * It should check bus status when send case
-	 */
-	ret = rcar_i2c_bus_barrier(priv);
-	if (ret < 0)
-		return ret;
-
-	rcar_i2c_set_addr(priv, 0);
-	rcar_i2c_status_clear(priv);
-	rcar_i2c_bus_phase(priv, RCAR_BUS_PHASE_ADDR);
-	rcar_i2c_irq_mask(priv, RCAR_IRQ_OPEN_FOR_SEND);
+	rcar_i2c_write(priv, ICMAR, (priv->msg->addr << 1) | read);
+	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;
 }
 
-#define rcar_i2c_send_restart(priv) rcar_i2c_status_bit_clear(priv, (MAT | MDE))
-#define rcar_i2c_recv_restart(priv) rcar_i2c_status_bit_clear(priv, (MAT | MDR))
-
 /*
  *		interrupt functions
  */
@@ -386,7 +275,7 @@ static int rcar_i2c_irq_send(struct rcar_i2c_priv *priv, u32 msr)
 	 * goto data phase.
 	 */
 	if (msr & MAT)
-		rcar_i2c_bus_phase(priv, RCAR_BUS_PHASE_DATA);
+		rcar_i2c_write(priv, ICMCR, RCAR_BUS_PHASE_DATA);
 
 	if (priv->pos < msg->len) {
 		/*
@@ -414,7 +303,7 @@ static int rcar_i2c_irq_send(struct rcar_i2c_priv *priv, u32 msr)
 			 * prepare stop condition here.
 			 * ID_DONE will be set on STOP irq.
 			 */
-			rcar_i2c_bus_phase(priv, RCAR_BUS_PHASE_STOP);
+			rcar_i2c_write(priv, ICMCR, RCAR_BUS_PHASE_STOP);
 		else
 			/*
 			 * If current msg is _NOT_ last msg,
@@ -425,7 +314,7 @@ static int rcar_i2c_irq_send(struct rcar_i2c_priv *priv, u32 msr)
 			return ID_DONE;
 	}
 
-	rcar_i2c_send_restart(priv);
+	rcar_i2c_write(priv, ICMSR, RCAR_IRQ_ACK_SEND);
 
 	return 0;
 }
@@ -462,11 +351,11 @@ static int rcar_i2c_irq_recv(struct rcar_i2c_priv *priv, u32 msr)
 	 * otherwise, go to DATA phase.
 	 */
 	if (priv->pos + 1 >= msg->len)
-		rcar_i2c_bus_phase(priv, RCAR_BUS_PHASE_STOP);
+		rcar_i2c_write(priv, ICMCR, RCAR_BUS_PHASE_STOP);
 	else
-		rcar_i2c_bus_phase(priv, RCAR_BUS_PHASE_DATA);
+		rcar_i2c_write(priv, ICMCR, RCAR_BUS_PHASE_DATA);
 
-	rcar_i2c_recv_restart(priv);
+	rcar_i2c_write(priv, ICMSR, RCAR_IRQ_ACK_RECV);
 
 	return 0;
 }
@@ -474,53 +363,31 @@ 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;
-	struct device *dev = rcar_i2c_priv_to_dev(priv);
 	u32 msr;
 
-	/*-------------- spin lock -----------------*/
-	spin_lock(&priv->lock);
+	msr = rcar_i2c_read(priv, ICMSR);
 
-	msr = rcar_i2c_status_get(priv);
-
-	/*
-	 * Arbitration lost
-	 */
+	/* Arbitration lost */
 	if (msr & MAL) {
-		/*
-		 * CAUTION
-		 *
-		 * When arbitration lost, device become _slave_ mode.
-		 */
-		dev_dbg(dev, "Arbitration Lost\n");
 		rcar_i2c_flags_set(priv, (ID_DONE | ID_ARBLOST));
 		goto out;
 	}
 
-	/*
-	 * Stop
-	 */
+	/* Stop */
 	if (msr & MST) {
-		dev_dbg(dev, "Stop\n");
 		rcar_i2c_flags_set(priv, ID_DONE);
 		goto out;
 	}
 
-	/*
-	 * Nack
-	 */
+	/* Nack */
 	if (msr & MNR) {
-		dev_dbg(dev, "Nack\n");
-
 		/* go to stop phase */
-		rcar_i2c_bus_phase(priv, RCAR_BUS_PHASE_STOP);
-		rcar_i2c_irq_mask(priv, RCAR_IRQ_OPEN_FOR_STOP);
+		rcar_i2c_write(priv, ICMCR, RCAR_BUS_PHASE_STOP);
+		rcar_i2c_write(priv, ICMIER, RCAR_IRQ_STOP);
 		rcar_i2c_flags_set(priv, ID_NACK);
 		goto out;
 	}
 
-	/*
-	 * recv/send
-	 */
 	if (rcar_i2c_is_recv(priv))
 		rcar_i2c_flags_set(priv, rcar_i2c_irq_recv(priv, msr));
 	else
@@ -528,14 +395,11 @@ static irqreturn_t rcar_i2c_irq(int irq, void *ptr)
 
 out:
 	if (rcar_i2c_flags_has(priv, ID_DONE)) {
-		rcar_i2c_irq_mask(priv, RCAR_IRQ_CLOSE);
-		rcar_i2c_status_clear(priv);
+		rcar_i2c_write(priv, ICMIER, 0);
+		rcar_i2c_write(priv, ICMSR, 0);
 		wake_up(&priv->wait);
 	}
 
-	spin_unlock(&priv->lock);
-	/*-------------- spin unlock -----------------*/
-
 	return IRQ_HANDLED;
 }
 
@@ -545,24 +409,24 @@ 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);
-	rcar_i2c_clock_start(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;
 
-	ret = -EINVAL;
 	for (i = 0; i < num; i++) {
-		/*-------------- spin lock -----------------*/
-		spin_lock_irqsave(&priv->lock, flags);
+		/* This HW can't send STOP after address phase */
+		if (msgs[i].len == 0) {
+			ret = -EOPNOTSUPP;
+			break;
+		}
 
 		/* init each data */
 		priv->msg	= &msgs[i];
@@ -571,21 +435,11 @@ static int rcar_i2c_master_xfer(struct i2c_adapter *adap,
 		if (priv->msg == &msgs[num - 1])
 			rcar_i2c_flags_set(priv, ID_LAST_MSG);
 
-		/* start send/recv */
-		if (rcar_i2c_is_recv(priv))
-			ret = rcar_i2c_recv(priv);
-		else
-			ret = rcar_i2c_send(priv);
-
-		spin_unlock_irqrestore(&priv->lock, flags);
-		/*-------------- spin unlock -----------------*/
+		ret = rcar_i2c_prepare_msg(priv);
 
 		if (ret < 0)
 			break;
 
-		/*
-		 * wait result
-		 */
 		timeout = wait_event_timeout(priv->wait,
 					     rcar_i2c_flags_has(priv, ID_DONE),
 					     5 * HZ);
@@ -594,9 +448,6 @@ static int rcar_i2c_master_xfer(struct i2c_adapter *adap,
 			break;
 		}
 
-		/*
-		 * error handling
-		 */
 		if (rcar_i2c_flags_has(priv, ID_NACK)) {
 			ret = -ENXIO;
 			break;
@@ -614,7 +465,7 @@ static int rcar_i2c_master_xfer(struct i2c_adapter *adap,
 
 		ret = i + 1; /* The number of transfer */
 	}
-
+out:
 	pm_runtime_put(dev);
 
 	if (ret < 0 && ret != -ENXIO)
@@ -625,7 +476,8 @@ static int rcar_i2c_master_xfer(struct i2c_adapter *adap,
 
 static u32 rcar_i2c_func(struct i2c_adapter *adap)
 {
-	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+	/* This HW can't do SMBUS_QUICK and NOSTART */
+	return I2C_FUNC_I2C | (I2C_FUNC_SMBUS_EMUL & ~I2C_FUNC_SMBUS_QUICK);
 }
 
 static const struct i2c_algorithm rcar_i2c_algo = {
@@ -639,6 +491,9 @@ static const struct of_device_id rcar_i2c_dt_ids[] = {
 	{ .compatible = "renesas,i2c-r8a7779", .data = (void *)I2C_RCAR_GEN1 },
 	{ .compatible = "renesas,i2c-r8a7790", .data = (void *)I2C_RCAR_GEN2 },
 	{ .compatible = "renesas,i2c-r8a7791", .data = (void *)I2C_RCAR_GEN2 },
+	{ .compatible = "renesas,i2c-r8a7792", .data = (void *)I2C_RCAR_GEN2 },
+	{ .compatible = "renesas,i2c-r8a7793", .data = (void *)I2C_RCAR_GEN2 },
+	{ .compatible = "renesas,i2c-r8a7794", .data = (void *)I2C_RCAR_GEN2 },
 	{},
 };
 MODULE_DEVICE_TABLE(of, rcar_i2c_dt_ids);
@@ -651,13 +506,11 @@ static int rcar_i2c_probe(struct platform_device *pdev)
 	struct resource *res;
 	struct device *dev = &pdev->dev;
 	u32 bus_speed;
-	int ret;
+	int irq, ret;
 
 	priv = devm_kzalloc(dev, sizeof(struct rcar_i2c_priv), GFP_KERNEL);
-	if (!priv) {
-		dev_err(dev, "no mem for private data\n");
+	if (!priv)
 		return -ENOMEM;
-	}
 
 	priv->clk = devm_clk_get(dev, NULL);
 	if (IS_ERR(priv->clk)) {
@@ -685,9 +538,8 @@ static int rcar_i2c_probe(struct platform_device *pdev)
 	if (IS_ERR(priv->io))
 		return PTR_ERR(priv->io);
 
-	priv->irq = platform_get_irq(pdev, 0);
+	irq = platform_get_irq(pdev, 0);
 	init_waitqueue_head(&priv->wait);
-	spin_lock_init(&priv->lock);
 
 	adap			= &priv->adap;
 	adap->nr		= pdev->id;
@@ -699,10 +551,10 @@ static int rcar_i2c_probe(struct platform_device *pdev)
 	i2c_set_adapdata(adap, priv);
 	strlcpy(adap->name, pdev->name, sizeof(adap->name));
 
-	ret = devm_request_irq(dev, priv->irq, rcar_i2c_irq, 0,
+	ret = devm_request_irq(dev, irq, rcar_i2c_irq, 0,
 			       dev_name(dev), priv);
 	if (ret < 0) {
-		dev_err(dev, "cannot get irq %d\n", priv->irq);
+		dev_err(dev, "cannot get irq %d\n", irq);
 		return ret;
 	}
 
@@ -752,6 +604,6 @@ static struct platform_driver rcar_i2c_driver = {
 
 module_platform_driver(rcar_i2c_driver);
 
-MODULE_LICENSE("GPL");
+MODULE_LICENSE("GPL v2");
 MODULE_DESCRIPTION("Renesas R-Car I2C bus driver");
 MODULE_AUTHOR("Kuninori Morimoto <kuninori.morimoto.gx@renesas.com>");
diff --git a/drivers/i2c/busses/i2c-riic.c b/drivers/i2c/busses/i2c-riic.c
index 9e1f8bacfb3..af3b3d032a9 100644
--- a/drivers/i2c/busses/i2c-riic.c
+++ b/drivers/i2c/busses/i2c-riic.c
@@ -404,7 +404,7 @@ static int riic_i2c_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id riic_i2c_dt_ids[] = {
+static const struct of_device_id riic_i2c_dt_ids[] = {
 	{ .compatible = "renesas,riic-rz" },
 	{ /* Sentinel */ },
 };
diff --git a/drivers/i2c/busses/i2c-s3c2410.c b/drivers/i2c/busses/i2c-s3c2410.c
index ae4491062e4..e828a1dba0e 100644
--- a/drivers/i2c/busses/i2c-s3c2410.c
+++ b/drivers/i2c/busses/i2c-s3c2410.c
@@ -1114,16 +1114,12 @@ static int s3c24xx_i2c_probe(struct platform_device *pdev)
 	}
 
 	i2c = devm_kzalloc(&pdev->dev, sizeof(struct s3c24xx_i2c), GFP_KERNEL);
-	if (!i2c) {
-		dev_err(&pdev->dev, "no memory for state\n");
+	if (!i2c)
 		return -ENOMEM;
-	}
 
 	i2c->pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
-	if (!i2c->pdata) {
-		dev_err(&pdev->dev, "no memory for platform data\n");
+	if (!i2c->pdata)
 		return -ENOMEM;
-	}
 
 	i2c->quirks = s3c24xx_get_device_quirks(pdev);
 	if (pdata)
@@ -1276,10 +1272,10 @@ static int s3c24xx_i2c_resume(struct device *dev)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct s3c24xx_i2c *i2c = platform_get_drvdata(pdev);
 
-	i2c->suspended = 0;
 	clk_prepare_enable(i2c->clk);
 	s3c24xx_i2c_init(i2c);
 	clk_disable_unprepare(i2c->clk);
+	i2c->suspended = 0;
 
 	return 0;
 }
diff --git a/drivers/i2c/busses/i2c-sh_mobile.c b/drivers/i2c/busses/i2c-sh_mobile.c
index 1d79585ba4b..8b5e79cb446 100644
--- a/drivers/i2c/busses/i2c-sh_mobile.c
+++ b/drivers/i2c/busses/i2c-sh_mobile.c
@@ -32,6 +32,7 @@
 #include <linux/clk.h>
 #include <linux/io.h>
 #include <linux/slab.h>
+#include <linux/of_device.h>
 #include <linux/i2c/i2c-sh_mobile.h>
 
 /* Transmit operation:                                                      */
@@ -139,6 +140,10 @@ struct sh_mobile_i2c_data {
 	bool send_stop;
 };
 
+struct sh_mobile_dt_config {
+	int clks_per_count;
+};
+
 #define IIC_FLAG_HAS_ICIC67	(1 << 0)
 
 #define STANDARD_MODE		100000
@@ -194,7 +199,7 @@ static void iic_set_clr(struct sh_mobile_i2c_data *pd, int offs,
 	iic_wr(pd, offs, (iic_rd(pd, offs) | set) & ~clr);
 }
 
-static u32 sh_mobile_i2c_iccl(unsigned long count_khz, u32 tLOW, u32 tf, int offset)
+static u32 sh_mobile_i2c_iccl(unsigned long count_khz, u32 tLOW, u32 tf)
 {
 	/*
 	 * Conditional expression:
@@ -206,10 +211,10 @@ static u32 sh_mobile_i2c_iccl(unsigned long count_khz, u32 tLOW, u32 tf, int off
 	 * account the fall time of SCL signal (tf).  Default tf value
 	 * should be 0.3 us, for safety.
 	 */
-	return (((count_khz * (tLOW + tf)) + 5000) / 10000) + offset;
+	return (((count_khz * (tLOW + tf)) + 5000) / 10000);
 }
 
-static u32 sh_mobile_i2c_icch(unsigned long count_khz, u32 tHIGH, u32 tf, int offset)
+static u32 sh_mobile_i2c_icch(unsigned long count_khz, u32 tHIGH, u32 tf)
 {
 	/*
 	 * Conditional expression:
@@ -225,52 +230,58 @@ static u32 sh_mobile_i2c_icch(unsigned long count_khz, u32 tHIGH, u32 tf, int of
 	 * to take into account the fall time of SDA signal (tf) at START
 	 * condition, in order to meet both tHIGH and tHD;STA specs.
 	 */
-	return (((count_khz * (tHIGH + tf)) + 5000) / 10000) + offset;
+	return (((count_khz * (tHIGH + tf)) + 5000) / 10000);
 }
 
-static void sh_mobile_i2c_init(struct sh_mobile_i2c_data *pd)
+static int sh_mobile_i2c_init(struct sh_mobile_i2c_data *pd)
 {
 	unsigned long i2c_clk_khz;
 	u32 tHIGH, tLOW, tf;
-	int offset;
+	uint16_t max_val;
 
 	/* Get clock rate after clock is enabled */
 	clk_prepare_enable(pd->clk);
 	i2c_clk_khz = clk_get_rate(pd->clk) / 1000;
+	clk_disable_unprepare(pd->clk);
 	i2c_clk_khz /= pd->clks_per_count;
 
 	if (pd->bus_speed == STANDARD_MODE) {
 		tLOW	= 47;	/* tLOW = 4.7 us */
 		tHIGH	= 40;	/* tHD;STA = tHIGH = 4.0 us */
 		tf	= 3;	/* tf = 0.3 us */
-		offset	= 0;	/* No offset */
 	} else if (pd->bus_speed == FAST_MODE) {
 		tLOW	= 13;	/* tLOW = 1.3 us */
 		tHIGH	= 6;	/* tHD;STA = tHIGH = 0.6 us */
 		tf	= 3;	/* tf = 0.3 us */
-		offset	= 0;	/* No offset */
 	} else {
 		dev_err(pd->dev, "unrecognized bus speed %lu Hz\n",
 			pd->bus_speed);
-		goto out;
+		return -EINVAL;
+	}
+
+	pd->iccl = sh_mobile_i2c_iccl(i2c_clk_khz, tLOW, tf);
+	pd->icch = sh_mobile_i2c_icch(i2c_clk_khz, tHIGH, tf);
+
+	max_val = pd->flags & IIC_FLAG_HAS_ICIC67 ? 0x1ff : 0xff;
+	if (pd->iccl > max_val || pd->icch > max_val) {
+		dev_err(pd->dev, "timing values out of range: L/H=0x%x/0x%x\n",
+			pd->iccl, pd->icch);
+		return -EINVAL;
 	}
 
-	pd->iccl = sh_mobile_i2c_iccl(i2c_clk_khz, tLOW, tf, offset);
 	/* one more bit of ICCL in ICIC */
-	if ((pd->iccl > 0xff) && (pd->flags & IIC_FLAG_HAS_ICIC67))
+	if (pd->iccl & 0x100)
 		pd->icic |= ICIC_ICCLB8;
 	else
 		pd->icic &= ~ICIC_ICCLB8;
 
-	pd->icch = sh_mobile_i2c_icch(i2c_clk_khz, tHIGH, tf, offset);
 	/* one more bit of ICCH in ICIC */
-	if ((pd->icch > 0xff) && (pd->flags & IIC_FLAG_HAS_ICIC67))
+	if (pd->icch & 0x100)
 		pd->icic |= ICIC_ICCHB8;
 	else
 		pd->icic &= ~ICIC_ICCHB8;
 
-out:
-	clk_disable_unprepare(pd->clk);
+	return 0;
 }
 
 static void activate_ch(struct sh_mobile_i2c_data *pd)
@@ -316,7 +327,7 @@ static unsigned char i2c_op(struct sh_mobile_i2c_data *pd,
 
 	switch (op) {
 	case OP_START: /* issue start and trigger DTE interrupt */
-		iic_wr(pd, ICCR, 0x94);
+		iic_wr(pd, ICCR, ICCR_ICE | ICCR_TRS | ICCR_BBSY);
 		break;
 	case OP_TX_FIRST: /* disable DTE interrupt and write data */
 		iic_wr(pd, ICIC, ICIC_WAITE | ICIC_ALE | ICIC_TACKE);
@@ -327,10 +338,11 @@ static unsigned char i2c_op(struct sh_mobile_i2c_data *pd,
 		break;
 	case OP_TX_STOP: /* write data and issue a stop afterwards */
 		iic_wr(pd, ICDR, data);
-		iic_wr(pd, ICCR, pd->send_stop ? 0x90 : 0x94);
+		iic_wr(pd, ICCR, pd->send_stop ? ICCR_ICE | ICCR_TRS
+					       : ICCR_ICE | ICCR_TRS | ICCR_BBSY);
 		break;
 	case OP_TX_TO_RX: /* select read mode */
-		iic_wr(pd, ICCR, 0x81);
+		iic_wr(pd, ICCR, ICCR_ICE | ICCR_SCP);
 		break;
 	case OP_RX: /* just read data */
 		ret = iic_rd(pd, ICDR);
@@ -338,13 +350,13 @@ static unsigned char i2c_op(struct sh_mobile_i2c_data *pd,
 	case OP_RX_STOP: /* enable DTE interrupt, issue stop */
 		iic_wr(pd, ICIC,
 		       ICIC_DTEE | ICIC_WAITE | ICIC_ALE | ICIC_TACKE);
-		iic_wr(pd, ICCR, 0xc0);
+		iic_wr(pd, ICCR, ICCR_ICE | ICCR_RACK);
 		break;
 	case OP_RX_STOP_DATA: /* enable DTE interrupt, read data, issue stop */
 		iic_wr(pd, ICIC,
 		       ICIC_DTEE | ICIC_WAITE | ICIC_ALE | ICIC_TACKE);
 		ret = iic_rd(pd, ICDR);
-		iic_wr(pd, ICCR, 0xc0);
+		iic_wr(pd, ICCR, ICCR_ICE | ICCR_RACK);
 		break;
 	}
 
@@ -479,7 +491,7 @@ static int start_ch(struct sh_mobile_i2c_data *pd, struct i2c_msg *usr_msg,
 {
 	if (usr_msg->len == 0 && (usr_msg->flags & I2C_M_RD)) {
 		dev_err(pd->dev, "Unsupported zero length i2c read\n");
-		return -EIO;
+		return -EOPNOTSUPP;
 	}
 
 	if (do_init) {
@@ -514,17 +526,12 @@ static int poll_dte(struct sh_mobile_i2c_data *pd)
 			break;
 
 		if (val & ICSR_TACK)
-			return -EIO;
+			return -ENXIO;
 
 		udelay(10);
 	}
 
-	if (!i) {
-		dev_warn(pd->dev, "Timeout polling for DTE!\n");
-		return -ETIMEDOUT;
-	}
-
-	return 0;
+	return i ? 0 : -ETIMEDOUT;
 }
 
 static int poll_busy(struct sh_mobile_i2c_data *pd)
@@ -542,20 +549,18 @@ static int poll_busy(struct sh_mobile_i2c_data *pd)
 		 */
 		if (!(val & ICSR_BUSY)) {
 			/* handle missing acknowledge and arbitration lost */
-			if ((val | pd->sr) & (ICSR_TACK | ICSR_AL))
-				return -EIO;
+			val |= pd->sr;
+			if (val & ICSR_TACK)
+				return -ENXIO;
+			if (val & ICSR_AL)
+				return -EAGAIN;
 			break;
 		}
 
 		udelay(10);
 	}
 
-	if (!i) {
-		dev_err(pd->dev, "Polling timed out\n");
-		return -ETIMEDOUT;
-	}
-
-	return 0;
+	return i ? 0 : -ETIMEDOUT;
 }
 
 static int sh_mobile_i2c_xfer(struct i2c_adapter *adapter,
@@ -617,42 +622,44 @@ static struct i2c_algorithm sh_mobile_i2c_algorithm = {
 	.master_xfer	= sh_mobile_i2c_xfer,
 };
 
-static int sh_mobile_i2c_hook_irqs(struct platform_device *dev, int hook)
+static const struct sh_mobile_dt_config default_dt_config = {
+	.clks_per_count = 1,
+};
+
+static const struct sh_mobile_dt_config rcar_gen2_dt_config = {
+	.clks_per_count = 2,
+};
+
+static const struct of_device_id sh_mobile_i2c_dt_ids[] = {
+	{ .compatible = "renesas,rmobile-iic", .data = &default_dt_config },
+	{ .compatible = "renesas,iic-r8a7790", .data = &rcar_gen2_dt_config },
+	{ .compatible = "renesas,iic-r8a7791", .data = &rcar_gen2_dt_config },
+	{ .compatible = "renesas,iic-r8a7792", .data = &rcar_gen2_dt_config },
+	{ .compatible = "renesas,iic-r8a7793", .data = &rcar_gen2_dt_config },
+	{ .compatible = "renesas,iic-r8a7794", .data = &rcar_gen2_dt_config },
+	{},
+};
+MODULE_DEVICE_TABLE(of, sh_mobile_i2c_dt_ids);
+
+static int sh_mobile_i2c_hook_irqs(struct platform_device *dev)
 {
 	struct resource *res;
-	int ret = -ENXIO;
-	int n, k = 0;
+	resource_size_t n;
+	int k = 0, ret;
 
 	while ((res = platform_get_resource(dev, IORESOURCE_IRQ, k))) {
-		for (n = res->start; hook && n <= res->end; n++) {
-			if (request_irq(n, sh_mobile_i2c_isr, 0,
-					dev_name(&dev->dev), dev)) {
-				for (n--; n >= res->start; n--)
-					free_irq(n, dev);
-
-				goto rollback;
+		for (n = res->start; n <= res->end; n++) {
+			ret = devm_request_irq(&dev->dev, n, sh_mobile_i2c_isr,
+					  0, dev_name(&dev->dev), dev);
+			if (ret) {
+				dev_err(&dev->dev, "cannot request IRQ %pa\n", &n);
+				return ret;
 			}
 		}
 		k++;
 	}
 
-	if (hook)
-		return k > 0 ? 0 : -ENOENT;
-
-	ret = 0;
-
- rollback:
-	k--;
-
-	while (k >= 0) {
-		res = platform_get_resource(dev, IORESOURCE_IRQ, k);
-		for (n = res->start; n <= res->end; n++)
-			free_irq(n, dev);
-
-		k--;
-	}
-
-	return ret;
+	return k > 0 ? 0 : -ENOENT;
 }
 
 static int sh_mobile_i2c_probe(struct platform_device *dev)
@@ -661,62 +668,64 @@ static int sh_mobile_i2c_probe(struct platform_device *dev)
 	struct sh_mobile_i2c_data *pd;
 	struct i2c_adapter *adap;
 	struct resource *res;
-	int size;
 	int ret;
+	u32 bus_speed;
 
-	pd = kzalloc(sizeof(struct sh_mobile_i2c_data), GFP_KERNEL);
-	if (pd == NULL) {
-		dev_err(&dev->dev, "cannot allocate private data\n");
+	pd = devm_kzalloc(&dev->dev, sizeof(struct sh_mobile_i2c_data), GFP_KERNEL);
+	if (!pd)
 		return -ENOMEM;
-	}
 
-	pd->clk = clk_get(&dev->dev, NULL);
+	pd->clk = devm_clk_get(&dev->dev, NULL);
 	if (IS_ERR(pd->clk)) {
 		dev_err(&dev->dev, "cannot get clock\n");
-		ret = PTR_ERR(pd->clk);
-		goto err;
+		return PTR_ERR(pd->clk);
 	}
 
-	ret = sh_mobile_i2c_hook_irqs(dev, 1);
-	if (ret) {
-		dev_err(&dev->dev, "cannot request IRQ\n");
-		goto err_clk;
-	}
+	ret = sh_mobile_i2c_hook_irqs(dev);
+	if (ret)
+		return ret;
 
 	pd->dev = &dev->dev;
 	platform_set_drvdata(dev, pd);
 
 	res = platform_get_resource(dev, IORESOURCE_MEM, 0);
-	if (res == NULL) {
-		dev_err(&dev->dev, "cannot find IO resource\n");
-		ret = -ENOENT;
-		goto err_irq;
-	}
-
-	size = resource_size(res);
 
-	pd->reg = ioremap(res->start, size);
-	if (pd->reg == NULL) {
-		dev_err(&dev->dev, "cannot map IO\n");
-		ret = -ENXIO;
-		goto err_irq;
-	}
+	pd->reg = devm_ioremap_resource(&dev->dev, res);
+	if (IS_ERR(pd->reg))
+		return PTR_ERR(pd->reg);
 
 	/* Use platform data bus speed or STANDARD_MODE */
-	pd->bus_speed = STANDARD_MODE;
-	if (pdata && pdata->bus_speed)
-		pd->bus_speed = pdata->bus_speed;
+	ret = of_property_read_u32(dev->dev.of_node, "clock-frequency", &bus_speed);
+	pd->bus_speed = ret ? STANDARD_MODE : bus_speed;
+
 	pd->clks_per_count = 1;
-	if (pdata && pdata->clks_per_count)
-		pd->clks_per_count = pdata->clks_per_count;
+
+	if (dev->dev.of_node) {
+		const struct of_device_id *match;
+
+		match = of_match_device(sh_mobile_i2c_dt_ids, &dev->dev);
+		if (match) {
+			const struct sh_mobile_dt_config *config;
+
+			config = match->data;
+			pd->clks_per_count = config->clks_per_count;
+		}
+	} else {
+		if (pdata && pdata->bus_speed)
+			pd->bus_speed = pdata->bus_speed;
+		if (pdata && pdata->clks_per_count)
+			pd->clks_per_count = pdata->clks_per_count;
+	}
 
 	/* The IIC blocks on SH-Mobile ARM processors
 	 * come with two new bits in ICIC.
 	 */
-	if (size > 0x17)
+	if (resource_size(res) > 0x17)
 		pd->flags |= IIC_FLAG_HAS_ICIC67;
 
-	sh_mobile_i2c_init(pd);
+	ret = sh_mobile_i2c_init(pd);
+	if (ret)
+		return ret;
 
 	/* Enable Runtime PM for this device.
 	 *
@@ -750,24 +759,14 @@ static int sh_mobile_i2c_probe(struct platform_device *dev)
 	ret = i2c_add_numbered_adapter(adap);
 	if (ret < 0) {
 		dev_err(&dev->dev, "cannot add numbered adapter\n");
-		goto err_all;
+		return ret;
 	}
 
 	dev_info(&dev->dev,
-		 "I2C adapter %d with bus speed %lu Hz (L/H=%x/%x)\n",
+		 "I2C adapter %d with bus speed %lu Hz (L/H=0x%x/0x%x)\n",
 		 adap->nr, pd->bus_speed, pd->iccl, pd->icch);
 
 	return 0;
-
- err_all:
-	iounmap(pd->reg);
- err_irq:
-	sh_mobile_i2c_hook_irqs(dev, 0);
- err_clk:
-	clk_put(pd->clk);
- err:
-	kfree(pd);
-	return ret;
 }
 
 static int sh_mobile_i2c_remove(struct platform_device *dev)
@@ -775,11 +774,7 @@ static int sh_mobile_i2c_remove(struct platform_device *dev)
 	struct sh_mobile_i2c_data *pd = platform_get_drvdata(dev);
 
 	i2c_del_adapter(&pd->adap);
-	iounmap(pd->reg);
-	sh_mobile_i2c_hook_irqs(dev, 0);
-	clk_put(pd->clk);
 	pm_runtime_disable(&dev->dev);
-	kfree(pd);
 	return 0;
 }
 
@@ -800,12 +795,6 @@ static const struct dev_pm_ops sh_mobile_i2c_dev_pm_ops = {
 	.runtime_resume = sh_mobile_i2c_runtime_nop,
 };
 
-static const struct of_device_id sh_mobile_i2c_dt_ids[] = {
-	{ .compatible = "renesas,rmobile-iic", },
-	{},
-};
-MODULE_DEVICE_TABLE(of, sh_mobile_i2c_dt_ids);
-
 static struct platform_driver sh_mobile_i2c_driver = {
 	.driver		= {
 		.name		= "i2c-sh_mobile",
diff --git a/drivers/i2c/busses/i2c-simtec.c b/drivers/i2c/busses/i2c-simtec.c
index 294c80f21d6..964e5c6f84a 100644
--- a/drivers/i2c/busses/i2c-simtec.c
+++ b/drivers/i2c/busses/i2c-simtec.c
@@ -77,10 +77,8 @@ static int simtec_i2c_probe(struct platform_device *dev)
 	int ret;
 
 	pd = kzalloc(sizeof(struct simtec_i2c_data), GFP_KERNEL);
-	if (pd == NULL) {
-		dev_err(&dev->dev, "cannot allocate private data\n");
+	if (pd == NULL)
 		return -ENOMEM;
-	}
 
 	platform_set_drvdata(dev, pd);
 
diff --git a/drivers/i2c/busses/i2c-sirf.c b/drivers/i2c/busses/i2c-sirf.c
index 8e3be7ed058..a3216defc1d 100644
--- a/drivers/i2c/busses/i2c-sirf.c
+++ b/drivers/i2c/busses/i2c-sirf.c
@@ -307,7 +307,6 @@ static int i2c_sirfsoc_probe(struct platform_device *pdev)
 
 	siic = devm_kzalloc(&pdev->dev, sizeof(*siic), GFP_KERNEL);
 	if (!siic) {
-		dev_err(&pdev->dev, "Can't allocate driver data\n");
 		err = -ENOMEM;
 		goto out;
 	}
diff --git a/drivers/i2c/busses/i2c-st.c b/drivers/i2c/busses/i2c-st.c
index 872016196ef..95b94767038 100644
--- a/drivers/i2c/busses/i2c-st.c
+++ b/drivers/i2c/busses/i2c-st.c
@@ -847,7 +847,7 @@ static int st_i2c_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id st_i2c_match[] = {
+static const struct of_device_id st_i2c_match[] = {
 	{ .compatible = "st,comms-ssc-i2c", },
 	{ .compatible = "st,comms-ssc4-i2c", },
 	{},
diff --git a/drivers/i2c/busses/i2c-stu300.c b/drivers/i2c/busses/i2c-stu300.c
index 29b1fb77894..fefb1c19ec1 100644
--- a/drivers/i2c/busses/i2c-stu300.c
+++ b/drivers/i2c/busses/i2c-stu300.c
@@ -868,10 +868,8 @@ static int stu300_probe(struct platform_device *pdev)
 	int ret = 0;
 
 	dev = devm_kzalloc(&pdev->dev, sizeof(struct stu300_dev), GFP_KERNEL);
-	if (!dev) {
-		dev_err(&pdev->dev, "could not allocate device struct\n");
+	if (!dev)
 		return -ENOMEM;
-	}
 
 	bus_nr = pdev->id;
 	dev->clk = devm_clk_get(&pdev->dev, NULL);
diff --git a/drivers/i2c/busses/i2c-tegra.c b/drivers/i2c/busses/i2c-tegra.c
index 00f04cb5b4e..f1bb2fc0679 100644
--- a/drivers/i2c/busses/i2c-tegra.c
+++ b/drivers/i2c/busses/i2c-tegra.c
@@ -732,10 +732,8 @@ static int tegra_i2c_probe(struct platform_device *pdev)
 	}
 
 	i2c_dev = devm_kzalloc(&pdev->dev, sizeof(*i2c_dev), GFP_KERNEL);
-	if (!i2c_dev) {
-		dev_err(&pdev->dev, "Could not allocate struct tegra_i2c_dev");
+	if (!i2c_dev)
 		return -ENOMEM;
-	}
 
 	i2c_dev->base = base;
 	i2c_dev->div_clk = div_clk;
diff --git a/drivers/i2c/busses/i2c-wmt.c b/drivers/i2c/busses/i2c-wmt.c
index 2c8a3e4f900..f80a38c2072 100644
--- a/drivers/i2c/busses/i2c-wmt.c
+++ b/drivers/i2c/busses/i2c-wmt.c
@@ -379,10 +379,8 @@ static int wmt_i2c_probe(struct platform_device *pdev)
 	u32 clk_rate;
 
 	i2c_dev = devm_kzalloc(&pdev->dev, sizeof(*i2c_dev), GFP_KERNEL);
-	if (!i2c_dev) {
-		dev_err(&pdev->dev, "device memory allocation failed\n");
+	if (!i2c_dev)
 		return -ENOMEM;
-	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	i2c_dev->base = devm_ioremap_resource(&pdev->dev, res);
@@ -454,7 +452,7 @@ static int wmt_i2c_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id wmt_i2c_dt_ids[] = {
+static const struct of_device_id wmt_i2c_dt_ids[] = {
 	{ .compatible = "wm,wm8505-i2c" },
 	{ /* Sentinel */ },
 };
diff --git a/drivers/i2c/busses/scx200_acb.c b/drivers/i2c/busses/scx200_acb.c
index cb66f9586f7..ff3f5747e43 100644
--- a/drivers/i2c/busses/scx200_acb.c
+++ b/drivers/i2c/busses/scx200_acb.c
@@ -431,10 +431,8 @@ static struct scx200_acb_iface *scx200_create_iface(const char *text,
 	struct i2c_adapter *adapter;
 
 	iface = kzalloc(sizeof(*iface), GFP_KERNEL);
-	if (!iface) {
-		pr_err("can't allocate memory\n");
+	if (!iface)
 		return NULL;
-	}
 
 	adapter = &iface->adapter;
 	i2c_set_adapdata(adapter, iface);
diff --git a/drivers/i2c/muxes/i2c-mux-pca954x.c b/drivers/i2c/muxes/i2c-mux-pca954x.c
index 550bd36aa5d..9bd4212782a 100644
--- a/drivers/i2c/muxes/i2c-mux-pca954x.c
+++ b/drivers/i2c/muxes/i2c-mux-pca954x.c
@@ -36,12 +36,11 @@
  */
 
 #include <linux/device.h>
-#include <linux/gpio.h>
+#include <linux/gpio/consumer.h>
 #include <linux/i2c.h>
 #include <linux/i2c-mux.h>
 #include <linux/i2c/pca954x.h>
 #include <linux/module.h>
-#include <linux/of_gpio.h>
 #include <linux/slab.h>
 
 #define PCA954X_MAX_NCHANS 8
@@ -186,7 +185,7 @@ static int pca954x_probe(struct i2c_client *client,
 {
 	struct i2c_adapter *adap = to_i2c_adapter(client->dev.parent);
 	struct pca954x_platform_data *pdata = dev_get_platdata(&client->dev);
-	struct device_node *np = client->dev.of_node;
+	struct gpio_desc *gpio;
 	int num, force, class;
 	struct pca954x *data;
 	int ret;
@@ -200,21 +199,10 @@ static int pca954x_probe(struct i2c_client *client,
 
 	i2c_set_clientdata(client, data);
 
-	if (IS_ENABLED(CONFIG_OF) && np) {
-		enum of_gpio_flags flags;
-		int gpio;
-
-		/* Get the mux out of reset if a reset GPIO is specified. */
-		gpio = of_get_named_gpio_flags(np, "reset-gpio", 0, &flags);
-		if (gpio_is_valid(gpio)) {
-			ret = devm_gpio_request_one(&client->dev, gpio,
-					flags & OF_GPIO_ACTIVE_LOW ?
-					GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW,
-					"pca954x reset");
-			if (ret < 0)
-				return ret;
-		}
-	}
+	/* Get the mux out of reset if a reset GPIO is specified. */
+	gpio = devm_gpiod_get(&client->dev, "reset");
+	if (!IS_ERR(gpio))
+		gpiod_direction_output(gpio, 0);
 
 	/* Write the mux register at addr to verify
 	 * that the mux is in fact present. This also
diff --git a/drivers/ide/ide-disk.c b/drivers/ide/ide-disk.c
index 16f69be820c..ee880382e3b 100644
--- a/drivers/ide/ide-disk.c
+++ b/drivers/ide/ide-disk.c
@@ -188,10 +188,9 @@ static ide_startstop_t ide_do_rw_disk(ide_drive_t *drive, struct request *rq,
 
 	ledtrig_ide_activity();
 
-	pr_debug("%s: %sing: block=%llu, sectors=%u, buffer=0x%08lx\n",
+	pr_debug("%s: %sing: block=%llu, sectors=%u\n",
 		 drive->name, rq_data_dir(rq) == READ ? "read" : "writ",
-		 (unsigned long long)block, blk_rq_sectors(rq),
-		 (unsigned long)rq->buffer);
+		 (unsigned long long)block, blk_rq_sectors(rq));
 
 	if (hwif->rw_disk)
 		hwif->rw_disk(drive, rq);
diff --git a/drivers/iio/Kconfig b/drivers/iio/Kconfig
index 5dd0e120a50..345395e9dc6 100644
--- a/drivers/iio/Kconfig
+++ b/drivers/iio/Kconfig
@@ -4,6 +4,7 @@
 
 menuconfig IIO
 	tristate "Industrial I/O support"
+	select ANON_INODES
 	help
 	  The industrial I/O subsystem provides a unified framework for
 	  drivers for many different types of embedded sensors using a
@@ -74,6 +75,7 @@ if IIO_TRIGGER
    source "drivers/iio/trigger/Kconfig"
 endif #IIO_TRIGGER
 source "drivers/iio/pressure/Kconfig"
+source "drivers/iio/proximity/Kconfig"
 source "drivers/iio/temperature/Kconfig"
 
 endif # IIO
diff --git a/drivers/iio/Makefile b/drivers/iio/Makefile
index 887d39090d7..698afc2d17c 100644
--- a/drivers/iio/Makefile
+++ b/drivers/iio/Makefile
@@ -24,5 +24,6 @@ obj-y += light/
 obj-y += magnetometer/
 obj-y += orientation/
 obj-y += pressure/
+obj-y += proximity/
 obj-y += temperature/
 obj-y += trigger/
diff --git a/drivers/iio/accel/Kconfig b/drivers/iio/accel/Kconfig
index e23e5085065..1e120fa1e15 100644
--- a/drivers/iio/accel/Kconfig
+++ b/drivers/iio/accel/Kconfig
@@ -65,4 +65,16 @@ config KXSD9
 	  Say yes here to build support for the Kionix KXSD9 accelerometer.
 	  Currently this only supports the device via an SPI interface.
 
+config MMA8452
+	tristate "Freescale MMA8452Q Accelerometer Driver"
+	depends on I2C
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	help
+	  Say yes here to build support for the Freescale MMA8452Q 3-axis
+	  accelerometer.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called mma8452.
+
 endmenu
diff --git a/drivers/iio/accel/Makefile b/drivers/iio/accel/Makefile
index c48d15f2561..dc0e379c259 100644
--- a/drivers/iio/accel/Makefile
+++ b/drivers/iio/accel/Makefile
@@ -6,6 +6,7 @@
 obj-$(CONFIG_BMA180) += bma180.o
 obj-$(CONFIG_HID_SENSOR_ACCEL_3D) += hid-sensor-accel-3d.o
 obj-$(CONFIG_KXSD9)	+= kxsd9.o
+obj-$(CONFIG_MMA8452)	+= mma8452.o
 
 obj-$(CONFIG_IIO_ST_ACCEL_3AXIS) += st_accel.o
 st_accel-y := st_accel_core.o
diff --git a/drivers/iio/accel/hid-sensor-accel-3d.c b/drivers/iio/accel/hid-sensor-accel-3d.c
index 3dcdbad6545..69abf9163df 100644
--- a/drivers/iio/accel/hid-sensor-accel-3d.c
+++ b/drivers/iio/accel/hid-sensor-accel-3d.c
@@ -22,6 +22,7 @@
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/slab.h>
+#include <linux/delay.h>
 #include <linux/hid-sensor-hub.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -42,6 +43,10 @@ struct accel_3d_state {
 	struct hid_sensor_common common_attributes;
 	struct hid_sensor_hub_attribute_info accel[ACCEL_3D_CHANNEL_MAX];
 	u32 accel_val[ACCEL_3D_CHANNEL_MAX];
+	int scale_pre_decml;
+	int scale_post_decml;
+	int scale_precision;
+	int value_offset;
 };
 
 static const u32 accel_3d_addresses[ACCEL_3D_CHANNEL_MAX] = {
@@ -56,6 +61,7 @@ static const struct iio_chan_spec accel_3d_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
 		BIT(IIO_CHAN_INFO_SCALE) |
 		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
@@ -65,6 +71,7 @@ static const struct iio_chan_spec accel_3d_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
 		BIT(IIO_CHAN_INFO_SCALE) |
 		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
@@ -74,6 +81,7 @@ static const struct iio_chan_spec accel_3d_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
 		BIT(IIO_CHAN_INFO_SCALE) |
 		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
@@ -104,31 +112,42 @@ static int accel_3d_read_raw(struct iio_dev *indio_dev,
 	u32 address;
 	int ret;
 	int ret_type;
+	s32 poll_value;
 
 	*val = 0;
 	*val2 = 0;
 	switch (mask) {
 	case 0:
+		poll_value = hid_sensor_read_poll_value(
+					&accel_state->common_attributes);
+		if (poll_value < 0)
+			return -EINVAL;
+
+		hid_sensor_power_state(&accel_state->common_attributes, true);
+		msleep_interruptible(poll_value * 2);
 		report_id = accel_state->accel[chan->scan_index].report_id;
 		address = accel_3d_addresses[chan->scan_index];
 		if (report_id >= 0)
 			*val = sensor_hub_input_attr_get_raw_value(
-				accel_state->common_attributes.hsdev,
-				HID_USAGE_SENSOR_ACCEL_3D, address,
-				report_id);
+					accel_state->common_attributes.hsdev,
+					HID_USAGE_SENSOR_ACCEL_3D, address,
+					report_id);
 		else {
 			*val = 0;
+			hid_sensor_power_state(&accel_state->common_attributes,
+						 false);
 			return -EINVAL;
 		}
+		hid_sensor_power_state(&accel_state->common_attributes, false);
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SCALE:
-		*val = accel_state->accel[CHANNEL_SCAN_INDEX_X].units;
-		ret_type = IIO_VAL_INT;
+		*val = accel_state->scale_pre_decml;
+		*val2 = accel_state->scale_post_decml;
+		ret_type = accel_state->scale_precision;
 		break;
 	case IIO_CHAN_INFO_OFFSET:
-		*val = hid_sensor_convert_exponent(
-			accel_state->accel[CHANNEL_SCAN_INDEX_X].unit_expo);
+		*val = accel_state->value_offset;
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SAMP_FREQ:
@@ -197,9 +216,8 @@ static int accel_3d_proc_event(struct hid_sensor_hub_device *hsdev,
 	struct iio_dev *indio_dev = platform_get_drvdata(priv);
 	struct accel_3d_state *accel_state = iio_priv(indio_dev);
 
-	dev_dbg(&indio_dev->dev, "accel_3d_proc_event [%d]\n",
-				accel_state->common_attributes.data_ready);
-	if (accel_state->common_attributes.data_ready)
+	dev_dbg(&indio_dev->dev, "accel_3d_proc_event\n");
+	if (atomic_read(&accel_state->common_attributes.data_ready))
 		hid_sensor_push_data(indio_dev,
 				accel_state->accel_val,
 				sizeof(accel_state->accel_val));
@@ -262,6 +280,11 @@ static int accel_3d_parse_report(struct platform_device *pdev,
 			st->accel[1].index, st->accel[1].report_id,
 			st->accel[2].index, st->accel[2].report_id);
 
+	st->scale_precision = hid_sensor_format_scale(
+				HID_USAGE_SENSOR_ACCEL_3D,
+				&st->accel[CHANNEL_SCAN_INDEX_X],
+				&st->scale_pre_decml, &st->scale_post_decml);
+
 	/* Set Sensitivity field ids, when there is no individual modifier */
 	if (st->common_attributes.sensitivity.index < 0) {
 		sensor_hub_input_get_attribute_info(hsdev,
@@ -333,7 +356,7 @@ static int hid_accel_3d_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
 		goto error_free_dev_mem;
 	}
-	accel_state->common_attributes.data_ready = false;
+	atomic_set(&accel_state->common_attributes.data_ready, 0);
 	ret = hid_sensor_setup_trigger(indio_dev, name,
 					&accel_state->common_attributes);
 	if (ret < 0) {
diff --git a/drivers/iio/accel/mma8452.c b/drivers/iio/accel/mma8452.c
new file mode 100644
index 00000000000..17aeea17056
--- /dev/null
+++ b/drivers/iio/accel/mma8452.c
@@ -0,0 +1,439 @@
+/*
+ * mma8452.c - Support for Freescale MMA8452Q 3-axis 12-bit accelerometer
+ *
+ * Copyright 2014 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.
+ *
+ * 7-bit I2C slave address 0x1c/0x1d (pin selectable)
+ *
+ * TODO: interrupt, thresholding, orientation / freefall events, autosleep
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/delay.h>
+
+#define MMA8452_STATUS 0x00
+#define MMA8452_OUT_X 0x01 /* MSB first, 12-bit  */
+#define MMA8452_OUT_Y 0x03
+#define MMA8452_OUT_Z 0x05
+#define MMA8452_WHO_AM_I 0x0d
+#define MMA8452_DATA_CFG 0x0e
+#define MMA8452_OFF_X 0x2f
+#define MMA8452_OFF_Y 0x30
+#define MMA8452_OFF_Z 0x31
+#define MMA8452_CTRL_REG1 0x2a
+#define MMA8452_CTRL_REG2 0x2b
+
+#define MMA8452_STATUS_DRDY (BIT(2) | BIT(1) | BIT(0))
+
+#define MMA8452_CTRL_DR_MASK (BIT(5) | BIT(4) | BIT(3))
+#define MMA8452_CTRL_DR_SHIFT 3
+#define MMA8452_CTRL_DR_DEFAULT 0x4 /* 50 Hz sample frequency */
+#define MMA8452_CTRL_ACTIVE BIT(0)
+
+#define MMA8452_DATA_CFG_FS_MASK (BIT(1) | BIT(0))
+#define MMA8452_DATA_CFG_FS_2G 0
+#define MMA8452_DATA_CFG_FS_4G 1
+#define MMA8452_DATA_CFG_FS_8G 2
+
+#define MMA8452_DEVICE_ID 0x2a
+
+struct mma8452_data {
+	struct i2c_client *client;
+	struct mutex lock;
+	u8 ctrl_reg1;
+	u8 data_cfg;
+};
+
+static int mma8452_drdy(struct mma8452_data *data)
+{
+	int tries = 150;
+
+	while (tries-- > 0) {
+		int ret = i2c_smbus_read_byte_data(data->client,
+			MMA8452_STATUS);
+		if (ret < 0)
+			return ret;
+		if ((ret & MMA8452_STATUS_DRDY) == MMA8452_STATUS_DRDY)
+			return 0;
+		msleep(20);
+	}
+
+	dev_err(&data->client->dev, "data not ready\n");
+	return -EIO;
+}
+
+static int mma8452_read(struct mma8452_data *data, __be16 buf[3])
+{
+	int ret = mma8452_drdy(data);
+	if (ret < 0)
+		return ret;
+	return i2c_smbus_read_i2c_block_data(data->client,
+		MMA8452_OUT_X, 3 * sizeof(__be16), (u8 *) buf);
+}
+
+static ssize_t mma8452_show_int_plus_micros(char *buf,
+	const int (*vals)[2], int n)
+{
+	size_t len = 0;
+
+	while (n-- > 0)
+		len += scnprintf(buf + len, PAGE_SIZE - len,
+			"%d.%06d ", vals[n][0], vals[n][1]);
+
+	/* replace trailing space by newline */
+	buf[len - 1] = '\n';
+
+	return len;
+}
+
+static int mma8452_get_int_plus_micros_index(const int (*vals)[2], int n,
+					int val, int val2)
+{
+	while (n-- > 0)
+		if (val == vals[n][0] && val2 == vals[n][1])
+			return n;
+
+	return -EINVAL;
+}
+
+static const int mma8452_samp_freq[8][2] = {
+	{800, 0}, {400, 0}, {200, 0}, {100, 0}, {50, 0}, {12, 500000},
+	{6, 250000}, {1, 560000}
+};
+
+static const int mma8452_scales[3][2] = {
+	{0, 977}, {0, 1953}, {0, 3906}
+};
+
+static ssize_t mma8452_show_samp_freq_avail(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	return mma8452_show_int_plus_micros(buf, mma8452_samp_freq,
+		ARRAY_SIZE(mma8452_samp_freq));
+}
+
+static ssize_t mma8452_show_scale_avail(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	return mma8452_show_int_plus_micros(buf, mma8452_scales,
+		ARRAY_SIZE(mma8452_scales));
+}
+
+static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(mma8452_show_samp_freq_avail);
+static IIO_DEVICE_ATTR(in_accel_scale_available, S_IRUGO,
+	mma8452_show_scale_avail, NULL, 0);
+
+static int mma8452_get_samp_freq_index(struct mma8452_data *data,
+	int val, int val2)
+{
+	return mma8452_get_int_plus_micros_index(mma8452_samp_freq,
+		ARRAY_SIZE(mma8452_samp_freq), val, val2);
+}
+
+static int mma8452_get_scale_index(struct mma8452_data *data,
+	int val, int val2)
+{
+	return mma8452_get_int_plus_micros_index(mma8452_scales,
+		ARRAY_SIZE(mma8452_scales), val, val2);
+}
+
+static int mma8452_read_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int *val, int *val2, long mask)
+{
+	struct mma8452_data *data = iio_priv(indio_dev);
+	__be16 buffer[3];
+	int i, ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		if (iio_buffer_enabled(indio_dev))
+			return -EBUSY;
+
+		mutex_lock(&data->lock);
+		ret = mma8452_read(data, buffer);
+		mutex_unlock(&data->lock);
+		if (ret < 0)
+			return ret;
+		*val = sign_extend32(
+			be16_to_cpu(buffer[chan->scan_index]) >> 4, 11);
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		i = data->data_cfg & MMA8452_DATA_CFG_FS_MASK;
+		*val = mma8452_scales[i][0];
+		*val2 = mma8452_scales[i][1];
+		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		i = (data->ctrl_reg1 & MMA8452_CTRL_DR_MASK) >>
+			MMA8452_CTRL_DR_SHIFT;
+		*val = mma8452_samp_freq[i][0];
+		*val2 = mma8452_samp_freq[i][1];
+		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_CALIBBIAS:
+		ret = i2c_smbus_read_byte_data(data->client, MMA8452_OFF_X +
+			chan->scan_index);
+		if (ret < 0)
+			return ret;
+		*val = sign_extend32(ret, 7);
+		return IIO_VAL_INT;
+	}
+	return -EINVAL;
+}
+
+static int mma8452_standby(struct mma8452_data *data)
+{
+	return i2c_smbus_write_byte_data(data->client, MMA8452_CTRL_REG1,
+		data->ctrl_reg1 & ~MMA8452_CTRL_ACTIVE);
+}
+
+static int mma8452_active(struct mma8452_data *data)
+{
+	return i2c_smbus_write_byte_data(data->client, MMA8452_CTRL_REG1,
+		data->ctrl_reg1);
+}
+
+static int mma8452_change_config(struct mma8452_data *data, u8 reg, u8 val)
+{
+	int ret;
+
+	mutex_lock(&data->lock);
+
+	/* config can only be changed when in standby */
+	ret = mma8452_standby(data);
+	if (ret < 0)
+		goto fail;
+
+	ret = i2c_smbus_write_byte_data(data->client, reg, val);
+	if (ret < 0)
+		goto fail;
+
+	ret = mma8452_active(data);
+	if (ret < 0)
+		goto fail;
+
+	ret = 0;
+fail:
+	mutex_unlock(&data->lock);
+	return ret;
+}
+
+static int mma8452_write_raw(struct iio_dev *indio_dev,
+			     struct iio_chan_spec const *chan,
+			     int val, int val2, long mask)
+{
+	struct mma8452_data *data = iio_priv(indio_dev);
+	int i;
+
+	if (iio_buffer_enabled(indio_dev))
+		return -EBUSY;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		i = mma8452_get_samp_freq_index(data, val, val2);
+		if (i < 0)
+			return -EINVAL;
+
+		data->ctrl_reg1 &= ~MMA8452_CTRL_DR_MASK;
+		data->ctrl_reg1 |= i << MMA8452_CTRL_DR_SHIFT;
+		return mma8452_change_config(data, MMA8452_CTRL_REG1,
+			data->ctrl_reg1);
+	case IIO_CHAN_INFO_SCALE:
+		i = mma8452_get_scale_index(data, val, val2);
+		if (i < 0)
+			return -EINVAL;
+		data->data_cfg &= ~MMA8452_DATA_CFG_FS_MASK;
+		data->data_cfg |= i;
+		return mma8452_change_config(data, MMA8452_DATA_CFG,
+			data->data_cfg);
+	case IIO_CHAN_INFO_CALIBBIAS:
+		if (val < -128 || val > 127)
+			return -EINVAL;
+		return mma8452_change_config(data, MMA8452_OFF_X +
+			chan->scan_index, val);
+	default:
+		return -EINVAL;
+	}
+}
+
+static irqreturn_t mma8452_trigger_handler(int irq, void *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct mma8452_data *data = iio_priv(indio_dev);
+	u8 buffer[16]; /* 3 16-bit channels + padding + ts */
+	int ret;
+
+	ret = mma8452_read(data, (__be16 *) buffer);
+	if (ret < 0)
+		goto done;
+
+	iio_push_to_buffers_with_timestamp(indio_dev, buffer,
+		iio_get_time_ns());
+
+done:
+	iio_trigger_notify_done(indio_dev->trig);
+	return IRQ_HANDLED;
+}
+
+#define MMA8452_CHANNEL(axis, idx) { \
+	.type = IIO_ACCEL, \
+	.modified = 1, \
+	.channel2 = IIO_MOD_##axis, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+		BIT(IIO_CHAN_INFO_CALIBBIAS), \
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SAMP_FREQ) | \
+		BIT(IIO_CHAN_INFO_SCALE), \
+	.scan_index = idx, \
+	.scan_type = { \
+		.sign = 's', \
+		.realbits = 12, \
+		.storagebits = 16, \
+		.shift = 4, \
+		.endianness = IIO_BE, \
+	}, \
+}
+
+static const struct iio_chan_spec mma8452_channels[] = {
+	MMA8452_CHANNEL(X, 0),
+	MMA8452_CHANNEL(Y, 1),
+	MMA8452_CHANNEL(Z, 2),
+	IIO_CHAN_SOFT_TIMESTAMP(3),
+};
+
+static struct attribute *mma8452_attributes[] = {
+	&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
+	&iio_dev_attr_in_accel_scale_available.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group mma8452_group = {
+	.attrs = mma8452_attributes,
+};
+
+static const struct iio_info mma8452_info = {
+	.attrs = &mma8452_group,
+	.read_raw = &mma8452_read_raw,
+	.write_raw = &mma8452_write_raw,
+	.driver_module = THIS_MODULE,
+};
+
+static const unsigned long mma8452_scan_masks[] = {0x7, 0};
+
+static int mma8452_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct mma8452_data *data;
+	struct iio_dev *indio_dev;
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(client, MMA8452_WHO_AM_I);
+	if (ret < 0)
+		return ret;
+	if (ret != MMA8452_DEVICE_ID)
+		return -ENODEV;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	data = iio_priv(indio_dev);
+	data->client = client;
+	mutex_init(&data->lock);
+
+	i2c_set_clientdata(client, indio_dev);
+	indio_dev->info = &mma8452_info;
+	indio_dev->name = id->name;
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = mma8452_channels;
+	indio_dev->num_channels = ARRAY_SIZE(mma8452_channels);
+	indio_dev->available_scan_masks = mma8452_scan_masks;
+
+	data->ctrl_reg1 = MMA8452_CTRL_ACTIVE |
+		(MMA8452_CTRL_DR_DEFAULT << MMA8452_CTRL_DR_SHIFT);
+	ret = i2c_smbus_write_byte_data(client, MMA8452_CTRL_REG1,
+		data->ctrl_reg1);
+	if (ret < 0)
+		return ret;
+
+	data->data_cfg = MMA8452_DATA_CFG_FS_2G;
+	ret = i2c_smbus_write_byte_data(client, MMA8452_DATA_CFG,
+		data->data_cfg);
+	if (ret < 0)
+		return ret;
+
+	ret = iio_triggered_buffer_setup(indio_dev, NULL,
+		mma8452_trigger_handler, NULL);
+	if (ret < 0)
+		return ret;
+
+	ret = iio_device_register(indio_dev);
+	if (ret < 0)
+		goto buffer_cleanup;
+	return 0;
+
+buffer_cleanup:
+	iio_triggered_buffer_cleanup(indio_dev);
+	return ret;
+}
+
+static int mma8452_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+
+	iio_device_unregister(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
+	mma8452_standby(iio_priv(indio_dev));
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int mma8452_suspend(struct device *dev)
+{
+	return mma8452_standby(iio_priv(i2c_get_clientdata(
+		to_i2c_client(dev))));
+}
+
+static int mma8452_resume(struct device *dev)
+{
+	return mma8452_active(iio_priv(i2c_get_clientdata(
+		to_i2c_client(dev))));
+}
+
+static SIMPLE_DEV_PM_OPS(mma8452_pm_ops, mma8452_suspend, mma8452_resume);
+#define MMA8452_PM_OPS (&mma8452_pm_ops)
+#else
+#define MMA8452_PM_OPS NULL
+#endif
+
+static const struct i2c_device_id mma8452_id[] = {
+	{ "mma8452", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mma8452_id);
+
+static struct i2c_driver mma8452_driver = {
+	.driver = {
+		.name	= "mma8452",
+		.pm	= MMA8452_PM_OPS,
+	},
+	.probe = mma8452_probe,
+	.remove = mma8452_remove,
+	.id_table = mma8452_id,
+};
+module_i2c_driver(mma8452_driver);
+
+MODULE_AUTHOR("Peter Meerwald <pmeerw@pmeerw.net>");
+MODULE_DESCRIPTION("Freescale MMA8452 accelerometer driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/accel/st_accel_core.c b/drivers/iio/accel/st_accel_core.c
index 38caedc76b9..a2abf7c2ce3 100644
--- a/drivers/iio/accel/st_accel_core.c
+++ b/drivers/iio/accel/st_accel_core.c
@@ -459,6 +459,8 @@ int st_accel_common_probe(struct iio_dev *indio_dev,
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->info = &accel_info;
 
+	st_sensors_power_enable(indio_dev);
+
 	err = st_sensors_check_device_support(indio_dev,
 				ARRAY_SIZE(st_accel_sensors), st_accel_sensors);
 	if (err < 0)
@@ -496,6 +498,9 @@ int st_accel_common_probe(struct iio_dev *indio_dev,
 	if (err)
 		goto st_accel_device_register_error;
 
+	dev_info(&indio_dev->dev, "registered accelerometer %s\n",
+		 indio_dev->name);
+
 	return 0;
 
 st_accel_device_register_error:
@@ -512,6 +517,8 @@ void st_accel_common_remove(struct iio_dev *indio_dev)
 {
 	struct st_sensor_data *adata = iio_priv(indio_dev);
 
+	st_sensors_power_disable(indio_dev);
+
 	iio_device_unregister(indio_dev);
 	if (adata->get_irq_data_ready(indio_dev) > 0)
 		st_sensors_deallocate_trigger(indio_dev);
diff --git a/drivers/iio/adc/Kconfig b/drivers/iio/adc/Kconfig
index 24c28e3f93a..a80d23628f1 100644
--- a/drivers/iio/adc/Kconfig
+++ b/drivers/iio/adc/Kconfig
@@ -96,9 +96,21 @@ config AD7923
 	  To compile this driver as a module, choose M here: the
 	  module will be called ad7923.
 
+config AD799X
+	tristate "Analog Devices AD799x ADC driver"
+	depends on I2C
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	help
+	  Say yes here to build support for Analog Devices:
+	  ad7991, ad7995, ad7999, ad7992, ad7993, ad7994, ad7997, ad7998
+	  i2c analog to digital converters (ADC). Provides direct access
+	  via sysfs.
+
 config AT91_ADC
 	tristate "Atmel AT91 ADC"
 	depends on ARCH_AT91
+	depends on INPUT
 	select IIO_BUFFER
 	select IIO_TRIGGERED_BUFFER
 	select SYSFS
@@ -107,7 +119,7 @@ config AT91_ADC
 
 config EXYNOS_ADC
 	tristate "Exynos ADC driver support"
-	depends on OF
+	depends on ARCH_EXYNOS || (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
@@ -146,11 +158,12 @@ config MCP320X
 	  called mcp320x.
 
 config MCP3422
-	tristate "Microchip Technology MCP3422/3/4 driver"
+	tristate "Microchip Technology MCP3422/3/4/6/7/8 driver"
 	depends on I2C
 	help
-	  Say yes here to build support for Microchip Technology's MCP3422,
-	  MCP3423 or MCP3424 analog to digital converters.
+	  Say yes here to build support for Microchip Technology's
+	  MCP3422, MCP3423, MCP3424, MCP3426, MCP3427 or MCP3428
+	  analog to digital converters.
 
 	  This driver can also be built as a module. If so, the module will be
 	  called mcp3422.
diff --git a/drivers/iio/adc/Makefile b/drivers/iio/adc/Makefile
index ab346d88c68..9d60f2deaaa 100644
--- a/drivers/iio/adc/Makefile
+++ b/drivers/iio/adc/Makefile
@@ -11,6 +11,7 @@ obj-$(CONFIG_AD7476) += ad7476.o
 obj-$(CONFIG_AD7791) += ad7791.o
 obj-$(CONFIG_AD7793) += ad7793.o
 obj-$(CONFIG_AD7887) += ad7887.o
+obj-$(CONFIG_AD799X) += ad799x.o
 obj-$(CONFIG_AT91_ADC) += at91_adc.o
 obj-$(CONFIG_EXYNOS_ADC) += exynos_adc.o
 obj-$(CONFIG_LP8788_ADC) += lp8788_adc.o
diff --git a/drivers/staging/iio/adc/ad799x_core.c b/drivers/iio/adc/ad799x.c
index 979ec77d6c2..39b4cb48d73 100644
--- a/drivers/staging/iio/adc/ad799x_core.c
+++ b/drivers/iio/adc/ad799x.c
@@ -37,8 +37,144 @@
 #include <linux/iio/sysfs.h>
 #include <linux/iio/events.h>
 #include <linux/iio/buffer.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
 
-#include "ad799x.h"
+#define AD799X_CHANNEL_SHIFT			4
+#define AD799X_STORAGEBITS			16
+/*
+ * AD7991, AD7995 and AD7999 defines
+ */
+
+#define AD7991_REF_SEL				0x08
+#define AD7991_FLTR				0x04
+#define AD7991_BIT_TRIAL_DELAY			0x02
+#define AD7991_SAMPLE_DELAY			0x01
+
+/*
+ * AD7992, AD7993, AD7994, AD7997 and AD7998 defines
+ */
+
+#define AD7998_FLTR				0x08
+#define AD7998_ALERT_EN				0x04
+#define AD7998_BUSY_ALERT			0x02
+#define AD7998_BUSY_ALERT_POL			0x01
+
+#define AD7998_CONV_RES_REG			0x0
+#define AD7998_ALERT_STAT_REG			0x1
+#define AD7998_CONF_REG				0x2
+#define AD7998_CYCLE_TMR_REG			0x3
+
+#define AD7998_DATALOW_REG(x)			((x) * 3 + 0x4)
+#define AD7998_DATAHIGH_REG(x)			((x) * 3 + 0x5)
+#define AD7998_HYST_REG(x)			((x) * 3 + 0x6)
+
+#define AD7998_CYC_MASK				0x7
+#define AD7998_CYC_DIS				0x0
+#define AD7998_CYC_TCONF_32			0x1
+#define AD7998_CYC_TCONF_64			0x2
+#define AD7998_CYC_TCONF_128			0x3
+#define AD7998_CYC_TCONF_256			0x4
+#define AD7998_CYC_TCONF_512			0x5
+#define AD7998_CYC_TCONF_1024			0x6
+#define AD7998_CYC_TCONF_2048			0x7
+
+#define AD7998_ALERT_STAT_CLEAR			0xFF
+
+/*
+ * AD7997 and AD7997 defines
+ */
+
+#define AD7997_8_READ_SINGLE			0x80
+#define AD7997_8_READ_SEQUENCE			0x70
+/* TODO: move this into a common header */
+#define RES_MASK(bits)	((1 << (bits)) - 1)
+
+enum {
+	ad7991,
+	ad7995,
+	ad7999,
+	ad7992,
+	ad7993,
+	ad7994,
+	ad7997,
+	ad7998
+};
+
+/**
+ * struct ad799x_chip_info - chip specific information
+ * @channel:		channel specification
+ * @num_channels:	number of channels
+ * @monitor_mode:	whether the chip supports monitor interrupts
+ * @default_config:	device default configuration
+ * @event_attrs:	pointer to the monitor event attribute group
+ */
+struct ad799x_chip_info {
+	struct iio_chan_spec		channel[9];
+	int				num_channels;
+	u16				default_config;
+	const struct iio_info		*info;
+};
+
+struct ad799x_state {
+	struct i2c_client		*client;
+	const struct ad799x_chip_info	*chip_info;
+	struct regulator		*reg;
+	struct regulator		*vref;
+	unsigned			id;
+	u16				config;
+
+	u8				*rx_buf;
+	unsigned int			transfer_size;
+};
+
+/**
+ * ad799x_trigger_handler() bh of trigger launched polling to ring buffer
+ *
+ * Currently there is no option in this driver to disable the saving of
+ * timestamps within the ring.
+ **/
+static irqreturn_t ad799x_trigger_handler(int irq, void *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct ad799x_state *st = iio_priv(indio_dev);
+	int b_sent;
+	u8 cmd;
+
+	switch (st->id) {
+	case ad7991:
+	case ad7995:
+	case ad7999:
+		cmd = st->config |
+			(*indio_dev->active_scan_mask << AD799X_CHANNEL_SHIFT);
+		break;
+	case ad7992:
+	case ad7993:
+	case ad7994:
+		cmd = (*indio_dev->active_scan_mask << AD799X_CHANNEL_SHIFT) |
+			AD7998_CONV_RES_REG;
+		break;
+	case ad7997:
+	case ad7998:
+		cmd = AD7997_8_READ_SEQUENCE | AD7998_CONV_RES_REG;
+		break;
+	default:
+		cmd = 0;
+	}
+
+	b_sent = i2c_smbus_read_i2c_block_data(st->client,
+			cmd, st->transfer_size, st->rx_buf);
+	if (b_sent < 0)
+		goto out;
+
+	iio_push_to_buffers_with_timestamp(indio_dev, st->rx_buf,
+			iio_get_time_ns());
+out:
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+}
 
 /*
  * ad799x register access by I2C
@@ -578,9 +714,10 @@ static int ad799x_probe(struct i2c_client *client,
 	indio_dev->channels = st->chip_info->channel;
 	indio_dev->num_channels = st->chip_info->num_channels;
 
-	ret = ad799x_register_ring_funcs_and_init(indio_dev);
+	ret = iio_triggered_buffer_setup(indio_dev, NULL,
+		&ad799x_trigger_handler, NULL);
 	if (ret)
-		goto error_disable_reg;
+		goto error_disable_vref;
 
 	if (client->irq > 0) {
 		ret = devm_request_threaded_irq(&client->dev,
@@ -601,12 +738,11 @@ static int ad799x_probe(struct i2c_client *client,
 	return 0;
 
 error_cleanup_ring:
-	ad799x_ring_cleanup(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
+error_disable_vref:
+	regulator_disable(st->vref);
 error_disable_reg:
-	if (!IS_ERR(st->vref))
-		regulator_disable(st->vref);
-	if (!IS_ERR(st->reg))
-		regulator_disable(st->reg);
+	regulator_disable(st->reg);
 
 	return ret;
 }
@@ -618,11 +754,9 @@ static int ad799x_remove(struct i2c_client *client)
 
 	iio_device_unregister(indio_dev);
 
-	ad799x_ring_cleanup(indio_dev);
-	if (!IS_ERR(st->vref))
-		regulator_disable(st->vref);
-	if (!IS_ERR(st->reg))
-		regulator_disable(st->reg);
+	iio_triggered_buffer_cleanup(indio_dev);
+	regulator_disable(st->vref);
+	regulator_disable(st->reg);
 	kfree(st->rx_buf);
 
 	return 0;
diff --git a/drivers/iio/adc/at91_adc.c b/drivers/iio/adc/at91_adc.c
index 89777ed9abd..3b5bacd4d8d 100644
--- a/drivers/iio/adc/at91_adc.c
+++ b/drivers/iio/adc/at91_adc.c
@@ -31,7 +31,108 @@
 #include <linux/iio/trigger_consumer.h>
 #include <linux/iio/triggered_buffer.h>
 
-#include <mach/at91_adc.h>
+/* Registers */
+#define AT91_ADC_CR		0x00		/* Control Register */
+#define		AT91_ADC_SWRST		(1 << 0)	/* Software Reset */
+#define		AT91_ADC_START		(1 << 1)	/* Start Conversion */
+
+#define AT91_ADC_MR		0x04		/* Mode Register */
+#define		AT91_ADC_TSAMOD		(3 << 0)	/* ADC mode */
+#define		AT91_ADC_TSAMOD_ADC_ONLY_MODE		(0 << 0)	/* ADC Mode */
+#define		AT91_ADC_TSAMOD_TS_ONLY_MODE		(1 << 0)	/* Touch Screen Only Mode */
+#define		AT91_ADC_TRGEN		(1 << 0)	/* Trigger Enable */
+#define		AT91_ADC_TRGSEL		(7 << 1)	/* Trigger Selection */
+#define			AT91_ADC_TRGSEL_TC0		(0 << 1)
+#define			AT91_ADC_TRGSEL_TC1		(1 << 1)
+#define			AT91_ADC_TRGSEL_TC2		(2 << 1)
+#define			AT91_ADC_TRGSEL_EXTERNAL	(6 << 1)
+#define		AT91_ADC_LOWRES		(1 << 4)	/* Low Resolution */
+#define		AT91_ADC_SLEEP		(1 << 5)	/* Sleep Mode */
+#define		AT91_ADC_PENDET		(1 << 6)	/* Pen contact detection enable */
+#define		AT91_ADC_PRESCAL_9260	(0x3f << 8)	/* Prescalar Rate Selection */
+#define		AT91_ADC_PRESCAL_9G45	(0xff << 8)
+#define			AT91_ADC_PRESCAL_(x)	((x) << 8)
+#define		AT91_ADC_STARTUP_9260	(0x1f << 16)	/* Startup Up Time */
+#define		AT91_ADC_STARTUP_9G45	(0x7f << 16)
+#define		AT91_ADC_STARTUP_9X5	(0xf << 16)
+#define			AT91_ADC_STARTUP_(x)	((x) << 16)
+#define		AT91_ADC_SHTIM		(0xf  << 24)	/* Sample & Hold Time */
+#define			AT91_ADC_SHTIM_(x)	((x) << 24)
+#define		AT91_ADC_PENDBC		(0x0f << 28)	/* Pen Debounce time */
+#define			AT91_ADC_PENDBC_(x)	((x) << 28)
+
+#define AT91_ADC_TSR		0x0C
+#define		AT91_ADC_TSR_SHTIM	(0xf  << 24)	/* Sample & Hold Time */
+#define			AT91_ADC_TSR_SHTIM_(x)	((x) << 24)
+
+#define AT91_ADC_CHER		0x10		/* Channel Enable Register */
+#define AT91_ADC_CHDR		0x14		/* Channel Disable Register */
+#define AT91_ADC_CHSR		0x18		/* Channel Status Register */
+#define		AT91_ADC_CH(n)		(1 << (n))	/* Channel Number */
+
+#define AT91_ADC_SR		0x1C		/* Status Register */
+#define		AT91_ADC_EOC(n)		(1 << (n))	/* End of Conversion on Channel N */
+#define		AT91_ADC_OVRE(n)	(1 << ((n) + 8))/* Overrun Error on Channel N */
+#define		AT91_ADC_DRDY		(1 << 16)	/* Data Ready */
+#define		AT91_ADC_GOVRE		(1 << 17)	/* General Overrun Error */
+#define		AT91_ADC_ENDRX		(1 << 18)	/* End of RX Buffer */
+#define		AT91_ADC_RXFUFF		(1 << 19)	/* RX Buffer Full */
+
+#define AT91_ADC_SR_9X5		0x30		/* Status Register for 9x5 */
+#define		AT91_ADC_SR_DRDY_9X5	(1 << 24)	/* Data Ready */
+
+#define AT91_ADC_LCDR		0x20		/* Last Converted Data Register */
+#define		AT91_ADC_LDATA		(0x3ff)
+
+#define AT91_ADC_IER		0x24		/* Interrupt Enable Register */
+#define AT91_ADC_IDR		0x28		/* Interrupt Disable Register */
+#define AT91_ADC_IMR		0x2C		/* Interrupt Mask Register */
+#define		AT91RL_ADC_IER_PEN	(1 << 20)
+#define		AT91RL_ADC_IER_NOPEN	(1 << 21)
+#define		AT91_ADC_IER_PEN	(1 << 29)
+#define		AT91_ADC_IER_NOPEN	(1 << 30)
+#define		AT91_ADC_IER_XRDY	(1 << 20)
+#define		AT91_ADC_IER_YRDY	(1 << 21)
+#define		AT91_ADC_IER_PRDY	(1 << 22)
+#define		AT91_ADC_ISR_PENS	(1 << 31)
+
+#define AT91_ADC_CHR(n)		(0x30 + ((n) * 4))	/* Channel Data Register N */
+#define		AT91_ADC_DATA		(0x3ff)
+
+#define AT91_ADC_CDR0_9X5	(0x50)			/* Channel Data Register 0 for 9X5 */
+
+#define AT91_ADC_ACR		0x94	/* Analog Control Register */
+#define		AT91_ADC_ACR_PENDETSENS	(0x3 << 0)	/* pull-up resistor */
+
+#define AT91_ADC_TSMR		0xB0
+#define		AT91_ADC_TSMR_TSMODE	(3 << 0)	/* Touch Screen Mode */
+#define			AT91_ADC_TSMR_TSMODE_NONE		(0 << 0)
+#define			AT91_ADC_TSMR_TSMODE_4WIRE_NO_PRESS	(1 << 0)
+#define			AT91_ADC_TSMR_TSMODE_4WIRE_PRESS	(2 << 0)
+#define			AT91_ADC_TSMR_TSMODE_5WIRE		(3 << 0)
+#define		AT91_ADC_TSMR_TSAV	(3 << 4)	/* Averages samples */
+#define			AT91_ADC_TSMR_TSAV_(x)		((x) << 4)
+#define		AT91_ADC_TSMR_SCTIM	(0x0f << 16)	/* Switch closure time */
+#define		AT91_ADC_TSMR_PENDBC	(0x0f << 28)	/* Pen Debounce time */
+#define			AT91_ADC_TSMR_PENDBC_(x)	((x) << 28)
+#define		AT91_ADC_TSMR_NOTSDMA	(1 << 22)	/* No Touchscreen DMA */
+#define		AT91_ADC_TSMR_PENDET_DIS	(0 << 24)	/* Pen contact detection disable */
+#define		AT91_ADC_TSMR_PENDET_ENA	(1 << 24)	/* Pen contact detection enable */
+
+#define AT91_ADC_TSXPOSR	0xB4
+#define AT91_ADC_TSYPOSR	0xB8
+#define AT91_ADC_TSPRESSR	0xBC
+
+#define AT91_ADC_TRGR_9260	AT91_ADC_MR
+#define AT91_ADC_TRGR_9G45	0x08
+#define AT91_ADC_TRGR_9X5	0xC0
+
+/* Trigger Register bit field */
+#define		AT91_ADC_TRGR_TRGPER	(0xffff << 16)
+#define			AT91_ADC_TRGR_TRGPER_(x)	((x) << 16)
+#define		AT91_ADC_TRGR_TRGMOD	(0x7 << 0)
+#define			AT91_ADC_TRGR_NONE		(0 << 0)
+#define			AT91_ADC_TRGR_MOD_PERIOD_TRIG	(5 << 0)
 
 #define AT91_ADC_CHAN(st, ch) \
 	(st->registers->channel_base + (ch * 4))
@@ -46,6 +147,29 @@
 #define TOUCH_SAMPLE_PERIOD_US		2000	/* 2ms */
 #define TOUCH_PEN_DETECT_DEBOUNCE_US	200
 
+#define MAX_RLPOS_BITS         10
+#define TOUCH_SAMPLE_PERIOD_US_RL      10000   /* 10ms, the SoC can't keep up with 2ms */
+#define TOUCH_SHTIM                    0xa
+
+/**
+ * struct at91_adc_reg_desc - Various informations relative to registers
+ * @channel_base:	Base offset for the channel data registers
+ * @drdy_mask:		Mask of the DRDY field in the relevant registers
+			(Interruptions registers mostly)
+ * @status_register:	Offset of the Interrupt Status Register
+ * @trigger_register:	Offset of the Trigger setup register
+ * @mr_prescal_mask:	Mask of the PRESCAL field in the adc MR register
+ * @mr_startup_mask:	Mask of the STARTUP field in the adc MR register
+ */
+struct at91_adc_reg_desc {
+	u8	channel_base;
+	u32	drdy_mask;
+	u8	status_register;
+	u8	trigger_register;
+	u32	mr_prescal_mask;
+	u32	mr_startup_mask;
+};
+
 struct at91_adc_caps {
 	bool	has_ts;		/* Support touch screen */
 	bool	has_tsmr;	/* only at91sam9x5, sama5d3 have TSMR reg */
@@ -64,12 +188,6 @@ struct at91_adc_caps {
 	struct at91_adc_reg_desc registers;
 };
 
-enum atmel_adc_ts_type {
-	ATMEL_ADC_TOUCHSCREEN_NONE = 0,
-	ATMEL_ADC_TOUCHSCREEN_4WIRE = 4,
-	ATMEL_ADC_TOUCHSCREEN_5WIRE = 5,
-};
-
 struct at91_adc_state {
 	struct clk		*adc_clk;
 	u16			*buffer;
@@ -114,6 +232,11 @@ struct at91_adc_state {
 
 	u16			ts_sample_period_val;
 	u32			ts_pressure_threshold;
+	u16			ts_pendbc;
+
+	bool			ts_bufferedmeasure;
+	u32			ts_prev_absx;
+	u32			ts_prev_absy;
 };
 
 static irqreturn_t at91_adc_trigger_handler(int irq, void *p)
@@ -220,7 +343,72 @@ static int at91_ts_sample(struct at91_adc_state *st)
 	return 0;
 }
 
-static irqreturn_t at91_adc_interrupt(int irq, void *private)
+static irqreturn_t at91_adc_rl_interrupt(int irq, void *private)
+{
+	struct iio_dev *idev = private;
+	struct at91_adc_state *st = iio_priv(idev);
+	u32 status = at91_adc_readl(st, st->registers->status_register);
+	unsigned int reg;
+
+	status &= at91_adc_readl(st, AT91_ADC_IMR);
+	if (status & st->registers->drdy_mask)
+		handle_adc_eoc_trigger(irq, idev);
+
+	if (status & AT91RL_ADC_IER_PEN) {
+		/* Disabling pen debounce is required to get a NOPEN irq */
+		reg = at91_adc_readl(st, AT91_ADC_MR);
+		reg &= ~AT91_ADC_PENDBC;
+		at91_adc_writel(st, AT91_ADC_MR, reg);
+
+		at91_adc_writel(st, AT91_ADC_IDR, AT91RL_ADC_IER_PEN);
+		at91_adc_writel(st, AT91_ADC_IER, AT91RL_ADC_IER_NOPEN
+				| AT91_ADC_EOC(3));
+		/* Set up period trigger for sampling */
+		at91_adc_writel(st, st->registers->trigger_register,
+			AT91_ADC_TRGR_MOD_PERIOD_TRIG |
+			AT91_ADC_TRGR_TRGPER_(st->ts_sample_period_val));
+	} else if (status & AT91RL_ADC_IER_NOPEN) {
+		reg = at91_adc_readl(st, AT91_ADC_MR);
+		reg |= AT91_ADC_PENDBC_(st->ts_pendbc) & AT91_ADC_PENDBC;
+		at91_adc_writel(st, AT91_ADC_MR, reg);
+		at91_adc_writel(st, st->registers->trigger_register,
+			AT91_ADC_TRGR_NONE);
+
+		at91_adc_writel(st, AT91_ADC_IDR, AT91RL_ADC_IER_NOPEN
+				| AT91_ADC_EOC(3));
+		at91_adc_writel(st, AT91_ADC_IER, AT91RL_ADC_IER_PEN);
+		st->ts_bufferedmeasure = false;
+		input_report_key(st->ts_input, BTN_TOUCH, 0);
+		input_sync(st->ts_input);
+	} else if (status & AT91_ADC_EOC(3)) {
+		/* Conversion finished */
+		if (st->ts_bufferedmeasure) {
+			/*
+			 * Last measurement is always discarded, since it can
+			 * be erroneous.
+			 * Always report previous measurement
+			 */
+			input_report_abs(st->ts_input, ABS_X, st->ts_prev_absx);
+			input_report_abs(st->ts_input, ABS_Y, st->ts_prev_absy);
+			input_report_key(st->ts_input, BTN_TOUCH, 1);
+			input_sync(st->ts_input);
+		} else
+			st->ts_bufferedmeasure = true;
+
+		/* Now make new measurement */
+		st->ts_prev_absx = at91_adc_readl(st, AT91_ADC_CHAN(st, 3))
+				   << MAX_RLPOS_BITS;
+		st->ts_prev_absx /= at91_adc_readl(st, AT91_ADC_CHAN(st, 2));
+
+		st->ts_prev_absy = at91_adc_readl(st, AT91_ADC_CHAN(st, 1))
+				   << MAX_RLPOS_BITS;
+		st->ts_prev_absy /= at91_adc_readl(st, AT91_ADC_CHAN(st, 0));
+	}
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t at91_adc_9x5_interrupt(int irq, void *private)
 {
 	struct iio_dev *idev = private;
 	struct at91_adc_state *st = iio_priv(idev);
@@ -653,6 +841,8 @@ static int at91_adc_probe_dt_ts(struct device_node *node,
 		return -EINVAL;
 	}
 
+	if (!st->caps->has_tsmr)
+		return 0;
 	prop = 0;
 	of_property_read_u32(node, "atmel,adc-ts-pressure-threshold", &prop);
 	st->ts_pressure_threshold = prop;
@@ -776,6 +966,7 @@ static int at91_adc_probe_pdata(struct at91_adc_state *st,
 	st->trigger_number = pdata->trigger_number;
 	st->trigger_list = pdata->trigger_list;
 	st->registers = &st->caps->registers;
+	st->touchscreen_type = pdata->touchscreen_type;
 
 	return 0;
 }
@@ -790,7 +981,10 @@ static int atmel_ts_open(struct input_dev *dev)
 {
 	struct at91_adc_state *st = input_get_drvdata(dev);
 
-	at91_adc_writel(st, AT91_ADC_IER, AT91_ADC_IER_PEN);
+	if (st->caps->has_tsmr)
+		at91_adc_writel(st, AT91_ADC_IER, AT91_ADC_IER_PEN);
+	else
+		at91_adc_writel(st, AT91_ADC_IER, AT91RL_ADC_IER_PEN);
 	return 0;
 }
 
@@ -798,45 +992,61 @@ static void atmel_ts_close(struct input_dev *dev)
 {
 	struct at91_adc_state *st = input_get_drvdata(dev);
 
-	at91_adc_writel(st, AT91_ADC_IDR, AT91_ADC_IER_PEN);
+	if (st->caps->has_tsmr)
+		at91_adc_writel(st, AT91_ADC_IDR, AT91_ADC_IER_PEN);
+	else
+		at91_adc_writel(st, AT91_ADC_IDR, AT91RL_ADC_IER_PEN);
 }
 
 static int at91_ts_hw_init(struct at91_adc_state *st, u32 adc_clk_khz)
 {
-	u32 reg = 0, pendbc;
+	u32 reg = 0;
 	int i = 0;
 
-	if (st->touchscreen_type == ATMEL_ADC_TOUCHSCREEN_4WIRE)
-		reg = AT91_ADC_TSMR_TSMODE_4WIRE_PRESS;
-	else
-		reg = AT91_ADC_TSMR_TSMODE_5WIRE;
-
 	/* a Pen Detect Debounce Time is necessary for the ADC Touch to avoid
 	 * pen detect noise.
 	 * The formula is : Pen Detect Debounce Time = (2 ^ pendbc) / ADCClock
 	 */
-	pendbc = round_up(TOUCH_PEN_DETECT_DEBOUNCE_US * adc_clk_khz / 1000, 1);
+	st->ts_pendbc = round_up(TOUCH_PEN_DETECT_DEBOUNCE_US * adc_clk_khz /
+				 1000, 1);
 
-	while (pendbc >> ++i)
+	while (st->ts_pendbc >> ++i)
 		;	/* Empty! Find the shift offset */
-	if (abs(pendbc - (1 << i)) < abs(pendbc - (1 << (i - 1))))
-		pendbc = i;
+	if (abs(st->ts_pendbc - (1 << i)) < abs(st->ts_pendbc - (1 << (i - 1))))
+		st->ts_pendbc = i;
 	else
-		pendbc = i - 1;
+		st->ts_pendbc = i - 1;
 
-	if (st->caps->has_tsmr) {
-		reg |= AT91_ADC_TSMR_TSAV_(st->caps->ts_filter_average)
-				& AT91_ADC_TSMR_TSAV;
-		reg |= AT91_ADC_TSMR_PENDBC_(pendbc) & AT91_ADC_TSMR_PENDBC;
-		reg |= AT91_ADC_TSMR_NOTSDMA;
-		reg |= AT91_ADC_TSMR_PENDET_ENA;
-		reg |= 0x03 << 8;	/* TSFREQ, need bigger than TSAV */
-
-		at91_adc_writel(st, AT91_ADC_TSMR, reg);
-	} else {
-		/* TODO: for 9g45 which has no TSMR */
+	if (!st->caps->has_tsmr) {
+		reg = at91_adc_readl(st, AT91_ADC_MR);
+		reg |= AT91_ADC_TSAMOD_TS_ONLY_MODE | AT91_ADC_PENDET;
+
+		reg |= AT91_ADC_PENDBC_(st->ts_pendbc) & AT91_ADC_PENDBC;
+		at91_adc_writel(st, AT91_ADC_MR, reg);
+
+		reg = AT91_ADC_TSR_SHTIM_(TOUCH_SHTIM) & AT91_ADC_TSR_SHTIM;
+		at91_adc_writel(st, AT91_ADC_TSR, reg);
+
+		st->ts_sample_period_val = round_up((TOUCH_SAMPLE_PERIOD_US_RL *
+						    adc_clk_khz / 1000) - 1, 1);
+
+		return 0;
 	}
 
+	if (st->touchscreen_type == ATMEL_ADC_TOUCHSCREEN_4WIRE)
+		reg = AT91_ADC_TSMR_TSMODE_4WIRE_PRESS;
+	else
+		reg = AT91_ADC_TSMR_TSMODE_5WIRE;
+
+	reg |= AT91_ADC_TSMR_TSAV_(st->caps->ts_filter_average)
+	       & AT91_ADC_TSMR_TSAV;
+	reg |= AT91_ADC_TSMR_PENDBC_(st->ts_pendbc) & AT91_ADC_TSMR_PENDBC;
+	reg |= AT91_ADC_TSMR_NOTSDMA;
+	reg |= AT91_ADC_TSMR_PENDET_ENA;
+	reg |= 0x03 << 8;	/* TSFREQ, needs to be bigger than TSAV */
+
+	at91_adc_writel(st, AT91_ADC_TSMR, reg);
+
 	/* Change adc internal resistor value for better pen detection,
 	 * default value is 100 kOhm.
 	 * 0 = 200 kOhm, 1 = 150 kOhm, 2 = 100 kOhm, 3 = 50 kOhm
@@ -845,7 +1055,7 @@ static int at91_ts_hw_init(struct at91_adc_state *st, u32 adc_clk_khz)
 	at91_adc_writel(st, AT91_ADC_ACR, st->caps->ts_pen_detect_sensitivity
 			& AT91_ADC_ACR_PENDETSENS);
 
-	/* Sample Peroid Time = (TRGPER + 1) / ADCClock */
+	/* Sample Period Time = (TRGPER + 1) / ADCClock */
 	st->ts_sample_period_val = round_up((TOUCH_SAMPLE_PERIOD_US *
 			adc_clk_khz / 1000) - 1, 1);
 
@@ -874,18 +1084,38 @@ static int at91_ts_register(struct at91_adc_state *st,
 	__set_bit(EV_ABS, input->evbit);
 	__set_bit(EV_KEY, input->evbit);
 	__set_bit(BTN_TOUCH, input->keybit);
-	input_set_abs_params(input, ABS_X, 0, (1 << MAX_POS_BITS) - 1, 0, 0);
-	input_set_abs_params(input, ABS_Y, 0, (1 << MAX_POS_BITS) - 1, 0, 0);
-	input_set_abs_params(input, ABS_PRESSURE, 0, 0xffffff, 0, 0);
+	if (st->caps->has_tsmr) {
+		input_set_abs_params(input, ABS_X, 0, (1 << MAX_POS_BITS) - 1,
+				     0, 0);
+		input_set_abs_params(input, ABS_Y, 0, (1 << MAX_POS_BITS) - 1,
+				     0, 0);
+		input_set_abs_params(input, ABS_PRESSURE, 0, 0xffffff, 0, 0);
+	} else {
+		if (st->touchscreen_type != ATMEL_ADC_TOUCHSCREEN_4WIRE) {
+			dev_err(&pdev->dev,
+				"This touchscreen controller only support 4 wires\n");
+			ret = -EINVAL;
+			goto err;
+		}
+
+		input_set_abs_params(input, ABS_X, 0, (1 << MAX_RLPOS_BITS) - 1,
+				     0, 0);
+		input_set_abs_params(input, ABS_Y, 0, (1 << MAX_RLPOS_BITS) - 1,
+				     0, 0);
+	}
 
 	st->ts_input = input;
 	input_set_drvdata(input, st);
 
 	ret = input_register_device(input);
 	if (ret)
-		input_free_device(st->ts_input);
+		goto err;
 
 	return ret;
+
+err:
+	input_free_device(st->ts_input);
+	return ret;
 }
 
 static void at91_ts_unregister(struct at91_adc_state *st)
@@ -943,11 +1173,13 @@ static int at91_adc_probe(struct platform_device *pdev)
 	 */
 	at91_adc_writel(st, AT91_ADC_CR, AT91_ADC_SWRST);
 	at91_adc_writel(st, AT91_ADC_IDR, 0xFFFFFFFF);
-	ret = request_irq(st->irq,
-			  at91_adc_interrupt,
-			  0,
-			  pdev->dev.driver->name,
-			  idev);
+
+	if (st->caps->has_tsmr)
+		ret = request_irq(st->irq, at91_adc_9x5_interrupt, 0,
+				  pdev->dev.driver->name, idev);
+	else
+		ret = request_irq(st->irq, at91_adc_rl_interrupt, 0,
+				  pdev->dev.driver->name, idev);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to allocate IRQ.\n");
 		return ret;
@@ -1051,12 +1283,6 @@ static int at91_adc_probe(struct platform_device *pdev)
 			goto error_disable_adc_clk;
 		}
 	} else {
-		if (!st->caps->has_tsmr) {
-			dev_err(&pdev->dev, "We don't support non-TSMR adc\n");
-			ret = -ENODEV;
-			goto error_disable_adc_clk;
-		}
-
 		ret = at91_ts_register(st, pdev);
 		if (ret)
 			goto error_disable_adc_clk;
@@ -1120,6 +1346,20 @@ static struct at91_adc_caps at91sam9260_caps = {
 	},
 };
 
+static struct at91_adc_caps at91sam9rl_caps = {
+	.has_ts = true,
+	.calc_startup_ticks = calc_startup_ticks_9260,	/* same as 9260 */
+	.num_channels = 6,
+	.registers = {
+		.channel_base = AT91_ADC_CHR(0),
+		.drdy_mask = AT91_ADC_DRDY,
+		.status_register = AT91_ADC_SR,
+		.trigger_register = AT91_ADC_TRGR_9G45,
+		.mr_prescal_mask = AT91_ADC_PRESCAL_9260,
+		.mr_startup_mask = AT91_ADC_STARTUP_9G45,
+	},
+};
+
 static struct at91_adc_caps at91sam9g45_caps = {
 	.has_ts = true,
 	.calc_startup_ticks = calc_startup_ticks_9260,	/* same as 9260 */
@@ -1154,6 +1394,7 @@ static struct at91_adc_caps at91sam9x5_caps = {
 
 static const struct of_device_id at91_adc_dt_ids[] = {
 	{ .compatible = "atmel,at91sam9260-adc", .data = &at91sam9260_caps },
+	{ .compatible = "atmel,at91sam9rl-adc", .data = &at91sam9rl_caps },
 	{ .compatible = "atmel,at91sam9g45-adc", .data = &at91sam9g45_caps },
 	{ .compatible = "atmel,at91sam9x5-adc", .data = &at91sam9x5_caps },
 	{},
@@ -1165,6 +1406,9 @@ static const struct platform_device_id at91_adc_ids[] = {
 		.name = "at91sam9260-adc",
 		.driver_data = (unsigned long)&at91sam9260_caps,
 	}, {
+		.name = "at91sam9rl-adc",
+		.driver_data = (unsigned long)&at91sam9rl_caps,
+	}, {
 		.name = "at91sam9g45-adc",
 		.driver_data = (unsigned long)&at91sam9g45_caps,
 	}, {
diff --git a/drivers/iio/adc/exynos_adc.c b/drivers/iio/adc/exynos_adc.c
index affa93f5178..010578f1d76 100644
--- a/drivers/iio/adc/exynos_adc.c
+++ b/drivers/iio/adc/exynos_adc.c
@@ -82,7 +82,7 @@ enum adc_version {
 #define ADC_CON_EN_START	(1u << 0)
 #define ADC_DATX_MASK		0xFFF
 
-#define EXYNOS_ADC_TIMEOUT	(msecs_to_jiffies(1000))
+#define EXYNOS_ADC_TIMEOUT	(msecs_to_jiffies(100))
 
 struct exynos_adc {
 	void __iomem		*regs;
@@ -112,6 +112,30 @@ static inline unsigned int exynos_adc_get_version(struct platform_device *pdev)
 	return (unsigned int)match->data;
 }
 
+static void exynos_adc_hw_init(struct exynos_adc *info)
+{
+	u32 con1, con2;
+
+	if (info->version == ADC_V2) {
+		con1 = ADC_V2_CON1_SOFT_RESET;
+		writel(con1, ADC_V2_CON1(info->regs));
+
+		con2 = ADC_V2_CON2_OSEL | ADC_V2_CON2_ESEL |
+			ADC_V2_CON2_HIGHF | ADC_V2_CON2_C_TIME(0);
+		writel(con2, ADC_V2_CON2(info->regs));
+
+		/* Enable interrupts */
+		writel(1, ADC_V2_INT_EN(info->regs));
+	} else {
+		/* set default prescaler values and Enable prescaler */
+		con1 =  ADC_V1_CON_PRSCLV(49) | ADC_V1_CON_PRSCEN;
+
+		/* Enable 12-bit ADC resolution */
+		con1 |= ADC_V1_CON_RES;
+		writel(con1, ADC_V1_CON(info->regs));
+	}
+}
+
 static int exynos_read_raw(struct iio_dev *indio_dev,
 				struct iio_chan_spec const *chan,
 				int *val,
@@ -121,11 +145,13 @@ static int exynos_read_raw(struct iio_dev *indio_dev,
 	struct exynos_adc *info = iio_priv(indio_dev);
 	unsigned long timeout;
 	u32 con1, con2;
+	int ret;
 
 	if (mask != IIO_CHAN_INFO_RAW)
 		return -EINVAL;
 
 	mutex_lock(&indio_dev->mlock);
+	reinit_completion(&info->completion);
 
 	/* Select the channel to be used and Trigger conversion */
 	if (info->version == ADC_V2) {
@@ -145,16 +171,21 @@ static int exynos_read_raw(struct iio_dev *indio_dev,
 				ADC_V1_CON(info->regs));
 	}
 
-	timeout = wait_for_completion_interruptible_timeout
+	timeout = wait_for_completion_timeout
 			(&info->completion, EXYNOS_ADC_TIMEOUT);
-	*val = info->value;
+	if (timeout == 0) {
+		dev_warn(&indio_dev->dev, "Conversion timed out! Resetting\n");
+		exynos_adc_hw_init(info);
+		ret = -ETIMEDOUT;
+	} else {
+		*val = info->value;
+		*val2 = 0;
+		ret = IIO_VAL_INT;
+	}
 
 	mutex_unlock(&indio_dev->mlock);
 
-	if (timeout == 0)
-		return -ETIMEDOUT;
-
-	return IIO_VAL_INT;
+	return ret;
 }
 
 static irqreturn_t exynos_adc_isr(int irq, void *dev_id)
@@ -226,30 +257,6 @@ static int exynos_adc_remove_devices(struct device *dev, void *c)
 	return 0;
 }
 
-static void exynos_adc_hw_init(struct exynos_adc *info)
-{
-	u32 con1, con2;
-
-	if (info->version == ADC_V2) {
-		con1 = ADC_V2_CON1_SOFT_RESET;
-		writel(con1, ADC_V2_CON1(info->regs));
-
-		con2 = ADC_V2_CON2_OSEL | ADC_V2_CON2_ESEL |
-			ADC_V2_CON2_HIGHF | ADC_V2_CON2_C_TIME(0);
-		writel(con2, ADC_V2_CON2(info->regs));
-
-		/* Enable interrupts */
-		writel(1, ADC_V2_INT_EN(info->regs));
-	} else {
-		/* set default prescaler values and Enable prescaler */
-		con1 =  ADC_V1_CON_PRSCLV(49) | ADC_V1_CON_PRSCEN;
-
-		/* Enable 12-bit ADC resolution */
-		con1 |= ADC_V1_CON_RES;
-		writel(con1, ADC_V1_CON(info->regs));
-	}
-}
-
 static int exynos_adc_probe(struct platform_device *pdev)
 {
 	struct exynos_adc *info = NULL;
@@ -290,32 +297,30 @@ static int exynos_adc_probe(struct platform_device *pdev)
 
 	init_completion(&info->completion);
 
-	ret = request_irq(info->irq, exynos_adc_isr,
-					0, dev_name(&pdev->dev), info);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "failed requesting irq, irq = %d\n",
-							info->irq);
-		return ret;
-	}
-
-	writel(1, info->enable_reg);
-
 	info->clk = devm_clk_get(&pdev->dev, "adc");
 	if (IS_ERR(info->clk)) {
 		dev_err(&pdev->dev, "failed getting clock, err = %ld\n",
 							PTR_ERR(info->clk));
-		ret = PTR_ERR(info->clk);
-		goto err_irq;
+		return PTR_ERR(info->clk);
 	}
 
 	info->vdd = devm_regulator_get(&pdev->dev, "vdd");
 	if (IS_ERR(info->vdd)) {
 		dev_err(&pdev->dev, "failed getting regulator, err = %ld\n",
 							PTR_ERR(info->vdd));
-		ret = PTR_ERR(info->vdd);
-		goto err_irq;
+		return PTR_ERR(info->vdd);
 	}
 
+	ret = regulator_enable(info->vdd);
+	if (ret)
+		return ret;
+
+	ret = clk_prepare_enable(info->clk);
+	if (ret)
+		goto err_disable_reg;
+
+	writel(1, info->enable_reg);
+
 	info->version = exynos_adc_get_version(pdev);
 
 	platform_set_drvdata(pdev, indio_dev);
@@ -332,16 +337,18 @@ static int exynos_adc_probe(struct platform_device *pdev)
 	else
 		indio_dev->num_channels = MAX_ADC_V2_CHANNELS;
 
+	ret = request_irq(info->irq, exynos_adc_isr,
+					0, dev_name(&pdev->dev), info);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed requesting irq, irq = %d\n",
+							info->irq);
+		goto err_disable_clk;
+	}
+
 	ret = iio_device_register(indio_dev);
 	if (ret)
 		goto err_irq;
 
-	ret = regulator_enable(info->vdd);
-	if (ret)
-		goto err_iio_dev;
-
-	clk_prepare_enable(info->clk);
-
 	exynos_adc_hw_init(info);
 
 	ret = of_platform_populate(np, exynos_adc_match, NULL, &indio_dev->dev);
@@ -355,12 +362,14 @@ static int exynos_adc_probe(struct platform_device *pdev)
 err_of_populate:
 	device_for_each_child(&indio_dev->dev, NULL,
 				exynos_adc_remove_devices);
-	regulator_disable(info->vdd);
-	clk_disable_unprepare(info->clk);
-err_iio_dev:
 	iio_device_unregister(indio_dev);
 err_irq:
 	free_irq(info->irq, info);
+err_disable_clk:
+	writel(0, info->enable_reg);
+	clk_disable_unprepare(info->clk);
+err_disable_reg:
+	regulator_disable(info->vdd);
 	return ret;
 }
 
@@ -371,11 +380,11 @@ static int exynos_adc_remove(struct platform_device *pdev)
 
 	device_for_each_child(&indio_dev->dev, NULL,
 				exynos_adc_remove_devices);
-	regulator_disable(info->vdd);
-	clk_disable_unprepare(info->clk);
-	writel(0, info->enable_reg);
 	iio_device_unregister(indio_dev);
 	free_irq(info->irq, info);
+	writel(0, info->enable_reg);
+	clk_disable_unprepare(info->clk);
+	regulator_disable(info->vdd);
 
 	return 0;
 }
@@ -397,8 +406,8 @@ static int exynos_adc_suspend(struct device *dev)
 		writel(con, ADC_V1_CON(info->regs));
 	}
 
-	clk_disable_unprepare(info->clk);
 	writel(0, info->enable_reg);
+	clk_disable_unprepare(info->clk);
 	regulator_disable(info->vdd);
 
 	return 0;
@@ -414,9 +423,11 @@ static int exynos_adc_resume(struct device *dev)
 	if (ret)
 		return ret;
 
-	writel(1, info->enable_reg);
-	clk_prepare_enable(info->clk);
+	ret = clk_prepare_enable(info->clk);
+	if (ret)
+		return ret;
 
+	writel(1, info->enable_reg);
 	exynos_adc_hw_init(info);
 
 	return 0;
diff --git a/drivers/iio/adc/max1363.c b/drivers/iio/adc/max1363.c
index 9cf3229a727..1b3b74be5c2 100644
--- a/drivers/iio/adc/max1363.c
+++ b/drivers/iio/adc/max1363.c
@@ -1252,8 +1252,8 @@ static const struct max1363_chip_info max1363_chip_info_tbl[] = {
 		.num_modes = ARRAY_SIZE(max1238_mode_list),
 		.default_mode = s0to11,
 		.info = &max1238_info,
-		.channels = max1238_channels,
-		.num_channels = ARRAY_SIZE(max1238_channels),
+		.channels = max1038_channels,
+		.num_channels = ARRAY_SIZE(max1038_channels),
 	},
 	[max11605] = {
 		.bits = 8,
@@ -1262,8 +1262,8 @@ static const struct max1363_chip_info max1363_chip_info_tbl[] = {
 		.num_modes = ARRAY_SIZE(max1238_mode_list),
 		.default_mode = s0to11,
 		.info = &max1238_info,
-		.channels = max1238_channels,
-		.num_channels = ARRAY_SIZE(max1238_channels),
+		.channels = max1038_channels,
+		.num_channels = ARRAY_SIZE(max1038_channels),
 	},
 	[max11606] = {
 		.bits = 10,
@@ -1312,8 +1312,8 @@ static const struct max1363_chip_info max1363_chip_info_tbl[] = {
 		.num_modes = ARRAY_SIZE(max1238_mode_list),
 		.default_mode = s0to11,
 		.info = &max1238_info,
-		.channels = max1238_channels,
-		.num_channels = ARRAY_SIZE(max1238_channels),
+		.channels = max1138_channels,
+		.num_channels = ARRAY_SIZE(max1138_channels),
 	},
 	[max11611] = {
 		.bits = 10,
@@ -1322,8 +1322,8 @@ static const struct max1363_chip_info max1363_chip_info_tbl[] = {
 		.num_modes = ARRAY_SIZE(max1238_mode_list),
 		.default_mode = s0to11,
 		.info = &max1238_info,
-		.channels = max1238_channels,
-		.num_channels = ARRAY_SIZE(max1238_channels),
+		.channels = max1138_channels,
+		.num_channels = ARRAY_SIZE(max1138_channels),
 	},
 	[max11612] = {
 		.bits = 12,
diff --git a/drivers/iio/adc/mcp3422.c b/drivers/iio/adc/mcp3422.c
index 47dcb34ff44..51672256072 100644
--- a/drivers/iio/adc/mcp3422.c
+++ b/drivers/iio/adc/mcp3422.c
@@ -1,10 +1,11 @@
 /*
- * mcp3422.c - driver for the Microchip mcp3422/3/4 chip family
+ * mcp3422.c - driver for the Microchip mcp3422/3/4/6/7/8 chip family
  *
  * Copyright (C) 2013, Angelo Compagnucci
  * Author: Angelo Compagnucci <angelo.compagnucci@gmail.com>
  *
  * Datasheet: http://ww1.microchip.com/downloads/en/devicedoc/22088b.pdf
+ *            http://ww1.microchip.com/downloads/en/DeviceDoc/22226a.pdf
  *
  * This driver exports the value of analog input voltage to sysfs, the
  * voltage unit is nV.
@@ -96,6 +97,7 @@ static const int mcp3422_sign_extend[4] = {
 /* Client data (each client gets its own) */
 struct mcp3422 {
 	struct i2c_client *i2c;
+	u8 id;
 	u8 config;
 	u8 pga[4];
 	struct mutex lock;
@@ -238,6 +240,8 @@ static int mcp3422_write_raw(struct iio_dev *iio,
 			temp = MCP3422_SRATE_15;
 			break;
 		case 3:
+			if (adc->id > 4)
+				return -EINVAL;
 			temp = MCP3422_SRATE_3;
 			break;
 		default:
@@ -271,6 +275,17 @@ static int mcp3422_write_raw_get_fmt(struct iio_dev *indio_dev,
 	}
 }
 
+static ssize_t mcp3422_show_samp_freqs(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct mcp3422 *adc = iio_priv(dev_to_iio_dev(dev));
+
+	if (adc->id > 4)
+		return sprintf(buf, "240 60 15\n");
+
+	return sprintf(buf, "240 60 15 3\n");
+}
+
 static ssize_t mcp3422_show_scales(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
@@ -284,12 +299,13 @@ static ssize_t mcp3422_show_scales(struct device *dev,
 		mcp3422_scales[sample_rate][3]);
 }
 
-static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("240 60 15 3");
+static IIO_DEVICE_ATTR(sampling_frequency_available, S_IRUGO,
+		mcp3422_show_samp_freqs, NULL, 0);
 static IIO_DEVICE_ATTR(in_voltage_scale_available, S_IRUGO,
 		mcp3422_show_scales, NULL, 0);
 
 static struct attribute *mcp3422_attributes[] = {
-	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
+	&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
 	&iio_dev_attr_in_voltage_scale_available.dev_attr.attr,
 	NULL,
 };
@@ -335,6 +351,7 @@ static int mcp3422_probe(struct i2c_client *client,
 
 	adc = iio_priv(indio_dev);
 	adc->i2c = client;
+	adc->id = (u8)(id->driver_data);
 
 	mutex_init(&adc->lock);
 
@@ -343,13 +360,16 @@ static int mcp3422_probe(struct i2c_client *client,
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->info = &mcp3422_info;
 
-	switch ((unsigned int)(id->driver_data)) {
+	switch (adc->id) {
 	case 2:
 	case 3:
+	case 6:
+	case 7:
 		indio_dev->channels = mcp3422_channels;
 		indio_dev->num_channels = ARRAY_SIZE(mcp3422_channels);
 		break;
 	case 4:
+	case 8:
 		indio_dev->channels = mcp3424_channels;
 		indio_dev->num_channels = ARRAY_SIZE(mcp3424_channels);
 		break;
@@ -375,6 +395,9 @@ static const struct i2c_device_id mcp3422_id[] = {
 	{ "mcp3422", 2 },
 	{ "mcp3423", 3 },
 	{ "mcp3424", 4 },
+	{ "mcp3426", 6 },
+	{ "mcp3427", 7 },
+	{ "mcp3428", 8 },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, mcp3422_id);
@@ -399,5 +422,5 @@ static struct i2c_driver mcp3422_driver = {
 module_i2c_driver(mcp3422_driver);
 
 MODULE_AUTHOR("Angelo Compagnucci <angelo.compagnucci@gmail.com>");
-MODULE_DESCRIPTION("Microchip mcp3422/3/4 driver");
+MODULE_DESCRIPTION("Microchip mcp3422/3/4/6/7/8 driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/common/hid-sensors/hid-sensor-attributes.c b/drivers/iio/common/hid-sensors/hid-sensor-attributes.c
index 75b54730a96..403dd3d8986 100644
--- a/drivers/iio/common/hid-sensors/hid-sensor-attributes.c
+++ b/drivers/iio/common/hid-sensors/hid-sensor-attributes.c
@@ -26,6 +26,40 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 
+struct {
+	u32 usage_id;
+	int unit; /* 0 for default others from HID sensor spec */
+	int scale_val0; /* scale, whole number */
+	int scale_val1; /* scale, fraction in micros */
+} static unit_conversion[] = {
+	{HID_USAGE_SENSOR_ACCEL_3D, 0, 9, 806650},
+	{HID_USAGE_SENSOR_ACCEL_3D,
+		HID_USAGE_SENSOR_UNITS_METERS_PER_SEC_SQRD, 1, 0},
+	{HID_USAGE_SENSOR_ACCEL_3D,
+		HID_USAGE_SENSOR_UNITS_G, 9, 806650},
+
+	{HID_USAGE_SENSOR_GYRO_3D, 0, 0, 17453},
+	{HID_USAGE_SENSOR_GYRO_3D,
+		HID_USAGE_SENSOR_UNITS_RADIANS_PER_SECOND, 1, 0},
+	{HID_USAGE_SENSOR_GYRO_3D,
+		HID_USAGE_SENSOR_UNITS_DEGREES_PER_SECOND, 0, 17453},
+
+	{HID_USAGE_SENSOR_COMPASS_3D, 0, 0, 1000},
+	{HID_USAGE_SENSOR_COMPASS_3D, HID_USAGE_SENSOR_UNITS_GAUSS, 1, 0},
+
+	{HID_USAGE_SENSOR_INCLINOMETER_3D, 0, 0, 17453},
+	{HID_USAGE_SENSOR_INCLINOMETER_3D,
+		HID_USAGE_SENSOR_UNITS_DEGREES, 0, 17453},
+	{HID_USAGE_SENSOR_INCLINOMETER_3D,
+		HID_USAGE_SENSOR_UNITS_RADIANS, 1, 0},
+
+	{HID_USAGE_SENSOR_ALS, 0, 1, 0},
+	{HID_USAGE_SENSOR_ALS, HID_USAGE_SENSOR_UNITS_LUX, 1, 0},
+
+	{HID_USAGE_SENSOR_PRESSURE, 0, 100000, 0},
+	{HID_USAGE_SENSOR_PRESSURE, HID_USAGE_SENSOR_UNITS_PASCAL, 1, 0},
+};
+
 static int pow_10(unsigned power)
 {
 	int i;
@@ -113,6 +147,26 @@ static u32 convert_to_vtf_format(int size, int exp, int val1, int val2)
 	return value;
 }
 
+s32 hid_sensor_read_poll_value(struct hid_sensor_common *st)
+{
+	s32 value = 0;
+	int ret;
+
+	ret = sensor_hub_get_feature(st->hsdev,
+		st->poll.report_id,
+		st->poll.index, &value);
+
+	if (ret < 0 || value < 0) {
+		return -EINVAL;
+	} else {
+		if (st->poll.units == HID_USAGE_SENSOR_UNITS_SECOND)
+			value = value * 1000;
+	}
+
+	return value;
+}
+EXPORT_SYMBOL(hid_sensor_read_poll_value);
+
 int hid_sensor_read_samp_freq_value(struct hid_sensor_common *st,
 				int *val1, int *val2)
 {
@@ -209,15 +263,108 @@ int hid_sensor_write_raw_hyst_value(struct hid_sensor_common *st,
 }
 EXPORT_SYMBOL(hid_sensor_write_raw_hyst_value);
 
-int hid_sensor_parse_common_attributes(struct hid_sensor_hub_device *hsdev,
+/*
+ * This fuction applies the unit exponent to the scale.
+ * For example:
+ * 9.806650 ->exp:2-> val0[980]val1[665000]
+ * 9.000806 ->exp:2-> val0[900]val1[80600]
+ * 0.174535 ->exp:2-> val0[17]val1[453500]
+ * 1.001745 ->exp:0-> val0[1]val1[1745]
+ * 1.001745 ->exp:2-> val0[100]val1[174500]
+ * 1.001745 ->exp:4-> val0[10017]val1[450000]
+ * 9.806650 ->exp:-2-> val0[0]val1[98066]
+ */
+static void adjust_exponent_micro(int *val0, int *val1, int scale0,
+				  int scale1, int exp)
+{
+	int i;
+	int x;
+	int res;
+	int rem;
+
+	if (exp > 0) {
+		*val0 = scale0 * pow_10(exp);
+		res = 0;
+		if (exp > 6) {
+			*val1 = 0;
+			return;
+		}
+		for (i = 0; i < exp; ++i) {
+			x = scale1 / pow_10(5 - i);
+			res += (pow_10(exp - 1 - i) * x);
+			scale1 = scale1 % pow_10(5 - i);
+		}
+		*val0 += res;
+			*val1 = scale1 * pow_10(exp);
+	} else if (exp < 0) {
+		exp = abs(exp);
+		if (exp > 6) {
+			*val0 = *val1 = 0;
+			return;
+		}
+		*val0 = scale0 / pow_10(exp);
+		rem = scale0 % pow_10(exp);
+		res = 0;
+		for (i = 0; i < (6 - exp); ++i) {
+			x = scale1 / pow_10(5 - i);
+			res += (pow_10(5 - exp - i) * x);
+			scale1 = scale1 % pow_10(5 - i);
+		}
+		*val1 = rem * pow_10(6 - exp) + res;
+	} else {
+		*val0 = scale0;
+		*val1 = scale1;
+	}
+}
+
+int hid_sensor_format_scale(u32 usage_id,
+			struct hid_sensor_hub_attribute_info *attr_info,
+			int *val0, int *val1)
+{
+	int i;
+	int exp;
+
+	*val0 = 1;
+	*val1 = 0;
+
+	for (i = 0; i < ARRAY_SIZE(unit_conversion); ++i) {
+		if (unit_conversion[i].usage_id == usage_id &&
+			unit_conversion[i].unit == attr_info->units) {
+			exp  = hid_sensor_convert_exponent(
+						attr_info->unit_expo);
+			adjust_exponent_micro(val0, val1,
+					unit_conversion[i].scale_val0,
+					unit_conversion[i].scale_val1, exp);
+			break;
+		}
+	}
+
+	return IIO_VAL_INT_PLUS_MICRO;
+}
+EXPORT_SYMBOL(hid_sensor_format_scale);
+
+int hid_sensor_get_reporting_interval(struct hid_sensor_hub_device *hsdev,
 					u32 usage_id,
 					struct hid_sensor_common *st)
 {
-
 	sensor_hub_input_get_attribute_info(hsdev,
 					HID_FEATURE_REPORT, usage_id,
 					HID_USAGE_SENSOR_PROP_REPORT_INTERVAL,
 					&st->poll);
+	/* Default unit of measure is milliseconds */
+	if (st->poll.units == 0)
+		st->poll.units = HID_USAGE_SENSOR_UNITS_MILLISECOND;
+	return 0;
+
+}
+
+int hid_sensor_parse_common_attributes(struct hid_sensor_hub_device *hsdev,
+					u32 usage_id,
+					struct hid_sensor_common *st)
+{
+
+
+	hid_sensor_get_reporting_interval(hsdev, usage_id, st);
 
 	sensor_hub_input_get_attribute_info(hsdev,
 					HID_FEATURE_REPORT, usage_id,
diff --git a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
index dbefbdaf7cd..73282cee0c8 100644
--- a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
+++ b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
@@ -28,16 +28,17 @@
 #include <linux/iio/sysfs.h>
 #include "hid-sensor-trigger.h"
 
-static int hid_sensor_data_rdy_trigger_set_state(struct iio_trigger *trig,
-						bool state)
+int hid_sensor_power_state(struct hid_sensor_common *st, bool state)
 {
-	struct hid_sensor_common *st = iio_trigger_get_drvdata(trig);
 	int state_val;
 	int report_val;
 
 	if (state) {
 		if (sensor_hub_device_open(st->hsdev))
 			return -EIO;
+
+		atomic_inc(&st->data_ready);
+
 		state_val = hid_sensor_get_usage_index(st->hsdev,
 			st->power_state.report_id,
 			st->power_state.index,
@@ -47,6 +48,8 @@ static int hid_sensor_data_rdy_trigger_set_state(struct iio_trigger *trig,
 			st->report_state.index,
 			HID_USAGE_SENSOR_PROP_REPORTING_STATE_ALL_EVENTS_ENUM);
 	} else {
+		if (!atomic_dec_and_test(&st->data_ready))
+			return 0;
 		sensor_hub_device_close(st->hsdev);
 		state_val = hid_sensor_get_usage_index(st->hsdev,
 			st->power_state.report_id,
@@ -57,7 +60,6 @@ static int hid_sensor_data_rdy_trigger_set_state(struct iio_trigger *trig,
 			st->report_state.index,
 			HID_USAGE_SENSOR_PROP_REPORTING_STATE_NO_EVENTS_ENUM);
 	}
-	st->data_ready = state;
 
 	if (state_val >= 0) {
 		state_val += st->power_state.logical_minimum;
@@ -75,6 +77,13 @@ static int hid_sensor_data_rdy_trigger_set_state(struct iio_trigger *trig,
 
 	return 0;
 }
+EXPORT_SYMBOL(hid_sensor_power_state);
+
+static int hid_sensor_data_rdy_trigger_set_state(struct iio_trigger *trig,
+						bool state)
+{
+	return hid_sensor_power_state(iio_trigger_get_drvdata(trig), state);
+}
 
 void hid_sensor_remove_trigger(struct hid_sensor_common *attrb)
 {
diff --git a/drivers/iio/common/hid-sensors/hid-sensor-trigger.h b/drivers/iio/common/hid-sensors/hid-sensor-trigger.h
index ca02f7811aa..0f8e78c249d 100644
--- a/drivers/iio/common/hid-sensors/hid-sensor-trigger.h
+++ b/drivers/iio/common/hid-sensors/hid-sensor-trigger.h
@@ -22,5 +22,6 @@
 int hid_sensor_setup_trigger(struct iio_dev *indio_dev, const char *name,
 				struct hid_sensor_common *attrb);
 void hid_sensor_remove_trigger(struct hid_sensor_common *attrb);
+int hid_sensor_power_state(struct hid_sensor_common *st, bool state);
 
 #endif
diff --git a/drivers/iio/common/st_sensors/st_sensors_core.c b/drivers/iio/common/st_sensors/st_sensors_core.c
index 7ba1ef27021..e8b932fed70 100644
--- a/drivers/iio/common/st_sensors/st_sensors_core.c
+++ b/drivers/iio/common/st_sensors/st_sensors_core.c
@@ -13,6 +13,7 @@
 #include <linux/slab.h>
 #include <linux/delay.h>
 #include <linux/iio/iio.h>
+#include <linux/regulator/consumer.h>
 #include <asm/unaligned.h>
 
 #include <linux/iio/common/st_sensors.h>
@@ -198,6 +199,42 @@ int st_sensors_set_axis_enable(struct iio_dev *indio_dev, u8 axis_enable)
 }
 EXPORT_SYMBOL(st_sensors_set_axis_enable);
 
+void st_sensors_power_enable(struct iio_dev *indio_dev)
+{
+	struct st_sensor_data *pdata = iio_priv(indio_dev);
+	int err;
+
+	/* Regulators not mandatory, but if requested we should enable them. */
+	pdata->vdd = devm_regulator_get_optional(indio_dev->dev.parent, "vdd");
+	if (!IS_ERR(pdata->vdd)) {
+		err = regulator_enable(pdata->vdd);
+		if (err != 0)
+			dev_warn(&indio_dev->dev,
+				 "Failed to enable specified Vdd supply\n");
+	}
+
+	pdata->vdd_io = devm_regulator_get_optional(indio_dev->dev.parent, "vddio");
+	if (!IS_ERR(pdata->vdd_io)) {
+		err = regulator_enable(pdata->vdd_io);
+		if (err != 0)
+			dev_warn(&indio_dev->dev,
+				 "Failed to enable specified Vdd_IO supply\n");
+	}
+}
+EXPORT_SYMBOL(st_sensors_power_enable);
+
+void st_sensors_power_disable(struct iio_dev *indio_dev)
+{
+	struct st_sensor_data *pdata = iio_priv(indio_dev);
+
+	if (!IS_ERR(pdata->vdd))
+		regulator_disable(pdata->vdd);
+
+	if (!IS_ERR(pdata->vdd_io))
+		regulator_disable(pdata->vdd_io);
+}
+EXPORT_SYMBOL(st_sensors_power_disable);
+
 static int st_sensors_set_drdy_int_pin(struct iio_dev *indio_dev,
 				       struct st_sensors_platform_data *pdata)
 {
diff --git a/drivers/iio/gyro/hid-sensor-gyro-3d.c b/drivers/iio/gyro/hid-sensor-gyro-3d.c
index 59d6bc3e04d..40f4e4935d0 100644
--- a/drivers/iio/gyro/hid-sensor-gyro-3d.c
+++ b/drivers/iio/gyro/hid-sensor-gyro-3d.c
@@ -22,6 +22,7 @@
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/slab.h>
+#include <linux/delay.h>
 #include <linux/hid-sensor-hub.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -42,6 +43,10 @@ struct gyro_3d_state {
 	struct hid_sensor_common common_attributes;
 	struct hid_sensor_hub_attribute_info gyro[GYRO_3D_CHANNEL_MAX];
 	u32 gyro_val[GYRO_3D_CHANNEL_MAX];
+	int scale_pre_decml;
+	int scale_post_decml;
+	int scale_precision;
+	int value_offset;
 };
 
 static const u32 gyro_3d_addresses[GYRO_3D_CHANNEL_MAX] = {
@@ -56,6 +61,7 @@ static const struct iio_chan_spec gyro_3d_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
 		BIT(IIO_CHAN_INFO_SCALE) |
 		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
@@ -65,6 +71,7 @@ static const struct iio_chan_spec gyro_3d_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
 		BIT(IIO_CHAN_INFO_SCALE) |
 		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
@@ -74,6 +81,7 @@ static const struct iio_chan_spec gyro_3d_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
 		BIT(IIO_CHAN_INFO_SCALE) |
 		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
@@ -104,31 +112,42 @@ static int gyro_3d_read_raw(struct iio_dev *indio_dev,
 	u32 address;
 	int ret;
 	int ret_type;
+	s32 poll_value;
 
 	*val = 0;
 	*val2 = 0;
 	switch (mask) {
 	case 0:
+		poll_value = hid_sensor_read_poll_value(
+					&gyro_state->common_attributes);
+		if (poll_value < 0)
+			return -EINVAL;
+
+		hid_sensor_power_state(&gyro_state->common_attributes, true);
+		msleep_interruptible(poll_value * 2);
 		report_id = gyro_state->gyro[chan->scan_index].report_id;
 		address = gyro_3d_addresses[chan->scan_index];
 		if (report_id >= 0)
 			*val = sensor_hub_input_attr_get_raw_value(
-				gyro_state->common_attributes.hsdev,
-				HID_USAGE_SENSOR_GYRO_3D, address,
-				report_id);
+					gyro_state->common_attributes.hsdev,
+					HID_USAGE_SENSOR_GYRO_3D, address,
+					report_id);
 		else {
 			*val = 0;
+			hid_sensor_power_state(&gyro_state->common_attributes,
+						false);
 			return -EINVAL;
 		}
+		hid_sensor_power_state(&gyro_state->common_attributes, false);
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SCALE:
-		*val = gyro_state->gyro[CHANNEL_SCAN_INDEX_X].units;
-		ret_type = IIO_VAL_INT;
+		*val = gyro_state->scale_pre_decml;
+		*val2 = gyro_state->scale_post_decml;
+		ret_type = gyro_state->scale_precision;
 		break;
 	case IIO_CHAN_INFO_OFFSET:
-		*val = hid_sensor_convert_exponent(
-			gyro_state->gyro[CHANNEL_SCAN_INDEX_X].unit_expo);
+		*val = gyro_state->value_offset;
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SAMP_FREQ:
@@ -197,9 +216,8 @@ static int gyro_3d_proc_event(struct hid_sensor_hub_device *hsdev,
 	struct iio_dev *indio_dev = platform_get_drvdata(priv);
 	struct gyro_3d_state *gyro_state = iio_priv(indio_dev);
 
-	dev_dbg(&indio_dev->dev, "gyro_3d_proc_event [%d]\n",
-				gyro_state->common_attributes.data_ready);
-	if (gyro_state->common_attributes.data_ready)
+	dev_dbg(&indio_dev->dev, "gyro_3d_proc_event\n");
+	if (atomic_read(&gyro_state->common_attributes.data_ready))
 		hid_sensor_push_data(indio_dev,
 				gyro_state->gyro_val,
 				sizeof(gyro_state->gyro_val));
@@ -262,6 +280,11 @@ static int gyro_3d_parse_report(struct platform_device *pdev,
 			st->gyro[1].index, st->gyro[1].report_id,
 			st->gyro[2].index, st->gyro[2].report_id);
 
+	st->scale_precision = hid_sensor_format_scale(
+				HID_USAGE_SENSOR_GYRO_3D,
+				&st->gyro[CHANNEL_SCAN_INDEX_X],
+				&st->scale_pre_decml, &st->scale_post_decml);
+
 	/* Set Sensitivity field ids, when there is no individual modifier */
 	if (st->common_attributes.sensitivity.index < 0) {
 		sensor_hub_input_get_attribute_info(hsdev,
@@ -330,7 +353,7 @@ static int hid_gyro_3d_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
 		goto error_free_dev_mem;
 	}
-	gyro_state->common_attributes.data_ready = false;
+	atomic_set(&gyro_state->common_attributes.data_ready, 0);
 	ret = hid_sensor_setup_trigger(indio_dev, name,
 					&gyro_state->common_attributes);
 	if (ret < 0) {
diff --git a/drivers/iio/gyro/itg3200_core.c b/drivers/iio/gyro/itg3200_core.c
index 4d3f3b92b36..8295e318399 100644
--- a/drivers/iio/gyro/itg3200_core.c
+++ b/drivers/iio/gyro/itg3200_core.c
@@ -110,8 +110,6 @@ static int itg3200_read_raw(struct iio_dev *indio_dev,
 	default:
 		return -EINVAL;
 	}
-
-	return ret;
 }
 
 static ssize_t itg3200_read_frequency(struct device *dev,
diff --git a/drivers/iio/gyro/st_gyro_core.c b/drivers/iio/gyro/st_gyro_core.c
index a8e174a47bc..ed74a906998 100644
--- a/drivers/iio/gyro/st_gyro_core.c
+++ b/drivers/iio/gyro/st_gyro_core.c
@@ -311,6 +311,8 @@ int st_gyro_common_probe(struct iio_dev *indio_dev,
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->info = &gyro_info;
 
+	st_sensors_power_enable(indio_dev);
+
 	err = st_sensors_check_device_support(indio_dev,
 				ARRAY_SIZE(st_gyro_sensors), st_gyro_sensors);
 	if (err < 0)
@@ -344,6 +346,9 @@ int st_gyro_common_probe(struct iio_dev *indio_dev,
 	if (err)
 		goto st_gyro_device_register_error;
 
+	dev_info(&indio_dev->dev, "registered gyroscope %s\n",
+		 indio_dev->name);
+
 	return 0;
 
 st_gyro_device_register_error:
@@ -360,6 +365,8 @@ void st_gyro_common_remove(struct iio_dev *indio_dev)
 {
 	struct st_sensor_data *gdata = iio_priv(indio_dev);
 
+	st_sensors_power_disable(indio_dev);
+
 	iio_device_unregister(indio_dev);
 	if (gdata->get_irq_data_ready(indio_dev) > 0)
 		st_sensors_deallocate_trigger(indio_dev);
diff --git a/drivers/iio/iio_core.h b/drivers/iio/iio_core.h
index f6db6af36ba..5f0ea77fe71 100644
--- a/drivers/iio/iio_core.h
+++ b/drivers/iio/iio_core.h
@@ -35,7 +35,7 @@ int __iio_add_chan_devattr(const char *postfix,
 			   struct list_head *attr_list);
 void iio_free_chan_devattr_list(struct list_head *attr_list);
 
-ssize_t iio_format_value(char *buf, unsigned int type, int val, int val2);
+ssize_t iio_format_value(char *buf, unsigned int type, int size, int *vals);
 
 /* Event interface flags */
 #define IIO_BUSY_BIT_POS 1
diff --git a/drivers/iio/imu/inv_mpu6050/Kconfig b/drivers/iio/imu/inv_mpu6050/Kconfig
index 361b2328453..2d0608ba88d 100644
--- a/drivers/iio/imu/inv_mpu6050/Kconfig
+++ b/drivers/iio/imu/inv_mpu6050/Kconfig
@@ -9,6 +9,8 @@ config INV_MPU6050_IIO
 	select IIO_TRIGGERED_BUFFER
 	help
 	  This driver supports the Invensense MPU6050 devices.
+	  This driver can also support MPU6500 in MPU6050 compatibility mode
+	  and also in MPU6500 mode with some limitations.
 	  It is a gyroscope/accelerometer combo device.
 	  This driver can be built as a module. The module will be called
 	  inv-mpu6050.
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index d8ad606c7cd..0c6517c94a9 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -767,6 +767,7 @@ static SIMPLE_DEV_PM_OPS(inv_mpu_pmops, inv_mpu_suspend, inv_mpu_resume);
  */
 static const struct i2c_device_id inv_mpu_id[] = {
 	{"mpu6050", INV_MPU6050},
+	{"mpu6500", INV_MPU6500},
 	{}
 };
 
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index 0ab382be1e6..e7799315d4d 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -59,6 +59,7 @@ struct inv_mpu6050_reg_map {
 /*device enum */
 enum inv_devices {
 	INV_MPU6050,
+	INV_MPU6500,
 	INV_NUM_PARTS
 };
 
diff --git a/drivers/iio/industrialio-buffer.c b/drivers/iio/industrialio-buffer.c
index e472cff6eea..36b1ae92e23 100644
--- a/drivers/iio/industrialio-buffer.c
+++ b/drivers/iio/industrialio-buffer.c
@@ -150,7 +150,16 @@ static ssize_t iio_show_fixed_type(struct device *dev,
 		type = IIO_BE;
 #endif
 	}
-	return sprintf(buf, "%s:%c%d/%d>>%u\n",
+	if (this_attr->c->scan_type.repeat > 1)
+		return sprintf(buf, "%s:%c%d/%dX%d>>%u\n",
+		       iio_endian_prefix[type],
+		       this_attr->c->scan_type.sign,
+		       this_attr->c->scan_type.realbits,
+		       this_attr->c->scan_type.storagebits,
+		       this_attr->c->scan_type.repeat,
+		       this_attr->c->scan_type.shift);
+		else
+			return sprintf(buf, "%s:%c%d/%d>>%u\n",
 		       iio_endian_prefix[type],
 		       this_attr->c->scan_type.sign,
 		       this_attr->c->scan_type.realbits,
@@ -475,14 +484,22 @@ static int iio_compute_scan_bytes(struct iio_dev *indio_dev,
 	for_each_set_bit(i, mask,
 			 indio_dev->masklength) {
 		ch = iio_find_channel_from_si(indio_dev, i);
-		length = ch->scan_type.storagebits / 8;
+		if (ch->scan_type.repeat > 1)
+			length = ch->scan_type.storagebits / 8 *
+				ch->scan_type.repeat;
+		else
+			length = ch->scan_type.storagebits / 8;
 		bytes = ALIGN(bytes, length);
 		bytes += length;
 	}
 	if (timestamp) {
 		ch = iio_find_channel_from_si(indio_dev,
 					      indio_dev->scan_index_timestamp);
-		length = ch->scan_type.storagebits / 8;
+		if (ch->scan_type.repeat > 1)
+			length = ch->scan_type.storagebits / 8 *
+				ch->scan_type.repeat;
+		else
+			length = ch->scan_type.storagebits / 8;
 		bytes = ALIGN(bytes, length);
 		bytes += length;
 	}
@@ -959,7 +976,11 @@ static int iio_buffer_update_demux(struct iio_dev *indio_dev,
 					       indio_dev->masklength,
 					       in_ind + 1);
 			ch = iio_find_channel_from_si(indio_dev, in_ind);
-			length = ch->scan_type.storagebits/8;
+			if (ch->scan_type.repeat > 1)
+				length = ch->scan_type.storagebits / 8 *
+					ch->scan_type.repeat;
+			else
+				length = ch->scan_type.storagebits / 8;
 			/* Make sure we are aligned */
 			in_loc += length;
 			if (in_loc % length)
@@ -971,7 +992,11 @@ static int iio_buffer_update_demux(struct iio_dev *indio_dev,
 			goto error_clear_mux_table;
 		}
 		ch = iio_find_channel_from_si(indio_dev, in_ind);
-		length = ch->scan_type.storagebits/8;
+		if (ch->scan_type.repeat > 1)
+			length = ch->scan_type.storagebits / 8 *
+				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)
@@ -992,7 +1017,11 @@ static int iio_buffer_update_demux(struct iio_dev *indio_dev,
 		}
 		ch = iio_find_channel_from_si(indio_dev,
 			indio_dev->scan_index_timestamp);
-		length = ch->scan_type.storagebits/8;
+		if (ch->scan_type.repeat > 1)
+			length = ch->scan_type.storagebits / 8 *
+				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)
diff --git a/drivers/iio/industrialio-core.c b/drivers/iio/industrialio-core.c
index ede16aec20f..4b1f375c565 100644
--- a/drivers/iio/industrialio-core.c
+++ b/drivers/iio/industrialio-core.c
@@ -84,6 +84,9 @@ static const char * const iio_modifier_names[] = {
 	[IIO_MOD_LIGHT_RED] = "red",
 	[IIO_MOD_LIGHT_GREEN] = "green",
 	[IIO_MOD_LIGHT_BLUE] = "blue",
+	[IIO_MOD_QUATERNION] = "quaternion",
+	[IIO_MOD_TEMP_AMBIENT] = "ambient",
+	[IIO_MOD_TEMP_OBJECT] = "object",
 };
 
 /* relies on pairs of these shared then separate */
@@ -340,7 +343,7 @@ ssize_t iio_enum_read(struct iio_dev *indio_dev,
 	else if (i >= e->num_items)
 		return -EINVAL;
 
-	return sprintf(buf, "%s\n", e->items[i]);
+	return snprintf(buf, PAGE_SIZE, "%s\n", e->items[i]);
 }
 EXPORT_SYMBOL_GPL(iio_enum_read);
 
@@ -373,41 +376,53 @@ EXPORT_SYMBOL_GPL(iio_enum_write);
  * @buf: The buffer to which the formated value gets written
  * @type: One of the IIO_VAL_... constants. This decides how the val and val2
  *        parameters are formatted.
- * @val: First part of the value, exact meaning depends on the type parameter.
- * @val2: Second part of the value, exact meaning depends on the type parameter.
+ * @vals: pointer to the values, exact meaning depends on the type parameter.
  */
-ssize_t iio_format_value(char *buf, unsigned int type, int val, int val2)
+ssize_t iio_format_value(char *buf, unsigned int type, int size, int *vals)
 {
 	unsigned long long tmp;
 	bool scale_db = false;
 
 	switch (type) {
 	case IIO_VAL_INT:
-		return sprintf(buf, "%d\n", val);
+		return sprintf(buf, "%d\n", vals[0]);
 	case IIO_VAL_INT_PLUS_MICRO_DB:
 		scale_db = true;
 	case IIO_VAL_INT_PLUS_MICRO:
-		if (val2 < 0)
-			return sprintf(buf, "-%ld.%06u%s\n", abs(val), -val2,
+		if (vals[1] < 0)
+			return sprintf(buf, "-%ld.%06u%s\n", abs(vals[0]),
+					-vals[1],
 				scale_db ? " dB" : "");
 		else
-			return sprintf(buf, "%d.%06u%s\n", val, val2,
+			return sprintf(buf, "%d.%06u%s\n", vals[0], vals[1],
 				scale_db ? " dB" : "");
 	case IIO_VAL_INT_PLUS_NANO:
-		if (val2 < 0)
-			return sprintf(buf, "-%ld.%09u\n", abs(val), -val2);
+		if (vals[1] < 0)
+			return sprintf(buf, "-%ld.%09u\n", abs(vals[0]),
+					-vals[1]);
 		else
-			return sprintf(buf, "%d.%09u\n", val, val2);
+			return sprintf(buf, "%d.%09u\n", vals[0], vals[1]);
 	case IIO_VAL_FRACTIONAL:
-		tmp = div_s64((s64)val * 1000000000LL, val2);
-		val2 = do_div(tmp, 1000000000LL);
-		val = tmp;
-		return sprintf(buf, "%d.%09u\n", val, val2);
+		tmp = div_s64((s64)vals[0] * 1000000000LL, vals[1]);
+		vals[1] = do_div(tmp, 1000000000LL);
+		vals[0] = tmp;
+		return sprintf(buf, "%d.%09u\n", vals[0], vals[1]);
 	case IIO_VAL_FRACTIONAL_LOG2:
-		tmp = (s64)val * 1000000000LL >> val2;
-		val2 = do_div(tmp, 1000000000LL);
-		val = tmp;
-		return sprintf(buf, "%d.%09u\n", val, val2);
+		tmp = (s64)vals[0] * 1000000000LL >> vals[1];
+		vals[1] = do_div(tmp, 1000000000LL);
+		vals[0] = tmp;
+		return sprintf(buf, "%d.%09u\n", vals[0], vals[1]);
+	case IIO_VAL_INT_MULTIPLE:
+	{
+		int i;
+		int len = 0;
+
+		for (i = 0; i < size; ++i)
+			len += snprintf(&buf[len], PAGE_SIZE - len, "%d ",
+								vals[i]);
+		len += snprintf(&buf[len], PAGE_SIZE - len, "\n");
+		return len;
+	}
 	default:
 		return 0;
 	}
@@ -419,14 +434,23 @@ static ssize_t iio_read_channel_info(struct device *dev,
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
-	int val, val2;
-	int ret = indio_dev->info->read_raw(indio_dev, this_attr->c,
-					    &val, &val2, this_attr->address);
+	int vals[INDIO_MAX_RAW_ELEMENTS];
+	int ret;
+	int val_len = 2;
+
+	if (indio_dev->info->read_raw_multi)
+		ret = indio_dev->info->read_raw_multi(indio_dev, this_attr->c,
+							INDIO_MAX_RAW_ELEMENTS,
+							vals, &val_len,
+							this_attr->address);
+	else
+		ret = indio_dev->info->read_raw(indio_dev, this_attr->c,
+				    &vals[0], &vals[1], this_attr->address);
 
 	if (ret < 0)
 		return ret;
 
-	return iio_format_value(buf, ret, val, val2);
+	return iio_format_value(buf, ret, val_len, vals);
 }
 
 /**
@@ -716,6 +740,8 @@ static int iio_device_add_info_mask_type(struct iio_dev *indio_dev,
 	int i, ret, attrcount = 0;
 
 	for_each_set_bit(i, infomask, sizeof(infomask)*8) {
+		if (i >= ARRAY_SIZE(iio_chan_info_postfix))
+			return -EINVAL;
 		ret = __iio_add_chan_devattr(iio_chan_info_postfix[i],
 					     chan,
 					     &iio_read_channel_info,
@@ -820,7 +846,7 @@ static ssize_t iio_show_dev_name(struct device *dev,
 				 char *buf)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
-	return sprintf(buf, "%s\n", indio_dev->name);
+	return snprintf(buf, PAGE_SIZE, "%s\n", indio_dev->name);
 }
 
 static DEVICE_ATTR(name, S_IRUGO, iio_show_dev_name, NULL);
diff --git a/drivers/iio/industrialio-event.c b/drivers/iio/industrialio-event.c
index ea6e06b9c7d..258a973a1fb 100644
--- a/drivers/iio/industrialio-event.c
+++ b/drivers/iio/industrialio-event.c
@@ -270,7 +270,7 @@ static ssize_t iio_ev_value_show(struct device *dev,
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
-	int val, val2;
+	int val, val2, val_arr[2];
 	int ret;
 
 	ret = indio_dev->info->read_event_value(indio_dev,
@@ -279,7 +279,9 @@ static ssize_t iio_ev_value_show(struct device *dev,
 		&val, &val2);
 	if (ret < 0)
 		return ret;
-	return iio_format_value(buf, ret, val, val2);
+	val_arr[0] = val;
+	val_arr[1] = val2;
+	return iio_format_value(buf, ret, 2, val_arr);
 }
 
 static ssize_t iio_ev_value_store(struct device *dev,
@@ -321,7 +323,9 @@ static int iio_device_add_event(struct iio_dev *indio_dev,
 	char *postfix;
 	int ret;
 
-	for_each_set_bit(i, mask, sizeof(*mask)) {
+	for_each_set_bit(i, mask, sizeof(*mask)*8) {
+		if (i >= ARRAY_SIZE(iio_ev_info_text))
+			return -EINVAL;
 		postfix = kasprintf(GFP_KERNEL, "%s_%s_%s",
 				iio_ev_type_text[type], iio_ev_dir_text[dir],
 				iio_ev_info_text[i]);
diff --git a/drivers/iio/inkern.c b/drivers/iio/inkern.c
index 0cf5f8e06cf..d833d55052e 100644
--- a/drivers/iio/inkern.c
+++ b/drivers/iio/inkern.c
@@ -417,12 +417,24 @@ static int iio_channel_read(struct iio_channel *chan, int *val, int *val2,
 	enum iio_chan_info_enum info)
 {
 	int unused;
+	int vals[INDIO_MAX_RAW_ELEMENTS];
+	int ret;
+	int val_len = 2;
 
 	if (val2 == NULL)
 		val2 = &unused;
 
-	return chan->indio_dev->info->read_raw(chan->indio_dev, chan->channel,
-						val, val2, info);
+	if (chan->indio_dev->info->read_raw_multi) {
+		ret = chan->indio_dev->info->read_raw_multi(chan->indio_dev,
+					chan->channel, INDIO_MAX_RAW_ELEMENTS,
+					vals, &val_len, info);
+		*val = vals[0];
+		*val2 = vals[1];
+	} else
+		ret = chan->indio_dev->info->read_raw(chan->indio_dev,
+					chan->channel, val, val2, info);
+
+	return ret;
 }
 
 int iio_read_channel_raw(struct iio_channel *chan, int *val)
@@ -443,6 +455,24 @@ err_unlock:
 }
 EXPORT_SYMBOL_GPL(iio_read_channel_raw);
 
+int iio_read_channel_average_raw(struct iio_channel *chan, int *val)
+{
+	int ret;
+
+	mutex_lock(&chan->indio_dev->info_exist_lock);
+	if (chan->indio_dev->info == NULL) {
+		ret = -ENODEV;
+		goto err_unlock;
+	}
+
+	ret = iio_channel_read(chan, val, NULL, IIO_CHAN_INFO_AVERAGE_RAW);
+err_unlock:
+	mutex_unlock(&chan->indio_dev->info_exist_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(iio_read_channel_average_raw);
+
 static int iio_convert_raw_to_processed_unlocked(struct iio_channel *chan,
 	int raw, int *processed, unsigned int scale)
 {
diff --git a/drivers/iio/light/gp2ap020a00f.c b/drivers/iio/light/gp2ap020a00f.c
index 5ea4a03c7e7..04bdb85d2d9 100644
--- a/drivers/iio/light/gp2ap020a00f.c
+++ b/drivers/iio/light/gp2ap020a00f.c
@@ -5,13 +5,13 @@
  * IIO features supported by the driver:
  *
  * Read-only raw channels:
- *   - illiminance_clear [lux]
- *   - illiminance_ir
+ *   - illuminance_clear [lux]
+ *   - illuminance_ir
  *   - proximity
  *
  * Triggered buffer:
- *   - illiminance_clear
- *   - illiminance_ir
+ *   - illuminance_clear
+ *   - illuminance_ir
  *   - proximity
  *
  * Events:
diff --git a/drivers/iio/light/hid-sensor-als.c b/drivers/iio/light/hid-sensor-als.c
index 621541fb10a..f34c94380b4 100644
--- a/drivers/iio/light/hid-sensor-als.c
+++ b/drivers/iio/light/hid-sensor-als.c
@@ -22,6 +22,7 @@
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/slab.h>
+#include <linux/delay.h>
 #include <linux/hid-sensor-hub.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -37,6 +38,10 @@ struct als_state {
 	struct hid_sensor_common common_attributes;
 	struct hid_sensor_hub_attribute_info als_illum;
 	u32 illum;
+	int scale_pre_decml;
+	int scale_post_decml;
+	int scale_precision;
+	int value_offset;
 };
 
 /* Channel definitions */
@@ -45,6 +50,7 @@ static const struct iio_chan_spec als_channels[] = {
 		.type = IIO_INTENSITY,
 		.modified = 1,
 		.channel2 = IIO_MOD_LIGHT_BOTH,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
 		BIT(IIO_CHAN_INFO_SCALE) |
 		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
@@ -75,6 +81,7 @@ static int als_read_raw(struct iio_dev *indio_dev,
 	u32 address;
 	int ret;
 	int ret_type;
+	s32 poll_value;
 
 	*val = 0;
 	*val2 = 0;
@@ -90,24 +97,35 @@ static int als_read_raw(struct iio_dev *indio_dev,
 			report_id = -1;
 			break;
 		}
-		if (report_id >= 0)
+		if (report_id >= 0) {
+			poll_value = hid_sensor_read_poll_value(
+						&als_state->common_attributes);
+			if (poll_value < 0)
+				return -EINVAL;
+
+			hid_sensor_power_state(&als_state->common_attributes,
+						true);
+			msleep_interruptible(poll_value * 2);
+
 			*val = sensor_hub_input_attr_get_raw_value(
-				als_state->common_attributes.hsdev,
-				HID_USAGE_SENSOR_ALS, address,
-				report_id);
-		else {
+					als_state->common_attributes.hsdev,
+					HID_USAGE_SENSOR_ALS, address,
+					report_id);
+			hid_sensor_power_state(&als_state->common_attributes,
+						false);
+		} else {
 			*val = 0;
 			return -EINVAL;
 		}
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SCALE:
-		*val = als_state->als_illum.units;
-		ret_type = IIO_VAL_INT;
+		*val = als_state->scale_pre_decml;
+		*val2 = als_state->scale_post_decml;
+		ret_type = als_state->scale_precision;
 		break;
 	case IIO_CHAN_INFO_OFFSET:
-		*val = hid_sensor_convert_exponent(
-				als_state->als_illum.unit_expo);
+		*val = als_state->value_offset;
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SAMP_FREQ:
@@ -176,9 +194,8 @@ static int als_proc_event(struct hid_sensor_hub_device *hsdev,
 	struct iio_dev *indio_dev = platform_get_drvdata(priv);
 	struct als_state *als_state = iio_priv(indio_dev);
 
-	dev_dbg(&indio_dev->dev, "als_proc_event [%d]\n",
-				als_state->common_attributes.data_ready);
-	if (als_state->common_attributes.data_ready)
+	dev_dbg(&indio_dev->dev, "als_proc_event\n");
+	if (atomic_read(&als_state->common_attributes.data_ready))
 		hid_sensor_push_data(indio_dev,
 				&als_state->illum,
 				sizeof(als_state->illum));
@@ -229,6 +246,11 @@ static int als_parse_report(struct platform_device *pdev,
 	dev_dbg(&pdev->dev, "als %x:%x\n", st->als_illum.index,
 			st->als_illum.report_id);
 
+	st->scale_precision = hid_sensor_format_scale(
+				HID_USAGE_SENSOR_ALS,
+				&st->als_illum,
+				&st->scale_pre_decml, &st->scale_post_decml);
+
 	/* Set Sensitivity field ids, when there is no individual modifier */
 	if (st->common_attributes.sensitivity.index < 0) {
 		sensor_hub_input_get_attribute_info(hsdev,
@@ -296,7 +318,7 @@ static int hid_als_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
 		goto error_free_dev_mem;
 	}
-	als_state->common_attributes.data_ready = false;
+	atomic_set(&als_state->common_attributes.data_ready, 0);
 	ret = hid_sensor_setup_trigger(indio_dev, name,
 				&als_state->common_attributes);
 	if (ret < 0) {
diff --git a/drivers/iio/light/hid-sensor-prox.c b/drivers/iio/light/hid-sensor-prox.c
index 1894ab196f9..d203ef4d892 100644
--- a/drivers/iio/light/hid-sensor-prox.c
+++ b/drivers/iio/light/hid-sensor-prox.c
@@ -21,6 +21,7 @@
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/slab.h>
+#include <linux/delay.h>
 #include <linux/hid-sensor-hub.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -75,6 +76,7 @@ static int prox_read_raw(struct iio_dev *indio_dev,
 	u32 address;
 	int ret;
 	int ret_type;
+	s32 poll_value;
 
 	*val = 0;
 	*val2 = 0;
@@ -90,12 +92,24 @@ static int prox_read_raw(struct iio_dev *indio_dev,
 			report_id = -1;
 			break;
 		}
-		if (report_id >= 0)
+		if (report_id >= 0) {
+			poll_value = hid_sensor_read_poll_value(
+					&prox_state->common_attributes);
+			if (poll_value < 0)
+				return -EINVAL;
+
+			hid_sensor_power_state(&prox_state->common_attributes,
+						true);
+
+			msleep_interruptible(poll_value * 2);
+
 			*val = sensor_hub_input_attr_get_raw_value(
 				prox_state->common_attributes.hsdev,
 				HID_USAGE_SENSOR_PROX, address,
 				report_id);
-		else {
+			hid_sensor_power_state(&prox_state->common_attributes,
+						false);
+		} else {
 			*val = 0;
 			return -EINVAL;
 		}
@@ -176,9 +190,8 @@ static int prox_proc_event(struct hid_sensor_hub_device *hsdev,
 	struct iio_dev *indio_dev = platform_get_drvdata(priv);
 	struct prox_state *prox_state = iio_priv(indio_dev);
 
-	dev_dbg(&indio_dev->dev, "prox_proc_event [%d]\n",
-				prox_state->common_attributes.data_ready);
-	if (prox_state->common_attributes.data_ready)
+	dev_dbg(&indio_dev->dev, "prox_proc_event\n");
+	if (atomic_read(&prox_state->common_attributes.data_ready))
 		hid_sensor_push_data(indio_dev,
 				&prox_state->human_presence,
 				sizeof(prox_state->human_presence));
@@ -297,7 +310,7 @@ static int hid_prox_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
 		goto error_free_dev_mem;
 	}
-	prox_state->common_attributes.data_ready = false;
+	atomic_set(&prox_state->common_attributes.data_ready, 0);
 	ret = hid_sensor_setup_trigger(indio_dev, name,
 				&prox_state->common_attributes);
 	if (ret) {
diff --git a/drivers/iio/magnetometer/Kconfig b/drivers/iio/magnetometer/Kconfig
index d86d226dcd6..05a364c543f 100644
--- a/drivers/iio/magnetometer/Kconfig
+++ b/drivers/iio/magnetometer/Kconfig
@@ -11,7 +11,8 @@ config AK8975
 	depends on GPIOLIB
 	help
 	  Say yes here to build support for Asahi Kasei AK8975 3-Axis
-	  Magnetometer.
+	  Magnetometer. This driver can also support AK8963, if i2c
+	  device name is identified as ak8963.
 
 	  To compile this driver as a module, choose M here: the module
 	  will be called ak8975.
diff --git a/drivers/iio/magnetometer/ak8975.c b/drivers/iio/magnetometer/ak8975.c
index 74866d1efd1..09ea5c481f4 100644
--- a/drivers/iio/magnetometer/ak8975.c
+++ b/drivers/iio/magnetometer/ak8975.c
@@ -31,6 +31,7 @@
 #include <linux/bitops.h>
 #include <linux/gpio.h>
 #include <linux/of_gpio.h>
+#include <linux/acpi.h>
 
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -85,7 +86,14 @@
 #define AK8975_MAX_CONVERSION_TIMEOUT	500
 #define AK8975_CONVERSION_DONE_POLL_TIME 10
 #define AK8975_DATA_READY_TIMEOUT	((100*HZ)/1000)
-#define RAW_TO_GAUSS(asa) ((((asa) + 128) * 3000) / 256)
+#define RAW_TO_GAUSS_8975(asa) ((((asa) + 128) * 3000) / 256)
+#define RAW_TO_GAUSS_8963(asa) ((((asa) + 128) * 6000) / 256)
+
+/* Compatible Asahi Kasei Compass parts */
+enum asahi_compass_chipset {
+	AK8975,
+	AK8963,
+};
 
 /*
  * Per-instance context data for the device.
@@ -101,6 +109,7 @@ struct ak8975_data {
 	int			eoc_irq;
 	wait_queue_head_t	data_ready_queue;
 	unsigned long		flags;
+	enum asahi_compass_chipset chipset;
 };
 
 static const int ak8975_index_to_reg[] = {
@@ -272,9 +281,21 @@ static int ak8975_setup(struct i2c_client *client)
  * Since ASA doesn't change, we cache the resultant scale factor into the
  * device context in ak8975_setup().
  */
-	data->raw_to_gauss[0] = RAW_TO_GAUSS(data->asa[0]);
-	data->raw_to_gauss[1] = RAW_TO_GAUSS(data->asa[1]);
-	data->raw_to_gauss[2] = RAW_TO_GAUSS(data->asa[2]);
+	if (data->chipset == AK8963) {
+		/*
+		 * H range is +-8190 and magnetometer range is +-4912.
+		 * So HuT using the above explanation for 8975,
+		 * 4912/8190 = ~ 6/10.
+		 * So the Hadj should use 6/10 instead of 3/10.
+		 */
+		data->raw_to_gauss[0] = RAW_TO_GAUSS_8963(data->asa[0]);
+		data->raw_to_gauss[1] = RAW_TO_GAUSS_8963(data->asa[1]);
+		data->raw_to_gauss[2] = RAW_TO_GAUSS_8963(data->asa[2]);
+	} else {
+		data->raw_to_gauss[0] = RAW_TO_GAUSS_8975(data->asa[0]);
+		data->raw_to_gauss[1] = RAW_TO_GAUSS_8975(data->asa[1]);
+		data->raw_to_gauss[2] = RAW_TO_GAUSS_8975(data->asa[2]);
+	}
 
 	return 0;
 }
@@ -455,6 +476,27 @@ static const struct iio_info ak8975_info = {
 	.driver_module = THIS_MODULE,
 };
 
+static const struct acpi_device_id ak_acpi_match[] = {
+	{"AK8975", AK8975},
+	{"AK8963", AK8963},
+	{"INVN6500", AK8963},
+	{ },
+};
+MODULE_DEVICE_TABLE(acpi, ak_acpi_match);
+
+static char *ak8975_match_acpi_device(struct device *dev,
+				enum asahi_compass_chipset *chipset)
+{
+	const struct acpi_device_id *id;
+
+	id = acpi_match_device(dev->driver->acpi_match_table, dev);
+	if (!id)
+		return NULL;
+	*chipset = (int)id->driver_data;
+
+	return (char *)dev_name(dev);
+}
+
 static int ak8975_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
@@ -462,6 +504,7 @@ static int ak8975_probe(struct i2c_client *client,
 	struct iio_dev *indio_dev;
 	int eoc_gpio;
 	int err;
+	char *name = NULL;
 
 	/* Grab and set up the supplied GPIO. */
 	if (client->dev.platform_data)
@@ -499,6 +542,19 @@ static int ak8975_probe(struct i2c_client *client,
 	data->eoc_gpio = eoc_gpio;
 	data->eoc_irq = 0;
 
+	/* id will be NULL when enumerated via ACPI */
+	if (id) {
+		data->chipset =
+			(enum asahi_compass_chipset)(id->driver_data);
+		name = (char *) id->name;
+	} else if (ACPI_HANDLE(&client->dev))
+		name = ak8975_match_acpi_device(&client->dev, &data->chipset);
+	else {
+		err = -ENOSYS;
+		goto exit_free_iio;
+	}
+	dev_dbg(&client->dev, "Asahi compass chip %s\n", name);
+
 	/* Perform some basic start-of-day setup of the device. */
 	err = ak8975_setup(client);
 	if (err < 0) {
@@ -513,9 +569,8 @@ static int ak8975_probe(struct i2c_client *client,
 	indio_dev->channels = ak8975_channels;
 	indio_dev->num_channels = ARRAY_SIZE(ak8975_channels);
 	indio_dev->info = &ak8975_info;
-	indio_dev->name = id->name;
 	indio_dev->modes = INDIO_DIRECT_MODE;
-
+	indio_dev->name = name;
 	err = iio_device_register(indio_dev);
 	if (err < 0)
 		goto exit_free_iio;
@@ -552,7 +607,8 @@ static int ak8975_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id ak8975_id[] = {
-	{"ak8975", 0},
+	{"ak8975", AK8975},
+	{"ak8963", AK8963},
 	{}
 };
 
@@ -569,6 +625,7 @@ static struct i2c_driver ak8975_driver = {
 	.driver = {
 		.name	= "ak8975",
 		.of_match_table = ak8975_of_match,
+		.acpi_match_table = ACPI_PTR(ak_acpi_match),
 	},
 	.probe		= ak8975_probe,
 	.remove		= ak8975_remove,
diff --git a/drivers/iio/magnetometer/hid-sensor-magn-3d.c b/drivers/iio/magnetometer/hid-sensor-magn-3d.c
index 6d162b7e7af..41cf29e2a37 100644
--- a/drivers/iio/magnetometer/hid-sensor-magn-3d.c
+++ b/drivers/iio/magnetometer/hid-sensor-magn-3d.c
@@ -22,6 +22,7 @@
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/slab.h>
+#include <linux/delay.h>
 #include <linux/hid-sensor-hub.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -42,6 +43,10 @@ struct magn_3d_state {
 	struct hid_sensor_common common_attributes;
 	struct hid_sensor_hub_attribute_info magn[MAGN_3D_CHANNEL_MAX];
 	u32 magn_val[MAGN_3D_CHANNEL_MAX];
+	int scale_pre_decml;
+	int scale_post_decml;
+	int scale_precision;
+	int value_offset;
 };
 
 static const u32 magn_3d_addresses[MAGN_3D_CHANNEL_MAX] = {
@@ -56,6 +61,7 @@ static const struct iio_chan_spec magn_3d_channels[] = {
 		.type = IIO_MAGN,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
 		BIT(IIO_CHAN_INFO_SCALE) |
 		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
@@ -65,6 +71,7 @@ static const struct iio_chan_spec magn_3d_channels[] = {
 		.type = IIO_MAGN,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
 		BIT(IIO_CHAN_INFO_SCALE) |
 		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
@@ -74,6 +81,7 @@ static const struct iio_chan_spec magn_3d_channels[] = {
 		.type = IIO_MAGN,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
 		BIT(IIO_CHAN_INFO_SCALE) |
 		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
@@ -104,11 +112,20 @@ static int magn_3d_read_raw(struct iio_dev *indio_dev,
 	u32 address;
 	int ret;
 	int ret_type;
+	s32 poll_value;
 
 	*val = 0;
 	*val2 = 0;
 	switch (mask) {
 	case 0:
+		poll_value = hid_sensor_read_poll_value(
+					&magn_state->common_attributes);
+		if (poll_value < 0)
+				return -EINVAL;
+
+		hid_sensor_power_state(&magn_state->common_attributes, true);
+		msleep_interruptible(poll_value * 2);
+
 		report_id =
 			magn_state->magn[chan->scan_index].report_id;
 		address = magn_3d_addresses[chan->scan_index];
@@ -119,17 +136,20 @@ static int magn_3d_read_raw(struct iio_dev *indio_dev,
 				report_id);
 		else {
 			*val = 0;
+			hid_sensor_power_state(&magn_state->common_attributes,
+						false);
 			return -EINVAL;
 		}
+		hid_sensor_power_state(&magn_state->common_attributes, false);
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SCALE:
-		*val = magn_state->magn[CHANNEL_SCAN_INDEX_X].units;
-		ret_type = IIO_VAL_INT;
+		*val = magn_state->scale_pre_decml;
+		*val2 = magn_state->scale_post_decml;
+		ret_type = magn_state->scale_precision;
 		break;
 	case IIO_CHAN_INFO_OFFSET:
-		*val = hid_sensor_convert_exponent(
-			magn_state->magn[CHANNEL_SCAN_INDEX_X].unit_expo);
+		*val = magn_state->value_offset;
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SAMP_FREQ:
@@ -198,9 +218,8 @@ static int magn_3d_proc_event(struct hid_sensor_hub_device *hsdev,
 	struct iio_dev *indio_dev = platform_get_drvdata(priv);
 	struct magn_3d_state *magn_state = iio_priv(indio_dev);
 
-	dev_dbg(&indio_dev->dev, "magn_3d_proc_event [%d]\n",
-				magn_state->common_attributes.data_ready);
-	if (magn_state->common_attributes.data_ready)
+	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->magn_val,
 				sizeof(magn_state->magn_val));
@@ -263,6 +282,11 @@ static int magn_3d_parse_report(struct platform_device *pdev,
 			st->magn[1].index, st->magn[1].report_id,
 			st->magn[2].index, st->magn[2].report_id);
 
+	st->scale_precision = hid_sensor_format_scale(
+				HID_USAGE_SENSOR_COMPASS_3D,
+				&st->magn[CHANNEL_SCAN_INDEX_X],
+				&st->scale_pre_decml, &st->scale_post_decml);
+
 	/* Set Sensitivity field ids, when there is no individual modifier */
 	if (st->common_attributes.sensitivity.index < 0) {
 		sensor_hub_input_get_attribute_info(hsdev,
@@ -334,7 +358,7 @@ static int hid_magn_3d_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
 		goto error_free_dev_mem;
 	}
-	magn_state->common_attributes.data_ready = false;
+	atomic_set(&magn_state->common_attributes.data_ready, 0);
 	ret = hid_sensor_setup_trigger(indio_dev, name,
 					&magn_state->common_attributes);
 	if (ret < 0) {
diff --git a/drivers/iio/magnetometer/mag3110.c b/drivers/iio/magnetometer/mag3110.c
index 8b77782474d..e3106b43ef4 100644
--- a/drivers/iio/magnetometer/mag3110.c
+++ b/drivers/iio/magnetometer/mag3110.c
@@ -199,6 +199,13 @@ static int mag3110_read_raw(struct iio_dev *indio_dev,
 		*val = mag3110_samp_freq[i][0];
 		*val2 = mag3110_samp_freq[i][1];
 		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_CALIBBIAS:
+		ret = i2c_smbus_read_word_swapped(data->client,
+			MAG3110_OFF_X +	2 * chan->scan_index);
+		if (ret < 0)
+			return ret;
+		*val = sign_extend32(ret >> 1, 14);
+		return IIO_VAL_INT;
 	}
 	return -EINVAL;
 }
@@ -223,6 +230,11 @@ static int mag3110_write_raw(struct iio_dev *indio_dev,
 		data->ctrl_reg1 |= rate << MAG3110_CTRL_DR_SHIFT;
 		return i2c_smbus_write_byte_data(data->client,
 			MAG3110_CTRL_REG1, data->ctrl_reg1);
+	case IIO_CHAN_INFO_CALIBBIAS:
+		if (val < -10000 || val > 10000)
+			return -EINVAL;
+		return i2c_smbus_write_word_swapped(data->client,
+			MAG3110_OFF_X + 2 * chan->scan_index, val << 1);
 	default:
 		return -EINVAL;
 	}
@@ -260,7 +272,8 @@ done:
 	.type = IIO_MAGN, \
 	.modified = 1, \
 	.channel2 = IIO_MOD_##axis, \
-	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+		BIT(IIO_CHAN_INFO_CALIBBIAS), \
 	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SAMP_FREQ) | \
 		BIT(IIO_CHAN_INFO_SCALE), \
 	.scan_index = idx, \
@@ -338,14 +351,14 @@ static int mag3110_probe(struct i2c_client *client,
 	indio_dev->num_channels = ARRAY_SIZE(mag3110_channels);
 	indio_dev->available_scan_masks = mag3110_scan_masks;
 
-	data->ctrl_reg1 = MAG3110_CTRL_DR_DEFAULT;
+	data->ctrl_reg1 = MAG3110_CTRL_DR_DEFAULT << MAG3110_CTRL_DR_SHIFT;
 	ret = i2c_smbus_write_byte_data(client, MAG3110_CTRL_REG1,
 		data->ctrl_reg1);
 	if (ret < 0)
 		return ret;
 
 	ret = i2c_smbus_write_byte_data(client, MAG3110_CTRL_REG2,
-		MAG3110_CTRL_AUTO_MRST_EN | MAG3110_CTRL_RAW);
+		MAG3110_CTRL_AUTO_MRST_EN);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/iio/magnetometer/st_magn_core.c b/drivers/iio/magnetometer/st_magn_core.c
index 52bbcfa1e07..240a21dd0c6 100644
--- a/drivers/iio/magnetometer/st_magn_core.c
+++ b/drivers/iio/magnetometer/st_magn_core.c
@@ -355,6 +355,8 @@ int st_magn_common_probe(struct iio_dev *indio_dev,
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->info = &magn_info;
 
+	st_sensors_power_enable(indio_dev);
+
 	err = st_sensors_check_device_support(indio_dev,
 				ARRAY_SIZE(st_magn_sensors), st_magn_sensors);
 	if (err < 0)
@@ -387,6 +389,9 @@ int st_magn_common_probe(struct iio_dev *indio_dev,
 	if (err)
 		goto st_magn_device_register_error;
 
+	dev_info(&indio_dev->dev, "registered magnetometer %s\n",
+		 indio_dev->name);
+
 	return 0;
 
 st_magn_device_register_error:
@@ -403,6 +408,8 @@ void st_magn_common_remove(struct iio_dev *indio_dev)
 {
 	struct st_sensor_data *mdata = iio_priv(indio_dev);
 
+	st_sensors_power_disable(indio_dev);
+
 	iio_device_unregister(indio_dev);
 	if (mdata->get_irq_data_ready(indio_dev) > 0)
 		st_sensors_deallocate_trigger(indio_dev);
diff --git a/drivers/iio/orientation/Kconfig b/drivers/iio/orientation/Kconfig
index 58c62c837e1..e3aa1e58d92 100644
--- a/drivers/iio/orientation/Kconfig
+++ b/drivers/iio/orientation/Kconfig
@@ -16,4 +16,16 @@ config HID_SENSOR_INCLINOMETER_3D
 	  Say yes here to build support for the HID SENSOR
 	  Inclinometer 3D.
 
+config HID_SENSOR_DEVICE_ROTATION
+	depends on HID_SENSOR_HUB
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	select HID_SENSOR_IIO_COMMON
+	select HID_SENSOR_IIO_TRIGGER
+	tristate "HID Device Rotation"
+	help
+	  Say yes here to build support for the HID SENSOR
+	  device rotation. The output of a device rotation sensor
+	  is presented using quaternion format.
+
 endmenu
diff --git a/drivers/iio/orientation/Makefile b/drivers/iio/orientation/Makefile
index 2c97572ee91..4734dabbde1 100644
--- a/drivers/iio/orientation/Makefile
+++ b/drivers/iio/orientation/Makefile
@@ -4,3 +4,4 @@
 
 # When adding new entries keep the list in alphabetical order
 obj-$(CONFIG_HID_SENSOR_INCLINOMETER_3D) += hid-sensor-incl-3d.o
+obj-$(CONFIG_HID_SENSOR_DEVICE_ROTATION) += hid-sensor-rotation.o
diff --git a/drivers/iio/orientation/hid-sensor-incl-3d.c b/drivers/iio/orientation/hid-sensor-incl-3d.c
index 070feab08fa..2478f6c2ef2 100644
--- a/drivers/iio/orientation/hid-sensor-incl-3d.c
+++ b/drivers/iio/orientation/hid-sensor-incl-3d.c
@@ -22,6 +22,7 @@
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/slab.h>
+#include <linux/delay.h>
 #include <linux/hid-sensor-hub.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -42,6 +43,10 @@ struct incl_3d_state {
 	struct hid_sensor_common common_attributes;
 	struct hid_sensor_hub_attribute_info incl[INCLI_3D_CHANNEL_MAX];
 	u32 incl_val[INCLI_3D_CHANNEL_MAX];
+	int scale_pre_decml;
+	int scale_post_decml;
+	int scale_precision;
+	int value_offset;
 };
 
 static const u32 incl_3d_addresses[INCLI_3D_CHANNEL_MAX] = {
@@ -106,11 +111,20 @@ static int incl_3d_read_raw(struct iio_dev *indio_dev,
 	int report_id = -1;
 	u32 address;
 	int ret_type;
+	s32 poll_value;
 
 	*val = 0;
 	*val2 = 0;
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW:
+		poll_value = hid_sensor_read_poll_value(
+					&incl_state->common_attributes);
+		if (poll_value < 0)
+			return -EINVAL;
+
+		hid_sensor_power_state(&incl_state->common_attributes, true);
+		msleep_interruptible(poll_value * 2);
+
 		report_id =
 			incl_state->incl[chan->scan_index].report_id;
 		address = incl_3d_addresses[chan->scan_index];
@@ -120,17 +134,20 @@ static int incl_3d_read_raw(struct iio_dev *indio_dev,
 				HID_USAGE_SENSOR_INCLINOMETER_3D, address,
 				report_id);
 		else {
+			hid_sensor_power_state(&incl_state->common_attributes,
+						false);
 			return -EINVAL;
 		}
+		hid_sensor_power_state(&incl_state->common_attributes, false);
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SCALE:
-		*val = incl_state->incl[CHANNEL_SCAN_INDEX_X].units;
-		ret_type = IIO_VAL_INT;
+		*val = incl_state->scale_pre_decml;
+		*val2 = incl_state->scale_post_decml;
+		ret_type = incl_state->scale_precision;
 		break;
 	case IIO_CHAN_INFO_OFFSET:
-		*val = hid_sensor_convert_exponent(
-			incl_state->incl[CHANNEL_SCAN_INDEX_X].unit_expo);
+		*val = incl_state->value_offset;
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SAMP_FREQ:
@@ -196,9 +213,8 @@ static int incl_3d_proc_event(struct hid_sensor_hub_device *hsdev,
 	struct iio_dev *indio_dev = platform_get_drvdata(priv);
 	struct incl_3d_state *incl_state = iio_priv(indio_dev);
 
-	dev_dbg(&indio_dev->dev, "incl_3d_proc_event [%d]\n",
-				incl_state->common_attributes.data_ready);
-	if (incl_state->common_attributes.data_ready)
+	dev_dbg(&indio_dev->dev, "incl_3d_proc_event\n");
+	if (atomic_read(&incl_state->common_attributes.data_ready))
 		hid_sensor_push_data(indio_dev,
 				(u8 *)incl_state->incl_val,
 				sizeof(incl_state->incl_val));
@@ -279,6 +295,11 @@ static int incl_3d_parse_report(struct platform_device *pdev,
 			st->incl[1].index, st->incl[1].report_id,
 			st->incl[2].index, st->incl[2].report_id);
 
+	st->scale_precision = hid_sensor_format_scale(
+				HID_USAGE_SENSOR_INCLINOMETER_3D,
+				&st->incl[CHANNEL_SCAN_INDEX_X],
+				&st->scale_pre_decml, &st->scale_post_decml);
+
 	/* Set Sensitivity field ids, when there is no individual modifier */
 	if (st->common_attributes.sensitivity.index < 0) {
 		sensor_hub_input_get_attribute_info(hsdev,
@@ -349,7 +370,7 @@ static int hid_incl_3d_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
 		goto error_free_dev_mem;
 	}
-	incl_state->common_attributes.data_ready = false;
+	atomic_set(&incl_state->common_attributes.data_ready, 0);
 	ret = hid_sensor_setup_trigger(indio_dev, name,
 					&incl_state->common_attributes);
 	if (ret) {
diff --git a/drivers/iio/orientation/hid-sensor-rotation.c b/drivers/iio/orientation/hid-sensor-rotation.c
new file mode 100644
index 00000000000..dccf848e8b0
--- /dev/null
+++ b/drivers/iio/orientation/hid-sensor-rotation.c
@@ -0,0 +1,346 @@
+/*
+ * HID Sensors 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/device.h>
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/slab.h>
+#include <linux/hid-sensor-hub.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
+#include "../common/hid-sensors/hid-sensor-trigger.h"
+
+struct dev_rot_state {
+	struct hid_sensor_hub_callbacks callbacks;
+	struct hid_sensor_common common_attributes;
+	struct hid_sensor_hub_attribute_info quaternion;
+	u32 sampled_vals[4];
+};
+
+/* Channel definitions */
+static const struct iio_chan_spec dev_rot_channels[] = {
+	{
+		.type = IIO_ROT,
+		.modified = 1,
+		.channel2 = IIO_MOD_QUATERNION,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+					BIT(IIO_CHAN_INFO_HYSTERESIS)
+	}
+};
+
+/* Adjust channel real bits based on report descriptor */
+static void dev_rot_adjust_channel_bit_mask(struct iio_chan_spec *chan,
+						int size)
+{
+	chan->scan_type.sign = 's';
+	/* Real storage bits will change based on the report desc. */
+	chan->scan_type.realbits = size * 8;
+	/* Maximum size of a sample to capture is u32 */
+	chan->scan_type.storagebits = sizeof(u32) * 8;
+	chan->scan_type.repeat = 4;
+}
+
+/* Channel read_raw handler */
+static int dev_rot_read_raw(struct iio_dev *indio_dev,
+				struct iio_chan_spec const *chan,
+				int size, int *vals, int *val_len,
+				long mask)
+{
+	struct dev_rot_state *rot_state = iio_priv(indio_dev);
+	int ret_type;
+	int i;
+
+	vals[0] = 0;
+	vals[1] = 0;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		if (size >= 4) {
+			for (i = 0; i < 4; ++i)
+				vals[i] = rot_state->sampled_vals[i];
+			ret_type = IIO_VAL_INT_MULTIPLE;
+			*val_len =  4;
+		} else
+			ret_type = -EINVAL;
+		break;
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		ret_type = hid_sensor_read_samp_freq_value(
+			&rot_state->common_attributes, &vals[0], &vals[1]);
+		break;
+	case IIO_CHAN_INFO_HYSTERESIS:
+		ret_type = hid_sensor_read_raw_hyst_value(
+			&rot_state->common_attributes, &vals[0], &vals[1]);
+		break;
+	default:
+		ret_type = -EINVAL;
+		break;
+	}
+
+	return ret_type;
+}
+
+/* Channel write_raw handler */
+static int dev_rot_write_raw(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       int val,
+			       int val2,
+			       long mask)
+{
+	struct dev_rot_state *rot_state = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		ret = hid_sensor_write_samp_freq_value(
+				&rot_state->common_attributes, val, val2);
+		break;
+	case IIO_CHAN_INFO_HYSTERESIS:
+		ret = hid_sensor_write_raw_hyst_value(
+				&rot_state->common_attributes, val, val2);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static const struct iio_info dev_rot_info = {
+	.driver_module = THIS_MODULE,
+	.read_raw_multi = &dev_rot_read_raw,
+	.write_raw = &dev_rot_write_raw,
+};
+
+/* Function to push data to buffer */
+static void hid_sensor_push_data(struct iio_dev *indio_dev, u8 *data, int len)
+{
+	dev_dbg(&indio_dev->dev, "hid_sensor_push_data >>\n");
+	iio_push_to_buffers(indio_dev, (u8 *)data);
+	dev_dbg(&indio_dev->dev, "hid_sensor_push_data <<\n");
+
+}
+
+/* Callback handler to send event after all samples are received and captured */
+static int dev_rot_proc_event(struct hid_sensor_hub_device *hsdev,
+				unsigned usage_id,
+				void *priv)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(priv);
+	struct dev_rot_state *rot_state = iio_priv(indio_dev);
+
+	dev_dbg(&indio_dev->dev, "dev_rot_proc_event\n");
+	if (atomic_read(&rot_state->common_attributes.data_ready))
+		hid_sensor_push_data(indio_dev,
+				(u8 *)rot_state->sampled_vals,
+				sizeof(rot_state->sampled_vals));
+
+	return 0;
+}
+
+/* Capture samples in local storage */
+static int dev_rot_capture_sample(struct hid_sensor_hub_device *hsdev,
+				unsigned usage_id,
+				size_t raw_len, char *raw_data,
+				void *priv)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(priv);
+	struct dev_rot_state *rot_state = iio_priv(indio_dev);
+
+	if (usage_id == HID_USAGE_SENSOR_ORIENT_QUATERNION) {
+		memcpy(rot_state->sampled_vals, raw_data,
+					sizeof(rot_state->sampled_vals));
+		dev_dbg(&indio_dev->dev, "Recd Quat len:%zu::%zu\n", raw_len,
+					sizeof(rot_state->sampled_vals));
+	}
+
+	return 0;
+}
+
+/* Parse report which is specific to an usage id*/
+static int dev_rot_parse_report(struct platform_device *pdev,
+				struct hid_sensor_hub_device *hsdev,
+				struct iio_chan_spec *channels,
+				unsigned usage_id,
+				struct dev_rot_state *st)
+{
+	int ret;
+
+	ret = sensor_hub_input_get_attribute_info(hsdev,
+				HID_INPUT_REPORT,
+				usage_id,
+				HID_USAGE_SENSOR_ORIENT_QUATERNION,
+				&st->quaternion);
+	if (ret)
+		return ret;
+
+	dev_rot_adjust_channel_bit_mask(&channels[0],
+		st->quaternion.size / 4);
+
+	dev_dbg(&pdev->dev, "dev_rot %x:%x\n", st->quaternion.index,
+		st->quaternion.report_id);
+
+	dev_dbg(&pdev->dev, "dev_rot: attrib size %d\n",
+				st->quaternion.size);
+
+	/* Set Sensitivity field ids, when there is no individual modifier */
+	if (st->common_attributes.sensitivity.index < 0) {
+		sensor_hub_input_get_attribute_info(hsdev,
+			HID_FEATURE_REPORT, usage_id,
+			HID_USAGE_SENSOR_DATA_MOD_CHANGE_SENSITIVITY_ABS |
+			HID_USAGE_SENSOR_DATA_ORIENTATION,
+			&st->common_attributes.sensitivity);
+		dev_dbg(&pdev->dev, "Sensitivity index:report %d:%d\n",
+			st->common_attributes.sensitivity.index,
+			st->common_attributes.sensitivity.report_id);
+	}
+
+	return 0;
+}
+
+/* Function to initialize the processing for usage id */
+static int hid_dev_rot_probe(struct platform_device *pdev)
+{
+	int ret;
+	static char *name = "dev_rotation";
+	struct iio_dev *indio_dev;
+	struct dev_rot_state *rot_state;
+	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
+	struct iio_chan_spec *channels;
+
+	indio_dev = devm_iio_device_alloc(&pdev->dev,
+					  sizeof(struct dev_rot_state));
+	if (indio_dev == NULL)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, indio_dev);
+
+	rot_state = iio_priv(indio_dev);
+	rot_state->common_attributes.hsdev = hsdev;
+	rot_state->common_attributes.pdev = pdev;
+
+	ret = hid_sensor_parse_common_attributes(hsdev,
+				HID_USAGE_SENSOR_DEVICE_ORIENTATION,
+				&rot_state->common_attributes);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to setup common attributes\n");
+		return ret;
+	}
+
+	channels = devm_kmemdup(&pdev->dev, dev_rot_channels,
+					sizeof(dev_rot_channels), GFP_KERNEL);
+	if (!channels) {
+		dev_err(&pdev->dev, "failed to duplicate channels\n");
+		return -ENOMEM;
+	}
+
+	ret = dev_rot_parse_report(pdev, hsdev, channels,
+			HID_USAGE_SENSOR_DEVICE_ORIENTATION, rot_state);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to setup attributes\n");
+		return ret;
+	}
+
+	indio_dev->channels = channels;
+	indio_dev->num_channels = ARRAY_SIZE(dev_rot_channels);
+	indio_dev->dev.parent = &pdev->dev;
+	indio_dev->info = &dev_rot_info;
+	indio_dev->name = name;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	ret = iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
+		NULL, NULL);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
+		return ret;
+	}
+	atomic_set(&rot_state->common_attributes.data_ready, 0);
+	ret = hid_sensor_setup_trigger(indio_dev, name,
+					&rot_state->common_attributes);
+	if (ret) {
+		dev_err(&pdev->dev, "trigger setup failed\n");
+		goto error_unreg_buffer_funcs;
+	}
+
+	ret = iio_device_register(indio_dev);
+	if (ret) {
+		dev_err(&pdev->dev, "device register failed\n");
+		goto error_remove_trigger;
+	}
+
+	rot_state->callbacks.send_event = dev_rot_proc_event;
+	rot_state->callbacks.capture_sample = dev_rot_capture_sample;
+	rot_state->callbacks.pdev = pdev;
+	ret = sensor_hub_register_callback(hsdev,
+					HID_USAGE_SENSOR_DEVICE_ORIENTATION,
+					&rot_state->callbacks);
+	if (ret) {
+		dev_err(&pdev->dev, "callback reg failed\n");
+		goto error_iio_unreg;
+	}
+
+	return 0;
+
+error_iio_unreg:
+	iio_device_unregister(indio_dev);
+error_remove_trigger:
+	hid_sensor_remove_trigger(&rot_state->common_attributes);
+error_unreg_buffer_funcs:
+	iio_triggered_buffer_cleanup(indio_dev);
+	return ret;
+}
+
+/* Function to deinitialize the processing for usage id */
+static int hid_dev_rot_remove(struct platform_device *pdev)
+{
+	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct dev_rot_state *rot_state = iio_priv(indio_dev);
+
+	sensor_hub_remove_callback(hsdev, HID_USAGE_SENSOR_DEVICE_ORIENTATION);
+	iio_device_unregister(indio_dev);
+	hid_sensor_remove_trigger(&rot_state->common_attributes);
+	iio_triggered_buffer_cleanup(indio_dev);
+
+	return 0;
+}
+
+static struct platform_device_id hid_dev_rot_ids[] = {
+	{
+		/* Format: HID-SENSOR-usage_id_in_hex_lowercase */
+		.name = "HID-SENSOR-20008a",
+	},
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(platform, hid_dev_rot_ids);
+
+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,
+};
+module_platform_driver(hid_dev_rot_platform_driver);
+
+MODULE_DESCRIPTION("HID Sensor Device Rotation");
+MODULE_AUTHOR("Srinivas Pandruvada <srinivas.pandruvada@linux.intel.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/pressure/Kconfig b/drivers/iio/pressure/Kconfig
index d88ff17fedb..ffac8ac1efc 100644
--- a/drivers/iio/pressure/Kconfig
+++ b/drivers/iio/pressure/Kconfig
@@ -19,6 +19,16 @@ config HID_SENSOR_PRESS
           To compile this driver as a module, choose M here: the module
           will be called hid-sensor-press.
 
+config MPL115
+	tristate "Freescale MPL115A2 pressure sensor driver"
+	depends on I2C
+	help
+	  Say yes here to build support for the Freescale MPL115A2
+	  pressure sensor connected via I2C.
+
+          To compile this driver as a module, choose M here: the module
+          will be called mpl115.
+
 config MPL3115
 	tristate "Freescale MPL3115A2 pressure sensor driver"
 	depends on I2C
diff --git a/drivers/iio/pressure/Makefile b/drivers/iio/pressure/Makefile
index 4a57bf65b04..c53d2500737 100644
--- a/drivers/iio/pressure/Makefile
+++ b/drivers/iio/pressure/Makefile
@@ -4,6 +4,7 @@
 
 # When adding new entries keep the list in alphabetical order
 obj-$(CONFIG_HID_SENSOR_PRESS)   += hid-sensor-press.o
+obj-$(CONFIG_MPL115) += mpl115.o
 obj-$(CONFIG_MPL3115) += mpl3115.o
 obj-$(CONFIG_IIO_ST_PRESS) += st_pressure.o
 st_pressure-y := st_pressure_core.o
diff --git a/drivers/iio/pressure/hid-sensor-press.c b/drivers/iio/pressure/hid-sensor-press.c
index e0e6409aa94..1cd190c7378 100644
--- a/drivers/iio/pressure/hid-sensor-press.c
+++ b/drivers/iio/pressure/hid-sensor-press.c
@@ -21,6 +21,7 @@
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/slab.h>
+#include <linux/delay.h>
 #include <linux/hid-sensor-hub.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -36,6 +37,10 @@ struct press_state {
 	struct hid_sensor_common common_attributes;
 	struct hid_sensor_hub_attribute_info press_attr;
 	u32 press_data;
+	int scale_pre_decml;
+	int scale_post_decml;
+	int scale_precision;
+	int value_offset;
 };
 
 /* Channel definitions */
@@ -75,6 +80,7 @@ static int press_read_raw(struct iio_dev *indio_dev,
 	u32 address;
 	int ret;
 	int ret_type;
+	s32 poll_value;
 
 	*val = 0;
 	*val2 = 0;
@@ -90,24 +96,35 @@ static int press_read_raw(struct iio_dev *indio_dev,
 			report_id = -1;
 			break;
 		}
-		if (report_id >= 0)
+		if (report_id >= 0) {
+			poll_value = hid_sensor_read_poll_value(
+					&press_state->common_attributes);
+			if (poll_value < 0)
+				return -EINVAL;
+			hid_sensor_power_state(&press_state->common_attributes,
+						true);
+
+			msleep_interruptible(poll_value * 2);
+
 			*val = sensor_hub_input_attr_get_raw_value(
 				press_state->common_attributes.hsdev,
 				HID_USAGE_SENSOR_PRESSURE, address,
 				report_id);
-		else {
+			hid_sensor_power_state(&press_state->common_attributes,
+						false);
+		} else {
 			*val = 0;
 			return -EINVAL;
 		}
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SCALE:
-		*val = press_state->press_attr.units;
-		ret_type = IIO_VAL_INT;
+		*val = press_state->scale_pre_decml;
+		*val2 = press_state->scale_post_decml;
+		ret_type = press_state->scale_precision;
 		break;
 	case IIO_CHAN_INFO_OFFSET:
-		*val = hid_sensor_convert_exponent(
-				press_state->press_attr.unit_expo);
+		*val = press_state->value_offset;
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SAMP_FREQ:
@@ -176,9 +193,8 @@ static int press_proc_event(struct hid_sensor_hub_device *hsdev,
 	struct iio_dev *indio_dev = platform_get_drvdata(priv);
 	struct press_state *press_state = iio_priv(indio_dev);
 
-	dev_dbg(&indio_dev->dev, "press_proc_event [%d]\n",
-				press_state->common_attributes.data_ready);
-	if (press_state->common_attributes.data_ready)
+	dev_dbg(&indio_dev->dev, "press_proc_event\n");
+	if (atomic_read(&press_state->common_attributes.data_ready))
 		hid_sensor_push_data(indio_dev,
 				&press_state->press_data,
 				sizeof(press_state->press_data));
@@ -229,6 +245,11 @@ static int press_parse_report(struct platform_device *pdev,
 	dev_dbg(&pdev->dev, "press %x:%x\n", st->press_attr.index,
 			st->press_attr.report_id);
 
+	st->scale_precision = hid_sensor_format_scale(
+				HID_USAGE_SENSOR_PRESSURE,
+				&st->press_attr,
+				&st->scale_pre_decml, &st->scale_post_decml);
+
 	/* Set Sensitivity field ids, when there is no individual modifier */
 	if (st->common_attributes.sensitivity.index < 0) {
 		sensor_hub_input_get_attribute_info(hsdev,
@@ -298,7 +319,7 @@ static int hid_press_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
 		goto error_free_dev_mem;
 	}
-	press_state->common_attributes.data_ready = false;
+	atomic_set(&press_state->common_attributes.data_ready, 0);
 	ret = hid_sensor_setup_trigger(indio_dev, name,
 				&press_state->common_attributes);
 	if (ret) {
diff --git a/drivers/iio/pressure/mpl115.c b/drivers/iio/pressure/mpl115.c
new file mode 100644
index 00000000000..f5ecd6e19f5
--- /dev/null
+++ b/drivers/iio/pressure/mpl115.c
@@ -0,0 +1,211 @@
+/*
+ * mpl115.c - Support for Freescale MPL115A2 pressure/temperature sensor
+ *
+ * Copyright (c) 2014 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.
+ *
+ * (7-bit I2C slave address 0x60)
+ *
+ * TODO: shutdown pin
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/iio/iio.h>
+#include <linux/delay.h>
+
+#define MPL115_PADC 0x00 /* pressure ADC output value, MSB first, 10 bit */
+#define MPL115_TADC 0x02 /* temperature ADC output value, MSB first, 10 bit */
+#define MPL115_A0 0x04 /* 12 bit integer, 3 bit fraction */
+#define MPL115_B1 0x06 /* 2 bit integer, 13 bit fraction */
+#define MPL115_B2 0x08 /* 1 bit integer, 14 bit fraction */
+#define MPL115_C12 0x0a /* 0 bit integer, 13 bit fraction */
+#define MPL115_CONVERT 0x12 /* convert temperature and pressure */
+
+struct mpl115_data {
+	struct i2c_client *client;
+	struct mutex lock;
+	s16 a0;
+	s16 b1, b2;
+	s16 c12;
+};
+
+static int mpl115_request(struct mpl115_data *data)
+{
+	int ret = i2c_smbus_write_byte_data(data->client, MPL115_CONVERT, 0);
+	if (ret < 0)
+		return ret;
+
+	usleep_range(3000, 4000);
+
+	return 0;
+}
+
+static int mpl115_comp_pressure(struct mpl115_data *data, int *val, int *val2)
+{
+	int ret;
+	u16 padc, tadc;
+	int a1, y1, pcomp;
+	unsigned kpa;
+
+	mutex_lock(&data->lock);
+	ret = mpl115_request(data);
+	if (ret < 0)
+		goto done;
+
+	ret = i2c_smbus_read_word_swapped(data->client, MPL115_PADC);
+	if (ret < 0)
+		goto done;
+	padc = ret >> 6;
+
+	ret = i2c_smbus_read_word_swapped(data->client, MPL115_TADC);
+	if (ret < 0)
+		goto done;
+	tadc = ret >> 6;
+
+	/* see Freescale AN3785 */
+	a1 = data->b1 + ((data->c12 * tadc) >> 11);
+	y1 = (data->a0 << 10) + a1 * padc;
+
+	/* compensated pressure with 4 fractional bits */
+	pcomp = (y1 + ((data->b2 * (int) tadc) >> 1)) >> 9;
+
+	kpa = pcomp * (115 - 50) / 1023 + (50 << 4);
+	*val = kpa >> 4;
+	*val2 = (kpa & 15) * (1000000 >> 4);
+done:
+	mutex_unlock(&data->lock);
+	return ret;
+}
+
+static int mpl115_read_temp(struct mpl115_data *data)
+{
+	int ret;
+
+	mutex_lock(&data->lock);
+	ret = mpl115_request(data);
+	if (ret < 0)
+		goto done;
+	ret = i2c_smbus_read_word_swapped(data->client, MPL115_TADC);
+done:
+	mutex_unlock(&data->lock);
+	return ret;
+}
+
+static int mpl115_read_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int *val, int *val2, long mask)
+{
+	struct mpl115_data *data = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_PROCESSED:
+		ret = mpl115_comp_pressure(data, val, val2);
+		if (ret < 0)
+			return ret;
+		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_RAW:
+		/* temperature -5.35 C / LSB, 472 LSB is 25 C */
+		ret = mpl115_read_temp(data);
+		if (ret < 0)
+			return ret;
+		*val = ret >> 6;
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_OFFSET:
+		*val = 605;
+		*val2 = 750000;
+		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_SCALE:
+		*val = -186;
+		*val2 = 915888;
+		return IIO_VAL_INT_PLUS_MICRO;
+	}
+	return -EINVAL;
+}
+
+static const struct iio_chan_spec mpl115_channels[] = {
+	{
+		.type = IIO_PRESSURE,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED),
+	},
+	{
+		.type = IIO_TEMP,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+			BIT(IIO_CHAN_INFO_OFFSET) | BIT(IIO_CHAN_INFO_SCALE),
+	},
+};
+
+static const struct iio_info mpl115_info = {
+	.read_raw = &mpl115_read_raw,
+	.driver_module = THIS_MODULE,
+};
+
+static int mpl115_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct mpl115_data *data;
+	struct iio_dev *indio_dev;
+	int ret;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_WORD_DATA))
+		return -ENODEV;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	data = iio_priv(indio_dev);
+	data->client = client;
+	mutex_init(&data->lock);
+
+	i2c_set_clientdata(client, indio_dev);
+	indio_dev->info = &mpl115_info;
+	indio_dev->name = id->name;
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = mpl115_channels;
+	indio_dev->num_channels = ARRAY_SIZE(mpl115_channels);
+
+	ret = i2c_smbus_read_word_swapped(data->client, MPL115_A0);
+	if (ret < 0)
+		return ret;
+	data->a0 = ret;
+	ret = i2c_smbus_read_word_swapped(data->client, MPL115_B1);
+	if (ret < 0)
+		return ret;
+	data->b1 = ret;
+	ret = i2c_smbus_read_word_swapped(data->client, MPL115_B2);
+	if (ret < 0)
+		return ret;
+	data->b2 = ret;
+	ret = i2c_smbus_read_word_swapped(data->client, MPL115_C12);
+	if (ret < 0)
+		return ret;
+	data->c12 = ret;
+
+	return devm_iio_device_register(&client->dev, indio_dev);
+}
+
+static const struct i2c_device_id mpl115_id[] = {
+	{ "mpl115", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mpl115_id);
+
+static struct i2c_driver mpl115_driver = {
+	.driver = {
+		.name	= "mpl115",
+	},
+	.probe = mpl115_probe,
+	.id_table = mpl115_id,
+};
+module_i2c_driver(mpl115_driver);
+
+MODULE_AUTHOR("Peter Meerwald <pmeerw@pmeerw.net>");
+MODULE_DESCRIPTION("Freescale MPL115 pressure/temperature driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/pressure/st_pressure_core.c b/drivers/iio/pressure/st_pressure_core.c
index 7418768ed49..cd7e01f3a93 100644
--- a/drivers/iio/pressure/st_pressure_core.c
+++ b/drivers/iio/pressure/st_pressure_core.c
@@ -23,7 +23,6 @@
 #include <linux/iio/sysfs.h>
 #include <linux/iio/trigger.h>
 #include <linux/iio/buffer.h>
-#include <linux/regulator/consumer.h>
 #include <asm/unaligned.h>
 
 #include <linux/iio/common/st_sensors.h>
@@ -387,40 +386,6 @@ static const struct iio_trigger_ops st_press_trigger_ops = {
 #define ST_PRESS_TRIGGER_OPS NULL
 #endif
 
-static void st_press_power_enable(struct iio_dev *indio_dev)
-{
-	struct st_sensor_data *pdata = iio_priv(indio_dev);
-	int err;
-
-	/* Regulators not mandatory, but if requested we should enable them. */
-	pdata->vdd = devm_regulator_get_optional(&indio_dev->dev, "vdd");
-	if (!IS_ERR(pdata->vdd)) {
-		err = regulator_enable(pdata->vdd);
-		if (err != 0)
-			dev_warn(&indio_dev->dev,
-				 "Failed to enable specified Vdd supply\n");
-	}
-
-	pdata->vdd_io = devm_regulator_get_optional(&indio_dev->dev, "vddio");
-	if (!IS_ERR(pdata->vdd_io)) {
-		err = regulator_enable(pdata->vdd_io);
-		if (err != 0)
-			dev_warn(&indio_dev->dev,
-				 "Failed to enable specified Vdd_IO supply\n");
-	}
-}
-
-static void st_press_power_disable(struct iio_dev *indio_dev)
-{
-	struct st_sensor_data *pdata = iio_priv(indio_dev);
-
-	if (!IS_ERR(pdata->vdd))
-		regulator_disable(pdata->vdd);
-
-	if (!IS_ERR(pdata->vdd_io))
-		regulator_disable(pdata->vdd_io);
-}
-
 int st_press_common_probe(struct iio_dev *indio_dev,
 				struct st_sensors_platform_data *plat_data)
 {
@@ -431,7 +396,7 @@ int st_press_common_probe(struct iio_dev *indio_dev,
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->info = &press_info;
 
-	st_press_power_enable(indio_dev);
+	st_sensors_power_enable(indio_dev);
 
 	err = st_sensors_check_device_support(indio_dev,
 					      ARRAY_SIZE(st_press_sensors),
@@ -474,6 +439,9 @@ int st_press_common_probe(struct iio_dev *indio_dev,
 	if (err)
 		goto st_press_device_register_error;
 
+	dev_info(&indio_dev->dev, "registered pressure sensor %s\n",
+		 indio_dev->name);
+
 	return err;
 
 st_press_device_register_error:
@@ -490,7 +458,7 @@ void st_press_common_remove(struct iio_dev *indio_dev)
 {
 	struct st_sensor_data *pdata = iio_priv(indio_dev);
 
-	st_press_power_disable(indio_dev);
+	st_sensors_power_disable(indio_dev);
 
 	iio_device_unregister(indio_dev);
 	if (pdata->get_irq_data_ready(indio_dev) > 0)
diff --git a/drivers/iio/proximity/Kconfig b/drivers/iio/proximity/Kconfig
new file mode 100644
index 00000000000..0c8cdf58f6a
--- /dev/null
+++ b/drivers/iio/proximity/Kconfig
@@ -0,0 +1,19 @@
+#
+# Proximity sensors
+#
+
+menu "Lightning sensors"
+
+config AS3935
+	tristate "AS3935 Franklin lightning sensor"
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	depends on SPI
+	help
+	  Say Y here to build SPI interface support for the Austrian
+	  Microsystems AS3935 lightning detection sensor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called as3935
+
+endmenu
diff --git a/drivers/iio/proximity/Makefile b/drivers/iio/proximity/Makefile
new file mode 100644
index 00000000000..743adee1c8b
--- /dev/null
+++ b/drivers/iio/proximity/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for IIO proximity sensors
+#
+
+# When adding new entries keep the list in alphabetical order
+obj-$(CONFIG_AS3935)		+= as3935.o
diff --git a/drivers/iio/proximity/as3935.c b/drivers/iio/proximity/as3935.c
new file mode 100644
index 00000000000..bf677bfe8eb
--- /dev/null
+++ b/drivers/iio/proximity/as3935.c
@@ -0,0 +1,456 @@
+/*
+ * as3935.c - Support for AS3935 Franklin lightning sensor
+ *
+ * Copyright (C) 2014 Matt Ranostay <mranostay@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/module.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/workqueue.h>
+#include <linux/mutex.h>
+#include <linux/err.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+#include <linux/spi/spi.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/of_gpio.h>
+
+
+#define AS3935_AFE_GAIN		0x00
+#define AS3935_AFE_MASK		0x3F
+#define AS3935_AFE_GAIN_MAX	0x1F
+#define AS3935_AFE_PWR_BIT	BIT(0)
+
+#define AS3935_INT		0x03
+#define AS3935_INT_MASK		0x07
+#define AS3935_EVENT_INT	BIT(3)
+#define AS3935_NOISE_INT	BIT(1)
+
+#define AS3935_DATA		0x07
+#define AS3935_DATA_MASK	0x3F
+
+#define AS3935_TUNE_CAP		0x08
+#define AS3935_CALIBRATE	0x3D
+
+#define AS3935_WRITE_DATA	BIT(15)
+#define AS3935_READ_DATA	BIT(14)
+#define AS3935_ADDRESS(x)	((x) << 8)
+
+#define MAX_PF_CAP		120
+#define TUNE_CAP_DIV		8
+
+struct as3935_state {
+	struct spi_device *spi;
+	struct iio_trigger *trig;
+	struct mutex lock;
+	struct delayed_work work;
+
+	u32 tune_cap;
+	u8 buf[2] ____cacheline_aligned;
+};
+
+static const struct iio_chan_spec as3935_channels[] = {
+	{
+		.type           = IIO_PROXIMITY,
+		.info_mask_separate =
+			BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_PROCESSED),
+		.scan_index     = 0,
+		.scan_type = {
+			.sign           = 'u',
+			.realbits       = 6,
+			.storagebits    = 8,
+		},
+	},
+	IIO_CHAN_SOFT_TIMESTAMP(1),
+};
+
+static int as3935_read(struct as3935_state *st, unsigned int reg, int *val)
+{
+	u8 cmd;
+	int ret;
+
+	cmd = (AS3935_READ_DATA | AS3935_ADDRESS(reg)) >> 8;
+	ret = spi_w8r8(st->spi, cmd);
+	if (ret < 0)
+		return ret;
+	*val = ret;
+
+	return 0;
+};
+
+static int as3935_write(struct as3935_state *st,
+				unsigned int reg,
+				unsigned int val)
+{
+	u8 *buf = st->buf;
+
+	buf[0] = (AS3935_WRITE_DATA | AS3935_ADDRESS(reg)) >> 8;
+	buf[1] = val;
+
+	return spi_write(st->spi, buf, 2);
+};
+
+static ssize_t as3935_sensor_sensitivity_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct as3935_state *st = iio_priv(dev_to_iio_dev(dev));
+	int val, ret;
+
+	ret = as3935_read(st, AS3935_AFE_GAIN, &val);
+	if (ret)
+		return ret;
+	val = (val & AS3935_AFE_MASK) >> 1;
+
+	return sprintf(buf, "%d\n", val);
+};
+
+static ssize_t as3935_sensor_sensitivity_store(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t len)
+{
+	struct as3935_state *st = iio_priv(dev_to_iio_dev(dev));
+	unsigned long val;
+	int ret;
+
+	ret = kstrtoul((const char *) buf, 10, &val);
+	if (ret)
+		return -EINVAL;
+
+	if (val > AS3935_AFE_GAIN_MAX)
+		return -EINVAL;
+
+	as3935_write(st, AS3935_AFE_GAIN, val << 1);
+
+	return len;
+};
+
+static IIO_DEVICE_ATTR(sensor_sensitivity, S_IRUGO | S_IWUSR,
+	as3935_sensor_sensitivity_show, as3935_sensor_sensitivity_store, 0);
+
+
+static struct attribute *as3935_attributes[] = {
+	&iio_dev_attr_sensor_sensitivity.dev_attr.attr,
+	NULL,
+};
+
+static struct attribute_group as3935_attribute_group = {
+	.attrs = as3935_attributes,
+};
+
+static int as3935_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan,
+			   int *val,
+			   int *val2,
+			   long m)
+{
+	struct as3935_state *st = iio_priv(indio_dev);
+	int ret;
+
+
+	switch (m) {
+	case IIO_CHAN_INFO_PROCESSED:
+	case IIO_CHAN_INFO_RAW:
+		*val2 = 0;
+		ret = as3935_read(st, AS3935_DATA, val);
+		if (ret)
+			return ret;
+
+		if (m == IIO_CHAN_INFO_RAW)
+			return IIO_VAL_INT;
+
+		/* storm out of range */
+		if (*val == AS3935_DATA_MASK)
+			return -EINVAL;
+		*val *= 1000;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return IIO_VAL_INT;
+}
+
+static const struct iio_info as3935_info = {
+	.driver_module = THIS_MODULE,
+	.attrs = &as3935_attribute_group,
+	.read_raw = &as3935_read_raw,
+};
+
+static irqreturn_t as3935_trigger_handler(int irq, void *private)
+{
+	struct iio_poll_func *pf = private;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct as3935_state *st = iio_priv(indio_dev);
+	int val, ret;
+
+	ret = as3935_read(st, AS3935_DATA, &val);
+	if (ret)
+		goto err_read;
+	val &= AS3935_DATA_MASK;
+	val *= 1000;
+
+	iio_push_to_buffers_with_timestamp(indio_dev, &val, pf->timestamp);
+err_read:
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+};
+
+static const struct iio_trigger_ops iio_interrupt_trigger_ops = {
+	.owner = THIS_MODULE,
+};
+
+static void as3935_event_work(struct work_struct *work)
+{
+	struct as3935_state *st;
+	int val;
+
+	st = container_of(work, struct as3935_state, work.work);
+
+	as3935_read(st, AS3935_INT, &val);
+	val &= AS3935_INT_MASK;
+
+	switch (val) {
+	case AS3935_EVENT_INT:
+		iio_trigger_poll(st->trig, iio_get_time_ns());
+		break;
+	case AS3935_NOISE_INT:
+		dev_warn(&st->spi->dev, "noise level is too high");
+		break;
+	}
+};
+
+static irqreturn_t as3935_interrupt_handler(int irq, void *private)
+{
+	struct iio_dev *indio_dev = private;
+	struct as3935_state *st = iio_priv(indio_dev);
+
+	/*
+	 * Delay work for >2 milliseconds after an interrupt to allow
+	 * estimated distance to recalculated.
+	 */
+
+	schedule_delayed_work(&st->work, msecs_to_jiffies(3));
+
+	return IRQ_HANDLED;
+}
+
+static void calibrate_as3935(struct as3935_state *st)
+{
+	mutex_lock(&st->lock);
+
+	/* mask disturber interrupt bit */
+	as3935_write(st, AS3935_INT, BIT(5));
+
+	as3935_write(st, AS3935_CALIBRATE, 0x96);
+	as3935_write(st, AS3935_TUNE_CAP,
+		BIT(5) | (st->tune_cap / TUNE_CAP_DIV));
+
+	mdelay(2);
+	as3935_write(st, AS3935_TUNE_CAP, (st->tune_cap / TUNE_CAP_DIV));
+
+	mutex_unlock(&st->lock);
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int as3935_suspend(struct spi_device *spi, pm_message_t msg)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+	struct as3935_state *st = iio_priv(indio_dev);
+	int val, ret;
+
+	mutex_lock(&st->lock);
+	ret = as3935_read(st, AS3935_AFE_GAIN, &val);
+	if (ret)
+		goto err_suspend;
+	val |= AS3935_AFE_PWR_BIT;
+
+	ret = as3935_write(st, AS3935_AFE_GAIN, val);
+
+err_suspend:
+	mutex_unlock(&st->lock);
+
+	return ret;
+}
+
+static int as3935_resume(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+	struct as3935_state *st = iio_priv(indio_dev);
+	int val, ret;
+
+	mutex_lock(&st->lock);
+	ret = as3935_read(st, AS3935_AFE_GAIN, &val);
+	if (ret)
+		goto err_resume;
+	val &= ~AS3935_AFE_PWR_BIT;
+	ret = as3935_write(st, AS3935_AFE_GAIN, val);
+
+err_resume:
+	mutex_unlock(&st->lock);
+
+	return ret;
+}
+#else
+#define as3935_suspend	NULL
+#define as3935_resume	NULL
+#endif
+
+static int as3935_probe(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev;
+	struct iio_trigger *trig;
+	struct as3935_state *st;
+	struct device_node *np = spi->dev.of_node;
+	int ret;
+
+	/* Be sure lightning event interrupt is specified */
+	if (!spi->irq) {
+		dev_err(&spi->dev, "unable to get event interrupt\n");
+		return -EINVAL;
+	}
+
+	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(st));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	st = iio_priv(indio_dev);
+	st->spi = spi;
+	st->tune_cap = 0;
+
+	spi_set_drvdata(spi, indio_dev);
+	mutex_init(&st->lock);
+	INIT_DELAYED_WORK(&st->work, as3935_event_work);
+
+	ret = of_property_read_u32(np,
+			"ams,tuning-capacitor-pf", &st->tune_cap);
+	if (ret) {
+		st->tune_cap = 0;
+		dev_warn(&spi->dev,
+			"no tuning-capacitor-pf set, defaulting to %d",
+			st->tune_cap);
+	}
+
+	if (st->tune_cap > MAX_PF_CAP) {
+		dev_err(&spi->dev,
+			"wrong tuning-capacitor-pf setting of %d\n",
+			st->tune_cap);
+		return -EINVAL;
+	}
+
+	indio_dev->dev.parent = &spi->dev;
+	indio_dev->name = spi_get_device_id(spi)->name;
+	indio_dev->channels = as3935_channels;
+	indio_dev->num_channels = ARRAY_SIZE(as3935_channels);
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->info = &as3935_info;
+
+	trig = devm_iio_trigger_alloc(&spi->dev, "%s-dev%d",
+				      indio_dev->name, indio_dev->id);
+
+	if (!trig)
+		return -ENOMEM;
+
+	st->trig = trig;
+	trig->dev.parent = indio_dev->dev.parent;
+	iio_trigger_set_drvdata(trig, indio_dev);
+	trig->ops = &iio_interrupt_trigger_ops;
+
+	ret = iio_trigger_register(trig);
+	if (ret) {
+		dev_err(&spi->dev, "failed to register trigger\n");
+		return ret;
+	}
+
+	ret = iio_triggered_buffer_setup(indio_dev, NULL,
+		&as3935_trigger_handler, NULL);
+
+	if (ret) {
+		dev_err(&spi->dev, "cannot setup iio trigger\n");
+		goto unregister_trigger;
+	}
+
+	calibrate_as3935(st);
+
+	ret = devm_request_irq(&spi->dev, spi->irq,
+				&as3935_interrupt_handler,
+				IRQF_TRIGGER_RISING,
+				dev_name(&spi->dev),
+				indio_dev);
+
+	if (ret) {
+		dev_err(&spi->dev, "unable to request irq\n");
+		goto unregister_buffer;
+	}
+
+	ret = iio_device_register(indio_dev);
+	if (ret < 0) {
+		dev_err(&spi->dev, "unable to register device\n");
+		goto unregister_buffer;
+	}
+	return 0;
+
+unregister_buffer:
+	iio_triggered_buffer_cleanup(indio_dev);
+
+unregister_trigger:
+	iio_trigger_unregister(st->trig);
+
+	return ret;
+};
+
+static int as3935_remove(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+	struct as3935_state *st = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
+	iio_trigger_unregister(st->trig);
+
+	return 0;
+};
+
+static const struct spi_device_id as3935_id[] = {
+	{"as3935", 0},
+	{},
+};
+MODULE_DEVICE_TABLE(spi, as3935_id);
+
+static struct spi_driver as3935_driver = {
+	.driver = {
+		.name	= "as3935",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= as3935_probe,
+	.remove		= as3935_remove,
+	.id_table	= as3935_id,
+	.suspend	= as3935_suspend,
+	.resume		= as3935_resume,
+};
+module_spi_driver(as3935_driver);
+
+MODULE_AUTHOR("Matt Ranostay <mranostay@gmail.com>");
+MODULE_DESCRIPTION("AS3935 lightning sensor");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("spi:as3935");
diff --git a/drivers/iio/temperature/Kconfig b/drivers/iio/temperature/Kconfig
index 372f8fb3085..21feaa4661b 100644
--- a/drivers/iio/temperature/Kconfig
+++ b/drivers/iio/temperature/Kconfig
@@ -3,6 +3,16 @@
 #
 menu "Temperature sensors"
 
+config MLX90614
+	tristate "MLX90614 contact-less infrared sensor"
+	depends on I2C
+	help
+	  If you say yes here you get support for the Melexis
+	  MLX90614 contact-less infrared sensor connected with I2C.
+
+	  This driver can also be built as a module. If so, the module will
+	  be called mlx90614.
+
 config TMP006
 	tristate "TMP006 infrared thermopile sensor"
 	depends on I2C
diff --git a/drivers/iio/temperature/Makefile b/drivers/iio/temperature/Makefile
index 24d7b602db3..40710a81158 100644
--- a/drivers/iio/temperature/Makefile
+++ b/drivers/iio/temperature/Makefile
@@ -2,4 +2,5 @@
 # Makefile for industrial I/O temperature drivers
 #
 
+obj-$(CONFIG_MLX90614) += mlx90614.o
 obj-$(CONFIG_TMP006) += tmp006.o
diff --git a/drivers/iio/temperature/mlx90614.c b/drivers/iio/temperature/mlx90614.c
new file mode 100644
index 00000000000..c8b6ac8b2d6
--- /dev/null
+++ b/drivers/iio/temperature/mlx90614.c
@@ -0,0 +1,150 @@
+/*
+ * mlx90614.c - Support for Melexis MLX90614 contactless IR temperature sensor
+ *
+ * Copyright (c) 2014 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.
+ *
+ * Driver for the Melexis MLX90614 I2C 16-bit IR thermopile sensor
+ *
+ * (7-bit I2C slave address 0x5a, 100KHz bus speed only!)
+ *
+ * TODO: sleep mode, configuration EEPROM
+ */
+
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+
+#include <linux/iio/iio.h>
+
+#define MLX90614_OP_RAM 0x00
+
+/* RAM offsets with 16-bit data, MSB first */
+#define MLX90614_TA 0x06 /* ambient temperature */
+#define MLX90614_TOBJ1 0x07 /* object temperature */
+
+struct mlx90614_data {
+	struct i2c_client *client;
+};
+
+static int mlx90614_read_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *channel, int *val,
+			    int *val2, long mask)
+{
+	struct mlx90614_data *data = iio_priv(indio_dev);
+	s32 ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW: /* 0.02K / LSB */
+		switch (channel->channel2) {
+		case IIO_MOD_TEMP_AMBIENT:
+			ret = i2c_smbus_read_word_data(data->client,
+			    MLX90614_OP_RAM | MLX90614_TA);
+			if (ret < 0)
+				return ret;
+			break;
+		case IIO_MOD_TEMP_OBJECT:
+			ret = i2c_smbus_read_word_data(data->client,
+			    MLX90614_OP_RAM | MLX90614_TOBJ1);
+			if (ret < 0)
+				return ret;
+			break;
+		default:
+			return -EINVAL;
+		}
+		*val = ret;
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_OFFSET:
+		*val = 13657;
+		*val2 = 500000;
+		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_SCALE:
+		*val = 20;
+		return IIO_VAL_INT;
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct iio_chan_spec mlx90614_channels[] = {
+	{
+		.type = IIO_TEMP,
+		.modified = 1,
+		.channel2 = IIO_MOD_TEMP_AMBIENT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
+		    BIT(IIO_CHAN_INFO_SCALE),
+	},
+	{
+		.type = IIO_TEMP,
+		.modified = 1,
+		.channel2 = IIO_MOD_TEMP_OBJECT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
+		    BIT(IIO_CHAN_INFO_SCALE),
+	},
+};
+
+static const struct iio_info mlx90614_info = {
+	.read_raw = mlx90614_read_raw,
+	.driver_module = THIS_MODULE,
+};
+
+static int mlx90614_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct iio_dev *indio_dev;
+	struct mlx90614_data *data;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_WORD_DATA))
+		return -ENODEV;
+
+	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->name = id->name;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->info = &mlx90614_info;
+
+	indio_dev->channels = mlx90614_channels;
+	indio_dev->num_channels = ARRAY_SIZE(mlx90614_channels);
+
+	return iio_device_register(indio_dev);
+}
+
+static int mlx90614_remove(struct i2c_client *client)
+{
+	iio_device_unregister(i2c_get_clientdata(client));
+
+	return 0;
+}
+
+static const struct i2c_device_id mlx90614_id[] = {
+	{ "mlx90614", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mlx90614_id);
+
+static struct i2c_driver mlx90614_driver = {
+	.driver = {
+		.name	= "mlx90614",
+		.owner	= THIS_MODULE,
+	},
+	.probe = mlx90614_probe,
+	.remove = mlx90614_remove,
+	.id_table = mlx90614_id,
+};
+module_i2c_driver(mlx90614_driver);
+
+MODULE_AUTHOR("Peter Meerwald <pmeerw@pmeerw.net>");
+MODULE_DESCRIPTION("Melexis MLX90614 contactless IR temperature sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/infiniband/Makefile b/drivers/infiniband/Makefile
index bf508b5550c..dc21836b5a8 100644
--- a/drivers/infiniband/Makefile
+++ b/drivers/infiniband/Makefile
@@ -1,18 +1,3 @@
 obj-$(CONFIG_INFINIBAND)		+= core/
-obj-$(CONFIG_INFINIBAND_MTHCA)		+= hw/mthca/
-obj-$(CONFIG_INFINIBAND_IPATH)		+= hw/ipath/
-obj-$(CONFIG_INFINIBAND_QIB)		+= hw/qib/
-obj-$(CONFIG_INFINIBAND_EHCA)		+= hw/ehca/
-obj-$(CONFIG_INFINIBAND_AMSO1100)	+= hw/amso1100/
-obj-$(CONFIG_INFINIBAND_CXGB3)		+= hw/cxgb3/
-obj-$(CONFIG_INFINIBAND_CXGB4)		+= hw/cxgb4/
-obj-$(CONFIG_MLX4_INFINIBAND)		+= hw/mlx4/
-obj-$(CONFIG_MLX5_INFINIBAND)		+= hw/mlx5/
-obj-$(CONFIG_INFINIBAND_NES)		+= hw/nes/
-obj-$(CONFIG_INFINIBAND_OCRDMA)		+= hw/ocrdma/
-obj-$(CONFIG_INFINIBAND_USNIC)		+= hw/usnic/
-obj-$(CONFIG_INFINIBAND_IPOIB)		+= ulp/ipoib/
-obj-$(CONFIG_INFINIBAND_SRP)		+= ulp/srp/
-obj-$(CONFIG_INFINIBAND_SRPT)		+= ulp/srpt/
-obj-$(CONFIG_INFINIBAND_ISER)		+= ulp/iser/
-obj-$(CONFIG_INFINIBAND_ISERT)		+= ulp/isert/
+obj-$(CONFIG_INFINIBAND)		+= hw/
+obj-$(CONFIG_INFINIBAND)		+= ulp/
diff --git a/drivers/infiniband/core/Makefile b/drivers/infiniband/core/Makefile
index 3ab3865544b..ffd0af6734a 100644
--- a/drivers/infiniband/core/Makefile
+++ b/drivers/infiniband/core/Makefile
@@ -18,7 +18,7 @@ ib_sa-y :=			sa_query.o multicast.o
 
 ib_cm-y :=			cm.o
 
-iw_cm-y :=			iwcm.o
+iw_cm-y :=			iwcm.o iwpm_util.o iwpm_msg.o
 
 rdma_cm-y :=			cma.o
 
diff --git a/drivers/infiniband/core/cma.c b/drivers/infiniband/core/cma.c
index 42c3058e6e9..d570030d899 100644
--- a/drivers/infiniband/core/cma.c
+++ b/drivers/infiniband/core/cma.c
@@ -3607,7 +3607,8 @@ static int cma_get_id_stats(struct sk_buff *skb, struct netlink_callback *cb)
 
 			id_stats = ibnl_put_msg(skb, &nlh, cb->nlh->nlmsg_seq,
 						sizeof *id_stats, RDMA_NL_RDMA_CM,
-						RDMA_NL_RDMA_CM_ID_STATS);
+						RDMA_NL_RDMA_CM_ID_STATS,
+						NLM_F_MULTI);
 			if (!id_stats)
 				goto out;
 
diff --git a/drivers/infiniband/core/iwpm_msg.c b/drivers/infiniband/core/iwpm_msg.c
new file mode 100644
index 00000000000..b85ddbc979e
--- /dev/null
+++ b/drivers/infiniband/core/iwpm_msg.c
@@ -0,0 +1,685 @@
+/*
+ * Copyright (c) 2014 Intel Corporation. All rights reserved.
+ * Copyright (c) 2014 Chelsio, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     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.
+ *
+ * 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 "iwpm_util.h"
+
+static const char iwpm_ulib_name[] = "iWarpPortMapperUser";
+static int iwpm_ulib_version = 3;
+static int iwpm_user_pid = IWPM_PID_UNDEFINED;
+static atomic_t echo_nlmsg_seq;
+
+int iwpm_valid_pid(void)
+{
+	return iwpm_user_pid > 0;
+}
+EXPORT_SYMBOL(iwpm_valid_pid);
+
+/*
+ * iwpm_register_pid - Send a netlink query to user space
+ *                     for the iwarp port mapper pid
+ *
+ * nlmsg attributes:
+ *	[IWPM_NLA_REG_PID_SEQ]
+ *	[IWPM_NLA_REG_IF_NAME]
+ *	[IWPM_NLA_REG_IBDEV_NAME]
+ *	[IWPM_NLA_REG_ULIB_NAME]
+ */
+int iwpm_register_pid(struct iwpm_dev_data *pm_msg, u8 nl_client)
+{
+	struct sk_buff *skb = NULL;
+	struct iwpm_nlmsg_request *nlmsg_request = NULL;
+	struct nlmsghdr *nlh;
+	u32 msg_seq;
+	const char *err_str = "";
+	int ret = -EINVAL;
+
+	if (!iwpm_valid_client(nl_client)) {
+		err_str = "Invalid port mapper client";
+		goto pid_query_error;
+	}
+	if (iwpm_registered_client(nl_client))
+		return 0;
+	skb = iwpm_create_nlmsg(RDMA_NL_IWPM_REG_PID, &nlh, nl_client);
+	if (!skb) {
+		err_str = "Unable to create a nlmsg";
+		goto pid_query_error;
+	}
+	nlh->nlmsg_seq = iwpm_get_nlmsg_seq();
+	nlmsg_request = iwpm_get_nlmsg_request(nlh->nlmsg_seq, nl_client, GFP_KERNEL);
+	if (!nlmsg_request) {
+		err_str = "Unable to allocate netlink request";
+		goto pid_query_error;
+	}
+	msg_seq = atomic_read(&echo_nlmsg_seq);
+
+	/* fill in the pid request message */
+	err_str = "Unable to put attribute of the nlmsg";
+	ret = ibnl_put_attr(skb, nlh, sizeof(u32), &msg_seq, IWPM_NLA_REG_PID_SEQ);
+	if (ret)
+		goto pid_query_error;
+	ret = ibnl_put_attr(skb, nlh, IWPM_IFNAME_SIZE,
+				pm_msg->if_name, IWPM_NLA_REG_IF_NAME);
+	if (ret)
+		goto pid_query_error;
+	ret = ibnl_put_attr(skb, nlh, IWPM_DEVNAME_SIZE,
+				pm_msg->dev_name, IWPM_NLA_REG_IBDEV_NAME);
+	if (ret)
+		goto pid_query_error;
+	ret = ibnl_put_attr(skb, nlh, IWPM_ULIBNAME_SIZE,
+				(char *)iwpm_ulib_name, IWPM_NLA_REG_ULIB_NAME);
+	if (ret)
+		goto pid_query_error;
+
+	pr_debug("%s: Multicasting a nlmsg (dev = %s ifname = %s iwpm = %s)\n",
+		__func__, pm_msg->dev_name, pm_msg->if_name, iwpm_ulib_name);
+
+	ret = ibnl_multicast(skb, nlh, RDMA_NL_GROUP_IWPM, GFP_KERNEL);
+	if (ret) {
+		skb = NULL; /* skb is freed in the netlink send-op handling */
+		iwpm_set_registered(nl_client, 1);
+		iwpm_user_pid = IWPM_PID_UNAVAILABLE;
+		err_str = "Unable to send a nlmsg";
+		goto pid_query_error;
+	}
+	nlmsg_request->req_buffer = pm_msg;
+	ret = iwpm_wait_complete_req(nlmsg_request);
+	return ret;
+pid_query_error:
+	pr_info("%s: %s (client = %d)\n", __func__, err_str, nl_client);
+	if (skb)
+		dev_kfree_skb(skb);
+	if (nlmsg_request)
+		iwpm_free_nlmsg_request(&nlmsg_request->kref);
+	return ret;
+}
+EXPORT_SYMBOL(iwpm_register_pid);
+
+/*
+ * iwpm_add_mapping - Send a netlink add mapping message
+ *                    to the port mapper
+ * nlmsg attributes:
+ *	[IWPM_NLA_MANAGE_MAPPING_SEQ]
+ *	[IWPM_NLA_MANAGE_ADDR]
+ */
+int iwpm_add_mapping(struct iwpm_sa_data *pm_msg, u8 nl_client)
+{
+	struct sk_buff *skb = NULL;
+	struct iwpm_nlmsg_request *nlmsg_request = NULL;
+	struct nlmsghdr *nlh;
+	u32 msg_seq;
+	const char *err_str = "";
+	int ret = -EINVAL;
+
+	if (!iwpm_valid_client(nl_client)) {
+		err_str = "Invalid port mapper client";
+		goto add_mapping_error;
+	}
+	if (!iwpm_registered_client(nl_client)) {
+		err_str = "Unregistered port mapper client";
+		goto add_mapping_error;
+	}
+	if (!iwpm_valid_pid())
+		return 0;
+	skb = iwpm_create_nlmsg(RDMA_NL_IWPM_ADD_MAPPING, &nlh, nl_client);
+	if (!skb) {
+		err_str = "Unable to create a nlmsg";
+		goto add_mapping_error;
+	}
+	nlh->nlmsg_seq = iwpm_get_nlmsg_seq();
+	nlmsg_request = iwpm_get_nlmsg_request(nlh->nlmsg_seq, nl_client, GFP_KERNEL);
+	if (!nlmsg_request) {
+		err_str = "Unable to allocate netlink request";
+		goto add_mapping_error;
+	}
+	msg_seq = atomic_read(&echo_nlmsg_seq);
+	/* fill in the add mapping message */
+	err_str = "Unable to put attribute of the nlmsg";
+	ret = ibnl_put_attr(skb, nlh, sizeof(u32), &msg_seq,
+				IWPM_NLA_MANAGE_MAPPING_SEQ);
+	if (ret)
+		goto add_mapping_error;
+	ret = ibnl_put_attr(skb, nlh, sizeof(struct sockaddr_storage),
+				&pm_msg->loc_addr, IWPM_NLA_MANAGE_ADDR);
+	if (ret)
+		goto add_mapping_error;
+	nlmsg_request->req_buffer = pm_msg;
+
+	ret = ibnl_unicast(skb, nlh, iwpm_user_pid);
+	if (ret) {
+		skb = NULL; /* skb is freed in the netlink send-op handling */
+		iwpm_user_pid = IWPM_PID_UNDEFINED;
+		err_str = "Unable to send a nlmsg";
+		goto add_mapping_error;
+	}
+	ret = iwpm_wait_complete_req(nlmsg_request);
+	return ret;
+add_mapping_error:
+	pr_info("%s: %s (client = %d)\n", __func__, err_str, nl_client);
+	if (skb)
+		dev_kfree_skb(skb);
+	if (nlmsg_request)
+		iwpm_free_nlmsg_request(&nlmsg_request->kref);
+	return ret;
+}
+EXPORT_SYMBOL(iwpm_add_mapping);
+
+/*
+ * iwpm_add_and_query_mapping - Send a netlink add and query
+ *                              mapping message to the port mapper
+ * nlmsg attributes:
+ *	[IWPM_NLA_QUERY_MAPPING_SEQ]
+ *	[IWPM_NLA_QUERY_LOCAL_ADDR]
+ *	[IWPM_NLA_QUERY_REMOTE_ADDR]
+ */
+int iwpm_add_and_query_mapping(struct iwpm_sa_data *pm_msg, u8 nl_client)
+{
+	struct sk_buff *skb = NULL;
+	struct iwpm_nlmsg_request *nlmsg_request = NULL;
+	struct nlmsghdr *nlh;
+	u32 msg_seq;
+	const char *err_str = "";
+	int ret = -EINVAL;
+
+	if (!iwpm_valid_client(nl_client)) {
+		err_str = "Invalid port mapper client";
+		goto query_mapping_error;
+	}
+	if (!iwpm_registered_client(nl_client)) {
+		err_str = "Unregistered port mapper client";
+		goto query_mapping_error;
+	}
+	if (!iwpm_valid_pid())
+		return 0;
+	ret = -ENOMEM;
+	skb = iwpm_create_nlmsg(RDMA_NL_IWPM_QUERY_MAPPING, &nlh, nl_client);
+	if (!skb) {
+		err_str = "Unable to create a nlmsg";
+		goto query_mapping_error;
+	}
+	nlh->nlmsg_seq = iwpm_get_nlmsg_seq();
+	nlmsg_request = iwpm_get_nlmsg_request(nlh->nlmsg_seq,
+				nl_client, GFP_KERNEL);
+	if (!nlmsg_request) {
+		err_str = "Unable to allocate netlink request";
+		goto query_mapping_error;
+	}
+	msg_seq = atomic_read(&echo_nlmsg_seq);
+
+	/* fill in the query message */
+	err_str = "Unable to put attribute of the nlmsg";
+	ret = ibnl_put_attr(skb, nlh, sizeof(u32), &msg_seq,
+				IWPM_NLA_QUERY_MAPPING_SEQ);
+	if (ret)
+		goto query_mapping_error;
+	ret = ibnl_put_attr(skb, nlh, sizeof(struct sockaddr_storage),
+				&pm_msg->loc_addr, IWPM_NLA_QUERY_LOCAL_ADDR);
+	if (ret)
+		goto query_mapping_error;
+	ret = ibnl_put_attr(skb, nlh, sizeof(struct sockaddr_storage),
+				&pm_msg->rem_addr, IWPM_NLA_QUERY_REMOTE_ADDR);
+	if (ret)
+		goto query_mapping_error;
+	nlmsg_request->req_buffer = pm_msg;
+
+	ret = ibnl_unicast(skb, nlh, iwpm_user_pid);
+	if (ret) {
+		skb = NULL; /* skb is freed in the netlink send-op handling */
+		err_str = "Unable to send a nlmsg";
+		goto query_mapping_error;
+	}
+	ret = iwpm_wait_complete_req(nlmsg_request);
+	return ret;
+query_mapping_error:
+	pr_info("%s: %s (client = %d)\n", __func__, err_str, nl_client);
+	if (skb)
+		dev_kfree_skb(skb);
+	if (nlmsg_request)
+		iwpm_free_nlmsg_request(&nlmsg_request->kref);
+	return ret;
+}
+EXPORT_SYMBOL(iwpm_add_and_query_mapping);
+
+/*
+ * iwpm_remove_mapping - Send a netlink remove mapping message
+ *                       to the port mapper
+ * nlmsg attributes:
+ *	[IWPM_NLA_MANAGE_MAPPING_SEQ]
+ *	[IWPM_NLA_MANAGE_ADDR]
+ */
+int iwpm_remove_mapping(struct sockaddr_storage *local_addr, u8 nl_client)
+{
+	struct sk_buff *skb = NULL;
+	struct nlmsghdr *nlh;
+	u32 msg_seq;
+	const char *err_str = "";
+	int ret = -EINVAL;
+
+	if (!iwpm_valid_client(nl_client)) {
+		err_str = "Invalid port mapper client";
+		goto remove_mapping_error;
+	}
+	if (!iwpm_registered_client(nl_client)) {
+		err_str = "Unregistered port mapper client";
+		goto remove_mapping_error;
+	}
+	if (!iwpm_valid_pid())
+		return 0;
+	skb = iwpm_create_nlmsg(RDMA_NL_IWPM_REMOVE_MAPPING, &nlh, nl_client);
+	if (!skb) {
+		ret = -ENOMEM;
+		err_str = "Unable to create a nlmsg";
+		goto remove_mapping_error;
+	}
+	msg_seq = atomic_read(&echo_nlmsg_seq);
+	nlh->nlmsg_seq = iwpm_get_nlmsg_seq();
+	err_str = "Unable to put attribute of the nlmsg";
+	ret = ibnl_put_attr(skb, nlh, sizeof(u32), &msg_seq,
+				IWPM_NLA_MANAGE_MAPPING_SEQ);
+	if (ret)
+		goto remove_mapping_error;
+	ret = ibnl_put_attr(skb, nlh, sizeof(struct sockaddr_storage),
+				local_addr, IWPM_NLA_MANAGE_ADDR);
+	if (ret)
+		goto remove_mapping_error;
+
+	ret = ibnl_unicast(skb, nlh, iwpm_user_pid);
+	if (ret) {
+		skb = NULL; /* skb is freed in the netlink send-op handling */
+		iwpm_user_pid = IWPM_PID_UNDEFINED;
+		err_str = "Unable to send a nlmsg";
+		goto remove_mapping_error;
+	}
+	iwpm_print_sockaddr(local_addr,
+			"remove_mapping: Local sockaddr:");
+	return 0;
+remove_mapping_error:
+	pr_info("%s: %s (client = %d)\n", __func__, err_str, nl_client);
+	if (skb)
+		dev_kfree_skb_any(skb);
+	return ret;
+}
+EXPORT_SYMBOL(iwpm_remove_mapping);
+
+/* netlink attribute policy for the received response to register pid request */
+static const struct nla_policy resp_reg_policy[IWPM_NLA_RREG_PID_MAX] = {
+	[IWPM_NLA_RREG_PID_SEQ]     = { .type = NLA_U32 },
+	[IWPM_NLA_RREG_IBDEV_NAME]  = { .type = NLA_STRING,
+					.len = IWPM_DEVNAME_SIZE - 1 },
+	[IWPM_NLA_RREG_ULIB_NAME]   = { .type = NLA_STRING,
+					.len = IWPM_ULIBNAME_SIZE - 1 },
+	[IWPM_NLA_RREG_ULIB_VER]    = { .type = NLA_U16 },
+	[IWPM_NLA_RREG_PID_ERR]     = { .type = NLA_U16 }
+};
+
+/*
+ * iwpm_register_pid_cb - Process a port mapper response to
+ *                        iwpm_register_pid()
+ */
+int iwpm_register_pid_cb(struct sk_buff *skb, struct netlink_callback *cb)
+{
+	struct iwpm_nlmsg_request *nlmsg_request = NULL;
+	struct nlattr *nltb[IWPM_NLA_RREG_PID_MAX];
+	struct iwpm_dev_data *pm_msg;
+	char *dev_name, *iwpm_name;
+	u32 msg_seq;
+	u8 nl_client;
+	u16 iwpm_version;
+	const char *msg_type = "Register Pid response";
+
+	if (iwpm_parse_nlmsg(cb, IWPM_NLA_RREG_PID_MAX,
+				resp_reg_policy, nltb, msg_type))
+		return -EINVAL;
+
+	msg_seq = nla_get_u32(nltb[IWPM_NLA_RREG_PID_SEQ]);
+	nlmsg_request = iwpm_find_nlmsg_request(msg_seq);
+	if (!nlmsg_request) {
+		pr_info("%s: Could not find a matching request (seq = %u)\n",
+				 __func__, msg_seq);
+		return -EINVAL;
+	}
+	pm_msg = nlmsg_request->req_buffer;
+	nl_client = nlmsg_request->nl_client;
+	dev_name = (char *)nla_data(nltb[IWPM_NLA_RREG_IBDEV_NAME]);
+	iwpm_name = (char *)nla_data(nltb[IWPM_NLA_RREG_ULIB_NAME]);
+	iwpm_version = nla_get_u16(nltb[IWPM_NLA_RREG_ULIB_VER]);
+
+	/* check device name, ulib name and version */
+	if (strcmp(pm_msg->dev_name, dev_name) ||
+			strcmp(iwpm_ulib_name, iwpm_name) ||
+			iwpm_version != iwpm_ulib_version) {
+
+		pr_info("%s: Incorrect info (dev = %s name = %s version = %d)\n",
+				__func__, dev_name, iwpm_name, iwpm_version);
+		nlmsg_request->err_code = IWPM_USER_LIB_INFO_ERR;
+		goto register_pid_response_exit;
+	}
+	iwpm_user_pid = cb->nlh->nlmsg_pid;
+	atomic_set(&echo_nlmsg_seq, cb->nlh->nlmsg_seq);
+	pr_debug("%s: iWarp Port Mapper (pid = %d) is available!\n",
+			__func__, iwpm_user_pid);
+	if (iwpm_valid_client(nl_client))
+		iwpm_set_registered(nl_client, 1);
+register_pid_response_exit:
+	nlmsg_request->request_done = 1;
+	/* always for found nlmsg_request */
+	kref_put(&nlmsg_request->kref, iwpm_free_nlmsg_request);
+	barrier();
+	wake_up(&nlmsg_request->waitq);
+	return 0;
+}
+EXPORT_SYMBOL(iwpm_register_pid_cb);
+
+/* netlink attribute policy for the received response to add mapping request */
+static const struct nla_policy resp_add_policy[IWPM_NLA_RMANAGE_MAPPING_MAX] = {
+	[IWPM_NLA_MANAGE_MAPPING_SEQ]     = { .type = NLA_U32 },
+	[IWPM_NLA_MANAGE_ADDR]            = { .len = sizeof(struct sockaddr_storage) },
+	[IWPM_NLA_MANAGE_MAPPED_LOC_ADDR] = { .len = sizeof(struct sockaddr_storage) },
+	[IWPM_NLA_RMANAGE_MAPPING_ERR]	  = { .type = NLA_U16 }
+};
+
+/*
+ * iwpm_add_mapping_cb - Process a port mapper response to
+ *                       iwpm_add_mapping()
+ */
+int iwpm_add_mapping_cb(struct sk_buff *skb, struct netlink_callback *cb)
+{
+	struct iwpm_sa_data *pm_msg;
+	struct iwpm_nlmsg_request *nlmsg_request = NULL;
+	struct nlattr *nltb[IWPM_NLA_RMANAGE_MAPPING_MAX];
+	struct sockaddr_storage *local_sockaddr;
+	struct sockaddr_storage *mapped_sockaddr;
+	const char *msg_type;
+	u32 msg_seq;
+
+	msg_type = "Add Mapping response";
+	if (iwpm_parse_nlmsg(cb, IWPM_NLA_RMANAGE_MAPPING_MAX,
+				resp_add_policy, nltb, msg_type))
+		return -EINVAL;
+
+	atomic_set(&echo_nlmsg_seq, cb->nlh->nlmsg_seq);
+
+	msg_seq = nla_get_u32(nltb[IWPM_NLA_MANAGE_MAPPING_SEQ]);
+	nlmsg_request = iwpm_find_nlmsg_request(msg_seq);
+	if (!nlmsg_request) {
+		pr_info("%s: Could not find a matching request (seq = %u)\n",
+				 __func__, msg_seq);
+		return -EINVAL;
+	}
+	pm_msg = nlmsg_request->req_buffer;
+	local_sockaddr = (struct sockaddr_storage *)
+			nla_data(nltb[IWPM_NLA_MANAGE_ADDR]);
+	mapped_sockaddr = (struct sockaddr_storage *)
+			nla_data(nltb[IWPM_NLA_MANAGE_MAPPED_LOC_ADDR]);
+
+	if (iwpm_compare_sockaddr(local_sockaddr, &pm_msg->loc_addr)) {
+		nlmsg_request->err_code = IWPM_USER_LIB_INFO_ERR;
+		goto add_mapping_response_exit;
+	}
+	if (mapped_sockaddr->ss_family != local_sockaddr->ss_family) {
+		pr_info("%s: Sockaddr family doesn't match the requested one\n",
+				__func__);
+		nlmsg_request->err_code = IWPM_USER_LIB_INFO_ERR;
+		goto add_mapping_response_exit;
+	}
+	memcpy(&pm_msg->mapped_loc_addr, mapped_sockaddr,
+			sizeof(*mapped_sockaddr));
+	iwpm_print_sockaddr(&pm_msg->loc_addr,
+			"add_mapping: Local sockaddr:");
+	iwpm_print_sockaddr(&pm_msg->mapped_loc_addr,
+			"add_mapping: Mapped local sockaddr:");
+
+add_mapping_response_exit:
+	nlmsg_request->request_done = 1;
+	/* always for found request */
+	kref_put(&nlmsg_request->kref, iwpm_free_nlmsg_request);
+	barrier();
+	wake_up(&nlmsg_request->waitq);
+	return 0;
+}
+EXPORT_SYMBOL(iwpm_add_mapping_cb);
+
+/* netlink attribute policy for the response to add and query mapping request */
+static const struct nla_policy resp_query_policy[IWPM_NLA_RQUERY_MAPPING_MAX] = {
+	[IWPM_NLA_QUERY_MAPPING_SEQ]      = { .type = NLA_U32 },
+	[IWPM_NLA_QUERY_LOCAL_ADDR]       = { .len = sizeof(struct sockaddr_storage) },
+	[IWPM_NLA_QUERY_REMOTE_ADDR]      = { .len = sizeof(struct sockaddr_storage) },
+	[IWPM_NLA_RQUERY_MAPPED_LOC_ADDR] = { .len = sizeof(struct sockaddr_storage) },
+	[IWPM_NLA_RQUERY_MAPPED_REM_ADDR] = { .len = sizeof(struct sockaddr_storage) },
+	[IWPM_NLA_RQUERY_MAPPING_ERR]	  = { .type = NLA_U16 }
+};
+
+/*
+ * iwpm_add_and_query_mapping_cb - Process a port mapper response to
+ *                                 iwpm_add_and_query_mapping()
+ */
+int iwpm_add_and_query_mapping_cb(struct sk_buff *skb,
+				struct netlink_callback *cb)
+{
+	struct iwpm_sa_data *pm_msg;
+	struct iwpm_nlmsg_request *nlmsg_request = NULL;
+	struct nlattr *nltb[IWPM_NLA_RQUERY_MAPPING_MAX];
+	struct sockaddr_storage *local_sockaddr, *remote_sockaddr;
+	struct sockaddr_storage *mapped_loc_sockaddr, *mapped_rem_sockaddr;
+	const char *msg_type;
+	u32 msg_seq;
+	u16 err_code;
+
+	msg_type = "Query Mapping response";
+	if (iwpm_parse_nlmsg(cb, IWPM_NLA_RQUERY_MAPPING_MAX,
+				resp_query_policy, nltb, msg_type))
+		return -EINVAL;
+	atomic_set(&echo_nlmsg_seq, cb->nlh->nlmsg_seq);
+
+	msg_seq = nla_get_u32(nltb[IWPM_NLA_QUERY_MAPPING_SEQ]);
+	nlmsg_request = iwpm_find_nlmsg_request(msg_seq);
+	if (!nlmsg_request) {
+		pr_info("%s: Could not find a matching request (seq = %u)\n",
+				 __func__, msg_seq);
+			return -EINVAL;
+	}
+	pm_msg = nlmsg_request->req_buffer;
+	local_sockaddr = (struct sockaddr_storage *)
+			nla_data(nltb[IWPM_NLA_QUERY_LOCAL_ADDR]);
+	remote_sockaddr = (struct sockaddr_storage *)
+			nla_data(nltb[IWPM_NLA_QUERY_REMOTE_ADDR]);
+	mapped_loc_sockaddr = (struct sockaddr_storage *)
+			nla_data(nltb[IWPM_NLA_RQUERY_MAPPED_LOC_ADDR]);
+	mapped_rem_sockaddr = (struct sockaddr_storage *)
+			nla_data(nltb[IWPM_NLA_RQUERY_MAPPED_REM_ADDR]);
+
+	err_code = nla_get_u16(nltb[IWPM_NLA_RQUERY_MAPPING_ERR]);
+	if (err_code == IWPM_REMOTE_QUERY_REJECT) {
+		pr_info("%s: Received a Reject (pid = %u, echo seq = %u)\n",
+			__func__, cb->nlh->nlmsg_pid, msg_seq);
+		nlmsg_request->err_code = IWPM_REMOTE_QUERY_REJECT;
+	}
+	if (iwpm_compare_sockaddr(local_sockaddr, &pm_msg->loc_addr) ||
+		iwpm_compare_sockaddr(remote_sockaddr, &pm_msg->rem_addr)) {
+		pr_info("%s: Incorrect local sockaddr\n", __func__);
+		nlmsg_request->err_code = IWPM_USER_LIB_INFO_ERR;
+		goto query_mapping_response_exit;
+	}
+	if (mapped_loc_sockaddr->ss_family != local_sockaddr->ss_family ||
+		mapped_rem_sockaddr->ss_family != remote_sockaddr->ss_family) {
+		pr_info("%s: Sockaddr family doesn't match the requested one\n",
+				__func__);
+		nlmsg_request->err_code = IWPM_USER_LIB_INFO_ERR;
+		goto query_mapping_response_exit;
+	}
+	memcpy(&pm_msg->mapped_loc_addr, mapped_loc_sockaddr,
+			sizeof(*mapped_loc_sockaddr));
+	memcpy(&pm_msg->mapped_rem_addr, mapped_rem_sockaddr,
+			sizeof(*mapped_rem_sockaddr));
+
+	iwpm_print_sockaddr(&pm_msg->loc_addr,
+			"query_mapping: Local sockaddr:");
+	iwpm_print_sockaddr(&pm_msg->mapped_loc_addr,
+			"query_mapping: Mapped local sockaddr:");
+	iwpm_print_sockaddr(&pm_msg->rem_addr,
+			"query_mapping: Remote sockaddr:");
+	iwpm_print_sockaddr(&pm_msg->mapped_rem_addr,
+			"query_mapping: Mapped remote sockaddr:");
+query_mapping_response_exit:
+	nlmsg_request->request_done = 1;
+	/* always for found request */
+	kref_put(&nlmsg_request->kref, iwpm_free_nlmsg_request);
+	barrier();
+	wake_up(&nlmsg_request->waitq);
+	return 0;
+}
+EXPORT_SYMBOL(iwpm_add_and_query_mapping_cb);
+
+/* netlink attribute policy for the received request for mapping info */
+static const struct nla_policy resp_mapinfo_policy[IWPM_NLA_MAPINFO_REQ_MAX] = {
+	[IWPM_NLA_MAPINFO_ULIB_NAME] = { .type = NLA_STRING,
+					.len = IWPM_ULIBNAME_SIZE - 1 },
+	[IWPM_NLA_MAPINFO_ULIB_VER]  = { .type = NLA_U16 }
+};
+
+/*
+ * iwpm_mapping_info_cb - Process a port mapper request for mapping info
+ */
+int iwpm_mapping_info_cb(struct sk_buff *skb, struct netlink_callback *cb)
+{
+	struct nlattr *nltb[IWPM_NLA_MAPINFO_REQ_MAX];
+	const char *msg_type = "Mapping Info response";
+	int iwpm_pid;
+	u8 nl_client;
+	char *iwpm_name;
+	u16 iwpm_version;
+	int ret = -EINVAL;
+
+	if (iwpm_parse_nlmsg(cb, IWPM_NLA_MAPINFO_REQ_MAX,
+				resp_mapinfo_policy, nltb, msg_type)) {
+		pr_info("%s: Unable to parse nlmsg\n", __func__);
+		return ret;
+	}
+	iwpm_name = (char *)nla_data(nltb[IWPM_NLA_MAPINFO_ULIB_NAME]);
+	iwpm_version = nla_get_u16(nltb[IWPM_NLA_MAPINFO_ULIB_VER]);
+	if (strcmp(iwpm_ulib_name, iwpm_name) ||
+			iwpm_version != iwpm_ulib_version) {
+		pr_info("%s: Invalid port mapper name = %s version = %d\n",
+				__func__, iwpm_name, iwpm_version);
+		return ret;
+	}
+	nl_client = RDMA_NL_GET_CLIENT(cb->nlh->nlmsg_type);
+	if (!iwpm_valid_client(nl_client)) {
+		pr_info("%s: Invalid port mapper client = %d\n",
+				__func__, nl_client);
+		return ret;
+	}
+	iwpm_set_registered(nl_client, 0);
+	atomic_set(&echo_nlmsg_seq, cb->nlh->nlmsg_seq);
+	if (!iwpm_mapinfo_available())
+		return 0;
+	iwpm_pid = cb->nlh->nlmsg_pid;
+	pr_debug("%s: iWarp Port Mapper (pid = %d) is available!\n",
+		 __func__, iwpm_pid);
+	ret = iwpm_send_mapinfo(nl_client, iwpm_pid);
+	return ret;
+}
+EXPORT_SYMBOL(iwpm_mapping_info_cb);
+
+/* netlink attribute policy for the received mapping info ack */
+static const struct nla_policy ack_mapinfo_policy[IWPM_NLA_MAPINFO_NUM_MAX] = {
+	[IWPM_NLA_MAPINFO_SEQ]    =   { .type = NLA_U32 },
+	[IWPM_NLA_MAPINFO_SEND_NUM] = { .type = NLA_U32 },
+	[IWPM_NLA_MAPINFO_ACK_NUM] =  { .type = NLA_U32 }
+};
+
+/*
+ * iwpm_ack_mapping_info_cb - Process a port mapper ack for
+ *                            the provided mapping info records
+ */
+int iwpm_ack_mapping_info_cb(struct sk_buff *skb, struct netlink_callback *cb)
+{
+	struct nlattr *nltb[IWPM_NLA_MAPINFO_NUM_MAX];
+	u32 mapinfo_send, mapinfo_ack;
+	const char *msg_type = "Mapping Info Ack";
+
+	if (iwpm_parse_nlmsg(cb, IWPM_NLA_MAPINFO_NUM_MAX,
+				ack_mapinfo_policy, nltb, msg_type))
+		return -EINVAL;
+	mapinfo_send = nla_get_u32(nltb[IWPM_NLA_MAPINFO_SEND_NUM]);
+	mapinfo_ack = nla_get_u32(nltb[IWPM_NLA_MAPINFO_ACK_NUM]);
+	if (mapinfo_ack != mapinfo_send)
+		pr_info("%s: Invalid mapinfo number (sent = %u ack-ed = %u)\n",
+			__func__, mapinfo_send, mapinfo_ack);
+	atomic_set(&echo_nlmsg_seq, cb->nlh->nlmsg_seq);
+	return 0;
+}
+EXPORT_SYMBOL(iwpm_ack_mapping_info_cb);
+
+/* netlink attribute policy for the received port mapper error message */
+static const struct nla_policy map_error_policy[IWPM_NLA_ERR_MAX] = {
+	[IWPM_NLA_ERR_SEQ]        = { .type = NLA_U32 },
+	[IWPM_NLA_ERR_CODE]       = { .type = NLA_U16 },
+};
+
+/*
+ * iwpm_mapping_error_cb - Process a port mapper error message
+ */
+int iwpm_mapping_error_cb(struct sk_buff *skb, struct netlink_callback *cb)
+{
+	struct iwpm_nlmsg_request *nlmsg_request = NULL;
+	int nl_client = RDMA_NL_GET_CLIENT(cb->nlh->nlmsg_type);
+	struct nlattr *nltb[IWPM_NLA_ERR_MAX];
+	u32 msg_seq;
+	u16 err_code;
+	const char *msg_type = "Mapping Error Msg";
+
+	if (iwpm_parse_nlmsg(cb, IWPM_NLA_ERR_MAX,
+				map_error_policy, nltb, msg_type))
+		return -EINVAL;
+
+	msg_seq = nla_get_u32(nltb[IWPM_NLA_ERR_SEQ]);
+	err_code = nla_get_u16(nltb[IWPM_NLA_ERR_CODE]);
+	pr_info("%s: Received msg seq = %u err code = %u client = %d\n",
+				__func__, msg_seq, err_code, nl_client);
+	/* look for nlmsg_request */
+	nlmsg_request = iwpm_find_nlmsg_request(msg_seq);
+	if (!nlmsg_request) {
+		/* not all errors have associated requests */
+		pr_debug("Could not find matching req (seq = %u)\n", msg_seq);
+		return 0;
+	}
+	atomic_set(&echo_nlmsg_seq, cb->nlh->nlmsg_seq);
+	nlmsg_request->err_code = err_code;
+	nlmsg_request->request_done = 1;
+	/* always for found request */
+	kref_put(&nlmsg_request->kref, iwpm_free_nlmsg_request);
+	barrier();
+	wake_up(&nlmsg_request->waitq);
+	return 0;
+}
+EXPORT_SYMBOL(iwpm_mapping_error_cb);
diff --git a/drivers/infiniband/core/iwpm_util.c b/drivers/infiniband/core/iwpm_util.c
new file mode 100644
index 00000000000..69e9f84c160
--- /dev/null
+++ b/drivers/infiniband/core/iwpm_util.c
@@ -0,0 +1,607 @@
+/*
+ * Copyright (c) 2014 Chelsio, Inc. All rights reserved.
+ * Copyright (c) 2014 Intel Corporation. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     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.
+ *
+ * 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 "iwpm_util.h"
+
+#define IWPM_HASH_BUCKET_SIZE	512
+#define IWPM_HASH_BUCKET_MASK	(IWPM_HASH_BUCKET_SIZE - 1)
+
+static LIST_HEAD(iwpm_nlmsg_req_list);
+static DEFINE_SPINLOCK(iwpm_nlmsg_req_lock);
+
+static struct hlist_head *iwpm_hash_bucket;
+static DEFINE_SPINLOCK(iwpm_mapinfo_lock);
+
+static DEFINE_MUTEX(iwpm_admin_lock);
+static struct iwpm_admin_data iwpm_admin;
+
+int iwpm_init(u8 nl_client)
+{
+	if (iwpm_valid_client(nl_client))
+		return -EINVAL;
+	mutex_lock(&iwpm_admin_lock);
+	if (atomic_read(&iwpm_admin.refcount) == 0) {
+		iwpm_hash_bucket = kzalloc(IWPM_HASH_BUCKET_SIZE *
+					sizeof(struct hlist_head), GFP_KERNEL);
+		if (!iwpm_hash_bucket) {
+			mutex_unlock(&iwpm_admin_lock);
+			pr_err("%s Unable to create mapinfo hash table\n", __func__);
+			return -ENOMEM;
+		}
+	}
+	atomic_inc(&iwpm_admin.refcount);
+	mutex_unlock(&iwpm_admin_lock);
+	iwpm_set_valid(nl_client, 1);
+	return 0;
+}
+EXPORT_SYMBOL(iwpm_init);
+
+static void free_hash_bucket(void);
+
+int iwpm_exit(u8 nl_client)
+{
+
+	if (!iwpm_valid_client(nl_client))
+		return -EINVAL;
+	mutex_lock(&iwpm_admin_lock);
+	if (atomic_read(&iwpm_admin.refcount) == 0) {
+		mutex_unlock(&iwpm_admin_lock);
+		pr_err("%s Incorrect usage - negative refcount\n", __func__);
+		return -EINVAL;
+	}
+	if (atomic_dec_and_test(&iwpm_admin.refcount)) {
+		free_hash_bucket();
+		pr_debug("%s: Mapinfo hash table is destroyed\n", __func__);
+	}
+	mutex_unlock(&iwpm_admin_lock);
+	iwpm_set_valid(nl_client, 0);
+	return 0;
+}
+EXPORT_SYMBOL(iwpm_exit);
+
+static struct hlist_head *get_hash_bucket_head(struct sockaddr_storage *,
+					       struct sockaddr_storage *);
+
+int iwpm_create_mapinfo(struct sockaddr_storage *local_sockaddr,
+			struct sockaddr_storage *mapped_sockaddr,
+			u8 nl_client)
+{
+	struct hlist_head *hash_bucket_head;
+	struct iwpm_mapping_info *map_info;
+	unsigned long flags;
+
+	if (!iwpm_valid_client(nl_client))
+		return -EINVAL;
+	map_info = kzalloc(sizeof(struct iwpm_mapping_info), GFP_KERNEL);
+	if (!map_info) {
+		pr_err("%s: Unable to allocate a mapping info\n", __func__);
+		return -ENOMEM;
+	}
+	memcpy(&map_info->local_sockaddr, local_sockaddr,
+	       sizeof(struct sockaddr_storage));
+	memcpy(&map_info->mapped_sockaddr, mapped_sockaddr,
+	       sizeof(struct sockaddr_storage));
+	map_info->nl_client = nl_client;
+
+	spin_lock_irqsave(&iwpm_mapinfo_lock, flags);
+	if (iwpm_hash_bucket) {
+		hash_bucket_head = get_hash_bucket_head(
+					&map_info->local_sockaddr,
+					&map_info->mapped_sockaddr);
+		hlist_add_head(&map_info->hlist_node, hash_bucket_head);
+	}
+	spin_unlock_irqrestore(&iwpm_mapinfo_lock, flags);
+	return 0;
+}
+EXPORT_SYMBOL(iwpm_create_mapinfo);
+
+int iwpm_remove_mapinfo(struct sockaddr_storage *local_sockaddr,
+			struct sockaddr_storage *mapped_local_addr)
+{
+	struct hlist_node *tmp_hlist_node;
+	struct hlist_head *hash_bucket_head;
+	struct iwpm_mapping_info *map_info = NULL;
+	unsigned long flags;
+	int ret = -EINVAL;
+
+	spin_lock_irqsave(&iwpm_mapinfo_lock, flags);
+	if (iwpm_hash_bucket) {
+		hash_bucket_head = get_hash_bucket_head(
+					local_sockaddr,
+					mapped_local_addr);
+		hlist_for_each_entry_safe(map_info, tmp_hlist_node,
+					hash_bucket_head, hlist_node) {
+
+			if (!iwpm_compare_sockaddr(&map_info->mapped_sockaddr,
+						mapped_local_addr)) {
+
+				hlist_del_init(&map_info->hlist_node);
+				kfree(map_info);
+				ret = 0;
+				break;
+			}
+		}
+	}
+	spin_unlock_irqrestore(&iwpm_mapinfo_lock, flags);
+	return ret;
+}
+EXPORT_SYMBOL(iwpm_remove_mapinfo);
+
+static void free_hash_bucket(void)
+{
+	struct hlist_node *tmp_hlist_node;
+	struct iwpm_mapping_info *map_info;
+	unsigned long flags;
+	int i;
+
+	/* remove all the mapinfo data from the list */
+	spin_lock_irqsave(&iwpm_mapinfo_lock, flags);
+	for (i = 0; i < IWPM_HASH_BUCKET_SIZE; i++) {
+		hlist_for_each_entry_safe(map_info, tmp_hlist_node,
+			&iwpm_hash_bucket[i], hlist_node) {
+
+				hlist_del_init(&map_info->hlist_node);
+				kfree(map_info);
+			}
+	}
+	/* free the hash list */
+	kfree(iwpm_hash_bucket);
+	iwpm_hash_bucket = NULL;
+	spin_unlock_irqrestore(&iwpm_mapinfo_lock, flags);
+}
+
+struct iwpm_nlmsg_request *iwpm_get_nlmsg_request(__u32 nlmsg_seq,
+					u8 nl_client, gfp_t gfp)
+{
+	struct iwpm_nlmsg_request *nlmsg_request = NULL;
+	unsigned long flags;
+
+	nlmsg_request = kzalloc(sizeof(struct iwpm_nlmsg_request), gfp);
+	if (!nlmsg_request) {
+		pr_err("%s Unable to allocate a nlmsg_request\n", __func__);
+		return NULL;
+	}
+	spin_lock_irqsave(&iwpm_nlmsg_req_lock, flags);
+	list_add_tail(&nlmsg_request->inprocess_list, &iwpm_nlmsg_req_list);
+	spin_unlock_irqrestore(&iwpm_nlmsg_req_lock, flags);
+
+	kref_init(&nlmsg_request->kref);
+	kref_get(&nlmsg_request->kref);
+	nlmsg_request->nlmsg_seq = nlmsg_seq;
+	nlmsg_request->nl_client = nl_client;
+	nlmsg_request->request_done = 0;
+	nlmsg_request->err_code = 0;
+	return nlmsg_request;
+}
+
+void iwpm_free_nlmsg_request(struct kref *kref)
+{
+	struct iwpm_nlmsg_request *nlmsg_request;
+	unsigned long flags;
+
+	nlmsg_request = container_of(kref, struct iwpm_nlmsg_request, kref);
+
+	spin_lock_irqsave(&iwpm_nlmsg_req_lock, flags);
+	list_del_init(&nlmsg_request->inprocess_list);
+	spin_unlock_irqrestore(&iwpm_nlmsg_req_lock, flags);
+
+	if (!nlmsg_request->request_done)
+		pr_debug("%s Freeing incomplete nlmsg request (seq = %u).\n",
+			__func__, nlmsg_request->nlmsg_seq);
+	kfree(nlmsg_request);
+}
+
+struct iwpm_nlmsg_request *iwpm_find_nlmsg_request(__u32 echo_seq)
+{
+	struct iwpm_nlmsg_request *nlmsg_request;
+	struct iwpm_nlmsg_request *found_request = NULL;
+	unsigned long flags;
+
+	spin_lock_irqsave(&iwpm_nlmsg_req_lock, flags);
+	list_for_each_entry(nlmsg_request, &iwpm_nlmsg_req_list,
+			    inprocess_list) {
+		if (nlmsg_request->nlmsg_seq == echo_seq) {
+			found_request = nlmsg_request;
+			kref_get(&nlmsg_request->kref);
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&iwpm_nlmsg_req_lock, flags);
+	return found_request;
+}
+
+int iwpm_wait_complete_req(struct iwpm_nlmsg_request *nlmsg_request)
+{
+	int ret;
+	init_waitqueue_head(&nlmsg_request->waitq);
+
+	ret = wait_event_timeout(nlmsg_request->waitq,
+			(nlmsg_request->request_done != 0), IWPM_NL_TIMEOUT);
+	if (!ret) {
+		ret = -EINVAL;
+		pr_info("%s: Timeout %d sec for netlink request (seq = %u)\n",
+			__func__, (IWPM_NL_TIMEOUT/HZ), nlmsg_request->nlmsg_seq);
+	} else {
+		ret = nlmsg_request->err_code;
+	}
+	kref_put(&nlmsg_request->kref, iwpm_free_nlmsg_request);
+	return ret;
+}
+
+int iwpm_get_nlmsg_seq(void)
+{
+	return atomic_inc_return(&iwpm_admin.nlmsg_seq);
+}
+
+int iwpm_valid_client(u8 nl_client)
+{
+	if (nl_client >= RDMA_NL_NUM_CLIENTS)
+		return 0;
+	return iwpm_admin.client_list[nl_client];
+}
+
+void iwpm_set_valid(u8 nl_client, int valid)
+{
+	if (nl_client >= RDMA_NL_NUM_CLIENTS)
+		return;
+	iwpm_admin.client_list[nl_client] = valid;
+}
+
+/* valid client */
+int iwpm_registered_client(u8 nl_client)
+{
+	return iwpm_admin.reg_list[nl_client];
+}
+
+/* valid client */
+void iwpm_set_registered(u8 nl_client, int reg)
+{
+	iwpm_admin.reg_list[nl_client] = reg;
+}
+
+int iwpm_compare_sockaddr(struct sockaddr_storage *a_sockaddr,
+				struct sockaddr_storage *b_sockaddr)
+{
+	if (a_sockaddr->ss_family != b_sockaddr->ss_family)
+		return 1;
+	if (a_sockaddr->ss_family == AF_INET) {
+		struct sockaddr_in *a4_sockaddr =
+			(struct sockaddr_in *)a_sockaddr;
+		struct sockaddr_in *b4_sockaddr =
+			(struct sockaddr_in *)b_sockaddr;
+		if (!memcmp(&a4_sockaddr->sin_addr,
+			&b4_sockaddr->sin_addr, sizeof(struct in_addr))
+			&& a4_sockaddr->sin_port == b4_sockaddr->sin_port)
+				return 0;
+
+	} else if (a_sockaddr->ss_family == AF_INET6) {
+		struct sockaddr_in6 *a6_sockaddr =
+			(struct sockaddr_in6 *)a_sockaddr;
+		struct sockaddr_in6 *b6_sockaddr =
+			(struct sockaddr_in6 *)b_sockaddr;
+		if (!memcmp(&a6_sockaddr->sin6_addr,
+			&b6_sockaddr->sin6_addr, sizeof(struct in6_addr))
+			&& a6_sockaddr->sin6_port == b6_sockaddr->sin6_port)
+				return 0;
+
+	} else {
+		pr_err("%s: Invalid sockaddr family\n", __func__);
+	}
+	return 1;
+}
+
+struct sk_buff *iwpm_create_nlmsg(u32 nl_op, struct nlmsghdr **nlh,
+						int nl_client)
+{
+	struct sk_buff *skb = NULL;
+
+	skb = dev_alloc_skb(NLMSG_GOODSIZE);
+	if (!skb) {
+		pr_err("%s Unable to allocate skb\n", __func__);
+		goto create_nlmsg_exit;
+	}
+	if (!(ibnl_put_msg(skb, nlh, 0, 0, nl_client, nl_op,
+			   NLM_F_REQUEST))) {
+		pr_warn("%s: Unable to put the nlmsg header\n", __func__);
+		dev_kfree_skb(skb);
+		skb = NULL;
+	}
+create_nlmsg_exit:
+	return skb;
+}
+
+int iwpm_parse_nlmsg(struct netlink_callback *cb, int policy_max,
+				   const struct nla_policy *nlmsg_policy,
+				   struct nlattr *nltb[], const char *msg_type)
+{
+	int nlh_len = 0;
+	int ret;
+	const char *err_str = "";
+
+	ret = nlmsg_validate(cb->nlh, nlh_len, policy_max-1, nlmsg_policy);
+	if (ret) {
+		err_str = "Invalid attribute";
+		goto parse_nlmsg_error;
+	}
+	ret = nlmsg_parse(cb->nlh, nlh_len, nltb, policy_max-1, nlmsg_policy);
+	if (ret) {
+		err_str = "Unable to parse the nlmsg";
+		goto parse_nlmsg_error;
+	}
+	ret = iwpm_validate_nlmsg_attr(nltb, policy_max);
+	if (ret) {
+		err_str = "Invalid NULL attribute";
+		goto parse_nlmsg_error;
+	}
+	return 0;
+parse_nlmsg_error:
+	pr_warn("%s: %s (msg type %s ret = %d)\n",
+			__func__, err_str, msg_type, ret);
+	return ret;
+}
+
+void iwpm_print_sockaddr(struct sockaddr_storage *sockaddr, char *msg)
+{
+	struct sockaddr_in6 *sockaddr_v6;
+	struct sockaddr_in *sockaddr_v4;
+
+	switch (sockaddr->ss_family) {
+	case AF_INET:
+		sockaddr_v4 = (struct sockaddr_in *)sockaddr;
+		pr_debug("%s IPV4 %pI4: %u(0x%04X)\n",
+			msg, &sockaddr_v4->sin_addr,
+			ntohs(sockaddr_v4->sin_port),
+			ntohs(sockaddr_v4->sin_port));
+		break;
+	case AF_INET6:
+		sockaddr_v6 = (struct sockaddr_in6 *)sockaddr;
+		pr_debug("%s IPV6 %pI6: %u(0x%04X)\n",
+			msg, &sockaddr_v6->sin6_addr,
+			ntohs(sockaddr_v6->sin6_port),
+			ntohs(sockaddr_v6->sin6_port));
+		break;
+	default:
+		break;
+	}
+}
+
+static u32 iwpm_ipv6_jhash(struct sockaddr_in6 *ipv6_sockaddr)
+{
+	u32 ipv6_hash = jhash(&ipv6_sockaddr->sin6_addr, sizeof(struct in6_addr), 0);
+	u32 hash = jhash_2words(ipv6_hash, (__force u32) ipv6_sockaddr->sin6_port, 0);
+	return hash;
+}
+
+static u32 iwpm_ipv4_jhash(struct sockaddr_in *ipv4_sockaddr)
+{
+	u32 ipv4_hash = jhash(&ipv4_sockaddr->sin_addr, sizeof(struct in_addr), 0);
+	u32 hash = jhash_2words(ipv4_hash, (__force u32) ipv4_sockaddr->sin_port, 0);
+	return hash;
+}
+
+static struct hlist_head *get_hash_bucket_head(struct sockaddr_storage
+					       *local_sockaddr,
+					       struct sockaddr_storage
+					       *mapped_sockaddr)
+{
+	u32 local_hash, mapped_hash, hash;
+
+	if (local_sockaddr->ss_family == AF_INET) {
+		local_hash = iwpm_ipv4_jhash((struct sockaddr_in *) local_sockaddr);
+		mapped_hash = iwpm_ipv4_jhash((struct sockaddr_in *) mapped_sockaddr);
+
+	} else if (local_sockaddr->ss_family == AF_INET6) {
+		local_hash = iwpm_ipv6_jhash((struct sockaddr_in6 *) local_sockaddr);
+		mapped_hash = iwpm_ipv6_jhash((struct sockaddr_in6 *) mapped_sockaddr);
+	} else {
+		pr_err("%s: Invalid sockaddr family\n", __func__);
+		return NULL;
+	}
+
+	if (local_hash == mapped_hash) /* if port mapper isn't available */
+		hash = local_hash;
+	else
+		hash = jhash_2words(local_hash, mapped_hash, 0);
+
+	return &iwpm_hash_bucket[hash & IWPM_HASH_BUCKET_MASK];
+}
+
+static int send_mapinfo_num(u32 mapping_num, u8 nl_client, int iwpm_pid)
+{
+	struct sk_buff *skb = NULL;
+	struct nlmsghdr *nlh;
+	u32 msg_seq;
+	const char *err_str = "";
+	int ret = -EINVAL;
+
+	skb = iwpm_create_nlmsg(RDMA_NL_IWPM_MAPINFO_NUM, &nlh, nl_client);
+	if (!skb) {
+		err_str = "Unable to create a nlmsg";
+		goto mapinfo_num_error;
+	}
+	nlh->nlmsg_seq = iwpm_get_nlmsg_seq();
+	msg_seq = 0;
+	err_str = "Unable to put attribute of mapinfo number nlmsg";
+	ret = ibnl_put_attr(skb, nlh, sizeof(u32), &msg_seq, IWPM_NLA_MAPINFO_SEQ);
+	if (ret)
+		goto mapinfo_num_error;
+	ret = ibnl_put_attr(skb, nlh, sizeof(u32),
+				&mapping_num, IWPM_NLA_MAPINFO_SEND_NUM);
+	if (ret)
+		goto mapinfo_num_error;
+	ret = ibnl_unicast(skb, nlh, iwpm_pid);
+	if (ret) {
+		skb = NULL;
+		err_str = "Unable to send a nlmsg";
+		goto mapinfo_num_error;
+	}
+	pr_debug("%s: Sent mapping number = %d\n", __func__, mapping_num);
+	return 0;
+mapinfo_num_error:
+	pr_info("%s: %s\n", __func__, err_str);
+	if (skb)
+		dev_kfree_skb(skb);
+	return ret;
+}
+
+static int send_nlmsg_done(struct sk_buff *skb, u8 nl_client, int iwpm_pid)
+{
+	struct nlmsghdr *nlh = NULL;
+	int ret = 0;
+
+	if (!skb)
+		return ret;
+	if (!(ibnl_put_msg(skb, &nlh, 0, 0, nl_client,
+			   RDMA_NL_IWPM_MAPINFO, NLM_F_MULTI))) {
+		pr_warn("%s Unable to put NLMSG_DONE\n", __func__);
+		return -ENOMEM;
+	}
+	nlh->nlmsg_type = NLMSG_DONE;
+	ret = ibnl_unicast(skb, (struct nlmsghdr *)skb->data, iwpm_pid);
+	if (ret)
+		pr_warn("%s Unable to send a nlmsg\n", __func__);
+	return ret;
+}
+
+int iwpm_send_mapinfo(u8 nl_client, int iwpm_pid)
+{
+	struct iwpm_mapping_info *map_info;
+	struct sk_buff *skb = NULL;
+	struct nlmsghdr *nlh;
+	int skb_num = 0, mapping_num = 0;
+	int i = 0, nlmsg_bytes = 0;
+	unsigned long flags;
+	const char *err_str = "";
+	int ret;
+
+	skb = dev_alloc_skb(NLMSG_GOODSIZE);
+	if (!skb) {
+		ret = -ENOMEM;
+		err_str = "Unable to allocate skb";
+		goto send_mapping_info_exit;
+	}
+	skb_num++;
+	spin_lock_irqsave(&iwpm_mapinfo_lock, flags);
+	for (i = 0; i < IWPM_HASH_BUCKET_SIZE; i++) {
+		hlist_for_each_entry(map_info, &iwpm_hash_bucket[i],
+				     hlist_node) {
+			if (map_info->nl_client != nl_client)
+				continue;
+			nlh = NULL;
+			if (!(ibnl_put_msg(skb, &nlh, 0, 0, nl_client,
+					RDMA_NL_IWPM_MAPINFO, NLM_F_MULTI))) {
+				ret = -ENOMEM;
+				err_str = "Unable to put the nlmsg header";
+				goto send_mapping_info_unlock;
+			}
+			err_str = "Unable to put attribute of the nlmsg";
+			ret = ibnl_put_attr(skb, nlh,
+					sizeof(struct sockaddr_storage),
+					&map_info->local_sockaddr,
+					IWPM_NLA_MAPINFO_LOCAL_ADDR);
+			if (ret)
+				goto send_mapping_info_unlock;
+
+			ret = ibnl_put_attr(skb, nlh,
+					sizeof(struct sockaddr_storage),
+					&map_info->mapped_sockaddr,
+					IWPM_NLA_MAPINFO_MAPPED_ADDR);
+			if (ret)
+				goto send_mapping_info_unlock;
+
+			iwpm_print_sockaddr(&map_info->local_sockaddr,
+				"send_mapping_info: Local sockaddr:");
+			iwpm_print_sockaddr(&map_info->mapped_sockaddr,
+				"send_mapping_info: Mapped local sockaddr:");
+			mapping_num++;
+			nlmsg_bytes += nlh->nlmsg_len;
+
+			/* check if all mappings can fit in one skb */
+			if (NLMSG_GOODSIZE - nlmsg_bytes < nlh->nlmsg_len * 2) {
+				/* and leave room for NLMSG_DONE */
+				nlmsg_bytes = 0;
+				skb_num++;
+				spin_unlock_irqrestore(&iwpm_mapinfo_lock,
+						       flags);
+				/* send the skb */
+				ret = send_nlmsg_done(skb, nl_client, iwpm_pid);
+				skb = NULL;
+				if (ret) {
+					err_str = "Unable to send map info";
+					goto send_mapping_info_exit;
+				}
+				if (skb_num == IWPM_MAPINFO_SKB_COUNT) {
+					ret = -ENOMEM;
+					err_str = "Insufficient skbs for map info";
+					goto send_mapping_info_exit;
+				}
+				skb = dev_alloc_skb(NLMSG_GOODSIZE);
+				if (!skb) {
+					ret = -ENOMEM;
+					err_str = "Unable to allocate skb";
+					goto send_mapping_info_exit;
+				}
+				spin_lock_irqsave(&iwpm_mapinfo_lock, flags);
+			}
+		}
+	}
+send_mapping_info_unlock:
+	spin_unlock_irqrestore(&iwpm_mapinfo_lock, flags);
+send_mapping_info_exit:
+	if (ret) {
+		pr_warn("%s: %s (ret = %d)\n", __func__, err_str, ret);
+		if (skb)
+			dev_kfree_skb(skb);
+		return ret;
+	}
+	send_nlmsg_done(skb, nl_client, iwpm_pid);
+	return send_mapinfo_num(mapping_num, nl_client, iwpm_pid);
+}
+
+int iwpm_mapinfo_available(void)
+{
+	unsigned long flags;
+	int full_bucket = 0, i = 0;
+
+	spin_lock_irqsave(&iwpm_mapinfo_lock, flags);
+	if (iwpm_hash_bucket) {
+		for (i = 0; i < IWPM_HASH_BUCKET_SIZE; i++) {
+			if (!hlist_empty(&iwpm_hash_bucket[i])) {
+				full_bucket = 1;
+				break;
+			}
+		}
+	}
+	spin_unlock_irqrestore(&iwpm_mapinfo_lock, flags);
+	return full_bucket;
+}
diff --git a/drivers/infiniband/core/iwpm_util.h b/drivers/infiniband/core/iwpm_util.h
new file mode 100644
index 00000000000..9777c869a14
--- /dev/null
+++ b/drivers/infiniband/core/iwpm_util.h
@@ -0,0 +1,238 @@
+/*
+ * Copyright (c) 2014 Intel Corporation. All rights reserved.
+ * Copyright (c) 2014 Chelsio, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     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.
+ *
+ * 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 _IWPM_UTIL_H
+#define _IWPM_UTIL_H
+
+#include <linux/module.h>
+#include <linux/io.h>
+#include <linux/in.h>
+#include <linux/in6.h>
+#include <linux/spinlock.h>
+#include <linux/kernel.h>
+#include <linux/netdevice.h>
+#include <linux/delay.h>
+#include <linux/workqueue.h>
+#include <linux/mutex.h>
+#include <linux/jhash.h>
+#include <linux/kref.h>
+#include <net/netlink.h>
+#include <linux/errno.h>
+#include <rdma/iw_portmap.h>
+#include <rdma/rdma_netlink.h>
+
+
+#define IWPM_NL_RETRANS		3
+#define IWPM_NL_TIMEOUT		(10*HZ)
+#define IWPM_MAPINFO_SKB_COUNT	20
+
+#define IWPM_PID_UNDEFINED     -1
+#define IWPM_PID_UNAVAILABLE   -2
+
+struct iwpm_nlmsg_request {
+	struct list_head    inprocess_list;
+	__u32               nlmsg_seq;
+	void                *req_buffer;
+	u8	            nl_client;
+	u8                  request_done;
+	u16                 err_code;
+	wait_queue_head_t   waitq;
+	struct kref         kref;
+};
+
+struct iwpm_mapping_info {
+	struct hlist_node hlist_node;
+	struct sockaddr_storage local_sockaddr;
+	struct sockaddr_storage mapped_sockaddr;
+	u8     nl_client;
+};
+
+struct iwpm_admin_data {
+	atomic_t refcount;
+	atomic_t nlmsg_seq;
+	int      client_list[RDMA_NL_NUM_CLIENTS];
+	int      reg_list[RDMA_NL_NUM_CLIENTS];
+};
+
+/**
+ * iwpm_get_nlmsg_request - Allocate and initialize netlink message request
+ * @nlmsg_seq: Sequence number of the netlink message
+ * @nl_client: The index of the netlink client
+ * @gfp: Indicates how the memory for the request should be allocated
+ *
+ * Returns the newly allocated netlink request object if successful,
+ * otherwise returns NULL
+ */
+struct iwpm_nlmsg_request *iwpm_get_nlmsg_request(__u32 nlmsg_seq,
+						u8 nl_client, gfp_t gfp);
+
+/**
+ * iwpm_free_nlmsg_request - Deallocate netlink message request
+ * @kref: Holds reference of netlink message request
+ */
+void iwpm_free_nlmsg_request(struct kref *kref);
+
+/**
+ * iwpm_find_nlmsg_request - Find netlink message request in the request list
+ * @echo_seq: Sequence number of the netlink request to find
+ *
+ * Returns the found netlink message request,
+ * if not found, returns NULL
+ */
+struct iwpm_nlmsg_request *iwpm_find_nlmsg_request(__u32 echo_seq);
+
+/**
+ * iwpm_wait_complete_req - Block while servicing the netlink request
+ * @nlmsg_request: Netlink message request to service
+ *
+ * Wakes up, after the request is completed or expired
+ * Returns 0 if the request is complete without error
+ */
+int iwpm_wait_complete_req(struct iwpm_nlmsg_request *nlmsg_request);
+
+/**
+ * iwpm_get_nlmsg_seq - Get the sequence number for a netlink
+ *			message to send to the port mapper
+ *
+ * Returns the sequence number for the netlink message.
+ */
+int iwpm_get_nlmsg_seq(void);
+
+/**
+ * iwpm_valid_client - Check if the port mapper client is valid
+ * @nl_client: The index of the netlink client
+ *
+ * Valid clients need to call iwpm_init() before using
+ * the port mapper
+ */
+int iwpm_valid_client(u8 nl_client);
+
+/**
+ * iwpm_set_valid - Set the port mapper client to valid or not
+ * @nl_client: The index of the netlink client
+ * @valid: 1 if valid or 0 if invalid
+ */
+void iwpm_set_valid(u8 nl_client, int valid);
+
+/**
+ * iwpm_registered_client - Check if the port mapper client is registered
+ * @nl_client: The index of the netlink client
+ *
+ * Call iwpm_register_pid() to register a client
+ */
+int iwpm_registered_client(u8 nl_client);
+
+/**
+ * iwpm_set_registered - Set the port mapper client to registered or not
+ * @nl_client: The index of the netlink client
+ * @reg: 1 if registered or 0 if not
+ */
+void iwpm_set_registered(u8 nl_client, int reg);
+
+/**
+ * iwpm_send_mapinfo - Send local and mapped IPv4/IPv6 address info of
+ *                     a client to the user space port mapper
+ * @nl_client: The index of the netlink client
+ * @iwpm_pid: The pid of the user space port mapper
+ *
+ * If successful, returns the number of sent mapping info records
+ */
+int iwpm_send_mapinfo(u8 nl_client, int iwpm_pid);
+
+/**
+ * iwpm_mapinfo_available - Check if any mapping info records is available
+ *		            in the hash table
+ *
+ * Returns 1 if mapping information is available, otherwise returns 0
+ */
+int iwpm_mapinfo_available(void);
+
+/**
+ * iwpm_compare_sockaddr - Compare two sockaddr storage structs
+ *
+ * Returns 0 if they are holding the same ip/tcp address info,
+ * otherwise returns 1
+ */
+int iwpm_compare_sockaddr(struct sockaddr_storage *a_sockaddr,
+			struct sockaddr_storage *b_sockaddr);
+
+/**
+ * iwpm_validate_nlmsg_attr - Check for NULL netlink attributes
+ * @nltb: Holds address of each netlink message attributes
+ * @nla_count: Number of netlink message attributes
+ *
+ * Returns error if any of the nla_count attributes is NULL
+ */
+static inline int iwpm_validate_nlmsg_attr(struct nlattr *nltb[],
+					   int nla_count)
+{
+	int i;
+	for (i = 1; i < nla_count; i++) {
+		if (!nltb[i])
+			return -EINVAL;
+	}
+	return 0;
+}
+
+/**
+ * iwpm_create_nlmsg - Allocate skb and form a netlink message
+ * @nl_op: Netlink message opcode
+ * @nlh: Holds address of the netlink message header in skb
+ * @nl_client: The index of the netlink client
+ *
+ * Returns the newly allcated skb, or NULL if the tailroom of the skb
+ * is insufficient to store the message header and payload
+ */
+struct sk_buff *iwpm_create_nlmsg(u32 nl_op, struct nlmsghdr **nlh,
+					int nl_client);
+
+/**
+ * iwpm_parse_nlmsg - Validate and parse the received netlink message
+ * @cb: Netlink callback structure
+ * @policy_max: Maximum attribute type to be expected
+ * @nlmsg_policy: Validation policy
+ * @nltb: Array to store policy_max parsed elements
+ * @msg_type: Type of netlink message
+ *
+ * Returns 0 on success or a negative error code
+ */
+int iwpm_parse_nlmsg(struct netlink_callback *cb, int policy_max,
+				const struct nla_policy *nlmsg_policy,
+				struct nlattr *nltb[], const char *msg_type);
+
+/**
+ * iwpm_print_sockaddr - Print IPv4/IPv6 address and TCP port
+ * @sockaddr: Socket address to print
+ * @msg: Message to print
+ */
+void iwpm_print_sockaddr(struct sockaddr_storage *sockaddr, char *msg);
+#endif
diff --git a/drivers/infiniband/core/netlink.c b/drivers/infiniband/core/netlink.c
index a1e9cba8494..23dd5a5c759 100644
--- a/drivers/infiniband/core/netlink.c
+++ b/drivers/infiniband/core/netlink.c
@@ -103,13 +103,13 @@ int ibnl_remove_client(int index)
 EXPORT_SYMBOL(ibnl_remove_client);
 
 void *ibnl_put_msg(struct sk_buff *skb, struct nlmsghdr **nlh, int seq,
-		   int len, int client, int op)
+		   int len, int client, int op, int flags)
 {
 	unsigned char *prev_tail;
 
 	prev_tail = skb_tail_pointer(skb);
 	*nlh = nlmsg_put(skb, 0, seq, RDMA_NL_GET_TYPE(client, op),
-			 len, NLM_F_MULTI);
+			 len, flags);
 	if (!*nlh)
 		goto out_nlmsg_trim;
 	(*nlh)->nlmsg_len = skb_tail_pointer(skb) - prev_tail;
@@ -172,6 +172,20 @@ static void ibnl_rcv(struct sk_buff *skb)
 	mutex_unlock(&ibnl_mutex);
 }
 
+int ibnl_unicast(struct sk_buff *skb, struct nlmsghdr *nlh,
+			__u32 pid)
+{
+	return nlmsg_unicast(nls, skb, pid);
+}
+EXPORT_SYMBOL(ibnl_unicast);
+
+int ibnl_multicast(struct sk_buff *skb, struct nlmsghdr *nlh,
+			unsigned int group, gfp_t flags)
+{
+	return nlmsg_multicast(nls, skb, 0, group, flags);
+}
+EXPORT_SYMBOL(ibnl_multicast);
+
 int __init ibnl_init(void)
 {
 	struct netlink_kernel_cfg cfg = {
diff --git a/drivers/infiniband/core/sa_query.c b/drivers/infiniband/core/sa_query.c
index f820958e404..233eaf541f5 100644
--- a/drivers/infiniband/core/sa_query.c
+++ b/drivers/infiniband/core/sa_query.c
@@ -618,7 +618,7 @@ static void init_mad(struct ib_sa_mad *mad, struct ib_mad_agent *agent)
 
 static int send_mad(struct ib_sa_query *query, int timeout_ms, gfp_t gfp_mask)
 {
-	bool preload = gfp_mask & __GFP_WAIT;
+	bool preload = !!(gfp_mask & __GFP_WAIT);
 	unsigned long flags;
 	int ret, id;
 
diff --git a/drivers/infiniband/core/sysfs.c b/drivers/infiniband/core/sysfs.c
index 7d3292c7b4b..cbd0383f622 100644
--- a/drivers/infiniband/core/sysfs.c
+++ b/drivers/infiniband/core/sysfs.c
@@ -429,15 +429,19 @@ static void ib_port_release(struct kobject *kobj)
 	struct attribute *a;
 	int i;
 
-	for (i = 0; (a = p->gid_group.attrs[i]); ++i)
-		kfree(a);
+	if (p->gid_group.attrs) {
+		for (i = 0; (a = p->gid_group.attrs[i]); ++i)
+			kfree(a);
 
-	kfree(p->gid_group.attrs);
+		kfree(p->gid_group.attrs);
+	}
 
-	for (i = 0; (a = p->pkey_group.attrs[i]); ++i)
-		kfree(a);
+	if (p->pkey_group.attrs) {
+		for (i = 0; (a = p->pkey_group.attrs[i]); ++i)
+			kfree(a);
 
-	kfree(p->pkey_group.attrs);
+		kfree(p->pkey_group.attrs);
+	}
 
 	kfree(p);
 }
@@ -534,10 +538,12 @@ static int add_port(struct ib_device *device, int port_num,
 	p->port_num   = port_num;
 
 	ret = kobject_init_and_add(&p->kobj, &port_type,
-				   kobject_get(device->ports_parent),
+				   device->ports_parent,
 				   "%d", port_num);
-	if (ret)
-		goto err_put;
+	if (ret) {
+		kfree(p);
+		return ret;
+	}
 
 	ret = sysfs_create_group(&p->kobj, &pma_group);
 	if (ret)
@@ -585,6 +591,7 @@ err_free_pkey:
 		kfree(p->pkey_group.attrs[i]);
 
 	kfree(p->pkey_group.attrs);
+	p->pkey_group.attrs = NULL;
 
 err_remove_gid:
 	sysfs_remove_group(&p->kobj, &p->gid_group);
@@ -594,13 +601,13 @@ err_free_gid:
 		kfree(p->gid_group.attrs[i]);
 
 	kfree(p->gid_group.attrs);
+	p->gid_group.attrs = NULL;
 
 err_remove_pma:
 	sysfs_remove_group(&p->kobj, &pma_group);
 
 err_put:
-	kobject_put(device->ports_parent);
-	kfree(p);
+	kobject_put(&p->kobj);
 	return ret;
 }
 
@@ -809,6 +816,22 @@ static struct attribute_group iw_stats_group = {
 	.attrs	= iw_proto_stats_attrs,
 };
 
+static void free_port_list_attributes(struct ib_device *device)
+{
+	struct kobject *p, *t;
+
+	list_for_each_entry_safe(p, t, &device->port_list, entry) {
+		struct ib_port *port = container_of(p, struct ib_port, kobj);
+		list_del(&p->entry);
+		sysfs_remove_group(p, &pma_group);
+		sysfs_remove_group(p, &port->pkey_group);
+		sysfs_remove_group(p, &port->gid_group);
+		kobject_put(p);
+	}
+
+	kobject_put(device->ports_parent);
+}
+
 int ib_device_register_sysfs(struct ib_device *device,
 			     int (*port_callback)(struct ib_device *,
 						  u8, struct kobject *))
@@ -835,7 +858,7 @@ int ib_device_register_sysfs(struct ib_device *device,
 	}
 
 	device->ports_parent = kobject_create_and_add("ports",
-					kobject_get(&class_dev->kobj));
+						      &class_dev->kobj);
 	if (!device->ports_parent) {
 		ret = -ENOMEM;
 		goto err_put;
@@ -862,21 +885,7 @@ int ib_device_register_sysfs(struct ib_device *device,
 	return 0;
 
 err_put:
-	{
-		struct kobject *p, *t;
-		struct ib_port *port;
-
-		list_for_each_entry_safe(p, t, &device->port_list, entry) {
-			list_del(&p->entry);
-			port = container_of(p, struct ib_port, kobj);
-			sysfs_remove_group(p, &pma_group);
-			sysfs_remove_group(p, &port->pkey_group);
-			sysfs_remove_group(p, &port->gid_group);
-			kobject_put(p);
-		}
-	}
-
-	kobject_put(&class_dev->kobj);
+	free_port_list_attributes(device);
 
 err_unregister:
 	device_unregister(class_dev);
@@ -887,22 +896,18 @@ err:
 
 void ib_device_unregister_sysfs(struct ib_device *device)
 {
-	struct kobject *p, *t;
-	struct ib_port *port;
-
 	/* Hold kobject until ib_dealloc_device() */
-	kobject_get(&device->dev.kobj);
+	struct kobject *kobj_dev = kobject_get(&device->dev.kobj);
+	int i;
 
-	list_for_each_entry_safe(p, t, &device->port_list, entry) {
-		list_del(&p->entry);
-		port = container_of(p, struct ib_port, kobj);
-		sysfs_remove_group(p, &pma_group);
-		sysfs_remove_group(p, &port->pkey_group);
-		sysfs_remove_group(p, &port->gid_group);
-		kobject_put(p);
-	}
+	if (device->node_type == RDMA_NODE_RNIC && device->get_protocol_stats)
+		sysfs_remove_group(kobj_dev, &iw_stats_group);
+
+	free_port_list_attributes(device);
+
+	for (i = 0; i < ARRAY_SIZE(ib_class_attributes); ++i)
+		device_remove_file(&device->dev, ib_class_attributes[i]);
 
-	kobject_put(device->ports_parent);
 	device_unregister(&device->dev);
 }
 
diff --git a/drivers/infiniband/core/user_mad.c b/drivers/infiniband/core/user_mad.c
index f0d588f8859..1acb9910055 100644
--- a/drivers/infiniband/core/user_mad.c
+++ b/drivers/infiniband/core/user_mad.c
@@ -98,7 +98,7 @@ struct ib_umad_port {
 
 struct ib_umad_device {
 	int                  start_port, end_port;
-	struct kref          ref;
+	struct kobject       kobj;
 	struct ib_umad_port  port[0];
 };
 
@@ -134,14 +134,18 @@ static DECLARE_BITMAP(dev_map, IB_UMAD_MAX_PORTS);
 static void ib_umad_add_one(struct ib_device *device);
 static void ib_umad_remove_one(struct ib_device *device);
 
-static void ib_umad_release_dev(struct kref *ref)
+static void ib_umad_release_dev(struct kobject *kobj)
 {
 	struct ib_umad_device *dev =
-		container_of(ref, struct ib_umad_device, ref);
+		container_of(kobj, struct ib_umad_device, kobj);
 
 	kfree(dev);
 }
 
+static struct kobj_type ib_umad_dev_ktype = {
+	.release = ib_umad_release_dev,
+};
+
 static int hdr_size(struct ib_umad_file *file)
 {
 	return file->use_pkey_index ? sizeof (struct ib_user_mad_hdr) :
@@ -780,27 +784,19 @@ static int ib_umad_open(struct inode *inode, struct file *filp)
 {
 	struct ib_umad_port *port;
 	struct ib_umad_file *file;
-	int ret;
+	int ret = -ENXIO;
 
 	port = container_of(inode->i_cdev, struct ib_umad_port, cdev);
-	if (port)
-		kref_get(&port->umad_dev->ref);
-	else
-		return -ENXIO;
 
 	mutex_lock(&port->file_mutex);
 
-	if (!port->ib_dev) {
-		ret = -ENXIO;
+	if (!port->ib_dev)
 		goto out;
-	}
 
+	ret = -ENOMEM;
 	file = kzalloc(sizeof *file, GFP_KERNEL);
-	if (!file) {
-		kref_put(&port->umad_dev->ref, ib_umad_release_dev);
-		ret = -ENOMEM;
+	if (!file)
 		goto out;
-	}
 
 	mutex_init(&file->mutex);
 	spin_lock_init(&file->send_lock);
@@ -814,6 +810,13 @@ static int ib_umad_open(struct inode *inode, struct file *filp)
 	list_add_tail(&file->port_list, &port->file_list);
 
 	ret = nonseekable_open(inode, filp);
+	if (ret) {
+		list_del(&file->port_list);
+		kfree(file);
+		goto out;
+	}
+
+	kobject_get(&port->umad_dev->kobj);
 
 out:
 	mutex_unlock(&port->file_mutex);
@@ -852,7 +855,7 @@ static int ib_umad_close(struct inode *inode, struct file *filp)
 	mutex_unlock(&file->port->file_mutex);
 
 	kfree(file);
-	kref_put(&dev->ref, ib_umad_release_dev);
+	kobject_put(&dev->kobj);
 
 	return 0;
 }
@@ -880,10 +883,6 @@ static int ib_umad_sm_open(struct inode *inode, struct file *filp)
 	int ret;
 
 	port = container_of(inode->i_cdev, struct ib_umad_port, sm_cdev);
-	if (port)
-		kref_get(&port->umad_dev->ref);
-	else
-		return -ENXIO;
 
 	if (filp->f_flags & O_NONBLOCK) {
 		if (down_trylock(&port->sm_sem)) {
@@ -898,17 +897,27 @@ static int ib_umad_sm_open(struct inode *inode, struct file *filp)
 	}
 
 	ret = ib_modify_port(port->ib_dev, port->port_num, 0, &props);
-	if (ret) {
-		up(&port->sm_sem);
-		goto fail;
-	}
+	if (ret)
+		goto err_up_sem;
 
 	filp->private_data = port;
 
-	return nonseekable_open(inode, filp);
+	ret = nonseekable_open(inode, filp);
+	if (ret)
+		goto err_clr_sm_cap;
+
+	kobject_get(&port->umad_dev->kobj);
+
+	return 0;
+
+err_clr_sm_cap:
+	swap(props.set_port_cap_mask, props.clr_port_cap_mask);
+	ib_modify_port(port->ib_dev, port->port_num, 0, &props);
+
+err_up_sem:
+	up(&port->sm_sem);
 
 fail:
-	kref_put(&port->umad_dev->ref, ib_umad_release_dev);
 	return ret;
 }
 
@@ -927,7 +936,7 @@ static int ib_umad_sm_close(struct inode *inode, struct file *filp)
 
 	up(&port->sm_sem);
 
-	kref_put(&port->umad_dev->ref, ib_umad_release_dev);
+	kobject_put(&port->umad_dev->kobj);
 
 	return ret;
 }
@@ -995,6 +1004,7 @@ static int find_overflow_devnum(void)
 }
 
 static int ib_umad_init_port(struct ib_device *device, int port_num,
+			     struct ib_umad_device *umad_dev,
 			     struct ib_umad_port *port)
 {
 	int devnum;
@@ -1027,6 +1037,7 @@ static int ib_umad_init_port(struct ib_device *device, int port_num,
 
 	cdev_init(&port->cdev, &umad_fops);
 	port->cdev.owner = THIS_MODULE;
+	port->cdev.kobj.parent = &umad_dev->kobj;
 	kobject_set_name(&port->cdev.kobj, "umad%d", port->dev_num);
 	if (cdev_add(&port->cdev, base, 1))
 		goto err_cdev;
@@ -1045,6 +1056,7 @@ static int ib_umad_init_port(struct ib_device *device, int port_num,
 	base += IB_UMAD_MAX_PORTS;
 	cdev_init(&port->sm_cdev, &umad_sm_fops);
 	port->sm_cdev.owner = THIS_MODULE;
+	port->sm_cdev.kobj.parent = &umad_dev->kobj;
 	kobject_set_name(&port->sm_cdev.kobj, "issm%d", port->dev_num);
 	if (cdev_add(&port->sm_cdev, base, 1))
 		goto err_sm_cdev;
@@ -1138,7 +1150,7 @@ static void ib_umad_add_one(struct ib_device *device)
 	if (!umad_dev)
 		return;
 
-	kref_init(&umad_dev->ref);
+	kobject_init(&umad_dev->kobj, &ib_umad_dev_ktype);
 
 	umad_dev->start_port = s;
 	umad_dev->end_port   = e;
@@ -1146,7 +1158,8 @@ static void ib_umad_add_one(struct ib_device *device)
 	for (i = s; i <= e; ++i) {
 		umad_dev->port[i - s].umad_dev = umad_dev;
 
-		if (ib_umad_init_port(device, i, &umad_dev->port[i - s]))
+		if (ib_umad_init_port(device, i, umad_dev,
+				      &umad_dev->port[i - s]))
 			goto err;
 	}
 
@@ -1158,7 +1171,7 @@ err:
 	while (--i >= s)
 		ib_umad_kill_port(&umad_dev->port[i - s]);
 
-	kref_put(&umad_dev->ref, ib_umad_release_dev);
+	kobject_put(&umad_dev->kobj);
 }
 
 static void ib_umad_remove_one(struct ib_device *device)
@@ -1172,7 +1185,7 @@ static void ib_umad_remove_one(struct ib_device *device)
 	for (i = 0; i <= umad_dev->end_port - umad_dev->start_port; ++i)
 		ib_umad_kill_port(&umad_dev->port[i]);
 
-	kref_put(&umad_dev->ref, ib_umad_release_dev);
+	kobject_put(&umad_dev->kobj);
 }
 
 static char *umad_devnode(struct device *dev, umode_t *mode)
diff --git a/drivers/infiniband/core/verbs.c b/drivers/infiniband/core/verbs.c
index 92525f855d8..c2b89cc5dbc 100644
--- a/drivers/infiniband/core/verbs.c
+++ b/drivers/infiniband/core/verbs.c
@@ -48,7 +48,7 @@
 
 #include "core_priv.h"
 
-int ib_rate_to_mult(enum ib_rate rate)
+__attribute_const__ int ib_rate_to_mult(enum ib_rate rate)
 {
 	switch (rate) {
 	case IB_RATE_2_5_GBPS: return  1;
@@ -65,7 +65,7 @@ int ib_rate_to_mult(enum ib_rate rate)
 }
 EXPORT_SYMBOL(ib_rate_to_mult);
 
-enum ib_rate mult_to_ib_rate(int mult)
+__attribute_const__ enum ib_rate mult_to_ib_rate(int mult)
 {
 	switch (mult) {
 	case 1:  return IB_RATE_2_5_GBPS;
@@ -82,7 +82,7 @@ enum ib_rate mult_to_ib_rate(int mult)
 }
 EXPORT_SYMBOL(mult_to_ib_rate);
 
-int ib_rate_to_mbps(enum ib_rate rate)
+__attribute_const__ int ib_rate_to_mbps(enum ib_rate rate)
 {
 	switch (rate) {
 	case IB_RATE_2_5_GBPS: return 2500;
@@ -107,7 +107,7 @@ int ib_rate_to_mbps(enum ib_rate rate)
 }
 EXPORT_SYMBOL(ib_rate_to_mbps);
 
-enum rdma_transport_type
+__attribute_const__ enum rdma_transport_type
 rdma_node_get_transport(enum rdma_node_type node_type)
 {
 	switch (node_type) {
diff --git a/drivers/infiniband/hw/Makefile b/drivers/infiniband/hw/Makefile
new file mode 100644
index 00000000000..e900b03531a
--- /dev/null
+++ b/drivers/infiniband/hw/Makefile
@@ -0,0 +1,12 @@
+obj-$(CONFIG_INFINIBAND_MTHCA)		+= mthca/
+obj-$(CONFIG_INFINIBAND_IPATH)		+= ipath/
+obj-$(CONFIG_INFINIBAND_QIB)		+= qib/
+obj-$(CONFIG_INFINIBAND_EHCA)		+= ehca/
+obj-$(CONFIG_INFINIBAND_AMSO1100)	+= amso1100/
+obj-$(CONFIG_INFINIBAND_CXGB3)		+= cxgb3/
+obj-$(CONFIG_INFINIBAND_CXGB4)		+= cxgb4/
+obj-$(CONFIG_MLX4_INFINIBAND)		+= mlx4/
+obj-$(CONFIG_MLX5_INFINIBAND)		+= mlx5/
+obj-$(CONFIG_INFINIBAND_NES)		+= nes/
+obj-$(CONFIG_INFINIBAND_OCRDMA)		+= ocrdma/
+obj-$(CONFIG_INFINIBAND_USNIC)		+= usnic/
diff --git a/drivers/infiniband/hw/cxgb3/cxio_hal.c b/drivers/infiniband/hw/cxgb3/cxio_hal.c
index c3f5aca4ef0..de1c61b417d 100644
--- a/drivers/infiniband/hw/cxgb3/cxio_hal.c
+++ b/drivers/infiniband/hw/cxgb3/cxio_hal.c
@@ -735,14 +735,12 @@ static int __cxio_tpt_op(struct cxio_rdev *rdev_p, u32 reset_tpt_entry,
 			((perm & TPT_MW_BIND) ? F_TPT_MW_BIND_ENABLE : 0) |
 			V_TPT_ADDR_TYPE((zbva ? TPT_ZBTO : TPT_VATO)) |
 			V_TPT_PAGE_SIZE(page_size));
-		tpt.rsvd_pbl_addr = reset_tpt_entry ? 0 :
-				    cpu_to_be32(V_TPT_PBL_ADDR(PBL_OFF(rdev_p, pbl_addr)>>3));
+		tpt.rsvd_pbl_addr = cpu_to_be32(V_TPT_PBL_ADDR(PBL_OFF(rdev_p, pbl_addr)>>3));
 		tpt.len = cpu_to_be32(len);
 		tpt.va_hi = cpu_to_be32((u32) (to >> 32));
 		tpt.va_low_or_fbo = cpu_to_be32((u32) (to & 0xFFFFFFFFULL));
 		tpt.rsvd_bind_cnt_or_pstag = 0;
-		tpt.rsvd_pbl_size = reset_tpt_entry ? 0 :
-				  cpu_to_be32(V_TPT_PBL_SIZE(pbl_size >> 2));
+		tpt.rsvd_pbl_size = cpu_to_be32(V_TPT_PBL_SIZE(pbl_size >> 2));
 	}
 	err = cxio_hal_ctrl_qp_write_mem(rdev_p,
 				       stag_idx +
diff --git a/drivers/infiniband/hw/cxgb3/iwch_cm.c b/drivers/infiniband/hw/cxgb3/iwch_cm.c
index 095bb046e2c..cb78b1e9bcd 100644
--- a/drivers/infiniband/hw/cxgb3/iwch_cm.c
+++ b/drivers/infiniband/hw/cxgb3/iwch_cm.c
@@ -418,6 +418,7 @@ static int send_abort(struct iwch_ep *ep, struct sk_buff *skb, gfp_t gfp)
 	skb->priority = CPL_PRIORITY_DATA;
 	set_arp_failure_handler(skb, abort_arp_failure);
 	req = (struct cpl_abort_req *) skb_put(skb, sizeof(*req));
+	memset(req, 0, sizeof(*req));
 	req->wr.wr_hi = htonl(V_WR_OP(FW_WROPCODE_OFLD_HOST_ABORT_CON_REQ));
 	req->wr.wr_lo = htonl(V_WR_TID(ep->hwtid));
 	OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_ABORT_REQ, ep->hwtid));
diff --git a/drivers/infiniband/hw/cxgb4/cm.c b/drivers/infiniband/hw/cxgb4/cm.c
index 1f863a96a48..96d7131ab97 100644
--- a/drivers/infiniband/hw/cxgb4/cm.c
+++ b/drivers/infiniband/hw/cxgb4/cm.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2009-2010 Chelsio, Inc. All rights reserved.
+ * Copyright (c) 2009-2014 Chelsio, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -47,6 +47,8 @@
 #include <net/ip6_route.h>
 #include <net/addrconf.h>
 
+#include <rdma/ib_addr.h>
+
 #include "iw_cxgb4.h"
 
 static char *states[] = {
@@ -294,6 +296,12 @@ void _c4iw_free_ep(struct kref *kref)
 		dst_release(ep->dst);
 		cxgb4_l2t_release(ep->l2t);
 	}
+	if (test_bit(RELEASE_MAPINFO, &ep->com.flags)) {
+		print_addr(&ep->com, __func__, "remove_mapinfo/mapping");
+		iwpm_remove_mapinfo(&ep->com.local_addr,
+				    &ep->com.mapped_local_addr);
+		iwpm_remove_mapping(&ep->com.local_addr, RDMA_NL_C4IW);
+	}
 	kfree(ep);
 }
 
@@ -341,10 +349,7 @@ static struct sk_buff *get_skb(struct sk_buff *skb, int len, gfp_t gfp)
 
 static struct net_device *get_real_dev(struct net_device *egress_dev)
 {
-	struct net_device *phys_dev = egress_dev;
-	if (egress_dev->priv_flags & IFF_802_1Q_VLAN)
-		phys_dev = vlan_dev_real_dev(egress_dev);
-	return phys_dev;
+	return rdma_vlan_dev_real_dev(egress_dev) ? : egress_dev;
 }
 
 static int our_interface(struct c4iw_dev *dev, struct net_device *egress_dev)
@@ -528,6 +533,38 @@ static int send_abort(struct c4iw_ep *ep, struct sk_buff *skb, gfp_t gfp)
 	return c4iw_l2t_send(&ep->com.dev->rdev, skb, ep->l2t);
 }
 
+/*
+ * c4iw_form_pm_msg - Form a port mapper message with mapping info
+ */
+static void c4iw_form_pm_msg(struct c4iw_ep *ep,
+				struct iwpm_sa_data *pm_msg)
+{
+	memcpy(&pm_msg->loc_addr, &ep->com.local_addr,
+		sizeof(ep->com.local_addr));
+	memcpy(&pm_msg->rem_addr, &ep->com.remote_addr,
+		sizeof(ep->com.remote_addr));
+}
+
+/*
+ * c4iw_form_reg_msg - Form a port mapper message with dev info
+ */
+static void c4iw_form_reg_msg(struct c4iw_dev *dev,
+				struct iwpm_dev_data *pm_msg)
+{
+	memcpy(pm_msg->dev_name, dev->ibdev.name, IWPM_DEVNAME_SIZE);
+	memcpy(pm_msg->if_name, dev->rdev.lldi.ports[0]->name,
+				IWPM_IFNAME_SIZE);
+}
+
+static void c4iw_record_pm_msg(struct c4iw_ep *ep,
+			struct iwpm_sa_data *pm_msg)
+{
+	memcpy(&ep->com.mapped_local_addr, &pm_msg->mapped_loc_addr,
+		sizeof(ep->com.mapped_local_addr));
+	memcpy(&ep->com.mapped_remote_addr, &pm_msg->mapped_rem_addr,
+		sizeof(ep->com.mapped_remote_addr));
+}
+
 static int send_connect(struct c4iw_ep *ep)
 {
 	struct cpl_act_open_req *req;
@@ -546,10 +583,14 @@ static int send_connect(struct c4iw_ep *ep)
 	int sizev6 = is_t4(ep->com.dev->rdev.lldi.adapter_type) ?
 				sizeof(struct cpl_act_open_req6) :
 				sizeof(struct cpl_t5_act_open_req6);
-	struct sockaddr_in *la = (struct sockaddr_in *)&ep->com.local_addr;
-	struct sockaddr_in *ra = (struct sockaddr_in *)&ep->com.remote_addr;
-	struct sockaddr_in6 *la6 = (struct sockaddr_in6 *)&ep->com.local_addr;
-	struct sockaddr_in6 *ra6 = (struct sockaddr_in6 *)&ep->com.remote_addr;
+	struct sockaddr_in *la = (struct sockaddr_in *)
+				 &ep->com.mapped_local_addr;
+	struct sockaddr_in *ra = (struct sockaddr_in *)
+				 &ep->com.mapped_remote_addr;
+	struct sockaddr_in6 *la6 = (struct sockaddr_in6 *)
+				   &ep->com.mapped_local_addr;
+	struct sockaddr_in6 *ra6 = (struct sockaddr_in6 *)
+				   &ep->com.mapped_remote_addr;
 
 	wrlen = (ep->com.remote_addr.ss_family == AF_INET) ?
 			roundup(sizev4, 16) :
@@ -1627,10 +1668,10 @@ static void send_fw_act_open_req(struct c4iw_ep *ep, unsigned int atid)
 	req->le.filter = cpu_to_be32(cxgb4_select_ntuple(
 				     ep->com.dev->rdev.lldi.ports[0],
 				     ep->l2t));
-	sin = (struct sockaddr_in *)&ep->com.local_addr;
+	sin = (struct sockaddr_in *)&ep->com.mapped_local_addr;
 	req->le.lport = sin->sin_port;
 	req->le.u.ipv4.lip = sin->sin_addr.s_addr;
-	sin = (struct sockaddr_in *)&ep->com.remote_addr;
+	sin = (struct sockaddr_in *)&ep->com.mapped_remote_addr;
 	req->le.pport = sin->sin_port;
 	req->le.u.ipv4.pip = sin->sin_addr.s_addr;
 	req->tcb.t_state_to_astid =
@@ -1746,16 +1787,16 @@ static int import_ep(struct c4iw_ep *ep, int iptype, __u8 *peer_ip,
 		if (!ep->l2t)
 			goto out;
 		ep->mtu = dst_mtu(dst);
-		ep->tx_chan = cxgb4_port_chan(n->dev);
-		ep->smac_idx = (cxgb4_port_viid(n->dev) & 0x7F) << 1;
+		ep->tx_chan = cxgb4_port_chan(pdev);
+		ep->smac_idx = (cxgb4_port_viid(pdev) & 0x7F) << 1;
 		step = cdev->rdev.lldi.ntxq /
 			cdev->rdev.lldi.nchan;
-		ep->txq_idx = cxgb4_port_idx(n->dev) * step;
-		ep->ctrlq_idx = cxgb4_port_idx(n->dev);
+		ep->txq_idx = cxgb4_port_idx(pdev) * step;
+		ep->ctrlq_idx = cxgb4_port_idx(pdev);
 		step = cdev->rdev.lldi.nrxq /
 			cdev->rdev.lldi.nchan;
 		ep->rss_qid = cdev->rdev.lldi.rxq_ids[
-			cxgb4_port_idx(n->dev) * step];
+			cxgb4_port_idx(pdev) * step];
 
 		if (clear_mpa_v1) {
 			ep->retry_with_mpa_v1 = 0;
@@ -1870,10 +1911,10 @@ static int act_open_rpl(struct c4iw_dev *dev, struct sk_buff *skb)
 	struct sockaddr_in6 *ra6;
 
 	ep = lookup_atid(t, atid);
-	la = (struct sockaddr_in *)&ep->com.local_addr;
-	ra = (struct sockaddr_in *)&ep->com.remote_addr;
-	la6 = (struct sockaddr_in6 *)&ep->com.local_addr;
-	ra6 = (struct sockaddr_in6 *)&ep->com.remote_addr;
+	la = (struct sockaddr_in *)&ep->com.mapped_local_addr;
+	ra = (struct sockaddr_in *)&ep->com.mapped_remote_addr;
+	la6 = (struct sockaddr_in6 *)&ep->com.mapped_local_addr;
+	ra6 = (struct sockaddr_in6 *)&ep->com.mapped_remote_addr;
 
 	PDBG("%s ep %p atid %u status %u errno %d\n", __func__, ep, atid,
 	     status, status2errno(status));
@@ -2730,13 +2771,15 @@ int c4iw_connect(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 	struct c4iw_dev *dev = to_c4iw_dev(cm_id->device);
 	struct c4iw_ep *ep;
 	int err = 0;
-	struct sockaddr_in *laddr = (struct sockaddr_in *)&cm_id->local_addr;
-	struct sockaddr_in *raddr = (struct sockaddr_in *)&cm_id->remote_addr;
-	struct sockaddr_in6 *laddr6 = (struct sockaddr_in6 *)&cm_id->local_addr;
-	struct sockaddr_in6 *raddr6 = (struct sockaddr_in6 *)
-				      &cm_id->remote_addr;
+	struct sockaddr_in *laddr;
+	struct sockaddr_in *raddr;
+	struct sockaddr_in6 *laddr6;
+	struct sockaddr_in6 *raddr6;
+	struct iwpm_dev_data pm_reg_msg;
+	struct iwpm_sa_data pm_msg;
 	__u8 *ra;
 	int iptype;
+	int iwpm_err = 0;
 
 	if ((conn_param->ord > c4iw_max_read_depth) ||
 	    (conn_param->ird > c4iw_max_read_depth)) {
@@ -2767,7 +2810,7 @@ int c4iw_connect(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 	if (!ep->com.qp) {
 		PDBG("%s qpn 0x%x not found!\n", __func__, conn_param->qpn);
 		err = -EINVAL;
-		goto fail2;
+		goto fail1;
 	}
 	ref_qp(ep);
 	PDBG("%s qpn 0x%x qp %p cm_id %p\n", __func__, conn_param->qpn,
@@ -2780,10 +2823,50 @@ int c4iw_connect(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 	if (ep->atid == -1) {
 		printk(KERN_ERR MOD "%s - cannot alloc atid.\n", __func__);
 		err = -ENOMEM;
-		goto fail2;
+		goto fail1;
 	}
 	insert_handle(dev, &dev->atid_idr, ep, ep->atid);
 
+	memcpy(&ep->com.local_addr, &cm_id->local_addr,
+	       sizeof(ep->com.local_addr));
+	memcpy(&ep->com.remote_addr, &cm_id->remote_addr,
+	       sizeof(ep->com.remote_addr));
+
+	/* No port mapper available, go with the specified peer information */
+	memcpy(&ep->com.mapped_local_addr, &cm_id->local_addr,
+	       sizeof(ep->com.mapped_local_addr));
+	memcpy(&ep->com.mapped_remote_addr, &cm_id->remote_addr,
+	       sizeof(ep->com.mapped_remote_addr));
+
+	c4iw_form_reg_msg(dev, &pm_reg_msg);
+	iwpm_err = iwpm_register_pid(&pm_reg_msg, RDMA_NL_C4IW);
+	if (iwpm_err) {
+		PDBG("%s: Port Mapper reg pid fail (err = %d).\n",
+			__func__, iwpm_err);
+	}
+	if (iwpm_valid_pid() && !iwpm_err) {
+		c4iw_form_pm_msg(ep, &pm_msg);
+		iwpm_err = iwpm_add_and_query_mapping(&pm_msg, RDMA_NL_C4IW);
+		if (iwpm_err)
+			PDBG("%s: Port Mapper query fail (err = %d).\n",
+				__func__, iwpm_err);
+		else
+			c4iw_record_pm_msg(ep, &pm_msg);
+	}
+	if (iwpm_create_mapinfo(&ep->com.local_addr,
+				&ep->com.mapped_local_addr, RDMA_NL_C4IW)) {
+		iwpm_remove_mapping(&ep->com.local_addr, RDMA_NL_C4IW);
+		err = -ENOMEM;
+		goto fail1;
+	}
+	print_addr(&ep->com, __func__, "add_query/create_mapinfo");
+	set_bit(RELEASE_MAPINFO, &ep->com.flags);
+
+	laddr = (struct sockaddr_in *)&ep->com.mapped_local_addr;
+	raddr = (struct sockaddr_in *)&ep->com.mapped_remote_addr;
+	laddr6 = (struct sockaddr_in6 *)&ep->com.mapped_local_addr;
+	raddr6 = (struct sockaddr_in6 *) &ep->com.mapped_remote_addr;
+
 	if (cm_id->remote_addr.ss_family == AF_INET) {
 		iptype = 4;
 		ra = (__u8 *)&raddr->sin_addr;
@@ -2794,7 +2877,7 @@ int c4iw_connect(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 		if ((__force int)raddr->sin_addr.s_addr == INADDR_ANY) {
 			err = pick_local_ipaddrs(dev, cm_id);
 			if (err)
-				goto fail2;
+				goto fail1;
 		}
 
 		/* find a route */
@@ -2814,7 +2897,7 @@ int c4iw_connect(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 		if (ipv6_addr_type(&raddr6->sin6_addr) == IPV6_ADDR_ANY) {
 			err = pick_local_ip6addrs(dev, cm_id);
 			if (err)
-				goto fail2;
+				goto fail1;
 		}
 
 		/* find a route */
@@ -2830,13 +2913,13 @@ int c4iw_connect(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 	if (!ep->dst) {
 		printk(KERN_ERR MOD "%s - cannot find route.\n", __func__);
 		err = -EHOSTUNREACH;
-		goto fail3;
+		goto fail2;
 	}
 
 	err = import_ep(ep, iptype, ra, ep->dst, ep->com.dev, true);
 	if (err) {
 		printk(KERN_ERR MOD "%s - cannot alloc l2e.\n", __func__);
-		goto fail4;
+		goto fail3;
 	}
 
 	PDBG("%s txq_idx %u tx_chan %u smac_idx %u rss_qid %u l2t_idx %u\n",
@@ -2845,10 +2928,6 @@ int c4iw_connect(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 
 	state_set(&ep->com, CONNECTING);
 	ep->tos = 0;
-	memcpy(&ep->com.local_addr, &cm_id->local_addr,
-	       sizeof(ep->com.local_addr));
-	memcpy(&ep->com.remote_addr, &cm_id->remote_addr,
-	       sizeof(ep->com.remote_addr));
 
 	/* send connect request to rnic */
 	err = send_connect(ep);
@@ -2856,12 +2935,12 @@ int c4iw_connect(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 		goto out;
 
 	cxgb4_l2t_release(ep->l2t);
-fail4:
-	dst_release(ep->dst);
 fail3:
+	dst_release(ep->dst);
+fail2:
 	remove_handle(ep->com.dev, &ep->com.dev->atid_idr, ep->atid);
 	cxgb4_free_atid(ep->com.dev->rdev.lldi.tids, ep->atid);
-fail2:
+fail1:
 	cm_id->rem_ref(cm_id);
 	c4iw_put_ep(&ep->com);
 out:
@@ -2871,7 +2950,8 @@ out:
 static int create_server6(struct c4iw_dev *dev, struct c4iw_listen_ep *ep)
 {
 	int err;
-	struct sockaddr_in6 *sin6 = (struct sockaddr_in6 *)&ep->com.local_addr;
+	struct sockaddr_in6 *sin6 = (struct sockaddr_in6 *)
+				    &ep->com.mapped_local_addr;
 
 	c4iw_init_wr_wait(&ep->com.wr_wait);
 	err = cxgb4_create_server6(ep->com.dev->rdev.lldi.ports[0],
@@ -2892,7 +2972,8 @@ static int create_server6(struct c4iw_dev *dev, struct c4iw_listen_ep *ep)
 static int create_server4(struct c4iw_dev *dev, struct c4iw_listen_ep *ep)
 {
 	int err;
-	struct sockaddr_in *sin = (struct sockaddr_in *)&ep->com.local_addr;
+	struct sockaddr_in *sin = (struct sockaddr_in *)
+				  &ep->com.mapped_local_addr;
 
 	if (dev->rdev.lldi.enable_fw_ofld_conn) {
 		do {
@@ -2927,6 +3008,9 @@ int c4iw_create_listen(struct iw_cm_id *cm_id, int backlog)
 	int err = 0;
 	struct c4iw_dev *dev = to_c4iw_dev(cm_id->device);
 	struct c4iw_listen_ep *ep;
+	struct iwpm_dev_data pm_reg_msg;
+	struct iwpm_sa_data pm_msg;
+	int iwpm_err = 0;
 
 	might_sleep();
 
@@ -2961,6 +3045,37 @@ int c4iw_create_listen(struct iw_cm_id *cm_id, int backlog)
 		goto fail2;
 	}
 	insert_handle(dev, &dev->stid_idr, ep, ep->stid);
+
+	/* No port mapper available, go with the specified info */
+	memcpy(&ep->com.mapped_local_addr, &cm_id->local_addr,
+	       sizeof(ep->com.mapped_local_addr));
+
+	c4iw_form_reg_msg(dev, &pm_reg_msg);
+	iwpm_err = iwpm_register_pid(&pm_reg_msg, RDMA_NL_C4IW);
+	if (iwpm_err) {
+		PDBG("%s: Port Mapper reg pid fail (err = %d).\n",
+			__func__, iwpm_err);
+	}
+	if (iwpm_valid_pid() && !iwpm_err) {
+		memcpy(&pm_msg.loc_addr, &ep->com.local_addr,
+				sizeof(ep->com.local_addr));
+		iwpm_err = iwpm_add_mapping(&pm_msg, RDMA_NL_C4IW);
+		if (iwpm_err)
+			PDBG("%s: Port Mapper query fail (err = %d).\n",
+				__func__, iwpm_err);
+		else
+			memcpy(&ep->com.mapped_local_addr,
+				&pm_msg.mapped_loc_addr,
+				sizeof(ep->com.mapped_local_addr));
+	}
+	if (iwpm_create_mapinfo(&ep->com.local_addr,
+				&ep->com.mapped_local_addr, RDMA_NL_C4IW)) {
+		err = -ENOMEM;
+		goto fail3;
+	}
+	print_addr(&ep->com, __func__, "add_mapping/create_mapinfo");
+
+	set_bit(RELEASE_MAPINFO, &ep->com.flags);
 	state_set(&ep->com, LISTEN);
 	if (ep->com.local_addr.ss_family == AF_INET)
 		err = create_server4(dev, ep);
@@ -2970,6 +3085,8 @@ int c4iw_create_listen(struct iw_cm_id *cm_id, int backlog)
 		cm_id->provider_data = ep;
 		goto out;
 	}
+
+fail3:
 	cxgb4_free_stid(ep->com.dev->rdev.lldi.tids, ep->stid,
 			ep->com.local_addr.ss_family);
 fail2:
diff --git a/drivers/infiniband/hw/cxgb4/cq.c b/drivers/infiniband/hw/cxgb4/cq.c
index cfaa56ada18..7151a02b4eb 100644
--- a/drivers/infiniband/hw/cxgb4/cq.c
+++ b/drivers/infiniband/hw/cxgb4/cq.c
@@ -940,7 +940,6 @@ struct ib_cq *c4iw_create_cq(struct ib_device *ibdev, int entries,
 		if (!mm2)
 			goto err4;
 
-		memset(&uresp, 0, sizeof(uresp));
 		uresp.qid_mask = rhp->rdev.cqmask;
 		uresp.cqid = chp->cq.cqid;
 		uresp.size = chp->cq.size;
@@ -951,7 +950,8 @@ struct ib_cq *c4iw_create_cq(struct ib_device *ibdev, int entries,
 		uresp.gts_key = ucontext->key;
 		ucontext->key += PAGE_SIZE;
 		spin_unlock(&ucontext->mmap_lock);
-		ret = ib_copy_to_udata(udata, &uresp, sizeof uresp);
+		ret = ib_copy_to_udata(udata, &uresp,
+				       sizeof(uresp) - sizeof(uresp.reserved));
 		if (ret)
 			goto err5;
 
diff --git a/drivers/infiniband/hw/cxgb4/device.c b/drivers/infiniband/hw/cxgb4/device.c
index f4fa50a609e..dd93aadc996 100644
--- a/drivers/infiniband/hw/cxgb4/device.c
+++ b/drivers/infiniband/hw/cxgb4/device.c
@@ -77,6 +77,16 @@ struct c4iw_debugfs_data {
 	int pos;
 };
 
+/* registered cxgb4 netlink callbacks */
+static struct ibnl_client_cbs c4iw_nl_cb_table[] = {
+	[RDMA_NL_IWPM_REG_PID] = {.dump = iwpm_register_pid_cb},
+	[RDMA_NL_IWPM_ADD_MAPPING] = {.dump = iwpm_add_mapping_cb},
+	[RDMA_NL_IWPM_QUERY_MAPPING] = {.dump = iwpm_add_and_query_mapping_cb},
+	[RDMA_NL_IWPM_HANDLE_ERR] = {.dump = iwpm_mapping_error_cb},
+	[RDMA_NL_IWPM_MAPINFO] = {.dump = iwpm_mapping_info_cb},
+	[RDMA_NL_IWPM_MAPINFO_NUM] = {.dump = iwpm_ack_mapping_info_cb}
+};
+
 static int count_idrs(int id, void *p, void *data)
 {
 	int *countp = data;
@@ -113,35 +123,49 @@ static int dump_qp(int id, void *p, void *data)
 				&qp->ep->com.local_addr;
 			struct sockaddr_in *rsin = (struct sockaddr_in *)
 				&qp->ep->com.remote_addr;
+			struct sockaddr_in *mapped_lsin = (struct sockaddr_in *)
+				&qp->ep->com.mapped_local_addr;
+			struct sockaddr_in *mapped_rsin = (struct sockaddr_in *)
+				&qp->ep->com.mapped_remote_addr;
 
 			cc = snprintf(qpd->buf + qpd->pos, space,
 				      "rc qp sq id %u rq id %u state %u "
 				      "onchip %u ep tid %u state %u "
-				      "%pI4:%u->%pI4:%u\n",
+				      "%pI4:%u/%u->%pI4:%u/%u\n",
 				      qp->wq.sq.qid, qp->wq.rq.qid,
 				      (int)qp->attr.state,
 				      qp->wq.sq.flags & T4_SQ_ONCHIP,
 				      qp->ep->hwtid, (int)qp->ep->com.state,
 				      &lsin->sin_addr, ntohs(lsin->sin_port),
-				      &rsin->sin_addr, ntohs(rsin->sin_port));
+				      ntohs(mapped_lsin->sin_port),
+				      &rsin->sin_addr, ntohs(rsin->sin_port),
+				      ntohs(mapped_rsin->sin_port));
 		} else {
 			struct sockaddr_in6 *lsin6 = (struct sockaddr_in6 *)
 				&qp->ep->com.local_addr;
 			struct sockaddr_in6 *rsin6 = (struct sockaddr_in6 *)
 				&qp->ep->com.remote_addr;
+			struct sockaddr_in6 *mapped_lsin6 =
+				(struct sockaddr_in6 *)
+				&qp->ep->com.mapped_local_addr;
+			struct sockaddr_in6 *mapped_rsin6 =
+				(struct sockaddr_in6 *)
+				&qp->ep->com.mapped_remote_addr;
 
 			cc = snprintf(qpd->buf + qpd->pos, space,
 				      "rc qp sq id %u rq id %u state %u "
 				      "onchip %u ep tid %u state %u "
-				      "%pI6:%u->%pI6:%u\n",
+				      "%pI6:%u/%u->%pI6:%u/%u\n",
 				      qp->wq.sq.qid, qp->wq.rq.qid,
 				      (int)qp->attr.state,
 				      qp->wq.sq.flags & T4_SQ_ONCHIP,
 				      qp->ep->hwtid, (int)qp->ep->com.state,
 				      &lsin6->sin6_addr,
 				      ntohs(lsin6->sin6_port),
+				      ntohs(mapped_lsin6->sin6_port),
 				      &rsin6->sin6_addr,
-				      ntohs(rsin6->sin6_port));
+				      ntohs(rsin6->sin6_port),
+				      ntohs(mapped_rsin6->sin6_port));
 		}
 	} else
 		cc = snprintf(qpd->buf + qpd->pos, space,
@@ -386,31 +410,43 @@ static int dump_ep(int id, void *p, void *data)
 			&ep->com.local_addr;
 		struct sockaddr_in *rsin = (struct sockaddr_in *)
 			&ep->com.remote_addr;
+		struct sockaddr_in *mapped_lsin = (struct sockaddr_in *)
+			&ep->com.mapped_local_addr;
+		struct sockaddr_in *mapped_rsin = (struct sockaddr_in *)
+			&ep->com.mapped_remote_addr;
 
 		cc = snprintf(epd->buf + epd->pos, space,
 			      "ep %p cm_id %p qp %p state %d flags 0x%lx "
 			      "history 0x%lx hwtid %d atid %d "
-			      "%pI4:%d <-> %pI4:%d\n",
+			      "%pI4:%d/%d <-> %pI4:%d/%d\n",
 			      ep, ep->com.cm_id, ep->com.qp,
 			      (int)ep->com.state, ep->com.flags,
 			      ep->com.history, ep->hwtid, ep->atid,
 			      &lsin->sin_addr, ntohs(lsin->sin_port),
-			      &rsin->sin_addr, ntohs(rsin->sin_port));
+			      ntohs(mapped_lsin->sin_port),
+			      &rsin->sin_addr, ntohs(rsin->sin_port),
+			      ntohs(mapped_rsin->sin_port));
 	} else {
 		struct sockaddr_in6 *lsin6 = (struct sockaddr_in6 *)
 			&ep->com.local_addr;
 		struct sockaddr_in6 *rsin6 = (struct sockaddr_in6 *)
 			&ep->com.remote_addr;
+		struct sockaddr_in6 *mapped_lsin6 = (struct sockaddr_in6 *)
+			&ep->com.mapped_local_addr;
+		struct sockaddr_in6 *mapped_rsin6 = (struct sockaddr_in6 *)
+			&ep->com.mapped_remote_addr;
 
 		cc = snprintf(epd->buf + epd->pos, space,
 			      "ep %p cm_id %p qp %p state %d flags 0x%lx "
 			      "history 0x%lx hwtid %d atid %d "
-			      "%pI6:%d <-> %pI6:%d\n",
+			      "%pI6:%d/%d <-> %pI6:%d/%d\n",
 			      ep, ep->com.cm_id, ep->com.qp,
 			      (int)ep->com.state, ep->com.flags,
 			      ep->com.history, ep->hwtid, ep->atid,
 			      &lsin6->sin6_addr, ntohs(lsin6->sin6_port),
-			      &rsin6->sin6_addr, ntohs(rsin6->sin6_port));
+			      ntohs(mapped_lsin6->sin6_port),
+			      &rsin6->sin6_addr, ntohs(rsin6->sin6_port),
+			      ntohs(mapped_rsin6->sin6_port));
 	}
 	if (cc < space)
 		epd->pos += cc;
@@ -431,23 +467,29 @@ static int dump_listen_ep(int id, void *p, void *data)
 	if (ep->com.local_addr.ss_family == AF_INET) {
 		struct sockaddr_in *lsin = (struct sockaddr_in *)
 			&ep->com.local_addr;
+		struct sockaddr_in *mapped_lsin = (struct sockaddr_in *)
+			&ep->com.mapped_local_addr;
 
 		cc = snprintf(epd->buf + epd->pos, space,
 			      "ep %p cm_id %p state %d flags 0x%lx stid %d "
-			      "backlog %d %pI4:%d\n",
+			      "backlog %d %pI4:%d/%d\n",
 			      ep, ep->com.cm_id, (int)ep->com.state,
 			      ep->com.flags, ep->stid, ep->backlog,
-			      &lsin->sin_addr, ntohs(lsin->sin_port));
+			      &lsin->sin_addr, ntohs(lsin->sin_port),
+			      ntohs(mapped_lsin->sin_port));
 	} else {
 		struct sockaddr_in6 *lsin6 = (struct sockaddr_in6 *)
 			&ep->com.local_addr;
+		struct sockaddr_in6 *mapped_lsin6 = (struct sockaddr_in6 *)
+			&ep->com.mapped_local_addr;
 
 		cc = snprintf(epd->buf + epd->pos, space,
 			      "ep %p cm_id %p state %d flags 0x%lx stid %d "
-			      "backlog %d %pI6:%d\n",
+			      "backlog %d %pI6:%d/%d\n",
 			      ep, ep->com.cm_id, (int)ep->com.state,
 			      ep->com.flags, ep->stid, ep->backlog,
-			      &lsin6->sin6_addr, ntohs(lsin6->sin6_port));
+			      &lsin6->sin6_addr, ntohs(lsin6->sin6_port),
+			      ntohs(mapped_lsin6->sin6_port));
 	}
 	if (cc < space)
 		epd->pos += cc;
@@ -687,6 +729,7 @@ static void c4iw_dealloc(struct uld_ctx *ctx)
 	if (ctx->dev->rdev.oc_mw_kva)
 		iounmap(ctx->dev->rdev.oc_mw_kva);
 	ib_dealloc_device(&ctx->dev->ibdev);
+	iwpm_exit(RDMA_NL_C4IW);
 	ctx->dev = NULL;
 }
 
@@ -736,6 +779,7 @@ static struct c4iw_dev *c4iw_alloc(const struct cxgb4_lld_info *infop)
 			pci_resource_len(devp->rdev.lldi.pdev, 2));
 		if (!devp->rdev.bar2_kva) {
 			pr_err(MOD "Unable to ioremap BAR2\n");
+			ib_dealloc_device(&devp->ibdev);
 			return ERR_PTR(-EINVAL);
 		}
 	} else if (ocqp_supported(infop)) {
@@ -747,6 +791,7 @@ static struct c4iw_dev *c4iw_alloc(const struct cxgb4_lld_info *infop)
 			devp->rdev.lldi.vr->ocq.size);
 		if (!devp->rdev.oc_mw_kva) {
 			pr_err(MOD "Unable to ioremap onchip mem\n");
+			ib_dealloc_device(&devp->ibdev);
 			return ERR_PTR(-EINVAL);
 		}
 	}
@@ -780,6 +825,14 @@ static struct c4iw_dev *c4iw_alloc(const struct cxgb4_lld_info *infop)
 					c4iw_debugfs_root);
 		setup_debugfs(devp);
 	}
+
+	ret = iwpm_init(RDMA_NL_C4IW);
+	if (ret) {
+		pr_err("port mapper initialization failed with %d\n", ret);
+		ib_dealloc_device(&devp->ibdev);
+		return ERR_PTR(ret);
+	}
+
 	return devp;
 }
 
@@ -1274,6 +1327,11 @@ static int __init c4iw_init_module(void)
 		printk(KERN_WARNING MOD
 		       "could not create debugfs entry, continuing\n");
 
+	if (ibnl_add_client(RDMA_NL_C4IW, RDMA_NL_IWPM_NUM_OPS,
+			    c4iw_nl_cb_table))
+		pr_err("%s[%u]: Failed to add netlink callback\n"
+		       , __func__, __LINE__);
+
 	cxgb4_register_uld(CXGB4_ULD_RDMA, &c4iw_uld_info);
 
 	return 0;
@@ -1291,6 +1349,7 @@ static void __exit c4iw_exit_module(void)
 	}
 	mutex_unlock(&dev_mutex);
 	cxgb4_unregister_uld(CXGB4_ULD_RDMA);
+	ibnl_remove_client(RDMA_NL_C4IW);
 	c4iw_cm_term();
 	debugfs_remove_recursive(c4iw_debugfs_root);
 }
diff --git a/drivers/infiniband/hw/cxgb4/iw_cxgb4.h b/drivers/infiniband/hw/cxgb4/iw_cxgb4.h
index 7474b490760..6f533fbcc4b 100644
--- a/drivers/infiniband/hw/cxgb4/iw_cxgb4.h
+++ b/drivers/infiniband/hw/cxgb4/iw_cxgb4.h
@@ -52,6 +52,8 @@
 
 #include <rdma/ib_verbs.h>
 #include <rdma/iw_cm.h>
+#include <rdma/rdma_netlink.h>
+#include <rdma/iw_portmap.h>
 
 #include "cxgb4.h"
 #include "cxgb4_uld.h"
@@ -728,6 +730,7 @@ enum c4iw_ep_flags {
 	CLOSE_SENT		= 3,
 	TIMEOUT                 = 4,
 	QP_REFERENCED           = 5,
+	RELEASE_MAPINFO		= 6,
 };
 
 enum c4iw_ep_history {
@@ -764,6 +767,8 @@ struct c4iw_ep_common {
 	struct mutex mutex;
 	struct sockaddr_storage local_addr;
 	struct sockaddr_storage remote_addr;
+	struct sockaddr_storage mapped_local_addr;
+	struct sockaddr_storage mapped_remote_addr;
 	struct c4iw_wr_wait wr_wait;
 	unsigned long flags;
 	unsigned long history;
@@ -807,6 +812,45 @@ struct c4iw_ep {
 	unsigned int retry_count;
 };
 
+static inline void print_addr(struct c4iw_ep_common *epc, const char *func,
+			      const char *msg)
+{
+
+#define SINA(a) (&(((struct sockaddr_in *)(a))->sin_addr.s_addr))
+#define SINP(a) ntohs(((struct sockaddr_in *)(a))->sin_port)
+#define SIN6A(a) (&(((struct sockaddr_in6 *)(a))->sin6_addr))
+#define SIN6P(a) ntohs(((struct sockaddr_in6 *)(a))->sin6_port)
+
+	if (c4iw_debug) {
+		switch (epc->local_addr.ss_family) {
+		case AF_INET:
+			PDBG("%s %s %pI4:%u/%u <-> %pI4:%u/%u\n",
+			     func, msg, SINA(&epc->local_addr),
+			     SINP(&epc->local_addr),
+			     SINP(&epc->mapped_local_addr),
+			     SINA(&epc->remote_addr),
+			     SINP(&epc->remote_addr),
+			     SINP(&epc->mapped_remote_addr));
+			break;
+		case AF_INET6:
+			PDBG("%s %s %pI6:%u/%u <-> %pI6:%u/%u\n",
+			     func, msg, SIN6A(&epc->local_addr),
+			     SIN6P(&epc->local_addr),
+			     SIN6P(&epc->mapped_local_addr),
+			     SIN6A(&epc->remote_addr),
+			     SIN6P(&epc->remote_addr),
+			     SIN6P(&epc->mapped_remote_addr));
+			break;
+		default:
+			break;
+		}
+	}
+#undef SINA
+#undef SINP
+#undef SIN6A
+#undef SIN6P
+}
+
 static inline struct c4iw_ep *to_ep(struct iw_cm_id *cm_id)
 {
 	return cm_id->provider_data;
diff --git a/drivers/infiniband/hw/cxgb4/provider.c b/drivers/infiniband/hw/cxgb4/provider.c
index a94a3e12c34..c777e22bd8d 100644
--- a/drivers/infiniband/hw/cxgb4/provider.c
+++ b/drivers/infiniband/hw/cxgb4/provider.c
@@ -122,7 +122,7 @@ static struct ib_ucontext *c4iw_alloc_ucontext(struct ib_device *ibdev,
 	INIT_LIST_HEAD(&context->mmaps);
 	spin_lock_init(&context->mmap_lock);
 
-	if (udata->outlen < sizeof(uresp)) {
+	if (udata->outlen < sizeof(uresp) - sizeof(uresp.reserved)) {
 		if (!warned++)
 			pr_err(MOD "Warning - downlevel libcxgb4 (non-fatal), device status page disabled.");
 		rhp->rdev.flags |= T4_STATUS_PAGE_DISABLED;
@@ -140,7 +140,8 @@ static struct ib_ucontext *c4iw_alloc_ucontext(struct ib_device *ibdev,
 		context->key += PAGE_SIZE;
 		spin_unlock(&context->mmap_lock);
 
-		ret = ib_copy_to_udata(udata, &uresp, sizeof(uresp));
+		ret = ib_copy_to_udata(udata, &uresp,
+				       sizeof(uresp) - sizeof(uresp.reserved));
 		if (ret)
 			goto err_mm;
 
diff --git a/drivers/infiniband/hw/cxgb4/user.h b/drivers/infiniband/hw/cxgb4/user.h
index 11ccd276e5d..cbd0ce17072 100644
--- a/drivers/infiniband/hw/cxgb4/user.h
+++ b/drivers/infiniband/hw/cxgb4/user.h
@@ -48,6 +48,7 @@ struct c4iw_create_cq_resp {
 	__u32 cqid;
 	__u32 size;
 	__u32 qid_mask;
+	__u32 reserved; /* explicit padding (optional for i386) */
 };
 
 
@@ -74,5 +75,6 @@ struct c4iw_create_qp_resp {
 struct c4iw_alloc_ucontext_resp {
 	__u64 status_page_key;
 	__u32 status_page_size;
+	__u32 reserved; /* explicit padding (optional for i386) */
 };
 #endif
diff --git a/drivers/infiniband/hw/ipath/ipath_diag.c b/drivers/infiniband/hw/ipath/ipath_diag.c
index e2f9a51f4a3..45802e97332 100644
--- a/drivers/infiniband/hw/ipath/ipath_diag.c
+++ b/drivers/infiniband/hw/ipath/ipath_diag.c
@@ -346,6 +346,10 @@ static ssize_t ipath_diagpkt_write(struct file *fp,
 			ret = -EFAULT;
 			goto bail;
 		}
+		dp.len = odp.len;
+		dp.unit = odp.unit;
+		dp.data = odp.data;
+		dp.pbc_wd = 0;
 	} else {
 		ret = -EINVAL;
 		goto bail;
diff --git a/drivers/infiniband/hw/ipath/ipath_intr.c b/drivers/infiniband/hw/ipath/ipath_intr.c
index 26dfbc8ee0f..01ba792791a 100644
--- a/drivers/infiniband/hw/ipath/ipath_intr.c
+++ b/drivers/infiniband/hw/ipath/ipath_intr.c
@@ -70,7 +70,7 @@ void ipath_disarm_senderrbufs(struct ipath_devdata *dd)
 	if (sbuf[0] || sbuf[1] || (piobcnt > 128 && (sbuf[2] || sbuf[3]))) {
 		int i;
 		if (ipath_debug & (__IPATH_PKTDBG|__IPATH_DBG) &&
-			dd->ipath_lastcancel > jiffies) {
+			time_after(dd->ipath_lastcancel, jiffies)) {
 			__IPATH_DBG_WHICH(__IPATH_PKTDBG|__IPATH_DBG,
 					  "SendbufErrs %lx %lx", sbuf[0],
 					  sbuf[1]);
@@ -755,7 +755,7 @@ static int handle_errors(struct ipath_devdata *dd, ipath_err_t errs)
 
 	/* likely due to cancel; so suppress message unless verbose */
 	if ((errs & (INFINIPATH_E_SPKTLEN | INFINIPATH_E_SPIOARMLAUNCH)) &&
-		dd->ipath_lastcancel > jiffies) {
+		time_after(dd->ipath_lastcancel, jiffies)) {
 		/* armlaunch takes precedence; it often causes both. */
 		ipath_cdbg(VERBOSE,
 			"Suppressed %s error (%llx) after sendbuf cancel\n",
diff --git a/drivers/infiniband/hw/ipath/ipath_sdma.c b/drivers/infiniband/hw/ipath/ipath_sdma.c
index 98ac18ec977..17a517766ad 100644
--- a/drivers/infiniband/hw/ipath/ipath_sdma.c
+++ b/drivers/infiniband/hw/ipath/ipath_sdma.c
@@ -247,7 +247,7 @@ static void sdma_abort_task(unsigned long opaque)
 
 	/* ipath_sdma_abort() is done, waiting for interrupt */
 	if (status == IPATH_SDMA_ABORT_DISARMED) {
-		if (jiffies < dd->ipath_sdma_abort_intr_timeout)
+		if (time_before(jiffies, dd->ipath_sdma_abort_intr_timeout))
 			goto resched_noprint;
 		/* give up, intr got lost somewhere */
 		ipath_dbg("give up waiting for SDMADISABLED intr\n");
@@ -341,7 +341,7 @@ resched:
 	 * JAG - this is bad to just have default be a loop without
 	 * state change
 	 */
-	if (jiffies > dd->ipath_sdma_abort_jiffies) {
+	if (time_after(jiffies, dd->ipath_sdma_abort_jiffies)) {
 		ipath_dbg("looping with status 0x%08lx\n",
 			  dd->ipath_sdma_status);
 		dd->ipath_sdma_abort_jiffies = jiffies + 5 * HZ;
diff --git a/drivers/infiniband/hw/mlx4/ah.c b/drivers/infiniband/hw/mlx4/ah.c
index 170dca60804..2d8c3397774 100644
--- a/drivers/infiniband/hw/mlx4/ah.c
+++ b/drivers/infiniband/hw/mlx4/ah.c
@@ -73,7 +73,7 @@ static struct ib_ah *create_iboe_ah(struct ib_pd *pd, struct ib_ah_attr *ah_attr
 {
 	struct mlx4_ib_dev *ibdev = to_mdev(pd->device);
 	struct mlx4_dev *dev = ibdev->dev;
-	int is_mcast;
+	int is_mcast = 0;
 	struct in6_addr in6;
 	u16 vlan_tag;
 
diff --git a/drivers/infiniband/hw/mlx4/cq.c b/drivers/infiniband/hw/mlx4/cq.c
index 5f640814cc8..1066eec854a 100644
--- a/drivers/infiniband/hw/mlx4/cq.c
+++ b/drivers/infiniband/hw/mlx4/cq.c
@@ -102,7 +102,7 @@ static int mlx4_ib_alloc_cq_buf(struct mlx4_ib_dev *dev, struct mlx4_ib_cq_buf *
 	int err;
 
 	err = mlx4_buf_alloc(dev->dev, nent * dev->dev->caps.cqe_size,
-			     PAGE_SIZE * 2, &buf->buf);
+			     PAGE_SIZE * 2, &buf->buf, GFP_KERNEL);
 
 	if (err)
 		goto out;
@@ -113,7 +113,7 @@ static int mlx4_ib_alloc_cq_buf(struct mlx4_ib_dev *dev, struct mlx4_ib_cq_buf *
 	if (err)
 		goto err_buf;
 
-	err = mlx4_buf_write_mtt(dev->dev, &buf->mtt, &buf->buf);
+	err = mlx4_buf_write_mtt(dev->dev, &buf->mtt, &buf->buf, GFP_KERNEL);
 	if (err)
 		goto err_mtt;
 
@@ -209,7 +209,7 @@ struct ib_cq *mlx4_ib_create_cq(struct ib_device *ibdev, int entries, int vector
 
 		uar = &to_mucontext(context)->uar;
 	} else {
-		err = mlx4_db_alloc(dev->dev, &cq->db, 1);
+		err = mlx4_db_alloc(dev->dev, &cq->db, 1, GFP_KERNEL);
 		if (err)
 			goto err_cq;
 
diff --git a/drivers/infiniband/hw/mlx4/mad.c b/drivers/infiniband/hw/mlx4/mad.c
index fd36ec67263..287ad0564ac 100644
--- a/drivers/infiniband/hw/mlx4/mad.c
+++ b/drivers/infiniband/hw/mlx4/mad.c
@@ -478,10 +478,6 @@ int mlx4_ib_send_to_slave(struct mlx4_ib_dev *dev, int slave, u8 port,
 	if (!tun_ctx || tun_ctx->state != DEMUX_PV_STATE_ACTIVE)
 		return -EAGAIN;
 
-	/* QP0 forwarding only for Dom0 */
-	if (!dest_qpt && (mlx4_master_func_num(dev->dev) != slave))
-		return -EINVAL;
-
 	if (!dest_qpt)
 		tun_qp = &tun_ctx->qp[0];
 	else
@@ -667,6 +663,21 @@ static int mlx4_ib_demux_mad(struct ib_device *ibdev, u8 port,
 	}
 	/* Class-specific handling */
 	switch (mad->mad_hdr.mgmt_class) {
+	case IB_MGMT_CLASS_SUBN_LID_ROUTED:
+	case IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE:
+		/* 255 indicates the dom0 */
+		if (slave != 255 && slave != mlx4_master_func_num(dev->dev)) {
+			if (!mlx4_vf_smi_enabled(dev->dev, slave, port))
+				return -EPERM;
+			/* for a VF. drop unsolicited MADs */
+			if (!(mad->mad_hdr.method & IB_MGMT_METHOD_RESP)) {
+				mlx4_ib_warn(ibdev, "demux QP0. rejecting unsolicited mad for slave %d class 0x%x, method 0x%x\n",
+					     slave, mad->mad_hdr.mgmt_class,
+					     mad->mad_hdr.method);
+				return -EINVAL;
+			}
+		}
+		break;
 	case IB_MGMT_CLASS_SUBN_ADM:
 		if (mlx4_ib_demux_sa_handler(ibdev, port, slave,
 					     (struct ib_sa_mad *) mad))
@@ -1165,10 +1176,6 @@ int mlx4_ib_send_to_wire(struct mlx4_ib_dev *dev, int slave, u8 port,
 	if (!sqp_ctx || sqp_ctx->state != DEMUX_PV_STATE_ACTIVE)
 		return -EAGAIN;
 
-	/* QP0 forwarding only for Dom0 */
-	if (dest_qpt == IB_QPT_SMI && (mlx4_master_func_num(dev->dev) != slave))
-		return -EINVAL;
-
 	if (dest_qpt == IB_QPT_SMI) {
 		src_qpnum = 0;
 		sqp = &sqp_ctx->qp[0];
@@ -1285,11 +1292,6 @@ static void mlx4_ib_multiplex_mad(struct mlx4_ib_demux_pv_ctx *ctx, struct ib_wc
 			     "belongs to another slave\n", wc->src_qp);
 		return;
 	}
-	if (slave != mlx4_master_func_num(dev->dev) && !(wc->src_qp & 0x2)) {
-		mlx4_ib_warn(ctx->ib_dev, "can't multiplex bad sqp:%d: "
-			     "non-master trying to send QP0 packets\n", wc->src_qp);
-		return;
-	}
 
 	/* Map transaction ID */
 	ib_dma_sync_single_for_cpu(ctx->ib_dev, tun_qp->ring[wr_ix].map,
@@ -1317,6 +1319,12 @@ static void mlx4_ib_multiplex_mad(struct mlx4_ib_demux_pv_ctx *ctx, struct ib_wc
 
 	/* Class-specific handling */
 	switch (tunnel->mad.mad_hdr.mgmt_class) {
+	case IB_MGMT_CLASS_SUBN_LID_ROUTED:
+	case IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE:
+		if (slave != mlx4_master_func_num(dev->dev) &&
+		    !mlx4_vf_smi_enabled(dev->dev, slave, ctx->port))
+			return;
+		break;
 	case IB_MGMT_CLASS_SUBN_ADM:
 		if (mlx4_ib_multiplex_sa_handler(ctx->ib_dev, ctx->port, slave,
 			      (struct ib_sa_mad *) &tunnel->mad))
@@ -1749,9 +1757,9 @@ static int create_pv_resources(struct ib_device *ibdev, int slave, int port,
 		return -EEXIST;
 
 	ctx->state = DEMUX_PV_STATE_STARTING;
-	/* have QP0 only on port owner, and only if link layer is IB */
-	if (ctx->slave == mlx4_master_func_num(to_mdev(ctx->ib_dev)->dev) &&
-	    rdma_port_get_link_layer(ibdev, ctx->port) == IB_LINK_LAYER_INFINIBAND)
+	/* have QP0 only if link layer is IB */
+	if (rdma_port_get_link_layer(ibdev, ctx->port) ==
+	    IB_LINK_LAYER_INFINIBAND)
 		ctx->has_smi = 1;
 
 	if (ctx->has_smi) {
diff --git a/drivers/infiniband/hw/mlx4/main.c b/drivers/infiniband/hw/mlx4/main.c
index 1b6dbe156a3..0f7027e7db1 100644
--- a/drivers/infiniband/hw/mlx4/main.c
+++ b/drivers/infiniband/hw/mlx4/main.c
@@ -48,6 +48,7 @@
 
 #include <linux/mlx4/driver.h>
 #include <linux/mlx4/cmd.h>
+#include <linux/mlx4/qp.h>
 
 #include "mlx4_ib.h"
 #include "user.h"
@@ -544,12 +545,11 @@ static int mlx4_ib_modify_device(struct ib_device *ibdev, int mask,
 	return 0;
 }
 
-static int mlx4_SET_PORT(struct mlx4_ib_dev *dev, u8 port, int reset_qkey_viols,
-			 u32 cap_mask)
+static int mlx4_ib_SET_PORT(struct mlx4_ib_dev *dev, u8 port, int reset_qkey_viols,
+			    u32 cap_mask)
 {
 	struct mlx4_cmd_mailbox *mailbox;
 	int err;
-	u8 is_eth = dev->dev->caps.port_type[port] == MLX4_PORT_TYPE_ETH;
 
 	mailbox = mlx4_alloc_cmd_mailbox(dev->dev);
 	if (IS_ERR(mailbox))
@@ -563,8 +563,8 @@ static int mlx4_SET_PORT(struct mlx4_ib_dev *dev, u8 port, int reset_qkey_viols,
 		((__be32 *) mailbox->buf)[1] = cpu_to_be32(cap_mask);
 	}
 
-	err = mlx4_cmd(dev->dev, mailbox->dma, port, is_eth, MLX4_CMD_SET_PORT,
-		       MLX4_CMD_TIME_CLASS_B, MLX4_CMD_NATIVE);
+	err = mlx4_cmd(dev->dev, mailbox->dma, port, 0, MLX4_CMD_SET_PORT,
+		       MLX4_CMD_TIME_CLASS_B, MLX4_CMD_WRAPPED);
 
 	mlx4_free_cmd_mailbox(dev->dev, mailbox);
 	return err;
@@ -573,11 +573,20 @@ static int mlx4_SET_PORT(struct mlx4_ib_dev *dev, u8 port, int reset_qkey_viols,
 static int mlx4_ib_modify_port(struct ib_device *ibdev, u8 port, int mask,
 			       struct ib_port_modify *props)
 {
+	struct mlx4_ib_dev *mdev = to_mdev(ibdev);
+	u8 is_eth = mdev->dev->caps.port_type[port] == MLX4_PORT_TYPE_ETH;
 	struct ib_port_attr attr;
 	u32 cap_mask;
 	int err;
 
-	mutex_lock(&to_mdev(ibdev)->cap_mask_mutex);
+	/* return OK if this is RoCE. CM calls ib_modify_port() regardless
+	 * of whether port link layer is ETH or IB. For ETH ports, qkey
+	 * violations and port capabilities are not meaningful.
+	 */
+	if (is_eth)
+		return 0;
+
+	mutex_lock(&mdev->cap_mask_mutex);
 
 	err = mlx4_ib_query_port(ibdev, port, &attr);
 	if (err)
@@ -586,9 +595,9 @@ static int mlx4_ib_modify_port(struct ib_device *ibdev, u8 port, int mask,
 	cap_mask = (attr.port_cap_flags | props->set_port_cap_mask) &
 		~props->clr_port_cap_mask;
 
-	err = mlx4_SET_PORT(to_mdev(ibdev), port,
-			    !!(mask & IB_PORT_RESET_QKEY_CNTR),
-			    cap_mask);
+	err = mlx4_ib_SET_PORT(mdev, port,
+			       !!(mask & IB_PORT_RESET_QKEY_CNTR),
+			       cap_mask);
 
 out:
 	mutex_unlock(&to_mdev(ibdev)->cap_mask_mutex);
@@ -1614,6 +1623,53 @@ static int mlx4_ib_inet6_event(struct notifier_block *this, unsigned long event,
 }
 #endif
 
+#define MLX4_IB_INVALID_MAC	((u64)-1)
+static void mlx4_ib_update_qps(struct mlx4_ib_dev *ibdev,
+			       struct net_device *dev,
+			       int port)
+{
+	u64 new_smac = 0;
+	u64 release_mac = MLX4_IB_INVALID_MAC;
+	struct mlx4_ib_qp *qp;
+
+	read_lock(&dev_base_lock);
+	new_smac = mlx4_mac_to_u64(dev->dev_addr);
+	read_unlock(&dev_base_lock);
+
+	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;
+		struct mlx4_update_qp_params update_params;
+
+		if (new_smac == old_smac)
+			goto unlock;
+
+		new_smac_index = mlx4_register_mac(ibdev->dev, port, new_smac);
+
+		if (new_smac_index < 0)
+			goto unlock;
+
+		update_params.smac_index = new_smac_index;
+		if (mlx4_update_qp(ibdev->dev, &qp->mqp, MLX4_UPDATE_QP_SMAC,
+				   &update_params)) {
+			release_mac = new_smac;
+			goto unlock;
+		}
+
+		qp->pri.smac = new_smac;
+		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);
+}
+
 static void mlx4_ib_get_dev_addr(struct net_device *dev,
 				 struct mlx4_ib_dev *ibdev, u8 port)
 {
@@ -1689,9 +1745,13 @@ static int mlx4_ib_init_gid_table(struct mlx4_ib_dev *ibdev)
 	return 0;
 }
 
-static void mlx4_ib_scan_netdevs(struct mlx4_ib_dev *ibdev)
+static void mlx4_ib_scan_netdevs(struct mlx4_ib_dev *ibdev,
+				 struct net_device *dev,
+				 unsigned long event)
+
 {
 	struct mlx4_ib_iboe *iboe;
+	int update_qps_port = -1;
 	int port;
 
 	iboe = &ibdev->iboe;
@@ -1719,6 +1779,11 @@ static void mlx4_ib_scan_netdevs(struct mlx4_ib_dev *ibdev)
 		}
 		curr_master = iboe->masters[port - 1];
 
+		if (dev == iboe->netdevs[port - 1] &&
+		    (event == NETDEV_CHANGEADDR || event == NETDEV_REGISTER ||
+		     event == NETDEV_UP || event == NETDEV_CHANGE))
+			update_qps_port = port;
+
 		if (curr_netdev) {
 			port_state = (netif_running(curr_netdev) && netif_carrier_ok(curr_netdev)) ?
 						IB_PORT_ACTIVE : IB_PORT_DOWN;
@@ -1752,6 +1817,9 @@ static void mlx4_ib_scan_netdevs(struct mlx4_ib_dev *ibdev)
 	}
 
 	spin_unlock(&iboe->lock);
+
+	if (update_qps_port > 0)
+		mlx4_ib_update_qps(ibdev, dev, update_qps_port);
 }
 
 static int mlx4_ib_netdev_event(struct notifier_block *this,
@@ -1764,7 +1832,7 @@ static int mlx4_ib_netdev_event(struct notifier_block *this,
 		return NOTIFY_DONE;
 
 	ibdev = container_of(this, struct mlx4_ib_dev, iboe.nb);
-	mlx4_ib_scan_netdevs(ibdev);
+	mlx4_ib_scan_netdevs(ibdev, dev, event);
 
 	return NOTIFY_DONE;
 }
@@ -2043,6 +2111,7 @@ static void *mlx4_ib_add(struct mlx4_dev *dev)
 		goto err_map;
 
 	for (i = 0; i < ibdev->num_ports; ++i) {
+		mutex_init(&ibdev->qp1_proxy_lock[i]);
 		if (mlx4_ib_port_link_layer(&ibdev->ib_dev, i + 1) ==
 						IB_LINK_LAYER_ETHERNET) {
 			err = mlx4_counter_alloc(ibdev->dev, &ibdev->counters[i]);
@@ -2126,7 +2195,7 @@ static void *mlx4_ib_add(struct mlx4_dev *dev)
 		for (i = 1 ; i <= ibdev->num_ports ; ++i)
 			reset_gid_table(ibdev, i);
 		rtnl_lock();
-		mlx4_ib_scan_netdevs(ibdev);
+		mlx4_ib_scan_netdevs(ibdev, NULL, 0);
 		rtnl_unlock();
 		mlx4_ib_init_gid_table(ibdev);
 	}
diff --git a/drivers/infiniband/hw/mlx4/mlx4_ib.h b/drivers/infiniband/hw/mlx4/mlx4_ib.h
index f589522fddf..369da3ca5d6 100644
--- a/drivers/infiniband/hw/mlx4/mlx4_ib.h
+++ b/drivers/infiniband/hw/mlx4/mlx4_ib.h
@@ -156,6 +156,7 @@ enum mlx4_ib_qp_flags {
 	MLX4_IB_QP_LSO = IB_QP_CREATE_IPOIB_UD_LSO,
 	MLX4_IB_QP_BLOCK_MULTICAST_LOOPBACK = IB_QP_CREATE_BLOCK_MULTICAST_LOOPBACK,
 	MLX4_IB_QP_NETIF = IB_QP_CREATE_NETIF_QP,
+	MLX4_IB_QP_CREATE_USE_GFP_NOIO = IB_QP_CREATE_USE_GFP_NOIO,
 	MLX4_IB_SRIOV_TUNNEL_QP = 1 << 30,
 	MLX4_IB_SRIOV_SQP = 1 << 31,
 };
@@ -522,6 +523,9 @@ struct mlx4_ib_dev {
 	int steer_qpn_count;
 	int steer_qpn_base;
 	int steering_support;
+	struct mlx4_ib_qp      *qp1_proxy[MLX4_MAX_PORTS];
+	/* lock when destroying qp1_proxy and getting netdev events */
+	struct mutex		qp1_proxy_lock[MLX4_MAX_PORTS];
 };
 
 struct ib_event_work {
diff --git a/drivers/infiniband/hw/mlx4/qp.c b/drivers/infiniband/hw/mlx4/qp.c
index 41308af4163..67780452f0c 100644
--- a/drivers/infiniband/hw/mlx4/qp.c
+++ b/drivers/infiniband/hw/mlx4/qp.c
@@ -608,9 +608,20 @@ static int qp_has_rq(struct ib_qp_init_attr *attr)
 	return !attr->srq;
 }
 
+static int qp0_enabled_vf(struct mlx4_dev *dev, int qpn)
+{
+	int i;
+	for (i = 0; i < dev->caps.num_ports; i++) {
+		if (qpn == dev->caps.qp0_proxy[i])
+			return !!dev->caps.qp0_qkey[i];
+	}
+	return 0;
+}
+
 static int create_qp_common(struct mlx4_ib_dev *dev, struct ib_pd *pd,
 			    struct ib_qp_init_attr *init_attr,
-			    struct ib_udata *udata, int sqpn, struct mlx4_ib_qp **caller_qp)
+			    struct ib_udata *udata, int sqpn, struct mlx4_ib_qp **caller_qp,
+			    gfp_t gfp)
 {
 	int qpn;
 	int err;
@@ -625,10 +636,13 @@ static int create_qp_common(struct mlx4_ib_dev *dev, struct ib_pd *pd,
 		     !(init_attr->create_flags & MLX4_IB_SRIOV_SQP))) {
 			if (init_attr->qp_type == IB_QPT_GSI)
 				qp_type = MLX4_IB_QPT_PROXY_GSI;
-			else if (mlx4_is_master(dev->dev))
-				qp_type = MLX4_IB_QPT_PROXY_SMI_OWNER;
-			else
-				qp_type = MLX4_IB_QPT_PROXY_SMI;
+			else {
+				if (mlx4_is_master(dev->dev) ||
+				    qp0_enabled_vf(dev->dev, sqpn))
+					qp_type = MLX4_IB_QPT_PROXY_SMI_OWNER;
+				else
+					qp_type = MLX4_IB_QPT_PROXY_SMI;
+			}
 		}
 		qpn = sqpn;
 		/* add extra sg entry for tunneling */
@@ -643,7 +657,9 @@ static int create_qp_common(struct mlx4_ib_dev *dev, struct ib_pd *pd,
 			return -EINVAL;
 		if (tnl_init->proxy_qp_type == IB_QPT_GSI)
 			qp_type = MLX4_IB_QPT_TUN_GSI;
-		else if (tnl_init->slave == mlx4_master_func_num(dev->dev))
+		else if (tnl_init->slave == mlx4_master_func_num(dev->dev) ||
+			 mlx4_vf_smi_enabled(dev->dev, tnl_init->slave,
+					     tnl_init->port))
 			qp_type = MLX4_IB_QPT_TUN_SMI_OWNER;
 		else
 			qp_type = MLX4_IB_QPT_TUN_SMI;
@@ -658,14 +674,14 @@ static int create_qp_common(struct mlx4_ib_dev *dev, struct ib_pd *pd,
 		if (qp_type == MLX4_IB_QPT_SMI || qp_type == MLX4_IB_QPT_GSI ||
 		    (qp_type & (MLX4_IB_QPT_PROXY_SMI | MLX4_IB_QPT_PROXY_SMI_OWNER |
 				MLX4_IB_QPT_PROXY_GSI | MLX4_IB_QPT_TUN_SMI_OWNER))) {
-			sqp = kzalloc(sizeof (struct mlx4_ib_sqp), GFP_KERNEL);
+			sqp = kzalloc(sizeof (struct mlx4_ib_sqp), gfp);
 			if (!sqp)
 				return -ENOMEM;
 			qp = &sqp->qp;
 			qp->pri.vid = 0xFFFF;
 			qp->alt.vid = 0xFFFF;
 		} else {
-			qp = kzalloc(sizeof (struct mlx4_ib_qp), GFP_KERNEL);
+			qp = kzalloc(sizeof (struct mlx4_ib_qp), gfp);
 			if (!qp)
 				return -ENOMEM;
 			qp->pri.vid = 0xFFFF;
@@ -748,14 +764,14 @@ static int create_qp_common(struct mlx4_ib_dev *dev, struct ib_pd *pd,
 			goto err;
 
 		if (qp_has_rq(init_attr)) {
-			err = mlx4_db_alloc(dev->dev, &qp->db, 0);
+			err = mlx4_db_alloc(dev->dev, &qp->db, 0, gfp);
 			if (err)
 				goto err;
 
 			*qp->db.db = 0;
 		}
 
-		if (mlx4_buf_alloc(dev->dev, qp->buf_size, PAGE_SIZE * 2, &qp->buf)) {
+		if (mlx4_buf_alloc(dev->dev, qp->buf_size, PAGE_SIZE * 2, &qp->buf, gfp)) {
 			err = -ENOMEM;
 			goto err_db;
 		}
@@ -765,13 +781,12 @@ static int create_qp_common(struct mlx4_ib_dev *dev, struct ib_pd *pd,
 		if (err)
 			goto err_buf;
 
-		err = mlx4_buf_write_mtt(dev->dev, &qp->mtt, &qp->buf);
+		err = mlx4_buf_write_mtt(dev->dev, &qp->mtt, &qp->buf, gfp);
 		if (err)
 			goto err_mtt;
 
-		qp->sq.wrid  = kmalloc(qp->sq.wqe_cnt * sizeof (u64), GFP_KERNEL);
-		qp->rq.wrid  = kmalloc(qp->rq.wqe_cnt * sizeof (u64), GFP_KERNEL);
-
+		qp->sq.wrid  = kmalloc(qp->sq.wqe_cnt * sizeof (u64), gfp);
+		qp->rq.wrid  = kmalloc(qp->rq.wqe_cnt * sizeof (u64), gfp);
 		if (!qp->sq.wrid || !qp->rq.wrid) {
 			err = -ENOMEM;
 			goto err_wrid;
@@ -801,7 +816,7 @@ static int create_qp_common(struct mlx4_ib_dev *dev, struct ib_pd *pd,
 			goto err_proxy;
 	}
 
-	err = mlx4_qp_alloc(dev->dev, qpn, &qp->mqp);
+	err = mlx4_qp_alloc(dev->dev, qpn, &qp->mqp, gfp);
 	if (err)
 		goto err_qpn;
 
@@ -1040,7 +1055,10 @@ struct ib_qp *mlx4_ib_create_qp(struct ib_pd *pd,
 	struct mlx4_ib_qp *qp = NULL;
 	int err;
 	u16 xrcdn = 0;
+	gfp_t gfp;
 
+	gfp = (init_attr->create_flags & MLX4_IB_QP_CREATE_USE_GFP_NOIO) ?
+		GFP_NOIO : GFP_KERNEL;
 	/*
 	 * We only support LSO, vendor flag1, and multicast loopback blocking,
 	 * and only for kernel UD QPs.
@@ -1049,7 +1067,8 @@ struct ib_qp *mlx4_ib_create_qp(struct ib_pd *pd,
 					MLX4_IB_QP_BLOCK_MULTICAST_LOOPBACK |
 					MLX4_IB_SRIOV_TUNNEL_QP |
 					MLX4_IB_SRIOV_SQP |
-					MLX4_IB_QP_NETIF))
+					MLX4_IB_QP_NETIF |
+					MLX4_IB_QP_CREATE_USE_GFP_NOIO))
 		return ERR_PTR(-EINVAL);
 
 	if (init_attr->create_flags & IB_QP_CREATE_NETIF_QP) {
@@ -1059,7 +1078,7 @@ struct ib_qp *mlx4_ib_create_qp(struct ib_pd *pd,
 
 	if (init_attr->create_flags &&
 	    (udata ||
-	     ((init_attr->create_flags & ~MLX4_IB_SRIOV_SQP) &&
+	     ((init_attr->create_flags & ~(MLX4_IB_SRIOV_SQP | MLX4_IB_QP_CREATE_USE_GFP_NOIO)) &&
 	      init_attr->qp_type != IB_QPT_UD) ||
 	     ((init_attr->create_flags & MLX4_IB_SRIOV_SQP) &&
 	      init_attr->qp_type > IB_QPT_GSI)))
@@ -1079,7 +1098,7 @@ struct ib_qp *mlx4_ib_create_qp(struct ib_pd *pd,
 	case IB_QPT_RC:
 	case IB_QPT_UC:
 	case IB_QPT_RAW_PACKET:
-		qp = kzalloc(sizeof *qp, GFP_KERNEL);
+		qp = kzalloc(sizeof *qp, gfp);
 		if (!qp)
 			return ERR_PTR(-ENOMEM);
 		qp->pri.vid = 0xFFFF;
@@ -1088,7 +1107,7 @@ struct ib_qp *mlx4_ib_create_qp(struct ib_pd *pd,
 	case IB_QPT_UD:
 	{
 		err = create_qp_common(to_mdev(pd->device), pd, init_attr,
-				       udata, 0, &qp);
+				       udata, 0, &qp, gfp);
 		if (err)
 			return ERR_PTR(err);
 
@@ -1106,7 +1125,7 @@ struct ib_qp *mlx4_ib_create_qp(struct ib_pd *pd,
 
 		err = create_qp_common(to_mdev(pd->device), pd, init_attr, udata,
 				       get_sqp_num(to_mdev(pd->device), init_attr),
-				       &qp);
+				       &qp, gfp);
 		if (err)
 			return ERR_PTR(err);
 
@@ -1132,6 +1151,12 @@ int mlx4_ib_destroy_qp(struct ib_qp *qp)
 	if (is_qp0(dev, mqp))
 		mlx4_CLOSE_PORT(dev->dev, mqp->port);
 
+	if (dev->qp1_proxy[mqp->port - 1] == mqp) {
+		mutex_lock(&dev->qp1_proxy_lock[mqp->port - 1]);
+		dev->qp1_proxy[mqp->port - 1] = NULL;
+		mutex_unlock(&dev->qp1_proxy_lock[mqp->port - 1]);
+	}
+
 	pd = get_pd(mqp);
 	destroy_qp_common(dev, mqp, !!pd->ibpd.uobject);
 
@@ -1646,6 +1671,8 @@ static int __mlx4_ib_modify_qp(struct ib_qp *ibqp,
 				err = handle_eth_ud_smac_index(dev, qp, (u8 *)attr->smac, context);
 				if (err)
 					return -EINVAL;
+				if (qp->mlx4_ib_qp_type == MLX4_IB_QPT_PROXY_GSI)
+					dev->qp1_proxy[qp->port - 1] = qp;
 			}
 		}
 	}
@@ -1930,6 +1957,19 @@ out:
 	return err;
 }
 
+static int vf_get_qp0_qkey(struct mlx4_dev *dev, int qpn, u32 *qkey)
+{
+	int i;
+	for (i = 0; i < dev->caps.num_ports; i++) {
+		if (qpn == dev->caps.qp0_proxy[i] ||
+		    qpn == dev->caps.qp0_tunnel[i]) {
+			*qkey = dev->caps.qp0_qkey[i];
+			return 0;
+		}
+	}
+	return -EINVAL;
+}
+
 static int build_sriov_qp0_header(struct mlx4_ib_sqp *sqp,
 				  struct ib_send_wr *wr,
 				  void *wqe, unsigned *mlx_seg_len)
@@ -1987,8 +2027,13 @@ static int build_sriov_qp0_header(struct mlx4_ib_sqp *sqp,
 			cpu_to_be32(mdev->dev->caps.qp0_tunnel[sqp->qp.port - 1]);
 
 	sqp->ud_header.bth.psn = cpu_to_be32((sqp->send_psn++) & ((1 << 24) - 1));
-	if (mlx4_get_parav_qkey(mdev->dev, sqp->qp.mqp.qpn, &qkey))
-		return -EINVAL;
+	if (mlx4_is_master(mdev->dev)) {
+		if (mlx4_get_parav_qkey(mdev->dev, sqp->qp.mqp.qpn, &qkey))
+			return -EINVAL;
+	} else {
+		if (vf_get_qp0_qkey(mdev->dev, sqp->qp.mqp.qpn, &qkey))
+			return -EINVAL;
+	}
 	sqp->ud_header.deth.qkey = cpu_to_be32(qkey);
 	sqp->ud_header.deth.source_qpn = cpu_to_be32(sqp->qp.mqp.qpn);
 
@@ -2370,7 +2415,8 @@ static void set_datagram_seg(struct mlx4_wqe_datagram_seg *dseg,
 
 static void set_tunnel_datagram_seg(struct mlx4_ib_dev *dev,
 				    struct mlx4_wqe_datagram_seg *dseg,
-				    struct ib_send_wr *wr, enum ib_qp_type qpt)
+				    struct ib_send_wr *wr,
+				    enum mlx4_ib_qp_type qpt)
 {
 	union mlx4_ext_av *av = &to_mah(wr->wr.ud.ah)->av;
 	struct mlx4_av sqp_av = {0};
@@ -2383,8 +2429,10 @@ static void set_tunnel_datagram_seg(struct mlx4_ib_dev *dev,
 			cpu_to_be32(0xf0000000);
 
 	memcpy(dseg->av, &sqp_av, sizeof (struct mlx4_av));
-	/* This function used only for sending on QP1 proxies */
-	dseg->dqpn = cpu_to_be32(dev->dev->caps.qp1_tunnel[port - 1]);
+	if (qpt == MLX4_IB_QPT_PROXY_GSI)
+		dseg->dqpn = cpu_to_be32(dev->dev->caps.qp1_tunnel[port - 1]);
+	else
+		dseg->dqpn = cpu_to_be32(dev->dev->caps.qp0_tunnel[port - 1]);
 	/* Use QKEY from the QP context, which is set by master */
 	dseg->qkey = cpu_to_be32(IB_QP_SET_QKEY);
 }
@@ -2679,11 +2727,6 @@ int mlx4_ib_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
 			break;
 
 		case MLX4_IB_QPT_PROXY_SMI_OWNER:
-			if (unlikely(!mlx4_is_master(to_mdev(ibqp->device)->dev))) {
-				err = -ENOSYS;
-				*bad_wr = wr;
-				goto out;
-			}
 			err = build_sriov_qp0_header(to_msqp(qp), wr, ctrl, &seglen);
 			if (unlikely(err)) {
 				*bad_wr = wr;
@@ -2700,16 +2743,13 @@ int mlx4_ib_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
 			size += seglen / 16;
 			break;
 		case MLX4_IB_QPT_PROXY_SMI:
-			/* don't allow QP0 sends on guests */
-			err = -ENOSYS;
-			*bad_wr = wr;
-			goto out;
 		case MLX4_IB_QPT_PROXY_GSI:
 			/* If we are tunneling special qps, this is a UD qp.
 			 * In this case we first add a UD segment targeting
 			 * the tunnel qp, and then add a header with address
 			 * information */
-			set_tunnel_datagram_seg(to_mdev(ibqp->device), wqe, wr, ibqp->qp_type);
+			set_tunnel_datagram_seg(to_mdev(ibqp->device), wqe, wr,
+						qp->mlx4_ib_qp_type);
 			wqe  += sizeof (struct mlx4_wqe_datagram_seg);
 			size += sizeof (struct mlx4_wqe_datagram_seg) / 16;
 			build_tunnel_header(wr, wqe, &seglen);
diff --git a/drivers/infiniband/hw/mlx4/srq.c b/drivers/infiniband/hw/mlx4/srq.c
index 60c5fb025fc..62d9285300a 100644
--- a/drivers/infiniband/hw/mlx4/srq.c
+++ b/drivers/infiniband/hw/mlx4/srq.c
@@ -134,13 +134,14 @@ struct ib_srq *mlx4_ib_create_srq(struct ib_pd *pd,
 		if (err)
 			goto err_mtt;
 	} else {
-		err = mlx4_db_alloc(dev->dev, &srq->db, 0);
+		err = mlx4_db_alloc(dev->dev, &srq->db, 0, GFP_KERNEL);
 		if (err)
 			goto err_srq;
 
 		*srq->db.db = 0;
 
-		if (mlx4_buf_alloc(dev->dev, buf_size, PAGE_SIZE * 2, &srq->buf)) {
+		if (mlx4_buf_alloc(dev->dev, buf_size, PAGE_SIZE * 2, &srq->buf,
+				   GFP_KERNEL)) {
 			err = -ENOMEM;
 			goto err_db;
 		}
@@ -165,7 +166,7 @@ struct ib_srq *mlx4_ib_create_srq(struct ib_pd *pd,
 		if (err)
 			goto err_buf;
 
-		err = mlx4_buf_write_mtt(dev->dev, &srq->mtt, &srq->buf);
+		err = mlx4_buf_write_mtt(dev->dev, &srq->mtt, &srq->buf, GFP_KERNEL);
 		if (err)
 			goto err_mtt;
 
diff --git a/drivers/infiniband/hw/mlx4/sysfs.c b/drivers/infiniband/hw/mlx4/sysfs.c
index 5a38e43eca6..cb4c66e723b 100644
--- a/drivers/infiniband/hw/mlx4/sysfs.c
+++ b/drivers/infiniband/hw/mlx4/sysfs.c
@@ -389,8 +389,10 @@ struct mlx4_port {
 	struct mlx4_ib_dev    *dev;
 	struct attribute_group pkey_group;
 	struct attribute_group gid_group;
-	u8                     port_num;
+	struct device_attribute	enable_smi_admin;
+	struct device_attribute	smi_enabled;
 	int		       slave;
+	u8                     port_num;
 };
 
 
@@ -558,6 +560,101 @@ err:
 	return NULL;
 }
 
+static ssize_t sysfs_show_smi_enabled(struct device *dev,
+				      struct device_attribute *attr, char *buf)
+{
+	struct mlx4_port *p =
+		container_of(attr, struct mlx4_port, smi_enabled);
+	ssize_t len = 0;
+
+	if (mlx4_vf_smi_enabled(p->dev->dev, p->slave, p->port_num))
+		len = sprintf(buf, "%d\n", 1);
+	else
+		len = sprintf(buf, "%d\n", 0);
+
+	return len;
+}
+
+static ssize_t sysfs_show_enable_smi_admin(struct device *dev,
+					   struct device_attribute *attr,
+					   char *buf)
+{
+	struct mlx4_port *p =
+		container_of(attr, struct mlx4_port, enable_smi_admin);
+	ssize_t len = 0;
+
+	if (mlx4_vf_get_enable_smi_admin(p->dev->dev, p->slave, p->port_num))
+		len = sprintf(buf, "%d\n", 1);
+	else
+		len = sprintf(buf, "%d\n", 0);
+
+	return len;
+}
+
+static ssize_t sysfs_store_enable_smi_admin(struct device *dev,
+					    struct device_attribute *attr,
+					    const char *buf, size_t count)
+{
+	struct mlx4_port *p =
+		container_of(attr, struct mlx4_port, enable_smi_admin);
+	int enable;
+
+	if (sscanf(buf, "%i", &enable) != 1 ||
+	    enable < 0 || enable > 1)
+		return -EINVAL;
+
+	if (mlx4_vf_set_enable_smi_admin(p->dev->dev, p->slave, p->port_num, enable))
+		return -EINVAL;
+	return count;
+}
+
+static int add_vf_smi_entries(struct mlx4_port *p)
+{
+	int is_eth = rdma_port_get_link_layer(&p->dev->ib_dev, p->port_num) ==
+			IB_LINK_LAYER_ETHERNET;
+	int ret;
+
+	/* do not display entries if eth transport, or if master */
+	if (is_eth || p->slave == mlx4_master_func_num(p->dev->dev))
+		return 0;
+
+	sysfs_attr_init(&p->smi_enabled.attr);
+	p->smi_enabled.show = sysfs_show_smi_enabled;
+	p->smi_enabled.store = NULL;
+	p->smi_enabled.attr.name = "smi_enabled";
+	p->smi_enabled.attr.mode = 0444;
+	ret = sysfs_create_file(&p->kobj, &p->smi_enabled.attr);
+	if (ret) {
+		pr_err("failed to create smi_enabled\n");
+		return ret;
+	}
+
+	sysfs_attr_init(&p->enable_smi_admin.attr);
+	p->enable_smi_admin.show = sysfs_show_enable_smi_admin;
+	p->enable_smi_admin.store = sysfs_store_enable_smi_admin;
+	p->enable_smi_admin.attr.name = "enable_smi_admin";
+	p->enable_smi_admin.attr.mode = 0644;
+	ret = sysfs_create_file(&p->kobj, &p->enable_smi_admin.attr);
+	if (ret) {
+		pr_err("failed to create enable_smi_admin\n");
+		sysfs_remove_file(&p->kobj, &p->smi_enabled.attr);
+		return ret;
+	}
+	return 0;
+}
+
+static void remove_vf_smi_entries(struct mlx4_port *p)
+{
+	int is_eth = rdma_port_get_link_layer(&p->dev->ib_dev, p->port_num) ==
+			IB_LINK_LAYER_ETHERNET;
+
+	if (is_eth || p->slave == mlx4_master_func_num(p->dev->dev))
+		return;
+
+	sysfs_remove_file(&p->kobj, &p->smi_enabled.attr);
+	sysfs_remove_file(&p->kobj, &p->enable_smi_admin.attr);
+}
+
 static int add_port(struct mlx4_ib_dev *dev, int port_num, int slave)
 {
 	struct mlx4_port *p;
@@ -602,6 +699,10 @@ static int add_port(struct mlx4_ib_dev *dev, int port_num, int slave)
 	if (ret)
 		goto err_free_gid;
 
+	ret = add_vf_smi_entries(p);
+	if (ret)
+		goto err_free_gid;
+
 	list_add_tail(&p->kobj.entry, &dev->pkeys.pkey_port_list[slave]);
 	return 0;
 
@@ -669,6 +770,7 @@ err_add:
 		mport = container_of(p, struct mlx4_port, kobj);
 		sysfs_remove_group(p, &mport->pkey_group);
 		sysfs_remove_group(p, &mport->gid_group);
+		remove_vf_smi_entries(mport);
 		kobject_put(p);
 	}
 	kobject_put(dev->dev_ports_parent[slave]);
@@ -713,6 +815,7 @@ static void unregister_pkey_tree(struct mlx4_ib_dev *device)
 			port = container_of(p, struct mlx4_port, kobj);
 			sysfs_remove_group(p, &port->pkey_group);
 			sysfs_remove_group(p, &port->gid_group);
+			remove_vf_smi_entries(port);
 			kobject_put(p);
 			kobject_put(device->dev_ports_parent[slave]);
 		}
diff --git a/drivers/infiniband/hw/mlx5/cq.c b/drivers/infiniband/hw/mlx5/cq.c
index 62bb6b49dc1..8ae4f896cb4 100644
--- a/drivers/infiniband/hw/mlx5/cq.c
+++ b/drivers/infiniband/hw/mlx5/cq.c
@@ -32,6 +32,7 @@
 
 #include <linux/kref.h>
 #include <rdma/ib_umem.h>
+#include <rdma/ib_user_verbs.h>
 #include "mlx5_ib.h"
 #include "user.h"
 
@@ -602,14 +603,24 @@ static int create_cq_user(struct mlx5_ib_dev *dev, struct ib_udata *udata,
 			  int *cqe_size, int *index, int *inlen)
 {
 	struct mlx5_ib_create_cq ucmd;
+	size_t ucmdlen;
 	int page_shift;
 	int npages;
 	int ncont;
 	int err;
 
-	if (ib_copy_from_udata(&ucmd, udata, sizeof(ucmd)))
+	ucmdlen =
+		(udata->inlen - sizeof(struct ib_uverbs_cmd_hdr) <
+		 sizeof(ucmd)) ? (sizeof(ucmd) -
+				  sizeof(ucmd.reserved)) : sizeof(ucmd);
+
+	if (ib_copy_from_udata(&ucmd, udata, ucmdlen))
 		return -EFAULT;
 
+	if (ucmdlen == sizeof(ucmd) &&
+	    ucmd.reserved != 0)
+		return -EINVAL;
+
 	if (ucmd.cqe_size != 64 && ucmd.cqe_size != 128)
 		return -EINVAL;
 
diff --git a/drivers/infiniband/hw/mlx5/mlx5_ib.h b/drivers/infiniband/hw/mlx5/mlx5_ib.h
index 50541586e0a..f2ccf1a5a29 100644
--- a/drivers/infiniband/hw/mlx5/mlx5_ib.h
+++ b/drivers/infiniband/hw/mlx5/mlx5_ib.h
@@ -264,8 +264,6 @@ struct mlx5_ib_mr {
 	__be64			*pas;
 	dma_addr_t		dma;
 	int			npages;
-	struct completion	done;
-	enum ib_wc_status	status;
 	struct mlx5_ib_dev     *dev;
 	struct mlx5_create_mkey_mbox_out out;
 	struct mlx5_core_sig_ctx    *sig;
@@ -277,6 +275,17 @@ struct mlx5_ib_fast_reg_page_list {
 	dma_addr_t			map;
 };
 
+struct mlx5_ib_umr_context {
+	enum ib_wc_status	status;
+	struct completion	done;
+};
+
+static inline void mlx5_ib_init_umr_context(struct mlx5_ib_umr_context *context)
+{
+	context->status = -1;
+	init_completion(&context->done);
+}
+
 struct umr_common {
 	struct ib_pd	*pd;
 	struct ib_cq	*cq;
diff --git a/drivers/infiniband/hw/mlx5/mr.c b/drivers/infiniband/hw/mlx5/mr.c
index 81392b26d07..afa873bd028 100644
--- a/drivers/infiniband/hw/mlx5/mr.c
+++ b/drivers/infiniband/hw/mlx5/mr.c
@@ -73,6 +73,8 @@ static void reg_mr_callback(int status, void *context)
 	struct mlx5_cache_ent *ent = &cache->ent[c];
 	u8 key;
 	unsigned long flags;
+	struct mlx5_mr_table *table = &dev->mdev.priv.mr_table;
+	int err;
 
 	spin_lock_irqsave(&ent->lock, flags);
 	ent->pending--;
@@ -107,6 +109,13 @@ static void reg_mr_callback(int status, void *context)
 	ent->cur++;
 	ent->size++;
 	spin_unlock_irqrestore(&ent->lock, flags);
+
+	write_lock_irqsave(&table->lock, flags);
+	err = radix_tree_insert(&table->tree, mlx5_base_mkey(mr->mmr.key),
+				&mr->mmr);
+	if (err)
+		pr_err("Error inserting to mr tree. 0x%x\n", -err);
+	write_unlock_irqrestore(&table->lock, flags);
 }
 
 static int add_keys(struct mlx5_ib_dev *dev, int c, int num)
@@ -699,7 +708,7 @@ static void prep_umr_unreg_wqe(struct mlx5_ib_dev *dev,
 
 void mlx5_umr_cq_handler(struct ib_cq *cq, void *cq_context)
 {
-	struct mlx5_ib_mr *mr;
+	struct mlx5_ib_umr_context *context;
 	struct ib_wc wc;
 	int err;
 
@@ -712,9 +721,9 @@ void mlx5_umr_cq_handler(struct ib_cq *cq, void *cq_context)
 		if (err == 0)
 			break;
 
-		mr = (struct mlx5_ib_mr *)(unsigned long)wc.wr_id;
-		mr->status = wc.status;
-		complete(&mr->done);
+		context = (struct mlx5_ib_umr_context *) (unsigned long) wc.wr_id;
+		context->status = wc.status;
+		complete(&context->done);
 	}
 	ib_req_notify_cq(cq, IB_CQ_NEXT_COMP);
 }
@@ -726,11 +735,12 @@ static struct mlx5_ib_mr *reg_umr(struct ib_pd *pd, struct ib_umem *umem,
 	struct mlx5_ib_dev *dev = to_mdev(pd->device);
 	struct device *ddev = dev->ib_dev.dma_device;
 	struct umr_common *umrc = &dev->umrc;
+	struct mlx5_ib_umr_context umr_context;
 	struct ib_send_wr wr, *bad;
 	struct mlx5_ib_mr *mr;
 	struct ib_sge sg;
 	int size = sizeof(u64) * npages;
-	int err;
+	int err = 0;
 	int i;
 
 	for (i = 0; i < 1; i++) {
@@ -751,7 +761,7 @@ static struct mlx5_ib_mr *reg_umr(struct ib_pd *pd, struct ib_umem *umem,
 	mr->pas = kmalloc(size + MLX5_UMR_ALIGN - 1, GFP_KERNEL);
 	if (!mr->pas) {
 		err = -ENOMEM;
-		goto error;
+		goto free_mr;
 	}
 
 	mlx5_ib_populate_pas(dev, umem, page_shift,
@@ -760,44 +770,46 @@ static struct mlx5_ib_mr *reg_umr(struct ib_pd *pd, struct ib_umem *umem,
 	mr->dma = dma_map_single(ddev, mr_align(mr->pas, MLX5_UMR_ALIGN), size,
 				 DMA_TO_DEVICE);
 	if (dma_mapping_error(ddev, mr->dma)) {
-		kfree(mr->pas);
 		err = -ENOMEM;
-		goto error;
+		goto free_pas;
 	}
 
 	memset(&wr, 0, sizeof(wr));
-	wr.wr_id = (u64)(unsigned long)mr;
+	wr.wr_id = (u64)(unsigned long)&umr_context;
 	prep_umr_reg_wqe(pd, &wr, &sg, mr->dma, npages, mr->mmr.key, page_shift, virt_addr, len, access_flags);
 
-	/* We serialize polls so one process does not kidnap another's
-	 * completion. This is not a problem since wr is completed in
-	 * around 1 usec
-	 */
+	mlx5_ib_init_umr_context(&umr_context);
 	down(&umrc->sem);
-	init_completion(&mr->done);
 	err = ib_post_send(umrc->qp, &wr, &bad);
 	if (err) {
 		mlx5_ib_warn(dev, "post send failed, err %d\n", err);
-		up(&umrc->sem);
-		goto error;
+		goto unmap_dma;
+	} else {
+		wait_for_completion(&umr_context.done);
+		if (umr_context.status != IB_WC_SUCCESS) {
+			mlx5_ib_warn(dev, "reg umr failed\n");
+			err = -EFAULT;
+		}
 	}
-	wait_for_completion(&mr->done);
-	up(&umrc->sem);
 
+	mr->mmr.iova = virt_addr;
+	mr->mmr.size = len;
+	mr->mmr.pd = to_mpd(pd)->pdn;
+
+unmap_dma:
+	up(&umrc->sem);
 	dma_unmap_single(ddev, mr->dma, size, DMA_TO_DEVICE);
+
+free_pas:
 	kfree(mr->pas);
 
-	if (mr->status != IB_WC_SUCCESS) {
-		mlx5_ib_warn(dev, "reg umr failed\n");
-		err = -EFAULT;
-		goto error;
+free_mr:
+	if (err) {
+		free_cached_mr(dev, mr);
+		return ERR_PTR(err);
 	}
 
 	return mr;
-
-error:
-	free_cached_mr(dev, mr);
-	return ERR_PTR(err);
 }
 
 static struct mlx5_ib_mr *reg_create(struct ib_pd *pd, u64 virt_addr,
@@ -926,24 +938,26 @@ error:
 static int unreg_umr(struct mlx5_ib_dev *dev, struct mlx5_ib_mr *mr)
 {
 	struct umr_common *umrc = &dev->umrc;
+	struct mlx5_ib_umr_context umr_context;
 	struct ib_send_wr wr, *bad;
 	int err;
 
 	memset(&wr, 0, sizeof(wr));
-	wr.wr_id = (u64)(unsigned long)mr;
+	wr.wr_id = (u64)(unsigned long)&umr_context;
 	prep_umr_unreg_wqe(dev, &wr, mr->mmr.key);
 
+	mlx5_ib_init_umr_context(&umr_context);
 	down(&umrc->sem);
-	init_completion(&mr->done);
 	err = ib_post_send(umrc->qp, &wr, &bad);
 	if (err) {
 		up(&umrc->sem);
 		mlx5_ib_dbg(dev, "err %d\n", err);
 		goto error;
+	} else {
+		wait_for_completion(&umr_context.done);
+		up(&umrc->sem);
 	}
-	wait_for_completion(&mr->done);
-	up(&umrc->sem);
-	if (mr->status != IB_WC_SUCCESS) {
+	if (umr_context.status != IB_WC_SUCCESS) {
 		mlx5_ib_warn(dev, "unreg umr failed\n");
 		err = -EFAULT;
 		goto error;
diff --git a/drivers/infiniband/hw/mlx5/qp.c b/drivers/infiniband/hw/mlx5/qp.c
index dc930ed21ec..d13ddf1c003 100644
--- a/drivers/infiniband/hw/mlx5/qp.c
+++ b/drivers/infiniband/hw/mlx5/qp.c
@@ -574,6 +574,10 @@ static int create_user_qp(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 	uar_index = uuarn_to_uar_index(&context->uuari, uuarn);
 	mlx5_ib_dbg(dev, "uuarn 0x%x, uar_index 0x%x\n", uuarn, uar_index);
 
+	qp->rq.offset = 0;
+	qp->sq.wqe_shift = ilog2(MLX5_SEND_WQE_BB);
+	qp->sq.offset = qp->rq.wqe_cnt << qp->rq.wqe_shift;
+
 	err = set_user_buf_size(dev, qp, &ucmd);
 	if (err)
 		goto err_uuar;
@@ -2078,6 +2082,7 @@ static int mlx5_set_bsf(struct ib_mr *sig_mr,
 	struct ib_sig_domain *wire = &sig_attrs->wire;
 	int ret, selector;
 
+	memset(bsf, 0, sizeof(*bsf));
 	switch (sig_attrs->mem.sig_type) {
 	case IB_SIG_TYPE_T10_DIF:
 		if (sig_attrs->wire.sig_type != IB_SIG_TYPE_T10_DIF)
@@ -2090,9 +2095,11 @@ static int mlx5_set_bsf(struct ib_mr *sig_mr,
 			/* Same block structure */
 			basic->bsf_size_sbs = 1 << 4;
 			if (mem->sig.dif.bg_type == wire->sig.dif.bg_type)
-				basic->wire.copy_byte_mask = 0xff;
-			else
-				basic->wire.copy_byte_mask = 0x3f;
+				basic->wire.copy_byte_mask |= 0xc0;
+			if (mem->sig.dif.app_tag == wire->sig.dif.app_tag)
+				basic->wire.copy_byte_mask |= 0x30;
+			if (mem->sig.dif.ref_tag == wire->sig.dif.ref_tag)
+				basic->wire.copy_byte_mask |= 0x0f;
 		} else
 			basic->wire.bs_selector = bs_selector(wire->sig.dif.pi_interval);
 
@@ -2131,9 +2138,13 @@ static int set_sig_data_segment(struct ib_send_wr *wr, struct mlx5_ib_qp *qp,
 	int ret;
 	int wqe_size;
 
-	if (!wr->wr.sig_handover.prot) {
+	if (!wr->wr.sig_handover.prot ||
+	    (data_key == wr->wr.sig_handover.prot->lkey &&
+	     data_va == wr->wr.sig_handover.prot->addr &&
+	     data_len == wr->wr.sig_handover.prot->length)) {
 		/**
 		 * Source domain doesn't contain signature information
+		 * or data and protection are interleaved in memory.
 		 * So need construct:
 		 *                  ------------------
 		 *                 |     data_klm     |
@@ -2187,23 +2198,13 @@ static int set_sig_data_segment(struct ib_send_wr *wr, struct mlx5_ib_qp *qp,
 		data_sentry->bcount = cpu_to_be16(block_size);
 		data_sentry->key = cpu_to_be32(data_key);
 		data_sentry->va = cpu_to_be64(data_va);
+		data_sentry->stride = cpu_to_be16(block_size);
+
 		prot_sentry->bcount = cpu_to_be16(prot_size);
 		prot_sentry->key = cpu_to_be32(prot_key);
+		prot_sentry->va = cpu_to_be64(prot_va);
+		prot_sentry->stride = cpu_to_be16(prot_size);
 
-		if (prot_key == data_key && prot_va == data_va) {
-			/**
-			 * The data and protection are interleaved
-			 * in a single memory region
-			 **/
-			prot_sentry->va = cpu_to_be64(data_va + block_size);
-			prot_sentry->stride = cpu_to_be16(block_size + prot_size);
-			data_sentry->stride = prot_sentry->stride;
-		} else {
-			/* The data and protection are two different buffers */
-			prot_sentry->va = cpu_to_be64(prot_va);
-			data_sentry->stride = cpu_to_be16(block_size);
-			prot_sentry->stride = cpu_to_be16(prot_size);
-		}
 		wqe_size = ALIGN(sizeof(*sblock_ctrl) + sizeof(*data_sentry) +
 				 sizeof(*prot_sentry), 64);
 	}
@@ -2275,7 +2276,10 @@ static int set_sig_umr_wr(struct ib_send_wr *wr, struct mlx5_ib_qp *qp,
 
 	/* length of the protected region, data + protection */
 	region_len = wr->sg_list->length;
-	if (wr->wr.sig_handover.prot)
+	if (wr->wr.sig_handover.prot &&
+	    (wr->wr.sig_handover.prot->lkey != wr->sg_list->lkey  ||
+	     wr->wr.sig_handover.prot->addr != wr->sg_list->addr  ||
+	     wr->wr.sig_handover.prot->length != wr->sg_list->length))
 		region_len += wr->wr.sig_handover.prot->length;
 
 	/**
diff --git a/drivers/infiniband/hw/mlx5/srq.c b/drivers/infiniband/hw/mlx5/srq.c
index 210b3eaf188..384af6dec5e 100644
--- a/drivers/infiniband/hw/mlx5/srq.c
+++ b/drivers/infiniband/hw/mlx5/srq.c
@@ -35,6 +35,7 @@
 #include <linux/mlx5/srq.h>
 #include <linux/slab.h>
 #include <rdma/ib_umem.h>
+#include <rdma/ib_user_verbs.h>
 
 #include "mlx5_ib.h"
 #include "user.h"
@@ -78,16 +79,27 @@ static int create_srq_user(struct ib_pd *pd, struct mlx5_ib_srq *srq,
 {
 	struct mlx5_ib_dev *dev = to_mdev(pd->device);
 	struct mlx5_ib_create_srq ucmd;
+	size_t ucmdlen;
 	int err;
 	int npages;
 	int page_shift;
 	int ncont;
 	u32 offset;
 
-	if (ib_copy_from_udata(&ucmd, udata, sizeof(ucmd))) {
+	ucmdlen =
+		(udata->inlen - sizeof(struct ib_uverbs_cmd_hdr) <
+		 sizeof(ucmd)) ? (sizeof(ucmd) -
+				  sizeof(ucmd.reserved)) : sizeof(ucmd);
+
+	if (ib_copy_from_udata(&ucmd, udata, ucmdlen)) {
 		mlx5_ib_dbg(dev, "failed copy udata\n");
 		return -EFAULT;
 	}
+
+	if (ucmdlen == sizeof(ucmd) &&
+	    ucmd.reserved != 0)
+		return -EINVAL;
+
 	srq->wq_sig = !!(ucmd.flags & MLX5_SRQ_FLAG_SIGNATURE);
 
 	srq->umem = ib_umem_get(pd->uobject->context, ucmd.buf_addr, buf_size,
diff --git a/drivers/infiniband/hw/mlx5/user.h b/drivers/infiniband/hw/mlx5/user.h
index 0f4f8e42a17..d0ba264ac1e 100644
--- a/drivers/infiniband/hw/mlx5/user.h
+++ b/drivers/infiniband/hw/mlx5/user.h
@@ -91,6 +91,7 @@ struct mlx5_ib_create_cq {
 	__u64	buf_addr;
 	__u64	db_addr;
 	__u32	cqe_size;
+	__u32	reserved; /* explicit padding (optional on i386) */
 };
 
 struct mlx5_ib_create_cq_resp {
@@ -109,6 +110,7 @@ struct mlx5_ib_create_srq {
 	__u64	buf_addr;
 	__u64	db_addr;
 	__u32	flags;
+	__u32	reserved; /* explicit padding (optional on i386) */
 };
 
 struct mlx5_ib_create_srq_resp {
diff --git a/drivers/infiniband/hw/nes/nes.c b/drivers/infiniband/hw/nes/nes.c
index 353c7b05a90..3b2a6dc8ea9 100644
--- a/drivers/infiniband/hw/nes/nes.c
+++ b/drivers/infiniband/hw/nes/nes.c
@@ -68,7 +68,6 @@ MODULE_VERSION(DRV_VERSION);
 int max_mtu = 9000;
 int interrupt_mod_interval = 0;
 
-
 /* Interoperability */
 int mpa_version = 1;
 module_param(mpa_version, int, 0644);
@@ -112,6 +111,16 @@ static struct pci_device_id nes_pci_table[] = {
 
 MODULE_DEVICE_TABLE(pci, nes_pci_table);
 
+/* registered nes netlink callbacks */
+static struct ibnl_client_cbs nes_nl_cb_table[] = {
+	[RDMA_NL_IWPM_REG_PID] = {.dump = iwpm_register_pid_cb},
+	[RDMA_NL_IWPM_ADD_MAPPING] = {.dump = iwpm_add_mapping_cb},
+	[RDMA_NL_IWPM_QUERY_MAPPING] = {.dump = iwpm_add_and_query_mapping_cb},
+	[RDMA_NL_IWPM_HANDLE_ERR] = {.dump = iwpm_mapping_error_cb},
+	[RDMA_NL_IWPM_MAPINFO] = {.dump = iwpm_mapping_info_cb},
+	[RDMA_NL_IWPM_MAPINFO_NUM] = {.dump = iwpm_ack_mapping_info_cb}
+};
+
 static int nes_inetaddr_event(struct notifier_block *, unsigned long, void *);
 static int nes_net_event(struct notifier_block *, unsigned long, void *);
 static int nes_notifiers_registered;
@@ -672,6 +681,17 @@ static int nes_probe(struct pci_dev *pcidev, const struct pci_device_id *ent)
 	}
 	nes_notifiers_registered++;
 
+	if (ibnl_add_client(RDMA_NL_NES, RDMA_NL_IWPM_NUM_OPS, nes_nl_cb_table))
+		printk(KERN_ERR PFX "%s[%u]: Failed to add netlink callback\n",
+			__func__, __LINE__);
+
+	ret = iwpm_init(RDMA_NL_NES);
+	if (ret) {
+		printk(KERN_ERR PFX "%s: port mapper initialization failed\n",
+				pci_name(pcidev));
+		goto bail7;
+	}
+
 	INIT_DELAYED_WORK(&nesdev->work, nes_recheck_link_status);
 
 	/* Initialize network devices */
@@ -710,6 +730,7 @@ static int nes_probe(struct pci_dev *pcidev, const struct pci_device_id *ent)
 
 	nes_debug(NES_DBG_INIT, "netdev_count=%d, nesadapter->netdev_count=%d\n",
 			nesdev->netdev_count, nesdev->nesadapter->netdev_count);
+	ibnl_remove_client(RDMA_NL_NES);
 
 	nes_notifiers_registered--;
 	if (nes_notifiers_registered == 0) {
@@ -773,6 +794,8 @@ static void nes_remove(struct pci_dev *pcidev)
 				nesdev->nesadapter->netdev_count--;
 			}
 		}
+	ibnl_remove_client(RDMA_NL_NES);
+	iwpm_exit(RDMA_NL_NES);
 
 	nes_notifiers_registered--;
 	if (nes_notifiers_registered == 0) {
diff --git a/drivers/infiniband/hw/nes/nes.h b/drivers/infiniband/hw/nes/nes.h
index 33cc58941a3..bd9d132f11c 100644
--- a/drivers/infiniband/hw/nes/nes.h
+++ b/drivers/infiniband/hw/nes/nes.h
@@ -51,6 +51,8 @@
 #include <rdma/ib_pack.h>
 #include <rdma/rdma_cm.h>
 #include <rdma/iw_cm.h>
+#include <rdma/rdma_netlink.h>
+#include <rdma/iw_portmap.h>
 
 #define NES_SEND_FIRST_WRITE
 
@@ -130,6 +132,7 @@
 #define NES_DBG_IW_TX       0x00040000
 #define NES_DBG_SHUTDOWN    0x00080000
 #define NES_DBG_PAU         0x00100000
+#define NES_DBG_NLMSG       0x00200000
 #define NES_DBG_RSVD1       0x10000000
 #define NES_DBG_RSVD2       0x20000000
 #define NES_DBG_RSVD3       0x40000000
diff --git a/drivers/infiniband/hw/nes/nes_cm.c b/drivers/infiniband/hw/nes/nes_cm.c
index dfa9df48450..6f09a72e78d 100644
--- a/drivers/infiniband/hw/nes/nes_cm.c
+++ b/drivers/infiniband/hw/nes/nes_cm.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2006 - 2011 Intel Corporation.  All rights reserved.
+ * Copyright (c) 2006 - 2014 Intel Corporation.  All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -59,6 +59,7 @@
 #include <net/route.h>
 #include <net/ip_fib.h>
 #include <net/tcp.h>
+#include <linux/fcntl.h>
 
 #include "nes.h"
 
@@ -166,7 +167,6 @@ int nes_rem_ref_cm_node(struct nes_cm_node *cm_node)
 {
 	return rem_ref_cm_node(cm_node->cm_core, cm_node);
 }
-
 /**
  * create_event
  */
@@ -482,11 +482,11 @@ static void form_cm_frame(struct sk_buff *skb,
 	iph->ttl = 0x40;
 	iph->protocol = 0x06;   /* IPPROTO_TCP */
 
-	iph->saddr = htonl(cm_node->loc_addr);
-	iph->daddr = htonl(cm_node->rem_addr);
+	iph->saddr = htonl(cm_node->mapped_loc_addr);
+	iph->daddr = htonl(cm_node->mapped_rem_addr);
 
-	tcph->source = htons(cm_node->loc_port);
-	tcph->dest = htons(cm_node->rem_port);
+	tcph->source = htons(cm_node->mapped_loc_port);
+	tcph->dest = htons(cm_node->mapped_rem_port);
 	tcph->seq = htonl(cm_node->tcp_cntxt.loc_seq_num);
 
 	if (flags & SET_ACK) {
@@ -525,6 +525,100 @@ static void form_cm_frame(struct sk_buff *skb,
 	cm_packets_created++;
 }
 
+/*
+ * nes_create_sockaddr - Record ip addr and tcp port in a sockaddr struct
+ */
+static void nes_create_sockaddr(__be32 ip_addr, __be16 port,
+				struct sockaddr_storage *addr)
+{
+	struct sockaddr_in *nes_sockaddr = (struct sockaddr_in *)addr;
+	nes_sockaddr->sin_family = AF_INET;
+	memcpy(&nes_sockaddr->sin_addr.s_addr, &ip_addr, sizeof(__be32));
+	nes_sockaddr->sin_port = port;
+}
+
+/*
+ * nes_create_mapinfo - Create a mapinfo object in the port mapper data base
+ */
+static int nes_create_mapinfo(struct nes_cm_info *cm_info)
+{
+	struct sockaddr_storage local_sockaddr;
+	struct sockaddr_storage mapped_sockaddr;
+
+	nes_create_sockaddr(htonl(cm_info->loc_addr), htons(cm_info->loc_port),
+				&local_sockaddr);
+	nes_create_sockaddr(htonl(cm_info->mapped_loc_addr),
+			htons(cm_info->mapped_loc_port), &mapped_sockaddr);
+
+	return iwpm_create_mapinfo(&local_sockaddr,
+				&mapped_sockaddr, RDMA_NL_NES);
+}
+
+/*
+ * nes_remove_mapinfo - Remove a mapinfo object from the port mapper data base
+ *                      and send a remove mapping op message to
+ *                      the userspace port mapper
+ */
+static int nes_remove_mapinfo(u32 loc_addr, u16 loc_port,
+			u32 mapped_loc_addr, u16 mapped_loc_port)
+{
+	struct sockaddr_storage local_sockaddr;
+	struct sockaddr_storage mapped_sockaddr;
+
+	nes_create_sockaddr(htonl(loc_addr), htons(loc_port), &local_sockaddr);
+	nes_create_sockaddr(htonl(mapped_loc_addr), htons(mapped_loc_port),
+				&mapped_sockaddr);
+
+	iwpm_remove_mapinfo(&local_sockaddr, &mapped_sockaddr);
+	return iwpm_remove_mapping(&local_sockaddr, RDMA_NL_NES);
+}
+
+/*
+ * nes_form_pm_msg - Form a port mapper message with mapping info
+ */
+static void nes_form_pm_msg(struct nes_cm_info *cm_info,
+				struct iwpm_sa_data *pm_msg)
+{
+	nes_create_sockaddr(htonl(cm_info->loc_addr), htons(cm_info->loc_port),
+				&pm_msg->loc_addr);
+	nes_create_sockaddr(htonl(cm_info->rem_addr), htons(cm_info->rem_port),
+				&pm_msg->rem_addr);
+}
+
+/*
+ * nes_form_reg_msg - Form a port mapper message with dev info
+ */
+static void nes_form_reg_msg(struct nes_vnic *nesvnic,
+			struct iwpm_dev_data *pm_msg)
+{
+	memcpy(pm_msg->dev_name, nesvnic->nesibdev->ibdev.name,
+				IWPM_DEVNAME_SIZE);
+	memcpy(pm_msg->if_name, nesvnic->netdev->name, IWPM_IFNAME_SIZE);
+}
+
+/*
+ * nes_record_pm_msg - Save the received mapping info
+ */
+static void nes_record_pm_msg(struct nes_cm_info *cm_info,
+			struct iwpm_sa_data *pm_msg)
+{
+	struct sockaddr_in *mapped_loc_addr =
+			(struct sockaddr_in *)&pm_msg->mapped_loc_addr;
+	struct sockaddr_in *mapped_rem_addr =
+			(struct sockaddr_in *)&pm_msg->mapped_rem_addr;
+
+	if (mapped_loc_addr->sin_family == AF_INET) {
+		cm_info->mapped_loc_addr =
+			ntohl(mapped_loc_addr->sin_addr.s_addr);
+		cm_info->mapped_loc_port = ntohs(mapped_loc_addr->sin_port);
+	}
+	if (mapped_rem_addr->sin_family == AF_INET) {
+		cm_info->mapped_rem_addr =
+			ntohl(mapped_rem_addr->sin_addr.s_addr);
+		cm_info->mapped_rem_port = ntohs(mapped_rem_addr->sin_port);
+	}
+}
+
 /**
  * print_core - dump a cm core
  */
@@ -1147,8 +1241,11 @@ static struct nes_cm_node *find_node(struct nes_cm_core *cm_core,
 			  loc_addr, loc_port,
 			  cm_node->rem_addr, cm_node->rem_port,
 			  rem_addr, rem_port);
-		if ((cm_node->loc_addr == loc_addr) && (cm_node->loc_port == loc_port) &&
-		    (cm_node->rem_addr == rem_addr) && (cm_node->rem_port == rem_port)) {
+		if ((cm_node->mapped_loc_addr == loc_addr) &&
+			(cm_node->mapped_loc_port == loc_port) &&
+			(cm_node->mapped_rem_addr == rem_addr) &&
+			(cm_node->mapped_rem_port == rem_port)) {
+
 			add_ref_cm_node(cm_node);
 			spin_unlock_irqrestore(&cm_core->ht_lock, flags);
 			return cm_node;
@@ -1165,18 +1262,28 @@ static struct nes_cm_node *find_node(struct nes_cm_core *cm_core,
  * find_listener - find a cm node listening on this addr-port pair
  */
 static struct nes_cm_listener *find_listener(struct nes_cm_core *cm_core,
-					     nes_addr_t dst_addr, u16 dst_port, enum nes_cm_listener_state listener_state)
+					nes_addr_t dst_addr, u16 dst_port,
+					enum nes_cm_listener_state listener_state, int local)
 {
 	unsigned long flags;
 	struct nes_cm_listener *listen_node;
+	nes_addr_t listen_addr;
+	u16 listen_port;
 
 	/* walk list and find cm_node associated with this session ID */
 	spin_lock_irqsave(&cm_core->listen_list_lock, flags);
 	list_for_each_entry(listen_node, &cm_core->listen_list.list, list) {
+		if (local) {
+			listen_addr = listen_node->loc_addr;
+			listen_port = listen_node->loc_port;
+		} else {
+			listen_addr = listen_node->mapped_loc_addr;
+			listen_port = listen_node->mapped_loc_port;
+		}
 		/* compare node pair, return node handle if a match */
-		if (((listen_node->loc_addr == dst_addr) ||
-		     listen_node->loc_addr == 0x00000000) &&
-		    (listen_node->loc_port == dst_port) &&
+		if (((listen_addr == dst_addr) ||
+		     listen_addr == 0x00000000) &&
+		    (listen_port == dst_port) &&
 		    (listener_state & listen_node->listener_state)) {
 			atomic_inc(&listen_node->ref_count);
 			spin_unlock_irqrestore(&cm_core->listen_list_lock, flags);
@@ -1189,7 +1296,6 @@ static struct nes_cm_listener *find_listener(struct nes_cm_core *cm_core,
 	return NULL;
 }
 
-
 /**
  * add_hte_node - add a cm node to the hash table
  */
@@ -1310,9 +1416,20 @@ static int mini_cm_dec_refcnt_listen(struct nes_cm_core *cm_core,
 
 		spin_unlock_irqrestore(&cm_core->listen_list_lock, flags);
 
-		if (listener->nesvnic)
-			nes_manage_apbvt(listener->nesvnic, listener->loc_port,
-					 PCI_FUNC(listener->nesvnic->nesdev->pcidev->devfn), NES_MANAGE_APBVT_DEL);
+		if (listener->nesvnic) {
+			nes_manage_apbvt(listener->nesvnic,
+				listener->mapped_loc_port,
+				PCI_FUNC(listener->nesvnic->nesdev->pcidev->devfn),
+				NES_MANAGE_APBVT_DEL);
+
+			nes_remove_mapinfo(listener->loc_addr,
+					listener->loc_port,
+					listener->mapped_loc_addr,
+					listener->mapped_loc_port);
+			nes_debug(NES_DBG_NLMSG,
+					"Delete APBVT mapped_loc_port = %04X\n",
+					listener->mapped_loc_port);
+		}
 
 		nes_debug(NES_DBG_CM, "destroying listener (%p)\n", listener);
 
@@ -1454,6 +1571,11 @@ static struct nes_cm_node *make_cm_node(struct nes_cm_core *cm_core,
 	cm_node->loc_port = cm_info->loc_port;
 	cm_node->rem_port = cm_info->rem_port;
 
+	cm_node->mapped_loc_addr = cm_info->mapped_loc_addr;
+	cm_node->mapped_rem_addr = cm_info->mapped_rem_addr;
+	cm_node->mapped_loc_port = cm_info->mapped_loc_port;
+	cm_node->mapped_rem_port = cm_info->mapped_rem_port;
+
 	cm_node->mpa_frame_rev = mpa_version;
 	cm_node->send_rdma0_op = SEND_RDMA_READ_ZERO;
 	cm_node->mpav2_ird_ord = 0;
@@ -1500,8 +1622,10 @@ static struct nes_cm_node *make_cm_node(struct nes_cm_core *cm_core,
 	cm_node->loopbackpartner = NULL;
 
 	/* get the mac addr for the remote node */
-	oldarpindex = nes_arp_table(nesdev, cm_node->rem_addr, NULL, NES_ARP_RESOLVE);
-	arpindex = nes_addr_resolve_neigh(nesvnic, cm_info->rem_addr, oldarpindex);
+	oldarpindex = nes_arp_table(nesdev, cm_node->mapped_rem_addr,
+				NULL, NES_ARP_RESOLVE);
+	arpindex = nes_addr_resolve_neigh(nesvnic,
+				cm_node->mapped_rem_addr, oldarpindex);
 	if (arpindex < 0) {
 		kfree(cm_node);
 		return NULL;
@@ -1563,11 +1687,14 @@ static int rem_ref_cm_node(struct nes_cm_core *cm_core,
 		mini_cm_dec_refcnt_listen(cm_core, cm_node->listener, 0);
 	} else {
 		if (cm_node->apbvt_set && cm_node->nesvnic) {
-			nes_manage_apbvt(cm_node->nesvnic, cm_node->loc_port,
-					 PCI_FUNC(
-						 cm_node->nesvnic->nesdev->pcidev->devfn),
+			nes_manage_apbvt(cm_node->nesvnic, cm_node->mapped_loc_port,
+					 PCI_FUNC(cm_node->nesvnic->nesdev->pcidev->devfn),
 					 NES_MANAGE_APBVT_DEL);
 		}
+		nes_debug(NES_DBG_NLMSG, "Delete APBVT mapped_loc_port = %04X\n",
+					cm_node->mapped_loc_port);
+		nes_remove_mapinfo(cm_node->loc_addr, cm_node->loc_port,
+			cm_node->mapped_loc_addr, cm_node->mapped_loc_port);
 	}
 
 	atomic_dec(&cm_core->node_cnt);
@@ -2235,17 +2362,21 @@ static void process_packet(struct nes_cm_node *cm_node, struct sk_buff *skb,
  * mini_cm_listen - create a listen node with params
  */
 static struct nes_cm_listener *mini_cm_listen(struct nes_cm_core *cm_core,
-					      struct nes_vnic *nesvnic, struct nes_cm_info *cm_info)
+			struct nes_vnic *nesvnic, struct nes_cm_info *cm_info)
 {
 	struct nes_cm_listener *listener;
+	struct iwpm_dev_data pm_reg_msg;
+	struct iwpm_sa_data pm_msg;
 	unsigned long flags;
+	int iwpm_err = 0;
 
 	nes_debug(NES_DBG_CM, "Search for 0x%08x : 0x%04x\n",
 		  cm_info->loc_addr, cm_info->loc_port);
 
 	/* cannot have multiple matching listeners */
-	listener = find_listener(cm_core, htonl(cm_info->loc_addr),
-				 htons(cm_info->loc_port), NES_CM_LISTENER_EITHER_STATE);
+	listener = find_listener(cm_core, cm_info->loc_addr, cm_info->loc_port,
+				NES_CM_LISTENER_EITHER_STATE, 1);
+
 	if (listener && listener->listener_state == NES_CM_LISTENER_ACTIVE_STATE) {
 		/* find automatically incs ref count ??? */
 		atomic_dec(&listener->ref_count);
@@ -2254,6 +2385,22 @@ static struct nes_cm_listener *mini_cm_listen(struct nes_cm_core *cm_core,
 	}
 
 	if (!listener) {
+		nes_form_reg_msg(nesvnic, &pm_reg_msg);
+		iwpm_err = iwpm_register_pid(&pm_reg_msg, RDMA_NL_NES);
+		if (iwpm_err) {
+			nes_debug(NES_DBG_NLMSG,
+			"Port Mapper reg pid fail (err = %d).\n", iwpm_err);
+		}
+		if (iwpm_valid_pid() && !iwpm_err) {
+			nes_form_pm_msg(cm_info, &pm_msg);
+			iwpm_err = iwpm_add_mapping(&pm_msg, RDMA_NL_NES);
+			if (iwpm_err)
+				nes_debug(NES_DBG_NLMSG,
+				"Port Mapper query fail (err = %d).\n", iwpm_err);
+			else
+				nes_record_pm_msg(cm_info, &pm_msg);
+		}
+
 		/* create a CM listen node (1/2 node to compare incoming traffic to) */
 		listener = kzalloc(sizeof(*listener), GFP_ATOMIC);
 		if (!listener) {
@@ -2261,8 +2408,10 @@ static struct nes_cm_listener *mini_cm_listen(struct nes_cm_core *cm_core,
 			return NULL;
 		}
 
-		listener->loc_addr = htonl(cm_info->loc_addr);
-		listener->loc_port = htons(cm_info->loc_port);
+		listener->loc_addr = cm_info->loc_addr;
+		listener->loc_port = cm_info->loc_port;
+		listener->mapped_loc_addr = cm_info->mapped_loc_addr;
+		listener->mapped_loc_port = cm_info->mapped_loc_port;
 		listener->reused_node = 0;
 
 		atomic_set(&listener->ref_count, 1);
@@ -2324,14 +2473,18 @@ static struct nes_cm_node *mini_cm_connect(struct nes_cm_core *cm_core,
 
 	if (cm_info->loc_addr == cm_info->rem_addr) {
 		loopbackremotelistener = find_listener(cm_core,
-						       ntohl(nesvnic->local_ipaddr), cm_node->rem_port,
-						       NES_CM_LISTENER_ACTIVE_STATE);
+			cm_node->mapped_loc_addr, cm_node->mapped_rem_port,
+			NES_CM_LISTENER_ACTIVE_STATE, 0);
 		if (loopbackremotelistener == NULL) {
 			create_event(cm_node, NES_CM_EVENT_ABORTED);
 		} else {
 			loopback_cm_info = *cm_info;
 			loopback_cm_info.loc_port = cm_info->rem_port;
 			loopback_cm_info.rem_port = cm_info->loc_port;
+			loopback_cm_info.mapped_loc_port =
+				cm_info->mapped_rem_port;
+			loopback_cm_info.mapped_rem_port =
+				cm_info->mapped_loc_port;
 			loopback_cm_info.cm_id = loopbackremotelistener->cm_id;
 			loopbackremotenode = make_cm_node(cm_core, nesvnic,
 							  &loopback_cm_info, loopbackremotelistener);
@@ -2560,6 +2713,12 @@ static int mini_cm_recv_pkt(struct nes_cm_core *cm_core,
 	nfo.rem_addr = ntohl(iph->saddr);
 	nfo.rem_port = ntohs(tcph->source);
 
+	/* If port mapper is available these should be mapped address info */
+	nfo.mapped_loc_addr = ntohl(iph->daddr);
+	nfo.mapped_loc_port = ntohs(tcph->dest);
+	nfo.mapped_rem_addr = ntohl(iph->saddr);
+	nfo.mapped_rem_port = ntohs(tcph->source);
+
 	tmp_daddr = cpu_to_be32(iph->daddr);
 	tmp_saddr = cpu_to_be32(iph->saddr);
 
@@ -2568,8 +2727,8 @@ static int mini_cm_recv_pkt(struct nes_cm_core *cm_core,
 
 	do {
 		cm_node = find_node(cm_core,
-				    nfo.rem_port, nfo.rem_addr,
-				    nfo.loc_port, nfo.loc_addr);
+				    nfo.mapped_rem_port, nfo.mapped_rem_addr,
+				    nfo.mapped_loc_port, nfo.mapped_loc_addr);
 
 		if (!cm_node) {
 			/* Only type of packet accepted are for */
@@ -2578,9 +2737,9 @@ static int mini_cm_recv_pkt(struct nes_cm_core *cm_core,
 				skb_handled = 0;
 				break;
 			}
-			listener = find_listener(cm_core, nfo.loc_addr,
-						 nfo.loc_port,
-						 NES_CM_LISTENER_ACTIVE_STATE);
+			listener = find_listener(cm_core, nfo.mapped_loc_addr,
+					nfo.mapped_loc_port,
+					NES_CM_LISTENER_ACTIVE_STATE, 0);
 			if (!listener) {
 				nfo.cm_id = NULL;
 				nfo.conn_type = 0;
@@ -3184,10 +3343,12 @@ int nes_accept(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 
 	nes_cm_init_tsa_conn(nesqp, cm_node);
 
-	nesqp->nesqp_context->tcpPorts[0] = cpu_to_le16(ntohs(laddr->sin_port));
-	nesqp->nesqp_context->tcpPorts[1] = cpu_to_le16(ntohs(raddr->sin_port));
+	nesqp->nesqp_context->tcpPorts[0] =
+				cpu_to_le16(cm_node->mapped_loc_port);
+	nesqp->nesqp_context->tcpPorts[1] =
+				cpu_to_le16(cm_node->mapped_rem_port);
 
-	nesqp->nesqp_context->ip0 = cpu_to_le32(ntohl(raddr->sin_addr.s_addr));
+	nesqp->nesqp_context->ip0 = cpu_to_le32(cm_node->mapped_rem_addr);
 
 	nesqp->nesqp_context->misc2 |= cpu_to_le32(
 		(u32)PCI_FUNC(nesdev->pcidev->devfn) <<
@@ -3211,9 +3372,9 @@ int nes_accept(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 	memset(&nes_quad, 0, sizeof(nes_quad));
 	nes_quad.DstIpAdrIndex =
 		cpu_to_le32((u32)PCI_FUNC(nesdev->pcidev->devfn) << 24);
-	nes_quad.SrcIpadr = raddr->sin_addr.s_addr;
-	nes_quad.TcpPorts[0] = raddr->sin_port;
-	nes_quad.TcpPorts[1] = laddr->sin_port;
+	nes_quad.SrcIpadr = htonl(cm_node->mapped_rem_addr);
+	nes_quad.TcpPorts[0] = htons(cm_node->mapped_rem_port);
+	nes_quad.TcpPorts[1] = htons(cm_node->mapped_loc_port);
 
 	/* Produce hash key */
 	crc_value = get_crc_value(&nes_quad);
@@ -3315,6 +3476,9 @@ int nes_connect(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 	int apbvt_set = 0;
 	struct sockaddr_in *laddr = (struct sockaddr_in *)&cm_id->local_addr;
 	struct sockaddr_in *raddr = (struct sockaddr_in *)&cm_id->remote_addr;
+	struct iwpm_dev_data pm_reg_msg;
+	struct iwpm_sa_data pm_msg;
+	int iwpm_err = 0;
 
 	if (cm_id->remote_addr.ss_family != AF_INET)
 		return -ENOSYS;
@@ -3352,20 +3516,44 @@ int nes_connect(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 	nes_debug(NES_DBG_CM, "mpa private data len =%u\n",
 		  conn_param->private_data_len);
 
+	/* set up the connection params for the node */
+	cm_info.loc_addr = ntohl(laddr->sin_addr.s_addr);
+	cm_info.loc_port = ntohs(laddr->sin_port);
+	cm_info.rem_addr = ntohl(raddr->sin_addr.s_addr);
+	cm_info.rem_port = ntohs(raddr->sin_port);
+	cm_info.cm_id = cm_id;
+	cm_info.conn_type = NES_CM_IWARP_CONN_TYPE;
+
+	/* No port mapper available, go with the specified peer information */
+	cm_info.mapped_loc_addr = cm_info.loc_addr;
+	cm_info.mapped_loc_port = cm_info.loc_port;
+	cm_info.mapped_rem_addr = cm_info.rem_addr;
+	cm_info.mapped_rem_port = cm_info.rem_port;
+
+	nes_form_reg_msg(nesvnic, &pm_reg_msg);
+	iwpm_err = iwpm_register_pid(&pm_reg_msg, RDMA_NL_NES);
+	if (iwpm_err) {
+		nes_debug(NES_DBG_NLMSG,
+			"Port Mapper reg pid fail (err = %d).\n", iwpm_err);
+	}
+	if (iwpm_valid_pid() && !iwpm_err) {
+		nes_form_pm_msg(&cm_info, &pm_msg);
+		iwpm_err = iwpm_add_and_query_mapping(&pm_msg, RDMA_NL_NES);
+		if (iwpm_err)
+			nes_debug(NES_DBG_NLMSG,
+			"Port Mapper query fail (err = %d).\n", iwpm_err);
+		else
+			nes_record_pm_msg(&cm_info, &pm_msg);
+	}
+
 	if (laddr->sin_addr.s_addr != raddr->sin_addr.s_addr) {
-		nes_manage_apbvt(nesvnic, ntohs(laddr->sin_port),
-				 PCI_FUNC(nesdev->pcidev->devfn),
-				 NES_MANAGE_APBVT_ADD);
+		nes_manage_apbvt(nesvnic, cm_info.mapped_loc_port,
+			PCI_FUNC(nesdev->pcidev->devfn), NES_MANAGE_APBVT_ADD);
 		apbvt_set = 1;
 	}
 
-	/* set up the connection params for the node */
-	cm_info.loc_addr = htonl(laddr->sin_addr.s_addr);
-	cm_info.loc_port = htons(laddr->sin_port);
-	cm_info.rem_addr = htonl(raddr->sin_addr.s_addr);
-	cm_info.rem_port = htons(raddr->sin_port);
-	cm_info.cm_id = cm_id;
-	cm_info.conn_type = NES_CM_IWARP_CONN_TYPE;
+	if (nes_create_mapinfo(&cm_info))
+		return -ENOMEM;
 
 	cm_id->add_ref(cm_id);
 
@@ -3375,10 +3563,14 @@ int nes_connect(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 					  &cm_info);
 	if (!cm_node) {
 		if (apbvt_set)
-			nes_manage_apbvt(nesvnic, ntohs(laddr->sin_port),
+			nes_manage_apbvt(nesvnic, cm_info.mapped_loc_port,
 					 PCI_FUNC(nesdev->pcidev->devfn),
 					 NES_MANAGE_APBVT_DEL);
 
+		nes_debug(NES_DBG_NLMSG, "Delete mapped_loc_port = %04X\n",
+				cm_info.mapped_loc_port);
+		nes_remove_mapinfo(cm_info.loc_addr, cm_info.loc_port,
+			cm_info.mapped_loc_addr, cm_info.mapped_loc_port);
 		cm_id->rem_ref(cm_id);
 		return -ENOMEM;
 	}
@@ -3424,13 +3616,16 @@ int nes_create_listen(struct iw_cm_id *cm_id, int backlog)
 			nesvnic->local_ipaddr, laddr->sin_addr.s_addr);
 
 	/* setup listen params in our api call struct */
-	cm_info.loc_addr = nesvnic->local_ipaddr;
-	cm_info.loc_port = laddr->sin_port;
+	cm_info.loc_addr = ntohl(nesvnic->local_ipaddr);
+	cm_info.loc_port = ntohs(laddr->sin_port);
 	cm_info.backlog = backlog;
 	cm_info.cm_id = cm_id;
 
 	cm_info.conn_type = NES_CM_IWARP_CONN_TYPE;
 
+	/* No port mapper available, go with the specified info */
+	cm_info.mapped_loc_addr = cm_info.loc_addr;
+	cm_info.mapped_loc_port = cm_info.loc_port;
 
 	cm_node = g_cm_core->api->listen(g_cm_core, nesvnic, &cm_info);
 	if (!cm_node) {
@@ -3442,7 +3637,10 @@ int nes_create_listen(struct iw_cm_id *cm_id, int backlog)
 	cm_id->provider_data = cm_node;
 
 	if (!cm_node->reused_node) {
-		err = nes_manage_apbvt(nesvnic, ntohs(laddr->sin_port),
+		if (nes_create_mapinfo(&cm_info))
+			return -ENOMEM;
+
+		err = nes_manage_apbvt(nesvnic, cm_node->mapped_loc_port,
 				       PCI_FUNC(nesvnic->nesdev->pcidev->devfn),
 				       NES_MANAGE_APBVT_ADD);
 		if (err) {
@@ -3567,9 +3765,11 @@ static void cm_event_connected(struct nes_cm_event *event)
 	nes_cm_init_tsa_conn(nesqp, cm_node);
 
 	/* set the QP tsa context */
-	nesqp->nesqp_context->tcpPorts[0] = cpu_to_le16(ntohs(laddr->sin_port));
-	nesqp->nesqp_context->tcpPorts[1] = cpu_to_le16(ntohs(raddr->sin_port));
-	nesqp->nesqp_context->ip0 = cpu_to_le32(ntohl(raddr->sin_addr.s_addr));
+	nesqp->nesqp_context->tcpPorts[0] =
+			cpu_to_le16(cm_node->mapped_loc_port);
+	nesqp->nesqp_context->tcpPorts[1] =
+			cpu_to_le16(cm_node->mapped_rem_port);
+	nesqp->nesqp_context->ip0 = cpu_to_le32(cm_node->mapped_rem_addr);
 
 	nesqp->nesqp_context->misc2 |= cpu_to_le32(
 			(u32)PCI_FUNC(nesdev->pcidev->devfn) <<
@@ -3599,9 +3799,9 @@ static void cm_event_connected(struct nes_cm_event *event)
 
 	nes_quad.DstIpAdrIndex =
 		cpu_to_le32((u32)PCI_FUNC(nesdev->pcidev->devfn) << 24);
-	nes_quad.SrcIpadr = raddr->sin_addr.s_addr;
-	nes_quad.TcpPorts[0] = raddr->sin_port;
-	nes_quad.TcpPorts[1] = laddr->sin_port;
+	nes_quad.SrcIpadr = htonl(cm_node->mapped_rem_addr);
+	nes_quad.TcpPorts[0] = htons(cm_node->mapped_rem_port);
+	nes_quad.TcpPorts[1] = htons(cm_node->mapped_loc_port);
 
 	/* Produce hash key */
 	crc_value = get_crc_value(&nes_quad);
@@ -3629,7 +3829,7 @@ static void cm_event_connected(struct nes_cm_event *event)
 	cm_event.ird = cm_node->ird_size;
 	cm_event.ord = cm_node->ord_size;
 
-	cm_event_laddr->sin_addr.s_addr = event->cm_info.rem_addr;
+	cm_event_laddr->sin_addr.s_addr = htonl(event->cm_info.rem_addr);
 	ret = cm_id->event_handler(cm_id, &cm_event);
 	nes_debug(NES_DBG_CM, "OFA CM event_handler returned, ret=%d\n", ret);
 
diff --git a/drivers/infiniband/hw/nes/nes_cm.h b/drivers/infiniband/hw/nes/nes_cm.h
index 522c99cd07c..f522cf63978 100644
--- a/drivers/infiniband/hw/nes/nes_cm.h
+++ b/drivers/infiniband/hw/nes/nes_cm.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2006 - 2011 Intel Corporation.  All rights reserved.
+ * Copyright (c) 2006 - 2014 Intel Corporation.  All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -293,8 +293,8 @@ struct nes_cm_listener {
 	struct list_head           list;
 	struct nes_cm_core         *cm_core;
 	u8                         loc_mac[ETH_ALEN];
-	nes_addr_t                 loc_addr;
-	u16                        loc_port;
+	nes_addr_t                 loc_addr, mapped_loc_addr;
+	u16                        loc_port, mapped_loc_port;
 	struct iw_cm_id            *cm_id;
 	enum nes_cm_conn_type      conn_type;
 	atomic_t                   ref_count;
@@ -308,7 +308,9 @@ struct nes_cm_listener {
 /* per connection node and node state information */
 struct nes_cm_node {
 	nes_addr_t                loc_addr, rem_addr;
+	nes_addr_t                mapped_loc_addr, mapped_rem_addr;
 	u16                       loc_port, rem_port;
+	u16                       mapped_loc_port, mapped_rem_port;
 
 	u8                        loc_mac[ETH_ALEN];
 	u8                        rem_mac[ETH_ALEN];
@@ -364,6 +366,10 @@ struct nes_cm_info {
 	u16 rem_port;
 	nes_addr_t loc_addr;
 	nes_addr_t rem_addr;
+	u16 mapped_loc_port;
+	u16 mapped_rem_port;
+	nes_addr_t mapped_loc_addr;
+	nes_addr_t mapped_rem_addr;
 
 	enum nes_cm_conn_type  conn_type;
 	int backlog;
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_stats.c b/drivers/infiniband/hw/ocrdma/ocrdma_stats.c
index 6c54106f5e6..41a9aec9998 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_stats.c
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_stats.c
@@ -510,16 +510,9 @@ exit:
 	return status;
 }
 
-static int ocrdma_debugfs_open(struct inode *inode, struct file *file)
-{
-	if (inode->i_private)
-		file->private_data = inode->i_private;
-	return 0;
-}
-
 static const struct file_operations ocrdma_dbg_ops = {
 	.owner = THIS_MODULE,
-	.open = ocrdma_debugfs_open,
+	.open = simple_open,
 	.read = ocrdma_dbgfs_ops_read,
 };
 
diff --git a/drivers/infiniband/hw/qib/qib_init.c b/drivers/infiniband/hw/qib/qib_init.c
index 5b7aeb224a3..8d3c78ddc90 100644
--- a/drivers/infiniband/hw/qib/qib_init.c
+++ b/drivers/infiniband/hw/qib/qib_init.c
@@ -1272,7 +1272,7 @@ static int qib_notify_dca(struct notifier_block *nb, unsigned long event,
  * Do all the generic driver unit- and chip-independent memory
  * allocation and initialization.
  */
-static int __init qlogic_ib_init(void)
+static int __init qib_ib_init(void)
 {
 	int ret;
 
@@ -1316,12 +1316,12 @@ bail:
 	return ret;
 }
 
-module_init(qlogic_ib_init);
+module_init(qib_ib_init);
 
 /*
  * Do the non-unit driver cleanup, memory free, etc. at unload.
  */
-static void __exit qlogic_ib_cleanup(void)
+static void __exit qib_ib_cleanup(void)
 {
 	int ret;
 
@@ -1346,7 +1346,7 @@ static void __exit qlogic_ib_cleanup(void)
 	qib_dev_cleanup();
 }
 
-module_exit(qlogic_ib_cleanup);
+module_exit(qib_ib_cleanup);
 
 /* this can only be called after a successful initialization */
 static void cleanup_device_data(struct qib_devdata *dd)
diff --git a/drivers/infiniband/hw/qib/qib_mad.c b/drivers/infiniband/hw/qib/qib_mad.c
index edad991d60e..22c720e5740 100644
--- a/drivers/infiniband/hw/qib/qib_mad.c
+++ b/drivers/infiniband/hw/qib/qib_mad.c
@@ -1028,7 +1028,7 @@ static int set_pkeys(struct qib_devdata *dd, u8 port, u16 *pkeys)
 
 		event.event = IB_EVENT_PKEY_CHANGE;
 		event.device = &dd->verbs_dev.ibdev;
-		event.element.port_num = 1;
+		event.element.port_num = port;
 		ib_dispatch_event(&event);
 	}
 	return 0;
diff --git a/drivers/infiniband/hw/qib/qib_qp.c b/drivers/infiniband/hw/qib/qib_qp.c
index 0cad0c40d74..7fcc150d603 100644
--- a/drivers/infiniband/hw/qib/qib_qp.c
+++ b/drivers/infiniband/hw/qib/qib_qp.c
@@ -985,7 +985,8 @@ struct ib_qp *qib_create_qp(struct ib_pd *ibpd,
 	struct ib_qp *ret;
 
 	if (init_attr->cap.max_send_sge > ib_qib_max_sges ||
-	    init_attr->cap.max_send_wr > ib_qib_max_qp_wrs) {
+	    init_attr->cap.max_send_wr > ib_qib_max_qp_wrs ||
+	    init_attr->create_flags) {
 		ret = ERR_PTR(-EINVAL);
 		goto bail;
 	}
diff --git a/drivers/infiniband/hw/usnic/usnic_ib_verbs.c b/drivers/infiniband/hw/usnic/usnic_ib_verbs.c
index d48d2c0a2e3..53bd6a2d9cd 100644
--- a/drivers/infiniband/hw/usnic/usnic_ib_verbs.c
+++ b/drivers/infiniband/hw/usnic/usnic_ib_verbs.c
@@ -466,6 +466,9 @@ struct ib_qp *usnic_ib_create_qp(struct ib_pd *pd,
 	ucontext = to_uucontext(pd->uobject->context);
 	us_ibdev = to_usdev(pd->device);
 
+	if (init_attr->create_flags)
+		return ERR_PTR(-EINVAL);
+
 	err = ib_copy_from_udata(&cmd, udata, sizeof(cmd));
 	if (err) {
 		usnic_err("%s: cannot copy udata for create_qp\n",
diff --git a/drivers/infiniband/hw/usnic/usnic_uiom_interval_tree.c b/drivers/infiniband/hw/usnic/usnic_uiom_interval_tree.c
index d135ad90d91..3a4288e0fba 100644
--- a/drivers/infiniband/hw/usnic/usnic_uiom_interval_tree.c
+++ b/drivers/infiniband/hw/usnic/usnic_uiom_interval_tree.c
@@ -1,3 +1,21 @@
+/*
+ * Copyright (c) 2014, Cisco Systems, Inc. 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/init.h>
 #include <linux/list.h>
 #include <linux/slab.h>
diff --git a/drivers/infiniband/ulp/Makefile b/drivers/infiniband/ulp/Makefile
new file mode 100644
index 00000000000..f3c7dcf0309
--- /dev/null
+++ b/drivers/infiniband/ulp/Makefile
@@ -0,0 +1,5 @@
+obj-$(CONFIG_INFINIBAND_IPOIB)		+= ipoib/
+obj-$(CONFIG_INFINIBAND_SRP)		+= srp/
+obj-$(CONFIG_INFINIBAND_SRPT)		+= srpt/
+obj-$(CONFIG_INFINIBAND_ISER)		+= iser/
+obj-$(CONFIG_INFINIBAND_ISERT)		+= isert/
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_cm.c b/drivers/infiniband/ulp/ipoib/ipoib_cm.c
index 1377f85911c..933efcea0d0 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_cm.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_cm.c
@@ -1030,10 +1030,20 @@ static struct ib_qp *ipoib_cm_create_tx_qp(struct net_device *dev, struct ipoib_
 		.cap.max_send_sge	= 1,
 		.sq_sig_type		= IB_SIGNAL_ALL_WR,
 		.qp_type		= IB_QPT_RC,
-		.qp_context		= tx
+		.qp_context		= tx,
+		.create_flags		= IB_QP_CREATE_USE_GFP_NOIO
 	};
 
-	return ib_create_qp(priv->pd, &attr);
+	struct ib_qp *tx_qp;
+
+	tx_qp = ib_create_qp(priv->pd, &attr);
+	if (PTR_ERR(tx_qp) == -EINVAL) {
+		ipoib_warn(priv, "can't use GFP_NOIO for QPs on device %s, using GFP_KERNEL\n",
+			   priv->ca->name);
+		attr.create_flags &= ~IB_QP_CREATE_USE_GFP_NOIO;
+		tx_qp = ib_create_qp(priv->pd, &attr);
+	}
+	return tx_qp;
 }
 
 static int ipoib_cm_send_req(struct net_device *dev,
@@ -1104,12 +1114,14 @@ static int ipoib_cm_tx_init(struct ipoib_cm_tx *p, u32 qpn,
 	struct ipoib_dev_priv *priv = netdev_priv(p->dev);
 	int ret;
 
-	p->tx_ring = vzalloc(ipoib_sendq_size * sizeof *p->tx_ring);
+	p->tx_ring = __vmalloc(ipoib_sendq_size * sizeof *p->tx_ring,
+			       GFP_NOIO, PAGE_KERNEL);
 	if (!p->tx_ring) {
 		ipoib_warn(priv, "failed to allocate tx ring\n");
 		ret = -ENOMEM;
 		goto err_tx;
 	}
+	memset(p->tx_ring, 0, ipoib_sendq_size * sizeof *p->tx_ring);
 
 	p->qp = ipoib_cm_create_tx_qp(p->dev, p);
 	if (IS_ERR(p->qp)) {
diff --git a/drivers/infiniband/ulp/iser/iscsi_iser.c b/drivers/infiniband/ulp/iser/iscsi_iser.c
index 25f195ef44b..eb7973957a6 100644
--- a/drivers/infiniband/ulp/iser/iscsi_iser.c
+++ b/drivers/infiniband/ulp/iser/iscsi_iser.c
@@ -99,6 +99,7 @@ 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)");
 
+static struct workqueue_struct *release_wq;
 struct iser_global ig;
 
 void
@@ -337,24 +338,6 @@ iscsi_iser_conn_create(struct iscsi_cls_session *cls_session, uint32_t conn_idx)
 	return cls_conn;
 }
 
-static void
-iscsi_iser_conn_destroy(struct iscsi_cls_conn *cls_conn)
-{
-	struct iscsi_conn *conn = cls_conn->dd_data;
-	struct iser_conn *ib_conn = conn->dd_data;
-
-	iscsi_conn_teardown(cls_conn);
-	/*
-	 * Userspace will normally call the stop callback and
-	 * already have freed the ib_conn, but if it goofed up then
-	 * we free it here.
-	 */
-	if (ib_conn) {
-		ib_conn->iscsi_conn = NULL;
-		iser_conn_put(ib_conn, 1); /* deref iscsi/ib conn unbinding */
-	}
-}
-
 static int
 iscsi_iser_conn_bind(struct iscsi_cls_session *cls_session,
 		     struct iscsi_cls_conn *cls_conn, uint64_t transport_eph,
@@ -392,29 +375,39 @@ iscsi_iser_conn_bind(struct iscsi_cls_session *cls_session,
 	conn->dd_data = ib_conn;
 	ib_conn->iscsi_conn = conn;
 
-	iser_conn_get(ib_conn); /* ref iscsi/ib conn binding */
 	return 0;
 }
 
+static int
+iscsi_iser_conn_start(struct iscsi_cls_conn *cls_conn)
+{
+	struct iscsi_conn *iscsi_conn;
+	struct iser_conn *ib_conn;
+
+	iscsi_conn = cls_conn->dd_data;
+	ib_conn = iscsi_conn->dd_data;
+	reinit_completion(&ib_conn->stop_completion);
+
+	return iscsi_conn_start(cls_conn);
+}
+
 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;
 
+	iser_dbg("stopping iscsi_conn: %p, ib_conn: %p\n", conn, ib_conn);
+	iscsi_conn_stop(cls_conn, flag);
+
 	/*
 	 * Userspace may have goofed up and not bound the connection or
 	 * might have only partially setup the connection.
 	 */
 	if (ib_conn) {
-		iscsi_conn_stop(cls_conn, flag);
-		/*
-		 * There is no unbind event so the stop callback
-		 * must release the ref from the bind.
-		 */
-		iser_conn_put(ib_conn, 1); /* deref iscsi/ib conn unbinding */
+		conn->dd_data = NULL;
+		complete(&ib_conn->stop_completion);
 	}
-	conn->dd_data = NULL;
 }
 
 static void iscsi_iser_session_destroy(struct iscsi_cls_session *cls_session)
@@ -515,28 +508,28 @@ iscsi_iser_set_param(struct iscsi_cls_conn *cls_conn,
 	case ISCSI_PARAM_HDRDGST_EN:
 		sscanf(buf, "%d", &value);
 		if (value) {
-			iser_err("DataDigest wasn't negotiated to None");
+			iser_err("DataDigest wasn't negotiated to None\n");
 			return -EPROTO;
 		}
 		break;
 	case ISCSI_PARAM_DATADGST_EN:
 		sscanf(buf, "%d", &value);
 		if (value) {
-			iser_err("DataDigest wasn't negotiated to None");
+			iser_err("DataDigest wasn't negotiated to None\n");
 			return -EPROTO;
 		}
 		break;
 	case ISCSI_PARAM_IFMARKER_EN:
 		sscanf(buf, "%d", &value);
 		if (value) {
-			iser_err("IFMarker wasn't negotiated to No");
+			iser_err("IFMarker wasn't negotiated to No\n");
 			return -EPROTO;
 		}
 		break;
 	case ISCSI_PARAM_OFMARKER_EN:
 		sscanf(buf, "%d", &value);
 		if (value) {
-			iser_err("OFMarker wasn't negotiated to No");
+			iser_err("OFMarker wasn't negotiated to No\n");
 			return -EPROTO;
 		}
 		break;
@@ -652,19 +645,20 @@ iscsi_iser_ep_disconnect(struct iscsi_endpoint *ep)
 	struct iser_conn *ib_conn;
 
 	ib_conn = ep->dd_data;
-	if (ib_conn->iscsi_conn)
-		/*
-		 * Must suspend xmit path if the ep is bound to the
-		 * iscsi_conn, so we know we are not accessing the ib_conn
-		 * when we free it.
-		 *
-		 * This may not be bound if the ep poll failed.
-		 */
-		iscsi_suspend_tx(ib_conn->iscsi_conn);
-
-
-	iser_info("ib conn %p state %d\n", ib_conn, ib_conn->state);
+	iser_info("ep %p ib conn %p state %d\n", ep, ib_conn, ib_conn->state);
 	iser_conn_terminate(ib_conn);
+
+	/*
+	 * if iser_conn and iscsi_conn are bound, we must wait iscsi_conn_stop
+	 * call and ISER_CONN_DOWN state before freeing 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);
+	} else {
+		iser_conn_release(ib_conn);
+	}
 }
 
 static umode_t iser_attr_is_visible(int param_type, int param)
@@ -748,13 +742,13 @@ static struct iscsi_transport iscsi_iser_transport = {
 	/* connection management */
 	.create_conn            = iscsi_iser_conn_create,
 	.bind_conn              = iscsi_iser_conn_bind,
-	.destroy_conn           = iscsi_iser_conn_destroy,
+	.destroy_conn           = iscsi_conn_teardown,
 	.attr_is_visible	= iser_attr_is_visible,
 	.set_param              = iscsi_iser_set_param,
 	.get_conn_param		= iscsi_conn_get_param,
 	.get_ep_param		= iscsi_iser_get_ep_param,
 	.get_session_param	= iscsi_session_get_param,
-	.start_conn             = iscsi_conn_start,
+	.start_conn             = iscsi_iser_conn_start,
 	.stop_conn              = iscsi_iser_conn_stop,
 	/* iscsi host params */
 	.get_host_param		= iscsi_host_get_param,
@@ -801,6 +795,12 @@ static int __init iser_init(void)
 	mutex_init(&ig.connlist_mutex);
 	INIT_LIST_HEAD(&ig.connlist);
 
+	release_wq = alloc_workqueue("release workqueue", 0, 0);
+	if (!release_wq) {
+		iser_err("failed to allocate release workqueue\n");
+		return -ENOMEM;
+	}
+
 	iscsi_iser_scsi_transport = iscsi_register_transport(
 							&iscsi_iser_transport);
 	if (!iscsi_iser_scsi_transport) {
@@ -819,7 +819,24 @@ register_transport_failure:
 
 static void __exit iser_exit(void)
 {
+	struct iser_conn *ib_conn, *n;
+	int connlist_empty;
+
 	iser_dbg("Removing iSER datamover...\n");
+	destroy_workqueue(release_wq);
+
+	mutex_lock(&ig.connlist_mutex);
+	connlist_empty = list_empty(&ig.connlist);
+	mutex_unlock(&ig.connlist_mutex);
+
+	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);
+		}
+	}
+
 	iscsi_unregister_transport(&iscsi_iser_transport);
 	kmem_cache_destroy(ig.desc_cache);
 }
diff --git a/drivers/infiniband/ulp/iser/iscsi_iser.h b/drivers/infiniband/ulp/iser/iscsi_iser.h
index 324129f80d4..97cd385bf7f 100644
--- a/drivers/infiniband/ulp/iser/iscsi_iser.h
+++ b/drivers/infiniband/ulp/iser/iscsi_iser.h
@@ -69,7 +69,7 @@
 
 #define DRV_NAME	"iser"
 #define PFX		DRV_NAME ": "
-#define DRV_VER		"1.3"
+#define DRV_VER		"1.4"
 
 #define iser_dbg(fmt, arg...)				\
 	do {						\
@@ -333,6 +333,8 @@ struct iser_conn {
 	int                          post_recv_buf_count; /* posted rx count  */
 	atomic_t                     post_send_buf_count; /* posted tx count   */
 	char 			     name[ISER_OBJECT_NAME_SIZE];
+	struct work_struct	     release_work;
+	struct completion	     stop_completion;
 	struct list_head	     conn_list;       /* entry in ig conn list */
 
 	char  			     *login_buf;
@@ -417,12 +419,12 @@ void iscsi_iser_recv(struct iscsi_conn *conn,
 
 void iser_conn_init(struct iser_conn *ib_conn);
 
-void iser_conn_get(struct iser_conn *ib_conn);
-
-int iser_conn_put(struct iser_conn *ib_conn, int destroy_cma_id_allowed);
+void iser_conn_release(struct iser_conn *ib_conn);
 
 void iser_conn_terminate(struct iser_conn *ib_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);
diff --git a/drivers/infiniband/ulp/iser/iser_verbs.c b/drivers/infiniband/ulp/iser/iser_verbs.c
index 32849f2becd..ea01075f9f9 100644
--- a/drivers/infiniband/ulp/iser/iser_verbs.c
+++ b/drivers/infiniband/ulp/iser/iser_verbs.c
@@ -581,14 +581,30 @@ static int iser_conn_state_comp_exch(struct iser_conn *ib_conn,
 	return ret;
 }
 
+void iser_release_work(struct work_struct *work)
+{
+	struct iser_conn *ib_conn;
+
+	ib_conn = container_of(work, struct iser_conn, release_work);
+
+	/* wait for .conn_stop callback */
+	wait_for_completion(&ib_conn->stop_completion);
+
+	/* wait for the qp`s post send and post receive buffers to empty */
+	wait_event_interruptible(ib_conn->wait,
+				 ib_conn->state == ISER_CONN_DOWN);
+
+	iser_conn_release(ib_conn);
+}
+
 /**
  * Frees all conn objects and deallocs conn descriptor
  */
-static void iser_conn_release(struct iser_conn *ib_conn, int can_destroy_id)
+void iser_conn_release(struct iser_conn *ib_conn)
 {
 	struct iser_device  *device = ib_conn->device;
 
-	BUG_ON(ib_conn->state != ISER_CONN_DOWN);
+	BUG_ON(ib_conn->state == ISER_CONN_UP);
 
 	mutex_lock(&ig.connlist_mutex);
 	list_del(&ib_conn->conn_list);
@@ -600,27 +616,13 @@ static void iser_conn_release(struct iser_conn *ib_conn, int can_destroy_id)
 	if (device != NULL)
 		iser_device_try_release(device);
 	/* if cma handler context, the caller actually destroy the id */
-	if (ib_conn->cma_id != NULL && can_destroy_id) {
+	if (ib_conn->cma_id != NULL) {
 		rdma_destroy_id(ib_conn->cma_id);
 		ib_conn->cma_id = NULL;
 	}
 	iscsi_destroy_endpoint(ib_conn->ep);
 }
 
-void iser_conn_get(struct iser_conn *ib_conn)
-{
-	atomic_inc(&ib_conn->refcount);
-}
-
-int iser_conn_put(struct iser_conn *ib_conn, int can_destroy_id)
-{
-	if (atomic_dec_and_test(&ib_conn->refcount)) {
-		iser_conn_release(ib_conn, can_destroy_id);
-		return 1;
-	}
-	return 0;
-}
-
 /**
  * triggers start of the disconnect procedures and wait for them to be done
  */
@@ -638,24 +640,19 @@ void iser_conn_terminate(struct iser_conn *ib_conn)
 	if (err)
 		iser_err("Failed to disconnect, conn: 0x%p err %d\n",
 			 ib_conn,err);
-
-	wait_event_interruptible(ib_conn->wait,
-				 ib_conn->state == ISER_CONN_DOWN);
-
-	iser_conn_put(ib_conn, 1); /* deref ib conn deallocate */
 }
 
-static int iser_connect_error(struct rdma_cm_id *cma_id)
+static void iser_connect_error(struct rdma_cm_id *cma_id)
 {
 	struct iser_conn *ib_conn;
+
 	ib_conn = (struct iser_conn *)cma_id->context;
 
 	ib_conn->state = ISER_CONN_DOWN;
 	wake_up_interruptible(&ib_conn->wait);
-	return iser_conn_put(ib_conn, 0); /* deref ib conn's cma id */
 }
 
-static int iser_addr_handler(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;
@@ -664,7 +661,8 @@ static int iser_addr_handler(struct rdma_cm_id *cma_id)
 	device = iser_device_find_by_ib_device(cma_id);
 	if (!device) {
 		iser_err("device lookup/creation failed\n");
-		return iser_connect_error(cma_id);
+		iser_connect_error(cma_id);
+		return;
 	}
 
 	ib_conn = (struct iser_conn *)cma_id->context;
@@ -686,13 +684,12 @@ static int iser_addr_handler(struct rdma_cm_id *cma_id)
 	ret = rdma_resolve_route(cma_id, 1000);
 	if (ret) {
 		iser_err("resolve route failed: %d\n", ret);
-		return iser_connect_error(cma_id);
+		iser_connect_error(cma_id);
+		return;
 	}
-
-	return 0;
 }
 
-static int iser_route_handler(struct rdma_cm_id *cma_id)
+static void iser_route_handler(struct rdma_cm_id *cma_id)
 {
 	struct rdma_conn_param conn_param;
 	int    ret;
@@ -720,9 +717,9 @@ static int iser_route_handler(struct rdma_cm_id *cma_id)
 		goto failure;
 	}
 
-	return 0;
+	return;
 failure:
-	return iser_connect_error(cma_id);
+	iser_connect_error(cma_id);
 }
 
 static void iser_connected_handler(struct rdma_cm_id *cma_id)
@@ -735,14 +732,13 @@ static void iser_connected_handler(struct rdma_cm_id *cma_id)
 	iser_info("remote qpn:%x my qpn:%x\n", attr.dest_qp_num, cma_id->qp->qp_num);
 
 	ib_conn = (struct iser_conn *)cma_id->context;
-	ib_conn->state = ISER_CONN_UP;
-	wake_up_interruptible(&ib_conn->wait);
+	if (iser_conn_state_comp_exch(ib_conn, ISER_CONN_PENDING, ISER_CONN_UP))
+		wake_up_interruptible(&ib_conn->wait);
 }
 
-static int iser_disconnected_handler(struct rdma_cm_id *cma_id)
+static void iser_disconnected_handler(struct rdma_cm_id *cma_id)
 {
 	struct iser_conn *ib_conn;
-	int ret;
 
 	ib_conn = (struct iser_conn *)cma_id->context;
 
@@ -762,24 +758,19 @@ static int iser_disconnected_handler(struct rdma_cm_id *cma_id)
 		ib_conn->state = ISER_CONN_DOWN;
 		wake_up_interruptible(&ib_conn->wait);
 	}
-
-	ret = iser_conn_put(ib_conn, 0); /* deref ib conn's cma id */
-	return ret;
 }
 
 static int iser_cma_handler(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 {
-	int ret = 0;
-
 	iser_info("event %d status %d conn %p id %p\n",
 		  event->event, event->status, cma_id->context, cma_id);
 
 	switch (event->event) {
 	case RDMA_CM_EVENT_ADDR_RESOLVED:
-		ret = iser_addr_handler(cma_id);
+		iser_addr_handler(cma_id);
 		break;
 	case RDMA_CM_EVENT_ROUTE_RESOLVED:
-		ret = iser_route_handler(cma_id);
+		iser_route_handler(cma_id);
 		break;
 	case RDMA_CM_EVENT_ESTABLISHED:
 		iser_connected_handler(cma_id);
@@ -789,18 +780,18 @@ static int iser_cma_handler(struct rdma_cm_id *cma_id, struct rdma_cm_event *eve
 	case RDMA_CM_EVENT_CONNECT_ERROR:
 	case RDMA_CM_EVENT_UNREACHABLE:
 	case RDMA_CM_EVENT_REJECTED:
-		ret = iser_connect_error(cma_id);
+		iser_connect_error(cma_id);
 		break;
 	case RDMA_CM_EVENT_DISCONNECTED:
 	case RDMA_CM_EVENT_DEVICE_REMOVAL:
 	case RDMA_CM_EVENT_ADDR_CHANGE:
-		ret = iser_disconnected_handler(cma_id);
+		iser_disconnected_handler(cma_id);
 		break;
 	default:
 		iser_err("Unexpected RDMA CM event (%d)\n", event->event);
 		break;
 	}
-	return ret;
+	return 0;
 }
 
 void iser_conn_init(struct iser_conn *ib_conn)
@@ -809,7 +800,7 @@ void iser_conn_init(struct iser_conn *ib_conn)
 	init_waitqueue_head(&ib_conn->wait);
 	ib_conn->post_recv_buf_count = 0;
 	atomic_set(&ib_conn->post_send_buf_count, 0);
-	atomic_set(&ib_conn->refcount, 1); /* ref ib conn allocation */
+	init_completion(&ib_conn->stop_completion);
 	INIT_LIST_HEAD(&ib_conn->conn_list);
 	spin_lock_init(&ib_conn->lock);
 }
@@ -837,7 +828,6 @@ int iser_connect(struct iser_conn   *ib_conn,
 
 	ib_conn->state = ISER_CONN_PENDING;
 
-	iser_conn_get(ib_conn); /* ref ib conn's cma id */
 	ib_conn->cma_id = rdma_create_id(iser_cma_handler,
 					     (void *)ib_conn,
 					     RDMA_PS_TCP, IB_QPT_RC);
@@ -874,9 +864,8 @@ id_failure:
 	ib_conn->cma_id = NULL;
 addr_failure:
 	ib_conn->state = ISER_CONN_DOWN;
-	iser_conn_put(ib_conn, 1); /* deref ib conn's cma id */
 connect_failure:
-	iser_conn_put(ib_conn, 1); /* deref ib conn deallocate */
+	iser_conn_release(ib_conn);
 	return err;
 }
 
diff --git a/drivers/infiniband/ulp/isert/ib_isert.c b/drivers/infiniband/ulp/isert/ib_isert.c
index c98fdb18593..b9d647468b9 100644
--- a/drivers/infiniband/ulp/isert/ib_isert.c
+++ b/drivers/infiniband/ulp/isert/ib_isert.c
@@ -28,6 +28,7 @@
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
 #include <target/iscsi/iscsi_transport.h>
+#include <linux/semaphore.h>
 
 #include "isert_proto.h"
 #include "ib_isert.h"
@@ -561,7 +562,15 @@ isert_connect_request(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 	struct isert_device *device;
 	struct ib_device *ib_dev = cma_id->device;
 	int ret = 0;
-	u8 pi_support = np->tpg_np->tpg->tpg_attrib.t10_pi;
+	u8 pi_support;
+
+	spin_lock_bh(&np->np_thread_lock);
+	if (!np->enabled) {
+		spin_unlock_bh(&np->np_thread_lock);
+		pr_debug("iscsi_np is not enabled, reject connect request\n");
+		return rdma_reject(cma_id, NULL, 0);
+	}
+	spin_unlock_bh(&np->np_thread_lock);
 
 	pr_debug("Entering isert_connect_request cma_id: %p, context: %p\n",
 		 cma_id, cma_id->context);
@@ -652,6 +661,7 @@ isert_connect_request(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 		goto out_mr;
 	}
 
+	pi_support = np->tpg_np->tpg->tpg_attrib.t10_pi;
 	if (pi_support && !device->pi_capable) {
 		pr_err("Protection information requested but not supported\n");
 		ret = -EINVAL;
@@ -663,11 +673,11 @@ isert_connect_request(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 		goto out_conn_dev;
 
 	mutex_lock(&isert_np->np_accept_mutex);
-	list_add_tail(&isert_np->np_accept_list, &isert_conn->conn_accept_node);
+	list_add_tail(&isert_conn->conn_accept_node, &isert_np->np_accept_list);
 	mutex_unlock(&isert_np->np_accept_mutex);
 
-	pr_debug("isert_connect_request() waking up np_accept_wq: %p\n", np);
-	wake_up(&isert_np->np_accept_wq);
+	pr_debug("isert_connect_request() up np_sem np: %p\n", np);
+	up(&isert_np->np_sem);
 	return 0;
 
 out_conn_dev:
@@ -1200,6 +1210,8 @@ sequence_cmd:
 
 	if (!rc && dump_payload == false && unsol_data)
 		iscsit_set_unsoliticed_dataout(cmd);
+	else if (dump_payload && imm_data)
+		target_put_sess_cmd(conn->sess->se_sess, &cmd->se_cmd);
 
 	return 0;
 }
@@ -2999,7 +3011,7 @@ isert_setup_np(struct iscsi_np *np,
 		pr_err("Unable to allocate struct isert_np\n");
 		return -ENOMEM;
 	}
-	init_waitqueue_head(&isert_np->np_accept_wq);
+	sema_init(&isert_np->np_sem, 0);
 	mutex_init(&isert_np->np_accept_mutex);
 	INIT_LIST_HEAD(&isert_np->np_accept_list);
 	init_completion(&isert_np->np_login_comp);
@@ -3048,18 +3060,6 @@ out:
 }
 
 static int
-isert_check_accept_queue(struct isert_np *isert_np)
-{
-	int empty;
-
-	mutex_lock(&isert_np->np_accept_mutex);
-	empty = list_empty(&isert_np->np_accept_list);
-	mutex_unlock(&isert_np->np_accept_mutex);
-
-	return empty;
-}
-
-static int
 isert_rdma_accept(struct isert_conn *isert_conn)
 {
 	struct rdma_cm_id *cm_id = isert_conn->conn_cm_id;
@@ -3151,16 +3151,14 @@ isert_accept_np(struct iscsi_np *np, struct iscsi_conn *conn)
 	int max_accept = 0, ret;
 
 accept_wait:
-	ret = wait_event_interruptible(isert_np->np_accept_wq,
-			!isert_check_accept_queue(isert_np) ||
-			np->np_thread_state == ISCSI_NP_THREAD_RESET);
+	ret = down_interruptible(&isert_np->np_sem);
 	if (max_accept > 5)
 		return -ENODEV;
 
 	spin_lock_bh(&np->np_thread_lock);
 	if (np->np_thread_state == ISCSI_NP_THREAD_RESET) {
 		spin_unlock_bh(&np->np_thread_lock);
-		pr_err("ISCSI_NP_THREAD_RESET for isert_accept_np\n");
+		pr_debug("ISCSI_NP_THREAD_RESET for isert_accept_np\n");
 		return -ENODEV;
 	}
 	spin_unlock_bh(&np->np_thread_lock);
diff --git a/drivers/infiniband/ulp/isert/ib_isert.h b/drivers/infiniband/ulp/isert/ib_isert.h
index 4c072ae34c0..da6612e6800 100644
--- a/drivers/infiniband/ulp/isert/ib_isert.h
+++ b/drivers/infiniband/ulp/isert/ib_isert.h
@@ -182,7 +182,7 @@ struct isert_device {
 };
 
 struct isert_np {
-	wait_queue_head_t	np_accept_wq;
+	struct semaphore	np_sem;
 	struct rdma_cm_id	*np_cm_id;
 	struct mutex		np_accept_mutex;
 	struct list_head	np_accept_list;
diff --git a/drivers/infiniband/ulp/srp/ib_srp.c b/drivers/infiniband/ulp/srp/ib_srp.c
index 66a908bf3fb..e3c2c5b4297 100644
--- a/drivers/infiniband/ulp/srp/ib_srp.c
+++ b/drivers/infiniband/ulp/srp/ib_srp.c
@@ -30,7 +30,7 @@
  * SOFTWARE.
  */
 
-#define pr_fmt(fmt) PFX fmt
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/module.h>
 #include <linux/init.h>
@@ -66,6 +66,8 @@ static unsigned int srp_sg_tablesize;
 static unsigned int cmd_sg_entries;
 static unsigned int indirect_sg_entries;
 static bool allow_ext_sg;
+static bool prefer_fr;
+static bool register_always;
 static int topspin_workarounds = 1;
 
 module_param(srp_sg_tablesize, uint, 0444);
@@ -87,6 +89,14 @@ module_param(topspin_workarounds, int, 0444);
 MODULE_PARM_DESC(topspin_workarounds,
 		 "Enable workarounds for Topspin/Cisco SRP target bugs if != 0");
 
+module_param(prefer_fr, bool, 0444);
+MODULE_PARM_DESC(prefer_fr,
+"Whether to use fast registration if both FMR and fast registration are supported");
+
+module_param(register_always, bool, 0444);
+MODULE_PARM_DESC(register_always,
+		 "Use memory registration even for contiguous memory regions");
+
 static struct kernel_param_ops srp_tmo_ops;
 
 static int srp_reconnect_delay = 10;
@@ -288,28 +298,174 @@ static int srp_new_cm_id(struct srp_target_port *target)
 	return 0;
 }
 
+static struct ib_fmr_pool *srp_alloc_fmr_pool(struct srp_target_port *target)
+{
+	struct srp_device *dev = target->srp_host->srp_dev;
+	struct ib_fmr_pool_param fmr_param;
+
+	memset(&fmr_param, 0, sizeof(fmr_param));
+	fmr_param.pool_size	    = target->scsi_host->can_queue;
+	fmr_param.dirty_watermark   = fmr_param.pool_size / 4;
+	fmr_param.cache		    = 1;
+	fmr_param.max_pages_per_fmr = dev->max_pages_per_mr;
+	fmr_param.page_shift	    = ilog2(dev->mr_page_size);
+	fmr_param.access	    = (IB_ACCESS_LOCAL_WRITE |
+				       IB_ACCESS_REMOTE_WRITE |
+				       IB_ACCESS_REMOTE_READ);
+
+	return ib_create_fmr_pool(dev->pd, &fmr_param);
+}
+
+/**
+ * srp_destroy_fr_pool() - free the resources owned by a pool
+ * @pool: Fast registration pool to be destroyed.
+ */
+static void srp_destroy_fr_pool(struct srp_fr_pool *pool)
+{
+	int i;
+	struct srp_fr_desc *d;
+
+	if (!pool)
+		return;
+
+	for (i = 0, d = &pool->desc[0]; i < pool->size; i++, d++) {
+		if (d->frpl)
+			ib_free_fast_reg_page_list(d->frpl);
+		if (d->mr)
+			ib_dereg_mr(d->mr);
+	}
+	kfree(pool);
+}
+
+/**
+ * srp_create_fr_pool() - allocate and initialize a pool for fast registration
+ * @device:            IB device to allocate fast registration descriptors for.
+ * @pd:                Protection domain associated with the FR descriptors.
+ * @pool_size:         Number of descriptors to allocate.
+ * @max_page_list_len: Maximum fast registration work request page list length.
+ */
+static struct srp_fr_pool *srp_create_fr_pool(struct ib_device *device,
+					      struct ib_pd *pd, int pool_size,
+					      int max_page_list_len)
+{
+	struct srp_fr_pool *pool;
+	struct srp_fr_desc *d;
+	struct ib_mr *mr;
+	struct ib_fast_reg_page_list *frpl;
+	int i, ret = -EINVAL;
+
+	if (pool_size <= 0)
+		goto err;
+	ret = -ENOMEM;
+	pool = kzalloc(sizeof(struct srp_fr_pool) +
+		       pool_size * sizeof(struct srp_fr_desc), GFP_KERNEL);
+	if (!pool)
+		goto err;
+	pool->size = pool_size;
+	pool->max_page_list_len = max_page_list_len;
+	spin_lock_init(&pool->lock);
+	INIT_LIST_HEAD(&pool->free_list);
+
+	for (i = 0, d = &pool->desc[0]; i < pool->size; i++, d++) {
+		mr = ib_alloc_fast_reg_mr(pd, max_page_list_len);
+		if (IS_ERR(mr)) {
+			ret = PTR_ERR(mr);
+			goto destroy_pool;
+		}
+		d->mr = mr;
+		frpl = ib_alloc_fast_reg_page_list(device, max_page_list_len);
+		if (IS_ERR(frpl)) {
+			ret = PTR_ERR(frpl);
+			goto destroy_pool;
+		}
+		d->frpl = frpl;
+		list_add_tail(&d->entry, &pool->free_list);
+	}
+
+out:
+	return pool;
+
+destroy_pool:
+	srp_destroy_fr_pool(pool);
+
+err:
+	pool = ERR_PTR(ret);
+	goto out;
+}
+
+/**
+ * srp_fr_pool_get() - obtain a descriptor suitable for fast registration
+ * @pool: Pool to obtain descriptor from.
+ */
+static struct srp_fr_desc *srp_fr_pool_get(struct srp_fr_pool *pool)
+{
+	struct srp_fr_desc *d = NULL;
+	unsigned long flags;
+
+	spin_lock_irqsave(&pool->lock, flags);
+	if (!list_empty(&pool->free_list)) {
+		d = list_first_entry(&pool->free_list, typeof(*d), entry);
+		list_del(&d->entry);
+	}
+	spin_unlock_irqrestore(&pool->lock, flags);
+
+	return d;
+}
+
+/**
+ * srp_fr_pool_put() - put an FR descriptor back in the free list
+ * @pool: Pool the descriptor was allocated from.
+ * @desc: Pointer to an array of fast registration descriptor pointers.
+ * @n:    Number of descriptors to put back.
+ *
+ * Note: The caller must already have queued an invalidation request for
+ * desc->mr->rkey before calling this function.
+ */
+static void srp_fr_pool_put(struct srp_fr_pool *pool, struct srp_fr_desc **desc,
+			    int n)
+{
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&pool->lock, flags);
+	for (i = 0; i < n; i++)
+		list_add(&desc[i]->entry, &pool->free_list);
+	spin_unlock_irqrestore(&pool->lock, flags);
+}
+
+static struct srp_fr_pool *srp_alloc_fr_pool(struct srp_target_port *target)
+{
+	struct srp_device *dev = target->srp_host->srp_dev;
+
+	return srp_create_fr_pool(dev->dev, dev->pd,
+				  target->scsi_host->can_queue,
+				  dev->max_pages_per_mr);
+}
+
 static int srp_create_target_ib(struct srp_target_port *target)
 {
+	struct srp_device *dev = target->srp_host->srp_dev;
 	struct ib_qp_init_attr *init_attr;
 	struct ib_cq *recv_cq, *send_cq;
 	struct ib_qp *qp;
+	struct ib_fmr_pool *fmr_pool = NULL;
+	struct srp_fr_pool *fr_pool = NULL;
+	const int m = 1 + dev->use_fast_reg;
 	int ret;
 
 	init_attr = kzalloc(sizeof *init_attr, GFP_KERNEL);
 	if (!init_attr)
 		return -ENOMEM;
 
-	recv_cq = ib_create_cq(target->srp_host->srp_dev->dev,
-			       srp_recv_completion, NULL, target,
+	recv_cq = ib_create_cq(dev->dev, srp_recv_completion, NULL, target,
 			       target->queue_size, target->comp_vector);
 	if (IS_ERR(recv_cq)) {
 		ret = PTR_ERR(recv_cq);
 		goto err;
 	}
 
-	send_cq = ib_create_cq(target->srp_host->srp_dev->dev,
-			       srp_send_completion, NULL, target,
-			       target->queue_size, target->comp_vector);
+	send_cq = ib_create_cq(dev->dev, srp_send_completion, NULL, target,
+			       m * target->queue_size, target->comp_vector);
 	if (IS_ERR(send_cq)) {
 		ret = PTR_ERR(send_cq);
 		goto err_recv_cq;
@@ -318,16 +474,16 @@ static int srp_create_target_ib(struct srp_target_port *target)
 	ib_req_notify_cq(recv_cq, IB_CQ_NEXT_COMP);
 
 	init_attr->event_handler       = srp_qp_event;
-	init_attr->cap.max_send_wr     = target->queue_size;
+	init_attr->cap.max_send_wr     = m * target->queue_size;
 	init_attr->cap.max_recv_wr     = target->queue_size;
 	init_attr->cap.max_recv_sge    = 1;
 	init_attr->cap.max_send_sge    = 1;
-	init_attr->sq_sig_type         = IB_SIGNAL_ALL_WR;
+	init_attr->sq_sig_type         = IB_SIGNAL_REQ_WR;
 	init_attr->qp_type             = IB_QPT_RC;
 	init_attr->send_cq             = send_cq;
 	init_attr->recv_cq             = recv_cq;
 
-	qp = ib_create_qp(target->srp_host->srp_dev->pd, init_attr);
+	qp = ib_create_qp(dev->pd, init_attr);
 	if (IS_ERR(qp)) {
 		ret = PTR_ERR(qp);
 		goto err_send_cq;
@@ -337,6 +493,30 @@ static int srp_create_target_ib(struct srp_target_port *target)
 	if (ret)
 		goto err_qp;
 
+	if (dev->use_fast_reg && dev->has_fr) {
+		fr_pool = srp_alloc_fr_pool(target);
+		if (IS_ERR(fr_pool)) {
+			ret = PTR_ERR(fr_pool);
+			shost_printk(KERN_WARNING, target->scsi_host, PFX
+				     "FR pool allocation failed (%d)\n", ret);
+			goto err_qp;
+		}
+		if (target->fr_pool)
+			srp_destroy_fr_pool(target->fr_pool);
+		target->fr_pool = fr_pool;
+	} else if (!dev->use_fast_reg && dev->has_fmr) {
+		fmr_pool = srp_alloc_fmr_pool(target);
+		if (IS_ERR(fmr_pool)) {
+			ret = PTR_ERR(fmr_pool);
+			shost_printk(KERN_WARNING, target->scsi_host, PFX
+				     "FMR pool allocation failed (%d)\n", ret);
+			goto err_qp;
+		}
+		if (target->fmr_pool)
+			ib_destroy_fmr_pool(target->fmr_pool);
+		target->fmr_pool = fmr_pool;
+	}
+
 	if (target->qp)
 		ib_destroy_qp(target->qp);
 	if (target->recv_cq)
@@ -371,8 +551,16 @@ err:
  */
 static void srp_free_target_ib(struct srp_target_port *target)
 {
+	struct srp_device *dev = target->srp_host->srp_dev;
 	int i;
 
+	if (dev->use_fast_reg) {
+		if (target->fr_pool)
+			srp_destroy_fr_pool(target->fr_pool);
+	} else {
+		if (target->fmr_pool)
+			ib_destroy_fmr_pool(target->fmr_pool);
+	}
 	ib_destroy_qp(target->qp);
 	ib_destroy_cq(target->send_cq);
 	ib_destroy_cq(target->recv_cq);
@@ -577,7 +765,8 @@ static void srp_disconnect_target(struct srp_target_port *target)
 
 static void srp_free_req_data(struct srp_target_port *target)
 {
-	struct ib_device *ibdev = target->srp_host->srp_dev->dev;
+	struct srp_device *dev = target->srp_host->srp_dev;
+	struct ib_device *ibdev = dev->dev;
 	struct srp_request *req;
 	int i;
 
@@ -586,7 +775,10 @@ static void srp_free_req_data(struct srp_target_port *target)
 
 	for (i = 0; i < target->req_ring_size; ++i) {
 		req = &target->req_ring[i];
-		kfree(req->fmr_list);
+		if (dev->use_fast_reg)
+			kfree(req->fr_list);
+		else
+			kfree(req->fmr_list);
 		kfree(req->map_page);
 		if (req->indirect_dma_addr) {
 			ib_dma_unmap_single(ibdev, req->indirect_dma_addr,
@@ -605,6 +797,7 @@ static int srp_alloc_req_data(struct srp_target_port *target)
 	struct srp_device *srp_dev = target->srp_host->srp_dev;
 	struct ib_device *ibdev = srp_dev->dev;
 	struct srp_request *req;
+	void *mr_list;
 	dma_addr_t dma_addr;
 	int i, ret = -ENOMEM;
 
@@ -617,12 +810,20 @@ static int srp_alloc_req_data(struct srp_target_port *target)
 
 	for (i = 0; i < target->req_ring_size; ++i) {
 		req = &target->req_ring[i];
-		req->fmr_list = kmalloc(target->cmd_sg_cnt * sizeof(void *),
-					GFP_KERNEL);
-		req->map_page = kmalloc(SRP_FMR_SIZE * sizeof(void *),
-					GFP_KERNEL);
+		mr_list = kmalloc(target->cmd_sg_cnt * sizeof(void *),
+				  GFP_KERNEL);
+		if (!mr_list)
+			goto out;
+		if (srp_dev->use_fast_reg)
+			req->fr_list = mr_list;
+		else
+			req->fmr_list = mr_list;
+		req->map_page = kmalloc(srp_dev->max_pages_per_mr *
+					sizeof(void *), GFP_KERNEL);
+		if (!req->map_page)
+			goto out;
 		req->indirect_desc = kmalloc(target->indirect_size, GFP_KERNEL);
-		if (!req->fmr_list || !req->map_page || !req->indirect_desc)
+		if (!req->indirect_desc)
 			goto out;
 
 		dma_addr = ib_dma_map_single(ibdev, req->indirect_desc,
@@ -759,21 +960,56 @@ static int srp_connect_target(struct srp_target_port *target)
 	}
 }
 
+static int srp_inv_rkey(struct srp_target_port *target, u32 rkey)
+{
+	struct ib_send_wr *bad_wr;
+	struct ib_send_wr wr = {
+		.opcode		    = IB_WR_LOCAL_INV,
+		.wr_id		    = LOCAL_INV_WR_ID_MASK,
+		.next		    = NULL,
+		.num_sge	    = 0,
+		.send_flags	    = 0,
+		.ex.invalidate_rkey = rkey,
+	};
+
+	return ib_post_send(target->qp, &wr, &bad_wr);
+}
+
 static void srp_unmap_data(struct scsi_cmnd *scmnd,
 			   struct srp_target_port *target,
 			   struct srp_request *req)
 {
-	struct ib_device *ibdev = target->srp_host->srp_dev->dev;
-	struct ib_pool_fmr **pfmr;
+	struct srp_device *dev = target->srp_host->srp_dev;
+	struct ib_device *ibdev = dev->dev;
+	int i, res;
 
 	if (!scsi_sglist(scmnd) ||
 	    (scmnd->sc_data_direction != DMA_TO_DEVICE &&
 	     scmnd->sc_data_direction != DMA_FROM_DEVICE))
 		return;
 
-	pfmr = req->fmr_list;
-	while (req->nfmr--)
-		ib_fmr_pool_unmap(*pfmr++);
+	if (dev->use_fast_reg) {
+		struct srp_fr_desc **pfr;
+
+		for (i = req->nmdesc, pfr = req->fr_list; i > 0; i--, pfr++) {
+			res = srp_inv_rkey(target, (*pfr)->mr->rkey);
+			if (res < 0) {
+				shost_printk(KERN_ERR, target->scsi_host, PFX
+				  "Queueing INV WR for rkey %#x failed (%d)\n",
+				  (*pfr)->mr->rkey, res);
+				queue_work(system_long_wq,
+					   &target->tl_err_work);
+			}
+		}
+		if (req->nmdesc)
+			srp_fr_pool_put(target->fr_pool, req->fr_list,
+					req->nmdesc);
+	} else {
+		struct ib_pool_fmr **pfmr;
+
+		for (i = req->nmdesc, pfmr = req->fmr_list; i > 0; i--, pfmr++)
+			ib_fmr_pool_unmap(*pfmr);
+	}
 
 	ib_dma_unmap_sg(ibdev, scsi_sglist(scmnd), scsi_sg_count(scmnd),
 			scmnd->sc_data_direction);
@@ -813,6 +1049,10 @@ static struct scsi_cmnd *srp_claim_req(struct srp_target_port *target,
 
 /**
  * srp_free_req() - Unmap data and add request to the free request list.
+ * @target: SRP target port.
+ * @req:    Request to be freed.
+ * @scmnd:  SCSI command associated with @req.
+ * @req_lim_delta: Amount to be added to @target->req_lim.
  */
 static void srp_free_req(struct srp_target_port *target,
 			 struct srp_request *req, struct scsi_cmnd *scmnd,
@@ -882,21 +1122,19 @@ static int srp_rport_reconnect(struct srp_rport *rport)
 	 * callbacks will have finished before a new QP is allocated.
 	 */
 	ret = srp_new_cm_id(target);
-	/*
-	 * Whether or not creating a new CM ID succeeded, create a new
-	 * QP. This guarantees that all completion callback function
-	 * invocations have finished before request resetting starts.
-	 */
-	if (ret == 0)
-		ret = srp_create_target_ib(target);
-	else
-		srp_create_target_ib(target);
 
 	for (i = 0; i < target->req_ring_size; ++i) {
 		struct srp_request *req = &target->req_ring[i];
 		srp_finish_req(target, req, NULL, DID_RESET << 16);
 	}
 
+	/*
+	 * Whether or not creating a new CM ID succeeded, create a new
+	 * QP. This guarantees that all callback functions for the old QP have
+	 * finished before any send requests are posted on the new QP.
+	 */
+	ret += srp_create_target_ib(target);
+
 	INIT_LIST_HEAD(&target->free_tx);
 	for (i = 0; i < target->queue_size; ++i)
 		list_add(&target->tx_ring[i]->list, &target->free_tx);
@@ -928,33 +1166,87 @@ static void srp_map_desc(struct srp_map_state *state, dma_addr_t dma_addr,
 static int srp_map_finish_fmr(struct srp_map_state *state,
 			      struct srp_target_port *target)
 {
-	struct srp_device *dev = target->srp_host->srp_dev;
 	struct ib_pool_fmr *fmr;
 	u64 io_addr = 0;
 
-	if (!state->npages)
-		return 0;
-
-	if (state->npages == 1) {
-		srp_map_desc(state, state->base_dma_addr, state->fmr_len,
-			     target->rkey);
-		state->npages = state->fmr_len = 0;
-		return 0;
-	}
-
-	fmr = ib_fmr_pool_map_phys(dev->fmr_pool, state->pages,
+	fmr = ib_fmr_pool_map_phys(target->fmr_pool, state->pages,
 				   state->npages, io_addr);
 	if (IS_ERR(fmr))
 		return PTR_ERR(fmr);
 
 	*state->next_fmr++ = fmr;
-	state->nfmr++;
+	state->nmdesc++;
+
+	srp_map_desc(state, 0, state->dma_len, fmr->fmr->rkey);
 
-	srp_map_desc(state, 0, state->fmr_len, fmr->fmr->rkey);
-	state->npages = state->fmr_len = 0;
 	return 0;
 }
 
+static int srp_map_finish_fr(struct srp_map_state *state,
+			     struct srp_target_port *target)
+{
+	struct srp_device *dev = target->srp_host->srp_dev;
+	struct ib_send_wr *bad_wr;
+	struct ib_send_wr wr;
+	struct srp_fr_desc *desc;
+	u32 rkey;
+
+	desc = srp_fr_pool_get(target->fr_pool);
+	if (!desc)
+		return -ENOMEM;
+
+	rkey = ib_inc_rkey(desc->mr->rkey);
+	ib_update_fast_reg_key(desc->mr, rkey);
+
+	memcpy(desc->frpl->page_list, state->pages,
+	       sizeof(state->pages[0]) * state->npages);
+
+	memset(&wr, 0, sizeof(wr));
+	wr.opcode = IB_WR_FAST_REG_MR;
+	wr.wr_id = FAST_REG_WR_ID_MASK;
+	wr.wr.fast_reg.iova_start = state->base_dma_addr;
+	wr.wr.fast_reg.page_list = desc->frpl;
+	wr.wr.fast_reg.page_list_len = state->npages;
+	wr.wr.fast_reg.page_shift = ilog2(dev->mr_page_size);
+	wr.wr.fast_reg.length = state->dma_len;
+	wr.wr.fast_reg.access_flags = (IB_ACCESS_LOCAL_WRITE |
+				       IB_ACCESS_REMOTE_READ |
+				       IB_ACCESS_REMOTE_WRITE);
+	wr.wr.fast_reg.rkey = desc->mr->lkey;
+
+	*state->next_fr++ = desc;
+	state->nmdesc++;
+
+	srp_map_desc(state, state->base_dma_addr, state->dma_len,
+		     desc->mr->rkey);
+
+	return ib_post_send(target->qp, &wr, &bad_wr);
+}
+
+static int srp_finish_mapping(struct srp_map_state *state,
+			      struct srp_target_port *target)
+{
+	int ret = 0;
+
+	if (state->npages == 0)
+		return 0;
+
+	if (state->npages == 1 && !register_always)
+		srp_map_desc(state, state->base_dma_addr, state->dma_len,
+			     target->rkey);
+	else
+		ret = target->srp_host->srp_dev->use_fast_reg ?
+			srp_map_finish_fr(state, target) :
+			srp_map_finish_fmr(state, target);
+
+	if (ret == 0) {
+		state->npages = 0;
+		state->dma_len = 0;
+	}
+
+	return ret;
+}
+
 static void srp_map_update_start(struct srp_map_state *state,
 				 struct scatterlist *sg, int sg_index,
 				 dma_addr_t dma_addr)
@@ -967,7 +1259,7 @@ static void srp_map_update_start(struct srp_map_state *state,
 static int srp_map_sg_entry(struct srp_map_state *state,
 			    struct srp_target_port *target,
 			    struct scatterlist *sg, int sg_index,
-			    int use_fmr)
+			    bool use_mr)
 {
 	struct srp_device *dev = target->srp_host->srp_dev;
 	struct ib_device *ibdev = dev->dev;
@@ -979,23 +1271,25 @@ static int srp_map_sg_entry(struct srp_map_state *state,
 	if (!dma_len)
 		return 0;
 
-	if (use_fmr == SRP_MAP_NO_FMR) {
-		/* Once we're in direct map mode for a request, we don't
-		 * go back to FMR mode, so no need to update anything
+	if (!use_mr) {
+		/*
+		 * Once we're in direct map mode for a request, we don't
+		 * go back to FMR or FR mode, so no need to update anything
 		 * other than the descriptor.
 		 */
 		srp_map_desc(state, dma_addr, dma_len, target->rkey);
 		return 0;
 	}
 
-	/* If we start at an offset into the FMR page, don't merge into
-	 * the current FMR. Finish it out, and use the kernel's MR for this
-	 * sg entry. This is to avoid potential bugs on some SRP targets
-	 * that were never quite defined, but went away when the initiator
-	 * avoided using FMR on such page fragments.
+	/*
+	 * Since not all RDMA HW drivers support non-zero page offsets for
+	 * FMR, if we start at an offset into a page, don't merge into the
+	 * current FMR mapping. Finish it out, and use the kernel's MR for
+	 * this sg entry.
 	 */
-	if (dma_addr & ~dev->fmr_page_mask || dma_len > dev->fmr_max_size) {
-		ret = srp_map_finish_fmr(state, target);
+	if ((!dev->use_fast_reg && dma_addr & ~dev->mr_page_mask) ||
+	    dma_len > dev->mr_max_size) {
+		ret = srp_finish_mapping(state, target);
 		if (ret)
 			return ret;
 
@@ -1004,52 +1298,106 @@ static int srp_map_sg_entry(struct srp_map_state *state,
 		return 0;
 	}
 
-	/* If this is the first sg to go into the FMR, save our position.
-	 * We need to know the first unmapped entry, its index, and the
-	 * first unmapped address within that entry to be able to restart
-	 * mapping after an error.
+	/*
+	 * If this is the first sg that will be mapped via FMR or via FR, save
+	 * our position. We need to know the first unmapped entry, its index,
+	 * and the first unmapped address within that entry to be able to
+	 * restart mapping after an error.
 	 */
 	if (!state->unmapped_sg)
 		srp_map_update_start(state, sg, sg_index, dma_addr);
 
 	while (dma_len) {
-		if (state->npages == SRP_FMR_SIZE) {
-			ret = srp_map_finish_fmr(state, target);
+		unsigned offset = dma_addr & ~dev->mr_page_mask;
+		if (state->npages == dev->max_pages_per_mr || offset != 0) {
+			ret = srp_finish_mapping(state, target);
 			if (ret)
 				return ret;
 
 			srp_map_update_start(state, sg, sg_index, dma_addr);
 		}
 
-		len = min_t(unsigned int, dma_len, dev->fmr_page_size);
+		len = min_t(unsigned int, dma_len, dev->mr_page_size - offset);
 
 		if (!state->npages)
 			state->base_dma_addr = dma_addr;
-		state->pages[state->npages++] = dma_addr;
-		state->fmr_len += len;
+		state->pages[state->npages++] = dma_addr & dev->mr_page_mask;
+		state->dma_len += len;
 		dma_addr += len;
 		dma_len -= len;
 	}
 
-	/* If the last entry of the FMR wasn't a full page, then we need to
+	/*
+	 * If the last entry of the MR wasn't a full page, then we need to
 	 * close it out and start a new one -- we can only merge at page
 	 * boundries.
 	 */
 	ret = 0;
-	if (len != dev->fmr_page_size) {
-		ret = srp_map_finish_fmr(state, target);
+	if (len != dev->mr_page_size) {
+		ret = srp_finish_mapping(state, target);
 		if (!ret)
 			srp_map_update_start(state, NULL, 0, 0);
 	}
 	return ret;
 }
 
+static int srp_map_sg(struct srp_map_state *state,
+		      struct srp_target_port *target, struct srp_request *req,
+		      struct scatterlist *scat, int count)
+{
+	struct srp_device *dev = target->srp_host->srp_dev;
+	struct ib_device *ibdev = dev->dev;
+	struct scatterlist *sg;
+	int i;
+	bool use_mr;
+
+	state->desc	= req->indirect_desc;
+	state->pages	= req->map_page;
+	if (dev->use_fast_reg) {
+		state->next_fr = req->fr_list;
+		use_mr = !!target->fr_pool;
+	} else {
+		state->next_fmr = req->fmr_list;
+		use_mr = !!target->fmr_pool;
+	}
+
+	for_each_sg(scat, sg, count, i) {
+		if (srp_map_sg_entry(state, target, sg, i, use_mr)) {
+			/*
+			 * Memory registration failed, so backtrack to the
+			 * first unmapped entry and continue on without using
+			 * memory registration.
+			 */
+			dma_addr_t dma_addr;
+			unsigned int dma_len;
+
+backtrack:
+			sg = state->unmapped_sg;
+			i = state->unmapped_index;
+
+			dma_addr = ib_sg_dma_address(ibdev, sg);
+			dma_len = ib_sg_dma_len(ibdev, sg);
+			dma_len -= (state->unmapped_addr - dma_addr);
+			dma_addr = state->unmapped_addr;
+			use_mr = false;
+			srp_map_desc(state, dma_addr, dma_len, target->rkey);
+		}
+	}
+
+	if (use_mr && srp_finish_mapping(state, target))
+		goto backtrack;
+
+	req->nmdesc = state->nmdesc;
+
+	return 0;
+}
+
 static int srp_map_data(struct scsi_cmnd *scmnd, struct srp_target_port *target,
 			struct srp_request *req)
 {
-	struct scatterlist *scat, *sg;
+	struct scatterlist *scat;
 	struct srp_cmd *cmd = req->cmd->buf;
-	int i, len, nents, count, use_fmr;
+	int len, nents, count;
 	struct srp_device *dev;
 	struct ib_device *ibdev;
 	struct srp_map_state state;
@@ -1081,7 +1429,7 @@ static int srp_map_data(struct scsi_cmnd *scmnd, struct srp_target_port *target,
 	fmt = SRP_DATA_DESC_DIRECT;
 	len = sizeof (struct srp_cmd) +	sizeof (struct srp_direct_buf);
 
-	if (count == 1) {
+	if (count == 1 && !register_always) {
 		/*
 		 * The midlayer only generated a single gather/scatter
 		 * entry, or DMA mapping coalesced everything to a
@@ -1094,13 +1442,13 @@ static int srp_map_data(struct scsi_cmnd *scmnd, struct srp_target_port *target,
 		buf->key = cpu_to_be32(target->rkey);
 		buf->len = cpu_to_be32(ib_sg_dma_len(ibdev, scat));
 
-		req->nfmr = 0;
+		req->nmdesc = 0;
 		goto map_complete;
 	}
 
-	/* We have more than one scatter/gather entry, so build our indirect
-	 * descriptor table, trying to merge as many entries with FMR as we
-	 * can.
+	/*
+	 * We have more than one scatter/gather entry, so build our indirect
+	 * descriptor table, trying to merge as many entries as we can.
 	 */
 	indirect_hdr = (void *) cmd->add_data;
 
@@ -1108,35 +1456,7 @@ static int srp_map_data(struct scsi_cmnd *scmnd, struct srp_target_port *target,
 				   target->indirect_size, DMA_TO_DEVICE);
 
 	memset(&state, 0, sizeof(state));
-	state.desc	= req->indirect_desc;
-	state.pages	= req->map_page;
-	state.next_fmr	= req->fmr_list;
-
-	use_fmr = dev->fmr_pool ? SRP_MAP_ALLOW_FMR : SRP_MAP_NO_FMR;
-
-	for_each_sg(scat, sg, count, i) {
-		if (srp_map_sg_entry(&state, target, sg, i, use_fmr)) {
-			/* FMR mapping failed, so backtrack to the first
-			 * unmapped entry and continue on without using FMR.
-			 */
-			dma_addr_t dma_addr;
-			unsigned int dma_len;
-
-backtrack:
-			sg = state.unmapped_sg;
-			i = state.unmapped_index;
-
-			dma_addr = ib_sg_dma_address(ibdev, sg);
-			dma_len = ib_sg_dma_len(ibdev, sg);
-			dma_len -= (state.unmapped_addr - dma_addr);
-			dma_addr = state.unmapped_addr;
-			use_fmr = SRP_MAP_NO_FMR;
-			srp_map_desc(&state, dma_addr, dma_len, target->rkey);
-		}
-	}
-
-	if (use_fmr == SRP_MAP_ALLOW_FMR && srp_map_finish_fmr(&state, target))
-		goto backtrack;
+	srp_map_sg(&state, target, req, scat, count);
 
 	/* We've mapped the request, now pull as much of the indirect
 	 * descriptor table as we can into the command buffer. If this
@@ -1144,9 +1464,9 @@ backtrack:
 	 * guaranteed to fit into the command, as the SCSI layer won't
 	 * give us more S/G entries than we allow.
 	 */
-	req->nfmr = state.nfmr;
 	if (state.ndesc == 1) {
-		/* FMR mapping was able to collapse this to one entry,
+		/*
+		 * Memory registration collapsed the sg-list into one entry,
 		 * so use a direct descriptor.
 		 */
 		struct srp_direct_buf *buf = (void *) cmd->add_data;
@@ -1455,6 +1775,7 @@ static void srp_handle_recv(struct srp_target_port *target, struct ib_wc *wc)
 
 /**
  * srp_tl_err_work() - handle a transport layer error
+ * @work: Work structure embedded in an SRP target port.
  *
  * Note: This function may get invoked before the rport has been created,
  * hence the target->rport test.
@@ -1468,14 +1789,24 @@ static void srp_tl_err_work(struct work_struct *work)
 		srp_start_tl_fail_timers(target->rport);
 }
 
-static void srp_handle_qp_err(enum ib_wc_status wc_status, bool send_err,
-			      struct srp_target_port *target)
+static void srp_handle_qp_err(u64 wr_id, enum ib_wc_status wc_status,
+			      bool send_err, struct srp_target_port *target)
 {
 	if (target->connected && !target->qp_in_error) {
-		shost_printk(KERN_ERR, target->scsi_host,
-			     PFX "failed %s status %d\n",
-			     send_err ? "send" : "receive",
-			     wc_status);
+		if (wr_id & LOCAL_INV_WR_ID_MASK) {
+			shost_printk(KERN_ERR, target->scsi_host, PFX
+				     "LOCAL_INV failed with status %d\n",
+				     wc_status);
+		} else if (wr_id & FAST_REG_WR_ID_MASK) {
+			shost_printk(KERN_ERR, target->scsi_host, PFX
+				     "FAST_REG_MR failed status %d\n",
+				     wc_status);
+		} else {
+			shost_printk(KERN_ERR, target->scsi_host,
+				     PFX "failed %s status %d for iu %p\n",
+				     send_err ? "send" : "receive",
+				     wc_status, (void *)(uintptr_t)wr_id);
+		}
 		queue_work(system_long_wq, &target->tl_err_work);
 	}
 	target->qp_in_error = true;
@@ -1491,7 +1822,7 @@ static void srp_recv_completion(struct ib_cq *cq, void *target_ptr)
 		if (likely(wc.status == IB_WC_SUCCESS)) {
 			srp_handle_recv(target, &wc);
 		} else {
-			srp_handle_qp_err(wc.status, false, target);
+			srp_handle_qp_err(wc.wr_id, wc.status, false, target);
 		}
 	}
 }
@@ -1507,7 +1838,7 @@ static void srp_send_completion(struct ib_cq *cq, void *target_ptr)
 			iu = (struct srp_iu *) (uintptr_t) wc.wr_id;
 			list_add(&iu->list, &target->free_tx);
 		} else {
-			srp_handle_qp_err(wc.status, true, target);
+			srp_handle_qp_err(wc.wr_id, wc.status, true, target);
 		}
 	}
 }
@@ -1521,7 +1852,7 @@ static int srp_queuecommand(struct Scsi_Host *shost, struct scsi_cmnd *scmnd)
 	struct srp_cmd *cmd;
 	struct ib_device *dev;
 	unsigned long flags;
-	int len, result;
+	int len, ret;
 	const bool in_scsi_eh = !in_interrupt() && current == shost->ehandler;
 
 	/*
@@ -1533,12 +1864,9 @@ static int srp_queuecommand(struct Scsi_Host *shost, struct scsi_cmnd *scmnd)
 	if (in_scsi_eh)
 		mutex_lock(&rport->mutex);
 
-	result = srp_chkready(target->rport);
-	if (unlikely(result)) {
-		scmnd->result = result;
-		scmnd->scsi_done(scmnd);
-		goto unlock_rport;
-	}
+	scmnd->result = srp_chkready(target->rport);
+	if (unlikely(scmnd->result))
+		goto err;
 
 	spin_lock_irqsave(&target->lock, flags);
 	iu = __srp_get_tx_iu(target, SRP_IU_CMD);
@@ -1553,7 +1881,6 @@ static int srp_queuecommand(struct Scsi_Host *shost, struct scsi_cmnd *scmnd)
 	ib_dma_sync_single_for_cpu(dev, iu->dma, target->max_iu_len,
 				   DMA_TO_DEVICE);
 
-	scmnd->result        = 0;
 	scmnd->host_scribble = (void *) req;
 
 	cmd = iu->buf;
@@ -1570,7 +1897,15 @@ static int srp_queuecommand(struct Scsi_Host *shost, struct scsi_cmnd *scmnd)
 	len = srp_map_data(scmnd, target, req);
 	if (len < 0) {
 		shost_printk(KERN_ERR, target->scsi_host,
-			     PFX "Failed to map data\n");
+			     PFX "Failed to map data (%d)\n", len);
+		/*
+		 * If we ran out of memory descriptors (-ENOMEM) because an
+		 * application is queuing many requests with more than
+		 * max_pages_per_mr sg-list elements, tell the SCSI mid-layer
+		 * to reduce queue depth temporarily.
+		 */
+		scmnd->result = len == -ENOMEM ?
+			DID_OK << 16 | QUEUE_FULL << 1 : DID_ERROR << 16;
 		goto err_iu;
 	}
 
@@ -1582,11 +1917,13 @@ static int srp_queuecommand(struct Scsi_Host *shost, struct scsi_cmnd *scmnd)
 		goto err_unmap;
 	}
 
+	ret = 0;
+
 unlock_rport:
 	if (in_scsi_eh)
 		mutex_unlock(&rport->mutex);
 
-	return 0;
+	return ret;
 
 err_unmap:
 	srp_unmap_data(scmnd, target, req);
@@ -1594,16 +1931,27 @@ err_unmap:
 err_iu:
 	srp_put_tx_iu(target, iu, SRP_IU_CMD);
 
+	/*
+	 * Avoid that the loops that iterate over the request ring can
+	 * encounter a dangling SCSI command pointer.
+	 */
+	req->scmnd = NULL;
+
 	spin_lock_irqsave(&target->lock, flags);
 	list_add(&req->list, &target->free_reqs);
 
 err_unlock:
 	spin_unlock_irqrestore(&target->lock, flags);
 
-	if (in_scsi_eh)
-		mutex_unlock(&rport->mutex);
+err:
+	if (scmnd->result) {
+		scmnd->scsi_done(scmnd);
+		ret = 0;
+	} else {
+		ret = SCSI_MLQUEUE_HOST_BUSY;
+	}
 
-	return SCSI_MLQUEUE_HOST_BUSY;
+	goto unlock_rport;
 }
 
 /*
@@ -2310,6 +2658,8 @@ static struct class srp_class = {
 
 /**
  * srp_conn_unique() - check whether the connection to a target is unique
+ * @host:   SRP host.
+ * @target: SRP target port.
  */
 static bool srp_conn_unique(struct srp_host *host,
 			    struct srp_target_port *target)
@@ -2605,7 +2955,8 @@ static ssize_t srp_create_target(struct device *dev,
 		container_of(dev, struct srp_host, dev);
 	struct Scsi_Host *target_host;
 	struct srp_target_port *target;
-	struct ib_device *ibdev = host->srp_dev->dev;
+	struct srp_device *srp_dev = host->srp_dev;
+	struct ib_device *ibdev = srp_dev->dev;
 	int ret;
 
 	target_host = scsi_host_alloc(&srp_template,
@@ -2650,9 +3001,9 @@ static ssize_t srp_create_target(struct device *dev,
 		goto err;
 	}
 
-	if (!host->srp_dev->fmr_pool && !target->allow_ext_sg &&
-				target->cmd_sg_cnt < target->sg_tablesize) {
-		pr_warn("No FMR pool and no external indirect descriptors, limiting sg_tablesize to cmd_sg_cnt\n");
+	if (!srp_dev->has_fmr && !srp_dev->has_fr && !target->allow_ext_sg &&
+	    target->cmd_sg_cnt < target->sg_tablesize) {
+		pr_warn("No MR pool and no external indirect descriptors, limiting sg_tablesize to cmd_sg_cnt\n");
 		target->sg_tablesize = target->cmd_sg_cnt;
 	}
 
@@ -2790,9 +3141,9 @@ static void srp_add_one(struct ib_device *device)
 {
 	struct srp_device *srp_dev;
 	struct ib_device_attr *dev_attr;
-	struct ib_fmr_pool_param fmr_param;
 	struct srp_host *host;
-	int max_pages_per_fmr, fmr_page_shift, s, e, p;
+	int mr_page_shift, s, e, p;
+	u64 max_pages_per_mr;
 
 	dev_attr = kmalloc(sizeof *dev_attr, GFP_KERNEL);
 	if (!dev_attr)
@@ -2807,15 +3158,39 @@ static void srp_add_one(struct ib_device *device)
 	if (!srp_dev)
 		goto free_attr;
 
+	srp_dev->has_fmr = (device->alloc_fmr && device->dealloc_fmr &&
+			    device->map_phys_fmr && device->unmap_fmr);
+	srp_dev->has_fr = (dev_attr->device_cap_flags &
+			   IB_DEVICE_MEM_MGT_EXTENSIONS);
+	if (!srp_dev->has_fmr && !srp_dev->has_fr)
+		dev_warn(&device->dev, "neither FMR nor FR is supported\n");
+
+	srp_dev->use_fast_reg = (srp_dev->has_fr &&
+				 (!srp_dev->has_fmr || prefer_fr));
+
 	/*
 	 * Use the smallest page size supported by the HCA, down to a
 	 * minimum of 4096 bytes. We're unlikely to build large sglists
 	 * out of smaller entries.
 	 */
-	fmr_page_shift		= max(12, ffs(dev_attr->page_size_cap) - 1);
-	srp_dev->fmr_page_size	= 1 << fmr_page_shift;
-	srp_dev->fmr_page_mask	= ~((u64) srp_dev->fmr_page_size - 1);
-	srp_dev->fmr_max_size	= srp_dev->fmr_page_size * SRP_FMR_SIZE;
+	mr_page_shift		= max(12, ffs(dev_attr->page_size_cap) - 1);
+	srp_dev->mr_page_size	= 1 << mr_page_shift;
+	srp_dev->mr_page_mask	= ~((u64) srp_dev->mr_page_size - 1);
+	max_pages_per_mr	= dev_attr->max_mr_size;
+	do_div(max_pages_per_mr, srp_dev->mr_page_size);
+	srp_dev->max_pages_per_mr = min_t(u64, SRP_MAX_PAGES_PER_MR,
+					  max_pages_per_mr);
+	if (srp_dev->use_fast_reg) {
+		srp_dev->max_pages_per_mr =
+			min_t(u32, srp_dev->max_pages_per_mr,
+			      dev_attr->max_fast_reg_page_list_len);
+	}
+	srp_dev->mr_max_size	= srp_dev->mr_page_size *
+				   srp_dev->max_pages_per_mr;
+	pr_debug("%s: mr_page_shift = %d, dev_attr->max_mr_size = %#llx, dev_attr->max_fast_reg_page_list_len = %u, max_pages_per_mr = %d, mr_max_size = %#x\n",
+		 device->name, mr_page_shift, dev_attr->max_mr_size,
+		 dev_attr->max_fast_reg_page_list_len,
+		 srp_dev->max_pages_per_mr, srp_dev->mr_max_size);
 
 	INIT_LIST_HEAD(&srp_dev->dev_list);
 
@@ -2831,27 +3206,6 @@ static void srp_add_one(struct ib_device *device)
 	if (IS_ERR(srp_dev->mr))
 		goto err_pd;
 
-	for (max_pages_per_fmr = SRP_FMR_SIZE;
-			max_pages_per_fmr >= SRP_FMR_MIN_SIZE;
-			max_pages_per_fmr /= 2, srp_dev->fmr_max_size /= 2) {
-		memset(&fmr_param, 0, sizeof fmr_param);
-		fmr_param.pool_size	    = SRP_FMR_POOL_SIZE;
-		fmr_param.dirty_watermark   = SRP_FMR_DIRTY_SIZE;
-		fmr_param.cache		    = 1;
-		fmr_param.max_pages_per_fmr = max_pages_per_fmr;
-		fmr_param.page_shift	    = fmr_page_shift;
-		fmr_param.access	    = (IB_ACCESS_LOCAL_WRITE |
-					       IB_ACCESS_REMOTE_WRITE |
-					       IB_ACCESS_REMOTE_READ);
-
-		srp_dev->fmr_pool = ib_create_fmr_pool(srp_dev->pd, &fmr_param);
-		if (!IS_ERR(srp_dev->fmr_pool))
-			break;
-	}
-
-	if (IS_ERR(srp_dev->fmr_pool))
-		srp_dev->fmr_pool = NULL;
-
 	if (device->node_type == RDMA_NODE_IB_SWITCH) {
 		s = 0;
 		e = 0;
@@ -2914,8 +3268,6 @@ static void srp_remove_one(struct ib_device *device)
 		kfree(host);
 	}
 
-	if (srp_dev->fmr_pool)
-		ib_destroy_fmr_pool(srp_dev->fmr_pool);
 	ib_dereg_mr(srp_dev->mr);
 	ib_dealloc_pd(srp_dev->pd);
 
diff --git a/drivers/infiniband/ulp/srp/ib_srp.h b/drivers/infiniband/ulp/srp/ib_srp.h
index aad27b7b4a4..e46ecb15aa0 100644
--- a/drivers/infiniband/ulp/srp/ib_srp.h
+++ b/drivers/infiniband/ulp/srp/ib_srp.h
@@ -66,13 +66,10 @@ enum {
 	SRP_TAG_NO_REQ		= ~0U,
 	SRP_TAG_TSK_MGMT	= 1U << 31,
 
-	SRP_FMR_SIZE		= 512,
-	SRP_FMR_MIN_SIZE	= 128,
-	SRP_FMR_POOL_SIZE	= 1024,
-	SRP_FMR_DIRTY_SIZE	= SRP_FMR_POOL_SIZE / 4,
+	SRP_MAX_PAGES_PER_MR	= 512,
 
-	SRP_MAP_ALLOW_FMR	= 0,
-	SRP_MAP_NO_FMR		= 1,
+	LOCAL_INV_WR_ID_MASK	= 1,
+	FAST_REG_WR_ID_MASK	= 2,
 };
 
 enum srp_target_state {
@@ -86,15 +83,24 @@ enum srp_iu_type {
 	SRP_IU_RSP,
 };
 
+/*
+ * @mr_page_mask: HCA memory registration page mask.
+ * @mr_page_size: HCA memory registration page size.
+ * @mr_max_size: Maximum size in bytes of a single FMR / FR registration
+ *   request.
+ */
 struct srp_device {
 	struct list_head	dev_list;
 	struct ib_device       *dev;
 	struct ib_pd	       *pd;
 	struct ib_mr	       *mr;
-	struct ib_fmr_pool     *fmr_pool;
-	u64			fmr_page_mask;
-	int			fmr_page_size;
-	int			fmr_max_size;
+	u64			mr_page_mask;
+	int			mr_page_size;
+	int			mr_max_size;
+	int			max_pages_per_mr;
+	bool			has_fmr;
+	bool			has_fr;
+	bool			use_fast_reg;
 };
 
 struct srp_host {
@@ -112,11 +118,14 @@ struct srp_request {
 	struct list_head	list;
 	struct scsi_cmnd       *scmnd;
 	struct srp_iu	       *cmd;
-	struct ib_pool_fmr    **fmr_list;
+	union {
+		struct ib_pool_fmr **fmr_list;
+		struct srp_fr_desc **fr_list;
+	};
 	u64		       *map_page;
 	struct srp_direct_buf  *indirect_desc;
 	dma_addr_t		indirect_dma_addr;
-	short			nfmr;
+	short			nmdesc;
 	short			index;
 };
 
@@ -131,6 +140,10 @@ struct srp_target_port {
 	struct ib_cq	       *send_cq ____cacheline_aligned_in_smp;
 	struct ib_cq	       *recv_cq;
 	struct ib_qp	       *qp;
+	union {
+		struct ib_fmr_pool     *fmr_pool;
+		struct srp_fr_pool     *fr_pool;
+	};
 	u32			lkey;
 	u32			rkey;
 	enum srp_target_state	state;
@@ -197,15 +210,66 @@ struct srp_iu {
 	enum dma_data_direction	direction;
 };
 
+/**
+ * struct srp_fr_desc - fast registration work request arguments
+ * @entry: Entry in srp_fr_pool.free_list.
+ * @mr:    Memory region.
+ * @frpl:  Fast registration page list.
+ */
+struct srp_fr_desc {
+	struct list_head		entry;
+	struct ib_mr			*mr;
+	struct ib_fast_reg_page_list	*frpl;
+};
+
+/**
+ * struct srp_fr_pool - pool of fast registration descriptors
+ *
+ * An entry is available for allocation if and only if it occurs in @free_list.
+ *
+ * @size:      Number of descriptors in this pool.
+ * @max_page_list_len: Maximum fast registration work request page list length.
+ * @lock:      Protects free_list.
+ * @free_list: List of free descriptors.
+ * @desc:      Fast registration descriptor pool.
+ */
+struct srp_fr_pool {
+	int			size;
+	int			max_page_list_len;
+	spinlock_t		lock;
+	struct list_head	free_list;
+	struct srp_fr_desc	desc[0];
+};
+
+/**
+ * struct srp_map_state - per-request DMA memory mapping state
+ * @desc:	    Pointer to the element of the SRP buffer descriptor array
+ *		    that is being filled in.
+ * @pages:	    Array with DMA addresses of pages being considered for
+ *		    memory registration.
+ * @base_dma_addr:  DMA address of the first page that has not yet been mapped.
+ * @dma_len:	    Number of bytes that will be registered with the next
+ *		    FMR or FR memory registration call.
+ * @total_len:	    Total number of bytes in the sg-list being mapped.
+ * @npages:	    Number of page addresses in the pages[] array.
+ * @nmdesc:	    Number of FMR or FR memory descriptors used for mapping.
+ * @ndesc:	    Number of SRP buffer descriptors that have been filled in.
+ * @unmapped_sg:    First element of the sg-list that is mapped via FMR or FR.
+ * @unmapped_index: Index of the first element mapped via FMR or FR.
+ * @unmapped_addr:  DMA address of the first element mapped via FMR or FR.
+ */
 struct srp_map_state {
-	struct ib_pool_fmr    **next_fmr;
+	union {
+		struct ib_pool_fmr **next_fmr;
+		struct srp_fr_desc **next_fr;
+	};
 	struct srp_direct_buf  *desc;
 	u64		       *pages;
 	dma_addr_t		base_dma_addr;
-	u32			fmr_len;
+	u32			dma_len;
 	u32			total_len;
 	unsigned int		npages;
-	unsigned int		nfmr;
+	unsigned int		nmdesc;
 	unsigned int		ndesc;
 	struct scatterlist     *unmapped_sg;
 	int			unmapped_index;
diff --git a/drivers/input/evdev.c b/drivers/input/evdev.c
index ce953d895f5..fd325ec9f06 100644
--- a/drivers/input/evdev.c
+++ b/drivers/input/evdev.c
@@ -629,12 +629,10 @@ static int str_to_user(const char *str, unsigned int maxlen, void __user *p)
 	return copy_to_user(p, str, len) ? -EFAULT : len;
 }
 
-#define OLD_KEY_MAX	0x1ff
 static int handle_eviocgbit(struct input_dev *dev,
 			    unsigned int type, unsigned int size,
 			    void __user *p, int compat_mode)
 {
-	static unsigned long keymax_warn_time;
 	unsigned long *bits;
 	int len;
 
@@ -652,24 +650,8 @@ static int handle_eviocgbit(struct input_dev *dev,
 	default: return -EINVAL;
 	}
 
-	/*
-	 * Work around bugs in userspace programs that like to do
-	 * EVIOCGBIT(EV_KEY, KEY_MAX) and not realize that 'len'
-	 * should be in bytes, not in bits.
-	 */
-	if (type == EV_KEY && size == OLD_KEY_MAX) {
-		len = OLD_KEY_MAX;
-		if (printk_timed_ratelimit(&keymax_warn_time, 10 * 1000))
-			pr_warning("(EVIOCGBIT): Suspicious buffer size %u, "
-				   "limiting output to %zu bytes. See "
-				   "http://userweb.kernel.org/~dtor/eviocgbit-bug.html\n",
-				   OLD_KEY_MAX,
-				   BITS_TO_LONGS(OLD_KEY_MAX) * sizeof(long));
-	}
-
 	return bits_to_user(bits, len, size, p, compat_mode);
 }
-#undef OLD_KEY_MAX
 
 static int evdev_handle_get_keycode(struct input_dev *dev, void __user *p)
 {
diff --git a/drivers/input/input-polldev.c b/drivers/input/input-polldev.c
index 7f161d93203..3664f81655c 100644
--- a/drivers/input/input-polldev.c
+++ b/drivers/input/input-polldev.c
@@ -147,6 +147,11 @@ static struct attribute_group input_polldev_attribute_group = {
 	.attrs = sysfs_attrs
 };
 
+static const struct attribute_group *input_polldev_attribute_groups[] = {
+	&input_polldev_attribute_group,
+	NULL
+};
+
 /**
  * input_allocate_polled_device - allocate memory for polled device
  *
@@ -171,6 +176,91 @@ struct input_polled_dev *input_allocate_polled_device(void)
 }
 EXPORT_SYMBOL(input_allocate_polled_device);
 
+struct input_polled_devres {
+	struct input_polled_dev *polldev;
+};
+
+static int devm_input_polldev_match(struct device *dev, void *res, void *data)
+{
+	struct input_polled_devres *devres = res;
+
+	return devres->polldev == data;
+}
+
+static void devm_input_polldev_release(struct device *dev, void *res)
+{
+	struct input_polled_devres *devres = res;
+	struct input_polled_dev *polldev = devres->polldev;
+
+	dev_dbg(dev, "%s: dropping reference/freeing %s\n",
+		__func__, dev_name(&polldev->input->dev));
+
+	input_put_device(polldev->input);
+	kfree(polldev);
+}
+
+static void devm_input_polldev_unregister(struct device *dev, void *res)
+{
+	struct input_polled_devres *devres = res;
+	struct input_polled_dev *polldev = devres->polldev;
+
+	dev_dbg(dev, "%s: unregistering device %s\n",
+		__func__, dev_name(&polldev->input->dev));
+	input_unregister_device(polldev->input);
+
+	/*
+	 * Note that we are still holding extra reference to the input
+	 * device so it will stick around until devm_input_polldev_release()
+	 * is called.
+	 */
+}
+
+/**
+ * devm_input_allocate_polled_device - allocate managed polled device
+ * @dev: device owning the polled device being created
+ *
+ * Returns prepared &struct input_polled_dev or %NULL.
+ *
+ * Managed polled input devices do not need to be explicitly unregistered
+ * or freed as it will be done automatically when owner device unbinds
+ * from * its driver (or binding fails). Once such managed polled device
+ * is allocated, it is ready to be set up and registered in the same
+ * fashion as regular polled input devices (using
+ * input_register_polled_device() function).
+ *
+ * If you want to manually unregister and free such managed polled devices,
+ * it can be still done by calling input_unregister_polled_device() and
+ * input_free_polled_device(), although it is rarely needed.
+ *
+ * NOTE: the owner device is set up as parent of input device and users
+ * should not override it.
+ */
+struct input_polled_dev *devm_input_allocate_polled_device(struct device *dev)
+{
+	struct input_polled_dev *polldev;
+	struct input_polled_devres *devres;
+
+	devres = devres_alloc(devm_input_polldev_release, sizeof(*devres),
+			      GFP_KERNEL);
+	if (!devres)
+		return NULL;
+
+	polldev = input_allocate_polled_device();
+	if (!polldev) {
+		devres_free(devres);
+		return NULL;
+	}
+
+	polldev->input->dev.parent = dev;
+	polldev->devres_managed = true;
+
+	devres->polldev = polldev;
+	devres_add(dev, devres);
+
+	return polldev;
+}
+EXPORT_SYMBOL(devm_input_allocate_polled_device);
+
 /**
  * input_free_polled_device - free memory allocated for polled device
  * @dev: device to free
@@ -181,7 +271,12 @@ EXPORT_SYMBOL(input_allocate_polled_device);
 void input_free_polled_device(struct input_polled_dev *dev)
 {
 	if (dev) {
-		input_free_device(dev->input);
+		if (dev->devres_managed)
+			WARN_ON(devres_destroy(dev->input->dev.parent,
+						devm_input_polldev_release,
+						devm_input_polldev_match,
+						dev));
+		input_put_device(dev->input);
 		kfree(dev);
 	}
 }
@@ -199,26 +294,35 @@ EXPORT_SYMBOL(input_free_polled_device);
  */
 int input_register_polled_device(struct input_polled_dev *dev)
 {
+	struct input_polled_devres *devres = NULL;
 	struct input_dev *input = dev->input;
 	int error;
 
+	if (dev->devres_managed) {
+		devres = devres_alloc(devm_input_polldev_unregister,
+				      sizeof(*devres), GFP_KERNEL);
+		if (!devres)
+			return -ENOMEM;
+
+		devres->polldev = dev;
+	}
+
 	input_set_drvdata(input, dev);
 	INIT_DELAYED_WORK(&dev->work, input_polled_device_work);
+
 	if (!dev->poll_interval)
 		dev->poll_interval = 500;
 	if (!dev->poll_interval_max)
 		dev->poll_interval_max = dev->poll_interval;
+
 	input->open = input_open_polled_device;
 	input->close = input_close_polled_device;
 
-	error = input_register_device(input);
-	if (error)
-		return error;
+	input->dev.groups = input_polldev_attribute_groups;
 
-	error = sysfs_create_group(&input->dev.kobj,
-				   &input_polldev_attribute_group);
+	error = input_register_device(input);
 	if (error) {
-		input_unregister_device(input);
+		devres_free(devres);
 		return error;
 	}
 
@@ -231,6 +335,12 @@ int input_register_polled_device(struct input_polled_dev *dev)
 	 */
 	input_get_device(input);
 
+	if (dev->devres_managed) {
+		dev_dbg(input->dev.parent, "%s: registering %s with devres.\n",
+			__func__, dev_name(&input->dev));
+		devres_add(input->dev.parent, devres);
+	}
+
 	return 0;
 }
 EXPORT_SYMBOL(input_register_polled_device);
@@ -245,8 +355,11 @@ EXPORT_SYMBOL(input_register_polled_device);
  */
 void input_unregister_polled_device(struct input_polled_dev *dev)
 {
-	sysfs_remove_group(&dev->input->dev.kobj,
-			   &input_polldev_attribute_group);
+	if (dev->devres_managed)
+		WARN_ON(devres_destroy(dev->input->dev.parent,
+					devm_input_polldev_unregister,
+					devm_input_polldev_match,
+					dev));
 
 	input_unregister_device(dev->input);
 }
diff --git a/drivers/input/keyboard/Kconfig b/drivers/input/keyboard/Kconfig
index 76842d7dc2e..f7e79b48134 100644
--- a/drivers/input/keyboard/Kconfig
+++ b/drivers/input/keyboard/Kconfig
@@ -71,7 +71,7 @@ config KEYBOARD_ATKBD
 	default y
 	select SERIO
 	select SERIO_LIBPS2
-	select SERIO_I8042 if X86
+	select SERIO_I8042 if ARCH_MIGHT_HAVE_PC_SERIO
 	select SERIO_GSCPS2 if GSC
 	help
 	  Say Y here if you want to use a standard AT or PS/2 keyboard. Usually
@@ -524,6 +524,17 @@ config KEYBOARD_STOWAWAY
 	  To compile this driver as a module, choose M here: the
 	  module will be called stowaway.
 
+config KEYBOARD_ST_KEYSCAN
+	tristate "STMicroelectronics keyscan support"
+	depends on ARCH_STI || COMPILE_TEST
+	select INPUT_MATRIXKMAP
+	help
+	  Say Y here if you want to use a keypad attached to the keyscan block
+	  on some STMicroelectronics SoC devices.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called st-keyscan.
+
 config KEYBOARD_SUNKBD
 	tristate "Sun Type 4 and Type 5 keyboard"
 	select SERIO
@@ -578,7 +589,7 @@ config KEYBOARD_OMAP
 
 config KEYBOARD_OMAP4
 	tristate "TI OMAP4+ keypad support"
-	depends on ARCH_OMAP2PLUS
+	depends on OF || ARCH_OMAP2PLUS
 	select INPUT_MATRIXKMAP
 	help
 	  Say Y here if you want to use the OMAP4+ keypad.
diff --git a/drivers/input/keyboard/Makefile b/drivers/input/keyboard/Makefile
index 11cff7b84b4..7504ae19049 100644
--- a/drivers/input/keyboard/Makefile
+++ b/drivers/input/keyboard/Makefile
@@ -51,6 +51,7 @@ obj-$(CONFIG_KEYBOARD_SH_KEYSC)		+= sh_keysc.o
 obj-$(CONFIG_KEYBOARD_SPEAR)		+= spear-keyboard.o
 obj-$(CONFIG_KEYBOARD_STMPE)		+= stmpe-keypad.o
 obj-$(CONFIG_KEYBOARD_STOWAWAY)		+= stowaway.o
+obj-$(CONFIG_KEYBOARD_ST_KEYSCAN)	+= st-keyscan.o
 obj-$(CONFIG_KEYBOARD_SUNKBD)		+= sunkbd.o
 obj-$(CONFIG_KEYBOARD_TC3589X)		+= tc3589x-keypad.o
 obj-$(CONFIG_KEYBOARD_TEGRA)		+= tegra-kbc.o
diff --git a/drivers/input/keyboard/adp5520-keys.c b/drivers/input/keyboard/adp5520-keys.c
index 4cc14c2fa7d..7f4a8b58efc 100644
--- a/drivers/input/keyboard/adp5520-keys.c
+++ b/drivers/input/keyboard/adp5520-keys.c
@@ -12,6 +12,7 @@
 #include <linux/input.h>
 #include <linux/mfd/adp5520.h>
 #include <linux/slab.h>
+#include <linux/device.h>
 
 struct adp5520_keys {
 	struct input_dev *input;
@@ -81,7 +82,7 @@ static int adp5520_keys_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	if (pdata == NULL) {
+	if (!pdata) {
 		dev_err(&pdev->dev, "missing platform data\n");
 		return -EINVAL;
 	}
@@ -89,17 +90,15 @@ static int adp5520_keys_probe(struct platform_device *pdev)
 	if (!(pdata->rows_en_mask && pdata->cols_en_mask))
 		return -EINVAL;
 
-	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
-	if (dev == NULL) {
+	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
 		dev_err(&pdev->dev, "failed to alloc memory\n");
 		return -ENOMEM;
 	}
 
-	input = input_allocate_device();
-	if (!input) {
-		ret = -ENOMEM;
-		goto err;
-	}
+	input = devm_input_allocate_device(&pdev->dev);
+	if (!input)
+		return -ENOMEM;
 
 	dev->master = pdev->dev.parent;
 	dev->input = input;
@@ -135,7 +134,7 @@ static int adp5520_keys_probe(struct platform_device *pdev)
 	ret = input_register_device(input);
 	if (ret) {
 		dev_err(&pdev->dev, "unable to register input device\n");
-		goto err;
+		return ret;
 	}
 
 	en_mask = pdata->rows_en_mask | pdata->cols_en_mask;
@@ -157,8 +156,7 @@ static int adp5520_keys_probe(struct platform_device *pdev)
 
 	if (ret) {
 		dev_err(&pdev->dev, "failed to write\n");
-		ret = -EIO;
-		goto err1;
+		return -EIO;
 	}
 
 	dev->notifier.notifier_call = adp5520_keys_notifier;
@@ -166,19 +164,11 @@ static int adp5520_keys_probe(struct platform_device *pdev)
 			ADP5520_KP_IEN | ADP5520_KR_IEN);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to register notifier\n");
-		goto err1;
+		return ret;
 	}
 
 	platform_set_drvdata(pdev, dev);
 	return 0;
-
-err1:
-	input_unregister_device(input);
-	input = NULL;
-err:
-	input_free_device(input);
-	kfree(dev);
-	return ret;
 }
 
 static int adp5520_keys_remove(struct platform_device *pdev)
@@ -188,8 +178,6 @@ static int adp5520_keys_remove(struct platform_device *pdev)
 	adp5520_unregister_notifier(dev->master, &dev->notifier,
 				ADP5520_KP_IEN | ADP5520_KR_IEN);
 
-	input_unregister_device(dev->input);
-	kfree(dev);
 	return 0;
 }
 
diff --git a/drivers/input/keyboard/clps711x-keypad.c b/drivers/input/keyboard/clps711x-keypad.c
index 3955aecee44..552b65c6e6b 100644
--- a/drivers/input/keyboard/clps711x-keypad.c
+++ b/drivers/input/keyboard/clps711x-keypad.c
@@ -185,7 +185,7 @@ static int clps711x_keypad_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id clps711x_keypad_of_match[] = {
+static const struct of_device_id clps711x_keypad_of_match[] = {
 	{ .compatible = "cirrus,clps711x-keypad", },
 	{ }
 };
diff --git a/drivers/input/keyboard/gpio_keys.c b/drivers/input/keyboard/gpio_keys.c
index 2db13246eb8..8c98e97f8e4 100644
--- a/drivers/input/keyboard/gpio_keys.c
+++ b/drivers/input/keyboard/gpio_keys.c
@@ -424,6 +424,16 @@ out:
 	return IRQ_HANDLED;
 }
 
+static void gpio_keys_quiesce_key(void *data)
+{
+	struct gpio_button_data *bdata = data;
+
+	if (bdata->timer_debounce)
+		del_timer_sync(&bdata->timer);
+
+	cancel_work_sync(&bdata->work);
+}
+
 static int gpio_keys_setup_key(struct platform_device *pdev,
 				struct input_dev *input,
 				struct gpio_button_data *bdata,
@@ -433,7 +443,8 @@ static int gpio_keys_setup_key(struct platform_device *pdev,
 	struct device *dev = &pdev->dev;
 	irq_handler_t isr;
 	unsigned long irqflags;
-	int irq, error;
+	int irq;
+	int error;
 
 	bdata->input = input;
 	bdata->button = button;
@@ -441,7 +452,8 @@ static int gpio_keys_setup_key(struct platform_device *pdev,
 
 	if (gpio_is_valid(button->gpio)) {
 
-		error = gpio_request_one(button->gpio, GPIOF_IN, desc);
+		error = devm_gpio_request_one(&pdev->dev, button->gpio,
+					      GPIOF_IN, desc);
 		if (error < 0) {
 			dev_err(dev, "Failed to request GPIO %d, error %d\n",
 				button->gpio, error);
@@ -463,7 +475,7 @@ static int gpio_keys_setup_key(struct platform_device *pdev,
 			dev_err(dev,
 				"Unable to get irq number for GPIO %d, error %d\n",
 				button->gpio, error);
-			goto fail;
+			return error;
 		}
 		bdata->irq = irq;
 
@@ -497,26 +509,33 @@ static int gpio_keys_setup_key(struct platform_device *pdev,
 	input_set_capability(input, button->type ?: EV_KEY, button->code);
 
 	/*
+	 * Install custom action to cancel debounce timer and
+	 * workqueue item.
+	 */
+	error = devm_add_action(&pdev->dev, gpio_keys_quiesce_key, bdata);
+	if (error) {
+		dev_err(&pdev->dev,
+			"failed to register quiesce action, error: %d\n",
+			error);
+		return error;
+	}
+
+	/*
 	 * If platform has specified that the button can be disabled,
 	 * we don't want it to share the interrupt line.
 	 */
 	if (!button->can_disable)
 		irqflags |= IRQF_SHARED;
 
-	error = request_any_context_irq(bdata->irq, isr, irqflags, desc, bdata);
+	error = devm_request_any_context_irq(&pdev->dev, bdata->irq,
+					     isr, irqflags, desc, bdata);
 	if (error < 0) {
 		dev_err(dev, "Unable to claim irq %d; error %d\n",
 			bdata->irq, error);
-		goto fail;
+		return error;
 	}
 
 	return 0;
-
-fail:
-	if (gpio_is_valid(button->gpio))
-		gpio_free(button->gpio);
-
-	return error;
 }
 
 static void gpio_keys_report_state(struct gpio_keys_drvdata *ddata)
@@ -578,23 +597,18 @@ gpio_keys_get_devtree_pdata(struct device *dev)
 	int i;
 
 	node = dev->of_node;
-	if (!node) {
-		error = -ENODEV;
-		goto err_out;
-	}
+	if (!node)
+		return ERR_PTR(-ENODEV);
 
 	nbuttons = of_get_child_count(node);
-	if (nbuttons == 0) {
-		error = -ENODEV;
-		goto err_out;
-	}
+	if (nbuttons == 0)
+		return ERR_PTR(-ENODEV);
 
-	pdata = kzalloc(sizeof(*pdata) + nbuttons * (sizeof *button),
-			GFP_KERNEL);
-	if (!pdata) {
-		error = -ENOMEM;
-		goto err_out;
-	}
+	pdata = devm_kzalloc(dev,
+			     sizeof(*pdata) + nbuttons * sizeof(*button),
+			     GFP_KERNEL);
+	if (!pdata)
+		return ERR_PTR(-ENOMEM);
 
 	pdata->buttons = (struct gpio_keys_button *)(pdata + 1);
 	pdata->nbuttons = nbuttons;
@@ -619,7 +633,7 @@ gpio_keys_get_devtree_pdata(struct device *dev)
 				dev_err(dev,
 					"Failed to get gpio flags, error: %d\n",
 					error);
-			goto err_free_pdata;
+			return ERR_PTR(error);
 		}
 
 		button = &pdata->buttons[i++];
@@ -630,8 +644,7 @@ gpio_keys_get_devtree_pdata(struct device *dev)
 		if (of_property_read_u32(pp, "linux,code", &button->code)) {
 			dev_err(dev, "Button without keycode: 0x%x\n",
 				button->gpio);
-			error = -EINVAL;
-			goto err_free_pdata;
+			return ERR_PTR(-EINVAL);
 		}
 
 		button->desc = of_get_property(pp, "label", NULL);
@@ -646,20 +659,13 @@ gpio_keys_get_devtree_pdata(struct device *dev)
 			button->debounce_interval = 5;
 	}
 
-	if (pdata->nbuttons == 0) {
-		error = -EINVAL;
-		goto err_free_pdata;
-	}
+	if (pdata->nbuttons == 0)
+		return ERR_PTR(-EINVAL);
 
 	return pdata;
-
-err_free_pdata:
-	kfree(pdata);
-err_out:
-	return ERR_PTR(error);
 }
 
-static struct of_device_id gpio_keys_of_match[] = {
+static const struct of_device_id gpio_keys_of_match[] = {
 	{ .compatible = "gpio-keys", },
 	{ },
 };
@@ -675,22 +681,13 @@ gpio_keys_get_devtree_pdata(struct device *dev)
 
 #endif
 
-static void gpio_remove_key(struct gpio_button_data *bdata)
-{
-	free_irq(bdata->irq, bdata);
-	if (bdata->timer_debounce)
-		del_timer_sync(&bdata->timer);
-	cancel_work_sync(&bdata->work);
-	if (gpio_is_valid(bdata->button->gpio))
-		gpio_free(bdata->button->gpio);
-}
-
 static int gpio_keys_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
 	const struct gpio_keys_platform_data *pdata = dev_get_platdata(dev);
 	struct gpio_keys_drvdata *ddata;
 	struct input_dev *input;
+	size_t size;
 	int i, error;
 	int wakeup = 0;
 
@@ -700,14 +697,18 @@ static int gpio_keys_probe(struct platform_device *pdev)
 			return PTR_ERR(pdata);
 	}
 
-	ddata = kzalloc(sizeof(struct gpio_keys_drvdata) +
-			pdata->nbuttons * sizeof(struct gpio_button_data),
-			GFP_KERNEL);
-	input = input_allocate_device();
-	if (!ddata || !input) {
+	size = sizeof(struct gpio_keys_drvdata) +
+			pdata->nbuttons * sizeof(struct gpio_button_data);
+	ddata = devm_kzalloc(dev, size, GFP_KERNEL);
+	if (!ddata) {
 		dev_err(dev, "failed to allocate state\n");
-		error = -ENOMEM;
-		goto fail1;
+		return -ENOMEM;
+	}
+
+	input = devm_input_allocate_device(dev);
+	if (!input) {
+		dev_err(dev, "failed to allocate input device\n");
+		return -ENOMEM;
 	}
 
 	ddata->pdata = pdata;
@@ -738,7 +739,7 @@ static int gpio_keys_probe(struct platform_device *pdev)
 
 		error = gpio_keys_setup_key(pdev, input, bdata, button);
 		if (error)
-			goto fail2;
+			return error;
 
 		if (button->wakeup)
 			wakeup = 1;
@@ -748,57 +749,31 @@ static int gpio_keys_probe(struct platform_device *pdev)
 	if (error) {
 		dev_err(dev, "Unable to export keys/switches, error: %d\n",
 			error);
-		goto fail2;
+		return error;
 	}
 
 	error = input_register_device(input);
 	if (error) {
 		dev_err(dev, "Unable to register input device, error: %d\n",
 			error);
-		goto fail3;
+		goto err_remove_group;
 	}
 
 	device_init_wakeup(&pdev->dev, wakeup);
 
 	return 0;
 
- fail3:
+err_remove_group:
 	sysfs_remove_group(&pdev->dev.kobj, &gpio_keys_attr_group);
- fail2:
-	while (--i >= 0)
-		gpio_remove_key(&ddata->data[i]);
-
- fail1:
-	input_free_device(input);
-	kfree(ddata);
-	/* If we have no platform data, we allocated pdata dynamically. */
-	if (!dev_get_platdata(&pdev->dev))
-		kfree(pdata);
-
 	return error;
 }
 
 static int gpio_keys_remove(struct platform_device *pdev)
 {
-	struct gpio_keys_drvdata *ddata = platform_get_drvdata(pdev);
-	struct input_dev *input = ddata->input;
-	int i;
-
 	sysfs_remove_group(&pdev->dev.kobj, &gpio_keys_attr_group);
 
 	device_init_wakeup(&pdev->dev, 0);
 
-	for (i = 0; i < ddata->pdata->nbuttons; i++)
-		gpio_remove_key(&ddata->data[i]);
-
-	input_unregister_device(input);
-
-	/* If we have no platform data, we allocated pdata dynamically. */
-	if (!dev_get_platdata(&pdev->dev))
-		kfree(ddata->pdata);
-
-	kfree(ddata);
-
 	return 0;
 }
 
diff --git a/drivers/input/keyboard/gpio_keys_polled.c b/drivers/input/keyboard/gpio_keys_polled.c
index e571e194ff8..432d36395f3 100644
--- a/drivers/input/keyboard/gpio_keys_polled.c
+++ b/drivers/input/keyboard/gpio_keys_polled.c
@@ -120,12 +120,10 @@ static struct gpio_keys_platform_data *gpio_keys_polled_get_devtree_pdata(struct
 	if (nbuttons == 0)
 		return NULL;
 
-	pdata = kzalloc(sizeof(*pdata) + nbuttons * (sizeof *button),
-			GFP_KERNEL);
-	if (!pdata) {
-		error = -ENOMEM;
-		goto err_out;
-	}
+	pdata = devm_kzalloc(dev, sizeof(*pdata) + nbuttons * sizeof(*button),
+			     GFP_KERNEL);
+	if (!pdata)
+		return ERR_PTR(-ENOMEM);
 
 	pdata->buttons = (struct gpio_keys_button *)(pdata + 1);
 	pdata->nbuttons = nbuttons;
@@ -151,7 +149,7 @@ static struct gpio_keys_platform_data *gpio_keys_polled_get_devtree_pdata(struct
 				dev_err(dev,
 					"Failed to get gpio flags, error: %d\n",
 					error);
-			goto err_free_pdata;
+			return ERR_PTR(error);
 		}
 
 		button = &pdata->buttons[i++];
@@ -162,8 +160,7 @@ static struct gpio_keys_platform_data *gpio_keys_polled_get_devtree_pdata(struct
 		if (of_property_read_u32(pp, "linux,code", &button->code)) {
 			dev_err(dev, "Button without keycode: 0x%x\n",
 				button->gpio);
-			error = -EINVAL;
-			goto err_free_pdata;
+			return ERR_PTR(-EINVAL);
 		}
 
 		button->desc = of_get_property(pp, "label", NULL);
@@ -178,20 +175,13 @@ static struct gpio_keys_platform_data *gpio_keys_polled_get_devtree_pdata(struct
 			button->debounce_interval = 5;
 	}
 
-	if (pdata->nbuttons == 0) {
-		error = -EINVAL;
-		goto err_free_pdata;
-	}
+	if (pdata->nbuttons == 0)
+		return ERR_PTR(-EINVAL);
 
 	return pdata;
-
-err_free_pdata:
-	kfree(pdata);
-err_out:
-	return ERR_PTR(error);
 }
 
-static struct of_device_id gpio_keys_polled_of_match[] = {
+static const struct of_device_id gpio_keys_polled_of_match[] = {
 	{ .compatible = "gpio-keys-polled", },
 	{ },
 };
@@ -213,6 +203,7 @@ static int gpio_keys_polled_probe(struct platform_device *pdev)
 	struct gpio_keys_polled_dev *bdev;
 	struct input_polled_dev *poll_dev;
 	struct input_dev *input;
+	size_t size;
 	int error;
 	int i;
 
@@ -228,24 +219,21 @@ static int gpio_keys_polled_probe(struct platform_device *pdev)
 
 	if (!pdata->poll_interval) {
 		dev_err(dev, "missing poll_interval value\n");
-		error = -EINVAL;
-		goto err_free_pdata;
+		return -EINVAL;
 	}
 
-	bdev = kzalloc(sizeof(struct gpio_keys_polled_dev) +
-		       pdata->nbuttons * sizeof(struct gpio_keys_button_data),
-		       GFP_KERNEL);
+	size = sizeof(struct gpio_keys_polled_dev) +
+			pdata->nbuttons * sizeof(struct gpio_keys_button_data);
+	bdev = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
 	if (!bdev) {
 		dev_err(dev, "no memory for private data\n");
-		error = -ENOMEM;
-		goto err_free_pdata;
+		return -ENOMEM;
 	}
 
-	poll_dev = input_allocate_polled_device();
+	poll_dev = devm_input_allocate_polled_device(&pdev->dev);
 	if (!poll_dev) {
 		dev_err(dev, "no memory for polled device\n");
-		error = -ENOMEM;
-		goto err_free_bdev;
+		return -ENOMEM;
 	}
 
 	poll_dev->private = bdev;
@@ -258,7 +246,6 @@ static int gpio_keys_polled_probe(struct platform_device *pdev)
 
 	input->name = pdev->name;
 	input->phys = DRV_NAME"/input0";
-	input->dev.parent = &pdev->dev;
 
 	input->id.bustype = BUS_HOST;
 	input->id.vendor = 0x0001;
@@ -277,16 +264,15 @@ static int gpio_keys_polled_probe(struct platform_device *pdev)
 
 		if (button->wakeup) {
 			dev_err(dev, DRV_NAME " does not support wakeup\n");
-			error = -EINVAL;
-			goto err_free_gpio;
+			return -EINVAL;
 		}
 
-		error = gpio_request_one(gpio, GPIOF_IN,
-					 button->desc ?: DRV_NAME);
+		error = devm_gpio_request_one(&pdev->dev, gpio, GPIOF_IN,
+					      button->desc ? : DRV_NAME);
 		if (error) {
 			dev_err(dev, "unable to claim gpio %u, err=%d\n",
 				gpio, error);
-			goto err_free_gpio;
+			return error;
 		}
 
 		bdata->can_sleep = gpio_cansleep(gpio);
@@ -306,7 +292,7 @@ static int gpio_keys_polled_probe(struct platform_device *pdev)
 	if (error) {
 		dev_err(dev, "unable to register polled device, err=%d\n",
 			error);
-		goto err_free_gpio;
+		return error;
 	}
 
 	/* report initial state of the buttons */
@@ -315,52 +301,10 @@ static int gpio_keys_polled_probe(struct platform_device *pdev)
 					     &bdev->data[i]);
 
 	return 0;
-
-err_free_gpio:
-	while (--i >= 0)
-		gpio_free(pdata->buttons[i].gpio);
-
-	input_free_polled_device(poll_dev);
-
-err_free_bdev:
-	kfree(bdev);
-
-err_free_pdata:
-	/* If we have no platform_data, we allocated pdata dynamically.  */
-	if (!dev_get_platdata(&pdev->dev))
-		kfree(pdata);
-
-	return error;
-}
-
-static int gpio_keys_polled_remove(struct platform_device *pdev)
-{
-	struct gpio_keys_polled_dev *bdev = platform_get_drvdata(pdev);
-	const struct gpio_keys_platform_data *pdata = bdev->pdata;
-	int i;
-
-	input_unregister_polled_device(bdev->poll_dev);
-
-	for (i = 0; i < pdata->nbuttons; i++)
-		gpio_free(pdata->buttons[i].gpio);
-
-	input_free_polled_device(bdev->poll_dev);
-
-	/*
-	 * If we had no platform_data, we allocated pdata dynamically and
-	 * must free it here.
-	 */
-	if (!dev_get_platdata(&pdev->dev))
-		kfree(pdata);
-
-	kfree(bdev);
-
-	return 0;
 }
 
 static struct platform_driver gpio_keys_polled_driver = {
 	.probe	= gpio_keys_polled_probe,
-	.remove	= gpio_keys_polled_remove,
 	.driver	= {
 		.name	= DRV_NAME,
 		.owner	= THIS_MODULE,
diff --git a/drivers/input/keyboard/imx_keypad.c b/drivers/input/keyboard/imx_keypad.c
index 97ec33572e5..8280cb16260 100644
--- a/drivers/input/keyboard/imx_keypad.c
+++ b/drivers/input/keyboard/imx_keypad.c
@@ -415,7 +415,7 @@ open_err:
 }
 
 #ifdef CONFIG_OF
-static struct of_device_id imx_keypad_of_match[] = {
+static const struct of_device_id imx_keypad_of_match[] = {
 	{ .compatible = "fsl,imx21-kpp", },
 	{ /* sentinel */ }
 };
diff --git a/drivers/input/keyboard/jornada680_kbd.c b/drivers/input/keyboard/jornada680_kbd.c
index 69b1f002ff5..0ba4428da24 100644
--- a/drivers/input/keyboard/jornada680_kbd.c
+++ b/drivers/input/keyboard/jornada680_kbd.c
@@ -16,6 +16,7 @@
  * published by the Free Software Foundation.
  */
 
+#include <linux/device.h>
 #include <linux/input.h>
 #include <linux/input-polldev.h>
 #include <linux/interrupt.h>
@@ -185,14 +186,15 @@ static int jornada680kbd_probe(struct platform_device *pdev)
 	struct input_dev *input_dev;
 	int i, error;
 
-	jornadakbd = kzalloc(sizeof(struct jornadakbd), GFP_KERNEL);
+	jornadakbd = devm_kzalloc(&pdev->dev, sizeof(struct jornadakbd),
+				  GFP_KERNEL);
 	if (!jornadakbd)
 		return -ENOMEM;
 
-	poll_dev = input_allocate_polled_device();
+	poll_dev = devm_input_allocate_polled_device(&pdev->dev);
 	if (!poll_dev) {
-		error = -ENOMEM;
-		goto failed;
+		dev_err(&pdev->dev, "failed to allocate polled input device\n");
+		return -ENOMEM;
 	}
 
 	platform_set_drvdata(pdev, jornadakbd);
@@ -224,27 +226,10 @@ static int jornada680kbd_probe(struct platform_device *pdev)
 	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
 
 	error = input_register_polled_device(jornadakbd->poll_dev);
-	if (error)
-		goto failed;
-
-	return 0;
-
- failed:
-	printk(KERN_ERR "Jornadakbd: failed to register driver, error: %d\n",
-		error);
-	input_free_polled_device(poll_dev);
-	kfree(jornadakbd);
-	return error;
-
-}
-
-static int jornada680kbd_remove(struct platform_device *pdev)
-{
-	struct jornadakbd *jornadakbd = platform_get_drvdata(pdev);
-
-	input_unregister_polled_device(jornadakbd->poll_dev);
-	input_free_polled_device(jornadakbd->poll_dev);
-	kfree(jornadakbd);
+	if (error) {
+		dev_err(&pdev->dev, "failed to register polled input device\n");
+		return error;
+	}
 
 	return 0;
 }
@@ -255,7 +240,6 @@ static struct platform_driver jornada680kbd_driver = {
 		.owner	= THIS_MODULE,
 	},
 	.probe	= jornada680kbd_probe,
-	.remove	= jornada680kbd_remove,
 };
 module_platform_driver(jornada680kbd_driver);
 
diff --git a/drivers/input/keyboard/mcs_touchkey.c b/drivers/input/keyboard/mcs_touchkey.c
index 1da8e0b44b5..375b05ca8e2 100644
--- a/drivers/input/keyboard/mcs_touchkey.c
+++ b/drivers/input/keyboard/mcs_touchkey.c
@@ -147,7 +147,7 @@ static int mcs_touchkey_probe(struct i2c_client *client,
 	}
 	dev_info(&client->dev, "Firmware version: %d\n", fw_ver);
 
-	input_dev->name = "MELPAS MCS Touchkey";
+	input_dev->name = "MELFAS MCS Touchkey";
 	input_dev->id.bustype = BUS_I2C;
 	input_dev->dev.parent = &client->dev;
 	input_dev->evbit[0] = BIT_MASK(EV_KEY);
diff --git a/drivers/input/keyboard/omap4-keypad.c b/drivers/input/keyboard/omap4-keypad.c
index 0400b3f2b4b..024b7bdffe5 100644
--- a/drivers/input/keyboard/omap4-keypad.c
+++ b/drivers/input/keyboard/omap4-keypad.c
@@ -28,11 +28,10 @@
 #include <linux/io.h>
 #include <linux/of.h>
 #include <linux/input.h>
+#include <linux/input/matrix_keypad.h>
 #include <linux/slab.h>
 #include <linux/pm_runtime.h>
 
-#include <linux/platform_data/omap4-keypad.h>
-
 /* OMAP4 registers */
 #define OMAP4_KBD_REVISION		0x00
 #define OMAP4_KBD_SYSCONFIG		0x10
@@ -218,7 +217,6 @@ static void omap4_keypad_close(struct input_dev *input)
 	pm_runtime_put_sync(input->dev.parent);
 }
 
-#ifdef CONFIG_OF
 static int omap4_keypad_parse_dt(struct device *dev,
 				 struct omap4_keypad *keypad_data)
 {
@@ -235,20 +233,9 @@ static int omap4_keypad_parse_dt(struct device *dev,
 
 	return 0;
 }
-#else
-static inline int omap4_keypad_parse_dt(struct device *dev,
-					struct omap4_keypad *keypad_data)
-{
-	return -ENOSYS;
-}
-#endif
 
 static int omap4_keypad_probe(struct platform_device *pdev)
 {
-	const struct omap4_keypad_platform_data *pdata =
-				dev_get_platdata(&pdev->dev);
-	const struct matrix_keymap_data *keymap_data =
-				pdata ? pdata->keymap_data : NULL;
 	struct omap4_keypad *keypad_data;
 	struct input_dev *input_dev;
 	struct resource *res;
@@ -277,14 +264,9 @@ static int omap4_keypad_probe(struct platform_device *pdev)
 
 	keypad_data->irq = irq;
 
-	if (pdata) {
-		keypad_data->rows = pdata->rows;
-		keypad_data->cols = pdata->cols;
-	} else {
-		error = omap4_keypad_parse_dt(&pdev->dev, keypad_data);
-		if (error)
-			return error;
-	}
+	error = omap4_keypad_parse_dt(&pdev->dev, keypad_data);
+	if (error)
+		return error;
 
 	res = request_mem_region(res->start, resource_size(res), pdev->name);
 	if (!res) {
@@ -363,7 +345,7 @@ static int omap4_keypad_probe(struct platform_device *pdev)
 		goto err_free_input;
 	}
 
-	error = matrix_keypad_build_keymap(keymap_data, NULL,
+	error = matrix_keypad_build_keymap(NULL, NULL,
 					   keypad_data->rows, keypad_data->cols,
 					   keypad_data->keymap, input_dev);
 	if (error) {
@@ -434,13 +416,11 @@ static int omap4_keypad_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_OF
 static const struct of_device_id omap_keypad_dt_match[] = {
 	{ .compatible = "ti,omap4-keypad" },
 	{},
 };
 MODULE_DEVICE_TABLE(of, omap_keypad_dt_match);
-#endif
 
 #ifdef CONFIG_PM_SLEEP
 static int omap4_keypad_suspend(struct device *dev)
@@ -482,7 +462,7 @@ static struct platform_driver omap4_keypad_driver = {
 		.name	= "omap4-keypad",
 		.owner	= THIS_MODULE,
 		.pm	= &omap4_keypad_pm_ops,
-		.of_match_table = of_match_ptr(omap_keypad_dt_match),
+		.of_match_table = omap_keypad_dt_match,
 	},
 };
 module_platform_driver(omap4_keypad_driver);
diff --git a/drivers/input/keyboard/pxa27x_keypad.c b/drivers/input/keyboard/pxa27x_keypad.c
index d8241ba0afa..a15063bea70 100644
--- a/drivers/input/keyboard/pxa27x_keypad.c
+++ b/drivers/input/keyboard/pxa27x_keypad.c
@@ -111,6 +111,8 @@ struct pxa27x_keypad {
 	unsigned short keycodes[MAX_KEYPAD_KEYS];
 	int rotary_rel_code[2];
 
+	unsigned int row_shift;
+
 	/* state row bits of each column scan */
 	uint32_t matrix_key_state[MAX_MATRIX_KEY_COLS];
 	uint32_t direct_key_state;
@@ -467,7 +469,8 @@ scan:
 			if ((bits_changed & (1 << row)) == 0)
 				continue;
 
-			code = MATRIX_SCAN_CODE(row, col, MATRIX_ROW_SHIFT);
+			code = MATRIX_SCAN_CODE(row, col, keypad->row_shift);
+
 			input_event(input_dev, EV_MSC, MSC_SCAN, code);
 			input_report_key(input_dev, keypad->keycodes[code],
 					 new_state[col] & (1 << row));
@@ -802,6 +805,8 @@ static int pxa27x_keypad_probe(struct platform_device *pdev)
 		goto failed_put_clk;
 	}
 
+	keypad->row_shift = get_count_order(pdata->matrix_key_cols);
+
 	if ((pdata->enable_rotary0 && keypad->rotary_rel_code[0] != -1) ||
 	    (pdata->enable_rotary1 && keypad->rotary_rel_code[1] != -1)) {
 		input_dev->evbit[0] |= BIT_MASK(EV_REL);
diff --git a/drivers/input/keyboard/st-keyscan.c b/drivers/input/keyboard/st-keyscan.c
new file mode 100644
index 00000000000..758b4873141
--- /dev/null
+++ b/drivers/input/keyboard/st-keyscan.c
@@ -0,0 +1,274 @@
+/*
+ * STMicroelectronics Key Scanning driver
+ *
+ * Copyright (c) 2014 STMicroelectonics Ltd.
+ * Author: Stuart Menefy <stuart.menefy@st.com>
+ *
+ * Based on sh_keysc.c, copyright 2008 Magnus Damm
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/input/matrix_keypad.h>
+
+#define ST_KEYSCAN_MAXKEYS 16
+
+#define KEYSCAN_CONFIG_OFF		0x0
+#define KEYSCAN_CONFIG_ENABLE		0x1
+#define KEYSCAN_DEBOUNCE_TIME_OFF	0x4
+#define KEYSCAN_MATRIX_STATE_OFF	0x8
+#define KEYSCAN_MATRIX_DIM_OFF		0xc
+#define KEYSCAN_MATRIX_DIM_X_SHIFT	0x0
+#define KEYSCAN_MATRIX_DIM_Y_SHIFT	0x2
+
+struct st_keyscan {
+	void __iomem *base;
+	int irq;
+	struct clk *clk;
+	struct input_dev *input_dev;
+	unsigned long last_state;
+	unsigned int n_rows;
+	unsigned int n_cols;
+	unsigned int debounce_us;
+};
+
+static irqreturn_t keyscan_isr(int irq, void *dev_id)
+{
+	struct st_keyscan *keypad = dev_id;
+	unsigned short *keycode = keypad->input_dev->keycode;
+	unsigned long state, change;
+	int bit_nr;
+
+	state = readl(keypad->base + KEYSCAN_MATRIX_STATE_OFF) & 0xffff;
+	change = keypad->last_state ^ state;
+	keypad->last_state = state;
+
+	for_each_set_bit(bit_nr, &change, BITS_PER_LONG)
+		input_report_key(keypad->input_dev,
+				 keycode[bit_nr], state & BIT(bit_nr));
+
+	input_sync(keypad->input_dev);
+
+	return IRQ_HANDLED;
+}
+
+static int keyscan_start(struct st_keyscan *keypad)
+{
+	int error;
+
+	error = clk_enable(keypad->clk);
+	if (error)
+		return error;
+
+	writel(keypad->debounce_us * (clk_get_rate(keypad->clk) / 1000000),
+	       keypad->base + KEYSCAN_DEBOUNCE_TIME_OFF);
+
+	writel(((keypad->n_cols - 1) << KEYSCAN_MATRIX_DIM_X_SHIFT) |
+	       ((keypad->n_rows - 1) << KEYSCAN_MATRIX_DIM_Y_SHIFT),
+	       keypad->base + KEYSCAN_MATRIX_DIM_OFF);
+
+	writel(KEYSCAN_CONFIG_ENABLE, keypad->base + KEYSCAN_CONFIG_OFF);
+
+	return 0;
+}
+
+static void keyscan_stop(struct st_keyscan *keypad)
+{
+	writel(0, keypad->base + KEYSCAN_CONFIG_OFF);
+
+	clk_disable(keypad->clk);
+}
+
+static int keyscan_open(struct input_dev *dev)
+{
+	struct st_keyscan *keypad = input_get_drvdata(dev);
+
+	return keyscan_start(keypad);
+}
+
+static void keyscan_close(struct input_dev *dev)
+{
+	struct st_keyscan *keypad = input_get_drvdata(dev);
+
+	keyscan_stop(keypad);
+}
+
+static int keypad_matrix_key_parse_dt(struct st_keyscan *keypad_data)
+{
+	struct device *dev = keypad_data->input_dev->dev.parent;
+	struct device_node *np = dev->of_node;
+	int error;
+
+	error = matrix_keypad_parse_of_params(dev, &keypad_data->n_rows,
+					      &keypad_data->n_cols);
+	if (error) {
+		dev_err(dev, "failed to parse keypad params\n");
+		return error;
+	}
+
+	of_property_read_u32(np, "st,debounce-us", &keypad_data->debounce_us);
+
+	dev_dbg(dev, "n_rows=%d n_col=%d debounce=%d\n",
+		keypad_data->n_rows, keypad_data->n_cols,
+		keypad_data->debounce_us);
+
+	return 0;
+}
+
+static int keyscan_probe(struct platform_device *pdev)
+{
+	struct st_keyscan *keypad_data;
+	struct input_dev *input_dev;
+	struct resource *res;
+	int error;
+
+	if (!pdev->dev.of_node) {
+		dev_err(&pdev->dev, "no DT data present\n");
+		return -EINVAL;
+	}
+
+	keypad_data = devm_kzalloc(&pdev->dev, sizeof(*keypad_data),
+				   GFP_KERNEL);
+	if (!keypad_data)
+		return -ENOMEM;
+
+	input_dev = devm_input_allocate_device(&pdev->dev);
+	if (!input_dev) {
+		dev_err(&pdev->dev, "failed to allocate the input device\n");
+		return -ENOMEM;
+	}
+
+	input_dev->name = pdev->name;
+	input_dev->phys = "keyscan-keys/input0";
+	input_dev->dev.parent = &pdev->dev;
+	input_dev->open = keyscan_open;
+	input_dev->close = keyscan_close;
+
+	input_dev->id.bustype = BUS_HOST;
+
+	error = keypad_matrix_key_parse_dt(keypad_data);
+	if (error)
+		return error;
+
+	error = matrix_keypad_build_keymap(NULL, NULL,
+					   keypad_data->n_rows,
+					   keypad_data->n_cols,
+					   NULL, input_dev);
+	if (error) {
+		dev_err(&pdev->dev, "failed to build keymap\n");
+		return error;
+	}
+
+	input_set_drvdata(input_dev, keypad_data);
+
+	keypad_data->input_dev = input_dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	keypad_data->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(keypad_data->base))
+		return PTR_ERR(keypad_data->base);
+
+	keypad_data->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(keypad_data->clk)) {
+		dev_err(&pdev->dev, "cannot get clock\n");
+		return PTR_ERR(keypad_data->clk);
+	}
+
+	error = clk_enable(keypad_data->clk);
+	if (error) {
+		dev_err(&pdev->dev, "failed to enable clock\n");
+		return error;
+	}
+
+	keyscan_stop(keypad_data);
+
+	keypad_data->irq = platform_get_irq(pdev, 0);
+	if (keypad_data->irq < 0) {
+		dev_err(&pdev->dev, "no IRQ specified\n");
+		return -EINVAL;
+	}
+
+	error = devm_request_irq(&pdev->dev, keypad_data->irq, keyscan_isr, 0,
+				 pdev->name, keypad_data);
+	if (error) {
+		dev_err(&pdev->dev, "failed to request IRQ\n");
+		return error;
+	}
+
+	error = input_register_device(input_dev);
+	if (error) {
+		dev_err(&pdev->dev, "failed to register input device\n");
+		return error;
+	}
+
+	platform_set_drvdata(pdev, keypad_data);
+
+	device_set_wakeup_capable(&pdev->dev, 1);
+
+	return 0;
+}
+
+static int keyscan_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct st_keyscan *keypad = platform_get_drvdata(pdev);
+	struct input_dev *input = keypad->input_dev;
+
+	mutex_lock(&input->mutex);
+
+	if (device_may_wakeup(dev))
+		enable_irq_wake(keypad->irq);
+	else if (input->users)
+		keyscan_stop(keypad);
+
+	mutex_unlock(&input->mutex);
+	return 0;
+}
+
+static int keyscan_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct st_keyscan *keypad = platform_get_drvdata(pdev);
+	struct input_dev *input = keypad->input_dev;
+	int retval = 0;
+
+	mutex_lock(&input->mutex);
+
+	if (device_may_wakeup(dev))
+		disable_irq_wake(keypad->irq);
+	else if (input->users)
+		retval = keyscan_start(keypad);
+
+	mutex_unlock(&input->mutex);
+	return retval;
+}
+
+static SIMPLE_DEV_PM_OPS(keyscan_dev_pm_ops, keyscan_suspend, keyscan_resume);
+
+static const struct of_device_id keyscan_of_match[] = {
+	{ .compatible = "st,sti-keyscan" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, keyscan_of_match);
+
+static struct platform_driver keyscan_device_driver = {
+	.probe		= keyscan_probe,
+	.driver		= {
+		.name	= "st-keyscan",
+		.pm	= &keyscan_dev_pm_ops,
+		.of_match_table = of_match_ptr(keyscan_of_match),
+	}
+};
+
+module_platform_driver(keyscan_device_driver);
+
+MODULE_AUTHOR("Stuart Menefy <stuart.menefy@st.com>");
+MODULE_DESCRIPTION("STMicroelectronics keyscan device driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/keyboard/tc3589x-keypad.c b/drivers/input/keyboard/tc3589x-keypad.c
index 74494a35752..ad7abae6907 100644
--- a/drivers/input/keyboard/tc3589x-keypad.c
+++ b/drivers/input/keyboard/tc3589x-keypad.c
@@ -296,6 +296,65 @@ static void tc3589x_keypad_close(struct input_dev *input)
 	tc3589x_keypad_disable(keypad);
 }
 
+#ifdef CONFIG_OF
+static const struct tc3589x_keypad_platform_data *
+tc3589x_keypad_of_probe(struct device *dev)
+{
+	struct device_node *np = dev->of_node;
+	struct tc3589x_keypad_platform_data *plat;
+	u32 cols, rows;
+	u32 debounce_ms;
+	int proplen;
+
+	if (!np)
+		return ERR_PTR(-ENODEV);
+
+	plat = devm_kzalloc(dev, sizeof(*plat), GFP_KERNEL);
+	if (!plat)
+		return ERR_PTR(-ENOMEM);
+
+	of_property_read_u32(np, "keypad,num-columns", &cols);
+	of_property_read_u32(np, "keypad,num-rows", &rows);
+	plat->kcol = (u8) cols;
+	plat->krow = (u8) rows;
+	if (!plat->krow || !plat->kcol ||
+	     plat->krow > TC_KPD_ROWS || plat->kcol > TC_KPD_COLUMNS) {
+		dev_err(dev,
+			"keypad columns/rows not properly specified (%ux%u)\n",
+			plat->kcol, plat->krow);
+		return ERR_PTR(-EINVAL);
+	}
+
+	if (!of_get_property(np, "linux,keymap", &proplen)) {
+		dev_err(dev, "property linux,keymap not found\n");
+		return ERR_PTR(-ENOENT);
+	}
+
+	plat->no_autorepeat = of_property_read_bool(np, "linux,no-autorepeat");
+	plat->enable_wakeup = of_property_read_bool(np, "linux,wakeup");
+
+	/* The custom delay format is ms/16 */
+	of_property_read_u32(np, "debounce-delay-ms", &debounce_ms);
+	if (debounce_ms)
+		plat->debounce_period = debounce_ms * 16;
+	else
+		plat->debounce_period = TC_KPD_DEBOUNCE_PERIOD;
+
+	plat->settle_time = TC_KPD_SETTLE_TIME;
+	/* FIXME: should be property of the IRQ resource? */
+	plat->irqtype = IRQF_TRIGGER_FALLING;
+
+	return plat;
+}
+#else
+static inline const struct tc3589x_keypad_platform_data *
+tc3589x_keypad_of_probe(struct device *dev)
+{
+	return ERR_PTR(-ENODEV);
+}
+#endif
+
+
 static int tc3589x_keypad_probe(struct platform_device *pdev)
 {
 	struct tc3589x *tc3589x = dev_get_drvdata(pdev->dev.parent);
@@ -306,8 +365,11 @@ static int tc3589x_keypad_probe(struct platform_device *pdev)
 
 	plat = tc3589x->pdata->keypad;
 	if (!plat) {
-		dev_err(&pdev->dev, "invalid keypad platform data\n");
-		return -EINVAL;
+		plat = tc3589x_keypad_of_probe(&pdev->dev);
+		if (IS_ERR(plat)) {
+			dev_err(&pdev->dev, "invalid keypad platform data\n");
+			return PTR_ERR(plat);
+		}
 	}
 
 	irq = platform_get_irq(pdev, 0);
diff --git a/drivers/input/misc/88pm860x_onkey.c b/drivers/input/misc/88pm860x_onkey.c
index abd8453e521..220ce0fa15d 100644
--- a/drivers/input/misc/88pm860x_onkey.c
+++ b/drivers/input/misc/88pm860x_onkey.c
@@ -26,6 +26,7 @@
 #include <linux/interrupt.h>
 #include <linux/mfd/88pm860x.h>
 #include <linux/slab.h>
+#include <linux/device.h>
 
 #define PM8607_WAKEUP		0x0b
 
@@ -68,7 +69,8 @@ static int pm860x_onkey_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	info = kzalloc(sizeof(struct pm860x_onkey_info), GFP_KERNEL);
+	info = devm_kzalloc(&pdev->dev, sizeof(struct pm860x_onkey_info),
+			    GFP_KERNEL);
 	if (!info)
 		return -ENOMEM;
 	info->chip = chip;
@@ -76,11 +78,10 @@ static int pm860x_onkey_probe(struct platform_device *pdev)
 	info->dev = &pdev->dev;
 	info->irq = irq;
 
-	info->idev = input_allocate_device();
+	info->idev = devm_input_allocate_device(&pdev->dev);
 	if (!info->idev) {
 		dev_err(chip->dev, "Failed to allocate input dev\n");
-		ret = -ENOMEM;
-		goto out;
+		return -ENOMEM;
 	}
 
 	info->idev->name = "88pm860x_on";
@@ -93,42 +94,22 @@ static int pm860x_onkey_probe(struct platform_device *pdev)
 	ret = input_register_device(info->idev);
 	if (ret) {
 		dev_err(chip->dev, "Can't register input device: %d\n", ret);
-		goto out_reg;
+		return ret;
 	}
 
-	ret = request_threaded_irq(info->irq, NULL, pm860x_onkey_handler,
-				   IRQF_ONESHOT, "onkey", info);
+	ret = devm_request_threaded_irq(&pdev->dev, info->irq, NULL,
+					pm860x_onkey_handler, IRQF_ONESHOT,
+					"onkey", info);
 	if (ret < 0) {
 		dev_err(chip->dev, "Failed to request IRQ: #%d: %d\n",
 			info->irq, ret);
-		goto out_irq;
+		return ret;
 	}
 
 	platform_set_drvdata(pdev, info);
 	device_init_wakeup(&pdev->dev, 1);
 
 	return 0;
-
-out_irq:
-	input_unregister_device(info->idev);
-	kfree(info);
-	return ret;
-
-out_reg:
-	input_free_device(info->idev);
-out:
-	kfree(info);
-	return ret;
-}
-
-static int pm860x_onkey_remove(struct platform_device *pdev)
-{
-	struct pm860x_onkey_info *info = platform_get_drvdata(pdev);
-
-	free_irq(info->irq, info);
-	input_unregister_device(info->idev);
-	kfree(info);
-	return 0;
 }
 
 #ifdef CONFIG_PM_SLEEP
@@ -161,7 +142,6 @@ static struct platform_driver pm860x_onkey_driver = {
 		.pm	= &pm860x_onkey_pm_ops,
 	},
 	.probe		= pm860x_onkey_probe,
-	.remove		= pm860x_onkey_remove,
 };
 module_platform_driver(pm860x_onkey_driver);
 
diff --git a/drivers/input/misc/Kconfig b/drivers/input/misc/Kconfig
index 5928ea71dd6..2ff4425a893 100644
--- a/drivers/input/misc/Kconfig
+++ b/drivers/input/misc/Kconfig
@@ -224,7 +224,7 @@ config INPUT_GP2A
 
 config INPUT_GPIO_BEEPER
 	tristate "Generic GPIO Beeper support"
-	depends on OF_GPIO
+	depends on GPIOLIB
 	help
 	  Say Y here if you have a beeper connected to a GPIO pin.
 
diff --git a/drivers/input/misc/ab8500-ponkey.c b/drivers/input/misc/ab8500-ponkey.c
index f2fbdd88ed2..95ef7dd6442 100644
--- a/drivers/input/misc/ab8500-ponkey.c
+++ b/drivers/input/misc/ab8500-ponkey.c
@@ -7,6 +7,7 @@
  * AB8500 Power-On Key handler
  */
 
+#include <linux/device.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
@@ -65,12 +66,14 @@ static int ab8500_ponkey_probe(struct platform_device *pdev)
 		return irq_dbr;
 	}
 
-	ponkey = kzalloc(sizeof(struct ab8500_ponkey), GFP_KERNEL);
-	input = input_allocate_device();
-	if (!ponkey || !input) {
-		error = -ENOMEM;
-		goto err_free_mem;
-	}
+	ponkey = devm_kzalloc(&pdev->dev, sizeof(struct ab8500_ponkey),
+			      GFP_KERNEL);
+	if (!ponkey)
+		return -ENOMEM;
+
+	input = devm_input_allocate_device(&pdev->dev);
+	if (!input)
+		return -ENOMEM;
 
 	ponkey->idev = input;
 	ponkey->ab8500 = ab8500;
@@ -82,52 +85,32 @@ static int ab8500_ponkey_probe(struct platform_device *pdev)
 
 	input_set_capability(input, EV_KEY, KEY_POWER);
 
-	error = request_any_context_irq(ponkey->irq_dbf, ab8500_ponkey_handler,
-					0, "ab8500-ponkey-dbf", ponkey);
+	error = devm_request_any_context_irq(&pdev->dev, ponkey->irq_dbf,
+					     ab8500_ponkey_handler, 0,
+					     "ab8500-ponkey-dbf", ponkey);
 	if (error < 0) {
 		dev_err(ab8500->dev, "Failed to request dbf IRQ#%d: %d\n",
 			ponkey->irq_dbf, error);
-		goto err_free_mem;
+		return error;
 	}
 
-	error = request_any_context_irq(ponkey->irq_dbr, ab8500_ponkey_handler,
-					0, "ab8500-ponkey-dbr", ponkey);
+	error = devm_request_any_context_irq(&pdev->dev, ponkey->irq_dbr,
+					     ab8500_ponkey_handler, 0,
+					     "ab8500-ponkey-dbr", ponkey);
 	if (error < 0) {
 		dev_err(ab8500->dev, "Failed to request dbr IRQ#%d: %d\n",
 			ponkey->irq_dbr, error);
-		goto err_free_dbf_irq;
+		return error;
 	}
 
 	error = input_register_device(ponkey->idev);
 	if (error) {
 		dev_err(ab8500->dev, "Can't register input device: %d\n", error);
-		goto err_free_dbr_irq;
+		return error;
 	}
 
 	platform_set_drvdata(pdev, ponkey);
 	return 0;
-
-err_free_dbr_irq:
-	free_irq(ponkey->irq_dbr, ponkey);
-err_free_dbf_irq:
-	free_irq(ponkey->irq_dbf, ponkey);
-err_free_mem:
-	input_free_device(input);
-	kfree(ponkey);
-
-	return error;
-}
-
-static int ab8500_ponkey_remove(struct platform_device *pdev)
-{
-	struct ab8500_ponkey *ponkey = platform_get_drvdata(pdev);
-
-	free_irq(ponkey->irq_dbf, ponkey);
-	free_irq(ponkey->irq_dbr, ponkey);
-	input_unregister_device(ponkey->idev);
-	kfree(ponkey);
-
-	return 0;
 }
 
 #ifdef CONFIG_OF
@@ -144,7 +127,6 @@ static struct platform_driver ab8500_ponkey_driver = {
 		.of_match_table = of_match_ptr(ab8500_ponkey_match),
 	},
 	.probe		= ab8500_ponkey_probe,
-	.remove		= ab8500_ponkey_remove,
 };
 module_platform_driver(ab8500_ponkey_driver);
 
diff --git a/drivers/input/misc/gpio-beeper.c b/drivers/input/misc/gpio-beeper.c
index b757435e2b3..8886af63eae 100644
--- a/drivers/input/misc/gpio-beeper.c
+++ b/drivers/input/misc/gpio-beeper.c
@@ -1,7 +1,7 @@
 /*
  * Generic GPIO beeper driver
  *
- * Copyright (C) 2013 Alexander Shiyan <shc_work@mail.ru>
+ * Copyright (C) 2013-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
@@ -11,7 +11,8 @@
 
 #include <linux/input.h>
 #include <linux/module.h>
-#include <linux/of_gpio.h>
+#include <linux/gpio/consumer.h>
+#include <linux/of.h>
 #include <linux/workqueue.h>
 #include <linux/platform_device.h>
 
@@ -19,14 +20,13 @@
 
 struct gpio_beeper {
 	struct work_struct	work;
-	int			gpio;
-	bool			active_low;
+	struct gpio_desc	*desc;
 	bool			beeping;
 };
 
 static void gpio_beeper_toggle(struct gpio_beeper *beep, bool on)
 {
-	gpio_set_value_cansleep(beep->gpio, on ^ beep->active_low);
+	gpiod_set_value_cansleep(beep->desc, on);
 }
 
 static void gpio_beeper_work(struct work_struct *work)
@@ -65,18 +65,16 @@ static void gpio_beeper_close(struct input_dev *input)
 static int gpio_beeper_probe(struct platform_device *pdev)
 {
 	struct gpio_beeper *beep;
-	enum of_gpio_flags flags;
 	struct input_dev *input;
-	unsigned long gflags;
 	int err;
 
 	beep = devm_kzalloc(&pdev->dev, sizeof(*beep), GFP_KERNEL);
 	if (!beep)
 		return -ENOMEM;
 
-	beep->gpio = of_get_gpio_flags(pdev->dev.of_node, 0, &flags);
-	if (!gpio_is_valid(beep->gpio))
-		return beep->gpio;
+	beep->desc = devm_gpiod_get(&pdev->dev, NULL);
+	if (IS_ERR(beep->desc))
+		return PTR_ERR(beep->desc);
 
 	input = devm_input_allocate_device(&pdev->dev);
 	if (!input)
@@ -94,10 +92,7 @@ static int gpio_beeper_probe(struct platform_device *pdev)
 
 	input_set_capability(input, EV_SND, SND_BELL);
 
-	beep->active_low = flags & OF_GPIO_ACTIVE_LOW;
-	gflags = beep->active_low ? GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW;
-
-	err = devm_gpio_request_one(&pdev->dev, beep->gpio, gflags, pdev->name);
+	err = gpiod_direction_output(beep->desc, 0);
 	if (err)
 		return err;
 
@@ -106,17 +101,19 @@ static int gpio_beeper_probe(struct platform_device *pdev)
 	return input_register_device(input);
 }
 
-static struct of_device_id gpio_beeper_of_match[] = {
+#ifdef CONFIG_OF
+static const struct of_device_id gpio_beeper_of_match[] = {
 	{ .compatible = BEEPER_MODNAME, },
 	{ }
 };
 MODULE_DEVICE_TABLE(of, gpio_beeper_of_match);
+#endif
 
 static struct platform_driver gpio_beeper_platform_driver = {
 	.driver	= {
 		.name		= BEEPER_MODNAME,
 		.owner		= THIS_MODULE,
-		.of_match_table	= gpio_beeper_of_match,
+		.of_match_table	= of_match_ptr(gpio_beeper_of_match),
 	},
 	.probe	= gpio_beeper_probe,
 };
diff --git a/drivers/input/misc/ims-pcu.c b/drivers/input/misc/ims-pcu.c
index 5a736397d9c..719410feb84 100644
--- a/drivers/input/misc/ims-pcu.c
+++ b/drivers/input/misc/ims-pcu.c
@@ -1566,6 +1566,7 @@ static int ims_pcu_buffers_alloc(struct ims_pcu *pcu)
 	if (!pcu->urb_ctrl_buf) {
 		dev_err(pcu->dev,
 			"Failed to allocate memory for read buffer\n");
+		error = -ENOMEM;
 		goto err_free_urb_out_buf;
 	}
 
diff --git a/drivers/input/misc/max8925_onkey.c b/drivers/input/misc/max8925_onkey.c
index eef41cfc054..3809618e6a5 100644
--- a/drivers/input/misc/max8925_onkey.c
+++ b/drivers/input/misc/max8925_onkey.c
@@ -26,6 +26,7 @@
 #include <linux/interrupt.h>
 #include <linux/mfd/max8925.h>
 #include <linux/slab.h>
+#include <linux/device.h>
 
 #define SW_INPUT		(1 << 7)	/* 0/1 -- up/down */
 #define HARDRESET_EN		(1 << 7)
@@ -81,12 +82,14 @@ static int max8925_onkey_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	info = kzalloc(sizeof(struct max8925_onkey_info), GFP_KERNEL);
-	input = input_allocate_device();
-	if (!info || !input) {
-		error = -ENOMEM;
-		goto err_free_mem;
-	}
+	info = devm_kzalloc(&pdev->dev, sizeof(struct max8925_onkey_info),
+			    GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+	input = devm_input_allocate_device(&pdev->dev);
+	if (!input)
+		return -ENOMEM;
 
 	info->idev = input;
 	info->i2c = chip->i2c;
@@ -100,55 +103,34 @@ static int max8925_onkey_probe(struct platform_device *pdev)
 	input->dev.parent = &pdev->dev;
 	input_set_capability(input, EV_KEY, KEY_POWER);
 
-	error = request_threaded_irq(irq[0], NULL, max8925_onkey_handler,
-				     IRQF_ONESHOT, "onkey-down", info);
+	error = devm_request_threaded_irq(&pdev->dev, irq[0], NULL,
+					  max8925_onkey_handler, IRQF_ONESHOT,
+					  "onkey-down", info);
 	if (error < 0) {
 		dev_err(chip->dev, "Failed to request IRQ: #%d: %d\n",
 			irq[0], error);
-		goto err_free_mem;
+		return error;
 	}
 
-	error = request_threaded_irq(irq[1], NULL, max8925_onkey_handler,
-				     IRQF_ONESHOT, "onkey-up", info);
+	error = devm_request_threaded_irq(&pdev->dev, irq[1], NULL,
+					  max8925_onkey_handler, IRQF_ONESHOT,
+					  "onkey-up", info);
 	if (error < 0) {
 		dev_err(chip->dev, "Failed to request IRQ: #%d: %d\n",
 			irq[1], error);
-		goto err_free_irq0;
+		return error;
 	}
 
 	error = input_register_device(info->idev);
 	if (error) {
 		dev_err(chip->dev, "Can't register input device: %d\n", error);
-		goto err_free_irq1;
+		return error;
 	}
 
 	platform_set_drvdata(pdev, info);
 	device_init_wakeup(&pdev->dev, 1);
 
 	return 0;
-
-err_free_irq1:
-	free_irq(irq[1], info);
-err_free_irq0:
-	free_irq(irq[0], info);
-err_free_mem:
-	input_free_device(input);
-	kfree(info);
-
-	return error;
-}
-
-static int max8925_onkey_remove(struct platform_device *pdev)
-{
-	struct max8925_onkey_info *info = platform_get_drvdata(pdev);
-	struct max8925_chip *chip = dev_get_drvdata(pdev->dev.parent);
-
-	free_irq(info->irq[0] + chip->irq_base, info);
-	free_irq(info->irq[1] + chip->irq_base, info);
-	input_unregister_device(info->idev);
-	kfree(info);
-
-	return 0;
 }
 
 #ifdef CONFIG_PM_SLEEP
@@ -190,7 +172,6 @@ static struct platform_driver max8925_onkey_driver = {
 		.pm	= &max8925_onkey_pm_ops,
 	},
 	.probe		= max8925_onkey_probe,
-	.remove		= max8925_onkey_remove,
 };
 module_platform_driver(max8925_onkey_driver);
 
diff --git a/drivers/input/misc/max8997_haptic.c b/drivers/input/misc/max8997_haptic.c
index 1fea5484941..a363ebbd9cc 100644
--- a/drivers/input/misc/max8997_haptic.c
+++ b/drivers/input/misc/max8997_haptic.c
@@ -181,11 +181,21 @@ static void max8997_haptic_enable(struct max8997_haptic *chip)
 	}
 
 	if (!chip->enabled) {
-		chip->enabled = true;
-		regulator_enable(chip->regulator);
+		error = regulator_enable(chip->regulator);
+		if (error) {
+			dev_err(chip->dev, "Failed to enable regulator\n");
+			goto out;
+		}
 		max8997_haptic_configure(chip);
-		if (chip->mode == MAX8997_EXTERNAL_MODE)
-			pwm_enable(chip->pwm);
+		if (chip->mode == MAX8997_EXTERNAL_MODE) {
+			error = pwm_enable(chip->pwm);
+			if (error) {
+				dev_err(chip->dev, "Failed to enable PWM\n");
+				regulator_disable(chip->regulator);
+				goto out;
+			}
+		}
+		chip->enabled = true;
 	}
 
 out:
diff --git a/drivers/input/misc/pmic8xxx-pwrkey.c b/drivers/input/misc/pmic8xxx-pwrkey.c
index 1cb8fda7a16..c91e3d33aea 100644
--- a/drivers/input/misc/pmic8xxx-pwrkey.c
+++ b/drivers/input/misc/pmic8xxx-pwrkey.c
@@ -92,15 +92,15 @@ static int pmic8xxx_pwrkey_probe(struct platform_device *pdev)
 	bool pull_up;
 
 	if (of_property_read_u32(pdev->dev.of_node, "debounce", &kpd_delay))
-		kpd_delay = 0;
+		kpd_delay = 15625;
 
-	pull_up = of_property_read_bool(pdev->dev.of_node, "pull-up");
-
-	if (kpd_delay > 62500) {
+	if (kpd_delay > 62500 || kpd_delay == 0) {
 		dev_err(&pdev->dev, "invalid power key trigger delay\n");
 		return -EINVAL;
 	}
 
+	pull_up = of_property_read_bool(pdev->dev.of_node, "pull-up");
+
 	regmap = dev_get_regmap(pdev->dev.parent, NULL);
 	if (!regmap) {
 		dev_err(&pdev->dev, "failed to locate regmap for the device\n");
diff --git a/drivers/input/misc/rotary_encoder.c b/drivers/input/misc/rotary_encoder.c
index 99b9e42aa74..93558a1c7f7 100644
--- a/drivers/input/misc/rotary_encoder.c
+++ b/drivers/input/misc/rotary_encoder.c
@@ -143,7 +143,7 @@ static irqreturn_t rotary_encoder_half_period_irq(int irq, void *dev_id)
 }
 
 #ifdef CONFIG_OF
-static struct of_device_id rotary_encoder_of_match[] = {
+static const struct of_device_id rotary_encoder_of_match[] = {
 	{ .compatible = "rotary-encoder", },
 	{ },
 };
diff --git a/drivers/input/misc/soc_button_array.c b/drivers/input/misc/soc_button_array.c
index 20c80f543d5..5a6334be30b 100644
--- a/drivers/input/misc/soc_button_array.c
+++ b/drivers/input/misc/soc_button_array.c
@@ -17,7 +17,6 @@
 #include <linux/acpi.h>
 #include <linux/gpio/consumer.h>
 #include <linux/gpio_keys.h>
-#include <linux/input.h>
 #include <linux/platform_device.h>
 #include <linux/pnp.h>
 
diff --git a/drivers/input/misc/twl6040-vibra.c b/drivers/input/misc/twl6040-vibra.c
index 77dc23b94eb..6d26eecc278 100644
--- a/drivers/input/misc/twl6040-vibra.c
+++ b/drivers/input/misc/twl6040-vibra.c
@@ -262,7 +262,7 @@ static int twl6040_vibra_probe(struct platform_device *pdev)
 	struct vibra_info *info;
 	int vddvibl_uV = 0;
 	int vddvibr_uV = 0;
-	int ret;
+	int error;
 
 	twl6040_core_node = of_find_node_by_name(twl6040_core_dev->of_node,
 						 "vibra");
@@ -309,12 +309,12 @@ static int twl6040_vibra_probe(struct platform_device *pdev)
 
 	mutex_init(&info->mutex);
 
-	ret = devm_request_threaded_irq(&pdev->dev, info->irq, NULL,
-					twl6040_vib_irq_handler, 0,
-					"twl6040_irq_vib", info);
-	if (ret) {
-		dev_err(info->dev, "VIB IRQ request failed: %d\n", ret);
-		return ret;
+	error = devm_request_threaded_irq(&pdev->dev, info->irq, NULL,
+					  twl6040_vib_irq_handler, 0,
+					  "twl6040_irq_vib", info);
+	if (error) {
+		dev_err(info->dev, "VIB IRQ request failed: %d\n", error);
+		return error;
 	}
 
 	info->supplies[0].supply = "vddvibl";
@@ -323,40 +323,40 @@ static int twl6040_vibra_probe(struct platform_device *pdev)
 	 * When booted with Device tree the regulators are attached to the
 	 * parent device (twl6040 MFD core)
 	 */
-	ret = regulator_bulk_get(twl6040_core_dev, ARRAY_SIZE(info->supplies),
-				 info->supplies);
-	if (ret) {
-		dev_err(info->dev, "couldn't get regulators %d\n", ret);
-		return ret;
+	error = devm_regulator_bulk_get(twl6040_core_dev,
+					ARRAY_SIZE(info->supplies),
+					info->supplies);
+	if (error) {
+		dev_err(info->dev, "couldn't get regulators %d\n", error);
+		return error;
 	}
 
 	if (vddvibl_uV) {
-		ret = regulator_set_voltage(info->supplies[0].consumer,
-					    vddvibl_uV, vddvibl_uV);
-		if (ret) {
+		error = regulator_set_voltage(info->supplies[0].consumer,
+					      vddvibl_uV, vddvibl_uV);
+		if (error) {
 			dev_err(info->dev, "failed to set VDDVIBL volt %d\n",
-				ret);
-			goto err_regulator;
+				error);
+			return error;
 		}
 	}
 
 	if (vddvibr_uV) {
-		ret = regulator_set_voltage(info->supplies[1].consumer,
-					    vddvibr_uV, vddvibr_uV);
-		if (ret) {
+		error = regulator_set_voltage(info->supplies[1].consumer,
+					      vddvibr_uV, vddvibr_uV);
+		if (error) {
 			dev_err(info->dev, "failed to set VDDVIBR volt %d\n",
-				ret);
-			goto err_regulator;
+				error);
+			return error;
 		}
 	}
 
 	INIT_WORK(&info->play_work, vibra_play_work);
 
-	info->input_dev = input_allocate_device();
-	if (info->input_dev == NULL) {
+	info->input_dev = devm_input_allocate_device(&pdev->dev);
+	if (!info->input_dev) {
 		dev_err(info->dev, "couldn't allocate input device\n");
-		ret = -ENOMEM;
-		goto err_regulator;
+		return -ENOMEM;
 	}
 
 	input_set_drvdata(info->input_dev, info);
@@ -367,44 +367,25 @@ static int twl6040_vibra_probe(struct platform_device *pdev)
 	info->input_dev->close = twl6040_vibra_close;
 	__set_bit(FF_RUMBLE, info->input_dev->ffbit);
 
-	ret = input_ff_create_memless(info->input_dev, NULL, vibra_play);
-	if (ret < 0) {
+	error = input_ff_create_memless(info->input_dev, NULL, vibra_play);
+	if (error) {
 		dev_err(info->dev, "couldn't register vibrator to FF\n");
-		goto err_ialloc;
+		return error;
 	}
 
-	ret = input_register_device(info->input_dev);
-	if (ret < 0) {
+	error = input_register_device(info->input_dev);
+	if (error) {
 		dev_err(info->dev, "couldn't register input device\n");
-		goto err_iff;
+		return error;
 	}
 
 	platform_set_drvdata(pdev, info);
 
 	return 0;
-
-err_iff:
-	input_ff_destroy(info->input_dev);
-err_ialloc:
-	input_free_device(info->input_dev);
-err_regulator:
-	regulator_bulk_free(ARRAY_SIZE(info->supplies), info->supplies);
-	return ret;
-}
-
-static int twl6040_vibra_remove(struct platform_device *pdev)
-{
-	struct vibra_info *info = platform_get_drvdata(pdev);
-
-	input_unregister_device(info->input_dev);
-	regulator_bulk_free(ARRAY_SIZE(info->supplies), info->supplies);
-
-	return 0;
 }
 
 static struct platform_driver twl6040_vibra_driver = {
 	.probe		= twl6040_vibra_probe,
-	.remove		= twl6040_vibra_remove,
 	.driver		= {
 		.name	= "twl6040-vibra",
 		.owner	= THIS_MODULE,
diff --git a/drivers/input/mouse/Kconfig b/drivers/input/mouse/Kconfig
index effa9c5f2c5..366fc7ad5eb 100644
--- a/drivers/input/mouse/Kconfig
+++ b/drivers/input/mouse/Kconfig
@@ -17,7 +17,7 @@ config MOUSE_PS2
 	default y
 	select SERIO
 	select SERIO_LIBPS2
-	select SERIO_I8042 if X86
+	select SERIO_I8042 if ARCH_MIGHT_HAVE_PC_SERIO
 	select SERIO_GSCPS2 if GSC
 	help
 	  Say Y here if you have a PS/2 mouse connected to your system. This
@@ -53,7 +53,7 @@ config MOUSE_PS2_LOGIPS2PP
 	default y
 	depends on MOUSE_PS2
 	help
-	  Say Y here if you have a Logictech PS/2++ mouse connected to
+	  Say Y here if you have a Logitech PS/2++ mouse connected to
 	  your system.
 
 	  If unsure, say Y.
diff --git a/drivers/input/mouse/elantech.c b/drivers/input/mouse/elantech.c
index b96e978a37b..ee2a04d90d2 100644
--- a/drivers/input/mouse/elantech.c
+++ b/drivers/input/mouse/elantech.c
@@ -473,8 +473,15 @@ static void elantech_report_absolute_v3(struct psmouse *psmouse,
 	input_report_key(dev, BTN_TOOL_FINGER, fingers == 1);
 	input_report_key(dev, BTN_TOOL_DOUBLETAP, fingers == 2);
 	input_report_key(dev, BTN_TOOL_TRIPLETAP, fingers == 3);
-	input_report_key(dev, BTN_LEFT, packet[0] & 0x01);
-	input_report_key(dev, BTN_RIGHT, packet[0] & 0x02);
+
+	/* For clickpads map both buttons to BTN_LEFT */
+	if (etd->fw_version & 0x001000) {
+		input_report_key(dev, BTN_LEFT, packet[0] & 0x03);
+	} else {
+		input_report_key(dev, BTN_LEFT, packet[0] & 0x01);
+		input_report_key(dev, BTN_RIGHT, packet[0] & 0x02);
+	}
+
 	input_report_abs(dev, ABS_PRESSURE, pres);
 	input_report_abs(dev, ABS_TOOL_WIDTH, width);
 
@@ -484,10 +491,17 @@ static void elantech_report_absolute_v3(struct psmouse *psmouse,
 static void elantech_input_sync_v4(struct psmouse *psmouse)
 {
 	struct input_dev *dev = psmouse->dev;
+	struct elantech_data *etd = psmouse->private;
 	unsigned char *packet = psmouse->packet;
 
-	input_report_key(dev, BTN_LEFT, packet[0] & 0x01);
-	input_report_key(dev, BTN_RIGHT, packet[0] & 0x02);
+	/* For clickpads map both buttons to BTN_LEFT */
+	if (etd->fw_version & 0x001000) {
+		input_report_key(dev, BTN_LEFT, packet[0] & 0x03);
+	} else {
+		input_report_key(dev, BTN_LEFT, packet[0] & 0x01);
+		input_report_key(dev, BTN_RIGHT, packet[0] & 0x02);
+	}
+
 	input_mt_report_pointer_emulation(dev, true);
 	input_sync(dev);
 }
@@ -835,7 +849,7 @@ static int elantech_set_absolute_mode(struct psmouse *psmouse)
 		if (etd->set_hw_resolution)
 			etd->reg_10 = 0x0b;
 		else
-			etd->reg_10 = 0x03;
+			etd->reg_10 = 0x01;
 
 		if (elantech_write_reg(psmouse, 0x10, etd->reg_10))
 			rc = -1;
@@ -1336,7 +1350,8 @@ static int elantech_reconnect(struct psmouse *psmouse)
 }
 
 /*
- * Some hw_version 3 models go into error state when we try to set bit 3 of r10
+ * Some hw_version 3 models go into error state when we try to set
+ * bit 3 and/or bit 1 of r10.
  */
 static const struct dmi_system_id no_hw_res_dmi_table[] = {
 #if defined(CONFIG_DMI) && defined(CONFIG_X86)
diff --git a/drivers/input/mouse/synaptics.c b/drivers/input/mouse/synaptics.c
index d68d33fb5ac..ec772d962f0 100644
--- a/drivers/input/mouse/synaptics.c
+++ b/drivers/input/mouse/synaptics.c
@@ -117,6 +117,31 @@ void synaptics_reset(struct psmouse *psmouse)
 }
 
 #ifdef CONFIG_MOUSE_PS2_SYNAPTICS
+struct min_max_quirk {
+	const char * const *pnp_ids;
+	int x_min, x_max, y_min, y_max;
+};
+
+static const struct min_max_quirk min_max_pnpid_table[] = {
+	{
+		(const char * const []){"LEN0033", NULL},
+		1024, 5052, 2258, 4832
+	},
+	{
+		(const char * const []){"LEN0035", "LEN0042", NULL},
+		1232, 5710, 1156, 4696
+	},
+	{
+		(const char * const []){"LEN0034", "LEN0036", "LEN2004", NULL},
+		1024, 5112, 2024, 4832
+	},
+	{
+		(const char * const []){"LEN2001", NULL},
+		1024, 5022, 2508, 4832
+	},
+	{ }
+};
+
 /* This list has been kindly provided by Synaptics. */
 static const char * const topbuttonpad_pnp_ids[] = {
 	"LEN0017",
@@ -129,7 +154,7 @@ static const char * const topbuttonpad_pnp_ids[] = {
 	"LEN002D",
 	"LEN002E",
 	"LEN0033", /* Helix */
-	"LEN0034", /* T431s, T540, X1 Carbon 2nd */
+	"LEN0034", /* T431s, L440, L540, T540, W540, X1 Carbon 2nd */
 	"LEN0035", /* X240 */
 	"LEN0036", /* T440 */
 	"LEN0037",
@@ -142,7 +167,7 @@ static const char * const topbuttonpad_pnp_ids[] = {
 	"LEN0048",
 	"LEN0049",
 	"LEN2000",
-	"LEN2001",
+	"LEN2001", /* Edge E431 */
 	"LEN2002",
 	"LEN2003",
 	"LEN2004", /* L440 */
@@ -156,6 +181,18 @@ 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
  ****************************************************************************/
@@ -304,20 +341,11 @@ static int synaptics_identify(struct psmouse *psmouse)
  * Resolution is left zero if touchpad does not support the query
  */
 
-static const int *quirk_min_max;
-
 static int synaptics_resolution(struct psmouse *psmouse)
 {
 	struct synaptics_data *priv = psmouse->private;
 	unsigned char resp[3];
-
-	if (quirk_min_max) {
-		priv->x_min = quirk_min_max[0];
-		priv->x_max = quirk_min_max[1];
-		priv->y_min = quirk_min_max[2];
-		priv->y_max = quirk_min_max[3];
-		return 0;
-	}
+	int i;
 
 	if (SYN_ID_MAJOR(priv->identity) < 4)
 		return 0;
@@ -329,6 +357,16 @@ 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)) {
+			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;
+			priv->y_max = min_max_pnpid_table[i].y_max;
+			return 0;
+		}
+	}
+
 	if (SYN_EXT_CAP_REQUESTS(priv->capabilities) >= 5 &&
 	    SYN_CAP_MAX_DIMENSIONS(priv->ext_cap_0c)) {
 		if (synaptics_send_cmd(psmouse, SYN_QUE_EXT_MAX_COORDS, resp)) {
@@ -1365,17 +1403,8 @@ static void set_input_params(struct psmouse *psmouse,
 
 	if (SYN_CAP_CLICKPAD(priv->ext_cap_0c)) {
 		__set_bit(INPUT_PROP_BUTTONPAD, dev->propbit);
-		/* See if this buttonpad has a top button area */
-		if (!strncmp(psmouse->ps2dev.serio->firmware_id, "PNP:", 4)) {
-			for (i = 0; topbuttonpad_pnp_ids[i]; i++) {
-				if (strstr(psmouse->ps2dev.serio->firmware_id,
-					   topbuttonpad_pnp_ids[i])) {
-					__set_bit(INPUT_PROP_TOPBUTTONPAD,
-						  dev->propbit);
-					break;
-				}
-			}
-		}
+		if (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);
 		__clear_bit(BTN_MIDDLE, dev->keybit);
@@ -1547,104 +1576,10 @@ static const struct dmi_system_id olpc_dmi_table[] __initconst = {
 	{ }
 };
 
-static const struct dmi_system_id min_max_dmi_table[] __initconst = {
-#if defined(CONFIG_DMI)
-	{
-		/* Lenovo ThinkPad Helix */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-			DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad Helix"),
-		},
-		.driver_data = (int []){1024, 5052, 2258, 4832},
-	},
-	{
-		/* Lenovo ThinkPad X240 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-			DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad X240"),
-		},
-		.driver_data = (int []){1232, 5710, 1156, 4696},
-	},
-	{
-		/* Lenovo ThinkPad Edge E431 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-			DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad Edge E431"),
-		},
-		.driver_data = (int []){1024, 5022, 2508, 4832},
-	},
-	{
-		/* Lenovo ThinkPad T431s */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-			DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T431"),
-		},
-		.driver_data = (int []){1024, 5112, 2024, 4832},
-	},
-	{
-		/* Lenovo ThinkPad T440s */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-			DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T440"),
-		},
-		.driver_data = (int []){1024, 5112, 2024, 4832},
-	},
-	{
-		/* Lenovo ThinkPad L440 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-			DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad L440"),
-		},
-		.driver_data = (int []){1024, 5112, 2024, 4832},
-	},
-	{
-		/* Lenovo ThinkPad T540p */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-			DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T540"),
-		},
-		.driver_data = (int []){1024, 5056, 2058, 4832},
-	},
-	{
-		/* Lenovo ThinkPad L540 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-			DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad L540"),
-		},
-		.driver_data = (int []){1024, 5112, 2024, 4832},
-	},
-	{
-		/* Lenovo Yoga S1 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-			DMI_EXACT_MATCH(DMI_PRODUCT_VERSION,
-					"ThinkPad S1 Yoga"),
-		},
-		.driver_data = (int []){1232, 5710, 1156, 4696},
-	},
-	{
-		/* Lenovo ThinkPad X1 Carbon Haswell (3rd generation) */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-			DMI_MATCH(DMI_PRODUCT_VERSION,
-					"ThinkPad X1 Carbon 2nd"),
-		},
-		.driver_data = (int []){1024, 5112, 2024, 4832},
-	},
-#endif
-	{ }
-};
-
 void __init synaptics_module_init(void)
 {
-	const struct dmi_system_id *min_max_dmi;
-
 	impaired_toshiba_kbc = dmi_check_system(toshiba_dmi_table);
 	broken_olpc_ec = dmi_check_system(olpc_dmi_table);
-
-	min_max_dmi = dmi_first_match(min_max_dmi_table);
-	if (min_max_dmi)
-		quirk_min_max = min_max_dmi->driver_data;
 }
 
 static int __synaptics_init(struct psmouse *psmouse, bool absolute_mode)
diff --git a/drivers/input/serio/ambakmi.c b/drivers/input/serio/ambakmi.c
index 762b08432de..8b748d99b93 100644
--- a/drivers/input/serio/ambakmi.c
+++ b/drivers/input/serio/ambakmi.c
@@ -79,7 +79,8 @@ static int amba_kmi_open(struct serio *io)
 	writeb(divisor, KMICLKDIV);
 	writeb(KMICR_EN, KMICR);
 
-	ret = request_irq(kmi->irq, amba_kmi_int, 0, "kmi-pl050", kmi);
+	ret = request_irq(kmi->irq, amba_kmi_int, IRQF_SHARED, "kmi-pl050",
+			  kmi);
 	if (ret) {
 		printk(KERN_ERR "kmi: failed to claim IRQ%d\n", kmi->irq);
 		writeb(0, KMICR);
diff --git a/drivers/input/serio/apbps2.c b/drivers/input/serio/apbps2.c
index 17e01a807dd..98be824544a 100644
--- a/drivers/input/serio/apbps2.c
+++ b/drivers/input/serio/apbps2.c
@@ -203,7 +203,7 @@ static int apbps2_of_remove(struct platform_device *of_dev)
 	return 0;
 }
 
-static struct of_device_id apbps2_of_match[] = {
+static const struct of_device_id apbps2_of_match[] = {
 	{ .name = "GAISLER_APBPS2", },
 	{ .name = "01_060", },
 	{}
diff --git a/drivers/input/serio/olpc_apsp.c b/drivers/input/serio/olpc_apsp.c
index 5d2fe7ece7c..d906f3ebc8c 100644
--- a/drivers/input/serio/olpc_apsp.c
+++ b/drivers/input/serio/olpc_apsp.c
@@ -262,7 +262,7 @@ static int olpc_apsp_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id olpc_apsp_dt_ids[] = {
+static const struct of_device_id olpc_apsp_dt_ids[] = {
 	{ .compatible = "olpc,ap-sp", },
 	{}
 };
diff --git a/drivers/input/tablet/wacom_sys.c b/drivers/input/tablet/wacom_sys.c
index 611fc3905d0..2c613cd41dd 100644
--- a/drivers/input/tablet/wacom_sys.c
+++ b/drivers/input/tablet/wacom_sys.c
@@ -349,6 +349,7 @@ static int wacom_parse_hid(struct usb_interface *intf,
 						break;
 
 					case MTTPC:
+					case MTTPC_B:
 						features->pktlen = WACOM_PKGLEN_MTTPC;
 						break;
 
@@ -380,6 +381,16 @@ static int wacom_parse_hid(struct usb_interface *intf,
 						i += 12;
 						break;
 
+					case MTTPC_B:
+						features->x_max =
+							get_unaligned_le16(&report[i + 3]);
+						features->x_phy =
+							get_unaligned_le16(&report[i + 6]);
+						features->unit = report[i - 5];
+						features->unitExpo = report[i - 3];
+						i += 9;
+						break;
+
 					default:
 						features->x_max =
 							get_unaligned_le16(&report[i + 3]);
@@ -430,6 +441,14 @@ static int wacom_parse_hid(struct usb_interface *intf,
 						i += 12;
 						break;
 
+					case MTTPC_B:
+						features->y_max =
+							get_unaligned_le16(&report[i + 3]);
+						features->y_phy =
+							get_unaligned_le16(&report[i + 6]);
+						i += 9;
+						break;
+
 					default:
 						features->y_max =
 							features->x_max;
diff --git a/drivers/input/tablet/wacom_wac.c b/drivers/input/tablet/wacom_wac.c
index 4822c57a375..977d05cd9e2 100644
--- a/drivers/input/tablet/wacom_wac.c
+++ b/drivers/input/tablet/wacom_wac.c
@@ -484,6 +484,8 @@ static int wacom_intuos_inout(struct wacom_wac *wacom)
 		input_report_key(input, BTN_TOUCH, 0);
 		input_report_abs(input, ABS_PRESSURE, 0);
 		input_report_abs(input, ABS_DISTANCE, wacom->features.distance_max);
+		if (features->quirks & WACOM_QUIRK_MULTI_INPUT)
+			wacom->shared->stylus_in_proximity = true;
 	}
 
 	/* Exit report */
@@ -928,12 +930,12 @@ static int wacom_24hdt_irq(struct wacom_wac *wacom)
 		input_mt_report_slot_state(input, MT_TOOL_FINGER, touch);
 
 		if (touch) {
-			int t_x = le16_to_cpup((__le16 *)&data[offset + 2]);
-			int c_x = le16_to_cpup((__le16 *)&data[offset + 4]);
-			int t_y = le16_to_cpup((__le16 *)&data[offset + 6]);
-			int c_y = le16_to_cpup((__le16 *)&data[offset + 8]);
-			int w = le16_to_cpup((__le16 *)&data[offset + 10]);
-			int h = le16_to_cpup((__le16 *)&data[offset + 12]);
+			int t_x = get_unaligned_le16(&data[offset + 2]);
+			int c_x = get_unaligned_le16(&data[offset + 4]);
+			int t_y = get_unaligned_le16(&data[offset + 6]);
+			int c_y = get_unaligned_le16(&data[offset + 8]);
+			int w = get_unaligned_le16(&data[offset + 10]);
+			int h = get_unaligned_le16(&data[offset + 12]);
 
 			input_report_abs(input, ABS_MT_POSITION_X, t_x);
 			input_report_abs(input, ABS_MT_POSITION_Y, t_y);
@@ -962,7 +964,7 @@ static int wacom_mt_touch(struct wacom_wac *wacom)
 	int x_offset = 0;
 
 	/* MTTPC does not support Height and Width */
-	if (wacom->features.type == MTTPC)
+	if (wacom->features.type == MTTPC || wacom->features.type == MTTPC_B)
 		x_offset = -4;
 
 	/*
@@ -978,7 +980,7 @@ static int wacom_mt_touch(struct wacom_wac *wacom)
 	for (i = 0; i < contacts_to_send; i++) {
 		int offset = (WACOM_BYTES_PER_MT_PACKET + x_offset) * i + 3;
 		bool touch = data[offset] & 0x1;
-		int id = le16_to_cpup((__le16 *)&data[offset + 1]);
+		int id = get_unaligned_le16(&data[offset + 1]);
 		int slot = input_mt_get_slot_by_key(input, id);
 
 		if (slot < 0)
@@ -987,8 +989,8 @@ static int wacom_mt_touch(struct wacom_wac *wacom)
 		input_mt_slot(input, slot);
 		input_mt_report_slot_state(input, MT_TOOL_FINGER, touch);
 		if (touch) {
-			int x = le16_to_cpup((__le16 *)&data[offset + x_offset + 7]);
-			int y = le16_to_cpup((__le16 *)&data[offset + x_offset + 9]);
+			int x = get_unaligned_le16(&data[offset + x_offset + 7]);
+			int y = get_unaligned_le16(&data[offset + x_offset + 9]);
 			input_report_abs(input, ABS_MT_POSITION_X, x);
 			input_report_abs(input, ABS_MT_POSITION_Y, y);
 		}
@@ -1047,6 +1049,10 @@ static int wacom_tpc_single_touch(struct wacom_wac *wacom, size_t len)
 			prox = data[0] & 0x01;
 			x = get_unaligned_le16(&data[1]);
 			y = get_unaligned_le16(&data[3]);
+		} else if (len == WACOM_PKGLEN_TPC1FG_B) {
+			prox = data[2] & 0x01;
+			x = get_unaligned_le16(&data[3]);
+			y = get_unaligned_le16(&data[5]);
 		} else {
 			prox = data[1] & 0x01;
 			x = le16_to_cpup((__le16 *)&data[2]);
@@ -1110,6 +1116,9 @@ static int wacom_tpc_irq(struct wacom_wac *wacom, size_t len)
 	case WACOM_PKGLEN_TPC2FG:
 		return wacom_tpc_mt_touch(wacom);
 
+	case WACOM_PKGLEN_PENABLED:
+		return wacom_tpc_pen(wacom);
+
 	default:
 		switch (data[0]) {
 		case WACOM_REPORT_TPC1FG:
@@ -1119,6 +1128,7 @@ static int wacom_tpc_irq(struct wacom_wac *wacom, size_t len)
 			return wacom_tpc_single_touch(wacom, len);
 
 		case WACOM_REPORT_TPCMT:
+		case WACOM_REPORT_TPCMT2:
 			return wacom_mt_touch(wacom);
 
 		case WACOM_REPORT_PENABLED:
@@ -1461,6 +1471,7 @@ void wacom_wac_irq(struct wacom_wac *wacom_wac, size_t len)
 	case TABLETPC2FG:
 	case MTSCREEN:
 	case MTTPC:
+	case MTTPC_B:
 		sync = wacom_tpc_irq(wacom_wac, len);
 		break;
 
@@ -1565,10 +1576,10 @@ static void wacom_abs_set_axis(struct input_dev *input_dev,
 	struct wacom_features *features = &wacom_wac->features;
 
 	if (features->device_type == BTN_TOOL_PEN) {
-		input_set_abs_params(input_dev, ABS_X, 0, features->x_max,
-				     features->x_fuzz, 0);
-		input_set_abs_params(input_dev, ABS_Y, 0, features->y_max,
-				     features->y_fuzz, 0);
+		input_set_abs_params(input_dev, ABS_X, features->x_min,
+				     features->x_max, features->x_fuzz, 0);
+		input_set_abs_params(input_dev, ABS_Y, features->y_min,
+				     features->y_max, features->y_fuzz, 0);
 		input_set_abs_params(input_dev, ABS_PRESSURE, 0,
 			features->pressure_max, features->pressure_fuzz, 0);
 
@@ -1802,6 +1813,7 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 
 	case MTSCREEN:
 	case MTTPC:
+	case MTTPC_B:
 	case TABLETPC2FG:
 		if (features->device_type == BTN_TOOL_FINGER) {
 			unsigned int flags = INPUT_MT_DIRECT;
@@ -2123,11 +2135,11 @@ static const struct wacom_features wacom_features_0x317 =
 	  63, INTUOSPL, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
 	  .touch_max = 16 };
 static const struct wacom_features wacom_features_0xF4 =
-	{ "Wacom Cintiq 24HD",       WACOM_PKGLEN_INTUOS,   104480, 65600, 2047,
-	  63, WACOM_24HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES };
+	{ "Wacom Cintiq 24HD",       WACOM_PKGLEN_INTUOS,   104280, 65400, 2047,
+	  63, WACOM_24HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES, 200, 200 };
 static const struct wacom_features wacom_features_0xF8 =
-	{ "Wacom Cintiq 24HD touch", WACOM_PKGLEN_INTUOS,   104480, 65600, 2047, /* Pen */
-	  63, WACOM_24HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
+	{ "Wacom Cintiq 24HD touch", WACOM_PKGLEN_INTUOS,   104280, 65400, 2047, /* Pen */
+	  63, WACOM_24HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES, 200, 200,
 	  .oVid = USB_VENDOR_ID_WACOM, .oPid = 0xf6 };
 static const struct wacom_features wacom_features_0xF6 =
 	{ "Wacom Cintiq 24HD touch", .type = WACOM_24HDT, /* Touch */
@@ -2142,8 +2154,8 @@ static const struct wacom_features wacom_features_0xC6 =
 	{ "Wacom Cintiq 12WX",    WACOM_PKGLEN_INTUOS,    53020, 33440, 1023,
 	  63, WACOM_BEE, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES };
 static const struct wacom_features wacom_features_0x304 =
-	{ "Wacom Cintiq 13HD",    WACOM_PKGLEN_INTUOS,    59552, 33848, 1023,
-	  63, WACOM_13HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES };
+	{ "Wacom Cintiq 13HD",    WACOM_PKGLEN_INTUOS,    59352, 33648, 1023,
+	  63, WACOM_13HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES, 200, 200 };
 static const struct wacom_features wacom_features_0xC7 =
 	{ "Wacom DTU1931",        WACOM_PKGLEN_GRAPHIRE,  37832, 30305,  511,
 	  0, PL, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
@@ -2157,24 +2169,24 @@ static const struct wacom_features wacom_features_0xFB =
 	{ "Wacom DTU1031",        WACOM_PKGLEN_DTUS,      22096, 13960,  511,
 	  0, DTUS, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
 static const struct wacom_features wacom_features_0x57 =
-	{ "Wacom DTK2241",        WACOM_PKGLEN_INTUOS,    95840, 54260, 2047,
-	  63, DTK, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES};
+	{ "Wacom DTK2241",        WACOM_PKGLEN_INTUOS,    95640, 54060, 2047,
+	  63, DTK, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES, 200, 200 };
 static const struct wacom_features wacom_features_0x59 = /* Pen */
-	{ "Wacom DTH2242",        WACOM_PKGLEN_INTUOS,    95840, 54260, 2047,
-	  63, DTK, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
+	{ "Wacom DTH2242",        WACOM_PKGLEN_INTUOS,    95640, 54060, 2047,
+	  63, DTK, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES, 200, 200,
 	  .oVid = USB_VENDOR_ID_WACOM, .oPid = 0x5D };
 static const struct wacom_features wacom_features_0x5D = /* Touch */
 	{ "Wacom DTH2242",       .type = WACOM_24HDT,
 	  .oVid = USB_VENDOR_ID_WACOM, .oPid = 0x59, .touch_max = 10 };
 static const struct wacom_features wacom_features_0xCC =
-	{ "Wacom Cintiq 21UX2",   WACOM_PKGLEN_INTUOS,    87200, 65600, 2047,
-	  63, WACOM_21UX2, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES };
+	{ "Wacom Cintiq 21UX2",   WACOM_PKGLEN_INTUOS,    87000, 65400, 2047,
+	  63, WACOM_21UX2, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES, 200, 200 };
 static const struct wacom_features wacom_features_0xFA =
-	{ "Wacom Cintiq 22HD",    WACOM_PKGLEN_INTUOS,    95840, 54260, 2047,
-	  63, WACOM_22HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES };
+	{ "Wacom Cintiq 22HD",    WACOM_PKGLEN_INTUOS,    95640, 54060, 2047,
+	  63, WACOM_22HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES, 200, 200 };
 static const struct wacom_features wacom_features_0x5B =
-	{ "Wacom Cintiq 22HDT", WACOM_PKGLEN_INTUOS,      95840, 54260, 2047,
-	  63, WACOM_22HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
+	{ "Wacom Cintiq 22HDT", WACOM_PKGLEN_INTUOS,      95640, 54060, 2047,
+	  63, WACOM_22HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES, 200, 200,
 	  .oVid = USB_VENDOR_ID_WACOM, .oPid = 0x5e };
 static const struct wacom_features wacom_features_0x5E =
 	{ "Wacom Cintiq 22HDT", .type = WACOM_24HDT,
@@ -2233,9 +2245,21 @@ static const struct wacom_features wacom_features_0x10E =
 static const struct wacom_features wacom_features_0x10F =
 	{ "Wacom ISDv4 10F",      WACOM_PKGLEN_MTTPC,     27760, 15694,  255,
 	  0, MTTPC, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+static const struct wacom_features wacom_features_0x116 =
+	{ "Wacom ISDv4 116",      WACOM_PKGLEN_GRAPHIRE,  26202, 16325,  255,
+	  0, TABLETPCE, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
 static const struct wacom_features wacom_features_0x4001 =
 	{ "Wacom ISDv4 4001",      WACOM_PKGLEN_MTTPC,     26202, 16325,  255,
 	  0, MTTPC, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+static const struct wacom_features wacom_features_0x4004 =
+	{ "Wacom ISDv4 4004",      WACOM_PKGLEN_MTTPC,     11060, 6220,  255,
+	  0, MTTPC_B, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+static const struct wacom_features wacom_features_0x5000 =
+	{ "Wacom ISDv4 5000",      WACOM_PKGLEN_MTTPC,     27848, 15752,  1023,
+	  0, MTTPC_B, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+static const struct wacom_features wacom_features_0x5002 =
+	{ "Wacom ISDv4 5002",      WACOM_PKGLEN_MTTPC,     29576, 16724,  1023,
+	  0, MTTPC_B, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
 static const struct wacom_features wacom_features_0x47 =
 	{ "Wacom Intuos2 6x8",    WACOM_PKGLEN_INTUOS,    20320, 16240, 1023,
 	  31, INTUOS, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
@@ -2316,8 +2340,8 @@ static const struct wacom_features wacom_features_0x6004 =
 	{ "ISD-V4",               WACOM_PKGLEN_GRAPHIRE,  12800,  8000,  255,
 	  0, TABLETPC, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
 static const struct wacom_features wacom_features_0x0307 =
-	{ "Wacom ISDv5 307", WACOM_PKGLEN_INTUOS,  59552,  33848, 2047,
-	  63, CINTIQ_HYBRID, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
+	{ "Wacom ISDv5 307", WACOM_PKGLEN_INTUOS,  59352,  33648, 2047,
+	  63, CINTIQ_HYBRID, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES, 200, 200,
 	  .oVid = USB_VENDOR_ID_WACOM, .oPid = 0x309 };
 static const struct wacom_features wacom_features_0x0309 =
 	{ "Wacom ISDv5 309", .type = WACOM_24HDT, /* Touch */
@@ -2447,6 +2471,7 @@ const struct usb_device_id wacom_ids[] = {
 	{ USB_DEVICE_WACOM(0x10D) },
 	{ USB_DEVICE_WACOM(0x10E) },
 	{ USB_DEVICE_WACOM(0x10F) },
+	{ USB_DEVICE_WACOM(0x116) },
 	{ USB_DEVICE_WACOM(0x300) },
 	{ USB_DEVICE_WACOM(0x301) },
 	{ USB_DEVICE_DETAILED(0x302, USB_CLASS_HID, 0, 0) },
@@ -2457,6 +2482,9 @@ const struct usb_device_id wacom_ids[] = {
 	{ USB_DEVICE_DETAILED(0x315, USB_CLASS_HID, 0, 0) },
 	{ USB_DEVICE_DETAILED(0x317, USB_CLASS_HID, 0, 0) },
 	{ USB_DEVICE_WACOM(0x4001) },
+	{ USB_DEVICE_WACOM(0x4004) },
+	{ USB_DEVICE_WACOM(0x5000) },
+	{ USB_DEVICE_WACOM(0x5002) },
 	{ USB_DEVICE_WACOM(0x47) },
 	{ USB_DEVICE_WACOM(0xF4) },
 	{ USB_DEVICE_WACOM(0xF8) },
diff --git a/drivers/input/tablet/wacom_wac.h b/drivers/input/tablet/wacom_wac.h
index f69c0ebe7fa..b2c9a9c1b55 100644
--- a/drivers/input/tablet/wacom_wac.h
+++ b/drivers/input/tablet/wacom_wac.h
@@ -22,6 +22,7 @@
 #define WACOM_PKGLEN_BBFUN	 9
 #define WACOM_PKGLEN_INTUOS	10
 #define WACOM_PKGLEN_TPC1FG	 5
+#define WACOM_PKGLEN_TPC1FG_B	10
 #define WACOM_PKGLEN_TPC2FG	14
 #define WACOM_PKGLEN_BBTOUCH	20
 #define WACOM_PKGLEN_BBTOUCH3	64
@@ -30,6 +31,7 @@
 #define WACOM_PKGLEN_MTOUCH	62
 #define WACOM_PKGLEN_MTTPC	40
 #define WACOM_PKGLEN_DTUS	68
+#define WACOM_PKGLEN_PENABLED	 8
 
 /* wacom data size per MT contact */
 #define WACOM_BYTES_PER_MT_PACKET	11
@@ -52,6 +54,7 @@
 #define WACOM_REPORT_TPC1FG		6
 #define WACOM_REPORT_TPC2FG		13
 #define WACOM_REPORT_TPCMT		13
+#define WACOM_REPORT_TPCMT2		3
 #define WACOM_REPORT_TPCHID		15
 #define WACOM_REPORT_TPCST		16
 #define WACOM_REPORT_DTUS		17
@@ -105,6 +108,7 @@ enum {
 	TABLETPC2FG,
 	MTSCREEN,
 	MTTPC,
+	MTTPC_B,
 	MAX_TYPE
 };
 
@@ -118,6 +122,8 @@ struct wacom_features {
 	int type;
 	int x_resolution;
 	int y_resolution;
+	int x_min;
+	int y_min;
 	int device_type;
 	int x_phy;
 	int y_phy;
diff --git a/drivers/input/touchscreen/88pm860x-ts.c b/drivers/input/touchscreen/88pm860x-ts.c
index 544e20c551f..0d4a9fad4a7 100644
--- a/drivers/input/touchscreen/88pm860x-ts.c
+++ b/drivers/input/touchscreen/88pm860x-ts.c
@@ -16,6 +16,7 @@
 #include <linux/input.h>
 #include <linux/mfd/88pm860x.h>
 #include <linux/slab.h>
+#include <linux/device.h>
 
 #define MEAS_LEN		(8)
 #define ACCURATE_BIT		(12)
@@ -234,16 +235,17 @@ static int pm860x_touch_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	touch = kzalloc(sizeof(struct pm860x_touch), GFP_KERNEL);
-	if (touch == NULL)
+	touch = devm_kzalloc(&pdev->dev, sizeof(struct pm860x_touch),
+			     GFP_KERNEL);
+	if (!touch)
 		return -ENOMEM;
+
 	platform_set_drvdata(pdev, touch);
 
-	touch->idev = input_allocate_device();
-	if (touch->idev == NULL) {
+	touch->idev = devm_input_allocate_device(&pdev->dev);
+	if (!touch->idev) {
 		dev_err(&pdev->dev, "Failed to allocate input device!\n");
-		ret = -ENOMEM;
-		goto out;
+		return -ENOMEM;
 	}
 
 	touch->idev->name = "88pm860x-touch";
@@ -258,10 +260,11 @@ static int pm860x_touch_probe(struct platform_device *pdev)
 	touch->res_x = res_x;
 	input_set_drvdata(touch->idev, touch);
 
-	ret = request_threaded_irq(touch->irq, NULL, pm860x_touch_handler,
-				   IRQF_ONESHOT, "touch", touch);
+	ret = devm_request_threaded_irq(&pdev->dev, touch->irq, NULL,
+					pm860x_touch_handler, IRQF_ONESHOT,
+					"touch", touch);
 	if (ret < 0)
-		goto out_irq;
+		return ret;
 
 	__set_bit(EV_ABS, touch->idev->evbit);
 	__set_bit(ABS_X, touch->idev->absbit);
@@ -279,28 +282,11 @@ static int pm860x_touch_probe(struct platform_device *pdev)
 	ret = input_register_device(touch->idev);
 	if (ret < 0) {
 		dev_err(chip->dev, "Failed to register touch!\n");
-		goto out_rg;
+		return ret;
 	}
 
 	platform_set_drvdata(pdev, touch);
 	return 0;
-out_rg:
-	free_irq(touch->irq, touch);
-out_irq:
-	input_free_device(touch->idev);
-out:
-	kfree(touch);
-	return ret;
-}
-
-static int pm860x_touch_remove(struct platform_device *pdev)
-{
-	struct pm860x_touch *touch = platform_get_drvdata(pdev);
-
-	input_unregister_device(touch->idev);
-	free_irq(touch->irq, touch);
-	kfree(touch);
-	return 0;
 }
 
 static struct platform_driver pm860x_touch_driver = {
@@ -309,7 +295,6 @@ static struct platform_driver pm860x_touch_driver = {
 		.owner	= THIS_MODULE,
 	},
 	.probe	= pm860x_touch_probe,
-	.remove	= pm860x_touch_remove,
 };
 module_platform_driver(pm860x_touch_driver);
 
diff --git a/drivers/input/touchscreen/Kconfig b/drivers/input/touchscreen/Kconfig
index 68edc9db2c6..a23a94bb4bc 100644
--- a/drivers/input/touchscreen/Kconfig
+++ b/drivers/input/touchscreen/Kconfig
@@ -11,6 +11,10 @@ menuconfig INPUT_TOUCHSCREEN
 
 if INPUT_TOUCHSCREEN
 
+config OF_TOUCHSCREEN
+	def_tristate INPUT
+	depends on INPUT && OF
+
 config TOUCHSCREEN_88PM860X
 	tristate "Marvell 88PM860x touchscreen"
 	depends on MFD_88PM860X
@@ -89,6 +93,7 @@ config TOUCHSCREEN_AD7879_SPI
 config TOUCHSCREEN_ATMEL_MXT
 	tristate "Atmel mXT I2C Touchscreen"
 	depends on I2C
+	select FW_LOADER
 	help
 	  Say Y here if you have Atmel mXT series I2C touchscreen,
 	  such as AT42QT602240/ATMXT224, connected to your system.
@@ -550,18 +555,6 @@ config TOUCHSCREEN_TI_AM335X_TSC
 	  To compile this driver as a module, choose M here: the
 	  module will be called ti_am335x_tsc.
 
-config TOUCHSCREEN_ATMEL_TSADCC
-	tristate "Atmel Touchscreen Interface"
-	depends on ARCH_AT91
-	help
-	  Say Y here if you have a 4-wire touchscreen connected to the
-          ADC Controller on your Atmel SoC.
-
-	  If unsure, say N.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called atmel_tsadcc.
-
 config TOUCHSCREEN_UCB1400
 	tristate "Philips UCB1400 touchscreen"
 	depends on AC97_BUS
@@ -640,7 +633,7 @@ config TOUCHSCREEN_WM9713
 
 config TOUCHSCREEN_WM97XX_ATMEL
 	tristate "WM97xx Atmel accelerated touch"
-	depends on TOUCHSCREEN_WM97XX && (AVR32 || ARCH_AT91)
+	depends on TOUCHSCREEN_WM97XX && AVR32
 	help
 	  Say Y here for support for streaming mode with WM97xx touchscreens
 	  on Atmel AT91 or AVR32 systems with an AC97C module.
@@ -858,7 +851,7 @@ config TOUCHSCREEN_TSC2007
 
 config TOUCHSCREEN_W90X900
 	tristate "W90P910 touchscreen driver"
-	depends on HAVE_CLK
+	depends on ARCH_W90X900
 	help
 	  Say Y here if you have a W90P910 based touchscreen.
 
@@ -897,6 +890,17 @@ config TOUCHSCREEN_STMPE
 	  To compile this driver as a module, choose M here: the
 	  module will be called stmpe-ts.
 
+config TOUCHSCREEN_SUN4I
+	tristate "Allwinner sun4i resistive touchscreen controller support"
+	depends on ARCH_SUNXI || COMPILE_TEST
+	depends on HWMON
+	help
+	  This selects support for the resistive touchscreen controller
+	  found on Allwinner sunxi SoCs.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called sun4i-ts.
+
 config TOUCHSCREEN_SUR40
 	tristate "Samsung SUR40 (Surface 2.0/PixelSense) touchscreen"
 	depends on USB
diff --git a/drivers/input/touchscreen/Makefile b/drivers/input/touchscreen/Makefile
index 4bc954b7c7c..126479d8c29 100644
--- a/drivers/input/touchscreen/Makefile
+++ b/drivers/input/touchscreen/Makefile
@@ -6,6 +6,7 @@
 
 wm97xx-ts-y := wm97xx-core.o
 
+obj-$(CONFIG_OF_TOUCHSCREEN)		+= of_touchscreen.o
 obj-$(CONFIG_TOUCHSCREEN_88PM860X)	+= 88pm860x-ts.o
 obj-$(CONFIG_TOUCHSCREEN_AD7877)	+= ad7877.o
 obj-$(CONFIG_TOUCHSCREEN_AD7879)	+= ad7879.o
@@ -13,7 +14,6 @@ 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_ATMEL_MXT)	+= atmel_mxt_ts.o
-obj-$(CONFIG_TOUCHSCREEN_ATMEL_TSADCC)	+= atmel_tsadcc.o
 obj-$(CONFIG_TOUCHSCREEN_AUO_PIXCIR)	+= auo-pixcir-ts.o
 obj-$(CONFIG_TOUCHSCREEN_BU21013)	+= bu21013_ts.o
 obj-$(CONFIG_TOUCHSCREEN_CY8CTMG110)	+= cy8ctmg110_ts.o
@@ -54,6 +54,7 @@ obj-$(CONFIG_TOUCHSCREEN_PIXCIR)	+= pixcir_i2c_ts.o
 obj-$(CONFIG_TOUCHSCREEN_S3C2410)	+= s3c2410_ts.o
 obj-$(CONFIG_TOUCHSCREEN_ST1232)	+= st1232.o
 obj-$(CONFIG_TOUCHSCREEN_STMPE)		+= stmpe-ts.o
+obj-$(CONFIG_TOUCHSCREEN_SUN4I)		+= sun4i-ts.o
 obj-$(CONFIG_TOUCHSCREEN_SUR40)		+= sur40.o
 obj-$(CONFIG_TOUCHSCREEN_TI_AM335X_TSC)	+= ti_am335x_tsc.o
 obj-$(CONFIG_TOUCHSCREEN_TOUCHIT213)	+= touchit213.o
diff --git a/drivers/input/touchscreen/ad7877.c b/drivers/input/touchscreen/ad7877.c
index 6793c85903a..523865daa1d 100644
--- a/drivers/input/touchscreen/ad7877.c
+++ b/drivers/input/touchscreen/ad7877.c
@@ -210,11 +210,6 @@ static bool gpio3;
 module_param(gpio3, bool, 0);
 MODULE_PARM_DESC(gpio3, "If gpio3 is set to 1 AUX3 acts as GPIO3");
 
-/*
- * ad7877_read/write are only used for initial setup and for sysfs controls.
- * The main traffic is done using spi_async() in the interrupt handler.
- */
-
 static int ad7877_read(struct spi_device *spi, u16 reg)
 {
 	struct ser_req *req;
diff --git a/drivers/input/touchscreen/ads7846.c b/drivers/input/touchscreen/ads7846.c
index 7f8aa981500..da201b8e37d 100644
--- a/drivers/input/touchscreen/ads7846.c
+++ b/drivers/input/touchscreen/ads7846.c
@@ -706,7 +706,7 @@ static void ads7846_read_state(struct ads7846 *ts)
 		m = &ts->msg[msg_idx];
 		error = spi_sync(ts->spi, m);
 		if (error) {
-			dev_err(&ts->spi->dev, "spi_async --> %d\n", error);
+			dev_err(&ts->spi->dev, "spi_sync --> %d\n", error);
 			packet->tc.ignore = true;
 			return;
 		}
diff --git a/drivers/input/touchscreen/atmel_mxt_ts.c b/drivers/input/touchscreen/atmel_mxt_ts.c
index a70400754e9..6e0b4a2120d 100644
--- a/drivers/input/touchscreen/atmel_mxt_ts.c
+++ b/drivers/input/touchscreen/atmel_mxt_ts.c
@@ -2,6 +2,8 @@
  * Atmel maXTouch Touchscreen driver
  *
  * Copyright (C) 2010 Samsung Electronics Co.Ltd
+ * Copyright (C) 2012 Google, Inc.
+ *
  * Author: Joonyoung Shim <jy0922.shim@samsung.com>
  *
  * This program is free software; you can redistribute  it and/or modify it
@@ -12,6 +14,8 @@
  */
 
 #include <linux/module.h>
+#include <linux/init.h>
+#include <linux/completion.h>
 #include <linux/delay.h>
 #include <linux/firmware.h>
 #include <linux/i2c.h>
@@ -25,12 +29,6 @@
 #define MXT_VER_21		21
 #define MXT_VER_22		22
 
-/* Slave addresses */
-#define MXT_APP_LOW		0x4a
-#define MXT_APP_HIGH		0x4b
-#define MXT_BOOT_LOW		0x24
-#define MXT_BOOT_HIGH		0x25
-
 /* Firmware */
 #define MXT_FW_NAME		"maxtouch.fw"
 
@@ -83,6 +81,9 @@
 #define MXT_COMMAND_REPORTALL	3
 #define MXT_COMMAND_DIAGNOSTIC	5
 
+/* Define for T6 status byte */
+#define MXT_T6_STATUS_RESET	(1 << 7)
+
 /* MXT_GEN_POWER_T7 field */
 #define MXT_POWER_IDLEACQINT	0
 #define MXT_POWER_ACTVACQINT	1
@@ -99,33 +100,26 @@
 
 /* MXT_TOUCH_MULTI_T9 field */
 #define MXT_TOUCH_CTRL		0
-#define MXT_TOUCH_XORIGIN	1
-#define MXT_TOUCH_YORIGIN	2
-#define MXT_TOUCH_XSIZE		3
-#define MXT_TOUCH_YSIZE		4
-#define MXT_TOUCH_BLEN		6
-#define MXT_TOUCH_TCHTHR	7
-#define MXT_TOUCH_TCHDI		8
-#define MXT_TOUCH_ORIENT	9
-#define MXT_TOUCH_MOVHYSTI	11
-#define MXT_TOUCH_MOVHYSTN	12
-#define MXT_TOUCH_NUMTOUCH	14
-#define MXT_TOUCH_MRGHYST	15
-#define MXT_TOUCH_MRGTHR	16
-#define MXT_TOUCH_AMPHYST	17
-#define MXT_TOUCH_XRANGE_LSB	18
-#define MXT_TOUCH_XRANGE_MSB	19
-#define MXT_TOUCH_YRANGE_LSB	20
-#define MXT_TOUCH_YRANGE_MSB	21
-#define MXT_TOUCH_XLOCLIP	22
-#define MXT_TOUCH_XHICLIP	23
-#define MXT_TOUCH_YLOCLIP	24
-#define MXT_TOUCH_YHICLIP	25
-#define MXT_TOUCH_XEDGECTRL	26
-#define MXT_TOUCH_XEDGEDIST	27
-#define MXT_TOUCH_YEDGECTRL	28
-#define MXT_TOUCH_YEDGEDIST	29
-#define MXT_TOUCH_JUMPLIMIT	30
+#define MXT_T9_ORIENT		9
+#define MXT_T9_RANGE		18
+
+/* MXT_TOUCH_MULTI_T9 status */
+#define MXT_T9_UNGRIP		(1 << 0)
+#define MXT_T9_SUPPRESS		(1 << 1)
+#define MXT_T9_AMP		(1 << 2)
+#define MXT_T9_VECTOR		(1 << 3)
+#define MXT_T9_MOVE		(1 << 4)
+#define MXT_T9_RELEASE		(1 << 5)
+#define MXT_T9_PRESS		(1 << 6)
+#define MXT_T9_DETECT		(1 << 7)
+
+struct t9_range {
+	u16 x;
+	u16 y;
+} __packed;
+
+/* MXT_TOUCH_MULTI_T9 orient */
+#define MXT_T9_ORIENT_SWITCH	(1 << 0)
 
 /* MXT_PROCI_GRIPFACE_T20 field */
 #define MXT_GRIPFACE_CTRL	0
@@ -174,17 +168,16 @@
 
 /* Define for MXT_GEN_COMMAND_T6 */
 #define MXT_BOOT_VALUE		0xa5
+#define MXT_RESET_VALUE		0x01
 #define MXT_BACKUP_VALUE	0x55
+
+/* Delay times */
 #define MXT_BACKUP_TIME		50	/* msec */
 #define MXT_RESET_TIME		200	/* msec */
-
-#define MXT_FWRESET_TIME	175	/* msec */
-
-/* MXT_SPT_GPIOPWM_T19 field */
-#define MXT_GPIO0_MASK		0x04
-#define MXT_GPIO1_MASK		0x08
-#define MXT_GPIO2_MASK		0x10
-#define MXT_GPIO3_MASK		0x20
+#define MXT_RESET_TIMEOUT	3000	/* msec */
+#define MXT_CRC_TIMEOUT		1000	/* msec */
+#define MXT_FW_RESET_TIME	3000	/* msec */
+#define MXT_FW_CHG_TIMEOUT	300	/* msec */
 
 /* Command to unlock bootloader */
 #define MXT_UNLOCK_CMD_MSB	0xaa
@@ -198,21 +191,8 @@
 #define MXT_FRAME_CRC_PASS	0x04
 #define MXT_APP_CRC_FAIL	0x40	/* valid 7 8 bit only */
 #define MXT_BOOT_STATUS_MASK	0x3f
-
-/* Touch status */
-#define MXT_UNGRIP		(1 << 0)
-#define MXT_SUPPRESS		(1 << 1)
-#define MXT_AMP			(1 << 2)
-#define MXT_VECTOR		(1 << 3)
-#define MXT_MOVE		(1 << 4)
-#define MXT_RELEASE		(1 << 5)
-#define MXT_PRESS		(1 << 6)
-#define MXT_DETECT		(1 << 7)
-
-/* Touch orient bits */
-#define MXT_XY_SWITCH		(1 << 0)
-#define MXT_X_INVERT		(1 << 1)
-#define MXT_Y_INVERT		(1 << 2)
+#define MXT_BOOT_EXTENDED_ID	(1 << 5)
+#define MXT_BOOT_ID_MASK	0x1f
 
 /* Touchscreen absolute values */
 #define MXT_MAX_AREA		0xff
@@ -232,8 +212,8 @@ struct mxt_info {
 struct mxt_object {
 	u8 type;
 	u16 start_address;
-	u8 size;		/* Size of each instance - 1 */
-	u8 instances;		/* Number of instances - 1 */
+	u8 size_minus_one;
+	u8 instances_minus_one;
 	u8 num_report_ids;
 } __packed;
 
@@ -250,19 +230,40 @@ struct mxt_data {
 	const struct mxt_platform_data *pdata;
 	struct mxt_object *object_table;
 	struct mxt_info info;
-	bool is_tp;
-
 	unsigned int irq;
 	unsigned int max_x;
 	unsigned int max_y;
+	bool in_bootloader;
+	u32 config_crc;
+	u8 bootloader_addr;
 
 	/* Cached parameters from object table */
 	u8 T6_reportid;
+	u16 T6_address;
 	u8 T9_reportid_min;
 	u8 T9_reportid_max;
 	u8 T19_reportid;
+
+	/* for fw update in bootloader */
+	struct completion bl_completion;
+
+	/* for reset handling */
+	struct completion reset_completion;
+
+	/* for config update handling */
+	struct completion crc_completion;
 };
 
+static size_t mxt_obj_size(const struct mxt_object *obj)
+{
+	return obj->size_minus_one + 1;
+}
+
+static size_t mxt_obj_instances(const struct mxt_object *obj)
+{
+	return obj->instances_minus_one + 1;
+}
+
 static bool mxt_object_readable(unsigned int type)
 {
 	switch (type) {
@@ -334,60 +335,190 @@ static void mxt_dump_message(struct device *dev,
 		message->reportid, 7, message->message);
 }
 
-static int mxt_check_bootloader(struct i2c_client *client,
-				     unsigned int state)
+static int mxt_wait_for_completion(struct mxt_data *data,
+				   struct completion *comp,
+				   unsigned int timeout_ms)
+{
+	struct device *dev = &data->client->dev;
+	unsigned long timeout = msecs_to_jiffies(timeout_ms);
+	long ret;
+
+	ret = wait_for_completion_interruptible_timeout(comp, timeout);
+	if (ret < 0) {
+		return ret;
+	} else if (ret == 0) {
+		dev_err(dev, "Wait for completion timed out.\n");
+		return -ETIMEDOUT;
+	}
+	return 0;
+}
+
+static int mxt_bootloader_read(struct mxt_data *data,
+			       u8 *val, unsigned int count)
+{
+	int ret;
+	struct i2c_msg msg;
+
+	msg.addr = data->bootloader_addr;
+	msg.flags = data->client->flags & I2C_M_TEN;
+	msg.flags |= I2C_M_RD;
+	msg.len = count;
+	msg.buf = val;
+
+	ret = i2c_transfer(data->client->adapter, &msg, 1);
+
+	if (ret == 1) {
+		ret = 0;
+	} else {
+		ret = ret < 0 ? ret : -EIO;
+		dev_err(&data->client->dev, "%s: i2c recv failed (%d)\n",
+			__func__, ret);
+	}
+
+	return ret;
+}
+
+static int mxt_bootloader_write(struct mxt_data *data,
+				const u8 * const val, unsigned int count)
+{
+	int ret;
+	struct i2c_msg msg;
+
+	msg.addr = data->bootloader_addr;
+	msg.flags = data->client->flags & I2C_M_TEN;
+	msg.len = count;
+	msg.buf = (u8 *)val;
+
+	ret = i2c_transfer(data->client->adapter, &msg, 1);
+	if (ret == 1) {
+		ret = 0;
+	} else {
+		ret = ret < 0 ? ret : -EIO;
+		dev_err(&data->client->dev, "%s: i2c send failed (%d)\n",
+			__func__, ret);
+	}
+
+	return ret;
+}
+
+static int mxt_lookup_bootloader_address(struct mxt_data *data)
+{
+	u8 appmode = data->client->addr;
+	u8 bootloader;
+
+	switch (appmode) {
+	case 0x4a:
+	case 0x4b:
+	case 0x4c:
+	case 0x4d:
+	case 0x5a:
+	case 0x5b:
+		bootloader = appmode - 0x26;
+		break;
+	default:
+		dev_err(&data->client->dev,
+			"Appmode i2c address 0x%02x not found\n",
+			appmode);
+		return -EINVAL;
+	}
+
+	data->bootloader_addr = bootloader;
+	return 0;
+}
+
+static u8 mxt_get_bootloader_version(struct mxt_data *data, u8 val)
+{
+	struct device *dev = &data->client->dev;
+	u8 buf[3];
+
+	if (val & MXT_BOOT_EXTENDED_ID) {
+		if (mxt_bootloader_read(data, &buf[0], 3) != 0) {
+			dev_err(dev, "%s: i2c failure\n", __func__);
+			return val;
+		}
+
+		dev_dbg(dev, "Bootloader ID:%d Version:%d\n", buf[1], buf[2]);
+
+		return buf[0];
+	} else {
+		dev_dbg(dev, "Bootloader ID:%d\n", val & MXT_BOOT_ID_MASK);
+
+		return val;
+	}
+}
+
+static int mxt_check_bootloader(struct mxt_data *data, unsigned int state)
 {
+	struct device *dev = &data->client->dev;
 	u8 val;
+	int ret;
 
 recheck:
-	if (i2c_master_recv(client, &val, 1) != 1) {
-		dev_err(&client->dev, "%s: i2c recv failed\n", __func__);
-		return -EIO;
+	if (state != MXT_WAITING_BOOTLOAD_CMD) {
+		/*
+		 * In application update mode, the interrupt
+		 * line signals state transitions. We must wait for the
+		 * CHG assertion before reading the status byte.
+		 * Once the status byte has been read, the line is deasserted.
+		 */
+		ret = mxt_wait_for_completion(data, &data->bl_completion,
+					      MXT_FW_CHG_TIMEOUT);
+		if (ret) {
+			/*
+			 * TODO: handle -ERESTARTSYS better by terminating
+			 * fw update process before returning to userspace
+			 * by writing length 0x000 to device (iff we are in
+			 * WAITING_FRAME_DATA state).
+			 */
+			dev_err(dev, "Update wait error %d\n", ret);
+			return ret;
+		}
 	}
 
+	ret = mxt_bootloader_read(data, &val, 1);
+	if (ret)
+		return ret;
+
+	if (state == MXT_WAITING_BOOTLOAD_CMD)
+		val = mxt_get_bootloader_version(data, val);
+
 	switch (state) {
 	case MXT_WAITING_BOOTLOAD_CMD:
 	case MXT_WAITING_FRAME_DATA:
 		val &= ~MXT_BOOT_STATUS_MASK;
 		break;
 	case MXT_FRAME_CRC_PASS:
-		if (val == MXT_FRAME_CRC_CHECK)
+		if (val == MXT_FRAME_CRC_CHECK) {
 			goto recheck;
+		} else if (val == MXT_FRAME_CRC_FAIL) {
+			dev_err(dev, "Bootloader CRC fail\n");
+			return -EINVAL;
+		}
 		break;
 	default:
 		return -EINVAL;
 	}
 
 	if (val != state) {
-		dev_err(&client->dev, "Unvalid bootloader mode state\n");
+		dev_err(dev, "Invalid bootloader state %02X != %02X\n",
+			val, state);
 		return -EINVAL;
 	}
 
 	return 0;
 }
 
-static int mxt_unlock_bootloader(struct i2c_client *client)
+static int mxt_unlock_bootloader(struct mxt_data *data)
 {
+	int ret;
 	u8 buf[2];
 
 	buf[0] = MXT_UNLOCK_CMD_LSB;
 	buf[1] = MXT_UNLOCK_CMD_MSB;
 
-	if (i2c_master_send(client, buf, 2) != 2) {
-		dev_err(&client->dev, "%s: i2c send failed\n", __func__);
-		return -EIO;
-	}
-
-	return 0;
-}
-
-static int mxt_fw_write(struct i2c_client *client,
-			     const u8 *data, unsigned int frame_size)
-{
-	if (i2c_master_send(client, data, frame_size) != frame_size) {
-		dev_err(&client->dev, "%s: i2c send failed\n", __func__);
-		return -EIO;
-	}
+	ret = mxt_bootloader_write(data, buf, 2);
+	if (ret)
+		return ret;
 
 	return 0;
 }
@@ -427,11 +558,6 @@ static int __mxt_read_reg(struct i2c_client *client,
 	return ret;
 }
 
-static int mxt_read_reg(struct i2c_client *client, u16 reg, u8 *val)
-{
-	return __mxt_read_reg(client, reg, 1, val);
-}
-
 static int __mxt_write_reg(struct i2c_client *client, u16 reg, u16 len,
 			   const void *val)
 {
@@ -479,7 +605,7 @@ mxt_get_object(struct mxt_data *data, u8 type)
 			return object;
 	}
 
-	dev_err(&data->client->dev, "Invalid object type\n");
+	dev_err(&data->client->dev, "Invalid object type T%u\n", type);
 	return NULL;
 }
 
@@ -505,7 +631,7 @@ static int mxt_write_object(struct mxt_data *data,
 	u16 reg;
 
 	object = mxt_get_object(data, type);
-	if (!object || offset >= object->size + 1)
+	if (!object || offset >= mxt_obj_size(object))
 		return -EINVAL;
 
 	reg = object->start_address;
@@ -515,18 +641,25 @@ static int mxt_write_object(struct mxt_data *data,
 static void mxt_input_button(struct mxt_data *data, struct mxt_message *message)
 {
 	struct input_dev *input = data->input_dev;
+	const struct mxt_platform_data *pdata = data->pdata;
 	bool button;
 	int i;
 
 	/* Active-low switch */
-	for (i = 0; i < MXT_NUM_GPIO; i++) {
-		if (data->pdata->key_map[i] == KEY_RESERVED)
+	for (i = 0; i < pdata->t19_num_keys; i++) {
+		if (pdata->t19_keymap[i] == KEY_RESERVED)
 			continue;
-		button = !(message->message[0] & MXT_GPIO0_MASK << i);
-		input_report_key(input, data->pdata->key_map[i], button);
+		button = !(message->message[0] & (1 << i));
+		input_report_key(input, pdata->t19_keymap[i], button);
 	}
 }
 
+static void mxt_input_sync(struct input_dev *input_dev)
+{
+	input_mt_report_pointer_emulation(input_dev, false);
+	input_sync(input_dev);
+}
+
 static void mxt_input_touchevent(struct mxt_data *data,
 				      struct mxt_message *message, int id)
 {
@@ -536,44 +669,60 @@ static void mxt_input_touchevent(struct mxt_data *data,
 	int x;
 	int y;
 	int area;
-	int pressure;
+	int amplitude;
 
 	x = (message->message[1] << 4) | ((message->message[3] >> 4) & 0xf);
 	y = (message->message[2] << 4) | ((message->message[3] & 0xf));
+
+	/* Handle 10/12 bit switching */
 	if (data->max_x < 1024)
-		x = x >> 2;
+		x >>= 2;
 	if (data->max_y < 1024)
-		y = y >> 2;
+		y >>= 2;
 
 	area = message->message[4];
-	pressure = message->message[5];
+	amplitude = message->message[5];
 
 	dev_dbg(dev,
 		"[%u] %c%c%c%c%c%c%c%c x: %5u y: %5u area: %3u amp: %3u\n",
 		id,
-		(status & MXT_DETECT) ? 'D' : '.',
-		(status & MXT_PRESS) ? 'P' : '.',
-		(status & MXT_RELEASE) ? 'R' : '.',
-		(status & MXT_MOVE) ? 'M' : '.',
-		(status & MXT_VECTOR) ? 'V' : '.',
-		(status & MXT_AMP) ? 'A' : '.',
-		(status & MXT_SUPPRESS) ? 'S' : '.',
-		(status & MXT_UNGRIP) ? 'U' : '.',
-		x, y, area, pressure);
+		(status & MXT_T9_DETECT) ? 'D' : '.',
+		(status & MXT_T9_PRESS) ? 'P' : '.',
+		(status & MXT_T9_RELEASE) ? 'R' : '.',
+		(status & MXT_T9_MOVE) ? 'M' : '.',
+		(status & MXT_T9_VECTOR) ? 'V' : '.',
+		(status & MXT_T9_AMP) ? 'A' : '.',
+		(status & MXT_T9_SUPPRESS) ? 'S' : '.',
+		(status & MXT_T9_UNGRIP) ? 'U' : '.',
+		x, y, area, amplitude);
 
 	input_mt_slot(input_dev, id);
-	input_mt_report_slot_state(input_dev, MT_TOOL_FINGER,
-				   status & MXT_DETECT);
 
-	if (status & MXT_DETECT) {
+	if (status & MXT_T9_DETECT) {
+		/*
+		 * Multiple bits may be set if the host is slow to read
+		 * the status messages, indicating all the events that
+		 * have happened.
+		 */
+		if (status & MXT_T9_RELEASE) {
+			input_mt_report_slot_state(input_dev,
+						   MT_TOOL_FINGER, 0);
+			mxt_input_sync(input_dev);
+		}
+
+		/* Touch active */
+		input_mt_report_slot_state(input_dev, MT_TOOL_FINGER, 1);
 		input_report_abs(input_dev, ABS_MT_POSITION_X, x);
 		input_report_abs(input_dev, ABS_MT_POSITION_Y, y);
-		input_report_abs(input_dev, ABS_MT_PRESSURE, pressure);
+		input_report_abs(input_dev, ABS_MT_PRESSURE, amplitude);
 		input_report_abs(input_dev, ABS_MT_TOUCH_MAJOR, area);
+	} else {
+		/* Touch no longer active, close out slot */
+		input_mt_report_slot_state(input_dev, MT_TOOL_FINGER, 0);
 	}
 }
 
-static unsigned mxt_extract_T6_csum(const u8 *csum)
+static u16 mxt_extract_T6_csum(const u8 *csum)
 {
 	return csum[0] | (csum[1] << 8) | (csum[2] << 16);
 }
@@ -584,28 +733,37 @@ static bool mxt_is_T9_message(struct mxt_data *data, struct mxt_message *msg)
 	return (id >= data->T9_reportid_min && id <= data->T9_reportid_max);
 }
 
-static irqreturn_t mxt_interrupt(int irq, void *dev_id)
+static irqreturn_t mxt_process_messages_until_invalid(struct mxt_data *data)
 {
-	struct mxt_data *data = dev_id;
 	struct mxt_message message;
 	const u8 *payload = &message.message[0];
 	struct device *dev = &data->client->dev;
 	u8 reportid;
 	bool update_input = false;
+	u32 crc;
 
 	do {
 		if (mxt_read_message(data, &message)) {
 			dev_err(dev, "Failed to read message\n");
-			goto end;
+			return IRQ_NONE;
 		}
 
 		reportid = message.reportid;
 
 		if (reportid == data->T6_reportid) {
 			u8 status = payload[0];
-			unsigned csum = mxt_extract_T6_csum(&payload[1]);
+
+			crc = mxt_extract_T6_csum(&payload[1]);
+			if (crc != data->config_crc) {
+				data->config_crc = crc;
+				complete(&data->crc_completion);
+			}
+
 			dev_dbg(dev, "Status: %02x Config Checksum: %06x\n",
-				status, csum);
+				status, data->config_crc);
+
+			if (status & MXT_T6_STATUS_RESET)
+				complete(&data->reset_completion);
 		} else if (mxt_is_T9_message(data, &message)) {
 			int id = reportid - data->T9_reportid_min;
 			mxt_input_touchevent(data, &message, id);
@@ -618,15 +776,96 @@ static irqreturn_t mxt_interrupt(int irq, void *dev_id)
 		}
 	} while (reportid != 0xff);
 
-	if (update_input) {
-		input_mt_report_pointer_emulation(data->input_dev, false);
-		input_sync(data->input_dev);
-	}
+	if (update_input)
+		mxt_input_sync(data->input_dev);
 
-end:
 	return IRQ_HANDLED;
 }
 
+static irqreturn_t mxt_interrupt(int irq, void *dev_id)
+{
+	struct mxt_data *data = dev_id;
+
+	if (data->in_bootloader) {
+		/* bootloader state transition completion */
+		complete(&data->bl_completion);
+		return IRQ_HANDLED;
+	}
+
+	return mxt_process_messages_until_invalid(data);
+}
+
+static int mxt_t6_command(struct mxt_data *data, u16 cmd_offset,
+			  u8 value, bool wait)
+{
+	u16 reg;
+	u8 command_register;
+	int timeout_counter = 0;
+	int ret;
+
+	reg = data->T6_address + cmd_offset;
+
+	ret = mxt_write_reg(data->client, reg, value);
+	if (ret)
+		return ret;
+
+	if (!wait)
+		return 0;
+
+	do {
+		msleep(20);
+		ret = __mxt_read_reg(data->client, reg, 1, &command_register);
+		if (ret)
+			return ret;
+	} while (command_register != 0 && timeout_counter++ <= 100);
+
+	if (timeout_counter > 100) {
+		dev_err(&data->client->dev, "Command failed!\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int mxt_soft_reset(struct mxt_data *data)
+{
+	struct device *dev = &data->client->dev;
+	int ret = 0;
+
+	dev_info(dev, "Resetting chip\n");
+
+	reinit_completion(&data->reset_completion);
+
+	ret = mxt_t6_command(data, MXT_COMMAND_RESET, MXT_RESET_VALUE, false);
+	if (ret)
+		return ret;
+
+	ret = mxt_wait_for_completion(data, &data->reset_completion,
+				      MXT_RESET_TIMEOUT);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static void mxt_update_crc(struct mxt_data *data, u8 cmd, u8 value)
+{
+	/*
+	 * On failure, CRC is set to 0 and config will always be
+	 * downloaded.
+	 */
+	data->config_crc = 0;
+	reinit_completion(&data->crc_completion);
+
+	mxt_t6_command(data, cmd, value, true);
+
+	/*
+	 * Wait for crc message. On failure, CRC is set to 0 and config will
+	 * always be downloaded.
+	 */
+	mxt_wait_for_completion(data, &data->crc_completion, MXT_CRC_TIMEOUT);
+}
+
 static int mxt_check_reg_init(struct mxt_data *data)
 {
 	const struct mxt_platform_data *pdata = data->pdata;
@@ -641,13 +880,23 @@ static int mxt_check_reg_init(struct mxt_data *data)
 		return 0;
 	}
 
+	mxt_update_crc(data, MXT_COMMAND_REPORTALL, 1);
+
+	if (data->config_crc == pdata->config_crc) {
+		dev_info(dev, "Config CRC 0x%06X: OK\n", data->config_crc);
+		return 0;
+	}
+
+	dev_info(dev, "Config CRC 0x%06X: does not match 0x%06X\n",
+		 data->config_crc, pdata->config_crc);
+
 	for (i = 0; i < data->info.object_num; i++) {
 		object = data->object_table + i;
 
 		if (!mxt_object_writable(object->type))
 			continue;
 
-		size = (object->size + 1) * (object->instances + 1);
+		size = mxt_obj_size(object) * mxt_obj_instances(object);
 		if (index + size > pdata->config_length) {
 			dev_err(dev, "Not enough config data!\n");
 			return -EINVAL;
@@ -660,6 +909,14 @@ static int mxt_check_reg_init(struct mxt_data *data)
 		index += size;
 	}
 
+	mxt_update_crc(data, MXT_COMMAND_BACKUPNV, MXT_BACKUP_VALUE);
+
+	ret = mxt_soft_reset(data);
+	if (ret)
+		return ret;
+
+	dev_info(dev, "Config successfully updated\n");
+
 	return 0;
 }
 
@@ -685,54 +942,6 @@ static int mxt_make_highchg(struct mxt_data *data)
 	return 0;
 }
 
-static void mxt_handle_pdata(struct mxt_data *data)
-{
-	const struct mxt_platform_data *pdata = data->pdata;
-	u8 voltage;
-
-	/* Set touchscreen lines */
-	mxt_write_object(data, MXT_TOUCH_MULTI_T9, MXT_TOUCH_XSIZE,
-			pdata->x_line);
-	mxt_write_object(data, MXT_TOUCH_MULTI_T9, MXT_TOUCH_YSIZE,
-			pdata->y_line);
-
-	/* Set touchscreen orient */
-	mxt_write_object(data, MXT_TOUCH_MULTI_T9, MXT_TOUCH_ORIENT,
-			pdata->orient);
-
-	/* Set touchscreen burst length */
-	mxt_write_object(data, MXT_TOUCH_MULTI_T9,
-			MXT_TOUCH_BLEN, pdata->blen);
-
-	/* Set touchscreen threshold */
-	mxt_write_object(data, MXT_TOUCH_MULTI_T9,
-			MXT_TOUCH_TCHTHR, pdata->threshold);
-
-	/* Set touchscreen resolution */
-	mxt_write_object(data, MXT_TOUCH_MULTI_T9,
-			MXT_TOUCH_XRANGE_LSB, (pdata->x_size - 1) & 0xff);
-	mxt_write_object(data, MXT_TOUCH_MULTI_T9,
-			MXT_TOUCH_XRANGE_MSB, (pdata->x_size - 1) >> 8);
-	mxt_write_object(data, MXT_TOUCH_MULTI_T9,
-			MXT_TOUCH_YRANGE_LSB, (pdata->y_size - 1) & 0xff);
-	mxt_write_object(data, MXT_TOUCH_MULTI_T9,
-			MXT_TOUCH_YRANGE_MSB, (pdata->y_size - 1) >> 8);
-
-	/* Set touchscreen voltage */
-	if (pdata->voltage) {
-		if (pdata->voltage < MXT_VOLTAGE_DEFAULT) {
-			voltage = (MXT_VOLTAGE_DEFAULT - pdata->voltage) /
-				MXT_VOLTAGE_STEP;
-			voltage = 0xff - voltage + 1;
-		} else
-			voltage = (pdata->voltage - MXT_VOLTAGE_DEFAULT) /
-				MXT_VOLTAGE_STEP;
-
-		mxt_write_object(data, MXT_SPT_CTECONFIG_T28,
-				MXT_CTE_VOLTAGE, voltage);
-	}
-}
-
 static int mxt_get_info(struct mxt_data *data)
 {
 	struct i2c_client *client = data->client;
@@ -772,7 +981,7 @@ static int mxt_get_object_table(struct mxt_data *data)
 		if (object->num_report_ids) {
 			min_id = reportid;
 			reportid += object->num_report_ids *
-					(object->instances + 1);
+					mxt_obj_instances(object);
 			max_id = reportid - 1;
 		} else {
 			min_id = 0;
@@ -780,13 +989,15 @@ static int mxt_get_object_table(struct mxt_data *data)
 		}
 
 		dev_dbg(&data->client->dev,
-			"Type %2d Start %3d Size %3d Instances %2d ReportIDs %3u : %3u\n",
-			object->type, object->start_address, object->size + 1,
-			object->instances + 1, min_id, max_id);
+			"T%u Start:%u Size:%zu Instances:%zu Report IDs:%u-%u\n",
+			object->type, object->start_address,
+			mxt_obj_size(object), mxt_obj_instances(object),
+			min_id, max_id);
 
 		switch (object->type) {
 		case MXT_GEN_COMMAND_T6:
 			data->T6_reportid = min_id;
+			data->T6_address = object->start_address;
 			break;
 		case MXT_TOUCH_MULTI_T9:
 			data->T9_reportid_min = min_id;
@@ -811,12 +1022,59 @@ static void mxt_free_object_table(struct mxt_data *data)
 	data->T19_reportid = 0;
 }
 
+static int mxt_read_t9_resolution(struct mxt_data *data)
+{
+	struct i2c_client *client = data->client;
+	int error;
+	struct t9_range range;
+	unsigned char orient;
+	struct mxt_object *object;
+
+	object = mxt_get_object(data, MXT_TOUCH_MULTI_T9);
+	if (!object)
+		return -EINVAL;
+
+	error = __mxt_read_reg(client,
+			       object->start_address + MXT_T9_RANGE,
+			       sizeof(range), &range);
+	if (error)
+		return error;
+
+	le16_to_cpus(&range.x);
+	le16_to_cpus(&range.y);
+
+	error =  __mxt_read_reg(client,
+				object->start_address + MXT_T9_ORIENT,
+				1, &orient);
+	if (error)
+		return error;
+
+	/* Handle default values */
+	if (range.x == 0)
+		range.x = 1023;
+
+	if (range.y == 0)
+		range.y = 1023;
+
+	if (orient & MXT_T9_ORIENT_SWITCH) {
+		data->max_x = range.y;
+		data->max_y = range.x;
+	} else {
+		data->max_x = range.x;
+		data->max_y = range.y;
+	}
+
+	dev_dbg(&client->dev,
+		"Touchscreen size X%uY%u\n", data->max_x, data->max_y);
+
+	return 0;
+}
+
 static int mxt_initialize(struct mxt_data *data)
 {
 	struct i2c_client *client = data->client;
 	struct mxt_info *info = &data->info;
 	int error;
-	u8 val;
 
 	error = mxt_get_info(data);
 	if (error)
@@ -832,47 +1090,29 @@ static int mxt_initialize(struct mxt_data *data)
 
 	/* Get object table information */
 	error = mxt_get_object_table(data);
-	if (error)
+	if (error) {
+		dev_err(&client->dev, "Error %d reading object table\n", error);
 		goto err_free_object_table;
+	}
 
 	/* Check register init values */
 	error = mxt_check_reg_init(data);
-	if (error)
-		goto err_free_object_table;
-
-	mxt_handle_pdata(data);
-
-	/* Backup to memory */
-	mxt_write_object(data, MXT_GEN_COMMAND_T6,
-			MXT_COMMAND_BACKUPNV,
-			MXT_BACKUP_VALUE);
-	msleep(MXT_BACKUP_TIME);
-
-	/* Soft reset */
-	mxt_write_object(data, MXT_GEN_COMMAND_T6,
-			MXT_COMMAND_RESET, 1);
-	msleep(MXT_RESET_TIME);
-
-	/* Update matrix size at info struct */
-	error = mxt_read_reg(client, MXT_MATRIX_X_SIZE, &val);
-	if (error)
+	if (error) {
+		dev_err(&client->dev, "Error %d initializing configuration\n",
+			error);
 		goto err_free_object_table;
-	info->matrix_xsize = val;
+	}
 
-	error = mxt_read_reg(client, MXT_MATRIX_Y_SIZE, &val);
-	if (error)
+	error = mxt_read_t9_resolution(data);
+	if (error) {
+		dev_err(&client->dev, "Failed to initialize T9 resolution\n");
 		goto err_free_object_table;
-	info->matrix_ysize = val;
-
-	dev_info(&client->dev,
-			"Family ID: %u Variant ID: %u Major.Minor.Build: %u.%u.%02X\n",
-			info->family_id, info->variant_id, info->version >> 4,
-			info->version & 0xf, info->build);
+	}
 
 	dev_info(&client->dev,
-			"Matrix X Size: %u Matrix Y Size: %u Object Num: %u\n",
-			info->matrix_xsize, info->matrix_ysize,
-			info->object_num);
+		 "Family: %u Variant: %u Firmware V%u.%u.%02X Objects: %u\n",
+		 info->family_id, info->variant_id, info->version >> 4,
+		 info->version & 0xf, info->build, info->object_num);
 
 	return 0;
 
@@ -881,20 +1121,6 @@ err_free_object_table:
 	return error;
 }
 
-static void mxt_calc_resolution(struct mxt_data *data)
-{
-	unsigned int max_x = data->pdata->x_size - 1;
-	unsigned int max_y = data->pdata->y_size - 1;
-
-	if (data->pdata->orient & MXT_XY_SWITCH) {
-		data->max_x = max_y;
-		data->max_y = max_x;
-	} else {
-		data->max_x = max_x;
-		data->max_y = max_y;
-	}
-}
-
 /* Firmware Version is returned as Major.Minor.Build */
 static ssize_t mxt_fw_version_show(struct device *dev,
 				   struct device_attribute *attr, char *buf)
@@ -921,11 +1147,11 @@ static ssize_t mxt_show_instance(char *buf, int count,
 {
 	int i;
 
-	if (object->instances > 0)
+	if (mxt_obj_instances(object) > 1)
 		count += scnprintf(buf + count, PAGE_SIZE - count,
 				   "Instance %u\n", instance);
 
-	for (i = 0; i < object->size + 1; i++)
+	for (i = 0; i < mxt_obj_size(object); i++)
 		count += scnprintf(buf + count, PAGE_SIZE - count,
 				"\t[%2u]: %02x (%d)\n", i, val[i], val[i]);
 	count += scnprintf(buf + count, PAGE_SIZE - count, "\n");
@@ -958,8 +1184,8 @@ static ssize_t mxt_object_show(struct device *dev,
 		count += scnprintf(buf + count, PAGE_SIZE - count,
 				"T%u:\n", object->type);
 
-		for (j = 0; j < object->instances + 1; j++) {
-			u16 size = object->size + 1;
+		for (j = 0; j < mxt_obj_instances(object); j++) {
+			u16 size = mxt_obj_size(object);
 			u16 addr = object->start_address + j * size;
 
 			error = __mxt_read_reg(data->client, addr, size, obuf);
@@ -975,13 +1201,38 @@ done:
 	return error ?: count;
 }
 
+static int mxt_check_firmware_format(struct device *dev,
+				     const struct firmware *fw)
+{
+	unsigned int pos = 0;
+	char c;
+
+	while (pos < fw->size) {
+		c = *(fw->data + pos);
+
+		if (c < '0' || (c > '9' && c < 'A') || c > 'F')
+			return 0;
+
+		pos++;
+	}
+
+	/*
+	 * To convert file try:
+	 * xxd -r -p mXTXXX__APP_VX-X-XX.enc > maxtouch.fw
+	 */
+	dev_err(dev, "Aborting: firmware file must be in binary format\n");
+
+	return -EINVAL;
+}
+
 static int mxt_load_fw(struct device *dev, const char *fn)
 {
 	struct mxt_data *data = dev_get_drvdata(dev);
-	struct i2c_client *client = data->client;
 	const struct firmware *fw = NULL;
 	unsigned int frame_size;
 	unsigned int pos = 0;
+	unsigned int retry = 0;
+	unsigned int frame = 0;
 	int ret;
 
 	ret = request_firmware(&fw, fn, dev);
@@ -990,59 +1241,91 @@ static int mxt_load_fw(struct device *dev, const char *fn)
 		return ret;
 	}
 
+	/* Check for incorrect enc file */
+	ret = mxt_check_firmware_format(dev, fw);
+	if (ret)
+		goto release_firmware;
+
+	ret = mxt_lookup_bootloader_address(data);
+	if (ret)
+		goto release_firmware;
+
 	/* Change to the bootloader mode */
-	mxt_write_object(data, MXT_GEN_COMMAND_T6,
-			MXT_COMMAND_RESET, MXT_BOOT_VALUE);
+	data->in_bootloader = true;
+
+	ret = mxt_t6_command(data, MXT_COMMAND_RESET, MXT_BOOT_VALUE, false);
+	if (ret)
+		goto release_firmware;
+
 	msleep(MXT_RESET_TIME);
 
-	/* Change to slave address of bootloader */
-	if (client->addr == MXT_APP_LOW)
-		client->addr = MXT_BOOT_LOW;
-	else
-		client->addr = MXT_BOOT_HIGH;
+	reinit_completion(&data->bl_completion);
 
-	ret = mxt_check_bootloader(client, MXT_WAITING_BOOTLOAD_CMD);
+	ret = mxt_check_bootloader(data, MXT_WAITING_BOOTLOAD_CMD);
 	if (ret)
-		goto out;
+		goto disable_irq;
 
 	/* Unlock bootloader */
-	mxt_unlock_bootloader(client);
+	mxt_unlock_bootloader(data);
 
 	while (pos < fw->size) {
-		ret = mxt_check_bootloader(client,
-						MXT_WAITING_FRAME_DATA);
+		ret = mxt_check_bootloader(data, MXT_WAITING_FRAME_DATA);
 		if (ret)
-			goto out;
+			goto disable_irq;
 
 		frame_size = ((*(fw->data + pos) << 8) | *(fw->data + pos + 1));
 
-		/* We should add 2 at frame size as the the firmware data is not
-		 * included the CRC bytes.
-		 */
+		/* Take account of CRC bytes */
 		frame_size += 2;
 
 		/* Write one frame to device */
-		mxt_fw_write(client, fw->data + pos, frame_size);
-
-		ret = mxt_check_bootloader(client,
-						MXT_FRAME_CRC_PASS);
+		ret = mxt_bootloader_write(data, fw->data + pos, frame_size);
 		if (ret)
-			goto out;
+			goto disable_irq;
 
-		pos += frame_size;
+		ret = mxt_check_bootloader(data, MXT_FRAME_CRC_PASS);
+		if (ret) {
+			retry++;
 
-		dev_dbg(dev, "Updated %d bytes / %zd bytes\n", pos, fw->size);
+			/* Back off by 20ms per retry */
+			msleep(retry * 20);
+
+			if (retry > 20) {
+				dev_err(dev, "Retry count exceeded\n");
+				goto disable_irq;
+			}
+		} else {
+			retry = 0;
+			pos += frame_size;
+			frame++;
+		}
+
+		if (frame % 50 == 0)
+			dev_dbg(dev, "Sent %d frames, %d/%zd bytes\n",
+				frame, pos, fw->size);
 	}
 
-out:
-	release_firmware(fw);
+	/* Wait for flash. */
+	ret = mxt_wait_for_completion(data, &data->bl_completion,
+				      MXT_FW_RESET_TIME);
+	if (ret)
+		goto disable_irq;
 
-	/* Change to slave address of application */
-	if (client->addr == MXT_BOOT_LOW)
-		client->addr = MXT_APP_LOW;
-	else
-		client->addr = MXT_APP_HIGH;
+	dev_dbg(dev, "Sent %d frames, %d bytes\n", frame, pos);
 
+	/*
+	 * Wait for device to reset. Some bootloader versions do not assert
+	 * the CHG line after bootloading has finished, so ignore potential
+	 * errors.
+	 */
+	mxt_wait_for_completion(data, &data->bl_completion, MXT_FW_RESET_TIME);
+
+	data->in_bootloader = false;
+
+disable_irq:
+	disable_irq(data->irq);
+release_firmware:
+	release_firmware(fw);
 	return ret;
 }
 
@@ -1053,28 +1336,23 @@ static ssize_t mxt_update_fw_store(struct device *dev,
 	struct mxt_data *data = dev_get_drvdata(dev);
 	int error;
 
-	disable_irq(data->irq);
-
 	error = mxt_load_fw(dev, MXT_FW_NAME);
 	if (error) {
 		dev_err(dev, "The firmware update failed(%d)\n", error);
 		count = error;
 	} else {
-		dev_dbg(dev, "The firmware update succeeded\n");
-
-		/* Wait for reset */
-		msleep(MXT_FWRESET_TIME);
+		dev_info(dev, "The firmware update succeeded\n");
 
 		mxt_free_object_table(data);
 
 		mxt_initialize(data);
-	}
 
-	enable_irq(data->irq);
+		enable_irq(data->irq);
 
-	error = mxt_make_highchg(data);
-	if (error)
-		return error;
+		error = mxt_make_highchg(data);
+		if (error)
+			return error;
+	}
 
 	return count;
 }
@@ -1134,6 +1412,8 @@ static int mxt_probe(struct i2c_client *client,
 	struct input_dev *input_dev;
 	int error;
 	unsigned int num_mt_slots;
+	unsigned int mt_flags = 0;
+	int i;
 
 	if (!pdata)
 		return -EINVAL;
@@ -1146,10 +1426,7 @@ static int mxt_probe(struct i2c_client *client,
 		goto err_free_mem;
 	}
 
-	data->is_tp = pdata && pdata->is_tp;
-
-	input_dev->name = (data->is_tp) ? "Atmel maXTouch Touchpad" :
-					  "Atmel maXTouch Touchscreen";
+	input_dev->name = "Atmel maXTouch Touchscreen";
 	snprintf(data->phys, sizeof(data->phys), "i2c-%u-%04x/input0",
 		 client->adapter->nr, client->addr);
 
@@ -1165,7 +1442,9 @@ static int mxt_probe(struct i2c_client *client,
 	data->pdata = pdata;
 	data->irq = client->irq;
 
-	mxt_calc_resolution(data);
+	init_completion(&data->bl_completion);
+	init_completion(&data->reset_completion);
+	init_completion(&data->crc_completion);
 
 	error = mxt_initialize(data);
 	if (error)
@@ -1175,20 +1454,15 @@ static int mxt_probe(struct i2c_client *client,
 	__set_bit(EV_KEY, input_dev->evbit);
 	__set_bit(BTN_TOUCH, input_dev->keybit);
 
-	if (data->is_tp) {
-		int i;
-		__set_bit(INPUT_PROP_POINTER, input_dev->propbit);
+	if (pdata->t19_num_keys) {
 		__set_bit(INPUT_PROP_BUTTONPAD, input_dev->propbit);
 
-		for (i = 0; i < MXT_NUM_GPIO; i++)
-			if (pdata->key_map[i] != KEY_RESERVED)
-				__set_bit(pdata->key_map[i], input_dev->keybit);
+		for (i = 0; i < pdata->t19_num_keys; i++)
+			if (pdata->t19_keymap[i] != KEY_RESERVED)
+				input_set_capability(input_dev, EV_KEY,
+						     pdata->t19_keymap[i]);
 
-		__set_bit(BTN_TOOL_FINGER, input_dev->keybit);
-		__set_bit(BTN_TOOL_DOUBLETAP, input_dev->keybit);
-		__set_bit(BTN_TOOL_TRIPLETAP, input_dev->keybit);
-		__set_bit(BTN_TOOL_QUADTAP, input_dev->keybit);
-		__set_bit(BTN_TOOL_QUINTTAP, input_dev->keybit);
+		mt_flags |= INPUT_MT_POINTER;
 
 		input_abs_set_res(input_dev, ABS_X, MXT_PIXELS_PER_MM);
 		input_abs_set_res(input_dev, ABS_Y, MXT_PIXELS_PER_MM);
@@ -1196,6 +1470,8 @@ static int mxt_probe(struct i2c_client *client,
 				  MXT_PIXELS_PER_MM);
 		input_abs_set_res(input_dev, ABS_MT_POSITION_Y,
 				  MXT_PIXELS_PER_MM);
+
+		input_dev->name = "Atmel maXTouch Touchpad";
 	}
 
 	/* For single touch */
@@ -1208,7 +1484,7 @@ static int mxt_probe(struct i2c_client *client,
 
 	/* For multi touch */
 	num_mt_slots = data->T9_reportid_max - data->T9_reportid_min + 1;
-	error = input_mt_init_slots(input_dev, num_mt_slots, 0);
+	error = input_mt_init_slots(input_dev, num_mt_slots, mt_flags);
 	if (error)
 		goto err_free_object;
 	input_set_abs_params(input_dev, ABS_MT_TOUCH_MAJOR,
@@ -1236,12 +1512,18 @@ static int mxt_probe(struct i2c_client *client,
 		goto err_free_irq;
 
 	error = input_register_device(input_dev);
-	if (error)
+	if (error) {
+		dev_err(&client->dev, "Error %d registering input device\n",
+			error);
 		goto err_free_irq;
+	}
 
 	error = sysfs_create_group(&client->dev.kobj, &mxt_attr_group);
-	if (error)
+	if (error) {
+		dev_err(&client->dev, "Failure %d creating sysfs group\n",
+			error);
 		goto err_unregister_device;
+	}
 
 	return 0;
 
@@ -1294,11 +1576,7 @@ static int mxt_resume(struct device *dev)
 	struct mxt_data *data = i2c_get_clientdata(client);
 	struct input_dev *input_dev = data->input_dev;
 
-	/* Soft reset */
-	mxt_write_object(data, MXT_GEN_COMMAND_T6,
-			MXT_COMMAND_RESET, 1);
-
-	msleep(MXT_RESET_TIME);
+	mxt_soft_reset(data);
 
 	mutex_lock(&input_dev->mutex);
 
diff --git a/drivers/input/touchscreen/atmel_tsadcc.c b/drivers/input/touchscreen/atmel_tsadcc.c
deleted file mode 100644
index a7c9d6967d1..00000000000
--- a/drivers/input/touchscreen/atmel_tsadcc.c
+++ /dev/null
@@ -1,358 +0,0 @@
-/*
- *  Atmel Touch Screen Driver
- *
- *  Copyright (c) 2008 ATMEL
- *  Copyright (c) 2008 Dan Liang
- *  Copyright (c) 2008 TimeSys Corporation
- *  Copyright (c) 2008 Justin Waters
- *
- *  Based on touchscreen code from Atmel 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.
- */
-#include <linux/err.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/input.h>
-#include <linux/slab.h>
-#include <linux/interrupt.h>
-#include <linux/clk.h>
-#include <linux/platform_device.h>
-#include <linux/io.h>
-#include <linux/platform_data/atmel.h>
-#include <mach/cpu.h>
-
-/* Register definitions based on AT91SAM9RL64 preliminary draft datasheet */
-
-#define ATMEL_TSADCC_CR		0x00	/* Control register */
-#define   ATMEL_TSADCC_SWRST	(1 << 0)	/* Software Reset*/
-#define	  ATMEL_TSADCC_START	(1 << 1)	/* Start conversion */
-
-#define ATMEL_TSADCC_MR		0x04	/* Mode register */
-#define	  ATMEL_TSADCC_TSAMOD	(3    <<  0)	/* ADC mode */
-#define	    ATMEL_TSADCC_TSAMOD_ADC_ONLY_MODE	(0x0)	/* ADC Mode */
-#define	    ATMEL_TSADCC_TSAMOD_TS_ONLY_MODE	(0x1)	/* Touch Screen Only Mode */
-#define	  ATMEL_TSADCC_LOWRES	(1    <<  4)	/* Resolution selection */
-#define	  ATMEL_TSADCC_SLEEP	(1    <<  5)	/* Sleep mode */
-#define	  ATMEL_TSADCC_PENDET	(1    <<  6)	/* Pen Detect selection */
-#define	  ATMEL_TSADCC_PRES	(1    <<  7)	/* Pressure Measurement Selection */
-#define	  ATMEL_TSADCC_PRESCAL	(0x3f <<  8)	/* Prescalar Rate Selection */
-#define	  ATMEL_TSADCC_EPRESCAL	(0xff <<  8)	/* Prescalar Rate Selection (Extended) */
-#define	  ATMEL_TSADCC_STARTUP	(0x7f << 16)	/* Start Up time */
-#define	  ATMEL_TSADCC_SHTIM	(0xf  << 24)	/* Sample & Hold time */
-#define	  ATMEL_TSADCC_PENDBC	(0xf  << 28)	/* Pen Detect debouncing time */
-
-#define ATMEL_TSADCC_TRGR	0x08	/* Trigger register */
-#define	  ATMEL_TSADCC_TRGMOD	(7      <<  0)	/* Trigger mode */
-#define	    ATMEL_TSADCC_TRGMOD_NONE		(0 << 0)
-#define     ATMEL_TSADCC_TRGMOD_EXT_RISING	(1 << 0)
-#define     ATMEL_TSADCC_TRGMOD_EXT_FALLING	(2 << 0)
-#define     ATMEL_TSADCC_TRGMOD_EXT_ANY		(3 << 0)
-#define     ATMEL_TSADCC_TRGMOD_PENDET		(4 << 0)
-#define     ATMEL_TSADCC_TRGMOD_PERIOD		(5 << 0)
-#define     ATMEL_TSADCC_TRGMOD_CONTINUOUS	(6 << 0)
-#define   ATMEL_TSADCC_TRGPER	(0xffff << 16)	/* Trigger period */
-
-#define ATMEL_TSADCC_TSR	0x0C	/* Touch Screen register */
-#define	  ATMEL_TSADCC_TSFREQ	(0xf <<  0)	/* TS Frequency in Interleaved mode */
-#define	  ATMEL_TSADCC_TSSHTIM	(0xf << 24)	/* Sample & Hold time */
-
-#define ATMEL_TSADCC_CHER	0x10	/* Channel Enable register */
-#define ATMEL_TSADCC_CHDR	0x14	/* Channel Disable register */
-#define ATMEL_TSADCC_CHSR	0x18	/* Channel Status register */
-#define	  ATMEL_TSADCC_CH(n)	(1 << (n))	/* Channel number */
-
-#define ATMEL_TSADCC_SR		0x1C	/* Status register */
-#define	  ATMEL_TSADCC_EOC(n)	(1 << ((n)+0))	/* End of conversion for channel N */
-#define	  ATMEL_TSADCC_OVRE(n)	(1 << ((n)+8))	/* Overrun error for channel N */
-#define	  ATMEL_TSADCC_DRDY	(1 << 16)	/* Data Ready */
-#define	  ATMEL_TSADCC_GOVRE	(1 << 17)	/* General Overrun Error */
-#define	  ATMEL_TSADCC_ENDRX	(1 << 18)	/* End of RX Buffer */
-#define	  ATMEL_TSADCC_RXBUFF	(1 << 19)	/* TX Buffer full */
-#define	  ATMEL_TSADCC_PENCNT	(1 << 20)	/* Pen contact */
-#define	  ATMEL_TSADCC_NOCNT	(1 << 21)	/* No contact */
-
-#define ATMEL_TSADCC_LCDR	0x20	/* Last Converted Data register */
-#define	  ATMEL_TSADCC_DATA	(0x3ff << 0)	/* Channel data */
-
-#define ATMEL_TSADCC_IER	0x24	/* Interrupt Enable register */
-#define ATMEL_TSADCC_IDR	0x28	/* Interrupt Disable register */
-#define ATMEL_TSADCC_IMR	0x2C	/* Interrupt Mask register */
-#define ATMEL_TSADCC_CDR0	0x30	/* Channel Data 0 */
-#define ATMEL_TSADCC_CDR1	0x34	/* Channel Data 1 */
-#define ATMEL_TSADCC_CDR2	0x38	/* Channel Data 2 */
-#define ATMEL_TSADCC_CDR3	0x3C	/* Channel Data 3 */
-#define ATMEL_TSADCC_CDR4	0x40	/* Channel Data 4 */
-#define ATMEL_TSADCC_CDR5	0x44	/* Channel Data 5 */
-
-#define ATMEL_TSADCC_XPOS	0x50
-#define ATMEL_TSADCC_Z1DAT	0x54
-#define ATMEL_TSADCC_Z2DAT	0x58
-
-#define PRESCALER_VAL(x)	((x) >> 8)
-
-#define ADC_DEFAULT_CLOCK	100000
-
-struct atmel_tsadcc {
-	struct input_dev	*input;
-	char			phys[32];
-	struct clk		*clk;
-	int			irq;
-	unsigned int		prev_absx;
-	unsigned int		prev_absy;
-	unsigned char		bufferedmeasure;
-};
-
-static void __iomem		*tsc_base;
-
-#define atmel_tsadcc_read(reg)		__raw_readl(tsc_base + (reg))
-#define atmel_tsadcc_write(reg, val)	__raw_writel((val), tsc_base + (reg))
-
-static irqreturn_t atmel_tsadcc_interrupt(int irq, void *dev)
-{
-	struct atmel_tsadcc	*ts_dev = (struct atmel_tsadcc *)dev;
-	struct input_dev	*input_dev = ts_dev->input;
-
-	unsigned int status;
-	unsigned int reg;
-
-	status = atmel_tsadcc_read(ATMEL_TSADCC_SR);
-	status &= atmel_tsadcc_read(ATMEL_TSADCC_IMR);
-
-	if (status & ATMEL_TSADCC_NOCNT) {
-		/* Contact lost */
-		reg = atmel_tsadcc_read(ATMEL_TSADCC_MR) | ATMEL_TSADCC_PENDBC;
-
-		atmel_tsadcc_write(ATMEL_TSADCC_MR, reg);
-		atmel_tsadcc_write(ATMEL_TSADCC_TRGR, ATMEL_TSADCC_TRGMOD_NONE);
-		atmel_tsadcc_write(ATMEL_TSADCC_IDR,
-				   ATMEL_TSADCC_EOC(3) | ATMEL_TSADCC_NOCNT);
-		atmel_tsadcc_write(ATMEL_TSADCC_IER, ATMEL_TSADCC_PENCNT);
-
-		input_report_key(input_dev, BTN_TOUCH, 0);
-		ts_dev->bufferedmeasure = 0;
-		input_sync(input_dev);
-
-	} else if (status & ATMEL_TSADCC_PENCNT) {
-		/* Pen detected */
-		reg = atmel_tsadcc_read(ATMEL_TSADCC_MR);
-		reg &= ~ATMEL_TSADCC_PENDBC;
-
-		atmel_tsadcc_write(ATMEL_TSADCC_IDR, ATMEL_TSADCC_PENCNT);
-		atmel_tsadcc_write(ATMEL_TSADCC_MR, reg);
-		atmel_tsadcc_write(ATMEL_TSADCC_IER,
-				   ATMEL_TSADCC_EOC(3) | ATMEL_TSADCC_NOCNT);
-		atmel_tsadcc_write(ATMEL_TSADCC_TRGR,
-				   ATMEL_TSADCC_TRGMOD_PERIOD | (0x0FFF << 16));
-
-	} else if (status & ATMEL_TSADCC_EOC(3)) {
-		/* Conversion finished */
-
-		if (ts_dev->bufferedmeasure) {
-			/* Last measurement is always discarded, since it can
-			 * be erroneous.
-			 * Always report previous measurement */
-			input_report_abs(input_dev, ABS_X, ts_dev->prev_absx);
-			input_report_abs(input_dev, ABS_Y, ts_dev->prev_absy);
-			input_report_key(input_dev, BTN_TOUCH, 1);
-			input_sync(input_dev);
-		} else
-			ts_dev->bufferedmeasure = 1;
-
-		/* Now make new measurement */
-		ts_dev->prev_absx = atmel_tsadcc_read(ATMEL_TSADCC_CDR3) << 10;
-		ts_dev->prev_absx /= atmel_tsadcc_read(ATMEL_TSADCC_CDR2);
-
-		ts_dev->prev_absy = atmel_tsadcc_read(ATMEL_TSADCC_CDR1) << 10;
-		ts_dev->prev_absy /= atmel_tsadcc_read(ATMEL_TSADCC_CDR0);
-	}
-
-	return IRQ_HANDLED;
-}
-
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int atmel_tsadcc_probe(struct platform_device *pdev)
-{
-	struct atmel_tsadcc	*ts_dev;
-	struct input_dev	*input_dev;
-	struct resource		*res;
-	struct at91_tsadcc_data *pdata = dev_get_platdata(&pdev->dev);
-	int		err;
-	unsigned int	prsc;
-	unsigned int	reg;
-
-	if (!pdata)
-		return -EINVAL;
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "no mmio resource defined.\n");
-		return -ENXIO;
-	}
-
-	/* Allocate memory for device */
-	ts_dev = kzalloc(sizeof(struct atmel_tsadcc), GFP_KERNEL);
-	if (!ts_dev) {
-		dev_err(&pdev->dev, "failed to allocate memory.\n");
-		return -ENOMEM;
-	}
-	platform_set_drvdata(pdev, ts_dev);
-
-	input_dev = input_allocate_device();
-	if (!input_dev) {
-		dev_err(&pdev->dev, "failed to allocate input device.\n");
-		err = -EBUSY;
-		goto err_free_mem;
-	}
-
-	ts_dev->irq = platform_get_irq(pdev, 0);
-	if (ts_dev->irq < 0) {
-		dev_err(&pdev->dev, "no irq ID is designated.\n");
-		err = -ENODEV;
-		goto err_free_dev;
-	}
-
-	if (!request_mem_region(res->start, resource_size(res),
-				"atmel tsadcc regs")) {
-		dev_err(&pdev->dev, "resources is unavailable.\n");
-		err = -EBUSY;
-		goto err_free_dev;
-	}
-
-	tsc_base = ioremap(res->start, resource_size(res));
-	if (!tsc_base) {
-		dev_err(&pdev->dev, "failed to map registers.\n");
-		err = -ENOMEM;
-		goto err_release_mem;
-	}
-
-	err = request_irq(ts_dev->irq, atmel_tsadcc_interrupt, 0,
-			pdev->dev.driver->name, ts_dev);
-	if (err) {
-		dev_err(&pdev->dev, "failed to allocate irq.\n");
-		goto err_unmap_regs;
-	}
-
-	ts_dev->clk = clk_get(&pdev->dev, "tsc_clk");
-	if (IS_ERR(ts_dev->clk)) {
-		dev_err(&pdev->dev, "failed to get ts_clk\n");
-		err = PTR_ERR(ts_dev->clk);
-		goto err_free_irq;
-	}
-
-	ts_dev->input = input_dev;
-	ts_dev->bufferedmeasure = 0;
-
-	snprintf(ts_dev->phys, sizeof(ts_dev->phys),
-		 "%s/input0", dev_name(&pdev->dev));
-
-	input_dev->name = "atmel touch screen controller";
-	input_dev->phys = ts_dev->phys;
-	input_dev->dev.parent = &pdev->dev;
-
-	__set_bit(EV_ABS, input_dev->evbit);
-	input_set_abs_params(input_dev, ABS_X, 0, 0x3FF, 0, 0);
-	input_set_abs_params(input_dev, ABS_Y, 0, 0x3FF, 0, 0);
-
-	input_set_capability(input_dev, EV_KEY, BTN_TOUCH);
-
-	/* clk_enable() always returns 0, no need to check it */
-	clk_enable(ts_dev->clk);
-
-	prsc = clk_get_rate(ts_dev->clk);
-	dev_info(&pdev->dev, "Master clock is set at: %d Hz\n", prsc);
-
-	if (!pdata->adc_clock)
-		pdata->adc_clock = ADC_DEFAULT_CLOCK;
-
-	prsc = (prsc / (2 * pdata->adc_clock)) - 1;
-
-	/* saturate if this value is too high */
-	if (cpu_is_at91sam9rl()) {
-		if (prsc > PRESCALER_VAL(ATMEL_TSADCC_PRESCAL))
-			prsc = PRESCALER_VAL(ATMEL_TSADCC_PRESCAL);
-	} else {
-		if (prsc > PRESCALER_VAL(ATMEL_TSADCC_EPRESCAL))
-			prsc = PRESCALER_VAL(ATMEL_TSADCC_EPRESCAL);
-	}
-
-	dev_info(&pdev->dev, "Prescaler is set at: %d\n", prsc);
-
-	reg = ATMEL_TSADCC_TSAMOD_TS_ONLY_MODE		|
-		((0x00 << 5) & ATMEL_TSADCC_SLEEP)	|	/* Normal Mode */
-		((0x01 << 6) & ATMEL_TSADCC_PENDET)	|	/* Enable Pen Detect */
-		(prsc << 8)				|
-		((0x26 << 16) & ATMEL_TSADCC_STARTUP)	|
-		((pdata->pendet_debounce << 28) & ATMEL_TSADCC_PENDBC);
-
-	atmel_tsadcc_write(ATMEL_TSADCC_CR, ATMEL_TSADCC_SWRST);
-	atmel_tsadcc_write(ATMEL_TSADCC_MR, reg);
-	atmel_tsadcc_write(ATMEL_TSADCC_TRGR, ATMEL_TSADCC_TRGMOD_NONE);
-	atmel_tsadcc_write(ATMEL_TSADCC_TSR,
-		(pdata->ts_sample_hold_time << 24) & ATMEL_TSADCC_TSSHTIM);
-
-	atmel_tsadcc_read(ATMEL_TSADCC_SR);
-	atmel_tsadcc_write(ATMEL_TSADCC_IER, ATMEL_TSADCC_PENCNT);
-
-	/* All went ok, so register to the input system */
-	err = input_register_device(input_dev);
-	if (err)
-		goto err_fail;
-
-	return 0;
-
-err_fail:
-	clk_disable(ts_dev->clk);
-	clk_put(ts_dev->clk);
-err_free_irq:
-	free_irq(ts_dev->irq, ts_dev);
-err_unmap_regs:
-	iounmap(tsc_base);
-err_release_mem:
-	release_mem_region(res->start, resource_size(res));
-err_free_dev:
-	input_free_device(input_dev);
-err_free_mem:
-	kfree(ts_dev);
-	return err;
-}
-
-static int atmel_tsadcc_remove(struct platform_device *pdev)
-{
-	struct atmel_tsadcc *ts_dev = platform_get_drvdata(pdev);
-	struct resource *res;
-
-	free_irq(ts_dev->irq, ts_dev);
-
-	input_unregister_device(ts_dev->input);
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	iounmap(tsc_base);
-	release_mem_region(res->start, resource_size(res));
-
-	clk_disable(ts_dev->clk);
-	clk_put(ts_dev->clk);
-
-	kfree(ts_dev);
-
-	return 0;
-}
-
-static struct platform_driver atmel_tsadcc_driver = {
-	.probe		= atmel_tsadcc_probe,
-	.remove		= atmel_tsadcc_remove,
-	.driver		= {
-		.name	= "atmel_tsadcc",
-	},
-};
-module_platform_driver(atmel_tsadcc_driver);
-
-MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION("Atmel TouchScreen Driver");
-MODULE_AUTHOR("Dan Liang <dan.liang@atmel.com>");
-
diff --git a/drivers/input/touchscreen/auo-pixcir-ts.c b/drivers/input/touchscreen/auo-pixcir-ts.c
index d3f9f6b0f9b..7f3c9478778 100644
--- a/drivers/input/touchscreen/auo-pixcir-ts.c
+++ b/drivers/input/touchscreen/auo-pixcir-ts.c
@@ -679,7 +679,7 @@ static const struct i2c_device_id auo_pixcir_idtable[] = {
 MODULE_DEVICE_TABLE(i2c, auo_pixcir_idtable);
 
 #ifdef CONFIG_OF
-static struct of_device_id auo_pixcir_ts_dt_idtable[] = {
+static const struct of_device_id auo_pixcir_ts_dt_idtable[] = {
 	{ .compatible = "auo,auo_pixcir_ts" },
 	{},
 };
diff --git a/drivers/input/touchscreen/da9034-ts.c b/drivers/input/touchscreen/da9034-ts.c
index 8ccf7bb4028..cf6f4b31db4 100644
--- a/drivers/input/touchscreen/da9034-ts.c
+++ b/drivers/input/touchscreen/da9034-ts.c
@@ -301,10 +301,11 @@ static int da9034_touch_probe(struct platform_device *pdev)
 	struct da9034_touch_pdata *pdata = dev_get_platdata(&pdev->dev);
 	struct da9034_touch *touch;
 	struct input_dev *input_dev;
-	int ret;
+	int error;
 
-	touch = kzalloc(sizeof(struct da9034_touch), GFP_KERNEL);
-	if (touch == NULL) {
+	touch = devm_kzalloc(&pdev->dev, sizeof(struct da9034_touch),
+			     GFP_KERNEL);
+	if (!touch) {
 		dev_err(&pdev->dev, "failed to allocate driver data\n");
 		return -ENOMEM;
 	}
@@ -315,18 +316,18 @@ static int da9034_touch_probe(struct platform_device *pdev)
 		touch->interval_ms	= pdata->interval_ms;
 		touch->x_inverted	= pdata->x_inverted;
 		touch->y_inverted	= pdata->y_inverted;
-	} else
+	} else {
 		/* fallback into default */
 		touch->interval_ms	= 10;
+	}
 
 	INIT_DELAYED_WORK(&touch->tsi_work, da9034_tsi_work);
 	touch->notifier.notifier_call = da9034_touch_notifier;
 
-	input_dev = input_allocate_device();
+	input_dev = devm_input_allocate_device(&pdev->dev);
 	if (!input_dev) {
 		dev_err(&pdev->dev, "failed to allocate input device\n");
-		ret = -ENOMEM;
-		goto err_free_touch;
+		return -ENOMEM;
 	}
 
 	input_dev->name		= pdev->name;
@@ -346,26 +347,9 @@ static int da9034_touch_probe(struct platform_device *pdev)
 	touch->input_dev = input_dev;
 	input_set_drvdata(input_dev, touch);
 
-	ret = input_register_device(input_dev);
-	if (ret)
-		goto err_free_input;
-
-	platform_set_drvdata(pdev, touch);
-	return 0;
-
-err_free_input:
-	input_free_device(input_dev);
-err_free_touch:
-	kfree(touch);
-	return ret;
-}
-
-static int da9034_touch_remove(struct platform_device *pdev)
-{
-	struct da9034_touch *touch = platform_get_drvdata(pdev);
-
-	input_unregister_device(touch->input_dev);
-	kfree(touch);
+	error = input_register_device(input_dev);
+	if (error)
+		return error;
 
 	return 0;
 }
@@ -376,7 +360,6 @@ static struct platform_driver da9034_touch_driver = {
 		.owner	= THIS_MODULE,
 	},
 	.probe		= da9034_touch_probe,
-	.remove		= da9034_touch_remove,
 };
 module_platform_driver(da9034_touch_driver);
 
diff --git a/drivers/input/touchscreen/edt-ft5x06.c b/drivers/input/touchscreen/edt-ft5x06.c
index f8815bebc9e..d4f33992ad8 100644
--- a/drivers/input/touchscreen/edt-ft5x06.c
+++ b/drivers/input/touchscreen/edt-ft5x06.c
@@ -271,7 +271,7 @@ static int edt_ft5x06_register_write(struct edt_ft5x06_ts_data *tsdata,
 		wrbuf[0] = addr;
 		wrbuf[1] = value;
 
-		return edt_ft5x06_ts_readwrite(tsdata->client, 3,
+		return edt_ft5x06_ts_readwrite(tsdata->client, 2,
 					wrbuf, 0, NULL);
 
 	default:
diff --git a/drivers/input/touchscreen/egalax_ts.c b/drivers/input/touchscreen/egalax_ts.c
index e6bcb13680b..c8057847d71 100644
--- a/drivers/input/touchscreen/egalax_ts.c
+++ b/drivers/input/touchscreen/egalax_ts.c
@@ -262,7 +262,7 @@ static int egalax_ts_resume(struct device *dev)
 
 static SIMPLE_DEV_PM_OPS(egalax_ts_pm_ops, egalax_ts_suspend, egalax_ts_resume);
 
-static struct of_device_id egalax_ts_dt_ids[] = {
+static const struct of_device_id egalax_ts_dt_ids[] = {
 	{ .compatible = "eeti,egalax_ts" },
 	{ /* sentinel */ }
 };
diff --git a/drivers/input/touchscreen/intel-mid-touch.c b/drivers/input/touchscreen/intel-mid-touch.c
index 4f6b156144e..c38ca4a7e38 100644
--- a/drivers/input/touchscreen/intel-mid-touch.c
+++ b/drivers/input/touchscreen/intel-mid-touch.c
@@ -36,6 +36,7 @@
 #include <linux/irq.h>
 #include <linux/delay.h>
 #include <asm/intel_scu_ipc.h>
+#include <linux/device.h>
 
 /* PMIC Interrupt registers */
 #define PMIC_REG_ID1		0x00 /* PMIC ID1 register */
@@ -580,12 +581,17 @@ static int mrstouch_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	tsdev = kzalloc(sizeof(struct mrstouch_dev), GFP_KERNEL);
-	input = input_allocate_device();
-	if (!tsdev || !input) {
+	tsdev = devm_kzalloc(&pdev->dev, sizeof(struct mrstouch_dev),
+			     GFP_KERNEL);
+	if (!tsdev) {
 		dev_err(&pdev->dev, "unable to allocate memory\n");
-		err = -ENOMEM;
-		goto err_free_mem;
+		return -ENOMEM;
+	}
+
+	input = devm_input_allocate_device(&pdev->dev);
+	if (!input) {
+		dev_err(&pdev->dev, "unable to allocate input device\n");
+		return -ENOMEM;
 	}
 
 	tsdev->dev = &pdev->dev;
@@ -598,7 +604,7 @@ static int mrstouch_probe(struct platform_device *pdev)
 	err = mrstouch_adc_init(tsdev);
 	if (err) {
 		dev_err(&pdev->dev, "ADC initialization failed\n");
-		goto err_free_mem;
+		return err;
 	}
 
 	input->name = "mrst_touchscreen";
@@ -618,38 +624,20 @@ static int mrstouch_probe(struct platform_device *pdev)
 	input_set_abs_params(tsdev->input, ABS_PRESSURE,
 			     MRST_PRESSURE_MIN, MRST_PRESSURE_MAX, 0, 0);
 
-	err = request_threaded_irq(tsdev->irq, NULL, mrstouch_pendet_irq,
-				   IRQF_ONESHOT, "mrstouch", tsdev);
+	err = devm_request_threaded_irq(&pdev->dev, tsdev->irq, NULL,
+					mrstouch_pendet_irq, IRQF_ONESHOT,
+					"mrstouch", tsdev);
 	if (err) {
 		dev_err(tsdev->dev, "unable to allocate irq\n");
-		goto err_free_mem;
+		return err;
 	}
 
 	err = input_register_device(tsdev->input);
 	if (err) {
 		dev_err(tsdev->dev, "unable to register input device\n");
-		goto err_free_irq;
+		return err;
 	}
 
-	platform_set_drvdata(pdev, tsdev);
-	return 0;
-
-err_free_irq:
-	free_irq(tsdev->irq, tsdev);
-err_free_mem:
-	input_free_device(input);
-	kfree(tsdev);
-	return err;
-}
-
-static int mrstouch_remove(struct platform_device *pdev)
-{
-	struct mrstouch_dev *tsdev = platform_get_drvdata(pdev);
-
-	free_irq(tsdev->irq, tsdev);
-	input_unregister_device(tsdev->input);
-	kfree(tsdev);
-
 	return 0;
 }
 
@@ -659,7 +647,6 @@ static struct platform_driver mrstouch_driver = {
 		.owner	= THIS_MODULE,
 	},
 	.probe		= mrstouch_probe,
-	.remove		= mrstouch_remove,
 };
 module_platform_driver(mrstouch_driver);
 
diff --git a/drivers/input/touchscreen/lpc32xx_ts.c b/drivers/input/touchscreen/lpc32xx_ts.c
index 2058253b55d..bb47d3442a3 100644
--- a/drivers/input/touchscreen/lpc32xx_ts.c
+++ b/drivers/input/touchscreen/lpc32xx_ts.c
@@ -384,7 +384,7 @@ static const struct dev_pm_ops lpc32xx_ts_pm_ops = {
 #endif
 
 #ifdef CONFIG_OF
-static struct of_device_id lpc32xx_tsc_of_match[] = {
+static const struct of_device_id lpc32xx_tsc_of_match[] = {
 	{ .compatible = "nxp,lpc3220-tsc", },
 	{ },
 };
diff --git a/drivers/input/touchscreen/mcs5000_ts.c b/drivers/input/touchscreen/mcs5000_ts.c
index 647e36f5930..00510a9836b 100644
--- a/drivers/input/touchscreen/mcs5000_ts.c
+++ b/drivers/input/touchscreen/mcs5000_ts.c
@@ -161,10 +161,9 @@ static irqreturn_t mcs5000_ts_interrupt(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static void mcs5000_ts_phys_init(struct mcs5000_ts_data *data)
+static void mcs5000_ts_phys_init(struct mcs5000_ts_data *data,
+				 const struct mcs_platform_data *platform_data)
 {
-	const struct mcs_platform_data *platform_data =
-		data->platform_data;
 	struct i2c_client *client = data->client;
 
 	/* Touch reset & sleep mode */
@@ -187,28 +186,32 @@ static void mcs5000_ts_phys_init(struct mcs5000_ts_data *data)
 }
 
 static int mcs5000_ts_probe(struct i2c_client *client,
-		const struct i2c_device_id *id)
+			    const struct i2c_device_id *id)
 {
+	const struct mcs_platform_data *pdata;
 	struct mcs5000_ts_data *data;
 	struct input_dev *input_dev;
-	int ret;
+	int error;
 
-	if (!dev_get_platdata(&client->dev))
+	pdata = dev_get_platdata(&client->dev);
+	if (!pdata)
 		return -EINVAL;
 
-	data = kzalloc(sizeof(struct mcs5000_ts_data), GFP_KERNEL);
-	input_dev = input_allocate_device();
-	if (!data || !input_dev) {
+	data = devm_kzalloc(&client->dev, sizeof(*data), GFP_KERNEL);
+	if (!data) {
 		dev_err(&client->dev, "Failed to allocate memory\n");
-		ret = -ENOMEM;
-		goto err_free_mem;
+		return -ENOMEM;
 	}
 
 	data->client = client;
-	data->input_dev = input_dev;
-	data->platform_data = dev_get_platdata(&client->dev);
 
-	input_dev->name = "MELPAS MCS-5000 Touchscreen";
+	input_dev = devm_input_allocate_device(&client->dev);
+	if (!input_dev) {
+		dev_err(&client->dev, "Failed to allocate input device\n");
+		return -ENOMEM;
+	}
+
+	input_dev->name = "MELFAS MCS-5000 Touchscreen";
 	input_dev->id.bustype = BUS_I2C;
 	input_dev->dev.parent = &client->dev;
 
@@ -219,44 +222,30 @@ static int mcs5000_ts_probe(struct i2c_client *client,
 	input_set_abs_params(input_dev, ABS_Y, 0, MCS5000_MAX_YC, 0, 0);
 
 	input_set_drvdata(input_dev, data);
+	data->input_dev = input_dev;
 
-	if (data->platform_data->cfg_pin)
-		data->platform_data->cfg_pin();
-
-	ret = request_threaded_irq(client->irq, NULL, mcs5000_ts_interrupt,
-			IRQF_TRIGGER_LOW | IRQF_ONESHOT, "mcs5000_ts", data);
+	if (pdata->cfg_pin)
+		pdata->cfg_pin();
 
-	if (ret < 0) {
+	error = devm_request_threaded_irq(&client->dev, client->irq,
+					  NULL, mcs5000_ts_interrupt,
+					  IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+					  "mcs5000_ts", data);
+	if (error) {
 		dev_err(&client->dev, "Failed to register interrupt\n");
-		goto err_free_mem;
+		return error;
 	}
 
-	ret = input_register_device(data->input_dev);
-	if (ret < 0)
-		goto err_free_irq;
+	error = input_register_device(data->input_dev);
+	if (error) {
+		dev_err(&client->dev, "Failed to register input device\n");
+		return error;
+	}
 
-	mcs5000_ts_phys_init(data);
+	mcs5000_ts_phys_init(data, pdata);
 	i2c_set_clientdata(client, data);
 
 	return 0;
-
-err_free_irq:
-	free_irq(client->irq, data);
-err_free_mem:
-	input_free_device(input_dev);
-	kfree(data);
-	return ret;
-}
-
-static int mcs5000_ts_remove(struct i2c_client *client)
-{
-	struct mcs5000_ts_data *data = i2c_get_clientdata(client);
-
-	free_irq(client->irq, data);
-	input_unregister_device(data->input_dev);
-	kfree(data);
-
-	return 0;
 }
 
 #ifdef CONFIG_PM
@@ -274,14 +263,15 @@ static int mcs5000_ts_resume(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct mcs5000_ts_data *data = i2c_get_clientdata(client);
+	const struct mcs_platform_data *pdata = dev_get_platdata(dev);
 
-	mcs5000_ts_phys_init(data);
+	mcs5000_ts_phys_init(data, pdata);
 
 	return 0;
 }
+#endif
 
 static SIMPLE_DEV_PM_OPS(mcs5000_ts_pm, mcs5000_ts_suspend, mcs5000_ts_resume);
-#endif
 
 static const struct i2c_device_id mcs5000_ts_id[] = {
 	{ "mcs5000_ts", 0 },
@@ -291,12 +281,9 @@ MODULE_DEVICE_TABLE(i2c, mcs5000_ts_id);
 
 static struct i2c_driver mcs5000_ts_driver = {
 	.probe		= mcs5000_ts_probe,
-	.remove		= mcs5000_ts_remove,
 	.driver = {
 		.name = "mcs5000_ts",
-#ifdef CONFIG_PM
 		.pm   = &mcs5000_ts_pm,
-#endif
 	},
 	.id_table	= mcs5000_ts_id,
 };
diff --git a/drivers/input/touchscreen/mms114.c b/drivers/input/touchscreen/mms114.c
index 8a598c06539..372bbf7658f 100644
--- a/drivers/input/touchscreen/mms114.c
+++ b/drivers/input/touchscreen/mms114.c
@@ -456,7 +456,7 @@ static int mms114_probe(struct i2c_client *client,
 	data->input_dev = input_dev;
 	data->pdata = pdata;
 
-	input_dev->name = "MELPAS MMS114 Touchscreen";
+	input_dev->name = "MELFAS MMS114 Touchscreen";
 	input_dev->id.bustype = BUS_I2C;
 	input_dev->dev.parent = &client->dev;
 	input_dev->open = mms114_input_open;
@@ -570,7 +570,7 @@ static const struct i2c_device_id mms114_id[] = {
 MODULE_DEVICE_TABLE(i2c, mms114_id);
 
 #ifdef CONFIG_OF
-static struct of_device_id mms114_dt_match[] = {
+static const struct of_device_id mms114_dt_match[] = {
 	{ .compatible = "melfas,mms114" },
 	{ }
 };
diff --git a/drivers/input/touchscreen/of_touchscreen.c b/drivers/input/touchscreen/of_touchscreen.c
new file mode 100644
index 00000000000..f8f9b84230b
--- /dev/null
+++ b/drivers/input/touchscreen/of_touchscreen.c
@@ -0,0 +1,45 @@
+/*
+ *  Generic DT helper functions for touchscreen devices
+ *
+ *  Copyright (c) 2014 Sebastian Reichel <sre@kernel.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/of.h>
+#include <linux/input.h>
+#include <linux/input/touchscreen.h>
+
+/**
+ * touchscreen_parse_of_params - parse common touchscreen DT properties
+ * @dev: device that should be parsed
+ *
+ * This function parses common DT properties for touchscreens and setups the
+ * input device accordingly. The function keeps previously setuped default
+ * values if no value is specified via DT.
+ */
+void touchscreen_parse_of_params(struct input_dev *dev)
+{
+	struct device_node *np = dev->dev.parent->of_node;
+	struct input_absinfo *absinfo;
+
+	input_alloc_absinfo(dev);
+	if (!dev->absinfo)
+		return;
+
+	absinfo = &dev->absinfo[ABS_X];
+	of_property_read_u32(np, "touchscreen-size-x", &absinfo->maximum);
+	of_property_read_u32(np, "touchscreen-fuzz-x", &absinfo->fuzz);
+
+	absinfo = &dev->absinfo[ABS_Y];
+	of_property_read_u32(np, "touchscreen-size-y", &absinfo->maximum);
+	of_property_read_u32(np, "touchscreen-fuzz-y", &absinfo->fuzz);
+
+	absinfo = &dev->absinfo[ABS_PRESSURE];
+	of_property_read_u32(np, "touchscreen-max-pressure", &absinfo->maximum);
+	of_property_read_u32(np, "touchscreen-fuzz-pressure", &absinfo->fuzz);
+}
+EXPORT_SYMBOL(touchscreen_parse_of_params);
diff --git a/drivers/input/touchscreen/pixcir_i2c_ts.c b/drivers/input/touchscreen/pixcir_i2c_ts.c
index 02392d2061d..19c6c0fdc94 100644
--- a/drivers/input/touchscreen/pixcir_i2c_ts.c
+++ b/drivers/input/touchscreen/pixcir_i2c_ts.c
@@ -24,12 +24,13 @@
 #include <linux/i2c.h>
 #include <linux/input.h>
 #include <linux/input/pixcir_ts.h>
+#include <linux/gpio.h>
 
 struct pixcir_i2c_ts_data {
 	struct i2c_client *client;
 	struct input_dev *input;
 	const struct pixcir_ts_platform_data *chip;
-	bool exiting;
+	bool running;
 };
 
 static void pixcir_ts_poscheck(struct pixcir_i2c_ts_data *data)
@@ -87,11 +88,12 @@ static void pixcir_ts_poscheck(struct pixcir_i2c_ts_data *data)
 static irqreturn_t pixcir_ts_isr(int irq, void *dev_id)
 {
 	struct pixcir_i2c_ts_data *tsdata = dev_id;
+	const struct pixcir_ts_platform_data *pdata = tsdata->chip;
 
-	while (!tsdata->exiting) {
+	while (tsdata->running) {
 		pixcir_ts_poscheck(tsdata);
 
-		if (tsdata->chip->attb_read_val())
+		if (gpio_get_value(pdata->gpio_attb))
 			break;
 
 		msleep(20);
@@ -100,25 +102,221 @@ static irqreturn_t pixcir_ts_isr(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
+static int pixcir_set_power_mode(struct pixcir_i2c_ts_data *ts,
+				 enum pixcir_power_mode mode)
+{
+	struct device *dev = &ts->client->dev;
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(ts->client, PIXCIR_REG_POWER_MODE);
+	if (ret < 0) {
+		dev_err(dev, "%s: can't read reg 0x%x : %d\n",
+			__func__, PIXCIR_REG_POWER_MODE, ret);
+		return ret;
+	}
+
+	ret &= ~PIXCIR_POWER_MODE_MASK;
+	ret |= mode;
+
+	/* Always AUTO_IDLE */
+	ret |= PIXCIR_POWER_ALLOW_IDLE;
+
+	ret = i2c_smbus_write_byte_data(ts->client, PIXCIR_REG_POWER_MODE, ret);
+	if (ret < 0) {
+		dev_err(dev, "%s: can't write reg 0x%x : %d\n",
+			__func__, PIXCIR_REG_POWER_MODE, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * Set the interrupt mode for the device i.e. ATTB line behaviour
+ *
+ * @polarity : 1 for active high, 0 for active low.
+ */
+static int pixcir_set_int_mode(struct pixcir_i2c_ts_data *ts,
+			       enum pixcir_int_mode mode, bool polarity)
+{
+	struct device *dev = &ts->client->dev;
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(ts->client, PIXCIR_REG_INT_MODE);
+	if (ret < 0) {
+		dev_err(dev, "%s: can't read reg 0x%x : %d\n",
+			__func__, PIXCIR_REG_INT_MODE, ret);
+		return ret;
+	}
+
+	ret &= ~PIXCIR_INT_MODE_MASK;
+	ret |= mode;
+
+	if (polarity)
+		ret |= PIXCIR_INT_POL_HIGH;
+	else
+		ret &= ~PIXCIR_INT_POL_HIGH;
+
+	ret = i2c_smbus_write_byte_data(ts->client, PIXCIR_REG_INT_MODE, ret);
+	if (ret < 0) {
+		dev_err(dev, "%s: can't write reg 0x%x : %d\n",
+			__func__, PIXCIR_REG_INT_MODE, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * Enable/disable interrupt generation
+ */
+static int pixcir_int_enable(struct pixcir_i2c_ts_data *ts, bool enable)
+{
+	struct device *dev = &ts->client->dev;
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(ts->client, PIXCIR_REG_INT_MODE);
+	if (ret < 0) {
+		dev_err(dev, "%s: can't read reg 0x%x : %d\n",
+			__func__, PIXCIR_REG_INT_MODE, ret);
+		return ret;
+	}
+
+	if (enable)
+		ret |= PIXCIR_INT_ENABLE;
+	else
+		ret &= ~PIXCIR_INT_ENABLE;
+
+	ret = i2c_smbus_write_byte_data(ts->client, PIXCIR_REG_INT_MODE, ret);
+	if (ret < 0) {
+		dev_err(dev, "%s: can't write reg 0x%x : %d\n",
+			__func__, PIXCIR_REG_INT_MODE, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int pixcir_start(struct pixcir_i2c_ts_data *ts)
+{
+	struct device *dev = &ts->client->dev;
+	int error;
+
+	/* LEVEL_TOUCH interrupt with active low polarity */
+	error = pixcir_set_int_mode(ts, PIXCIR_INT_LEVEL_TOUCH, 0);
+	if (error) {
+		dev_err(dev, "Failed to set interrupt mode: %d\n", error);
+		return error;
+	}
+
+	ts->running = true;
+	mb();	/* Update status before IRQ can fire */
+
+	/* enable interrupt generation */
+	error = pixcir_int_enable(ts, true);
+	if (error) {
+		dev_err(dev, "Failed to enable interrupt generation: %d\n",
+			error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int pixcir_stop(struct pixcir_i2c_ts_data *ts)
+{
+	int error;
+
+	/* Disable interrupt generation */
+	error = pixcir_int_enable(ts, false);
+	if (error) {
+		dev_err(&ts->client->dev,
+			"Failed to disable interrupt generation: %d\n",
+			error);
+		return error;
+	}
+
+	/* Exit ISR if running, no more report parsing */
+	ts->running = false;
+	mb();	/* update status before we synchronize irq */
+
+	/* Wait till running ISR is complete */
+	synchronize_irq(ts->client->irq);
+
+	return 0;
+}
+
+static int pixcir_input_open(struct input_dev *dev)
+{
+	struct pixcir_i2c_ts_data *ts = input_get_drvdata(dev);
+
+	return pixcir_start(ts);
+}
+
+static void pixcir_input_close(struct input_dev *dev)
+{
+	struct pixcir_i2c_ts_data *ts = input_get_drvdata(dev);
+
+	pixcir_stop(ts);
+}
+
 #ifdef CONFIG_PM_SLEEP
 static int pixcir_i2c_ts_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
+	struct pixcir_i2c_ts_data *ts = i2c_get_clientdata(client);
+	struct input_dev *input = ts->input;
+	int ret = 0;
+
+	mutex_lock(&input->mutex);
+
+	if (device_may_wakeup(&client->dev)) {
+		if (!input->users) {
+			ret = pixcir_start(ts);
+			if (ret) {
+				dev_err(dev, "Failed to start\n");
+				goto unlock;
+			}
+		}
 
-	if (device_may_wakeup(&client->dev))
 		enable_irq_wake(client->irq);
+	} else if (input->users) {
+		ret = pixcir_stop(ts);
+	}
 
-	return 0;
+unlock:
+	mutex_unlock(&input->mutex);
+
+	return ret;
 }
 
 static int pixcir_i2c_ts_resume(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
+	struct pixcir_i2c_ts_data *ts = i2c_get_clientdata(client);
+	struct input_dev *input = ts->input;
+	int ret = 0;
+
+	mutex_lock(&input->mutex);
 
-	if (device_may_wakeup(&client->dev))
+	if (device_may_wakeup(&client->dev)) {
 		disable_irq_wake(client->irq);
 
-	return 0;
+		if (!input->users) {
+			ret = pixcir_stop(ts);
+			if (ret) {
+				dev_err(dev, "Failed to stop\n");
+				goto unlock;
+			}
+		}
+	} else if (input->users) {
+		ret = pixcir_start(ts);
+	}
+
+unlock:
+	mutex_unlock(&input->mutex);
+
+	return ret;
 }
 #endif
 
@@ -130,6 +328,7 @@ static int pixcir_i2c_ts_probe(struct i2c_client *client,
 {
 	const struct pixcir_ts_platform_data *pdata =
 			dev_get_platdata(&client->dev);
+	struct device *dev = &client->dev;
 	struct pixcir_i2c_ts_data *tsdata;
 	struct input_dev *input;
 	int error;
@@ -139,12 +338,19 @@ static int pixcir_i2c_ts_probe(struct i2c_client *client,
 		return -EINVAL;
 	}
 
-	tsdata = kzalloc(sizeof(*tsdata), GFP_KERNEL);
-	input = input_allocate_device();
-	if (!tsdata || !input) {
-		dev_err(&client->dev, "Failed to allocate driver data!\n");
-		error = -ENOMEM;
-		goto err_free_mem;
+	if (!gpio_is_valid(pdata->gpio_attb)) {
+		dev_err(dev, "Invalid gpio_attb in pdata\n");
+		return -EINVAL;
+	}
+
+	tsdata = devm_kzalloc(dev, sizeof(*tsdata), GFP_KERNEL);
+	if (!tsdata)
+		return -ENOMEM;
+
+	input = devm_input_allocate_device(dev);
+	if (!input) {
+		dev_err(dev, "Failed to allocate input device\n");
+		return -ENOMEM;
 	}
 
 	tsdata->client = client;
@@ -153,6 +359,8 @@ static int pixcir_i2c_ts_probe(struct i2c_client *client,
 
 	input->name = client->name;
 	input->id.bustype = BUS_I2C;
+	input->open = pixcir_input_open;
+	input->close = pixcir_input_close;
 	input->dev.parent = &client->dev;
 
 	__set_bit(EV_KEY, input->evbit);
@@ -165,44 +373,47 @@ static int pixcir_i2c_ts_probe(struct i2c_client *client,
 
 	input_set_drvdata(input, tsdata);
 
-	error = request_threaded_irq(client->irq, NULL, pixcir_ts_isr,
-				     IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
-				     client->name, tsdata);
+	error = devm_gpio_request_one(dev, pdata->gpio_attb,
+				      GPIOF_DIR_IN, "pixcir_i2c_attb");
 	if (error) {
-		dev_err(&client->dev, "Unable to request touchscreen IRQ.\n");
-		goto err_free_mem;
+		dev_err(dev, "Failed to request ATTB gpio\n");
+		return error;
 	}
 
+	error = devm_request_threaded_irq(dev, client->irq, NULL, pixcir_ts_isr,
+					  IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+					  client->name, tsdata);
+	if (error) {
+		dev_err(dev, "failed to request irq %d\n", client->irq);
+		return error;
+	}
+
+	/* Always be in IDLE mode to save power, device supports auto wake */
+	error = pixcir_set_power_mode(tsdata, PIXCIR_POWER_IDLE);
+	if (error) {
+		dev_err(dev, "Failed to set IDLE mode\n");
+		return error;
+	}
+
+	/* Stop device till opened */
+	error = pixcir_stop(tsdata);
+	if (error)
+		return error;
+
 	error = input_register_device(input);
 	if (error)
-		goto err_free_irq;
+		return error;
 
 	i2c_set_clientdata(client, tsdata);
 	device_init_wakeup(&client->dev, 1);
 
 	return 0;
-
-err_free_irq:
-	free_irq(client->irq, tsdata);
-err_free_mem:
-	input_free_device(input);
-	kfree(tsdata);
-	return error;
 }
 
 static int pixcir_i2c_ts_remove(struct i2c_client *client)
 {
-	struct pixcir_i2c_ts_data *tsdata = i2c_get_clientdata(client);
-
 	device_init_wakeup(&client->dev, 0);
 
-	tsdata->exiting = true;
-	mb();
-	free_irq(client->irq, tsdata);
-
-	input_unregister_device(tsdata->input);
-	kfree(tsdata);
-
 	return 0;
 }
 
diff --git a/drivers/input/touchscreen/sun4i-ts.c b/drivers/input/touchscreen/sun4i-ts.c
new file mode 100644
index 00000000000..2ba82602495
--- /dev/null
+++ b/drivers/input/touchscreen/sun4i-ts.c
@@ -0,0 +1,339 @@
+/*
+ * Allwinner sunxi resistive touchscreen controller driver
+ *
+ * Copyright (C) 2013 - 2014 Hans de Goede <hdegoede@redhat.com>
+ *
+ * The hwmon parts are based on work by Corentin LABBE which is:
+ * Copyright (C) 2013 Corentin LABBE <clabbe.montjoie@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.
+ */
+
+/*
+ * The sun4i-ts controller is capable of detecting a second touch, but when a
+ * second touch is present then the accuracy becomes so bad the reported touch
+ * location is not useable.
+ *
+ * The original android driver contains some complicated heuristics using the
+ * aprox. distance between the 2 touches to see if the user is making a pinch
+ * open / close movement, and then reports emulated multi-touch events around
+ * the last touch coordinate (as the dual-touch coordinates are worthless).
+ *
+ * These kinds of heuristics are just asking for trouble (and don't belong
+ * in the kernel). So this driver offers straight forward, reliable single
+ * touch functionality only.
+ */
+
+#include <linux/err.h>
+#include <linux/hwmon.h>
+#include <linux/init.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#define TP_CTRL0		0x00
+#define TP_CTRL1		0x04
+#define TP_CTRL2		0x08
+#define TP_CTRL3		0x0c
+#define TP_INT_FIFOC		0x10
+#define TP_INT_FIFOS		0x14
+#define TP_TPR			0x18
+#define TP_CDAT			0x1c
+#define TEMP_DATA		0x20
+#define TP_DATA			0x24
+
+/* TP_CTRL0 bits */
+#define ADC_FIRST_DLY(x)	((x) << 24) /* 8 bits */
+#define ADC_FIRST_DLY_MODE(x)	((x) << 23)
+#define ADC_CLK_SEL(x)		((x) << 22)
+#define ADC_CLK_DIV(x)		((x) << 20) /* 3 bits */
+#define FS_DIV(x)		((x) << 16) /* 4 bits */
+#define T_ACQ(x)		((x) << 0) /* 16 bits */
+
+/* TP_CTRL1 bits */
+#define STYLUS_UP_DEBOUN(x)	((x) << 12) /* 8 bits */
+#define STYLUS_UP_DEBOUN_EN(x)	((x) << 9)
+#define TOUCH_PAN_CALI_EN(x)	((x) << 6)
+#define TP_DUAL_EN(x)		((x) << 5)
+#define TP_MODE_EN(x)		((x) << 4)
+#define TP_ADC_SELECT(x)	((x) << 3)
+#define ADC_CHAN_SELECT(x)	((x) << 0)  /* 3 bits */
+
+/* TP_CTRL2 bits */
+#define TP_SENSITIVE_ADJUST(x)	((x) << 28) /* 4 bits */
+#define TP_MODE_SELECT(x)	((x) << 26) /* 2 bits */
+#define PRE_MEA_EN(x)		((x) << 24)
+#define PRE_MEA_THRE_CNT(x)	((x) << 0) /* 24 bits */
+
+/* TP_CTRL3 bits */
+#define FILTER_EN(x)		((x) << 2)
+#define FILTER_TYPE(x)		((x) << 0)  /* 2 bits */
+
+/* TP_INT_FIFOC irq and fifo mask / control bits */
+#define TEMP_IRQ_EN(x)		((x) << 18)
+#define OVERRUN_IRQ_EN(x)	((x) << 17)
+#define DATA_IRQ_EN(x)		((x) << 16)
+#define TP_DATA_XY_CHANGE(x)	((x) << 13)
+#define FIFO_TRIG(x)		((x) << 8)  /* 5 bits */
+#define DATA_DRQ_EN(x)		((x) << 7)
+#define FIFO_FLUSH(x)		((x) << 4)
+#define TP_UP_IRQ_EN(x)		((x) << 1)
+#define TP_DOWN_IRQ_EN(x)	((x) << 0)
+
+/* TP_INT_FIFOS irq and fifo status bits */
+#define TEMP_DATA_PENDING	BIT(18)
+#define FIFO_OVERRUN_PENDING	BIT(17)
+#define FIFO_DATA_PENDING	BIT(16)
+#define TP_IDLE_FLG		BIT(2)
+#define TP_UP_PENDING		BIT(1)
+#define TP_DOWN_PENDING		BIT(0)
+
+/* TP_TPR bits */
+#define TEMP_ENABLE(x)		((x) << 16)
+#define TEMP_PERIOD(x)		((x) << 0)  /* t = x * 256 * 16 / clkin */
+
+struct sun4i_ts_data {
+	struct device *dev;
+	struct input_dev *input;
+	void __iomem *base;
+	unsigned int irq;
+	bool ignore_fifo_data;
+	int temp_data;
+};
+
+static void sun4i_ts_irq_handle_input(struct sun4i_ts_data *ts, u32 reg_val)
+{
+	u32 x, y;
+
+	if (reg_val & FIFO_DATA_PENDING) {
+		x = readl(ts->base + TP_DATA);
+		y = readl(ts->base + TP_DATA);
+		/* The 1st location reported after an up event is unreliable */
+		if (!ts->ignore_fifo_data) {
+			input_report_abs(ts->input, ABS_X, x);
+			input_report_abs(ts->input, ABS_Y, y);
+			/*
+			 * The hardware has a separate down status bit, but
+			 * that gets set before we get the first location,
+			 * resulting in reporting a click on the old location.
+			 */
+			input_report_key(ts->input, BTN_TOUCH, 1);
+			input_sync(ts->input);
+		} else {
+			ts->ignore_fifo_data = false;
+		}
+	}
+
+	if (reg_val & TP_UP_PENDING) {
+		ts->ignore_fifo_data = true;
+		input_report_key(ts->input, BTN_TOUCH, 0);
+		input_sync(ts->input);
+	}
+}
+
+static irqreturn_t sun4i_ts_irq(int irq, void *dev_id)
+{
+	struct sun4i_ts_data *ts = dev_id;
+	u32 reg_val;
+
+	reg_val  = readl(ts->base + TP_INT_FIFOS);
+
+	if (reg_val & TEMP_DATA_PENDING)
+		ts->temp_data = readl(ts->base + TEMP_DATA);
+
+	if (ts->input)
+		sun4i_ts_irq_handle_input(ts, reg_val);
+
+	writel(reg_val, ts->base + TP_INT_FIFOS);
+
+	return IRQ_HANDLED;
+}
+
+static int sun4i_ts_open(struct input_dev *dev)
+{
+	struct sun4i_ts_data *ts = input_get_drvdata(dev);
+
+	/* Flush, set trig level to 1, enable temp, data and up irqs */
+	writel(TEMP_IRQ_EN(1) | DATA_IRQ_EN(1) | FIFO_TRIG(1) | FIFO_FLUSH(1) |
+		TP_UP_IRQ_EN(1), ts->base + TP_INT_FIFOC);
+
+	return 0;
+}
+
+static void sun4i_ts_close(struct input_dev *dev)
+{
+	struct sun4i_ts_data *ts = input_get_drvdata(dev);
+
+	/* Deactivate all input IRQs */
+	writel(TEMP_IRQ_EN(1), ts->base + TP_INT_FIFOC);
+}
+
+static ssize_t show_temp(struct device *dev, struct device_attribute *devattr,
+			 char *buf)
+{
+	struct sun4i_ts_data *ts = dev_get_drvdata(dev);
+
+	/* No temp_data until the first irq */
+	if (ts->temp_data == -1)
+		return -EAGAIN;
+
+	return sprintf(buf, "%d\n", (ts->temp_data - 1447) * 100);
+}
+
+static ssize_t show_temp_label(struct device *dev,
+			      struct device_attribute *devattr, char *buf)
+{
+	return sprintf(buf, "SoC temperature\n");
+}
+
+static DEVICE_ATTR(temp1_input, S_IRUGO, show_temp, NULL);
+static DEVICE_ATTR(temp1_label, S_IRUGO, show_temp_label, NULL);
+
+static struct attribute *sun4i_ts_attrs[] = {
+	&dev_attr_temp1_input.attr,
+	&dev_attr_temp1_label.attr,
+	NULL
+};
+ATTRIBUTE_GROUPS(sun4i_ts);
+
+static int sun4i_ts_probe(struct platform_device *pdev)
+{
+	struct sun4i_ts_data *ts;
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	struct device *hwmon;
+	int error;
+	bool ts_attached;
+
+	ts = devm_kzalloc(dev, sizeof(struct sun4i_ts_data), GFP_KERNEL);
+	if (!ts)
+		return -ENOMEM;
+
+	ts->dev = dev;
+	ts->ignore_fifo_data = true;
+	ts->temp_data = -1;
+
+	ts_attached = of_property_read_bool(np, "allwinner,ts-attached");
+	if (ts_attached) {
+		ts->input = devm_input_allocate_device(dev);
+		if (!ts->input)
+			return -ENOMEM;
+
+		ts->input->name = pdev->name;
+		ts->input->phys = "sun4i_ts/input0";
+		ts->input->open = sun4i_ts_open;
+		ts->input->close = sun4i_ts_close;
+		ts->input->id.bustype = BUS_HOST;
+		ts->input->id.vendor = 0x0001;
+		ts->input->id.product = 0x0001;
+		ts->input->id.version = 0x0100;
+		ts->input->evbit[0] =  BIT(EV_SYN) | BIT(EV_KEY) | BIT(EV_ABS);
+		__set_bit(BTN_TOUCH, ts->input->keybit);
+		input_set_abs_params(ts->input, ABS_X, 0, 4095, 0, 0);
+		input_set_abs_params(ts->input, ABS_Y, 0, 4095, 0, 0);
+		input_set_drvdata(ts->input, ts);
+	}
+
+	ts->base = devm_ioremap_resource(dev,
+			      platform_get_resource(pdev, IORESOURCE_MEM, 0));
+	if (IS_ERR(ts->base))
+		return PTR_ERR(ts->base);
+
+	ts->irq = platform_get_irq(pdev, 0);
+	error = devm_request_irq(dev, ts->irq, sun4i_ts_irq, 0, "sun4i-ts", ts);
+	if (error)
+		return error;
+
+	/*
+	 * Select HOSC clk, clkin = clk / 6, adc samplefreq = clkin / 8192,
+	 * t_acq = clkin / (16 * 64)
+	 */
+	writel(ADC_CLK_SEL(0) | ADC_CLK_DIV(2) | FS_DIV(7) | T_ACQ(63),
+	       ts->base + TP_CTRL0);
+
+	/*
+	 * sensitive_adjust = 15 : max, which is not all that sensitive,
+	 * tp_mode = 0 : only x and y coordinates, as we don't use dual touch
+	 */
+	writel(TP_SENSITIVE_ADJUST(15) | TP_MODE_SELECT(0),
+	       ts->base + TP_CTRL2);
+
+	/* Enable median filter, type 1 : 5/3 */
+	writel(FILTER_EN(1) | FILTER_TYPE(1), ts->base + TP_CTRL3);
+
+	/* Enable temperature measurement, period 1953 (2 seconds) */
+	writel(TEMP_ENABLE(1) | TEMP_PERIOD(1953), ts->base + TP_TPR);
+
+	/*
+	 * Set stylus up debounce to aprox 10 ms, enable debounce, and
+	 * finally enable tp mode.
+	 */
+	writel(STYLUS_UP_DEBOUN(5) | STYLUS_UP_DEBOUN_EN(1) | TP_MODE_EN(1),
+	       ts->base + TP_CTRL1);
+
+	hwmon = devm_hwmon_device_register_with_groups(ts->dev, "sun4i_ts",
+						       ts, sun4i_ts_groups);
+	if (IS_ERR(hwmon))
+		return PTR_ERR(hwmon);
+
+	writel(TEMP_IRQ_EN(1), ts->base + TP_INT_FIFOC);
+
+	if (ts_attached) {
+		error = input_register_device(ts->input);
+		if (error) {
+			writel(0, ts->base + TP_INT_FIFOC);
+			return error;
+		}
+	}
+
+	platform_set_drvdata(pdev, ts);
+	return 0;
+}
+
+static int sun4i_ts_remove(struct platform_device *pdev)
+{
+	struct sun4i_ts_data *ts = platform_get_drvdata(pdev);
+
+	/* Explicit unregister to avoid open/close changing the imask later */
+	if (ts->input)
+		input_unregister_device(ts->input);
+
+	/* Deactivate all IRQs */
+	writel(0, ts->base + TP_INT_FIFOC);
+
+	return 0;
+}
+
+static const struct of_device_id sun4i_ts_of_match[] = {
+	{ .compatible = "allwinner,sun4i-a10-ts", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, sun4i_ts_of_match);
+
+static struct platform_driver sun4i_ts_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "sun4i-ts",
+		.of_match_table = of_match_ptr(sun4i_ts_of_match),
+	},
+	.probe	= sun4i_ts_probe,
+	.remove	= sun4i_ts_remove,
+};
+
+module_platform_driver(sun4i_ts_driver);
+
+MODULE_DESCRIPTION("Allwinner sun4i resistive touchscreen controller driver");
+MODULE_AUTHOR("Hans de Goede <hdegoede@redhat.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/tsc2005.c b/drivers/input/touchscreen/tsc2005.c
index 550adcbbfc2..52380b68ebd 100644
--- a/drivers/input/touchscreen/tsc2005.c
+++ b/drivers/input/touchscreen/tsc2005.c
@@ -25,11 +25,15 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/input.h>
+#include <linux/input/touchscreen.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/pm.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
 #include <linux/spi/spi.h>
 #include <linux/spi/tsc2005.h>
+#include <linux/regulator/consumer.h>
 
 /*
  * The touchscreen interface operates as follows:
@@ -100,6 +104,11 @@
 					 TSC2005_CFR2_AVG_7)
 
 #define MAX_12BIT			0xfff
+#define TSC2005_DEF_X_FUZZ		4
+#define TSC2005_DEF_Y_FUZZ		8
+#define TSC2005_DEF_P_FUZZ		2
+#define TSC2005_DEF_RESISTOR		280
+
 #define TSC2005_SPI_MAX_SPEED_HZ	10000000
 #define TSC2005_PENUP_TIME_MS		40
 
@@ -143,6 +152,9 @@ struct tsc2005 {
 
 	bool			pen_down;
 
+	struct regulator	*vio;
+
+	int			reset_gpio;
 	void			(*set_reset)(bool enable);
 };
 
@@ -337,6 +349,14 @@ static void tsc2005_stop_scan(struct tsc2005 *ts)
 	tsc2005_cmd(ts, TSC2005_CMD_STOP);
 }
 
+static void tsc2005_set_reset(struct tsc2005 *ts, bool enable)
+{
+	if (ts->reset_gpio >= 0)
+		gpio_set_value(ts->reset_gpio, enable);
+	else if (ts->set_reset)
+		ts->set_reset(enable);
+}
+
 /* must be called with ts->mutex held */
 static void __tsc2005_disable(struct tsc2005 *ts)
 {
@@ -355,7 +375,7 @@ static void __tsc2005_enable(struct tsc2005 *ts)
 {
 	tsc2005_start_scan(ts);
 
-	if (ts->esd_timeout && ts->set_reset) {
+	if (ts->esd_timeout && (ts->set_reset || ts->reset_gpio)) {
 		ts->last_valid_interrupt = jiffies;
 		schedule_delayed_work(&ts->esd_work,
 				round_jiffies_relative(
@@ -414,9 +434,9 @@ static ssize_t tsc2005_selftest_show(struct device *dev,
 	}
 
 	/* hardware reset */
-	ts->set_reset(false);
+	tsc2005_set_reset(ts, false);
 	usleep_range(100, 500); /* only 10us required */
-	ts->set_reset(true);
+	tsc2005_set_reset(ts, true);
 
 	if (!success)
 		goto out;
@@ -459,7 +479,7 @@ static umode_t tsc2005_attr_is_visible(struct kobject *kobj,
 	umode_t mode = attr->mode;
 
 	if (attr == &dev_attr_selftest.attr) {
-		if (!ts->set_reset)
+		if (!ts->set_reset && !ts->reset_gpio)
 			mode = 0;
 	}
 
@@ -509,9 +529,9 @@ static void tsc2005_esd_work(struct work_struct *work)
 
 	tsc2005_update_pen_state(ts, 0, 0, 0);
 
-	ts->set_reset(false);
+	tsc2005_set_reset(ts, false);
 	usleep_range(100, 500); /* only 10us required */
-	ts->set_reset(true);
+	tsc2005_set_reset(ts, true);
 
 	enable_irq(ts->spi->irq);
 	tsc2005_start_scan(ts);
@@ -572,29 +592,47 @@ static void tsc2005_setup_spi_xfer(struct tsc2005 *ts)
 static int tsc2005_probe(struct spi_device *spi)
 {
 	const struct tsc2005_platform_data *pdata = dev_get_platdata(&spi->dev);
+	struct device_node *np = spi->dev.of_node;
+
 	struct tsc2005 *ts;
 	struct input_dev *input_dev;
-	unsigned int max_x, max_y, max_p;
-	unsigned int fudge_x, fudge_y, fudge_p;
+	unsigned int max_x = MAX_12BIT;
+	unsigned int max_y = MAX_12BIT;
+	unsigned int max_p = MAX_12BIT;
+	unsigned int fudge_x = TSC2005_DEF_X_FUZZ;
+	unsigned int fudge_y = TSC2005_DEF_Y_FUZZ;
+	unsigned int fudge_p = TSC2005_DEF_P_FUZZ;
+	unsigned int x_plate_ohm = TSC2005_DEF_RESISTOR;
+	unsigned int esd_timeout;
 	int error;
 
-	if (!pdata) {
-		dev_dbg(&spi->dev, "no platform data\n");
+	if (!np && !pdata) {
+		dev_err(&spi->dev, "no platform data\n");
 		return -ENODEV;
 	}
 
-	fudge_x	= pdata->ts_x_fudge	   ? : 4;
-	fudge_y	= pdata->ts_y_fudge	   ? : 8;
-	fudge_p	= pdata->ts_pressure_fudge ? : 2;
-	max_x	= pdata->ts_x_max	   ? : MAX_12BIT;
-	max_y	= pdata->ts_y_max	   ? : MAX_12BIT;
-	max_p	= pdata->ts_pressure_max   ? : MAX_12BIT;
-
 	if (spi->irq <= 0) {
-		dev_dbg(&spi->dev, "no irq\n");
+		dev_err(&spi->dev, "no irq\n");
 		return -ENODEV;
 	}
 
+	if (pdata) {
+		fudge_x	= pdata->ts_x_fudge;
+		fudge_y	= pdata->ts_y_fudge;
+		fudge_p	= pdata->ts_pressure_fudge;
+		max_x	= pdata->ts_x_max;
+		max_y	= pdata->ts_y_max;
+		max_p	= pdata->ts_pressure_max;
+		x_plate_ohm = pdata->ts_x_plate_ohm;
+		esd_timeout = pdata->esd_timeout_ms;
+	} else {
+		x_plate_ohm = TSC2005_DEF_RESISTOR;
+		of_property_read_u32(np, "ti,x-plate-ohms", &x_plate_ohm);
+		esd_timeout = 0;
+		of_property_read_u32(np, "ti,esd-recovery-timeout-ms",
+								&esd_timeout);
+	}
+
 	spi->mode = SPI_MODE_0;
 	spi->bits_per_word = 8;
 	if (!spi->max_speed_hz)
@@ -604,19 +642,48 @@ static int tsc2005_probe(struct spi_device *spi)
 	if (error)
 		return error;
 
-	ts = kzalloc(sizeof(*ts), GFP_KERNEL);
-	input_dev = input_allocate_device();
-	if (!ts || !input_dev) {
-		error = -ENOMEM;
-		goto err_free_mem;
-	}
+	ts = devm_kzalloc(&spi->dev, sizeof(*ts), GFP_KERNEL);
+	if (!ts)
+		return -ENOMEM;
+
+	input_dev = devm_input_allocate_device(&spi->dev);
+	if (!input_dev)
+		return -ENOMEM;
 
 	ts->spi = spi;
 	ts->idev = input_dev;
 
-	ts->x_plate_ohm	= pdata->ts_x_plate_ohm	? : 280;
-	ts->esd_timeout	= pdata->esd_timeout_ms;
-	ts->set_reset	= pdata->set_reset;
+	ts->x_plate_ohm = x_plate_ohm;
+	ts->esd_timeout = esd_timeout;
+
+	if (np) {
+		ts->reset_gpio = of_get_named_gpio(np, "reset-gpios", 0);
+		if (ts->reset_gpio == -EPROBE_DEFER)
+			return ts->reset_gpio;
+		if (ts->reset_gpio < 0) {
+			dev_err(&spi->dev, "error acquiring reset gpio: %d\n",
+				ts->reset_gpio);
+			return ts->reset_gpio;
+		}
+
+		error = devm_gpio_request_one(&spi->dev, ts->reset_gpio, 0,
+					      "reset-gpios");
+		if (error) {
+			dev_err(&spi->dev, "error requesting reset gpio: %d\n",
+				error);
+			return error;
+		}
+
+		ts->vio = devm_regulator_get(&spi->dev, "vio");
+		if (IS_ERR(ts->vio)) {
+			error = PTR_ERR(ts->vio);
+			dev_err(&spi->dev, "vio regulator missing (%d)", error);
+			return error;
+		}
+	} else {
+		ts->reset_gpio = -1;
+		ts->set_reset = pdata->set_reset;
+	}
 
 	mutex_init(&ts->mutex);
 
@@ -641,6 +708,9 @@ static int tsc2005_probe(struct spi_device *spi)
 	input_set_abs_params(input_dev, ABS_Y, 0, max_y, fudge_y, 0);
 	input_set_abs_params(input_dev, ABS_PRESSURE, 0, max_p, fudge_p, 0);
 
+	if (np)
+		touchscreen_parse_of_params(input_dev);
+
 	input_dev->open = tsc2005_open;
 	input_dev->close = tsc2005_close;
 
@@ -649,12 +719,20 @@ static int tsc2005_probe(struct spi_device *spi)
 	/* Ensure the touchscreen is off */
 	tsc2005_stop_scan(ts);
 
-	error = request_threaded_irq(spi->irq, NULL, tsc2005_irq_thread,
-				     IRQF_TRIGGER_RISING | IRQF_ONESHOT,
-				     "tsc2005", ts);
+	error = devm_request_threaded_irq(&spi->dev, spi->irq, NULL,
+					  tsc2005_irq_thread,
+					  IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+					  "tsc2005", ts);
 	if (error) {
 		dev_err(&spi->dev, "Failed to request irq, err: %d\n", error);
-		goto err_free_mem;
+		return error;
+	}
+
+	/* enable regulator for DT */
+	if (ts->vio) {
+		error = regulator_enable(ts->vio);
+		if (error)
+			return error;
 	}
 
 	spi_set_drvdata(spi, ts);
@@ -662,7 +740,7 @@ static int tsc2005_probe(struct spi_device *spi)
 	if (error) {
 		dev_err(&spi->dev,
 			"Failed to create sysfs attributes, err: %d\n", error);
-		goto err_clear_drvdata;
+		goto disable_regulator;
 	}
 
 	error = input_register_device(ts->idev);
@@ -677,11 +755,9 @@ static int tsc2005_probe(struct spi_device *spi)
 
 err_remove_sysfs:
 	sysfs_remove_group(&spi->dev.kobj, &tsc2005_attr_group);
-err_clear_drvdata:
-	free_irq(spi->irq, ts);
-err_free_mem:
-	input_free_device(input_dev);
-	kfree(ts);
+disable_regulator:
+	if (ts->vio)
+		regulator_disable(ts->vio);
 	return error;
 }
 
@@ -689,11 +765,10 @@ static int tsc2005_remove(struct spi_device *spi)
 {
 	struct tsc2005 *ts = spi_get_drvdata(spi);
 
-	sysfs_remove_group(&ts->spi->dev.kobj, &tsc2005_attr_group);
+	sysfs_remove_group(&spi->dev.kobj, &tsc2005_attr_group);
 
-	free_irq(ts->spi->irq, ts);
-	input_unregister_device(ts->idev);
-	kfree(ts);
+	if (ts->vio)
+		regulator_disable(ts->vio);
 
 	return 0;
 }
diff --git a/drivers/input/touchscreen/zforce_ts.c b/drivers/input/touchscreen/zforce_ts.c
index 01d30cedde4..feea85b52fa 100644
--- a/drivers/input/touchscreen/zforce_ts.c
+++ b/drivers/input/touchscreen/zforce_ts.c
@@ -880,7 +880,7 @@ static struct i2c_device_id zforce_idtable[] = {
 MODULE_DEVICE_TABLE(i2c, zforce_idtable);
 
 #ifdef CONFIG_OF
-static struct of_device_id zforce_dt_idtable[] = {
+static const struct of_device_id zforce_dt_idtable[] = {
 	{ .compatible = "neonode,zforce" },
 	{},
 };
diff --git a/drivers/iommu/Kconfig b/drivers/iommu/Kconfig
index df56e4c74a7..d260605e6d5 100644
--- a/drivers/iommu/Kconfig
+++ b/drivers/iommu/Kconfig
@@ -178,13 +178,13 @@ config TEGRA_IOMMU_SMMU
 
 config EXYNOS_IOMMU
 	bool "Exynos IOMMU Support"
-	depends on ARCH_EXYNOS && EXYNOS_DEV_SYSMMU
+	depends on ARCH_EXYNOS
 	select IOMMU_API
 	help
-	  Support for the IOMMU(System MMU) of Samsung Exynos application
-	  processor family. This enables H/W multimedia accellerators to see
-	  non-linear physical memory chunks as a linear memory in their
-	  address spaces
+	  Support for the IOMMU (System MMU) of Samsung Exynos application
+	  processor family. This enables H/W multimedia accelerators to see
+	  non-linear physical memory chunks as linear memory in their
+	  address space.
 
 	  If unsure, say N here.
 
@@ -193,9 +193,9 @@ config EXYNOS_IOMMU_DEBUG
 	depends on EXYNOS_IOMMU
 	help
 	  Select this to see the detailed log message that shows what
-	  happens in the IOMMU driver
+	  happens in the IOMMU driver.
 
-	  Say N unless you need kernel log message for IOMMU debugging
+	  Say N unless you need kernel log message for IOMMU debugging.
 
 config SHMOBILE_IPMMU
 	bool
@@ -272,6 +272,18 @@ config SHMOBILE_IOMMU_L1SIZE
 	default 256 if SHMOBILE_IOMMU_ADDRSIZE_64MB
 	default 128 if SHMOBILE_IOMMU_ADDRSIZE_32MB
 
+config IPMMU_VMSA
+	bool "Renesas VMSA-compatible IPMMU"
+	depends on ARM_LPAE
+	depends on ARCH_SHMOBILE || COMPILE_TEST
+	select IOMMU_API
+	select ARM_DMA_USE_IOMMU
+	help
+	  Support for the Renesas VMSA-compatible IPMMU Renesas found in the
+	  R-Mobile APE6 and R-Car H2/M2 SoCs.
+
+	  If unsure, say N.
+
 config SPAPR_TCE_IOMMU
 	bool "sPAPR TCE IOMMU Support"
 	depends on PPC_POWERNV || PPC_PSERIES
diff --git a/drivers/iommu/Makefile b/drivers/iommu/Makefile
index 5d58bf16e9e..8893bad048e 100644
--- a/drivers/iommu/Makefile
+++ b/drivers/iommu/Makefile
@@ -7,6 +7,7 @@ obj-$(CONFIG_AMD_IOMMU_V2) += amd_iommu_v2.o
 obj-$(CONFIG_ARM_SMMU) += arm-smmu.o
 obj-$(CONFIG_DMAR_TABLE) += dmar.o
 obj-$(CONFIG_INTEL_IOMMU) += iova.o intel-iommu.o
+obj-$(CONFIG_IPMMU_VMSA) += ipmmu-vmsa.o
 obj-$(CONFIG_IRQ_REMAP) += intel_irq_remapping.o irq_remapping.o
 obj-$(CONFIG_OMAP_IOMMU) += omap-iommu.o
 obj-$(CONFIG_OMAP_IOMMU) += omap-iommu2.o
diff --git a/drivers/iommu/amd_iommu.c b/drivers/iommu/amd_iommu.c
index c949520bd19..4aec6a29e31 100644
--- a/drivers/iommu/amd_iommu.c
+++ b/drivers/iommu/amd_iommu.c
@@ -3499,8 +3499,6 @@ int __init amd_iommu_init_passthrough(void)
 {
 	struct iommu_dev_data *dev_data;
 	struct pci_dev *dev = NULL;
-	struct amd_iommu *iommu;
-	u16 devid;
 	int ret;
 
 	ret = alloc_passthrough_domain();
@@ -3514,12 +3512,6 @@ int __init amd_iommu_init_passthrough(void)
 		dev_data = get_dev_data(&dev->dev);
 		dev_data->passthrough = true;
 
-		devid = get_device_id(&dev->dev);
-
-		iommu = amd_iommu_rlookup_table[devid];
-		if (!iommu)
-			continue;
-
 		attach_device(&dev->dev, pt_domain);
 	}
 
@@ -3999,7 +3991,7 @@ static struct irq_remap_table *get_irq_table(u16 devid, bool ioapic)
 	iommu_flush_dte(iommu, devid);
 	if (devid != alias) {
 		irq_lookup_table[alias] = table;
-		set_dte_irq_entry(devid, table);
+		set_dte_irq_entry(alias, table);
 		iommu_flush_dte(iommu, alias);
 	}
 
diff --git a/drivers/iommu/amd_iommu_init.c b/drivers/iommu/amd_iommu_init.c
index b76c58dbe30..0e08545d729 100644
--- a/drivers/iommu/amd_iommu_init.c
+++ b/drivers/iommu/amd_iommu_init.c
@@ -788,7 +788,7 @@ static void __init set_device_exclusion_range(u16 devid, struct ivmd_header *m)
 		 * per device. But we can enable the exclusion range per
 		 * device. This is done here
 		 */
-		set_dev_entry_bit(m->devid, DEV_ENTRY_EX);
+		set_dev_entry_bit(devid, DEV_ENTRY_EX);
 		iommu->exclusion_start = m->range_start;
 		iommu->exclusion_length = m->range_length;
 	}
diff --git a/drivers/iommu/amd_iommu_v2.c b/drivers/iommu/amd_iommu_v2.c
index 5208828792e..d4daa05efe6 100644
--- a/drivers/iommu/amd_iommu_v2.c
+++ b/drivers/iommu/amd_iommu_v2.c
@@ -45,6 +45,8 @@ struct pri_queue {
 struct pasid_state {
 	struct list_head list;			/* For global state-list */
 	atomic_t count;				/* Reference count */
+	atomic_t mmu_notifier_count;		/* Counting nested mmu_notifier
+						   calls */
 	struct task_struct *task;		/* Task bound to this PASID */
 	struct mm_struct *mm;			/* mm_struct for the faults */
 	struct mmu_notifier mn;                 /* mmu_otifier handle */
@@ -56,6 +58,8 @@ struct pasid_state {
 };
 
 struct device_state {
+	struct list_head list;
+	u16 devid;
 	atomic_t count;
 	struct pci_dev *pdev;
 	struct pasid_state **states;
@@ -81,13 +85,9 @@ struct fault {
 	u16 flags;
 };
 
-static struct device_state **state_table;
+static LIST_HEAD(state_list);
 static spinlock_t state_lock;
 
-/* List and lock for all pasid_states */
-static LIST_HEAD(pasid_state_list);
-static DEFINE_SPINLOCK(ps_lock);
-
 static struct workqueue_struct *iommu_wq;
 
 /*
@@ -99,7 +99,6 @@ static u64 *empty_page_table;
 
 static void free_pasid_states(struct device_state *dev_state);
 static void unbind_pasid(struct device_state *dev_state, int pasid);
-static int task_exit(struct notifier_block *nb, unsigned long e, void *data);
 
 static u16 device_id(struct pci_dev *pdev)
 {
@@ -111,13 +110,25 @@ static u16 device_id(struct pci_dev *pdev)
 	return devid;
 }
 
+static struct device_state *__get_device_state(u16 devid)
+{
+	struct device_state *dev_state;
+
+	list_for_each_entry(dev_state, &state_list, list) {
+		if (dev_state->devid == devid)
+			return dev_state;
+	}
+
+	return NULL;
+}
+
 static struct device_state *get_device_state(u16 devid)
 {
 	struct device_state *dev_state;
 	unsigned long flags;
 
 	spin_lock_irqsave(&state_lock, flags);
-	dev_state = state_table[devid];
+	dev_state = __get_device_state(devid);
 	if (dev_state != NULL)
 		atomic_inc(&dev_state->count);
 	spin_unlock_irqrestore(&state_lock, flags);
@@ -158,29 +169,6 @@ static void put_device_state_wait(struct device_state *dev_state)
 	free_device_state(dev_state);
 }
 
-static struct notifier_block profile_nb = {
-	.notifier_call = task_exit,
-};
-
-static void link_pasid_state(struct pasid_state *pasid_state)
-{
-	spin_lock(&ps_lock);
-	list_add_tail(&pasid_state->list, &pasid_state_list);
-	spin_unlock(&ps_lock);
-}
-
-static void __unlink_pasid_state(struct pasid_state *pasid_state)
-{
-	list_del(&pasid_state->list);
-}
-
-static void unlink_pasid_state(struct pasid_state *pasid_state)
-{
-	spin_lock(&ps_lock);
-	__unlink_pasid_state(pasid_state);
-	spin_unlock(&ps_lock);
-}
-
 /* Must be called under dev_state->lock */
 static struct pasid_state **__get_pasid_state_ptr(struct device_state *dev_state,
 						  int pasid, bool alloc)
@@ -337,7 +325,6 @@ static void unbind_pasid(struct device_state *dev_state, int pasid)
 	if (pasid_state == NULL)
 		return;
 
-	unlink_pasid_state(pasid_state);
 	__unbind_pasid(pasid_state);
 	put_pasid_state_wait(pasid_state); /* Reference taken in this function */
 }
@@ -379,7 +366,12 @@ static void free_pasid_states(struct device_state *dev_state)
 			continue;
 
 		put_pasid_state(pasid_state);
-		unbind_pasid(dev_state, i);
+
+		/*
+		 * This will call the mn_release function and
+		 * unbind the PASID
+		 */
+		mmu_notifier_unregister(&pasid_state->mn, pasid_state->mm);
 	}
 
 	if (dev_state->pasid_levels == 2)
@@ -443,8 +435,11 @@ static void mn_invalidate_range_start(struct mmu_notifier *mn,
 	pasid_state = mn_to_state(mn);
 	dev_state   = pasid_state->device_state;
 
-	amd_iommu_domain_set_gcr3(dev_state->domain, pasid_state->pasid,
-				  __pa(empty_page_table));
+	if (atomic_add_return(1, &pasid_state->mmu_notifier_count) == 1) {
+		amd_iommu_domain_set_gcr3(dev_state->domain,
+					  pasid_state->pasid,
+					  __pa(empty_page_table));
+	}
 }
 
 static void mn_invalidate_range_end(struct mmu_notifier *mn,
@@ -457,11 +452,31 @@ static void mn_invalidate_range_end(struct mmu_notifier *mn,
 	pasid_state = mn_to_state(mn);
 	dev_state   = pasid_state->device_state;
 
-	amd_iommu_domain_set_gcr3(dev_state->domain, pasid_state->pasid,
-				  __pa(pasid_state->mm->pgd));
+	if (atomic_dec_and_test(&pasid_state->mmu_notifier_count)) {
+		amd_iommu_domain_set_gcr3(dev_state->domain,
+					  pasid_state->pasid,
+					  __pa(pasid_state->mm->pgd));
+	}
+}
+
+static void mn_release(struct mmu_notifier *mn, struct mm_struct *mm)
+{
+	struct pasid_state *pasid_state;
+	struct device_state *dev_state;
+
+	might_sleep();
+
+	pasid_state = mn_to_state(mn);
+	dev_state   = pasid_state->device_state;
+
+	if (pasid_state->device_state->inv_ctx_cb)
+		dev_state->inv_ctx_cb(dev_state->pdev, pasid_state->pasid);
+
+	unbind_pasid(dev_state, pasid_state->pasid);
 }
 
 static struct mmu_notifier_ops iommu_mn = {
+	.release		= mn_release,
 	.clear_flush_young      = mn_clear_flush_young,
 	.change_pte             = mn_change_pte,
 	.invalidate_page        = mn_invalidate_page,
@@ -504,8 +519,10 @@ static void do_fault(struct work_struct *work)
 
 	write = !!(fault->flags & PPR_FAULT_WRITE);
 
+	down_read(&fault->state->mm->mmap_sem);
 	npages = get_user_pages(fault->state->task, fault->state->mm,
 				fault->address, 1, write, 0, &page, NULL);
+	up_read(&fault->state->mm->mmap_sem);
 
 	if (npages == 1) {
 		put_page(page);
@@ -604,53 +621,6 @@ static struct notifier_block ppr_nb = {
 	.notifier_call = ppr_notifier,
 };
 
-static int task_exit(struct notifier_block *nb, unsigned long e, void *data)
-{
-	struct pasid_state *pasid_state;
-	struct task_struct *task;
-
-	task = data;
-
-	/*
-	 * Using this notifier is a hack - but there is no other choice
-	 * at the moment. What I really want is a sleeping notifier that
-	 * is called when an MM goes down. But such a notifier doesn't
-	 * exist yet. The notifier needs to sleep because it has to make
-	 * sure that the device does not use the PASID and the address
-	 * space anymore before it is destroyed. This includes waiting
-	 * for pending PRI requests to pass the workqueue. The
-	 * MMU-Notifiers would be a good fit, but they use RCU and so
-	 * they are not allowed to sleep. Lets see how we can solve this
-	 * in a more intelligent way in the future.
-	 */
-again:
-	spin_lock(&ps_lock);
-	list_for_each_entry(pasid_state, &pasid_state_list, list) {
-		struct device_state *dev_state;
-		int pasid;
-
-		if (pasid_state->task != task)
-			continue;
-
-		/* Drop Lock and unbind */
-		spin_unlock(&ps_lock);
-
-		dev_state = pasid_state->device_state;
-		pasid     = pasid_state->pasid;
-
-		if (pasid_state->device_state->inv_ctx_cb)
-			dev_state->inv_ctx_cb(dev_state->pdev, pasid);
-
-		unbind_pasid(dev_state, pasid);
-
-		/* Task may be in the list multiple times */
-		goto again;
-	}
-	spin_unlock(&ps_lock);
-
-	return NOTIFY_OK;
-}
-
 int amd_iommu_bind_pasid(struct pci_dev *pdev, int pasid,
 			 struct task_struct *task)
 {
@@ -680,6 +650,7 @@ int amd_iommu_bind_pasid(struct pci_dev *pdev, int pasid,
 		goto out;
 
 	atomic_set(&pasid_state->count, 1);
+	atomic_set(&pasid_state->mmu_notifier_count, 0);
 	init_waitqueue_head(&pasid_state->wq);
 	spin_lock_init(&pasid_state->lock);
 
@@ -703,8 +674,6 @@ int amd_iommu_bind_pasid(struct pci_dev *pdev, int pasid,
 	if (ret)
 		goto out_clear_state;
 
-	link_pasid_state(pasid_state);
-
 	return 0;
 
 out_clear_state:
@@ -725,6 +694,7 @@ EXPORT_SYMBOL(amd_iommu_bind_pasid);
 
 void amd_iommu_unbind_pasid(struct pci_dev *pdev, int pasid)
 {
+	struct pasid_state *pasid_state;
 	struct device_state *dev_state;
 	u16 devid;
 
@@ -741,7 +711,17 @@ void amd_iommu_unbind_pasid(struct pci_dev *pdev, int pasid)
 	if (pasid < 0 || pasid >= dev_state->max_pasids)
 		goto out;
 
-	unbind_pasid(dev_state, pasid);
+	pasid_state = get_pasid_state(dev_state, pasid);
+	if (pasid_state == NULL)
+		goto out;
+	/*
+	 * Drop reference taken here. We are safe because we still hold
+	 * the reference taken in the amd_iommu_bind_pasid function.
+	 */
+	put_pasid_state(pasid_state);
+
+	/* This will call the mn_release function and unbind the PASID */
+	mmu_notifier_unregister(&pasid_state->mn, pasid_state->mm);
 
 out:
 	put_device_state(dev_state);
@@ -771,7 +751,8 @@ int amd_iommu_init_device(struct pci_dev *pdev, int pasids)
 
 	spin_lock_init(&dev_state->lock);
 	init_waitqueue_head(&dev_state->wq);
-	dev_state->pdev = pdev;
+	dev_state->pdev  = pdev;
+	dev_state->devid = devid;
 
 	tmp = pasids;
 	for (dev_state->pasid_levels = 0; (tmp - 1) & ~0x1ff; tmp >>= 9)
@@ -801,13 +782,13 @@ int amd_iommu_init_device(struct pci_dev *pdev, int pasids)
 
 	spin_lock_irqsave(&state_lock, flags);
 
-	if (state_table[devid] != NULL) {
+	if (__get_device_state(devid) != NULL) {
 		spin_unlock_irqrestore(&state_lock, flags);
 		ret = -EBUSY;
 		goto out_free_domain;
 	}
 
-	state_table[devid] = dev_state;
+	list_add_tail(&dev_state->list, &state_list);
 
 	spin_unlock_irqrestore(&state_lock, flags);
 
@@ -839,13 +820,13 @@ void amd_iommu_free_device(struct pci_dev *pdev)
 
 	spin_lock_irqsave(&state_lock, flags);
 
-	dev_state = state_table[devid];
+	dev_state = __get_device_state(devid);
 	if (dev_state == NULL) {
 		spin_unlock_irqrestore(&state_lock, flags);
 		return;
 	}
 
-	state_table[devid] = NULL;
+	list_del(&dev_state->list);
 
 	spin_unlock_irqrestore(&state_lock, flags);
 
@@ -872,7 +853,7 @@ int amd_iommu_set_invalid_ppr_cb(struct pci_dev *pdev,
 	spin_lock_irqsave(&state_lock, flags);
 
 	ret = -EINVAL;
-	dev_state = state_table[devid];
+	dev_state = __get_device_state(devid);
 	if (dev_state == NULL)
 		goto out_unlock;
 
@@ -903,7 +884,7 @@ int amd_iommu_set_invalidate_ctx_cb(struct pci_dev *pdev,
 	spin_lock_irqsave(&state_lock, flags);
 
 	ret = -EINVAL;
-	dev_state = state_table[devid];
+	dev_state = __get_device_state(devid);
 	if (dev_state == NULL)
 		goto out_unlock;
 
@@ -920,7 +901,6 @@ EXPORT_SYMBOL(amd_iommu_set_invalidate_ctx_cb);
 
 static int __init amd_iommu_v2_init(void)
 {
-	size_t state_table_size;
 	int ret;
 
 	pr_info("AMD IOMMUv2 driver by Joerg Roedel <joerg.roedel@amd.com>\n");
@@ -936,16 +916,10 @@ static int __init amd_iommu_v2_init(void)
 
 	spin_lock_init(&state_lock);
 
-	state_table_size = MAX_DEVICES * sizeof(struct device_state *);
-	state_table = (void *)__get_free_pages(GFP_KERNEL | __GFP_ZERO,
-					       get_order(state_table_size));
-	if (state_table == NULL)
-		return -ENOMEM;
-
 	ret = -ENOMEM;
 	iommu_wq = create_workqueue("amd_iommu_v2");
 	if (iommu_wq == NULL)
-		goto out_free;
+		goto out;
 
 	ret = -ENOMEM;
 	empty_page_table = (u64 *)get_zeroed_page(GFP_KERNEL);
@@ -953,29 +927,24 @@ static int __init amd_iommu_v2_init(void)
 		goto out_destroy_wq;
 
 	amd_iommu_register_ppr_notifier(&ppr_nb);
-	profile_event_register(PROFILE_TASK_EXIT, &profile_nb);
 
 	return 0;
 
 out_destroy_wq:
 	destroy_workqueue(iommu_wq);
 
-out_free:
-	free_pages((unsigned long)state_table, get_order(state_table_size));
-
+out:
 	return ret;
 }
 
 static void __exit amd_iommu_v2_exit(void)
 {
 	struct device_state *dev_state;
-	size_t state_table_size;
 	int i;
 
 	if (!amd_iommu_v2_supported())
 		return;
 
-	profile_event_unregister(PROFILE_TASK_EXIT, &profile_nb);
 	amd_iommu_unregister_ppr_notifier(&ppr_nb);
 
 	flush_workqueue(iommu_wq);
@@ -998,9 +967,6 @@ static void __exit amd_iommu_v2_exit(void)
 
 	destroy_workqueue(iommu_wq);
 
-	state_table_size = MAX_DEVICES * sizeof(struct device_state *);
-	free_pages((unsigned long)state_table, get_order(state_table_size));
-
 	free_page((unsigned long)empty_page_table);
 }
 
diff --git a/drivers/iommu/arm-smmu.c b/drivers/iommu/arm-smmu.c
index 647c3c7fd74..1599354e974 100644
--- a/drivers/iommu/arm-smmu.c
+++ b/drivers/iommu/arm-smmu.c
@@ -1167,7 +1167,7 @@ static int arm_smmu_domain_add_master(struct arm_smmu_domain *smmu_domain,
 	for (i = 0; i < master->num_streamids; ++i) {
 		u32 idx, s2cr;
 		idx = master->smrs ? master->smrs[i].idx : master->streamids[i];
-		s2cr = (S2CR_TYPE_TRANS << S2CR_TYPE_SHIFT) |
+		s2cr = S2CR_TYPE_TRANS |
 		       (smmu_domain->root_cfg.cbndx << S2CR_CBNDX_SHIFT);
 		writel_relaxed(s2cr, gr0_base + ARM_SMMU_GR0_S2CR(idx));
 	}
@@ -1804,7 +1804,7 @@ static int arm_smmu_device_cfg_probe(struct arm_smmu_device *smmu)
 	 * allocation (PTRS_PER_PGD).
 	 */
 #ifdef CONFIG_64BIT
-	smmu->s1_output_size = min(39UL, size);
+	smmu->s1_output_size = min((unsigned long)VA_BITS, size);
 #else
 	smmu->s1_output_size = min(32UL, size);
 #endif
diff --git a/drivers/iommu/dmar.c b/drivers/iommu/dmar.c
index 39f8b717fe8..9a4f05e5b23 100644
--- a/drivers/iommu/dmar.c
+++ b/drivers/iommu/dmar.c
@@ -994,7 +994,7 @@ static void free_iommu(struct intel_iommu *iommu)
 	if (iommu->irq) {
 		free_irq(iommu->irq, iommu);
 		irq_set_handler_data(iommu->irq, NULL);
-		destroy_irq(iommu->irq);
+		dmar_free_hwirq(iommu->irq);
 	}
 
 	if (iommu->qi) {
@@ -1550,8 +1550,8 @@ int dmar_set_interrupt(struct intel_iommu *iommu)
 	if (iommu->irq)
 		return 0;
 
-	irq = create_irq();
-	if (!irq) {
+	irq = dmar_alloc_hwirq();
+	if (irq <= 0) {
 		pr_err("IOMMU: no free vectors\n");
 		return -EINVAL;
 	}
@@ -1563,7 +1563,7 @@ int dmar_set_interrupt(struct intel_iommu *iommu)
 	if (ret) {
 		irq_set_handler_data(irq, NULL);
 		iommu->irq = 0;
-		destroy_irq(irq);
+		dmar_free_hwirq(irq);
 		return ret;
 	}
 
diff --git a/drivers/iommu/exynos-iommu.c b/drivers/iommu/exynos-iommu.c
index 074018979cd..99054d2c040 100644
--- a/drivers/iommu/exynos-iommu.c
+++ b/drivers/iommu/exynos-iommu.c
@@ -29,7 +29,8 @@
 #include <asm/cacheflush.h>
 #include <asm/pgtable.h>
 
-#include <mach/sysmmu.h>
+typedef u32 sysmmu_iova_t;
+typedef u32 sysmmu_pte_t;
 
 /* We does not consider super section mapping (16MB) */
 #define SECT_ORDER 20
@@ -44,28 +45,44 @@
 #define LPAGE_MASK (~(LPAGE_SIZE - 1))
 #define SPAGE_MASK (~(SPAGE_SIZE - 1))
 
-#define lv1ent_fault(sent) (((*(sent) & 3) == 0) || ((*(sent) & 3) == 3))
-#define lv1ent_page(sent) ((*(sent) & 3) == 1)
+#define lv1ent_fault(sent) ((*(sent) == ZERO_LV2LINK) || \
+			   ((*(sent) & 3) == 0) || ((*(sent) & 3) == 3))
+#define lv1ent_zero(sent) (*(sent) == ZERO_LV2LINK)
+#define lv1ent_page_zero(sent) ((*(sent) & 3) == 1)
+#define lv1ent_page(sent) ((*(sent) != ZERO_LV2LINK) && \
+			  ((*(sent) & 3) == 1))
 #define lv1ent_section(sent) ((*(sent) & 3) == 2)
 
 #define lv2ent_fault(pent) ((*(pent) & 3) == 0)
 #define lv2ent_small(pent) ((*(pent) & 2) == 2)
 #define lv2ent_large(pent) ((*(pent) & 3) == 1)
 
+static u32 sysmmu_page_offset(sysmmu_iova_t iova, u32 size)
+{
+	return iova & (size - 1);
+}
+
 #define section_phys(sent) (*(sent) & SECT_MASK)
-#define section_offs(iova) ((iova) & 0xFFFFF)
+#define section_offs(iova) sysmmu_page_offset((iova), SECT_SIZE)
 #define lpage_phys(pent) (*(pent) & LPAGE_MASK)
-#define lpage_offs(iova) ((iova) & 0xFFFF)
+#define lpage_offs(iova) sysmmu_page_offset((iova), LPAGE_SIZE)
 #define spage_phys(pent) (*(pent) & SPAGE_MASK)
-#define spage_offs(iova) ((iova) & 0xFFF)
-
-#define lv1ent_offset(iova) ((iova) >> SECT_ORDER)
-#define lv2ent_offset(iova) (((iova) & 0xFF000) >> SPAGE_ORDER)
+#define spage_offs(iova) sysmmu_page_offset((iova), SPAGE_SIZE)
 
 #define NUM_LV1ENTRIES 4096
-#define NUM_LV2ENTRIES 256
+#define NUM_LV2ENTRIES (SECT_SIZE / SPAGE_SIZE)
+
+static u32 lv1ent_offset(sysmmu_iova_t iova)
+{
+	return iova >> SECT_ORDER;
+}
+
+static u32 lv2ent_offset(sysmmu_iova_t iova)
+{
+	return (iova >> SPAGE_ORDER) & (NUM_LV2ENTRIES - 1);
+}
 
-#define LV2TABLE_SIZE (NUM_LV2ENTRIES * sizeof(long))
+#define LV2TABLE_SIZE (NUM_LV2ENTRIES * sizeof(sysmmu_pte_t))
 
 #define SPAGES_PER_LPAGE (LPAGE_SIZE / SPAGE_SIZE)
 
@@ -80,6 +97,13 @@
 #define CTRL_BLOCK	0x7
 #define CTRL_DISABLE	0x0
 
+#define CFG_LRU		0x1
+#define CFG_QOS(n)	((n & 0xF) << 7)
+#define CFG_MASK	0x0150FFFF /* Selecting bit 0-15, 20, 22 and 24 */
+#define CFG_ACGEN	(1 << 24) /* System MMU 3.3 only */
+#define CFG_SYSSEL	(1 << 22) /* System MMU 3.2 only */
+#define CFG_FLPDCACHE	(1 << 20) /* System MMU 3.2+ only */
+
 #define REG_MMU_CTRL		0x000
 #define REG_MMU_CFG		0x004
 #define REG_MMU_STATUS		0x008
@@ -96,19 +120,32 @@
 
 #define REG_MMU_VERSION		0x034
 
+#define MMU_MAJ_VER(val)	((val) >> 7)
+#define MMU_MIN_VER(val)	((val) & 0x7F)
+#define MMU_RAW_VER(reg)	(((reg) >> 21) & ((1 << 11) - 1)) /* 11 bits */
+
+#define MAKE_MMU_VER(maj, min)	((((maj) & 0xF) << 7) | ((min) & 0x7F))
+
 #define REG_PB0_SADDR		0x04C
 #define REG_PB0_EADDR		0x050
 #define REG_PB1_SADDR		0x054
 #define REG_PB1_EADDR		0x058
 
-static unsigned long *section_entry(unsigned long *pgtable, unsigned long iova)
+#define has_sysmmu(dev)		(dev->archdata.iommu != NULL)
+
+static struct kmem_cache *lv2table_kmem_cache;
+static sysmmu_pte_t *zero_lv2_table;
+#define ZERO_LV2LINK mk_lv1ent_page(virt_to_phys(zero_lv2_table))
+
+static sysmmu_pte_t *section_entry(sysmmu_pte_t *pgtable, sysmmu_iova_t iova)
 {
 	return pgtable + lv1ent_offset(iova);
 }
 
-static unsigned long *page_entry(unsigned long *sent, unsigned long iova)
+static sysmmu_pte_t *page_entry(sysmmu_pte_t *sent, sysmmu_iova_t iova)
 {
-	return (unsigned long *)__va(lv2table_base(sent)) + lv2ent_offset(iova);
+	return (sysmmu_pte_t *)phys_to_virt(
+				lv2table_base(sent)) + lv2ent_offset(iova);
 }
 
 enum exynos_sysmmu_inttype {
@@ -124,16 +161,6 @@ enum exynos_sysmmu_inttype {
 	SYSMMU_FAULTS_NUM
 };
 
-/*
- * @itype: type of fault.
- * @pgtable_base: the physical address of page table base. This is 0 if @itype
- *                is SYSMMU_BUSERROR.
- * @fault_addr: the device (virtual) address that the System MMU tried to
- *             translated. This is 0 if @itype is SYSMMU_BUSERROR.
- */
-typedef int (*sysmmu_fault_handler_t)(enum exynos_sysmmu_inttype itype,
-			unsigned long pgtable_base, unsigned long fault_addr);
-
 static unsigned short fault_reg_offset[SYSMMU_FAULTS_NUM] = {
 	REG_PAGE_FAULT_ADDR,
 	REG_AR_FAULT_ADDR,
@@ -157,27 +184,34 @@ static char *sysmmu_fault_name[SYSMMU_FAULTS_NUM] = {
 	"UNKNOWN FAULT"
 };
 
+/* attached to dev.archdata.iommu of the master device */
+struct exynos_iommu_owner {
+	struct list_head client; /* entry of exynos_iommu_domain.clients */
+	struct device *dev;
+	struct device *sysmmu;
+	struct iommu_domain *domain;
+	void *vmm_data;         /* IO virtual memory manager's data */
+	spinlock_t lock;        /* Lock to preserve consistency of System MMU */
+};
+
 struct exynos_iommu_domain {
 	struct list_head clients; /* list of sysmmu_drvdata.node */
-	unsigned long *pgtable; /* lv1 page table, 16KB */
+	sysmmu_pte_t *pgtable; /* lv1 page table, 16KB */
 	short *lv2entcnt; /* free lv2 entry counter for each section */
 	spinlock_t lock; /* lock for this structure */
 	spinlock_t pgtablelock; /* lock for modifying page table @ pgtable */
 };
 
 struct sysmmu_drvdata {
-	struct list_head node; /* entry of exynos_iommu_domain.clients */
 	struct device *sysmmu;	/* System MMU's device descriptor */
-	struct device *dev;	/* Owner of system MMU */
-	char *dbgname;
-	int nsfrs;
-	void __iomem **sfrbases;
-	struct clk *clk[2];
+	struct device *master;	/* Owner of system MMU */
+	void __iomem *sfrbase;
+	struct clk *clk;
+	struct clk *clk_master;
 	int activations;
-	rwlock_t lock;
+	spinlock_t lock;
 	struct iommu_domain *domain;
-	sysmmu_fault_handler_t fault_handler;
-	unsigned long pgtable;
+	phys_addr_t pgtable;
 };
 
 static bool set_sysmmu_active(struct sysmmu_drvdata *data)
@@ -204,6 +238,11 @@ static void sysmmu_unblock(void __iomem *sfrbase)
 	__raw_writel(CTRL_ENABLE, sfrbase + REG_MMU_CTRL);
 }
 
+static unsigned int __raw_sysmmu_version(struct sysmmu_drvdata *data)
+{
+	return MMU_RAW_VER(__raw_readl(data->sfrbase + REG_MMU_VERSION));
+}
+
 static bool sysmmu_block(void __iomem *sfrbase)
 {
 	int i = 120;
@@ -226,434 +265,428 @@ static void __sysmmu_tlb_invalidate(void __iomem *sfrbase)
 }
 
 static void __sysmmu_tlb_invalidate_entry(void __iomem *sfrbase,
-						unsigned long iova)
+				sysmmu_iova_t iova, unsigned int num_inv)
 {
-	__raw_writel((iova & SPAGE_MASK) | 1, sfrbase + REG_MMU_FLUSH_ENTRY);
+	unsigned int i;
+
+	for (i = 0; i < num_inv; i++) {
+		__raw_writel((iova & SPAGE_MASK) | 1,
+				sfrbase + REG_MMU_FLUSH_ENTRY);
+		iova += SPAGE_SIZE;
+	}
 }
 
 static void __sysmmu_set_ptbase(void __iomem *sfrbase,
-				       unsigned long pgd)
+				       phys_addr_t pgd)
 {
-	__raw_writel(0x1, sfrbase + REG_MMU_CFG); /* 16KB LV1, LRU */
 	__raw_writel(pgd, sfrbase + REG_PT_BASE_ADDR);
 
 	__sysmmu_tlb_invalidate(sfrbase);
 }
 
-static void __sysmmu_set_prefbuf(void __iomem *sfrbase, unsigned long base,
-						unsigned long size, int idx)
-{
-	__raw_writel(base, sfrbase + REG_PB0_SADDR + idx * 8);
-	__raw_writel(size - 1 + base,  sfrbase + REG_PB0_EADDR + idx * 8);
-}
-
-static void __set_fault_handler(struct sysmmu_drvdata *data,
-					sysmmu_fault_handler_t handler)
-{
-	unsigned long flags;
-
-	write_lock_irqsave(&data->lock, flags);
-	data->fault_handler = handler;
-	write_unlock_irqrestore(&data->lock, flags);
-}
-
-void exynos_sysmmu_set_fault_handler(struct device *dev,
-					sysmmu_fault_handler_t handler)
-{
-	struct sysmmu_drvdata *data = dev_get_drvdata(dev->archdata.iommu);
-
-	__set_fault_handler(data, handler);
-}
-
-static int default_fault_handler(enum exynos_sysmmu_inttype itype,
-		     unsigned long pgtable_base, unsigned long fault_addr)
+static void show_fault_information(const char *name,
+		enum exynos_sysmmu_inttype itype,
+		phys_addr_t pgtable_base, sysmmu_iova_t fault_addr)
 {
-	unsigned long *ent;
+	sysmmu_pte_t *ent;
 
 	if ((itype >= SYSMMU_FAULTS_NUM) || (itype < SYSMMU_PAGEFAULT))
 		itype = SYSMMU_FAULT_UNKNOWN;
 
-	pr_err("%s occurred at 0x%lx(Page table base: 0x%lx)\n",
-			sysmmu_fault_name[itype], fault_addr, pgtable_base);
+	pr_err("%s occurred at %#x by %s(Page table base: %pa)\n",
+		sysmmu_fault_name[itype], fault_addr, name, &pgtable_base);
 
-	ent = section_entry(__va(pgtable_base), fault_addr);
-	pr_err("\tLv1 entry: 0x%lx\n", *ent);
+	ent = section_entry(phys_to_virt(pgtable_base), fault_addr);
+	pr_err("\tLv1 entry: %#x\n", *ent);
 
 	if (lv1ent_page(ent)) {
 		ent = page_entry(ent, fault_addr);
-		pr_err("\t Lv2 entry: 0x%lx\n", *ent);
+		pr_err("\t Lv2 entry: %#x\n", *ent);
 	}
-
-	pr_err("Generating Kernel OOPS... because it is unrecoverable.\n");
-
-	BUG();
-
-	return 0;
 }
 
 static irqreturn_t exynos_sysmmu_irq(int irq, void *dev_id)
 {
 	/* SYSMMU is in blocked when interrupt occurred. */
 	struct sysmmu_drvdata *data = dev_id;
-	struct resource *irqres;
-	struct platform_device *pdev;
 	enum exynos_sysmmu_inttype itype;
-	unsigned long addr = -1;
-
-	int i, ret = -ENOSYS;
-
-	read_lock(&data->lock);
+	sysmmu_iova_t addr = -1;
+	int ret = -ENOSYS;
 
 	WARN_ON(!is_sysmmu_active(data));
 
-	pdev = to_platform_device(data->sysmmu);
-	for (i = 0; i < (pdev->num_resources / 2); i++) {
-		irqres = platform_get_resource(pdev, IORESOURCE_IRQ, i);
-		if (irqres && ((int)irqres->start == irq))
-			break;
-	}
+	spin_lock(&data->lock);
 
-	if (i == pdev->num_resources) {
+	if (!IS_ERR(data->clk_master))
+		clk_enable(data->clk_master);
+
+	itype = (enum exynos_sysmmu_inttype)
+		__ffs(__raw_readl(data->sfrbase + REG_INT_STATUS));
+	if (WARN_ON(!((itype >= 0) && (itype < SYSMMU_FAULT_UNKNOWN))))
 		itype = SYSMMU_FAULT_UNKNOWN;
+	else
+		addr = __raw_readl(data->sfrbase + fault_reg_offset[itype]);
+
+	if (itype == SYSMMU_FAULT_UNKNOWN) {
+		pr_err("%s: Fault is not occurred by System MMU '%s'!\n",
+			__func__, dev_name(data->sysmmu));
+		pr_err("%s: Please check if IRQ is correctly configured.\n",
+			__func__);
+		BUG();
 	} else {
-		itype = (enum exynos_sysmmu_inttype)
-			__ffs(__raw_readl(data->sfrbases[i] + REG_INT_STATUS));
-		if (WARN_ON(!((itype >= 0) && (itype < SYSMMU_FAULT_UNKNOWN))))
-			itype = SYSMMU_FAULT_UNKNOWN;
-		else
-			addr = __raw_readl(
-				data->sfrbases[i] + fault_reg_offset[itype]);
+		unsigned int base =
+				__raw_readl(data->sfrbase + REG_PT_BASE_ADDR);
+		show_fault_information(dev_name(data->sysmmu),
+					itype, base, addr);
+		if (data->domain)
+			ret = report_iommu_fault(data->domain,
+					data->master, addr, itype);
 	}
 
-	if (data->domain)
-		ret = report_iommu_fault(data->domain, data->dev,
-				addr, itype);
+	/* fault is not recovered by fault handler */
+	BUG_ON(ret != 0);
 
-	if ((ret == -ENOSYS) && data->fault_handler) {
-		unsigned long base = data->pgtable;
-		if (itype != SYSMMU_FAULT_UNKNOWN)
-			base = __raw_readl(
-					data->sfrbases[i] + REG_PT_BASE_ADDR);
-		ret = data->fault_handler(itype, base, addr);
-	}
+	__raw_writel(1 << itype, data->sfrbase + REG_INT_CLEAR);
 
-	if (!ret && (itype != SYSMMU_FAULT_UNKNOWN))
-		__raw_writel(1 << itype, data->sfrbases[i] + REG_INT_CLEAR);
-	else
-		dev_dbg(data->sysmmu, "(%s) %s is not handled.\n",
-				data->dbgname, sysmmu_fault_name[itype]);
+	sysmmu_unblock(data->sfrbase);
 
-	if (itype != SYSMMU_FAULT_UNKNOWN)
-		sysmmu_unblock(data->sfrbases[i]);
+	if (!IS_ERR(data->clk_master))
+		clk_disable(data->clk_master);
 
-	read_unlock(&data->lock);
+	spin_unlock(&data->lock);
 
 	return IRQ_HANDLED;
 }
 
-static bool __exynos_sysmmu_disable(struct sysmmu_drvdata *data)
+static void __sysmmu_disable_nocount(struct sysmmu_drvdata *data)
 {
+	if (!IS_ERR(data->clk_master))
+		clk_enable(data->clk_master);
+
+	__raw_writel(CTRL_DISABLE, data->sfrbase + REG_MMU_CTRL);
+	__raw_writel(0, data->sfrbase + REG_MMU_CFG);
+
+	clk_disable(data->clk);
+	if (!IS_ERR(data->clk_master))
+		clk_disable(data->clk_master);
+}
+
+static bool __sysmmu_disable(struct sysmmu_drvdata *data)
+{
+	bool disabled;
 	unsigned long flags;
-	bool disabled = false;
-	int i;
 
-	write_lock_irqsave(&data->lock, flags);
+	spin_lock_irqsave(&data->lock, flags);
 
-	if (!set_sysmmu_inactive(data))
-		goto finish;
+	disabled = set_sysmmu_inactive(data);
 
-	for (i = 0; i < data->nsfrs; i++)
-		__raw_writel(CTRL_DISABLE, data->sfrbases[i] + REG_MMU_CTRL);
+	if (disabled) {
+		data->pgtable = 0;
+		data->domain = NULL;
 
-	if (data->clk[1])
-		clk_disable(data->clk[1]);
-	if (data->clk[0])
-		clk_disable(data->clk[0]);
+		__sysmmu_disable_nocount(data);
 
-	disabled = true;
-	data->pgtable = 0;
-	data->domain = NULL;
-finish:
-	write_unlock_irqrestore(&data->lock, flags);
+		dev_dbg(data->sysmmu, "Disabled\n");
+	} else  {
+		dev_dbg(data->sysmmu, "%d times left to disable\n",
+					data->activations);
+	}
 
-	if (disabled)
-		dev_dbg(data->sysmmu, "(%s) Disabled\n", data->dbgname);
-	else
-		dev_dbg(data->sysmmu, "(%s) %d times left to be disabled\n",
-					data->dbgname, data->activations);
+	spin_unlock_irqrestore(&data->lock, flags);
 
 	return disabled;
 }
 
-/* __exynos_sysmmu_enable: Enables System MMU
- *
- * returns -error if an error occurred and System MMU is not enabled,
- * 0 if the System MMU has been just enabled and 1 if System MMU was already
- * enabled before.
- */
-static int __exynos_sysmmu_enable(struct sysmmu_drvdata *data,
-			unsigned long pgtable, struct iommu_domain *domain)
+static void __sysmmu_init_config(struct sysmmu_drvdata *data)
 {
-	int i, ret = 0;
-	unsigned long flags;
+	unsigned int cfg = CFG_LRU | CFG_QOS(15);
+	unsigned int ver;
+
+	ver = __raw_sysmmu_version(data);
+	if (MMU_MAJ_VER(ver) == 3) {
+		if (MMU_MIN_VER(ver) >= 2) {
+			cfg |= CFG_FLPDCACHE;
+			if (MMU_MIN_VER(ver) == 3) {
+				cfg |= CFG_ACGEN;
+				cfg &= ~CFG_LRU;
+			} else {
+				cfg |= CFG_SYSSEL;
+			}
+		}
+	}
 
-	write_lock_irqsave(&data->lock, flags);
+	__raw_writel(cfg, data->sfrbase + REG_MMU_CFG);
+}
 
-	if (!set_sysmmu_active(data)) {
-		if (WARN_ON(pgtable != data->pgtable)) {
-			ret = -EBUSY;
-			set_sysmmu_inactive(data);
-		} else {
-			ret = 1;
-		}
+static void __sysmmu_enable_nocount(struct sysmmu_drvdata *data)
+{
+	if (!IS_ERR(data->clk_master))
+		clk_enable(data->clk_master);
+	clk_enable(data->clk);
 
-		dev_dbg(data->sysmmu, "(%s) Already enabled\n", data->dbgname);
-		goto finish;
-	}
+	__raw_writel(CTRL_BLOCK, data->sfrbase + REG_MMU_CTRL);
 
-	if (data->clk[0])
-		clk_enable(data->clk[0]);
-	if (data->clk[1])
-		clk_enable(data->clk[1]);
+	__sysmmu_init_config(data);
 
-	data->pgtable = pgtable;
+	__sysmmu_set_ptbase(data->sfrbase, data->pgtable);
 
-	for (i = 0; i < data->nsfrs; i++) {
-		__sysmmu_set_ptbase(data->sfrbases[i], pgtable);
+	__raw_writel(CTRL_ENABLE, data->sfrbase + REG_MMU_CTRL);
 
-		if ((readl(data->sfrbases[i] + REG_MMU_VERSION) >> 28) == 3) {
-			/* System MMU version is 3.x */
-			__raw_writel((1 << 12) | (2 << 28),
-					data->sfrbases[i] + REG_MMU_CFG);
-			__sysmmu_set_prefbuf(data->sfrbases[i], 0, -1, 0);
-			__sysmmu_set_prefbuf(data->sfrbases[i], 0, -1, 1);
-		}
+	if (!IS_ERR(data->clk_master))
+		clk_disable(data->clk_master);
+}
 
-		__raw_writel(CTRL_ENABLE, data->sfrbases[i] + REG_MMU_CTRL);
+static int __sysmmu_enable(struct sysmmu_drvdata *data,
+			phys_addr_t pgtable, struct iommu_domain *domain)
+{
+	int ret = 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(&data->lock, flags);
+	if (set_sysmmu_active(data)) {
+		data->pgtable = pgtable;
+		data->domain = domain;
+
+		__sysmmu_enable_nocount(data);
+
+		dev_dbg(data->sysmmu, "Enabled\n");
+	} else {
+		ret = (pgtable == data->pgtable) ? 1 : -EBUSY;
+
+		dev_dbg(data->sysmmu, "already enabled\n");
 	}
 
-	data->domain = domain;
+	if (WARN_ON(ret < 0))
+		set_sysmmu_inactive(data); /* decrement count */
 
-	dev_dbg(data->sysmmu, "(%s) Enabled\n", data->dbgname);
-finish:
-	write_unlock_irqrestore(&data->lock, flags);
+	spin_unlock_irqrestore(&data->lock, flags);
 
 	return ret;
 }
 
-int exynos_sysmmu_enable(struct device *dev, unsigned long pgtable)
+/* __exynos_sysmmu_enable: Enables System MMU
+ *
+ * returns -error if an error occurred and System MMU is not enabled,
+ * 0 if the System MMU has been just enabled and 1 if System MMU was already
+ * enabled before.
+ */
+static int __exynos_sysmmu_enable(struct device *dev, phys_addr_t pgtable,
+				  struct iommu_domain *domain)
 {
-	struct sysmmu_drvdata *data = dev_get_drvdata(dev->archdata.iommu);
-	int ret;
+	int ret = 0;
+	unsigned long flags;
+	struct exynos_iommu_owner *owner = dev->archdata.iommu;
+	struct sysmmu_drvdata *data;
 
-	BUG_ON(!memblock_is_memory(pgtable));
+	BUG_ON(!has_sysmmu(dev));
 
-	ret = pm_runtime_get_sync(data->sysmmu);
-	if (ret < 0) {
-		dev_dbg(data->sysmmu, "(%s) Failed to enable\n", data->dbgname);
-		return ret;
-	}
+	spin_lock_irqsave(&owner->lock, flags);
 
-	ret = __exynos_sysmmu_enable(data, pgtable, NULL);
-	if (WARN_ON(ret < 0)) {
-		pm_runtime_put(data->sysmmu);
-		dev_err(data->sysmmu,
-			"(%s) Already enabled with page table %#lx\n",
-			data->dbgname, data->pgtable);
-	} else {
-		data->dev = dev;
-	}
+	data = dev_get_drvdata(owner->sysmmu);
+
+	ret = __sysmmu_enable(data, pgtable, domain);
+	if (ret >= 0)
+		data->master = dev;
+
+	spin_unlock_irqrestore(&owner->lock, flags);
 
 	return ret;
 }
 
+int exynos_sysmmu_enable(struct device *dev, phys_addr_t pgtable)
+{
+	BUG_ON(!memblock_is_memory(pgtable));
+
+	return __exynos_sysmmu_enable(dev, pgtable, NULL);
+}
+
 static bool exynos_sysmmu_disable(struct device *dev)
 {
-	struct sysmmu_drvdata *data = dev_get_drvdata(dev->archdata.iommu);
-	bool disabled;
+	unsigned long flags;
+	bool disabled = true;
+	struct exynos_iommu_owner *owner = dev->archdata.iommu;
+	struct sysmmu_drvdata *data;
+
+	BUG_ON(!has_sysmmu(dev));
+
+	spin_lock_irqsave(&owner->lock, flags);
 
-	disabled = __exynos_sysmmu_disable(data);
-	pm_runtime_put(data->sysmmu);
+	data = dev_get_drvdata(owner->sysmmu);
+
+	disabled = __sysmmu_disable(data);
+	if (disabled)
+		data->master = NULL;
+
+	spin_unlock_irqrestore(&owner->lock, flags);
 
 	return disabled;
 }
 
-static void sysmmu_tlb_invalidate_entry(struct device *dev, unsigned long iova)
+static void __sysmmu_tlb_invalidate_flpdcache(struct sysmmu_drvdata *data,
+					      sysmmu_iova_t iova)
+{
+	if (__raw_sysmmu_version(data) == MAKE_MMU_VER(3, 3))
+		__raw_writel(iova | 0x1, data->sfrbase + REG_MMU_FLUSH_ENTRY);
+}
+
+static void sysmmu_tlb_invalidate_flpdcache(struct device *dev,
+					    sysmmu_iova_t iova)
+{
+	unsigned long flags;
+	struct exynos_iommu_owner *owner = dev->archdata.iommu;
+	struct sysmmu_drvdata *data = dev_get_drvdata(owner->sysmmu);
+
+	if (!IS_ERR(data->clk_master))
+		clk_enable(data->clk_master);
+
+	spin_lock_irqsave(&data->lock, flags);
+	if (is_sysmmu_active(data))
+		__sysmmu_tlb_invalidate_flpdcache(data, iova);
+	spin_unlock_irqrestore(&data->lock, flags);
+
+	if (!IS_ERR(data->clk_master))
+		clk_disable(data->clk_master);
+}
+
+static void sysmmu_tlb_invalidate_entry(struct device *dev, sysmmu_iova_t iova,
+					size_t size)
 {
+	struct exynos_iommu_owner *owner = dev->archdata.iommu;
 	unsigned long flags;
-	struct sysmmu_drvdata *data = dev_get_drvdata(dev->archdata.iommu);
+	struct sysmmu_drvdata *data;
 
-	read_lock_irqsave(&data->lock, flags);
+	data = dev_get_drvdata(owner->sysmmu);
 
+	spin_lock_irqsave(&data->lock, flags);
 	if (is_sysmmu_active(data)) {
-		int i;
-		for (i = 0; i < data->nsfrs; i++) {
-			if (sysmmu_block(data->sfrbases[i])) {
-				__sysmmu_tlb_invalidate_entry(
-						data->sfrbases[i], iova);
-				sysmmu_unblock(data->sfrbases[i]);
-			}
+		unsigned int num_inv = 1;
+
+		if (!IS_ERR(data->clk_master))
+			clk_enable(data->clk_master);
+
+		/*
+		 * L2TLB invalidation required
+		 * 4KB page: 1 invalidation
+		 * 64KB page: 16 invalidation
+		 * 1MB page: 64 invalidation
+		 * because it is set-associative TLB
+		 * with 8-way and 64 sets.
+		 * 1MB page can be cached in one of all sets.
+		 * 64KB page can be one of 16 consecutive sets.
+		 */
+		if (MMU_MAJ_VER(__raw_sysmmu_version(data)) == 2)
+			num_inv = min_t(unsigned int, size / PAGE_SIZE, 64);
+
+		if (sysmmu_block(data->sfrbase)) {
+			__sysmmu_tlb_invalidate_entry(
+				data->sfrbase, iova, num_inv);
+			sysmmu_unblock(data->sfrbase);
 		}
+		if (!IS_ERR(data->clk_master))
+			clk_disable(data->clk_master);
 	} else {
-		dev_dbg(data->sysmmu,
-			"(%s) Disabled. Skipping invalidating TLB.\n",
-			data->dbgname);
+		dev_dbg(dev, "disabled. Skipping TLB invalidation @ %#x\n",
+			iova);
 	}
-
-	read_unlock_irqrestore(&data->lock, flags);
+	spin_unlock_irqrestore(&data->lock, flags);
 }
 
 void exynos_sysmmu_tlb_invalidate(struct device *dev)
 {
+	struct exynos_iommu_owner *owner = dev->archdata.iommu;
 	unsigned long flags;
-	struct sysmmu_drvdata *data = dev_get_drvdata(dev->archdata.iommu);
+	struct sysmmu_drvdata *data;
 
-	read_lock_irqsave(&data->lock, flags);
+	data = dev_get_drvdata(owner->sysmmu);
 
+	spin_lock_irqsave(&data->lock, flags);
 	if (is_sysmmu_active(data)) {
-		int i;
-		for (i = 0; i < data->nsfrs; i++) {
-			if (sysmmu_block(data->sfrbases[i])) {
-				__sysmmu_tlb_invalidate(data->sfrbases[i]);
-				sysmmu_unblock(data->sfrbases[i]);
-			}
+		if (!IS_ERR(data->clk_master))
+			clk_enable(data->clk_master);
+		if (sysmmu_block(data->sfrbase)) {
+			__sysmmu_tlb_invalidate(data->sfrbase);
+			sysmmu_unblock(data->sfrbase);
 		}
+		if (!IS_ERR(data->clk_master))
+			clk_disable(data->clk_master);
 	} else {
-		dev_dbg(data->sysmmu,
-			"(%s) Disabled. Skipping invalidating TLB.\n",
-			data->dbgname);
+		dev_dbg(dev, "disabled. Skipping TLB invalidation\n");
 	}
-
-	read_unlock_irqrestore(&data->lock, flags);
+	spin_unlock_irqrestore(&data->lock, flags);
 }
 
-static int exynos_sysmmu_probe(struct platform_device *pdev)
+static int __init exynos_sysmmu_probe(struct platform_device *pdev)
 {
-	int i, ret;
-	struct device *dev;
+	int irq, ret;
+	struct device *dev = &pdev->dev;
 	struct sysmmu_drvdata *data;
+	struct resource *res;
 
-	dev = &pdev->dev;
+	data = devm_kzalloc(dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
-	data = kzalloc(sizeof(*data), GFP_KERNEL);
-	if (!data) {
-		dev_dbg(dev, "Not enough memory\n");
-		ret = -ENOMEM;
-		goto err_alloc;
-	}
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	data->sfrbase = devm_ioremap_resource(dev, res);
+	if (IS_ERR(data->sfrbase))
+		return PTR_ERR(data->sfrbase);
 
-	ret = dev_set_drvdata(dev, data);
-	if (ret) {
-		dev_dbg(dev, "Unabled to initialize driver data\n");
-		goto err_init;
+	irq = platform_get_irq(pdev, 0);
+	if (irq <= 0) {
+		dev_err(dev, "Unable to find IRQ resource\n");
+		return irq;
 	}
 
-	data->nsfrs = pdev->num_resources / 2;
-	data->sfrbases = kmalloc(sizeof(*data->sfrbases) * data->nsfrs,
-								GFP_KERNEL);
-	if (data->sfrbases == NULL) {
-		dev_dbg(dev, "Not enough memory\n");
-		ret = -ENOMEM;
-		goto err_init;
-	}
-
-	for (i = 0; i < data->nsfrs; i++) {
-		struct resource *res;
-		res = platform_get_resource(pdev, IORESOURCE_MEM, i);
-		if (!res) {
-			dev_dbg(dev, "Unable to find IOMEM region\n");
-			ret = -ENOENT;
-			goto err_res;
-		}
-
-		data->sfrbases[i] = ioremap(res->start, resource_size(res));
-		if (!data->sfrbases[i]) {
-			dev_dbg(dev, "Unable to map IOMEM @ PA:%#x\n",
-							res->start);
-			ret = -ENOENT;
-			goto err_res;
-		}
+	ret = devm_request_irq(dev, irq, exynos_sysmmu_irq, 0,
+				dev_name(dev), data);
+	if (ret) {
+		dev_err(dev, "Unabled to register handler of irq %d\n", irq);
+		return ret;
 	}
 
-	for (i = 0; i < data->nsfrs; i++) {
-		ret = platform_get_irq(pdev, i);
-		if (ret <= 0) {
-			dev_dbg(dev, "Unable to find IRQ resource\n");
-			goto err_irq;
-		}
-
-		ret = request_irq(ret, exynos_sysmmu_irq, 0,
-					dev_name(dev), data);
+	data->clk = devm_clk_get(dev, "sysmmu");
+	if (IS_ERR(data->clk)) {
+		dev_err(dev, "Failed to get clock!\n");
+		return PTR_ERR(data->clk);
+	} else  {
+		ret = clk_prepare(data->clk);
 		if (ret) {
-			dev_dbg(dev, "Unabled to register interrupt handler\n");
-			goto err_irq;
+			dev_err(dev, "Failed to prepare clk\n");
+			return ret;
 		}
 	}
 
-	if (dev_get_platdata(dev)) {
-		char *deli, *beg;
-		struct sysmmu_platform_data *platdata = dev_get_platdata(dev);
-
-		beg = platdata->clockname;
-
-		for (deli = beg; (*deli != '\0') && (*deli != ','); deli++)
-			/* NOTHING */;
-
-		if (*deli == '\0')
-			deli = NULL;
-		else
-			*deli = '\0';
-
-		data->clk[0] = clk_get(dev, beg);
-		if (IS_ERR(data->clk[0])) {
-			data->clk[0] = NULL;
-			dev_dbg(dev, "No clock descriptor registered\n");
-		}
-
-		if (data->clk[0] && deli) {
-			*deli = ',';
-			data->clk[1] = clk_get(dev, deli + 1);
-			if (IS_ERR(data->clk[1]))
-				data->clk[1] = NULL;
+	data->clk_master = devm_clk_get(dev, "master");
+	if (!IS_ERR(data->clk_master)) {
+		ret = clk_prepare(data->clk_master);
+		if (ret) {
+			clk_unprepare(data->clk);
+			dev_err(dev, "Failed to prepare master's clk\n");
+			return ret;
 		}
-
-		data->dbgname = platdata->dbgname;
 	}
 
 	data->sysmmu = dev;
-	rwlock_init(&data->lock);
-	INIT_LIST_HEAD(&data->node);
+	spin_lock_init(&data->lock);
 
-	__set_fault_handler(data, &default_fault_handler);
+	platform_set_drvdata(pdev, data);
 
-	if (dev->parent)
-		pm_runtime_enable(dev);
+	pm_runtime_enable(dev);
 
-	dev_dbg(dev, "(%s) Initialized\n", data->dbgname);
 	return 0;
-err_irq:
-	while (i-- > 0) {
-		int irq;
-
-		irq = platform_get_irq(pdev, i);
-		free_irq(irq, data);
-	}
-err_res:
-	while (data->nsfrs-- > 0)
-		iounmap(data->sfrbases[data->nsfrs]);
-	kfree(data->sfrbases);
-err_init:
-	kfree(data);
-err_alloc:
-	dev_err(dev, "Failed to initialize\n");
-	return ret;
 }
 
-static struct platform_driver exynos_sysmmu_driver = {
-	.probe		= exynos_sysmmu_probe,
-	.driver		= {
+static const struct of_device_id sysmmu_of_match[] __initconst = {
+	{ .compatible	= "samsung,exynos-sysmmu", },
+	{ },
+};
+
+static struct platform_driver exynos_sysmmu_driver __refdata = {
+	.probe	= exynos_sysmmu_probe,
+	.driver	= {
 		.owner		= THIS_MODULE,
 		.name		= "exynos-sysmmu",
+		.of_match_table	= sysmmu_of_match,
 	}
 };
 
@@ -667,21 +700,32 @@ static inline void pgtable_flush(void *vastart, void *vaend)
 static int exynos_iommu_domain_init(struct iommu_domain *domain)
 {
 	struct exynos_iommu_domain *priv;
+	int i;
 
 	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
 
-	priv->pgtable = (unsigned long *)__get_free_pages(
-						GFP_KERNEL | __GFP_ZERO, 2);
+	priv->pgtable = (sysmmu_pte_t *)__get_free_pages(GFP_KERNEL, 2);
 	if (!priv->pgtable)
 		goto err_pgtable;
 
-	priv->lv2entcnt = (short *)__get_free_pages(
-						GFP_KERNEL | __GFP_ZERO, 1);
+	priv->lv2entcnt = (short *)__get_free_pages(GFP_KERNEL | __GFP_ZERO, 1);
 	if (!priv->lv2entcnt)
 		goto err_counter;
 
+	/* w/a of 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;
+		priv->pgtable[i + 2] = ZERO_LV2LINK;
+		priv->pgtable[i + 3] = ZERO_LV2LINK;
+		priv->pgtable[i + 4] = ZERO_LV2LINK;
+		priv->pgtable[i + 5] = ZERO_LV2LINK;
+		priv->pgtable[i + 6] = ZERO_LV2LINK;
+		priv->pgtable[i + 7] = ZERO_LV2LINK;
+	}
+
 	pgtable_flush(priv->pgtable, priv->pgtable + NUM_LV1ENTRIES);
 
 	spin_lock_init(&priv->lock);
@@ -705,7 +749,7 @@ err_pgtable:
 static void exynos_iommu_domain_destroy(struct iommu_domain *domain)
 {
 	struct exynos_iommu_domain *priv = domain->priv;
-	struct sysmmu_drvdata *data;
+	struct exynos_iommu_owner *owner;
 	unsigned long flags;
 	int i;
 
@@ -713,16 +757,20 @@ static void exynos_iommu_domain_destroy(struct iommu_domain *domain)
 
 	spin_lock_irqsave(&priv->lock, flags);
 
-	list_for_each_entry(data, &priv->clients, node) {
-		while (!exynos_sysmmu_disable(data->dev))
+	list_for_each_entry(owner, &priv->clients, client) {
+		while (!exynos_sysmmu_disable(owner->dev))
 			; /* until System MMU is actually disabled */
 	}
 
+	while (!list_empty(&priv->clients))
+		list_del_init(priv->clients.next);
+
 	spin_unlock_irqrestore(&priv->lock, flags);
 
 	for (i = 0; i < NUM_LV1ENTRIES; i++)
 		if (lv1ent_page(priv->pgtable + i))
-			kfree(__va(lv2table_base(priv->pgtable + i)));
+			kmem_cache_free(lv2table_kmem_cache,
+				phys_to_virt(lv2table_base(priv->pgtable + i)));
 
 	free_pages((unsigned long)priv->pgtable, 2);
 	free_pages((unsigned long)priv->lv2entcnt, 1);
@@ -733,114 +781,134 @@ static void exynos_iommu_domain_destroy(struct iommu_domain *domain)
 static int exynos_iommu_attach_device(struct iommu_domain *domain,
 				   struct device *dev)
 {
-	struct sysmmu_drvdata *data = dev_get_drvdata(dev->archdata.iommu);
+	struct exynos_iommu_owner *owner = dev->archdata.iommu;
 	struct exynos_iommu_domain *priv = domain->priv;
+	phys_addr_t pagetable = virt_to_phys(priv->pgtable);
 	unsigned long flags;
 	int ret;
 
-	ret = pm_runtime_get_sync(data->sysmmu);
-	if (ret < 0)
-		return ret;
-
-	ret = 0;
-
 	spin_lock_irqsave(&priv->lock, flags);
 
-	ret = __exynos_sysmmu_enable(data, __pa(priv->pgtable), domain);
-
+	ret = __exynos_sysmmu_enable(dev, pagetable, domain);
 	if (ret == 0) {
-		/* 'data->node' must not be appeared in priv->clients */
-		BUG_ON(!list_empty(&data->node));
-		data->dev = dev;
-		list_add_tail(&data->node, &priv->clients);
+		list_add_tail(&owner->client, &priv->clients);
+		owner->domain = domain;
 	}
 
 	spin_unlock_irqrestore(&priv->lock, flags);
 
 	if (ret < 0) {
-		dev_err(dev, "%s: Failed to attach IOMMU with pgtable %#lx\n",
-				__func__, __pa(priv->pgtable));
-		pm_runtime_put(data->sysmmu);
-	} else if (ret > 0) {
-		dev_dbg(dev, "%s: IOMMU with pgtable 0x%lx already attached\n",
-					__func__, __pa(priv->pgtable));
-	} else {
-		dev_dbg(dev, "%s: Attached new IOMMU with pgtable 0x%lx\n",
-					__func__, __pa(priv->pgtable));
+		dev_err(dev, "%s: Failed to attach IOMMU with pgtable %pa\n",
+					__func__, &pagetable);
+		return ret;
 	}
 
+	dev_dbg(dev, "%s: Attached IOMMU with pgtable %pa %s\n",
+		__func__, &pagetable, (ret == 0) ? "" : ", again");
+
 	return ret;
 }
 
 static void exynos_iommu_detach_device(struct iommu_domain *domain,
 				    struct device *dev)
 {
-	struct sysmmu_drvdata *data = dev_get_drvdata(dev->archdata.iommu);
+	struct exynos_iommu_owner *owner;
 	struct exynos_iommu_domain *priv = domain->priv;
-	struct list_head *pos;
+	phys_addr_t pagetable = virt_to_phys(priv->pgtable);
 	unsigned long flags;
-	bool found = false;
 
 	spin_lock_irqsave(&priv->lock, flags);
 
-	list_for_each(pos, &priv->clients) {
-		if (list_entry(pos, struct sysmmu_drvdata, node) == data) {
-			found = true;
+	list_for_each_entry(owner, &priv->clients, client) {
+		if (owner == dev->archdata.iommu) {
+			if (exynos_sysmmu_disable(dev)) {
+				list_del_init(&owner->client);
+				owner->domain = NULL;
+			}
 			break;
 		}
 	}
 
-	if (!found)
-		goto finish;
-
-	if (__exynos_sysmmu_disable(data)) {
-		dev_dbg(dev, "%s: Detached IOMMU with pgtable %#lx\n",
-					__func__, __pa(priv->pgtable));
-		list_del_init(&data->node);
-
-	} else {
-		dev_dbg(dev, "%s: Detaching IOMMU with pgtable %#lx delayed",
-					__func__, __pa(priv->pgtable));
-	}
-
-finish:
 	spin_unlock_irqrestore(&priv->lock, flags);
 
-	if (found)
-		pm_runtime_put(data->sysmmu);
+	if (owner == dev->archdata.iommu)
+		dev_dbg(dev, "%s: Detached IOMMU with pgtable %pa\n",
+					__func__, &pagetable);
+	else
+		dev_err(dev, "%s: No IOMMU is attached\n", __func__);
 }
 
-static unsigned long *alloc_lv2entry(unsigned long *sent, unsigned long iova,
-					short *pgcounter)
+static sysmmu_pte_t *alloc_lv2entry(struct exynos_iommu_domain *priv,
+		sysmmu_pte_t *sent, sysmmu_iova_t iova, short *pgcounter)
 {
+	if (lv1ent_section(sent)) {
+		WARN(1, "Trying mapping on %#08x mapped with 1MiB page", iova);
+		return ERR_PTR(-EADDRINUSE);
+	}
+
 	if (lv1ent_fault(sent)) {
-		unsigned long *pent;
+		sysmmu_pte_t *pent;
+		bool need_flush_flpd_cache = lv1ent_zero(sent);
 
-		pent = kzalloc(LV2TABLE_SIZE, GFP_ATOMIC);
-		BUG_ON((unsigned long)pent & (LV2TABLE_SIZE - 1));
+		pent = kmem_cache_zalloc(lv2table_kmem_cache, GFP_ATOMIC);
+		BUG_ON((unsigned int)pent & (LV2TABLE_SIZE - 1));
 		if (!pent)
-			return NULL;
+			return ERR_PTR(-ENOMEM);
 
-		*sent = mk_lv1ent_page(__pa(pent));
+		*sent = mk_lv1ent_page(virt_to_phys(pent));
 		*pgcounter = NUM_LV2ENTRIES;
 		pgtable_flush(pent, pent + NUM_LV2ENTRIES);
 		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.
+		 * 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
+		 * Thus any replacement of zero_l2_table with other valid L2
+		 * page table must involve FLPD cache invalidation for System
+		 * MMU v3.3.
+		 * FLPD cache invalidation is performed with TLB invalidation
+		 * by VPN without blocking. It is safe to invalidate TLB without
+		 * blocking because the target address of TLB invalidation is
+		 * not currently mapped.
+		 */
+		if (need_flush_flpd_cache) {
+			struct exynos_iommu_owner *owner;
+
+			spin_lock(&priv->lock);
+			list_for_each_entry(owner, &priv->clients, client)
+				sysmmu_tlb_invalidate_flpdcache(
+							owner->dev, iova);
+			spin_unlock(&priv->lock);
+		}
 	}
 
 	return page_entry(sent, iova);
 }
 
-static int lv1set_section(unsigned long *sent, phys_addr_t paddr, short *pgcnt)
+static int lv1set_section(struct exynos_iommu_domain *priv,
+			  sysmmu_pte_t *sent, sysmmu_iova_t iova,
+			  phys_addr_t paddr, short *pgcnt)
 {
-	if (lv1ent_section(sent))
+	if (lv1ent_section(sent)) {
+		WARN(1, "Trying mapping on 1MiB@%#08x that is mapped",
+			iova);
 		return -EADDRINUSE;
+	}
 
 	if (lv1ent_page(sent)) {
-		if (*pgcnt != NUM_LV2ENTRIES)
+		if (*pgcnt != NUM_LV2ENTRIES) {
+			WARN(1, "Trying mapping on 1MiB@%#08x that is mapped",
+				iova);
 			return -EADDRINUSE;
+		}
 
-		kfree(page_entry(sent, 0));
-
+		kmem_cache_free(lv2table_kmem_cache, page_entry(sent, 0));
 		*pgcnt = 0;
 	}
 
@@ -848,14 +916,26 @@ static int lv1set_section(unsigned long *sent, phys_addr_t paddr, short *pgcnt)
 
 	pgtable_flush(sent, sent + 1);
 
+	spin_lock(&priv->lock);
+	if (lv1ent_page_zero(sent)) {
+		struct exynos_iommu_owner *owner;
+		/*
+		 * Flushing FLPD cache in System MMU v3.3 that may cache a FLPD
+		 * entry by speculative prefetch of SLPD which has no mapping.
+		 */
+		list_for_each_entry(owner, &priv->clients, client)
+			sysmmu_tlb_invalidate_flpdcache(owner->dev, iova);
+	}
+	spin_unlock(&priv->lock);
+
 	return 0;
 }
 
-static int lv2set_page(unsigned long *pent, phys_addr_t paddr, size_t size,
+static int lv2set_page(sysmmu_pte_t *pent, phys_addr_t paddr, size_t size,
 								short *pgcnt)
 {
 	if (size == SPAGE_SIZE) {
-		if (!lv2ent_fault(pent))
+		if (WARN_ON(!lv2ent_fault(pent)))
 			return -EADDRINUSE;
 
 		*pent = mk_lv2ent_spage(paddr);
@@ -863,9 +943,11 @@ static int lv2set_page(unsigned long *pent, phys_addr_t paddr, size_t size,
 		*pgcnt -= 1;
 	} else { /* size == LPAGE_SIZE */
 		int i;
+
 		for (i = 0; i < SPAGES_PER_LPAGE; i++, pent++) {
-			if (!lv2ent_fault(pent)) {
-				memset(pent, 0, sizeof(*pent) * i);
+			if (WARN_ON(!lv2ent_fault(pent))) {
+				if (i > 0)
+					memset(pent - i, 0, sizeof(*pent) * i);
 				return -EADDRINUSE;
 			}
 
@@ -878,11 +960,38 @@ static int lv2set_page(unsigned long *pent, phys_addr_t paddr, size_t size,
 	return 0;
 }
 
-static int exynos_iommu_map(struct iommu_domain *domain, unsigned long iova,
+/*
+ * *CAUTION* to the I/O virtual memory managers that support exynos-iommu:
+ *
+ * System MMU v3.x have an 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.
+ *
+ * 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)
+ *
+ * Precisely, any start address of I/O virtual region must be aligned by
+ * 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.
+ * - 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,
 			 phys_addr_t paddr, size_t size, int prot)
 {
 	struct exynos_iommu_domain *priv = domain->priv;
-	unsigned long *entry;
+	sysmmu_pte_t *entry;
+	sysmmu_iova_t iova = (sysmmu_iova_t)l_iova;
 	unsigned long flags;
 	int ret = -ENOMEM;
 
@@ -893,38 +1002,52 @@ static int exynos_iommu_map(struct iommu_domain *domain, unsigned long iova,
 	entry = section_entry(priv->pgtable, iova);
 
 	if (size == SECT_SIZE) {
-		ret = lv1set_section(entry, paddr,
+		ret = lv1set_section(priv, entry, iova, paddr,
 					&priv->lv2entcnt[lv1ent_offset(iova)]);
 	} else {
-		unsigned long *pent;
+		sysmmu_pte_t *pent;
 
-		pent = alloc_lv2entry(entry, iova,
+		pent = alloc_lv2entry(priv, entry, iova,
 					&priv->lv2entcnt[lv1ent_offset(iova)]);
 
-		if (!pent)
-			ret = -ENOMEM;
+		if (IS_ERR(pent))
+			ret = PTR_ERR(pent);
 		else
 			ret = lv2set_page(pent, paddr, size,
 					&priv->lv2entcnt[lv1ent_offset(iova)]);
 	}
 
-	if (ret) {
-		pr_debug("%s: Failed to map iova 0x%lx/0x%x bytes\n",
-							__func__, iova, size);
-	}
+	if (ret)
+		pr_err("%s: Failed(%d) to map %#zx bytes @ %#x\n",
+			__func__, ret, size, iova);
 
 	spin_unlock_irqrestore(&priv->pgtablelock, flags);
 
 	return ret;
 }
 
+static void exynos_iommu_tlb_invalidate_entry(struct exynos_iommu_domain *priv,
+						sysmmu_iova_t iova, size_t size)
+{
+	struct exynos_iommu_owner *owner;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	list_for_each_entry(owner, &priv->clients, client)
+		sysmmu_tlb_invalidate_entry(owner->dev, iova, size);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
 static size_t exynos_iommu_unmap(struct iommu_domain *domain,
-					       unsigned long iova, size_t size)
+					unsigned long l_iova, size_t size)
 {
 	struct exynos_iommu_domain *priv = domain->priv;
-	struct sysmmu_drvdata *data;
+	sysmmu_iova_t iova = (sysmmu_iova_t)l_iova;
+	sysmmu_pte_t *ent;
+	size_t err_pgsize;
 	unsigned long flags;
-	unsigned long *ent;
 
 	BUG_ON(priv->pgtable == NULL);
 
@@ -933,9 +1056,12 @@ static size_t exynos_iommu_unmap(struct iommu_domain *domain,
 	ent = section_entry(priv->pgtable, iova);
 
 	if (lv1ent_section(ent)) {
-		BUG_ON(size < SECT_SIZE);
+		if (WARN_ON(size < SECT_SIZE)) {
+			err_pgsize = SECT_SIZE;
+			goto err;
+		}
 
-		*ent = 0;
+		*ent = ZERO_LV2LINK; /* w/a for h/w bug in Sysmem MMU v3.3 */
 		pgtable_flush(ent, ent + 1);
 		size = SECT_SIZE;
 		goto done;
@@ -959,34 +1085,42 @@ static size_t exynos_iommu_unmap(struct iommu_domain *domain,
 	if (lv2ent_small(ent)) {
 		*ent = 0;
 		size = SPAGE_SIZE;
+		pgtable_flush(ent, ent + 1);
 		priv->lv2entcnt[lv1ent_offset(iova)] += 1;
 		goto done;
 	}
 
 	/* lv1ent_large(ent) == true here */
-	BUG_ON(size < LPAGE_SIZE);
+	if (WARN_ON(size < LPAGE_SIZE)) {
+		err_pgsize = LPAGE_SIZE;
+		goto err;
+	}
 
 	memset(ent, 0, sizeof(*ent) * SPAGES_PER_LPAGE);
+	pgtable_flush(ent, ent + SPAGES_PER_LPAGE);
 
 	size = LPAGE_SIZE;
 	priv->lv2entcnt[lv1ent_offset(iova)] += SPAGES_PER_LPAGE;
 done:
 	spin_unlock_irqrestore(&priv->pgtablelock, flags);
 
-	spin_lock_irqsave(&priv->lock, flags);
-	list_for_each_entry(data, &priv->clients, node)
-		sysmmu_tlb_invalidate_entry(data->dev, iova);
-	spin_unlock_irqrestore(&priv->lock, flags);
-
+	exynos_iommu_tlb_invalidate_entry(priv, iova, size);
 
 	return size;
+err:
+	spin_unlock_irqrestore(&priv->pgtablelock, flags);
+
+	pr_err("%s: Failed: size(%#zx) @ %#x is smaller than page size %#zx\n",
+		__func__, size, iova, err_pgsize);
+
+	return 0;
 }
 
 static phys_addr_t exynos_iommu_iova_to_phys(struct iommu_domain *domain,
 					  dma_addr_t iova)
 {
 	struct exynos_iommu_domain *priv = domain->priv;
-	unsigned long *entry;
+	sysmmu_pte_t *entry;
 	unsigned long flags;
 	phys_addr_t phys = 0;
 
@@ -1010,14 +1144,42 @@ static phys_addr_t exynos_iommu_iova_to_phys(struct iommu_domain *domain,
 	return phys;
 }
 
+static int exynos_iommu_add_device(struct device *dev)
+{
+	struct iommu_group *group;
+	int ret;
+
+	group = iommu_group_get(dev);
+
+	if (!group) {
+		group = iommu_group_alloc();
+		if (IS_ERR(group)) {
+			dev_err(dev, "Failed to allocate IOMMU group\n");
+			return PTR_ERR(group);
+		}
+	}
+
+	ret = iommu_group_add_device(group, dev);
+	iommu_group_put(group);
+
+	return ret;
+}
+
+static void exynos_iommu_remove_device(struct device *dev)
+{
+	iommu_group_remove_device(dev);
+}
+
 static struct iommu_ops exynos_iommu_ops = {
-	.domain_init = &exynos_iommu_domain_init,
-	.domain_destroy = &exynos_iommu_domain_destroy,
-	.attach_dev = &exynos_iommu_attach_device,
-	.detach_dev = &exynos_iommu_detach_device,
-	.map = &exynos_iommu_map,
-	.unmap = &exynos_iommu_unmap,
-	.iova_to_phys = &exynos_iommu_iova_to_phys,
+	.domain_init = exynos_iommu_domain_init,
+	.domain_destroy = exynos_iommu_domain_destroy,
+	.attach_dev = exynos_iommu_attach_device,
+	.detach_dev = exynos_iommu_detach_device,
+	.map = exynos_iommu_map,
+	.unmap = exynos_iommu_unmap,
+	.iova_to_phys = exynos_iommu_iova_to_phys,
+	.add_device = exynos_iommu_add_device,
+	.remove_device = exynos_iommu_remove_device,
 	.pgsize_bitmap = SECT_SIZE | LPAGE_SIZE | SPAGE_SIZE,
 };
 
@@ -1025,11 +1187,41 @@ static int __init exynos_iommu_init(void)
 {
 	int ret;
 
+	lv2table_kmem_cache = kmem_cache_create("exynos-iommu-lv2table",
+				LV2TABLE_SIZE, LV2TABLE_SIZE, 0, NULL);
+	if (!lv2table_kmem_cache) {
+		pr_err("%s: Failed to create kmem cache\n", __func__);
+		return -ENOMEM;
+	}
+
 	ret = platform_driver_register(&exynos_sysmmu_driver);
+	if (ret) {
+		pr_err("%s: Failed to register driver\n", __func__);
+		goto err_reg_driver;
+	}
 
-	if (ret == 0)
-		bus_set_iommu(&platform_bus_type, &exynos_iommu_ops);
+	zero_lv2_table = kmem_cache_zalloc(lv2table_kmem_cache, GFP_KERNEL);
+	if (zero_lv2_table == NULL) {
+		pr_err("%s: Failed to allocate zero level2 page table\n",
+			__func__);
+		ret = -ENOMEM;
+		goto err_zero_lv2;
+	}
 
+	ret = bus_set_iommu(&platform_bus_type, &exynos_iommu_ops);
+	if (ret) {
+		pr_err("%s: Failed to register exynos-iommu driver.\n",
+								__func__);
+		goto err_set_iommu;
+	}
+
+	return 0;
+err_set_iommu:
+	kmem_cache_free(lv2table_kmem_cache, zero_lv2_table);
+err_zero_lv2:
+	platform_driver_unregister(&exynos_sysmmu_driver);
+err_reg_driver:
+	kmem_cache_destroy(lv2table_kmem_cache);
 	return ret;
 }
 subsys_initcall(exynos_iommu_init);
diff --git a/drivers/iommu/fsl_pamu.c b/drivers/iommu/fsl_pamu.c
index cba0498eb01..b99dd88e31b 100644
--- a/drivers/iommu/fsl_pamu.c
+++ b/drivers/iommu/fsl_pamu.c
@@ -592,8 +592,7 @@ found_cpu_node:
 		/* advance to next node in cache hierarchy */
 		node = of_find_node_by_phandle(*prop);
 		if (!node) {
-			pr_debug("Invalid node for cache hierarchy %s\n",
-				node->full_name);
+			pr_debug("Invalid node for cache hierarchy\n");
 			return ~(u32)0;
 		}
 	}
diff --git a/drivers/iommu/intel-iommu.c b/drivers/iommu/intel-iommu.c
index f256ffc02e2..6bb32773c3a 100644
--- a/drivers/iommu/intel-iommu.c
+++ b/drivers/iommu/intel-iommu.c
@@ -39,6 +39,7 @@
 #include <linux/dmi.h>
 #include <linux/pci-ats.h>
 #include <linux/memblock.h>
+#include <linux/dma-contiguous.h>
 #include <asm/irq_remapping.h>
 #include <asm/cacheflush.h>
 #include <asm/iommu.h>
@@ -3193,7 +3194,7 @@ static void *intel_alloc_coherent(struct device *dev, size_t size,
 				  dma_addr_t *dma_handle, gfp_t flags,
 				  struct dma_attrs *attrs)
 {
-	void *vaddr;
+	struct page *page = NULL;
 	int order;
 
 	size = PAGE_ALIGN(size);
@@ -3208,17 +3209,31 @@ static void *intel_alloc_coherent(struct device *dev, size_t size,
 			flags |= GFP_DMA32;
 	}
 
-	vaddr = (void *)__get_free_pages(flags, order);
-	if (!vaddr)
+	if (flags & __GFP_WAIT) {
+		unsigned int count = size >> PAGE_SHIFT;
+
+		page = dma_alloc_from_contiguous(dev, count, order);
+		if (page && iommu_no_mapping(dev) &&
+		    page_to_phys(page) + size > dev->coherent_dma_mask) {
+			dma_release_from_contiguous(dev, page, count);
+			page = NULL;
+		}
+	}
+
+	if (!page)
+		page = alloc_pages(flags, order);
+	if (!page)
 		return NULL;
-	memset(vaddr, 0, size);
+	memset(page_address(page), 0, size);
 
-	*dma_handle = __intel_map_single(dev, virt_to_bus(vaddr), size,
+	*dma_handle = __intel_map_single(dev, page_to_phys(page), size,
 					 DMA_BIDIRECTIONAL,
 					 dev->coherent_dma_mask);
 	if (*dma_handle)
-		return vaddr;
-	free_pages((unsigned long)vaddr, order);
+		return page_address(page);
+	if (!dma_release_from_contiguous(dev, page, size >> PAGE_SHIFT))
+		__free_pages(page, order);
+
 	return NULL;
 }
 
@@ -3226,12 +3241,14 @@ static void intel_free_coherent(struct device *dev, size_t size, void *vaddr,
 				dma_addr_t dma_handle, struct dma_attrs *attrs)
 {
 	int order;
+	struct page *page = virt_to_page(vaddr);
 
 	size = PAGE_ALIGN(size);
 	order = get_order(size);
 
 	intel_unmap_page(dev, dma_handle, size, DMA_BIDIRECTIONAL, NULL);
-	free_pages((unsigned long)vaddr, order);
+	if (!dma_release_from_contiguous(dev, page, size >> PAGE_SHIFT))
+		__free_pages(page, order);
 }
 
 static void intel_unmap_sg(struct device *dev, struct scatterlist *sglist,
diff --git a/drivers/iommu/ipmmu-vmsa.c b/drivers/iommu/ipmmu-vmsa.c
new file mode 100644
index 00000000000..53cde086e83
--- /dev/null
+++ b/drivers/iommu/ipmmu-vmsa.c
@@ -0,0 +1,1255 @@
+/*
+ * IPMMU VMSA
+ *
+ * Copyright (C) 2014 Renesas Electronics Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ */
+
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/export.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/iommu.h>
+#include <linux/module.h>
+#include <linux/platform_data/ipmmu-vmsa.h>
+#include <linux/platform_device.h>
+#include <linux/sizes.h>
+#include <linux/slab.h>
+
+#include <asm/dma-iommu.h>
+#include <asm/pgalloc.h>
+
+struct ipmmu_vmsa_device {
+	struct device *dev;
+	void __iomem *base;
+	struct list_head list;
+
+	const struct ipmmu_vmsa_platform_data *pdata;
+	unsigned int num_utlbs;
+
+	struct dma_iommu_mapping *mapping;
+};
+
+struct ipmmu_vmsa_domain {
+	struct ipmmu_vmsa_device *mmu;
+	struct iommu_domain *io_domain;
+
+	unsigned int context_id;
+	spinlock_t lock;			/* Protects mappings */
+	pgd_t *pgd;
+};
+
+struct ipmmu_vmsa_archdata {
+	struct ipmmu_vmsa_device *mmu;
+	unsigned int utlb;
+};
+
+static DEFINE_SPINLOCK(ipmmu_devices_lock);
+static LIST_HEAD(ipmmu_devices);
+
+#define TLB_LOOP_TIMEOUT		100	/* 100us */
+
+/* -----------------------------------------------------------------------------
+ * Registers Definition
+ */
+
+#define IM_CTX_SIZE			0x40
+
+#define IMCTR				0x0000
+#define IMCTR_TRE			(1 << 17)
+#define IMCTR_AFE			(1 << 16)
+#define IMCTR_RTSEL_MASK		(3 << 4)
+#define IMCTR_RTSEL_SHIFT		4
+#define IMCTR_TREN			(1 << 3)
+#define IMCTR_INTEN			(1 << 2)
+#define IMCTR_FLUSH			(1 << 1)
+#define IMCTR_MMUEN			(1 << 0)
+
+#define IMCAAR				0x0004
+
+#define IMTTBCR				0x0008
+#define IMTTBCR_EAE			(1 << 31)
+#define IMTTBCR_PMB			(1 << 30)
+#define IMTTBCR_SH1_NON_SHAREABLE	(0 << 28)
+#define IMTTBCR_SH1_OUTER_SHAREABLE	(2 << 28)
+#define IMTTBCR_SH1_INNER_SHAREABLE	(3 << 28)
+#define IMTTBCR_SH1_MASK		(3 << 28)
+#define IMTTBCR_ORGN1_NC		(0 << 26)
+#define IMTTBCR_ORGN1_WB_WA		(1 << 26)
+#define IMTTBCR_ORGN1_WT		(2 << 26)
+#define IMTTBCR_ORGN1_WB		(3 << 26)
+#define IMTTBCR_ORGN1_MASK		(3 << 26)
+#define IMTTBCR_IRGN1_NC		(0 << 24)
+#define IMTTBCR_IRGN1_WB_WA		(1 << 24)
+#define IMTTBCR_IRGN1_WT		(2 << 24)
+#define IMTTBCR_IRGN1_WB		(3 << 24)
+#define IMTTBCR_IRGN1_MASK		(3 << 24)
+#define IMTTBCR_TSZ1_MASK		(7 << 16)
+#define IMTTBCR_TSZ1_SHIFT		16
+#define IMTTBCR_SH0_NON_SHAREABLE	(0 << 12)
+#define IMTTBCR_SH0_OUTER_SHAREABLE	(2 << 12)
+#define IMTTBCR_SH0_INNER_SHAREABLE	(3 << 12)
+#define IMTTBCR_SH0_MASK		(3 << 12)
+#define IMTTBCR_ORGN0_NC		(0 << 10)
+#define IMTTBCR_ORGN0_WB_WA		(1 << 10)
+#define IMTTBCR_ORGN0_WT		(2 << 10)
+#define IMTTBCR_ORGN0_WB		(3 << 10)
+#define IMTTBCR_ORGN0_MASK		(3 << 10)
+#define IMTTBCR_IRGN0_NC		(0 << 8)
+#define IMTTBCR_IRGN0_WB_WA		(1 << 8)
+#define IMTTBCR_IRGN0_WT		(2 << 8)
+#define IMTTBCR_IRGN0_WB		(3 << 8)
+#define IMTTBCR_IRGN0_MASK		(3 << 8)
+#define IMTTBCR_SL0_LVL_2		(0 << 4)
+#define IMTTBCR_SL0_LVL_1		(1 << 4)
+#define IMTTBCR_TSZ0_MASK		(7 << 0)
+#define IMTTBCR_TSZ0_SHIFT		O
+
+#define IMBUSCR				0x000c
+#define IMBUSCR_DVM			(1 << 2)
+#define IMBUSCR_BUSSEL_SYS		(0 << 0)
+#define IMBUSCR_BUSSEL_CCI		(1 << 0)
+#define IMBUSCR_BUSSEL_IMCAAR		(2 << 0)
+#define IMBUSCR_BUSSEL_CCI_IMCAAR	(3 << 0)
+#define IMBUSCR_BUSSEL_MASK		(3 << 0)
+
+#define IMTTLBR0			0x0010
+#define IMTTUBR0			0x0014
+#define IMTTLBR1			0x0018
+#define IMTTUBR1			0x001c
+
+#define IMSTR				0x0020
+#define IMSTR_ERRLVL_MASK		(3 << 12)
+#define IMSTR_ERRLVL_SHIFT		12
+#define IMSTR_ERRCODE_TLB_FORMAT	(1 << 8)
+#define IMSTR_ERRCODE_ACCESS_PERM	(4 << 8)
+#define IMSTR_ERRCODE_SECURE_ACCESS	(5 << 8)
+#define IMSTR_ERRCODE_MASK		(7 << 8)
+#define IMSTR_MHIT			(1 << 4)
+#define IMSTR_ABORT			(1 << 2)
+#define IMSTR_PF			(1 << 1)
+#define IMSTR_TF			(1 << 0)
+
+#define IMMAIR0				0x0028
+#define IMMAIR1				0x002c
+#define IMMAIR_ATTR_MASK		0xff
+#define IMMAIR_ATTR_DEVICE		0x04
+#define IMMAIR_ATTR_NC			0x44
+#define IMMAIR_ATTR_WBRWA		0xff
+#define IMMAIR_ATTR_SHIFT(n)		((n) << 3)
+#define IMMAIR_ATTR_IDX_NC		0
+#define IMMAIR_ATTR_IDX_WBRWA		1
+#define IMMAIR_ATTR_IDX_DEV		2
+
+#define IMEAR				0x0030
+
+#define IMPCTR				0x0200
+#define IMPSTR				0x0208
+#define IMPEAR				0x020c
+#define IMPMBA(n)			(0x0280 + ((n) * 4))
+#define IMPMBD(n)			(0x02c0 + ((n) * 4))
+
+#define IMUCTR(n)			(0x0300 + ((n) * 16))
+#define IMUCTR_FIXADDEN			(1 << 31)
+#define IMUCTR_FIXADD_MASK		(0xff << 16)
+#define IMUCTR_FIXADD_SHIFT		16
+#define IMUCTR_TTSEL_MMU(n)		((n) << 4)
+#define IMUCTR_TTSEL_PMB		(8 << 4)
+#define IMUCTR_TTSEL_MASK		(15 << 4)
+#define IMUCTR_FLUSH			(1 << 1)
+#define IMUCTR_MMUEN			(1 << 0)
+
+#define IMUASID(n)			(0x0308 + ((n) * 16))
+#define IMUASID_ASID8_MASK		(0xff << 8)
+#define IMUASID_ASID8_SHIFT		8
+#define IMUASID_ASID0_MASK		(0xff << 0)
+#define IMUASID_ASID0_SHIFT		0
+
+/* -----------------------------------------------------------------------------
+ * Page Table Bits
+ */
+
+/*
+ * VMSA states in section B3.6.3 "Control of Secure or Non-secure memory access,
+ * Long-descriptor format" that the NStable bit being set in a table descriptor
+ * will result in the NStable and NS bits of all child entries being ignored and
+ * considered as being set. The IPMMU seems not to comply with this, as it
+ * generates a secure access page fault if any of the NStable and NS bits isn't
+ * set when running in non-secure mode.
+ */
+#ifndef PMD_NSTABLE
+#define PMD_NSTABLE			(_AT(pmdval_t, 1) << 63)
+#endif
+
+#define ARM_VMSA_PTE_XN			(((pteval_t)3) << 53)
+#define ARM_VMSA_PTE_CONT		(((pteval_t)1) << 52)
+#define ARM_VMSA_PTE_AF			(((pteval_t)1) << 10)
+#define ARM_VMSA_PTE_SH_NS		(((pteval_t)0) << 8)
+#define ARM_VMSA_PTE_SH_OS		(((pteval_t)2) << 8)
+#define ARM_VMSA_PTE_SH_IS		(((pteval_t)3) << 8)
+#define ARM_VMSA_PTE_SH_MASK		(((pteval_t)3) << 8)
+#define ARM_VMSA_PTE_NS			(((pteval_t)1) << 5)
+#define ARM_VMSA_PTE_PAGE		(((pteval_t)3) << 0)
+
+/* Stage-1 PTE */
+#define ARM_VMSA_PTE_nG			(((pteval_t)1) << 11)
+#define ARM_VMSA_PTE_AP_UNPRIV		(((pteval_t)1) << 6)
+#define ARM_VMSA_PTE_AP_RDONLY		(((pteval_t)2) << 6)
+#define ARM_VMSA_PTE_AP_MASK		(((pteval_t)3) << 6)
+#define ARM_VMSA_PTE_ATTRINDX_MASK	(((pteval_t)3) << 2)
+#define ARM_VMSA_PTE_ATTRINDX_SHIFT	2
+
+#define ARM_VMSA_PTE_ATTRS_MASK \
+	(ARM_VMSA_PTE_XN | ARM_VMSA_PTE_CONT | ARM_VMSA_PTE_nG | \
+	 ARM_VMSA_PTE_AF | ARM_VMSA_PTE_SH_MASK | ARM_VMSA_PTE_AP_MASK | \
+	 ARM_VMSA_PTE_NS | ARM_VMSA_PTE_ATTRINDX_MASK)
+
+#define ARM_VMSA_PTE_CONT_ENTRIES	16
+#define ARM_VMSA_PTE_CONT_SIZE		(PAGE_SIZE * ARM_VMSA_PTE_CONT_ENTRIES)
+
+#define IPMMU_PTRS_PER_PTE		512
+#define IPMMU_PTRS_PER_PMD		512
+#define IPMMU_PTRS_PER_PGD		4
+
+/* -----------------------------------------------------------------------------
+ * Read/Write Access
+ */
+
+static u32 ipmmu_read(struct ipmmu_vmsa_device *mmu, unsigned int offset)
+{
+	return ioread32(mmu->base + offset);
+}
+
+static void ipmmu_write(struct ipmmu_vmsa_device *mmu, unsigned int offset,
+			u32 data)
+{
+	iowrite32(data, mmu->base + offset);
+}
+
+static u32 ipmmu_ctx_read(struct ipmmu_vmsa_domain *domain, unsigned int reg)
+{
+	return ipmmu_read(domain->mmu, domain->context_id * IM_CTX_SIZE + reg);
+}
+
+static void ipmmu_ctx_write(struct ipmmu_vmsa_domain *domain, unsigned int reg,
+			    u32 data)
+{
+	ipmmu_write(domain->mmu, domain->context_id * IM_CTX_SIZE + reg, data);
+}
+
+/* -----------------------------------------------------------------------------
+ * TLB and microTLB Management
+ */
+
+/* Wait for any pending TLB invalidations to complete */
+static void ipmmu_tlb_sync(struct ipmmu_vmsa_domain *domain)
+{
+	unsigned int count = 0;
+
+	while (ipmmu_ctx_read(domain, IMCTR) & IMCTR_FLUSH) {
+		cpu_relax();
+		if (++count == TLB_LOOP_TIMEOUT) {
+			dev_err_ratelimited(domain->mmu->dev,
+			"TLB sync timed out -- MMU may be deadlocked\n");
+			return;
+		}
+		udelay(1);
+	}
+}
+
+static void ipmmu_tlb_invalidate(struct ipmmu_vmsa_domain *domain)
+{
+	u32 reg;
+
+	reg = ipmmu_ctx_read(domain, IMCTR);
+	reg |= IMCTR_FLUSH;
+	ipmmu_ctx_write(domain, IMCTR, reg);
+
+	ipmmu_tlb_sync(domain);
+}
+
+/*
+ * Enable MMU translation for the microTLB.
+ */
+static void ipmmu_utlb_enable(struct ipmmu_vmsa_domain *domain,
+			      unsigned int utlb)
+{
+	struct ipmmu_vmsa_device *mmu = domain->mmu;
+
+	/*
+	 * TODO: Reference-count the microTLB as several bus masters can be
+	 * connected to the same microTLB.
+	 */
+
+	/* TODO: What should we set the ASID to ? */
+	ipmmu_write(mmu, IMUASID(utlb), 0);
+	/* TODO: Do we need to flush the microTLB ? */
+	ipmmu_write(mmu, IMUCTR(utlb),
+		    IMUCTR_TTSEL_MMU(domain->context_id) | IMUCTR_FLUSH |
+		    IMUCTR_MMUEN);
+}
+
+/*
+ * Disable MMU translation for the microTLB.
+ */
+static void ipmmu_utlb_disable(struct ipmmu_vmsa_domain *domain,
+			       unsigned int utlb)
+{
+	struct ipmmu_vmsa_device *mmu = domain->mmu;
+
+	ipmmu_write(mmu, IMUCTR(utlb), 0);
+}
+
+static void ipmmu_flush_pgtable(struct ipmmu_vmsa_device *mmu, void *addr,
+				size_t size)
+{
+	unsigned long offset = (unsigned long)addr & ~PAGE_MASK;
+
+	/*
+	 * TODO: Add support for coherent walk through CCI with DVM and remove
+	 * cache handling.
+	 */
+	dma_map_page(mmu->dev, virt_to_page(addr), offset, size, DMA_TO_DEVICE);
+}
+
+/* -----------------------------------------------------------------------------
+ * Domain/Context Management
+ */
+
+static int ipmmu_domain_init_context(struct ipmmu_vmsa_domain *domain)
+{
+	phys_addr_t ttbr;
+	u32 reg;
+
+	/*
+	 * TODO: When adding support for multiple contexts, find an unused
+	 * context.
+	 */
+	domain->context_id = 0;
+
+	/* TTBR0 */
+	ipmmu_flush_pgtable(domain->mmu, domain->pgd,
+			    IPMMU_PTRS_PER_PGD * sizeof(*domain->pgd));
+	ttbr = __pa(domain->pgd);
+	ipmmu_ctx_write(domain, IMTTLBR0, ttbr);
+	ipmmu_ctx_write(domain, IMTTUBR0, ttbr >> 32);
+
+	/*
+	 * TTBCR
+	 * We use long descriptors with inner-shareable WBWA tables and allocate
+	 * the whole 32-bit VA space to TTBR0.
+	 */
+	ipmmu_ctx_write(domain, IMTTBCR, IMTTBCR_EAE |
+			IMTTBCR_SH0_INNER_SHAREABLE | IMTTBCR_ORGN0_WB_WA |
+			IMTTBCR_IRGN0_WB_WA | IMTTBCR_SL0_LVL_1);
+
+	/*
+	 * MAIR0
+	 * We need three attributes only, non-cacheable, write-back read/write
+	 * allocate and device memory.
+	 */
+	reg = (IMMAIR_ATTR_NC << IMMAIR_ATTR_SHIFT(IMMAIR_ATTR_IDX_NC))
+	    | (IMMAIR_ATTR_WBRWA << IMMAIR_ATTR_SHIFT(IMMAIR_ATTR_IDX_WBRWA))
+	    | (IMMAIR_ATTR_DEVICE << IMMAIR_ATTR_SHIFT(IMMAIR_ATTR_IDX_DEV));
+	ipmmu_ctx_write(domain, IMMAIR0, reg);
+
+	/* IMBUSCR */
+	ipmmu_ctx_write(domain, IMBUSCR,
+			ipmmu_ctx_read(domain, IMBUSCR) &
+			~(IMBUSCR_DVM | IMBUSCR_BUSSEL_MASK));
+
+	/*
+	 * IMSTR
+	 * Clear all interrupt flags.
+	 */
+	ipmmu_ctx_write(domain, IMSTR, ipmmu_ctx_read(domain, IMSTR));
+
+	/*
+	 * IMCTR
+	 * Enable the MMU and interrupt generation. The long-descriptor
+	 * translation table format doesn't use TEX remapping. Don't enable AF
+	 * software management as we have no use for it. Flush the TLB as
+	 * required when modifying the context registers.
+	 */
+	ipmmu_ctx_write(domain, IMCTR, IMCTR_INTEN | IMCTR_FLUSH | IMCTR_MMUEN);
+
+	return 0;
+}
+
+static void ipmmu_domain_destroy_context(struct ipmmu_vmsa_domain *domain)
+{
+	/*
+	 * Disable the context. Flush the TLB as required when modifying the
+	 * context registers.
+	 *
+	 * TODO: Is TLB flush really needed ?
+	 */
+	ipmmu_ctx_write(domain, IMCTR, IMCTR_FLUSH);
+	ipmmu_tlb_sync(domain);
+}
+
+/* -----------------------------------------------------------------------------
+ * Fault Handling
+ */
+
+static irqreturn_t ipmmu_domain_irq(struct ipmmu_vmsa_domain *domain)
+{
+	const u32 err_mask = IMSTR_MHIT | IMSTR_ABORT | IMSTR_PF | IMSTR_TF;
+	struct ipmmu_vmsa_device *mmu = domain->mmu;
+	u32 status;
+	u32 iova;
+
+	status = ipmmu_ctx_read(domain, IMSTR);
+	if (!(status & err_mask))
+		return IRQ_NONE;
+
+	iova = ipmmu_ctx_read(domain, IMEAR);
+
+	/*
+	 * Clear the error status flags. Unlike traditional interrupt flag
+	 * registers that must be cleared by writing 1, this status register
+	 * seems to require 0. The error address register must be read before,
+	 * otherwise its value will be 0.
+	 */
+	ipmmu_ctx_write(domain, IMSTR, 0);
+
+	/* Log fatal errors. */
+	if (status & IMSTR_MHIT)
+		dev_err_ratelimited(mmu->dev, "Multiple TLB hits @0x%08x\n",
+				    iova);
+	if (status & IMSTR_ABORT)
+		dev_err_ratelimited(mmu->dev, "Page Table Walk Abort @0x%08x\n",
+				    iova);
+
+	if (!(status & (IMSTR_PF | IMSTR_TF)))
+		return IRQ_NONE;
+
+	/*
+	 * Try to handle page faults and translation faults.
+	 *
+	 * TODO: We need to look up the faulty device based on the I/O VA. Use
+	 * the IOMMU device for now.
+	 */
+	if (!report_iommu_fault(domain->io_domain, mmu->dev, iova, 0))
+		return IRQ_HANDLED;
+
+	dev_err_ratelimited(mmu->dev,
+			    "Unhandled fault: status 0x%08x iova 0x%08x\n",
+			    status, iova);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t ipmmu_irq(int irq, void *dev)
+{
+	struct ipmmu_vmsa_device *mmu = dev;
+	struct iommu_domain *io_domain;
+	struct ipmmu_vmsa_domain *domain;
+
+	if (!mmu->mapping)
+		return IRQ_NONE;
+
+	io_domain = mmu->mapping->domain;
+	domain = io_domain->priv;
+
+	return ipmmu_domain_irq(domain);
+}
+
+/* -----------------------------------------------------------------------------
+ * Page Table Management
+ */
+
+#define pud_pgtable(pud) pfn_to_page(__phys_to_pfn(pud_val(pud) & PHYS_MASK))
+
+static void ipmmu_free_ptes(pmd_t *pmd)
+{
+	pgtable_t table = pmd_pgtable(*pmd);
+	__free_page(table);
+}
+
+static void ipmmu_free_pmds(pud_t *pud)
+{
+	pmd_t *pmd = pmd_offset(pud, 0);
+	pgtable_t table;
+	unsigned int i;
+
+	for (i = 0; i < IPMMU_PTRS_PER_PMD; ++i) {
+		if (!pmd_table(*pmd))
+			continue;
+
+		ipmmu_free_ptes(pmd);
+		pmd++;
+	}
+
+	table = pud_pgtable(*pud);
+	__free_page(table);
+}
+
+static void ipmmu_free_pgtables(struct ipmmu_vmsa_domain *domain)
+{
+	pgd_t *pgd, *pgd_base = domain->pgd;
+	unsigned int i;
+
+	/*
+	 * Recursively free the page tables for this domain. We don't care about
+	 * speculative TLB filling, because the TLB will be nuked next time this
+	 * context bank is re-allocated and no devices currently map to these
+	 * tables.
+	 */
+	pgd = pgd_base;
+	for (i = 0; i < IPMMU_PTRS_PER_PGD; ++i) {
+		if (pgd_none(*pgd))
+			continue;
+		ipmmu_free_pmds((pud_t *)pgd);
+		pgd++;
+	}
+
+	kfree(pgd_base);
+}
+
+/*
+ * We can't use the (pgd|pud|pmd|pte)_populate or the set_(pgd|pud|pmd|pte)
+ * functions as they would flush the CPU TLB.
+ */
+
+static pte_t *ipmmu_alloc_pte(struct ipmmu_vmsa_device *mmu, pmd_t *pmd,
+			      unsigned long iova)
+{
+	pte_t *pte;
+
+	if (!pmd_none(*pmd))
+		return pte_offset_kernel(pmd, iova);
+
+	pte = (pte_t *)get_zeroed_page(GFP_ATOMIC);
+	if (!pte)
+		return NULL;
+
+	ipmmu_flush_pgtable(mmu, pte, PAGE_SIZE);
+	*pmd = __pmd(__pa(pte) | PMD_NSTABLE | PMD_TYPE_TABLE);
+	ipmmu_flush_pgtable(mmu, pmd, sizeof(*pmd));
+
+	return pte + pte_index(iova);
+}
+
+static pmd_t *ipmmu_alloc_pmd(struct ipmmu_vmsa_device *mmu, pgd_t *pgd,
+			      unsigned long iova)
+{
+	pud_t *pud = (pud_t *)pgd;
+	pmd_t *pmd;
+
+	if (!pud_none(*pud))
+		return pmd_offset(pud, iova);
+
+	pmd = (pmd_t *)get_zeroed_page(GFP_ATOMIC);
+	if (!pmd)
+		return NULL;
+
+	ipmmu_flush_pgtable(mmu, pmd, PAGE_SIZE);
+	*pud = __pud(__pa(pmd) | PMD_NSTABLE | PMD_TYPE_TABLE);
+	ipmmu_flush_pgtable(mmu, pud, sizeof(*pud));
+
+	return pmd + pmd_index(iova);
+}
+
+static u64 ipmmu_page_prot(unsigned int prot, u64 type)
+{
+	u64 pgprot = ARM_VMSA_PTE_XN | ARM_VMSA_PTE_nG | ARM_VMSA_PTE_AF
+		   | ARM_VMSA_PTE_SH_IS | ARM_VMSA_PTE_AP_UNPRIV
+		   | ARM_VMSA_PTE_NS | type;
+
+	if (!(prot & IOMMU_WRITE) && (prot & IOMMU_READ))
+		pgprot |= ARM_VMSA_PTE_AP_RDONLY;
+
+	if (prot & IOMMU_CACHE)
+		pgprot |= IMMAIR_ATTR_IDX_WBRWA << ARM_VMSA_PTE_ATTRINDX_SHIFT;
+
+	if (prot & IOMMU_EXEC)
+		pgprot &= ~ARM_VMSA_PTE_XN;
+	else if (!(prot & (IOMMU_READ | IOMMU_WRITE)))
+		/* If no access create a faulting entry to avoid TLB fills. */
+		pgprot &= ~ARM_VMSA_PTE_PAGE;
+
+	return pgprot;
+}
+
+static int ipmmu_alloc_init_pte(struct ipmmu_vmsa_device *mmu, pmd_t *pmd,
+				unsigned long iova, unsigned long pfn,
+				size_t size, int prot)
+{
+	pteval_t pteval = ipmmu_page_prot(prot, ARM_VMSA_PTE_PAGE);
+	unsigned int num_ptes = 1;
+	pte_t *pte, *start;
+	unsigned int i;
+
+	pte = ipmmu_alloc_pte(mmu, pmd, iova);
+	if (!pte)
+		return -ENOMEM;
+
+	start = pte;
+
+	/*
+	 * Install the page table entries. We can be called both for a single
+	 * page or for a block of 16 physically contiguous pages. In the latter
+	 * case set the PTE contiguous hint.
+	 */
+	if (size == SZ_64K) {
+		pteval |= ARM_VMSA_PTE_CONT;
+		num_ptes = ARM_VMSA_PTE_CONT_ENTRIES;
+	}
+
+	for (i = num_ptes; i; --i)
+		*pte++ = pfn_pte(pfn++, __pgprot(pteval));
+
+	ipmmu_flush_pgtable(mmu, start, sizeof(*pte) * num_ptes);
+
+	return 0;
+}
+
+static int ipmmu_alloc_init_pmd(struct ipmmu_vmsa_device *mmu, pmd_t *pmd,
+				unsigned long iova, unsigned long pfn,
+				int prot)
+{
+	pmdval_t pmdval = ipmmu_page_prot(prot, PMD_TYPE_SECT);
+
+	*pmd = pfn_pmd(pfn, __pgprot(pmdval));
+	ipmmu_flush_pgtable(mmu, pmd, sizeof(*pmd));
+
+	return 0;
+}
+
+static int ipmmu_create_mapping(struct ipmmu_vmsa_domain *domain,
+				unsigned long iova, phys_addr_t paddr,
+				size_t size, int prot)
+{
+	struct ipmmu_vmsa_device *mmu = domain->mmu;
+	pgd_t *pgd = domain->pgd;
+	unsigned long flags;
+	unsigned long pfn;
+	pmd_t *pmd;
+	int ret;
+
+	if (!pgd)
+		return -EINVAL;
+
+	if (size & ~PAGE_MASK)
+		return -EINVAL;
+
+	if (paddr & ~((1ULL << 40) - 1))
+		return -ERANGE;
+
+	pfn = __phys_to_pfn(paddr);
+	pgd += pgd_index(iova);
+
+	/* Update the page tables. */
+	spin_lock_irqsave(&domain->lock, flags);
+
+	pmd = ipmmu_alloc_pmd(mmu, pgd, iova);
+	if (!pmd) {
+		ret = -ENOMEM;
+		goto done;
+	}
+
+	switch (size) {
+	case SZ_2M:
+		ret = ipmmu_alloc_init_pmd(mmu, pmd, iova, pfn, prot);
+		break;
+	case SZ_64K:
+	case SZ_4K:
+		ret = ipmmu_alloc_init_pte(mmu, pmd, iova, pfn, size, prot);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+done:
+	spin_unlock_irqrestore(&domain->lock, flags);
+
+	if (!ret)
+		ipmmu_tlb_invalidate(domain);
+
+	return ret;
+}
+
+static void ipmmu_clear_pud(struct ipmmu_vmsa_device *mmu, pud_t *pud)
+{
+	/* Free the page table. */
+	pgtable_t table = pud_pgtable(*pud);
+	__free_page(table);
+
+	/* Clear the PUD. */
+	*pud = __pud(0);
+	ipmmu_flush_pgtable(mmu, pud, sizeof(*pud));
+}
+
+static void ipmmu_clear_pmd(struct ipmmu_vmsa_device *mmu, pud_t *pud,
+			    pmd_t *pmd)
+{
+	unsigned int i;
+
+	/* Free the page table. */
+	if (pmd_table(*pmd)) {
+		pgtable_t table = pmd_pgtable(*pmd);
+		__free_page(table);
+	}
+
+	/* Clear the PMD. */
+	*pmd = __pmd(0);
+	ipmmu_flush_pgtable(mmu, pmd, sizeof(*pmd));
+
+	/* Check whether the PUD is still needed. */
+	pmd = pmd_offset(pud, 0);
+	for (i = 0; i < IPMMU_PTRS_PER_PMD; ++i) {
+		if (!pmd_none(pmd[i]))
+			return;
+	}
+
+	/* Clear the parent PUD. */
+	ipmmu_clear_pud(mmu, pud);
+}
+
+static void ipmmu_clear_pte(struct ipmmu_vmsa_device *mmu, pud_t *pud,
+			    pmd_t *pmd, pte_t *pte, unsigned int num_ptes)
+{
+	unsigned int i;
+
+	/* Clear the PTE. */
+	for (i = num_ptes; i; --i)
+		pte[i-1] = __pte(0);
+
+	ipmmu_flush_pgtable(mmu, pte, sizeof(*pte) * num_ptes);
+
+	/* Check whether the PMD is still needed. */
+	pte = pte_offset_kernel(pmd, 0);
+	for (i = 0; i < IPMMU_PTRS_PER_PTE; ++i) {
+		if (!pte_none(pte[i]))
+			return;
+	}
+
+	/* Clear the parent PMD. */
+	ipmmu_clear_pmd(mmu, pud, pmd);
+}
+
+static int ipmmu_split_pmd(struct ipmmu_vmsa_device *mmu, pmd_t *pmd)
+{
+	pte_t *pte, *start;
+	pteval_t pteval;
+	unsigned long pfn;
+	unsigned int i;
+
+	pte = (pte_t *)get_zeroed_page(GFP_ATOMIC);
+	if (!pte)
+		return -ENOMEM;
+
+	/* Copy the PMD attributes. */
+	pteval = (pmd_val(*pmd) & ARM_VMSA_PTE_ATTRS_MASK)
+	       | ARM_VMSA_PTE_CONT | ARM_VMSA_PTE_PAGE;
+
+	pfn = pmd_pfn(*pmd);
+	start = pte;
+
+	for (i = IPMMU_PTRS_PER_PTE; i; --i)
+		*pte++ = pfn_pte(pfn++, __pgprot(pteval));
+
+	ipmmu_flush_pgtable(mmu, start, PAGE_SIZE);
+	*pmd = __pmd(__pa(start) | PMD_NSTABLE | PMD_TYPE_TABLE);
+	ipmmu_flush_pgtable(mmu, pmd, sizeof(*pmd));
+
+	return 0;
+}
+
+static void ipmmu_split_pte(struct ipmmu_vmsa_device *mmu, pte_t *pte)
+{
+	unsigned int i;
+
+	for (i = ARM_VMSA_PTE_CONT_ENTRIES; i; --i)
+		pte[i-1] = __pte(pte_val(*pte) & ~ARM_VMSA_PTE_CONT);
+
+	ipmmu_flush_pgtable(mmu, pte, sizeof(*pte) * ARM_VMSA_PTE_CONT_ENTRIES);
+}
+
+static int ipmmu_clear_mapping(struct ipmmu_vmsa_domain *domain,
+			       unsigned long iova, size_t size)
+{
+	struct ipmmu_vmsa_device *mmu = domain->mmu;
+	unsigned long flags;
+	pgd_t *pgd = domain->pgd;
+	pud_t *pud;
+	pmd_t *pmd;
+	pte_t *pte;
+	int ret = 0;
+
+	if (!pgd)
+		return -EINVAL;
+
+	if (size & ~PAGE_MASK)
+		return -EINVAL;
+
+	pgd += pgd_index(iova);
+	pud = (pud_t *)pgd;
+
+	spin_lock_irqsave(&domain->lock, flags);
+
+	/* If there's no PUD or PMD we're done. */
+	if (pud_none(*pud))
+		goto done;
+
+	pmd = pmd_offset(pud, iova);
+	if (pmd_none(*pmd))
+		goto done;
+
+	/*
+	 * When freeing a 2MB block just clear the PMD. In the unlikely case the
+	 * block is mapped as individual pages this will free the corresponding
+	 * PTE page table.
+	 */
+	if (size == SZ_2M) {
+		ipmmu_clear_pmd(mmu, pud, pmd);
+		goto done;
+	}
+
+	/*
+	 * If the PMD has been mapped as a section remap it as pages to allow
+	 * freeing individual pages.
+	 */
+	if (pmd_sect(*pmd))
+		ipmmu_split_pmd(mmu, pmd);
+
+	pte = pte_offset_kernel(pmd, iova);
+
+	/*
+	 * When freeing a 64kB block just clear the PTE entries. We don't have
+	 * to care about the contiguous hint of the surrounding entries.
+	 */
+	if (size == SZ_64K) {
+		ipmmu_clear_pte(mmu, pud, pmd, pte, ARM_VMSA_PTE_CONT_ENTRIES);
+		goto done;
+	}
+
+	/*
+	 * If the PTE has been mapped with the contiguous hint set remap it and
+	 * its surrounding PTEs to allow unmapping a single page.
+	 */
+	if (pte_val(*pte) & ARM_VMSA_PTE_CONT)
+		ipmmu_split_pte(mmu, pte);
+
+	/* Clear the PTE. */
+	ipmmu_clear_pte(mmu, pud, pmd, pte, 1);
+
+done:
+	spin_unlock_irqrestore(&domain->lock, flags);
+
+	if (ret)
+		ipmmu_tlb_invalidate(domain);
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * IOMMU Operations
+ */
+
+static int ipmmu_domain_init(struct iommu_domain *io_domain)
+{
+	struct ipmmu_vmsa_domain *domain;
+
+	domain = kzalloc(sizeof(*domain), GFP_KERNEL);
+	if (!domain)
+		return -ENOMEM;
+
+	spin_lock_init(&domain->lock);
+
+	domain->pgd = kzalloc(IPMMU_PTRS_PER_PGD * sizeof(pgd_t), GFP_KERNEL);
+	if (!domain->pgd) {
+		kfree(domain);
+		return -ENOMEM;
+	}
+
+	io_domain->priv = domain;
+	domain->io_domain = io_domain;
+
+	return 0;
+}
+
+static void ipmmu_domain_destroy(struct iommu_domain *io_domain)
+{
+	struct ipmmu_vmsa_domain *domain = io_domain->priv;
+
+	/*
+	 * Free the domain resources. We assume that all devices have already
+	 * been detached.
+	 */
+	ipmmu_domain_destroy_context(domain);
+	ipmmu_free_pgtables(domain);
+	kfree(domain);
+}
+
+static int ipmmu_attach_device(struct iommu_domain *io_domain,
+			       struct device *dev)
+{
+	struct ipmmu_vmsa_archdata *archdata = dev->archdata.iommu;
+	struct ipmmu_vmsa_device *mmu = archdata->mmu;
+	struct ipmmu_vmsa_domain *domain = io_domain->priv;
+	unsigned long flags;
+	int ret = 0;
+
+	if (!mmu) {
+		dev_err(dev, "Cannot attach to IPMMU\n");
+		return -ENXIO;
+	}
+
+	spin_lock_irqsave(&domain->lock, flags);
+
+	if (!domain->mmu) {
+		/* The domain hasn't been used yet, initialize it. */
+		domain->mmu = mmu;
+		ret = ipmmu_domain_init_context(domain);
+	} else if (domain->mmu != mmu) {
+		/*
+		 * Something is wrong, we can't attach two devices using
+		 * different IOMMUs to the same domain.
+		 */
+		dev_err(dev, "Can't attach IPMMU %s to domain on IPMMU %s\n",
+			dev_name(mmu->dev), dev_name(domain->mmu->dev));
+		ret = -EINVAL;
+	}
+
+	spin_unlock_irqrestore(&domain->lock, flags);
+
+	if (ret < 0)
+		return ret;
+
+	ipmmu_utlb_enable(domain, archdata->utlb);
+
+	return 0;
+}
+
+static void ipmmu_detach_device(struct iommu_domain *io_domain,
+				struct device *dev)
+{
+	struct ipmmu_vmsa_archdata *archdata = dev->archdata.iommu;
+	struct ipmmu_vmsa_domain *domain = io_domain->priv;
+
+	ipmmu_utlb_disable(domain, archdata->utlb);
+
+	/*
+	 * TODO: Optimize by disabling the context when no device is attached.
+	 */
+}
+
+static int ipmmu_map(struct iommu_domain *io_domain, unsigned long iova,
+		     phys_addr_t paddr, size_t size, int prot)
+{
+	struct ipmmu_vmsa_domain *domain = io_domain->priv;
+
+	if (!domain)
+		return -ENODEV;
+
+	return ipmmu_create_mapping(domain, iova, paddr, size, prot);
+}
+
+static size_t ipmmu_unmap(struct iommu_domain *io_domain, unsigned long iova,
+			  size_t size)
+{
+	struct ipmmu_vmsa_domain *domain = io_domain->priv;
+	int ret;
+
+	ret = ipmmu_clear_mapping(domain, iova, size);
+	return ret ? 0 : size;
+}
+
+static phys_addr_t ipmmu_iova_to_phys(struct iommu_domain *io_domain,
+				      dma_addr_t iova)
+{
+	struct ipmmu_vmsa_domain *domain = io_domain->priv;
+	pgd_t pgd;
+	pud_t pud;
+	pmd_t pmd;
+	pte_t pte;
+
+	/* TODO: Is locking needed ? */
+
+	if (!domain->pgd)
+		return 0;
+
+	pgd = *(domain->pgd + pgd_index(iova));
+	if (pgd_none(pgd))
+		return 0;
+
+	pud = *pud_offset(&pgd, iova);
+	if (pud_none(pud))
+		return 0;
+
+	pmd = *pmd_offset(&pud, iova);
+	if (pmd_none(pmd))
+		return 0;
+
+	if (pmd_sect(pmd))
+		return __pfn_to_phys(pmd_pfn(pmd)) | (iova & ~PMD_MASK);
+
+	pte = *(pmd_page_vaddr(pmd) + pte_index(iova));
+	if (pte_none(pte))
+		return 0;
+
+	return __pfn_to_phys(pte_pfn(pte)) | (iova & ~PAGE_MASK);
+}
+
+static int ipmmu_find_utlb(struct ipmmu_vmsa_device *mmu, struct device *dev)
+{
+	const struct ipmmu_vmsa_master *master = mmu->pdata->masters;
+	const char *devname = dev_name(dev);
+	unsigned int i;
+
+	for (i = 0; i < mmu->pdata->num_masters; ++i, ++master) {
+		if (strcmp(master->name, devname) == 0)
+			return master->utlb;
+	}
+
+	return -1;
+}
+
+static int ipmmu_add_device(struct device *dev)
+{
+	struct ipmmu_vmsa_archdata *archdata;
+	struct ipmmu_vmsa_device *mmu;
+	struct iommu_group *group;
+	int utlb = -1;
+	int ret;
+
+	if (dev->archdata.iommu) {
+		dev_warn(dev, "IOMMU driver already assigned to device %s\n",
+			 dev_name(dev));
+		return -EINVAL;
+	}
+
+	/* Find the master corresponding to the device. */
+	spin_lock(&ipmmu_devices_lock);
+
+	list_for_each_entry(mmu, &ipmmu_devices, list) {
+		utlb = ipmmu_find_utlb(mmu, dev);
+		if (utlb >= 0) {
+			/*
+			 * TODO Take a reference to the MMU to protect
+			 * against device removal.
+			 */
+			break;
+		}
+	}
+
+	spin_unlock(&ipmmu_devices_lock);
+
+	if (utlb < 0)
+		return -ENODEV;
+
+	if (utlb >= mmu->num_utlbs)
+		return -EINVAL;
+
+	/* Create a device group and add the device to it. */
+	group = iommu_group_alloc();
+	if (IS_ERR(group)) {
+		dev_err(dev, "Failed to allocate IOMMU group\n");
+		return PTR_ERR(group);
+	}
+
+	ret = iommu_group_add_device(group, dev);
+	iommu_group_put(group);
+
+	if (ret < 0) {
+		dev_err(dev, "Failed to add device to IPMMU group\n");
+		return ret;
+	}
+
+	archdata = kzalloc(sizeof(*archdata), GFP_KERNEL);
+	if (!archdata) {
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	archdata->mmu = mmu;
+	archdata->utlb = utlb;
+	dev->archdata.iommu = archdata;
+
+	/*
+	 * Create the ARM mapping, used by the ARM DMA mapping core to allocate
+	 * VAs. This will allocate a corresponding IOMMU domain.
+	 *
+	 * TODO:
+	 * - Create one mapping per context (TLB).
+	 * - Make the mapping size configurable ? We currently use a 2GB mapping
+	 *   at a 1GB offset to ensure that NULL VAs will fault.
+	 */
+	if (!mmu->mapping) {
+		struct dma_iommu_mapping *mapping;
+
+		mapping = arm_iommu_create_mapping(&platform_bus_type,
+						   SZ_1G, SZ_2G);
+		if (IS_ERR(mapping)) {
+			dev_err(mmu->dev, "failed to create ARM IOMMU mapping\n");
+			return PTR_ERR(mapping);
+		}
+
+		mmu->mapping = mapping;
+	}
+
+	/* Attach the ARM VA mapping to the device. */
+	ret = arm_iommu_attach_device(dev, mmu->mapping);
+	if (ret < 0) {
+		dev_err(dev, "Failed to attach device to VA mapping\n");
+		goto error;
+	}
+
+	return 0;
+
+error:
+	kfree(dev->archdata.iommu);
+	dev->archdata.iommu = NULL;
+	iommu_group_remove_device(dev);
+	return ret;
+}
+
+static void ipmmu_remove_device(struct device *dev)
+{
+	arm_iommu_detach_device(dev);
+	iommu_group_remove_device(dev);
+	kfree(dev->archdata.iommu);
+	dev->archdata.iommu = NULL;
+}
+
+static struct iommu_ops ipmmu_ops = {
+	.domain_init = ipmmu_domain_init,
+	.domain_destroy = ipmmu_domain_destroy,
+	.attach_dev = ipmmu_attach_device,
+	.detach_dev = ipmmu_detach_device,
+	.map = ipmmu_map,
+	.unmap = ipmmu_unmap,
+	.iova_to_phys = ipmmu_iova_to_phys,
+	.add_device = ipmmu_add_device,
+	.remove_device = ipmmu_remove_device,
+	.pgsize_bitmap = SZ_2M | SZ_64K | SZ_4K,
+};
+
+/* -----------------------------------------------------------------------------
+ * Probe/remove and init
+ */
+
+static void ipmmu_device_reset(struct ipmmu_vmsa_device *mmu)
+{
+	unsigned int i;
+
+	/* Disable all contexts. */
+	for (i = 0; i < 4; ++i)
+		ipmmu_write(mmu, i * IM_CTX_SIZE + IMCTR, 0);
+}
+
+static int ipmmu_probe(struct platform_device *pdev)
+{
+	struct ipmmu_vmsa_device *mmu;
+	struct resource *res;
+	int irq;
+	int ret;
+
+	if (!pdev->dev.platform_data) {
+		dev_err(&pdev->dev, "missing platform data\n");
+		return -EINVAL;
+	}
+
+	mmu = devm_kzalloc(&pdev->dev, sizeof(*mmu), GFP_KERNEL);
+	if (!mmu) {
+		dev_err(&pdev->dev, "cannot allocate device data\n");
+		return -ENOMEM;
+	}
+
+	mmu->dev = &pdev->dev;
+	mmu->pdata = pdev->dev.platform_data;
+	mmu->num_utlbs = 32;
+
+	/* Map I/O memory and request IRQ. */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	mmu->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(mmu->base))
+		return PTR_ERR(mmu->base);
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "no IRQ found\n");
+		return irq;
+	}
+
+	ret = devm_request_irq(&pdev->dev, irq, ipmmu_irq, 0,
+			       dev_name(&pdev->dev), mmu);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to request IRQ %d\n", irq);
+		return irq;
+	}
+
+	ipmmu_device_reset(mmu);
+
+	/*
+	 * We can't create the ARM mapping here as it requires the bus to have
+	 * an IOMMU, which only happens when bus_set_iommu() is called in
+	 * ipmmu_init() after the probe function returns.
+	 */
+
+	spin_lock(&ipmmu_devices_lock);
+	list_add(&mmu->list, &ipmmu_devices);
+	spin_unlock(&ipmmu_devices_lock);
+
+	platform_set_drvdata(pdev, mmu);
+
+	return 0;
+}
+
+static int ipmmu_remove(struct platform_device *pdev)
+{
+	struct ipmmu_vmsa_device *mmu = platform_get_drvdata(pdev);
+
+	spin_lock(&ipmmu_devices_lock);
+	list_del(&mmu->list);
+	spin_unlock(&ipmmu_devices_lock);
+
+	arm_iommu_release_mapping(mmu->mapping);
+
+	ipmmu_device_reset(mmu);
+
+	return 0;
+}
+
+static struct platform_driver ipmmu_driver = {
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = "ipmmu-vmsa",
+	},
+	.probe = ipmmu_probe,
+	.remove	= ipmmu_remove,
+};
+
+static int __init ipmmu_init(void)
+{
+	int ret;
+
+	ret = platform_driver_register(&ipmmu_driver);
+	if (ret < 0)
+		return ret;
+
+	if (!iommu_present(&platform_bus_type))
+		bus_set_iommu(&platform_bus_type, &ipmmu_ops);
+
+	return 0;
+}
+
+static void __exit ipmmu_exit(void)
+{
+	return platform_driver_unregister(&ipmmu_driver);
+}
+
+subsys_initcall(ipmmu_init);
+module_exit(ipmmu_exit);
+
+MODULE_DESCRIPTION("IOMMU API for Renesas VMSA-compatible IPMMU");
+MODULE_AUTHOR("Laurent Pinchart <laurent.pinchart@ideasonboard.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iommu/irq_remapping.c b/drivers/iommu/irq_remapping.c
index 228632c99ad..33c43952408 100644
--- a/drivers/iommu/irq_remapping.c
+++ b/drivers/iommu/irq_remapping.c
@@ -51,7 +51,7 @@ static void irq_remapping_disable_io_apic(void)
 
 static int do_setup_msi_irqs(struct pci_dev *dev, int nvec)
 {
-	int node, ret, sub_handle, nvec_pow2, index = 0;
+	int ret, sub_handle, nvec_pow2, index = 0;
 	unsigned int irq;
 	struct msi_desc *msidesc;
 
@@ -61,8 +61,7 @@ static int do_setup_msi_irqs(struct pci_dev *dev, int nvec)
 	WARN_ON(msidesc->msi_attrib.multiple);
 	WARN_ON(msidesc->nvec_used);
 
-	node = dev_to_node(&dev->dev);
-	irq = __create_irqs(get_nr_irqs_gsi(), nvec, node);
+	irq = irq_alloc_hwirqs(nvec, dev_to_node(&dev->dev));
 	if (irq == 0)
 		return -ENOSPC;
 
@@ -89,7 +88,7 @@ static int do_setup_msi_irqs(struct pci_dev *dev, int nvec)
 	return 0;
 
 error:
-	destroy_irqs(irq, nvec);
+	irq_free_hwirqs(irq, nvec);
 
 	/*
 	 * Restore altered MSI descriptor fields and prevent just destroyed
@@ -109,12 +108,11 @@ static int do_setup_msix_irqs(struct pci_dev *dev, int nvec)
 	unsigned int irq;
 
 	node		= dev_to_node(&dev->dev);
-	irq		= get_nr_irqs_gsi();
 	sub_handle	= 0;
 
 	list_for_each_entry(msidesc, &dev->msi_list, list) {
 
-		irq = create_irq_nr(irq, node);
+		irq = irq_alloc_hwirq(node);
 		if (irq == 0)
 			return -1;
 
@@ -137,7 +135,7 @@ static int do_setup_msix_irqs(struct pci_dev *dev, int nvec)
 	return 0;
 
 error:
-	destroy_irq(irq);
+	irq_free_hwirq(irq);
 	return ret;
 }
 
diff --git a/drivers/iommu/msm_iommu_dev.c b/drivers/iommu/msm_iommu_dev.c
index 08ba4972da9..61def7cb526 100644
--- a/drivers/iommu/msm_iommu_dev.c
+++ b/drivers/iommu/msm_iommu_dev.c
@@ -127,13 +127,12 @@ static void msm_iommu_reset(void __iomem *base, int ncb)
 
 static int msm_iommu_probe(struct platform_device *pdev)
 {
-	struct resource *r, *r2;
+	struct resource *r;
 	struct clk *iommu_clk;
 	struct clk *iommu_pclk;
 	struct msm_iommu_drvdata *drvdata;
 	struct msm_iommu_dev *iommu_dev = pdev->dev.platform_data;
 	void __iomem *regs_base;
-	resource_size_t	len;
 	int ret, irq, par;
 
 	if (pdev->id == -1) {
@@ -178,35 +177,16 @@ static int msm_iommu_probe(struct platform_device *pdev)
 		iommu_clk = NULL;
 
 	r = platform_get_resource_byname(pdev, IORESOURCE_MEM, "physbase");
-
-	if (!r) {
-		ret = -ENODEV;
-		goto fail_clk;
-	}
-
-	len = resource_size(r);
-
-	r2 = request_mem_region(r->start, len, r->name);
-	if (!r2) {
-		pr_err("Could not request memory region: start=%p, len=%d\n",
-							(void *) r->start, len);
-		ret = -EBUSY;
+	regs_base = devm_ioremap_resource(&pdev->dev, r);
+	if (IS_ERR(regs_base)) {
+		ret = PTR_ERR(regs_base);
 		goto fail_clk;
 	}
 
-	regs_base = ioremap(r2->start, len);
-
-	if (!regs_base) {
-		pr_err("Could not ioremap: start=%p, len=%d\n",
-			 (void *) r2->start, len);
-		ret = -EBUSY;
-		goto fail_mem;
-	}
-
 	irq = platform_get_irq_byname(pdev, "secure_irq");
 	if (irq < 0) {
 		ret = -ENODEV;
-		goto fail_io;
+		goto fail_clk;
 	}
 
 	msm_iommu_reset(regs_base, iommu_dev->ncb);
@@ -222,14 +202,14 @@ static int msm_iommu_probe(struct platform_device *pdev)
 	if (!par) {
 		pr_err("%s: Invalid PAR value detected\n", iommu_dev->name);
 		ret = -ENODEV;
-		goto fail_io;
+		goto fail_clk;
 	}
 
 	ret = request_irq(irq, msm_iommu_fault_handler, 0,
 			"msm_iommu_secure_irpt_handler", drvdata);
 	if (ret) {
 		pr_err("Request IRQ %d failed with ret=%d\n", irq, ret);
-		goto fail_io;
+		goto fail_clk;
 	}
 
 
@@ -250,10 +230,6 @@ static int msm_iommu_probe(struct platform_device *pdev)
 	clk_disable(iommu_pclk);
 
 	return 0;
-fail_io:
-	iounmap(regs_base);
-fail_mem:
-	release_mem_region(r->start, len);
 fail_clk:
 	if (iommu_clk) {
 		clk_disable(iommu_clk);
diff --git a/drivers/iommu/omap-iommu.c b/drivers/iommu/omap-iommu.c
index 7fcbfc498fa..895af06a667 100644
--- a/drivers/iommu/omap-iommu.c
+++ b/drivers/iommu/omap-iommu.c
@@ -34,6 +34,9 @@
 #include "omap-iopgtable.h"
 #include "omap-iommu.h"
 
+#define to_iommu(dev)							\
+	((struct omap_iommu *)platform_get_drvdata(to_platform_device(dev)))
+
 #define for_each_iotlb_cr(obj, n, __i, cr)				\
 	for (__i = 0;							\
 	     (__i < (n)) && (cr = __iotlb_read_cr((obj), __i), true);	\
@@ -391,6 +394,7 @@ static void flush_iotlb_page(struct omap_iommu *obj, u32 da)
 				__func__, start, da, bytes);
 			iotlb_load_cr(obj, &cr);
 			iommu_write_reg(obj, 1, MMU_FLUSH_ENTRY);
+			break;
 		}
 	}
 	pm_runtime_put_sync(obj->dev);
@@ -1037,19 +1041,18 @@ static void iopte_cachep_ctor(void *iopte)
 	clean_dcache_area(iopte, IOPTE_TABLE_SIZE);
 }
 
-static u32 iotlb_init_entry(struct iotlb_entry *e, u32 da, u32 pa,
-				   u32 flags)
+static u32 iotlb_init_entry(struct iotlb_entry *e, u32 da, u32 pa, int pgsz)
 {
 	memset(e, 0, sizeof(*e));
 
 	e->da		= da;
 	e->pa		= pa;
-	e->valid	= 1;
+	e->valid	= MMU_CAM_V;
 	/* FIXME: add OMAP1 support */
-	e->pgsz		= flags & MMU_CAM_PGSZ_MASK;
-	e->endian	= flags & MMU_RAM_ENDIAN_MASK;
-	e->elsz		= flags & MMU_RAM_ELSZ_MASK;
-	e->mixed	= flags & MMU_RAM_MIXED_MASK;
+	e->pgsz		= pgsz;
+	e->endian	= MMU_RAM_ENDIAN_LITTLE;
+	e->elsz		= MMU_RAM_ELSZ_8;
+	e->mixed	= 0;
 
 	return iopgsz_to_bytes(e->pgsz);
 }
@@ -1062,9 +1065,8 @@ static int omap_iommu_map(struct iommu_domain *domain, unsigned long da,
 	struct device *dev = oiommu->dev;
 	struct iotlb_entry e;
 	int omap_pgsz;
-	u32 ret, flags;
+	u32 ret;
 
-	/* we only support mapping a single iommu page for now */
 	omap_pgsz = bytes_to_iopgsz(bytes);
 	if (omap_pgsz < 0) {
 		dev_err(dev, "invalid size to map: %d\n", bytes);
@@ -1073,9 +1075,7 @@ static int omap_iommu_map(struct iommu_domain *domain, unsigned long da,
 
 	dev_dbg(dev, "mapping da 0x%lx to pa 0x%x size 0x%x\n", da, pa, bytes);
 
-	flags = omap_pgsz | prot;
-
-	iotlb_init_entry(&e, da, pa, flags);
+	iotlb_init_entry(&e, da, pa, omap_pgsz);
 
 	ret = omap_iopgtable_store_entry(oiommu, &e);
 	if (ret)
@@ -1248,12 +1248,6 @@ static phys_addr_t omap_iommu_iova_to_phys(struct iommu_domain *domain,
 	return ret;
 }
 
-static int omap_iommu_domain_has_cap(struct iommu_domain *domain,
-				    unsigned long cap)
-{
-	return 0;
-}
-
 static int omap_iommu_add_device(struct device *dev)
 {
 	struct omap_iommu_arch_data *arch_data;
@@ -1305,7 +1299,6 @@ static struct iommu_ops omap_iommu_ops = {
 	.map		= omap_iommu_map,
 	.unmap		= omap_iommu_unmap,
 	.iova_to_phys	= omap_iommu_iova_to_phys,
-	.domain_has_cap	= omap_iommu_domain_has_cap,
 	.add_device	= omap_iommu_add_device,
 	.remove_device	= omap_iommu_remove_device,
 	.pgsize_bitmap	= OMAP_IOMMU_PGSIZES,
diff --git a/drivers/iommu/omap-iopgtable.h b/drivers/iommu/omap-iopgtable.h
index b6f9a51746c..f891683e3f0 100644
--- a/drivers/iommu/omap-iopgtable.h
+++ b/drivers/iommu/omap-iopgtable.h
@@ -93,6 +93,3 @@ static inline phys_addr_t omap_iommu_translate(u32 d, u32 va, u32 mask)
 /* to find an entry in the second-level page table. */
 #define iopte_index(da)		(((da) >> IOPTE_SHIFT) & (PTRS_PER_IOPTE - 1))
 #define iopte_offset(iopgd, da)	(iopgd_page_vaddr(iopgd) + iopte_index(da))
-
-#define to_iommu(dev)							\
-	(platform_get_drvdata(to_platform_device(dev)))
diff --git a/drivers/iommu/shmobile-ipmmu.c b/drivers/iommu/shmobile-ipmmu.c
index e3bc2e19b6d..bd97adecb1f 100644
--- a/drivers/iommu/shmobile-ipmmu.c
+++ b/drivers/iommu/shmobile-ipmmu.c
@@ -94,11 +94,6 @@ static int ipmmu_probe(struct platform_device *pdev)
 	struct resource *res;
 	struct shmobile_ipmmu_platform_data *pdata = pdev->dev.platform_data;
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "cannot get platform resources\n");
-		return -ENOENT;
-	}
 	ipmmu = devm_kzalloc(&pdev->dev, sizeof(*ipmmu), GFP_KERNEL);
 	if (!ipmmu) {
 		dev_err(&pdev->dev, "cannot allocate device data\n");
@@ -106,19 +101,18 @@ static int ipmmu_probe(struct platform_device *pdev)
 	}
 	spin_lock_init(&ipmmu->flush_lock);
 	ipmmu->dev = &pdev->dev;
-	ipmmu->ipmmu_base = devm_ioremap_nocache(&pdev->dev, res->start,
-						resource_size(res));
-	if (!ipmmu->ipmmu_base) {
-		dev_err(&pdev->dev, "ioremap_nocache failed\n");
-		return -ENOMEM;
-	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	ipmmu->ipmmu_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(ipmmu->ipmmu_base))
+		return PTR_ERR(ipmmu->ipmmu_base);
+
 	ipmmu->dev_names = pdata->dev_names;
 	ipmmu->num_dev_names = pdata->num_dev_names;
 	platform_set_drvdata(pdev, ipmmu);
 	ipmmu_reg_write(ipmmu, IMCTR1, 0x0); /* disable TLB */
 	ipmmu_reg_write(ipmmu, IMCTR2, 0x0); /* disable PMB */
-	ipmmu_iommu_init(ipmmu);
-	return 0;
+	return ipmmu_iommu_init(ipmmu);
 }
 
 static struct platform_driver ipmmu_driver = {
diff --git a/drivers/irqchip/Kconfig b/drivers/irqchip/Kconfig
index d770f740663..bbb746e3550 100644
--- a/drivers/irqchip/Kconfig
+++ b/drivers/irqchip/Kconfig
@@ -30,6 +30,12 @@ config ARM_VIC_NR
 	  The maximum number of VICs available in the system, for
 	  power management.
 
+config BRCMSTB_L2_IRQ
+	bool
+	depends on ARM
+	select GENERIC_IRQ_CHIP
+	select IRQ_DOMAIN
+
 config DW_APB_ICTL
 	bool
 	select IRQ_DOMAIN
diff --git a/drivers/irqchip/Makefile b/drivers/irqchip/Makefile
index f180f8d5fb7..62a13e5ef98 100644
--- a/drivers/irqchip/Makefile
+++ b/drivers/irqchip/Makefile
@@ -29,3 +29,4 @@ 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
diff --git a/drivers/irqchip/irq-armada-370-xp.c b/drivers/irqchip/irq-armada-370-xp.c
index 3899ba7821c..c887e6eebc4 100644
--- a/drivers/irqchip/irq-armada-370-xp.c
+++ b/drivers/irqchip/irq-armada-370-xp.c
@@ -19,6 +19,7 @@
 #include <linux/irq.h>
 #include <linux/interrupt.h>
 #include <linux/irqchip/chained_irq.h>
+#include <linux/cpu.h>
 #include <linux/io.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
@@ -310,7 +311,8 @@ static int armada_370_xp_mpic_irq_map(struct irq_domain *h,
 }
 
 #ifdef CONFIG_SMP
-void armada_mpic_send_doorbell(const struct cpumask *mask, unsigned int irq)
+static void armada_mpic_send_doorbell(const struct cpumask *mask,
+				      unsigned int irq)
 {
 	int cpu;
 	unsigned long map = 0;
@@ -330,7 +332,7 @@ void armada_mpic_send_doorbell(const struct cpumask *mask, unsigned int irq)
 		ARMADA_370_XP_SW_TRIG_INT_OFFS);
 }
 
-void armada_xp_mpic_smp_cpu_init(void)
+static void armada_xp_mpic_smp_cpu_init(void)
 {
 	/* Clear pending IPIs */
 	writel(0, per_cpu_int_base + ARMADA_370_XP_IN_DRBEL_CAUSE_OFFS);
@@ -342,6 +344,20 @@ void armada_xp_mpic_smp_cpu_init(void)
 	/* Unmask IPI interrupt */
 	writel(0, per_cpu_int_base + ARMADA_370_XP_INT_CLEAR_MASK_OFFS);
 }
+
+static int armada_xp_mpic_secondary_init(struct notifier_block *nfb,
+					 unsigned long action, void *hcpu)
+{
+	if (action == CPU_STARTING || action == CPU_STARTING_FROZEN)
+		armada_xp_mpic_smp_cpu_init();
+	return NOTIFY_OK;
+}
+
+static struct notifier_block armada_370_xp_mpic_cpu_notifier = {
+	.notifier_call = armada_xp_mpic_secondary_init,
+	.priority = 100,
+};
+
 #endif /* CONFIG_SMP */
 
 static struct irq_domain_ops armada_370_xp_mpic_irq_ops = {
@@ -497,6 +513,10 @@ static int __init armada_370_xp_mpic_of_init(struct device_node *node,
 	if (parent_irq <= 0) {
 		irq_set_default_host(armada_370_xp_mpic_domain);
 		set_handle_irq(armada_370_xp_handle_irq);
+#ifdef CONFIG_SMP
+		set_smp_cross_call(armada_mpic_send_doorbell);
+		register_cpu_notifier(&armada_370_xp_mpic_cpu_notifier);
+#endif
 	} else {
 		irq_set_chained_handler(parent_irq,
 					armada_370_xp_mpic_handle_cascade_irq);
diff --git a/drivers/irqchip/irq-brcmstb-l2.c b/drivers/irqchip/irq-brcmstb-l2.c
new file mode 100644
index 00000000000..8ee2a36d584
--- /dev/null
+++ b/drivers/irqchip/irq-brcmstb-l2.c
@@ -0,0 +1,202 @@
+/*
+ * Generic Broadcom Set Top Box 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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)	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/irqchip.h>
+#include <linux/irqchip/chained_irq.h>
+
+#include <asm/mach/irq.h>
+
+#include "irqchip.h"
+
+/* Register offsets in the L2 interrupt controller */
+#define CPU_STATUS	0x00
+#define CPU_SET		0x04
+#define CPU_CLEAR	0x08
+#define CPU_MASK_STATUS	0x0c
+#define CPU_MASK_SET	0x10
+#define CPU_MASK_CLEAR	0x14
+
+/* L2 intc private data structure */
+struct brcmstb_l2_intc_data {
+	int parent_irq;
+	void __iomem *base;
+	struct irq_domain *domain;
+	bool can_wake;
+	u32 saved_mask; /* for suspend/resume */
+};
+
+static void brcmstb_l2_intc_irq_handle(unsigned int irq, struct irq_desc *desc)
+{
+	struct brcmstb_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 + CPU_STATUS) &
+		~(__raw_readl(b->base + CPU_MASK_STATUS));
+
+	if (status == 0) {
+		do_bad_IRQ(irq, desc);
+		goto out;
+	}
+
+	do {
+		irq = ffs(status) - 1;
+		/* ack at our level */
+		__raw_writel(1 << irq, b->base + CPU_CLEAR);
+		status &= ~(1 << irq);
+		generic_handle_irq(irq_find_mapping(b->domain, irq));
+	} while (status);
+out:
+	chained_irq_exit(chip, desc);
+}
+
+static void brcmstb_l2_intc_suspend(struct irq_data *d)
+{
+	struct irq_chip_generic *gc = irq_data_get_irq_chip_data(d);
+	struct brcmstb_l2_intc_data *b = gc->private;
+
+	irq_gc_lock(gc);
+	/* Save the current mask */
+	b->saved_mask = __raw_readl(b->base + CPU_MASK_STATUS);
+
+	if (b->can_wake) {
+		/* Program the wakeup mask */
+		__raw_writel(~gc->wake_active, b->base + CPU_MASK_SET);
+		__raw_writel(gc->wake_active, b->base + CPU_MASK_CLEAR);
+	}
+	irq_gc_unlock(gc);
+}
+
+static void brcmstb_l2_intc_resume(struct irq_data *d)
+{
+	struct irq_chip_generic *gc = irq_data_get_irq_chip_data(d);
+	struct brcmstb_l2_intc_data *b = gc->private;
+
+	irq_gc_lock(gc);
+	/* Clear unmasked non-wakeup interrupts */
+	__raw_writel(~b->saved_mask & ~gc->wake_active, b->base + CPU_CLEAR);
+
+	/* Restore the saved mask */
+	__raw_writel(b->saved_mask, b->base + CPU_MASK_SET);
+	__raw_writel(~b->saved_mask, b->base + CPU_MASK_CLEAR);
+	irq_gc_unlock(gc);
+}
+
+int __init brcmstb_l2_intc_of_init(struct device_node *np,
+					struct device_node *parent)
+{
+	unsigned int clr = IRQ_NOREQUEST | IRQ_NOPROBE | IRQ_NOAUTOEN;
+	struct brcmstb_l2_intc_data *data;
+	struct irq_chip_generic *gc;
+	struct irq_chip_type *ct;
+	int ret;
+
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->base = of_iomap(np, 0);
+	if (!data->base) {
+		pr_err("failed to remap intc L2 registers\n");
+		ret = -ENOMEM;
+		goto out_free;
+	}
+
+	/* Disable all interrupts by default */
+	__raw_writel(0xffffffff, data->base + CPU_MASK_SET);
+	__raw_writel(0xffffffff, data->base + CPU_CLEAR);
+
+	data->parent_irq = irq_of_parse_and_map(np, 0);
+	if (data->parent_irq < 0) {
+		pr_err("failed to find parent interrupt\n");
+		ret = data->parent_irq;
+		goto out_unmap;
+	}
+
+	data->domain = irq_domain_add_linear(np, 32,
+				&irq_generic_chip_ops, NULL);
+	if (!data->domain) {
+		ret = -ENOMEM;
+		goto out_unmap;
+	}
+
+	/* Allocate a single Generic IRQ chip for this node */
+	ret = irq_alloc_domain_generic_chips(data->domain, 32, 1,
+				np->full_name, handle_level_irq, clr, 0, 0);
+	if (ret) {
+		pr_err("failed to allocate generic irq chip\n");
+		goto out_free_domain;
+	}
+
+	/* Set the IRQ chaining logic */
+	irq_set_handler_data(data->parent_irq, data);
+	irq_set_chained_handler(data->parent_irq, brcmstb_l2_intc_irq_handle);
+
+	gc = irq_get_domain_generic_chip(data->domain, 0);
+	gc->reg_base = data->base;
+	gc->private = data;
+	ct = gc->chip_types;
+
+	ct->chip.irq_ack = irq_gc_ack_set_bit;
+	ct->regs.ack = CPU_CLEAR;
+
+	ct->chip.irq_mask = irq_gc_mask_disable_reg;
+	ct->regs.disable = CPU_MASK_SET;
+
+	ct->chip.irq_unmask = irq_gc_unmask_enable_reg;
+	ct->regs.enable = CPU_MASK_CLEAR;
+
+	ct->chip.irq_suspend = brcmstb_l2_intc_suspend;
+	ct->chip.irq_resume = brcmstb_l2_intc_resume;
+
+	if (of_property_read_bool(np, "brcm,irq-can-wake")) {
+		data->can_wake = true;
+		/* This IRQ chip can wake the system, set all child interrupts
+		 * in wake_enabled mask
+		 */
+		gc->wake_enabled = 0xffffffff;
+		ct->chip.irq_set_wake = irq_gc_set_wake;
+	}
+
+	pr_info("registered L2 intc (mem: 0x%p, parent irq: %d)\n",
+			data->base, data->parent_irq);
+
+	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,l2-intc", brcmstb_l2_intc_of_init);
diff --git a/drivers/irqchip/irq-gic.c b/drivers/irqchip/irq-gic.c
index 57d165e026f..7e11c9d6ae8 100644
--- a/drivers/irqchip/irq-gic.c
+++ b/drivers/irqchip/irq-gic.c
@@ -291,7 +291,7 @@ static void __exception_irq_entry gic_handle_irq(struct pt_regs *regs)
 
 	do {
 		irqstat = readl_relaxed(cpu_base + GIC_CPU_INTACK);
-		irqnr = irqstat & ~0x1c00;
+		irqnr = irqstat & GICC_IAR_INT_ID_MASK;
 
 		if (likely(irqnr > 15 && irqnr < 1021)) {
 			irqnr = irq_find_mapping(gic->domain, irqnr);
diff --git a/drivers/irqchip/irq-mxs.c b/drivers/irqchip/irq-mxs.c
index 63b3d4eb0ef..4044ff28766 100644
--- a/drivers/irqchip/irq-mxs.c
+++ b/drivers/irqchip/irq-mxs.c
@@ -96,7 +96,7 @@ static struct irq_domain_ops icoll_irq_domain_ops = {
 	.xlate = irq_domain_xlate_onecell,
 };
 
-static void __init icoll_of_init(struct device_node *np,
+static int __init icoll_of_init(struct device_node *np,
 			  struct device_node *interrupt_parent)
 {
 	icoll_base = of_iomap(np, 0);
@@ -110,6 +110,6 @@ static void __init icoll_of_init(struct device_node *np,
 
 	icoll_domain = irq_domain_add_linear(np, ICOLL_NUM_IRQS,
 					     &icoll_irq_domain_ops, NULL);
-	WARN_ON(!icoll_domain);
+	return icoll_domain ? 0 : -ENODEV;
 }
 IRQCHIP_DECLARE(mxs, "fsl,icoll", icoll_of_init);
diff --git a/drivers/irqchip/irq-orion.c b/drivers/irqchip/irq-orion.c
index e25f246cd2f..34d18b48bb7 100644
--- a/drivers/irqchip/irq-orion.c
+++ b/drivers/irqchip/irq-orion.c
@@ -42,7 +42,7 @@ __exception_irq_entry orion_handle_irq(struct pt_regs *regs)
 		u32 stat = readl_relaxed(gc->reg_base + ORION_IRQ_CAUSE) &
 			gc->mask_cache;
 		while (stat) {
-			u32 hwirq = ffs(stat) - 1;
+			u32 hwirq = __fls(stat);
 			u32 irq = irq_find_mapping(orion_irq_domain,
 						   gc->irq_base + hwirq);
 			handle_IRQ(irq, regs);
@@ -117,7 +117,7 @@ static void orion_bridge_irq_handler(unsigned int irq, struct irq_desc *desc)
 		   gc->mask_cache;
 
 	while (stat) {
-		u32 hwirq = ffs(stat) - 1;
+		u32 hwirq = __fls(stat);
 
 		generic_handle_irq(irq_find_mapping(d, gc->irq_base + hwirq));
 		stat &= ~(1 << hwirq);
diff --git a/drivers/irqchip/irq-s3c24xx.c b/drivers/irqchip/irq-s3c24xx.c
index bbcc944ed94..78a6accd205 100644
--- a/drivers/irqchip/irq-s3c24xx.c
+++ b/drivers/irqchip/irq-s3c24xx.c
@@ -1323,8 +1323,7 @@ static struct s3c24xx_irq_of_ctrl s3c2410_ctrl[] = {
 };
 
 int __init s3c2410_init_intc_of(struct device_node *np,
-			struct device_node *interrupt_parent,
-			struct s3c24xx_irq_of_ctrl *ctrl, int num_ctrl)
+			struct device_node *interrupt_parent)
 {
 	return s3c_init_intc_of(np, interrupt_parent,
 				s3c2410_ctrl, ARRAY_SIZE(s3c2410_ctrl));
@@ -1346,8 +1345,7 @@ static struct s3c24xx_irq_of_ctrl s3c2416_ctrl[] = {
 };
 
 int __init s3c2416_init_intc_of(struct device_node *np,
-			struct device_node *interrupt_parent,
-			struct s3c24xx_irq_of_ctrl *ctrl, int num_ctrl)
+			struct device_node *interrupt_parent)
 {
 	return s3c_init_intc_of(np, interrupt_parent,
 				s3c2416_ctrl, ARRAY_SIZE(s3c2416_ctrl));
diff --git a/drivers/irqchip/irq-sirfsoc.c b/drivers/irqchip/irq-sirfsoc.c
index 581eefe331a..5e54f6d71e7 100644
--- a/drivers/irqchip/irq-sirfsoc.c
+++ b/drivers/irqchip/irq-sirfsoc.c
@@ -58,7 +58,8 @@ static void __exception_irq_entry sirfsoc_handle_irq(struct pt_regs *regs)
 	handle_IRQ(irqnr, regs);
 }
 
-static int __init sirfsoc_irq_init(struct device_node *np, struct device_node *parent)
+static int __init sirfsoc_irq_init(struct device_node *np,
+	struct device_node *parent)
 {
 	void __iomem *base = of_iomap(np, 0);
 	if (!base)
diff --git a/drivers/irqchip/irqchip.c b/drivers/irqchip/irqchip.c
index cad3e249555..0fe2f718d81 100644
--- a/drivers/irqchip/irqchip.c
+++ b/drivers/irqchip/irqchip.c
@@ -19,11 +19,11 @@
  * special section.
  */
 static const struct of_device_id
-irqchip_of_match_end __used __section(__irqchip_of_end);
+irqchip_of_match_end __used __section(__irqchip_of_table_end);
 
-extern struct of_device_id __irqchip_begin[];
+extern struct of_device_id __irqchip_of_table[];
 
 void __init irqchip_init(void)
 {
-	of_irq_init(__irqchip_begin);
+	of_irq_init(__irqchip_of_table);
 }
diff --git a/drivers/irqchip/irqchip.h b/drivers/irqchip/irqchip.h
index e445ba2d6ad..0f6486d4f1b 100644
--- a/drivers/irqchip/irqchip.h
+++ b/drivers/irqchip/irqchip.h
@@ -11,6 +11,8 @@
 #ifndef _IRQCHIP_H
 #define _IRQCHIP_H
 
+#include <linux/of.h>
+
 /*
  * This macro must be used by the different irqchip drivers to declare
  * the association between their DT compatible string and their
@@ -21,9 +23,6 @@
  * @compstr: compatible string of the irqchip driver
  * @fn: initialization function
  */
-#define IRQCHIP_DECLARE(name,compstr,fn)				\
-	static const struct of_device_id irqchip_of_match_##name	\
-	__used __section(__irqchip_of_table)				\
-	= { .compatible = compstr, .data = fn }
+#define IRQCHIP_DECLARE(name, compat, fn) OF_DECLARE_2(irqchip, name, compat, fn)
 
 #endif
diff --git a/drivers/leds/Kconfig b/drivers/leds/Kconfig
index 6de9dfbf61c..089841ca180 100644
--- a/drivers/leds/Kconfig
+++ b/drivers/leds/Kconfig
@@ -479,6 +479,8 @@ config LEDS_OT200
 	  This option enables support for the LEDs on the Bachmann OT200.
 	  Say Y to enable LEDs on the Bachmann OT200.
 
+comment "LED driver for blink(1) USB RGB LED is under Special HID drivers (HID_THINGM)"
+
 config LEDS_BLINKM
 	tristate "LED support for the BlinkM I2C RGB LED"
 	depends on LEDS_CLASS
@@ -487,6 +489,14 @@ 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_VERSATILE
+	tristate "LED support for the ARM Versatile and RealView"
+	depends on ARCH_REALVIEW || ARCH_VERSATILE
+	depends on LEDS_CLASS
+	help
+	  This option enabled support for the LEDs on the ARM Versatile
+	  and RealView boards. Say Y to enabled these.
+
 comment "LED Triggers"
 source "drivers/leds/trigger/Kconfig"
 
diff --git a/drivers/leds/Makefile b/drivers/leds/Makefile
index 3cd76dbd9be..8b4c956e11b 100644
--- a/drivers/leds/Makefile
+++ b/drivers/leds/Makefile
@@ -54,6 +54,7 @@ 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_VERSATILE)		+= leds-versatile.o
 
 # LED SPI Drivers
 obj-$(CONFIG_LEDS_DAC124S085)		+= leds-dac124s085.o
diff --git a/drivers/leds/leds-pwm.c b/drivers/leds/leds-pwm.c
index 7d0aaed1e23..aa770ec1e89 100644
--- a/drivers/leds/leds-pwm.c
+++ b/drivers/leds/leds-pwm.c
@@ -181,7 +181,6 @@ static int led_pwm_probe(struct platform_device *pdev)
 			led_dat->cdev.name = cur_led->name;
 			led_dat->cdev.default_trigger = cur_led->default_trigger;
 			led_dat->active_low = cur_led->active_low;
-			led_dat->period = cur_led->pwm_period_ns;
 			led_dat->cdev.brightness_set = led_pwm_set;
 			led_dat->cdev.brightness = LED_OFF;
 			led_dat->cdev.max_brightness = cur_led->max_brightness;
@@ -191,6 +190,10 @@ static int led_pwm_probe(struct platform_device *pdev)
 			if (led_dat->can_sleep)
 				INIT_WORK(&led_dat->work, led_pwm_work);
 
+			led_dat->period = pwm_get_period(led_dat->pwm);
+			if (!led_dat->period && (cur_led->pwm_period_ns > 0))
+				led_dat->period = cur_led->pwm_period_ns;
+
 			ret = led_classdev_register(&pdev->dev, &led_dat->cdev);
 			if (ret < 0)
 				goto err;
diff --git a/drivers/leds/leds-versatile.c b/drivers/leds/leds-versatile.c
new file mode 100644
index 00000000000..80553022d66
--- /dev/null
+++ b/drivers/leds/leds-versatile.c
@@ -0,0 +1,110 @@
+/*
+ * Driver for the 8 user LEDs found on the RealViews and Versatiles
+ * Based on DaVinci's DM365 board code
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ * Author: Linus Walleij <triad@df.lth.se>
+ */
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/leds.h>
+#include <linux/platform_device.h>
+
+struct versatile_led {
+	void __iomem		*base;
+	struct led_classdev	cdev;
+	u8			mask;
+};
+
+/*
+ * The triggers lines up below will only be used if the
+ * LED triggers are compiled in.
+ */
+static const struct {
+	const char *name;
+	const char *trigger;
+} versatile_leds[] = {
+	{ "versatile:0", "heartbeat", },
+	{ "versatile:1", "mmc0", },
+	{ "versatile:2", "cpu0" },
+	{ "versatile:3", "cpu1" },
+	{ "versatile:4", "cpu2" },
+	{ "versatile:5", "cpu3" },
+	{ "versatile:6", },
+	{ "versatile:7", },
+};
+
+static void versatile_led_set(struct led_classdev *cdev,
+			      enum led_brightness b)
+{
+	struct versatile_led *led = container_of(cdev,
+						 struct versatile_led, cdev);
+	u32 reg = readl(led->base);
+
+	if (b != LED_OFF)
+		reg |= led->mask;
+	else
+		reg &= ~led->mask;
+	writel(reg, led->base);
+}
+
+static enum led_brightness versatile_led_get(struct led_classdev *cdev)
+{
+	struct versatile_led *led = container_of(cdev,
+						 struct versatile_led, cdev);
+	u32 reg = readl(led->base);
+
+	return (reg & led->mask) ? LED_FULL : LED_OFF;
+}
+
+static int versatile_leds_probe(struct platform_device *dev)
+{
+	int i;
+	struct resource *res;
+	void __iomem *base;
+
+	res = platform_get_resource(dev, IORESOURCE_MEM, 0);
+	base = devm_ioremap_resource(&dev->dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	/* All off */
+	writel(0, base);
+	for (i = 0; i < ARRAY_SIZE(versatile_leds); i++) {
+		struct versatile_led *led;
+
+		led = kzalloc(sizeof(*led), GFP_KERNEL);
+		if (!led)
+			break;
+
+		led->base = base;
+		led->cdev.name = versatile_leds[i].name;
+		led->cdev.brightness_set = versatile_led_set;
+		led->cdev.brightness_get = versatile_led_get;
+		led->cdev.default_trigger = versatile_leds[i].trigger;
+		led->mask = BIT(i);
+
+		if (led_classdev_register(NULL, &led->cdev) < 0) {
+			kfree(led);
+			break;
+		}
+	}
+
+	return 0;
+}
+
+static struct platform_driver versatile_leds_driver = {
+	.driver = {
+		.name   = "versatile-leds",
+	},
+	.probe = versatile_leds_probe,
+};
+
+module_platform_driver(versatile_leds_driver);
+
+MODULE_AUTHOR("Linus Walleij <linus.walleij@linaro.org>");
+MODULE_DESCRIPTION("ARM Versatile LED driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/macintosh/windfarm_pm121.c b/drivers/macintosh/windfarm_pm121.c
index 7fe58b0ae8b..b350fb86ff0 100644
--- a/drivers/macintosh/windfarm_pm121.c
+++ b/drivers/macintosh/windfarm_pm121.c
@@ -555,8 +555,18 @@ static void pm121_create_sys_fans(int loop_id)
 	pid_param.interval	= PM121_SYS_INTERVAL;
 	pid_param.history_len	= PM121_SYS_HISTORY_SIZE;
 	pid_param.itarget	= param->itarget;
-	pid_param.min		= control->ops->get_min(control);
-	pid_param.max		= control->ops->get_max(control);
+	if(control)
+	{
+		pid_param.min		= control->ops->get_min(control);
+		pid_param.max		= control->ops->get_max(control);
+	} else {
+		/*
+		 * This is probably not the right!?
+		 * Perhaps goto fail  if control == NULL  above?
+		 */
+		pid_param.min		= 0;
+		pid_param.max		= 0;
+	}
 
 	wf_pid_init(&pm121_sys_state[loop_id]->pid, &pid_param);
 
@@ -571,7 +581,7 @@ static void pm121_create_sys_fans(int loop_id)
 	   control the same control */
 	printk(KERN_WARNING "pm121: failed to set up %s loop "
 	       "setting \"%s\" to max speed.\n",
-	       loop_names[loop_id], control->name);
+	       loop_names[loop_id], control ? control->name : "uninitialized value");
 
 	if (control)
 		wf_control_set_max(control);
diff --git a/drivers/mcb/mcb-core.c b/drivers/mcb/mcb-core.c
index bbe12932d40..9018ab83517 100644
--- a/drivers/mcb/mcb-core.c
+++ b/drivers/mcb/mcb-core.c
@@ -183,14 +183,14 @@ EXPORT_SYMBOL_GPL(mcb_device_register);
  *
  * Allocate a new @mcb_bus.
  */
-struct mcb_bus *mcb_alloc_bus(void)
+struct mcb_bus *mcb_alloc_bus(struct device *carrier)
 {
 	struct mcb_bus *bus;
 	int bus_nr;
 
 	bus = kzalloc(sizeof(struct mcb_bus), GFP_KERNEL);
 	if (!bus)
-		return NULL;
+		return ERR_PTR(-ENOMEM);
 
 	bus_nr = ida_simple_get(&mcb_ida, 0, 0, GFP_KERNEL);
 	if (bus_nr < 0) {
@@ -200,7 +200,7 @@ struct mcb_bus *mcb_alloc_bus(void)
 
 	INIT_LIST_HEAD(&bus->children);
 	bus->bus_nr = bus_nr;
-
+	bus->carrier = carrier;
 	return bus;
 }
 EXPORT_SYMBOL_GPL(mcb_alloc_bus);
@@ -378,6 +378,13 @@ void mcb_release_mem(struct resource *mem)
 }
 EXPORT_SYMBOL_GPL(mcb_release_mem);
 
+static int __mcb_get_irq(struct mcb_device *dev)
+{
+	struct resource *irq = &dev->irq;
+
+	return irq->start;
+}
+
 /**
  * mcb_get_irq() - Get device's IRQ number
  * @dev: The @mcb_device the IRQ is for
@@ -386,9 +393,12 @@ EXPORT_SYMBOL_GPL(mcb_release_mem);
  */
 int mcb_get_irq(struct mcb_device *dev)
 {
-	struct resource *irq = &dev->irq;
+	struct mcb_bus *bus = dev->bus;
 
-	return irq->start;
+	if (bus->get_irq)
+		return bus->get_irq(dev);
+
+	return __mcb_get_irq(dev);
 }
 EXPORT_SYMBOL_GPL(mcb_get_irq);
 
diff --git a/drivers/mcb/mcb-pci.c b/drivers/mcb/mcb-pci.c
index 99c742cbfb5..b5918196564 100644
--- a/drivers/mcb/mcb-pci.c
+++ b/drivers/mcb/mcb-pci.c
@@ -20,6 +20,15 @@ struct priv {
 	void __iomem *base;
 };
 
+static int mcb_pci_get_irq(struct mcb_device *mdev)
+{
+	struct mcb_bus *mbus = mdev->bus;
+	struct device *dev = mbus->carrier;
+	struct pci_dev *pdev = to_pci_dev(dev);
+
+	return pdev->irq;
+}
+
 static int mcb_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 {
 	struct priv *priv;
@@ -67,7 +76,13 @@ static int mcb_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	pci_set_drvdata(pdev, priv);
 
-	priv->bus = mcb_alloc_bus();
+	priv->bus = mcb_alloc_bus(&pdev->dev);
+	if (IS_ERR(priv->bus)) {
+		ret = PTR_ERR(priv->bus);
+		goto err_drvdata;
+	}
+
+	priv->bus->get_irq = mcb_pci_get_irq;
 
 	ret = chameleon_parse_cells(priv->bus, mapbase, priv->base);
 	if (ret < 0)
diff --git a/drivers/md/bcache/bcache.h b/drivers/md/bcache/bcache.h
index 82c9c5d3525..d2ebcf32309 100644
--- a/drivers/md/bcache/bcache.h
+++ b/drivers/md/bcache/bcache.h
@@ -828,7 +828,7 @@ static inline bool cached_dev_get(struct cached_dev *dc)
 		return false;
 
 	/* Paired with the mb in cached_dev_attach */
-	smp_mb__after_atomic_inc();
+	smp_mb__after_atomic();
 	return true;
 }
 
diff --git a/drivers/md/bcache/closure.h b/drivers/md/bcache/closure.h
index 7ef7461912b..a08e3eeac3c 100644
--- a/drivers/md/bcache/closure.h
+++ b/drivers/md/bcache/closure.h
@@ -243,7 +243,7 @@ static inline void set_closure_fn(struct closure *cl, closure_fn *fn,
 	cl->fn = fn;
 	cl->wq = wq;
 	/* between atomic_dec() in closure_put() */
-	smp_mb__before_atomic_dec();
+	smp_mb__before_atomic();
 }
 
 static inline void closure_queue(struct closure *cl)
diff --git a/drivers/md/bitmap.c b/drivers/md/bitmap.c
index 9a8e66ae04f..67f8b31e205 100644
--- a/drivers/md/bitmap.c
+++ b/drivers/md/bitmap.c
@@ -669,17 +669,13 @@ static inline unsigned long file_page_offset(struct bitmap_storage *store,
 /*
  * return a pointer to the page in the filemap that contains the given bit
  *
- * this lookup is complicated by the fact that the bitmap sb might be exactly
- * 1 page (e.g., x86) or less than 1 page -- so the bitmap might start on page
- * 0 or page 1
  */
 static inline struct page *filemap_get_page(struct bitmap_storage *store,
 					    unsigned long chunk)
 {
 	if (file_page_index(store, chunk) >= store->file_pages)
 		return NULL;
-	return store->filemap[file_page_index(store, chunk)
-			      - file_page_index(store, 0)];
+	return store->filemap[file_page_index(store, chunk)];
 }
 
 static int bitmap_storage_alloc(struct bitmap_storage *store,
diff --git a/drivers/md/dm-bufio.c b/drivers/md/dm-bufio.c
index 66c5d130c8c..4e84095833d 100644
--- a/drivers/md/dm-bufio.c
+++ b/drivers/md/dm-bufio.c
@@ -607,9 +607,9 @@ static void write_endio(struct bio *bio, int error)
 
 	BUG_ON(!test_bit(B_WRITING, &b->state));
 
-	smp_mb__before_clear_bit();
+	smp_mb__before_atomic();
 	clear_bit(B_WRITING, &b->state);
-	smp_mb__after_clear_bit();
+	smp_mb__after_atomic();
 
 	wake_up_bit(&b->state, B_WRITING);
 }
@@ -997,9 +997,9 @@ static void read_endio(struct bio *bio, int error)
 
 	BUG_ON(!test_bit(B_READING, &b->state));
 
-	smp_mb__before_clear_bit();
+	smp_mb__before_atomic();
 	clear_bit(B_READING, &b->state);
-	smp_mb__after_clear_bit();
+	smp_mb__after_atomic();
 
 	wake_up_bit(&b->state, B_READING);
 }
diff --git a/drivers/md/dm-cache-target.c b/drivers/md/dm-cache-target.c
index 9380be7b189..5f054c44b48 100644
--- a/drivers/md/dm-cache-target.c
+++ b/drivers/md/dm-cache-target.c
@@ -2178,6 +2178,8 @@ static int cache_create(struct cache_args *ca, struct cache **result)
 	ti->num_discard_bios = 1;
 	ti->discards_supported = true;
 	ti->discard_zeroes_data_unsupported = true;
+	/* Discard bios must be split on a block boundary */
+	ti->split_discard_bios = true;
 
 	cache->features = ca->features;
 	ti->per_bio_data_size = get_per_bio_data_size(cache);
diff --git a/drivers/md/dm-crypt.c b/drivers/md/dm-crypt.c
index 784695d22fd..53b213226c0 100644
--- a/drivers/md/dm-crypt.c
+++ b/drivers/md/dm-crypt.c
@@ -19,7 +19,6 @@
 #include <linux/crypto.h>
 #include <linux/workqueue.h>
 #include <linux/backing-dev.h>
-#include <linux/percpu.h>
 #include <linux/atomic.h>
 #include <linux/scatterlist.h>
 #include <asm/page.h>
@@ -43,6 +42,7 @@ struct convert_context {
 	struct bvec_iter iter_out;
 	sector_t cc_sector;
 	atomic_t cc_pending;
+	struct ablkcipher_request *req;
 };
 
 /*
@@ -111,15 +111,7 @@ struct iv_tcw_private {
 enum flags { DM_CRYPT_SUSPENDED, DM_CRYPT_KEY_VALID };
 
 /*
- * Duplicated per-CPU state for cipher.
- */
-struct crypt_cpu {
-	struct ablkcipher_request *req;
-};
-
-/*
- * The fields in here must be read only after initialization,
- * changing state should be in crypt_cpu.
+ * The fields in here must be read only after initialization.
  */
 struct crypt_config {
 	struct dm_dev *dev;
@@ -150,12 +142,6 @@ struct crypt_config {
 	sector_t iv_offset;
 	unsigned int iv_size;
 
-	/*
-	 * Duplicated per cpu state. Access through
-	 * per_cpu_ptr() only.
-	 */
-	struct crypt_cpu __percpu *cpu;
-
 	/* ESSIV: struct crypto_cipher *essiv_tfm */
 	void *iv_private;
 	struct crypto_ablkcipher **tfms;
@@ -192,11 +178,6 @@ static void clone_init(struct dm_crypt_io *, struct bio *);
 static void kcryptd_queue_crypt(struct dm_crypt_io *io);
 static u8 *iv_of_dmreq(struct crypt_config *cc, struct dm_crypt_request *dmreq);
 
-static struct crypt_cpu *this_crypt_config(struct crypt_config *cc)
-{
-	return this_cpu_ptr(cc->cpu);
-}
-
 /*
  * Use this to access cipher attributes that are the same for each CPU.
  */
@@ -903,16 +884,15 @@ static void kcryptd_async_done(struct crypto_async_request *async_req,
 static void crypt_alloc_req(struct crypt_config *cc,
 			    struct convert_context *ctx)
 {
-	struct crypt_cpu *this_cc = this_crypt_config(cc);
 	unsigned key_index = ctx->cc_sector & (cc->tfms_count - 1);
 
-	if (!this_cc->req)
-		this_cc->req = mempool_alloc(cc->req_pool, GFP_NOIO);
+	if (!ctx->req)
+		ctx->req = mempool_alloc(cc->req_pool, GFP_NOIO);
 
-	ablkcipher_request_set_tfm(this_cc->req, cc->tfms[key_index]);
-	ablkcipher_request_set_callback(this_cc->req,
+	ablkcipher_request_set_tfm(ctx->req, cc->tfms[key_index]);
+	ablkcipher_request_set_callback(ctx->req,
 	    CRYPTO_TFM_REQ_MAY_BACKLOG | CRYPTO_TFM_REQ_MAY_SLEEP,
-	    kcryptd_async_done, dmreq_of_req(cc, this_cc->req));
+	    kcryptd_async_done, dmreq_of_req(cc, ctx->req));
 }
 
 /*
@@ -921,7 +901,6 @@ static void crypt_alloc_req(struct crypt_config *cc,
 static int crypt_convert(struct crypt_config *cc,
 			 struct convert_context *ctx)
 {
-	struct crypt_cpu *this_cc = this_crypt_config(cc);
 	int r;
 
 	atomic_set(&ctx->cc_pending, 1);
@@ -932,7 +911,7 @@ static int crypt_convert(struct crypt_config *cc,
 
 		atomic_inc(&ctx->cc_pending);
 
-		r = crypt_convert_block(cc, ctx, this_cc->req);
+		r = crypt_convert_block(cc, ctx, ctx->req);
 
 		switch (r) {
 		/* async */
@@ -941,7 +920,7 @@ static int crypt_convert(struct crypt_config *cc,
 			reinit_completion(&ctx->restart);
 			/* fall through*/
 		case -EINPROGRESS:
-			this_cc->req = NULL;
+			ctx->req = NULL;
 			ctx->cc_sector++;
 			continue;
 
@@ -1040,6 +1019,7 @@ static struct dm_crypt_io *crypt_io_alloc(struct crypt_config *cc,
 	io->sector = sector;
 	io->error = 0;
 	io->base_io = NULL;
+	io->ctx.req = NULL;
 	atomic_set(&io->io_pending, 0);
 
 	return io;
@@ -1065,6 +1045,8 @@ static void crypt_dec_pending(struct dm_crypt_io *io)
 	if (!atomic_dec_and_test(&io->io_pending))
 		return;
 
+	if (io->ctx.req)
+		mempool_free(io->ctx.req, cc->req_pool);
 	mempool_free(io, cc->io_pool);
 
 	if (likely(!base_io))
@@ -1492,8 +1474,6 @@ static int crypt_wipe_key(struct crypt_config *cc)
 static void crypt_dtr(struct dm_target *ti)
 {
 	struct crypt_config *cc = ti->private;
-	struct crypt_cpu *cpu_cc;
-	int cpu;
 
 	ti->private = NULL;
 
@@ -1505,13 +1485,6 @@ static void crypt_dtr(struct dm_target *ti)
 	if (cc->crypt_queue)
 		destroy_workqueue(cc->crypt_queue);
 
-	if (cc->cpu)
-		for_each_possible_cpu(cpu) {
-			cpu_cc = per_cpu_ptr(cc->cpu, cpu);
-			if (cpu_cc->req)
-				mempool_free(cpu_cc->req, cc->req_pool);
-		}
-
 	crypt_free_tfms(cc);
 
 	if (cc->bs)
@@ -1530,9 +1503,6 @@ static void crypt_dtr(struct dm_target *ti)
 	if (cc->dev)
 		dm_put_device(ti, cc->dev);
 
-	if (cc->cpu)
-		free_percpu(cc->cpu);
-
 	kzfree(cc->cipher);
 	kzfree(cc->cipher_string);
 
@@ -1588,13 +1558,6 @@ static int crypt_ctr_cipher(struct dm_target *ti,
 	if (tmp)
 		DMWARN("Ignoring unexpected additional cipher options");
 
-	cc->cpu = __alloc_percpu(sizeof(*(cc->cpu)),
-				 __alignof__(struct crypt_cpu));
-	if (!cc->cpu) {
-		ti->error = "Cannot allocate per cpu state";
-		goto bad_mem;
-	}
-
 	/*
 	 * For compatibility with the original dm-crypt mapping format, if
 	 * only the cipher name is supplied, use cbc-plain.
diff --git a/drivers/md/dm-mpath.c b/drivers/md/dm-mpath.c
index aa009e86587..ebfa411d1a7 100644
--- a/drivers/md/dm-mpath.c
+++ b/drivers/md/dm-mpath.c
@@ -445,11 +445,11 @@ static int queue_if_no_path(struct multipath *m, unsigned queue_if_no_path,
 	else
 		m->saved_queue_if_no_path = queue_if_no_path;
 	m->queue_if_no_path = queue_if_no_path;
-	if (!m->queue_if_no_path)
-		dm_table_run_md_queue_async(m->ti->table);
-
 	spin_unlock_irqrestore(&m->lock, flags);
 
+	if (!queue_if_no_path)
+		dm_table_run_md_queue_async(m->ti->table);
+
 	return 0;
 }
 
@@ -954,7 +954,7 @@ out:
  */
 static int reinstate_path(struct pgpath *pgpath)
 {
-	int r = 0;
+	int r = 0, run_queue = 0;
 	unsigned long flags;
 	struct multipath *m = pgpath->pg->m;
 
@@ -978,7 +978,7 @@ static int reinstate_path(struct pgpath *pgpath)
 
 	if (!m->nr_valid_paths++) {
 		m->current_pgpath = NULL;
-		dm_table_run_md_queue_async(m->ti->table);
+		run_queue = 1;
 	} else if (m->hw_handler_name && (m->current_pg == pgpath->pg)) {
 		if (queue_work(kmpath_handlerd, &pgpath->activate_path.work))
 			m->pg_init_in_progress++;
@@ -991,6 +991,8 @@ static int reinstate_path(struct pgpath *pgpath)
 
 out:
 	spin_unlock_irqrestore(&m->lock, flags);
+	if (run_queue)
+		dm_table_run_md_queue_async(m->ti->table);
 
 	return r;
 }
diff --git a/drivers/md/dm-snap.c b/drivers/md/dm-snap.c
index ebddef5237e..8e0caed0bf7 100644
--- a/drivers/md/dm-snap.c
+++ b/drivers/md/dm-snap.c
@@ -642,7 +642,7 @@ static void free_pending_exception(struct dm_snap_pending_exception *pe)
 	struct dm_snapshot *s = pe->snap;
 
 	mempool_free(pe, s->pending_pool);
-	smp_mb__before_atomic_dec();
+	smp_mb__before_atomic();
 	atomic_dec(&s->pending_exceptions_count);
 }
 
@@ -783,7 +783,7 @@ static int init_hash_tables(struct dm_snapshot *s)
 static void merge_shutdown(struct dm_snapshot *s)
 {
 	clear_bit_unlock(RUNNING_MERGE, &s->state_bits);
-	smp_mb__after_clear_bit();
+	smp_mb__after_atomic();
 	wake_up_bit(&s->state_bits, RUNNING_MERGE);
 }
 
diff --git a/drivers/md/dm-thin.c b/drivers/md/dm-thin.c
index 13abade76ad..242ac2ea5f2 100644
--- a/drivers/md/dm-thin.c
+++ b/drivers/md/dm-thin.c
@@ -27,6 +27,9 @@
 #define MAPPING_POOL_SIZE 1024
 #define PRISON_CELLS 1024
 #define COMMIT_PERIOD HZ
+#define NO_SPACE_TIMEOUT_SECS 60
+
+static unsigned no_space_timeout_secs = NO_SPACE_TIMEOUT_SECS;
 
 DECLARE_DM_KCOPYD_THROTTLE_WITH_MODULE_PARM(snapshot_copy_throttle,
 		"A percentage of time allocated for copy on write");
@@ -175,6 +178,7 @@ struct pool {
 	struct workqueue_struct *wq;
 	struct work_struct worker;
 	struct delayed_work waker;
+	struct delayed_work no_space_timeout;
 
 	unsigned long last_commit_jiffies;
 	unsigned ref_count;
@@ -935,7 +939,7 @@ static int commit(struct pool *pool)
 {
 	int r;
 
-	if (get_pool_mode(pool) != PM_WRITE)
+	if (get_pool_mode(pool) >= PM_READ_ONLY)
 		return -EINVAL;
 
 	r = dm_pool_commit_metadata(pool->pmd);
@@ -1590,6 +1594,20 @@ static void do_waker(struct work_struct *ws)
 	queue_delayed_work(pool->wq, &pool->waker, COMMIT_PERIOD);
 }
 
+/*
+ * We're holding onto IO to allow userland time to react.  After the
+ * timeout either the pool will have been resized (and thus back in
+ * PM_WRITE mode), or we degrade to PM_READ_ONLY and start erroring IO.
+ */
+static void do_no_space_timeout(struct work_struct *ws)
+{
+	struct pool *pool = container_of(to_delayed_work(ws), struct pool,
+					 no_space_timeout);
+
+	if (get_pool_mode(pool) == PM_OUT_OF_DATA_SPACE && !pool->pf.error_if_no_space)
+		set_pool_mode(pool, PM_READ_ONLY);
+}
+
 /*----------------------------------------------------------------*/
 
 struct noflush_work {
@@ -1654,6 +1672,7 @@ static void set_pool_mode(struct pool *pool, enum pool_mode new_mode)
 	struct pool_c *pt = pool->ti->private;
 	bool needs_check = dm_pool_metadata_needs_check(pool->pmd);
 	enum pool_mode old_mode = get_pool_mode(pool);
+	unsigned long no_space_timeout = ACCESS_ONCE(no_space_timeout_secs) * HZ;
 
 	/*
 	 * Never allow the pool to transition to PM_WRITE mode if user
@@ -1715,6 +1734,9 @@ static void set_pool_mode(struct pool *pool, enum pool_mode new_mode)
 		pool->process_discard = process_discard;
 		pool->process_prepared_mapping = process_prepared_mapping;
 		pool->process_prepared_discard = process_prepared_discard_passdown;
+
+		if (!pool->pf.error_if_no_space && no_space_timeout)
+			queue_delayed_work(pool->wq, &pool->no_space_timeout, no_space_timeout);
 		break;
 
 	case PM_WRITE:
@@ -2100,6 +2122,7 @@ static struct pool *pool_create(struct mapped_device *pool_md,
 
 	INIT_WORK(&pool->worker, do_worker);
 	INIT_DELAYED_WORK(&pool->waker, do_waker);
+	INIT_DELAYED_WORK(&pool->no_space_timeout, do_no_space_timeout);
 	spin_lock_init(&pool->lock);
 	bio_list_init(&pool->deferred_flush_bios);
 	INIT_LIST_HEAD(&pool->prepared_mappings);
@@ -2662,6 +2685,7 @@ static void pool_postsuspend(struct dm_target *ti)
 	struct pool *pool = pt->pool;
 
 	cancel_delayed_work(&pool->waker);
+	cancel_delayed_work(&pool->no_space_timeout);
 	flush_workqueue(pool->wq);
 	(void) commit(pool);
 }
@@ -3487,6 +3511,9 @@ static void dm_thin_exit(void)
 module_init(dm_thin_init);
 module_exit(dm_thin_exit);
 
+module_param_named(no_space_timeout, no_space_timeout_secs, uint, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(no_space_timeout, "Out of data space queue IO timeout in seconds");
+
 MODULE_DESCRIPTION(DM_NAME " thin provisioning target");
 MODULE_AUTHOR("Joe Thornber <dm-devel@redhat.com>");
 MODULE_LICENSE("GPL");
diff --git a/drivers/md/dm.c b/drivers/md/dm.c
index 455e6491649..aa9e093343d 100644
--- a/drivers/md/dm.c
+++ b/drivers/md/dm.c
@@ -1544,7 +1544,6 @@ static int setup_clone(struct request *clone, struct request *rq,
 	clone->cmd = rq->cmd;
 	clone->cmd_len = rq->cmd_len;
 	clone->sense = rq->sense;
-	clone->buffer = rq->buffer;
 	clone->end_io = end_clone_request;
 	clone->end_io_data = tio;
 
@@ -2447,7 +2446,7 @@ static void dm_wq_work(struct work_struct *work)
 static void dm_queue_flush(struct mapped_device *md)
 {
 	clear_bit(DMF_BLOCK_IO_FOR_SUSPEND, &md->flags);
-	smp_mb__after_clear_bit();
+	smp_mb__after_atomic();
 	queue_work(md->wq, &md->work);
 }
 
diff --git a/drivers/md/md.c b/drivers/md/md.c
index 8fda38d23e3..34846856dbc 100644
--- a/drivers/md/md.c
+++ b/drivers/md/md.c
@@ -3448,6 +3448,8 @@ level_store(struct mddev *mddev, const char *buf, size_t len)
 		mddev->level = LEVEL_NONE;
 		return rv;
 	}
+	if (mddev->ro)
+		return  -EROFS;
 
 	/* request to change the personality.  Need to ensure:
 	 *  - array is not engaged in resync/recovery/reshape
@@ -3634,6 +3636,8 @@ layout_store(struct mddev *mddev, const char *buf, size_t len)
 		int err;
 		if (mddev->pers->check_reshape == NULL)
 			return -EBUSY;
+		if (mddev->ro)
+			return -EROFS;
 		mddev->new_layout = n;
 		err = mddev->pers->check_reshape(mddev);
 		if (err) {
@@ -3723,6 +3727,8 @@ chunk_size_store(struct mddev *mddev, const char *buf, size_t len)
 		int err;
 		if (mddev->pers->check_reshape == NULL)
 			return -EBUSY;
+		if (mddev->ro)
+			return -EROFS;
 		mddev->new_chunk_sectors = n >> 9;
 		err = mddev->pers->check_reshape(mddev);
 		if (err) {
@@ -6135,6 +6141,8 @@ static int update_size(struct mddev *mddev, sector_t num_sectors)
 	 */
 	if (mddev->sync_thread)
 		return -EBUSY;
+	if (mddev->ro)
+		return -EROFS;
 
 	rdev_for_each(rdev, mddev) {
 		sector_t avail = rdev->sectors;
@@ -6157,6 +6165,8 @@ static int update_raid_disks(struct mddev *mddev, int raid_disks)
 	/* change the number of raid disks */
 	if (mddev->pers->check_reshape == NULL)
 		return -EINVAL;
+	if (mddev->ro)
+		return -EROFS;
 	if (raid_disks <= 0 ||
 	    (mddev->max_disks && raid_disks >= mddev->max_disks))
 		return -EINVAL;
@@ -7381,8 +7391,10 @@ void md_do_sync(struct md_thread *thread)
 	/* just incase thread restarts... */
 	if (test_bit(MD_RECOVERY_DONE, &mddev->recovery))
 		return;
-	if (mddev->ro) /* never try to sync a read-only array */
+	if (mddev->ro) {/* never try to sync a read-only array */
+		set_bit(MD_RECOVERY_INTR, &mddev->recovery);
 		return;
+	}
 
 	if (test_bit(MD_RECOVERY_SYNC, &mddev->recovery)) {
 		if (test_bit(MD_RECOVERY_CHECK, &mddev->recovery)) {
@@ -7824,6 +7836,7 @@ void md_check_recovery(struct mddev *mddev)
 			/* There is no thread, but we need to call
 			 * ->spare_active and clear saved_raid_disk
 			 */
+			set_bit(MD_RECOVERY_INTR, &mddev->recovery);
 			md_reap_sync_thread(mddev);
 			clear_bit(MD_RECOVERY_NEEDED, &mddev->recovery);
 			goto unlock;
@@ -8330,7 +8343,7 @@ static int md_clear_badblocks(struct badblocks *bb, sector_t s, int sectors)
 			if (a < s) {
 				/* we need to split this range */
 				if (bb->count >= MD_MAX_BADBLOCKS) {
-					rv = 0;
+					rv = -ENOSPC;
 					goto out;
 				}
 				memmove(p+lo+1, p+lo, (bb->count - lo) * 8);
@@ -8516,7 +8529,8 @@ static int md_notify_reboot(struct notifier_block *this,
 		if (mddev_trylock(mddev)) {
 			if (mddev->pers)
 				__md_stop_writes(mddev);
-			mddev->safemode = 2;
+			if (mddev->persistent)
+				mddev->safemode = 2;
 			mddev_unlock(mddev);
 		}
 		need_delay = 1;
diff --git a/drivers/md/raid10.c b/drivers/md/raid10.c
index 33fc408e5ea..cb882aae9e2 100644
--- a/drivers/md/raid10.c
+++ b/drivers/md/raid10.c
@@ -1172,6 +1172,13 @@ static void __make_request(struct mddev *mddev, struct bio *bio)
 	int max_sectors;
 	int sectors;
 
+	/*
+	 * Register the new request and wait if the reconstruction
+	 * thread has put up a bar for new requests.
+	 * Continue immediately if no resync is active currently.
+	 */
+	wait_barrier(conf);
+
 	sectors = bio_sectors(bio);
 	while (test_bit(MD_RECOVERY_RESHAPE, &mddev->recovery) &&
 	    bio->bi_iter.bi_sector < conf->reshape_progress &&
@@ -1552,12 +1559,6 @@ static void make_request(struct mddev *mddev, struct bio *bio)
 
 	md_write_start(mddev, bio);
 
-	/*
-	 * Register the new request and wait if the reconstruction
-	 * thread has put up a bar for new requests.
-	 * Continue immediately if no resync is active currently.
-	 */
-	wait_barrier(conf);
 
 	do {
 
diff --git a/drivers/md/raid5.c b/drivers/md/raid5.c
index ad1b9bea446..6234b2e8458 100644
--- a/drivers/md/raid5.c
+++ b/drivers/md/raid5.c
@@ -292,9 +292,12 @@ static void do_release_stripe(struct r5conf *conf, struct stripe_head *sh,
 	BUG_ON(atomic_read(&conf->active_stripes)==0);
 	if (test_bit(STRIPE_HANDLE, &sh->state)) {
 		if (test_bit(STRIPE_DELAYED, &sh->state) &&
-		    !test_bit(STRIPE_PREREAD_ACTIVE, &sh->state))
+		    !test_bit(STRIPE_PREREAD_ACTIVE, &sh->state)) {
 			list_add_tail(&sh->lru, &conf->delayed_list);
-		else if (test_bit(STRIPE_BIT_DELAY, &sh->state) &&
+			if (atomic_read(&conf->preread_active_stripes)
+			    < IO_THRESHOLD)
+				md_wakeup_thread(conf->mddev->thread);
+		} else if (test_bit(STRIPE_BIT_DELAY, &sh->state) &&
 			   sh->bm_seq - conf->seq_write > 0)
 			list_add_tail(&sh->lru, &conf->bitmap_list);
 		else {
@@ -413,6 +416,11 @@ static void release_stripe(struct stripe_head *sh)
 	int hash;
 	bool wakeup;
 
+	/* Avoid release_list until the last reference.
+	 */
+	if (atomic_add_unless(&sh->count, -1, 1))
+		return;
+
 	if (unlikely(!conf->mddev->thread) ||
 		test_and_set_bit(STRIPE_ON_RELEASE_LIST, &sh->state))
 		goto slow_path;
@@ -479,6 +487,7 @@ static void shrink_buffers(struct stripe_head *sh)
 	int num = sh->raid_conf->pool_size;
 
 	for (i = 0; i < num ; i++) {
+		WARN_ON(sh->dev[i].page != sh->dev[i].orig_page);
 		p = sh->dev[i].page;
 		if (!p)
 			continue;
@@ -499,6 +508,7 @@ static int grow_buffers(struct stripe_head *sh)
 			return 1;
 		}
 		sh->dev[i].page = page;
+		sh->dev[i].orig_page = page;
 	}
 	return 0;
 }
@@ -855,6 +865,9 @@ static void ops_run_io(struct stripe_head *sh, struct stripe_head_state *s)
 			if (test_bit(R5_ReadNoMerge, &sh->dev[i].flags))
 				bi->bi_rw |= REQ_NOMERGE;
 
+			if (test_bit(R5_SkipCopy, &sh->dev[i].flags))
+				WARN_ON(test_bit(R5_UPTODATE, &sh->dev[i].flags));
+			sh->dev[i].vec.bv_page = sh->dev[i].page;
 			bi->bi_vcnt = 1;
 			bi->bi_io_vec[0].bv_len = STRIPE_SIZE;
 			bi->bi_io_vec[0].bv_offset = 0;
@@ -899,6 +912,9 @@ static void ops_run_io(struct stripe_head *sh, struct stripe_head_state *s)
 			else
 				rbi->bi_iter.bi_sector = (sh->sector
 						  + rrdev->data_offset);
+			if (test_bit(R5_SkipCopy, &sh->dev[i].flags))
+				WARN_ON(test_bit(R5_UPTODATE, &sh->dev[i].flags));
+			sh->dev[i].rvec.bv_page = sh->dev[i].page;
 			rbi->bi_vcnt = 1;
 			rbi->bi_io_vec[0].bv_len = STRIPE_SIZE;
 			rbi->bi_io_vec[0].bv_offset = 0;
@@ -927,8 +943,9 @@ static void ops_run_io(struct stripe_head *sh, struct stripe_head_state *s)
 }
 
 static struct dma_async_tx_descriptor *
-async_copy_data(int frombio, struct bio *bio, struct page *page,
-	sector_t sector, struct dma_async_tx_descriptor *tx)
+async_copy_data(int frombio, struct bio *bio, struct page **page,
+	sector_t sector, struct dma_async_tx_descriptor *tx,
+	struct stripe_head *sh)
 {
 	struct bio_vec bvl;
 	struct bvec_iter iter;
@@ -965,11 +982,16 @@ async_copy_data(int frombio, struct bio *bio, struct page *page,
 		if (clen > 0) {
 			b_offset += bvl.bv_offset;
 			bio_page = bvl.bv_page;
-			if (frombio)
-				tx = async_memcpy(page, bio_page, page_offset,
+			if (frombio) {
+				if (sh->raid_conf->skip_copy &&
+				    b_offset == 0 && page_offset == 0 &&
+				    clen == STRIPE_SIZE)
+					*page = bio_page;
+				else
+					tx = async_memcpy(*page, bio_page, page_offset,
 						  b_offset, clen, &submit);
-			else
-				tx = async_memcpy(bio_page, page, b_offset,
+			} else
+				tx = async_memcpy(bio_page, *page, b_offset,
 						  page_offset, clen, &submit);
 		}
 		/* chain the operations */
@@ -1045,8 +1067,8 @@ static void ops_run_biofill(struct stripe_head *sh)
 			spin_unlock_irq(&sh->stripe_lock);
 			while (rbi && rbi->bi_iter.bi_sector <
 				dev->sector + STRIPE_SECTORS) {
-				tx = async_copy_data(0, rbi, dev->page,
-					dev->sector, tx);
+				tx = async_copy_data(0, rbi, &dev->page,
+					dev->sector, tx, sh);
 				rbi = r5_next_bio(rbi, dev->sector);
 			}
 		}
@@ -1384,6 +1406,7 @@ ops_run_biodrain(struct stripe_head *sh, struct dma_async_tx_descriptor *tx)
 			BUG_ON(dev->written);
 			wbi = dev->written = chosen;
 			spin_unlock_irq(&sh->stripe_lock);
+			WARN_ON(dev->page != dev->orig_page);
 
 			while (wbi && wbi->bi_iter.bi_sector <
 				dev->sector + STRIPE_SECTORS) {
@@ -1393,9 +1416,15 @@ ops_run_biodrain(struct stripe_head *sh, struct dma_async_tx_descriptor *tx)
 					set_bit(R5_SyncIO, &dev->flags);
 				if (wbi->bi_rw & REQ_DISCARD)
 					set_bit(R5_Discard, &dev->flags);
-				else
-					tx = async_copy_data(1, wbi, dev->page,
-						dev->sector, tx);
+				else {
+					tx = async_copy_data(1, wbi, &dev->page,
+						dev->sector, tx, sh);
+					if (dev->page != dev->orig_page) {
+						set_bit(R5_SkipCopy, &dev->flags);
+						clear_bit(R5_UPTODATE, &dev->flags);
+						clear_bit(R5_OVERWRITE, &dev->flags);
+					}
+				}
 				wbi = r5_next_bio(wbi, dev->sector);
 			}
 		}
@@ -1426,7 +1455,7 @@ static void ops_complete_reconstruct(void *stripe_head_ref)
 		struct r5dev *dev = &sh->dev[i];
 
 		if (dev->written || i == pd_idx || i == qd_idx) {
-			if (!discard)
+			if (!discard && !test_bit(R5_SkipCopy, &dev->flags))
 				set_bit(R5_UPTODATE, &dev->flags);
 			if (fua)
 				set_bit(R5_WantFUA, &dev->flags);
@@ -1839,8 +1868,10 @@ static int resize_stripes(struct r5conf *conf, int newsize)
 		osh = get_free_stripe(conf, hash);
 		unlock_device_hash_lock(conf, hash);
 		atomic_set(&nsh->count, 1);
-		for(i=0; i<conf->pool_size; i++)
+		for(i=0; i<conf->pool_size; i++) {
 			nsh->dev[i].page = osh->dev[i].page;
+			nsh->dev[i].orig_page = osh->dev[i].page;
+		}
 		for( ; i<newsize; i++)
 			nsh->dev[i].page = NULL;
 		nsh->hash_lock_index = hash;
@@ -1896,6 +1927,7 @@ static int resize_stripes(struct r5conf *conf, int newsize)
 			if (nsh->dev[i].page == NULL) {
 				struct page *p = alloc_page(GFP_NOIO);
 				nsh->dev[i].page = p;
+				nsh->dev[i].orig_page = p;
 				if (!p)
 					err = -ENOMEM;
 			}
@@ -2133,24 +2165,20 @@ static void raid5_end_write_request(struct bio *bi, int error)
 }
 
 static sector_t compute_blocknr(struct stripe_head *sh, int i, int previous);
-	
+
 static void raid5_build_block(struct stripe_head *sh, int i, int previous)
 {
 	struct r5dev *dev = &sh->dev[i];
 
 	bio_init(&dev->req);
 	dev->req.bi_io_vec = &dev->vec;
-	dev->req.bi_vcnt++;
-	dev->req.bi_max_vecs++;
+	dev->req.bi_max_vecs = 1;
 	dev->req.bi_private = sh;
-	dev->vec.bv_page = dev->page;
 
 	bio_init(&dev->rreq);
 	dev->rreq.bi_io_vec = &dev->rvec;
-	dev->rreq.bi_vcnt++;
-	dev->rreq.bi_max_vecs++;
+	dev->rreq.bi_max_vecs = 1;
 	dev->rreq.bi_private = sh;
-	dev->rvec.bv_page = dev->page;
 
 	dev->flags = 0;
 	dev->sector = compute_blocknr(sh, i, previous);
@@ -2750,6 +2778,11 @@ handle_failed_stripe(struct r5conf *conf, struct stripe_head *sh,
 		/* and fail all 'written' */
 		bi = sh->dev[i].written;
 		sh->dev[i].written = NULL;
+		if (test_and_clear_bit(R5_SkipCopy, &sh->dev[i].flags)) {
+			WARN_ON(test_bit(R5_UPTODATE, &sh->dev[i].flags));
+			sh->dev[i].page = sh->dev[i].orig_page;
+		}
+
 		if (bi) bitmap_end = 1;
 		while (bi && bi->bi_iter.bi_sector <
 		       sh->dev[i].sector + STRIPE_SECTORS) {
@@ -2886,8 +2919,11 @@ static int fetch_block(struct stripe_head *sh, struct stripe_head_state *s,
 	     (s->failed >= 1 && fdev[0]->toread) ||
 	     (s->failed >= 2 && fdev[1]->toread) ||
 	     (sh->raid_conf->level <= 5 && s->failed && fdev[0]->towrite &&
+	      (!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))) {
+	     (sh->raid_conf->level == 6 && s->failed && s->to_write &&
+	      s->to_write < 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
 		 */
@@ -2991,12 +3027,17 @@ static void handle_stripe_clean_event(struct r5conf *conf,
 			dev = &sh->dev[i];
 			if (!test_bit(R5_LOCKED, &dev->flags) &&
 			    (test_bit(R5_UPTODATE, &dev->flags) ||
-			     test_bit(R5_Discard, &dev->flags))) {
+			     test_bit(R5_Discard, &dev->flags) ||
+			     test_bit(R5_SkipCopy, &dev->flags))) {
 				/* We can return any write requests */
 				struct bio *wbi, *wbi2;
 				pr_debug("Return write for disc %d\n", i);
 				if (test_and_clear_bit(R5_Discard, &dev->flags))
 					clear_bit(R5_UPTODATE, &dev->flags);
+				if (test_and_clear_bit(R5_SkipCopy, &dev->flags)) {
+					WARN_ON(test_bit(R5_UPTODATE, &dev->flags));
+					dev->page = dev->orig_page;
+				}
 				wbi = dev->written;
 				dev->written = NULL;
 				while (wbi && wbi->bi_iter.bi_sector <
@@ -3015,6 +3056,8 @@ static void handle_stripe_clean_event(struct r5conf *conf,
 						0);
 			} else if (test_bit(R5_Discard, &dev->flags))
 				discard_pending = 1;
+			WARN_ON(test_bit(R5_SkipCopy, &dev->flags));
+			WARN_ON(dev->page != dev->orig_page);
 		}
 	if (!discard_pending &&
 	    test_bit(R5_Discard, &sh->dev[sh->pd_idx].flags)) {
@@ -3086,7 +3129,8 @@ static void handle_stripe_dirtying(struct r5conf *conf,
 		    !test_bit(R5_LOCKED, &dev->flags) &&
 		    !(test_bit(R5_UPTODATE, &dev->flags) ||
 		    test_bit(R5_Wantcompute, &dev->flags))) {
-			if (test_bit(R5_Insync, &dev->flags)) rcw++;
+			if (test_bit(R5_Insync, &dev->flags))
+				rcw++;
 			else
 				rcw += 2*disks;
 		}
@@ -3107,10 +3151,10 @@ static void handle_stripe_dirtying(struct r5conf *conf,
 			    !(test_bit(R5_UPTODATE, &dev->flags) ||
 			    test_bit(R5_Wantcompute, &dev->flags)) &&
 			    test_bit(R5_Insync, &dev->flags)) {
-				if (
-				  test_bit(STRIPE_PREREAD_ACTIVE, &sh->state)) {
-					pr_debug("Read_old block "
-						 "%d for r-m-w\n", i);
+				if (test_bit(STRIPE_PREREAD_ACTIVE,
+					     &sh->state)) {
+					pr_debug("Read_old block %d for r-m-w\n",
+						 i);
 					set_bit(R5_LOCKED, &dev->flags);
 					set_bit(R5_Wantread, &dev->flags);
 					s->locked++;
@@ -3133,10 +3177,9 @@ static void handle_stripe_dirtying(struct r5conf *conf,
 			    !(test_bit(R5_UPTODATE, &dev->flags) ||
 			      test_bit(R5_Wantcompute, &dev->flags))) {
 				rcw++;
-				if (!test_bit(R5_Insync, &dev->flags))
-					continue; /* it's a failed drive */
-				if (
-				  test_bit(STRIPE_PREREAD_ACTIVE, &sh->state)) {
+				if (test_bit(R5_Insync, &dev->flags) &&
+				    test_bit(STRIPE_PREREAD_ACTIVE,
+					     &sh->state)) {
 					pr_debug("Read_old block "
 						"%d for Reconstruct\n", i);
 					set_bit(R5_LOCKED, &dev->flags);
@@ -4400,7 +4443,7 @@ static void raid5_unplug(struct blk_plug_cb *blk_cb, bool from_schedule)
 			 * STRIPE_ON_UNPLUG_LIST clear but the stripe
 			 * is still in our list
 			 */
-			smp_mb__before_clear_bit();
+			smp_mb__before_atomic();
 			clear_bit(STRIPE_ON_UNPLUG_LIST, &sh->state);
 			/*
 			 * STRIPE_ON_RELEASE_LIST could be set here. In that
@@ -5031,8 +5074,8 @@ static inline sector_t sync_request(struct mddev *mddev, sector_t sector_nr, int
 	bitmap_start_sync(mddev->bitmap, sector_nr, &sync_blocks, still_degraded);
 
 	set_bit(STRIPE_SYNC_REQUESTED, &sh->state);
+	set_bit(STRIPE_HANDLE, &sh->state);
 
-	handle_stripe(sh);
 	release_stripe(sh);
 
 	return STRIPE_SECTORS;
@@ -5072,7 +5115,7 @@ static int  retry_aligned_read(struct r5conf *conf, struct bio *raid_bio)
 			/* already done this stripe */
 			continue;
 
-		sh = get_active_stripe(conf, sector, 0, 1, 0);
+		sh = get_active_stripe(conf, sector, 0, 1, 1);
 
 		if (!sh) {
 			/* failed to get a stripe - must wait */
@@ -5355,6 +5398,50 @@ raid5_preread_bypass_threshold = __ATTR(preread_bypass_threshold,
 					raid5_store_preread_threshold);
 
 static ssize_t
+raid5_show_skip_copy(struct mddev *mddev, char *page)
+{
+	struct r5conf *conf = mddev->private;
+	if (conf)
+		return sprintf(page, "%d\n", conf->skip_copy);
+	else
+		return 0;
+}
+
+static ssize_t
+raid5_store_skip_copy(struct mddev *mddev, const char *page, size_t len)
+{
+	struct r5conf *conf = mddev->private;
+	unsigned long new;
+	if (len >= PAGE_SIZE)
+		return -EINVAL;
+	if (!conf)
+		return -ENODEV;
+
+	if (kstrtoul(page, 10, &new))
+		return -EINVAL;
+	new = !!new;
+	if (new == conf->skip_copy)
+		return len;
+
+	mddev_suspend(mddev);
+	conf->skip_copy = new;
+	if (new)
+		mddev->queue->backing_dev_info.capabilities |=
+						BDI_CAP_STABLE_WRITES;
+	else
+		mddev->queue->backing_dev_info.capabilities &=
+						~BDI_CAP_STABLE_WRITES;
+	mddev_resume(mddev);
+	return len;
+}
+
+static struct md_sysfs_entry
+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)
 {
 	struct r5conf *conf = mddev->private;
@@ -5439,6 +5526,7 @@ static struct attribute *raid5_attrs[] =  {
 	&raid5_stripecache_active.attr,
 	&raid5_preread_bypass_threshold.attr,
 	&raid5_group_thread_cnt.attr,
+	&raid5_skip_copy.attr,
 	NULL,
 };
 static struct attribute_group raid5_attrs_group = {
diff --git a/drivers/md/raid5.h b/drivers/md/raid5.h
index 01ad8ae8f57..bc72cd4be5f 100644
--- a/drivers/md/raid5.h
+++ b/drivers/md/raid5.h
@@ -232,7 +232,7 @@ struct stripe_head {
 		 */
 		struct bio	req, rreq;
 		struct bio_vec	vec, rvec;
-		struct page	*page;
+		struct page	*page, *orig_page;
 		struct bio	*toread, *read, *towrite, *written;
 		sector_t	sector;			/* sector of this page */
 		unsigned long	flags;
@@ -299,6 +299,7 @@ enum r5dev_flags {
 			 * data in, and now is a good time to write it out.
 			 */
 	R5_Discard,	/* Discard the stripe */
+	R5_SkipCopy,	/* Don't copy data from bio to stripe cache */
 };
 
 /*
@@ -436,6 +437,7 @@ struct r5conf {
 	atomic_t		pending_full_writes; /* full write backlog */
 	int			bypass_count; /* bypassed prereads */
 	int			bypass_threshold; /* preread nice */
+	int			skip_copy; /* Don't copy data from bio to stripe cache */
 	struct list_head	*last_hold; /* detect hold_list promotions */
 
 	atomic_t		reshape_stripes; /* stripes with pending writes for reshape */
diff --git a/drivers/media/dvb-core/dvb-usb-ids.h b/drivers/media/dvb-core/dvb-usb-ids.h
index 1bdc0e7e8b7..11d2bea23b0 100644
--- a/drivers/media/dvb-core/dvb-usb-ids.h
+++ b/drivers/media/dvb-core/dvb-usb-ids.h
@@ -356,11 +356,13 @@
 #define USB_PID_ELGATO_EYETV_DTT_2			0x003f
 #define USB_PID_ELGATO_EYETV_DTT_Dlx			0x0020
 #define USB_PID_ELGATO_EYETV_SAT			0x002a
+#define USB_PID_ELGATO_EYETV_SAT_V2			0x0025
 #define USB_PID_DVB_T_USB_STICK_HIGH_SPEED_COLD		0x5000
 #define USB_PID_DVB_T_USB_STICK_HIGH_SPEED_WARM		0x5001
 #define USB_PID_FRIIO_WHITE				0x0001
 #define USB_PID_TVWAY_PLUS				0x0002
 #define USB_PID_SVEON_STV20				0xe39d
+#define USB_PID_SVEON_STV20_RTL2832U			0xd39d
 #define USB_PID_SVEON_STV22				0xe401
 #define USB_PID_SVEON_STV22_IT9137			0xe411
 #define USB_PID_AZUREWAVE_AZ6027			0x3275
@@ -375,4 +377,5 @@
 #define USB_PID_CTVDIGDUAL_V2				0xe410
 #define USB_PID_PCTV_2002E                              0x025c
 #define USB_PID_PCTV_2002E_SE                           0x025d
+#define USB_PID_SVEON_STV27                             0xd3af
 #endif
diff --git a/drivers/media/dvb-frontends/Kconfig b/drivers/media/dvb-frontends/Kconfig
index 025fc5496bf..1469d44acb2 100644
--- a/drivers/media/dvb-frontends/Kconfig
+++ b/drivers/media/dvb-frontends/Kconfig
@@ -446,6 +446,13 @@ config DVB_RTL2832
 	help
 	  Say Y when you want to support this frontend.
 
+config DVB_SI2168
+	tristate "Silicon Labs Si2168"
+	depends on DVB_CORE && I2C && I2C_MUX
+	default m if !MEDIA_SUBDRV_AUTOSELECT
+	help
+	  Say Y when you want to support this frontend.
+
 comment "DVB-C (cable) frontends"
 	depends on DVB_CORE
 
diff --git a/drivers/media/dvb-frontends/Makefile b/drivers/media/dvb-frontends/Makefile
index 282aba2fe8d..dda0bee36f2 100644
--- a/drivers/media/dvb-frontends/Makefile
+++ b/drivers/media/dvb-frontends/Makefile
@@ -78,6 +78,7 @@ obj-$(CONFIG_DVB_AF9013) += af9013.o
 obj-$(CONFIG_DVB_CX24116) += cx24116.o
 obj-$(CONFIG_DVB_CX24117) += cx24117.o
 obj-$(CONFIG_DVB_SI21XX) += si21xx.o
+obj-$(CONFIG_DVB_SI2168) += si2168.o
 obj-$(CONFIG_DVB_STV0288) += stv0288.o
 obj-$(CONFIG_DVB_STB6000) += stb6000.o
 obj-$(CONFIG_DVB_S921) += s921.o
diff --git a/drivers/media/dvb-frontends/si2168.c b/drivers/media/dvb-frontends/si2168.c
new file mode 100644
index 00000000000..8637d2ed762
--- /dev/null
+++ b/drivers/media/dvb-frontends/si2168.c
@@ -0,0 +1,760 @@
+/*
+ * Silicon Labs Si2168 DVB-T/T2/C demodulator 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 "si2168_priv.h"
+
+static const struct dvb_frontend_ops si2168_ops;
+
+/* execute firmware command */
+static int si2168_cmd_execute(struct si2168 *s, struct si2168_cmd *cmd)
+{
+	int ret;
+	unsigned long timeout;
+
+	mutex_lock(&s->i2c_mutex);
+
+	if (cmd->wlen) {
+		/* write cmd and args for firmware */
+		ret = i2c_master_send(s->client, cmd->args, cmd->wlen);
+		if (ret < 0) {
+			goto err_mutex_unlock;
+		} else if (ret != cmd->wlen) {
+			ret = -EREMOTEIO;
+			goto err_mutex_unlock;
+		}
+	}
+
+	if (cmd->rlen) {
+		/* wait cmd execution terminate */
+		#define TIMEOUT 50
+		timeout = jiffies + msecs_to_jiffies(TIMEOUT);
+		while (!time_after(jiffies, timeout)) {
+			ret = i2c_master_recv(s->client, cmd->args, cmd->rlen);
+			if (ret < 0) {
+				goto err_mutex_unlock;
+			} else if (ret != cmd->rlen) {
+				ret = -EREMOTEIO;
+				goto err_mutex_unlock;
+			}
+
+			/* firmware ready? */
+			if ((cmd->args[0] >> 7) & 0x01)
+				break;
+		}
+
+		dev_dbg(&s->client->dev, "%s: cmd execution took %d ms\n",
+				__func__,
+				jiffies_to_msecs(jiffies) -
+				(jiffies_to_msecs(timeout) - TIMEOUT));
+
+		if (!(cmd->args[0] >> 7) & 0x01) {
+			ret = -ETIMEDOUT;
+			goto err_mutex_unlock;
+		}
+	}
+
+	ret = 0;
+
+err_mutex_unlock:
+	mutex_unlock(&s->i2c_mutex);
+	if (ret)
+		goto err;
+
+	return 0;
+err:
+	dev_dbg(&s->client->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static int si2168_read_status(struct dvb_frontend *fe, fe_status_t *status)
+{
+	struct si2168 *s = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int ret;
+	struct si2168_cmd cmd;
+
+	*status = 0;
+
+	if (!s->active) {
+		ret = -EAGAIN;
+		goto err;
+	}
+
+	switch (c->delivery_system) {
+	case SYS_DVBT:
+		cmd.args[0] = 0xa0;
+		cmd.args[1] = 0x01;
+		cmd.wlen = 2;
+		cmd.rlen = 13;
+		break;
+	case SYS_DVBC_ANNEX_A:
+		cmd.args[0] = 0x90;
+		cmd.args[1] = 0x01;
+		cmd.wlen = 2;
+		cmd.rlen = 9;
+		break;
+	case SYS_DVBT2:
+		cmd.args[0] = 0x50;
+		cmd.args[1] = 0x01;
+		cmd.wlen = 2;
+		cmd.rlen = 14;
+		break;
+	default:
+		ret = -EINVAL;
+		goto err;
+	}
+
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	/*
+	 * Possible values seen, in order from strong signal to weak:
+	 * 16 0001 0110 full lock
+	 * 1e 0001 1110 partial lock
+	 * 1a 0001 1010 partial lock
+	 * 18 0001 1000 no lock
+	 *
+	 * [b3:b1] lock bits
+	 * [b4] statistics ready? Set in a few secs after lock is gained.
+	 */
+
+	switch ((cmd.args[2] >> 1) & 0x03) {
+	case 0x01:
+		*status = FE_HAS_SIGNAL | FE_HAS_CARRIER;
+		break;
+	case 0x03:
+		*status = FE_HAS_SIGNAL | FE_HAS_CARRIER | FE_HAS_VITERBI |
+				FE_HAS_SYNC | FE_HAS_LOCK;
+		break;
+	}
+
+	s->fe_status = *status;
+
+	dev_dbg(&s->client->dev, "%s: status=%02x args=%*ph\n",
+			__func__, *status, cmd.rlen, cmd.args);
+
+	return 0;
+err:
+	dev_dbg(&s->client->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static int si2168_set_frontend(struct dvb_frontend *fe)
+{
+	struct si2168 *s = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int ret;
+	struct si2168_cmd cmd;
+	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,
+			c->frequency, c->bandwidth_hz, c->symbol_rate,
+			c->inversion);
+
+	if (!s->active) {
+		ret = -EAGAIN;
+		goto err;
+	}
+
+	switch (c->delivery_system) {
+	case SYS_DVBT:
+		delivery_system = 0x20;
+		break;
+	case SYS_DVBC_ANNEX_A:
+		delivery_system = 0x30;
+		break;
+	case SYS_DVBT2:
+		delivery_system = 0x70;
+		break;
+	default:
+		ret = -EINVAL;
+		goto err;
+	}
+
+	if (c->bandwidth_hz <= 5000000)
+		bandwidth = 0x05;
+	else if (c->bandwidth_hz <= 6000000)
+		bandwidth = 0x06;
+	else if (c->bandwidth_hz <= 7000000)
+		bandwidth = 0x07;
+	else if (c->bandwidth_hz <= 8000000)
+		bandwidth = 0x08;
+	else if (c->bandwidth_hz <= 9000000)
+		bandwidth = 0x09;
+	else if (c->bandwidth_hz <= 10000000)
+		bandwidth = 0x0a;
+	else
+		bandwidth = 0x0f;
+
+	/* program tuner */
+	if (fe->ops.tuner_ops.set_params) {
+		ret = fe->ops.tuner_ops.set_params(fe);
+		if (ret)
+			goto err;
+	}
+
+	memcpy(cmd.args, "\x88\x02\x02\x02\x02", 5);
+	cmd.wlen = 5;
+	cmd.rlen = 5;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	/* that has no big effect */
+	if (c->delivery_system == SYS_DVBT)
+		memcpy(cmd.args, "\x89\x21\x06\x11\xff\x98", 6);
+	else if (c->delivery_system == SYS_DVBC_ANNEX_A)
+		memcpy(cmd.args, "\x89\x21\x06\x11\x89\xf0", 6);
+	else if (c->delivery_system == SYS_DVBT2)
+		memcpy(cmd.args, "\x89\x21\x06\x11\x89\x20", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 3;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x51\x03", 2);
+	cmd.wlen = 2;
+	cmd.rlen = 12;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x12\x08\x04", 3);
+	cmd.wlen = 3;
+	cmd.rlen = 3;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x01\x04\x00\x00", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x03\x10\x17\x00", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x02\x10\x15\x00", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x0c\x10\x12\x00", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x06\x10\x24\x00", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x0b\x10\x88\x13", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x07\x10\x00\x24", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x0a\x10\x00\x00", 6);
+	cmd.args[4] = delivery_system | bandwidth;
+	cmd.wlen = 6;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x04\x10\x15\x00", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x05\x10\xa1\x00", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x0f\x10\x10\x00", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x0d\x10\xd0\x02", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x01\x10\x00\x00", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 1;
+	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 = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x08\x10\xd7\x15", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x04\x03\x00\x00", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x03\x03\x00\x00", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x08\x03\x00\x00", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x07\x03\x01\x02", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x06\x03\x00\x00", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x05\x03\x00\x00", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x01\x03\x0c\x40", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x01\x10\x16\x00", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x01\x12\x00\x00", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	cmd.args[0] = 0x85;
+	cmd.wlen = 1;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	s->delivery_system = c->delivery_system;
+
+	return 0;
+err:
+	dev_dbg(&s->client->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static int si2168_init(struct dvb_frontend *fe)
+{
+	struct si2168 *s = fe->demodulator_priv;
+	int ret, len, remaining;
+	const struct firmware *fw = NULL;
+	u8 *fw_file = SI2168_FIRMWARE;
+	const unsigned int i2c_wr_max = 8;
+	struct si2168_cmd cmd;
+
+	dev_dbg(&s->client->dev, "%s:\n", __func__);
+
+	cmd.args[0] = 0x13;
+	cmd.wlen = 1;
+	cmd.rlen = 0;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	cmd.args[0] = 0xc0;
+	cmd.args[1] = 0x12;
+	cmd.args[2] = 0x00;
+	cmd.args[3] = 0x0c;
+	cmd.args[4] = 0x00;
+	cmd.args[5] = 0x0d;
+	cmd.args[6] = 0x16;
+	cmd.args[7] = 0x00;
+	cmd.args[8] = 0x00;
+	cmd.args[9] = 0x00;
+	cmd.args[10] = 0x00;
+	cmd.args[11] = 0x00;
+	cmd.args[12] = 0x00;
+	cmd.wlen = 13;
+	cmd.rlen = 0;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	cmd.args[0] = 0xc0;
+	cmd.args[1] = 0x06;
+	cmd.args[2] = 0x01;
+	cmd.args[3] = 0x0f;
+	cmd.args[4] = 0x00;
+	cmd.args[5] = 0x20;
+	cmd.args[6] = 0x20;
+	cmd.args[7] = 0x01;
+	cmd.wlen = 8;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	cmd.args[0] = 0x02;
+	cmd.wlen = 1;
+	cmd.rlen = 13;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	cmd.args[0] = 0x05;
+	cmd.args[1] = 0x00;
+	cmd.args[2] = 0xaa;
+	cmd.args[3] = 0x4d;
+	cmd.args[4] = 0x56;
+	cmd.args[5] = 0x40;
+	cmd.args[6] = 0x00;
+	cmd.args[7] = 0x00;
+	cmd.wlen = 8;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		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);
+
+	/* 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: firmare file '%s' not found\n",
+				KBUILD_MODNAME, fw_file);
+		goto err;
+	}
+
+	dev_info(&s->client->dev, "%s: downloading firmware from file '%s'\n",
+			KBUILD_MODNAME, fw_file);
+
+	for (remaining = fw->size; remaining > 0; remaining -= i2c_wr_max) {
+		len = remaining;
+		if (len > i2c_wr_max)
+			len = i2c_wr_max;
+
+		memcpy(cmd.args, &fw->data[fw->size - remaining], len);
+		cmd.wlen = len;
+		cmd.rlen = 1;
+		ret = si2168_cmd_execute(s, &cmd);
+		if (ret) {
+			dev_err(&s->client->dev,
+					"%s: firmware download failed=%d\n",
+					KBUILD_MODNAME, ret);
+			goto err;
+		}
+	}
+
+	release_firmware(fw);
+	fw = NULL;
+
+	cmd.args[0] = 0x01;
+	cmd.args[1] = 0x01;
+	cmd.wlen = 2;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	dev_info(&s->client->dev, "%s: found a '%s' in warm state\n",
+			KBUILD_MODNAME, si2168_ops.info.name);
+
+	s->active = true;
+
+	return 0;
+err:
+	if (fw)
+		release_firmware(fw);
+
+	dev_dbg(&s->client->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static int si2168_sleep(struct dvb_frontend *fe)
+{
+	struct si2168 *s = fe->demodulator_priv;
+
+	dev_dbg(&s->client->dev, "%s:\n", __func__);
+
+	s->active = false;
+
+	return 0;
+}
+
+static int si2168_get_tune_settings(struct dvb_frontend *fe,
+	struct dvb_frontend_tune_settings *s)
+{
+	s->min_delay_ms = 900;
+
+	return 0;
+}
+
+/*
+ * I2C gate logic
+ * We must use unlocked i2c_transfer() here because I2C lock is already taken
+ * by tuner driver.
+ */
+static int si2168_select(struct i2c_adapter *adap, void *mux_priv, u32 chan)
+{
+	struct si2168 *s = mux_priv;
+	int ret;
+	struct i2c_msg gate_open_msg = {
+		.addr = s->client->addr,
+		.flags = 0,
+		.len = 3,
+		.buf = "\xc0\x0d\x01",
+	};
+
+	mutex_lock(&s->i2c_mutex);
+
+	/* 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);
+		if (ret >= 0)
+			ret = -EREMOTEIO;
+	} else {
+		ret = 0;
+	}
+
+	return ret;
+}
+
+static int si2168_deselect(struct i2c_adapter *adap, void *mux_priv, u32 chan)
+{
+	struct si2168 *s = mux_priv;
+	int ret;
+	struct i2c_msg gate_close_msg = {
+		.addr = s->client->addr,
+		.flags = 0,
+		.len = 3,
+		.buf = "\xc0\x0d\x00",
+	};
+
+	/* 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);
+		if (ret >= 0)
+			ret = -EREMOTEIO;
+	} else {
+		ret = 0;
+	}
+
+	mutex_unlock(&s->i2c_mutex);
+
+	return ret;
+}
+
+static const struct dvb_frontend_ops si2168_ops = {
+	.delsys = {SYS_DVBT, SYS_DVBT2, SYS_DVBC_ANNEX_A},
+	.info = {
+		.name = "Silicon Labs Si2168",
+		.caps =	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_32 |
+			FE_CAN_QAM_64 |
+			FE_CAN_QAM_128 |
+			FE_CAN_QAM_256 |
+			FE_CAN_QAM_AUTO |
+			FE_CAN_TRANSMISSION_MODE_AUTO |
+			FE_CAN_GUARD_INTERVAL_AUTO |
+			FE_CAN_HIERARCHY_AUTO |
+			FE_CAN_MUTE_TS |
+			FE_CAN_2G_MODULATION
+	},
+
+	.get_tune_settings = si2168_get_tune_settings,
+
+	.init = si2168_init,
+	.sleep = si2168_sleep,
+
+	.set_frontend = si2168_set_frontend,
+
+	.read_status = si2168_read_status,
+};
+
+static int si2168_probe(struct i2c_client *client,
+		const struct i2c_device_id *id)
+{
+	struct si2168_config *config = client->dev.platform_data;
+	struct si2168 *s;
+	int ret;
+	struct si2168_cmd cmd;
+
+	dev_dbg(&client->dev, "%s:\n", __func__);
+
+	s = kzalloc(sizeof(struct si2168), GFP_KERNEL);
+	if (!s) {
+		ret = -ENOMEM;
+		dev_err(&client->dev, "%s: kzalloc() failed\n", KBUILD_MODNAME);
+		goto err;
+	}
+
+	s->client = client;
+	mutex_init(&s->i2c_mutex);
+
+	/* check if the demod is there */
+	cmd.wlen = 0;
+	cmd.rlen = 1;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	/* create mux i2c adapter for tuner */
+	s->adapter = i2c_add_mux_adapter(client->adapter, &client->dev, s,
+			0, 0, 0, si2168_select, si2168_deselect);
+	if (s->adapter == NULL)
+		goto err;
+
+	/* create dvb_frontend */
+	memcpy(&s->fe.ops, &si2168_ops, sizeof(struct dvb_frontend_ops));
+	s->fe.demodulator_priv = s;
+
+	*config->i2c_adapter = s->adapter;
+	*config->fe = &s->fe;
+
+	i2c_set_clientdata(client, s);
+
+	dev_info(&s->client->dev,
+			"%s: Silicon Labs Si2168 successfully attached\n",
+			KBUILD_MODNAME);
+	return 0;
+err:
+	kfree(s);
+	dev_dbg(&client->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static int si2168_remove(struct i2c_client *client)
+{
+	struct si2168 *s = i2c_get_clientdata(client);
+
+	dev_dbg(&client->dev, "%s:\n", __func__);
+
+	i2c_del_mux_adapter(s->adapter);
+
+	s->fe.ops.release = NULL;
+	s->fe.demodulator_priv = NULL;
+
+	kfree(s);
+
+	return 0;
+}
+
+static const struct i2c_device_id si2168_id[] = {
+	{"si2168", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, si2168_id);
+
+static struct i2c_driver si2168_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "si2168",
+	},
+	.probe		= si2168_probe,
+	.remove		= si2168_remove,
+	.id_table	= si2168_id,
+};
+
+module_i2c_driver(si2168_driver);
+
+MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
+MODULE_DESCRIPTION("Silicon Labs Si2168 DVB-T/T2/C demodulator driver");
+MODULE_LICENSE("GPL");
+MODULE_FIRMWARE(SI2168_FIRMWARE);
diff --git a/drivers/media/dvb-frontends/si2168.h b/drivers/media/dvb-frontends/si2168.h
new file mode 100644
index 00000000000..3c5b5ab0179
--- /dev/null
+++ b/drivers/media/dvb-frontends/si2168.h
@@ -0,0 +1,39 @@
+/*
+ * Silicon Labs Si2168 DVB-T/T2/C demodulator 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.
+ */
+
+#ifndef SI2168_H
+#define SI2168_H
+
+#include <linux/dvb/frontend.h>
+/*
+ * I2C address
+ * 0x64
+ */
+struct si2168_config {
+	/*
+	 * frontend
+	 * returned by driver
+	 */
+	struct dvb_frontend **fe;
+
+	/*
+	 * tuner I2C adapter
+	 * returned by driver
+	 */
+	struct i2c_adapter **i2c_adapter;
+};
+
+#endif
diff --git a/drivers/media/dvb-frontends/si2168_priv.h b/drivers/media/dvb-frontends/si2168_priv.h
new file mode 100644
index 00000000000..2a343e896f4
--- /dev/null
+++ b/drivers/media/dvb-frontends/si2168_priv.h
@@ -0,0 +1,46 @@
+/*
+ * Silicon Labs Si2168 DVB-T/T2/C demodulator 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.
+ */
+
+#ifndef SI2168_PRIV_H
+#define SI2168_PRIV_H
+
+#include "si2168.h"
+#include "dvb_frontend.h"
+#include <linux/firmware.h>
+#include <linux/i2c-mux.h>
+
+#define SI2168_FIRMWARE "dvb-demod-si2168-01.fw"
+
+/* state struct */
+struct si2168 {
+	struct i2c_client *client;
+	struct i2c_adapter *adapter;
+	struct mutex i2c_mutex;
+	struct dvb_frontend fe;
+	fe_delivery_system_t delivery_system;
+	fe_status_t fe_status;
+	bool active;
+};
+
+/* firmare command struct */
+#define SI2157_ARGLEN      30
+struct si2168_cmd {
+	u8 args[SI2157_ARGLEN];
+	unsigned wlen;
+	unsigned rlen;
+};
+
+#endif
diff --git a/drivers/media/i2c/ad9389b.c b/drivers/media/i2c/ad9389b.c
index 1b7ecfd8867..fada1756620 100644
--- a/drivers/media/i2c/ad9389b.c
+++ b/drivers/media/i2c/ad9389b.c
@@ -571,35 +571,6 @@ static const struct v4l2_subdev_core_ops ad9389b_core_ops = {
 	.interrupt_service_routine = ad9389b_isr,
 };
 
-/* ------------------------------ PAD OPS ------------------------------ */
-
-static int ad9389b_get_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
-{
-	struct ad9389b_state *state = get_ad9389b_state(sd);
-
-	if (edid->pad != 0)
-		return -EINVAL;
-	if (edid->blocks == 0 || edid->blocks > 256)
-		return -EINVAL;
-	if (!edid->edid)
-		return -EINVAL;
-	if (!state->edid.segments) {
-		v4l2_dbg(1, debug, sd, "EDID segment 0 not found\n");
-		return -ENODATA;
-	}
-	if (edid->start_block >= state->edid.segments * 2)
-		return -E2BIG;
-	if (edid->blocks + edid->start_block >= state->edid.segments * 2)
-		edid->blocks = state->edid.segments * 2 - edid->start_block;
-	memcpy(edid->edid, &state->edid.data[edid->start_block * 128],
-	       128 * edid->blocks);
-	return 0;
-}
-
-static const struct v4l2_subdev_pad_ops ad9389b_pad_ops = {
-	.get_edid = ad9389b_get_edid,
-};
-
 /* ------------------------------ VIDEO OPS ------------------------------ */
 
 /* Enable/disable ad9389b output */
@@ -678,6 +649,9 @@ static int ad9389b_g_dv_timings(struct v4l2_subdev *sd,
 static int ad9389b_enum_dv_timings(struct v4l2_subdev *sd,
 				   struct v4l2_enum_dv_timings *timings)
 {
+	if (timings->pad != 0)
+		return -EINVAL;
+
 	return v4l2_enum_dv_timings_cap(timings, &ad9389b_timings_cap,
 			NULL, NULL);
 }
@@ -685,6 +659,9 @@ static int ad9389b_enum_dv_timings(struct v4l2_subdev *sd,
 static int ad9389b_dv_timings_cap(struct v4l2_subdev *sd,
 				  struct v4l2_dv_timings_cap *cap)
 {
+	if (cap->pad != 0)
+		return -EINVAL;
+
 	*cap = ad9389b_timings_cap;
 	return 0;
 }
@@ -693,10 +670,39 @@ static const struct v4l2_subdev_video_ops ad9389b_video_ops = {
 	.s_stream = ad9389b_s_stream,
 	.s_dv_timings = ad9389b_s_dv_timings,
 	.g_dv_timings = ad9389b_g_dv_timings,
+};
+
+/* ------------------------------ PAD OPS ------------------------------ */
+
+static int ad9389b_get_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
+{
+	struct ad9389b_state *state = get_ad9389b_state(sd);
+
+	if (edid->pad != 0)
+		return -EINVAL;
+	if (edid->blocks == 0 || edid->blocks > 256)
+		return -EINVAL;
+	if (!state->edid.segments) {
+		v4l2_dbg(1, debug, sd, "EDID segment 0 not found\n");
+		return -ENODATA;
+	}
+	if (edid->start_block >= state->edid.segments * 2)
+		return -E2BIG;
+	if (edid->blocks + edid->start_block >= state->edid.segments * 2)
+		edid->blocks = state->edid.segments * 2 - edid->start_block;
+	memcpy(edid->edid, &state->edid.data[edid->start_block * 128],
+	       128 * edid->blocks);
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops ad9389b_pad_ops = {
+	.get_edid = ad9389b_get_edid,
 	.enum_dv_timings = ad9389b_enum_dv_timings,
 	.dv_timings_cap = ad9389b_dv_timings_cap,
 };
 
+/* ------------------------------ AUDIO OPS ------------------------------ */
+
 static int ad9389b_s_audio_stream(struct v4l2_subdev *sd, int enable)
 {
 	v4l2_dbg(1, debug, sd, "%s: %sable\n", __func__, (enable ? "en" : "dis"));
diff --git a/drivers/media/i2c/adv7180.c b/drivers/media/i2c/adv7180.c
index 5e638b15945..ac1cdbe251a 100644
--- a/drivers/media/i2c/adv7180.c
+++ b/drivers/media/i2c/adv7180.c
@@ -461,6 +461,7 @@ static int adv7180_g_mbus_config(struct v4l2_subdev *sd,
 }
 
 static const struct v4l2_subdev_video_ops adv7180_video_ops = {
+	.s_std = adv7180_s_std,
 	.querystd = adv7180_querystd,
 	.g_input_status = adv7180_g_input_status,
 	.s_routing = adv7180_s_routing,
@@ -472,7 +473,6 @@ static const struct v4l2_subdev_video_ops adv7180_video_ops = {
 };
 
 static const struct v4l2_subdev_core_ops adv7180_core_ops = {
-	.s_std = adv7180_s_std,
 	.s_power = adv7180_s_power,
 };
 
diff --git a/drivers/media/i2c/adv7183.c b/drivers/media/i2c/adv7183.c
index d45e0e3a781..df461b07b2f 100644
--- a/drivers/media/i2c/adv7183.c
+++ b/drivers/media/i2c/adv7183.c
@@ -501,8 +501,6 @@ static const struct v4l2_ctrl_ops adv7183_ctrl_ops = {
 
 static const struct v4l2_subdev_core_ops adv7183_core_ops = {
 	.log_status = adv7183_log_status,
-	.g_std = adv7183_g_std,
-	.s_std = adv7183_s_std,
 	.reset = adv7183_reset,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	.g_register = adv7183_g_register,
@@ -511,6 +509,8 @@ static const struct v4l2_subdev_core_ops adv7183_core_ops = {
 };
 
 static const struct v4l2_subdev_video_ops adv7183_video_ops = {
+	.g_std = adv7183_g_std,
+	.s_std = adv7183_s_std,
 	.s_routing = adv7183_s_routing,
 	.querystd = adv7183_querystd,
 	.g_input_status = adv7183_g_input_status,
diff --git a/drivers/media/i2c/adv7511.c b/drivers/media/i2c/adv7511.c
index 942ca4b9929..f98acf4aafd 100644
--- a/drivers/media/i2c/adv7511.c
+++ b/drivers/media/i2c/adv7511.c
@@ -597,34 +597,6 @@ static int adv7511_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
 	return 0;
 }
 
-static int adv7511_get_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
-{
-	struct adv7511_state *state = get_adv7511_state(sd);
-
-	if (edid->pad != 0)
-		return -EINVAL;
-	if ((edid->blocks == 0) || (edid->blocks > 256))
-		return -EINVAL;
-	if (!edid->edid)
-		return -EINVAL;
-	if (!state->edid.segments) {
-		v4l2_dbg(1, debug, sd, "EDID segment 0 not found\n");
-		return -ENODATA;
-	}
-	if (edid->start_block >= state->edid.segments * 2)
-		return -E2BIG;
-	if ((edid->blocks + edid->start_block) >= state->edid.segments * 2)
-		edid->blocks = state->edid.segments * 2 - edid->start_block;
-
-	memcpy(edid->edid, &state->edid.data[edid->start_block * 128],
-			128 * edid->blocks);
-	return 0;
-}
-
-static const struct v4l2_subdev_pad_ops adv7511_pad_ops = {
-	.get_edid = adv7511_get_edid,
-};
-
 static const struct v4l2_subdev_core_ops adv7511_core_ops = {
 	.log_status = adv7511_log_status,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
@@ -700,12 +672,18 @@ static int adv7511_g_dv_timings(struct v4l2_subdev *sd,
 static int adv7511_enum_dv_timings(struct v4l2_subdev *sd,
 				   struct v4l2_enum_dv_timings *timings)
 {
+	if (timings->pad != 0)
+		return -EINVAL;
+
 	return v4l2_enum_dv_timings_cap(timings, &adv7511_timings_cap, NULL, NULL);
 }
 
 static int adv7511_dv_timings_cap(struct v4l2_subdev *sd,
 				  struct v4l2_dv_timings_cap *cap)
 {
+	if (cap->pad != 0)
+		return -EINVAL;
+
 	*cap = adv7511_timings_cap;
 	return 0;
 }
@@ -714,8 +692,6 @@ static const struct v4l2_subdev_video_ops adv7511_video_ops = {
 	.s_stream = adv7511_s_stream,
 	.s_dv_timings = adv7511_s_dv_timings,
 	.g_dv_timings = adv7511_g_dv_timings,
-	.enum_dv_timings = adv7511_enum_dv_timings,
-	.dv_timings_cap = adv7511_dv_timings_cap,
 };
 
 /* ------------------------------ AUDIO OPS ------------------------------ */
@@ -797,6 +773,36 @@ static const struct v4l2_subdev_audio_ops adv7511_audio_ops = {
 	.s_routing = adv7511_s_routing,
 };
 
+/* ---------------------------- PAD OPS ------------------------------------- */
+
+static int adv7511_get_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+
+	if (edid->pad != 0)
+		return -EINVAL;
+	if ((edid->blocks == 0) || (edid->blocks > 256))
+		return -EINVAL;
+	if (!state->edid.segments) {
+		v4l2_dbg(1, debug, sd, "EDID segment 0 not found\n");
+		return -ENODATA;
+	}
+	if (edid->start_block >= state->edid.segments * 2)
+		return -E2BIG;
+	if ((edid->blocks + edid->start_block) >= state->edid.segments * 2)
+		edid->blocks = state->edid.segments * 2 - edid->start_block;
+
+	memcpy(edid->edid, &state->edid.data[edid->start_block * 128],
+			128 * edid->blocks);
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops adv7511_pad_ops = {
+	.get_edid = adv7511_get_edid,
+	.enum_dv_timings = adv7511_enum_dv_timings,
+	.dv_timings_cap = adv7511_dv_timings_cap,
+};
+
 /* --------------------- SUBDEV OPS --------------------------------------- */
 
 static const struct v4l2_subdev_ops adv7511_ops = {
diff --git a/drivers/media/i2c/adv7604.c b/drivers/media/i2c/adv7604.c
index 98cc5407f1b..1778d320272 100644
--- a/drivers/media/i2c/adv7604.c
+++ b/drivers/media/i2c/adv7604.c
@@ -27,19 +27,21 @@
  * REF_03 - Analog devices, ADV7604, Hardware Manual, Rev. F, August 2010
  */
 
-
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/slab.h>
-#include <linux/i2c.h>
-#include <linux/delay.h>
+#include <linux/v4l2-dv-timings.h>
 #include <linux/videodev2.h>
 #include <linux/workqueue.h>
-#include <linux/v4l2-dv-timings.h>
-#include <media/v4l2-device.h>
+
+#include <media/adv7604.h>
 #include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
 #include <media/v4l2-dv-timings.h>
-#include <media/adv7604.h>
+#include <media/v4l2-of.h>
 
 static int debug;
 module_param(debug, int, 0644);
@@ -53,6 +55,76 @@ MODULE_LICENSE("GPL");
 /* ADV7604 system clock frequency */
 #define ADV7604_fsc (28636360)
 
+#define ADV7604_RGB_OUT					(1 << 1)
+
+#define ADV7604_OP_FORMAT_SEL_8BIT			(0 << 0)
+#define ADV7604_OP_FORMAT_SEL_10BIT			(1 << 0)
+#define ADV7604_OP_FORMAT_SEL_12BIT			(2 << 0)
+
+#define ADV7604_OP_MODE_SEL_SDR_422			(0 << 5)
+#define ADV7604_OP_MODE_SEL_DDR_422			(1 << 5)
+#define ADV7604_OP_MODE_SEL_SDR_444			(2 << 5)
+#define ADV7604_OP_MODE_SEL_DDR_444			(3 << 5)
+#define ADV7604_OP_MODE_SEL_SDR_422_2X			(4 << 5)
+#define ADV7604_OP_MODE_SEL_ADI_CM			(5 << 5)
+
+#define ADV7604_OP_CH_SEL_GBR				(0 << 5)
+#define ADV7604_OP_CH_SEL_GRB				(1 << 5)
+#define ADV7604_OP_CH_SEL_BGR				(2 << 5)
+#define ADV7604_OP_CH_SEL_RGB				(3 << 5)
+#define ADV7604_OP_CH_SEL_BRG				(4 << 5)
+#define ADV7604_OP_CH_SEL_RBG				(5 << 5)
+
+#define ADV7604_OP_SWAP_CB_CR				(1 << 0)
+
+enum adv7604_type {
+	ADV7604,
+	ADV7611,
+};
+
+struct adv7604_reg_seq {
+	unsigned int reg;
+	u8 val;
+};
+
+struct adv7604_format_info {
+	enum v4l2_mbus_pixelcode code;
+	u8 op_ch_sel;
+	bool rgb_out;
+	bool swap_cb_cr;
+	u8 op_format_sel;
+};
+
+struct adv7604_chip_info {
+	enum adv7604_type type;
+
+	bool has_afe;
+	unsigned int max_port;
+	unsigned int num_dv_ports;
+
+	unsigned int edid_enable_reg;
+	unsigned int edid_status_reg;
+	unsigned int lcf_reg;
+
+	unsigned int cable_det_mask;
+	unsigned int tdms_lock_mask;
+	unsigned int fmt_change_digital_mask;
+
+	const struct adv7604_format_info *formats;
+	unsigned int nformats;
+
+	void (*set_termination)(struct v4l2_subdev *sd, bool enable);
+	void (*setup_irqs)(struct v4l2_subdev *sd);
+	unsigned int (*read_hdmi_pixelclock)(struct v4l2_subdev *sd);
+	unsigned int (*read_cable_det)(struct v4l2_subdev *sd);
+
+	/* 0 = AFE, 1 = HDMI */
+	const struct adv7604_reg_seq *recommended_settings[2];
+	unsigned int num_recommended_settings[2];
+
+	unsigned long page_mask;
+};
+
 /*
  **********************************************************************
  *
@@ -60,13 +132,24 @@ MODULE_LICENSE("GPL");
  *
  **********************************************************************
  */
+
 struct adv7604_state {
+	const struct adv7604_chip_info *info;
 	struct adv7604_platform_data pdata;
+
+	struct gpio_desc *hpd_gpio[4];
+
 	struct v4l2_subdev sd;
-	struct media_pad pad;
+	struct media_pad pads[ADV7604_PAD_MAX];
+	unsigned int source_pad;
+
 	struct v4l2_ctrl_handler hdl;
-	enum adv7604_input_port selected_input;
+
+	enum adv7604_pad selected_input;
+
 	struct v4l2_dv_timings timings;
+	const struct adv7604_format_info *format;
+
 	struct {
 		u8 edid[256];
 		u32 present;
@@ -80,18 +163,7 @@ struct adv7604_state {
 	bool restart_stdi_once;
 
 	/* i2c clients */
-	struct i2c_client *i2c_avlink;
-	struct i2c_client *i2c_cec;
-	struct i2c_client *i2c_infoframe;
-	struct i2c_client *i2c_esdp;
-	struct i2c_client *i2c_dpp;
-	struct i2c_client *i2c_afe;
-	struct i2c_client *i2c_repeater;
-	struct i2c_client *i2c_edid;
-	struct i2c_client *i2c_hdmi;
-	struct i2c_client *i2c_test;
-	struct i2c_client *i2c_cp;
-	struct i2c_client *i2c_vdp;
+	struct i2c_client *i2c_clients[ADV7604_PAGE_MAX];
 
 	/* controls */
 	struct v4l2_ctrl *detect_tx_5v_ctrl;
@@ -101,6 +173,11 @@ struct adv7604_state {
 	struct v4l2_ctrl *rgb_quantization_range_ctrl;
 };
 
+static bool adv7604_has_afe(struct adv7604_state *state)
+{
+	return state->info->has_afe;
+}
+
 /* Supported CEA and DMT timings */
 static const struct v4l2_dv_timings adv7604_timings[] = {
 	V4L2_DV_BT_CEA_720X480P59_94,
@@ -256,11 +333,6 @@ static inline struct adv7604_state *to_state(struct v4l2_subdev *sd)
 	return container_of(sd, struct adv7604_state, sd);
 }
 
-static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
-{
-	return &container_of(ctrl->handler, struct adv7604_state, hdl)->sd;
-}
-
 static inline unsigned hblanking(const struct v4l2_bt_timings *t)
 {
 	return V4L2_DV_BT_BLANKING_WIDTH(t);
@@ -298,14 +370,18 @@ static s32 adv_smbus_read_byte_data_check(struct i2c_client *client,
 	return -EIO;
 }
 
-static s32 adv_smbus_read_byte_data(struct i2c_client *client, u8 command)
+static s32 adv_smbus_read_byte_data(struct adv7604_state *state,
+				    enum adv7604_page page, u8 command)
 {
-	return adv_smbus_read_byte_data_check(client, command, true);
+	return adv_smbus_read_byte_data_check(state->i2c_clients[page],
+					      command, true);
 }
 
-static s32 adv_smbus_write_byte_data(struct i2c_client *client,
-					u8 command, u8 value)
+static s32 adv_smbus_write_byte_data(struct adv7604_state *state,
+				     enum adv7604_page page, u8 command,
+				     u8 value)
 {
+	struct i2c_client *client = state->i2c_clients[page];
 	union i2c_smbus_data data;
 	int err;
 	int i;
@@ -325,9 +401,11 @@ static s32 adv_smbus_write_byte_data(struct i2c_client *client,
 	return err;
 }
 
-static s32 adv_smbus_write_i2c_block_data(struct i2c_client *client,
-	       u8 command, unsigned length, const u8 *values)
+static s32 adv_smbus_write_i2c_block_data(struct adv7604_state *state,
+					  enum adv7604_page page, u8 command,
+					  unsigned length, const u8 *values)
 {
+	struct i2c_client *client = state->i2c_clients[page];
 	union i2c_smbus_data data;
 
 	if (length > I2C_SMBUS_BLOCK_MAX)
@@ -343,149 +421,150 @@ static s32 adv_smbus_write_i2c_block_data(struct i2c_client *client,
 
 static inline int io_read(struct v4l2_subdev *sd, u8 reg)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_read_byte_data(client, reg);
+	return adv_smbus_read_byte_data(state, ADV7604_PAGE_IO, reg);
 }
 
 static inline int io_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_write_byte_data(client, reg, val);
+	return adv_smbus_write_byte_data(state, ADV7604_PAGE_IO, reg, val);
 }
 
-static inline int io_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
+static inline int io_write_clr_set(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
 {
-	return io_write(sd, reg, (io_read(sd, reg) & mask) | val);
+	return io_write(sd, reg, (io_read(sd, reg) & ~mask) | val);
 }
 
 static inline int avlink_read(struct v4l2_subdev *sd, u8 reg)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_read_byte_data(state->i2c_avlink, reg);
+	return adv_smbus_read_byte_data(state, ADV7604_PAGE_AVLINK, reg);
 }
 
 static inline int avlink_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_write_byte_data(state->i2c_avlink, reg, val);
+	return adv_smbus_write_byte_data(state, ADV7604_PAGE_AVLINK, reg, val);
 }
 
 static inline int cec_read(struct v4l2_subdev *sd, u8 reg)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_read_byte_data(state->i2c_cec, reg);
+	return adv_smbus_read_byte_data(state, ADV7604_PAGE_CEC, reg);
 }
 
 static inline int cec_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_write_byte_data(state->i2c_cec, reg, val);
+	return adv_smbus_write_byte_data(state, ADV7604_PAGE_CEC, reg, val);
 }
 
-static inline int cec_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
+static inline int cec_write_clr_set(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
 {
-	return cec_write(sd, reg, (cec_read(sd, reg) & mask) | val);
+	return cec_write(sd, reg, (cec_read(sd, reg) & ~mask) | val);
 }
 
 static inline int infoframe_read(struct v4l2_subdev *sd, u8 reg)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_read_byte_data(state->i2c_infoframe, reg);
+	return adv_smbus_read_byte_data(state, ADV7604_PAGE_INFOFRAME, reg);
 }
 
 static inline int infoframe_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_write_byte_data(state->i2c_infoframe, reg, val);
+	return adv_smbus_write_byte_data(state, ADV7604_PAGE_INFOFRAME,
+					 reg, val);
 }
 
 static inline int esdp_read(struct v4l2_subdev *sd, u8 reg)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_read_byte_data(state->i2c_esdp, reg);
+	return adv_smbus_read_byte_data(state, ADV7604_PAGE_ESDP, reg);
 }
 
 static inline int esdp_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_write_byte_data(state->i2c_esdp, reg, val);
+	return adv_smbus_write_byte_data(state, ADV7604_PAGE_ESDP, reg, val);
 }
 
 static inline int dpp_read(struct v4l2_subdev *sd, u8 reg)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_read_byte_data(state->i2c_dpp, reg);
+	return adv_smbus_read_byte_data(state, ADV7604_PAGE_DPP, reg);
 }
 
 static inline int dpp_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_write_byte_data(state->i2c_dpp, reg, val);
+	return adv_smbus_write_byte_data(state, ADV7604_PAGE_DPP, reg, val);
 }
 
 static inline int afe_read(struct v4l2_subdev *sd, u8 reg)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_read_byte_data(state->i2c_afe, reg);
+	return adv_smbus_read_byte_data(state, ADV7604_PAGE_AFE, reg);
 }
 
 static inline int afe_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_write_byte_data(state->i2c_afe, reg, val);
+	return adv_smbus_write_byte_data(state, ADV7604_PAGE_AFE, reg, val);
 }
 
 static inline int rep_read(struct v4l2_subdev *sd, u8 reg)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_read_byte_data(state->i2c_repeater, reg);
+	return adv_smbus_read_byte_data(state, ADV7604_PAGE_REP, reg);
 }
 
 static inline int rep_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_write_byte_data(state->i2c_repeater, reg, val);
+	return adv_smbus_write_byte_data(state, ADV7604_PAGE_REP, reg, val);
 }
 
-static inline int rep_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
+static inline int rep_write_clr_set(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
 {
-	return rep_write(sd, reg, (rep_read(sd, reg) & mask) | val);
+	return rep_write(sd, reg, (rep_read(sd, reg) & ~mask) | val);
 }
 
 static inline int edid_read(struct v4l2_subdev *sd, u8 reg)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_read_byte_data(state->i2c_edid, reg);
+	return adv_smbus_read_byte_data(state, ADV7604_PAGE_EDID, reg);
 }
 
 static inline int edid_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_write_byte_data(state->i2c_edid, reg, val);
+	return adv_smbus_write_byte_data(state, ADV7604_PAGE_EDID, reg, val);
 }
 
 static inline int edid_read_block(struct v4l2_subdev *sd, unsigned len, u8 *val)
 {
 	struct adv7604_state *state = to_state(sd);
-	struct i2c_client *client = state->i2c_edid;
+	struct i2c_client *client = state->i2c_clients[ADV7604_PAGE_EDID];
 	u8 msgbuf0[1] = { 0 };
 	u8 msgbuf1[256];
 	struct i2c_msg msg[2] = {
@@ -518,11 +597,25 @@ static inline int edid_write_block(struct v4l2_subdev *sd,
 	v4l2_dbg(2, debug, sd, "%s: write EDID block (%d byte)\n", __func__, len);
 
 	for (i = 0; !err && i < len; i += I2C_SMBUS_BLOCK_MAX)
-		err = adv_smbus_write_i2c_block_data(state->i2c_edid, i,
-				I2C_SMBUS_BLOCK_MAX, val + i);
+		err = adv_smbus_write_i2c_block_data(state, ADV7604_PAGE_EDID,
+				i, I2C_SMBUS_BLOCK_MAX, val + i);
 	return err;
 }
 
+static void adv7604_set_hpd(struct adv7604_state *state, unsigned int hpd)
+{
+	unsigned int i;
+
+	for (i = 0; i < state->info->num_dv_ports; ++i) {
+		if (IS_ERR(state->hpd_gpio[i]))
+			continue;
+
+		gpiod_set_value_cansleep(state->hpd_gpio[i], hpd & BIT(i));
+	}
+
+	v4l2_subdev_notify(&state->sd, ADV7604_HOTPLUG, &hpd);
+}
+
 static void adv7604_delayed_work_enable_hotplug(struct work_struct *work)
 {
 	struct delayed_work *dwork = to_delayed_work(work);
@@ -532,73 +625,210 @@ static void adv7604_delayed_work_enable_hotplug(struct work_struct *work)
 
 	v4l2_dbg(2, debug, sd, "%s: enable hotplug\n", __func__);
 
-	v4l2_subdev_notify(sd, ADV7604_HOTPLUG, (void *)&state->edid.present);
+	adv7604_set_hpd(state, state->edid.present);
 }
 
 static inline int hdmi_read(struct v4l2_subdev *sd, u8 reg)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_read_byte_data(state->i2c_hdmi, reg);
+	return adv_smbus_read_byte_data(state, ADV7604_PAGE_HDMI, reg);
+}
+
+static u16 hdmi_read16(struct v4l2_subdev *sd, u8 reg, u16 mask)
+{
+	return ((hdmi_read(sd, reg) << 8) | hdmi_read(sd, reg + 1)) & mask;
 }
 
 static inline int hdmi_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_write_byte_data(state->i2c_hdmi, reg, val);
+	return adv_smbus_write_byte_data(state, ADV7604_PAGE_HDMI, reg, val);
 }
 
-static inline int hdmi_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
+static inline int hdmi_write_clr_set(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
 {
-	return hdmi_write(sd, reg, (hdmi_read(sd, reg) & mask) | val);
+	return hdmi_write(sd, reg, (hdmi_read(sd, reg) & ~mask) | val);
 }
 
 static inline int test_read(struct v4l2_subdev *sd, u8 reg)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_read_byte_data(state->i2c_test, reg);
+	return adv_smbus_read_byte_data(state, ADV7604_PAGE_TEST, reg);
 }
 
 static inline int test_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_write_byte_data(state->i2c_test, reg, val);
+	return adv_smbus_write_byte_data(state, ADV7604_PAGE_TEST, reg, val);
 }
 
 static inline int cp_read(struct v4l2_subdev *sd, u8 reg)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_read_byte_data(state->i2c_cp, reg);
+	return adv_smbus_read_byte_data(state, ADV7604_PAGE_CP, reg);
+}
+
+static u16 cp_read16(struct v4l2_subdev *sd, u8 reg, u16 mask)
+{
+	return ((cp_read(sd, reg) << 8) | cp_read(sd, reg + 1)) & mask;
 }
 
 static inline int cp_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_write_byte_data(state->i2c_cp, reg, val);
+	return adv_smbus_write_byte_data(state, ADV7604_PAGE_CP, reg, val);
 }
 
-static inline int cp_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
+static inline int cp_write_clr_set(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
 {
-	return cp_write(sd, reg, (cp_read(sd, reg) & mask) | val);
+	return cp_write(sd, reg, (cp_read(sd, reg) & ~mask) | val);
 }
 
 static inline int vdp_read(struct v4l2_subdev *sd, u8 reg)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_read_byte_data(state->i2c_vdp, reg);
+	return adv_smbus_read_byte_data(state, ADV7604_PAGE_VDP, reg);
 }
 
 static inline int vdp_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return adv_smbus_write_byte_data(state->i2c_vdp, reg, val);
+	return adv_smbus_write_byte_data(state, ADV7604_PAGE_VDP, reg, val);
+}
+
+#define ADV7604_REG(page, offset)	(((page) << 8) | (offset))
+#define ADV7604_REG_SEQ_TERM		0xffff
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int adv7604_read_reg(struct v4l2_subdev *sd, unsigned int reg)
+{
+	struct adv7604_state *state = to_state(sd);
+	unsigned int page = reg >> 8;
+
+	if (!(BIT(page) & state->info->page_mask))
+		return -EINVAL;
+
+	reg &= 0xff;
+
+	return adv_smbus_read_byte_data(state, page, reg);
+}
+#endif
+
+static int adv7604_write_reg(struct v4l2_subdev *sd, unsigned int reg, u8 val)
+{
+	struct adv7604_state *state = to_state(sd);
+	unsigned int page = reg >> 8;
+
+	if (!(BIT(page) & state->info->page_mask))
+		return -EINVAL;
+
+	reg &= 0xff;
+
+	return adv_smbus_write_byte_data(state, page, reg, val);
+}
+
+static void adv7604_write_reg_seq(struct v4l2_subdev *sd,
+				  const struct adv7604_reg_seq *reg_seq)
+{
+	unsigned int i;
+
+	for (i = 0; reg_seq[i].reg != ADV7604_REG_SEQ_TERM; i++)
+		adv7604_write_reg(sd, reg_seq[i].reg, reg_seq[i].val);
+}
+
+/* -----------------------------------------------------------------------------
+ * Format helpers
+ */
+
+static const struct adv7604_format_info adv7604_formats[] = {
+	{ V4L2_MBUS_FMT_RGB888_1X24, ADV7604_OP_CH_SEL_RGB, true, false,
+	  ADV7604_OP_MODE_SEL_SDR_444 | ADV7604_OP_FORMAT_SEL_8BIT },
+	{ V4L2_MBUS_FMT_YUYV8_2X8, ADV7604_OP_CH_SEL_RGB, false, false,
+	  ADV7604_OP_MODE_SEL_SDR_422 | ADV7604_OP_FORMAT_SEL_8BIT },
+	{ V4L2_MBUS_FMT_YVYU8_2X8, ADV7604_OP_CH_SEL_RGB, false, true,
+	  ADV7604_OP_MODE_SEL_SDR_422 | ADV7604_OP_FORMAT_SEL_8BIT },
+	{ V4L2_MBUS_FMT_YUYV10_2X10, ADV7604_OP_CH_SEL_RGB, false, false,
+	  ADV7604_OP_MODE_SEL_SDR_422 | ADV7604_OP_FORMAT_SEL_10BIT },
+	{ V4L2_MBUS_FMT_YVYU10_2X10, ADV7604_OP_CH_SEL_RGB, false, true,
+	  ADV7604_OP_MODE_SEL_SDR_422 | ADV7604_OP_FORMAT_SEL_10BIT },
+	{ V4L2_MBUS_FMT_YUYV12_2X12, ADV7604_OP_CH_SEL_RGB, false, false,
+	  ADV7604_OP_MODE_SEL_SDR_422 | ADV7604_OP_FORMAT_SEL_12BIT },
+	{ V4L2_MBUS_FMT_YVYU12_2X12, ADV7604_OP_CH_SEL_RGB, false, true,
+	  ADV7604_OP_MODE_SEL_SDR_422 | ADV7604_OP_FORMAT_SEL_12BIT },
+	{ V4L2_MBUS_FMT_UYVY8_1X16, ADV7604_OP_CH_SEL_RBG, false, false,
+	  ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_8BIT },
+	{ V4L2_MBUS_FMT_VYUY8_1X16, ADV7604_OP_CH_SEL_RBG, false, true,
+	  ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_8BIT },
+	{ V4L2_MBUS_FMT_YUYV8_1X16, ADV7604_OP_CH_SEL_RGB, false, false,
+	  ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_8BIT },
+	{ V4L2_MBUS_FMT_YVYU8_1X16, ADV7604_OP_CH_SEL_RGB, false, true,
+	  ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_8BIT },
+	{ V4L2_MBUS_FMT_UYVY10_1X20, ADV7604_OP_CH_SEL_RBG, false, false,
+	  ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_10BIT },
+	{ V4L2_MBUS_FMT_VYUY10_1X20, ADV7604_OP_CH_SEL_RBG, false, true,
+	  ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_10BIT },
+	{ V4L2_MBUS_FMT_YUYV10_1X20, ADV7604_OP_CH_SEL_RGB, false, false,
+	  ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_10BIT },
+	{ V4L2_MBUS_FMT_YVYU10_1X20, ADV7604_OP_CH_SEL_RGB, false, true,
+	  ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_10BIT },
+	{ V4L2_MBUS_FMT_UYVY12_1X24, ADV7604_OP_CH_SEL_RBG, false, false,
+	  ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_12BIT },
+	{ V4L2_MBUS_FMT_VYUY12_1X24, ADV7604_OP_CH_SEL_RBG, false, true,
+	  ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_12BIT },
+	{ V4L2_MBUS_FMT_YUYV12_1X24, ADV7604_OP_CH_SEL_RGB, false, false,
+	  ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_12BIT },
+	{ V4L2_MBUS_FMT_YVYU12_1X24, ADV7604_OP_CH_SEL_RGB, false, true,
+	  ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_12BIT },
+};
+
+static const struct adv7604_format_info adv7611_formats[] = {
+	{ V4L2_MBUS_FMT_RGB888_1X24, ADV7604_OP_CH_SEL_RGB, true, false,
+	  ADV7604_OP_MODE_SEL_SDR_444 | ADV7604_OP_FORMAT_SEL_8BIT },
+	{ V4L2_MBUS_FMT_YUYV8_2X8, ADV7604_OP_CH_SEL_RGB, false, false,
+	  ADV7604_OP_MODE_SEL_SDR_422 | ADV7604_OP_FORMAT_SEL_8BIT },
+	{ V4L2_MBUS_FMT_YVYU8_2X8, ADV7604_OP_CH_SEL_RGB, false, true,
+	  ADV7604_OP_MODE_SEL_SDR_422 | ADV7604_OP_FORMAT_SEL_8BIT },
+	{ V4L2_MBUS_FMT_YUYV12_2X12, ADV7604_OP_CH_SEL_RGB, false, false,
+	  ADV7604_OP_MODE_SEL_SDR_422 | ADV7604_OP_FORMAT_SEL_12BIT },
+	{ V4L2_MBUS_FMT_YVYU12_2X12, ADV7604_OP_CH_SEL_RGB, false, true,
+	  ADV7604_OP_MODE_SEL_SDR_422 | ADV7604_OP_FORMAT_SEL_12BIT },
+	{ V4L2_MBUS_FMT_UYVY8_1X16, ADV7604_OP_CH_SEL_RBG, false, false,
+	  ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_8BIT },
+	{ V4L2_MBUS_FMT_VYUY8_1X16, ADV7604_OP_CH_SEL_RBG, false, true,
+	  ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_8BIT },
+	{ V4L2_MBUS_FMT_YUYV8_1X16, ADV7604_OP_CH_SEL_RGB, false, false,
+	  ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_8BIT },
+	{ V4L2_MBUS_FMT_YVYU8_1X16, ADV7604_OP_CH_SEL_RGB, false, true,
+	  ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_8BIT },
+	{ V4L2_MBUS_FMT_UYVY12_1X24, ADV7604_OP_CH_SEL_RBG, false, false,
+	  ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_12BIT },
+	{ V4L2_MBUS_FMT_VYUY12_1X24, ADV7604_OP_CH_SEL_RBG, false, true,
+	  ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_12BIT },
+	{ V4L2_MBUS_FMT_YUYV12_1X24, ADV7604_OP_CH_SEL_RGB, false, false,
+	  ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_12BIT },
+	{ V4L2_MBUS_FMT_YVYU12_1X24, ADV7604_OP_CH_SEL_RGB, false, true,
+	  ADV7604_OP_MODE_SEL_SDR_422_2X | ADV7604_OP_FORMAT_SEL_12BIT },
+};
+
+static const struct adv7604_format_info *
+adv7604_format_info(struct adv7604_state *state, enum v4l2_mbus_pixelcode code)
+{
+	unsigned int i;
+
+	for (i = 0; i < state->info->nformats; ++i) {
+		if (state->info->formats[i].code == code)
+			return &state->info->formats[i];
+	}
+
+	return NULL;
 }
 
 /* ----------------------------------------------------------------------- */
@@ -607,18 +837,18 @@ static inline bool is_analog_input(struct v4l2_subdev *sd)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return state->selected_input == ADV7604_INPUT_VGA_RGB ||
-	       state->selected_input == ADV7604_INPUT_VGA_COMP;
+	return state->selected_input == ADV7604_PAD_VGA_RGB ||
+	       state->selected_input == ADV7604_PAD_VGA_COMP;
 }
 
 static inline bool is_digital_input(struct v4l2_subdev *sd)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	return state->selected_input == ADV7604_INPUT_HDMI_PORT_A ||
-	       state->selected_input == ADV7604_INPUT_HDMI_PORT_B ||
-	       state->selected_input == ADV7604_INPUT_HDMI_PORT_C ||
-	       state->selected_input == ADV7604_INPUT_HDMI_PORT_D;
+	return state->selected_input == ADV7604_PAD_HDMI_PORT_A ||
+	       state->selected_input == ADV7604_PAD_HDMI_PORT_B ||
+	       state->selected_input == ADV7604_PAD_HDMI_PORT_C ||
+	       state->selected_input == ADV7604_PAD_HDMI_PORT_D;
 }
 
 /* ----------------------------------------------------------------------- */
@@ -644,119 +874,61 @@ static void adv7604_inv_register(struct v4l2_subdev *sd)
 static int adv7604_g_register(struct v4l2_subdev *sd,
 					struct v4l2_dbg_register *reg)
 {
-	reg->size = 1;
-	switch (reg->reg >> 8) {
-	case 0:
-		reg->val = io_read(sd, reg->reg & 0xff);
-		break;
-	case 1:
-		reg->val = avlink_read(sd, reg->reg & 0xff);
-		break;
-	case 2:
-		reg->val = cec_read(sd, reg->reg & 0xff);
-		break;
-	case 3:
-		reg->val = infoframe_read(sd, reg->reg & 0xff);
-		break;
-	case 4:
-		reg->val = esdp_read(sd, reg->reg & 0xff);
-		break;
-	case 5:
-		reg->val = dpp_read(sd, reg->reg & 0xff);
-		break;
-	case 6:
-		reg->val = afe_read(sd, reg->reg & 0xff);
-		break;
-	case 7:
-		reg->val = rep_read(sd, reg->reg & 0xff);
-		break;
-	case 8:
-		reg->val = edid_read(sd, reg->reg & 0xff);
-		break;
-	case 9:
-		reg->val = hdmi_read(sd, reg->reg & 0xff);
-		break;
-	case 0xa:
-		reg->val = test_read(sd, reg->reg & 0xff);
-		break;
-	case 0xb:
-		reg->val = cp_read(sd, reg->reg & 0xff);
-		break;
-	case 0xc:
-		reg->val = vdp_read(sd, reg->reg & 0xff);
-		break;
-	default:
+	int ret;
+
+	ret = adv7604_read_reg(sd, reg->reg);
+	if (ret < 0) {
 		v4l2_info(sd, "Register %03llx not supported\n", reg->reg);
 		adv7604_inv_register(sd);
-		break;
+		return ret;
 	}
+
+	reg->size = 1;
+	reg->val = ret;
+
 	return 0;
 }
 
 static int adv7604_s_register(struct v4l2_subdev *sd,
 					const struct v4l2_dbg_register *reg)
 {
-	u8 val = reg->val & 0xff;
+	int ret;
 
-	switch (reg->reg >> 8) {
-	case 0:
-		io_write(sd, reg->reg & 0xff, val);
-		break;
-	case 1:
-		avlink_write(sd, reg->reg & 0xff, val);
-		break;
-	case 2:
-		cec_write(sd, reg->reg & 0xff, val);
-		break;
-	case 3:
-		infoframe_write(sd, reg->reg & 0xff, val);
-		break;
-	case 4:
-		esdp_write(sd, reg->reg & 0xff, val);
-		break;
-	case 5:
-		dpp_write(sd, reg->reg & 0xff, val);
-		break;
-	case 6:
-		afe_write(sd, reg->reg & 0xff, val);
-		break;
-	case 7:
-		rep_write(sd, reg->reg & 0xff, val);
-		break;
-	case 8:
-		edid_write(sd, reg->reg & 0xff, val);
-		break;
-	case 9:
-		hdmi_write(sd, reg->reg & 0xff, val);
-		break;
-	case 0xa:
-		test_write(sd, reg->reg & 0xff, val);
-		break;
-	case 0xb:
-		cp_write(sd, reg->reg & 0xff, val);
-		break;
-	case 0xc:
-		vdp_write(sd, reg->reg & 0xff, val);
-		break;
-	default:
+	ret = adv7604_write_reg(sd, reg->reg, reg->val);
+	if (ret < 0) {
 		v4l2_info(sd, "Register %03llx not supported\n", reg->reg);
 		adv7604_inv_register(sd);
-		break;
+		return ret;
 	}
+
 	return 0;
 }
 #endif
 
+static unsigned int adv7604_read_cable_det(struct v4l2_subdev *sd)
+{
+	u8 value = io_read(sd, 0x6f);
+
+	return ((value & 0x10) >> 4)
+	     | ((value & 0x08) >> 2)
+	     | ((value & 0x04) << 0)
+	     | ((value & 0x02) << 2);
+}
+
+static unsigned int adv7611_read_cable_det(struct v4l2_subdev *sd)
+{
+	u8 value = io_read(sd, 0x6f);
+
+	return value & 1;
+}
+
 static int adv7604_s_detect_tx_5v_ctrl(struct v4l2_subdev *sd)
 {
 	struct adv7604_state *state = to_state(sd);
-	u8 reg_io_6f = io_read(sd, 0x6f);
+	const struct adv7604_chip_info *info = state->info;
 
 	return v4l2_ctrl_s_ctrl(state->detect_tx_5v_ctrl,
-			((reg_io_6f & 0x10) >> 4) |
-			((reg_io_6f & 0x08) >> 2) |
-			(reg_io_6f & 0x04) |
-			((reg_io_6f & 0x02) << 2));
+				info->read_cable_det(sd));
 }
 
 static int find_and_set_predefined_video_timings(struct v4l2_subdev *sd,
@@ -787,11 +959,13 @@ static int configure_predefined_video_timings(struct v4l2_subdev *sd,
 
 	v4l2_dbg(1, debug, sd, "%s", __func__);
 
-	/* reset to default values */
-	io_write(sd, 0x16, 0x43);
-	io_write(sd, 0x17, 0x5a);
+	if (adv7604_has_afe(state)) {
+		/* reset to default values */
+		io_write(sd, 0x16, 0x43);
+		io_write(sd, 0x17, 0x5a);
+	}
 	/* disable embedded syncs for auto graphics mode */
-	cp_write_and_or(sd, 0x81, 0xef, 0x00);
+	cp_write_clr_set(sd, 0x81, 0x10, 0x00);
 	cp_write(sd, 0x8f, 0x00);
 	cp_write(sd, 0x90, 0x00);
 	cp_write(sd, 0xa2, 0x00);
@@ -829,7 +1003,6 @@ static void configure_custom_video_timings(struct v4l2_subdev *sd,
 		const struct v4l2_bt_timings *bt)
 {
 	struct adv7604_state *state = to_state(sd);
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	u32 width = htotal(bt);
 	u32 height = vtotal(bt);
 	u16 cp_start_sav = bt->hsync + bt->hbackporch - 4;
@@ -850,12 +1023,13 @@ static void configure_custom_video_timings(struct v4l2_subdev *sd,
 		io_write(sd, 0x00, 0x07); /* video std */
 		io_write(sd, 0x01, 0x02); /* prim mode */
 		/* enable embedded syncs for auto graphics mode */
-		cp_write_and_or(sd, 0x81, 0xef, 0x10);
+		cp_write_clr_set(sd, 0x81, 0x10, 0x10);
 
 		/* Should only be set in auto-graphics mode [REF_02, p. 91-92] */
 		/* setup PLL_DIV_MAN_EN and PLL_DIV_RATIO */
 		/* IO-map reg. 0x16 and 0x17 should be written in sequence */
-		if (adv_smbus_write_i2c_block_data(client, 0x16, 2, pll))
+		if (adv_smbus_write_i2c_block_data(state, ADV7604_PAGE_IO,
+						   0x16, 2, pll))
 			v4l2_err(sd, "writing to reg 0x16 and 0x17 failed\n");
 
 		/* active video - horizontal timing */
@@ -906,7 +1080,8 @@ static void adv7604_set_offset(struct v4l2_subdev *sd, bool auto_offset, u16 off
 	offset_buf[3] = offset_c & 0x0ff;
 
 	/* Registers must be written in this order with no i2c access in between */
-	if (adv_smbus_write_i2c_block_data(state->i2c_cp, 0x77, 4, offset_buf))
+	if (adv_smbus_write_i2c_block_data(state, ADV7604_PAGE_CP,
+					   0x77, 4, offset_buf))
 		v4l2_err(sd, "%s: i2c error writing to CP reg 0x77, 0x78, 0x79, 0x7a\n", __func__);
 }
 
@@ -935,7 +1110,8 @@ static void adv7604_set_gain(struct v4l2_subdev *sd, bool auto_gain, u16 gain_a,
 	gain_buf[3] = ((gain_c & 0x0ff));
 
 	/* Registers must be written in this order with no i2c access in between */
-	if (adv_smbus_write_i2c_block_data(state->i2c_cp, 0x73, 4, gain_buf))
+	if (adv_smbus_write_i2c_block_data(state, ADV7604_PAGE_CP,
+					   0x73, 4, gain_buf))
 		v4l2_err(sd, "%s: i2c error writing to CP reg 0x73, 0x74, 0x75, 0x76\n", __func__);
 }
 
@@ -954,24 +1130,24 @@ static void set_rgb_quantization_range(struct v4l2_subdev *sd)
 
 	switch (state->rgb_quantization_range) {
 	case V4L2_DV_RGB_RANGE_AUTO:
-		if (state->selected_input == ADV7604_INPUT_VGA_RGB) {
+		if (state->selected_input == ADV7604_PAD_VGA_RGB) {
 			/* Receiving analog RGB signal
 			 * Set RGB full range (0-255) */
-			io_write_and_or(sd, 0x02, 0x0f, 0x10);
+			io_write_clr_set(sd, 0x02, 0xf0, 0x10);
 			break;
 		}
 
-		if (state->selected_input == ADV7604_INPUT_VGA_COMP) {
+		if (state->selected_input == ADV7604_PAD_VGA_COMP) {
 			/* Receiving analog YPbPr signal
 			 * Set automode */
-			io_write_and_or(sd, 0x02, 0x0f, 0xf0);
+			io_write_clr_set(sd, 0x02, 0xf0, 0xf0);
 			break;
 		}
 
 		if (hdmi_signal) {
 			/* Receiving HDMI signal
 			 * Set automode */
-			io_write_and_or(sd, 0x02, 0x0f, 0xf0);
+			io_write_clr_set(sd, 0x02, 0xf0, 0xf0);
 			break;
 		}
 
@@ -980,10 +1156,10 @@ static void set_rgb_quantization_range(struct v4l2_subdev *sd)
 		 * input format (CE/IT) in automatic mode */
 		if (state->timings.bt.standards & V4L2_DV_BT_STD_CEA861) {
 			/* RGB limited range (16-235) */
-			io_write_and_or(sd, 0x02, 0x0f, 0x00);
+			io_write_clr_set(sd, 0x02, 0xf0, 0x00);
 		} else {
 			/* RGB full range (0-255) */
-			io_write_and_or(sd, 0x02, 0x0f, 0x10);
+			io_write_clr_set(sd, 0x02, 0xf0, 0x10);
 
 			if (is_digital_input(sd) && rgb_output) {
 				adv7604_set_offset(sd, false, 0x40, 0x40, 0x40);
@@ -994,25 +1170,25 @@ static void set_rgb_quantization_range(struct v4l2_subdev *sd)
 		}
 		break;
 	case V4L2_DV_RGB_RANGE_LIMITED:
-		if (state->selected_input == ADV7604_INPUT_VGA_COMP) {
+		if (state->selected_input == ADV7604_PAD_VGA_COMP) {
 			/* YCrCb limited range (16-235) */
-			io_write_and_or(sd, 0x02, 0x0f, 0x20);
+			io_write_clr_set(sd, 0x02, 0xf0, 0x20);
 			break;
 		}
 
 		/* RGB limited range (16-235) */
-		io_write_and_or(sd, 0x02, 0x0f, 0x00);
+		io_write_clr_set(sd, 0x02, 0xf0, 0x00);
 
 		break;
 	case V4L2_DV_RGB_RANGE_FULL:
-		if (state->selected_input == ADV7604_INPUT_VGA_COMP) {
+		if (state->selected_input == ADV7604_PAD_VGA_COMP) {
 			/* YCrCb full range (0-255) */
-			io_write_and_or(sd, 0x02, 0x0f, 0x60);
+			io_write_clr_set(sd, 0x02, 0xf0, 0x60);
 			break;
 		}
 
 		/* RGB full range (0-255) */
-		io_write_and_or(sd, 0x02, 0x0f, 0x10);
+		io_write_clr_set(sd, 0x02, 0xf0, 0x10);
 
 		if (is_analog_input(sd) || hdmi_signal)
 			break;
@@ -1030,7 +1206,9 @@ static void set_rgb_quantization_range(struct v4l2_subdev *sd)
 
 static int adv7604_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct v4l2_subdev *sd =
+		&container_of(ctrl->handler, struct adv7604_state, hdl)->sd;
+
 	struct adv7604_state *state = to_state(sd);
 
 	switch (ctrl->id) {
@@ -1051,6 +1229,8 @@ static int adv7604_s_ctrl(struct v4l2_ctrl *ctrl)
 		set_rgb_quantization_range(sd);
 		return 0;
 	case V4L2_CID_ADV_RX_ANALOG_SAMPLING_PHASE:
+		if (!adv7604_has_afe(state))
+			return -EINVAL;
 		/* Set the analog sampling phase. This is needed to find the
 		   best sampling phase for analog video: an application or
 		   driver has to try a number of phases and analyze the picture
@@ -1060,7 +1240,7 @@ static int adv7604_s_ctrl(struct v4l2_ctrl *ctrl)
 	case V4L2_CID_ADV_RX_FREE_RUN_COLOR_MANUAL:
 		/* Use the default blue color for free running mode,
 		   or supply your own. */
-		cp_write_and_or(sd, 0xbf, ~0x04, (ctrl->val << 2));
+		cp_write_clr_set(sd, 0xbf, 0x04, ctrl->val << 2);
 		return 0;
 	case V4L2_CID_ADV_RX_FREE_RUN_COLOR:
 		cp_write(sd, 0xc0, (ctrl->val & 0xff0000) >> 16);
@@ -1088,7 +1268,10 @@ static inline bool no_signal_tmds(struct v4l2_subdev *sd)
 
 static inline bool no_lock_tmds(struct v4l2_subdev *sd)
 {
-	return (io_read(sd, 0x6a) & 0xe0) != 0xe0;
+	struct adv7604_state *state = to_state(sd);
+	const struct adv7604_chip_info *info = state->info;
+
+	return (io_read(sd, 0x6a) & info->tdms_lock_mask) != info->tdms_lock_mask;
 }
 
 static inline bool is_hdmi(struct v4l2_subdev *sd)
@@ -1098,6 +1281,15 @@ static inline bool is_hdmi(struct v4l2_subdev *sd)
 
 static inline bool no_lock_sspd(struct v4l2_subdev *sd)
 {
+	struct adv7604_state *state = to_state(sd);
+
+	/*
+	 * Chips without a AFE don't expose registers for the SSPD, so just assume
+	 * that we have a lock.
+	 */
+	if (adv7604_has_afe(state))
+		return false;
+
 	/* TODO channel 2 */
 	return ((cp_read(sd, 0xb5) & 0xd0) != 0xd0);
 }
@@ -1127,6 +1319,11 @@ static inline bool no_signal(struct v4l2_subdev *sd)
 
 static inline bool no_lock_cp(struct v4l2_subdev *sd)
 {
+	struct adv7604_state *state = to_state(sd);
+
+	if (!adv7604_has_afe(state))
+		return false;
+
 	/* CP has detected a non standard number of lines on the incoming
 	   video compared to what it is configured to receive by s_dv_timings */
 	return io_read(sd, 0x12) & 0x01;
@@ -1195,28 +1392,40 @@ static int stdi2dv_timings(struct v4l2_subdev *sd,
 	return -1;
 }
 
+
 static int read_stdi(struct v4l2_subdev *sd, struct stdi_readback *stdi)
 {
+	struct adv7604_state *state = to_state(sd);
+	const struct adv7604_chip_info *info = state->info;
+	u8 polarity;
+
 	if (no_lock_stdi(sd) || no_lock_sspd(sd)) {
 		v4l2_dbg(2, debug, sd, "%s: STDI and/or SSPD not locked\n", __func__);
 		return -1;
 	}
 
 	/* read STDI */
-	stdi->bl = ((cp_read(sd, 0xb1) & 0x3f) << 8) | cp_read(sd, 0xb2);
-	stdi->lcf = ((cp_read(sd, 0xb3) & 0x7) << 8) | cp_read(sd, 0xb4);
+	stdi->bl = cp_read16(sd, 0xb1, 0x3fff);
+	stdi->lcf = cp_read16(sd, info->lcf_reg, 0x7ff);
 	stdi->lcvs = cp_read(sd, 0xb3) >> 3;
 	stdi->interlaced = io_read(sd, 0x12) & 0x10;
 
-	/* read SSPD */
-	if ((cp_read(sd, 0xb5) & 0x03) == 0x01) {
-		stdi->hs_pol = ((cp_read(sd, 0xb5) & 0x10) ?
-				((cp_read(sd, 0xb5) & 0x08) ? '+' : '-') : 'x');
-		stdi->vs_pol = ((cp_read(sd, 0xb5) & 0x40) ?
-				((cp_read(sd, 0xb5) & 0x20) ? '+' : '-') : 'x');
+	if (adv7604_has_afe(state)) {
+		/* read SSPD */
+		polarity = cp_read(sd, 0xb5);
+		if ((polarity & 0x03) == 0x01) {
+			stdi->hs_pol = polarity & 0x10
+				     ? (polarity & 0x08 ? '+' : '-') : 'x';
+			stdi->vs_pol = polarity & 0x40
+				     ? (polarity & 0x20 ? '+' : '-') : 'x';
+		} else {
+			stdi->hs_pol = 'x';
+			stdi->vs_pol = 'x';
+		}
 	} else {
-		stdi->hs_pol = 'x';
-		stdi->vs_pol = 'x';
+		polarity = hdmi_read(sd, 0x05);
+		stdi->hs_pol = polarity & 0x20 ? '+' : '-';
+		stdi->vs_pol = polarity & 0x10 ? '+' : '-';
 	}
 
 	if (no_lock_stdi(sd) || no_lock_sspd(sd)) {
@@ -1243,8 +1452,14 @@ static int read_stdi(struct v4l2_subdev *sd, struct stdi_readback *stdi)
 static int adv7604_enum_dv_timings(struct v4l2_subdev *sd,
 			struct v4l2_enum_dv_timings *timings)
 {
+	struct adv7604_state *state = to_state(sd);
+
 	if (timings->index >= ARRAY_SIZE(adv7604_timings) - 1)
 		return -EINVAL;
+
+	if (timings->pad >= state->source_pad)
+		return -EINVAL;
+
 	memset(timings->reserved, 0, sizeof(timings->reserved));
 	timings->timings = adv7604_timings[timings->index];
 	return 0;
@@ -1253,14 +1468,30 @@ static int adv7604_enum_dv_timings(struct v4l2_subdev *sd,
 static int adv7604_dv_timings_cap(struct v4l2_subdev *sd,
 			struct v4l2_dv_timings_cap *cap)
 {
+	struct adv7604_state *state = to_state(sd);
+
+	if (cap->pad >= state->source_pad)
+		return -EINVAL;
+
 	cap->type = V4L2_DV_BT_656_1120;
 	cap->bt.max_width = 1920;
 	cap->bt.max_height = 1200;
 	cap->bt.min_pixelclock = 25000000;
-	if (is_digital_input(sd))
+
+	switch (cap->pad) {
+	case ADV7604_PAD_HDMI_PORT_A:
+	case ADV7604_PAD_HDMI_PORT_B:
+	case ADV7604_PAD_HDMI_PORT_C:
+	case ADV7604_PAD_HDMI_PORT_D:
 		cap->bt.max_pixelclock = 225000000;
-	else
+		break;
+	case ADV7604_PAD_VGA_RGB:
+	case ADV7604_PAD_VGA_COMP:
+	default:
 		cap->bt.max_pixelclock = 170000000;
+		break;
+	}
+
 	cap->bt.standards = V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT |
 			 V4L2_DV_BT_STD_GTF | V4L2_DV_BT_STD_CVT;
 	cap->bt.capabilities = V4L2_DV_BT_CAP_PROGRESSIVE |
@@ -1284,10 +1515,43 @@ static void adv7604_fill_optional_dv_timings_fields(struct v4l2_subdev *sd,
 	}
 }
 
+static unsigned int adv7604_read_hdmi_pixelclock(struct v4l2_subdev *sd)
+{
+	unsigned int freq;
+	int a, b;
+
+	a = hdmi_read(sd, 0x06);
+	b = hdmi_read(sd, 0x3b);
+	if (a < 0 || b < 0)
+		return 0;
+	freq =  a * 1000000 + ((b & 0x30) >> 4) * 250000;
+
+	if (is_hdmi(sd)) {
+		/* adjust for deep color mode */
+		unsigned bits_per_channel = ((hdmi_read(sd, 0x0b) & 0x60) >> 4) + 8;
+
+		freq = freq * 8 / bits_per_channel;
+	}
+
+	return freq;
+}
+
+static unsigned int adv7611_read_hdmi_pixelclock(struct v4l2_subdev *sd)
+{
+	int a, b;
+
+	a = hdmi_read(sd, 0x51);
+	b = hdmi_read(sd, 0x52);
+	if (a < 0 || b < 0)
+		return 0;
+	return ((a << 1) | (b >> 7)) * 1000000 + (b & 0x7f) * 1000000 / 128;
+}
+
 static int adv7604_query_dv_timings(struct v4l2_subdev *sd,
 			struct v4l2_dv_timings *timings)
 {
 	struct adv7604_state *state = to_state(sd);
+	const struct adv7604_chip_info *info = state->info;
 	struct v4l2_bt_timings *bt = &timings->bt;
 	struct stdi_readback stdi;
 
@@ -1311,44 +1575,25 @@ static int adv7604_query_dv_timings(struct v4l2_subdev *sd,
 		V4L2_DV_INTERLACED : V4L2_DV_PROGRESSIVE;
 
 	if (is_digital_input(sd)) {
-		uint32_t freq;
-
 		timings->type = V4L2_DV_BT_656_1120;
 
-		bt->width = (hdmi_read(sd, 0x07) & 0x0f) * 256 + hdmi_read(sd, 0x08);
-		bt->height = (hdmi_read(sd, 0x09) & 0x0f) * 256 + hdmi_read(sd, 0x0a);
-		freq = (hdmi_read(sd, 0x06) * 1000000) +
-			((hdmi_read(sd, 0x3b) & 0x30) >> 4) * 250000;
-		if (is_hdmi(sd)) {
-			/* adjust for deep color mode */
-			unsigned bits_per_channel = ((hdmi_read(sd, 0x0b) & 0x60) >> 4) + 8;
-
-			freq = freq * 8 / bits_per_channel;
-		}
-		bt->pixelclock = freq;
-		bt->hfrontporch = (hdmi_read(sd, 0x20) & 0x03) * 256 +
-			hdmi_read(sd, 0x21);
-		bt->hsync = (hdmi_read(sd, 0x22) & 0x03) * 256 +
-			hdmi_read(sd, 0x23);
-		bt->hbackporch = (hdmi_read(sd, 0x24) & 0x03) * 256 +
-			hdmi_read(sd, 0x25);
-		bt->vfrontporch = ((hdmi_read(sd, 0x2a) & 0x1f) * 256 +
-			hdmi_read(sd, 0x2b)) / 2;
-		bt->vsync = ((hdmi_read(sd, 0x2e) & 0x1f) * 256 +
-			hdmi_read(sd, 0x2f)) / 2;
-		bt->vbackporch = ((hdmi_read(sd, 0x32) & 0x1f) * 256 +
-			hdmi_read(sd, 0x33)) / 2;
+		/* FIXME: All masks are incorrect for ADV7611 */
+		bt->width = hdmi_read16(sd, 0x07, 0xfff);
+		bt->height = hdmi_read16(sd, 0x09, 0xfff);
+		bt->pixelclock = info->read_hdmi_pixelclock(sd);
+		bt->hfrontporch = hdmi_read16(sd, 0x20, 0x3ff);
+		bt->hsync = hdmi_read16(sd, 0x22, 0x3ff);
+		bt->hbackporch = hdmi_read16(sd, 0x24, 0x3ff);
+		bt->vfrontporch = hdmi_read16(sd, 0x2a, 0x1fff) / 2;
+		bt->vsync = hdmi_read16(sd, 0x2e, 0x1fff) / 2;
+		bt->vbackporch = hdmi_read16(sd, 0x32, 0x1fff) / 2;
 		bt->polarities = ((hdmi_read(sd, 0x05) & 0x10) ? V4L2_DV_VSYNC_POS_POL : 0) |
 			((hdmi_read(sd, 0x05) & 0x20) ? V4L2_DV_HSYNC_POS_POL : 0);
 		if (bt->interlaced == V4L2_DV_INTERLACED) {
-			bt->height += (hdmi_read(sd, 0x0b) & 0x0f) * 256 +
-					hdmi_read(sd, 0x0c);
-			bt->il_vfrontporch = ((hdmi_read(sd, 0x2c) & 0x1f) * 256 +
-					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 +
-					hdmi_read(sd, 0x35)) / 2;
+			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;
 		}
 		adv7604_fill_optional_dv_timings_fields(sd, timings);
 	} else {
@@ -1378,11 +1623,11 @@ static int adv7604_query_dv_timings(struct v4l2_subdev *sd,
 				v4l2_dbg(1, debug, sd, "%s: restart STDI\n", __func__);
 				/* TODO restart STDI for Sync Channel 2 */
 				/* enter one-shot mode */
-				cp_write_and_or(sd, 0x86, 0xf9, 0x00);
+				cp_write_clr_set(sd, 0x86, 0x06, 0x00);
 				/* trigger STDI restart */
-				cp_write_and_or(sd, 0x86, 0xf9, 0x04);
+				cp_write_clr_set(sd, 0x86, 0x06, 0x04);
 				/* reset to continuous mode */
-				cp_write_and_or(sd, 0x86, 0xf9, 0x02);
+				cp_write_clr_set(sd, 0x86, 0x06, 0x02);
 				state->restart_stdi_once = false;
 				return -ENOLINK;
 			}
@@ -1441,7 +1686,7 @@ static int adv7604_s_dv_timings(struct v4l2_subdev *sd,
 
 	state->timings = *timings;
 
-	cp_write(sd, 0x91, bt->interlaced ? 0x50 : 0x10);
+	cp_write_clr_set(sd, 0x91, 0x40, bt->interlaced ? 0x40 : 0x00);
 
 	/* Use prim_mode and vid_std when available */
 	err = configure_predefined_video_timings(sd, timings);
@@ -1468,6 +1713,16 @@ static int adv7604_g_dv_timings(struct v4l2_subdev *sd,
 	return 0;
 }
 
+static void adv7604_set_termination(struct v4l2_subdev *sd, bool enable)
+{
+	hdmi_write(sd, 0x01, enable ? 0x00 : 0x78);
+}
+
+static void adv7611_set_termination(struct v4l2_subdev *sd, bool enable)
+{
+	hdmi_write(sd, 0x83, enable ? 0xfe : 0xff);
+}
+
 static void enable_input(struct v4l2_subdev *sd)
 {
 	struct adv7604_state *state = to_state(sd);
@@ -1475,10 +1730,10 @@ static void enable_input(struct v4l2_subdev *sd)
 	if (is_analog_input(sd)) {
 		io_write(sd, 0x15, 0xb0);   /* Disable Tristate of Pins (no audio) */
 	} else if (is_digital_input(sd)) {
-		hdmi_write_and_or(sd, 0x00, 0xfc, state->selected_input);
-		hdmi_write(sd, 0x01, 0x00); /* Enable HDMI clock terminators */
+		hdmi_write_clr_set(sd, 0x00, 0x03, state->selected_input);
+		state->info->set_termination(sd, true);
 		io_write(sd, 0x15, 0xa0);   /* Disable Tristate of Pins */
-		hdmi_write_and_or(sd, 0x1a, 0xef, 0x00); /* Unmute audio */
+		hdmi_write_clr_set(sd, 0x1a, 0x10, 0x00); /* Unmute audio */
 	} else {
 		v4l2_dbg(2, debug, sd, "%s: Unknown port %d selected\n",
 				__func__, state->selected_input);
@@ -1487,67 +1742,36 @@ static void enable_input(struct v4l2_subdev *sd)
 
 static void disable_input(struct v4l2_subdev *sd)
 {
-	hdmi_write_and_or(sd, 0x1a, 0xef, 0x10); /* Mute audio */
+	struct adv7604_state *state = to_state(sd);
+
+	hdmi_write_clr_set(sd, 0x1a, 0x10, 0x10); /* Mute audio */
 	msleep(16); /* 512 samples with >= 32 kHz sample rate [REF_03, c. 7.16.10] */
 	io_write(sd, 0x15, 0xbe);   /* Tristate all outputs from video core */
-	hdmi_write(sd, 0x01, 0x78); /* Disable HDMI clock terminators */
+	state->info->set_termination(sd, false);
 }
 
 static void select_input(struct v4l2_subdev *sd)
 {
 	struct adv7604_state *state = to_state(sd);
+	const struct adv7604_chip_info *info = state->info;
 
 	if (is_analog_input(sd)) {
-		/* reset ADI recommended settings for HDMI: */
-		/* "ADV7604 Register Settings Recommendations (rev. 2.5, June 2010)" p. 4. */
-		hdmi_write(sd, 0x0d, 0x04); /* HDMI filter optimization */
-		hdmi_write(sd, 0x3d, 0x00); /* DDC bus active pull-up control */
-		hdmi_write(sd, 0x3e, 0x74); /* TMDS PLL optimization */
-		hdmi_write(sd, 0x4e, 0x3b); /* TMDS PLL optimization */
-		hdmi_write(sd, 0x57, 0x74); /* TMDS PLL optimization */
-		hdmi_write(sd, 0x58, 0x63); /* TMDS PLL optimization */
-		hdmi_write(sd, 0x8d, 0x18); /* equaliser */
-		hdmi_write(sd, 0x8e, 0x34); /* equaliser */
-		hdmi_write(sd, 0x93, 0x88); /* equaliser */
-		hdmi_write(sd, 0x94, 0x2e); /* equaliser */
-		hdmi_write(sd, 0x96, 0x00); /* enable automatic EQ changing */
+		adv7604_write_reg_seq(sd, info->recommended_settings[0]);
 
 		afe_write(sd, 0x00, 0x08); /* power up ADC */
 		afe_write(sd, 0x01, 0x06); /* power up Analog Front End */
 		afe_write(sd, 0xc8, 0x00); /* phase control */
-
-		/* set ADI recommended settings for digitizer */
-		/* "ADV7604 Register Settings Recommendations (rev. 2.5, June 2010)" p. 17. */
-		afe_write(sd, 0x12, 0x7b); /* ADC noise shaping filter controls */
-		afe_write(sd, 0x0c, 0x1f); /* CP core gain controls */
-		cp_write(sd, 0x3e, 0x04); /* CP core pre-gain control */
-		cp_write(sd, 0xc3, 0x39); /* CP coast control. Graphics mode */
-		cp_write(sd, 0x40, 0x5c); /* CP core pre-gain control. Graphics mode */
 	} else if (is_digital_input(sd)) {
 		hdmi_write(sd, 0x00, state->selected_input & 0x03);
 
-		/* set ADI recommended settings for HDMI: */
-		/* "ADV7604 Register Settings Recommendations (rev. 2.5, June 2010)" p. 4. */
-		hdmi_write(sd, 0x0d, 0x84); /* HDMI filter optimization */
-		hdmi_write(sd, 0x3d, 0x10); /* DDC bus active pull-up control */
-		hdmi_write(sd, 0x3e, 0x39); /* TMDS PLL optimization */
-		hdmi_write(sd, 0x4e, 0x3b); /* TMDS PLL optimization */
-		hdmi_write(sd, 0x57, 0xb6); /* TMDS PLL optimization */
-		hdmi_write(sd, 0x58, 0x03); /* TMDS PLL optimization */
-		hdmi_write(sd, 0x8d, 0x18); /* equaliser */
-		hdmi_write(sd, 0x8e, 0x34); /* equaliser */
-		hdmi_write(sd, 0x93, 0x8b); /* equaliser */
-		hdmi_write(sd, 0x94, 0x2d); /* equaliser */
-		hdmi_write(sd, 0x96, 0x01); /* enable automatic EQ changing */
-
-		afe_write(sd, 0x00, 0xff); /* power down ADC */
-		afe_write(sd, 0x01, 0xfe); /* power down Analog Front End */
-		afe_write(sd, 0xc8, 0x40); /* phase control */
-
-		/* reset ADI recommended settings for digitizer */
-		/* "ADV7604 Register Settings Recommendations (rev. 2.5, June 2010)" p. 17. */
-		afe_write(sd, 0x12, 0xfb); /* ADC noise shaping filter controls */
-		afe_write(sd, 0x0c, 0x0d); /* CP core gain controls */
+		adv7604_write_reg_seq(sd, info->recommended_settings[1]);
+
+		if (adv7604_has_afe(state)) {
+			afe_write(sd, 0x00, 0xff); /* power down ADC */
+			afe_write(sd, 0x01, 0xfe); /* power down Analog Front End */
+			afe_write(sd, 0xc8, 0x40); /* phase control */
+		}
+
 		cp_write(sd, 0x3e, 0x00); /* CP core pre-gain control */
 		cp_write(sd, 0xc3, 0x39); /* CP coast control. Graphics mode */
 		cp_write(sd, 0x40, 0x80); /* CP core pre-gain control. Graphics mode */
@@ -1568,6 +1792,9 @@ static int adv7604_s_routing(struct v4l2_subdev *sd,
 	if (input == state->selected_input)
 		return 0;
 
+	if (input > state->info->max_port)
+		return -EINVAL;
+
 	state->selected_input = input;
 
 	disable_input(sd);
@@ -1579,34 +1806,139 @@ static int adv7604_s_routing(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int adv7604_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned int index,
-			     enum v4l2_mbus_pixelcode *code)
+static int adv7604_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (index)
+	struct adv7604_state *state = to_state(sd);
+
+	if (code->index >= state->info->nformats)
 		return -EINVAL;
-	/* Good enough for now */
-	*code = V4L2_MBUS_FMT_FIXED;
+
+	code->code = state->info->formats[code->index].code;
+
 	return 0;
 }
 
-static int adv7604_g_mbus_fmt(struct v4l2_subdev *sd,
-		struct v4l2_mbus_framefmt *fmt)
+static void adv7604_fill_format(struct adv7604_state *state,
+				struct v4l2_mbus_framefmt *format)
 {
-	struct adv7604_state *state = to_state(sd);
+	memset(format, 0, sizeof(*format));
+
+	format->width = state->timings.bt.width;
+	format->height = state->timings.bt.height;
+	format->field = V4L2_FIELD_NONE;
 
-	fmt->width = state->timings.bt.width;
-	fmt->height = state->timings.bt.height;
-	fmt->code = V4L2_MBUS_FMT_FIXED;
-	fmt->field = V4L2_FIELD_NONE;
-	if (state->timings.bt.standards & V4L2_DV_BT_STD_CEA861) {
-		fmt->colorspace = (state->timings.bt.height <= 576) ?
+	if (state->timings.bt.standards & V4L2_DV_BT_STD_CEA861)
+		format->colorspace = (state->timings.bt.height <= 576) ?
 			V4L2_COLORSPACE_SMPTE170M : V4L2_COLORSPACE_REC709;
+}
+
+/*
+ * Compute the op_ch_sel value required to obtain on the bus the component order
+ * corresponding to the selected format taking into account bus reordering
+ * applied by the board at the output of the device.
+ *
+ * The following table gives the op_ch_value from the format component order
+ * (expressed as op_ch_sel value in column) and the bus reordering (expressed as
+ * adv7604_bus_order value in row).
+ *
+ *           |	GBR(0)	GRB(1)	BGR(2)	RGB(3)	BRG(4)	RBG(5)
+ * ----------+-------------------------------------------------
+ * RGB (NOP) |	GBR	GRB	BGR	RGB	BRG	RBG
+ * GRB (1-2) |	BGR	RGB	GBR	GRB	RBG	BRG
+ * RBG (2-3) |	GRB	GBR	BRG	RBG	BGR	RGB
+ * BGR (1-3) |	RBG	BRG	RGB	BGR	GRB	GBR
+ * BRG (ROR) |	BRG	RBG	GRB	GBR	RGB	BGR
+ * GBR (ROL) |	RGB	BGR	RBG	BRG	GBR	GRB
+ */
+static unsigned int adv7604_op_ch_sel(struct adv7604_state *state)
+{
+#define _SEL(a,b,c,d,e,f)	{ \
+	ADV7604_OP_CH_SEL_##a, ADV7604_OP_CH_SEL_##b, ADV7604_OP_CH_SEL_##c, \
+	ADV7604_OP_CH_SEL_##d, ADV7604_OP_CH_SEL_##e, ADV7604_OP_CH_SEL_##f }
+#define _BUS(x)			[ADV7604_BUS_ORDER_##x]
+
+	static const unsigned int op_ch_sel[6][6] = {
+		_BUS(RGB) /* NOP */ = _SEL(GBR, GRB, BGR, RGB, BRG, RBG),
+		_BUS(GRB) /* 1-2 */ = _SEL(BGR, RGB, GBR, GRB, RBG, BRG),
+		_BUS(RBG) /* 2-3 */ = _SEL(GRB, GBR, BRG, RBG, BGR, RGB),
+		_BUS(BGR) /* 1-3 */ = _SEL(RBG, BRG, RGB, BGR, GRB, GBR),
+		_BUS(BRG) /* ROR */ = _SEL(BRG, RBG, GRB, GBR, RGB, BGR),
+		_BUS(GBR) /* ROL */ = _SEL(RGB, BGR, RBG, BRG, GBR, GRB),
+	};
+
+	return op_ch_sel[state->pdata.bus_order][state->format->op_ch_sel >> 5];
+}
+
+static void adv7604_setup_format(struct adv7604_state *state)
+{
+	struct v4l2_subdev *sd = &state->sd;
+
+	io_write_clr_set(sd, 0x02, 0x02,
+			state->format->rgb_out ? ADV7604_RGB_OUT : 0);
+	io_write(sd, 0x03, state->format->op_format_sel |
+		 state->pdata.op_format_mode_sel);
+	io_write_clr_set(sd, 0x04, 0xe0, adv7604_op_ch_sel(state));
+	io_write_clr_set(sd, 0x05, 0x01,
+			state->format->swap_cb_cr ? ADV7604_OP_SWAP_CB_CR : 0);
+}
+
+static int adv7604_get_format(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			      struct v4l2_subdev_format *format)
+{
+	struct adv7604_state *state = to_state(sd);
+
+	if (format->pad != state->source_pad)
+		return -EINVAL;
+
+	adv7604_fill_format(state, &format->format);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *fmt;
+
+		fmt = v4l2_subdev_get_try_format(fh, format->pad);
+		format->format.code = fmt->code;
+	} else {
+		format->format.code = state->format->code;
 	}
+
+	return 0;
+}
+
+static int adv7604_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			      struct v4l2_subdev_format *format)
+{
+	struct adv7604_state *state = to_state(sd);
+	const struct adv7604_format_info *info;
+
+	if (format->pad != state->source_pad)
+		return -EINVAL;
+
+	info = adv7604_format_info(state, format->format.code);
+	if (info == NULL)
+		info = adv7604_format_info(state, V4L2_MBUS_FMT_YUYV8_2X8);
+
+	adv7604_fill_format(state, &format->format);
+	format->format.code = info->code;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *fmt;
+
+		fmt = v4l2_subdev_get_try_format(fh, format->pad);
+		fmt->code = format->format.code;
+	} else {
+		state->format = info;
+		adv7604_setup_format(state);
+	}
+
 	return 0;
 }
 
 static int adv7604_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
 {
+	struct adv7604_state *state = to_state(sd);
+	const struct adv7604_chip_info *info = state->info;
 	const u8 irq_reg_0x43 = io_read(sd, 0x43);
 	const u8 irq_reg_0x6b = io_read(sd, 0x6b);
 	const u8 irq_reg_0x70 = io_read(sd, 0x70);
@@ -1625,7 +1957,9 @@ static int adv7604_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
 
 	/* format change */
 	fmt_change = irq_reg_0x43 & 0x98;
-	fmt_change_digital = is_digital_input(sd) ? (irq_reg_0x6b & 0xc0) : 0;
+	fmt_change_digital = is_digital_input(sd)
+			   ? irq_reg_0x6b & info->fmt_change_digital_mask
+			   : 0;
 
 	if (fmt_change || fmt_change_digital) {
 		v4l2_dbg(1, debug, sd,
@@ -1647,7 +1981,7 @@ static int adv7604_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
 	}
 
 	/* tx 5v detect */
-	tx_5v = io_read(sd, 0x70) & 0x1e;
+	tx_5v = io_read(sd, 0x70) & info->cable_det_mask;
 	if (tx_5v) {
 		v4l2_dbg(1, debug, sd, "%s: tx_5v: 0x%x\n", __func__, tx_5v);
 		io_write(sd, 0x71, tx_5v);
@@ -1663,7 +1997,7 @@ static int adv7604_get_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
 	struct adv7604_state *state = to_state(sd);
 	u8 *data = NULL;
 
-	if (edid->pad > ADV7604_EDID_PORT_D)
+	if (edid->pad > ADV7604_PAD_HDMI_PORT_D)
 		return -EINVAL;
 	if (edid->blocks == 0)
 		return -EINVAL;
@@ -1673,17 +2007,15 @@ static int adv7604_get_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
 		return -EINVAL;
 	if (edid->start_block == 1)
 		edid->blocks = 1;
-	if (!edid->edid)
-		return -EINVAL;
 
 	if (edid->blocks > state->edid.blocks)
 		edid->blocks = state->edid.blocks;
 
 	switch (edid->pad) {
-	case ADV7604_EDID_PORT_A:
-	case ADV7604_EDID_PORT_B:
-	case ADV7604_EDID_PORT_C:
-	case ADV7604_EDID_PORT_D:
+	case ADV7604_PAD_HDMI_PORT_A:
+	case ADV7604_PAD_HDMI_PORT_B:
+	case ADV7604_PAD_HDMI_PORT_C:
+	case ADV7604_PAD_HDMI_PORT_D:
 		if (state->edid.present & (1 << edid->pad))
 			data = state->edid.edid;
 		break;
@@ -1731,20 +2063,20 @@ static int get_edid_spa_location(const u8 *edid)
 static int adv7604_set_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
 {
 	struct adv7604_state *state = to_state(sd);
+	const struct adv7604_chip_info *info = state->info;
 	int spa_loc;
-	int tmp = 0;
 	int err;
 	int i;
 
-	if (edid->pad > ADV7604_EDID_PORT_D)
+	if (edid->pad > ADV7604_PAD_HDMI_PORT_D)
 		return -EINVAL;
 	if (edid->start_block != 0)
 		return -EINVAL;
 	if (edid->blocks == 0) {
 		/* Disable hotplug and I2C access to EDID RAM from DDC port */
 		state->edid.present &= ~(1 << edid->pad);
-		v4l2_subdev_notify(sd, ADV7604_HOTPLUG, (void *)&state->edid.present);
-		rep_write_and_or(sd, 0x77, 0xf0, state->edid.present);
+		adv7604_set_hpd(state, state->edid.present);
+		rep_write_clr_set(sd, info->edid_enable_reg, 0x0f, state->edid.present);
 
 		/* Fall back to a 16:9 aspect ratio */
 		state->aspect_ratio.numerator = 16;
@@ -1761,43 +2093,47 @@ static int adv7604_set_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
 		edid->blocks = 2;
 		return -E2BIG;
 	}
-	if (!edid->edid)
-		return -EINVAL;
 
 	v4l2_dbg(2, debug, sd, "%s: write EDID pad %d, edid.present = 0x%x\n",
 			__func__, edid->pad, state->edid.present);
 
 	/* Disable hotplug and I2C access to EDID RAM from DDC port */
 	cancel_delayed_work_sync(&state->delayed_work_enable_hotplug);
-	v4l2_subdev_notify(sd, ADV7604_HOTPLUG, (void *)&tmp);
-	rep_write_and_or(sd, 0x77, 0xf0, 0x00);
+	adv7604_set_hpd(state, 0);
+	rep_write_clr_set(sd, info->edid_enable_reg, 0x0f, 0x00);
 
 	spa_loc = get_edid_spa_location(edid->edid);
 	if (spa_loc < 0)
 		spa_loc = 0xc0; /* Default value [REF_02, p. 116] */
 
 	switch (edid->pad) {
-	case ADV7604_EDID_PORT_A:
+	case ADV7604_PAD_HDMI_PORT_A:
 		state->spa_port_a[0] = edid->edid[spa_loc];
 		state->spa_port_a[1] = edid->edid[spa_loc + 1];
 		break;
-	case ADV7604_EDID_PORT_B:
+	case ADV7604_PAD_HDMI_PORT_B:
 		rep_write(sd, 0x70, edid->edid[spa_loc]);
 		rep_write(sd, 0x71, edid->edid[spa_loc + 1]);
 		break;
-	case ADV7604_EDID_PORT_C:
+	case ADV7604_PAD_HDMI_PORT_C:
 		rep_write(sd, 0x72, edid->edid[spa_loc]);
 		rep_write(sd, 0x73, edid->edid[spa_loc + 1]);
 		break;
-	case ADV7604_EDID_PORT_D:
+	case ADV7604_PAD_HDMI_PORT_D:
 		rep_write(sd, 0x74, edid->edid[spa_loc]);
 		rep_write(sd, 0x75, edid->edid[spa_loc + 1]);
 		break;
 	default:
 		return -EINVAL;
 	}
-	rep_write(sd, 0x76, spa_loc & 0xff);
-	rep_write_and_or(sd, 0x77, 0xbf, (spa_loc >> 2) & 0x40);
+
+	if (info->type == ADV7604) {
+		rep_write(sd, 0x76, spa_loc & 0xff);
+		rep_write_clr_set(sd, 0x77, 0x40, (spa_loc & 0x100) >> 2);
+	} else {
+		/* FIXME: Where is the SPA location LSB register ? */
+		rep_write_clr_set(sd, 0x71, 0x01, (spa_loc & 0x100) >> 8);
+	}
 
 	edid->edid[spa_loc] = state->spa_port_a[0];
 	edid->edid[spa_loc + 1] = state->spa_port_a[1];
@@ -1816,10 +2152,10 @@ static int adv7604_set_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
 
 	/* adv7604 calculates the checksums and enables I2C access to internal
 	   EDID RAM from DDC port. */
-	rep_write_and_or(sd, 0x77, 0xf0, state->edid.present);
+	rep_write_clr_set(sd, info->edid_enable_reg, 0x0f, state->edid.present);
 
 	for (i = 0; i < 1000; i++) {
-		if (rep_read(sd, 0x7d) & state->edid.present)
+		if (rep_read(sd, info->edid_status_reg) & state->edid.present)
 			break;
 		mdelay(1);
 	}
@@ -1882,17 +2218,20 @@ static void print_avi_infoframe(struct v4l2_subdev *sd)
 static int adv7604_log_status(struct v4l2_subdev *sd)
 {
 	struct adv7604_state *state = to_state(sd);
+	const struct adv7604_chip_info *info = state->info;
 	struct v4l2_dv_timings timings;
 	struct stdi_readback stdi;
 	u8 reg_io_0x02 = io_read(sd, 0x02);
+	u8 edid_enabled;
+	u8 cable_det;
 
-	char *csc_coeff_sel_rb[16] = {
+	static const char * const csc_coeff_sel_rb[16] = {
 		"bypassed", "YPbPr601 -> RGB", "reserved", "YPbPr709 -> RGB",
 		"reserved", "RGB -> YPbPr601", "reserved", "RGB -> YPbPr709",
 		"reserved", "YPbPr709 -> YPbPr601", "YPbPr601 -> YPbPr709",
 		"reserved", "reserved", "reserved", "reserved", "manual"
 	};
-	char *input_color_space_txt[16] = {
+	static const char * const input_color_space_txt[16] = {
 		"RGB limited range (16-235)", "RGB full range (0-255)",
 		"YCbCr Bt.601 (16-235)", "YCbCr Bt.709 (16-235)",
 		"xvYCC Bt.601", "xvYCC Bt.709",
@@ -1900,12 +2239,12 @@ static int adv7604_log_status(struct v4l2_subdev *sd)
 		"invalid", "invalid", "invalid", "invalid", "invalid",
 		"invalid", "invalid", "automatic"
 	};
-	char *rgb_quantization_range_txt[] = {
+	static const char * const rgb_quantization_range_txt[] = {
 		"Automatic",
 		"RGB limited range (16-235)",
 		"RGB full range (0-255)",
 	};
-	char *deep_color_mode_txt[4] = {
+	static const char * const deep_color_mode_txt[4] = {
 		"8-bits per channel",
 		"10-bits per channel",
 		"12-bits per channel",
@@ -1914,20 +2253,22 @@ static int adv7604_log_status(struct v4l2_subdev *sd)
 
 	v4l2_info(sd, "-----Chip status-----\n");
 	v4l2_info(sd, "Chip power: %s\n", no_power(sd) ? "off" : "on");
+	edid_enabled = rep_read(sd, info->edid_status_reg);
 	v4l2_info(sd, "EDID enabled port A: %s, B: %s, C: %s, D: %s\n",
-			((rep_read(sd, 0x7d) & 0x01) ? "Yes" : "No"),
-			((rep_read(sd, 0x7d) & 0x02) ? "Yes" : "No"),
-			((rep_read(sd, 0x7d) & 0x04) ? "Yes" : "No"),
-			((rep_read(sd, 0x7d) & 0x08) ? "Yes" : "No"));
+			((edid_enabled & 0x01) ? "Yes" : "No"),
+			((edid_enabled & 0x02) ? "Yes" : "No"),
+			((edid_enabled & 0x04) ? "Yes" : "No"),
+			((edid_enabled & 0x08) ? "Yes" : "No"));
 	v4l2_info(sd, "CEC: %s\n", !!(cec_read(sd, 0x2a) & 0x01) ?
 			"enabled" : "disabled");
 
 	v4l2_info(sd, "-----Signal status-----\n");
+	cable_det = info->read_cable_det(sd);
 	v4l2_info(sd, "Cable detected (+5V power) port A: %s, B: %s, C: %s, D: %s\n",
-			((io_read(sd, 0x6f) & 0x10) ? "Yes" : "No"),
-			((io_read(sd, 0x6f) & 0x08) ? "Yes" : "No"),
-			((io_read(sd, 0x6f) & 0x04) ? "Yes" : "No"),
-			((io_read(sd, 0x6f) & 0x02) ? "Yes" : "No"));
+			((cable_det & 0x01) ? "Yes" : "No"),
+			((cable_det & 0x02) ? "Yes" : "No"),
+			((cable_det & 0x04) ? "Yes" : "No"),
+			((cable_det & 0x08) ? "Yes" : "No"));
 	v4l2_info(sd, "TMDS signal detected: %s\n",
 			no_signal_tmds(sd) ? "false" : "true");
 	v4l2_info(sd, "TMDS signal locked: %s\n",
@@ -2021,13 +2362,6 @@ static const struct v4l2_ctrl_ops adv7604_ctrl_ops = {
 
 static const struct v4l2_subdev_core_ops adv7604_core_ops = {
 	.log_status = adv7604_log_status,
-	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
-	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
-	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
-	.g_ctrl = v4l2_subdev_g_ctrl,
-	.s_ctrl = v4l2_subdev_s_ctrl,
-	.queryctrl = v4l2_subdev_queryctrl,
-	.querymenu = v4l2_subdev_querymenu,
 	.interrupt_service_routine = adv7604_isr,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	.g_register = adv7604_g_register,
@@ -2041,17 +2375,16 @@ static const struct v4l2_subdev_video_ops adv7604_video_ops = {
 	.s_dv_timings = adv7604_s_dv_timings,
 	.g_dv_timings = adv7604_g_dv_timings,
 	.query_dv_timings = adv7604_query_dv_timings,
-	.enum_dv_timings = adv7604_enum_dv_timings,
-	.dv_timings_cap = adv7604_dv_timings_cap,
-	.enum_mbus_fmt = adv7604_enum_mbus_fmt,
-	.g_mbus_fmt = adv7604_g_mbus_fmt,
-	.try_mbus_fmt = adv7604_g_mbus_fmt,
-	.s_mbus_fmt = adv7604_g_mbus_fmt,
 };
 
 static const struct v4l2_subdev_pad_ops adv7604_pad_ops = {
+	.enum_mbus_code = adv7604_enum_mbus_code,
+	.get_fmt = adv7604_get_format,
+	.set_fmt = adv7604_set_format,
 	.get_edid = adv7604_get_edid,
 	.set_edid = adv7604_set_edid,
+	.dv_timings_cap = adv7604_dv_timings_cap,
+	.enum_dv_timings = adv7604_enum_dv_timings,
 };
 
 static const struct v4l2_subdev_ops adv7604_ops = {
@@ -2100,6 +2433,7 @@ static const struct v4l2_ctrl_config adv7604_ctrl_free_run_color = {
 static int adv7604_core_init(struct v4l2_subdev *sd)
 {
 	struct adv7604_state *state = to_state(sd);
+	const struct adv7604_chip_info *info = state->info;
 	struct adv7604_platform_data *pdata = &state->pdata;
 
 	hdmi_write(sd, 0x48,
@@ -2108,28 +2442,33 @@ static int adv7604_core_init(struct v4l2_subdev *sd)
 
 	disable_input(sd);
 
+	if (pdata->default_input >= 0 &&
+	    pdata->default_input < state->source_pad) {
+		state->selected_input = pdata->default_input;
+		select_input(sd);
+		enable_input(sd);
+	}
+
 	/* power */
 	io_write(sd, 0x0c, 0x42);   /* Power up part and power down VDP */
 	io_write(sd, 0x0b, 0x44);   /* Power down ESDP block */
 	cp_write(sd, 0xcf, 0x01);   /* Power down macrovision */
 
 	/* video format */
-	io_write_and_or(sd, 0x02, 0xf0,
+	io_write_clr_set(sd, 0x02, 0x0f,
 			pdata->alt_gamma << 3 |
 			pdata->op_656_range << 2 |
-			pdata->rgb_out << 1 |
 			pdata->alt_data_sat << 0);
-	io_write(sd, 0x03, pdata->op_format_sel);
-	io_write_and_or(sd, 0x04, 0x1f, pdata->op_ch_sel << 5);
-	io_write_and_or(sd, 0x05, 0xf0, pdata->blank_data << 3 |
-					pdata->insert_av_codes << 2 |
-					pdata->replicate_av_codes << 1 |
-					pdata->invert_cbcr << 0);
+	io_write_clr_set(sd, 0x05, 0x0e, pdata->blank_data << 3 |
+			pdata->insert_av_codes << 2 |
+			pdata->replicate_av_codes << 1);
+	adv7604_setup_format(state);
 
 	cp_write(sd, 0x69, 0x30);   /* Enable CP CSC */
 
 	/* VS, HS polarities */
-	io_write(sd, 0x06, 0xa0 | pdata->inv_vs_pol << 2 | pdata->inv_hs_pol << 1);
+	io_write(sd, 0x06, 0xa0 | pdata->inv_vs_pol << 2 |
+		 pdata->inv_hs_pol << 1 | pdata->inv_llc_pol);
 
 	/* Adjust drive strength */
 	io_write(sd, 0x14, 0x40 | pdata->dr_str_data << 4 |
@@ -2146,52 +2485,46 @@ static int adv7604_core_init(struct v4l2_subdev *sd)
 				     for digital formats */
 
 	/* HDMI audio */
-	hdmi_write_and_or(sd, 0x15, 0xfc, 0x03); /* Mute on FIFO over-/underflow [REF_01, c. 1.2.18] */
-	hdmi_write_and_or(sd, 0x1a, 0xf1, 0x08); /* Wait 1 s before unmute */
-	hdmi_write_and_or(sd, 0x68, 0xf9, 0x06); /* FIFO reset on over-/underflow [REF_01, c. 1.2.19] */
+	hdmi_write_clr_set(sd, 0x15, 0x03, 0x03); /* Mute on FIFO over-/underflow [REF_01, c. 1.2.18] */
+	hdmi_write_clr_set(sd, 0x1a, 0x0e, 0x08); /* Wait 1 s before unmute */
+	hdmi_write_clr_set(sd, 0x68, 0x06, 0x06); /* FIFO reset on over-/underflow [REF_01, c. 1.2.19] */
 
 	/* TODO from platform data */
 	afe_write(sd, 0xb5, 0x01);  /* Setting MCLK to 256Fs */
 
-	afe_write(sd, 0x02, pdata->ain_sel); /* Select analog input muxing mode */
-	io_write_and_or(sd, 0x30, ~(1 << 4), pdata->output_bus_lsb_to_msb << 4);
+	if (adv7604_has_afe(state)) {
+		afe_write(sd, 0x02, pdata->ain_sel); /* Select analog input muxing mode */
+		io_write_clr_set(sd, 0x30, 1 << 4, pdata->output_bus_lsb_to_msb << 4);
+	}
 
 	/* interrupts */
-	io_write(sd, 0x40, 0xc2); /* Configure INT1 */
-	io_write(sd, 0x41, 0xd7); /* STDI irq for any change, disable INT2 */
+	io_write(sd, 0x40, 0xc0 | pdata->int1_config); /* Configure INT1 */
 	io_write(sd, 0x46, 0x98); /* Enable SSPD, STDI and CP unlocked interrupts */
-	io_write(sd, 0x6e, 0xc1); /* Enable V_LOCKED, DE_REGEN_LCK, HDMI_MODE interrupts */
-	io_write(sd, 0x73, 0x1e); /* Enable CABLE_DET_A_ST (+5v) interrupts */
+	io_write(sd, 0x6e, info->fmt_change_digital_mask); /* Enable V_LOCKED and DE_REGEN_LCK interrupts */
+	io_write(sd, 0x73, info->cable_det_mask); /* Enable cable detection (+5v) interrupts */
+	info->setup_irqs(sd);
 
 	return v4l2_ctrl_handler_setup(sd->ctrl_handler);
 }
 
+static void adv7604_setup_irqs(struct v4l2_subdev *sd)
+{
+	io_write(sd, 0x41, 0xd7); /* STDI irq for any change, disable INT2 */
+}
+
+static void adv7611_setup_irqs(struct v4l2_subdev *sd)
+{
+	io_write(sd, 0x41, 0xd0); /* STDI irq for any change, disable INT2 */
+}
+
 static void adv7604_unregister_clients(struct adv7604_state *state)
 {
-	if (state->i2c_avlink)
-		i2c_unregister_device(state->i2c_avlink);
-	if (state->i2c_cec)
-		i2c_unregister_device(state->i2c_cec);
-	if (state->i2c_infoframe)
-		i2c_unregister_device(state->i2c_infoframe);
-	if (state->i2c_esdp)
-		i2c_unregister_device(state->i2c_esdp);
-	if (state->i2c_dpp)
-		i2c_unregister_device(state->i2c_dpp);
-	if (state->i2c_afe)
-		i2c_unregister_device(state->i2c_afe);
-	if (state->i2c_repeater)
-		i2c_unregister_device(state->i2c_repeater);
-	if (state->i2c_edid)
-		i2c_unregister_device(state->i2c_edid);
-	if (state->i2c_hdmi)
-		i2c_unregister_device(state->i2c_hdmi);
-	if (state->i2c_test)
-		i2c_unregister_device(state->i2c_test);
-	if (state->i2c_cp)
-		i2c_unregister_device(state->i2c_cp);
-	if (state->i2c_vdp)
-		i2c_unregister_device(state->i2c_vdp);
+	unsigned int i;
+
+	for (i = 1; i < ARRAY_SIZE(state->i2c_clients); ++i) {
+		if (state->i2c_clients[i])
+			i2c_unregister_device(state->i2c_clients[i]);
+	}
 }
 
 static struct i2c_client *adv7604_dummy_client(struct v4l2_subdev *sd,
@@ -2204,15 +2537,219 @@ static struct i2c_client *adv7604_dummy_client(struct v4l2_subdev *sd,
 	return i2c_new_dummy(client->adapter, io_read(sd, io_reg) >> 1);
 }
 
+static const struct adv7604_reg_seq adv7604_recommended_settings_afe[] = {
+	/* reset ADI recommended settings for HDMI: */
+	/* "ADV7604 Register Settings Recommendations (rev. 2.5, June 2010)" p. 4. */
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x0d), 0x04 }, /* HDMI filter optimization */
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x0d), 0x04 }, /* HDMI filter optimization */
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x3d), 0x00 }, /* DDC bus active pull-up control */
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x3e), 0x74 }, /* TMDS PLL optimization */
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x4e), 0x3b }, /* TMDS PLL optimization */
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x57), 0x74 }, /* TMDS PLL optimization */
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x58), 0x63 }, /* TMDS PLL optimization */
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x8d), 0x18 }, /* equaliser */
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x8e), 0x34 }, /* equaliser */
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x93), 0x88 }, /* equaliser */
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x94), 0x2e }, /* equaliser */
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x96), 0x00 }, /* enable automatic EQ changing */
+
+	/* set ADI recommended settings for digitizer */
+	/* "ADV7604 Register Settings Recommendations (rev. 2.5, June 2010)" p. 17. */
+	{ ADV7604_REG(ADV7604_PAGE_AFE, 0x12), 0x7b }, /* ADC noise shaping filter controls */
+	{ ADV7604_REG(ADV7604_PAGE_AFE, 0x0c), 0x1f }, /* CP core gain controls */
+	{ ADV7604_REG(ADV7604_PAGE_CP, 0x3e), 0x04 }, /* CP core pre-gain control */
+	{ ADV7604_REG(ADV7604_PAGE_CP, 0xc3), 0x39 }, /* CP coast control. Graphics mode */
+	{ ADV7604_REG(ADV7604_PAGE_CP, 0x40), 0x5c }, /* CP core pre-gain control. Graphics mode */
+
+	{ ADV7604_REG_SEQ_TERM, 0 },
+};
+
+static const struct adv7604_reg_seq adv7604_recommended_settings_hdmi[] = {
+	/* set ADI recommended settings for HDMI: */
+	/* "ADV7604 Register Settings Recommendations (rev. 2.5, June 2010)" p. 4. */
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x0d), 0x84 }, /* HDMI filter optimization */
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x3d), 0x10 }, /* DDC bus active pull-up control */
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x3e), 0x39 }, /* TMDS PLL optimization */
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x4e), 0x3b }, /* TMDS PLL optimization */
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x57), 0xb6 }, /* TMDS PLL optimization */
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x58), 0x03 }, /* TMDS PLL optimization */
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x8d), 0x18 }, /* equaliser */
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x8e), 0x34 }, /* equaliser */
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x93), 0x8b }, /* equaliser */
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x94), 0x2d }, /* equaliser */
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x96), 0x01 }, /* enable automatic EQ changing */
+
+	/* reset ADI recommended settings for digitizer */
+	/* "ADV7604 Register Settings Recommendations (rev. 2.5, June 2010)" p. 17. */
+	{ ADV7604_REG(ADV7604_PAGE_AFE, 0x12), 0xfb }, /* ADC noise shaping filter controls */
+	{ ADV7604_REG(ADV7604_PAGE_AFE, 0x0c), 0x0d }, /* CP core gain controls */
+
+	{ ADV7604_REG_SEQ_TERM, 0 },
+};
+
+static const struct adv7604_reg_seq adv7611_recommended_settings_hdmi[] = {
+	{ ADV7604_REG(ADV7604_PAGE_CP, 0x6c), 0x00 },
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x6f), 0x0c },
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x87), 0x70 },
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x57), 0xda },
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x58), 0x01 },
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x03), 0x98 },
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x4c), 0x44 },
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x8d), 0x04 },
+	{ ADV7604_REG(ADV7604_PAGE_HDMI, 0x8e), 0x1e },
+
+	{ ADV7604_REG_SEQ_TERM, 0 },
+};
+
+static const struct adv7604_chip_info adv7604_chip_info[] = {
+	[ADV7604] = {
+		.type = ADV7604,
+		.has_afe = true,
+		.max_port = ADV7604_PAD_VGA_COMP,
+		.num_dv_ports = 4,
+		.edid_enable_reg = 0x77,
+		.edid_status_reg = 0x7d,
+		.lcf_reg = 0xb3,
+		.tdms_lock_mask = 0xe0,
+		.cable_det_mask = 0x1e,
+		.fmt_change_digital_mask = 0xc1,
+		.formats = adv7604_formats,
+		.nformats = ARRAY_SIZE(adv7604_formats),
+		.set_termination = adv7604_set_termination,
+		.setup_irqs = adv7604_setup_irqs,
+		.read_hdmi_pixelclock = adv7604_read_hdmi_pixelclock,
+		.read_cable_det = adv7604_read_cable_det,
+		.recommended_settings = {
+		    [0] = adv7604_recommended_settings_afe,
+		    [1] = adv7604_recommended_settings_hdmi,
+		},
+		.num_recommended_settings = {
+		    [0] = ARRAY_SIZE(adv7604_recommended_settings_afe),
+		    [1] = ARRAY_SIZE(adv7604_recommended_settings_hdmi),
+		},
+		.page_mask = BIT(ADV7604_PAGE_IO) | BIT(ADV7604_PAGE_AVLINK) |
+			BIT(ADV7604_PAGE_CEC) | BIT(ADV7604_PAGE_INFOFRAME) |
+			BIT(ADV7604_PAGE_ESDP) | BIT(ADV7604_PAGE_DPP) |
+			BIT(ADV7604_PAGE_AFE) | BIT(ADV7604_PAGE_REP) |
+			BIT(ADV7604_PAGE_EDID) | BIT(ADV7604_PAGE_HDMI) |
+			BIT(ADV7604_PAGE_TEST) | BIT(ADV7604_PAGE_CP) |
+			BIT(ADV7604_PAGE_VDP),
+	},
+	[ADV7611] = {
+		.type = ADV7611,
+		.has_afe = false,
+		.max_port = ADV7604_PAD_HDMI_PORT_A,
+		.num_dv_ports = 1,
+		.edid_enable_reg = 0x74,
+		.edid_status_reg = 0x76,
+		.lcf_reg = 0xa3,
+		.tdms_lock_mask = 0x43,
+		.cable_det_mask = 0x01,
+		.fmt_change_digital_mask = 0x03,
+		.formats = adv7611_formats,
+		.nformats = ARRAY_SIZE(adv7611_formats),
+		.set_termination = adv7611_set_termination,
+		.setup_irqs = adv7611_setup_irqs,
+		.read_hdmi_pixelclock = adv7611_read_hdmi_pixelclock,
+		.read_cable_det = adv7611_read_cable_det,
+		.recommended_settings = {
+		    [1] = adv7611_recommended_settings_hdmi,
+		},
+		.num_recommended_settings = {
+		    [1] = ARRAY_SIZE(adv7611_recommended_settings_hdmi),
+		},
+		.page_mask = BIT(ADV7604_PAGE_IO) | BIT(ADV7604_PAGE_CEC) |
+			BIT(ADV7604_PAGE_INFOFRAME) | BIT(ADV7604_PAGE_AFE) |
+			BIT(ADV7604_PAGE_REP) |  BIT(ADV7604_PAGE_EDID) |
+			BIT(ADV7604_PAGE_HDMI) | BIT(ADV7604_PAGE_CP),
+	},
+};
+
+static struct i2c_device_id adv7604_i2c_id[] = {
+	{ "adv7604", (kernel_ulong_t)&adv7604_chip_info[ADV7604] },
+	{ "adv7611", (kernel_ulong_t)&adv7604_chip_info[ADV7611] },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, adv7604_i2c_id);
+
+static struct of_device_id adv7604_of_id[] __maybe_unused = {
+	{ .compatible = "adi,adv7611", .data = &adv7604_chip_info[ADV7611] },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, adv7604_of_id);
+
+static int adv7604_parse_dt(struct adv7604_state *state)
+{
+	struct v4l2_of_endpoint bus_cfg;
+	struct device_node *endpoint;
+	struct device_node *np;
+	unsigned int flags;
+
+	np = state->i2c_clients[ADV7604_PAGE_IO]->dev.of_node;
+
+	/* Parse the endpoint. */
+	endpoint = of_graph_get_next_endpoint(np, NULL);
+	if (!endpoint)
+		return -EINVAL;
+
+	v4l2_of_parse_endpoint(endpoint, &bus_cfg);
+	of_node_put(endpoint);
+
+	flags = bus_cfg.bus.parallel.flags;
+
+	if (flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
+		state->pdata.inv_hs_pol = 1;
+
+	if (flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
+		state->pdata.inv_vs_pol = 1;
+
+	if (flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
+		state->pdata.inv_llc_pol = 1;
+
+	if (bus_cfg.bus_type == V4L2_MBUS_BT656) {
+		state->pdata.insert_av_codes = 1;
+		state->pdata.op_656_range = 1;
+	}
+
+	/* Disable the interrupt for now as no DT-based board uses it. */
+	state->pdata.int1_config = ADV7604_INT1_CONFIG_DISABLED;
+
+	/* Use the default I2C addresses. */
+	state->pdata.i2c_addresses[ADV7604_PAGE_AVLINK] = 0x42;
+	state->pdata.i2c_addresses[ADV7604_PAGE_CEC] = 0x40;
+	state->pdata.i2c_addresses[ADV7604_PAGE_INFOFRAME] = 0x3e;
+	state->pdata.i2c_addresses[ADV7604_PAGE_ESDP] = 0x38;
+	state->pdata.i2c_addresses[ADV7604_PAGE_DPP] = 0x3c;
+	state->pdata.i2c_addresses[ADV7604_PAGE_AFE] = 0x26;
+	state->pdata.i2c_addresses[ADV7604_PAGE_REP] = 0x32;
+	state->pdata.i2c_addresses[ADV7604_PAGE_EDID] = 0x36;
+	state->pdata.i2c_addresses[ADV7604_PAGE_HDMI] = 0x34;
+	state->pdata.i2c_addresses[ADV7604_PAGE_TEST] = 0x30;
+	state->pdata.i2c_addresses[ADV7604_PAGE_CP] = 0x22;
+	state->pdata.i2c_addresses[ADV7604_PAGE_VDP] = 0x24;
+
+	/* Hardcode the remaining platform data fields. */
+	state->pdata.disable_pwrdnb = 0;
+	state->pdata.disable_cable_det_rst = 0;
+	state->pdata.default_input = -1;
+	state->pdata.blank_data = 1;
+	state->pdata.alt_data_sat = 1;
+	state->pdata.op_format_mode_sel = ADV7604_OP_FORMAT_MODE0;
+	state->pdata.bus_order = ADV7604_BUS_ORDER_RGB;
+
+	return 0;
+}
+
 static int adv7604_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id)
 {
 	static const struct v4l2_dv_timings cea640x480 =
 		V4L2_DV_BT_CEA_640X480P59_94;
 	struct adv7604_state *state;
-	struct adv7604_platform_data *pdata = client->dev.platform_data;
 	struct v4l2_ctrl_handler *hdl;
 	struct v4l2_subdev *sd;
+	unsigned int i;
+	u16 val;
 	int err;
 
 	/* Check if the adapter supports the needed features */
@@ -2227,32 +2764,80 @@ static int adv7604_probe(struct i2c_client *client,
 		return -ENOMEM;
 	}
 
+	state->i2c_clients[ADV7604_PAGE_IO] = client;
+
 	/* initialize variables */
 	state->restart_stdi_once = true;
 	state->selected_input = ~0;
 
-	/* platform data */
-	if (!pdata) {
+	if (IS_ENABLED(CONFIG_OF) && client->dev.of_node) {
+		const struct of_device_id *oid;
+
+		oid = of_match_node(adv7604_of_id, client->dev.of_node);
+		state->info = oid->data;
+
+		err = adv7604_parse_dt(state);
+		if (err < 0) {
+			v4l_err(client, "DT parsing error\n");
+			return err;
+		}
+	} else if (client->dev.platform_data) {
+		struct adv7604_platform_data *pdata = client->dev.platform_data;
+
+		state->info = (const struct adv7604_chip_info *)id->driver_data;
+		state->pdata = *pdata;
+	} else {
 		v4l_err(client, "No platform data!\n");
 		return -ENODEV;
 	}
-	state->pdata = *pdata;
+
+	/* Request GPIOs. */
+	for (i = 0; i < state->info->num_dv_ports; ++i) {
+		state->hpd_gpio[i] =
+			devm_gpiod_get_index(&client->dev, "hpd", i);
+		if (IS_ERR(state->hpd_gpio[i]))
+			continue;
+
+		gpiod_direction_output(state->hpd_gpio[i], 0);
+
+		v4l_info(client, "Handling HPD %u GPIO\n", i);
+	}
+
 	state->timings = cea640x480;
+	state->format = adv7604_format_info(state, V4L2_MBUS_FMT_YUYV8_2X8);
 
 	sd = &state->sd;
 	v4l2_i2c_subdev_init(sd, client, &adv7604_ops);
+	snprintf(sd->name, sizeof(sd->name), "%s %d-%04x",
+		id->name, i2c_adapter_id(client->adapter),
+		client->addr);
 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 
-	/* i2c access to adv7604? */
-	if (adv_smbus_read_byte_data_check(client, 0xfb, false) != 0x68) {
-		v4l2_info(sd, "not an adv7604 on address 0x%x\n",
-				client->addr << 1);
-		return -ENODEV;
+	/*
+	 * Verify that the chip is present. On ADV7604 the RD_INFO register only
+	 * identifies the revision, while on ADV7611 it identifies the model as
+	 * well. Use the HDMI slave address on ADV7604 and RD_INFO on ADV7611.
+	 */
+	if (state->info->type == ADV7604) {
+		val = adv_smbus_read_byte_data_check(client, 0xfb, false);
+		if (val != 0x68) {
+			v4l2_info(sd, "not an adv7604 on address 0x%x\n",
+					client->addr << 1);
+			return -ENODEV;
+		}
+	} else {
+		val = (adv_smbus_read_byte_data_check(client, 0xea, false) << 8)
+		    | (adv_smbus_read_byte_data_check(client, 0xeb, false) << 0);
+		if (val != 0x2051) {
+			v4l2_info(sd, "not an adv7611 on address 0x%x\n",
+					client->addr << 1);
+			return -ENODEV;
+		}
 	}
 
 	/* control handlers */
 	hdl = &state->hdl;
-	v4l2_ctrl_handler_init(hdl, 9);
+	v4l2_ctrl_handler_init(hdl, adv7604_has_afe(state) ? 9 : 8);
 
 	v4l2_ctrl_new_std(hdl, &adv7604_ctrl_ops,
 			V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
@@ -2265,15 +2850,17 @@ static int adv7604_probe(struct i2c_client *client,
 
 	/* private controls */
 	state->detect_tx_5v_ctrl = v4l2_ctrl_new_std(hdl, NULL,
-			V4L2_CID_DV_RX_POWER_PRESENT, 0, 0x0f, 0, 0);
+			V4L2_CID_DV_RX_POWER_PRESENT, 0,
+			(1 << state->info->num_dv_ports) - 1, 0, 0);
 	state->rgb_quantization_range_ctrl =
 		v4l2_ctrl_new_std_menu(hdl, &adv7604_ctrl_ops,
 			V4L2_CID_DV_RX_RGB_RANGE, V4L2_DV_RGB_RANGE_FULL,
 			0, V4L2_DV_RGB_RANGE_AUTO);
 
 	/* custom controls */
-	state->analog_sampling_phase_ctrl =
-		v4l2_ctrl_new_custom(hdl, &adv7604_ctrl_analog_sampling_phase, NULL);
+	if (adv7604_has_afe(state))
+		state->analog_sampling_phase_ctrl =
+			v4l2_ctrl_new_custom(hdl, &adv7604_ctrl_analog_sampling_phase, NULL);
 	state->free_run_color_manual_ctrl =
 		v4l2_ctrl_new_custom(hdl, &adv7604_ctrl_free_run_color_manual, NULL);
 	state->free_run_color_ctrl =
@@ -2286,7 +2873,8 @@ static int adv7604_probe(struct i2c_client *client,
 	}
 	state->detect_tx_5v_ctrl->is_private = true;
 	state->rgb_quantization_range_ctrl->is_private = true;
-	state->analog_sampling_phase_ctrl->is_private = true;
+	if (adv7604_has_afe(state))
+		state->analog_sampling_phase_ctrl->is_private = true;
 	state->free_run_color_manual_ctrl->is_private = true;
 	state->free_run_color_ctrl->is_private = true;
 
@@ -2295,25 +2883,18 @@ static int adv7604_probe(struct i2c_client *client,
 		goto err_hdl;
 	}
 
-	state->i2c_avlink = adv7604_dummy_client(sd, pdata->i2c_avlink, 0xf3);
-	state->i2c_cec = adv7604_dummy_client(sd, pdata->i2c_cec, 0xf4);
-	state->i2c_infoframe = adv7604_dummy_client(sd, pdata->i2c_infoframe, 0xf5);
-	state->i2c_esdp = adv7604_dummy_client(sd, pdata->i2c_esdp, 0xf6);
-	state->i2c_dpp = adv7604_dummy_client(sd, pdata->i2c_dpp, 0xf7);
-	state->i2c_afe = adv7604_dummy_client(sd, pdata->i2c_afe, 0xf8);
-	state->i2c_repeater = adv7604_dummy_client(sd, pdata->i2c_repeater, 0xf9);
-	state->i2c_edid = adv7604_dummy_client(sd, pdata->i2c_edid, 0xfa);
-	state->i2c_hdmi = adv7604_dummy_client(sd, pdata->i2c_hdmi, 0xfb);
-	state->i2c_test = adv7604_dummy_client(sd, pdata->i2c_test, 0xfc);
-	state->i2c_cp = adv7604_dummy_client(sd, pdata->i2c_cp, 0xfd);
-	state->i2c_vdp = adv7604_dummy_client(sd, pdata->i2c_vdp, 0xfe);
-	if (!state->i2c_avlink || !state->i2c_cec || !state->i2c_infoframe ||
-	    !state->i2c_esdp || !state->i2c_dpp || !state->i2c_afe ||
-	    !state->i2c_repeater || !state->i2c_edid || !state->i2c_hdmi ||
-	    !state->i2c_test || !state->i2c_cp || !state->i2c_vdp) {
-		err = -ENOMEM;
-		v4l2_err(sd, "failed to create all i2c clients\n");
-		goto err_i2c;
+	for (i = 1; i < ADV7604_PAGE_MAX; ++i) {
+		if (!(BIT(i) & state->info->page_mask))
+			continue;
+
+		state->i2c_clients[i] =
+			adv7604_dummy_client(sd, state->pdata.i2c_addresses[i],
+					     0xf2 + i);
+		if (state->i2c_clients[i] == NULL) {
+			err = -ENOMEM;
+			v4l2_err(sd, "failed to create i2c client %u\n", i);
+			goto err_i2c;
+		}
 	}
 
 	/* work queues */
@@ -2327,8 +2908,14 @@ static int adv7604_probe(struct i2c_client *client,
 	INIT_DELAYED_WORK(&state->delayed_work_enable_hotplug,
 			adv7604_delayed_work_enable_hotplug);
 
-	state->pad.flags = MEDIA_PAD_FL_SOURCE;
-	err = media_entity_init(&sd->entity, 1, &state->pad, 0);
+	state->source_pad = state->info->num_dv_ports
+			  + (state->info->has_afe ? 2 : 0);
+	for (i = 0; i < state->source_pad; ++i)
+		state->pads[i].flags = MEDIA_PAD_FL_SINK;
+	state->pads[state->source_pad].flags = MEDIA_PAD_FL_SOURCE;
+
+	err = media_entity_init(&sd->entity, state->source_pad + 1,
+				state->pads, 0);
 	if (err)
 		goto err_work_queues;
 
@@ -2337,6 +2924,11 @@ static int adv7604_probe(struct i2c_client *client,
 		goto err_entity;
 	v4l2_info(sd, "%s found @ 0x%x (%s)\n", client->name,
 			client->addr << 1, client->adapter->name);
+
+	err = v4l2_async_register_subdev(sd);
+	if (err)
+		goto err_entity;
+
 	return 0;
 
 err_entity:
@@ -2360,6 +2952,7 @@ static int adv7604_remove(struct i2c_client *client)
 
 	cancel_delayed_work(&state->delayed_work_enable_hotplug);
 	destroy_workqueue(state->work_queues);
+	v4l2_async_unregister_subdev(sd);
 	v4l2_device_unregister_subdev(sd);
 	media_entity_cleanup(&sd->entity);
 	adv7604_unregister_clients(to_state(sd));
@@ -2369,20 +2962,15 @@ static int adv7604_remove(struct i2c_client *client)
 
 /* ----------------------------------------------------------------------- */
 
-static struct i2c_device_id adv7604_id[] = {
-	{ "adv7604", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, adv7604_id);
-
 static struct i2c_driver adv7604_driver = {
 	.driver = {
 		.owner = THIS_MODULE,
 		.name = "adv7604",
+		.of_match_table = of_match_ptr(adv7604_of_id),
 	},
 	.probe = adv7604_probe,
 	.remove = adv7604_remove,
-	.id_table = adv7604_id,
+	.id_table = adv7604_i2c_id,
 };
 
 module_i2c_driver(adv7604_driver);
diff --git a/drivers/media/i2c/adv7842.c b/drivers/media/i2c/adv7842.c
index 636ac08925f..0d554919cdd 100644
--- a/drivers/media/i2c/adv7842.c
+++ b/drivers/media/i2c/adv7842.c
@@ -1399,6 +1399,9 @@ static int read_stdi(struct v4l2_subdev *sd, struct stdi_readback *stdi)
 static int adv7842_enum_dv_timings(struct v4l2_subdev *sd,
 				   struct v4l2_enum_dv_timings *timings)
 {
+	if (timings->pad != 0)
+		return -EINVAL;
+
 	return v4l2_enum_dv_timings_cap(timings,
 		adv7842_get_dv_timings_cap(sd), adv7842_check_dv_timings, NULL);
 }
@@ -1406,6 +1409,9 @@ static int adv7842_enum_dv_timings(struct v4l2_subdev *sd,
 static int adv7842_dv_timings_cap(struct v4l2_subdev *sd,
 				  struct v4l2_dv_timings_cap *cap)
 {
+	if (cap->pad != 0)
+		return -EINVAL;
+
 	*cap = *adv7842_get_dv_timings_cap(sd);
 	return 0;
 }
@@ -2000,6 +2006,7 @@ static int adv7842_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
 	if (irq_status[5] & 0x08) {
 		v4l2_dbg(1, debug, sd, "%s: irq %s mode\n", __func__,
 			 (io_read(sd, 0x65) & 0x08) ? "HDMI" : "DVI");
+		set_rgb_quantization_range(sd);
 		if (handled)
 			*handled = true;
 	}
@@ -2029,8 +2036,6 @@ static int adv7842_get_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
 		return -EINVAL;
 	if (edid->start_block == 1)
 		edid->blocks = 1;
-	if (!edid->edid)
-		return -EINVAL;
 
 	switch (edid->pad) {
 	case ADV7842_EDID_PORT_A:
@@ -2065,8 +2070,6 @@ static int adv7842_set_edid(struct v4l2_subdev *sd, struct v4l2_edid *e)
 		return -EINVAL;
 	if (e->blocks > 2)
 		return -E2BIG;
-	if (!e->edid)
-		return -EINVAL;
 
 	/* todo, per edid */
 	state->aspect_ratio = v4l2_calc_aspect_ratio(e->edid[0x15],
@@ -2610,6 +2613,12 @@ static int adv7842_core_init(struct v4l2_subdev *sd)
 
 	disable_input(sd);
 
+	/*
+	 * Disable I2C access to internal EDID ram from HDMI DDC ports
+	 * Disable auto edid enable when leaving powerdown mode
+	 */
+	rep_write_and_or(sd, 0x77, 0xd3, 0x20);
+
 	/* power */
 	io_write(sd, 0x0c, 0x42);   /* Power up part and power down VDP */
 	io_write(sd, 0x15, 0x80);   /* Power up pads */
@@ -2690,9 +2699,6 @@ static int adv7842_core_init(struct v4l2_subdev *sd)
 
 	enable_input(sd);
 
-	/* disable I2C access to internal EDID ram from HDMI DDC ports */
-	rep_write_and_or(sd, 0x77, 0xf3, 0x00);
-
 	if (pdata->hpa_auto) {
 		/* HPA auto, HPA 0.5s after Edid set and Cable detect */
 		hdmi_write(sd, 0x69, 0x5c);
@@ -2869,8 +2875,6 @@ static const struct v4l2_ctrl_ops adv7842_ctrl_ops = {
 
 static const struct v4l2_subdev_core_ops adv7842_core_ops = {
 	.log_status = adv7842_log_status,
-	.g_std = adv7842_g_std,
-	.s_std = adv7842_s_std,
 	.ioctl = adv7842_ioctl,
 	.interrupt_service_routine = adv7842_isr,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
@@ -2880,14 +2884,14 @@ static const struct v4l2_subdev_core_ops adv7842_core_ops = {
 };
 
 static const struct v4l2_subdev_video_ops adv7842_video_ops = {
+	.g_std = adv7842_g_std,
+	.s_std = adv7842_s_std,
 	.s_routing = adv7842_s_routing,
 	.querystd = adv7842_querystd,
 	.g_input_status = adv7842_g_input_status,
 	.s_dv_timings = adv7842_s_dv_timings,
 	.g_dv_timings = adv7842_g_dv_timings,
 	.query_dv_timings = adv7842_query_dv_timings,
-	.enum_dv_timings = adv7842_enum_dv_timings,
-	.dv_timings_cap = adv7842_dv_timings_cap,
 	.enum_mbus_fmt = adv7842_enum_mbus_fmt,
 	.g_mbus_fmt = adv7842_g_mbus_fmt,
 	.try_mbus_fmt = adv7842_g_mbus_fmt,
@@ -2897,6 +2901,8 @@ static const struct v4l2_subdev_video_ops adv7842_video_ops = {
 static const struct v4l2_subdev_pad_ops adv7842_pad_ops = {
 	.get_edid = adv7842_get_edid,
 	.set_edid = adv7842_set_edid,
+	.enum_dv_timings = adv7842_enum_dv_timings,
+	.dv_timings_cap = adv7842_dv_timings_cap,
 };
 
 static const struct v4l2_subdev_ops adv7842_ops = {
diff --git a/drivers/media/i2c/bt819.c b/drivers/media/i2c/bt819.c
index 369cf6ff88f..76b334a6a56 100644
--- a/drivers/media/i2c/bt819.c
+++ b/drivers/media/i2c/bt819.c
@@ -387,10 +387,10 @@ static const struct v4l2_subdev_core_ops bt819_core_ops = {
 	.s_ctrl = v4l2_subdev_s_ctrl,
 	.queryctrl = v4l2_subdev_queryctrl,
 	.querymenu = v4l2_subdev_querymenu,
-	.s_std = bt819_s_std,
 };
 
 static const struct v4l2_subdev_video_ops bt819_video_ops = {
+	.s_std = bt819_s_std,
 	.s_routing = bt819_s_routing,
 	.s_stream = bt819_s_stream,
 	.querystd = bt819_querystd,
diff --git a/drivers/media/i2c/cx25840/cx25840-core.c b/drivers/media/i2c/cx25840/cx25840-core.c
index 2e3771d5735..e453a3ffe7d 100644
--- a/drivers/media/i2c/cx25840/cx25840-core.c
+++ b/drivers/media/i2c/cx25840/cx25840-core.c
@@ -5041,8 +5041,6 @@ static const struct v4l2_subdev_core_ops cx25840_core_ops = {
 	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
 	.queryctrl = v4l2_subdev_queryctrl,
 	.querymenu = v4l2_subdev_querymenu,
-	.s_std = cx25840_s_std,
-	.g_std = cx25840_g_std,
 	.reset = cx25840_reset,
 	.load_fw = cx25840_load_fw,
 	.s_io_pin_config = common_s_io_pin_config,
@@ -5067,6 +5065,8 @@ static const struct v4l2_subdev_audio_ops cx25840_audio_ops = {
 };
 
 static const struct v4l2_subdev_video_ops cx25840_video_ops = {
+	.s_std = cx25840_s_std,
+	.g_std = cx25840_g_std,
 	.s_routing = cx25840_s_video_routing,
 	.s_mbus_fmt = cx25840_s_mbus_fmt,
 	.s_stream = cx25840_s_stream,
diff --git a/drivers/media/i2c/ks0127.c b/drivers/media/i2c/ks0127.c
index c3e94ae82c0..25b81bc58c8 100644
--- a/drivers/media/i2c/ks0127.c
+++ b/drivers/media/i2c/ks0127.c
@@ -648,11 +648,8 @@ static int ks0127_g_input_status(struct v4l2_subdev *sd, u32 *status)
 
 /* ----------------------------------------------------------------------- */
 
-static const struct v4l2_subdev_core_ops ks0127_core_ops = {
-	.s_std = ks0127_s_std,
-};
-
 static const struct v4l2_subdev_video_ops ks0127_video_ops = {
+	.s_std = ks0127_s_std,
 	.s_routing = ks0127_s_routing,
 	.s_stream = ks0127_s_stream,
 	.querystd = ks0127_querystd,
@@ -660,7 +657,6 @@ static const struct v4l2_subdev_video_ops ks0127_video_ops = {
 };
 
 static const struct v4l2_subdev_ops ks0127_ops = {
-	.core = &ks0127_core_ops,
 	.video = &ks0127_video_ops,
 };
 
diff --git a/drivers/media/i2c/m5mols/m5mols_capture.c b/drivers/media/i2c/m5mols/m5mols_capture.c
index ab34ccedf31..1a03d02bd4d 100644
--- a/drivers/media/i2c/m5mols/m5mols_capture.c
+++ b/drivers/media/i2c/m5mols/m5mols_capture.c
@@ -26,7 +26,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-subdev.h>
 #include <media/m5mols.h>
-#include <media/s5p_fimc.h>
+#include <media/exynos-fimc.h>
 
 #include "m5mols.h"
 #include "m5mols_reg.h"
diff --git a/drivers/media/i2c/ml86v7667.c b/drivers/media/i2c/ml86v7667.c
index a9110d8bbbc..2cace7313a2 100644
--- a/drivers/media/i2c/ml86v7667.c
+++ b/drivers/media/i2c/ml86v7667.c
@@ -276,6 +276,7 @@ static const struct v4l2_ctrl_ops ml86v7667_ctrl_ops = {
 };
 
 static struct v4l2_subdev_video_ops ml86v7667_subdev_video_ops = {
+	.s_std = ml86v7667_s_std,
 	.querystd = ml86v7667_querystd,
 	.g_input_status = ml86v7667_g_input_status,
 	.enum_mbus_fmt = ml86v7667_enum_mbus_fmt,
@@ -286,7 +287,6 @@ static struct v4l2_subdev_video_ops ml86v7667_subdev_video_ops = {
 };
 
 static struct v4l2_subdev_core_ops ml86v7667_subdev_core_ops = {
-	.s_std = ml86v7667_s_std,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	.g_register = ml86v7667_g_register,
 	.s_register = ml86v7667_s_register,
diff --git a/drivers/media/i2c/msp3400-driver.c b/drivers/media/i2c/msp3400-driver.c
index 8190fec6808..4d9c6bc3426 100644
--- a/drivers/media/i2c/msp3400-driver.c
+++ b/drivers/media/i2c/msp3400-driver.c
@@ -649,10 +649,10 @@ static const struct v4l2_subdev_core_ops msp_core_ops = {
 	.s_ctrl = v4l2_subdev_s_ctrl,
 	.queryctrl = v4l2_subdev_queryctrl,
 	.querymenu = v4l2_subdev_querymenu,
-	.s_std = msp_s_std,
 };
 
 static const struct v4l2_subdev_video_ops msp_video_ops = {
+	.s_std = msp_s_std,
 	.querystd = msp_querystd,
 };
 
diff --git a/drivers/media/i2c/mt9p031.c b/drivers/media/i2c/mt9p031.c
index 33daace8129..e18797ff7fa 100644
--- a/drivers/media/i2c/mt9p031.c
+++ b/drivers/media/i2c/mt9p031.c
@@ -647,6 +647,28 @@ static int mt9p031_set_crop(struct v4l2_subdev *subdev,
 #define V4L2_CID_BLC_ANALOG_OFFSET	(V4L2_CID_USER_BASE | 0x1004)
 #define V4L2_CID_BLC_DIGITAL_OFFSET	(V4L2_CID_USER_BASE | 0x1005)
 
+static int mt9p031_restore_blc(struct mt9p031 *mt9p031)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
+	int ret;
+
+	if (mt9p031->blc_auto->cur.val != 0) {
+		ret = mt9p031_set_mode2(mt9p031, 0,
+					MT9P031_READ_MODE_2_ROW_BLC);
+		if (ret < 0)
+			return ret;
+	}
+
+	if (mt9p031->blc_offset->cur.val != 0) {
+		ret = mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
+				    mt9p031->blc_offset->cur.val);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
 static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct mt9p031 *mt9p031 =
@@ -655,6 +677,9 @@ static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
 	u16 data;
 	int ret;
 
+	if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE)
+		return 0;
+
 	switch (ctrl->id) {
 	case V4L2_CID_EXPOSURE:
 		ret = mt9p031_write(client, MT9P031_SHUTTER_WIDTH_UPPER,
@@ -709,18 +734,20 @@ static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
 					MT9P031_READ_MODE_2_ROW_MIR, 0);
 
 	case V4L2_CID_TEST_PATTERN:
+		/* The digital side of the Black Level Calibration function must
+		 * be disabled when generating a test pattern to avoid artifacts
+		 * in the image. Activate (deactivate) the BLC-related controls
+		 * when the test pattern is enabled (disabled).
+		 */
+		v4l2_ctrl_activate(mt9p031->blc_auto, ctrl->val == 0);
+		v4l2_ctrl_activate(mt9p031->blc_offset, ctrl->val == 0);
+
 		if (!ctrl->val) {
-			/* Restore the black level compensation settings. */
-			if (mt9p031->blc_auto->cur.val != 0) {
-				ret = mt9p031_s_ctrl(mt9p031->blc_auto);
-				if (ret < 0)
-					return ret;
-			}
-			if (mt9p031->blc_offset->cur.val != 0) {
-				ret = mt9p031_s_ctrl(mt9p031->blc_offset);
-				if (ret < 0)
-					return ret;
-			}
+			/* Restore the BLC settings. */
+			ret = mt9p031_restore_blc(mt9p031);
+			if (ret < 0)
+				return ret;
+
 			return mt9p031_write(client, MT9P031_TEST_PATTERN,
 					     MT9P031_TEST_PATTERN_DISABLE);
 		}
@@ -735,9 +762,7 @@ static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
 		if (ret < 0)
 			return ret;
 
-		/* Disable digital black level compensation when using a test
-		 * pattern.
-		 */
+		/* Disable digital BLC when generating a test pattern. */
 		ret = mt9p031_set_mode2(mt9p031, MT9P031_READ_MODE_2_ROW_BLC,
 					0);
 		if (ret < 0)
diff --git a/drivers/media/i2c/ov7670.c b/drivers/media/i2c/ov7670.c
index e8a1ce20403..cdd7c1b7259 100644
--- a/drivers/media/i2c/ov7670.c
+++ b/drivers/media/i2c/ov7670.c
@@ -1109,7 +1109,7 @@ static int ov7670_enum_framesizes(struct v4l2_subdev *sd,
 	 * windows that fall outside that.
 	 */
 	for (i = 0; i < n_win_sizes; i++) {
-		struct ov7670_win_size *win = &info->devtype->win_sizes[index];
+		struct ov7670_win_size *win = &info->devtype->win_sizes[i];
 		if (info->min_width && win->width < info->min_width)
 			continue;
 		if (info->min_height && win->height < info->min_height)
diff --git a/drivers/media/i2c/s5c73m3/s5c73m3-core.c b/drivers/media/i2c/s5c73m3/s5c73m3-core.c
index a4459301b5f..ee0f57e01b5 100644
--- a/drivers/media/i2c/s5c73m3/s5c73m3-core.c
+++ b/drivers/media/i2c/s5c73m3/s5c73m3-core.c
@@ -1616,7 +1616,7 @@ static int s5c73m3_get_platform_data(struct s5c73m3 *state)
 	if (ret < 0)
 		return -EINVAL;
 
-	node_ep = v4l2_of_get_next_endpoint(node, NULL);
+	node_ep = of_graph_get_next_endpoint(node, NULL);
 	if (!node_ep) {
 		dev_warn(dev, "no endpoint defined for node: %s\n",
 						node->full_name);
diff --git a/drivers/media/i2c/saa6752hs.c b/drivers/media/i2c/saa6752hs.c
index 8272c0b9c5b..04e9e55018a 100644
--- a/drivers/media/i2c/saa6752hs.c
+++ b/drivers/media/i2c/saa6752hs.c
@@ -643,10 +643,10 @@ static const struct v4l2_ctrl_ops saa6752hs_ctrl_ops = {
 
 static const struct v4l2_subdev_core_ops saa6752hs_core_ops = {
 	.init = saa6752hs_init,
-	.s_std = saa6752hs_s_std,
 };
 
 static const struct v4l2_subdev_video_ops saa6752hs_video_ops = {
+	.s_std = saa6752hs_s_std,
 	.s_mbus_fmt = saa6752hs_s_mbus_fmt,
 	.try_mbus_fmt = saa6752hs_try_mbus_fmt,
 	.g_mbus_fmt = saa6752hs_g_mbus_fmt,
diff --git a/drivers/media/i2c/saa7110.c b/drivers/media/i2c/saa7110.c
index ac43e929a1d..99689ee57d7 100644
--- a/drivers/media/i2c/saa7110.c
+++ b/drivers/media/i2c/saa7110.c
@@ -365,10 +365,10 @@ static const struct v4l2_subdev_core_ops saa7110_core_ops = {
 	.s_ctrl = v4l2_subdev_s_ctrl,
 	.queryctrl = v4l2_subdev_queryctrl,
 	.querymenu = v4l2_subdev_querymenu,
-	.s_std = saa7110_s_std,
 };
 
 static const struct v4l2_subdev_video_ops saa7110_video_ops = {
+	.s_std = saa7110_s_std,
 	.s_routing = saa7110_s_routing,
 	.s_stream = saa7110_s_stream,
 	.querystd = saa7110_querystd,
diff --git a/drivers/media/i2c/saa7115.c b/drivers/media/i2c/saa7115.c
index afdbcb045ce..35a44648150 100644
--- a/drivers/media/i2c/saa7115.c
+++ b/drivers/media/i2c/saa7115.c
@@ -1582,7 +1582,6 @@ static const struct v4l2_subdev_core_ops saa711x_core_ops = {
 	.s_ctrl = v4l2_subdev_s_ctrl,
 	.queryctrl = v4l2_subdev_queryctrl,
 	.querymenu = v4l2_subdev_querymenu,
-	.s_std = saa711x_s_std,
 	.reset = saa711x_reset,
 	.s_gpio = saa711x_s_gpio,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
@@ -1601,6 +1600,7 @@ static const struct v4l2_subdev_audio_ops saa711x_audio_ops = {
 };
 
 static const struct v4l2_subdev_video_ops saa711x_video_ops = {
+	.s_std = saa711x_s_std,
 	.s_routing = saa711x_s_routing,
 	.s_crystal_freq = saa711x_s_crystal_freq,
 	.s_mbus_fmt = saa711x_s_mbus_fmt,
diff --git a/drivers/media/i2c/saa717x.c b/drivers/media/i2c/saa717x.c
index 401ca114ab9..6922a9f9a5c 100644
--- a/drivers/media/i2c/saa717x.c
+++ b/drivers/media/i2c/saa717x.c
@@ -1198,7 +1198,6 @@ static const struct v4l2_subdev_core_ops saa717x_core_ops = {
 	.g_register = saa717x_g_register,
 	.s_register = saa717x_s_register,
 #endif
-	.s_std = saa717x_s_std,
 	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
 	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
 	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
@@ -1216,6 +1215,7 @@ static const struct v4l2_subdev_tuner_ops saa717x_tuner_ops = {
 };
 
 static const struct v4l2_subdev_video_ops saa717x_video_ops = {
+	.s_std = saa717x_s_std,
 	.s_routing = saa717x_s_video_routing,
 	.s_mbus_fmt = saa717x_s_mbus_fmt,
 	.s_stream = saa717x_s_stream,
diff --git a/drivers/media/i2c/saa7191.c b/drivers/media/i2c/saa7191.c
index 606a4baf944..8e9699268a6 100644
--- a/drivers/media/i2c/saa7191.c
+++ b/drivers/media/i2c/saa7191.c
@@ -573,10 +573,10 @@ static int saa7191_g_input_status(struct v4l2_subdev *sd, u32 *status)
 static const struct v4l2_subdev_core_ops saa7191_core_ops = {
 	.g_ctrl = saa7191_g_ctrl,
 	.s_ctrl = saa7191_s_ctrl,
-	.s_std = saa7191_s_std,
 };
 
 static const struct v4l2_subdev_video_ops saa7191_video_ops = {
+	.s_std = saa7191_s_std,
 	.s_routing = saa7191_s_routing,
 	.querystd = saa7191_querystd,
 	.g_input_status = saa7191_g_input_status,
diff --git a/drivers/media/i2c/smiapp-pll.h b/drivers/media/i2c/smiapp-pll.h
index a4a649834a1..5ce2b61da3c 100644
--- a/drivers/media/i2c/smiapp-pll.h
+++ b/drivers/media/i2c/smiapp-pll.h
@@ -46,7 +46,7 @@ struct smiapp_pll {
 			uint8_t bus_width;
 		} parallel;
 	};
-	uint8_t flags;
+	unsigned long flags;
 	uint8_t binning_horizontal;
 	uint8_t binning_vertical;
 	uint8_t scale_m;
diff --git a/drivers/media/i2c/smiapp/smiapp-core.c b/drivers/media/i2c/smiapp/smiapp-core.c
index 8741cae9c9f..06fb03291d5 100644
--- a/drivers/media/i2c/smiapp/smiapp-core.c
+++ b/drivers/media/i2c/smiapp/smiapp-core.c
@@ -606,7 +606,7 @@ static int smiapp_get_limits(struct smiapp_sensor *sensor, int const *limit,
 		if (rval)
 			return rval;
 		sensor->limits[limit[i]] = val;
-		dev_dbg(&client->dev, "0x%8.8x \"%s\" = %d, 0x%x\n",
+		dev_dbg(&client->dev, "0x%8.8x \"%s\" = %u, 0x%x\n",
 			smiapp_reg_limits[limit[i]].addr,
 			smiapp_reg_limits[limit[i]].what, val, val);
 	}
@@ -741,8 +741,8 @@ static int smiapp_get_mbus_formats(struct smiapp_sensor *sensor)
 		if (rval)
 			return rval;
 
-		dev_dbg(&client->dev, "bpp %d, compressed %d\n",
-			fmt >> 8, (u8)fmt);
+		dev_dbg(&client->dev, "%u: bpp %u, compressed %u\n",
+			i, fmt >> 8, (u8)fmt);
 
 		for (j = 0; j < ARRAY_SIZE(smiapp_csi_data_formats); j++) {
 			const struct smiapp_csi_data_format *f =
@@ -1128,7 +1128,7 @@ static int smiapp_power_on(struct smiapp_sensor *sensor)
 	}
 	usleep_range(1000, 1000);
 
-	if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
+	if (gpio_is_valid(sensor->platform_data->xshutdown))
 		gpio_set_value(sensor->platform_data->xshutdown, 1);
 
 	sleep = SMIAPP_RESET_DELAY(sensor->platform_data->ext_clk);
@@ -1238,7 +1238,7 @@ static int smiapp_power_on(struct smiapp_sensor *sensor)
 	return 0;
 
 out_cci_addr_fail:
-	if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
+	if (gpio_is_valid(sensor->platform_data->xshutdown))
 		gpio_set_value(sensor->platform_data->xshutdown, 0);
 	if (sensor->platform_data->set_xclk)
 		sensor->platform_data->set_xclk(&sensor->src->sd, 0);
@@ -1264,7 +1264,7 @@ static void smiapp_power_off(struct smiapp_sensor *sensor)
 			     SMIAPP_REG_U8_SOFTWARE_RESET,
 			     SMIAPP_SOFTWARE_RESET);
 
-	if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
+	if (gpio_is_valid(sensor->platform_data->xshutdown))
 		gpio_set_value(sensor->platform_data->xshutdown, 0);
 	if (sensor->platform_data->set_xclk)
 		sensor->platform_data->set_xclk(&sensor->src->sd, 0);
@@ -1766,7 +1766,7 @@ static void smiapp_set_compose_binner(struct v4l2_subdev *subdev,
 	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
 	unsigned int i;
 	unsigned int binh = 1, binv = 1;
-	unsigned int best = scaling_goodness(
+	int best = scaling_goodness(
 		subdev,
 		crops[SMIAPP_PAD_SINK]->width, sel->r.width,
 		crops[SMIAPP_PAD_SINK]->height, sel->r.height, sel->flags);
@@ -2355,17 +2355,17 @@ static int smiapp_registered(struct v4l2_subdev *subdev)
 	unsigned int i;
 	int rval;
 
-	sensor->vana = devm_regulator_get(&client->dev, "VANA");
+	sensor->vana = devm_regulator_get(&client->dev, "vana");
 	if (IS_ERR(sensor->vana)) {
 		dev_err(&client->dev, "could not get regulator for vana\n");
-		return -ENODEV;
+		return PTR_ERR(sensor->vana);
 	}
 
 	if (!sensor->platform_data->set_xclk) {
 		sensor->ext_clk = devm_clk_get(&client->dev, "ext_clk");
 		if (IS_ERR(sensor->ext_clk)) {
 			dev_err(&client->dev, "could not get clock\n");
-			return -ENODEV;
+			return PTR_ERR(sensor->ext_clk);
 		}
 
 		rval = clk_set_rate(sensor->ext_clk,
@@ -2374,18 +2374,19 @@ static int smiapp_registered(struct v4l2_subdev *subdev)
 			dev_err(&client->dev,
 				"unable to set clock freq to %u\n",
 				sensor->platform_data->ext_clk);
-			return -ENODEV;
+			return rval;
 		}
 	}
 
-	if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN) {
-		if (devm_gpio_request_one(&client->dev,
-					  sensor->platform_data->xshutdown, 0,
-					  "SMIA++ xshutdown") != 0) {
+	if (gpio_is_valid(sensor->platform_data->xshutdown)) {
+		rval = devm_gpio_request_one(
+			&client->dev, sensor->platform_data->xshutdown, 0,
+			"SMIA++ xshutdown");
+		if (rval < 0) {
 			dev_err(&client->dev,
 				"unable to acquire reset gpio %d\n",
 				sensor->platform_data->xshutdown);
-			return -ENODEV;
+			return rval;
 		}
 	}
 
@@ -2423,6 +2424,12 @@ static int smiapp_registered(struct v4l2_subdev *subdev)
 		sensor->hvflip_inv_mask = SMIAPP_IMAGE_ORIENTATION_HFLIP |
 					  SMIAPP_IMAGE_ORIENTATION_VFLIP;
 
+	rval = smiapp_call_quirk(sensor, limits);
+	if (rval) {
+		dev_err(&client->dev, "limits quirks failed\n");
+		goto out_power_off;
+	}
+
 	rval = smiapp_get_mbus_formats(sensor);
 	if (rval) {
 		rval = -ENODEV;
@@ -2483,12 +2490,6 @@ static int smiapp_registered(struct v4l2_subdev *subdev)
 		}
 	}
 
-	rval = smiapp_call_quirk(sensor, limits);
-	if (rval) {
-		dev_err(&client->dev, "limits quirks failed\n");
-		goto out_nvm_release;
-	}
-
 	/* We consider this as profile 0 sensor if any of these are zero. */
 	if (!sensor->limits[SMIAPP_LIMIT_MIN_OP_SYS_CLK_DIV] ||
 	    !sensor->limits[SMIAPP_LIMIT_MAX_OP_SYS_CLK_DIV] ||
@@ -2543,8 +2544,9 @@ static int smiapp_registered(struct v4l2_subdev *subdev)
 		}
 
 		snprintf(this->sd.name,
-			 sizeof(this->sd.name), "%s %s",
-			 sensor->minfo.name, _this->name);
+			 sizeof(this->sd.name), "%s %d-%4.4x %s",
+			 sensor->minfo.name, i2c_adapter_id(client->adapter),
+			 client->addr, _this->name);
 
 		this->sink_fmt.width =
 			sensor->limits[SMIAPP_LIMIT_X_ADDR_MAX] + 1;
@@ -2616,12 +2618,11 @@ static int smiapp_registered(struct v4l2_subdev *subdev)
 	pll->bus_type = SMIAPP_PLL_BUS_TYPE_CSI2;
 	pll->csi2.lanes = sensor->platform_data->lanes;
 	pll->ext_clk_freq_hz = sensor->platform_data->ext_clk;
+	pll->flags = smiapp_call_quirk(sensor, pll_flags);
+
 	/* Profile 0 sensors have no separate OP clock branch. */
 	if (sensor->minfo.smiapp_profile == SMIAPP_PROFILE_0)
 		pll->flags |= SMIAPP_PLL_FLAG_NO_OP_CLOCKS;
-	if (smiapp_needs_quirk(sensor,
-			       SMIAPP_QUIRK_FLAG_OP_PIX_CLOCK_PER_LANE))
-		pll->flags |= SMIAPP_PLL_FLAG_OP_PIX_CLOCK_PER_LANE;
 	pll->scale_n = sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN];
 
 	rval = smiapp_update_mode(sensor);
@@ -2830,7 +2831,7 @@ static int smiapp_remove(struct i2c_client *client)
 	unsigned int i;
 
 	if (sensor->power_count) {
-		if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
+		if (gpio_is_valid(sensor->platform_data->xshutdown))
 			gpio_set_value(sensor->platform_data->xshutdown, 0);
 		if (sensor->platform_data->set_xclk)
 			sensor->platform_data->set_xclk(&sensor->src->sd, 0);
diff --git a/drivers/media/i2c/smiapp/smiapp-quirk.c b/drivers/media/i2c/smiapp/smiapp-quirk.c
index bb8c506e0e3..e0bee875212 100644
--- a/drivers/media/i2c/smiapp/smiapp-quirk.c
+++ b/drivers/media/i2c/smiapp/smiapp-quirk.c
@@ -28,7 +28,7 @@
 
 static int smiapp_write_8(struct smiapp_sensor *sensor, u16 reg, u8 val)
 {
-	return smiapp_write(sensor, (SMIA_REG_8BIT << 16) | reg, val);
+	return smiapp_write(sensor, SMIAPP_REG_MK_U8(reg), val);
 }
 
 static int smiapp_write_8s(struct smiapp_sensor *sensor,
@@ -61,52 +61,6 @@ void smiapp_replace_limit(struct smiapp_sensor *sensor,
 	sensor->limits[limit] = val;
 }
 
-bool smiapp_quirk_reg(struct smiapp_sensor *sensor,
-		      u32 reg, u32 *val)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
-	const struct smia_reg *sreg;
-
-	if (!sensor->minfo.quirk)
-		return false;
-
-	sreg = sensor->minfo.quirk->regs;
-
-	if (!sreg)
-		return false;
-
-	while (sreg->type) {
-		u16 type = reg >> 16;
-		u16 reg16 = reg;
-
-		if (sreg->type != type || sreg->reg != reg16) {
-			sreg++;
-			continue;
-		}
-
-		switch ((u8)type) {
-		case SMIA_REG_8BIT:
-			dev_dbg(&client->dev, "quirk: 0x%8.8x: 0x%2.2x\n",
-				reg, sreg->val);
-			break;
-		case SMIA_REG_16BIT:
-			dev_dbg(&client->dev, "quirk: 0x%8.8x: 0x%4.4x\n",
-				reg, sreg->val);
-			break;
-		case SMIA_REG_32BIT:
-			dev_dbg(&client->dev, "quirk: 0x%8.8x: 0x%8.8x\n",
-				reg, sreg->val);
-			break;
-		}
-
-		*val = sreg->val;
-
-		return true;
-	}
-
-	return false;
-}
-
 static int jt8ew9_limits(struct smiapp_sensor *sensor)
 {
 	if (sensor->minfo.revision_number_major < 0x03)
@@ -266,12 +220,17 @@ static int jt8ev1_post_streamoff(struct smiapp_sensor *sensor)
 	return smiapp_write_8(sensor, 0x3328, 0x80);
 }
 
+static unsigned long jt8ev1_pll_flags(struct smiapp_sensor *sensor)
+{
+	return SMIAPP_PLL_FLAG_OP_PIX_CLOCK_PER_LANE;
+}
+
 const struct smiapp_quirk smiapp_jt8ev1_quirk = {
 	.limits = jt8ev1_limits,
 	.post_poweron = jt8ev1_post_poweron,
 	.pre_streamon = jt8ev1_pre_streamon,
 	.post_streamoff = jt8ev1_post_streamoff,
-	.flags = SMIAPP_QUIRK_FLAG_OP_PIX_CLOCK_PER_LANE,
+	.pll_flags = jt8ev1_pll_flags,
 };
 
 static int tcm8500md_limits(struct smiapp_sensor *sensor)
diff --git a/drivers/media/i2c/smiapp/smiapp-quirk.h b/drivers/media/i2c/smiapp/smiapp-quirk.h
index 504a6d80ced..46e9ea8bfa0 100644
--- a/drivers/media/i2c/smiapp/smiapp-quirk.h
+++ b/drivers/media/i2c/smiapp/smiapp-quirk.h
@@ -35,19 +35,30 @@ struct smiapp_sensor;
  * @post_poweron: Called always after the sensor has been fully powered on.
  * @pre_streamon: Called just before streaming is enabled.
  * @post_streamon: Called right after stopping streaming.
+ * @reg_access: Register access quirk. The quirk may divert the access
+ *		to another register, or no register at all.
+ *
+ *		@write: Is this read (false) or write (true) access?
+ *		@reg: Pointer to the register to access
+ *		@value: Register value, set by the caller on write, or
+ *			by the quirk on read
+ *
+ *		@return: 0 on success, -ENOIOCTLCMD if no register
+ *			 access may be done by the caller (default read
+ *			 value is zero), else negative error code on error
  */
 struct smiapp_quirk {
 	int (*limits)(struct smiapp_sensor *sensor);
 	int (*post_poweron)(struct smiapp_sensor *sensor);
 	int (*pre_streamon)(struct smiapp_sensor *sensor);
 	int (*post_streamoff)(struct smiapp_sensor *sensor);
-	const struct smia_reg *regs;
+	unsigned long (*pll_flags)(struct smiapp_sensor *sensor);
+	int (*reg_access)(struct smiapp_sensor *sensor, bool write, u32 *reg,
+			  u32 *val);
 	unsigned long flags;
 };
 
-/* op pix clock is for all lanes in total normally */
-#define SMIAPP_QUIRK_FLAG_OP_PIX_CLOCK_PER_LANE			(1 << 0)
-#define SMIAPP_QUIRK_FLAG_8BIT_READ_ONLY			(1 << 1)
+#define SMIAPP_QUIRK_FLAG_8BIT_READ_ONLY			(1 << 0)
 
 struct smiapp_reg_8 {
 	u16 reg;
@@ -56,12 +67,9 @@ struct smiapp_reg_8 {
 
 void smiapp_replace_limit(struct smiapp_sensor *sensor,
 			  u32 limit, u32 val);
-bool smiapp_quirk_reg(struct smiapp_sensor *sensor,
-		      u32 reg, u32 *val);
 
-#define SMIAPP_MK_QUIRK_REG(_reg, _val) \
+#define SMIAPP_MK_QUIRK_REG_8(_reg, _val) \
 	{				\
-		.type = (_reg >> 16),	\
 		.reg = (u16)_reg,	\
 		.val = _val,		\
 	}
diff --git a/drivers/media/i2c/smiapp/smiapp-reg-defs.h b/drivers/media/i2c/smiapp/smiapp-reg-defs.h
index 3aa0ca948d8..c488ef02807 100644
--- a/drivers/media/i2c/smiapp/smiapp-reg-defs.h
+++ b/drivers/media/i2c/smiapp/smiapp-reg-defs.h
@@ -21,11 +21,11 @@
  * 02110-1301 USA
  *
  */
-#define SMIAPP_REG_MK_U8(r) ((SMIA_REG_8BIT << 16) | (r))
-#define SMIAPP_REG_MK_U16(r) ((SMIA_REG_16BIT << 16) | (r))
-#define SMIAPP_REG_MK_U32(r) ((SMIA_REG_32BIT << 16) | (r))
+#define SMIAPP_REG_MK_U8(r) ((SMIAPP_REG_8BIT << 16) | (r))
+#define SMIAPP_REG_MK_U16(r) ((SMIAPP_REG_16BIT << 16) | (r))
+#define SMIAPP_REG_MK_U32(r) ((SMIAPP_REG_32BIT << 16) | (r))
 
-#define SMIAPP_REG_MK_F32(r) (SMIA_REG_FLAG_FLOAT | (SMIA_REG_32BIT << 16) | (r))
+#define SMIAPP_REG_MK_F32(r) (SMIAPP_REG_FLAG_FLOAT | (SMIAPP_REG_32BIT << 16) | (r))
 
 #define SMIAPP_REG_U16_MODEL_ID					SMIAPP_REG_MK_U16(0x0000)
 #define SMIAPP_REG_U8_REVISION_NUMBER_MAJOR			SMIAPP_REG_MK_U8(0x0002)
diff --git a/drivers/media/i2c/smiapp/smiapp-regs.c b/drivers/media/i2c/smiapp/smiapp-regs.c
index 4fac32cfcb3..a2098007fb7 100644
--- a/drivers/media/i2c/smiapp/smiapp-regs.c
+++ b/drivers/media/i2c/smiapp/smiapp-regs.c
@@ -114,14 +114,14 @@ static int ____smiapp_read(struct smiapp_sensor *sensor, u16 reg,
 	*val = 0;
 	/* high byte comes first */
 	switch (len) {
-	case SMIA_REG_32BIT:
+	case SMIAPP_REG_32BIT:
 		*val = (data[0] << 24) + (data[1] << 16) + (data[2] << 8) +
 			data[3];
 		break;
-	case SMIA_REG_16BIT:
+	case SMIAPP_REG_16BIT:
 		*val = (data[0] << 8) + data[1];
 		break;
-	case SMIA_REG_8BIT:
+	case SMIAPP_REG_8BIT:
 		*val = data[0];
 		break;
 	default:
@@ -165,31 +165,28 @@ static int __smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val,
 			 bool only8)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
-	unsigned int len = (u8)(reg >> 16);
+	u8 len = SMIAPP_REG_WIDTH(reg);
 	int rval;
 
-	if (len != SMIA_REG_8BIT && len != SMIA_REG_16BIT
-	    && len != SMIA_REG_32BIT)
+	if (len != SMIAPP_REG_8BIT && len != SMIAPP_REG_16BIT
+	    && len != SMIAPP_REG_32BIT)
 		return -EINVAL;
 
-	if (smiapp_quirk_reg(sensor, reg, val))
-		goto found_quirk;
-
-	if (len == SMIA_REG_8BIT && !only8)
-		rval = ____smiapp_read(sensor, (u16)reg, len, val);
+	if (len == SMIAPP_REG_8BIT || !only8)
+		rval = ____smiapp_read(sensor, SMIAPP_REG_ADDR(reg), len, val);
 	else
-		rval = ____smiapp_read_8only(sensor, (u16)reg, len, val);
+		rval = ____smiapp_read_8only(sensor, SMIAPP_REG_ADDR(reg), len,
+					     val);
 	if (rval < 0)
 		return rval;
 
-found_quirk:
-	if (reg & SMIA_REG_FLAG_FLOAT)
+	if (reg & SMIAPP_REG_FLAG_FLOAT)
 		*val = float_to_u32_mul_1000000(client, *val);
 
 	return 0;
 }
 
-int smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val)
+int smiapp_read_no_quirk(struct smiapp_sensor *sensor, u32 reg, u32 *val)
 {
 	return __smiapp_read(
 		sensor, reg, val,
@@ -197,28 +194,47 @@ int smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val)
 				   SMIAPP_QUIRK_FLAG_8BIT_READ_ONLY));
 }
 
+int smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val)
+{
+	int rval;
+
+	*val = 0;
+	rval = smiapp_call_quirk(sensor, reg_access, false, &reg, val);
+	if (rval == -ENOIOCTLCMD)
+		return 0;
+	if (rval < 0)
+		return rval;
+
+	return smiapp_read_no_quirk(sensor, reg, val);
+}
+
 int smiapp_read_8only(struct smiapp_sensor *sensor, u32 reg, u32 *val)
 {
+	int rval;
+
+	*val = 0;
+	rval = smiapp_call_quirk(sensor, reg_access, false, &reg, val);
+	if (rval == -ENOIOCTLCMD)
+		return 0;
+	if (rval < 0)
+		return rval;
+
 	return __smiapp_read(sensor, reg, val, true);
 }
 
-/*
- * Write to a 8/16-bit register.
- * Returns zero if successful, or non-zero otherwise.
- */
-int smiapp_write(struct smiapp_sensor *sensor, u32 reg, u32 val)
+int smiapp_write_no_quirk(struct smiapp_sensor *sensor, u32 reg, u32 val)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
 	struct i2c_msg msg;
 	unsigned char data[6];
 	unsigned int retries;
-	unsigned int flags = reg >> 24;
-	unsigned int len = (u8)(reg >> 16);
-	u16 offset = reg;
+	u8 flags = SMIAPP_REG_FLAGS(reg);
+	u8 len = SMIAPP_REG_WIDTH(reg);
+	u16 offset = SMIAPP_REG_ADDR(reg);
 	int r;
 
-	if ((len != SMIA_REG_8BIT && len != SMIA_REG_16BIT &&
-	     len != SMIA_REG_32BIT) || flags)
+	if ((len != SMIAPP_REG_8BIT && len != SMIAPP_REG_16BIT &&
+	     len != SMIAPP_REG_32BIT) || flags)
 		return -EINVAL;
 
 	msg.addr = client->addr;
@@ -231,14 +247,14 @@ int smiapp_write(struct smiapp_sensor *sensor, u32 reg, u32 val)
 	data[1] = (u8) (reg & 0xff);
 
 	switch (len) {
-	case SMIA_REG_8BIT:
+	case SMIAPP_REG_8BIT:
 		data[2] = val;
 		break;
-	case SMIA_REG_16BIT:
+	case SMIAPP_REG_16BIT:
 		data[2] = val >> 8;
 		data[3] = val;
 		break;
-	case SMIA_REG_32BIT:
+	case SMIAPP_REG_32BIT:
 		data[2] = val >> 24;
 		data[3] = val >> 16;
 		data[4] = val >> 8;
@@ -271,3 +287,20 @@ int smiapp_write(struct smiapp_sensor *sensor, u32 reg, u32 val)
 
 	return r;
 }
+
+/*
+ * Write to a 8/16-bit register.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+int smiapp_write(struct smiapp_sensor *sensor, u32 reg, u32 val)
+{
+	int rval;
+
+	rval = smiapp_call_quirk(sensor, reg_access, true, &reg, &val);
+	if (rval == -ENOIOCTLCMD)
+		return 0;
+	if (rval < 0)
+		return rval;
+
+	return smiapp_write_no_quirk(sensor, reg, val);
+}
diff --git a/drivers/media/i2c/smiapp/smiapp-regs.h b/drivers/media/i2c/smiapp/smiapp-regs.h
index eefc6c84d5f..35521125a2c 100644
--- a/drivers/media/i2c/smiapp/smiapp-regs.h
+++ b/drivers/media/i2c/smiapp/smiapp-regs.h
@@ -28,22 +28,23 @@
 #include <linux/i2c.h>
 #include <linux/types.h>
 
+#define SMIAPP_REG_ADDR(reg)		((u16)reg)
+#define SMIAPP_REG_WIDTH(reg)		((u8)(reg >> 16))
+#define SMIAPP_REG_FLAGS(reg)		((u8)(reg >> 24))
+
 /* Use upper 8 bits of the type field for flags */
-#define SMIA_REG_FLAG_FLOAT		(1 << 24)
+#define SMIAPP_REG_FLAG_FLOAT		(1 << 24)
 
-#define SMIA_REG_8BIT			1
-#define SMIA_REG_16BIT			2
-#define SMIA_REG_32BIT			4
-struct smia_reg {
-	u16 type;
-	u16 reg;			/* 16-bit offset */
-	u32 val;			/* 8/16/32-bit value */
-};
+#define SMIAPP_REG_8BIT			1
+#define SMIAPP_REG_16BIT		2
+#define SMIAPP_REG_32BIT		4
 
 struct smiapp_sensor;
 
+int smiapp_read_no_quirk(struct smiapp_sensor *sensor, u32 reg, u32 *val);
 int smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val);
 int smiapp_read_8only(struct smiapp_sensor *sensor, u32 reg, u32 *val);
+int smiapp_write_no_quirk(struct smiapp_sensor *sensor, u32 reg, u32 val);
 int smiapp_write(struct smiapp_sensor *sensor, u32 reg, u32 val);
 
 #endif
diff --git a/drivers/media/i2c/soc_camera/tw9910.c b/drivers/media/i2c/soc_camera/tw9910.c
index ab54628d941..416402eb4f8 100644
--- a/drivers/media/i2c/soc_camera/tw9910.c
+++ b/drivers/media/i2c/soc_camera/tw9910.c
@@ -814,8 +814,6 @@ done:
 }
 
 static struct v4l2_subdev_core_ops tw9910_subdev_core_ops = {
-	.s_std		= tw9910_s_std,
-	.g_std		= tw9910_g_std,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	.g_register	= tw9910_g_register,
 	.s_register	= tw9910_s_register,
@@ -872,7 +870,15 @@ static int tw9910_s_mbus_config(struct v4l2_subdev *sd,
 	return i2c_smbus_write_byte_data(client, OUTCTR1, val);
 }
 
+static int tw9910_g_tvnorms(struct v4l2_subdev *sd, v4l2_std_id *norm)
+{
+	*norm = V4L2_STD_NTSC | V4L2_STD_PAL;
+	return 0;
+}
+
 static struct v4l2_subdev_video_ops tw9910_subdev_video_ops = {
+	.s_std		= tw9910_s_std,
+	.g_std		= tw9910_g_std,
 	.s_stream	= tw9910_s_stream,
 	.g_mbus_fmt	= tw9910_g_fmt,
 	.s_mbus_fmt	= tw9910_s_fmt,
@@ -882,6 +888,7 @@ static struct v4l2_subdev_video_ops tw9910_subdev_video_ops = {
 	.enum_mbus_fmt	= tw9910_enum_fmt,
 	.g_mbus_config	= tw9910_g_mbus_config,
 	.s_mbus_config	= tw9910_s_mbus_config,
+	.g_tvnorms	= tw9910_g_tvnorms,
 };
 
 static struct v4l2_subdev_ops tw9910_subdev_ops = {
diff --git a/drivers/media/i2c/sony-btf-mpx.c b/drivers/media/i2c/sony-btf-mpx.c
index 32d82320b48..1da8004f5a8 100644
--- a/drivers/media/i2c/sony-btf-mpx.c
+++ b/drivers/media/i2c/sony-btf-mpx.c
@@ -327,18 +327,18 @@ static int sony_btf_mpx_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner
 
 /* --------------------------------------------------------------------------*/
 
-static const struct v4l2_subdev_core_ops sony_btf_mpx_core_ops = {
-	.s_std = sony_btf_mpx_s_std,
-};
-
 static const struct v4l2_subdev_tuner_ops sony_btf_mpx_tuner_ops = {
 	.s_tuner = sony_btf_mpx_s_tuner,
 	.g_tuner = sony_btf_mpx_g_tuner,
 };
 
+static const struct v4l2_subdev_video_ops sony_btf_mpx_video_ops = {
+	.s_std = sony_btf_mpx_s_std,
+};
+
 static const struct v4l2_subdev_ops sony_btf_mpx_ops = {
-	.core = &sony_btf_mpx_core_ops,
 	.tuner = &sony_btf_mpx_tuner_ops,
+	.video = &sony_btf_mpx_video_ops,
 };
 
 /* --------------------------------------------------------------------------*/
diff --git a/drivers/media/i2c/ths8200.c b/drivers/media/i2c/ths8200.c
index f72561e7973..656d889c1c7 100644
--- a/drivers/media/i2c/ths8200.c
+++ b/drivers/media/i2c/ths8200.c
@@ -410,6 +410,9 @@ static int ths8200_g_dv_timings(struct v4l2_subdev *sd,
 static int ths8200_enum_dv_timings(struct v4l2_subdev *sd,
 				   struct v4l2_enum_dv_timings *timings)
 {
+	if (timings->pad != 0)
+		return -EINVAL;
+
 	return v4l2_enum_dv_timings_cap(timings, &ths8200_timings_cap,
 			NULL, NULL);
 }
@@ -417,6 +420,9 @@ static int ths8200_enum_dv_timings(struct v4l2_subdev *sd,
 static int ths8200_dv_timings_cap(struct v4l2_subdev *sd,
 				  struct v4l2_dv_timings_cap *cap)
 {
+	if (cap->pad != 0)
+		return -EINVAL;
+
 	*cap = ths8200_timings_cap;
 	return 0;
 }
@@ -426,6 +432,9 @@ static const struct v4l2_subdev_video_ops ths8200_video_ops = {
 	.s_stream = ths8200_s_stream,
 	.s_dv_timings = ths8200_s_dv_timings,
 	.g_dv_timings = ths8200_g_dv_timings,
+};
+
+static const struct v4l2_subdev_pad_ops ths8200_pad_ops = {
 	.enum_dv_timings = ths8200_enum_dv_timings,
 	.dv_timings_cap = ths8200_dv_timings_cap,
 };
@@ -434,6 +443,7 @@ static const struct v4l2_subdev_video_ops ths8200_video_ops = {
 static const struct v4l2_subdev_ops ths8200_ops = {
 	.core  = &ths8200_core_ops,
 	.video = &ths8200_video_ops,
+	.pad = &ths8200_pad_ops,
 };
 
 static int ths8200_probe(struct i2c_client *client,
diff --git a/drivers/media/i2c/tvaudio.c b/drivers/media/i2c/tvaudio.c
index d76c53a8f02..070c152da95 100644
--- a/drivers/media/i2c/tvaudio.c
+++ b/drivers/media/i2c/tvaudio.c
@@ -1862,7 +1862,6 @@ static const struct v4l2_subdev_core_ops tvaudio_core_ops = {
 	.s_ctrl = v4l2_subdev_s_ctrl,
 	.queryctrl = v4l2_subdev_queryctrl,
 	.querymenu = v4l2_subdev_querymenu,
-	.s_std = tvaudio_s_std,
 };
 
 static const struct v4l2_subdev_tuner_ops tvaudio_tuner_ops = {
@@ -1876,10 +1875,15 @@ static const struct v4l2_subdev_audio_ops tvaudio_audio_ops = {
 	.s_routing = tvaudio_s_routing,
 };
 
+static const struct v4l2_subdev_video_ops tvaudio_video_ops = {
+	.s_std = tvaudio_s_std,
+};
+
 static const struct v4l2_subdev_ops tvaudio_ops = {
 	.core = &tvaudio_core_ops,
 	.tuner = &tvaudio_tuner_ops,
 	.audio = &tvaudio_audio_ops,
+	.video = &tvaudio_video_ops,
 };
 
 /* ----------------------------------------------------------------------- */
diff --git a/drivers/media/i2c/tvp514x.c b/drivers/media/i2c/tvp514x.c
index ca001178c5b..b9dabc9f405 100644
--- a/drivers/media/i2c/tvp514x.c
+++ b/drivers/media/i2c/tvp514x.c
@@ -1010,10 +1010,10 @@ static const struct v4l2_subdev_core_ops tvp514x_core_ops = {
 	.s_ctrl = v4l2_subdev_s_ctrl,
 	.queryctrl = v4l2_subdev_queryctrl,
 	.querymenu = v4l2_subdev_querymenu,
-	.s_std = tvp514x_s_std,
 };
 
 static const struct v4l2_subdev_video_ops tvp514x_video_ops = {
+	.s_std = tvp514x_s_std,
 	.s_routing = tvp514x_s_routing,
 	.querystd = tvp514x_querystd,
 	.enum_mbus_fmt = tvp514x_enum_mbus_fmt,
diff --git a/drivers/media/i2c/tvp5150.c b/drivers/media/i2c/tvp5150.c
index 4fd3688e116..a9121254e37 100644
--- a/drivers/media/i2c/tvp5150.c
+++ b/drivers/media/i2c/tvp5150.c
@@ -913,7 +913,7 @@ static int tvp5150_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
 
 static int tvp5150_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
 {
-	struct tvp5150 *decoder = container_of(sd, struct tvp5150, sd);
+	struct tvp5150 *decoder = to_tvp5150(sd);
 
 	a->c	= decoder->rect;
 	a->type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
@@ -923,7 +923,7 @@ static int tvp5150_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
 
 static int tvp5150_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
 {
-	struct tvp5150 *decoder = container_of(sd, struct tvp5150, sd);
+	struct tvp5150 *decoder = to_tvp5150(sd);
 	v4l2_std_id std;
 
 	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
@@ -1063,7 +1063,6 @@ static const struct v4l2_ctrl_ops tvp5150_ctrl_ops = {
 
 static const struct v4l2_subdev_core_ops tvp5150_core_ops = {
 	.log_status = tvp5150_log_status,
-	.s_std = tvp5150_s_std,
 	.reset = tvp5150_reset,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	.g_register = tvp5150_g_register,
@@ -1076,6 +1075,7 @@ static const struct v4l2_subdev_tuner_ops tvp5150_tuner_ops = {
 };
 
 static const struct v4l2_subdev_video_ops tvp5150_video_ops = {
+	.s_std = tvp5150_s_std,
 	.s_routing = tvp5150_s_routing,
 	.enum_mbus_fmt = tvp5150_enum_mbus_fmt,
 	.s_mbus_fmt = tvp5150_mbus_fmt,
diff --git a/drivers/media/i2c/tvp7002.c b/drivers/media/i2c/tvp7002.c
index c4e1e2cb309..11f2387e1da 100644
--- a/drivers/media/i2c/tvp7002.c
+++ b/drivers/media/i2c/tvp7002.c
@@ -833,6 +833,9 @@ static int tvp7002_log_status(struct v4l2_subdev *sd)
 static int tvp7002_enum_dv_timings(struct v4l2_subdev *sd,
 		struct v4l2_enum_dv_timings *timings)
 {
+	if (timings->pad != 0)
+		return -EINVAL;
+
 	/* Check requested format index is within range */
 	if (timings->index >= NUM_TIMINGS)
 		return -EINVAL;
@@ -924,7 +927,6 @@ static const struct v4l2_subdev_core_ops tvp7002_core_ops = {
 static const struct v4l2_subdev_video_ops tvp7002_video_ops = {
 	.g_dv_timings = tvp7002_g_dv_timings,
 	.s_dv_timings = tvp7002_s_dv_timings,
-	.enum_dv_timings = tvp7002_enum_dv_timings,
 	.query_dv_timings = tvp7002_query_dv_timings,
 	.s_stream = tvp7002_s_stream,
 	.g_mbus_fmt = tvp7002_mbus_fmt,
@@ -938,6 +940,7 @@ static const struct v4l2_subdev_pad_ops tvp7002_pad_ops = {
 	.enum_mbus_code = tvp7002_enum_mbus_code,
 	.get_fmt = tvp7002_get_pad_format,
 	.set_fmt = tvp7002_set_pad_format,
+	.enum_dv_timings = tvp7002_enum_dv_timings,
 };
 
 /* V4L2 top level operation handlers */
diff --git a/drivers/media/i2c/tw2804.c b/drivers/media/i2c/tw2804.c
index f58607df619..7347480c0b0 100644
--- a/drivers/media/i2c/tw2804.c
+++ b/drivers/media/i2c/tw2804.c
@@ -342,12 +342,12 @@ static const struct v4l2_ctrl_ops tw2804_ctrl_ops = {
 };
 
 static const struct v4l2_subdev_video_ops tw2804_video_ops = {
+	.s_std = tw2804_s_std,
 	.s_routing = tw2804_s_video_routing,
 };
 
 static const struct v4l2_subdev_core_ops tw2804_core_ops = {
 	.log_status = tw2804_log_status,
-	.s_std = tw2804_s_std,
 };
 
 static const struct v4l2_subdev_ops tw2804_ops = {
diff --git a/drivers/media/i2c/tw9903.c b/drivers/media/i2c/tw9903.c
index 285b759a5f7..12c7d211a4a 100644
--- a/drivers/media/i2c/tw9903.c
+++ b/drivers/media/i2c/tw9903.c
@@ -187,10 +187,10 @@ static const struct v4l2_ctrl_ops tw9903_ctrl_ops = {
 
 static const struct v4l2_subdev_core_ops tw9903_core_ops = {
 	.log_status = tw9903_log_status,
-	.s_std = tw9903_s_std,
 };
 
 static const struct v4l2_subdev_video_ops tw9903_video_ops = {
+	.s_std = tw9903_s_std,
 	.s_routing = tw9903_s_video_routing,
 };
 
diff --git a/drivers/media/i2c/tw9906.c b/drivers/media/i2c/tw9906.c
index f6bef25bd9c..2672d89265f 100644
--- a/drivers/media/i2c/tw9906.c
+++ b/drivers/media/i2c/tw9906.c
@@ -157,10 +157,10 @@ static const struct v4l2_ctrl_ops tw9906_ctrl_ops = {
 
 static const struct v4l2_subdev_core_ops tw9906_core_ops = {
 	.log_status = tw9906_log_status,
-	.s_std = tw9906_s_std,
 };
 
 static const struct v4l2_subdev_video_ops tw9906_video_ops = {
+	.s_std = tw9906_s_std,
 	.s_routing = tw9906_s_video_routing,
 };
 
diff --git a/drivers/media/i2c/vp27smpx.c b/drivers/media/i2c/vp27smpx.c
index 6a3a3ff7ee6..819ab6d1298 100644
--- a/drivers/media/i2c/vp27smpx.c
+++ b/drivers/media/i2c/vp27smpx.c
@@ -124,7 +124,6 @@ static int vp27smpx_log_status(struct v4l2_subdev *sd)
 
 static const struct v4l2_subdev_core_ops vp27smpx_core_ops = {
 	.log_status = vp27smpx_log_status,
-	.s_std = vp27smpx_s_std,
 };
 
 static const struct v4l2_subdev_tuner_ops vp27smpx_tuner_ops = {
@@ -133,9 +132,14 @@ static const struct v4l2_subdev_tuner_ops vp27smpx_tuner_ops = {
 	.g_tuner = vp27smpx_g_tuner,
 };
 
+static const struct v4l2_subdev_video_ops vp27smpx_video_ops = {
+	.s_std = vp27smpx_s_std,
+};
+
 static const struct v4l2_subdev_ops vp27smpx_ops = {
 	.core = &vp27smpx_core_ops,
 	.tuner = &vp27smpx_tuner_ops,
+	.video = &vp27smpx_video_ops,
 };
 
 /* ----------------------------------------------------------------------- */
diff --git a/drivers/media/i2c/vpx3220.c b/drivers/media/i2c/vpx3220.c
index ece90df6a04..016e766e72b 100644
--- a/drivers/media/i2c/vpx3220.c
+++ b/drivers/media/i2c/vpx3220.c
@@ -457,10 +457,10 @@ static const struct v4l2_subdev_core_ops vpx3220_core_ops = {
 	.s_ctrl = v4l2_subdev_s_ctrl,
 	.queryctrl = v4l2_subdev_queryctrl,
 	.querymenu = v4l2_subdev_querymenu,
-	.s_std = vpx3220_s_std,
 };
 
 static const struct v4l2_subdev_video_ops vpx3220_video_ops = {
+	.s_std = vpx3220_s_std,
 	.s_routing = vpx3220_s_routing,
 	.s_stream = vpx3220_s_stream,
 	.querystd = vpx3220_querystd,
diff --git a/drivers/media/media-device.c b/drivers/media/media-device.c
index d5a7a135f75..88b97c9e64a 100644
--- a/drivers/media/media-device.c
+++ b/drivers/media/media-device.c
@@ -93,6 +93,7 @@ static long media_device_enum_entities(struct media_device *mdev,
 	struct media_entity *ent;
 	struct media_entity_desc u_ent;
 
+	memset(&u_ent, 0, sizeof(u_ent));
 	if (copy_from_user(&u_ent.id, &uent->id, sizeof(u_ent.id)))
 		return -EFAULT;
 
@@ -372,7 +373,8 @@ static void media_device_release(struct media_devnode *mdev)
  * - dev must point to the parent device
  * - model must be filled with the device model name
  */
-int __must_check media_device_register(struct media_device *mdev)
+int __must_check __media_device_register(struct media_device *mdev,
+					 struct module *owner)
 {
 	int ret;
 
@@ -388,7 +390,7 @@ int __must_check media_device_register(struct media_device *mdev)
 	mdev->devnode.fops = &media_device_fops;
 	mdev->devnode.parent = mdev->dev;
 	mdev->devnode.release = media_device_release;
-	ret = media_devnode_register(&mdev->devnode);
+	ret = media_devnode_register(&mdev->devnode, owner);
 	if (ret < 0)
 		return ret;
 
@@ -400,7 +402,7 @@ int __must_check media_device_register(struct media_device *mdev)
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(media_device_register);
+EXPORT_SYMBOL_GPL(__media_device_register);
 
 /**
  * media_device_unregister - unregister a media device
diff --git a/drivers/media/media-devnode.c b/drivers/media/media-devnode.c
index fb0f0469fad..7acd19c881d 100644
--- a/drivers/media/media-devnode.c
+++ b/drivers/media/media-devnode.c
@@ -232,7 +232,8 @@ static const struct file_operations media_devnode_fops = {
  * the media_devnode structure is *not* called, so the caller is responsible for
  * freeing any data.
  */
-int __must_check media_devnode_register(struct media_devnode *mdev)
+int __must_check media_devnode_register(struct media_devnode *mdev,
+					struct module *owner)
 {
 	int minor;
 	int ret;
@@ -253,7 +254,7 @@ int __must_check media_devnode_register(struct media_devnode *mdev)
 
 	/* Part 2: Initialize and register the character device */
 	cdev_init(&mdev->cdev, &media_devnode_fops);
-	mdev->cdev.owner = mdev->fops->owner;
+	mdev->cdev.owner = owner;
 
 	ret = cdev_add(&mdev->cdev, MKDEV(MAJOR(media_dev_t), mdev->minor), 1);
 	if (ret < 0) {
diff --git a/drivers/media/parport/bw-qcam.c b/drivers/media/parport/bw-qcam.c
index 8a0e84c7d49..416507a8366 100644
--- a/drivers/media/parport/bw-qcam.c
+++ b/drivers/media/parport/bw-qcam.c
@@ -937,7 +937,7 @@ static struct qcam *qcam_init(struct parport *port)
 		return NULL;
 
 	v4l2_dev = &qcam->v4l2_dev;
-	snprintf(v4l2_dev->name, sizeof(v4l2_dev->name), "bw-qcam%d", num_cams);
+	snprintf(v4l2_dev->name, sizeof(v4l2_dev->name), "bw-qcam%u", num_cams);
 
 	if (v4l2_device_register(port->dev, v4l2_dev) < 0) {
 		v4l2_err(v4l2_dev, "Could not register v4l2_device\n");
diff --git a/drivers/media/pci/bt8xx/bttv-cards.c b/drivers/media/pci/bt8xx/bttv-cards.c
index d06963b3dcf..d8ec583c154 100644
--- a/drivers/media/pci/bt8xx/bttv-cards.c
+++ b/drivers/media/pci/bt8xx/bttv-cards.c
@@ -52,6 +52,7 @@ static void osprey_eeprom(struct bttv *btv, const u8 ee[256]);
 static void modtec_eeprom(struct bttv *btv);
 static void init_PXC200(struct bttv *btv);
 static void init_RTV24(struct bttv *btv);
+static void init_PCI8604PW(struct bttv *btv);
 
 static void rv605_muxsel(struct bttv *btv, unsigned int input);
 static void eagle_muxsel(struct bttv *btv, unsigned int input);
@@ -2871,6 +2872,22 @@ struct tvcard bttv_tvcards[] = {
 		.has_remote     = 1,
 		.has_radio      = 1,
 	},
+	/* ---- card 0xa6---------------------------------- */
+	[BTTV_BOARD_PCI_8604PW] = {
+		/* PCI-8604PW with special unlock sequence */
+		.name           = "PCI-8604PW",
+		.video_inputs   = 2,
+		/* .audio_inputs= 0, */
+		.svhs           = NO_SVHS,
+		/* The second input is available on CN4, if populated.
+		 * The other 5x2 header (CN2?) connects to the same inputs
+		 * as the on-board BNCs */
+		.muxsel         = MUXSEL(2, 3),
+		.tuner_type     = TUNER_ABSENT,
+		.no_msp34xx	= 1,
+		.no_tda7432	= 1,
+		.pll            = PLL_35,
+	},
 };
 
 static const unsigned int bttv_num_tvcards = ARRAY_SIZE(bttv_tvcards);
@@ -3305,6 +3322,9 @@ void bttv_init_card1(struct bttv *btv)
 	case BTTV_BOARD_ADLINK_RTV24:
 		init_RTV24( btv );
 		break;
+	case BTTV_BOARD_PCI_8604PW:
+		init_PCI8604PW(btv);
+		break;
 
 	}
 	if (!bttv_tvcards[btv->c.type].has_dvb)
@@ -4185,6 +4205,96 @@ init_RTV24 (struct bttv *btv)
 
 
 /* ----------------------------------------------------------------------- */
+/*
+ *  The PCI-8604PW contains a CPLD, probably an ispMACH 4A, that filters
+ *  the PCI REQ signals comming from the four BT878 chips. After power
+ *  up, the CPLD does not forward requests to the bus, which prevents
+ *  the BT878 from fetching RISC instructions from memory. While the
+ *  CPLD is connected to most of the GPIOs of PCI device 0xD, only
+ *  five appear to play a role in unlocking the REQ signal. The following
+ *  sequence has been determined by trial and error without access to the
+ *  original driver.
+ *
+ *  Eight GPIOs of device 0xC are provided on connector CN4 (4 in, 4 out).
+ *  Devices 0xE and 0xF do not appear to have anything connected to their
+ *  GPIOs.
+ *
+ *  The correct GPIO_OUT_EN value might have some more bits set. It should
+ *  be possible to derive it from a boundary scan of the CPLD. Its JTAG
+ *  pins are routed to test points.
+ *
+ */
+/* ----------------------------------------------------------------------- */
+static void
+init_PCI8604PW(struct bttv *btv)
+{
+	int state;
+
+	if ((PCI_SLOT(btv->c.pci->devfn) & ~3) != 0xC) {
+		pr_warn("This is not a PCI-8604PW\n");
+		return;
+	}
+
+	if (PCI_SLOT(btv->c.pci->devfn) != 0xD)
+		return;
+
+	btwrite(0x080002, BT848_GPIO_OUT_EN);
+
+	state = (btread(BT848_GPIO_DATA) >> 21) & 7;
+
+	for (;;) {
+		switch (state) {
+		case 1:
+		case 5:
+		case 6:
+		case 4:
+			pr_debug("PCI-8604PW in state %i, toggling pin\n",
+				 state);
+			btwrite(0x080000, BT848_GPIO_DATA);
+			msleep(1);
+			btwrite(0x000000, BT848_GPIO_DATA);
+			msleep(1);
+			break;
+		case 7:
+			pr_info("PCI-8604PW unlocked\n");
+			return;
+		case 0:
+			/* FIXME: If we are in state 7 and toggle GPIO[19] one
+			   more time, the CPLD goes into state 0, where PCI bus
+			   mastering is inhibited again. We have not managed to
+			   get out of that state. */
+
+			pr_err("PCI-8604PW locked until reset\n");
+			return;
+		default:
+			pr_err("PCI-8604PW in unknown state %i\n", state);
+			return;
+		}
+
+		state = (state << 4) | ((btread(BT848_GPIO_DATA) >> 21) & 7);
+
+		switch (state) {
+		case 0x15:
+		case 0x56:
+		case 0x64:
+		case 0x47:
+		/* The transition from state 7 to state 0 is, as explained
+		   above, valid but undesired and with this code impossible
+		   as we exit as soon as we are in state 7.
+		case 0x70: */
+			break;
+		default:
+			pr_err("PCI-8604PW invalid transition %i -> %i\n",
+			       state >> 4, state & 7);
+			return;
+		}
+		state &= 7;
+	}
+}
+
+
+
+/* ----------------------------------------------------------------------- */
 /* Miro Pro radio stuff -- the tea5757 is connected to some GPIO ports     */
 /*
  * Copyright (c) 1999 Csaba Halasz <qgehali@uni-miskolc.hu>
diff --git a/drivers/media/pci/bt8xx/bttv-driver.c b/drivers/media/pci/bt8xx/bttv-driver.c
index afcd53bfcf8..da780f42b12 100644
--- a/drivers/media/pci/bt8xx/bttv-driver.c
+++ b/drivers/media/pci/bt8xx/bttv-driver.c
@@ -1182,7 +1182,7 @@ set_tvnorm(struct bttv *btv, unsigned int norm)
 		break;
 	}
 	id = tvnorm->v4l2_id;
-	bttv_call_all(btv, core, s_std, id);
+	bttv_call_all(btv, video, s_std, id);
 
 	return 0;
 }
diff --git a/drivers/media/pci/bt8xx/bttv.h b/drivers/media/pci/bt8xx/bttv.h
index bb5da349a46..f0812624466 100644
--- a/drivers/media/pci/bt8xx/bttv.h
+++ b/drivers/media/pci/bt8xx/bttv.h
@@ -189,6 +189,7 @@
 #define BTTV_BOARD_BT848_CAP_14            0xa3
 #define BTTV_BOARD_CYBERVISION_CV06        0xa4
 #define BTTV_BOARD_KWORLD_VSTREAM_XPERT    0xa5
+#define BTTV_BOARD_PCI_8604PW              0xa6
 
 /* more card-specific defines */
 #define PT2254_L_CHANNEL 0x10
diff --git a/drivers/media/pci/bt8xx/dst.c b/drivers/media/pci/bt8xx/dst.c
index 430b3eb1181..f2261dfe5d1 100644
--- a/drivers/media/pci/bt8xx/dst.c
+++ b/drivers/media/pci/bt8xx/dst.c
@@ -1544,7 +1544,7 @@ static int dst_send_burst(struct dvb_frontend *fe, fe_sec_mini_cmd_t minicmd)
 }
 
 
-static int dst_init(struct dvb_frontend *fe)
+static int bt8xx_dst_init(struct dvb_frontend *fe)
 {
 	struct dst_state *state = fe->demodulator_priv;
 
@@ -1707,7 +1707,7 @@ static int dst_get_frontend(struct dvb_frontend *fe)
 	return 0;
 }
 
-static void dst_release(struct dvb_frontend *fe)
+static void bt8xx_dst_release(struct dvb_frontend *fe)
 {
 	struct dst_state *state = fe->demodulator_priv;
 	if (state->dst_ca) {
@@ -1776,8 +1776,8 @@ static struct dvb_frontend_ops dst_dvbt_ops = {
 			FE_CAN_GUARD_INTERVAL_AUTO
 	},
 
-	.release = dst_release,
-	.init = dst_init,
+	.release = bt8xx_dst_release,
+	.init = bt8xx_dst_init,
 	.tune = dst_tune_frontend,
 	.set_frontend = dst_set_frontend,
 	.get_frontend = dst_get_frontend,
@@ -1801,8 +1801,8 @@ static struct dvb_frontend_ops dst_dvbs_ops = {
 		.caps = FE_CAN_FEC_AUTO | FE_CAN_QPSK
 	},
 
-	.release = dst_release,
-	.init = dst_init,
+	.release = bt8xx_dst_release,
+	.init = bt8xx_dst_init,
 	.tune = dst_tune_frontend,
 	.set_frontend = dst_set_frontend,
 	.get_frontend = dst_get_frontend,
@@ -1834,8 +1834,8 @@ static struct dvb_frontend_ops dst_dvbc_ops = {
 			FE_CAN_QAM_256
 	},
 
-	.release = dst_release,
-	.init = dst_init,
+	.release = bt8xx_dst_release,
+	.init = bt8xx_dst_init,
 	.tune = dst_tune_frontend,
 	.set_frontend = dst_set_frontend,
 	.get_frontend = dst_get_frontend,
@@ -1857,8 +1857,8 @@ static struct dvb_frontend_ops dst_atsc_ops = {
 		.caps = FE_CAN_FEC_AUTO | FE_CAN_QAM_AUTO | FE_CAN_QAM_64 | FE_CAN_QAM_256 | FE_CAN_8VSB
 	},
 
-	.release = dst_release,
-	.init = dst_init,
+	.release = bt8xx_dst_release,
+	.init = bt8xx_dst_init,
 	.tune = dst_tune_frontend,
 	.set_frontend = dst_set_frontend,
 	.get_frontend = dst_get_frontend,
diff --git a/drivers/media/pci/cx18/cx18-av-core.c b/drivers/media/pci/cx18/cx18-av-core.c
index c4890a430dc..2d3afe0431a 100644
--- a/drivers/media/pci/cx18/cx18-av-core.c
+++ b/drivers/media/pci/cx18/cx18-av-core.c
@@ -1263,7 +1263,6 @@ static const struct v4l2_subdev_core_ops cx18_av_general_ops = {
 	.log_status = cx18_av_log_status,
 	.load_fw = cx18_av_load_fw,
 	.reset = cx18_av_reset,
-	.s_std = cx18_av_s_std,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	.g_register = cx18_av_g_register,
 	.s_register = cx18_av_s_register,
@@ -1283,6 +1282,7 @@ static const struct v4l2_subdev_audio_ops cx18_av_audio_ops = {
 };
 
 static const struct v4l2_subdev_video_ops cx18_av_video_ops = {
+	.s_std = cx18_av_s_std,
 	.s_routing = cx18_av_s_video_routing,
 	.s_stream = cx18_av_s_stream,
 	.s_mbus_fmt = cx18_av_s_mbus_fmt,
diff --git a/drivers/media/pci/cx18/cx18-fileops.c b/drivers/media/pci/cx18/cx18-fileops.c
index 4bfd865a410..76a3b4ac541 100644
--- a/drivers/media/pci/cx18/cx18-fileops.c
+++ b/drivers/media/pci/cx18/cx18-fileops.c
@@ -760,7 +760,7 @@ int cx18_v4l2_close(struct file *filp)
 		/* Mark that the radio is no longer in use */
 		clear_bit(CX18_F_I_RADIO_USER, &cx->i_flags);
 		/* Switch tuner to TV */
-		cx18_call_all(cx, core, s_std, cx->std);
+		cx18_call_all(cx, video, s_std, cx->std);
 		/* Select correct audio input (i.e. TV tuner or Line in) */
 		cx18_audio_set_io(cx);
 		if (atomic_read(&cx->ana_capturing) > 0) {
diff --git a/drivers/media/pci/cx18/cx18-gpio.c b/drivers/media/pci/cx18/cx18-gpio.c
index 5374aeb0cd2..38dc6b8f825 100644
--- a/drivers/media/pci/cx18/cx18-gpio.c
+++ b/drivers/media/pci/cx18/cx18-gpio.c
@@ -180,7 +180,6 @@ static int gpiomux_s_audio_routing(struct v4l2_subdev *sd,
 
 static const struct v4l2_subdev_core_ops gpiomux_core_ops = {
 	.log_status = gpiomux_log_status,
-	.s_std = gpiomux_s_std,
 };
 
 static const struct v4l2_subdev_tuner_ops gpiomux_tuner_ops = {
@@ -191,10 +190,15 @@ static const struct v4l2_subdev_audio_ops gpiomux_audio_ops = {
 	.s_routing = gpiomux_s_audio_routing,
 };
 
+static const struct v4l2_subdev_video_ops gpiomux_video_ops = {
+	.s_std = gpiomux_s_std,
+};
+
 static const struct v4l2_subdev_ops gpiomux_ops = {
 	.core = &gpiomux_core_ops,
 	.tuner = &gpiomux_tuner_ops,
 	.audio = &gpiomux_audio_ops,
+	.video = &gpiomux_video_ops,
 };
 
 /*
diff --git a/drivers/media/pci/cx18/cx18-ioctl.c b/drivers/media/pci/cx18/cx18-ioctl.c
index 1110bcb14e2..fefb2cd3583 100644
--- a/drivers/media/pci/cx18/cx18-ioctl.c
+++ b/drivers/media/pci/cx18/cx18-ioctl.c
@@ -602,7 +602,7 @@ int cx18_s_std(struct file *file, void *fh, v4l2_std_id std)
 			(unsigned long long) cx->std);
 
 	/* Tuner */
-	cx18_call_all(cx, core, s_std, cx->std);
+	cx18_call_all(cx, video, s_std, cx->std);
 	return 0;
 }
 
diff --git a/drivers/media/pci/cx23885/cx23885-video.c b/drivers/media/pci/cx23885/cx23885-video.c
index 7891f34157d..e0a59523cf3 100644
--- a/drivers/media/pci/cx23885/cx23885-video.c
+++ b/drivers/media/pci/cx23885/cx23885-video.c
@@ -326,7 +326,7 @@ int cx23885_set_tvnorm(struct cx23885_dev *dev, v4l2_std_id norm)
 
 	dev->tvnorm = norm;
 
-	call_all(dev, core, s_std, norm);
+	call_all(dev, video, s_std, norm);
 
 	return 0;
 }
@@ -1589,7 +1589,7 @@ static int cx23885_set_freq_via_ops(struct cx23885_dev *dev,
 		fe = &dev->ts1.analog_fe;
 
 	if (fe && fe->ops.tuner_ops.set_analog_params) {
-		call_all(dev, core, s_std, dev->tvnorm);
+		call_all(dev, video, s_std, dev->tvnorm);
 		fe->ops.tuner_ops.set_analog_params(fe, &params);
 	}
 	else
diff --git a/drivers/media/pci/cx88/cx88-core.c b/drivers/media/pci/cx88/cx88-core.c
index ad59dc9235a..e061c88b697 100644
--- a/drivers/media/pci/cx88/cx88-core.c
+++ b/drivers/media/pci/cx88/cx88-core.c
@@ -1012,7 +1012,7 @@ int cx88_set_tvnorm(struct cx88_core *core, v4l2_std_id norm)
 	set_tvaudio(core);
 
 	// tell i2c chips
-	call_all(core, core, s_std, norm);
+	call_all(core, video, s_std, norm);
 
 	/* The chroma_agc control should be inaccessible if the video format is SECAM */
 	v4l2_ctrl_grab(core->chroma_agc, cxiformat == VideoFormatSECAM);
diff --git a/drivers/media/pci/ivtv/ivtv-alsa-pcm.c b/drivers/media/pci/ivtv/ivtv-alsa-pcm.c
index e1863dbf4ed..7a9b98bc208 100644
--- a/drivers/media/pci/ivtv/ivtv-alsa-pcm.c
+++ b/drivers/media/pci/ivtv/ivtv-alsa-pcm.c
@@ -159,6 +159,12 @@ static int snd_ivtv_pcm_capture_open(struct snd_pcm_substream *substream)
 
 	/* Instruct the CX2341[56] to start sending packets */
 	snd_ivtv_lock(itvsc);
+
+	if (ivtv_init_on_first_open(itv)) {
+		snd_ivtv_unlock(itvsc);
+		return -ENXIO;
+	}
+
 	s = &itv->streams[IVTV_ENC_STREAM_TYPE_PCM];
 
 	v4l2_fh_init(&item.fh, s->vdev);
diff --git a/drivers/media/pci/ivtv/ivtv-fileops.c b/drivers/media/pci/ivtv/ivtv-fileops.c
index 9caffd8aa99..e5ff6277ca8 100644
--- a/drivers/media/pci/ivtv/ivtv-fileops.c
+++ b/drivers/media/pci/ivtv/ivtv-fileops.c
@@ -894,7 +894,7 @@ int ivtv_v4l2_close(struct file *filp)
 		/* Mark that the radio is no longer in use */
 		clear_bit(IVTV_F_I_RADIO_USER, &itv->i_flags);
 		/* Switch tuner to TV */
-		ivtv_call_all(itv, core, s_std, itv->std);
+		ivtv_call_all(itv, video, s_std, itv->std);
 		/* Select correct audio input (i.e. TV tuner or Line in) */
 		ivtv_audio_set_io(itv);
 		if (itv->hw_flags & IVTV_HW_SAA711X) {
diff --git a/drivers/media/pci/ivtv/ivtv-ioctl.c b/drivers/media/pci/ivtv/ivtv-ioctl.c
index 807b275a847..b3667a00db3 100644
--- a/drivers/media/pci/ivtv/ivtv-ioctl.c
+++ b/drivers/media/pci/ivtv/ivtv-ioctl.c
@@ -1090,7 +1090,7 @@ void ivtv_s_std_enc(struct ivtv *itv, v4l2_std_id std)
 		itv->vbi.sliced_decoder_line_size = itv->is_60hz ? 272 : 284;
 
 	/* Tuner */
-	ivtv_call_all(itv, core, s_std, itv->std);
+	ivtv_call_all(itv, video, s_std, itv->std);
 }
 
 void ivtv_s_std_dec(struct ivtv *itv, v4l2_std_id std)
diff --git a/drivers/media/pci/saa7134/Kconfig b/drivers/media/pci/saa7134/Kconfig
index 7883393571e..18ae7554630 100644
--- a/drivers/media/pci/saa7134/Kconfig
+++ b/drivers/media/pci/saa7134/Kconfig
@@ -1,7 +1,7 @@
 config VIDEO_SAA7134
 	tristate "Philips SAA7134 support"
 	depends on VIDEO_DEV && PCI && I2C
-	select VIDEOBUF_DMA_SG
+	select VIDEOBUF2_DMA_SG
 	select VIDEO_TUNER
 	select VIDEO_TVEEPROM
 	select CRC32
@@ -37,7 +37,7 @@ config VIDEO_SAA7134_RC
 config VIDEO_SAA7134_DVB
 	tristate "DVB/ATSC Support for saa7134 based TV cards"
 	depends on VIDEO_SAA7134 && DVB_CORE
-	select VIDEOBUF_DVB
+	select VIDEOBUF2_DVB
 	select DVB_PLL if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_MT352 if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_TDA1004X if MEDIA_SUBDRV_AUTOSELECT
diff --git a/drivers/media/pci/saa7134/saa7134-alsa.c b/drivers/media/pci/saa7134/saa7134-alsa.c
index e04a4d5d667..40569894c1c 100644
--- a/drivers/media/pci/saa7134/saa7134-alsa.c
+++ b/drivers/media/pci/saa7134/saa7134-alsa.c
@@ -27,6 +27,7 @@
 #include <sound/pcm_params.h>
 #include <sound/initval.h>
 #include <linux/interrupt.h>
+#include <linux/vmalloc.h>
 
 #include "saa7134.h"
 #include "saa7134-reg.h"
@@ -274,6 +275,82 @@ static int snd_card_saa7134_capture_trigger(struct snd_pcm_substream * substream
 	return err;
 }
 
+static int saa7134_alsa_dma_init(struct saa7134_dev *dev, int nr_pages)
+{
+	struct saa7134_dmasound *dma = &dev->dmasound;
+	struct page *pg;
+	int i;
+
+	dma->vaddr = vmalloc_32(nr_pages << PAGE_SHIFT);
+	if (NULL == dma->vaddr) {
+		dprintk("vmalloc_32(%d pages) failed\n", nr_pages);
+		return -ENOMEM;
+	}
+
+	dprintk("vmalloc is at addr 0x%08lx, size=%d\n",
+				(unsigned long)dma->vaddr,
+				nr_pages << PAGE_SHIFT);
+
+	memset(dma->vaddr, 0, nr_pages << PAGE_SHIFT);
+	dma->nr_pages = nr_pages;
+
+	dma->sglist = vzalloc(dma->nr_pages * sizeof(*dma->sglist));
+	if (NULL == dma->sglist)
+		goto vzalloc_err;
+
+	sg_init_table(dma->sglist, dma->nr_pages);
+	for (i = 0; i < dma->nr_pages; i++) {
+		pg = vmalloc_to_page(dma->vaddr + i * PAGE_SIZE);
+		if (NULL == pg)
+			goto vmalloc_to_page_err;
+		sg_set_page(&dma->sglist[i], pg, PAGE_SIZE, 0);
+	}
+	return 0;
+
+vmalloc_to_page_err:
+	vfree(dma->sglist);
+	dma->sglist = NULL;
+vzalloc_err:
+	vfree(dma->vaddr);
+	dma->vaddr = NULL;
+	return -ENOMEM;
+}
+
+static int saa7134_alsa_dma_map(struct saa7134_dev *dev)
+{
+	struct saa7134_dmasound *dma = &dev->dmasound;
+
+	dma->sglen = dma_map_sg(&dev->pci->dev, dma->sglist,
+			dma->nr_pages, PCI_DMA_FROMDEVICE);
+
+	if (0 == dma->sglen) {
+		pr_warn("%s: saa7134_alsa_map_sg failed\n", __func__);
+		return -ENOMEM;
+	}
+	return 0;
+}
+
+static int saa7134_alsa_dma_unmap(struct saa7134_dev *dev)
+{
+	struct saa7134_dmasound *dma = &dev->dmasound;
+
+	if (!dma->sglen)
+		return 0;
+
+	dma_unmap_sg(&dev->pci->dev, dma->sglist, dma->sglen, PCI_DMA_FROMDEVICE);
+	dma->sglen = 0;
+	return 0;
+}
+
+static int saa7134_alsa_dma_free(struct saa7134_dmasound *dma)
+{
+	vfree(dma->sglist);
+	dma->sglist = NULL;
+	vfree(dma->vaddr);
+	dma->vaddr = NULL;
+	return 0;
+}
+
 /*
  * DMA buffer initialization
  *
@@ -291,9 +368,8 @@ static int dsp_buffer_init(struct saa7134_dev *dev)
 
 	BUG_ON(!dev->dmasound.bufsize);
 
-	videobuf_dma_init(&dev->dmasound.dma);
-	err = videobuf_dma_init_kernel(&dev->dmasound.dma, PCI_DMA_FROMDEVICE,
-				       (dev->dmasound.bufsize + PAGE_SIZE) >> PAGE_SHIFT);
+	err = saa7134_alsa_dma_init(dev,
+			       (dev->dmasound.bufsize + PAGE_SIZE) >> PAGE_SHIFT);
 	if (0 != err)
 		return err;
 	return 0;
@@ -310,7 +386,7 @@ static int dsp_buffer_free(struct saa7134_dev *dev)
 {
 	BUG_ON(!dev->dmasound.blksize);
 
-	videobuf_dma_free(&dev->dmasound.dma);
+	saa7134_alsa_dma_free(&dev->dmasound);
 
 	dev->dmasound.blocks  = 0;
 	dev->dmasound.blksize = 0;
@@ -632,7 +708,7 @@ static int snd_card_saa7134_hw_params(struct snd_pcm_substream * substream,
 	/* release the old buffer */
 	if (substream->runtime->dma_area) {
 		saa7134_pgtable_free(dev->pci, &dev->dmasound.pt);
-		videobuf_dma_unmap(&dev->pci->dev, &dev->dmasound.dma);
+		saa7134_alsa_dma_unmap(dev);
 		dsp_buffer_free(dev);
 		substream->runtime->dma_area = NULL;
 	}
@@ -648,21 +724,22 @@ static int snd_card_saa7134_hw_params(struct snd_pcm_substream * substream,
 		return err;
 	}
 
-	if (0 != (err = videobuf_dma_map(&dev->pci->dev, &dev->dmasound.dma))) {
+	err = saa7134_alsa_dma_map(dev);
+	if (err) {
 		dsp_buffer_free(dev);
 		return err;
 	}
-	if (0 != (err = saa7134_pgtable_alloc(dev->pci,&dev->dmasound.pt))) {
-		videobuf_dma_unmap(&dev->pci->dev, &dev->dmasound.dma);
+	err = saa7134_pgtable_alloc(dev->pci, &dev->dmasound.pt);
+	if (err) {
+		saa7134_alsa_dma_unmap(dev);
 		dsp_buffer_free(dev);
 		return err;
 	}
-	if (0 != (err = saa7134_pgtable_build(dev->pci,&dev->dmasound.pt,
-						dev->dmasound.dma.sglist,
-						dev->dmasound.dma.sglen,
-						0))) {
+	err = saa7134_pgtable_build(dev->pci, &dev->dmasound.pt,
+				dev->dmasound.sglist, dev->dmasound.sglen, 0);
+	if (err) {
 		saa7134_pgtable_free(dev->pci, &dev->dmasound.pt);
-		videobuf_dma_unmap(&dev->pci->dev, &dev->dmasound.dma);
+		saa7134_alsa_dma_unmap(dev);
 		dsp_buffer_free(dev);
 		return err;
 	}
@@ -671,7 +748,7 @@ static int snd_card_saa7134_hw_params(struct snd_pcm_substream * substream,
 	   byte, but it doesn't work. So I allocate the DMA using the
 	   V4L functions, and force ALSA to use that as the DMA area */
 
-	substream->runtime->dma_area = dev->dmasound.dma.vaddr;
+	substream->runtime->dma_area = dev->dmasound.vaddr;
 	substream->runtime->dma_bytes = dev->dmasound.bufsize;
 	substream->runtime->dma_addr = 0;
 
@@ -698,7 +775,7 @@ static int snd_card_saa7134_hw_free(struct snd_pcm_substream * substream)
 
 	if (substream->runtime->dma_area) {
 		saa7134_pgtable_free(dev->pci, &dev->dmasound.pt);
-		videobuf_dma_unmap(&dev->pci->dev, &dev->dmasound.dma);
+		saa7134_alsa_dma_unmap(dev);
 		dsp_buffer_free(dev);
 		substream->runtime->dma_area = NULL;
 	}
diff --git a/drivers/media/pci/saa7134/saa7134-core.c b/drivers/media/pci/saa7134/saa7134-core.c
index 1362b4aab47..be19a051a49 100644
--- a/drivers/media/pci/saa7134/saa7134-core.c
+++ b/drivers/media/pci/saa7134/saa7134-core.c
@@ -69,6 +69,10 @@ module_param_named(no_overlay, saa7134_no_overlay, int, 0444);
 MODULE_PARM_DESC(no_overlay,"allow override overlay default (0 disables, 1 enables)"
 		" [some VIA/SIS chipsets are known to have problem with overlay]");
 
+bool saa7134_userptr;
+module_param(saa7134_userptr, bool, 0644);
+MODULE_PARM_DESC(saa7134_userptr, "enable page-aligned userptr support");
+
 static unsigned int video_nr[] = {[0 ... (SAA7134_MAXBOARDS - 1)] = UNSET };
 static unsigned int vbi_nr[]   = {[0 ... (SAA7134_MAXBOARDS - 1)] = UNSET };
 static unsigned int radio_nr[] = {[0 ... (SAA7134_MAXBOARDS - 1)] = UNSET };
@@ -203,16 +207,16 @@ int saa7134_buffer_count(unsigned int size, unsigned int count)
 
 int saa7134_buffer_startpage(struct saa7134_buf *buf)
 {
-	return saa7134_buffer_pages(buf->vb.bsize) * buf->vb.i;
+	return saa7134_buffer_pages(vb2_plane_size(&buf->vb2, 0)) * buf->vb2.v4l2_buf.index;
 }
 
 unsigned long saa7134_buffer_base(struct saa7134_buf *buf)
 {
 	unsigned long base;
-	struct videobuf_dmabuf *dma=videobuf_to_dma(&buf->vb);
+	struct sg_table *dma = vb2_dma_sg_plane_desc(&buf->vb2, 0);
 
 	base  = saa7134_buffer_startpage(buf) * 4096;
-	base += dma->sglist[0].offset;
+	base += dma->sgl[0].offset;
 	return base;
 }
 
@@ -237,14 +241,16 @@ int saa7134_pgtable_build(struct pci_dev *pci, struct saa7134_pgtable *pt,
 			  unsigned int startpage)
 {
 	__le32        *ptr;
-	unsigned int  i,p;
+	unsigned int  i, p;
 
 	BUG_ON(NULL == pt || NULL == pt->cpu);
 
 	ptr = pt->cpu + startpage;
-	for (i = 0; i < length; i++, list++)
+	for (i = 0; i < length; i++, list = sg_next(list)) {
 		for (p = 0; p * 4096 < list->length; p++, ptr++)
-			*ptr = cpu_to_le32(sg_dma_address(list) - list->offset);
+			*ptr = cpu_to_le32(sg_dma_address(list) +
+						list->offset + p * 4096);
+	}
 	return 0;
 }
 
@@ -258,44 +264,31 @@ void saa7134_pgtable_free(struct pci_dev *pci, struct saa7134_pgtable *pt)
 
 /* ------------------------------------------------------------------ */
 
-void saa7134_dma_free(struct videobuf_queue *q,struct saa7134_buf *buf)
-{
-	struct videobuf_dmabuf *dma=videobuf_to_dma(&buf->vb);
-	BUG_ON(in_interrupt());
-
-	videobuf_waiton(q, &buf->vb, 0, 0);
-	videobuf_dma_unmap(q->dev, dma);
-	videobuf_dma_free(dma);
-	buf->vb.state = VIDEOBUF_NEEDS_INIT;
-}
-
-/* ------------------------------------------------------------------ */
-
 int saa7134_buffer_queue(struct saa7134_dev *dev,
 			 struct saa7134_dmaqueue *q,
 			 struct saa7134_buf *buf)
 {
 	struct saa7134_buf *next = NULL;
+	unsigned long flags;
 
-	assert_spin_locked(&dev->slock);
-	dprintk("buffer_queue %p\n",buf);
+	spin_lock_irqsave(&dev->slock, flags);
+	dprintk("buffer_queue %p\n", buf);
 	if (NULL == q->curr) {
 		if (!q->need_two) {
 			q->curr = buf;
-			buf->activate(dev,buf,NULL);
+			buf->activate(dev, buf, NULL);
 		} else if (list_empty(&q->queue)) {
-			list_add_tail(&buf->vb.queue,&q->queue);
-			buf->vb.state = VIDEOBUF_QUEUED;
+			list_add_tail(&buf->entry, &q->queue);
 		} else {
-			next = list_entry(q->queue.next,struct saa7134_buf,
-					  vb.queue);
+			next = list_entry(q->queue.next, struct saa7134_buf,
+					  entry);
 			q->curr = buf;
-			buf->activate(dev,buf,next);
+			buf->activate(dev, buf, next);
 		}
 	} else {
-		list_add_tail(&buf->vb.queue,&q->queue);
-		buf->vb.state = VIDEOBUF_QUEUED;
+		list_add_tail(&buf->entry, &q->queue);
 	}
+	spin_unlock_irqrestore(&dev->slock, flags);
 	return 0;
 }
 
@@ -303,13 +296,12 @@ void saa7134_buffer_finish(struct saa7134_dev *dev,
 			   struct saa7134_dmaqueue *q,
 			   unsigned int state)
 {
-	assert_spin_locked(&dev->slock);
-	dprintk("buffer_finish %p\n",q->curr);
+	dprintk("buffer_finish %p\n", q->curr);
 
 	/* finish current buffer */
-	q->curr->vb.state = state;
-	v4l2_get_timestamp(&q->curr->vb.ts);
-	wake_up(&q->curr->vb.done);
+	v4l2_get_timestamp(&q->curr->vb2.v4l2_buf.timestamp);
+	q->curr->vb2.v4l2_buf.sequence = q->seq_nr++;
+	vb2_buffer_done(&q->curr->vb2, state);
 	q->curr = NULL;
 }
 
@@ -323,36 +315,31 @@ void saa7134_buffer_next(struct saa7134_dev *dev,
 
 	if (!list_empty(&q->queue)) {
 		/* activate next one from queue */
-		buf = list_entry(q->queue.next,struct saa7134_buf,vb.queue);
+		buf = list_entry(q->queue.next, struct saa7134_buf, entry);
 		dprintk("buffer_next %p [prev=%p/next=%p]\n",
-			buf,q->queue.prev,q->queue.next);
-		list_del(&buf->vb.queue);
+			buf, q->queue.prev, q->queue.next);
+		list_del(&buf->entry);
 		if (!list_empty(&q->queue))
-			next = list_entry(q->queue.next,struct saa7134_buf,
-					  vb.queue);
+			next = list_entry(q->queue.next, struct saa7134_buf, entry);
 		q->curr = buf;
-		buf->activate(dev,buf,next);
+		buf->activate(dev, buf, next);
 		dprintk("buffer_next #2 prev=%p/next=%p\n",
-			q->queue.prev,q->queue.next);
+			q->queue.prev, q->queue.next);
 	} else {
 		/* nothing to do -- just stop DMA */
-		dprintk("buffer_next %p\n",NULL);
+		dprintk("buffer_next %p\n", NULL);
 		saa7134_set_dmabits(dev);
 		del_timer(&q->timeout);
-
-		if (card_has_mpeg(dev))
-			if (dev->ts_started)
-				saa7134_ts_stop(dev);
 	}
 }
 
 void saa7134_buffer_timeout(unsigned long data)
 {
-	struct saa7134_dmaqueue *q = (struct saa7134_dmaqueue*)data;
+	struct saa7134_dmaqueue *q = (struct saa7134_dmaqueue *)data;
 	struct saa7134_dev *dev = q->dev;
 	unsigned long flags;
 
-	spin_lock_irqsave(&dev->slock,flags);
+	spin_lock_irqsave(&dev->slock, flags);
 
 	/* try to reset the hardware (SWRST) */
 	saa_writeb(SAA7134_REGION_ENABLE, 0x00);
@@ -362,13 +349,33 @@ void saa7134_buffer_timeout(unsigned long data)
 	/* flag current buffer as failed,
 	   try to start over with the next one. */
 	if (q->curr) {
-		dprintk("timeout on %p\n",q->curr);
-		saa7134_buffer_finish(dev,q,VIDEOBUF_ERROR);
+		dprintk("timeout on %p\n", q->curr);
+		saa7134_buffer_finish(dev, q, VB2_BUF_STATE_ERROR);
 	}
-	saa7134_buffer_next(dev,q);
-	spin_unlock_irqrestore(&dev->slock,flags);
+	saa7134_buffer_next(dev, q);
+	spin_unlock_irqrestore(&dev->slock, flags);
 }
 
+void saa7134_stop_streaming(struct saa7134_dev *dev, struct saa7134_dmaqueue *q)
+{
+	unsigned long flags;
+	struct list_head *pos, *n;
+	struct saa7134_buf *tmp;
+
+	spin_lock_irqsave(&dev->slock, flags);
+	if (!list_empty(&q->queue)) {
+		list_for_each_safe(pos, n, &q->queue) {
+			 tmp = list_entry(pos, struct saa7134_buf, entry);
+			 vb2_buffer_done(&tmp->vb2, VB2_BUF_STATE_ERROR);
+			 list_del(pos);
+			 tmp = NULL;
+		}
+	}
+	spin_unlock_irqrestore(&dev->slock, flags);
+	saa7134_buffer_timeout((unsigned long)q); /* also calls del_timer(&q->timeout) */
+}
+EXPORT_SYMBOL_GPL(saa7134_stop_streaming);
+
 /* ------------------------------------------------------------------ */
 
 int saa7134_set_dmabits(struct saa7134_dev *dev)
@@ -388,12 +395,11 @@ int saa7134_set_dmabits(struct saa7134_dev *dev)
 		ctrl |= SAA7134_MAIN_CTRL_TE0;
 		irq  |= SAA7134_IRQ1_INTE_RA0_1 |
 			SAA7134_IRQ1_INTE_RA0_0;
-		cap = dev->video_q.curr->vb.field;
+		cap = dev->field;
 	}
 
 	/* video capture -- dma 1+2 (planar modes) */
-	if (dev->video_q.curr &&
-	    dev->video_q.curr->fmt->planar) {
+	if (dev->video_q.curr && dev->fmt->planar) {
 		ctrl |= SAA7134_MAIN_CTRL_TE4 |
 			SAA7134_MAIN_CTRL_TE5;
 	}
@@ -1047,6 +1053,8 @@ static int saa7134_initdev(struct pci_dev *pci_dev,
 
 	dev->video_dev = vdev_init(dev,&saa7134_video_template,"video");
 	dev->video_dev->ctrl_handler = &dev->ctrl_handler;
+	dev->video_dev->lock = &dev->lock;
+	dev->video_dev->queue = &dev->video_vbq;
 	err = video_register_device(dev->video_dev,VFL_TYPE_GRABBER,
 				    video_nr[dev->nr]);
 	if (err < 0) {
@@ -1059,6 +1067,8 @@ static int saa7134_initdev(struct pci_dev *pci_dev,
 
 	dev->vbi_dev = vdev_init(dev, &saa7134_video_template, "vbi");
 	dev->vbi_dev->ctrl_handler = &dev->ctrl_handler;
+	dev->vbi_dev->lock = &dev->lock;
+	dev->vbi_dev->queue = &dev->vbi_vbq;
 
 	err = video_register_device(dev->vbi_dev,VFL_TYPE_VBI,
 				    vbi_nr[dev->nr]);
@@ -1070,6 +1080,7 @@ static int saa7134_initdev(struct pci_dev *pci_dev,
 	if (card_has_radio(dev)) {
 		dev->radio_dev = vdev_init(dev,&saa7134_radio_template,"radio");
 		dev->radio_dev->ctrl_handler = &dev->radio_ctrl_handler;
+		dev->radio_dev->lock = &dev->lock;
 		err = video_register_device(dev->radio_dev,VFL_TYPE_RADIO,
 					    radio_nr[dev->nr]);
 		if (err < 0)
@@ -1189,7 +1200,7 @@ static int saa7134_buffer_requeue(struct saa7134_dev *dev,
 
 	if (!list_empty(&q->queue))
 		next = list_entry(q->queue.next, struct saa7134_buf,
-					  vb.queue);
+					  entry);
 	buf->activate(dev, buf, next);
 
 	return 0;
@@ -1360,10 +1371,3 @@ EXPORT_SYMBOL(saa7134_pgtable_free);
 EXPORT_SYMBOL(saa7134_pgtable_build);
 EXPORT_SYMBOL(saa7134_pgtable_alloc);
 EXPORT_SYMBOL(saa7134_set_dmabits);
-
-/* ----------------------------------------------------------- */
-/*
- * Local variables:
- * c-basic-offset: 8
- * End:
- */
diff --git a/drivers/media/pci/saa7134/saa7134-dvb.c b/drivers/media/pci/saa7134/saa7134-dvb.c
index 4a08ae31df2..73ffbabf831 100644
--- a/drivers/media/pci/saa7134/saa7134-dvb.c
+++ b/drivers/media/pci/saa7134/saa7134-dvb.c
@@ -602,10 +602,10 @@ static int configure_tda827x_fe(struct saa7134_dev *dev,
 				struct tda1004x_config *cdec_conf,
 				struct tda827x_config *tuner_conf)
 {
-	struct videobuf_dvb_frontend *fe0;
+	struct vb2_dvb_frontend *fe0;
 
 	/* Get the first frontend */
-	fe0 = videobuf_dvb_get_frontend(&dev->frontends, 1);
+	fe0 = vb2_dvb_get_frontend(&dev->frontends, 1);
 
 	if (!fe0)
 		return -EINVAL;
@@ -1215,29 +1215,38 @@ static int dvb_init(struct saa7134_dev *dev)
 {
 	int ret;
 	int attach_xc3028 = 0;
-	struct videobuf_dvb_frontend *fe0;
+	struct vb2_dvb_frontend *fe0;
+	struct vb2_queue *q;
 
 	/* FIXME: add support for multi-frontend */
 	mutex_init(&dev->frontends.lock);
 	INIT_LIST_HEAD(&dev->frontends.felist);
 
 	printk(KERN_INFO "%s() allocating 1 frontend\n", __func__);
-	fe0 = videobuf_dvb_alloc_frontend(&dev->frontends, 1);
+	fe0 = vb2_dvb_alloc_frontend(&dev->frontends, 1);
 	if (!fe0) {
 		printk(KERN_ERR "%s() failed to alloc\n", __func__);
 		return -ENOMEM;
 	}
 
-	/* init struct videobuf_dvb */
+	/* init struct vb2_dvb */
 	dev->ts.nr_bufs    = 32;
 	dev->ts.nr_packets = 32*4;
 	fe0->dvb.name = dev->name;
-	videobuf_queue_sg_init(&fe0->dvb.dvbq, &saa7134_ts_qops,
-			    &dev->pci->dev, &dev->slock,
-			    V4L2_BUF_TYPE_VIDEO_CAPTURE,
-			    V4L2_FIELD_ALTERNATE,
-			    sizeof(struct saa7134_buf),
-			    dev, NULL);
+	q = &fe0->dvb.dvbq;
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes = VB2_MMAP | VB2_READ;
+	q->drv_priv = &dev->ts_q;
+	q->ops = &saa7134_ts_qops;
+	q->mem_ops = &vb2_dma_sg_memops;
+	q->buf_struct_size = sizeof(struct saa7134_buf);
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &dev->lock;
+	ret = vb2_queue_init(q);
+	if (ret) {
+		vb2_dvb_dealloc_frontends(&dev->frontends);
+		return ret;
+	}
 
 	switch (dev->board) {
 	case SAA7134_BOARD_PINNACLE_300I_DVBT_PAL:
@@ -1876,7 +1885,7 @@ static int dvb_init(struct saa7134_dev *dev)
 	fe0->dvb.frontend->callback = saa7134_tuner_callback;
 
 	/* register everything else */
-	ret = videobuf_dvb_register_bus(&dev->frontends, THIS_MODULE, dev,
+	ret = vb2_dvb_register_bus(&dev->frontends, THIS_MODULE, dev,
 					&dev->pci->dev, adapter_nr, 0);
 
 	/* this sequence is necessary to make the tda1004x load its firmware
@@ -1893,16 +1902,17 @@ static int dvb_init(struct saa7134_dev *dev)
 	return ret;
 
 detach_frontend:
-	videobuf_dvb_dealloc_frontends(&dev->frontends);
+	vb2_dvb_dealloc_frontends(&dev->frontends);
+	vb2_queue_release(&fe0->dvb.dvbq);
 	return -EINVAL;
 }
 
 static int dvb_fini(struct saa7134_dev *dev)
 {
-	struct videobuf_dvb_frontend *fe0;
+	struct vb2_dvb_frontend *fe0;
 
 	/* Get the first frontend */
-	fe0 = videobuf_dvb_get_frontend(&dev->frontends, 1);
+	fe0 = vb2_dvb_get_frontend(&dev->frontends, 1);
 	if (!fe0)
 		return -EINVAL;
 
@@ -1933,7 +1943,8 @@ static int dvb_fini(struct saa7134_dev *dev)
 			}
 		}
 	}
-	videobuf_dvb_unregister_bus(&dev->frontends);
+	vb2_dvb_unregister_bus(&dev->frontends);
+	vb2_queue_release(&fe0->dvb.dvbq);
 	return 0;
 }
 
@@ -1955,10 +1966,3 @@ static void __exit dvb_unregister(void)
 
 module_init(dvb_register);
 module_exit(dvb_unregister);
-
-/* ------------------------------------------------------------------ */
-/*
- * Local variables:
- * c-basic-offset: 8
- * End:
- */
diff --git a/drivers/media/pci/saa7134/saa7134-empress.c b/drivers/media/pci/saa7134/saa7134-empress.c
index 0a9047e754b..e65c760e4e8 100644
--- a/drivers/media/pci/saa7134/saa7134-empress.c
+++ b/drivers/media/pci/saa7134/saa7134-empress.c
@@ -48,21 +48,16 @@ MODULE_PARM_DESC(debug,"enable debug messages");
 
 /* ------------------------------------------------------------------ */
 
-static void ts_reset_encoder(struct saa7134_dev* dev)
-{
-	if (!dev->empress_started)
-		return;
-
-	saa_writeb(SAA7134_SPECIAL_MODE, 0x00);
-	msleep(10);
-	saa_writeb(SAA7134_SPECIAL_MODE, 0x01);
-	msleep(100);
-	dev->empress_started = 0;
-}
-
-static int ts_init_encoder(struct saa7134_dev* dev)
+static int start_streaming(struct vb2_queue *vq, unsigned int count)
 {
+	struct saa7134_dmaqueue *dmaq = vq->drv_priv;
+	struct saa7134_dev *dev = dmaq->dev;
 	u32 leading_null_bytes = 0;
+	int err;
+
+	err = saa7134_ts_start_streaming(vq, count);
+	if (err)
+		return err;
 
 	/* If more cards start to need this, then this
 	   should probably be added to the card definitions. */
@@ -73,97 +68,43 @@ static int ts_init_encoder(struct saa7134_dev* dev)
 		leading_null_bytes = 1;
 		break;
 	}
-	ts_reset_encoder(dev);
 	saa_call_all(dev, core, init, leading_null_bytes);
-	dev->empress_started = 1;
-	return 0;
-}
-
-/* ------------------------------------------------------------------ */
-
-static int ts_open(struct file *file)
-{
-	struct video_device *vdev = video_devdata(file);
-	struct saa7134_dev *dev = video_drvdata(file);
-	struct saa7134_fh *fh;
-
-	/* allocate + initialize per filehandle data */
-	fh = kzalloc(sizeof(*fh), GFP_KERNEL);
-	if (NULL == fh)
-		return -ENOMEM;
-
-	v4l2_fh_init(&fh->fh, vdev);
-	file->private_data = fh;
-	fh->is_empress = true;
-	v4l2_fh_add(&fh->fh);
-
 	/* Unmute audio */
 	saa_writeb(SAA7134_AUDIO_MUTE_CTRL,
-		saa_readb(SAA7134_AUDIO_MUTE_CTRL) & ~(1 << 6));
-
-	return 0;
-}
-
-static int ts_release(struct file *file)
-{
-	struct saa7134_dev *dev = video_drvdata(file);
-	struct saa7134_fh *fh = file->private_data;
-
-	if (res_check(fh, RESOURCE_EMPRESS)) {
-		videobuf_stop(&dev->empress_tsq);
-		videobuf_mmap_free(&dev->empress_tsq);
-
-		/* stop the encoder */
-		ts_reset_encoder(dev);
-
-		/* Mute audio */
-		saa_writeb(SAA7134_AUDIO_MUTE_CTRL,
-				saa_readb(SAA7134_AUDIO_MUTE_CTRL) | (1 << 6));
-	}
-
-	v4l2_fh_del(&fh->fh);
-	v4l2_fh_exit(&fh->fh);
+			saa_readb(SAA7134_AUDIO_MUTE_CTRL) & ~(1 << 6));
+	dev->empress_started = 1;
 	return 0;
 }
 
-static ssize_t
-ts_read(struct file *file, char __user *data, size_t count, loff_t *ppos)
+static void stop_streaming(struct vb2_queue *vq)
 {
-	struct saa7134_dev *dev = video_drvdata(file);
+	struct saa7134_dmaqueue *dmaq = vq->drv_priv;
+	struct saa7134_dev *dev = dmaq->dev;
 
-	if (res_locked(dev, RESOURCE_EMPRESS))
-		return -EBUSY;
-	if (!dev->empress_started)
-		ts_init_encoder(dev);
-
-	return videobuf_read_stream(&dev->empress_tsq,
-				    data, count, ppos, 0,
-				    file->f_flags & O_NONBLOCK);
-}
-
-static unsigned int
-ts_poll(struct file *file, struct poll_table_struct *wait)
-{
-	unsigned long req_events = poll_requested_events(wait);
-	struct saa7134_dev *dev = video_drvdata(file);
-	struct saa7134_fh *fh = file->private_data;
-	unsigned int rc = 0;
-
-	if (v4l2_event_pending(&fh->fh))
-		rc = POLLPRI;
-	else if (req_events & POLLPRI)
-		poll_wait(file, &fh->fh.wait, wait);
-	return rc | videobuf_poll_stream(file, &dev->empress_tsq, wait);
+	saa7134_ts_stop_streaming(vq);
+	saa_writeb(SAA7134_SPECIAL_MODE, 0x00);
+	msleep(20);
+	saa_writeb(SAA7134_SPECIAL_MODE, 0x01);
+	msleep(100);
+	/* Mute audio */
+	saa_writeb(SAA7134_AUDIO_MUTE_CTRL,
+			saa_readb(SAA7134_AUDIO_MUTE_CTRL) | (1 << 6));
+	dev->empress_started = 0;
 }
 
+static struct vb2_ops saa7134_empress_qops = {
+	.queue_setup	= saa7134_ts_queue_setup,
+	.buf_init	= saa7134_ts_buffer_init,
+	.buf_prepare	= saa7134_ts_buffer_prepare,
+	.buf_finish	= saa7134_ts_buffer_finish,
+	.buf_queue	= saa7134_vb2_buffer_queue,
+	.wait_prepare	= vb2_ops_wait_prepare,
+	.wait_finish	= vb2_ops_wait_finish,
+	.start_streaming = start_streaming,
+	.stop_streaming = stop_streaming,
+};
 
-static int
-ts_mmap(struct file *file, struct vm_area_struct * vma)
-{
-	struct saa7134_dev *dev = video_drvdata(file);
-
-	return videobuf_mmap_mapper(&dev->empress_tsq, vma);
-}
+/* ------------------------------------------------------------------ */
 
 static int empress_enum_fmt_vid_cap(struct file *file, void  *priv,
 					struct v4l2_fmtdesc *f)
@@ -233,11 +174,11 @@ static int empress_try_fmt_vid_cap(struct file *file, void *priv,
 static const struct v4l2_file_operations ts_fops =
 {
 	.owner	  = THIS_MODULE,
-	.open	  = ts_open,
-	.release  = ts_release,
-	.read	  = ts_read,
-	.poll	  = ts_poll,
-	.mmap	  = ts_mmap,
+	.open	  = v4l2_fh_open,
+	.release  = vb2_fop_release,
+	.read	  = vb2_fop_read,
+	.poll	  = vb2_fop_poll,
+	.mmap	  = vb2_fop_mmap,
 	.ioctl	  = video_ioctl2,
 };
 
@@ -247,12 +188,12 @@ static const struct v4l2_ioctl_ops ts_ioctl_ops = {
 	.vidioc_try_fmt_vid_cap		= empress_try_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap		= empress_s_fmt_vid_cap,
 	.vidioc_g_fmt_vid_cap		= empress_g_fmt_vid_cap,
-	.vidioc_reqbufs			= saa7134_reqbufs,
-	.vidioc_querybuf		= saa7134_querybuf,
-	.vidioc_qbuf			= saa7134_qbuf,
-	.vidioc_dqbuf			= saa7134_dqbuf,
-	.vidioc_streamon		= saa7134_streamon,
-	.vidioc_streamoff		= saa7134_streamoff,
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.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_g_frequency		= saa7134_g_frequency,
 	.vidioc_s_frequency		= saa7134_s_frequency,
 	.vidioc_g_tuner			= saa7134_g_tuner,
@@ -262,6 +203,7 @@ static const struct v4l2_ioctl_ops ts_ioctl_ops = {
 	.vidioc_s_input			= saa7134_s_input,
 	.vidioc_s_std			= saa7134_s_std,
 	.vidioc_g_std			= saa7134_g_std,
+	.vidioc_querystd		= saa7134_querystd,
 	.vidioc_log_status		= v4l2_ctrl_log_status,
 	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
 	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
@@ -314,6 +256,7 @@ static bool empress_ctrl_filter(const struct v4l2_ctrl *ctrl)
 static int empress_init(struct saa7134_dev *dev)
 {
 	struct v4l2_ctrl_handler *hdl = &dev->empress_ctrl_handler;
+	struct vb2_queue *q;
 	int err;
 
 	dprintk("%s: %s\n",dev->name,__func__);
@@ -323,6 +266,7 @@ static int empress_init(struct saa7134_dev *dev)
 	*(dev->empress_dev) = saa7134_empress_template;
 	dev->empress_dev->v4l2_dev  = &dev->v4l2_dev;
 	dev->empress_dev->release = video_device_release;
+	dev->empress_dev->lock = &dev->lock;
 	snprintf(dev->empress_dev->name, sizeof(dev->empress_dev->name),
 		 "%s empress (%s)", dev->name,
 		 saa7134_boards[dev->board].name);
@@ -339,6 +283,26 @@ static int empress_init(struct saa7134_dev *dev)
 
 	INIT_WORK(&dev->empress_workqueue, empress_signal_update);
 
+	q = &dev->empress_vbq;
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	/*
+	 * Do not add VB2_USERPTR: the saa7134 DMA engine cannot handle
+	 * transfers that do not start at the beginning of a page. A USERPTR
+	 * can start anywhere in a page, so USERPTR support is a no-go.
+	 */
+	q->io_modes = VB2_MMAP | VB2_READ;
+	q->drv_priv = &dev->ts_q;
+	q->ops = &saa7134_empress_qops;
+	q->gfp_flags = GFP_DMA32;
+	q->mem_ops = &vb2_dma_sg_memops;
+	q->buf_struct_size = sizeof(struct saa7134_buf);
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &dev->lock;
+	err = vb2_queue_init(q);
+	if (err)
+		return err;
+	dev->empress_dev->queue = q;
+
 	video_set_drvdata(dev->empress_dev, dev);
 	err = video_register_device(dev->empress_dev,VFL_TYPE_GRABBER,
 				    empress_nr[dev->nr]);
@@ -352,13 +316,6 @@ static int empress_init(struct saa7134_dev *dev)
 	printk(KERN_INFO "%s: registered device %s [mpeg]\n",
 	       dev->name, video_device_node_name(dev->empress_dev));
 
-	videobuf_queue_sg_init(&dev->empress_tsq, &saa7134_ts_qops,
-			    &dev->pci->dev, &dev->slock,
-			    V4L2_BUF_TYPE_VIDEO_CAPTURE,
-			    V4L2_FIELD_ALTERNATE,
-			    sizeof(struct saa7134_buf),
-			    dev, NULL);
-
 	empress_signal_update(&dev->empress_workqueue);
 	return 0;
 }
@@ -371,6 +328,7 @@ static int empress_fini(struct saa7134_dev *dev)
 		return 0;
 	flush_work(&dev->empress_workqueue);
 	video_unregister_device(dev->empress_dev);
+	vb2_queue_release(&dev->empress_vbq);
 	v4l2_ctrl_handler_free(&dev->empress_ctrl_handler);
 	dev->empress_dev = NULL;
 	return 0;
@@ -395,10 +353,3 @@ static void __exit empress_unregister(void)
 
 module_init(empress_register);
 module_exit(empress_unregister);
-
-/* ----------------------------------------------------------- */
-/*
- * Local variables:
- * c-basic-offset: 8
- * End:
- */
diff --git a/drivers/media/pci/saa7134/saa7134-i2c.c b/drivers/media/pci/saa7134/saa7134-i2c.c
index c68169d7580..f4da674e7f2 100644
--- a/drivers/media/pci/saa7134/saa7134-i2c.c
+++ b/drivers/media/pci/saa7134/saa7134-i2c.c
@@ -427,10 +427,3 @@ int saa7134_i2c_unregister(struct saa7134_dev *dev)
 	i2c_del_adapter(&dev->i2c_adap);
 	return 0;
 }
-
-/* ----------------------------------------------------------- */
-/*
- * Local variables:
- * c-basic-offset: 8
- * End:
- */
diff --git a/drivers/media/pci/saa7134/saa7134-reg.h b/drivers/media/pci/saa7134/saa7134-reg.h
index e7e0af101fa..b6ea6f4f9b6 100644
--- a/drivers/media/pci/saa7134/saa7134-reg.h
+++ b/drivers/media/pci/saa7134/saa7134-reg.h
@@ -167,17 +167,22 @@
 #define SAA7134_HSYNC_START                     0x106
 #define SAA7134_HSYNC_STOP                      0x107
 #define SAA7134_SYNC_CTRL                       0x108
+#define   SAA7134_SYNC_CTRL_AUFD                (1 << 7)
 #define SAA7134_LUMA_CTRL                       0x109
+#define   SAA7134_LUMA_CTRL_LDEL                (1 << 5)
 #define SAA7134_DEC_LUMA_BRIGHT                 0x10a
 #define SAA7134_DEC_LUMA_CONTRAST               0x10b
 #define SAA7134_DEC_CHROMA_SATURATION           0x10c
 #define SAA7134_DEC_CHROMA_HUE                  0x10d
 #define SAA7134_CHROMA_CTRL1                    0x10e
+#define   SAA7134_CHROMA_CTRL1_AUTO0            (1 << 1)
+#define   SAA7134_CHROMA_CTRL1_FCTC             (1 << 2)
 #define SAA7134_CHROMA_GAIN                     0x10f
 #define SAA7134_CHROMA_CTRL2                    0x110
 #define SAA7134_MODE_DELAY_CTRL                 0x111
 
 #define SAA7134_ANALOG_ADC                      0x114
+#define   SAA7134_ANALOG_ADC_AUTO1              (1 << 2)
 #define SAA7134_VGATE_START                     0x115
 #define SAA7134_VGATE_STOP                      0x116
 #define SAA7134_MISC_VGATE_MSB                  0x117
@@ -369,10 +374,3 @@
 #define SAA7135_DSP_RWCLEAR_RERR		    1
 
 #define SAA7133_I2S_AUDIO_CONTROL               0x591
-/* ------------------------------------------------------------------ */
-/*
- * Local variables:
- * c-basic-offset: 8
- * End:
- */
-
diff --git a/drivers/media/pci/saa7134/saa7134-ts.c b/drivers/media/pci/saa7134/saa7134-ts.c
index 2e3f4b412d8..bd25323bd94 100644
--- a/drivers/media/pci/saa7134/saa7134-ts.c
+++ b/drivers/media/pci/saa7134/saa7134-ts.c
@@ -39,26 +39,29 @@ MODULE_PARM_DESC(ts_debug,"enable debug messages [ts]");
 	printk(KERN_DEBUG "%s/ts: " fmt, dev->name , ## arg)
 
 /* ------------------------------------------------------------------ */
-
 static int buffer_activate(struct saa7134_dev *dev,
 			   struct saa7134_buf *buf,
 			   struct saa7134_buf *next)
 {
 
 	dprintk("buffer_activate [%p]",buf);
-	buf->vb.state = VIDEOBUF_ACTIVE;
 	buf->top_seen = 0;
 
+	if (!dev->ts_started)
+		dev->ts_field = V4L2_FIELD_TOP;
+
 	if (NULL == next)
 		next = buf;
-	if (V4L2_FIELD_TOP == buf->vb.field) {
+	if (V4L2_FIELD_TOP == dev->ts_field) {
 		dprintk("- [top]     buf=%p next=%p\n",buf,next);
 		saa_writel(SAA7134_RS_BA1(5),saa7134_buffer_base(buf));
 		saa_writel(SAA7134_RS_BA2(5),saa7134_buffer_base(next));
+		dev->ts_field = V4L2_FIELD_BOTTOM;
 	} else {
 		dprintk("- [bottom]  buf=%p next=%p\n",buf,next);
 		saa_writel(SAA7134_RS_BA1(5),saa7134_buffer_base(next));
 		saa_writel(SAA7134_RS_BA2(5),saa7134_buffer_base(buf));
+		dev->ts_field = V4L2_FIELD_TOP;
 	}
 
 	/* start DMA */
@@ -72,96 +75,123 @@ static int buffer_activate(struct saa7134_dev *dev,
 	return 0;
 }
 
-static int buffer_prepare(struct videobuf_queue *q, struct videobuf_buffer *vb,
-		enum v4l2_field field)
+int saa7134_ts_buffer_init(struct vb2_buffer *vb2)
 {
-	struct saa7134_dev *dev = q->priv_data;
-	struct saa7134_buf *buf = container_of(vb,struct saa7134_buf,vb);
+	struct saa7134_dmaqueue *dmaq = vb2->vb2_queue->drv_priv;
+	struct saa7134_buf *buf = container_of(vb2, struct saa7134_buf, vb2);
+
+	dmaq->curr = NULL;
+	buf->activate = buffer_activate;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(saa7134_ts_buffer_init);
+
+int saa7134_ts_buffer_prepare(struct vb2_buffer *vb2)
+{
+	struct saa7134_dmaqueue *dmaq = vb2->vb2_queue->drv_priv;
+	struct saa7134_dev *dev = dmaq->dev;
+	struct saa7134_buf *buf = container_of(vb2, struct saa7134_buf, vb2);
+	struct sg_table *dma = vb2_dma_sg_plane_desc(vb2, 0);
 	unsigned int lines, llength, size;
-	int err;
+	int ret;
 
-	dprintk("buffer_prepare [%p,%s]\n",buf,v4l2_field_names[field]);
+	dprintk("buffer_prepare [%p]\n", buf);
 
 	llength = TS_PACKET_SIZE;
 	lines = dev->ts.nr_packets;
 
 	size = lines * llength;
-	if (0 != buf->vb.baddr  &&  buf->vb.bsize < size)
+	if (vb2_plane_size(vb2, 0) < size)
 		return -EINVAL;
 
-	if (buf->vb.size != size) {
-		saa7134_dma_free(q,buf);
-	}
-
-	if (VIDEOBUF_NEEDS_INIT == buf->vb.state) {
-
-		struct videobuf_dmabuf *dma=videobuf_to_dma(&buf->vb);
-
-		dprintk("buffer_prepare: needs_init\n");
-
-		buf->vb.width  = llength;
-		buf->vb.height = lines;
-		buf->vb.size   = size;
-		buf->pt        = &dev->ts.pt_ts;
-
-		err = videobuf_iolock(q,&buf->vb,NULL);
-		if (err)
-			goto oops;
-		err = saa7134_pgtable_build(dev->pci,buf->pt,
-					    dma->sglist,
-					    dma->sglen,
-					    saa7134_buffer_startpage(buf));
-		if (err)
-			goto oops;
-	}
-
-	buf->vb.state = VIDEOBUF_PREPARED;
-	buf->activate = buffer_activate;
-	buf->vb.field = field;
-	return 0;
+	vb2_set_plane_payload(vb2, 0, size);
+	vb2->v4l2_buf.field = dev->field;
 
- oops:
-	saa7134_dma_free(q,buf);
-	return err;
+	ret = dma_map_sg(&dev->pci->dev, dma->sgl, dma->nents, DMA_FROM_DEVICE);
+	if (!ret)
+		return -EIO;
+	return saa7134_pgtable_build(dev->pci, &dmaq->pt, dma->sgl, dma->nents,
+				    saa7134_buffer_startpage(buf));
 }
+EXPORT_SYMBOL_GPL(saa7134_ts_buffer_prepare);
 
-static int
-buffer_setup(struct videobuf_queue *q, unsigned int *count, unsigned int *size)
+void saa7134_ts_buffer_finish(struct vb2_buffer *vb2)
 {
-	struct saa7134_dev *dev = q->priv_data;
+	struct saa7134_dmaqueue *dmaq = vb2->vb2_queue->drv_priv;
+	struct saa7134_dev *dev = dmaq->dev;
+	struct saa7134_buf *buf = container_of(vb2, struct saa7134_buf, vb2);
+	struct sg_table *dma = vb2_dma_sg_plane_desc(&buf->vb2, 0);
 
-	*size = TS_PACKET_SIZE * dev->ts.nr_packets;
-	if (0 == *count)
-		*count = dev->ts.nr_bufs;
-	*count = saa7134_buffer_count(*size,*count);
+	dma_unmap_sg(&dev->pci->dev, dma->sgl, dma->nents, DMA_FROM_DEVICE);
+}
+EXPORT_SYMBOL_GPL(saa7134_ts_buffer_finish);
 
+int saa7134_ts_queue_setup(struct vb2_queue *q, const struct v4l2_format *fmt,
+			   unsigned int *nbuffers, unsigned int *nplanes,
+			   unsigned int sizes[], void *alloc_ctxs[])
+{
+	struct saa7134_dmaqueue *dmaq = q->drv_priv;
+	struct saa7134_dev *dev = dmaq->dev;
+	int size = TS_PACKET_SIZE * dev->ts.nr_packets;
+
+	if (0 == *nbuffers)
+		*nbuffers = dev->ts.nr_bufs;
+	*nbuffers = saa7134_buffer_count(size, *nbuffers);
+	if (*nbuffers < 3)
+		*nbuffers = 3;
+	*nplanes = 1;
+	sizes[0] = size;
 	return 0;
 }
+EXPORT_SYMBOL_GPL(saa7134_ts_queue_setup);
 
-static void buffer_queue(struct videobuf_queue *q, struct videobuf_buffer *vb)
+int saa7134_ts_start_streaming(struct vb2_queue *vq, unsigned int count)
 {
-	struct saa7134_dev *dev = q->priv_data;
-	struct saa7134_buf *buf = container_of(vb,struct saa7134_buf,vb);
-
-	saa7134_buffer_queue(dev,&dev->ts_q,buf);
+	struct saa7134_dmaqueue *dmaq = vq->drv_priv;
+	struct saa7134_dev *dev = dmaq->dev;
+
+	/*
+	 * Planar video capture and TS share the same DMA channel,
+	 * so only one can be active at a time.
+	 */
+	if (vb2_is_busy(&dev->video_vbq) && dev->fmt->planar) {
+		struct saa7134_buf *buf, *tmp;
+
+		list_for_each_entry_safe(buf, tmp, &dmaq->queue, entry) {
+			list_del(&buf->entry);
+			vb2_buffer_done(&buf->vb2, VB2_BUF_STATE_QUEUED);
+		}
+		if (dmaq->curr) {
+			vb2_buffer_done(&dmaq->curr->vb2, VB2_BUF_STATE_QUEUED);
+			dmaq->curr = NULL;
+		}
+		return -EBUSY;
+	}
+	dmaq->seq_nr = 0;
+	return 0;
 }
+EXPORT_SYMBOL_GPL(saa7134_ts_start_streaming);
 
-static void buffer_release(struct videobuf_queue *q, struct videobuf_buffer *vb)
+void saa7134_ts_stop_streaming(struct vb2_queue *vq)
 {
-	struct saa7134_buf *buf = container_of(vb,struct saa7134_buf,vb);
-	struct saa7134_dev *dev = q->priv_data;
-
-	if (dev->ts_started)
-		saa7134_ts_stop(dev);
+	struct saa7134_dmaqueue *dmaq = vq->drv_priv;
+	struct saa7134_dev *dev = dmaq->dev;
 
-	saa7134_dma_free(q,buf);
+	saa7134_ts_stop(dev);
+	saa7134_stop_streaming(dev, dmaq);
 }
-
-struct videobuf_queue_ops saa7134_ts_qops = {
-	.buf_setup    = buffer_setup,
-	.buf_prepare  = buffer_prepare,
-	.buf_queue    = buffer_queue,
-	.buf_release  = buffer_release,
+EXPORT_SYMBOL_GPL(saa7134_ts_stop_streaming);
+
+struct vb2_ops saa7134_ts_qops = {
+	.queue_setup	= saa7134_ts_queue_setup,
+	.buf_init	= saa7134_ts_buffer_init,
+	.buf_prepare	= saa7134_ts_buffer_prepare,
+	.buf_finish	= saa7134_ts_buffer_finish,
+	.buf_queue	= saa7134_vb2_buffer_queue,
+	.wait_prepare	= vb2_ops_wait_prepare,
+	.wait_finish	= vb2_ops_wait_finish,
+	.stop_streaming = saa7134_ts_stop_streaming,
 };
 EXPORT_SYMBOL_GPL(saa7134_ts_qops);
 
@@ -213,7 +243,7 @@ int saa7134_ts_init1(struct saa7134_dev *dev)
 	dev->ts_q.dev              = dev;
 	dev->ts_q.need_two         = 1;
 	dev->ts_started            = 0;
-	saa7134_pgtable_alloc(dev->pci,&dev->ts.pt_ts);
+	saa7134_pgtable_alloc(dev->pci, &dev->ts_q.pt);
 
 	/* init TS hw */
 	saa7134_ts_init_hw(dev);
@@ -226,7 +256,8 @@ int saa7134_ts_stop(struct saa7134_dev *dev)
 {
 	dprintk("TS stop\n");
 
-	BUG_ON(!dev->ts_started);
+	if (!dev->ts_started)
+		return 0;
 
 	/* Stop TS stream */
 	switch (saa7134_boards[dev->board].ts_type) {
@@ -247,7 +278,8 @@ int saa7134_ts_start(struct saa7134_dev *dev)
 {
 	dprintk("TS start\n");
 
-	BUG_ON(dev->ts_started);
+	if (WARN_ON(dev->ts_started))
+		return 0;
 
 	/* dma: setup channel 5 (= TS) */
 	saa_writeb(SAA7134_TS_DMA0, (dev->ts.nr_packets - 1) & 0xff);
@@ -259,7 +291,7 @@ int saa7134_ts_start(struct saa7134_dev *dev)
 	saa_writel(SAA7134_RS_PITCH(5), TS_PACKET_SIZE);
 	saa_writel(SAA7134_RS_CONTROL(5), SAA7134_RS_CONTROL_BURST_16 |
 					  SAA7134_RS_CONTROL_ME |
-					  (dev->ts.pt_ts.dma >> 12));
+					  (dev->ts_q.pt.dma >> 12));
 
 	/* reset hardware TS buffers */
 	saa_writeb(SAA7134_TS_SERIAL1, 0x00);
@@ -293,7 +325,7 @@ int saa7134_ts_start(struct saa7134_dev *dev)
 
 int saa7134_ts_fini(struct saa7134_dev *dev)
 {
-	saa7134_pgtable_free(dev->pci,&dev->ts.pt_ts);
+	saa7134_pgtable_free(dev->pci, &dev->ts_q.pt);
 	return 0;
 }
 
@@ -303,25 +335,18 @@ void saa7134_irq_ts_done(struct saa7134_dev *dev, unsigned long status)
 
 	spin_lock(&dev->slock);
 	if (dev->ts_q.curr) {
-		field = dev->ts_q.curr->vb.field;
-		if (field == V4L2_FIELD_TOP) {
+		field = dev->ts_field;
+		if (field != V4L2_FIELD_TOP) {
 			if ((status & 0x100000) != 0x000000)
 				goto done;
 		} else {
 			if ((status & 0x100000) != 0x100000)
 				goto done;
 		}
-		saa7134_buffer_finish(dev,&dev->ts_q,VIDEOBUF_DONE);
+		saa7134_buffer_finish(dev, &dev->ts_q, VB2_BUF_STATE_DONE);
 	}
 	saa7134_buffer_next(dev,&dev->ts_q);
 
  done:
 	spin_unlock(&dev->slock);
 }
-
-/* ----------------------------------------------------------- */
-/*
- * Local variables:
- * c-basic-offset: 8
- * End:
- */
diff --git a/drivers/media/pci/saa7134/saa7134-tvaudio.c b/drivers/media/pci/saa7134/saa7134-tvaudio.c
index 0f34e09d98d..3afbcb70b51 100644
--- a/drivers/media/pci/saa7134/saa7134-tvaudio.c
+++ b/drivers/media/pci/saa7134/saa7134-tvaudio.c
@@ -1079,10 +1079,3 @@ int saa7134_tvaudio_do_scan(struct saa7134_dev *dev)
 
 EXPORT_SYMBOL(saa_dsp_writel);
 EXPORT_SYMBOL(saa7134_tvaudio_setmute);
-
-/* ----------------------------------------------------------- */
-/*
- * Local variables:
- * c-basic-offset: 8
- * End:
- */
diff --git a/drivers/media/pci/saa7134/saa7134-vbi.c b/drivers/media/pci/saa7134/saa7134-vbi.c
index d4da18d049f..c06dbe17a87 100644
--- a/drivers/media/pci/saa7134/saa7134-vbi.c
+++ b/drivers/media/pci/saa7134/saa7134-vbi.c
@@ -67,10 +67,10 @@ static void task_init(struct saa7134_dev *dev, struct saa7134_buf *buf,
 	saa_writeb(SAA7134_VBI_PHASE_OFFSET_LUMA(task),   0x00);
 	saa_writeb(SAA7134_VBI_PHASE_OFFSET_CHROMA(task), 0x00);
 
-	saa_writeb(SAA7134_VBI_H_LEN1(task), buf->vb.width   & 0xff);
-	saa_writeb(SAA7134_VBI_H_LEN2(task), buf->vb.width   >> 8);
-	saa_writeb(SAA7134_VBI_V_LEN1(task), buf->vb.height  & 0xff);
-	saa_writeb(SAA7134_VBI_V_LEN2(task), buf->vb.height  >> 8);
+	saa_writeb(SAA7134_VBI_H_LEN1(task), dev->vbi_hlen & 0xff);
+	saa_writeb(SAA7134_VBI_H_LEN2(task), dev->vbi_hlen >> 8);
+	saa_writeb(SAA7134_VBI_V_LEN1(task), dev->vbi_vlen & 0xff);
+	saa_writeb(SAA7134_VBI_V_LEN2(task), dev->vbi_vlen >> 8);
 
 	saa_andorb(SAA7134_DATA_PATH(task), 0xc0, 0x00);
 }
@@ -81,14 +81,14 @@ static int buffer_activate(struct saa7134_dev *dev,
 			   struct saa7134_buf *buf,
 			   struct saa7134_buf *next)
 {
-	unsigned long control,base;
+	struct saa7134_dmaqueue *dmaq = buf->vb2.vb2_queue->drv_priv;
+	unsigned long control, base;
 
-	dprintk("buffer_activate [%p]\n",buf);
-	buf->vb.state = VIDEOBUF_ACTIVE;
+	dprintk("buffer_activate [%p]\n", buf);
 	buf->top_seen = 0;
 
-	task_init(dev,buf,TASK_A);
-	task_init(dev,buf,TASK_B);
+	task_init(dev, buf, TASK_A);
+	task_init(dev, buf, TASK_B);
 	saa_writeb(SAA7134_OFMT_DATA_A, 0x06);
 	saa_writeb(SAA7134_OFMT_DATA_B, 0x06);
 
@@ -96,107 +96,99 @@ static int buffer_activate(struct saa7134_dev *dev,
 	base    = saa7134_buffer_base(buf);
 	control = SAA7134_RS_CONTROL_BURST_16 |
 		SAA7134_RS_CONTROL_ME |
-		(buf->pt->dma >> 12);
-	saa_writel(SAA7134_RS_BA1(2),base);
-	saa_writel(SAA7134_RS_BA2(2),base + buf->vb.size/2);
-	saa_writel(SAA7134_RS_PITCH(2),buf->vb.width);
-	saa_writel(SAA7134_RS_CONTROL(2),control);
-	saa_writel(SAA7134_RS_BA1(3),base);
-	saa_writel(SAA7134_RS_BA2(3),base + buf->vb.size/2);
-	saa_writel(SAA7134_RS_PITCH(3),buf->vb.width);
-	saa_writel(SAA7134_RS_CONTROL(3),control);
+		(dmaq->pt.dma >> 12);
+	saa_writel(SAA7134_RS_BA1(2), base);
+	saa_writel(SAA7134_RS_BA2(2), base + dev->vbi_hlen * dev->vbi_vlen);
+	saa_writel(SAA7134_RS_PITCH(2), dev->vbi_hlen);
+	saa_writel(SAA7134_RS_CONTROL(2), control);
+	saa_writel(SAA7134_RS_BA1(3), base);
+	saa_writel(SAA7134_RS_BA2(3), base + dev->vbi_hlen * dev->vbi_vlen);
+	saa_writel(SAA7134_RS_PITCH(3), dev->vbi_hlen);
+	saa_writel(SAA7134_RS_CONTROL(3), control);
 
 	/* start DMA */
 	saa7134_set_dmabits(dev);
-	mod_timer(&dev->vbi_q.timeout, jiffies+BUFFER_TIMEOUT);
+	mod_timer(&dmaq->timeout, jiffies + BUFFER_TIMEOUT);
 
 	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 *vb2)
 {
-	struct saa7134_dev *dev = q->priv_data;
-	struct saa7134_buf *buf = container_of(vb,struct saa7134_buf,vb);
-	struct saa7134_tvnorm *norm = dev->tvnorm;
-	unsigned int lines, llength, size;
-	int err;
-
-	lines   = norm->vbi_v_stop_0 - norm->vbi_v_start_0 +1;
-	if (lines > VBI_LINE_COUNT)
-		lines = VBI_LINE_COUNT;
-	llength = VBI_LINE_LENGTH;
-	size = lines * llength * 2;
-	if (0 != buf->vb.baddr  &&  buf->vb.bsize < size)
+	struct saa7134_dmaqueue *dmaq = vb2->vb2_queue->drv_priv;
+	struct saa7134_dev *dev = dmaq->dev;
+	struct saa7134_buf *buf = container_of(vb2, struct saa7134_buf, vb2);
+	struct sg_table *dma = vb2_dma_sg_plane_desc(&buf->vb2, 0);
+	unsigned int size;
+	int ret;
+
+	if (dma->sgl->offset) {
+		pr_err("The buffer is not page-aligned\n");
 		return -EINVAL;
-
-	if (buf->vb.size != size)
-		saa7134_dma_free(q,buf);
-
-	if (VIDEOBUF_NEEDS_INIT == buf->vb.state) {
-		struct videobuf_dmabuf *dma=videobuf_to_dma(&buf->vb);
-
-		buf->vb.width  = llength;
-		buf->vb.height = lines;
-		buf->vb.size   = size;
-		buf->pt        = &dev->pt_vbi;
-
-		err = videobuf_iolock(q,&buf->vb,NULL);
-		if (err)
-			goto oops;
-		err = saa7134_pgtable_build(dev->pci,buf->pt,
-					    dma->sglist,
-					    dma->sglen,
-					    saa7134_buffer_startpage(buf));
-		if (err)
-			goto oops;
 	}
-	buf->vb.state = VIDEOBUF_PREPARED;
-	buf->activate = buffer_activate;
-	buf->vb.field = field;
-	return 0;
+	size = dev->vbi_hlen * dev->vbi_vlen * 2;
+	if (vb2_plane_size(vb2, 0) < size)
+		return -EINVAL;
+
+	vb2_set_plane_payload(vb2, 0, size);
 
- oops:
-	saa7134_dma_free(q,buf);
-	return err;
+	ret = dma_map_sg(&dev->pci->dev, dma->sgl, dma->nents, DMA_FROM_DEVICE);
+	if (!ret)
+		return -EIO;
+	return saa7134_pgtable_build(dev->pci, &dmaq->pt, dma->sgl, dma->nents,
+				    saa7134_buffer_startpage(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 *nbuffers, unsigned int *nplanes,
+			   unsigned int sizes[], void *alloc_ctxs[])
 {
-	struct saa7134_dev *dev = q->priv_data;
-	int llength,lines;
-
-	lines   = dev->tvnorm->vbi_v_stop_0 - dev->tvnorm->vbi_v_start_0 +1;
-	llength = VBI_LINE_LENGTH;
-	*size = lines * llength * 2;
-	if (0 == *count)
-		*count = vbibufs;
-	*count = saa7134_buffer_count(*size,*count);
+	struct saa7134_dmaqueue *dmaq = q->drv_priv;
+	struct saa7134_dev *dev = dmaq->dev;
+	unsigned int size;
+
+	dev->vbi_vlen = dev->tvnorm->vbi_v_stop_0 - dev->tvnorm->vbi_v_start_0 + 1;
+	if (dev->vbi_vlen > VBI_LINE_COUNT)
+		dev->vbi_vlen = VBI_LINE_COUNT;
+	dev->vbi_hlen = VBI_LINE_LENGTH;
+	size = dev->vbi_hlen * dev->vbi_vlen * 2;
+
+	*nbuffers = saa7134_buffer_count(size, *nbuffers);
+	*nplanes = 1;
+	sizes[0] = size;
 	return 0;
 }
 
-static void buffer_queue(struct videobuf_queue *q, struct videobuf_buffer *vb)
+static int buffer_init(struct vb2_buffer *vb2)
 {
-	struct saa7134_dev *dev = q->priv_data;
-	struct saa7134_buf *buf = container_of(vb,struct saa7134_buf,vb);
+	struct saa7134_dmaqueue *dmaq = vb2->vb2_queue->drv_priv;
+	struct saa7134_buf *buf = container_of(vb2, struct saa7134_buf, vb2);
 
-	saa7134_buffer_queue(dev,&dev->vbi_q,buf);
+	dmaq->curr = NULL;
+	buf->activate = buffer_activate;
+	return 0;
 }
 
-static void buffer_release(struct videobuf_queue *q, struct videobuf_buffer *vb)
+static void buffer_finish(struct vb2_buffer *vb2)
 {
-	struct saa7134_buf *buf = container_of(vb,struct saa7134_buf,vb);
+	struct saa7134_dmaqueue *dmaq = vb2->vb2_queue->drv_priv;
+	struct saa7134_dev *dev = dmaq->dev;
+	struct saa7134_buf *buf = container_of(vb2, struct saa7134_buf, vb2);
+	struct sg_table *dma = vb2_dma_sg_plane_desc(&buf->vb2, 0);
 
-	saa7134_dma_free(q,buf);
+	dma_unmap_sg(&dev->pci->dev, dma->sgl, dma->nents, DMA_FROM_DEVICE);
 }
 
-struct videobuf_queue_ops saa7134_vbi_qops = {
-	.buf_setup    = buffer_setup,
-	.buf_prepare  = buffer_prepare,
-	.buf_queue    = buffer_queue,
-	.buf_release  = buffer_release,
+struct vb2_ops saa7134_vbi_qops = {
+	.queue_setup	= queue_setup,
+	.buf_init	= buffer_init,
+	.buf_prepare	= buffer_prepare,
+	.buf_finish	= buffer_finish,
+	.buf_queue	= saa7134_vb2_buffer_queue,
+	.wait_prepare	= vb2_ops_wait_prepare,
+	.wait_finish	= vb2_ops_wait_finish,
+	.start_streaming = saa7134_vb2_start_streaming,
+	.stop_streaming = saa7134_vb2_stop_streaming,
 };
 
 /* ------------------------------------------------------------------ */
@@ -226,7 +218,6 @@ void saa7134_irq_vbi_done(struct saa7134_dev *dev, unsigned long status)
 {
 	spin_lock(&dev->slock);
 	if (dev->vbi_q.curr) {
-		dev->vbi_fieldcount++;
 		/* make sure we have seen both fields */
 		if ((status & 0x10) == 0x00) {
 			dev->vbi_q.curr->top_seen = 1;
@@ -235,18 +226,10 @@ void saa7134_irq_vbi_done(struct saa7134_dev *dev, unsigned long status)
 		if (!dev->vbi_q.curr->top_seen)
 			goto done;
 
-		dev->vbi_q.curr->vb.field_count = dev->vbi_fieldcount;
-		saa7134_buffer_finish(dev,&dev->vbi_q,VIDEOBUF_DONE);
+		saa7134_buffer_finish(dev, &dev->vbi_q, VB2_BUF_STATE_DONE);
 	}
-	saa7134_buffer_next(dev,&dev->vbi_q);
+	saa7134_buffer_next(dev, &dev->vbi_q);
 
  done:
 	spin_unlock(&dev->slock);
 }
-
-/* ----------------------------------------------------------- */
-/*
- * Local variables:
- * c-basic-offset: 8
- * End:
- */
diff --git a/drivers/media/pci/saa7134/saa7134-video.c b/drivers/media/pci/saa7134/saa7134-video.c
index eb472b5b26a..d3759998076 100644
--- a/drivers/media/pci/saa7134/saa7134-video.c
+++ b/drivers/media/pci/saa7134/saa7134-video.c
@@ -381,42 +381,6 @@ static struct saa7134_format* format_by_fourcc(unsigned int fourcc)
 	return NULL;
 }
 
-/* ----------------------------------------------------------------------- */
-/* resource management                                                     */
-
-static int res_get(struct saa7134_dev *dev, struct saa7134_fh *fh, unsigned int bit)
-{
-	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("res: get %d\n",bit);
-	mutex_unlock(&dev->lock);
-	return 1;
-}
-
-static
-void res_free(struct saa7134_dev *dev, struct saa7134_fh *fh, unsigned int bits)
-{
-	BUG_ON((fh->resources & bits) != bits);
-
-	mutex_lock(&dev->lock);
-	fh->resources  &= ~bits;
-	dev->resources &= ~bits;
-	dprintk("res: put %d\n",bits);
-	mutex_unlock(&dev->lock);
-}
-
 /* ------------------------------------------------------------------ */
 
 static void set_tvnorm(struct saa7134_dev *dev, struct saa7134_tvnorm *norm)
@@ -452,19 +416,26 @@ static void video_mux(struct saa7134_dev *dev, int input)
 
 static void saa7134_set_decoder(struct saa7134_dev *dev)
 {
-	int luma_control, sync_control, mux;
+	int luma_control, sync_control, chroma_ctrl1, mux;
 
 	struct saa7134_tvnorm *norm = dev->tvnorm;
 	mux = card_in(dev, dev->ctl_input).vmux;
 
 	luma_control = norm->luma_control;
 	sync_control = norm->sync_control;
+	chroma_ctrl1 = norm->chroma_ctrl1;
 
 	if (mux > 5)
 		luma_control |= 0x80; /* svideo */
 	if (noninterlaced || dev->nosignal)
 		sync_control |= 0x20;
 
+	/* switch on auto standard detection */
+	sync_control |= SAA7134_SYNC_CTRL_AUFD;
+	chroma_ctrl1 |= SAA7134_CHROMA_CTRL1_AUTO0;
+	chroma_ctrl1 &= ~SAA7134_CHROMA_CTRL1_FCTC;
+	luma_control &= ~SAA7134_LUMA_CTRL_LDEL;
+
 	/* setup video decoder */
 	saa_writeb(SAA7134_INCR_DELAY,            0x08);
 	saa_writeb(SAA7134_ANALOG_IN_CTRL1,       0xc0 | mux);
@@ -487,7 +458,7 @@ static void saa7134_set_decoder(struct saa7134_dev *dev)
 		dev->ctl_invert ? -dev->ctl_saturation : dev->ctl_saturation);
 
 	saa_writeb(SAA7134_DEC_CHROMA_HUE,        dev->ctl_hue);
-	saa_writeb(SAA7134_CHROMA_CTRL1,          norm->chroma_ctrl1);
+	saa_writeb(SAA7134_CHROMA_CTRL1,          chroma_ctrl1);
 	saa_writeb(SAA7134_CHROMA_GAIN,           norm->chroma_gain);
 
 	saa_writeb(SAA7134_CHROMA_CTRL2,          norm->chroma_ctrl2);
@@ -506,10 +477,10 @@ void saa7134_set_tvnorm_hw(struct saa7134_dev *dev)
 	saa7134_set_decoder(dev);
 
 	if (card_in(dev, dev->ctl_input).tv)
-		saa_call_all(dev, core, s_std, dev->tvnorm->id);
+		saa_call_all(dev, video, s_std, dev->tvnorm->id);
 	/* Set the correct norm for the saa6752hs. This function
 	   does nothing if there is no saa6752hs. */
-	saa_call_empress(dev, core, s_std, dev->tvnorm->id);
+	saa_call_empress(dev, video, s_std, dev->tvnorm->id);
 }
 
 static void set_h_prescale(struct saa7134_dev *dev, int task, int prescale)
@@ -817,35 +788,35 @@ static int buffer_activate(struct saa7134_dev *dev,
 			   struct saa7134_buf *buf,
 			   struct saa7134_buf *next)
 {
+	struct saa7134_dmaqueue *dmaq = buf->vb2.vb2_queue->drv_priv;
 	unsigned long base,control,bpl;
 	unsigned long bpl_uv,lines_uv,base2,base3,tmp; /* planar */
 
 	dprintk("buffer_activate buf=%p\n",buf);
-	buf->vb.state = VIDEOBUF_ACTIVE;
 	buf->top_seen = 0;
 
-	set_size(dev,TASK_A,buf->vb.width,buf->vb.height,
-		 V4L2_FIELD_HAS_BOTH(buf->vb.field));
-	if (buf->fmt->yuv)
+	set_size(dev, TASK_A, dev->width, dev->height,
+		 V4L2_FIELD_HAS_BOTH(dev->field));
+	if (dev->fmt->yuv)
 		saa_andorb(SAA7134_DATA_PATH(TASK_A), 0x3f, 0x03);
 	else
 		saa_andorb(SAA7134_DATA_PATH(TASK_A), 0x3f, 0x01);
-	saa_writeb(SAA7134_OFMT_VIDEO_A, buf->fmt->pm);
+	saa_writeb(SAA7134_OFMT_VIDEO_A, dev->fmt->pm);
 
 	/* DMA: setup channel 0 (= Video Task A0) */
 	base  = saa7134_buffer_base(buf);
-	if (buf->fmt->planar)
-		bpl = buf->vb.width;
+	if (dev->fmt->planar)
+		bpl = dev->width;
 	else
-		bpl = (buf->vb.width * buf->fmt->depth) / 8;
+		bpl = (dev->width * dev->fmt->depth) / 8;
 	control = SAA7134_RS_CONTROL_BURST_16 |
 		SAA7134_RS_CONTROL_ME |
-		(buf->pt->dma >> 12);
-	if (buf->fmt->bswap)
+		(dmaq->pt.dma >> 12);
+	if (dev->fmt->bswap)
 		control |= SAA7134_RS_CONTROL_BSWAP;
-	if (buf->fmt->wswap)
+	if (dev->fmt->wswap)
 		control |= SAA7134_RS_CONTROL_WSWAP;
-	if (V4L2_FIELD_HAS_BOTH(buf->vb.field)) {
+	if (V4L2_FIELD_HAS_BOTH(dev->field)) {
 		/* interlaced */
 		saa_writel(SAA7134_RS_BA1(0),base);
 		saa_writel(SAA7134_RS_BA2(0),base+bpl);
@@ -858,17 +829,17 @@ static int buffer_activate(struct saa7134_dev *dev,
 	}
 	saa_writel(SAA7134_RS_CONTROL(0),control);
 
-	if (buf->fmt->planar) {
+	if (dev->fmt->planar) {
 		/* DMA: setup channel 4+5 (= planar task A) */
-		bpl_uv   = bpl >> buf->fmt->hshift;
-		lines_uv = buf->vb.height >> buf->fmt->vshift;
-		base2    = base + bpl * buf->vb.height;
+		bpl_uv   = bpl >> dev->fmt->hshift;
+		lines_uv = dev->height >> dev->fmt->vshift;
+		base2    = base + bpl * dev->height;
 		base3    = base2 + bpl_uv * lines_uv;
-		if (buf->fmt->uvswap)
+		if (dev->fmt->uvswap)
 			tmp = base2, base2 = base3, base3 = tmp;
 		dprintk("uv: bpl=%ld lines=%ld base2/3=%ld/%ld\n",
 			bpl_uv,lines_uv,base2,base3);
-		if (V4L2_FIELD_HAS_BOTH(buf->vb.field)) {
+		if (V4L2_FIELD_HAS_BOTH(dev->field)) {
 			/* interlaced */
 			saa_writel(SAA7134_RS_BA1(4),base2);
 			saa_writel(SAA7134_RS_BA2(4),base2+bpl_uv);
@@ -891,22 +862,65 @@ static int buffer_activate(struct saa7134_dev *dev,
 
 	/* start DMA */
 	saa7134_set_dmabits(dev);
-	mod_timer(&dev->video_q.timeout, jiffies+BUFFER_TIMEOUT);
+	mod_timer(&dmaq->timeout, jiffies + BUFFER_TIMEOUT);
+	return 0;
+}
+
+static int buffer_init(struct vb2_buffer *vb2)
+{
+	struct saa7134_dmaqueue *dmaq = vb2->vb2_queue->drv_priv;
+	struct saa7134_buf *buf = container_of(vb2, struct saa7134_buf, vb2);
+
+	dmaq->curr = NULL;
+	buf->activate = buffer_activate;
 	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 *vb2)
 {
-	struct saa7134_dev *dev = q->priv_data;
-	struct saa7134_buf *buf = container_of(vb,struct saa7134_buf,vb);
+	struct saa7134_dmaqueue *dmaq = vb2->vb2_queue->drv_priv;
+	struct saa7134_dev *dev = dmaq->dev;
+	struct saa7134_buf *buf = container_of(vb2, struct saa7134_buf, vb2);
+	struct sg_table *dma = vb2_dma_sg_plane_desc(&buf->vb2, 0);
 	unsigned int size;
-	int err;
+	int ret;
 
-	/* sanity checks */
-	if (NULL == dev->fmt)
+	if (dma->sgl->offset) {
+		pr_err("The buffer is not page-aligned\n");
 		return -EINVAL;
+	}
+	size = (dev->width * dev->height * dev->fmt->depth) >> 3;
+	if (vb2_plane_size(vb2, 0) < size)
+		return -EINVAL;
+
+	vb2_set_plane_payload(vb2, 0, size);
+	vb2->v4l2_buf.field = dev->field;
+
+	ret = dma_map_sg(&dev->pci->dev, dma->sgl, dma->nents, DMA_FROM_DEVICE);
+	if (!ret)
+		return -EIO;
+	return saa7134_pgtable_build(dev->pci, &dmaq->pt, dma->sgl, dma->nents,
+				    saa7134_buffer_startpage(buf));
+}
+
+static void buffer_finish(struct vb2_buffer *vb2)
+{
+	struct saa7134_dmaqueue *dmaq = vb2->vb2_queue->drv_priv;
+	struct saa7134_dev *dev = dmaq->dev;
+	struct saa7134_buf *buf = container_of(vb2, struct saa7134_buf, vb2);
+	struct sg_table *dma = vb2_dma_sg_plane_desc(&buf->vb2, 0);
+
+	dma_unmap_sg(&dev->pci->dev, dma->sgl, dma->nents, DMA_FROM_DEVICE);
+}
+
+static int queue_setup(struct vb2_queue *q, const struct v4l2_format *fmt,
+			   unsigned int *nbuffers, unsigned int *nplanes,
+			   unsigned int sizes[], void *alloc_ctxs[])
+{
+	struct saa7134_dmaqueue *dmaq = q->drv_priv;
+	struct saa7134_dev *dev = dmaq->dev;
+	int size = dev->fmt->depth * dev->width * dev->height >> 3;
+
 	if (dev->width    < 48 ||
 	    dev->height   < 32 ||
 	    dev->width/4  > dev->crop_current.width  ||
@@ -914,83 +928,88 @@ static int buffer_prepare(struct videobuf_queue *q,
 	    dev->width    > dev->crop_bounds.width  ||
 	    dev->height   > dev->crop_bounds.height)
 		return -EINVAL;
-	size = (dev->width * dev->height * dev->fmt->depth) >> 3;
-	if (0 != buf->vb.baddr  &&  buf->vb.bsize < size)
-		return -EINVAL;
-
-	dprintk("buffer_prepare [%d,size=%dx%d,bytes=%d,fields=%s,%s]\n",
-		vb->i, dev->width, dev->height, size, v4l2_field_names[field],
-		dev->fmt->name);
-	if (buf->vb.width  != dev->width  ||
-	    buf->vb.height != dev->height ||
-	    buf->vb.size   != size       ||
-	    buf->vb.field  != field      ||
-	    buf->fmt       != dev->fmt) {
-		saa7134_dma_free(q,buf);
-	}
 
-	if (VIDEOBUF_NEEDS_INIT == buf->vb.state) {
-		struct videobuf_dmabuf *dma=videobuf_to_dma(&buf->vb);
-
-		buf->vb.width  = dev->width;
-		buf->vb.height = dev->height;
-		buf->vb.size   = size;
-		buf->vb.field  = field;
-		buf->fmt       = dev->fmt;
-		buf->pt        = &dev->pt_cap;
-		dev->video_q.curr = NULL;
-
-		err = videobuf_iolock(q,&buf->vb,&dev->ovbuf);
-		if (err)
-			goto oops;
-		err = saa7134_pgtable_build(dev->pci,buf->pt,
-					    dma->sglist,
-					    dma->sglen,
-					    saa7134_buffer_startpage(buf));
-		if (err)
-			goto oops;
-	}
-	buf->vb.state = VIDEOBUF_PREPARED;
-	buf->activate = buffer_activate;
+	*nbuffers = saa7134_buffer_count(size, *nbuffers);
+	*nplanes = 1;
+	sizes[0] = size;
 	return 0;
-
- oops:
-	saa7134_dma_free(q,buf);
-	return err;
 }
 
-static int
-buffer_setup(struct videobuf_queue *q, unsigned int *count, unsigned int *size)
+/*
+ * move buffer to hardware queue
+ */
+void saa7134_vb2_buffer_queue(struct vb2_buffer *vb)
 {
-	struct saa7134_dev *dev = q->priv_data;
+	struct saa7134_dmaqueue *dmaq = vb->vb2_queue->drv_priv;
+	struct saa7134_dev *dev = dmaq->dev;
+	struct saa7134_buf *buf = container_of(vb, struct saa7134_buf, vb2);
 
-	*size = dev->fmt->depth * dev->width * dev->height >> 3;
-	if (0 == *count)
-		*count = gbuffers;
-	*count = saa7134_buffer_count(*size,*count);
-	return 0;
+	saa7134_buffer_queue(dev, dmaq, buf);
 }
+EXPORT_SYMBOL_GPL(saa7134_vb2_buffer_queue);
 
-static void buffer_queue(struct videobuf_queue *q, struct videobuf_buffer *vb)
+int saa7134_vb2_start_streaming(struct vb2_queue *vq, unsigned int count)
 {
-	struct saa7134_dev *dev = q->priv_data;
-	struct saa7134_buf *buf = container_of(vb,struct saa7134_buf,vb);
+	struct saa7134_dmaqueue *dmaq = vq->drv_priv;
+	struct saa7134_dev *dev = dmaq->dev;
+
+	/*
+	 * Planar video capture and TS share the same DMA channel,
+	 * so only one can be active at a time.
+	 */
+	if (card_is_empress(dev) && vb2_is_busy(&dev->empress_vbq) &&
+	    dmaq == &dev->video_q && dev->fmt->planar) {
+		struct saa7134_buf *buf, *tmp;
 
-	saa7134_buffer_queue(dev, &dev->video_q, buf);
+		list_for_each_entry_safe(buf, tmp, &dmaq->queue, entry) {
+			list_del(&buf->entry);
+			vb2_buffer_done(&buf->vb2, VB2_BUF_STATE_QUEUED);
+		}
+		if (dmaq->curr) {
+			vb2_buffer_done(&dmaq->curr->vb2, VB2_BUF_STATE_QUEUED);
+			dmaq->curr = NULL;
+		}
+		return -EBUSY;
+	}
+
+	/* The SAA7134 has a 1K FIFO; the datasheet suggests that when
+	 * configured conservatively, there's 22 usec of buffering for video.
+	 * We therefore request a DMA latency of 20 usec, giving us 2 usec of
+	 * margin in case the FIFO is configured differently to the datasheet.
+	 * Unfortunately, I lack register-level documentation to check the
+	 * Linux FIFO setup and confirm the perfect value.
+	 */
+	if ((dmaq == &dev->video_q && !vb2_is_streaming(&dev->vbi_vbq)) ||
+	    (dmaq == &dev->vbi_q && !vb2_is_streaming(&dev->video_vbq)))
+		pm_qos_add_request(&dev->qos_request,
+			PM_QOS_CPU_DMA_LATENCY, 20);
+	dmaq->seq_nr = 0;
+
+	return 0;
 }
 
-static void buffer_release(struct videobuf_queue *q, struct videobuf_buffer *vb)
+void saa7134_vb2_stop_streaming(struct vb2_queue *vq)
 {
-	struct saa7134_buf *buf = container_of(vb,struct saa7134_buf,vb);
+	struct saa7134_dmaqueue *dmaq = vq->drv_priv;
+	struct saa7134_dev *dev = dmaq->dev;
+
+	saa7134_stop_streaming(dev, dmaq);
 
-	saa7134_dma_free(q,buf);
+	if ((dmaq == &dev->video_q && !vb2_is_streaming(&dev->vbi_vbq)) ||
+	    (dmaq == &dev->vbi_q && !vb2_is_streaming(&dev->video_vbq)))
+		pm_qos_remove_request(&dev->qos_request);
 }
 
-static struct videobuf_queue_ops video_qops = {
-	.buf_setup    = buffer_setup,
-	.buf_prepare  = buffer_prepare,
-	.buf_queue    = buffer_queue,
-	.buf_release  = buffer_release,
+static struct vb2_ops vb2_qops = {
+	.queue_setup	= queue_setup,
+	.buf_init	= buffer_init,
+	.buf_prepare	= buffer_prepare,
+	.buf_finish	= buffer_finish,
+	.buf_queue	= saa7134_vb2_buffer_queue,
+	.wait_prepare	= vb2_ops_wait_prepare,
+	.wait_finish	= vb2_ops_wait_finish,
+	.start_streaming = saa7134_vb2_start_streaming,
+	.stop_streaming = saa7134_vb2_stop_streaming,
 };
 
 /* ------------------------------------------------------------------ */
@@ -1068,7 +1087,7 @@ static int saa7134_s_ctrl(struct v4l2_ctrl *ctrl)
 	default:
 		return -EINVAL;
 	}
-	if (restart_overlay && res_locked(dev, RESOURCE_OVERLAY)) {
+	if (restart_overlay && dev->overlay_owner) {
 		spin_lock_irqsave(&dev->slock, flags);
 		stop_preview(dev);
 		start_preview(dev);
@@ -1079,182 +1098,57 @@ static int saa7134_s_ctrl(struct v4l2_ctrl *ctrl)
 
 /* ------------------------------------------------------------------ */
 
-static struct videobuf_queue *saa7134_queue(struct file *file)
+static inline struct vb2_queue *saa7134_queue(struct file *file)
 {
-	struct video_device *vdev = video_devdata(file);
-	struct saa7134_dev *dev = video_drvdata(file);
-	struct saa7134_fh *fh = file->private_data;
-	struct videobuf_queue *q = NULL;
-
-	switch (vdev->vfl_type) {
-	case VFL_TYPE_GRABBER:
-		q = fh->is_empress ? &dev->empress_tsq : &dev->cap;
-		break;
-	case VFL_TYPE_VBI:
-		q = &dev->vbi;
-		break;
-	default:
-		BUG();
-	}
-	return q;
-}
-
-static int saa7134_resource(struct file *file)
-{
-	struct video_device *vdev = video_devdata(file);
-	struct saa7134_fh *fh = file->private_data;
-
-	if (vdev->vfl_type == VFL_TYPE_GRABBER)
-		return fh->is_empress ? RESOURCE_EMPRESS : RESOURCE_VIDEO;
-
-	if (vdev->vfl_type == VFL_TYPE_VBI)
-		return RESOURCE_VBI;
-
-	BUG();
-	return 0;
+	return video_devdata(file)->queue;
 }
 
 static int video_open(struct file *file)
 {
 	struct video_device *vdev = video_devdata(file);
 	struct saa7134_dev *dev = video_drvdata(file);
-	struct saa7134_fh *fh;
-
-	/* allocate + initialize per filehandle data */
-	fh = kzalloc(sizeof(*fh),GFP_KERNEL);
-	if (NULL == fh)
-		return -ENOMEM;
+	int ret = v4l2_fh_open(file);
 
-	v4l2_fh_init(&fh->fh, vdev);
-	file->private_data = fh;
+	if (ret < 0)
+		return ret;
 
+	mutex_lock(&dev->lock);
 	if (vdev->vfl_type == VFL_TYPE_RADIO) {
 		/* switch to radio mode */
-		saa7134_tvaudio_setinput(dev,&card(dev).radio);
+		saa7134_tvaudio_setinput(dev, &card(dev).radio);
 		saa_call_all(dev, tuner, s_radio);
 	} else {
 		/* switch to video/vbi mode */
-		video_mux(dev,dev->ctl_input);
+		video_mux(dev, dev->ctl_input);
 	}
-	v4l2_fh_add(&fh->fh);
+	mutex_unlock(&dev->lock);
 
 	return 0;
 }
 
-static ssize_t
-video_read(struct file *file, char __user *data, size_t count, loff_t *ppos)
-{
-	struct video_device *vdev = video_devdata(file);
-	struct saa7134_dev *dev = video_drvdata(file);
-	struct saa7134_fh *fh = file->private_data;
-
-	switch (vdev->vfl_type) {
-	case VFL_TYPE_GRABBER:
-		if (res_locked(dev, RESOURCE_VIDEO))
-			return -EBUSY;
-		return videobuf_read_one(saa7134_queue(file),
-					 data, count, ppos,
-					 file->f_flags & O_NONBLOCK);
-	case VFL_TYPE_VBI:
-		if (!res_get(dev, fh, RESOURCE_VBI))
-			return -EBUSY;
-		return videobuf_read_stream(saa7134_queue(file),
-					    data, count, ppos, 1,
-					    file->f_flags & O_NONBLOCK);
-		break;
-	default:
-		BUG();
-		return 0;
-	}
-}
-
-static unsigned int
-video_poll(struct file *file, struct poll_table_struct *wait)
-{
-	unsigned long req_events = poll_requested_events(wait);
-	struct video_device *vdev = video_devdata(file);
-	struct saa7134_dev *dev = video_drvdata(file);
-	struct saa7134_fh *fh = file->private_data;
-	struct videobuf_buffer *buf = NULL;
-	unsigned int rc = 0;
-
-	if (v4l2_event_pending(&fh->fh))
-		rc = POLLPRI;
-	else if (req_events & POLLPRI)
-		poll_wait(file, &fh->fh.wait, wait);
-
-	if (vdev->vfl_type == VFL_TYPE_VBI)
-		return rc | videobuf_poll_stream(file, &dev->vbi, wait);
-
-	if (res_check(fh, RESOURCE_VIDEO)) {
-		mutex_lock(&dev->cap.vb_lock);
-		if (!list_empty(&dev->cap.stream))
-			buf = list_entry(dev->cap.stream.next, struct videobuf_buffer, stream);
-	} else {
-		mutex_lock(&dev->cap.vb_lock);
-		if (UNSET == dev->cap.read_off) {
-			/* need to capture a new frame */
-			if (res_locked(dev, RESOURCE_VIDEO))
-				goto err;
-			if (0 != dev->cap.ops->buf_prepare(&dev->cap,
-					dev->cap.read_buf, dev->cap.field))
-				goto err;
-			dev->cap.ops->buf_queue(&dev->cap, dev->cap.read_buf);
-			dev->cap.read_off = 0;
-		}
-		buf = dev->cap.read_buf;
-	}
-
-	if (!buf)
-		goto err;
-
-	poll_wait(file, &buf->done, wait);
-	if (buf->state == VIDEOBUF_DONE || buf->state == VIDEOBUF_ERROR)
-		rc |= POLLIN | POLLRDNORM;
-	mutex_unlock(&dev->cap.vb_lock);
-	return rc;
-
-err:
-	mutex_unlock(&dev->cap.vb_lock);
-	return rc | POLLERR;
-}
-
 static int video_release(struct file *file)
 {
 	struct video_device *vdev = video_devdata(file);
 	struct saa7134_dev *dev = video_drvdata(file);
-	struct saa7134_fh *fh = file->private_data;
+	struct v4l2_fh *fh = file->private_data;
 	struct saa6588_command cmd;
 	unsigned long flags;
 
+	mutex_lock(&dev->lock);
 	saa7134_tvaudio_close(dev);
 
 	/* turn off overlay */
-	if (res_check(fh, RESOURCE_OVERLAY)) {
+	if (fh == dev->overlay_owner) {
 		spin_lock_irqsave(&dev->slock,flags);
 		stop_preview(dev);
 		spin_unlock_irqrestore(&dev->slock,flags);
-		res_free(dev, fh, RESOURCE_OVERLAY);
-	}
-
-	/* stop video capture */
-	if (res_check(fh, RESOURCE_VIDEO)) {
-		pm_qos_remove_request(&dev->qos_request);
-		videobuf_streamoff(&dev->cap);
-		res_free(dev, fh, RESOURCE_VIDEO);
-		videobuf_mmap_free(&dev->cap);
-	}
-	if (dev->cap.read_buf) {
-		buffer_release(&dev->cap, dev->cap.read_buf);
-		kfree(dev->cap.read_buf);
+		dev->overlay_owner = NULL;
 	}
 
-	/* stop vbi capture */
-	if (res_check(fh, RESOURCE_VBI)) {
-		videobuf_stop(&dev->vbi);
-		res_free(dev, fh, RESOURCE_VBI);
-		videobuf_mmap_free(&dev->vbi);
-	}
+	if (vdev->vfl_type == VFL_TYPE_RADIO)
+		v4l2_fh_release(file);
+	else
+		_vb2_fop_release(file, NULL);
 
 	/* ts-capture will not work in planar mode, so turn it off Hac: 04.05*/
 	saa_andorb(SAA7134_OFMT_VIDEO_A, 0x1f, 0);
@@ -1265,19 +1159,11 @@ static int video_release(struct file *file)
 	saa_call_all(dev, core, s_power, 0);
 	if (vdev->vfl_type == VFL_TYPE_RADIO)
 		saa_call_all(dev, core, ioctl, SAA6588_CMD_CLOSE, &cmd);
+	mutex_unlock(&dev->lock);
 
-	v4l2_fh_del(&fh->fh);
-	v4l2_fh_exit(&fh->fh);
-	file->private_data = NULL;
-	kfree(fh);
 	return 0;
 }
 
-static int video_mmap(struct file *file, struct vm_area_struct * vma)
-{
-	return videobuf_mmap_mapper(saa7134_queue(file), vma);
-}
-
 static ssize_t radio_read(struct file *file, char __user *data,
 			 size_t count, loff_t *ppos)
 {
@@ -1290,7 +1176,9 @@ static ssize_t radio_read(struct file *file, char __user *data,
 	cmd.instance = file;
 	cmd.result = -ENODEV;
 
+	mutex_lock(&dev->lock);
 	saa_call_all(dev, core, ioctl, SAA6588_CMD_READ, &cmd);
+	mutex_unlock(&dev->lock);
 
 	return cmd.result;
 }
@@ -1304,7 +1192,9 @@ static unsigned int radio_poll(struct file *file, poll_table *wait)
 	cmd.instance = file;
 	cmd.event_list = wait;
 	cmd.result = 0;
+	mutex_lock(&dev->lock);
 	saa_call_all(dev, core, ioctl, SAA6588_CMD_POLL, &cmd);
+	mutex_unlock(&dev->lock);
 
 	return rc | cmd.result;
 }
@@ -1338,7 +1228,7 @@ static int saa7134_g_fmt_vid_cap(struct file *file, void *priv,
 
 	f->fmt.pix.width        = dev->width;
 	f->fmt.pix.height       = dev->height;
-	f->fmt.pix.field        = dev->cap.field;
+	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;
@@ -1362,7 +1252,6 @@ static int saa7134_g_fmt_vid_overlay(struct file *file, void *priv,
 		printk(KERN_ERR "V4L2_BUF_TYPE_VIDEO_OVERLAY: no_overlay\n");
 		return -EINVAL;
 	}
-	mutex_lock(&dev->lock);
 	f->fmt.win = dev->win;
 	f->fmt.win.clips = clips;
 	if (clips == NULL)
@@ -1376,7 +1265,6 @@ static int saa7134_g_fmt_vid_overlay(struct file *file, void *priv,
 					sizeof(struct v4l2_rect)))
 			err = -EFAULT;
 	}
-	mutex_unlock(&dev->lock);
 
 	return err;
 }
@@ -1457,10 +1345,10 @@ static int saa7134_s_fmt_vid_cap(struct file *file, void *priv,
 	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->cap.field = f->fmt.pix.field;
+	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;
 }
 
@@ -1481,25 +1369,20 @@ static int saa7134_s_fmt_vid_overlay(struct file *file, void *priv,
 	if (0 != err)
 		return err;
 
-	mutex_lock(&dev->lock);
-
 	dev->win    = f->fmt.win;
 	dev->nclips = f->fmt.win.clipcount;
 
 	if (copy_from_user(dev->clips, f->fmt.win.clips,
-			   sizeof(struct v4l2_clip) * dev->nclips)) {
-		mutex_unlock(&dev->lock);
+			   sizeof(struct v4l2_clip) * dev->nclips))
 		return -EFAULT;
-	}
 
-	if (res_check(priv, RESOURCE_OVERLAY)) {
+	if (priv == dev->overlay_owner) {
 		spin_lock_irqsave(&dev->slock, flags);
 		stop_preview(dev);
 		start_preview(dev);
 		spin_unlock_irqrestore(&dev->slock, flags);
 	}
 
-	mutex_unlock(&dev->lock);
 	return 0;
 }
 
@@ -1551,9 +1434,7 @@ int saa7134_s_input(struct file *file, void *priv, unsigned int i)
 		return -EINVAL;
 	if (NULL == card_in(dev, i).name)
 		return -EINVAL;
-	mutex_lock(&dev->lock);
 	video_mux(dev, i);
-	mutex_unlock(&dev->lock);
 	return 0;
 }
 EXPORT_SYMBOL_GPL(saa7134_s_input);
@@ -1563,7 +1444,6 @@ int saa7134_querycap(struct file *file, void *priv,
 {
 	struct saa7134_dev *dev = video_drvdata(file);
 	struct video_device *vdev = video_devdata(file);
-	struct saa7134_fh *fh = priv;
 	u32 radio_caps, video_caps, vbi_caps;
 
 	unsigned int tuner_type = dev->tuner_type;
@@ -1582,7 +1462,7 @@ int saa7134_querycap(struct file *file, void *priv,
 		radio_caps |= V4L2_CAP_RDS_CAPTURE;
 
 	video_caps = V4L2_CAP_VIDEO_CAPTURE;
-	if (saa7134_no_overlay <= 0 && !fh->is_empress)
+	if (saa7134_no_overlay <= 0 && !is_empress(file))
 		video_caps |= V4L2_CAP_VIDEO_OVERLAY;
 
 	vbi_caps = V4L2_CAP_VBI_CAPTURE;
@@ -1613,12 +1493,12 @@ EXPORT_SYMBOL_GPL(saa7134_querycap);
 int saa7134_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
 	struct saa7134_dev *dev = video_drvdata(file);
-	struct saa7134_fh *fh = priv;
+	struct v4l2_fh *fh = priv;
 	unsigned long flags;
 	unsigned int i;
 	v4l2_std_id fixup;
 
-	if (fh->is_empress && res_locked(dev, RESOURCE_OVERLAY)) {
+	if (is_empress(file) && dev->overlay_owner) {
 		/* Don't change the std from the mpeg device
 		   if overlay is active. */
 		return -EBUSY;
@@ -1657,8 +1537,7 @@ int saa7134_s_std(struct file *file, void *priv, v4l2_std_id id)
 
 	id = tvnorms[i].id;
 
-	mutex_lock(&dev->lock);
-	if (!fh->is_empress && res_check(fh, RESOURCE_OVERLAY)) {
+	if (!is_empress(file) && fh == dev->overlay_owner) {
 		spin_lock_irqsave(&dev->slock, flags);
 		stop_preview(dev);
 		spin_unlock_irqrestore(&dev->slock, flags);
@@ -1672,7 +1551,6 @@ int saa7134_s_std(struct file *file, void *priv, v4l2_std_id id)
 		set_tvnorm(dev, &tvnorms[i]);
 
 	saa7134_tvaudio_do_scan(dev);
-	mutex_unlock(&dev->lock);
 	return 0;
 }
 EXPORT_SYMBOL_GPL(saa7134_s_std);
@@ -1686,6 +1564,35 @@ int saa7134_g_std(struct file *file, void *priv, v4l2_std_id *id)
 }
 EXPORT_SYMBOL_GPL(saa7134_g_std);
 
+static v4l2_std_id saa7134_read_std(struct saa7134_dev *dev)
+{
+	static v4l2_std_id stds[] = {
+		V4L2_STD_UNKNOWN,
+		V4L2_STD_NTSC,
+		V4L2_STD_PAL,
+		V4L2_STD_SECAM };
+
+	v4l2_std_id result = 0;
+
+	u8 st1 = saa_readb(SAA7134_STATUS_VIDEO1);
+	u8 st2 = saa_readb(SAA7134_STATUS_VIDEO2);
+
+	if (!(st2 & 0x1)) /* RDCAP == 0 */
+		result = V4L2_STD_UNKNOWN;
+	else
+		result = stds[st1 & 0x03];
+
+	return result;
+}
+
+int saa7134_querystd(struct file *file, void *priv, v4l2_std_id *std)
+{
+	struct saa7134_dev *dev = video_drvdata(file);
+	*std &= saa7134_read_std(dev);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(saa7134_querystd);
+
 static int saa7134_cropcap(struct file *file, void *priv,
 					struct v4l2_cropcap *cap)
 {
@@ -1730,9 +1637,9 @@ static int saa7134_s_crop(struct file *file, void *f, const struct v4l2_crop *cr
 	    crop->type != V4L2_BUF_TYPE_VIDEO_OVERLAY)
 		return -EINVAL;
 
-	if (res_locked(dev, RESOURCE_OVERLAY))
+	if (dev->overlay_owner)
 		return -EBUSY;
-	if (res_locked(dev, RESOURCE_VIDEO))
+	if (vb2_is_streaming(&dev->video_vbq))
 		return -EBUSY;
 
 	*c = crop->c;
@@ -1826,12 +1733,10 @@ int saa7134_s_frequency(struct file *file, void *priv,
 
 	if (0 != f->tuner)
 		return -EINVAL;
-	mutex_lock(&dev->lock);
 
 	saa_call_all(dev, tuner, s_frequency, f);
 
 	saa7134_tvaudio_do_scan(dev);
-	mutex_unlock(&dev->lock);
 	return 0;
 }
 EXPORT_SYMBOL_GPL(saa7134_s_frequency);
@@ -1915,92 +1820,24 @@ static int saa7134_overlay(struct file *file, void *priv, unsigned int on)
 			return -EINVAL;
 		}
 
-		if (!res_get(dev, priv, RESOURCE_OVERLAY))
+		if (dev->overlay_owner && priv != dev->overlay_owner)
 			return -EBUSY;
+		dev->overlay_owner = priv;
 		spin_lock_irqsave(&dev->slock, flags);
 		start_preview(dev);
 		spin_unlock_irqrestore(&dev->slock, flags);
 	}
 	if (!on) {
-		if (!res_check(priv, RESOURCE_OVERLAY))
+		if (priv != dev->overlay_owner)
 			return -EINVAL;
 		spin_lock_irqsave(&dev->slock, flags);
 		stop_preview(dev);
 		spin_unlock_irqrestore(&dev->slock, flags);
-		res_free(dev, priv, RESOURCE_OVERLAY);
+		dev->overlay_owner = NULL;
 	}
 	return 0;
 }
 
-int saa7134_reqbufs(struct file *file, void *priv,
-					struct v4l2_requestbuffers *p)
-{
-	return videobuf_reqbufs(saa7134_queue(file), p);
-}
-EXPORT_SYMBOL_GPL(saa7134_reqbufs);
-
-int saa7134_querybuf(struct file *file, void *priv,
-					struct v4l2_buffer *b)
-{
-	return videobuf_querybuf(saa7134_queue(file), b);
-}
-EXPORT_SYMBOL_GPL(saa7134_querybuf);
-
-int saa7134_qbuf(struct file *file, void *priv, struct v4l2_buffer *b)
-{
-	return videobuf_qbuf(saa7134_queue(file), b);
-}
-EXPORT_SYMBOL_GPL(saa7134_qbuf);
-
-int saa7134_dqbuf(struct file *file, void *priv, struct v4l2_buffer *b)
-{
-	return videobuf_dqbuf(saa7134_queue(file), b,
-				file->f_flags & O_NONBLOCK);
-}
-EXPORT_SYMBOL_GPL(saa7134_dqbuf);
-
-int saa7134_streamon(struct file *file, void *priv,
-					enum v4l2_buf_type type)
-{
-	struct saa7134_dev *dev = video_drvdata(file);
-	int res = saa7134_resource(file);
-
-	if (!res_get(dev, priv, res))
-		return -EBUSY;
-
-	/* The SAA7134 has a 1K FIFO; the datasheet suggests that when
-	 * configured conservatively, there's 22 usec of buffering for video.
-	 * We therefore request a DMA latency of 20 usec, giving us 2 usec of
-	 * margin in case the FIFO is configured differently to the datasheet.
-	 * Unfortunately, I lack register-level documentation to check the
-	 * Linux FIFO setup and confirm the perfect value.
-	 */
-	if (res != RESOURCE_EMPRESS)
-		pm_qos_add_request(&dev->qos_request,
-			   PM_QOS_CPU_DMA_LATENCY, 20);
-
-	return videobuf_streamon(saa7134_queue(file));
-}
-EXPORT_SYMBOL_GPL(saa7134_streamon);
-
-int saa7134_streamoff(struct file *file, void *priv,
-					enum v4l2_buf_type type)
-{
-	struct saa7134_dev *dev = video_drvdata(file);
-	int err;
-	int res = saa7134_resource(file);
-
-	if (res != RESOURCE_EMPRESS)
-		pm_qos_remove_request(&dev->qos_request);
-
-	err = videobuf_streamoff(saa7134_queue(file));
-	if (err < 0)
-		return err;
-	res_free(dev, priv, res);
-	return 0;
-}
-EXPORT_SYMBOL_GPL(saa7134_streamoff);
-
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 static int vidioc_g_register (struct file *file, void *priv,
 			      struct v4l2_dbg_register *reg)
@@ -2058,10 +1895,10 @@ 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,
+	.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 = {
@@ -2078,17 +1915,18 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_try_fmt_vbi_cap		= saa7134_try_get_set_fmt_vbi_cap,
 	.vidioc_s_fmt_vbi_cap		= saa7134_try_get_set_fmt_vbi_cap,
 	.vidioc_cropcap			= saa7134_cropcap,
-	.vidioc_reqbufs			= saa7134_reqbufs,
-	.vidioc_querybuf		= saa7134_querybuf,
-	.vidioc_qbuf			= saa7134_qbuf,
-	.vidioc_dqbuf			= saa7134_dqbuf,
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
 	.vidioc_s_std			= saa7134_s_std,
 	.vidioc_g_std			= saa7134_g_std,
+	.vidioc_querystd		= saa7134_querystd,
 	.vidioc_enum_input		= saa7134_enum_input,
 	.vidioc_g_input			= saa7134_g_input,
 	.vidioc_s_input			= saa7134_s_input,
-	.vidioc_streamon		= saa7134_streamon,
-	.vidioc_streamoff		= saa7134_streamoff,
+	.vidioc_streamon		= vb2_ioctl_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
 	.vidioc_g_tuner			= saa7134_g_tuner,
 	.vidioc_s_tuner			= saa7134_s_tuner,
 	.vidioc_g_crop			= saa7134_g_crop,
@@ -2112,7 +1950,7 @@ static const struct v4l2_file_operations radio_fops = {
 	.open	  = video_open,
 	.read     = radio_read,
 	.release  = video_release,
-	.ioctl	  = video_ioctl2,
+	.unlocked_ioctl	= video_ioctl2,
 	.poll     = radio_poll,
 };
 
@@ -2190,6 +2028,8 @@ static const struct v4l2_ctrl_config saa7134_ctrl_automute = {
 int saa7134_video_init1(struct saa7134_dev *dev)
 {
 	struct v4l2_ctrl_handler *hdl = &dev->ctrl_handler;
+	struct vb2_queue *q;
+	int ret;
 
 	/* sanitycheck insmod options */
 	if (gbuffers < 2 || gbuffers > VIDEO_MAX_FRAME)
@@ -2241,6 +2081,7 @@ int saa7134_video_init1(struct saa7134_dev *dev)
 	dev->fmt = format_by_fourcc(V4L2_PIX_FMT_BGR24);
 	dev->width    = 720;
 	dev->height   = 576;
+	dev->field = V4L2_FIELD_INTERLACED;
 	dev->win.w.width = dev->width;
 	dev->win.w.height = dev->height;
 	dev->win.field = V4L2_FIELD_INTERLACED;
@@ -2252,20 +2093,47 @@ int saa7134_video_init1(struct saa7134_dev *dev)
 	if (saa7134_boards[dev->board].video_out)
 		saa7134_videoport_init(dev);
 
-	videobuf_queue_sg_init(&dev->cap, &video_qops,
-			    &dev->pci->dev, &dev->slock,
-			    V4L2_BUF_TYPE_VIDEO_CAPTURE,
-			    V4L2_FIELD_INTERLACED,
-			    sizeof(struct saa7134_buf),
-			    dev, NULL);
-	videobuf_queue_sg_init(&dev->vbi, &saa7134_vbi_qops,
-			    &dev->pci->dev, &dev->slock,
-			    V4L2_BUF_TYPE_VBI_CAPTURE,
-			    V4L2_FIELD_SEQ_TB,
-			    sizeof(struct saa7134_buf),
-			    dev, NULL);
-	saa7134_pgtable_alloc(dev->pci, &dev->pt_cap);
-	saa7134_pgtable_alloc(dev->pci, &dev->pt_vbi);
+	q = &dev->video_vbq;
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	/*
+	 * Do not add VB2_USERPTR unless explicitly requested: the saa7134 DMA
+	 * engine cannot handle transfers that do not start at the beginning
+	 * of a page. A user-provided pointer can start anywhere in a page, so
+	 * USERPTR support is a no-go unless the application knows about these
+	 * limitations and has special support for this.
+	 */
+	q->io_modes = VB2_MMAP | VB2_READ;
+	if (saa7134_userptr)
+		q->io_modes |= VB2_USERPTR;
+	q->drv_priv = &dev->video_q;
+	q->ops = &vb2_qops;
+	q->gfp_flags = GFP_DMA32;
+	q->mem_ops = &vb2_dma_sg_memops;
+	q->buf_struct_size = sizeof(struct saa7134_buf);
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &dev->lock;
+	ret = vb2_queue_init(q);
+	if (ret)
+		return ret;
+	saa7134_pgtable_alloc(dev->pci, &dev->video_q.pt);
+
+	q = &dev->vbi_vbq;
+	q->type = V4L2_BUF_TYPE_VBI_CAPTURE;
+	/* Don't add VB2_USERPTR, see comment above */
+	q->io_modes = VB2_MMAP | VB2_READ;
+	if (saa7134_userptr)
+		q->io_modes |= VB2_USERPTR;
+	q->drv_priv = &dev->vbi_q;
+	q->ops = &saa7134_vbi_qops;
+	q->gfp_flags = GFP_DMA32;
+	q->mem_ops = &vb2_dma_sg_memops;
+	q->buf_struct_size = sizeof(struct saa7134_buf);
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &dev->lock;
+	ret = vb2_queue_init(q);
+	if (ret)
+		return ret;
+	saa7134_pgtable_alloc(dev->pci, &dev->vbi_q.pt);
 
 	return 0;
 }
@@ -2273,8 +2141,10 @@ int saa7134_video_init1(struct saa7134_dev *dev)
 void saa7134_video_fini(struct saa7134_dev *dev)
 {
 	/* free stuff */
-	saa7134_pgtable_free(dev->pci, &dev->pt_cap);
-	saa7134_pgtable_free(dev->pci, &dev->pt_vbi);
+	vb2_queue_release(&dev->video_vbq);
+	saa7134_pgtable_free(dev->pci, &dev->video_q.pt);
+	vb2_queue_release(&dev->vbi_vbq);
+	saa7134_pgtable_free(dev->pci, &dev->vbi_q.pt);
 	v4l2_ctrl_handler_free(&dev->ctrl_handler);
 	if (card_has_radio(dev))
 		v4l2_ctrl_handler_free(&dev->radio_ctrl_handler);
@@ -2367,8 +2237,7 @@ void saa7134_irq_video_done(struct saa7134_dev *dev, unsigned long status)
 
 	spin_lock(&dev->slock);
 	if (dev->video_q.curr) {
-		dev->video_fieldcount++;
-		field = dev->video_q.curr->vb.field;
+		field = dev->field;
 		if (V4L2_FIELD_HAS_BOTH(field)) {
 			/* make sure we have seen both fields */
 			if ((status & 0x10) == 0x00) {
@@ -2384,18 +2253,10 @@ void saa7134_irq_video_done(struct saa7134_dev *dev, unsigned long status)
 			if ((status & 0x10) != 0x00)
 				goto done;
 		}
-		dev->video_q.curr->vb.field_count = dev->video_fieldcount;
-		saa7134_buffer_finish(dev,&dev->video_q,VIDEOBUF_DONE);
+		saa7134_buffer_finish(dev, &dev->video_q, VB2_BUF_STATE_DONE);
 	}
-	saa7134_buffer_next(dev,&dev->video_q);
+	saa7134_buffer_next(dev, &dev->video_q);
 
  done:
 	spin_unlock(&dev->slock);
 }
-
-/* ----------------------------------------------------------- */
-/*
- * Local variables:
- * c-basic-offset: 8
- * End:
- */
diff --git a/drivers/media/pci/saa7134/saa7134.h b/drivers/media/pci/saa7134/saa7134.h
index 2474e848f2c..e47edd4b57c 100644
--- a/drivers/media/pci/saa7134/saa7134.h
+++ b/drivers/media/pci/saa7134/saa7134.h
@@ -41,11 +41,11 @@
 #include <media/tuner.h>
 #include <media/rc-core.h>
 #include <media/ir-kbd-i2c.h>
-#include <media/videobuf-dma-sg.h>
+#include <media/videobuf2-dma-sg.h>
 #include <sound/core.h>
 #include <sound/pcm.h>
 #if IS_ENABLED(CONFIG_VIDEO_SAA7134_DVB)
-#include <media/videobuf-dvb.h>
+#include <media/videobuf2-dvb.h>
 #endif
 #include "tda8290.h"
 
@@ -453,17 +453,15 @@ struct saa7134_thread {
 /* buffer for one video/vbi/ts frame */
 struct saa7134_buf {
 	/* common v4l buffer stuff -- must be first */
-	struct videobuf_buffer vb;
+	struct vb2_buffer vb2;
 
 	/* saa7134 specific */
-	struct saa7134_format   *fmt;
 	unsigned int            top_seen;
 	int (*activate)(struct saa7134_dev *dev,
 			struct saa7134_buf *buf,
 			struct saa7134_buf *next);
 
-	/* page tables */
-	struct saa7134_pgtable  *pt;
+	struct list_head	entry;
 };
 
 struct saa7134_dmaqueue {
@@ -472,13 +470,8 @@ struct saa7134_dmaqueue {
 	struct list_head           queue;
 	struct timer_list          timeout;
 	unsigned int               need_two;
-};
-
-/* video filehandle status */
-struct saa7134_fh {
-	struct v4l2_fh             fh;
-	bool			   is_empress;
-	unsigned int               resources;
+	unsigned int               seq_nr;
+	struct saa7134_pgtable     pt;
 };
 
 /* dmasound dsp status */
@@ -504,7 +497,10 @@ struct saa7134_dmasound {
 	unsigned int               blksize;
 	unsigned int               bufsize;
 	struct saa7134_pgtable     pt;
-	struct videobuf_dmabuf     dma;
+	void			   *vaddr;
+	struct scatterlist	   *sglist;
+	int                        sglen;
+	int                        nr_pages;
 	unsigned int               dma_blk;
 	unsigned int               read_offset;
 	unsigned int               read_count;
@@ -515,7 +511,6 @@ struct saa7134_dmasound {
 /* ts/mpeg status */
 struct saa7134_ts {
 	/* TS capture */
-	struct saa7134_pgtable     pt_ts;
 	int                        nr_packets;
 	int                        nr_bufs;
 };
@@ -584,21 +579,35 @@ struct saa7134_dev {
 	struct v4l2_window         win;
 	struct v4l2_clip           clips[8];
 	unsigned int               nclips;
+	struct v4l2_fh		   *overlay_owner;
 
 
 	/* video+ts+vbi capture */
 	struct saa7134_dmaqueue    video_q;
-	struct videobuf_queue      cap;
-	struct saa7134_pgtable     pt_cap;
+	struct vb2_queue           video_vbq;
 	struct saa7134_dmaqueue    vbi_q;
-	struct videobuf_queue      vbi;
-	struct saa7134_pgtable     pt_vbi;
-	unsigned int               video_fieldcount;
-	unsigned int               vbi_fieldcount;
+	struct vb2_queue           vbi_vbq;
+	enum v4l2_field		   field;
 	struct saa7134_format      *fmt;
 	unsigned int               width, height;
+	unsigned int               vbi_hlen, vbi_vlen;
 	struct pm_qos_request	   qos_request;
 
+	/* SAA7134_MPEG_* */
+	struct saa7134_ts          ts;
+	struct saa7134_dmaqueue    ts_q;
+	enum v4l2_field		   ts_field;
+	int                        ts_started;
+	struct saa7134_mpeg_ops    *mops;
+
+	/* SAA7134_MPEG_EMPRESS only */
+	struct video_device        *empress_dev;
+	struct v4l2_subdev	   *empress_sd;
+	struct vb2_queue           empress_vbq;
+	struct work_struct         empress_workqueue;
+	int                        empress_started;
+	struct v4l2_ctrl_handler   empress_ctrl_handler;
+
 	/* various v4l controls */
 	struct saa7134_tvnorm      *tvnorm;              /* video */
 	struct saa7134_tvaudio     *tvaudio;
@@ -635,23 +644,9 @@ struct saa7134_dev {
 	/* I2C keyboard data */
 	struct IR_i2c_init_data    init_data;
 
-	/* SAA7134_MPEG_* */
-	struct saa7134_ts          ts;
-	struct saa7134_dmaqueue    ts_q;
-	int                        ts_started;
-	struct saa7134_mpeg_ops    *mops;
-
-	/* SAA7134_MPEG_EMPRESS only */
-	struct video_device        *empress_dev;
-	struct v4l2_subdev	   *empress_sd;
-	struct videobuf_queue      empress_tsq;
-	struct work_struct         empress_workqueue;
-	int                        empress_started;
-	struct v4l2_ctrl_handler   empress_ctrl_handler;
-
 #if IS_ENABLED(CONFIG_VIDEO_SAA7134_DVB)
 	/* SAA7134_MPEG_DVB only */
-	struct videobuf_dvb_frontends frontends;
+	struct vb2_dvb_frontends frontends;
 	int (*original_demod_sleep)(struct dvb_frontend *fe);
 	int (*original_set_voltage)(struct dvb_frontend *fe, fe_sec_voltage_t voltage);
 	int (*original_set_high_voltage)(struct dvb_frontend *fe, long arg);
@@ -705,14 +700,12 @@ struct saa7134_dev {
 	_rc;								\
 })
 
-static inline int res_check(struct saa7134_fh *fh, unsigned int bit)
+static inline bool is_empress(struct file *file)
 {
-	return fh->resources & bit;
-}
+	struct video_device *vdev = video_devdata(file);
+	struct saa7134_dev *dev = video_get_drvdata(vdev);
 
-static inline int res_locked(struct saa7134_dev *dev, unsigned int bit)
-{
-	return dev->resources & bit;
+	return vdev->queue == &dev->empress_vbq;
 }
 
 /* ----------------------------------------------------------- */
@@ -721,6 +714,7 @@ static inline int res_locked(struct saa7134_dev *dev, unsigned int bit)
 extern struct list_head  saa7134_devlist;
 extern struct mutex saa7134_devlist_lock;
 extern int saa7134_no_overlay;
+extern bool saa7134_userptr;
 
 void saa7134_track_gpio(struct saa7134_dev *dev, char *msg);
 void saa7134_set_gpio(struct saa7134_dev *dev, int bit_no, int value);
@@ -743,7 +737,7 @@ void saa7134_buffer_finish(struct saa7134_dev *dev, struct saa7134_dmaqueue *q,
 			   unsigned int state);
 void saa7134_buffer_next(struct saa7134_dev *dev, struct saa7134_dmaqueue *q);
 void saa7134_buffer_timeout(unsigned long data);
-void saa7134_dma_free(struct videobuf_queue *q,struct saa7134_buf *buf);
+void saa7134_stop_streaming(struct saa7134_dev *dev, struct saa7134_dmaqueue *q);
 
 int saa7134_set_dmabits(struct saa7134_dev *dev);
 
@@ -777,8 +771,13 @@ extern unsigned int video_debug;
 extern struct video_device saa7134_video_template;
 extern struct video_device saa7134_radio_template;
 
+void saa7134_vb2_buffer_queue(struct vb2_buffer *vb);
+int saa7134_vb2_start_streaming(struct vb2_queue *vq, unsigned int count);
+void saa7134_vb2_stop_streaming(struct vb2_queue *vq);
+
 int saa7134_s_std(struct file *file, void *priv, v4l2_std_id id);
 int saa7134_g_std(struct file *file, void *priv, v4l2_std_id *id);
+int saa7134_querystd(struct file *file, void *priv, v4l2_std_id *std);
 int saa7134_enum_input(struct file *file, void *priv, struct v4l2_input *i);
 int saa7134_g_input(struct file *file, void *priv, unsigned int *i);
 int saa7134_s_input(struct file *file, void *priv, unsigned int i);
@@ -792,16 +791,6 @@ int saa7134_g_frequency(struct file *file, void *priv,
 					struct v4l2_frequency *f);
 int saa7134_s_frequency(struct file *file, void *priv,
 					const struct v4l2_frequency *f);
-int saa7134_reqbufs(struct file *file, void *priv,
-					struct v4l2_requestbuffers *p);
-int saa7134_querybuf(struct file *file, void *priv,
-					struct v4l2_buffer *b);
-int saa7134_qbuf(struct file *file, void *priv, struct v4l2_buffer *b);
-int saa7134_dqbuf(struct file *file, void *priv, struct v4l2_buffer *b);
-int saa7134_streamon(struct file *file, void *priv,
-					enum v4l2_buf_type type);
-int saa7134_streamoff(struct file *file, void *priv,
-					enum v4l2_buf_type type);
 
 int saa7134_videoport_init(struct saa7134_dev *dev);
 void saa7134_set_tvnorm_hw(struct saa7134_dev *dev);
@@ -818,7 +807,16 @@ void saa7134_video_fini(struct saa7134_dev *dev);
 
 #define TS_PACKET_SIZE 188 /* TS packets 188 bytes */
 
-extern struct videobuf_queue_ops saa7134_ts_qops;
+int saa7134_ts_buffer_init(struct vb2_buffer *vb2);
+int saa7134_ts_buffer_prepare(struct vb2_buffer *vb2);
+void saa7134_ts_buffer_finish(struct vb2_buffer *vb2);
+int saa7134_ts_queue_setup(struct vb2_queue *q, const struct v4l2_format *fmt,
+			   unsigned int *nbuffers, unsigned int *nplanes,
+			   unsigned int sizes[], void *alloc_ctxs[]);
+int saa7134_ts_start_streaming(struct vb2_queue *vq, unsigned int count);
+void saa7134_ts_stop_streaming(struct vb2_queue *vq);
+
+extern struct vb2_ops saa7134_ts_qops;
 
 int saa7134_ts_init1(struct saa7134_dev *dev);
 int saa7134_ts_fini(struct saa7134_dev *dev);
@@ -835,7 +833,7 @@ int saa7134_ts_stop(struct saa7134_dev *dev);
 /* ----------------------------------------------------------- */
 /* saa7134-vbi.c                                               */
 
-extern struct videobuf_queue_ops saa7134_vbi_qops;
+extern struct vb2_ops saa7134_vbi_qops;
 extern struct video_device saa7134_vbi_template;
 
 int saa7134_vbi_init1(struct saa7134_dev *dev);
diff --git a/drivers/media/pci/saa7146/mxb.c b/drivers/media/pci/saa7146/mxb.c
index 33abe332d17..c4c8fce8f2b 100644
--- a/drivers/media/pci/saa7146/mxb.c
+++ b/drivers/media/pci/saa7146/mxb.c
@@ -357,7 +357,7 @@ static int mxb_init_done(struct saa7146_dev* dev)
 	tea6420_route(mxb, 6);
 
 	/* select video mode in saa7111a */
-	saa7111a_call(mxb, core, s_std, std);
+	saa7111a_call(mxb, video, s_std, std);
 
 	/* select tuner-output on saa7111a */
 	i = 0;
@@ -379,8 +379,8 @@ static int mxb_init_done(struct saa7146_dev* dev)
 	/* These two gpio calls set the GPIO pins that control the tda9820 */
 	saa7146_write(dev, GPIO_CTRL, 0x00404050);
 	saa7111a_call(mxb, core, s_gpio, 1);
-	saa7111a_call(mxb, core, s_std, std);
-	tuner_call(mxb, core, s_std, std);
+	saa7111a_call(mxb, video, s_std, std);
+	tuner_call(mxb, video, s_std, std);
 
 	/* switch to tuner-channel on tea6415c */
 	tea6415c_call(mxb, video, s_routing, 3, 17, 0);
@@ -771,9 +771,9 @@ static int std_callback(struct saa7146_dev *dev, struct saa7146_standard *standa
 		/* These two gpio calls set the GPIO pins that control the tda9820 */
 		saa7146_write(dev, GPIO_CTRL, 0x00404050);
 		saa7111a_call(mxb, core, s_gpio, 0);
-		saa7111a_call(mxb, core, s_std, std);
+		saa7111a_call(mxb, video, s_std, std);
 		if (mxb->cur_input == 0)
-			tuner_call(mxb, core, s_std, std);
+			tuner_call(mxb, video, s_std, std);
 	} else {
 		v4l2_std_id std = V4L2_STD_PAL_BG;
 
@@ -783,9 +783,9 @@ static int std_callback(struct saa7146_dev *dev, struct saa7146_standard *standa
 		/* These two gpio calls set the GPIO pins that control the tda9820 */
 		saa7146_write(dev, GPIO_CTRL, 0x00404050);
 		saa7111a_call(mxb, core, s_gpio, 1);
-		saa7111a_call(mxb, core, s_std, std);
+		saa7111a_call(mxb, video, s_std, std);
 		if (mxb->cur_input == 0)
-			tuner_call(mxb, core, s_std, std);
+			tuner_call(mxb, video, s_std, std);
 	}
 	return 0;
 }
diff --git a/drivers/media/pci/sta2x11/sta2x11_vip.c b/drivers/media/pci/sta2x11/sta2x11_vip.c
index bb11443ed63..d2abd3b5c2b 100644
--- a/drivers/media/pci/sta2x11/sta2x11_vip.c
+++ b/drivers/media/pci/sta2x11/sta2x11_vip.c
@@ -357,7 +357,7 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 }
 
 /* abort streaming and wait for last buffer */
-static int stop_streaming(struct vb2_queue *vq)
+static void stop_streaming(struct vb2_queue *vq)
 {
 	struct sta2x11_vip *vip = vb2_get_drv_priv(vq);
 	struct vip_buffer *vip_buf, *node;
@@ -374,7 +374,6 @@ static int stop_streaming(struct vb2_queue *vq)
 		list_del(&vip_buf->list);
 	}
 	spin_unlock(&vip->lock);
-	return 0;
 }
 
 static struct vb2_ops vip_video_qops = {
@@ -445,7 +444,7 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id std)
 	int status;
 
 	if (V4L2_STD_ALL == std) {
-		v4l2_subdev_call(vip->decoder, core, s_std, std);
+		v4l2_subdev_call(vip->decoder, video, s_std, std);
 		ssleep(2);
 		v4l2_subdev_call(vip->decoder, video, querystd, &newstd);
 		v4l2_subdev_call(vip->decoder, video, g_input_status, &status);
@@ -468,7 +467,7 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id std)
 			vip->format = formats_50[0];
 	}
 
-	return v4l2_subdev_call(vip->decoder, core, s_std, std);
+	return v4l2_subdev_call(vip->decoder, video, s_std, std);
 }
 
 /**
diff --git a/drivers/media/pci/ttpci/av7110_av.c b/drivers/media/pci/ttpci/av7110_av.c
index 301029ca453..9544cfc0660 100644
--- a/drivers/media/pci/ttpci/av7110_av.c
+++ b/drivers/media/pci/ttpci/av7110_av.c
@@ -958,8 +958,10 @@ static unsigned int dvb_video_poll(struct file *file, poll_table *wait)
 		if (av7110->playing) {
 			if (FREE_COND)
 				mask |= (POLLOUT | POLLWRNORM);
-			} else /* if not playing: may play if asked for */
-				mask |= (POLLOUT | POLLWRNORM);
+		} else {
+			/* if not playing: may play if asked for */
+			mask |= (POLLOUT | POLLWRNORM);
+		}
 	}
 
 	return mask;
diff --git a/drivers/media/pci/zoran/zoran_device.c b/drivers/media/pci/zoran/zoran_device.c
index 519164c572c..bf34b93f23e 100644
--- a/drivers/media/pci/zoran/zoran_device.c
+++ b/drivers/media/pci/zoran/zoran_device.c
@@ -1572,7 +1572,7 @@ zoran_init_hardware (struct zoran *zr)
 	}
 
 	decoder_call(zr, core, init, 0);
-	decoder_call(zr, core, s_std, zr->norm);
+	decoder_call(zr, video, s_std, zr->norm);
 	decoder_call(zr, video, s_routing,
 		zr->card.input[zr->input].muxsel, 0, 0);
 
diff --git a/drivers/media/pci/zoran/zoran_driver.c b/drivers/media/pci/zoran/zoran_driver.c
index e7e9840c6c3..099d5fbebb7 100644
--- a/drivers/media/pci/zoran/zoran_driver.c
+++ b/drivers/media/pci/zoran/zoran_driver.c
@@ -1469,7 +1469,7 @@ zoran_set_norm (struct zoran *zr,
 	if (on)
 		zr36057_overlay(zr, 0);
 
-	decoder_call(zr, core, s_std, norm);
+	decoder_call(zr, video, s_std, norm);
 	encoder_call(zr, video, s_std_output, norm);
 
 	if (on)
diff --git a/drivers/media/platform/blackfin/bfin_capture.c b/drivers/media/platform/blackfin/bfin_capture.c
index 200bec91182..16e4b1c525c 100644
--- a/drivers/media/platform/blackfin/bfin_capture.c
+++ b/drivers/media/platform/blackfin/bfin_capture.c
@@ -427,15 +427,12 @@ static int bcap_start_streaming(struct vb2_queue *vq, unsigned int count)
 	return 0;
 }
 
-static int bcap_stop_streaming(struct vb2_queue *vq)
+static void bcap_stop_streaming(struct vb2_queue *vq)
 {
 	struct bcap_device *bcap_dev = vb2_get_drv_priv(vq);
 	struct ppi_if *ppi = bcap_dev->ppi;
 	int ret;
 
-	if (!vb2_is_streaming(vq))
-		return 0;
-
 	bcap_dev->stop = true;
 	wait_for_completion(&bcap_dev->comp);
 	ppi->ops->stop(ppi);
@@ -452,7 +449,6 @@ static int bcap_stop_streaming(struct vb2_queue *vq)
 		list_del(&bcap_dev->cur_frm->list);
 		vb2_buffer_done(&bcap_dev->cur_frm->vb, VB2_BUF_STATE_ERROR);
 	}
-	return 0;
 }
 
 static struct vb2_ops bcap_video_qops = {
@@ -635,7 +631,7 @@ static int bcap_s_std(struct file *file, void *priv, v4l2_std_id std)
 	if (vb2_is_busy(&bcap_dev->buffer_queue))
 		return -EBUSY;
 
-	ret = v4l2_subdev_call(bcap_dev->sd, core, s_std, std);
+	ret = v4l2_subdev_call(bcap_dev->sd, video, s_std, std);
 	if (ret < 0)
 		return ret;
 
@@ -648,7 +644,9 @@ static int bcap_enum_dv_timings(struct file *file, void *priv,
 {
 	struct bcap_device *bcap_dev = video_drvdata(file);
 
-	return v4l2_subdev_call(bcap_dev->sd, video,
+	timings->pad = 0;
+
+	return v4l2_subdev_call(bcap_dev->sd, pad,
 			enum_dv_timings, timings);
 }
 
@@ -1069,7 +1067,7 @@ static int bcap_probe(struct platform_device *pdev)
 	/* now we can probe the default state */
 	if (config->inputs[0].capabilities & V4L2_IN_CAP_STD) {
 		v4l2_std_id std;
-		ret = v4l2_subdev_call(bcap_dev->sd, core, g_std, &std);
+		ret = v4l2_subdev_call(bcap_dev->sd, video, g_std, &std);
 		if (ret) {
 			v4l2_err(&bcap_dev->v4l2_dev,
 					"Unable to get std\n");
diff --git a/drivers/media/platform/coda.c b/drivers/media/platform/coda.c
index 3e5199ee5d2..b1783791d42 100644
--- a/drivers/media/platform/coda.c
+++ b/drivers/media/platform/coda.c
@@ -2269,7 +2269,7 @@ out:
 	return ret;
 }
 
-static int coda_stop_streaming(struct vb2_queue *q)
+static void coda_stop_streaming(struct vb2_queue *q)
 {
 	struct coda_ctx *ctx = vb2_get_drv_priv(q);
 	struct coda_dev *dev = ctx->dev;
@@ -2295,8 +2295,6 @@ static int coda_stop_streaming(struct vb2_queue *q)
 			ctx->bitstream.vaddr, ctx->bitstream.size);
 		ctx->runcounter = 0;
 	}
-
-	return 0;
 }
 
 static struct vb2_ops coda_qops = {
@@ -3235,7 +3233,7 @@ static int coda_probe(struct platform_device *pdev)
 	}
 
 	if (devm_request_threaded_irq(&pdev->dev, irq, NULL, coda_irq_handler,
-		IRQF_ONESHOT, CODA_NAME, dev) < 0) {
+		IRQF_ONESHOT, dev_name(&pdev->dev), dev) < 0) {
 		dev_err(&pdev->dev, "failed to request irq\n");
 		return -ENOENT;
 	}
diff --git a/drivers/media/platform/davinci/vpbe_display.c b/drivers/media/platform/davinci/vpbe_display.c
index b4f12d00be0..bf5eff99452 100644
--- a/drivers/media/platform/davinci/vpbe_display.c
+++ b/drivers/media/platform/davinci/vpbe_display.c
@@ -355,8 +355,17 @@ static int vpbe_start_streaming(struct vb2_queue *vq, unsigned int count)
 
 	/* Set parameters in OSD and VENC */
 	ret = vpbe_set_osd_display_params(fh->disp_dev, layer);
-	if (ret < 0)
+	if (ret < 0) {
+		struct vpbe_disp_buffer *buf, *tmp;
+
+		vb2_buffer_done(&layer->cur_frm->vb, VB2_BUF_STATE_QUEUED);
+		list_for_each_entry_safe(buf, tmp, &layer->dma_queue, list) {
+			list_del(&buf->list);
+			vb2_buffer_done(&buf->vb, VB2_BUF_STATE_QUEUED);
+		}
+
 		return ret;
+	}
 
 	/*
 	 * if request format is yuv420 semiplanar, need to
@@ -368,23 +377,36 @@ static int vpbe_start_streaming(struct vb2_queue *vq, unsigned int count)
 	return ret;
 }
 
-static int vpbe_stop_streaming(struct vb2_queue *vq)
+static void vpbe_stop_streaming(struct vb2_queue *vq)
 {
 	struct vpbe_fh *fh = vb2_get_drv_priv(vq);
 	struct vpbe_layer *layer = fh->layer;
+	struct vpbe_display *disp = fh->disp_dev;
+	unsigned long flags;
 
 	if (!vb2_is_streaming(vq))
-		return 0;
+		return;
 
 	/* release all active buffers */
+	spin_lock_irqsave(&disp->dma_queue_lock, flags);
+	if (layer->cur_frm == layer->next_frm) {
+		vb2_buffer_done(&layer->cur_frm->vb, VB2_BUF_STATE_ERROR);
+	} else {
+		if (layer->cur_frm != NULL)
+			vb2_buffer_done(&layer->cur_frm->vb,
+					VB2_BUF_STATE_ERROR);
+		if (layer->next_frm != NULL)
+			vb2_buffer_done(&layer->next_frm->vb,
+					VB2_BUF_STATE_ERROR);
+	}
+
 	while (!list_empty(&layer->dma_queue)) {
 		layer->next_frm = list_entry(layer->dma_queue.next,
 						struct vpbe_disp_buffer, list);
 		list_del(&layer->next_frm->list);
 		vb2_buffer_done(&layer->next_frm->vb, VB2_BUF_STATE_ERROR);
 	}
-
-	return 0;
+	spin_unlock_irqrestore(&disp->dma_queue_lock, flags);
 }
 
 static struct vb2_ops video_qops = {
@@ -666,29 +688,6 @@ static int vpbe_try_format(struct vpbe_display *disp_dev,
 	return 0;
 }
 
-static int vpbe_display_g_priority(struct file *file, void *priv,
-				enum v4l2_priority *p)
-{
-	struct vpbe_fh *fh = file->private_data;
-	struct vpbe_layer *layer = fh->layer;
-
-	*p = v4l2_prio_max(&layer->prio);
-
-	return 0;
-}
-
-static int vpbe_display_s_priority(struct file *file, void *priv,
-				enum v4l2_priority p)
-{
-	struct vpbe_fh *fh = file->private_data;
-	struct vpbe_layer *layer = fh->layer;
-	int ret;
-
-	ret = v4l2_prio_change(&layer->prio, &fh->prio, p);
-
-	return ret;
-}
-
 static int vpbe_display_querycap(struct file *file, void  *priv,
 			       struct v4l2_capability *cap)
 {
@@ -1478,6 +1477,7 @@ static int vpbe_display_open(struct file *file)
 {
 	struct vpbe_fh *fh = NULL;
 	struct vpbe_layer *layer = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
 	struct vpbe_display *disp_dev = layer->disp_dev;
 	struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
 	struct osd_state *osd_device = disp_dev->osd_device;
@@ -1490,6 +1490,7 @@ static int vpbe_display_open(struct file *file)
 			"unable to allocate memory for file handle object\n");
 		return -ENOMEM;
 	}
+	v4l2_fh_init(&fh->fh, vdev);
 	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
 			"vpbe display open plane = %d\n",
 			layer->device_id);
@@ -1518,9 +1519,7 @@ static int vpbe_display_open(struct file *file)
 	layer->usrs++;
 	/* Set io_allowed member to false */
 	fh->io_allowed = 0;
-	/* Initialize priority of this instance to default priority */
-	fh->prio = V4L2_PRIORITY_UNSET;
-	v4l2_prio_open(&layer->prio, &fh->prio);
+	v4l2_fh_add(&fh->fh);
 	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
 			"vpbe display device opened successfully\n");
 	return 0;
@@ -1575,8 +1574,9 @@ static int vpbe_display_release(struct file *file)
 		osd_device->ops.release_layer(osd_device,
 				layer->layer_info.id);
 	}
-	/* Close the priority */
-	v4l2_prio_close(&layer->prio, fh->prio);
+
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
 	file->private_data = NULL;
 	mutex_unlock(&layer->opslock);
 
@@ -1604,8 +1604,6 @@ static const struct v4l2_ioctl_ops vpbe_ioctl_ops = {
 	.vidioc_cropcap		 = vpbe_display_cropcap,
 	.vidioc_g_crop		 = vpbe_display_g_crop,
 	.vidioc_s_crop		 = vpbe_display_s_crop,
-	.vidioc_g_priority	 = vpbe_display_g_priority,
-	.vidioc_s_priority	 = vpbe_display_s_priority,
 	.vidioc_s_std		 = vpbe_display_s_std,
 	.vidioc_g_std		 = vpbe_display_g_std,
 	.vidioc_enum_output	 = vpbe_display_enum_output,
@@ -1685,8 +1683,6 @@ static int init_vpbe_layer(int i, struct vpbe_display *disp_dev,
 	vpbe_display_layer->layer_info.id =
 		((i == VPBE_DISPLAY_DEVICE_0) ? WIN_VID0 : WIN_VID1);
 
-	/* Initialize prio member of layer object */
-	v4l2_prio_init(&vpbe_display_layer->prio);
 
 	return 0;
 }
@@ -1713,6 +1709,7 @@ static int register_device(struct vpbe_layer *vpbe_display_layer,
 	vpbe_display_layer->disp_dev = disp_dev;
 	/* set the driver data in platform device */
 	platform_set_drvdata(pdev, disp_dev);
+	set_bit(V4L2_FL_USE_FH_PRIO, &vpbe_display_layer->video_dev.flags);
 	video_set_drvdata(&vpbe_display_layer->video_dev,
 			  vpbe_display_layer);
 
diff --git a/drivers/media/platform/davinci/vpfe_capture.c b/drivers/media/platform/davinci/vpfe_capture.c
index d762246eabf..a51bda2fb63 100644
--- a/drivers/media/platform/davinci/vpfe_capture.c
+++ b/drivers/media/platform/davinci/vpfe_capture.c
@@ -498,6 +498,7 @@ unlock:
 static int vpfe_open(struct file *file)
 {
 	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
 	struct vpfe_fh *fh;
 
 	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_open\n");
@@ -517,6 +518,7 @@ static int vpfe_open(struct file *file)
 	/* store pointer to fh in private_data member of file */
 	file->private_data = fh;
 	fh->vpfe_dev = vpfe_dev;
+	v4l2_fh_init(&fh->fh, vdev);
 	mutex_lock(&vpfe_dev->lock);
 	/* If decoder is not initialized. initialize it */
 	if (!vpfe_dev->initialized) {
@@ -529,9 +531,7 @@ static int vpfe_open(struct file *file)
 	vpfe_dev->usrs++;
 	/* Set io_allowed member to false */
 	fh->io_allowed = 0;
-	/* Initialize priority of this instance to default priority */
-	fh->prio = V4L2_PRIORITY_UNSET;
-	v4l2_prio_open(&vpfe_dev->prio, &fh->prio);
+	v4l2_fh_add(&fh->fh);
 	mutex_unlock(&vpfe_dev->lock);
 	return 0;
 }
@@ -734,12 +734,14 @@ static int vpfe_release(struct file *file)
 		}
 		vpfe_dev->io_usrs = 0;
 		vpfe_dev->numbuffers = config_params.numbuffers;
+		videobuf_stop(&vpfe_dev->buffer_queue);
+		videobuf_mmap_free(&vpfe_dev->buffer_queue);
 	}
 
 	/* Decrement device usrs counter */
 	vpfe_dev->usrs--;
-	/* Close the priority */
-	v4l2_prio_close(&vpfe_dev->prio, fh->prio);
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
 	/* If this is the last file handle */
 	if (!vpfe_dev->usrs) {
 		vpfe_dev->initialized = 0;
@@ -1215,7 +1217,7 @@ static int vpfe_s_std(struct file *file, void *priv, v4l2_std_id std_id)
 	}
 
 	ret = v4l2_device_call_until_err(&vpfe_dev->v4l2_dev, sdinfo->grp_id,
-					 core, s_std, std_id);
+					 video, s_std, std_id);
 	if (ret < 0) {
 		v4l2_err(&vpfe_dev->v4l2_dev, "Failed to set standard\n");
 		goto unlock_out;
@@ -1908,14 +1910,13 @@ static int vpfe_probe(struct platform_device *pdev)
 	/* Initialize field of the device objects */
 	vpfe_dev->numbuffers = config_params.numbuffers;
 
-	/* Initialize prio member of device object */
-	v4l2_prio_init(&vpfe_dev->prio);
 	/* register video device */
 	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);
 	vpfe_dev->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	set_bit(V4L2_FL_USE_FH_PRIO, &vpfe_dev->video_dev->flags);
 	ret = video_register_device(vpfe_dev->video_dev,
 				    VFL_TYPE_GRABBER, -1);
 
diff --git a/drivers/media/platform/davinci/vpif_capture.c b/drivers/media/platform/davinci/vpif_capture.c
index 756da78bac2..a7ed1649790 100644
--- a/drivers/media/platform/davinci/vpif_capture.c
+++ b/drivers/media/platform/davinci/vpif_capture.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (C) 2009 Texas Instruments Inc
+ * Copyright (C) 2014 Lad, Prabhakar <prabhakar.csengg@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
@@ -65,14 +66,26 @@ static struct vpif_config_params config_params = {
 	.channel_bufsize[1] = 720 * 576 * 2,
 };
 
+#define VPIF_DRIVER_NAME	"vpif_capture"
+
 /* global variables */
 static struct vpif_device vpif_obj = { {NULL} };
 static struct device *vpif_dev;
 static void vpif_calculate_offsets(struct channel_obj *ch);
 static void vpif_config_addr(struct channel_obj *ch, int muxmode);
 
+static u8 channel_first_int[VPIF_NUMBER_OF_OBJECTS][2] = { {1, 1} };
+
+/* Is set to 1 in case of SDTV formats, 2 in case of HDTV formats. */
+static int ycmux_mode;
+
+static inline struct vpif_cap_buffer *to_vpif_buffer(struct vb2_buffer *vb)
+{
+	return container_of(vb, struct vpif_cap_buffer, vb);
+}
+
 /**
- * buffer_prepare :  callback function for buffer prepare
+ * vpif_buffer_prepare :  callback function for buffer prepare
  * @vb: ptr to vb2_buffer
  *
  * This is the callback function for buffer prepare when vb2_qbuf()
@@ -81,10 +94,8 @@ static void vpif_config_addr(struct channel_obj *ch, int muxmode);
  */
 static int vpif_buffer_prepare(struct vb2_buffer *vb)
 {
-	/* Get the file handle object and channel object */
-	struct vpif_fh *fh = vb2_get_drv_priv(vb->vb2_queue);
 	struct vb2_queue *q = vb->vb2_queue;
-	struct channel_obj *ch = fh->channel;
+	struct channel_obj *ch = vb2_get_drv_priv(q);
 	struct common_obj *common;
 	unsigned long addr;
 
@@ -92,26 +103,22 @@ static int vpif_buffer_prepare(struct vb2_buffer *vb)
 
 	common = &ch->common[VPIF_VIDEO_INDEX];
 
-	if (vb->state != VB2_BUF_STATE_ACTIVE &&
-		vb->state != VB2_BUF_STATE_PREPARED) {
-		vb2_set_plane_payload(vb, 0, common->fmt.fmt.pix.sizeimage);
-		if (vb2_plane_vaddr(vb, 0) &&
-		vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0))
-			goto exit;
-		addr = vb2_dma_contig_plane_dma_addr(vb, 0);
+	vb2_set_plane_payload(vb, 0, common->fmt.fmt.pix.sizeimage);
+	if (vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0))
+		return -EINVAL;
 
-		if (q->streaming) {
-			if (!IS_ALIGNED((addr + common->ytop_off), 8) ||
-				!IS_ALIGNED((addr + common->ybtm_off), 8) ||
-				!IS_ALIGNED((addr + common->ctop_off), 8) ||
-				!IS_ALIGNED((addr + common->cbtm_off), 8))
-				goto exit;
-		}
+	vb->v4l2_buf.field = common->fmt.fmt.pix.field;
+
+	addr = vb2_dma_contig_plane_dma_addr(vb, 0);
+	if (!IS_ALIGNED((addr + common->ytop_off), 8) ||
+		!IS_ALIGNED((addr + common->ybtm_off), 8) ||
+		!IS_ALIGNED((addr + common->ctop_off), 8) ||
+		!IS_ALIGNED((addr + common->cbtm_off), 8)) {
+		vpif_dbg(1, debug, "offset is not aligned\n");
+		return -EINVAL;
 	}
+
 	return 0;
-exit:
-	vpif_dbg(1, debug, "buffer_prepare:offset is not aligned to 8 bytes\n");
-	return -EINVAL;
 }
 
 /**
@@ -131,49 +138,26 @@ static int vpif_buffer_queue_setup(struct vb2_queue *vq,
 				unsigned int *nbuffers, unsigned int *nplanes,
 				unsigned int sizes[], void *alloc_ctxs[])
 {
-	/* Get the file handle object and channel object */
-	struct vpif_fh *fh = vb2_get_drv_priv(vq);
-	struct channel_obj *ch = fh->channel;
+	struct channel_obj *ch = vb2_get_drv_priv(vq);
 	struct common_obj *common;
-	unsigned long size;
 
 	common = &ch->common[VPIF_VIDEO_INDEX];
 
 	vpif_dbg(2, debug, "vpif_buffer_setup\n");
 
-	/* If memory type is not mmap, return */
-	if (V4L2_MEMORY_MMAP == common->memory) {
-		/* Calculate the size of the buffer */
-		size = config_params.channel_bufsize[ch->channel_id];
-		/*
-		 * Checking if the buffer size exceeds the available buffer
-		 * ycmux_mode = 0 means 1 channel mode HD and
-		 * ycmux_mode = 1 means 2 channels mode SD
-		 */
-		if (ch->vpifparams.std_info.ycmux_mode == 0) {
-			if (config_params.video_limit[ch->channel_id])
-				while (size * *nbuffers >
-					(config_params.video_limit[0]
-						+ config_params.video_limit[1]))
-					(*nbuffers)--;
-		} else {
-			if (config_params.video_limit[ch->channel_id])
-				while (size * *nbuffers >
-				config_params.video_limit[ch->channel_id])
-					(*nbuffers)--;
-		}
-
-	} else {
-		size = common->fmt.fmt.pix.sizeimage;
-	}
+	if (fmt && fmt->fmt.pix.sizeimage < common->fmt.fmt.pix.sizeimage)
+		return -EINVAL;
 
-	if (*nbuffers < config_params.min_numbuffers)
-		*nbuffers = config_params.min_numbuffers;
+	if (vq->num_buffers + *nbuffers < 3)
+		*nbuffers = 3 - vq->num_buffers;
 
 	*nplanes = 1;
-	sizes[0] = size;
+	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : common->fmt.fmt.pix.sizeimage;
 	alloc_ctxs[0] = common->alloc_ctx;
 
+	/* Calculate the offset for Y and C data in the buffer */
+	vpif_calculate_offsets(ch);
+
 	return 0;
 }
 
@@ -183,11 +167,8 @@ static int vpif_buffer_queue_setup(struct vb2_queue *vq,
  */
 static void vpif_buffer_queue(struct vb2_buffer *vb)
 {
-	/* Get the file handle object and channel object */
-	struct vpif_fh *fh = vb2_get_drv_priv(vb->vb2_queue);
-	struct channel_obj *ch = fh->channel;
-	struct vpif_cap_buffer *buf = container_of(vb,
-				struct vpif_cap_buffer, vb);
+	struct channel_obj *ch = vb2_get_drv_priv(vb->vb2_queue);
+	struct vpif_cap_buffer *buf = to_vpif_buffer(vb);
 	struct common_obj *common;
 	unsigned long flags;
 
@@ -202,102 +183,25 @@ static void vpif_buffer_queue(struct vb2_buffer *vb)
 }
 
 /**
- * vpif_buf_cleanup : Callback function to free buffer
+ * vpif_start_streaming : Starts the DMA engine for streaming
  * @vb: ptr to vb2_buffer
- *
- * This function is called from the videobuf2 layer to free memory
- * allocated to  the buffers
+ * @count: number of buffers
  */
-static void vpif_buf_cleanup(struct vb2_buffer *vb)
-{
-	/* Get the file handle object and channel object */
-	struct vpif_fh *fh = vb2_get_drv_priv(vb->vb2_queue);
-	struct vpif_cap_buffer *buf = container_of(vb,
-					struct vpif_cap_buffer, vb);
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common;
-	unsigned long flags;
-
-	common = &ch->common[VPIF_VIDEO_INDEX];
-
-	spin_lock_irqsave(&common->irqlock, flags);
-	if (vb->state == VB2_BUF_STATE_ACTIVE)
-		list_del_init(&buf->list);
-	spin_unlock_irqrestore(&common->irqlock, flags);
-
-}
-
-static void vpif_wait_prepare(struct vb2_queue *vq)
-{
-	struct vpif_fh *fh = vb2_get_drv_priv(vq);
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common;
-
-	common = &ch->common[VPIF_VIDEO_INDEX];
-	mutex_unlock(&common->lock);
-}
-
-static void vpif_wait_finish(struct vb2_queue *vq)
-{
-	struct vpif_fh *fh = vb2_get_drv_priv(vq);
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common;
-
-	common = &ch->common[VPIF_VIDEO_INDEX];
-	mutex_lock(&common->lock);
-}
-
-static int vpif_buffer_init(struct vb2_buffer *vb)
-{
-	struct vpif_cap_buffer *buf = container_of(vb,
-					struct vpif_cap_buffer, vb);
-
-	INIT_LIST_HEAD(&buf->list);
-
-	return 0;
-}
-
-static u8 channel_first_int[VPIF_NUMBER_OF_OBJECTS][2] =
-	{ {1, 1} };
-
 static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
 {
 	struct vpif_capture_config *vpif_config_data =
 					vpif_dev->platform_data;
-	struct vpif_fh *fh = vb2_get_drv_priv(vq);
-	struct channel_obj *ch = fh->channel;
+	struct channel_obj *ch = vb2_get_drv_priv(vq);
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
 	struct vpif_params *vpif = &ch->vpifparams;
-	unsigned long addr = 0;
-	unsigned long flags;
+	struct vpif_cap_buffer *buf, *tmp;
+	unsigned long addr, flags;
 	int ret;
 
 	spin_lock_irqsave(&common->irqlock, flags);
 
-	/* Get the next frame from the buffer queue */
-	common->cur_frm = common->next_frm = list_entry(common->dma_queue.next,
-				    struct vpif_cap_buffer, list);
-	/* 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;
-	/* Initialize field_id and started member */
+	/* Initialize field_id */
 	ch->field_id = 0;
-	common->started = 1;
-	addr = vb2_dma_contig_plane_dma_addr(&common->cur_frm->vb, 0);
-
-	/* Calculate the offset for Y and C data in the buffer */
-	vpif_calculate_offsets(ch);
-
-	if ((vpif->std_info.frm_fmt &&
-	    ((common->fmt.fmt.pix.field != V4L2_FIELD_NONE) &&
-	     (common->fmt.fmt.pix.field != V4L2_FIELD_ANY))) ||
-	    (!vpif->std_info.frm_fmt &&
-	     (common->fmt.fmt.pix.field == V4L2_FIELD_NONE))) {
-		vpif_dbg(1, debug, "conflict in field format and std format\n");
-		return -EINVAL;
-	}
 
 	/* configure 1 or 2 channel mode */
 	if (vpif_config_data->setup_input_channel_mode) {
@@ -305,21 +209,37 @@ static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
 			setup_input_channel_mode(vpif->std_info.ycmux_mode);
 		if (ret < 0) {
 			vpif_dbg(1, debug, "can't set vpif channel mode\n");
-			return ret;
+			goto err;
 		}
 	}
 
+	ret = v4l2_subdev_call(ch->sd, video, s_stream, 1);
+	if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV) {
+		vpif_dbg(1, debug, "stream on failed in subdev\n");
+		goto err;
+	}
+
 	/* Call vpif_set_params function to set the parameters and addresses */
 	ret = vpif_set_video_params(vpif, ch->channel_id);
-
 	if (ret < 0) {
 		vpif_dbg(1, debug, "can't set video params\n");
-		return ret;
+		goto err;
 	}
 
-	common->started = ret;
+	ycmux_mode = ret;
 	vpif_config_addr(ch, ret);
 
+	/* Get the next frame from the buffer queue */
+	common->cur_frm = common->next_frm = list_entry(common->dma_queue.next,
+				    struct vpif_cap_buffer, list);
+	/* 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);
+
 	common->set_addr(addr + common->ytop_off,
 			 addr + common->ybtm_off,
 			 addr + common->ctop_off,
@@ -330,36 +250,75 @@ static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
 	 * VPIF register
 	 */
 	channel_first_int[VPIF_VIDEO_INDEX][ch->channel_id] = 1;
-	if ((VPIF_CHANNEL0_VIDEO == ch->channel_id)) {
+	if (VPIF_CHANNEL0_VIDEO == ch->channel_id) {
 		channel0_intr_assert();
 		channel0_intr_enable(1);
 		enable_channel0(1);
 	}
-	if ((VPIF_CHANNEL1_VIDEO == ch->channel_id) ||
-	    (common->started == 2)) {
+	if (VPIF_CHANNEL1_VIDEO == ch->channel_id ||
+		ycmux_mode == 2) {
 		channel1_intr_assert();
 		channel1_intr_enable(1);
 		enable_channel1(1);
 	}
 
 	return 0;
+
+err:
+	list_for_each_entry_safe(buf, tmp, &common->dma_queue, list) {
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_QUEUED);
+	}
+
+	return ret;
 }
 
-/* abort streaming and wait for last buffer */
-static int vpif_stop_streaming(struct vb2_queue *vq)
+/**
+ * vpif_stop_streaming : Stop the DMA engine
+ * @vq: ptr to vb2_queue
+ *
+ * This callback stops the DMA engine and any remaining buffers
+ * in the DMA queue are released.
+ */
+static void vpif_stop_streaming(struct vb2_queue *vq)
 {
-	struct vpif_fh *fh = vb2_get_drv_priv(vq);
-	struct channel_obj *ch = fh->channel;
+	struct channel_obj *ch = vb2_get_drv_priv(vq);
 	struct common_obj *common;
 	unsigned long flags;
-
-	if (!vb2_is_streaming(vq))
-		return 0;
+	int ret;
 
 	common = &ch->common[VPIF_VIDEO_INDEX];
 
+	/* Disable channel as per its device type and channel id */
+	if (VPIF_CHANNEL0_VIDEO == ch->channel_id) {
+		enable_channel0(0);
+		channel0_intr_enable(0);
+	}
+	if (VPIF_CHANNEL1_VIDEO == ch->channel_id ||
+		ycmux_mode == 2) {
+		enable_channel1(0);
+		channel1_intr_enable(0);
+	}
+
+	ycmux_mode = 0;
+
+	ret = v4l2_subdev_call(ch->sd, video, s_stream, 0);
+	if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV)
+		vpif_dbg(1, debug, "stream off failed in subdev\n");
+
 	/* release all active buffers */
 	spin_lock_irqsave(&common->irqlock, flags);
+	if (common->cur_frm == common->next_frm) {
+		vb2_buffer_done(&common->cur_frm->vb, VB2_BUF_STATE_ERROR);
+	} else {
+		if (common->cur_frm != NULL)
+			vb2_buffer_done(&common->cur_frm->vb,
+					VB2_BUF_STATE_ERROR);
+		if (common->next_frm != NULL)
+			vb2_buffer_done(&common->next_frm->vb,
+					VB2_BUF_STATE_ERROR);
+	}
+
 	while (!list_empty(&common->dma_queue)) {
 		common->next_frm = list_entry(common->dma_queue.next,
 						struct vpif_cap_buffer, list);
@@ -367,19 +326,13 @@ static int vpif_stop_streaming(struct vb2_queue *vq)
 		vb2_buffer_done(&common->next_frm->vb, VB2_BUF_STATE_ERROR);
 	}
 	spin_unlock_irqrestore(&common->irqlock, flags);
-
-	return 0;
 }
 
 static struct vb2_ops video_qops = {
 	.queue_setup		= vpif_buffer_queue_setup,
-	.wait_prepare		= vpif_wait_prepare,
-	.wait_finish		= vpif_wait_finish,
-	.buf_init		= vpif_buffer_init,
 	.buf_prepare		= vpif_buffer_prepare,
 	.start_streaming	= vpif_start_streaming,
 	.stop_streaming		= vpif_stop_streaming,
-	.buf_cleanup		= vpif_buf_cleanup,
 	.buf_queue		= vpif_buffer_queue,
 };
 
@@ -456,9 +409,6 @@ static irqreturn_t vpif_channel_isr(int irq, void *dev_id)
 	for (i = 0; i < VPIF_NUMBER_OF_OBJECTS; i++) {
 		common = &ch->common[i];
 		/* skip If streaming is not started in this channel */
-		if (0 == common->started)
-			continue;
-
 		/* Check the field format */
 		if (1 == ch->vpifparams.std_info.frm_fmt) {
 			/* Progressive mode */
@@ -660,11 +610,6 @@ static void vpif_config_format(struct channel_obj *ch)
 	vpif_dbg(2, debug, "vpif_config_format\n");
 
 	common->fmt.fmt.pix.field = V4L2_FIELD_ANY;
-	if (config_params.numbuffers[ch->channel_id] == 0)
-		common->memory = V4L2_MEMORY_USERPTR;
-	else
-		common->memory = V4L2_MEMORY_MMAP;
-
 	common->fmt.fmt.pix.sizeimage
 	    = config_params.channel_bufsize[ch->channel_id];
 
@@ -814,426 +759,6 @@ static void vpif_config_addr(struct channel_obj *ch, int muxmode)
 }
 
 /**
- * vpif_mmap : It is used to map kernel space buffers into user spaces
- * @filep: file pointer
- * @vma: ptr to vm_area_struct
- */
-static int vpif_mmap(struct file *filep, struct vm_area_struct *vma)
-{
-	/* Get the channel object and file handle object */
-	struct vpif_fh *fh = filep->private_data;
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common = &(ch->common[VPIF_VIDEO_INDEX]);
-	int ret;
-
-	vpif_dbg(2, debug, "vpif_mmap\n");
-
-	if (mutex_lock_interruptible(&common->lock))
-		return -ERESTARTSYS;
-	ret = vb2_mmap(&common->buffer_queue, vma);
-	mutex_unlock(&common->lock);
-	return ret;
-}
-
-/**
- * vpif_poll: It is used for select/poll system call
- * @filep: file pointer
- * @wait: poll table to wait
- */
-static unsigned int vpif_poll(struct file *filep, poll_table * wait)
-{
-	struct vpif_fh *fh = filep->private_data;
-	struct channel_obj *channel = fh->channel;
-	struct common_obj *common = &(channel->common[VPIF_VIDEO_INDEX]);
-	unsigned int res = 0;
-
-	vpif_dbg(2, debug, "vpif_poll\n");
-
-	if (common->started) {
-		mutex_lock(&common->lock);
-		res = vb2_poll(&common->buffer_queue, filep, wait);
-		mutex_unlock(&common->lock);
-	}
-	return res;
-}
-
-/**
- * vpif_open : vpif open handler
- * @filep: file ptr
- *
- * It creates object of file handle structure and stores it in private_data
- * member of filepointer
- */
-static int vpif_open(struct file *filep)
-{
-	struct video_device *vdev = video_devdata(filep);
-	struct common_obj *common;
-	struct video_obj *vid_ch;
-	struct channel_obj *ch;
-	struct vpif_fh *fh;
-
-	vpif_dbg(2, debug, "vpif_open\n");
-
-	ch = video_get_drvdata(vdev);
-
-	vid_ch = &ch->video;
-	common = &ch->common[VPIF_VIDEO_INDEX];
-
-	/* Allocate memory for the file handle object */
-	fh = kzalloc(sizeof(struct vpif_fh), GFP_KERNEL);
-	if (NULL == fh) {
-		vpif_err("unable to allocate memory for file handle object\n");
-		return -ENOMEM;
-	}
-
-	if (mutex_lock_interruptible(&common->lock)) {
-		kfree(fh);
-		return -ERESTARTSYS;
-	}
-	/* store pointer to fh in private_data member of filep */
-	filep->private_data = fh;
-	fh->channel = ch;
-	fh->initialized = 0;
-	/* If decoder is not initialized. initialize it */
-	if (!ch->initialized) {
-		fh->initialized = 1;
-		ch->initialized = 1;
-		memset(&(ch->vpifparams), 0, sizeof(struct vpif_params));
-	}
-	/* Increment channel usrs counter */
-	ch->usrs++;
-	/* Set io_allowed member to false */
-	fh->io_allowed[VPIF_VIDEO_INDEX] = 0;
-	/* Initialize priority of this instance to default priority */
-	fh->prio = V4L2_PRIORITY_UNSET;
-	v4l2_prio_open(&ch->prio, &fh->prio);
-	mutex_unlock(&common->lock);
-	return 0;
-}
-
-/**
- * vpif_release : function to clean up file close
- * @filep: file pointer
- *
- * This function deletes buffer queue, frees the buffers and the vpif file
- * handle
- */
-static int vpif_release(struct file *filep)
-{
-	struct vpif_fh *fh = filep->private_data;
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common;
-
-	vpif_dbg(2, debug, "vpif_release\n");
-
-	common = &ch->common[VPIF_VIDEO_INDEX];
-
-	mutex_lock(&common->lock);
-	/* if this instance is doing IO */
-	if (fh->io_allowed[VPIF_VIDEO_INDEX]) {
-		/* Reset io_usrs member of channel object */
-		common->io_usrs = 0;
-		/* Disable channel as per its device type and channel id */
-		if (VPIF_CHANNEL0_VIDEO == ch->channel_id) {
-			enable_channel0(0);
-			channel0_intr_enable(0);
-		}
-		if ((VPIF_CHANNEL1_VIDEO == ch->channel_id) ||
-		    (2 == common->started)) {
-			enable_channel1(0);
-			channel1_intr_enable(0);
-		}
-		common->started = 0;
-		/* Free buffers allocated */
-		vb2_queue_release(&common->buffer_queue);
-		vb2_dma_contig_cleanup_ctx(common->alloc_ctx);
-	}
-
-	/* Decrement channel usrs counter */
-	ch->usrs--;
-
-	/* Close the priority */
-	v4l2_prio_close(&ch->prio, fh->prio);
-
-	if (fh->initialized)
-		ch->initialized = 0;
-
-	mutex_unlock(&common->lock);
-	filep->private_data = NULL;
-	kfree(fh);
-	return 0;
-}
-
-/**
- * vpif_reqbufs() - request buffer handler
- * @file: file ptr
- * @priv: file handle
- * @reqbuf: request buffer structure ptr
- */
-static int vpif_reqbufs(struct file *file, void *priv,
-			struct v4l2_requestbuffers *reqbuf)
-{
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common;
-	u8 index = 0;
-	struct vb2_queue *q;
-	int ret;
-
-	vpif_dbg(2, debug, "vpif_reqbufs\n");
-
-	/**
-	 * This file handle has not initialized the channel,
-	 * It is not allowed to do settings
-	 */
-	if ((VPIF_CHANNEL0_VIDEO == ch->channel_id)
-	    || (VPIF_CHANNEL1_VIDEO == ch->channel_id)) {
-		if (!fh->initialized) {
-			vpif_dbg(1, debug, "Channel Busy\n");
-			return -EBUSY;
-		}
-	}
-
-	if (V4L2_BUF_TYPE_VIDEO_CAPTURE != reqbuf->type || !vpif_dev)
-		return -EINVAL;
-
-	index = VPIF_VIDEO_INDEX;
-
-	common = &ch->common[index];
-
-	if (0 != common->io_usrs)
-		return -EBUSY;
-
-	/* Initialize videobuf2 queue as per the buffer type */
-	common->alloc_ctx = vb2_dma_contig_init_ctx(vpif_dev);
-	if (IS_ERR(common->alloc_ctx)) {
-		vpif_err("Failed to get the context\n");
-		return PTR_ERR(common->alloc_ctx);
-	}
-	q = &common->buffer_queue;
-	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	q->io_modes = VB2_MMAP | VB2_USERPTR;
-	q->drv_priv = fh;
-	q->ops = &video_qops;
-	q->mem_ops = &vb2_dma_contig_memops;
-	q->buf_struct_size = sizeof(struct vpif_cap_buffer);
-	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
-	q->min_buffers_needed = 1;
-
-	ret = vb2_queue_init(q);
-	if (ret) {
-		vpif_err("vpif_capture: vb2_queue_init() failed\n");
-		vb2_dma_contig_cleanup_ctx(common->alloc_ctx);
-		return ret;
-	}
-	/* Set io allowed member of file handle to TRUE */
-	fh->io_allowed[index] = 1;
-	/* Increment io usrs member of channel object to 1 */
-	common->io_usrs = 1;
-	/* Store type of memory requested in channel object */
-	common->memory = reqbuf->memory;
-	INIT_LIST_HEAD(&common->dma_queue);
-
-	/* Allocate buffers */
-	return vb2_reqbufs(&common->buffer_queue, reqbuf);
-}
-
-/**
- * vpif_querybuf() - query buffer handler
- * @file: file ptr
- * @priv: file handle
- * @buf: v4l2 buffer structure ptr
- */
-static int vpif_querybuf(struct file *file, void *priv,
-				struct v4l2_buffer *buf)
-{
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-
-	vpif_dbg(2, debug, "vpif_querybuf\n");
-
-	if (common->fmt.type != buf->type)
-		return -EINVAL;
-
-	if (common->memory != V4L2_MEMORY_MMAP) {
-		vpif_dbg(1, debug, "Invalid memory\n");
-		return -EINVAL;
-	}
-
-	return vb2_querybuf(&common->buffer_queue, buf);
-}
-
-/**
- * vpif_qbuf() - query buffer handler
- * @file: file ptr
- * @priv: file handle
- * @buf: v4l2 buffer structure ptr
- */
-static int vpif_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
-{
-
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-	struct v4l2_buffer tbuf = *buf;
-
-	vpif_dbg(2, debug, "vpif_qbuf\n");
-
-	if (common->fmt.type != tbuf.type) {
-		vpif_err("invalid buffer type\n");
-		return -EINVAL;
-	}
-
-	if (!fh->io_allowed[VPIF_VIDEO_INDEX]) {
-		vpif_err("fh io not allowed\n");
-		return -EACCES;
-	}
-
-	return vb2_qbuf(&common->buffer_queue, buf);
-}
-
-/**
- * vpif_dqbuf() - query buffer handler
- * @file: file ptr
- * @priv: file handle
- * @buf: v4l2 buffer structure ptr
- */
-static int vpif_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
-{
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-
-	vpif_dbg(2, debug, "vpif_dqbuf\n");
-
-	return vb2_dqbuf(&common->buffer_queue, buf,
-			 (file->f_flags & O_NONBLOCK));
-}
-
-/**
- * vpif_streamon() - streamon handler
- * @file: file ptr
- * @priv: file handle
- * @buftype: v4l2 buffer type
- */
-static int vpif_streamon(struct file *file, void *priv,
-				enum v4l2_buf_type buftype)
-{
-
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-	struct channel_obj *oth_ch = vpif_obj.dev[!ch->channel_id];
-	struct vpif_params *vpif;
-	int ret = 0;
-
-	vpif_dbg(2, debug, "vpif_streamon\n");
-
-	vpif = &ch->vpifparams;
-
-	if (buftype != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
-		vpif_dbg(1, debug, "buffer type not supported\n");
-		return -EINVAL;
-	}
-
-	/* If file handle is not allowed IO, return error */
-	if (!fh->io_allowed[VPIF_VIDEO_INDEX]) {
-		vpif_dbg(1, debug, "io not allowed\n");
-		return -EACCES;
-	}
-
-	/* If Streaming is already started, return error */
-	if (common->started) {
-		vpif_dbg(1, debug, "channel->started\n");
-		return -EBUSY;
-	}
-
-	if ((ch->channel_id == VPIF_CHANNEL0_VIDEO &&
-	    oth_ch->common[VPIF_VIDEO_INDEX].started &&
-	    vpif->std_info.ycmux_mode == 0) ||
-	   ((ch->channel_id == VPIF_CHANNEL1_VIDEO) &&
-	    (2 == oth_ch->common[VPIF_VIDEO_INDEX].started))) {
-		vpif_dbg(1, debug, "other channel is being used\n");
-		return -EBUSY;
-	}
-
-	ret = vpif_check_format(ch, &common->fmt.fmt.pix, 0);
-	if (ret)
-		return ret;
-
-	/* Enable streamon on the sub device */
-	ret = v4l2_subdev_call(ch->sd, video, s_stream, 1);
-
-	if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV) {
-		vpif_dbg(1, debug, "stream on failed in subdev\n");
-		return ret;
-	}
-
-	/* Call vb2_streamon to start streaming in videobuf2 */
-	ret = vb2_streamon(&common->buffer_queue, buftype);
-	if (ret) {
-		vpif_dbg(1, debug, "vb2_streamon\n");
-		return ret;
-	}
-
-	return ret;
-}
-
-/**
- * vpif_streamoff() - streamoff handler
- * @file: file ptr
- * @priv: file handle
- * @buftype: v4l2 buffer type
- */
-static int vpif_streamoff(struct file *file, void *priv,
-				enum v4l2_buf_type buftype)
-{
-
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-	int ret;
-
-	vpif_dbg(2, debug, "vpif_streamoff\n");
-
-	if (buftype != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
-		vpif_dbg(1, debug, "buffer type not supported\n");
-		return -EINVAL;
-	}
-
-	/* If io is allowed for this file handle, return error */
-	if (!fh->io_allowed[VPIF_VIDEO_INDEX]) {
-		vpif_dbg(1, debug, "io not allowed\n");
-		return -EACCES;
-	}
-
-	/* If streaming is not started, return error */
-	if (!common->started) {
-		vpif_dbg(1, debug, "channel->started\n");
-		return -EINVAL;
-	}
-
-	/* disable channel */
-	if (VPIF_CHANNEL0_VIDEO == ch->channel_id) {
-		enable_channel0(0);
-		channel0_intr_enable(0);
-	} else {
-		enable_channel1(0);
-		channel1_intr_enable(0);
-	}
-
-	common->started = 0;
-
-	ret = v4l2_subdev_call(ch->sd, video, s_stream, 0);
-
-	if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV)
-		vpif_dbg(1, debug, "stream off failed in subdev\n");
-
-	return vb2_streamoff(&common->buffer_queue, buftype);
-}
-
-/**
  * vpif_input_to_subdev() - Maps input to sub device
  * @vpif_cfg - global config ptr
  * @chan_cfg - channel config ptr
@@ -1336,8 +861,8 @@ static int vpif_set_input(
  */
 static int vpif_querystd(struct file *file, void *priv, v4l2_std_id *std_id)
 {
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
 	int ret = 0;
 
 	vpif_dbg(2, debug, "vpif_querystd\n");
@@ -1363,11 +888,22 @@ static int vpif_querystd(struct file *file, void *priv, v4l2_std_id *std_id)
  */
 static int vpif_g_std(struct file *file, void *priv, v4l2_std_id *std)
 {
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
+	struct vpif_capture_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct vpif_capture_chan_config *chan_cfg;
+	struct v4l2_input input;
 
 	vpif_dbg(2, debug, "vpif_g_std\n");
 
+	if (config->chan_config[ch->channel_id].inputs == NULL)
+		return -ENODATA;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+	input = chan_cfg->inputs[ch->input_idx].input;
+	if (input.capabilities != V4L2_IN_CAP_STD)
+		return -ENODATA;
+
 	*std = ch->video.stdid;
 	return 0;
 }
@@ -1380,31 +916,26 @@ static int vpif_g_std(struct file *file, void *priv, v4l2_std_id *std)
  */
 static int vpif_s_std(struct file *file, void *priv, v4l2_std_id std_id)
 {
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
+	struct vpif_capture_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-	int ret = 0;
+	struct vpif_capture_chan_config *chan_cfg;
+	struct v4l2_input input;
+	int ret;
 
 	vpif_dbg(2, debug, "vpif_s_std\n");
 
-	if (common->started) {
-		vpif_err("streaming in progress\n");
-		return -EBUSY;
-	}
-
-	if ((VPIF_CHANNEL0_VIDEO == ch->channel_id) ||
-	    (VPIF_CHANNEL1_VIDEO == ch->channel_id)) {
-		if (!fh->initialized) {
-			vpif_dbg(1, debug, "Channel Busy\n");
-			return -EBUSY;
-		}
-	}
+	if (config->chan_config[ch->channel_id].inputs == NULL)
+		return -ENODATA;
 
-	ret = v4l2_prio_check(&ch->prio, fh->prio);
-	if (0 != ret)
-		return ret;
+	chan_cfg = &config->chan_config[ch->channel_id];
+	input = chan_cfg->inputs[ch->input_idx].input;
+	if (input.capabilities != V4L2_IN_CAP_STD)
+		return -ENODATA;
 
-	fh->initialized = 1;
+	if (vb2_is_busy(&common->buffer_queue))
+		return -EBUSY;
 
 	/* Call encoder subdevice function to set the standard */
 	ch->video.stdid = std_id;
@@ -1420,7 +951,7 @@ static int vpif_s_std(struct file *file, void *priv, v4l2_std_id std_id)
 	vpif_config_format(ch);
 
 	/* set standard in the sub device */
-	ret = v4l2_subdev_call(ch->sd, core, s_std, std_id);
+	ret = v4l2_subdev_call(ch->sd, video, s_std, std_id);
 	if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV) {
 		vpif_dbg(1, debug, "Failed to set standard for sub devices\n");
 		return ret;
@@ -1439,9 +970,9 @@ static int vpif_enum_input(struct file *file, void *priv,
 {
 
 	struct vpif_capture_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
 	struct vpif_capture_chan_config *chan_cfg;
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
 
 	chan_cfg = &config->chan_config[ch->channel_id];
 
@@ -1463,8 +994,8 @@ static int vpif_enum_input(struct file *file, void *priv,
  */
 static int vpif_g_input(struct file *file, void *priv, unsigned int *index)
 {
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
 
 	*index = ch->input_idx;
 	return 0;
@@ -1479,35 +1010,19 @@ static int vpif_g_input(struct file *file, void *priv, unsigned int *index)
 static int vpif_s_input(struct file *file, void *priv, unsigned int index)
 {
 	struct vpif_capture_config *config = vpif_dev->platform_data;
-	struct vpif_capture_chan_config *chan_cfg;
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-	int ret;
+	struct vpif_capture_chan_config *chan_cfg;
 
 	chan_cfg = &config->chan_config[ch->channel_id];
 
 	if (index >= chan_cfg->input_count)
 		return -EINVAL;
 
-	if (common->started) {
-		vpif_err("Streaming in progress\n");
+	if (vb2_is_busy(&common->buffer_queue))
 		return -EBUSY;
-	}
-
-	if ((VPIF_CHANNEL0_VIDEO == ch->channel_id) ||
-	    (VPIF_CHANNEL1_VIDEO == ch->channel_id)) {
-		if (!fh->initialized) {
-			vpif_dbg(1, debug, "Channel Busy\n");
-			return -EBUSY;
-		}
-	}
 
-	ret = v4l2_prio_check(&ch->prio, fh->prio);
-	if (0 != ret)
-		return ret;
-
-	fh->initialized = 1;
 	return vpif_set_input(config, ch, index);
 }
 
@@ -1520,8 +1035,8 @@ static int vpif_s_input(struct file *file, void *priv, unsigned int index)
 static int vpif_enum_fmt_vid_cap(struct file *file, void  *priv,
 					struct v4l2_fmtdesc *fmt)
 {
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
 
 	if (fmt->index != 0) {
 		vpif_dbg(1, debug, "Invalid format index\n");
@@ -1550,8 +1065,8 @@ static int vpif_enum_fmt_vid_cap(struct file *file, void  *priv,
 static int vpif_try_fmt_vid_cap(struct file *file, void *priv,
 				struct v4l2_format *fmt)
 {
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
 	struct v4l2_pix_format *pixfmt = &fmt->fmt.pix;
 
 	return vpif_check_format(ch, pixfmt, 1);
@@ -1567,8 +1082,8 @@ static int vpif_try_fmt_vid_cap(struct file *file, void *priv,
 static int vpif_g_fmt_vid_cap(struct file *file, void *priv,
 				struct v4l2_format *fmt)
 {
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
 
 	/* Check the validity of the buffer type */
@@ -1589,33 +1104,16 @@ static int vpif_g_fmt_vid_cap(struct file *file, void *priv,
 static int vpif_s_fmt_vid_cap(struct file *file, void *priv,
 				struct v4l2_format *fmt)
 {
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
 	struct v4l2_pix_format *pixfmt;
 	int ret = 0;
 
 	vpif_dbg(2, debug, "%s\n", __func__);
 
-	/* If streaming is started, return error */
-	if (common->started) {
-		vpif_dbg(1, debug, "Streaming is started\n");
+	if (vb2_is_busy(&common->buffer_queue))
 		return -EBUSY;
-	}
-
-	if ((VPIF_CHANNEL0_VIDEO == ch->channel_id) ||
-	    (VPIF_CHANNEL1_VIDEO == ch->channel_id)) {
-		if (!fh->initialized) {
-			vpif_dbg(1, debug, "Channel Busy\n");
-			return -EBUSY;
-		}
-	}
-
-	ret = v4l2_prio_check(&ch->prio, fh->prio);
-	if (0 != ret)
-		return ret;
-
-	fh->initialized = 1;
 
 	pixfmt = &fmt->fmt.pix;
 	/* Check for valid field format */
@@ -1641,7 +1139,7 @@ static int vpif_querycap(struct file *file, void  *priv,
 
 	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
 	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
-	snprintf(cap->driver, sizeof(cap->driver), "%s", dev_name(vpif_dev));
+	strlcpy(cap->driver, VPIF_DRIVER_NAME, sizeof(cap->driver));
 	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
 		 dev_name(vpif_dev));
 	strlcpy(cap->card, config->card_name, sizeof(cap->card));
@@ -1650,61 +1148,6 @@ static int vpif_querycap(struct file *file, void  *priv,
 }
 
 /**
- * vpif_g_priority() - get priority handler
- * @file: file ptr
- * @priv: file handle
- * @prio: ptr to v4l2_priority structure
- */
-static int vpif_g_priority(struct file *file, void *priv,
-			   enum v4l2_priority *prio)
-{
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
-
-	*prio = v4l2_prio_max(&ch->prio);
-
-	return 0;
-}
-
-/**
- * vpif_s_priority() - set priority handler
- * @file: file ptr
- * @priv: file handle
- * @prio: ptr to v4l2_priority structure
- */
-static int vpif_s_priority(struct file *file, void *priv, enum v4l2_priority p)
-{
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
-
-	return v4l2_prio_change(&ch->prio, &fh->prio, p);
-}
-
-/**
- * vpif_cropcap() - cropcap handler
- * @file: file ptr
- * @priv: file handle
- * @crop: ptr to v4l2_cropcap structure
- */
-static int vpif_cropcap(struct file *file, void *priv,
-			struct v4l2_cropcap *crop)
-{
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-
-	if (V4L2_BUF_TYPE_VIDEO_CAPTURE != crop->type)
-		return -EINVAL;
-
-	crop->bounds.left = 0;
-	crop->bounds.top = 0;
-	crop->bounds.height = common->height;
-	crop->bounds.width = common->width;
-	crop->defrect = crop->bounds;
-	return 0;
-}
-
-/**
  * vpif_enum_dv_timings() - ENUM_DV_TIMINGS handler
  * @file: file ptr
  * @priv: file handle
@@ -1714,13 +1157,27 @@ static int
 vpif_enum_dv_timings(struct file *file, void *priv,
 		     struct v4l2_enum_dv_timings *timings)
 {
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
+	struct vpif_capture_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct vpif_capture_chan_config *chan_cfg;
+	struct v4l2_input input;
 	int ret;
 
-	ret = v4l2_subdev_call(ch->sd, video, enum_dv_timings, timings);
+	if (config->chan_config[ch->channel_id].inputs == NULL)
+		return -ENODATA;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+	input = chan_cfg->inputs[ch->input_idx].input;
+	if (input.capabilities != V4L2_IN_CAP_DV_TIMINGS)
+		return -ENODATA;
+
+	timings->pad = 0;
+
+	ret = v4l2_subdev_call(ch->sd, pad, enum_dv_timings, timings);
 	if (ret == -ENOIOCTLCMD || ret == -ENODEV)
 		return -EINVAL;
+
 	return ret;
 }
 
@@ -1734,13 +1191,25 @@ static int
 vpif_query_dv_timings(struct file *file, void *priv,
 		      struct v4l2_dv_timings *timings)
 {
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
+	struct vpif_capture_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct vpif_capture_chan_config *chan_cfg;
+	struct v4l2_input input;
 	int ret;
 
+	if (config->chan_config[ch->channel_id].inputs == NULL)
+		return -ENODATA;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+	input = chan_cfg->inputs[ch->input_idx].input;
+	if (input.capabilities != V4L2_IN_CAP_DV_TIMINGS)
+		return -ENODATA;
+
 	ret = v4l2_subdev_call(ch->sd, video, query_dv_timings, timings);
 	if (ret == -ENOIOCTLCMD || ret == -ENODEV)
 		return -ENODATA;
+
 	return ret;
 }
 
@@ -1753,19 +1222,34 @@ vpif_query_dv_timings(struct file *file, void *priv,
 static int vpif_s_dv_timings(struct file *file, void *priv,
 		struct v4l2_dv_timings *timings)
 {
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
+	struct vpif_capture_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
 	struct vpif_params *vpifparams = &ch->vpifparams;
 	struct vpif_channel_config_params *std_info = &vpifparams->std_info;
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
 	struct video_obj *vid_ch = &ch->video;
 	struct v4l2_bt_timings *bt = &vid_ch->dv_timings.bt;
+	struct vpif_capture_chan_config *chan_cfg;
+	struct v4l2_input input;
 	int ret;
 
+	if (config->chan_config[ch->channel_id].inputs == NULL)
+		return -ENODATA;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+	input = chan_cfg->inputs[ch->input_idx].input;
+	if (input.capabilities != V4L2_IN_CAP_DV_TIMINGS)
+		return -ENODATA;
+
 	if (timings->type != V4L2_DV_BT_656_1120) {
 		vpif_dbg(2, debug, "Timing type not defined\n");
 		return -EINVAL;
 	}
 
+	if (vb2_is_busy(&common->buffer_queue))
+		return -EBUSY;
+
 	/* Configure subdevice timings, if any */
 	ret = v4l2_subdev_call(ch->sd, video, s_dv_timings, timings);
 	if (ret == -ENOIOCTLCMD || ret == -ENODEV)
@@ -1841,9 +1325,20 @@ static int vpif_s_dv_timings(struct file *file, void *priv,
 static int vpif_g_dv_timings(struct file *file, void *priv,
 		struct v4l2_dv_timings *timings)
 {
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
+	struct vpif_capture_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
 	struct video_obj *vid_ch = &ch->video;
+	struct vpif_capture_chan_config *chan_cfg;
+	struct v4l2_input input;
+
+	if (config->chan_config[ch->channel_id].inputs == NULL)
+		return -ENODATA;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+	input = chan_cfg->inputs[ch->input_idx].input;
+	if (input.capabilities != V4L2_IN_CAP_DV_TIMINGS)
+		return -ENODATA;
 
 	*timings = vid_ch->dv_timings;
 
@@ -1867,49 +1362,45 @@ static int vpif_log_status(struct file *filep, void *priv)
 
 /* vpif capture ioctl operations */
 static const struct v4l2_ioctl_ops vpif_ioctl_ops = {
-	.vidioc_querycap        	= vpif_querycap,
-	.vidioc_g_priority		= vpif_g_priority,
-	.vidioc_s_priority		= vpif_s_priority,
+	.vidioc_querycap		= vpif_querycap,
 	.vidioc_enum_fmt_vid_cap	= vpif_enum_fmt_vid_cap,
-	.vidioc_g_fmt_vid_cap  		= vpif_g_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap		= vpif_g_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap		= vpif_s_fmt_vid_cap,
 	.vidioc_try_fmt_vid_cap		= vpif_try_fmt_vid_cap,
+
 	.vidioc_enum_input		= vpif_enum_input,
 	.vidioc_s_input			= vpif_s_input,
 	.vidioc_g_input			= vpif_g_input,
-	.vidioc_reqbufs         	= vpif_reqbufs,
-	.vidioc_querybuf        	= vpif_querybuf,
+
+	.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_expbuf			= vb2_ioctl_expbuf,
+	.vidioc_streamon		= vb2_ioctl_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
+
 	.vidioc_querystd		= vpif_querystd,
-	.vidioc_s_std           	= vpif_s_std,
+	.vidioc_s_std			= vpif_s_std,
 	.vidioc_g_std			= vpif_g_std,
-	.vidioc_qbuf            	= vpif_qbuf,
-	.vidioc_dqbuf           	= vpif_dqbuf,
-	.vidioc_streamon        	= vpif_streamon,
-	.vidioc_streamoff       	= vpif_streamoff,
-	.vidioc_cropcap         	= vpif_cropcap,
-	.vidioc_enum_dv_timings         = vpif_enum_dv_timings,
-	.vidioc_query_dv_timings        = vpif_query_dv_timings,
-	.vidioc_s_dv_timings            = vpif_s_dv_timings,
-	.vidioc_g_dv_timings            = vpif_g_dv_timings,
+
+	.vidioc_enum_dv_timings		= vpif_enum_dv_timings,
+	.vidioc_query_dv_timings	= vpif_query_dv_timings,
+	.vidioc_s_dv_timings		= vpif_s_dv_timings,
+	.vidioc_g_dv_timings		= vpif_g_dv_timings,
+
 	.vidioc_log_status		= vpif_log_status,
 };
 
 /* vpif file operations */
 static struct v4l2_file_operations vpif_fops = {
 	.owner = THIS_MODULE,
-	.open = vpif_open,
-	.release = vpif_release,
+	.open = v4l2_fh_open,
+	.release = vb2_fop_release,
 	.unlocked_ioctl = video_ioctl2,
-	.mmap = vpif_mmap,
-	.poll = vpif_poll
-};
-
-/* vpif video template */
-static struct video_device vpif_video_template = {
-	.name		= "vpif",
-	.fops		= &vpif_fops,
-	.minor		= -1,
-	.ioctl_ops	= &vpif_ioctl_ops,
+	.mmap = vb2_fop_mmap,
+	.poll = vb2_fop_poll
 };
 
 /**
@@ -1987,7 +1478,9 @@ static int vpif_async_bound(struct v4l2_async_notifier *notifier,
 static int vpif_probe_complete(void)
 {
 	struct common_obj *common;
+	struct video_device *vdev;
 	struct channel_obj *ch;
+	struct vb2_queue *q;
 	int i, j, err, k;
 
 	for (j = 0; j < VPIF_CAPTURE_MAX_DEVICES; j++) {
@@ -1996,17 +1489,52 @@ static int vpif_probe_complete(void)
 		common = &(ch->common[VPIF_VIDEO_INDEX]);
 		spin_lock_init(&common->irqlock);
 		mutex_init(&common->lock);
-		ch->video_dev->lock = &common->lock;
-		/* Initialize prio member of channel object */
-		v4l2_prio_init(&ch->prio);
-		video_set_drvdata(ch->video_dev, ch);
 
 		/* select input 0 */
 		err = vpif_set_input(vpif_obj.config, ch, 0);
 		if (err)
 			goto probe_out;
 
-		err = video_register_device(ch->video_dev,
+		/* Initialize vb2 queue */
+		q = &common->buffer_queue;
+		q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+		q->drv_priv = ch;
+		q->ops = &video_qops;
+		q->mem_ops = &vb2_dma_contig_memops;
+		q->buf_struct_size = sizeof(struct vpif_cap_buffer);
+		q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+		q->min_buffers_needed = 1;
+		q->lock = &common->lock;
+
+		err = vb2_queue_init(q);
+		if (err) {
+			vpif_err("vpif_capture: vb2_queue_init() failed\n");
+			goto probe_out;
+		}
+
+		common->alloc_ctx = vb2_dma_contig_init_ctx(vpif_dev);
+		if (IS_ERR(common->alloc_ctx)) {
+			vpif_err("Failed to get the context\n");
+			err = PTR_ERR(common->alloc_ctx);
+			goto probe_out;
+		}
+
+		INIT_LIST_HEAD(&common->dma_queue);
+
+		/* Initialize the video_device structure */
+		vdev = ch->video_dev;
+		strlcpy(vdev->name, VPIF_DRIVER_NAME, sizeof(vdev->name));
+		vdev->release = video_device_release;
+		vdev->fops = &vpif_fops;
+		vdev->ioctl_ops = &vpif_ioctl_ops;
+		vdev->v4l2_dev = &vpif_obj.v4l2_dev;
+		vdev->vfl_dir = VFL_DIR_RX;
+		vdev->queue = q;
+		vdev->lock = &common->lock;
+		set_bit(V4L2_FL_USE_FH_PRIO, &vdev->flags);
+		video_set_drvdata(ch->video_dev, ch);
+		err = video_register_device(vdev,
 					    VFL_TYPE_GRABBER, (j ? 1 : 0));
 		if (err)
 			goto probe_out;
@@ -2019,6 +1547,8 @@ probe_out:
 	for (k = 0; k < j; k++) {
 		/* Get the pointer to the channel object */
 		ch = vpif_obj.dev[k];
+		common = &ch->common[k];
+		vb2_dma_contig_cleanup_ctx(common->alloc_ctx);
 		/* Unregister video device */
 		video_unregister_device(ch->video_dev);
 	}
@@ -2055,7 +1585,6 @@ static __init int vpif_probe(struct platform_device *pdev)
 	struct video_device *vfd;
 	struct resource *res;
 	int subdev_count;
-	size_t size;
 
 	vpif_dev = &pdev->dev;
 
@@ -2073,7 +1602,7 @@ static __init int vpif_probe(struct platform_device *pdev)
 
 	while ((res = platform_get_resource(pdev, IORESOURCE_IRQ, res_idx))) {
 		err = devm_request_irq(&pdev->dev, res->start, vpif_channel_isr,
-					IRQF_SHARED, "VPIF_Capture",
+					IRQF_SHARED, VPIF_DRIVER_NAME,
 					(void *)(&vpif_obj.dev[res_idx]->
 					channel_id));
 		if (err) {
@@ -2097,34 +1626,10 @@ static __init int vpif_probe(struct platform_device *pdev)
 			goto vpif_unregister;
 		}
 
-		/* Initialize field of video device */
-		*vfd = vpif_video_template;
-		vfd->v4l2_dev = &vpif_obj.v4l2_dev;
-		vfd->release = video_device_release;
-		snprintf(vfd->name, sizeof(vfd->name),
-			 "VPIF_Capture_DRIVER_V%s",
-			 VPIF_CAPTURE_VERSION);
 		/* Set video_dev to the video device */
 		ch->video_dev = vfd;
 	}
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (res) {
-		size = resource_size(res);
-		/* The resources are divided into two equal memory and when we
-		 * have HD output we can add them together
-		 */
-		for (j = 0; j < VPIF_CAPTURE_MAX_DEVICES; j++) {
-			ch = vpif_obj.dev[j];
-			ch->channel_id = j;
-			/* only enabled if second resource exists */
-			config_params.video_limit[ch->channel_id] = 0;
-			if (size)
-				config_params.video_limit[ch->channel_id] =
-									size/2;
-		}
-	}
-
 	vpif_obj.config = pdev->dev.platform_data;
 
 	subdev_count = vpif_obj.config->subdev_count;
@@ -2197,6 +1702,7 @@ vpif_unregister:
  */
 static int vpif_remove(struct platform_device *device)
 {
+	struct common_obj *common;
 	struct channel_obj *ch;
 	int i;
 
@@ -2207,6 +1713,8 @@ static int vpif_remove(struct platform_device *device)
 	for (i = 0; i < VPIF_CAPTURE_MAX_DEVICES; i++) {
 		/* Get the pointer to the channel object */
 		ch = vpif_obj.dev[i];
+		common = &ch->common[i];
+		vb2_dma_contig_cleanup_ctx(common->alloc_ctx);
 		/* Unregister video device */
 		video_unregister_device(ch->video_dev);
 		kfree(vpif_obj.dev[i]);
@@ -2214,7 +1722,7 @@ static int vpif_remove(struct platform_device *device)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 /**
  * vpif_suspend: vpif device suspend
  */
@@ -2229,18 +1737,20 @@ static int vpif_suspend(struct device *dev)
 		/* Get the pointer to the channel object */
 		ch = vpif_obj.dev[i];
 		common = &ch->common[VPIF_VIDEO_INDEX];
+
+		if (!vb2_is_streaming(&common->buffer_queue))
+			continue;
+
 		mutex_lock(&common->lock);
-		if (ch->usrs && common->io_usrs) {
-			/* Disable channel */
-			if (ch->channel_id == VPIF_CHANNEL0_VIDEO) {
-				enable_channel0(0);
-				channel0_intr_enable(0);
-			}
-			if (ch->channel_id == VPIF_CHANNEL1_VIDEO ||
-			    common->started == 2) {
-				enable_channel1(0);
-				channel1_intr_enable(0);
-			}
+		/* Disable channel */
+		if (ch->channel_id == VPIF_CHANNEL0_VIDEO) {
+			enable_channel0(0);
+			channel0_intr_enable(0);
+		}
+		if (ch->channel_id == VPIF_CHANNEL1_VIDEO ||
+			ycmux_mode == 2) {
+			enable_channel1(0);
+			channel1_intr_enable(0);
 		}
 		mutex_unlock(&common->lock);
 	}
@@ -2261,40 +1771,35 @@ static int vpif_resume(struct device *dev)
 		/* Get the pointer to the channel object */
 		ch = vpif_obj.dev[i];
 		common = &ch->common[VPIF_VIDEO_INDEX];
+
+		if (!vb2_is_streaming(&common->buffer_queue))
+			continue;
+
 		mutex_lock(&common->lock);
-		if (ch->usrs && common->io_usrs) {
-			/* Disable channel */
-			if (ch->channel_id == VPIF_CHANNEL0_VIDEO) {
-				enable_channel0(1);
-				channel0_intr_enable(1);
-			}
-			if (ch->channel_id == VPIF_CHANNEL1_VIDEO ||
-			    common->started == 2) {
-				enable_channel1(1);
-				channel1_intr_enable(1);
-			}
+		/* Enable channel */
+		if (ch->channel_id == VPIF_CHANNEL0_VIDEO) {
+			enable_channel0(1);
+			channel0_intr_enable(1);
+		}
+		if (ch->channel_id == VPIF_CHANNEL1_VIDEO ||
+			ycmux_mode == 2) {
+			enable_channel1(1);
+			channel1_intr_enable(1);
 		}
 		mutex_unlock(&common->lock);
 	}
 
 	return 0;
 }
-
-static const struct dev_pm_ops vpif_dev_pm_ops = {
-	.suspend = vpif_suspend,
-	.resume = vpif_resume,
-};
-
-#define vpif_pm_ops (&vpif_dev_pm_ops)
-#else
-#define vpif_pm_ops NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(vpif_pm_ops, vpif_suspend, vpif_resume);
+
 static __refdata struct platform_driver vpif_driver = {
 	.driver	= {
-		.name	= "vpif_capture",
+		.name	= VPIF_DRIVER_NAME,
 		.owner	= THIS_MODULE,
-		.pm	= vpif_pm_ops,
+		.pm	= &vpif_pm_ops,
 	},
 	.probe = vpif_probe,
 	.remove = vpif_remove,
diff --git a/drivers/media/platform/davinci/vpif_capture.h b/drivers/media/platform/davinci/vpif_capture.h
index 5a29d9a0cae..1ee17824f48 100644
--- a/drivers/media/platform/davinci/vpif_capture.h
+++ b/drivers/media/platform/davinci/vpif_capture.h
@@ -19,8 +19,6 @@
 #ifndef VPIF_CAPTURE_H
 #define VPIF_CAPTURE_H
 
-#ifdef __KERNEL__
-
 /* Header files */
 #include <media/videobuf2-dma-contig.h>
 #include <media/v4l2-device.h>
@@ -63,11 +61,6 @@ struct common_obj {
 	struct vpif_cap_buffer *cur_frm;
 	/* Pointer pointing to current v4l2_buffer */
 	struct vpif_cap_buffer *next_frm;
-	/*
-	 * This field keeps track of type of buffer exchange mechanism
-	 * user has selected
-	 */
-	enum v4l2_memory memory;
 	/* Used to store pixel format */
 	struct v4l2_format fmt;
 	/* Buffer queue used in video-buf */
@@ -80,10 +73,6 @@ struct common_obj {
 	spinlock_t irqlock;
 	/* lock used to access this structure */
 	struct mutex lock;
-	/* number of users performing IO */
-	u32 io_usrs;
-	/* Indicates whether streaming started */
-	u8 started;
 	/* Function pointer to set the addresses */
 	void (*set_addr) (unsigned long, unsigned long, unsigned long,
 			  unsigned long);
@@ -104,10 +93,6 @@ struct common_obj {
 struct channel_obj {
 	/* Identifies video device for this channel */
 	struct video_device *video_dev;
-	/* Used to keep track of state of the priority */
-	struct v4l2_prio_state prio;
-	/* number of open instances of the channel */
-	int usrs;
 	/* Indicates id of the field which is being displayed */
 	u32 field_id;
 	/* flag to indicate whether decoder is initialized */
@@ -126,18 +111,6 @@ struct channel_obj {
 	struct video_obj video;
 };
 
-/* File handle structure */
-struct vpif_fh {
-	/* pointer to channel object for opened device */
-	struct channel_obj *channel;
-	/* Indicates whether this file handle is doing IO */
-	u8 io_allowed[VPIF_NUMBER_OF_OBJECTS];
-	/* Used to keep track priority of this instance */
-	enum v4l2_priority prio;
-	/* Used to indicate channel is initialize or not */
-	u8 initialized;
-};
-
 struct vpif_device {
 	struct v4l2_device v4l2_dev;
 	struct channel_obj *dev[VPIF_CAPTURE_NUM_CHANNELS];
@@ -157,5 +130,4 @@ struct vpif_config_params {
 	u8 max_device_type;
 };
 
-#endif				/* End of __KERNEL__ */
 #endif				/* VPIF_CAPTURE_H */
diff --git a/drivers/media/platform/davinci/vpif_display.c b/drivers/media/platform/davinci/vpif_display.c
index 0ac841e35aa..5bb085b19bc 100644
--- a/drivers/media/platform/davinci/vpif_display.c
+++ b/drivers/media/platform/davinci/vpif_display.c
@@ -3,6 +3,7 @@
  * Display driver for TI DaVinci VPIF
  *
  * Copyright (C) 2009 Texas Instruments Incorporated - http://www.ti.com/
+ * Copyright (C) 2014 Lad, Prabhakar <prabhakar.csengg@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
@@ -35,129 +36,110 @@ MODULE_VERSION(VPIF_DISPLAY_VERSION);
 		v4l2_dbg(level, debug, &vpif_obj.v4l2_dev, fmt, ## arg)
 
 static int debug = 1;
-static u32 ch2_numbuffers = 3;
-static u32 ch3_numbuffers = 3;
-static u32 ch2_bufsize = 1920 * 1080 * 2;
-static u32 ch3_bufsize = 720 * 576 * 2;
 
 module_param(debug, int, 0644);
-module_param(ch2_numbuffers, uint, S_IRUGO);
-module_param(ch3_numbuffers, uint, S_IRUGO);
-module_param(ch2_bufsize, uint, S_IRUGO);
-module_param(ch3_bufsize, uint, S_IRUGO);
 
 MODULE_PARM_DESC(debug, "Debug level 0-1");
-MODULE_PARM_DESC(ch2_numbuffers, "Channel2 buffer count (default:3)");
-MODULE_PARM_DESC(ch3_numbuffers, "Channel3 buffer count (default:3)");
-MODULE_PARM_DESC(ch2_bufsize, "Channel2 buffer size (default:1920 x 1080 x 2)");
-MODULE_PARM_DESC(ch3_bufsize, "Channel3 buffer size (default:720 x 576 x 2)");
-
-static struct vpif_config_params config_params = {
-	.min_numbuffers		= 3,
-	.numbuffers[0]		= 3,
-	.numbuffers[1]		= 3,
-	.min_bufsize[0]		= 720 * 480 * 2,
-	.min_bufsize[1]		= 720 * 480 * 2,
-	.channel_bufsize[0]	= 1920 * 1080 * 2,
-	.channel_bufsize[1]	= 720 * 576 * 2,
-};
+
+#define VPIF_DRIVER_NAME	"vpif_display"
+
+/* Is set to 1 in case of SDTV formats, 2 in case of HDTV formats. */
+static int ycmux_mode;
+
+static u8 channel_first_int[VPIF_NUMOBJECTS][2] = { {1, 1} };
 
 static struct vpif_device vpif_obj = { {NULL} };
 static struct device *vpif_dev;
 static void vpif_calculate_offsets(struct channel_obj *ch);
 static void vpif_config_addr(struct channel_obj *ch, int muxmode);
 
-/*
- * buffer_prepare: This is the callback function called from vb2_qbuf()
- * function the buffer is prepared and user space virtual address is converted
- * into physical address
+static inline struct vpif_disp_buffer *to_vpif_buffer(struct vb2_buffer *vb)
+{
+	return container_of(vb, struct vpif_disp_buffer, vb);
+}
+
+/**
+ * vpif_buffer_prepare :  callback function for buffer prepare
+ * @vb: ptr to vb2_buffer
+ *
+ * This is the callback function for buffer prepare when vb2_qbuf()
+ * function is called. The buffer is prepared and user space virtual address
+ * or user address is converted into  physical address
  */
 static int vpif_buffer_prepare(struct vb2_buffer *vb)
 {
-	struct vpif_fh *fh = vb2_get_drv_priv(vb->vb2_queue);
-	struct vb2_queue *q = vb->vb2_queue;
+	struct channel_obj *ch = vb2_get_drv_priv(vb->vb2_queue);
 	struct common_obj *common;
-	unsigned long addr;
-
-	common = &fh->channel->common[VPIF_VIDEO_INDEX];
-	if (vb->state != VB2_BUF_STATE_ACTIVE &&
-		vb->state != VB2_BUF_STATE_PREPARED) {
-		vb2_set_plane_payload(vb, 0, common->fmt.fmt.pix.sizeimage);
-		if (vb2_plane_vaddr(vb, 0) &&
-		vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0))
-			goto buf_align_exit;
-
-		addr = vb2_dma_contig_plane_dma_addr(vb, 0);
-		if (q->streaming &&
-			(V4L2_BUF_TYPE_SLICED_VBI_OUTPUT != q->type)) {
-			if (!ISALIGNED(addr + common->ytop_off) ||
+
+	common = &ch->common[VPIF_VIDEO_INDEX];
+
+	vb2_set_plane_payload(vb, 0, common->fmt.fmt.pix.sizeimage);
+	if (vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0))
+		return -EINVAL;
+
+	vb->v4l2_buf.field = common->fmt.fmt.pix.field;
+
+	if (vb->vb2_queue->type != V4L2_BUF_TYPE_SLICED_VBI_OUTPUT) {
+		unsigned long addr = vb2_dma_contig_plane_dma_addr(vb, 0);
+
+		if (!ISALIGNED(addr + common->ytop_off) ||
 			!ISALIGNED(addr + common->ybtm_off) ||
 			!ISALIGNED(addr + common->ctop_off) ||
-			!ISALIGNED(addr + common->cbtm_off))
-				goto buf_align_exit;
+			!ISALIGNED(addr + common->cbtm_off)) {
+			vpif_err("buffer offset not aligned to 8 bytes\n");
+			return -EINVAL;
 		}
 	}
-	return 0;
 
-buf_align_exit:
-	vpif_err("buffer offset not aligned to 8 bytes\n");
-	return -EINVAL;
+	return 0;
 }
 
-/*
- * vpif_buffer_queue_setup: This function allocates memory for the buffers
+/**
+ * vpif_buffer_queue_setup : Callback function for buffer setup.
+ * @vq: vb2_queue ptr
+ * @fmt: v4l2 format
+ * @nbuffers: ptr to number of buffers requested by application
+ * @nplanes:: contains number of distinct video planes needed to hold a frame
+ * @sizes[]: contains the size (in bytes) of each plane.
+ * @alloc_ctxs: ptr to allocation context
+ *
+ * This callback function is called when reqbuf() is called to adjust
+ * the buffer count and buffer size
  */
 static int vpif_buffer_queue_setup(struct vb2_queue *vq,
 				const struct v4l2_format *fmt,
 				unsigned int *nbuffers, unsigned int *nplanes,
 				unsigned int sizes[], void *alloc_ctxs[])
 {
-	struct vpif_fh *fh = vb2_get_drv_priv(vq);
-	struct channel_obj *ch = fh->channel;
+	struct channel_obj *ch = vb2_get_drv_priv(vq);
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-	unsigned long size;
-
-	if (V4L2_MEMORY_MMAP == common->memory) {
-		size = config_params.channel_bufsize[ch->channel_id];
-		/*
-		* Checking if the buffer size exceeds the available buffer
-		* ycmux_mode = 0 means 1 channel mode HD and
-		* ycmux_mode = 1 means 2 channels mode SD
-		*/
-		if (ch->vpifparams.std_info.ycmux_mode == 0) {
-			if (config_params.video_limit[ch->channel_id])
-				while (size * *nbuffers >
-					(config_params.video_limit[0]
-						+ config_params.video_limit[1]))
-					(*nbuffers)--;
-		} else {
-			if (config_params.video_limit[ch->channel_id])
-				while (size * *nbuffers >
-				config_params.video_limit[ch->channel_id])
-					(*nbuffers)--;
-		}
-	} else {
-		size = common->fmt.fmt.pix.sizeimage;
-	}
 
-	if (*nbuffers < config_params.min_numbuffers)
-			*nbuffers = config_params.min_numbuffers;
+	if (fmt && fmt->fmt.pix.sizeimage < common->fmt.fmt.pix.sizeimage)
+		return -EINVAL;
+
+	if (vq->num_buffers + *nbuffers < 3)
+		*nbuffers = 3 - vq->num_buffers;
 
 	*nplanes = 1;
-	sizes[0] = size;
+	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : common->fmt.fmt.pix.sizeimage;
 	alloc_ctxs[0] = common->alloc_ctx;
+
+	/* Calculate the offset for Y and C data  in the buffer */
+	vpif_calculate_offsets(ch);
+
 	return 0;
 }
 
-/*
- * vpif_buffer_queue: This function adds the buffer to DMA queue
+/**
+ * vpif_buffer_queue : Callback function to add buffer to DMA queue
+ * @vb: ptr to vb2_buffer
+ *
+ * This callback fucntion queues the buffer to DMA engine
  */
 static void vpif_buffer_queue(struct vb2_buffer *vb)
 {
-	struct vpif_fh *fh = vb2_get_drv_priv(vb->vb2_queue);
-	struct vpif_disp_buffer *buf = container_of(vb,
-				struct vpif_disp_buffer, vb);
-	struct channel_obj *ch = fh->channel;
+	struct vpif_disp_buffer *buf = to_vpif_buffer(vb);
+	struct channel_obj *ch = vb2_get_drv_priv(vb->vb2_queue);
 	struct common_obj *common;
 	unsigned long flags;
 
@@ -169,98 +151,26 @@ static void vpif_buffer_queue(struct vb2_buffer *vb)
 	spin_unlock_irqrestore(&common->irqlock, flags);
 }
 
-/*
- * vpif_buf_cleanup: This function is called from the videobuf2 layer to
- * free memory allocated to the buffers
+/**
+ * vpif_start_streaming : Starts the DMA engine for streaming
+ * @vb: ptr to vb2_buffer
+ * @count: number of buffers
  */
-static void vpif_buf_cleanup(struct vb2_buffer *vb)
-{
-	struct vpif_fh *fh = vb2_get_drv_priv(vb->vb2_queue);
-	struct vpif_disp_buffer *buf = container_of(vb,
-					struct vpif_disp_buffer, vb);
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common;
-	unsigned long flags;
-
-	common = &ch->common[VPIF_VIDEO_INDEX];
-
-	spin_lock_irqsave(&common->irqlock, flags);
-	if (vb->state == VB2_BUF_STATE_ACTIVE)
-		list_del_init(&buf->list);
-	spin_unlock_irqrestore(&common->irqlock, flags);
-}
-
-static void vpif_wait_prepare(struct vb2_queue *vq)
-{
-	struct vpif_fh *fh = vb2_get_drv_priv(vq);
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common;
-
-	common = &ch->common[VPIF_VIDEO_INDEX];
-	mutex_unlock(&common->lock);
-}
-
-static void vpif_wait_finish(struct vb2_queue *vq)
-{
-	struct vpif_fh *fh = vb2_get_drv_priv(vq);
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common;
-
-	common = &ch->common[VPIF_VIDEO_INDEX];
-	mutex_lock(&common->lock);
-}
-
-static int vpif_buffer_init(struct vb2_buffer *vb)
-{
-	struct vpif_disp_buffer *buf = container_of(vb,
-					struct vpif_disp_buffer, vb);
-
-	INIT_LIST_HEAD(&buf->list);
-
-	return 0;
-}
-
-static u8 channel_first_int[VPIF_NUMOBJECTS][2] = { {1, 1} };
-
 static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
 {
 	struct vpif_display_config *vpif_config_data =
 					vpif_dev->platform_data;
-	struct vpif_fh *fh = vb2_get_drv_priv(vq);
-	struct channel_obj *ch = fh->channel;
+	struct channel_obj *ch = vb2_get_drv_priv(vq);
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
 	struct vpif_params *vpif = &ch->vpifparams;
-	unsigned long addr = 0;
-	unsigned long flags;
+	struct vpif_disp_buffer *buf, *tmp;
+	unsigned long addr, flags;
 	int ret;
 
 	spin_lock_irqsave(&common->irqlock, flags);
 
-	/* Get the next frame from the buffer queue */
-	common->next_frm = common->cur_frm =
-			    list_entry(common->dma_queue.next,
-				       struct vpif_disp_buffer, list);
-
-	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;
-
-	/* Initialize field_id and started member */
+	/* Initialize field_id */
 	ch->field_id = 0;
-	common->started = 1;
-	addr = vb2_dma_contig_plane_dma_addr(&common->cur_frm->vb, 0);
-	/* Calculate the offset for Y and C data  in the buffer */
-	vpif_calculate_offsets(ch);
-
-	if ((ch->vpifparams.std_info.frm_fmt &&
-		((common->fmt.fmt.pix.field != V4L2_FIELD_NONE)
-		&& (common->fmt.fmt.pix.field != V4L2_FIELD_ANY)))
-		|| (!ch->vpifparams.std_info.frm_fmt
-		&& (common->fmt.fmt.pix.field == V4L2_FIELD_NONE))) {
-		vpif_err("conflict in field format and std format\n");
-		return -EINVAL;
-	}
 
 	/* clock settings */
 	if (vpif_config_data->set_clock) {
@@ -268,24 +178,37 @@ static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
 		ycmux_mode, ch->vpifparams.std_info.hd_sd);
 		if (ret < 0) {
 			vpif_err("can't set clock\n");
-			return ret;
+			goto err;
 		}
 	}
 
 	/* set the parameters and addresses */
 	ret = vpif_set_video_params(vpif, ch->channel_id + 2);
 	if (ret < 0)
-		return ret;
+		goto err;
 
-	common->started = ret;
+	ycmux_mode = ret;
 	vpif_config_addr(ch, ret);
+	/* Get the next frame from the buffer queue */
+	common->next_frm = common->cur_frm =
+			    list_entry(common->dma_queue.next,
+				       struct vpif_disp_buffer, list);
+
+	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),
 			    (addr + common->ybtm_off),
 			    (addr + common->ctop_off),
 			    (addr + common->cbtm_off));
 
-	/* Set interrupt for both the fields in VPIF
-	    Register enable channel in VPIF register */
+	/*
+	 * Set interrupt for both the fields in VPIF
+	 * Register enable channel in VPIF register
+	 */
 	channel_first_int[VPIF_VIDEO_INDEX][ch->channel_id] = 1;
 	if (VPIF_CHANNEL2_VIDEO == ch->channel_id) {
 		channel2_intr_assert();
@@ -295,8 +218,7 @@ static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
 			channel2_clipping_enable(1);
 	}
 
-	if ((VPIF_CHANNEL3_VIDEO == ch->channel_id)
-		|| (common->started == 2)) {
+	if (VPIF_CHANNEL3_VIDEO == ch->channel_id || ycmux_mode == 2) {
 		channel3_intr_assert();
 		channel3_intr_enable(1);
 		enable_channel3(1);
@@ -305,23 +227,54 @@ static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
 	}
 
 	return 0;
+
+err:
+	list_for_each_entry_safe(buf, tmp, &common->dma_queue, list) {
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_QUEUED);
+	}
+
+	return ret;
 }
 
-/* abort streaming and wait for last buffer */
-static int vpif_stop_streaming(struct vb2_queue *vq)
+/**
+ * vpif_stop_streaming : Stop the DMA engine
+ * @vq: ptr to vb2_queue
+ *
+ * This callback stops the DMA engine and any remaining buffers
+ * in the DMA queue are released.
+ */
+static void vpif_stop_streaming(struct vb2_queue *vq)
 {
-	struct vpif_fh *fh = vb2_get_drv_priv(vq);
-	struct channel_obj *ch = fh->channel;
+	struct channel_obj *ch = vb2_get_drv_priv(vq);
 	struct common_obj *common;
 	unsigned long flags;
 
-	if (!vb2_is_streaming(vq))
-		return 0;
-
 	common = &ch->common[VPIF_VIDEO_INDEX];
 
+	/* Disable channel */
+	if (VPIF_CHANNEL2_VIDEO == ch->channel_id) {
+		enable_channel2(0);
+		channel2_intr_enable(0);
+	}
+	if (VPIF_CHANNEL3_VIDEO == ch->channel_id || ycmux_mode == 2) {
+		enable_channel3(0);
+		channel3_intr_enable(0);
+	}
+
 	/* release all active buffers */
 	spin_lock_irqsave(&common->irqlock, flags);
+	if (common->cur_frm == common->next_frm) {
+		vb2_buffer_done(&common->cur_frm->vb, VB2_BUF_STATE_ERROR);
+	} else {
+		if (common->cur_frm != NULL)
+			vb2_buffer_done(&common->cur_frm->vb,
+					VB2_BUF_STATE_ERROR);
+		if (common->next_frm != NULL)
+			vb2_buffer_done(&common->next_frm->vb,
+					VB2_BUF_STATE_ERROR);
+	}
+
 	while (!list_empty(&common->dma_queue)) {
 		common->next_frm = list_entry(common->dma_queue.next,
 						struct vpif_disp_buffer, list);
@@ -329,19 +282,15 @@ static int vpif_stop_streaming(struct vb2_queue *vq)
 		vb2_buffer_done(&common->next_frm->vb, VB2_BUF_STATE_ERROR);
 	}
 	spin_unlock_irqrestore(&common->irqlock, flags);
-
-	return 0;
 }
 
 static struct vb2_ops video_qops = {
 	.queue_setup		= vpif_buffer_queue_setup,
-	.wait_prepare		= vpif_wait_prepare,
-	.wait_finish		= vpif_wait_finish,
-	.buf_init		= vpif_buffer_init,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
 	.buf_prepare		= vpif_buffer_prepare,
 	.start_streaming	= vpif_start_streaming,
 	.stop_streaming		= vpif_stop_streaming,
-	.buf_cleanup		= vpif_buf_cleanup,
 	.buf_queue		= vpif_buffer_queue,
 };
 
@@ -423,8 +372,6 @@ static irqreturn_t vpif_channel_isr(int irq, void *dev_id)
 	for (i = 0; i < VPIF_NUMOBJECTS; i++) {
 		common = &ch->common[i];
 		/* If streaming is started in this channel */
-		if (0 == common->started)
-			continue;
 
 		if (1 == ch->vpifparams.std_info.frm_fmt) {
 			spin_lock(&common->irqlock);
@@ -520,6 +467,7 @@ static int vpif_update_resolution(struct channel_obj *ch)
 			return -EINVAL;
 	}
 
+	common->fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV422P;
 	common->fmt.fmt.pix.width = std_info->width;
 	common->fmt.fmt.pix.height = std_info->height;
 	vpif_dbg(1, debug, "Pixel details: Width = %d,Height = %d\n",
@@ -528,6 +476,17 @@ static int vpif_update_resolution(struct channel_obj *ch)
 	/* Set height and width paramateres */
 	common->height = std_info->height;
 	common->width = std_info->width;
+	common->fmt.fmt.pix.sizeimage = common->height * common->width * 2;
+
+	if (vid_ch->stdid)
+		common->fmt.fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	else
+		common->fmt.fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
+
+	if (ch->vpifparams.std_info.frm_fmt)
+		common->fmt.fmt.pix.field = V4L2_FIELD_NONE;
+	else
+		common->fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
 
 	return 0;
 }
@@ -598,70 +557,6 @@ static void vpif_calculate_offsets(struct channel_obj *ch)
 	ch->vpifparams.video_params.stdid = ch->vpifparams.std_info.stdid;
 }
 
-static void vpif_config_format(struct channel_obj *ch)
-{
-	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-
-	common->fmt.fmt.pix.field = V4L2_FIELD_ANY;
-	if (config_params.numbuffers[ch->channel_id] == 0)
-		common->memory = V4L2_MEMORY_USERPTR;
-	else
-		common->memory = V4L2_MEMORY_MMAP;
-
-	common->fmt.fmt.pix.sizeimage =
-			config_params.channel_bufsize[ch->channel_id];
-	common->fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV422P;
-	common->fmt.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
-}
-
-static int vpif_check_format(struct channel_obj *ch,
-			     struct v4l2_pix_format *pixfmt)
-{
-	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-	enum v4l2_field field = pixfmt->field;
-	u32 sizeimage, hpitch, vpitch;
-
-	if (pixfmt->pixelformat != V4L2_PIX_FMT_YUV422P)
-		goto invalid_fmt_exit;
-
-	if (!(VPIF_VALID_FIELD(field)))
-		goto invalid_fmt_exit;
-
-	if (pixfmt->bytesperline <= 0)
-		goto invalid_pitch_exit;
-
-	sizeimage = pixfmt->sizeimage;
-
-	if (vpif_update_resolution(ch))
-		return -EINVAL;
-
-	hpitch = pixfmt->bytesperline;
-	vpitch = sizeimage / (hpitch * 2);
-
-	/* Check for valid value of pitch */
-	if ((hpitch < ch->vpifparams.std_info.width) ||
-	    (vpitch < ch->vpifparams.std_info.height))
-		goto invalid_pitch_exit;
-
-	/* Check for 8 byte alignment */
-	if (!ISALIGNED(hpitch)) {
-		vpif_err("invalid pitch alignment\n");
-		return -EINVAL;
-	}
-	pixfmt->width = common->fmt.fmt.pix.width;
-	pixfmt->height = common->fmt.fmt.pix.height;
-
-	return 0;
-
-invalid_fmt_exit:
-	vpif_err("invalid field format\n");
-	return -EINVAL;
-
-invalid_pitch_exit:
-	vpif_err("invalid pitch\n");
-	return -EINVAL;
-}
-
 static void vpif_config_addr(struct channel_obj *ch, int muxmode)
 {
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
@@ -676,139 +571,6 @@ static void vpif_config_addr(struct channel_obj *ch, int muxmode)
 	}
 }
 
-/*
- * vpif_mmap: It is used to map kernel space buffers into user spaces
- */
-static int vpif_mmap(struct file *filep, struct vm_area_struct *vma)
-{
-	struct vpif_fh *fh = filep->private_data;
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common = &(ch->common[VPIF_VIDEO_INDEX]);
-	int ret;
-
-	vpif_dbg(2, debug, "vpif_mmap\n");
-
-	if (mutex_lock_interruptible(&common->lock))
-		return -ERESTARTSYS;
-	ret = vb2_mmap(&common->buffer_queue, vma);
-	mutex_unlock(&common->lock);
-	return ret;
-}
-
-/*
- * vpif_poll: It is used for select/poll system call
- */
-static unsigned int vpif_poll(struct file *filep, poll_table *wait)
-{
-	struct vpif_fh *fh = filep->private_data;
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-	unsigned int res = 0;
-
-	if (common->started) {
-		mutex_lock(&common->lock);
-		res = vb2_poll(&common->buffer_queue, filep, wait);
-		mutex_unlock(&common->lock);
-	}
-
-	return res;
-}
-
-/*
- * vpif_open: It creates object of file handle structure and stores it in
- * private_data member of filepointer
- */
-static int vpif_open(struct file *filep)
-{
-	struct video_device *vdev = video_devdata(filep);
-	struct channel_obj *ch = video_get_drvdata(vdev);
-	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-	struct vpif_fh *fh;
-
-	/* Allocate memory for the file handle object */
-	fh = kzalloc(sizeof(struct vpif_fh), GFP_KERNEL);
-	if (fh == NULL) {
-		vpif_err("unable to allocate memory for file handle object\n");
-		return -ENOMEM;
-	}
-
-	if (mutex_lock_interruptible(&common->lock)) {
-		kfree(fh);
-		return -ERESTARTSYS;
-	}
-	/* store pointer to fh in private_data member of filep */
-	filep->private_data = fh;
-	fh->channel = ch;
-	fh->initialized = 0;
-	if (!ch->initialized) {
-		fh->initialized = 1;
-		ch->initialized = 1;
-		memset(&ch->vpifparams, 0, sizeof(ch->vpifparams));
-	}
-
-	/* Increment channel usrs counter */
-	atomic_inc(&ch->usrs);
-	/* Set io_allowed[VPIF_VIDEO_INDEX] member to false */
-	fh->io_allowed[VPIF_VIDEO_INDEX] = 0;
-	/* Initialize priority of this instance to default priority */
-	fh->prio = V4L2_PRIORITY_UNSET;
-	v4l2_prio_open(&ch->prio, &fh->prio);
-	mutex_unlock(&common->lock);
-
-	return 0;
-}
-
-/*
- * vpif_release: This function deletes buffer queue, frees the buffers and
- * the vpif file handle
- */
-static int vpif_release(struct file *filep)
-{
-	struct vpif_fh *fh = filep->private_data;
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-
-	mutex_lock(&common->lock);
-	/* if this instance is doing IO */
-	if (fh->io_allowed[VPIF_VIDEO_INDEX]) {
-		/* Reset io_usrs member of channel object */
-		common->io_usrs = 0;
-		/* Disable channel */
-		if (VPIF_CHANNEL2_VIDEO == ch->channel_id) {
-			enable_channel2(0);
-			channel2_intr_enable(0);
-		}
-		if ((VPIF_CHANNEL3_VIDEO == ch->channel_id) ||
-		    (2 == common->started)) {
-			enable_channel3(0);
-			channel3_intr_enable(0);
-		}
-		common->started = 0;
-
-		/* Free buffers allocated */
-		vb2_queue_release(&common->buffer_queue);
-		vb2_dma_contig_cleanup_ctx(common->alloc_ctx);
-
-		common->numbuffers =
-		    config_params.numbuffers[ch->channel_id];
-	}
-
-	/* Decrement channel usrs counter */
-	atomic_dec(&ch->usrs);
-	/* If this file handle has initialize encoder device, reset it */
-	if (fh->initialized)
-		ch->initialized = 0;
-
-	/* Close the priority */
-	v4l2_prio_close(&ch->prio, fh->prio);
-	filep->private_data = NULL;
-	fh->initialized = 0;
-	mutex_unlock(&common->lock);
-	kfree(fh);
-
-	return 0;
-}
-
 /* functions implementing ioctls */
 /**
  * vpif_querycap() - QUERYCAP handler
@@ -823,7 +585,7 @@ static int vpif_querycap(struct file *file, void  *priv,
 
 	cap->device_caps = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
 	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
-	snprintf(cap->driver, sizeof(cap->driver), "%s", dev_name(vpif_dev));
+	strlcpy(cap->driver, VPIF_DRIVER_NAME, sizeof(cap->driver));
 	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
 		 dev_name(vpif_dev));
 	strlcpy(cap->card, config->card_name, sizeof(cap->card));
@@ -834,24 +596,22 @@ static int vpif_querycap(struct file *file, void  *priv,
 static int vpif_enum_fmt_vid_out(struct file *file, void  *priv,
 					struct v4l2_fmtdesc *fmt)
 {
-	if (fmt->index != 0) {
-		vpif_err("Invalid format index\n");
+	if (fmt->index != 0)
 		return -EINVAL;
-	}
 
 	/* Fill in the information about format */
 	fmt->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
 	strcpy(fmt->description, "YCbCr4:2:2 YC Planar");
 	fmt->pixelformat = V4L2_PIX_FMT_YUV422P;
-
+	fmt->flags = 0;
 	return 0;
 }
 
 static int vpif_g_fmt_vid_out(struct file *file, void *priv,
 				struct v4l2_format *fmt)
 {
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
 
 	/* Check the validity of the buffer type */
@@ -864,193 +624,84 @@ static int vpif_g_fmt_vid_out(struct file *file, void *priv,
 	return 0;
 }
 
-static int vpif_s_fmt_vid_out(struct file *file, void *priv,
+static int vpif_try_fmt_vid_out(struct file *file, void *priv,
 				struct v4l2_format *fmt)
 {
-	struct vpif_fh *fh = priv;
-	struct v4l2_pix_format *pixfmt;
-	struct channel_obj *ch = fh->channel;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-	int ret = 0;
-
-	if ((VPIF_CHANNEL2_VIDEO == ch->channel_id)
-	    || (VPIF_CHANNEL3_VIDEO == ch->channel_id)) {
-		if (!fh->initialized) {
-			vpif_dbg(1, debug, "Channel Busy\n");
-			return -EBUSY;
-		}
+	struct v4l2_pix_format *pixfmt = &fmt->fmt.pix;
 
-		/* Check for the priority */
-		ret = v4l2_prio_check(&ch->prio, fh->prio);
-		if (0 != ret)
-			return ret;
-		fh->initialized = 1;
-	}
+	/*
+	 * to supress v4l-compliance warnings silently correct
+	 * the pixelformat
+	 */
+	if (pixfmt->pixelformat != V4L2_PIX_FMT_YUV422P)
+		pixfmt->pixelformat = common->fmt.fmt.pix.pixelformat;
 
-	if (common->started) {
-		vpif_dbg(1, debug, "Streaming in progress\n");
-		return -EBUSY;
-	}
+	if (vpif_update_resolution(ch))
+		return -EINVAL;
 
-	pixfmt = &fmt->fmt.pix;
-	/* Check for valid field format */
-	ret = vpif_check_format(ch, pixfmt);
-	if (ret)
-		return ret;
+	pixfmt->colorspace = common->fmt.fmt.pix.colorspace;
+	pixfmt->field = common->fmt.fmt.pix.field;
+	pixfmt->bytesperline = common->fmt.fmt.pix.width;
+	pixfmt->width = common->fmt.fmt.pix.width;
+	pixfmt->height = common->fmt.fmt.pix.height;
+	pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height * 2;
+	pixfmt->priv = 0;
 
-	/* store the pix format in the channel object */
-	common->fmt.fmt.pix = *pixfmt;
-	/* store the format in the channel object */
-	common->fmt = *fmt;
 	return 0;
 }
 
-static int vpif_try_fmt_vid_out(struct file *file, void *priv,
+static int vpif_s_fmt_vid_out(struct file *file, void *priv,
 				struct v4l2_format *fmt)
 {
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
 	struct v4l2_pix_format *pixfmt = &fmt->fmt.pix;
-	int ret = 0;
-
-	ret = vpif_check_format(ch, pixfmt);
-	if (ret) {
-		*pixfmt = common->fmt.fmt.pix;
-		pixfmt->sizeimage = pixfmt->width * pixfmt->height * 2;
-	}
-
-	return ret;
-}
-
-static int vpif_reqbufs(struct file *file, void *priv,
-			struct v4l2_requestbuffers *reqbuf)
-{
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common;
-	enum v4l2_field field;
-	struct vb2_queue *q;
-	u8 index = 0;
 	int ret;
 
-	/* This file handle has not initialized the channel,
-	   It is not allowed to do settings */
-	if ((VPIF_CHANNEL2_VIDEO == ch->channel_id)
-	    || (VPIF_CHANNEL3_VIDEO == ch->channel_id)) {
-		if (!fh->initialized) {
-			vpif_err("Channel Busy\n");
-			return -EBUSY;
-		}
-	}
-
-	if (V4L2_BUF_TYPE_VIDEO_OUTPUT != reqbuf->type)
-		return -EINVAL;
-
-	index = VPIF_VIDEO_INDEX;
-
-	common = &ch->common[index];
-
-	if (common->fmt.type != reqbuf->type || !vpif_dev)
-		return -EINVAL;
-	if (0 != common->io_usrs)
+	if (vb2_is_busy(&common->buffer_queue))
 		return -EBUSY;
 
-	if (reqbuf->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
-		if (common->fmt.fmt.pix.field == V4L2_FIELD_ANY)
-			field = V4L2_FIELD_INTERLACED;
-		else
-			field = common->fmt.fmt.pix.field;
-	} else {
-		field = V4L2_VBI_INTERLACED;
-	}
-	/* Initialize videobuf2 queue as per the buffer type */
-	common->alloc_ctx = vb2_dma_contig_init_ctx(vpif_dev);
-	if (IS_ERR(common->alloc_ctx)) {
-		vpif_err("Failed to get the context\n");
-		return PTR_ERR(common->alloc_ctx);
-	}
-	q = &common->buffer_queue;
-	q->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
-	q->io_modes = VB2_MMAP | VB2_USERPTR;
-	q->drv_priv = fh;
-	q->ops = &video_qops;
-	q->mem_ops = &vb2_dma_contig_memops;
-	q->buf_struct_size = sizeof(struct vpif_disp_buffer);
-	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
-	q->min_buffers_needed = 1;
-
-	ret = vb2_queue_init(q);
-	if (ret) {
-		vpif_err("vpif_display: vb2_queue_init() failed\n");
-		vb2_dma_contig_cleanup_ctx(common->alloc_ctx);
+	ret = vpif_try_fmt_vid_out(file, priv, fmt);
+	if (ret)
 		return ret;
-	}
-	/* Set io allowed member of file handle to TRUE */
-	fh->io_allowed[index] = 1;
-	/* Increment io usrs member of channel object to 1 */
-	common->io_usrs = 1;
-	/* Store type of memory requested in channel object */
-	common->memory = reqbuf->memory;
-	INIT_LIST_HEAD(&common->dma_queue);
-	/* Allocate buffers */
-	return vb2_reqbufs(&common->buffer_queue, reqbuf);
-}
-
-static int vpif_querybuf(struct file *file, void *priv,
-				struct v4l2_buffer *tbuf)
-{
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
 
-	if (common->fmt.type != tbuf->type)
-		return -EINVAL;
+	/* store the pix format in the channel object */
+	common->fmt.fmt.pix = *pixfmt;
 
-	return vb2_querybuf(&common->buffer_queue, tbuf);
+	/* store the format in the channel object */
+	common->fmt = *fmt;
+	return 0;
 }
 
-static int vpif_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+static int vpif_s_std(struct file *file, void *priv, v4l2_std_id std_id)
 {
-	struct vpif_fh *fh = NULL;
-	struct channel_obj *ch = NULL;
-	struct common_obj *common = NULL;
-
-	if (!buf || !priv)
-		return -EINVAL;
-
-	fh = priv;
-	ch = fh->channel;
-	if (!ch)
-		return -EINVAL;
+	struct vpif_display_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+	struct vpif_display_chan_config *chan_cfg;
+	struct v4l2_output output;
+	int ret;
 
-	common = &(ch->common[VPIF_VIDEO_INDEX]);
-	if (common->fmt.type != buf->type)
-		return -EINVAL;
+	if (config->chan_config[ch->channel_id].outputs == NULL)
+		return -ENODATA;
 
-	if (!fh->io_allowed[VPIF_VIDEO_INDEX]) {
-		vpif_err("fh->io_allowed\n");
-		return -EACCES;
-	}
+	chan_cfg = &config->chan_config[ch->channel_id];
+	output = chan_cfg->outputs[ch->output_idx].output;
+	if (output.capabilities != V4L2_OUT_CAP_STD)
+		return -ENODATA;
 
-	return vb2_qbuf(&common->buffer_queue, buf);
-}
+	if (vb2_is_busy(&common->buffer_queue))
+		return -EBUSY;
 
-static int vpif_s_std(struct file *file, void *priv, v4l2_std_id std_id)
-{
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-	int ret = 0;
 
 	if (!(std_id & VPIF_V4L2_STD))
 		return -EINVAL;
 
-	if (common->started) {
-		vpif_err("streaming in progress\n");
-		return -EBUSY;
-	}
-
 	/* Call encoder subdevice function to set the standard */
 	ch->video.stdid = std_id;
 	memset(&ch->video.dv_timings, 0, sizeof(ch->video.dv_timings));
@@ -1058,16 +709,7 @@ static int vpif_s_std(struct file *file, void *priv, v4l2_std_id std_id)
 	if (vpif_update_resolution(ch))
 		return -EINVAL;
 
-	if ((ch->vpifparams.std_info.width *
-		ch->vpifparams.std_info.height * 2) >
-		config_params.channel_bufsize[ch->channel_id]) {
-		vpif_err("invalid std for this size\n");
-		return -EINVAL;
-	}
-
 	common->fmt.fmt.pix.bytesperline = common->fmt.fmt.pix.width;
-	/* Configure the default format information */
-	vpif_config_format(ch);
 
 	ret = v4l2_device_call_until_err(&vpif_obj.v4l2_dev, 1, video,
 						s_std_output, std_id);
@@ -1076,7 +718,7 @@ static int vpif_s_std(struct file *file, void *priv, v4l2_std_id std_id)
 		return ret;
 	}
 
-	ret = v4l2_device_call_until_err(&vpif_obj.v4l2_dev, 1, core,
+	ret = v4l2_device_call_until_err(&vpif_obj.v4l2_dev, 1, video,
 							s_std, std_id);
 	if (ret < 0)
 		vpif_err("Failed to set standard for sub devices\n");
@@ -1085,132 +727,21 @@ static int vpif_s_std(struct file *file, void *priv, v4l2_std_id std_id)
 
 static int vpif_g_std(struct file *file, void *priv, v4l2_std_id *std)
 {
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
-
-	*std = ch->video.stdid;
-	return 0;
-}
-
-static int vpif_dqbuf(struct file *file, void *priv, struct v4l2_buffer *p)
-{
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-
-	return vb2_dqbuf(&common->buffer_queue, p,
-					(file->f_flags & O_NONBLOCK));
-}
-
-static int vpif_streamon(struct file *file, void *priv,
-				enum v4l2_buf_type buftype)
-{
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-	struct channel_obj *oth_ch = vpif_obj.dev[!ch->channel_id];
-	int ret = 0;
-
-	if (buftype != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
-		vpif_err("buffer type not supported\n");
-		return -EINVAL;
-	}
-
-	if (!fh->io_allowed[VPIF_VIDEO_INDEX]) {
-		vpif_err("fh->io_allowed\n");
-		return -EACCES;
-	}
-
-	/* If Streaming is already started, return error */
-	if (common->started) {
-		vpif_err("channel->started\n");
-		return -EBUSY;
-	}
-
-	if ((ch->channel_id == VPIF_CHANNEL2_VIDEO
-		&& oth_ch->common[VPIF_VIDEO_INDEX].started &&
-		ch->vpifparams.std_info.ycmux_mode == 0)
-		|| ((ch->channel_id == VPIF_CHANNEL3_VIDEO)
-		&& (2 == oth_ch->common[VPIF_VIDEO_INDEX].started))) {
-		vpif_err("other channel is using\n");
-		return -EBUSY;
-	}
-
-	ret = vpif_check_format(ch, &common->fmt.fmt.pix);
-	if (ret < 0)
-		return ret;
-
-	/* Call vb2_streamon to start streaming in videobuf2 */
-	ret = vb2_streamon(&common->buffer_queue, buftype);
-	if (ret < 0) {
-		vpif_err("vb2_streamon\n");
-		return ret;
-	}
-
-	return ret;
-}
-
-static int vpif_streamoff(struct file *file, void *priv,
-				enum v4l2_buf_type buftype)
-{
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-	struct vpif_display_config *vpif_config_data =
-					vpif_dev->platform_data;
-
-	if (buftype != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
-		vpif_err("buffer type not supported\n");
-		return -EINVAL;
-	}
-
-	if (!fh->io_allowed[VPIF_VIDEO_INDEX]) {
-		vpif_err("fh->io_allowed\n");
-		return -EACCES;
-	}
-
-	if (!common->started) {
-		vpif_err("channel->started\n");
-		return -EINVAL;
-	}
-
-	if (buftype == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
-		/* disable channel */
-		if (VPIF_CHANNEL2_VIDEO == ch->channel_id) {
-			if (vpif_config_data->
-				chan_config[VPIF_CHANNEL2_VIDEO].clip_en)
-				channel2_clipping_enable(0);
-			enable_channel2(0);
-			channel2_intr_enable(0);
-		}
-		if ((VPIF_CHANNEL3_VIDEO == ch->channel_id) ||
-					(2 == common->started)) {
-			if (vpif_config_data->
-				chan_config[VPIF_CHANNEL3_VIDEO].clip_en)
-				channel3_clipping_enable(0);
-			enable_channel3(0);
-			channel3_intr_enable(0);
-		}
-	}
-
-	common->started = 0;
-	return vb2_streamoff(&common->buffer_queue, buftype);
-}
+	struct vpif_display_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct vpif_display_chan_config *chan_cfg;
+	struct v4l2_output output;
 
-static int vpif_cropcap(struct file *file, void *priv,
-			struct v4l2_cropcap *crop)
-{
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-	if (V4L2_BUF_TYPE_VIDEO_OUTPUT != crop->type)
-		return -EINVAL;
+	if (config->chan_config[ch->channel_id].outputs == NULL)
+		return -ENODATA;
 
-	crop->bounds.left = crop->bounds.top = 0;
-	crop->defrect.left = crop->defrect.top = 0;
-	crop->defrect.height = crop->bounds.height = common->height;
-	crop->defrect.width = crop->bounds.width = common->width;
+	chan_cfg = &config->chan_config[ch->channel_id];
+	output = chan_cfg->outputs[ch->output_idx].output;
+	if (output.capabilities != V4L2_OUT_CAP_STD)
+		return -ENODATA;
 
+	*std = ch->video.stdid;
 	return 0;
 }
 
@@ -1219,9 +750,9 @@ static int vpif_enum_output(struct file *file, void *fh,
 {
 
 	struct vpif_display_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
 	struct vpif_display_chan_config *chan_cfg;
-	struct vpif_fh *vpif_handler = fh;
-	struct channel_obj *ch = vpif_handler->channel;
 
 	chan_cfg = &config->chan_config[ch->channel_id];
 	if (output->index >= chan_cfg->output_count) {
@@ -1315,52 +846,32 @@ static int vpif_set_output(struct vpif_display_config *vpif_cfg,
 static int vpif_s_output(struct file *file, void *priv, unsigned int i)
 {
 	struct vpif_display_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
 	struct vpif_display_chan_config *chan_cfg;
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
 
+	if (vb2_is_busy(&common->buffer_queue))
+		return -EBUSY;
+
 	chan_cfg = &config->chan_config[ch->channel_id];
 
 	if (i >= chan_cfg->output_count)
 		return -EINVAL;
 
-	if (common->started) {
-		vpif_err("Streaming in progress\n");
-		return -EBUSY;
-	}
-
 	return vpif_set_output(config, ch, i);
 }
 
 static int vpif_g_output(struct file *file, void *priv, unsigned int *i)
 {
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
 
 	*i = ch->output_idx;
 
 	return 0;
 }
 
-static int vpif_g_priority(struct file *file, void *priv, enum v4l2_priority *p)
-{
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
-
-	*p = v4l2_prio_max(&ch->prio);
-
-	return 0;
-}
-
-static int vpif_s_priority(struct file *file, void *priv, enum v4l2_priority p)
-{
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
-
-	return v4l2_prio_change(&ch->prio, &fh->prio, p);
-}
-
 /**
  * vpif_enum_dv_timings() - ENUM_DV_TIMINGS handler
  * @file: file ptr
@@ -1371,11 +882,24 @@ static int
 vpif_enum_dv_timings(struct file *file, void *priv,
 		     struct v4l2_enum_dv_timings *timings)
 {
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
+	struct vpif_display_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct vpif_display_chan_config *chan_cfg;
+	struct v4l2_output output;
 	int ret;
 
-	ret = v4l2_subdev_call(ch->sd, video, enum_dv_timings, timings);
+	if (config->chan_config[ch->channel_id].outputs == NULL)
+		return -ENODATA;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+	output = chan_cfg->outputs[ch->output_idx].output;
+	if (output.capabilities != V4L2_OUT_CAP_DV_TIMINGS)
+		return -ENODATA;
+
+	timings->pad = 0;
+
+	ret = v4l2_subdev_call(ch->sd, pad, enum_dv_timings, timings);
 	if (ret == -ENOIOCTLCMD || ret == -ENODEV)
 		return -EINVAL;
 	return ret;
@@ -1390,14 +914,29 @@ vpif_enum_dv_timings(struct file *file, void *priv,
 static int vpif_s_dv_timings(struct file *file, void *priv,
 		struct v4l2_dv_timings *timings)
 {
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
+	struct vpif_display_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
 	struct vpif_params *vpifparams = &ch->vpifparams;
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
 	struct vpif_channel_config_params *std_info = &vpifparams->std_info;
 	struct video_obj *vid_ch = &ch->video;
 	struct v4l2_bt_timings *bt = &vid_ch->dv_timings.bt;
+	struct vpif_display_chan_config *chan_cfg;
+	struct v4l2_output output;
 	int ret;
 
+	if (config->chan_config[ch->channel_id].outputs == NULL)
+		return -ENODATA;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+	output = chan_cfg->outputs[ch->output_idx].output;
+	if (output.capabilities != V4L2_OUT_CAP_DV_TIMINGS)
+		return -ENODATA;
+
+	if (vb2_is_busy(&common->buffer_queue))
+		return -EBUSY;
+
 	if (timings->type != V4L2_DV_BT_656_1120) {
 		vpif_dbg(2, debug, "Timing type not defined\n");
 		return -EINVAL;
@@ -1479,13 +1018,27 @@ static int vpif_s_dv_timings(struct file *file, void *priv,
 static int vpif_g_dv_timings(struct file *file, void *priv,
 		struct v4l2_dv_timings *timings)
 {
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
+	struct vpif_display_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct vpif_display_chan_config *chan_cfg;
 	struct video_obj *vid_ch = &ch->video;
+	struct v4l2_output output;
+
+	if (config->chan_config[ch->channel_id].outputs == NULL)
+		goto error;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+	output = chan_cfg->outputs[ch->output_idx].output;
+
+	if (output.capabilities != V4L2_OUT_CAP_DV_TIMINGS)
+		goto error;
 
 	*timings = vid_ch->dv_timings;
 
 	return 0;
+error:
+	return -ENODATA;
 }
 
 /*
@@ -1505,83 +1058,49 @@ static int vpif_log_status(struct file *filep, void *priv)
 
 /* vpif display ioctl operations */
 static const struct v4l2_ioctl_ops vpif_ioctl_ops = {
-	.vidioc_querycap        	= vpif_querycap,
-	.vidioc_g_priority		= vpif_g_priority,
-	.vidioc_s_priority		= vpif_s_priority,
+	.vidioc_querycap		= vpif_querycap,
 	.vidioc_enum_fmt_vid_out	= vpif_enum_fmt_vid_out,
-	.vidioc_g_fmt_vid_out  		= vpif_g_fmt_vid_out,
-	.vidioc_s_fmt_vid_out   	= vpif_s_fmt_vid_out,
-	.vidioc_try_fmt_vid_out 	= vpif_try_fmt_vid_out,
-	.vidioc_reqbufs         	= vpif_reqbufs,
-	.vidioc_querybuf        	= vpif_querybuf,
-	.vidioc_qbuf            	= vpif_qbuf,
-	.vidioc_dqbuf           	= vpif_dqbuf,
-	.vidioc_streamon        	= vpif_streamon,
-	.vidioc_streamoff       	= vpif_streamoff,
-	.vidioc_s_std           	= vpif_s_std,
+	.vidioc_g_fmt_vid_out		= vpif_g_fmt_vid_out,
+	.vidioc_s_fmt_vid_out		= vpif_s_fmt_vid_out,
+	.vidioc_try_fmt_vid_out		= vpif_try_fmt_vid_out,
+
+	.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_expbuf			= vb2_ioctl_expbuf,
+	.vidioc_streamon		= vb2_ioctl_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
+
+	.vidioc_s_std			= vpif_s_std,
 	.vidioc_g_std			= vpif_g_std,
+
 	.vidioc_enum_output		= vpif_enum_output,
 	.vidioc_s_output		= vpif_s_output,
 	.vidioc_g_output		= vpif_g_output,
-	.vidioc_cropcap         	= vpif_cropcap,
-	.vidioc_enum_dv_timings         = vpif_enum_dv_timings,
-	.vidioc_s_dv_timings            = vpif_s_dv_timings,
-	.vidioc_g_dv_timings            = vpif_g_dv_timings,
+
+	.vidioc_enum_dv_timings		= vpif_enum_dv_timings,
+	.vidioc_s_dv_timings		= vpif_s_dv_timings,
+	.vidioc_g_dv_timings		= vpif_g_dv_timings,
+
 	.vidioc_log_status		= vpif_log_status,
 };
 
 static const struct v4l2_file_operations vpif_fops = {
 	.owner		= THIS_MODULE,
-	.open		= vpif_open,
-	.release	= vpif_release,
+	.open		= v4l2_fh_open,
+	.release	= vb2_fop_release,
 	.unlocked_ioctl	= video_ioctl2,
-	.mmap		= vpif_mmap,
-	.poll		= vpif_poll
-};
-
-static struct video_device vpif_video_template = {
-	.name		= "vpif",
-	.fops		= &vpif_fops,
-	.ioctl_ops	= &vpif_ioctl_ops,
+	.mmap		= vb2_fop_mmap,
+	.poll		= vb2_fop_poll
 };
 
 /*Configure the channels, buffer sizei, request irq */
 static int initialize_vpif(void)
 {
 	int free_channel_objects_index;
-	int free_buffer_channel_index;
-	int free_buffer_index;
-	int err = 0, i, j;
-
-	/* Default number of buffers should be 3 */
-	if ((ch2_numbuffers > 0) &&
-	    (ch2_numbuffers < config_params.min_numbuffers))
-		ch2_numbuffers = config_params.min_numbuffers;
-	if ((ch3_numbuffers > 0) &&
-	    (ch3_numbuffers < config_params.min_numbuffers))
-		ch3_numbuffers = config_params.min_numbuffers;
-
-	/* Set buffer size to min buffers size if invalid buffer size is
-	 * given */
-	if (ch2_bufsize < config_params.min_bufsize[VPIF_CHANNEL2_VIDEO])
-		ch2_bufsize =
-		    config_params.min_bufsize[VPIF_CHANNEL2_VIDEO];
-	if (ch3_bufsize < config_params.min_bufsize[VPIF_CHANNEL3_VIDEO])
-		ch3_bufsize =
-		    config_params.min_bufsize[VPIF_CHANNEL3_VIDEO];
-
-	config_params.numbuffers[VPIF_CHANNEL2_VIDEO] = ch2_numbuffers;
-
-	if (ch2_numbuffers) {
-		config_params.channel_bufsize[VPIF_CHANNEL2_VIDEO] =
-							ch2_bufsize;
-	}
-	config_params.numbuffers[VPIF_CHANNEL3_VIDEO] = ch3_numbuffers;
-
-	if (ch3_numbuffers) {
-		config_params.channel_bufsize[VPIF_CHANNEL3_VIDEO] =
-							ch3_bufsize;
-	}
+	int err, i, j;
 
 	/* Allocate memory for six channel objects */
 	for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) {
@@ -1595,10 +1114,6 @@ static int initialize_vpif(void)
 		}
 	}
 
-	free_channel_objects_index = VPIF_DISPLAY_MAX_DEVICES;
-	free_buffer_channel_index = VPIF_DISPLAY_NUM_CHANNELS;
-	free_buffer_index = config_params.numbuffers[i - 1];
-
 	return 0;
 
 vpif_init_free_channel_objects:
@@ -1627,21 +1142,18 @@ static int vpif_async_bound(struct v4l2_async_notifier *notifier,
 static int vpif_probe_complete(void)
 {
 	struct common_obj *common;
+	struct video_device *vdev;
 	struct channel_obj *ch;
+	struct vb2_queue *q;
 	int j, err, k;
 
 	for (j = 0; j < VPIF_DISPLAY_MAX_DEVICES; j++) {
 		ch = vpif_obj.dev[j];
 		/* Initialize field of the channel objects */
-		atomic_set(&ch->usrs, 0);
 		for (k = 0; k < VPIF_NUMOBJECTS; k++) {
-			ch->common[k].numbuffers = 0;
 			common = &ch->common[k];
-			common->io_usrs = 0;
-			common->started = 0;
 			spin_lock_init(&common->irqlock);
 			mutex_init(&common->lock);
-			common->numbuffers = 0;
 			common->set_addr = NULL;
 			common->ytop_off = 0;
 			common->ybtm_off = 0;
@@ -1650,38 +1162,71 @@ static int vpif_probe_complete(void)
 			common->cur_frm = NULL;
 			common->next_frm = NULL;
 			memset(&common->fmt, 0, sizeof(common->fmt));
-			common->numbuffers = config_params.numbuffers[k];
 		}
 		ch->initialized = 0;
 		if (vpif_obj.config->subdev_count)
 			ch->sd = vpif_obj.sd[0];
 		ch->channel_id = j;
-		if (j < 2)
-			ch->common[VPIF_VIDEO_INDEX].numbuffers =
-			    config_params.numbuffers[ch->channel_id];
-		else
-			ch->common[VPIF_VIDEO_INDEX].numbuffers = 0;
 
 		memset(&ch->vpifparams, 0, sizeof(ch->vpifparams));
 
-		/* Initialize prio member of channel object */
-		v4l2_prio_init(&ch->prio);
 		ch->common[VPIF_VIDEO_INDEX].fmt.type =
 						V4L2_BUF_TYPE_VIDEO_OUTPUT;
-		ch->video_dev->lock = &common->lock;
-		video_set_drvdata(ch->video_dev, ch);
 
 		/* select output 0 */
 		err = vpif_set_output(vpif_obj.config, ch, 0);
 		if (err)
 			goto probe_out;
 
+		/* set initial format */
+		ch->video.stdid = V4L2_STD_525_60;
+		memset(&ch->video.dv_timings, 0, sizeof(ch->video.dv_timings));
+		vpif_update_resolution(ch);
+
+		/* Initialize vb2 queue */
+		q = &common->buffer_queue;
+		q->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+		q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+		q->drv_priv = ch;
+		q->ops = &video_qops;
+		q->mem_ops = &vb2_dma_contig_memops;
+		q->buf_struct_size = sizeof(struct vpif_disp_buffer);
+		q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+		q->min_buffers_needed = 1;
+		q->lock = &common->lock;
+		err = vb2_queue_init(q);
+		if (err) {
+			vpif_err("vpif_display: vb2_queue_init() failed\n");
+			goto probe_out;
+		}
+
+		common->alloc_ctx = vb2_dma_contig_init_ctx(vpif_dev);
+		if (IS_ERR(common->alloc_ctx)) {
+			vpif_err("Failed to get the context\n");
+			err = PTR_ERR(common->alloc_ctx);
+			goto probe_out;
+		}
+
+		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);
 
-		err = video_register_device(ch->video_dev,
-					  VFL_TYPE_GRABBER, (j ? 3 : 2));
+		/* Initialize the video_device structure */
+		vdev = ch->video_dev;
+		strlcpy(vdev->name, VPIF_DRIVER_NAME, sizeof(vdev->name));
+		vdev->release = video_device_release;
+		vdev->fops = &vpif_fops;
+		vdev->ioctl_ops = &vpif_ioctl_ops;
+		vdev->v4l2_dev = &vpif_obj.v4l2_dev;
+		vdev->vfl_dir = VFL_DIR_TX;
+		vdev->queue = q;
+		vdev->lock = &common->lock;
+		set_bit(V4L2_FL_USE_FH_PRIO, &vdev->flags);
+		video_set_drvdata(ch->video_dev, ch);
+		err = video_register_device(vdev, VFL_TYPE_GRABBER,
+					    (j ? 3 : 2));
 		if (err < 0)
 			goto probe_out;
 	}
@@ -1691,6 +1236,8 @@ static int vpif_probe_complete(void)
 probe_out:
 	for (k = 0; k < j; k++) {
 		ch = vpif_obj.dev[k];
+		common = &ch->common[k];
+		vb2_dma_contig_cleanup_ctx(common->alloc_ctx);
 		video_unregister_device(ch->video_dev);
 		video_device_release(ch->video_dev);
 		ch->video_dev = NULL;
@@ -1717,7 +1264,6 @@ static __init int vpif_probe(struct platform_device *pdev)
 	struct video_device *vfd;
 	struct resource *res;
 	int subdev_count;
-	size_t size;
 
 	vpif_dev = &pdev->dev;
 	err = initialize_vpif();
@@ -1735,7 +1281,7 @@ static __init int vpif_probe(struct platform_device *pdev)
 
 	while ((res = platform_get_resource(pdev, IORESOURCE_IRQ, res_idx))) {
 		err = devm_request_irq(&pdev->dev, res->start, vpif_channel_isr,
-					IRQF_SHARED, "VPIF_Display",
+					IRQF_SHARED, VPIF_DRIVER_NAME,
 					(void *)(&vpif_obj.dev[res_idx]->
 					channel_id));
 		if (err) {
@@ -1761,36 +1307,10 @@ static __init int vpif_probe(struct platform_device *pdev)
 			goto vpif_unregister;
 		}
 
-		/* Initialize field of video device */
-		*vfd = vpif_video_template;
-		vfd->v4l2_dev = &vpif_obj.v4l2_dev;
-		vfd->release = video_device_release;
-		vfd->vfl_dir = VFL_DIR_TX;
-		snprintf(vfd->name, sizeof(vfd->name),
-			 "VPIF_Display_DRIVER_V%s",
-			 VPIF_DISPLAY_VERSION);
-
 		/* Set video_dev to the video device */
 		ch->video_dev = vfd;
 	}
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (res) {
-		size = resource_size(res);
-		/* The resources are divided into two equal memory and when
-		 * we have HD output we can add them together
-		 */
-		for (j = 0; j < VPIF_DISPLAY_MAX_DEVICES; j++) {
-			ch = vpif_obj.dev[j];
-			ch->channel_id = j;
-
-			/* only enabled if second resource exists */
-			config_params.video_limit[ch->channel_id] = 0;
-			if (size)
-				config_params.video_limit[ch->channel_id] =
-									size/2;
-		}
-	}
 	vpif_obj.config = pdev->dev.platform_data;
 	subdev_count = vpif_obj.config->subdev_count;
 	subdevdata = vpif_obj.config->subdevinfo;
@@ -1856,6 +1376,7 @@ vpif_unregister:
  */
 static int vpif_remove(struct platform_device *device)
 {
+	struct common_obj *common;
 	struct channel_obj *ch;
 	int i;
 
@@ -1866,6 +1387,8 @@ static int vpif_remove(struct platform_device *device)
 	for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) {
 		/* Get the pointer to the channel object */
 		ch = vpif_obj.dev[i];
+		common = &ch->common[i];
+		vb2_dma_contig_cleanup_ctx(common->alloc_ctx);
 		/* Unregister video device */
 		video_unregister_device(ch->video_dev);
 
@@ -1876,7 +1399,7 @@ static int vpif_remove(struct platform_device *device)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int vpif_suspend(struct device *dev)
 {
 	struct common_obj *common;
@@ -1887,18 +1410,20 @@ static int vpif_suspend(struct device *dev)
 		/* Get the pointer to the channel object */
 		ch = vpif_obj.dev[i];
 		common = &ch->common[VPIF_VIDEO_INDEX];
+
+		if (!vb2_is_streaming(&common->buffer_queue))
+			continue;
+
 		mutex_lock(&common->lock);
-		if (atomic_read(&ch->usrs) && common->io_usrs) {
-			/* Disable channel */
-			if (ch->channel_id == VPIF_CHANNEL2_VIDEO) {
-				enable_channel2(0);
-				channel2_intr_enable(0);
-			}
-			if (ch->channel_id == VPIF_CHANNEL3_VIDEO ||
-					common->started == 2) {
-				enable_channel3(0);
-				channel3_intr_enable(0);
-			}
+		/* Disable channel */
+		if (ch->channel_id == VPIF_CHANNEL2_VIDEO) {
+			enable_channel2(0);
+			channel2_intr_enable(0);
+		}
+		if (ch->channel_id == VPIF_CHANNEL3_VIDEO ||
+			ycmux_mode == 2) {
+			enable_channel3(0);
+			channel3_intr_enable(0);
 		}
 		mutex_unlock(&common->lock);
 	}
@@ -1917,18 +1442,20 @@ static int vpif_resume(struct device *dev)
 		/* Get the pointer to the channel object */
 		ch = vpif_obj.dev[i];
 		common = &ch->common[VPIF_VIDEO_INDEX];
+
+		if (!vb2_is_streaming(&common->buffer_queue))
+			continue;
+
 		mutex_lock(&common->lock);
-		if (atomic_read(&ch->usrs) && common->io_usrs) {
-			/* Enable channel */
-			if (ch->channel_id == VPIF_CHANNEL2_VIDEO) {
-				enable_channel2(1);
-				channel2_intr_enable(1);
-			}
-			if (ch->channel_id == VPIF_CHANNEL3_VIDEO ||
-					common->started == 2) {
-				enable_channel3(1);
-				channel3_intr_enable(1);
-			}
+		/* Enable channel */
+		if (ch->channel_id == VPIF_CHANNEL2_VIDEO) {
+			enable_channel2(1);
+			channel2_intr_enable(1);
+		}
+		if (ch->channel_id == VPIF_CHANNEL3_VIDEO ||
+				ycmux_mode == 2) {
+			enable_channel3(1);
+			channel3_intr_enable(1);
 		}
 		mutex_unlock(&common->lock);
 	}
@@ -1936,21 +1463,15 @@ static int vpif_resume(struct device *dev)
 	return 0;
 }
 
-static const struct dev_pm_ops vpif_pm = {
-	.suspend        = vpif_suspend,
-	.resume         = vpif_resume,
-};
-
-#define vpif_pm_ops (&vpif_pm)
-#else
-#define vpif_pm_ops NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(vpif_pm_ops, vpif_suspend, vpif_resume);
+
 static __refdata struct platform_driver vpif_driver = {
 	.driver	= {
-			.name	= "vpif_display",
+			.name	= VPIF_DRIVER_NAME,
 			.owner	= THIS_MODULE,
-			.pm	= vpif_pm_ops,
+			.pm	= &vpif_pm_ops,
 	},
 	.probe	= vpif_probe,
 	.remove	= vpif_remove,
diff --git a/drivers/media/platform/davinci/vpif_display.h b/drivers/media/platform/davinci/vpif_display.h
index 4d0485b99a8..7b21a760767 100644
--- a/drivers/media/platform/davinci/vpif_display.h
+++ b/drivers/media/platform/davinci/vpif_display.h
@@ -13,8 +13,8 @@
  * GNU General Public License for more details.
  */
 
-#ifndef DAVINCIHD_DISPLAY_H
-#define DAVINCIHD_DISPLAY_H
+#ifndef VPIF_DISPLAY_H
+#define VPIF_DISPLAY_H
 
 /* Header files */
 #include <media/videobuf2-dma-contig.h>
@@ -67,17 +67,10 @@ struct vpif_disp_buffer {
 };
 
 struct common_obj {
-	/* Buffer specific parameters */
-	u8 *fbuffers[VIDEO_MAX_FRAME];		/* List of buffer pointers for
-						 * storing frames */
-	u32 numbuffers;				/* number of buffers */
 	struct vpif_disp_buffer *cur_frm;	/* Pointer pointing to current
 						 * vb2_buffer */
 	struct vpif_disp_buffer *next_frm;	/* Pointer pointing to next
 						 * vb2_buffer */
-	enum v4l2_memory memory;		/* This field keeps track of
-						 * type of buffer exchange
-						 * method user has selected */
 	struct v4l2_format fmt;			/* Used to store the format */
 	struct vb2_queue buffer_queue;		/* Buffer queue used in
 						 * video-buf */
@@ -90,10 +83,6 @@ struct common_obj {
 	/* channel specific parameters */
 	struct mutex lock;			/* lock used to access this
 						 * structure */
-	u32 io_usrs;				/* number of users performing
-						 * IO */
-	u8 started;				/* Indicates whether streaming
-						 * started */
 	u32 ytop_off;				/* offset of Y top from the
 						 * starting of the buffer */
 	u32 ybtm_off;				/* offset of Y bottom from the
@@ -103,7 +92,7 @@ struct common_obj {
 	u32 cbtm_off;				/* offset of C bottom from the
 						 * starting of the buffer */
 	/* Function pointer to set the addresses */
-	void (*set_addr) (unsigned long, unsigned long,
+	void (*set_addr)(unsigned long, unsigned long,
 				unsigned long, unsigned long);
 	u32 height;
 	u32 width;
@@ -113,10 +102,6 @@ struct channel_obj {
 	/* V4l2 specific parameters */
 	struct video_device *video_dev;	/* Identifies video device for
 					 * this channel */
-	struct v4l2_prio_state prio;	/* Used to keep track of state of
-					 * the priority */
-	atomic_t usrs;			/* number of open instances of
-					 * the channel */
 	u32 field_id;			/* Indicates id of the field
 					 * which is being displayed */
 	u8 initialized;			/* flag to indicate whether
@@ -130,19 +115,6 @@ struct channel_obj {
 	struct video_obj video;
 };
 
-/* File handle structure */
-struct vpif_fh {
-	struct channel_obj *channel;	/* pointer to channel object for
-					 * opened device */
-	u8 io_allowed[VPIF_NUMOBJECTS];	/* Indicates whether this file handle
-					 * is doing IO */
-	enum v4l2_priority prio;	/* Used to keep track priority of
-					 * this instance */
-	u8 initialized;			/* Used to keep track of whether this
-					 * file handle has initialized
-					 * channel or not */
-};
-
 /* vpif device structure */
 struct vpif_device {
 	struct v4l2_device v4l2_dev;
@@ -152,12 +124,4 @@ struct vpif_device {
 	struct vpif_display_config *config;
 };
 
-struct vpif_config_params {
-	u32 min_bufsize[VPIF_DISPLAY_NUM_CHANNELS];
-	u32 channel_bufsize[VPIF_DISPLAY_NUM_CHANNELS];
-	u8 numbuffers[VPIF_DISPLAY_NUM_CHANNELS];
-	u32 video_limit[VPIF_DISPLAY_NUM_CHANNELS];
-	u8 min_numbuffers;
-};
-
-#endif				/* DAVINCIHD_DISPLAY_H */
+#endif				/* VPIF_DISPLAY_H */
diff --git a/drivers/media/platform/exynos-gsc/gsc-m2m.c b/drivers/media/platform/exynos-gsc/gsc-m2m.c
index d0ea94f58d6..e434f1f03d7 100644
--- a/drivers/media/platform/exynos-gsc/gsc-m2m.c
+++ b/drivers/media/platform/exynos-gsc/gsc-m2m.c
@@ -66,15 +66,13 @@ static int gsc_m2m_start_streaming(struct vb2_queue *q, unsigned int count)
 	return ret > 0 ? 0 : ret;
 }
 
-static int gsc_m2m_stop_streaming(struct vb2_queue *q)
+static void gsc_m2m_stop_streaming(struct vb2_queue *q)
 {
 	struct gsc_ctx *ctx = q->drv_priv;
 
 	__gsc_m2m_job_abort(ctx);
 
 	pm_runtime_put(&ctx->gsc_dev->pdev->dev);
-
-	return 0;
 }
 
 void gsc_m2m_job_finish(struct gsc_ctx *ctx, int vb_state)
diff --git a/drivers/media/platform/exynos4-is/Kconfig b/drivers/media/platform/exynos4-is/Kconfig
index e1b2ceba00c..5dcaa0a8054 100644
--- a/drivers/media/platform/exynos4-is/Kconfig
+++ b/drivers/media/platform/exynos4-is/Kconfig
@@ -3,6 +3,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 OF && COMMON_CLK
 	help
 	  Say Y here to enable camera host interface devices for
 	  Samsung S5P and EXYNOS SoC series.
@@ -17,7 +18,7 @@ config VIDEO_S5P_FIMC
 	depends on I2C
 	select VIDEOBUF2_DMA_CONTIG
 	select V4L2_MEM2MEM_DEV
-	select MFD_SYSCON if OF
+	select MFD_SYSCON
 	select VIDEO_EXYNOS4_IS_COMMON
 	help
 	  This is a V4L2 driver for Samsung S5P and EXYNOS4 SoC camera host
diff --git a/drivers/media/platform/exynos4-is/common.c b/drivers/media/platform/exynos4-is/common.c
index 0ec210b4da1..0eb34ecb8ee 100644
--- a/drivers/media/platform/exynos4-is/common.c
+++ b/drivers/media/platform/exynos4-is/common.c
@@ -10,7 +10,7 @@
  */
 
 #include <linux/module.h>
-#include <media/s5p_fimc.h>
+#include <media/exynos-fimc.h>
 #include "common.h"
 
 /* Called with the media graph mutex held or entity->stream_count > 0. */
diff --git a/drivers/media/platform/exynos4-is/fimc-capture.c b/drivers/media/platform/exynos4-is/fimc-capture.c
index 92ae812abce..3d2babd5067 100644
--- a/drivers/media/platform/exynos4-is/fimc-capture.c
+++ b/drivers/media/platform/exynos4-is/fimc-capture.c
@@ -294,15 +294,15 @@ static int start_streaming(struct vb2_queue *q, unsigned int count)
 	return 0;
 }
 
-static int stop_streaming(struct vb2_queue *q)
+static void stop_streaming(struct vb2_queue *q)
 {
 	struct fimc_ctx *ctx = q->drv_priv;
 	struct fimc_dev *fimc = ctx->fimc_dev;
 
 	if (!fimc_capture_active(fimc))
-		return -EINVAL;
+		return;
 
-	return fimc_stop_capture(fimc, false);
+	fimc_stop_capture(fimc, false);
 }
 
 int fimc_capture_suspend(struct fimc_dev *fimc)
diff --git a/drivers/media/platform/exynos4-is/fimc-core.c b/drivers/media/platform/exynos4-is/fimc-core.c
index da2fc86cc52..b70fd996d79 100644
--- a/drivers/media/platform/exynos4-is/fimc-core.c
+++ b/drivers/media/platform/exynos4-is/fimc-core.c
@@ -56,8 +56,8 @@ static struct fimc_fmt fimc_formats[] = {
 		.colplanes	= 1,
 		.flags		= FMT_FLAGS_M2M,
 	}, {
-		.name		= "ARGB8888, 32 bpp",
-		.fourcc		= V4L2_PIX_FMT_RGB32,
+		.name		= "BGRA8888, 32 bpp",
+		.fourcc		= V4L2_PIX_FMT_BGR32,
 		.depth		= { 32 },
 		.color		= FIMC_FMT_RGB888,
 		.memplanes	= 1,
@@ -122,7 +122,7 @@ static struct fimc_fmt fimc_formats[] = {
 	}, {
 		.name		= "YUV 4:2:2 planar, Y/Cb/Cr",
 		.fourcc		= V4L2_PIX_FMT_YUV422P,
-		.depth		= { 12 },
+		.depth		= { 16 },
 		.color		= FIMC_FMT_YCBYCR422,
 		.memplanes	= 1,
 		.colplanes	= 3,
@@ -450,7 +450,7 @@ void fimc_prepare_dma_offset(struct fimc_ctx *ctx, struct fimc_frame *f)
 	bool pix_hoff = ctx->fimc_dev->drv_data->dma_pix_hoff;
 	u32 i, depth = 0;
 
-	for (i = 0; i < f->fmt->colplanes; i++)
+	for (i = 0; i < f->fmt->memplanes; i++)
 		depth += f->fmt->depth[i];
 
 	f->dma_offset.y_h = f->offs_h;
diff --git a/drivers/media/platform/exynos4-is/fimc-core.h b/drivers/media/platform/exynos4-is/fimc-core.h
index 1790fb4e32e..6c75c6ced1f 100644
--- a/drivers/media/platform/exynos4-is/fimc-core.h
+++ b/drivers/media/platform/exynos4-is/fimc-core.h
@@ -27,7 +27,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-mem2mem.h>
 #include <media/v4l2-mediabus.h>
-#include <media/s5p_fimc.h>
+#include <media/exynos-fimc.h>
 
 #define dbg(fmt, args...) \
 	pr_debug("%s:%d: " fmt "\n", __func__, __LINE__, ##args)
diff --git a/drivers/media/platform/exynos4-is/fimc-is.c b/drivers/media/platform/exynos4-is/fimc-is.c
index 128b73b6cce..5476dce3ad2 100644
--- a/drivers/media/platform/exynos4-is/fimc-is.c
+++ b/drivers/media/platform/exynos4-is/fimc-is.c
@@ -367,6 +367,9 @@ static void fimc_is_free_cpu_memory(struct fimc_is *is)
 {
 	struct device *dev = &is->pdev->dev;
 
+	if (is->memory.vaddr == NULL)
+		return;
+
 	dma_free_coherent(dev, is->memory.size, is->memory.vaddr,
 			  is->memory.paddr);
 }
diff --git a/drivers/media/platform/exynos4-is/fimc-isp-video.c b/drivers/media/platform/exynos4-is/fimc-isp-video.c
index e92b4e115ad..93f9cf2ebcd 100644
--- a/drivers/media/platform/exynos4-is/fimc-isp-video.c
+++ b/drivers/media/platform/exynos4-is/fimc-isp-video.c
@@ -30,7 +30,7 @@
 #include <media/v4l2-ioctl.h>
 #include <media/videobuf2-core.h>
 #include <media/videobuf2-dma-contig.h>
-#include <media/s5p_fimc.h>
+#include <media/exynos-fimc.h>
 
 #include "common.h"
 #include "media-dev.h"
@@ -125,7 +125,7 @@ static int isp_video_capture_start_streaming(struct vb2_queue *q,
 	return ret;
 }
 
-static int isp_video_capture_stop_streaming(struct vb2_queue *q)
+static void isp_video_capture_stop_streaming(struct vb2_queue *q)
 {
 	struct fimc_isp *isp = vb2_get_drv_priv(q);
 	struct fimc_is *is = fimc_isp_to_is(isp);
@@ -134,7 +134,7 @@ static int isp_video_capture_stop_streaming(struct vb2_queue *q)
 
 	ret = fimc_pipeline_call(&isp->video_capture.ve, set_stream, 0);
 	if (ret < 0)
-		return ret;
+		return;
 
 	dma->cmd = DMA_OUTPUT_COMMAND_DISABLE;
 	dma->notify_dma_done = DMA_OUTPUT_NOTIFY_DMA_DONE_DISABLE;
@@ -155,7 +155,6 @@ static int isp_video_capture_stop_streaming(struct vb2_queue *q)
 	clear_bit(ST_ISP_VID_CAP_STREAMING, &isp->state);
 
 	isp->video_capture.buf_count = 0;
-	return 0;
 }
 
 static int isp_video_capture_buffer_prepare(struct vb2_buffer *vb)
diff --git a/drivers/media/platform/exynos4-is/fimc-isp.h b/drivers/media/platform/exynos4-is/fimc-isp.h
index 4dc55a18d97..b99be09b49f 100644
--- a/drivers/media/platform/exynos4-is/fimc-isp.h
+++ b/drivers/media/platform/exynos4-is/fimc-isp.h
@@ -24,7 +24,7 @@
 #include <media/videobuf2-core.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-mediabus.h>
-#include <media/s5p_fimc.h>
+#include <media/exynos-fimc.h>
 
 extern int fimc_isp_debug;
 
diff --git a/drivers/media/platform/exynos4-is/fimc-lite-reg.c b/drivers/media/platform/exynos4-is/fimc-lite-reg.c
index d0dc7ee0445..bc3ec7d25a3 100644
--- a/drivers/media/platform/exynos4-is/fimc-lite-reg.c
+++ b/drivers/media/platform/exynos4-is/fimc-lite-reg.c
@@ -12,7 +12,7 @@
 #include <linux/bitops.h>
 #include <linux/delay.h>
 #include <linux/io.h>
-#include <media/s5p_fimc.h>
+#include <media/exynos-fimc.h>
 
 #include "fimc-lite-reg.h"
 #include "fimc-lite.h"
diff --git a/drivers/media/platform/exynos4-is/fimc-lite.c b/drivers/media/platform/exynos4-is/fimc-lite.c
index 3ad660b55b6..a97d2352f1d 100644
--- a/drivers/media/platform/exynos4-is/fimc-lite.c
+++ b/drivers/media/platform/exynos4-is/fimc-lite.c
@@ -30,7 +30,7 @@
 #include <media/v4l2-mem2mem.h>
 #include <media/videobuf2-core.h>
 #include <media/videobuf2-dma-contig.h>
-#include <media/s5p_fimc.h>
+#include <media/exynos-fimc.h>
 
 #include "common.h"
 #include "fimc-core.h"
@@ -350,14 +350,14 @@ static int start_streaming(struct vb2_queue *q, unsigned int count)
 	return 0;
 }
 
-static int stop_streaming(struct vb2_queue *q)
+static void stop_streaming(struct vb2_queue *q)
 {
 	struct fimc_lite *fimc = q->drv_priv;
 
 	if (!fimc_lite_active(fimc))
-		return -EINVAL;
+		return;
 
-	return fimc_lite_stop_capture(fimc, false);
+	fimc_lite_stop_capture(fimc, false);
 }
 
 static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *pfmt,
diff --git a/drivers/media/platform/exynos4-is/fimc-lite.h b/drivers/media/platform/exynos4-is/fimc-lite.h
index 7428b2d22b5..ea19dc7be63 100644
--- a/drivers/media/platform/exynos4-is/fimc-lite.h
+++ b/drivers/media/platform/exynos4-is/fimc-lite.h
@@ -23,7 +23,7 @@
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-mediabus.h>
-#include <media/s5p_fimc.h>
+#include <media/exynos-fimc.h>
 
 #define FIMC_LITE_DRV_NAME	"exynos-fimc-lite"
 #define FLITE_CLK_NAME		"flite"
diff --git a/drivers/media/platform/exynos4-is/fimc-m2m.c b/drivers/media/platform/exynos4-is/fimc-m2m.c
index 36971d915b5..0ad1b6f84a2 100644
--- a/drivers/media/platform/exynos4-is/fimc-m2m.c
+++ b/drivers/media/platform/exynos4-is/fimc-m2m.c
@@ -85,7 +85,7 @@ static int start_streaming(struct vb2_queue *q, unsigned int count)
 	return ret > 0 ? 0 : ret;
 }
 
-static int stop_streaming(struct vb2_queue *q)
+static void stop_streaming(struct vb2_queue *q)
 {
 	struct fimc_ctx *ctx = q->drv_priv;
 	int ret;
@@ -95,7 +95,6 @@ static int stop_streaming(struct vb2_queue *q)
 		fimc_m2m_job_finish(ctx, VB2_BUF_STATE_ERROR);
 
 	pm_runtime_put(&ctx->fimc_dev->pdev->dev);
-	return 0;
 }
 
 static void fimc_device_run(void *priv)
@@ -197,7 +196,7 @@ static int fimc_queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
 
 	*num_planes = f->fmt->memplanes;
 	for (i = 0; i < f->fmt->memplanes; i++) {
-		sizes[i] = (f->f_width * f->f_height * f->fmt->depth[i]) / 8;
+		sizes[i] = f->payload[i];
 		allocators[i] = ctx->fimc_dev->alloc_ctx;
 	}
 	return 0;
@@ -342,7 +341,7 @@ static void __set_frame_format(struct fimc_frame *frame, struct fimc_fmt *fmt,
 {
 	int i;
 
-	for (i = 0; i < fmt->colplanes; i++) {
+	for (i = 0; i < fmt->memplanes; i++) {
 		frame->bytesperline[i] = pixm->plane_fmt[i].bytesperline;
 		frame->payload[i] = pixm->plane_fmt[i].sizeimage;
 	}
@@ -461,7 +460,7 @@ static int fimc_m2m_try_crop(struct fimc_ctx *ctx, struct v4l2_crop *cr)
 	else
 		halign = ffs(fimc->variant->min_vsize_align) - 1;
 
-	for (i = 0; i < f->fmt->colplanes; i++)
+	for (i = 0; i < f->fmt->memplanes; i++)
 		depth += f->fmt->depth[i];
 
 	v4l_bound_align_image(&cr->c.width, min_size, f->o_width,
diff --git a/drivers/media/platform/exynos4-is/fimc-reg.c b/drivers/media/platform/exynos4-is/fimc-reg.c
index 1db8cb4c46e..2d77fd8f440 100644
--- a/drivers/media/platform/exynos4-is/fimc-reg.c
+++ b/drivers/media/platform/exynos4-is/fimc-reg.c
@@ -13,7 +13,7 @@
 #include <linux/io.h>
 #include <linux/regmap.h>
 
-#include <media/s5p_fimc.h>
+#include <media/exynos-fimc.h>
 #include "media-dev.h"
 
 #include "fimc-reg.h"
diff --git a/drivers/media/platform/exynos4-is/media-dev.c b/drivers/media/platform/exynos4-is/media-dev.c
index e62211a80f0..344718df5c6 100644
--- a/drivers/media/platform/exynos4-is/media-dev.c
+++ b/drivers/media/platform/exynos4-is/media-dev.c
@@ -31,7 +31,7 @@
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-of.h>
 #include <media/media-device.h>
-#include <media/s5p_fimc.h>
+#include <media/exynos-fimc.h>
 
 #include "media-dev.h"
 #include "fimc-core.h"
@@ -39,10 +39,6 @@
 #include "fimc-lite.h"
 #include "mipi-csis.h"
 
-static int __fimc_md_set_camclk(struct fimc_md *fmd,
-				struct fimc_source_info *si,
-				bool on);
-
 /* Set up image sensor subdev -> FIMC capture node notifications. */
 static void __setup_sensor_notification(struct fimc_md *fmd,
 					struct v4l2_subdev *sensor,
@@ -223,17 +219,10 @@ static int __fimc_pipeline_open(struct exynos_media_pipeline *ep,
 			return ret;
 	}
 
-	ret = fimc_md_set_camclk(sd, true);
-	if (ret < 0)
-		goto err_wbclk;
-
 	ret = fimc_pipeline_s_power(p, 1);
 	if (!ret)
 		return 0;
 
-	fimc_md_set_camclk(sd, false);
-
-err_wbclk:
 	if (!IS_ERR(fmd->wbclk[CLK_IDX_WB_B]) && p->subdevs[IDX_IS_ISP])
 		clk_disable_unprepare(fmd->wbclk[CLK_IDX_WB_B]);
 
@@ -259,7 +248,6 @@ static int __fimc_pipeline_close(struct exynos_media_pipeline *ep)
 	}
 
 	ret = fimc_pipeline_s_power(p, 0);
-	fimc_md_set_camclk(sd, false);
 
 	fmd = entity_to_fimc_mdev(&sd->entity);
 
@@ -337,75 +325,14 @@ static void fimc_md_pipelines_free(struct fimc_md *fmd)
 	}
 }
 
-/*
- * Sensor subdevice helper functions
- */
-static struct v4l2_subdev *fimc_md_register_sensor(struct fimc_md *fmd,
-						struct fimc_source_info *si)
-{
-	struct i2c_adapter *adapter;
-	struct v4l2_subdev *sd = NULL;
-
-	if (!si || !fmd)
-		return NULL;
-	/*
-	 * If FIMC bus type is not Writeback FIFO assume it is same
-	 * as sensor_bus_type.
-	 */
-	si->fimc_bus_type = si->sensor_bus_type;
-
-	adapter = i2c_get_adapter(si->i2c_bus_num);
-	if (!adapter) {
-		v4l2_warn(&fmd->v4l2_dev,
-			  "Failed to get I2C adapter %d, deferring probe\n",
-			  si->i2c_bus_num);
-		return ERR_PTR(-EPROBE_DEFER);
-	}
-	sd = v4l2_i2c_new_subdev_board(&fmd->v4l2_dev, adapter,
-						si->board_info, NULL);
-	if (IS_ERR_OR_NULL(sd)) {
-		i2c_put_adapter(adapter);
-		v4l2_warn(&fmd->v4l2_dev,
-			  "Failed to acquire subdev %s, deferring probe\n",
-			  si->board_info->type);
-		return ERR_PTR(-EPROBE_DEFER);
-	}
-	v4l2_set_subdev_hostdata(sd, si);
-	sd->grp_id = GRP_ID_SENSOR;
-
-	v4l2_info(&fmd->v4l2_dev, "Registered sensor subdevice %s\n",
-		  sd->name);
-	return sd;
-}
-
-static void fimc_md_unregister_sensor(struct v4l2_subdev *sd)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	struct i2c_adapter *adapter;
-
-	if (!client || client->dev.of_node)
-		return;
-
-	v4l2_device_unregister_subdev(sd);
-
-	adapter = client->adapter;
-	i2c_unregister_device(client);
-	if (adapter)
-		i2c_put_adapter(adapter);
-}
-
-#ifdef CONFIG_OF
 /* Parse port node and register as a sub-device any sensor specified there. */
 static int fimc_md_parse_port_node(struct fimc_md *fmd,
 				   struct device_node *port,
 				   unsigned int index)
 {
+	struct fimc_source_info *pd = &fmd->sensor[index].pdata;
 	struct device_node *rem, *ep, *np;
-	struct fimc_source_info *pd;
 	struct v4l2_of_endpoint endpoint;
-	u32 val;
-
-	pd = &fmd->sensor[index].pdata;
 
 	/* Assume here a port node can have only one endpoint node. */
 	ep = of_get_next_child(port, NULL);
@@ -425,20 +352,6 @@ static int fimc_md_parse_port_node(struct fimc_md *fmd,
 							ep->full_name);
 		return 0;
 	}
-	if (!of_property_read_u32(rem, "samsung,camclk-out", &val))
-		pd->clk_id = val;
-
-	if (!of_property_read_u32(rem, "clock-frequency", &val))
-		pd->clk_frequency = val;
-	else
-		pd->clk_frequency = DEFAULT_SENSOR_CLK_FREQ;
-
-	if (pd->clk_frequency == 0) {
-		v4l2_err(&fmd->v4l2_dev, "Wrong clock frequency at node %s\n",
-			 rem->full_name);
-		of_node_put(rem);
-		return -EINVAL;
-	}
 
 	if (fimc_input_is_parallel(endpoint.base.port)) {
 		if (endpoint.bus_type == V4L2_MBUS_PARALLEL)
@@ -485,14 +398,26 @@ static int fimc_md_parse_port_node(struct fimc_md *fmd,
 }
 
 /* Register all SoC external sub-devices */
-static int fimc_md_of_sensors_register(struct fimc_md *fmd,
-				       struct device_node *np)
+static int fimc_md_register_sensor_entities(struct fimc_md *fmd)
 {
 	struct device_node *parent = fmd->pdev->dev.of_node;
 	struct device_node *node, *ports;
 	int index = 0;
 	int ret;
 
+	/*
+	 * Runtime resume one of the FIMC entities to make sure
+	 * the sclk_cam clocks are not globally disabled.
+	 */
+	if (!fmd->pmf)
+		return -ENXIO;
+
+	ret = pm_runtime_get_sync(fmd->pmf);
+	if (ret < 0)
+		return ret;
+
+	fmd->num_sensors = 0;
+
 	/* Attach sensors linked to MIPI CSI-2 receivers */
 	for_each_available_child_of_node(parent, node) {
 		struct device_node *port;
@@ -506,14 +431,14 @@ static int fimc_md_of_sensors_register(struct fimc_md *fmd,
 
 		ret = fimc_md_parse_port_node(fmd, port, index);
 		if (ret < 0)
-			return ret;
+			goto rpm_put;
 		index++;
 	}
 
 	/* Attach sensors listed in the parallel-ports node */
 	ports = of_get_child_by_name(parent, "parallel-ports");
 	if (!ports)
-		return 0;
+		goto rpm_put;
 
 	for_each_child_of_node(ports, node) {
 		ret = fimc_md_parse_port_node(fmd, node, index);
@@ -521,8 +446,9 @@ static int fimc_md_of_sensors_register(struct fimc_md *fmd,
 			break;
 		index++;
 	}
-
-	return 0;
+rpm_put:
+	pm_runtime_put(fmd->pmf);
+	return ret;
 }
 
 static int __of_get_csis_id(struct device_node *np)
@@ -535,68 +461,10 @@ static int __of_get_csis_id(struct device_node *np)
 	of_property_read_u32(np, "reg", &reg);
 	return reg - FIMC_INPUT_MIPI_CSI2_0;
 }
-#else
-#define fimc_md_of_sensors_register(fmd, np) (-ENOSYS)
-#define __of_get_csis_id(np) (-ENOSYS)
-#endif
-
-static int fimc_md_register_sensor_entities(struct fimc_md *fmd)
-{
-	struct s5p_platform_fimc *pdata = fmd->pdev->dev.platform_data;
-	struct device_node *of_node = fmd->pdev->dev.of_node;
-	int num_clients = 0;
-	int ret, i;
-
-	/*
-	 * Runtime resume one of the FIMC entities to make sure
-	 * the sclk_cam clocks are not globally disabled.
-	 */
-	if (!fmd->pmf)
-		return -ENXIO;
-
-	ret = pm_runtime_get_sync(fmd->pmf);
-	if (ret < 0)
-		return ret;
-
-	if (of_node) {
-		fmd->num_sensors = 0;
-		ret = fimc_md_of_sensors_register(fmd, of_node);
-	} else if (pdata) {
-		WARN_ON(pdata->num_clients > ARRAY_SIZE(fmd->sensor));
-		num_clients = min_t(u32, pdata->num_clients,
-				    ARRAY_SIZE(fmd->sensor));
-		fmd->num_sensors = num_clients;
-
-		for (i = 0; i < num_clients; i++) {
-			struct fimc_sensor_info *si = &fmd->sensor[i];
-			struct v4l2_subdev *sd;
-
-			si->pdata = pdata->source_info[i];
-			ret = __fimc_md_set_camclk(fmd, &si->pdata, true);
-			if (ret)
-				break;
-			sd = fimc_md_register_sensor(fmd, &si->pdata);
-			ret = __fimc_md_set_camclk(fmd, &si->pdata, false);
-
-			if (IS_ERR(sd)) {
-				si->subdev = NULL;
-				ret = PTR_ERR(sd);
-				break;
-			}
-			si->subdev = sd;
-			if (ret)
-				break;
-		}
-	}
-
-	pm_runtime_put(fmd->pmf);
-	return ret;
-}
 
 /*
  * MIPI-CSIS, FIMC and FIMC-LITE platform devices registration.
  */
-
 static int register_fimc_lite_entity(struct fimc_md *fmd,
 				     struct fimc_lite *fimc_lite)
 {
@@ -753,35 +621,9 @@ dev_unlock:
 	return ret;
 }
 
-static int fimc_md_pdev_match(struct device *dev, void *data)
-{
-	struct platform_device *pdev = to_platform_device(dev);
-	int plat_entity = -1;
-	int ret;
-	char *p;
-
-	if (!get_device(dev))
-		return -ENODEV;
-
-	if (!strcmp(pdev->name, CSIS_DRIVER_NAME)) {
-		plat_entity = IDX_CSIS;
-	} else {
-		p = strstr(pdev->name, "fimc");
-		if (p && *(p + 4) == 0)
-			plat_entity = IDX_FIMC;
-	}
-
-	if (plat_entity >= 0)
-		ret = fimc_md_register_platform_entity(data, pdev,
-						       plat_entity);
-	put_device(dev);
-	return 0;
-}
-
 /* Register FIMC, FIMC-LITE and CSIS media entities */
-#ifdef CONFIG_OF
-static int fimc_md_register_of_platform_entities(struct fimc_md *fmd,
-						 struct device_node *parent)
+static int fimc_md_register_platform_entities(struct fimc_md *fmd,
+					      struct device_node *parent)
 {
 	struct device_node *node;
 	int ret = 0;
@@ -815,9 +657,6 @@ static int fimc_md_register_of_platform_entities(struct fimc_md *fmd,
 
 	return ret;
 }
-#else
-#define fimc_md_register_of_platform_entities(fmd, node) (-ENOSYS)
-#endif
 
 static void fimc_md_unregister_entities(struct fimc_md *fmd)
 {
@@ -845,14 +684,6 @@ static void fimc_md_unregister_entities(struct fimc_md *fmd)
 		v4l2_device_unregister_subdev(fmd->csis[i].sd);
 		fmd->csis[i].sd = NULL;
 	}
-	if (fmd->pdev->dev.of_node == NULL) {
-		for (i = 0; i < fmd->num_sensors; i++) {
-			if (fmd->sensor[i].subdev == NULL)
-				continue;
-			fimc_md_unregister_sensor(fmd->sensor[i].subdev);
-			fmd->sensor[i].subdev = NULL;
-		}
-	}
 
 	if (fmd->fimc_is)
 		v4l2_device_unregister_subdev(&fmd->fimc_is->isp.subdev);
@@ -1137,7 +968,7 @@ static void fimc_md_put_clocks(struct fimc_md *fmd)
 
 static int fimc_md_get_clocks(struct fimc_md *fmd)
 {
-	struct device *dev = NULL;
+	struct device *dev = &fmd->pdev->dev;
 	char clk_name[32];
 	struct clk *clock;
 	int i, ret = 0;
@@ -1145,16 +976,12 @@ static int fimc_md_get_clocks(struct fimc_md *fmd)
 	for (i = 0; i < FIMC_MAX_CAMCLKS; i++)
 		fmd->camclk[i].clock = ERR_PTR(-EINVAL);
 
-	if (fmd->pdev->dev.of_node)
-		dev = &fmd->pdev->dev;
-
 	for (i = 0; i < FIMC_MAX_CAMCLKS; i++) {
 		snprintf(clk_name, sizeof(clk_name), "sclk_cam%u", i);
 		clock = clk_get(dev, clk_name);
 
 		if (IS_ERR(clock)) {
-			dev_err(&fmd->pdev->dev, "Failed to get clock: %s\n",
-								clk_name);
+			dev_err(dev, "Failed to get clock: %s\n", clk_name);
 			ret = PTR_ERR(clock);
 			break;
 		}
@@ -1188,86 +1015,6 @@ static int fimc_md_get_clocks(struct fimc_md *fmd)
 	return ret;
 }
 
-static int __fimc_md_set_camclk(struct fimc_md *fmd,
-				struct fimc_source_info *si,
-				bool on)
-{
-	struct fimc_camclk_info *camclk;
-	int ret = 0;
-
-	/*
-	 * When device tree is used the sensor drivers are supposed to
-	 * control the clock themselves. This whole function will be
-	 * removed once S5PV210 platform is converted to the device tree.
-	 */
-	if (fmd->pdev->dev.of_node)
-		return 0;
-
-	if (WARN_ON(si->clk_id >= FIMC_MAX_CAMCLKS) || !fmd || !fmd->pmf)
-		return -EINVAL;
-
-	camclk = &fmd->camclk[si->clk_id];
-
-	dbg("camclk %d, f: %lu, use_count: %d, on: %d",
-	    si->clk_id, si->clk_frequency, camclk->use_count, on);
-
-	if (on) {
-		if (camclk->use_count > 0 &&
-		    camclk->frequency != si->clk_frequency)
-			return -EINVAL;
-
-		if (camclk->use_count++ == 0) {
-			clk_set_rate(camclk->clock, si->clk_frequency);
-			camclk->frequency = si->clk_frequency;
-			ret = pm_runtime_get_sync(fmd->pmf);
-			if (ret < 0)
-				return ret;
-			ret = clk_prepare_enable(camclk->clock);
-			dbg("Enabled camclk %d: f: %lu", si->clk_id,
-			    clk_get_rate(camclk->clock));
-		}
-		return ret;
-	}
-
-	if (WARN_ON(camclk->use_count == 0))
-		return 0;
-
-	if (--camclk->use_count == 0) {
-		clk_disable_unprepare(camclk->clock);
-		pm_runtime_put(fmd->pmf);
-		dbg("Disabled camclk %d", si->clk_id);
-	}
-	return ret;
-}
-
-/**
- * fimc_md_set_camclk - peripheral sensor clock setup
- * @sd: sensor subdev to configure sclk_cam clock for
- * @on: 1 to enable or 0 to disable the clock
- *
- * There are 2 separate clock outputs available in the SoC for external
- * image processors. These clocks are shared between all registered FIMC
- * devices to which sensors can be attached, either directly or through
- * the MIPI CSI receiver. The clock is allowed here to be used by
- * multiple sensors concurrently if they use same frequency.
- * This function should only be called when the graph mutex is held.
- */
-int fimc_md_set_camclk(struct v4l2_subdev *sd, bool on)
-{
-	struct fimc_source_info *si = v4l2_get_subdev_hostdata(sd);
-	struct fimc_md *fmd = entity_to_fimc_mdev(&sd->entity);
-
-	/*
-	 * If there is a clock provider registered the sensors will
-	 * handle their clock themselves, no need to control it on
-	 * the host interface side.
-	 */
-	if (fmd->clk_provider.num_clocks > 0)
-		return 0;
-
-	return __fimc_md_set_camclk(fmd, si, on);
-}
-
 static int __fimc_md_modify_pipeline(struct media_entity *entity, bool enable)
 {
 	struct exynos_video_entity *ve;
@@ -1426,7 +1173,6 @@ static int fimc_md_get_pinctrl(struct fimc_md *fmd)
 	return 0;
 }
 
-#ifdef CONFIG_OF
 static int cam_clk_prepare(struct clk_hw *hw)
 {
 	struct cam_clk *camclk = to_cam_clk(hw);
@@ -1518,10 +1264,6 @@ err:
 	fimc_md_unregister_clk_provider(fmd);
 	return ret;
 }
-#else
-#define fimc_md_register_clk_provider(fmd) (0)
-#define fimc_md_unregister_clk_provider(fmd) (0)
-#endif
 
 static int subdev_notifier_bound(struct v4l2_async_notifier *notifier,
 				 struct v4l2_subdev *subdev,
@@ -1585,8 +1327,8 @@ static int fimc_md_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	spin_lock_init(&fmd->slock);
-	fmd->pdev = pdev;
 	INIT_LIST_HEAD(&fmd->pipelines);
+	fmd->pdev = pdev;
 
 	strlcpy(fmd->media_dev.model, "SAMSUNG S5P FIMC",
 		sizeof(fmd->media_dev.model));
@@ -1599,6 +1341,7 @@ static int fimc_md_probe(struct platform_device *pdev)
 	strlcpy(v4l2_dev->name, "s5p-fimc-md", sizeof(v4l2_dev->name));
 
 	fmd->use_isp = fimc_md_is_isp_available(dev->of_node);
+	fmd->user_subdev_api = true;
 
 	ret = v4l2_device_register(dev, &fmd->v4l2_dev);
 	if (ret < 0) {
@@ -1616,8 +1359,6 @@ static int fimc_md_probe(struct platform_device *pdev)
 	if (ret)
 		goto err_md;
 
-	fmd->user_subdev_api = (dev->of_node != NULL);
-
 	ret = fimc_md_get_pinctrl(fmd);
 	if (ret < 0) {
 		if (ret != EPROBE_DEFER)
@@ -1630,22 +1371,16 @@ static int fimc_md_probe(struct platform_device *pdev)
 	/* Protect the media graph while we're registering entities */
 	mutex_lock(&fmd->media_dev.graph_mutex);
 
-	if (dev->of_node)
-		ret = fimc_md_register_of_platform_entities(fmd, dev->of_node);
-	else
-		ret = bus_for_each_dev(&platform_bus_type, NULL, fmd,
-						fimc_md_pdev_match);
+	ret = fimc_md_register_platform_entities(fmd, dev->of_node);
 	if (ret) {
 		mutex_unlock(&fmd->media_dev.graph_mutex);
 		goto err_clk;
 	}
 
-	if (dev->platform_data || dev->of_node) {
-		ret = fimc_md_register_sensor_entities(fmd);
-		if (ret) {
-			mutex_unlock(&fmd->media_dev.graph_mutex);
-			goto err_m_ent;
-		}
+	ret = fimc_md_register_sensor_entities(fmd);
+	if (ret) {
+		mutex_unlock(&fmd->media_dev.graph_mutex);
+		goto err_m_ent;
 	}
 
 	mutex_unlock(&fmd->media_dev.graph_mutex);
diff --git a/drivers/media/platform/exynos4-is/media-dev.h b/drivers/media/platform/exynos4-is/media-dev.h
index ee1e2519f72..03214541f14 100644
--- a/drivers/media/platform/exynos4-is/media-dev.h
+++ b/drivers/media/platform/exynos4-is/media-dev.h
@@ -19,7 +19,7 @@
 #include <media/media-entity.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-subdev.h>
-#include <media/s5p_fimc.h>
+#include <media/exynos-fimc.h>
 
 #include "fimc-core.h"
 #include "fimc-lite.h"
diff --git a/drivers/media/platform/exynos4-is/mipi-csis.c b/drivers/media/platform/exynos4-is/mipi-csis.c
index 3678ba59725..ae54ef5f535 100644
--- a/drivers/media/platform/exynos4-is/mipi-csis.c
+++ b/drivers/media/platform/exynos4-is/mipi-csis.c
@@ -22,14 +22,13 @@
 #include <linux/of.h>
 #include <linux/of_graph.h>
 #include <linux/phy/phy.h>
-#include <linux/platform_data/mipi-csis.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/regulator/consumer.h>
 #include <linux/slab.h>
 #include <linux/spinlock.h>
 #include <linux/videodev2.h>
-#include <media/s5p_fimc.h>
+#include <media/exynos-fimc.h>
 #include <media/v4l2-of.h>
 #include <media/v4l2-subdev.h>
 
@@ -730,26 +729,6 @@ static irqreturn_t s5pcsis_irq_handler(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static int s5pcsis_get_platform_data(struct platform_device *pdev,
-				     struct csis_state *state)
-{
-	struct s5p_platform_mipi_csis *pdata = pdev->dev.platform_data;
-
-	if (pdata == NULL) {
-		dev_err(&pdev->dev, "Platform data not specified\n");
-		return -EINVAL;
-	}
-
-	state->clk_frequency = pdata->clk_rate;
-	state->num_lanes = pdata->lanes;
-	state->hs_settle = pdata->hs_settle;
-	state->index = max(0, pdev->id);
-	state->max_num_lanes = state->index ? CSIS1_MAX_LANES :
-					      CSIS0_MAX_LANES;
-	return 0;
-}
-
-#ifdef CONFIG_OF
 static int s5pcsis_parse_dt(struct platform_device *pdev,
 			    struct csis_state *state)
 {
@@ -787,9 +766,6 @@ static int s5pcsis_parse_dt(struct platform_device *pdev,
 
 	return 0;
 }
-#else
-#define s5pcsis_parse_dt(pdev, state) (-ENOSYS)
-#endif
 
 static int s5pcsis_pm_resume(struct device *dev, bool runtime);
 static const struct of_device_id s5pcsis_of_match[];
@@ -812,19 +788,14 @@ static int s5pcsis_probe(struct platform_device *pdev)
 	spin_lock_init(&state->slock);
 	state->pdev = pdev;
 
-	if (dev->of_node) {
-		of_id = of_match_node(s5pcsis_of_match, dev->of_node);
-		if (WARN_ON(of_id == NULL))
-			return -EINVAL;
-
-		drv_data = of_id->data;
-		state->interrupt_mask = drv_data->interrupt_mask;
+	of_id = of_match_node(s5pcsis_of_match, dev->of_node);
+	if (WARN_ON(of_id == NULL))
+		return -EINVAL;
 
-		ret = s5pcsis_parse_dt(pdev, state);
-	} else {
-		ret = s5pcsis_get_platform_data(pdev, state);
-	}
+	drv_data = of_id->data;
+	state->interrupt_mask = drv_data->interrupt_mask;
 
+	ret = s5pcsis_parse_dt(pdev, state);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/media/platform/fsl-viu.c b/drivers/media/platform/fsl-viu.c
index dbf0ce38a8e..d5dc198502e 100644
--- a/drivers/media/platform/fsl-viu.c
+++ b/drivers/media/platform/fsl-viu.c
@@ -964,7 +964,7 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id id)
 	struct viu_fh *fh = priv;
 
 	fh->dev->std = id;
-	decoder_call(fh->dev, core, s_std, id);
+	decoder_call(fh->dev, video, s_std, id);
 	return 0;
 }
 
diff --git a/drivers/media/platform/marvell-ccic/mcam-core.c b/drivers/media/platform/marvell-ccic/mcam-core.c
index 8b34c485be7..be4b5121210 100644
--- a/drivers/media/platform/marvell-ccic/mcam-core.c
+++ b/drivers/media/platform/marvell-ccic/mcam-core.c
@@ -1156,7 +1156,7 @@ static int mcam_vb_start_streaming(struct vb2_queue *vq, unsigned int count)
 	return mcam_read_setup(cam);
 }
 
-static int mcam_vb_stop_streaming(struct vb2_queue *vq)
+static void mcam_vb_stop_streaming(struct vb2_queue *vq)
 {
 	struct mcam_camera *cam = vb2_get_drv_priv(vq);
 	unsigned long flags;
@@ -1164,10 +1164,10 @@ static int mcam_vb_stop_streaming(struct vb2_queue *vq)
 	if (cam->state == S_BUFWAIT) {
 		/* They never gave us buffers */
 		cam->state = S_IDLE;
-		return 0;
+		return;
 	}
 	if (cam->state != S_STREAMING)
-		return -EINVAL;
+		return;
 	mcam_ctlr_stop_dma(cam);
 	/*
 	 * Reset the CCIC PHY after stopping streaming,
@@ -1182,7 +1182,6 @@ static int mcam_vb_stop_streaming(struct vb2_queue *vq)
 	spin_lock_irqsave(&cam->dev_lock, flags);
 	INIT_LIST_HEAD(&cam->buffers);
 	spin_unlock_irqrestore(&cam->dev_lock, flags);
-	return 0;
 }
 
 
diff --git a/drivers/media/platform/mem2mem_testdev.c b/drivers/media/platform/mem2mem_testdev.c
index 4f3096b1706..0714070ed7f 100644
--- a/drivers/media/platform/mem2mem_testdev.c
+++ b/drivers/media/platform/mem2mem_testdev.c
@@ -787,7 +787,7 @@ static int m2mtest_start_streaming(struct vb2_queue *q, unsigned count)
 	return 0;
 }
 
-static int m2mtest_stop_streaming(struct vb2_queue *q)
+static void m2mtest_stop_streaming(struct vb2_queue *q)
 {
 	struct m2mtest_ctx *ctx = vb2_get_drv_priv(q);
 	struct vb2_buffer *vb;
@@ -799,12 +799,11 @@ static int m2mtest_stop_streaming(struct vb2_queue *q)
 		else
 			vb = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
 		if (vb == NULL)
-			return 0;
+			return;
 		spin_lock_irqsave(&ctx->dev->irqlock, flags);
 		v4l2_m2m_buf_done(vb, VB2_BUF_STATE_ERROR);
 		spin_unlock_irqrestore(&ctx->dev->irqlock, flags);
 	}
-	return 0;
 }
 
 static struct vb2_ops m2mtest_qops = {
diff --git a/drivers/media/platform/mx2_emmaprp.c b/drivers/media/platform/mx2_emmaprp.c
index 0b7480e8214..fa8f7cabe36 100644
--- a/drivers/media/platform/mx2_emmaprp.c
+++ b/drivers/media/platform/mx2_emmaprp.c
@@ -207,10 +207,8 @@ struct emmaprp_dev {
 	struct mutex		dev_mutex;
 	spinlock_t		irqlock;
 
-	int			irq_emma;
 	void __iomem		*base_emma;
 	struct clk		*clk_emma_ahb, *clk_emma_ipg;
-	struct resource		*res_emma;
 
 	struct v4l2_m2m_dev	*m2m_dev;
 	struct vb2_alloc_ctx	*alloc_ctx;
@@ -901,9 +899,8 @@ static int emmaprp_probe(struct platform_device *pdev)
 {
 	struct emmaprp_dev *pcdev;
 	struct video_device *vfd;
-	struct resource *res_emma;
-	int irq_emma;
-	int ret;
+	struct resource *res;
+	int irq, ret;
 
 	pcdev = devm_kzalloc(&pdev->dev, sizeof(*pcdev), GFP_KERNEL);
 	if (!pcdev)
@@ -920,12 +917,10 @@ static int emmaprp_probe(struct platform_device *pdev)
 	if (IS_ERR(pcdev->clk_emma_ahb))
 		return PTR_ERR(pcdev->clk_emma_ahb);
 
-	irq_emma = platform_get_irq(pdev, 0);
-	res_emma = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (irq_emma < 0 || res_emma == NULL) {
-		dev_err(&pdev->dev, "Missing platform resources data\n");
-		return -ENODEV;
-	}
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	pcdev->base_emma = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(pcdev->base_emma))
+		return PTR_ERR(pcdev->base_emma);
 
 	ret = v4l2_device_register(&pdev->dev, &pcdev->v4l2_dev);
 	if (ret)
@@ -952,20 +947,11 @@ static int emmaprp_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, pcdev);
 
-	pcdev->base_emma = devm_ioremap_resource(&pdev->dev, res_emma);
-	if (IS_ERR(pcdev->base_emma)) {
-		ret = PTR_ERR(pcdev->base_emma);
-		goto rel_vdev;
-	}
-
-	pcdev->irq_emma = irq_emma;
-	pcdev->res_emma = res_emma;
-
-	if (devm_request_irq(&pdev->dev, pcdev->irq_emma, emmaprp_irq,
-			     0, MEM2MEM_NAME, pcdev) < 0) {
-		ret = -ENODEV;
+	irq = platform_get_irq(pdev, 0);
+	ret = devm_request_irq(&pdev->dev, irq, emmaprp_irq, 0,
+			       dev_name(&pdev->dev), pcdev);
+	if (ret)
 		goto rel_vdev;
-	}
 
 	pcdev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
 	if (IS_ERR(pcdev->alloc_ctx)) {
@@ -999,6 +985,8 @@ rel_vdev:
 unreg_dev:
 	v4l2_device_unregister(&pcdev->v4l2_dev);
 
+	mutex_destroy(&pcdev->dev_mutex);
+
 	return ret;
 }
 
@@ -1012,6 +1000,7 @@ static int emmaprp_remove(struct platform_device *pdev)
 	v4l2_m2m_release(pcdev->m2m_dev);
 	vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
 	v4l2_device_unregister(&pcdev->v4l2_dev);
+	mutex_destroy(&pcdev->dev_mutex);
 
 	return 0;
 }
diff --git a/drivers/media/platform/s3c-camif/camif-capture.c b/drivers/media/platform/s3c-camif/camif-capture.c
index 4e4d1631e04..deba425e3d8 100644
--- a/drivers/media/platform/s3c-camif/camif-capture.c
+++ b/drivers/media/platform/s3c-camif/camif-capture.c
@@ -435,10 +435,10 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 	return 0;
 }
 
-static int stop_streaming(struct vb2_queue *vq)
+static void stop_streaming(struct vb2_queue *vq)
 {
 	struct camif_vp *vp = vb2_get_drv_priv(vq);
-	return camif_stop_capture(vp);
+	camif_stop_capture(vp);
 }
 
 static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *pfmt,
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-core.c b/drivers/media/platform/s5p-jpeg/jpeg-core.c
index 8a18972012f..0dcb796ecad 100644
--- a/drivers/media/platform/s5p-jpeg/jpeg-core.c
+++ b/drivers/media/platform/s5p-jpeg/jpeg-core.c
@@ -192,8 +192,7 @@ static struct s5p_jpeg_fmt sjpeg_formats[] = {
 		.colplanes	= 2,
 		.h_align	= 4,
 		.v_align	= 4,
-		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
-				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+		.flags		= SJPEG_FMT_FLAG_DEC_CAPTURE |
 				  SJPEG_FMT_FLAG_S5P |
 				  SJPEG_FMT_NON_RGB,
 		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_420,
@@ -959,7 +958,7 @@ static int s5p_jpeg_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
 static struct s5p_jpeg_fmt *s5p_jpeg_find_format(struct s5p_jpeg_ctx *ctx,
 				u32 pixelformat, unsigned int fmt_type)
 {
-	unsigned int k, fmt_flag, ver_flag;
+	unsigned int k, fmt_flag;
 
 	if (ctx->mode == S5P_JPEG_ENCODE)
 		fmt_flag = (fmt_type == FMT_TYPE_OUTPUT) ?
@@ -970,16 +969,11 @@ static struct s5p_jpeg_fmt *s5p_jpeg_find_format(struct s5p_jpeg_ctx *ctx,
 				SJPEG_FMT_FLAG_DEC_OUTPUT :
 				SJPEG_FMT_FLAG_DEC_CAPTURE;
 
-	if (ctx->jpeg->variant->version == SJPEG_S5P)
-		ver_flag = SJPEG_FMT_FLAG_S5P;
-	else
-		ver_flag = SJPEG_FMT_FLAG_EXYNOS4;
-
 	for (k = 0; k < ARRAY_SIZE(sjpeg_formats); k++) {
 		struct s5p_jpeg_fmt *fmt = &sjpeg_formats[k];
 		if (fmt->fourcc == pixelformat &&
 		    fmt->flags & fmt_flag &&
-		    fmt->flags & ver_flag) {
+		    fmt->flags & ctx->jpeg->variant->fmt_ver_flag) {
 			return fmt;
 		}
 	}
@@ -1069,15 +1063,17 @@ static int s5p_jpeg_try_fmt_vid_cap(struct file *file, void *priv,
 		return -EINVAL;
 	}
 
+	if ((ctx->jpeg->variant->version != SJPEG_EXYNOS4) ||
+	    (ctx->mode != S5P_JPEG_DECODE))
+		goto exit;
+
 	/*
 	 * The exynos4x12 device requires resulting YUV image
 	 * subsampling not to be lower than the input jpeg subsampling.
 	 * If this requirement is not met then downgrade the requested
 	 * capture format to the one with subsampling equal to the input jpeg.
 	 */
-	if ((ctx->jpeg->variant->version != SJPEG_S5P) &&
-	    (ctx->mode == S5P_JPEG_DECODE) &&
-	    (fmt->flags & SJPEG_FMT_NON_RGB) &&
+	if ((fmt->flags & SJPEG_FMT_NON_RGB) &&
 	    (fmt->subsampling < ctx->subsampling)) {
 		ret = s5p_jpeg_adjust_fourcc_to_subsampling(ctx->subsampling,
 							    fmt->fourcc,
@@ -1090,6 +1086,23 @@ static int s5p_jpeg_try_fmt_vid_cap(struct file *file, void *priv,
 							FMT_TYPE_CAPTURE);
 	}
 
+	/*
+	 * Decompression of a JPEG file with 4:2:0 subsampling and odd
+	 * width to the YUV 4:2:0 compliant formats produces a raw image
+	 * with broken luma component. Adjust capture format to RGB565
+	 * in such a case.
+	 */
+	if (ctx->subsampling == V4L2_JPEG_CHROMA_SUBSAMPLING_420 &&
+	    (ctx->out_q.w & 1) &&
+	    (pix->pixelformat == V4L2_PIX_FMT_NV12 ||
+	     pix->pixelformat == V4L2_PIX_FMT_NV21 ||
+	     pix->pixelformat == V4L2_PIX_FMT_YUV420)) {
+		pix->pixelformat = V4L2_PIX_FMT_RGB565;
+		fmt = s5p_jpeg_find_format(ctx, pix->pixelformat,
+							FMT_TYPE_CAPTURE);
+	}
+
+exit:
 	return vidioc_try_fmt(f, fmt, ctx, FMT_TYPE_CAPTURE);
 }
 
@@ -1111,6 +1124,32 @@ static int s5p_jpeg_try_fmt_vid_out(struct file *file, void *priv,
 	return vidioc_try_fmt(f, fmt, ctx, FMT_TYPE_OUTPUT);
 }
 
+static int exynos4_jpeg_get_output_buffer_size(struct s5p_jpeg_ctx *ctx,
+						struct v4l2_format *f,
+						int fmt_depth)
+{
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	u32 pix_fmt = f->fmt.pix.pixelformat;
+	int w = pix->width, h = pix->height, wh_align;
+
+	if (pix_fmt == V4L2_PIX_FMT_RGB32 ||
+	    pix_fmt == V4L2_PIX_FMT_NV24 ||
+	    pix_fmt == V4L2_PIX_FMT_NV42 ||
+	    pix_fmt == V4L2_PIX_FMT_NV12 ||
+	    pix_fmt == V4L2_PIX_FMT_NV21 ||
+	    pix_fmt == V4L2_PIX_FMT_YUV420)
+		wh_align = 4;
+	else
+		wh_align = 1;
+
+	jpeg_bound_align_image(&w, S5P_JPEG_MIN_WIDTH,
+			       S5P_JPEG_MAX_WIDTH, wh_align,
+			       &h, S5P_JPEG_MIN_HEIGHT,
+			       S5P_JPEG_MAX_HEIGHT, wh_align);
+
+	return w * h * fmt_depth >> 3;
+}
+
 static int s5p_jpeg_s_fmt(struct s5p_jpeg_ctx *ct, struct v4l2_format *f)
 {
 	struct vb2_queue *vq;
@@ -1137,10 +1176,24 @@ static int s5p_jpeg_s_fmt(struct s5p_jpeg_ctx *ct, struct v4l2_format *f)
 	q_data->fmt = s5p_jpeg_find_format(ct, pix->pixelformat, f_type);
 	q_data->w = pix->width;
 	q_data->h = pix->height;
-	if (q_data->fmt->fourcc != V4L2_PIX_FMT_JPEG)
-		q_data->size = q_data->w * q_data->h * q_data->fmt->depth >> 3;
-	else
+	if (q_data->fmt->fourcc != V4L2_PIX_FMT_JPEG) {
+		/*
+		 * During encoding Exynos4x12 SoCs access wider memory area
+		 * than it results from Image_x and Image_y values written to
+		 * the JPEG_IMAGE_SIZE register. In order to avoid sysmmu
+		 * page fault calculate proper buffer size in such a case.
+		 */
+		if (ct->jpeg->variant->version == SJPEG_EXYNOS4 &&
+		    f_type == FMT_TYPE_OUTPUT && ct->mode == S5P_JPEG_ENCODE)
+			q_data->size = exynos4_jpeg_get_output_buffer_size(ct,
+							f,
+							q_data->fmt->depth);
+		else
+			q_data->size = q_data->w * q_data->h *
+						q_data->fmt->depth >> 3;
+	} else {
 		q_data->size = pix->sizeimage;
+	}
 
 	if (f_type == FMT_TYPE_OUTPUT) {
 		ctrl_subs = v4l2_ctrl_find(&ct->ctrl_handler,
@@ -1182,8 +1235,7 @@ static int s5p_jpeg_g_selection(struct file *file, void *priv,
 	struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
 
 	if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT &&
-	    s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
-	    ctx->jpeg->variant->version != SJPEG_S5P)
+	    s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
 
 	/* For JPEG blob active == default == bounds */
@@ -1571,7 +1623,7 @@ static struct v4l2_m2m_ops s5p_jpeg_m2m_ops = {
 	.job_abort	= s5p_jpeg_job_abort,
 }
 ;
-static struct v4l2_m2m_ops exynos_jpeg_m2m_ops = {
+static struct v4l2_m2m_ops exynos4_jpeg_m2m_ops = {
 	.device_run	= exynos4_jpeg_device_run,
 	.job_ready	= s5p_jpeg_job_ready,
 	.job_abort	= s5p_jpeg_job_abort,
@@ -1670,13 +1722,11 @@ static int s5p_jpeg_start_streaming(struct vb2_queue *q, unsigned int count)
 	return ret > 0 ? 0 : ret;
 }
 
-static int s5p_jpeg_stop_streaming(struct vb2_queue *q)
+static void s5p_jpeg_stop_streaming(struct vb2_queue *q)
 {
 	struct s5p_jpeg_ctx *ctx = vb2_get_drv_priv(q);
 
 	pm_runtime_put(ctx->jpeg->dev);
-
-	return 0;
 }
 
 static struct vb2_ops s5p_jpeg_qops = {
@@ -1845,7 +1895,7 @@ static irqreturn_t exynos4_jpeg_irq(int irq, void *priv)
 	return IRQ_HANDLED;
 }
 
-static void *jpeg_get_drv_data(struct platform_device *pdev);
+static void *jpeg_get_drv_data(struct device *dev);
 
 /*
  * ============================================================================
@@ -1857,18 +1907,14 @@ static int s5p_jpeg_probe(struct platform_device *pdev)
 {
 	struct s5p_jpeg *jpeg;
 	struct resource *res;
-	struct v4l2_m2m_ops *samsung_jpeg_m2m_ops;
 	int ret;
 
-	if (!pdev->dev.of_node)
-		return -ENODEV;
-
 	/* JPEG IP abstraction struct */
 	jpeg = devm_kzalloc(&pdev->dev, sizeof(struct s5p_jpeg), GFP_KERNEL);
 	if (!jpeg)
 		return -ENOMEM;
 
-	jpeg->variant = jpeg_get_drv_data(pdev);
+	jpeg->variant = jpeg_get_drv_data(&pdev->dev);
 
 	mutex_init(&jpeg->lock);
 	spin_lock_init(&jpeg->slock);
@@ -1911,13 +1957,8 @@ static int s5p_jpeg_probe(struct platform_device *pdev)
 		goto clk_get_rollback;
 	}
 
-	if (jpeg->variant->version == SJPEG_S5P)
-		samsung_jpeg_m2m_ops = &s5p_jpeg_m2m_ops;
-	else
-		samsung_jpeg_m2m_ops = &exynos_jpeg_m2m_ops;
-
 	/* mem2mem device */
-	jpeg->m2m_dev = v4l2_m2m_init(samsung_jpeg_m2m_ops);
+	jpeg->m2m_dev = v4l2_m2m_init(jpeg->variant->m2m_ops);
 	if (IS_ERR(jpeg->m2m_dev)) {
 		v4l2_err(&jpeg->v4l2_dev, "Failed to init mem2mem device\n");
 		ret = PTR_ERR(jpeg->m2m_dev);
@@ -2102,15 +2143,18 @@ static const struct dev_pm_ops s5p_jpeg_pm_ops = {
 	SET_RUNTIME_PM_OPS(s5p_jpeg_runtime_suspend, s5p_jpeg_runtime_resume, NULL)
 };
 
-#ifdef CONFIG_OF
 static struct s5p_jpeg_variant s5p_jpeg_drvdata = {
 	.version	= SJPEG_S5P,
 	.jpeg_irq	= s5p_jpeg_irq,
+	.m2m_ops	= &s5p_jpeg_m2m_ops,
+	.fmt_ver_flag	= SJPEG_FMT_FLAG_S5P,
 };
 
 static struct s5p_jpeg_variant exynos4_jpeg_drvdata = {
 	.version	= SJPEG_EXYNOS4,
 	.jpeg_irq	= exynos4_jpeg_irq,
+	.m2m_ops	= &exynos4_jpeg_m2m_ops,
+	.fmt_ver_flag	= SJPEG_FMT_FLAG_EXYNOS4,
 };
 
 static const struct of_device_id samsung_jpeg_match[] = {
@@ -2129,19 +2173,21 @@ static const struct of_device_id samsung_jpeg_match[] = {
 
 MODULE_DEVICE_TABLE(of, samsung_jpeg_match);
 
-static void *jpeg_get_drv_data(struct platform_device *pdev)
+static void *jpeg_get_drv_data(struct device *dev)
 {
 	struct s5p_jpeg_variant *driver_data = NULL;
 	const struct of_device_id *match;
 
-	match = of_match_node(of_match_ptr(samsung_jpeg_match),
-					 pdev->dev.of_node);
+	if (!IS_ENABLED(CONFIG_OF) || !dev->of_node)
+		return &s5p_jpeg_drvdata;
+
+	match = of_match_node(samsung_jpeg_match, dev->of_node);
+
 	if (match)
 		driver_data = (struct s5p_jpeg_variant *)match->data;
 
 	return driver_data;
 }
-#endif
 
 static struct platform_driver s5p_jpeg_driver = {
 	.probe = s5p_jpeg_probe,
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-core.h b/drivers/media/platform/s5p-jpeg/jpeg-core.h
index f482dbf55d5..3e478632972 100644
--- a/drivers/media/platform/s5p-jpeg/jpeg-core.h
+++ b/drivers/media/platform/s5p-jpeg/jpeg-core.h
@@ -117,8 +117,10 @@ struct s5p_jpeg {
 };
 
 struct s5p_jpeg_variant {
-	unsigned int	version;
-	irqreturn_t	(*jpeg_irq)(int irq, void *priv);
+	unsigned int		version;
+	unsigned int		fmt_ver_flag;
+	struct v4l2_m2m_ops	*m2m_ops;
+	irqreturn_t		(*jpeg_irq)(int irq, void *priv);
 };
 
 /**
diff --git a/drivers/media/platform/s5p-mfc/regs-mfc-v6.h b/drivers/media/platform/s5p-mfc/regs-mfc-v6.h
index 8d0b686d9ad..51cb2dd0e13 100644
--- a/drivers/media/platform/s5p-mfc/regs-mfc-v6.h
+++ b/drivers/media/platform/s5p-mfc/regs-mfc-v6.h
@@ -141,6 +141,7 @@
 #define S5P_FIMV_D_SLICE_IF_ENABLE_V6		0xf4c4
 #define S5P_FIMV_D_PICTURE_TAG_V6		0xf4c8
 #define S5P_FIMV_D_STREAM_DATA_SIZE_V6		0xf4d0
+#define S5P_FIMV_D_INIT_BUFFER_OPTIONS_V6	0xf47c
 
 /* Display information register */
 #define S5P_FIMV_D_DISPLAY_FRAME_WIDTH_V6	0xf500
@@ -381,8 +382,7 @@
 	 (DIV_ROUND_UP((mbw) * (mbh), 32) * 16))
 #define S5P_FIMV_SCRATCH_BUF_SIZE_H264_DEC_V6(w, h)	(((w) * 192) + 64)
 #define S5P_FIMV_SCRATCH_BUF_SIZE_MPEG4_DEC_V6(w, h) \
-			((w) * ((h) * 64 + 144) + (2048/16 * (h) * 64) + \
-			 (2048/16 * 256 + 8320))
+			((w) * 144 + 8192 * (h) + 49216 + 1048576)
 #define S5P_FIMV_SCRATCH_BUF_SIZE_VC1_DEC_V6(w, h) \
 						(2096 * ((w) + (h) + 1))
 #define S5P_FIMV_SCRATCH_BUF_SIZE_H263_DEC_V6(w, h)	((w) * 400)
diff --git a/drivers/media/platform/s5p-mfc/regs-mfc-v7.h b/drivers/media/platform/s5p-mfc/regs-mfc-v7.h
index ea5ec2a711a..1a5c6fdf784 100644
--- a/drivers/media/platform/s5p-mfc/regs-mfc-v7.h
+++ b/drivers/media/platform/s5p-mfc/regs-mfc-v7.h
@@ -18,8 +18,6 @@
 #define S5P_FIMV_CODEC_VP8_ENC_V7	25
 
 /* Additional registers for v7 */
-#define S5P_FIMV_D_INIT_BUFFER_OPTIONS_V7		0xf47c
-
 #define S5P_FIMV_E_SOURCE_FIRST_ADDR_V7			0xf9e0
 #define S5P_FIMV_E_SOURCE_SECOND_ADDR_V7		0xf9e4
 #define S5P_FIMV_E_SOURCE_THIRD_ADDR_V7			0xf9e8
@@ -56,6 +54,7 @@
 			(SZ_1M + ((w) * 144) + (8192 * (h)) + 49216)
 
 #define S5P_FIMV_SCRATCH_BUF_SIZE_VP8_ENC_V7(w, h) \
-			(((w) * 48) + (((w) + 1) / 2 * 128) + 144 + 8192)
+			(((w) * 48) + 8192 + ((((w) + 1) / 2) * 128) + 144 + \
+			((((((w) * 16) * ((h) * 16)) * 3) / 2) * 4))
 
 #endif /*_REGS_MFC_V7_H*/
diff --git a/drivers/media/platform/s5p-mfc/regs-mfc-v8.h b/drivers/media/platform/s5p-mfc/regs-mfc-v8.h
new file mode 100644
index 00000000000..cc7cbec51b5
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/regs-mfc-v8.h
@@ -0,0 +1,124 @@
+/*
+ * Register definition file for Samsung MFC V8.x Interface (FIMV) driver
+ *
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ *		http://www.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 _REGS_MFC_V8_H
+#define _REGS_MFC_V8_H
+
+#include <linux/sizes.h>
+#include "regs-mfc-v7.h"
+
+/* Additional registers for v8 */
+#define S5P_FIMV_D_MVC_NUM_VIEWS_V8		0xf104
+#define S5P_FIMV_D_FIRST_PLANE_DPB_SIZE_V8	0xf144
+#define S5P_FIMV_D_SECOND_PLANE_DPB_SIZE_V8	0xf148
+#define S5P_FIMV_D_MV_BUFFER_SIZE_V8		0xf150
+
+#define S5P_FIMV_D_FIRST_PLANE_DPB_STRIDE_SIZE_V8	0xf138
+#define S5P_FIMV_D_SECOND_PLANE_DPB_STRIDE_SIZE_V8	0xf13c
+
+#define S5P_FIMV_D_FIRST_PLANE_DPB_V8		0xf160
+#define S5P_FIMV_D_SECOND_PLANE_DPB_V8		0xf260
+#define S5P_FIMV_D_MV_BUFFER_V8			0xf460
+
+#define S5P_FIMV_D_NUM_MV_V8			0xf134
+#define S5P_FIMV_D_INIT_BUFFER_OPTIONS_V8	0xf154
+
+#define S5P_FIMV_D_SCRATCH_BUFFER_ADDR_V8	0xf560
+#define S5P_FIMV_D_SCRATCH_BUFFER_SIZE_V8	0xf564
+
+#define S5P_FIMV_D_CPB_BUFFER_ADDR_V8		0xf5b0
+#define S5P_FIMV_D_CPB_BUFFER_SIZE_V8		0xf5b4
+#define S5P_FIMV_D_AVAILABLE_DPB_FLAG_LOWER_V8	0xf5bc
+#define S5P_FIMV_D_CPB_BUFFER_OFFSET_V8		0xf5c0
+#define S5P_FIMV_D_SLICE_IF_ENABLE_V8		0xf5c4
+#define S5P_FIMV_D_STREAM_DATA_SIZE_V8		0xf5d0
+
+/* Display information register */
+#define S5P_FIMV_D_DISPLAY_FRAME_WIDTH_V8	0xf600
+#define S5P_FIMV_D_DISPLAY_FRAME_HEIGHT_V8	0xf604
+
+/* Display status */
+#define S5P_FIMV_D_DISPLAY_STATUS_V8		0xf608
+
+#define S5P_FIMV_D_DISPLAY_FIRST_PLANE_ADDR_V8	0xf60c
+#define S5P_FIMV_D_DISPLAY_SECOND_PLANE_ADDR_V8	0xf610
+
+#define S5P_FIMV_D_DISPLAY_FRAME_TYPE_V8	0xf618
+#define S5P_FIMV_D_DISPLAY_CROP_INFO1_V8	0xf61c
+#define S5P_FIMV_D_DISPLAY_CROP_INFO2_V8	0xf620
+#define S5P_FIMV_D_DISPLAY_PICTURE_PROFILE_V8	0xf624
+
+/* Decoded picture information register */
+#define S5P_FIMV_D_DECODED_STATUS_V8		0xf644
+#define S5P_FIMV_D_DECODED_FIRST_PLANE_ADDR_V8	0xf648
+#define S5P_FIMV_D_DECODED_SECOND_PLANE_ADDR_V8	0xf64c
+#define S5P_FIMV_D_DECODED_THIRD_PLANE_ADDR_V8	0xf650
+#define S5P_FIMV_D_DECODED_FRAME_TYPE_V8	0xf654
+#define S5P_FIMV_D_DECODED_NAL_SIZE_V8          0xf664
+
+/* Returned value register for specific setting */
+#define S5P_FIMV_D_RET_PICTURE_TAG_TOP_V8	0xf674
+#define S5P_FIMV_D_RET_PICTURE_TAG_BOT_V8	0xf678
+#define S5P_FIMV_D_MVC_VIEW_ID_V8		0xf6d8
+
+/* SEI related information */
+#define S5P_FIMV_D_FRAME_PACK_SEI_AVAIL_V8	0xf6dc
+
+/* Encoder Registers */
+#define S5P_FIMV_E_FIXED_PICTURE_QP_V8		0xf794
+#define S5P_FIMV_E_RC_CONFIG_V8			0xf798
+#define S5P_FIMV_E_RC_QP_BOUND_V8		0xf79c
+#define S5P_FIMV_E_RC_RPARAM_V8			0xf7a4
+#define S5P_FIMV_E_MB_RC_CONFIG_V8		0xf7a8
+#define S5P_FIMV_E_PADDING_CTRL_V8		0xf7ac
+#define S5P_FIMV_E_MV_HOR_RANGE_V8		0xf7b4
+#define S5P_FIMV_E_MV_VER_RANGE_V8		0xf7b8
+
+#define S5P_FIMV_E_VBV_BUFFER_SIZE_V8		0xf78c
+#define S5P_FIMV_E_VBV_INIT_DELAY_V8		0xf790
+
+#define S5P_FIMV_E_ASPECT_RATIO_V8		0xfb4c
+#define S5P_FIMV_E_EXTENDED_SAR_V8		0xfb50
+#define S5P_FIMV_E_H264_OPTIONS_V8		0xfb54
+
+/* MFCv8 Context buffer sizes */
+#define MFC_CTX_BUF_SIZE_V8		(30 * SZ_1K)	/*  30KB */
+#define MFC_H264_DEC_CTX_BUF_SIZE_V8	(2 * SZ_1M)	/*  2MB */
+#define MFC_OTHER_DEC_CTX_BUF_SIZE_V8	(20 * SZ_1K)	/*  20KB */
+#define MFC_H264_ENC_CTX_BUF_SIZE_V8	(100 * SZ_1K)	/* 100KB */
+#define MFC_OTHER_ENC_CTX_BUF_SIZE_V8	(10 * SZ_1K)	/*  10KB */
+
+/* Buffer size defines */
+#define S5P_FIMV_TMV_BUFFER_SIZE_V8(w, h)	(((w) + 1) * ((h) + 1) * 8)
+
+#define S5P_FIMV_SCRATCH_BUF_SIZE_H264_DEC_V8(w, h)	(((w) * 704) + 2176)
+#define S5P_FIMV_SCRATCH_BUF_SIZE_VP8_DEC_V8(w, h) \
+		(((w) * 576 + (h) * 128)  + 4128)
+
+#define S5P_FIMV_SCRATCH_BUF_SIZE_H264_ENC_V8(w, h) \
+			(((w) * 592) + 2336)
+#define S5P_FIMV_SCRATCH_BUF_SIZE_VP8_ENC_V8(w, h) \
+			(((w) * 576) + 10512 + \
+			((((((w) * 16) * ((h) * 16)) * 3) / 2) * 4))
+#define S5P_FIMV_ME_BUFFER_SIZE_V8(imw, imh, mbw, mbh) \
+	((DIV_ROUND_UP((mbw * 16), 64) *  DIV_ROUND_UP((mbh * 16), 64) * 256) \
+	 + (DIV_ROUND_UP((mbw) * (mbh), 32) * 16))
+
+/* BUffer alignment defines */
+#define S5P_FIMV_D_ALIGN_PLANE_SIZE_V8	64
+
+/* MFCv8 variant defines */
+#define MAX_FW_SIZE_V8			(SZ_1M)		/* 1MB */
+#define MAX_CPB_SIZE_V8			(3 * SZ_1M)	/* 3MB */
+#define MFC_VERSION_V8			0x80
+#define MFC_NUM_PORTS_V8		1
+
+#endif /*_REGS_MFC_V8_H*/
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc.c b/drivers/media/platform/s5p-mfc/s5p_mfc.c
index 89356ae9023..41723180d10 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc.c
@@ -309,12 +309,15 @@ static void s5p_mfc_handle_frame(struct s5p_mfc_ctx *ctx,
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	unsigned int dst_frame_status;
+	unsigned int dec_frame_status;
 	struct s5p_mfc_buf *src_buf;
 	unsigned long flags;
 	unsigned int res_change;
 
 	dst_frame_status = s5p_mfc_hw_call(dev->mfc_ops, get_dspl_status, dev)
 				& S5P_FIMV_DEC_STATUS_DECODING_STATUS_MASK;
+	dec_frame_status = s5p_mfc_hw_call(dev->mfc_ops, get_dec_status, dev)
+				& S5P_FIMV_DEC_STATUS_DECODING_STATUS_MASK;
 	res_change = (s5p_mfc_hw_call(dev->mfc_ops, get_dspl_status, dev)
 				& S5P_FIMV_DEC_STATUS_RESOLUTION_MASK)
 				>> S5P_FIMV_DEC_STATUS_RESOLUTION_SHIFT;
@@ -339,16 +342,23 @@ static void s5p_mfc_handle_frame(struct s5p_mfc_ctx *ctx,
 	/* All frames remaining in the buffer have been extracted  */
 	if (dst_frame_status == S5P_FIMV_DEC_STATUS_DECODING_EMPTY) {
 		if (ctx->state == MFCINST_RES_CHANGE_FLUSH) {
+			static const struct v4l2_event ev_src_ch = {
+				.type = V4L2_EVENT_SOURCE_CHANGE,
+				.u.src_change.changes =
+					V4L2_EVENT_SRC_CH_RESOLUTION,
+			};
+
 			s5p_mfc_handle_frame_all_extracted(ctx);
 			ctx->state = MFCINST_RES_CHANGE_END;
+			v4l2_event_queue_fh(&ctx->fh, &ev_src_ch);
+
 			goto leave_handle_frame;
 		} else {
 			s5p_mfc_handle_frame_all_extracted(ctx);
 		}
 	}
 
-	if (dst_frame_status == S5P_FIMV_DEC_STATUS_DECODING_DISPLAY ||
-		dst_frame_status == S5P_FIMV_DEC_STATUS_DECODING_ONLY)
+	if (dec_frame_status == S5P_FIMV_DEC_STATUS_DECODING_DISPLAY)
 		s5p_mfc_handle_frame_copy_time(ctx);
 
 	/* A frame has been decoded and is in the buffer  */
@@ -366,6 +376,7 @@ static void s5p_mfc_handle_frame(struct s5p_mfc_ctx *ctx,
 		ctx->consumed_stream += s5p_mfc_hw_call(dev->mfc_ops,
 						get_consumed_stream, dev);
 		if (ctx->codec_mode != S5P_MFC_CODEC_H264_DEC &&
+			ctx->codec_mode != S5P_MFC_CODEC_VP8_DEC &&
 			ctx->consumed_stream + STUFF_BYTE <
 			src_buf->b->v4l2_planes[0].bytesused) {
 			/* Run MFC again on the same buffer */
@@ -641,6 +652,7 @@ static irqreturn_t s5p_mfc_irq(int irq, void *priv)
 
 	case S5P_MFC_R2H_CMD_CLOSE_INSTANCE_RET:
 		clear_work_bit(ctx);
+		ctx->inst_no = MFC_NO_INSTANCE_SET;
 		ctx->state = MFCINST_FREE;
 		wake_up(&ctx->queue);
 		goto irq_cleanup_hw;
@@ -761,7 +773,7 @@ static int s5p_mfc_open(struct file *file)
 		goto err_bad_node;
 	}
 	ctx->fh.ctrl_handler = &ctx->ctrl_handler;
-	ctx->inst_no = -1;
+	ctx->inst_no = MFC_NO_INSTANCE_SET;
 	/* Load firmware if this is the first instance */
 	if (dev->num_inst == 1) {
 		dev->watchdog_timer.expires = jiffies +
@@ -871,29 +883,11 @@ static int s5p_mfc_release(struct file *file)
 	vb2_queue_release(&ctx->vq_dst);
 	/* Mark context as idle */
 	clear_work_bit_irqsave(ctx);
-	/* If instance was initialised then
+	/* If instance was initialised and not yet freed,
 	 * return instance and free resources */
-	if (ctx->inst_no != MFC_NO_INSTANCE_SET) {
+	if (ctx->state != MFCINST_FREE && ctx->state != MFCINST_INIT) {
 		mfc_debug(2, "Has to free instance\n");
-		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);
-		/* Wait until instance is returned or timeout occurred */
-		if (s5p_mfc_wait_for_done_ctx
-		    (ctx, S5P_MFC_R2H_CMD_CLOSE_INSTANCE_RET, 0)) {
-			s5p_mfc_clock_off();
-			mfc_err("Err returning instance\n");
-		}
-		mfc_debug(2, "After free 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);
-		if (ctx->type == MFCINST_DECODER)
-			s5p_mfc_hw_call(dev->mfc_ops, release_dec_desc_buffer,
-					ctx);
-
-		ctx->inst_no = MFC_NO_INSTANCE_SET;
+		s5p_mfc_close_mfc_inst(dev, ctx);
 	}
 	/* hardware locking scheme */
 	if (dev->curr_ctx == ctx->num)
@@ -1207,6 +1201,7 @@ static int s5p_mfc_probe(struct platform_device *pdev)
 	/* Initialize HW ops and commands based on MFC version */
 	s5p_mfc_init_hw_ops(dev);
 	s5p_mfc_init_hw_cmds(dev);
+	s5p_mfc_init_regs(dev);
 
 	pr_debug("%s--\n", __func__);
 	return 0;
@@ -1352,6 +1347,7 @@ struct s5p_mfc_buf_align mfc_buf_align_v5 = {
 
 static struct s5p_mfc_variant mfc_drvdata_v5 = {
 	.version	= MFC_VERSION,
+	.version_bit	= MFC_V5_BIT,
 	.port_num	= MFC_NUM_PORTS,
 	.buf_size	= &buf_size_v5,
 	.buf_align	= &mfc_buf_align_v5,
@@ -1378,6 +1374,7 @@ struct s5p_mfc_buf_align mfc_buf_align_v6 = {
 
 static struct s5p_mfc_variant mfc_drvdata_v6 = {
 	.version	= MFC_VERSION_V6,
+	.version_bit	= MFC_V6_BIT,
 	.port_num	= MFC_NUM_PORTS_V6,
 	.buf_size	= &buf_size_v6,
 	.buf_align	= &mfc_buf_align_v6,
@@ -1404,12 +1401,40 @@ struct s5p_mfc_buf_align mfc_buf_align_v7 = {
 
 static struct s5p_mfc_variant mfc_drvdata_v7 = {
 	.version	= MFC_VERSION_V7,
+	.version_bit	= MFC_V7_BIT,
 	.port_num	= MFC_NUM_PORTS_V7,
 	.buf_size	= &buf_size_v7,
 	.buf_align	= &mfc_buf_align_v7,
 	.fw_name        = "s5p-mfc-v7.fw",
 };
 
+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,
+	.h264_enc_ctx	= MFC_H264_ENC_CTX_BUF_SIZE_V8,
+	.other_enc_ctx	= MFC_OTHER_ENC_CTX_BUF_SIZE_V8,
+};
+
+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 = {
+	.base = 0,
+};
+
+static struct s5p_mfc_variant mfc_drvdata_v8 = {
+	.version	= MFC_VERSION_V8,
+	.version_bit	= MFC_V8_BIT,
+	.port_num	= MFC_NUM_PORTS_V8,
+	.buf_size	= &buf_size_v8,
+	.buf_align	= &mfc_buf_align_v8,
+	.fw_name        = "s5p-mfc-v8.fw",
+};
+
 static struct platform_device_id mfc_driver_ids[] = {
 	{
 		.name = "s5p-mfc",
@@ -1423,6 +1448,9 @@ static struct platform_device_id mfc_driver_ids[] = {
 	}, {
 		.name = "s5p-mfc-v7",
 		.driver_data = (unsigned long)&mfc_drvdata_v7,
+	}, {
+		.name = "s5p-mfc-v8",
+		.driver_data = (unsigned long)&mfc_drvdata_v8,
 	},
 	{},
 };
@@ -1438,6 +1466,9 @@ static const struct of_device_id exynos_mfc_match[] = {
 	}, {
 		.compatible = "samsung,mfc-v7",
 		.data = &mfc_drvdata_v7,
+	}, {
+		.compatible = "samsung,mfc-v8",
+		.data = &mfc_drvdata_v8,
 	},
 	{},
 };
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_common.h b/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
index 5c28cc3e699..b04360cd34f 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
@@ -23,8 +23,7 @@
 #include <media/v4l2-ioctl.h>
 #include <media/videobuf2-core.h>
 #include "regs-mfc.h"
-#include "regs-mfc-v6.h"
-#include "regs-mfc-v7.h"
+#include "regs-mfc-v8.h"
 
 /* Definitions related to MFC memory */
 
@@ -223,6 +222,7 @@ struct s5p_mfc_buf_align {
 struct s5p_mfc_variant {
 	unsigned int version;
 	unsigned int port_num;
+	u32 version_bit;
 	struct s5p_mfc_buf_size *buf_size;
 	struct s5p_mfc_buf_align *buf_align;
 	char	*fw_name;
@@ -330,6 +330,7 @@ struct s5p_mfc_dev {
 	int warn_start;
 	struct s5p_mfc_hw_ops *mfc_ops;
 	struct s5p_mfc_hw_cmds *mfc_cmds;
+	const struct s5p_mfc_regs *mfc_regs;
 };
 
 /**
@@ -663,6 +664,7 @@ struct s5p_mfc_fmt {
 	u32 codec_mode;
 	enum s5p_mfc_fmt_type type;
 	u32 num_planes;
+	u32 versions;
 };
 
 /**
@@ -700,6 +702,13 @@ void set_work_bit_irqsave(struct s5p_mfc_ctx *ctx);
 				(dev->variant->port_num ? 1 : 0) : 0) : 0)
 #define IS_TWOPORT(dev)		(dev->variant->port_num == 2 ? 1 : 0)
 #define IS_MFCV6_PLUS(dev)	(dev->variant->version >= 0x60 ? 1 : 0)
-#define IS_MFCV7(dev)		(dev->variant->version >= 0x70 ? 1 : 0)
+#define IS_MFCV7_PLUS(dev)	(dev->variant->version >= 0x70 ? 1 : 0)
+#define IS_MFCV8(dev)		(dev->variant->version >= 0x80 ? 1 : 0)
+
+#define MFC_V5_BIT	BIT(0)
+#define MFC_V6_BIT	BIT(1)
+#define MFC_V7_BIT	BIT(2)
+#define MFC_V8_BIT	BIT(3)
+
 
 #endif /* S5P_MFC_COMMON_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
index ee05f2dd439..6c3f8f74390 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
@@ -400,3 +400,65 @@ int s5p_mfc_wakeup(struct s5p_mfc_dev *dev)
 	return 0;
 }
 
+int s5p_mfc_open_mfc_inst(struct s5p_mfc_dev *dev, struct s5p_mfc_ctx *ctx)
+{
+	int ret = 0;
+
+	ret = s5p_mfc_hw_call(dev->mfc_ops, alloc_instance_buffer, ctx);
+	if (ret) {
+		mfc_err("Failed allocating instance buffer\n");
+		goto err;
+	}
+
+	if (ctx->type == MFCINST_DECODER) {
+		ret = s5p_mfc_hw_call(dev->mfc_ops,
+					alloc_dec_temp_buffers, ctx);
+		if (ret) {
+			mfc_err("Failed allocating temporary buffers\n");
+			goto err_free_inst_buf;
+		}
+	}
+
+	set_work_bit_irqsave(ctx);
+	s5p_mfc_clean_ctx_int_flags(ctx);
+	s5p_mfc_hw_call(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 */
+		mfc_err("Error getting instance from hardware\n");
+		ret = -EIO;
+		goto err_free_desc_buf;
+	}
+
+	mfc_debug(2, "Got instance number: %d\n", ctx->inst_no);
+	return ret;
+
+err_free_desc_buf:
+	if (ctx->type == MFCINST_DECODER)
+		s5p_mfc_hw_call(dev->mfc_ops, release_dec_desc_buffer, ctx);
+err_free_inst_buf:
+	s5p_mfc_hw_call(dev->mfc_ops, release_instance_buffer, ctx);
+err:
+	return ret;
+}
+
+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);
+	/* 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);
+	if (ctx->type == MFCINST_DECODER)
+		s5p_mfc_hw_call(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_ctrl.h b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.h
index 6a9b6f8606b..8e5df041edf 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.h
@@ -28,4 +28,7 @@ int s5p_mfc_wakeup(struct s5p_mfc_dev *dev);
 
 int s5p_mfc_reset(struct s5p_mfc_dev *dev);
 
+int s5p_mfc_open_mfc_inst(struct s5p_mfc_dev *dev, struct s5p_mfc_ctx *ctx);
+void s5p_mfc_close_mfc_inst(struct s5p_mfc_dev *dev, struct s5p_mfc_ctx *ctx);
+
 #endif /* S5P_MFC_CTRL_H */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
index 8faf9691712..4d93835dec9 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
@@ -25,15 +25,13 @@
 #include <media/v4l2-event.h>
 #include <media/videobuf2-core.h>
 #include "s5p_mfc_common.h"
+#include "s5p_mfc_ctrl.h"
 #include "s5p_mfc_debug.h"
 #include "s5p_mfc_dec.h"
 #include "s5p_mfc_intr.h"
 #include "s5p_mfc_opr.h"
 #include "s5p_mfc_pm.h"
 
-#define DEF_SRC_FMT_DEC	V4L2_PIX_FMT_H264
-#define DEF_DST_FMT_DEC	V4L2_PIX_FMT_NV12MT_16X16
-
 static struct s5p_mfc_fmt formats[] = {
 	{
 		.name		= "4:2:0 2 Planes 16x16 Tiles",
@@ -41,6 +39,7 @@ static struct s5p_mfc_fmt formats[] = {
 		.codec_mode	= S5P_MFC_CODEC_NONE,
 		.type		= MFC_FMT_RAW,
 		.num_planes	= 2,
+		.versions	= MFC_V6_BIT | MFC_V7_BIT,
 	},
 	{
 		.name		= "4:2:0 2 Planes 64x32 Tiles",
@@ -48,6 +47,7 @@ static struct s5p_mfc_fmt formats[] = {
 		.codec_mode	= S5P_MFC_CODEC_NONE,
 		.type		= MFC_FMT_RAW,
 		.num_planes	= 2,
+		.versions	= MFC_V5_BIT,
 	},
 	{
 		.name		= "4:2:0 2 Planes Y/CbCr",
@@ -55,6 +55,7 @@ static struct s5p_mfc_fmt formats[] = {
 		.codec_mode	= S5P_MFC_CODEC_NONE,
 		.type		= MFC_FMT_RAW,
 		.num_planes	= 2,
+		.versions	= MFC_V6_BIT | MFC_V7_BIT | MFC_V8_BIT,
 	},
 	{
 		.name		= "4:2:0 2 Planes Y/CrCb",
@@ -62,6 +63,7 @@ static struct s5p_mfc_fmt formats[] = {
 		.codec_mode	= S5P_MFC_CODEC_NONE,
 		.type		= MFC_FMT_RAW,
 		.num_planes	= 2,
+		.versions	= MFC_V6_BIT | MFC_V7_BIT | MFC_V8_BIT,
 	},
 	{
 		.name		= "H264 Encoded Stream",
@@ -69,6 +71,8 @@ static struct s5p_mfc_fmt formats[] = {
 		.codec_mode	= S5P_MFC_CODEC_H264_DEC,
 		.type		= MFC_FMT_DEC,
 		.num_planes	= 1,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
 	},
 	{
 		.name		= "H264/MVC Encoded Stream",
@@ -76,6 +80,7 @@ static struct s5p_mfc_fmt formats[] = {
 		.codec_mode	= S5P_MFC_CODEC_H264_MVC_DEC,
 		.type		= MFC_FMT_DEC,
 		.num_planes	= 1,
+		.versions	= MFC_V6_BIT | MFC_V7_BIT | MFC_V8_BIT,
 	},
 	{
 		.name		= "H263 Encoded Stream",
@@ -83,6 +88,8 @@ static struct s5p_mfc_fmt formats[] = {
 		.codec_mode	= S5P_MFC_CODEC_H263_DEC,
 		.type		= MFC_FMT_DEC,
 		.num_planes	= 1,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
 	},
 	{
 		.name		= "MPEG1 Encoded Stream",
@@ -90,6 +97,8 @@ static struct s5p_mfc_fmt formats[] = {
 		.codec_mode	= S5P_MFC_CODEC_MPEG2_DEC,
 		.type		= MFC_FMT_DEC,
 		.num_planes	= 1,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
 	},
 	{
 		.name		= "MPEG2 Encoded Stream",
@@ -97,6 +106,8 @@ static struct s5p_mfc_fmt formats[] = {
 		.codec_mode	= S5P_MFC_CODEC_MPEG2_DEC,
 		.type		= MFC_FMT_DEC,
 		.num_planes	= 1,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
 	},
 	{
 		.name		= "MPEG4 Encoded Stream",
@@ -104,6 +115,8 @@ static struct s5p_mfc_fmt formats[] = {
 		.codec_mode	= S5P_MFC_CODEC_MPEG4_DEC,
 		.type		= MFC_FMT_DEC,
 		.num_planes	= 1,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
 	},
 	{
 		.name		= "XviD Encoded Stream",
@@ -111,6 +124,8 @@ static struct s5p_mfc_fmt formats[] = {
 		.codec_mode	= S5P_MFC_CODEC_MPEG4_DEC,
 		.type		= MFC_FMT_DEC,
 		.num_planes	= 1,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
 	},
 	{
 		.name		= "VC1 Encoded Stream",
@@ -118,6 +133,8 @@ static struct s5p_mfc_fmt formats[] = {
 		.codec_mode	= S5P_MFC_CODEC_VC1_DEC,
 		.type		= MFC_FMT_DEC,
 		.num_planes	= 1,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
 	},
 	{
 		.name		= "VC1 RCV Encoded Stream",
@@ -125,6 +142,8 @@ static struct s5p_mfc_fmt formats[] = {
 		.codec_mode	= S5P_MFC_CODEC_VC1RCV_DEC,
 		.type		= MFC_FMT_DEC,
 		.num_planes	= 1,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
 	},
 	{
 		.name		= "VP8 Encoded Stream",
@@ -132,6 +151,7 @@ static struct s5p_mfc_fmt formats[] = {
 		.codec_mode	= S5P_MFC_CODEC_VP8_DEC,
 		.type		= MFC_FMT_DEC,
 		.num_planes	= 1,
+		.versions	= MFC_V6_BIT | MFC_V7_BIT | MFC_V8_BIT,
 	},
 };
 
@@ -262,8 +282,10 @@ static int vidioc_querycap(struct file *file, void *priv,
 }
 
 /* Enumerate format */
-static int vidioc_enum_fmt(struct v4l2_fmtdesc *f, bool mplane, bool out)
+static int vidioc_enum_fmt(struct file *file, struct v4l2_fmtdesc *f,
+							bool mplane, bool out)
 {
+	struct s5p_mfc_dev *dev = video_drvdata(file);
 	struct s5p_mfc_fmt *fmt;
 	int i, j = 0;
 
@@ -276,6 +298,8 @@ static int vidioc_enum_fmt(struct v4l2_fmtdesc *f, bool mplane, bool out)
 			continue;
 		else if (!out && formats[i].type != MFC_FMT_RAW)
 			continue;
+		else if ((dev->variant->version_bit & formats[i].versions) == 0)
+			continue;
 
 		if (j == f->index)
 			break;
@@ -292,25 +316,25 @@ static int vidioc_enum_fmt(struct v4l2_fmtdesc *f, bool mplane, bool out)
 static int vidioc_enum_fmt_vid_cap(struct file *file, void *pirv,
 							struct v4l2_fmtdesc *f)
 {
-	return vidioc_enum_fmt(f, false, false);
+	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(f, true, false);
+	return vidioc_enum_fmt(file, f, true, false);
 }
 
-static int vidioc_enum_fmt_vid_out(struct file *file, void *prov,
+static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
 							struct v4l2_fmtdesc *f)
 {
-	return vidioc_enum_fmt(f, false, true);
+	return vidioc_enum_fmt(file, f, false, true);
 }
 
-static int vidioc_enum_fmt_vid_out_mplane(struct file *file, void *prov,
+static int vidioc_enum_fmt_vid_out_mplane(struct file *file, void *priv,
 							struct v4l2_fmtdesc *f)
 {
-	return vidioc_enum_fmt(f, true, true);
+	return vidioc_enum_fmt(file, f, true, true);
 }
 
 /* Get format */
@@ -386,11 +410,9 @@ static int vidioc_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
 			mfc_err("Unknown codec\n");
 			return -EINVAL;
 		}
-		if (!IS_MFCV6_PLUS(dev)) {
-			if (fmt->fourcc == V4L2_PIX_FMT_VP8) {
-				mfc_err("Not supported format.\n");
-				return -EINVAL;
-			}
+		if ((dev->variant->version_bit & fmt->versions) == 0) {
+			mfc_err("Unsupported format by this MFC version.\n");
+			return -EINVAL;
 		}
 	} else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
 		fmt = find_format(f, MFC_FMT_RAW);
@@ -398,13 +420,8 @@ static int vidioc_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
 			mfc_err("Unsupported format for destination.\n");
 			return -EINVAL;
 		}
-		if (IS_MFCV6_PLUS(dev) &&
-				(fmt->fourcc == V4L2_PIX_FMT_NV12MT)) {
-			mfc_err("Not supported format.\n");
-			return -EINVAL;
-		} else if (!IS_MFCV6_PLUS(dev) &&
-				(fmt->fourcc != V4L2_PIX_FMT_NV12MT)) {
-			mfc_err("Not supported format.\n");
+		if ((dev->variant->version_bit & fmt->versions) == 0) {
+			mfc_err("Unsupported format by this MFC version.\n");
 			return -EINVAL;
 		}
 	}
@@ -462,104 +479,131 @@ out:
 	return ret;
 }
 
-/* Reqeust buffers */
-static int vidioc_reqbufs(struct file *file, void *priv,
-					  struct v4l2_requestbuffers *reqbufs)
+static int reqbufs_output(struct s5p_mfc_dev *dev, struct s5p_mfc_ctx *ctx,
+				struct v4l2_requestbuffers *reqbufs)
 {
-	struct s5p_mfc_dev *dev = video_drvdata(file);
-	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
 	int ret = 0;
 
-	if (reqbufs->memory != V4L2_MEMORY_MMAP) {
-		mfc_err("Only V4L2_MEMORY_MAP is supported\n");
-		return -EINVAL;
-	}
-	if (reqbufs->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
-		/* Can only request buffers after an instance has been opened.*/
-		if (ctx->state == MFCINST_INIT) {
-			ctx->src_bufs_cnt = 0;
-			if (reqbufs->count == 0) {
-				mfc_debug(2, "Freeing buffers\n");
-				s5p_mfc_clock_on();
-				ret = vb2_reqbufs(&ctx->vq_src, reqbufs);
-				s5p_mfc_clock_off();
-				return ret;
-			}
-			/* Decoding */
-			if (ctx->output_state != QUEUE_FREE) {
-				mfc_err("Bufs have already been requested\n");
-				return -EINVAL;
-			}
-			s5p_mfc_clock_on();
-			ret = vb2_reqbufs(&ctx->vq_src, reqbufs);
-			s5p_mfc_clock_off();
-			if (ret) {
-				mfc_err("vb2_reqbufs on output failed\n");
-				return ret;
-			}
-			mfc_debug(2, "vb2_reqbufs: %d\n", ret);
-			ctx->output_state = QUEUE_BUFS_REQUESTED;
-		}
-	} else if (reqbufs->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
-		ctx->dst_bufs_cnt = 0;
-		if (reqbufs->count == 0) {
-			mfc_debug(2, "Freeing buffers\n");
-			s5p_mfc_clock_on();
-			ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
-			s5p_mfc_clock_off();
-			return ret;
+	s5p_mfc_clock_on();
+
+	if (reqbufs->count == 0) {
+		mfc_debug(2, "Freeing buffers\n");
+		ret = vb2_reqbufs(&ctx->vq_src, reqbufs);
+		if (ret)
+			goto out;
+		s5p_mfc_close_mfc_inst(dev, ctx);
+		ctx->src_bufs_cnt = 0;
+		ctx->output_state = QUEUE_FREE;
+	} else if (ctx->output_state == QUEUE_FREE) {
+		/* Can only request buffers when we have a valid format set. */
+		WARN_ON(ctx->src_bufs_cnt != 0);
+		if (ctx->state != MFCINST_INIT) {
+			mfc_err("Reqbufs called in an invalid state\n");
+			ret = -EINVAL;
+			goto out;
 		}
-		if (ctx->capture_state != QUEUE_FREE) {
-			mfc_err("Bufs have already been requested\n");
-			return -EINVAL;
-		}
-		ctx->capture_state = QUEUE_BUFS_REQUESTED;
-		s5p_mfc_clock_on();
-		ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
-		s5p_mfc_clock_off();
+
+		mfc_debug(2, "Allocating %d buffers for OUTPUT queue\n",
+				reqbufs->count);
+		ret = vb2_reqbufs(&ctx->vq_src, reqbufs);
+		if (ret)
+			goto out;
+
+		ret = s5p_mfc_open_mfc_inst(dev, ctx);
 		if (ret) {
-			mfc_err("vb2_reqbufs on capture failed\n");
-			return ret;
-		}
-		if (reqbufs->count < ctx->pb_count) {
-			mfc_err("Not enough buffers allocated\n");
 			reqbufs->count = 0;
-			s5p_mfc_clock_on();
-			ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
-			s5p_mfc_clock_off();
-			return -ENOMEM;
+			vb2_reqbufs(&ctx->vq_src, reqbufs);
+			goto out;
 		}
+
+		ctx->output_state = QUEUE_BUFS_REQUESTED;
+	} else {
+		mfc_err("Buffers have already been requested\n");
+		ret = -EINVAL;
+	}
+out:
+	s5p_mfc_clock_off();
+	if (ret)
+		mfc_err("Failed allocating buffers for OUTPUT queue\n");
+	return ret;
+}
+
+static int reqbufs_capture(struct s5p_mfc_dev *dev, struct s5p_mfc_ctx *ctx,
+				struct v4l2_requestbuffers *reqbufs)
+{
+	int ret = 0;
+
+	s5p_mfc_clock_on();
+
+	if (reqbufs->count == 0) {
+		mfc_debug(2, "Freeing buffers\n");
+		ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
+		if (ret)
+			goto out;
+		s5p_mfc_hw_call(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);
+		mfc_debug(2, "Allocating %d buffers for CAPTURE queue\n",
+				reqbufs->count);
+		ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
+		if (ret)
+			goto out;
+
+		ctx->capture_state = QUEUE_BUFS_REQUESTED;
 		ctx->total_dpb_count = reqbufs->count;
+
 		ret = s5p_mfc_hw_call(dev->mfc_ops, alloc_codec_buffers, ctx);
 		if (ret) {
 			mfc_err("Failed to allocate decoding buffers\n");
 			reqbufs->count = 0;
-			s5p_mfc_clock_on();
-			ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
-			s5p_mfc_clock_off();
-			return -ENOMEM;
-		}
-		if (ctx->dst_bufs_cnt == ctx->total_dpb_count) {
-			ctx->capture_state = QUEUE_BUFS_MMAPED;
-		} else {
-			mfc_err("Not all buffers passed to buf_init\n");
-			reqbufs->count = 0;
-			s5p_mfc_clock_on();
-			ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
-			s5p_mfc_hw_call(dev->mfc_ops, release_codec_buffers,
-					ctx);
-			s5p_mfc_clock_off();
-			return -ENOMEM;
+			vb2_reqbufs(&ctx->vq_dst, reqbufs);
+			ret = -ENOMEM;
+			ctx->capture_state = QUEUE_FREE;
+			goto out;
 		}
+
+		WARN_ON(ctx->dst_bufs_cnt != ctx->total_dpb_count);
+		ctx->capture_state = QUEUE_BUFS_MMAPED;
+
 		if (s5p_mfc_ctx_ready(ctx))
 			set_work_bit_irqsave(ctx);
 		s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
-		s5p_mfc_wait_for_done_ctx(ctx,
-					S5P_MFC_R2H_CMD_INIT_BUFFERS_RET, 0);
+		s5p_mfc_wait_for_done_ctx(ctx, S5P_MFC_R2H_CMD_INIT_BUFFERS_RET,
+					  0);
+	} else {
+		mfc_err("Buffers have already been requested\n");
+		ret = -EINVAL;
 	}
+out:
+	s5p_mfc_clock_off();
+	if (ret)
+		mfc_err("Failed allocating buffers for CAPTURE queue\n");
 	return ret;
 }
 
+/* Reqeust buffers */
+static int vidioc_reqbufs(struct file *file, void *priv,
+					  struct v4l2_requestbuffers *reqbufs)
+{
+	struct s5p_mfc_dev *dev = video_drvdata(file);
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+
+	if (reqbufs->memory != V4L2_MEMORY_MMAP) {
+		mfc_err("Only V4L2_MEMORY_MAP is supported\n");
+		return -EINVAL;
+	}
+
+	if (reqbufs->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		return reqbufs_output(dev, ctx, reqbufs);
+	} else if (reqbufs->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		return reqbufs_capture(dev, ctx, reqbufs);
+	} else {
+		mfc_err("Invalid type requested\n");
+		return -EINVAL;
+	}
+}
+
 /* Query buffer */
 static int vidioc_querybuf(struct file *file, void *priv,
 						   struct v4l2_buffer *buf)
@@ -573,7 +617,7 @@ static int vidioc_querybuf(struct file *file, void *priv,
 		return -EINVAL;
 	}
 	mfc_debug(2, "State: %d, buf->type: %d\n", ctx->state, buf->type);
-	if (ctx->state == MFCINST_INIT &&
+	if (ctx->state == MFCINST_GOT_INST &&
 			buf->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
 		ret = vb2_querybuf(&ctx->vq_src, buf);
 	} else if (ctx->state == MFCINST_RUNNING &&
@@ -649,39 +693,11 @@ static int vidioc_streamon(struct file *file, void *priv,
 			   enum v4l2_buf_type type)
 {
 	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
-	struct s5p_mfc_dev *dev = ctx->dev;
 	int ret = -EINVAL;
 
 	mfc_debug_enter();
-	if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
-
-		if (ctx->state == MFCINST_INIT) {
-			ctx->dst_bufs_cnt = 0;
-			ctx->src_bufs_cnt = 0;
-			ctx->capture_state = QUEUE_FREE;
-			ctx->output_state = QUEUE_FREE;
-			s5p_mfc_hw_call(dev->mfc_ops, alloc_instance_buffer,
-					ctx);
-			s5p_mfc_hw_call(dev->mfc_ops, alloc_dec_temp_buffers,
-					ctx);
-			set_work_bit_irqsave(ctx);
-			s5p_mfc_clean_ctx_int_flags(ctx);
-			s5p_mfc_hw_call(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 */
-				mfc_err("Error getting instance from hardware\n");
-				s5p_mfc_hw_call(dev->mfc_ops,
-						release_instance_buffer, ctx);
-				s5p_mfc_hw_call(dev->mfc_ops,
-						release_dec_desc_buffer, ctx);
-				return -EIO;
-			}
-			mfc_debug(2, "Got instance number: %d\n", ctx->inst_no);
-		}
+	if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
 		ret = vb2_streamon(&ctx->vq_src, type);
-		}
 	else if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
 		ret = vb2_streamon(&ctx->vq_dst, type);
 	mfc_debug_leave();
@@ -851,6 +867,8 @@ static int vidioc_subscribe_event(struct v4l2_fh *fh,
 	switch (sub->type) {
 	case V4L2_EVENT_EOS:
 		return v4l2_event_subscribe(fh, sub, 2, NULL);
+	case V4L2_EVENT_SOURCE_CHANGE:
+		return v4l2_src_change_event_subscribe(fh, sub);
 	default:
 		return -EINVAL;
 	}
@@ -1027,7 +1045,7 @@ static int s5p_mfc_start_streaming(struct vb2_queue *q, unsigned int count)
 	return 0;
 }
 
-static int s5p_mfc_stop_streaming(struct vb2_queue *q)
+static void s5p_mfc_stop_streaming(struct vb2_queue *q)
 {
 	unsigned long flags;
 	struct s5p_mfc_ctx *ctx = fh_to_ctx(q->drv_priv);
@@ -1071,7 +1089,6 @@ static int s5p_mfc_stop_streaming(struct vb2_queue *q)
 	}
 	if (aborted)
 		ctx->state = MFCINST_RUNNING;
-	return 0;
 }
 
 
@@ -1191,9 +1208,14 @@ void s5p_mfc_dec_ctrls_delete(struct s5p_mfc_ctx *ctx)
 void s5p_mfc_dec_init(struct s5p_mfc_ctx *ctx)
 {
 	struct v4l2_format f;
-	f.fmt.pix_mp.pixelformat = DEF_SRC_FMT_DEC;
+	f.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_H264;
 	ctx->src_fmt = find_format(&f, MFC_FMT_DEC);
-	f.fmt.pix_mp.pixelformat = DEF_DST_FMT_DEC;
+	if (IS_MFCV8(ctx->dev))
+		f.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_NV12M;
+	else if (IS_MFCV6_PLUS(ctx->dev))
+		f.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_NV12MT_16X16;
+	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);
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
index df83cd157ba..d26b2484ca1 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
@@ -26,6 +26,7 @@
 #include <media/v4l2-ctrls.h>
 #include <media/videobuf2-core.h>
 #include "s5p_mfc_common.h"
+#include "s5p_mfc_ctrl.h"
 #include "s5p_mfc_debug.h"
 #include "s5p_mfc_enc.h"
 #include "s5p_mfc_intr.h"
@@ -41,6 +42,7 @@ static struct s5p_mfc_fmt formats[] = {
 		.codec_mode	= S5P_MFC_CODEC_NONE,
 		.type		= MFC_FMT_RAW,
 		.num_planes	= 2,
+		.versions	= MFC_V6_BIT | MFC_V7_BIT,
 	},
 	{
 		.name		= "4:2:0 2 Planes 64x32 Tiles",
@@ -48,6 +50,7 @@ static struct s5p_mfc_fmt formats[] = {
 		.codec_mode	= S5P_MFC_CODEC_NONE,
 		.type		= MFC_FMT_RAW,
 		.num_planes	= 2,
+		.versions	= MFC_V5_BIT,
 	},
 	{
 		.name		= "4:2:0 2 Planes Y/CbCr",
@@ -55,6 +58,8 @@ static struct s5p_mfc_fmt formats[] = {
 		.codec_mode	= S5P_MFC_CODEC_NONE,
 		.type		= MFC_FMT_RAW,
 		.num_planes	= 2,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
 	},
 	{
 		.name		= "4:2:0 2 Planes Y/CrCb",
@@ -62,6 +67,8 @@ static struct s5p_mfc_fmt formats[] = {
 		.codec_mode	= S5P_MFC_CODEC_NONE,
 		.type		= MFC_FMT_RAW,
 		.num_planes	= 2,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
 	},
 	{
 		.name		= "H264 Encoded Stream",
@@ -69,6 +76,8 @@ static struct s5p_mfc_fmt formats[] = {
 		.codec_mode	= S5P_MFC_CODEC_H264_ENC,
 		.type		= MFC_FMT_ENC,
 		.num_planes	= 1,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
 	},
 	{
 		.name		= "MPEG4 Encoded Stream",
@@ -76,6 +85,8 @@ static struct s5p_mfc_fmt formats[] = {
 		.codec_mode	= S5P_MFC_CODEC_MPEG4_ENC,
 		.type		= MFC_FMT_ENC,
 		.num_planes	= 1,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
 	},
 	{
 		.name		= "H263 Encoded Stream",
@@ -83,6 +94,8 @@ static struct s5p_mfc_fmt formats[] = {
 		.codec_mode	= S5P_MFC_CODEC_H263_ENC,
 		.type		= MFC_FMT_ENC,
 		.num_planes	= 1,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
 	},
 	{
 		.name		= "VP8 Encoded Stream",
@@ -90,6 +103,7 @@ static struct s5p_mfc_fmt formats[] = {
 		.codec_mode	= S5P_MFC_CODEC_VP8_ENC,
 		.type		= MFC_FMT_ENC,
 		.num_planes	= 1,
+		.versions	= MFC_V7_BIT | MFC_V8_BIT,
 	},
 };
 
@@ -772,13 +786,16 @@ static int enc_post_seq_start(struct s5p_mfc_ctx *ctx)
 
 	if (p->seq_hdr_mode == V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE) {
 		spin_lock_irqsave(&dev->irqlock, flags);
-		dst_mb = list_entry(ctx->dst_queue.next,
-				struct s5p_mfc_buf, list);
-		list_del(&dst_mb->list);
-		ctx->dst_queue_cnt--;
-		vb2_set_plane_payload(dst_mb->b, 0,
-			s5p_mfc_hw_call(dev->mfc_ops, get_enc_strm_size, dev));
-		vb2_buffer_done(dst_mb->b, VB2_BUF_STATE_DONE);
+		if (!list_empty(&ctx->dst_queue)) {
+			dst_mb = list_entry(ctx->dst_queue.next,
+					struct s5p_mfc_buf, list);
+			list_del(&dst_mb->list);
+			ctx->dst_queue_cnt--;
+			vb2_set_plane_payload(dst_mb->b, 0,
+				s5p_mfc_hw_call(dev->mfc_ops, get_enc_strm_size,
+						dev));
+			vb2_buffer_done(dst_mb->b, VB2_BUF_STATE_DONE);
+		}
 		spin_unlock_irqrestore(&dev->irqlock, flags);
 	}
 
@@ -883,8 +900,7 @@ static int enc_post_frame_start(struct s5p_mfc_ctx *ctx)
 		mfc_debug(2, "enc src count: %d, enc ref count: %d\n",
 			  ctx->src_queue_cnt, ctx->ref_queue_cnt);
 	}
-	if (strm_size > 0) {
-		/* at least one more dest. buffers exist always  */
+	if ((ctx->dst_queue_cnt > 0) && (strm_size > 0)) {
 		mb_entry = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf,
 									list);
 		list_del(&mb_entry->list);
@@ -937,8 +953,10 @@ static int vidioc_querycap(struct file *file, void *priv,
 	return 0;
 }
 
-static int vidioc_enum_fmt(struct v4l2_fmtdesc *f, bool mplane, bool out)
+static int vidioc_enum_fmt(struct file *file, struct v4l2_fmtdesc *f,
+							bool mplane, bool out)
 {
+	struct s5p_mfc_dev *dev = video_drvdata(file);
 	struct s5p_mfc_fmt *fmt;
 	int i, j = 0;
 
@@ -951,6 +969,9 @@ static int vidioc_enum_fmt(struct v4l2_fmtdesc *f, bool mplane, bool out)
 			continue;
 		else if (!out && formats[i].type != MFC_FMT_ENC)
 			continue;
+		else if ((dev->variant->version_bit & formats[i].versions) == 0)
+			continue;
+
 		if (j == f->index) {
 			fmt = &formats[i];
 			strlcpy(f->description, fmt->name,
@@ -966,25 +987,25 @@ static int vidioc_enum_fmt(struct v4l2_fmtdesc *f, bool mplane, bool out)
 static int vidioc_enum_fmt_vid_cap(struct file *file, void *pirv,
 				   struct v4l2_fmtdesc *f)
 {
-	return vidioc_enum_fmt(f, false, false);
+	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(f, true, false);
+	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(f, false, true);
+	return vidioc_enum_fmt(file, f, false, true);
 }
 
 static int vidioc_enum_fmt_vid_out_mplane(struct file *file, void *prov,
 					  struct v4l2_fmtdesc *f)
 {
-	return vidioc_enum_fmt(f, true, true);
+	return vidioc_enum_fmt(file, f, true, true);
 }
 
 static int vidioc_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
@@ -1035,16 +1056,14 @@ static int vidioc_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
 			mfc_err("failed to try output format\n");
 			return -EINVAL;
 		}
-
-		if (!IS_MFCV7(dev) && (fmt->fourcc == V4L2_PIX_FMT_VP8)) {
-			mfc_err("VP8 is supported only in MFC v7\n");
-			return -EINVAL;
-		}
-
 		if (pix_fmt_mp->plane_fmt[0].sizeimage == 0) {
 			mfc_err("must be set encoding output size\n");
 			return -EINVAL;
 		}
+		if ((dev->variant->version_bit & fmt->versions) == 0) {
+			mfc_err("Unsupported format by this MFC version.\n");
+			return -EINVAL;
+		}
 
 		pix_fmt_mp->plane_fmt[0].bytesperline =
 			pix_fmt_mp->plane_fmt[0].sizeimage;
@@ -1055,22 +1074,15 @@ static int vidioc_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
 			return -EINVAL;
 		}
 
-		if (!IS_MFCV6_PLUS(dev)) {
-			if (fmt->fourcc == V4L2_PIX_FMT_NV12MT_16X16) {
-				mfc_err("Not supported format.\n");
-				return -EINVAL;
-			}
-		} else if (IS_MFCV6_PLUS(dev)) {
-			if (fmt->fourcc == V4L2_PIX_FMT_NV12MT) {
-				mfc_err("Not supported format.\n");
-				return -EINVAL;
-			}
-		}
-
 		if (fmt->num_planes != pix_fmt_mp->num_planes) {
 			mfc_err("failed to try output format\n");
 			return -EINVAL;
 		}
+		if ((dev->variant->version_bit & fmt->versions) == 0) {
+			mfc_err("Unsupported format by this MFC version.\n");
+			return -EINVAL;
+		}
+
 		v4l_bound_align_image(&pix_fmt_mp->width, 8, 1920, 1,
 			&pix_fmt_mp->height, 4, 1080, 1, 0);
 	} else {
@@ -1104,20 +1116,7 @@ static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
 		pix_fmt_mp->plane_fmt[0].bytesperline = 0;
 		ctx->dst_bufs_cnt = 0;
 		ctx->capture_state = QUEUE_FREE;
-		s5p_mfc_hw_call(dev->mfc_ops, alloc_instance_buffer, ctx);
-		set_work_bit_irqsave(ctx);
-		s5p_mfc_clean_ctx_int_flags(ctx);
-		s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
-		if (s5p_mfc_wait_for_done_ctx(ctx, \
-				S5P_MFC_R2H_CMD_OPEN_INSTANCE_RET, 1)) {
-				/* Error or timeout */
-			mfc_err("Error getting instance from hardware\n");
-			s5p_mfc_hw_call(dev->mfc_ops, release_instance_buffer,
-					ctx);
-			ret = -EIO;
-			goto out;
-		}
-		mfc_debug(2, "Got instance number: %d\n", ctx->inst_no);
+		ret = s5p_mfc_open_mfc_inst(dev, ctx);
 	} else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
 		/* src_fmt is validated by call to vidioc_try_fmt */
 		ctx->src_fmt = find_format(f, MFC_FMT_RAW);
@@ -1138,7 +1137,7 @@ static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
 		ctx->output_state = QUEUE_FREE;
 	} else {
 		mfc_err("invalid buf type\n");
-		return -EINVAL;
+		ret = -EINVAL;
 	}
 out:
 	mfc_debug_leave();
@@ -1954,7 +1953,7 @@ static int s5p_mfc_start_streaming(struct vb2_queue *q, unsigned int count)
 	return 0;
 }
 
-static int s5p_mfc_stop_streaming(struct vb2_queue *q)
+static void s5p_mfc_stop_streaming(struct vb2_queue *q)
 {
 	unsigned long flags;
 	struct s5p_mfc_ctx *ctx = fh_to_ctx(q->drv_priv);
@@ -1983,7 +1982,6 @@ static int s5p_mfc_stop_streaming(struct vb2_queue *q)
 		ctx->src_queue_cnt = 0;
 	}
 	spin_unlock_irqrestore(&dev->irqlock, flags);
-	return 0;
 }
 
 static void s5p_mfc_buf_queue(struct vb2_buffer *vb)
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
index 3c01c339d69..c9a227428e6 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
@@ -31,6 +31,12 @@ void s5p_mfc_init_hw_ops(struct s5p_mfc_dev *dev)
 	dev->mfc_ops = s5p_mfc_ops;
 }
 
+void s5p_mfc_init_regs(struct s5p_mfc_dev *dev)
+{
+	if (IS_MFCV6_PLUS(dev))
+		dev->mfc_regs = s5p_mfc_init_regs_v6_plus(dev);
+}
+
 int s5p_mfc_alloc_priv_buf(struct device *dev,
 					struct s5p_mfc_priv_buf *b)
 {
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
index 754c540e7a7..7a7ad32ee60 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
@@ -17,6 +17,259 @@
 
 #include "s5p_mfc_common.h"
 
+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;
+
+	/* 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 */
+
+	/* 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 */
+};
+
 struct s5p_mfc_hw_ops {
 	int (*alloc_dec_temp_buffers)(struct s5p_mfc_ctx *ctx);
 	void (*release_dec_desc_buffer)(struct s5p_mfc_ctx *ctx);
@@ -80,6 +333,7 @@ struct s5p_mfc_hw_ops {
 };
 
 void s5p_mfc_init_hw_ops(struct s5p_mfc_dev *dev);
+void s5p_mfc_init_regs(struct s5p_mfc_dev *dev);
 int s5p_mfc_alloc_priv_buf(struct device *dev,
 					struct s5p_mfc_priv_buf *b);
 void s5p_mfc_release_priv_buf(struct device *dev,
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 f64621ae9b5..4f5e0ead90c 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
@@ -43,10 +43,10 @@
 	} while (0)
 #endif /* S5P_MFC_DEBUG_REGWRITE */
 
-#define READL(offset)		readl(dev->regs_base + (offset))
-#define WRITEL(data, offset)	writel((data), dev->regs_base + (offset))
-#define OFFSETA(x)		(((x) - dev->port_a) >> S5P_FIMV_MEM_OFFSET)
-#define OFFSETB(x)		(((x) - dev->port_b) >> S5P_FIMV_MEM_OFFSET)
+#define READL(reg) \
+	(WARN_ON_ONCE(!(reg)) ? 0 : readl(reg))
+#define WRITEL(data, reg) \
+	(WARN_ON_ONCE(!(reg)) ? 0 : writel((data), (reg)))
 
 /* Allocate temporary buffers for decoding */
 static int s5p_mfc_alloc_dec_temp_buffers_v6(struct s5p_mfc_ctx *ctx)
@@ -77,7 +77,12 @@ static int s5p_mfc_alloc_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
 			  ctx->luma_size, ctx->chroma_size, ctx->mv_size);
 		mfc_debug(2, "Totals bufs: %d\n", ctx->total_dpb_count);
 	} else if (ctx->type == MFCINST_ENCODER) {
-		ctx->tmv_buffer_size = S5P_FIMV_NUM_TMV_BUFFERS_V6 *
+		if (IS_MFCV8(dev))
+			ctx->tmv_buffer_size = S5P_FIMV_NUM_TMV_BUFFERS_V6 *
+			ALIGN(S5P_FIMV_TMV_BUFFER_SIZE_V8(mb_width, mb_height),
+			S5P_FIMV_TMV_BUFFER_ALIGN_V6);
+		else
+			ctx->tmv_buffer_size = S5P_FIMV_NUM_TMV_BUFFERS_V6 *
 			ALIGN(S5P_FIMV_TMV_BUFFER_SIZE_V6(mb_width, mb_height),
 			S5P_FIMV_TMV_BUFFER_ALIGN_V6);
 
@@ -87,10 +92,16 @@ static int s5p_mfc_alloc_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
 		ctx->chroma_dpb_size = ALIGN((mb_width * mb_height) *
 				S5P_FIMV_CHROMA_MB_TO_PIXEL_V6,
 				S5P_FIMV_CHROMA_DPB_BUFFER_ALIGN_V6);
-		ctx->me_buffer_size = ALIGN(S5P_FIMV_ME_BUFFER_SIZE_V6(
-					ctx->img_width, ctx->img_height,
-					mb_width, mb_height),
-					S5P_FIMV_ME_BUFFER_ALIGN_V6);
+		if (IS_MFCV8(dev))
+			ctx->me_buffer_size = ALIGN(S5P_FIMV_ME_BUFFER_SIZE_V8(
+						ctx->img_width, ctx->img_height,
+						mb_width, mb_height),
+						S5P_FIMV_ME_BUFFER_ALIGN_V6);
+		else
+			ctx->me_buffer_size = ALIGN(S5P_FIMV_ME_BUFFER_SIZE_V6(
+						ctx->img_width, ctx->img_height,
+						mb_width, mb_height),
+						S5P_FIMV_ME_BUFFER_ALIGN_V6);
 
 		mfc_debug(2, "recon luma size: %d chroma size: %d\n",
 			  ctx->luma_dpb_size, ctx->chroma_dpb_size);
@@ -102,8 +113,14 @@ static int s5p_mfc_alloc_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
 	switch (ctx->codec_mode) {
 	case S5P_MFC_CODEC_H264_DEC:
 	case S5P_MFC_CODEC_H264_MVC_DEC:
-		ctx->scratch_buf_size =
-			S5P_FIMV_SCRATCH_BUF_SIZE_H264_DEC_V6(
+		if (IS_MFCV8(dev))
+			ctx->scratch_buf_size =
+				S5P_FIMV_SCRATCH_BUF_SIZE_H264_DEC_V8(
+					mb_width,
+					mb_height);
+		else
+			ctx->scratch_buf_size =
+				S5P_FIMV_SCRATCH_BUF_SIZE_H264_DEC_V6(
 					mb_width,
 					mb_height);
 		ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
@@ -113,7 +130,7 @@ static int s5p_mfc_alloc_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
 			(ctx->mv_count * ctx->mv_size);
 		break;
 	case S5P_MFC_CODEC_MPEG4_DEC:
-		if (IS_MFCV7(dev)) {
+		if (IS_MFCV7_PLUS(dev)) {
 			ctx->scratch_buf_size =
 				S5P_FIMV_SCRATCH_BUF_SIZE_MPEG4_DEC_V7(
 						mb_width,
@@ -153,19 +170,31 @@ static int s5p_mfc_alloc_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
 		ctx->bank1.size = ctx->scratch_buf_size;
 		break;
 	case S5P_MFC_CODEC_VP8_DEC:
-		ctx->scratch_buf_size =
-			S5P_FIMV_SCRATCH_BUF_SIZE_VP8_DEC_V6(
-					mb_width,
-					mb_height);
+		if (IS_MFCV8(dev))
+			ctx->scratch_buf_size =
+				S5P_FIMV_SCRATCH_BUF_SIZE_VP8_DEC_V8(
+						mb_width,
+						mb_height);
+		else
+			ctx->scratch_buf_size =
+				S5P_FIMV_SCRATCH_BUF_SIZE_VP8_DEC_V6(
+						mb_width,
+						mb_height);
 		ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
 				S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
 		ctx->bank1.size = ctx->scratch_buf_size;
 		break;
 	case S5P_MFC_CODEC_H264_ENC:
-		ctx->scratch_buf_size =
-			S5P_FIMV_SCRATCH_BUF_SIZE_H264_ENC_V6(
+		if (IS_MFCV8(dev))
+			ctx->scratch_buf_size =
+				S5P_FIMV_SCRATCH_BUF_SIZE_H264_ENC_V8(
 					mb_width,
 					mb_height);
+		else
+			ctx->scratch_buf_size =
+				S5P_FIMV_SCRATCH_BUF_SIZE_H264_ENC_V6(
+						mb_width,
+						mb_height);
 		ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
 				S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
 		ctx->bank1.size =
@@ -189,10 +218,16 @@ static int s5p_mfc_alloc_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
 		ctx->bank2.size = 0;
 		break;
 	case S5P_MFC_CODEC_VP8_ENC:
-		ctx->scratch_buf_size =
-			S5P_FIMV_SCRATCH_BUF_SIZE_VP8_ENC_V7(
+		if (IS_MFCV8(dev))
+			ctx->scratch_buf_size =
+				S5P_FIMV_SCRATCH_BUF_SIZE_VP8_ENC_V8(
 					mb_width,
 					mb_height);
+		else
+			ctx->scratch_buf_size =
+				S5P_FIMV_SCRATCH_BUF_SIZE_VP8_ENC_V7(
+						mb_width,
+						mb_height);
 		ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
 				S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
 		ctx->bank1.size =
@@ -332,6 +367,12 @@ static void s5p_mfc_dec_calc_dpb_size_v6(struct s5p_mfc_ctx *ctx)
 
 	ctx->luma_size = calc_plane(ctx->img_width, ctx->img_height);
 	ctx->chroma_size = calc_plane(ctx->img_width, (ctx->img_height >> 1));
+	if (IS_MFCV8(ctx->dev)) {
+		/* MFCv8 needs additional 64 bytes for luma,chroma dpb*/
+		ctx->luma_size += S5P_FIMV_D_ALIGN_PLANE_SIZE_V8;
+		ctx->chroma_size += S5P_FIMV_D_ALIGN_PLANE_SIZE_V8;
+	}
+
 	if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC ||
 			ctx->codec_mode == S5P_MFC_CODEC_H264_MVC_DEC) {
 		ctx->mv_size = S5P_MFC_DEC_MV_SIZE_V6(ctx->img_width,
@@ -354,7 +395,7 @@ static void s5p_mfc_enc_calc_src_size_v6(struct s5p_mfc_ctx *ctx)
 	ctx->chroma_size = ALIGN((mb_width * mb_height) * 128, 256);
 
 	/* MFCv7 needs pad bytes for Luma and Chroma */
-	if (IS_MFCV7(ctx->dev)) {
+	if (IS_MFCV7_PLUS(ctx->dev)) {
 		ctx->luma_size += MFC_LUMA_PAD_BYTES_V7;
 		ctx->chroma_size += MFC_CHROMA_PAD_BYTES_V7;
 	}
@@ -366,16 +407,17 @@ static int s5p_mfc_set_dec_stream_buffer_v6(struct s5p_mfc_ctx *ctx,
 			unsigned int strm_size)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
 	struct s5p_mfc_buf_size *buf_size = dev->variant->buf_size;
 
 	mfc_debug_enter();
 	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, S5P_FIMV_D_STREAM_DATA_SIZE_V6);
-	WRITEL(buf_addr, S5P_FIMV_D_CPB_BUFFER_ADDR_V6);
-	WRITEL(buf_size->cpb, S5P_FIMV_D_CPB_BUFFER_SIZE_V6);
-	WRITEL(start_num_byte, S5P_FIMV_D_CPB_BUFFER_OFFSET_V6);
+	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;
@@ -387,6 +429,7 @@ static int s5p_mfc_set_dec_frame_buffer_v6(struct s5p_mfc_ctx *ctx)
 	unsigned int frame_size, i;
 	unsigned int frame_size_ch, frame_size_mv;
 	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
 	size_t buf_addr1;
 	int buf_size1;
 	int align_gap;
@@ -398,19 +441,27 @@ 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, S5P_FIMV_D_NUM_DPB_V6);
-	WRITEL(ctx->luma_size, S5P_FIMV_D_LUMA_DPB_SIZE_V6);
-	WRITEL(ctx->chroma_size, S5P_FIMV_D_CHROMA_DPB_SIZE_V6);
+	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);
+
+	if (IS_MFCV8(dev)) {
+		WRITEL(ctx->img_width,
+			mfc_regs->d_first_plane_dpb_stride_size);
+		WRITEL(ctx->img_width,
+			mfc_regs->d_second_plane_dpb_stride_size);
+	}
 
-	WRITEL(buf_addr1, S5P_FIMV_D_SCRATCH_BUFFER_ADDR_V6);
-	WRITEL(ctx->scratch_buf_size, S5P_FIMV_D_SCRATCH_BUFFER_SIZE_V6);
 	buf_addr1 += ctx->scratch_buf_size;
 	buf_size1 -= ctx->scratch_buf_size;
 
 	if (ctx->codec_mode == S5P_FIMV_CODEC_H264_DEC ||
 			ctx->codec_mode == S5P_FIMV_CODEC_H264_MVC_DEC){
-		WRITEL(ctx->mv_size, S5P_FIMV_D_MV_BUFFER_SIZE_V6);
-		WRITEL(ctx->mv_count, S5P_FIMV_D_NUM_MV_V6);
+		WRITEL(ctx->mv_size, mfc_regs->d_mv_buffer_size);
+		WRITEL(ctx->mv_count, mfc_regs->d_num_mv);
 	}
 
 	frame_size = ctx->luma_size;
@@ -424,11 +475,11 @@ static int s5p_mfc_set_dec_frame_buffer_v6(struct s5p_mfc_ctx *ctx)
 		mfc_debug(2, "Luma %d: %x\n", i,
 					ctx->dst_bufs[i].cookie.raw.luma);
 		WRITEL(ctx->dst_bufs[i].cookie.raw.luma,
-				S5P_FIMV_D_LUMA_DPB_V6 + i * 4);
+				mfc_regs->d_first_plane_dpb + i * 4);
 		mfc_debug(2, "\tChroma %d: %x\n", i,
 					ctx->dst_bufs[i].cookie.raw.chroma);
 		WRITEL(ctx->dst_bufs[i].cookie.raw.chroma,
-				S5P_FIMV_D_CHROMA_DPB_V6 + i * 4);
+				mfc_regs->d_second_plane_dpb + i * 4);
 	}
 	if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC ||
 			ctx->codec_mode == S5P_MFC_CODEC_H264_MVC_DEC) {
@@ -441,7 +492,7 @@ static int s5p_mfc_set_dec_frame_buffer_v6(struct s5p_mfc_ctx *ctx)
 
 			mfc_debug(2, "\tBuf1: %x, size: %d\n",
 					buf_addr1, buf_size1);
-			WRITEL(buf_addr1, S5P_FIMV_D_MV_BUFFER_V6 + i * 4);
+			WRITEL(buf_addr1, mfc_regs->d_mv_buffer + i * 4);
 			buf_addr1 += frame_size_mv;
 			buf_size1 -= frame_size_mv;
 		}
@@ -454,7 +505,7 @@ static int s5p_mfc_set_dec_frame_buffer_v6(struct s5p_mfc_ctx *ctx)
 		return -ENOMEM;
 	}
 
-	WRITEL(ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+	WRITEL(ctx->inst_no, mfc_regs->instance_id);
 	s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
 			S5P_FIMV_CH_INIT_BUFS_V6, NULL);
 
@@ -467,9 +518,10 @@ static int s5p_mfc_set_enc_stream_buffer_v6(struct s5p_mfc_ctx *ctx,
 		unsigned long addr, unsigned int size)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
 
-	WRITEL(addr, S5P_FIMV_E_STREAM_BUFFER_ADDR_V6); /* 16B align */
-	WRITEL(size, S5P_FIMV_E_STREAM_BUFFER_SIZE_V6);
+	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);
@@ -481,14 +533,10 @@ static void s5p_mfc_set_enc_frame_buffer_v6(struct s5p_mfc_ctx *ctx,
 		unsigned long y_addr, unsigned long c_addr)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
 
-	if (IS_MFCV7(dev)) {
-		WRITEL(y_addr, S5P_FIMV_E_SOURCE_FIRST_ADDR_V7);
-		WRITEL(c_addr, S5P_FIMV_E_SOURCE_SECOND_ADDR_V7);
-	} else {
-		WRITEL(y_addr, S5P_FIMV_E_SOURCE_LUMA_ADDR_V6);
-		WRITEL(c_addr, S5P_FIMV_E_SOURCE_CHROMA_ADDR_V6);
-	}
+	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);
@@ -498,18 +546,14 @@ static void s5p_mfc_get_enc_frame_buffer_v6(struct s5p_mfc_ctx *ctx,
 		unsigned long *y_addr, unsigned long *c_addr)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
 	unsigned long enc_recon_y_addr, enc_recon_c_addr;
 
-	if (IS_MFCV7(dev)) {
-		*y_addr = READL(S5P_FIMV_E_ENCODED_SOURCE_FIRST_ADDR_V7);
-		*c_addr = READL(S5P_FIMV_E_ENCODED_SOURCE_SECOND_ADDR_V7);
-	} else {
-		*y_addr = READL(S5P_FIMV_E_ENCODED_SOURCE_LUMA_ADDR_V6);
-		*c_addr = READL(S5P_FIMV_E_ENCODED_SOURCE_CHROMA_ADDR_V6);
-	}
+	*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(S5P_FIMV_E_RECON_LUMA_DPB_ADDR_V6);
-	enc_recon_c_addr = READL(S5P_FIMV_E_RECON_CHROMA_DPB_ADDR_V6);
+	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);
@@ -519,6 +563,7 @@ static void s5p_mfc_get_enc_frame_buffer_v6(struct s5p_mfc_ctx *ctx,
 static int s5p_mfc_set_enc_ref_buffer_v6(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
 	size_t buf_addr1;
 	int i, buf_size1;
 
@@ -530,24 +575,24 @@ 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, S5P_FIMV_E_LUMA_DPB_V6 + (4 * i));
+		WRITEL(buf_addr1, mfc_regs->e_luma_dpb + (4 * i));
 		buf_addr1 += ctx->luma_dpb_size;
-		WRITEL(buf_addr1, S5P_FIMV_E_CHROMA_DPB_V6 + (4 * i));
+		WRITEL(buf_addr1, mfc_regs->e_chroma_dpb + (4 * i));
 		buf_addr1 += ctx->chroma_dpb_size;
-		WRITEL(buf_addr1, S5P_FIMV_E_ME_BUFFER_V6 + (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, S5P_FIMV_E_SCRATCH_BUFFER_ADDR_V6);
-	WRITEL(ctx->scratch_buf_size, S5P_FIMV_E_SCRATCH_BUFFER_SIZE_V6);
+	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, S5P_FIMV_E_TMV_BUFFER0_V6);
+	WRITEL(buf_addr1, mfc_regs->e_tmv_buffer0);
 	buf_addr1 += ctx->tmv_buffer_size >> 1;
-	WRITEL(buf_addr1, S5P_FIMV_E_TMV_BUFFER1_V6);
+	WRITEL(buf_addr1, mfc_regs->e_tmv_buffer1);
 	buf_addr1 += ctx->tmv_buffer_size >> 1;
 	buf_size1 -= ctx->tmv_buffer_size;
 
@@ -558,7 +603,7 @@ static int s5p_mfc_set_enc_ref_buffer_v6(struct s5p_mfc_ctx *ctx)
 		return -ENOMEM;
 	}
 
-	WRITEL(ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+	WRITEL(ctx->inst_no, mfc_regs->instance_id);
 	s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
 			S5P_FIMV_CH_INIT_BUFS_V6, NULL);
 
@@ -570,18 +615,19 @@ static int s5p_mfc_set_enc_ref_buffer_v6(struct s5p_mfc_ctx *ctx)
 static int s5p_mfc_set_slice_mode(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
 
 	/* multi-slice control */
 	/* multi-slice MB number or bit size */
-	WRITEL(ctx->slice_mode, S5P_FIMV_E_MSLICE_MODE_V6);
+	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, S5P_FIMV_E_MSLICE_SIZE_MB_V6);
+		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, S5P_FIMV_E_MSLICE_SIZE_BITS_V6);
+		WRITEL(ctx->slice_size.bits, mfc_regs->e_mslice_size_bits);
 	} else {
-		WRITEL(0x0, S5P_FIMV_E_MSLICE_SIZE_MB_V6);
-		WRITEL(0x0, S5P_FIMV_E_MSLICE_SIZE_BITS_V6);
+		WRITEL(0x0, mfc_regs->e_mslice_size_mb);
+		WRITEL(0x0, mfc_regs->e_mslice_size_bits);
 	}
 
 	return 0;
@@ -590,27 +636,28 @@ static int s5p_mfc_set_slice_mode(struct s5p_mfc_ctx *ctx)
 static int s5p_mfc_set_enc_params(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
 	struct s5p_mfc_enc_params *p = &ctx->enc_params;
 	unsigned int reg = 0;
 
 	mfc_debug_enter();
 
 	/* width */
-	WRITEL(ctx->img_width, S5P_FIMV_E_FRAME_WIDTH_V6); /* 16 align */
+	WRITEL(ctx->img_width, mfc_regs->e_frame_width); /* 16 align */
 	/* height */
-	WRITEL(ctx->img_height, S5P_FIMV_E_FRAME_HEIGHT_V6); /* 16 align */
+	WRITEL(ctx->img_height, mfc_regs->e_frame_height); /* 16 align */
 
 	/* cropped width */
-	WRITEL(ctx->img_width, S5P_FIMV_E_CROPPED_FRAME_WIDTH_V6);
+	WRITEL(ctx->img_width, mfc_regs->e_cropped_frame_width);
 	/* cropped height */
-	WRITEL(ctx->img_height, S5P_FIMV_E_CROPPED_FRAME_HEIGHT_V6);
+	WRITEL(ctx->img_height, mfc_regs->e_cropped_frame_height);
 	/* cropped offset */
-	WRITEL(0x0, S5P_FIMV_E_FRAME_CROP_OFFSET_V6);
+	WRITEL(0x0, mfc_regs->e_frame_crop_offset);
 
 	/* pictype : IDR period */
 	reg = 0;
 	reg |= p->gop_size & 0xFFFF;
-	WRITEL(reg, S5P_FIMV_E_GOP_CONFIG_V6);
+	WRITEL(reg, mfc_regs->e_gop_config);
 
 	/* multi-slice control */
 	/* multi-slice MB number or bit size */
@@ -618,65 +665,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, S5P_FIMV_E_ENC_OPTIONS_V6);
+		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, S5P_FIMV_E_ENC_OPTIONS_V6);
+		WRITEL(reg, mfc_regs->e_enc_options);
 		ctx->slice_size.bits = p->slice_bit;
 	} else {
 		reg &= ~(0x1 << 3);
-		WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+		WRITEL(reg, mfc_regs->e_enc_options);
 	}
 
 	s5p_mfc_set_slice_mode(ctx);
 
 	/* cyclic intra refresh */
-	WRITEL(p->intra_refresh_mb, S5P_FIMV_E_IR_SIZE_V6);
-	reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+	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, S5P_FIMV_E_ENC_OPTIONS_V6);
+	WRITEL(reg, mfc_regs->e_enc_options);
 
 	/* 'NON_REFERENCE_STORE_ENABLE' for debugging */
-	reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+	reg = READL(mfc_regs->e_enc_options);
 	reg &= ~(0x1 << 9);
-	WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+	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(S5P_FIMV_E_ENC_OPTIONS_V6);
+		reg = READL(mfc_regs->e_enc_options);
 		reg &= ~(0x1 << 7);
-		WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+		WRITEL(reg, mfc_regs->e_enc_options);
 		/* 0: NV12(CbCr), 1: NV21(CrCb) */
-		WRITEL(0x0, S5P_FIMV_PIXEL_FORMAT_V6);
+		WRITEL(0x0, mfc_regs->pixel_format);
 	} else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV21M) {
 		/* 0: Linear, 1: 2D tiled*/
-		reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+		reg = READL(mfc_regs->e_enc_options);
 		reg &= ~(0x1 << 7);
-		WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+		WRITEL(reg, mfc_regs->e_enc_options);
 		/* 0: NV12(CbCr), 1: NV21(CrCb) */
-		WRITEL(0x1, S5P_FIMV_PIXEL_FORMAT_V6);
+		WRITEL(0x1, mfc_regs->pixel_format);
 	} else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT_16X16) {
 		/* 0: Linear, 1: 2D tiled*/
-		reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+		reg = READL(mfc_regs->e_enc_options);
 		reg |= (0x1 << 7);
-		WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+		WRITEL(reg, mfc_regs->e_enc_options);
 		/* 0: NV12(CbCr), 1: NV21(CrCb) */
-		WRITEL(0x0, S5P_FIMV_PIXEL_FORMAT_V6);
+		WRITEL(0x0, mfc_regs->pixel_format);
 	}
 
 	/* memory structure recon. frame */
 	/* 0: Linear, 1: 2D tiled */
-	reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+	reg = READL(mfc_regs->e_enc_options);
 	reg |= (0x1 << 8);
-	WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+	WRITEL(reg, mfc_regs->e_enc_options);
 
 	/* padding control & value */
-	WRITEL(0x0, S5P_FIMV_E_PADDING_CTRL_V6);
+	WRITEL(0x0, mfc_regs->e_padding_ctrl);
 	if (p->pad) {
 		reg = 0;
 		/** enable */
@@ -687,64 +734,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, S5P_FIMV_E_PADDING_CTRL_V6);
+		WRITEL(reg, mfc_regs->e_padding_ctrl);
 	}
 
 	/* rate control config. */
 	reg = 0;
 	/* frame-level rate control */
 	reg |= ((p->rc_frame & 0x1) << 9);
-	WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+	WRITEL(reg, mfc_regs->e_rc_config);
 
 	/* bit rate */
 	if (p->rc_frame)
 		WRITEL(p->rc_bitrate,
-			S5P_FIMV_E_RC_BIT_RATE_V6);
+			mfc_regs->e_rc_bit_rate);
 	else
-		WRITEL(1, S5P_FIMV_E_RC_BIT_RATE_V6);
+		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, S5P_FIMV_E_RC_RPARAM_V6);
+			WRITEL(1, mfc_regs->e_rc_mode);
 		else					  /* loose CBR */
-			WRITEL(2, S5P_FIMV_E_RC_RPARAM_V6);
+			WRITEL(2, mfc_regs->e_rc_mode);
 	}
 
 	/* seq header ctrl */
-	reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+	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, S5P_FIMV_E_ENC_OPTIONS_V6);
+	WRITEL(reg, mfc_regs->e_enc_options);
 
 	/* 'DROP_CONTROL_ENABLE', disable */
-	reg = READL(S5P_FIMV_E_RC_CONFIG_V6);
+	reg = READL(mfc_regs->e_rc_config);
 	reg &= ~(0x1 << 10);
-	WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+	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, S5P_FIMV_E_MV_HOR_RANGE_V6);
+	WRITEL(reg, mfc_regs->e_mv_hor_range);
 
 	reg = (p->mv_v_range & S5P_FIMV_E_MV_RANGE_V6_MASK);
-	WRITEL(reg, S5P_FIMV_E_MV_VER_RANGE_V6);
+	WRITEL(reg, mfc_regs->e_mv_ver_range);
 
-	WRITEL(0x0, S5P_FIMV_E_FRAME_INSERTION_V6);
-	WRITEL(0x0, S5P_FIMV_E_ROI_BUFFER_ADDR_V6);
-	WRITEL(0x0, S5P_FIMV_E_PARAM_CHANGE_V6);
-	WRITEL(0x0, S5P_FIMV_E_RC_ROI_CTRL_V6);
-	WRITEL(0x0, S5P_FIMV_E_PICTURE_TAG_V6);
+	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, S5P_FIMV_E_BIT_COUNT_ENABLE_V6);
-	WRITEL(0x0, S5P_FIMV_E_MAX_BIT_COUNT_V6);
-	WRITEL(0x0, S5P_FIMV_E_MIN_BIT_COUNT_V6);
+	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, S5P_FIMV_E_METADATA_BUFFER_ADDR_V6);
-	WRITEL(0x0, S5P_FIMV_E_METADATA_BUFFER_SIZE_V6);
+	WRITEL(0x0, mfc_regs->e_metadata_buffer_addr);
+	WRITEL(0x0, mfc_regs->e_metadata_buffer_size);
 
 	mfc_debug_leave();
 
@@ -754,6 +801,7 @@ static int s5p_mfc_set_enc_params(struct s5p_mfc_ctx *ctx)
 static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
 	struct s5p_mfc_enc_params *p = &ctx->enc_params;
 	struct s5p_mfc_h264_enc_params *p_h264 = &p->codec.h264;
 	unsigned int reg = 0;
@@ -764,10 +812,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(S5P_FIMV_E_GOP_CONFIG_V6);
+	reg = READL(mfc_regs->e_gop_config);
 	reg &= ~(0x3 << 16);
 	reg |= ((p->num_b_frame & 0x3) << 16);
-	WRITEL(reg, S5P_FIMV_E_GOP_CONFIG_V6);
+	WRITEL(reg, mfc_regs->e_gop_config);
 
 	/* profile & level */
 	reg = 0;
@@ -775,18 +823,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, S5P_FIMV_E_PICTURE_PROFILE_V6);
+	WRITEL(reg, mfc_regs->e_picture_profile);
 
 	/* rate control config. */
-	reg = READL(S5P_FIMV_E_RC_CONFIG_V6);
+	reg = READL(mfc_regs->e_rc_config);
 	/** macroblock level rate control */
 	reg &= ~(0x1 << 8);
 	reg |= ((p->rc_mb & 0x1) << 8);
-	WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+	WRITEL(reg, mfc_regs->e_rc_config);
+
 	/** frame QP */
 	reg &= ~(0x3F);
 	reg |= p_h264->rc_frame_qp & 0x3F;
-	WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+	WRITEL(reg, mfc_regs->e_rc_config);
 
 	/* max & min value of QP */
 	reg = 0;
@@ -794,16 +843,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, S5P_FIMV_E_RC_QP_BOUND_V6);
+	WRITEL(reg, mfc_regs->e_rc_qp_bound);
 
 	/* other QPs */
-	WRITEL(0x0, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+	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, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+		WRITEL(reg, mfc_regs->e_fixed_picture_qp);
 	}
 
 	/* frame rate */
@@ -811,38 +860,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, S5P_FIMV_E_RC_FRAME_RATE_V6);
+		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,
-				S5P_FIMV_E_VBV_BUFFER_SIZE_V6);
+				mfc_regs->e_vbv_buffer_size);
 
 		if (p->rc_frame)
-			WRITEL(p->vbv_delay, S5P_FIMV_E_VBV_INIT_DELAY_V6);
+			WRITEL(p->vbv_delay, mfc_regs->e_vbv_init_delay);
 	}
 
 	/* interlace */
 	reg = 0;
 	reg |= ((p_h264->interlace & 0x1) << 3);
-	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+	WRITEL(reg, mfc_regs->e_h264_options);
 
 	/* height */
 	if (p_h264->interlace) {
 		WRITEL(ctx->img_height >> 1,
-				S5P_FIMV_E_FRAME_HEIGHT_V6); /* 32 align */
+				mfc_regs->e_frame_height); /* 32 align */
 		/* cropped height */
 		WRITEL(ctx->img_height >> 1,
-				S5P_FIMV_E_CROPPED_FRAME_HEIGHT_V6);
+				mfc_regs->e_cropped_frame_height);
 	}
 
 	/* loop filter ctrl */
-	reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+	reg = READL(mfc_regs->e_h264_options);
 	reg &= ~(0x3 << 1);
 	reg |= ((p_h264->loop_filter_mode & 0x3) << 1);
-	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+	WRITEL(reg, mfc_regs->e_h264_options);
 
 	/* loopfilter alpha offset */
 	if (p_h264->loop_filter_alpha < 0) {
@@ -852,7 +901,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, S5P_FIMV_E_H264_LF_ALPHA_OFFSET_V6);
+	WRITEL(reg, mfc_regs->e_h264_lf_alpha_offset);
 
 	/* loopfilter beta offset */
 	if (p_h264->loop_filter_beta < 0) {
@@ -862,28 +911,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, S5P_FIMV_E_H264_LF_BETA_OFFSET_V6);
+	WRITEL(reg, mfc_regs->e_h264_lf_beta_offset);
 
 	/* entropy coding mode */
-	reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+	reg = READL(mfc_regs->e_h264_options);
 	reg &= ~(0x1);
 	reg |= p_h264->entropy_mode & 0x1;
-	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+	WRITEL(reg, mfc_regs->e_h264_options);
 
 	/* number of ref. picture */
-	reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+	reg = READL(mfc_regs->e_h264_options);
 	reg &= ~(0x1 << 7);
 	reg |= (((p_h264->num_ref_pic_4p - 1) & 0x1) << 7);
-	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+	WRITEL(reg, mfc_regs->e_h264_options);
 
 	/* 8x8 transform enable */
-	reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+	reg = READL(mfc_regs->e_h264_options);
 	reg &= ~(0x3 << 12);
 	reg |= ((p_h264->_8x8_transform & 0x3) << 12);
-	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+	WRITEL(reg, mfc_regs->e_h264_options);
 
 	/* macroblock adaptive scaling features */
-	WRITEL(0x0, S5P_FIMV_E_MB_RC_CONFIG_V6);
+	WRITEL(0x0, mfc_regs->e_mb_rc_config);
 	if (p->rc_mb) {
 		reg = 0;
 		/** dark region */
@@ -894,92 +943,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, S5P_FIMV_E_MB_RC_CONFIG_V6);
+		WRITEL(reg, mfc_regs->e_mb_rc_config);
 	}
 
 	/* aspect ratio VUI */
-	reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+	READL(mfc_regs->e_h264_options);
 	reg &= ~(0x1 << 5);
 	reg |= ((p_h264->vui_sar & 0x1) << 5);
-	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+	WRITEL(reg, mfc_regs->e_h264_options);
 
-	WRITEL(0x0, S5P_FIMV_E_ASPECT_RATIO_V6);
-	WRITEL(0x0, S5P_FIMV_E_EXTENDED_SAR_V6);
+	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, S5P_FIMV_E_ASPECT_RATIO_V6);
+		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, S5P_FIMV_E_EXTENDED_SAR_V6);
+			WRITEL(reg, mfc_regs->e_extended_sar);
 		}
 	}
 
 	/* intra picture period for H.264 open GOP */
 	/* control */
-	reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+	READL(mfc_regs->e_h264_options);
 	reg &= ~(0x1 << 4);
 	reg |= ((p_h264->open_gop & 0x1) << 4);
-	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+	WRITEL(reg, mfc_regs->e_h264_options);
+
 	/* value */
-	WRITEL(0x0, S5P_FIMV_E_H264_I_PERIOD_V6);
+	WRITEL(0x0, mfc_regs->e_h264_i_period);
 	if (p_h264->open_gop) {
 		reg = 0;
 		reg |= p_h264->open_gop_size & 0xFFFF;
-		WRITEL(reg, S5P_FIMV_E_H264_I_PERIOD_V6);
+		WRITEL(reg, mfc_regs->e_h264_i_period);
 	}
 
 	/* 'WEIGHTED_BI_PREDICTION' for B is disable */
-	reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+	READL(mfc_regs->e_h264_options);
 	reg &= ~(0x3 << 9);
-	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+	WRITEL(reg, mfc_regs->e_h264_options);
 
 	/* 'CONSTRAINED_INTRA_PRED_ENABLE' is disable */
-	reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+	READL(mfc_regs->e_h264_options);
 	reg &= ~(0x1 << 14);
-	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+	WRITEL(reg, mfc_regs->e_h264_options);
 
 	/* ASO */
-	reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+	READL(mfc_regs->e_h264_options);
 	reg &= ~(0x1 << 6);
 	reg |= ((p_h264->aso & 0x1) << 6);
-	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+	WRITEL(reg, mfc_regs->e_h264_options);
 
 	/* hier qp enable */
-	reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+	READL(mfc_regs->e_h264_options);
 	reg &= ~(0x1 << 8);
 	reg |= ((p_h264->open_gop & 0x1) << 8);
-	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+	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, S5P_FIMV_E_H264_NUM_T_LAYER_V6);
+		WRITEL(reg, mfc_regs->e_h264_num_t_layer);
 		/* QP value for each layer */
-		for (i = 0; i < (p_h264->hier_qp_layer & 0x7); i++)
+		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],
-				S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER0_V6 +
-				i * 4);
+				mfc_regs->e_h264_hierarchical_qp_layer0
+				+ i * 4);
+		}
 	}
 	/* number of coding layer should be zero when hierarchical is disable */
-	WRITEL(reg, S5P_FIMV_E_H264_NUM_T_LAYER_V6);
+	WRITEL(reg, mfc_regs->e_h264_num_t_layer);
 
 	/* frame packing SEI generation */
-	reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+	READL(mfc_regs->e_h264_options);
 	reg &= ~(0x1 << 25);
 	reg |= ((p_h264->sei_frame_packing & 0x1) << 25);
-	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+	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, S5P_FIMV_E_H264_FRAME_PACKING_SEI_INFO_V6);
+		WRITEL(reg, mfc_regs->e_h264_frame_packing_sei_info);
 	}
 
 	if (p_h264->fmo) {
@@ -989,8 +1041,8 @@ static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
 				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,
-				S5P_FIMV_E_H264_FMO_RUN_LENGTH_MINUS1_0_V6 +
-				i * 4);
+					mfc_regs->e_h264_fmo_run_length_minus1_0
+					+ i * 4);
 			break;
 		case V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_SCATTERED_SLICES:
 			if (p_h264->fmo_slice_grp > 4)
@@ -1001,10 +1053,10 @@ static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
 			if (p_h264->fmo_slice_grp > 2)
 				p_h264->fmo_slice_grp = 2;
 			WRITEL(p_h264->fmo_chg_dir & 0x1,
-				S5P_FIMV_E_H264_FMO_SLICE_GRP_CHANGE_DIR_V6);
+				mfc_regs->e_h264_fmo_slice_grp_change_dir);
 			/* the valid range is 0 ~ number of macroblocks -1 */
 			WRITEL(p_h264->fmo_chg_rate,
-				S5P_FIMV_E_H264_FMO_SLICE_GRP_CHANGE_RATE_MINUS1_V6);
+			mfc_regs->e_h264_fmo_slice_grp_change_rate_minus1);
 			break;
 		default:
 			mfc_err("Unsupported map type for FMO: %d\n",
@@ -1015,11 +1067,11 @@ static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
 		}
 
 		WRITEL(p_h264->fmo_map_type,
-				S5P_FIMV_E_H264_FMO_SLICE_GRP_MAP_TYPE_V6);
+				mfc_regs->e_h264_fmo_slice_grp_map_type);
 		WRITEL(p_h264->fmo_slice_grp - 1,
-				S5P_FIMV_E_H264_FMO_NUM_SLICE_GRP_MINUS1_V6);
+				mfc_regs->e_h264_fmo_num_slice_grp_minus1);
 	} else {
-		WRITEL(0, S5P_FIMV_E_H264_FMO_NUM_SLICE_GRP_MINUS1_V6);
+		WRITEL(0, mfc_regs->e_h264_fmo_num_slice_grp_minus1);
 	}
 
 	mfc_debug_leave();
@@ -1030,6 +1082,7 @@ static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
 static int s5p_mfc_set_enc_params_mpeg4(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
 	struct s5p_mfc_enc_params *p = &ctx->enc_params;
 	struct s5p_mfc_mpeg4_enc_params *p_mpeg4 = &p->codec.mpeg4;
 	unsigned int reg = 0;
@@ -1039,10 +1092,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(S5P_FIMV_E_GOP_CONFIG_V6);
+	reg = READL(mfc_regs->e_gop_config);
 	reg &= ~(0x3 << 16);
 	reg |= ((p->num_b_frame & 0x3) << 16);
-	WRITEL(reg, S5P_FIMV_E_GOP_CONFIG_V6);
+	WRITEL(reg, mfc_regs->e_gop_config);
 
 	/* profile & level */
 	reg = 0;
@@ -1050,18 +1103,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, S5P_FIMV_E_PICTURE_PROFILE_V6);
+	WRITEL(reg, mfc_regs->e_picture_profile);
 
 	/* rate control config. */
-	reg = READL(S5P_FIMV_E_RC_CONFIG_V6);
+	reg = READL(mfc_regs->e_rc_config);
 	/** macroblock level rate control */
 	reg &= ~(0x1 << 8);
 	reg |= ((p->rc_mb & 0x1) << 8);
-	WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+	WRITEL(reg, mfc_regs->e_rc_config);
+
 	/** frame QP */
 	reg &= ~(0x3F);
 	reg |= p_mpeg4->rc_frame_qp & 0x3F;
-	WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+	WRITEL(reg, mfc_regs->e_rc_config);
 
 	/* max & min value of QP */
 	reg = 0;
@@ -1069,16 +1123,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, S5P_FIMV_E_RC_QP_BOUND_V6);
+	WRITEL(reg, mfc_regs->e_rc_qp_bound);
 
 	/* other QPs */
-	WRITEL(0x0, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+	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, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+		WRITEL(reg, mfc_regs->e_fixed_picture_qp);
 	}
 
 	/* frame rate */
@@ -1086,21 +1140,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, S5P_FIMV_E_RC_FRAME_RATE_V6);
+		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, S5P_FIMV_E_VBV_BUFFER_SIZE_V6);
+		WRITEL(p->vbv_size & 0xFFFF, mfc_regs->e_vbv_buffer_size);
 
 		if (p->rc_frame)
-			WRITEL(p->vbv_delay, S5P_FIMV_E_VBV_INIT_DELAY_V6);
+			WRITEL(p->vbv_delay, mfc_regs->e_vbv_init_delay);
 	}
 
 	/* Disable HEC */
-	WRITEL(0x0, S5P_FIMV_E_MPEG4_OPTIONS_V6);
-	WRITEL(0x0, S5P_FIMV_E_MPEG4_HEC_PERIOD_V6);
+	WRITEL(0x0, mfc_regs->e_mpeg4_options);
+	WRITEL(0x0, mfc_regs->e_mpeg4_hec_period);
 
 	mfc_debug_leave();
 
@@ -1110,6 +1164,7 @@ static int s5p_mfc_set_enc_params_mpeg4(struct s5p_mfc_ctx *ctx)
 static int s5p_mfc_set_enc_params_h263(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
 	struct s5p_mfc_enc_params *p = &ctx->enc_params;
 	struct s5p_mfc_mpeg4_enc_params *p_h263 = &p->codec.mpeg4;
 	unsigned int reg = 0;
@@ -1122,18 +1177,19 @@ static int s5p_mfc_set_enc_params_h263(struct s5p_mfc_ctx *ctx)
 	reg = 0;
 	/** profile */
 	reg |= (0x1 << 4);
-	WRITEL(reg, S5P_FIMV_E_PICTURE_PROFILE_V6);
+	WRITEL(reg, mfc_regs->e_picture_profile);
 
 	/* rate control config. */
-	reg = READL(S5P_FIMV_E_RC_CONFIG_V6);
+	reg = READL(mfc_regs->e_rc_config);
 	/** macroblock level rate control */
 	reg &= ~(0x1 << 8);
 	reg |= ((p->rc_mb & 0x1) << 8);
-	WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+	WRITEL(reg, mfc_regs->e_rc_config);
+
 	/** frame QP */
 	reg &= ~(0x3F);
 	reg |= p_h263->rc_frame_qp & 0x3F;
-	WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+	WRITEL(reg, mfc_regs->e_rc_config);
 
 	/* max & min value of QP */
 	reg = 0;
@@ -1141,16 +1197,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, S5P_FIMV_E_RC_QP_BOUND_V6);
+	WRITEL(reg, mfc_regs->e_rc_qp_bound);
 
 	/* other QPs */
-	WRITEL(0x0, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+	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, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+		WRITEL(reg, mfc_regs->e_fixed_picture_qp);
 	}
 
 	/* frame rate */
@@ -1158,16 +1214,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, S5P_FIMV_E_RC_FRAME_RATE_V6);
+		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, S5P_FIMV_E_VBV_BUFFER_SIZE_V6);
+		WRITEL(p->vbv_size & 0xFFFF, mfc_regs->e_vbv_buffer_size);
 
 		if (p->rc_frame)
-			WRITEL(p->vbv_delay, S5P_FIMV_E_VBV_INIT_DELAY_V6);
+			WRITEL(p->vbv_delay, mfc_regs->e_vbv_init_delay);
 	}
 
 	mfc_debug_leave();
@@ -1178,6 +1234,7 @@ static int s5p_mfc_set_enc_params_h263(struct s5p_mfc_ctx *ctx)
 static int s5p_mfc_set_enc_params_vp8(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
 	struct s5p_mfc_enc_params *p = &ctx->enc_params;
 	struct s5p_mfc_vp8_enc_params *p_vp8 = &p->codec.vp8;
 	unsigned int reg = 0;
@@ -1188,57 +1245,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(S5P_FIMV_E_GOP_CONFIG_V6);
+	reg = READL(mfc_regs->e_gop_config);
 	reg &= ~(0x3 << 16);
 	reg |= ((p->num_b_frame & 0x3) << 16);
-	WRITEL(reg, S5P_FIMV_E_GOP_CONFIG_V6);
+	WRITEL(reg, mfc_regs->e_gop_config);
 
 	/* profile - 0 ~ 3 */
 	reg = p_vp8->profile & 0x3;
-	WRITEL(reg, S5P_FIMV_E_PICTURE_PROFILE_V6);
+	WRITEL(reg, mfc_regs->e_picture_profile);
 
 	/* rate control config. */
-	reg = READL(S5P_FIMV_E_RC_CONFIG_V6);
+	reg = READL(mfc_regs->e_rc_config);
 	/** macroblock level rate control */
 	reg &= ~(0x1 << 8);
 	reg |= ((p->rc_mb & 0x1) << 8);
-	WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+	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, S5P_FIMV_E_RC_FRAME_RATE_V6);
+		WRITEL(reg, mfc_regs->e_rc_frame_rate);
 	}
 
 	/* frame QP */
 	reg &= ~(0x7F);
 	reg |= p_vp8->rc_frame_qp & 0x7F;
-	WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+	WRITEL(reg, mfc_regs->e_rc_config);
 
 	/* other QPs */
-	WRITEL(0x0, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+	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, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+		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, S5P_FIMV_E_RC_QP_BOUND_V6);
+	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, S5P_FIMV_E_VBV_BUFFER_SIZE_V6);
+		WRITEL(p->vbv_size & 0xFFFF, mfc_regs->e_vbv_buffer_size);
 
 		if (p->rc_frame)
-			WRITEL(p->vbv_delay, S5P_FIMV_E_VBV_INIT_DELAY_V6);
+			WRITEL(p->vbv_delay, mfc_regs->e_vbv_init_delay);
 	}
 
 	/* VP8 specific params */
@@ -1260,7 +1317,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, S5P_FIMV_E_VP8_OPTIONS_V7);
+	WRITEL(reg, mfc_regs->e_vp8_options);
 
 	mfc_debug_leave();
 
@@ -1271,6 +1328,7 @@ static int s5p_mfc_set_enc_params_vp8(struct s5p_mfc_ctx *ctx)
 static int s5p_mfc_init_decode_v6(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
 	unsigned int reg = 0;
 	int fmo_aso_ctrl = 0;
 
@@ -1278,9 +1336,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(S5P_FIMV_D_CPB_BUFFER_ADDR_V6),
-		  READL(S5P_FIMV_D_CPB_BUFFER_ADDR_V6),
-		  READL(S5P_FIMV_D_CPB_BUFFER_ADDR_V6));
+		  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);
@@ -1291,11 +1349,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, S5P_FIMV_D_DISPLAY_DELAY_V6);
+		WRITEL(ctx->display_delay, mfc_regs->d_display_delay);
 	}
 
-	if (IS_MFCV7(dev)) {
-		WRITEL(reg, S5P_FIMV_D_DEC_OPTIONS_V6);
+	if (IS_MFCV7_PLUS(dev)) {
+		WRITEL(reg, mfc_regs->d_dec_options);
 		reg = 0;
 	}
 
@@ -1309,22 +1367,22 @@ static int s5p_mfc_init_decode_v6(struct s5p_mfc_ctx *ctx)
 	if (ctx->dst_fmt->fourcc == V4L2_PIX_FMT_NV12MT_16X16)
 		reg |= (0x1 << S5P_FIMV_D_OPT_TILE_MODE_SHIFT_V6);
 
-	if (IS_MFCV7(dev))
-		WRITEL(reg, S5P_FIMV_D_INIT_BUFFER_OPTIONS_V7);
+	if (IS_MFCV7_PLUS(dev))
+		WRITEL(reg, mfc_regs->d_init_buffer_options);
 	else
-		WRITEL(reg, S5P_FIMV_D_DEC_OPTIONS_V6);
+		WRITEL(reg, mfc_regs->d_dec_options);
 
 	/* 0: NV12(CbCr), 1: NV21(CrCb) */
 	if (ctx->dst_fmt->fourcc == V4L2_PIX_FMT_NV21M)
-		WRITEL(0x1, S5P_FIMV_PIXEL_FORMAT_V6);
+		WRITEL(0x1, mfc_regs->pixel_format);
 	else
-		WRITEL(0x0, S5P_FIMV_PIXEL_FORMAT_V6);
+		WRITEL(0x0, mfc_regs->pixel_format);
 
 
 	/* sei parse */
-	WRITEL(ctx->sei_fp_parse & 0x1, S5P_FIMV_D_SEI_ENABLE_V6);
+	WRITEL(ctx->sei_fp_parse & 0x1, mfc_regs->d_sei_enable);
 
-	WRITEL(ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+	WRITEL(ctx->inst_no, mfc_regs->instance_id);
 	s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
 			S5P_FIMV_CH_SEQ_HEADER_V6, NULL);
 
@@ -1335,11 +1393,12 @@ static int s5p_mfc_init_decode_v6(struct s5p_mfc_ctx *ctx)
 static inline void s5p_mfc_set_flush(struct s5p_mfc_ctx *ctx, int flush)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
 
 	if (flush) {
 		dev->curr_ctx = ctx->num;
 		s5p_mfc_clean_ctx_int_flags(ctx);
-		WRITEL(ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+		WRITEL(ctx->inst_no, mfc_regs->instance_id);
 		s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
 				S5P_FIMV_H2R_CMD_FLUSH_V6, NULL);
 	}
@@ -1350,11 +1409,12 @@ static int s5p_mfc_decode_one_frame_v6(struct s5p_mfc_ctx *ctx,
 			enum s5p_mfc_decode_arg last_frame)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
 
-	WRITEL(ctx->dec_dst_flag, S5P_FIMV_D_AVAILABLE_DPB_FLAG_LOWER_V6);
-	WRITEL(ctx->slice_interface & 0x1, S5P_FIMV_D_SLICE_IF_ENABLE_V6);
+	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, S5P_FIMV_INSTANCE_ID_V6);
+	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) {
@@ -1378,6 +1438,7 @@ static int s5p_mfc_decode_one_frame_v6(struct s5p_mfc_ctx *ctx,
 static int s5p_mfc_init_encode_v6(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
 
 	if (ctx->codec_mode == S5P_MFC_CODEC_H264_ENC)
 		s5p_mfc_set_enc_params_h264(ctx);
@@ -1393,13 +1454,13 @@ static int s5p_mfc_init_encode_v6(struct s5p_mfc_ctx *ctx)
 		return -EINVAL;
 	}
 
-	/* Set stride lengths */
-	if (IS_MFCV7(dev)) {
-		WRITEL(ctx->img_width, S5P_FIMV_E_SOURCE_FIRST_STRIDE_V7);
-		WRITEL(ctx->img_width, S5P_FIMV_E_SOURCE_SECOND_STRIDE_V7);
+	/* 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->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+	WRITEL(ctx->inst_no, mfc_regs->instance_id);
 	s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
 			S5P_FIMV_CH_SEQ_HEADER_V6, NULL);
 
@@ -1409,14 +1470,16 @@ static int s5p_mfc_init_encode_v6(struct s5p_mfc_ctx *ctx)
 static int s5p_mfc_h264_set_aso_slice_order_v6(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
 	struct s5p_mfc_enc_params *p = &ctx->enc_params;
 	struct s5p_mfc_h264_enc_params *p_h264 = &p->codec.h264;
 	int i;
 
 	if (p_h264->aso) {
-		for (i = 0; i < 8; i++)
+		for (i = 0; i < ARRAY_SIZE(p_h264->aso_slice_order); i++) {
 			WRITEL(p_h264->aso_slice_order[i],
-				S5P_FIMV_E_H264_ASO_SLICE_ORDER_0_V6 + i * 4);
+				mfc_regs->e_h264_aso_slice_order_0 + i * 4);
+		}
 	}
 	return 0;
 }
@@ -1425,6 +1488,7 @@ static int s5p_mfc_h264_set_aso_slice_order_v6(struct s5p_mfc_ctx *ctx)
 static int s5p_mfc_encode_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;
 
 	mfc_debug(2, "++\n");
 
@@ -1435,7 +1499,7 @@ static int s5p_mfc_encode_one_frame_v6(struct s5p_mfc_ctx *ctx)
 
 	s5p_mfc_set_slice_mode(ctx);
 
-	WRITEL(ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+	WRITEL(ctx->inst_no, mfc_regs->instance_id);
 	s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
 			S5P_FIMV_CH_FRAME_START_V6, NULL);
 
@@ -1810,28 +1874,26 @@ 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)
 {
-	mfc_write(dev, 0, S5P_FIMV_RISC2HOST_CMD_V6);
-	mfc_write(dev, 0, S5P_FIMV_RISC2HOST_INT_V6);
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
+	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)
 {
-	struct s5p_mfc_dev *dev = ctx->dev;
-
 	s5p_mfc_clock_on();
-	WRITEL(data, ofs);
+	WRITEL(data, (void *)ofs);
 	s5p_mfc_clock_off();
 }
 
 static unsigned int
 s5p_mfc_read_info_v6(struct s5p_mfc_ctx *ctx, unsigned int ofs)
 {
-	struct s5p_mfc_dev *dev = ctx->dev;
 	int ret;
 
 	s5p_mfc_clock_on();
-	ret = READL(ofs);
+	ret = READL((void *)ofs);
 	s5p_mfc_clock_off();
 
 	return ret;
@@ -1839,50 +1901,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 mfc_read(dev, S5P_FIMV_D_DISPLAY_LUMA_ADDR_V6);
+	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 mfc_read(dev, S5P_FIMV_D_DECODED_LUMA_ADDR_V6);
+	return READL(dev->mfc_regs->d_decoded_first_plane_addr);
 }
 
 static int s5p_mfc_get_dspl_status_v6(struct s5p_mfc_dev *dev)
 {
-	return mfc_read(dev, S5P_FIMV_D_DISPLAY_STATUS_V6);
+	return READL(dev->mfc_regs->d_display_status);
 }
 
 static int s5p_mfc_get_dec_status_v6(struct s5p_mfc_dev *dev)
 {
-	return mfc_read(dev, S5P_FIMV_D_DECODED_STATUS_V6);
+	return READL(dev->mfc_regs->d_decoded_status);
 }
 
 static int s5p_mfc_get_dec_frame_type_v6(struct s5p_mfc_dev *dev)
 {
-	return mfc_read(dev, S5P_FIMV_D_DECODED_FRAME_TYPE_V6) &
+	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)
 {
-	return mfc_read(ctx->dev, S5P_FIMV_D_DISPLAY_FRAME_TYPE_V6) &
+	struct s5p_mfc_dev *dev = ctx->dev;
+	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 mfc_read(dev, S5P_FIMV_D_DECODED_NAL_SIZE_V6);
+	return READL(dev->mfc_regs->d_decoded_nal_size);
 }
 
 static int s5p_mfc_get_int_reason_v6(struct s5p_mfc_dev *dev)
 {
-	return mfc_read(dev, S5P_FIMV_RISC2HOST_CMD_V6) &
+	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 mfc_read(dev, S5P_FIMV_ERROR_CODE_V6);
+	return READL(dev->mfc_regs->error_code);
 }
 
 static int s5p_mfc_err_dec_v6(unsigned int err)
@@ -1897,82 +1960,323 @@ static int s5p_mfc_err_dspl_v6(unsigned int err)
 
 static int s5p_mfc_get_img_width_v6(struct s5p_mfc_dev *dev)
 {
-	return mfc_read(dev, S5P_FIMV_D_DISPLAY_FRAME_WIDTH_V6);
+	return READL(dev->mfc_regs->d_display_frame_width);
 }
 
 static int s5p_mfc_get_img_height_v6(struct s5p_mfc_dev *dev)
 {
-	return mfc_read(dev, S5P_FIMV_D_DISPLAY_FRAME_HEIGHT_V6);
+	return READL(dev->mfc_regs->d_display_frame_height);
 }
 
 static int s5p_mfc_get_dpb_count_v6(struct s5p_mfc_dev *dev)
 {
-	return mfc_read(dev, S5P_FIMV_D_MIN_NUM_DPB_V6);
+	return READL(dev->mfc_regs->d_min_num_dpb);
 }
 
 static int s5p_mfc_get_mv_count_v6(struct s5p_mfc_dev *dev)
 {
-	return mfc_read(dev, S5P_FIMV_D_MIN_NUM_MV_V6);
+	return READL(dev->mfc_regs->d_min_num_mv);
 }
 
 static int s5p_mfc_get_inst_no_v6(struct s5p_mfc_dev *dev)
 {
-	return mfc_read(dev, S5P_FIMV_RET_INSTANCE_ID_V6);
+	return READL(dev->mfc_regs->ret_instance_id);
 }
 
 static int s5p_mfc_get_enc_dpb_count_v6(struct s5p_mfc_dev *dev)
 {
-	return mfc_read(dev, S5P_FIMV_E_NUM_DPB_V6);
+	return READL(dev->mfc_regs->e_num_dpb);
 }
 
 static int s5p_mfc_get_enc_strm_size_v6(struct s5p_mfc_dev *dev)
 {
-	return mfc_read(dev, S5P_FIMV_E_STREAM_SIZE_V6);
+	return READL(dev->mfc_regs->e_stream_size);
 }
 
 static int s5p_mfc_get_enc_slice_type_v6(struct s5p_mfc_dev *dev)
 {
-	return mfc_read(dev, S5P_FIMV_E_SLICE_TYPE_V6);
+	return READL(dev->mfc_regs->e_slice_type);
 }
 
 static int s5p_mfc_get_enc_pic_count_v6(struct s5p_mfc_dev *dev)
 {
-	return mfc_read(dev, S5P_FIMV_E_PICTURE_COUNT_V6);
+	return READL(dev->mfc_regs->e_picture_count);
 }
 
 static int s5p_mfc_get_sei_avail_status_v6(struct s5p_mfc_ctx *ctx)
 {
-	return mfc_read(ctx->dev, S5P_FIMV_D_FRAME_PACK_SEI_AVAIL_V6);
+	struct s5p_mfc_dev *dev = ctx->dev;
+	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 mfc_read(dev, S5P_FIMV_D_MVC_NUM_VIEWS_V6);
+	return READL(dev->mfc_regs->d_mvc_num_views);
 }
 
 static int s5p_mfc_get_mvc_view_id_v6(struct s5p_mfc_dev *dev)
 {
-	return mfc_read(dev, S5P_FIMV_D_MVC_VIEW_ID_V6);
+	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, PIC_TIME_TOP_V6);
+	return s5p_mfc_read_info_v6(ctx,
+		(unsigned int) 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, PIC_TIME_BOT_V6);
+	return s5p_mfc_read_info_v6(ctx,
+		(unsigned int) 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, CROP_INFO_H_V6);
+	return s5p_mfc_read_info_v6(ctx,
+		(unsigned int) 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, CROP_INFO_V_V6);
+	return s5p_mfc_read_info_v6(ctx,
+		(unsigned int) ctx->dev->mfc_regs->d_display_crop_info2);
+}
+
+static struct s5p_mfc_regs mfc_regs;
+
+/* Initialize registers for MFC v6 onwards */
+const struct s5p_mfc_regs *s5p_mfc_init_regs_v6_plus(struct s5p_mfc_dev *dev)
+{
+	memset(&mfc_regs, 0, sizeof(mfc_regs));
+
+#define S5P_MFC_REG_ADDR(dev, reg) ((dev)->regs_base + (reg))
+#define R(m, r) mfc_regs.m = S5P_MFC_REG_ADDR(dev, r)
+	/* codec common registers */
+	R(risc_on, S5P_FIMV_RISC_ON_V6);
+	R(risc2host_int, S5P_FIMV_RISC2HOST_INT_V6);
+	R(host2risc_int, S5P_FIMV_HOST2RISC_INT_V6);
+	R(risc_base_address, S5P_FIMV_RISC_BASE_ADDRESS_V6);
+	R(mfc_reset, S5P_FIMV_MFC_RESET_V6);
+	R(host2risc_command, S5P_FIMV_HOST2RISC_CMD_V6);
+	R(risc2host_command, S5P_FIMV_RISC2HOST_CMD_V6);
+	R(firmware_version, S5P_FIMV_FW_VERSION_V6);
+	R(instance_id, S5P_FIMV_INSTANCE_ID_V6);
+	R(codec_type, S5P_FIMV_CODEC_TYPE_V6);
+	R(context_mem_addr, S5P_FIMV_CONTEXT_MEM_ADDR_V6);
+	R(context_mem_size, S5P_FIMV_CONTEXT_MEM_SIZE_V6);
+	R(pixel_format, S5P_FIMV_PIXEL_FORMAT_V6);
+	R(ret_instance_id, S5P_FIMV_RET_INSTANCE_ID_V6);
+	R(error_code, S5P_FIMV_ERROR_CODE_V6);
+
+	/* decoder registers */
+	R(d_crc_ctrl, S5P_FIMV_D_CRC_CTRL_V6);
+	R(d_dec_options, S5P_FIMV_D_DEC_OPTIONS_V6);
+	R(d_display_delay, S5P_FIMV_D_DISPLAY_DELAY_V6);
+	R(d_sei_enable, S5P_FIMV_D_SEI_ENABLE_V6);
+	R(d_min_num_dpb, S5P_FIMV_D_MIN_NUM_DPB_V6);
+	R(d_min_num_mv, S5P_FIMV_D_MIN_NUM_MV_V6);
+	R(d_mvc_num_views, S5P_FIMV_D_MVC_NUM_VIEWS_V6);
+	R(d_num_dpb, S5P_FIMV_D_NUM_DPB_V6);
+	R(d_num_mv, S5P_FIMV_D_NUM_MV_V6);
+	R(d_init_buffer_options, S5P_FIMV_D_INIT_BUFFER_OPTIONS_V6);
+	R(d_first_plane_dpb_size, S5P_FIMV_D_LUMA_DPB_SIZE_V6);
+	R(d_second_plane_dpb_size, S5P_FIMV_D_CHROMA_DPB_SIZE_V6);
+	R(d_mv_buffer_size, S5P_FIMV_D_MV_BUFFER_SIZE_V6);
+	R(d_first_plane_dpb, S5P_FIMV_D_LUMA_DPB_V6);
+	R(d_second_plane_dpb, S5P_FIMV_D_CHROMA_DPB_V6);
+	R(d_mv_buffer, S5P_FIMV_D_MV_BUFFER_V6);
+	R(d_scratch_buffer_addr, S5P_FIMV_D_SCRATCH_BUFFER_ADDR_V6);
+	R(d_scratch_buffer_size, S5P_FIMV_D_SCRATCH_BUFFER_SIZE_V6);
+	R(d_cpb_buffer_addr, S5P_FIMV_D_CPB_BUFFER_ADDR_V6);
+	R(d_cpb_buffer_size, S5P_FIMV_D_CPB_BUFFER_SIZE_V6);
+	R(d_available_dpb_flag_lower, S5P_FIMV_D_AVAILABLE_DPB_FLAG_LOWER_V6);
+	R(d_cpb_buffer_offset, S5P_FIMV_D_CPB_BUFFER_OFFSET_V6);
+	R(d_slice_if_enable, S5P_FIMV_D_SLICE_IF_ENABLE_V6);
+	R(d_stream_data_size, S5P_FIMV_D_STREAM_DATA_SIZE_V6);
+	R(d_display_frame_width, S5P_FIMV_D_DISPLAY_FRAME_WIDTH_V6);
+	R(d_display_frame_height, S5P_FIMV_D_DISPLAY_FRAME_HEIGHT_V6);
+	R(d_display_status, S5P_FIMV_D_DISPLAY_STATUS_V6);
+	R(d_display_first_plane_addr, S5P_FIMV_D_DISPLAY_LUMA_ADDR_V6);
+	R(d_display_second_plane_addr, S5P_FIMV_D_DISPLAY_CHROMA_ADDR_V6);
+	R(d_display_frame_type, S5P_FIMV_D_DISPLAY_FRAME_TYPE_V6);
+	R(d_display_crop_info1, S5P_FIMV_D_DISPLAY_CROP_INFO1_V6);
+	R(d_display_crop_info2, S5P_FIMV_D_DISPLAY_CROP_INFO2_V6);
+	R(d_display_aspect_ratio, S5P_FIMV_D_DISPLAY_ASPECT_RATIO_V6);
+	R(d_display_extended_ar, S5P_FIMV_D_DISPLAY_EXTENDED_AR_V6);
+	R(d_decoded_status, S5P_FIMV_D_DECODED_STATUS_V6);
+	R(d_decoded_first_plane_addr, S5P_FIMV_D_DECODED_LUMA_ADDR_V6);
+	R(d_decoded_second_plane_addr, S5P_FIMV_D_DECODED_CHROMA_ADDR_V6);
+	R(d_decoded_frame_type, S5P_FIMV_D_DECODED_FRAME_TYPE_V6);
+	R(d_decoded_nal_size, S5P_FIMV_D_DECODED_NAL_SIZE_V6);
+	R(d_ret_picture_tag_top, S5P_FIMV_D_RET_PICTURE_TAG_TOP_V6);
+	R(d_ret_picture_tag_bot, S5P_FIMV_D_RET_PICTURE_TAG_BOT_V6);
+	R(d_h264_info, S5P_FIMV_D_H264_INFO_V6);
+	R(d_mvc_view_id, S5P_FIMV_D_MVC_VIEW_ID_V6);
+	R(d_frame_pack_sei_avail, S5P_FIMV_D_FRAME_PACK_SEI_AVAIL_V6);
+
+	/* encoder registers */
+	R(e_frame_width, S5P_FIMV_E_FRAME_WIDTH_V6);
+	R(e_frame_height, S5P_FIMV_E_FRAME_HEIGHT_V6);
+	R(e_cropped_frame_width, S5P_FIMV_E_CROPPED_FRAME_WIDTH_V6);
+	R(e_cropped_frame_height, S5P_FIMV_E_CROPPED_FRAME_HEIGHT_V6);
+	R(e_frame_crop_offset, S5P_FIMV_E_FRAME_CROP_OFFSET_V6);
+	R(e_enc_options, S5P_FIMV_E_ENC_OPTIONS_V6);
+	R(e_picture_profile, S5P_FIMV_E_PICTURE_PROFILE_V6);
+	R(e_vbv_buffer_size, S5P_FIMV_E_VBV_BUFFER_SIZE_V6);
+	R(e_vbv_init_delay, S5P_FIMV_E_VBV_INIT_DELAY_V6);
+	R(e_fixed_picture_qp, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+	R(e_rc_config, S5P_FIMV_E_RC_CONFIG_V6);
+	R(e_rc_qp_bound, S5P_FIMV_E_RC_QP_BOUND_V6);
+	R(e_rc_mode, S5P_FIMV_E_RC_RPARAM_V6);
+	R(e_mb_rc_config, S5P_FIMV_E_MB_RC_CONFIG_V6);
+	R(e_padding_ctrl, S5P_FIMV_E_PADDING_CTRL_V6);
+	R(e_mv_hor_range, S5P_FIMV_E_MV_HOR_RANGE_V6);
+	R(e_mv_ver_range, S5P_FIMV_E_MV_VER_RANGE_V6);
+	R(e_num_dpb, S5P_FIMV_E_NUM_DPB_V6);
+	R(e_luma_dpb, S5P_FIMV_E_LUMA_DPB_V6);
+	R(e_chroma_dpb, S5P_FIMV_E_CHROMA_DPB_V6);
+	R(e_me_buffer, S5P_FIMV_E_ME_BUFFER_V6);
+	R(e_scratch_buffer_addr, S5P_FIMV_E_SCRATCH_BUFFER_ADDR_V6);
+	R(e_scratch_buffer_size, S5P_FIMV_E_SCRATCH_BUFFER_SIZE_V6);
+	R(e_tmv_buffer0, S5P_FIMV_E_TMV_BUFFER0_V6);
+	R(e_tmv_buffer1, S5P_FIMV_E_TMV_BUFFER1_V6);
+	R(e_source_first_plane_addr, S5P_FIMV_E_SOURCE_LUMA_ADDR_V6);
+	R(e_source_second_plane_addr, S5P_FIMV_E_SOURCE_CHROMA_ADDR_V6);
+	R(e_stream_buffer_addr, S5P_FIMV_E_STREAM_BUFFER_ADDR_V6);
+	R(e_stream_buffer_size, S5P_FIMV_E_STREAM_BUFFER_SIZE_V6);
+	R(e_roi_buffer_addr, S5P_FIMV_E_ROI_BUFFER_ADDR_V6);
+	R(e_param_change, S5P_FIMV_E_PARAM_CHANGE_V6);
+	R(e_ir_size, S5P_FIMV_E_IR_SIZE_V6);
+	R(e_gop_config, S5P_FIMV_E_GOP_CONFIG_V6);
+	R(e_mslice_mode, S5P_FIMV_E_MSLICE_MODE_V6);
+	R(e_mslice_size_mb, S5P_FIMV_E_MSLICE_SIZE_MB_V6);
+	R(e_mslice_size_bits, S5P_FIMV_E_MSLICE_SIZE_BITS_V6);
+	R(e_frame_insertion, S5P_FIMV_E_FRAME_INSERTION_V6);
+	R(e_rc_frame_rate, S5P_FIMV_E_RC_FRAME_RATE_V6);
+	R(e_rc_bit_rate, S5P_FIMV_E_RC_BIT_RATE_V6);
+	R(e_rc_roi_ctrl, S5P_FIMV_E_RC_ROI_CTRL_V6);
+	R(e_picture_tag, S5P_FIMV_E_PICTURE_TAG_V6);
+	R(e_bit_count_enable, S5P_FIMV_E_BIT_COUNT_ENABLE_V6);
+	R(e_max_bit_count, S5P_FIMV_E_MAX_BIT_COUNT_V6);
+	R(e_min_bit_count, S5P_FIMV_E_MIN_BIT_COUNT_V6);
+	R(e_metadata_buffer_addr, S5P_FIMV_E_METADATA_BUFFER_ADDR_V6);
+	R(e_metadata_buffer_size, S5P_FIMV_E_METADATA_BUFFER_SIZE_V6);
+	R(e_encoded_source_first_plane_addr,
+			S5P_FIMV_E_ENCODED_SOURCE_LUMA_ADDR_V6);
+	R(e_encoded_source_second_plane_addr,
+			S5P_FIMV_E_ENCODED_SOURCE_CHROMA_ADDR_V6);
+	R(e_stream_size, S5P_FIMV_E_STREAM_SIZE_V6);
+	R(e_slice_type, S5P_FIMV_E_SLICE_TYPE_V6);
+	R(e_picture_count, S5P_FIMV_E_PICTURE_COUNT_V6);
+	R(e_ret_picture_tag, S5P_FIMV_E_RET_PICTURE_TAG_V6);
+	R(e_recon_luma_dpb_addr, S5P_FIMV_E_RECON_LUMA_DPB_ADDR_V6);
+	R(e_recon_chroma_dpb_addr, S5P_FIMV_E_RECON_CHROMA_DPB_ADDR_V6);
+	R(e_mpeg4_options, S5P_FIMV_E_MPEG4_OPTIONS_V6);
+	R(e_mpeg4_hec_period, S5P_FIMV_E_MPEG4_HEC_PERIOD_V6);
+	R(e_aspect_ratio, S5P_FIMV_E_ASPECT_RATIO_V6);
+	R(e_extended_sar, S5P_FIMV_E_EXTENDED_SAR_V6);
+	R(e_h264_options, S5P_FIMV_E_H264_OPTIONS_V6);
+	R(e_h264_lf_alpha_offset, S5P_FIMV_E_H264_LF_ALPHA_OFFSET_V6);
+	R(e_h264_lf_beta_offset, S5P_FIMV_E_H264_LF_BETA_OFFSET_V6);
+	R(e_h264_i_period, S5P_FIMV_E_H264_I_PERIOD_V6);
+	R(e_h264_fmo_slice_grp_map_type,
+			S5P_FIMV_E_H264_FMO_SLICE_GRP_MAP_TYPE_V6);
+	R(e_h264_fmo_num_slice_grp_minus1,
+			S5P_FIMV_E_H264_FMO_NUM_SLICE_GRP_MINUS1_V6);
+	R(e_h264_fmo_slice_grp_change_dir,
+			S5P_FIMV_E_H264_FMO_SLICE_GRP_CHANGE_DIR_V6);
+	R(e_h264_fmo_slice_grp_change_rate_minus1,
+			S5P_FIMV_E_H264_FMO_SLICE_GRP_CHANGE_RATE_MINUS1_V6);
+	R(e_h264_fmo_run_length_minus1_0,
+			S5P_FIMV_E_H264_FMO_RUN_LENGTH_MINUS1_0_V6);
+	R(e_h264_aso_slice_order_0, S5P_FIMV_E_H264_ASO_SLICE_ORDER_0_V6);
+	R(e_h264_num_t_layer, S5P_FIMV_E_H264_NUM_T_LAYER_V6);
+	R(e_h264_hierarchical_qp_layer0,
+			S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER0_V6);
+	R(e_h264_frame_packing_sei_info,
+			S5P_FIMV_E_H264_FRAME_PACKING_SEI_INFO_V6);
+
+	if (!IS_MFCV7_PLUS(dev))
+		goto done;
+
+	/* Initialize registers used in MFC v7+ */
+	R(e_source_first_plane_addr, S5P_FIMV_E_SOURCE_FIRST_ADDR_V7);
+	R(e_source_second_plane_addr, S5P_FIMV_E_SOURCE_SECOND_ADDR_V7);
+	R(e_source_third_plane_addr, S5P_FIMV_E_SOURCE_THIRD_ADDR_V7);
+	R(e_source_first_plane_stride, S5P_FIMV_E_SOURCE_FIRST_STRIDE_V7);
+	R(e_source_second_plane_stride, S5P_FIMV_E_SOURCE_SECOND_STRIDE_V7);
+	R(e_source_third_plane_stride, S5P_FIMV_E_SOURCE_THIRD_STRIDE_V7);
+	R(e_encoded_source_first_plane_addr,
+			S5P_FIMV_E_ENCODED_SOURCE_FIRST_ADDR_V7);
+	R(e_encoded_source_second_plane_addr,
+			S5P_FIMV_E_ENCODED_SOURCE_SECOND_ADDR_V7);
+	R(e_vp8_options, S5P_FIMV_E_VP8_OPTIONS_V7);
+
+	if (!IS_MFCV8(dev))
+		goto done;
+
+	/* Initialize registers used in MFC v8 only.
+	 * Also, over-write the registers which have
+	 * a different offset for MFC v8. */
+	R(d_stream_data_size, S5P_FIMV_D_STREAM_DATA_SIZE_V8);
+	R(d_cpb_buffer_addr, S5P_FIMV_D_CPB_BUFFER_ADDR_V8);
+	R(d_cpb_buffer_size, S5P_FIMV_D_CPB_BUFFER_SIZE_V8);
+	R(d_cpb_buffer_offset, S5P_FIMV_D_CPB_BUFFER_OFFSET_V8);
+	R(d_first_plane_dpb_size, S5P_FIMV_D_FIRST_PLANE_DPB_SIZE_V8);
+	R(d_second_plane_dpb_size, S5P_FIMV_D_SECOND_PLANE_DPB_SIZE_V8);
+	R(d_scratch_buffer_addr, S5P_FIMV_D_SCRATCH_BUFFER_ADDR_V8);
+	R(d_scratch_buffer_size, S5P_FIMV_D_SCRATCH_BUFFER_SIZE_V8);
+	R(d_first_plane_dpb_stride_size,
+			S5P_FIMV_D_FIRST_PLANE_DPB_STRIDE_SIZE_V8);
+	R(d_second_plane_dpb_stride_size,
+			S5P_FIMV_D_SECOND_PLANE_DPB_STRIDE_SIZE_V8);
+	R(d_mv_buffer_size, S5P_FIMV_D_MV_BUFFER_SIZE_V8);
+	R(d_num_mv, S5P_FIMV_D_NUM_MV_V8);
+	R(d_first_plane_dpb, S5P_FIMV_D_FIRST_PLANE_DPB_V8);
+	R(d_second_plane_dpb, S5P_FIMV_D_SECOND_PLANE_DPB_V8);
+	R(d_mv_buffer, S5P_FIMV_D_MV_BUFFER_V8);
+	R(d_init_buffer_options, S5P_FIMV_D_INIT_BUFFER_OPTIONS_V8);
+	R(d_available_dpb_flag_lower, S5P_FIMV_D_AVAILABLE_DPB_FLAG_LOWER_V8);
+	R(d_slice_if_enable, S5P_FIMV_D_SLICE_IF_ENABLE_V8);
+	R(d_display_first_plane_addr, S5P_FIMV_D_DISPLAY_FIRST_PLANE_ADDR_V8);
+	R(d_display_second_plane_addr, S5P_FIMV_D_DISPLAY_SECOND_PLANE_ADDR_V8);
+	R(d_decoded_first_plane_addr, S5P_FIMV_D_DECODED_FIRST_PLANE_ADDR_V8);
+	R(d_decoded_second_plane_addr, S5P_FIMV_D_DECODED_SECOND_PLANE_ADDR_V8);
+	R(d_display_status, S5P_FIMV_D_DISPLAY_STATUS_V8);
+	R(d_decoded_status, S5P_FIMV_D_DECODED_STATUS_V8);
+	R(d_decoded_frame_type, S5P_FIMV_D_DECODED_FRAME_TYPE_V8);
+	R(d_display_frame_type, S5P_FIMV_D_DISPLAY_FRAME_TYPE_V8);
+	R(d_decoded_nal_size, S5P_FIMV_D_DECODED_NAL_SIZE_V8);
+	R(d_display_frame_width, S5P_FIMV_D_DISPLAY_FRAME_WIDTH_V8);
+	R(d_display_frame_height, S5P_FIMV_D_DISPLAY_FRAME_HEIGHT_V8);
+	R(d_frame_pack_sei_avail, S5P_FIMV_D_FRAME_PACK_SEI_AVAIL_V8);
+	R(d_mvc_num_views, S5P_FIMV_D_MVC_NUM_VIEWS_V8);
+	R(d_mvc_view_id, S5P_FIMV_D_MVC_VIEW_ID_V8);
+	R(d_ret_picture_tag_top, S5P_FIMV_D_RET_PICTURE_TAG_TOP_V8);
+	R(d_ret_picture_tag_bot, S5P_FIMV_D_RET_PICTURE_TAG_BOT_V8);
+	R(d_display_crop_info1, S5P_FIMV_D_DISPLAY_CROP_INFO1_V8);
+	R(d_display_crop_info2, S5P_FIMV_D_DISPLAY_CROP_INFO2_V8);
+
+	/* encoder registers */
+	R(e_padding_ctrl, S5P_FIMV_E_PADDING_CTRL_V8);
+	R(e_rc_config, S5P_FIMV_E_RC_CONFIG_V8);
+	R(e_rc_mode, S5P_FIMV_E_RC_RPARAM_V8);
+	R(e_mv_hor_range, S5P_FIMV_E_MV_HOR_RANGE_V8);
+	R(e_mv_ver_range, S5P_FIMV_E_MV_VER_RANGE_V8);
+	R(e_rc_qp_bound, S5P_FIMV_E_RC_QP_BOUND_V8);
+	R(e_fixed_picture_qp, S5P_FIMV_E_FIXED_PICTURE_QP_V8);
+	R(e_vbv_buffer_size, S5P_FIMV_E_VBV_BUFFER_SIZE_V8);
+	R(e_vbv_init_delay, S5P_FIMV_E_VBV_INIT_DELAY_V8);
+	R(e_mb_rc_config, S5P_FIMV_E_MB_RC_CONFIG_V8);
+	R(e_aspect_ratio, S5P_FIMV_E_ASPECT_RATIO_V8);
+	R(e_extended_sar, S5P_FIMV_E_EXTENDED_SAR_V8);
+	R(e_h264_options, S5P_FIMV_E_H264_OPTIONS_V8);
+
+done:
+	return &mfc_regs;
+#undef S5P_MFC_REG_ADDR
+#undef R
 }
 
 /* Initialize opr function pointers for MFC v6 */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.h b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.h
index ab164efa127..80558484bb4 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.h
@@ -40,11 +40,6 @@
 #define FRAME_DELTA_H264_H263		1
 #define TIGHT_CBR_MAX			10
 
-/* Definitions for shared memory compatibility */
-#define PIC_TIME_TOP_V6		S5P_FIMV_D_RET_PICTURE_TAG_TOP_V6
-#define PIC_TIME_BOT_V6		S5P_FIMV_D_RET_PICTURE_TAG_BOT_V6
-#define CROP_INFO_H_V6		S5P_FIMV_D_DISPLAY_CROP_INFO1_V6
-#define CROP_INFO_V_V6		S5P_FIMV_D_DISPLAY_CROP_INFO2_V6
-
 struct s5p_mfc_hw_ops *s5p_mfc_init_hw_ops_v6(void);
+const struct s5p_mfc_regs *s5p_mfc_init_regs_v6_plus(struct s5p_mfc_dev *dev);
 #endif /* S5P_MFC_OPR_V6_H_ */
diff --git a/drivers/media/platform/s5p-tv/hdmi_drv.c b/drivers/media/platform/s5p-tv/hdmi_drv.c
index 534722c04ec..754740f4b67 100644
--- a/drivers/media/platform/s5p-tv/hdmi_drv.c
+++ b/drivers/media/platform/s5p-tv/hdmi_drv.c
@@ -674,6 +674,8 @@ static int hdmi_g_mbus_fmt(struct v4l2_subdev *sd,
 static int hdmi_enum_dv_timings(struct v4l2_subdev *sd,
 	struct v4l2_enum_dv_timings *timings)
 {
+	if (timings->pad != 0)
+		return -EINVAL;
 	if (timings->index >= ARRAY_SIZE(hdmi_timings))
 		return -EINVAL;
 	timings->timings = hdmi_timings[timings->index].dv_timings;
@@ -687,8 +689,11 @@ static int hdmi_dv_timings_cap(struct v4l2_subdev *sd,
 {
 	struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
 
+	if (cap->pad != 0)
+		return -EINVAL;
+
 	/* Let the phy fill in the pixelclock range */
-	v4l2_subdev_call(hdev->phy_sd, video, dv_timings_cap, cap);
+	v4l2_subdev_call(hdev->phy_sd, pad, dv_timings_cap, cap);
 	cap->type = V4L2_DV_BT_656_1120;
 	cap->bt.min_width = 720;
 	cap->bt.max_width = 1920;
@@ -707,12 +712,15 @@ static const struct v4l2_subdev_core_ops hdmi_sd_core_ops = {
 static const struct v4l2_subdev_video_ops hdmi_sd_video_ops = {
 	.s_dv_timings = hdmi_s_dv_timings,
 	.g_dv_timings = hdmi_g_dv_timings,
-	.enum_dv_timings = hdmi_enum_dv_timings,
-	.dv_timings_cap = hdmi_dv_timings_cap,
 	.g_mbus_fmt = hdmi_g_mbus_fmt,
 	.s_stream = hdmi_s_stream,
 };
 
+static const struct v4l2_subdev_pad_ops hdmi_sd_pad_ops = {
+	.enum_dv_timings = hdmi_enum_dv_timings,
+	.dv_timings_cap = hdmi_dv_timings_cap,
+};
+
 static const struct v4l2_subdev_ops hdmi_sd_ops = {
 	.core = &hdmi_sd_core_ops,
 	.video = &hdmi_sd_video_ops,
diff --git a/drivers/media/platform/s5p-tv/hdmiphy_drv.c b/drivers/media/platform/s5p-tv/hdmiphy_drv.c
index e19a0af1ea4..c2f2e35642f 100644
--- a/drivers/media/platform/s5p-tv/hdmiphy_drv.c
+++ b/drivers/media/platform/s5p-tv/hdmiphy_drv.c
@@ -225,6 +225,9 @@ static int hdmiphy_s_dv_timings(struct v4l2_subdev *sd,
 static int hdmiphy_dv_timings_cap(struct v4l2_subdev *sd,
 	struct v4l2_dv_timings_cap *cap)
 {
+	if (cap->pad != 0)
+		return -EINVAL;
+
 	cap->type = V4L2_DV_BT_656_1120;
 	/* The phy only determines the pixelclock, leave the other values
 	 * at 0 to signify that we have no information for them. */
@@ -259,13 +262,17 @@ static const struct v4l2_subdev_core_ops hdmiphy_core_ops = {
 
 static const struct v4l2_subdev_video_ops hdmiphy_video_ops = {
 	.s_dv_timings = hdmiphy_s_dv_timings,
-	.dv_timings_cap = hdmiphy_dv_timings_cap,
 	.s_stream =  hdmiphy_s_stream,
 };
 
+static const struct v4l2_subdev_pad_ops hdmiphy_pad_ops = {
+	.dv_timings_cap = hdmiphy_dv_timings_cap,
+};
+
 static const struct v4l2_subdev_ops hdmiphy_ops = {
 	.core = &hdmiphy_core_ops,
 	.video = &hdmiphy_video_ops,
+	.pad = &hdmiphy_pad_ops,
 };
 
 static int hdmiphy_probe(struct i2c_client *client,
diff --git a/drivers/media/platform/s5p-tv/mixer_video.c b/drivers/media/platform/s5p-tv/mixer_video.c
index a1ce55fd30f..8a8dbc8fdfd 100644
--- a/drivers/media/platform/s5p-tv/mixer_video.c
+++ b/drivers/media/platform/s5p-tv/mixer_video.c
@@ -509,9 +509,11 @@ static int mxr_enum_dv_timings(struct file *file, void *fh,
 	struct mxr_device *mdev = layer->mdev;
 	int ret;
 
+	timings->pad = 0;
+
 	/* lock protects from changing sd_out */
 	mutex_lock(&mdev->mutex);
-	ret = v4l2_subdev_call(to_outsd(mdev), video, enum_dv_timings, timings);
+	ret = v4l2_subdev_call(to_outsd(mdev), pad, enum_dv_timings, timings);
 	mutex_unlock(&mdev->mutex);
 
 	return ret ? -EINVAL : 0;
@@ -567,9 +569,11 @@ static int mxr_dv_timings_cap(struct file *file, void *fh,
 	struct mxr_device *mdev = layer->mdev;
 	int ret;
 
+	cap->pad = 0;
+
 	/* lock protects from changing sd_out */
 	mutex_lock(&mdev->mutex);
-	ret = v4l2_subdev_call(to_outsd(mdev), video, dv_timings_cap, cap);
+	ret = v4l2_subdev_call(to_outsd(mdev), pad, dv_timings_cap, cap);
 	mutex_unlock(&mdev->mutex);
 
 	return ret ? -EINVAL : 0;
@@ -985,7 +989,7 @@ static void mxr_watchdog(unsigned long arg)
 	spin_unlock_irqrestore(&layer->enq_slock, flags);
 }
 
-static int stop_streaming(struct vb2_queue *vq)
+static void stop_streaming(struct vb2_queue *vq)
 {
 	struct mxr_layer *layer = vb2_get_drv_priv(vq);
 	struct mxr_device *mdev = layer->mdev;
@@ -1031,7 +1035,6 @@ static int stop_streaming(struct vb2_queue *vq)
 	mxr_streamer_put(mdev);
 	/* allow changes in output configuration */
 	mxr_output_put(mdev);
-	return 0;
 }
 
 static struct vb2_ops mxr_video_qops = {
diff --git a/drivers/media/platform/soc_camera/atmel-isi.c b/drivers/media/platform/soc_camera/atmel-isi.c
index f0b6c900034..38c723aca43 100644
--- a/drivers/media/platform/soc_camera/atmel-isi.c
+++ b/drivers/media/platform/soc_camera/atmel-isi.c
@@ -406,7 +406,7 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 }
 
 /* abort streaming and wait for last buffer */
-static int stop_streaming(struct vb2_queue *vq)
+static void stop_streaming(struct vb2_queue *vq)
 {
 	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
@@ -433,7 +433,7 @@ static int stop_streaming(struct vb2_queue *vq)
 	if (time_after(jiffies, timeout)) {
 		dev_err(icd->parent,
 			"Timeout waiting for finishing codec request\n");
-		return -ETIMEDOUT;
+		return;
 	}
 
 	/* Disable interrupts */
@@ -444,8 +444,6 @@ static int stop_streaming(struct vb2_queue *vq)
 	ret = atmel_isi_wait_status(isi, WAIT_ISI_DISABLE);
 	if (ret < 0)
 		dev_err(icd->parent, "Disable ISI timed out\n");
-
-	return ret;
 }
 
 static struct vb2_ops isi_video_qops = {
diff --git a/drivers/media/platform/soc_camera/mx2_camera.c b/drivers/media/platform/soc_camera/mx2_camera.c
index 3e844803bdc..b40bc2e5ba4 100644
--- a/drivers/media/platform/soc_camera/mx2_camera.c
+++ b/drivers/media/platform/soc_camera/mx2_camera.c
@@ -741,7 +741,7 @@ static int mx2_start_streaming(struct vb2_queue *q, unsigned int count)
 	return 0;
 }
 
-static int mx2_stop_streaming(struct vb2_queue *q)
+static void mx2_stop_streaming(struct vb2_queue *q)
 {
 	struct soc_camera_device *icd = soc_camera_from_vb2q(q);
 	struct soc_camera_host *ici =
@@ -773,8 +773,6 @@ static int mx2_stop_streaming(struct vb2_queue *q)
 
 	dma_free_coherent(ici->v4l2_dev.dev,
 			  pcdev->discard_size, b, pcdev->discard_buffer_dma);
-
-	return 0;
 }
 
 static struct vb2_ops mx2_videobuf_ops = {
diff --git a/drivers/media/platform/soc_camera/mx3_camera.c b/drivers/media/platform/soc_camera/mx3_camera.c
index 9ed81ac6881..83315dfeef6 100644
--- a/drivers/media/platform/soc_camera/mx3_camera.c
+++ b/drivers/media/platform/soc_camera/mx3_camera.c
@@ -406,7 +406,7 @@ static int mx3_videobuf_init(struct vb2_buffer *vb)
 	return 0;
 }
 
-static int mx3_stop_streaming(struct vb2_queue *q)
+static void mx3_stop_streaming(struct vb2_queue *q)
 {
 	struct soc_camera_device *icd = soc_camera_from_vb2q(q);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
@@ -430,8 +430,6 @@ static int mx3_stop_streaming(struct vb2_queue *q)
 	}
 
 	spin_unlock_irqrestore(&mx3_cam->lock, flags);
-
-	return 0;
 }
 
 static struct vb2_ops mx3_videobuf_ops = {
diff --git a/drivers/media/platform/soc_camera/rcar_vin.c b/drivers/media/platform/soc_camera/rcar_vin.c
index 704eee76648..e594230e84d 100644
--- a/drivers/media/platform/soc_camera/rcar_vin.c
+++ b/drivers/media/platform/soc_camera/rcar_vin.c
@@ -513,7 +513,7 @@ static int rcar_vin_videobuf_init(struct vb2_buffer *vb)
 	return 0;
 }
 
-static int rcar_vin_stop_streaming(struct vb2_queue *vq)
+static void rcar_vin_stop_streaming(struct vb2_queue *vq)
 {
 	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
@@ -524,8 +524,6 @@ static int rcar_vin_stop_streaming(struct vb2_queue *vq)
 	list_for_each_safe(buf_head, tmp, &priv->capture)
 		list_del_init(buf_head);
 	spin_unlock_irq(&priv->lock);
-
-	return 0;
 }
 
 static struct vb2_ops rcar_vin_vb2_ops = {
diff --git a/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c b/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c
index 3e75a469cd4..20ad4a571d3 100644
--- a/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c
+++ b/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c
@@ -471,7 +471,7 @@ static int sh_mobile_ceu_videobuf_init(struct vb2_buffer *vb)
 	return 0;
 }
 
-static int sh_mobile_ceu_stop_streaming(struct vb2_queue *q)
+static void sh_mobile_ceu_stop_streaming(struct vb2_queue *q)
 {
 	struct soc_camera_device *icd = container_of(q, struct soc_camera_device, vb2_vidq);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
@@ -487,7 +487,7 @@ static int sh_mobile_ceu_stop_streaming(struct vb2_queue *q)
 
 	spin_unlock_irq(&pcdev->lock);
 
-	return sh_mobile_ceu_soft_reset(pcdev);
+	sh_mobile_ceu_soft_reset(pcdev);
 }
 
 static struct vb2_ops sh_mobile_ceu_videobuf_ops = {
diff --git a/drivers/media/platform/soc_camera/soc_camera.c b/drivers/media/platform/soc_camera/soc_camera.c
index 4b8c024fc48..7fec8cdaf09 100644
--- a/drivers/media/platform/soc_camera/soc_camera.c
+++ b/drivers/media/platform/soc_camera/soc_camera.c
@@ -314,7 +314,7 @@ static int soc_camera_s_std(struct file *file, void *priv, v4l2_std_id a)
 	struct soc_camera_device *icd = file->private_data;
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 
-	return v4l2_subdev_call(sd, core, s_std, a);
+	return v4l2_subdev_call(sd, video, s_std, a);
 }
 
 static int soc_camera_g_std(struct file *file, void *priv, v4l2_std_id *a)
@@ -322,7 +322,7 @@ static int soc_camera_g_std(struct file *file, void *priv, v4l2_std_id *a)
 	struct soc_camera_device *icd = file->private_data;
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 
-	return v4l2_subdev_call(sd, core, g_std, a);
+	return v4l2_subdev_call(sd, video, g_std, a);
 }
 
 static int soc_camera_enum_framesizes(struct file *file, void *fh,
@@ -1277,6 +1277,8 @@ static int soc_camera_probe_finish(struct soc_camera_device *icd)
 	sd->grp_id = soc_camera_grp_id(icd);
 	v4l2_set_subdev_hostdata(sd, icd);
 
+	v4l2_subdev_call(sd, video, g_tvnorms, &icd->vdev->tvnorms);
+
 	ret = v4l2_ctrl_add_handler(&icd->ctrl_handler, sd->ctrl_handler, NULL);
 	if (ret < 0)
 		return ret;
@@ -1997,6 +1999,12 @@ static int soc_camera_video_start(struct soc_camera_device *icd)
 		return -ENODEV;
 
 	video_set_drvdata(icd->vdev, icd);
+	if (icd->vdev->tvnorms == 0) {
+		/* disable the STD API if there are no tvnorms defined */
+		v4l2_disable_ioctl(icd->vdev, VIDIOC_G_STD);
+		v4l2_disable_ioctl(icd->vdev, VIDIOC_S_STD);
+		v4l2_disable_ioctl(icd->vdev, VIDIOC_ENUMSTD);
+	}
 	ret = video_register_device(icd->vdev, VFL_TYPE_GRABBER, -1);
 	if (ret < 0) {
 		dev_err(icd->pdev, "video_register_device failed: %d\n", ret);
diff --git a/drivers/media/platform/ti-vpe/csc.c b/drivers/media/platform/ti-vpe/csc.c
index acfea500710..940df4000c4 100644
--- a/drivers/media/platform/ti-vpe/csc.c
+++ b/drivers/media/platform/ti-vpe/csc.c
@@ -180,16 +180,16 @@ struct csc_data *csc_create(struct platform_device *pdev)
 	csc->pdev = pdev;
 
 	csc->res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
-			"vpe_csc");
+			"csc");
 	if (csc->res == NULL) {
 		dev_err(&pdev->dev, "missing platform resources data\n");
 		return ERR_PTR(-ENODEV);
 	}
 
 	csc->base = devm_ioremap_resource(&pdev->dev, csc->res);
-	if (!csc->base) {
+	if (IS_ERR(csc->base)) {
 		dev_err(&pdev->dev, "failed to ioremap\n");
-		return ERR_PTR(-ENOMEM);
+		return csc->base;
 	}
 
 	return csc;
diff --git a/drivers/media/platform/ti-vpe/sc.c b/drivers/media/platform/ti-vpe/sc.c
index 93f0af546b7..6314171ffe9 100644
--- a/drivers/media/platform/ti-vpe/sc.c
+++ b/drivers/media/platform/ti-vpe/sc.c
@@ -302,9 +302,9 @@ struct sc_data *sc_create(struct platform_device *pdev)
 	}
 
 	sc->base = devm_ioremap_resource(&pdev->dev, sc->res);
-	if (!sc->base) {
+	if (IS_ERR(sc->base)) {
 		dev_err(&pdev->dev, "failed to ioremap\n");
-		return ERR_PTR(-ENOMEM);
+		return sc->base;
 	}
 
 	return sc;
diff --git a/drivers/media/platform/ti-vpe/vpdma.c b/drivers/media/platform/ti-vpe/vpdma.c
index e8175e7938e..a51a0135980 100644
--- a/drivers/media/platform/ti-vpe/vpdma.c
+++ b/drivers/media/platform/ti-vpe/vpdma.c
@@ -614,8 +614,17 @@ static void dump_dtd(struct vpdma_dtd *dtd)
 /*
  * append an outbound data transfer descriptor to the given descriptor list,
  * this sets up a 'client to memory' VPDMA transfer for the given VPDMA channel
+ *
+ * @list: vpdma desc list to which we add this decriptor
+ * @width: width of the image in pixels in memory
+ * @c_rect: compose params of output image
+ * @fmt: vpdma data format of the buffer
+ * dma_addr: dma address as seen by VPDMA
+ * chan: VPDMA channel
+ * flags: VPDMA flags to configure some descriptor fileds
  */
-void vpdma_add_out_dtd(struct vpdma_desc_list *list, struct v4l2_rect *c_rect,
+void vpdma_add_out_dtd(struct vpdma_desc_list *list, int width,
+		const struct v4l2_rect *c_rect,
 		const struct vpdma_data_format *fmt, dma_addr_t dma_addr,
 		enum vpdma_channel chan, u32 flags)
 {
@@ -623,6 +632,7 @@ void vpdma_add_out_dtd(struct vpdma_desc_list *list, struct v4l2_rect *c_rect,
 	int field = 0;
 	int notify = 1;
 	int channel, next_chan;
+	struct v4l2_rect rect = *c_rect;
 	int depth = fmt->depth;
 	int stride;
 	struct vpdma_dtd *dtd;
@@ -630,11 +640,15 @@ void vpdma_add_out_dtd(struct vpdma_desc_list *list, struct v4l2_rect *c_rect,
 	channel = next_chan = chan_info[chan].num;
 
 	if (fmt->type == VPDMA_DATA_FMT_TYPE_YUV &&
-			fmt->data_type == DATA_TYPE_C420)
+			fmt->data_type == DATA_TYPE_C420) {
+		rect.height >>= 1;
+		rect.top >>= 1;
 		depth = 8;
+	}
 
-	stride = ALIGN((depth * c_rect->width) >> 3, VPDMA_STRIDE_ALIGN);
-	dma_addr += (c_rect->left * depth) >> 3;
+	stride = ALIGN((depth * width) >> 3, VPDMA_STRIDE_ALIGN);
+
+	dma_addr += rect.top * stride + (rect.left * depth >> 3);
 
 	dtd = list->next;
 	WARN_ON((void *)(dtd + 1) > (list->buf.addr + list->buf.size));
@@ -664,31 +678,48 @@ void vpdma_add_out_dtd(struct vpdma_desc_list *list, struct v4l2_rect *c_rect,
 /*
  * append an inbound data transfer descriptor to the given descriptor list,
  * this sets up a 'memory to client' VPDMA transfer for the given VPDMA channel
+ *
+ * @list: vpdma desc list to which we add this decriptor
+ * @width: width of the image in pixels in memory(not the cropped width)
+ * @c_rect: crop params of input image
+ * @fmt: vpdma data format of the buffer
+ * dma_addr: dma address as seen by VPDMA
+ * chan: VPDMA channel
+ * field: top or bottom field info of the input image
+ * flags: VPDMA flags to configure some descriptor fileds
+ * frame_width/height: the complete width/height of the image presented to the
+ *			client (this makes sense when multiple channels are
+ *			connected to the same client, forming a larger frame)
+ * start_h, start_v: position where the given channel starts providing pixel
+ *			data to the client (makes sense when multiple channels
+ *			contribute to the client)
  */
-void vpdma_add_in_dtd(struct vpdma_desc_list *list, int frame_width,
-		int frame_height, struct v4l2_rect *c_rect,
+void vpdma_add_in_dtd(struct vpdma_desc_list *list, int width,
+		const struct v4l2_rect *c_rect,
 		const struct vpdma_data_format *fmt, dma_addr_t dma_addr,
-		enum vpdma_channel chan, int field, u32 flags)
+		enum vpdma_channel chan, int field, u32 flags, int frame_width,
+		int frame_height, int start_h, int start_v)
 {
 	int priority = 0;
 	int notify = 1;
 	int depth = fmt->depth;
 	int channel, next_chan;
+	struct v4l2_rect rect = *c_rect;
 	int stride;
-	int height = c_rect->height;
 	struct vpdma_dtd *dtd;
 
 	channel = next_chan = chan_info[chan].num;
 
 	if (fmt->type == VPDMA_DATA_FMT_TYPE_YUV &&
 			fmt->data_type == DATA_TYPE_C420) {
-		height >>= 1;
-		frame_height >>= 1;
+		rect.height >>= 1;
+		rect.top >>= 1;
 		depth = 8;
 	}
 
-	stride = ALIGN((depth * c_rect->width) >> 3, VPDMA_STRIDE_ALIGN);
-	dma_addr += (c_rect->left * depth) >> 3;
+	stride = ALIGN((depth * width) >> 3, VPDMA_STRIDE_ALIGN);
+
+	dma_addr += rect.top * stride + (rect.left * depth >> 3);
 
 	dtd = list->next;
 	WARN_ON((void *)(dtd + 1) > (list->buf.addr + list->buf.size));
@@ -701,13 +732,14 @@ void vpdma_add_in_dtd(struct vpdma_desc_list *list, int frame_width,
 					!!(flags & VPDMA_DATA_ODD_LINE_SKIP),
 					stride);
 
-	dtd->xfer_length_height = dtd_xfer_length_height(c_rect->width, height);
+	dtd->xfer_length_height = dtd_xfer_length_height(rect.width,
+					rect.height);
 	dtd->start_addr = (u32) dma_addr;
 	dtd->pkt_ctl = dtd_pkt_ctl(!!(flags & VPDMA_DATA_MODE_TILED),
 				DTD_DIR_IN, channel, priority, next_chan);
 	dtd->frame_width_height = dtd_frame_width_height(frame_width,
 					frame_height);
-	dtd->start_h_v = dtd_start_h_v(c_rect->left, c_rect->top);
+	dtd->start_h_v = dtd_start_h_v(start_h, start_v);
 	dtd->client_attr0 = 0;
 	dtd->client_attr1 = 0;
 
@@ -781,7 +813,7 @@ static void vpdma_firmware_cb(const struct firmware *f, void *context)
 	/* already initialized */
 	if (read_field_reg(vpdma, VPDMA_LIST_ATTR, VPDMA_LIST_RDY_MASK,
 			VPDMA_LIST_RDY_SHFT)) {
-		vpdma->ready = true;
+		vpdma->cb(vpdma->pdev);
 		return;
 	}
 
@@ -811,7 +843,7 @@ static void vpdma_firmware_cb(const struct firmware *f, void *context)
 		goto free_buf;
 	}
 
-	vpdma->ready = true;
+	vpdma->cb(vpdma->pdev);
 
 free_buf:
 	vpdma_unmap_desc_buf(vpdma, &fw_dma_buf);
@@ -839,7 +871,8 @@ static int vpdma_load_firmware(struct vpdma_data *vpdma)
 	return 0;
 }
 
-struct vpdma_data *vpdma_create(struct platform_device *pdev)
+struct vpdma_data *vpdma_create(struct platform_device *pdev,
+		void (*cb)(struct platform_device *pdev))
 {
 	struct resource *res;
 	struct vpdma_data *vpdma;
@@ -854,6 +887,7 @@ struct vpdma_data *vpdma_create(struct platform_device *pdev)
 	}
 
 	vpdma->pdev = pdev;
+	vpdma->cb = cb;
 
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "vpdma");
 	if (res == NULL) {
diff --git a/drivers/media/platform/ti-vpe/vpdma.h b/drivers/media/platform/ti-vpe/vpdma.h
index cf40f11b3c8..2bd8fb05038 100644
--- a/drivers/media/platform/ti-vpe/vpdma.h
+++ b/drivers/media/platform/ti-vpe/vpdma.h
@@ -35,8 +35,8 @@ struct vpdma_data {
 
 	struct platform_device	*pdev;
 
-	/* tells whether vpdma firmware is loaded or not */
-	bool ready;
+	/* callback to VPE driver when the firmware is loaded */
+	void (*cb)(struct platform_device *pdev);
 };
 
 enum vpdma_data_format_type {
@@ -186,13 +186,15 @@ void vpdma_add_cfd_adb(struct vpdma_desc_list *list, int client,
 		struct vpdma_buf *adb);
 void vpdma_add_sync_on_channel_ctd(struct vpdma_desc_list *list,
 		enum vpdma_channel chan);
-void vpdma_add_out_dtd(struct vpdma_desc_list *list, struct v4l2_rect *c_rect,
+void vpdma_add_out_dtd(struct vpdma_desc_list *list, int width,
+		const struct v4l2_rect *c_rect,
 		const struct vpdma_data_format *fmt, dma_addr_t dma_addr,
 		enum vpdma_channel chan, u32 flags);
-void vpdma_add_in_dtd(struct vpdma_desc_list *list, int frame_width,
-		int frame_height, struct v4l2_rect *c_rect,
+void vpdma_add_in_dtd(struct vpdma_desc_list *list, int width,
+		const struct v4l2_rect *c_rect,
 		const struct vpdma_data_format *fmt, dma_addr_t dma_addr,
-		enum vpdma_channel chan, int field, u32 flags);
+		enum vpdma_channel chan, int field, u32 flags, int frame_width,
+		int frame_height, int start_h, int start_v);
 
 /* vpdma list interrupt management */
 void vpdma_enable_list_complete_irq(struct vpdma_data *vpdma, int list_num,
@@ -208,6 +210,7 @@ void vpdma_set_frame_start_event(struct vpdma_data *vpdma,
 void vpdma_dump_regs(struct vpdma_data *vpdma);
 
 /* initialize vpdma, passed with VPE's platform device pointer */
-struct vpdma_data *vpdma_create(struct platform_device *pdev);
+struct vpdma_data *vpdma_create(struct platform_device *pdev,
+		void (*cb)(struct platform_device *pdev));
 
 #endif
diff --git a/drivers/media/platform/ti-vpe/vpe.c b/drivers/media/platform/ti-vpe/vpe.c
index 5c421886d97..972f43f6920 100644
--- a/drivers/media/platform/ti-vpe/vpe.c
+++ b/drivers/media/platform/ti-vpe/vpe.c
@@ -410,8 +410,10 @@ static struct vpe_q_data *get_q_data(struct vpe_ctx *ctx,
 {
 	switch (type) {
 	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 		return &ctx->q_data[Q_DATA_SRC];
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 		return &ctx->q_data[Q_DATA_DST];
 	default:
 		BUG();
@@ -986,7 +988,6 @@ static void add_out_dtd(struct vpe_ctx *ctx, int port)
 	struct vpe_q_data *q_data = &ctx->q_data[Q_DATA_DST];
 	const struct vpe_port_data *p_data = &port_data[port];
 	struct vb2_buffer *vb = ctx->dst_vb;
-	struct v4l2_rect *c_rect = &q_data->c_rect;
 	struct vpe_fmt *fmt = q_data->fmt;
 	const struct vpdma_data_format *vpdma_fmt;
 	int mv_buf_selector = !ctx->src_mv_buf_selector;
@@ -1015,8 +1016,8 @@ static void add_out_dtd(struct vpe_ctx *ctx, int port)
 	if (q_data->flags & Q_DATA_MODE_TILED)
 		flags |= VPDMA_DATA_MODE_TILED;
 
-	vpdma_add_out_dtd(&ctx->desc_list, c_rect, vpdma_fmt, dma_addr,
-		p_data->channel, flags);
+	vpdma_add_out_dtd(&ctx->desc_list, q_data->width, &q_data->c_rect,
+		vpdma_fmt, dma_addr, p_data->channel, flags);
 }
 
 static void add_in_dtd(struct vpe_ctx *ctx, int port)
@@ -1024,11 +1025,11 @@ static void add_in_dtd(struct vpe_ctx *ctx, int port)
 	struct vpe_q_data *q_data = &ctx->q_data[Q_DATA_SRC];
 	const struct vpe_port_data *p_data = &port_data[port];
 	struct vb2_buffer *vb = ctx->src_vbs[p_data->vb_index];
-	struct v4l2_rect *c_rect = &q_data->c_rect;
 	struct vpe_fmt *fmt = q_data->fmt;
 	const struct vpdma_data_format *vpdma_fmt;
 	int mv_buf_selector = ctx->src_mv_buf_selector;
 	int field = vb->v4l2_buf.field == V4L2_FIELD_BOTTOM;
+	int frame_width, frame_height;
 	dma_addr_t dma_addr;
 	u32 flags = 0;
 
@@ -1055,8 +1056,15 @@ static void add_in_dtd(struct vpe_ctx *ctx, int port)
 	if (q_data->flags & Q_DATA_MODE_TILED)
 		flags |= VPDMA_DATA_MODE_TILED;
 
-	vpdma_add_in_dtd(&ctx->desc_list, q_data->width, q_data->height,
-		c_rect, vpdma_fmt, dma_addr, p_data->channel, field, flags);
+	frame_width = q_data->c_rect.width;
+	frame_height = q_data->c_rect.height;
+
+	if (p_data->vb_part && fmt->fourcc == V4L2_PIX_FMT_NV12)
+		frame_height /= 2;
+
+	vpdma_add_in_dtd(&ctx->desc_list, q_data->width, &q_data->c_rect,
+		vpdma_fmt, dma_addr, p_data->channel, field, flags, frame_width,
+		frame_height, 0, 0);
 }
 
 /*
@@ -1585,6 +1593,151 @@ static int vpe_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
 	return set_srcdst_params(ctx);
 }
 
+static int __vpe_try_selection(struct vpe_ctx *ctx, struct v4l2_selection *s)
+{
+	struct vpe_q_data *q_data;
+
+	if ((s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) &&
+	    (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT))
+		return -EINVAL;
+
+	q_data = get_q_data(ctx, s->type);
+	if (!q_data)
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_COMPOSE:
+		/*
+		 * COMPOSE target is only valid for capture buffer type, return
+		 * error for output buffer type
+		 */
+		if (s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+			return -EINVAL;
+		break;
+	case V4L2_SEL_TGT_CROP:
+		/*
+		 * CROP target is only valid for output buffer type, return
+		 * error for capture buffer type
+		 */
+		if (s->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			return -EINVAL;
+		break;
+	/*
+	 * bound and default crop/compose targets are invalid targets to
+	 * try/set
+	 */
+	default:
+		return -EINVAL;
+	}
+
+	if (s->r.top < 0 || s->r.left < 0) {
+		vpe_err(ctx->dev, "negative values for top and left\n");
+		s->r.top = s->r.left = 0;
+	}
+
+	v4l_bound_align_image(&s->r.width, MIN_W, q_data->width, 1,
+		&s->r.height, MIN_H, q_data->height, H_ALIGN, S_ALIGN);
+
+	/* adjust left/top if cropping rectangle is out of bounds */
+	if (s->r.left + s->r.width > q_data->width)
+		s->r.left = q_data->width - s->r.width;
+	if (s->r.top + s->r.height > q_data->height)
+		s->r.top = q_data->height - s->r.height;
+
+	return 0;
+}
+
+static int vpe_g_selection(struct file *file, void *fh,
+		struct v4l2_selection *s)
+{
+	struct vpe_ctx *ctx = file2ctx(file);
+	struct vpe_q_data *q_data;
+	bool use_c_rect = false;
+
+	if ((s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) &&
+	    (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT))
+		return -EINVAL;
+
+	q_data = get_q_data(ctx, s->type);
+	if (!q_data)
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		if (s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+			return -EINVAL;
+		break;
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		if (s->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			return -EINVAL;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		if (s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+			return -EINVAL;
+		use_c_rect = true;
+		break;
+	case V4L2_SEL_TGT_CROP:
+		if (s->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			return -EINVAL;
+		use_c_rect = true;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (use_c_rect) {
+		/*
+		 * for CROP/COMPOSE target type, return c_rect params from the
+		 * respective buffer type
+		 */
+		s->r = q_data->c_rect;
+	} else {
+		/*
+		 * for DEFAULT/BOUNDS target type, return width and height from
+		 * S_FMT of the respective buffer type
+		 */
+		s->r.left = 0;
+		s->r.top = 0;
+		s->r.width = q_data->width;
+		s->r.height = q_data->height;
+	}
+
+	return 0;
+}
+
+
+static int vpe_s_selection(struct file *file, void *fh,
+		struct v4l2_selection *s)
+{
+	struct vpe_ctx *ctx = file2ctx(file);
+	struct vpe_q_data *q_data;
+	struct v4l2_selection sel = *s;
+	int ret;
+
+	ret = __vpe_try_selection(ctx, &sel);
+	if (ret)
+		return ret;
+
+	q_data = get_q_data(ctx, sel.type);
+	if (!q_data)
+		return -EINVAL;
+
+	if ((q_data->c_rect.left == sel.r.left) &&
+			(q_data->c_rect.top == sel.r.top) &&
+			(q_data->c_rect.width == sel.r.width) &&
+			(q_data->c_rect.height == sel.r.height)) {
+		vpe_dbg(ctx->dev,
+			"requested crop/compose values are already set\n");
+		return 0;
+	}
+
+	q_data->c_rect = sel.r;
+
+	return set_srcdst_params(ctx);
+}
+
 static int vpe_reqbufs(struct file *file, void *priv,
 		       struct v4l2_requestbuffers *reqbufs)
 {
@@ -1672,6 +1825,9 @@ static const struct v4l2_ioctl_ops vpe_ioctl_ops = {
 	.vidioc_try_fmt_vid_out_mplane	= vpe_try_fmt,
 	.vidioc_s_fmt_vid_out_mplane	= vpe_s_fmt,
 
+	.vidioc_g_selection		= vpe_g_selection,
+	.vidioc_s_selection		= vpe_s_selection,
+
 	.vidioc_reqbufs		= vpe_reqbufs,
 	.vidioc_querybuf	= vpe_querybuf,
 
@@ -1784,7 +1940,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 
 	memset(src_vq, 0, sizeof(*src_vq));
 	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
-	src_vq->io_modes = VB2_MMAP;
+	src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
 	src_vq->drv_priv = ctx;
 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	src_vq->ops = &vpe_qops;
@@ -1797,7 +1953,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 
 	memset(dst_vq, 0, sizeof(*dst_vq));
 	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
-	dst_vq->io_modes = VB2_MMAP;
+	dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
 	dst_vq->drv_priv = ctx;
 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	dst_vq->ops = &vpe_qops;
@@ -1831,11 +1987,6 @@ static int vpe_open(struct file *file)
 
 	vpe_dbg(dev, "vpe_open\n");
 
-	if (!dev->vpdma->ready) {
-		vpe_err(dev, "vpdma firmware not loaded\n");
-		return -ENODEV;
-	}
-
 	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
 	if (!ctx)
 		return -ENOMEM;
@@ -2055,10 +2206,40 @@ static void vpe_runtime_put(struct platform_device *pdev)
 	WARN_ON(r < 0 && r != -ENOSYS);
 }
 
+static void vpe_fw_cb(struct platform_device *pdev)
+{
+	struct vpe_dev *dev = platform_get_drvdata(pdev);
+	struct video_device *vfd;
+	int ret;
+
+	vfd = &dev->vfd;
+	*vfd = vpe_videodev;
+	vfd->lock = &dev->dev_mutex;
+	vfd->v4l2_dev = &dev->v4l2_dev;
+
+	ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
+	if (ret) {
+		vpe_err(dev, "Failed to register video device\n");
+
+		vpe_set_clock_enable(dev, 0);
+		vpe_runtime_put(pdev);
+		pm_runtime_disable(&pdev->dev);
+		v4l2_m2m_release(dev->m2m_dev);
+		vb2_dma_contig_cleanup_ctx(dev->alloc_ctx);
+		v4l2_device_unregister(&dev->v4l2_dev);
+
+		return;
+	}
+
+	video_set_drvdata(vfd, dev);
+	snprintf(vfd->name, sizeof(vfd->name), "%s", vpe_videodev.name);
+	dev_info(dev->v4l2_dev.dev, "Device registered as /dev/video%d\n",
+		vfd->num);
+}
+
 static int vpe_probe(struct platform_device *pdev)
 {
 	struct vpe_dev *dev;
-	struct video_device *vfd;
 	int ret, irq, func;
 
 	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
@@ -2139,28 +2320,12 @@ static int vpe_probe(struct platform_device *pdev)
 		goto runtime_put;
 	}
 
-	dev->vpdma = vpdma_create(pdev);
+	dev->vpdma = vpdma_create(pdev, vpe_fw_cb);
 	if (IS_ERR(dev->vpdma)) {
 		ret = PTR_ERR(dev->vpdma);
 		goto runtime_put;
 	}
 
-	vfd = &dev->vfd;
-	*vfd = vpe_videodev;
-	vfd->lock = &dev->dev_mutex;
-	vfd->v4l2_dev = &dev->v4l2_dev;
-
-	ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
-	if (ret) {
-		vpe_err(dev, "Failed to register video device\n");
-		goto runtime_put;
-	}
-
-	video_set_drvdata(vfd, dev);
-	snprintf(vfd->name, sizeof(vfd->name), "%s", vpe_videodev.name);
-	dev_info(dev->v4l2_dev.dev, "Device registered as /dev/video%d\n",
-		vfd->num);
-
 	return 0;
 
 runtime_put:
diff --git a/drivers/media/platform/timblogiw.c b/drivers/media/platform/timblogiw.c
index ccdadd623a3..3cb2f356487 100644
--- a/drivers/media/platform/timblogiw.c
+++ b/drivers/media/platform/timblogiw.c
@@ -347,7 +347,7 @@ static int timblogiw_s_std(struct file *file, void  *priv, v4l2_std_id std)
 	mutex_lock(&lw->lock);
 
 	if (TIMBLOGIW_HAS_DECODER(lw))
-		err = v4l2_subdev_call(lw->sd_enc, core, s_std, std);
+		err = v4l2_subdev_call(lw->sd_enc, video, s_std, std);
 
 	if (!err)
 		fh->cur_norm = timblogiw_get_norm(std);
@@ -800,7 +800,7 @@ static int timblogiw_probe(struct platform_device *pdev)
 	if (!pdata->encoder.module_name)
 		dev_info(&pdev->dev, "Running without decoder\n");
 
-	lw = kzalloc(sizeof(*lw), GFP_KERNEL);
+	lw = devm_kzalloc(&pdev->dev, sizeof(*lw), GFP_KERNEL);
 	if (!lw) {
 		err = -ENOMEM;
 		goto err;
@@ -820,7 +820,7 @@ static int timblogiw_probe(struct platform_device *pdev)
 	strlcpy(lw->v4l2_dev.name, DRIVER_NAME, sizeof(lw->v4l2_dev.name));
 	err = v4l2_device_register(NULL, &lw->v4l2_dev);
 	if (err)
-		goto err_register;
+		goto err;
 
 	lw->video_dev.v4l2_dev = &lw->v4l2_dev;
 
@@ -837,8 +837,6 @@ static int timblogiw_probe(struct platform_device *pdev)
 
 err_request:
 	v4l2_device_unregister(&lw->v4l2_dev);
-err_register:
-	kfree(lw);
 err:
 	dev_err(&pdev->dev, "Failed to register: %d\n", err);
 
@@ -853,8 +851,6 @@ static int timblogiw_remove(struct platform_device *pdev)
 
 	v4l2_device_unregister(&lw->v4l2_dev);
 
-	kfree(lw);
-
 	return 0;
 }
 
diff --git a/drivers/media/platform/vino.c b/drivers/media/platform/vino.c
index c6af974c5b4..470d3533611 100644
--- a/drivers/media/platform/vino.c
+++ b/drivers/media/platform/vino.c
@@ -2586,7 +2586,7 @@ static int vino_acquire_input(struct vino_channel_settings *vcs)
 			}
 			if (data_norm == 3)
 				data_norm = VINO_DATA_NORM_PAL;
-			ret = decoder_call(core, s_std, norm);
+			ret = decoder_call(video, s_std, norm);
 		}
 
 		spin_lock_irqsave(&vino_drvdata->input_lock, flags);
@@ -2675,7 +2675,7 @@ static int vino_set_input(struct vino_channel_settings *vcs, int input)
 				}
 				if (data_norm == 3)
 					data_norm = VINO_DATA_NORM_PAL;
-				ret = decoder_call(core, s_std, norm);
+				ret = decoder_call(video, s_std, norm);
 			}
 
 			spin_lock_irqsave(&vino_drvdata->input_lock, flags);
@@ -2809,7 +2809,7 @@ static int vino_set_data_norm(struct vino_channel_settings *vcs,
 		 * as it may take a while... */
 
 		norm = vino_data_norms[data_norm].std;
-		err = decoder_call(core, s_std, norm);
+		err = decoder_call(video, s_std, norm);
 
 		spin_lock_irqsave(&vino_drvdata->input_lock, *flags);
 
diff --git a/drivers/media/platform/vivi.c b/drivers/media/platform/vivi.c
index 3890f4f42a7..d00bf3df0f8 100644
--- a/drivers/media/platform/vivi.c
+++ b/drivers/media/platform/vivi.c
@@ -906,12 +906,11 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 }
 
 /* abort streaming and wait for last buffer */
-static int stop_streaming(struct vb2_queue *vq)
+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);
-	return 0;
 }
 
 static void vivi_lock(struct vb2_queue *vq)
diff --git a/drivers/media/platform/vsp1/Makefile b/drivers/media/platform/vsp1/Makefile
index 151cecd0ea2..6a93f928dfd 100644
--- a/drivers/media/platform/vsp1/Makefile
+++ b/drivers/media/platform/vsp1/Makefile
@@ -1,6 +1,6 @@
 vsp1-y					:= vsp1_drv.o vsp1_entity.o vsp1_video.o
 vsp1-y					+= vsp1_rpf.o vsp1_rwpf.o vsp1_wpf.o
 vsp1-y					+= vsp1_hsit.o vsp1_lif.o vsp1_lut.o
-vsp1-y					+= vsp1_sru.o vsp1_uds.o
+vsp1-y					+= vsp1_bru.o vsp1_sru.o vsp1_uds.o
 
 obj-$(CONFIG_VIDEO_RENESAS_VSP1)	+= vsp1.o
diff --git a/drivers/media/platform/vsp1/vsp1.h b/drivers/media/platform/vsp1/vsp1.h
index 0313210c6e9..6ca2cf20d54 100644
--- a/drivers/media/platform/vsp1/vsp1.h
+++ b/drivers/media/platform/vsp1/vsp1.h
@@ -28,6 +28,7 @@ struct clk;
 struct device;
 
 struct vsp1_platform_data;
+struct vsp1_bru;
 struct vsp1_hsit;
 struct vsp1_lif;
 struct vsp1_lut;
@@ -45,11 +46,11 @@ struct vsp1_device {
 
 	void __iomem *mmio;
 	struct clk *clock;
-	struct clk *rt_clock;
 
 	struct mutex lock;
 	int ref_count;
 
+	struct vsp1_bru *bru;
 	struct vsp1_hsit *hsi;
 	struct vsp1_hsit *hst;
 	struct vsp1_lif *lif;
diff --git a/drivers/media/platform/vsp1/vsp1_bru.c b/drivers/media/platform/vsp1/vsp1_bru.c
new file mode 100644
index 00000000000..f8069548006
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_bru.c
@@ -0,0 +1,395 @@
+/*
+ * vsp1_bru.c  --  R-Car VSP1 Blend ROP Unit
+ *
+ * Copyright (C) 2013 Renesas Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.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/device.h>
+#include <linux/gfp.h>
+
+#include <media/v4l2-subdev.h>
+
+#include "vsp1.h"
+#include "vsp1_bru.h"
+
+#define BRU_MIN_SIZE				4U
+#define BRU_MAX_SIZE				8190U
+
+/* -----------------------------------------------------------------------------
+ * Device Access
+ */
+
+static inline u32 vsp1_bru_read(struct vsp1_bru *bru, u32 reg)
+{
+	return vsp1_read(bru->entity.vsp1, reg);
+}
+
+static inline void vsp1_bru_write(struct vsp1_bru *bru, u32 reg, u32 data)
+{
+	vsp1_write(bru->entity.vsp1, reg, data);
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Core Operations
+ */
+
+static bool bru_is_input_enabled(struct vsp1_bru *bru, unsigned int input)
+{
+	return media_entity_remote_pad(&bru->entity.pads[input]) != NULL;
+}
+
+static int bru_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct vsp1_bru *bru = to_bru(subdev);
+	struct v4l2_mbus_framefmt *format;
+	unsigned int i;
+
+	if (!enable)
+		return 0;
+
+	format = &bru->entity.formats[BRU_PAD_SOURCE];
+
+	/* The hardware is extremely flexible but we have no userspace API to
+	 * expose all the parameters, nor is it clear whether we would have use
+	 * cases for all the supported modes. Let's just harcode the parameters
+	 * to sane default values for now.
+	 */
+
+	/* Disable both color data normalization and dithering. */
+	vsp1_bru_write(bru, VI6_BRU_INCTRL, 0);
+
+	/* Set the background position to cover the whole output image and
+	 * set its color to opaque black.
+	 */
+	vsp1_bru_write(bru, VI6_BRU_VIRRPF_SIZE,
+		       (format->width << VI6_BRU_VIRRPF_SIZE_HSIZE_SHIFT) |
+		       (format->height << VI6_BRU_VIRRPF_SIZE_VSIZE_SHIFT));
+	vsp1_bru_write(bru, VI6_BRU_VIRRPF_LOC, 0);
+	vsp1_bru_write(bru, VI6_BRU_VIRRPF_COL,
+		       0xff << VI6_BRU_VIRRPF_COL_A_SHIFT);
+
+	/* Route BRU input 1 as SRC input to the ROP unit and configure the ROP
+	 * unit with a NOP operation to make BRU input 1 available as the
+	 * Blend/ROP unit B SRC input.
+	 */
+	vsp1_bru_write(bru, VI6_BRU_ROP, VI6_BRU_ROP_DSTSEL_BRUIN(1) |
+		       VI6_BRU_ROP_CROP(VI6_ROP_NOP) |
+		       VI6_BRU_ROP_AROP(VI6_ROP_NOP));
+
+	for (i = 0; i < 4; ++i) {
+		u32 ctrl = 0;
+
+		/* Configure all Blend/ROP units corresponding to an enabled BRU
+		 * input for alpha blending. Blend/ROP units corresponding to
+		 * disabled BRU inputs are used in ROP NOP mode to ignore the
+		 * SRC input.
+		 */
+		if (bru_is_input_enabled(bru, i))
+			ctrl |= VI6_BRU_CTRL_RBC;
+		else
+			ctrl |= VI6_BRU_CTRL_CROP(VI6_ROP_NOP)
+			     |  VI6_BRU_CTRL_AROP(VI6_ROP_NOP);
+
+		/* Select the virtual RPF as the Blend/ROP unit A DST input to
+		 * serve as a background color.
+		 */
+		if (i == 0)
+			ctrl |= VI6_BRU_CTRL_DSTSEL_VRPF;
+
+		/* Route BRU inputs 0 to 3 as SRC inputs to Blend/ROP units A to
+		 * D in that order. The Blend/ROP unit B SRC is hardwired to the
+		 * ROP unit output, the corresponding register bits must be set
+		 * to 0.
+		 */
+		if (i != 1)
+			ctrl |= VI6_BRU_CTRL_SRCSEL_BRUIN(i);
+
+		vsp1_bru_write(bru, VI6_BRU_CTRL(i), ctrl);
+
+		/* Harcode the blending formula to
+		 *
+		 *	DSTc = DSTc * (1 - SRCa) + SRCc * SRCa
+		 *	DSTa = DSTa * (1 - SRCa) + SRCa
+		 */
+		vsp1_bru_write(bru, VI6_BRU_BLD(i),
+			       VI6_BRU_BLD_CCMDX_255_SRC_A |
+			       VI6_BRU_BLD_CCMDY_SRC_A |
+			       VI6_BRU_BLD_ACMDX_255_SRC_A |
+			       VI6_BRU_BLD_ACMDY_COEFY |
+			       (0xff << VI6_BRU_BLD_COEFY_SHIFT));
+	}
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Pad Operations
+ */
+
+/*
+ * The BRU can't perform format conversion, all sink and source formats must be
+ * identical. We pick the format on the first sink pad (pad 0) and propagate it
+ * to all other pads.
+ */
+
+static int bru_enum_mbus_code(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_fh *fh,
+			      struct v4l2_subdev_mbus_code_enum *code)
+{
+	static const unsigned int codes[] = {
+		V4L2_MBUS_FMT_ARGB8888_1X32,
+		V4L2_MBUS_FMT_AYUV8_1X32,
+	};
+	struct v4l2_mbus_framefmt *format;
+
+	if (code->pad == BRU_PAD_SINK(0)) {
+		if (code->index >= ARRAY_SIZE(codes))
+			return -EINVAL;
+
+		code->code = codes[code->index];
+	} else {
+		if (code->index)
+			return -EINVAL;
+
+		format = v4l2_subdev_get_try_format(fh, BRU_PAD_SINK(0));
+		code->code = format->code;
+	}
+
+	return 0;
+}
+
+static int bru_enum_frame_size(struct v4l2_subdev *subdev,
+			       struct v4l2_subdev_fh *fh,
+			       struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index)
+		return -EINVAL;
+
+	if (fse->code != V4L2_MBUS_FMT_ARGB8888_1X32 &&
+	    fse->code != V4L2_MBUS_FMT_AYUV8_1X32)
+		return -EINVAL;
+
+	fse->min_width = BRU_MIN_SIZE;
+	fse->max_width = BRU_MAX_SIZE;
+	fse->min_height = BRU_MIN_SIZE;
+	fse->max_height = BRU_MAX_SIZE;
+
+	return 0;
+}
+
+static struct v4l2_rect *bru_get_compose(struct vsp1_bru *bru,
+					 struct v4l2_subdev_fh *fh,
+					 unsigned int pad, u32 which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_crop(fh, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &bru->compose[pad];
+	default:
+		return NULL;
+	}
+}
+
+static int bru_get_format(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct vsp1_bru *bru = to_bru(subdev);
+
+	fmt->format = *vsp1_entity_get_pad_format(&bru->entity, fh, fmt->pad,
+						  fmt->which);
+
+	return 0;
+}
+
+static void bru_try_format(struct vsp1_bru *bru, struct v4l2_subdev_fh *fh,
+			   unsigned int pad, struct v4l2_mbus_framefmt *fmt,
+			   enum v4l2_subdev_format_whence which)
+{
+	struct v4l2_mbus_framefmt *format;
+
+	switch (pad) {
+	case BRU_PAD_SINK(0):
+		/* Default to YUV if the requested format is not supported. */
+		if (fmt->code != V4L2_MBUS_FMT_ARGB8888_1X32 &&
+		    fmt->code != V4L2_MBUS_FMT_AYUV8_1X32)
+			fmt->code = V4L2_MBUS_FMT_AYUV8_1X32;
+		break;
+
+	default:
+		/* The BRU can't perform format conversion. */
+		format = vsp1_entity_get_pad_format(&bru->entity, fh,
+						    BRU_PAD_SINK(0), which);
+		fmt->code = format->code;
+		break;
+	}
+
+	fmt->width = clamp(fmt->width, BRU_MIN_SIZE, BRU_MAX_SIZE);
+	fmt->height = clamp(fmt->height, BRU_MIN_SIZE, BRU_MAX_SIZE);
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+}
+
+static int bru_set_format(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct vsp1_bru *bru = to_bru(subdev);
+	struct v4l2_mbus_framefmt *format;
+
+	bru_try_format(bru, fh, fmt->pad, &fmt->format, fmt->which);
+
+	format = vsp1_entity_get_pad_format(&bru->entity, fh, fmt->pad,
+					    fmt->which);
+	*format = fmt->format;
+
+	/* Reset the compose rectangle */
+	if (fmt->pad != BRU_PAD_SOURCE) {
+		struct v4l2_rect *compose;
+
+		compose = bru_get_compose(bru, fh, fmt->pad, fmt->which);
+		compose->left = 0;
+		compose->top = 0;
+		compose->width = format->width;
+		compose->height = format->height;
+	}
+
+	/* Propagate the format code to all pads */
+	if (fmt->pad == BRU_PAD_SINK(0)) {
+		unsigned int i;
+
+		for (i = 0; i <= BRU_PAD_SOURCE; ++i) {
+			format = vsp1_entity_get_pad_format(&bru->entity, fh,
+							    i, fmt->which);
+			format->code = fmt->format.code;
+		}
+	}
+
+	return 0;
+}
+
+static int bru_get_selection(struct v4l2_subdev *subdev,
+			     struct v4l2_subdev_fh *fh,
+			     struct v4l2_subdev_selection *sel)
+{
+	struct vsp1_bru *bru = to_bru(subdev);
+
+	if (sel->pad == BRU_PAD_SOURCE)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = BRU_MAX_SIZE;
+		sel->r.height = BRU_MAX_SIZE;
+		return 0;
+
+	case V4L2_SEL_TGT_COMPOSE:
+		sel->r = *bru_get_compose(bru, fh, sel->pad, sel->which);
+		return 0;
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static int bru_set_selection(struct v4l2_subdev *subdev,
+			     struct v4l2_subdev_fh *fh,
+			     struct v4l2_subdev_selection *sel)
+{
+	struct vsp1_bru *bru = to_bru(subdev);
+	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect *compose;
+
+	if (sel->pad == BRU_PAD_SOURCE)
+		return -EINVAL;
+
+	if (sel->target != V4L2_SEL_TGT_COMPOSE)
+		return -EINVAL;
+
+	/* The compose rectangle top left corner must be inside the output
+	 * frame.
+	 */
+	format = vsp1_entity_get_pad_format(&bru->entity, fh, BRU_PAD_SOURCE,
+					    sel->which);
+	sel->r.left = clamp_t(unsigned int, sel->r.left, 0, format->width - 1);
+	sel->r.top = clamp_t(unsigned int, sel->r.top, 0, format->height - 1);
+
+	/* Scaling isn't supported, the compose rectangle size must be identical
+	 * to the sink format size.
+	 */
+	format = vsp1_entity_get_pad_format(&bru->entity, fh, sel->pad,
+					    sel->which);
+	sel->r.width = format->width;
+	sel->r.height = format->height;
+
+	compose = bru_get_compose(bru, fh, sel->pad, sel->which);
+	*compose = sel->r;
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Operations
+ */
+
+static struct v4l2_subdev_video_ops bru_video_ops = {
+	.s_stream = bru_s_stream,
+};
+
+static struct v4l2_subdev_pad_ops bru_pad_ops = {
+	.enum_mbus_code = bru_enum_mbus_code,
+	.enum_frame_size = bru_enum_frame_size,
+	.get_fmt = bru_get_format,
+	.set_fmt = bru_set_format,
+	.get_selection = bru_get_selection,
+	.set_selection = bru_set_selection,
+};
+
+static struct v4l2_subdev_ops bru_ops = {
+	.video	= &bru_video_ops,
+	.pad    = &bru_pad_ops,
+};
+
+/* -----------------------------------------------------------------------------
+ * Initialization and Cleanup
+ */
+
+struct vsp1_bru *vsp1_bru_create(struct vsp1_device *vsp1)
+{
+	struct v4l2_subdev *subdev;
+	struct vsp1_bru *bru;
+	int ret;
+
+	bru = devm_kzalloc(vsp1->dev, sizeof(*bru), GFP_KERNEL);
+	if (bru == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	bru->entity.type = VSP1_ENTITY_BRU;
+
+	ret = vsp1_entity_init(vsp1, &bru->entity, 5);
+	if (ret < 0)
+		return ERR_PTR(ret);
+
+	/* Initialize the V4L2 subdev. */
+	subdev = &bru->entity.subdev;
+	v4l2_subdev_init(subdev, &bru_ops);
+
+	subdev->entity.ops = &vsp1_media_ops;
+	subdev->internal_ops = &vsp1_subdev_internal_ops;
+	snprintf(subdev->name, sizeof(subdev->name), "%s bru",
+		 dev_name(vsp1->dev));
+	v4l2_set_subdevdata(subdev, bru);
+	subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	vsp1_entity_init_formats(subdev, NULL);
+
+	return bru;
+}
diff --git a/drivers/media/platform/vsp1/vsp1_bru.h b/drivers/media/platform/vsp1/vsp1_bru.h
new file mode 100644
index 00000000000..37062704dbf
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_bru.h
@@ -0,0 +1,39 @@
+/*
+ * vsp1_bru.h  --  R-Car VSP1 Blend ROP Unit
+ *
+ * Copyright (C) 2013 Renesas Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.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.
+ */
+#ifndef __VSP1_BRU_H__
+#define __VSP1_BRU_H__
+
+#include <media/media-entity.h>
+#include <media/v4l2-subdev.h>
+
+#include "vsp1_entity.h"
+
+struct vsp1_device;
+
+#define BRU_PAD_SINK(n)				(n)
+#define BRU_PAD_SOURCE				4
+
+struct vsp1_bru {
+	struct vsp1_entity entity;
+
+	struct v4l2_rect compose[4];
+};
+
+static inline struct vsp1_bru *to_bru(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct vsp1_bru, entity.subdev);
+}
+
+struct vsp1_bru *vsp1_bru_create(struct vsp1_device *vsp1);
+
+#endif /* __VSP1_BRU_H__ */
diff --git a/drivers/media/platform/vsp1/vsp1_drv.c b/drivers/media/platform/vsp1/vsp1_drv.c
index 2f74f0e0ddf..c69ee0657f7 100644
--- a/drivers/media/platform/vsp1/vsp1_drv.c
+++ b/drivers/media/platform/vsp1/vsp1_drv.c
@@ -16,10 +16,12 @@
 #include <linux/device.h>
 #include <linux/interrupt.h>
 #include <linux/module.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/videodev2.h>
 
 #include "vsp1.h"
+#include "vsp1_bru.h"
 #include "vsp1_hsit.h"
 #include "vsp1_lif.h"
 #include "vsp1_lut.h"
@@ -155,6 +157,14 @@ static int vsp1_create_entities(struct vsp1_device *vsp1)
 	}
 
 	/* Instantiate all the entities. */
+	vsp1->bru = vsp1_bru_create(vsp1);
+	if (IS_ERR(vsp1->bru)) {
+		ret = PTR_ERR(vsp1->bru);
+		goto done;
+	}
+
+	list_add_tail(&vsp1->bru->entity.list_dev, &vsp1->entities);
+
 	vsp1->hsi = vsp1_hsit_create(vsp1, true);
 	if (IS_ERR(vsp1->hsi)) {
 		ret = PTR_ERR(vsp1->hsi);
@@ -329,33 +339,6 @@ static int vsp1_device_init(struct vsp1_device *vsp1)
 	return 0;
 }
 
-static int vsp1_clocks_enable(struct vsp1_device *vsp1)
-{
-	int ret;
-
-	ret = clk_prepare_enable(vsp1->clock);
-	if (ret < 0)
-		return ret;
-
-	if (IS_ERR(vsp1->rt_clock))
-		return 0;
-
-	ret = clk_prepare_enable(vsp1->rt_clock);
-	if (ret < 0) {
-		clk_disable_unprepare(vsp1->clock);
-		return ret;
-	}
-
-	return 0;
-}
-
-static void vsp1_clocks_disable(struct vsp1_device *vsp1)
-{
-	if (!IS_ERR(vsp1->rt_clock))
-		clk_disable_unprepare(vsp1->rt_clock);
-	clk_disable_unprepare(vsp1->clock);
-}
-
 /*
  * vsp1_device_get - Acquire the VSP1 device
  *
@@ -373,7 +356,7 @@ struct vsp1_device *vsp1_device_get(struct vsp1_device *vsp1)
 	if (vsp1->ref_count > 0)
 		goto done;
 
-	ret = vsp1_clocks_enable(vsp1);
+	ret = clk_prepare_enable(vsp1->clock);
 	if (ret < 0) {
 		__vsp1 = NULL;
 		goto done;
@@ -381,7 +364,7 @@ struct vsp1_device *vsp1_device_get(struct vsp1_device *vsp1)
 
 	ret = vsp1_device_init(vsp1);
 	if (ret < 0) {
-		vsp1_clocks_disable(vsp1);
+		clk_disable_unprepare(vsp1->clock);
 		__vsp1 = NULL;
 		goto done;
 	}
@@ -405,7 +388,7 @@ void vsp1_device_put(struct vsp1_device *vsp1)
 	mutex_lock(&vsp1->lock);
 
 	if (--vsp1->ref_count == 0)
-		vsp1_clocks_disable(vsp1);
+		clk_disable_unprepare(vsp1->clock);
 
 	mutex_unlock(&vsp1->lock);
 }
@@ -424,7 +407,7 @@ static int vsp1_pm_suspend(struct device *dev)
 	if (vsp1->ref_count == 0)
 		return 0;
 
-	vsp1_clocks_disable(vsp1);
+	clk_disable_unprepare(vsp1->clock);
 	return 0;
 }
 
@@ -437,7 +420,7 @@ static int vsp1_pm_resume(struct device *dev)
 	if (vsp1->ref_count)
 		return 0;
 
-	return vsp1_clocks_enable(vsp1);
+	return clk_prepare_enable(vsp1->clock);
 }
 #endif
 
@@ -449,34 +432,59 @@ static const struct dev_pm_ops vsp1_pm_ops = {
  * Platform Driver
  */
 
-static struct vsp1_platform_data *
-vsp1_get_platform_data(struct platform_device *pdev)
+static int vsp1_validate_platform_data(struct platform_device *pdev,
+				       struct vsp1_platform_data *pdata)
 {
-	struct vsp1_platform_data *pdata = pdev->dev.platform_data;
-
 	if (pdata == NULL) {
 		dev_err(&pdev->dev, "missing platform data\n");
-		return NULL;
+		return -EINVAL;
 	}
 
 	if (pdata->rpf_count <= 0 || pdata->rpf_count > VPS1_MAX_RPF) {
 		dev_err(&pdev->dev, "invalid number of RPF (%u)\n",
 			pdata->rpf_count);
-		return NULL;
+		return -EINVAL;
 	}
 
 	if (pdata->uds_count <= 0 || pdata->uds_count > VPS1_MAX_UDS) {
 		dev_err(&pdev->dev, "invalid number of UDS (%u)\n",
 			pdata->uds_count);
-		return NULL;
+		return -EINVAL;
 	}
 
 	if (pdata->wpf_count <= 0 || pdata->wpf_count > VPS1_MAX_WPF) {
 		dev_err(&pdev->dev, "invalid number of WPF (%u)\n",
 			pdata->wpf_count);
-		return NULL;
+		return -EINVAL;
 	}
 
+	return 0;
+}
+
+static struct vsp1_platform_data *
+vsp1_get_platform_data(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct vsp1_platform_data *pdata;
+
+	if (!IS_ENABLED(CONFIG_OF) || np == NULL)
+		return pdev->dev.platform_data;
+
+	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+	if (pdata == NULL)
+		return NULL;
+
+	if (of_property_read_bool(np, "renesas,has-lif"))
+		pdata->features |= VSP1_HAS_LIF;
+	if (of_property_read_bool(np, "renesas,has-lut"))
+		pdata->features |= VSP1_HAS_LUT;
+	if (of_property_read_bool(np, "renesas,has-sru"))
+		pdata->features |= VSP1_HAS_SRU;
+
+	of_property_read_u32(np, "renesas,#rpf", &pdata->rpf_count);
+	of_property_read_u32(np, "renesas,#uds", &pdata->uds_count);
+	of_property_read_u32(np, "renesas,#wpf", &pdata->wpf_count);
+
 	return pdata;
 }
 
@@ -499,6 +507,10 @@ static int vsp1_probe(struct platform_device *pdev)
 	if (vsp1->pdata == NULL)
 		return -ENODEV;
 
+	ret = vsp1_validate_platform_data(pdev, vsp1->pdata);
+	if (ret < 0)
+		return ret;
+
 	/* I/O, IRQ and clock resources */
 	io = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	vsp1->mmio = devm_ioremap_resource(&pdev->dev, io);
@@ -511,9 +523,6 @@ static int vsp1_probe(struct platform_device *pdev)
 		return PTR_ERR(vsp1->clock);
 	}
 
-	/* The RT clock is optional */
-	vsp1->rt_clock = devm_clk_get(&pdev->dev, "rt");
-
 	irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (!irq) {
 		dev_err(&pdev->dev, "missing IRQ\n");
@@ -548,6 +557,11 @@ static int vsp1_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static const struct of_device_id vsp1_of_match[] = {
+	{ .compatible = "renesas,vsp1" },
+	{ },
+};
+
 static struct platform_driver vsp1_platform_driver = {
 	.probe		= vsp1_probe,
 	.remove		= vsp1_remove,
@@ -555,6 +569,7 @@ static struct platform_driver vsp1_platform_driver = {
 		.owner	= THIS_MODULE,
 		.name	= "vsp1",
 		.pm	= &vsp1_pm_ops,
+		.of_match_table = vsp1_of_match,
 	},
 };
 
diff --git a/drivers/media/platform/vsp1/vsp1_entity.c b/drivers/media/platform/vsp1/vsp1_entity.c
index 3fc9e4266ca..44167834285 100644
--- a/drivers/media/platform/vsp1/vsp1_entity.c
+++ b/drivers/media/platform/vsp1/vsp1_entity.c
@@ -100,8 +100,10 @@ static int vsp1_entity_link_setup(struct media_entity *entity,
 		if (source->sink)
 			return -EBUSY;
 		source->sink = remote->entity;
+		source->sink_pad = remote->index;
 	} else {
 		source->sink = NULL;
+		source->sink_pad = 0;
 	}
 
 	return 0;
@@ -116,42 +118,43 @@ const struct media_entity_operations vsp1_media_ops = {
  * Initialization
  */
 
+static const struct vsp1_route vsp1_routes[] = {
+	{ VSP1_ENTITY_BRU, 0, VI6_DPR_BRU_ROUTE,
+	  { VI6_DPR_NODE_BRU_IN(0), VI6_DPR_NODE_BRU_IN(1),
+	    VI6_DPR_NODE_BRU_IN(2), VI6_DPR_NODE_BRU_IN(3), } },
+	{ VSP1_ENTITY_HSI, 0, VI6_DPR_HSI_ROUTE, { VI6_DPR_NODE_HSI, } },
+	{ VSP1_ENTITY_HST, 0, VI6_DPR_HST_ROUTE, { VI6_DPR_NODE_HST, } },
+	{ VSP1_ENTITY_LIF, 0, 0, { VI6_DPR_NODE_LIF, } },
+	{ VSP1_ENTITY_LUT, 0, VI6_DPR_LUT_ROUTE, { VI6_DPR_NODE_LUT, } },
+	{ VSP1_ENTITY_RPF, 0, VI6_DPR_RPF_ROUTE(0), { VI6_DPR_NODE_RPF(0), } },
+	{ VSP1_ENTITY_RPF, 1, VI6_DPR_RPF_ROUTE(1), { VI6_DPR_NODE_RPF(1), } },
+	{ VSP1_ENTITY_RPF, 2, VI6_DPR_RPF_ROUTE(2), { VI6_DPR_NODE_RPF(2), } },
+	{ VSP1_ENTITY_RPF, 3, VI6_DPR_RPF_ROUTE(3), { VI6_DPR_NODE_RPF(3), } },
+	{ VSP1_ENTITY_RPF, 4, VI6_DPR_RPF_ROUTE(4), { VI6_DPR_NODE_RPF(4), } },
+	{ VSP1_ENTITY_SRU, 0, VI6_DPR_SRU_ROUTE, { VI6_DPR_NODE_SRU, } },
+	{ VSP1_ENTITY_UDS, 0, VI6_DPR_UDS_ROUTE(0), { VI6_DPR_NODE_UDS(0), } },
+	{ VSP1_ENTITY_UDS, 1, VI6_DPR_UDS_ROUTE(1), { VI6_DPR_NODE_UDS(1), } },
+	{ VSP1_ENTITY_UDS, 2, VI6_DPR_UDS_ROUTE(2), { VI6_DPR_NODE_UDS(2), } },
+	{ VSP1_ENTITY_WPF, 0, 0, { VI6_DPR_NODE_WPF(0), } },
+	{ VSP1_ENTITY_WPF, 1, 0, { VI6_DPR_NODE_WPF(1), } },
+	{ VSP1_ENTITY_WPF, 2, 0, { VI6_DPR_NODE_WPF(2), } },
+	{ VSP1_ENTITY_WPF, 3, 0, { VI6_DPR_NODE_WPF(3), } },
+};
+
 int vsp1_entity_init(struct vsp1_device *vsp1, struct vsp1_entity *entity,
 		     unsigned int num_pads)
 {
-	static const struct {
-		unsigned int id;
-		unsigned int reg;
-	} routes[] = {
-		{ VI6_DPR_NODE_HSI, VI6_DPR_HSI_ROUTE },
-		{ VI6_DPR_NODE_HST, VI6_DPR_HST_ROUTE },
-		{ VI6_DPR_NODE_LIF, 0 },
-		{ VI6_DPR_NODE_LUT, VI6_DPR_LUT_ROUTE },
-		{ VI6_DPR_NODE_RPF(0), VI6_DPR_RPF_ROUTE(0) },
-		{ VI6_DPR_NODE_RPF(1), VI6_DPR_RPF_ROUTE(1) },
-		{ VI6_DPR_NODE_RPF(2), VI6_DPR_RPF_ROUTE(2) },
-		{ VI6_DPR_NODE_RPF(3), VI6_DPR_RPF_ROUTE(3) },
-		{ VI6_DPR_NODE_RPF(4), VI6_DPR_RPF_ROUTE(4) },
-		{ VI6_DPR_NODE_SRU, VI6_DPR_SRU_ROUTE },
-		{ VI6_DPR_NODE_UDS(0), VI6_DPR_UDS_ROUTE(0) },
-		{ VI6_DPR_NODE_UDS(1), VI6_DPR_UDS_ROUTE(1) },
-		{ VI6_DPR_NODE_UDS(2), VI6_DPR_UDS_ROUTE(2) },
-		{ VI6_DPR_NODE_WPF(0), 0 },
-		{ VI6_DPR_NODE_WPF(1), 0 },
-		{ VI6_DPR_NODE_WPF(2), 0 },
-		{ VI6_DPR_NODE_WPF(3), 0 },
-	};
-
 	unsigned int i;
 
-	for (i = 0; i < ARRAY_SIZE(routes); ++i) {
-		if (routes[i].id == entity->id) {
-			entity->route = routes[i].reg;
+	for (i = 0; i < ARRAY_SIZE(vsp1_routes); ++i) {
+		if (vsp1_routes[i].type == entity->type &&
+		    vsp1_routes[i].index == entity->index) {
+			entity->route = &vsp1_routes[i];
 			break;
 		}
 	}
 
-	if (i == ARRAY_SIZE(routes))
+	if (i == ARRAY_SIZE(vsp1_routes))
 		return -EINVAL;
 
 	entity->vsp1 = vsp1;
diff --git a/drivers/media/platform/vsp1/vsp1_entity.h b/drivers/media/platform/vsp1/vsp1_entity.h
index f6fd6988aeb..7afbd8a7ba6 100644
--- a/drivers/media/platform/vsp1/vsp1_entity.h
+++ b/drivers/media/platform/vsp1/vsp1_entity.h
@@ -20,6 +20,7 @@
 struct vsp1_device;
 
 enum vsp1_entity_type {
+	VSP1_ENTITY_BRU,
 	VSP1_ENTITY_HSI,
 	VSP1_ENTITY_HST,
 	VSP1_ENTITY_LIF,
@@ -30,13 +31,31 @@ enum vsp1_entity_type {
 	VSP1_ENTITY_WPF,
 };
 
+/*
+ * struct vsp1_route - Entity routing configuration
+ * @type: Entity type this routing entry is associated with
+ * @index: Entity index this routing entry is associated with
+ * @reg: Output routing configuration register
+ * @inputs: Target node value for each input
+ *
+ * Each $vsp1_route entry describes routing configuration for the entity
+ * specified by the entry's @type and @index. @reg indicates the register that
+ * holds output routing configuration for the entity, and the @inputs array
+ * store the target node value for each input of the entity.
+ */
+struct vsp1_route {
+	enum vsp1_entity_type type;
+	unsigned int index;
+	unsigned int reg;
+	unsigned int inputs[4];
+};
+
 struct vsp1_entity {
 	struct vsp1_device *vsp1;
 
 	enum vsp1_entity_type type;
 	unsigned int index;
-	unsigned int id;
-	unsigned int route;
+	const struct vsp1_route *route;
 
 	struct list_head list_dev;
 	struct list_head list_pipe;
@@ -45,6 +64,7 @@ struct vsp1_entity {
 	unsigned int source_pad;
 
 	struct media_entity *sink;
+	unsigned int sink_pad;
 
 	struct v4l2_subdev subdev;
 	struct v4l2_mbus_framefmt *formats;
diff --git a/drivers/media/platform/vsp1/vsp1_hsit.c b/drivers/media/platform/vsp1/vsp1_hsit.c
index 285485350d8..db2950a73c6 100644
--- a/drivers/media/platform/vsp1/vsp1_hsit.c
+++ b/drivers/media/platform/vsp1/vsp1_hsit.c
@@ -193,13 +193,10 @@ struct vsp1_hsit *vsp1_hsit_create(struct vsp1_device *vsp1, bool inverse)
 
 	hsit->inverse = inverse;
 
-	if (inverse) {
+	if (inverse)
 		hsit->entity.type = VSP1_ENTITY_HSI;
-		hsit->entity.id = VI6_DPR_NODE_HSI;
-	} else {
+	else
 		hsit->entity.type = VSP1_ENTITY_HST;
-		hsit->entity.id = VI6_DPR_NODE_HST;
-	}
 
 	ret = vsp1_entity_init(vsp1, &hsit->entity, 2);
 	if (ret < 0)
diff --git a/drivers/media/platform/vsp1/vsp1_lif.c b/drivers/media/platform/vsp1/vsp1_lif.c
index 135a7895701..d4fb23e9c4a 100644
--- a/drivers/media/platform/vsp1/vsp1_lif.c
+++ b/drivers/media/platform/vsp1/vsp1_lif.c
@@ -215,7 +215,6 @@ struct vsp1_lif *vsp1_lif_create(struct vsp1_device *vsp1)
 		return ERR_PTR(-ENOMEM);
 
 	lif->entity.type = VSP1_ENTITY_LIF;
-	lif->entity.id = VI6_DPR_NODE_LIF;
 
 	ret = vsp1_entity_init(vsp1, &lif->entity, 2);
 	if (ret < 0)
diff --git a/drivers/media/platform/vsp1/vsp1_lut.c b/drivers/media/platform/vsp1/vsp1_lut.c
index 4e9dc7c86ef..fea36ebe256 100644
--- a/drivers/media/platform/vsp1/vsp1_lut.c
+++ b/drivers/media/platform/vsp1/vsp1_lut.c
@@ -229,7 +229,6 @@ struct vsp1_lut *vsp1_lut_create(struct vsp1_device *vsp1)
 		return ERR_PTR(-ENOMEM);
 
 	lut->entity.type = VSP1_ENTITY_LUT;
-	lut->entity.id = VI6_DPR_NODE_LUT;
 
 	ret = vsp1_entity_init(vsp1, &lut->entity, 2);
 	if (ret < 0)
diff --git a/drivers/media/platform/vsp1/vsp1_regs.h b/drivers/media/platform/vsp1/vsp1_regs.h
index 28650806c20..3e74b44286f 100644
--- a/drivers/media/platform/vsp1/vsp1_regs.h
+++ b/drivers/media/platform/vsp1/vsp1_regs.h
@@ -451,13 +451,111 @@
  * BRU Control Registers
  */
 
+#define VI6_ROP_NOP			0
+#define VI6_ROP_AND			1
+#define VI6_ROP_AND_REV			2
+#define VI6_ROP_COPY			3
+#define VI6_ROP_AND_INV			4
+#define VI6_ROP_CLEAR			5
+#define VI6_ROP_XOR			6
+#define VI6_ROP_OR			7
+#define VI6_ROP_NOR			8
+#define VI6_ROP_EQUIV			9
+#define VI6_ROP_INVERT			10
+#define VI6_ROP_OR_REV			11
+#define VI6_ROP_COPY_INV		12
+#define VI6_ROP_OR_INV			13
+#define VI6_ROP_NAND			14
+#define VI6_ROP_SET			15
+
 #define VI6_BRU_INCTRL			0x2c00
+#define VI6_BRU_INCTRL_NRM		(1 << 28)
+#define VI6_BRU_INCTRL_DnON		(1 << (16 + (n)))
+#define VI6_BRU_INCTRL_DITHn_OFF	(0 << ((n) * 4))
+#define VI6_BRU_INCTRL_DITHn_18BPP	(1 << ((n) * 4))
+#define VI6_BRU_INCTRL_DITHn_16BPP	(2 << ((n) * 4))
+#define VI6_BRU_INCTRL_DITHn_15BPP	(3 << ((n) * 4))
+#define VI6_BRU_INCTRL_DITHn_12BPP	(4 << ((n) * 4))
+#define VI6_BRU_INCTRL_DITHn_8BPP	(5 << ((n) * 4))
+#define VI6_BRU_INCTRL_DITHn_MASK	(7 << ((n) * 4))
+#define VI6_BRU_INCTRL_DITHn_SHIFT	((n) * 4)
+
 #define VI6_BRU_VIRRPF_SIZE		0x2c04
+#define VI6_BRU_VIRRPF_SIZE_HSIZE_MASK	(0x1fff << 16)
+#define VI6_BRU_VIRRPF_SIZE_HSIZE_SHIFT	16
+#define VI6_BRU_VIRRPF_SIZE_VSIZE_MASK	(0x1fff << 0)
+#define VI6_BRU_VIRRPF_SIZE_VSIZE_SHIFT	0
+
 #define VI6_BRU_VIRRPF_LOC		0x2c08
+#define VI6_BRU_VIRRPF_LOC_HCOORD_MASK	(0x1fff << 16)
+#define VI6_BRU_VIRRPF_LOC_HCOORD_SHIFT	16
+#define VI6_BRU_VIRRPF_LOC_VCOORD_MASK	(0x1fff << 0)
+#define VI6_BRU_VIRRPF_LOC_VCOORD_SHIFT	0
+
 #define VI6_BRU_VIRRPF_COL		0x2c0c
+#define VI6_BRU_VIRRPF_COL_A_MASK	(0xff << 24)
+#define VI6_BRU_VIRRPF_COL_A_SHIFT	24
+#define VI6_BRU_VIRRPF_COL_RCR_MASK	(0xff << 16)
+#define VI6_BRU_VIRRPF_COL_RCR_SHIFT	16
+#define VI6_BRU_VIRRPF_COL_GY_MASK	(0xff << 8)
+#define VI6_BRU_VIRRPF_COL_GY_SHIFT	8
+#define VI6_BRU_VIRRPF_COL_BCB_MASK	(0xff << 0)
+#define VI6_BRU_VIRRPF_COL_BCB_SHIFT	0
+
 #define VI6_BRU_CTRL(n)			(0x2c10 + (n) * 8)
+#define VI6_BRU_CTRL_RBC		(1 << 31)
+#define VI6_BRU_CTRL_DSTSEL_BRUIN(n)	((n) << 20)
+#define VI6_BRU_CTRL_DSTSEL_VRPF	(4 << 20)
+#define VI6_BRU_CTRL_DSTSEL_MASK	(7 << 20)
+#define VI6_BRU_CTRL_SRCSEL_BRUIN(n)	((n) << 16)
+#define VI6_BRU_CTRL_SRCSEL_VRPF	(4 << 16)
+#define VI6_BRU_CTRL_SRCSEL_MASK	(7 << 16)
+#define VI6_BRU_CTRL_CROP(rop)		((rop) << 4)
+#define VI6_BRU_CTRL_CROP_MASK		(0xf << 4)
+#define VI6_BRU_CTRL_AROP(rop)		((rop) << 0)
+#define VI6_BRU_CTRL_AROP_MASK		(0xf << 0)
+
 #define VI6_BRU_BLD(n)			(0x2c14 + (n) * 8)
+#define VI6_BRU_BLD_CBES		(1 << 31)
+#define VI6_BRU_BLD_CCMDX_DST_A		(0 << 28)
+#define VI6_BRU_BLD_CCMDX_255_DST_A	(1 << 28)
+#define VI6_BRU_BLD_CCMDX_SRC_A		(2 << 28)
+#define VI6_BRU_BLD_CCMDX_255_SRC_A	(3 << 28)
+#define VI6_BRU_BLD_CCMDX_COEFX		(4 << 28)
+#define VI6_BRU_BLD_CCMDX_MASK		(7 << 28)
+#define VI6_BRU_BLD_CCMDY_DST_A		(0 << 24)
+#define VI6_BRU_BLD_CCMDY_255_DST_A	(1 << 24)
+#define VI6_BRU_BLD_CCMDY_SRC_A		(2 << 24)
+#define VI6_BRU_BLD_CCMDY_255_SRC_A	(3 << 24)
+#define VI6_BRU_BLD_CCMDY_COEFY		(4 << 24)
+#define VI6_BRU_BLD_CCMDY_MASK		(7 << 24)
+#define VI6_BRU_BLD_CCMDY_SHIFT		24
+#define VI6_BRU_BLD_ABES		(1 << 23)
+#define VI6_BRU_BLD_ACMDX_DST_A		(0 << 20)
+#define VI6_BRU_BLD_ACMDX_255_DST_A	(1 << 20)
+#define VI6_BRU_BLD_ACMDX_SRC_A		(2 << 20)
+#define VI6_BRU_BLD_ACMDX_255_SRC_A	(3 << 20)
+#define VI6_BRU_BLD_ACMDX_COEFX		(4 << 20)
+#define VI6_BRU_BLD_ACMDX_MASK		(7 << 20)
+#define VI6_BRU_BLD_ACMDY_DST_A		(0 << 16)
+#define VI6_BRU_BLD_ACMDY_255_DST_A	(1 << 16)
+#define VI6_BRU_BLD_ACMDY_SRC_A		(2 << 16)
+#define VI6_BRU_BLD_ACMDY_255_SRC_A	(3 << 16)
+#define VI6_BRU_BLD_ACMDY_COEFY		(4 << 16)
+#define VI6_BRU_BLD_ACMDY_MASK		(7 << 16)
+#define VI6_BRU_BLD_COEFX_MASK		(0xff << 8)
+#define VI6_BRU_BLD_COEFX_SHIFT		8
+#define VI6_BRU_BLD_COEFY_MASK		(0xff << 0)
+#define VI6_BRU_BLD_COEFY_SHIFT		0
+
 #define VI6_BRU_ROP			0x2c30
+#define VI6_BRU_ROP_DSTSEL_BRUIN(n)	((n) << 20)
+#define VI6_BRU_ROP_DSTSEL_VRPF		(4 << 20)
+#define VI6_BRU_ROP_DSTSEL_MASK		(7 << 20)
+#define VI6_BRU_ROP_CROP(rop)		((rop) << 4)
+#define VI6_BRU_ROP_CROP_MASK		(0xf << 4)
+#define VI6_BRU_ROP_AROP(rop)		((rop) << 0)
+#define VI6_BRU_ROP_AROP_MASK		(0xf << 0)
 
 /* -----------------------------------------------------------------------------
  * HGO Control Registers
diff --git a/drivers/media/platform/vsp1/vsp1_rpf.c b/drivers/media/platform/vsp1/vsp1_rpf.c
index 2b04d0f95c6..c3d98642a4a 100644
--- a/drivers/media/platform/vsp1/vsp1_rpf.c
+++ b/drivers/media/platform/vsp1/vsp1_rpf.c
@@ -96,8 +96,10 @@ static int rpf_s_stream(struct v4l2_subdev *subdev, int enable)
 	vsp1_rpf_write(rpf, VI6_RPF_INFMT, infmt);
 	vsp1_rpf_write(rpf, VI6_RPF_DSWAP, fmtinfo->swap);
 
-	/* Output location. Composing isn't supported yet. */
-	vsp1_rpf_write(rpf, VI6_RPF_LOC, 0);
+	/* Output location */
+	vsp1_rpf_write(rpf, VI6_RPF_LOC,
+		       (rpf->location.left << VI6_RPF_LOC_HCOORD_SHIFT) |
+		       (rpf->location.top << VI6_RPF_LOC_VCOORD_SHIFT));
 
 	/* Disable alpha, mask and color key. Set the alpha channel to a fixed
 	 * value of 255.
@@ -176,7 +178,6 @@ struct vsp1_rwpf *vsp1_rpf_create(struct vsp1_device *vsp1, unsigned int index)
 
 	rpf->entity.type = VSP1_ENTITY_RPF;
 	rpf->entity.index = index;
-	rpf->entity.id = VI6_DPR_NODE_RPF(index);
 
 	ret = vsp1_entity_init(vsp1, &rpf->entity, 2);
 	if (ret < 0)
diff --git a/drivers/media/platform/vsp1/vsp1_rwpf.h b/drivers/media/platform/vsp1/vsp1_rwpf.h
index 5c5ee81bbea..b4fb65e5877 100644
--- a/drivers/media/platform/vsp1/vsp1_rwpf.h
+++ b/drivers/media/platform/vsp1/vsp1_rwpf.h
@@ -30,6 +30,10 @@ struct vsp1_rwpf {
 	unsigned int max_width;
 	unsigned int max_height;
 
+	struct {
+		unsigned int left;
+		unsigned int top;
+	} location;
 	struct v4l2_rect crop;
 
 	unsigned int offsets[2];
diff --git a/drivers/media/platform/vsp1/vsp1_sru.c b/drivers/media/platform/vsp1/vsp1_sru.c
index 7ab1a0b2d65..aa0e04c56f3 100644
--- a/drivers/media/platform/vsp1/vsp1_sru.c
+++ b/drivers/media/platform/vsp1/vsp1_sru.c
@@ -327,7 +327,6 @@ struct vsp1_sru *vsp1_sru_create(struct vsp1_device *vsp1)
 		return ERR_PTR(-ENOMEM);
 
 	sru->entity.type = VSP1_ENTITY_SRU;
-	sru->entity.id = VI6_DPR_NODE_SRU;
 
 	ret = vsp1_entity_init(vsp1, &sru->entity, 2);
 	if (ret < 0)
diff --git a/drivers/media/platform/vsp1/vsp1_uds.c b/drivers/media/platform/vsp1/vsp1_uds.c
index 622342ac777..0293bdbb440 100644
--- a/drivers/media/platform/vsp1/vsp1_uds.c
+++ b/drivers/media/platform/vsp1/vsp1_uds.c
@@ -131,7 +131,7 @@ static int uds_s_stream(struct v4l2_subdev *subdev, int enable)
 		return 0;
 
 	/* Enable multi-tap scaling. */
-	vsp1_uds_write(uds, VI6_UDS_CTRL, VI6_UDS_CTRL_BC);
+	vsp1_uds_write(uds, VI6_UDS_CTRL, VI6_UDS_CTRL_AON | VI6_UDS_CTRL_BC);
 
 	vsp1_uds_write(uds, VI6_UDS_PASS_BWIDTH,
 		       (uds_passband_width(uds->hscale)
@@ -139,7 +139,6 @@ static int uds_s_stream(struct v4l2_subdev *subdev, int enable)
 		       (uds_passband_width(uds->vscale)
 				<< VI6_UDS_PASS_BWIDTH_V_SHIFT));
 
-
 	/* Set the scaling ratios and the output size. */
 	format = &uds->entity.formats[UDS_PAD_SOURCE];
 
@@ -323,7 +322,6 @@ struct vsp1_uds *vsp1_uds_create(struct vsp1_device *vsp1, unsigned int index)
 
 	uds->entity.type = VSP1_ENTITY_UDS;
 	uds->entity.index = index;
-	uds->entity.id = VI6_DPR_NODE_UDS(index);
 
 	ret = vsp1_entity_init(vsp1, &uds->entity, 2);
 	if (ret < 0)
diff --git a/drivers/media/platform/vsp1/vsp1_video.c b/drivers/media/platform/vsp1/vsp1_video.c
index b48f135ffc0..8a1253e51f0 100644
--- a/drivers/media/platform/vsp1/vsp1_video.c
+++ b/drivers/media/platform/vsp1/vsp1_video.c
@@ -28,6 +28,7 @@
 #include <media/videobuf2-dma-contig.h>
 
 #include "vsp1.h"
+#include "vsp1_bru.h"
 #include "vsp1_entity.h"
 #include "vsp1_rwpf.h"
 #include "vsp1_video.h"
@@ -280,6 +281,9 @@ static int vsp1_pipeline_validate_branch(struct vsp1_rwpf *input,
 	struct media_pad *pad;
 	bool uds_found = false;
 
+	input->location.left = 0;
+	input->location.top = 0;
+
 	pad = media_entity_remote_pad(&input->entity.pads[RWPF_PAD_SOURCE]);
 
 	while (1) {
@@ -292,6 +296,17 @@ static int vsp1_pipeline_validate_branch(struct vsp1_rwpf *input,
 
 		entity = to_vsp1_entity(media_entity_to_v4l2_subdev(pad->entity));
 
+		/* A BRU is present in the pipeline, store the compose rectangle
+		 * location in the input RPF for use when configuring the RPF.
+		 */
+		if (entity->type == VSP1_ENTITY_BRU) {
+			struct vsp1_bru *bru = to_bru(&entity->subdev);
+			struct v4l2_rect *rect = &bru->compose[pad->index];
+
+			input->location.left = rect->left;
+			input->location.top = rect->top;
+		}
+
 		/* We've reached the WPF, we're done. */
 		if (entity->type == VSP1_ENTITY_WPF)
 			break;
@@ -363,6 +378,8 @@ static int vsp1_pipeline_validate(struct vsp1_pipeline *pipe,
 			rwpf->video.pipe_index = 0;
 		} else if (e->type == VSP1_ENTITY_LIF) {
 			pipe->lif = e;
+		} else if (e->type == VSP1_ENTITY_BRU) {
+			pipe->bru = e;
 		}
 	}
 
@@ -392,6 +409,7 @@ error:
 	pipe->num_video = 0;
 	pipe->num_inputs = 0;
 	pipe->output = NULL;
+	pipe->bru = NULL;
 	pipe->lif = NULL;
 	return ret;
 }
@@ -430,6 +448,7 @@ static void vsp1_pipeline_cleanup(struct vsp1_pipeline *pipe)
 		pipe->num_video = 0;
 		pipe->num_inputs = 0;
 		pipe->output = NULL;
+		pipe->bru = NULL;
 		pipe->lif = NULL;
 	}
 
@@ -461,7 +480,7 @@ static int vsp1_pipeline_stop(struct vsp1_pipeline *pipe)
 
 	list_for_each_entry(entity, &pipe->entities, list_pipe) {
 		if (entity->route)
-			vsp1_write(entity->vsp1, entity->route,
+			vsp1_write(entity->vsp1, entity->route->reg,
 				   VI6_DPR_NODE_UNUSED);
 
 		v4l2_subdev_call(&entity->subdev, video, s_stream, 0);
@@ -680,11 +699,12 @@ static void vsp1_entity_route_setup(struct vsp1_entity *source)
 {
 	struct vsp1_entity *sink;
 
-	if (source->route == 0)
+	if (source->route->reg == 0)
 		return;
 
 	sink = container_of(source->sink, struct vsp1_entity, subdev.entity);
-	vsp1_write(source->vsp1, source->route, sink->id);
+	vsp1_write(source->vsp1, source->route->reg,
+		   sink->route->inputs[source->sink_pad]);
 }
 
 static int vsp1_video_start_streaming(struct vb2_queue *vq, unsigned int count)
@@ -720,7 +740,7 @@ static int vsp1_video_start_streaming(struct vb2_queue *vq, unsigned int count)
 	return 0;
 }
 
-static int vsp1_video_stop_streaming(struct vb2_queue *vq)
+static void vsp1_video_stop_streaming(struct vb2_queue *vq)
 {
 	struct vsp1_video *video = vb2_get_drv_priv(vq);
 	struct vsp1_pipeline *pipe = to_vsp1_pipeline(&video->video.entity);
@@ -743,8 +763,6 @@ static int vsp1_video_stop_streaming(struct vb2_queue *vq)
 	spin_lock_irqsave(&video->irqlock, flags);
 	INIT_LIST_HEAD(&video->irqqueue);
 	spin_unlock_irqrestore(&video->irqlock, flags);
-
-	return 0;
 }
 
 static struct vb2_ops vsp1_video_queue_qops = {
diff --git a/drivers/media/platform/vsp1/vsp1_video.h b/drivers/media/platform/vsp1/vsp1_video.h
index 53e4b374594..c04d48fa299 100644
--- a/drivers/media/platform/vsp1/vsp1_video.h
+++ b/drivers/media/platform/vsp1/vsp1_video.h
@@ -75,6 +75,7 @@ struct vsp1_pipeline {
 	unsigned int num_inputs;
 	struct vsp1_rwpf *inputs[VPS1_MAX_RPF];
 	struct vsp1_rwpf *output;
+	struct vsp1_entity *bru;
 	struct vsp1_entity *lif;
 
 	struct list_head entities;
diff --git a/drivers/media/platform/vsp1/vsp1_wpf.c b/drivers/media/platform/vsp1/vsp1_wpf.c
index 11a61c601da..1294340dcb3 100644
--- a/drivers/media/platform/vsp1/vsp1_wpf.c
+++ b/drivers/media/platform/vsp1/vsp1_wpf.c
@@ -58,13 +58,21 @@ static int wpf_s_stream(struct v4l2_subdev *subdev, int enable)
 		return 0;
 	}
 
-	/* Sources */
+	/* Sources. If the pipeline has a single input configure it as the
+	 * master layer. Otherwise configure all inputs as sub-layers and
+	 * select the virtual RPF as the master layer.
+	 */
 	for (i = 0; i < pipe->num_inputs; ++i) {
 		struct vsp1_rwpf *input = pipe->inputs[i];
 
-		srcrpf |= VI6_WPF_SRCRPF_RPF_ACT_MST(input->entity.index);
+		srcrpf |= pipe->num_inputs == 1
+			? VI6_WPF_SRCRPF_RPF_ACT_MST(input->entity.index)
+			: VI6_WPF_SRCRPF_RPF_ACT_SUB(input->entity.index);
 	}
 
+	if (pipe->num_inputs > 1)
+		srcrpf |= VI6_WPF_SRCRPF_VIRACT_MST;
+
 	vsp1_wpf_write(wpf, VI6_WPF_SRCRPF, srcrpf);
 
 	/* Destination stride. */
@@ -181,7 +189,6 @@ struct vsp1_rwpf *vsp1_wpf_create(struct vsp1_device *vsp1, unsigned int index)
 
 	wpf->entity.type = VSP1_ENTITY_WPF;
 	wpf->entity.index = index;
-	wpf->entity.id = VI6_DPR_NODE_WPF(index);
 
 	ret = vsp1_entity_init(vsp1, &wpf->entity, 2);
 	if (ret < 0)
diff --git a/drivers/media/rc/mceusb.c b/drivers/media/rc/mceusb.c
index 5d8f3d40d82..d5c1df3c9db 100644
--- a/drivers/media/rc/mceusb.c
+++ b/drivers/media/rc/mceusb.c
@@ -747,11 +747,19 @@ static void mce_request_packet(struct mceusb_dev *ir, unsigned char *data,
 		}
 
 		/* outbound data */
-		pipe = usb_sndintpipe(ir->usbdev,
-				      ir->usb_ep_out->bEndpointAddress);
-		usb_fill_int_urb(async_urb, ir->usbdev, pipe,
-			async_buf, size, mce_async_callback,
-			ir, ir->usb_ep_out->bInterval);
+		if (usb_endpoint_xfer_int(ir->usb_ep_out)) {
+			pipe = usb_sndintpipe(ir->usbdev,
+					 ir->usb_ep_out->bEndpointAddress);
+			usb_fill_int_urb(async_urb, ir->usbdev, pipe, async_buf,
+					 size, mce_async_callback, ir,
+					 ir->usb_ep_out->bInterval);
+		} else {
+			pipe = usb_sndbulkpipe(ir->usbdev,
+					 ir->usb_ep_out->bEndpointAddress);
+			usb_fill_bulk_urb(async_urb, ir->usbdev, pipe,
+					 async_buf, size, mce_async_callback,
+					 ir);
+		}
 		memcpy(async_buf, data, size);
 
 	} else if (urb_type == MCEUSB_RX) {
@@ -1269,32 +1277,26 @@ static int mceusb_dev_probe(struct usb_interface *intf,
 	for (i = 0; i < idesc->desc.bNumEndpoints; ++i) {
 		ep = &idesc->endpoint[i].desc;
 
-		if ((ep_in == NULL)
-			&& ((ep->bEndpointAddress & USB_ENDPOINT_DIR_MASK)
-			    == USB_DIR_IN)
-			&& (((ep->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK)
-			    == USB_ENDPOINT_XFER_BULK)
-			|| ((ep->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK)
-			    == USB_ENDPOINT_XFER_INT))) {
-
-			ep_in = ep;
-			ep_in->bmAttributes = USB_ENDPOINT_XFER_INT;
-			ep_in->bInterval = 1;
-			dev_dbg(&intf->dev, "acceptable inbound endpoint found");
+		if (ep_in == NULL) {
+			if (usb_endpoint_is_bulk_in(ep)) {
+				ep_in = ep;
+				dev_dbg(&intf->dev, "acceptable bulk inbound endpoint found\n");
+			} else if (usb_endpoint_is_int_in(ep)) {
+				ep_in = ep;
+				ep_in->bInterval = 1;
+				dev_dbg(&intf->dev, "acceptable interrupt inbound endpoint found\n");
+			}
 		}
 
-		if ((ep_out == NULL)
-			&& ((ep->bEndpointAddress & USB_ENDPOINT_DIR_MASK)
-			    == USB_DIR_OUT)
-			&& (((ep->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK)
-			    == USB_ENDPOINT_XFER_BULK)
-			|| ((ep->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK)
-			    == USB_ENDPOINT_XFER_INT))) {
-
-			ep_out = ep;
-			ep_out->bmAttributes = USB_ENDPOINT_XFER_INT;
-			ep_out->bInterval = 1;
-			dev_dbg(&intf->dev, "acceptable outbound endpoint found");
+		if (ep_out == NULL) {
+			if (usb_endpoint_is_bulk_out(ep)) {
+				ep_out = ep;
+				dev_dbg(&intf->dev, "acceptable bulk outbound endpoint found\n");
+			} else if (usb_endpoint_is_int_out(ep)) {
+				ep_out = ep;
+				ep_out->bInterval = 1;
+				dev_dbg(&intf->dev, "acceptable interrupt outbound endpoint found\n");
+			}
 		}
 	}
 	if (ep_in == NULL) {
@@ -1302,7 +1304,10 @@ static int mceusb_dev_probe(struct usb_interface *intf,
 		return -ENODEV;
 	}
 
-	pipe = usb_rcvintpipe(dev, ep_in->bEndpointAddress);
+	if (usb_endpoint_xfer_int(ep_in))
+		pipe = usb_rcvintpipe(dev, ep_in->bEndpointAddress);
+	else
+		pipe = usb_rcvbulkpipe(dev, ep_in->bEndpointAddress);
 	maxp = usb_maxpacket(dev, pipe, usb_pipeout(pipe));
 
 	ir = kzalloc(sizeof(struct mceusb_dev), GFP_KERNEL);
diff --git a/drivers/media/rc/redrat3.c b/drivers/media/rc/redrat3.c
index 47cd373e229..79abbc8d960 100644
--- a/drivers/media/rc/redrat3.c
+++ b/drivers/media/rc/redrat3.c
@@ -60,28 +60,6 @@
 #define DRIVER_DESC "RedRat3 USB IR Transceiver Driver"
 #define DRIVER_NAME "redrat3"
 
-/* module parameters */
-#ifdef CONFIG_USB_DEBUG
-static int debug = 1;
-#else
-static int debug;
-#endif
-
-#define RR3_DEBUG_STANDARD		0x1
-#define RR3_DEBUG_FUNCTION_TRACE	0x2
-
-#define rr3_dbg(dev, fmt, ...)					\
-	do {							\
-		if (debug & RR3_DEBUG_STANDARD)			\
-			dev_info(dev, fmt, ## __VA_ARGS__);	\
-	} while (0)
-
-#define rr3_ftr(dev, fmt, ...)					\
-	do {							\
-		if (debug & RR3_DEBUG_FUNCTION_TRACE)		\
-			dev_info(dev, fmt, ## __VA_ARGS__);	\
-	} while (0)
-
 /* bulk data transfer types */
 #define RR3_ERROR		0x01
 #define RR3_MOD_SIGNAL_IN	0x20
@@ -237,13 +215,11 @@ static void redrat3_issue_async(struct redrat3_dev *rr3)
 {
 	int res;
 
-	rr3_ftr(rr3->dev, "Entering %s\n", __func__);
-
 	res = usb_submit_urb(rr3->read_urb, GFP_ATOMIC);
 	if (res)
-		rr3_dbg(rr3->dev, "%s: receive request FAILED! "
-			"(res %d, len %d)\n", __func__, res,
-			rr3->read_urb->transfer_buffer_length);
+		dev_dbg(rr3->dev,
+			"%s: receive request FAILED! (res %d, len %d)\n",
+			__func__, res, rr3->read_urb->transfer_buffer_length);
 }
 
 static void redrat3_dump_fw_error(struct redrat3_dev *rr3, int code)
@@ -359,7 +335,7 @@ static void redrat3_rx_timeout(unsigned long data)
 {
 	struct redrat3_dev *rr3 = (struct redrat3_dev *)data;
 
-	rr3_dbg(rr3->dev, "calling ir_raw_event_reset\n");
+	dev_dbg(rr3->dev, "calling ir_raw_event_reset\n");
 	ir_raw_event_reset(rr3->rc);
 }
 
@@ -377,8 +353,6 @@ static void redrat3_process_ir_data(struct redrat3_dev *rr3)
 		return;
 	}
 
-	rr3_ftr(rr3->dev, "Entered %s\n", __func__);
-
 	dev = rr3->dev;
 
 	/* Make sure we reset the IR kfifo after a bit of inactivity */
@@ -386,7 +360,7 @@ static void redrat3_process_ir_data(struct redrat3_dev *rr3)
 	mod_timer(&rr3->rx_timeout, jiffies + delay);
 
 	mod_freq = redrat3_val_to_mod_freq(&rr3->irdata);
-	rr3_dbg(dev, "Got mod_freq of %u\n", mod_freq);
+	dev_dbg(dev, "Got mod_freq of %u\n", mod_freq);
 
 	/* process each rr3 encoded byte into an int */
 	sig_size = be16_to_cpu(rr3->irdata.sig_size);
@@ -408,7 +382,7 @@ static void redrat3_process_ir_data(struct redrat3_dev *rr3)
 		/* cap the value to IR_MAX_DURATION */
 		rawir.duration &= IR_MAX_DURATION;
 
-		rr3_dbg(dev, "storing %s with duration %d (i: %d)\n",
+		dev_dbg(dev, "storing %s with duration %d (i: %d)\n",
 			rawir.pulse ? "pulse" : "space", rawir.duration, i);
 		ir_raw_event_store_with_filter(rr3->rc, &rawir);
 	}
@@ -421,12 +395,12 @@ static void redrat3_process_ir_data(struct redrat3_dev *rr3)
 			rawir.duration = US_TO_NS(2800);
 		else
 			rawir.duration = trailer;
-		rr3_dbg(dev, "storing trailing space with duration %d\n",
+		dev_dbg(dev, "storing trailing space with duration %d\n",
 			rawir.duration);
 		ir_raw_event_store_with_filter(rr3->rc, &rawir);
 	}
 
-	rr3_dbg(dev, "calling ir_raw_event_handle\n");
+	dev_dbg(dev, "calling ir_raw_event_handle\n");
 	ir_raw_event_handle(rr3->rc);
 }
 
@@ -464,8 +438,6 @@ static int redrat3_enable_detector(struct redrat3_dev *rr3)
 	struct device *dev = rr3->dev;
 	u8 ret;
 
-	rr3_ftr(dev, "Entering %s\n", __func__);
-
 	ret = redrat3_send_cmd(RR3_RC_DET_ENABLE, rr3);
 	if (ret != 0)
 		dev_dbg(dev, "%s: unexpected ret of %d\n",
@@ -486,7 +458,6 @@ static int redrat3_enable_detector(struct redrat3_dev *rr3)
 static inline void redrat3_delete(struct redrat3_dev *rr3,
 				  struct usb_device *udev)
 {
-	rr3_ftr(rr3->dev, "%s cleaning up\n", __func__);
 	usb_kill_urb(rr3->read_urb);
 	usb_kill_urb(rr3->flash_urb);
 	usb_free_urb(rr3->read_urb);
@@ -519,7 +490,7 @@ static u32 redrat3_get_timeout(struct redrat3_dev *rr3)
 	else {
 		timeout = redrat3_len_to_us(be32_to_cpup(tmp));
 
-		rr3_dbg(rr3->dev, "Got timeout of %d ms\n", timeout / 1000);
+		dev_dbg(rr3->dev, "Got timeout of %d ms\n", timeout / 1000);
 	}
 
 	kfree(tmp);
@@ -535,8 +506,6 @@ static void redrat3_reset(struct redrat3_dev *rr3)
 	u8 *val;
 	int len = sizeof(u8);
 
-	rr3_ftr(dev, "Entering %s\n", __func__);
-
 	rxpipe = usb_rcvctrlpipe(udev, 0);
 	txpipe = usb_sndctrlpipe(udev, 0);
 
@@ -550,19 +519,19 @@ static void redrat3_reset(struct redrat3_dev *rr3)
 	rc = usb_control_msg(udev, rxpipe, RR3_RESET,
 			     USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
 			     RR3_CPUCS_REG_ADDR, 0, val, len, HZ * 25);
-	rr3_dbg(dev, "reset returned 0x%02x\n", rc);
+	dev_dbg(dev, "reset returned 0x%02x\n", rc);
 
 	*val = 5;
 	rc = usb_control_msg(udev, txpipe, RR3_SET_IR_PARAM,
 			     USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_OUT,
 			     RR3_IR_IO_LENGTH_FUZZ, 0, val, len, HZ * 25);
-	rr3_dbg(dev, "set ir parm len fuzz %d rc 0x%02x\n", *val, rc);
+	dev_dbg(dev, "set ir parm len fuzz %d rc 0x%02x\n", *val, rc);
 
 	*val = RR3_DRIVER_MAXLENS;
 	rc = usb_control_msg(udev, txpipe, RR3_SET_IR_PARAM,
 			     USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_OUT,
 			     RR3_IR_IO_MAX_LENGTHS, 0, val, len, HZ * 25);
-	rr3_dbg(dev, "set ir parm max lens %d rc 0x%02x\n", *val, rc);
+	dev_dbg(dev, "set ir parm max lens %d rc 0x%02x\n", *val, rc);
 
 	kfree(val);
 }
@@ -572,8 +541,6 @@ static void redrat3_get_firmware_rev(struct redrat3_dev *rr3)
 	int rc = 0;
 	char *buffer;
 
-	rr3_ftr(rr3->dev, "Entering %s\n", __func__);
-
 	buffer = kzalloc(sizeof(char) * (RR3_FW_VERSION_LEN + 1), GFP_KERNEL);
 	if (!buffer) {
 		dev_err(rr3->dev, "Memory allocation failure\n");
@@ -591,7 +558,6 @@ static void redrat3_get_firmware_rev(struct redrat3_dev *rr3)
 		dev_err(rr3->dev, "Problem fetching firmware ID\n");
 
 	kfree(buffer);
-	rr3_ftr(rr3->dev, "Exiting %s\n", __func__);
 }
 
 static void redrat3_read_packet_start(struct redrat3_dev *rr3, unsigned len)
@@ -599,8 +565,6 @@ static void redrat3_read_packet_start(struct redrat3_dev *rr3, unsigned len)
 	struct redrat3_header *header = rr3->bulk_in_buf;
 	unsigned pktlen, pkttype;
 
-	rr3_ftr(rr3->dev, "Entering %s\n", __func__);
-
 	/* grab the Length and type of transfer */
 	pktlen = be16_to_cpu(header->length);
 	pkttype = be16_to_cpu(header->transfer_type);
@@ -622,12 +586,12 @@ static void redrat3_read_packet_start(struct redrat3_dev *rr3, unsigned len)
 	case RR3_MOD_SIGNAL_IN:
 		memcpy(&rr3->irdata, rr3->bulk_in_buf, len);
 		rr3->bytes_read = len;
-		rr3_dbg(rr3->dev, "bytes_read %d, pktlen %d\n",
+		dev_dbg(rr3->dev, "bytes_read %d, pktlen %d\n",
 			rr3->bytes_read, pktlen);
 		break;
 
 	default:
-		rr3_dbg(rr3->dev, "ignoring packet with type 0x%02x, len of %d, 0x%02x\n",
+		dev_dbg(rr3->dev, "ignoring packet with type 0x%02x, len of %d, 0x%02x\n",
 						pkttype, len, pktlen);
 		break;
 	}
@@ -637,8 +601,6 @@ static void redrat3_read_packet_continue(struct redrat3_dev *rr3, unsigned len)
 {
 	void *irdata = &rr3->irdata;
 
-	rr3_ftr(rr3->dev, "Entering %s\n", __func__);
-
 	if (len + rr3->bytes_read > sizeof(rr3->irdata)) {
 		dev_warn(rr3->dev, "too much data for packet\n");
 		rr3->bytes_read = 0;
@@ -648,7 +610,7 @@ static void redrat3_read_packet_continue(struct redrat3_dev *rr3, unsigned len)
 	memcpy(irdata + rr3->bytes_read, rr3->bulk_in_buf, len);
 
 	rr3->bytes_read += len;
-	rr3_dbg(rr3->dev, "bytes_read %d, pktlen %d\n", rr3->bytes_read,
+	dev_dbg(rr3->dev, "bytes_read %d, pktlen %d\n", rr3->bytes_read,
 				 be16_to_cpu(rr3->irdata.header.length));
 }
 
@@ -659,8 +621,6 @@ static int redrat3_get_ir_data(struct redrat3_dev *rr3, unsigned len)
 	unsigned pkttype;
 	int ret = 0;
 
-	rr3_ftr(dev, "Entering %s\n", __func__);
-
 	if (rr3->bytes_read == 0 && len >= sizeof(struct redrat3_header)) {
 		redrat3_read_packet_start(rr3, len);
 	} else if (rr3->bytes_read != 0) {
@@ -681,7 +641,7 @@ static int redrat3_get_ir_data(struct redrat3_dev *rr3, unsigned len)
 	if (pkttype == RR3_MOD_SIGNAL_IN)
 		redrat3_process_ir_data(rr3);
 	else
-		rr3_dbg(dev, "discarding non-signal data packet (type 0x%02x)\n",
+		dev_dbg(dev, "discarding non-signal data packet (type 0x%02x)\n",
 								pkttype);
 
 out:
@@ -705,8 +665,6 @@ static void redrat3_handle_async(struct urb *urb)
 		return;
 	}
 
-	rr3_ftr(rr3->dev, "Entering %s\n", __func__);
-
 	switch (urb->status) {
 	case 0:
 		ret = redrat3_get_ir_data(rr3, urb->actual_length);
@@ -743,7 +701,7 @@ static int redrat3_set_tx_carrier(struct rc_dev *rcdev, u32 carrier)
 	struct redrat3_dev *rr3 = rcdev->priv;
 	struct device *dev = rr3->dev;
 
-	rr3_dbg(dev, "Setting modulation frequency to %u", carrier);
+	dev_dbg(dev, "Setting modulation frequency to %u", carrier);
 	if (carrier == 0)
 		return -EINVAL;
 
@@ -764,8 +722,6 @@ static int redrat3_transmit_ir(struct rc_dev *rcdev, unsigned *txbuf,
 	u8 curlencheck = 0;
 	unsigned i, sendbuf_len;
 
-	rr3_ftr(dev, "Entering %s\n", __func__);
-
 	if (rr3->transmitting) {
 		dev_warn(dev, "%s: transmitter already in use\n", __func__);
 		return -EAGAIN;
@@ -801,7 +757,7 @@ static int redrat3_transmit_ir(struct rc_dev *rcdev, unsigned *txbuf,
 				break;
 		}
 		if (lencheck == curlencheck) {
-			rr3_dbg(dev, "txbuf[%d]=%u, pos %d, enc %u\n",
+			dev_dbg(dev, "txbuf[%d]=%u, pos %d, enc %u\n",
 				i, txbuf[i], curlencheck, cur_sample_len);
 			if (curlencheck < RR3_DRIVER_MAXLENS) {
 				/* now convert the value to a proper
@@ -835,7 +791,7 @@ static int redrat3_transmit_ir(struct rc_dev *rcdev, unsigned *txbuf,
 	pipe = usb_sndbulkpipe(rr3->udev, rr3->ep_out->bEndpointAddress);
 	ret = usb_bulk_msg(rr3->udev, pipe, irdata,
 			    sendbuf_len, &ret_len, 10 * HZ);
-	rr3_dbg(dev, "sent %d bytes, (ret %d)\n", ret_len, ret);
+	dev_dbg(dev, "sent %d bytes, (ret %d)\n", ret_len, ret);
 
 	/* now tell the hardware to transmit what we sent it */
 	pipe = usb_rcvctrlpipe(rr3->udev, 0);
@@ -957,8 +913,6 @@ static int redrat3_dev_probe(struct usb_interface *intf,
 	int pipe, i;
 	int retval = -ENOMEM;
 
-	rr3_ftr(dev, "%s called\n", __func__);
-
 	uhi = intf->cur_altsetting;
 
 	/* find our bulk-in and bulk-out endpoints */
@@ -971,7 +925,7 @@ static int redrat3_dev_probe(struct usb_interface *intf,
 		    ((addr & USB_ENDPOINT_DIR_MASK) == USB_DIR_IN) &&
 		    ((attrs & USB_ENDPOINT_XFERTYPE_MASK) ==
 		     USB_ENDPOINT_XFER_BULK)) {
-			rr3_dbg(dev, "found bulk-in endpoint at 0x%02x\n",
+			dev_dbg(dev, "found bulk-in endpoint at 0x%02x\n",
 				ep->bEndpointAddress);
 			/* data comes in on 0x82, 0x81 is for other data... */
 			if (ep->bEndpointAddress == RR3_BULK_IN_EP_ADDR)
@@ -982,7 +936,7 @@ static int redrat3_dev_probe(struct usb_interface *intf,
 		    ((addr & USB_ENDPOINT_DIR_MASK) == USB_DIR_OUT) &&
 		    ((attrs & USB_ENDPOINT_XFERTYPE_MASK) ==
 		     USB_ENDPOINT_XFER_BULK)) {
-			rr3_dbg(dev, "found bulk-out endpoint at 0x%02x\n",
+			dev_dbg(dev, "found bulk-out endpoint at 0x%02x\n",
 				ep->bEndpointAddress);
 			ep_out = ep;
 		}
@@ -1074,7 +1028,6 @@ static int redrat3_dev_probe(struct usb_interface *intf,
 	/* we can register the device now, as it is ready */
 	usb_set_intfdata(intf, rr3);
 
-	rr3_ftr(dev, "Exiting %s\n", __func__);
 	return 0;
 
 led_free_error:
@@ -1093,8 +1046,6 @@ static void redrat3_dev_disconnect(struct usb_interface *intf)
 	struct usb_device *udev = interface_to_usbdev(intf);
 	struct redrat3_dev *rr3 = usb_get_intfdata(intf);
 
-	rr3_ftr(&intf->dev, "Entering %s\n", __func__);
-
 	if (!rr3)
 		return;
 
@@ -1103,14 +1054,12 @@ static void redrat3_dev_disconnect(struct usb_interface *intf)
 	led_classdev_unregister(&rr3->led);
 	del_timer_sync(&rr3->rx_timeout);
 	redrat3_delete(rr3, udev);
-
-	rr3_ftr(&intf->dev, "RedRat3 IR Transceiver now disconnected\n");
 }
 
 static int redrat3_dev_suspend(struct usb_interface *intf, pm_message_t message)
 {
 	struct redrat3_dev *rr3 = usb_get_intfdata(intf);
-	rr3_ftr(rr3->dev, "suspend\n");
+
 	led_classdev_suspend(&rr3->led);
 	usb_kill_urb(rr3->read_urb);
 	usb_kill_urb(rr3->flash_urb);
@@ -1120,7 +1069,7 @@ static int redrat3_dev_suspend(struct usb_interface *intf, pm_message_t message)
 static int redrat3_dev_resume(struct usb_interface *intf)
 {
 	struct redrat3_dev *rr3 = usb_get_intfdata(intf);
-	rr3_ftr(rr3->dev, "resume\n");
+
 	if (usb_submit_urb(rr3->read_urb, GFP_ATOMIC))
 		return -EIO;
 	led_classdev_resume(&rr3->led);
@@ -1144,8 +1093,3 @@ MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_AUTHOR(DRIVER_AUTHOR2);
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(usb, redrat3_dev_table);
-
-module_param(debug, int, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(debug, "Enable module debug spew. 0 = no debugging (default) "
-		 "0x1 = standard debug messages, 0x2 = function tracing debug. "
-		 "Flag bits are addative (i.e., 0x3 for both debug types).");
diff --git a/drivers/media/rc/streamzap.c b/drivers/media/rc/streamzap.c
index f4e0bc3d382..bd5e4ff9e0b 100644
--- a/drivers/media/rc/streamzap.c
+++ b/drivers/media/rc/streamzap.c
@@ -42,12 +42,6 @@
 #define DRIVER_NAME	"streamzap"
 #define DRIVER_DESC	"Streamzap Remote Control driver"
 
-#ifdef CONFIG_USB_DEBUG
-static bool debug = 1;
-#else
-static bool debug;
-#endif
-
 #define USB_STREAMZAP_VENDOR_ID		0x0e9c
 #define USB_STREAMZAP_PRODUCT_ID	0x0000
 
@@ -528,6 +522,3 @@ module_usb_driver(streamzap_driver);
 MODULE_AUTHOR("Jarod Wilson <jarod@wilsonet.com>");
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
-
-module_param(debug, bool, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(debug, "Enable debugging messages");
diff --git a/drivers/media/tuners/Kconfig b/drivers/media/tuners/Kconfig
index a1284889cd1..22b6b8bb1d9 100644
--- a/drivers/media/tuners/Kconfig
+++ b/drivers/media/tuners/Kconfig
@@ -230,6 +230,13 @@ config MEDIA_TUNER_TUA9001
 	help
 	  Infineon TUA 9001 silicon tuner driver.
 
+config MEDIA_TUNER_SI2157
+	tristate "Silicon Labs Si2157 silicon tuner"
+	depends on MEDIA_SUPPORT && I2C
+	default m if !MEDIA_SUBDRV_AUTOSELECT
+	help
+	  Silicon Labs Si2157 silicon tuner driver.
+
 config MEDIA_TUNER_IT913X
 	tristate "ITE Tech IT913x silicon tuner"
 	depends on MEDIA_SUPPORT && I2C
diff --git a/drivers/media/tuners/Makefile b/drivers/media/tuners/Makefile
index efe82a904b1..a6ff0c628df 100644
--- a/drivers/media/tuners/Makefile
+++ b/drivers/media/tuners/Makefile
@@ -31,6 +31,7 @@ obj-$(CONFIG_MEDIA_TUNER_TDA18212) += tda18212.o
 obj-$(CONFIG_MEDIA_TUNER_E4000) += e4000.o
 obj-$(CONFIG_MEDIA_TUNER_FC2580) += fc2580.o
 obj-$(CONFIG_MEDIA_TUNER_TUA9001) += tua9001.o
+obj-$(CONFIG_MEDIA_TUNER_SI2157) += si2157.o
 obj-$(CONFIG_MEDIA_TUNER_M88TS2022) += m88ts2022.o
 obj-$(CONFIG_MEDIA_TUNER_FC0011) += fc0011.o
 obj-$(CONFIG_MEDIA_TUNER_FC0012) += fc0012.o
diff --git a/drivers/media/tuners/fc2580.c b/drivers/media/tuners/fc2580.c
index 3aecaf46509..f0c9c42867d 100644
--- a/drivers/media/tuners/fc2580.c
+++ b/drivers/media/tuners/fc2580.c
@@ -195,7 +195,7 @@ static int fc2580_set_params(struct dvb_frontend *fe)
 
 	f_ref = 2UL * priv->cfg->clock / r_val;
 	n_val = div_u64_rem(f_vco, f_ref, &k_val);
-	k_val_reg = 1UL * k_val * (1 << 20) / f_ref;
+	k_val_reg = div_u64(1ULL * k_val * (1 << 20), f_ref);
 
 	ret = fc2580_wr_reg(priv, 0x18, r18_val | ((k_val_reg >> 16) & 0xff));
 	if (ret < 0)
@@ -348,8 +348,8 @@ static int fc2580_set_params(struct dvb_frontend *fe)
 	if (ret < 0)
 		goto err;
 
-	ret = fc2580_wr_reg(priv, 0x37, 1UL * priv->cfg->clock * \
-			fc2580_if_filter_lut[i].mul / 1000000000);
+	ret = fc2580_wr_reg(priv, 0x37, div_u64(1ULL * priv->cfg->clock *
+			fc2580_if_filter_lut[i].mul, 1000000000));
 	if (ret < 0)
 		goto err;
 
diff --git a/drivers/media/tuners/fc2580_priv.h b/drivers/media/tuners/fc2580_priv.h
index be38a9e637e..646c9945213 100644
--- a/drivers/media/tuners/fc2580_priv.h
+++ b/drivers/media/tuners/fc2580_priv.h
@@ -22,6 +22,7 @@
 #define FC2580_PRIV_H
 
 #include "fc2580.h"
+#include <linux/math64.h>
 
 struct fc2580_reg_val {
 	u8 reg;
diff --git a/drivers/media/tuners/si2157.c b/drivers/media/tuners/si2157.c
new file mode 100644
index 00000000000..271a752cee5
--- /dev/null
+++ b/drivers/media/tuners/si2157.c
@@ -0,0 +1,260 @@
+/*
+ * Silicon Labs Si2157 silicon tuner 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 "si2157_priv.h"
+
+/* execute firmware command */
+static int si2157_cmd_execute(struct si2157 *s, struct si2157_cmd *cmd)
+{
+	int ret;
+	u8 buf[1];
+	unsigned long timeout;
+
+	mutex_lock(&s->i2c_mutex);
+
+	if (cmd->len) {
+		/* write cmd and args for firmware */
+		ret = i2c_master_send(s->client, cmd->args, cmd->len);
+		if (ret < 0) {
+			goto err_mutex_unlock;
+		} else if (ret != cmd->len) {
+			ret = -EREMOTEIO;
+			goto err_mutex_unlock;
+		}
+	}
+
+	/* wait cmd execution terminate */
+	#define TIMEOUT 80
+	timeout = jiffies + msecs_to_jiffies(TIMEOUT);
+	while (!time_after(jiffies, timeout)) {
+		ret = i2c_master_recv(s->client, buf, 1);
+		if (ret < 0) {
+			goto err_mutex_unlock;
+		} else if (ret != 1) {
+			ret = -EREMOTEIO;
+			goto err_mutex_unlock;
+		}
+
+		/* firmware ready? */
+		if ((buf[0] >> 7) & 0x01)
+			break;
+	}
+
+	dev_dbg(&s->client->dev, "%s: cmd execution took %d ms\n", __func__,
+			jiffies_to_msecs(jiffies) -
+			(jiffies_to_msecs(timeout) - TIMEOUT));
+
+	if (!(buf[0] >> 7) & 0x01) {
+		ret = -ETIMEDOUT;
+		goto err_mutex_unlock;
+	} else {
+		ret = 0;
+	}
+
+err_mutex_unlock:
+	mutex_unlock(&s->i2c_mutex);
+	if (ret)
+		goto err;
+
+	return 0;
+err:
+	dev_dbg(&s->client->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static int si2157_init(struct dvb_frontend *fe)
+{
+	struct si2157 *s = fe->tuner_priv;
+
+	dev_dbg(&s->client->dev, "%s:\n", __func__);
+
+	s->active = true;
+
+	return 0;
+}
+
+static int si2157_sleep(struct dvb_frontend *fe)
+{
+	struct si2157 *s = fe->tuner_priv;
+
+	dev_dbg(&s->client->dev, "%s:\n", __func__);
+
+	s->active = false;
+
+	return 0;
+}
+
+static int si2157_set_params(struct dvb_frontend *fe)
+{
+	struct si2157 *s = fe->tuner_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int ret;
+	struct si2157_cmd cmd;
+
+	dev_dbg(&s->client->dev,
+			"%s: delivery_system=%d frequency=%u bandwidth_hz=%u\n",
+			__func__, c->delivery_system, c->frequency,
+			c->bandwidth_hz);
+
+	if (!s->active) {
+		ret = -EAGAIN;
+		goto err;
+	}
+
+	/* configure? */
+	cmd.args[0] = 0xc0;
+	cmd.args[1] = 0x00;
+	cmd.args[2] = 0x0c;
+	cmd.args[3] = 0x00;
+	cmd.args[4] = 0x00;
+	cmd.args[5] = 0x01;
+	cmd.args[6] = 0x01;
+	cmd.args[7] = 0x01;
+	cmd.args[8] = 0x01;
+	cmd.args[9] = 0x01;
+	cmd.args[10] = 0x01;
+	cmd.args[11] = 0x02;
+	cmd.args[12] = 0x00;
+	cmd.args[13] = 0x00;
+	cmd.args[14] = 0x01;
+	cmd.len = 15;
+	ret = si2157_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	cmd.args[0] = 0x02;
+	cmd.len = 1;
+	ret = si2157_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	cmd.args[0] = 0x01;
+	cmd.args[1] = 0x01;
+	cmd.len = 2;
+	ret = si2157_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	/* set frequency */
+	cmd.args[0] = 0x41;
+	cmd.args[1] = 0x00;
+	cmd.args[2] = 0x00;
+	cmd.args[3] = 0x00;
+	cmd.args[4] = (c->frequency >>  0) & 0xff;
+	cmd.args[5] = (c->frequency >>  8) & 0xff;
+	cmd.args[6] = (c->frequency >> 16) & 0xff;
+	cmd.args[7] = (c->frequency >> 24) & 0xff;
+	cmd.len = 8;
+	ret = si2157_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	return 0;
+err:
+	dev_dbg(&s->client->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static const struct dvb_tuner_ops si2157_tuner_ops = {
+	.info = {
+		.name           = "Silicon Labs Si2157",
+		.frequency_min  = 110000000,
+		.frequency_max  = 862000000,
+	},
+
+	.init = si2157_init,
+	.sleep = si2157_sleep,
+	.set_params = si2157_set_params,
+};
+
+static int si2157_probe(struct i2c_client *client,
+		const struct i2c_device_id *id)
+{
+	struct si2157_config *cfg = client->dev.platform_data;
+	struct dvb_frontend *fe = cfg->fe;
+	struct si2157 *s;
+	struct si2157_cmd cmd;
+	int ret;
+
+	s = kzalloc(sizeof(struct si2157), GFP_KERNEL);
+	if (!s) {
+		ret = -ENOMEM;
+		dev_err(&client->dev, "%s: kzalloc() failed\n", KBUILD_MODNAME);
+		goto err;
+	}
+
+	s->client = client;
+	s->fe = cfg->fe;
+	mutex_init(&s->i2c_mutex);
+
+	/* check if the tuner is there */
+	cmd.len = 0;
+	ret = si2157_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	fe->tuner_priv = s;
+	memcpy(&fe->ops.tuner_ops, &si2157_tuner_ops,
+			sizeof(struct dvb_tuner_ops));
+
+	i2c_set_clientdata(client, s);
+
+	dev_info(&s->client->dev,
+			"%s: Silicon Labs Si2157 successfully attached\n",
+			KBUILD_MODNAME);
+	return 0;
+err:
+	dev_dbg(&client->dev, "%s: failed=%d\n", __func__, ret);
+	kfree(s);
+
+	return ret;
+}
+
+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__);
+
+	memset(&fe->ops.tuner_ops, 0, sizeof(struct dvb_tuner_ops));
+	fe->tuner_priv = NULL;
+	kfree(s);
+
+	return 0;
+}
+
+static const struct i2c_device_id si2157_id[] = {
+	{"si2157", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, si2157_id);
+
+static struct i2c_driver si2157_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "si2157",
+	},
+	.probe		= si2157_probe,
+	.remove		= si2157_remove,
+	.id_table	= si2157_id,
+};
+
+module_i2c_driver(si2157_driver);
+
+MODULE_DESCRIPTION("Silicon Labs Si2157 silicon tuner driver");
+MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/tuners/si2157.h b/drivers/media/tuners/si2157.h
new file mode 100644
index 00000000000..f469a092b66
--- /dev/null
+++ b/drivers/media/tuners/si2157.h
@@ -0,0 +1,34 @@
+/*
+ * Silicon Labs Si2157 silicon tuner 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.
+ */
+
+#ifndef SI2157_H
+#define SI2157_H
+
+#include <linux/kconfig.h>
+#include "dvb_frontend.h"
+
+/*
+ * I2C address
+ * 0x60
+ */
+struct si2157_config {
+	/*
+	 * frontend
+	 */
+	struct dvb_frontend *fe;
+};
+
+#endif
diff --git a/drivers/media/tuners/si2157_priv.h b/drivers/media/tuners/si2157_priv.h
new file mode 100644
index 00000000000..6cc6c6fdab7
--- /dev/null
+++ b/drivers/media/tuners/si2157_priv.h
@@ -0,0 +1,37 @@
+/*
+ * Silicon Labs Si2157 silicon tuner 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.
+ */
+
+#ifndef SI2157_PRIV_H
+#define SI2157_PRIV_H
+
+#include "si2157.h"
+
+/* state struct */
+struct si2157 {
+	struct mutex i2c_mutex;
+	struct i2c_client *client;
+	struct dvb_frontend *fe;
+	bool active;
+};
+
+/* firmare command struct */
+#define SI2157_ARGLEN      30
+struct si2157_cmd {
+	u8 args[SI2157_ARGLEN];
+	unsigned len;
+};
+
+#endif
diff --git a/drivers/media/tuners/xc5000.c b/drivers/media/tuners/xc5000.c
index 5cd09a681b6..2b3d514be67 100644
--- a/drivers/media/tuners/xc5000.c
+++ b/drivers/media/tuners/xc5000.c
@@ -25,6 +25,7 @@
 #include <linux/moduleparam.h>
 #include <linux/videodev2.h>
 #include <linux/delay.h>
+#include <linux/workqueue.h>
 #include <linux/dvb/frontend.h>
 #include <linux/i2c.h>
 
@@ -65,26 +66,25 @@ struct xc5000_priv {
 	u16 pll_register_no;
 	u8 init_status_supported;
 	u8 fw_checksum_supported;
+
+	struct dvb_frontend *fe;
+	struct delayed_work timer_sleep;
 };
 
 /* Misc Defines */
 #define MAX_TV_STANDARD			24
 #define XC_MAX_I2C_WRITE_LENGTH		64
 
+/* Time to suspend after the .sleep callback is called */
+#define XC5000_SLEEP_TIME		5000 /* ms */
+
 /* Signal Types */
 #define XC_RF_MODE_AIR			0
 #define XC_RF_MODE_CABLE		1
 
-/* Result codes */
-#define XC_RESULT_SUCCESS		0
-#define XC_RESULT_RESET_FAILURE		1
-#define XC_RESULT_I2C_WRITE_FAILURE	2
-#define XC_RESULT_I2C_READ_FAILURE	3
-#define XC_RESULT_OUT_OF_RANGE		5
-
 /* Product id */
 #define XC_PRODUCT_ID_FW_NOT_LOADED	0x2000
-#define XC_PRODUCT_ID_FW_LOADED 	0x1388
+#define XC_PRODUCT_ID_FW_LOADED	0x1388
 
 /* Registers */
 #define XREG_INIT         0x00
@@ -152,16 +152,16 @@ struct xc5000_priv {
 
 */
 struct XC_TV_STANDARD {
-	char *Name;
-	u16 AudioMode;
-	u16 VideoMode;
+	char *name;
+	u16 audio_mode;
+	u16 video_mode;
 };
 
 /* Tuner standards */
 #define MN_NTSC_PAL_BTSC	0
 #define MN_NTSC_PAL_A2		1
 #define MN_NTSC_PAL_EIAJ	2
-#define MN_NTSC_PAL_Mono	3
+#define MN_NTSC_PAL_MONO	3
 #define BG_PAL_A2		4
 #define BG_PAL_NICAM		5
 #define BG_PAL_MONO		6
@@ -171,19 +171,19 @@ struct XC_TV_STANDARD {
 #define DK_PAL_NICAM		10
 #define DK_PAL_MONO		11
 #define DK_SECAM_A2DK1		12
-#define DK_SECAM_A2LDK3 	13
-#define DK_SECAM_A2MONO 	14
+#define DK_SECAM_A2LDK3		13
+#define DK_SECAM_A2MONO		14
 #define L_SECAM_NICAM		15
 #define LC_SECAM_NICAM		16
 #define DTV6			17
 #define DTV8			18
 #define DTV7_8			19
 #define DTV7			20
-#define FM_Radio_INPUT2 	21
-#define FM_Radio_INPUT1 	22
-#define FM_Radio_INPUT1_MONO	23
+#define FM_RADIO_INPUT2		21
+#define FM_RADIO_INPUT1		22
+#define FM_RADIO_INPUT1_MONO	23
 
-static struct XC_TV_STANDARD XC5000_Standard[MAX_TV_STANDARD] = {
+static struct XC_TV_STANDARD xc5000_standard[MAX_TV_STANDARD] = {
 	{"M/N-NTSC/PAL-BTSC", 0x0400, 0x8020},
 	{"M/N-NTSC/PAL-A2",   0x0600, 0x8020},
 	{"M/N-NTSC/PAL-EIAJ", 0x0440, 0x8020},
@@ -249,7 +249,7 @@ static inline const struct xc5000_fw_cfg *xc5000_assign_firmware(int chip_id)
 static int xc_load_fw_and_init_tuner(struct dvb_frontend *fe, int force);
 static int xc5000_is_firmware_loaded(struct dvb_frontend *fe);
 static int xc5000_readreg(struct xc5000_priv *priv, u16 reg, u16 *val);
-static int xc5000_TunerReset(struct dvb_frontend *fe);
+static int xc5000_tuner_reset(struct dvb_frontend *fe);
 
 static int xc_send_i2c_data(struct xc5000_priv *priv, u8 *buf, int len)
 {
@@ -258,9 +258,9 @@ static int xc_send_i2c_data(struct xc5000_priv *priv, u8 *buf, int len)
 
 	if (i2c_transfer(priv->i2c_props.adap, &msg, 1) != 1) {
 		printk(KERN_ERR "xc5000: I2C write failed (len=%i)\n", len);
-		return XC_RESULT_I2C_WRITE_FAILURE;
+		return -EREMOTEIO;
 	}
-	return XC_RESULT_SUCCESS;
+	return 0;
 }
 
 #if 0
@@ -297,15 +297,10 @@ static int xc5000_readreg(struct xc5000_priv *priv, u16 reg, u16 *val)
 	}
 
 	*val = (bval[0] << 8) | bval[1];
-	return XC_RESULT_SUCCESS;
-}
-
-static void xc_wait(int wait_ms)
-{
-	msleep(wait_ms);
+	return 0;
 }
 
-static int xc5000_TunerReset(struct dvb_frontend *fe)
+static int xc5000_tuner_reset(struct dvb_frontend *fe)
 {
 	struct xc5000_priv *priv = fe->tuner_priv;
 	int ret;
@@ -320,43 +315,43 @@ static int xc5000_TunerReset(struct dvb_frontend *fe)
 					   XC5000_TUNER_RESET, 0);
 		if (ret) {
 			printk(KERN_ERR "xc5000: reset failed\n");
-			return XC_RESULT_RESET_FAILURE;
+			return ret;
 		}
 	} else {
 		printk(KERN_ERR "xc5000: no tuner reset callback function, fatal\n");
-		return XC_RESULT_RESET_FAILURE;
+		return -EINVAL;
 	}
-	return XC_RESULT_SUCCESS;
+	return 0;
 }
 
-static int xc_write_reg(struct xc5000_priv *priv, u16 regAddr, u16 i2cData)
+static int xc_write_reg(struct xc5000_priv *priv, u16 reg_addr, u16 i2c_data)
 {
 	u8 buf[4];
-	int WatchDogTimer = 100;
+	int watch_dog_timer = 100;
 	int result;
 
-	buf[0] = (regAddr >> 8) & 0xFF;
-	buf[1] = regAddr & 0xFF;
-	buf[2] = (i2cData >> 8) & 0xFF;
-	buf[3] = i2cData & 0xFF;
+	buf[0] = (reg_addr >> 8) & 0xFF;
+	buf[1] = reg_addr & 0xFF;
+	buf[2] = (i2c_data >> 8) & 0xFF;
+	buf[3] = i2c_data & 0xFF;
 	result = xc_send_i2c_data(priv, buf, 4);
-	if (result == XC_RESULT_SUCCESS) {
+	if (result == 0) {
 		/* wait for busy flag to clear */
-		while ((WatchDogTimer > 0) && (result == XC_RESULT_SUCCESS)) {
+		while ((watch_dog_timer > 0) && (result == 0)) {
 			result = xc5000_readreg(priv, XREG_BUSY, (u16 *)buf);
-			if (result == XC_RESULT_SUCCESS) {
+			if (result == 0) {
 				if ((buf[0] == 0) && (buf[1] == 0)) {
 					/* busy flag cleared */
 					break;
 				} else {
-					xc_wait(5); /* wait 5 ms */
-					WatchDogTimer--;
+					msleep(5); /* wait 5 ms */
+					watch_dog_timer--;
 				}
 			}
 		}
 	}
-	if (WatchDogTimer <= 0)
-		result = XC_RESULT_I2C_WRITE_FAILURE;
+	if (watch_dog_timer <= 0)
+		result = -EREMOTEIO;
 
 	return result;
 }
@@ -375,13 +370,13 @@ static int xc_load_i2c_sequence(struct dvb_frontend *fe, const u8 *i2c_sequence)
 		len = i2c_sequence[index] * 256 + i2c_sequence[index+1];
 		if (len == 0x0000) {
 			/* RESET command */
-			result = xc5000_TunerReset(fe);
+			result = xc5000_tuner_reset(fe);
 			index += 2;
-			if (result != XC_RESULT_SUCCESS)
+			if (result != 0)
 				return result;
 		} else if (len & 0x8000) {
 			/* WAIT command */
-			xc_wait(len & 0x7FFF);
+			msleep(len & 0x7FFF);
 			index += 2;
 		} else {
 			/* Send i2c data whilst ensuring individual transactions
@@ -404,7 +399,7 @@ static int xc_load_i2c_sequence(struct dvb_frontend *fe, const u8 *i2c_sequence)
 				result = xc_send_i2c_data(priv, buf,
 					nbytes_to_send);
 
-				if (result != XC_RESULT_SUCCESS)
+				if (result != 0)
 					return result;
 
 				pos += nbytes_to_send - 2;
@@ -412,7 +407,7 @@ static int xc_load_i2c_sequence(struct dvb_frontend *fe, const u8 *i2c_sequence)
 			index += len;
 		}
 	}
-	return XC_RESULT_SUCCESS;
+	return 0;
 }
 
 static int xc_initialize(struct xc5000_priv *priv)
@@ -421,29 +416,29 @@ static int xc_initialize(struct xc5000_priv *priv)
 	return xc_write_reg(priv, XREG_INIT, 0);
 }
 
-static int xc_SetTVStandard(struct xc5000_priv *priv,
-	u16 VideoMode, u16 AudioMode, u8 RadioMode)
+static int xc_set_tv_standard(struct xc5000_priv *priv,
+	u16 video_mode, u16 audio_mode, u8 radio_mode)
 {
 	int ret;
-	dprintk(1, "%s(0x%04x,0x%04x)\n", __func__, VideoMode, AudioMode);
-	if (RadioMode) {
+	dprintk(1, "%s(0x%04x,0x%04x)\n", __func__, video_mode, audio_mode);
+	if (radio_mode) {
 		dprintk(1, "%s() Standard = %s\n",
 			__func__,
-			XC5000_Standard[RadioMode].Name);
+			xc5000_standard[radio_mode].name);
 	} else {
 		dprintk(1, "%s() Standard = %s\n",
 			__func__,
-			XC5000_Standard[priv->video_standard].Name);
+			xc5000_standard[priv->video_standard].name);
 	}
 
-	ret = xc_write_reg(priv, XREG_VIDEO_MODE, VideoMode);
-	if (ret == XC_RESULT_SUCCESS)
-		ret = xc_write_reg(priv, XREG_AUDIO_MODE, AudioMode);
+	ret = xc_write_reg(priv, XREG_VIDEO_MODE, video_mode);
+	if (ret == 0)
+		ret = xc_write_reg(priv, XREG_AUDIO_MODE, audio_mode);
 
 	return ret;
 }
 
-static int xc_SetSignalSource(struct xc5000_priv *priv, u16 rf_mode)
+static int xc_set_signal_source(struct xc5000_priv *priv, u16 rf_mode)
 {
 	dprintk(1, "%s(%d) Source = %s\n", __func__, rf_mode,
 		rf_mode == XC_RF_MODE_AIR ? "ANTENNA" : "CABLE");
@@ -459,7 +454,7 @@ static int xc_SetSignalSource(struct xc5000_priv *priv, u16 rf_mode)
 
 static const struct dvb_tuner_ops xc5000_tuner_ops;
 
-static int xc_set_RF_frequency(struct xc5000_priv *priv, u32 freq_hz)
+static int xc_set_rf_frequency(struct xc5000_priv *priv, u32 freq_hz)
 {
 	u16 freq_code;
 
@@ -467,7 +462,7 @@ static int xc_set_RF_frequency(struct xc5000_priv *priv, u32 freq_hz)
 
 	if ((freq_hz > xc5000_tuner_ops.info.frequency_max) ||
 		(freq_hz < xc5000_tuner_ops.info.frequency_min))
-		return XC_RESULT_OUT_OF_RANGE;
+		return -EINVAL;
 
 	freq_code = (u16)(freq_hz / 15625);
 
@@ -488,7 +483,7 @@ static int xc_set_IF_frequency(struct xc5000_priv *priv, u32 freq_khz)
 }
 
 
-static int xc_get_ADC_Envelope(struct xc5000_priv *priv, u16 *adc_envelope)
+static int xc_get_adc_envelope(struct xc5000_priv *priv, u16 *adc_envelope)
 {
 	return xc5000_readreg(priv, XREG_ADC_ENV, adc_envelope);
 }
@@ -496,14 +491,14 @@ static int xc_get_ADC_Envelope(struct xc5000_priv *priv, u16 *adc_envelope)
 static int xc_get_frequency_error(struct xc5000_priv *priv, u32 *freq_error_hz)
 {
 	int result;
-	u16 regData;
+	u16 reg_data;
 	u32 tmp;
 
-	result = xc5000_readreg(priv, XREG_FREQ_ERROR, &regData);
-	if (result != XC_RESULT_SUCCESS)
+	result = xc5000_readreg(priv, XREG_FREQ_ERROR, &reg_data);
+	if (result != 0)
 		return result;
 
-	tmp = (u32)regData;
+	tmp = (u32)reg_data;
 	(*freq_error_hz) = (tmp * 15625) / 1000;
 	return result;
 }
@@ -521,7 +516,7 @@ static int xc_get_version(struct xc5000_priv *priv,
 	int result;
 
 	result = xc5000_readreg(priv, XREG_VERSION, &data);
-	if (result != XC_RESULT_SUCCESS)
+	if (result != 0)
 		return result;
 
 	(*hw_majorversion) = (data >> 12) & 0x0F;
@@ -539,14 +534,14 @@ static int xc_get_buildversion(struct xc5000_priv *priv, u16 *buildrev)
 
 static int xc_get_hsync_freq(struct xc5000_priv *priv, u32 *hsync_freq_hz)
 {
-	u16 regData;
+	u16 reg_data;
 	int result;
 
-	result = xc5000_readreg(priv, XREG_HSYNC_FREQ, &regData);
-	if (result != XC_RESULT_SUCCESS)
+	result = xc5000_readreg(priv, XREG_HSYNC_FREQ, &reg_data);
+	if (result != 0)
 		return result;
 
-	(*hsync_freq_hz) = ((regData & 0x0fff) * 763)/100;
+	(*hsync_freq_hz) = ((reg_data & 0x0fff) * 763)/100;
 	return result;
 }
 
@@ -570,19 +565,19 @@ static int xc_get_totalgain(struct xc5000_priv *priv, u16 *totalgain)
 	return xc5000_readreg(priv, XREG_TOTALGAIN, totalgain);
 }
 
-static u16 WaitForLock(struct xc5000_priv *priv)
+static u16 wait_for_lock(struct xc5000_priv *priv)
 {
-	u16 lockState = 0;
-	int watchDogCount = 40;
-
-	while ((lockState == 0) && (watchDogCount > 0)) {
-		xc_get_lock_status(priv, &lockState);
-		if (lockState != 1) {
-			xc_wait(5);
-			watchDogCount--;
+	u16 lock_state = 0;
+	int watch_dog_count = 40;
+
+	while ((lock_state == 0) && (watch_dog_count > 0)) {
+		xc_get_lock_status(priv, &lock_state);
+		if (lock_state != 1) {
+			msleep(5);
+			watch_dog_count--;
 		}
 	}
-	return lockState;
+	return lock_state;
 }
 
 #define XC_TUNE_ANALOG  0
@@ -593,11 +588,11 @@ static int xc_tune_channel(struct xc5000_priv *priv, u32 freq_hz, int mode)
 
 	dprintk(1, "%s(%u)\n", __func__, freq_hz);
 
-	if (xc_set_RF_frequency(priv, freq_hz) != XC_RESULT_SUCCESS)
+	if (xc_set_rf_frequency(priv, freq_hz) != 0)
 		return 0;
 
 	if (mode == XC_TUNE_ANALOG) {
-		if (WaitForLock(priv) == 1)
+		if (wait_for_lock(priv) == 1)
 			found = 1;
 	}
 
@@ -607,7 +602,7 @@ static int xc_tune_channel(struct xc5000_priv *priv, u32 freq_hz, int mode)
 static int xc_set_xtal(struct dvb_frontend *fe)
 {
 	struct xc5000_priv *priv = fe->tuner_priv;
-	int ret = XC_RESULT_SUCCESS;
+	int ret = 0;
 
 	switch (priv->chip_id) {
 	default:
@@ -649,23 +644,22 @@ static int xc5000_fwupload(struct dvb_frontend *fe)
 		priv->i2c_props.adap->dev.parent);
 	if (ret) {
 		printk(KERN_ERR "xc5000: Upload failed. (file not found?)\n");
-		ret = XC_RESULT_RESET_FAILURE;
 		goto out;
 	} else {
 		printk(KERN_DEBUG "xc5000: firmware read %Zu bytes.\n",
 		       fw->size);
-		ret = XC_RESULT_SUCCESS;
+		ret = 0;
 	}
 
 	if (fw->size != desired_fw->size) {
 		printk(KERN_ERR "xc5000: firmware incorrect size\n");
-		ret = XC_RESULT_RESET_FAILURE;
+		ret = -EINVAL;
 	} else {
 		printk(KERN_INFO "xc5000: firmware uploading...\n");
 		ret = xc_load_i2c_sequence(fe,  fw->data);
-		if (XC_RESULT_SUCCESS == ret)
+		if (0 == ret)
 			ret = xc_set_xtal(fe);
-		if (XC_RESULT_SUCCESS == ret)
+		if (0 == ret)
 			printk(KERN_INFO "xc5000: firmware upload complete...\n");
 		else
 			printk(KERN_ERR "xc5000: firmware upload failed...\n");
@@ -695,9 +689,9 @@ static void xc_debug_dump(struct xc5000_priv *priv)
 	 * Frame Lines needs two frame times after initial lock
 	 * before it is valid.
 	 */
-	xc_wait(100);
+	msleep(100);
 
-	xc_get_ADC_Envelope(priv,  &adc_envelope);
+	xc_get_adc_envelope(priv,  &adc_envelope);
 	dprintk(1, "*** ADC envelope (0-1023) = %d\n", adc_envelope);
 
 	xc_get_frequency_error(priv, &freq_error_hz);
@@ -744,7 +738,7 @@ static int xc5000_set_params(struct dvb_frontend *fe)
 	u32 freq = fe->dtv_property_cache.frequency;
 	u32 delsys  = fe->dtv_property_cache.delivery_system;
 
-	if (xc_load_fw_and_init_tuner(fe, 0) != XC_RESULT_SUCCESS) {
+	if (xc_load_fw_and_init_tuner(fe, 0) != 0) {
 		dprintk(1, "Unable to load firmware and init tuner\n");
 		return -EINVAL;
 	}
@@ -820,24 +814,24 @@ static int xc5000_set_params(struct dvb_frontend *fe)
 	dprintk(1, "%s() frequency=%d (compensated to %d)\n",
 		__func__, freq, priv->freq_hz);
 
-	ret = xc_SetSignalSource(priv, priv->rf_mode);
-	if (ret != XC_RESULT_SUCCESS) {
+	ret = xc_set_signal_source(priv, priv->rf_mode);
+	if (ret != 0) {
 		printk(KERN_ERR
-			"xc5000: xc_SetSignalSource(%d) failed\n",
+			"xc5000: xc_set_signal_source(%d) failed\n",
 			priv->rf_mode);
 		return -EREMOTEIO;
 	}
 
-	ret = xc_SetTVStandard(priv,
-		XC5000_Standard[priv->video_standard].VideoMode,
-		XC5000_Standard[priv->video_standard].AudioMode, 0);
-	if (ret != XC_RESULT_SUCCESS) {
-		printk(KERN_ERR "xc5000: xc_SetTVStandard failed\n");
+	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 != XC_RESULT_SUCCESS) {
+	if (ret != 0) {
 		printk(KERN_ERR "xc5000: xc_Set_IF_frequency(%d) failed\n",
 		       priv->if_khz);
 		return -EIO;
@@ -862,15 +856,15 @@ static int xc5000_is_firmware_loaded(struct dvb_frontend *fe)
 	u16 id;
 
 	ret = xc5000_readreg(priv, XREG_PRODUCT_ID, &id);
-	if (ret == XC_RESULT_SUCCESS) {
+	if (ret == 0) {
 		if (id == XC_PRODUCT_ID_FW_NOT_LOADED)
-			ret = XC_RESULT_RESET_FAILURE;
+			ret = -ENOENT;
 		else
-			ret = XC_RESULT_SUCCESS;
+			ret = 0;
 	}
 
 	dprintk(1, "%s() returns %s id = 0x%x\n", __func__,
-		ret == XC_RESULT_SUCCESS ? "True" : "False", id);
+		ret == 0 ? "True" : "False", id);
 	return ret;
 }
 
@@ -936,19 +930,19 @@ static int xc5000_set_tv_freq(struct dvb_frontend *fe,
 	}
 
 tune_channel:
-	ret = xc_SetSignalSource(priv, priv->rf_mode);
-	if (ret != XC_RESULT_SUCCESS) {
+	ret = xc_set_signal_source(priv, priv->rf_mode);
+	if (ret != 0) {
 		printk(KERN_ERR
-			"xc5000: xc_SetSignalSource(%d) failed\n",
+			"xc5000: xc_set_signal_source(%d) failed\n",
 			priv->rf_mode);
 		return -EREMOTEIO;
 	}
 
-	ret = xc_SetTVStandard(priv,
-		XC5000_Standard[priv->video_standard].VideoMode,
-		XC5000_Standard[priv->video_standard].AudioMode, 0);
-	if (ret != XC_RESULT_SUCCESS) {
-		printk(KERN_ERR "xc5000: xc_SetTVStandard failed\n");
+	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;
 	}
 
@@ -966,7 +960,7 @@ tune_channel:
 			/* PLL is unlocked, force reload of the firmware */
 			dprintk(1, "xc5000: PLL not locked (0x%x).  Reloading...\n",
 				pll_lock_status);
-			if (xc_load_fw_and_init_tuner(fe, 1) != XC_RESULT_SUCCESS) {
+			if (xc_load_fw_and_init_tuner(fe, 1) != 0) {
 				printk(KERN_ERR "xc5000: Unable to reload fw\n");
 				return -EREMOTEIO;
 			}
@@ -993,11 +987,11 @@ static int xc5000_set_radio_freq(struct dvb_frontend *fe,
 	}
 
 	if (priv->radio_input == XC5000_RADIO_FM1)
-		radio_input = FM_Radio_INPUT1;
+		radio_input = FM_RADIO_INPUT1;
 	else if  (priv->radio_input == XC5000_RADIO_FM2)
-		radio_input = FM_Radio_INPUT2;
+		radio_input = FM_RADIO_INPUT2;
 	else if  (priv->radio_input == XC5000_RADIO_FM1_MONO)
-		radio_input = FM_Radio_INPUT1_MONO;
+		radio_input = FM_RADIO_INPUT1_MONO;
 	else {
 		dprintk(1, "%s() unknown radio input %d\n", __func__,
 			priv->radio_input);
@@ -1008,18 +1002,18 @@ static int xc5000_set_radio_freq(struct dvb_frontend *fe,
 
 	priv->rf_mode = XC_RF_MODE_AIR;
 
-	ret = xc_SetTVStandard(priv, XC5000_Standard[radio_input].VideoMode,
-			       XC5000_Standard[radio_input].AudioMode, radio_input);
+	ret = xc_set_tv_standard(priv, xc5000_standard[radio_input].video_mode,
+			       xc5000_standard[radio_input].audio_mode, radio_input);
 
-	if (ret != XC_RESULT_SUCCESS) {
-		printk(KERN_ERR "xc5000: xc_SetTVStandard failed\n");
+	if (ret != 0) {
+		printk(KERN_ERR "xc5000: xc_set_tv_standard failed\n");
 		return -EREMOTEIO;
 	}
 
-	ret = xc_SetSignalSource(priv, priv->rf_mode);
-	if (ret != XC_RESULT_SUCCESS) {
+	ret = xc_set_signal_source(priv, priv->rf_mode);
+	if (ret != 0) {
 		printk(KERN_ERR
-			"xc5000: xc_SetSignalSource(%d) failed\n",
+			"xc5000: xc_set_signal_source(%d) failed\n",
 			priv->rf_mode);
 		return -EREMOTEIO;
 	}
@@ -1044,7 +1038,7 @@ static int xc5000_set_analog_params(struct dvb_frontend *fe,
 	if (priv->i2c_props.adap == NULL)
 		return -EINVAL;
 
-	if (xc_load_fw_and_init_tuner(fe, 0) != XC_RESULT_SUCCESS) {
+	if (xc_load_fw_and_init_tuner(fe, 0) != 0) {
 		dprintk(1, "Unable to load firmware and init tuner\n");
 		return -EINVAL;
 	}
@@ -1105,23 +1099,25 @@ static int xc5000_get_status(struct dvb_frontend *fe, u32 *status)
 static int xc_load_fw_and_init_tuner(struct dvb_frontend *fe, int force)
 {
 	struct xc5000_priv *priv = fe->tuner_priv;
-	int ret = XC_RESULT_SUCCESS;
+	int ret = 0;
 	u16 pll_lock_status;
 	u16 fw_ck;
 
-	if (force || xc5000_is_firmware_loaded(fe) != XC_RESULT_SUCCESS) {
+	cancel_delayed_work(&priv->timer_sleep);
+
+	if (force || xc5000_is_firmware_loaded(fe) != 0) {
 
 fw_retry:
 
 		ret = xc5000_fwupload(fe);
-		if (ret != XC_RESULT_SUCCESS)
+		if (ret != 0)
 			return ret;
 
 		msleep(20);
 
 		if (priv->fw_checksum_supported) {
 			if (xc5000_readreg(priv, XREG_FW_CHECKSUM, &fw_ck)
-			    != XC_RESULT_SUCCESS) {
+			    != 0) {
 				dprintk(1, "%s() FW checksum reading failed.\n",
 					__func__);
 				goto fw_retry;
@@ -1137,7 +1133,7 @@ fw_retry:
 		/* Start the tuner self-calibration process */
 		ret |= xc_initialize(priv);
 
-		if (ret != XC_RESULT_SUCCESS)
+		if (ret != 0)
 			goto fw_retry;
 
 		/* Wait for calibration to complete.
@@ -1145,10 +1141,10 @@ fw_retry:
 		 * I2C transactions until calibration is complete.  This way we
 		 * don't have to rely on clock stretching working.
 		 */
-		xc_wait(100);
+		msleep(100);
 
 		if (priv->init_status_supported) {
-			if (xc5000_readreg(priv, XREG_INIT_STATUS, &fw_ck) != XC_RESULT_SUCCESS) {
+			if (xc5000_readreg(priv, XREG_INIT_STATUS, &fw_ck) != 0) {
 				dprintk(1, "%s() FW failed reading init status.\n",
 					__func__);
 				goto fw_retry;
@@ -1177,27 +1173,39 @@ fw_retry:
 	return ret;
 }
 
-static int xc5000_sleep(struct dvb_frontend *fe)
+static void xc5000_do_timer_sleep(struct work_struct *timer_sleep)
 {
+	struct xc5000_priv *priv =container_of(timer_sleep, struct xc5000_priv,
+					       timer_sleep.work);
+	struct dvb_frontend *fe = priv->fe;
 	int ret;
 
 	dprintk(1, "%s()\n", __func__);
 
-	/* Avoid firmware reload on slow devices */
-	if (no_poweroff)
-		return 0;
-
 	/* According to Xceive technical support, the "powerdown" register
 	   was removed in newer versions of the firmware.  The "supported"
 	   way to sleep the tuner is to pull the reset pin low for 10ms */
-	ret = xc5000_TunerReset(fe);
-	if (ret != XC_RESULT_SUCCESS) {
+	ret = xc5000_tuner_reset(fe);
+	if (ret != 0)
 		printk(KERN_ERR
 			"xc5000: %s() unable to shutdown tuner\n",
 			__func__);
-		return -EREMOTEIO;
-	} else
-		return XC_RESULT_SUCCESS;
+}
+
+static int xc5000_sleep(struct dvb_frontend *fe)
+{
+	struct xc5000_priv *priv = fe->tuner_priv;
+
+	dprintk(1, "%s()\n", __func__);
+
+	/* Avoid firmware reload on slow devices */
+	if (no_poweroff)
+		return 0;
+
+	schedule_delayed_work(&priv->timer_sleep,
+			      msecs_to_jiffies(XC5000_SLEEP_TIME));
+
+	return 0;
 }
 
 static int xc5000_init(struct dvb_frontend *fe)
@@ -1205,7 +1213,7 @@ static int xc5000_init(struct dvb_frontend *fe)
 	struct xc5000_priv *priv = fe->tuner_priv;
 	dprintk(1, "%s()\n", __func__);
 
-	if (xc_load_fw_and_init_tuner(fe, 0) != XC_RESULT_SUCCESS) {
+	if (xc_load_fw_and_init_tuner(fe, 0) != 0) {
 		printk(KERN_ERR "xc5000: Unable to initialise tuner\n");
 		return -EREMOTEIO;
 	}
@@ -1224,8 +1232,10 @@ static int xc5000_release(struct dvb_frontend *fe)
 
 	mutex_lock(&xc5000_list_mutex);
 
-	if (priv)
+	if (priv) {
+		cancel_delayed_work(&priv->timer_sleep);
 		hybrid_tuner_release_state(priv);
+	}
 
 	mutex_unlock(&xc5000_list_mutex);
 
@@ -1297,6 +1307,8 @@ struct dvb_frontend *xc5000_attach(struct dvb_frontend *fe,
 		/* new tuner instance */
 		priv->bandwidth = 6000000;
 		fe->tuner_priv = priv;
+		priv->fe = fe;
+		INIT_DELAYED_WORK(&priv->timer_sleep, xc5000_do_timer_sleep);
 		break;
 	default:
 		/* existing tuner instance */
@@ -1327,7 +1339,7 @@ struct dvb_frontend *xc5000_attach(struct dvb_frontend *fe,
 	/* Check if firmware has been loaded. It is possible that another
 	   instance of the driver has loaded the firmware.
 	 */
-	if (xc5000_readreg(priv, XREG_PRODUCT_ID, &id) != XC_RESULT_SUCCESS)
+	if (xc5000_readreg(priv, XREG_PRODUCT_ID, &id) != 0)
 		goto fail;
 
 	switch (id) {
diff --git a/drivers/media/usb/au0828/au0828-dvb.c b/drivers/media/usb/au0828/au0828-dvb.c
index 4ae8b107464..d8b5d948027 100644
--- a/drivers/media/usb/au0828/au0828-dvb.c
+++ b/drivers/media/usb/au0828/au0828-dvb.c
@@ -114,16 +114,20 @@ static void urb_completion(struct urb *purb)
 	int ptype = usb_pipetype(purb->pipe);
 	unsigned char *ptr;
 
-	dprintk(2, "%s()\n", __func__);
+	dprintk(2, "%s: %d\n", __func__, purb->actual_length);
 
-	if (!dev)
+	if (!dev) {
+		dprintk(2, "%s: no dev!\n", __func__);
 		return;
+	}
 
-	if (dev->urb_streaming == 0)
+	if (dev->urb_streaming == 0) {
+		dprintk(2, "%s: not streaming!\n", __func__);
 		return;
+	}
 
 	if (ptype != PIPE_BULK) {
-		printk(KERN_ERR "%s() Unsupported URB type %d\n",
+		printk(KERN_ERR "%s: Unsupported URB type %d\n",
 		       __func__, ptype);
 		return;
 	}
@@ -252,8 +256,6 @@ static void au0828_stop_transport(struct au0828_dev *dev, int full_stop)
 	au0828_write(dev, 0x60b, 0x00);
 }
 
-
-
 static int au0828_dvb_start_feed(struct dvb_demux_feed *feed)
 {
 	struct dvb_demux *demux = feed->demux;
@@ -296,6 +298,8 @@ static int au0828_dvb_stop_feed(struct dvb_demux_feed *feed)
 	dprintk(1, "%s()\n", __func__);
 
 	if (dvb) {
+		cancel_work_sync(&dev->restart_streaming);
+
 		mutex_lock(&dvb->lock);
 		dvb->stop_count++;
 		dprintk(1, "%s(), start_count: %d, stop_count: %d\n", __func__,
@@ -338,6 +342,41 @@ static void au0828_restart_dvb_streaming(struct work_struct *work)
 	mutex_unlock(&dvb->lock);
 }
 
+static int au0828_set_frontend(struct dvb_frontend *fe)
+{
+	struct au0828_dev *dev = fe->dvb->priv;
+	struct au0828_dvb *dvb = &dev->dvb;
+	int ret, was_streaming;
+
+	mutex_lock(&dvb->lock);
+	was_streaming = dev->urb_streaming;
+	if (was_streaming) {
+		au0828_stop_transport(dev, 1);
+
+		/*
+		 * We can't hold a mutex here, as the restart_streaming
+		 * kthread may also hold it.
+		 */
+		mutex_unlock(&dvb->lock);
+		cancel_work_sync(&dev->restart_streaming);
+		mutex_lock(&dvb->lock);
+
+		stop_urb_transfer(dev);
+	}
+	mutex_unlock(&dvb->lock);
+
+	ret = dvb->set_frontend(fe);
+
+	if (was_streaming) {
+		mutex_lock(&dvb->lock);
+		au0828_start_transport(dev);
+		start_urb_transfer(dev);
+		mutex_unlock(&dvb->lock);
+	}
+
+	return ret;
+}
+
 static int dvb_register(struct au0828_dev *dev)
 {
 	struct au0828_dvb *dvb = &dev->dvb;
@@ -382,6 +421,10 @@ static int dvb_register(struct au0828_dev *dev)
 		goto fail_frontend;
 	}
 
+	/* Hook dvb frontend */
+	dvb->set_frontend = dvb->frontend->ops.set_frontend;
+	dvb->frontend->ops.set_frontend = au0828_set_frontend;
+
 	/* register demux stuff */
 	dvb->demux.dmx.capabilities =
 		DMX_TS_FILTERING | DMX_SECTION_FILTERING |
@@ -471,6 +514,8 @@ void au0828_dvb_unregister(struct au0828_dev *dev)
 	if (dvb->frontend == NULL)
 		return;
 
+	cancel_work_sync(&dev->restart_streaming);
+
 	dvb_net_release(&dvb->net);
 	dvb->demux.dmx.remove_frontend(&dvb->demux.dmx, &dvb->fe_mem);
 	dvb->demux.dmx.remove_frontend(&dvb->demux.dmx, &dvb->fe_hw);
diff --git a/drivers/media/usb/au0828/au0828-video.c b/drivers/media/usb/au0828/au0828-video.c
index f6154546b5c..9038194513c 100644
--- a/drivers/media/usb/au0828/au0828-video.c
+++ b/drivers/media/usb/au0828/au0828-video.c
@@ -1109,7 +1109,7 @@ static void au0828_init_tuner(struct au0828_dev *dev)
 	/* If we've never sent the standard in tuner core, do so now.
 	   We don't do this at device probe because we don't want to
 	   incur the cost of a firmware load */
-	v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_std, dev->std);
+	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_std, dev->std);
 	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_frequency, &f);
 	i2c_gate_ctrl(dev, 0);
 }
@@ -1368,7 +1368,7 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id norm)
 	   have to make the au0828 bridge adjust the size of its capture
 	   buffer, which is currently hardcoded at 720x480 */
 
-	v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_std, norm);
+	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_std, norm);
 
 	i2c_gate_ctrl(dev, 0);
 
diff --git a/drivers/media/usb/au0828/au0828.h b/drivers/media/usb/au0828/au0828.h
index 5439772c155..7112b9d956f 100644
--- a/drivers/media/usb/au0828/au0828.h
+++ b/drivers/media/usb/au0828/au0828.h
@@ -104,6 +104,8 @@ struct au0828_dvb {
 	int feeding;
 	int start_count;
 	int stop_count;
+
+	int (*set_frontend)(struct dvb_frontend *fe);
 };
 
 enum au0828_stream_state {
diff --git a/drivers/media/usb/cx231xx/cx231xx-417.c b/drivers/media/usb/cx231xx/cx231xx-417.c
index 2f63029e7a3..30a0c69fb42 100644
--- a/drivers/media/usb/cx231xx/cx231xx-417.c
+++ b/drivers/media/usb/cx231xx/cx231xx-417.c
@@ -1516,7 +1516,7 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id id)
 		dev->ts1.height = 576;
 		cx2341x_handler_set_50hz(&dev->mpeg_ctrl_handler, true);
 	}
-	call_all(dev, core, s_std, dev->norm);
+	call_all(dev, video, s_std, dev->norm);
 	/* do mode control overrides */
 	cx231xx_do_mode_ctrl_overrides(dev);
 
diff --git a/drivers/media/usb/cx231xx/cx231xx-video.c b/drivers/media/usb/cx231xx/cx231xx-video.c
index 99062610171..1f8751379e2 100644
--- a/drivers/media/usb/cx231xx/cx231xx-video.c
+++ b/drivers/media/usb/cx231xx/cx231xx-video.c
@@ -1009,7 +1009,7 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id norm)
 	dev->width = 720;
 	dev->height = (dev->norm & V4L2_STD_625_50) ? 576 : 480;
 
-	call_all(dev, core, s_std, dev->norm);
+	call_all(dev, video, s_std, dev->norm);
 
 	/* We need to reset basic properties in the decoder related to
 	   resolution (since a standard change effects things like the number
@@ -1108,7 +1108,7 @@ int cx231xx_s_input(struct file *file, void *priv, unsigned int i)
 		/* There's a tuner, so reset the standard and put it on the
 		   last known frequency (since it was probably powered down
 		   until now */
-		call_all(dev, core, s_std, dev->norm);
+		call_all(dev, video, s_std, dev->norm);
 	}
 
 	return 0;
@@ -2099,7 +2099,7 @@ int cx231xx_register_analog_devices(struct cx231xx *dev)
 	/* Set the initial input */
 	video_mux(dev, dev->video_input);
 
-	call_all(dev, core, s_std, dev->norm);
+	call_all(dev, video, s_std, dev->norm);
 
 	v4l2_ctrl_handler_init(&dev->ctrl_handler, 10);
 	v4l2_ctrl_handler_init(&dev->radio_ctrl_handler, 5);
diff --git a/drivers/media/usb/dvb-usb-v2/Makefile b/drivers/media/usb/dvb-usb-v2/Makefile
index 7407b8338cc..bc38f03394c 100644
--- a/drivers/media/usb/dvb-usb-v2/Makefile
+++ b/drivers/media/usb/dvb-usb-v2/Makefile
@@ -41,4 +41,3 @@ ccflags-y += -I$(srctree)/drivers/media/dvb-core
 ccflags-y += -I$(srctree)/drivers/media/dvb-frontends
 ccflags-y += -I$(srctree)/drivers/media/tuners
 ccflags-y += -I$(srctree)/drivers/media/common
-ccflags-y += -I$(srctree)/drivers/staging/media/rtl2832u_sdr
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 de02db802ac..e3558061893 100644
--- a/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c
+++ b/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c
@@ -399,7 +399,7 @@ static int dvb_usb_stop_feed(struct dvb_demux_feed *dvbdmxfeed)
 
 	/* clear 'streaming' status bit */
 	clear_bit(ADAP_STREAMING, &adap->state_bits);
-	smp_mb__after_clear_bit();
+	smp_mb__after_atomic();
 	wake_up_bit(&adap->state_bits, ADAP_STREAMING);
 skip_feed_stop:
 
@@ -550,7 +550,7 @@ static int dvb_usb_fe_init(struct dvb_frontend *fe)
 err:
 	if (!adap->suspend_resume_active) {
 		clear_bit(ADAP_INIT, &adap->state_bits);
-		smp_mb__after_clear_bit();
+		smp_mb__after_atomic();
 		wake_up_bit(&adap->state_bits, ADAP_INIT);
 	}
 
@@ -591,7 +591,7 @@ err:
 	if (!adap->suspend_resume_active) {
 		adap->active_fe = -1;
 		clear_bit(ADAP_SLEEP, &adap->state_bits);
-		smp_mb__after_clear_bit();
+		smp_mb__after_atomic();
 		wake_up_bit(&adap->state_bits, ADAP_SLEEP);
 	}
 
diff --git a/drivers/media/usb/dvb-usb-v2/rtl28xxu.c b/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
index 61d196e8b3a..a676e445284 100644
--- a/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
+++ b/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
@@ -24,7 +24,6 @@
 
 #include "rtl2830.h"
 #include "rtl2832.h"
-#include "rtl2832_sdr.h"
 
 #include "qt1010.h"
 #include "mt2060.h"
@@ -36,6 +35,45 @@
 #include "tua9001.h"
 #include "r820t.h"
 
+/*
+ * RTL2832_SDR module is in staging. That logic is added in order to avoid any
+ * hard dependency to drivers/staging/ directory as we want compile mainline
+ * driver even whole staging directory is missing.
+ */
+#include <media/v4l2-subdev.h>
+
+#if IS_ENABLED(CONFIG_DVB_RTL2832_SDR)
+struct dvb_frontend *rtl2832_sdr_attach(struct dvb_frontend *fe,
+	struct i2c_adapter *i2c, const struct rtl2832_config *cfg,
+	struct v4l2_subdev *sd);
+#else
+static inline struct dvb_frontend *rtl2832_sdr_attach(struct dvb_frontend *fe,
+	struct i2c_adapter *i2c, const struct rtl2832_config *cfg,
+	struct v4l2_subdev *sd)
+{
+	return NULL;
+}
+#endif
+
+#ifdef CONFIG_MEDIA_ATTACH
+#define dvb_attach_sdr(FUNCTION, ARGS...) ({ \
+	void *__r = NULL; \
+	typeof(&FUNCTION) __a = symbol_request(FUNCTION); \
+	if (__a) { \
+		__r = (void *) __a(ARGS); \
+		if (__r == NULL) \
+			symbol_put(FUNCTION); \
+	} \
+	__r; \
+})
+
+#else
+#define dvb_attach_sdr(FUNCTION, ARGS...) ({ \
+	FUNCTION(ARGS); \
+})
+
+#endif
+
 static int rtl28xxu_disable_rc;
 module_param_named(disable_rc, rtl28xxu_disable_rc, int, 0644);
 MODULE_PARM_DESC(disable_rc, "disable RTL2832U remote controller");
@@ -908,7 +946,7 @@ static int rtl2832u_tuner_attach(struct dvb_usb_adapter *adap)
 				adap->fe[0]->ops.tuner_ops.get_rf_strength;
 
 		/* attach SDR */
-		dvb_attach(rtl2832_sdr_attach, adap->fe[0], &d->i2c_adap,
+		dvb_attach_sdr(rtl2832_sdr_attach, adap->fe[0], &d->i2c_adap,
 				&rtl28xxu_rtl2832_fc0012_config, NULL);
 		break;
 	case TUNER_RTL2832_FC0013:
@@ -920,7 +958,7 @@ static int rtl2832u_tuner_attach(struct dvb_usb_adapter *adap)
 				adap->fe[0]->ops.tuner_ops.get_rf_strength;
 
 		/* attach SDR */
-		dvb_attach(rtl2832_sdr_attach, adap->fe[0], &d->i2c_adap,
+		dvb_attach_sdr(rtl2832_sdr_attach, adap->fe[0], &d->i2c_adap,
 				&rtl28xxu_rtl2832_fc0013_config, NULL);
 		break;
 	case TUNER_RTL2832_E4000: {
@@ -951,7 +989,7 @@ static int rtl2832u_tuner_attach(struct dvb_usb_adapter *adap)
 			i2c_set_adapdata(i2c_adap_internal, d);
 
 			/* attach SDR */
-			dvb_attach(rtl2832_sdr_attach, adap->fe[0],
+			dvb_attach_sdr(rtl2832_sdr_attach, adap->fe[0],
 					i2c_adap_internal,
 					&rtl28xxu_rtl2832_e4000_config, sd);
 		}
@@ -982,7 +1020,7 @@ static int rtl2832u_tuner_attach(struct dvb_usb_adapter *adap)
 				adap->fe[0]->ops.tuner_ops.get_rf_strength;
 
 		/* attach SDR */
-		dvb_attach(rtl2832_sdr_attach, adap->fe[0], &d->i2c_adap,
+		dvb_attach_sdr(rtl2832_sdr_attach, adap->fe[0], &d->i2c_adap,
 				&rtl28xxu_rtl2832_r820t_config, NULL);
 		break;
 	case TUNER_RTL2832_R828D:
@@ -1499,6 +1537,12 @@ static const struct usb_device_id rtl28xxu_id_table[] = {
 		&rtl2832u_props, "Crypto ReDi PC 50 A", NULL) },
 	{ DVB_USB_DEVICE(USB_VID_KYE, 0x707f,
 		&rtl2832u_props, "Genius TVGo DVB-T03", NULL) },
+	{ DVB_USB_DEVICE(USB_VID_KWORLD_2, 0xd395,
+		&rtl2832u_props, "Peak DVB-T USB", NULL) },
+	{ DVB_USB_DEVICE(USB_VID_KWORLD_2, USB_PID_SVEON_STV20_RTL2832U,
+		&rtl2832u_props, "Sveon STV20", NULL) },
+	{ DVB_USB_DEVICE(USB_VID_KWORLD_2, USB_PID_SVEON_STV27,
+		&rtl2832u_props, "Sveon STV27", NULL) },
 
 	/* RTL2832P devices: */
 	{ DVB_USB_DEVICE(USB_VID_HANFTEK, 0x0131,
diff --git a/drivers/media/usb/dvb-usb/az6027.c b/drivers/media/usb/dvb-usb/az6027.c
index c11138ebf6f..0df52ab32a7 100644
--- a/drivers/media/usb/dvb-usb/az6027.c
+++ b/drivers/media/usb/dvb-usb/az6027.c
@@ -1088,6 +1088,7 @@ static struct usb_device_id az6027_usb_table[] = {
 	{ USB_DEVICE(USB_VID_TECHNISAT, USB_PID_TECHNISAT_USB2_HDCI_V1) },
 	{ USB_DEVICE(USB_VID_TECHNISAT, USB_PID_TECHNISAT_USB2_HDCI_V2) },
 	{ USB_DEVICE(USB_VID_ELGATO, USB_PID_ELGATO_EYETV_SAT) },
+	{ USB_DEVICE(USB_VID_ELGATO, USB_PID_ELGATO_EYETV_SAT_V2) },
 	{ },
 };
 
@@ -1136,7 +1137,7 @@ static struct dvb_usb_device_properties az6027_properties = {
 
 	.i2c_algo         = &az6027_i2c_algo,
 
-	.num_device_descs = 6,
+	.num_device_descs = 7,
 	.devices = {
 		{
 			.name = "AZUREWAVE DVB-S/S2 USB2.0 (AZ6027)",
@@ -1162,6 +1163,10 @@ static struct dvb_usb_device_properties az6027_properties = {
 			.name = "Elgato EyeTV Sat",
 			.cold_ids = { &az6027_usb_table[5], NULL },
 			.warm_ids = { NULL },
+		}, {
+			.name = "Elgato EyeTV Sat",
+			.cold_ids = { &az6027_usb_table[6], NULL },
+			.warm_ids = { NULL },
 		},
 		{ NULL },
 	}
diff --git a/drivers/media/usb/dvb-usb/dib0700.h b/drivers/media/usb/dvb-usb/dib0700.h
index 637b6123f39..927617d9561 100644
--- a/drivers/media/usb/dvb-usb/dib0700.h
+++ b/drivers/media/usb/dvb-usb/dib0700.h
@@ -59,7 +59,7 @@ extern int dib0700_set_gpio(struct dvb_usb_device *, enum dib07x0_gpios gpio, u8
 extern int dib0700_ctrl_clock(struct dvb_usb_device *d, u32 clk_MHz, u8 clock_out_gp3);
 extern int dib0700_ctrl_rd(struct dvb_usb_device *d, u8 *tx, u8 txlen, u8 *rx, u8 rxlen);
 extern int dib0700_download_firmware(struct usb_device *udev, const struct firmware *fw);
-extern int dib0700_rc_setup(struct dvb_usb_device *d);
+extern int dib0700_rc_setup(struct dvb_usb_device *d, struct usb_interface *intf);
 extern int dib0700_streaming_ctrl(struct dvb_usb_adapter *adap, int onoff);
 extern struct i2c_algorithm dib0700_i2c_algo;
 extern int dib0700_identify_state(struct usb_device *udev, struct dvb_usb_device_properties *props,
diff --git a/drivers/media/usb/dvb-usb/dib0700_core.c b/drivers/media/usb/dvb-usb/dib0700_core.c
index bf2a908d74c..c14285fa827 100644
--- a/drivers/media/usb/dvb-usb/dib0700_core.c
+++ b/drivers/media/usb/dvb-usb/dib0700_core.c
@@ -754,17 +754,20 @@ resubmit:
 	usb_submit_urb(purb, GFP_ATOMIC);
 }
 
-int dib0700_rc_setup(struct dvb_usb_device *d)
+int dib0700_rc_setup(struct dvb_usb_device *d, struct usb_interface *intf)
 {
 	struct dib0700_state *st = d->priv;
 	struct urb *purb;
-	int ret;
+	const struct usb_endpoint_descriptor *e;
+	int ret, rc_ep = 1;
+	unsigned int pipe = 0;
 
 	/* Poll-based. Don't initialize bulk mode */
-	if (st->fw_version < 0x10200)
+	if (st->fw_version < 0x10200 || !intf)
 		return 0;
 
 	/* Starting in firmware 1.20, the RC info is provided on a bulk pipe */
+
 	purb = usb_alloc_urb(0, GFP_KERNEL);
 	if (purb == NULL) {
 		err("rc usb alloc urb failed");
@@ -779,9 +782,35 @@ int dib0700_rc_setup(struct dvb_usb_device *d)
 	}
 
 	purb->status = -EINPROGRESS;
-	usb_fill_bulk_urb(purb, d->udev, usb_rcvbulkpipe(d->udev, 1),
-			  purb->transfer_buffer, RC_MSG_SIZE_V1_20,
-			  dib0700_rc_urb_completion, d);
+
+	/*
+	 * Some devices like the Hauppauge NovaTD model 52009 use an interrupt
+	 * endpoint, while others use a bulk one.
+	 */
+	e = &intf->altsetting[0].endpoint[rc_ep].desc;
+	if (usb_endpoint_dir_in(e)) {
+		if (usb_endpoint_xfer_bulk(e)) {
+			pipe = usb_rcvbulkpipe(d->udev, rc_ep);
+			usb_fill_bulk_urb(purb, d->udev, pipe,
+					  purb->transfer_buffer,
+					  RC_MSG_SIZE_V1_20,
+					  dib0700_rc_urb_completion, d);
+
+		} else if (usb_endpoint_xfer_int(e)) {
+			pipe = usb_rcvintpipe(d->udev, rc_ep);
+			usb_fill_int_urb(purb, d->udev, pipe,
+					  purb->transfer_buffer,
+					  RC_MSG_SIZE_V1_20,
+					  dib0700_rc_urb_completion, d, 1);
+		}
+	}
+
+	if (!pipe) {
+		err("There's no endpoint for remote controller");
+		kfree(purb->transfer_buffer);
+		usb_free_urb(purb);
+		return 0;
+	}
 
 	ret = usb_submit_urb(purb, GFP_ATOMIC);
 	if (ret) {
@@ -820,7 +849,7 @@ static int dib0700_probe(struct usb_interface *intf,
 			else
 				dev->props.rc.core.bulk_mode = false;
 
-			dib0700_rc_setup(dev);
+			dib0700_rc_setup(dev, intf);
 
 			return 0;
 		}
diff --git a/drivers/media/usb/dvb-usb/dib0700_devices.c b/drivers/media/usb/dvb-usb/dib0700_devices.c
index 829323e42ca..10e0db8d185 100644
--- a/drivers/media/usb/dvb-usb/dib0700_devices.c
+++ b/drivers/media/usb/dvb-usb/dib0700_devices.c
@@ -514,7 +514,7 @@ static int dib0700_rc_query_old_firmware(struct dvb_usb_device *d)
 
 	/* info("%d: %2X %2X %2X %2X",dvb_usb_dib0700_ir_proto,(int)key[3-2],(int)key[3-3],(int)key[3-1],(int)key[3]);  */
 
-	dib0700_rc_setup(d); /* reset ir sensor data to prevent false events */
+	dib0700_rc_setup(d, NULL); /* reset ir sensor data to prevent false events */
 
 	d->last_event = 0;
 	switch (d->props.rc.core.protocol) {
diff --git a/drivers/media/usb/dvb-usb/technisat-usb2.c b/drivers/media/usb/dvb-usb/technisat-usb2.c
index 98d24aefb64..d947e037900 100644
--- a/drivers/media/usb/dvb-usb/technisat-usb2.c
+++ b/drivers/media/usb/dvb-usb/technisat-usb2.c
@@ -214,10 +214,10 @@ static void technisat_usb2_frontend_reset(struct usb_device *udev)
 
 /* LED control */
 enum technisat_usb2_led_state {
-	LED_OFF,
-	LED_BLINK,
-	LED_ON,
-	LED_UNDEFINED
+	TECH_LED_OFF,
+	TECH_LED_BLINK,
+	TECH_LED_ON,
+	TECH_LED_UNDEFINED
 };
 
 static int technisat_usb2_set_led(struct dvb_usb_device *d, int red, enum technisat_usb2_led_state state)
@@ -229,14 +229,14 @@ static int technisat_usb2_set_led(struct dvb_usb_device *d, int red, enum techni
 		0
 	};
 
-	if (disable_led_control && state != LED_OFF)
+	if (disable_led_control && state != TECH_LED_OFF)
 		return 0;
 
 	switch (state) {
-	case LED_ON:
+	case TECH_LED_ON:
 		led[1] = 0x82;
 		break;
-	case LED_BLINK:
+	case TECH_LED_BLINK:
 		led[1] = 0x82;
 		if (red) {
 			led[2] = 0x02;
@@ -251,7 +251,7 @@ static int technisat_usb2_set_led(struct dvb_usb_device *d, int red, enum techni
 		break;
 
 	default:
-	case LED_OFF:
+	case TECH_LED_OFF:
 		led[1] = 0x80;
 		break;
 	}
@@ -310,11 +310,11 @@ static void technisat_usb2_green_led_control(struct work_struct *work)
 				goto schedule;
 
 			if (ber > 1000)
-				technisat_usb2_set_led(state->dev, 0, LED_BLINK);
+				technisat_usb2_set_led(state->dev, 0, TECH_LED_BLINK);
 			else
-				technisat_usb2_set_led(state->dev, 0, LED_ON);
+				technisat_usb2_set_led(state->dev, 0, TECH_LED_ON);
 		} else
-			technisat_usb2_set_led(state->dev, 0, LED_OFF);
+			technisat_usb2_set_led(state->dev, 0, TECH_LED_OFF);
 	}
 
 schedule:
@@ -365,9 +365,9 @@ static int technisat_usb2_power_ctrl(struct dvb_usb_device *d, int level)
 		return 0;
 
 	/* green led is turned off in any case - will be turned on when tuning */
-	technisat_usb2_set_led(d, 0, LED_OFF);
+	technisat_usb2_set_led(d, 0, TECH_LED_OFF);
 	/* red led is turned on all the time */
-	technisat_usb2_set_led(d, 1, LED_ON);
+	technisat_usb2_set_led(d, 1, TECH_LED_ON);
 	return 0;
 }
 
@@ -667,7 +667,7 @@ static int technisat_usb2_rc_query(struct dvb_usb_device *d)
 		return 0;
 
 	if (!disable_led_control)
-		technisat_usb2_set_led(d, 1, LED_BLINK);
+		technisat_usb2_set_led(d, 1, TECH_LED_BLINK);
 
 	return 0;
 }
diff --git a/drivers/media/usb/em28xx/Kconfig b/drivers/media/usb/em28xx/Kconfig
index d23a912096f..f5d7198753c 100644
--- a/drivers/media/usb/em28xx/Kconfig
+++ b/drivers/media/usb/em28xx/Kconfig
@@ -57,6 +57,8 @@ config VIDEO_EM28XX_DVB
 	select DVB_M88DS3103 if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_M88TS2022 if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_DRX39XYJ if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_SI2168 if MEDIA_SUBDRV_AUTOSELECT
+	select MEDIA_TUNER_SI2157 if MEDIA_SUBDRV_AUTOSELECT
 	---help---
 	  This adds support for DVB cards based on the
 	  Empiatech em28xx chips.
diff --git a/drivers/media/usb/em28xx/em28xx-audio.c b/drivers/media/usb/em28xx/em28xx-audio.c
index 342490f44ed..e881ef7b644 100644
--- a/drivers/media/usb/em28xx/em28xx-audio.c
+++ b/drivers/media/usb/em28xx/em28xx-audio.c
@@ -92,7 +92,7 @@ static void em28xx_audio_isocirq(struct urb *urb)
 
 	if (dev->disconnected) {
 		dprintk("device disconnected while streaming. URB status=%d.\n", urb->status);
-		atomic_set(&dev->stream_started, 0);
+		atomic_set(&dev->adev.stream_started, 0);
 		return;
 	}
 
@@ -109,7 +109,7 @@ static void em28xx_audio_isocirq(struct urb *urb)
 		break;
 	}
 
-	if (atomic_read(&dev->stream_started) == 0)
+	if (atomic_read(&dev->adev.stream_started) == 0)
 		return;
 
 	if (dev->adev.capture_pcm_substream) {
@@ -185,7 +185,7 @@ static int em28xx_init_audio_isoc(struct em28xx *dev)
 			em28xx_errdev("submit of audio urb failed (error=%i)\n",
 				      errCode);
 			em28xx_deinit_isoc_audio(dev);
-			atomic_set(&dev->stream_started, 0);
+			atomic_set(&dev->adev.stream_started, 0);
 			return errCode;
 		}
 
@@ -332,9 +332,9 @@ static int snd_em28xx_pcm_close(struct snd_pcm_substream *substream)
 	dev->mute = 1;
 	mutex_lock(&dev->lock);
 	dev->adev.users--;
-	if (atomic_read(&dev->stream_started) > 0) {
-		atomic_set(&dev->stream_started, 0);
-		schedule_work(&dev->wq_trigger);
+	if (atomic_read(&dev->adev.stream_started) > 0) {
+		atomic_set(&dev->adev.stream_started, 0);
+		schedule_work(&dev->adev.wq_trigger);
 	}
 
 	em28xx_audio_analog_set(dev);
@@ -381,12 +381,13 @@ static int snd_em28xx_hw_capture_params(struct snd_pcm_substream *substream,
 static int snd_em28xx_hw_capture_free(struct snd_pcm_substream *substream)
 {
 	struct em28xx *dev = snd_pcm_substream_chip(substream);
+	struct em28xx_audio *adev = &dev->adev;
 
 	dprintk("Stop capture, if needed\n");
 
-	if (atomic_read(&dev->stream_started) > 0) {
-		atomic_set(&dev->stream_started, 0);
-		schedule_work(&dev->wq_trigger);
+	if (atomic_read(&adev->stream_started) > 0) {
+		atomic_set(&adev->stream_started, 0);
+		schedule_work(&adev->wq_trigger);
 	}
 
 	return 0;
@@ -407,9 +408,11 @@ static int snd_em28xx_prepare(struct snd_pcm_substream *substream)
 
 static void audio_trigger(struct work_struct *work)
 {
-	struct em28xx *dev = container_of(work, struct em28xx, wq_trigger);
+	struct em28xx_audio *adev =
+			    container_of(work, struct em28xx_audio, wq_trigger);
+	struct em28xx *dev = container_of(adev, struct em28xx, adev);
 
-	if (atomic_read(&dev->stream_started)) {
+	if (atomic_read(&adev->stream_started)) {
 		dprintk("starting capture");
 		em28xx_init_audio_isoc(dev);
 	} else {
@@ -431,17 +434,17 @@ static int snd_em28xx_capture_trigger(struct snd_pcm_substream *substream,
 	case SNDRV_PCM_TRIGGER_PAUSE_RELEASE: /* fall through */
 	case SNDRV_PCM_TRIGGER_RESUME: /* fall through */
 	case SNDRV_PCM_TRIGGER_START:
-		atomic_set(&dev->stream_started, 1);
+		atomic_set(&dev->adev.stream_started, 1);
 		break;
 	case SNDRV_PCM_TRIGGER_PAUSE_PUSH: /* fall through */
 	case SNDRV_PCM_TRIGGER_SUSPEND: /* fall through */
 	case SNDRV_PCM_TRIGGER_STOP:
-		atomic_set(&dev->stream_started, 0);
+		atomic_set(&dev->adev.stream_started, 0);
 		break;
 	default:
 		retval = -EINVAL;
 	}
-	schedule_work(&dev->wq_trigger);
+	schedule_work(&dev->adev.wq_trigger);
 	return retval;
 }
 
@@ -928,7 +931,7 @@ static int em28xx_audio_init(struct em28xx *dev)
 	strcpy(card->shortname, "Em28xx Audio");
 	strcpy(card->longname, "Empia Em28xx Audio");
 
-	INIT_WORK(&dev->wq_trigger, audio_trigger);
+	INIT_WORK(&adev->wq_trigger, audio_trigger);
 
 	if (dev->audio_mode.ac97 != EM28XX_NO_AC97) {
 		em28xx_cvol_new(card, dev, "Video", AC97_VIDEO);
@@ -983,7 +986,7 @@ static int em28xx_audio_fini(struct em28xx *dev)
 
 	if (dev->adev.sndcard) {
 		snd_card_disconnect(dev->adev.sndcard);
-		flush_work(&dev->wq_trigger);
+		flush_work(&dev->adev.wq_trigger);
 
 		em28xx_audio_free_urb(dev);
 
@@ -1005,7 +1008,7 @@ static int em28xx_audio_suspend(struct em28xx *dev)
 
 	em28xx_info("Suspending audio extension");
 	em28xx_deinit_isoc_audio(dev);
-	atomic_set(&dev->stream_started, 0);
+	atomic_set(&dev->adev.stream_started, 0);
 	return 0;
 }
 
@@ -1019,7 +1022,7 @@ static int em28xx_audio_resume(struct em28xx *dev)
 
 	em28xx_info("Resuming audio extension");
 	/* Nothing to do other than schedule_work() ?? */
-	schedule_work(&dev->wq_trigger);
+	schedule_work(&dev->adev.wq_trigger);
 	return 0;
 }
 
diff --git a/drivers/media/usb/em28xx/em28xx-camera.c b/drivers/media/usb/em28xx/em28xx-camera.c
index 505e0505be0..12d4c0326e3 100644
--- a/drivers/media/usb/em28xx/em28xx-camera.c
+++ b/drivers/media/usb/em28xx/em28xx-camera.c
@@ -330,13 +330,14 @@ int em28xx_init_camera(struct em28xx *dev)
 	char clk_name[V4L2_SUBDEV_NAME_SIZE];
 	struct i2c_client *client = &dev->i2c_client[dev->def_i2c_bus];
 	struct i2c_adapter *adap = &dev->i2c_adap[dev->def_i2c_bus];
+	struct em28xx_v4l2 *v4l2 = dev->v4l2;
 	int ret = 0;
 
 	v4l2_clk_name_i2c(clk_name, sizeof(clk_name),
 			  i2c_adapter_id(adap), client->addr);
-	dev->clk = v4l2_clk_register_fixed(clk_name, "mclk", -EINVAL);
-	if (IS_ERR(dev->clk))
-		return PTR_ERR(dev->clk);
+	v4l2->clk = v4l2_clk_register_fixed(clk_name, "mclk", -EINVAL);
+	if (IS_ERR(v4l2->clk))
+		return PTR_ERR(v4l2->clk);
 
 	switch (dev->em28xx_sensor) {
 	case EM28XX_MT9V011:
@@ -348,8 +349,8 @@ int em28xx_init_camera(struct em28xx *dev)
 			.platform_data = &pdata,
 		};
 
-		dev->sensor_xres = 640;
-		dev->sensor_yres = 480;
+		v4l2->sensor_xres = 640;
+		v4l2->sensor_yres = 480;
 
 		/*
 		 * FIXME: mt9v011 uses I2S speed as xtal clk - at least with
@@ -362,41 +363,41 @@ int em28xx_init_camera(struct em28xx *dev)
 		 */
 		dev->board.xclk = EM28XX_XCLK_FREQUENCY_4_3MHZ;
 		em28xx_write_reg(dev, EM28XX_R0F_XCLK, dev->board.xclk);
-		dev->sensor_xtal = 4300000;
-		pdata.xtal = dev->sensor_xtal;
+		v4l2->sensor_xtal = 4300000;
+		pdata.xtal = v4l2->sensor_xtal;
 		if (NULL ==
-		    v4l2_i2c_new_subdev_board(&dev->v4l2_dev, adap,
+		    v4l2_i2c_new_subdev_board(&dev->v4l2->v4l2_dev, adap,
 					      &mt9v011_info, NULL)) {
 			ret = -ENODEV;
 			break;
 		}
 		/* probably means GRGB 16 bit bayer */
-		dev->vinmode = 0x0d;
-		dev->vinctl = 0x00;
+		v4l2->vinmode = 0x0d;
+		v4l2->vinctl = 0x00;
 
 		break;
 	}
 	case EM28XX_MT9M001:
-		dev->sensor_xres = 1280;
-		dev->sensor_yres = 1024;
+		v4l2->sensor_xres = 1280;
+		v4l2->sensor_yres = 1024;
 
 		em28xx_initialize_mt9m001(dev);
 
 		/* probably means BGGR 16 bit bayer */
-		dev->vinmode = 0x0c;
-		dev->vinctl = 0x00;
+		v4l2->vinmode = 0x0c;
+		v4l2->vinctl = 0x00;
 
 		break;
 	case EM28XX_MT9M111:
-		dev->sensor_xres = 640;
-		dev->sensor_yres = 512;
+		v4l2->sensor_xres = 640;
+		v4l2->sensor_yres = 512;
 
 		dev->board.xclk = EM28XX_XCLK_FREQUENCY_48MHZ;
 		em28xx_write_reg(dev, EM28XX_R0F_XCLK, dev->board.xclk);
 		em28xx_initialize_mt9m111(dev);
 
-		dev->vinmode = 0x0a;
-		dev->vinctl = 0x00;
+		v4l2->vinmode = 0x0a;
+		v4l2->vinctl = 0x00;
 
 		break;
 	case EM28XX_OV2640:
@@ -418,11 +419,11 @@ int em28xx_init_camera(struct em28xx *dev)
 		 * - adjust bridge xclk
 		 * - disable 16 bit (12 bit) output formats on high resolutions
 		 */
-		dev->sensor_xres = 640;
-		dev->sensor_yres = 480;
+		v4l2->sensor_xres = 640;
+		v4l2->sensor_yres = 480;
 
 		subdev =
-		     v4l2_i2c_new_subdev_board(&dev->v4l2_dev, adap,
+		     v4l2_i2c_new_subdev_board(&dev->v4l2->v4l2_dev, adap,
 					       &ov2640_info, NULL);
 		if (NULL == subdev) {
 			ret = -ENODEV;
@@ -437,8 +438,8 @@ int em28xx_init_camera(struct em28xx *dev)
 		/* NOTE: for UXGA=1600x1200 switch to 12MHz */
 		dev->board.xclk = EM28XX_XCLK_FREQUENCY_24MHZ;
 		em28xx_write_reg(dev, EM28XX_R0F_XCLK, dev->board.xclk);
-		dev->vinmode = 0x08;
-		dev->vinctl = 0x00;
+		v4l2->vinmode = 0x08;
+		v4l2->vinctl = 0x00;
 
 		break;
 	}
@@ -448,8 +449,8 @@ int em28xx_init_camera(struct em28xx *dev)
 	}
 
 	if (ret < 0) {
-		v4l2_clk_unregister_fixed(dev->clk);
-		dev->clk = NULL;
+		v4l2_clk_unregister_fixed(v4l2->clk);
+		v4l2->clk = NULL;
 	}
 
 	return ret;
diff --git a/drivers/media/usb/em28xx/em28xx-cards.c b/drivers/media/usb/em28xx/em28xx-cards.c
index 50aa5a5317f..15ad4704555 100644
--- a/drivers/media/usb/em28xx/em28xx-cards.c
+++ b/drivers/media/usb/em28xx/em28xx-cards.c
@@ -467,6 +467,18 @@ static struct em28xx_reg_seq speedlink_vad_laplace_reg_seq[] = {
 	{	-1,			-1,	-1,	-1},
 };
 
+static struct em28xx_reg_seq pctv_292e[] = {
+	{EM2874_R80_GPIO_P0_CTRL,      0xff, 0xff,      0},
+	{0x0d,                         0xff, 0xff,    950},
+	{EM2874_R80_GPIO_P0_CTRL,      0xbd, 0xff,    100},
+	{EM2874_R80_GPIO_P0_CTRL,      0xfd, 0xff,    410},
+	{EM2874_R80_GPIO_P0_CTRL,      0x7d, 0xff,    300},
+	{EM2874_R80_GPIO_P0_CTRL,      0x7c, 0xff,     60},
+	{0x0d,                         0x42, 0xff,     50},
+	{EM2874_R5F_TS_ENABLE,         0x85, 0xff,      0},
+	{-1,                             -1,   -1,     -1},
+};
+
 /*
  *  Button definitions
  */
@@ -2220,6 +2232,17 @@ struct em28xx_board em28xx_boards[] = {
 		.has_dvb       = 1,
 		.ir_codes      = RC_MAP_PINNACLE_PCTV_HD,
 	},
+	/* 2013:025f PCTV tripleStick (292e).
+	 * Empia EM28178, Silicon Labs Si2168, Silicon Labs Si2157 */
+	[EM28178_BOARD_PCTV_292E] = {
+		.name          = "PCTV tripleStick (292e)",
+		.def_i2c_bus   = 1,
+		.i2c_speed     = EM28XX_I2C_CLK_WAIT_ENABLE | EM28XX_I2C_FREQ_400_KHZ,
+		.tuner_type    = TUNER_ABSENT,
+		.tuner_gpio    = pctv_292e,
+		.has_dvb       = 1,
+		.ir_codes      = RC_MAP_PINNACLE_PCTV_HD,
+	},
 };
 EXPORT_SYMBOL_GPL(em28xx_boards);
 
@@ -2397,6 +2420,8 @@ struct usb_device_id em28xx_id_table[] = {
 			.driver_info = EM2765_BOARD_SPEEDLINK_VAD_LAPLACE },
 	{ USB_DEVICE(0x2013, 0x0258),
 			.driver_info = EM28178_BOARD_PCTV_461E },
+	{ USB_DEVICE(0x2013, 0x025f),
+			.driver_info = EM28178_BOARD_PCTV_292E },
 	{ },
 };
 MODULE_DEVICE_TABLE(usb, em28xx_id_table);
@@ -2682,8 +2707,6 @@ static void em28xx_card_setup(struct em28xx *dev)
 	if (dev->board.is_webcam) {
 		if (em28xx_detect_sensor(dev) < 0)
 			dev->board.is_webcam = 0;
-		else
-			dev->progressive = 1;
 	}
 
 	switch (dev->model) {
@@ -2718,11 +2741,6 @@ static void em28xx_card_setup(struct em28xx *dev)
 		    dev->board.name, dev->model);
 
 	dev->tuner_type = em28xx_boards[dev->model].tuner_type;
-	if (em28xx_boards[dev->model].tuner_addr)
-		dev->tuner_addr = em28xx_boards[dev->model].tuner_addr;
-
-	if (em28xx_boards[dev->model].tda9887_conf)
-		dev->tda9887_conf = em28xx_boards[dev->model].tda9887_conf;
 
 	/* request some modules */
 	switch (dev->model) {
@@ -2991,8 +3009,6 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 	const char *chip_name = default_chip_name;
 
 	dev->udev = udev;
-	mutex_init(&dev->vb_queue_lock);
-	mutex_init(&dev->vb_vbi_queue_lock);
 	mutex_init(&dev->ctrl_urb_lock);
 	spin_lock_init(&dev->slock);
 
@@ -3416,15 +3432,14 @@ static int em28xx_usb_probe(struct usb_interface *interface,
 
 	/* Select USB transfer types to use */
 	if (has_video) {
-	    if (!dev->analog_ep_isoc || (try_bulk && dev->analog_ep_bulk))
-		dev->analog_xfer_bulk = 1;
-	    em28xx_info("analog set to %s mode.\n",
-			dev->analog_xfer_bulk ? "bulk" : "isoc");
+		if (!dev->analog_ep_isoc || (try_bulk && dev->analog_ep_bulk))
+			dev->analog_xfer_bulk = 1;
+		em28xx_info("analog set to %s mode.\n",
+			    dev->analog_xfer_bulk ? "bulk" : "isoc");
 	}
 	if (has_dvb) {
-	    if (!dev->dvb_ep_isoc || (try_bulk && dev->dvb_ep_bulk))
-		dev->dvb_xfer_bulk = 1;
-
+		if (!dev->dvb_ep_isoc || (try_bulk && dev->dvb_ep_bulk))
+			dev->dvb_xfer_bulk = 1;
 		em28xx_info("dvb set to %s mode.\n",
 			    dev->dvb_xfer_bulk ? "bulk" : "isoc");
 	}
diff --git a/drivers/media/usb/em28xx/em28xx-dvb.c b/drivers/media/usb/em28xx/em28xx-dvb.c
index f599b18ef7c..a121ed9561f 100644
--- a/drivers/media/usb/em28xx/em28xx-dvb.c
+++ b/drivers/media/usb/em28xx/em28xx-dvb.c
@@ -55,6 +55,8 @@
 #include "mb86a20s.h"
 #include "m88ds3103.h"
 #include "m88ts2022.h"
+#include "si2168.h"
+#include "si2157.h"
 
 MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@infradead.org>");
 MODULE_LICENSE("GPL");
@@ -93,6 +95,7 @@ struct em28xx_dvb {
 	struct semaphore      pll_mutex;
 	bool			dont_attach_fe1;
 	int			lna_gpio;
+	struct i2c_client	*i2c_client_demod;
 	struct i2c_client	*i2c_client_tuner;
 };
 
@@ -743,6 +746,21 @@ static int em28xx_pctv_290e_set_lna(struct dvb_frontend *fe)
 #endif
 }
 
+static int em28xx_pctv_292e_set_lna(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct em28xx_i2c_bus *i2c_bus = fe->dvb->priv;
+	struct em28xx *dev = i2c_bus->dev;
+	u8 lna;
+
+	if (c->lna == 1)
+		lna = 0x01;
+	else
+		lna = 0x00;
+
+	return em28xx_write_reg_bits(dev, EM2874_R80_GPIO_P0_CTRL, lna, 0x01);
+}
+
 static int em28xx_mt352_terratec_xs_init(struct dvb_frontend *fe)
 {
 	/* Values extracted from a USB trace of the Terratec Windows driver */
@@ -1496,6 +1514,63 @@ static int em28xx_dvb_init(struct em28xx *dev)
 			dvb->i2c_client_tuner = client;
 		}
 		break;
+	case EM28178_BOARD_PCTV_292E:
+		{
+			struct i2c_adapter *adapter;
+			struct i2c_client *client;
+			struct i2c_board_info info;
+			struct si2168_config si2168_config;
+			struct si2157_config si2157_config;
+
+			/* attach demod */
+			si2168_config.i2c_adapter = &adapter;
+			si2168_config.fe = &dvb->fe[0];
+			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 = i2c_new_device(&dev->i2c_adap[dev->def_i2c_bus], &info);
+			if (client == NULL || client->dev.driver == NULL) {
+				result = -ENODEV;
+				goto out_free;
+			}
+
+			if (!try_module_get(client->dev.driver->owner)) {
+				i2c_unregister_device(client);
+				result = -ENODEV;
+				goto out_free;
+			}
+
+			dvb->i2c_client_demod = client;
+
+			/* attach tuner */
+			si2157_config.fe = dvb->fe[0];
+			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 = i2c_new_device(adapter, &info);
+			if (client == NULL || client->dev.driver == NULL) {
+				module_put(dvb->i2c_client_demod->dev.driver->owner);
+				i2c_unregister_device(dvb->i2c_client_demod);
+				result = -ENODEV;
+				goto out_free;
+			}
+
+			if (!try_module_get(client->dev.driver->owner)) {
+				i2c_unregister_device(client);
+				module_put(dvb->i2c_client_demod->dev.driver->owner);
+				i2c_unregister_device(dvb->i2c_client_demod);
+				result = -ENODEV;
+				goto out_free;
+			}
+
+			dvb->i2c_client_tuner = client;
+			dvb->fe[0]->ops.set_lna = em28xx_pctv_292e_set_lna;
+		}
+		break;
 	default:
 		em28xx_errdev("/2: The frontend of your DVB/ATSC card"
 				" isn't supported yet\n");
@@ -1582,6 +1657,13 @@ static int em28xx_dvb_fini(struct em28xx *dev)
 		i2c_unregister_device(client);
 	}
 
+	/* remove I2C demod */
+	client = dvb->i2c_client_demod;
+	if (client) {
+		module_put(client->dev.driver->owner);
+		i2c_unregister_device(client);
+	}
+
 	em28xx_unregister_dvb(dvb);
 	kfree(dvb);
 	dev->dvb = NULL;
@@ -1647,6 +1729,13 @@ static int em28xx_dvb_resume(struct em28xx *dev)
 			i2c_unregister_device(client);
 		}
 
+		/* remove I2C demod */
+		client = dvb->i2c_client_demod;
+		if (client) {
+			module_put(client->dev.driver->owner);
+			i2c_unregister_device(client);
+		}
+
 		em28xx_unregister_dvb(dvb);
 		kfree(dvb);
 		dev->dvb = NULL;
diff --git a/drivers/media/usb/em28xx/em28xx-i2c.c b/drivers/media/usb/em28xx/em28xx-i2c.c
index ba6433c3a64..b58d4ebf641 100644
--- a/drivers/media/usb/em28xx/em28xx-i2c.c
+++ b/drivers/media/usb/em28xx/em28xx-i2c.c
@@ -939,7 +939,6 @@ int em28xx_i2c_register(struct em28xx *dev, unsigned bus,
 	dev->i2c_bus[bus].algo_type = algo_type;
 	dev->i2c_bus[bus].dev = dev;
 	dev->i2c_adap[bus].algo_data = &dev->i2c_bus[bus];
-	i2c_set_adapdata(&dev->i2c_adap[bus], &dev->v4l2_dev);
 
 	retval = i2c_add_adapter(&dev->i2c_adap[bus]);
 	if (retval < 0) {
diff --git a/drivers/media/usb/em28xx/em28xx-v4l.h b/drivers/media/usb/em28xx/em28xx-v4l.h
index bce438691e0..432862c20bb 100644
--- a/drivers/media/usb/em28xx/em28xx-v4l.h
+++ b/drivers/media/usb/em28xx/em28xx-v4l.h
@@ -16,5 +16,5 @@
 
 
 int em28xx_start_analog_streaming(struct vb2_queue *vq, unsigned int count);
-int em28xx_stop_vbi_streaming(struct vb2_queue *vq);
+void em28xx_stop_vbi_streaming(struct vb2_queue *vq);
 extern struct vb2_ops em28xx_vbi_qops;
diff --git a/drivers/media/usb/em28xx/em28xx-vbi.c b/drivers/media/usb/em28xx/em28xx-vbi.c
index db3d655600d..6d7f657f6f5 100644
--- a/drivers/media/usb/em28xx/em28xx-vbi.c
+++ b/drivers/media/usb/em28xx/em28xx-vbi.c
@@ -47,12 +47,13 @@ static int vbi_queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
 			   unsigned int sizes[], void *alloc_ctxs[])
 {
 	struct em28xx *dev = vb2_get_drv_priv(vq);
+	struct em28xx_v4l2 *v4l2 = dev->v4l2;
 	unsigned long size;
 
 	if (fmt)
 		size = fmt->fmt.pix.sizeimage;
 	else
-		size = dev->vbi_width * dev->vbi_height * 2;
+		size = v4l2->vbi_width * v4l2->vbi_height * 2;
 
 	if (0 == *nbuffers)
 		*nbuffers = 32;
@@ -69,11 +70,12 @@ static int vbi_queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
 
 static int vbi_buffer_prepare(struct vb2_buffer *vb)
 {
-	struct em28xx        *dev = vb2_get_drv_priv(vb->vb2_queue);
-	struct em28xx_buffer *buf = container_of(vb, struct em28xx_buffer, vb);
+	struct em28xx        *dev  = vb2_get_drv_priv(vb->vb2_queue);
+	struct em28xx_v4l2   *v4l2 = dev->v4l2;
+	struct em28xx_buffer *buf  = container_of(vb, struct em28xx_buffer, vb);
 	unsigned long        size;
 
-	size = dev->vbi_width * dev->vbi_height * 2;
+	size = v4l2->vbi_width * v4l2->vbi_height * 2;
 
 	if (vb2_plane_size(vb, 0) < size) {
 		printk(KERN_INFO "%s data will not fit into plane (%lu < %lu)\n",
diff --git a/drivers/media/usb/em28xx/em28xx-video.c b/drivers/media/usb/em28xx/em28xx-video.c
index 0856e5d367b..f6b49c98e2c 100644
--- a/drivers/media/usb/em28xx/em28xx-video.c
+++ b/drivers/media/usb/em28xx/em28xx-video.c
@@ -141,6 +141,33 @@ static struct em28xx_fmt format[] = {
 	},
 };
 
+/*FIXME: maxw should be dependent of alt mode */
+static inline unsigned int norm_maxw(struct em28xx *dev)
+{
+	struct em28xx_v4l2 *v4l2 = dev->v4l2;
+
+	if (dev->board.is_webcam)
+		return v4l2->sensor_xres;
+
+	if (dev->board.max_range_640_480)
+		return 640;
+
+	return 720;
+}
+
+static inline unsigned int norm_maxh(struct em28xx *dev)
+{
+	struct em28xx_v4l2 *v4l2 = dev->v4l2;
+
+	if (dev->board.is_webcam)
+		return v4l2->sensor_yres;
+
+	if (dev->board.max_range_640_480)
+		return 480;
+
+	return (v4l2->norm & V4L2_STD_625_50) ? 576 : 480;
+}
+
 static int em28xx_vbi_supported(struct em28xx *dev)
 {
 	/* Modprobe option to manually disable */
@@ -166,10 +193,11 @@ static int em28xx_vbi_supported(struct em28xx *dev)
  */
 static void em28xx_wake_i2c(struct em28xx *dev)
 {
-	v4l2_device_call_all(&dev->v4l2_dev, 0, core,  reset, 0);
-	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_routing,
+	struct v4l2_device *v4l2_dev = &dev->v4l2->v4l2_dev;
+	v4l2_device_call_all(v4l2_dev, 0, core,  reset, 0);
+	v4l2_device_call_all(v4l2_dev, 0, video, s_routing,
 			INPUT(dev->ctl_input)->vmux, 0, 0);
-	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_stream, 0);
+	v4l2_device_call_all(v4l2_dev, 0, video, s_stream, 0);
 }
 
 static int em28xx_colorlevels_set_default(struct em28xx *dev)
@@ -194,8 +222,9 @@ static int em28xx_set_outfmt(struct em28xx *dev)
 {
 	int ret;
 	u8 fmt, vinctrl;
+	struct em28xx_v4l2 *v4l2 = dev->v4l2;
 
-	fmt = dev->format->reg;
+	fmt = v4l2->format->reg;
 	if (!dev->is_em25xx)
 		fmt |= 0x20;
 	/*
@@ -211,20 +240,20 @@ static int em28xx_set_outfmt(struct em28xx *dev)
 	if (ret < 0)
 		return ret;
 
-	ret = em28xx_write_reg(dev, EM28XX_R10_VINMODE, dev->vinmode);
+	ret = em28xx_write_reg(dev, EM28XX_R10_VINMODE, v4l2->vinmode);
 	if (ret < 0)
 		return ret;
 
-	vinctrl = dev->vinctl;
+	vinctrl = v4l2->vinctl;
 	if (em28xx_vbi_supported(dev) == 1) {
 		vinctrl |= EM28XX_VINCTRL_VBI_RAW;
 		em28xx_write_reg(dev, EM28XX_R34_VBI_START_H, 0x00);
-		em28xx_write_reg(dev, EM28XX_R36_VBI_WIDTH, dev->vbi_width/4);
-		em28xx_write_reg(dev, EM28XX_R37_VBI_HEIGHT, dev->vbi_height);
-		if (dev->norm & V4L2_STD_525_60) {
+		em28xx_write_reg(dev, EM28XX_R36_VBI_WIDTH, v4l2->vbi_width/4);
+		em28xx_write_reg(dev, EM28XX_R37_VBI_HEIGHT, v4l2->vbi_height);
+		if (v4l2->norm & V4L2_STD_525_60) {
 			/* NTSC */
 			em28xx_write_reg(dev, EM28XX_R35_VBI_START_V, 0x09);
-		} else if (dev->norm & V4L2_STD_625_50) {
+		} else if (v4l2->norm & V4L2_STD_625_50) {
 			/* PAL */
 			em28xx_write_reg(dev, EM28XX_R35_VBI_START_V, 0x07);
 		}
@@ -274,7 +303,7 @@ static void em28xx_capture_area_set(struct em28xx *dev, u8 hstart, u8 vstart,
 
 static int em28xx_scaler_set(struct em28xx *dev, u16 h, u16 v)
 {
-	u8 mode;
+	u8 mode = 0x00;
 	/* the em2800 scaler only supports scaling down to 50% */
 
 	if (dev->board.is_em2800) {
@@ -293,22 +322,22 @@ static int em28xx_scaler_set(struct em28xx *dev, u16 h, u16 v)
 		   to work correctly */
 		mode = (h || v) ? 0x30 : 0x00;
 	}
-	return em28xx_write_reg_bits(dev, EM28XX_R26_COMPR, mode, 0x30);
+	return em28xx_write_reg(dev, EM28XX_R26_COMPR, mode);
 }
 
 /* FIXME: this only function read values from dev */
 static int em28xx_resolution_set(struct em28xx *dev)
 {
-	int width, height;
-	width = norm_maxw(dev);
-	height = norm_maxh(dev);
+	struct em28xx_v4l2 *v4l2 = dev->v4l2;
+	int width = norm_maxw(dev);
+	int height = norm_maxh(dev);
 
 	/* Properly setup VBI */
-	dev->vbi_width = 720;
-	if (dev->norm & V4L2_STD_525_60)
-		dev->vbi_height = 12;
+	v4l2->vbi_width = 720;
+	if (v4l2->norm & V4L2_STD_525_60)
+		v4l2->vbi_height = 12;
 	else
-		dev->vbi_height = 18;
+		v4l2->vbi_height = 18;
 
 	em28xx_set_outfmt(dev);
 
@@ -326,15 +355,16 @@ static int em28xx_resolution_set(struct em28xx *dev)
 	else
 		em28xx_capture_area_set(dev, 0, 0, width, height);
 
-	return em28xx_scaler_set(dev, dev->hscale, dev->vscale);
+	return em28xx_scaler_set(dev, v4l2->hscale, v4l2->vscale);
 }
 
 /* Set USB alternate setting for analog video */
 static int em28xx_set_alternate(struct em28xx *dev)
 {
+	struct em28xx_v4l2 *v4l2 = dev->v4l2;
 	int errCode;
 	int i;
-	unsigned int min_pkt_size = dev->width * 2 + 4;
+	unsigned int min_pkt_size = v4l2->width * 2 + 4;
 
 	/* NOTE: for isoc transfers, only alt settings > 0 are allowed
 		 bulk transfers seem to work only with alt=0 ! */
@@ -351,7 +381,7 @@ static int em28xx_set_alternate(struct em28xx *dev)
 	   the frame size should be increased, otherwise, only
 	   green screen will be received.
 	 */
-	if (dev->width * 2 * dev->height > 720 * 240 * 2)
+	if (v4l2->width * 2 * v4l2->height > 720 * 240 * 2)
 		min_pkt_size *= 2;
 
 	for (i = 0; i < dev->num_alt; i++) {
@@ -404,7 +434,7 @@ static inline void finish_buffer(struct em28xx *dev,
 {
 	em28xx_isocdbg("[%p/%d] wakeup\n", buf, buf->top_field);
 
-	buf->vb.v4l2_buf.sequence = dev->field_count++;
+	buf->vb.v4l2_buf.sequence = dev->v4l2->field_count++;
 	buf->vb.v4l2_buf.field = V4L2_FIELD_INTERLACED;
 	v4l2_get_timestamp(&buf->vb.v4l2_buf.timestamp);
 
@@ -419,9 +449,10 @@ static void em28xx_copy_video(struct em28xx *dev,
 			      unsigned char *usb_buf,
 			      unsigned long len)
 {
+	struct em28xx_v4l2 *v4l2 = dev->v4l2;
 	void *fieldstart, *startwrite, *startread;
 	int  linesdone, currlinedone, offset, lencopy, remain;
-	int bytesperline = dev->width << 1;
+	int bytesperline = v4l2->width << 1;
 
 	if (buf->pos + len > buf->length)
 		len = buf->length - buf->pos;
@@ -429,7 +460,7 @@ static void em28xx_copy_video(struct em28xx *dev,
 	startread = usb_buf;
 	remain = len;
 
-	if (dev->progressive || buf->top_field)
+	if (v4l2->progressive || buf->top_field)
 		fieldstart = buf->vb_buf;
 	else /* interlaced mode, even nr. of lines */
 		fieldstart = buf->vb_buf + bytesperline;
@@ -437,7 +468,7 @@ static void em28xx_copy_video(struct em28xx *dev,
 	linesdone = buf->pos / bytesperline;
 	currlinedone = buf->pos % bytesperline;
 
-	if (dev->progressive)
+	if (v4l2->progressive)
 		offset = linesdone * bytesperline + currlinedone;
 	else
 		offset = linesdone * bytesperline * 2 + currlinedone;
@@ -461,7 +492,7 @@ static void em28xx_copy_video(struct em28xx *dev,
 	remain -= lencopy;
 
 	while (remain > 0) {
-		if (dev->progressive)
+		if (v4l2->progressive)
 			startwrite += lencopy;
 		else
 			startwrite += lencopy + bytesperline;
@@ -507,7 +538,7 @@ static void em28xx_copy_vbi(struct em28xx *dev,
 	offset = buf->pos;
 	/* Make sure the bottom field populates the second half of the frame */
 	if (buf->top_field == 0)
-		offset += dev->vbi_width * dev->vbi_height;
+		offset += dev->v4l2->vbi_width * dev->v4l2->vbi_height;
 
 	memcpy(buf->vb_buf + offset, usb_buf, len);
 	buf->pos += len;
@@ -583,13 +614,15 @@ finish_field_prepare_next(struct em28xx *dev,
 			  struct em28xx_buffer *buf,
 			  struct em28xx_dmaqueue *dma_q)
 {
-	if (dev->progressive || dev->top_field) { /* Brand new frame */
+	struct em28xx_v4l2 *v4l2 = dev->v4l2;
+
+	if (v4l2->progressive || v4l2->top_field) { /* Brand new frame */
 		if (buf != NULL)
 			finish_buffer(dev, buf);
 		buf = get_next_buf(dev, dma_q);
 	}
 	if (buf != NULL) {
-		buf->top_field = dev->top_field;
+		buf->top_field = v4l2->top_field;
 		buf->pos = 0;
 	}
 
@@ -603,6 +636,7 @@ static inline void process_frame_data_em28xx(struct em28xx *dev,
 					     unsigned char *data_pkt,
 					     unsigned int  data_len)
 {
+	struct em28xx_v4l2      *v4l2 = dev->v4l2;
 	struct em28xx_buffer    *buf = dev->usb_ctl.vid_buf;
 	struct em28xx_buffer    *vbi_buf = dev->usb_ctl.vbi_buf;
 	struct em28xx_dmaqueue  *dma_q = &dev->vidq;
@@ -622,17 +656,17 @@ static inline void process_frame_data_em28xx(struct em28xx *dev,
 			data_len -= 4;
 		} else if (data_pkt[0] == 0x33 && data_pkt[1] == 0x95) {
 			/* Field start (VBI mode) */
-			dev->capture_type = 0;
-			dev->vbi_read = 0;
+			v4l2->capture_type = 0;
+			v4l2->vbi_read = 0;
 			em28xx_isocdbg("VBI START HEADER !!!\n");
-			dev->top_field = !(data_pkt[2] & 1);
+			v4l2->top_field = !(data_pkt[2] & 1);
 			data_pkt += 4;
 			data_len -= 4;
 		} else if (data_pkt[0] == 0x22 && data_pkt[1] == 0x5a) {
 			/* Field start (VBI disabled) */
-			dev->capture_type = 2;
+			v4l2->capture_type = 2;
 			em28xx_isocdbg("VIDEO START HEADER !!!\n");
-			dev->top_field = !(data_pkt[2] & 1);
+			v4l2->top_field = !(data_pkt[2] & 1);
 			data_pkt += 4;
 			data_len -= 4;
 		}
@@ -640,37 +674,37 @@ static inline void process_frame_data_em28xx(struct em28xx *dev,
 	/* NOTE: With bulk transfers, intermediate data packets
 	 * have no continuation header */
 
-	if (dev->capture_type == 0) {
+	if (v4l2->capture_type == 0) {
 		vbi_buf = finish_field_prepare_next(dev, vbi_buf, vbi_dma_q);
 		dev->usb_ctl.vbi_buf = vbi_buf;
-		dev->capture_type = 1;
+		v4l2->capture_type = 1;
 	}
 
-	if (dev->capture_type == 1) {
-		int vbi_size = dev->vbi_width * dev->vbi_height;
-		int vbi_data_len = ((dev->vbi_read + data_len) > vbi_size) ?
-				   (vbi_size - dev->vbi_read) : data_len;
+	if (v4l2->capture_type == 1) {
+		int vbi_size = v4l2->vbi_width * v4l2->vbi_height;
+		int vbi_data_len = ((v4l2->vbi_read + data_len) > vbi_size) ?
+				   (vbi_size - v4l2->vbi_read) : data_len;
 
 		/* Copy VBI data */
 		if (vbi_buf != NULL)
 			em28xx_copy_vbi(dev, vbi_buf, data_pkt, vbi_data_len);
-		dev->vbi_read += vbi_data_len;
+		v4l2->vbi_read += vbi_data_len;
 
 		if (vbi_data_len < data_len) {
 			/* Continue with copying video data */
-			dev->capture_type = 2;
+			v4l2->capture_type = 2;
 			data_pkt += vbi_data_len;
 			data_len -= vbi_data_len;
 		}
 	}
 
-	if (dev->capture_type == 2) {
+	if (v4l2->capture_type == 2) {
 		buf = finish_field_prepare_next(dev, buf, dma_q);
 		dev->usb_ctl.vid_buf = buf;
-		dev->capture_type = 3;
+		v4l2->capture_type = 3;
 	}
 
-	if (dev->capture_type == 3 && buf != NULL && data_len > 0)
+	if (v4l2->capture_type == 3 && buf != NULL && data_len > 0)
 		em28xx_copy_video(dev, buf, data_pkt, data_len);
 }
 
@@ -683,6 +717,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;
 
 	/* Check for header */
@@ -691,7 +726,7 @@ static inline void process_frame_data_em25xx(struct em28xx *dev,
 	if (data_len >= 2) {	/* em25xx header is only 2 bytes long */
 		if ((data_pkt[0] == EM25XX_FRMDATAHDR_BYTE1) &&
 		    ((data_pkt[1] & ~EM25XX_FRMDATAHDR_BYTE2_MASK) == 0x00)) {
-			dev->top_field = !(data_pkt[1] &
+			v4l2->top_field = !(data_pkt[1] &
 					   EM25XX_FRMDATAHDR_BYTE2_FRAME_ID);
 			frame_end = data_pkt[1] &
 				    EM25XX_FRMDATAHDR_BYTE2_FRAME_END;
@@ -841,12 +876,14 @@ static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
 		       unsigned int sizes[], void *alloc_ctxs[])
 {
 	struct em28xx *dev = vb2_get_drv_priv(vq);
+	struct em28xx_v4l2 *v4l2 = dev->v4l2;
 	unsigned long size;
 
 	if (fmt)
 		size = fmt->fmt.pix.sizeimage;
 	else
-		size = (dev->width * dev->height * dev->format->depth + 7) >> 3;
+		size =
+		    (v4l2->width * v4l2->height * v4l2->format->depth + 7) >> 3;
 
 	if (size == 0)
 		return -EINVAL;
@@ -864,12 +901,13 @@ static int
 buffer_prepare(struct vb2_buffer *vb)
 {
 	struct em28xx        *dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct em28xx_v4l2   *v4l2 = dev->v4l2;
 	struct em28xx_buffer *buf = container_of(vb, struct em28xx_buffer, vb);
 	unsigned long size;
 
 	em28xx_videodbg("%s, field=%d\n", __func__, vb->v4l2_buf.field);
 
-	size = (dev->width * dev->height * dev->format->depth + 7) >> 3;
+	size = (v4l2->width * v4l2->height * v4l2->format->depth + 7) >> 3;
 
 	if (vb2_plane_size(vb, 0) < size) {
 		em28xx_videodbg("%s data will not fit into plane (%lu < %lu)\n",
@@ -884,6 +922,7 @@ buffer_prepare(struct vb2_buffer *vb)
 int em28xx_start_analog_streaming(struct vb2_queue *vq, unsigned int count)
 {
 	struct em28xx *dev = vb2_get_drv_priv(vq);
+	struct em28xx_v4l2 *v4l2 = dev->v4l2;
 	struct v4l2_frequency f;
 	int rc = 0;
 
@@ -895,7 +934,7 @@ int em28xx_start_analog_streaming(struct vb2_queue *vq, unsigned int count)
 	if (rc)
 		return rc;
 
-	if (dev->streaming_users == 0) {
+	if (v4l2->streaming_users == 0) {
 		/* First active streaming user, so allocate all the URBs */
 
 		/* Allocate the USB bandwidth */
@@ -906,7 +945,7 @@ int em28xx_start_analog_streaming(struct vb2_queue *vq, unsigned int count)
 		*/
 		em28xx_wake_i2c(dev);
 
-		dev->capture_type = -1;
+		v4l2->capture_type = -1;
 		rc = em28xx_init_usb_xfer(dev, EM28XX_ANALOG_MODE,
 					  dev->analog_xfer_bulk,
 					  EM28XX_NUM_BUFS,
@@ -924,22 +963,24 @@ int em28xx_start_analog_streaming(struct vb2_queue *vq, unsigned int count)
 
 		/* Ask tuner to go to analog or radio mode */
 		memset(&f, 0, sizeof(f));
-		f.frequency = dev->ctl_freq;
+		f.frequency = v4l2->frequency;
 		if (vq->owner && vq->owner->vdev->vfl_type == VFL_TYPE_RADIO)
 			f.type = V4L2_TUNER_RADIO;
 		else
 			f.type = V4L2_TUNER_ANALOG_TV;
-		v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_frequency, &f);
+		v4l2_device_call_all(&v4l2->v4l2_dev,
+				     0, tuner, s_frequency, &f);
 	}
 
-	dev->streaming_users++;
+	v4l2->streaming_users++;
 
 	return rc;
 }
 
-static int em28xx_stop_streaming(struct vb2_queue *vq)
+static void em28xx_stop_streaming(struct vb2_queue *vq)
 {
 	struct em28xx *dev = vb2_get_drv_priv(vq);
+	struct em28xx_v4l2 *v4l2 = dev->v4l2;
 	struct em28xx_dmaqueue *vidq = &dev->vidq;
 	unsigned long flags = 0;
 
@@ -947,7 +988,7 @@ static int em28xx_stop_streaming(struct vb2_queue *vq)
 
 	res_free(dev, vq->type);
 
-	if (dev->streaming_users-- == 1) {
+	if (v4l2->streaming_users-- == 1) {
 		/* Last active user, so shutdown all the URBS */
 		em28xx_uninit_usb_xfer(dev, EM28XX_ANALOG_MODE);
 	}
@@ -961,13 +1002,12 @@ static int em28xx_stop_streaming(struct vb2_queue *vq)
 	}
 	dev->usb_ctl.vid_buf = NULL;
 	spin_unlock_irqrestore(&dev->slock, flags);
-
-	return 0;
 }
 
-int em28xx_stop_vbi_streaming(struct vb2_queue *vq)
+void em28xx_stop_vbi_streaming(struct vb2_queue *vq)
 {
 	struct em28xx *dev = vb2_get_drv_priv(vq);
+	struct em28xx_v4l2 *v4l2 = dev->v4l2;
 	struct em28xx_dmaqueue *vbiq = &dev->vbiq;
 	unsigned long flags = 0;
 
@@ -975,7 +1015,7 @@ int em28xx_stop_vbi_streaming(struct vb2_queue *vq)
 
 	res_free(dev, vq->type);
 
-	if (dev->streaming_users-- == 1) {
+	if (v4l2->streaming_users-- == 1) {
 		/* Last active user, so shutdown all the URBS */
 		em28xx_uninit_usb_xfer(dev, EM28XX_ANALOG_MODE);
 	}
@@ -989,8 +1029,6 @@ int em28xx_stop_vbi_streaming(struct vb2_queue *vq)
 	}
 	dev->usb_ctl.vbi_buf = NULL;
 	spin_unlock_irqrestore(&dev->slock, flags);
-
-	return 0;
 }
 
 static void
@@ -1024,9 +1062,10 @@ static int em28xx_vb2_setup(struct em28xx *dev)
 {
 	int rc;
 	struct vb2_queue *q;
+	struct em28xx_v4l2 *v4l2 = dev->v4l2;
 
 	/* Setup Videobuf2 for Video capture */
-	q = &dev->vb_vidq;
+	q = &v4l2->vb_vidq;
 	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 	q->io_modes = VB2_READ | VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
 	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
@@ -1040,7 +1079,7 @@ static int em28xx_vb2_setup(struct em28xx *dev)
 		return rc;
 
 	/* Setup Videobuf2 for VBI capture */
-	q = &dev->vb_vbiq;
+	q = &v4l2->vb_vbiq;
 	q->type = V4L2_BUF_TYPE_VBI_CAPTURE;
 	q->io_modes = VB2_READ | VB2_MMAP | VB2_USERPTR;
 	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
@@ -1060,6 +1099,7 @@ static int em28xx_vb2_setup(struct em28xx *dev)
 
 static void video_mux(struct em28xx *dev, int index)
 {
+	struct v4l2_device *v4l2_dev = &dev->v4l2->v4l2_dev;
 	dev->ctl_input = index;
 	dev->ctl_ainput = INPUT(index)->amux;
 	dev->ctl_aoutput = INPUT(index)->aout;
@@ -1067,21 +1107,21 @@ static void video_mux(struct em28xx *dev, int index)
 	if (!dev->ctl_aoutput)
 		dev->ctl_aoutput = EM28XX_AOUT_MASTER;
 
-	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_routing,
+	v4l2_device_call_all(v4l2_dev, 0, video, s_routing,
 			INPUT(index)->vmux, 0, 0);
 
 	if (dev->board.has_msp34xx) {
 		if (dev->i2s_speed) {
-			v4l2_device_call_all(&dev->v4l2_dev, 0, audio,
+			v4l2_device_call_all(v4l2_dev, 0, audio,
 				s_i2s_clock_freq, dev->i2s_speed);
 		}
 		/* Note: this is msp3400 specific */
-		v4l2_device_call_all(&dev->v4l2_dev, 0, audio, s_routing,
+		v4l2_device_call_all(v4l2_dev, 0, audio, s_routing,
 			 dev->ctl_ainput, MSP_OUTPUT(MSP_SC_IN_DSP_SCART1), 0);
 	}
 
 	if (dev->board.adecoder != EM28XX_NOADECODER) {
-		v4l2_device_call_all(&dev->v4l2_dev, 0, audio, s_routing,
+		v4l2_device_call_all(v4l2_dev, 0, audio, s_routing,
 			dev->ctl_ainput, dev->ctl_aoutput, 0);
 	}
 
@@ -1112,7 +1152,9 @@ static void em28xx_ctrl_notify(struct v4l2_ctrl *ctrl, void *priv)
 
 static int em28xx_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct em28xx *dev = container_of(ctrl->handler, struct em28xx, ctrl_handler);
+	struct em28xx_v4l2 *v4l2 =
+		  container_of(ctrl->handler, struct em28xx_v4l2, ctrl_handler);
+	struct em28xx *dev = v4l2->dev;
 	int ret = -EINVAL;
 
 	switch (ctrl->id) {
@@ -1187,19 +1229,20 @@ static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 {
 	struct em28xx_fh      *fh  = priv;
 	struct em28xx         *dev = fh->dev;
+	struct em28xx_v4l2    *v4l2 = dev->v4l2;
 
-	f->fmt.pix.width = dev->width;
-	f->fmt.pix.height = dev->height;
-	f->fmt.pix.pixelformat = dev->format->fourcc;
-	f->fmt.pix.bytesperline = (dev->width * dev->format->depth + 7) >> 3;
-	f->fmt.pix.sizeimage = f->fmt.pix.bytesperline  * dev->height;
+	f->fmt.pix.width = v4l2->width;
+	f->fmt.pix.height = v4l2->height;
+	f->fmt.pix.pixelformat = v4l2->format->fourcc;
+	f->fmt.pix.bytesperline = (v4l2->width * v4l2->format->depth + 7) >> 3;
+	f->fmt.pix.sizeimage = f->fmt.pix.bytesperline * v4l2->height;
 	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 
 	/* FIXME: TOP? NONE? BOTTOM? ALTENATE? */
-	if (dev->progressive)
+	if (v4l2->progressive)
 		f->fmt.pix.field = V4L2_FIELD_NONE;
 	else
-		f->fmt.pix.field = dev->interlaced ?
+		f->fmt.pix.field = v4l2->interlaced_fieldmode ?
 			   V4L2_FIELD_INTERLACED : V4L2_FIELD_TOP;
 	return 0;
 }
@@ -1220,6 +1263,7 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 {
 	struct em28xx_fh      *fh    = priv;
 	struct em28xx         *dev   = fh->dev;
+	struct em28xx_v4l2    *v4l2  = dev->v4l2;
 	unsigned int          width  = f->fmt.pix.width;
 	unsigned int          height = f->fmt.pix.height;
 	unsigned int          maxw   = norm_maxw(dev);
@@ -1261,10 +1305,10 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 	f->fmt.pix.bytesperline = (width * fmt->depth + 7) >> 3;
 	f->fmt.pix.sizeimage = f->fmt.pix.bytesperline * height;
 	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
-	if (dev->progressive)
+	if (v4l2->progressive)
 		f->fmt.pix.field = V4L2_FIELD_NONE;
 	else
-		f->fmt.pix.field = dev->interlaced ?
+		f->fmt.pix.field = v4l2->interlaced_fieldmode ?
 			   V4L2_FIELD_INTERLACED : V4L2_FIELD_TOP;
 	f->fmt.pix.priv = 0;
 
@@ -1275,17 +1319,19 @@ static int em28xx_set_video_format(struct em28xx *dev, unsigned int fourcc,
 				   unsigned width, unsigned height)
 {
 	struct em28xx_fmt     *fmt;
+	struct em28xx_v4l2    *v4l2 = dev->v4l2;
 
 	fmt = format_by_fourcc(fourcc);
 	if (!fmt)
 		return -EINVAL;
 
-	dev->format = fmt;
-	dev->width  = width;
-	dev->height = height;
+	v4l2->format = fmt;
+	v4l2->width  = width;
+	v4l2->height = height;
 
 	/* set new image size */
-	size_to_scale(dev, dev->width, dev->height, &dev->hscale, &dev->vscale);
+	size_to_scale(dev, v4l2->width, v4l2->height,
+			   &v4l2->hscale, &v4l2->vscale);
 
 	em28xx_resolution_set(dev);
 
@@ -1296,8 +1342,9 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 			struct v4l2_format *f)
 {
 	struct em28xx *dev = video_drvdata(file);
+	struct em28xx_v4l2 *v4l2 = dev->v4l2;
 
-	if (dev->streaming_users > 0)
+	if (v4l2->streaming_users > 0)
 		return -EBUSY;
 
 	vidioc_try_fmt_vid_cap(file, priv, f);
@@ -1311,7 +1358,7 @@ static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *norm)
 	struct em28xx_fh   *fh  = priv;
 	struct em28xx      *dev = fh->dev;
 
-	*norm = dev->norm;
+	*norm = dev->v4l2->norm;
 
 	return 0;
 }
@@ -1321,24 +1368,25 @@ static int vidioc_querystd(struct file *file, void *priv, v4l2_std_id *norm)
 	struct em28xx_fh   *fh  = priv;
 	struct em28xx      *dev = fh->dev;
 
-	v4l2_device_call_all(&dev->v4l2_dev, 0, video, querystd, norm);
+	v4l2_device_call_all(&dev->v4l2->v4l2_dev, 0, video, querystd, norm);
 
 	return 0;
 }
 
 static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id norm)
 {
-	struct em28xx_fh   *fh  = priv;
-	struct em28xx      *dev = fh->dev;
+	struct em28xx_fh   *fh   = priv;
+	struct em28xx      *dev  = fh->dev;
+	struct em28xx_v4l2 *v4l2 = dev->v4l2;
 	struct v4l2_format f;
 
-	if (norm == dev->norm)
+	if (norm == v4l2->norm)
 		return 0;
 
-	if (dev->streaming_users > 0)
+	if (v4l2->streaming_users > 0)
 		return -EBUSY;
 
-	dev->norm = norm;
+	v4l2->norm = norm;
 
 	/* Adjusts width/height, if needed */
 	f.fmt.pix.width = 720;
@@ -1346,12 +1394,13 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id norm)
 	vidioc_try_fmt_vid_cap(file, priv, &f);
 
 	/* set new image size */
-	dev->width = f.fmt.pix.width;
-	dev->height = f.fmt.pix.height;
-	size_to_scale(dev, dev->width, dev->height, &dev->hscale, &dev->vscale);
+	v4l2->width = f.fmt.pix.width;
+	v4l2->height = f.fmt.pix.height;
+	size_to_scale(dev, v4l2->width, v4l2->height,
+			   &v4l2->hscale, &v4l2->vscale);
 
 	em28xx_resolution_set(dev);
-	v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_std, dev->norm);
+	v4l2_device_call_all(&v4l2->v4l2_dev, 0, video, s_std, v4l2->norm);
 
 	return 0;
 }
@@ -1359,16 +1408,17 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id norm)
 static int vidioc_g_parm(struct file *file, void *priv,
 			 struct v4l2_streamparm *p)
 {
-	struct em28xx_fh   *fh  = priv;
-	struct em28xx      *dev = fh->dev;
+	struct em28xx_fh   *fh   = priv;
+	struct em28xx      *dev  = fh->dev;
+	struct em28xx_v4l2 *v4l2 = dev->v4l2;
 	int rc = 0;
 
 	p->parm.capture.readbuffers = EM28XX_MIN_BUF;
 	if (dev->board.is_webcam)
-		rc = v4l2_device_call_until_err(&dev->v4l2_dev, 0,
+		rc = v4l2_device_call_until_err(&v4l2->v4l2_dev, 0,
 						video, g_parm, p);
 	else
-		v4l2_video_std_frame_period(dev->norm,
+		v4l2_video_std_frame_period(v4l2->norm,
 						 &p->parm.capture.timeperframe);
 
 	return rc;
@@ -1381,7 +1431,8 @@ static int vidioc_s_parm(struct file *file, void *priv,
 	struct em28xx      *dev = fh->dev;
 
 	p->parm.capture.readbuffers = EM28XX_MIN_BUF;
-	return v4l2_device_call_until_err(&dev->v4l2_dev, 0, video, s_parm, p);
+	return v4l2_device_call_until_err(&dev->v4l2->v4l2_dev,
+					  0, video, s_parm, p);
 }
 
 static const char *iname[] = {
@@ -1418,7 +1469,7 @@ static int vidioc_enum_input(struct file *file, void *priv,
 		(EM28XX_VMUX_CABLE == INPUT(n)->type))
 		i->type = V4L2_INPUT_TYPE_TUNER;
 
-	i->std = dev->vdev->tvnorms;
+	i->std = dev->v4l2->vdev->tvnorms;
 	/* webcams do not have the STD API */
 	if (dev->board.is_webcam)
 		i->capabilities = 0;
@@ -1520,7 +1571,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 
 	strcpy(t->name, "Tuner");
 
-	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, g_tuner, t);
+	v4l2_device_call_all(&dev->v4l2->v4l2_dev, 0, tuner, g_tuner, t);
 	return 0;
 }
 
@@ -1533,7 +1584,7 @@ static int vidioc_s_tuner(struct file *file, void *priv,
 	if (0 != t->index)
 		return -EINVAL;
 
-	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_tuner, t);
+	v4l2_device_call_all(&dev->v4l2->v4l2_dev, 0, tuner, s_tuner, t);
 	return 0;
 }
 
@@ -1542,27 +1593,29 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 {
 	struct em28xx_fh      *fh  = priv;
 	struct em28xx         *dev = fh->dev;
+	struct em28xx_v4l2    *v4l2 = dev->v4l2;
 
 	if (0 != f->tuner)
 		return -EINVAL;
 
-	f->frequency = dev->ctl_freq;
+	f->frequency = v4l2->frequency;
 	return 0;
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
 				const struct v4l2_frequency *f)
 {
-	struct v4l2_frequency new_freq = *f;
-	struct em28xx_fh      *fh  = priv;
-	struct em28xx         *dev = fh->dev;
+	struct v4l2_frequency  new_freq = *f;
+	struct em28xx_fh          *fh   = priv;
+	struct em28xx             *dev  = fh->dev;
+	struct em28xx_v4l2        *v4l2 = dev->v4l2;
 
 	if (0 != f->tuner)
 		return -EINVAL;
 
-	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_frequency, f);
-	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, g_frequency, &new_freq);
-	dev->ctl_freq = new_freq.frequency;
+	v4l2_device_call_all(&v4l2->v4l2_dev, 0, tuner, s_frequency, f);
+	v4l2_device_call_all(&v4l2->v4l2_dev, 0, tuner, g_frequency, &new_freq);
+	v4l2->frequency = new_freq.frequency;
 
 	return 0;
 }
@@ -1579,7 +1632,8 @@ static int vidioc_g_chip_info(struct file *file, void *priv,
 	if (chip->match.addr == 1)
 		strlcpy(chip->name, "ac97", sizeof(chip->name));
 	else
-		strlcpy(chip->name, dev->v4l2_dev.name, sizeof(chip->name));
+		strlcpy(chip->name,
+			dev->v4l2->v4l2_dev.name, sizeof(chip->name));
 	return 0;
 }
 
@@ -1660,9 +1714,10 @@ static int vidioc_s_register(struct file *file, void *priv,
 static int vidioc_querycap(struct file *file, void  *priv,
 					struct v4l2_capability *cap)
 {
-	struct video_device *vdev = video_devdata(file);
-	struct em28xx_fh      *fh  = priv;
-	struct em28xx         *dev = fh->dev;
+	struct video_device   *vdev = video_devdata(file);
+	struct em28xx_fh      *fh   = priv;
+	struct em28xx         *dev  = fh->dev;
+	struct em28xx_v4l2    *v4l2 = dev->v4l2;
 
 	strlcpy(cap->driver, "em28xx", sizeof(cap->driver));
 	strlcpy(cap->card, em28xx_boards[dev->model].name, sizeof(cap->card));
@@ -1684,9 +1739,9 @@ static int vidioc_querycap(struct file *file, void  *priv,
 
 	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS |
 		V4L2_CAP_READWRITE | V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
-	if (dev->vbi_dev)
+	if (v4l2->vbi_dev)
 		cap->capabilities |= V4L2_CAP_VBI_CAPTURE;
-	if (dev->radio_dev)
+	if (v4l2->radio_dev)
 		cap->capabilities |= V4L2_CAP_RADIO;
 	return 0;
 }
@@ -1751,24 +1806,25 @@ static int vidioc_enum_framesizes(struct file *file, void *priv,
 static int vidioc_g_fmt_vbi_cap(struct file *file, void *priv,
 				struct v4l2_format *format)
 {
-	struct em28xx_fh      *fh  = priv;
-	struct em28xx         *dev = fh->dev;
+	struct em28xx_fh      *fh   = priv;
+	struct em28xx         *dev  = fh->dev;
+	struct em28xx_v4l2    *v4l2 = dev->v4l2;
 
-	format->fmt.vbi.samples_per_line = dev->vbi_width;
+	format->fmt.vbi.samples_per_line = v4l2->vbi_width;
 	format->fmt.vbi.sample_format = V4L2_PIX_FMT_GREY;
 	format->fmt.vbi.offset = 0;
 	format->fmt.vbi.flags = 0;
 	format->fmt.vbi.sampling_rate = 6750000 * 4 / 2;
-	format->fmt.vbi.count[0] = dev->vbi_height;
-	format->fmt.vbi.count[1] = dev->vbi_height;
+	format->fmt.vbi.count[0] = v4l2->vbi_height;
+	format->fmt.vbi.count[1] = v4l2->vbi_height;
 	memset(format->fmt.vbi.reserved, 0, sizeof(format->fmt.vbi.reserved));
 
 	/* Varies by video standard (NTSC, PAL, etc.) */
-	if (dev->norm & V4L2_STD_525_60) {
+	if (v4l2->norm & V4L2_STD_525_60) {
 		/* NTSC */
 		format->fmt.vbi.start[0] = 10;
 		format->fmt.vbi.start[1] = 273;
-	} else if (dev->norm & V4L2_STD_625_50) {
+	} else if (v4l2->norm & V4L2_STD_625_50) {
 		/* PAL */
 		format->fmt.vbi.start[0] = 6;
 		format->fmt.vbi.start[1] = 318;
@@ -1791,7 +1847,7 @@ static int radio_g_tuner(struct file *file, void *priv,
 
 	strcpy(t->name, "Radio");
 
-	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, g_tuner, t);
+	v4l2_device_call_all(&dev->v4l2->v4l2_dev, 0, tuner, g_tuner, t);
 
 	return 0;
 }
@@ -1804,12 +1860,27 @@ static int radio_s_tuner(struct file *file, void *priv,
 	if (0 != t->index)
 		return -EINVAL;
 
-	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_tuner, t);
+	v4l2_device_call_all(&dev->v4l2->v4l2_dev, 0, tuner, s_tuner, t);
 
 	return 0;
 }
 
 /*
+ * em28xx_free_v4l2() - Free struct em28xx_v4l2
+ *
+ * @ref: struct kref for struct em28xx_v4l2
+ *
+ * Called when all users of struct em28xx_v4l2 are gone
+ */
+static void em28xx_free_v4l2(struct kref *ref)
+{
+	struct em28xx_v4l2 *v4l2 = container_of(ref, struct em28xx_v4l2, ref);
+
+	v4l2->dev->v4l2 = NULL;
+	kfree(v4l2);
+}
+
+/*
  * em28xx_v4l2_open()
  * inits the device and starts isoc transfer
  */
@@ -1817,6 +1888,7 @@ static int em28xx_v4l2_open(struct file *filp)
 {
 	struct video_device *vdev = video_devdata(filp);
 	struct em28xx *dev = video_drvdata(filp);
+	struct em28xx_v4l2 *v4l2 = dev->v4l2;
 	enum v4l2_buf_type fh_type = 0;
 	struct em28xx_fh *fh;
 
@@ -1835,7 +1907,7 @@ static int em28xx_v4l2_open(struct file *filp)
 
 	em28xx_videodbg("open dev=%s type=%s users=%d\n",
 			video_device_node_name(vdev), v4l2_type_names[fh_type],
-			dev->users);
+			v4l2->users);
 
 	if (mutex_lock_interruptible(&dev->lock))
 		return -ERESTARTSYS;
@@ -1850,7 +1922,7 @@ static int em28xx_v4l2_open(struct file *filp)
 	fh->type = fh_type;
 	filp->private_data = fh;
 
-	if (dev->users == 0) {
+	if (v4l2->users == 0) {
 		em28xx_set_mode(dev, EM28XX_ANALOG_MODE);
 
 		if (vdev->vfl_type != VFL_TYPE_RADIO)
@@ -1865,11 +1937,12 @@ static int em28xx_v4l2_open(struct file *filp)
 
 	if (vdev->vfl_type == VFL_TYPE_RADIO) {
 		em28xx_videodbg("video_open: setting radio device\n");
-		v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_radio);
+		v4l2_device_call_all(&v4l2->v4l2_dev, 0, tuner, s_radio);
 	}
 
 	kref_get(&dev->ref);
-	dev->users++;
+	kref_get(&v4l2->ref);
+	v4l2->users++;
 
 	mutex_unlock(&dev->lock);
 	v4l2_fh_add(&fh->fh);
@@ -1884,6 +1957,8 @@ static int em28xx_v4l2_open(struct file *filp)
 */
 static int em28xx_v4l2_fini(struct em28xx *dev)
 {
+	struct em28xx_v4l2 *v4l2 = dev->v4l2;
+
 	if (dev->is_audio_only) {
 		/* Shouldn't initialize IR for this interface */
 		return 0;
@@ -1894,39 +1969,45 @@ static int em28xx_v4l2_fini(struct em28xx *dev)
 		return 0;
 	}
 
+	if (v4l2 == NULL)
+		return 0;
+
 	em28xx_info("Closing video extension");
 
 	mutex_lock(&dev->lock);
 
-	v4l2_device_disconnect(&dev->v4l2_dev);
+	v4l2_device_disconnect(&v4l2->v4l2_dev);
 
 	em28xx_uninit_usb_xfer(dev, EM28XX_ANALOG_MODE);
 
-	if (dev->radio_dev) {
+	if (v4l2->radio_dev) {
 		em28xx_info("V4L2 device %s deregistered\n",
-			    video_device_node_name(dev->radio_dev));
-		video_unregister_device(dev->radio_dev);
+			    video_device_node_name(v4l2->radio_dev));
+		video_unregister_device(v4l2->radio_dev);
 	}
-	if (dev->vbi_dev) {
+	if (v4l2->vbi_dev) {
 		em28xx_info("V4L2 device %s deregistered\n",
-			    video_device_node_name(dev->vbi_dev));
-		video_unregister_device(dev->vbi_dev);
+			    video_device_node_name(v4l2->vbi_dev));
+		video_unregister_device(v4l2->vbi_dev);
 	}
-	if (dev->vdev) {
+	if (v4l2->vdev) {
 		em28xx_info("V4L2 device %s deregistered\n",
-			    video_device_node_name(dev->vdev));
-		video_unregister_device(dev->vdev);
+			    video_device_node_name(v4l2->vdev));
+		video_unregister_device(v4l2->vdev);
 	}
 
-	v4l2_ctrl_handler_free(&dev->ctrl_handler);
-	v4l2_device_unregister(&dev->v4l2_dev);
+	v4l2_ctrl_handler_free(&v4l2->ctrl_handler);
+	v4l2_device_unregister(&v4l2->v4l2_dev);
 
-	if (dev->clk) {
-		v4l2_clk_unregister_fixed(dev->clk);
-		dev->clk = NULL;
+	if (v4l2->clk) {
+		v4l2_clk_unregister_fixed(v4l2->clk);
+		v4l2->clk = NULL;
 	}
 
+	kref_put(&v4l2->ref, em28xx_free_v4l2);
+
 	mutex_unlock(&dev->lock);
+
 	kref_put(&dev->ref, em28xx_free_device);
 
 	return 0;
@@ -1965,22 +2046,23 @@ static int em28xx_v4l2_resume(struct em28xx *dev)
  */
 static int em28xx_v4l2_close(struct file *filp)
 {
-	struct em28xx_fh *fh  = filp->private_data;
-	struct em28xx    *dev = fh->dev;
+	struct em28xx_fh      *fh   = filp->private_data;
+	struct em28xx         *dev  = fh->dev;
+	struct em28xx_v4l2    *v4l2 = dev->v4l2;
 	int              errCode;
 
-	em28xx_videodbg("users=%d\n", dev->users);
+	em28xx_videodbg("users=%d\n", v4l2->users);
 
 	vb2_fop_release(filp);
 	mutex_lock(&dev->lock);
 
-	if (dev->users == 1) {
+	if (v4l2->users == 1) {
 		/* No sense to try to write to the device */
 		if (dev->disconnected)
 			goto exit;
 
 		/* Save some power by putting tuner to sleep */
-		v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_power, 0);
+		v4l2_device_call_all(&v4l2->v4l2_dev, 0, core, s_power, 0);
 
 		/* do this before setting alternate! */
 		em28xx_set_mode(dev, EM28XX_SUSPEND);
@@ -1996,30 +2078,14 @@ static int em28xx_v4l2_close(struct file *filp)
 	}
 
 exit:
-	dev->users--;
+	v4l2->users--;
+	kref_put(&v4l2->ref, em28xx_free_v4l2);
 	mutex_unlock(&dev->lock);
 	kref_put(&dev->ref, em28xx_free_device);
 
 	return 0;
 }
 
-/*
- * em28xx_videodevice_release()
- * called when the last user of the video device exits and frees the memeory
- */
-static void em28xx_videodevice_release(struct video_device *vdev)
-{
-	struct em28xx *dev = video_get_drvdata(vdev);
-
-	video_device_release(vdev);
-	if (vdev == dev->vdev)
-		dev->vdev = NULL;
-	else if (vdev == dev->vbi_dev)
-		dev->vbi_dev = NULL;
-	else if (vdev == dev->radio_dev)
-		dev->radio_dev = NULL;
-}
-
 static const struct v4l2_file_operations em28xx_v4l_fops = {
 	.owner         = THIS_MODULE,
 	.open          = em28xx_v4l2_open,
@@ -2076,7 +2142,7 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 static const struct video_device em28xx_video_template = {
 	.fops		= &em28xx_v4l_fops,
 	.ioctl_ops	= &video_ioctl_ops,
-	.release	= em28xx_videodevice_release,
+	.release	= video_device_release,
 	.tvnorms	= V4L2_STD_ALL,
 };
 
@@ -2105,7 +2171,7 @@ static const struct v4l2_ioctl_ops radio_ioctl_ops = {
 static struct video_device em28xx_radio_template = {
 	.fops		= &radio_fops,
 	.ioctl_ops	= &radio_ioctl_ops,
-	.release	= em28xx_videodevice_release,
+	.release	= video_device_release,
 };
 
 /* I2C possible address to saa7115, tvp5150, msp3400, tvaudio */
@@ -2139,7 +2205,7 @@ static struct video_device *em28xx_vdev_init(struct em28xx *dev,
 		return NULL;
 
 	*vfd		= *template;
-	vfd->v4l2_dev	= &dev->v4l2_dev;
+	vfd->v4l2_dev	= &dev->v4l2->v4l2_dev;
 	vfd->debug	= video_debug;
 	vfd->lock	= &dev->lock;
 	set_bit(V4L2_FL_USE_FH_PRIO, &vfd->flags);
@@ -2153,13 +2219,12 @@ static struct video_device *em28xx_vdev_init(struct em28xx *dev,
 	return vfd;
 }
 
-static void em28xx_tuner_setup(struct em28xx *dev)
+static void em28xx_tuner_setup(struct em28xx *dev, unsigned short tuner_addr)
 {
-	struct tuner_setup           tun_setup;
-	struct v4l2_frequency        f;
-
-	if (dev->tuner_type == TUNER_ABSENT)
-		return;
+	struct em28xx_v4l2      *v4l2 = dev->v4l2;
+	struct v4l2_device      *v4l2_dev = &v4l2->v4l2_dev;
+	struct tuner_setup      tun_setup;
+	struct v4l2_frequency   f;
 
 	memset(&tun_setup, 0, sizeof(tun_setup));
 
@@ -2170,23 +2235,26 @@ static void em28xx_tuner_setup(struct em28xx *dev)
 		tun_setup.type = dev->board.radio.type;
 		tun_setup.addr = dev->board.radio_addr;
 
-		v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_type_addr, &tun_setup);
+		v4l2_device_call_all(v4l2_dev,
+				     0, tuner, s_type_addr, &tun_setup);
 	}
 
 	if ((dev->tuner_type != TUNER_ABSENT) && (dev->tuner_type)) {
 		tun_setup.type   = dev->tuner_type;
-		tun_setup.addr   = dev->tuner_addr;
+		tun_setup.addr   = tuner_addr;
 
-		v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_type_addr, &tun_setup);
+		v4l2_device_call_all(v4l2_dev,
+				     0, tuner, s_type_addr, &tun_setup);
 	}
 
-	if (dev->tda9887_conf) {
+	if (dev->board.tda9887_conf) {
 		struct v4l2_priv_tun_config tda9887_cfg;
 
 		tda9887_cfg.tuner = TUNER_TDA9887;
-		tda9887_cfg.priv = &dev->tda9887_conf;
+		tda9887_cfg.priv = &dev->board.tda9887_conf;
 
-		v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_config, &tda9887_cfg);
+		v4l2_device_call_all(v4l2_dev,
+				     0, tuner, s_config, &tda9887_cfg);
 	}
 
 	if (dev->tuner_type == TUNER_XC2028) {
@@ -2201,15 +2269,15 @@ static void em28xx_tuner_setup(struct em28xx *dev)
 		xc2028_cfg.tuner = TUNER_XC2028;
 		xc2028_cfg.priv  = &ctl;
 
-		v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_config, &xc2028_cfg);
+		v4l2_device_call_all(v4l2_dev, 0, tuner, s_config, &xc2028_cfg);
 	}
 
 	/* configure tuner */
 	f.tuner = 0;
 	f.type = V4L2_TUNER_ANALOG_TV;
 	f.frequency = 9076;     /* just a magic number */
-	dev->ctl_freq = f.frequency;
-	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_frequency, &f);
+	v4l2->frequency = f.frequency;
+	v4l2_device_call_all(v4l2_dev, 0, tuner, s_frequency, &f);
 }
 
 static int em28xx_v4l2_init(struct em28xx *dev)
@@ -2217,7 +2285,8 @@ static int em28xx_v4l2_init(struct em28xx *dev)
 	u8 val;
 	int ret;
 	unsigned int maxw;
-	struct v4l2_ctrl_handler *hdl = &dev->ctrl_handler;
+	struct v4l2_ctrl_handler *hdl;
+	struct em28xx_v4l2 *v4l2;
 
 	if (dev->is_audio_only) {
 		/* Shouldn't initialize IR for this interface */
@@ -2233,71 +2302,93 @@ static int em28xx_v4l2_init(struct em28xx *dev)
 
 	mutex_lock(&dev->lock);
 
-	ret = v4l2_device_register(&dev->udev->dev, &dev->v4l2_dev);
+	v4l2 = kzalloc(sizeof(struct em28xx_v4l2), GFP_KERNEL);
+	if (v4l2 == NULL) {
+		em28xx_info("em28xx_v4l: memory allocation failed\n");
+		mutex_unlock(&dev->lock);
+		return -ENOMEM;
+	}
+	kref_init(&v4l2->ref);
+	v4l2->dev = dev;
+	dev->v4l2 = v4l2;
+
+	ret = v4l2_device_register(&dev->udev->dev, &v4l2->v4l2_dev);
 	if (ret < 0) {
 		em28xx_errdev("Call to v4l2_device_register() failed!\n");
 		goto err;
 	}
 
+	hdl = &v4l2->ctrl_handler;
 	v4l2_ctrl_handler_init(hdl, 8);
-	dev->v4l2_dev.ctrl_handler = hdl;
+	v4l2->v4l2_dev.ctrl_handler = hdl;
+
+	if (dev->board.is_webcam)
+		v4l2->progressive = 1;
 
 	/*
 	 * Default format, used for tvp5150 or saa711x output formats
 	 */
-	dev->vinmode = 0x10;
-	dev->vinctl  = EM28XX_VINCTRL_INTERLACED |
-		       EM28XX_VINCTRL_CCIR656_ENABLE;
+	v4l2->vinmode = 0x10;
+	v4l2->vinctl  = EM28XX_VINCTRL_INTERLACED |
+			EM28XX_VINCTRL_CCIR656_ENABLE;
 
 	/* request some modules */
 
 	if (dev->board.has_msp34xx)
-		v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap[dev->def_i2c_bus],
-			"msp3400", 0, msp3400_addrs);
+		v4l2_i2c_new_subdev(&v4l2->v4l2_dev,
+				    &dev->i2c_adap[dev->def_i2c_bus],
+				    "msp3400", 0, msp3400_addrs);
 
 	if (dev->board.decoder == EM28XX_SAA711X)
-		v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap[dev->def_i2c_bus],
-			"saa7115_auto", 0, saa711x_addrs);
+		v4l2_i2c_new_subdev(&v4l2->v4l2_dev,
+				    &dev->i2c_adap[dev->def_i2c_bus],
+				    "saa7115_auto", 0, saa711x_addrs);
 
 	if (dev->board.decoder == EM28XX_TVP5150)
-		v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap[dev->def_i2c_bus],
-			"tvp5150", 0, tvp5150_addrs);
+		v4l2_i2c_new_subdev(&v4l2->v4l2_dev,
+				    &dev->i2c_adap[dev->def_i2c_bus],
+				    "tvp5150", 0, tvp5150_addrs);
 
 	if (dev->board.adecoder == EM28XX_TVAUDIO)
-		v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap[dev->def_i2c_bus],
-			"tvaudio", dev->board.tvaudio_addr, NULL);
+		v4l2_i2c_new_subdev(&v4l2->v4l2_dev,
+				    &dev->i2c_adap[dev->def_i2c_bus],
+				    "tvaudio", dev->board.tvaudio_addr, NULL);
 
 	/* Initialize tuner and camera */
 
 	if (dev->board.tuner_type != TUNER_ABSENT) {
-		int has_demod = (dev->tda9887_conf & TDA9887_PRESENT);
+		unsigned short tuner_addr = dev->board.tuner_addr;
+		int has_demod = (dev->board.tda9887_conf & TDA9887_PRESENT);
 
 		if (dev->board.radio.type)
-			v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap[dev->def_i2c_bus],
-				"tuner", dev->board.radio_addr, NULL);
+			v4l2_i2c_new_subdev(&v4l2->v4l2_dev,
+					  &dev->i2c_adap[dev->def_i2c_bus],
+					  "tuner", dev->board.radio_addr, NULL);
 
 		if (has_demod)
-			v4l2_i2c_new_subdev(&dev->v4l2_dev,
+			v4l2_i2c_new_subdev(&v4l2->v4l2_dev,
 				&dev->i2c_adap[dev->def_i2c_bus], "tuner",
 				0, v4l2_i2c_tuner_addrs(ADDRS_DEMOD));
-		if (dev->tuner_addr == 0) {
+		if (tuner_addr == 0) {
 			enum v4l2_i2c_tuner_type type =
 				has_demod ? ADDRS_TV_WITH_DEMOD : ADDRS_TV;
 			struct v4l2_subdev *sd;
 
-			sd = v4l2_i2c_new_subdev(&dev->v4l2_dev,
+			sd = v4l2_i2c_new_subdev(&v4l2->v4l2_dev,
 				&dev->i2c_adap[dev->def_i2c_bus], "tuner",
 				0, v4l2_i2c_tuner_addrs(type));
 
 			if (sd)
-				dev->tuner_addr = v4l2_i2c_subdev_addr(sd);
+				tuner_addr = v4l2_i2c_subdev_addr(sd);
 		} else {
-			v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap[dev->def_i2c_bus],
-				"tuner", dev->tuner_addr, NULL);
+			v4l2_i2c_new_subdev(&v4l2->v4l2_dev,
+					    &dev->i2c_adap[dev->def_i2c_bus],
+					    "tuner", tuner_addr, NULL);
 		}
+
+		em28xx_tuner_setup(dev, tuner_addr);
 	}
 
-	em28xx_tuner_setup(dev);
 	if (dev->em28xx_sensor != EM28XX_NOSENSOR)
 		em28xx_init_camera(dev);
 
@@ -2348,12 +2439,12 @@ static int em28xx_v4l2_init(struct em28xx *dev)
 	}
 
 	/* set default norm */
-	dev->norm = V4L2_STD_PAL;
-	v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_std, dev->norm);
-	dev->interlaced = EM28XX_INTERLACED_DEFAULT;
+	v4l2->norm = V4L2_STD_PAL;
+	v4l2_device_call_all(&v4l2->v4l2_dev, 0, video, s_std, v4l2->norm);
+	v4l2->interlaced_fieldmode = EM28XX_INTERLACED_DEFAULT;
 
 	/* Analog specific initialization */
-	dev->format = &format[0];
+	v4l2->format = &format[0];
 
 	maxw = norm_maxw(dev);
 	/* MaxPacketSize for em2800 is too small to capture at full resolution
@@ -2376,74 +2467,75 @@ static int em28xx_v4l2_init(struct em28xx *dev)
 			 (EM28XX_XCLK_AUDIO_UNMUTE | val));
 
 	em28xx_set_outfmt(dev);
-	em28xx_compression_disable(dev);
 
 	/* Add image controls */
 	/* NOTE: at this point, the subdevices are already registered, so bridge
 	 * controls are only added/enabled when no subdevice provides them */
-	if (NULL == v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_CONTRAST))
-		v4l2_ctrl_new_std(&dev->ctrl_handler, &em28xx_ctrl_ops,
+	if (NULL == v4l2_ctrl_find(hdl, V4L2_CID_CONTRAST))
+		v4l2_ctrl_new_std(hdl, &em28xx_ctrl_ops,
 				  V4L2_CID_CONTRAST,
 				  0, 0x1f, 1, CONTRAST_DEFAULT);
-	if (NULL == v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_BRIGHTNESS))
-		v4l2_ctrl_new_std(&dev->ctrl_handler, &em28xx_ctrl_ops,
+	if (NULL == v4l2_ctrl_find(hdl, V4L2_CID_BRIGHTNESS))
+		v4l2_ctrl_new_std(hdl, &em28xx_ctrl_ops,
 				  V4L2_CID_BRIGHTNESS,
 				  -0x80, 0x7f, 1, BRIGHTNESS_DEFAULT);
-	if (NULL == v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_SATURATION))
-		v4l2_ctrl_new_std(&dev->ctrl_handler, &em28xx_ctrl_ops,
+	if (NULL == v4l2_ctrl_find(hdl, V4L2_CID_SATURATION))
+		v4l2_ctrl_new_std(hdl, &em28xx_ctrl_ops,
 				  V4L2_CID_SATURATION,
 				  0, 0x1f, 1, SATURATION_DEFAULT);
-	if (NULL == v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_BLUE_BALANCE))
-		v4l2_ctrl_new_std(&dev->ctrl_handler, &em28xx_ctrl_ops,
+	if (NULL == v4l2_ctrl_find(hdl, V4L2_CID_BLUE_BALANCE))
+		v4l2_ctrl_new_std(hdl, &em28xx_ctrl_ops,
 				  V4L2_CID_BLUE_BALANCE,
 				  -0x30, 0x30, 1, BLUE_BALANCE_DEFAULT);
-	if (NULL == v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_RED_BALANCE))
-		v4l2_ctrl_new_std(&dev->ctrl_handler, &em28xx_ctrl_ops,
+	if (NULL == v4l2_ctrl_find(hdl, V4L2_CID_RED_BALANCE))
+		v4l2_ctrl_new_std(hdl, &em28xx_ctrl_ops,
 				  V4L2_CID_RED_BALANCE,
 				  -0x30, 0x30, 1, RED_BALANCE_DEFAULT);
-	if (NULL == v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_SHARPNESS))
-		v4l2_ctrl_new_std(&dev->ctrl_handler, &em28xx_ctrl_ops,
+	if (NULL == v4l2_ctrl_find(hdl, V4L2_CID_SHARPNESS))
+		v4l2_ctrl_new_std(hdl, &em28xx_ctrl_ops,
 				  V4L2_CID_SHARPNESS,
 				  0, 0x0f, 1, SHARPNESS_DEFAULT);
 
 	/* Reset image controls */
 	em28xx_colorlevels_set_default(dev);
-	v4l2_ctrl_handler_setup(&dev->ctrl_handler);
-	ret = dev->ctrl_handler.error;
+	v4l2_ctrl_handler_setup(hdl);
+	ret = hdl->error;
 	if (ret)
 		goto unregister_dev;
 
 	/* allocate and fill video video_device struct */
-	dev->vdev = em28xx_vdev_init(dev, &em28xx_video_template, "video");
-	if (!dev->vdev) {
+	v4l2->vdev = em28xx_vdev_init(dev, &em28xx_video_template, "video");
+	if (!v4l2->vdev) {
 		em28xx_errdev("cannot allocate video_device.\n");
 		ret = -ENODEV;
 		goto unregister_dev;
 	}
-	dev->vdev->queue = &dev->vb_vidq;
-	dev->vdev->queue->lock = &dev->vb_queue_lock;
+	mutex_init(&v4l2->vb_queue_lock);
+	mutex_init(&v4l2->vb_vbi_queue_lock);
+	v4l2->vdev->queue = &v4l2->vb_vidq;
+	v4l2->vdev->queue->lock = &v4l2->vb_queue_lock;
 
 	/* disable inapplicable ioctls */
 	if (dev->board.is_webcam) {
-		v4l2_disable_ioctl(dev->vdev, VIDIOC_QUERYSTD);
-		v4l2_disable_ioctl(dev->vdev, VIDIOC_G_STD);
-		v4l2_disable_ioctl(dev->vdev, VIDIOC_S_STD);
+		v4l2_disable_ioctl(v4l2->vdev, VIDIOC_QUERYSTD);
+		v4l2_disable_ioctl(v4l2->vdev, VIDIOC_G_STD);
+		v4l2_disable_ioctl(v4l2->vdev, VIDIOC_S_STD);
 	} else {
-		v4l2_disable_ioctl(dev->vdev, VIDIOC_S_PARM);
+		v4l2_disable_ioctl(v4l2->vdev, VIDIOC_S_PARM);
 	}
 	if (dev->tuner_type == TUNER_ABSENT) {
-		v4l2_disable_ioctl(dev->vdev, VIDIOC_G_TUNER);
-		v4l2_disable_ioctl(dev->vdev, VIDIOC_S_TUNER);
-		v4l2_disable_ioctl(dev->vdev, VIDIOC_G_FREQUENCY);
-		v4l2_disable_ioctl(dev->vdev, VIDIOC_S_FREQUENCY);
+		v4l2_disable_ioctl(v4l2->vdev, VIDIOC_G_TUNER);
+		v4l2_disable_ioctl(v4l2->vdev, VIDIOC_S_TUNER);
+		v4l2_disable_ioctl(v4l2->vdev, VIDIOC_G_FREQUENCY);
+		v4l2_disable_ioctl(v4l2->vdev, VIDIOC_S_FREQUENCY);
 	}
 	if (!dev->audio_mode.has_audio) {
-		v4l2_disable_ioctl(dev->vdev, VIDIOC_G_AUDIO);
-		v4l2_disable_ioctl(dev->vdev, VIDIOC_S_AUDIO);
+		v4l2_disable_ioctl(v4l2->vdev, VIDIOC_G_AUDIO);
+		v4l2_disable_ioctl(v4l2->vdev, VIDIOC_S_AUDIO);
 	}
 
 	/* register v4l2 video video_device */
-	ret = video_register_device(dev->vdev, VFL_TYPE_GRABBER,
+	ret = video_register_device(v4l2->vdev, VFL_TYPE_GRABBER,
 				       video_nr[dev->devno]);
 	if (ret) {
 		em28xx_errdev("unable to register video device (error=%i).\n",
@@ -2453,27 +2545,27 @@ static int em28xx_v4l2_init(struct em28xx *dev)
 
 	/* Allocate and fill vbi video_device struct */
 	if (em28xx_vbi_supported(dev) == 1) {
-		dev->vbi_dev = em28xx_vdev_init(dev, &em28xx_video_template,
+		v4l2->vbi_dev = em28xx_vdev_init(dev, &em28xx_video_template,
 						"vbi");
 
-		dev->vbi_dev->queue = &dev->vb_vbiq;
-		dev->vbi_dev->queue->lock = &dev->vb_vbi_queue_lock;
+		v4l2->vbi_dev->queue = &v4l2->vb_vbiq;
+		v4l2->vbi_dev->queue->lock = &v4l2->vb_vbi_queue_lock;
 
 		/* disable inapplicable ioctls */
-		v4l2_disable_ioctl(dev->vdev, VIDIOC_S_PARM);
+		v4l2_disable_ioctl(v4l2->vdev, VIDIOC_S_PARM);
 		if (dev->tuner_type == TUNER_ABSENT) {
-			v4l2_disable_ioctl(dev->vbi_dev, VIDIOC_G_TUNER);
-			v4l2_disable_ioctl(dev->vbi_dev, VIDIOC_S_TUNER);
-			v4l2_disable_ioctl(dev->vbi_dev, VIDIOC_G_FREQUENCY);
-			v4l2_disable_ioctl(dev->vbi_dev, VIDIOC_S_FREQUENCY);
+			v4l2_disable_ioctl(v4l2->vbi_dev, VIDIOC_G_TUNER);
+			v4l2_disable_ioctl(v4l2->vbi_dev, VIDIOC_S_TUNER);
+			v4l2_disable_ioctl(v4l2->vbi_dev, VIDIOC_G_FREQUENCY);
+			v4l2_disable_ioctl(v4l2->vbi_dev, VIDIOC_S_FREQUENCY);
 		}
 		if (!dev->audio_mode.has_audio) {
-			v4l2_disable_ioctl(dev->vbi_dev, VIDIOC_G_AUDIO);
-			v4l2_disable_ioctl(dev->vbi_dev, VIDIOC_S_AUDIO);
+			v4l2_disable_ioctl(v4l2->vbi_dev, VIDIOC_G_AUDIO);
+			v4l2_disable_ioctl(v4l2->vbi_dev, VIDIOC_S_AUDIO);
 		}
 
 		/* register v4l2 vbi video_device */
-		ret = video_register_device(dev->vbi_dev, VFL_TYPE_VBI,
+		ret = video_register_device(v4l2->vbi_dev, VFL_TYPE_VBI,
 					    vbi_nr[dev->devno]);
 		if (ret < 0) {
 			em28xx_errdev("unable to register vbi device\n");
@@ -2482,32 +2574,32 @@ static int em28xx_v4l2_init(struct em28xx *dev)
 	}
 
 	if (em28xx_boards[dev->model].radio.type == EM28XX_RADIO) {
-		dev->radio_dev = em28xx_vdev_init(dev, &em28xx_radio_template,
-						  "radio");
-		if (!dev->radio_dev) {
+		v4l2->radio_dev = em28xx_vdev_init(dev, &em28xx_radio_template,
+						   "radio");
+		if (!v4l2->radio_dev) {
 			em28xx_errdev("cannot allocate video_device.\n");
 			ret = -ENODEV;
 			goto unregister_dev;
 		}
-		ret = video_register_device(dev->radio_dev, VFL_TYPE_RADIO,
+		ret = video_register_device(v4l2->radio_dev, VFL_TYPE_RADIO,
 					    radio_nr[dev->devno]);
 		if (ret < 0) {
 			em28xx_errdev("can't register radio device\n");
 			goto unregister_dev;
 		}
 		em28xx_info("Registered radio device as %s\n",
-			    video_device_node_name(dev->radio_dev));
+			    video_device_node_name(v4l2->radio_dev));
 	}
 
 	em28xx_info("V4L2 video device registered as %s\n",
-		    video_device_node_name(dev->vdev));
+		    video_device_node_name(v4l2->vdev));
 
-	if (dev->vbi_dev)
+	if (v4l2->vbi_dev)
 		em28xx_info("V4L2 VBI device registered as %s\n",
-			    video_device_node_name(dev->vbi_dev));
+			    video_device_node_name(v4l2->vbi_dev));
 
 	/* Save some power by putting tuner to sleep */
-	v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_power, 0);
+	v4l2_device_call_all(&v4l2->v4l2_dev, 0, core, s_power, 0);
 
 	/* initialize videobuf2 stuff */
 	em28xx_vb2_setup(dev);
@@ -2520,9 +2612,11 @@ static int em28xx_v4l2_init(struct em28xx *dev)
 	return 0;
 
 unregister_dev:
-	v4l2_ctrl_handler_free(&dev->ctrl_handler);
-	v4l2_device_unregister(&dev->v4l2_dev);
+	v4l2_ctrl_handler_free(&v4l2->ctrl_handler);
+	v4l2_device_unregister(&v4l2->v4l2_dev);
 err:
+	dev->v4l2 = NULL;
+	kref_put(&v4l2->ref, em28xx_free_v4l2);
 	mutex_unlock(&dev->lock);
 	return ret;
 }
diff --git a/drivers/media/usb/em28xx/em28xx.h b/drivers/media/usb/em28xx/em28xx.h
index 2051fc9fb93..b4c837d77e5 100644
--- a/drivers/media/usb/em28xx/em28xx.h
+++ b/drivers/media/usb/em28xx/em28xx.h
@@ -140,6 +140,7 @@
 #define EM2765_BOARD_SPEEDLINK_VAD_LAPLACE	  91
 #define EM28178_BOARD_PCTV_461E                   92
 #define EM2874_BOARD_KWORLD_UB435Q_V3		  93
+#define EM28178_BOARD_PCTV_292E                   94
 
 /* Limits minimum and default number of buffers */
 #define EM28XX_MIN_BUF 4
@@ -497,6 +498,60 @@ struct em28xx_eeprom {
 #define EM28XX_RESOURCE_VIDEO 0x01
 #define EM28XX_RESOURCE_VBI   0x02
 
+struct em28xx_v4l2 {
+	struct kref ref;
+	struct em28xx *dev;
+
+	struct v4l2_device v4l2_dev;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_clk *clk;
+
+	struct video_device *vdev;
+	struct video_device *vbi_dev;
+	struct video_device *radio_dev;
+
+	/* Videobuf2 */
+	struct vb2_queue vb_vidq;
+	struct vb2_queue vb_vbiq;
+	struct mutex vb_queue_lock;
+	struct mutex vb_vbi_queue_lock;
+
+	u8 vinmode;
+	u8 vinctl;
+
+	/* Camera specific fields */
+	int sensor_xres;
+	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 */
+
+	struct em28xx_fmt *format;
+	v4l2_std_id norm;	/* selected tv norm */
+
+	/* Progressive/interlaced mode */
+	bool progressive;
+	int interlaced_fieldmode; /* 1=interlaced fields, 0=just top fields */
+	/* FIXME: everything else than interlaced_fieldmode=1 doesn't work */
+
+	/* Frame properties */
+	int width;		/* current frame width */
+	int height;		/* current frame height */
+	unsigned hscale;	/* horizontal scale factor (see datasheet) */
+	unsigned vscale;	/* vertical scale factor (see datasheet) */
+	unsigned int vbi_width;
+	unsigned int vbi_height; /* lines per field */
+
+	/* Capture state tracking */
+	int capture_type;
+	bool top_field;
+	int vbi_read;
+	unsigned int field_count;
+};
+
 struct em28xx_audio {
 	char name[50];
 	unsigned num_urb;
@@ -513,6 +568,10 @@ struct em28xx_audio {
 
 	int users;
 	spinlock_t slock;
+
+	/* Controls streaming */
+	struct work_struct wq_trigger;	/* trigger to start/stop audio */
+	atomic_t       stream_started;	/* stream should be running if true */
 };
 
 struct em28xx;
@@ -541,6 +600,12 @@ struct em28xx_i2c_bus {
 struct em28xx {
 	struct kref ref;
 
+	/* Sub-module data */
+	struct em28xx_v4l2 *v4l2;
+	struct em28xx_dvb *dvb;
+	struct em28xx_audio adev;
+	struct em28xx_IR *ir;
+
 	/* generic device properties */
 	char name[30];		/* name (including minor) of the device */
 	int model;		/* index in the device_data struct */
@@ -554,29 +619,9 @@ struct em28xx {
 	unsigned int has_alsa_audio:1;
 	unsigned int is_audio_only:1;
 
-	struct v4l2_device v4l2_dev;
-	struct v4l2_ctrl_handler ctrl_handler;
-	struct v4l2_clk *clk;
 	struct em28xx_board board;
 
-	/* Webcam specific fields */
-	enum em28xx_sensor em28xx_sensor;
-	int sensor_xres, sensor_yres;
-	int sensor_xtal;
-
-	/* Progressive (non-interlaced) mode */
-	int progressive;
-
-	/* Vinmode/Vinctl used at the driver */
-	int vinmode, vinctl;
-
-	/* Controls audio streaming */
-	struct work_struct wq_trigger;	/* Trigger to start/stop audio for alsa module */
-	atomic_t       stream_started;	/* stream should be running if true */
-
-	struct em28xx_fmt *format;
-
-	struct em28xx_IR *ir;
+	enum em28xx_sensor em28xx_sensor;	/* camera specific */
 
 	/* Some older em28xx chips needs a waiting time after writing */
 	unsigned int wait_after_write;
@@ -588,8 +633,6 @@ struct em28xx {
 	struct em28xx_audio_mode audio_mode;
 
 	int tuner_type;		/* type of the tuner */
-	int tuner_addr;		/* tuner address */
-	int tda9887_conf;
 
 	/* i2c i/o */
 	struct i2c_adapter i2c_adap[NUM_I2C_BUSES];
@@ -602,52 +645,21 @@ struct em28xx {
 	struct rt_mutex i2c_bus_lock;
 
 	/* video for linux */
-	int users;		/* user count for exclusive use */
-	int streaming_users;    /* Number of actively streaming users */
-	struct video_device *vdev;	/* video for linux device struct */
-	v4l2_std_id norm;	/* selected tv norm */
-	int ctl_freq;		/* selected frequency */
 	unsigned int ctl_input;	/* selected input */
 	unsigned int ctl_ainput;/* selected audio input */
 	unsigned int ctl_aoutput;/* selected audio output */
 	int mute;
 	int volume;
-	/* frame properties */
-	int width;		/* current frame width */
-	int height;		/* current frame height */
-	unsigned hscale;	/* horizontal scale factor (see datasheet) */
-	unsigned vscale;	/* vertical scale factor (see datasheet) */
-	int interlaced;		/* 1=interlace fileds, 0=just top fileds */
-	unsigned int video_bytesread;	/* Number of bytes read */
 
 	unsigned long hash;	/* eeprom hash - for boards with generic ID */
 	unsigned long i2c_hash;	/* i2c devicelist hash -
 				   for boards with generic ID */
 
-	struct em28xx_audio adev;
-
-	/* capture state tracking */
-	int capture_type;
-	unsigned char top_field:1;
-	int vbi_read;
-	unsigned int vbi_width;
-	unsigned int vbi_height; /* lines per field */
-
 	struct work_struct         request_module_wk;
 
 	/* locks */
 	struct mutex lock;
 	struct mutex ctrl_urb_lock;	/* protects urb_buf */
-	/* spinlock_t queue_lock; */
-	struct list_head inqueue, outqueue;
-	struct video_device *vbi_dev;
-	struct video_device *radio_dev;
-
-	/* Videobuf2 */
-	struct vb2_queue vb_vidq;
-	struct vb2_queue vb_vbiq;
-	struct mutex vb_queue_lock;
-	struct mutex vb_vbi_queue_lock;
 
 	/* resources in use */
 	unsigned int resources;
@@ -662,9 +674,6 @@ struct em28xx {
 	struct em28xx_usb_ctl usb_ctl;
 	spinlock_t slock;
 
-	unsigned int field_count;
-	unsigned int vbi_field_count;
-
 	/* usb transfer */
 	struct usb_device *udev;	/* the usb device */
 	u8 ifnum;		/* number of the assigned usb interface */
@@ -708,8 +717,6 @@ struct em28xx {
 	/* Snapshot button input device */
 	char snapshot_button_path[30];	/* path of the input dev */
 	struct input_dev *sbutton_input_dev;
-
-	struct em28xx_dvb *dvb;
 };
 
 #define kref_to_dev(d) container_of(d, struct em28xx, ref)
@@ -797,32 +804,4 @@ int em28xx_init_camera(struct em28xx *dev);
 	printk(KERN_WARNING "%s: "fmt,\
 			dev->name , ##arg); } while (0)
 
-static inline int em28xx_compression_disable(struct em28xx *dev)
-{
-	/* side effect of disabling scaler and mixer */
-	return em28xx_write_reg(dev, EM28XX_R26_COMPR, 0x00);
-}
-
-/*FIXME: maxw should be dependent of alt mode */
-static inline unsigned int norm_maxw(struct em28xx *dev)
-{
-	if (dev->board.is_webcam)
-		return dev->sensor_xres;
-
-	if (dev->board.max_range_640_480)
-		return 640;
-
-	return 720;
-}
-
-static inline unsigned int norm_maxh(struct em28xx *dev)
-{
-	if (dev->board.is_webcam)
-		return dev->sensor_yres;
-
-	if (dev->board.max_range_640_480)
-		return 480;
-
-	return (dev->norm & V4L2_STD_625_50) ? 576 : 480;
-}
 #endif
diff --git a/drivers/media/usb/gspca/Kconfig b/drivers/media/usb/gspca/Kconfig
index 4f0c6d566c8..eed10d78253 100644
--- a/drivers/media/usb/gspca/Kconfig
+++ b/drivers/media/usb/gspca/Kconfig
@@ -50,6 +50,16 @@ config USB_GSPCA_CPIA1
 	  To compile this driver as a module, choose M here: the
 	  module will be called gspca_cpia1.
 
+config USB_GSPCA_DTCS033
+	tristate "DTCS033 (Scopium) USB Astro-Camera Driver"
+	depends on VIDEO_V4L2 && USB_GSPCA
+	help
+	  Say Y here if you want support for the Scopium camera
+	  for planetary astrophotography.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called gspca_dtcs033.
+
 config USB_GSPCA_ETOMS
 	tristate "Etoms USB Camera Driver"
 	depends on VIDEO_V4L2 && USB_GSPCA
diff --git a/drivers/media/usb/gspca/Makefile b/drivers/media/usb/gspca/Makefile
index 5855131ab8b..f46975e4c82 100644
--- a/drivers/media/usb/gspca/Makefile
+++ b/drivers/media/usb/gspca/Makefile
@@ -2,6 +2,7 @@ obj-$(CONFIG_USB_GSPCA)          += gspca_main.o
 obj-$(CONFIG_USB_GSPCA_BENQ)     += gspca_benq.o
 obj-$(CONFIG_USB_GSPCA_CONEX)    += gspca_conex.o
 obj-$(CONFIG_USB_GSPCA_CPIA1)    += gspca_cpia1.o
+obj-$(CONFIG_USB_GSPCA_DTCS033)  += gspca_dtcs033.o
 obj-$(CONFIG_USB_GSPCA_ETOMS)    += gspca_etoms.o
 obj-$(CONFIG_USB_GSPCA_FINEPIX)  += gspca_finepix.o
 obj-$(CONFIG_USB_GSPCA_JEILINJ)  += gspca_jeilinj.o
@@ -48,6 +49,7 @@ gspca_main-objs     := gspca.o autogain_functions.o
 gspca_benq-objs     := benq.o
 gspca_conex-objs    := conex.o
 gspca_cpia1-objs    := cpia1.o
+gspca_dtcs033-objs  := dtcs033.o
 gspca_etoms-objs    := etoms.o
 gspca_finepix-objs  := finepix.o
 gspca_jeilinj-objs  := jeilinj.o
diff --git a/drivers/media/usb/gspca/dtcs033.c b/drivers/media/usb/gspca/dtcs033.c
new file mode 100644
index 00000000000..96bfd4e0f0e
--- /dev/null
+++ b/drivers/media/usb/gspca/dtcs033.c
@@ -0,0 +1,441 @@
+/*
+ * Subdriver for Scopium astro-camera (DTCS033, 0547:7303)
+ *
+ * Copyright (C) 2014 Robert Butora (robert.butora.fi@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
+ * 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 pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+#define MODULE_NAME "dtcs033"
+#include "gspca.h"
+
+MODULE_AUTHOR("Robert Butora <robert.butora.fi@gmail.com>");
+MODULE_DESCRIPTION("Scopium DTCS033 astro-cam USB Camera Driver");
+MODULE_LICENSE("GPL");
+
+struct dtcs033_usb_requests {
+	u8 bRequestType;
+	u8 bRequest;
+	u16 wValue;
+	u16 wIndex;
+	u16 wLength;
+};
+
+/* send a usb request */
+static void reg_rw(struct gspca_dev *gspca_dev,
+		u8 bRequestType, u8 bRequest,
+		u16 wValue, u16 wIndex, u16 wLength)
+{
+	struct usb_device *udev = gspca_dev->dev;
+	int ret;
+
+	if (gspca_dev->usb_err < 0)
+		return;
+
+	ret = usb_control_msg(udev,
+		usb_rcvctrlpipe(udev, 0),
+		bRequest,
+		bRequestType,
+		wValue, wIndex,
+		gspca_dev->usb_buf, wLength, 500);
+
+	if (ret < 0) {
+		gspca_dev->usb_err = ret;
+		pr_err("usb_control_msg error %d\n", ret);
+	}
+
+	return;
+}
+/* send several usb in/out requests */
+static int reg_reqs(struct gspca_dev *gspca_dev,
+		    const struct dtcs033_usb_requests *preqs, int n_reqs)
+{
+	int i = 0;
+	const struct dtcs033_usb_requests *preq;
+
+	while ((i < n_reqs) && (gspca_dev->usb_err >= 0)) {
+
+		preq = &preqs[i];
+
+		reg_rw(gspca_dev, preq->bRequestType, preq->bRequest,
+			preq->wValue, preq->wIndex, preq->wLength);
+
+		if (gspca_dev->usb_err < 0) {
+
+			PERR("usb error request no: %d / %d\n",
+				i, n_reqs);
+		} else if (preq->bRequestType & USB_DIR_IN) {
+
+			PDEBUG(D_STREAM,
+			"USB IN (%d) returned[%d] %02X %02X %02X %s",
+				i,
+				preq->wLength,
+				gspca_dev->usb_buf[0],
+				gspca_dev->usb_buf[1],
+				gspca_dev->usb_buf[2],
+				preq->wLength > 3 ? "...\n" : "\n");
+		}
+
+		i++;
+	}
+	return gspca_dev->usb_err;
+}
+
+/* -- subdriver interface implementation -- */
+
+#define DT_COLS (640)
+static const struct v4l2_pix_format dtcs033_mode[] = {
+	/* raw Bayer patterned output */
+	{DT_COLS, 480, V4L2_PIX_FMT_GREY, V4L2_FIELD_NONE,
+		.bytesperline = DT_COLS,
+		.sizeimage = DT_COLS*480,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+	},
+	/* this mode will demosaic the Bayer pattern */
+	{DT_COLS, 480, V4L2_PIX_FMT_SRGGB8, V4L2_FIELD_NONE,
+		.bytesperline = DT_COLS,
+		.sizeimage = DT_COLS*480,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+	}
+};
+
+/* config called at probe time */
+static int sd_config(struct gspca_dev *gspca_dev,
+		const struct usb_device_id *id)
+{
+	gspca_dev->cam.cam_mode = dtcs033_mode;
+	gspca_dev->cam.nmodes = ARRAY_SIZE(dtcs033_mode);
+
+	gspca_dev->cam.bulk = 1;
+	gspca_dev->cam.bulk_nurbs = 1;
+	gspca_dev->cam.bulk_size = DT_COLS*512;
+
+	return 0;
+}
+
+/* init called at probe and resume time */
+static int sd_init(struct gspca_dev *gspca_dev)
+{
+	return 0;
+}
+
+/* start stop the camera */
+static int  dtcs033_start(struct gspca_dev *gspca_dev);
+static void dtcs033_stopN(struct gspca_dev *gspca_dev);
+
+/* intercept camera image data */
+static void dtcs033_pkt_scan(struct gspca_dev *gspca_dev,
+			u8 *data,  /* packet data */
+			int len)   /* packet data length */
+{
+	/* drop incomplete frames */
+	if (len != DT_COLS*512) {
+		gspca_dev->last_packet_type = DISCARD_PACKET;
+		/* gspca.c: discard invalidates the whole frame. */
+		return;
+	}
+
+	/* forward complete frames */
+	gspca_frame_add(gspca_dev, FIRST_PACKET, NULL, 0);
+	gspca_frame_add(gspca_dev, INTER_PACKET,
+		data + 16*DT_COLS,
+		len  - 32*DT_COLS); /* skip first & last 16 lines */
+	gspca_frame_add(gspca_dev, LAST_PACKET,  NULL, 0);
+
+	return;
+}
+
+/* -- controls: exposure and gain -- */
+
+static void dtcs033_setexposure(struct gspca_dev *gspca_dev,
+			s32 expo, s32 gain)
+{
+	/* gain [dB] encoding */
+	u16 sGain   = (u16)gain;
+	u16 gainVal = 224+(sGain-14)*(768-224)/(33-14);
+	u16 wIndex =  0x0100|(0x00FF&gainVal);
+	u16 wValue = (0xFF00&gainVal)>>8;
+
+	/* exposure time [msec] encoding */
+	u16 sXTime   = (u16)expo;
+	u16 xtimeVal = (524*(150-(sXTime-1)))/150;
+
+	const u8 bRequestType =
+		USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE;
+	const u8 bRequest = 0x18;
+
+	reg_rw(gspca_dev,
+		bRequestType, bRequest, wValue, wIndex, 0);
+	if (gspca_dev->usb_err < 0)
+		PERR("usb error in setexposure(gain) sequence.\n");
+
+	reg_rw(gspca_dev,
+		bRequestType, bRequest, (xtimeVal<<4), 0x6300, 0);
+	if (gspca_dev->usb_err < 0)
+		PERR("usb error in setexposure(time) sequence.\n");
+}
+
+/* specific webcam descriptor */
+struct sd {
+	struct gspca_dev gspca_dev;/* !! must be the first item */
+
+	/* exposure & gain controls */
+	struct {
+		struct v4l2_ctrl *exposure;
+		struct v4l2_ctrl *gain;
+	};
+};
+
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+	container_of(ctrl->handler,
+		struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	gspca_dev->usb_err = 0;
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE:
+		dtcs033_setexposure(gspca_dev,
+				ctrl->val, sd->gain->val);
+		break;
+	case V4L2_CID_GAIN:
+		dtcs033_setexposure(gspca_dev,
+				sd->exposure->val, ctrl->val);
+		break;
+	}
+	return gspca_dev->usb_err;
+}
+
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
+
+static int dtcs033_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 2);
+	/*                               min max step default */
+	sd->exposure = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+				V4L2_CID_EXPOSURE,
+				1,  150,  1,  75);/* [msec] */
+	sd->gain     = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+				V4L2_CID_GAIN,
+				14,  33,  1,  24);/* [dB] */
+	if (hdl->error) {
+		PERR("Could not initialize controls: %d\n",
+			hdl->error);
+		return hdl->error;
+	}
+
+	v4l2_ctrl_cluster(2, &sd->exposure);
+	return 0;
+}
+
+/* sub-driver description */
+static const struct sd_desc sd_desc = {
+	.name     = MODULE_NAME,
+	.config   = sd_config,
+	.init     = sd_init,
+	.start    = dtcs033_start,
+	.stopN    = dtcs033_stopN,
+	.pkt_scan = dtcs033_pkt_scan,
+	.init_controls = dtcs033_init_controls,
+};
+
+/* -- module initialisation -- */
+
+static const struct usb_device_id device_table[] = {
+	{USB_DEVICE(0x0547, 0x7303)},
+	{}
+};
+MODULE_DEVICE_TABLE(usb, device_table);
+
+/* device connect */
+static int sd_probe(struct usb_interface *intf,
+			const struct usb_device_id *id)
+{
+	return gspca_dev_probe(intf, id,
+			&sd_desc, sizeof(struct sd),
+			THIS_MODULE);
+}
+
+static struct usb_driver sd_driver = {
+	.name       = MODULE_NAME,
+	.id_table   = device_table,
+	.probe      = sd_probe,
+	.disconnect   = gspca_disconnect,
+#ifdef CONFIG_PM
+	.suspend      = gspca_suspend,
+	.resume       = gspca_resume,
+	.reset_resume = gspca_resume,
+#endif
+};
+module_usb_driver(sd_driver);
+
+
+/* ---------------------------------------------------------
+ USB requests to start/stop the camera [USB 2.0 spec Ch.9].
+
+ bRequestType :
+ 0x40 =  USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+ 0xC0 =  USB_DIR_IN  | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+*/
+static const struct dtcs033_usb_requests dtcs033_start_reqs[] = {
+/* -- bRequest,wValue,wIndex,wLength */
+{ 0x40, 0x01, 0x0001, 0x000F, 0x0000 },
+{ 0x40, 0x01, 0x0000, 0x000F, 0x0000 },
+{ 0x40, 0x01, 0x0001, 0x000F, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x7F00, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x1001, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x0004, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x7F01, 0x0000 },
+{ 0x40, 0x18, 0x30E0, 0x0009, 0x0000 },
+{ 0x40, 0x18, 0x0500, 0x012C, 0x0000 },
+{ 0x40, 0x18, 0x0380, 0x0200, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x035C, 0x0000 },
+{ 0x40, 0x18, 0x05C0, 0x0438, 0x0000 },
+{ 0x40, 0x18, 0x0440, 0x0500, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x0668, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x0700, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x0800, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x0900, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x0A00, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x0B00, 0x0000 },
+{ 0x40, 0x18, 0x30E0, 0x6009, 0x0000 },
+{ 0x40, 0x18, 0x0500, 0x612C, 0x0000 },
+{ 0x40, 0x18, 0x2090, 0x6274, 0x0000 },
+{ 0x40, 0x18, 0x05C0, 0x6338, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x6400, 0x0000 },
+{ 0x40, 0x18, 0x05C0, 0x6538, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x6600, 0x0000 },
+{ 0x40, 0x18, 0x0680, 0x6744, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x6800, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x6900, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x6A00, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x6B00, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x6C00, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x6D00, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x6E00, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x808C, 0x0000 },
+{ 0x40, 0x18, 0x0010, 0x8101, 0x0000 },
+{ 0x40, 0x18, 0x30E0, 0x8200, 0x0000 },
+{ 0x40, 0x18, 0x0810, 0x832C, 0x0000 },
+{ 0x40, 0x18, 0x0680, 0x842B, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x8500, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x8600, 0x0000 },
+{ 0x40, 0x18, 0x0280, 0x8715, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x880C, 0x0000 },
+{ 0x40, 0x18, 0x0010, 0x8901, 0x0000 },
+{ 0x40, 0x18, 0x30E0, 0x8A00, 0x0000 },
+{ 0x40, 0x18, 0x0810, 0x8B2C, 0x0000 },
+{ 0x40, 0x18, 0x0680, 0x8C2B, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x8D00, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x8E00, 0x0000 },
+{ 0x40, 0x18, 0x0280, 0x8F15, 0x0000 },
+{ 0x40, 0x18, 0x0010, 0xD040, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0xD100, 0x0000 },
+{ 0x40, 0x18, 0x00B0, 0xD20A, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0xD300, 0x0000 },
+{ 0x40, 0x18, 0x30E2, 0xD40D, 0x0000 },
+{ 0x40, 0x18, 0x0001, 0xD5C0, 0x0000 },
+{ 0x40, 0x18, 0x00A0, 0xD60A, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0xD700, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x7F00, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x1501, 0x0000 },
+{ 0x40, 0x18, 0x0001, 0x01FF, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x0200, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x0304, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x1101, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x1201, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x1300, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x1400, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x1601, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x1800, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x1900, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x1A00, 0x0000 },
+{ 0x40, 0x18, 0x2000, 0x1B00, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x1C00, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x2100, 0x0000 },
+{ 0x40, 0x18, 0x00C0, 0x228E, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x3001, 0x0000 },
+{ 0x40, 0x18, 0x0010, 0x3101, 0x0000 },
+{ 0x40, 0x18, 0x0008, 0x3301, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x3400, 0x0000 },
+{ 0x40, 0x18, 0x0012, 0x3549, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x3620, 0x0000 },
+{ 0x40, 0x18, 0x0001, 0x3700, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x4000, 0x0000 },
+{ 0x40, 0x18, 0xFFFF, 0x41FF, 0x0000 },
+{ 0x40, 0x18, 0xFFFF, 0x42FF, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x500F, 0x0000 },
+{ 0x40, 0x18, 0x2272, 0x5108, 0x0000 },
+{ 0x40, 0x18, 0x2272, 0x5208, 0x0000 },
+{ 0x40, 0x18, 0xFFFF, 0x53FF, 0x0000 },
+{ 0x40, 0x18, 0xFFFF, 0x54FF, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x6000, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x6102, 0x0000 },
+{ 0x40, 0x18, 0x0010, 0x6214, 0x0000 },
+{ 0x40, 0x18, 0x0C80, 0x6300, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x6401, 0x0000 },
+{ 0x40, 0x18, 0x0680, 0x6551, 0x0000 },
+{ 0x40, 0x18, 0xFFFF, 0x66FF, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x6702, 0x0000 },
+{ 0x40, 0x18, 0x0010, 0x6800, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x6900, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x6A00, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x6B00, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x6C00, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x6D01, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x6E00, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x6F00, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x7000, 0x0000 },
+{ 0x40, 0x18, 0x0001, 0x7118, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x2001, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x1101, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x1301, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x1300, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x1501, 0x0000 },
+{ 0xC0, 0x11, 0x0000, 0x24C0, 0x0003 },
+{ 0x40, 0x18, 0x0000, 0x3000, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x3620, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x1501, 0x0000 },
+{ 0x40, 0x18, 0x0010, 0x6300, 0x0000 },
+{ 0x40, 0x18, 0x0002, 0x01F0, 0x0000 },
+{ 0x40, 0x01, 0x0003, 0x000F, 0x0000 }
+};
+
+static const struct dtcs033_usb_requests dtcs033_stop_reqs[] = {
+/* -- bRequest,wValue,wIndex,wLength */
+{ 0x40, 0x01, 0x0001, 0x000F, 0x0000 },
+{ 0x40, 0x01, 0x0000, 0x000F, 0x0000 },
+{ 0x40, 0x18, 0x0000, 0x0003, 0x0000 }
+};
+static int dtcs033_start(struct gspca_dev *gspca_dev)
+{
+	return reg_reqs(gspca_dev, dtcs033_start_reqs,
+		ARRAY_SIZE(dtcs033_start_reqs));
+}
+
+static void dtcs033_stopN(struct gspca_dev *gspca_dev)
+{
+	reg_reqs(gspca_dev, dtcs033_stop_reqs,
+		ARRAY_SIZE(dtcs033_stop_reqs));
+	return;
+}
diff --git a/drivers/media/usb/gspca/gl860/gl860-mi2020.c b/drivers/media/usb/gspca/gl860/gl860-mi2020.c
index 2edda6b7d65..a785828d4b9 100644
--- a/drivers/media/usb/gspca/gl860/gl860-mi2020.c
+++ b/drivers/media/usb/gspca/gl860/gl860-mi2020.c
@@ -35,32 +35,34 @@ static u8 dat_hvflip5[] = {0x8c, 0xa1, 0x03};
 static u8 dat_hvflip6[] = {0x90, 0x00, 0x06};
 
 static struct idxdata tbl_middle_hvflip_low[] = {
-	{0x33, "\x90\x00\x06"},
-	{6, "\xff\xff\xff"},
-	{0x33, "\x90\x00\x06"},
-	{6, "\xff\xff\xff"},
-	{0x33, "\x90\x00\x06"},
-	{6, "\xff\xff\xff"},
-	{0x33, "\x90\x00\x06"},
-	{6, "\xff\xff\xff"},
+	{0x33, {0x90, 0x00, 0x06}},
+	{6, {0xff, 0xff, 0xff}},
+	{0x33, {0x90, 0x00, 0x06}},
+	{6, {0xff, 0xff, 0xff}},
+	{0x33, {0x90, 0x00, 0x06}},
+	{6, {0xff, 0xff, 0xff}},
+	{0x33, {0x90, 0x00, 0x06}},
+	{6, {0xff, 0xff, 0xff}},
 };
 
 static struct idxdata tbl_middle_hvflip_big[] = {
-	{0x33, "\x8c\xa1\x03"}, {0x33, "\x90\x00\x01"}, {0x33, "\x8c\xa1\x20"},
-	{0x33, "\x90\x00\x00"}, {0x33, "\x8c\xa7\x02"}, {0x33, "\x90\x00\x00"},
-	{102, "\xff\xff\xff"},
-	{0x33, "\x8c\xa1\x03"}, {0x33, "\x90\x00\x02"}, {0x33, "\x8c\xa1\x20"},
-	{0x33, "\x90\x00\x72"}, {0x33, "\x8c\xa7\x02"}, {0x33, "\x90\x00\x01"},
+	{0x33, {0x8c, 0xa1, 0x03}}, {0x33, {0x90, 0x00, 0x01}},
+	{0x33, {0x8c, 0xa1, 0x20}}, {0x33, {0x90, 0x00, 0x00}},
+	{0x33, {0x8c, 0xa7, 0x02}}, {0x33, {0x90, 0x00, 0x00}},
+	{102, {0xff, 0xff, 0xff}},
+	{0x33, {0x8c, 0xa1, 0x03}}, {0x33, {0x90, 0x00, 0x02}},
+	{0x33, {0x8c, 0xa1, 0x20}}, {0x33, {0x90, 0x00, 0x72}},
+	{0x33, {0x8c, 0xa7, 0x02}}, {0x33, {0x90, 0x00, 0x01}},
 };
 
 static struct idxdata tbl_end_hvflip[] = {
-	{0x33, "\x8c\xa1\x02"}, {0x33, "\x90\x00\x1f"},
-	{6, "\xff\xff\xff"},
-	{0x33, "\x8c\xa1\x02"}, {0x33, "\x90\x00\x1f"},
-	{6, "\xff\xff\xff"},
-	{0x33, "\x8c\xa1\x02"}, {0x33, "\x90\x00\x1f"},
-	{6, "\xff\xff\xff"},
-	{0x33, "\x8c\xa1\x02"}, {0x33, "\x90\x00\x1f"},
+	{0x33, {0x8c, 0xa1, 0x02}}, {0x33, {0x90, 0x00, 0x1f}},
+	{6, {0xff, 0xff, 0xff}},
+	{0x33, {0x8c, 0xa1, 0x02}}, {0x33, {0x90, 0x00, 0x1f}},
+	{6, {0xff, 0xff, 0xff}},
+	{0x33, {0x8c, 0xa1, 0x02}}, {0x33, {0x90, 0x00, 0x1f}},
+	{6, {0xff, 0xff, 0xff}},
+	{0x33, {0x8c, 0xa1, 0x02}}, {0x33, {0x90, 0x00, 0x1f}},
 };
 
 static u8 dat_freq1[] = { 0x8c, 0xa4, 0x04 };
@@ -82,197 +84,267 @@ static struct validx tbl_common_0B[] = {
 };
 
 static struct idxdata tbl_common_3B[] = {
-	{0x33, "\x86\x25\x01"}, {0x33, "\x86\x25\x00"},
-	{2, "\xff\xff\xff"},
-	{0x30, "\x1a\x0a\xcc"}, {0x32, "\x02\x00\x08"}, {0x33, "\xf4\x03\x1d"},
-	{6, "\xff\xff\xff"}, /* 12 */
-	{0x34, "\x1e\x8f\x09"}, {0x34, "\x1c\x01\x28"}, {0x34, "\x1e\x8f\x09"},
-	{2, "\xff\xff\xff"}, /* - */
-	{0x34, "\x1e\x8f\x09"}, {0x32, "\x14\x06\xe6"}, {0x33, "\x8c\x22\x23"},
-	{0x33, "\x90\x00\x00"}, {0x33, "\x8c\xa2\x0f"}, {0x33, "\x90\x00\x0d"},
-	{0x33, "\x8c\xa2\x10"}, {0x33, "\x90\x00\x0b"}, {0x33, "\x8c\xa2\x11"},
-	{0x33, "\x90\x00\x07"}, {0x33, "\xf4\x03\x1d"}, {0x35, "\xa2\x00\xe2"},
-	{0x33, "\x8c\xab\x05"}, {0x33, "\x90\x00\x01"}, {0x32, "\x6e\x00\x86"},
-	{0x32, "\x70\x0f\xaa"}, {0x32, "\x72\x0f\xe4"}, {0x33, "\x8c\xa3\x4a"},
-	{0x33, "\x90\x00\x5a"}, {0x33, "\x8c\xa3\x4b"}, {0x33, "\x90\x00\xa6"},
-	{0x33, "\x8c\xa3\x61"}, {0x33, "\x90\x00\xc8"}, {0x33, "\x8c\xa3\x62"},
-	{0x33, "\x90\x00\xe1"}, {0x34, "\xce\x01\xa8"}, {0x34, "\xd0\x66\x33"},
-	{0x34, "\xd2\x31\x9a"}, {0x34, "\xd4\x94\x63"}, {0x34, "\xd6\x4b\x25"},
-	{0x34, "\xd8\x26\x70"}, {0x34, "\xda\x72\x4c"}, {0x34, "\xdc\xff\x04"},
-	{0x34, "\xde\x01\x5b"}, {0x34, "\xe6\x01\x13"}, {0x34, "\xee\x0b\xf0"},
-	{0x34, "\xf6\x0b\xa4"}, {0x35, "\x00\xf6\xe7"}, {0x35, "\x08\x0d\xfd"},
-	{0x35, "\x10\x25\x63"}, {0x35, "\x18\x35\x6c"}, {0x35, "\x20\x42\x7e"},
-	{0x35, "\x28\x19\x44"}, {0x35, "\x30\x39\xd4"}, {0x35, "\x38\xf5\xa8"},
-	{0x35, "\x4c\x07\x90"}, {0x35, "\x44\x07\xb8"}, {0x35, "\x5c\x06\x88"},
-	{0x35, "\x54\x07\xff"}, {0x34, "\xe0\x01\x52"}, {0x34, "\xe8\x00\xcc"},
-	{0x34, "\xf0\x0d\x83"}, {0x34, "\xf8\x0c\xb3"}, {0x35, "\x02\xfe\xba"},
-	{0x35, "\x0a\x04\xe0"}, {0x35, "\x12\x1c\x63"}, {0x35, "\x1a\x2b\x5a"},
-	{0x35, "\x22\x32\x5e"}, {0x35, "\x2a\x0d\x28"}, {0x35, "\x32\x2c\x02"},
-	{0x35, "\x3a\xf4\xfa"}, {0x35, "\x4e\x07\xef"}, {0x35, "\x46\x07\x88"},
-	{0x35, "\x5e\x07\xc1"}, {0x35, "\x56\x04\x64"}, {0x34, "\xe4\x01\x15"},
-	{0x34, "\xec\x00\x82"}, {0x34, "\xf4\x0c\xce"}, {0x34, "\xfc\x0c\xba"},
-	{0x35, "\x06\x1f\x02"}, {0x35, "\x0e\x02\xe3"}, {0x35, "\x16\x1a\x50"},
-	{0x35, "\x1e\x24\x39"}, {0x35, "\x26\x23\x4c"}, {0x35, "\x2e\xf9\x1b"},
-	{0x35, "\x36\x23\x19"}, {0x35, "\x3e\x12\x08"}, {0x35, "\x52\x07\x22"},
-	{0x35, "\x4a\x03\xd3"}, {0x35, "\x62\x06\x54"}, {0x35, "\x5a\x04\x5d"},
-	{0x34, "\xe2\x01\x04"}, {0x34, "\xea\x00\xa0"}, {0x34, "\xf2\x0c\xbc"},
-	{0x34, "\xfa\x0c\x5b"}, {0x35, "\x04\x17\xf2"}, {0x35, "\x0c\x02\x08"},
-	{0x35, "\x14\x28\x43"}, {0x35, "\x1c\x28\x62"}, {0x35, "\x24\x2b\x60"},
-	{0x35, "\x2c\x07\x33"}, {0x35, "\x34\x1f\xb0"}, {0x35, "\x3c\xed\xcd"},
-	{0x35, "\x50\x00\x06"}, {0x35, "\x48\x07\xff"}, {0x35, "\x60\x05\x89"},
-	{0x35, "\x58\x07\xff"}, {0x35, "\x40\x00\xa0"}, {0x35, "\x42\x00\x00"},
-	{0x32, "\x10\x01\xfc"}, {0x33, "\x8c\xa1\x18"}, {0x33, "\x90\x00\x3c"},
-	{0x33, "\x78\x00\x00"},
-	{2, "\xff\xff\xff"},
-	{0x35, "\xb8\x1f\x20"}, {0x33, "\x8c\xa2\x06"}, {0x33, "\x90\x00\x10"},
-	{0x33, "\x8c\xa2\x07"}, {0x33, "\x90\x00\x08"}, {0x33, "\x8c\xa2\x42"},
-	{0x33, "\x90\x00\x0b"}, {0x33, "\x8c\xa2\x4a"}, {0x33, "\x90\x00\x8c"},
-	{0x35, "\xba\xfa\x08"}, {0x33, "\x8c\xa2\x02"}, {0x33, "\x90\x00\x22"},
-	{0x33, "\x8c\xa2\x03"}, {0x33, "\x90\x00\xbb"}, {0x33, "\x8c\xa4\x04"},
-	{0x33, "\x90\x00\x80"}, {0x33, "\x8c\xa7\x9d"}, {0x33, "\x90\x00\x00"},
-	{0x33, "\x8c\xa7\x9e"}, {0x33, "\x90\x00\x00"}, {0x33, "\x8c\xa2\x0c"},
-	{0x33, "\x90\x00\x17"}, {0x33, "\x8c\xa2\x15"}, {0x33, "\x90\x00\x04"},
-	{0x33, "\x8c\xa2\x14"}, {0x33, "\x90\x00\x20"}, {0x33, "\x8c\xa1\x03"},
-	{0x33, "\x90\x00\x00"}, {0x33, "\x8c\x27\x17"}, {0x33, "\x90\x21\x11"},
-	{0x33, "\x8c\x27\x1b"}, {0x33, "\x90\x02\x4f"}, {0x33, "\x8c\x27\x25"},
-	{0x33, "\x90\x06\x0f"}, {0x33, "\x8c\x27\x39"}, {0x33, "\x90\x21\x11"},
-	{0x33, "\x8c\x27\x3d"}, {0x33, "\x90\x01\x20"}, {0x33, "\x8c\x27\x47"},
-	{0x33, "\x90\x09\x4c"}, {0x33, "\x8c\x27\x03"}, {0x33, "\x90\x02\x84"},
-	{0x33, "\x8c\x27\x05"}, {0x33, "\x90\x01\xe2"}, {0x33, "\x8c\x27\x07"},
-	{0x33, "\x90\x06\x40"}, {0x33, "\x8c\x27\x09"}, {0x33, "\x90\x04\xb0"},
-	{0x33, "\x8c\x27\x0d"}, {0x33, "\x90\x00\x00"}, {0x33, "\x8c\x27\x0f"},
-	{0x33, "\x90\x00\x00"}, {0x33, "\x8c\x27\x11"}, {0x33, "\x90\x04\xbd"},
-	{0x33, "\x8c\x27\x13"}, {0x33, "\x90\x06\x4d"}, {0x33, "\x8c\x27\x15"},
-	{0x33, "\x90\x00\x00"}, {0x33, "\x8c\x27\x17"}, {0x33, "\x90\x21\x11"},
-	{0x33, "\x8c\x27\x19"}, {0x33, "\x90\x04\x6c"}, {0x33, "\x8c\x27\x1b"},
-	{0x33, "\x90\x02\x4f"}, {0x33, "\x8c\x27\x1d"}, {0x33, "\x90\x01\x02"},
-	{0x33, "\x8c\x27\x1f"}, {0x33, "\x90\x02\x79"}, {0x33, "\x8c\x27\x21"},
-	{0x33, "\x90\x01\x55"}, {0x33, "\x8c\x27\x23"}, {0x33, "\x90\x02\x85"},
-	{0x33, "\x8c\x27\x25"}, {0x33, "\x90\x06\x0f"}, {0x33, "\x8c\x27\x27"},
-	{0x33, "\x90\x20\x20"}, {0x33, "\x8c\x27\x29"}, {0x33, "\x90\x20\x20"},
-	{0x33, "\x8c\x27\x2b"}, {0x33, "\x90\x10\x20"}, {0x33, "\x8c\x27\x2d"},
-	{0x33, "\x90\x20\x07"}, {0x33, "\x8c\x27\x2f"}, {0x33, "\x90\x00\x04"},
-	{0x33, "\x8c\x27\x31"}, {0x33, "\x90\x00\x04"}, {0x33, "\x8c\x27\x33"},
-	{0x33, "\x90\x04\xbb"}, {0x33, "\x8c\x27\x35"}, {0x33, "\x90\x06\x4b"},
-	{0x33, "\x8c\x27\x37"}, {0x33, "\x90\x00\x00"}, {0x33, "\x8c\x27\x39"},
-	{0x33, "\x90\x21\x11"}, {0x33, "\x8c\x27\x3b"}, {0x33, "\x90\x00\x24"},
-	{0x33, "\x8c\x27\x3d"}, {0x33, "\x90\x01\x20"}, {0x33, "\x8c\x27\x41"},
-	{0x33, "\x90\x01\x69"}, {0x33, "\x8c\x27\x45"}, {0x33, "\x90\x04\xed"},
-	{0x33, "\x8c\x27\x47"}, {0x33, "\x90\x09\x4c"}, {0x33, "\x8c\x27\x51"},
-	{0x33, "\x90\x00\x00"}, {0x33, "\x8c\x27\x53"}, {0x33, "\x90\x03\x20"},
-	{0x33, "\x8c\x27\x55"}, {0x33, "\x90\x00\x00"}, {0x33, "\x8c\x27\x57"},
-	{0x33, "\x90\x02\x58"}, {0x33, "\x8c\x27\x5f"}, {0x33, "\x90\x00\x00"},
-	{0x33, "\x8c\x27\x61"}, {0x33, "\x90\x06\x40"}, {0x33, "\x8c\x27\x63"},
-	{0x33, "\x90\x00\x00"}, {0x33, "\x8c\x27\x65"}, {0x33, "\x90\x04\xb0"},
-	{0x33, "\x8c\x22\x2e"}, {0x33, "\x90\x00\xa1"}, {0x33, "\x8c\xa4\x08"},
-	{0x33, "\x90\x00\x1f"}, {0x33, "\x8c\xa4\x09"}, {0x33, "\x90\x00\x21"},
-	{0x33, "\x8c\xa4\x0a"}, {0x33, "\x90\x00\x25"}, {0x33, "\x8c\xa4\x0b"},
-	{0x33, "\x90\x00\x27"}, {0x33, "\x8c\x24\x11"}, {0x33, "\x90\x00\xa1"},
-	{0x33, "\x8c\x24\x13"}, {0x33, "\x90\x00\xc1"}, {0x33, "\x8c\x24\x15"},
-	{0x33, "\x90\x00\x6a"}, {0x33, "\x8c\x24\x17"}, {0x33, "\x90\x00\x80"},
-	{0x33, "\x8c\xa1\x03"}, {0x33, "\x90\x00\x05"},
-	{2, "\xff\xff\xff"},
-	{0x33, "\x8c\xa1\x03"}, {0x33, "\x90\x00\x06"},
-	{3, "\xff\xff\xff"},
+	{0x33, {0x86, 0x25, 0x01}}, {0x33, {0x86, 0x25, 0x00}},
+	{2, {0xff, 0xff, 0xff}},
+	{0x30, {0x1a, 0x0a, 0xcc}}, {0x32, {0x02, 0x00, 0x08}},
+	{0x33, {0xf4, 0x03, 0x1d}},
+	{6, {0xff, 0xff, 0xff}}, /* 12 */
+	{0x34, {0x1e, 0x8f, 0x09}}, {0x34, {0x1c, 0x01, 0x28}},
+	{0x34, {0x1e, 0x8f, 0x09}},
+	{2, {0xff, 0xff, 0xff}}, /* - */
+	{0x34, {0x1e, 0x8f, 0x09}}, {0x32, {0x14, 0x06, 0xe6}},
+	{0x33, {0x8c, 0x22, 0x23}}, {0x33, {0x90, 0x00, 0x00}},
+	{0x33, {0x8c, 0xa2, 0x0f}}, {0x33, {0x90, 0x00, 0x0d}},
+	{0x33, {0x8c, 0xa2, 0x10}}, {0x33, {0x90, 0x00, 0x0b}},
+	{0x33, {0x8c, 0xa2, 0x11}}, {0x33, {0x90, 0x00, 0x07}},
+	{0x33, {0xf4, 0x03, 0x1d}}, {0x35, {0xa2, 0x00, 0xe2}},
+	{0x33, {0x8c, 0xab, 0x05}}, {0x33, {0x90, 0x00, 0x01}},
+	{0x32, {0x6e, 0x00, 0x86}}, {0x32, {0x70, 0x0f, 0xaa}},
+	{0x32, {0x72, 0x0f, 0xe4}}, {0x33, {0x8c, 0xa3, 0x4a}},
+	{0x33, {0x90, 0x00, 0x5a}}, {0x33, {0x8c, 0xa3, 0x4b}},
+	{0x33, {0x90, 0x00, 0xa6}}, {0x33, {0x8c, 0xa3, 0x61}},
+	{0x33, {0x90, 0x00, 0xc8}}, {0x33, {0x8c, 0xa3, 0x62}},
+	{0x33, {0x90, 0x00, 0xe1}}, {0x34, {0xce, 0x01, 0xa8}},
+	{0x34, {0xd0, 0x66, 0x33}}, {0x34, {0xd2, 0x31, 0x9a}},
+	{0x34, {0xd4, 0x94, 0x63}}, {0x34, {0xd6, 0x4b, 0x25}},
+	{0x34, {0xd8, 0x26, 0x70}}, {0x34, {0xda, 0x72, 0x4c}},
+	{0x34, {0xdc, 0xff, 0x04}}, {0x34, {0xde, 0x01, 0x5b}},
+	{0x34, {0xe6, 0x01, 0x13}}, {0x34, {0xee, 0x0b, 0xf0}},
+	{0x34, {0xf6, 0x0b, 0xa4}}, {0x35, {0x00, 0xf6, 0xe7}},
+	{0x35, {0x08, 0x0d, 0xfd}}, {0x35, {0x10, 0x25, 0x63}},
+	{0x35, {0x18, 0x35, 0x6c}}, {0x35, {0x20, 0x42, 0x7e}},
+	{0x35, {0x28, 0x19, 0x44}}, {0x35, {0x30, 0x39, 0xd4}},
+	{0x35, {0x38, 0xf5, 0xa8}}, {0x35, {0x4c, 0x07, 0x90}},
+	{0x35, {0x44, 0x07, 0xb8}}, {0x35, {0x5c, 0x06, 0x88}},
+	{0x35, {0x54, 0x07, 0xff}}, {0x34, {0xe0, 0x01, 0x52}},
+	{0x34, {0xe8, 0x00, 0xcc}}, {0x34, {0xf0, 0x0d, 0x83}},
+	{0x34, {0xf8, 0x0c, 0xb3}}, {0x35, {0x02, 0xfe, 0xba}},
+	{0x35, {0x0a, 0x04, 0xe0}}, {0x35, {0x12, 0x1c, 0x63}},
+	{0x35, {0x1a, 0x2b, 0x5a}}, {0x35, {0x22, 0x32, 0x5e}},
+	{0x35, {0x2a, 0x0d, 0x28}}, {0x35, {0x32, 0x2c, 0x02}},
+	{0x35, {0x3a, 0xf4, 0xfa}}, {0x35, {0x4e, 0x07, 0xef}},
+	{0x35, {0x46, 0x07, 0x88}}, {0x35, {0x5e, 0x07, 0xc1}},
+	{0x35, {0x56, 0x04, 0x64}}, {0x34, {0xe4, 0x01, 0x15}},
+	{0x34, {0xec, 0x00, 0x82}}, {0x34, {0xf4, 0x0c, 0xce}},
+	{0x34, {0xfc, 0x0c, 0xba}}, {0x35, {0x06, 0x1f, 0x02}},
+	{0x35, {0x0e, 0x02, 0xe3}}, {0x35, {0x16, 0x1a, 0x50}},
+	{0x35, {0x1e, 0x24, 0x39}}, {0x35, {0x26, 0x23, 0x4c}},
+	{0x35, {0x2e, 0xf9, 0x1b}}, {0x35, {0x36, 0x23, 0x19}},
+	{0x35, {0x3e, 0x12, 0x08}}, {0x35, {0x52, 0x07, 0x22}},
+	{0x35, {0x4a, 0x03, 0xd3}}, {0x35, {0x62, 0x06, 0x54}},
+	{0x35, {0x5a, 0x04, 0x5d}}, {0x34, {0xe2, 0x01, 0x04}},
+	{0x34, {0xea, 0x00, 0xa0}}, {0x34, {0xf2, 0x0c, 0xbc}},
+	{0x34, {0xfa, 0x0c, 0x5b}}, {0x35, {0x04, 0x17, 0xf2}},
+	{0x35, {0x0c, 0x02, 0x08}}, {0x35, {0x14, 0x28, 0x43}},
+	{0x35, {0x1c, 0x28, 0x62}}, {0x35, {0x24, 0x2b, 0x60}},
+	{0x35, {0x2c, 0x07, 0x33}}, {0x35, {0x34, 0x1f, 0xb0}},
+	{0x35, {0x3c, 0xed, 0xcd}}, {0x35, {0x50, 0x00, 0x06}},
+	{0x35, {0x48, 0x07, 0xff}}, {0x35, {0x60, 0x05, 0x89}},
+	{0x35, {0x58, 0x07, 0xff}}, {0x35, {0x40, 0x00, 0xa0}},
+	{0x35, {0x42, 0x00, 0x00}}, {0x32, {0x10, 0x01, 0xfc}},
+	{0x33, {0x8c, 0xa1, 0x18}}, {0x33, {0x90, 0x00, 0x3c}},
+	{0x33, {0x78, 0x00, 0x00}},
+	{2, {0xff, 0xff, 0xff}},
+	{0x35, {0xb8, 0x1f, 0x20}}, {0x33, {0x8c, 0xa2, 0x06}},
+	{0x33, {0x90, 0x00, 0x10}}, {0x33, {0x8c, 0xa2, 0x07}},
+	{0x33, {0x90, 0x00, 0x08}}, {0x33, {0x8c, 0xa2, 0x42}},
+	{0x33, {0x90, 0x00, 0x0b}}, {0x33, {0x8c, 0xa2, 0x4a}},
+	{0x33, {0x90, 0x00, 0x8c}}, {0x35, {0xba, 0xfa, 0x08}},
+	{0x33, {0x8c, 0xa2, 0x02}}, {0x33, {0x90, 0x00, 0x22}},
+	{0x33, {0x8c, 0xa2, 0x03}}, {0x33, {0x90, 0x00, 0xbb}},
+	{0x33, {0x8c, 0xa4, 0x04}}, {0x33, {0x90, 0x00, 0x80}},
+	{0x33, {0x8c, 0xa7, 0x9d}}, {0x33, {0x90, 0x00, 0x00}},
+	{0x33, {0x8c, 0xa7, 0x9e}}, {0x33, {0x90, 0x00, 0x00}},
+	{0x33, {0x8c, 0xa2, 0x0c}}, {0x33, {0x90, 0x00, 0x17}},
+	{0x33, {0x8c, 0xa2, 0x15}}, {0x33, {0x90, 0x00, 0x04}},
+	{0x33, {0x8c, 0xa2, 0x14}}, {0x33, {0x90, 0x00, 0x20}},
+	{0x33, {0x8c, 0xa1, 0x03}}, {0x33, {0x90, 0x00, 0x00}},
+	{0x33, {0x8c, 0x27, 0x17}}, {0x33, {0x90, 0x21, 0x11}},
+	{0x33, {0x8c, 0x27, 0x1b}}, {0x33, {0x90, 0x02, 0x4f}},
+	{0x33, {0x8c, 0x27, 0x25}}, {0x33, {0x90, 0x06, 0x0f}},
+	{0x33, {0x8c, 0x27, 0x39}}, {0x33, {0x90, 0x21, 0x11}},
+	{0x33, {0x8c, 0x27, 0x3d}}, {0x33, {0x90, 0x01, 0x20}},
+	{0x33, {0x8c, 0x27, 0x47}}, {0x33, {0x90, 0x09, 0x4c}},
+	{0x33, {0x8c, 0x27, 0x03}}, {0x33, {0x90, 0x02, 0x84}},
+	{0x33, {0x8c, 0x27, 0x05}}, {0x33, {0x90, 0x01, 0xe2}},
+	{0x33, {0x8c, 0x27, 0x07}}, {0x33, {0x90, 0x06, 0x40}},
+	{0x33, {0x8c, 0x27, 0x09}}, {0x33, {0x90, 0x04, 0xb0}},
+	{0x33, {0x8c, 0x27, 0x0d}}, {0x33, {0x90, 0x00, 0x00}},
+	{0x33, {0x8c, 0x27, 0x0f}}, {0x33, {0x90, 0x00, 0x00}},
+	{0x33, {0x8c, 0x27, 0x11}}, {0x33, {0x90, 0x04, 0xbd}},
+	{0x33, {0x8c, 0x27, 0x13}}, {0x33, {0x90, 0x06, 0x4d}},
+	{0x33, {0x8c, 0x27, 0x15}}, {0x33, {0x90, 0x00, 0x00}},
+	{0x33, {0x8c, 0x27, 0x17}}, {0x33, {0x90, 0x21, 0x11}},
+	{0x33, {0x8c, 0x27, 0x19}}, {0x33, {0x90, 0x04, 0x6c}},
+	{0x33, {0x8c, 0x27, 0x1b}}, {0x33, {0x90, 0x02, 0x4f}},
+	{0x33, {0x8c, 0x27, 0x1d}}, {0x33, {0x90, 0x01, 0x02}},
+	{0x33, {0x8c, 0x27, 0x1f}}, {0x33, {0x90, 0x02, 0x79}},
+	{0x33, {0x8c, 0x27, 0x21}}, {0x33, {0x90, 0x01, 0x55}},
+	{0x33, {0x8c, 0x27, 0x23}}, {0x33, {0x90, 0x02, 0x85}},
+	{0x33, {0x8c, 0x27, 0x25}}, {0x33, {0x90, 0x06, 0x0f}},
+	{0x33, {0x8c, 0x27, 0x27}}, {0x33, {0x90, 0x20, 0x20}},
+	{0x33, {0x8c, 0x27, 0x29}}, {0x33, {0x90, 0x20, 0x20}},
+	{0x33, {0x8c, 0x27, 0x2b}}, {0x33, {0x90, 0x10, 0x20}},
+	{0x33, {0x8c, 0x27, 0x2d}}, {0x33, {0x90, 0x20, 0x07}},
+	{0x33, {0x8c, 0x27, 0x2f}}, {0x33, {0x90, 0x00, 0x04}},
+	{0x33, {0x8c, 0x27, 0x31}}, {0x33, {0x90, 0x00, 0x04}},
+	{0x33, {0x8c, 0x27, 0x33}}, {0x33, {0x90, 0x04, 0xbb}},
+	{0x33, {0x8c, 0x27, 0x35}}, {0x33, {0x90, 0x06, 0x4b}},
+	{0x33, {0x8c, 0x27, 0x37}}, {0x33, {0x90, 0x00, 0x00}},
+	{0x33, {0x8c, 0x27, 0x39}}, {0x33, {0x90, 0x21, 0x11}},
+	{0x33, {0x8c, 0x27, 0x3b}}, {0x33, {0x90, 0x00, 0x24}},
+	{0x33, {0x8c, 0x27, 0x3d}}, {0x33, {0x90, 0x01, 0x20}},
+	{0x33, {0x8c, 0x27, 0x41}}, {0x33, {0x90, 0x01, 0x69}},
+	{0x33, {0x8c, 0x27, 0x45}}, {0x33, {0x90, 0x04, 0xed}},
+	{0x33, {0x8c, 0x27, 0x47}}, {0x33, {0x90, 0x09, 0x4c}},
+	{0x33, {0x8c, 0x27, 0x51}}, {0x33, {0x90, 0x00, 0x00}},
+	{0x33, {0x8c, 0x27, 0x53}}, {0x33, {0x90, 0x03, 0x20}},
+	{0x33, {0x8c, 0x27, 0x55}}, {0x33, {0x90, 0x00, 0x00}},
+	{0x33, {0x8c, 0x27, 0x57}}, {0x33, {0x90, 0x02, 0x58}},
+	{0x33, {0x8c, 0x27, 0x5f}}, {0x33, {0x90, 0x00, 0x00}},
+	{0x33, {0x8c, 0x27, 0x61}}, {0x33, {0x90, 0x06, 0x40}},
+	{0x33, {0x8c, 0x27, 0x63}}, {0x33, {0x90, 0x00, 0x00}},
+	{0x33, {0x8c, 0x27, 0x65}}, {0x33, {0x90, 0x04, 0xb0}},
+	{0x33, {0x8c, 0x22, 0x2e}}, {0x33, {0x90, 0x00, 0xa1}},
+	{0x33, {0x8c, 0xa4, 0x08}}, {0x33, {0x90, 0x00, 0x1f}},
+	{0x33, {0x8c, 0xa4, 0x09}}, {0x33, {0x90, 0x00, 0x21}},
+	{0x33, {0x8c, 0xa4, 0x0a}}, {0x33, {0x90, 0x00, 0x25}},
+	{0x33, {0x8c, 0xa4, 0x0b}}, {0x33, {0x90, 0x00, 0x27}},
+	{0x33, {0x8c, 0x24, 0x11}}, {0x33, {0x90, 0x00, 0xa1}},
+	{0x33, {0x8c, 0x24, 0x13}}, {0x33, {0x90, 0x00, 0xc1}},
+	{0x33, {0x8c, 0x24, 0x15}}, {0x33, {0x90, 0x00, 0x6a}},
+	{0x33, {0x8c, 0x24, 0x17}}, {0x33, {0x90, 0x00, 0x80}},
+	{0x33, {0x8c, 0xa1, 0x03}}, {0x33, {0x90, 0x00, 0x05}},
+	{2, {0xff, 0xff, 0xff}},
+	{0x33, {0x8c, 0xa1, 0x03}}, {0x33, {0x90, 0x00, 0x06}},
+	{3, {0xff, 0xff, 0xff}},
 };
 
 static struct idxdata tbl_init_post_alt_low1[] = {
-	{0x33, "\x8c\x27\x15"}, {0x33, "\x90\x00\x25"}, {0x33, "\x8c\x22\x2e"},
-	{0x33, "\x90\x00\x81"}, {0x33, "\x8c\xa4\x08"}, {0x33, "\x90\x00\x17"},
-	{0x33, "\x8c\xa4\x09"}, {0x33, "\x90\x00\x1a"}, {0x33, "\x8c\xa4\x0a"},
-	{0x33, "\x90\x00\x1d"}, {0x33, "\x8c\xa4\x0b"}, {0x33, "\x90\x00\x20"},
-	{0x33, "\x8c\x24\x11"}, {0x33, "\x90\x00\x81"}, {0x33, "\x8c\x24\x13"},
-	{0x33, "\x90\x00\x9b"},
+	{0x33, {0x8c, 0x27, 0x15}}, {0x33, {0x90, 0x00, 0x25}},
+	{0x33, {0x8c, 0x22, 0x2e}}, {0x33, {0x90, 0x00, 0x81}},
+	{0x33, {0x8c, 0xa4, 0x08}}, {0x33, {0x90, 0x00, 0x17}},
+	{0x33, {0x8c, 0xa4, 0x09}}, {0x33, {0x90, 0x00, 0x1a}},
+	{0x33, {0x8c, 0xa4, 0x0a}}, {0x33, {0x90, 0x00, 0x1d}},
+	{0x33, {0x8c, 0xa4, 0x0b}}, {0x33, {0x90, 0x00, 0x20}},
+	{0x33, {0x8c, 0x24, 0x11}}, {0x33, {0x90, 0x00, 0x81}},
+	{0x33, {0x8c, 0x24, 0x13}}, {0x33, {0x90, 0x00, 0x9b}},
 };
 
 static struct idxdata tbl_init_post_alt_low2[] = {
-	{0x33, "\x8c\x27\x03"}, {0x33, "\x90\x03\x24"}, {0x33, "\x8c\x27\x05"},
-	{0x33, "\x90\x02\x58"}, {0x33, "\x8c\xa1\x03"}, {0x33, "\x90\x00\x05"},
-	{2, "\xff\xff\xff"},
-	{0x33, "\x8c\xa1\x03"}, {0x33, "\x90\x00\x06"},
-	{2, "\xff\xff\xff"},
+	{0x33, {0x8c, 0x27, 0x03}}, {0x33, {0x90, 0x03, 0x24}},
+	{0x33, {0x8c, 0x27, 0x05}}, {0x33, {0x90, 0x02, 0x58}},
+	{0x33, {0x8c, 0xa1, 0x03}}, {0x33, {0x90, 0x00, 0x05}},
+	{2, {0xff, 0xff, 0xff}},
+	{0x33, {0x8c, 0xa1, 0x03}}, {0x33, {0x90, 0x00, 0x06}},
+	{2, {0xff, 0xff, 0xff}},
 };
 
 static struct idxdata tbl_init_post_alt_low3[] = {
-	{0x34, "\x1e\x8f\x09"}, {0x34, "\x1c\x01\x28"}, {0x34, "\x1e\x8f\x09"},
-	{2, "\xff\xff\xff"},
-	{0x34, "\x1e\x8f\x09"}, {0x32, "\x14\x06\xe6"}, {0x33, "\x8c\xa1\x20"},
-	{0x33, "\x90\x00\x00"}, {0x33, "\x8c\xa1\x03"}, {0x33, "\x90\x00\x01"},
-	{0x33, "\x2e\x01\x00"}, {0x34, "\x04\x00\x2a"}, {0x33, "\x8c\xa7\x02"},
-	{0x33, "\x90\x00\x00"}, {0x33, "\x8c\x27\x95"}, {0x33, "\x90\x01\x00"},
-	{2, "\xff\xff\xff"},
-	{0x33, "\x8c\xa1\x20"}, {0x33, "\x90\x00\x72"}, {0x33, "\x8c\xa1\x03"},
-	{0x33, "\x90\x00\x02"}, {0x33, "\x8c\xa7\x02"}, {0x33, "\x90\x00\x01"},
-	{2, "\xff\xff\xff"},
-	{0x33, "\x8c\xa1\x20"}, {0x33, "\x90\x00\x00"}, {0x33, "\x8c\xa1\x03"},
-	{0x33, "\x90\x00\x01"}, {0x33, "\x8c\xa7\x02"}, {0x33, "\x90\x00\x00"},
-	{2, "\xff\xff\xff"},
-	{0x33, "\x8c\xa1\x03"}, {0x33, "\x90\x00\x05"},
-	{2, "\xff\xff\xff"},
-	{0x33, "\x8c\xa1\x03"}, {0x33, "\x90\x00\x06"},
-	{2, "\xff\xff\xff"},
-	{0x33, "\x8c\xa1\x03"}, {0x33, "\x90\x00\x05"},
-	{2, "\xff\xff\xff"},
-	{0x33, "\x8c\xa1\x03"}, {0x33, "\x90\x00\x06"},
+	{0x34, {0x1e, 0x8f, 0x09}}, {0x34, {0x1c, 0x01, 0x28}},
+	{0x34, {0x1e, 0x8f, 0x09}},
+	{2, {0xff, 0xff, 0xff}},
+	{0x34, {0x1e, 0x8f, 0x09}}, {0x32, {0x14, 0x06, 0xe6}},
+	{0x33, {0x8c, 0xa1, 0x20}}, {0x33, {0x90, 0x00, 0x00}},
+	{0x33, {0x8c, 0xa1, 0x03}}, {0x33, {0x90, 0x00, 0x01}},
+	{0x33, {0x2e, 0x01, 0x00}}, {0x34, {0x04, 0x00, 0x2a}},
+	{0x33, {0x8c, 0xa7, 0x02}}, {0x33, {0x90, 0x00, 0x00}},
+	{0x33, {0x8c, 0x27, 0x95}}, {0x33, {0x90, 0x01, 0x00}},
+	{2, {0xff, 0xff, 0xff}},
+	{0x33, {0x8c, 0xa1, 0x20}}, {0x33, {0x90, 0x00, 0x72}},
+	{0x33, {0x8c, 0xa1, 0x03}}, {0x33, {0x90, 0x00, 0x02}},
+	{0x33, {0x8c, 0xa7, 0x02}}, {0x33, {0x90, 0x00, 0x01}},
+	{2, {0xff, 0xff, 0xff}},
+	{0x33, {0x8c, 0xa1, 0x20}}, {0x33, {0x90, 0x00, 0x00}},
+	{0x33, {0x8c, 0xa1, 0x03}}, {0x33, {0x90, 0x00, 0x01}},
+	{0x33, {0x8c, 0xa7, 0x02}}, {0x33, {0x90, 0x00, 0x00}},
+	{2, {0xff, 0xff, 0xff}},
+	{0x33, {0x8c, 0xa1, 0x03}}, {0x33, {0x90, 0x00, 0x05}},
+	{2, {0xff, 0xff, 0xff}},
+	{0x33, {0x8c, 0xa1, 0x03}}, {0x33, {0x90, 0x00, 0x06}},
+	{2, {0xff, 0xff, 0xff}},
+	{0x33, {0x8c, 0xa1, 0x03}}, {0x33, {0x90, 0x00, 0x05}},
+	{2, {0xff, 0xff, 0xff}},
+	{0x33, {0x8c, 0xa1, 0x03}}, {0x33, {0x90, 0x00, 0x06}},
 };
 
 static struct idxdata tbl_init_post_alt_big[] = {
-	{0x33, "\x8c\xa1\x03"}, {0x33, "\x90\x00\x05"},
-	{2, "\xff\xff\xff"},
-	{0x33, "\x8c\xa1\x03"}, {0x33, "\x90\x00\x06"},
-	{2, "\xff\xff\xff"},
-	{0x34, "\x1e\x8f\x09"}, {0x34, "\x1c\x01\x28"}, {0x34, "\x1e\x8f\x09"},
-	{2, "\xff\xff\xff"},
-	{0x34, "\x1e\x8f\x09"}, {0x32, "\x14\x06\xe6"}, {0x33, "\x8c\xa1\x03"},
-	{0x33, "\x90\x00\x05"},
-	{2, "\xff\xff\xff"},
-	{0x33, "\x8c\xa1\x03"}, {0x33, "\x90\x00\x06"},
-	{2, "\xff\xff\xff"},
-	{0x33, "\x8c\xa1\x03"}, {0x33, "\x90\x00\x05"},
-	{2, "\xff\xff\xff"},
-	{0x33, "\x8c\xa1\x03"}, {0x33, "\x90\x00\x06"}, {0x33, "\x8c\xa1\x20"},
-	{0x33, "\x90\x00\x72"}, {0x33, "\x8c\xa1\x30"}, {0x33, "\x90\x00\x03"},
-	{0x33, "\x8c\xa1\x31"}, {0x33, "\x90\x00\x02"}, {0x33, "\x8c\xa1\x32"},
-	{0x33, "\x90\x00\x03"}, {0x33, "\x8c\xa1\x34"}, {0x33, "\x90\x00\x03"},
-	{0x33, "\x8c\xa1\x03"}, {0x33, "\x90\x00\x02"}, {0x33, "\x2e\x01\x00"},
-	{0x34, "\x04\x00\x2a"}, {0x33, "\x8c\xa7\x02"}, {0x33, "\x90\x00\x01"},
-	{0x33, "\x8c\x27\x97"}, {0x33, "\x90\x01\x00"},
-	{51, "\xff\xff\xff"},
-	{0x33, "\x8c\xa1\x20"}, {0x33, "\x90\x00\x00"}, {0x33, "\x8c\xa1\x03"},
-	{0x33, "\x90\x00\x01"}, {0x33, "\x8c\xa7\x02"}, {0x33, "\x90\x00\x00"},
-	{51, "\xff\xff\xff"},
-	{0x33, "\x8c\xa1\x20"}, {0x33, "\x90\x00\x72"}, {0x33, "\x8c\xa1\x03"},
-	{0x33, "\x90\x00\x02"}, {0x33, "\x8c\xa7\x02"}, {0x33, "\x90\x00\x01"},
-	{51, "\xff\xff\xff"},
+	{0x33, {0x8c, 0xa1, 0x03}}, {0x33, {0x90, 0x00, 0x05}},
+	{2, {0xff, 0xff, 0xff}},
+	{0x33, {0x8c, 0xa1, 0x03}}, {0x33, {0x90, 0x00, 0x06}},
+	{2, {0xff, 0xff, 0xff}},
+	{0x34, {0x1e, 0x8f, 0x09}}, {0x34, {0x1c, 0x01, 0x28}},
+	{0x34, {0x1e, 0x8f, 0x09}},
+	{2, {0xff, 0xff, 0xff}},
+	{0x34, {0x1e, 0x8f, 0x09}}, {0x32, {0x14, 0x06, 0xe6}},
+	{0x33, {0x8c, 0xa1, 0x03}},
+	{0x33, {0x90, 0x00, 0x05}},
+	{2, {0xff, 0xff, 0xff}},
+	{0x33, {0x8c, 0xa1, 0x03}}, {0x33, {0x90, 0x00, 0x06}},
+	{2, {0xff, 0xff, 0xff}},
+	{0x33, {0x8c, 0xa1, 0x03}}, {0x33, {0x90, 0x00, 0x05}},
+	{2, {0xff, 0xff, 0xff}},
+	{0x33, {0x8c, 0xa1, 0x03}}, {0x33, {0x90, 0x00, 0x06}},
+	{0x33, {0x8c, 0xa1, 0x20}}, {0x33, {0x90, 0x00, 0x72}},
+	{0x33, {0x8c, 0xa1, 0x30}}, {0x33, {0x90, 0x00, 0x03}},
+	{0x33, {0x8c, 0xa1, 0x31}}, {0x33, {0x90, 0x00, 0x02}},
+	{0x33, {0x8c, 0xa1, 0x32}}, {0x33, {0x90, 0x00, 0x03}},
+	{0x33, {0x8c, 0xa1, 0x34}}, {0x33, {0x90, 0x00, 0x03}},
+	{0x33, {0x8c, 0xa1, 0x03}}, {0x33, {0x90, 0x00, 0x02}},
+	{0x33, {0x2e, 0x01, 0x00}}, {0x34, {0x04, 0x00, 0x2a}},
+	{0x33, {0x8c, 0xa7, 0x02}}, {0x33, {0x90, 0x00, 0x01}},
+	{0x33, {0x8c, 0x27, 0x97}}, {0x33, {0x90, 0x01, 0x00}},
+	{51, {0xff, 0xff, 0xff}},
+	{0x33, {0x8c, 0xa1, 0x20}}, {0x33, {0x90, 0x00, 0x00}},
+	{0x33, {0x8c, 0xa1, 0x03}}, {0x33, {0x90, 0x00, 0x01}},
+	{0x33, {0x8c, 0xa7, 0x02}}, {0x33, {0x90, 0x00, 0x00}},
+	{51, {0xff, 0xff, 0xff}},
+	{0x33, {0x8c, 0xa1, 0x20}}, {0x33, {0x90, 0x00, 0x72}},
+	{0x33, {0x8c, 0xa1, 0x03}}, {0x33, {0x90, 0x00, 0x02}},
+	{0x33, {0x8c, 0xa7, 0x02}}, {0x33, {0x90, 0x00, 0x01}},
+	{51, {0xff, 0xff, 0xff}},
 };
 
 static struct idxdata tbl_init_post_alt_3B[] = {
-	{0x32, "\x10\x01\xf8"}, {0x34, "\xce\x01\xa8"}, {0x34, "\xd0\x66\x33"},
-	{0x34, "\xd2\x31\x9a"}, {0x34, "\xd4\x94\x63"}, {0x34, "\xd6\x4b\x25"},
-	{0x34, "\xd8\x26\x70"}, {0x34, "\xda\x72\x4c"}, {0x34, "\xdc\xff\x04"},
-	{0x34, "\xde\x01\x5b"}, {0x34, "\xe6\x01\x13"}, {0x34, "\xee\x0b\xf0"},
-	{0x34, "\xf6\x0b\xa4"}, {0x35, "\x00\xf6\xe7"}, {0x35, "\x08\x0d\xfd"},
-	{0x35, "\x10\x25\x63"}, {0x35, "\x18\x35\x6c"}, {0x35, "\x20\x42\x7e"},
-	{0x35, "\x28\x19\x44"}, {0x35, "\x30\x39\xd4"}, {0x35, "\x38\xf5\xa8"},
-	{0x35, "\x4c\x07\x90"}, {0x35, "\x44\x07\xb8"}, {0x35, "\x5c\x06\x88"},
-	{0x35, "\x54\x07\xff"}, {0x34, "\xe0\x01\x52"}, {0x34, "\xe8\x00\xcc"},
-	{0x34, "\xf0\x0d\x83"}, {0x34, "\xf8\x0c\xb3"}, {0x35, "\x02\xfe\xba"},
-	{0x35, "\x0a\x04\xe0"}, {0x35, "\x12\x1c\x63"}, {0x35, "\x1a\x2b\x5a"},
-	{0x35, "\x22\x32\x5e"}, {0x35, "\x2a\x0d\x28"}, {0x35, "\x32\x2c\x02"},
-	{0x35, "\x3a\xf4\xfa"}, {0x35, "\x4e\x07\xef"}, {0x35, "\x46\x07\x88"},
-	{0x35, "\x5e\x07\xc1"}, {0x35, "\x56\x04\x64"}, {0x34, "\xe4\x01\x15"},
-	{0x34, "\xec\x00\x82"}, {0x34, "\xf4\x0c\xce"}, {0x34, "\xfc\x0c\xba"},
-	{0x35, "\x06\x1f\x02"}, {0x35, "\x0e\x02\xe3"}, {0x35, "\x16\x1a\x50"},
-	{0x35, "\x1e\x24\x39"}, {0x35, "\x26\x23\x4c"}, {0x35, "\x2e\xf9\x1b"},
-	{0x35, "\x36\x23\x19"}, {0x35, "\x3e\x12\x08"}, {0x35, "\x52\x07\x22"},
-	{0x35, "\x4a\x03\xd3"}, {0x35, "\x62\x06\x54"}, {0x35, "\x5a\x04\x5d"},
-	{0x34, "\xe2\x01\x04"}, {0x34, "\xea\x00\xa0"}, {0x34, "\xf2\x0c\xbc"},
-	{0x34, "\xfa\x0c\x5b"}, {0x35, "\x04\x17\xf2"}, {0x35, "\x0c\x02\x08"},
-	{0x35, "\x14\x28\x43"}, {0x35, "\x1c\x28\x62"}, {0x35, "\x24\x2b\x60"},
-	{0x35, "\x2c\x07\x33"}, {0x35, "\x34\x1f\xb0"}, {0x35, "\x3c\xed\xcd"},
-	{0x35, "\x50\x00\x06"}, {0x35, "\x48\x07\xff"}, {0x35, "\x60\x05\x89"},
-	{0x35, "\x58\x07\xff"}, {0x35, "\x40\x00\xa0"}, {0x35, "\x42\x00\x00"},
-	{0x32, "\x10\x01\xfc"}, {0x33, "\x8c\xa1\x18"}, {0x33, "\x90\x00\x3c"},
+	{0x32, {0x10, 0x01, 0xf8}}, {0x34, {0xce, 0x01, 0xa8}},
+	{0x34, {0xd0, 0x66, 0x33}}, {0x34, {0xd2, 0x31, 0x9a}},
+	{0x34, {0xd4, 0x94, 0x63}}, {0x34, {0xd6, 0x4b, 0x25}},
+	{0x34, {0xd8, 0x26, 0x70}}, {0x34, {0xda, 0x72, 0x4c}},
+	{0x34, {0xdc, 0xff, 0x04}}, {0x34, {0xde, 0x01, 0x5b}},
+	{0x34, {0xe6, 0x01, 0x13}}, {0x34, {0xee, 0x0b, 0xf0}},
+	{0x34, {0xf6, 0x0b, 0xa4}}, {0x35, {0x00, 0xf6, 0xe7}},
+	{0x35, {0x08, 0x0d, 0xfd}}, {0x35, {0x10, 0x25, 0x63}},
+	{0x35, {0x18, 0x35, 0x6c}}, {0x35, {0x20, 0x42, 0x7e}},
+	{0x35, {0x28, 0x19, 0x44}}, {0x35, {0x30, 0x39, 0xd4}},
+	{0x35, {0x38, 0xf5, 0xa8}}, {0x35, {0x4c, 0x07, 0x90}},
+	{0x35, {0x44, 0x07, 0xb8}}, {0x35, {0x5c, 0x06, 0x88}},
+	{0x35, {0x54, 0x07, 0xff}}, {0x34, {0xe0, 0x01, 0x52}},
+	{0x34, {0xe8, 0x00, 0xcc}}, {0x34, {0xf0, 0x0d, 0x83}},
+	{0x34, {0xf8, 0x0c, 0xb3}}, {0x35, {0x02, 0xfe, 0xba}},
+	{0x35, {0x0a, 0x04, 0xe0}}, {0x35, {0x12, 0x1c, 0x63}},
+	{0x35, {0x1a, 0x2b, 0x5a}}, {0x35, {0x22, 0x32, 0x5e}},
+	{0x35, {0x2a, 0x0d, 0x28}}, {0x35, {0x32, 0x2c, 0x02}},
+	{0x35, {0x3a, 0xf4, 0xfa}}, {0x35, {0x4e, 0x07, 0xef}},
+	{0x35, {0x46, 0x07, 0x88}}, {0x35, {0x5e, 0x07, 0xc1}},
+	{0x35, {0x56, 0x04, 0x64}}, {0x34, {0xe4, 0x01, 0x15}},
+	{0x34, {0xec, 0x00, 0x82}}, {0x34, {0xf4, 0x0c, 0xce}},
+	{0x34, {0xfc, 0x0c, 0xba}}, {0x35, {0x06, 0x1f, 0x02}},
+	{0x35, {0x0e, 0x02, 0xe3}}, {0x35, {0x16, 0x1a, 0x50}},
+	{0x35, {0x1e, 0x24, 0x39}}, {0x35, {0x26, 0x23, 0x4c}},
+	{0x35, {0x2e, 0xf9, 0x1b}}, {0x35, {0x36, 0x23, 0x19}},
+	{0x35, {0x3e, 0x12, 0x08}}, {0x35, {0x52, 0x07, 0x22}},
+	{0x35, {0x4a, 0x03, 0xd3}}, {0x35, {0x62, 0x06, 0x54}},
+	{0x35, {0x5a, 0x04, 0x5d}}, {0x34, {0xe2, 0x01, 0x04}},
+	{0x34, {0xea, 0x00, 0xa0}}, {0x34, {0xf2, 0x0c, 0xbc}},
+	{0x34, {0xfa, 0x0c, 0x5b}}, {0x35, {0x04, 0x17, 0xf2}},
+	{0x35, {0x0c, 0x02, 0x08}}, {0x35, {0x14, 0x28, 0x43}},
+	{0x35, {0x1c, 0x28, 0x62}}, {0x35, {0x24, 0x2b, 0x60}},
+	{0x35, {0x2c, 0x07, 0x33}}, {0x35, {0x34, 0x1f, 0xb0}},
+	{0x35, {0x3c, 0xed, 0xcd}}, {0x35, {0x50, 0x00, 0x06}},
+	{0x35, {0x48, 0x07, 0xff}}, {0x35, {0x60, 0x05, 0x89}},
+	{0x35, {0x58, 0x07, 0xff}}, {0x35, {0x40, 0x00, 0xa0}},
+	{0x35, {0x42, 0x00, 0x00}}, {0x32, {0x10, 0x01, 0xfc}},
+	{0x33, {0x8c, 0xa1, 0x18}}, {0x33, {0x90, 0x00, 0x3c}},
 };
 
 static u8 *dat_640  = "\xd0\x02\xd1\x08\xd2\xe1\xd3\x02\xd4\x10\xd5\x81";
diff --git a/drivers/media/usb/gspca/sonixb.c b/drivers/media/usb/gspca/sonixb.c
index 7277dbd2afc..ecbcb39feb7 100644
--- a/drivers/media/usb/gspca/sonixb.c
+++ b/drivers/media/usb/gspca/sonixb.c
@@ -1430,10 +1430,8 @@ static const struct usb_device_id device_table[] = {
 	{USB_DEVICE(0x0c45, 0x600d), SB(PAS106, 101)},
 	{USB_DEVICE(0x0c45, 0x6011), SB(OV6650, 101)},
 	{USB_DEVICE(0x0c45, 0x6019), SB(OV7630, 101)},
-#if !IS_ENABLED(CONFIG_USB_SN9C102)
 	{USB_DEVICE(0x0c45, 0x6024), SB(TAS5130CXX, 102)},
 	{USB_DEVICE(0x0c45, 0x6025), SB(TAS5130CXX, 102)},
-#endif
 	{USB_DEVICE(0x0c45, 0x6027), SB(OV7630, 101)}, /* Genius Eye 310 */
 	{USB_DEVICE(0x0c45, 0x6028), SB(PAS202, 102)},
 	{USB_DEVICE(0x0c45, 0x6029), SB(PAS106, 102)},
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-hdw.c b/drivers/media/usb/pvrusb2/pvrusb2-hdw.c
index ea05f678b55..9623b621821 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-hdw.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-hdw.c
@@ -2910,7 +2910,7 @@ static void pvr2_subdev_update(struct pvr2_hdw *hdw)
 			v4l2_std_id vs;
 			vs = hdw->std_mask_cur;
 			v4l2_device_call_all(&hdw->v4l2_dev, 0,
-					     core, s_std, vs);
+					     video, s_std, vs);
 			pvr2_hdw_cx25840_vbi_hack(hdw);
 		}
 		hdw->tuner_signal_stale = !0;
diff --git a/drivers/media/usb/pwc/pwc-if.c b/drivers/media/usb/pwc/pwc-if.c
index 84a6720b1d0..a73b0bced96 100644
--- a/drivers/media/usb/pwc/pwc-if.c
+++ b/drivers/media/usb/pwc/pwc-if.c
@@ -681,12 +681,11 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 	return r;
 }
 
-static int stop_streaming(struct vb2_queue *vq)
+static void stop_streaming(struct vb2_queue *vq)
 {
 	struct pwc_device *pdev = vb2_get_drv_priv(vq);
 
-	if (mutex_lock_interruptible(&pdev->v4l2_lock))
-		return -ERESTARTSYS;
+	mutex_lock(&pdev->v4l2_lock);
 	if (pdev->udev) {
 		pwc_set_leds(pdev, 0, 0);
 		pwc_camera_power(pdev, 0);
@@ -695,8 +694,6 @@ static int stop_streaming(struct vb2_queue *vq)
 
 	pwc_cleanup_queued_bufs(pdev);
 	mutex_unlock(&pdev->v4l2_lock);
-
-	return 0;
 }
 
 static struct vb2_ops pwc_vb_queue_ops = {
diff --git a/drivers/media/usb/s2255/s2255drv.c b/drivers/media/usb/s2255/s2255drv.c
index 1d4ba2b8049..a44466bc7b8 100644
--- a/drivers/media/usb/s2255/s2255drv.c
+++ b/drivers/media/usb/s2255/s2255drv.c
@@ -714,7 +714,7 @@ static void buffer_queue(struct vb2_buffer *vb)
 }
 
 static int start_streaming(struct vb2_queue *vq, unsigned int count);
-static int stop_streaming(struct vb2_queue *vq);
+static void stop_streaming(struct vb2_queue *vq);
 
 static struct vb2_ops s2255_video_qops = {
 	.queue_setup = queue_setup,
@@ -1109,7 +1109,7 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 }
 
 /* abort streaming and wait for last buffer */
-static int stop_streaming(struct vb2_queue *vq)
+static void stop_streaming(struct vb2_queue *vq)
 {
 	struct s2255_vc *vc = vb2_get_drv_priv(vq);
 	struct s2255_buffer *buf, *node;
@@ -1123,7 +1123,6 @@ static int stop_streaming(struct vb2_queue *vq)
 			buf, buf->vb.v4l2_buf.index);
 	}
 	spin_unlock_irqrestore(&vc->qlock, flags);
-	return 0;
 }
 
 static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id i)
@@ -1522,7 +1521,7 @@ static void s2255_destroy(struct s2255_dev *dev)
 	/* board shutdown stops the read pipe if it is running */
 	s2255_board_shutdown(dev);
 	/* make sure firmware still not trying to load */
-	del_timer(&dev->timer);  /* only started in .probe and .open */
+	del_timer_sync(&dev->timer);  /* only started in .probe and .open */
 	if (dev->fw_data->fw_urb) {
 		usb_kill_urb(dev->fw_data->fw_urb);
 		usb_free_urb(dev->fw_data->fw_urb);
@@ -2243,7 +2242,7 @@ static int s2255_probe(struct usb_interface *interface,
 	dev->cmdbuf = kzalloc(S2255_CMDBUF_SIZE, GFP_KERNEL);
 	if (dev->cmdbuf == NULL) {
 		s2255_dev_err(&interface->dev, "out of memory\n");
-		return -ENOMEM;
+		goto errorFWDATA1;
 	}
 
 	atomic_set(&dev->num_channels, 0);
@@ -2352,7 +2351,7 @@ errorREQFW:
 errorFWDATA2:
 	usb_free_urb(dev->fw_data->fw_urb);
 errorFWURB:
-	del_timer(&dev->timer);
+	del_timer_sync(&dev->timer);
 errorEP:
 	usb_put_dev(dev->udev);
 errorUDEV:
diff --git a/drivers/media/usb/stk1160/stk1160-core.c b/drivers/media/usb/stk1160/stk1160-core.c
index 34a26e0cfe7..03504dcf3c5 100644
--- a/drivers/media/usb/stk1160/stk1160-core.c
+++ b/drivers/media/usb/stk1160/stk1160-core.c
@@ -67,17 +67,25 @@ int stk1160_read_reg(struct stk1160 *dev, u16 reg, u8 *value)
 {
 	int ret;
 	int pipe = usb_rcvctrlpipe(dev->udev, 0);
+	u8 *buf;
 
 	*value = 0;
+
+	buf = kmalloc(sizeof(u8), GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
 	ret = usb_control_msg(dev->udev, pipe, 0x00,
 			USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
-			0x00, reg, value, sizeof(u8), HZ);
+			0x00, reg, buf, sizeof(u8), HZ);
 	if (ret < 0) {
 		stk1160_err("read failed on reg 0x%x (%d)\n",
 			reg, ret);
+		kfree(buf);
 		return ret;
 	}
 
+	*value = *buf;
+	kfree(buf);
 	return 0;
 }
 
diff --git a/drivers/media/usb/stk1160/stk1160-v4l.c b/drivers/media/usb/stk1160/stk1160-v4l.c
index 37bc00f418f..5461341a31c 100644
--- a/drivers/media/usb/stk1160/stk1160-v4l.c
+++ b/drivers/media/usb/stk1160/stk1160-v4l.c
@@ -406,7 +406,7 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id norm)
 
 	stk1160_set_std(dev);
 
-	v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_std,
+	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_std,
 			dev->norm);
 
 	return 0;
@@ -583,10 +583,10 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 }
 
 /* abort streaming and wait for last buffer */
-static int stop_streaming(struct vb2_queue *vq)
+static void stop_streaming(struct vb2_queue *vq)
 {
 	struct stk1160 *dev = vb2_get_drv_priv(vq);
-	return stk1160_stop_streaming(dev);
+	stk1160_stop_streaming(dev);
 }
 
 static struct vb2_ops stk1160_video_qops = {
@@ -682,7 +682,7 @@ int stk1160_video_register(struct stk1160 *dev)
 	dev->fmt = &format[0];
 	stk1160_set_std(dev);
 
-	v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_std,
+	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_std,
 			dev->norm);
 
 	video_set_drvdata(&dev->vdev, dev);
diff --git a/drivers/media/usb/stk1160/stk1160.h b/drivers/media/usb/stk1160/stk1160.h
index 05b05b160e1..abdea484c99 100644
--- a/drivers/media/usb/stk1160/stk1160.h
+++ b/drivers/media/usb/stk1160/stk1160.h
@@ -143,7 +143,6 @@ struct stk1160 {
 	int num_alt;
 
 	struct stk1160_isoc_ctl isoc_ctl;
-	char urb_buf[255];	 /* urb control msg buffer */
 
 	/* frame properties */
 	int width;		  /* current frame width */
diff --git a/drivers/media/usb/tm6000/tm6000-cards.c b/drivers/media/usb/tm6000/tm6000-cards.c
index 1ccaaddaa30..2e8c3afe4ec 100644
--- a/drivers/media/usb/tm6000/tm6000-cards.c
+++ b/drivers/media/usb/tm6000/tm6000-cards.c
@@ -1120,7 +1120,7 @@ static int tm6000_init_dev(struct tm6000_core *dev)
 	tm6000_config_tuner(dev);
 
 	/* Set video standard */
-	v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_std, dev->norm);
+	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_std, dev->norm);
 
 	/* Set tuner frequency - also loads firmware on xc2028/xc3028 */
 	f.tuner = 0;
diff --git a/drivers/media/usb/tm6000/tm6000-video.c b/drivers/media/usb/tm6000/tm6000-video.c
index cc1aa14996f..e6b3d5d83d4 100644
--- a/drivers/media/usb/tm6000/tm6000-video.c
+++ b/drivers/media/usb/tm6000/tm6000-video.c
@@ -1071,7 +1071,7 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id norm)
 	if (rc < 0)
 		return rc;
 
-	v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_std, dev->norm);
+	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_std, dev->norm);
 
 	return 0;
 }
diff --git a/drivers/media/usb/usbtv/usbtv-video.c b/drivers/media/usb/usbtv/usbtv-video.c
index 20365bd69d0..2967e808408 100644
--- a/drivers/media/usb/usbtv/usbtv-video.c
+++ b/drivers/media/usb/usbtv/usbtv-video.c
@@ -634,15 +634,12 @@ static int usbtv_start_streaming(struct vb2_queue *vq, unsigned int count)
 	return usbtv_start(usbtv);
 }
 
-static int usbtv_stop_streaming(struct vb2_queue *vq)
+static void usbtv_stop_streaming(struct vb2_queue *vq)
 {
 	struct usbtv *usbtv = vb2_get_drv_priv(vq);
 
-	if (usbtv->udev == NULL)
-		return -ENODEV;
-
-	usbtv_stop(usbtv);
-	return 0;
+	if (usbtv->udev)
+		usbtv_stop(usbtv);
 }
 
 static struct vb2_ops usbtv_vb2_ops = {
diff --git a/drivers/media/usb/usbvision/usbvision-video.c b/drivers/media/usb/usbvision/usbvision-video.c
index 5c9e3123ad2..68bc9615660 100644
--- a/drivers/media/usb/usbvision/usbvision-video.c
+++ b/drivers/media/usb/usbvision/usbvision-video.c
@@ -597,7 +597,7 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id id)
 
 	usbvision->tvnorm_id = id;
 
-	call_all(usbvision, core, s_std, usbvision->tvnorm_id);
+	call_all(usbvision, video, s_std, usbvision->tvnorm_id);
 	/* propagate the change to the decoder */
 	usbvision_muxsel(usbvision, usbvision->ctl_input);
 
diff --git a/drivers/media/usb/uvc/uvc_video.c b/drivers/media/usb/uvc/uvc_video.c
index 8d52baf5952..9144a2f3ed8 100644
--- a/drivers/media/usb/uvc/uvc_video.c
+++ b/drivers/media/usb/uvc/uvc_video.c
@@ -361,6 +361,14 @@ static int uvc_commit_video(struct uvc_streaming *stream,
  * Clocks and timestamps
  */
 
+static inline void uvc_video_get_ts(struct timespec *ts)
+{
+	if (uvc_clock_param == CLOCK_MONOTONIC)
+		ktime_get_ts(ts);
+	else
+		ktime_get_real_ts(ts);
+}
+
 static void
 uvc_video_clock_decode(struct uvc_streaming *stream, struct uvc_buffer *buf,
 		       const __u8 *data, int len)
@@ -420,7 +428,7 @@ uvc_video_clock_decode(struct uvc_streaming *stream, struct uvc_buffer *buf,
 	stream->clock.last_sof = dev_sof;
 
 	host_sof = usb_get_current_frame_number(stream->dev->udev);
-	ktime_get_ts(&ts);
+	uvc_video_get_ts(&ts);
 
 	/* The UVC specification allows device implementations that can't obtain
 	 * the USB frame number to keep their own frame counters as long as they
@@ -1011,10 +1019,7 @@ static int uvc_video_decode_start(struct uvc_streaming *stream,
 			return -ENODATA;
 		}
 
-		if (uvc_clock_param == CLOCK_MONOTONIC)
-			ktime_get_ts(&ts);
-		else
-			ktime_get_real_ts(&ts);
+		uvc_video_get_ts(&ts);
 
 		buf->buf.v4l2_buf.sequence = stream->sequence;
 		buf->buf.v4l2_buf.timestamp.tv_sec = ts.tv_sec;
@@ -1133,6 +1138,17 @@ static int uvc_video_encode_data(struct uvc_streaming *stream,
  */
 
 /*
+ * Set error flag for incomplete buffer.
+ */
+static void uvc_video_validate_buffer(const struct uvc_streaming *stream,
+				      struct uvc_buffer *buf)
+{
+	if (buf->length != buf->bytesused &&
+	    !(stream->cur_format->flags & UVC_FMT_FLAG_COMPRESSED))
+		buf->error = 1;
+}
+
+/*
  * Completion handler for video URBs.
  */
 static void uvc_video_decode_isoc(struct urb *urb, struct uvc_streaming *stream,
@@ -1156,9 +1172,11 @@ static void uvc_video_decode_isoc(struct urb *urb, struct uvc_streaming *stream,
 		do {
 			ret = uvc_video_decode_start(stream, buf, mem,
 				urb->iso_frame_desc[i].actual_length);
-			if (ret == -EAGAIN)
+			if (ret == -EAGAIN) {
+				uvc_video_validate_buffer(stream, buf);
 				buf = uvc_queue_next_buffer(&stream->queue,
 							    buf);
+			}
 		} while (ret == -EAGAIN);
 
 		if (ret < 0)
@@ -1173,11 +1191,7 @@ static void uvc_video_decode_isoc(struct urb *urb, struct uvc_streaming *stream,
 			urb->iso_frame_desc[i].actual_length);
 
 		if (buf->state == UVC_BUF_STATE_READY) {
-			if (buf->length != buf->bytesused &&
-			    !(stream->cur_format->flags &
-			      UVC_FMT_FLAG_COMPRESSED))
-				buf->error = 1;
-
+			uvc_video_validate_buffer(stream, buf);
 			buf = uvc_queue_next_buffer(&stream->queue, buf);
 		}
 	}
diff --git a/drivers/media/v4l2-core/Kconfig b/drivers/media/v4l2-core/Kconfig
index 2189bfb2e82..9ca0f8d59a1 100644
--- a/drivers/media/v4l2-core/Kconfig
+++ b/drivers/media/v4l2-core/Kconfig
@@ -83,3 +83,7 @@ config VIDEOBUF2_DMA_SG
 	#depends on HAS_DMA
 	select VIDEOBUF2_CORE
 	select VIDEOBUF2_MEMOPS
+
+config VIDEOBUF2_DVB
+	tristate
+	select VIDEOBUF2_CORE
diff --git a/drivers/media/v4l2-core/Makefile b/drivers/media/v4l2-core/Makefile
index c6ae7bad951..63d29f27538 100644
--- a/drivers/media/v4l2-core/Makefile
+++ b/drivers/media/v4l2-core/Makefile
@@ -33,6 +33,7 @@ obj-$(CONFIG_VIDEOBUF2_MEMOPS) += videobuf2-memops.o
 obj-$(CONFIG_VIDEOBUF2_VMALLOC) += videobuf2-vmalloc.o
 obj-$(CONFIG_VIDEOBUF2_DMA_CONTIG) += videobuf2-dma-contig.o
 obj-$(CONFIG_VIDEOBUF2_DMA_SG) += videobuf2-dma-sg.o
+obj-$(CONFIG_VIDEOBUF2_DVB) += videobuf2-dvb.o
 
 ccflags-y += -I$(srctree)/drivers/media/dvb-core
 ccflags-y += -I$(srctree)/drivers/media/dvb-frontends
diff --git a/drivers/media/v4l2-core/tuner-core.c b/drivers/media/v4l2-core/tuner-core.c
index 20c09229a08..06c18ba16fa 100644
--- a/drivers/media/v4l2-core/tuner-core.c
+++ b/drivers/media/v4l2-core/tuner-core.c
@@ -1301,7 +1301,6 @@ static int tuner_command(struct i2c_client *client, unsigned cmd, void *arg)
 
 static const struct v4l2_subdev_core_ops tuner_core_ops = {
 	.log_status = tuner_log_status,
-	.s_std = tuner_s_std,
 	.s_power = tuner_s_power,
 };
 
@@ -1315,9 +1314,14 @@ static const struct v4l2_subdev_tuner_ops tuner_tuner_ops = {
 	.s_config = tuner_s_config,
 };
 
+static const struct v4l2_subdev_video_ops tuner_video_ops = {
+	.s_std = tuner_s_std,
+};
+
 static const struct v4l2_subdev_ops tuner_ops = {
 	.core = &tuner_core_ops,
 	.tuner = &tuner_tuner_ops,
+	.video = &tuner_video_ops,
 };
 
 /*
diff --git a/drivers/media/v4l2-core/v4l2-compat-ioctl32.c b/drivers/media/v4l2-core/v4l2-compat-ioctl32.c
index 04b2daf567b..7e2411c3641 100644
--- a/drivers/media/v4l2-core/v4l2-compat-ioctl32.c
+++ b/drivers/media/v4l2-core/v4l2-compat-ioctl32.c
@@ -178,6 +178,9 @@ struct v4l2_create_buffers32 {
 
 static int __get_v4l2_format32(struct v4l2_format *kp, struct v4l2_format32 __user *up)
 {
+	if (get_user(kp->type, &up->type))
+		return -EFAULT;
+
 	switch (kp->type) {
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
@@ -204,17 +207,16 @@ static int __get_v4l2_format32(struct v4l2_format *kp, struct v4l2_format32 __us
 
 static int get_v4l2_format32(struct v4l2_format *kp, struct v4l2_format32 __user *up)
 {
-	if (!access_ok(VERIFY_READ, up, sizeof(struct v4l2_format32)) ||
-			get_user(kp->type, &up->type))
-			return -EFAULT;
+	if (!access_ok(VERIFY_READ, up, sizeof(struct v4l2_format32)))
+		return -EFAULT;
 	return __get_v4l2_format32(kp, up);
 }
 
 static int get_v4l2_create32(struct v4l2_create_buffers *kp, struct v4l2_create_buffers32 __user *up)
 {
 	if (!access_ok(VERIFY_READ, up, sizeof(struct v4l2_create_buffers32)) ||
-	    copy_from_user(kp, up, offsetof(struct v4l2_create_buffers32, format.fmt)))
-			return -EFAULT;
+	    copy_from_user(kp, up, offsetof(struct v4l2_create_buffers32, format)))
+		return -EFAULT;
 	return __get_v4l2_format32(&kp->format, &up->format);
 }
 
diff --git a/drivers/media/v4l2-core/v4l2-device.c b/drivers/media/v4l2-core/v4l2-device.c
index 02d1b632711..015f92aab44 100644
--- a/drivers/media/v4l2-core/v4l2-device.c
+++ b/drivers/media/v4l2-core/v4l2-device.c
@@ -158,7 +158,17 @@ int v4l2_device_register_subdev(struct v4l2_device *v4l2_dev,
 	/* Warn if we apparently re-register a subdev */
 	WARN_ON(sd->v4l2_dev != NULL);
 
-	if (!try_module_get(sd->owner))
+	/*
+	 * The reason to acquire the module here is to avoid unloading
+	 * a module of sub-device which is registered to a media
+	 * device. To make it possible to unload modules for media
+	 * devices that also register sub-devices, do not
+	 * try_module_get() such sub-device owners.
+	 */
+	sd->owner_v4l2_dev = v4l2_dev->dev && v4l2_dev->dev->driver &&
+		sd->owner == v4l2_dev->dev->driver->owner;
+
+	if (!sd->owner_v4l2_dev && !try_module_get(sd->owner))
 		return -ENODEV;
 
 	sd->v4l2_dev = v4l2_dev;
@@ -192,7 +202,8 @@ error_unregister:
 	if (sd->internal_ops && sd->internal_ops->unregistered)
 		sd->internal_ops->unregistered(sd);
 error_module:
-	module_put(sd->owner);
+	if (!sd->owner_v4l2_dev)
+		module_put(sd->owner);
 	sd->v4l2_dev = NULL;
 	return err;
 }
@@ -280,6 +291,7 @@ void v4l2_device_unregister_subdev(struct v4l2_subdev *sd)
 	}
 #endif
 	video_unregister_device(sd->devnode);
-	module_put(sd->owner);
+	if (!sd->owner_v4l2_dev)
+		module_put(sd->owner);
 }
 EXPORT_SYMBOL_GPL(v4l2_device_unregister_subdev);
diff --git a/drivers/media/v4l2-core/v4l2-dv-timings.c b/drivers/media/v4l2-core/v4l2-dv-timings.c
index 48b20dfcc4d..4ae54caadd0 100644
--- a/drivers/media/v4l2-core/v4l2-dv-timings.c
+++ b/drivers/media/v4l2-core/v4l2-dv-timings.c
@@ -131,6 +131,17 @@ const struct v4l2_dv_timings v4l2_dv_timings_presets[] = {
 	V4L2_DV_BT_DMT_2560X1600P75,
 	V4L2_DV_BT_DMT_2560X1600P85,
 	V4L2_DV_BT_DMT_2560X1600P120_RB,
+	V4L2_DV_BT_CEA_3840X2160P24,
+	V4L2_DV_BT_CEA_3840X2160P25,
+	V4L2_DV_BT_CEA_3840X2160P30,
+	V4L2_DV_BT_CEA_3840X2160P50,
+	V4L2_DV_BT_CEA_3840X2160P60,
+	V4L2_DV_BT_CEA_4096X2160P24,
+	V4L2_DV_BT_CEA_4096X2160P25,
+	V4L2_DV_BT_CEA_4096X2160P30,
+	V4L2_DV_BT_CEA_4096X2160P50,
+	V4L2_DV_BT_DMT_4096X2160P59_94_RB,
+	V4L2_DV_BT_CEA_4096X2160P60,
 	{ }
 };
 EXPORT_SYMBOL_GPL(v4l2_dv_timings_presets);
diff --git a/drivers/media/v4l2-core/v4l2-event.c b/drivers/media/v4l2-core/v4l2-event.c
index 86dcb5483c4..8761aab99de 100644
--- a/drivers/media/v4l2-core/v4l2-event.c
+++ b/drivers/media/v4l2-core/v4l2-event.c
@@ -318,3 +318,39 @@ int v4l2_event_subdev_unsubscribe(struct v4l2_subdev *sd, struct v4l2_fh *fh,
 	return v4l2_event_unsubscribe(fh, sub);
 }
 EXPORT_SYMBOL_GPL(v4l2_event_subdev_unsubscribe);
+
+static void v4l2_event_src_replace(struct v4l2_event *old,
+				const struct v4l2_event *new)
+{
+	u32 old_changes = old->u.src_change.changes;
+
+	old->u.src_change = new->u.src_change;
+	old->u.src_change.changes |= old_changes;
+}
+
+static void v4l2_event_src_merge(const struct v4l2_event *old,
+				struct v4l2_event *new)
+{
+	new->u.src_change.changes |= old->u.src_change.changes;
+}
+
+static const struct v4l2_subscribed_event_ops v4l2_event_src_ch_ops = {
+	.replace = v4l2_event_src_replace,
+	.merge = v4l2_event_src_merge,
+};
+
+int v4l2_src_change_event_subscribe(struct v4l2_fh *fh,
+				const struct v4l2_event_subscription *sub)
+{
+	if (sub->type == V4L2_EVENT_SOURCE_CHANGE)
+		return v4l2_event_subscribe(fh, sub, 0, &v4l2_event_src_ch_ops);
+	return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(v4l2_src_change_event_subscribe);
+
+int v4l2_src_change_event_subdev_subscribe(struct v4l2_subdev *sd,
+		struct v4l2_fh *fh, struct v4l2_event_subscription *sub)
+{
+	return v4l2_src_change_event_subscribe(fh, sub);
+}
+EXPORT_SYMBOL_GPL(v4l2_src_change_event_subdev_subscribe);
diff --git a/drivers/media/v4l2-core/v4l2-ioctl.c b/drivers/media/v4l2-core/v4l2-ioctl.c
index d9113cc71c7..16bffd851bf 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_cropcap(const void *arg, bool write_only)
 	const struct v4l2_cropcap *p = arg;
 
 	pr_cont("type=%s, bounds wxh=%dx%d, x,y=%d,%d, "
-		"defrect wxh=%dx%d, x,y=%d,%d\n, "
+		"defrect wxh=%dx%d, x,y=%d,%d, "
 		"pixelaspect %d/%d\n",
 		prt_names(p->type, v4l2_type_names),
 		p->bounds.width, p->bounds.height,
@@ -2260,7 +2260,7 @@ done:
 }
 
 static int check_array_args(unsigned int cmd, void *parg, size_t *array_size,
-			    void * __user *user_ptr, void ***kernel_ptr)
+			    void __user **user_ptr, void ***kernel_ptr)
 {
 	int ret = 0;
 
@@ -2277,7 +2277,7 @@ static int check_array_args(unsigned int cmd, void *parg, size_t *array_size,
 				break;
 			}
 			*user_ptr = (void __user *)buf->m.planes;
-			*kernel_ptr = (void *)&buf->m.planes;
+			*kernel_ptr = (void **)&buf->m.planes;
 			*array_size = sizeof(struct v4l2_plane) * buf->length;
 			ret = 1;
 		}
@@ -2294,7 +2294,7 @@ static int check_array_args(unsigned int cmd, void *parg, size_t *array_size,
 				break;
 			}
 			*user_ptr = (void __user *)edid->edid;
-			*kernel_ptr = (void *)&edid->edid;
+			*kernel_ptr = (void **)&edid->edid;
 			*array_size = edid->blocks * 128;
 			ret = 1;
 		}
@@ -2312,7 +2312,7 @@ static int check_array_args(unsigned int cmd, void *parg, size_t *array_size,
 				break;
 			}
 			*user_ptr = (void __user *)ctrls->controls;
-			*kernel_ptr = (void *)&ctrls->controls;
+			*kernel_ptr = (void **)&ctrls->controls;
 			*array_size = sizeof(struct v4l2_ext_control)
 				    * ctrls->count;
 			ret = 1;
@@ -2412,7 +2412,7 @@ video_usercopy(struct file *file, unsigned int cmd, unsigned long arg,
 	}
 
 	if (has_array_args) {
-		*kernel_ptr = user_ptr;
+		*kernel_ptr = (void __force *)user_ptr;
 		if (copy_to_user(user_ptr, mbuf, array_size))
 			err = -EFAULT;
 		goto out_array_args;
diff --git a/drivers/media/v4l2-core/v4l2-subdev.c b/drivers/media/v4l2-core/v4l2-subdev.c
index aea84ac5688..058c1a6e839 100644
--- a/drivers/media/v4l2-core/v4l2-subdev.c
+++ b/drivers/media/v4l2-core/v4l2-subdev.c
@@ -305,11 +305,23 @@ static long subdev_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 					fse);
 	}
 
-	case VIDIOC_SUBDEV_G_FRAME_INTERVAL:
+	case VIDIOC_SUBDEV_G_FRAME_INTERVAL: {
+		struct v4l2_subdev_frame_interval *fi = arg;
+
+		if (fi->pad >= sd->entity.num_pads)
+			return -EINVAL;
+
 		return v4l2_subdev_call(sd, video, g_frame_interval, arg);
+	}
+
+	case VIDIOC_SUBDEV_S_FRAME_INTERVAL: {
+		struct v4l2_subdev_frame_interval *fi = arg;
+
+		if (fi->pad >= sd->entity.num_pads)
+			return -EINVAL;
 
-	case VIDIOC_SUBDEV_S_FRAME_INTERVAL:
 		return v4l2_subdev_call(sd, video, s_frame_interval, arg);
+	}
 
 	case VIDIOC_SUBDEV_ENUM_FRAME_INTERVAL: {
 		struct v4l2_subdev_frame_interval_enum *fie = arg;
@@ -349,11 +361,54 @@ static long subdev_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 			sd, pad, set_selection, subdev_fh, sel);
 	}
 
-	case VIDIOC_G_EDID:
-		return v4l2_subdev_call(sd, pad, get_edid, arg);
+	case VIDIOC_G_EDID: {
+		struct v4l2_subdev_edid *edid = arg;
+
+		if (edid->pad >= sd->entity.num_pads)
+			return -EINVAL;
+		if (edid->blocks && edid->edid == NULL)
+			return -EINVAL;
+
+		return v4l2_subdev_call(sd, pad, get_edid, edid);
+	}
+
+	case VIDIOC_S_EDID: {
+		struct v4l2_subdev_edid *edid = arg;
+
+		if (edid->pad >= sd->entity.num_pads)
+			return -EINVAL;
+		if (edid->blocks && edid->edid == NULL)
+			return -EINVAL;
+
+		return v4l2_subdev_call(sd, pad, set_edid, edid);
+	}
+
+	case VIDIOC_SUBDEV_DV_TIMINGS_CAP: {
+		struct v4l2_dv_timings_cap *cap = arg;
+
+		if (cap->pad >= sd->entity.num_pads)
+			return -EINVAL;
+
+		return v4l2_subdev_call(sd, pad, dv_timings_cap, cap);
+	}
+
+	case VIDIOC_SUBDEV_ENUM_DV_TIMINGS: {
+		struct v4l2_enum_dv_timings *dvt = arg;
+
+		if (dvt->pad >= sd->entity.num_pads)
+			return -EINVAL;
+
+		return v4l2_subdev_call(sd, pad, enum_dv_timings, dvt);
+	}
+
+	case VIDIOC_SUBDEV_QUERY_DV_TIMINGS:
+		return v4l2_subdev_call(sd, video, query_dv_timings, arg);
+
+	case VIDIOC_SUBDEV_G_DV_TIMINGS:
+		return v4l2_subdev_call(sd, video, g_dv_timings, arg);
 
-	case VIDIOC_S_EDID:
-		return v4l2_subdev_call(sd, pad, set_edid, arg);
+	case VIDIOC_SUBDEV_S_DV_TIMINGS:
+		return v4l2_subdev_call(sd, video, s_dv_timings, arg);
 #endif
 	default:
 		return v4l2_subdev_call(sd, core, ioctl, cmd, arg);
diff --git a/drivers/media/v4l2-core/videobuf-dma-contig.c b/drivers/media/v4l2-core/videobuf-dma-contig.c
index 7e6b209b700..bf80f0f7dfb 100644
--- a/drivers/media/v4l2-core/videobuf-dma-contig.c
+++ b/drivers/media/v4l2-core/videobuf-dma-contig.c
@@ -305,7 +305,7 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 	/* Try to remap memory */
 	size = vma->vm_end - vma->vm_start;
 	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
-	retval = vm_iomap_memory(vma, vma->vm_start, size);
+	retval = vm_iomap_memory(vma, mem->dma_handle, size);
 	if (retval) {
 		dev_err(q->dev, "mmap: remap failed with error %d. ",
 			retval);
diff --git a/drivers/media/v4l2-core/videobuf2-core.c b/drivers/media/v4l2-core/videobuf2-core.c
index f9059bb7384..349e659d75f 100644
--- a/drivers/media/v4l2-core/videobuf2-core.c
+++ b/drivers/media/v4l2-core/videobuf2-core.c
@@ -6,6 +6,9 @@
  * Author: Pawel Osciak <pawel@osciak.com>
  *	   Marek Szyprowski <m.szyprowski@samsung.com>
  *
+ * The vb2_thread implementation was based on code from videobuf-dvb.c:
+ *	(c) 2004 Gerd Knorr <kraxel@bytesex.org> [SUSE Labs]
+ *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
  * the Free Software Foundation.
@@ -18,76 +21,154 @@
 #include <linux/poll.h>
 #include <linux/slab.h>
 #include <linux/sched.h>
+#include <linux/freezer.h>
+#include <linux/kthread.h>
 
 #include <media/v4l2-dev.h>
 #include <media/v4l2-fh.h>
 #include <media/v4l2-event.h>
+#include <media/v4l2-common.h>
 #include <media/videobuf2-core.h>
 
 static int debug;
 module_param(debug, int, 0644);
 
-#define dprintk(level, fmt, arg...)					\
-	do {								\
-		if (debug >= level)					\
-			printk(KERN_DEBUG "vb2: " fmt, ## arg);		\
+#define dprintk(level, fmt, arg...)					      \
+	do {								      \
+		if (debug >= level)					      \
+			pr_debug("vb2: %s: " fmt, __func__, ## arg); \
 	} while (0)
 
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 
 /*
- * If advanced debugging is on, then count how often each op is called,
- * which can either be per-buffer or per-queue.
+ * If advanced debugging is on, then count how often each op is called
+ * successfully, which can either be per-buffer or per-queue.
  *
- * If the op failed then the 'fail_' variant is called to decrease the
- * counter. That makes it easy to check that the 'init' and 'cleanup'
+ * This makes it easy to check that the 'init' and 'cleanup'
  * (and variations thereof) stay balanced.
  */
 
+#define log_memop(vb, op)						\
+	dprintk(2, "call_memop(%p, %d, %s)%s\n",			\
+		(vb)->vb2_queue, (vb)->v4l2_buf.index, #op,		\
+		(vb)->vb2_queue->mem_ops->op ? "" : " (nop)")
+
 #define call_memop(vb, op, args...)					\
 ({									\
 	struct vb2_queue *_q = (vb)->vb2_queue;				\
-	dprintk(2, "call_memop(%p, %d, %s)%s\n",			\
-		_q, (vb)->v4l2_buf.index, #op,				\
-		_q->mem_ops->op ? "" : " (nop)");			\
+	int err;							\
+									\
+	log_memop(vb, op);						\
+	err = _q->mem_ops->op ? _q->mem_ops->op(args) : 0;		\
+	if (!err)							\
+		(vb)->cnt_mem_ ## op++;					\
+	err;								\
+})
+
+#define call_ptr_memop(vb, op, args...)					\
+({									\
+	struct vb2_queue *_q = (vb)->vb2_queue;				\
+	void *ptr;							\
+									\
+	log_memop(vb, op);						\
+	ptr = _q->mem_ops->op ? _q->mem_ops->op(args) : NULL;		\
+	if (!IS_ERR_OR_NULL(ptr))					\
+		(vb)->cnt_mem_ ## op++;					\
+	ptr;								\
+})
+
+#define call_void_memop(vb, op, args...)				\
+({									\
+	struct vb2_queue *_q = (vb)->vb2_queue;				\
+									\
+	log_memop(vb, op);						\
+	if (_q->mem_ops->op)						\
+		_q->mem_ops->op(args);					\
 	(vb)->cnt_mem_ ## op++;						\
-	_q->mem_ops->op ? _q->mem_ops->op(args) : 0;			\
 })
-#define fail_memop(vb, op) ((vb)->cnt_mem_ ## op--)
+
+#define log_qop(q, op)							\
+	dprintk(2, "call_qop(%p, %s)%s\n", q, #op,			\
+		(q)->ops->op ? "" : " (nop)")
 
 #define call_qop(q, op, args...)					\
 ({									\
-	dprintk(2, "call_qop(%p, %s)%s\n", q, #op,			\
-		(q)->ops->op ? "" : " (nop)");				\
+	int err;							\
+									\
+	log_qop(q, op);							\
+	err = (q)->ops->op ? (q)->ops->op(args) : 0;			\
+	if (!err)							\
+		(q)->cnt_ ## op++;					\
+	err;								\
+})
+
+#define call_void_qop(q, op, args...)					\
+({									\
+	log_qop(q, op);							\
+	if ((q)->ops->op)						\
+		(q)->ops->op(args);					\
 	(q)->cnt_ ## op++;						\
-	(q)->ops->op ? (q)->ops->op(args) : 0;				\
 })
-#define fail_qop(q, op) ((q)->cnt_ ## op--)
+
+#define log_vb_qop(vb, op, args...)					\
+	dprintk(2, "call_vb_qop(%p, %d, %s)%s\n",			\
+		(vb)->vb2_queue, (vb)->v4l2_buf.index, #op,		\
+		(vb)->vb2_queue->ops->op ? "" : " (nop)")
 
 #define call_vb_qop(vb, op, args...)					\
 ({									\
-	struct vb2_queue *_q = (vb)->vb2_queue;				\
-	dprintk(2, "call_vb_qop(%p, %d, %s)%s\n",			\
-		_q, (vb)->v4l2_buf.index, #op,				\
-		_q->ops->op ? "" : " (nop)");				\
+	int err;							\
+									\
+	log_vb_qop(vb, op);						\
+	err = (vb)->vb2_queue->ops->op ?				\
+		(vb)->vb2_queue->ops->op(args) : 0;			\
+	if (!err)							\
+		(vb)->cnt_ ## op++;					\
+	err;								\
+})
+
+#define call_void_vb_qop(vb, op, args...)				\
+({									\
+	log_vb_qop(vb, op);						\
+	if ((vb)->vb2_queue->ops->op)					\
+		(vb)->vb2_queue->ops->op(args);				\
 	(vb)->cnt_ ## op++;						\
-	_q->ops->op ? _q->ops->op(args) : 0;				\
 })
-#define fail_vb_qop(vb, op) ((vb)->cnt_ ## op--)
 
 #else
 
 #define call_memop(vb, op, args...)					\
-	((vb)->vb2_queue->mem_ops->op ? (vb)->vb2_queue->mem_ops->op(args) : 0)
-#define fail_memop(vb, op)
+	((vb)->vb2_queue->mem_ops->op ?					\
+		(vb)->vb2_queue->mem_ops->op(args) : 0)
+
+#define call_ptr_memop(vb, op, args...)					\
+	((vb)->vb2_queue->mem_ops->op ?					\
+		(vb)->vb2_queue->mem_ops->op(args) : NULL)
+
+#define call_void_memop(vb, op, args...)				\
+	do {								\
+		if ((vb)->vb2_queue->mem_ops->op)			\
+			(vb)->vb2_queue->mem_ops->op(args);		\
+	} while (0)
 
 #define call_qop(q, op, args...)					\
 	((q)->ops->op ? (q)->ops->op(args) : 0)
-#define fail_qop(q, op)
+
+#define call_void_qop(q, op, args...)					\
+	do {								\
+		if ((q)->ops->op)					\
+			(q)->ops->op(args);				\
+	} while (0)
 
 #define call_vb_qop(vb, op, args...)					\
 	((vb)->vb2_queue->ops->op ? (vb)->vb2_queue->ops->op(args) : 0)
-#define fail_vb_qop(vb, op)
+
+#define call_void_vb_qop(vb, op, args...)				\
+	do {								\
+		if ((vb)->vb2_queue->ops->op)				\
+			(vb)->vb2_queue->ops->op(args);			\
+	} while (0)
 
 #endif
 
@@ -118,7 +199,7 @@ static int __vb2_buf_mem_alloc(struct vb2_buffer *vb)
 	for (plane = 0; plane < vb->num_planes; ++plane) {
 		unsigned long size = PAGE_ALIGN(q->plane_sizes[plane]);
 
-		mem_priv = call_memop(vb, alloc, q->alloc_ctx[plane],
+		mem_priv = call_ptr_memop(vb, alloc, q->alloc_ctx[plane],
 				      size, q->gfp_flags);
 		if (IS_ERR_OR_NULL(mem_priv))
 			goto free;
@@ -130,10 +211,9 @@ static int __vb2_buf_mem_alloc(struct vb2_buffer *vb)
 
 	return 0;
 free:
-	fail_memop(vb, alloc);
 	/* Free already allocated memory if one of the allocations failed */
 	for (; plane > 0; --plane) {
-		call_memop(vb, put, vb->planes[plane - 1].mem_priv);
+		call_void_memop(vb, put, vb->planes[plane - 1].mem_priv);
 		vb->planes[plane - 1].mem_priv = NULL;
 	}
 
@@ -148,9 +228,9 @@ static void __vb2_buf_mem_free(struct vb2_buffer *vb)
 	unsigned int plane;
 
 	for (plane = 0; plane < vb->num_planes; ++plane) {
-		call_memop(vb, put, vb->planes[plane].mem_priv);
+		call_void_memop(vb, put, vb->planes[plane].mem_priv);
 		vb->planes[plane].mem_priv = NULL;
-		dprintk(3, "Freed plane %d of buffer %d\n", plane,
+		dprintk(3, "freed plane %d of buffer %d\n", plane,
 			vb->v4l2_buf.index);
 	}
 }
@@ -165,7 +245,7 @@ static void __vb2_buf_userptr_put(struct vb2_buffer *vb)
 
 	for (plane = 0; plane < vb->num_planes; ++plane) {
 		if (vb->planes[plane].mem_priv)
-			call_memop(vb, put_userptr, vb->planes[plane].mem_priv);
+			call_void_memop(vb, put_userptr, vb->planes[plane].mem_priv);
 		vb->planes[plane].mem_priv = NULL;
 	}
 }
@@ -180,9 +260,9 @@ static void __vb2_plane_dmabuf_put(struct vb2_buffer *vb, struct vb2_plane *p)
 		return;
 
 	if (p->dbuf_mapped)
-		call_memop(vb, unmap_dmabuf, p->mem_priv);
+		call_void_memop(vb, unmap_dmabuf, p->mem_priv);
 
-	call_memop(vb, detach_dmabuf, p->mem_priv);
+	call_void_memop(vb, detach_dmabuf, p->mem_priv);
 	dma_buf_put(p->dbuf);
 	memset(p, 0, sizeof(*p));
 }
@@ -245,7 +325,7 @@ static void __setup_offsets(struct vb2_queue *q, unsigned int n)
 		for (plane = 0; plane < vb->num_planes; ++plane) {
 			vb->v4l2_planes[plane].m.mem_offset = off;
 
-			dprintk(3, "Buffer %d, plane %d offset 0x%08lx\n",
+			dprintk(3, "buffer %d, plane %d offset 0x%08lx\n",
 					buffer, plane, off);
 
 			off += vb->v4l2_planes[plane].length;
@@ -272,7 +352,7 @@ static int __vb2_queue_alloc(struct vb2_queue *q, enum v4l2_memory memory,
 		/* Allocate videobuf buffer structures */
 		vb = kzalloc(q->buf_struct_size, GFP_KERNEL);
 		if (!vb) {
-			dprintk(1, "Memory alloc for buffer struct failed\n");
+			dprintk(1, "memory alloc for buffer struct failed\n");
 			break;
 		}
 
@@ -291,7 +371,7 @@ static int __vb2_queue_alloc(struct vb2_queue *q, enum v4l2_memory memory,
 		if (memory == V4L2_MEMORY_MMAP) {
 			ret = __vb2_buf_mem_alloc(vb);
 			if (ret) {
-				dprintk(1, "Failed allocating memory for "
+				dprintk(1, "failed allocating memory for "
 						"buffer %d\n", buffer);
 				kfree(vb);
 				break;
@@ -303,9 +383,8 @@ static int __vb2_queue_alloc(struct vb2_queue *q, enum v4l2_memory memory,
 			 */
 			ret = call_vb_qop(vb, buf_init, vb);
 			if (ret) {
-				dprintk(1, "Buffer %d %p initialization"
+				dprintk(1, "buffer %d %p initialization"
 					" failed\n", buffer, vb);
-				fail_vb_qop(vb, buf_init);
 				__vb2_buf_mem_free(vb);
 				kfree(vb);
 				break;
@@ -319,7 +398,7 @@ static int __vb2_queue_alloc(struct vb2_queue *q, enum v4l2_memory memory,
 	if (memory == V4L2_MEMORY_MMAP)
 		__setup_offsets(q, buffer);
 
-	dprintk(1, "Allocated %d buffers, %d plane(s) each\n",
+	dprintk(1, "allocated %d buffers, %d plane(s) each\n",
 			buffer, num_planes);
 
 	return buffer;
@@ -371,7 +450,7 @@ static int __vb2_queue_free(struct vb2_queue *q, unsigned int buffers)
 		if (q->bufs[buffer] == NULL)
 			continue;
 		if (q->bufs[buffer]->state == VB2_BUF_STATE_PREPARING) {
-			dprintk(1, "reqbufs: preparing buffers, cannot free\n");
+			dprintk(1, "preparing buffers, cannot free\n");
 			return -EAGAIN;
 		}
 	}
@@ -382,7 +461,7 @@ static int __vb2_queue_free(struct vb2_queue *q, unsigned int buffers)
 		struct vb2_buffer *vb = q->bufs[buffer];
 
 		if (vb && vb->planes[0].mem_priv)
-			call_vb_qop(vb, buf_cleanup, vb);
+			call_void_vb_qop(vb, buf_cleanup, vb);
 	}
 
 	/* Release video buffer memory */
@@ -476,13 +555,13 @@ static int __verify_planes_array(struct vb2_buffer *vb, const struct v4l2_buffer
 
 	/* Is memory for copying plane information present? */
 	if (NULL == b->m.planes) {
-		dprintk(1, "Multi-planar buffer passed but "
+		dprintk(1, "multi-planar buffer passed but "
 			   "planes array not provided\n");
 		return -EINVAL;
 	}
 
 	if (b->length < vb->num_planes || b->length > VIDEO_MAX_PLANES) {
-		dprintk(1, "Incorrect planes array length, "
+		dprintk(1, "incorrect planes array length, "
 			   "expected %d, got %d\n", vb->num_planes, b->length);
 		return -EINVAL;
 	}
@@ -656,12 +735,12 @@ int vb2_querybuf(struct vb2_queue *q, struct v4l2_buffer *b)
 	int ret;
 
 	if (b->type != q->type) {
-		dprintk(1, "querybuf: wrong buffer type\n");
+		dprintk(1, "wrong buffer type\n");
 		return -EINVAL;
 	}
 
 	if (b->index >= q->num_buffers) {
-		dprintk(1, "querybuf: buffer index out of range\n");
+		dprintk(1, "buffer index out of range\n");
 		return -EINVAL;
 	}
 	vb = q->bufs[b->index];
@@ -721,12 +800,12 @@ static int __verify_memory_type(struct vb2_queue *q,
 {
 	if (memory != V4L2_MEMORY_MMAP && memory != V4L2_MEMORY_USERPTR &&
 	    memory != V4L2_MEMORY_DMABUF) {
-		dprintk(1, "reqbufs: unsupported memory type\n");
+		dprintk(1, "unsupported memory type\n");
 		return -EINVAL;
 	}
 
 	if (type != q->type) {
-		dprintk(1, "reqbufs: requested type is incorrect\n");
+		dprintk(1, "requested type is incorrect\n");
 		return -EINVAL;
 	}
 
@@ -735,17 +814,17 @@ static int __verify_memory_type(struct vb2_queue *q,
 	 * are available.
 	 */
 	if (memory == V4L2_MEMORY_MMAP && __verify_mmap_ops(q)) {
-		dprintk(1, "reqbufs: MMAP for current setup unsupported\n");
+		dprintk(1, "MMAP for current setup unsupported\n");
 		return -EINVAL;
 	}
 
 	if (memory == V4L2_MEMORY_USERPTR && __verify_userptr_ops(q)) {
-		dprintk(1, "reqbufs: USERPTR for current setup unsupported\n");
+		dprintk(1, "USERPTR for current setup unsupported\n");
 		return -EINVAL;
 	}
 
 	if (memory == V4L2_MEMORY_DMABUF && __verify_dmabuf_ops(q)) {
-		dprintk(1, "reqbufs: DMABUF for current setup unsupported\n");
+		dprintk(1, "DMABUF for current setup unsupported\n");
 		return -EINVAL;
 	}
 
@@ -754,8 +833,8 @@ static int __verify_memory_type(struct vb2_queue *q,
 	 * create_bufs is called with count == 0, but count == 0 should still
 	 * do the memory and type validation.
 	 */
-	if (q->fileio) {
-		dprintk(1, "reqbufs: file io in progress\n");
+	if (vb2_fileio_is_active(q)) {
+		dprintk(1, "file io in progress\n");
 		return -EBUSY;
 	}
 	return 0;
@@ -790,7 +869,7 @@ static int __reqbufs(struct vb2_queue *q, struct v4l2_requestbuffers *req)
 	int ret;
 
 	if (q->streaming) {
-		dprintk(1, "reqbufs: streaming active\n");
+		dprintk(1, "streaming active\n");
 		return -EBUSY;
 	}
 
@@ -800,7 +879,7 @@ static int __reqbufs(struct vb2_queue *q, struct v4l2_requestbuffers *req)
 		 * are not in use and can be freed.
 		 */
 		if (q->memory == V4L2_MEMORY_MMAP && __buffers_in_use(q)) {
-			dprintk(1, "reqbufs: memory in use, cannot free\n");
+			dprintk(1, "memory in use, cannot free\n");
 			return -EBUSY;
 		}
 
@@ -826,7 +905,7 @@ static int __reqbufs(struct vb2_queue *q, struct v4l2_requestbuffers *req)
 	 * Make sure the requested values and current defaults are sane.
 	 */
 	num_buffers = min_t(unsigned int, req->count, VIDEO_MAX_FRAME);
-	num_buffers = max_t(unsigned int, req->count, q->min_buffers_needed);
+	num_buffers = max_t(unsigned int, num_buffers, q->min_buffers_needed);
 	memset(q->plane_sizes, 0, sizeof(q->plane_sizes));
 	memset(q->alloc_ctx, 0, sizeof(q->alloc_ctx));
 	q->memory = req->memory;
@@ -837,15 +916,13 @@ static int __reqbufs(struct vb2_queue *q, struct v4l2_requestbuffers *req)
 	 */
 	ret = call_qop(q, queue_setup, q, NULL, &num_buffers, &num_planes,
 		       q->plane_sizes, q->alloc_ctx);
-	if (ret) {
-		fail_qop(q, queue_setup);
+	if (ret)
 		return ret;
-	}
 
 	/* Finally, allocate buffers and video memory */
 	allocated_buffers = __vb2_queue_alloc(q, req->memory, num_buffers, num_planes);
 	if (allocated_buffers == 0) {
-		dprintk(1, "Memory allocation failed\n");
+		dprintk(1, "memory allocation failed\n");
 		return -ENOMEM;
 	}
 
@@ -864,8 +941,6 @@ static int __reqbufs(struct vb2_queue *q, struct v4l2_requestbuffers *req)
 
 		ret = call_qop(q, queue_setup, q, NULL, &num_buffers,
 			       &num_planes, q->plane_sizes, q->alloc_ctx);
-		if (ret)
-			fail_qop(q, queue_setup);
 
 		if (!ret && allocated_buffers < num_buffers)
 			ret = -ENOMEM;
@@ -931,8 +1006,7 @@ static int __create_bufs(struct vb2_queue *q, struct v4l2_create_buffers *create
 	int ret;
 
 	if (q->num_buffers == VIDEO_MAX_FRAME) {
-		dprintk(1, "%s(): maximum number of buffers already allocated\n",
-			__func__);
+		dprintk(1, "maximum number of buffers already allocated\n");
 		return -ENOBUFS;
 	}
 
@@ -950,16 +1024,14 @@ static int __create_bufs(struct vb2_queue *q, struct v4l2_create_buffers *create
 	 */
 	ret = call_qop(q, queue_setup, q, &create->format, &num_buffers,
 		       &num_planes, q->plane_sizes, q->alloc_ctx);
-	if (ret) {
-		fail_qop(q, queue_setup);
+	if (ret)
 		return ret;
-	}
 
 	/* Finally, allocate buffers and video memory */
 	allocated_buffers = __vb2_queue_alloc(q, create->memory, num_buffers,
 				num_planes);
 	if (allocated_buffers == 0) {
-		dprintk(1, "Memory allocation failed\n");
+		dprintk(1, "memory allocation failed\n");
 		return -ENOMEM;
 	}
 
@@ -975,8 +1047,6 @@ static int __create_bufs(struct vb2_queue *q, struct v4l2_create_buffers *create
 		 */
 		ret = call_qop(q, queue_setup, q, &create->format, &num_buffers,
 			       &num_planes, q->plane_sizes, q->alloc_ctx);
-		if (ret)
-			fail_qop(q, queue_setup);
 
 		if (!ret && allocated_buffers < num_buffers)
 			ret = -ENOMEM;
@@ -1038,7 +1108,7 @@ void *vb2_plane_vaddr(struct vb2_buffer *vb, unsigned int plane_no)
 	if (plane_no > vb->num_planes || !vb->planes[plane_no].mem_priv)
 		return NULL;
 
-	return call_memop(vb, vaddr, vb->planes[plane_no].mem_priv);
+	return call_ptr_memop(vb, vaddr, vb->planes[plane_no].mem_priv);
 
 }
 EXPORT_SYMBOL_GPL(vb2_plane_vaddr);
@@ -1059,7 +1129,7 @@ void *vb2_plane_cookie(struct vb2_buffer *vb, unsigned int plane_no)
 	if (plane_no > vb->num_planes || !vb->planes[plane_no].mem_priv)
 		return NULL;
 
-	return call_memop(vb, cookie, vb->planes[plane_no].mem_priv);
+	return call_ptr_memop(vb, cookie, vb->planes[plane_no].mem_priv);
 }
 EXPORT_SYMBOL_GPL(vb2_plane_cookie);
 
@@ -1094,9 +1164,8 @@ void vb2_buffer_done(struct vb2_buffer *vb, enum vb2_buffer_state state)
 	if (!q->start_streaming_called) {
 		if (WARN_ON(state != VB2_BUF_STATE_QUEUED))
 			state = VB2_BUF_STATE_QUEUED;
-	} else if (!WARN_ON(!q->start_streaming_called)) {
-		if (WARN_ON(state != VB2_BUF_STATE_DONE &&
-			    state != VB2_BUF_STATE_ERROR))
+	} else if (WARN_ON(state != VB2_BUF_STATE_DONE &&
+			   state != VB2_BUF_STATE_ERROR)) {
 			state = VB2_BUF_STATE_ERROR;
 	}
 
@@ -1107,12 +1176,12 @@ void vb2_buffer_done(struct vb2_buffer *vb, enum vb2_buffer_state state)
 	 */
 	vb->cnt_buf_done++;
 #endif
-	dprintk(4, "Done processing on buffer %d, state: %d\n",
+	dprintk(4, "done processing on buffer %d, state: %d\n",
 			vb->v4l2_buf.index, state);
 
 	/* sync buffers */
 	for (plane = 0; plane < vb->num_planes; ++plane)
-		call_memop(vb, finish, vb->planes[plane].mem_priv);
+		call_void_memop(vb, finish, vb->planes[plane].mem_priv);
 
 	/* Add the buffer to the done buffers list */
 	spin_lock_irqsave(&q->done_lock, flags);
@@ -1143,15 +1212,30 @@ static void __fill_vb2_buffer(struct vb2_buffer *vb, const struct v4l2_buffer *b
 	if (V4L2_TYPE_IS_MULTIPLANAR(b->type)) {
 		/* Fill in driver-provided information for OUTPUT types */
 		if (V4L2_TYPE_IS_OUTPUT(b->type)) {
+			bool bytesused_is_used;
+
+			/* Check if bytesused == 0 for all planes */
+			for (plane = 0; plane < vb->num_planes; ++plane)
+				if (b->m.planes[plane].bytesused)
+					break;
+			bytesused_is_used = plane < vb->num_planes;
+
 			/*
 			 * Will have to go up to b->length when API starts
 			 * accepting variable number of planes.
+			 *
+			 * If bytesused_is_used is false, then fall back to the
+			 * full buffer size. In that case userspace clearly
+			 * never bothered to set it and it's a safe assumption
+			 * that they really meant to use the full plane sizes.
 			 */
 			for (plane = 0; plane < vb->num_planes; ++plane) {
-				v4l2_planes[plane].bytesused =
-					b->m.planes[plane].bytesused;
-				v4l2_planes[plane].data_offset =
-					b->m.planes[plane].data_offset;
+				struct v4l2_plane *pdst = &v4l2_planes[plane];
+				struct v4l2_plane *psrc = &b->m.planes[plane];
+
+				pdst->bytesused = bytesused_is_used ?
+					psrc->bytesused : psrc->length;
+				pdst->data_offset = psrc->data_offset;
 			}
 		}
 
@@ -1169,8 +1253,6 @@ static void __fill_vb2_buffer(struct vb2_buffer *vb, const struct v4l2_buffer *b
 					b->m.planes[plane].m.fd;
 				v4l2_planes[plane].length =
 					b->m.planes[plane].length;
-				v4l2_planes[plane].data_offset =
-					b->m.planes[plane].data_offset;
 			}
 		}
 	} else {
@@ -1179,11 +1261,15 @@ static void __fill_vb2_buffer(struct vb2_buffer *vb, const struct v4l2_buffer *b
 		 * so fill in relevant v4l2_buffer struct fields instead.
 		 * In videobuf we use our internal V4l2_planes struct for
 		 * single-planar buffers as well, for simplicity.
+		 *
+		 * If bytesused == 0, then fall back to the full buffer size
+		 * as that's a sensible default.
 		 */
-		if (V4L2_TYPE_IS_OUTPUT(b->type)) {
-			v4l2_planes[0].bytesused = b->bytesused;
-			v4l2_planes[0].data_offset = 0;
-		}
+		if (V4L2_TYPE_IS_OUTPUT(b->type))
+			v4l2_planes[0].bytesused =
+				b->bytesused ? b->bytesused : b->length;
+		else
+			v4l2_planes[0].bytesused = 0;
 
 		if (b->memory == V4L2_MEMORY_USERPTR) {
 			v4l2_planes[0].m.userptr = b->m.userptr;
@@ -1193,9 +1279,7 @@ static void __fill_vb2_buffer(struct vb2_buffer *vb, const struct v4l2_buffer *b
 		if (b->memory == V4L2_MEMORY_DMABUF) {
 			v4l2_planes[0].m.fd = b->m.fd;
 			v4l2_planes[0].length = b->length;
-			v4l2_planes[0].data_offset = 0;
 		}
-
 	}
 
 	/* Zero flags that the vb2 core handles */
@@ -1226,6 +1310,15 @@ static void __fill_vb2_buffer(struct vb2_buffer *vb, const struct v4l2_buffer *b
 }
 
 /**
+ * __qbuf_mmap() - handle qbuf of an MMAP buffer
+ */
+static int __qbuf_mmap(struct vb2_buffer *vb, const struct v4l2_buffer *b)
+{
+	__fill_vb2_buffer(vb, b, vb->v4l2_planes);
+	return call_vb_qop(vb, buf_prepare, vb);
+}
+
+/**
  * __qbuf_userptr() - handle qbuf of a USERPTR buffer
  */
 static int __qbuf_userptr(struct vb2_buffer *vb, const struct v4l2_buffer *b)
@@ -1238,6 +1331,7 @@ static int __qbuf_userptr(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 	int write = !V4L2_TYPE_IS_OUTPUT(q->type);
 	bool reacquired = vb->planes[0].mem_priv == NULL;
 
+	memset(planes, 0, sizeof(planes[0]) * vb->num_planes);
 	/* Copy relevant information provided by the userspace */
 	__fill_vb2_buffer(vb, b, planes);
 
@@ -1248,12 +1342,12 @@ static int __qbuf_userptr(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 		    && vb->v4l2_planes[plane].length == planes[plane].length)
 			continue;
 
-		dprintk(3, "qbuf: userspace address for plane %d changed, "
+		dprintk(3, "userspace address for plane %d changed, "
 				"reacquiring memory\n", plane);
 
 		/* Check if the provided plane buffer is large enough */
 		if (planes[plane].length < q->plane_sizes[plane]) {
-			dprintk(1, "qbuf: provided buffer size %u is less than "
+			dprintk(1, "provided buffer size %u is less than "
 						"setup size %u for plane %d\n",
 						planes[plane].length,
 						q->plane_sizes[plane], plane);
@@ -1265,22 +1359,21 @@ static int __qbuf_userptr(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 		if (vb->planes[plane].mem_priv) {
 			if (!reacquired) {
 				reacquired = true;
-				call_vb_qop(vb, buf_cleanup, vb);
+				call_void_vb_qop(vb, buf_cleanup, vb);
 			}
-			call_memop(vb, put_userptr, vb->planes[plane].mem_priv);
+			call_void_memop(vb, put_userptr, vb->planes[plane].mem_priv);
 		}
 
 		vb->planes[plane].mem_priv = NULL;
 		memset(&vb->v4l2_planes[plane], 0, sizeof(struct v4l2_plane));
 
 		/* Acquire each plane's memory */
-		mem_priv = call_memop(vb, get_userptr, q->alloc_ctx[plane],
+		mem_priv = call_ptr_memop(vb, get_userptr, q->alloc_ctx[plane],
 				      planes[plane].m.userptr,
 				      planes[plane].length, write);
 		if (IS_ERR_OR_NULL(mem_priv)) {
-			dprintk(1, "qbuf: failed acquiring userspace "
+			dprintk(1, "failed acquiring userspace "
 						"memory for plane %d\n", plane);
-			fail_memop(vb, get_userptr);
 			ret = mem_priv ? PTR_ERR(mem_priv) : -EINVAL;
 			goto err;
 		}
@@ -1302,17 +1395,15 @@ static int __qbuf_userptr(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 		 */
 		ret = call_vb_qop(vb, buf_init, vb);
 		if (ret) {
-			dprintk(1, "qbuf: buffer initialization failed\n");
-			fail_vb_qop(vb, buf_init);
+			dprintk(1, "buffer initialization failed\n");
 			goto err;
 		}
 	}
 
 	ret = call_vb_qop(vb, buf_prepare, vb);
 	if (ret) {
-		dprintk(1, "qbuf: buffer preparation failed\n");
-		fail_vb_qop(vb, buf_prepare);
-		call_vb_qop(vb, buf_cleanup, vb);
+		dprintk(1, "buffer preparation failed\n");
+		call_void_vb_qop(vb, buf_cleanup, vb);
 		goto err;
 	}
 
@@ -1321,7 +1412,7 @@ err:
 	/* In case of errors, release planes that were already acquired */
 	for (plane = 0; plane < vb->num_planes; ++plane) {
 		if (vb->planes[plane].mem_priv)
-			call_memop(vb, put_userptr, vb->planes[plane].mem_priv);
+			call_void_memop(vb, put_userptr, vb->planes[plane].mem_priv);
 		vb->planes[plane].mem_priv = NULL;
 		vb->v4l2_planes[plane].m.userptr = 0;
 		vb->v4l2_planes[plane].length = 0;
@@ -1331,20 +1422,6 @@ err:
 }
 
 /**
- * __qbuf_mmap() - handle qbuf of an MMAP buffer
- */
-static int __qbuf_mmap(struct vb2_buffer *vb, const struct v4l2_buffer *b)
-{
-	int ret;
-
-	__fill_vb2_buffer(vb, b, vb->v4l2_planes);
-	ret = call_vb_qop(vb, buf_prepare, vb);
-	if (ret)
-		fail_vb_qop(vb, buf_prepare);
-	return ret;
-}
-
-/**
  * __qbuf_dmabuf() - handle qbuf of a DMABUF buffer
  */
 static int __qbuf_dmabuf(struct vb2_buffer *vb, const struct v4l2_buffer *b)
@@ -1357,6 +1434,7 @@ static int __qbuf_dmabuf(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 	int write = !V4L2_TYPE_IS_OUTPUT(q->type);
 	bool reacquired = vb->planes[0].mem_priv == NULL;
 
+	memset(planes, 0, sizeof(planes[0]) * vb->num_planes);
 	/* Copy relevant information provided by the userspace */
 	__fill_vb2_buffer(vb, b, planes);
 
@@ -1364,7 +1442,7 @@ static int __qbuf_dmabuf(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 		struct dma_buf *dbuf = dma_buf_get(planes[plane].m.fd);
 
 		if (IS_ERR_OR_NULL(dbuf)) {
-			dprintk(1, "qbuf: invalid dmabuf fd for plane %d\n",
+			dprintk(1, "invalid dmabuf fd for plane %d\n",
 				plane);
 			ret = -EINVAL;
 			goto err;
@@ -1374,9 +1452,8 @@ static int __qbuf_dmabuf(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 		if (planes[plane].length == 0)
 			planes[plane].length = dbuf->size;
 
-		if (planes[plane].length < planes[plane].data_offset +
-		    q->plane_sizes[plane]) {
-			dprintk(1, "qbuf: invalid dmabuf length for plane %d\n",
+		if (planes[plane].length < q->plane_sizes[plane]) {
+			dprintk(1, "invalid dmabuf length for plane %d\n",
 				plane);
 			ret = -EINVAL;
 			goto err;
@@ -1389,11 +1466,11 @@ static int __qbuf_dmabuf(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 			continue;
 		}
 
-		dprintk(1, "qbuf: buffer for plane %d changed\n", plane);
+		dprintk(1, "buffer for plane %d changed\n", plane);
 
 		if (!reacquired) {
 			reacquired = true;
-			call_vb_qop(vb, buf_cleanup, vb);
+			call_void_vb_qop(vb, buf_cleanup, vb);
 		}
 
 		/* Release previously acquired memory if present */
@@ -1401,11 +1478,10 @@ static int __qbuf_dmabuf(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 		memset(&vb->v4l2_planes[plane], 0, sizeof(struct v4l2_plane));
 
 		/* Acquire each plane's memory */
-		mem_priv = call_memop(vb, attach_dmabuf, q->alloc_ctx[plane],
+		mem_priv = call_ptr_memop(vb, attach_dmabuf, q->alloc_ctx[plane],
 			dbuf, planes[plane].length, write);
 		if (IS_ERR(mem_priv)) {
-			dprintk(1, "qbuf: failed to attach dmabuf\n");
-			fail_memop(vb, attach_dmabuf);
+			dprintk(1, "failed to attach dmabuf\n");
 			ret = PTR_ERR(mem_priv);
 			dma_buf_put(dbuf);
 			goto err;
@@ -1422,9 +1498,8 @@ static int __qbuf_dmabuf(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 	for (plane = 0; plane < vb->num_planes; ++plane) {
 		ret = call_memop(vb, map_dmabuf, vb->planes[plane].mem_priv);
 		if (ret) {
-			dprintk(1, "qbuf: failed to map dmabuf for plane %d\n",
+			dprintk(1, "failed to map dmabuf for plane %d\n",
 				plane);
-			fail_memop(vb, map_dmabuf);
 			goto err;
 		}
 		vb->planes[plane].dbuf_mapped = 1;
@@ -1444,17 +1519,15 @@ static int __qbuf_dmabuf(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 		 */
 		ret = call_vb_qop(vb, buf_init, vb);
 		if (ret) {
-			dprintk(1, "qbuf: buffer initialization failed\n");
-			fail_vb_qop(vb, buf_init);
+			dprintk(1, "buffer initialization failed\n");
 			goto err;
 		}
 	}
 
 	ret = call_vb_qop(vb, buf_prepare, vb);
 	if (ret) {
-		dprintk(1, "qbuf: buffer preparation failed\n");
-		fail_vb_qop(vb, buf_prepare);
-		call_vb_qop(vb, buf_cleanup, vb);
+		dprintk(1, "buffer preparation failed\n");
+		call_void_vb_qop(vb, buf_cleanup, vb);
 		goto err;
 	}
 
@@ -1479,9 +1552,9 @@ static void __enqueue_in_driver(struct vb2_buffer *vb)
 
 	/* sync buffers */
 	for (plane = 0; plane < vb->num_planes; ++plane)
-		call_memop(vb, prepare, vb->planes[plane].mem_priv);
+		call_void_memop(vb, prepare, vb->planes[plane].mem_priv);
 
-	call_vb_qop(vb, buf_queue, vb);
+	call_void_vb_qop(vb, buf_queue, vb);
 }
 
 static int __buf_prepare(struct vb2_buffer *vb, const struct v4l2_buffer *b)
@@ -1492,10 +1565,22 @@ static int __buf_prepare(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 
 	ret = __verify_length(vb, b);
 	if (ret < 0) {
-		dprintk(1, "%s(): plane parameters verification failed: %d\n",
-			__func__, ret);
+		dprintk(1, "plane parameters verification failed: %d\n", ret);
 		return ret;
 	}
+	if (b->field == V4L2_FIELD_ALTERNATE && V4L2_TYPE_IS_OUTPUT(q->type)) {
+		/*
+		 * If the format's field is ALTERNATE, then the buffer's field
+		 * should be either TOP or BOTTOM, not ALTERNATE since that
+		 * makes no sense. The driver has to know whether the
+		 * buffer represents a top or a bottom field in order to
+		 * program any DMA correctly. Using ALTERNATE is wrong, since
+		 * that just says that it is either a top or a bottom field,
+		 * but not which of the two it is.
+		 */
+		dprintk(1, "the field is incorrectly set to ALTERNATE for an output buffer\n");
+		return -EINVAL;
+	}
 
 	vb->state = VB2_BUF_STATE_PREPARING;
 	vb->v4l2_buf.timestamp.tv_sec = 0;
@@ -1520,9 +1605,9 @@ static int __buf_prepare(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 		 * mmap_sem and then takes the driver's lock again.
 		 */
 		mmap_sem = &current->mm->mmap_sem;
-		call_qop(q, wait_prepare, q);
+		call_void_qop(q, wait_prepare, q);
 		down_read(mmap_sem);
-		call_qop(q, wait_finish, q);
+		call_void_qop(q, wait_finish, q);
 
 		ret = __qbuf_userptr(vb, b);
 
@@ -1537,7 +1622,7 @@ static int __buf_prepare(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 	}
 
 	if (ret)
-		dprintk(1, "qbuf: buffer preparation failed: %d\n", ret);
+		dprintk(1, "buffer preparation failed: %d\n", ret);
 	vb->state = ret ? VB2_BUF_STATE_DEQUEUED : VB2_BUF_STATE_PREPARED;
 
 	return ret;
@@ -1547,23 +1632,23 @@ static int vb2_queue_or_prepare_buf(struct vb2_queue *q, struct v4l2_buffer *b,
 				    const char *opname)
 {
 	if (b->type != q->type) {
-		dprintk(1, "%s(): invalid buffer type\n", opname);
+		dprintk(1, "%s: invalid buffer type\n", opname);
 		return -EINVAL;
 	}
 
 	if (b->index >= q->num_buffers) {
-		dprintk(1, "%s(): buffer index out of range\n", opname);
+		dprintk(1, "%s: buffer index out of range\n", opname);
 		return -EINVAL;
 	}
 
 	if (q->bufs[b->index] == NULL) {
 		/* Should never happen */
-		dprintk(1, "%s(): buffer is NULL\n", opname);
+		dprintk(1, "%s: buffer is NULL\n", opname);
 		return -EINVAL;
 	}
 
 	if (b->memory != q->memory) {
-		dprintk(1, "%s(): invalid memory type\n", opname);
+		dprintk(1, "%s: invalid memory type\n", opname);
 		return -EINVAL;
 	}
 
@@ -1590,8 +1675,8 @@ int vb2_prepare_buf(struct vb2_queue *q, struct v4l2_buffer *b)
 	struct vb2_buffer *vb;
 	int ret;
 
-	if (q->fileio) {
-		dprintk(1, "%s(): file io in progress\n", __func__);
+	if (vb2_fileio_is_active(q)) {
+		dprintk(1, "file io in progress\n");
 		return -EBUSY;
 	}
 
@@ -1601,7 +1686,7 @@ int vb2_prepare_buf(struct vb2_queue *q, struct v4l2_buffer *b)
 
 	vb = q->bufs[b->index];
 	if (vb->state != VB2_BUF_STATE_DEQUEUED) {
-		dprintk(1, "%s(): invalid buffer state %d\n", __func__,
+		dprintk(1, "invalid buffer state %d\n",
 			vb->state);
 		return -EINVAL;
 	}
@@ -1611,7 +1696,7 @@ int vb2_prepare_buf(struct vb2_queue *q, struct v4l2_buffer *b)
 		/* Fill buffer information for the userspace */
 		__fill_v4l2_buffer(vb, b);
 
-		dprintk(1, "%s() of buffer %d succeeded\n", __func__, vb->v4l2_buf.index);
+		dprintk(1, "prepare of buffer %d succeeded\n", vb->v4l2_buf.index);
 	}
 	return ret;
 }
@@ -1647,8 +1732,7 @@ static int vb2_start_streaming(struct vb2_queue *q)
 	if (!ret)
 		return 0;
 
-	fail_qop(q, start_streaming);
-	dprintk(1, "qbuf: driver refused to start streaming\n");
+	dprintk(1, "driver refused to start streaming\n");
 	if (WARN_ON(atomic_read(&q->owned_by_drv_count))) {
 		unsigned i;
 
@@ -1686,11 +1770,10 @@ static int vb2_internal_qbuf(struct vb2_queue *q, struct v4l2_buffer *b)
 	case VB2_BUF_STATE_PREPARED:
 		break;
 	case VB2_BUF_STATE_PREPARING:
-		dprintk(1, "qbuf: buffer still being prepared\n");
+		dprintk(1, "buffer still being prepared\n");
 		return -EINVAL;
 	default:
-		dprintk(1, "%s(): invalid buffer state %d\n", __func__,
-			vb->state);
+		dprintk(1, "invalid buffer state %d\n", vb->state);
 		return -EINVAL;
 	}
 
@@ -1737,7 +1820,7 @@ static int vb2_internal_qbuf(struct vb2_queue *q, struct v4l2_buffer *b)
 			return ret;
 	}
 
-	dprintk(1, "%s() of buffer %d succeeded\n", __func__, vb->v4l2_buf.index);
+	dprintk(1, "qbuf of buffer %d succeeded\n", vb->v4l2_buf.index);
 	return 0;
 }
 
@@ -1760,8 +1843,8 @@ static int vb2_internal_qbuf(struct vb2_queue *q, struct v4l2_buffer *b)
  */
 int vb2_qbuf(struct vb2_queue *q, struct v4l2_buffer *b)
 {
-	if (q->fileio) {
-		dprintk(1, "%s(): file io in progress\n", __func__);
+	if (vb2_fileio_is_active(q)) {
+		dprintk(1, "file io in progress\n");
 		return -EBUSY;
 	}
 
@@ -1790,7 +1873,7 @@ static int __vb2_wait_for_done_vb(struct vb2_queue *q, int nonblocking)
 		int ret;
 
 		if (!q->streaming) {
-			dprintk(1, "Streaming off, will not wait for buffers\n");
+			dprintk(1, "streaming off, will not wait for buffers\n");
 			return -EINVAL;
 		}
 
@@ -1802,7 +1885,7 @@ static int __vb2_wait_for_done_vb(struct vb2_queue *q, int nonblocking)
 		}
 
 		if (nonblocking) {
-			dprintk(1, "Nonblocking and no buffers to dequeue, "
+			dprintk(1, "nonblocking and no buffers to dequeue, "
 								"will not wait\n");
 			return -EAGAIN;
 		}
@@ -1812,12 +1895,12 @@ static int __vb2_wait_for_done_vb(struct vb2_queue *q, int nonblocking)
 		 * become ready or for streamoff. Driver's lock is released to
 		 * allow streamoff or qbuf to be called while waiting.
 		 */
-		call_qop(q, wait_prepare, q);
+		call_void_qop(q, wait_prepare, q);
 
 		/*
 		 * All locks have been released, it is safe to sleep now.
 		 */
-		dprintk(3, "Will sleep waiting for buffers\n");
+		dprintk(3, "will sleep waiting for buffers\n");
 		ret = wait_event_interruptible(q->done_wq,
 				!list_empty(&q->done_list) || !q->streaming);
 
@@ -1825,9 +1908,9 @@ static int __vb2_wait_for_done_vb(struct vb2_queue *q, int nonblocking)
 		 * We need to reevaluate both conditions again after reacquiring
 		 * the locks or return an error if one occurred.
 		 */
-		call_qop(q, wait_finish, q);
+		call_void_qop(q, wait_finish, q);
 		if (ret) {
-			dprintk(1, "Sleep was interrupted\n");
+			dprintk(1, "sleep was interrupted\n");
 			return ret;
 		}
 	}
@@ -1882,7 +1965,7 @@ static int __vb2_get_done_vb(struct vb2_queue *q, struct vb2_buffer **vb,
 int vb2_wait_for_all_buffers(struct vb2_queue *q)
 {
 	if (!q->streaming) {
-		dprintk(1, "Streaming off, will not wait for buffers\n");
+		dprintk(1, "streaming off, will not wait for buffers\n");
 		return -EINVAL;
 	}
 
@@ -1911,7 +1994,7 @@ static void __vb2_dqbuf(struct vb2_buffer *vb)
 		for (i = 0; i < vb->num_planes; ++i) {
 			if (!vb->planes[i].dbuf_mapped)
 				continue;
-			call_memop(vb, unmap_dmabuf, vb->planes[i].mem_priv);
+			call_void_memop(vb, unmap_dmabuf, vb->planes[i].mem_priv);
 			vb->planes[i].dbuf_mapped = 0;
 		}
 }
@@ -1922,7 +2005,7 @@ static int vb2_internal_dqbuf(struct vb2_queue *q, struct v4l2_buffer *b, bool n
 	int ret;
 
 	if (b->type != q->type) {
-		dprintk(1, "dqbuf: invalid buffer type\n");
+		dprintk(1, "invalid buffer type\n");
 		return -EINVAL;
 	}
 	ret = __vb2_get_done_vb(q, &vb, b, nonblocking);
@@ -1931,17 +2014,17 @@ static int vb2_internal_dqbuf(struct vb2_queue *q, struct v4l2_buffer *b, bool n
 
 	switch (vb->state) {
 	case VB2_BUF_STATE_DONE:
-		dprintk(3, "dqbuf: Returning done buffer\n");
+		dprintk(3, "returning done buffer\n");
 		break;
 	case VB2_BUF_STATE_ERROR:
-		dprintk(3, "dqbuf: Returning done buffer with errors\n");
+		dprintk(3, "returning done buffer with errors\n");
 		break;
 	default:
-		dprintk(1, "dqbuf: Invalid buffer state\n");
+		dprintk(1, "invalid buffer state\n");
 		return -EINVAL;
 	}
 
-	call_vb_qop(vb, buf_finish, vb);
+	call_void_vb_qop(vb, buf_finish, vb);
 
 	/* Fill buffer information for the userspace */
 	__fill_v4l2_buffer(vb, b);
@@ -1980,8 +2063,8 @@ static int vb2_internal_dqbuf(struct vb2_queue *q, struct v4l2_buffer *b, bool n
  */
 int vb2_dqbuf(struct vb2_queue *q, struct v4l2_buffer *b, bool nonblocking)
 {
-	if (q->fileio) {
-		dprintk(1, "dqbuf: file io in progress\n");
+	if (vb2_fileio_is_active(q)) {
+		dprintk(1, "file io in progress\n");
 		return -EBUSY;
 	}
 	return vb2_internal_dqbuf(q, b, nonblocking);
@@ -2003,10 +2086,7 @@ static void __vb2_queue_cancel(struct vb2_queue *q)
 	 * buffers.
 	 */
 	if (q->start_streaming_called)
-		call_qop(q, stop_streaming, q);
-	q->streaming = 0;
-	q->start_streaming_called = 0;
-	q->queued_count = 0;
+		call_void_qop(q, stop_streaming, q);
 
 	if (WARN_ON(atomic_read(&q->owned_by_drv_count))) {
 		for (i = 0; i < q->num_buffers; ++i)
@@ -2016,6 +2096,10 @@ static void __vb2_queue_cancel(struct vb2_queue *q)
 		WARN_ON(atomic_read(&q->owned_by_drv_count));
 	}
 
+	q->streaming = 0;
+	q->start_streaming_called = 0;
+	q->queued_count = 0;
+
 	/*
 	 * Remove all buffers from videobuf's list...
 	 */
@@ -2042,7 +2126,7 @@ static void __vb2_queue_cancel(struct vb2_queue *q)
 
 		if (vb->state != VB2_BUF_STATE_DEQUEUED) {
 			vb->state = VB2_BUF_STATE_PREPARED;
-			call_vb_qop(vb, buf_finish, vb);
+			call_void_vb_qop(vb, buf_finish, vb);
 		}
 		__vb2_dqbuf(vb);
 	}
@@ -2053,26 +2137,22 @@ static int vb2_internal_streamon(struct vb2_queue *q, enum v4l2_buf_type type)
 	int ret;
 
 	if (type != q->type) {
-		dprintk(1, "streamon: invalid stream type\n");
+		dprintk(1, "invalid stream type\n");
 		return -EINVAL;
 	}
 
 	if (q->streaming) {
-		dprintk(3, "streamon successful: already streaming\n");
+		dprintk(3, "already streaming\n");
 		return 0;
 	}
 
 	if (!q->num_buffers) {
-		dprintk(1, "streamon: no buffers have been allocated\n");
+		dprintk(1, "no buffers have been allocated\n");
 		return -EINVAL;
 	}
 
-	if (!q->num_buffers) {
-		dprintk(1, "streamon: no buffers have been allocated\n");
-		return -EINVAL;
-	}
 	if (q->num_buffers < q->min_buffers_needed) {
-		dprintk(1, "streamon: need at least %u allocated buffers\n",
+		dprintk(1, "need at least %u allocated buffers\n",
 				q->min_buffers_needed);
 		return -EINVAL;
 	}
@@ -2091,7 +2171,7 @@ static int vb2_internal_streamon(struct vb2_queue *q, enum v4l2_buf_type type)
 
 	q->streaming = 1;
 
-	dprintk(3, "Streamon successful\n");
+	dprintk(3, "successful\n");
 	return 0;
 }
 
@@ -2110,8 +2190,8 @@ static int vb2_internal_streamon(struct vb2_queue *q, enum v4l2_buf_type type)
  */
 int vb2_streamon(struct vb2_queue *q, enum v4l2_buf_type type)
 {
-	if (q->fileio) {
-		dprintk(1, "streamon: file io in progress\n");
+	if (vb2_fileio_is_active(q)) {
+		dprintk(1, "file io in progress\n");
 		return -EBUSY;
 	}
 	return vb2_internal_streamon(q, type);
@@ -2121,7 +2201,7 @@ EXPORT_SYMBOL_GPL(vb2_streamon);
 static int vb2_internal_streamoff(struct vb2_queue *q, enum v4l2_buf_type type)
 {
 	if (type != q->type) {
-		dprintk(1, "streamoff: invalid stream type\n");
+		dprintk(1, "invalid stream type\n");
 		return -EINVAL;
 	}
 
@@ -2136,7 +2216,7 @@ static int vb2_internal_streamoff(struct vb2_queue *q, enum v4l2_buf_type type)
 	 */
 	__vb2_queue_cancel(q);
 
-	dprintk(3, "Streamoff successful\n");
+	dprintk(3, "successful\n");
 	return 0;
 }
 
@@ -2157,8 +2237,8 @@ static int vb2_internal_streamoff(struct vb2_queue *q, enum v4l2_buf_type type)
  */
 int vb2_streamoff(struct vb2_queue *q, enum v4l2_buf_type type)
 {
-	if (q->fileio) {
-		dprintk(1, "streamoff: file io in progress\n");
+	if (vb2_fileio_is_active(q)) {
+		dprintk(1, "file io in progress\n");
 		return -EBUSY;
 	}
 	return vb2_internal_streamoff(q, type);
@@ -2211,22 +2291,22 @@ int vb2_expbuf(struct vb2_queue *q, struct v4l2_exportbuffer *eb)
 	struct dma_buf *dbuf;
 
 	if (q->memory != V4L2_MEMORY_MMAP) {
-		dprintk(1, "Queue is not currently set up for mmap\n");
+		dprintk(1, "queue is not currently set up for mmap\n");
 		return -EINVAL;
 	}
 
 	if (!q->mem_ops->get_dmabuf) {
-		dprintk(1, "Queue does not support DMA buffer exporting\n");
+		dprintk(1, "queue does not support DMA buffer exporting\n");
 		return -EINVAL;
 	}
 
 	if (eb->flags & ~(O_CLOEXEC | O_ACCMODE)) {
-		dprintk(1, "Queue does support only O_CLOEXEC and access mode flags\n");
+		dprintk(1, "queue does support only O_CLOEXEC and access mode flags\n");
 		return -EINVAL;
 	}
 
 	if (eb->type != q->type) {
-		dprintk(1, "qbuf: invalid buffer type\n");
+		dprintk(1, "invalid buffer type\n");
 		return -EINVAL;
 	}
 
@@ -2242,13 +2322,17 @@ int vb2_expbuf(struct vb2_queue *q, struct v4l2_exportbuffer *eb)
 		return -EINVAL;
 	}
 
+	if (vb2_fileio_is_active(q)) {
+		dprintk(1, "expbuf: file io in progress\n");
+		return -EBUSY;
+	}
+
 	vb_plane = &vb->planes[eb->plane];
 
-	dbuf = call_memop(vb, get_dmabuf, vb_plane->mem_priv, eb->flags & O_ACCMODE);
+	dbuf = call_ptr_memop(vb, get_dmabuf, vb_plane->mem_priv, eb->flags & O_ACCMODE);
 	if (IS_ERR_OR_NULL(dbuf)) {
-		dprintk(1, "Failed to export buffer %d, plane %d\n",
+		dprintk(1, "failed to export buffer %d, plane %d\n",
 			eb->index, eb->plane);
-		fail_memop(vb, get_dmabuf);
 		return -EINVAL;
 	}
 
@@ -2291,12 +2375,12 @@ int vb2_mmap(struct vb2_queue *q, struct vm_area_struct *vma)
 {
 	unsigned long off = vma->vm_pgoff << PAGE_SHIFT;
 	struct vb2_buffer *vb;
-	unsigned int buffer, plane;
+	unsigned int buffer = 0, plane = 0;
 	int ret;
 	unsigned long length;
 
 	if (q->memory != V4L2_MEMORY_MMAP) {
-		dprintk(1, "Queue is not currently set up for mmap\n");
+		dprintk(1, "queue is not currently set up for mmap\n");
 		return -EINVAL;
 	}
 
@@ -2304,20 +2388,24 @@ int vb2_mmap(struct vb2_queue *q, struct vm_area_struct *vma)
 	 * Check memory area access mode.
 	 */
 	if (!(vma->vm_flags & VM_SHARED)) {
-		dprintk(1, "Invalid vma flags, VM_SHARED needed\n");
+		dprintk(1, "invalid vma flags, VM_SHARED needed\n");
 		return -EINVAL;
 	}
 	if (V4L2_TYPE_IS_OUTPUT(q->type)) {
 		if (!(vma->vm_flags & VM_WRITE)) {
-			dprintk(1, "Invalid vma flags, VM_WRITE needed\n");
+			dprintk(1, "invalid vma flags, VM_WRITE needed\n");
 			return -EINVAL;
 		}
 	} else {
 		if (!(vma->vm_flags & VM_READ)) {
-			dprintk(1, "Invalid vma flags, VM_READ needed\n");
+			dprintk(1, "invalid vma flags, VM_READ needed\n");
 			return -EINVAL;
 		}
 	}
+	if (vb2_fileio_is_active(q)) {
+		dprintk(1, "mmap: file io in progress\n");
+		return -EBUSY;
+	}
 
 	/*
 	 * Find the plane corresponding to the offset passed by userspace.
@@ -2341,12 +2429,10 @@ int vb2_mmap(struct vb2_queue *q, struct vm_area_struct *vma)
 	}
 
 	ret = call_memop(vb, mmap, vb->planes[plane].mem_priv, vma);
-	if (ret) {
-		fail_memop(vb, mmap);
+	if (ret)
 		return ret;
-	}
 
-	dprintk(3, "Buffer %d, plane %d successfully mapped\n", buffer, plane);
+	dprintk(3, "buffer %d, plane %d successfully mapped\n", buffer, plane);
 	return 0;
 }
 EXPORT_SYMBOL_GPL(vb2_mmap);
@@ -2364,7 +2450,7 @@ unsigned long vb2_get_unmapped_area(struct vb2_queue *q,
 	int ret;
 
 	if (q->memory != V4L2_MEMORY_MMAP) {
-		dprintk(1, "Queue is not currently set up for mmap\n");
+		dprintk(1, "queue is not currently set up for mmap\n");
 		return -EINVAL;
 	}
 
@@ -2429,7 +2515,7 @@ unsigned int vb2_poll(struct vb2_queue *q, struct file *file, poll_table *wait)
 	/*
 	 * Start file I/O emulator only if streaming API has not been used yet.
 	 */
-	if (q->num_buffers == 0 && q->fileio == NULL) {
+	if (q->num_buffers == 0 && !vb2_fileio_is_active(q)) {
 		if (!V4L2_TYPE_IS_OUTPUT(q->type) && (q->io_modes & VB2_READ) &&
 				(req_events & (POLLIN | POLLRDNORM))) {
 			if (__vb2_init_fileio(q, 1))
@@ -2574,6 +2660,7 @@ struct vb2_fileio_buf {
  */
 struct vb2_fileio_data {
 	struct v4l2_requestbuffers req;
+	struct v4l2_plane p;
 	struct v4l2_buffer b;
 	struct vb2_fileio_buf bufs[VIDEO_MAX_FRAME];
 	unsigned int cur_index;
@@ -2634,7 +2721,8 @@ static int __vb2_init_fileio(struct vb2_queue *q, int read)
 	fileio->req.count = count;
 	fileio->req.memory = V4L2_MEMORY_MMAP;
 	fileio->req.type = q->type;
-	ret = vb2_reqbufs(q, &fileio->req);
+	q->fileio = fileio;
+	ret = __reqbufs(q, &fileio->req);
 	if (ret)
 		goto err_kfree;
 
@@ -2663,16 +2751,24 @@ static int __vb2_init_fileio(struct vb2_queue *q, int read)
 	 * Read mode requires pre queuing of all buffers.
 	 */
 	if (read) {
+		bool is_multiplanar = V4L2_TYPE_IS_MULTIPLANAR(q->type);
+
 		/*
 		 * Queue all buffers.
 		 */
 		for (i = 0; i < q->num_buffers; i++) {
 			struct v4l2_buffer *b = &fileio->b;
+
 			memset(b, 0, sizeof(*b));
 			b->type = q->type;
+			if (is_multiplanar) {
+				memset(&fileio->p, 0, sizeof(fileio->p));
+				b->m.planes = &fileio->p;
+				b->length = 1;
+			}
 			b->memory = q->memory;
 			b->index = i;
-			ret = vb2_qbuf(q, b);
+			ret = vb2_internal_qbuf(q, b);
 			if (ret)
 				goto err_reqbufs;
 			fileio->bufs[i].queued = 1;
@@ -2688,19 +2784,18 @@ static int __vb2_init_fileio(struct vb2_queue *q, int read)
 	/*
 	 * Start streaming.
 	 */
-	ret = vb2_streamon(q, q->type);
+	ret = vb2_internal_streamon(q, q->type);
 	if (ret)
 		goto err_reqbufs;
 
-	q->fileio = fileio;
-
 	return ret;
 
 err_reqbufs:
 	fileio->req.count = 0;
-	vb2_reqbufs(q, &fileio->req);
+	__reqbufs(q, &fileio->req);
 
 err_kfree:
+	q->fileio = NULL;
 	kfree(fileio);
 	return ret;
 }
@@ -2738,9 +2833,18 @@ static size_t __vb2_perform_fileio(struct vb2_queue *q, char __user *data, size_
 {
 	struct vb2_fileio_data *fileio;
 	struct vb2_fileio_buf *buf;
+	bool is_multiplanar = V4L2_TYPE_IS_MULTIPLANAR(q->type);
+	/*
+	 * When using write() to write data to an output video node the vb2 core
+	 * should set timestamps if V4L2_BUF_FLAG_TIMESTAMP_COPY is set. Nobody
+	 * else is able to provide this information with the write() operation.
+	 */
+	bool set_timestamp = !read &&
+		(q->timestamp_flags & V4L2_BUF_FLAG_TIMESTAMP_MASK) ==
+		V4L2_BUF_FLAG_TIMESTAMP_COPY;
 	int ret, index;
 
-	dprintk(3, "file io: mode %s, offset %ld, count %zd, %sblocking\n",
+	dprintk(3, "mode %s, offset %ld, count %zd, %sblocking\n",
 		read ? "read" : "write", (long)*ppos, count,
 		nonblock ? "non" : "");
 
@@ -2750,9 +2854,9 @@ static size_t __vb2_perform_fileio(struct vb2_queue *q, char __user *data, size_
 	/*
 	 * Initialize emulator on first call.
 	 */
-	if (!q->fileio) {
+	if (!vb2_fileio_is_active(q)) {
 		ret = __vb2_init_fileio(q, read);
-		dprintk(3, "file io: vb2_init_fileio result: %d\n", ret);
+		dprintk(3, "vb2_init_fileio result: %d\n", ret);
 		if (ret)
 			return ret;
 	}
@@ -2769,8 +2873,13 @@ static size_t __vb2_perform_fileio(struct vb2_queue *q, char __user *data, size_
 		memset(&fileio->b, 0, sizeof(fileio->b));
 		fileio->b.type = q->type;
 		fileio->b.memory = q->memory;
+		if (is_multiplanar) {
+			memset(&fileio->p, 0, sizeof(fileio->p));
+			fileio->b.m.planes = &fileio->p;
+			fileio->b.length = 1;
+		}
 		ret = vb2_internal_dqbuf(q, &fileio->b, nonblock);
-		dprintk(5, "file io: vb2_dqbuf result: %d\n", ret);
+		dprintk(5, "vb2_dqbuf result: %d\n", ret);
 		if (ret)
 			return ret;
 		fileio->dq_count += 1;
@@ -2800,14 +2909,14 @@ static size_t __vb2_perform_fileio(struct vb2_queue *q, char __user *data, size_
 	/*
 	 * Transfer data to userspace.
 	 */
-	dprintk(3, "file io: copying %zd bytes - buffer %d, offset %u\n",
+	dprintk(3, "copying %zd bytes - buffer %d, offset %u\n",
 		count, index, buf->pos);
 	if (read)
 		ret = copy_to_user(data, buf->vaddr + buf->pos, count);
 	else
 		ret = copy_from_user(buf->vaddr + buf->pos, data, count);
 	if (ret) {
-		dprintk(3, "file io: error copying data\n");
+		dprintk(3, "error copying data\n");
 		return -EFAULT;
 	}
 
@@ -2827,7 +2936,7 @@ static size_t __vb2_perform_fileio(struct vb2_queue *q, char __user *data, size_
 		 */
 		if (read && (fileio->flags & VB2_FILEIO_READ_ONCE) &&
 		    fileio->dq_count == 1) {
-			dprintk(3, "file io: read limit reached\n");
+			dprintk(3, "read limit reached\n");
 			return __vb2_cleanup_fileio(q);
 		}
 
@@ -2839,8 +2948,16 @@ static size_t __vb2_perform_fileio(struct vb2_queue *q, char __user *data, size_
 		fileio->b.memory = q->memory;
 		fileio->b.index = index;
 		fileio->b.bytesused = buf->pos;
+		if (is_multiplanar) {
+			memset(&fileio->p, 0, sizeof(fileio->p));
+			fileio->p.bytesused = buf->pos;
+			fileio->b.m.planes = &fileio->p;
+			fileio->b.length = 1;
+		}
+		if (set_timestamp)
+			v4l2_get_timestamp(&fileio->b.timestamp);
 		ret = vb2_internal_qbuf(q, &fileio->b);
-		dprintk(5, "file io: vb2_dbuf result: %d\n", ret);
+		dprintk(5, "vb2_dbuf result: %d\n", ret);
 		if (ret)
 			return ret;
 
@@ -2890,6 +3007,147 @@ size_t vb2_write(struct vb2_queue *q, const char __user *data, size_t count,
 }
 EXPORT_SYMBOL_GPL(vb2_write);
 
+struct vb2_threadio_data {
+	struct task_struct *thread;
+	vb2_thread_fnc fnc;
+	void *priv;
+	bool stop;
+};
+
+static int vb2_thread(void *data)
+{
+	struct vb2_queue *q = data;
+	struct vb2_threadio_data *threadio = q->threadio;
+	struct vb2_fileio_data *fileio = q->fileio;
+	bool set_timestamp = false;
+	int prequeue = 0;
+	int index = 0;
+	int ret = 0;
+
+	if (V4L2_TYPE_IS_OUTPUT(q->type)) {
+		prequeue = q->num_buffers;
+		set_timestamp =
+			(q->timestamp_flags & V4L2_BUF_FLAG_TIMESTAMP_MASK) ==
+			V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	}
+
+	set_freezable();
+
+	for (;;) {
+		struct vb2_buffer *vb;
+
+		/*
+		 * Call vb2_dqbuf to get buffer back.
+		 */
+		memset(&fileio->b, 0, sizeof(fileio->b));
+		fileio->b.type = q->type;
+		fileio->b.memory = q->memory;
+		if (prequeue) {
+			fileio->b.index = index++;
+			prequeue--;
+		} else {
+			call_void_qop(q, wait_finish, q);
+			ret = vb2_internal_dqbuf(q, &fileio->b, 0);
+			call_void_qop(q, wait_prepare, q);
+			dprintk(5, "file io: vb2_dqbuf result: %d\n", ret);
+		}
+		if (threadio->stop)
+			break;
+		if (ret)
+			break;
+		try_to_freeze();
+
+		vb = q->bufs[fileio->b.index];
+		if (!(fileio->b.flags & V4L2_BUF_FLAG_ERROR))
+			ret = threadio->fnc(vb, threadio->priv);
+		if (ret)
+			break;
+		call_void_qop(q, wait_finish, q);
+		if (set_timestamp)
+			v4l2_get_timestamp(&fileio->b.timestamp);
+		ret = vb2_internal_qbuf(q, &fileio->b);
+		call_void_qop(q, wait_prepare, q);
+		if (ret)
+			break;
+	}
+
+	/* Hmm, linux becomes *very* unhappy without this ... */
+	while (!kthread_should_stop()) {
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule();
+	}
+	return 0;
+}
+
+/*
+ * This function should not be used for anything else but the videobuf2-dvb
+ * support. If you think you have another good use-case for this, then please
+ * contact the linux-media mailinglist first.
+ */
+int vb2_thread_start(struct vb2_queue *q, vb2_thread_fnc fnc, void *priv,
+		     const char *thread_name)
+{
+	struct vb2_threadio_data *threadio;
+	int ret = 0;
+
+	if (q->threadio)
+		return -EBUSY;
+	if (vb2_is_busy(q))
+		return -EBUSY;
+	if (WARN_ON(q->fileio))
+		return -EBUSY;
+
+	threadio = kzalloc(sizeof(*threadio), GFP_KERNEL);
+	if (threadio == NULL)
+		return -ENOMEM;
+	threadio->fnc = fnc;
+	threadio->priv = priv;
+
+	ret = __vb2_init_fileio(q, !V4L2_TYPE_IS_OUTPUT(q->type));
+	dprintk(3, "file io: vb2_init_fileio result: %d\n", ret);
+	if (ret)
+		goto nomem;
+	q->threadio = threadio;
+	threadio->thread = kthread_run(vb2_thread, q, "vb2-%s", thread_name);
+	if (IS_ERR(threadio->thread)) {
+		ret = PTR_ERR(threadio->thread);
+		threadio->thread = NULL;
+		goto nothread;
+	}
+	return 0;
+
+nothread:
+	__vb2_cleanup_fileio(q);
+nomem:
+	kfree(threadio);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(vb2_thread_start);
+
+int vb2_thread_stop(struct vb2_queue *q)
+{
+	struct vb2_threadio_data *threadio = q->threadio;
+	struct vb2_fileio_data *fileio = q->fileio;
+	int err;
+
+	if (threadio == NULL)
+		return 0;
+	call_void_qop(q, wait_finish, q);
+	threadio->stop = true;
+	vb2_internal_streamoff(q, q->type);
+	call_void_qop(q, wait_prepare, q);
+	q->fileio = NULL;
+	fileio->req.count = 0;
+	vb2_reqbufs(q, &fileio->req);
+	kfree(fileio);
+	err = kthread_stop(threadio->thread);
+	threadio->thread = NULL;
+	kfree(threadio);
+	q->fileio = NULL;
+	q->threadio = NULL;
+	return err;
+}
+EXPORT_SYMBOL_GPL(vb2_thread_stop);
 
 /*
  * The following functions are not part of the vb2 core API, but are helper
@@ -3116,7 +3374,7 @@ unsigned int vb2_fop_poll(struct file *file, poll_table *wait)
 
 	/* Try to be smart: only lock if polling might start fileio,
 	   otherwise locking will only introduce unwanted delays. */
-	if (q->num_buffers == 0 && q->fileio == NULL) {
+	if (q->num_buffers == 0 && !vb2_fileio_is_active(q)) {
 		if (!V4L2_TYPE_IS_OUTPUT(q->type) && (q->io_modes & VB2_READ) &&
 				(req_events & (POLLIN | POLLRDNORM)))
 			must_lock = true;
diff --git a/drivers/media/v4l2-core/videobuf2-dma-sg.c b/drivers/media/v4l2-core/videobuf2-dma-sg.c
index c779f210d2c..adefc31bb85 100644
--- a/drivers/media/v4l2-core/videobuf2-dma-sg.c
+++ b/drivers/media/v4l2-core/videobuf2-dma-sg.c
@@ -211,7 +211,7 @@ static void *vb2_dma_sg_get_userptr(void *alloc_ctx, unsigned long vaddr,
 		     ++num_pages_from_user, vaddr += PAGE_SIZE) {
 			unsigned long pfn;
 
-			if (follow_pfn(buf->vma, vaddr, &pfn)) {
+			if (follow_pfn(vma, vaddr, &pfn)) {
 				dprintk(1, "no page for address %lu\n", vaddr);
 				break;
 			}
diff --git a/drivers/media/v4l2-core/videobuf2-dvb.c b/drivers/media/v4l2-core/videobuf2-dvb.c
new file mode 100644
index 00000000000..d09269846b7
--- /dev/null
+++ b/drivers/media/v4l2-core/videobuf2-dvb.c
@@ -0,0 +1,336 @@
+/*
+ *
+ * some helper function for simple DVB cards which simply DMA the
+ * complete transport stream and let the computer sort everything else
+ * (i.e. we are using the software demux, ...).  Also uses the
+ * video-buf to manage DMA buffers.
+ *
+ * (c) 2004 Gerd Knorr <kraxel@bytesex.org> [SUSE Labs]
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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/init.h>
+#include <linux/device.h>
+#include <linux/slab.h>
+
+#include <media/videobuf2-dvb.h>
+
+/* ------------------------------------------------------------------ */
+
+MODULE_AUTHOR("Gerd Knorr <kraxel@bytesex.org> [SuSE Labs]");
+MODULE_LICENSE("GPL");
+
+/* ------------------------------------------------------------------ */
+
+static int dvb_fnc(struct vb2_buffer *vb, void *priv)
+{
+	struct vb2_dvb *dvb = priv;
+
+	dvb_dmx_swfilter(&dvb->demux, vb2_plane_vaddr(vb, 0),
+				      vb2_get_plane_payload(vb, 0));
+	return 0;
+}
+
+static int vb2_dvb_start_feed(struct dvb_demux_feed *feed)
+{
+	struct dvb_demux *demux = feed->demux;
+	struct vb2_dvb *dvb = demux->priv;
+	int rc = 0;
+
+	if (!demux->dmx.frontend)
+		return -EINVAL;
+
+	mutex_lock(&dvb->lock);
+	dvb->nfeeds++;
+
+	if (!dvb->dvbq.threadio) {
+		rc = vb2_thread_start(&dvb->dvbq, dvb_fnc, dvb, dvb->name);
+		if (rc)
+			dvb->nfeeds--;
+	}
+	if (!rc)
+		rc = dvb->nfeeds;
+	mutex_unlock(&dvb->lock);
+	return rc;
+}
+
+static int vb2_dvb_stop_feed(struct dvb_demux_feed *feed)
+{
+	struct dvb_demux *demux = feed->demux;
+	struct vb2_dvb *dvb = demux->priv;
+	int err = 0;
+
+	mutex_lock(&dvb->lock);
+	dvb->nfeeds--;
+	if (0 == dvb->nfeeds)
+		err = vb2_thread_stop(&dvb->dvbq);
+	mutex_unlock(&dvb->lock);
+	return err;
+}
+
+static int vb2_dvb_register_adapter(struct vb2_dvb_frontends *fe,
+			  struct module *module,
+			  void *adapter_priv,
+			  struct device *device,
+			  char *adapter_name,
+			  short *adapter_nr,
+			  int mfe_shared)
+{
+	int result;
+
+	mutex_init(&fe->lock);
+
+	/* register adapter */
+	result = dvb_register_adapter(&fe->adapter, adapter_name, module,
+		device, adapter_nr);
+	if (result < 0) {
+		pr_warn("%s: dvb_register_adapter failed (errno = %d)\n",
+		       adapter_name, result);
+	}
+	fe->adapter.priv = adapter_priv;
+	fe->adapter.mfe_shared = mfe_shared;
+
+	return result;
+}
+
+static int vb2_dvb_register_frontend(struct dvb_adapter *adapter,
+	struct vb2_dvb *dvb)
+{
+	int result;
+
+	/* register frontend */
+	result = dvb_register_frontend(adapter, dvb->frontend);
+	if (result < 0) {
+		pr_warn("%s: dvb_register_frontend failed (errno = %d)\n",
+		       dvb->name, result);
+		goto fail_frontend;
+	}
+
+	/* register demux stuff */
+	dvb->demux.dmx.capabilities =
+		DMX_TS_FILTERING | DMX_SECTION_FILTERING |
+		DMX_MEMORY_BASED_FILTERING;
+	dvb->demux.priv       = dvb;
+	dvb->demux.filternum  = 256;
+	dvb->demux.feednum    = 256;
+	dvb->demux.start_feed = vb2_dvb_start_feed;
+	dvb->demux.stop_feed  = vb2_dvb_stop_feed;
+	result = dvb_dmx_init(&dvb->demux);
+	if (result < 0) {
+		pr_warn("%s: dvb_dmx_init failed (errno = %d)\n",
+		       dvb->name, result);
+		goto fail_dmx;
+	}
+
+	dvb->dmxdev.filternum    = 256;
+	dvb->dmxdev.demux        = &dvb->demux.dmx;
+	dvb->dmxdev.capabilities = 0;
+	result = dvb_dmxdev_init(&dvb->dmxdev, adapter);
+
+	if (result < 0) {
+		pr_warn("%s: dvb_dmxdev_init failed (errno = %d)\n",
+		       dvb->name, 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) {
+		pr_warn("%s: add_frontend failed (DMX_FRONTEND_0, errno = %d)\n",
+		       dvb->name, 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) {
+		pr_warn("%s: add_frontend failed (DMX_MEMORY_FE, errno = %d)\n",
+		       dvb->name, result);
+		goto fail_fe_mem;
+	}
+
+	result = dvb->demux.dmx.connect_frontend(&dvb->demux.dmx, &dvb->fe_hw);
+	if (result < 0) {
+		pr_warn("%s: connect_frontend failed (errno = %d)\n",
+		       dvb->name, result);
+		goto fail_fe_conn;
+	}
+
+	/* register network adapter */
+	result = dvb_net_init(adapter, &dvb->net, &dvb->demux.dmx);
+	if (result < 0) {
+		pr_warn("%s: dvb_net_init failed (errno = %d)\n",
+		       dvb->name, result);
+		goto fail_fe_conn;
+	}
+	return 0;
+
+fail_fe_conn:
+	dvb->demux.dmx.remove_frontend(&dvb->demux.dmx, &dvb->fe_mem);
+fail_fe_mem:
+	dvb->demux.dmx.remove_frontend(&dvb->demux.dmx, &dvb->fe_hw);
+fail_fe_hw:
+	dvb_dmxdev_release(&dvb->dmxdev);
+fail_dmxdev:
+	dvb_dmx_release(&dvb->demux);
+fail_dmx:
+	dvb_unregister_frontend(dvb->frontend);
+fail_frontend:
+	dvb_frontend_detach(dvb->frontend);
+	dvb->frontend = NULL;
+
+	return result;
+}
+
+/* ------------------------------------------------------------------ */
+/* Register a single adapter and one or more frontends */
+int vb2_dvb_register_bus(struct vb2_dvb_frontends *f,
+			 struct module *module,
+			 void *adapter_priv,
+			 struct device *device,
+			 short *adapter_nr,
+			 int mfe_shared)
+{
+	struct list_head *list, *q;
+	struct vb2_dvb_frontend *fe;
+	int res;
+
+	fe = vb2_dvb_get_frontend(f, 1);
+	if (!fe) {
+		pr_warn("Unable to register the adapter which has no frontends\n");
+		return -EINVAL;
+	}
+
+	/* Bring up the adapter */
+	res = vb2_dvb_register_adapter(f, module, adapter_priv, device,
+		fe->dvb.name, adapter_nr, mfe_shared);
+	if (res < 0) {
+		pr_warn("vb2_dvb_register_adapter failed (errno = %d)\n", res);
+		return res;
+	}
+
+	/* Attach all of the frontends to the adapter */
+	mutex_lock(&f->lock);
+	list_for_each_safe(list, q, &f->felist) {
+		fe = list_entry(list, struct vb2_dvb_frontend, felist);
+		res = vb2_dvb_register_frontend(&f->adapter, &fe->dvb);
+		if (res < 0) {
+			pr_warn("%s: vb2_dvb_register_frontend failed (errno = %d)\n",
+				fe->dvb.name, res);
+			goto err;
+		}
+	}
+	mutex_unlock(&f->lock);
+	return 0;
+
+err:
+	mutex_unlock(&f->lock);
+	vb2_dvb_unregister_bus(f);
+	return res;
+}
+EXPORT_SYMBOL(vb2_dvb_register_bus);
+
+void vb2_dvb_unregister_bus(struct vb2_dvb_frontends *f)
+{
+	vb2_dvb_dealloc_frontends(f);
+
+	dvb_unregister_adapter(&f->adapter);
+}
+EXPORT_SYMBOL(vb2_dvb_unregister_bus);
+
+struct vb2_dvb_frontend *vb2_dvb_get_frontend(
+	struct vb2_dvb_frontends *f, int id)
+{
+	struct list_head *list, *q;
+	struct vb2_dvb_frontend *fe, *ret = NULL;
+
+	mutex_lock(&f->lock);
+
+	list_for_each_safe(list, q, &f->felist) {
+		fe = list_entry(list, struct vb2_dvb_frontend, felist);
+		if (fe->id == id) {
+			ret = fe;
+			break;
+		}
+	}
+
+	mutex_unlock(&f->lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(vb2_dvb_get_frontend);
+
+int vb2_dvb_find_frontend(struct vb2_dvb_frontends *f,
+	struct dvb_frontend *p)
+{
+	struct list_head *list, *q;
+	struct vb2_dvb_frontend *fe = NULL;
+	int ret = 0;
+
+	mutex_lock(&f->lock);
+
+	list_for_each_safe(list, q, &f->felist) {
+		fe = list_entry(list, struct vb2_dvb_frontend, felist);
+		if (fe->dvb.frontend == p) {
+			ret = fe->id;
+			break;
+		}
+	}
+
+	mutex_unlock(&f->lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(vb2_dvb_find_frontend);
+
+struct vb2_dvb_frontend *vb2_dvb_alloc_frontend(
+	struct vb2_dvb_frontends *f, int id)
+{
+	struct vb2_dvb_frontend *fe;
+
+	fe = kzalloc(sizeof(struct vb2_dvb_frontend), GFP_KERNEL);
+	if (fe == NULL)
+		return NULL;
+
+	fe->id = id;
+	mutex_init(&fe->dvb.lock);
+
+	mutex_lock(&f->lock);
+	list_add_tail(&fe->felist, &f->felist);
+	mutex_unlock(&f->lock);
+	return fe;
+}
+EXPORT_SYMBOL(vb2_dvb_alloc_frontend);
+
+void vb2_dvb_dealloc_frontends(struct vb2_dvb_frontends *f)
+{
+	struct list_head *list, *q;
+	struct vb2_dvb_frontend *fe;
+
+	mutex_lock(&f->lock);
+	list_for_each_safe(list, q, &f->felist) {
+		fe = list_entry(list, struct vb2_dvb_frontend, felist);
+		if (fe->dvb.net.dvbdev) {
+			dvb_net_release(&fe->dvb.net);
+			fe->dvb.demux.dmx.remove_frontend(&fe->dvb.demux.dmx,
+				&fe->dvb.fe_mem);
+			fe->dvb.demux.dmx.remove_frontend(&fe->dvb.demux.dmx,
+				&fe->dvb.fe_hw);
+			dvb_dmxdev_release(&fe->dvb.dmxdev);
+			dvb_dmx_release(&fe->dvb.demux);
+			dvb_unregister_frontend(fe->dvb.frontend);
+		}
+		if (fe->dvb.frontend)
+			/* always allocated, may have been reset */
+			dvb_frontend_detach(fe->dvb.frontend);
+		list_del(list); /* remove list entry */
+		kfree(fe);	/* free frontend allocation */
+	}
+	mutex_unlock(&f->lock);
+}
+EXPORT_SYMBOL(vb2_dvb_dealloc_frontends);
diff --git a/drivers/memory/mvebu-devbus.c b/drivers/memory/mvebu-devbus.c
index 110c0362705..ff7138fd66d 100644
--- a/drivers/memory/mvebu-devbus.c
+++ b/drivers/memory/mvebu-devbus.c
@@ -2,7 +2,7 @@
  * Marvell EBU SoC Device Bus Controller
  * (memory controller for NOR/NAND/SRAM/FPGA devices)
  *
- * Copyright (C) 2013 Marvell
+ * Copyright (C) 2013-2014 Marvell
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -30,19 +30,47 @@
 #include <linux/platform_device.h>
 
 /* Register definitions */
-#define DEV_WIDTH_BIT		30
-#define BADR_SKEW_BIT		28
-#define RD_HOLD_BIT		23
-#define ACC_NEXT_BIT		17
-#define RD_SETUP_BIT		12
-#define ACC_FIRST_BIT		6
-
-#define SYNC_ENABLE_BIT		24
-#define WR_HIGH_BIT		16
-#define WR_LOW_BIT		8
-
-#define READ_PARAM_OFFSET	0x0
-#define WRITE_PARAM_OFFSET	0x4
+#define ARMADA_DEV_WIDTH_SHIFT		30
+#define ARMADA_BADR_SKEW_SHIFT		28
+#define ARMADA_RD_HOLD_SHIFT		23
+#define ARMADA_ACC_NEXT_SHIFT		17
+#define ARMADA_RD_SETUP_SHIFT		12
+#define ARMADA_ACC_FIRST_SHIFT		6
+
+#define ARMADA_SYNC_ENABLE_SHIFT	24
+#define ARMADA_WR_HIGH_SHIFT		16
+#define ARMADA_WR_LOW_SHIFT		8
+
+#define ARMADA_READ_PARAM_OFFSET	0x0
+#define ARMADA_WRITE_PARAM_OFFSET	0x4
+
+#define ORION_RESERVED			(0x2 << 30)
+#define ORION_BADR_SKEW_SHIFT		28
+#define ORION_WR_HIGH_EXT_BIT		BIT(27)
+#define ORION_WR_HIGH_EXT_MASK		0x8
+#define ORION_WR_LOW_EXT_BIT		BIT(26)
+#define ORION_WR_LOW_EXT_MASK		0x8
+#define ORION_ALE_WR_EXT_BIT		BIT(25)
+#define ORION_ALE_WR_EXT_MASK		0x8
+#define ORION_ACC_NEXT_EXT_BIT		BIT(24)
+#define ORION_ACC_NEXT_EXT_MASK		0x10
+#define ORION_ACC_FIRST_EXT_BIT		BIT(23)
+#define ORION_ACC_FIRST_EXT_MASK	0x10
+#define ORION_TURN_OFF_EXT_BIT		BIT(22)
+#define ORION_TURN_OFF_EXT_MASK		0x8
+#define ORION_DEV_WIDTH_SHIFT		20
+#define ORION_WR_HIGH_SHIFT		17
+#define ORION_WR_HIGH_MASK		0x7
+#define ORION_WR_LOW_SHIFT		14
+#define ORION_WR_LOW_MASK		0x7
+#define ORION_ALE_WR_SHIFT		11
+#define ORION_ALE_WR_MASK		0x7
+#define ORION_ACC_NEXT_SHIFT		7
+#define ORION_ACC_NEXT_MASK		0xF
+#define ORION_ACC_FIRST_SHIFT		3
+#define ORION_ACC_FIRST_MASK		0xF
+#define ORION_TURN_OFF_SHIFT		0
+#define ORION_TURN_OFF_MASK		0x7
 
 struct devbus_read_params {
 	u32 bus_width;
@@ -89,117 +117,167 @@ static int get_timing_param_ps(struct devbus *devbus,
 	return 0;
 }
 
-static int devbus_set_timing_params(struct devbus *devbus,
-				    struct device_node *node)
+static int devbus_get_timing_params(struct devbus *devbus,
+				    struct device_node *node,
+				    struct devbus_read_params *r,
+				    struct devbus_write_params *w)
 {
-	struct devbus_read_params r;
-	struct devbus_write_params w;
-	u32 value;
 	int err;
 
-	dev_dbg(devbus->dev, "Setting timing parameter, tick is %lu ps\n",
-		devbus->tick_ps);
-
-	/* Get read timings */
-	err = of_property_read_u32(node, "devbus,bus-width", &r.bus_width);
+	err = of_property_read_u32(node, "devbus,bus-width", &r->bus_width);
 	if (err < 0) {
 		dev_err(devbus->dev,
 			"%s has no 'devbus,bus-width' property\n",
 			node->full_name);
 		return err;
 	}
-	/* Convert bit width to byte width */
-	r.bus_width /= 8;
+
+	/*
+	 * The bus width is encoded into the register as 0 for 8 bits,
+	 * and 1 for 16 bits, so we do the necessary conversion here.
+	 */
+	if (r->bus_width == 8)
+		r->bus_width = 0;
+	else if (r->bus_width == 16)
+		r->bus_width = 1;
+	else {
+		dev_err(devbus->dev, "invalid bus width %d\n", r->bus_width);
+		return -EINVAL;
+	}
 
 	err = get_timing_param_ps(devbus, node, "devbus,badr-skew-ps",
-				 &r.badr_skew);
+				 &r->badr_skew);
 	if (err < 0)
 		return err;
 
 	err = get_timing_param_ps(devbus, node, "devbus,turn-off-ps",
-				 &r.turn_off);
+				 &r->turn_off);
 	if (err < 0)
 		return err;
 
 	err = get_timing_param_ps(devbus, node, "devbus,acc-first-ps",
-				 &r.acc_first);
+				 &r->acc_first);
 	if (err < 0)
 		return err;
 
 	err = get_timing_param_ps(devbus, node, "devbus,acc-next-ps",
-				 &r.acc_next);
+				 &r->acc_next);
 	if (err < 0)
 		return err;
 
-	err = get_timing_param_ps(devbus, node, "devbus,rd-setup-ps",
-				 &r.rd_setup);
-	if (err < 0)
-		return err;
-
-	err = get_timing_param_ps(devbus, node, "devbus,rd-hold-ps",
-				 &r.rd_hold);
-	if (err < 0)
-		return err;
-
-	/* Get write timings */
-	err = of_property_read_u32(node, "devbus,sync-enable",
-				  &w.sync_enable);
-	if (err < 0) {
-		dev_err(devbus->dev,
-			"%s has no 'devbus,sync-enable' property\n",
-			node->full_name);
-		return err;
+	if (of_device_is_compatible(devbus->dev->of_node, "marvell,mvebu-devbus")) {
+		err = get_timing_param_ps(devbus, node, "devbus,rd-setup-ps",
+					  &r->rd_setup);
+		if (err < 0)
+			return err;
+
+		err = get_timing_param_ps(devbus, node, "devbus,rd-hold-ps",
+					  &r->rd_hold);
+		if (err < 0)
+			return err;
+
+		err = of_property_read_u32(node, "devbus,sync-enable",
+					   &w->sync_enable);
+		if (err < 0) {
+			dev_err(devbus->dev,
+				"%s has no 'devbus,sync-enable' property\n",
+				node->full_name);
+			return err;
+		}
 	}
 
 	err = get_timing_param_ps(devbus, node, "devbus,ale-wr-ps",
-				 &w.ale_wr);
+				 &w->ale_wr);
 	if (err < 0)
 		return err;
 
 	err = get_timing_param_ps(devbus, node, "devbus,wr-low-ps",
-				 &w.wr_low);
+				 &w->wr_low);
 	if (err < 0)
 		return err;
 
 	err = get_timing_param_ps(devbus, node, "devbus,wr-high-ps",
-				 &w.wr_high);
+				 &w->wr_high);
 	if (err < 0)
 		return err;
 
+	return 0;
+}
+
+static void devbus_orion_set_timing_params(struct devbus *devbus,
+					  struct device_node *node,
+					  struct devbus_read_params *r,
+					  struct devbus_write_params *w)
+{
+	u32 value;
+
+	/*
+	 * The hardware designers found it would be a good idea to
+	 * split most of the values in the register into two fields:
+	 * one containing all the low-order bits, and another one
+	 * containing just the high-order bit. For all of those
+	 * fields, we have to split the value into these two parts.
+	 */
+	value =	(r->turn_off   & ORION_TURN_OFF_MASK)  << ORION_TURN_OFF_SHIFT  |
+		(r->acc_first  & ORION_ACC_FIRST_MASK) << ORION_ACC_FIRST_SHIFT |
+		(r->acc_next   & ORION_ACC_NEXT_MASK)  << ORION_ACC_NEXT_SHIFT  |
+		(w->ale_wr     & ORION_ALE_WR_MASK)    << ORION_ALE_WR_SHIFT    |
+		(w->wr_low     & ORION_WR_LOW_MASK)    << ORION_WR_LOW_SHIFT    |
+		(w->wr_high    & ORION_WR_HIGH_MASK)   << ORION_WR_HIGH_SHIFT   |
+		r->bus_width                           << ORION_DEV_WIDTH_SHIFT |
+		((r->turn_off  & ORION_TURN_OFF_EXT_MASK)  ? ORION_TURN_OFF_EXT_BIT  : 0) |
+		((r->acc_first & ORION_ACC_FIRST_EXT_MASK) ? ORION_ACC_FIRST_EXT_BIT : 0) |
+		((r->acc_next  & ORION_ACC_NEXT_EXT_MASK)  ? ORION_ACC_NEXT_EXT_BIT  : 0) |
+		((w->ale_wr    & ORION_ALE_WR_EXT_MASK)    ? ORION_ALE_WR_EXT_BIT    : 0) |
+		((w->wr_low    & ORION_WR_LOW_EXT_MASK)    ? ORION_WR_LOW_EXT_BIT    : 0) |
+		((w->wr_high   & ORION_WR_HIGH_EXT_MASK)   ? ORION_WR_HIGH_EXT_BIT   : 0) |
+		(r->badr_skew << ORION_BADR_SKEW_SHIFT) |
+		ORION_RESERVED;
+
+	writel(value, devbus->base);
+}
+
+static void devbus_armada_set_timing_params(struct devbus *devbus,
+					   struct device_node *node,
+					   struct devbus_read_params *r,
+					   struct devbus_write_params *w)
+{
+	u32 value;
+
 	/* Set read timings */
-	value = r.bus_width << DEV_WIDTH_BIT |
-		r.badr_skew << BADR_SKEW_BIT |
-		r.rd_hold   << RD_HOLD_BIT   |
-		r.acc_next  << ACC_NEXT_BIT  |
-		r.rd_setup  << RD_SETUP_BIT  |
-		r.acc_first << ACC_FIRST_BIT |
-		r.turn_off;
+	value = r->bus_width << ARMADA_DEV_WIDTH_SHIFT |
+		r->badr_skew << ARMADA_BADR_SKEW_SHIFT |
+		r->rd_hold   << ARMADA_RD_HOLD_SHIFT   |
+		r->acc_next  << ARMADA_ACC_NEXT_SHIFT  |
+		r->rd_setup  << ARMADA_RD_SETUP_SHIFT  |
+		r->acc_first << ARMADA_ACC_FIRST_SHIFT |
+		r->turn_off;
 
 	dev_dbg(devbus->dev, "read parameters register 0x%p = 0x%x\n",
-		devbus->base + READ_PARAM_OFFSET,
+		devbus->base + ARMADA_READ_PARAM_OFFSET,
 		value);
 
-	writel(value, devbus->base + READ_PARAM_OFFSET);
+	writel(value, devbus->base + ARMADA_READ_PARAM_OFFSET);
 
 	/* Set write timings */
-	value = w.sync_enable  << SYNC_ENABLE_BIT |
-		w.wr_low       << WR_LOW_BIT      |
-		w.wr_high      << WR_HIGH_BIT     |
-		w.ale_wr;
+	value = w->sync_enable  << ARMADA_SYNC_ENABLE_SHIFT |
+		w->wr_low       << ARMADA_WR_LOW_SHIFT      |
+		w->wr_high      << ARMADA_WR_HIGH_SHIFT     |
+		w->ale_wr;
 
 	dev_dbg(devbus->dev, "write parameters register: 0x%p = 0x%x\n",
-		devbus->base + WRITE_PARAM_OFFSET,
+		devbus->base + ARMADA_WRITE_PARAM_OFFSET,
 		value);
 
-	writel(value, devbus->base + WRITE_PARAM_OFFSET);
-
-	return 0;
+	writel(value, devbus->base + ARMADA_WRITE_PARAM_OFFSET);
 }
 
 static int mvebu_devbus_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
 	struct device_node *node = pdev->dev.of_node;
+	struct devbus_read_params r;
+	struct devbus_write_params w;
 	struct devbus *devbus;
 	struct resource *res;
 	struct clk *clk;
@@ -229,10 +307,21 @@ static int mvebu_devbus_probe(struct platform_device *pdev)
 	rate = clk_get_rate(clk) / 1000;
 	devbus->tick_ps = 1000000000 / rate;
 
-	/* Read the device tree node and set the new timing parameters */
-	err = devbus_set_timing_params(devbus, node);
-	if (err < 0)
-		return err;
+	dev_dbg(devbus->dev, "Setting timing parameter, tick is %lu ps\n",
+		devbus->tick_ps);
+
+	if (!of_property_read_bool(node, "devbus,keep-config")) {
+		/* Read the Device Tree node */
+		err = devbus_get_timing_params(devbus, node, &r, &w);
+		if (err < 0)
+			return err;
+
+		/* Set the new timing parameters */
+		if (of_device_is_compatible(node, "marvell,orion-devbus"))
+			devbus_orion_set_timing_params(devbus, node, &r, &w);
+		else
+			devbus_armada_set_timing_params(devbus, node, &r, &w);
+	}
 
 	/*
 	 * We need to create a child device explicitly from here to
@@ -248,6 +337,7 @@ static int mvebu_devbus_probe(struct platform_device *pdev)
 
 static const struct of_device_id mvebu_devbus_of_match[] = {
 	{ .compatible = "marvell,mvebu-devbus" },
+	{ .compatible = "marvell,orion-devbus" },
 	{},
 };
 MODULE_DEVICE_TABLE(of, mvebu_devbus_of_match);
diff --git a/drivers/memstick/host/Kconfig b/drivers/memstick/host/Kconfig
index 1b37cf8cd20..7310e32b599 100644
--- a/drivers/memstick/host/Kconfig
+++ b/drivers/memstick/host/Kconfig
@@ -52,3 +52,13 @@ config MEMSTICK_REALTEK_PCI
 
 	  To compile this driver as a module, choose M here: the module will
 	  be called rtsx_pci_ms.
+
+config MEMSTICK_REALTEK_USB
+	tristate "Realtek USB Memstick Card Interface Driver"
+	depends on MFD_RTSX_USB
+	help
+	  Say Y here to include driver code to support Memstick card interface
+	  of Realtek RTS5129/39 series USB card reader
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called rts5139_ms.
diff --git a/drivers/memstick/host/Makefile b/drivers/memstick/host/Makefile
index af3459d7686..491c9557441 100644
--- a/drivers/memstick/host/Makefile
+++ b/drivers/memstick/host/Makefile
@@ -6,3 +6,4 @@ obj-$(CONFIG_MEMSTICK_TIFM_MS)		+= tifm_ms.o
 obj-$(CONFIG_MEMSTICK_JMICRON_38X)	+= jmb38x_ms.o
 obj-$(CONFIG_MEMSTICK_R592)		+= r592.o
 obj-$(CONFIG_MEMSTICK_REALTEK_PCI)	+= rtsx_pci_ms.o
+obj-$(CONFIG_MEMSTICK_REALTEK_USB)	+= rtsx_usb_ms.o
diff --git a/drivers/memstick/host/rtsx_usb_ms.c b/drivers/memstick/host/rtsx_usb_ms.c
new file mode 100644
index 00000000000..a7282b7d4de
--- /dev/null
+++ b/drivers/memstick/host/rtsx_usb_ms.c
@@ -0,0 +1,839 @@
+/* Realtek USB Memstick Card Interface driver
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2
+ * as 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/>.
+ *
+ * Author:
+ *   Roger Tseng <rogerable@realtek.com>
+ */
+
+#include <linux/module.h>
+#include <linux/highmem.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/workqueue.h>
+#include <linux/memstick.h>
+#include <linux/kthread.h>
+#include <linux/mfd/rtsx_usb.h>
+#include <linux/pm_runtime.h>
+#include <linux/mutex.h>
+#include <linux/sched.h>
+#include <linux/completion.h>
+#include <asm/unaligned.h>
+
+struct rtsx_usb_ms {
+	struct platform_device	*pdev;
+	struct rtsx_ucr	*ucr;
+	struct memstick_host	*msh;
+	struct memstick_request	*req;
+
+	struct mutex		host_mutex;
+	struct work_struct	handle_req;
+
+	struct task_struct	*detect_ms;
+	struct completion	detect_ms_exit;
+
+	u8			ssc_depth;
+	unsigned int		clock;
+	int			power_mode;
+	unsigned char           ifmode;
+	bool			eject;
+};
+
+static inline struct device *ms_dev(struct rtsx_usb_ms *host)
+{
+	return &(host->pdev->dev);
+}
+
+static inline void ms_clear_error(struct rtsx_usb_ms *host)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	rtsx_usb_ep0_write_register(ucr, CARD_STOP,
+				  MS_STOP | MS_CLR_ERR,
+				  MS_STOP | MS_CLR_ERR);
+
+	rtsx_usb_clear_dma_err(ucr);
+	rtsx_usb_clear_fsm_err(ucr);
+}
+
+#ifdef DEBUG
+
+static void ms_print_debug_regs(struct rtsx_usb_ms *host)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	u16 i;
+	u8 *ptr;
+
+	/* Print MS host internal registers */
+	rtsx_usb_init_cmd(ucr);
+
+	/* MS_CFG to MS_INT_REG */
+	for (i = 0xFD40; i <= 0xFD44; i++)
+		rtsx_usb_add_cmd(ucr, READ_REG_CMD, i, 0, 0);
+
+	/* CARD_SHARE_MODE to CARD_GPIO */
+	for (i = 0xFD51; i <= 0xFD56; i++)
+		rtsx_usb_add_cmd(ucr, READ_REG_CMD, i, 0, 0);
+
+	/* CARD_PULL_CTLx */
+	for (i = 0xFD60; i <= 0xFD65; i++)
+		rtsx_usb_add_cmd(ucr, READ_REG_CMD, i, 0, 0);
+
+	/* CARD_DATA_SOURCE, CARD_SELECT, CARD_CLK_EN, CARD_PWR_CTL */
+	rtsx_usb_add_cmd(ucr, READ_REG_CMD, CARD_DATA_SOURCE, 0, 0);
+	rtsx_usb_add_cmd(ucr, READ_REG_CMD, CARD_SELECT, 0, 0);
+	rtsx_usb_add_cmd(ucr, READ_REG_CMD, CARD_CLK_EN, 0, 0);
+	rtsx_usb_add_cmd(ucr, READ_REG_CMD, CARD_PWR_CTL, 0, 0);
+
+	rtsx_usb_send_cmd(ucr, MODE_CR, 100);
+	rtsx_usb_get_rsp(ucr, 21, 100);
+
+	ptr = ucr->rsp_buf;
+	for (i = 0xFD40; i <= 0xFD44; i++)
+		dev_dbg(ms_dev(host), "0x%04X: 0x%02x\n", i, *(ptr++));
+	for (i = 0xFD51; i <= 0xFD56; i++)
+		dev_dbg(ms_dev(host), "0x%04X: 0x%02x\n", i, *(ptr++));
+	for (i = 0xFD60; i <= 0xFD65; i++)
+		dev_dbg(ms_dev(host), "0x%04X: 0x%02x\n", i, *(ptr++));
+
+	dev_dbg(ms_dev(host), "0x%04X: 0x%02x\n", CARD_DATA_SOURCE, *(ptr++));
+	dev_dbg(ms_dev(host), "0x%04X: 0x%02x\n", CARD_SELECT, *(ptr++));
+	dev_dbg(ms_dev(host), "0x%04X: 0x%02x\n", CARD_CLK_EN, *(ptr++));
+	dev_dbg(ms_dev(host), "0x%04X: 0x%02x\n", CARD_PWR_CTL, *(ptr++));
+}
+
+#else
+
+static void ms_print_debug_regs(struct rtsx_usb_ms *host)
+{
+}
+
+#endif
+
+static int ms_pull_ctl_disable_lqfp48(struct rtsx_ucr *ucr)
+{
+	rtsx_usb_init_cmd(ucr);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0xA5);
+
+	return rtsx_usb_send_cmd(ucr, MODE_C, 100);
+}
+
+static int ms_pull_ctl_disable_qfn24(struct rtsx_ucr *ucr)
+{
+	rtsx_usb_init_cmd(ucr);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x65);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x56);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0x59);
+
+	return rtsx_usb_send_cmd(ucr, MODE_C, 100);
+}
+
+static int ms_pull_ctl_enable_lqfp48(struct rtsx_ucr *ucr)
+{
+	rtsx_usb_init_cmd(ucr);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0xA5);
+
+	return rtsx_usb_send_cmd(ucr, MODE_C, 100);
+}
+
+static int ms_pull_ctl_enable_qfn24(struct rtsx_ucr *ucr)
+{
+	rtsx_usb_init_cmd(ucr);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x65);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0x59);
+
+	return rtsx_usb_send_cmd(ucr, MODE_C, 100);
+}
+
+static int ms_power_on(struct rtsx_usb_ms *host)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	int err;
+
+	dev_dbg(ms_dev(host), "%s\n", __func__);
+
+	rtsx_usb_init_cmd(ucr);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_SELECT, 0x07, MS_MOD_SEL);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_SHARE_MODE,
+			CARD_SHARE_MASK, CARD_SHARE_MS);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_CLK_EN,
+			MS_CLK_EN, MS_CLK_EN);
+	err = rtsx_usb_send_cmd(ucr, MODE_C, 100);
+	if (err < 0)
+		return err;
+
+	if (CHECK_PKG(ucr, LQFP48))
+		err = ms_pull_ctl_enable_lqfp48(ucr);
+	else
+		err = ms_pull_ctl_enable_qfn24(ucr);
+	if (err < 0)
+		return err;
+
+	err = rtsx_usb_write_register(ucr, CARD_PWR_CTL,
+			POWER_MASK, PARTIAL_POWER_ON);
+	if (err)
+		return err;
+
+	usleep_range(800, 1000);
+
+	rtsx_usb_init_cmd(ucr);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PWR_CTL,
+			POWER_MASK, POWER_ON);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_OE,
+			MS_OUTPUT_EN, MS_OUTPUT_EN);
+
+	return rtsx_usb_send_cmd(ucr, MODE_C, 100);
+}
+
+static int ms_power_off(struct rtsx_usb_ms *host)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	int err;
+
+	dev_dbg(ms_dev(host), "%s\n", __func__);
+
+	rtsx_usb_init_cmd(ucr);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_CLK_EN, MS_CLK_EN, 0);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_OE, MS_OUTPUT_EN, 0);
+
+	err = rtsx_usb_send_cmd(ucr, MODE_C, 100);
+	if (err < 0)
+		return err;
+
+	if (CHECK_PKG(ucr, LQFP48))
+		return ms_pull_ctl_disable_lqfp48(ucr);
+
+	return ms_pull_ctl_disable_qfn24(ucr);
+}
+
+static int ms_transfer_data(struct rtsx_usb_ms *host, unsigned char data_dir,
+		u8 tpc, u8 cfg, struct scatterlist *sg)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	int err;
+	unsigned int length = sg->length;
+	u16 sec_cnt = (u16)(length / 512);
+	u8 trans_mode, dma_dir, flag;
+	unsigned int pipe;
+	struct memstick_dev *card = host->msh->card;
+
+	dev_dbg(ms_dev(host), "%s: tpc = 0x%02x, data_dir = %s, length = %d\n",
+			__func__, tpc, (data_dir == READ) ? "READ" : "WRITE",
+			length);
+
+	if (data_dir == READ) {
+		flag = MODE_CDIR;
+		dma_dir = DMA_DIR_FROM_CARD;
+		if (card->id.type != MEMSTICK_TYPE_PRO)
+			trans_mode = MS_TM_NORMAL_READ;
+		else
+			trans_mode = MS_TM_AUTO_READ;
+		pipe = usb_rcvbulkpipe(ucr->pusb_dev, EP_BULK_IN);
+	} else {
+		flag = MODE_CDOR;
+		dma_dir = DMA_DIR_TO_CARD;
+		if (card->id.type != MEMSTICK_TYPE_PRO)
+			trans_mode = MS_TM_NORMAL_WRITE;
+		else
+			trans_mode = MS_TM_AUTO_WRITE;
+		pipe = usb_sndbulkpipe(ucr->pusb_dev, EP_BULK_OUT);
+	}
+
+	rtsx_usb_init_cmd(ucr);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
+	if (card->id.type == MEMSTICK_TYPE_PRO) {
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_SECTOR_CNT_H,
+				0xFF, (u8)(sec_cnt >> 8));
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_SECTOR_CNT_L,
+				0xFF, (u8)sec_cnt);
+	}
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MC_DMA_TC3,
+			0xFF, (u8)(length >> 24));
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MC_DMA_TC2,
+			0xFF, (u8)(length >> 16));
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MC_DMA_TC1,
+			0xFF, (u8)(length >> 8));
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MC_DMA_TC0, 0xFF,
+			(u8)length);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MC_DMA_CTL,
+			0x03 | DMA_PACK_SIZE_MASK, dma_dir | DMA_EN | DMA_512);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_DATA_SOURCE,
+			0x01, RING_BUFFER);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_TRANSFER,
+			0xFF, MS_TRANSFER_START | trans_mode);
+	rtsx_usb_add_cmd(ucr, CHECK_REG_CMD, MS_TRANSFER,
+			MS_TRANSFER_END, MS_TRANSFER_END);
+
+	err = rtsx_usb_send_cmd(ucr, flag | STAGE_MS_STATUS, 100);
+	if (err)
+		return err;
+
+	err = rtsx_usb_transfer_data(ucr, pipe, sg, length,
+			1, NULL, 10000);
+	if (err)
+		goto err_out;
+
+	err = rtsx_usb_get_rsp(ucr, 3, 15000);
+	if (err)
+		goto err_out;
+
+	if (ucr->rsp_buf[0] & MS_TRANSFER_ERR ||
+	    ucr->rsp_buf[1] & (MS_CRC16_ERR | MS_RDY_TIMEOUT)) {
+		err = -EIO;
+		goto err_out;
+	}
+	return 0;
+err_out:
+	ms_clear_error(host);
+	return err;
+}
+
+static int ms_write_bytes(struct rtsx_usb_ms *host, u8 tpc,
+		u8 cfg, u8 cnt, u8 *data, u8 *int_reg)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	int err, i;
+
+	dev_dbg(ms_dev(host), "%s: tpc = 0x%02x\n", __func__, tpc);
+
+	rtsx_usb_init_cmd(ucr);
+
+	for (i = 0; i < cnt; i++)
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD,
+				PPBUF_BASE2 + i, 0xFF, data[i]);
+
+	if (cnt % 2)
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD,
+				PPBUF_BASE2 + i, 0xFF, 0xFF);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_DATA_SOURCE,
+			0x01, PINGPONG_BUFFER);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_TRANSFER,
+			0xFF, MS_TRANSFER_START | MS_TM_WRITE_BYTES);
+	rtsx_usb_add_cmd(ucr, CHECK_REG_CMD, MS_TRANSFER,
+			MS_TRANSFER_END, MS_TRANSFER_END);
+	rtsx_usb_add_cmd(ucr, READ_REG_CMD, MS_TRANS_CFG, 0, 0);
+
+	err = rtsx_usb_send_cmd(ucr, MODE_CR, 100);
+	if (err)
+		return err;
+
+	err = rtsx_usb_get_rsp(ucr, 2, 5000);
+	if (err || (ucr->rsp_buf[0] & MS_TRANSFER_ERR)) {
+		u8 val;
+
+		rtsx_usb_ep0_read_register(ucr, MS_TRANS_CFG, &val);
+		dev_dbg(ms_dev(host), "MS_TRANS_CFG: 0x%02x\n", val);
+
+		if (int_reg)
+			*int_reg = val & 0x0F;
+
+		ms_print_debug_regs(host);
+
+		ms_clear_error(host);
+
+		if (!(tpc & 0x08)) {
+			if (val & MS_CRC16_ERR)
+				return -EIO;
+		} else {
+			if (!(val & 0x80)) {
+				if (val & (MS_INT_ERR | MS_INT_CMDNK))
+					return -EIO;
+			}
+		}
+
+		return -ETIMEDOUT;
+	}
+
+	if (int_reg)
+		*int_reg = ucr->rsp_buf[1] & 0x0F;
+
+	return 0;
+}
+
+static int ms_read_bytes(struct rtsx_usb_ms *host, u8 tpc,
+		u8 cfg, u8 cnt, u8 *data, u8 *int_reg)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	int err, i;
+	u8 *ptr;
+
+	dev_dbg(ms_dev(host), "%s: tpc = 0x%02x\n", __func__, tpc);
+
+	rtsx_usb_init_cmd(ucr);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_DATA_SOURCE,
+			0x01, PINGPONG_BUFFER);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MS_TRANSFER,
+			0xFF, MS_TRANSFER_START | MS_TM_READ_BYTES);
+	rtsx_usb_add_cmd(ucr, CHECK_REG_CMD, MS_TRANSFER,
+			MS_TRANSFER_END, MS_TRANSFER_END);
+	for (i = 0; i < cnt - 1; i++)
+		rtsx_usb_add_cmd(ucr, READ_REG_CMD, PPBUF_BASE2 + i, 0, 0);
+	if (cnt % 2)
+		rtsx_usb_add_cmd(ucr, READ_REG_CMD, PPBUF_BASE2 + cnt, 0, 0);
+	else
+		rtsx_usb_add_cmd(ucr, READ_REG_CMD,
+				PPBUF_BASE2 + cnt - 1, 0, 0);
+
+	rtsx_usb_add_cmd(ucr, READ_REG_CMD, MS_TRANS_CFG, 0, 0);
+
+	err = rtsx_usb_send_cmd(ucr, MODE_CR, 100);
+	if (err)
+		return err;
+
+	err = rtsx_usb_get_rsp(ucr, cnt + 2, 5000);
+	if (err || (ucr->rsp_buf[0] & MS_TRANSFER_ERR)) {
+		u8 val;
+
+		rtsx_usb_ep0_read_register(ucr, MS_TRANS_CFG, &val);
+		dev_dbg(ms_dev(host), "MS_TRANS_CFG: 0x%02x\n", val);
+
+		if (int_reg && (host->ifmode != MEMSTICK_SERIAL))
+			*int_reg = val & 0x0F;
+
+		ms_print_debug_regs(host);
+
+		ms_clear_error(host);
+
+		if (!(tpc & 0x08)) {
+			if (val & MS_CRC16_ERR)
+				return -EIO;
+		} else {
+			if (!(val & 0x80)) {
+				if (val & (MS_INT_ERR | MS_INT_CMDNK))
+					return -EIO;
+			}
+		}
+
+		return -ETIMEDOUT;
+	}
+
+	ptr = ucr->rsp_buf + 1;
+	for (i = 0; i < cnt; i++)
+		data[i] = *ptr++;
+
+
+	if (int_reg && (host->ifmode != MEMSTICK_SERIAL))
+		*int_reg = *ptr & 0x0F;
+
+	return 0;
+}
+
+static int rtsx_usb_ms_issue_cmd(struct rtsx_usb_ms *host)
+{
+	struct memstick_request *req = host->req;
+	int err = 0;
+	u8 cfg = 0, int_reg;
+
+	dev_dbg(ms_dev(host), "%s\n", __func__);
+
+	if (req->need_card_int) {
+		if (host->ifmode != MEMSTICK_SERIAL)
+			cfg = WAIT_INT;
+	}
+
+	if (req->long_data) {
+		err = ms_transfer_data(host, req->data_dir,
+				req->tpc, cfg, &(req->sg));
+	} else {
+		if (req->data_dir == READ)
+			err = ms_read_bytes(host, req->tpc, cfg,
+					req->data_len, req->data, &int_reg);
+		else
+			err = ms_write_bytes(host, req->tpc, cfg,
+					req->data_len, req->data, &int_reg);
+	}
+	if (err < 0)
+		return err;
+
+	if (req->need_card_int) {
+		if (host->ifmode == MEMSTICK_SERIAL) {
+			err = ms_read_bytes(host, MS_TPC_GET_INT,
+					NO_WAIT_INT, 1, &req->int_reg, NULL);
+			if (err < 0)
+				return err;
+		} else {
+
+			if (int_reg & MS_INT_CMDNK)
+				req->int_reg |= MEMSTICK_INT_CMDNAK;
+			if (int_reg & MS_INT_BREQ)
+				req->int_reg |= MEMSTICK_INT_BREQ;
+			if (int_reg & MS_INT_ERR)
+				req->int_reg |= MEMSTICK_INT_ERR;
+			if (int_reg & MS_INT_CED)
+				req->int_reg |= MEMSTICK_INT_CED;
+		}
+		dev_dbg(ms_dev(host), "int_reg: 0x%02x\n", req->int_reg);
+	}
+
+	return 0;
+}
+
+static void rtsx_usb_ms_handle_req(struct work_struct *work)
+{
+	struct rtsx_usb_ms *host = container_of(work,
+			struct rtsx_usb_ms, handle_req);
+	struct rtsx_ucr *ucr = host->ucr;
+	struct memstick_host *msh = host->msh;
+	int rc;
+
+	if (!host->req) {
+		do {
+			rc = memstick_next_req(msh, &host->req);
+			dev_dbg(ms_dev(host), "next req %d\n", rc);
+
+			if (!rc) {
+				mutex_lock(&ucr->dev_mutex);
+
+				if (rtsx_usb_card_exclusive_check(ucr,
+							RTSX_USB_MS_CARD))
+					host->req->error = -EIO;
+				else
+					host->req->error =
+						rtsx_usb_ms_issue_cmd(host);
+
+				mutex_unlock(&ucr->dev_mutex);
+
+				dev_dbg(ms_dev(host), "req result %d\n",
+						host->req->error);
+			}
+		} while (!rc);
+	}
+
+}
+
+static void rtsx_usb_ms_request(struct memstick_host *msh)
+{
+	struct rtsx_usb_ms *host = memstick_priv(msh);
+
+	dev_dbg(ms_dev(host), "--> %s\n", __func__);
+
+	if (!host->eject)
+		schedule_work(&host->handle_req);
+}
+
+static int rtsx_usb_ms_set_param(struct memstick_host *msh,
+		enum memstick_param param, int value)
+{
+	struct rtsx_usb_ms *host = memstick_priv(msh);
+	struct rtsx_ucr *ucr = host->ucr;
+	unsigned int clock = 0;
+	u8 ssc_depth = 0;
+	int err;
+
+	dev_dbg(ms_dev(host), "%s: param = %d, value = %d\n",
+			__func__, param, value);
+
+	mutex_lock(&ucr->dev_mutex);
+
+	err = rtsx_usb_card_exclusive_check(ucr, RTSX_USB_MS_CARD);
+	if (err)
+		goto out;
+
+	switch (param) {
+	case MEMSTICK_POWER:
+		if (value == host->power_mode)
+			break;
+
+		if (value == MEMSTICK_POWER_ON) {
+			pm_runtime_get_sync(ms_dev(host));
+			err = ms_power_on(host);
+		} else if (value == MEMSTICK_POWER_OFF) {
+			err = ms_power_off(host);
+			if (host->msh->card)
+				pm_runtime_put_noidle(ms_dev(host));
+			else
+				pm_runtime_put(ms_dev(host));
+		} else
+			err = -EINVAL;
+		if (!err)
+			host->power_mode = value;
+		break;
+
+	case MEMSTICK_INTERFACE:
+		if (value == MEMSTICK_SERIAL) {
+			clock = 19000000;
+			ssc_depth = SSC_DEPTH_512K;
+			err = rtsx_usb_write_register(ucr, MS_CFG, 0x5A,
+				       MS_BUS_WIDTH_1 | PUSH_TIME_DEFAULT);
+			if (err < 0)
+				break;
+		} else if (value == MEMSTICK_PAR4) {
+			clock = 39000000;
+			ssc_depth = SSC_DEPTH_1M;
+
+			err = rtsx_usb_write_register(ucr, MS_CFG, 0x5A,
+					MS_BUS_WIDTH_4 | PUSH_TIME_ODD |
+					MS_NO_CHECK_INT);
+			if (err < 0)
+				break;
+		} else {
+			err = -EINVAL;
+			break;
+		}
+
+		err = rtsx_usb_switch_clock(ucr, clock,
+				ssc_depth, false, true, false);
+		if (err < 0) {
+			dev_dbg(ms_dev(host), "switch clock failed\n");
+			break;
+		}
+
+		host->ssc_depth = ssc_depth;
+		host->clock = clock;
+		host->ifmode = value;
+		break;
+	default:
+		err = -EINVAL;
+		break;
+	}
+out:
+	mutex_unlock(&ucr->dev_mutex);
+
+	/* power-on delay */
+	if (param == MEMSTICK_POWER && value == MEMSTICK_POWER_ON)
+		usleep_range(10000, 12000);
+
+	dev_dbg(ms_dev(host), "%s: return = %d\n", __func__, err);
+	return err;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int rtsx_usb_ms_suspend(struct device *dev)
+{
+	struct rtsx_usb_ms *host = dev_get_drvdata(dev);
+	struct memstick_host *msh = host->msh;
+
+	dev_dbg(ms_dev(host), "--> %s\n", __func__);
+
+	memstick_suspend_host(msh);
+	return 0;
+}
+
+static int rtsx_usb_ms_resume(struct device *dev)
+{
+	struct rtsx_usb_ms *host = dev_get_drvdata(dev);
+	struct memstick_host *msh = host->msh;
+
+	dev_dbg(ms_dev(host), "--> %s\n", __func__);
+
+	memstick_resume_host(msh);
+	return 0;
+}
+#endif /* CONFIG_PM_SLEEP */
+
+/*
+ * Thread function of ms card slot detection. The thread starts right after
+ * successful host addition. It stops while the driver removal function sets
+ * host->eject true.
+ */
+static int rtsx_usb_detect_ms_card(void *__host)
+{
+	struct rtsx_usb_ms *host = (struct rtsx_usb_ms *)__host;
+	struct rtsx_ucr *ucr = host->ucr;
+	u8 val = 0;
+	int err;
+
+	for (;;) {
+		mutex_lock(&ucr->dev_mutex);
+
+		/* Check pending MS card changes */
+		err = rtsx_usb_read_register(ucr, CARD_INT_PEND, &val);
+		if (err) {
+			mutex_unlock(&ucr->dev_mutex);
+			goto poll_again;
+		}
+
+		/* Clear the pending */
+		rtsx_usb_write_register(ucr, CARD_INT_PEND,
+				XD_INT | MS_INT | SD_INT,
+				XD_INT | MS_INT | SD_INT);
+
+		mutex_unlock(&ucr->dev_mutex);
+
+		if (val & MS_INT) {
+			dev_dbg(ms_dev(host), "MS slot change detected\n");
+			memstick_detect_change(host->msh);
+		}
+
+poll_again:
+		if (host->eject)
+			break;
+
+		msleep(1000);
+	}
+
+	complete(&host->detect_ms_exit);
+	return 0;
+}
+
+static int rtsx_usb_ms_drv_probe(struct platform_device *pdev)
+{
+	struct memstick_host *msh;
+	struct rtsx_usb_ms *host;
+	struct rtsx_ucr *ucr;
+	int err;
+
+	ucr = usb_get_intfdata(to_usb_interface(pdev->dev.parent));
+	if (!ucr)
+		return -ENXIO;
+
+	dev_dbg(&(pdev->dev),
+			"Realtek USB Memstick controller found\n");
+
+	msh = memstick_alloc_host(sizeof(*host), &pdev->dev);
+	if (!msh)
+		return -ENOMEM;
+
+	host = memstick_priv(msh);
+	host->ucr = ucr;
+	host->msh = msh;
+	host->pdev = pdev;
+	host->power_mode = MEMSTICK_POWER_OFF;
+	platform_set_drvdata(pdev, host);
+
+	mutex_init(&host->host_mutex);
+	INIT_WORK(&host->handle_req, rtsx_usb_ms_handle_req);
+
+	init_completion(&host->detect_ms_exit);
+	host->detect_ms = kthread_create(rtsx_usb_detect_ms_card, host,
+			"rtsx_usb_ms_%d", pdev->id);
+	if (IS_ERR(host->detect_ms)) {
+		dev_dbg(&(pdev->dev),
+				"Unable to create polling thread.\n");
+		err = PTR_ERR(host->detect_ms);
+		goto err_out;
+	}
+
+	msh->request = rtsx_usb_ms_request;
+	msh->set_param = rtsx_usb_ms_set_param;
+	msh->caps = MEMSTICK_CAP_PAR4;
+
+	pm_runtime_enable(&pdev->dev);
+	err = memstick_add_host(msh);
+	if (err)
+		goto err_out;
+
+	wake_up_process(host->detect_ms);
+	return 0;
+err_out:
+	memstick_free_host(msh);
+	return err;
+}
+
+static int rtsx_usb_ms_drv_remove(struct platform_device *pdev)
+{
+	struct rtsx_usb_ms *host = platform_get_drvdata(pdev);
+	struct memstick_host *msh;
+	int err;
+
+	msh = host->msh;
+	host->eject = true;
+	cancel_work_sync(&host->handle_req);
+
+	mutex_lock(&host->host_mutex);
+	if (host->req) {
+		dev_dbg(&(pdev->dev),
+			"%s: Controller removed during transfer\n",
+			dev_name(&msh->dev));
+		host->req->error = -ENOMEDIUM;
+		do {
+			err = memstick_next_req(msh, &host->req);
+			if (!err)
+				host->req->error = -ENOMEDIUM;
+		} while (!err);
+	}
+	mutex_unlock(&host->host_mutex);
+
+	wait_for_completion(&host->detect_ms_exit);
+	memstick_remove_host(msh);
+	memstick_free_host(msh);
+
+	/* Balance possible unbalanced usage count
+	 * e.g. unconditional module removal
+	 */
+	if (pm_runtime_active(ms_dev(host)))
+		pm_runtime_put(ms_dev(host));
+
+	pm_runtime_disable(&pdev->dev);
+	platform_set_drvdata(pdev, NULL);
+
+	dev_dbg(&(pdev->dev),
+		": Realtek USB Memstick controller has been removed\n");
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(rtsx_usb_ms_pm_ops,
+		rtsx_usb_ms_suspend, rtsx_usb_ms_resume);
+
+static struct platform_device_id rtsx_usb_ms_ids[] = {
+	{
+		.name = "rtsx_usb_ms",
+	}, {
+		/* sentinel */
+	}
+};
+MODULE_DEVICE_TABLE(platform, rtsx_usb_ms_ids);
+
+static struct platform_driver rtsx_usb_ms_driver = {
+	.probe		= rtsx_usb_ms_drv_probe,
+	.remove		= rtsx_usb_ms_drv_remove,
+	.id_table       = rtsx_usb_ms_ids,
+	.driver		= {
+		.owner	= THIS_MODULE,
+		.name	= "rtsx_usb_ms",
+		.pm	= &rtsx_usb_ms_pm_ops,
+	},
+};
+module_platform_driver(rtsx_usb_ms_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Roger Tseng <rogerable@realtek.com>");
+MODULE_DESCRIPTION("Realtek USB Memstick Card Host Driver");
diff --git a/drivers/message/fusion/mptbase.c b/drivers/message/fusion/mptbase.c
index 570b18a113f..ebc0af7d769 100644
--- a/drivers/message/fusion/mptbase.c
+++ b/drivers/message/fusion/mptbase.c
@@ -1037,7 +1037,7 @@ mpt_free_msg_frame(MPT_ADAPTER *ioc, MPT_FRAME_HDR *mf)
 		goto out;
 	/* signature to know if this mf is freed */
 	mf->u.frame.linkage.arg1 = cpu_to_le32(0xdeadbeaf);
-	list_add_tail(&mf->u.frame.linkage.list, &ioc->FreeQ);
+	list_add(&mf->u.frame.linkage.list, &ioc->FreeQ);
 #ifdef MFCNT
 	ioc->mfcnt--;
 #endif
diff --git a/drivers/message/fusion/mptctl.c b/drivers/message/fusion/mptctl.c
index dcc8385adeb..8a050e88568 100644
--- a/drivers/message/fusion/mptctl.c
+++ b/drivers/message/fusion/mptctl.c
@@ -2432,9 +2432,9 @@ mptctl_hp_hostinfo(unsigned long arg, unsigned int data_size)
 	int			rc, cim_rev;
 	ToolboxIstwiReadWriteRequest_t	*IstwiRWRequest;
 	MPT_FRAME_HDR		*mf = NULL;
-	MPIHeader_t		*mpi_hdr;
 	unsigned long		timeleft;
 	int			retval;
+	u32			msgcontext;
 
 	/* Reset long to int. Should affect IA64 and SPARC only
 	 */
@@ -2581,11 +2581,11 @@ mptctl_hp_hostinfo(unsigned long arg, unsigned int data_size)
 	}
 
 	IstwiRWRequest = (ToolboxIstwiReadWriteRequest_t *)mf;
-	mpi_hdr = (MPIHeader_t *) mf;
+	msgcontext = IstwiRWRequest->MsgContext;
 	memset(IstwiRWRequest,0,sizeof(ToolboxIstwiReadWriteRequest_t));
+	IstwiRWRequest->MsgContext = msgcontext;
 	IstwiRWRequest->Function = MPI_FUNCTION_TOOLBOX;
 	IstwiRWRequest->Tool = MPI_TOOLBOX_ISTWI_READ_WRITE_TOOL;
-	IstwiRWRequest->MsgContext = mpi_hdr->MsgContext;
 	IstwiRWRequest->Flags = MPI_TB_ISTWI_FLAGS_READ;
 	IstwiRWRequest->NumAddressBytes = 0x01;
 	IstwiRWRequest->DataLength = cpu_to_le16(0x04);
diff --git a/drivers/message/fusion/mptfc.c b/drivers/message/fusion/mptfc.c
index fd75108c355..02a3eefd693 100644
--- a/drivers/message/fusion/mptfc.c
+++ b/drivers/message/fusion/mptfc.c
@@ -649,7 +649,7 @@ mptfc_slave_alloc(struct scsi_device *sdev)
 }
 
 static int
-mptfc_qcmd_lck(struct scsi_cmnd *SCpnt, void (*done)(struct scsi_cmnd *))
+mptfc_qcmd(struct Scsi_Host *shost, struct scsi_cmnd *SCpnt)
 {
 	struct mptfc_rport_info	*ri;
 	struct fc_rport	*rport = starget_to_rport(scsi_target(SCpnt->device));
@@ -658,14 +658,14 @@ mptfc_qcmd_lck(struct scsi_cmnd *SCpnt, void (*done)(struct scsi_cmnd *))
 
 	if (!vdevice || !vdevice->vtarget) {
 		SCpnt->result = DID_NO_CONNECT << 16;
-		done(SCpnt);
+		SCpnt->scsi_done(SCpnt);
 		return 0;
 	}
 
 	err = fc_remote_port_chkready(rport);
 	if (unlikely(err)) {
 		SCpnt->result = err;
-		done(SCpnt);
+		SCpnt->scsi_done(SCpnt);
 		return 0;
 	}
 
@@ -673,15 +673,13 @@ mptfc_qcmd_lck(struct scsi_cmnd *SCpnt, void (*done)(struct scsi_cmnd *))
 	ri = *((struct mptfc_rport_info **)rport->dd_data);
 	if (unlikely(!ri)) {
 		SCpnt->result = DID_IMM_RETRY << 16;
-		done(SCpnt);
+		SCpnt->scsi_done(SCpnt);
 		return 0;
 	}
 
-	return mptscsih_qcmd(SCpnt,done);
+	return mptscsih_qcmd(SCpnt);
 }
 
-static DEF_SCSI_QCMD(mptfc_qcmd)
-
 /*
  *	mptfc_display_port_link_speed - displaying link speed
  *	@ioc: Pointer to MPT_ADAPTER structure
diff --git a/drivers/message/fusion/mptsas.c b/drivers/message/fusion/mptsas.c
index 00d339c361f..711fcb5cec8 100644
--- a/drivers/message/fusion/mptsas.c
+++ b/drivers/message/fusion/mptsas.c
@@ -1896,7 +1896,7 @@ mptsas_slave_alloc(struct scsi_device *sdev)
 }
 
 static int
-mptsas_qcmd_lck(struct scsi_cmnd *SCpnt, void (*done)(struct scsi_cmnd *))
+mptsas_qcmd(struct Scsi_Host *shost, struct scsi_cmnd *SCpnt)
 {
 	MPT_SCSI_HOST	*hd;
 	MPT_ADAPTER	*ioc;
@@ -1904,11 +1904,11 @@ mptsas_qcmd_lck(struct scsi_cmnd *SCpnt, void (*done)(struct scsi_cmnd *))
 
 	if (!vdevice || !vdevice->vtarget || vdevice->vtarget->deleted) {
 		SCpnt->result = DID_NO_CONNECT << 16;
-		done(SCpnt);
+		SCpnt->scsi_done(SCpnt);
 		return 0;
 	}
 
-	hd = shost_priv(SCpnt->device->host);
+	hd = shost_priv(shost);
 	ioc = hd->ioc;
 
 	if (ioc->sas_discovery_quiesce_io)
@@ -1917,11 +1917,9 @@ mptsas_qcmd_lck(struct scsi_cmnd *SCpnt, void (*done)(struct scsi_cmnd *))
 	if (ioc->debug_level & MPT_DEBUG_SCSI)
 		scsi_print_command(SCpnt);
 
-	return mptscsih_qcmd(SCpnt,done);
+	return mptscsih_qcmd(SCpnt);
 }
 
-static DEF_SCSI_QCMD(mptsas_qcmd)
-
 /**
  *	mptsas_mptsas_eh_timed_out - resets the scsi_cmnd timeout
  *		if the device under question is currently in the
diff --git a/drivers/message/fusion/mptscsih.c b/drivers/message/fusion/mptscsih.c
index 727819cc703..2a1c6f21af2 100644
--- a/drivers/message/fusion/mptscsih.c
+++ b/drivers/message/fusion/mptscsih.c
@@ -1304,7 +1304,6 @@ int mptscsih_show_info(struct seq_file *m, struct Scsi_Host *host)
 /**
  *	mptscsih_qcmd - Primary Fusion MPT SCSI initiator IO start routine.
  *	@SCpnt: Pointer to scsi_cmnd structure
- *	@done: Pointer SCSI mid-layer IO completion function
  *
  *	(linux scsi_host_template.queuecommand routine)
  *	This is the primary SCSI IO start routine.  Create a MPI SCSIIORequest
@@ -1313,7 +1312,7 @@ int mptscsih_show_info(struct seq_file *m, struct Scsi_Host *host)
  *	Returns 0. (rtn value discarded by linux scsi mid-layer)
  */
 int
-mptscsih_qcmd(struct scsi_cmnd *SCpnt, void (*done)(struct scsi_cmnd *))
+mptscsih_qcmd(struct scsi_cmnd *SCpnt)
 {
 	MPT_SCSI_HOST		*hd;
 	MPT_FRAME_HDR		*mf;
@@ -1329,10 +1328,9 @@ mptscsih_qcmd(struct scsi_cmnd *SCpnt, void (*done)(struct scsi_cmnd *))
 
 	hd = shost_priv(SCpnt->device->host);
 	ioc = hd->ioc;
-	SCpnt->scsi_done = done;
 
-	dmfprintk(ioc, printk(MYIOC_s_DEBUG_FMT "qcmd: SCpnt=%p, done()=%p\n",
-		ioc->name, SCpnt, done));
+	dmfprintk(ioc, printk(MYIOC_s_DEBUG_FMT "qcmd: SCpnt=%p\n",
+		ioc->name, SCpnt));
 
 	if (ioc->taskmgmt_quiesce_io)
 		return SCSI_MLQUEUE_HOST_BUSY;
diff --git a/drivers/message/fusion/mptscsih.h b/drivers/message/fusion/mptscsih.h
index 83f503162f7..99e3390807f 100644
--- a/drivers/message/fusion/mptscsih.h
+++ b/drivers/message/fusion/mptscsih.h
@@ -113,7 +113,7 @@ extern int mptscsih_resume(struct pci_dev *pdev);
 #endif
 extern int mptscsih_show_info(struct seq_file *, struct Scsi_Host *);
 extern const char * mptscsih_info(struct Scsi_Host *SChost);
-extern int mptscsih_qcmd(struct scsi_cmnd *SCpnt, void (*done)(struct scsi_cmnd *));
+extern int mptscsih_qcmd(struct scsi_cmnd *SCpnt);
 extern int mptscsih_IssueTaskMgmt(MPT_SCSI_HOST *hd, u8 type, u8 channel,
 	u8 id, int lun, int ctx2abort, ulong timeout);
 extern void mptscsih_slave_destroy(struct scsi_device *device);
diff --git a/drivers/message/fusion/mptspi.c b/drivers/message/fusion/mptspi.c
index 5653e505f91..49d11338294 100644
--- a/drivers/message/fusion/mptspi.c
+++ b/drivers/message/fusion/mptspi.c
@@ -780,33 +780,31 @@ static int mptspi_slave_configure(struct scsi_device *sdev)
 }
 
 static int
-mptspi_qcmd_lck(struct scsi_cmnd *SCpnt, void (*done)(struct scsi_cmnd *))
+mptspi_qcmd(struct Scsi_Host *shost, struct scsi_cmnd *SCpnt)
 {
-	struct _MPT_SCSI_HOST *hd = shost_priv(SCpnt->device->host);
+	struct _MPT_SCSI_HOST *hd = shost_priv(shost);
 	VirtDevice	*vdevice = SCpnt->device->hostdata;
 	MPT_ADAPTER *ioc = hd->ioc;
 
 	if (!vdevice || !vdevice->vtarget) {
 		SCpnt->result = DID_NO_CONNECT << 16;
-		done(SCpnt);
+		SCpnt->scsi_done(SCpnt);
 		return 0;
 	}
 
 	if (SCpnt->device->channel == 1 &&
 		mptscsih_is_phys_disk(ioc, 0, SCpnt->device->id) == 0) {
 		SCpnt->result = DID_NO_CONNECT << 16;
-		done(SCpnt);
+		SCpnt->scsi_done(SCpnt);
 		return 0;
 	}
 
 	if (spi_dv_pending(scsi_target(SCpnt->device)))
 		ddvprintk(ioc, scsi_print_command(SCpnt));
 
-	return mptscsih_qcmd(SCpnt,done);
+	return mptscsih_qcmd(SCpnt);
 }
 
-static DEF_SCSI_QCMD(mptspi_qcmd)
-
 static void mptspi_slave_destroy(struct scsi_device *sdev)
 {
 	struct scsi_target *starget = scsi_target(sdev);
diff --git a/drivers/mfd/Kconfig b/drivers/mfd/Kconfig
index 33834120d05..ee8204cc31e 100644
--- a/drivers/mfd/Kconfig
+++ b/drivers/mfd/Kconfig
@@ -67,6 +67,18 @@ config MFD_BCM590XX
 	help
 	  Support for the BCM590xx PMUs from Broadcom
 
+config MFD_AXP20X
+	bool "X-Powers AXP20X"
+	select MFD_CORE
+	select REGMAP_I2C
+	select REGMAP_IRQ
+	depends on I2C=y
+	help
+	  If you say Y here you get support for the X-Powers AXP202 and AXP209.
+	  This driver include only the core APIs. You have to select individual
+	  components like regulators or the PEK (Power Enable Key) under the
+	  corresponding menus.
+
 config MFD_CROS_EC
 	tristate "ChromeOS Embedded Controller"
 	select MFD_CORE
@@ -250,6 +262,16 @@ config MFD_INTEL_MSIC
 	  Passage) chip. This chip embeds audio, battery, GPIO, etc.
 	  devices used in Intel Medfield platforms.
 
+config MFD_IPAQ_MICRO
+	bool "Atmel Micro ASIC (iPAQ h3100/h3600/h3700) Support"
+	depends on SA1100_H3100 || SA1100_H3600
+	select MFD_CORE
+	help
+	  Select this to get support for the Microcontroller found in
+	  the Compaq iPAQ handheld computers. This is an Atmel
+	  AT90LS8535 microcontroller flashed with a special iPAQ
+	  firmware using the custom protocol implemented in this driver.
+
 config MFD_JANZ_CMODIO
 	tristate "Janz CMOD-IO PCI MODULbus Carrier Board"
 	select MFD_CORE
@@ -331,15 +353,15 @@ config MFD_88PM860X
 	  battery-charger under the corresponding menus.
 
 config MFD_MAX14577
-	bool "Maxim Semiconductor MAX14577 MUIC + Charger Support"
+	bool "Maxim Semiconductor MAX14577/77836 MUIC + Charger Support"
 	depends on I2C=y
 	select MFD_CORE
 	select REGMAP_I2C
 	select REGMAP_IRQ
 	select IRQ_DOMAIN
 	help
-	  Say yes here to add support for Maxim Semiconductor MAX14577.
-	  This is a Micro-USB IC with Charger controls on chip.
+	  Say yes here to add support for Maxim Semiconductor MAX14577 and
+	  MAX77836 Micro-USB ICs with battery charger.
 	  This driver provides common support for accessing the device;
 	  additional drivers must be enabled in order to use the functionality
 	  of the device.
@@ -675,6 +697,7 @@ config MFD_DB8500_PRCMU
 config MFD_STMPE
 	bool "STMicroelectronics STMPE"
 	depends on (I2C=y || SPI_MASTER=y)
+	depends on OF
 	select MFD_CORE
 	help
 	  Support for the STMPE family of I/O Expanders from
@@ -719,6 +742,14 @@ config MFD_STA2X11
 	select MFD_CORE
 	select REGMAP_MMIO
 
+config MFD_SUN6I_PRCM
+	bool "Allwinner A31 PRCM controller"
+	depends on ARCH_SUNXI
+	select MFD_CORE
+	help
+	  Support for the PRCM (Power/Reset/Clock Management) unit available
+	  in A31 SoC.
+
 config MFD_SYSCON
 	bool "System Controller Register R/W Based on Regmap"
 	select REGMAP_MMIO
@@ -1227,12 +1258,17 @@ config MCP_UCB1200_TS
 
 endmenu
 
-config VEXPRESS_CONFIG
-	bool "ARM Versatile Express platform infrastructure"
-	depends on ARM || ARM64
+config MFD_VEXPRESS_SYSREG
+	bool "Versatile Express System Registers"
+	depends on VEXPRESS_CONFIG && GPIOLIB
+	default y
+	select CLKSRC_MMIO
+	select GPIO_GENERIC_PLATFORM
+	select MFD_CORE
+	select MFD_SYSCON
 	help
-	  Platform configuration infrastructure for the ARM Ltd.
-	  Versatile Express.
+	  System Registers are the platform configuration block
+	  on the ARM Ltd. Versatile Express board.
 
 endmenu
 endif
diff --git a/drivers/mfd/Makefile b/drivers/mfd/Makefile
index 2851275e265..8afedba535c 100644
--- a/drivers/mfd/Makefile
+++ b/drivers/mfd/Makefile
@@ -29,6 +29,7 @@ obj-$(CONFIG_MFD_STA2X11)	+= sta2x11-mfd.o
 obj-$(CONFIG_MFD_STMPE)		+= stmpe.o
 obj-$(CONFIG_STMPE_I2C)		+= stmpe-i2c.o
 obj-$(CONFIG_STMPE_SPI)		+= stmpe-spi.o
+obj-$(CONFIG_MFD_SUN6I_PRCM)	+= sun6i-prcm.o
 obj-$(CONFIG_MFD_TC3589X)	+= tc3589x.o
 obj-$(CONFIG_MFD_T7L66XB)	+= t7l66xb.o tmio_core.o
 obj-$(CONFIG_MFD_TC6387XB)	+= tc6387xb.o tmio_core.o
@@ -102,6 +103,7 @@ obj-$(CONFIG_PMIC_DA9052)	+= da9052-irq.o
 obj-$(CONFIG_PMIC_DA9052)	+= da9052-core.o
 obj-$(CONFIG_MFD_DA9052_SPI)	+= da9052-spi.o
 obj-$(CONFIG_MFD_DA9052_I2C)	+= da9052-i2c.o
+obj-$(CONFIG_MFD_AXP20X)	+= axp20x.o
 
 obj-$(CONFIG_MFD_LP3943)	+= lp3943.o
 obj-$(CONFIG_MFD_LP8788)	+= lp8788.o lp8788-irq.o
@@ -161,8 +163,9 @@ obj-$(CONFIG_MFD_RC5T583)	+= rc5t583.o rc5t583-irq.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
-obj-$(CONFIG_VEXPRESS_CONFIG)	+= vexpress-config.o vexpress-sysreg.o
+obj-$(CONFIG_MFD_VEXPRESS_SYSREG)	+= vexpress-sysreg.o
 obj-$(CONFIG_MFD_RETU)		+= retu-mfd.o
 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
diff --git a/drivers/mfd/abx500-core.c b/drivers/mfd/abx500-core.c
index f3a15aa54d7..fe418995108 100644
--- a/drivers/mfd/abx500-core.c
+++ b/drivers/mfd/abx500-core.c
@@ -151,22 +151,6 @@ int abx500_startup_irq_enabled(struct device *dev, unsigned int irq)
 }
 EXPORT_SYMBOL(abx500_startup_irq_enabled);
 
-void abx500_dump_all_banks(void)
-{
-	struct abx500_ops *ops;
-	struct device dummy_child = {NULL};
-	struct abx500_device_entry *dev_entry;
-
-	list_for_each_entry(dev_entry, &abx500_list, list) {
-		dummy_child.parent = dev_entry->dev;
-		ops = &dev_entry->ops;
-
-		if ((ops != NULL) && (ops->dump_all_banks != NULL))
-			ops->dump_all_banks(&dummy_child);
-	}
-}
-EXPORT_SYMBOL(abx500_dump_all_banks);
-
 MODULE_AUTHOR("Mattias Wallin <mattias.wallin@stericsson.com>");
 MODULE_DESCRIPTION("ABX500 core driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/arizona-core.c b/drivers/mfd/arizona-core.c
index 1c3ae57082e..cfc191abae4 100644
--- a/drivers/mfd/arizona-core.c
+++ b/drivers/mfd/arizona-core.c
@@ -508,19 +508,31 @@ int arizona_of_get_type(struct device *dev)
 }
 EXPORT_SYMBOL_GPL(arizona_of_get_type);
 
+int arizona_of_get_named_gpio(struct arizona *arizona, const char *prop,
+			      bool mandatory)
+{
+	int gpio;
+
+	gpio = of_get_named_gpio(arizona->dev->of_node, prop, 0);
+	if (gpio < 0) {
+		if (mandatory)
+			dev_err(arizona->dev,
+				"Mandatory DT gpio %s missing/malformed: %d\n",
+				prop, gpio);
+
+		gpio = 0;
+	}
+
+	return gpio;
+}
+EXPORT_SYMBOL_GPL(arizona_of_get_named_gpio);
+
 static int arizona_of_get_core_pdata(struct arizona *arizona)
 {
+	struct arizona_pdata *pdata = &arizona->pdata;
 	int ret, i;
 
-	arizona->pdata.reset = of_get_named_gpio(arizona->dev->of_node,
-						 "wlf,reset", 0);
-	if (arizona->pdata.reset < 0)
-		arizona->pdata.reset = 0;
-
-	arizona->pdata.ldoena = of_get_named_gpio(arizona->dev->of_node,
-						  "wlf,ldoena", 0);
-	if (arizona->pdata.ldoena < 0)
-		arizona->pdata.ldoena = 0;
+	pdata->reset = arizona_of_get_named_gpio(arizona, "wlf,reset", true);
 
 	ret = of_property_read_u32_array(arizona->dev->of_node,
 					 "wlf,gpio-defaults",
@@ -571,6 +583,7 @@ static const char *wm5102_supplies[] = {
 	"CPVDD",
 	"SPKVDDL",
 	"SPKVDDR",
+	"MICVDD",
 };
 
 static const struct mfd_cell wm5102_devs[] = {
@@ -652,6 +665,9 @@ int arizona_dev_init(struct arizona *arizona)
 		return -EINVAL;
 	}
 
+	/* Mark DCVDD as external, LDO1 driver will clear if internal */
+	arizona->external_dcvdd = true;
+
 	ret = mfd_add_devices(arizona->dev, -1, early_devs,
 			      ARRAY_SIZE(early_devs), NULL, 0, NULL);
 	if (ret != 0) {
@@ -851,14 +867,6 @@ int arizona_dev_init(struct arizona *arizona)
 			     arizona->pdata.gpio_defaults[i]);
 	}
 
-	/*
-	 * LDO1 can only be used to supply DCVDD so if it has no
-	 * consumers then DCVDD is supplied externally.
-	 */
-	if (arizona->pdata.ldo1 &&
-	    arizona->pdata.ldo1->num_consumer_supplies == 0)
-		arizona->external_dcvdd = true;
-
 	pm_runtime_set_autosuspend_delay(arizona->dev, 100);
 	pm_runtime_use_autosuspend(arizona->dev);
 	pm_runtime_enable(arizona->dev);
diff --git a/drivers/mfd/arizona-irq.c b/drivers/mfd/arizona-irq.c
index 88758ab9402..17102f58910 100644
--- a/drivers/mfd/arizona-irq.c
+++ b/drivers/mfd/arizona-irq.c
@@ -285,7 +285,7 @@ int arizona_irq_init(struct arizona *arizona)
 				  IRQF_ONESHOT, -1, irq,
 				  &arizona->irq_chip);
 	if (ret != 0) {
-		dev_err(arizona->dev, "Failed to add AOD IRQs: %d\n", ret);
+		dev_err(arizona->dev, "Failed to add main IRQs: %d\n", ret);
 		goto err_aod;
 	}
 
diff --git a/drivers/mfd/as3711.c b/drivers/mfd/as3711.c
index ec684fcedb4..d9706ede8d3 100644
--- a/drivers/mfd/as3711.c
+++ b/drivers/mfd/as3711.c
@@ -114,7 +114,7 @@ static const struct regmap_config as3711_regmap_config = {
 };
 
 #ifdef CONFIG_OF
-static struct of_device_id as3711_of_match[] = {
+static const struct of_device_id as3711_of_match[] = {
 	{.compatible = "ams,as3711",},
 	{}
 };
diff --git a/drivers/mfd/axp20x.c b/drivers/mfd/axp20x.c
new file mode 100644
index 00000000000..dee653989e3
--- /dev/null
+++ b/drivers/mfd/axp20x.c
@@ -0,0 +1,258 @@
+/*
+ * axp20x.c - MFD core driver for the X-Powers AXP202 and AXP209
+ *
+ * AXP20x comprises an adaptive USB-Compatible PWM charger, 2 BUCK DC-DC
+ * converters, 5 LDOs, multiple 12-bit ADCs of voltage, current and temperature
+ * as well as 4 configurable GPIOs.
+ *
+ * Author: 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.
+ */
+
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+#include <linux/regulator/consumer.h>
+#include <linux/mfd/axp20x.h>
+#include <linux/mfd/core.h>
+#include <linux/of_device.h>
+#include <linux/of_irq.h>
+
+#define AXP20X_OFF	0x80
+
+static const struct regmap_range axp20x_writeable_ranges[] = {
+	regmap_reg_range(AXP20X_DATACACHE(0), AXP20X_IRQ5_STATE),
+	regmap_reg_range(AXP20X_DCDC_MODE, AXP20X_FG_RES),
+};
+
+static const struct regmap_range axp20x_volatile_ranges[] = {
+	regmap_reg_range(AXP20X_IRQ1_EN, AXP20X_IRQ5_STATE),
+};
+
+static const struct regmap_access_table axp20x_writeable_table = {
+	.yes_ranges	= axp20x_writeable_ranges,
+	.n_yes_ranges	= ARRAY_SIZE(axp20x_writeable_ranges),
+};
+
+static const struct regmap_access_table axp20x_volatile_table = {
+	.yes_ranges	= axp20x_volatile_ranges,
+	.n_yes_ranges	= ARRAY_SIZE(axp20x_volatile_ranges),
+};
+
+static struct resource axp20x_pek_resources[] = {
+	{
+		.name	= "PEK_DBR",
+		.start	= AXP20X_IRQ_PEK_RIS_EDGE,
+		.end	= AXP20X_IRQ_PEK_RIS_EDGE,
+		.flags	= IORESOURCE_IRQ,
+	}, {
+		.name	= "PEK_DBF",
+		.start	= AXP20X_IRQ_PEK_FAL_EDGE,
+		.end	= AXP20X_IRQ_PEK_FAL_EDGE,
+		.flags	= IORESOURCE_IRQ,
+	},
+};
+
+static const struct regmap_config axp20x_regmap_config = {
+	.reg_bits	= 8,
+	.val_bits	= 8,
+	.wr_table	= &axp20x_writeable_table,
+	.volatile_table	= &axp20x_volatile_table,
+	.max_register	= AXP20X_FG_RES,
+	.cache_type	= REGCACHE_RBTREE,
+};
+
+#define AXP20X_IRQ(_irq, _off, _mask) \
+	[AXP20X_IRQ_##_irq] = { .reg_offset = (_off), .mask = BIT(_mask) }
+
+static const struct regmap_irq axp20x_regmap_irqs[] = {
+	AXP20X_IRQ(ACIN_OVER_V,		0, 7),
+	AXP20X_IRQ(ACIN_PLUGIN,		0, 6),
+	AXP20X_IRQ(ACIN_REMOVAL,	0, 5),
+	AXP20X_IRQ(VBUS_OVER_V,		0, 4),
+	AXP20X_IRQ(VBUS_PLUGIN,		0, 3),
+	AXP20X_IRQ(VBUS_REMOVAL,	0, 2),
+	AXP20X_IRQ(VBUS_V_LOW,		0, 1),
+	AXP20X_IRQ(BATT_PLUGIN,		1, 7),
+	AXP20X_IRQ(BATT_REMOVAL,	1, 6),
+	AXP20X_IRQ(BATT_ENT_ACT_MODE,	1, 5),
+	AXP20X_IRQ(BATT_EXIT_ACT_MODE,	1, 4),
+	AXP20X_IRQ(CHARG,		1, 3),
+	AXP20X_IRQ(CHARG_DONE,		1, 2),
+	AXP20X_IRQ(BATT_TEMP_HIGH,	1, 1),
+	AXP20X_IRQ(BATT_TEMP_LOW,	1, 0),
+	AXP20X_IRQ(DIE_TEMP_HIGH,	2, 7),
+	AXP20X_IRQ(CHARG_I_LOW,		2, 6),
+	AXP20X_IRQ(DCDC1_V_LONG,	2, 5),
+	AXP20X_IRQ(DCDC2_V_LONG,	2, 4),
+	AXP20X_IRQ(DCDC3_V_LONG,	2, 3),
+	AXP20X_IRQ(PEK_SHORT,		2, 1),
+	AXP20X_IRQ(PEK_LONG,		2, 0),
+	AXP20X_IRQ(N_OE_PWR_ON,		3, 7),
+	AXP20X_IRQ(N_OE_PWR_OFF,	3, 6),
+	AXP20X_IRQ(VBUS_VALID,		3, 5),
+	AXP20X_IRQ(VBUS_NOT_VALID,	3, 4),
+	AXP20X_IRQ(VBUS_SESS_VALID,	3, 3),
+	AXP20X_IRQ(VBUS_SESS_END,	3, 2),
+	AXP20X_IRQ(LOW_PWR_LVL1,	3, 1),
+	AXP20X_IRQ(LOW_PWR_LVL2,	3, 0),
+	AXP20X_IRQ(TIMER,		4, 7),
+	AXP20X_IRQ(PEK_RIS_EDGE,	4, 6),
+	AXP20X_IRQ(PEK_FAL_EDGE,	4, 5),
+	AXP20X_IRQ(GPIO3_INPUT,		4, 3),
+	AXP20X_IRQ(GPIO2_INPUT,		4, 2),
+	AXP20X_IRQ(GPIO1_INPUT,		4, 1),
+	AXP20X_IRQ(GPIO0_INPUT,		4, 0),
+};
+
+static const struct of_device_id axp20x_of_match[] = {
+	{ .compatible = "x-powers,axp202", .data = (void *) AXP202_ID },
+	{ .compatible = "x-powers,axp209", .data = (void *) AXP209_ID },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, axp20x_of_match);
+
+/*
+ * This is useless for OF-enabled devices, but it is needed by I2C subsystem
+ */
+static const struct i2c_device_id axp20x_i2c_id[] = {
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, axp20x_i2c_id);
+
+static const struct regmap_irq_chip axp20x_regmap_irq_chip = {
+	.name			= "axp20x_irq_chip",
+	.status_base		= AXP20X_IRQ1_STATE,
+	.ack_base		= AXP20X_IRQ1_STATE,
+	.mask_base		= AXP20X_IRQ1_EN,
+	.num_regs		= 5,
+	.irqs			= axp20x_regmap_irqs,
+	.num_irqs		= ARRAY_SIZE(axp20x_regmap_irqs),
+	.mask_invert		= true,
+	.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",
+		.num_resources		= ARRAY_SIZE(axp20x_pek_resources),
+		.resources		= axp20x_pek_resources,
+	}, {
+		.name			= "axp20x-regulator",
+		.parent_supplies	= axp20x_supplies,
+		.num_parent_supplies	= ARRAY_SIZE(axp20x_supplies),
+	},
+};
+
+static struct axp20x_dev *axp20x_pm_power_off;
+static void axp20x_power_off(void)
+{
+	regmap_write(axp20x_pm_power_off->regmap, AXP20X_OFF_CTRL,
+		     AXP20X_OFF);
+}
+
+static int axp20x_i2c_probe(struct i2c_client *i2c,
+			 const struct i2c_device_id *id)
+{
+	struct axp20x_dev *axp20x;
+	const struct of_device_id *of_id;
+	int ret;
+
+	axp20x = devm_kzalloc(&i2c->dev, sizeof(*axp20x), GFP_KERNEL);
+	if (!axp20x)
+		return -ENOMEM;
+
+	of_id = of_match_device(axp20x_of_match, &i2c->dev);
+	if (!of_id) {
+		dev_err(&i2c->dev, "Unable to setup AXP20X data\n");
+		return -ENODEV;
+	}
+	axp20x->variant = (long) of_id->data;
+
+	axp20x->i2c_client = i2c;
+	axp20x->dev = &i2c->dev;
+	dev_set_drvdata(axp20x->dev, axp20x);
+
+	axp20x->regmap = devm_regmap_init_i2c(i2c, &axp20x_regmap_config);
+	if (IS_ERR(axp20x->regmap)) {
+		ret = PTR_ERR(axp20x->regmap);
+		dev_err(&i2c->dev, "regmap init failed: %d\n", ret);
+		return ret;
+	}
+
+	ret = regmap_add_irq_chip(axp20x->regmap, i2c->irq,
+				  IRQF_ONESHOT | IRQF_SHARED, -1,
+				  &axp20x_regmap_irq_chip,
+				  &axp20x->regmap_irqc);
+	if (ret) {
+		dev_err(&i2c->dev, "failed to add irq chip: %d\n", ret);
+		return ret;
+	}
+
+	ret = mfd_add_devices(axp20x->dev, -1, axp20x_cells,
+			      ARRAY_SIZE(axp20x_cells), NULL, 0, NULL);
+
+	if (ret) {
+		dev_err(&i2c->dev, "failed to add MFD devices: %d\n", ret);
+		regmap_del_irq_chip(i2c->irq, axp20x->regmap_irqc);
+		return ret;
+	}
+
+	if (!pm_power_off) {
+		axp20x_pm_power_off = axp20x;
+		pm_power_off = axp20x_power_off;
+	}
+
+	dev_info(&i2c->dev, "AXP20X driver loaded\n");
+
+	return 0;
+}
+
+static int axp20x_i2c_remove(struct i2c_client *i2c)
+{
+	struct axp20x_dev *axp20x = i2c_get_clientdata(i2c);
+
+	if (axp20x == axp20x_pm_power_off) {
+		axp20x_pm_power_off = NULL;
+		pm_power_off = NULL;
+	}
+
+	mfd_remove_devices(axp20x->dev);
+	regmap_del_irq_chip(axp20x->i2c_client->irq, axp20x->regmap_irqc);
+
+	return 0;
+}
+
+static struct i2c_driver axp20x_i2c_driver = {
+	.driver = {
+		.name	= "axp20x",
+		.owner	= THIS_MODULE,
+		.of_match_table	= of_match_ptr(axp20x_of_match),
+	},
+	.probe		= axp20x_i2c_probe,
+	.remove		= axp20x_i2c_remove,
+	.id_table	= axp20x_i2c_id,
+};
+
+module_i2c_driver(axp20x_i2c_driver);
+
+MODULE_DESCRIPTION("PMIC MFD core driver for AXP20X");
+MODULE_AUTHOR("Carlo Caione <carlo@caione.org>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/bcm590xx.c b/drivers/mfd/bcm590xx.c
index e9a33c79431..e334de000e8 100644
--- a/drivers/mfd/bcm590xx.c
+++ b/drivers/mfd/bcm590xx.c
@@ -28,42 +28,80 @@ static const struct mfd_cell bcm590xx_devs[] = {
 	},
 };
 
-static const struct regmap_config bcm590xx_regmap_config = {
+static const struct regmap_config bcm590xx_regmap_config_pri = {
 	.reg_bits	= 8,
 	.val_bits	= 8,
-	.max_register	= BCM590XX_MAX_REGISTER,
+	.max_register	= BCM590XX_MAX_REGISTER_PRI,
 	.cache_type	= REGCACHE_RBTREE,
 };
 
-static int bcm590xx_i2c_probe(struct i2c_client *i2c,
+static const struct regmap_config bcm590xx_regmap_config_sec = {
+	.reg_bits	= 8,
+	.val_bits	= 8,
+	.max_register	= BCM590XX_MAX_REGISTER_SEC,
+	.cache_type	= REGCACHE_RBTREE,
+};
+
+static int bcm590xx_i2c_probe(struct i2c_client *i2c_pri,
 			      const struct i2c_device_id *id)
 {
 	struct bcm590xx *bcm590xx;
 	int ret;
 
-	bcm590xx = devm_kzalloc(&i2c->dev, sizeof(*bcm590xx), GFP_KERNEL);
+	bcm590xx = devm_kzalloc(&i2c_pri->dev, sizeof(*bcm590xx), GFP_KERNEL);
 	if (!bcm590xx)
 		return -ENOMEM;
 
-	i2c_set_clientdata(i2c, bcm590xx);
-	bcm590xx->dev = &i2c->dev;
-	bcm590xx->i2c_client = i2c;
+	i2c_set_clientdata(i2c_pri, bcm590xx);
+	bcm590xx->dev = &i2c_pri->dev;
+	bcm590xx->i2c_pri = i2c_pri;
 
-	bcm590xx->regmap = devm_regmap_init_i2c(i2c, &bcm590xx_regmap_config);
-	if (IS_ERR(bcm590xx->regmap)) {
-		ret = PTR_ERR(bcm590xx->regmap);
-		dev_err(&i2c->dev, "regmap initialization failed: %d\n", ret);
+	bcm590xx->regmap_pri = devm_regmap_init_i2c(i2c_pri,
+						 &bcm590xx_regmap_config_pri);
+	if (IS_ERR(bcm590xx->regmap_pri)) {
+		ret = PTR_ERR(bcm590xx->regmap_pri);
+		dev_err(&i2c_pri->dev, "primary regmap init failed: %d\n", ret);
 		return ret;
 	}
 
-	ret = mfd_add_devices(&i2c->dev, -1, bcm590xx_devs,
+	/* Secondary I2C slave address is the base address with A(2) asserted */
+	bcm590xx->i2c_sec = i2c_new_dummy(i2c_pri->adapter,
+					  i2c_pri->addr | BIT(2));
+	if (IS_ERR_OR_NULL(bcm590xx->i2c_sec)) {
+		dev_err(&i2c_pri->dev, "failed to add secondary I2C device\n");
+		return -ENODEV;
+	}
+	i2c_set_clientdata(bcm590xx->i2c_sec, bcm590xx);
+
+	bcm590xx->regmap_sec = devm_regmap_init_i2c(bcm590xx->i2c_sec,
+						&bcm590xx_regmap_config_sec);
+	if (IS_ERR(bcm590xx->regmap_sec)) {
+		ret = PTR_ERR(bcm590xx->regmap_sec);
+		dev_err(&bcm590xx->i2c_sec->dev,
+			"secondary regmap init failed: %d\n", ret);
+		goto err;
+	}
+
+	ret = mfd_add_devices(&i2c_pri->dev, -1, bcm590xx_devs,
 			      ARRAY_SIZE(bcm590xx_devs), NULL, 0, NULL);
-	if (ret < 0)
-		dev_err(&i2c->dev, "failed to add sub-devices: %d\n", ret);
+	if (ret < 0) {
+		dev_err(&i2c_pri->dev, "failed to add sub-devices: %d\n", ret);
+		goto err;
+	}
+
+	return 0;
 
+err:
+	i2c_unregister_device(bcm590xx->i2c_sec);
 	return ret;
 }
 
+static int bcm590xx_i2c_remove(struct i2c_client *i2c)
+{
+	mfd_remove_devices(&i2c->dev);
+	return 0;
+}
+
 static const struct of_device_id bcm590xx_of_match[] = {
 	{ .compatible = "brcm,bcm59056" },
 	{ }
@@ -83,6 +121,7 @@ static struct i2c_driver bcm590xx_i2c_driver = {
 		   .of_match_table = of_match_ptr(bcm590xx_of_match),
 	},
 	.probe = bcm590xx_i2c_probe,
+	.remove = bcm590xx_i2c_remove,
 	.id_table = bcm590xx_i2c_id,
 };
 module_i2c_driver(bcm590xx_i2c_driver);
@@ -90,4 +129,4 @@ module_i2c_driver(bcm590xx_i2c_driver);
 MODULE_AUTHOR("Matt Porter <mporter@linaro.org>");
 MODULE_DESCRIPTION("BCM590xx multi-function driver");
 MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("platform:bcm590xx");
+MODULE_ALIAS("i2c:bcm590xx");
diff --git a/drivers/mfd/cros_ec.c b/drivers/mfd/cros_ec.c
index 783fe2e73e1..38fe9bf0d16 100644
--- a/drivers/mfd/cros_ec.c
+++ b/drivers/mfd/cros_ec.c
@@ -30,7 +30,7 @@ int cros_ec_prepare_tx(struct cros_ec_device *ec_dev,
 	uint8_t *out;
 	int csum, i;
 
-	BUG_ON(msg->out_len > EC_HOST_PARAM_SIZE);
+	BUG_ON(msg->out_len > EC_PROTO2_MAX_PARAM_SIZE);
 	out = ec_dev->dout;
 	out[0] = EC_CMD_VERSION0 + msg->version;
 	out[1] = msg->cmd;
@@ -90,6 +90,11 @@ static const struct mfd_cell cros_devs[] = {
 		.id = 1,
 		.of_compatible = "google,cros-ec-keyb",
 	},
+	{
+		.name = "cros-ec-i2c-tunnel",
+		.id = 2,
+		.of_compatible = "google,cros-ec-i2c-tunnel",
+	},
 };
 
 int cros_ec_register(struct cros_ec_device *ec_dev)
@@ -184,3 +189,6 @@ int cros_ec_resume(struct cros_ec_device *ec_dev)
 EXPORT_SYMBOL(cros_ec_resume);
 
 #endif
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ChromeOS EC core driver");
diff --git a/drivers/mfd/cros_ec_spi.c b/drivers/mfd/cros_ec_spi.c
index 84af8d7a429..0b8d3282916 100644
--- a/drivers/mfd/cros_ec_spi.c
+++ b/drivers/mfd/cros_ec_spi.c
@@ -39,14 +39,22 @@
 #define EC_MSG_PREAMBLE_COUNT		32
 
 /*
-  * We must get a response from the EC in 5ms. This is a very long
-  * time, but the flash write command can take 2-3ms. The EC command
-  * processing is currently not very fast (about 500us). We could
-  * look at speeding this up and making the flash write command a
-  * 'slow' command, requiring a GET_STATUS wait loop, like flash
-  * erase.
-  */
-#define EC_MSG_DEADLINE_MS		5
+ * Allow for a long time for the EC to respond.  We support i2c
+ * tunneling and support fairly long messages for the tunnel (249
+ * bytes long at the moment).  If we're talking to a 100 kHz device
+ * on the other end and need to transfer ~256 bytes, then we need:
+ *  10 us/bit * ~10 bits/byte * ~256 bytes = ~25ms
+ *
+ * We'll wait 4 times that to handle clock stretching and other
+ * paranoia.
+ *
+ * It's pretty unlikely that we'll really see a 249 byte tunnel in
+ * anything other than testing.  If this was more common we might
+ * consider having slow commands like this require a GET_STATUS
+ * wait loop.  The 'flash write' command would be another candidate
+ * for this, clocking in at 2-3ms.
+ */
+#define EC_MSG_DEADLINE_MS		100
 
 /*
   * Time between raising the SPI chip select (for the end of a
@@ -65,11 +73,13 @@
  *	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_command_spi_xfer 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,
@@ -111,7 +121,9 @@ static int cros_ec_spi_receive_response(struct cros_ec_device *ec_dev,
 
 	/* Receive data until we see the header byte */
 	deadline = jiffies + msecs_to_jiffies(EC_MSG_DEADLINE_MS);
-	do {
+	while (true) {
+		unsigned long start_jiffies = jiffies;
+
 		memset(&trans, 0, sizeof(trans));
 		trans.cs_change = 1;
 		trans.rx_buf = ptr = ec_dev->din;
@@ -132,12 +144,19 @@ static int cros_ec_spi_receive_response(struct cros_ec_device *ec_dev,
 				break;
 			}
 		}
+		if (ptr != end)
+			break;
 
-		if (time_after(jiffies, deadline)) {
+		/*
+		 * Use the time at the start of the loop as a timeout.  This
+		 * gives us one last shot at getting the transfer and is useful
+		 * in case we got context switched out for a while.
+		 */
+		if (time_after(start_jiffies, deadline)) {
 			dev_warn(ec_dev->dev, "EC failed to respond in time\n");
 			return -ETIMEDOUT;
 		}
-	} while (ptr == end);
+	}
 
 	/*
 	 * ptr now points to the header byte. Copy any valid data to the
@@ -208,6 +227,13 @@ static int cros_ec_command_spi_xfer(struct cros_ec_device *ec_dev,
 	int ret = 0, final_ret;
 	struct timespec ts;
 
+	/*
+	 * 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);
 
@@ -219,7 +245,7 @@ static int cros_ec_command_spi_xfer(struct cros_ec_device *ec_dev,
 		ktime_get_ts(&ts);
 		delay = timespec_to_ns(&ts) - ec_spi->last_transfer_ns;
 		if (delay < EC_SPI_RECOVERY_TIME_NS)
-			ndelay(delay);
+			ndelay(EC_SPI_RECOVERY_TIME_NS - delay);
 	}
 
 	/* Transmit phase - send our message */
@@ -260,7 +286,7 @@ static int cros_ec_command_spi_xfer(struct cros_ec_device *ec_dev,
 		ret = final_ret;
 	if (ret < 0) {
 		dev_err(ec_dev->dev, "spi transfer failed: %d\n", ret);
-		return ret;
+		goto exit;
 	}
 
 	/* check response error code */
@@ -269,14 +295,16 @@ static int cros_ec_command_spi_xfer(struct cros_ec_device *ec_dev,
 		dev_warn(ec_dev->dev, "command 0x%02x returned an error %d\n",
 			 ec_msg->cmd, ptr[0]);
 		debug_packet(ec_dev->dev, "in_err", ptr, len);
-		return -EINVAL;
+		ret = -EINVAL;
+		goto exit;
 	}
 	len = ptr[1];
 	sum = ptr[0] + ptr[1];
 	if (len > ec_msg->in_len) {
 		dev_err(ec_dev->dev, "packet too long (%d bytes, expected %d)",
 			len, ec_msg->in_len);
-		return -ENOSPC;
+		ret = -ENOSPC;
+		goto exit;
 	}
 
 	/* copy response packet payload and compute checksum */
@@ -293,10 +321,14 @@ static int cros_ec_command_spi_xfer(struct cros_ec_device *ec_dev,
 		dev_err(ec_dev->dev,
 			"bad packet checksum, expected %02x, got %02x\n",
 			sum, ptr[len + 2]);
-		return -EBADMSG;
+		ret = -EBADMSG;
+		goto exit;
 	}
 
-	return 0;
+	ret = 0;
+exit:
+	mutex_unlock(&ec_spi->lock);
+	return ret;
 }
 
 static void cros_ec_spi_dt_probe(struct cros_ec_spi *ec_spi, struct device *dev)
@@ -327,6 +359,7 @@ 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/db8500-prcmu.c b/drivers/mfd/db8500-prcmu.c
index 7694e0700d3..193cf168ba8 100644
--- a/drivers/mfd/db8500-prcmu.c
+++ b/drivers/mfd/db8500-prcmu.c
@@ -1734,18 +1734,17 @@ static struct cpufreq_frequency_table db8500_cpufreq_table[] = {
 
 static long round_armss_rate(unsigned long rate)
 {
+	struct cpufreq_frequency_table *pos;
 	long freq = 0;
-	int i = 0;
 
 	/* cpufreq table frequencies is in KHz. */
 	rate = rate / 1000;
 
 	/* Find the corresponding arm opp from the cpufreq table. */
-	while (db8500_cpufreq_table[i].frequency != CPUFREQ_TABLE_END) {
-		freq = db8500_cpufreq_table[i].frequency;
+	cpufreq_for_each_entry(pos, db8500_cpufreq_table) {
+		freq = pos->frequency;
 		if (freq == rate)
 			break;
-		i++;
 	}
 
 	/* Return the last valid value, even if a match was not found. */
@@ -1886,23 +1885,21 @@ static void set_clock_rate(u8 clock, unsigned long rate)
 
 static int set_armss_rate(unsigned long rate)
 {
-	int i = 0;
+	struct cpufreq_frequency_table *pos;
 
 	/* cpufreq table frequencies is in KHz. */
 	rate = rate / 1000;
 
 	/* Find the corresponding arm opp from the cpufreq table. */
-	while (db8500_cpufreq_table[i].frequency != CPUFREQ_TABLE_END) {
-		if (db8500_cpufreq_table[i].frequency == rate)
+	cpufreq_for_each_entry(pos, db8500_cpufreq_table)
+		if (pos->frequency == rate)
 			break;
-		i++;
-	}
 
-	if (db8500_cpufreq_table[i].frequency != rate)
+	if (pos->frequency != rate)
 		return -EINVAL;
 
 	/* Set the new arm opp. */
-	return db8500_prcmu_set_arm_opp(db8500_cpufreq_table[i].driver_data);
+	return db8500_prcmu_set_arm_opp(pos->driver_data);
 }
 
 static int set_plldsi_rate(unsigned long rate)
@@ -2303,9 +2300,6 @@ int prcmu_ac_wake_req(void)
 
 	if (!wait_for_completion_timeout(&mb0_transfer.ac_wake_work,
 			msecs_to_jiffies(5000))) {
-#if defined(CONFIG_DBX500_PRCMU_DEBUG)
-		db8500_prcmu_debug_dump(__func__, true, true);
-#endif
 		pr_crit("prcmu: %s timed out (5 s) waiting for a reply.\n",
 			__func__);
 		ret = -EFAULT;
@@ -3115,7 +3109,7 @@ static int db8500_prcmu_register_ab8500(struct device *parent,
 {
 	struct device_node *np;
 	struct resource ab8500_resource;
-	struct mfd_cell ab8500_cell = {
+	const struct mfd_cell ab8500_cell = {
 		.name = "ab8500-core",
 		.of_compatible = "stericsson,ab8500",
 		.id = AB8500_VERSION_AB8500,
diff --git a/drivers/mfd/ipaq-micro.c b/drivers/mfd/ipaq-micro.c
new file mode 100644
index 00000000000..7e50fe0118e
--- /dev/null
+++ b/drivers/mfd/ipaq-micro.c
@@ -0,0 +1,482 @@
+/*
+ * Compaq iPAQ h3xxx Atmel microcontroller companion support
+ *
+ * This is an Atmel AT90LS8535 with a special flashed-in firmware that
+ * implements the special protocol used by this driver.
+ *
+ * based on previous kernel 2.4 version by Andrew Christian
+ * Author : Alessandro Gardich <gremlin@gremlin.it>
+ * Author : Dmitry Artamonow <mad_soft@inbox.ru>
+ * 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/module.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/pm.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/ipaq-micro.h>
+#include <linux/string.h>
+#include <linux/random.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+
+#include <mach/hardware.h>
+
+static void ipaq_micro_trigger_tx(struct ipaq_micro *micro)
+{
+	struct ipaq_micro_txdev *tx = &micro->tx;
+	struct ipaq_micro_msg *msg = micro->msg;
+	int i, bp;
+	u8 checksum;
+	u32 val;
+
+	bp = 0;
+	tx->buf[bp++] = CHAR_SOF;
+
+	checksum = ((msg->id & 0x0f) << 4) | (msg->tx_len & 0x0f);
+	tx->buf[bp++] = checksum;
+
+	for (i = 0; i < msg->tx_len; i++) {
+		tx->buf[bp++] = msg->tx_data[i];
+		checksum += msg->tx_data[i];
+	}
+
+	tx->buf[bp++] = checksum;
+	tx->len = bp;
+	tx->index = 0;
+	print_hex_dump(KERN_DEBUG, "data: ", DUMP_PREFIX_OFFSET, 16, 1,
+		       tx->buf, tx->len, true);
+
+	/* Enable interrupt */
+	val = readl(micro->base + UTCR3);
+	val |= UTCR3_TIE;
+	writel(val, micro->base + UTCR3);
+}
+
+int ipaq_micro_tx_msg(struct ipaq_micro *micro, struct ipaq_micro_msg *msg)
+{
+	unsigned long flags;
+
+	dev_dbg(micro->dev, "TX msg: %02x, %d bytes\n", msg->id, msg->tx_len);
+
+	spin_lock_irqsave(&micro->lock, flags);
+	if (micro->msg) {
+		list_add_tail(&msg->node, &micro->queue);
+		spin_unlock_irqrestore(&micro->lock, flags);
+		return 0;
+	}
+	micro->msg = msg;
+	ipaq_micro_trigger_tx(micro);
+	spin_unlock_irqrestore(&micro->lock, flags);
+	return 0;
+}
+EXPORT_SYMBOL(ipaq_micro_tx_msg);
+
+static void micro_rx_msg(struct ipaq_micro *micro, u8 id, int len, u8 *data)
+{
+	int i;
+
+	dev_dbg(micro->dev, "RX msg: %02x, %d bytes\n", id, len);
+
+	spin_lock(&micro->lock);
+	switch (id) {
+	case MSG_VERSION:
+	case MSG_EEPROM_READ:
+	case MSG_EEPROM_WRITE:
+	case MSG_BACKLIGHT:
+	case MSG_NOTIFY_LED:
+	case MSG_THERMAL_SENSOR:
+	case MSG_BATTERY:
+		/* Handle synchronous messages */
+		if (micro->msg && micro->msg->id == id) {
+			struct ipaq_micro_msg *msg = micro->msg;
+
+			memcpy(msg->rx_data, data, len);
+			msg->rx_len = len;
+			complete(&micro->msg->ack);
+			if (!list_empty(&micro->queue)) {
+				micro->msg = list_entry(micro->queue.next,
+							struct ipaq_micro_msg,
+							node);
+				list_del_init(&micro->msg->node);
+				ipaq_micro_trigger_tx(micro);
+			} else
+				micro->msg = NULL;
+			dev_dbg(micro->dev, "OK RX message 0x%02x\n", id);
+		} else {
+			dev_err(micro->dev,
+				"out of band RX message 0x%02x\n", id);
+			if(!micro->msg)
+				dev_info(micro->dev, "no message queued\n");
+			else
+				dev_info(micro->dev, "expected message %02x\n",
+					 micro->msg->id);
+		}
+		break;
+	case MSG_KEYBOARD:
+		if (micro->key)
+			micro->key(micro->key_data, len, data);
+		else
+			dev_dbg(micro->dev, "key message ignored, no handle \n");
+		break;
+	case MSG_TOUCHSCREEN:
+		if (micro->ts)
+			micro->ts(micro->ts_data, len, data);
+		else
+			dev_dbg(micro->dev, "touchscreen message ignored, no handle \n");
+		break;
+	default:
+		dev_err(micro->dev,
+			"unknown msg %d [%d] ", id, len);
+		for (i = 0; i < len; ++i)
+			pr_cont("0x%02x ", data[i]);
+		pr_cont("\n");
+	}
+	spin_unlock(&micro->lock);
+}
+
+static void micro_process_char(struct ipaq_micro *micro, u8 ch)
+{
+	struct ipaq_micro_rxdev *rx = &micro->rx;
+
+	switch (rx->state) {
+	case STATE_SOF:	/* Looking for SOF */
+		if (ch == CHAR_SOF)
+			rx->state = STATE_ID; /* Next byte is the id and len */
+		break;
+	case STATE_ID: /* Looking for id and len byte */
+		rx->id = (ch & 0xf0) >> 4 ;
+		rx->len = (ch & 0x0f);
+		rx->index = 0;
+		rx->chksum = ch;
+		rx->state = (rx->len > 0) ? STATE_DATA : STATE_CHKSUM;
+		break;
+	case STATE_DATA: /* Looking for 'len' data bytes */
+		rx->chksum += ch;
+		rx->buf[rx->index] = ch;
+		if (++rx->index == rx->len)
+			rx->state = STATE_CHKSUM;
+		break;
+	case STATE_CHKSUM: /* Looking for the checksum */
+		if (ch == rx->chksum)
+			micro_rx_msg(micro, rx->id, rx->len, rx->buf);
+		rx->state = STATE_SOF;
+		break;
+	}
+}
+
+static void micro_rx_chars(struct ipaq_micro *micro)
+{
+	u32 status, ch;
+
+	while ((status = readl(micro->base + UTSR1)) & UTSR1_RNE) {
+		ch = readl(micro->base + UTDR);
+		if (status & UTSR1_PRE)
+			dev_err(micro->dev, "rx: parity error\n");
+		else if (status & UTSR1_FRE)
+			dev_err(micro->dev, "rx: framing error\n");
+		else if (status & UTSR1_ROR)
+			dev_err(micro->dev, "rx: overrun error\n");
+		micro_process_char(micro, ch);
+	}
+}
+
+static void ipaq_micro_get_version(struct ipaq_micro *micro)
+{
+	struct ipaq_micro_msg msg = {
+		.id = MSG_VERSION,
+	};
+
+	ipaq_micro_tx_msg_sync(micro, &msg);
+	if (msg.rx_len == 4) {
+		memcpy(micro->version, msg.rx_data, 4);
+		micro->version[4] = '\0';
+	} else if (msg.rx_len == 9) {
+		memcpy(micro->version, msg.rx_data, 4);
+		micro->version[4] = '\0';
+		/* Bytes 4-7 are "pack", byte 8 is "boot type" */
+	} else {
+		dev_err(micro->dev,
+			"illegal version message %d bytes\n", msg.rx_len);
+	}
+}
+
+static void ipaq_micro_eeprom_read(struct ipaq_micro *micro,
+				   u8 address, u8 len, u8 *data)
+{
+	struct ipaq_micro_msg msg = {
+		.id = MSG_EEPROM_READ,
+	};
+	u8 i;
+
+	for (i = 0; i < len; i++) {
+		msg.tx_data[0] = address + i;
+		msg.tx_data[1] = 1;
+		msg.tx_len = 2;
+		ipaq_micro_tx_msg_sync(micro, &msg);
+		memcpy(data + (i * 2), msg.rx_data, 2);
+	}
+}
+
+static char *ipaq_micro_str(u8 *wchar, u8 len)
+{
+	char retstr[256];
+	u8 i;
+
+	for (i = 0; i < len / 2; i++)
+		retstr[i] = wchar[i * 2];
+	return kstrdup(retstr, GFP_KERNEL);
+}
+
+static u16 ipaq_micro_to_u16(u8 *data)
+{
+	return data[1] << 8 | data[0];
+}
+
+static void ipaq_micro_eeprom_dump(struct ipaq_micro *micro)
+{
+	u8 dump[256];
+	char *str;
+
+	ipaq_micro_eeprom_read(micro, 0, 128, dump);
+	str = ipaq_micro_str(dump, 10);
+	if (str) {
+		dev_info(micro->dev, "HM version %s\n", str);
+		kfree(str);
+	}
+	str = ipaq_micro_str(dump+10, 40);
+	if (str) {
+		dev_info(micro->dev, "serial number: %s\n", str);
+		/* Feed the random pool with this */
+		add_device_randomness(str, strlen(str));
+		kfree(str);
+	}
+	str = ipaq_micro_str(dump+50, 20);
+	if (str) {
+		dev_info(micro->dev, "module ID: %s\n", str);
+		kfree(str);
+	}
+	str = ipaq_micro_str(dump+70, 10);
+	if (str) {
+		dev_info(micro->dev, "product revision: %s\n", str);
+		kfree(str);
+	}
+	dev_info(micro->dev, "product ID: %u\n", ipaq_micro_to_u16(dump+80));
+	dev_info(micro->dev, "frame rate: %u fps\n",
+		 ipaq_micro_to_u16(dump+82));
+	dev_info(micro->dev, "page mode: %u\n", ipaq_micro_to_u16(dump+84));
+	dev_info(micro->dev, "country ID: %u\n", ipaq_micro_to_u16(dump+86));
+	dev_info(micro->dev, "color display: %s\n",
+		 ipaq_micro_to_u16(dump+88) ? "yes" : "no");
+	dev_info(micro->dev, "ROM size: %u MiB\n", ipaq_micro_to_u16(dump+90));
+	dev_info(micro->dev, "RAM size: %u KiB\n", ipaq_micro_to_u16(dump+92));
+	dev_info(micro->dev, "screen: %u x %u\n",
+		 ipaq_micro_to_u16(dump+94), ipaq_micro_to_u16(dump+96));
+	print_hex_dump(KERN_DEBUG, "eeprom: ", DUMP_PREFIX_OFFSET, 16, 1,
+		       dump, 256, true);
+
+}
+
+static void micro_tx_chars(struct ipaq_micro *micro)
+{
+	struct ipaq_micro_txdev *tx = &micro->tx;
+	u32 val;
+
+	while ((tx->index < tx->len) &&
+	       (readl(micro->base + UTSR1) & UTSR1_TNF)) {
+		writel(tx->buf[tx->index], micro->base + UTDR);
+		tx->index++;
+	}
+
+	/* Stop interrupts */
+	val = readl(micro->base + UTCR3);
+	val &= ~UTCR3_TIE;
+	writel(val, micro->base + UTCR3);
+}
+
+static void micro_reset_comm(struct ipaq_micro *micro)
+{
+	struct ipaq_micro_rxdev *rx = &micro->rx;
+	u32 val;
+
+	if (micro->msg)
+		complete(&micro->msg->ack);
+
+	/* Initialize Serial channel protocol frame */
+	rx->state = STATE_SOF;  /* Reset the state machine */
+
+	/* Set up interrupts */
+	writel(0x01, micro->sdlc + 0x0); /* Select UART mode */
+
+	/* Clean up CR3 */
+	writel(0x0, micro->base + UTCR3);
+
+	/* Format: 8N1 */
+	writel(UTCR0_8BitData | UTCR0_1StpBit, micro->base + UTCR0);
+
+	/* Baud rate: 115200 */
+	writel(0x0, micro->base + UTCR1);
+	writel(0x1, micro->base + UTCR2);
+
+	/* Clear SR0 */
+	writel(0xff, micro->base + UTSR0);
+
+	/* Enable RX int, disable TX int */
+	writel(UTCR3_TXE | UTCR3_RXE | UTCR3_RIE, micro->base + UTCR3);
+	val = readl(micro->base + UTCR3);
+	val &= ~UTCR3_TIE;
+	writel(val, micro->base + UTCR3);
+}
+
+static irqreturn_t micro_serial_isr(int irq, void *dev_id)
+{
+	struct ipaq_micro *micro = dev_id;
+	struct ipaq_micro_txdev *tx = &micro->tx;
+	u32 status;
+
+	status = readl(micro->base + UTSR0);
+	do {
+		if (status & (UTSR0_RID | UTSR0_RFS)) {
+			if (status & UTSR0_RID)
+				/* Clear the Receiver IDLE bit */
+				writel(UTSR0_RID, micro->base + UTSR0);
+			micro_rx_chars(micro);
+		}
+
+		/* Clear break bits */
+		if (status & (UTSR0_RBB | UTSR0_REB))
+			writel(status & (UTSR0_RBB | UTSR0_REB),
+			       micro->base + UTSR0);
+
+		if (status & UTSR0_TFS)
+			micro_tx_chars(micro);
+
+		status = readl(micro->base + UTSR0);
+
+	} while (((tx->index < tx->len) && (status & UTSR0_TFS)) ||
+		 (status & (UTSR0_RFS | UTSR0_RID)));
+
+	return IRQ_HANDLED;
+}
+
+static const struct mfd_cell micro_cells[] = {
+	{ .name = "ipaq-micro-backlight", },
+	{ .name = "ipaq-micro-battery", },
+	{ .name = "ipaq-micro-keys", },
+	{ .name = "ipaq-micro-ts", },
+	{ .name = "ipaq-micro-leds", },
+};
+
+static int micro_resume(struct device *dev)
+{
+	struct ipaq_micro *micro = dev_get_drvdata(dev);
+
+	micro_reset_comm(micro);
+	mdelay(10);
+
+	return 0;
+}
+
+static int micro_probe(struct platform_device *pdev)
+{
+	struct ipaq_micro *micro;
+	struct resource *res;
+	int ret;
+	int irq;
+
+	micro = devm_kzalloc(&pdev->dev, sizeof(*micro), GFP_KERNEL);
+	if (!micro)
+		return -ENOMEM;
+
+	micro->dev = &pdev->dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -EINVAL;
+
+	micro->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(micro->base))
+		return PTR_ERR(micro->base);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (!res)
+		return -EINVAL;
+
+	micro->sdlc = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(micro->sdlc))
+		return PTR_ERR(micro->sdlc);
+
+	micro_reset_comm(micro);
+
+	irq = platform_get_irq(pdev, 0);
+	if (!irq)
+		return -EINVAL;
+	ret = devm_request_irq(&pdev->dev, irq, micro_serial_isr,
+			       IRQF_SHARED, "ipaq-micro",
+			       micro);
+	if (ret) {
+		dev_err(&pdev->dev, "unable to grab serial port IRQ\n");
+		return ret;
+	} else
+		dev_info(&pdev->dev, "grabbed serial port IRQ\n");
+
+	spin_lock_init(&micro->lock);
+	INIT_LIST_HEAD(&micro->queue);
+	platform_set_drvdata(pdev, micro);
+
+	ret = mfd_add_devices(&pdev->dev, pdev->id, micro_cells,
+			      ARRAY_SIZE(micro_cells), NULL, 0, NULL);
+	if (ret) {
+		dev_err(&pdev->dev, "error adding MFD cells");
+		return ret;
+	}
+
+	/* Check version */
+	ipaq_micro_get_version(micro);
+	dev_info(&pdev->dev, "Atmel micro ASIC version %s\n", micro->version);
+	ipaq_micro_eeprom_dump(micro);
+
+	return 0;
+}
+
+static int micro_remove(struct platform_device *pdev)
+{
+	struct ipaq_micro *micro = platform_get_drvdata(pdev);
+	u32 val;
+
+	mfd_remove_devices(&pdev->dev);
+
+	val = readl(micro->base + UTCR3);
+	val &= ~(UTCR3_RXE | UTCR3_RIE); /* disable receive interrupt */
+	val &= ~(UTCR3_TXE | UTCR3_TIE); /* disable transmit interrupt */
+	writel(val, micro->base + UTCR3);
+
+	return 0;
+}
+
+static const struct dev_pm_ops micro_dev_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(NULL, micro_resume)
+};
+
+static struct platform_driver micro_device_driver = {
+	.driver   = {
+		.name	= "ipaq-h3xxx-micro",
+		.pm	= &micro_dev_pm_ops,
+	},
+	.probe    = micro_probe,
+	.remove   = micro_remove,
+	/* .shutdown = micro_suspend, // FIXME */
+};
+module_platform_driver(micro_device_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("driver for iPAQ Atmel micro core and backlight");
diff --git a/drivers/mfd/kempld-core.c b/drivers/mfd/kempld-core.c
index 07692604e11..f7ff0188603 100644
--- a/drivers/mfd/kempld-core.c
+++ b/drivers/mfd/kempld-core.c
@@ -86,7 +86,7 @@ enum kempld_cells {
 	KEMPLD_UART,
 };
 
-static struct mfd_cell kempld_devs[] = {
+static const struct mfd_cell kempld_devs[] = {
 	[KEMPLD_I2C] = {
 		.name = "kempld-i2c",
 	},
@@ -288,9 +288,38 @@ EXPORT_SYMBOL_GPL(kempld_release_mutex);
  */
 static int kempld_get_info(struct kempld_device_data *pld)
 {
+	int ret;
 	struct kempld_platform_data *pdata = dev_get_platdata(pld->dev);
+	char major, minor;
+
+	ret = pdata->get_info(pld);
+	if (ret)
+		return ret;
+
+	/* The Kontron PLD firmware version string has the following format:
+	 * Pwxy.zzzz
+	 *   P:    Fixed
+	 *   w:    PLD number    - 1 hex digit
+	 *   x:    Major version - 1 alphanumerical digit (0-9A-V)
+	 *   y:    Minor version - 1 alphanumerical digit (0-9A-V)
+	 *   zzzz: Build number  - 4 zero padded hex digits */
 
-	return pdata->get_info(pld);
+	if (pld->info.major < 10)
+		major = pld->info.major + '0';
+	else
+		major = (pld->info.major - 10) + 'A';
+	if (pld->info.minor < 10)
+		minor = pld->info.minor + '0';
+	else
+		minor = (pld->info.minor - 10) + 'A';
+
+	ret = scnprintf(pld->info.version, sizeof(pld->info.version),
+			"P%X%c%c.%04X", pld->info.number, major, minor,
+			pld->info.buildnr);
+	if (ret < 0)
+		return ret;
+
+	return 0;
 }
 
 /*
@@ -307,9 +336,71 @@ static int kempld_register_cells(struct kempld_device_data *pld)
 	return pdata->register_cells(pld);
 }
 
+static const char *kempld_get_type_string(struct kempld_device_data *pld)
+{
+	const char *version_type;
+
+	switch (pld->info.type) {
+	case 0:
+		version_type = "release";
+		break;
+	case 1:
+		version_type = "debug";
+		break;
+	case 2:
+		version_type = "custom";
+		break;
+	default:
+		version_type = "unspecified";
+		break;
+	}
+
+	return version_type;
+}
+
+static ssize_t kempld_version_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct kempld_device_data *pld = dev_get_drvdata(dev);
+
+	return scnprintf(buf, PAGE_SIZE, "%s\n", pld->info.version);
+}
+
+static ssize_t kempld_specification_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct kempld_device_data *pld = dev_get_drvdata(dev);
+
+	return scnprintf(buf, PAGE_SIZE, "%d.%d\n", pld->info.spec_major,
+		       pld->info.spec_minor);
+}
+
+static ssize_t kempld_type_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct kempld_device_data *pld = dev_get_drvdata(dev);
+
+	return scnprintf(buf, PAGE_SIZE, "%s\n", kempld_get_type_string(pld));
+}
+
+static DEVICE_ATTR(pld_version, S_IRUGO, kempld_version_show, NULL);
+static DEVICE_ATTR(pld_specification, S_IRUGO, kempld_specification_show,
+		   NULL);
+static DEVICE_ATTR(pld_type, S_IRUGO, kempld_type_show, NULL);
+
+static struct attribute *pld_attributes[] = {
+	&dev_attr_pld_version.attr,
+	&dev_attr_pld_specification.attr,
+	&dev_attr_pld_type.attr,
+	NULL
+};
+
+static const struct attribute_group pld_attr_group = {
+	.attrs = pld_attributes,
+};
+
 static int kempld_detect_device(struct kempld_device_data *pld)
 {
-	char *version_type;
 	u8 index_reg;
 	int ret;
 
@@ -335,27 +426,19 @@ static int kempld_detect_device(struct kempld_device_data *pld)
 	if (ret)
 		return ret;
 
-	switch (pld->info.type) {
-	case 0:
-		version_type = "release";
-		break;
-	case 1:
-		version_type = "debug";
-		break;
-	case 2:
-		version_type = "custom";
-		break;
-	default:
-		version_type = "unspecified";
-	}
+	dev_info(pld->dev, "Found Kontron PLD - %s (%s), spec %d.%d\n",
+		 pld->info.version, kempld_get_type_string(pld),
+		 pld->info.spec_major, pld->info.spec_minor);
+
+	ret = sysfs_create_group(&pld->dev->kobj, &pld_attr_group);
+	if (ret)
+		return ret;
 
-	dev_info(pld->dev, "Found Kontron PLD %d\n", pld->info.number);
-	dev_info(pld->dev, "%s version %d.%d build %d, specification %d.%d\n",
-		 version_type, pld->info.major, pld->info.minor,
-		 pld->info.buildnr, pld->info.spec_major,
-		 pld->info.spec_minor);
+	ret = kempld_register_cells(pld);
+	if (ret)
+		sysfs_remove_group(&pld->dev->kobj, &pld_attr_group);
 
-	return kempld_register_cells(pld);
+	return ret;
 }
 
 static int kempld_probe(struct platform_device *pdev)
@@ -399,6 +482,8 @@ static int kempld_remove(struct platform_device *pdev)
 	struct kempld_device_data *pld = platform_get_drvdata(pdev);
 	struct kempld_platform_data *pdata = dev_get_platdata(pld->dev);
 
+	sysfs_remove_group(&pld->dev->kobj, &pld_attr_group);
+
 	mfd_remove_devices(&pdev->dev);
 	pdata->release_hardware_mutex(pld);
 
diff --git a/drivers/mfd/lp3943.c b/drivers/mfd/lp3943.c
index e32226836fb..335b930112b 100644
--- a/drivers/mfd/lp3943.c
+++ b/drivers/mfd/lp3943.c
@@ -62,7 +62,7 @@ static const struct lp3943_reg_cfg lp3943_mux_cfg[] = {
 	{ LP3943_REG_MUX3, 0xC0, 6 },
 };
 
-static struct mfd_cell lp3943_devs[] = {
+static const struct mfd_cell lp3943_devs[] = {
 	{
 		.name = "lp3943-pwm",
 		.of_compatible = "ti,lp3943-pwm",
diff --git a/drivers/mfd/lpc_ich.c b/drivers/mfd/lpc_ich.c
index 3f10ea3f45d..7d8482ff586 100644
--- a/drivers/mfd/lpc_ich.c
+++ b/drivers/mfd/lpc_ich.c
@@ -488,6 +488,7 @@ static struct lpc_ich_info lpc_chipset_info[] = {
 	[LPC_PPT] = {
 		.name = "Panther Point",
 		.iTCO_version = 2,
+		.gpio_version = ICH_V5_GPIO,
 	},
 	[LPC_LPT] = {
 		.name = "Lynx Point",
diff --git a/drivers/mfd/max14577.c b/drivers/mfd/max14577.c
index 5f13cefe8de..4a5e885383f 100644
--- a/drivers/mfd/max14577.c
+++ b/drivers/mfd/max14577.c
@@ -1,7 +1,7 @@
 /*
- * max14577.c - mfd core driver for the Maxim 14577
+ * max14577.c - mfd core driver for the Maxim 14577/77836
  *
- * Copyright (C) 2013 Samsung Electrnoics
+ * Copyright (C) 2014 Samsung Electrnoics
  * Chanwoo Choi <cw00.choi@samsung.com>
  * Krzysztof Kozlowski <k.kozlowski@samsung.com>
  *
@@ -21,11 +21,12 @@
 #include <linux/err.h>
 #include <linux/module.h>
 #include <linux/interrupt.h>
+#include <linux/of_device.h>
 #include <linux/mfd/core.h>
 #include <linux/mfd/max14577.h>
 #include <linux/mfd/max14577-private.h>
 
-static struct mfd_cell max14577_devs[] = {
+static const struct mfd_cell max14577_devs[] = {
 	{
 		.name = "max14577-muic",
 		.of_compatible = "maxim,max14577-muic",
@@ -37,7 +38,38 @@ static struct mfd_cell max14577_devs[] = {
 	{ .name = "max14577-charger", },
 };
 
-static bool max14577_volatile_reg(struct device *dev, unsigned int reg)
+static const struct mfd_cell max77836_devs[] = {
+	{
+		.name = "max77836-muic",
+		.of_compatible = "maxim,max77836-muic",
+	},
+	{
+		.name = "max77836-regulator",
+		.of_compatible = "maxim,max77836-regulator",
+	},
+	{
+		.name = "max77836-charger",
+		.of_compatible = "maxim,max77836-charger",
+	},
+	{
+		.name = "max77836-battery",
+		.of_compatible = "maxim,max77836-battery",
+	},
+};
+
+static const struct of_device_id max14577_dt_match[] = {
+	{
+		.compatible = "maxim,max14577",
+		.data = (void *)MAXIM_DEVICE_TYPE_MAX14577,
+	},
+	{
+		.compatible = "maxim,max77836",
+		.data = (void *)MAXIM_DEVICE_TYPE_MAX77836,
+	},
+	{},
+};
+
+static bool max14577_muic_volatile_reg(struct device *dev, unsigned int reg)
 {
 	switch (reg) {
 	case MAX14577_REG_INT1 ... MAX14577_REG_STATUS3:
@@ -48,49 +80,221 @@ static bool max14577_volatile_reg(struct device *dev, unsigned int reg)
 	return false;
 }
 
-static const struct regmap_config max14577_regmap_config = {
+static bool max77836_muic_volatile_reg(struct device *dev, unsigned int reg)
+{
+	/* Any max14577 volatile registers are also max77836 volatile. */
+	if (max14577_muic_volatile_reg(dev, reg))
+		return true;
+
+	switch (reg) {
+	case MAX77836_FG_REG_VCELL_MSB ... MAX77836_FG_REG_SOC_LSB:
+	case MAX77836_FG_REG_CRATE_MSB ... MAX77836_FG_REG_CRATE_LSB:
+	case MAX77836_FG_REG_STATUS_H ... MAX77836_FG_REG_STATUS_L:
+	case MAX77836_PMIC_REG_INTSRC:
+	case MAX77836_PMIC_REG_TOPSYS_INT:
+	case MAX77836_PMIC_REG_TOPSYS_STAT:
+		return true;
+	default:
+		break;
+	}
+	return false;
+}
+
+static const struct regmap_config max14577_muic_regmap_config = {
 	.reg_bits	= 8,
 	.val_bits	= 8,
-	.volatile_reg	= max14577_volatile_reg,
+	.volatile_reg	= max14577_muic_volatile_reg,
 	.max_register	= MAX14577_REG_END,
 };
 
+static const struct regmap_config max77836_pmic_regmap_config = {
+	.reg_bits	= 8,
+	.val_bits	= 8,
+	.volatile_reg	= max77836_muic_volatile_reg,
+	.max_register	= MAX77836_PMIC_REG_END,
+};
+
 static const struct regmap_irq max14577_irqs[] = {
 	/* INT1 interrupts */
-	{ .reg_offset = 0, .mask = INT1_ADC_MASK, },
-	{ .reg_offset = 0, .mask = INT1_ADCLOW_MASK, },
-	{ .reg_offset = 0, .mask = INT1_ADCERR_MASK, },
+	{ .reg_offset = 0, .mask = MAX14577_INT1_ADC_MASK, },
+	{ .reg_offset = 0, .mask = MAX14577_INT1_ADCLOW_MASK, },
+	{ .reg_offset = 0, .mask = MAX14577_INT1_ADCERR_MASK, },
 	/* INT2 interrupts */
-	{ .reg_offset = 1, .mask = INT2_CHGTYP_MASK, },
-	{ .reg_offset = 1, .mask = INT2_CHGDETRUN_MASK, },
-	{ .reg_offset = 1, .mask = INT2_DCDTMR_MASK, },
-	{ .reg_offset = 1, .mask = INT2_DBCHG_MASK, },
-	{ .reg_offset = 1, .mask = INT2_VBVOLT_MASK, },
+	{ .reg_offset = 1, .mask = MAX14577_INT2_CHGTYP_MASK, },
+	{ .reg_offset = 1, .mask = MAX14577_INT2_CHGDETRUN_MASK, },
+	{ .reg_offset = 1, .mask = MAX14577_INT2_DCDTMR_MASK, },
+	{ .reg_offset = 1, .mask = MAX14577_INT2_DBCHG_MASK, },
+	{ .reg_offset = 1, .mask = MAX14577_INT2_VBVOLT_MASK, },
 	/* INT3 interrupts */
-	{ .reg_offset = 2, .mask = INT3_EOC_MASK, },
-	{ .reg_offset = 2, .mask = INT3_CGMBC_MASK, },
-	{ .reg_offset = 2, .mask = INT3_OVP_MASK, },
-	{ .reg_offset = 2, .mask = INT3_MBCCHGERR_MASK, },
+	{ .reg_offset = 2, .mask = MAX14577_INT3_EOC_MASK, },
+	{ .reg_offset = 2, .mask = MAX14577_INT3_CGMBC_MASK, },
+	{ .reg_offset = 2, .mask = MAX14577_INT3_OVP_MASK, },
+	{ .reg_offset = 2, .mask = MAX14577_INT3_MBCCHGERR_MASK, },
 };
 
 static const struct regmap_irq_chip max14577_irq_chip = {
 	.name			= "max14577",
 	.status_base		= MAX14577_REG_INT1,
 	.mask_base		= MAX14577_REG_INTMASK1,
-	.mask_invert		= 1,
+	.mask_invert		= true,
 	.num_regs		= 3,
 	.irqs			= max14577_irqs,
 	.num_irqs		= ARRAY_SIZE(max14577_irqs),
 };
 
+static const struct regmap_irq max77836_muic_irqs[] = {
+	/* INT1 interrupts */
+	{ .reg_offset = 0, .mask = MAX14577_INT1_ADC_MASK, },
+	{ .reg_offset = 0, .mask = MAX14577_INT1_ADCLOW_MASK, },
+	{ .reg_offset = 0, .mask = MAX14577_INT1_ADCERR_MASK, },
+	{ .reg_offset = 0, .mask = MAX77836_INT1_ADC1K_MASK, },
+	/* INT2 interrupts */
+	{ .reg_offset = 1, .mask = MAX14577_INT2_CHGTYP_MASK, },
+	{ .reg_offset = 1, .mask = MAX14577_INT2_CHGDETRUN_MASK, },
+	{ .reg_offset = 1, .mask = MAX14577_INT2_DCDTMR_MASK, },
+	{ .reg_offset = 1, .mask = MAX14577_INT2_DBCHG_MASK, },
+	{ .reg_offset = 1, .mask = MAX14577_INT2_VBVOLT_MASK, },
+	{ .reg_offset = 1, .mask = MAX77836_INT2_VIDRM_MASK, },
+	/* INT3 interrupts */
+	{ .reg_offset = 2, .mask = MAX14577_INT3_EOC_MASK, },
+	{ .reg_offset = 2, .mask = MAX14577_INT3_CGMBC_MASK, },
+	{ .reg_offset = 2, .mask = MAX14577_INT3_OVP_MASK, },
+	{ .reg_offset = 2, .mask = MAX14577_INT3_MBCCHGERR_MASK, },
+};
+
+static const struct regmap_irq_chip max77836_muic_irq_chip = {
+	.name			= "max77836-muic",
+	.status_base		= MAX14577_REG_INT1,
+	.mask_base		= MAX14577_REG_INTMASK1,
+	.mask_invert		= true,
+	.num_regs		= 3,
+	.irqs			= max77836_muic_irqs,
+	.num_irqs		= ARRAY_SIZE(max77836_muic_irqs),
+};
+
+static const struct regmap_irq max77836_pmic_irqs[] = {
+	{ .reg_offset = 0, .mask = MAX77836_TOPSYS_INT_T120C_MASK, },
+	{ .reg_offset = 0, .mask = MAX77836_TOPSYS_INT_T140C_MASK, },
+};
+
+static const struct regmap_irq_chip max77836_pmic_irq_chip = {
+	.name			= "max77836-pmic",
+	.status_base		= MAX77836_PMIC_REG_TOPSYS_INT,
+	.mask_base		= MAX77836_PMIC_REG_TOPSYS_INT_MASK,
+	.mask_invert		= false,
+	.num_regs		= 1,
+	.irqs			= max77836_pmic_irqs,
+	.num_irqs		= ARRAY_SIZE(max77836_pmic_irqs),
+};
+
+static void max14577_print_dev_type(struct max14577 *max14577)
+{
+	u8 reg_data, vendor_id, device_id;
+	int ret;
+
+	ret = max14577_read_reg(max14577->regmap, MAX14577_REG_DEVICEID,
+			&reg_data);
+	if (ret) {
+		dev_err(max14577->dev,
+			"Failed to read DEVICEID register: %d\n", ret);
+		return;
+	}
+
+	vendor_id = ((reg_data & DEVID_VENDORID_MASK) >>
+				DEVID_VENDORID_SHIFT);
+	device_id = ((reg_data & DEVID_DEVICEID_MASK) >>
+				DEVID_DEVICEID_SHIFT);
+
+	dev_info(max14577->dev, "Device type: %u (ID: 0x%x, vendor: 0x%x)\n",
+			max14577->dev_type, device_id, vendor_id);
+}
+
+/*
+ * Max77836 specific initialization code for driver probe.
+ * Adds new I2C dummy device, regmap and regmap IRQ chip.
+ * Unmasks Interrupt Source register.
+ *
+ * On success returns 0.
+ * On failure returns errno and reverts any changes done so far (e.g. remove
+ * I2C dummy device), except masking the INT SRC register.
+ */
+static int max77836_init(struct max14577 *max14577)
+{
+	int ret;
+	u8 intsrc_mask;
+
+	max14577->i2c_pmic = i2c_new_dummy(max14577->i2c->adapter,
+			I2C_ADDR_PMIC);
+	if (!max14577->i2c_pmic) {
+		dev_err(max14577->dev, "Failed to register PMIC I2C device\n");
+		return -ENODEV;
+	}
+	i2c_set_clientdata(max14577->i2c_pmic, max14577);
+
+	max14577->regmap_pmic = devm_regmap_init_i2c(max14577->i2c_pmic,
+			&max77836_pmic_regmap_config);
+	if (IS_ERR(max14577->regmap_pmic)) {
+		ret = PTR_ERR(max14577->regmap_pmic);
+		dev_err(max14577->dev, "Failed to allocate PMIC register map: %d\n",
+				ret);
+		goto err;
+	}
+
+	/* Un-mask MAX77836 Interrupt Source register */
+	ret = max14577_read_reg(max14577->regmap_pmic,
+			MAX77836_PMIC_REG_INTSRC_MASK, &intsrc_mask);
+	if (ret < 0) {
+		dev_err(max14577->dev, "Failed to read PMIC register\n");
+		goto err;
+	}
+
+	intsrc_mask &= ~(MAX77836_INTSRC_MASK_TOP_INT_MASK);
+	intsrc_mask &= ~(MAX77836_INTSRC_MASK_MUIC_CHG_INT_MASK);
+	ret = max14577_write_reg(max14577->regmap_pmic,
+			MAX77836_PMIC_REG_INTSRC_MASK, intsrc_mask);
+	if (ret < 0) {
+		dev_err(max14577->dev, "Failed to write PMIC register\n");
+		goto err;
+	}
+
+	ret = regmap_add_irq_chip(max14577->regmap_pmic, max14577->irq,
+			IRQF_TRIGGER_FALLING | IRQF_ONESHOT | IRQF_SHARED,
+			0, &max77836_pmic_irq_chip,
+			&max14577->irq_data_pmic);
+	if (ret != 0) {
+		dev_err(max14577->dev, "Failed to request PMIC IRQ %d: %d\n",
+				max14577->irq, ret);
+		goto err;
+	}
+
+	return 0;
+
+err:
+	i2c_unregister_device(max14577->i2c_pmic);
+
+	return ret;
+}
+
+/*
+ * Max77836 specific de-initialization code for driver remove.
+ */
+static void max77836_remove(struct max14577 *max14577)
+{
+	regmap_del_irq_chip(max14577->irq, max14577->irq_data_pmic);
+	i2c_unregister_device(max14577->i2c_pmic);
+}
+
 static int max14577_i2c_probe(struct i2c_client *i2c,
 			      const struct i2c_device_id *id)
 {
 	struct max14577 *max14577;
 	struct max14577_platform_data *pdata = dev_get_platdata(&i2c->dev);
 	struct device_node *np = i2c->dev.of_node;
-	u8 reg_data;
 	int ret = 0;
+	const struct regmap_irq_chip *irq_chip;
+	const struct mfd_cell *mfd_devs;
+	unsigned int mfd_devs_size;
+	int irq_flags;
 
 	if (np) {
 		pdata = devm_kzalloc(&i2c->dev, sizeof(*pdata), GFP_KERNEL);
@@ -113,7 +317,8 @@ static int max14577_i2c_probe(struct i2c_client *i2c,
 	max14577->i2c = i2c;
 	max14577->irq = i2c->irq;
 
-	max14577->regmap = devm_regmap_init_i2c(i2c, &max14577_regmap_config);
+	max14577->regmap = devm_regmap_init_i2c(i2c,
+			&max14577_muic_regmap_config);
 	if (IS_ERR(max14577->regmap)) {
 		ret = PTR_ERR(max14577->regmap);
 		dev_err(max14577->dev, "Failed to allocate register map: %d\n",
@@ -121,23 +326,37 @@ static int max14577_i2c_probe(struct i2c_client *i2c,
 		return ret;
 	}
 
-	ret = max14577_read_reg(max14577->regmap, MAX14577_REG_DEVICEID,
-			&reg_data);
-	if (ret) {
-		dev_err(max14577->dev, "Device not found on this channel: %d\n",
-				ret);
-		return ret;
+	if (np) {
+		const struct of_device_id *of_id;
+
+		of_id = of_match_device(max14577_dt_match, &i2c->dev);
+		if (of_id)
+			max14577->dev_type =
+				(enum maxim_device_type)of_id->data;
+	} else {
+		max14577->dev_type = id->driver_data;
+	}
+
+	max14577_print_dev_type(max14577);
+
+	switch (max14577->dev_type) {
+	case MAXIM_DEVICE_TYPE_MAX77836:
+		irq_chip = &max77836_muic_irq_chip;
+		mfd_devs = max77836_devs;
+		mfd_devs_size = ARRAY_SIZE(max77836_devs);
+		irq_flags = IRQF_TRIGGER_FALLING | IRQF_ONESHOT | IRQF_SHARED;
+		break;
+	case MAXIM_DEVICE_TYPE_MAX14577:
+	default:
+		irq_chip = &max14577_irq_chip;
+		mfd_devs = max14577_devs;
+		mfd_devs_size = ARRAY_SIZE(max14577_devs);
+		irq_flags = IRQF_TRIGGER_FALLING | IRQF_ONESHOT;
+		break;
 	}
-	max14577->vendor_id = ((reg_data & DEVID_VENDORID_MASK) >>
-				DEVID_VENDORID_SHIFT);
-	max14577->device_id = ((reg_data & DEVID_DEVICEID_MASK) >>
-				DEVID_DEVICEID_SHIFT);
-	dev_info(max14577->dev, "Device ID: 0x%x, vendor: 0x%x\n",
-			max14577->device_id, max14577->vendor_id);
 
 	ret = regmap_add_irq_chip(max14577->regmap, max14577->irq,
-				  IRQF_TRIGGER_FALLING | IRQF_ONESHOT, 0,
-				  &max14577_irq_chip,
+				  irq_flags, 0, irq_chip,
 				  &max14577->irq_data);
 	if (ret != 0) {
 		dev_err(&i2c->dev, "Failed to request IRQ %d: %d\n",
@@ -145,8 +364,15 @@ static int max14577_i2c_probe(struct i2c_client *i2c,
 		return ret;
 	}
 
-	ret = mfd_add_devices(max14577->dev, -1, max14577_devs,
-			ARRAY_SIZE(max14577_devs), NULL, 0,
+	/* Max77836 specific initialization code (additional regmap) */
+	if (max14577->dev_type == MAXIM_DEVICE_TYPE_MAX77836) {
+		ret = max77836_init(max14577);
+		if (ret < 0)
+			goto err_max77836;
+	}
+
+	ret = mfd_add_devices(max14577->dev, -1, mfd_devs,
+			mfd_devs_size, NULL, 0,
 			regmap_irq_get_domain(max14577->irq_data));
 	if (ret < 0)
 		goto err_mfd;
@@ -156,6 +382,9 @@ static int max14577_i2c_probe(struct i2c_client *i2c,
 	return 0;
 
 err_mfd:
+	if (max14577->dev_type == MAXIM_DEVICE_TYPE_MAX77836)
+		max77836_remove(max14577);
+err_max77836:
 	regmap_del_irq_chip(max14577->irq, max14577->irq_data);
 
 	return ret;
@@ -167,12 +396,15 @@ static int max14577_i2c_remove(struct i2c_client *i2c)
 
 	mfd_remove_devices(max14577->dev);
 	regmap_del_irq_chip(max14577->irq, max14577->irq_data);
+	if (max14577->dev_type == MAXIM_DEVICE_TYPE_MAX77836)
+		max77836_remove(max14577);
 
 	return 0;
 }
 
 static const struct i2c_device_id max14577_i2c_id[] = {
-	{ "max14577", 0 },
+	{ "max14577", MAXIM_DEVICE_TYPE_MAX14577, },
+	{ "max77836", MAXIM_DEVICE_TYPE_MAX77836, },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, max14577_i2c_id);
@@ -183,20 +415,18 @@ static int max14577_suspend(struct device *dev)
 	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
 	struct max14577 *max14577 = i2c_get_clientdata(i2c);
 
-	if (device_may_wakeup(dev)) {
+	if (device_may_wakeup(dev))
 		enable_irq_wake(max14577->irq);
-		/*
-		 * MUIC IRQ must be disabled during suspend if this is
-		 * a wake up source because it will be handled before
-		 * resuming I2C.
-		 *
-		 * When device is woken up from suspend (e.g. by ADC change),
-		 * an interrupt occurs before resuming I2C bus controller.
-		 * Interrupt handler tries to read registers but this read
-		 * will fail because I2C is still suspended.
-		 */
-		disable_irq(max14577->irq);
-	}
+	/*
+	 * MUIC IRQ must be disabled during suspend because if it happens
+	 * while suspended it will be handled before resuming I2C.
+	 *
+	 * When device is woken up from suspend (e.g. by ADC change),
+	 * an interrupt occurs before resuming I2C bus controller.
+	 * Interrupt handler tries to read registers but this read
+	 * will fail because I2C is still suspended.
+	 */
+	disable_irq(max14577->irq);
 
 	return 0;
 }
@@ -206,20 +436,14 @@ static int max14577_resume(struct device *dev)
 	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
 	struct max14577 *max14577 = i2c_get_clientdata(i2c);
 
-	if (device_may_wakeup(dev)) {
+	if (device_may_wakeup(dev))
 		disable_irq_wake(max14577->irq);
-		enable_irq(max14577->irq);
-	}
+	enable_irq(max14577->irq);
 
 	return 0;
 }
 #endif /* CONFIG_PM_SLEEP */
 
-static struct of_device_id max14577_dt_match[] = {
-	{ .compatible = "maxim,max14577", },
-	{},
-};
-
 static SIMPLE_DEV_PM_OPS(max14577_pm, max14577_suspend, max14577_resume);
 
 static struct i2c_driver max14577_i2c_driver = {
@@ -236,6 +460,9 @@ static struct i2c_driver max14577_i2c_driver = {
 
 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);
+
 	return i2c_add_driver(&max14577_i2c_driver);
 }
 subsys_initcall(max14577_i2c_init);
@@ -247,5 +474,5 @@ static void __exit max14577_i2c_exit(void)
 module_exit(max14577_i2c_exit);
 
 MODULE_AUTHOR("Chanwoo Choi <cw00.choi@samsung.com>, Krzysztof Kozlowski <k.kozlowski@samsung.com>");
-MODULE_DESCRIPTION("MAXIM 14577 multi-function core driver");
+MODULE_DESCRIPTION("Maxim 14577/77836 multi-function core driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/max77686.c b/drivers/mfd/max77686.c
index e5fce765acc..ce869acf27a 100644
--- a/drivers/mfd/max77686.c
+++ b/drivers/mfd/max77686.c
@@ -47,7 +47,7 @@ static struct regmap_config max77686_regmap_config = {
 };
 
 #ifdef CONFIG_OF
-static struct of_device_id max77686_pmic_dt_match[] = {
+static const struct of_device_id max77686_pmic_dt_match[] = {
 	{.compatible = "maxim,max77686", .data = NULL},
 	{},
 };
diff --git a/drivers/mfd/max77693.c b/drivers/mfd/max77693.c
index c5535f01846..7e05428c756 100644
--- a/drivers/mfd/max77693.c
+++ b/drivers/mfd/max77693.c
@@ -243,7 +243,7 @@ static const struct dev_pm_ops max77693_pm = {
 };
 
 #ifdef CONFIG_OF
-static struct of_device_id max77693_dt_match[] = {
+static const struct of_device_id max77693_dt_match[] = {
 	{ .compatible = "maxim,max77693" },
 	{},
 };
diff --git a/drivers/mfd/max8907.c b/drivers/mfd/max8907.c
index 07740314b29..232749c8813 100644
--- a/drivers/mfd/max8907.c
+++ b/drivers/mfd/max8907.c
@@ -305,7 +305,7 @@ static int max8907_i2c_remove(struct i2c_client *i2c)
 }
 
 #ifdef CONFIG_OF
-static struct of_device_id max8907_of_match[] = {
+static const struct of_device_id max8907_of_match[] = {
 	{ .compatible = "maxim,max8907" },
 	{ },
 };
diff --git a/drivers/mfd/max8997.c b/drivers/mfd/max8997.c
index 8cf7a015cfe..595364ee178 100644
--- a/drivers/mfd/max8997.c
+++ b/drivers/mfd/max8997.c
@@ -51,7 +51,7 @@ static const struct mfd_cell max8997_devs[] = {
 };
 
 #ifdef CONFIG_OF
-static struct of_device_id max8997_pmic_dt_match[] = {
+static const struct of_device_id max8997_pmic_dt_match[] = {
 	{ .compatible = "maxim,max8997-pmic", .data = (void *)TYPE_MAX8997 },
 	{},
 };
diff --git a/drivers/mfd/max8998.c b/drivers/mfd/max8998.c
index 592db06098e..a37cb7444b6 100644
--- a/drivers/mfd/max8998.c
+++ b/drivers/mfd/max8998.c
@@ -132,7 +132,7 @@ int max8998_update_reg(struct i2c_client *i2c, u8 reg, u8 val, u8 mask)
 EXPORT_SYMBOL(max8998_update_reg);
 
 #ifdef CONFIG_OF
-static struct of_device_id max8998_dt_match[] = {
+static const struct of_device_id max8998_dt_match[] = {
 	{ .compatible = "maxim,max8998", .data = (void *)TYPE_MAX8998 },
 	{ .compatible = "national,lp3974", .data = (void *)TYPE_LP3974 },
 	{ .compatible = "ti,lp3974", .data = (void *)TYPE_LP3974 },
diff --git a/drivers/mfd/mc13xxx-core.c b/drivers/mfd/mc13xxx-core.c
index 06e64b6fcb8..acf5dd712eb 100644
--- a/drivers/mfd/mc13xxx-core.c
+++ b/drivers/mfd/mc13xxx-core.c
@@ -660,30 +660,22 @@ int mc13xxx_common_init(struct device *dev)
 	if (ret)
 		return ret;
 
+	mutex_init(&mc13xxx->lock);
+
 	ret = request_threaded_irq(mc13xxx->irq, NULL, mc13xxx_irq_thread,
 			IRQF_ONESHOT | IRQF_TRIGGER_HIGH, "mc13xxx", mc13xxx);
 	if (ret)
 		return ret;
 
-	mutex_init(&mc13xxx->lock);
-
 	if (mc13xxx_probe_flags_dt(mc13xxx) < 0 && pdata)
 		mc13xxx->flags = pdata->flags;
 
 	if (mc13xxx->flags & MC13XXX_USE_ADC)
 		mc13xxx_add_subdevice(mc13xxx, "%s-adc");
 
-	if (mc13xxx->flags & MC13XXX_USE_CODEC)
-		mc13xxx_add_subdevice_pdata(mc13xxx, "%s-codec",
-					pdata->codec, sizeof(*pdata->codec));
-
 	if (mc13xxx->flags & MC13XXX_USE_RTC)
 		mc13xxx_add_subdevice(mc13xxx, "%s-rtc");
 
-	if (mc13xxx->flags & MC13XXX_USE_TOUCHSCREEN)
-		mc13xxx_add_subdevice_pdata(mc13xxx, "%s-ts",
-				&pdata->touch, sizeof(pdata->touch));
-
 	if (pdata) {
 		mc13xxx_add_subdevice_pdata(mc13xxx, "%s-regulator",
 			&pdata->regulators, sizeof(pdata->regulators));
@@ -691,10 +683,20 @@ int mc13xxx_common_init(struct device *dev)
 				pdata->leds, sizeof(*pdata->leds));
 		mc13xxx_add_subdevice_pdata(mc13xxx, "%s-pwrbutton",
 				pdata->buttons, sizeof(*pdata->buttons));
+		if (mc13xxx->flags & MC13XXX_USE_CODEC)
+			mc13xxx_add_subdevice_pdata(mc13xxx, "%s-codec",
+				pdata->codec, sizeof(*pdata->codec));
+		if (mc13xxx->flags & MC13XXX_USE_TOUCHSCREEN)
+			mc13xxx_add_subdevice_pdata(mc13xxx, "%s-ts",
+				&pdata->touch, sizeof(pdata->touch));
 	} else {
 		mc13xxx_add_subdevice(mc13xxx, "%s-regulator");
 		mc13xxx_add_subdevice(mc13xxx, "%s-led");
 		mc13xxx_add_subdevice(mc13xxx, "%s-pwrbutton");
+		if (mc13xxx->flags & MC13XXX_USE_CODEC)
+			mc13xxx_add_subdevice(mc13xxx, "%s-codec");
+		if (mc13xxx->flags & MC13XXX_USE_TOUCHSCREEN)
+			mc13xxx_add_subdevice(mc13xxx, "%s-ts");
 	}
 
 	return 0;
diff --git a/drivers/mfd/menelaus.c b/drivers/mfd/menelaus.c
index ad25bfa3fb0..5e2667afe2b 100644
--- a/drivers/mfd/menelaus.c
+++ b/drivers/mfd/menelaus.c
@@ -1287,29 +1287,8 @@ static struct i2c_driver menelaus_i2c_driver = {
 	.id_table	= menelaus_id,
 };
 
-static int __init menelaus_init(void)
-{
-	int res;
-
-	res = i2c_add_driver(&menelaus_i2c_driver);
-	if (res < 0) {
-		pr_err(DRIVER_NAME ": driver registration failed\n");
-		return res;
-	}
-
-	return 0;
-}
-
-static void __exit menelaus_exit(void)
-{
-	i2c_del_driver(&menelaus_i2c_driver);
-
-	/* FIXME: Shutdown menelaus parts that can be shut down */
-}
+module_i2c_driver(menelaus_i2c_driver);
 
 MODULE_AUTHOR("Texas Instruments, Inc. (and others)");
 MODULE_DESCRIPTION("I2C interface for Menelaus.");
 MODULE_LICENSE("GPL");
-
-module_init(menelaus_init);
-module_exit(menelaus_exit);
diff --git a/drivers/mfd/mfd-core.c b/drivers/mfd/mfd-core.c
index 26764924473..892d343193a 100644
--- a/drivers/mfd/mfd-core.c
+++ b/drivers/mfd/mfd-core.c
@@ -102,7 +102,7 @@ static int mfd_add_device(struct device *parent, int id,
 	pdev->dev.dma_mask = parent->dma_mask;
 	pdev->dev.dma_parms = parent->dma_parms;
 
-	ret = devm_regulator_bulk_register_supply_alias(
+	ret = regulator_bulk_register_supply_alias(
 			&pdev->dev, cell->parent_supplies,
 			parent, cell->parent_supplies,
 			cell->num_parent_supplies);
@@ -182,9 +182,9 @@ static int mfd_add_device(struct device *parent, int id,
 	return 0;
 
 fail_alias:
-	devm_regulator_bulk_unregister_supply_alias(&pdev->dev,
-						    cell->parent_supplies,
-						    cell->num_parent_supplies);
+	regulator_bulk_unregister_supply_alias(&pdev->dev,
+					       cell->parent_supplies,
+					       cell->num_parent_supplies);
 fail_res:
 	kfree(res);
 fail_device:
@@ -238,6 +238,9 @@ static int mfd_remove_devices_fn(struct device *dev, void *c)
 	pdev = to_platform_device(dev);
 	cell = mfd_get_cell(pdev);
 
+	regulator_bulk_unregister_supply_alias(dev, cell->parent_supplies,
+					       cell->num_parent_supplies);
+
 	/* find the base address of usage_count pointers (for freeing) */
 	if (!*usage_count || (cell->usage_count < *usage_count))
 		*usage_count = cell->usage_count;
diff --git a/drivers/mfd/omap-usb-host.c b/drivers/mfd/omap-usb-host.c
index 651e249287d..b48d80c367f 100644
--- a/drivers/mfd/omap-usb-host.c
+++ b/drivers/mfd/omap-usb-host.c
@@ -557,7 +557,7 @@ static int usbhs_omap_get_dt_pdata(struct device *dev,
 	return 0;
 }
 
-static struct of_device_id usbhs_child_match_table[] = {
+static const struct of_device_id usbhs_child_match_table[] = {
 	{ .compatible = "ti,omap-ehci", },
 	{ .compatible = "ti,omap-ohci", },
 	{ }
diff --git a/drivers/mfd/pm8921-core.c b/drivers/mfd/pm8921-core.c
index b97a97187ae..95951380354 100644
--- a/drivers/mfd/pm8921-core.c
+++ b/drivers/mfd/pm8921-core.c
@@ -26,7 +26,6 @@
 #include <linux/regmap.h>
 #include <linux/of_platform.h>
 #include <linux/mfd/core.h>
-#include <linux/mfd/pm8xxx/core.h>
 
 #define	SSBI_REG_ADDR_IRQ_BASE		0x1BB
 
@@ -57,7 +56,6 @@
 #define PM8921_NR_IRQS		256
 
 struct pm_irq_chip {
-	struct device		*dev;
 	struct regmap		*regmap;
 	spinlock_t		pm_irq_lock;
 	struct irq_domain	*irqdomain;
@@ -67,11 +65,6 @@ struct pm_irq_chip {
 	u8			config[0];
 };
 
-struct pm8921 {
-	struct device			*dev;
-	struct pm_irq_chip		*irq_chip;
-};
-
 static int pm8xxx_read_block_irq(struct pm_irq_chip *chip, unsigned int bp,
 				 unsigned int *ip)
 {
@@ -255,55 +248,6 @@ static struct irq_chip pm8xxx_irq_chip = {
 	.flags		= IRQCHIP_MASK_ON_SUSPEND | IRQCHIP_SKIP_SET_WAKE,
 };
 
-/**
- * pm8xxx_get_irq_stat - get the status of the irq line
- * @chip: pointer to identify a pmic irq controller
- * @irq: the irq number
- *
- * The pm8xxx gpio and mpp rely on the interrupt block to read
- * the values on their pins. This function is to facilitate reading
- * the status of a gpio or an mpp line. The caller has to convert the
- * gpio number to irq number.
- *
- * RETURNS:
- * an int indicating the value read on that line
- */
-static int pm8xxx_get_irq_stat(struct pm_irq_chip *chip, int irq)
-{
-	int pmirq, rc;
-	unsigned int  block, bits, bit;
-	unsigned long flags;
-	struct irq_data *irq_data = irq_get_irq_data(irq);
-
-	pmirq = irq_data->hwirq;
-
-	block = pmirq / 8;
-	bit = pmirq % 8;
-
-	spin_lock_irqsave(&chip->pm_irq_lock, flags);
-
-	rc = regmap_write(chip->regmap, SSBI_REG_ADDR_IRQ_BLK_SEL, block);
-	if (rc) {
-		pr_err("Failed Selecting block irq=%d pmirq=%d blk=%d rc=%d\n",
-			irq, pmirq, block, rc);
-		goto bail_out;
-	}
-
-	rc = regmap_read(chip->regmap, SSBI_REG_ADDR_IRQ_RT_STATUS, &bits);
-	if (rc) {
-		pr_err("Failed Configuring irq=%d pmirq=%d blk=%d rc=%d\n",
-			irq, pmirq, block, rc);
-		goto bail_out;
-	}
-
-	rc = (bits & (1 << bit)) ? 1 : 0;
-
-bail_out:
-	spin_unlock_irqrestore(&chip->pm_irq_lock, flags);
-
-	return rc;
-}
-
 static int pm8xxx_irq_domain_map(struct irq_domain *d, unsigned int irq,
 				   irq_hw_number_t hwirq)
 {
@@ -324,56 +268,6 @@ static const struct irq_domain_ops pm8xxx_irq_domain_ops = {
 	.map = pm8xxx_irq_domain_map,
 };
 
-static int pm8921_readb(const struct device *dev, u16 addr, u8 *val)
-{
-	const struct pm8xxx_drvdata *pm8921_drvdata = dev_get_drvdata(dev);
-	const struct pm8921 *pmic = pm8921_drvdata->pm_chip_data;
-
-	return ssbi_read(pmic->dev->parent, addr, val, 1);
-}
-
-static int pm8921_writeb(const struct device *dev, u16 addr, u8 val)
-{
-	const struct pm8xxx_drvdata *pm8921_drvdata = dev_get_drvdata(dev);
-	const struct pm8921 *pmic = pm8921_drvdata->pm_chip_data;
-
-	return ssbi_write(pmic->dev->parent, addr, &val, 1);
-}
-
-static int pm8921_read_buf(const struct device *dev, u16 addr, u8 *buf,
-									int cnt)
-{
-	const struct pm8xxx_drvdata *pm8921_drvdata = dev_get_drvdata(dev);
-	const struct pm8921 *pmic = pm8921_drvdata->pm_chip_data;
-
-	return ssbi_read(pmic->dev->parent, addr, buf, cnt);
-}
-
-static int pm8921_write_buf(const struct device *dev, u16 addr, u8 *buf,
-									int cnt)
-{
-	const struct pm8xxx_drvdata *pm8921_drvdata = dev_get_drvdata(dev);
-	const struct pm8921 *pmic = pm8921_drvdata->pm_chip_data;
-
-	return ssbi_write(pmic->dev->parent, addr, buf, cnt);
-}
-
-static int pm8921_read_irq_stat(const struct device *dev, int irq)
-{
-	const struct pm8xxx_drvdata *pm8921_drvdata = dev_get_drvdata(dev);
-	const struct pm8921 *pmic = pm8921_drvdata->pm_chip_data;
-
-	return pm8xxx_get_irq_stat(pmic->irq_chip, irq);
-}
-
-static struct pm8xxx_drvdata pm8921_drvdata = {
-	.pmic_readb		= pm8921_readb,
-	.pmic_writeb		= pm8921_writeb,
-	.pmic_read_buf		= pm8921_read_buf,
-	.pmic_write_buf		= pm8921_write_buf,
-	.pmic_read_irq_stat	= pm8921_read_irq_stat,
-};
-
 static const struct regmap_config ssbi_regmap_config = {
 	.reg_bits = 16,
 	.val_bits = 8,
@@ -392,7 +286,6 @@ MODULE_DEVICE_TABLE(of, pm8921_id_table);
 
 static int pm8921_probe(struct platform_device *pdev)
 {
-	struct pm8921 *pmic;
 	struct regmap *regmap;
 	int irq, rc;
 	unsigned int val;
@@ -404,12 +297,6 @@ static int pm8921_probe(struct platform_device *pdev)
 	if (irq < 0)
 		return irq;
 
-	pmic = devm_kzalloc(&pdev->dev, sizeof(struct pm8921), GFP_KERNEL);
-	if (!pmic) {
-		pr_err("Cannot alloc pm8921 struct\n");
-		return -ENOMEM;
-	}
-
 	regmap = devm_regmap_init(&pdev->dev, NULL, pdev->dev.parent,
 				  &ssbi_regmap_config);
 	if (IS_ERR(regmap))
@@ -434,18 +321,13 @@ static int pm8921_probe(struct platform_device *pdev)
 	pr_info("PMIC revision 2: %02X\n", val);
 	rev |= val << BITS_PER_BYTE;
 
-	pmic->dev = &pdev->dev;
-	pm8921_drvdata.pm_chip_data = pmic;
-	platform_set_drvdata(pdev, &pm8921_drvdata);
-
 	chip = devm_kzalloc(&pdev->dev, sizeof(*chip) +
 					sizeof(chip->config[0]) * nirqs,
 					GFP_KERNEL);
 	if (!chip)
 		return -ENOMEM;
 
-	pmic->irq_chip = chip;
-	chip->dev = &pdev->dev;
+	platform_set_drvdata(pdev, chip);
 	chip->regmap = regmap;
 	chip->num_irqs = nirqs;
 	chip->num_blocks = DIV_ROUND_UP(chip->num_irqs, 8);
@@ -481,8 +363,7 @@ static int pm8921_remove_child(struct device *dev, void *unused)
 static int pm8921_remove(struct platform_device *pdev)
 {
 	int irq = platform_get_irq(pdev, 0);
-	struct pm8921 *pmic = pm8921_drvdata.pm_chip_data;
-	struct pm_irq_chip *chip = pmic->irq_chip;
+	struct pm_irq_chip *chip = platform_get_drvdata(pdev);
 
 	device_for_each_child(&pdev->dev, NULL, pm8921_remove_child);
 	irq_set_chained_handler(irq, NULL);
diff --git a/drivers/mfd/rdc321x-southbridge.c b/drivers/mfd/rdc321x-southbridge.c
index c79569750be..6575585f1d1 100644
--- a/drivers/mfd/rdc321x-southbridge.c
+++ b/drivers/mfd/rdc321x-southbridge.c
@@ -38,7 +38,7 @@ static struct resource rdc321x_wdt_resource[] = {
 };
 
 static struct rdc321x_gpio_pdata rdc321x_gpio_pdata = {
-	.max_gpios	= RDC321X_MAX_GPIO,
+	.max_gpios	= RDC321X_NUM_GPIO,
 };
 
 static struct resource rdc321x_gpio_resources[] = {
diff --git a/drivers/mfd/rtsx_usb.c b/drivers/mfd/rtsx_usb.c
index b53b9d46cc4..6352bec8419 100644
--- a/drivers/mfd/rtsx_usb.c
+++ b/drivers/mfd/rtsx_usb.c
@@ -29,7 +29,7 @@ static int polling_pipe = 1;
 module_param(polling_pipe, int, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(polling_pipe, "polling pipe (0: ctl, 1: bulk)");
 
-static struct mfd_cell rtsx_usb_cells[] = {
+static const struct mfd_cell rtsx_usb_cells[] = {
 	[RTSX_USB_SD_CARD] = {
 		.name = "rtsx_usb_sdmmc",
 		.pdata_size = 0,
@@ -67,7 +67,7 @@ static int rtsx_usb_bulk_transfer_sglist(struct rtsx_ucr *ucr,
 	ucr->sg_timer.expires = jiffies + msecs_to_jiffies(timeout);
 	add_timer(&ucr->sg_timer);
 	usb_sg_wait(&ucr->current_sg);
-	del_timer(&ucr->sg_timer);
+	del_timer_sync(&ucr->sg_timer);
 
 	if (act_len)
 		*act_len = ucr->current_sg.bytes;
@@ -644,14 +644,14 @@ static int rtsx_usb_probe(struct usb_interface *intf,
 	if (ret)
 		goto out_init_fail;
 
+	/* initialize USB SG transfer timer */
+	setup_timer(&ucr->sg_timer, rtsx_usb_sg_timed_out, (unsigned long) ucr);
+
 	ret = mfd_add_devices(&intf->dev, usb_dev->devnum, rtsx_usb_cells,
 			ARRAY_SIZE(rtsx_usb_cells), NULL, 0, NULL);
 	if (ret)
 		goto out_init_fail;
 
-	/* initialize USB SG transfer timer */
-	init_timer(&ucr->sg_timer);
-	setup_timer(&ucr->sg_timer, rtsx_usb_sg_timed_out, (unsigned long) ucr);
 #ifdef CONFIG_PM
 	intf->needs_remote_wakeup = 1;
 	usb_enable_autosuspend(usb_dev);
@@ -687,9 +687,15 @@ static int rtsx_usb_suspend(struct usb_interface *intf, pm_message_t message)
 	dev_dbg(&intf->dev, "%s called with pm message 0x%04u\n",
 			__func__, message.event);
 
+	/*
+	 * Call to make sure LED is off during suspend to save more power.
+	 * It is NOT a permanent state and could be turned on anytime later.
+	 * Thus no need to call turn_on when resunming.
+	 */
 	mutex_lock(&ucr->dev_mutex);
 	rtsx_usb_turn_off_led(ucr);
 	mutex_unlock(&ucr->dev_mutex);
+
 	return 0;
 }
 
diff --git a/drivers/mfd/sec-core.c b/drivers/mfd/sec-core.c
index 1cf27521fff..be06d0abbf1 100644
--- a/drivers/mfd/sec-core.c
+++ b/drivers/mfd/sec-core.c
@@ -25,7 +25,6 @@
 #include <linux/mfd/core.h>
 #include <linux/mfd/samsung/core.h>
 #include <linux/mfd/samsung/irq.h>
-#include <linux/mfd/samsung/rtc.h>
 #include <linux/mfd/samsung/s2mpa01.h>
 #include <linux/mfd/samsung/s2mps11.h>
 #include <linux/mfd/samsung/s2mps14.h>
@@ -91,7 +90,7 @@ static const struct mfd_cell s2mpa01_devs[] = {
 };
 
 #ifdef CONFIG_OF
-static struct of_device_id sec_dt_match[] = {
+static const struct of_device_id sec_dt_match[] = {
 	{	.compatible = "samsung,s5m8767-pmic",
 		.data = (void *)S5M8767X,
 	}, {
@@ -196,20 +195,6 @@ static const struct regmap_config s5m8767_regmap_config = {
 	.cache_type = REGCACHE_FLAT,
 };
 
-static const struct regmap_config s5m_rtc_regmap_config = {
-	.reg_bits = 8,
-	.val_bits = 8,
-
-	.max_register = SEC_RTC_REG_MAX,
-};
-
-static const struct regmap_config s2mps14_rtc_regmap_config = {
-	.reg_bits = 8,
-	.val_bits = 8,
-
-	.max_register = S2MPS_RTC_REG_MAX,
-};
-
 #ifdef CONFIG_OF
 /*
  * Only the common platform data elements for s5m8767 are parsed here from the
@@ -264,8 +249,9 @@ static int sec_pmic_probe(struct i2c_client *i2c,
 			    const struct i2c_device_id *id)
 {
 	struct sec_platform_data *pdata = dev_get_platdata(&i2c->dev);
-	const struct regmap_config *regmap, *regmap_rtc;
+	const struct regmap_config *regmap;
 	struct sec_pmic_dev *sec_pmic;
+	unsigned long device_type;
 	int ret;
 
 	sec_pmic = devm_kzalloc(&i2c->dev, sizeof(struct sec_pmic_dev),
@@ -277,7 +263,7 @@ static int sec_pmic_probe(struct i2c_client *i2c,
 	sec_pmic->dev = &i2c->dev;
 	sec_pmic->i2c = i2c;
 	sec_pmic->irq = i2c->irq;
-	sec_pmic->type = sec_i2c_get_driver_data(i2c, id);
+	device_type = sec_i2c_get_driver_data(i2c, id);
 
 	if (sec_pmic->dev->of_node) {
 		pdata = sec_pmic_i2c_parse_dt_pdata(sec_pmic->dev);
@@ -285,7 +271,7 @@ static int sec_pmic_probe(struct i2c_client *i2c,
 			ret = PTR_ERR(pdata);
 			return ret;
 		}
-		pdata->device_type = sec_pmic->type;
+		pdata->device_type = device_type;
 	}
 	if (pdata) {
 		sec_pmic->device_type = pdata->device_type;
@@ -298,39 +284,21 @@ static int sec_pmic_probe(struct i2c_client *i2c,
 	switch (sec_pmic->device_type) {
 	case S2MPA01:
 		regmap = &s2mpa01_regmap_config;
-		/*
-		 * The rtc-s5m driver does not support S2MPA01 and there
-		 * is no mfd_cell for S2MPA01 RTC device.
-		 * However we must pass something to devm_regmap_init_i2c()
-		 * so use S5M-like regmap config even though it wouldn't work.
-		 */
-		regmap_rtc = &s5m_rtc_regmap_config;
 		break;
 	case S2MPS11X:
 		regmap = &s2mps11_regmap_config;
-		/*
-		 * The rtc-s5m driver does not support S2MPS11 and there
-		 * is no mfd_cell for S2MPS11 RTC device.
-		 * However we must pass something to devm_regmap_init_i2c()
-		 * so use S5M-like regmap config even though it wouldn't work.
-		 */
-		regmap_rtc = &s5m_rtc_regmap_config;
 		break;
 	case S2MPS14X:
 		regmap = &s2mps14_regmap_config;
-		regmap_rtc = &s2mps14_rtc_regmap_config;
 		break;
 	case S5M8763X:
 		regmap = &s5m8763_regmap_config;
-		regmap_rtc = &s5m_rtc_regmap_config;
 		break;
 	case S5M8767X:
 		regmap = &s5m8767_regmap_config;
-		regmap_rtc = &s5m_rtc_regmap_config;
 		break;
 	default:
 		regmap = &sec_regmap_config;
-		regmap_rtc = &s5m_rtc_regmap_config;
 		break;
 	}
 
@@ -342,21 +310,6 @@ static int sec_pmic_probe(struct i2c_client *i2c,
 		return ret;
 	}
 
-	sec_pmic->rtc = i2c_new_dummy(i2c->adapter, RTC_I2C_ADDR);
-	if (!sec_pmic->rtc) {
-		dev_err(&i2c->dev, "Failed to allocate I2C for RTC\n");
-		return -ENODEV;
-	}
-	i2c_set_clientdata(sec_pmic->rtc, sec_pmic);
-
-	sec_pmic->regmap_rtc = devm_regmap_init_i2c(sec_pmic->rtc, regmap_rtc);
-	if (IS_ERR(sec_pmic->regmap_rtc)) {
-		ret = PTR_ERR(sec_pmic->regmap_rtc);
-		dev_err(&i2c->dev, "Failed to allocate RTC register map: %d\n",
-			ret);
-		goto err_regmap_rtc;
-	}
-
 	if (pdata && pdata->cfg_pmic_irq)
 		pdata->cfg_pmic_irq();
 
@@ -403,8 +356,6 @@ static int sec_pmic_probe(struct i2c_client *i2c,
 
 err_mfd:
 	sec_irq_exit(sec_pmic);
-err_regmap_rtc:
-	i2c_unregister_device(sec_pmic->rtc);
 	return ret;
 }
 
@@ -414,7 +365,6 @@ static int sec_pmic_remove(struct i2c_client *i2c)
 
 	mfd_remove_devices(sec_pmic->dev);
 	sec_irq_exit(sec_pmic);
-	i2c_unregister_device(sec_pmic->rtc);
 	return 0;
 }
 
@@ -424,19 +374,18 @@ static int sec_pmic_suspend(struct device *dev)
 	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
 	struct sec_pmic_dev *sec_pmic = i2c_get_clientdata(i2c);
 
-	if (device_may_wakeup(dev)) {
+	if (device_may_wakeup(dev))
 		enable_irq_wake(sec_pmic->irq);
-		/*
-		 * PMIC IRQ must be disabled during suspend for RTC alarm
-		 * to work properly.
-		 * When device is woken up from suspend by RTC Alarm, an
-		 * interrupt occurs before resuming I2C bus controller.
-		 * The interrupt is handled by regmap_irq_thread which tries
-		 * to read RTC registers. This read fails (I2C is still
-		 * suspended) and RTC Alarm interrupt is disabled.
-		 */
-		disable_irq(sec_pmic->irq);
-	}
+	/*
+	 * PMIC IRQ must be disabled during suspend for RTC alarm
+	 * to work properly.
+	 * When device is woken up from suspend, an
+	 * interrupt occurs before resuming I2C bus controller.
+	 * The interrupt is handled by regmap_irq_thread which tries
+	 * to read RTC registers. This read fails (I2C is still
+	 * suspended) and RTC Alarm interrupt is disabled.
+	 */
+	disable_irq(sec_pmic->irq);
 
 	return 0;
 }
@@ -446,10 +395,9 @@ static int sec_pmic_resume(struct device *dev)
 	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
 	struct sec_pmic_dev *sec_pmic = i2c_get_clientdata(i2c);
 
-	if (device_may_wakeup(dev)) {
+	if (device_may_wakeup(dev))
 		disable_irq_wake(sec_pmic->irq);
-		enable_irq(sec_pmic->irq);
-	}
+	enable_irq(sec_pmic->irq);
 
 	return 0;
 }
diff --git a/drivers/mfd/sec-irq.c b/drivers/mfd/sec-irq.c
index 64e7913aadc..654e2c1dbf7 100644
--- a/drivers/mfd/sec-irq.c
+++ b/drivers/mfd/sec-irq.c
@@ -385,7 +385,7 @@ int sec_irq_init(struct sec_pmic_dev *sec_pmic)
 				  &sec_pmic->irq_data);
 		break;
 	default:
-		dev_err(sec_pmic->dev, "Unknown device type %d\n",
+		dev_err(sec_pmic->dev, "Unknown device type %lu\n",
 			sec_pmic->device_type);
 		return -EINVAL;
 	}
diff --git a/drivers/mfd/sm501.c b/drivers/mfd/sm501.c
index e7dc441a8f8..81e6d0932bf 100644
--- a/drivers/mfd/sm501.c
+++ b/drivers/mfd/sm501.c
@@ -1726,7 +1726,7 @@ static struct pci_driver sm501_pci_driver = {
 
 MODULE_ALIAS("platform:sm501");
 
-static struct of_device_id of_sm501_match_tbl[] = {
+static const struct of_device_id of_sm501_match_tbl[] = {
 	{ .compatible = "smi,sm501", },
 	{ /* end */ }
 };
diff --git a/drivers/mfd/stmpe-i2c.c b/drivers/mfd/stmpe-i2c.c
index 0da02e11d58..a45f9c0a330 100644
--- a/drivers/mfd/stmpe-i2c.c
+++ b/drivers/mfd/stmpe-i2c.c
@@ -14,6 +14,7 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/types.h>
+#include <linux/of_device.h>
 #include "stmpe.h"
 
 static int i2c_reg_read(struct stmpe *stmpe, u8 reg)
@@ -52,15 +53,41 @@ static struct stmpe_client_info i2c_ci = {
 	.write_block = i2c_block_write,
 };
 
+static const struct of_device_id stmpe_of_match[] = {
+	{ .compatible = "st,stmpe610", .data = (void *)STMPE610, },
+	{ .compatible = "st,stmpe801", .data = (void *)STMPE801, },
+	{ .compatible = "st,stmpe811", .data = (void *)STMPE811, },
+	{ .compatible = "st,stmpe1601", .data = (void *)STMPE1601, },
+	{ .compatible = "st,stmpe1801", .data = (void *)STMPE1801, },
+	{ .compatible = "st,stmpe2401", .data = (void *)STMPE2401, },
+	{ .compatible = "st,stmpe2403", .data = (void *)STMPE2403, },
+	{},
+};
+MODULE_DEVICE_TABLE(of, stmpe_of_match);
+
 static int
 stmpe_i2c_probe(struct i2c_client *i2c, const struct i2c_device_id *id)
 {
+	int partnum;
+	const struct of_device_id *of_id;
+
 	i2c_ci.data = (void *)id;
 	i2c_ci.irq = i2c->irq;
 	i2c_ci.client = i2c;
 	i2c_ci.dev = &i2c->dev;
 
-	return stmpe_probe(&i2c_ci, id->driver_data);
+	of_id = of_match_device(stmpe_of_match, &i2c->dev);
+	if (!of_id) {
+		/*
+		 * This happens when the I2C ID matches the node name
+		 * but no real compatible string has been given.
+		 */
+		dev_info(&i2c->dev, "matching on node name, compatible is preferred\n");
+		partnum = id->driver_data;
+	} else
+		partnum = (int)of_id->data;
+
+	return stmpe_probe(&i2c_ci, partnum);
 }
 
 static int stmpe_i2c_remove(struct i2c_client *i2c)
@@ -89,6 +116,7 @@ static struct i2c_driver stmpe_i2c_driver = {
 #ifdef CONFIG_PM
 		.pm = &stmpe_dev_pm_ops,
 #endif
+		.of_match_table = stmpe_of_match,
 	},
 	.probe		= stmpe_i2c_probe,
 	.remove		= stmpe_i2c_remove,
diff --git a/drivers/mfd/stmpe.c b/drivers/mfd/stmpe.c
index 4a91f6771fb..3b6bfa7184a 100644
--- a/drivers/mfd/stmpe.c
+++ b/drivers/mfd/stmpe.c
@@ -20,6 +20,7 @@
 #include <linux/slab.h>
 #include <linux/mfd/core.h>
 #include <linux/delay.h>
+#include <linux/regulator/consumer.h>
 #include "stmpe.h"
 
 static int __stmpe_enable(struct stmpe *stmpe, unsigned int blocks)
@@ -605,9 +606,18 @@ static int stmpe1601_enable(struct stmpe *stmpe, unsigned int blocks,
 
 	if (blocks & STMPE_BLOCK_GPIO)
 		mask |= STMPE1601_SYS_CTRL_ENABLE_GPIO;
+	else
+		mask &= ~STMPE1601_SYS_CTRL_ENABLE_GPIO;
 
 	if (blocks & STMPE_BLOCK_KEYPAD)
 		mask |= STMPE1601_SYS_CTRL_ENABLE_KPC;
+	else
+		mask &= ~STMPE1601_SYS_CTRL_ENABLE_KPC;
+
+	if (blocks & STMPE_BLOCK_PWM)
+		mask |= STMPE1601_SYS_CTRL_ENABLE_SPWM;
+	else
+		mask &= ~STMPE1601_SYS_CTRL_ENABLE_SPWM;
 
 	return __stmpe_set_bits(stmpe, STMPE1601_REG_SYS_CTRL, mask,
 				enable ? mask : 0);
@@ -986,9 +996,6 @@ static int stmpe_irq_init(struct stmpe *stmpe, struct device_node *np)
 	int base = 0;
 	int num_irqs = stmpe->variant->num_irqs;
 
-	if (!np)
-		base = stmpe->irq_base;
-
 	stmpe->domain = irq_domain_add_simple(np, num_irqs, base,
 					      &stmpe_irq_ops, stmpe);
 	if (!stmpe->domain) {
@@ -1067,7 +1074,7 @@ static int stmpe_chip_init(struct stmpe *stmpe)
 static int stmpe_add_device(struct stmpe *stmpe, const struct mfd_cell *cell)
 {
 	return mfd_add_devices(stmpe->dev, stmpe->pdata->id, cell, 1,
-			       NULL, stmpe->irq_base, stmpe->domain);
+			       NULL, 0, stmpe->domain);
 }
 
 static int stmpe_devices_init(struct stmpe *stmpe)
@@ -1171,12 +1178,23 @@ int stmpe_probe(struct stmpe_client_info *ci, int partnum)
 	stmpe->dev = ci->dev;
 	stmpe->client = ci->client;
 	stmpe->pdata = pdata;
-	stmpe->irq_base = pdata->irq_base;
 	stmpe->ci = ci;
 	stmpe->partnum = partnum;
 	stmpe->variant = stmpe_variant_info[partnum];
 	stmpe->regs = stmpe->variant->regs;
 	stmpe->num_gpios = stmpe->variant->num_gpios;
+	stmpe->vcc = devm_regulator_get_optional(ci->dev, "vcc");
+	if (!IS_ERR(stmpe->vcc)) {
+		ret = regulator_enable(stmpe->vcc);
+		if (ret)
+			dev_warn(ci->dev, "failed to enable VCC supply\n");
+	}
+	stmpe->vio = devm_regulator_get_optional(ci->dev, "vio");
+	if (!IS_ERR(stmpe->vio)) {
+		ret = regulator_enable(stmpe->vio);
+		if (ret)
+			dev_warn(ci->dev, "failed to enable VIO supply\n");
+	}
 	dev_set_drvdata(stmpe->dev, stmpe);
 
 	if (ci->init)
@@ -1243,6 +1261,11 @@ int stmpe_probe(struct stmpe_client_info *ci, int partnum)
 
 int stmpe_remove(struct stmpe *stmpe)
 {
+	if (!IS_ERR(stmpe->vio))
+		regulator_disable(stmpe->vio);
+	if (!IS_ERR(stmpe->vcc))
+		regulator_disable(stmpe->vcc);
+
 	mfd_remove_devices(stmpe->dev);
 
 	return 0;
diff --git a/drivers/mfd/stmpe.h b/drivers/mfd/stmpe.h
index 6639f1b0fef..9e4d21d37a1 100644
--- a/drivers/mfd/stmpe.h
+++ b/drivers/mfd/stmpe.h
@@ -192,7 +192,7 @@ int stmpe_remove(struct stmpe *stmpe);
 
 #define STMPE1601_SYS_CTRL_ENABLE_GPIO		(1 << 3)
 #define STMPE1601_SYS_CTRL_ENABLE_KPC		(1 << 1)
-#define STMPE1601_SYSCON_ENABLE_SPWM		(1 << 0)
+#define STMPE1601_SYS_CTRL_ENABLE_SPWM		(1 << 0)
 
 /* The 1601/2403 share the same masks */
 #define STMPE1601_AUTOSLEEP_TIMEOUT_MASK	(0x7)
diff --git a/drivers/mfd/sun6i-prcm.c b/drivers/mfd/sun6i-prcm.c
new file mode 100644
index 00000000000..718fc4d2adc
--- /dev/null
+++ b/drivers/mfd/sun6i-prcm.c
@@ -0,0 +1,134 @@
+/*
+ * Copyright (C) 2014 Free Electrons
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Boris BREZILLON <boris.brezillon@free-electrons.com>
+ *
+ * Allwinner PRCM (Power/Reset/Clock Management) driver
+ *
+ */
+
+#include <linux/mfd/core.h>
+#include <linux/module.h>
+#include <linux/of.h>
+
+struct prcm_data {
+	int nsubdevs;
+	const struct mfd_cell *subdevs;
+};
+
+static const struct resource sun6i_a31_ar100_clk_res[] = {
+	{
+		.start = 0x0,
+		.end = 0x3,
+		.flags = IORESOURCE_MEM,
+	},
+};
+
+static const struct resource sun6i_a31_apb0_clk_res[] = {
+	{
+		.start = 0xc,
+		.end = 0xf,
+		.flags = IORESOURCE_MEM,
+	},
+};
+
+static const struct resource sun6i_a31_apb0_gates_clk_res[] = {
+	{
+		.start = 0x28,
+		.end = 0x2b,
+		.flags = IORESOURCE_MEM,
+	},
+};
+
+static const struct resource sun6i_a31_apb0_rstc_res[] = {
+	{
+		.start = 0xb0,
+		.end = 0xb3,
+		.flags = IORESOURCE_MEM,
+	},
+};
+
+static const struct mfd_cell sun6i_a31_prcm_subdevs[] = {
+	{
+		.name = "sun6i-a31-ar100-clk",
+		.of_compatible = "allwinner,sun6i-a31-ar100-clk",
+		.num_resources = ARRAY_SIZE(sun6i_a31_ar100_clk_res),
+		.resources = sun6i_a31_ar100_clk_res,
+	},
+	{
+		.name = "sun6i-a31-apb0-clk",
+		.of_compatible = "allwinner,sun6i-a31-apb0-clk",
+		.num_resources = ARRAY_SIZE(sun6i_a31_apb0_clk_res),
+		.resources = sun6i_a31_apb0_clk_res,
+	},
+	{
+		.name = "sun6i-a31-apb0-gates-clk",
+		.of_compatible = "allwinner,sun6i-a31-apb0-gates-clk",
+		.num_resources = ARRAY_SIZE(sun6i_a31_apb0_gates_clk_res),
+		.resources = sun6i_a31_apb0_gates_clk_res,
+	},
+	{
+		.name = "sun6i-a31-apb0-clock-reset",
+		.of_compatible = "allwinner,sun6i-a31-clock-reset",
+		.num_resources = ARRAY_SIZE(sun6i_a31_apb0_rstc_res),
+		.resources = sun6i_a31_apb0_rstc_res,
+	},
+};
+
+static const struct prcm_data sun6i_a31_prcm_data = {
+	.nsubdevs = ARRAY_SIZE(sun6i_a31_prcm_subdevs),
+	.subdevs = sun6i_a31_prcm_subdevs,
+};
+
+static const struct of_device_id sun6i_prcm_dt_ids[] = {
+	{
+		.compatible = "allwinner,sun6i-a31-prcm",
+		.data = &sun6i_a31_prcm_data,
+	},
+	{ /* sentinel */ },
+};
+
+static int sun6i_prcm_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	const struct of_device_id *match;
+	const struct prcm_data *data;
+	struct resource *res;
+	int ret;
+
+	match = of_match_node(sun6i_prcm_dt_ids, np);
+	if (!match)
+		return -EINVAL;
+
+	data = match->data;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "no prcm memory region provided\n");
+		return -ENOENT;
+	}
+
+	ret = mfd_add_devices(&pdev->dev, 0, data->subdevs, data->nsubdevs,
+			      res, -1, NULL);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to add subdevices\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static struct platform_driver sun6i_prcm_driver = {
+	.driver = {
+		.name = "sun6i-prcm",
+		.owner = THIS_MODULE,
+		.of_match_table = sun6i_prcm_dt_ids,
+	},
+	.probe = sun6i_prcm_probe,
+};
+module_platform_driver(sun6i_prcm_driver);
+
+MODULE_AUTHOR("Boris BREZILLON <boris.brezillon@free-electrons.com>");
+MODULE_DESCRIPTION("Allwinner sun6i PRCM driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/syscon.c b/drivers/mfd/syscon.c
index dbea55de439..ca15878ce5c 100644
--- a/drivers/mfd/syscon.c
+++ b/drivers/mfd/syscon.c
@@ -18,6 +18,7 @@
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/of_platform.h>
+#include <linux/platform_data/syscon.h>
 #include <linux/platform_device.h>
 #include <linux/regmap.h>
 #include <linux/mfd/syscon.h>
@@ -94,7 +95,11 @@ struct regmap *syscon_regmap_lookup_by_phandle(struct device_node *np,
 	struct device_node *syscon_np;
 	struct regmap *regmap;
 
-	syscon_np = of_parse_phandle(np, property, 0);
+	if (property)
+		syscon_np = of_parse_phandle(np, property, 0);
+	else
+		syscon_np = np;
+
 	if (!syscon_np)
 		return ERR_PTR(-ENODEV);
 
@@ -119,6 +124,7 @@ static struct regmap_config syscon_regmap_config = {
 static int syscon_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
+	struct syscon_platform_data *pdata = dev_get_platdata(dev);
 	struct syscon *syscon;
 	struct resource *res;
 	void __iomem *base;
@@ -136,6 +142,8 @@ static int syscon_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	syscon_regmap_config.max_register = res->end - res->start - 3;
+	if (pdata)
+		syscon_regmap_config.name = pdata->label;
 	syscon->regmap = devm_regmap_init_mmio(dev, base,
 					&syscon_regmap_config);
 	if (IS_ERR(syscon->regmap)) {
diff --git a/drivers/mfd/tps6507x.c b/drivers/mfd/tps6507x.c
index 3b27482a174..a2e1990c9de 100644
--- a/drivers/mfd/tps6507x.c
+++ b/drivers/mfd/tps6507x.c
@@ -119,7 +119,7 @@ static const struct i2c_device_id tps6507x_i2c_id[] = {
 MODULE_DEVICE_TABLE(i2c, tps6507x_i2c_id);
 
 #ifdef CONFIG_OF
-static struct of_device_id tps6507x_of_match[] = {
+static const struct of_device_id tps6507x_of_match[] = {
 	{.compatible = "ti,tps6507x", },
 	{},
 };
diff --git a/drivers/mfd/tps65090.c b/drivers/mfd/tps65090.c
index ba1a25d758c..1c3e6e2efe4 100644
--- a/drivers/mfd/tps65090.c
+++ b/drivers/mfd/tps65090.c
@@ -32,14 +32,6 @@
 #define NUM_INT_REG 2
 #define TOTAL_NUM_REG 0x18
 
-/* interrupt status registers */
-#define TPS65090_INT_STS	0x0
-#define TPS65090_INT_STS2	0x1
-
-/* interrupt mask registers */
-#define TPS65090_INT_MSK	0x2
-#define TPS65090_INT_MSK2	0x3
-
 #define TPS65090_INT1_MASK_VAC_STATUS_CHANGE		1
 #define TPS65090_INT1_MASK_VSYS_STATUS_CHANGE		2
 #define TPS65090_INT1_MASK_BAT_STATUS_CHANGE		3
@@ -64,11 +56,16 @@ static struct resource charger_resources[] = {
 	}
 };
 
-static const struct mfd_cell tps65090s[] = {
-	{
+enum tps65090_cells {
+	PMIC = 0,
+	CHARGER = 1,
+};
+
+static struct mfd_cell tps65090s[] = {
+	[PMIC] = {
 		.name = "tps65090-pmic",
 	},
-	{
+	[CHARGER] = {
 		.name = "tps65090-charger",
 		.num_resources = ARRAY_SIZE(charger_resources),
 		.resources = &charger_resources[0],
@@ -139,17 +136,26 @@ static struct regmap_irq_chip tps65090_irq_chip = {
 	.irqs = tps65090_irqs,
 	.num_irqs = ARRAY_SIZE(tps65090_irqs),
 	.num_regs = NUM_INT_REG,
-	.status_base = TPS65090_INT_STS,
-	.mask_base = TPS65090_INT_MSK,
+	.status_base = TPS65090_REG_INTR_STS,
+	.mask_base = TPS65090_REG_INTR_MASK,
 	.mask_invert = true,
 };
 
 static bool is_volatile_reg(struct device *dev, unsigned int reg)
 {
-	if ((reg == TPS65090_INT_STS) || (reg == TPS65090_INT_STS2))
-		return true;
-	else
+	/* Nearly all registers have status bits mixed in, except a few */
+	switch (reg) {
+	case TPS65090_REG_INTR_MASK:
+	case TPS65090_REG_INTR_MASK2:
+	case TPS65090_REG_CG_CTRL0:
+	case TPS65090_REG_CG_CTRL1:
+	case TPS65090_REG_CG_CTRL2:
+	case TPS65090_REG_CG_CTRL3:
+	case TPS65090_REG_CG_CTRL4:
+	case TPS65090_REG_CG_CTRL5:
 		return false;
+	}
+	return true;
 }
 
 static const struct regmap_config tps65090_regmap_config = {
@@ -211,6 +217,9 @@ static int tps65090_i2c_probe(struct i2c_client *client,
 					"IRQ init failed with err: %d\n", ret);
 			return ret;
 		}
+	} else {
+		/* Don't tell children they have an IRQ that'll never fire */
+		tps65090s[CHARGER].num_resources = 0;
 	}
 
 	ret = mfd_add_devices(tps65090->dev, -1, tps65090s,
diff --git a/drivers/mfd/tps65218.c b/drivers/mfd/tps65218.c
index a74bfb59f18..0d256cb002e 100644
--- a/drivers/mfd/tps65218.c
+++ b/drivers/mfd/tps65218.c
@@ -197,6 +197,7 @@ static struct regmap_irq_chip tps65218_irq_chip = {
 
 static const struct of_device_id of_tps65218_match_table[] = {
 	{ .compatible = "ti,tps65218", },
+	{}
 };
 
 static int tps65218_probe(struct i2c_client *client,
diff --git a/drivers/mfd/tps6586x.c b/drivers/mfd/tps6586x.c
index bbd54414a75..8e1dbc46958 100644
--- a/drivers/mfd/tps6586x.c
+++ b/drivers/mfd/tps6586x.c
@@ -444,7 +444,7 @@ static struct tps6586x_platform_data *tps6586x_parse_dt(struct i2c_client *clien
 	return pdata;
 }
 
-static struct of_device_id tps6586x_of_match[] = {
+static const struct of_device_id tps6586x_of_match[] = {
 	{ .compatible = "ti,tps6586x", },
 	{ },
 };
@@ -495,6 +495,10 @@ static void tps6586x_print_version(struct i2c_client *client, int version)
 	case TPS658623:
 		name = "TPS658623";
 		break;
+	case TPS658640:
+	case TPS658640v2:
+		name = "TPS658640";
+		break;
 	case TPS658643:
 		name = "TPS658643";
 		break;
diff --git a/drivers/mfd/tps65910.c b/drivers/mfd/tps65910.c
index 460a014ca62..f9e42ea1cb1 100644
--- a/drivers/mfd/tps65910.c
+++ b/drivers/mfd/tps65910.c
@@ -379,7 +379,7 @@ err_sleep_init:
 }
 
 #ifdef CONFIG_OF
-static struct of_device_id tps65910_of_match[] = {
+static const struct of_device_id tps65910_of_match[] = {
 	{ .compatible = "ti,tps65910", .data = (void *)TPS65910},
 	{ .compatible = "ti,tps65911", .data = (void *)TPS65911},
 	{ },
diff --git a/drivers/mfd/twl-core.c b/drivers/mfd/twl-core.c
index e87140bef66..db11b4f4061 100644
--- a/drivers/mfd/twl-core.c
+++ b/drivers/mfd/twl-core.c
@@ -98,7 +98,11 @@
 #define TWL4030_BASEADD_BACKUP		0x0014
 #define TWL4030_BASEADD_INT		0x002E
 #define TWL4030_BASEADD_PM_MASTER	0x0036
+
 #define TWL4030_BASEADD_PM_RECEIVER	0x005B
+#define TWL4030_DCDC_GLOBAL_CFG		0x06
+#define SMARTREFLEX_ENABLE		BIT(3)
+
 #define TWL4030_BASEADD_RTC		0x001C
 #define TWL4030_BASEADD_SECURED_REG	0x0000
 
@@ -1204,6 +1208,11 @@ twl_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	 * Disable TWL4030/TWL5030 I2C Pull-up on I2C1 and I2C4(SR) interface.
 	 * Program I2C_SCL_CTRL_PU(bit 0)=0, I2C_SDA_CTRL_PU (bit 2)=0,
 	 * SR_I2C_SCL_CTRL_PU(bit 4)=0 and SR_I2C_SDA_CTRL_PU(bit 6)=0.
+	 *
+	 * Also, always enable SmartReflex bit as that's needed for omaps to
+	 * to do anything over I2C4 for voltage scaling even if SmartReflex
+	 * is disabled. Without the SmartReflex bit omap sys_clkreq idle
+	 * signal will never trigger for retention idle.
 	 */
 	if (twl_class_is_4030()) {
 		u8 temp;
@@ -1212,6 +1221,12 @@ twl_probe(struct i2c_client *client, const struct i2c_device_id *id)
 		temp &= ~(SR_I2C_SDA_CTRL_PU | SR_I2C_SCL_CTRL_PU | \
 			I2C_SDA_CTRL_PU | I2C_SCL_CTRL_PU);
 		twl_i2c_write_u8(TWL4030_MODULE_INTBR, temp, REG_GPPUPDCTR1);
+
+		twl_i2c_read_u8(TWL_MODULE_PM_RECEIVER, &temp,
+				TWL4030_DCDC_GLOBAL_CFG);
+		temp |= SMARTREFLEX_ENABLE;
+		twl_i2c_write_u8(TWL_MODULE_PM_RECEIVER, temp,
+				 TWL4030_DCDC_GLOBAL_CFG);
 	}
 
 	if (node) {
diff --git a/drivers/mfd/twl6040.c b/drivers/mfd/twl6040.c
index 6e88f25832f..ae26d84b3a5 100644
--- a/drivers/mfd/twl6040.c
+++ b/drivers/mfd/twl6040.c
@@ -87,8 +87,13 @@ static struct reg_default twl6040_defaults[] = {
 };
 
 static struct reg_default twl6040_patch[] = {
-	/* Select I2C bus access to dual access registers */
-	{ TWL6040_REG_ACCCTL, 0x09 },
+	/*
+	 * Select I2C bus access to dual access registers
+	 * Interrupt register is cleared on read
+	 * Select fast mode for i2c (400KHz)
+	 */
+	{ TWL6040_REG_ACCCTL,
+		TWL6040_I2CSEL | TWL6040_INTCLRMODE | TWL6040_I2CMODE(1) },
 };
 
 
@@ -286,6 +291,8 @@ int twl6040_power(struct twl6040 *twl6040, int on)
 		if (twl6040->power_count++)
 			goto out;
 
+		clk_prepare_enable(twl6040->clk32k);
+
 		/* Allow writes to the chip */
 		regcache_cache_only(twl6040->regmap, false);
 
@@ -341,6 +348,8 @@ int twl6040_power(struct twl6040 *twl6040, int on)
 
 		twl6040->sysclk = 0;
 		twl6040->mclk = 0;
+
+		clk_disable_unprepare(twl6040->clk32k);
 	}
 
 out:
@@ -432,12 +441,9 @@ int twl6040_set_pll(struct twl6040 *twl6040, int pll_id,
 					    TWL6040_HPLLENA;
 				break;
 			case 19200000:
-				/*
-				* PLL disabled
-				* (enable PLL if MCLK jitter quality
-				*  doesn't meet specification)
-				*/
-				hppllctl |= TWL6040_MCLK_19200KHZ;
+				/* PLL enabled, bypass mode */
+				hppllctl |= TWL6040_MCLK_19200KHZ |
+					    TWL6040_HPLLBP | TWL6040_HPLLENA;
 				break;
 			case 26000000:
 				/* PLL enabled, active mode */
@@ -445,9 +451,9 @@ int twl6040_set_pll(struct twl6040 *twl6040, int pll_id,
 					    TWL6040_HPLLENA;
 				break;
 			case 38400000:
-				/* PLL enabled, active mode */
+				/* PLL enabled, bypass mode */
 				hppllctl |= TWL6040_MCLK_38400KHZ |
-					    TWL6040_HPLLENA;
+					    TWL6040_HPLLBP | TWL6040_HPLLENA;
 				break;
 			default:
 				dev_err(twl6040->dev,
@@ -639,6 +645,12 @@ static int twl6040_probe(struct i2c_client *client,
 
 	i2c_set_clientdata(client, twl6040);
 
+	twl6040->clk32k = devm_clk_get(&client->dev, "clk32k");
+	if (IS_ERR(twl6040->clk32k)) {
+		dev_info(&client->dev, "clk32k is not handled\n");
+		twl6040->clk32k = NULL;
+	}
+
 	twl6040->supplies[0].supply = "vio";
 	twl6040->supplies[1].supply = "v2v1";
 	ret = devm_regulator_bulk_get(&client->dev, TWL6040_NUM_SUPPLIES,
@@ -660,6 +672,9 @@ static int twl6040_probe(struct i2c_client *client,
 	mutex_init(&twl6040->mutex);
 	init_completion(&twl6040->ready);
 
+	regmap_register_patch(twl6040->regmap, twl6040_patch,
+			      ARRAY_SIZE(twl6040_patch));
+
 	twl6040->rev = twl6040_reg_read(twl6040, TWL6040_REG_ASICREV);
 	if (twl6040->rev < 0) {
 		dev_err(&client->dev, "Failed to read revision register: %d\n",
@@ -679,6 +694,9 @@ static int twl6040_probe(struct i2c_client *client,
 					    GPIOF_OUT_INIT_LOW, "audpwron");
 		if (ret)
 			goto gpio_err;
+
+		/* Clear any pending interrupt */
+		twl6040_reg_read(twl6040, TWL6040_REG_INTID);
 	}
 
 	ret = regmap_add_irq_chip(twl6040->regmap, twl6040->irq, IRQF_ONESHOT,
@@ -707,10 +725,6 @@ static int twl6040_probe(struct i2c_client *client,
 		goto readyirq_err;
 	}
 
-	/* dual-access registers controlled by I2C only */
-	regmap_register_patch(twl6040->regmap, twl6040_patch,
-			      ARRAY_SIZE(twl6040_patch));
-
 	/*
 	 * The main functionality of twl6040 to provide audio on OMAP4+ systems.
 	 * We can add the ASoC codec child whenever this driver has been loaded.
diff --git a/drivers/mfd/vexpress-config.c b/drivers/mfd/vexpress-config.c
deleted file mode 100644
index d0db89d13e0..00000000000
--- a/drivers/mfd/vexpress-config.c
+++ /dev/null
@@ -1,287 +0,0 @@
-/*
- * 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.
- *
- * Copyright (C) 2012 ARM Limited
- */
-
-#define pr_fmt(fmt) "vexpress-config: " fmt
-
-#include <linux/bitops.h>
-#include <linux/completion.h>
-#include <linux/export.h>
-#include <linux/list.h>
-#include <linux/of.h>
-#include <linux/of_device.h>
-#include <linux/slab.h>
-#include <linux/string.h>
-#include <linux/vexpress.h>
-
-
-#define VEXPRESS_CONFIG_MAX_BRIDGES 2
-
-static struct vexpress_config_bridge {
-	struct device_node *node;
-	struct vexpress_config_bridge_info *info;
-	struct list_head transactions;
-	spinlock_t transactions_lock;
-} vexpress_config_bridges[VEXPRESS_CONFIG_MAX_BRIDGES];
-
-static DECLARE_BITMAP(vexpress_config_bridges_map,
-		ARRAY_SIZE(vexpress_config_bridges));
-static DEFINE_MUTEX(vexpress_config_bridges_mutex);
-
-struct vexpress_config_bridge *vexpress_config_bridge_register(
-		struct device_node *node,
-		struct vexpress_config_bridge_info *info)
-{
-	struct vexpress_config_bridge *bridge;
-	int i;
-
-	pr_debug("Registering bridge '%s'\n", info->name);
-
-	mutex_lock(&vexpress_config_bridges_mutex);
-	i = find_first_zero_bit(vexpress_config_bridges_map,
-			ARRAY_SIZE(vexpress_config_bridges));
-	if (i >= ARRAY_SIZE(vexpress_config_bridges)) {
-		pr_err("Can't register more bridges!\n");
-		mutex_unlock(&vexpress_config_bridges_mutex);
-		return NULL;
-	}
-	__set_bit(i, vexpress_config_bridges_map);
-	bridge = &vexpress_config_bridges[i];
-
-	bridge->node = node;
-	bridge->info = info;
-	INIT_LIST_HEAD(&bridge->transactions);
-	spin_lock_init(&bridge->transactions_lock);
-
-	mutex_unlock(&vexpress_config_bridges_mutex);
-
-	return bridge;
-}
-EXPORT_SYMBOL(vexpress_config_bridge_register);
-
-void vexpress_config_bridge_unregister(struct vexpress_config_bridge *bridge)
-{
-	struct vexpress_config_bridge __bridge = *bridge;
-	int i;
-
-	mutex_lock(&vexpress_config_bridges_mutex);
-	for (i = 0; i < ARRAY_SIZE(vexpress_config_bridges); i++)
-		if (&vexpress_config_bridges[i] == bridge)
-			__clear_bit(i, vexpress_config_bridges_map);
-	mutex_unlock(&vexpress_config_bridges_mutex);
-
-	WARN_ON(!list_empty(&__bridge.transactions));
-	while (!list_empty(&__bridge.transactions))
-		cpu_relax();
-}
-EXPORT_SYMBOL(vexpress_config_bridge_unregister);
-
-
-struct vexpress_config_func {
-	struct vexpress_config_bridge *bridge;
-	void *func;
-};
-
-struct vexpress_config_func *__vexpress_config_func_get(struct device *dev,
-		struct device_node *node)
-{
-	struct device_node *bridge_node;
-	struct vexpress_config_func *func;
-	int i;
-
-	if (WARN_ON(dev && node && dev->of_node != node))
-		return NULL;
-	if (dev && !node)
-		node = dev->of_node;
-
-	func = kzalloc(sizeof(*func), GFP_KERNEL);
-	if (!func)
-		return NULL;
-
-	bridge_node = of_node_get(node);
-	while (bridge_node) {
-		const __be32 *prop = of_get_property(bridge_node,
-				"arm,vexpress,config-bridge", NULL);
-
-		if (prop) {
-			bridge_node = of_find_node_by_phandle(
-					be32_to_cpup(prop));
-			break;
-		}
-
-		bridge_node = of_get_next_parent(bridge_node);
-	}
-
-	mutex_lock(&vexpress_config_bridges_mutex);
-	for (i = 0; i < ARRAY_SIZE(vexpress_config_bridges); i++) {
-		struct vexpress_config_bridge *bridge =
-				&vexpress_config_bridges[i];
-
-		if (test_bit(i, vexpress_config_bridges_map) &&
-				bridge->node == bridge_node) {
-			func->bridge = bridge;
-			func->func = bridge->info->func_get(dev, node);
-			break;
-		}
-	}
-	mutex_unlock(&vexpress_config_bridges_mutex);
-
-	if (!func->func) {
-		of_node_put(node);
-		kfree(func);
-		return NULL;
-	}
-
-	return func;
-}
-EXPORT_SYMBOL(__vexpress_config_func_get);
-
-void vexpress_config_func_put(struct vexpress_config_func *func)
-{
-	func->bridge->info->func_put(func->func);
-	of_node_put(func->bridge->node);
-	kfree(func);
-}
-EXPORT_SYMBOL(vexpress_config_func_put);
-
-struct vexpress_config_trans {
-	struct vexpress_config_func *func;
-	int offset;
-	bool write;
-	u32 *data;
-	int status;
-	struct completion completion;
-	struct list_head list;
-};
-
-static void vexpress_config_dump_trans(const char *what,
-		struct vexpress_config_trans *trans)
-{
-	pr_debug("%s %s trans %p func 0x%p offset %d data 0x%x status %d\n",
-			what, trans->write ? "write" : "read", trans,
-			trans->func->func, trans->offset,
-			trans->data ? *trans->data : 0, trans->status);
-}
-
-static int vexpress_config_schedule(struct vexpress_config_trans *trans)
-{
-	int status;
-	struct vexpress_config_bridge *bridge = trans->func->bridge;
-	unsigned long flags;
-
-	init_completion(&trans->completion);
-	trans->status = -EFAULT;
-
-	spin_lock_irqsave(&bridge->transactions_lock, flags);
-
-	if (list_empty(&bridge->transactions)) {
-		vexpress_config_dump_trans("Executing", trans);
-		status = bridge->info->func_exec(trans->func->func,
-				trans->offset, trans->write, trans->data);
-	} else {
-		vexpress_config_dump_trans("Queuing", trans);
-		status = VEXPRESS_CONFIG_STATUS_WAIT;
-	}
-
-	switch (status) {
-	case VEXPRESS_CONFIG_STATUS_DONE:
-		vexpress_config_dump_trans("Finished", trans);
-		trans->status = status;
-		break;
-	case VEXPRESS_CONFIG_STATUS_WAIT:
-		list_add_tail(&trans->list, &bridge->transactions);
-		break;
-	}
-
-	spin_unlock_irqrestore(&bridge->transactions_lock, flags);
-
-	return status;
-}
-
-void vexpress_config_complete(struct vexpress_config_bridge *bridge,
-		int status)
-{
-	struct vexpress_config_trans *trans;
-	unsigned long flags;
-	const char *message = "Completed";
-
-	spin_lock_irqsave(&bridge->transactions_lock, flags);
-
-	trans = list_first_entry(&bridge->transactions,
-			struct vexpress_config_trans, list);
-	trans->status = status;
-
-	do {
-		vexpress_config_dump_trans(message, trans);
-		list_del(&trans->list);
-		complete(&trans->completion);
-
-		if (list_empty(&bridge->transactions))
-			break;
-
-		trans = list_first_entry(&bridge->transactions,
-				struct vexpress_config_trans, list);
-		vexpress_config_dump_trans("Executing pending", trans);
-		trans->status = bridge->info->func_exec(trans->func->func,
-				trans->offset, trans->write, trans->data);
-		message = "Finished pending";
-	} while (trans->status == VEXPRESS_CONFIG_STATUS_DONE);
-
-	spin_unlock_irqrestore(&bridge->transactions_lock, flags);
-}
-EXPORT_SYMBOL(vexpress_config_complete);
-
-int vexpress_config_wait(struct vexpress_config_trans *trans)
-{
-	wait_for_completion(&trans->completion);
-
-	return trans->status;
-}
-EXPORT_SYMBOL(vexpress_config_wait);
-
-int vexpress_config_read(struct vexpress_config_func *func, int offset,
-		u32 *data)
-{
-	struct vexpress_config_trans trans = {
-		.func = func,
-		.offset = offset,
-		.write = false,
-		.data = data,
-		.status = 0,
-	};
-	int status = vexpress_config_schedule(&trans);
-
-	if (status == VEXPRESS_CONFIG_STATUS_WAIT)
-		status = vexpress_config_wait(&trans);
-
-	return status;
-}
-EXPORT_SYMBOL(vexpress_config_read);
-
-int vexpress_config_write(struct vexpress_config_func *func, int offset,
-		u32 data)
-{
-	struct vexpress_config_trans trans = {
-		.func = func,
-		.offset = offset,
-		.write = true,
-		.data = &data,
-		.status = 0,
-	};
-	int status = vexpress_config_schedule(&trans);
-
-	if (status == VEXPRESS_CONFIG_STATUS_WAIT)
-		status = vexpress_config_wait(&trans);
-
-	return status;
-}
-EXPORT_SYMBOL(vexpress_config_write);
diff --git a/drivers/mfd/vexpress-sysreg.c b/drivers/mfd/vexpress-sysreg.c
index 35281e804e7..9e21e4fc959 100644
--- a/drivers/mfd/vexpress-sysreg.c
+++ b/drivers/mfd/vexpress-sysreg.c
@@ -11,23 +11,22 @@
  * Copyright (C) 2012 ARM Limited
  */
 
+#include <linux/basic_mmio_gpio.h>
 #include <linux/err.h>
-#include <linux/gpio.h>
 #include <linux/io.h>
-#include <linux/leds.h>
+#include <linux/mfd/core.h>
 #include <linux/of_address.h>
+#include <linux/of_platform.h>
+#include <linux/platform_data/syscon.h>
 #include <linux/platform_device.h>
-#include <linux/regulator/driver.h>
 #include <linux/slab.h>
 #include <linux/stat.h>
-#include <linux/timer.h>
 #include <linux/vexpress.h>
 
 #define SYS_ID			0x000
 #define SYS_SW			0x004
 #define SYS_LED			0x008
 #define SYS_100HZ		0x024
-#define SYS_FLAGS		0x030
 #define SYS_FLAGSSET		0x030
 #define SYS_FLAGSCLR		0x034
 #define SYS_NVFLAGS		0x038
@@ -46,465 +45,209 @@
 #define SYS_CFGSTAT		0x0a8
 
 #define SYS_HBI_MASK		0xfff
-#define SYS_ID_HBI_SHIFT	16
 #define SYS_PROCIDx_HBI_SHIFT	0
 
-#define SYS_LED_LED(n)		(1 << (n))
-
 #define SYS_MCI_CARDIN		(1 << 0)
 #define SYS_MCI_WPROT		(1 << 1)
 
-#define SYS_FLASH_WPn		(1 << 0)
-
 #define SYS_MISC_MASTERSITE	(1 << 14)
 
-#define SYS_CFGCTRL_START	(1 << 31)
-#define SYS_CFGCTRL_WRITE	(1 << 30)
-#define SYS_CFGCTRL_DCC(n)	(((n) & 0xf) << 26)
-#define SYS_CFGCTRL_FUNC(n)	(((n) & 0x3f) << 20)
-#define SYS_CFGCTRL_SITE(n)	(((n) & 0x3) << 16)
-#define SYS_CFGCTRL_POSITION(n)	(((n) & 0xf) << 12)
-#define SYS_CFGCTRL_DEVICE(n)	(((n) & 0xfff) << 0)
-
-#define SYS_CFGSTAT_ERR		(1 << 1)
-#define SYS_CFGSTAT_COMPLETE	(1 << 0)
-
-
-static void __iomem *vexpress_sysreg_base;
-static struct device *vexpress_sysreg_dev;
-static int vexpress_master_site;
-
 
-void vexpress_flags_set(u32 data)
-{
-	writel(~0, vexpress_sysreg_base + SYS_FLAGSCLR);
-	writel(data, vexpress_sysreg_base + SYS_FLAGSSET);
-}
+static void __iomem *__vexpress_sysreg_base;
 
-u32 vexpress_get_procid(int site)
+static void __iomem *vexpress_sysreg_base(void)
 {
-	if (site == VEXPRESS_SITE_MASTER)
-		site = vexpress_master_site;
-
-	return readl(vexpress_sysreg_base + (site == VEXPRESS_SITE_DB1 ?
-			SYS_PROCID0 : SYS_PROCID1));
-}
+	if (!__vexpress_sysreg_base) {
+		struct device_node *node = of_find_compatible_node(NULL, NULL,
+				"arm,vexpress-sysreg");
 
-u32 vexpress_get_hbi(int site)
-{
-	u32 id;
-
-	switch (site) {
-	case VEXPRESS_SITE_MB:
-		id = readl(vexpress_sysreg_base + SYS_ID);
-		return (id >> SYS_ID_HBI_SHIFT) & SYS_HBI_MASK;
-	case VEXPRESS_SITE_MASTER:
-	case VEXPRESS_SITE_DB1:
-	case VEXPRESS_SITE_DB2:
-		id = vexpress_get_procid(site);
-		return (id >> SYS_PROCIDx_HBI_SHIFT) & SYS_HBI_MASK;
+		__vexpress_sysreg_base = of_iomap(node, 0);
 	}
 
-	return ~0;
-}
+	WARN_ON(!__vexpress_sysreg_base);
 
-void __iomem *vexpress_get_24mhz_clock_base(void)
-{
-	return vexpress_sysreg_base + SYS_24MHZ;
+	return __vexpress_sysreg_base;
 }
 
 
-static void vexpress_sysreg_find_prop(struct device_node *node,
-		const char *name, u32 *val)
+static int vexpress_sysreg_get_master(void)
 {
-	of_node_get(node);
-	while (node) {
-		if (of_property_read_u32(node, name, val) == 0) {
-			of_node_put(node);
-			return;
-		}
-		node = of_get_next_parent(node);
-	}
-}
-
-unsigned __vexpress_get_site(struct device *dev, struct device_node *node)
-{
-	u32 site = 0;
-
-	WARN_ON(dev && node && dev->of_node != node);
-	if (dev && !node)
-		node = dev->of_node;
-
-	if (node) {
-		vexpress_sysreg_find_prop(node, "arm,vexpress,site", &site);
-	} else if (dev && dev->bus == &platform_bus_type) {
-		struct platform_device *pdev = to_platform_device(dev);
-
-		if (pdev->num_resources == 1 &&
-				pdev->resource[0].flags == IORESOURCE_BUS)
-			site = pdev->resource[0].start;
-	} else if (dev && strncmp(dev_name(dev), "ct:", 3) == 0) {
-		site = VEXPRESS_SITE_MASTER;
-	}
+	if (readl(vexpress_sysreg_base() + SYS_MISC) & SYS_MISC_MASTERSITE)
+		return VEXPRESS_SITE_DB2;
 
-	if (site == VEXPRESS_SITE_MASTER)
-		site = vexpress_master_site;
-
-	return site;
+	return VEXPRESS_SITE_DB1;
 }
 
-
-struct vexpress_sysreg_config_func {
-	u32 template;
-	u32 device;
-};
-
-static struct vexpress_config_bridge *vexpress_sysreg_config_bridge;
-static struct timer_list vexpress_sysreg_config_timer;
-static u32 *vexpress_sysreg_config_data;
-static int vexpress_sysreg_config_tries;
-
-static void *vexpress_sysreg_config_func_get(struct device *dev,
-		struct device_node *node)
+void vexpress_flags_set(u32 data)
 {
-	struct vexpress_sysreg_config_func *config_func;
-	u32 site = 0;
-	u32 position = 0;
-	u32 dcc = 0;
-	u32 func_device[2];
-	int err = -EFAULT;
-
-	if (node) {
-		of_node_get(node);
-		vexpress_sysreg_find_prop(node, "arm,vexpress,site", &site);
-		vexpress_sysreg_find_prop(node, "arm,vexpress,position",
-				&position);
-		vexpress_sysreg_find_prop(node, "arm,vexpress,dcc", &dcc);
-		err = of_property_read_u32_array(node,
-				"arm,vexpress-sysreg,func", func_device,
-				ARRAY_SIZE(func_device));
-		of_node_put(node);
-	} else if (dev && dev->bus == &platform_bus_type) {
-		struct platform_device *pdev = to_platform_device(dev);
-
-		if (pdev->num_resources == 1 &&
-				pdev->resource[0].flags == IORESOURCE_BUS) {
-			site = pdev->resource[0].start;
-			func_device[0] = pdev->resource[0].end;
-			func_device[1] = pdev->id;
-			err = 0;
-		}
-	}
-	if (err)
-		return NULL;
-
-	config_func = kzalloc(sizeof(*config_func), GFP_KERNEL);
-	if (!config_func)
-		return NULL;
-
-	config_func->template = SYS_CFGCTRL_DCC(dcc);
-	config_func->template |= SYS_CFGCTRL_FUNC(func_device[0]);
-	config_func->template |= SYS_CFGCTRL_SITE(site == VEXPRESS_SITE_MASTER ?
-			vexpress_master_site : site);
-	config_func->template |= SYS_CFGCTRL_POSITION(position);
-	config_func->device |= func_device[1];
-
-	dev_dbg(vexpress_sysreg_dev, "func 0x%p = 0x%x, %d\n", config_func,
-			config_func->template, config_func->device);
-
-	return config_func;
+	writel(~0, vexpress_sysreg_base() + SYS_FLAGSCLR);
+	writel(data, vexpress_sysreg_base() + SYS_FLAGSSET);
 }
 
-static void vexpress_sysreg_config_func_put(void *func)
+unsigned int vexpress_get_mci_cardin(struct device *dev)
 {
-	kfree(func);
+	return readl(vexpress_sysreg_base() + SYS_MCI) & SYS_MCI_CARDIN;
 }
 
-static int vexpress_sysreg_config_func_exec(void *func, int offset,
-		bool write, u32 *data)
+u32 vexpress_get_procid(int site)
 {
-	int status;
-	struct vexpress_sysreg_config_func *config_func = func;
-	u32 command;
-
-	if (WARN_ON(!vexpress_sysreg_base))
-		return -ENOENT;
-
-	command = readl(vexpress_sysreg_base + SYS_CFGCTRL);
-	if (WARN_ON(command & SYS_CFGCTRL_START))
-		return -EBUSY;
-
-	command = SYS_CFGCTRL_START;
-	command |= write ? SYS_CFGCTRL_WRITE : 0;
-	command |= config_func->template;
-	command |= SYS_CFGCTRL_DEVICE(config_func->device + offset);
-
-	/* Use a canary for reads */
-	if (!write)
-		*data = 0xdeadbeef;
-
-	dev_dbg(vexpress_sysreg_dev, "command %x, data %x\n",
-			command, *data);
-	writel(*data, vexpress_sysreg_base + SYS_CFGDATA);
-	writel(0, vexpress_sysreg_base + SYS_CFGSTAT);
-	writel(command, vexpress_sysreg_base + SYS_CFGCTRL);
-	mb();
-
-	if (vexpress_sysreg_dev) {
-		/* Schedule completion check */
-		if (!write)
-			vexpress_sysreg_config_data = data;
-		vexpress_sysreg_config_tries = 100;
-		mod_timer(&vexpress_sysreg_config_timer,
-				jiffies + usecs_to_jiffies(100));
-		status = VEXPRESS_CONFIG_STATUS_WAIT;
-	} else {
-		/* Early execution, no timer available, have to spin */
-		u32 cfgstat;
-
-		do {
-			cpu_relax();
-			cfgstat = readl(vexpress_sysreg_base + SYS_CFGSTAT);
-		} while (!cfgstat);
-
-		if (!write && (cfgstat & SYS_CFGSTAT_COMPLETE))
-			*data = readl(vexpress_sysreg_base + SYS_CFGDATA);
-		status = VEXPRESS_CONFIG_STATUS_DONE;
-
-		if (cfgstat & SYS_CFGSTAT_ERR)
-			status = -EINVAL;
-	}
+	if (site == VEXPRESS_SITE_MASTER)
+		site = vexpress_sysreg_get_master();
 
-	return status;
+	return readl(vexpress_sysreg_base() + (site == VEXPRESS_SITE_DB1 ?
+			SYS_PROCID0 : SYS_PROCID1));
 }
 
-struct vexpress_config_bridge_info vexpress_sysreg_config_bridge_info = {
-	.name = "vexpress-sysreg",
-	.func_get = vexpress_sysreg_config_func_get,
-	.func_put = vexpress_sysreg_config_func_put,
-	.func_exec = vexpress_sysreg_config_func_exec,
-};
-
-static void vexpress_sysreg_config_complete(unsigned long data)
+void __iomem *vexpress_get_24mhz_clock_base(void)
 {
-	int status = VEXPRESS_CONFIG_STATUS_DONE;
-	u32 cfgstat = readl(vexpress_sysreg_base + SYS_CFGSTAT);
-
-	if (cfgstat & SYS_CFGSTAT_ERR)
-		status = -EINVAL;
-	if (!vexpress_sysreg_config_tries--)
-		status = -ETIMEDOUT;
-
-	if (status < 0) {
-		dev_err(vexpress_sysreg_dev, "error %d\n", status);
-	} else if (!(cfgstat & SYS_CFGSTAT_COMPLETE)) {
-		mod_timer(&vexpress_sysreg_config_timer,
-				jiffies + usecs_to_jiffies(50));
-		return;
-	}
-
-	if (vexpress_sysreg_config_data) {
-		*vexpress_sysreg_config_data = readl(vexpress_sysreg_base +
-				SYS_CFGDATA);
-		dev_dbg(vexpress_sysreg_dev, "read data %x\n",
-				*vexpress_sysreg_config_data);
-		vexpress_sysreg_config_data = NULL;
-	}
-
-	vexpress_config_complete(vexpress_sysreg_config_bridge, status);
+	return vexpress_sysreg_base() + SYS_24MHZ;
 }
 
 
-void vexpress_sysreg_setup(struct device_node *node)
-{
-	if (WARN_ON(!vexpress_sysreg_base))
-		return;
-
-	if (readl(vexpress_sysreg_base + SYS_MISC) & SYS_MISC_MASTERSITE)
-		vexpress_master_site = VEXPRESS_SITE_DB2;
-	else
-		vexpress_master_site = VEXPRESS_SITE_DB1;
-
-	vexpress_sysreg_config_bridge = vexpress_config_bridge_register(
-			node, &vexpress_sysreg_config_bridge_info);
-	WARN_ON(!vexpress_sysreg_config_bridge);
-}
-
 void __init vexpress_sysreg_early_init(void __iomem *base)
 {
-	vexpress_sysreg_base = base;
-	vexpress_sysreg_setup(NULL);
-}
-
-void __init vexpress_sysreg_of_early_init(void)
-{
-	struct device_node *node;
-
-	if (vexpress_sysreg_base)
-		return;
+	__vexpress_sysreg_base = base;
 
-	node = of_find_compatible_node(NULL, NULL, "arm,vexpress-sysreg");
-	if (node) {
-		vexpress_sysreg_base = of_iomap(node, 0);
-		vexpress_sysreg_setup(node);
-	}
+	vexpress_config_set_master(vexpress_sysreg_get_master());
 }
 
 
-#ifdef CONFIG_GPIOLIB
-
-#define VEXPRESS_SYSREG_GPIO(_name, _reg, _value) \
-	[VEXPRESS_GPIO_##_name] = { \
-		.reg = _reg, \
-		.value = _reg##_##_value, \
-	}
+/* The sysreg block is just a random collection of various functions... */
 
-static struct vexpress_sysreg_gpio {
-	unsigned long reg;
-	u32 value;
-} vexpress_sysreg_gpios[] = {
-	VEXPRESS_SYSREG_GPIO(MMC_CARDIN,	SYS_MCI,	CARDIN),
-	VEXPRESS_SYSREG_GPIO(MMC_WPROT,		SYS_MCI,	WPROT),
-	VEXPRESS_SYSREG_GPIO(FLASH_WPn,		SYS_FLASH,	WPn),
-	VEXPRESS_SYSREG_GPIO(LED0,		SYS_LED,	LED(0)),
-	VEXPRESS_SYSREG_GPIO(LED1,		SYS_LED,	LED(1)),
-	VEXPRESS_SYSREG_GPIO(LED2,		SYS_LED,	LED(2)),
-	VEXPRESS_SYSREG_GPIO(LED3,		SYS_LED,	LED(3)),
-	VEXPRESS_SYSREG_GPIO(LED4,		SYS_LED,	LED(4)),
-	VEXPRESS_SYSREG_GPIO(LED5,		SYS_LED,	LED(5)),
-	VEXPRESS_SYSREG_GPIO(LED6,		SYS_LED,	LED(6)),
-	VEXPRESS_SYSREG_GPIO(LED7,		SYS_LED,	LED(7)),
+static struct syscon_platform_data vexpress_sysreg_sys_id_pdata = {
+	.label = "sys_id",
 };
 
-static int vexpress_sysreg_gpio_direction_input(struct gpio_chip *chip,
-				       unsigned offset)
-{
-	return 0;
-}
-
-static int vexpress_sysreg_gpio_get(struct gpio_chip *chip,
-				       unsigned offset)
-{
-	struct vexpress_sysreg_gpio *gpio = &vexpress_sysreg_gpios[offset];
-	u32 reg_value = readl(vexpress_sysreg_base + gpio->reg);
-
-	return !!(reg_value & gpio->value);
-}
-
-static void vexpress_sysreg_gpio_set(struct gpio_chip *chip,
-				       unsigned offset, int value)
-{
-	struct vexpress_sysreg_gpio *gpio = &vexpress_sysreg_gpios[offset];
-	u32 reg_value = readl(vexpress_sysreg_base + gpio->reg);
-
-	if (value)
-		reg_value |= gpio->value;
-	else
-		reg_value &= ~gpio->value;
-
-	writel(reg_value, vexpress_sysreg_base + gpio->reg);
-}
-
-static int vexpress_sysreg_gpio_direction_output(struct gpio_chip *chip,
-						unsigned offset, int value)
-{
-	vexpress_sysreg_gpio_set(chip, offset, value);
-
-	return 0;
-}
-
-static struct gpio_chip vexpress_sysreg_gpio_chip = {
-	.label = "vexpress-sysreg",
-	.direction_input = vexpress_sysreg_gpio_direction_input,
-	.direction_output = vexpress_sysreg_gpio_direction_output,
-	.get = vexpress_sysreg_gpio_get,
-	.set = vexpress_sysreg_gpio_set,
-	.ngpio = ARRAY_SIZE(vexpress_sysreg_gpios),
-	.base = 0,
+static struct bgpio_pdata vexpress_sysreg_sys_led_pdata = {
+	.label = "sys_led",
+	.base = -1,
+	.ngpio = 8,
 };
 
-
-#define VEXPRESS_SYSREG_GREEN_LED(_name, _default_trigger, _gpio) \
-	{ \
-		.name = "v2m:green:"_name, \
-		.default_trigger = _default_trigger, \
-		.gpio = VEXPRESS_GPIO_##_gpio, \
-	}
-
-struct gpio_led vexpress_sysreg_leds[] = {
-	VEXPRESS_SYSREG_GREEN_LED("user1",	"heartbeat",	LED0),
-	VEXPRESS_SYSREG_GREEN_LED("user2",	"mmc0",		LED1),
-	VEXPRESS_SYSREG_GREEN_LED("user3",	"cpu0",		LED2),
-	VEXPRESS_SYSREG_GREEN_LED("user4",	"cpu1",		LED3),
-	VEXPRESS_SYSREG_GREEN_LED("user5",	"cpu2",		LED4),
-	VEXPRESS_SYSREG_GREEN_LED("user6",	"cpu3",		LED5),
-	VEXPRESS_SYSREG_GREEN_LED("user7",	"cpu4",		LED6),
-	VEXPRESS_SYSREG_GREEN_LED("user8",	"cpu5",		LED7),
+static struct bgpio_pdata vexpress_sysreg_sys_mci_pdata = {
+	.label = "sys_mci",
+	.base = -1,
+	.ngpio = 2,
 };
 
-struct gpio_led_platform_data vexpress_sysreg_leds_pdata = {
-	.num_leds = ARRAY_SIZE(vexpress_sysreg_leds),
-	.leds = vexpress_sysreg_leds,
+static struct bgpio_pdata vexpress_sysreg_sys_flash_pdata = {
+	.label = "sys_flash",
+	.base = -1,
+	.ngpio = 1,
 };
 
-#endif
-
+static struct syscon_platform_data vexpress_sysreg_sys_misc_pdata = {
+	.label = "sys_misc",
+};
 
-static ssize_t vexpress_sysreg_sys_id_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
-{
-	return sprintf(buf, "0x%08x\n", readl(vexpress_sysreg_base + SYS_ID));
-}
+static struct syscon_platform_data vexpress_sysreg_sys_procid_pdata = {
+	.label = "sys_procid",
+};
 
-DEVICE_ATTR(sys_id, S_IRUGO, vexpress_sysreg_sys_id_show, NULL);
+static struct mfd_cell vexpress_sysreg_cells[] = {
+	{
+		.name = "syscon",
+		.num_resources = 1,
+		.resources = (struct resource []) {
+			DEFINE_RES_MEM(SYS_ID, 0x4),
+		},
+		.platform_data = &vexpress_sysreg_sys_id_pdata,
+		.pdata_size = sizeof(vexpress_sysreg_sys_id_pdata),
+	}, {
+		.name = "basic-mmio-gpio",
+		.of_compatible = "arm,vexpress-sysreg,sys_led",
+		.num_resources = 1,
+		.resources = (struct resource []) {
+			DEFINE_RES_MEM_NAMED(SYS_LED, 0x4, "dat"),
+		},
+		.platform_data = &vexpress_sysreg_sys_led_pdata,
+		.pdata_size = sizeof(vexpress_sysreg_sys_led_pdata),
+	}, {
+		.name = "basic-mmio-gpio",
+		.of_compatible = "arm,vexpress-sysreg,sys_mci",
+		.num_resources = 1,
+		.resources = (struct resource []) {
+			DEFINE_RES_MEM_NAMED(SYS_MCI, 0x4, "dat"),
+		},
+		.platform_data = &vexpress_sysreg_sys_mci_pdata,
+		.pdata_size = sizeof(vexpress_sysreg_sys_mci_pdata),
+	}, {
+		.name = "basic-mmio-gpio",
+		.of_compatible = "arm,vexpress-sysreg,sys_flash",
+		.num_resources = 1,
+		.resources = (struct resource []) {
+			DEFINE_RES_MEM_NAMED(SYS_FLASH, 0x4, "dat"),
+		},
+		.platform_data = &vexpress_sysreg_sys_flash_pdata,
+		.pdata_size = sizeof(vexpress_sysreg_sys_flash_pdata),
+	}, {
+		.name = "syscon",
+		.num_resources = 1,
+		.resources = (struct resource []) {
+			DEFINE_RES_MEM(SYS_MISC, 0x4),
+		},
+		.platform_data = &vexpress_sysreg_sys_misc_pdata,
+		.pdata_size = sizeof(vexpress_sysreg_sys_misc_pdata),
+	}, {
+		.name = "syscon",
+		.num_resources = 1,
+		.resources = (struct resource []) {
+			DEFINE_RES_MEM(SYS_PROCID0, 0x8),
+		},
+		.platform_data = &vexpress_sysreg_sys_procid_pdata,
+		.pdata_size = sizeof(vexpress_sysreg_sys_procid_pdata),
+	}, {
+		.name = "vexpress-syscfg",
+		.num_resources = 1,
+		.resources = (struct resource []) {
+			DEFINE_RES_MEM(SYS_CFGDATA, 0xc),
+		},
+	}
+};
 
 static int vexpress_sysreg_probe(struct platform_device *pdev)
 {
-	int err;
-	struct resource *res = platform_get_resource(pdev,
-			IORESOURCE_MEM, 0);
-
-	if (!devm_request_mem_region(&pdev->dev, res->start,
-			resource_size(res), pdev->name)) {
-		dev_err(&pdev->dev, "Failed to request memory region!\n");
-		return -EBUSY;
-	}
+	struct resource *mem;
+	void __iomem *base;
+	struct bgpio_chip *mmc_gpio_chip;
+	u32 dt_hbi;
 
-	if (!vexpress_sysreg_base) {
-		vexpress_sysreg_base = devm_ioremap(&pdev->dev, res->start,
-				resource_size(res));
-		vexpress_sysreg_setup(pdev->dev.of_node);
-	}
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!mem)
+		return -EINVAL;
 
-	if (!vexpress_sysreg_base) {
-		dev_err(&pdev->dev, "Failed to obtain base address!\n");
-		return -EFAULT;
-	}
+	base = devm_ioremap(&pdev->dev, mem->start, resource_size(mem));
+	if (!base)
+		return -ENOMEM;
 
-	setup_timer(&vexpress_sysreg_config_timer,
-			vexpress_sysreg_config_complete, 0);
+	vexpress_config_set_master(vexpress_sysreg_get_master());
 
-	vexpress_sysreg_dev = &pdev->dev;
+	/* Confirm board type against DT property, if available */
+	if (of_property_read_u32(of_allnodes, "arm,hbi", &dt_hbi) == 0) {
+		u32 id = vexpress_get_procid(VEXPRESS_SITE_MASTER);
+		u32 hbi = (id >> SYS_PROCIDx_HBI_SHIFT) & SYS_HBI_MASK;
 
-#ifdef CONFIG_GPIOLIB
-	vexpress_sysreg_gpio_chip.dev = &pdev->dev;
-	err = gpiochip_add(&vexpress_sysreg_gpio_chip);
-	if (err) {
-		vexpress_config_bridge_unregister(
-				vexpress_sysreg_config_bridge);
-		dev_err(&pdev->dev, "Failed to register GPIO chip! (%d)\n",
-				err);
-		return err;
+		if (WARN_ON(dt_hbi != hbi))
+			dev_warn(&pdev->dev, "DT HBI (%x) is not matching hardware (%x)!\n",
+					dt_hbi, hbi);
 	}
 
-	platform_device_register_data(vexpress_sysreg_dev, "leds-gpio",
-			PLATFORM_DEVID_AUTO, &vexpress_sysreg_leds_pdata,
-			sizeof(vexpress_sysreg_leds_pdata));
-#endif
-
-	device_create_file(vexpress_sysreg_dev, &dev_attr_sys_id);
-
-	return 0;
+	/*
+	 * Duplicated SYS_MCI pseudo-GPIO controller for compatibility with
+	 * older trees using sysreg node for MMC control lines.
+	 */
+	mmc_gpio_chip = devm_kzalloc(&pdev->dev, sizeof(*mmc_gpio_chip),
+			GFP_KERNEL);
+	if (!mmc_gpio_chip)
+		return -ENOMEM;
+	bgpio_init(mmc_gpio_chip, &pdev->dev, 0x4, base + SYS_MCI,
+			NULL, NULL, NULL, NULL, 0);
+	mmc_gpio_chip->gc.ngpio = 2;
+	gpiochip_add(&mmc_gpio_chip->gc);
+
+	return mfd_add_devices(&pdev->dev, PLATFORM_DEVID_AUTO,
+			vexpress_sysreg_cells,
+			ARRAY_SIZE(vexpress_sysreg_cells), mem, 0, NULL);
 }
 
 static const struct of_device_id vexpress_sysreg_match[] = {
@@ -522,7 +265,12 @@ static struct platform_driver vexpress_sysreg_driver = {
 
 static int __init vexpress_sysreg_init(void)
 {
-	vexpress_sysreg_of_early_init();
+	struct device_node *node;
+
+	/* Need the sysreg early, before any other device... */
+	for_each_matching_node(node, vexpress_sysreg_match)
+		of_platform_device_create(node, NULL, NULL);
+
 	return platform_driver_register(&vexpress_sysreg_driver);
 }
 core_initcall(vexpress_sysreg_init);
diff --git a/drivers/mfd/wm5102-tables.c b/drivers/mfd/wm5102-tables.c
index 070f8cfbbd7..c8a993bd17a 100644
--- a/drivers/mfd/wm5102-tables.c
+++ b/drivers/mfd/wm5102-tables.c
@@ -333,7 +333,7 @@ static const struct reg_default wm5102_reg_default[] = {
 	{ 0x000001AA, 0x0004 },   /* R426   - FLL2 GPIO Clock */ 
 	{ 0x00000200, 0x0006 },   /* R512   - Mic Charge Pump 1 */ 
 	{ 0x00000210, 0x00D4 },   /* R528   - LDO1 Control 1 */ 
-	{ 0x00000212, 0x0001 },   /* R530   - LDO1 Control 2 */
+	{ 0x00000212, 0x0000 },   /* R530   - LDO1 Control 2 */
 	{ 0x00000213, 0x0344 },   /* R531   - LDO2 Control 1 */ 
 	{ 0x00000218, 0x01A6 },   /* R536   - Mic Bias Ctrl 1 */ 
 	{ 0x00000219, 0x01A6 },   /* R537   - Mic Bias Ctrl 2 */ 
@@ -1037,6 +1037,8 @@ static bool wm5102_readable_register(struct device *dev, unsigned int reg)
 	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_4:
 	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_5:
 	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_6:
+	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_7:
+	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_8:
 	case ARIZONA_COMFORT_NOISE_GENERATOR:
 	case ARIZONA_HAPTICS_CONTROL_1:
 	case ARIZONA_HAPTICS_CONTROL_2:
diff --git a/drivers/mfd/wm5110-tables.c b/drivers/mfd/wm5110-tables.c
index 1942b6f231d..41a7f6fb780 100644
--- a/drivers/mfd/wm5110-tables.c
+++ b/drivers/mfd/wm5110-tables.c
@@ -468,10 +468,12 @@ static const struct reg_default wm5110_reg_default[] = {
 	{ 0x00000062, 0x01FF },    /* R98    - Sample Rate Sequence Select 2 */
 	{ 0x00000063, 0x01FF },    /* R99    - Sample Rate Sequence Select 3 */
 	{ 0x00000064, 0x01FF },    /* R100   - Sample Rate Sequence Select 4 */
-	{ 0x00000068, 0x01FF },    /* R104   - Always On Triggers Sequence Select 1 */
-	{ 0x00000069, 0x01FF },    /* R105   - Always On Triggers Sequence Select 2 */
-	{ 0x0000006A, 0x01FF },    /* R106   - Always On Triggers Sequence Select 3 */
-	{ 0x0000006B, 0x01FF },    /* R107   - Always On Triggers Sequence Select 4 */
+	{ 0x00000066, 0x01FF },    /* R102   - Always On Triggers Sequence Select 1 */
+	{ 0x00000067, 0x01FF },    /* R103   - Always On Triggers Sequence Select 2 */
+	{ 0x00000068, 0x01FF },    /* R104   - Always On Triggers Sequence Select 3 */
+	{ 0x00000069, 0x01FF },    /* R105   - Always On Triggers Sequence Select 4 */
+	{ 0x0000006A, 0x01FF },    /* R106   - Always On Triggers Sequence Select 5 */
+	{ 0x0000006B, 0x01FF },    /* R107   - Always On Triggers Sequence Select 6 */
 	{ 0x00000070, 0x0000 },    /* R112   - Comfort Noise Generator */
 	{ 0x00000090, 0x0000 },    /* R144   - Haptics Control 1 */
 	{ 0x00000091, 0x7FFF },    /* R145   - Haptics Control 2 */
@@ -549,6 +551,7 @@ static const struct reg_default wm5110_reg_default[] = {
 	{ 0x000002A8, 0x1422 },    /* R680   - Mic Detect Level 3 */
 	{ 0x000002A9, 0x300A },    /* R681   - Mic Detect Level 4 */
 	{ 0x000002C3, 0x0000 },    /* R707   - Mic noise mix control 1 */
+	{ 0x000002CB, 0x0000 },    /* R715   - Isolation control */
 	{ 0x000002D3, 0x0000 },    /* R723   - Jack detect analogue */
 	{ 0x00000300, 0x0000 },    /* R768   - Input Enables */
 	{ 0x00000308, 0x0000 },    /* R776   - Input Rate */
@@ -1498,6 +1501,8 @@ static bool wm5110_readable_register(struct device *dev, unsigned int reg)
 	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_2:
 	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_3:
 	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_4:
+	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_5:
+	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_6:
 	case ARIZONA_COMFORT_NOISE_GENERATOR:
 	case ARIZONA_HAPTICS_CONTROL_1:
 	case ARIZONA_HAPTICS_CONTROL_2:
@@ -1580,6 +1585,7 @@ static bool wm5110_readable_register(struct device *dev, unsigned int reg)
 	case ARIZONA_MIC_DETECT_LEVEL_3:
 	case ARIZONA_MIC_DETECT_LEVEL_4:
 	case ARIZONA_MIC_NOISE_MIX_CONTROL_1:
+	case ARIZONA_ISOLATION_CONTROL:
 	case ARIZONA_JACK_DETECT_ANALOGUE:
 	case ARIZONA_INPUT_ENABLES:
 	case ARIZONA_INPUT_ENABLES_STATUS:
diff --git a/drivers/mfd/wm8400-core.c b/drivers/mfd/wm8400-core.c
index e5eae751aa1..c6fb5d16ca0 100644
--- a/drivers/mfd/wm8400-core.c
+++ b/drivers/mfd/wm8400-core.c
@@ -64,7 +64,7 @@ EXPORT_SYMBOL_GPL(wm8400_block_read);
 
 static int wm8400_register_codec(struct wm8400 *wm8400)
 {
-	struct mfd_cell cell = {
+	const struct mfd_cell cell = {
 		.name = "wm8400-codec",
 		.platform_data = wm8400,
 		.pdata_size = sizeof(*wm8400),
diff --git a/drivers/mfd/wm8997-tables.c b/drivers/mfd/wm8997-tables.c
index 5aa80768777..c7a81da64ee 100644
--- a/drivers/mfd/wm8997-tables.c
+++ b/drivers/mfd/wm8997-tables.c
@@ -174,10 +174,10 @@ static const struct reg_default wm8997_reg_default[] = {
 	{ 0x00000062, 0x01FF },    /* R98    - Sample Rate Sequence Select 2 */
 	{ 0x00000063, 0x01FF },    /* R99    - Sample Rate Sequence Select 3 */
 	{ 0x00000064, 0x01FF },    /* R100   - Sample Rate Sequence Select 4 */
-	{ 0x00000068, 0x01FF },    /* R104   - Always On Triggers Sequence Select 1 */
-	{ 0x00000069, 0x01FF },    /* R105   - Always On Triggers Sequence Select 2 */
-	{ 0x0000006A, 0x01FF },    /* R106   - Always On Triggers Sequence Select 3 */
-	{ 0x0000006B, 0x01FF },    /* R107   - Always On Triggers Sequence Select 4 */
+	{ 0x00000068, 0x01FF },    /* R104   - Always On Triggers Sequence Select 3 */
+	{ 0x00000069, 0x01FF },    /* R105   - Always On Triggers Sequence Select 4 */
+	{ 0x0000006A, 0x01FF },    /* R106   - Always On Triggers Sequence Select 5 */
+	{ 0x0000006B, 0x01FF },    /* R107   - Always On Triggers Sequence Select 6 */
 	{ 0x00000070, 0x0000 },    /* R112   - Comfort Noise Generator */
 	{ 0x00000090, 0x0000 },    /* R144   - Haptics Control 1 */
 	{ 0x00000091, 0x7FFF },    /* R145   - Haptics Control 2 */
@@ -814,10 +814,10 @@ static bool wm8997_readable_register(struct device *dev, unsigned int reg)
 	case ARIZONA_SAMPLE_RATE_SEQUENCE_SELECT_2:
 	case ARIZONA_SAMPLE_RATE_SEQUENCE_SELECT_3:
 	case ARIZONA_SAMPLE_RATE_SEQUENCE_SELECT_4:
-	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_1:
-	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_2:
 	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_3:
 	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_4:
+	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_5:
+	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_6:
 	case ARIZONA_COMFORT_NOISE_GENERATOR:
 	case ARIZONA_HAPTICS_CONTROL_1:
 	case ARIZONA_HAPTICS_CONTROL_2:
@@ -846,6 +846,7 @@ static bool wm8997_readable_register(struct device *dev, unsigned int reg)
 	case ARIZONA_RATE_ESTIMATOR_3:
 	case ARIZONA_RATE_ESTIMATOR_4:
 	case ARIZONA_RATE_ESTIMATOR_5:
+	case ARIZONA_DYNAMIC_FREQUENCY_SCALING_1:
 	case ARIZONA_FLL1_CONTROL_1:
 	case ARIZONA_FLL1_CONTROL_2:
 	case ARIZONA_FLL1_CONTROL_3:
@@ -880,6 +881,7 @@ static bool wm8997_readable_register(struct device *dev, unsigned int reg)
 	case ARIZONA_FLL2_GPIO_CLOCK:
 	case ARIZONA_MIC_CHARGE_PUMP_1:
 	case ARIZONA_LDO1_CONTROL_1:
+	case ARIZONA_LDO1_CONTROL_2:
 	case ARIZONA_LDO2_CONTROL_1:
 	case ARIZONA_MIC_BIAS_CTRL_1:
 	case ARIZONA_MIC_BIAS_CTRL_2:
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index 8baff0effc7..a43d0c46727 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -54,6 +54,7 @@ config AD525X_DPOT_SPI
 config ATMEL_PWM
 	tristate "Atmel AT32/AT91 PWM support"
 	depends on HAVE_CLK
+	depends on AVR32 || AT91SAM9263 || AT91SAM9RL || AT91SAM9G45
 	help
 	  This option enables device driver support for the PWM channels
 	  on certain Atmel processors.  Pulse Width Modulation is used for
@@ -200,7 +201,7 @@ config ICS932S401
 
 config ATMEL_SSC
 	tristate "Device driver for Atmel SSC peripheral"
-	depends on HAS_IOMEM
+	depends on HAS_IOMEM && (AVR32 || ARCH_AT91 || COMPILE_TEST)
 	---help---
 	  This option enables device driver support for Atmel Synchronized
 	  Serial Communication peripheral (SSC).
@@ -468,7 +469,7 @@ config BMP085_SPI
 config PCH_PHUB
 	tristate "Intel EG20T PCH/LAPIS Semicon IOH(ML7213/ML7223/ML7831) PHUB"
 	select GENERIC_NET_UTILS
-	depends on PCI
+	depends on PCI && (X86_32 || COMPILE_TEST)
 	help
 	  This driver is for PCH(Platform controller Hub) PHUB(Packet Hub) of
 	  Intel Topcliff which is an IOH(Input/Output Hub) for x86 embedded
@@ -515,6 +516,15 @@ config SRAM
 	  the genalloc API. It is supposed to be used for small on-chip SRAM
 	  areas found on many SoCs.
 
+config VEXPRESS_SYSCFG
+	bool "Versatile Express System Configuration driver"
+	depends on VEXPRESS_CONFIG
+	default y
+	help
+	  ARM Ltd. Versatile Express uses specialised platform configuration
+	  bus. System Configuration interface is one of the possible means
+	  of generating transactions on this bus.
+
 source "drivers/misc/c2port/Kconfig"
 source "drivers/misc/eeprom/Kconfig"
 source "drivers/misc/cb710/Kconfig"
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index 7eb4b69580c..d59ce1261b3 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -55,3 +55,4 @@ obj-$(CONFIG_SRAM)		+= sram.o
 obj-y				+= mic/
 obj-$(CONFIG_GENWQE)		+= genwqe/
 obj-$(CONFIG_ECHO)		+= echo/
+obj-$(CONFIG_VEXPRESS_SYSCFG)	+= vexpress-syscfg.o
diff --git a/drivers/misc/arm-charlcd.c b/drivers/misc/arm-charlcd.c
index b7ebf8021d9..c72e96b523e 100644
--- a/drivers/misc/arm-charlcd.c
+++ b/drivers/misc/arm-charlcd.c
@@ -11,6 +11,7 @@
 #include <linux/module.h>
 #include <linux/interrupt.h>
 #include <linux/platform_device.h>
+#include <linux/of.h>
 #include <linux/completion.h>
 #include <linux/delay.h>
 #include <linux/io.h>
@@ -366,11 +367,17 @@ static const struct dev_pm_ops charlcd_pm_ops = {
 	.resume = charlcd_resume,
 };
 
+static const struct of_device_id charlcd_match[] = {
+	{ .compatible = "arm,versatile-lcd", },
+	{}
+};
+
 static struct platform_driver charlcd_driver = {
 	.driver = {
 		.name = DRIVERNAME,
 		.owner = THIS_MODULE,
 		.pm = &charlcd_pm_ops,
+		.of_match_table = of_match_ptr(charlcd_match),
 	},
 	.remove = __exit_p(charlcd_remove),
 };
diff --git a/drivers/misc/ds1682.c b/drivers/misc/ds1682.c
index 6a672f9ef52..b909fb30232 100644
--- a/drivers/misc/ds1682.c
+++ b/drivers/misc/ds1682.c
@@ -85,7 +85,6 @@ static ssize_t ds1682_store(struct device *dev, struct device_attribute *attr,
 {
 	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
 	struct i2c_client *client = to_i2c_client(dev);
-	char *endp;
 	u64 val;
 	__le32 val_le;
 	int rc;
@@ -93,8 +92,8 @@ static ssize_t ds1682_store(struct device *dev, struct device_attribute *attr,
 	dev_dbg(dev, "ds1682_store() called on %s\n", attr->attr.name);
 
 	/* Decode input */
-	val = simple_strtoull(buf, &endp, 0);
-	if (buf == endp) {
+	rc = kstrtoull(buf, 0, &val);
+	if (rc < 0) {
 		dev_dbg(dev, "input string not a number\n");
 		return -EINVAL;
 	}
diff --git a/drivers/misc/genwqe/card_debugfs.c b/drivers/misc/genwqe/card_debugfs.c
index 50d2096ea1c..0a33ade6410 100644
--- a/drivers/misc/genwqe/card_debugfs.c
+++ b/drivers/misc/genwqe/card_debugfs.c
@@ -348,7 +348,7 @@ int genwqe_init_debugfs(struct genwqe_dev *cd)
 	char name[64];
 	unsigned int i;
 
-	sprintf(card_name, "%s%u_card", GENWQE_DEVNAME, cd->card_idx);
+	sprintf(card_name, "%s%d_card", GENWQE_DEVNAME, cd->card_idx);
 
 	root = debugfs_create_dir(card_name, cd->debugfs_genwqe);
 	if (!root) {
@@ -454,7 +454,7 @@ int genwqe_init_debugfs(struct genwqe_dev *cd)
 	}
 
 	for (i = 0; i <  GENWQE_MAX_VFS; i++) {
-		sprintf(name, "vf%d_jobtimeout_msec", i);
+		sprintf(name, "vf%u_jobtimeout_msec", i);
 
 		file = debugfs_create_u32(name, 0666, root,
 					  &cd->vf_jobtimeout_msec[i]);
diff --git a/drivers/misc/genwqe/card_utils.c b/drivers/misc/genwqe/card_utils.c
index d049d271699..62cc6bb3f62 100644
--- a/drivers/misc/genwqe/card_utils.c
+++ b/drivers/misc/genwqe/card_utils.c
@@ -454,7 +454,7 @@ int genwqe_setup_sgl(struct genwqe_dev *cd, struct genwqe_sgl *sgl,
  */
 int genwqe_free_sync_sgl(struct genwqe_dev *cd, struct genwqe_sgl *sgl)
 {
-	int rc;
+	int rc = 0;
 	struct pci_dev *pci_dev = cd->pci_dev;
 
 	if (sgl->fpage) {
@@ -718,7 +718,7 @@ int genwqe_set_interrupt_capability(struct genwqe_dev *cd, int count)
 	int rc;
 	struct pci_dev *pci_dev = cd->pci_dev;
 
-	rc = pci_enable_msi_block(pci_dev, count);
+	rc = pci_enable_msi_exact(pci_dev, count);
 	if (rc == 0)
 		cd->flags |= GENWQE_FLAG_MSI_ENABLED;
 	return rc;
diff --git a/drivers/misc/mei/amthif.c b/drivers/misc/mei/amthif.c
index b8deb345548..0d6234db00f 100644
--- a/drivers/misc/mei/amthif.c
+++ b/drivers/misc/mei/amthif.c
@@ -111,8 +111,6 @@ int mei_amthif_host_init(struct mei_device *dev)
 		return ret;
 	}
 
-	cl->state = MEI_FILE_CONNECTING;
-
 	ret = mei_cl_connect(cl, NULL);
 
 	dev->iamthif_state = MEI_IAMTHIF_IDLE;
diff --git a/drivers/misc/mei/bus.c b/drivers/misc/mei/bus.c
index ddc5ac92a20..0e993ef28b9 100644
--- a/drivers/misc/mei/bus.c
+++ b/drivers/misc/mei/bus.c
@@ -247,7 +247,7 @@ static int ___mei_cl_send(struct mei_cl *cl, u8 *buf, size_t length,
 		return id;
 
 	if (length > dev->me_clients[id].props.max_msg_length)
-		return -EINVAL;
+		return -EFBIG;
 
 	cb = mei_io_cb_init(cl, NULL);
 	if (!cb)
@@ -427,8 +427,6 @@ int mei_cl_enable_device(struct mei_cl_device *device)
 
 	mutex_lock(&dev->device_lock);
 
-	cl->state = MEI_FILE_CONNECTING;
-
 	err = mei_cl_connect(cl, NULL);
 	if (err < 0) {
 		mutex_unlock(&dev->device_lock);
diff --git a/drivers/misc/mei/client.c b/drivers/misc/mei/client.c
index 8c078b808cd..59d20c599b1 100644
--- a/drivers/misc/mei/client.c
+++ b/drivers/misc/mei/client.c
@@ -18,6 +18,7 @@
 #include <linux/sched.h>
 #include <linux/wait.h>
 #include <linux/delay.h>
+#include <linux/pm_runtime.h>
 
 #include <linux/mei.h>
 
@@ -415,6 +416,10 @@ void mei_host_client_init(struct work_struct *work)
 	dev->reset_count = 0;
 
 	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);
 }
 
 /**
@@ -425,6 +430,12 @@ void mei_host_client_init(struct work_struct *work)
  */
 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");
+		return false;
+	}
+
 	if (!dev->hbuf_is_ready) {
 		dev_dbg(&dev->pdev->dev, "hbuf is not ready\n");
 		return false;
@@ -460,9 +471,18 @@ int mei_cl_disconnect(struct mei_cl *cl)
 	if (cl->state != MEI_FILE_DISCONNECTING)
 		return 0;
 
+	rets = pm_runtime_get(&dev->pdev->dev);
+	if (rets < 0 && rets != -EINPROGRESS) {
+		pm_runtime_put_noidle(&dev->pdev->dev);
+		cl_err(dev, cl, "rpm: get failed %d\n", rets);
+		return rets;
+	}
+
 	cb = mei_io_cb_init(cl, NULL);
-	if (!cb)
-		return -ENOMEM;
+	if (!cb) {
+		rets = -ENOMEM;
+		goto free;
+	}
 
 	cb->fop_type = MEI_FOP_CLOSE;
 	if (mei_hbuf_acquire(dev)) {
@@ -494,8 +514,7 @@ int mei_cl_disconnect(struct mei_cl *cl)
 			cl_err(dev, cl, "wrong status client disconnect.\n");
 
 		if (err)
-			cl_dbg(dev, cl, "wait failed disconnect err=%08x\n",
-					err);
+			cl_dbg(dev, cl, "wait failed disconnect err=%d\n", err);
 
 		cl_err(dev, cl, "failed to disconnect from FW client.\n");
 	}
@@ -503,6 +522,10 @@ int mei_cl_disconnect(struct mei_cl *cl)
 	mei_io_list_flush(&dev->ctrl_rd_list, 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);
+
 	mei_io_cb_free(cb);
 	return rets;
 }
@@ -557,6 +580,13 @@ int mei_cl_connect(struct mei_cl *cl, struct file *file)
 
 	dev = cl->dev;
 
+	rets = pm_runtime_get(&dev->pdev->dev);
+	if (rets < 0 && rets != -EINPROGRESS) {
+		pm_runtime_put_noidle(&dev->pdev->dev);
+		cl_err(dev, cl, "rpm: get failed %d\n", rets);
+		return rets;
+	}
+
 	cb = mei_io_cb_init(cl, file);
 	if (!cb) {
 		rets = -ENOMEM;
@@ -567,6 +597,7 @@ int mei_cl_connect(struct mei_cl *cl, struct file *file)
 
 	/* run hbuf acquire last so we don't have to undo */
 	if (!mei_cl_is_other_connecting(cl) && mei_hbuf_acquire(dev)) {
+		cl->state = MEI_FILE_CONNECTING;
 		if (mei_hbm_cl_connect_req(dev, cl)) {
 			rets = -ENODEV;
 			goto out;
@@ -596,6 +627,10 @@ int mei_cl_connect(struct mei_cl *cl, struct file *file)
 	rets = cl->status;
 
 out:
+	cl_dbg(dev, cl, "rpm: autosuspend\n");
+	pm_runtime_mark_last_busy(&dev->pdev->dev);
+	pm_runtime_put_autosuspend(&dev->pdev->dev);
+
 	mei_io_cb_free(cb);
 	return rets;
 }
@@ -713,23 +748,31 @@ int mei_cl_read_start(struct mei_cl *cl, size_t length)
 		return  -ENOTTY;
 	}
 
+	rets = pm_runtime_get(&dev->pdev->dev);
+	if (rets < 0 && rets != -EINPROGRESS) {
+		pm_runtime_put_noidle(&dev->pdev->dev);
+		cl_err(dev, cl, "rpm: get failed %d\n", rets);
+		return rets;
+	}
+
 	cb = mei_io_cb_init(cl, NULL);
-	if (!cb)
-		return -ENOMEM;
+	if (!cb) {
+		rets = -ENOMEM;
+		goto out;
+	}
 
 	/* always allocate at least client max message */
 	length = max_t(size_t, length, dev->me_clients[i].props.max_msg_length);
 	rets = mei_io_cb_alloc_resp_buf(cb, length);
 	if (rets)
-		goto err;
+		goto out;
 
 	cb->fop_type = MEI_FOP_READ;
 	if (mei_hbuf_acquire(dev)) {
-		if (mei_hbm_cl_flow_control_req(dev, cl)) {
-			cl_err(dev, cl, "flow control send failed\n");
-			rets = -ENODEV;
-			goto err;
-		}
+		rets = mei_hbm_cl_flow_control_req(dev, cl);
+		if (rets < 0)
+			goto out;
+
 		list_add_tail(&cb->list, &dev->read_list.list);
 	} else {
 		list_add_tail(&cb->list, &dev->ctrl_wr_list.list);
@@ -737,9 +780,14 @@ int mei_cl_read_start(struct mei_cl *cl, size_t length)
 
 	cl->read_cb = cb;
 
-	return rets;
-err:
-	mei_io_cb_free(cb);
+out:
+	cl_dbg(dev, cl, "rpm: autosuspend\n");
+	pm_runtime_mark_last_busy(&dev->pdev->dev);
+	pm_runtime_put_autosuspend(&dev->pdev->dev);
+
+	if (rets)
+		mei_io_cb_free(cb);
+
 	return rets;
 }
 
@@ -776,7 +824,7 @@ int mei_cl_irq_write(struct mei_cl *cl, struct mei_cl_cb *cb,
 		return rets;
 
 	if (rets == 0) {
-		cl_dbg(dev, cl,	"No flow control credentials: not sending.\n");
+		cl_dbg(dev, cl, "No flow control credentials: not sending.\n");
 		return 0;
 	}
 
@@ -856,6 +904,12 @@ int mei_cl_write(struct mei_cl *cl, struct mei_cl_cb *cb, bool blocking)
 
 	cl_dbg(dev, cl, "mei_cl_write %d\n", buf->size);
 
+	rets = pm_runtime_get(&dev->pdev->dev);
+	if (rets < 0 && rets != -EINPROGRESS) {
+		pm_runtime_put_noidle(&dev->pdev->dev);
+		cl_err(dev, cl, "rpm: get failed %d\n", rets);
+		return rets;
+	}
 
 	cb->fop_type = MEI_FOP_WRITE;
 	cb->buf_idx = 0;
@@ -926,6 +980,10 @@ 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);
+
 	return rets;
 }
 
diff --git a/drivers/misc/mei/hbm.c b/drivers/misc/mei/hbm.c
index 4960288e543..804106209d7 100644
--- a/drivers/misc/mei/hbm.c
+++ b/drivers/misc/mei/hbm.c
@@ -14,10 +14,12 @@
  *
  */
 
+#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 "mei_dev.h"
 #include "hbm.h"
@@ -58,6 +60,34 @@ static int mei_cl_conn_status_to_errno(enum mei_cl_connect_status status)
 }
 
 /**
+ * mei_hbm_idle - set hbm to idle state
+ *
+ * @dev: the device structure
+ */
+void mei_hbm_idle(struct mei_device *dev)
+{
+	dev->init_clients_timer = 0;
+	dev->hbm_state = MEI_HBM_IDLE;
+}
+
+/**
+ * mei_hbm_reset - reset hbm counters and book keeping data structurs
+ *
+ * @dev: the device structure
+ */
+void mei_hbm_reset(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;
+
+	mei_hbm_idle(dev);
+}
+
+/**
  * mei_hbm_me_cl_allocate - allocates storage for me clients
  *
  * @dev: the device structure
@@ -69,9 +99,7 @@ static int mei_hbm_me_cl_allocate(struct mei_device *dev)
 	struct mei_me_client *clients;
 	int b;
 
-	dev->me_clients_num = 0;
-	dev->me_client_presentation_num = 0;
-	dev->me_client_index = 0;
+	mei_hbm_reset(dev);
 
 	/* count how many ME clients we have */
 	for_each_set_bit(b, dev->me_clients_map, MEI_CLIENTS_MAX)
@@ -80,9 +108,6 @@ static int mei_hbm_me_cl_allocate(struct mei_device *dev)
 	if (dev->me_clients_num == 0)
 		return 0;
 
-	kfree(dev->me_clients);
-	dev->me_clients = NULL;
-
 	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 */
@@ -133,17 +158,6 @@ bool mei_hbm_cl_addr_equal(struct mei_cl *cl, void *buf)
 }
 
 
-/**
- * mei_hbm_idle - set hbm to idle state
- *
- * @dev: the device structure
- */
-void mei_hbm_idle(struct mei_device *dev)
-{
-	dev->init_clients_timer = 0;
-	dev->hbm_state = MEI_HBM_IDLE;
-}
-
 int mei_hbm_start_wait(struct mei_device *dev)
 {
 	int ret;
@@ -289,6 +303,34 @@ static int mei_hbm_prop_req(struct mei_device *dev)
 	return 0;
 }
 
+/*
+ * mei_hbm_pg - sends pg command
+ *
+ * @dev: the device structure
+ * @pg_cmd: the pg command code
+ *
+ * This function returns -EIO on write failure
+ */
+int mei_hbm_pg(struct mei_device *dev, u8 pg_cmd)
+{
+	struct mei_msg_hdr *mei_hdr = &dev->wr_msg.hdr;
+	struct hbm_power_gate *req;
+	const size_t len = sizeof(struct hbm_power_gate);
+	int ret;
+
+	mei_hbm_hdr(mei_hdr, len);
+
+	req = (struct hbm_power_gate *)dev->wr_msg.data;
+	memset(req, 0, len);
+	req->hbm_cmd = 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");
+	return ret;
+}
+EXPORT_SYMBOL_GPL(mei_hbm_pg);
+
 /**
  * mei_hbm_stop_req - send stop request message
  *
@@ -701,6 +743,27 @@ int mei_hbm_dispatch(struct mei_device *dev, struct mei_msg_hdr *hdr)
 		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->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->pg_event = MEI_PG_EVENT_RECEIVED;
+		if (waitqueue_active(&dev->wait_pg))
+			wake_up(&dev->wait_pg);
+		else
+			/*
+			* If the driver is not waiting on this then
+			* this is HW initiated exit from PG.
+			* Start runtime pm resume sequence to exit from PG.
+			*/
+			pm_request_resume(&dev->pdev->dev);
+		break;
+
 	case HOST_CLIENT_PROPERTIES_RES_CMD:
 		dev_dbg(&dev->pdev->dev, "hbm: properties response: message received.\n");
 
diff --git a/drivers/misc/mei/hbm.h b/drivers/misc/mei/hbm.h
index 20e8782711c..683eb2835ce 100644
--- a/drivers/misc/mei/hbm.h
+++ b/drivers/misc/mei/hbm.h
@@ -50,6 +50,7 @@ static inline void mei_hbm_hdr(struct mei_msg_hdr *hdr, size_t length)
 }
 
 void mei_hbm_idle(struct mei_device *dev);
+void mei_hbm_reset(struct mei_device *dev);
 int mei_hbm_start_req(struct mei_device *dev);
 int mei_hbm_start_wait(struct mei_device *dev);
 int mei_hbm_cl_flow_control_req(struct mei_device *dev, struct mei_cl *cl);
@@ -57,6 +58,7 @@ int mei_hbm_cl_disconnect_req(struct mei_device *dev, struct mei_cl *cl);
 int mei_hbm_cl_disconnect_rsp(struct mei_device *dev, struct mei_cl *cl);
 int mei_hbm_cl_connect_req(struct mei_device *dev, struct mei_cl *cl);
 bool mei_hbm_version_is_supported(struct mei_device *dev);
+int mei_hbm_pg(struct mei_device *dev, u8 pg_cmd);
 
 #endif /* _MEI_HBM_H_ */
 
diff --git a/drivers/misc/mei/hw-me-regs.h b/drivers/misc/mei/hw-me-regs.h
index cabc0438368..a7856c0ac57 100644
--- a/drivers/misc/mei/hw-me-regs.h
+++ b/drivers/misc/mei/hw-me-regs.h
@@ -133,6 +133,8 @@
 #define ME_CB_RW   8
 /* ME_CSR_HA - ME Control Status Host Access register (read only) */
 #define ME_CSR_HA  0xC
+/* H_HGC_CSR - PGI register */
+#define H_HPG_CSR  0x10
 
 
 /* register bits of H_CSR (Host Control Status register) */
@@ -162,6 +164,8 @@ access to ME_CBD */
 #define ME_CBWP_HRA       0x00FF0000
 /* ME CB Read Pointer HRA - host read only access to ME_CBRP */
 #define ME_CBRP_HRA       0x0000FF00
+/* ME Power Gate Isolation Capability HRA  - host ready only access */
+#define ME_PGIC_HRA       0x00000040
 /* ME Reset HRA - host read only access to ME_RST */
 #define ME_RST_HRA        0x00000010
 /* ME Ready HRA - host read only access to ME_RDY */
@@ -173,4 +177,9 @@ access to ME_CBD */
 /* ME Interrupt Enable HRA - host read only access to ME_IE */
 #define ME_IE_HRA         0x00000001
 
+
+/* register bits - H_HPG_CSR */
+#define H_HPG_CSR_PGIHEXR       0x00000001
+#define H_HPG_CSR_PGI           0x00000002
+
 #endif /* _MEI_HW_MEI_REGS_H_ */
diff --git a/drivers/misc/mei/hw-me.c b/drivers/misc/mei/hw-me.c
index 8dbdaaef1af..6a2d272cea4 100644
--- a/drivers/misc/mei/hw-me.c
+++ b/drivers/misc/mei/hw-me.c
@@ -109,10 +109,27 @@ static inline void mei_hcsr_set(struct mei_me_hw *hw, u32 hcsr)
  */
 static void mei_me_hw_config(struct mei_device *dev)
 {
+	struct mei_me_hw *hw = to_me_hw(dev);
 	u32 hcsr = mei_hcsr_read(to_me_hw(dev));
 	/* Doesn't change in runtime */
 	dev->hbuf_depth = (hcsr & H_CBD) >> 24;
+
+	hw->pg_state = MEI_PG_OFF;
+}
+
+/**
+ * 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
+ */
+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
  *
@@ -164,6 +181,9 @@ static void mei_me_hw_reset_release(struct mei_device *dev)
 	hcsr |= H_IG;
 	hcsr &= ~H_RST;
 	mei_hcsr_set(hw, hcsr);
+
+	/* complete this write before we set host ready on another CPU */
+	mmiowb();
 }
 /**
  * mei_me_hw_reset - resets fw via mei csr register.
@@ -183,8 +203,21 @@ static int mei_me_hw_reset(struct mei_device *dev, bool intr_enable)
 	else
 		hcsr &= ~H_IE;
 
+	dev->recvd_hw_ready = false;
 	mei_me_reg_write(hw, H_CSR, hcsr);
 
+	/*
+	 * Host reads the H_CSR once to ensure that the
+	 * posted write to H_CSR completes.
+	 */
+	hcsr = mei_hcsr_read(hw);
+
+	if ((hcsr & H_RST) == 0)
+		dev_warn(&dev->pdev->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);
+
 	if (intr_enable == false)
 		mei_me_hw_reset_release(dev);
 
@@ -201,6 +234,7 @@ static int mei_me_hw_reset(struct mei_device *dev, bool intr_enable)
 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);
 }
@@ -233,10 +267,7 @@ static bool mei_me_hw_is_ready(struct mei_device *dev)
 static int mei_me_hw_ready_wait(struct mei_device *dev)
 {
 	int err;
-	if (mei_me_hw_is_ready(dev))
-		return 0;
 
-	dev->recvd_hw_ready = false;
 	mutex_unlock(&dev->device_lock);
 	err = wait_event_interruptible_timeout(dev->wait_hw_ready,
 			dev->recvd_hw_ready,
@@ -431,6 +462,144 @@ static int mei_me_read_slots(struct mei_device *dev, unsigned char *buffer,
 }
 
 /**
+ * mei_me_pg_enter - write pg enter register to mei device.
+ *
+ * @dev: the device structure
+ */
+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.
+ *
+ * @dev: the device structure
+ */
+static void mei_me_pg_exit(struct mei_device *dev)
+{
+	struct mei_me_hw *hw = to_me_hw(dev);
+	u32 reg = mei_me_reg_read(hw, H_HPG_CSR);
+
+	WARN(!(reg & H_HPG_CSR_PGI), "PGI is not set\n");
+
+	reg |= H_HPG_CSR_PGIHEXR;
+	mei_me_reg_write(hw, H_HPG_CSR, reg);
+}
+
+/**
+ * mei_me_pg_set_sync - perform pg entry procedure
+ *
+ * @dev: the device structure
+ *
+ * returns 0 on success an error code otherwise
+ */
+int mei_me_pg_set_sync(struct mei_device *dev)
+{
+	struct mei_me_hw *hw = to_me_hw(dev);
+	unsigned long timeout = mei_secs_to_jiffies(MEI_PGI_TIMEOUT);
+	int ret;
+
+	dev->pg_event = MEI_PG_EVENT_WAIT;
+
+	ret = mei_hbm_pg(dev, MEI_PG_ISOLATION_ENTRY_REQ_CMD);
+	if (ret)
+		return ret;
+
+	mutex_unlock(&dev->device_lock);
+	wait_event_timeout(dev->wait_pg,
+		dev->pg_event == MEI_PG_EVENT_RECEIVED, timeout);
+	mutex_lock(&dev->device_lock);
+
+	if (dev->pg_event == MEI_PG_EVENT_RECEIVED) {
+		mei_me_pg_enter(dev);
+		ret = 0;
+	} else {
+		ret = -ETIME;
+	}
+
+	dev->pg_event = MEI_PG_EVENT_IDLE;
+	hw->pg_state = MEI_PG_ON;
+
+	return ret;
+}
+
+/**
+ * mei_me_pg_unset_sync - perform pg exit procedure
+ *
+ * @dev: the device structure
+ *
+ * returns 0 on success an error code otherwise
+ */
+int mei_me_pg_unset_sync(struct mei_device *dev)
+{
+	struct mei_me_hw *hw = to_me_hw(dev);
+	unsigned long timeout = mei_secs_to_jiffies(MEI_PGI_TIMEOUT);
+	int ret;
+
+	if (dev->pg_event == MEI_PG_EVENT_RECEIVED)
+		goto reply;
+
+	dev->pg_event = MEI_PG_EVENT_WAIT;
+
+	mei_me_pg_exit(dev);
+
+	mutex_unlock(&dev->device_lock);
+	wait_event_timeout(dev->wait_pg,
+		dev->pg_event == MEI_PG_EVENT_RECEIVED, timeout);
+	mutex_lock(&dev->device_lock);
+
+reply:
+	if (dev->pg_event == MEI_PG_EVENT_RECEIVED)
+		ret = mei_hbm_pg(dev, MEI_PG_ISOLATION_EXIT_RES_CMD);
+	else
+		ret = -ETIME;
+
+	dev->pg_event = MEI_PG_EVENT_IDLE;
+	hw->pg_state = MEI_PG_OFF;
+
+	return ret;
+}
+
+/**
+ * mei_me_pg_is_enabled - detect if PG is supported by HW
+ *
+ * @dev: the device structure
+ *
+ * returns: true is pg supported, false otherwise
+ */
+static bool mei_me_pg_is_enabled(struct mei_device *dev)
+{
+	struct mei_me_hw *hw = to_me_hw(dev);
+	u32 reg = mei_me_reg_read(hw, ME_CSR_HA);
+
+	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)
+		goto notsupported;
+
+	return true;
+
+notsupported:
+	dev_dbg(&dev->pdev->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,
+		HBM_MAJOR_VERSION_PGI,
+		HBM_MINOR_VERSION_PGI);
+
+	return false;
+}
+
+/**
  * mei_me_irq_quick_handler - The ISR of the MEI device
  *
  * @irq: The irq number
@@ -491,14 +660,13 @@ irqreturn_t mei_me_irq_thread_handler(int irq, void *dev_id)
 	/*  check if we need to start the dev */
 	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->recvd_hw_ready = true;
 			wake_up_interruptible(&dev->wait_hw_ready);
 		} else {
-
-			dev_dbg(&dev->pdev->dev, "Reset Completed.\n");
-			mei_me_hw_reset_release(dev);
+			dev_dbg(&dev->pdev->dev, "Spurious Interrupt\n");
 		}
 		goto end;
 	}
@@ -524,9 +692,15 @@ irqreturn_t mei_me_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);
-
-	dev->hbuf_is_ready = mei_hbuf_is_ready(dev);
+	/*
+	 * During PG handshake only allowed write is the replay to the
+	 * PG exit message, so block calling write function
+	 * if the pg state is not idle
+	 */
+	if (dev->pg_event == MEI_PG_EVENT_IDLE) {
+		rets = mei_irq_write_handler(dev, &complete_list);
+		dev->hbuf_is_ready = mei_hbuf_is_ready(dev);
+	}
 
 	mei_irq_compl_handler(dev, &complete_list);
 
@@ -535,8 +709,65 @@ end:
 	mutex_unlock(&dev->device_lock);
 	return IRQ_HANDLED;
 }
+
+/**
+ * mei_me_fw_status - retrieve fw status from the pci config space
+ *
+ * @dev: the device structure
+ * @fw_status: fw status registers storage
+ *
+ * returns 0 on success an error code otherwise
+ */
+static int mei_me_fw_status(struct mei_device *dev,
+			    struct mei_fw_status *fw_status)
+{
+	const u32 pci_cfg_reg[] = {PCI_CFG_HFS_1, PCI_CFG_HFS_2};
+	int i;
+
+	if (!fw_status)
+		return -EINVAL;
+
+	switch (dev->pdev->device) {
+	case MEI_DEV_ID_IBXPK_1:
+	case MEI_DEV_ID_IBXPK_2:
+	case MEI_DEV_ID_CPT_1:
+	case MEI_DEV_ID_PBG_1:
+	case MEI_DEV_ID_PPT_1:
+	case MEI_DEV_ID_PPT_2:
+	case MEI_DEV_ID_PPT_3:
+	case MEI_DEV_ID_LPT_H:
+	case MEI_DEV_ID_LPT_W:
+	case MEI_DEV_ID_LPT_LP:
+	case MEI_DEV_ID_LPT_HR:
+	case MEI_DEV_ID_WPT_LP:
+		fw_status->count = 2;
+		break;
+	case MEI_DEV_ID_ICH10_1:
+	case MEI_DEV_ID_ICH10_2:
+	case MEI_DEV_ID_ICH10_3:
+	case MEI_DEV_ID_ICH10_4:
+		fw_status->count = 1;
+		break;
+	default:
+		fw_status->count = 0;
+		break;
+	}
+
+	for (i = 0; i < fw_status->count && i < MEI_FW_STATUS_MAX; i++) {
+		int ret;
+		ret = pci_read_config_dword(dev->pdev,
+				pci_cfg_reg[i], &fw_status->status[i]);
+		if (ret)
+			return ret;
+	}
+	return 0;
+}
+
 static const struct mei_hw_ops mei_me_hw_ops = {
 
+	.pg_state  = mei_me_pg_state,
+
+	.fw_status = mei_me_fw_status,
 	.host_is_ready = mei_me_host_is_ready,
 
 	.hw_is_ready = mei_me_hw_is_ready,
@@ -544,6 +775,8 @@ static const struct mei_hw_ops mei_me_hw_ops = {
 	.hw_config = mei_me_hw_config,
 	.hw_start = mei_me_hw_start,
 
+	.pg_is_enabled = mei_me_pg_is_enabled,
+
 	.intr_clear = mei_me_intr_clear,
 	.intr_enable = mei_me_intr_enable,
 	.intr_disable = mei_me_intr_disable,
@@ -559,14 +792,81 @@ static const struct mei_hw_ops mei_me_hw_ops = {
 	.read = mei_me_read_slots
 };
 
+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;
+}
+
+#define MEI_CFG_FW_NM                           \
+	.quirk_probe = mei_me_fw_type_nm
+
+static bool mei_me_fw_type_sps(struct pci_dev *pdev)
+{
+	u32 reg;
+	/* Read ME FW Status check for SPS Firmware */
+	pci_read_config_dword(pdev, PCI_CFG_HFS_1, &reg);
+	/* if bits [19:16] = 15, running SPS Firmware */
+	return (reg & 0xf0000) == 0xf0000;
+}
+
+#define MEI_CFG_FW_SPS                           \
+	.quirk_probe = mei_me_fw_type_sps
+
+
+#define MEI_CFG_LEGACY_HFS                      \
+	.fw_status.count = 0
+
+#define MEI_CFG_ICH_HFS                        \
+	.fw_status.count = 1,                   \
+	.fw_status.status[0] = PCI_CFG_HFS_1
+
+#define MEI_CFG_PCH_HFS                         \
+	.fw_status.count = 2,                   \
+	.fw_status.status[0] = PCI_CFG_HFS_1,   \
+	.fw_status.status[1] = PCI_CFG_HFS_2
+
+
+/* ICH Legacy devices */
+const struct mei_cfg mei_me_legacy_cfg = {
+	MEI_CFG_LEGACY_HFS,
+};
+
+/* ICH devices */
+const struct mei_cfg mei_me_ich_cfg = {
+	MEI_CFG_ICH_HFS,
+};
+
+/* PCH devices */
+const struct mei_cfg mei_me_pch_cfg = {
+	MEI_CFG_PCH_HFS,
+};
+
+
+/* PCH Cougar Point and Patsburg with quirk for Node Manager exclusion */
+const struct mei_cfg mei_me_pch_cpt_pbg_cfg = {
+	MEI_CFG_PCH_HFS,
+	MEI_CFG_FW_NM,
+};
+
+/* PCH Lynx Point with quirk for SPS Firmware exclusion */
+const struct mei_cfg mei_me_lpt_cfg = {
+	MEI_CFG_PCH_HFS,
+	MEI_CFG_FW_SPS,
+};
+
 /**
  * mei_me_dev_init - allocates and initializes the mei device structure
  *
  * @pdev: The pci device structure
+ * @cfg: per device generation config
  *
  * returns The mei_device_device pointer on success, NULL on failure.
  */
-struct mei_device *mei_me_dev_init(struct pci_dev *pdev)
+struct mei_device *mei_me_dev_init(struct pci_dev *pdev,
+				   const struct mei_cfg *cfg)
 {
 	struct mei_device *dev;
 
@@ -575,7 +875,7 @@ struct mei_device *mei_me_dev_init(struct pci_dev *pdev)
 	if (!dev)
 		return NULL;
 
-	mei_device_init(dev);
+	mei_device_init(dev, cfg);
 
 	dev->ops = &mei_me_hw_ops;
 
diff --git a/drivers/misc/mei/hw-me.h b/drivers/misc/mei/hw-me.h
index 893d5119fa9..12b0f4bbe1f 100644
--- a/drivers/misc/mei/hw-me.h
+++ b/drivers/misc/mei/hw-me.h
@@ -24,6 +24,8 @@
 #include "mei_dev.h"
 #include "client.h"
 
+#define MEI_ME_RPM_TIMEOUT    500 /* ms */
+
 struct mei_me_hw {
 	void __iomem *mem_addr;
 	/*
@@ -31,11 +33,22 @@ struct mei_me_hw {
 	 */
 	u32 host_hw_state;
 	u32 me_hw_state;
+	enum mei_pg_state pg_state;
 };
 
 #define to_me_hw(dev) (struct mei_me_hw *)((dev)->hw)
 
-struct mei_device *mei_me_dev_init(struct pci_dev *pdev);
+extern const struct mei_cfg mei_me_legacy_cfg;
+extern const struct mei_cfg mei_me_ich_cfg;
+extern const struct mei_cfg mei_me_pch_cfg;
+extern const struct mei_cfg mei_me_pch_cpt_pbg_cfg;
+extern const struct mei_cfg mei_me_lpt_cfg;
+
+struct mei_device *mei_me_dev_init(struct pci_dev *pdev,
+				   const struct mei_cfg *cfg);
+
+int mei_me_pg_set_sync(struct mei_device *dev);
+int mei_me_pg_unset_sync(struct mei_device *dev);
 
 irqreturn_t mei_me_irq_quick_handler(int irq, void *dev_id);
 irqreturn_t mei_me_irq_thread_handler(int irq, void *dev_id);
diff --git a/drivers/misc/mei/hw-txe-regs.h b/drivers/misc/mei/hw-txe-regs.h
index 7283c24c1af..f19229c4e65 100644
--- a/drivers/misc/mei/hw-txe-regs.h
+++ b/drivers/misc/mei/hw-txe-regs.h
@@ -89,7 +89,7 @@ enum {
 #  define PCI_CFG_TXE_FW_STS0_ERR_CODE_MSK  0x0000F000
 #  define PCI_CFG_TXE_FW_STS0_OP_MODE_MSK   0x000F0000
 #  define PCI_CFG_TXE_FW_STS0_RST_CNT_MSK   0x00F00000
-
+#define PCI_CFG_TXE_FW_STS1   0x48
 
 #define IPC_BASE_ADDR	0x80400 /* SeC IPC Base Address */
 
diff --git a/drivers/misc/mei/hw-txe.c b/drivers/misc/mei/hw-txe.c
index f60182a52f9..93273783dec 100644
--- a/drivers/misc/mei/hw-txe.c
+++ b/drivers/misc/mei/hw-txe.c
@@ -158,7 +158,7 @@ static bool mei_txe_aliveness_set(struct mei_device *dev, u32 req)
 	dev_dbg(&dev->pdev->dev, "Aliveness current=%d request=%d\n",
 				hw->aliveness, req);
 	if (do_req) {
-		hw->recvd_aliveness = false;
+		dev->pg_event = MEI_PG_EVENT_WAIT;
 		mei_txe_br_reg_write(hw, SICR_HOST_ALIVENESS_REQ_REG, req);
 	}
 	return do_req;
@@ -213,6 +213,7 @@ static int mei_txe_aliveness_poll(struct mei_device *dev, u32 expected)
 	do {
 		hw->aliveness = mei_txe_aliveness_get(dev);
 		if (hw->aliveness == expected) {
+			dev->pg_event = MEI_PG_EVENT_IDLE;
 			dev_dbg(&dev->pdev->dev,
 				"aliveness settled after %d msecs\n", t);
 			return t;
@@ -223,6 +224,7 @@ static int mei_txe_aliveness_poll(struct mei_device *dev, u32 expected)
 		t += MSEC_PER_SEC / 5;
 	} while (t < SEC_ALIVENESS_WAIT_TIMEOUT);
 
+	dev->pg_event = MEI_PG_EVENT_IDLE;
 	dev_err(&dev->pdev->dev, "aliveness timed out\n");
 	return -ETIME;
 }
@@ -249,19 +251,22 @@ static int mei_txe_aliveness_wait(struct mei_device *dev, u32 expected)
 		return 0;
 
 	mutex_unlock(&dev->device_lock);
-	err = wait_event_timeout(hw->wait_aliveness,
-			hw->recvd_aliveness, timeout);
+	err = wait_event_timeout(hw->wait_aliveness_resp,
+			dev->pg_event == MEI_PG_EVENT_RECEIVED, timeout);
 	mutex_lock(&dev->device_lock);
 
 	hw->aliveness = mei_txe_aliveness_get(dev);
 	ret = hw->aliveness == expected ? 0 : -ETIME;
 
 	if (ret)
-		dev_err(&dev->pdev->dev, "aliveness timed out");
+		dev_warn(&dev->pdev->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 msecs\n",
-				jiffies_to_msecs(timeout - err));
-	hw->recvd_aliveness = false;
+		dev_dbg(&dev->pdev->dev, "aliveness settled after = %d msec aliveness = %d event = %d\n",
+			jiffies_to_msecs(timeout - err),
+			hw->aliveness, dev->pg_event);
+
+	dev->pg_event = MEI_PG_EVENT_IDLE;
 	return ret;
 }
 
@@ -280,6 +285,32 @@ int mei_txe_aliveness_set_sync(struct mei_device *dev, u32 req)
 }
 
 /**
+ * mei_txe_pg_is_enabled - detect if PG is supported by HW
+ *
+ * @dev: the device structure
+ *
+ * returns: true is pg supported, false otherwise
+ */
+static bool mei_txe_pg_is_enabled(struct mei_device *dev)
+{
+	return true;
+}
+
+/**
+ * mei_txe_pg_state  - translate aliveness register value
+ *   to the mei power gating state
+ *
+ * @dev: the device structure
+ *
+ * returns: 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;
+}
+
+/**
  * mei_txe_input_ready_interrupt_enable - sets the Input Ready Interrupt
  *
  * @dev: the device structure
@@ -589,7 +620,10 @@ static int mei_txe_write(struct mei_device *dev,
 	mei_txe_input_ready_interrupt_enable(dev);
 
 	if (!mei_txe_is_input_ready(dev)) {
-		dev_err(&dev->pdev->dev, "Input is not ready");
+		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",
+			FW_STS_PRM(fw_status));
 		return -EAGAIN;
 	}
 
@@ -960,9 +994,9 @@ irqreturn_t mei_txe_irq_thread_handler(int irq, void *dev_id)
 		/* Clear the interrupt cause */
 		dev_dbg(&dev->pdev->dev,
 			"Aliveness Interrupt: Status: %d\n", hw->aliveness);
-		hw->recvd_aliveness = true;
-		if (waitqueue_active(&hw->wait_aliveness))
-			wake_up(&hw->wait_aliveness);
+		dev->pg_event = MEI_PG_EVENT_RECEIVED;
+		if (waitqueue_active(&hw->wait_aliveness_resp))
+			wake_up(&hw->wait_aliveness_resp);
 	}
 
 
@@ -1008,15 +1042,51 @@ end:
 	return IRQ_HANDLED;
 }
 
+
+/**
+ * mei_txe_fw_status - retrieve fw status from the pci config space
+ *
+ * @dev: the device structure
+ * @fw_status: fw status registers storage
+ *
+ * returns: 0 on success an error code otherwise
+ */
+static int mei_txe_fw_status(struct mei_device *dev,
+			     struct mei_fw_status *fw_status)
+{
+	const u32 pci_cfg_reg[] = {PCI_CFG_TXE_FW_STS0, PCI_CFG_TXE_FW_STS1};
+	int i;
+
+	if (!fw_status)
+		return -EINVAL;
+
+	fw_status->count = 2;
+
+	for (i = 0; i < fw_status->count && i < MEI_FW_STATUS_MAX; i++) {
+		int ret;
+		ret = pci_read_config_dword(dev->pdev,
+				pci_cfg_reg[i], &fw_status->status[i]);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
 static const struct mei_hw_ops mei_txe_hw_ops = {
 
+	.fw_status = mei_txe_fw_status,
 	.host_is_ready = mei_txe_host_is_ready,
 
+	.pg_state = mei_txe_pg_state,
+
 	.hw_is_ready = mei_txe_hw_is_ready,
 	.hw_reset = mei_txe_hw_reset,
 	.hw_config = mei_txe_hw_config,
 	.hw_start = mei_txe_hw_start,
 
+	.pg_is_enabled = mei_txe_pg_is_enabled,
+
 	.intr_clear = mei_txe_intr_clear,
 	.intr_enable = mei_txe_intr_enable,
 	.intr_disable = mei_txe_intr_disable,
@@ -1034,14 +1104,27 @@ 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;
  *
  */
-struct mei_device *mei_txe_dev_init(struct pci_dev *pdev)
+struct mei_device *mei_txe_dev_init(struct pci_dev *pdev,
+				    const struct mei_cfg *cfg)
 {
 	struct mei_device *dev;
 	struct mei_txe_hw *hw;
@@ -1051,11 +1134,11 @@ struct mei_device *mei_txe_dev_init(struct pci_dev *pdev)
 	if (!dev)
 		return NULL;
 
-	mei_device_init(dev);
+	mei_device_init(dev, cfg);
 
 	hw = to_txe_hw(dev);
 
-	init_waitqueue_head(&hw->wait_aliveness);
+	init_waitqueue_head(&hw->wait_aliveness_resp);
 
 	dev->ops = &mei_txe_hw_ops;
 
diff --git a/drivers/misc/mei/hw-txe.h b/drivers/misc/mei/hw-txe.h
index 0812d98633a..e244af79167 100644
--- a/drivers/misc/mei/hw-txe.h
+++ b/drivers/misc/mei/hw-txe.h
@@ -22,6 +22,8 @@
 #include "hw.h"
 #include "hw-txe-regs.h"
 
+#define MEI_TXI_RPM_TIMEOUT    500 /* ms */
+
 /* Flatten Hierarchy interrupt cause */
 #define TXE_INTR_READINESS_BIT  0 /* HISR_INT_0_STS */
 #define TXE_INTR_READINESS      HISR_INT_0_STS
@@ -35,12 +37,11 @@
 /**
  * struct mei_txe_hw - txe hardware specifics
  *
- * @mem_addr:        SeC and BRIDGE bars
- * @aliveness:       aliveness (power gating) state of the hardware
- * @readiness:       readiness state of the hardware
- * @wait_aliveness:  aliveness wait queue
- * @recvd_aliveness: aliveness interrupt was recived
- * @intr_cause:      translated interrupt cause
+ * @mem_addr:            SeC and BRIDGE bars
+ * @aliveness:           aliveness (power gating) state of the hardware
+ * @readiness:           readiness state of the hardware
+ * @wait_aliveness_resp: aliveness wait queue
+ * @intr_cause:          translated interrupt cause
  */
 struct mei_txe_hw {
 	void __iomem *mem_addr[NUM_OF_MEM_BARS];
@@ -48,8 +49,7 @@ struct mei_txe_hw {
 	u32 readiness;
 	u32 slots;
 
-	wait_queue_head_t wait_aliveness;
-	bool recvd_aliveness;
+	wait_queue_head_t wait_aliveness_resp;
 
 	unsigned long intr_cause;
 };
@@ -61,7 +61,10 @@ static inline struct mei_device *hw_txe_to_mei(struct mei_txe_hw *hw)
 	return container_of((void *)hw, struct mei_device, hw);
 }
 
-struct mei_device *mei_txe_dev_init(struct pci_dev *pdev);
+extern const struct mei_cfg mei_txe_cfg;
+
+struct mei_device *mei_txe_dev_init(struct pci_dev *pdev,
+	const struct mei_cfg *cfg);
 
 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 6b476ab49b2..dd448e58cc8 100644
--- a/drivers/misc/mei/hw.h
+++ b/drivers/misc/mei/hw.h
@@ -31,14 +31,21 @@
 #define MEI_IAMTHIF_STALL_TIMER    12  /* HPS */
 #define MEI_IAMTHIF_READ_TIMER     10  /* HPS */
 
+#define MEI_PGI_TIMEOUT            1  /* PG Isolation time response 1 sec */
 #define MEI_HBM_TIMEOUT            1   /* 1 second */
 
 /*
  * MEI Version
  */
-#define HBM_MINOR_VERSION                   0
+#define HBM_MINOR_VERSION                   1
 #define HBM_MAJOR_VERSION                   1
 
+/*
+ * MEI version with PGI support
+ */
+#define HBM_MINOR_VERSION_PGI               1
+#define HBM_MAJOR_VERSION_PGI               1
+
 /* Host bus message command opcode */
 #define MEI_HBM_CMD_OP_MSK                  0x7f
 /* Host bus message command RESPONSE */
@@ -69,6 +76,11 @@
 
 #define MEI_FLOW_CONTROL_CMD                0x08
 
+#define MEI_PG_ISOLATION_ENTRY_REQ_CMD      0x0a
+#define MEI_PG_ISOLATION_ENTRY_RES_CMD      0x8a
+#define MEI_PG_ISOLATION_EXIT_REQ_CMD       0x0b
+#define MEI_PG_ISOLATION_EXIT_RES_CMD       0x8b
+
 /*
  * MEI Stop Reason
  * used by hbm_host_stop_request.reason
@@ -208,6 +220,17 @@ struct hbm_props_response {
 } __packed;
 
 /**
+ * struct hbm_power_gate - power gate request/response
+ *
+ * @hbm_cmd - bus message command header
+ * @reserved[3]
+ */
+struct hbm_power_gate {
+	u8 hbm_cmd;
+	u8 reserved[3];
+} __packed;
+
+/**
  * struct hbm_client_connect_request - connect/disconnect request
  *
  * @hbm_cmd - bus message command header
diff --git a/drivers/misc/mei/init.c b/drivers/misc/mei/init.c
index 4460975c0ee..00692922248 100644
--- a/drivers/misc/mei/init.c
+++ b/drivers/misc/mei/init.c
@@ -74,9 +74,13 @@ int mei_reset(struct mei_device *dev)
 	if (state != MEI_DEV_INITIALIZING &&
 	    state != MEI_DEV_DISABLED &&
 	    state != MEI_DEV_POWER_DOWN &&
-	    state != MEI_DEV_POWER_UP)
-		dev_warn(&dev->pdev->dev, "unexpected reset: dev_state = %s\n",
-			 mei_dev_state_str(state));
+	    state != MEI_DEV_POWER_UP) {
+		struct mei_fw_status fw_status;
+		mei_fw_status(dev, &fw_status);
+		dev_warn(&dev->pdev->dev,
+			"unexpected reset: dev_state = %s " FW_STS_FMT "\n",
+			mei_dev_state_str(state), FW_STS_PRM(fw_status));
+	}
 
 	/* we're already in reset, cancel the init timer
 	 * if the reset was called due the hbm protocol error
@@ -118,8 +122,8 @@ int mei_reset(struct mei_device *dev)
 		mei_amthif_reset_params(dev);
 	}
 
+	mei_hbm_reset(dev);
 
-	dev->me_clients_num = 0;
 	dev->rd_msg_hdr = 0;
 	dev->wd_pending = false;
 
@@ -303,15 +307,58 @@ void mei_stop(struct mei_device *dev)
 }
 EXPORT_SYMBOL_GPL(mei_stop);
 
+/**
+ * mei_write_is_idle - check if the write queues are idle
+ *
+ * @dev: the device structure
+ *
+ * returns true of there is no pending write
+ */
+bool mei_write_is_idle(struct mei_device *dev)
+{
+	bool idle = (dev->dev_state == MEI_DEV_ENABLED &&
+		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",
+		idle,
+		mei_dev_state_str(dev->dev_state),
+		list_empty(&dev->ctrl_wr_list.list),
+		list_empty(&dev->write_list.list));
 
-void mei_device_init(struct mei_device *dev)
+	return idle;
+}
+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)
 {
 	/* setup our list array */
 	INIT_LIST_HEAD(&dev->file_list);
 	INIT_LIST_HEAD(&dev->device_list);
 	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_stop_wd);
 	dev->dev_state = MEI_DEV_INITIALIZING;
@@ -340,6 +387,9 @@ void mei_device_init(struct mei_device *dev)
 	 * 0: Reserved for MEI Bus Message communications
 	 */
 	bitmap_set(dev->host_clients_map, 0, 1);
+
+	dev->pg_event = MEI_PG_EVENT_IDLE;
+	dev->cfg      = cfg;
 }
 EXPORT_SYMBOL_GPL(mei_device_init);
 
diff --git a/drivers/misc/mei/main.c b/drivers/misc/mei/main.c
index 147413145c9..66f0a1a0645 100644
--- a/drivers/misc/mei/main.c
+++ b/drivers/misc/mei/main.c
@@ -467,7 +467,6 @@ static int mei_ioctl_connect_client(struct file *file,
 	}
 
 	cl->me_client_id = dev->me_clients[i].client_id;
-	cl->state = MEI_FILE_CONNECTING;
 
 	dev_dbg(&dev->pdev->dev, "Connect to FW Client ID = %d\n",
 			cl->me_client_id);
diff --git a/drivers/misc/mei/mei_dev.h b/drivers/misc/mei/mei_dev.h
index 94a516716d2..5c7e990e2f2 100644
--- a/drivers/misc/mei/mei_dev.h
+++ b/drivers/misc/mei/mei_dev.h
@@ -153,6 +153,20 @@ struct mei_msg_data {
 	unsigned char *data;
 };
 
+/* Maximum number of processed FW status registers */
+#define MEI_FW_STATUS_MAX 2
+
+/*
+ * struct mei_fw_status - storage of FW status data
+ *
+ * @count - number of actually available elements in array
+ * @status - FW status registers
+ */
+struct mei_fw_status {
+	int count;
+	u32 status[MEI_FW_STATUS_MAX];
+};
+
 /**
  * struct mei_me_client - representation of me (fw) client
  *
@@ -213,6 +227,7 @@ struct mei_cl {
 
 /** struct mei_hw_ops
  *
+ * @fw_status        - read FW status from PCI config space
  * @host_is_ready    - query for host readiness
 
  * @hw_is_ready      - query if hw is ready
@@ -220,6 +235,9 @@ struct mei_cl {
  * @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
+
  * @intr_clear       - clear pending interrupts
  * @intr_enable      - enable interrupts
  * @intr_disable     - disable interrupts
@@ -237,6 +255,8 @@ struct mei_cl {
  */
 struct mei_hw_ops {
 
+	int (*fw_status)(struct mei_device *dev,
+		struct mei_fw_status *fw_status);
 	bool (*host_is_ready)(struct mei_device *dev);
 
 	bool (*hw_is_ready)(struct mei_device *dev);
@@ -244,6 +264,9 @@ struct mei_hw_ops {
 	int (*hw_start)(struct mei_device *dev);
 	void (*hw_config)(struct mei_device *dev);
 
+	enum mei_pg_state (*pg_state)(struct mei_device *dev);
+	bool (*pg_is_enabled)(struct mei_device *dev);
+
 	void (*intr_clear)(struct mei_device *dev);
 	void (*intr_enable)(struct mei_device *dev);
 	void (*intr_disable)(struct mei_device *dev);
@@ -331,16 +354,61 @@ struct mei_cl_device {
 	void *priv_data;
 };
 
+
+ /**
+ * enum mei_pg_event - power gating transition events
+ *
+ * @MEI_PG_EVENT_IDLE: the driver is not in power gating transition
+ * @MEI_PG_EVENT_WAIT: the driver is waiting for a pg event to complete
+ * @MEI_PG_EVENT_RECEIVED: the driver received pg event
+ */
+enum mei_pg_event {
+	MEI_PG_EVENT_IDLE,
+	MEI_PG_EVENT_WAIT,
+	MEI_PG_EVENT_RECEIVED,
+};
+
+/**
+ * enum mei_pg_state - device internal power gating state
+ *
+ * @MEI_PG_OFF: device is not power gated - it is active
+ * @MEI_PG_ON:  device is power gated - it is in lower power state
+ */
+enum mei_pg_state {
+	MEI_PG_OFF = 0,
+	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)
+
+
 /**
  * struct mei_device -  MEI private device struct
 
  * @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
  */
 struct mei_device {
 	struct pci_dev *pdev;	/* pointer to pci device struct */
@@ -371,6 +439,7 @@ struct mei_device {
 	 * waiting queue for receive message from FW
 	 */
 	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_stop_wd;
 
@@ -382,6 +451,14 @@ struct mei_device {
 	enum mei_hbm_state hbm_state;
 	u16 init_clients_timer;
 
+	/*
+	 * Power Gating support
+	 */
+	enum mei_pg_event pg_event;
+#ifdef CONFIG_PM_RUNTIME
+	struct dev_pm_domain pg_domain;
+#endif /* CONFIG_PM_RUNTIME */
+
 	unsigned char rd_msg_buf[MEI_RD_MSG_BUF_SIZE];	/* control messages */
 	u32 rd_msg_hdr;
 
@@ -442,6 +519,7 @@ struct mei_device {
 
 
 	const struct mei_hw_ops *ops;
+	const struct mei_cfg *cfg;
 	char hw[0] __aligned(sizeof(void *));
 };
 
@@ -474,7 +552,7 @@ static inline u32 mei_slots2data(int slots)
 /*
  * mei init function prototypes
  */
-void mei_device_init(struct mei_device *dev);
+void mei_device_init(struct mei_device *dev, const struct mei_cfg *cfg);
 int mei_reset(struct mei_device *dev);
 int mei_start(struct mei_device *dev);
 int mei_restart(struct mei_device *dev);
@@ -553,10 +631,22 @@ void mei_watchdog_unregister(struct mei_device *dev);
  * Register Access Function
  */
 
+
 static inline void mei_hw_config(struct mei_device *dev)
 {
 	dev->ops->hw_config(dev);
 }
+
+static inline enum mei_pg_state mei_pg_state(struct mei_device *dev)
+{
+	return dev->ops->pg_state(dev);
+}
+
+static inline bool mei_pg_is_enabled(struct mei_device *dev)
+{
+	return dev->ops->pg_is_enabled(dev);
+}
+
 static inline int mei_hw_reset(struct mei_device *dev, bool enable)
 {
 	return dev->ops->hw_reset(dev, enable);
@@ -629,8 +719,17 @@ 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);
+
+#define FW_STS_FMT "%08X %08X"
+#define FW_STS_PRM(fw_status) \
+	(fw_status).count > 0 ? (fw_status).status[0] : 0xDEADBEEF, \
+	(fw_status).count > 1 ? (fw_status).status[1] : 0xDEADBEEF
+
 bool mei_hbuf_acquire(struct mei_device *dev);
 
+bool mei_write_is_idle(struct mei_device *dev);
+
 #if IS_ENABLED(CONFIG_DEBUG_FS)
 int mei_dbgfs_register(struct mei_device *dev, const char *name);
 void mei_dbgfs_deregister(struct mei_device *dev);
diff --git a/drivers/misc/mei/pci-me.c b/drivers/misc/mei/pci-me.c
index 95889e2e31f..1b46c64a649 100644
--- a/drivers/misc/mei/pci-me.c
+++ b/drivers/misc/mei/pci-me.c
@@ -33,6 +33,8 @@
 #include <linux/interrupt.h>
 #include <linux/miscdevice.h>
 
+#include <linux/pm_runtime.h>
+
 #include <linux/mei.h>
 
 #include "mei_dev.h"
@@ -42,42 +44,44 @@
 
 /* mei_pci_tbl - PCI Device ID Table */
 static const struct pci_device_id mei_me_pci_tbl[] = {
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_82946GZ)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_82G35)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_82Q965)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_82G965)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_82GM965)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_82GME965)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9_82Q35)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9_82G33)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9_82Q33)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9_82X38)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9_3200)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9_6)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9_7)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9_8)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9_9)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9_10)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9M_1)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9M_2)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9M_3)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9M_4)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH10_1)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH10_2)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH10_3)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH10_4)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_IBXPK_1)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_IBXPK_2)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_CPT_1)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_PBG_1)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_PPT_1)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_PPT_2)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_PPT_3)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_LPT_H)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_LPT_W)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_LPT_LP)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_LPT_HR)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_WPT_LP)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_82946GZ, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_82G35, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_82Q965, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_82G965, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_82GM965, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_82GME965, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9_82Q35, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9_82G33, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9_82Q33, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9_82X38, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9_3200, mei_me_legacy_cfg)},
+
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9_6, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9_7, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9_8, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9_9, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9_10, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9M_1, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9M_2, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9M_3, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9M_4, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH10_1, mei_me_ich_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH10_2, mei_me_ich_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH10_3, mei_me_ich_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH10_4, mei_me_ich_cfg)},
+
+	{MEI_PCI_DEVICE(MEI_DEV_ID_IBXPK_1, mei_me_pch_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_IBXPK_2, mei_me_pch_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_CPT_1, mei_me_pch_cpt_pbg_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_PBG_1, mei_me_pch_cpt_pbg_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_PPT_1, mei_me_pch_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_PPT_2, mei_me_pch_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_PPT_3, mei_me_pch_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_LPT_H, mei_me_lpt_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_LPT_W, mei_me_lpt_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_LPT_LP, mei_me_pch_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_LPT_HR, mei_me_lpt_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_WPT_LP, mei_me_pch_cfg)},
 
 	/* required last entry */
 	{0, }
@@ -85,44 +89,33 @@ static const struct pci_device_id mei_me_pci_tbl[] = {
 
 MODULE_DEVICE_TABLE(pci, mei_me_pci_tbl);
 
+#ifdef CONFIG_PM_RUNTIME
+static inline void mei_me_set_pm_domain(struct mei_device *dev);
+static inline void mei_me_unset_pm_domain(struct mei_device *dev);
+#else
+static inline void mei_me_set_pm_domain(struct mei_device *dev) {}
+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
  *
  * @pdev: PCI device structure
- * @ent: entry into pci_device_table
+ * @cfg: per generation config
  *
  * returns true if ME Interface is valid, false otherwise
  */
 static bool mei_me_quirk_probe(struct pci_dev *pdev,
-				const struct pci_device_id *ent)
+				const struct mei_cfg *cfg)
 {
-	u32 reg;
-	/* Cougar Point || Patsburg */
-	if (ent->device == MEI_DEV_ID_CPT_1 ||
-	    ent->device == MEI_DEV_ID_PBG_1) {
-		pci_read_config_dword(pdev, PCI_CFG_HFS_2, &reg);
-		/* make sure that bit 9 (NM) is up and bit 10 (DM) is down */
-		if ((reg & 0x600) == 0x200)
-			goto no_mei;
-	}
-
-	/* Lynx Point */
-	if (ent->device == MEI_DEV_ID_LPT_H  ||
-	    ent->device == MEI_DEV_ID_LPT_W  ||
-	    ent->device == MEI_DEV_ID_LPT_HR) {
-		/* Read ME FW Status check for SPS Firmware */
-		pci_read_config_dword(pdev, PCI_CFG_HFS_1, &reg);
-		/* if bits [19:16] = 15, running SPS Firmware */
-		if ((reg & 0xf0000) == 0xf0000)
-			goto no_mei;
+	if (cfg->quirk_probe && cfg->quirk_probe(pdev)) {
+		dev_info(&pdev->dev, "Device doesn't have valid ME Interface\n");
+		return false;
 	}
 
 	return true;
-
-no_mei:
-	dev_info(&pdev->dev, "Device doesn't have valid ME Interface\n");
-	return false;
 }
+
 /**
  * mei_probe - Device Initialization Routine
  *
@@ -133,15 +126,14 @@ no_mei:
  */
 static int mei_me_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_me_hw *hw;
 	int err;
 
 
-	if (!mei_me_quirk_probe(pdev, ent)) {
-		err = -ENODEV;
-		goto end;
-	}
+	if (!mei_me_quirk_probe(pdev, cfg))
+		return -ENODEV;
 
 	/* enable pci dev */
 	err = pci_enable_device(pdev);
@@ -173,7 +165,7 @@ static int mei_me_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 
 	/* allocates and initializes the mei dev structure */
-	dev = mei_me_dev_init(pdev);
+	dev = mei_me_dev_init(pdev, cfg);
 	if (!dev) {
 		err = -ENOMEM;
 		goto release_regions;
@@ -212,6 +204,9 @@ static int mei_me_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto release_irq;
 	}
 
+	pm_runtime_set_autosuspend_delay(&pdev->dev, MEI_ME_RPM_TIMEOUT);
+	pm_runtime_use_autosuspend(&pdev->dev);
+
 	err = mei_register(dev);
 	if (err)
 		goto release_irq;
@@ -220,6 +215,17 @@ static int mei_me_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	schedule_delayed_work(&dev->timer_work, HZ);
 
+	/*
+	* For not wake-able HW runtime pm framework
+	* can't be used on pci device level.
+	* Use domain runtime pm callbacks instead.
+	*/
+	if (!pci_dev_run_wake(pdev))
+		mei_me_set_pm_domain(dev);
+
+	if (mei_pg_is_enabled(dev))
+		pm_runtime_put_noidle(&pdev->dev);
+
 	dev_dbg(&pdev->dev, "initialization successful.\n");
 
 	return 0;
@@ -259,12 +265,18 @@ static void mei_me_remove(struct pci_dev *pdev)
 	if (!dev)
 		return;
 
+	if (mei_pg_is_enabled(dev))
+		pm_runtime_get_noresume(&pdev->dev);
+
 	hw = to_me_hw(dev);
 
 
 	dev_dbg(&pdev->dev, "stop\n");
 	mei_stop(dev);
 
+	if (!pci_dev_run_wake(pdev))
+		mei_me_unset_pm_domain(dev);
+
 	/* disable interrupts */
 	mei_disable_interrupts(dev);
 
@@ -343,12 +355,120 @@ static int mei_me_pci_resume(struct device *device)
 
 	return 0;
 }
+#endif /* CONFIG_PM_SLEEP */
+
+#ifdef CONFIG_PM_RUNTIME
+static int mei_me_pm_runtime_idle(struct device *device)
+{
+	struct pci_dev *pdev = to_pci_dev(device);
+	struct mei_device *dev;
+
+	dev_dbg(&pdev->dev, "rpm: me: runtime_idle\n");
+
+	dev = pci_get_drvdata(pdev);
+	if (!dev)
+		return -ENODEV;
+	if (mei_write_is_idle(dev))
+		pm_schedule_suspend(device, MEI_ME_RPM_TIMEOUT * 2);
+
+	return -EBUSY;
+}
+
+static int mei_me_pm_runtime_suspend(struct device *device)
+{
+	struct pci_dev *pdev = to_pci_dev(device);
+	struct mei_device *dev;
+	int ret;
+
+	dev_dbg(&pdev->dev, "rpm: me: runtime suspend\n");
+
+	dev = pci_get_drvdata(pdev);
+	if (!dev)
+		return -ENODEV;
+
+	mutex_lock(&dev->device_lock);
+
+	if (mei_write_is_idle(dev))
+		ret = mei_me_pg_set_sync(dev);
+	else
+		ret = -EAGAIN;
+
+	mutex_unlock(&dev->device_lock);
+
+	dev_dbg(&pdev->dev, "rpm: me: runtime suspend ret=%d\n", ret);
+
+	return ret;
+}
+
+static int mei_me_pm_runtime_resume(struct device *device)
+{
+	struct pci_dev *pdev = to_pci_dev(device);
+	struct mei_device *dev;
+	int ret;
+
+	dev_dbg(&pdev->dev, "rpm: me: runtime resume\n");
+
+	dev = pci_get_drvdata(pdev);
+	if (!dev)
+		return -ENODEV;
+
+	mutex_lock(&dev->device_lock);
+
+	ret = mei_me_pg_unset_sync(dev);
+
+	mutex_unlock(&dev->device_lock);
+
+	dev_dbg(&pdev->dev, "rpm: me: runtime resume ret = %d\n", ret);
+
+	return ret;
+}
+
+/**
+ * mei_me_set_pm_domain - fill and set pm domian stucture for device
+ *
+ * @dev: mei_device
+ */
+static inline void mei_me_set_pm_domain(struct mei_device *dev)
+{
+	struct pci_dev *pdev  = dev->pdev;
+
+	if (pdev->dev.bus && pdev->dev.bus->pm) {
+		dev->pg_domain.ops = *pdev->dev.bus->pm;
+
+		dev->pg_domain.ops.runtime_suspend = mei_me_pm_runtime_suspend;
+		dev->pg_domain.ops.runtime_resume = mei_me_pm_runtime_resume;
+		dev->pg_domain.ops.runtime_idle = mei_me_pm_runtime_idle;
+
+		pdev->dev.pm_domain = &dev->pg_domain;
+	}
+}
+
+/**
+ * mei_me_unset_pm_domain - clean pm domian stucture 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;
+}
+#endif /* CONFIG_PM_RUNTIME */
+
+#ifdef CONFIG_PM
+static const struct dev_pm_ops mei_me_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(mei_me_pci_suspend,
+				mei_me_pci_resume)
+	SET_RUNTIME_PM_OPS(
+		mei_me_pm_runtime_suspend,
+		mei_me_pm_runtime_resume,
+		mei_me_pm_runtime_idle)
+};
 
-static SIMPLE_DEV_PM_OPS(mei_me_pm_ops, mei_me_pci_suspend, mei_me_pci_resume);
 #define MEI_ME_PM_OPS	(&mei_me_pm_ops)
 #else
 #define MEI_ME_PM_OPS	NULL
-#endif /* CONFIG_PM_SLEEP */
+#endif /* CONFIG_PM */
 /*
  *  PCI driver structure
  */
diff --git a/drivers/misc/mei/pci-txe.c b/drivers/misc/mei/pci-txe.c
index ad3adb009a1..2343c6236df 100644
--- a/drivers/misc/mei/pci-txe.c
+++ b/drivers/misc/mei/pci-txe.c
@@ -27,6 +27,7 @@
 #include <linux/jiffies.h>
 #include <linux/interrupt.h>
 #include <linux/workqueue.h>
+#include <linux/pm_runtime.h>
 
 #include <linux/mei.h>
 
@@ -35,11 +36,18 @@
 #include "hw-txe.h"
 
 static const struct pci_device_id mei_txe_pci_tbl[] = {
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x0F18)}, /* Baytrail */
+	{MEI_PCI_DEVICE(0x0F18, mei_txe_cfg)}, /* Baytrail */
 	{0, }
 };
 MODULE_DEVICE_TABLE(pci, mei_txe_pci_tbl);
 
+#ifdef CONFIG_PM_RUNTIME
+static inline void mei_txe_set_pm_domain(struct mei_device *dev);
+static inline void mei_txe_unset_pm_domain(struct mei_device *dev);
+#else
+static inline void mei_txe_set_pm_domain(struct mei_device *dev) {}
+static inline void mei_txe_unset_pm_domain(struct mei_device *dev) {}
+#endif /* CONFIG_PM_RUNTIME */
 
 static void mei_txe_pci_iounmap(struct pci_dev *pdev, struct mei_txe_hw *hw)
 {
@@ -61,6 +69,7 @@ static void mei_txe_pci_iounmap(struct pci_dev *pdev, struct mei_txe_hw *hw)
  */
 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;
@@ -91,7 +100,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);
+	dev = mei_txe_dev_init(pdev, cfg);
 	if (!dev) {
 		err = -ENOMEM;
 		goto release_regions;
@@ -137,12 +146,25 @@ static int mei_txe_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto release_irq;
 	}
 
+	pm_runtime_set_autosuspend_delay(&pdev->dev, MEI_TXI_RPM_TIMEOUT);
+	pm_runtime_use_autosuspend(&pdev->dev);
+
 	err = mei_register(dev);
 	if (err)
 		goto release_irq;
 
 	pci_set_drvdata(pdev, dev);
 
+	/*
+	* For not wake-able HW runtime pm framework
+	* can't be used on pci device level.
+	* Use domain runtime pm callbacks instead.
+	*/
+	if (!pci_dev_run_wake(pdev))
+		mei_txe_set_pm_domain(dev);
+
+	pm_runtime_put_noidle(&pdev->dev);
+
 	return 0;
 
 release_irq:
@@ -187,10 +209,15 @@ static void mei_txe_remove(struct pci_dev *pdev)
 		return;
 	}
 
+	pm_runtime_get_noresume(&pdev->dev);
+
 	hw = to_txe_hw(dev);
 
 	mei_stop(dev);
 
+	if (!pci_dev_run_wake(pdev))
+		mei_txe_unset_pm_domain(dev);
+
 	/* disable interrupts */
 	mei_disable_interrupts(dev);
 	free_irq(pdev->irq, dev);
@@ -265,15 +292,131 @@ static int mei_txe_pci_resume(struct device *device)
 
 	return err;
 }
+#endif /* CONFIG_PM_SLEEP */
+
+#ifdef CONFIG_PM_RUNTIME
+static int mei_txe_pm_runtime_idle(struct device *device)
+{
+	struct pci_dev *pdev = to_pci_dev(device);
+	struct mei_device *dev;
+
+	dev_dbg(&pdev->dev, "rpm: txe: runtime_idle\n");
+
+	dev = pci_get_drvdata(pdev);
+	if (!dev)
+		return -ENODEV;
+	if (mei_write_is_idle(dev))
+		pm_schedule_suspend(device, MEI_TXI_RPM_TIMEOUT * 2);
+
+	return -EBUSY;
+}
+static int mei_txe_pm_runtime_suspend(struct device *device)
+{
+	struct pci_dev *pdev = to_pci_dev(device);
+	struct mei_device *dev;
+	int ret;
+
+	dev_dbg(&pdev->dev, "rpm: txe: runtime suspend\n");
+
+	dev = pci_get_drvdata(pdev);
+	if (!dev)
+		return -ENODEV;
+
+	mutex_lock(&dev->device_lock);
+
+	if (mei_write_is_idle(dev))
+		ret = mei_txe_aliveness_set_sync(dev, 0);
+	else
+		ret = -EAGAIN;
+
+	/*
+	 * If everything is okay we're about to enter PCI low
+	 * power state (D3) therefor we need to disable the
+	 * interrupts towards host.
+	 * However if device is not wakeable we do not enter
+	 * D-low state and we need to keep the interrupt kicking
+	 */
+	 if (!ret && pci_dev_run_wake(pdev))
+		mei_disable_interrupts(dev);
+
+	dev_dbg(&pdev->dev, "rpm: txe: runtime suspend ret=%d\n", ret);
+
+	mutex_unlock(&dev->device_lock);
+	return ret;
+}
+
+static int mei_txe_pm_runtime_resume(struct device *device)
+{
+	struct pci_dev *pdev = to_pci_dev(device);
+	struct mei_device *dev;
+	int ret;
+
+	dev_dbg(&pdev->dev, "rpm: txe: runtime resume\n");
+
+	dev = pci_get_drvdata(pdev);
+	if (!dev)
+		return -ENODEV;
+
+	mutex_lock(&dev->device_lock);
+
+	mei_enable_interrupts(dev);
+
+	ret = mei_txe_aliveness_set_sync(dev, 1);
+
+	mutex_unlock(&dev->device_lock);
+
+	dev_dbg(&pdev->dev, "rpm: txe: runtime resume ret = %d\n", ret);
+
+	return ret;
+}
+
+/**
+ * mei_txe_set_pm_domain - fill and set pm domian stucture for device
+ *
+ * @dev: mei_device
+ */
+static inline void mei_txe_set_pm_domain(struct mei_device *dev)
+{
+	struct pci_dev *pdev  = dev->pdev;
+
+	if (pdev->dev.bus && pdev->dev.bus->pm) {
+		dev->pg_domain.ops = *pdev->dev.bus->pm;
+
+		dev->pg_domain.ops.runtime_suspend = mei_txe_pm_runtime_suspend;
+		dev->pg_domain.ops.runtime_resume = mei_txe_pm_runtime_resume;
+		dev->pg_domain.ops.runtime_idle = mei_txe_pm_runtime_idle;
+
+		pdev->dev.pm_domain = &dev->pg_domain;
+	}
+}
 
-static SIMPLE_DEV_PM_OPS(mei_txe_pm_ops,
-			 mei_txe_pci_suspend,
-			 mei_txe_pci_resume);
+/**
+ * mei_txe_unset_pm_domain - clean pm domian stucture 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;
+}
+#endif /* CONFIG_PM_RUNTIME */
+
+#ifdef CONFIG_PM
+static const struct dev_pm_ops mei_txe_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(mei_txe_pci_suspend,
+				mei_txe_pci_resume)
+	SET_RUNTIME_PM_OPS(
+		mei_txe_pm_runtime_suspend,
+		mei_txe_pm_runtime_resume,
+		mei_txe_pm_runtime_idle)
+};
 
 #define MEI_TXE_PM_OPS	(&mei_txe_pm_ops)
 #else
 #define MEI_TXE_PM_OPS	NULL
-#endif /* CONFIG_PM_SLEEP */
+#endif /* CONFIG_PM */
+
 /*
  *  PCI driver structure
  */
diff --git a/drivers/misc/mei/wd.c b/drivers/misc/mei/wd.c
index ebf1cbc198f..a84a664dfcc 100644
--- a/drivers/misc/mei/wd.c
+++ b/drivers/misc/mei/wd.c
@@ -84,8 +84,6 @@ int mei_wd_host_init(struct mei_device *dev)
 		return ret;
 	}
 
-	cl->state = MEI_FILE_CONNECTING;
-
 	ret = mei_cl_connect(cl, NULL);
 
 	if (ret) {
diff --git a/drivers/misc/sgi-gru/grufile.c b/drivers/misc/sgi-gru/grufile.c
index 0535d1e0bc7..104a05f6b73 100644
--- a/drivers/misc/sgi-gru/grufile.c
+++ b/drivers/misc/sgi-gru/grufile.c
@@ -6,7 +6,7 @@
  * This file supports the user system call for file open, close, mmap, etc.
  * This also incudes the driver initialization code.
  *
- *  Copyright (c) 2008 Silicon Graphics, Inc.  All Rights Reserved.
+ *  Copyright (c) 2008-2014 Silicon Graphics, Inc.  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
@@ -58,6 +58,11 @@ static int max_user_cbrs, max_user_dsr_bytes;
 
 static struct miscdevice gru_miscdev;
 
+static int gru_supported(void)
+{
+	return is_uv_system() &&
+		(uv_hub_info->hub_revision < UV3_HUB_REVISION_BASE);
+}
 
 /*
  * gru_vma_close
@@ -518,7 +523,7 @@ static int __init gru_init(void)
 {
 	int ret;
 
-	if (!is_uv_system() || (is_uvx_hub() && !is_uv2_hub()))
+	if (!gru_supported())
 		return 0;
 
 #if defined CONFIG_IA64
@@ -573,7 +578,7 @@ exit0:
 
 static void __exit gru_exit(void)
 {
-	if (!is_uv_system())
+	if (!gru_supported())
 		return;
 
 	gru_teardown_tlb_irqs();
diff --git a/drivers/misc/vexpress-syscfg.c b/drivers/misc/vexpress-syscfg.c
new file mode 100644
index 00000000000..73068e50e56
--- /dev/null
+++ b/drivers/misc/vexpress-syscfg.c
@@ -0,0 +1,324 @@
+/*
+ * 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.
+ *
+ * Copyright (C) 2014 ARM Limited
+ */
+
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/syscore_ops.h>
+#include <linux/vexpress.h>
+
+
+#define SYS_CFGDATA		0x0
+
+#define SYS_CFGCTRL		0x4
+#define SYS_CFGCTRL_START	(1 << 31)
+#define SYS_CFGCTRL_WRITE	(1 << 30)
+#define SYS_CFGCTRL_DCC(n)	(((n) & 0xf) << 26)
+#define SYS_CFGCTRL_FUNC(n)	(((n) & 0x3f) << 20)
+#define SYS_CFGCTRL_SITE(n)	(((n) & 0x3) << 16)
+#define SYS_CFGCTRL_POSITION(n)	(((n) & 0xf) << 12)
+#define SYS_CFGCTRL_DEVICE(n)	(((n) & 0xfff) << 0)
+
+#define SYS_CFGSTAT		0x8
+#define SYS_CFGSTAT_ERR		(1 << 1)
+#define SYS_CFGSTAT_COMPLETE	(1 << 0)
+
+
+struct vexpress_syscfg {
+	struct device *dev;
+	void __iomem *base;
+	struct list_head funcs;
+};
+
+struct vexpress_syscfg_func {
+	struct list_head list;
+	struct vexpress_syscfg *syscfg;
+	struct regmap *regmap;
+	int num_templates;
+	u32 template[0]; /* Keep it last! */
+};
+
+
+static int vexpress_syscfg_exec(struct vexpress_syscfg_func *func,
+		int index, bool write, u32 *data)
+{
+	struct vexpress_syscfg *syscfg = func->syscfg;
+	u32 command, status;
+	int tries;
+	long timeout;
+
+	if (WARN_ON(index > func->num_templates))
+		return -EINVAL;
+
+	command = readl(syscfg->base + SYS_CFGCTRL);
+	if (WARN_ON(command & SYS_CFGCTRL_START))
+		return -EBUSY;
+
+	command = func->template[index];
+	command |= SYS_CFGCTRL_START;
+	command |= write ? SYS_CFGCTRL_WRITE : 0;
+
+	/* Use a canary for reads */
+	if (!write)
+		*data = 0xdeadbeef;
+
+	dev_dbg(syscfg->dev, "func %p, command %x, data %x\n",
+			func, command, *data);
+	writel(*data, syscfg->base + SYS_CFGDATA);
+	writel(0, syscfg->base + SYS_CFGSTAT);
+	writel(command, syscfg->base + SYS_CFGCTRL);
+	mb();
+
+	/* The operation can take ages... Go to sleep, 100us initially */
+	tries = 100;
+	timeout = 100;
+	do {
+		if (!irqs_disabled()) {
+			set_current_state(TASK_INTERRUPTIBLE);
+			schedule_timeout(usecs_to_jiffies(timeout));
+			if (signal_pending(current))
+				return -EINTR;
+		} else {
+			udelay(timeout);
+		}
+
+		status = readl(syscfg->base + SYS_CFGSTAT);
+		if (status & SYS_CFGSTAT_ERR)
+			return -EFAULT;
+
+		if (timeout > 20)
+			timeout -= 20;
+	} while (--tries && !(status & SYS_CFGSTAT_COMPLETE));
+	if (WARN_ON_ONCE(!tries))
+		return -ETIMEDOUT;
+
+	if (!write) {
+		*data = readl(syscfg->base + SYS_CFGDATA);
+		dev_dbg(syscfg->dev, "func %p, read data %x\n", func, *data);
+	}
+
+	return 0;
+}
+
+static int vexpress_syscfg_read(void *context, unsigned int index,
+		unsigned int *val)
+{
+	struct vexpress_syscfg_func *func = context;
+
+	return vexpress_syscfg_exec(func, index, false, val);
+}
+
+static int vexpress_syscfg_write(void *context, unsigned int index,
+		unsigned int val)
+{
+	struct vexpress_syscfg_func *func = context;
+
+	return vexpress_syscfg_exec(func, index, true, &val);
+}
+
+struct regmap_config vexpress_syscfg_regmap_config = {
+	.lock = vexpress_config_lock,
+	.unlock = vexpress_config_unlock,
+	.reg_bits = 32,
+	.val_bits = 32,
+	.reg_read = vexpress_syscfg_read,
+	.reg_write = vexpress_syscfg_write,
+	.reg_format_endian = REGMAP_ENDIAN_LITTLE,
+	.val_format_endian = REGMAP_ENDIAN_LITTLE,
+};
+
+
+static struct regmap *vexpress_syscfg_regmap_init(struct device *dev,
+		void *context)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct vexpress_syscfg *syscfg = context;
+	struct vexpress_syscfg_func *func;
+	struct property *prop;
+	const __be32 *val = NULL;
+	__be32 energy_quirk[4];
+	int num;
+	u32 site, position, dcc;
+	int i;
+
+	if (dev->of_node) {
+		int err = vexpress_config_get_topo(dev->of_node, &site,
+				&position, &dcc);
+
+		if (err)
+			return ERR_PTR(err);
+
+		prop = of_find_property(dev->of_node,
+				"arm,vexpress-sysreg,func", NULL);
+		if (!prop)
+			return ERR_PTR(-EINVAL);
+
+		num = prop->length / sizeof(u32) / 2;
+		val = prop->value;
+	} else {
+		if (pdev->num_resources != 1 ||
+				pdev->resource[0].flags != IORESOURCE_BUS)
+			return ERR_PTR(-EFAULT);
+
+		site = pdev->resource[0].start;
+		if (site == VEXPRESS_SITE_MASTER)
+			site = vexpress_config_get_master();
+		position = 0;
+		dcc = 0;
+		num = 1;
+	}
+
+	/*
+	 * "arm,vexpress-energy" function used to be described
+	 * by its first device only, now it requires both
+	 */
+	if (num == 1 && of_device_is_compatible(dev->of_node,
+			"arm,vexpress-energy")) {
+		num = 2;
+		energy_quirk[0] = *val;
+		energy_quirk[2] = *val++;
+		energy_quirk[1] = *val;
+		energy_quirk[3] = cpu_to_be32(be32_to_cpup(val) + 1);
+		val = energy_quirk;
+	}
+
+	func = kzalloc(sizeof(*func) + sizeof(*func->template) * num,
+			GFP_KERNEL);
+	if (!func)
+		return NULL;
+
+	func->syscfg = syscfg;
+	func->num_templates = num;
+
+	for (i = 0; i < num; i++) {
+		u32 function, device;
+
+		if (dev->of_node) {
+			function = be32_to_cpup(val++);
+			device = be32_to_cpup(val++);
+		} else {
+			function = pdev->resource[0].end;
+			device = pdev->id;
+		}
+
+		dev_dbg(dev, "func %p: %u/%u/%u/%u/%u\n",
+				func, site, position, dcc,
+				function, device);
+
+		func->template[i] = SYS_CFGCTRL_DCC(dcc);
+		func->template[i] |= SYS_CFGCTRL_SITE(site);
+		func->template[i] |= SYS_CFGCTRL_POSITION(position);
+		func->template[i] |= SYS_CFGCTRL_FUNC(function);
+		func->template[i] |= SYS_CFGCTRL_DEVICE(device);
+	}
+
+	vexpress_syscfg_regmap_config.max_register = num - 1;
+
+	func->regmap = regmap_init(dev, NULL, func,
+			&vexpress_syscfg_regmap_config);
+
+	if (IS_ERR(func->regmap))
+		kfree(func);
+	else
+		list_add(&func->list, &syscfg->funcs);
+
+	return func->regmap;
+}
+
+static void vexpress_syscfg_regmap_exit(struct regmap *regmap, void *context)
+{
+	struct vexpress_syscfg *syscfg = context;
+	struct vexpress_syscfg_func *func, *tmp;
+
+	regmap_exit(regmap);
+
+	list_for_each_entry_safe(func, tmp, &syscfg->funcs, list) {
+		if (func->regmap == regmap) {
+			list_del(&syscfg->funcs);
+			kfree(func);
+			break;
+		}
+	}
+}
+
+static struct vexpress_config_bridge_ops vexpress_syscfg_bridge_ops = {
+	.regmap_init = vexpress_syscfg_regmap_init,
+	.regmap_exit = vexpress_syscfg_regmap_exit,
+};
+
+
+/* Non-DT hack, to be gone... */
+static struct device *vexpress_syscfg_bridge;
+
+int vexpress_syscfg_device_register(struct platform_device *pdev)
+{
+	pdev->dev.parent = vexpress_syscfg_bridge;
+
+	return platform_device_register(pdev);
+}
+
+
+int vexpress_syscfg_probe(struct platform_device *pdev)
+{
+	struct vexpress_syscfg *syscfg;
+	struct resource *res;
+	struct device *bridge;
+
+	syscfg = devm_kzalloc(&pdev->dev, sizeof(*syscfg), GFP_KERNEL);
+	if (!syscfg)
+		return -ENOMEM;
+	syscfg->dev = &pdev->dev;
+	INIT_LIST_HEAD(&syscfg->funcs);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!devm_request_mem_region(&pdev->dev, res->start,
+			resource_size(res), pdev->name))
+		return -EBUSY;
+
+	syscfg->base = devm_ioremap(&pdev->dev, res->start, resource_size(res));
+	if (!syscfg->base)
+		return -EFAULT;
+
+	/* Must use dev.parent (MFD), as that's where DT phandle points at... */
+	bridge = vexpress_config_bridge_register(pdev->dev.parent,
+			&vexpress_syscfg_bridge_ops, syscfg);
+	if (IS_ERR(bridge))
+		return PTR_ERR(bridge);
+
+	/* Non-DT case */
+	if (!pdev->dev.of_node)
+		vexpress_syscfg_bridge = bridge;
+
+	return 0;
+}
+
+static const struct platform_device_id vexpress_syscfg_id_table[] = {
+	{ "vexpress-syscfg", },
+	{},
+};
+
+static struct platform_driver vexpress_syscfg_driver = {
+	.driver.name = "vexpress-syscfg",
+	.id_table = vexpress_syscfg_id_table,
+	.probe = vexpress_syscfg_probe,
+};
+
+static int __init vexpress_syscfg_init(void)
+{
+	return platform_driver_register(&vexpress_syscfg_driver);
+}
+core_initcall(vexpress_syscfg_init);
diff --git a/drivers/mmc/core/bus.c b/drivers/mmc/core/bus.c
index 824644875d4..d2dbf02022b 100644
--- a/drivers/mmc/core/bus.c
+++ b/drivers/mmc/core/bus.c
@@ -341,16 +341,17 @@ int mmc_add_card(struct mmc_card *card)
 	if (mmc_host_is_spi(card->host)) {
 		pr_info("%s: new %s%s%s card on SPI\n",
 			mmc_hostname(card->host),
-			mmc_card_highspeed(card) ? "high speed " : "",
-			mmc_card_ddr_mode(card) ? "DDR " : "",
+			mmc_card_hs(card) ? "high speed " : "",
+			mmc_card_ddr52(card) ? "DDR " : "",
 			type);
 	} else {
 		pr_info("%s: new %s%s%s%s%s card at address %04x\n",
 			mmc_hostname(card->host),
 			mmc_card_uhs(card) ? "ultra high speed " :
-			(mmc_card_highspeed(card) ? "high speed " : ""),
+			(mmc_card_hs(card) ? "high speed " : ""),
+			mmc_card_hs400(card) ? "HS400 " :
 			(mmc_card_hs200(card) ? "HS200 " : ""),
-			mmc_card_ddr_mode(card) ? "DDR " : "",
+			mmc_card_ddr52(card) ? "DDR " : "",
 			uhs_bus_speed_mode, type, card->rca);
 	}
 
diff --git a/drivers/mmc/core/core.c b/drivers/mmc/core/core.c
index acbc3f2aaaf..7dc0c85fdb6 100644
--- a/drivers/mmc/core/core.c
+++ b/drivers/mmc/core/core.c
@@ -800,6 +800,10 @@ void mmc_set_data_timeout(struct mmc_data *data, const struct mmc_card *card)
 			data->timeout_ns = limit_us * 1000;
 			data->timeout_clks = 0;
 		}
+
+		/* assign limit value if invalid */
+		if (timeout_us == 0)
+			data->timeout_ns = limit_us * 1000;
 	}
 
 	/*
@@ -1310,31 +1314,38 @@ int mmc_regulator_set_ocr(struct mmc_host *mmc,
 }
 EXPORT_SYMBOL_GPL(mmc_regulator_set_ocr);
 
+#endif /* CONFIG_REGULATOR */
+
 int mmc_regulator_get_supply(struct mmc_host *mmc)
 {
 	struct device *dev = mmc_dev(mmc);
-	struct regulator *supply;
 	int ret;
 
-	supply = devm_regulator_get(dev, "vmmc");
-	mmc->supply.vmmc = supply;
+	mmc->supply.vmmc = devm_regulator_get_optional(dev, "vmmc");
 	mmc->supply.vqmmc = devm_regulator_get_optional(dev, "vqmmc");
 
-	if (IS_ERR(supply))
-		return PTR_ERR(supply);
+	if (IS_ERR(mmc->supply.vmmc)) {
+		if (PTR_ERR(mmc->supply.vmmc) == -EPROBE_DEFER)
+			return -EPROBE_DEFER;
+		dev_info(dev, "No vmmc regulator found\n");
+	} else {
+		ret = mmc_regulator_get_ocrmask(mmc->supply.vmmc);
+		if (ret > 0)
+			mmc->ocr_avail = ret;
+		else
+			dev_warn(dev, "Failed getting OCR mask: %d\n", ret);
+	}
 
-	ret = mmc_regulator_get_ocrmask(supply);
-	if (ret > 0)
-		mmc->ocr_avail = ret;
-	else
-		dev_warn(mmc_dev(mmc), "Failed getting OCR mask: %d\n", ret);
+	if (IS_ERR(mmc->supply.vqmmc)) {
+		if (PTR_ERR(mmc->supply.vqmmc) == -EPROBE_DEFER)
+			return -EPROBE_DEFER;
+		dev_info(dev, "No vqmmc regulator found\n");
+	}
 
 	return 0;
 }
 EXPORT_SYMBOL_GPL(mmc_regulator_get_supply);
 
-#endif /* CONFIG_REGULATOR */
-
 /*
  * Mask off any voltages we don't support and select
  * the lowest voltage
@@ -1533,8 +1544,13 @@ void mmc_power_up(struct mmc_host *host, u32 ocr)
 	host->ios.timing = MMC_TIMING_LEGACY;
 	mmc_set_ios(host);
 
-	/* Set signal voltage to 3.3V */
-	__mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_330);
+	/* Try to set signal voltage to 3.3V but fall back to 1.8v or 1.2v */
+	if (__mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_330) == 0)
+		dev_dbg(mmc_dev(host), "Initial signal voltage of 3.3v\n");
+	else if (__mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_180) == 0)
+		dev_dbg(mmc_dev(host), "Initial signal voltage of 1.8v\n");
+	else if (__mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_120) == 0)
+		dev_dbg(mmc_dev(host), "Initial signal voltage of 1.2v\n");
 
 	/*
 	 * This delay should be sufficient to allow the power supply
@@ -2183,7 +2199,7 @@ int mmc_set_blocklen(struct mmc_card *card, unsigned int blocklen)
 {
 	struct mmc_command cmd = {0};
 
-	if (mmc_card_blockaddr(card) || mmc_card_ddr_mode(card))
+	if (mmc_card_blockaddr(card) || mmc_card_ddr52(card))
 		return 0;
 
 	cmd.opcode = MMC_SET_BLOCKLEN;
@@ -2263,7 +2279,6 @@ static int mmc_do_hw_reset(struct mmc_host *host, int check)
 		}
 	}
 
-	host->card->state &= ~(MMC_STATE_HIGHSPEED | MMC_STATE_HIGHSPEED_DDR);
 	if (mmc_host_is_spi(host)) {
 		host->ios.chip_select = MMC_CS_HIGH;
 		host->ios.bus_mode = MMC_BUSMODE_PUSHPULL;
@@ -2403,6 +2418,11 @@ void mmc_rescan(struct work_struct *work)
 		container_of(work, struct mmc_host, detect.work);
 	int i;
 
+	if (host->trigger_card_event && host->ops->card_event) {
+		host->ops->card_event(host);
+		host->trigger_card_event = false;
+	}
+
 	if (host->rescan_disable)
 		return;
 
diff --git a/drivers/mmc/core/debugfs.c b/drivers/mmc/core/debugfs.c
index 54829c0ed00..91eb1622324 100644
--- a/drivers/mmc/core/debugfs.c
+++ b/drivers/mmc/core/debugfs.c
@@ -135,8 +135,14 @@ static int mmc_ios_show(struct seq_file *s, void *data)
 	case MMC_TIMING_UHS_DDR50:
 		str = "sd uhs DDR50";
 		break;
+	case MMC_TIMING_MMC_DDR52:
+		str = "mmc DDR52";
+		break;
 	case MMC_TIMING_MMC_HS200:
-		str = "mmc high-speed SDR200";
+		str = "mmc HS200";
+		break;
+	case MMC_TIMING_MMC_HS400:
+		str = "mmc HS400";
 		break;
 	default:
 		str = "invalid";
diff --git a/drivers/mmc/core/host.c b/drivers/mmc/core/host.c
index fdea825dbb2..95cceae9694 100644
--- a/drivers/mmc/core/host.c
+++ b/drivers/mmc/core/host.c
@@ -447,6 +447,10 @@ int mmc_of_parse(struct mmc_host *host)
 		host->caps2 |= MMC_CAP2_HS200_1_8V_SDR;
 	if (of_find_property(np, "mmc-hs200-1_2v", &len))
 		host->caps2 |= MMC_CAP2_HS200_1_2V_SDR;
+	if (of_find_property(np, "mmc-hs400-1_8v", &len))
+		host->caps2 |= MMC_CAP2_HS400_1_8V | MMC_CAP2_HS200_1_8V_SDR;
+	if (of_find_property(np, "mmc-hs400-1_2v", &len))
+		host->caps2 |= MMC_CAP2_HS400_1_2V | MMC_CAP2_HS200_1_2V_SDR;
 
 	return 0;
 
diff --git a/drivers/mmc/core/mmc.c b/drivers/mmc/core/mmc.c
index 1ab5f3a0af5..793c6f7ddb0 100644
--- a/drivers/mmc/core/mmc.c
+++ b/drivers/mmc/core/mmc.c
@@ -240,31 +240,62 @@ static int mmc_get_ext_csd(struct mmc_card *card, u8 **new_ext_csd)
 static void mmc_select_card_type(struct mmc_card *card)
 {
 	struct mmc_host *host = card->host;
-	u8 card_type = card->ext_csd.raw_card_type & EXT_CSD_CARD_TYPE_MASK;
+	u8 card_type = card->ext_csd.raw_card_type;
 	u32 caps = host->caps, caps2 = host->caps2;
-	unsigned int hs_max_dtr = 0;
+	unsigned int hs_max_dtr = 0, hs200_max_dtr = 0;
+	unsigned int avail_type = 0;
 
-	if (card_type & EXT_CSD_CARD_TYPE_26)
+	if (caps & MMC_CAP_MMC_HIGHSPEED &&
+	    card_type & EXT_CSD_CARD_TYPE_HS_26) {
 		hs_max_dtr = MMC_HIGH_26_MAX_DTR;
+		avail_type |= EXT_CSD_CARD_TYPE_HS_26;
+	}
 
 	if (caps & MMC_CAP_MMC_HIGHSPEED &&
-			card_type & EXT_CSD_CARD_TYPE_52)
+	    card_type & EXT_CSD_CARD_TYPE_HS_52) {
 		hs_max_dtr = MMC_HIGH_52_MAX_DTR;
+		avail_type |= EXT_CSD_CARD_TYPE_HS_52;
+	}
+
+	if (caps & MMC_CAP_1_8V_DDR &&
+	    card_type & EXT_CSD_CARD_TYPE_DDR_1_8V) {
+		hs_max_dtr = MMC_HIGH_DDR_MAX_DTR;
+		avail_type |= EXT_CSD_CARD_TYPE_DDR_1_8V;
+	}
 
-	if ((caps & MMC_CAP_1_8V_DDR &&
-			card_type & EXT_CSD_CARD_TYPE_DDR_1_8V) ||
-	    (caps & MMC_CAP_1_2V_DDR &&
-			card_type & EXT_CSD_CARD_TYPE_DDR_1_2V))
+	if (caps & MMC_CAP_1_2V_DDR &&
+	    card_type & EXT_CSD_CARD_TYPE_DDR_1_2V) {
 		hs_max_dtr = MMC_HIGH_DDR_MAX_DTR;
+		avail_type |= EXT_CSD_CARD_TYPE_DDR_1_2V;
+	}
+
+	if (caps2 & MMC_CAP2_HS200_1_8V_SDR &&
+	    card_type & EXT_CSD_CARD_TYPE_HS200_1_8V) {
+		hs200_max_dtr = MMC_HS200_MAX_DTR;
+		avail_type |= EXT_CSD_CARD_TYPE_HS200_1_8V;
+	}
+
+	if (caps2 & MMC_CAP2_HS200_1_2V_SDR &&
+	    card_type & EXT_CSD_CARD_TYPE_HS200_1_2V) {
+		hs200_max_dtr = MMC_HS200_MAX_DTR;
+		avail_type |= EXT_CSD_CARD_TYPE_HS200_1_2V;
+	}
+
+	if (caps2 & MMC_CAP2_HS400_1_8V &&
+	    card_type & EXT_CSD_CARD_TYPE_HS400_1_8V) {
+		hs200_max_dtr = MMC_HS200_MAX_DTR;
+		avail_type |= EXT_CSD_CARD_TYPE_HS400_1_8V;
+	}
 
-	if ((caps2 & MMC_CAP2_HS200_1_8V_SDR &&
-			card_type & EXT_CSD_CARD_TYPE_SDR_1_8V) ||
-	    (caps2 & MMC_CAP2_HS200_1_2V_SDR &&
-			card_type & EXT_CSD_CARD_TYPE_SDR_1_2V))
-		hs_max_dtr = MMC_HS200_MAX_DTR;
+	if (caps2 & MMC_CAP2_HS400_1_2V &&
+	    card_type & EXT_CSD_CARD_TYPE_HS400_1_2V) {
+		hs200_max_dtr = MMC_HS200_MAX_DTR;
+		avail_type |= EXT_CSD_CARD_TYPE_HS400_1_2V;
+	}
 
 	card->ext_csd.hs_max_dtr = hs_max_dtr;
-	card->ext_csd.card_type = card_type;
+	card->ext_csd.hs200_max_dtr = hs200_max_dtr;
+	card->mmc_avail_type = avail_type;
 }
 
 /*
@@ -480,6 +511,8 @@ static int mmc_read_ext_csd(struct mmc_card *card, u8 *ext_csd)
 			ext_csd[EXT_CSD_PWR_CL_DDR_52_195];
 		card->ext_csd.raw_pwr_cl_ddr_52_360 =
 			ext_csd[EXT_CSD_PWR_CL_DDR_52_360];
+		card->ext_csd.raw_pwr_cl_ddr_200_360 =
+			ext_csd[EXT_CSD_PWR_CL_DDR_200_360];
 	}
 
 	if (card->ext_csd.rev >= 5) {
@@ -646,7 +679,10 @@ static int mmc_compare_ext_csds(struct mmc_card *card, unsigned bus_width)
 		(card->ext_csd.raw_pwr_cl_ddr_52_195 ==
 			bw_ext_csd[EXT_CSD_PWR_CL_DDR_52_195]) &&
 		(card->ext_csd.raw_pwr_cl_ddr_52_360 ==
-			bw_ext_csd[EXT_CSD_PWR_CL_DDR_52_360]));
+			bw_ext_csd[EXT_CSD_PWR_CL_DDR_52_360]) &&
+		(card->ext_csd.raw_pwr_cl_ddr_200_360 ==
+			bw_ext_csd[EXT_CSD_PWR_CL_DDR_200_360]));
+
 	if (err)
 		err = -EINVAL;
 
@@ -694,18 +730,10 @@ static struct attribute *mmc_std_attrs[] = {
 	&dev_attr_rel_sectors.attr,
 	NULL,
 };
-
-static struct attribute_group mmc_std_attr_group = {
-	.attrs = mmc_std_attrs,
-};
-
-static const struct attribute_group *mmc_attr_groups[] = {
-	&mmc_std_attr_group,
-	NULL,
-};
+ATTRIBUTE_GROUPS(mmc_std);
 
 static struct device_type mmc_type = {
-	.groups = mmc_attr_groups,
+	.groups = mmc_std_groups,
 };
 
 /*
@@ -714,17 +742,13 @@ static struct device_type mmc_type = {
  * extended CSD register, select it by executing the
  * mmc_switch command.
  */
-static int mmc_select_powerclass(struct mmc_card *card,
-		unsigned int bus_width)
+static int __mmc_select_powerclass(struct mmc_card *card,
+				   unsigned int bus_width)
 {
-	int err = 0;
+	struct mmc_host *host = card->host;
+	struct mmc_ext_csd *ext_csd = &card->ext_csd;
 	unsigned int pwrclass_val = 0;
-	struct mmc_host *host;
-
-	BUG_ON(!card);
-
-	host = card->host;
-	BUG_ON(!host);
+	int err = 0;
 
 	/* Power class selection is supported for versions >= 4.0 */
 	if (card->csd.mmca_vsn < CSD_SPEC_VER_4)
@@ -736,14 +760,14 @@ static int mmc_select_powerclass(struct mmc_card *card,
 
 	switch (1 << host->ios.vdd) {
 	case MMC_VDD_165_195:
-		if (host->ios.clock <= 26000000)
-			pwrclass_val = card->ext_csd.raw_pwr_cl_26_195;
-		else if	(host->ios.clock <= 52000000)
+		if (host->ios.clock <= MMC_HIGH_26_MAX_DTR)
+			pwrclass_val = ext_csd->raw_pwr_cl_26_195;
+		else if (host->ios.clock <= MMC_HIGH_52_MAX_DTR)
 			pwrclass_val = (bus_width <= EXT_CSD_BUS_WIDTH_8) ?
-				card->ext_csd.raw_pwr_cl_52_195 :
-				card->ext_csd.raw_pwr_cl_ddr_52_195;
-		else if (host->ios.clock <= 200000000)
-			pwrclass_val = card->ext_csd.raw_pwr_cl_200_195;
+				ext_csd->raw_pwr_cl_52_195 :
+				ext_csd->raw_pwr_cl_ddr_52_195;
+		else if (host->ios.clock <= MMC_HS200_MAX_DTR)
+			pwrclass_val = ext_csd->raw_pwr_cl_200_195;
 		break;
 	case MMC_VDD_27_28:
 	case MMC_VDD_28_29:
@@ -754,14 +778,16 @@ static int mmc_select_powerclass(struct mmc_card *card,
 	case MMC_VDD_33_34:
 	case MMC_VDD_34_35:
 	case MMC_VDD_35_36:
-		if (host->ios.clock <= 26000000)
-			pwrclass_val = card->ext_csd.raw_pwr_cl_26_360;
-		else if	(host->ios.clock <= 52000000)
+		if (host->ios.clock <= MMC_HIGH_26_MAX_DTR)
+			pwrclass_val = ext_csd->raw_pwr_cl_26_360;
+		else if (host->ios.clock <= MMC_HIGH_52_MAX_DTR)
 			pwrclass_val = (bus_width <= EXT_CSD_BUS_WIDTH_8) ?
-				card->ext_csd.raw_pwr_cl_52_360 :
-				card->ext_csd.raw_pwr_cl_ddr_52_360;
-		else if (host->ios.clock <= 200000000)
-			pwrclass_val = card->ext_csd.raw_pwr_cl_200_360;
+				ext_csd->raw_pwr_cl_52_360 :
+				ext_csd->raw_pwr_cl_ddr_52_360;
+		else if (host->ios.clock <= MMC_HS200_MAX_DTR)
+			pwrclass_val = (bus_width == EXT_CSD_DDR_BUS_WIDTH_8) ?
+				ext_csd->raw_pwr_cl_ddr_200_360 :
+				ext_csd->raw_pwr_cl_200_360;
 		break;
 	default:
 		pr_warning("%s: Voltage range not supported "
@@ -787,40 +813,79 @@ static int mmc_select_powerclass(struct mmc_card *card,
 	return err;
 }
 
+static int mmc_select_powerclass(struct mmc_card *card)
+{
+	struct mmc_host *host = card->host;
+	u32 bus_width, ext_csd_bits;
+	int err, ddr;
+
+	/* Power class selection is supported for versions >= 4.0 */
+	if (card->csd.mmca_vsn < CSD_SPEC_VER_4)
+		return 0;
+
+	bus_width = host->ios.bus_width;
+	/* Power class values are defined only for 4/8 bit bus */
+	if (bus_width == MMC_BUS_WIDTH_1)
+		return 0;
+
+	ddr = card->mmc_avail_type & EXT_CSD_CARD_TYPE_DDR_52;
+	if (ddr)
+		ext_csd_bits = (bus_width == MMC_BUS_WIDTH_8) ?
+			EXT_CSD_DDR_BUS_WIDTH_8 : EXT_CSD_DDR_BUS_WIDTH_4;
+	else
+		ext_csd_bits = (bus_width == MMC_BUS_WIDTH_8) ?
+			EXT_CSD_BUS_WIDTH_8 :  EXT_CSD_BUS_WIDTH_4;
+
+	err = __mmc_select_powerclass(card, ext_csd_bits);
+	if (err)
+		pr_warn("%s: power class selection to bus width %d ddr %d failed\n",
+			mmc_hostname(host), 1 << bus_width, ddr);
+
+	return err;
+}
+
 /*
- * Selects the desired buswidth and switch to the HS200 mode
- * if bus width set without error
+ * Set the bus speed for the selected speed mode.
  */
-static int mmc_select_hs200(struct mmc_card *card)
+static void mmc_set_bus_speed(struct mmc_card *card)
+{
+	unsigned int max_dtr = (unsigned int)-1;
+
+	if ((mmc_card_hs200(card) || mmc_card_hs400(card)) &&
+	     max_dtr > card->ext_csd.hs200_max_dtr)
+		max_dtr = card->ext_csd.hs200_max_dtr;
+	else if (mmc_card_hs(card) && max_dtr > card->ext_csd.hs_max_dtr)
+		max_dtr = card->ext_csd.hs_max_dtr;
+	else if (max_dtr > card->csd.max_dtr)
+		max_dtr = card->csd.max_dtr;
+
+	mmc_set_clock(card->host, max_dtr);
+}
+
+/*
+ * Select the bus width amoung 4-bit and 8-bit(SDR).
+ * If the bus width is changed successfully, return the selected width value.
+ * Zero is returned instead of error value if the wide width is not supported.
+ */
+static int mmc_select_bus_width(struct mmc_card *card)
 {
-	int idx, err = -EINVAL;
-	struct mmc_host *host;
 	static unsigned ext_csd_bits[] = {
-		EXT_CSD_BUS_WIDTH_4,
 		EXT_CSD_BUS_WIDTH_8,
+		EXT_CSD_BUS_WIDTH_4,
 	};
 	static unsigned bus_widths[] = {
-		MMC_BUS_WIDTH_4,
 		MMC_BUS_WIDTH_8,
+		MMC_BUS_WIDTH_4,
 	};
+	struct mmc_host *host = card->host;
+	unsigned idx, bus_width = 0;
+	int err = 0;
 
-	BUG_ON(!card);
-
-	host = card->host;
-
-	if (card->ext_csd.card_type & EXT_CSD_CARD_TYPE_SDR_1_2V &&
-			host->caps2 & MMC_CAP2_HS200_1_2V_SDR)
-		err = __mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_120);
-
-	if (err && card->ext_csd.card_type & EXT_CSD_CARD_TYPE_SDR_1_8V &&
-			host->caps2 & MMC_CAP2_HS200_1_8V_SDR)
-		err = __mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_180);
-
-	/* If fails try again during next card power cycle */
-	if (err)
-		goto err;
+	if ((card->csd.mmca_vsn < CSD_SPEC_VER_4) &&
+	    !(host->caps & (MMC_CAP_4_BIT_DATA | MMC_CAP_8_BIT_DATA)))
+		return 0;
 
-	idx = (host->caps & MMC_CAP_8_BIT_DATA) ? 1 : 0;
+	idx = (host->caps & MMC_CAP_8_BIT_DATA) ? 0 : 1;
 
 	/*
 	 * Unlike SD, MMC cards dont have a configuration register to notify
@@ -828,8 +893,7 @@ static int mmc_select_hs200(struct mmc_card *card)
 	 * the supported bus width or compare the ext csd values of current
 	 * bus width and ext csd values of 1 bit mode read earlier.
 	 */
-	for (; idx >= 0; idx--) {
-
+	for (; idx < ARRAY_SIZE(bus_widths); idx++) {
 		/*
 		 * Host is capable of 8bit transfer, then switch
 		 * the device to work in 8bit transfer mode. If the
@@ -844,27 +908,266 @@ static int mmc_select_hs200(struct mmc_card *card)
 		if (err)
 			continue;
 
-		mmc_set_bus_width(card->host, bus_widths[idx]);
+		bus_width = bus_widths[idx];
+		mmc_set_bus_width(host, bus_width);
 
+		/*
+		 * If controller can't handle bus width test,
+		 * compare ext_csd previously read in 1 bit mode
+		 * against ext_csd at new bus width
+		 */
 		if (!(host->caps & MMC_CAP_BUS_WIDTH_TEST))
-			err = mmc_compare_ext_csds(card, bus_widths[idx]);
+			err = mmc_compare_ext_csds(card, bus_width);
 		else
-			err = mmc_bus_test(card, bus_widths[idx]);
-		if (!err)
+			err = mmc_bus_test(card, bus_width);
+
+		if (!err) {
+			err = bus_width;
 			break;
+		} else {
+			pr_warn("%s: switch to bus width %d failed\n",
+				mmc_hostname(host), ext_csd_bits[idx]);
+		}
 	}
 
-	/* switch to HS200 mode if bus width set successfully */
+	return err;
+}
+
+/*
+ * Switch to the high-speed mode
+ */
+static int mmc_select_hs(struct mmc_card *card)
+{
+	int err;
+
+	err = __mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
+			   EXT_CSD_HS_TIMING, EXT_CSD_TIMING_HS,
+			   card->ext_csd.generic_cmd6_time,
+			   true, true, true);
 	if (!err)
+		mmc_set_timing(card->host, MMC_TIMING_MMC_HS);
+
+	return err;
+}
+
+/*
+ * Activate wide bus and DDR if supported.
+ */
+static int mmc_select_hs_ddr(struct mmc_card *card)
+{
+	struct mmc_host *host = card->host;
+	u32 bus_width, ext_csd_bits;
+	int err = 0;
+
+	if (!(card->mmc_avail_type & EXT_CSD_CARD_TYPE_DDR_52))
+		return 0;
+
+	bus_width = host->ios.bus_width;
+	if (bus_width == MMC_BUS_WIDTH_1)
+		return 0;
+
+	ext_csd_bits = (bus_width == MMC_BUS_WIDTH_8) ?
+		EXT_CSD_DDR_BUS_WIDTH_8 : EXT_CSD_DDR_BUS_WIDTH_4;
+
+	err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
+			EXT_CSD_BUS_WIDTH,
+			ext_csd_bits,
+			card->ext_csd.generic_cmd6_time);
+	if (err) {
+		pr_warn("%s: switch to bus width %d ddr failed\n",
+			mmc_hostname(host), 1 << bus_width);
+		return err;
+	}
+
+	/*
+	 * eMMC cards can support 3.3V to 1.2V i/o (vccq)
+	 * signaling.
+	 *
+	 * EXT_CSD_CARD_TYPE_DDR_1_8V means 3.3V or 1.8V vccq.
+	 *
+	 * 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.
+	 *
+	 * 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;
+	}
+
+	mmc_set_timing(host, MMC_TIMING_MMC_DDR52);
+
+	return err;
+}
+
+static int mmc_select_hs400(struct mmc_card *card)
+{
+	struct mmc_host *host = card->host;
+	int err = 0;
+
+	/*
+	 * HS400 mode requires 8-bit bus width
+	 */
+	if (!(card->mmc_avail_type & EXT_CSD_CARD_TYPE_HS400 &&
+	      host->ios.bus_width == MMC_BUS_WIDTH_8))
+		return 0;
+
+	/*
+	 * Before switching to dual data rate operation for HS400,
+	 * it is required to convert from HS200 mode to HS mode.
+	 */
+	mmc_set_timing(card->host, MMC_TIMING_MMC_HS);
+	mmc_set_bus_speed(card);
+
+	err = __mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
+			   EXT_CSD_HS_TIMING, EXT_CSD_TIMING_HS,
+			   card->ext_csd.generic_cmd6_time,
+			   true, true, true);
+	if (err) {
+		pr_warn("%s: switch to high-speed from hs200 failed, err:%d\n",
+			mmc_hostname(host), err);
+		return err;
+	}
+
+	err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
+			 EXT_CSD_BUS_WIDTH,
+			 EXT_CSD_DDR_BUS_WIDTH_8,
+			 card->ext_csd.generic_cmd6_time);
+	if (err) {
+		pr_warn("%s: switch to bus width for hs400 failed, err:%d\n",
+			mmc_hostname(host), err);
+		return err;
+	}
+
+	err = __mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
+			   EXT_CSD_HS_TIMING, EXT_CSD_TIMING_HS400,
+			   card->ext_csd.generic_cmd6_time,
+			   true, true, true);
+	if (err) {
+		pr_warn("%s: switch to hs400 failed, err:%d\n",
+			 mmc_hostname(host), err);
+		return err;
+	}
+
+	mmc_set_timing(host, MMC_TIMING_MMC_HS400);
+	mmc_set_bus_speed(card);
+
+	return 0;
+}
+
+/*
+ * For device supporting HS200 mode, the following sequence
+ * should be done before executing the tuning process.
+ * 1. set the desired bus width(4-bit or 8-bit, 1-bit is not supported)
+ * 2. switch to HS200 mode
+ * 3. set the clock to > 52Mhz and <=200MHz
+ */
+static int mmc_select_hs200(struct mmc_card *card)
+{
+	struct mmc_host *host = card->host;
+	int err = -EINVAL;
+
+	if (card->mmc_avail_type & EXT_CSD_CARD_TYPE_HS200_1_2V)
+		err = __mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_120);
+
+	if (err && card->mmc_avail_type & EXT_CSD_CARD_TYPE_HS200_1_8V)
+		err = __mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_180);
+
+	/* If fails try again during next card power cycle */
+	if (err)
+		goto err;
+
+	/*
+	 * Set the bus width(4 or 8) with host's support and
+	 * switch to HS200 mode if bus width is set successfully.
+	 */
+	err = mmc_select_bus_width(card);
+	if (!IS_ERR_VALUE(err)) {
 		err = __mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
-				EXT_CSD_HS_TIMING, 2,
-				card->ext_csd.generic_cmd6_time,
-				true, true, true);
+				   EXT_CSD_HS_TIMING, EXT_CSD_TIMING_HS200,
+				   card->ext_csd.generic_cmd6_time,
+				   true, true, true);
+		if (!err)
+			mmc_set_timing(host, MMC_TIMING_MMC_HS200);
+	}
 err:
 	return err;
 }
 
 /*
+ * Activate High Speed or HS200 mode if supported.
+ */
+static int mmc_select_timing(struct mmc_card *card)
+{
+	int err = 0;
+
+	if ((card->csd.mmca_vsn < CSD_SPEC_VER_4 &&
+	     card->ext_csd.hs_max_dtr == 0))
+		goto bus_speed;
+
+	if (card->mmc_avail_type & EXT_CSD_CARD_TYPE_HS200)
+		err = mmc_select_hs200(card);
+	else if (card->mmc_avail_type & EXT_CSD_CARD_TYPE_HS)
+		err = mmc_select_hs(card);
+
+	if (err && err != -EBADMSG)
+		return err;
+
+	if (err) {
+		pr_warn("%s: switch to %s failed\n",
+			mmc_card_hs(card) ? "high-speed" :
+			(mmc_card_hs200(card) ? "hs200" : ""),
+			mmc_hostname(card->host));
+		err = 0;
+	}
+
+bus_speed:
+	/*
+	 * Set the bus speed to the selected bus timing.
+	 * If timing is not selected, backward compatible is the default.
+	 */
+	mmc_set_bus_speed(card);
+	return err;
+}
+
+/*
+ * Execute tuning sequence to seek the proper bus operating
+ * conditions for HS200 and HS400, which sends CMD21 to the device.
+ */
+static int mmc_hs200_tuning(struct mmc_card *card)
+{
+	struct mmc_host *host = card->host;
+	int err = 0;
+
+	/*
+	 * Timing should be adjusted to the HS400 target
+	 * operation frequency for tuning process
+	 */
+	if (card->mmc_avail_type & EXT_CSD_CARD_TYPE_HS400 &&
+	    host->ios.bus_width == MMC_BUS_WIDTH_8)
+		if (host->ops->prepare_hs400_tuning)
+			host->ops->prepare_hs400_tuning(host, &host->ios);
+
+	if (host->ops->execute_tuning) {
+		mmc_host_clk_hold(host);
+		err = host->ops->execute_tuning(host,
+				MMC_SEND_TUNING_BLOCK_HS200);
+		mmc_host_clk_release(host);
+
+		if (err)
+			pr_warn("%s: tuning execution failed\n",
+				mmc_hostname(host));
+	}
+
+	return err;
+}
+
+/*
  * Handle the detection and initialisation of a card.
  *
  * In the case of a resume, "oldcard" will contain the card
@@ -874,9 +1177,8 @@ static int mmc_init_card(struct mmc_host *host, u32 ocr,
 	struct mmc_card *oldcard)
 {
 	struct mmc_card *card;
-	int err, ddr = 0;
+	int err;
 	u32 cid[4];
-	unsigned int max_dtr;
 	u32 rocr;
 	u8 *ext_csd = NULL;
 
@@ -1068,206 +1370,34 @@ static int mmc_init_card(struct mmc_host *host, u32 ocr,
 	}
 
 	/*
-	 * Activate high speed (if supported)
-	 */
-	if (card->ext_csd.hs_max_dtr != 0) {
-		err = 0;
-		if (card->ext_csd.hs_max_dtr > 52000000 &&
-		    host->caps2 & MMC_CAP2_HS200)
-			err = mmc_select_hs200(card);
-		else if	(host->caps & MMC_CAP_MMC_HIGHSPEED)
-			err = __mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
-					EXT_CSD_HS_TIMING, 1,
-					card->ext_csd.generic_cmd6_time,
-					true, true, true);
-
-		if (err && err != -EBADMSG)
-			goto free_card;
-
-		if (err) {
-			pr_warning("%s: switch to highspeed failed\n",
-			       mmc_hostname(card->host));
-			err = 0;
-		} else {
-			if (card->ext_csd.hs_max_dtr > 52000000 &&
-			    host->caps2 & MMC_CAP2_HS200) {
-				mmc_card_set_hs200(card);
-				mmc_set_timing(card->host,
-					       MMC_TIMING_MMC_HS200);
-			} else {
-				mmc_card_set_highspeed(card);
-				mmc_set_timing(card->host, MMC_TIMING_MMC_HS);
-			}
-		}
-	}
-
-	/*
-	 * Compute bus speed.
-	 */
-	max_dtr = (unsigned int)-1;
-
-	if (mmc_card_highspeed(card) || mmc_card_hs200(card)) {
-		if (max_dtr > card->ext_csd.hs_max_dtr)
-			max_dtr = card->ext_csd.hs_max_dtr;
-		if (mmc_card_highspeed(card) && (max_dtr > 52000000))
-			max_dtr = 52000000;
-	} else if (max_dtr > card->csd.max_dtr) {
-		max_dtr = card->csd.max_dtr;
-	}
-
-	mmc_set_clock(host, max_dtr);
-
-	/*
-	 * Indicate DDR mode (if supported).
+	 * Select timing interface
 	 */
-	if (mmc_card_highspeed(card)) {
-		if ((card->ext_csd.card_type & EXT_CSD_CARD_TYPE_DDR_1_8V)
-			&& (host->caps & MMC_CAP_1_8V_DDR))
-				ddr = MMC_1_8V_DDR_MODE;
-		else if ((card->ext_csd.card_type & EXT_CSD_CARD_TYPE_DDR_1_2V)
-			&& (host->caps & MMC_CAP_1_2V_DDR))
-				ddr = MMC_1_2V_DDR_MODE;
-	}
+	err = mmc_select_timing(card);
+	if (err)
+		goto free_card;
 
-	/*
-	 * Indicate HS200 SDR mode (if supported).
-	 */
 	if (mmc_card_hs200(card)) {
-		u32 ext_csd_bits;
-		u32 bus_width = card->host->ios.bus_width;
-
-		/*
-		 * For devices supporting HS200 mode, the bus width has
-		 * to be set before executing the tuning function. If
-		 * set before tuning, then device will respond with CRC
-		 * errors for responses on CMD line. So for HS200 the
-		 * sequence will be
-		 * 1. set bus width 4bit / 8 bit (1 bit not supported)
-		 * 2. switch to HS200 mode
-		 * 3. set the clock to > 52Mhz <=200MHz and
-		 * 4. execute tuning for HS200
-		 */
-		if ((host->caps2 & MMC_CAP2_HS200) &&
-		    card->host->ops->execute_tuning) {
-			mmc_host_clk_hold(card->host);
-			err = card->host->ops->execute_tuning(card->host,
-				MMC_SEND_TUNING_BLOCK_HS200);
-			mmc_host_clk_release(card->host);
-		}
-		if (err) {
-			pr_warning("%s: tuning execution failed\n",
-				   mmc_hostname(card->host));
+		err = mmc_hs200_tuning(card);
+		if (err)
 			goto err;
-		}
 
-		ext_csd_bits = (bus_width == MMC_BUS_WIDTH_8) ?
-				EXT_CSD_BUS_WIDTH_8 : EXT_CSD_BUS_WIDTH_4;
-		err = mmc_select_powerclass(card, ext_csd_bits);
+		err = mmc_select_hs400(card);
 		if (err)
-			pr_warning("%s: power class selection to bus width %d"
-				   " failed\n", mmc_hostname(card->host),
-				   1 << bus_width);
+			goto err;
+	} else if (mmc_card_hs(card)) {
+		/* Select the desired bus width optionally */
+		err = mmc_select_bus_width(card);
+		if (!IS_ERR_VALUE(err)) {
+			err = mmc_select_hs_ddr(card);
+			if (err)
+				goto err;
+		}
 	}
 
 	/*
-	 * Activate wide bus and DDR (if supported).
+	 * Choose the power class with selected bus interface
 	 */
-	if (!mmc_card_hs200(card) &&
-	    (card->csd.mmca_vsn >= CSD_SPEC_VER_4) &&
-	    (host->caps & (MMC_CAP_4_BIT_DATA | MMC_CAP_8_BIT_DATA))) {
-		static unsigned ext_csd_bits[][2] = {
-			{ EXT_CSD_BUS_WIDTH_8, EXT_CSD_DDR_BUS_WIDTH_8 },
-			{ EXT_CSD_BUS_WIDTH_4, EXT_CSD_DDR_BUS_WIDTH_4 },
-			{ EXT_CSD_BUS_WIDTH_1, EXT_CSD_BUS_WIDTH_1 },
-		};
-		static unsigned bus_widths[] = {
-			MMC_BUS_WIDTH_8,
-			MMC_BUS_WIDTH_4,
-			MMC_BUS_WIDTH_1
-		};
-		unsigned idx, bus_width = 0;
-
-		if (host->caps & MMC_CAP_8_BIT_DATA)
-			idx = 0;
-		else
-			idx = 1;
-		for (; idx < ARRAY_SIZE(bus_widths); idx++) {
-			bus_width = bus_widths[idx];
-			if (bus_width == MMC_BUS_WIDTH_1)
-				ddr = 0; /* no DDR for 1-bit width */
-			err = mmc_select_powerclass(card, ext_csd_bits[idx][0]);
-			if (err)
-				pr_warning("%s: power class selection to "
-					   "bus width %d failed\n",
-					   mmc_hostname(card->host),
-					   1 << bus_width);
-
-			err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
-					 EXT_CSD_BUS_WIDTH,
-					 ext_csd_bits[idx][0],
-					 card->ext_csd.generic_cmd6_time);
-			if (!err) {
-				mmc_set_bus_width(card->host, bus_width);
-
-				/*
-				 * If controller can't handle bus width test,
-				 * compare ext_csd previously read in 1 bit mode
-				 * against ext_csd at new bus width
-				 */
-				if (!(host->caps & MMC_CAP_BUS_WIDTH_TEST))
-					err = mmc_compare_ext_csds(card,
-						bus_width);
-				else
-					err = mmc_bus_test(card, bus_width);
-				if (!err)
-					break;
-			}
-		}
-
-		if (!err && ddr) {
-			err = mmc_select_powerclass(card, ext_csd_bits[idx][1]);
-			if (err)
-				pr_warning("%s: power class selection to "
-					   "bus width %d ddr %d failed\n",
-					   mmc_hostname(card->host),
-					   1 << bus_width, ddr);
-
-			err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
-					 EXT_CSD_BUS_WIDTH,
-					 ext_csd_bits[idx][1],
-					 card->ext_csd.generic_cmd6_time);
-		}
-		if (err) {
-			pr_warning("%s: switch to bus width %d ddr %d "
-				"failed\n", mmc_hostname(card->host),
-				1 << bus_width, ddr);
-			goto free_card;
-		} else if (ddr) {
-			/*
-			 * eMMC cards can support 3.3V to 1.2V i/o (vccq)
-			 * signaling.
-			 *
-			 * EXT_CSD_CARD_TYPE_DDR_1_8V means 3.3V or 1.8V vccq.
-			 *
-			 * 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.
-			 *
-			 * WARNING: eMMC rules are NOT the same as SD DDR
-			 */
-			if (ddr == MMC_1_2V_DDR_MODE) {
-				err = __mmc_set_signal_voltage(host,
-					MMC_SIGNAL_VOLTAGE_120);
-				if (err)
-					goto err;
-			}
-			mmc_card_set_ddr_mode(card);
-			mmc_set_timing(card->host, MMC_TIMING_UHS_DDR50);
-			mmc_set_bus_width(card->host, bus_width);
-		}
-	}
+	mmc_select_powerclass(card);
 
 	/*
 	 * Enable HPI feature (if supported)
@@ -1507,7 +1637,6 @@ static int _mmc_suspend(struct mmc_host *host, bool is_suspend)
 		err = mmc_sleep(host);
 	else if (!mmc_host_is_spi(host))
 		err = mmc_deselect_cards(host);
-	host->card->state &= ~(MMC_STATE_HIGHSPEED | MMC_STATE_HIGHSPEED_200);
 
 	if (!err) {
 		mmc_power_off(host);
@@ -1637,7 +1766,6 @@ static int mmc_power_restore(struct mmc_host *host)
 {
 	int ret;
 
-	host->card->state &= ~(MMC_STATE_HIGHSPEED | MMC_STATE_HIGHSPEED_200);
 	mmc_claim_host(host);
 	ret = mmc_init_card(host, host->card->ocr, host->card);
 	mmc_release_host(host);
diff --git a/drivers/mmc/core/sd.c b/drivers/mmc/core/sd.c
index 2dd359d2242..0c44510bf71 100644
--- a/drivers/mmc/core/sd.c
+++ b/drivers/mmc/core/sd.c
@@ -707,18 +707,10 @@ static struct attribute *sd_std_attrs[] = {
 	&dev_attr_serial.attr,
 	NULL,
 };
-
-static struct attribute_group sd_std_attr_group = {
-	.attrs = sd_std_attrs,
-};
-
-static const struct attribute_group *sd_attr_groups[] = {
-	&sd_std_attr_group,
-	NULL,
-};
+ATTRIBUTE_GROUPS(sd_std);
 
 struct device_type sd_type = {
-	.groups = sd_attr_groups,
+	.groups = sd_std_groups,
 };
 
 /*
@@ -895,7 +887,7 @@ unsigned mmc_sd_get_max_clock(struct mmc_card *card)
 {
 	unsigned max_dtr = (unsigned int)-1;
 
-	if (mmc_card_highspeed(card)) {
+	if (mmc_card_hs(card)) {
 		if (max_dtr > card->sw_caps.hs_max_dtr)
 			max_dtr = card->sw_caps.hs_max_dtr;
 	} else if (max_dtr > card->csd.max_dtr) {
@@ -905,12 +897,6 @@ unsigned mmc_sd_get_max_clock(struct mmc_card *card)
 	return max_dtr;
 }
 
-void mmc_sd_go_highspeed(struct mmc_card *card)
-{
-	mmc_card_set_highspeed(card);
-	mmc_set_timing(card->host, MMC_TIMING_SD_HS);
-}
-
 /*
  * Handle the detection and initialisation of a card.
  *
@@ -985,16 +971,13 @@ static int mmc_sd_init_card(struct mmc_host *host, u32 ocr,
 		err = mmc_sd_init_uhs_card(card);
 		if (err)
 			goto free_card;
-
-		/* Card is an ultra-high-speed card */
-		mmc_card_set_uhs(card);
 	} else {
 		/*
 		 * Attempt to change to high-speed (if supported)
 		 */
 		err = mmc_sd_switch_hs(card);
 		if (err > 0)
-			mmc_sd_go_highspeed(card);
+			mmc_set_timing(card->host, MMC_TIMING_SD_HS);
 		else if (err)
 			goto free_card;
 
@@ -1089,7 +1072,7 @@ static int _mmc_sd_suspend(struct mmc_host *host)
 
 	if (!mmc_host_is_spi(host))
 		err = mmc_deselect_cards(host);
-	host->card->state &= ~MMC_STATE_HIGHSPEED;
+
 	if (!err) {
 		mmc_power_off(host);
 		mmc_card_set_suspended(host->card);
@@ -1198,7 +1181,6 @@ static int mmc_sd_power_restore(struct mmc_host *host)
 {
 	int ret;
 
-	host->card->state &= ~MMC_STATE_HIGHSPEED;
 	mmc_claim_host(host);
 	ret = mmc_sd_init_card(host, host->card->ocr, host->card);
 	mmc_release_host(host);
diff --git a/drivers/mmc/core/sd.h b/drivers/mmc/core/sd.h
index 4b34b24f3f7..aab824a9a7f 100644
--- a/drivers/mmc/core/sd.h
+++ b/drivers/mmc/core/sd.h
@@ -12,6 +12,5 @@ int mmc_sd_setup_card(struct mmc_host *host, struct mmc_card *card,
 	bool reinit);
 unsigned mmc_sd_get_max_clock(struct mmc_card *card);
 int mmc_sd_switch_hs(struct mmc_card *card);
-void mmc_sd_go_highspeed(struct mmc_card *card);
 
 #endif
diff --git a/drivers/mmc/core/sdio.c b/drivers/mmc/core/sdio.c
index 4d721c6e2af..e636d9e99e4 100644
--- a/drivers/mmc/core/sdio.c
+++ b/drivers/mmc/core/sdio.c
@@ -363,7 +363,7 @@ static unsigned mmc_sdio_get_max_clock(struct mmc_card *card)
 {
 	unsigned max_dtr;
 
-	if (mmc_card_highspeed(card)) {
+	if (mmc_card_hs(card)) {
 		/*
 		 * The SDIO specification doesn't mention how
 		 * the CIS transfer speed register relates to
@@ -733,7 +733,6 @@ try_again:
 		mmc_set_clock(host, card->cis.max_dtr);
 
 		if (card->cccr.high_speed) {
-			mmc_card_set_highspeed(card);
 			mmc_set_timing(card->host, MMC_TIMING_SD_HS);
 		}
 
@@ -792,16 +791,13 @@ try_again:
 		err = mmc_sdio_init_uhs_card(card);
 		if (err)
 			goto remove;
-
-		/* Card is an ultra-high-speed card */
-		mmc_card_set_uhs(card);
 	} else {
 		/*
 		 * Switch to high-speed (if supported).
 		 */
 		err = sdio_enable_hs(card);
 		if (err > 0)
-			mmc_sd_go_highspeed(card);
+			mmc_set_timing(card->host, MMC_TIMING_SD_HS);
 		else if (err)
 			goto remove;
 
@@ -943,40 +939,21 @@ static int mmc_sdio_pre_suspend(struct mmc_host *host)
  */
 static int mmc_sdio_suspend(struct mmc_host *host)
 {
-	int i, err = 0;
-
-	for (i = 0; i < host->card->sdio_funcs; i++) {
-		struct sdio_func *func = host->card->sdio_func[i];
-		if (func && sdio_func_present(func) && func->dev.driver) {
-			const struct dev_pm_ops *pmops = func->dev.driver->pm;
-			err = pmops->suspend(&func->dev);
-			if (err)
-				break;
-		}
-	}
-	while (err && --i >= 0) {
-		struct sdio_func *func = host->card->sdio_func[i];
-		if (func && sdio_func_present(func) && func->dev.driver) {
-			const struct dev_pm_ops *pmops = func->dev.driver->pm;
-			pmops->resume(&func->dev);
-		}
-	}
-
-	if (!err && mmc_card_keep_power(host) && mmc_card_wake_sdio_irq(host)) {
+	if (mmc_card_keep_power(host) && mmc_card_wake_sdio_irq(host)) {
 		mmc_claim_host(host);
 		sdio_disable_wide(host->card);
 		mmc_release_host(host);
 	}
 
-	if (!err && !mmc_card_keep_power(host))
+	if (!mmc_card_keep_power(host))
 		mmc_power_off(host);
 
-	return err;
+	return 0;
 }
 
 static int mmc_sdio_resume(struct mmc_host *host)
 {
-	int i, err = 0;
+	int err = 0;
 
 	BUG_ON(!host);
 	BUG_ON(!host->card);
@@ -1019,24 +996,6 @@ static int mmc_sdio_resume(struct mmc_host *host)
 		wake_up_process(host->sdio_irq_thread);
 	mmc_release_host(host);
 
-	/*
-	 * If the card looked to be the same as before suspending, then
-	 * we proceed to resume all card functions.  If one of them returns
-	 * an error then we simply return that error to the core and the
-	 * card will be redetected as new.  It is the responsibility of
-	 * the function driver to perform further tests with the extra
-	 * knowledge it has of the card to confirm the card is indeed the
-	 * same as before suspending (same MAC address for network cards,
-	 * etc.) and return an error otherwise.
-	 */
-	for (i = 0; !err && i < host->card->sdio_funcs; i++) {
-		struct sdio_func *func = host->card->sdio_func[i];
-		if (func && sdio_func_present(func) && func->dev.driver) {
-			const struct dev_pm_ops *pmops = func->dev.driver->pm;
-			err = pmops->resume(&func->dev);
-		}
-	}
-
 	host->pm_flags &= ~MMC_PM_KEEP_POWER;
 	return err;
 }
diff --git a/drivers/mmc/core/sdio_bus.c b/drivers/mmc/core/sdio_bus.c
index 92d1ba8e815..4fa8fef9147 100644
--- a/drivers/mmc/core/sdio_bus.c
+++ b/drivers/mmc/core/sdio_bus.c
@@ -197,20 +197,8 @@ static int sdio_bus_remove(struct device *dev)
 
 #ifdef CONFIG_PM
 
-#ifdef CONFIG_PM_SLEEP
-static int pm_no_operation(struct device *dev)
-{
-	/*
-	 * Prevent the PM core from calling SDIO device drivers' suspend
-	 * callback routines, which it is not supposed to do, by using this
-	 * empty function as the bus type suspend callaback for SDIO.
-	 */
-	return 0;
-}
-#endif
-
 static const struct dev_pm_ops sdio_bus_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(pm_no_operation, pm_no_operation)
+	SET_SYSTEM_SLEEP_PM_OPS(pm_generic_suspend, pm_generic_resume)
 	SET_RUNTIME_PM_OPS(
 		pm_generic_runtime_suspend,
 		pm_generic_runtime_resume,
diff --git a/drivers/mmc/core/sdio_irq.c b/drivers/mmc/core/sdio_irq.c
index aaa90460ed2..5cc13c8d35b 100644
--- a/drivers/mmc/core/sdio_irq.c
+++ b/drivers/mmc/core/sdio_irq.c
@@ -90,6 +90,15 @@ static int process_sdio_pending_irqs(struct mmc_host *host)
 	return ret;
 }
 
+void sdio_run_irqs(struct mmc_host *host)
+{
+	mmc_claim_host(host);
+	host->sdio_irq_pending = true;
+	process_sdio_pending_irqs(host);
+	mmc_release_host(host);
+}
+EXPORT_SYMBOL_GPL(sdio_run_irqs);
+
 static int sdio_irq_thread(void *_host)
 {
 	struct mmc_host *host = _host;
@@ -189,14 +198,20 @@ static int sdio_card_irq_get(struct mmc_card *card)
 	WARN_ON(!host->claimed);
 
 	if (!host->sdio_irqs++) {
-		atomic_set(&host->sdio_irq_thread_abort, 0);
-		host->sdio_irq_thread =
-			kthread_run(sdio_irq_thread, host, "ksdioirqd/%s",
-				mmc_hostname(host));
-		if (IS_ERR(host->sdio_irq_thread)) {
-			int err = PTR_ERR(host->sdio_irq_thread);
-			host->sdio_irqs--;
-			return err;
+		if (!(host->caps2 & MMC_CAP2_SDIO_IRQ_NOTHREAD)) {
+			atomic_set(&host->sdio_irq_thread_abort, 0);
+			host->sdio_irq_thread =
+				kthread_run(sdio_irq_thread, host,
+					    "ksdioirqd/%s", mmc_hostname(host));
+			if (IS_ERR(host->sdio_irq_thread)) {
+				int err = PTR_ERR(host->sdio_irq_thread);
+				host->sdio_irqs--;
+				return err;
+			}
+		} else {
+			mmc_host_clk_hold(host);
+			host->ops->enable_sdio_irq(host, 1);
+			mmc_host_clk_release(host);
 		}
 	}
 
@@ -211,8 +226,14 @@ static int sdio_card_irq_put(struct mmc_card *card)
 	BUG_ON(host->sdio_irqs < 1);
 
 	if (!--host->sdio_irqs) {
-		atomic_set(&host->sdio_irq_thread_abort, 1);
-		kthread_stop(host->sdio_irq_thread);
+		if (!(host->caps2 & MMC_CAP2_SDIO_IRQ_NOTHREAD)) {
+			atomic_set(&host->sdio_irq_thread_abort, 1);
+			kthread_stop(host->sdio_irq_thread);
+		} else {
+			mmc_host_clk_hold(host);
+			host->ops->enable_sdio_irq(host, 0);
+			mmc_host_clk_release(host);
+		}
 	}
 
 	return 0;
diff --git a/drivers/mmc/core/slot-gpio.c b/drivers/mmc/core/slot-gpio.c
index f7650b899e3..5f89cb83d5f 100644
--- a/drivers/mmc/core/slot-gpio.c
+++ b/drivers/mmc/core/slot-gpio.c
@@ -32,9 +32,7 @@ static irqreturn_t mmc_gpio_cd_irqt(int irq, void *dev_id)
 	/* Schedule a card detection after a debounce timeout */
 	struct mmc_host *host = dev_id;
 
-	if (host->ops->card_event)
-		host->ops->card_event(host);
-
+	host->trigger_card_event = true;
 	mmc_detect_change(host, msecs_to_jiffies(200));
 
 	return IRQ_HANDLED;
diff --git a/drivers/mmc/host/Kconfig b/drivers/mmc/host/Kconfig
index 8aaf8c1f3f6..7fee22432e9 100644
--- a/drivers/mmc/host/Kconfig
+++ b/drivers/mmc/host/Kconfig
@@ -168,7 +168,7 @@ config MMC_SDHCI_ESDHC_IMX
 
 config MMC_SDHCI_DOVE
 	tristate "SDHCI support on Marvell's Dove SoC"
-	depends on ARCH_DOVE
+	depends on ARCH_DOVE || MACH_DOVE
 	depends on MMC_SDHCI_PLTFM
 	select MMC_SDHCI_IO_ACCESSORS
 	help
@@ -283,6 +283,15 @@ config MMC_SDHCI_BCM2835
 
 	  If unsure, say N.
 
+config MMC_MOXART
+	tristate "MOXART SD/MMC Host Controller support"
+	depends on ARCH_MOXART && MMC
+	help
+	  This selects support for the MOXART SD/MMC Host Controller.
+	  MOXA provides one multi-functional card reader which can
+	  be found on some embedded hardware such as UC-7112-LX.
+	  If you have a controller with this interface, say Y here.
+
 config MMC_OMAP
 	tristate "TI OMAP Multimedia Card Interface support"
 	depends on ARCH_OMAP
@@ -688,9 +697,29 @@ config MMC_WMT
 	  To compile this driver as a module, choose M here: the
 	  module will be called wmt-sdmmc.
 
+config MMC_USDHI6ROL0
+	tristate "Renesas USDHI6ROL0 SD/SDIO Host Controller support"
+	help
+	  This selects support for the Renesas USDHI6ROL0 SD/SDIO
+	  Host Controller
+
 config MMC_REALTEK_PCI
 	tristate "Realtek PCI-E SD/MMC Card Interface Driver"
 	depends on MFD_RTSX_PCI
 	help
 	  Say Y here to include driver code to support SD/MMC card interface
 	  of Realtek PCI-E card reader
+
+config MMC_REALTEK_USB
+	tristate "Realtek USB SD/MMC Card Interface Driver"
+	depends on MFD_RTSX_USB
+	help
+	  Say Y here to include driver code to support SD/MMC card interface
+	  of Realtek RTS5129/39 series card reader
+
+config MMC_SUNXI
+	tristate "Allwinner sunxi SD/MMC Host Controller support"
+	depends on ARCH_SUNXI
+	help
+	  This selects support for the SD/MMC Host Controller on
+	  Allwinner sunxi SoCs.
diff --git a/drivers/mmc/host/Makefile b/drivers/mmc/host/Makefile
index 0c8aa5e1e30..7f81ddf1dd2 100644
--- a/drivers/mmc/host/Makefile
+++ b/drivers/mmc/host/Makefile
@@ -50,8 +50,12 @@ obj-$(CONFIG_MMC_JZ4740)	+= jz4740_mmc.o
 obj-$(CONFIG_MMC_VUB300)	+= vub300.o
 obj-$(CONFIG_MMC_USHC)		+= ushc.o
 obj-$(CONFIG_MMC_WMT)		+= wmt-sdmmc.o
+obj-$(CONFIG_MMC_MOXART)	+= moxart-mmc.o
+obj-$(CONFIG_MMC_SUNXI)		+= sunxi-mmc.o
+obj-$(CONFIG_MMC_USDHI6ROL0)	+= usdhi6rol0.o
 
 obj-$(CONFIG_MMC_REALTEK_PCI)	+= rtsx_pci_sdmmc.o
+obj-$(CONFIG_MMC_REALTEK_USB)	+= rtsx_usb_sdmmc.o
 
 obj-$(CONFIG_MMC_SDHCI_PLTFM)		+= sdhci-pltfm.o
 obj-$(CONFIG_MMC_SDHCI_CNS3XXX)		+= sdhci-cns3xxx.o
diff --git a/drivers/mmc/host/atmel-mci.c b/drivers/mmc/host/atmel-mci.c
index 42706ea0ba8..aece7cafbb9 100644
--- a/drivers/mmc/host/atmel-mci.c
+++ b/drivers/mmc/host/atmel-mci.c
@@ -820,16 +820,9 @@ static void atmci_pdc_complete(struct atmel_mci *host)
 
 	atmci_pdc_cleanup(host);
 
-	/*
-	 * If the card was removed, data will be NULL. No point trying
-	 * to send the stop command or waiting for NBUSY in this case.
-	 */
-	if (host->data) {
-		dev_dbg(&host->pdev->dev,
-		        "(%s) set pending xfer complete\n", __func__);
-		atmci_set_pending(host, EVENT_XFER_COMPLETE);
-		tasklet_schedule(&host->tasklet);
-	}
+	dev_dbg(&host->pdev->dev, "(%s) set pending xfer complete\n", __func__);
+	atmci_set_pending(host, EVENT_XFER_COMPLETE);
+	tasklet_schedule(&host->tasklet);
 }
 
 static void atmci_dma_cleanup(struct atmel_mci *host)
diff --git a/drivers/mmc/host/dw_mmc-exynos.c b/drivers/mmc/host/dw_mmc-exynos.c
index 3423c5ed50c..0fbc53ac7ea 100644
--- a/drivers/mmc/host/dw_mmc-exynos.c
+++ b/drivers/mmc/host/dw_mmc-exynos.c
@@ -187,7 +187,7 @@ static void dw_mci_exynos_set_ios(struct dw_mci *host, struct mmc_ios *ios)
 	unsigned long actual;
 	u8 div = priv->ciu_div + 1;
 
-	if (ios->timing == MMC_TIMING_UHS_DDR50) {
+	if (ios->timing == MMC_TIMING_MMC_DDR52) {
 		mci_writel(host, CLKSEL, priv->ddr_timing);
 		/* Should be double rate for DDR mode */
 		if (ios->bus_width == MMC_BUS_WIDTH_8)
@@ -386,8 +386,7 @@ static int dw_mci_exynos_execute_tuning(struct dw_mci_slot *slot, u32 opcode,
 
 /* Common capabilities of Exynos4/Exynos5 SoC */
 static unsigned long exynos_dwmmc_caps[4] = {
-	MMC_CAP_UHS_DDR50 | MMC_CAP_1_8V_DDR |
-		MMC_CAP_8_BIT_DATA | MMC_CAP_CMD23,
+	MMC_CAP_1_8V_DDR | MMC_CAP_8_BIT_DATA | MMC_CAP_CMD23,
 	MMC_CAP_CMD23,
 	MMC_CAP_CMD23,
 	MMC_CAP_CMD23,
@@ -426,7 +425,7 @@ static int dw_mci_exynos_probe(struct platform_device *pdev)
 	return dw_mci_pltfm_register(pdev, drv_data);
 }
 
-const struct dev_pm_ops dw_mci_exynos_pmops = {
+static const struct dev_pm_ops dw_mci_exynos_pmops = {
 	SET_SYSTEM_SLEEP_PM_OPS(dw_mci_exynos_suspend, dw_mci_exynos_resume)
 	.resume_noirq = dw_mci_exynos_resume_noirq,
 	.thaw_noirq = dw_mci_exynos_resume_noirq,
diff --git a/drivers/mmc/host/dw_mmc.c b/drivers/mmc/host/dw_mmc.c
index cced599d5ae..1ac227c603b 100644
--- a/drivers/mmc/host/dw_mmc.c
+++ b/drivers/mmc/host/dw_mmc.c
@@ -235,12 +235,6 @@ err:
 }
 #endif /* defined(CONFIG_DEBUG_FS) */
 
-static void dw_mci_set_timeout(struct dw_mci *host)
-{
-	/* timeout (maximum) */
-	mci_writel(host, TMOUT, 0xffffffff);
-}
-
 static u32 dw_mci_prepare_command(struct mmc_host *mmc, struct mmc_command *cmd)
 {
 	struct mmc_data	*data;
@@ -257,9 +251,8 @@ static u32 dw_mci_prepare_command(struct mmc_host *mmc, struct mmc_command *cmd)
 	    (cmd->opcode == SD_IO_RW_DIRECT &&
 	     ((cmd->arg >> 9) & 0x1FFFF) == SDIO_CCCR_ABORT))
 		cmdr |= SDMMC_CMD_STOP;
-	else
-		if (cmd->opcode != MMC_SEND_STATUS && cmd->data)
-			cmdr |= SDMMC_CMD_PRV_DAT_WAIT;
+	else if (cmd->opcode != MMC_SEND_STATUS && cmd->data)
+		cmdr |= SDMMC_CMD_PRV_DAT_WAIT;
 
 	if (cmd->flags & MMC_RSP_PRESENT) {
 		/* We expect a response, so set this bit */
@@ -850,8 +843,6 @@ static void __dw_mci_start_request(struct dw_mci *host,
 	u32 cmdflags;
 
 	mrq = slot->mrq;
-	if (host->pdata->select_slot)
-		host->pdata->select_slot(slot->id);
 
 	host->cur_slot = slot;
 	host->mrq = mrq;
@@ -864,7 +855,7 @@ static void __dw_mci_start_request(struct dw_mci *host,
 
 	data = cmd->data;
 	if (data) {
-		dw_mci_set_timeout(host);
+		mci_writel(host, TMOUT, 0xFFFFFFFF);
 		mci_writel(host, BYTCNT, data->blksz*data->blocks);
 		mci_writel(host, BLKSIZ, data->blksz);
 	}
@@ -962,7 +953,7 @@ static void dw_mci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	regs = mci_readl(slot->host, UHS_REG);
 
 	/* DDR mode set */
-	if (ios->timing == MMC_TIMING_UHS_DDR50)
+	if (ios->timing == MMC_TIMING_MMC_DDR52)
 		regs |= ((0x1 << slot->id) << 16);
 	else
 		regs &= ~((0x1 << slot->id) << 16);
@@ -985,17 +976,11 @@ static void dw_mci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	switch (ios->power_mode) {
 	case MMC_POWER_UP:
 		set_bit(DW_MMC_CARD_NEED_INIT, &slot->flags);
-		/* Power up slot */
-		if (slot->host->pdata->setpower)
-			slot->host->pdata->setpower(slot->id, mmc->ocr_avail);
 		regs = mci_readl(slot->host, PWREN);
 		regs |= (1 << slot->id);
 		mci_writel(slot->host, PWREN, regs);
 		break;
 	case MMC_POWER_OFF:
-		/* Power down slot */
-		if (slot->host->pdata->setpower)
-			slot->host->pdata->setpower(slot->id, 0);
 		regs = mci_readl(slot->host, PWREN);
 		regs &= ~(1 << slot->id);
 		mci_writel(slot->host, PWREN, regs);
@@ -1009,15 +994,13 @@ static int dw_mci_get_ro(struct mmc_host *mmc)
 {
 	int read_only;
 	struct dw_mci_slot *slot = mmc_priv(mmc);
-	struct dw_mci_board *brd = slot->host->pdata;
+	int gpio_ro = mmc_gpio_get_ro(mmc);
 
 	/* Use platform get_ro function, else try on board write protect */
 	if (slot->quirks & DW_MCI_SLOT_QUIRK_NO_WRITE_PROTECT)
 		read_only = 0;
-	else if (brd->get_ro)
-		read_only = brd->get_ro(slot->id);
-	else if (gpio_is_valid(slot->wp_gpio))
-		read_only = gpio_get_value(slot->wp_gpio);
+	else if (!IS_ERR_VALUE(gpio_ro))
+		read_only = gpio_ro;
 	else
 		read_only =
 			mci_readl(slot->host, WRTPRT) & (1 << slot->id) ? 1 : 0;
@@ -1039,8 +1022,6 @@ static int dw_mci_get_cd(struct mmc_host *mmc)
 	/* Use platform get_cd function, else try onboard card detect */
 	if (brd->quirks & DW_MCI_QUIRK_BROKEN_CARD_DETECTION)
 		present = 1;
-	else if (brd->get_cd)
-		present = !brd->get_cd(slot->id);
 	else if (!IS_ERR_VALUE(gpio_cd))
 		present = gpio_cd;
 	else
@@ -1248,7 +1229,7 @@ static int dw_mci_data_complete(struct dw_mci *host, struct mmc_data *data)
 			data->error = -EIO;
 		}
 
-		dev_err(host->dev, "data error, status 0x%08x\n", status);
+		dev_dbg(host->dev, "data error, status 0x%08x\n", status);
 
 		/*
 		 * After an error, there may be data lingering
@@ -2045,86 +2026,15 @@ static int dw_mci_of_get_slot_quirks(struct device *dev, u8 slot)
 
 	return quirks;
 }
-
-/* find out bus-width for a given slot */
-static u32 dw_mci_of_get_bus_wd(struct device *dev, u8 slot)
-{
-	struct device_node *np = dw_mci_of_find_slot_node(dev, slot);
-	u32 bus_wd = 1;
-
-	if (!np)
-		return 1;
-
-	if (of_property_read_u32(np, "bus-width", &bus_wd))
-		dev_err(dev, "bus-width property not found, assuming width"
-			       " as 1\n");
-	return bus_wd;
-}
-
-/* find the write protect gpio for a given slot; or -1 if none specified */
-static int dw_mci_of_get_wp_gpio(struct device *dev, u8 slot)
-{
-	struct device_node *np = dw_mci_of_find_slot_node(dev, slot);
-	int gpio;
-
-	if (!np)
-		return -EINVAL;
-
-	gpio = of_get_named_gpio(np, "wp-gpios", 0);
-
-	/* Having a missing entry is valid; return silently */
-	if (!gpio_is_valid(gpio))
-		return -EINVAL;
-
-	if (devm_gpio_request(dev, gpio, "dw-mci-wp")) {
-		dev_warn(dev, "gpio [%d] request failed\n", gpio);
-		return -EINVAL;
-	}
-
-	return gpio;
-}
-
-/* find the cd gpio for a given slot */
-static void dw_mci_of_get_cd_gpio(struct device *dev, u8 slot,
-					struct mmc_host *mmc)
-{
-	struct device_node *np = dw_mci_of_find_slot_node(dev, slot);
-	int gpio;
-
-	if (!np)
-		return;
-
-	gpio = of_get_named_gpio(np, "cd-gpios", 0);
-
-	/* Having a missing entry is valid; return silently */
-	if (!gpio_is_valid(gpio))
-		return;
-
-	if (mmc_gpio_request_cd(mmc, gpio, 0))
-		dev_warn(dev, "gpio [%d] request failed\n", gpio);
-}
 #else /* CONFIG_OF */
 static int dw_mci_of_get_slot_quirks(struct device *dev, u8 slot)
 {
 	return 0;
 }
-static u32 dw_mci_of_get_bus_wd(struct device *dev, u8 slot)
-{
-	return 1;
-}
 static struct device_node *dw_mci_of_find_slot_node(struct device *dev, u8 slot)
 {
 	return NULL;
 }
-static int dw_mci_of_get_wp_gpio(struct device *dev, u8 slot)
-{
-	return -EINVAL;
-}
-static void dw_mci_of_get_cd_gpio(struct device *dev, u8 slot,
-					struct mmc_host *mmc)
-{
-	return;
-}
 #endif /* CONFIG_OF */
 
 static int dw_mci_init_slot(struct dw_mci *host, unsigned int id)
@@ -2134,7 +2044,6 @@ static int dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 	const struct dw_mci_drv_data *drv_data = host->drv_data;
 	int ctrl_id, ret;
 	u32 freq[2];
-	u8 bus_width;
 
 	mmc = mmc_alloc_host(sizeof(struct dw_mci_slot), host->dev);
 	if (!mmc)
@@ -2158,17 +2067,7 @@ static int dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 		mmc->f_max = freq[1];
 	}
 
-	if (host->pdata->get_ocr)
-		mmc->ocr_avail = host->pdata->get_ocr(id);
-	else
-		mmc->ocr_avail = MMC_VDD_32_33 | MMC_VDD_33_34;
-
-	/*
-	 * Start with slot power disabled, it will be enabled when a card
-	 * is detected.
-	 */
-	if (host->pdata->setpower)
-		host->pdata->setpower(id, 0);
+	mmc->ocr_avail = MMC_VDD_32_33 | MMC_VDD_33_34;
 
 	if (host->pdata->caps)
 		mmc->caps = host->pdata->caps;
@@ -2189,19 +2088,7 @@ static int dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 	if (host->pdata->caps2)
 		mmc->caps2 = host->pdata->caps2;
 
-	if (host->pdata->get_bus_wd)
-		bus_width = host->pdata->get_bus_wd(slot->id);
-	else if (host->dev->of_node)
-		bus_width = dw_mci_of_get_bus_wd(host->dev, slot->id);
-	else
-		bus_width = 1;
-
-	switch (bus_width) {
-	case 8:
-		mmc->caps |= MMC_CAP_8_BIT_DATA;
-	case 4:
-		mmc->caps |= MMC_CAP_4_BIT_DATA;
-	}
+	mmc_of_parse(mmc);
 
 	if (host->pdata->blk_settings) {
 		mmc->max_segs = host->pdata->blk_settings->max_segs;
@@ -2226,8 +2113,10 @@ static int dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 #endif /* CONFIG_MMC_DW_IDMAC */
 	}
 
-	slot->wp_gpio = dw_mci_of_get_wp_gpio(host->dev, slot->id);
-	dw_mci_of_get_cd_gpio(host->dev, slot->id, mmc);
+	if (dw_mci_get_cd(mmc))
+		set_bit(DW_MMC_CARD_PRESENT, &slot->flags);
+	else
+		clear_bit(DW_MMC_CARD_PRESENT, &slot->flags);
 
 	ret = mmc_add_host(mmc);
 	if (ret)
@@ -2249,10 +2138,6 @@ err_setup_bus:
 
 static void dw_mci_cleanup_slot(struct dw_mci_slot *slot, unsigned int id)
 {
-	/* Shutdown detect IRQ */
-	if (slot->host->pdata->exit)
-		slot->host->pdata->exit(id);
-
 	/* Debugfs stuff is cleaned up by mmc core */
 	mmc_remove_host(slot->mmc);
 	slot->host->slot[id] = NULL;
@@ -2399,24 +2284,9 @@ static struct dw_mci_board *dw_mci_parse_dt(struct dw_mci *host)
 			return ERR_PTR(ret);
 	}
 
-	if (of_find_property(np, "keep-power-in-suspend", NULL))
-		pdata->pm_caps |= MMC_PM_KEEP_POWER;
-
-	if (of_find_property(np, "enable-sdio-wakeup", NULL))
-		pdata->pm_caps |= MMC_PM_WAKE_SDIO_IRQ;
-
 	if (of_find_property(np, "supports-highspeed", NULL))
 		pdata->caps |= MMC_CAP_SD_HIGHSPEED | MMC_CAP_MMC_HIGHSPEED;
 
-	if (of_find_property(np, "caps2-mmc-hs200-1_8v", NULL))
-		pdata->caps2 |= MMC_CAP2_HS200_1_8V_SDR;
-
-	if (of_find_property(np, "caps2-mmc-hs200-1_2v", NULL))
-		pdata->caps2 |= MMC_CAP2_HS200_1_2V_SDR;
-
-	if (of_get_property(np, "cd-inverted", NULL))
-		pdata->caps2 |= MMC_CAP2_CD_ACTIVE_HIGH;
-
 	return pdata;
 }
 
@@ -2442,9 +2312,9 @@ int dw_mci_probe(struct dw_mci *host)
 		}
 	}
 
-	if (!host->pdata->select_slot && host->pdata->num_slots > 1) {
+	if (host->pdata->num_slots > 1) {
 		dev_err(host->dev,
-			"Platform data must supply select_slot function\n");
+			"Platform data must supply num_slots.\n");
 		return -ENODEV;
 	}
 
@@ -2474,12 +2344,19 @@ int dw_mci_probe(struct dw_mci *host)
 			ret = clk_set_rate(host->ciu_clk, host->pdata->bus_hz);
 			if (ret)
 				dev_warn(host->dev,
-					 "Unable to set bus rate to %ul\n",
+					 "Unable to set bus rate to %uHz\n",
 					 host->pdata->bus_hz);
 		}
 		host->bus_hz = clk_get_rate(host->ciu_clk);
 	}
 
+	if (!host->bus_hz) {
+		dev_err(host->dev,
+			"Platform data must supply bus speed\n");
+		ret = -ENODEV;
+		goto err_clk_ciu;
+	}
+
 	if (drv_data && drv_data->init) {
 		ret = drv_data->init(host);
 		if (ret) {
@@ -2516,13 +2393,6 @@ int dw_mci_probe(struct dw_mci *host)
 		}
 	}
 
-	if (!host->bus_hz) {
-		dev_err(host->dev,
-			"Platform data must supply bus speed\n");
-		ret = -ENODEV;
-		goto err_regulator;
-	}
-
 	host->quirks = host->pdata->quirks;
 
 	spin_lock_init(&host->lock);
@@ -2666,8 +2536,6 @@ err_workqueue:
 err_dmaunmap:
 	if (host->use_dma && host->dma_ops->exit)
 		host->dma_ops->exit(host);
-
-err_regulator:
 	if (host->vmmc)
 		regulator_disable(host->vmmc);
 
diff --git a/drivers/mmc/host/dw_mmc.h b/drivers/mmc/host/dw_mmc.h
index 68349779c39..738fa241d05 100644
--- a/drivers/mmc/host/dw_mmc.h
+++ b/drivers/mmc/host/dw_mmc.h
@@ -195,7 +195,6 @@ extern int dw_mci_resume(struct dw_mci *host);
  * @mmc: The mmc_host representing this slot.
  * @host: The MMC controller this slot is using.
  * @quirks: Slot-level quirks (DW_MCI_SLOT_QUIRK_XXX)
- * @wp_gpio: If gpio_is_valid() we'll use this to read write protect.
  * @ctype: Card type for this slot.
  * @mrq: mmc_request currently being processed or waiting to be
  *	processed, or NULL when the slot is idle.
@@ -214,7 +213,6 @@ struct dw_mci_slot {
 	struct dw_mci		*host;
 
 	int			quirks;
-	int			wp_gpio;
 
 	u32			ctype;
 
diff --git a/drivers/mmc/host/jz4740_mmc.c b/drivers/mmc/host/jz4740_mmc.c
index de2139cf344..537d6c7a5ae 100644
--- a/drivers/mmc/host/jz4740_mmc.c
+++ b/drivers/mmc/host/jz4740_mmc.c
@@ -515,10 +515,13 @@ static irqreturn_t jz_mmc_irq_worker(int irq, void *devid)
 
 		jz4740_mmc_send_command(host, req->stop);
 
-		timeout = jz4740_mmc_poll_irq(host, JZ_MMC_IRQ_PRG_DONE);
-		if (timeout) {
-			host->state = JZ4740_MMC_STATE_DONE;
-			break;
+		if (mmc_resp_type(req->stop) & MMC_RSP_BUSY) {
+			timeout = jz4740_mmc_poll_irq(host,
+						      JZ_MMC_IRQ_PRG_DONE);
+			if (timeout) {
+				host->state = JZ4740_MMC_STATE_DONE;
+				break;
+			}
 		}
 	case JZ4740_MMC_STATE_DONE:
 		break;
diff --git a/drivers/mmc/host/mmci.c b/drivers/mmc/host/mmci.c
index 771c60ab4a3..7ad463e9741 100644
--- a/drivers/mmc/host/mmci.c
+++ b/drivers/mmc/host/mmci.c
@@ -13,6 +13,7 @@
 #include <linux/init.h>
 #include <linux/ioport.h>
 #include <linux/device.h>
+#include <linux/io.h>
 #include <linux/interrupt.h>
 #include <linux/kernel.h>
 #include <linux/slab.h>
@@ -23,6 +24,7 @@
 #include <linux/mmc/pm.h>
 #include <linux/mmc/host.h>
 #include <linux/mmc/card.h>
+#include <linux/mmc/slot-gpio.h>
 #include <linux/amba/bus.h>
 #include <linux/clk.h>
 #include <linux/scatterlist.h>
@@ -299,7 +301,8 @@ static void mmci_set_clkreg(struct mmci_host *host, unsigned int desired)
 	if (host->mmc->ios.bus_width == MMC_BUS_WIDTH_8)
 		clk |= MCI_ST_8BIT_BUS;
 
-	if (host->mmc->ios.timing == MMC_TIMING_UHS_DDR50)
+	if (host->mmc->ios.timing == MMC_TIMING_UHS_DDR50 ||
+	    host->mmc->ios.timing == MMC_TIMING_MMC_DDR52)
 		clk |= MCI_ST_UX500_NEG_EDGE;
 
 	mmci_write_clkreg(host, clk);
@@ -364,7 +367,6 @@ static void mmci_init_sg(struct mmci_host *host, struct mmc_data *data)
 #ifdef CONFIG_DMA_ENGINE
 static void mmci_dma_setup(struct mmci_host *host)
 {
-	struct mmci_platform_data *plat = host->plat;
 	const char *rxname, *txname;
 	dma_cap_mask_t mask;
 
@@ -378,25 +380,6 @@ static void mmci_dma_setup(struct mmci_host *host)
 	dma_cap_zero(mask);
 	dma_cap_set(DMA_SLAVE, mask);
 
-	if (plat && plat->dma_filter) {
-		if (!host->dma_rx_channel && plat->dma_rx_param) {
-			host->dma_rx_channel = dma_request_channel(mask,
-							   plat->dma_filter,
-							   plat->dma_rx_param);
-			/* E.g if no DMA hardware is present */
-			if (!host->dma_rx_channel)
-				dev_err(mmc_dev(host->mmc), "no RX DMA channel\n");
-		}
-
-		if (!host->dma_tx_channel && plat->dma_tx_param) {
-			host->dma_tx_channel = dma_request_channel(mask,
-							   plat->dma_filter,
-							   plat->dma_tx_param);
-			if (!host->dma_tx_channel)
-				dev_warn(mmc_dev(host->mmc), "no TX DMA channel\n");
-		}
-	}
-
 	/*
 	 * If only an RX channel is specified, the driver will
 	 * attempt to use it bidirectionally, however if it is
@@ -444,11 +427,9 @@ static void mmci_dma_setup(struct mmci_host *host)
  */
 static inline void mmci_dma_release(struct mmci_host *host)
 {
-	struct mmci_platform_data *plat = host->plat;
-
 	if (host->dma_rx_channel)
 		dma_release_channel(host->dma_rx_channel);
-	if (host->dma_tx_channel && plat->dma_tx_param)
+	if (host->dma_tx_channel)
 		dma_release_channel(host->dma_tx_channel);
 	host->dma_rx_channel = host->dma_tx_channel = NULL;
 }
@@ -784,7 +765,8 @@ static void mmci_start_data(struct mmci_host *host, struct mmc_data *data)
 			mmci_write_clkreg(host, clk);
 		}
 
-	if (host->mmc->ios.timing == MMC_TIMING_UHS_DDR50)
+	if (host->mmc->ios.timing == MMC_TIMING_UHS_DDR50 ||
+	    host->mmc->ios.timing == MMC_TIMING_MMC_DDR52)
 		datactrl |= MCI_ST_DPSM_DDRMODE;
 
 	/*
@@ -1285,7 +1267,7 @@ static void mmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 		 * indicating signal direction for the signals in
 		 * the SD/MMC bus and feedback-clock usage.
 		 */
-		pwr |= host->plat->sigdir;
+		pwr |= host->pwr_reg_add;
 
 		if (ios->bus_width == MMC_BUS_WIDTH_4)
 			pwr &= ~MCI_ST_DATA74DIREN;
@@ -1326,35 +1308,18 @@ static void mmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	pm_runtime_put_autosuspend(mmc_dev(mmc));
 }
 
-static int mmci_get_ro(struct mmc_host *mmc)
-{
-	struct mmci_host *host = mmc_priv(mmc);
-
-	if (host->gpio_wp == -ENOSYS)
-		return -ENOSYS;
-
-	return gpio_get_value_cansleep(host->gpio_wp);
-}
-
 static int mmci_get_cd(struct mmc_host *mmc)
 {
 	struct mmci_host *host = mmc_priv(mmc);
 	struct mmci_platform_data *plat = host->plat;
-	unsigned int status;
+	unsigned int status = mmc_gpio_get_cd(mmc);
 
-	if (host->gpio_cd == -ENOSYS) {
+	if (status == -ENOSYS) {
 		if (!plat->status)
 			return 1; /* Assume always present */
 
 		status = plat->status(mmc_dev(host->mmc));
-	} else
-		status = !!gpio_get_value_cansleep(host->gpio_cd)
-			^ plat->cd_invert;
-
-	/*
-	 * Use positive logic throughout - status is zero for no card,
-	 * non-zero for card inserted.
-	 */
+	}
 	return status;
 }
 
@@ -1391,70 +1356,44 @@ static int mmci_sig_volt_switch(struct mmc_host *mmc, struct mmc_ios *ios)
 	return ret;
 }
 
-static irqreturn_t mmci_cd_irq(int irq, void *dev_id)
-{
-	struct mmci_host *host = dev_id;
-
-	mmc_detect_change(host->mmc, msecs_to_jiffies(500));
-
-	return IRQ_HANDLED;
-}
-
 static struct mmc_host_ops mmci_ops = {
 	.request	= mmci_request,
 	.pre_req	= mmci_pre_request,
 	.post_req	= mmci_post_request,
 	.set_ios	= mmci_set_ios,
-	.get_ro		= mmci_get_ro,
+	.get_ro		= mmc_gpio_get_ro,
 	.get_cd		= mmci_get_cd,
 	.start_signal_voltage_switch = mmci_sig_volt_switch,
 };
 
-#ifdef CONFIG_OF
-static void mmci_dt_populate_generic_pdata(struct device_node *np,
-					struct mmci_platform_data *pdata)
+static int mmci_of_parse(struct device_node *np, struct mmc_host *mmc)
 {
-	int bus_width = 0;
-
-	pdata->gpio_wp = of_get_named_gpio(np, "wp-gpios", 0);
-	pdata->gpio_cd = of_get_named_gpio(np, "cd-gpios", 0);
+	struct mmci_host *host = mmc_priv(mmc);
+	int ret = mmc_of_parse(mmc);
 
-	if (of_get_property(np, "cd-inverted", NULL))
-		pdata->cd_invert = true;
-	else
-		pdata->cd_invert = false;
+	if (ret)
+		return ret;
 
-	of_property_read_u32(np, "max-frequency", &pdata->f_max);
-	if (!pdata->f_max)
-		pr_warn("%s has no 'max-frequency' property\n", np->full_name);
+	if (of_get_property(np, "st,sig-dir-dat0", NULL))
+		host->pwr_reg_add |= MCI_ST_DATA0DIREN;
+	if (of_get_property(np, "st,sig-dir-dat2", NULL))
+		host->pwr_reg_add |= MCI_ST_DATA2DIREN;
+	if (of_get_property(np, "st,sig-dir-dat31", NULL))
+		host->pwr_reg_add |= MCI_ST_DATA31DIREN;
+	if (of_get_property(np, "st,sig-dir-dat74", NULL))
+		host->pwr_reg_add |= MCI_ST_DATA74DIREN;
+	if (of_get_property(np, "st,sig-dir-cmd", NULL))
+		host->pwr_reg_add |= MCI_ST_CMDDIREN;
+	if (of_get_property(np, "st,sig-pin-fbclk", NULL))
+		host->pwr_reg_add |= MCI_ST_FBCLKEN;
 
 	if (of_get_property(np, "mmc-cap-mmc-highspeed", NULL))
-		pdata->capabilities |= MMC_CAP_MMC_HIGHSPEED;
+		mmc->caps |= MMC_CAP_MMC_HIGHSPEED;
 	if (of_get_property(np, "mmc-cap-sd-highspeed", NULL))
-		pdata->capabilities |= MMC_CAP_SD_HIGHSPEED;
+		mmc->caps |= MMC_CAP_SD_HIGHSPEED;
 
-	of_property_read_u32(np, "bus-width", &bus_width);
-	switch (bus_width) {
-	case 0 :
-		/* No bus-width supplied. */
-		break;
-	case 4 :
-		pdata->capabilities |= MMC_CAP_4_BIT_DATA;
-		break;
-	case 8 :
-		pdata->capabilities |= MMC_CAP_8_BIT_DATA;
-		break;
-	default :
-		pr_warn("%s: Unsupported bus width\n", np->full_name);
-	}
-}
-#else
-static void mmci_dt_populate_generic_pdata(struct device_node *np,
-					struct mmci_platform_data *pdata)
-{
-	return;
+	return 0;
 }
-#endif
 
 static int mmci_probe(struct amba_device *dev,
 	const struct amba_id *id)
@@ -1478,26 +1417,17 @@ static int mmci_probe(struct amba_device *dev,
 			return -ENOMEM;
 	}
 
-	if (np)
-		mmci_dt_populate_generic_pdata(np, plat);
+	mmc = mmc_alloc_host(sizeof(struct mmci_host), &dev->dev);
+	if (!mmc)
+		return -ENOMEM;
 
-	ret = amba_request_regions(dev, DRIVER_NAME);
+	ret = mmci_of_parse(np, mmc);
 	if (ret)
-		goto out;
-
-	mmc = mmc_alloc_host(sizeof(struct mmci_host), &dev->dev);
-	if (!mmc) {
-		ret = -ENOMEM;
-		goto rel_regions;
-	}
+		goto host_free;
 
 	host = mmc_priv(mmc);
 	host->mmc = mmc;
 
-	host->gpio_wp = -ENOSYS;
-	host->gpio_cd = -ENOSYS;
-	host->gpio_cd_irq = -1;
-
 	host->hw_designer = amba_manf(dev);
 	host->hw_revision = amba_rev(dev);
 	dev_dbg(mmc_dev(mmc), "designer ID = 0x%02x\n", host->hw_designer);
@@ -1529,10 +1459,11 @@ static int mmci_probe(struct amba_device *dev,
 		dev_dbg(mmc_dev(mmc), "eventual mclk rate: %u Hz\n",
 			host->mclk);
 	}
+
 	host->phybase = dev->res.start;
-	host->base = ioremap(dev->res.start, resource_size(&dev->res));
-	if (!host->base) {
-		ret = -ENOMEM;
+	host->base = devm_ioremap_resource(&dev->dev, &dev->res);
+	if (IS_ERR(host->base)) {
+		ret = PTR_ERR(host->base);
 		goto clk_disable;
 	}
 
@@ -1546,15 +1477,13 @@ static int mmci_probe(struct amba_device *dev,
 	else
 		mmc->f_min = DIV_ROUND_UP(host->mclk, 512);
 	/*
-	 * If the platform data supplies a maximum operating
-	 * frequency, this takes precedence. Else, we fall back
-	 * to using the module parameter, which has a (low)
-	 * default value in case it is not specified. Either
-	 * value must not exceed the clock rate into the block,
-	 * of course.
+	 * If no maximum operating frequency is supplied, fall back to use
+	 * the module parameter, which has a (low) default value in case it
+	 * is not specified. Either value must not exceed the clock rate into
+	 * the block, of course.
 	 */
-	if (plat->f_max)
-		mmc->f_max = min(host->mclk, plat->f_max);
+	if (mmc->f_max)
+		mmc->f_max = min(host->mclk, mmc->f_max);
 	else
 		mmc->f_max = min(host->mclk, fmax);
 	dev_dbg(mmc_dev(mmc), "clocking block at %u Hz\n", mmc->f_max);
@@ -1566,8 +1495,15 @@ static int mmci_probe(struct amba_device *dev,
 	else if (plat->ocr_mask)
 		dev_warn(mmc_dev(mmc), "Platform OCR mask is ignored\n");
 
-	mmc->caps = plat->capabilities;
-	mmc->caps2 = plat->capabilities2;
+	/* DT takes precedence over platform data. */
+	if (!np) {
+		if (!plat->cd_invert)
+			mmc->caps2 |= MMC_CAP2_CD_ACTIVE_HIGH;
+		mmc->caps2 |= MMC_CAP2_RO_ACTIVE_HIGH;
+	}
+
+	/* We support these capabilities. */
+	mmc->caps |= MMC_CAP_CMD23;
 
 	if (variant->busy_detect) {
 		mmci_ops.card_busy = mmci_card_busy;
@@ -1579,7 +1515,7 @@ static int mmci_probe(struct amba_device *dev,
 	mmc->ops = &mmci_ops;
 
 	/* We support these PM capabilities. */
-	mmc->pm_caps = MMC_PM_KEEP_POWER;
+	mmc->pm_caps |= MMC_PM_KEEP_POWER;
 
 	/*
 	 * We can do SGIO
@@ -1616,62 +1552,30 @@ static int mmci_probe(struct amba_device *dev,
 	writel(0, host->base + MMCIMASK1);
 	writel(0xfff, host->base + MMCICLEAR);
 
-	if (plat->gpio_cd == -EPROBE_DEFER) {
-		ret = -EPROBE_DEFER;
-		goto err_gpio_cd;
-	}
-	if (gpio_is_valid(plat->gpio_cd)) {
-		ret = gpio_request(plat->gpio_cd, DRIVER_NAME " (cd)");
-		if (ret == 0)
-			ret = gpio_direction_input(plat->gpio_cd);
-		if (ret == 0)
-			host->gpio_cd = plat->gpio_cd;
-		else if (ret != -ENOSYS)
-			goto err_gpio_cd;
-
-		/*
-		 * A gpio pin that will detect cards when inserted and removed
-		 * will most likely want to trigger on the edges if it is
-		 * 0 when ejected and 1 when inserted (or mutatis mutandis
-		 * for the inverted case) so we request triggers on both
-		 * edges.
-		 */
-		ret = request_any_context_irq(gpio_to_irq(plat->gpio_cd),
-				mmci_cd_irq,
-				IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
-				DRIVER_NAME " (cd)", host);
-		if (ret >= 0)
-			host->gpio_cd_irq = gpio_to_irq(plat->gpio_cd);
-	}
-	if (plat->gpio_wp == -EPROBE_DEFER) {
-		ret = -EPROBE_DEFER;
-		goto err_gpio_wp;
+	/* 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 (gpio_is_valid(plat->gpio_wp)) {
-		ret = gpio_request(plat->gpio_wp, DRIVER_NAME " (wp)");
-		if (ret == 0)
-			ret = gpio_direction_input(plat->gpio_wp);
-		if (ret == 0)
-			host->gpio_wp = plat->gpio_wp;
-		else if (ret != -ENOSYS)
-			goto err_gpio_wp;
+	if (!np && gpio_is_valid(plat->gpio_wp)) {
+		ret = mmc_gpio_request_ro(mmc, plat->gpio_wp);
+		if (ret)
+			goto clk_disable;
 	}
 
-	if ((host->plat->status || host->gpio_cd != -ENOSYS)
-	    && host->gpio_cd_irq < 0)
-		mmc->caps |= MMC_CAP_NEEDS_POLL;
-
-	ret = request_irq(dev->irq[0], mmci_irq, IRQF_SHARED, DRIVER_NAME " (cmd)", host);
+	ret = devm_request_irq(&dev->dev, dev->irq[0], mmci_irq, IRQF_SHARED,
+			DRIVER_NAME " (cmd)", host);
 	if (ret)
-		goto unmap;
+		goto clk_disable;
 
 	if (!dev->irq[1])
 		host->singleirq = true;
 	else {
-		ret = request_irq(dev->irq[1], mmci_pio_irq, IRQF_SHARED,
-				  DRIVER_NAME " (pio)", host);
+		ret = devm_request_irq(&dev->dev, dev->irq[1], mmci_pio_irq,
+				IRQF_SHARED, DRIVER_NAME " (pio)", host);
 		if (ret)
-			goto irq0_free;
+			goto clk_disable;
 	}
 
 	writel(MCI_IRQENABLE, host->base + MMCIMASK0);
@@ -1693,25 +1597,10 @@ static int mmci_probe(struct amba_device *dev,
 
 	return 0;
 
- irq0_free:
-	free_irq(dev->irq[0], host);
- unmap:
-	if (host->gpio_wp != -ENOSYS)
-		gpio_free(host->gpio_wp);
- err_gpio_wp:
-	if (host->gpio_cd_irq >= 0)
-		free_irq(host->gpio_cd_irq, host);
-	if (host->gpio_cd != -ENOSYS)
-		gpio_free(host->gpio_cd);
- err_gpio_cd:
-	iounmap(host->base);
  clk_disable:
 	clk_disable_unprepare(host->clk);
  host_free:
 	mmc_free_host(mmc);
- rel_regions:
-	amba_release_regions(dev);
- out:
 	return ret;
 }
 
@@ -1737,92 +1626,46 @@ static int mmci_remove(struct amba_device *dev)
 		writel(0, host->base + MMCIDATACTRL);
 
 		mmci_dma_release(host);
-		free_irq(dev->irq[0], host);
-		if (!host->singleirq)
-			free_irq(dev->irq[1], host);
-
-		if (host->gpio_wp != -ENOSYS)
-			gpio_free(host->gpio_wp);
-		if (host->gpio_cd_irq >= 0)
-			free_irq(host->gpio_cd_irq, host);
-		if (host->gpio_cd != -ENOSYS)
-			gpio_free(host->gpio_cd);
-
-		iounmap(host->base);
 		clk_disable_unprepare(host->clk);
-
 		mmc_free_host(mmc);
-
-		amba_release_regions(dev);
-	}
-
-	return 0;
-}
-
-#ifdef CONFIG_SUSPEND
-static int mmci_suspend(struct device *dev)
-{
-	struct amba_device *adev = to_amba_device(dev);
-	struct mmc_host *mmc = amba_get_drvdata(adev);
-
-	if (mmc) {
-		struct mmci_host *host = mmc_priv(mmc);
-		pm_runtime_get_sync(dev);
-		writel(0, host->base + MMCIMASK0);
 	}
 
 	return 0;
 }
 
-static int mmci_resume(struct device *dev)
-{
-	struct amba_device *adev = to_amba_device(dev);
-	struct mmc_host *mmc = amba_get_drvdata(adev);
-
-	if (mmc) {
-		struct mmci_host *host = mmc_priv(mmc);
-		writel(MCI_IRQENABLE, host->base + MMCIMASK0);
-		pm_runtime_put(dev);
-	}
-
-	return 0;
-}
-#endif
-
-#ifdef CONFIG_PM_RUNTIME
+#ifdef CONFIG_PM
 static void mmci_save(struct mmci_host *host)
 {
 	unsigned long flags;
 
-	if (host->variant->pwrreg_nopower) {
-		spin_lock_irqsave(&host->lock, flags);
+	spin_lock_irqsave(&host->lock, flags);
 
-		writel(0, host->base + MMCIMASK0);
+	writel(0, host->base + MMCIMASK0);
+	if (host->variant->pwrreg_nopower) {
 		writel(0, host->base + MMCIDATACTRL);
 		writel(0, host->base + MMCIPOWER);
 		writel(0, host->base + MMCICLOCK);
-		mmci_reg_delay(host);
-
-		spin_unlock_irqrestore(&host->lock, flags);
 	}
+	mmci_reg_delay(host);
 
+	spin_unlock_irqrestore(&host->lock, flags);
 }
 
 static void mmci_restore(struct mmci_host *host)
 {
 	unsigned long flags;
 
-	if (host->variant->pwrreg_nopower) {
-		spin_lock_irqsave(&host->lock, flags);
+	spin_lock_irqsave(&host->lock, flags);
 
+	if (host->variant->pwrreg_nopower) {
 		writel(host->clk_reg, host->base + MMCICLOCK);
 		writel(host->datactrl_reg, host->base + MMCIDATACTRL);
 		writel(host->pwr_reg, host->base + MMCIPOWER);
-		writel(MCI_IRQENABLE, host->base + MMCIMASK0);
-		mmci_reg_delay(host);
-
-		spin_unlock_irqrestore(&host->lock, flags);
 	}
+	writel(MCI_IRQENABLE, host->base + MMCIMASK0);
+	mmci_reg_delay(host);
+
+	spin_unlock_irqrestore(&host->lock, flags);
 }
 
 static int mmci_runtime_suspend(struct device *dev)
@@ -1857,8 +1700,9 @@ static int mmci_runtime_resume(struct device *dev)
 #endif
 
 static const struct dev_pm_ops mmci_dev_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(mmci_suspend, mmci_resume)
-	SET_RUNTIME_PM_OPS(mmci_runtime_suspend, mmci_runtime_resume, NULL)
+	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+				pm_runtime_force_resume)
+	SET_PM_RUNTIME_PM_OPS(mmci_runtime_suspend, mmci_runtime_resume, NULL)
 };
 
 static struct amba_id mmci_ids[] = {
diff --git a/drivers/mmc/host/mmci.h b/drivers/mmc/host/mmci.h
index 58b1b8896bf..347d942d740 100644
--- a/drivers/mmc/host/mmci.h
+++ b/drivers/mmc/host/mmci.h
@@ -13,6 +13,16 @@
 #define MCI_PWR_ON		0x03
 #define MCI_OD			(1 << 6)
 #define MCI_ROD			(1 << 7)
+/*
+ * The ST Micro version does not have ROD and reuse the voltage registers for
+ * direction settings.
+ */
+#define MCI_ST_DATA2DIREN	(1 << 2)
+#define MCI_ST_CMDDIREN		(1 << 3)
+#define MCI_ST_DATA0DIREN	(1 << 4)
+#define MCI_ST_DATA31DIREN	(1 << 5)
+#define MCI_ST_FBCLKEN		(1 << 7)
+#define MCI_ST_DATA74DIREN	(1 << 8)
 
 #define MMCICLOCK		0x004
 #define MCI_CLK_ENABLE		(1 << 8)
@@ -176,9 +186,6 @@ struct mmci_host {
 	struct mmc_data		*data;
 	struct mmc_host		*mmc;
 	struct clk		*clk;
-	int			gpio_cd;
-	int			gpio_wp;
-	int			gpio_cd_irq;
 	bool			singleirq;
 
 	spinlock_t		lock;
@@ -186,6 +193,7 @@ struct mmci_host {
 	unsigned int		mclk;
 	unsigned int		cclk;
 	u32			pwr_reg;
+	u32			pwr_reg_add;
 	u32			clk_reg;
 	u32			datactrl_reg;
 	u32			busy_status;
diff --git a/drivers/mmc/host/moxart-mmc.c b/drivers/mmc/host/moxart-mmc.c
new file mode 100644
index 00000000000..74924a04026
--- /dev/null
+++ b/drivers/mmc/host/moxart-mmc.c
@@ -0,0 +1,730 @@
+/*
+ * MOXA ART MMC host driver.
+ *
+ * Copyright (C) 2014 Jonas Jensen
+ *
+ * Jonas Jensen <jonas.jensen@gmail.com>
+ *
+ * Based on code from
+ * Moxa Technologies Co., Ltd. <www.moxa.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/blkdev.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmaengine.h>
+#include <linux/mmc/host.h>
+#include <linux/mmc/sd.h>
+#include <linux/sched.h>
+#include <linux/io.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/clk.h>
+#include <linux/bitops.h>
+#include <linux/of_dma.h>
+#include <linux/spinlock.h>
+
+#define REG_COMMAND		0
+#define REG_ARGUMENT		4
+#define REG_RESPONSE0		8
+#define REG_RESPONSE1		12
+#define REG_RESPONSE2		16
+#define REG_RESPONSE3		20
+#define REG_RESPONSE_COMMAND	24
+#define REG_DATA_CONTROL	28
+#define REG_DATA_TIMER		32
+#define REG_DATA_LENGTH		36
+#define REG_STATUS		40
+#define REG_CLEAR		44
+#define REG_INTERRUPT_MASK	48
+#define REG_POWER_CONTROL	52
+#define REG_CLOCK_CONTROL	56
+#define REG_BUS_WIDTH		60
+#define REG_DATA_WINDOW		64
+#define REG_FEATURE		68
+#define REG_REVISION		72
+
+/* REG_COMMAND */
+#define CMD_SDC_RESET		BIT(10)
+#define CMD_EN			BIT(9)
+#define CMD_APP_CMD		BIT(8)
+#define CMD_LONG_RSP		BIT(7)
+#define CMD_NEED_RSP		BIT(6)
+#define CMD_IDX_MASK		0x3f
+
+/* REG_RESPONSE_COMMAND */
+#define RSP_CMD_APP		BIT(6)
+#define RSP_CMD_IDX_MASK	0x3f
+
+/* REG_DATA_CONTROL */
+#define DCR_DATA_FIFO_RESET     BIT(8)
+#define DCR_DATA_THRES          BIT(7)
+#define DCR_DATA_EN		BIT(6)
+#define DCR_DMA_EN		BIT(5)
+#define DCR_DATA_WRITE		BIT(4)
+#define DCR_BLK_SIZE		0x0f
+
+/* REG_DATA_LENGTH */
+#define DATA_LEN_MASK		0xffffff
+
+/* REG_STATUS */
+#define WRITE_PROT		BIT(12)
+#define CARD_DETECT		BIT(11)
+/* 1-10 below can be sent to either registers, interrupt or clear. */
+#define CARD_CHANGE		BIT(10)
+#define FIFO_ORUN		BIT(9)
+#define FIFO_URUN		BIT(8)
+#define DATA_END		BIT(7)
+#define CMD_SENT		BIT(6)
+#define DATA_CRC_OK		BIT(5)
+#define RSP_CRC_OK		BIT(4)
+#define DATA_TIMEOUT		BIT(3)
+#define RSP_TIMEOUT		BIT(2)
+#define DATA_CRC_FAIL		BIT(1)
+#define RSP_CRC_FAIL		BIT(0)
+
+#define MASK_RSP		(RSP_TIMEOUT | RSP_CRC_FAIL | \
+				 RSP_CRC_OK  | CARD_DETECT  | CMD_SENT)
+
+#define MASK_DATA		(DATA_CRC_OK   | DATA_END | \
+				 DATA_CRC_FAIL | DATA_TIMEOUT)
+
+#define MASK_INTR_PIO		(FIFO_URUN | FIFO_ORUN | CARD_CHANGE)
+
+/* REG_POWER_CONTROL */
+#define SD_POWER_ON		BIT(4)
+#define SD_POWER_MASK		0x0f
+
+/* REG_CLOCK_CONTROL */
+#define CLK_HISPD		BIT(9)
+#define CLK_OFF			BIT(8)
+#define CLK_SD			BIT(7)
+#define CLK_DIV_MASK		0x7f
+
+/* REG_BUS_WIDTH */
+#define BUS_WIDTH_8		BIT(2)
+#define BUS_WIDTH_4		BIT(1)
+#define BUS_WIDTH_1		BIT(0)
+
+#define MMC_VDD_360		23
+#define MIN_POWER		(MMC_VDD_360 - SD_POWER_MASK)
+#define MAX_RETRIES		500000
+
+struct moxart_host {
+	spinlock_t			lock;
+
+	void __iomem			*base;
+
+	phys_addr_t			reg_phys;
+
+	struct dma_chan			*dma_chan_tx;
+	struct dma_chan                 *dma_chan_rx;
+	struct dma_async_tx_descriptor	*tx_desc;
+	struct mmc_host			*mmc;
+	struct mmc_request		*mrq;
+	struct scatterlist		*cur_sg;
+	struct completion		dma_complete;
+	struct completion		pio_complete;
+
+	u32				num_sg;
+	u32				data_remain;
+	u32				data_len;
+	u32				fifo_width;
+	u32				timeout;
+	u32				rate;
+
+	long				sysclk;
+
+	bool				have_dma;
+	bool				is_removed;
+};
+
+static inline void moxart_init_sg(struct moxart_host *host,
+				  struct mmc_data *data)
+{
+	host->cur_sg = data->sg;
+	host->num_sg = data->sg_len;
+	host->data_remain = host->cur_sg->length;
+
+	if (host->data_remain > host->data_len)
+		host->data_remain = host->data_len;
+}
+
+static inline int moxart_next_sg(struct moxart_host *host)
+{
+	int remain;
+	struct mmc_data *data = host->mrq->cmd->data;
+
+	host->cur_sg++;
+	host->num_sg--;
+
+	if (host->num_sg > 0) {
+		host->data_remain = host->cur_sg->length;
+		remain = host->data_len - data->bytes_xfered;
+		if (remain > 0 && remain < host->data_remain)
+			host->data_remain = remain;
+	}
+
+	return host->num_sg;
+}
+
+static int moxart_wait_for_status(struct moxart_host *host,
+				  u32 mask, u32 *status)
+{
+	int ret = -ETIMEDOUT;
+	u32 i;
+
+	for (i = 0; i < MAX_RETRIES; i++) {
+		*status = readl(host->base + REG_STATUS);
+		if (!(*status & mask)) {
+			udelay(5);
+			continue;
+		}
+		writel(*status & mask, host->base + REG_CLEAR);
+		ret = 0;
+		break;
+	}
+
+	if (ret)
+		dev_err(mmc_dev(host->mmc), "timed out waiting for status\n");
+
+	return ret;
+}
+
+
+static void moxart_send_command(struct moxart_host *host,
+	struct mmc_command *cmd)
+{
+	u32 status, cmdctrl;
+
+	writel(RSP_TIMEOUT  | RSP_CRC_OK |
+	       RSP_CRC_FAIL | CMD_SENT, host->base + REG_CLEAR);
+	writel(cmd->arg, host->base + REG_ARGUMENT);
+
+	cmdctrl = cmd->opcode & CMD_IDX_MASK;
+	if (cmdctrl == SD_APP_SET_BUS_WIDTH    || cmdctrl == SD_APP_OP_COND   ||
+	    cmdctrl == SD_APP_SEND_SCR         || cmdctrl == SD_APP_SD_STATUS ||
+	    cmdctrl == SD_APP_SEND_NUM_WR_BLKS)
+		cmdctrl |= CMD_APP_CMD;
+
+	if (cmd->flags & MMC_RSP_PRESENT)
+		cmdctrl |= CMD_NEED_RSP;
+
+	if (cmd->flags & MMC_RSP_136)
+		cmdctrl |= CMD_LONG_RSP;
+
+	writel(cmdctrl | CMD_EN, host->base + REG_COMMAND);
+
+	if (moxart_wait_for_status(host, MASK_RSP, &status) == -ETIMEDOUT)
+		cmd->error = -ETIMEDOUT;
+
+	if (status & RSP_TIMEOUT) {
+		cmd->error = -ETIMEDOUT;
+		return;
+	}
+	if (status & RSP_CRC_FAIL) {
+		cmd->error = -EIO;
+		return;
+	}
+	if (status & RSP_CRC_OK) {
+		if (cmd->flags & MMC_RSP_136) {
+			cmd->resp[3] = readl(host->base + REG_RESPONSE0);
+			cmd->resp[2] = readl(host->base + REG_RESPONSE1);
+			cmd->resp[1] = readl(host->base + REG_RESPONSE2);
+			cmd->resp[0] = readl(host->base + REG_RESPONSE3);
+		} else {
+			cmd->resp[0] = readl(host->base + REG_RESPONSE0);
+		}
+	}
+}
+
+static void moxart_dma_complete(void *param)
+{
+	struct moxart_host *host = param;
+
+	complete(&host->dma_complete);
+}
+
+static void moxart_transfer_dma(struct mmc_data *data, struct moxart_host *host)
+{
+	u32 len, dir_data, dir_slave;
+	unsigned long dma_time;
+	struct dma_async_tx_descriptor *desc = NULL;
+	struct dma_chan *dma_chan;
+
+	if (host->data_len == data->bytes_xfered)
+		return;
+
+	if (data->flags & MMC_DATA_WRITE) {
+		dma_chan = host->dma_chan_tx;
+		dir_data = DMA_TO_DEVICE;
+		dir_slave = DMA_MEM_TO_DEV;
+	} else {
+		dma_chan = host->dma_chan_rx;
+		dir_data = DMA_FROM_DEVICE;
+		dir_slave = DMA_DEV_TO_MEM;
+	}
+
+	len = dma_map_sg(dma_chan->device->dev, data->sg,
+			 data->sg_len, dir_data);
+
+	if (len > 0) {
+		desc = dmaengine_prep_slave_sg(dma_chan, data->sg,
+					       len, dir_slave,
+					       DMA_PREP_INTERRUPT |
+					       DMA_CTRL_ACK);
+	} else {
+		dev_err(mmc_dev(host->mmc), "dma_map_sg returned zero length\n");
+	}
+
+	if (desc) {
+		host->tx_desc = desc;
+		desc->callback = moxart_dma_complete;
+		desc->callback_param = host;
+		dmaengine_submit(desc);
+		dma_async_issue_pending(dma_chan);
+	}
+
+	data->bytes_xfered += host->data_remain;
+
+	dma_time = wait_for_completion_interruptible_timeout(
+		   &host->dma_complete, host->timeout);
+
+	dma_unmap_sg(dma_chan->device->dev,
+		     data->sg, data->sg_len,
+		     dir_data);
+}
+
+
+static void moxart_transfer_pio(struct moxart_host *host)
+{
+	struct mmc_data *data = host->mrq->cmd->data;
+	u32 *sgp, len = 0, remain, status;
+
+	if (host->data_len == data->bytes_xfered)
+		return;
+
+	sgp = sg_virt(host->cur_sg);
+	remain = host->data_remain;
+
+	if (data->flags & MMC_DATA_WRITE) {
+		while (remain > 0) {
+			if (moxart_wait_for_status(host, FIFO_URUN, &status)
+			     == -ETIMEDOUT) {
+				data->error = -ETIMEDOUT;
+				complete(&host->pio_complete);
+				return;
+			}
+			for (len = 0; len < remain && len < host->fifo_width;) {
+				iowrite32(*sgp, host->base + REG_DATA_WINDOW);
+				sgp++;
+				len += 4;
+			}
+			remain -= len;
+		}
+
+	} else {
+		while (remain > 0) {
+			if (moxart_wait_for_status(host, FIFO_ORUN, &status)
+			    == -ETIMEDOUT) {
+				data->error = -ETIMEDOUT;
+				complete(&host->pio_complete);
+				return;
+			}
+			for (len = 0; len < remain && len < host->fifo_width;) {
+				/* SCR data must be read in big endian. */
+				if (data->mrq->cmd->opcode == SD_APP_SEND_SCR)
+					*sgp = ioread32be(host->base +
+							  REG_DATA_WINDOW);
+				else
+					*sgp = ioread32(host->base +
+							REG_DATA_WINDOW);
+				sgp++;
+				len += 4;
+			}
+			remain -= len;
+		}
+	}
+
+	data->bytes_xfered += host->data_remain - remain;
+	host->data_remain = remain;
+
+	if (host->data_len != data->bytes_xfered)
+		moxart_next_sg(host);
+	else
+		complete(&host->pio_complete);
+}
+
+static void moxart_prepare_data(struct moxart_host *host)
+{
+	struct mmc_data *data = host->mrq->cmd->data;
+	u32 datactrl;
+	int blksz_bits;
+
+	if (!data)
+		return;
+
+	host->data_len = data->blocks * data->blksz;
+	blksz_bits = ffs(data->blksz) - 1;
+	BUG_ON(1 << blksz_bits != data->blksz);
+
+	moxart_init_sg(host, data);
+
+	datactrl = DCR_DATA_EN | (blksz_bits & DCR_BLK_SIZE);
+
+	if (data->flags & MMC_DATA_WRITE)
+		datactrl |= DCR_DATA_WRITE;
+
+	if ((host->data_len > host->fifo_width) && host->have_dma)
+		datactrl |= DCR_DMA_EN;
+
+	writel(DCR_DATA_FIFO_RESET, host->base + REG_DATA_CONTROL);
+	writel(MASK_DATA | FIFO_URUN | FIFO_ORUN, host->base + REG_CLEAR);
+	writel(host->rate, host->base + REG_DATA_TIMER);
+	writel(host->data_len, host->base + REG_DATA_LENGTH);
+	writel(datactrl, host->base + REG_DATA_CONTROL);
+}
+
+static void moxart_request(struct mmc_host *mmc, struct mmc_request *mrq)
+{
+	struct moxart_host *host = mmc_priv(mmc);
+	unsigned long pio_time, flags;
+	u32 status;
+
+	spin_lock_irqsave(&host->lock, flags);
+
+	init_completion(&host->dma_complete);
+	init_completion(&host->pio_complete);
+
+	host->mrq = mrq;
+
+	if (readl(host->base + REG_STATUS) & CARD_DETECT) {
+		mrq->cmd->error = -ETIMEDOUT;
+		goto request_done;
+	}
+
+	moxart_prepare_data(host);
+	moxart_send_command(host, host->mrq->cmd);
+
+	if (mrq->cmd->data) {
+		if ((host->data_len > host->fifo_width) && host->have_dma) {
+
+			writel(CARD_CHANGE, host->base + REG_INTERRUPT_MASK);
+
+			spin_unlock_irqrestore(&host->lock, flags);
+
+			moxart_transfer_dma(mrq->cmd->data, host);
+
+			spin_lock_irqsave(&host->lock, flags);
+		} else {
+
+			writel(MASK_INTR_PIO, host->base + REG_INTERRUPT_MASK);
+
+			spin_unlock_irqrestore(&host->lock, flags);
+
+			/* PIO transfers start from interrupt. */
+			pio_time = wait_for_completion_interruptible_timeout(
+				   &host->pio_complete, host->timeout);
+
+			spin_lock_irqsave(&host->lock, flags);
+		}
+
+		if (host->is_removed) {
+			dev_err(mmc_dev(host->mmc), "card removed\n");
+			mrq->cmd->error = -ETIMEDOUT;
+			goto request_done;
+		}
+
+		if (moxart_wait_for_status(host, MASK_DATA, &status)
+		    == -ETIMEDOUT) {
+			mrq->cmd->data->error = -ETIMEDOUT;
+			goto request_done;
+		}
+
+		if (status & DATA_CRC_FAIL)
+			mrq->cmd->data->error = -ETIMEDOUT;
+
+		if (mrq->cmd->data->stop)
+			moxart_send_command(host, mrq->cmd->data->stop);
+	}
+
+request_done:
+	spin_unlock_irqrestore(&host->lock, flags);
+	mmc_request_done(host->mmc, mrq);
+}
+
+static irqreturn_t moxart_irq(int irq, void *devid)
+{
+	struct moxart_host *host = (struct moxart_host *)devid;
+	u32 status;
+	unsigned long flags;
+
+	spin_lock_irqsave(&host->lock, flags);
+
+	status = readl(host->base + REG_STATUS);
+	if (status & CARD_CHANGE) {
+		host->is_removed = status & CARD_DETECT;
+		if (host->is_removed && host->have_dma) {
+			dmaengine_terminate_all(host->dma_chan_tx);
+			dmaengine_terminate_all(host->dma_chan_rx);
+		}
+		host->mrq = NULL;
+		writel(MASK_INTR_PIO, host->base + REG_CLEAR);
+		writel(CARD_CHANGE, host->base + REG_INTERRUPT_MASK);
+		mmc_detect_change(host->mmc, 0);
+	}
+	if (status & (FIFO_ORUN | FIFO_URUN) && host->mrq)
+		moxart_transfer_pio(host);
+
+	spin_unlock_irqrestore(&host->lock, flags);
+
+	return IRQ_HANDLED;
+}
+
+static void moxart_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
+{
+	struct moxart_host *host = mmc_priv(mmc);
+	unsigned long flags;
+	u8 power, div;
+	u32 ctrl;
+
+	spin_lock_irqsave(&host->lock, flags);
+
+	if (ios->clock) {
+		for (div = 0; div < CLK_DIV_MASK; ++div) {
+			if (ios->clock >= host->sysclk / (2 * (div + 1)))
+				break;
+		}
+		ctrl = CLK_SD | div;
+		host->rate = host->sysclk / (2 * (div + 1));
+		if (host->rate > host->sysclk)
+			ctrl |= CLK_HISPD;
+		writel(ctrl, host->base + REG_CLOCK_CONTROL);
+	}
+
+	if (ios->power_mode == MMC_POWER_OFF) {
+		writel(readl(host->base + REG_POWER_CONTROL) & ~SD_POWER_ON,
+		       host->base + REG_POWER_CONTROL);
+	} else {
+		if (ios->vdd < MIN_POWER)
+			power = 0;
+		else
+			power = ios->vdd - MIN_POWER;
+
+		writel(SD_POWER_ON | (u32) power,
+		       host->base + REG_POWER_CONTROL);
+	}
+
+	switch (ios->bus_width) {
+	case MMC_BUS_WIDTH_4:
+		writel(BUS_WIDTH_4, host->base + REG_BUS_WIDTH);
+		break;
+	case MMC_BUS_WIDTH_8:
+		writel(BUS_WIDTH_8, host->base + REG_BUS_WIDTH);
+		break;
+	default:
+		writel(BUS_WIDTH_1, host->base + REG_BUS_WIDTH);
+		break;
+	}
+
+	spin_unlock_irqrestore(&host->lock, flags);
+}
+
+
+static int moxart_get_ro(struct mmc_host *mmc)
+{
+	struct moxart_host *host = mmc_priv(mmc);
+
+	return !!(readl(host->base + REG_STATUS) & WRITE_PROT);
+}
+
+static struct mmc_host_ops moxart_ops = {
+	.request = moxart_request,
+	.set_ios = moxart_set_ios,
+	.get_ro = moxart_get_ro,
+};
+
+static int moxart_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *node = dev->of_node;
+	struct resource res_mmc;
+	struct mmc_host *mmc;
+	struct moxart_host *host = NULL;
+	struct dma_slave_config cfg;
+	struct clk *clk;
+	void __iomem *reg_mmc;
+	dma_cap_mask_t mask;
+	int irq, ret;
+	u32 i;
+
+	mmc = mmc_alloc_host(sizeof(struct moxart_host), dev);
+	if (!mmc) {
+		dev_err(dev, "mmc_alloc_host failed\n");
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	ret = of_address_to_resource(node, 0, &res_mmc);
+	if (ret) {
+		dev_err(dev, "of_address_to_resource failed\n");
+		goto out;
+	}
+
+	irq = irq_of_parse_and_map(node, 0);
+	if (irq <= 0) {
+		dev_err(dev, "irq_of_parse_and_map failed\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	clk = of_clk_get(node, 0);
+	if (IS_ERR(clk)) {
+		dev_err(dev, "of_clk_get failed\n");
+		ret = PTR_ERR(clk);
+		goto out;
+	}
+
+	reg_mmc = devm_ioremap_resource(dev, &res_mmc);
+	if (IS_ERR(reg_mmc)) {
+		ret = PTR_ERR(reg_mmc);
+		goto out;
+	}
+
+	mmc_of_parse(mmc);
+
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+
+	host = mmc_priv(mmc);
+	host->mmc = mmc;
+	host->base = reg_mmc;
+	host->reg_phys = res_mmc.start;
+	host->timeout = msecs_to_jiffies(1000);
+	host->sysclk = clk_get_rate(clk);
+	host->fifo_width = readl(host->base + REG_FEATURE) << 2;
+	host->dma_chan_tx = of_dma_request_slave_channel(node, "tx");
+	host->dma_chan_rx = of_dma_request_slave_channel(node, "rx");
+
+	spin_lock_init(&host->lock);
+
+	mmc->ops = &moxart_ops;
+	mmc->f_max = DIV_ROUND_CLOSEST(host->sysclk, 2);
+	mmc->f_min = DIV_ROUND_CLOSEST(host->sysclk, CLK_DIV_MASK * 2);
+	mmc->ocr_avail = 0xffff00;	/* Support 2.0v - 3.6v power. */
+
+	if (IS_ERR(host->dma_chan_tx) || IS_ERR(host->dma_chan_rx)) {
+		dev_dbg(dev, "PIO mode transfer enabled\n");
+		host->have_dma = false;
+	} else {
+		dev_dbg(dev, "DMA channels found (%p,%p)\n",
+			 host->dma_chan_tx, host->dma_chan_rx);
+		host->have_dma = true;
+
+		cfg.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+		cfg.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+
+		cfg.direction = DMA_MEM_TO_DEV;
+		cfg.src_addr = 0;
+		cfg.dst_addr = host->reg_phys + REG_DATA_WINDOW;
+		dmaengine_slave_config(host->dma_chan_tx, &cfg);
+
+		cfg.direction = DMA_DEV_TO_MEM;
+		cfg.src_addr = host->reg_phys + REG_DATA_WINDOW;
+		cfg.dst_addr = 0;
+		dmaengine_slave_config(host->dma_chan_rx, &cfg);
+	}
+
+	switch ((readl(host->base + REG_BUS_WIDTH) >> 3) & 3) {
+	case 1:
+		mmc->caps |= MMC_CAP_4_BIT_DATA;
+		break;
+	case 2:
+		mmc->caps |= MMC_CAP_4_BIT_DATA | MMC_CAP_8_BIT_DATA;
+		break;
+	default:
+		break;
+	}
+
+	writel(0, host->base + REG_INTERRUPT_MASK);
+
+	writel(CMD_SDC_RESET, host->base + REG_COMMAND);
+	for (i = 0; i < MAX_RETRIES; i++) {
+		if (!(readl(host->base + REG_COMMAND) & CMD_SDC_RESET))
+			break;
+		udelay(5);
+	}
+
+	ret = devm_request_irq(dev, irq, moxart_irq, 0, "moxart-mmc", host);
+	if (ret)
+		goto out;
+
+	dev_set_drvdata(dev, mmc);
+	mmc_add_host(mmc);
+
+	dev_dbg(dev, "IRQ=%d, FIFO is %d bytes\n", irq, host->fifo_width);
+
+	return 0;
+
+out:
+	if (mmc)
+		mmc_free_host(mmc);
+	return ret;
+}
+
+static int moxart_remove(struct platform_device *pdev)
+{
+	struct mmc_host *mmc = dev_get_drvdata(&pdev->dev);
+	struct moxart_host *host = mmc_priv(mmc);
+
+	dev_set_drvdata(&pdev->dev, NULL);
+
+	if (mmc) {
+		if (!IS_ERR(host->dma_chan_tx))
+			dma_release_channel(host->dma_chan_tx);
+		if (!IS_ERR(host->dma_chan_rx))
+			dma_release_channel(host->dma_chan_rx);
+		mmc_remove_host(mmc);
+		mmc_free_host(mmc);
+
+		writel(0, host->base + REG_INTERRUPT_MASK);
+		writel(0, host->base + REG_POWER_CONTROL);
+		writel(readl(host->base + REG_CLOCK_CONTROL) | CLK_OFF,
+		       host->base + REG_CLOCK_CONTROL);
+	}
+
+	kfree(host);
+
+	return 0;
+}
+
+static const struct of_device_id moxart_mmc_match[] = {
+	{ .compatible = "moxa,moxart-mmc" },
+	{ .compatible = "faraday,ftsdc010" },
+	{ }
+};
+
+static struct platform_driver moxart_mmc_driver = {
+	.probe      = moxart_probe,
+	.remove     = moxart_remove,
+	.driver     = {
+		.name		= "mmc-moxart",
+		.owner		= THIS_MODULE,
+		.of_match_table	= moxart_mmc_match,
+	},
+};
+module_platform_driver(moxart_mmc_driver);
+
+MODULE_ALIAS("platform:mmc-moxart");
+MODULE_DESCRIPTION("MOXA ART MMC driver");
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Jonas Jensen <jonas.jensen@gmail.com>");
diff --git a/drivers/mmc/host/mvsdio.c b/drivers/mmc/host/mvsdio.c
index 45aa2206741..9377284f854 100644
--- a/drivers/mmc/host/mvsdio.c
+++ b/drivers/mmc/host/mvsdio.c
@@ -354,6 +354,20 @@ static irqreturn_t mvsd_irq(int irq, void *dev)
 		intr_status, mvsd_read(MVSD_NOR_INTR_EN),
 		mvsd_read(MVSD_HW_STATE));
 
+	/*
+	 * It looks like, SDIO IP can issue one late, spurious irq
+	 * although all irqs should be disabled. To work around this,
+	 * bail out early, if we didn't expect any irqs to occur.
+	 */
+	if (!mvsd_read(MVSD_NOR_INTR_EN) && !mvsd_read(MVSD_ERR_INTR_EN)) {
+		dev_dbg(host->dev, "spurious irq detected intr 0x%04x intr_en 0x%04x erri 0x%04x erri_en 0x%04x\n",
+			mvsd_read(MVSD_NOR_INTR_STATUS),
+			mvsd_read(MVSD_NOR_INTR_EN),
+			mvsd_read(MVSD_ERR_INTR_STATUS),
+			mvsd_read(MVSD_ERR_INTR_EN));
+		return IRQ_HANDLED;
+	}
+
 	spin_lock(&host->lock);
 
 	/* PIO handling, if needed. Messy business... */
@@ -801,10 +815,10 @@ static int mvsd_probe(struct platform_device *pdev)
 		goto out;
 
 	if (!(mmc->caps & MMC_CAP_NEEDS_POLL))
-		dev_notice(&pdev->dev, "using GPIO for card detection\n");
+		dev_dbg(&pdev->dev, "using GPIO for card detection\n");
 	else
-		dev_notice(&pdev->dev,
-			   "lacking card detect (fall back to polling)\n");
+		dev_dbg(&pdev->dev, "lacking card detect (fall back to polling)\n");
+
 	return 0;
 
 out:
diff --git a/drivers/mmc/host/mxcmmc.c b/drivers/mmc/host/mxcmmc.c
index f7199c83f5c..ed1cb93c378 100644
--- a/drivers/mmc/host/mxcmmc.c
+++ b/drivers/mmc/host/mxcmmc.c
@@ -124,9 +124,8 @@ enum mxcmci_type {
 
 struct mxcmci_host {
 	struct mmc_host		*mmc;
-	struct resource		*res;
 	void __iomem		*base;
-	int			irq;
+	dma_addr_t		phys_base;
 	int			detect_irq;
 	struct dma_chan		*dma;
 	struct dma_async_tx_descriptor *desc;
@@ -154,8 +153,6 @@ struct mxcmci_host {
 	struct work_struct	datawork;
 	spinlock_t		lock;
 
-	struct regulator	*vcc;
-
 	int			burstlen;
 	int			dmareq;
 	struct dma_slave_config dma_slave_config;
@@ -241,37 +238,15 @@ static inline void mxcmci_writew(struct mxcmci_host *host, u16 val, int reg)
 
 static void mxcmci_set_clk_rate(struct mxcmci_host *host, unsigned int clk_ios);
 
-static inline void mxcmci_init_ocr(struct mxcmci_host *host)
-{
-	host->vcc = regulator_get(mmc_dev(host->mmc), "vmmc");
-
-	if (IS_ERR(host->vcc)) {
-		host->vcc = NULL;
-	} else {
-		host->mmc->ocr_avail = mmc_regulator_get_ocrmask(host->vcc);
-		if (host->pdata && host->pdata->ocr_avail)
-			dev_warn(mmc_dev(host->mmc),
-				"pdata->ocr_avail will not be used\n");
-	}
-
-	if (host->vcc == NULL) {
-		/* fall-back to platform data */
-		if (host->pdata && host->pdata->ocr_avail)
-			host->mmc->ocr_avail = host->pdata->ocr_avail;
-		else
-			host->mmc->ocr_avail = MMC_VDD_32_33 | MMC_VDD_33_34;
-	}
-}
-
-static inline void mxcmci_set_power(struct mxcmci_host *host,
-				    unsigned char power_mode,
-				    unsigned int vdd)
+static void mxcmci_set_power(struct mxcmci_host *host, unsigned int vdd)
 {
-	if (host->vcc) {
-		if (power_mode == MMC_POWER_UP)
-			mmc_regulator_set_ocr(host->mmc, host->vcc, vdd);
-		else if (power_mode == MMC_POWER_OFF)
-			mmc_regulator_set_ocr(host->mmc, host->vcc, 0);
+	if (!IS_ERR(host->mmc->supply.vmmc)) {
+		if (host->power_mode == MMC_POWER_UP)
+			mmc_regulator_set_ocr(host->mmc,
+					      host->mmc->supply.vmmc, vdd);
+		else if (host->power_mode == MMC_POWER_OFF)
+			mmc_regulator_set_ocr(host->mmc,
+					      host->mmc->supply.vmmc, 0);
 	}
 
 	if (host->pdata && host->pdata->setpower)
@@ -299,7 +274,6 @@ static void mxcmci_softreset(struct mxcmci_host *host)
 
 	mxcmci_writew(host, 0xff, MMC_REG_RES_TO);
 }
-static int mxcmci_setup_dma(struct mmc_host *mmc);
 
 #if IS_ENABLED(CONFIG_PPC_MPC512x)
 static inline void buffer_swap32(u32 *buf, int len)
@@ -868,8 +842,8 @@ static int mxcmci_setup_dma(struct mmc_host *mmc)
 	struct mxcmci_host *host = mmc_priv(mmc);
 	struct dma_slave_config *config = &host->dma_slave_config;
 
-	config->dst_addr = host->res->start + MMC_REG_BUFFER_ACCESS;
-	config->src_addr = host->res->start + MMC_REG_BUFFER_ACCESS;
+	config->dst_addr = host->phys_base + MMC_REG_BUFFER_ACCESS;
+	config->src_addr = host->phys_base + MMC_REG_BUFFER_ACCESS;
 	config->dst_addr_width = 4;
 	config->src_addr_width = 4;
 	config->dst_maxburst = host->burstlen;
@@ -911,8 +885,8 @@ static void mxcmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 		host->cmdat &= ~CMD_DAT_CONT_BUS_WIDTH_4;
 
 	if (host->power_mode != ios->power_mode) {
-		mxcmci_set_power(host, ios->power_mode, ios->vdd);
 		host->power_mode = ios->power_mode;
+		mxcmci_set_power(host, ios->vdd);
 
 		if (ios->power_mode == MMC_POWER_ON)
 			host->cmdat |= CMD_DAT_CONT_INIT;
@@ -1040,8 +1014,8 @@ static const struct mmc_host_ops mxcmci_ops = {
 static int mxcmci_probe(struct platform_device *pdev)
 {
 	struct mmc_host *mmc;
-	struct mxcmci_host *host = NULL;
-	struct resource *iores, *r;
+	struct mxcmci_host *host;
+	struct resource *res;
 	int ret = 0, irq;
 	bool dat3_card_detect = false;
 	dma_cap_mask_t mask;
@@ -1052,21 +1026,25 @@ static int mxcmci_probe(struct platform_device *pdev)
 
 	of_id = of_match_device(mxcmci_of_match, &pdev->dev);
 
-	iores = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	irq = platform_get_irq(pdev, 0);
-	if (!iores || irq < 0)
+	if (irq < 0)
 		return -EINVAL;
 
-	r = request_mem_region(iores->start, resource_size(iores), pdev->name);
-	if (!r)
-		return -EBUSY;
+	mmc = mmc_alloc_host(sizeof(*host), &pdev->dev);
+	if (!mmc)
+		return -ENOMEM;
+
+	host = mmc_priv(mmc);
 
-	mmc = mmc_alloc_host(sizeof(struct mxcmci_host), &pdev->dev);
-	if (!mmc) {
-		ret = -ENOMEM;
-		goto out_release_mem;
+	host->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(host->base)) {
+		ret = PTR_ERR(host->base);
+		goto out_free;
 	}
 
+	host->phys_base = res->start;
+
 	ret = mmc_of_parse(mmc);
 	if (ret)
 		goto out_free;
@@ -1084,13 +1062,6 @@ static int mxcmci_probe(struct platform_device *pdev)
 	mmc->max_req_size = mmc->max_blk_size * mmc->max_blk_count;
 	mmc->max_seg_size = mmc->max_req_size;
 
-	host = mmc_priv(mmc);
-	host->base = ioremap(r->start, resource_size(r));
-	if (!host->base) {
-		ret = -ENOMEM;
-		goto out_free;
-	}
-
 	if (of_id) {
 		const struct platform_device_id *id_entry = of_id->data;
 		host->devtype = id_entry->driver_data;
@@ -1112,7 +1083,14 @@ static int mxcmci_probe(struct platform_device *pdev)
 			&& !of_property_read_bool(pdev->dev.of_node, "cd-gpios"))
 		dat3_card_detect = true;
 
-	mxcmci_init_ocr(host);
+	ret = mmc_regulator_get_supply(mmc);
+	if (ret) {
+		if (pdata && ret != -EPROBE_DEFER)
+			mmc->ocr_avail = pdata->ocr_avail ? :
+				MMC_VDD_32_33 | MMC_VDD_33_34;
+		else
+			goto out_free;
+	}
 
 	if (dat3_card_detect)
 		host->default_irq_mask =
@@ -1120,19 +1098,16 @@ static int mxcmci_probe(struct platform_device *pdev)
 	else
 		host->default_irq_mask = 0;
 
-	host->res = r;
-	host->irq = irq;
-
 	host->clk_ipg = devm_clk_get(&pdev->dev, "ipg");
 	if (IS_ERR(host->clk_ipg)) {
 		ret = PTR_ERR(host->clk_ipg);
-		goto out_iounmap;
+		goto out_free;
 	}
 
 	host->clk_per = devm_clk_get(&pdev->dev, "per");
 	if (IS_ERR(host->clk_per)) {
 		ret = PTR_ERR(host->clk_per);
-		goto out_iounmap;
+		goto out_free;
 	}
 
 	clk_prepare_enable(host->clk_per);
@@ -1159,9 +1134,9 @@ static int mxcmci_probe(struct platform_device *pdev)
 	if (!host->pdata) {
 		host->dma = dma_request_slave_channel(&pdev->dev, "rx-tx");
 	} else {
-		r = platform_get_resource(pdev, IORESOURCE_DMA, 0);
-		if (r) {
-			host->dmareq = r->start;
+		res = platform_get_resource(pdev, IORESOURCE_DMA, 0);
+		if (res) {
+			host->dmareq = res->start;
 			host->dma_data.peripheral_type = IMX_DMATYPE_SDHC;
 			host->dma_data.priority = DMA_PRIO_LOW;
 			host->dma_data.dma_request = host->dmareq;
@@ -1178,7 +1153,8 @@ static int mxcmci_probe(struct platform_device *pdev)
 
 	INIT_WORK(&host->datawork, mxcmci_datawork);
 
-	ret = request_irq(host->irq, mxcmci_irq, 0, DRIVER_NAME, host);
+	ret = devm_request_irq(&pdev->dev, irq, mxcmci_irq, 0,
+			       dev_name(&pdev->dev), host);
 	if (ret)
 		goto out_free_dma;
 
@@ -1188,7 +1164,7 @@ static int mxcmci_probe(struct platform_device *pdev)
 		ret = host->pdata->init(&pdev->dev, mxcmci_detect_irq,
 				host->mmc);
 		if (ret)
-			goto out_free_irq;
+			goto out_free_dma;
 	}
 
 	init_timer(&host->watchdog);
@@ -1199,20 +1175,17 @@ static int mxcmci_probe(struct platform_device *pdev)
 
 	return 0;
 
-out_free_irq:
-	free_irq(host->irq, host);
 out_free_dma:
 	if (host->dma)
 		dma_release_channel(host->dma);
+
 out_clk_put:
 	clk_disable_unprepare(host->clk_per);
 	clk_disable_unprepare(host->clk_ipg);
-out_iounmap:
-	iounmap(host->base);
+
 out_free:
 	mmc_free_host(mmc);
-out_release_mem:
-	release_mem_region(iores->start, resource_size(iores));
+
 	return ret;
 }
 
@@ -1223,30 +1196,21 @@ static int mxcmci_remove(struct platform_device *pdev)
 
 	mmc_remove_host(mmc);
 
-	if (host->vcc)
-		regulator_put(host->vcc);
-
 	if (host->pdata && host->pdata->exit)
 		host->pdata->exit(&pdev->dev, mmc);
 
-	free_irq(host->irq, host);
-	iounmap(host->base);
-
 	if (host->dma)
 		dma_release_channel(host->dma);
 
 	clk_disable_unprepare(host->clk_per);
 	clk_disable_unprepare(host->clk_ipg);
 
-	release_mem_region(host->res->start, resource_size(host->res));
-
 	mmc_free_host(mmc);
 
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int mxcmci_suspend(struct device *dev)
+static int __maybe_unused mxcmci_suspend(struct device *dev)
 {
 	struct mmc_host *mmc = dev_get_drvdata(dev);
 	struct mxcmci_host *host = mmc_priv(mmc);
@@ -1256,7 +1220,7 @@ static int mxcmci_suspend(struct device *dev)
 	return 0;
 }
 
-static int mxcmci_resume(struct device *dev)
+static int __maybe_unused mxcmci_resume(struct device *dev)
 {
 	struct mmc_host *mmc = dev_get_drvdata(dev);
 	struct mxcmci_host *host = mmc_priv(mmc);
@@ -1266,11 +1230,7 @@ static int mxcmci_resume(struct device *dev)
 	return 0;
 }
 
-static const struct dev_pm_ops mxcmci_pm_ops = {
-	.suspend	= mxcmci_suspend,
-	.resume		= mxcmci_resume,
-};
-#endif
+static SIMPLE_DEV_PM_OPS(mxcmci_pm_ops, mxcmci_suspend, mxcmci_resume);
 
 static struct platform_driver mxcmci_driver = {
 	.probe		= mxcmci_probe,
@@ -1279,9 +1239,7 @@ static struct platform_driver mxcmci_driver = {
 	.driver		= {
 		.name		= DRIVER_NAME,
 		.owner		= THIS_MODULE,
-#ifdef CONFIG_PM
 		.pm	= &mxcmci_pm_ops,
-#endif
 		.of_match_table	= mxcmci_of_match,
 	}
 };
diff --git a/drivers/mmc/host/mxs-mmc.c b/drivers/mmc/host/mxs-mmc.c
index 073e871a0fc..babfea03ba8 100644
--- a/drivers/mmc/host/mxs-mmc.c
+++ b/drivers/mmc/host/mxs-mmc.c
@@ -70,6 +70,7 @@ struct mxs_mmc_host {
 	unsigned char			bus_width;
 	spinlock_t			lock;
 	int				sdio_irq_en;
+	bool				broken_cd;
 };
 
 static int mxs_mmc_get_cd(struct mmc_host *mmc)
@@ -78,6 +79,9 @@ static int mxs_mmc_get_cd(struct mmc_host *mmc)
 	struct mxs_ssp *ssp = &host->ssp;
 	int present, ret;
 
+	if (host->broken_cd)
+		return -ENOSYS;
+
 	ret = mmc_gpio_get_cd(mmc);
 	if (ret >= 0)
 		return ret;
@@ -568,6 +572,7 @@ static int mxs_mmc_probe(struct platform_device *pdev)
 {
 	const struct of_device_id *of_id =
 			of_match_device(mxs_mmc_dt_ids, &pdev->dev);
+	struct device_node *np = pdev->dev.of_node;
 	struct mxs_mmc_host *host;
 	struct mmc_host *mmc;
 	struct resource *iores;
@@ -634,6 +639,8 @@ static int mxs_mmc_probe(struct platform_device *pdev)
 	mmc->caps = MMC_CAP_SD_HIGHSPEED | MMC_CAP_MMC_HIGHSPEED |
 		    MMC_CAP_SDIO_IRQ | MMC_CAP_NEEDS_POLL;
 
+	host->broken_cd = of_property_read_bool(np, "broken-cd");
+
 	mmc->f_min = 400000;
 	mmc->f_max = 288000000;
 
diff --git a/drivers/mmc/host/omap.c b/drivers/mmc/host/omap.c
index 5c2e58b2930..81974ecdfcb 100644
--- a/drivers/mmc/host/omap.c
+++ b/drivers/mmc/host/omap.c
@@ -177,7 +177,7 @@ static void mmc_omap_fclk_offdelay(struct mmc_omap_slot *slot)
 	unsigned long tick_ns;
 
 	if (slot != NULL && slot->host->fclk_enabled && slot->fclk_freq > 0) {
-		tick_ns = (1000000000 + slot->fclk_freq - 1) / slot->fclk_freq;
+		tick_ns = DIV_ROUND_UP(NSEC_PER_SEC, slot->fclk_freq);
 		ndelay(8 * tick_ns);
 	}
 }
@@ -435,7 +435,7 @@ static void mmc_omap_send_stop_work(struct work_struct *work)
 	struct mmc_data *data = host->stop_data;
 	unsigned long tick_ns;
 
-	tick_ns = (1000000000 + slot->fclk_freq - 1)/slot->fclk_freq;
+	tick_ns = DIV_ROUND_UP(NSEC_PER_SEC, slot->fclk_freq);
 	ndelay(8*tick_ns);
 
 	mmc_omap_start_command(host, data->stop);
@@ -477,7 +477,7 @@ mmc_omap_send_abort(struct mmc_omap_host *host, int maxloops)
 	u16 stat = 0;
 
 	/* Sending abort takes 80 clocks. Have some extra and round up */
-	timeout = (120*1000000 + slot->fclk_freq - 1)/slot->fclk_freq;
+	timeout = DIV_ROUND_UP(120 * USEC_PER_SEC, slot->fclk_freq);
 	restarts = 0;
 	while (restarts < maxloops) {
 		OMAP_MMC_WRITE(host, STAT, 0xFFFF);
@@ -677,8 +677,8 @@ mmc_omap_xfer_data(struct mmc_omap_host *host, int write)
 	if (n > host->buffer_bytes_left)
 		n = host->buffer_bytes_left;
 
-	nwords = n / 2;
-	nwords += n & 1; /* handle odd number of bytes to transfer */
+	/* Round up to handle odd number of bytes to transfer */
+	nwords = DIV_ROUND_UP(n, 2);
 
 	host->buffer_bytes_left -= n;
 	host->total_bytes_left -= n;
diff --git a/drivers/mmc/host/omap_hsmmc.c b/drivers/mmc/host/omap_hsmmc.c
index e91ee21549d..6b7b7558592 100644
--- a/drivers/mmc/host/omap_hsmmc.c
+++ b/drivers/mmc/host/omap_hsmmc.c
@@ -31,7 +31,7 @@
 #include <linux/of.h>
 #include <linux/of_gpio.h>
 #include <linux/of_device.h>
-#include <linux/omap-dma.h>
+#include <linux/omap-dmaengine.h>
 #include <linux/mmc/host.h>
 #include <linux/mmc/core.h>
 #include <linux/mmc/mmc.h>
@@ -582,7 +582,7 @@ static void omap_hsmmc_set_clock(struct omap_hsmmc_host *host)
 	 *	- MMC/SD clock coming out of controller > 25MHz
 	 */
 	if ((mmc_slot(host).features & HSMMC_HAS_HSPE_SUPPORT) &&
-	    (ios->timing != MMC_TIMING_UHS_DDR50) &&
+	    (ios->timing != MMC_TIMING_MMC_DDR52) &&
 	    ((OMAP_HSMMC_READ(host->base, CAPA) & HSS) == HSS)) {
 		regval = OMAP_HSMMC_READ(host->base, HCTL);
 		if (clkdiv && (clk_get_rate(host->fclk)/clkdiv) > 25000000)
@@ -602,7 +602,7 @@ static void omap_hsmmc_set_bus_width(struct omap_hsmmc_host *host)
 	u32 con;
 
 	con = OMAP_HSMMC_READ(host->base, CON);
-	if (ios->timing == MMC_TIMING_UHS_DDR50)
+	if (ios->timing == MMC_TIMING_MMC_DDR52)
 		con |= DDR;	/* configure in DDR mode */
 	else
 		con &= ~DDR;
@@ -920,16 +920,17 @@ omap_hsmmc_xfer_done(struct omap_hsmmc_host *host, struct mmc_data *data)
 static void
 omap_hsmmc_cmd_done(struct omap_hsmmc_host *host, struct mmc_command *cmd)
 {
-	host->cmd = NULL;
-
 	if (host->mrq->sbc && (host->cmd == host->mrq->sbc) &&
 	    !host->mrq->sbc->error && !(host->flags & AUTO_CMD23)) {
+		host->cmd = NULL;
 		omap_hsmmc_start_dma_transfer(host);
 		omap_hsmmc_start_command(host, host->mrq->cmd,
 						host->mrq->data);
 		return;
 	}
 
+	host->cmd = NULL;
+
 	if (cmd->flags & MMC_RSP_PRESENT) {
 		if (cmd->flags & MMC_RSP_136) {
 			/* response type 2 */
@@ -1851,6 +1852,7 @@ static int omap_hsmmc_probe(struct platform_device *pdev)
 	unsigned tx_req, rx_req;
 	struct pinctrl *pinctrl;
 	const struct omap_mmc_of_data *data;
+	void __iomem *base;
 
 	match = of_match_device(of_match_ptr(omap_mmc_of_match), &pdev->dev);
 	if (match) {
@@ -1881,9 +1883,9 @@ static int omap_hsmmc_probe(struct platform_device *pdev)
 	if (res == NULL || irq < 0)
 		return -ENXIO;
 
-	res = request_mem_region(res->start, resource_size(res), pdev->name);
-	if (res == NULL)
-		return -EBUSY;
+	base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
 
 	ret = omap_hsmmc_gpio_init(pdata);
 	if (ret)
@@ -1904,7 +1906,7 @@ static int omap_hsmmc_probe(struct platform_device *pdev)
 	host->irq	= irq;
 	host->slot_id	= 0;
 	host->mapbase	= res->start + pdata->reg_offset;
-	host->base	= ioremap(host->mapbase, SZ_4K);
+	host->base	= base + pdata->reg_offset;
 	host->power_mode = MMC_POWER_OFF;
 	host->next_data.cookie = 1;
 	host->pbias_enabled = 0;
@@ -1922,7 +1924,7 @@ static int omap_hsmmc_probe(struct platform_device *pdev)
 
 	spin_lock_init(&host->irq_lock);
 
-	host->fclk = clk_get(&pdev->dev, "fck");
+	host->fclk = devm_clk_get(&pdev->dev, "fck");
 	if (IS_ERR(host->fclk)) {
 		ret = PTR_ERR(host->fclk);
 		host->fclk = NULL;
@@ -1941,7 +1943,7 @@ static int omap_hsmmc_probe(struct platform_device *pdev)
 
 	omap_hsmmc_context_save(host);
 
-	host->dbclk = clk_get(&pdev->dev, "mmchsdb_fck");
+	host->dbclk = devm_clk_get(&pdev->dev, "mmchsdb_fck");
 	/*
 	 * MMC can still work without debounce clock.
 	 */
@@ -1949,7 +1951,6 @@ static int omap_hsmmc_probe(struct platform_device *pdev)
 		host->dbclk = NULL;
 	} else if (clk_prepare_enable(host->dbclk) != 0) {
 		dev_warn(mmc_dev(host->mmc), "Failed to enable debounce clk\n");
-		clk_put(host->dbclk);
 		host->dbclk = NULL;
 	}
 
@@ -2018,7 +2019,7 @@ static int omap_hsmmc_probe(struct platform_device *pdev)
 	}
 
 	/* Request IRQ for MMC operations */
-	ret = request_irq(host->irq, omap_hsmmc_irq, 0,
+	ret = devm_request_irq(&pdev->dev, host->irq, omap_hsmmc_irq, 0,
 			mmc_hostname(mmc), host);
 	if (ret) {
 		dev_err(mmc_dev(host->mmc), "Unable to grab HSMMC IRQ\n");
@@ -2029,7 +2030,7 @@ static int omap_hsmmc_probe(struct platform_device *pdev)
 		if (pdata->init(&pdev->dev) != 0) {
 			dev_err(mmc_dev(host->mmc),
 				"Unable to configure MMC IRQs\n");
-			goto err_irq_cd_init;
+			goto err_irq;
 		}
 	}
 
@@ -2044,9 +2045,9 @@ static int omap_hsmmc_probe(struct platform_device *pdev)
 
 	/* Request IRQ for card detect */
 	if ((mmc_slot(host).card_detect_irq)) {
-		ret = request_threaded_irq(mmc_slot(host).card_detect_irq,
-					   NULL,
-					   omap_hsmmc_detect,
+		ret = devm_request_threaded_irq(&pdev->dev,
+						mmc_slot(host).card_detect_irq,
+						NULL, omap_hsmmc_detect,
 					   IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
 					   mmc_hostname(mmc), host);
 		if (ret) {
@@ -2089,15 +2090,12 @@ static int omap_hsmmc_probe(struct platform_device *pdev)
 
 err_slot_name:
 	mmc_remove_host(mmc);
-	free_irq(mmc_slot(host).card_detect_irq, host);
 err_irq_cd:
 	if (host->use_reg)
 		omap_hsmmc_reg_put(host);
 err_reg:
 	if (host->pdata->cleanup)
 		host->pdata->cleanup(&pdev->dev);
-err_irq_cd_init:
-	free_irq(host->irq, host);
 err_irq:
 	if (host->tx_chan)
 		dma_release_channel(host->tx_chan);
@@ -2105,27 +2103,19 @@ err_irq:
 		dma_release_channel(host->rx_chan);
 	pm_runtime_put_sync(host->dev);
 	pm_runtime_disable(host->dev);
-	clk_put(host->fclk);
-	if (host->dbclk) {
+	if (host->dbclk)
 		clk_disable_unprepare(host->dbclk);
-		clk_put(host->dbclk);
-	}
 err1:
-	iounmap(host->base);
 	mmc_free_host(mmc);
 err_alloc:
 	omap_hsmmc_gpio_free(pdata);
 err:
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (res)
-		release_mem_region(res->start, resource_size(res));
 	return ret;
 }
 
 static int omap_hsmmc_remove(struct platform_device *pdev)
 {
 	struct omap_hsmmc_host *host = platform_get_drvdata(pdev);
-	struct resource *res;
 
 	pm_runtime_get_sync(host->dev);
 	mmc_remove_host(host->mmc);
@@ -2133,9 +2123,6 @@ static int omap_hsmmc_remove(struct platform_device *pdev)
 		omap_hsmmc_reg_put(host);
 	if (host->pdata->cleanup)
 		host->pdata->cleanup(&pdev->dev);
-	free_irq(host->irq, host);
-	if (mmc_slot(host).card_detect_irq)
-		free_irq(mmc_slot(host).card_detect_irq, host);
 
 	if (host->tx_chan)
 		dma_release_channel(host->tx_chan);
@@ -2144,20 +2131,12 @@ static int omap_hsmmc_remove(struct platform_device *pdev)
 
 	pm_runtime_put_sync(host->dev);
 	pm_runtime_disable(host->dev);
-	clk_put(host->fclk);
-	if (host->dbclk) {
+	if (host->dbclk)
 		clk_disable_unprepare(host->dbclk);
-		clk_put(host->dbclk);
-	}
 
 	omap_hsmmc_gpio_free(host->pdata);
-	iounmap(host->base);
 	mmc_free_host(host->mmc);
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (res)
-		release_mem_region(res->start, resource_size(res));
-
 	return 0;
 }
 
diff --git a/drivers/mmc/host/rtsx_pci_sdmmc.c b/drivers/mmc/host/rtsx_pci_sdmmc.c
index 0b9ded13a3a..0d519649b57 100644
--- a/drivers/mmc/host/rtsx_pci_sdmmc.c
+++ b/drivers/mmc/host/rtsx_pci_sdmmc.c
@@ -236,6 +236,9 @@ static void sd_send_cmd_get_rsp(struct realtek_pci_sdmmc *host,
 	case MMC_RSP_R1:
 		rsp_type = SD_RSP_TYPE_R1;
 		break;
+	case MMC_RSP_R1 & ~MMC_RSP_CRC:
+		rsp_type = SD_RSP_TYPE_R1 | SD_NO_CHECK_CRC7;
+		break;
 	case MMC_RSP_R1B:
 		rsp_type = SD_RSP_TYPE_R1b;
 		break;
@@ -816,6 +819,7 @@ static int sd_set_timing(struct realtek_pci_sdmmc *host, unsigned char timing)
 		rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, CLK_CTL, CLK_LOW_FREQ, 0);
 		break;
 
+	case MMC_TIMING_MMC_DDR52:
 	case MMC_TIMING_UHS_DDR50:
 		rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, SD_CFG1,
 				0x0C | SD_ASYNC_FIFO_NOT_RST,
@@ -896,6 +900,7 @@ static void sdmmc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 		host->vpclk = true;
 		host->double_clk = false;
 		break;
+	case MMC_TIMING_MMC_DDR52:
 	case MMC_TIMING_UHS_DDR50:
 	case MMC_TIMING_UHS_SDR25:
 		host->ssc_depth = RTSX_SSC_DEPTH_1M;
diff --git a/drivers/mmc/host/rtsx_usb_sdmmc.c b/drivers/mmc/host/rtsx_usb_sdmmc.c
new file mode 100644
index 00000000000..5d3766e792f
--- /dev/null
+++ b/drivers/mmc/host/rtsx_usb_sdmmc.c
@@ -0,0 +1,1456 @@
+/* Realtek USB SD/MMC Card Interface driver
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2
+ * as 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/>.
+ *
+ * Author:
+ *   Roger Tseng <rogerable@realtek.com>
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/usb.h>
+#include <linux/mmc/host.h>
+#include <linux/mmc/mmc.h>
+#include <linux/mmc/sd.h>
+#include <linux/mmc/sdio.h>
+#include <linux/mmc/card.h>
+#include <linux/scatterlist.h>
+#include <linux/pm_runtime.h>
+
+#include <linux/mfd/rtsx_usb.h>
+#include <asm/unaligned.h>
+
+#if defined(CONFIG_LEDS_CLASS) || (defined(CONFIG_LEDS_CLASS_MODULE) && \
+		defined(CONFIG_MMC_REALTEK_USB_MODULE))
+#include <linux/leds.h>
+#include <linux/workqueue.h>
+#define RTSX_USB_USE_LEDS_CLASS
+#endif
+
+struct rtsx_usb_sdmmc {
+	struct platform_device	*pdev;
+	struct rtsx_ucr	*ucr;
+	struct mmc_host		*mmc;
+	struct mmc_request	*mrq;
+
+	struct mutex		host_mutex;
+
+	u8			ssc_depth;
+	unsigned int		clock;
+	bool			vpclk;
+	bool			double_clk;
+	bool			host_removal;
+	bool			card_exist;
+	bool			initial_mode;
+	bool			ddr_mode;
+
+	unsigned char		power_mode;
+
+#ifdef RTSX_USB_USE_LEDS_CLASS
+	struct led_classdev	led;
+	char			led_name[32];
+	struct work_struct	led_work;
+#endif
+};
+
+static inline struct device *sdmmc_dev(struct rtsx_usb_sdmmc *host)
+{
+	return &(host->pdev->dev);
+}
+
+static inline void sd_clear_error(struct rtsx_usb_sdmmc *host)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	rtsx_usb_ep0_write_register(ucr, CARD_STOP,
+				  SD_STOP | SD_CLR_ERR,
+				  SD_STOP | SD_CLR_ERR);
+
+	rtsx_usb_clear_dma_err(ucr);
+	rtsx_usb_clear_fsm_err(ucr);
+}
+
+#ifdef DEBUG
+static void sd_print_debug_regs(struct rtsx_usb_sdmmc *host)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	u8 val = 0;
+
+	rtsx_usb_ep0_read_register(ucr, SD_STAT1, &val);
+	dev_dbg(sdmmc_dev(host), "SD_STAT1: 0x%x\n", val);
+	rtsx_usb_ep0_read_register(ucr, SD_STAT2, &val);
+	dev_dbg(sdmmc_dev(host), "SD_STAT2: 0x%x\n", val);
+	rtsx_usb_ep0_read_register(ucr, SD_BUS_STAT, &val);
+	dev_dbg(sdmmc_dev(host), "SD_BUS_STAT: 0x%x\n", val);
+}
+#else
+#define sd_print_debug_regs(host)
+#endif /* DEBUG */
+
+static int sd_read_data(struct rtsx_usb_sdmmc *host, struct mmc_command *cmd,
+	       u16 byte_cnt, u8 *buf, int buf_len, int timeout)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	int err;
+	u8 trans_mode;
+
+	if (!buf)
+		buf_len = 0;
+
+	rtsx_usb_init_cmd(ucr);
+	if (cmd != NULL) {
+		dev_dbg(sdmmc_dev(host), "%s: SD/MMC CMD%d\n", __func__
+				, cmd->opcode);
+		if (cmd->opcode == MMC_SEND_TUNING_BLOCK)
+			trans_mode = SD_TM_AUTO_TUNING;
+		else
+			trans_mode = SD_TM_NORMAL_READ;
+
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD,
+				SD_CMD0, 0xFF, (u8)(cmd->opcode) | 0x40);
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD,
+				SD_CMD1, 0xFF, (u8)(cmd->arg >> 24));
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD,
+				SD_CMD2, 0xFF, (u8)(cmd->arg >> 16));
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD,
+				SD_CMD3, 0xFF, (u8)(cmd->arg >> 8));
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD,
+				SD_CMD4, 0xFF, (u8)cmd->arg);
+	} else {
+		trans_mode = SD_TM_AUTO_READ_3;
+	}
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, (u8)byte_cnt);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_BYTE_CNT_H,
+			0xFF, (u8)(byte_cnt >> 8));
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_BLOCK_CNT_L, 0xFF, 1);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_BLOCK_CNT_H, 0xFF, 0);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_CFG2, 0xFF,
+			SD_CALCULATE_CRC7 | SD_CHECK_CRC16 |
+			SD_NO_WAIT_BUSY_END | SD_CHECK_CRC7 | SD_RSP_LEN_6);
+	if (trans_mode != SD_TM_AUTO_TUNING)
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD,
+				CARD_DATA_SOURCE, 0x01, PINGPONG_BUFFER);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_TRANSFER,
+			0xFF, trans_mode | SD_TRANSFER_START);
+	rtsx_usb_add_cmd(ucr, CHECK_REG_CMD, SD_TRANSFER,
+			SD_TRANSFER_END, SD_TRANSFER_END);
+
+	if (cmd != NULL) {
+		rtsx_usb_add_cmd(ucr, READ_REG_CMD, SD_CMD1, 0, 0);
+		rtsx_usb_add_cmd(ucr, READ_REG_CMD, SD_CMD2, 0, 0);
+		rtsx_usb_add_cmd(ucr, READ_REG_CMD, SD_CMD3, 0, 0);
+		rtsx_usb_add_cmd(ucr, READ_REG_CMD, SD_CMD4, 0, 0);
+	}
+
+	err = rtsx_usb_send_cmd(ucr, MODE_CR, timeout);
+	if (err) {
+		dev_dbg(sdmmc_dev(host),
+			"rtsx_usb_send_cmd failed (err = %d)\n", err);
+		return err;
+	}
+
+	err = rtsx_usb_get_rsp(ucr, !cmd ? 1 : 5, timeout);
+	if (err || (ucr->rsp_buf[0] & SD_TRANSFER_ERR)) {
+		sd_print_debug_regs(host);
+
+		if (!err) {
+			dev_dbg(sdmmc_dev(host),
+				"Transfer failed (SD_TRANSFER = %02x)\n",
+				ucr->rsp_buf[0]);
+			err = -EIO;
+		} else {
+			dev_dbg(sdmmc_dev(host),
+				"rtsx_usb_get_rsp failed (err = %d)\n", err);
+		}
+
+		return err;
+	}
+
+	if (cmd != NULL) {
+		cmd->resp[0] = get_unaligned_be32(ucr->rsp_buf + 1);
+		dev_dbg(sdmmc_dev(host), "cmd->resp[0] = 0x%08x\n",
+				cmd->resp[0]);
+	}
+
+	if (buf && buf_len) {
+		/* 2-byte aligned part */
+		err = rtsx_usb_read_ppbuf(ucr, buf, byte_cnt - (byte_cnt % 2));
+		if (err) {
+			dev_dbg(sdmmc_dev(host),
+				"rtsx_usb_read_ppbuf failed (err = %d)\n", err);
+			return err;
+		}
+
+		/* unaligned byte */
+		if (byte_cnt % 2)
+			return rtsx_usb_read_register(ucr,
+					PPBUF_BASE2 + byte_cnt,
+					buf + byte_cnt - 1);
+	}
+
+	return 0;
+}
+
+static int sd_write_data(struct rtsx_usb_sdmmc *host, struct mmc_command *cmd,
+		u16 byte_cnt, u8 *buf, int buf_len, int timeout)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	int err;
+	u8 trans_mode;
+
+	if (!buf)
+		buf_len = 0;
+
+	if (buf && buf_len) {
+		err = rtsx_usb_write_ppbuf(ucr, buf, buf_len);
+		if (err) {
+			dev_dbg(sdmmc_dev(host),
+				"rtsx_usb_write_ppbuf failed (err = %d)\n",
+				err);
+			return err;
+		}
+	}
+
+	trans_mode = (cmd != NULL) ? SD_TM_AUTO_WRITE_2 : SD_TM_AUTO_WRITE_3;
+	rtsx_usb_init_cmd(ucr);
+
+	if (cmd != NULL) {
+		dev_dbg(sdmmc_dev(host), "%s: SD/MMC CMD%d\n", __func__,
+				cmd->opcode);
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD,
+				SD_CMD0, 0xFF, (u8)(cmd->opcode) | 0x40);
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD,
+				SD_CMD1, 0xFF, (u8)(cmd->arg >> 24));
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD,
+				SD_CMD2, 0xFF, (u8)(cmd->arg >> 16));
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD,
+				SD_CMD3, 0xFF, (u8)(cmd->arg >> 8));
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD,
+				SD_CMD4, 0xFF, (u8)cmd->arg);
+	}
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, (u8)byte_cnt);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_BYTE_CNT_H,
+			0xFF, (u8)(byte_cnt >> 8));
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_BLOCK_CNT_L, 0xFF, 1);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_BLOCK_CNT_H, 0xFF, 0);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_CFG2, 0xFF,
+		SD_CALCULATE_CRC7 | SD_CHECK_CRC16 |
+		SD_NO_WAIT_BUSY_END | SD_CHECK_CRC7 | SD_RSP_LEN_6);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD,
+			CARD_DATA_SOURCE, 0x01, PINGPONG_BUFFER);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
+			trans_mode | SD_TRANSFER_START);
+	rtsx_usb_add_cmd(ucr, CHECK_REG_CMD, SD_TRANSFER,
+			SD_TRANSFER_END, SD_TRANSFER_END);
+
+	if (cmd != NULL) {
+		rtsx_usb_add_cmd(ucr, READ_REG_CMD, SD_CMD1, 0, 0);
+		rtsx_usb_add_cmd(ucr, READ_REG_CMD, SD_CMD2, 0, 0);
+		rtsx_usb_add_cmd(ucr, READ_REG_CMD, SD_CMD3, 0, 0);
+		rtsx_usb_add_cmd(ucr, READ_REG_CMD, SD_CMD4, 0, 0);
+	}
+
+	err = rtsx_usb_send_cmd(ucr, MODE_CR, timeout);
+	if (err) {
+		dev_dbg(sdmmc_dev(host),
+			"rtsx_usb_send_cmd failed (err = %d)\n", err);
+		return err;
+	}
+
+	err = rtsx_usb_get_rsp(ucr, !cmd ? 1 : 5, timeout);
+	if (err) {
+		sd_print_debug_regs(host);
+		dev_dbg(sdmmc_dev(host),
+			"rtsx_usb_get_rsp failed (err = %d)\n", err);
+		return err;
+	}
+
+	if (cmd != NULL) {
+		cmd->resp[0] = get_unaligned_be32(ucr->rsp_buf + 1);
+		dev_dbg(sdmmc_dev(host), "cmd->resp[0] = 0x%08x\n",
+				cmd->resp[0]);
+	}
+
+	return 0;
+}
+
+static void sd_send_cmd_get_rsp(struct rtsx_usb_sdmmc *host,
+		struct mmc_command *cmd)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	u8 cmd_idx = (u8)cmd->opcode;
+	u32 arg = cmd->arg;
+	int err = 0;
+	int timeout = 100;
+	int i;
+	u8 *ptr;
+	int stat_idx = 0;
+	int len = 2;
+	u8 rsp_type;
+
+	dev_dbg(sdmmc_dev(host), "%s: SD/MMC CMD %d, arg = 0x%08x\n",
+			__func__, cmd_idx, arg);
+
+	/* Response type:
+	 * R0
+	 * R1, R5, R6, R7
+	 * R1b
+	 * R2
+	 * R3, R4
+	 */
+	switch (mmc_resp_type(cmd)) {
+	case MMC_RSP_NONE:
+		rsp_type = SD_RSP_TYPE_R0;
+		break;
+	case MMC_RSP_R1:
+		rsp_type = SD_RSP_TYPE_R1;
+		break;
+	case MMC_RSP_R1 & ~MMC_RSP_CRC:
+		rsp_type = SD_RSP_TYPE_R1 | SD_NO_CHECK_CRC7;
+		break;
+	case MMC_RSP_R1B:
+		rsp_type = SD_RSP_TYPE_R1b;
+		break;
+	case MMC_RSP_R2:
+		rsp_type = SD_RSP_TYPE_R2;
+		break;
+	case MMC_RSP_R3:
+		rsp_type = SD_RSP_TYPE_R3;
+		break;
+	default:
+		dev_dbg(sdmmc_dev(host), "cmd->flag is not valid\n");
+		err = -EINVAL;
+		goto out;
+	}
+
+	if (rsp_type == SD_RSP_TYPE_R1b)
+		timeout = 3000;
+
+	if (cmd->opcode == SD_SWITCH_VOLTAGE) {
+		err = rtsx_usb_write_register(ucr, SD_BUS_STAT,
+				SD_CLK_TOGGLE_EN | SD_CLK_FORCE_STOP,
+				SD_CLK_TOGGLE_EN);
+		if (err)
+			goto out;
+	}
+
+	rtsx_usb_init_cmd(ucr);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_CMD0, 0xFF, 0x40 | cmd_idx);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_CMD1, 0xFF, (u8)(arg >> 24));
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_CMD2, 0xFF, (u8)(arg >> 16));
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_CMD3, 0xFF, (u8)(arg >> 8));
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_CMD4, 0xFF, (u8)arg);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_CFG2, 0xFF, rsp_type);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_DATA_SOURCE,
+			0x01, PINGPONG_BUFFER);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_TRANSFER,
+			0xFF, SD_TM_CMD_RSP | SD_TRANSFER_START);
+	rtsx_usb_add_cmd(ucr, CHECK_REG_CMD, SD_TRANSFER,
+		     SD_TRANSFER_END | SD_STAT_IDLE,
+		     SD_TRANSFER_END | SD_STAT_IDLE);
+
+	if (rsp_type == SD_RSP_TYPE_R2) {
+		/* Read data from ping-pong buffer */
+		for (i = PPBUF_BASE2; i < PPBUF_BASE2 + 16; i++)
+			rtsx_usb_add_cmd(ucr, READ_REG_CMD, (u16)i, 0, 0);
+		stat_idx = 16;
+	} else if (rsp_type != SD_RSP_TYPE_R0) {
+		/* Read data from SD_CMDx registers */
+		for (i = SD_CMD0; i <= SD_CMD4; i++)
+			rtsx_usb_add_cmd(ucr, READ_REG_CMD, (u16)i, 0, 0);
+		stat_idx = 5;
+	}
+	len += stat_idx;
+
+	rtsx_usb_add_cmd(ucr, READ_REG_CMD, SD_STAT1, 0, 0);
+
+	err = rtsx_usb_send_cmd(ucr, MODE_CR, 100);
+	if (err) {
+		dev_dbg(sdmmc_dev(host),
+			"rtsx_usb_send_cmd error (err = %d)\n", err);
+		goto out;
+	}
+
+	err = rtsx_usb_get_rsp(ucr, len, timeout);
+	if (err || (ucr->rsp_buf[0] & SD_TRANSFER_ERR)) {
+		sd_print_debug_regs(host);
+		sd_clear_error(host);
+
+		if (!err) {
+			dev_dbg(sdmmc_dev(host),
+				"Transfer failed (SD_TRANSFER = %02x)\n",
+					ucr->rsp_buf[0]);
+			err = -EIO;
+		} else {
+			dev_dbg(sdmmc_dev(host),
+				"rtsx_usb_get_rsp failed (err = %d)\n", err);
+		}
+
+		goto out;
+	}
+
+	if (rsp_type == SD_RSP_TYPE_R0) {
+		err = 0;
+		goto out;
+	}
+
+	/* Skip result of CHECK_REG_CMD */
+	ptr = ucr->rsp_buf + 1;
+
+	/* Check (Start,Transmission) bit of Response */
+	if ((ptr[0] & 0xC0) != 0) {
+		err = -EILSEQ;
+		dev_dbg(sdmmc_dev(host), "Invalid response bit\n");
+		goto out;
+	}
+
+	/* Check CRC7 */
+	if (!(rsp_type & SD_NO_CHECK_CRC7)) {
+		if (ptr[stat_idx] & SD_CRC7_ERR) {
+			err = -EILSEQ;
+			dev_dbg(sdmmc_dev(host), "CRC7 error\n");
+			goto out;
+		}
+	}
+
+	if (rsp_type == SD_RSP_TYPE_R2) {
+		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",
+					i, cmd->resp[i]);
+		}
+	} else {
+		cmd->resp[0] = get_unaligned_be32(ptr + 1);
+		dev_dbg(sdmmc_dev(host), "cmd->resp[0] = 0x%08x\n",
+				cmd->resp[0]);
+	}
+
+out:
+	cmd->error = err;
+}
+
+static int sd_rw_multi(struct rtsx_usb_sdmmc *host, struct mmc_request *mrq)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	struct mmc_data *data = mrq->data;
+	int read = (data->flags & MMC_DATA_READ) ? 1 : 0;
+	u8 cfg2, trans_mode;
+	int err;
+	u8 flag;
+	size_t data_len = data->blksz * data->blocks;
+	unsigned int pipe;
+
+	if (read) {
+		dev_dbg(sdmmc_dev(host), "%s: read %zu bytes\n",
+				__func__, data_len);
+		cfg2 = SD_CALCULATE_CRC7 | SD_CHECK_CRC16 |
+			SD_NO_WAIT_BUSY_END | SD_CHECK_CRC7 | SD_RSP_LEN_0;
+		trans_mode = SD_TM_AUTO_READ_3;
+	} else {
+		dev_dbg(sdmmc_dev(host), "%s: write %zu bytes\n",
+				__func__, data_len);
+		cfg2 = SD_NO_CALCULATE_CRC7 | SD_CHECK_CRC16 |
+			SD_NO_WAIT_BUSY_END | SD_NO_CHECK_CRC7 | SD_RSP_LEN_0;
+		trans_mode = SD_TM_AUTO_WRITE_3;
+	}
+
+	rtsx_usb_init_cmd(ucr);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, 0x00);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_BYTE_CNT_H, 0xFF, 0x02);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_BLOCK_CNT_L,
+			0xFF, (u8)data->blocks);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_BLOCK_CNT_H,
+			0xFF, (u8)(data->blocks >> 8));
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_DATA_SOURCE,
+			0x01, RING_BUFFER);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MC_DMA_TC3,
+			0xFF, (u8)(data_len >> 24));
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MC_DMA_TC2,
+			0xFF, (u8)(data_len >> 16));
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MC_DMA_TC1,
+			0xFF, (u8)(data_len >> 8));
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MC_DMA_TC0,
+			0xFF, (u8)data_len);
+	if (read) {
+		flag = MODE_CDIR;
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MC_DMA_CTL,
+				0x03 | DMA_PACK_SIZE_MASK,
+				DMA_DIR_FROM_CARD | DMA_EN | DMA_512);
+	} else {
+		flag = MODE_CDOR;
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, MC_DMA_CTL,
+				0x03 | DMA_PACK_SIZE_MASK,
+				DMA_DIR_TO_CARD | DMA_EN | DMA_512);
+	}
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_CFG2, 0xFF, cfg2);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
+			trans_mode | SD_TRANSFER_START);
+	rtsx_usb_add_cmd(ucr, CHECK_REG_CMD, SD_TRANSFER,
+			SD_TRANSFER_END, SD_TRANSFER_END);
+
+	err = rtsx_usb_send_cmd(ucr, flag, 100);
+	if (err)
+		return err;
+
+	if (read)
+		pipe = usb_rcvbulkpipe(ucr->pusb_dev, EP_BULK_IN);
+	else
+		pipe = usb_sndbulkpipe(ucr->pusb_dev, EP_BULK_OUT);
+
+	err = rtsx_usb_transfer_data(ucr, pipe, data->sg, data_len,
+			data->sg_len,  NULL, 10000);
+	if (err) {
+		dev_dbg(sdmmc_dev(host), "rtsx_usb_transfer_data error %d\n"
+				, err);
+		sd_clear_error(host);
+		return err;
+	}
+
+	return rtsx_usb_get_rsp(ucr, 1, 2000);
+}
+
+static inline void sd_enable_initial_mode(struct rtsx_usb_sdmmc *host)
+{
+	rtsx_usb_write_register(host->ucr, SD_CFG1,
+			SD_CLK_DIVIDE_MASK, SD_CLK_DIVIDE_128);
+}
+
+static inline void sd_disable_initial_mode(struct rtsx_usb_sdmmc *host)
+{
+	rtsx_usb_write_register(host->ucr, SD_CFG1,
+			SD_CLK_DIVIDE_MASK, SD_CLK_DIVIDE_0);
+}
+
+static void sd_normal_rw(struct rtsx_usb_sdmmc *host,
+		struct mmc_request *mrq)
+{
+	struct mmc_command *cmd = mrq->cmd;
+	struct mmc_data *data = mrq->data;
+	u8 *buf;
+
+	buf = kzalloc(data->blksz, GFP_NOIO);
+	if (!buf) {
+		cmd->error = -ENOMEM;
+		return;
+	}
+
+	if (data->flags & MMC_DATA_READ) {
+		if (host->initial_mode)
+			sd_disable_initial_mode(host);
+
+		cmd->error = sd_read_data(host, cmd, (u16)data->blksz, buf,
+				data->blksz, 200);
+
+		if (host->initial_mode)
+			sd_enable_initial_mode(host);
+
+		sg_copy_from_buffer(data->sg, data->sg_len, buf, data->blksz);
+	} else {
+		sg_copy_to_buffer(data->sg, data->sg_len, buf, data->blksz);
+
+		cmd->error = sd_write_data(host, cmd, (u16)data->blksz, buf,
+				data->blksz, 200);
+	}
+
+	kfree(buf);
+}
+
+static int sd_change_phase(struct rtsx_usb_sdmmc *host, u8 sample_point, int tx)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	int err;
+
+	dev_dbg(sdmmc_dev(host), "%s: %s sample_point = %d\n",
+			__func__, tx ? "TX" : "RX", sample_point);
+
+	rtsx_usb_init_cmd(ucr);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CLK_DIV, CLK_CHANGE, CLK_CHANGE);
+
+	if (tx)
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_VPCLK0_CTL,
+				0x0F, sample_point);
+	else
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_VPCLK1_CTL,
+				0x0F, sample_point);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_VPCLK0_CTL, PHASE_NOT_RESET, 0);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_VPCLK0_CTL,
+			PHASE_NOT_RESET, PHASE_NOT_RESET);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CLK_DIV, CLK_CHANGE, 0);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_CFG1, SD_ASYNC_FIFO_RST, 0);
+
+	err = rtsx_usb_send_cmd(ucr, MODE_C, 100);
+	if (err)
+		return err;
+
+	return 0;
+}
+
+static inline u32 get_phase_point(u32 phase_map, unsigned int idx)
+{
+	idx &= MAX_PHASE;
+	return phase_map & (1 << idx);
+}
+
+static int get_phase_len(u32 phase_map, unsigned int idx)
+{
+	int i;
+
+	for (i = 0; i < MAX_PHASE + 1; i++) {
+		if (get_phase_point(phase_map, idx + i) == 0)
+			return i;
+	}
+	return MAX_PHASE + 1;
+}
+
+static u8 sd_search_final_phase(struct rtsx_usb_sdmmc *host, u32 phase_map)
+{
+	int start = 0, len = 0;
+	int start_final = 0, len_final = 0;
+	u8 final_phase = 0xFF;
+
+	if (phase_map == 0) {
+		dev_dbg(sdmmc_dev(host), "Phase: [map:%x]\n", phase_map);
+		return final_phase;
+	}
+
+	while (start < MAX_PHASE + 1) {
+		len = get_phase_len(phase_map, start);
+		if (len_final < len) {
+			start_final = start;
+			len_final = len;
+		}
+		start += len ? len : 1;
+	}
+
+	final_phase = (start_final + len_final / 2) & MAX_PHASE;
+	dev_dbg(sdmmc_dev(host), "Phase: [map:%x] [maxlen:%d] [final:%d]\n",
+		phase_map, len_final, final_phase);
+
+	return final_phase;
+}
+
+static void sd_wait_data_idle(struct rtsx_usb_sdmmc *host)
+{
+	int err, i;
+	u8 val = 0;
+
+	for (i = 0; i < 100; i++) {
+		err = rtsx_usb_ep0_read_register(host->ucr,
+				SD_DATA_STATE, &val);
+		if (val & SD_DATA_IDLE)
+			return;
+
+		usleep_range(100, 1000);
+	}
+}
+
+static int sd_tuning_rx_cmd(struct rtsx_usb_sdmmc *host,
+		u8 opcode, u8 sample_point)
+{
+	int err;
+	struct mmc_command cmd = {0};
+
+	err = sd_change_phase(host, sample_point, 0);
+	if (err)
+		return err;
+
+	cmd.opcode = MMC_SEND_TUNING_BLOCK;
+	err = sd_read_data(host, &cmd, 0x40, NULL, 0, 100);
+	if (err) {
+		/* Wait till SD DATA IDLE */
+		sd_wait_data_idle(host);
+		sd_clear_error(host);
+		return err;
+	}
+
+	return 0;
+}
+
+static void sd_tuning_phase(struct rtsx_usb_sdmmc *host,
+		u8 opcode, u16 *phase_map)
+{
+	int err, i;
+	u16 raw_phase_map = 0;
+
+	for (i = MAX_PHASE; i >= 0; i--) {
+		err = sd_tuning_rx_cmd(host, opcode, (u8)i);
+		if (!err)
+			raw_phase_map |= 1 << i;
+	}
+
+	if (phase_map)
+		*phase_map = raw_phase_map;
+}
+
+static int sd_tuning_rx(struct rtsx_usb_sdmmc *host, u8 opcode)
+{
+	int err, i;
+	u16 raw_phase_map[RX_TUNING_CNT] = {0}, phase_map;
+	u8 final_phase;
+
+	/* setting fixed default TX phase */
+	err = sd_change_phase(host, 0x01, 1);
+	if (err) {
+		dev_dbg(sdmmc_dev(host), "TX phase setting failed\n");
+		return err;
+	}
+
+	/* tuning RX phase */
+	for (i = 0; i < RX_TUNING_CNT; i++) {
+		sd_tuning_phase(host, opcode, &(raw_phase_map[i]));
+
+		if (raw_phase_map[i] == 0)
+			break;
+	}
+
+	phase_map = 0xFFFF;
+	for (i = 0; i < RX_TUNING_CNT; i++) {
+		dev_dbg(sdmmc_dev(host), "RX raw_phase_map[%d] = 0x%04x\n",
+				i, raw_phase_map[i]);
+		phase_map &= raw_phase_map[i];
+	}
+	dev_dbg(sdmmc_dev(host), "RX phase_map = 0x%04x\n", phase_map);
+
+	if (phase_map) {
+		final_phase = sd_search_final_phase(host, phase_map);
+		if (final_phase == 0xFF)
+			return -EINVAL;
+
+		err = sd_change_phase(host, final_phase, 0);
+		if (err)
+			return err;
+	} else {
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int sdmmc_get_ro(struct mmc_host *mmc)
+{
+	struct rtsx_usb_sdmmc *host = mmc_priv(mmc);
+	struct rtsx_ucr *ucr = host->ucr;
+	int err;
+	u16 val;
+
+	if (host->host_removal)
+		return -ENOMEDIUM;
+
+	mutex_lock(&ucr->dev_mutex);
+
+	/* Check SD card detect */
+	err = rtsx_usb_get_card_status(ucr, &val);
+
+	mutex_unlock(&ucr->dev_mutex);
+
+
+	/* Treat failed detection as non-ro */
+	if (err)
+		return 0;
+
+	if (val & SD_WP)
+		return 1;
+
+	return 0;
+}
+
+static int sdmmc_get_cd(struct mmc_host *mmc)
+{
+	struct rtsx_usb_sdmmc *host = mmc_priv(mmc);
+	struct rtsx_ucr *ucr = host->ucr;
+	int err;
+	u16 val;
+
+	if (host->host_removal)
+		return -ENOMEDIUM;
+
+	mutex_lock(&ucr->dev_mutex);
+
+	/* Check SD card detect */
+	err = rtsx_usb_get_card_status(ucr, &val);
+
+	mutex_unlock(&ucr->dev_mutex);
+
+	/* Treat failed detection as non-exist */
+	if (err)
+		goto no_card;
+
+	if (val & SD_CD) {
+		host->card_exist = true;
+		return 1;
+	}
+
+no_card:
+	host->card_exist = false;
+	return 0;
+}
+
+static void sdmmc_request(struct mmc_host *mmc, struct mmc_request *mrq)
+{
+	struct rtsx_usb_sdmmc *host = mmc_priv(mmc);
+	struct rtsx_ucr *ucr = host->ucr;
+	struct mmc_command *cmd = mrq->cmd;
+	struct mmc_data *data = mrq->data;
+	unsigned int data_size = 0;
+
+	dev_dbg(sdmmc_dev(host), "%s\n", __func__);
+
+	if (host->host_removal) {
+		cmd->error = -ENOMEDIUM;
+		goto finish;
+	}
+
+	if ((!host->card_exist)) {
+		cmd->error = -ENOMEDIUM;
+		goto finish_detect_card;
+	}
+
+	/*
+	 * Reject SDIO CMDs to speed up card identification
+	 * since unsupported
+	 */
+	if (cmd->opcode == SD_IO_SEND_OP_COND ||
+	    cmd->opcode == SD_IO_RW_DIRECT ||
+	    cmd->opcode == SD_IO_RW_EXTENDED) {
+		cmd->error = -EINVAL;
+		goto finish;
+	}
+
+	mutex_lock(&ucr->dev_mutex);
+
+	mutex_lock(&host->host_mutex);
+	host->mrq = mrq;
+	mutex_unlock(&host->host_mutex);
+
+	if (mrq->data)
+		data_size = data->blocks * data->blksz;
+
+	if (!data_size) {
+		sd_send_cmd_get_rsp(host, cmd);
+	} else if ((!(data_size % 512) && cmd->opcode != MMC_SEND_EXT_CSD) ||
+		   mmc_op_multi(cmd->opcode)) {
+		sd_send_cmd_get_rsp(host, cmd);
+
+		if (!cmd->error) {
+			sd_rw_multi(host, mrq);
+
+			if (mmc_op_multi(cmd->opcode) && mrq->stop) {
+				sd_send_cmd_get_rsp(host, mrq->stop);
+				rtsx_usb_write_register(ucr, MC_FIFO_CTL,
+						FIFO_FLUSH, FIFO_FLUSH);
+			}
+		}
+	} else {
+		sd_normal_rw(host, mrq);
+	}
+
+	if (mrq->data) {
+		if (cmd->error || data->error)
+			data->bytes_xfered = 0;
+		else
+			data->bytes_xfered = data->blocks * data->blksz;
+	}
+
+	mutex_unlock(&ucr->dev_mutex);
+
+finish_detect_card:
+	if (cmd->error) {
+		/*
+		 * detect card when fail to update card existence state and
+		 * speed up card removal when retry
+		 */
+		sdmmc_get_cd(mmc);
+		dev_dbg(sdmmc_dev(host), "cmd->error = %d\n", cmd->error);
+	}
+
+finish:
+	mutex_lock(&host->host_mutex);
+	host->mrq = NULL;
+	mutex_unlock(&host->host_mutex);
+
+	mmc_request_done(mmc, mrq);
+}
+
+static int sd_set_bus_width(struct rtsx_usb_sdmmc *host,
+		unsigned char bus_width)
+{
+	int err = 0;
+	u8 width[] = {
+		[MMC_BUS_WIDTH_1] = SD_BUS_WIDTH_1BIT,
+		[MMC_BUS_WIDTH_4] = SD_BUS_WIDTH_4BIT,
+		[MMC_BUS_WIDTH_8] = SD_BUS_WIDTH_8BIT,
+	};
+
+	if (bus_width <= MMC_BUS_WIDTH_8)
+		err = rtsx_usb_write_register(host->ucr, SD_CFG1,
+				0x03, width[bus_width]);
+
+	return err;
+}
+
+static int sd_pull_ctl_disable_lqfp48(struct rtsx_ucr *ucr)
+{
+	rtsx_usb_init_cmd(ucr);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0xA5);
+
+	return rtsx_usb_send_cmd(ucr, MODE_C, 100);
+}
+
+static int sd_pull_ctl_disable_qfn24(struct rtsx_ucr *ucr)
+{
+	rtsx_usb_init_cmd(ucr);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x65);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x56);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0x59);
+
+	return rtsx_usb_send_cmd(ucr, MODE_C, 100);
+}
+
+static int sd_pull_ctl_enable_lqfp48(struct rtsx_ucr *ucr)
+{
+	rtsx_usb_init_cmd(ucr);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0xAA);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0xAA);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0xA9);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0xA5);
+
+	return rtsx_usb_send_cmd(ucr, MODE_C, 100);
+}
+
+static int sd_pull_ctl_enable_qfn24(struct rtsx_ucr *ucr)
+{
+	rtsx_usb_init_cmd(ucr);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0xA5);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x9A);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0xA5);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x9A);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x65);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0x5A);
+
+	return rtsx_usb_send_cmd(ucr, MODE_C, 100);
+}
+
+static int sd_power_on(struct rtsx_usb_sdmmc *host)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	int err;
+
+	dev_dbg(sdmmc_dev(host), "%s\n", __func__);
+	rtsx_usb_init_cmd(ucr);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_SELECT, 0x07, SD_MOD_SEL);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_SHARE_MODE,
+			CARD_SHARE_MASK, CARD_SHARE_SD);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_CLK_EN,
+			SD_CLK_EN, SD_CLK_EN);
+	err = rtsx_usb_send_cmd(ucr, MODE_C, 100);
+	if (err)
+		return err;
+
+	if (CHECK_PKG(ucr, LQFP48))
+		err = sd_pull_ctl_enable_lqfp48(ucr);
+	else
+		err = sd_pull_ctl_enable_qfn24(ucr);
+	if (err)
+		return err;
+
+	err = rtsx_usb_write_register(ucr, CARD_PWR_CTL,
+			POWER_MASK, PARTIAL_POWER_ON);
+	if (err)
+		return err;
+
+	usleep_range(800, 1000);
+
+	rtsx_usb_init_cmd(ucr);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PWR_CTL,
+			POWER_MASK|LDO3318_PWR_MASK, POWER_ON|LDO_ON);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_OE,
+			SD_OUTPUT_EN, SD_OUTPUT_EN);
+
+	return rtsx_usb_send_cmd(ucr, MODE_C, 100);
+}
+
+static int sd_power_off(struct rtsx_usb_sdmmc *host)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	int err;
+
+	dev_dbg(sdmmc_dev(host), "%s\n", __func__);
+	rtsx_usb_init_cmd(ucr);
+
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_CLK_EN, SD_CLK_EN, 0);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_OE, SD_OUTPUT_EN, 0);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PWR_CTL,
+			POWER_MASK, POWER_OFF);
+	rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_PWR_CTL,
+			POWER_MASK|LDO3318_PWR_MASK, POWER_OFF|LDO_SUSPEND);
+
+	err = rtsx_usb_send_cmd(ucr, MODE_C, 100);
+	if (err)
+		return err;
+
+	if (CHECK_PKG(ucr, LQFP48))
+			return sd_pull_ctl_disable_lqfp48(ucr);
+	return sd_pull_ctl_disable_qfn24(ucr);
+}
+
+static int sd_set_power_mode(struct rtsx_usb_sdmmc *host,
+		unsigned char power_mode)
+{
+	int err;
+
+	if (power_mode != MMC_POWER_OFF)
+		power_mode = MMC_POWER_ON;
+
+	if (power_mode == host->power_mode)
+		return 0;
+
+	if (power_mode == MMC_POWER_OFF) {
+		err = sd_power_off(host);
+		pm_runtime_put(sdmmc_dev(host));
+	} else {
+		pm_runtime_get_sync(sdmmc_dev(host));
+		err = sd_power_on(host);
+	}
+
+	if (!err)
+		host->power_mode = power_mode;
+
+	return err;
+}
+
+static int sd_set_timing(struct rtsx_usb_sdmmc *host,
+		unsigned char timing, bool *ddr_mode)
+{
+	struct rtsx_ucr *ucr = host->ucr;
+	int err;
+
+	*ddr_mode = false;
+
+	rtsx_usb_init_cmd(ucr);
+
+	switch (timing) {
+	case MMC_TIMING_UHS_SDR104:
+	case MMC_TIMING_UHS_SDR50:
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_CFG1,
+				0x0C | SD_ASYNC_FIFO_RST,
+				SD_30_MODE | SD_ASYNC_FIFO_RST);
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_CLK_SOURCE, 0xFF,
+				CRC_VAR_CLK0 | SD30_FIX_CLK | SAMPLE_VAR_CLK1);
+		break;
+
+	case MMC_TIMING_UHS_DDR50:
+		*ddr_mode = true;
+
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_CFG1,
+				0x0C | SD_ASYNC_FIFO_RST,
+				SD_DDR_MODE | SD_ASYNC_FIFO_RST);
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_CLK_SOURCE, 0xFF,
+				CRC_VAR_CLK0 | SD30_FIX_CLK | SAMPLE_VAR_CLK1);
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_PUSH_POINT_CTL,
+				DDR_VAR_TX_CMD_DAT, DDR_VAR_TX_CMD_DAT);
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_SAMPLE_POINT_CTL,
+				DDR_VAR_RX_DAT | DDR_VAR_RX_CMD,
+				DDR_VAR_RX_DAT | DDR_VAR_RX_CMD);
+		break;
+
+	case MMC_TIMING_MMC_HS:
+	case MMC_TIMING_SD_HS:
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_CFG1,
+				0x0C, SD_20_MODE);
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_CLK_SOURCE, 0xFF,
+				CRC_FIX_CLK | SD30_VAR_CLK0 | SAMPLE_VAR_CLK1);
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_PUSH_POINT_CTL,
+				SD20_TX_SEL_MASK, SD20_TX_14_AHEAD);
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_SAMPLE_POINT_CTL,
+				SD20_RX_SEL_MASK, SD20_RX_14_DELAY);
+		break;
+
+	default:
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD,
+				SD_CFG1, 0x0C, SD_20_MODE);
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, CARD_CLK_SOURCE, 0xFF,
+				CRC_FIX_CLK | SD30_VAR_CLK0 | SAMPLE_VAR_CLK1);
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD,
+				SD_PUSH_POINT_CTL, 0xFF, 0);
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_SAMPLE_POINT_CTL,
+				SD20_RX_SEL_MASK, SD20_RX_POS_EDGE);
+		break;
+	}
+
+	err = rtsx_usb_send_cmd(ucr, MODE_C, 100);
+
+	return err;
+}
+
+static void sdmmc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
+{
+	struct rtsx_usb_sdmmc *host = mmc_priv(mmc);
+	struct rtsx_ucr *ucr = host->ucr;
+
+	dev_dbg(sdmmc_dev(host), "%s\n", __func__);
+	mutex_lock(&ucr->dev_mutex);
+
+	if (rtsx_usb_card_exclusive_check(ucr, RTSX_USB_SD_CARD)) {
+		mutex_unlock(&ucr->dev_mutex);
+		return;
+	}
+
+	sd_set_power_mode(host, ios->power_mode);
+	sd_set_bus_width(host, ios->bus_width);
+	sd_set_timing(host, ios->timing, &host->ddr_mode);
+
+	host->vpclk = false;
+	host->double_clk = true;
+
+	switch (ios->timing) {
+	case MMC_TIMING_UHS_SDR104:
+	case MMC_TIMING_UHS_SDR50:
+		host->ssc_depth = SSC_DEPTH_2M;
+		host->vpclk = true;
+		host->double_clk = false;
+		break;
+	case MMC_TIMING_UHS_DDR50:
+	case MMC_TIMING_UHS_SDR25:
+		host->ssc_depth = SSC_DEPTH_1M;
+		break;
+	default:
+		host->ssc_depth = SSC_DEPTH_512K;
+		break;
+	}
+
+	host->initial_mode = (ios->clock <= 1000000) ? true : false;
+	host->clock = ios->clock;
+
+	rtsx_usb_switch_clock(host->ucr, host->clock, host->ssc_depth,
+			host->initial_mode, host->double_clk, host->vpclk);
+
+	mutex_unlock(&ucr->dev_mutex);
+	dev_dbg(sdmmc_dev(host), "%s end\n", __func__);
+}
+
+static int sdmmc_switch_voltage(struct mmc_host *mmc, struct mmc_ios *ios)
+{
+	struct rtsx_usb_sdmmc *host = mmc_priv(mmc);
+	struct rtsx_ucr *ucr = host->ucr;
+	int err = 0;
+
+	dev_dbg(sdmmc_dev(host), "%s: signal_voltage = %d\n",
+			__func__, ios->signal_voltage);
+
+	if (host->host_removal)
+		return -ENOMEDIUM;
+
+	if (ios->signal_voltage == MMC_SIGNAL_VOLTAGE_120)
+		return -EPERM;
+
+	mutex_lock(&ucr->dev_mutex);
+
+	err = rtsx_usb_card_exclusive_check(ucr, RTSX_USB_SD_CARD);
+	if (err) {
+		mutex_unlock(&ucr->dev_mutex);
+		return err;
+	}
+
+	/* Let mmc core do the busy checking, simply stop the forced-toggle
+	 * clock(while issuing CMD11) and switch voltage.
+	 */
+	rtsx_usb_init_cmd(ucr);
+
+	if (ios->signal_voltage == MMC_SIGNAL_VOLTAGE_330) {
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_PAD_CTL,
+				SD_IO_USING_1V8, SD_IO_USING_3V3);
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, LDO_POWER_CFG,
+				TUNE_SD18_MASK, TUNE_SD18_3V3);
+	} else {
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_BUS_STAT,
+				SD_CLK_TOGGLE_EN | SD_CLK_FORCE_STOP,
+				SD_CLK_FORCE_STOP);
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, SD_PAD_CTL,
+				SD_IO_USING_1V8, SD_IO_USING_1V8);
+		rtsx_usb_add_cmd(ucr, WRITE_REG_CMD, LDO_POWER_CFG,
+				TUNE_SD18_MASK, TUNE_SD18_1V8);
+	}
+
+	err = rtsx_usb_send_cmd(ucr, MODE_C, 100);
+	mutex_unlock(&ucr->dev_mutex);
+
+	return err;
+}
+
+static int sdmmc_card_busy(struct mmc_host *mmc)
+{
+	struct rtsx_usb_sdmmc *host = mmc_priv(mmc);
+	struct rtsx_ucr *ucr = host->ucr;
+	int err;
+	u8 stat;
+	u8 mask = SD_DAT3_STATUS | SD_DAT2_STATUS | SD_DAT1_STATUS
+		| SD_DAT0_STATUS;
+
+	dev_dbg(sdmmc_dev(host), "%s\n", __func__);
+
+	mutex_lock(&ucr->dev_mutex);
+
+	err = rtsx_usb_write_register(ucr, SD_BUS_STAT,
+			SD_CLK_TOGGLE_EN | SD_CLK_FORCE_STOP,
+			SD_CLK_TOGGLE_EN);
+	if (err)
+		goto out;
+
+	mdelay(1);
+
+	err = rtsx_usb_read_register(ucr, SD_BUS_STAT, &stat);
+	if (err)
+		goto out;
+
+	err = rtsx_usb_write_register(ucr, SD_BUS_STAT,
+			SD_CLK_TOGGLE_EN | SD_CLK_FORCE_STOP, 0);
+out:
+	mutex_unlock(&ucr->dev_mutex);
+
+	if (err)
+		return err;
+
+	/* check if any pin between dat[0:3] is low */
+	if ((stat & mask) != mask)
+		return 1;
+	else
+		return 0;
+}
+
+static int sdmmc_execute_tuning(struct mmc_host *mmc, u32 opcode)
+{
+	struct rtsx_usb_sdmmc *host = mmc_priv(mmc);
+	struct rtsx_ucr *ucr = host->ucr;
+	int err = 0;
+
+	if (host->host_removal)
+		return -ENOMEDIUM;
+
+	mutex_lock(&ucr->dev_mutex);
+
+	if (!host->ddr_mode)
+		err = sd_tuning_rx(host, MMC_SEND_TUNING_BLOCK);
+
+	mutex_unlock(&ucr->dev_mutex);
+
+	return err;
+}
+
+static const struct mmc_host_ops rtsx_usb_sdmmc_ops = {
+	.request = sdmmc_request,
+	.set_ios = sdmmc_set_ios,
+	.get_ro = sdmmc_get_ro,
+	.get_cd = sdmmc_get_cd,
+	.start_signal_voltage_switch = sdmmc_switch_voltage,
+	.card_busy = sdmmc_card_busy,
+	.execute_tuning = sdmmc_execute_tuning,
+};
+
+#ifdef RTSX_USB_USE_LEDS_CLASS
+static void rtsx_usb_led_control(struct led_classdev *led,
+	enum led_brightness brightness)
+{
+	struct rtsx_usb_sdmmc *host = container_of(led,
+			struct rtsx_usb_sdmmc, led);
+
+	if (host->host_removal)
+		return;
+
+	host->led.brightness = brightness;
+	schedule_work(&host->led_work);
+}
+
+static void rtsx_usb_update_led(struct work_struct *work)
+{
+	struct rtsx_usb_sdmmc *host =
+		container_of(work, struct rtsx_usb_sdmmc, led_work);
+	struct rtsx_ucr *ucr = host->ucr;
+
+	mutex_lock(&ucr->dev_mutex);
+
+	if (host->led.brightness == LED_OFF)
+		rtsx_usb_turn_off_led(ucr);
+	else
+		rtsx_usb_turn_on_led(ucr);
+
+	mutex_unlock(&ucr->dev_mutex);
+}
+#endif
+
+static void rtsx_usb_init_host(struct rtsx_usb_sdmmc *host)
+{
+	struct mmc_host *mmc = host->mmc;
+
+	mmc->f_min = 250000;
+	mmc->f_max = 208000000;
+	mmc->ocr_avail = MMC_VDD_32_33 | MMC_VDD_33_34 | MMC_VDD_165_195;
+	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_CAP_UHS_SDR50 |
+		MMC_CAP_NEEDS_POLL;
+
+	mmc->max_current_330 = 400;
+	mmc->max_current_180 = 800;
+	mmc->ops = &rtsx_usb_sdmmc_ops;
+	mmc->max_segs = 256;
+	mmc->max_seg_size = 65536;
+	mmc->max_blk_size = 512;
+	mmc->max_blk_count = 65535;
+	mmc->max_req_size = 524288;
+
+	host->power_mode = MMC_POWER_OFF;
+}
+
+static int rtsx_usb_sdmmc_drv_probe(struct platform_device *pdev)
+{
+	struct mmc_host *mmc;
+	struct rtsx_usb_sdmmc *host;
+	struct rtsx_ucr *ucr;
+#ifdef RTSX_USB_USE_LEDS_CLASS
+	int err;
+#endif
+
+	ucr = usb_get_intfdata(to_usb_interface(pdev->dev.parent));
+	if (!ucr)
+		return -ENXIO;
+
+	dev_dbg(&(pdev->dev), ": Realtek USB SD/MMC controller found\n");
+
+	mmc = mmc_alloc_host(sizeof(*host), &pdev->dev);
+	if (!mmc)
+		return -ENOMEM;
+
+	host = mmc_priv(mmc);
+	host->ucr = ucr;
+	host->mmc = mmc;
+	host->pdev = pdev;
+	platform_set_drvdata(pdev, host);
+
+	mutex_init(&host->host_mutex);
+	rtsx_usb_init_host(host);
+	pm_runtime_enable(&pdev->dev);
+
+#ifdef RTSX_USB_USE_LEDS_CLASS
+	snprintf(host->led_name, sizeof(host->led_name),
+		"%s::", mmc_hostname(mmc));
+	host->led.name = host->led_name;
+	host->led.brightness = LED_OFF;
+	host->led.default_trigger = mmc_hostname(mmc);
+	host->led.brightness_set = rtsx_usb_led_control;
+
+	err = led_classdev_register(mmc_dev(mmc), &host->led);
+	if (err)
+		dev_err(&(pdev->dev),
+				"Failed to register LED device: %d\n", err);
+	INIT_WORK(&host->led_work, rtsx_usb_update_led);
+
+#endif
+	mmc_add_host(mmc);
+
+	return 0;
+}
+
+static int rtsx_usb_sdmmc_drv_remove(struct platform_device *pdev)
+{
+	struct rtsx_usb_sdmmc *host = platform_get_drvdata(pdev);
+	struct mmc_host *mmc;
+
+	if (!host)
+		return 0;
+
+	mmc = host->mmc;
+	host->host_removal = true;
+
+	mutex_lock(&host->host_mutex);
+	if (host->mrq) {
+		dev_dbg(&(pdev->dev),
+			"%s: Controller removed during transfer\n",
+			mmc_hostname(mmc));
+		host->mrq->cmd->error = -ENOMEDIUM;
+		if (host->mrq->stop)
+			host->mrq->stop->error = -ENOMEDIUM;
+		mmc_request_done(mmc, host->mrq);
+	}
+	mutex_unlock(&host->host_mutex);
+
+	mmc_remove_host(mmc);
+
+#ifdef RTSX_USB_USE_LEDS_CLASS
+	cancel_work_sync(&host->led_work);
+	led_classdev_unregister(&host->led);
+#endif
+
+	mmc_free_host(mmc);
+	pm_runtime_disable(&pdev->dev);
+	platform_set_drvdata(pdev, NULL);
+
+	dev_dbg(&(pdev->dev),
+		": Realtek USB SD/MMC module has been removed\n");
+
+	return 0;
+}
+
+static struct platform_device_id rtsx_usb_sdmmc_ids[] = {
+	{
+		.name = "rtsx_usb_sdmmc",
+	}, {
+		/* sentinel */
+	}
+};
+MODULE_DEVICE_TABLE(platform, rtsx_usb_sdmmc_ids);
+
+static struct platform_driver rtsx_usb_sdmmc_driver = {
+	.probe		= rtsx_usb_sdmmc_drv_probe,
+	.remove		= rtsx_usb_sdmmc_drv_remove,
+	.id_table       = rtsx_usb_sdmmc_ids,
+	.driver		= {
+		.owner	= THIS_MODULE,
+		.name	= "rtsx_usb_sdmmc",
+	},
+};
+module_platform_driver(rtsx_usb_sdmmc_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Roger Tseng <rogerable@realtek.com>");
+MODULE_DESCRIPTION("Realtek USB SD/MMC Card Host Driver");
diff --git a/drivers/mmc/host/sdhci-acpi.c b/drivers/mmc/host/sdhci-acpi.c
index ebb3f392b58..8ce3c28cb76 100644
--- a/drivers/mmc/host/sdhci-acpi.c
+++ b/drivers/mmc/host/sdhci-acpi.c
@@ -102,11 +102,19 @@ static void sdhci_acpi_int_hw_reset(struct sdhci_host *host)
 }
 
 static const struct sdhci_ops sdhci_acpi_ops_dflt = {
+	.set_clock = sdhci_set_clock,
 	.enable_dma = sdhci_acpi_enable_dma,
+	.set_bus_width = sdhci_set_bus_width,
+	.reset = sdhci_reset,
+	.set_uhs_signaling = sdhci_set_uhs_signaling,
 };
 
 static const struct sdhci_ops sdhci_acpi_ops_int = {
+	.set_clock = sdhci_set_clock,
 	.enable_dma = sdhci_acpi_enable_dma,
+	.set_bus_width = sdhci_set_bus_width,
+	.reset = sdhci_reset,
+	.set_uhs_signaling = sdhci_set_uhs_signaling,
 	.hw_reset   = sdhci_acpi_int_hw_reset,
 };
 
diff --git a/drivers/mmc/host/sdhci-bcm-kona.c b/drivers/mmc/host/sdhci-bcm-kona.c
index 6f166e63b81..dd780c315a6 100644
--- a/drivers/mmc/host/sdhci-bcm-kona.c
+++ b/drivers/mmc/host/sdhci-bcm-kona.c
@@ -206,9 +206,13 @@ static void sdhci_bcm_kona_init_74_clocks(struct sdhci_host *host,
 }
 
 static struct sdhci_ops sdhci_bcm_kona_ops = {
+	.set_clock = sdhci_set_clock,
 	.get_max_clock = sdhci_bcm_kona_get_max_clk,
 	.get_timeout_clock = sdhci_bcm_kona_get_timeout_clock,
 	.platform_send_init_74_clocks = sdhci_bcm_kona_init_74_clocks,
+	.set_bus_width = sdhci_set_bus_width,
+	.reset = sdhci_reset,
+	.set_uhs_signaling = sdhci_set_uhs_signaling,
 	.card_event = sdhci_bcm_kona_card_event,
 };
 
diff --git a/drivers/mmc/host/sdhci-bcm2835.c b/drivers/mmc/host/sdhci-bcm2835.c
index f6d8d67c545..46af9a439d7 100644
--- a/drivers/mmc/host/sdhci-bcm2835.c
+++ b/drivers/mmc/host/sdhci-bcm2835.c
@@ -131,8 +131,12 @@ static const struct sdhci_ops bcm2835_sdhci_ops = {
 	.read_l = bcm2835_sdhci_readl,
 	.read_w = bcm2835_sdhci_readw,
 	.read_b = bcm2835_sdhci_readb,
+	.set_clock = sdhci_set_clock,
 	.get_max_clock = sdhci_pltfm_clk_get_max_clock,
 	.get_min_clock = bcm2835_sdhci_get_min_clock,
+	.set_bus_width = sdhci_set_bus_width,
+	.reset = sdhci_reset,
+	.set_uhs_signaling = sdhci_set_uhs_signaling,
 };
 
 static const struct sdhci_pltfm_data bcm2835_sdhci_pdata = {
diff --git a/drivers/mmc/host/sdhci-cns3xxx.c b/drivers/mmc/host/sdhci-cns3xxx.c
index f2cc26633cb..14b74075589 100644
--- a/drivers/mmc/host/sdhci-cns3xxx.c
+++ b/drivers/mmc/host/sdhci-cns3xxx.c
@@ -30,13 +30,12 @@ static void sdhci_cns3xxx_set_clock(struct sdhci_host *host, unsigned int clock)
 	u16 clk;
 	unsigned long timeout;
 
-	if (clock == host->clock)
-		return;
+	host->mmc->actual_clock = 0;
 
 	sdhci_writew(host, 0, SDHCI_CLOCK_CONTROL);
 
 	if (clock == 0)
-		goto out;
+		return;
 
 	while (host->max_clk / div > clock) {
 		/*
@@ -75,13 +74,14 @@ static void sdhci_cns3xxx_set_clock(struct sdhci_host *host, unsigned int clock)
 
 	clk |= SDHCI_CLOCK_CARD_EN;
 	sdhci_writew(host, clk, SDHCI_CLOCK_CONTROL);
-out:
-	host->clock = clock;
 }
 
 static const struct sdhci_ops sdhci_cns3xxx_ops = {
 	.get_max_clock	= sdhci_cns3xxx_get_max_clk,
 	.set_clock	= sdhci_cns3xxx_set_clock,
+	.set_bus_width	= sdhci_set_bus_width,
+	.reset          = sdhci_reset,
+	.set_uhs_signaling = sdhci_set_uhs_signaling,
 };
 
 static const struct sdhci_pltfm_data sdhci_cns3xxx_pdata = {
@@ -90,8 +90,7 @@ static const struct sdhci_pltfm_data sdhci_cns3xxx_pdata = {
 		  SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK |
 		  SDHCI_QUIRK_INVERTED_WRITE_PROTECT |
 		  SDHCI_QUIRK_CAP_CLOCK_BASE_BROKEN |
-		  SDHCI_QUIRK_BROKEN_TIMEOUT_VAL |
-		  SDHCI_QUIRK_NONSTANDARD_CLOCK,
+		  SDHCI_QUIRK_BROKEN_TIMEOUT_VAL,
 };
 
 static int sdhci_cns3xxx_probe(struct platform_device *pdev)
diff --git a/drivers/mmc/host/sdhci-dove.c b/drivers/mmc/host/sdhci-dove.c
index 736d7a2eb7e..e6278ec007d 100644
--- a/drivers/mmc/host/sdhci-dove.c
+++ b/drivers/mmc/host/sdhci-dove.c
@@ -21,28 +21,17 @@
 
 #include <linux/clk.h>
 #include <linux/err.h>
-#include <linux/gpio.h>
 #include <linux/io.h>
 #include <linux/mmc/host.h>
 #include <linux/module.h>
 #include <linux/of.h>
-#include <linux/of_gpio.h>
 
 #include "sdhci-pltfm.h"
 
 struct sdhci_dove_priv {
 	struct clk *clk;
-	int gpio_cd;
 };
 
-static irqreturn_t sdhci_dove_carddetect_irq(int irq, void *data)
-{
-	struct sdhci_host *host = data;
-
-	tasklet_schedule(&host->card_tasklet);
-	return IRQ_HANDLED;
-}
-
 static u16 sdhci_dove_readw(struct sdhci_host *host, int reg)
 {
 	u16 ret;
@@ -60,8 +49,6 @@ static u16 sdhci_dove_readw(struct sdhci_host *host, int reg)
 
 static u32 sdhci_dove_readl(struct sdhci_host *host, int reg)
 {
-	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
-	struct sdhci_dove_priv *priv = pltfm_host->priv;
 	u32 ret;
 
 	ret = readl(host->ioaddr + reg);
@@ -71,14 +58,6 @@ static u32 sdhci_dove_readl(struct sdhci_host *host, int reg)
 		/* Mask the support for 3.0V */
 		ret &= ~SDHCI_CAN_VDD_300;
 		break;
-	case SDHCI_PRESENT_STATE:
-		if (gpio_is_valid(priv->gpio_cd)) {
-			if (gpio_get_value(priv->gpio_cd) == 0)
-				ret |= SDHCI_CARD_PRESENT;
-			else
-				ret &= ~SDHCI_CARD_PRESENT;
-		}
-		break;
 	}
 	return ret;
 }
@@ -86,6 +65,10 @@ static u32 sdhci_dove_readl(struct sdhci_host *host, int reg)
 static const struct sdhci_ops sdhci_dove_ops = {
 	.read_w	= sdhci_dove_readw,
 	.read_l	= sdhci_dove_readl,
+	.set_clock = sdhci_set_clock,
+	.set_bus_width = sdhci_set_bus_width,
+	.reset = sdhci_reset,
+	.set_uhs_signaling = sdhci_set_uhs_signaling,
 };
 
 static const struct sdhci_pltfm_data sdhci_dove_pdata = {
@@ -113,28 +96,9 @@ static int sdhci_dove_probe(struct platform_device *pdev)
 
 	priv->clk = devm_clk_get(&pdev->dev, NULL);
 
-	if (pdev->dev.of_node) {
-		priv->gpio_cd = of_get_named_gpio(pdev->dev.of_node,
-						  "cd-gpios", 0);
-	} else {
-		priv->gpio_cd = -EINVAL;
-	}
-
-	if (gpio_is_valid(priv->gpio_cd)) {
-		ret = gpio_request(priv->gpio_cd, "sdhci-cd");
-		if (ret) {
-			dev_err(&pdev->dev, "card detect gpio request failed: %d\n",
-				ret);
-			return ret;
-		}
-		gpio_direction_input(priv->gpio_cd);
-	}
-
 	host = sdhci_pltfm_init(pdev, &sdhci_dove_pdata, 0);
-	if (IS_ERR(host)) {
-		ret = PTR_ERR(host);
-		goto err_sdhci_pltfm_init;
-	}
+	if (IS_ERR(host))
+		return PTR_ERR(host);
 
 	pltfm_host = sdhci_priv(host);
 	pltfm_host->priv = priv;
@@ -142,39 +106,20 @@ static int sdhci_dove_probe(struct platform_device *pdev)
 	if (!IS_ERR(priv->clk))
 		clk_prepare_enable(priv->clk);
 
-	sdhci_get_of_property(pdev);
+	ret = mmc_of_parse(host->mmc);
+	if (ret)
+		goto err_sdhci_add;
 
 	ret = sdhci_add_host(host);
 	if (ret)
 		goto err_sdhci_add;
 
-	/*
-	 * We must request the IRQ after sdhci_add_host(), as the tasklet only
-	 * gets setup in sdhci_add_host() and we oops.
-	 */
-	if (gpio_is_valid(priv->gpio_cd)) {
-		ret = request_irq(gpio_to_irq(priv->gpio_cd),
-				  sdhci_dove_carddetect_irq,
-				  IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING,
-				  mmc_hostname(host->mmc), host);
-		if (ret) {
-			dev_err(&pdev->dev, "card detect irq request failed: %d\n",
-				ret);
-			goto err_request_irq;
-		}
-	}
-
 	return 0;
 
-err_request_irq:
-	sdhci_remove_host(host, 0);
 err_sdhci_add:
 	if (!IS_ERR(priv->clk))
 		clk_disable_unprepare(priv->clk);
 	sdhci_pltfm_free(pdev);
-err_sdhci_pltfm_init:
-	if (gpio_is_valid(priv->gpio_cd))
-		gpio_free(priv->gpio_cd);
 	return ret;
 }
 
@@ -186,11 +131,6 @@ static int sdhci_dove_remove(struct platform_device *pdev)
 
 	sdhci_pltfm_unregister(pdev);
 
-	if (gpio_is_valid(priv->gpio_cd)) {
-		free_irq(gpio_to_irq(priv->gpio_cd), host);
-		gpio_free(priv->gpio_cd);
-	}
-
 	if (!IS_ERR(priv->clk))
 		clk_disable_unprepare(priv->clk);
 
diff --git a/drivers/mmc/host/sdhci-esdhc-imx.c b/drivers/mmc/host/sdhci-esdhc-imx.c
index b841bb7cd37..ccec0e32590 100644
--- a/drivers/mmc/host/sdhci-esdhc-imx.c
+++ b/drivers/mmc/host/sdhci-esdhc-imx.c
@@ -160,7 +160,6 @@ struct pltfm_imx_data {
 		MULTIBLK_IN_PROCESS, /* exact multiblock cmd in process */
 		WAIT_FOR_INT,        /* sent CMD12, waiting for response INT */
 	} multiblock_status;
-	u32 uhs_mode;
 	u32 is_ddr;
 };
 
@@ -382,7 +381,6 @@ static u16 esdhc_readw_le(struct sdhci_host *host, int reg)
 		if (val & ESDHC_MIX_CTRL_SMPCLK_SEL)
 			ret |= SDHCI_CTRL_TUNED_CLK;
 
-		ret |= (imx_data->uhs_mode & SDHCI_CTRL_UHS_MASK);
 		ret &= ~SDHCI_CTRL_PRESET_VAL_ENABLE;
 
 		return ret;
@@ -429,7 +427,6 @@ static void esdhc_writew_le(struct sdhci_host *host, u16 val, int reg)
 		else
 			new_val &= ~ESDHC_VENDOR_SPEC_VSELECT;
 		writel(new_val, host->ioaddr + ESDHC_VENDOR_SPEC);
-		imx_data->uhs_mode = val & SDHCI_CTRL_UHS_MASK;
 		if (imx_data->socdata->flags & ESDHC_FLAG_MAN_TUNING) {
 			new_val = readl(host->ioaddr + ESDHC_MIX_CTRL);
 			if (val & SDHCI_CTRL_TUNED_CLK)
@@ -600,12 +597,14 @@ static inline void esdhc_pltfm_set_clock(struct sdhci_host *host,
 	u32 temp, val;
 
 	if (clock == 0) {
+		host->mmc->actual_clock = 0;
+
 		if (esdhc_is_usdhc(imx_data)) {
 			val = readl(host->ioaddr + ESDHC_VENDOR_SPEC);
 			writel(val & ~ESDHC_VENDOR_SPEC_FRC_SDCLK_ON,
 					host->ioaddr + ESDHC_VENDOR_SPEC);
 		}
-		goto out;
+		return;
 	}
 
 	if (esdhc_is_usdhc(imx_data) && !imx_data->is_ddr)
@@ -645,8 +644,6 @@ static inline void esdhc_pltfm_set_clock(struct sdhci_host *host,
 	}
 
 	mdelay(1);
-out:
-	host->clock = clock;
 }
 
 static unsigned int esdhc_pltfm_get_ro(struct sdhci_host *host)
@@ -668,7 +665,7 @@ static unsigned int esdhc_pltfm_get_ro(struct sdhci_host *host)
 	return -ENOSYS;
 }
 
-static int esdhc_pltfm_bus_width(struct sdhci_host *host, int width)
+static void esdhc_pltfm_set_bus_width(struct sdhci_host *host, int width)
 {
 	u32 ctrl;
 
@@ -686,8 +683,6 @@ static int esdhc_pltfm_bus_width(struct sdhci_host *host, int width)
 
 	esdhc_clrset_le(host, ESDHC_CTRL_BUSWIDTH_MASK, ctrl,
 			SDHCI_HOST_CONTROL);
-
-	return 0;
 }
 
 static void esdhc_prepare_tuning(struct sdhci_host *host, u32 val)
@@ -697,6 +692,7 @@ static void esdhc_prepare_tuning(struct sdhci_host *host, u32 val)
 	/* FIXME: delay a bit for card to be ready for next tuning due to errors */
 	mdelay(1);
 
+	/* This is balanced by the runtime put in sdhci_tasklet_finish */
 	pm_runtime_get_sync(host->mmc->parent);
 	reg = readl(host->ioaddr + ESDHC_MIX_CTRL);
 	reg |= ESDHC_MIX_CTRL_EXE_TUNE | ESDHC_MIX_CTRL_SMPCLK_SEL |
@@ -713,13 +709,12 @@ static void esdhc_request_done(struct mmc_request *mrq)
 	complete(&mrq->completion);
 }
 
-static int esdhc_send_tuning_cmd(struct sdhci_host *host, u32 opcode)
+static int esdhc_send_tuning_cmd(struct sdhci_host *host, u32 opcode,
+				 struct scatterlist *sg)
 {
 	struct mmc_command cmd = {0};
 	struct mmc_request mrq = {NULL};
 	struct mmc_data data = {0};
-	struct scatterlist sg;
-	char tuning_pattern[ESDHC_TUNING_BLOCK_PATTERN_LEN];
 
 	cmd.opcode = opcode;
 	cmd.arg = 0;
@@ -728,11 +723,9 @@ static int esdhc_send_tuning_cmd(struct sdhci_host *host, u32 opcode)
 	data.blksz = ESDHC_TUNING_BLOCK_PATTERN_LEN;
 	data.blocks = 1;
 	data.flags = MMC_DATA_READ;
-	data.sg = &sg;
+	data.sg = sg;
 	data.sg_len = 1;
 
-	sg_init_one(&sg, tuning_pattern, sizeof(tuning_pattern));
-
 	mrq.cmd = &cmd;
 	mrq.cmd->mrq = &mrq;
 	mrq.data = &data;
@@ -742,14 +735,12 @@ static int esdhc_send_tuning_cmd(struct sdhci_host *host, u32 opcode)
 	mrq.done = esdhc_request_done;
 	init_completion(&(mrq.completion));
 
-	disable_irq(host->irq);
-	spin_lock(&host->lock);
+	spin_lock_irq(&host->lock);
 	host->mrq = &mrq;
 
 	sdhci_send_command(host, mrq.cmd);
 
-	spin_unlock(&host->lock);
-	enable_irq(host->irq);
+	spin_unlock_irq(&host->lock);
 
 	wait_for_completion(&mrq.completion);
 
@@ -772,13 +763,21 @@ static void esdhc_post_tuning(struct sdhci_host *host)
 
 static int esdhc_executing_tuning(struct sdhci_host *host, u32 opcode)
 {
+	struct scatterlist sg;
+	char *tuning_pattern;
 	int min, max, avg, ret;
 
+	tuning_pattern = kmalloc(ESDHC_TUNING_BLOCK_PATTERN_LEN, GFP_KERNEL);
+	if (!tuning_pattern)
+		return -ENOMEM;
+
+	sg_init_one(&sg, tuning_pattern, ESDHC_TUNING_BLOCK_PATTERN_LEN);
+
 	/* find the mininum delay first which can pass tuning */
 	min = ESDHC_TUNE_CTRL_MIN;
 	while (min < ESDHC_TUNE_CTRL_MAX) {
 		esdhc_prepare_tuning(host, min);
-		if (!esdhc_send_tuning_cmd(host, opcode))
+		if (!esdhc_send_tuning_cmd(host, opcode, &sg))
 			break;
 		min += ESDHC_TUNE_CTRL_STEP;
 	}
@@ -787,7 +786,7 @@ static int esdhc_executing_tuning(struct sdhci_host *host, u32 opcode)
 	max = min + ESDHC_TUNE_CTRL_STEP;
 	while (max < ESDHC_TUNE_CTRL_MAX) {
 		esdhc_prepare_tuning(host, max);
-		if (esdhc_send_tuning_cmd(host, opcode)) {
+		if (esdhc_send_tuning_cmd(host, opcode, &sg)) {
 			max -= ESDHC_TUNE_CTRL_STEP;
 			break;
 		}
@@ -797,9 +796,11 @@ static int esdhc_executing_tuning(struct sdhci_host *host, u32 opcode)
 	/* use average delay to get the best timing */
 	avg = (min + max) / 2;
 	esdhc_prepare_tuning(host, avg);
-	ret = esdhc_send_tuning_cmd(host, opcode);
+	ret = esdhc_send_tuning_cmd(host, opcode, &sg);
 	esdhc_post_tuning(host);
 
+	kfree(tuning_pattern);
+
 	dev_dbg(mmc_dev(host->mmc), "tunning %s at 0x%x ret %d\n",
 		ret ? "failed" : "passed", avg, ret);
 
@@ -837,28 +838,21 @@ static int esdhc_change_pinstate(struct sdhci_host *host,
 	return pinctrl_select_state(imx_data->pinctrl, pinctrl);
 }
 
-static int esdhc_set_uhs_signaling(struct sdhci_host *host, unsigned int uhs)
+static void esdhc_set_uhs_signaling(struct sdhci_host *host, unsigned timing)
 {
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
 	struct pltfm_imx_data *imx_data = pltfm_host->priv;
 	struct esdhc_platform_data *boarddata = &imx_data->boarddata;
 
-	switch (uhs) {
+	switch (timing) {
 	case MMC_TIMING_UHS_SDR12:
-		imx_data->uhs_mode = SDHCI_CTRL_UHS_SDR12;
-		break;
 	case MMC_TIMING_UHS_SDR25:
-		imx_data->uhs_mode = SDHCI_CTRL_UHS_SDR25;
-		break;
 	case MMC_TIMING_UHS_SDR50:
-		imx_data->uhs_mode = SDHCI_CTRL_UHS_SDR50;
-		break;
 	case MMC_TIMING_UHS_SDR104:
 	case MMC_TIMING_MMC_HS200:
-		imx_data->uhs_mode = SDHCI_CTRL_UHS_SDR104;
 		break;
 	case MMC_TIMING_UHS_DDR50:
-		imx_data->uhs_mode = SDHCI_CTRL_UHS_DDR50;
+	case MMC_TIMING_MMC_DDR52:
 		writel(readl(host->ioaddr + ESDHC_MIX_CTRL) |
 				ESDHC_MIX_CTRL_DDREN,
 				host->ioaddr + ESDHC_MIX_CTRL);
@@ -875,7 +869,15 @@ static int esdhc_set_uhs_signaling(struct sdhci_host *host, unsigned int uhs)
 		break;
 	}
 
-	return esdhc_change_pinstate(host, uhs);
+	esdhc_change_pinstate(host, timing);
+}
+
+static void esdhc_reset(struct sdhci_host *host, u8 mask)
+{
+	sdhci_reset(host, mask);
+
+	sdhci_writel(host, host->ier, SDHCI_INT_ENABLE);
+	sdhci_writel(host, host->ier, SDHCI_SIGNAL_ENABLE);
 }
 
 static struct sdhci_ops sdhci_esdhc_ops = {
@@ -888,8 +890,9 @@ static struct sdhci_ops sdhci_esdhc_ops = {
 	.get_max_clock = esdhc_pltfm_get_max_clock,
 	.get_min_clock = esdhc_pltfm_get_min_clock,
 	.get_ro = esdhc_pltfm_get_ro,
-	.platform_bus_width = esdhc_pltfm_bus_width,
+	.set_bus_width = esdhc_pltfm_set_bus_width,
 	.set_uhs_signaling = esdhc_set_uhs_signaling,
+	.reset = esdhc_reset,
 };
 
 static const struct sdhci_pltfm_data sdhci_esdhc_imx_pdata = {
@@ -1170,8 +1173,10 @@ static int sdhci_esdhc_runtime_suspend(struct device *dev)
 
 	ret = sdhci_runtime_suspend_host(host);
 
-	clk_disable_unprepare(imx_data->clk_per);
-	clk_disable_unprepare(imx_data->clk_ipg);
+	if (!sdhci_sdio_irq_enabled(host)) {
+		clk_disable_unprepare(imx_data->clk_per);
+		clk_disable_unprepare(imx_data->clk_ipg);
+	}
 	clk_disable_unprepare(imx_data->clk_ahb);
 
 	return ret;
@@ -1183,8 +1188,10 @@ static int sdhci_esdhc_runtime_resume(struct device *dev)
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
 	struct pltfm_imx_data *imx_data = pltfm_host->priv;
 
-	clk_prepare_enable(imx_data->clk_per);
-	clk_prepare_enable(imx_data->clk_ipg);
+	if (!sdhci_sdio_irq_enabled(host)) {
+		clk_prepare_enable(imx_data->clk_per);
+		clk_prepare_enable(imx_data->clk_ipg);
+	}
 	clk_prepare_enable(imx_data->clk_ahb);
 
 	return sdhci_runtime_resume_host(host);
diff --git a/drivers/mmc/host/sdhci-esdhc.h b/drivers/mmc/host/sdhci-esdhc.h
index a7d9f95a7b0..3497cfaf683 100644
--- a/drivers/mmc/host/sdhci-esdhc.h
+++ b/drivers/mmc/host/sdhci-esdhc.h
@@ -20,10 +20,8 @@
 
 #define ESDHC_DEFAULT_QUIRKS	(SDHCI_QUIRK_FORCE_BLK_SZ_2048 | \
 				SDHCI_QUIRK_NO_BUSY_IRQ | \
-				SDHCI_QUIRK_NONSTANDARD_CLOCK | \
 				SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK | \
-				SDHCI_QUIRK_PIO_NEEDS_DELAY | \
-				SDHCI_QUIRK_RESTORE_IRQS_AFTER_RESET)
+				SDHCI_QUIRK_PIO_NEEDS_DELAY)
 
 #define ESDHC_SYSTEM_CONTROL	0x2c
 #define ESDHC_CLOCK_MASK	0x0000fff0
diff --git a/drivers/mmc/host/sdhci-of-arasan.c b/drivers/mmc/host/sdhci-of-arasan.c
index f7c7cf62437..5bd1092310f 100644
--- a/drivers/mmc/host/sdhci-of-arasan.c
+++ b/drivers/mmc/host/sdhci-of-arasan.c
@@ -52,8 +52,12 @@ static unsigned int sdhci_arasan_get_timeout_clock(struct sdhci_host *host)
 }
 
 static struct sdhci_ops sdhci_arasan_ops = {
+	.set_clock = sdhci_set_clock,
 	.get_max_clock = sdhci_pltfm_clk_get_max_clock,
 	.get_timeout_clock = sdhci_arasan_get_timeout_clock,
+	.set_bus_width = sdhci_set_bus_width,
+	.reset = sdhci_reset,
+	.set_uhs_signaling = sdhci_set_uhs_signaling,
 };
 
 static struct sdhci_pltfm_data sdhci_arasan_pdata = {
diff --git a/drivers/mmc/host/sdhci-of-esdhc.c b/drivers/mmc/host/sdhci-of-esdhc.c
index 0b249970b11..8be4dcfb49a 100644
--- a/drivers/mmc/host/sdhci-of-esdhc.c
+++ b/drivers/mmc/host/sdhci-of-esdhc.c
@@ -199,13 +199,14 @@ static unsigned int esdhc_of_get_min_clock(struct sdhci_host *host)
 
 static void esdhc_of_set_clock(struct sdhci_host *host, unsigned int clock)
 {
-
 	int pre_div = 2;
 	int div = 1;
 	u32 temp;
 
+	host->mmc->actual_clock = 0;
+
 	if (clock == 0)
-		goto out;
+		return;
 
 	/* Workaround to reduce the clock frequency for p1010 esdhc */
 	if (of_find_compatible_node(NULL, NULL, "fsl,p1010-esdhc")) {
@@ -238,24 +239,8 @@ static void esdhc_of_set_clock(struct sdhci_host *host, unsigned int clock)
 		| (pre_div << ESDHC_PREDIV_SHIFT));
 	sdhci_writel(host, temp, ESDHC_SYSTEM_CONTROL);
 	mdelay(1);
-out:
-	host->clock = clock;
-}
-
-#ifdef CONFIG_PM
-static u32 esdhc_proctl;
-static void esdhc_of_suspend(struct sdhci_host *host)
-{
-	esdhc_proctl = sdhci_be32bs_readl(host, SDHCI_HOST_CONTROL);
 }
 
-static void esdhc_of_resume(struct sdhci_host *host)
-{
-	esdhc_of_enable_dma(host);
-	sdhci_be32bs_writel(host, esdhc_proctl, SDHCI_HOST_CONTROL);
-}
-#endif
-
 static void esdhc_of_platform_init(struct sdhci_host *host)
 {
 	u32 vvn;
@@ -269,7 +254,7 @@ static void esdhc_of_platform_init(struct sdhci_host *host)
 		host->quirks &= ~SDHCI_QUIRK_NO_BUSY_IRQ;
 }
 
-static int esdhc_pltfm_bus_width(struct sdhci_host *host, int width)
+static void esdhc_pltfm_set_bus_width(struct sdhci_host *host, int width)
 {
 	u32 ctrl;
 
@@ -289,8 +274,6 @@ static int esdhc_pltfm_bus_width(struct sdhci_host *host, int width)
 
 	clrsetbits_be32(host->ioaddr + SDHCI_HOST_CONTROL,
 			ESDHC_CTRL_BUSWIDTH_MASK, ctrl);
-
-	return 0;
 }
 
 static const struct sdhci_ops sdhci_esdhc_ops = {
@@ -305,13 +288,46 @@ static const struct sdhci_ops sdhci_esdhc_ops = {
 	.get_max_clock = esdhc_of_get_max_clock,
 	.get_min_clock = esdhc_of_get_min_clock,
 	.platform_init = esdhc_of_platform_init,
-#ifdef CONFIG_PM
-	.platform_suspend = esdhc_of_suspend,
-	.platform_resume = esdhc_of_resume,
-#endif
 	.adma_workaround = esdhci_of_adma_workaround,
-	.platform_bus_width = esdhc_pltfm_bus_width,
+	.set_bus_width = esdhc_pltfm_set_bus_width,
+	.reset = sdhci_reset,
+	.set_uhs_signaling = sdhci_set_uhs_signaling,
+};
+
+#ifdef CONFIG_PM
+
+static u32 esdhc_proctl;
+static int esdhc_of_suspend(struct device *dev)
+{
+	struct sdhci_host *host = dev_get_drvdata(dev);
+
+	esdhc_proctl = sdhci_be32bs_readl(host, SDHCI_HOST_CONTROL);
+
+	return sdhci_suspend_host(host);
+}
+
+static int esdhc_of_resume(struct device *dev)
+{
+	struct sdhci_host *host = dev_get_drvdata(dev);
+	int ret = sdhci_resume_host(host);
+
+	if (ret == 0) {
+		/* Isn't this already done by sdhci_resume_host() ? --rmk */
+		esdhc_of_enable_dma(host);
+		sdhci_be32bs_writel(host, esdhc_proctl, SDHCI_HOST_CONTROL);
+	}
+
+	return ret;
+}
+
+static const struct dev_pm_ops esdhc_pmops = {
+	.suspend	= esdhc_of_suspend,
+	.resume		= esdhc_of_resume,
 };
+#define ESDHC_PMOPS (&esdhc_pmops)
+#else
+#define ESDHC_PMOPS NULL
+#endif
 
 static const struct sdhci_pltfm_data sdhci_esdhc_pdata = {
 	/*
@@ -374,7 +390,7 @@ static struct platform_driver sdhci_esdhc_driver = {
 		.name = "sdhci-esdhc",
 		.owner = THIS_MODULE,
 		.of_match_table = sdhci_esdhc_of_match,
-		.pm = SDHCI_PLTFM_PMOPS,
+		.pm = ESDHC_PMOPS,
 	},
 	.probe = sdhci_esdhc_probe,
 	.remove = sdhci_esdhc_remove,
diff --git a/drivers/mmc/host/sdhci-of-hlwd.c b/drivers/mmc/host/sdhci-of-hlwd.c
index 57c514a81ca..b341661369a 100644
--- a/drivers/mmc/host/sdhci-of-hlwd.c
+++ b/drivers/mmc/host/sdhci-of-hlwd.c
@@ -58,6 +58,10 @@ static const struct sdhci_ops sdhci_hlwd_ops = {
 	.write_l = sdhci_hlwd_writel,
 	.write_w = sdhci_hlwd_writew,
 	.write_b = sdhci_hlwd_writeb,
+	.set_clock = sdhci_set_clock,
+	.set_bus_width = sdhci_set_bus_width,
+	.reset = sdhci_reset,
+	.set_uhs_signaling = sdhci_set_uhs_signaling,
 };
 
 static const struct sdhci_pltfm_data sdhci_hlwd_pdata = {
diff --git a/drivers/mmc/host/sdhci-pci-o2micro.c b/drivers/mmc/host/sdhci-pci-o2micro.c
index f49666bcc52..5670e381b0c 100644
--- a/drivers/mmc/host/sdhci-pci-o2micro.c
+++ b/drivers/mmc/host/sdhci-pci-o2micro.c
@@ -21,6 +21,45 @@
 #include "sdhci-pci.h"
 #include "sdhci-pci-o2micro.h"
 
+static void o2_pci_set_baseclk(struct sdhci_pci_chip *chip, u32 value)
+{
+	u32 scratch_32;
+	pci_read_config_dword(chip->pdev,
+			      O2_SD_PLL_SETTING, &scratch_32);
+
+	scratch_32 &= 0x0000FFFF;
+	scratch_32 |= value;
+
+	pci_write_config_dword(chip->pdev,
+			       O2_SD_PLL_SETTING, scratch_32);
+}
+
+static void o2_pci_led_enable(struct sdhci_pci_chip *chip)
+{
+	int ret;
+	u32 scratch_32;
+
+	/* Set led of SD host function enable */
+	ret = pci_read_config_dword(chip->pdev,
+				    O2_SD_FUNC_REG0, &scratch_32);
+	if (ret)
+		return;
+
+	scratch_32 &= ~O2_SD_FREG0_LEDOFF;
+	pci_write_config_dword(chip->pdev,
+			       O2_SD_FUNC_REG0, scratch_32);
+
+	ret = pci_read_config_dword(chip->pdev,
+				    O2_SD_TEST_REG, &scratch_32);
+	if (ret)
+		return;
+
+	scratch_32 |= O2_SD_LED_ENABLE;
+	pci_write_config_dword(chip->pdev,
+			       O2_SD_TEST_REG, scratch_32);
+
+}
+
 void sdhci_pci_o2_fujin2_pci_init(struct sdhci_pci_chip *chip)
 {
 	u32 scratch_32;
@@ -216,6 +255,40 @@ int sdhci_pci_o2_probe(struct sdhci_pci_chip *chip)
 		scratch &= 0x7f;
 		pci_write_config_byte(chip->pdev, O2_SD_LOCK_WP, scratch);
 
+		/* DevId=8520 subId= 0x11 or 0x12  Type Chip support */
+		if (chip->pdev->device == PCI_DEVICE_ID_O2_FUJIN2) {
+			ret = pci_read_config_dword(chip->pdev,
+						    O2_SD_FUNC_REG0,
+						    &scratch_32);
+			scratch_32 = ((scratch_32 & 0xFF000000) >> 24);
+
+			/* Check Whether subId is 0x11 or 0x12 */
+			if ((scratch_32 == 0x11) || (scratch_32 == 0x12)) {
+				scratch_32 = 0x2c280000;
+
+				/* Set Base Clock to 208MZ */
+				o2_pci_set_baseclk(chip, scratch_32);
+				ret = pci_read_config_dword(chip->pdev,
+							    O2_SD_FUNC_REG4,
+							    &scratch_32);
+
+				/* Enable Base Clk setting change */
+				scratch_32 |= O2_SD_FREG4_ENABLE_CLK_SET;
+				pci_write_config_dword(chip->pdev,
+						       O2_SD_FUNC_REG4,
+						       scratch_32);
+
+				/* Set Tuning Window to 4 */
+				pci_write_config_byte(chip->pdev,
+						      O2_SD_TUNING_CTRL, 0x44);
+
+				break;
+			}
+		}
+
+		/* Enable 8520 led function */
+		o2_pci_led_enable(chip);
+
 		/* Set timeout CLK */
 		ret = pci_read_config_dword(chip->pdev,
 					    O2_SD_CLK_SETTING, &scratch_32);
@@ -276,7 +349,7 @@ int sdhci_pci_o2_probe(struct sdhci_pci_chip *chip)
 		pci_write_config_byte(chip->pdev, O2_SD_LOCK_WP, scratch);
 
 		ret = pci_read_config_dword(chip->pdev,
-					    O2_SD_FUNC_REG0, &scratch_32);
+					    O2_SD_PLL_SETTING, &scratch_32);
 
 		if ((scratch_32 & 0xff000000) == 0x01000000) {
 			scratch_32 &= 0x0000FFFF;
@@ -299,6 +372,9 @@ int sdhci_pci_o2_probe(struct sdhci_pci_chip *chip)
 					       O2_SD_FUNC_REG4, scratch_32);
 		}
 
+		/* Set Tuning Windows to 5 */
+		pci_write_config_byte(chip->pdev,
+				O2_SD_TUNING_CTRL, 0x55);
 		/* Lock WP */
 		ret = pci_read_config_byte(chip->pdev,
 					   O2_SD_LOCK_WP, &scratch);
diff --git a/drivers/mmc/host/sdhci-pci-o2micro.h b/drivers/mmc/host/sdhci-pci-o2micro.h
index dbec4c93348..f7ffc908d9a 100644
--- a/drivers/mmc/host/sdhci-pci-o2micro.h
+++ b/drivers/mmc/host/sdhci-pci-o2micro.h
@@ -57,6 +57,9 @@
 #define O2_SD_UHS2_L1_CTRL	0x35C
 #define O2_SD_FUNC_REG3		0x3E0
 #define O2_SD_FUNC_REG4		0x3E4
+#define O2_SD_LED_ENABLE	BIT(6)
+#define O2_SD_FREG0_LEDOFF	BIT(13)
+#define O2_SD_FREG4_ENABLE_CLK_SET	BIT(22)
 
 #define O2_SD_VENDOR_SETTING	0x110
 #define O2_SD_VENDOR_SETTING2	0x1C8
diff --git a/drivers/mmc/host/sdhci-pci.c b/drivers/mmc/host/sdhci-pci.c
index fdc61212036..52c42fcc284 100644
--- a/drivers/mmc/host/sdhci-pci.c
+++ b/drivers/mmc/host/sdhci-pci.c
@@ -1031,7 +1031,7 @@ static int sdhci_pci_enable_dma(struct sdhci_host *host)
 	return 0;
 }
 
-static int sdhci_pci_bus_width(struct sdhci_host *host, int width)
+static void sdhci_pci_set_bus_width(struct sdhci_host *host, int width)
 {
 	u8 ctrl;
 
@@ -1052,8 +1052,6 @@ static int sdhci_pci_bus_width(struct sdhci_host *host, int width)
 	}
 
 	sdhci_writeb(host, ctrl, SDHCI_HOST_CONTROL);
-
-	return 0;
 }
 
 static void sdhci_pci_gpio_hw_reset(struct sdhci_host *host)
@@ -1080,8 +1078,11 @@ static void sdhci_pci_hw_reset(struct sdhci_host *host)
 }
 
 static const struct sdhci_ops sdhci_pci_ops = {
+	.set_clock	= sdhci_set_clock,
 	.enable_dma	= sdhci_pci_enable_dma,
-	.platform_bus_width	= sdhci_pci_bus_width,
+	.set_bus_width	= sdhci_pci_set_bus_width,
+	.reset		= sdhci_reset,
+	.set_uhs_signaling = sdhci_set_uhs_signaling,
 	.hw_reset		= sdhci_pci_hw_reset,
 };
 
diff --git a/drivers/mmc/host/sdhci-pltfm.c b/drivers/mmc/host/sdhci-pltfm.c
index bef250e9541..7e834fb78f4 100644
--- a/drivers/mmc/host/sdhci-pltfm.c
+++ b/drivers/mmc/host/sdhci-pltfm.c
@@ -45,6 +45,10 @@ unsigned int sdhci_pltfm_clk_get_max_clock(struct sdhci_host *host)
 EXPORT_SYMBOL_GPL(sdhci_pltfm_clk_get_max_clock);
 
 static const struct sdhci_ops sdhci_pltfm_ops = {
+	.set_clock = sdhci_set_clock,
+	.set_bus_width = sdhci_set_bus_width,
+	.reset = sdhci_reset,
+	.set_uhs_signaling = sdhci_set_uhs_signaling,
 };
 
 #ifdef CONFIG_OF
diff --git a/drivers/mmc/host/sdhci-pxav2.c b/drivers/mmc/host/sdhci-pxav2.c
index d51e061ec57..3c0f3c0a1cc 100644
--- a/drivers/mmc/host/sdhci-pxav2.c
+++ b/drivers/mmc/host/sdhci-pxav2.c
@@ -51,11 +51,13 @@
 #define MMC_CARD		0x1000
 #define MMC_WIDTH		0x0100
 
-static void pxav2_set_private_registers(struct sdhci_host *host, u8 mask)
+static void pxav2_reset(struct sdhci_host *host, u8 mask)
 {
 	struct platform_device *pdev = to_platform_device(mmc_dev(host->mmc));
 	struct sdhci_pxa_platdata *pdata = pdev->dev.platform_data;
 
+	sdhci_reset(host, mask);
+
 	if (mask == SDHCI_RESET_ALL) {
 		u16 tmp = 0;
 
@@ -88,7 +90,7 @@ static void pxav2_set_private_registers(struct sdhci_host *host, u8 mask)
 	}
 }
 
-static int pxav2_mmc_set_width(struct sdhci_host *host, int width)
+static void pxav2_mmc_set_bus_width(struct sdhci_host *host, int width)
 {
 	u8 ctrl;
 	u16 tmp;
@@ -107,14 +109,14 @@ static int pxav2_mmc_set_width(struct sdhci_host *host, int width)
 	}
 	writew(tmp, host->ioaddr + SD_CE_ATA_2);
 	writeb(ctrl, host->ioaddr + SDHCI_HOST_CONTROL);
-
-	return 0;
 }
 
 static const struct sdhci_ops pxav2_sdhci_ops = {
+	.set_clock     = sdhci_set_clock,
 	.get_max_clock = sdhci_pltfm_clk_get_max_clock,
-	.platform_reset_exit = pxav2_set_private_registers,
-	.platform_bus_width = pxav2_mmc_set_width,
+	.set_bus_width = pxav2_mmc_set_bus_width,
+	.reset         = pxav2_reset,
+	.set_uhs_signaling = sdhci_set_uhs_signaling,
 };
 
 #ifdef CONFIG_OF
diff --git a/drivers/mmc/host/sdhci-pxav3.c b/drivers/mmc/host/sdhci-pxav3.c
index 2fd73b38c30..f4f12894756 100644
--- a/drivers/mmc/host/sdhci-pxav3.c
+++ b/drivers/mmc/host/sdhci-pxav3.c
@@ -112,11 +112,13 @@ static int mv_conf_mbus_windows(struct platform_device *pdev,
 	return 0;
 }
 
-static void pxav3_set_private_registers(struct sdhci_host *host, u8 mask)
+static void pxav3_reset(struct sdhci_host *host, u8 mask)
 {
 	struct platform_device *pdev = to_platform_device(mmc_dev(host->mmc));
 	struct sdhci_pxa_platdata *pdata = pdev->dev.platform_data;
 
+	sdhci_reset(host, mask);
+
 	if (mask == SDHCI_RESET_ALL) {
 		/*
 		 * tune timing of read data/command when crc error happen
@@ -184,7 +186,7 @@ static void pxav3_gen_init_74_clocks(struct sdhci_host *host, u8 power_mode)
 	pxa->power_mode = power_mode;
 }
 
-static int pxav3_set_uhs_signaling(struct sdhci_host *host, unsigned int uhs)
+static void pxav3_set_uhs_signaling(struct sdhci_host *host, unsigned int uhs)
 {
 	u16 ctrl_2;
 
@@ -218,15 +220,16 @@ static int pxav3_set_uhs_signaling(struct sdhci_host *host, unsigned int uhs)
 	dev_dbg(mmc_dev(host->mmc),
 		"%s uhs = %d, ctrl_2 = %04X\n",
 		__func__, uhs, ctrl_2);
-
-	return 0;
 }
 
 static const struct sdhci_ops pxav3_sdhci_ops = {
-	.platform_reset_exit = pxav3_set_private_registers,
+	.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,
 };
 
 static struct sdhci_pltfm_data sdhci_pxav3_pdata = {
diff --git a/drivers/mmc/host/sdhci-s3c.c b/drivers/mmc/host/sdhci-s3c.c
index d61eb5a7083..fa5954a0544 100644
--- a/drivers/mmc/host/sdhci-s3c.c
+++ b/drivers/mmc/host/sdhci-s3c.c
@@ -33,9 +33,6 @@
 
 #define MAX_BUS_CLK	(4)
 
-/* Number of gpio's used is max data bus width + command and clock lines */
-#define NUM_GPIOS(x)	(x + 2)
-
 /**
  * struct sdhci_s3c - S3C SDHCI instance
  * @host: The SDHCI host created
@@ -58,6 +55,8 @@ struct sdhci_s3c {
 	struct clk		*clk_io;
 	struct clk		*clk_bus[MAX_BUS_CLK];
 	unsigned long		clk_rates[MAX_BUS_CLK];
+
+	bool			no_divider;
 };
 
 /**
@@ -70,6 +69,7 @@ struct sdhci_s3c {
  */
 struct sdhci_s3c_drv_data {
 	unsigned int	sdhci_quirks;
+	bool		no_divider;
 };
 
 static inline struct sdhci_s3c *to_s3c(struct sdhci_host *host)
@@ -119,7 +119,7 @@ static unsigned int sdhci_s3c_consider_clock(struct sdhci_s3c *ourhost,
 	 * If controller uses a non-standard clock division, find the best clock
 	 * speed possible with selected clock source and skip the division.
 	 */
-	if (ourhost->host->quirks & SDHCI_QUIRK_NONSTANDARD_CLOCK) {
+	if (ourhost->no_divider) {
 		rate = clk_round_rate(clksrc, wanted);
 		return wanted - rate;
 	}
@@ -161,9 +161,13 @@ static void sdhci_s3c_set_clock(struct sdhci_host *host, unsigned int clock)
 	int src;
 	u32 ctrl;
 
+	host->mmc->actual_clock = 0;
+
 	/* don't bother if the clock is going off. */
-	if (clock == 0)
+	if (clock == 0) {
+		sdhci_set_clock(host, clock);
 		return;
+	}
 
 	for (src = 0; src < MAX_BUS_CLK; src++) {
 		delta = sdhci_s3c_consider_clock(ourhost, src, clock);
@@ -215,6 +219,8 @@ static void sdhci_s3c_set_clock(struct sdhci_host *host, unsigned int clock)
 	if (clock < 25 * 1000000)
 		ctrl |= (S3C_SDHCI_CTRL3_FCSEL3 | S3C_SDHCI_CTRL3_FCSEL2);
 	writel(ctrl, host->ioaddr + S3C_SDHCI_CONTROL3);
+
+	sdhci_set_clock(host, clock);
 }
 
 /**
@@ -295,10 +301,11 @@ static void sdhci_cmu_set_clock(struct sdhci_host *host, unsigned int clock)
 	unsigned long timeout;
 	u16 clk = 0;
 
+	host->mmc->actual_clock = 0;
+
 	/* If the clock is going off, set to 0 at clock control register */
 	if (clock == 0) {
 		sdhci_writew(host, 0, SDHCI_CLOCK_CONTROL);
-		host->clock = clock;
 		return;
 	}
 
@@ -306,8 +313,6 @@ static void sdhci_cmu_set_clock(struct sdhci_host *host, unsigned int clock)
 
 	clk_set_rate(ourhost->clk_bus[ourhost->cur_clk], clock);
 
-	host->clock = clock;
-
 	clk = SDHCI_CLOCK_INT_EN;
 	sdhci_writew(host, clk, SDHCI_CLOCK_CONTROL);
 
@@ -329,14 +334,14 @@ static void sdhci_cmu_set_clock(struct sdhci_host *host, unsigned int clock)
 }
 
 /**
- * sdhci_s3c_platform_bus_width - support 8bit buswidth
+ * sdhci_s3c_set_bus_width - support 8bit buswidth
  * @host: The SDHCI host being queried
  * @width: MMC_BUS_WIDTH_ macro for the bus width being requested
  *
  * We have 8-bit width support but is not a v3 controller.
  * So we add platform_bus_width() and support 8bit width.
  */
-static int sdhci_s3c_platform_bus_width(struct sdhci_host *host, int width)
+static void sdhci_s3c_set_bus_width(struct sdhci_host *host, int width)
 {
 	u8 ctrl;
 
@@ -358,93 +363,23 @@ static int sdhci_s3c_platform_bus_width(struct sdhci_host *host, int width)
 	}
 
 	sdhci_writeb(host, ctrl, SDHCI_HOST_CONTROL);
-
-	return 0;
 }
 
 static struct sdhci_ops sdhci_s3c_ops = {
 	.get_max_clock		= sdhci_s3c_get_max_clk,
 	.set_clock		= sdhci_s3c_set_clock,
 	.get_min_clock		= sdhci_s3c_get_min_clock,
-	.platform_bus_width	= sdhci_s3c_platform_bus_width,
+	.set_bus_width		= sdhci_s3c_set_bus_width,
+	.reset			= sdhci_reset,
+	.set_uhs_signaling	= sdhci_set_uhs_signaling,
 };
 
-static void sdhci_s3c_notify_change(struct platform_device *dev, int state)
-{
-	struct sdhci_host *host = platform_get_drvdata(dev);
-#ifdef CONFIG_PM_RUNTIME
-	struct sdhci_s3c *sc = sdhci_priv(host);
-#endif
-	unsigned long flags;
-
-	if (host) {
-		spin_lock_irqsave(&host->lock, flags);
-		if (state) {
-			dev_dbg(&dev->dev, "card inserted.\n");
-#ifdef CONFIG_PM_RUNTIME
-			clk_prepare_enable(sc->clk_io);
-#endif
-			host->flags &= ~SDHCI_DEVICE_DEAD;
-			host->quirks |= SDHCI_QUIRK_BROKEN_CARD_DETECTION;
-		} else {
-			dev_dbg(&dev->dev, "card removed.\n");
-			host->flags |= SDHCI_DEVICE_DEAD;
-			host->quirks &= ~SDHCI_QUIRK_BROKEN_CARD_DETECTION;
-#ifdef CONFIG_PM_RUNTIME
-			clk_disable_unprepare(sc->clk_io);
-#endif
-		}
-		tasklet_schedule(&host->card_tasklet);
-		spin_unlock_irqrestore(&host->lock, flags);
-	}
-}
-
-static irqreturn_t sdhci_s3c_gpio_card_detect_thread(int irq, void *dev_id)
-{
-	struct sdhci_s3c *sc = dev_id;
-	int status = gpio_get_value(sc->ext_cd_gpio);
-	if (sc->pdata->ext_cd_gpio_invert)
-		status = !status;
-	sdhci_s3c_notify_change(sc->pdev, status);
-	return IRQ_HANDLED;
-}
-
-static void sdhci_s3c_setup_card_detect_gpio(struct sdhci_s3c *sc)
-{
-	struct s3c_sdhci_platdata *pdata = sc->pdata;
-	struct device *dev = &sc->pdev->dev;
-
-	if (devm_gpio_request(dev, pdata->ext_cd_gpio, "SDHCI EXT CD") == 0) {
-		sc->ext_cd_gpio = pdata->ext_cd_gpio;
-		sc->ext_cd_irq = gpio_to_irq(pdata->ext_cd_gpio);
-		if (sc->ext_cd_irq &&
-		    request_threaded_irq(sc->ext_cd_irq, NULL,
-					 sdhci_s3c_gpio_card_detect_thread,
-					 IRQF_TRIGGER_RISING |
-					 IRQF_TRIGGER_FALLING |
-					 IRQF_ONESHOT,
-					 dev_name(dev), sc) == 0) {
-			int status = gpio_get_value(sc->ext_cd_gpio);
-			if (pdata->ext_cd_gpio_invert)
-				status = !status;
-			sdhci_s3c_notify_change(sc->pdev, status);
-		} else {
-			dev_warn(dev, "cannot request irq for card detect\n");
-			sc->ext_cd_irq = 0;
-		}
-	} else {
-		dev_err(dev, "cannot request gpio for card detect\n");
-	}
-}
-
 #ifdef CONFIG_OF
 static int sdhci_s3c_parse_dt(struct device *dev,
 		struct sdhci_host *host, struct s3c_sdhci_platdata *pdata)
 {
 	struct device_node *node = dev->of_node;
-	struct sdhci_s3c *ourhost = to_s3c(host);
 	u32 max_width;
-	int gpio;
 
 	/* if the bus-width property is not specified, assume width as 1 */
 	if (of_property_read_u32(node, "bus-width", &max_width))
@@ -462,18 +397,8 @@ static int sdhci_s3c_parse_dt(struct device *dev,
 		return 0;
 	}
 
-	gpio = of_get_named_gpio(node, "cd-gpios", 0);
-	if (gpio_is_valid(gpio)) {
-		pdata->cd_type = S3C_SDHCI_CD_GPIO;
-		pdata->ext_cd_gpio = gpio;
-		ourhost->ext_cd_gpio = -1;
-		if (of_get_property(node, "cd-inverted", NULL))
-			pdata->ext_cd_gpio_invert = 1;
+	if (of_get_named_gpio(node, "cd-gpios", 0))
 		return 0;
-	} else if (gpio != -ENOENT) {
-		dev_err(dev, "invalid card detect gpio specified\n");
-		return -EINVAL;
-	}
 
 	/* assuming internal card detect that will be configured by pinctrl */
 	pdata->cd_type = S3C_SDHCI_CD_INTERNAL;
@@ -606,8 +531,10 @@ static int sdhci_s3c_probe(struct platform_device *pdev)
 	/* Setup quirks for the controller */
 	host->quirks |= SDHCI_QUIRK_NO_ENDATTR_IN_NOPDESC;
 	host->quirks |= SDHCI_QUIRK_NO_HISPD_BIT;
-	if (drv_data)
+	if (drv_data) {
 		host->quirks |= drv_data->sdhci_quirks;
+		sc->no_divider = drv_data->no_divider;
+	}
 
 #ifndef CONFIG_MMC_SDHCI_S3C_DMA
 
@@ -656,7 +583,7 @@ static int sdhci_s3c_probe(struct platform_device *pdev)
 	 * If controller does not have internal clock divider,
 	 * we can use overriding functions instead of default.
 	 */
-	if (host->quirks & SDHCI_QUIRK_NONSTANDARD_CLOCK) {
+	if (sc->no_divider) {
 		sdhci_s3c_ops.set_clock = sdhci_cmu_set_clock;
 		sdhci_s3c_ops.get_min_clock = sdhci_cmu_get_min_clock;
 		sdhci_s3c_ops.get_max_clock = sdhci_cmu_get_max_clock;
@@ -674,6 +601,8 @@ static int sdhci_s3c_probe(struct platform_device *pdev)
 	pm_runtime_use_autosuspend(&pdev->dev);
 	pm_suspend_ignore_children(&pdev->dev, 1);
 
+	mmc_of_parse(host->mmc);
+
 	ret = sdhci_add_host(host);
 	if (ret) {
 		dev_err(dev, "sdhci_add_host() failed\n");
@@ -682,15 +611,6 @@ static int sdhci_s3c_probe(struct platform_device *pdev)
 		goto err_req_regs;
 	}
 
-	/* The following two methods of card detection might call
-	   sdhci_s3c_notify_change() immediately, so they can be called
-	   only after sdhci_add_host(). Setup errors are ignored. */
-	if (pdata->cd_type == S3C_SDHCI_CD_EXTERNAL && pdata->ext_cd_init)
-		pdata->ext_cd_init(&sdhci_s3c_notify_change);
-	if (pdata->cd_type == S3C_SDHCI_CD_GPIO &&
-	    gpio_is_valid(pdata->ext_cd_gpio))
-		sdhci_s3c_setup_card_detect_gpio(sc);
-
 #ifdef CONFIG_PM_RUNTIME
 	if (pdata->cd_type != S3C_SDHCI_CD_INTERNAL)
 		clk_disable_unprepare(sc->clk_io);
@@ -711,16 +631,12 @@ static int sdhci_s3c_remove(struct platform_device *pdev)
 {
 	struct sdhci_host *host =  platform_get_drvdata(pdev);
 	struct sdhci_s3c *sc = sdhci_priv(host);
-	struct s3c_sdhci_platdata *pdata = sc->pdata;
-
-	if (pdata->cd_type == S3C_SDHCI_CD_EXTERNAL && pdata->ext_cd_cleanup)
-		pdata->ext_cd_cleanup(&sdhci_s3c_notify_change);
 
 	if (sc->ext_cd_irq)
 		free_irq(sc->ext_cd_irq, sc);
 
 #ifdef CONFIG_PM_RUNTIME
-	if (pdata->cd_type != S3C_SDHCI_CD_INTERNAL)
+	if (sc->pdata->cd_type != S3C_SDHCI_CD_INTERNAL)
 		clk_prepare_enable(sc->clk_io);
 #endif
 	sdhci_remove_host(host, 1);
@@ -797,7 +713,7 @@ static const struct dev_pm_ops sdhci_s3c_pmops = {
 
 #if defined(CONFIG_CPU_EXYNOS4210) || defined(CONFIG_SOC_EXYNOS4212)
 static struct sdhci_s3c_drv_data exynos4_sdhci_drv_data = {
-	.sdhci_quirks = SDHCI_QUIRK_NONSTANDARD_CLOCK,
+	.no_divider = true,
 };
 #define EXYNOS4_SDHCI_DRV_DATA ((kernel_ulong_t)&exynos4_sdhci_drv_data)
 #else
diff --git a/drivers/mmc/host/sdhci-sirf.c b/drivers/mmc/host/sdhci-sirf.c
index 696122c1b46..17004531d08 100644
--- a/drivers/mmc/host/sdhci-sirf.c
+++ b/drivers/mmc/host/sdhci-sirf.c
@@ -28,7 +28,11 @@ static unsigned int sdhci_sirf_get_max_clk(struct sdhci_host *host)
 }
 
 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,
+	.reset = sdhci_reset,
+	.set_uhs_signaling = sdhci_set_uhs_signaling,
 };
 
 static struct sdhci_pltfm_data sdhci_sirf_pdata = {
diff --git a/drivers/mmc/host/sdhci-spear.c b/drivers/mmc/host/sdhci-spear.c
index 0316dec3f00..9d535c7336e 100644
--- a/drivers/mmc/host/sdhci-spear.c
+++ b/drivers/mmc/host/sdhci-spear.c
@@ -38,7 +38,10 @@ struct spear_sdhci {
 
 /* sdhci ops */
 static const struct sdhci_ops sdhci_pltfm_ops = {
-	/* Nothing to do for now. */
+	.set_clock = sdhci_set_clock,
+	.set_bus_width = sdhci_set_bus_width,
+	.reset = sdhci_reset,
+	.set_uhs_signaling = sdhci_set_uhs_signaling,
 };
 
 #ifdef CONFIG_OF
diff --git a/drivers/mmc/host/sdhci-tegra.c b/drivers/mmc/host/sdhci-tegra.c
index a835898a68d..d93a063a36f 100644
--- a/drivers/mmc/host/sdhci-tegra.c
+++ b/drivers/mmc/host/sdhci-tegra.c
@@ -32,11 +32,17 @@
 
 /* Tegra SDHOST controller vendor register definitions */
 #define SDHCI_TEGRA_VENDOR_MISC_CTRL		0x120
+#define SDHCI_MISC_CTRL_ENABLE_SDR104		0x8
+#define SDHCI_MISC_CTRL_ENABLE_SDR50		0x10
 #define SDHCI_MISC_CTRL_ENABLE_SDHCI_SPEC_300	0x20
+#define SDHCI_MISC_CTRL_ENABLE_DDR50		0x200
 
 #define NVQUIRK_FORCE_SDHCI_SPEC_200	BIT(0)
 #define NVQUIRK_ENABLE_BLOCK_GAP_DET	BIT(1)
 #define NVQUIRK_ENABLE_SDHCI_SPEC_300	BIT(2)
+#define NVQUIRK_DISABLE_SDR50		BIT(3)
+#define NVQUIRK_DISABLE_SDR104		BIT(4)
+#define NVQUIRK_DISABLE_DDR50		BIT(5)
 
 struct sdhci_tegra_soc_data {
 	const struct sdhci_pltfm_data *pdata;
@@ -48,19 +54,6 @@ struct sdhci_tegra {
 	int power_gpio;
 };
 
-static u32 tegra_sdhci_readl(struct sdhci_host *host, int reg)
-{
-	u32 val;
-
-	if (unlikely(reg == SDHCI_PRESENT_STATE)) {
-		/* Use wp_gpio here instead? */
-		val = readl(host->ioaddr + reg);
-		return val | SDHCI_WRITE_PROTECT;
-	}
-
-	return readl(host->ioaddr + reg);
-}
-
 static u16 tegra_sdhci_readw(struct sdhci_host *host, int reg)
 {
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
@@ -108,26 +101,33 @@ static unsigned int tegra_sdhci_get_ro(struct sdhci_host *host)
 	return mmc_gpio_get_ro(host->mmc);
 }
 
-static void tegra_sdhci_reset_exit(struct sdhci_host *host, u8 mask)
+static void tegra_sdhci_reset(struct sdhci_host *host, u8 mask)
 {
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
 	struct sdhci_tegra *tegra_host = pltfm_host->priv;
 	const struct sdhci_tegra_soc_data *soc_data = tegra_host->soc_data;
+	u32 misc_ctrl;
+
+	sdhci_reset(host, mask);
 
 	if (!(mask & SDHCI_RESET_ALL))
 		return;
 
+	misc_ctrl = sdhci_readw(host, SDHCI_TEGRA_VENDOR_MISC_CTRL);
 	/* Erratum: Enable SDHCI spec v3.00 support */
-	if (soc_data->nvquirks & NVQUIRK_ENABLE_SDHCI_SPEC_300) {
-		u32 misc_ctrl;
-
-		misc_ctrl = sdhci_readb(host, SDHCI_TEGRA_VENDOR_MISC_CTRL);
+	if (soc_data->nvquirks & NVQUIRK_ENABLE_SDHCI_SPEC_300)
 		misc_ctrl |= SDHCI_MISC_CTRL_ENABLE_SDHCI_SPEC_300;
-		sdhci_writeb(host, misc_ctrl, SDHCI_TEGRA_VENDOR_MISC_CTRL);
-	}
+	/* Don't advertise UHS modes which aren't supported yet */
+	if (soc_data->nvquirks & NVQUIRK_DISABLE_SDR50)
+		misc_ctrl &= ~SDHCI_MISC_CTRL_ENABLE_SDR50;
+	if (soc_data->nvquirks & NVQUIRK_DISABLE_DDR50)
+		misc_ctrl &= ~SDHCI_MISC_CTRL_ENABLE_DDR50;
+	if (soc_data->nvquirks & NVQUIRK_DISABLE_SDR104)
+		misc_ctrl &= ~SDHCI_MISC_CTRL_ENABLE_SDR104;
+	sdhci_writew(host, misc_ctrl, SDHCI_TEGRA_VENDOR_MISC_CTRL);
 }
 
-static int tegra_sdhci_buswidth(struct sdhci_host *host, int bus_width)
+static void tegra_sdhci_set_bus_width(struct sdhci_host *host, int bus_width)
 {
 	u32 ctrl;
 
@@ -144,23 +144,25 @@ static int tegra_sdhci_buswidth(struct sdhci_host *host, int bus_width)
 			ctrl &= ~SDHCI_CTRL_4BITBUS;
 	}
 	sdhci_writeb(host, ctrl, SDHCI_HOST_CONTROL);
-	return 0;
 }
 
 static const struct sdhci_ops tegra_sdhci_ops = {
 	.get_ro     = tegra_sdhci_get_ro,
-	.read_l     = tegra_sdhci_readl,
 	.read_w     = tegra_sdhci_readw,
 	.write_l    = tegra_sdhci_writel,
-	.platform_bus_width = tegra_sdhci_buswidth,
-	.platform_reset_exit = tegra_sdhci_reset_exit,
+	.set_clock  = sdhci_set_clock,
+	.set_bus_width = tegra_sdhci_set_bus_width,
+	.reset      = tegra_sdhci_reset,
+	.set_uhs_signaling = sdhci_set_uhs_signaling,
+	.get_max_clock = sdhci_pltfm_clk_get_max_clock,
 };
 
 static const struct sdhci_pltfm_data sdhci_tegra20_pdata = {
 	.quirks = SDHCI_QUIRK_BROKEN_TIMEOUT_VAL |
 		  SDHCI_QUIRK_SINGLE_POWER_WRITE |
 		  SDHCI_QUIRK_NO_HISPD_BIT |
-		  SDHCI_QUIRK_BROKEN_ADMA_ZEROLEN_DESC,
+		  SDHCI_QUIRK_BROKEN_ADMA_ZEROLEN_DESC |
+		  SDHCI_QUIRK_CAP_CLOCK_BASE_BROKEN,
 	.ops  = &tegra_sdhci_ops,
 };
 
@@ -175,13 +177,16 @@ static const struct sdhci_pltfm_data sdhci_tegra30_pdata = {
 		  SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK |
 		  SDHCI_QUIRK_SINGLE_POWER_WRITE |
 		  SDHCI_QUIRK_NO_HISPD_BIT |
-		  SDHCI_QUIRK_BROKEN_ADMA_ZEROLEN_DESC,
+		  SDHCI_QUIRK_BROKEN_ADMA_ZEROLEN_DESC |
+		  SDHCI_QUIRK_CAP_CLOCK_BASE_BROKEN,
 	.ops  = &tegra_sdhci_ops,
 };
 
 static struct sdhci_tegra_soc_data soc_data_tegra30 = {
 	.pdata = &sdhci_tegra30_pdata,
-	.nvquirks = NVQUIRK_ENABLE_SDHCI_SPEC_300,
+	.nvquirks = NVQUIRK_ENABLE_SDHCI_SPEC_300 |
+		    NVQUIRK_DISABLE_SDR50 |
+		    NVQUIRK_DISABLE_SDR104,
 };
 
 static const struct sdhci_pltfm_data sdhci_tegra114_pdata = {
@@ -189,12 +194,16 @@ static const struct sdhci_pltfm_data sdhci_tegra114_pdata = {
 		  SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK |
 		  SDHCI_QUIRK_SINGLE_POWER_WRITE |
 		  SDHCI_QUIRK_NO_HISPD_BIT |
-		  SDHCI_QUIRK_BROKEN_ADMA_ZEROLEN_DESC,
+		  SDHCI_QUIRK_BROKEN_ADMA_ZEROLEN_DESC |
+		  SDHCI_QUIRK_CAP_CLOCK_BASE_BROKEN,
 	.ops  = &tegra_sdhci_ops,
 };
 
 static struct sdhci_tegra_soc_data soc_data_tegra114 = {
 	.pdata = &sdhci_tegra114_pdata,
+	.nvquirks = NVQUIRK_DISABLE_SDR50 |
+		    NVQUIRK_DISABLE_DDR50 |
+		    NVQUIRK_DISABLE_SDR104,
 };
 
 static const struct of_device_id sdhci_tegra_dt_match[] = {
diff --git a/drivers/mmc/host/sdhci.c b/drivers/mmc/host/sdhci.c
index 9a79fc4b60c..47055f3f01b 100644
--- a/drivers/mmc/host/sdhci.c
+++ b/drivers/mmc/host/sdhci.c
@@ -44,6 +44,8 @@
 
 #define MAX_TUNING_LOOP 40
 
+#define ADMA_SIZE	((128 * 2 + 1) * 4)
+
 static unsigned int debug_quirks = 0;
 static unsigned int debug_quirks2;
 
@@ -131,43 +133,26 @@ static void sdhci_dumpregs(struct sdhci_host *host)
  *                                                                           *
 \*****************************************************************************/
 
-static void sdhci_clear_set_irqs(struct sdhci_host *host, u32 clear, u32 set)
-{
-	u32 ier;
-
-	ier = sdhci_readl(host, SDHCI_INT_ENABLE);
-	ier &= ~clear;
-	ier |= set;
-	sdhci_writel(host, ier, SDHCI_INT_ENABLE);
-	sdhci_writel(host, ier, SDHCI_SIGNAL_ENABLE);
-}
-
-static void sdhci_unmask_irqs(struct sdhci_host *host, u32 irqs)
-{
-	sdhci_clear_set_irqs(host, 0, irqs);
-}
-
-static void sdhci_mask_irqs(struct sdhci_host *host, u32 irqs)
-{
-	sdhci_clear_set_irqs(host, irqs, 0);
-}
-
 static void sdhci_set_card_detection(struct sdhci_host *host, bool enable)
 {
-	u32 present, irqs;
+	u32 present;
 
 	if ((host->quirks & SDHCI_QUIRK_BROKEN_CARD_DETECTION) ||
 	    (host->mmc->caps & MMC_CAP_NONREMOVABLE))
 		return;
 
-	present = sdhci_readl(host, SDHCI_PRESENT_STATE) &
-			      SDHCI_CARD_PRESENT;
-	irqs = present ? SDHCI_INT_CARD_REMOVE : SDHCI_INT_CARD_INSERT;
+	if (enable) {
+		present = sdhci_readl(host, SDHCI_PRESENT_STATE) &
+				      SDHCI_CARD_PRESENT;
 
-	if (enable)
-		sdhci_unmask_irqs(host, irqs);
-	else
-		sdhci_mask_irqs(host, irqs);
+		host->ier |= present ? SDHCI_INT_CARD_REMOVE :
+				       SDHCI_INT_CARD_INSERT;
+	} else {
+		host->ier &= ~(SDHCI_INT_CARD_REMOVE | SDHCI_INT_CARD_INSERT);
+	}
+
+	sdhci_writel(host, host->ier, SDHCI_INT_ENABLE);
+	sdhci_writel(host, host->ier, SDHCI_SIGNAL_ENABLE);
 }
 
 static void sdhci_enable_card_detection(struct sdhci_host *host)
@@ -180,22 +165,9 @@ static void sdhci_disable_card_detection(struct sdhci_host *host)
 	sdhci_set_card_detection(host, false);
 }
 
-static void sdhci_reset(struct sdhci_host *host, u8 mask)
+void sdhci_reset(struct sdhci_host *host, u8 mask)
 {
 	unsigned long timeout;
-	u32 uninitialized_var(ier);
-
-	if (host->quirks & SDHCI_QUIRK_NO_CARD_NO_RESET) {
-		if (!(sdhci_readl(host, SDHCI_PRESENT_STATE) &
-			SDHCI_CARD_PRESENT))
-			return;
-	}
-
-	if (host->quirks & SDHCI_QUIRK_RESTORE_IRQS_AFTER_RESET)
-		ier = sdhci_readl(host, SDHCI_INT_ENABLE);
-
-	if (host->ops->platform_reset_enter)
-		host->ops->platform_reset_enter(host, mask);
 
 	sdhci_writeb(host, mask, SDHCI_SOFTWARE_RESET);
 
@@ -220,16 +192,27 @@ static void sdhci_reset(struct sdhci_host *host, u8 mask)
 		timeout--;
 		mdelay(1);
 	}
+}
+EXPORT_SYMBOL_GPL(sdhci_reset);
+
+static void sdhci_do_reset(struct sdhci_host *host, u8 mask)
+{
+	if (host->quirks & SDHCI_QUIRK_NO_CARD_NO_RESET) {
+		if (!(sdhci_readl(host, SDHCI_PRESENT_STATE) &
+			SDHCI_CARD_PRESENT))
+			return;
+	}
 
-	if (host->ops->platform_reset_exit)
-		host->ops->platform_reset_exit(host, mask);
+	host->ops->reset(host, mask);
 
-	if (host->quirks & SDHCI_QUIRK_RESTORE_IRQS_AFTER_RESET)
-		sdhci_clear_set_irqs(host, SDHCI_INT_ALL_MASK, ier);
+	if (mask & SDHCI_RESET_ALL) {
+		if (host->flags & (SDHCI_USE_SDMA | SDHCI_USE_ADMA)) {
+			if (host->ops->enable_dma)
+				host->ops->enable_dma(host);
+		}
 
-	if (host->flags & (SDHCI_USE_SDMA | SDHCI_USE_ADMA)) {
-		if ((host->ops->enable_dma) && (mask & SDHCI_RESET_ALL))
-			host->ops->enable_dma(host);
+		/* Resetting the controller clears many */
+		host->preset_enabled = false;
 	}
 }
 
@@ -238,15 +221,18 @@ static void sdhci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios);
 static void sdhci_init(struct sdhci_host *host, int soft)
 {
 	if (soft)
-		sdhci_reset(host, SDHCI_RESET_CMD|SDHCI_RESET_DATA);
+		sdhci_do_reset(host, SDHCI_RESET_CMD|SDHCI_RESET_DATA);
 	else
-		sdhci_reset(host, SDHCI_RESET_ALL);
+		sdhci_do_reset(host, SDHCI_RESET_ALL);
 
-	sdhci_clear_set_irqs(host, SDHCI_INT_ALL_MASK,
-		SDHCI_INT_BUS_POWER | SDHCI_INT_DATA_END_BIT |
-		SDHCI_INT_DATA_CRC | SDHCI_INT_DATA_TIMEOUT | SDHCI_INT_INDEX |
-		SDHCI_INT_END_BIT | SDHCI_INT_CRC | SDHCI_INT_TIMEOUT |
-		SDHCI_INT_DATA_END | SDHCI_INT_RESPONSE);
+	host->ier = SDHCI_INT_BUS_POWER | SDHCI_INT_DATA_END_BIT |
+		    SDHCI_INT_DATA_CRC | SDHCI_INT_DATA_TIMEOUT |
+		    SDHCI_INT_INDEX | SDHCI_INT_END_BIT | SDHCI_INT_CRC |
+		    SDHCI_INT_TIMEOUT | SDHCI_INT_DATA_END |
+		    SDHCI_INT_RESPONSE;
+
+	sdhci_writel(host, host->ier, SDHCI_INT_ENABLE);
+	sdhci_writel(host, host->ier, SDHCI_SIGNAL_ENABLE);
 
 	if (soft) {
 		/* force clock reconfiguration */
@@ -502,11 +488,6 @@ static int sdhci_adma_table_pre(struct sdhci_host *host,
 	else
 		direction = DMA_TO_DEVICE;
 
-	/*
-	 * The ADMA descriptor table is mapped further down as we
-	 * need to fill it with data first.
-	 */
-
 	host->align_addr = dma_map_single(mmc_dev(host->mmc),
 		host->align_buffer, 128 * 4, direction);
 	if (dma_mapping_error(mmc_dev(host->mmc), host->align_addr))
@@ -567,7 +548,7 @@ static int sdhci_adma_table_pre(struct sdhci_host *host,
 		 * If this triggers then we have a calculation bug
 		 * somewhere. :/
 		 */
-		WARN_ON((desc - host->adma_desc) > (128 * 2 + 1) * 4);
+		WARN_ON((desc - host->adma_desc) > ADMA_SIZE);
 	}
 
 	if (host->quirks & SDHCI_QUIRK_NO_ENDATTR_IN_NOPDESC) {
@@ -595,17 +576,8 @@ static int sdhci_adma_table_pre(struct sdhci_host *host,
 			host->align_addr, 128 * 4, direction);
 	}
 
-	host->adma_addr = dma_map_single(mmc_dev(host->mmc),
-		host->adma_desc, (128 * 2 + 1) * 4, DMA_TO_DEVICE);
-	if (dma_mapping_error(mmc_dev(host->mmc), host->adma_addr))
-		goto unmap_entries;
-	BUG_ON(host->adma_addr & 0x3);
-
 	return 0;
 
-unmap_entries:
-	dma_unmap_sg(mmc_dev(host->mmc), data->sg,
-		data->sg_len, direction);
 unmap_align:
 	dma_unmap_single(mmc_dev(host->mmc), host->align_addr,
 		128 * 4, direction);
@@ -623,19 +595,25 @@ static void sdhci_adma_table_post(struct sdhci_host *host,
 	u8 *align;
 	char *buffer;
 	unsigned long flags;
+	bool has_unaligned;
 
 	if (data->flags & MMC_DATA_READ)
 		direction = DMA_FROM_DEVICE;
 	else
 		direction = DMA_TO_DEVICE;
 
-	dma_unmap_single(mmc_dev(host->mmc), host->adma_addr,
-		(128 * 2 + 1) * 4, DMA_TO_DEVICE);
-
 	dma_unmap_single(mmc_dev(host->mmc), host->align_addr,
 		128 * 4, direction);
 
-	if (data->flags & MMC_DATA_READ) {
+	/* Do a quick scan of the SG list for any unaligned mappings */
+	has_unaligned = false;
+	for_each_sg(data->sg, sg, host->sg_count, i)
+		if (sg_dma_address(sg) & 3) {
+			has_unaligned = true;
+			break;
+		}
+
+	if (has_unaligned && data->flags & MMC_DATA_READ) {
 		dma_sync_sg_for_cpu(mmc_dev(host->mmc), data->sg,
 			data->sg_len, direction);
 
@@ -721,9 +699,12 @@ static void sdhci_set_transfer_irqs(struct sdhci_host *host)
 	u32 dma_irqs = SDHCI_INT_DMA_END | SDHCI_INT_ADMA_ERROR;
 
 	if (host->flags & SDHCI_REQ_USE_DMA)
-		sdhci_clear_set_irqs(host, pio_irqs, dma_irqs);
+		host->ier = (host->ier & ~pio_irqs) | dma_irqs;
 	else
-		sdhci_clear_set_irqs(host, dma_irqs, pio_irqs);
+		host->ier = (host->ier & ~dma_irqs) | pio_irqs;
+
+	sdhci_writel(host, host->ier, SDHCI_INT_ENABLE);
+	sdhci_writel(host, host->ier, SDHCI_SIGNAL_ENABLE);
 }
 
 static void sdhci_prepare_data(struct sdhci_host *host, struct mmc_command *cmd)
@@ -976,8 +957,8 @@ static void sdhci_finish_data(struct sdhci_host *host)
 		 * upon error conditions.
 		 */
 		if (data->error) {
-			sdhci_reset(host, SDHCI_RESET_CMD);
-			sdhci_reset(host, SDHCI_RESET_DATA);
+			sdhci_do_reset(host, SDHCI_RESET_CMD);
+			sdhci_do_reset(host, SDHCI_RESET_DATA);
 		}
 
 		sdhci_send_command(host, data->stop);
@@ -1107,24 +1088,23 @@ static void sdhci_finish_command(struct sdhci_host *host)
 
 static u16 sdhci_get_preset_value(struct sdhci_host *host)
 {
-	u16 ctrl, preset = 0;
-
-	ctrl = sdhci_readw(host, SDHCI_HOST_CONTROL2);
+	u16 preset = 0;
 
-	switch (ctrl & SDHCI_CTRL_UHS_MASK) {
-	case SDHCI_CTRL_UHS_SDR12:
+	switch (host->timing) {
+	case MMC_TIMING_UHS_SDR12:
 		preset = sdhci_readw(host, SDHCI_PRESET_FOR_SDR12);
 		break;
-	case SDHCI_CTRL_UHS_SDR25:
+	case MMC_TIMING_UHS_SDR25:
 		preset = sdhci_readw(host, SDHCI_PRESET_FOR_SDR25);
 		break;
-	case SDHCI_CTRL_UHS_SDR50:
+	case MMC_TIMING_UHS_SDR50:
 		preset = sdhci_readw(host, SDHCI_PRESET_FOR_SDR50);
 		break;
-	case SDHCI_CTRL_UHS_SDR104:
+	case MMC_TIMING_UHS_SDR104:
+	case MMC_TIMING_MMC_HS200:
 		preset = sdhci_readw(host, SDHCI_PRESET_FOR_SDR104);
 		break;
-	case SDHCI_CTRL_UHS_DDR50:
+	case MMC_TIMING_UHS_DDR50:
 		preset = sdhci_readw(host, SDHCI_PRESET_FOR_DDR50);
 		break;
 	default:
@@ -1136,32 +1116,22 @@ static u16 sdhci_get_preset_value(struct sdhci_host *host)
 	return preset;
 }
 
-static void sdhci_set_clock(struct sdhci_host *host, unsigned int clock)
+void sdhci_set_clock(struct sdhci_host *host, unsigned int clock)
 {
 	int div = 0; /* Initialized for compiler warning */
 	int real_div = div, clk_mul = 1;
 	u16 clk = 0;
 	unsigned long timeout;
 
-	if (clock && clock == host->clock)
-		return;
-
 	host->mmc->actual_clock = 0;
 
-	if (host->ops->set_clock) {
-		host->ops->set_clock(host, clock);
-		if (host->quirks & SDHCI_QUIRK_NONSTANDARD_CLOCK)
-			return;
-	}
-
 	sdhci_writew(host, 0, SDHCI_CLOCK_CONTROL);
 
 	if (clock == 0)
-		goto out;
+		return;
 
 	if (host->version >= SDHCI_SPEC_300) {
-		if (sdhci_readw(host, SDHCI_HOST_CONTROL2) &
-			SDHCI_CTRL_PRESET_VAL_ENABLE) {
+		if (host->preset_enabled) {
 			u16 pre_val;
 
 			clk = sdhci_readw(host, SDHCI_CLOCK_CONTROL);
@@ -1247,26 +1217,16 @@ clock_set:
 
 	clk |= SDHCI_CLOCK_CARD_EN;
 	sdhci_writew(host, clk, SDHCI_CLOCK_CONTROL);
-
-out:
-	host->clock = clock;
 }
+EXPORT_SYMBOL_GPL(sdhci_set_clock);
 
-static inline void sdhci_update_clock(struct sdhci_host *host)
-{
-	unsigned int clock;
-
-	clock = host->clock;
-	host->clock = 0;
-	sdhci_set_clock(host, clock);
-}
-
-static int sdhci_set_power(struct sdhci_host *host, unsigned short power)
+static void sdhci_set_power(struct sdhci_host *host, unsigned char mode,
+			    unsigned short vdd)
 {
 	u8 pwr = 0;
 
-	if (power != (unsigned short)-1) {
-		switch (1 << power) {
+	if (mode != MMC_POWER_OFF) {
+		switch (1 << vdd) {
 		case MMC_VDD_165_195:
 			pwr = SDHCI_POWER_180;
 			break;
@@ -1284,7 +1244,7 @@ static int sdhci_set_power(struct sdhci_host *host, unsigned short power)
 	}
 
 	if (host->pwr == pwr)
-		return -1;
+		return;
 
 	host->pwr = pwr;
 
@@ -1292,38 +1252,43 @@ static int sdhci_set_power(struct sdhci_host *host, unsigned short power)
 		sdhci_writeb(host, 0, SDHCI_POWER_CONTROL);
 		if (host->quirks2 & SDHCI_QUIRK2_CARD_ON_NEEDS_BUS_ON)
 			sdhci_runtime_pm_bus_off(host);
-		return 0;
-	}
-
-	/*
-	 * Spec says that we should clear the power reg before setting
-	 * a new value. Some controllers don't seem to like this though.
-	 */
-	if (!(host->quirks & SDHCI_QUIRK_SINGLE_POWER_WRITE))
-		sdhci_writeb(host, 0, SDHCI_POWER_CONTROL);
+		vdd = 0;
+	} else {
+		/*
+		 * Spec says that we should clear the power reg before setting
+		 * a new value. Some controllers don't seem to like this though.
+		 */
+		if (!(host->quirks & SDHCI_QUIRK_SINGLE_POWER_WRITE))
+			sdhci_writeb(host, 0, SDHCI_POWER_CONTROL);
 
-	/*
-	 * At least the Marvell CaFe chip gets confused if we set the voltage
-	 * and set turn on power at the same time, so set the voltage first.
-	 */
-	if (host->quirks & SDHCI_QUIRK_NO_SIMULT_VDD_AND_POWER)
-		sdhci_writeb(host, pwr, SDHCI_POWER_CONTROL);
+		/*
+		 * At least the Marvell CaFe chip gets confused if we set the
+		 * voltage and set turn on power at the same time, so set the
+		 * voltage first.
+		 */
+		if (host->quirks & SDHCI_QUIRK_NO_SIMULT_VDD_AND_POWER)
+			sdhci_writeb(host, pwr, SDHCI_POWER_CONTROL);
 
-	pwr |= SDHCI_POWER_ON;
+		pwr |= SDHCI_POWER_ON;
 
-	sdhci_writeb(host, pwr, SDHCI_POWER_CONTROL);
+		sdhci_writeb(host, pwr, SDHCI_POWER_CONTROL);
 
-	if (host->quirks2 & SDHCI_QUIRK2_CARD_ON_NEEDS_BUS_ON)
-		sdhci_runtime_pm_bus_on(host);
+		if (host->quirks2 & SDHCI_QUIRK2_CARD_ON_NEEDS_BUS_ON)
+			sdhci_runtime_pm_bus_on(host);
 
-	/*
-	 * Some controllers need an extra 10ms delay of 10ms before they
-	 * can apply clock after applying power
-	 */
-	if (host->quirks & SDHCI_QUIRK_DELAY_AFTER_POWER)
-		mdelay(10);
+		/*
+		 * Some controllers need an extra 10ms delay of 10ms before
+		 * they can apply clock after applying power
+		 */
+		if (host->quirks & SDHCI_QUIRK_DELAY_AFTER_POWER)
+			mdelay(10);
+	}
 
-	return power;
+	if (host->vmmc) {
+		spin_unlock_irq(&host->lock);
+		mmc_regulator_set_ocr(host->mmc, host->vmmc, vdd);
+		spin_lock_irq(&host->lock);
+	}
 }
 
 /*****************************************************************************\
@@ -1427,10 +1392,53 @@ static void sdhci_request(struct mmc_host *mmc, struct mmc_request *mrq)
 	spin_unlock_irqrestore(&host->lock, flags);
 }
 
+void sdhci_set_bus_width(struct sdhci_host *host, int width)
+{
+	u8 ctrl;
+
+	ctrl = sdhci_readb(host, SDHCI_HOST_CONTROL);
+	if (width == MMC_BUS_WIDTH_8) {
+		ctrl &= ~SDHCI_CTRL_4BITBUS;
+		if (host->version >= SDHCI_SPEC_300)
+			ctrl |= SDHCI_CTRL_8BITBUS;
+	} else {
+		if (host->version >= SDHCI_SPEC_300)
+			ctrl &= ~SDHCI_CTRL_8BITBUS;
+		if (width == MMC_BUS_WIDTH_4)
+			ctrl |= SDHCI_CTRL_4BITBUS;
+		else
+			ctrl &= ~SDHCI_CTRL_4BITBUS;
+	}
+	sdhci_writeb(host, ctrl, SDHCI_HOST_CONTROL);
+}
+EXPORT_SYMBOL_GPL(sdhci_set_bus_width);
+
+void sdhci_set_uhs_signaling(struct sdhci_host *host, unsigned timing)
+{
+	u16 ctrl_2;
+
+	ctrl_2 = sdhci_readw(host, SDHCI_HOST_CONTROL2);
+	/* Select Bus Speed Mode for host */
+	ctrl_2 &= ~SDHCI_CTRL_UHS_MASK;
+	if ((timing == MMC_TIMING_MMC_HS200) ||
+	    (timing == MMC_TIMING_UHS_SDR104))
+		ctrl_2 |= SDHCI_CTRL_UHS_SDR104;
+	else if (timing == MMC_TIMING_UHS_SDR12)
+		ctrl_2 |= SDHCI_CTRL_UHS_SDR12;
+	else if (timing == MMC_TIMING_UHS_SDR25)
+		ctrl_2 |= SDHCI_CTRL_UHS_SDR25;
+	else if (timing == MMC_TIMING_UHS_SDR50)
+		ctrl_2 |= SDHCI_CTRL_UHS_SDR50;
+	else if ((timing == MMC_TIMING_UHS_DDR50) ||
+		 (timing == MMC_TIMING_MMC_DDR52))
+		ctrl_2 |= SDHCI_CTRL_UHS_DDR50;
+	sdhci_writew(host, ctrl_2, SDHCI_HOST_CONTROL2);
+}
+EXPORT_SYMBOL_GPL(sdhci_set_uhs_signaling);
+
 static void sdhci_do_set_ios(struct sdhci_host *host, struct mmc_ios *ios)
 {
 	unsigned long flags;
-	int vdd_bit = -1;
 	u8 ctrl;
 
 	spin_lock_irqsave(&host->lock, flags);
@@ -1456,45 +1464,17 @@ static void sdhci_do_set_ios(struct sdhci_host *host, struct mmc_ios *ios)
 		!(host->quirks2 & SDHCI_QUIRK2_PRESET_VALUE_BROKEN))
 		sdhci_enable_preset_value(host, false);
 
-	sdhci_set_clock(host, ios->clock);
-
-	if (ios->power_mode == MMC_POWER_OFF)
-		vdd_bit = sdhci_set_power(host, -1);
-	else
-		vdd_bit = sdhci_set_power(host, ios->vdd);
-
-	if (host->vmmc && vdd_bit != -1) {
-		spin_unlock_irqrestore(&host->lock, flags);
-		mmc_regulator_set_ocr(host->mmc, host->vmmc, vdd_bit);
-		spin_lock_irqsave(&host->lock, flags);
+	if (!ios->clock || ios->clock != host->clock) {
+		host->ops->set_clock(host, ios->clock);
+		host->clock = ios->clock;
 	}
 
+	sdhci_set_power(host, ios->power_mode, ios->vdd);
+
 	if (host->ops->platform_send_init_74_clocks)
 		host->ops->platform_send_init_74_clocks(host, ios->power_mode);
 
-	/*
-	 * If your platform has 8-bit width support but is not a v3 controller,
-	 * or if it requires special setup code, you should implement that in
-	 * platform_bus_width().
-	 */
-	if (host->ops->platform_bus_width) {
-		host->ops->platform_bus_width(host, ios->bus_width);
-	} else {
-		ctrl = sdhci_readb(host, SDHCI_HOST_CONTROL);
-		if (ios->bus_width == MMC_BUS_WIDTH_8) {
-			ctrl &= ~SDHCI_CTRL_4BITBUS;
-			if (host->version >= SDHCI_SPEC_300)
-				ctrl |= SDHCI_CTRL_8BITBUS;
-		} else {
-			if (host->version >= SDHCI_SPEC_300)
-				ctrl &= ~SDHCI_CTRL_8BITBUS;
-			if (ios->bus_width == MMC_BUS_WIDTH_4)
-				ctrl |= SDHCI_CTRL_4BITBUS;
-			else
-				ctrl &= ~SDHCI_CTRL_4BITBUS;
-		}
-		sdhci_writeb(host, ctrl, SDHCI_HOST_CONTROL);
-	}
+	host->ops->set_bus_width(host, ios->bus_width);
 
 	ctrl = sdhci_readb(host, SDHCI_HOST_CONTROL);
 
@@ -1510,19 +1490,20 @@ static void sdhci_do_set_ios(struct sdhci_host *host, struct mmc_ios *ios)
 
 		/* In case of UHS-I modes, set High Speed Enable */
 		if ((ios->timing == MMC_TIMING_MMC_HS200) ||
+		    (ios->timing == MMC_TIMING_MMC_DDR52) ||
 		    (ios->timing == MMC_TIMING_UHS_SDR50) ||
 		    (ios->timing == MMC_TIMING_UHS_SDR104) ||
 		    (ios->timing == MMC_TIMING_UHS_DDR50) ||
 		    (ios->timing == MMC_TIMING_UHS_SDR25))
 			ctrl |= SDHCI_CTRL_HISPD;
 
-		ctrl_2 = sdhci_readw(host, SDHCI_HOST_CONTROL2);
-		if (!(ctrl_2 & SDHCI_CTRL_PRESET_VAL_ENABLE)) {
+		if (!host->preset_enabled) {
 			sdhci_writeb(host, ctrl, SDHCI_HOST_CONTROL);
 			/*
 			 * We only need to set Driver Strength if the
 			 * preset value enable is not set.
 			 */
+			ctrl_2 = sdhci_readw(host, SDHCI_HOST_CONTROL2);
 			ctrl_2 &= ~SDHCI_CTRL_DRV_TYPE_MASK;
 			if (ios->drv_type == MMC_SET_DRIVER_TYPE_A)
 				ctrl_2 |= SDHCI_CTRL_DRV_TYPE_A;
@@ -1546,7 +1527,7 @@ static void sdhci_do_set_ios(struct sdhci_host *host, struct mmc_ios *ios)
 			sdhci_writeb(host, ctrl, SDHCI_HOST_CONTROL);
 
 			/* Re-enable SD Clock */
-			sdhci_update_clock(host);
+			host->ops->set_clock(host, host->clock);
 		}
 
 
@@ -1555,25 +1536,8 @@ static void sdhci_do_set_ios(struct sdhci_host *host, struct mmc_ios *ios)
 		clk &= ~SDHCI_CLOCK_CARD_EN;
 		sdhci_writew(host, clk, SDHCI_CLOCK_CONTROL);
 
-		if (host->ops->set_uhs_signaling)
-			host->ops->set_uhs_signaling(host, ios->timing);
-		else {
-			ctrl_2 = sdhci_readw(host, SDHCI_HOST_CONTROL2);
-			/* Select Bus Speed Mode for host */
-			ctrl_2 &= ~SDHCI_CTRL_UHS_MASK;
-			if ((ios->timing == MMC_TIMING_MMC_HS200) ||
-			    (ios->timing == MMC_TIMING_UHS_SDR104))
-				ctrl_2 |= SDHCI_CTRL_UHS_SDR104;
-			else if (ios->timing == MMC_TIMING_UHS_SDR12)
-				ctrl_2 |= SDHCI_CTRL_UHS_SDR12;
-			else if (ios->timing == MMC_TIMING_UHS_SDR25)
-				ctrl_2 |= SDHCI_CTRL_UHS_SDR25;
-			else if (ios->timing == MMC_TIMING_UHS_SDR50)
-				ctrl_2 |= SDHCI_CTRL_UHS_SDR50;
-			else if (ios->timing == MMC_TIMING_UHS_DDR50)
-				ctrl_2 |= SDHCI_CTRL_UHS_DDR50;
-			sdhci_writew(host, ctrl_2, SDHCI_HOST_CONTROL2);
-		}
+		host->ops->set_uhs_signaling(host, ios->timing);
+		host->timing = ios->timing;
 
 		if (!(host->quirks2 & SDHCI_QUIRK2_PRESET_VALUE_BROKEN) &&
 				((ios->timing == MMC_TIMING_UHS_SDR12) ||
@@ -1590,7 +1554,7 @@ static void sdhci_do_set_ios(struct sdhci_host *host, struct mmc_ios *ios)
 		}
 
 		/* Re-enable SD Clock */
-		sdhci_update_clock(host);
+		host->ops->set_clock(host, host->clock);
 	} else
 		sdhci_writeb(host, ctrl, SDHCI_HOST_CONTROL);
 
@@ -1600,7 +1564,7 @@ static void sdhci_do_set_ios(struct sdhci_host *host, struct mmc_ios *ios)
 	 * it on each ios seems to solve the problem.
 	 */
 	if(host->quirks & SDHCI_QUIRK_RESET_CMD_DATA_ON_IOS)
-		sdhci_reset(host, SDHCI_RESET_CMD | SDHCI_RESET_DATA);
+		sdhci_do_reset(host, SDHCI_RESET_CMD | SDHCI_RESET_DATA);
 
 	mmiowb();
 	spin_unlock_irqrestore(&host->lock, flags);
@@ -1709,24 +1673,16 @@ static int sdhci_get_ro(struct mmc_host *mmc)
 
 static void sdhci_enable_sdio_irq_nolock(struct sdhci_host *host, int enable)
 {
-	if (host->flags & SDHCI_DEVICE_DEAD)
-		goto out;
-
-	if (enable)
-		host->flags |= SDHCI_SDIO_IRQ_ENABLED;
-	else
-		host->flags &= ~SDHCI_SDIO_IRQ_ENABLED;
-
-	/* SDIO IRQ will be enabled as appropriate in runtime resume */
-	if (host->runtime_suspended)
-		goto out;
+	if (!(host->flags & SDHCI_DEVICE_DEAD)) {
+		if (enable)
+			host->ier |= SDHCI_INT_CARD_INT;
+		else
+			host->ier &= ~SDHCI_INT_CARD_INT;
 
-	if (enable)
-		sdhci_unmask_irqs(host, SDHCI_INT_CARD_INT);
-	else
-		sdhci_mask_irqs(host, SDHCI_INT_CARD_INT);
-out:
-	mmiowb();
+		sdhci_writel(host, host->ier, SDHCI_INT_ENABLE);
+		sdhci_writel(host, host->ier, SDHCI_SIGNAL_ENABLE);
+		mmiowb();
+	}
 }
 
 static void sdhci_enable_sdio_irq(struct mmc_host *mmc, int enable)
@@ -1734,9 +1690,18 @@ static void sdhci_enable_sdio_irq(struct mmc_host *mmc, int enable)
 	struct sdhci_host *host = mmc_priv(mmc);
 	unsigned long flags;
 
+	sdhci_runtime_pm_get(host);
+
 	spin_lock_irqsave(&host->lock, flags);
+	if (enable)
+		host->flags |= SDHCI_SDIO_IRQ_ENABLED;
+	else
+		host->flags &= ~SDHCI_SDIO_IRQ_ENABLED;
+
 	sdhci_enable_sdio_irq_nolock(host, enable);
 	spin_unlock_irqrestore(&host->lock, flags);
+
+	sdhci_runtime_pm_put(host);
 }
 
 static int sdhci_do_start_signal_voltage_switch(struct sdhci_host *host,
@@ -1855,22 +1820,15 @@ static int sdhci_card_busy(struct mmc_host *mmc)
 
 static int sdhci_execute_tuning(struct mmc_host *mmc, u32 opcode)
 {
-	struct sdhci_host *host;
+	struct sdhci_host *host = mmc_priv(mmc);
 	u16 ctrl;
-	u32 ier;
 	int tuning_loop_counter = MAX_TUNING_LOOP;
-	unsigned long timeout;
 	int err = 0;
-	bool requires_tuning_nonuhs = false;
 	unsigned long flags;
 
-	host = mmc_priv(mmc);
-
 	sdhci_runtime_pm_get(host);
 	spin_lock_irqsave(&host->lock, flags);
 
-	ctrl = sdhci_readw(host, SDHCI_HOST_CONTROL2);
-
 	/*
 	 * The Host Controller needs tuning only in case of SDR104 mode
 	 * and for SDR50 mode when Use Tuning for SDR50 is set in the
@@ -1878,15 +1836,18 @@ static int sdhci_execute_tuning(struct mmc_host *mmc, u32 opcode)
 	 * If the Host Controller supports the HS200 mode then the
 	 * tuning function has to be executed.
 	 */
-	if (((ctrl & SDHCI_CTRL_UHS_MASK) == SDHCI_CTRL_UHS_SDR50) &&
-	    (host->flags & SDHCI_SDR50_NEEDS_TUNING ||
-	     host->flags & SDHCI_SDR104_NEEDS_TUNING))
-		requires_tuning_nonuhs = true;
-
-	if (((ctrl & SDHCI_CTRL_UHS_MASK) == SDHCI_CTRL_UHS_SDR104) ||
-	    requires_tuning_nonuhs)
-		ctrl |= SDHCI_CTRL_EXEC_TUNING;
-	else {
+	switch (host->timing) {
+	case MMC_TIMING_MMC_HS200:
+	case MMC_TIMING_UHS_SDR104:
+		break;
+
+	case MMC_TIMING_UHS_SDR50:
+		if (host->flags & SDHCI_SDR50_NEEDS_TUNING ||
+		    host->flags & SDHCI_SDR104_NEEDS_TUNING)
+			break;
+		/* FALLTHROUGH */
+
+	default:
 		spin_unlock_irqrestore(&host->lock, flags);
 		sdhci_runtime_pm_put(host);
 		return 0;
@@ -1899,6 +1860,8 @@ static int sdhci_execute_tuning(struct mmc_host *mmc, u32 opcode)
 		return err;
 	}
 
+	ctrl = sdhci_readw(host, SDHCI_HOST_CONTROL2);
+	ctrl |= SDHCI_CTRL_EXEC_TUNING;
 	sdhci_writew(host, ctrl, SDHCI_HOST_CONTROL2);
 
 	/*
@@ -1911,21 +1874,17 @@ static int sdhci_execute_tuning(struct mmc_host *mmc, u32 opcode)
 	 * to make sure we don't hit a controller bug, we _only_
 	 * enable Buffer Read Ready interrupt here.
 	 */
-	ier = sdhci_readl(host, SDHCI_INT_ENABLE);
-	sdhci_clear_set_irqs(host, ier, SDHCI_INT_DATA_AVAIL);
+	sdhci_writel(host, SDHCI_INT_DATA_AVAIL, SDHCI_INT_ENABLE);
+	sdhci_writel(host, SDHCI_INT_DATA_AVAIL, SDHCI_SIGNAL_ENABLE);
 
 	/*
 	 * Issue CMD19 repeatedly till Execute Tuning is set to 0 or the number
 	 * of loops reaches 40 times or a timeout of 150ms occurs.
 	 */
-	timeout = 150;
 	do {
 		struct mmc_command cmd = {0};
 		struct mmc_request mrq = {NULL};
 
-		if (!tuning_loop_counter && !timeout)
-			break;
-
 		cmd.opcode = opcode;
 		cmd.arg = 0;
 		cmd.flags = MMC_RSP_R1 | MMC_CMD_ADTC;
@@ -1933,6 +1892,9 @@ static int sdhci_execute_tuning(struct mmc_host *mmc, u32 opcode)
 		cmd.data = NULL;
 		cmd.error = 0;
 
+		if (tuning_loop_counter-- == 0)
+			break;
+
 		mrq.cmd = &cmd;
 		host->mrq = &mrq;
 
@@ -1990,26 +1952,25 @@ static int sdhci_execute_tuning(struct mmc_host *mmc, u32 opcode)
 		host->tuning_done = 0;
 
 		ctrl = sdhci_readw(host, SDHCI_HOST_CONTROL2);
-		tuning_loop_counter--;
-		timeout--;
-		mdelay(1);
+
+		/* eMMC spec does not require a delay between tuning cycles */
+		if (opcode == MMC_SEND_TUNING_BLOCK)
+			mdelay(1);
 	} while (ctrl & SDHCI_CTRL_EXEC_TUNING);
 
 	/*
 	 * The Host Driver has exhausted the maximum number of loops allowed,
 	 * so use fixed sampling frequency.
 	 */
-	if (!tuning_loop_counter || !timeout) {
+	if (tuning_loop_counter < 0) {
 		ctrl &= ~SDHCI_CTRL_TUNED_CLK;
 		sdhci_writew(host, ctrl, SDHCI_HOST_CONTROL2);
+	}
+	if (!(ctrl & SDHCI_CTRL_TUNED_CLK)) {
+		pr_info(DRIVER_NAME ": Tuning procedure"
+			" failed, falling back to fixed sampling"
+			" clock\n");
 		err = -EIO;
-	} else {
-		if (!(ctrl & SDHCI_CTRL_TUNED_CLK)) {
-			pr_info(DRIVER_NAME ": Tuning procedure"
-				" failed, falling back to fixed sampling"
-				" clock\n");
-			err = -EIO;
-		}
 	}
 
 out:
@@ -2044,7 +2005,8 @@ out:
 	if (err && (host->flags & SDHCI_USING_RETUNING_TIMER))
 		err = 0;
 
-	sdhci_clear_set_irqs(host, SDHCI_INT_DATA_AVAIL, ier);
+	sdhci_writel(host, host->ier, SDHCI_INT_ENABLE);
+	sdhci_writel(host, host->ier, SDHCI_SIGNAL_ENABLE);
 	spin_unlock_irqrestore(&host->lock, flags);
 	sdhci_runtime_pm_put(host);
 
@@ -2054,26 +2016,30 @@ out:
 
 static void sdhci_enable_preset_value(struct sdhci_host *host, bool enable)
 {
-	u16 ctrl;
-
 	/* Host Controller v3.00 defines preset value registers */
 	if (host->version < SDHCI_SPEC_300)
 		return;
 
-	ctrl = sdhci_readw(host, SDHCI_HOST_CONTROL2);
-
 	/*
 	 * We only enable or disable Preset Value if they are not already
 	 * enabled or disabled respectively. Otherwise, we bail out.
 	 */
-	if (enable && !(ctrl & SDHCI_CTRL_PRESET_VAL_ENABLE)) {
-		ctrl |= SDHCI_CTRL_PRESET_VAL_ENABLE;
-		sdhci_writew(host, ctrl, SDHCI_HOST_CONTROL2);
-		host->flags |= SDHCI_PV_ENABLED;
-	} else if (!enable && (ctrl & SDHCI_CTRL_PRESET_VAL_ENABLE)) {
-		ctrl &= ~SDHCI_CTRL_PRESET_VAL_ENABLE;
+	if (host->preset_enabled != enable) {
+		u16 ctrl = sdhci_readw(host, SDHCI_HOST_CONTROL2);
+
+		if (enable)
+			ctrl |= SDHCI_CTRL_PRESET_VAL_ENABLE;
+		else
+			ctrl &= ~SDHCI_CTRL_PRESET_VAL_ENABLE;
+
 		sdhci_writew(host, ctrl, SDHCI_HOST_CONTROL2);
-		host->flags &= ~SDHCI_PV_ENABLED;
+
+		if (enable)
+			host->flags |= SDHCI_PV_ENABLED;
+		else
+			host->flags &= ~SDHCI_PV_ENABLED;
+
+		host->preset_enabled = enable;
 	}
 }
 
@@ -2095,8 +2061,8 @@ static void sdhci_card_event(struct mmc_host *mmc)
 		pr_err("%s: Resetting controller.\n",
 			mmc_hostname(host->mmc));
 
-		sdhci_reset(host, SDHCI_RESET_CMD);
-		sdhci_reset(host, SDHCI_RESET_DATA);
+		sdhci_do_reset(host, SDHCI_RESET_CMD);
+		sdhci_do_reset(host, SDHCI_RESET_DATA);
 
 		host->mrq->cmd->error = -ENOMEDIUM;
 		tasklet_schedule(&host->finish_tasklet);
@@ -2124,15 +2090,6 @@ static const struct mmc_host_ops sdhci_ops = {
  *                                                                           *
 \*****************************************************************************/
 
-static void sdhci_tasklet_card(unsigned long param)
-{
-	struct sdhci_host *host = (struct sdhci_host*)param;
-
-	sdhci_card_event(host->mmc);
-
-	mmc_detect_change(host->mmc, msecs_to_jiffies(200));
-}
-
 static void sdhci_tasklet_finish(unsigned long param)
 {
 	struct sdhci_host *host;
@@ -2169,12 +2126,12 @@ static void sdhci_tasklet_finish(unsigned long param)
 		/* Some controllers need this kick or reset won't work here */
 		if (host->quirks & SDHCI_QUIRK_CLOCK_BEFORE_RESET)
 			/* This is to force an update */
-			sdhci_update_clock(host);
+			host->ops->set_clock(host, host->clock);
 
 		/* Spec says we should do both at the same time, but Ricoh
 		   controllers do not like that. */
-		sdhci_reset(host, SDHCI_RESET_CMD);
-		sdhci_reset(host, SDHCI_RESET_DATA);
+		sdhci_do_reset(host, SDHCI_RESET_CMD);
+		sdhci_do_reset(host, SDHCI_RESET_DATA);
 	}
 
 	host->mrq = NULL;
@@ -2424,101 +2381,94 @@ static void sdhci_data_irq(struct sdhci_host *host, u32 intmask)
 
 static irqreturn_t sdhci_irq(int irq, void *dev_id)
 {
-	irqreturn_t result;
+	irqreturn_t result = IRQ_NONE;
 	struct sdhci_host *host = dev_id;
-	u32 intmask, unexpected = 0;
-	int cardint = 0, max_loops = 16;
+	u32 intmask, mask, unexpected = 0;
+	int max_loops = 16;
 
 	spin_lock(&host->lock);
 
-	if (host->runtime_suspended) {
+	if (host->runtime_suspended && !sdhci_sdio_irq_enabled(host)) {
 		spin_unlock(&host->lock);
 		return IRQ_NONE;
 	}
 
 	intmask = sdhci_readl(host, SDHCI_INT_STATUS);
-
 	if (!intmask || intmask == 0xffffffff) {
 		result = IRQ_NONE;
 		goto out;
 	}
 
-again:
-	DBG("*** %s got interrupt: 0x%08x\n",
-		mmc_hostname(host->mmc), intmask);
-
-	if (intmask & (SDHCI_INT_CARD_INSERT | SDHCI_INT_CARD_REMOVE)) {
-		u32 present = sdhci_readl(host, SDHCI_PRESENT_STATE) &
-			      SDHCI_CARD_PRESENT;
-
-		/*
-		 * There is a observation on i.mx esdhc.  INSERT bit will be
-		 * immediately set again when it gets cleared, if a card is
-		 * inserted.  We have to mask the irq to prevent interrupt
-		 * storm which will freeze the system.  And the REMOVE gets
-		 * the same situation.
-		 *
-		 * More testing are needed here to ensure it works for other
-		 * platforms though.
-		 */
-		sdhci_mask_irqs(host, present ? SDHCI_INT_CARD_INSERT :
-						SDHCI_INT_CARD_REMOVE);
-		sdhci_unmask_irqs(host, present ? SDHCI_INT_CARD_REMOVE :
-						  SDHCI_INT_CARD_INSERT);
-
-		sdhci_writel(host, intmask & (SDHCI_INT_CARD_INSERT |
-			     SDHCI_INT_CARD_REMOVE), SDHCI_INT_STATUS);
-		intmask &= ~(SDHCI_INT_CARD_INSERT | SDHCI_INT_CARD_REMOVE);
-		tasklet_schedule(&host->card_tasklet);
-	}
-
-	if (intmask & SDHCI_INT_CMD_MASK) {
-		sdhci_writel(host, intmask & SDHCI_INT_CMD_MASK,
-			SDHCI_INT_STATUS);
-		sdhci_cmd_irq(host, intmask & SDHCI_INT_CMD_MASK);
-	}
+	do {
+		/* Clear selected interrupts. */
+		mask = intmask & (SDHCI_INT_CMD_MASK | SDHCI_INT_DATA_MASK |
+				  SDHCI_INT_BUS_POWER);
+		sdhci_writel(host, mask, SDHCI_INT_STATUS);
 
-	if (intmask & SDHCI_INT_DATA_MASK) {
-		sdhci_writel(host, intmask & SDHCI_INT_DATA_MASK,
-			SDHCI_INT_STATUS);
-		sdhci_data_irq(host, intmask & SDHCI_INT_DATA_MASK);
-	}
+		DBG("*** %s got interrupt: 0x%08x\n",
+			mmc_hostname(host->mmc), intmask);
 
-	intmask &= ~(SDHCI_INT_CMD_MASK | SDHCI_INT_DATA_MASK);
+		if (intmask & (SDHCI_INT_CARD_INSERT | SDHCI_INT_CARD_REMOVE)) {
+			u32 present = sdhci_readl(host, SDHCI_PRESENT_STATE) &
+				      SDHCI_CARD_PRESENT;
 
-	intmask &= ~SDHCI_INT_ERROR;
+			/*
+			 * There is a observation on i.mx esdhc.  INSERT
+			 * bit will be immediately set again when it gets
+			 * cleared, if a card is inserted.  We have to mask
+			 * the irq to prevent interrupt storm which will
+			 * freeze the system.  And the REMOVE gets the
+			 * same situation.
+			 *
+			 * More testing are needed here to ensure it works
+			 * for other platforms though.
+			 */
+			host->ier &= ~(SDHCI_INT_CARD_INSERT |
+				       SDHCI_INT_CARD_REMOVE);
+			host->ier |= present ? SDHCI_INT_CARD_REMOVE :
+					       SDHCI_INT_CARD_INSERT;
+			sdhci_writel(host, host->ier, SDHCI_INT_ENABLE);
+			sdhci_writel(host, host->ier, SDHCI_SIGNAL_ENABLE);
+
+			sdhci_writel(host, intmask & (SDHCI_INT_CARD_INSERT |
+				     SDHCI_INT_CARD_REMOVE), SDHCI_INT_STATUS);
+
+			host->thread_isr |= intmask & (SDHCI_INT_CARD_INSERT |
+						       SDHCI_INT_CARD_REMOVE);
+			result = IRQ_WAKE_THREAD;
+		}
 
-	if (intmask & SDHCI_INT_BUS_POWER) {
-		pr_err("%s: Card is consuming too much power!\n",
-			mmc_hostname(host->mmc));
-		sdhci_writel(host, SDHCI_INT_BUS_POWER, SDHCI_INT_STATUS);
-	}
+		if (intmask & SDHCI_INT_CMD_MASK)
+			sdhci_cmd_irq(host, intmask & SDHCI_INT_CMD_MASK);
 
-	intmask &= ~SDHCI_INT_BUS_POWER;
+		if (intmask & SDHCI_INT_DATA_MASK)
+			sdhci_data_irq(host, intmask & SDHCI_INT_DATA_MASK);
 
-	if (intmask & SDHCI_INT_CARD_INT)
-		cardint = 1;
+		if (intmask & SDHCI_INT_BUS_POWER)
+			pr_err("%s: Card is consuming too much power!\n",
+				mmc_hostname(host->mmc));
 
-	intmask &= ~SDHCI_INT_CARD_INT;
+		if (intmask & SDHCI_INT_CARD_INT) {
+			sdhci_enable_sdio_irq_nolock(host, false);
+			host->thread_isr |= SDHCI_INT_CARD_INT;
+			result = IRQ_WAKE_THREAD;
+		}
 
-	if (intmask) {
-		unexpected |= intmask;
-		sdhci_writel(host, intmask, SDHCI_INT_STATUS);
-	}
+		intmask &= ~(SDHCI_INT_CARD_INSERT | SDHCI_INT_CARD_REMOVE |
+			     SDHCI_INT_CMD_MASK | SDHCI_INT_DATA_MASK |
+			     SDHCI_INT_ERROR | SDHCI_INT_BUS_POWER |
+			     SDHCI_INT_CARD_INT);
 
-	result = IRQ_HANDLED;
+		if (intmask) {
+			unexpected |= intmask;
+			sdhci_writel(host, intmask, SDHCI_INT_STATUS);
+		}
 
-	intmask = sdhci_readl(host, SDHCI_INT_STATUS);
+		if (result == IRQ_NONE)
+			result = IRQ_HANDLED;
 
-	/*
-	 * If we know we'll call the driver to signal SDIO IRQ, disregard
-	 * further indications of Card Interrupt in the status to avoid a
-	 * needless loop.
-	 */
-	if (cardint)
-		intmask &= ~SDHCI_INT_CARD_INT;
-	if (intmask && --max_loops)
-		goto again;
+		intmask = sdhci_readl(host, SDHCI_INT_STATUS);
+	} while (intmask && --max_loops);
 out:
 	spin_unlock(&host->lock);
 
@@ -2527,15 +2477,38 @@ out:
 			   mmc_hostname(host->mmc), unexpected);
 		sdhci_dumpregs(host);
 	}
-	/*
-	 * We have to delay this as it calls back into the driver.
-	 */
-	if (cardint)
-		mmc_signal_sdio_irq(host->mmc);
 
 	return result;
 }
 
+static irqreturn_t sdhci_thread_irq(int irq, void *dev_id)
+{
+	struct sdhci_host *host = dev_id;
+	unsigned long flags;
+	u32 isr;
+
+	spin_lock_irqsave(&host->lock, flags);
+	isr = host->thread_isr;
+	host->thread_isr = 0;
+	spin_unlock_irqrestore(&host->lock, flags);
+
+	if (isr & (SDHCI_INT_CARD_INSERT | SDHCI_INT_CARD_REMOVE)) {
+		sdhci_card_event(host->mmc);
+		mmc_detect_change(host->mmc, msecs_to_jiffies(200));
+	}
+
+	if (isr & SDHCI_INT_CARD_INT) {
+		sdio_run_irqs(host->mmc);
+
+		spin_lock_irqsave(&host->lock, flags);
+		if (host->flags & SDHCI_SDIO_IRQ_ENABLED)
+			sdhci_enable_sdio_irq_nolock(host, true);
+		spin_unlock_irqrestore(&host->lock, flags);
+	}
+
+	return isr ? IRQ_HANDLED : IRQ_NONE;
+}
+
 /*****************************************************************************\
  *                                                                           *
  * Suspend/resume                                                            *
@@ -2572,9 +2545,6 @@ EXPORT_SYMBOL_GPL(sdhci_disable_irq_wakeups);
 
 int sdhci_suspend_host(struct sdhci_host *host)
 {
-	if (host->ops->platform_suspend)
-		host->ops->platform_suspend(host);
-
 	sdhci_disable_card_detection(host);
 
 	/* Disable tuning since we are suspending */
@@ -2584,7 +2554,9 @@ int sdhci_suspend_host(struct sdhci_host *host)
 	}
 
 	if (!device_may_wakeup(mmc_dev(host->mmc))) {
-		sdhci_mask_irqs(host, SDHCI_INT_ALL_MASK);
+		host->ier = 0;
+		sdhci_writel(host, 0, SDHCI_INT_ENABLE);
+		sdhci_writel(host, 0, SDHCI_SIGNAL_ENABLE);
 		free_irq(host->irq, host);
 	} else {
 		sdhci_enable_irq_wakeups(host);
@@ -2605,8 +2577,9 @@ int sdhci_resume_host(struct sdhci_host *host)
 	}
 
 	if (!device_may_wakeup(mmc_dev(host->mmc))) {
-		ret = request_irq(host->irq, sdhci_irq, IRQF_SHARED,
-				  mmc_hostname(host->mmc), host);
+		ret = request_threaded_irq(host->irq, sdhci_irq,
+					   sdhci_thread_irq, IRQF_SHARED,
+					   mmc_hostname(host->mmc), host);
 		if (ret)
 			return ret;
 	} else {
@@ -2628,9 +2601,6 @@ int sdhci_resume_host(struct sdhci_host *host)
 
 	sdhci_enable_card_detection(host);
 
-	if (host->ops->platform_resume)
-		host->ops->platform_resume(host);
-
 	/* Set the re-tuning expiration flag */
 	if (host->flags & SDHCI_USING_RETUNING_TIMER)
 		host->flags |= SDHCI_NEEDS_RETUNING;
@@ -2682,10 +2652,12 @@ int sdhci_runtime_suspend_host(struct sdhci_host *host)
 	}
 
 	spin_lock_irqsave(&host->lock, flags);
-	sdhci_mask_irqs(host, SDHCI_INT_ALL_MASK);
+	host->ier &= SDHCI_INT_CARD_INT;
+	sdhci_writel(host, host->ier, SDHCI_INT_ENABLE);
+	sdhci_writel(host, host->ier, SDHCI_SIGNAL_ENABLE);
 	spin_unlock_irqrestore(&host->lock, flags);
 
-	synchronize_irq(host->irq);
+	synchronize_hardirq(host->irq);
 
 	spin_lock_irqsave(&host->lock, flags);
 	host->runtime_suspended = true;
@@ -2729,7 +2701,7 @@ int sdhci_runtime_resume_host(struct sdhci_host *host)
 	host->runtime_suspended = false;
 
 	/* Enable SDIO IRQ */
-	if ((host->flags & SDHCI_SDIO_IRQ_ENABLED))
+	if (host->flags & SDHCI_SDIO_IRQ_ENABLED)
 		sdhci_enable_sdio_irq_nolock(host, true);
 
 	/* Enable Card Detection */
@@ -2788,7 +2760,7 @@ int sdhci_add_host(struct sdhci_host *host)
 	if (debug_quirks2)
 		host->quirks2 = debug_quirks2;
 
-	sdhci_reset(host, SDHCI_RESET_ALL);
+	sdhci_do_reset(host, SDHCI_RESET_ALL);
 
 	host->version = sdhci_readw(host, SDHCI_HOST_VERSION);
 	host->version = (host->version & SDHCI_SPEC_VER_MASK)
@@ -2848,15 +2820,29 @@ int sdhci_add_host(struct sdhci_host *host)
 		 * (128) and potentially one alignment transfer for
 		 * each of those entries.
 		 */
-		host->adma_desc = kmalloc((128 * 2 + 1) * 4, GFP_KERNEL);
+		host->adma_desc = dma_alloc_coherent(mmc_dev(host->mmc),
+						     ADMA_SIZE, &host->adma_addr,
+						     GFP_KERNEL);
 		host->align_buffer = kmalloc(128 * 4, GFP_KERNEL);
 		if (!host->adma_desc || !host->align_buffer) {
-			kfree(host->adma_desc);
+			dma_free_coherent(mmc_dev(host->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",
 				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));
+			host->flags &= ~SDHCI_USE_ADMA;
+			dma_free_coherent(mmc_dev(host->mmc), ADMA_SIZE,
+					  host->adma_desc, host->adma_addr);
+			kfree(host->align_buffer);
+			host->adma_desc = NULL;
+			host->align_buffer = NULL;
 		}
 	}
 
@@ -2941,6 +2927,7 @@ int sdhci_add_host(struct sdhci_host *host)
 	mmc->max_busy_timeout = (1 << 27) / host->timeout_clk;
 
 	mmc->caps |= MMC_CAP_SDIO_IRQ | MMC_CAP_ERASE | MMC_CAP_CMD23;
+	mmc->caps2 |= MMC_CAP2_SDIO_IRQ_NOTHREAD;
 
 	if (host->quirks & SDHCI_QUIRK_MULTIBLOCK_READ_ACMD12)
 		host->flags |= SDHCI_AUTO_CMD12;
@@ -3212,8 +3199,6 @@ int sdhci_add_host(struct sdhci_host *host)
 	/*
 	 * Init tasklets.
 	 */
-	tasklet_init(&host->card_tasklet,
-		sdhci_tasklet_card, (unsigned long)host);
 	tasklet_init(&host->finish_tasklet,
 		sdhci_tasklet_finish, (unsigned long)host);
 
@@ -3230,8 +3215,8 @@ int sdhci_add_host(struct sdhci_host *host)
 
 	sdhci_init(host, 0);
 
-	ret = request_irq(host->irq, sdhci_irq, IRQF_SHARED,
-		mmc_hostname(mmc), host);
+	ret = request_threaded_irq(host->irq, sdhci_irq, sdhci_thread_irq,
+				   IRQF_SHARED,	mmc_hostname(mmc), host);
 	if (ret) {
 		pr_err("%s: Failed to request IRQ %d: %d\n",
 		       mmc_hostname(mmc), host->irq, ret);
@@ -3273,12 +3258,12 @@ int sdhci_add_host(struct sdhci_host *host)
 
 #ifdef SDHCI_USE_LEDS_CLASS
 reset:
-	sdhci_reset(host, SDHCI_RESET_ALL);
-	sdhci_mask_irqs(host, SDHCI_INT_ALL_MASK);
+	sdhci_do_reset(host, SDHCI_RESET_ALL);
+	sdhci_writel(host, 0, SDHCI_INT_ENABLE);
+	sdhci_writel(host, 0, SDHCI_SIGNAL_ENABLE);
 	free_irq(host->irq, host);
 #endif
 untasklet:
-	tasklet_kill(&host->card_tasklet);
 	tasklet_kill(&host->finish_tasklet);
 
 	return ret;
@@ -3315,14 +3300,14 @@ void sdhci_remove_host(struct sdhci_host *host, int dead)
 #endif
 
 	if (!dead)
-		sdhci_reset(host, SDHCI_RESET_ALL);
+		sdhci_do_reset(host, SDHCI_RESET_ALL);
 
-	sdhci_mask_irqs(host, SDHCI_INT_ALL_MASK);
+	sdhci_writel(host, 0, SDHCI_INT_ENABLE);
+	sdhci_writel(host, 0, SDHCI_SIGNAL_ENABLE);
 	free_irq(host->irq, host);
 
 	del_timer_sync(&host->timer);
 
-	tasklet_kill(&host->card_tasklet);
 	tasklet_kill(&host->finish_tasklet);
 
 	if (host->vmmc) {
@@ -3335,7 +3320,9 @@ void sdhci_remove_host(struct sdhci_host *host, int dead)
 		regulator_put(host->vqmmc);
 	}
 
-	kfree(host->adma_desc);
+	if (host->adma_desc)
+		dma_free_coherent(mmc_dev(host->mmc), ADMA_SIZE,
+				  host->adma_desc, host->adma_addr);
 	kfree(host->align_buffer);
 
 	host->adma_desc = NULL;
diff --git a/drivers/mmc/host/sdhci.h b/drivers/mmc/host/sdhci.h
index 0a3ed01887d..4a5cd5e3fa3 100644
--- a/drivers/mmc/host/sdhci.h
+++ b/drivers/mmc/host/sdhci.h
@@ -281,18 +281,14 @@ 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);
-	int		(*platform_bus_width)(struct sdhci_host *host,
-					       int width);
+	void		(*set_bus_width)(struct sdhci_host *host, int width);
 	void (*platform_send_init_74_clocks)(struct sdhci_host *host,
 					     u8 power_mode);
 	unsigned int    (*get_ro)(struct sdhci_host *host);
-	void	(*platform_reset_enter)(struct sdhci_host *host, u8 mask);
-	void	(*platform_reset_exit)(struct sdhci_host *host, u8 mask);
+	void		(*reset)(struct sdhci_host *host, u8 mask);
 	int	(*platform_execute_tuning)(struct sdhci_host *host, u32 opcode);
-	int	(*set_uhs_signaling)(struct sdhci_host *host, unsigned int uhs);
+	void	(*set_uhs_signaling)(struct sdhci_host *host, unsigned int uhs);
 	void	(*hw_reset)(struct sdhci_host *host);
-	void	(*platform_suspend)(struct sdhci_host *host);
-	void	(*platform_resume)(struct sdhci_host *host);
 	void    (*adma_workaround)(struct sdhci_host *host, u32 intmask);
 	void	(*platform_init)(struct sdhci_host *host);
 	void    (*card_event)(struct sdhci_host *host);
@@ -397,6 +393,16 @@ extern void sdhci_remove_host(struct sdhci_host *host, int dead);
 extern void sdhci_send_command(struct sdhci_host *host,
 				struct mmc_command *cmd);
 
+static inline bool sdhci_sdio_irq_enabled(struct sdhci_host *host)
+{
+	return !!(host->flags & SDHCI_SDIO_IRQ_ENABLED);
+}
+
+void sdhci_set_clock(struct sdhci_host *host, unsigned int clock);
+void sdhci_set_bus_width(struct sdhci_host *host, int width);
+void sdhci_reset(struct sdhci_host *host, u8 mask);
+void sdhci_set_uhs_signaling(struct sdhci_host *host, unsigned timing);
+
 #ifdef CONFIG_PM
 extern int sdhci_suspend_host(struct sdhci_host *host);
 extern int sdhci_resume_host(struct sdhci_host *host);
diff --git a/drivers/mmc/host/sh_mmcif.c b/drivers/mmc/host/sh_mmcif.c
index 54730f4aac8..656fbba4c42 100644
--- a/drivers/mmc/host/sh_mmcif.c
+++ b/drivers/mmc/host/sh_mmcif.c
@@ -803,12 +803,13 @@ static u32 sh_mmcif_set_cmd(struct sh_mmcif_host *host,
 			break;
 		}
 		switch (host->timing) {
-		case MMC_TIMING_UHS_DDR50:
+		case MMC_TIMING_MMC_DDR52:
 			/*
 			 * MMC core will only set this timing, if the host
-			 * advertises the MMC_CAP_UHS_DDR50 capability. MMCIF
-			 * implementations with this capability, e.g. sh73a0,
-			 * will have to set it in their platform data.
+			 * advertises the MMC_CAP_1_8V_DDR/MMC_CAP_1_2V_DDR
+			 * capability. MMCIF implementations with this
+			 * capability, e.g. sh73a0, will have to set it
+			 * in their platform data.
 			 */
 			tmp |= CMD_SET_DARS;
 			break;
diff --git a/drivers/mmc/host/sunxi-mmc.c b/drivers/mmc/host/sunxi-mmc.c
new file mode 100644
index 00000000000..024f67c98cd
--- /dev/null
+++ b/drivers/mmc/host/sunxi-mmc.c
@@ -0,0 +1,1049 @@
+/*
+ * Driver for sunxi SD/MMC host controllers
+ * (C) Copyright 2007-2011 Reuuimlla Technology Co., Ltd.
+ * (C) Copyright 2007-2011 Aaron Maoye <leafy.myeh@reuuimllatech.com>
+ * (C) Copyright 2013-2014 O2S GmbH <www.o2s.ch>
+ * (C) Copyright 2013-2014 David Lanzend�rfer <david.lanzendoerfer@o2s.ch>
+ * (C) Copyright 2013-2014 Hans de Goede <hdegoede@redhat.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/io.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+
+#include <linux/clk.h>
+#include <linux/clk-private.h>
+#include <linux/clk/sunxi.h>
+
+#include <linux/gpio.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+#include <linux/scatterlist.h>
+#include <linux/dma-mapping.h>
+#include <linux/slab.h>
+#include <linux/reset.h>
+
+#include <linux/of_address.h>
+#include <linux/of_gpio.h>
+#include <linux/of_platform.h>
+
+#include <linux/mmc/host.h>
+#include <linux/mmc/sd.h>
+#include <linux/mmc/sdio.h>
+#include <linux/mmc/mmc.h>
+#include <linux/mmc/core.h>
+#include <linux/mmc/card.h>
+#include <linux/mmc/slot-gpio.h>
+
+/* register offset definitions */
+#define SDXC_REG_GCTRL	(0x00) /* SMC Global Control Register */
+#define SDXC_REG_CLKCR	(0x04) /* SMC Clock Control Register */
+#define SDXC_REG_TMOUT	(0x08) /* SMC Time Out Register */
+#define SDXC_REG_WIDTH	(0x0C) /* SMC Bus Width Register */
+#define SDXC_REG_BLKSZ	(0x10) /* SMC Block Size Register */
+#define SDXC_REG_BCNTR	(0x14) /* SMC Byte Count Register */
+#define SDXC_REG_CMDR	(0x18) /* SMC Command Register */
+#define SDXC_REG_CARG	(0x1C) /* SMC Argument Register */
+#define SDXC_REG_RESP0	(0x20) /* SMC Response Register 0 */
+#define SDXC_REG_RESP1	(0x24) /* SMC Response Register 1 */
+#define SDXC_REG_RESP2	(0x28) /* SMC Response Register 2 */
+#define SDXC_REG_RESP3	(0x2C) /* SMC Response Register 3 */
+#define SDXC_REG_IMASK	(0x30) /* SMC Interrupt Mask Register */
+#define SDXC_REG_MISTA	(0x34) /* SMC Masked Interrupt Status Register */
+#define SDXC_REG_RINTR	(0x38) /* SMC Raw Interrupt Status Register */
+#define SDXC_REG_STAS	(0x3C) /* SMC Status Register */
+#define SDXC_REG_FTRGL	(0x40) /* SMC FIFO Threshold Watermark Registe */
+#define SDXC_REG_FUNS	(0x44) /* SMC Function Select Register */
+#define SDXC_REG_CBCR	(0x48) /* SMC CIU Byte Count Register */
+#define SDXC_REG_BBCR	(0x4C) /* SMC BIU Byte Count Register */
+#define SDXC_REG_DBGC	(0x50) /* SMC Debug Enable Register */
+#define SDXC_REG_HWRST	(0x78) /* SMC Card Hardware Reset for Register */
+#define SDXC_REG_DMAC	(0x80) /* SMC IDMAC Control Register */
+#define SDXC_REG_DLBA	(0x84) /* SMC IDMAC Descriptor List Base Addre */
+#define SDXC_REG_IDST	(0x88) /* SMC IDMAC Status Register */
+#define SDXC_REG_IDIE	(0x8C) /* SMC IDMAC Interrupt Enable Register */
+#define SDXC_REG_CHDA	(0x90)
+#define SDXC_REG_CBDA	(0x94)
+
+#define mmc_readl(host, reg) \
+	readl((host)->reg_base + SDXC_##reg)
+#define mmc_writel(host, reg, value) \
+	writel((value), (host)->reg_base + SDXC_##reg)
+
+/* global control register bits */
+#define SDXC_SOFT_RESET			BIT(0)
+#define SDXC_FIFO_RESET			BIT(1)
+#define SDXC_DMA_RESET			BIT(2)
+#define SDXC_INTERRUPT_ENABLE_BIT	BIT(4)
+#define SDXC_DMA_ENABLE_BIT		BIT(5)
+#define SDXC_DEBOUNCE_ENABLE_BIT	BIT(8)
+#define SDXC_POSEDGE_LATCH_DATA		BIT(9)
+#define SDXC_DDR_MODE			BIT(10)
+#define SDXC_MEMORY_ACCESS_DONE		BIT(29)
+#define SDXC_ACCESS_DONE_DIRECT		BIT(30)
+#define SDXC_ACCESS_BY_AHB		BIT(31)
+#define SDXC_ACCESS_BY_DMA		(0 << 31)
+#define SDXC_HARDWARE_RESET \
+	(SDXC_SOFT_RESET | SDXC_FIFO_RESET | SDXC_DMA_RESET)
+
+/* clock control bits */
+#define SDXC_CARD_CLOCK_ON		BIT(16)
+#define SDXC_LOW_POWER_ON		BIT(17)
+
+/* bus width */
+#define SDXC_WIDTH1			0
+#define SDXC_WIDTH4			1
+#define SDXC_WIDTH8			2
+
+/* smc command bits */
+#define SDXC_RESP_EXPIRE		BIT(6)
+#define SDXC_LONG_RESPONSE		BIT(7)
+#define SDXC_CHECK_RESPONSE_CRC		BIT(8)
+#define SDXC_DATA_EXPIRE		BIT(9)
+#define SDXC_WRITE			BIT(10)
+#define SDXC_SEQUENCE_MODE		BIT(11)
+#define SDXC_SEND_AUTO_STOP		BIT(12)
+#define SDXC_WAIT_PRE_OVER		BIT(13)
+#define SDXC_STOP_ABORT_CMD		BIT(14)
+#define SDXC_SEND_INIT_SEQUENCE		BIT(15)
+#define SDXC_UPCLK_ONLY			BIT(21)
+#define SDXC_READ_CEATA_DEV		BIT(22)
+#define SDXC_CCS_EXPIRE			BIT(23)
+#define SDXC_ENABLE_BIT_BOOT		BIT(24)
+#define SDXC_ALT_BOOT_OPTIONS		BIT(25)
+#define SDXC_BOOT_ACK_EXPIRE		BIT(26)
+#define SDXC_BOOT_ABORT			BIT(27)
+#define SDXC_VOLTAGE_SWITCH	        BIT(28)
+#define SDXC_USE_HOLD_REGISTER	        BIT(29)
+#define SDXC_START			BIT(31)
+
+/* interrupt bits */
+#define SDXC_RESP_ERROR			BIT(1)
+#define SDXC_COMMAND_DONE		BIT(2)
+#define SDXC_DATA_OVER			BIT(3)
+#define SDXC_TX_DATA_REQUEST		BIT(4)
+#define SDXC_RX_DATA_REQUEST		BIT(5)
+#define SDXC_RESP_CRC_ERROR		BIT(6)
+#define SDXC_DATA_CRC_ERROR		BIT(7)
+#define SDXC_RESP_TIMEOUT		BIT(8)
+#define SDXC_DATA_TIMEOUT		BIT(9)
+#define SDXC_VOLTAGE_CHANGE_DONE	BIT(10)
+#define SDXC_FIFO_RUN_ERROR		BIT(11)
+#define SDXC_HARD_WARE_LOCKED		BIT(12)
+#define SDXC_START_BIT_ERROR		BIT(13)
+#define SDXC_AUTO_COMMAND_DONE		BIT(14)
+#define SDXC_END_BIT_ERROR		BIT(15)
+#define SDXC_SDIO_INTERRUPT		BIT(16)
+#define SDXC_CARD_INSERT		BIT(30)
+#define SDXC_CARD_REMOVE		BIT(31)
+#define SDXC_INTERRUPT_ERROR_BIT \
+	(SDXC_RESP_ERROR | SDXC_RESP_CRC_ERROR | SDXC_DATA_CRC_ERROR | \
+	 SDXC_RESP_TIMEOUT | SDXC_DATA_TIMEOUT | SDXC_FIFO_RUN_ERROR | \
+	 SDXC_HARD_WARE_LOCKED | SDXC_START_BIT_ERROR | SDXC_END_BIT_ERROR)
+#define SDXC_INTERRUPT_DONE_BIT \
+	(SDXC_AUTO_COMMAND_DONE | SDXC_DATA_OVER | \
+	 SDXC_COMMAND_DONE | SDXC_VOLTAGE_CHANGE_DONE)
+
+/* status */
+#define SDXC_RXWL_FLAG			BIT(0)
+#define SDXC_TXWL_FLAG			BIT(1)
+#define SDXC_FIFO_EMPTY			BIT(2)
+#define SDXC_FIFO_FULL			BIT(3)
+#define SDXC_CARD_PRESENT		BIT(8)
+#define SDXC_CARD_DATA_BUSY		BIT(9)
+#define SDXC_DATA_FSM_BUSY		BIT(10)
+#define SDXC_DMA_REQUEST		BIT(31)
+#define SDXC_FIFO_SIZE			16
+
+/* Function select */
+#define SDXC_CEATA_ON			(0xceaa << 16)
+#define SDXC_SEND_IRQ_RESPONSE		BIT(0)
+#define SDXC_SDIO_READ_WAIT		BIT(1)
+#define SDXC_ABORT_READ_DATA		BIT(2)
+#define SDXC_SEND_CCSD			BIT(8)
+#define SDXC_SEND_AUTO_STOPCCSD		BIT(9)
+#define SDXC_CEATA_DEV_IRQ_ENABLE	BIT(10)
+
+/* IDMA controller bus mod bit field */
+#define SDXC_IDMAC_SOFT_RESET		BIT(0)
+#define SDXC_IDMAC_FIX_BURST		BIT(1)
+#define SDXC_IDMAC_IDMA_ON		BIT(7)
+#define SDXC_IDMAC_REFETCH_DES		BIT(31)
+
+/* IDMA status bit field */
+#define SDXC_IDMAC_TRANSMIT_INTERRUPT		BIT(0)
+#define SDXC_IDMAC_RECEIVE_INTERRUPT		BIT(1)
+#define SDXC_IDMAC_FATAL_BUS_ERROR		BIT(2)
+#define SDXC_IDMAC_DESTINATION_INVALID		BIT(4)
+#define SDXC_IDMAC_CARD_ERROR_SUM		BIT(5)
+#define SDXC_IDMAC_NORMAL_INTERRUPT_SUM		BIT(8)
+#define SDXC_IDMAC_ABNORMAL_INTERRUPT_SUM	BIT(9)
+#define SDXC_IDMAC_HOST_ABORT_INTERRUPT		BIT(10)
+#define SDXC_IDMAC_IDLE				(0 << 13)
+#define SDXC_IDMAC_SUSPEND			(1 << 13)
+#define SDXC_IDMAC_DESC_READ			(2 << 13)
+#define SDXC_IDMAC_DESC_CHECK			(3 << 13)
+#define SDXC_IDMAC_READ_REQUEST_WAIT		(4 << 13)
+#define SDXC_IDMAC_WRITE_REQUEST_WAIT		(5 << 13)
+#define SDXC_IDMAC_READ				(6 << 13)
+#define SDXC_IDMAC_WRITE			(7 << 13)
+#define SDXC_IDMAC_DESC_CLOSE			(8 << 13)
+
+/*
+* If the idma-des-size-bits of property is ie 13, bufsize bits are:
+*  Bits  0-12: buf1 size
+*  Bits 13-25: buf2 size
+*  Bits 26-31: not used
+* Since we only ever set buf1 size, we can simply store it directly.
+*/
+#define SDXC_IDMAC_DES0_DIC	BIT(1)  /* disable interrupt on completion */
+#define SDXC_IDMAC_DES0_LD	BIT(2)  /* last descriptor */
+#define SDXC_IDMAC_DES0_FD	BIT(3)  /* first descriptor */
+#define SDXC_IDMAC_DES0_CH	BIT(4)  /* chain mode */
+#define SDXC_IDMAC_DES0_ER	BIT(5)  /* end of ring */
+#define SDXC_IDMAC_DES0_CES	BIT(30) /* card error summary */
+#define SDXC_IDMAC_DES0_OWN	BIT(31) /* 1-idma owns it, 0-host owns it */
+
+struct sunxi_idma_des {
+	u32	config;
+	u32	buf_size;
+	u32	buf_addr_ptr1;
+	u32	buf_addr_ptr2;
+};
+
+struct sunxi_mmc_host {
+	struct mmc_host	*mmc;
+	struct reset_control *reset;
+
+	/* IO mapping base */
+	void __iomem	*reg_base;
+
+	/* clock management */
+	struct clk	*clk_ahb;
+	struct clk	*clk_mmc;
+
+	/* irq */
+	spinlock_t	lock;
+	int		irq;
+	u32		int_sum;
+	u32		sdio_imask;
+
+	/* dma */
+	u32		idma_des_size_bits;
+	dma_addr_t	sg_dma;
+	void		*sg_cpu;
+	bool		wait_dma;
+
+	struct mmc_request *mrq;
+	struct mmc_request *manual_stop_mrq;
+	int		ferror;
+};
+
+static int sunxi_mmc_reset_host(struct sunxi_mmc_host *host)
+{
+	unsigned long expire = jiffies + msecs_to_jiffies(250);
+	u32 rval;
+
+	mmc_writel(host, REG_CMDR, SDXC_HARDWARE_RESET);
+	do {
+		rval = mmc_readl(host, REG_GCTRL);
+	} while (time_before(jiffies, expire) && (rval & SDXC_HARDWARE_RESET));
+
+	if (rval & SDXC_HARDWARE_RESET) {
+		dev_err(mmc_dev(host->mmc), "fatal err reset timeout\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int sunxi_mmc_init_host(struct mmc_host *mmc)
+{
+	u32 rval;
+	struct sunxi_mmc_host *host = mmc_priv(mmc);
+
+	if (sunxi_mmc_reset_host(host))
+		return -EIO;
+
+	mmc_writel(host, REG_FTRGL, 0x20070008);
+	mmc_writel(host, REG_TMOUT, 0xffffffff);
+	mmc_writel(host, REG_IMASK, host->sdio_imask);
+	mmc_writel(host, REG_RINTR, 0xffffffff);
+	mmc_writel(host, REG_DBGC, 0xdeb);
+	mmc_writel(host, REG_FUNS, SDXC_CEATA_ON);
+	mmc_writel(host, REG_DLBA, host->sg_dma);
+
+	rval = mmc_readl(host, REG_GCTRL);
+	rval |= SDXC_INTERRUPT_ENABLE_BIT;
+	rval &= ~SDXC_ACCESS_DONE_DIRECT;
+	mmc_writel(host, REG_GCTRL, rval);
+
+	return 0;
+}
+
+static void sunxi_mmc_init_idma_des(struct sunxi_mmc_host *host,
+				    struct mmc_data *data)
+{
+	struct sunxi_idma_des *pdes = (struct sunxi_idma_des *)host->sg_cpu;
+	struct sunxi_idma_des *pdes_pa = (struct sunxi_idma_des *)host->sg_dma;
+	int i, max_len = (1 << host->idma_des_size_bits);
+
+	for (i = 0; i < data->sg_len; i++) {
+		pdes[i].config = SDXC_IDMAC_DES0_CH | SDXC_IDMAC_DES0_OWN |
+				 SDXC_IDMAC_DES0_DIC;
+
+		if (data->sg[i].length == max_len)
+			pdes[i].buf_size = 0; /* 0 == max_len */
+		else
+			pdes[i].buf_size = data->sg[i].length;
+
+		pdes[i].buf_addr_ptr1 = sg_dma_address(&data->sg[i]);
+		pdes[i].buf_addr_ptr2 = (u32)&pdes_pa[i + 1];
+	}
+
+	pdes[0].config |= SDXC_IDMAC_DES0_FD;
+	pdes[i - 1].config = SDXC_IDMAC_DES0_OWN | SDXC_IDMAC_DES0_LD;
+
+	/*
+	 * Avoid the io-store starting the idmac hitting io-mem before the
+	 * descriptors hit the main-mem.
+	 */
+	wmb();
+}
+
+static enum dma_data_direction sunxi_mmc_get_dma_dir(struct mmc_data *data)
+{
+	if (data->flags & MMC_DATA_WRITE)
+		return DMA_TO_DEVICE;
+	else
+		return DMA_FROM_DEVICE;
+}
+
+static int sunxi_mmc_map_dma(struct sunxi_mmc_host *host,
+			     struct mmc_data *data)
+{
+	u32 i, dma_len;
+	struct scatterlist *sg;
+
+	dma_len = dma_map_sg(mmc_dev(host->mmc), data->sg, data->sg_len,
+			     sunxi_mmc_get_dma_dir(data));
+	if (dma_len == 0) {
+		dev_err(mmc_dev(host->mmc), "dma_map_sg failed\n");
+		return -ENOMEM;
+	}
+
+	for_each_sg(data->sg, sg, data->sg_len, i) {
+		if (sg->offset & 3 || sg->length & 3) {
+			dev_err(mmc_dev(host->mmc),
+				"unaligned scatterlist: os %x length %d\n",
+				sg->offset, sg->length);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static void sunxi_mmc_start_dma(struct sunxi_mmc_host *host,
+				struct mmc_data *data)
+{
+	u32 rval;
+
+	sunxi_mmc_init_idma_des(host, data);
+
+	rval = mmc_readl(host, REG_GCTRL);
+	rval |= SDXC_DMA_ENABLE_BIT;
+	mmc_writel(host, REG_GCTRL, rval);
+	rval |= SDXC_DMA_RESET;
+	mmc_writel(host, REG_GCTRL, rval);
+
+	mmc_writel(host, REG_DMAC, SDXC_IDMAC_SOFT_RESET);
+
+	if (!(data->flags & MMC_DATA_WRITE))
+		mmc_writel(host, REG_IDIE, SDXC_IDMAC_RECEIVE_INTERRUPT);
+
+	mmc_writel(host, REG_DMAC,
+		   SDXC_IDMAC_FIX_BURST | SDXC_IDMAC_IDMA_ON);
+}
+
+static void sunxi_mmc_send_manual_stop(struct sunxi_mmc_host *host,
+				       struct mmc_request *req)
+{
+	u32 arg, cmd_val, ri;
+	unsigned long expire = jiffies + msecs_to_jiffies(1000);
+
+	cmd_val = SDXC_START | SDXC_RESP_EXPIRE |
+		  SDXC_STOP_ABORT_CMD | SDXC_CHECK_RESPONSE_CRC;
+
+	if (req->cmd->opcode == SD_IO_RW_EXTENDED) {
+		cmd_val |= SD_IO_RW_DIRECT;
+		arg = (1 << 31) | (0 << 28) | (SDIO_CCCR_ABORT << 9) |
+		      ((req->cmd->arg >> 28) & 0x7);
+	} else {
+		cmd_val |= MMC_STOP_TRANSMISSION;
+		arg = 0;
+	}
+
+	mmc_writel(host, REG_CARG, arg);
+	mmc_writel(host, REG_CMDR, cmd_val);
+
+	do {
+		ri = mmc_readl(host, REG_RINTR);
+	} while (!(ri & (SDXC_COMMAND_DONE | SDXC_INTERRUPT_ERROR_BIT)) &&
+		 time_before(jiffies, expire));
+
+	if (!(ri & SDXC_COMMAND_DONE) || (ri & SDXC_INTERRUPT_ERROR_BIT)) {
+		dev_err(mmc_dev(host->mmc), "send stop command failed\n");
+		if (req->stop)
+			req->stop->resp[0] = -ETIMEDOUT;
+	} else {
+		if (req->stop)
+			req->stop->resp[0] = mmc_readl(host, REG_RESP0);
+	}
+
+	mmc_writel(host, REG_RINTR, 0xffff);
+}
+
+static void sunxi_mmc_dump_errinfo(struct sunxi_mmc_host *host)
+{
+	struct mmc_command *cmd = host->mrq->cmd;
+	struct mmc_data *data = host->mrq->data;
+
+	/* For some cmds timeout is normal with sd/mmc cards */
+	if ((host->int_sum & SDXC_INTERRUPT_ERROR_BIT) ==
+		SDXC_RESP_TIMEOUT && (cmd->opcode == SD_IO_SEND_OP_COND ||
+				      cmd->opcode == SD_IO_RW_DIRECT))
+		return;
+
+	dev_err(mmc_dev(host->mmc),
+		"smc %d err, cmd %d,%s%s%s%s%s%s%s%s%s%s !!\n",
+		host->mmc->index, cmd->opcode,
+		data ? (data->flags & MMC_DATA_WRITE ? " WR" : " RD") : "",
+		host->int_sum & SDXC_RESP_ERROR     ? " RE"     : "",
+		host->int_sum & SDXC_RESP_CRC_ERROR  ? " RCE"    : "",
+		host->int_sum & SDXC_DATA_CRC_ERROR  ? " DCE"    : "",
+		host->int_sum & SDXC_RESP_TIMEOUT ? " RTO"    : "",
+		host->int_sum & SDXC_DATA_TIMEOUT ? " DTO"    : "",
+		host->int_sum & SDXC_FIFO_RUN_ERROR  ? " FE"     : "",
+		host->int_sum & SDXC_HARD_WARE_LOCKED ? " HL"     : "",
+		host->int_sum & SDXC_START_BIT_ERROR ? " SBE"    : "",
+		host->int_sum & SDXC_END_BIT_ERROR   ? " EBE"    : ""
+		);
+}
+
+/* Called in interrupt context! */
+static irqreturn_t sunxi_mmc_finalize_request(struct sunxi_mmc_host *host)
+{
+	struct mmc_request *mrq = host->mrq;
+	struct mmc_data *data = mrq->data;
+	u32 rval;
+
+	mmc_writel(host, REG_IMASK, host->sdio_imask);
+	mmc_writel(host, REG_IDIE, 0);
+
+	if (host->int_sum & SDXC_INTERRUPT_ERROR_BIT) {
+		sunxi_mmc_dump_errinfo(host);
+		mrq->cmd->error = -ETIMEDOUT;
+
+		if (data) {
+			data->error = -ETIMEDOUT;
+			host->manual_stop_mrq = mrq;
+		}
+
+		if (mrq->stop)
+			mrq->stop->error = -ETIMEDOUT;
+	} else {
+		if (mrq->cmd->flags & MMC_RSP_136) {
+			mrq->cmd->resp[0] = mmc_readl(host, REG_RESP3);
+			mrq->cmd->resp[1] = mmc_readl(host, REG_RESP2);
+			mrq->cmd->resp[2] = mmc_readl(host, REG_RESP1);
+			mrq->cmd->resp[3] = mmc_readl(host, REG_RESP0);
+		} else {
+			mrq->cmd->resp[0] = mmc_readl(host, REG_RESP0);
+		}
+
+		if (data)
+			data->bytes_xfered = data->blocks * data->blksz;
+	}
+
+	if (data) {
+		mmc_writel(host, REG_IDST, 0x337);
+		mmc_writel(host, REG_DMAC, 0);
+		rval = mmc_readl(host, REG_GCTRL);
+		rval |= SDXC_DMA_RESET;
+		mmc_writel(host, REG_GCTRL, rval);
+		rval &= ~SDXC_DMA_ENABLE_BIT;
+		mmc_writel(host, REG_GCTRL, rval);
+		rval |= SDXC_FIFO_RESET;
+		mmc_writel(host, REG_GCTRL, rval);
+		dma_unmap_sg(mmc_dev(host->mmc), data->sg, data->sg_len,
+				     sunxi_mmc_get_dma_dir(data));
+	}
+
+	mmc_writel(host, REG_RINTR, 0xffff);
+
+	host->mrq = NULL;
+	host->int_sum = 0;
+	host->wait_dma = false;
+
+	return host->manual_stop_mrq ? IRQ_WAKE_THREAD : IRQ_HANDLED;
+}
+
+static irqreturn_t sunxi_mmc_irq(int irq, void *dev_id)
+{
+	struct sunxi_mmc_host *host = dev_id;
+	struct mmc_request *mrq;
+	u32 msk_int, idma_int;
+	bool finalize = false;
+	bool sdio_int = false;
+	irqreturn_t ret = IRQ_HANDLED;
+
+	spin_lock(&host->lock);
+
+	idma_int  = mmc_readl(host, REG_IDST);
+	msk_int   = mmc_readl(host, REG_MISTA);
+
+	dev_dbg(mmc_dev(host->mmc), "irq: rq %p mi %08x idi %08x\n",
+		host->mrq, msk_int, idma_int);
+
+	mrq = host->mrq;
+	if (mrq) {
+		if (idma_int & SDXC_IDMAC_RECEIVE_INTERRUPT)
+			host->wait_dma = false;
+
+		host->int_sum |= msk_int;
+
+		/* Wait for COMMAND_DONE on RESPONSE_TIMEOUT before finalize */
+		if ((host->int_sum & SDXC_RESP_TIMEOUT) &&
+				!(host->int_sum & SDXC_COMMAND_DONE))
+			mmc_writel(host, REG_IMASK,
+				   host->sdio_imask | SDXC_COMMAND_DONE);
+		/* Don't wait for dma on error */
+		else if (host->int_sum & SDXC_INTERRUPT_ERROR_BIT)
+			finalize = true;
+		else if ((host->int_sum & SDXC_INTERRUPT_DONE_BIT) &&
+				!host->wait_dma)
+			finalize = true;
+	}
+
+	if (msk_int & SDXC_SDIO_INTERRUPT)
+		sdio_int = true;
+
+	mmc_writel(host, REG_RINTR, msk_int);
+	mmc_writel(host, REG_IDST, idma_int);
+
+	if (finalize)
+		ret = sunxi_mmc_finalize_request(host);
+
+	spin_unlock(&host->lock);
+
+	if (finalize && ret == IRQ_HANDLED)
+		mmc_request_done(host->mmc, mrq);
+
+	if (sdio_int)
+		mmc_signal_sdio_irq(host->mmc);
+
+	return ret;
+}
+
+static irqreturn_t sunxi_mmc_handle_manual_stop(int irq, void *dev_id)
+{
+	struct sunxi_mmc_host *host = dev_id;
+	struct mmc_request *mrq;
+	unsigned long iflags;
+
+	spin_lock_irqsave(&host->lock, iflags);
+	mrq = host->manual_stop_mrq;
+	spin_unlock_irqrestore(&host->lock, iflags);
+
+	if (!mrq) {
+		dev_err(mmc_dev(host->mmc), "no request for manual stop\n");
+		return IRQ_HANDLED;
+	}
+
+	dev_err(mmc_dev(host->mmc), "data error, sending stop command\n");
+	sunxi_mmc_send_manual_stop(host, mrq);
+
+	spin_lock_irqsave(&host->lock, iflags);
+	host->manual_stop_mrq = NULL;
+	spin_unlock_irqrestore(&host->lock, iflags);
+
+	mmc_request_done(host->mmc, mrq);
+
+	return IRQ_HANDLED;
+}
+
+static int sunxi_mmc_oclk_onoff(struct sunxi_mmc_host *host, u32 oclk_en)
+{
+	unsigned long expire = jiffies + msecs_to_jiffies(250);
+	u32 rval;
+
+	rval = mmc_readl(host, REG_CLKCR);
+	rval &= ~(SDXC_CARD_CLOCK_ON | SDXC_LOW_POWER_ON);
+
+	if (oclk_en)
+		rval |= SDXC_CARD_CLOCK_ON;
+
+	mmc_writel(host, REG_CLKCR, rval);
+
+	rval = SDXC_START | SDXC_UPCLK_ONLY | SDXC_WAIT_PRE_OVER;
+	mmc_writel(host, REG_CMDR, rval);
+
+	do {
+		rval = mmc_readl(host, REG_CMDR);
+	} while (time_before(jiffies, expire) && (rval & SDXC_START));
+
+	/* clear irq status bits set by the command */
+	mmc_writel(host, REG_RINTR,
+		   mmc_readl(host, REG_RINTR) & ~SDXC_SDIO_INTERRUPT);
+
+	if (rval & SDXC_START) {
+		dev_err(mmc_dev(host->mmc), "fatal err update clk timeout\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int sunxi_mmc_clk_set_rate(struct sunxi_mmc_host *host,
+				  struct mmc_ios *ios)
+{
+	u32 rate, oclk_dly, rval, sclk_dly, src_clk;
+	int ret;
+
+	rate = clk_round_rate(host->clk_mmc, ios->clock);
+	dev_dbg(mmc_dev(host->mmc), "setting clk to %d, rounded %d\n",
+		ios->clock, rate);
+
+	/* setting clock rate */
+	ret = clk_set_rate(host->clk_mmc, rate);
+	if (ret) {
+		dev_err(mmc_dev(host->mmc), "error setting clk to %d: %d\n",
+			rate, ret);
+		return ret;
+	}
+
+	ret = sunxi_mmc_oclk_onoff(host, 0);
+	if (ret)
+		return ret;
+
+	/* clear internal divider */
+	rval = mmc_readl(host, REG_CLKCR);
+	rval &= ~0xff;
+	mmc_writel(host, REG_CLKCR, rval);
+
+	/* determine delays */
+	if (rate <= 400000) {
+		oclk_dly = 0;
+		sclk_dly = 7;
+	} else if (rate <= 25000000) {
+		oclk_dly = 0;
+		sclk_dly = 5;
+	} else if (rate <= 50000000) {
+		if (ios->timing == MMC_TIMING_UHS_DDR50) {
+			oclk_dly = 2;
+			sclk_dly = 4;
+		} else {
+			oclk_dly = 3;
+			sclk_dly = 5;
+		}
+	} else {
+		/* rate > 50000000 */
+		oclk_dly = 2;
+		sclk_dly = 4;
+	}
+
+	src_clk = clk_get_rate(clk_get_parent(host->clk_mmc));
+	if (src_clk >= 300000000 && src_clk <= 400000000) {
+		if (oclk_dly)
+			oclk_dly--;
+		if (sclk_dly)
+			sclk_dly--;
+	}
+
+	clk_sunxi_mmc_phase_control(host->clk_mmc, sclk_dly, oclk_dly);
+
+	return sunxi_mmc_oclk_onoff(host, 1);
+}
+
+static void sunxi_mmc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
+{
+	struct sunxi_mmc_host *host = mmc_priv(mmc);
+	u32 rval;
+
+	/* Set the power state */
+	switch (ios->power_mode) {
+	case MMC_POWER_ON:
+		break;
+
+	case MMC_POWER_UP:
+		mmc_regulator_set_ocr(mmc, mmc->supply.vmmc, ios->vdd);
+
+		host->ferror = sunxi_mmc_init_host(mmc);
+		if (host->ferror)
+			return;
+
+		dev_dbg(mmc_dev(mmc), "power on!\n");
+		break;
+
+	case MMC_POWER_OFF:
+		dev_dbg(mmc_dev(mmc), "power off!\n");
+		sunxi_mmc_reset_host(host);
+		mmc_regulator_set_ocr(mmc, mmc->supply.vmmc, 0);
+		break;
+	}
+
+	/* set bus width */
+	switch (ios->bus_width) {
+	case MMC_BUS_WIDTH_1:
+		mmc_writel(host, REG_WIDTH, SDXC_WIDTH1);
+		break;
+	case MMC_BUS_WIDTH_4:
+		mmc_writel(host, REG_WIDTH, SDXC_WIDTH4);
+		break;
+	case MMC_BUS_WIDTH_8:
+		mmc_writel(host, REG_WIDTH, SDXC_WIDTH8);
+		break;
+	}
+
+	/* set ddr mode */
+	rval = mmc_readl(host, REG_GCTRL);
+	if (ios->timing == MMC_TIMING_UHS_DDR50)
+		rval |= SDXC_DDR_MODE;
+	else
+		rval &= ~SDXC_DDR_MODE;
+	mmc_writel(host, REG_GCTRL, rval);
+
+	/* set up clock */
+	if (ios->clock && ios->power_mode) {
+		host->ferror = sunxi_mmc_clk_set_rate(host, ios);
+		/* Android code had a usleep_range(50000, 55000); here */
+	}
+}
+
+static void sunxi_mmc_enable_sdio_irq(struct mmc_host *mmc, int enable)
+{
+	struct sunxi_mmc_host *host = mmc_priv(mmc);
+	unsigned long flags;
+	u32 imask;
+
+	spin_lock_irqsave(&host->lock, flags);
+
+	imask = mmc_readl(host, REG_IMASK);
+	if (enable) {
+		host->sdio_imask = SDXC_SDIO_INTERRUPT;
+		imask |= SDXC_SDIO_INTERRUPT;
+	} else {
+		host->sdio_imask = 0;
+		imask &= ~SDXC_SDIO_INTERRUPT;
+	}
+	mmc_writel(host, REG_IMASK, imask);
+	spin_unlock_irqrestore(&host->lock, flags);
+}
+
+static void sunxi_mmc_hw_reset(struct mmc_host *mmc)
+{
+	struct sunxi_mmc_host *host = mmc_priv(mmc);
+	mmc_writel(host, REG_HWRST, 0);
+	udelay(10);
+	mmc_writel(host, REG_HWRST, 1);
+	udelay(300);
+}
+
+static void sunxi_mmc_request(struct mmc_host *mmc, struct mmc_request *mrq)
+{
+	struct sunxi_mmc_host *host = mmc_priv(mmc);
+	struct mmc_command *cmd = mrq->cmd;
+	struct mmc_data *data = mrq->data;
+	unsigned long iflags;
+	u32 imask = SDXC_INTERRUPT_ERROR_BIT;
+	u32 cmd_val = SDXC_START | (cmd->opcode & 0x3f);
+	int ret;
+
+	/* Check for set_ios errors (should never happen) */
+	if (host->ferror) {
+		mrq->cmd->error = host->ferror;
+		mmc_request_done(mmc, mrq);
+		return;
+	}
+
+	if (data) {
+		ret = sunxi_mmc_map_dma(host, data);
+		if (ret < 0) {
+			dev_err(mmc_dev(mmc), "map DMA failed\n");
+			cmd->error = ret;
+			data->error = ret;
+			mmc_request_done(mmc, mrq);
+			return;
+		}
+	}
+
+	if (cmd->opcode == MMC_GO_IDLE_STATE) {
+		cmd_val |= SDXC_SEND_INIT_SEQUENCE;
+		imask |= SDXC_COMMAND_DONE;
+	}
+
+	if (cmd->flags & MMC_RSP_PRESENT) {
+		cmd_val |= SDXC_RESP_EXPIRE;
+		if (cmd->flags & MMC_RSP_136)
+			cmd_val |= SDXC_LONG_RESPONSE;
+		if (cmd->flags & MMC_RSP_CRC)
+			cmd_val |= SDXC_CHECK_RESPONSE_CRC;
+
+		if ((cmd->flags & MMC_CMD_MASK) == MMC_CMD_ADTC) {
+			cmd_val |= SDXC_DATA_EXPIRE | SDXC_WAIT_PRE_OVER;
+			if (cmd->data->flags & MMC_DATA_STREAM) {
+				imask |= SDXC_AUTO_COMMAND_DONE;
+				cmd_val |= SDXC_SEQUENCE_MODE |
+					   SDXC_SEND_AUTO_STOP;
+			}
+
+			if (cmd->data->stop) {
+				imask |= SDXC_AUTO_COMMAND_DONE;
+				cmd_val |= SDXC_SEND_AUTO_STOP;
+			} else {
+				imask |= SDXC_DATA_OVER;
+			}
+
+			if (cmd->data->flags & MMC_DATA_WRITE)
+				cmd_val |= SDXC_WRITE;
+			else
+				host->wait_dma = true;
+		} else {
+			imask |= SDXC_COMMAND_DONE;
+		}
+	} else {
+		imask |= SDXC_COMMAND_DONE;
+	}
+
+	dev_dbg(mmc_dev(mmc), "cmd %d(%08x) arg %x ie 0x%08x len %d\n",
+		cmd_val & 0x3f, cmd_val, cmd->arg, imask,
+		mrq->data ? mrq->data->blksz * mrq->data->blocks : 0);
+
+	spin_lock_irqsave(&host->lock, iflags);
+
+	if (host->mrq || host->manual_stop_mrq) {
+		spin_unlock_irqrestore(&host->lock, iflags);
+
+		if (data)
+			dma_unmap_sg(mmc_dev(mmc), data->sg, data->sg_len,
+				     sunxi_mmc_get_dma_dir(data));
+
+		dev_err(mmc_dev(mmc), "request already pending\n");
+		mrq->cmd->error = -EBUSY;
+		mmc_request_done(mmc, mrq);
+		return;
+	}
+
+	if (data) {
+		mmc_writel(host, REG_BLKSZ, data->blksz);
+		mmc_writel(host, REG_BCNTR, data->blksz * data->blocks);
+		sunxi_mmc_start_dma(host, data);
+	}
+
+	host->mrq = mrq;
+	mmc_writel(host, REG_IMASK, host->sdio_imask | imask);
+	mmc_writel(host, REG_CARG, cmd->arg);
+	mmc_writel(host, REG_CMDR, cmd_val);
+
+	spin_unlock_irqrestore(&host->lock, iflags);
+}
+
+static const struct of_device_id sunxi_mmc_of_match[] = {
+	{ .compatible = "allwinner,sun4i-a10-mmc", },
+	{ .compatible = "allwinner,sun5i-a13-mmc", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, sunxi_mmc_of_match);
+
+static struct mmc_host_ops sunxi_mmc_ops = {
+	.request	 = sunxi_mmc_request,
+	.set_ios	 = sunxi_mmc_set_ios,
+	.get_ro		 = mmc_gpio_get_ro,
+	.get_cd		 = mmc_gpio_get_cd,
+	.enable_sdio_irq = sunxi_mmc_enable_sdio_irq,
+	.hw_reset	 = sunxi_mmc_hw_reset,
+};
+
+static int sunxi_mmc_resource_request(struct sunxi_mmc_host *host,
+				      struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	int ret;
+
+	if (of_device_is_compatible(np, "allwinner,sun4i-a10-mmc"))
+		host->idma_des_size_bits = 13;
+	else
+		host->idma_des_size_bits = 16;
+
+	ret = mmc_regulator_get_supply(host->mmc);
+	if (ret) {
+		if (ret != -EPROBE_DEFER)
+			dev_err(&pdev->dev, "Could not get vmmc supply\n");
+		return ret;
+	}
+
+	host->reg_base = devm_ioremap_resource(&pdev->dev,
+			      platform_get_resource(pdev, IORESOURCE_MEM, 0));
+	if (IS_ERR(host->reg_base))
+		return PTR_ERR(host->reg_base);
+
+	host->clk_ahb = devm_clk_get(&pdev->dev, "ahb");
+	if (IS_ERR(host->clk_ahb)) {
+		dev_err(&pdev->dev, "Could not get ahb clock\n");
+		return PTR_ERR(host->clk_ahb);
+	}
+
+	host->clk_mmc = devm_clk_get(&pdev->dev, "mmc");
+	if (IS_ERR(host->clk_mmc)) {
+		dev_err(&pdev->dev, "Could not get mmc clock\n");
+		return PTR_ERR(host->clk_mmc);
+	}
+
+	host->reset = devm_reset_control_get(&pdev->dev, "ahb");
+
+	ret = clk_prepare_enable(host->clk_ahb);
+	if (ret) {
+		dev_err(&pdev->dev, "Enable ahb clk err %d\n", ret);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(host->clk_mmc);
+	if (ret) {
+		dev_err(&pdev->dev, "Enable mmc clk err %d\n", ret);
+		goto error_disable_clk_ahb;
+	}
+
+	if (!IS_ERR(host->reset)) {
+		ret = reset_control_deassert(host->reset);
+		if (ret) {
+			dev_err(&pdev->dev, "reset err %d\n", ret);
+			goto error_disable_clk_mmc;
+		}
+	}
+
+	/*
+	 * Sometimes the controller asserts the irq on boot for some reason,
+	 * make sure the controller is in a sane state before enabling irqs.
+	 */
+	ret = sunxi_mmc_reset_host(host);
+	if (ret)
+		goto error_assert_reset;
+
+	host->irq = platform_get_irq(pdev, 0);
+	return devm_request_threaded_irq(&pdev->dev, host->irq, sunxi_mmc_irq,
+			sunxi_mmc_handle_manual_stop, 0, "sunxi-mmc", host);
+
+error_assert_reset:
+	if (!IS_ERR(host->reset))
+		reset_control_assert(host->reset);
+error_disable_clk_mmc:
+	clk_disable_unprepare(host->clk_mmc);
+error_disable_clk_ahb:
+	clk_disable_unprepare(host->clk_ahb);
+	return ret;
+}
+
+static int sunxi_mmc_probe(struct platform_device *pdev)
+{
+	struct sunxi_mmc_host *host;
+	struct mmc_host *mmc;
+	int ret;
+
+	mmc = mmc_alloc_host(sizeof(struct sunxi_mmc_host), &pdev->dev);
+	if (!mmc) {
+		dev_err(&pdev->dev, "mmc alloc host failed\n");
+		return -ENOMEM;
+	}
+
+	host = mmc_priv(mmc);
+	host->mmc = mmc;
+	spin_lock_init(&host->lock);
+
+	ret = sunxi_mmc_resource_request(host, pdev);
+	if (ret)
+		goto error_free_host;
+
+	host->sg_cpu = dma_alloc_coherent(&pdev->dev, PAGE_SIZE,
+					  &host->sg_dma, GFP_KERNEL);
+	if (!host->sg_cpu) {
+		dev_err(&pdev->dev, "Failed to allocate DMA descriptor mem\n");
+		ret = -ENOMEM;
+		goto error_free_host;
+	}
+
+	mmc->ops		= &sunxi_mmc_ops;
+	mmc->max_blk_count	= 8192;
+	mmc->max_blk_size	= 4096;
+	mmc->max_segs		= PAGE_SIZE / sizeof(struct sunxi_idma_des);
+	mmc->max_seg_size	= (1 << host->idma_des_size_bits);
+	mmc->max_req_size	= mmc->max_seg_size * mmc->max_segs;
+	/* 400kHz ~ 50MHz */
+	mmc->f_min		=   400000;
+	mmc->f_max		= 50000000;
+	mmc->caps	       |= MMC_CAP_MMC_HIGHSPEED | MMC_CAP_SD_HIGHSPEED;
+
+	ret = mmc_of_parse(mmc);
+	if (ret)
+		goto error_free_dma;
+
+	ret = mmc_add_host(mmc);
+	if (ret)
+		goto error_free_dma;
+
+	dev_info(&pdev->dev, "base:0x%p irq:%u\n", host->reg_base, host->irq);
+	platform_set_drvdata(pdev, mmc);
+	return 0;
+
+error_free_dma:
+	dma_free_coherent(&pdev->dev, PAGE_SIZE, host->sg_cpu, host->sg_dma);
+error_free_host:
+	mmc_free_host(mmc);
+	return ret;
+}
+
+static int sunxi_mmc_remove(struct platform_device *pdev)
+{
+	struct mmc_host	*mmc = platform_get_drvdata(pdev);
+	struct sunxi_mmc_host *host = mmc_priv(mmc);
+
+	mmc_remove_host(mmc);
+	disable_irq(host->irq);
+	sunxi_mmc_reset_host(host);
+
+	if (!IS_ERR(host->reset))
+		reset_control_assert(host->reset);
+
+	clk_disable_unprepare(host->clk_mmc);
+	clk_disable_unprepare(host->clk_ahb);
+
+	dma_free_coherent(&pdev->dev, PAGE_SIZE, host->sg_cpu, host->sg_dma);
+	mmc_free_host(mmc);
+
+	return 0;
+}
+
+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,
+	.remove		= sunxi_mmc_remove,
+};
+module_platform_driver(sunxi_mmc_driver);
+
+MODULE_DESCRIPTION("Allwinner's SD/MMC Card Controller Driver");
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("David Lanzend�rfer <david.lanzendoerfer@o2s.ch>");
+MODULE_ALIAS("platform:sunxi-mmc");
diff --git a/drivers/mmc/host/usdhi6rol0.c b/drivers/mmc/host/usdhi6rol0.c
new file mode 100644
index 00000000000..eb2bbbef19c
--- /dev/null
+++ b/drivers/mmc/host/usdhi6rol0.c
@@ -0,0 +1,1847 @@
+/*
+ * Copyright (C) 2013-2014 Renesas Electronics Europe Ltd.
+ * Author: Guennadi Liakhovetski <g.liakhovetski@gmx.de>
+ *
+ * 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.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmaengine.h>
+#include <linux/highmem.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/log2.h>
+#include <linux/mmc/host.h>
+#include <linux/mmc/mmc.h>
+#include <linux/mmc/sd.h>
+#include <linux/mmc/sdio.h>
+#include <linux/module.h>
+#include <linux/pagemap.h>
+#include <linux/platform_device.h>
+#include <linux/scatterlist.h>
+#include <linux/string.h>
+#include <linux/time.h>
+#include <linux/virtio.h>
+#include <linux/workqueue.h>
+
+#define USDHI6_SD_CMD		0x0000
+#define USDHI6_SD_PORT_SEL	0x0004
+#define USDHI6_SD_ARG		0x0008
+#define USDHI6_SD_STOP		0x0010
+#define USDHI6_SD_SECCNT	0x0014
+#define USDHI6_SD_RSP10		0x0018
+#define USDHI6_SD_RSP32		0x0020
+#define USDHI6_SD_RSP54		0x0028
+#define USDHI6_SD_RSP76		0x0030
+#define USDHI6_SD_INFO1		0x0038
+#define USDHI6_SD_INFO2		0x003c
+#define USDHI6_SD_INFO1_MASK	0x0040
+#define USDHI6_SD_INFO2_MASK	0x0044
+#define USDHI6_SD_CLK_CTRL	0x0048
+#define USDHI6_SD_SIZE		0x004c
+#define USDHI6_SD_OPTION	0x0050
+#define USDHI6_SD_ERR_STS1	0x0058
+#define USDHI6_SD_ERR_STS2	0x005c
+#define USDHI6_SD_BUF0		0x0060
+#define USDHI6_SDIO_MODE	0x0068
+#define USDHI6_SDIO_INFO1	0x006c
+#define USDHI6_SDIO_INFO1_MASK	0x0070
+#define USDHI6_CC_EXT_MODE	0x01b0
+#define USDHI6_SOFT_RST		0x01c0
+#define USDHI6_VERSION		0x01c4
+#define USDHI6_HOST_MODE	0x01c8
+#define USDHI6_SDIF_MODE	0x01cc
+
+#define USDHI6_SD_CMD_APP		0x0040
+#define USDHI6_SD_CMD_MODE_RSP_AUTO	0x0000
+#define USDHI6_SD_CMD_MODE_RSP_NONE	0x0300
+#define USDHI6_SD_CMD_MODE_RSP_R1	0x0400	/* Also R5, R6, R7 */
+#define USDHI6_SD_CMD_MODE_RSP_R1B	0x0500	/* R1b */
+#define USDHI6_SD_CMD_MODE_RSP_R2	0x0600
+#define USDHI6_SD_CMD_MODE_RSP_R3	0x0700	/* Also R4 */
+#define USDHI6_SD_CMD_DATA		0x0800
+#define USDHI6_SD_CMD_READ		0x1000
+#define USDHI6_SD_CMD_MULTI		0x2000
+#define USDHI6_SD_CMD_CMD12_AUTO_OFF	0x4000
+
+#define USDHI6_CC_EXT_MODE_SDRW		BIT(1)
+
+#define USDHI6_SD_INFO1_RSP_END		BIT(0)
+#define USDHI6_SD_INFO1_ACCESS_END	BIT(2)
+#define USDHI6_SD_INFO1_CARD_OUT	BIT(3)
+#define USDHI6_SD_INFO1_CARD_IN		BIT(4)
+#define USDHI6_SD_INFO1_CD		BIT(5)
+#define USDHI6_SD_INFO1_WP		BIT(7)
+#define USDHI6_SD_INFO1_D3_CARD_OUT	BIT(8)
+#define USDHI6_SD_INFO1_D3_CARD_IN	BIT(9)
+
+#define USDHI6_SD_INFO2_CMD_ERR		BIT(0)
+#define USDHI6_SD_INFO2_CRC_ERR		BIT(1)
+#define USDHI6_SD_INFO2_END_ERR		BIT(2)
+#define USDHI6_SD_INFO2_TOUT		BIT(3)
+#define USDHI6_SD_INFO2_IWA_ERR		BIT(4)
+#define USDHI6_SD_INFO2_IRA_ERR		BIT(5)
+#define USDHI6_SD_INFO2_RSP_TOUT	BIT(6)
+#define USDHI6_SD_INFO2_SDDAT0		BIT(7)
+#define USDHI6_SD_INFO2_BRE		BIT(8)
+#define USDHI6_SD_INFO2_BWE		BIT(9)
+#define USDHI6_SD_INFO2_SCLKDIVEN	BIT(13)
+#define USDHI6_SD_INFO2_CBSY		BIT(14)
+#define USDHI6_SD_INFO2_ILA		BIT(15)
+
+#define USDHI6_SD_INFO1_CARD_INSERT (USDHI6_SD_INFO1_CARD_IN | USDHI6_SD_INFO1_D3_CARD_IN)
+#define USDHI6_SD_INFO1_CARD_EJECT (USDHI6_SD_INFO1_CARD_OUT | USDHI6_SD_INFO1_D3_CARD_OUT)
+#define USDHI6_SD_INFO1_CARD (USDHI6_SD_INFO1_CARD_INSERT | USDHI6_SD_INFO1_CARD_EJECT)
+#define USDHI6_SD_INFO1_CARD_CD (USDHI6_SD_INFO1_CARD_IN | USDHI6_SD_INFO1_CARD_OUT)
+
+#define USDHI6_SD_INFO2_ERR	(USDHI6_SD_INFO2_CMD_ERR |	\
+	USDHI6_SD_INFO2_CRC_ERR | USDHI6_SD_INFO2_END_ERR |	\
+	USDHI6_SD_INFO2_TOUT | USDHI6_SD_INFO2_IWA_ERR |	\
+	USDHI6_SD_INFO2_IRA_ERR | USDHI6_SD_INFO2_RSP_TOUT |	\
+	USDHI6_SD_INFO2_ILA)
+
+#define USDHI6_SD_INFO1_IRQ	(USDHI6_SD_INFO1_RSP_END | USDHI6_SD_INFO1_ACCESS_END | \
+				 USDHI6_SD_INFO1_CARD)
+
+#define USDHI6_SD_INFO2_IRQ	(USDHI6_SD_INFO2_ERR | USDHI6_SD_INFO2_BRE | \
+				 USDHI6_SD_INFO2_BWE | 0x0800 | USDHI6_SD_INFO2_ILA)
+
+#define USDHI6_SD_CLK_CTRL_SCLKEN	BIT(8)
+
+#define USDHI6_SD_STOP_STP		BIT(0)
+#define USDHI6_SD_STOP_SEC		BIT(8)
+
+#define USDHI6_SDIO_INFO1_IOIRQ		BIT(0)
+#define USDHI6_SDIO_INFO1_EXPUB52	BIT(14)
+#define USDHI6_SDIO_INFO1_EXWT		BIT(15)
+
+#define USDHI6_SD_ERR_STS1_CRC_NO_ERROR	BIT(13)
+
+#define USDHI6_SOFT_RST_RESERVED	(BIT(1) | BIT(2))
+#define USDHI6_SOFT_RST_RESET		BIT(0)
+
+#define USDHI6_SD_OPTION_TIMEOUT_SHIFT	4
+#define USDHI6_SD_OPTION_TIMEOUT_MASK	(0xf << USDHI6_SD_OPTION_TIMEOUT_SHIFT)
+#define USDHI6_SD_OPTION_WIDTH_1	BIT(15)
+
+#define USDHI6_SD_PORT_SEL_PORTS_SHIFT	8
+
+#define USDHI6_SD_CLK_CTRL_DIV_MASK	0xff
+
+#define USDHI6_SDIO_INFO1_IRQ	(USDHI6_SDIO_INFO1_IOIRQ | 3 | \
+				 USDHI6_SDIO_INFO1_EXPUB52 | USDHI6_SDIO_INFO1_EXWT)
+
+#define USDHI6_MIN_DMA 64
+
+enum usdhi6_wait_for {
+	USDHI6_WAIT_FOR_REQUEST,
+	USDHI6_WAIT_FOR_CMD,
+	USDHI6_WAIT_FOR_MREAD,
+	USDHI6_WAIT_FOR_MWRITE,
+	USDHI6_WAIT_FOR_READ,
+	USDHI6_WAIT_FOR_WRITE,
+	USDHI6_WAIT_FOR_DATA_END,
+	USDHI6_WAIT_FOR_STOP,
+	USDHI6_WAIT_FOR_DMA,
+};
+
+struct usdhi6_page {
+	struct page *page;
+	void *mapped;		/* mapped page */
+};
+
+struct usdhi6_host {
+	struct mmc_host *mmc;
+	struct mmc_request *mrq;
+	void __iomem *base;
+	struct clk *clk;
+
+	/* SG memory handling */
+
+	/* Common for multiple and single block requests */
+	struct usdhi6_page pg;	/* current page from an SG */
+	void *blk_page;		/* either a mapped page, or the bounce buffer */
+	size_t offset;		/* offset within a page, including sg->offset */
+
+	/* Blocks, crossing a page boundary */
+	size_t head_len;
+	struct usdhi6_page head_pg;
+
+	/* A bounce buffer for unaligned blocks or blocks, crossing a page boundary */
+	struct scatterlist bounce_sg;
+	u8 bounce_buf[512];
+
+	/* Multiple block requests only */
+	struct scatterlist *sg;	/* current SG segment */
+	int page_idx;		/* page index within an SG segment */
+
+	enum usdhi6_wait_for wait;
+	u32 status_mask;
+	u32 status2_mask;
+	u32 sdio_mask;
+	u32 io_error;
+	u32 irq_status;
+	unsigned long imclk;
+	unsigned long rate;
+	bool app_cmd;
+
+	/* Timeout handling */
+	struct delayed_work timeout_work;
+	unsigned long timeout;
+
+	/* DMA support */
+	struct dma_chan *chan_rx;
+	struct dma_chan *chan_tx;
+	bool dma_active;
+};
+
+/*			I/O primitives					*/
+
+static void usdhi6_write(struct usdhi6_host *host, u32 reg, u32 data)
+{
+	iowrite32(data, host->base + reg);
+	dev_vdbg(mmc_dev(host->mmc), "%s(0x%p + 0x%x) = 0x%x\n", __func__,
+		host->base, reg, data);
+}
+
+static void usdhi6_write16(struct usdhi6_host *host, u32 reg, u16 data)
+{
+	iowrite16(data, host->base + reg);
+	dev_vdbg(mmc_dev(host->mmc), "%s(0x%p + 0x%x) = 0x%x\n", __func__,
+		host->base, reg, data);
+}
+
+static u32 usdhi6_read(struct usdhi6_host *host, u32 reg)
+{
+	u32 data = ioread32(host->base + reg);
+	dev_vdbg(mmc_dev(host->mmc), "%s(0x%p + 0x%x) = 0x%x\n", __func__,
+		host->base, reg, data);
+	return data;
+}
+
+static u16 usdhi6_read16(struct usdhi6_host *host, u32 reg)
+{
+	u16 data = ioread16(host->base + reg);
+	dev_vdbg(mmc_dev(host->mmc), "%s(0x%p + 0x%x) = 0x%x\n", __func__,
+		host->base, reg, data);
+	return data;
+}
+
+static void usdhi6_irq_enable(struct usdhi6_host *host, u32 info1, u32 info2)
+{
+	host->status_mask = USDHI6_SD_INFO1_IRQ & ~info1;
+	host->status2_mask = USDHI6_SD_INFO2_IRQ & ~info2;
+	usdhi6_write(host, USDHI6_SD_INFO1_MASK, host->status_mask);
+	usdhi6_write(host, USDHI6_SD_INFO2_MASK, host->status2_mask);
+}
+
+static void usdhi6_wait_for_resp(struct usdhi6_host *host)
+{
+	usdhi6_irq_enable(host, USDHI6_SD_INFO1_RSP_END |
+			  USDHI6_SD_INFO1_ACCESS_END | USDHI6_SD_INFO1_CARD_CD,
+			  USDHI6_SD_INFO2_ERR);
+}
+
+static void usdhi6_wait_for_brwe(struct usdhi6_host *host, bool read)
+{
+	usdhi6_irq_enable(host, USDHI6_SD_INFO1_ACCESS_END |
+			  USDHI6_SD_INFO1_CARD_CD, USDHI6_SD_INFO2_ERR |
+			  (read ? USDHI6_SD_INFO2_BRE : USDHI6_SD_INFO2_BWE));
+}
+
+static void usdhi6_only_cd(struct usdhi6_host *host)
+{
+	/* Mask all except card hotplug */
+	usdhi6_irq_enable(host, USDHI6_SD_INFO1_CARD_CD, 0);
+}
+
+static void usdhi6_mask_all(struct usdhi6_host *host)
+{
+	usdhi6_irq_enable(host, 0, 0);
+}
+
+static int usdhi6_error_code(struct usdhi6_host *host)
+{
+	u32 err;
+
+	usdhi6_write(host, USDHI6_SD_STOP, USDHI6_SD_STOP_STP);
+
+	if (host->io_error &
+	    (USDHI6_SD_INFO2_RSP_TOUT | USDHI6_SD_INFO2_TOUT)) {
+		u32 rsp54 = usdhi6_read(host, USDHI6_SD_RSP54);
+		int opc = host->mrq ? host->mrq->cmd->opcode : -1;
+
+		err = usdhi6_read(host, USDHI6_SD_ERR_STS2);
+		/* Response timeout is often normal, don't spam the log */
+		if (host->wait == USDHI6_WAIT_FOR_CMD)
+			dev_dbg(mmc_dev(host->mmc),
+				"T-out sts 0x%x, resp 0x%x, state %u, CMD%d\n",
+				err, rsp54, host->wait, opc);
+		else
+			dev_warn(mmc_dev(host->mmc),
+				 "T-out sts 0x%x, resp 0x%x, state %u, CMD%d\n",
+				 err, rsp54, host->wait, opc);
+		return -ETIMEDOUT;
+	}
+
+	err = usdhi6_read(host, USDHI6_SD_ERR_STS1);
+	if (err != USDHI6_SD_ERR_STS1_CRC_NO_ERROR)
+		dev_warn(mmc_dev(host->mmc), "Err sts 0x%x, state %u, CMD%d\n",
+			 err, host->wait, host->mrq ? host->mrq->cmd->opcode : -1);
+	if (host->io_error & USDHI6_SD_INFO2_ILA)
+		return -EILSEQ;
+
+	return -EIO;
+}
+
+/*			Scatter-Gather management			*/
+
+/*
+ * In PIO mode we have to map each page separately, using kmap(). That way
+ * adjacent pages are mapped to non-adjacent virtual addresses. That's why we
+ * have to use a bounce buffer for blocks, crossing page boundaries. Such blocks
+ * have been observed with an SDIO WiFi card (b43 driver).
+ */
+static void usdhi6_blk_bounce(struct usdhi6_host *host,
+			      struct scatterlist *sg)
+{
+	struct mmc_data *data = host->mrq->data;
+	size_t blk_head = host->head_len;
+
+	dev_dbg(mmc_dev(host->mmc), "%s(): CMD%u of %u SG: %ux%u @ 0x%x\n",
+		__func__, host->mrq->cmd->opcode, data->sg_len,
+		data->blksz, data->blocks, sg->offset);
+
+	host->head_pg.page	= host->pg.page;
+	host->head_pg.mapped	= host->pg.mapped;
+	host->pg.page		= nth_page(host->pg.page, 1);
+	host->pg.mapped		= kmap(host->pg.page);
+
+	host->blk_page = host->bounce_buf;
+	host->offset = 0;
+
+	if (data->flags & MMC_DATA_READ)
+		return;
+
+	memcpy(host->bounce_buf, host->head_pg.mapped + PAGE_SIZE - blk_head,
+	       blk_head);
+	memcpy(host->bounce_buf + blk_head, host->pg.mapped,
+	       data->blksz - blk_head);
+}
+
+/* Only called for multiple block IO */
+static void usdhi6_sg_prep(struct usdhi6_host *host)
+{
+	struct mmc_request *mrq = host->mrq;
+	struct mmc_data *data = mrq->data;
+
+	usdhi6_write(host, USDHI6_SD_SECCNT, data->blocks);
+
+	host->sg = data->sg;
+	/* TODO: if we always map, this is redundant */
+	host->offset = host->sg->offset;
+}
+
+/* Map the first page in an SG segment: common for multiple and single block IO */
+static void *usdhi6_sg_map(struct usdhi6_host *host)
+{
+	struct mmc_data *data = host->mrq->data;
+	struct scatterlist *sg = data->sg_len > 1 ? host->sg : data->sg;
+	size_t head = PAGE_SIZE - sg->offset;
+	size_t blk_head = head % data->blksz;
+
+	WARN(host->pg.page, "%p not properly unmapped!\n", host->pg.page);
+	if (WARN(sg_dma_len(sg) % data->blksz,
+		 "SG size %zd isn't a multiple of block size %zd\n",
+		 sg_dma_len(sg), data->blksz))
+		return NULL;
+
+	host->pg.page = sg_page(sg);
+	host->pg.mapped = kmap(host->pg.page);
+	host->offset = sg->offset;
+
+	/*
+	 * Block size must be a power of 2 for multi-block transfers,
+	 * therefore blk_head is equal for all pages in this SG
+	 */
+	host->head_len = blk_head;
+
+	if (head < data->blksz)
+		/*
+		 * The first block in the SG crosses a page boundary.
+		 * Max blksz = 512, so blocks can only span 2 pages
+		 */
+		usdhi6_blk_bounce(host, sg);
+	else
+		host->blk_page = host->pg.mapped;
+
+	dev_dbg(mmc_dev(host->mmc), "Mapped %p (%lx) at %p + %u for CMD%u @ 0x%p\n",
+		host->pg.page, page_to_pfn(host->pg.page), host->pg.mapped,
+		sg->offset, host->mrq->cmd->opcode, host->mrq);
+
+	return host->blk_page + host->offset;
+}
+
+/* Unmap the current page: common for multiple and single block IO */
+static void usdhi6_sg_unmap(struct usdhi6_host *host, bool force)
+{
+	struct mmc_data *data = host->mrq->data;
+	struct page *page = host->head_pg.page;
+
+	if (page) {
+		/* Previous block was cross-page boundary */
+		struct scatterlist *sg = data->sg_len > 1 ?
+			host->sg : data->sg;
+		size_t blk_head = host->head_len;
+
+		if (!data->error && data->flags & MMC_DATA_READ) {
+			memcpy(host->head_pg.mapped + PAGE_SIZE - blk_head,
+			       host->bounce_buf, blk_head);
+			memcpy(host->pg.mapped, host->bounce_buf + blk_head,
+			       data->blksz - blk_head);
+		}
+
+		flush_dcache_page(page);
+		kunmap(page);
+
+		host->head_pg.page = NULL;
+
+		if (!force && sg_dma_len(sg) + sg->offset >
+		    (host->page_idx << PAGE_SHIFT) + data->blksz - blk_head)
+			/* More blocks in this SG, don't unmap the next page */
+			return;
+	}
+
+	page = host->pg.page;
+	if (!page)
+		return;
+
+	flush_dcache_page(page);
+	kunmap(page);
+
+	host->pg.page = NULL;
+}
+
+/* Called from MMC_WRITE_MULTIPLE_BLOCK or MMC_READ_MULTIPLE_BLOCK */
+static void usdhi6_sg_advance(struct usdhi6_host *host)
+{
+	struct mmc_data *data = host->mrq->data;
+	size_t done, total;
+
+	/* New offset: set at the end of the previous block */
+	if (host->head_pg.page) {
+		/* Finished a cross-page block, jump to the new page */
+		host->page_idx++;
+		host->offset = data->blksz - host->head_len;
+		host->blk_page = host->pg.mapped;
+		usdhi6_sg_unmap(host, false);
+	} else {
+		host->offset += data->blksz;
+		/* The completed block didn't cross a page boundary */
+		if (host->offset == PAGE_SIZE) {
+			/* If required, we'll map the page below */
+			host->offset = 0;
+			host->page_idx++;
+		}
+	}
+
+	/*
+	 * Now host->blk_page + host->offset point at the end of our last block
+	 * and host->page_idx is the index of the page, in which our new block
+	 * is located, if any
+	 */
+
+	done = (host->page_idx << PAGE_SHIFT) + host->offset;
+	total = host->sg->offset + sg_dma_len(host->sg);
+
+	dev_dbg(mmc_dev(host->mmc), "%s(): %zu of %zu @ %u\n", __func__,
+		done, total, host->offset);
+
+	if (done < total && host->offset) {
+		/* More blocks in this page */
+		if (host->offset + data->blksz > PAGE_SIZE)
+			/* We approached at a block, that spans 2 pages */
+			usdhi6_blk_bounce(host, host->sg);
+
+		return;
+	}
+
+	/* Finished current page or an SG segment */
+	usdhi6_sg_unmap(host, false);
+
+	if (done == total) {
+		/*
+		 * End of an SG segment or the complete SG: jump to the next
+		 * segment, we'll map it later in usdhi6_blk_read() or
+		 * usdhi6_blk_write()
+		 */
+		struct scatterlist *next = sg_next(host->sg);
+
+		host->page_idx = 0;
+
+		if (!next)
+			host->wait = USDHI6_WAIT_FOR_DATA_END;
+		host->sg = next;
+
+		if (WARN(next && sg_dma_len(next) % data->blksz,
+			 "SG size %zd isn't a multiple of block size %zd\n",
+			 sg_dma_len(next), data->blksz))
+			data->error = -EINVAL;
+
+		return;
+	}
+
+	/* We cannot get here after crossing a page border */
+
+	/* Next page in the same SG */
+	host->pg.page = nth_page(sg_page(host->sg), host->page_idx);
+	host->pg.mapped = kmap(host->pg.page);
+	host->blk_page = host->pg.mapped;
+
+	dev_dbg(mmc_dev(host->mmc), "Mapped %p (%lx) at %p for CMD%u @ 0x%p\n",
+		host->pg.page, page_to_pfn(host->pg.page), host->pg.mapped,
+		host->mrq->cmd->opcode, host->mrq);
+}
+
+/*			DMA handling					*/
+
+static void usdhi6_dma_release(struct usdhi6_host *host)
+{
+	host->dma_active = false;
+	if (host->chan_tx) {
+		struct dma_chan *chan = host->chan_tx;
+		host->chan_tx = NULL;
+		dma_release_channel(chan);
+	}
+	if (host->chan_rx) {
+		struct dma_chan *chan = host->chan_rx;
+		host->chan_rx = NULL;
+		dma_release_channel(chan);
+	}
+}
+
+static void usdhi6_dma_stop_unmap(struct usdhi6_host *host)
+{
+	struct mmc_data *data = host->mrq->data;
+
+	if (!host->dma_active)
+		return;
+
+	usdhi6_write(host, USDHI6_CC_EXT_MODE, 0);
+	host->dma_active = false;
+
+	if (data->flags & MMC_DATA_READ)
+		dma_unmap_sg(host->chan_rx->device->dev, data->sg,
+			     data->sg_len, DMA_FROM_DEVICE);
+	else
+		dma_unmap_sg(host->chan_tx->device->dev, data->sg,
+			     data->sg_len, DMA_TO_DEVICE);
+}
+
+static void usdhi6_dma_complete(void *arg)
+{
+	struct usdhi6_host *host = arg;
+	struct mmc_request *mrq = host->mrq;
+
+	if (WARN(!mrq || !mrq->data, "%s: NULL data in DMA completion for %p!\n",
+		 dev_name(mmc_dev(host->mmc)), mrq))
+		return;
+
+	dev_dbg(mmc_dev(host->mmc), "%s(): CMD%u DMA completed\n", __func__,
+		mrq->cmd->opcode);
+
+	usdhi6_dma_stop_unmap(host);
+	usdhi6_wait_for_brwe(host, mrq->data->flags & MMC_DATA_READ);
+}
+
+static int usdhi6_dma_setup(struct usdhi6_host *host, struct dma_chan *chan,
+			    enum dma_transfer_direction dir)
+{
+	struct mmc_data *data = host->mrq->data;
+	struct scatterlist *sg = data->sg;
+	struct dma_async_tx_descriptor *desc = NULL;
+	dma_cookie_t cookie = -EINVAL;
+	enum dma_data_direction data_dir;
+	int ret;
+
+	switch (dir) {
+	case DMA_MEM_TO_DEV:
+		data_dir = DMA_TO_DEVICE;
+		break;
+	case DMA_DEV_TO_MEM:
+		data_dir = DMA_FROM_DEVICE;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = dma_map_sg(chan->device->dev, sg, data->sg_len, data_dir);
+	if (ret > 0) {
+		host->dma_active = true;
+		desc = dmaengine_prep_slave_sg(chan, sg, ret, dir,
+					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	}
+
+	if (desc) {
+		desc->callback = usdhi6_dma_complete;
+		desc->callback_param = host;
+		cookie = dmaengine_submit(desc);
+	}
+
+	dev_dbg(mmc_dev(host->mmc), "%s(): mapped %d -> %d, cookie %d @ %p\n",
+		__func__, data->sg_len, ret, cookie, desc);
+
+	if (cookie < 0) {
+		/* DMA failed, fall back to PIO */
+		if (ret >= 0)
+			ret = cookie;
+		usdhi6_dma_release(host);
+		dev_warn(mmc_dev(host->mmc),
+			 "DMA failed: %d, falling back to PIO\n", ret);
+	}
+
+	return cookie;
+}
+
+static int usdhi6_dma_start(struct usdhi6_host *host)
+{
+	if (!host->chan_rx || !host->chan_tx)
+		return -ENODEV;
+
+	if (host->mrq->data->flags & MMC_DATA_READ)
+		return usdhi6_dma_setup(host, host->chan_rx, DMA_DEV_TO_MEM);
+
+	return usdhi6_dma_setup(host, host->chan_tx, DMA_MEM_TO_DEV);
+}
+
+static void usdhi6_dma_kill(struct usdhi6_host *host)
+{
+	struct mmc_data *data = host->mrq->data;
+
+	dev_dbg(mmc_dev(host->mmc), "%s(): SG of %u: %ux%u\n",
+		__func__, data->sg_len, data->blocks, data->blksz);
+	/* Abort DMA */
+	if (data->flags & MMC_DATA_READ)
+		dmaengine_terminate_all(host->chan_rx);
+	else
+		dmaengine_terminate_all(host->chan_tx);
+}
+
+static void usdhi6_dma_check_error(struct usdhi6_host *host)
+{
+	struct mmc_data *data = host->mrq->data;
+
+	dev_dbg(mmc_dev(host->mmc), "%s(): IO error %d, status 0x%x\n",
+		__func__, host->io_error, usdhi6_read(host, USDHI6_SD_INFO1));
+
+	if (host->io_error) {
+		data->error = usdhi6_error_code(host);
+		data->bytes_xfered = 0;
+		usdhi6_dma_kill(host);
+		usdhi6_dma_release(host);
+		dev_warn(mmc_dev(host->mmc),
+			 "DMA failed: %d, falling back to PIO\n", data->error);
+		return;
+	}
+
+	/*
+	 * The datasheet tells us to check a response from the card, whereas
+	 * responses only come after the command phase, not after the data
+	 * phase. Let's check anyway.
+	 */
+	if (host->irq_status & USDHI6_SD_INFO1_RSP_END)
+		dev_warn(mmc_dev(host->mmc), "Unexpected response received!\n");
+}
+
+static void usdhi6_dma_kick(struct usdhi6_host *host)
+{
+	if (host->mrq->data->flags & MMC_DATA_READ)
+		dma_async_issue_pending(host->chan_rx);
+	else
+		dma_async_issue_pending(host->chan_tx);
+}
+
+static void usdhi6_dma_request(struct usdhi6_host *host, phys_addr_t start)
+{
+	struct dma_slave_config cfg = {
+		.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES,
+		.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES,
+	};
+	int ret;
+
+	host->chan_tx = dma_request_slave_channel(mmc_dev(host->mmc), "tx");
+	dev_dbg(mmc_dev(host->mmc), "%s: TX: got channel %p\n", __func__,
+		host->chan_tx);
+
+	if (!host->chan_tx)
+		return;
+
+	cfg.direction = DMA_MEM_TO_DEV;
+	cfg.dst_addr = start + USDHI6_SD_BUF0;
+	cfg.dst_maxburst = 128;	/* 128 words * 4 bytes = 512 bytes */
+	cfg.src_addr = 0;
+	ret = dmaengine_slave_config(host->chan_tx, &cfg);
+	if (ret < 0)
+		goto e_release_tx;
+
+	host->chan_rx = dma_request_slave_channel(mmc_dev(host->mmc), "rx");
+	dev_dbg(mmc_dev(host->mmc), "%s: RX: got channel %p\n", __func__,
+		host->chan_rx);
+
+	if (!host->chan_rx)
+		goto e_release_tx;
+
+	cfg.direction = DMA_DEV_TO_MEM;
+	cfg.src_addr = cfg.dst_addr;
+	cfg.src_maxburst = 128;	/* 128 words * 4 bytes = 512 bytes */
+	cfg.dst_addr = 0;
+	ret = dmaengine_slave_config(host->chan_rx, &cfg);
+	if (ret < 0)
+		goto e_release_rx;
+
+	return;
+
+e_release_rx:
+	dma_release_channel(host->chan_rx);
+	host->chan_rx = NULL;
+e_release_tx:
+	dma_release_channel(host->chan_tx);
+	host->chan_tx = NULL;
+}
+
+/*			API helpers					*/
+
+static void usdhi6_clk_set(struct usdhi6_host *host, struct mmc_ios *ios)
+{
+	unsigned long rate = ios->clock;
+	u32 val;
+	unsigned int i;
+
+	for (i = 1000; i; i--) {
+		if (usdhi6_read(host, USDHI6_SD_INFO2) & USDHI6_SD_INFO2_SCLKDIVEN)
+			break;
+		usleep_range(10, 100);
+	}
+
+	if (!i) {
+		dev_err(mmc_dev(host->mmc), "SD bus busy, clock set aborted\n");
+		return;
+	}
+
+	val = usdhi6_read(host, USDHI6_SD_CLK_CTRL) & ~USDHI6_SD_CLK_CTRL_DIV_MASK;
+
+	if (rate) {
+		unsigned long new_rate;
+
+		if (host->imclk <= rate) {
+			if (ios->timing != MMC_TIMING_UHS_DDR50) {
+				/* Cannot have 1-to-1 clock in DDR mode */
+				new_rate = host->imclk;
+				val |= 0xff;
+			} else {
+				new_rate = host->imclk / 2;
+			}
+		} else {
+			unsigned long div =
+				roundup_pow_of_two(DIV_ROUND_UP(host->imclk, rate));
+			val |= div >> 2;
+			new_rate = host->imclk / div;
+		}
+
+		if (host->rate == new_rate)
+			return;
+
+		host->rate = new_rate;
+
+		dev_dbg(mmc_dev(host->mmc), "target %lu, div %u, set %lu\n",
+			rate, (val & 0xff) << 2, new_rate);
+	}
+
+	/*
+	 * if old or new rate is equal to input rate, have to switch the clock
+	 * off before changing and on after
+	 */
+	if (host->imclk == rate || host->imclk == host->rate || !rate)
+		usdhi6_write(host, USDHI6_SD_CLK_CTRL,
+			     val & ~USDHI6_SD_CLK_CTRL_SCLKEN);
+
+	if (!rate) {
+		host->rate = 0;
+		return;
+	}
+
+	usdhi6_write(host, USDHI6_SD_CLK_CTRL, val);
+
+	if (host->imclk == rate || host->imclk == host->rate ||
+	    !(val & USDHI6_SD_CLK_CTRL_SCLKEN))
+		usdhi6_write(host, USDHI6_SD_CLK_CTRL,
+			     val | USDHI6_SD_CLK_CTRL_SCLKEN);
+}
+
+static void usdhi6_set_power(struct usdhi6_host *host, struct mmc_ios *ios)
+{
+	struct mmc_host *mmc = host->mmc;
+
+	if (!IS_ERR(mmc->supply.vmmc))
+		/* Errors ignored... */
+		mmc_regulator_set_ocr(mmc, mmc->supply.vmmc,
+				      ios->power_mode ? ios->vdd : 0);
+}
+
+static int usdhi6_reset(struct usdhi6_host *host)
+{
+	int i;
+
+	usdhi6_write(host, USDHI6_SOFT_RST, USDHI6_SOFT_RST_RESERVED);
+	cpu_relax();
+	usdhi6_write(host, USDHI6_SOFT_RST, USDHI6_SOFT_RST_RESERVED | USDHI6_SOFT_RST_RESET);
+	for (i = 1000; i; i--)
+		if (usdhi6_read(host, USDHI6_SOFT_RST) & USDHI6_SOFT_RST_RESET)
+			break;
+
+	return i ? 0 : -ETIMEDOUT;
+}
+
+static void usdhi6_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
+{
+	struct usdhi6_host *host = mmc_priv(mmc);
+	u32 option, mode;
+	int ret;
+
+	dev_dbg(mmc_dev(mmc), "%uHz, OCR: %u, power %u, bus-width %u, timing %u\n",
+		ios->clock, ios->vdd, ios->power_mode, ios->bus_width, ios->timing);
+
+	switch (ios->power_mode) {
+	case MMC_POWER_OFF:
+		usdhi6_set_power(host, ios);
+		usdhi6_only_cd(host);
+		break;
+	case MMC_POWER_UP:
+		/*
+		 * We only also touch USDHI6_SD_OPTION from .request(), which
+		 * cannot race with MMC_POWER_UP
+		 */
+		ret = usdhi6_reset(host);
+		if (ret < 0) {
+			dev_err(mmc_dev(mmc), "Cannot reset the interface!\n");
+		} else {
+			usdhi6_set_power(host, ios);
+			usdhi6_only_cd(host);
+		}
+		break;
+	case MMC_POWER_ON:
+		option = usdhi6_read(host, USDHI6_SD_OPTION);
+		/*
+		 * The eMMC standard only allows 4 or 8 bits in the DDR mode,
+		 * the same probably holds for SD cards. We check here anyway,
+		 * since the datasheet explicitly requires 4 bits for DDR.
+		 */
+		if (ios->bus_width == MMC_BUS_WIDTH_1) {
+			if (ios->timing == MMC_TIMING_UHS_DDR50)
+				dev_err(mmc_dev(mmc),
+					"4 bits are required for DDR\n");
+			option |= USDHI6_SD_OPTION_WIDTH_1;
+			mode = 0;
+		} else {
+			option &= ~USDHI6_SD_OPTION_WIDTH_1;
+			mode = ios->timing == MMC_TIMING_UHS_DDR50;
+		}
+		usdhi6_write(host, USDHI6_SD_OPTION, option);
+		usdhi6_write(host, USDHI6_SDIF_MODE, mode);
+		break;
+	}
+
+	if (host->rate != ios->clock)
+		usdhi6_clk_set(host, ios);
+}
+
+/* This is data timeout. Response timeout is fixed to 640 clock cycles */
+static void usdhi6_timeout_set(struct usdhi6_host *host)
+{
+	struct mmc_request *mrq = host->mrq;
+	u32 val;
+	unsigned long ticks;
+
+	if (!mrq->data)
+		ticks = host->rate / 1000 * mrq->cmd->busy_timeout;
+	else
+		ticks = host->rate / 1000000 * (mrq->data->timeout_ns / 1000) +
+			mrq->data->timeout_clks;
+
+	if (!ticks || ticks > 1 << 27)
+		/* Max timeout */
+		val = 14;
+	else if (ticks < 1 << 13)
+		/* Min timeout */
+		val = 0;
+	else
+		val = order_base_2(ticks) - 13;
+
+	dev_dbg(mmc_dev(host->mmc), "Set %s timeout %lu ticks @ %lu Hz\n",
+		mrq->data ? "data" : "cmd", ticks, host->rate);
+
+	/* Timeout Counter mask: 0xf0 */
+	usdhi6_write(host, USDHI6_SD_OPTION, (val << USDHI6_SD_OPTION_TIMEOUT_SHIFT) |
+		     (usdhi6_read(host, USDHI6_SD_OPTION) & ~USDHI6_SD_OPTION_TIMEOUT_MASK));
+}
+
+static void usdhi6_request_done(struct usdhi6_host *host)
+{
+	struct mmc_request *mrq = host->mrq;
+	struct mmc_data *data = mrq->data;
+
+	if (WARN(host->pg.page || host->head_pg.page,
+		 "Page %p or %p not unmapped: wait %u, CMD%d(%c) @ +0x%x %ux%u in SG%u!\n",
+		 host->pg.page, host->head_pg.page, host->wait, mrq->cmd->opcode,
+		 data ? (data->flags & MMC_DATA_READ ? 'R' : 'W') : '-',
+		 data ? host->offset : 0, data ? data->blocks : 0,
+		 data ? data->blksz : 0, data ? data->sg_len : 0))
+		usdhi6_sg_unmap(host, true);
+
+	if (mrq->cmd->error ||
+	    (data && data->error) ||
+	    (mrq->stop && mrq->stop->error))
+		dev_dbg(mmc_dev(host->mmc), "%s(CMD%d: %ux%u): err %d %d %d\n",
+			__func__, mrq->cmd->opcode, data ? data->blocks : 0,
+			data ? data->blksz : 0,
+			mrq->cmd->error,
+			data ? data->error : 1,
+			mrq->stop ? mrq->stop->error : 1);
+
+	/* Disable DMA */
+	usdhi6_write(host, USDHI6_CC_EXT_MODE, 0);
+	host->wait = USDHI6_WAIT_FOR_REQUEST;
+	host->mrq = NULL;
+
+	mmc_request_done(host->mmc, mrq);
+}
+
+static int usdhi6_cmd_flags(struct usdhi6_host *host)
+{
+	struct mmc_request *mrq = host->mrq;
+	struct mmc_command *cmd = mrq->cmd;
+	u16 opc = cmd->opcode;
+
+	if (host->app_cmd) {
+		host->app_cmd = false;
+		opc |= USDHI6_SD_CMD_APP;
+	}
+
+	if (mrq->data) {
+		opc |= USDHI6_SD_CMD_DATA;
+
+		if (mrq->data->flags & MMC_DATA_READ)
+			opc |= USDHI6_SD_CMD_READ;
+
+		if (cmd->opcode == MMC_READ_MULTIPLE_BLOCK ||
+		    cmd->opcode == MMC_WRITE_MULTIPLE_BLOCK ||
+		    (cmd->opcode == SD_IO_RW_EXTENDED &&
+		     mrq->data->blocks > 1)) {
+			opc |= USDHI6_SD_CMD_MULTI;
+			if (!mrq->stop)
+				opc |= USDHI6_SD_CMD_CMD12_AUTO_OFF;
+		}
+
+		switch (mmc_resp_type(cmd)) {
+		case MMC_RSP_NONE:
+			opc |= USDHI6_SD_CMD_MODE_RSP_NONE;
+			break;
+		case MMC_RSP_R1:
+			opc |= USDHI6_SD_CMD_MODE_RSP_R1;
+			break;
+		case MMC_RSP_R1B:
+			opc |= USDHI6_SD_CMD_MODE_RSP_R1B;
+			break;
+		case MMC_RSP_R2:
+			opc |= USDHI6_SD_CMD_MODE_RSP_R2;
+			break;
+		case MMC_RSP_R3:
+			opc |= USDHI6_SD_CMD_MODE_RSP_R3;
+			break;
+		default:
+			dev_warn(mmc_dev(host->mmc),
+				 "Unknown response type %d\n",
+				 mmc_resp_type(cmd));
+			return -EINVAL;
+		}
+	}
+
+	return opc;
+}
+
+static int usdhi6_rq_start(struct usdhi6_host *host)
+{
+	struct mmc_request *mrq = host->mrq;
+	struct mmc_command *cmd = mrq->cmd;
+	struct mmc_data *data = mrq->data;
+	int opc = usdhi6_cmd_flags(host);
+	int i;
+
+	if (opc < 0)
+		return opc;
+
+	for (i = 1000; i; i--) {
+		if (!(usdhi6_read(host, USDHI6_SD_INFO2) & USDHI6_SD_INFO2_CBSY))
+			break;
+		usleep_range(10, 100);
+	}
+
+	if (!i) {
+		dev_dbg(mmc_dev(host->mmc), "Command active, request aborted\n");
+		return -EAGAIN;
+	}
+
+	if (data) {
+		bool use_dma;
+		int ret = 0;
+
+		host->page_idx = 0;
+
+		if (cmd->opcode == SD_IO_RW_EXTENDED && data->blocks > 1) {
+			switch (data->blksz) {
+			case 512:
+				break;
+			case 32:
+			case 64:
+			case 128:
+			case 256:
+				if (mrq->stop)
+					ret = -EINVAL;
+				break;
+			default:
+				ret = -EINVAL;
+			}
+		} else if ((cmd->opcode == MMC_READ_MULTIPLE_BLOCK ||
+			    cmd->opcode == MMC_WRITE_MULTIPLE_BLOCK) &&
+			   data->blksz != 512) {
+			ret = -EINVAL;
+		}
+
+		if (ret < 0) {
+			dev_warn(mmc_dev(host->mmc), "%s(): %u blocks of %u bytes\n",
+				 __func__, data->blocks, data->blksz);
+			return -EINVAL;
+		}
+
+		if (cmd->opcode == MMC_READ_MULTIPLE_BLOCK ||
+		    cmd->opcode == MMC_WRITE_MULTIPLE_BLOCK ||
+		    (cmd->opcode == SD_IO_RW_EXTENDED &&
+		     data->blocks > 1))
+			usdhi6_sg_prep(host);
+
+		usdhi6_write(host, USDHI6_SD_SIZE, data->blksz);
+
+		if ((data->blksz >= USDHI6_MIN_DMA ||
+		     data->blocks > 1) &&
+		    (data->blksz % 4 ||
+		     data->sg->offset % 4))
+			dev_dbg(mmc_dev(host->mmc),
+				"Bad SG of %u: %ux%u @ %u\n", data->sg_len,
+				data->blksz, data->blocks, data->sg->offset);
+
+		/* Enable DMA for USDHI6_MIN_DMA bytes or more */
+		use_dma = data->blksz >= USDHI6_MIN_DMA &&
+			!(data->blksz % 4) &&
+			usdhi6_dma_start(host) >= DMA_MIN_COOKIE;
+
+		if (use_dma)
+			usdhi6_write(host, USDHI6_CC_EXT_MODE, USDHI6_CC_EXT_MODE_SDRW);
+
+		dev_dbg(mmc_dev(host->mmc),
+			"%s(): request opcode %u, %u blocks of %u bytes in %u segments, %s %s @+0x%x%s\n",
+			__func__, cmd->opcode, data->blocks, data->blksz,
+			data->sg_len, use_dma ? "DMA" : "PIO",
+			data->flags & MMC_DATA_READ ? "read" : "write",
+			data->sg->offset, mrq->stop ? " + stop" : "");
+	} else {
+		dev_dbg(mmc_dev(host->mmc), "%s(): request opcode %u\n",
+			__func__, cmd->opcode);
+	}
+
+	/* We have to get a command completion interrupt with DMA too */
+	usdhi6_wait_for_resp(host);
+
+	host->wait = USDHI6_WAIT_FOR_CMD;
+	schedule_delayed_work(&host->timeout_work, host->timeout);
+
+	/* SEC bit is required to enable block counting by the core */
+	usdhi6_write(host, USDHI6_SD_STOP,
+		     data && data->blocks > 1 ? USDHI6_SD_STOP_SEC : 0);
+	usdhi6_write(host, USDHI6_SD_ARG, cmd->arg);
+
+	/* Kick command execution */
+	usdhi6_write(host, USDHI6_SD_CMD, opc);
+
+	return 0;
+}
+
+static void usdhi6_request(struct mmc_host *mmc, struct mmc_request *mrq)
+{
+	struct usdhi6_host *host = mmc_priv(mmc);
+	int ret;
+
+	cancel_delayed_work_sync(&host->timeout_work);
+
+	host->mrq = mrq;
+	host->sg = NULL;
+
+	usdhi6_timeout_set(host);
+	ret = usdhi6_rq_start(host);
+	if (ret < 0) {
+		mrq->cmd->error = ret;
+		usdhi6_request_done(host);
+	}
+}
+
+static int usdhi6_get_cd(struct mmc_host *mmc)
+{
+	struct usdhi6_host *host = mmc_priv(mmc);
+	/* Read is atomic, no need to lock */
+	u32 status = usdhi6_read(host, USDHI6_SD_INFO1) & USDHI6_SD_INFO1_CD;
+
+/*
+ *	level	status.CD	CD_ACTIVE_HIGH	card present
+ *	1	0		0		0
+ *	1	0		1		1
+ *	0	1		0		1
+ *	0	1		1		0
+ */
+	return !status ^ !(mmc->caps2 & MMC_CAP2_CD_ACTIVE_HIGH);
+}
+
+static int usdhi6_get_ro(struct mmc_host *mmc)
+{
+	struct usdhi6_host *host = mmc_priv(mmc);
+	/* No locking as above */
+	u32 status = usdhi6_read(host, USDHI6_SD_INFO1) & USDHI6_SD_INFO1_WP;
+
+/*
+ *	level	status.WP	RO_ACTIVE_HIGH	card read-only
+ *	1	0		0		0
+ *	1	0		1		1
+ *	0	1		0		1
+ *	0	1		1		0
+ */
+	return !status ^ !(mmc->caps2 & MMC_CAP2_RO_ACTIVE_HIGH);
+}
+
+static void usdhi6_enable_sdio_irq(struct mmc_host *mmc, int enable)
+{
+	struct usdhi6_host *host = mmc_priv(mmc);
+
+	dev_dbg(mmc_dev(mmc), "%s(): %sable\n", __func__, enable ? "en" : "dis");
+
+	if (enable) {
+		host->sdio_mask = USDHI6_SDIO_INFO1_IRQ & ~USDHI6_SDIO_INFO1_IOIRQ;
+		usdhi6_write(host, USDHI6_SDIO_INFO1_MASK, host->sdio_mask);
+		usdhi6_write(host, USDHI6_SDIO_MODE, 1);
+	} else {
+		usdhi6_write(host, USDHI6_SDIO_MODE, 0);
+		usdhi6_write(host, USDHI6_SDIO_INFO1_MASK, USDHI6_SDIO_INFO1_IRQ);
+		host->sdio_mask = USDHI6_SDIO_INFO1_IRQ;
+	}
+}
+
+static struct mmc_host_ops usdhi6_ops = {
+	.request	= usdhi6_request,
+	.set_ios	= usdhi6_set_ios,
+	.get_cd		= usdhi6_get_cd,
+	.get_ro		= usdhi6_get_ro,
+	.enable_sdio_irq = usdhi6_enable_sdio_irq,
+};
+
+/*			State machine handlers				*/
+
+static void usdhi6_resp_cmd12(struct usdhi6_host *host)
+{
+	struct mmc_command *cmd = host->mrq->stop;
+	cmd->resp[0] = usdhi6_read(host, USDHI6_SD_RSP10);
+}
+
+static void usdhi6_resp_read(struct usdhi6_host *host)
+{
+	struct mmc_command *cmd = host->mrq->cmd;
+	u32 *rsp = cmd->resp, tmp = 0;
+	int i;
+
+/*
+ * RSP10	39-8
+ * RSP32	71-40
+ * RSP54	103-72
+ * RSP76	127-104
+ * R2-type response:
+ * resp[0]	= r[127..96]
+ * resp[1]	= r[95..64]
+ * resp[2]	= r[63..32]
+ * resp[3]	= r[31..0]
+ * Other responses:
+ * resp[0]	= r[39..8]
+ */
+
+	if (mmc_resp_type(cmd) == MMC_RSP_NONE)
+		return;
+
+	if (!(host->irq_status & USDHI6_SD_INFO1_RSP_END)) {
+		dev_err(mmc_dev(host->mmc),
+			"CMD%d: response expected but is missing!\n", cmd->opcode);
+		return;
+	}
+
+	if (mmc_resp_type(cmd) & MMC_RSP_136)
+		for (i = 0; i < 4; i++) {
+			if (i)
+				rsp[3 - i] = tmp >> 24;
+			tmp = usdhi6_read(host, USDHI6_SD_RSP10 + i * 8);
+			rsp[3 - i] |= tmp << 8;
+		}
+	else if (cmd->opcode == MMC_READ_MULTIPLE_BLOCK ||
+		 cmd->opcode == MMC_WRITE_MULTIPLE_BLOCK)
+		/* Read RSP54 to avoid conflict with auto CMD12 */
+		rsp[0] = usdhi6_read(host, USDHI6_SD_RSP54);
+	else
+		rsp[0] = usdhi6_read(host, USDHI6_SD_RSP10);
+
+	dev_dbg(mmc_dev(host->mmc), "Response 0x%x\n", rsp[0]);
+}
+
+static int usdhi6_blk_read(struct usdhi6_host *host)
+{
+	struct mmc_data *data = host->mrq->data;
+	u32 *p;
+	int i, rest;
+
+	if (host->io_error) {
+		data->error = usdhi6_error_code(host);
+		goto error;
+	}
+
+	if (host->pg.page) {
+		p = host->blk_page + host->offset;
+	} else {
+		p = usdhi6_sg_map(host);
+		if (!p) {
+			data->error = -ENOMEM;
+			goto error;
+		}
+	}
+
+	for (i = 0; i < data->blksz / 4; i++, p++)
+		*p = usdhi6_read(host, USDHI6_SD_BUF0);
+
+	rest = data->blksz % 4;
+	for (i = 0; i < (rest + 1) / 2; i++) {
+		u16 d = usdhi6_read16(host, USDHI6_SD_BUF0);
+		((u8 *)p)[2 * i] = ((u8 *)&d)[0];
+		if (rest > 1 && !i)
+			((u8 *)p)[2 * i + 1] = ((u8 *)&d)[1];
+	}
+
+	return 0;
+
+error:
+	dev_dbg(mmc_dev(host->mmc), "%s(): %d\n", __func__, data->error);
+	host->wait = USDHI6_WAIT_FOR_REQUEST;
+	return data->error;
+}
+
+static int usdhi6_blk_write(struct usdhi6_host *host)
+{
+	struct mmc_data *data = host->mrq->data;
+	u32 *p;
+	int i, rest;
+
+	if (host->io_error) {
+		data->error = usdhi6_error_code(host);
+		goto error;
+	}
+
+	if (host->pg.page) {
+		p = host->blk_page + host->offset;
+	} else {
+		p = usdhi6_sg_map(host);
+		if (!p) {
+			data->error = -ENOMEM;
+			goto error;
+		}
+	}
+
+	for (i = 0; i < data->blksz / 4; i++, p++)
+		usdhi6_write(host, USDHI6_SD_BUF0, *p);
+
+	rest = data->blksz % 4;
+	for (i = 0; i < (rest + 1) / 2; i++) {
+		u16 d;
+		((u8 *)&d)[0] = ((u8 *)p)[2 * i];
+		if (rest > 1 && !i)
+			((u8 *)&d)[1] = ((u8 *)p)[2 * i + 1];
+		else
+			((u8 *)&d)[1] = 0;
+		usdhi6_write16(host, USDHI6_SD_BUF0, d);
+	}
+
+	return 0;
+
+error:
+	dev_dbg(mmc_dev(host->mmc), "%s(): %d\n", __func__, data->error);
+	host->wait = USDHI6_WAIT_FOR_REQUEST;
+	return data->error;
+}
+
+static int usdhi6_stop_cmd(struct usdhi6_host *host)
+{
+	struct mmc_request *mrq = host->mrq;
+
+	switch (mrq->cmd->opcode) {
+	case MMC_READ_MULTIPLE_BLOCK:
+	case MMC_WRITE_MULTIPLE_BLOCK:
+		if (mrq->stop->opcode == MMC_STOP_TRANSMISSION) {
+			host->wait = USDHI6_WAIT_FOR_STOP;
+			return 0;
+		}
+		/* Unsupported STOP command */
+	default:
+		dev_err(mmc_dev(host->mmc),
+			"unsupported stop CMD%d for CMD%d\n",
+			mrq->stop->opcode, mrq->cmd->opcode);
+		mrq->stop->error = -EOPNOTSUPP;
+	}
+
+	return -EOPNOTSUPP;
+}
+
+static bool usdhi6_end_cmd(struct usdhi6_host *host)
+{
+	struct mmc_request *mrq = host->mrq;
+	struct mmc_command *cmd = mrq->cmd;
+
+	if (host->io_error) {
+		cmd->error = usdhi6_error_code(host);
+		return false;
+	}
+
+	usdhi6_resp_read(host);
+
+	if (!mrq->data)
+		return false;
+
+	if (host->dma_active) {
+		usdhi6_dma_kick(host);
+		if (!mrq->stop)
+			host->wait = USDHI6_WAIT_FOR_DMA;
+		else if (usdhi6_stop_cmd(host) < 0)
+			return false;
+	} else if (mrq->data->flags & MMC_DATA_READ) {
+		if (cmd->opcode == MMC_READ_MULTIPLE_BLOCK ||
+		    (cmd->opcode == SD_IO_RW_EXTENDED &&
+		     mrq->data->blocks > 1))
+			host->wait = USDHI6_WAIT_FOR_MREAD;
+		else
+			host->wait = USDHI6_WAIT_FOR_READ;
+	} else {
+		if (cmd->opcode == MMC_WRITE_MULTIPLE_BLOCK ||
+		    (cmd->opcode == SD_IO_RW_EXTENDED &&
+		     mrq->data->blocks > 1))
+			host->wait = USDHI6_WAIT_FOR_MWRITE;
+		else
+			host->wait = USDHI6_WAIT_FOR_WRITE;
+	}
+
+	return true;
+}
+
+static bool usdhi6_read_block(struct usdhi6_host *host)
+{
+	/* ACCESS_END IRQ is already unmasked */
+	int ret = usdhi6_blk_read(host);
+
+	/*
+	 * Have to force unmapping both pages: the single block could have been
+	 * cross-page, in which case for single-block IO host->page_idx == 0.
+	 * So, if we don't force, the second page won't be unmapped.
+	 */
+	usdhi6_sg_unmap(host, true);
+
+	if (ret < 0)
+		return false;
+
+	host->wait = USDHI6_WAIT_FOR_DATA_END;
+	return true;
+}
+
+static bool usdhi6_mread_block(struct usdhi6_host *host)
+{
+	int ret = usdhi6_blk_read(host);
+
+	if (ret < 0)
+		return false;
+
+	usdhi6_sg_advance(host);
+
+	return !host->mrq->data->error &&
+		(host->wait != USDHI6_WAIT_FOR_DATA_END || !host->mrq->stop);
+}
+
+static bool usdhi6_write_block(struct usdhi6_host *host)
+{
+	int ret = usdhi6_blk_write(host);
+
+	/* See comment in usdhi6_read_block() */
+	usdhi6_sg_unmap(host, true);
+
+	if (ret < 0)
+		return false;
+
+	host->wait = USDHI6_WAIT_FOR_DATA_END;
+	return true;
+}
+
+static bool usdhi6_mwrite_block(struct usdhi6_host *host)
+{
+	int ret = usdhi6_blk_write(host);
+
+	if (ret < 0)
+		return false;
+
+	usdhi6_sg_advance(host);
+
+	return !host->mrq->data->error &&
+		(host->wait != USDHI6_WAIT_FOR_DATA_END || !host->mrq->stop);
+}
+
+/*			Interrupt & timeout handlers			*/
+
+static irqreturn_t usdhi6_sd_bh(int irq, void *dev_id)
+{
+	struct usdhi6_host *host = dev_id;
+	struct mmc_request *mrq;
+	struct mmc_command *cmd;
+	struct mmc_data *data;
+	bool io_wait = false;
+
+	cancel_delayed_work_sync(&host->timeout_work);
+
+	mrq = host->mrq;
+	if (!mrq)
+		return IRQ_HANDLED;
+
+	cmd = mrq->cmd;
+	data = mrq->data;
+
+	switch (host->wait) {
+	case USDHI6_WAIT_FOR_REQUEST:
+		/* We're too late, the timeout has already kicked in */
+		return IRQ_HANDLED;
+	case USDHI6_WAIT_FOR_CMD:
+		/* Wait for data? */
+		io_wait = usdhi6_end_cmd(host);
+		break;
+	case USDHI6_WAIT_FOR_MREAD:
+		/* Wait for more data? */
+		io_wait = usdhi6_mread_block(host);
+		break;
+	case USDHI6_WAIT_FOR_READ:
+		/* Wait for data end? */
+		io_wait = usdhi6_read_block(host);
+		break;
+	case USDHI6_WAIT_FOR_MWRITE:
+		/* Wait data to write? */
+		io_wait = usdhi6_mwrite_block(host);
+		break;
+	case USDHI6_WAIT_FOR_WRITE:
+		/* Wait for data end? */
+		io_wait = usdhi6_write_block(host);
+		break;
+	case USDHI6_WAIT_FOR_DMA:
+		usdhi6_dma_check_error(host);
+		break;
+	case USDHI6_WAIT_FOR_STOP:
+		usdhi6_write(host, USDHI6_SD_STOP, 0);
+		if (host->io_error) {
+			int ret = usdhi6_error_code(host);
+			if (mrq->stop)
+				mrq->stop->error = ret;
+			else
+				mrq->data->error = ret;
+			dev_warn(mmc_dev(host->mmc), "%s(): %d\n", __func__, ret);
+			break;
+		}
+		usdhi6_resp_cmd12(host);
+		mrq->stop->error = 0;
+		break;
+	case USDHI6_WAIT_FOR_DATA_END:
+		if (host->io_error) {
+			mrq->data->error = usdhi6_error_code(host);
+			dev_warn(mmc_dev(host->mmc), "%s(): %d\n", __func__,
+				 mrq->data->error);
+		}
+		break;
+	default:
+		cmd->error = -EFAULT;
+		dev_err(mmc_dev(host->mmc), "Invalid state %u\n", host->wait);
+		usdhi6_request_done(host);
+		return IRQ_HANDLED;
+	}
+
+	if (io_wait) {
+		schedule_delayed_work(&host->timeout_work, host->timeout);
+		/* Wait for more data or ACCESS_END */
+		if (!host->dma_active)
+			usdhi6_wait_for_brwe(host, mrq->data->flags & MMC_DATA_READ);
+		return IRQ_HANDLED;
+	}
+
+	if (!cmd->error) {
+		if (data) {
+			if (!data->error) {
+				if (host->wait != USDHI6_WAIT_FOR_STOP &&
+				    host->mrq->stop &&
+				    !host->mrq->stop->error &&
+				    !usdhi6_stop_cmd(host)) {
+					/* Sending STOP */
+					usdhi6_wait_for_resp(host);
+
+					schedule_delayed_work(&host->timeout_work,
+							      host->timeout);
+
+					return IRQ_HANDLED;
+				}
+
+				data->bytes_xfered = data->blocks * data->blksz;
+			} else {
+				/* Data error: might need to unmap the last page */
+				dev_warn(mmc_dev(host->mmc), "%s(): data error %d\n",
+					 __func__, data->error);
+				usdhi6_sg_unmap(host, true);
+			}
+		} else if (cmd->opcode == MMC_APP_CMD) {
+			host->app_cmd = true;
+		}
+	}
+
+	usdhi6_request_done(host);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t usdhi6_sd(int irq, void *dev_id)
+{
+	struct usdhi6_host *host = dev_id;
+	u16 status, status2, error;
+
+	status = usdhi6_read(host, USDHI6_SD_INFO1) & ~host->status_mask &
+		~USDHI6_SD_INFO1_CARD;
+	status2 = usdhi6_read(host, USDHI6_SD_INFO2) & ~host->status2_mask;
+
+	usdhi6_only_cd(host);
+
+	dev_dbg(mmc_dev(host->mmc),
+		"IRQ status = 0x%08x, status2 = 0x%08x\n", status, status2);
+
+	if (!status && !status2)
+		return IRQ_NONE;
+
+	error = status2 & USDHI6_SD_INFO2_ERR;
+
+	/* Ack / clear interrupts */
+	if (USDHI6_SD_INFO1_IRQ & status)
+		usdhi6_write(host, USDHI6_SD_INFO1,
+			     0xffff & ~(USDHI6_SD_INFO1_IRQ & status));
+
+	if (USDHI6_SD_INFO2_IRQ & status2) {
+		if (error)
+			/* In error cases BWE and BRE aren't cleared automatically */
+			status2 |= USDHI6_SD_INFO2_BWE | USDHI6_SD_INFO2_BRE;
+
+		usdhi6_write(host, USDHI6_SD_INFO2,
+			     0xffff & ~(USDHI6_SD_INFO2_IRQ & status2));
+	}
+
+	host->io_error = error;
+	host->irq_status = status;
+
+	if (error) {
+		/* Don't pollute the log with unsupported command timeouts */
+		if (host->wait != USDHI6_WAIT_FOR_CMD ||
+		    error != USDHI6_SD_INFO2_RSP_TOUT)
+			dev_warn(mmc_dev(host->mmc),
+				 "%s(): INFO2 error bits 0x%08x\n",
+				 __func__, error);
+		else
+			dev_dbg(mmc_dev(host->mmc),
+				"%s(): INFO2 error bits 0x%08x\n",
+				__func__, error);
+	}
+
+	return IRQ_WAKE_THREAD;
+}
+
+static irqreturn_t usdhi6_sdio(int irq, void *dev_id)
+{
+	struct usdhi6_host *host = dev_id;
+	u32 status = usdhi6_read(host, USDHI6_SDIO_INFO1) & ~host->sdio_mask;
+
+	dev_dbg(mmc_dev(host->mmc), "%s(): status 0x%x\n", __func__, status);
+
+	if (!status)
+		return IRQ_NONE;
+
+	usdhi6_write(host, USDHI6_SDIO_INFO1, ~status);
+
+	mmc_signal_sdio_irq(host->mmc);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t usdhi6_cd(int irq, void *dev_id)
+{
+	struct usdhi6_host *host = dev_id;
+	struct mmc_host *mmc = host->mmc;
+	u16 status;
+
+	/* We're only interested in hotplug events here */
+	status = usdhi6_read(host, USDHI6_SD_INFO1) & ~host->status_mask &
+		USDHI6_SD_INFO1_CARD;
+
+	if (!status)
+		return IRQ_NONE;
+
+	/* Ack */
+	usdhi6_write(host, USDHI6_SD_INFO1, !status);
+
+	if (!work_pending(&mmc->detect.work) &&
+	    (((status & USDHI6_SD_INFO1_CARD_INSERT) &&
+	      !mmc->card) ||
+	     ((status & USDHI6_SD_INFO1_CARD_EJECT) &&
+	      mmc->card)))
+		mmc_detect_change(mmc, msecs_to_jiffies(100));
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * Actually this should not be needed, if the built-in timeout works reliably in
+ * the both PIO cases and DMA never fails. But if DMA does fail, a timeout
+ * handler might be the only way to catch the error.
+ */
+static void usdhi6_timeout_work(struct work_struct *work)
+{
+	struct delayed_work *d = container_of(work, struct delayed_work, work);
+	struct usdhi6_host *host = container_of(d, struct usdhi6_host, timeout_work);
+	struct mmc_request *mrq = host->mrq;
+	struct mmc_data *data = mrq ? mrq->data : NULL;
+
+	dev_warn(mmc_dev(host->mmc),
+		 "%s timeout wait %u CMD%d: IRQ 0x%08x:0x%08x, last IRQ 0x%08x\n",
+		 host->dma_active ? "DMA" : "PIO",
+		 host->wait, mrq ? mrq->cmd->opcode : -1,
+		 usdhi6_read(host, USDHI6_SD_INFO1),
+		 usdhi6_read(host, USDHI6_SD_INFO2), host->irq_status);
+
+	if (host->dma_active) {
+		usdhi6_dma_kill(host);
+		usdhi6_dma_stop_unmap(host);
+	}
+
+	switch (host->wait) {
+	default:
+		dev_err(mmc_dev(host->mmc), "Invalid state %u\n", host->wait);
+		/* mrq can be NULL in this actually impossible case */
+	case USDHI6_WAIT_FOR_CMD:
+		usdhi6_error_code(host);
+		if (mrq)
+			mrq->cmd->error = -ETIMEDOUT;
+		break;
+	case USDHI6_WAIT_FOR_STOP:
+		usdhi6_error_code(host);
+		mrq->stop->error = -ETIMEDOUT;
+		break;
+	case USDHI6_WAIT_FOR_DMA:
+	case USDHI6_WAIT_FOR_MREAD:
+	case USDHI6_WAIT_FOR_MWRITE:
+	case USDHI6_WAIT_FOR_READ:
+	case USDHI6_WAIT_FOR_WRITE:
+		dev_dbg(mmc_dev(host->mmc),
+			"%c: page #%u @ +0x%x %ux%u in SG%u. Current SG %u bytes @ %u\n",
+			data->flags & MMC_DATA_READ ? 'R' : 'W', host->page_idx,
+			host->offset, data->blocks, data->blksz, data->sg_len,
+			sg_dma_len(host->sg), host->sg->offset);
+		usdhi6_sg_unmap(host, true);
+		/*
+		 * If USDHI6_WAIT_FOR_DATA_END times out, we have already unmapped
+		 * the page
+		 */
+	case USDHI6_WAIT_FOR_DATA_END:
+		usdhi6_error_code(host);
+		data->error = -ETIMEDOUT;
+	}
+
+	if (mrq)
+		usdhi6_request_done(host);
+}
+
+/*			 Probe / release				*/
+
+static const struct of_device_id usdhi6_of_match[] = {
+	{.compatible = "renesas,usdhi6rol0"},
+	{}
+};
+MODULE_DEVICE_TABLE(of, usdhi6_of_match);
+
+static int usdhi6_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct mmc_host *mmc;
+	struct usdhi6_host *host;
+	struct resource *res;
+	int irq_cd, irq_sd, irq_sdio;
+	u32 version;
+	int ret;
+
+	if (!dev->of_node)
+		return -ENODEV;
+
+	irq_cd = platform_get_irq_byname(pdev, "card detect");
+	irq_sd = platform_get_irq_byname(pdev, "data");
+	irq_sdio = platform_get_irq_byname(pdev, "SDIO");
+	if (irq_sd < 0 || irq_sdio < 0)
+		return -ENODEV;
+
+	mmc = mmc_alloc_host(sizeof(struct usdhi6_host), dev);
+	if (!mmc)
+		return -ENOMEM;
+
+	ret = mmc_of_parse(mmc);
+	if (ret < 0)
+		goto e_free_mmc;
+
+	mmc_regulator_get_supply(mmc);
+
+	host		= mmc_priv(mmc);
+	host->mmc	= mmc;
+	host->wait	= USDHI6_WAIT_FOR_REQUEST;
+	host->timeout	= msecs_to_jiffies(4000);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	host->base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(host->base)) {
+		ret = PTR_ERR(host->base);
+		goto e_free_mmc;
+	}
+
+	host->clk = devm_clk_get(dev, NULL);
+	if (IS_ERR(host->clk))
+		goto e_free_mmc;
+
+	host->imclk = clk_get_rate(host->clk);
+
+	ret = clk_prepare_enable(host->clk);
+	if (ret < 0)
+		goto e_free_mmc;
+
+	version = usdhi6_read(host, USDHI6_VERSION);
+	if ((version & 0xfff) != 0xa0d) {
+		dev_err(dev, "Version not recognized %x\n", version);
+		goto e_clk_off;
+	}
+
+	dev_info(dev, "A USDHI6ROL0 SD host detected with %d ports\n",
+		 usdhi6_read(host, USDHI6_SD_PORT_SEL) >> USDHI6_SD_PORT_SEL_PORTS_SHIFT);
+
+	usdhi6_mask_all(host);
+
+	if (irq_cd >= 0) {
+		ret = devm_request_irq(dev, irq_cd, usdhi6_cd, 0,
+				       dev_name(dev), host);
+		if (ret < 0)
+			goto e_clk_off;
+	} else {
+		mmc->caps |= MMC_CAP_NEEDS_POLL;
+	}
+
+	ret = devm_request_threaded_irq(dev, irq_sd, usdhi6_sd, usdhi6_sd_bh, 0,
+			       dev_name(dev), host);
+	if (ret < 0)
+		goto e_clk_off;
+
+	ret = devm_request_irq(dev, irq_sdio, usdhi6_sdio, 0,
+			       dev_name(dev), host);
+	if (ret < 0)
+		goto e_clk_off;
+
+	INIT_DELAYED_WORK(&host->timeout_work, usdhi6_timeout_work);
+
+	usdhi6_dma_request(host, res->start);
+
+	mmc->ops = &usdhi6_ops;
+	mmc->caps |= MMC_CAP_SD_HIGHSPEED | MMC_CAP_MMC_HIGHSPEED |
+		MMC_CAP_UHS_SDR50 | MMC_CAP_UHS_DDR50 | MMC_CAP_SDIO_IRQ;
+	/* Set .max_segs to some random number. Feel free to adjust. */
+	mmc->max_segs = 32;
+	mmc->max_blk_size = 512;
+	mmc->max_req_size = PAGE_CACHE_SIZE * mmc->max_segs;
+	mmc->max_blk_count = mmc->max_req_size / mmc->max_blk_size;
+	/*
+	 * Setting .max_seg_size to 1 page would simplify our page-mapping code,
+	 * But OTOH, having large segments makes DMA more efficient. We could
+	 * check, whether we managed to get DMA and fall back to 1 page
+	 * segments, but if we do manage to obtain DMA and then it fails at
+	 * run-time and we fall back to PIO, we will continue getting large
+	 * segments. So, we wouldn't be able to get rid of the code anyway.
+	 */
+	mmc->max_seg_size = mmc->max_req_size;
+	if (!mmc->f_max)
+		mmc->f_max = host->imclk;
+	mmc->f_min = host->imclk / 512;
+
+	platform_set_drvdata(pdev, host);
+
+	ret = mmc_add_host(mmc);
+	if (ret < 0)
+		goto e_clk_off;
+
+	return 0;
+
+e_clk_off:
+	clk_disable_unprepare(host->clk);
+e_free_mmc:
+	mmc_free_host(mmc);
+
+	return ret;
+}
+
+static int usdhi6_remove(struct platform_device *pdev)
+{
+	struct usdhi6_host *host = platform_get_drvdata(pdev);
+
+	mmc_remove_host(host->mmc);
+
+	usdhi6_mask_all(host);
+	cancel_delayed_work_sync(&host->timeout_work);
+	usdhi6_dma_release(host);
+	clk_disable_unprepare(host->clk);
+	mmc_free_host(host->mmc);
+
+	return 0;
+}
+
+static struct platform_driver usdhi6_driver = {
+	.probe		= usdhi6_probe,
+	.remove		= usdhi6_remove,
+	.driver		= {
+		.name	= "usdhi6rol0",
+		.owner	= THIS_MODULE,
+		.of_match_table = usdhi6_of_match,
+	},
+};
+
+module_platform_driver(usdhi6_driver);
+
+MODULE_DESCRIPTION("Renesas usdhi6rol0 SD/SDIO host driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:usdhi6rol0");
+MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
diff --git a/drivers/mmc/host/wmt-sdmmc.c b/drivers/mmc/host/wmt-sdmmc.c
index 498d1f79908..282891a8e45 100644
--- a/drivers/mmc/host/wmt-sdmmc.c
+++ b/drivers/mmc/host/wmt-sdmmc.c
@@ -840,7 +840,7 @@ static int wmt_mci_probe(struct platform_device *pdev)
 	priv->dma_desc_buffer = dma_alloc_coherent(&pdev->dev,
 						   mmc->max_blk_count * 16,
 						   &priv->dma_desc_device_addr,
-						   208);
+						   GFP_KERNEL);
 	if (!priv->dma_desc_buffer) {
 		dev_err(&pdev->dev, "DMA alloc fail\n");
 		ret = -EPERM;
diff --git a/drivers/mtd/Kconfig b/drivers/mtd/Kconfig
index 5d49a212961..94b821042d9 100644
--- a/drivers/mtd/Kconfig
+++ b/drivers/mtd/Kconfig
@@ -321,6 +321,8 @@ source "drivers/mtd/onenand/Kconfig"
 
 source "drivers/mtd/lpddr/Kconfig"
 
+source "drivers/mtd/spi-nor/Kconfig"
+
 source "drivers/mtd/ubi/Kconfig"
 
 endif # MTD
diff --git a/drivers/mtd/Makefile b/drivers/mtd/Makefile
index 4cfb31e6c96..99bb9a1f6e1 100644
--- a/drivers/mtd/Makefile
+++ b/drivers/mtd/Makefile
@@ -32,4 +32,5 @@ inftl-objs		:= inftlcore.o inftlmount.o
 
 obj-y		+= chips/ lpddr/ maps/ devices/ nand/ onenand/ tests/
 
+obj-$(CONFIG_MTD_SPI_NOR)	+= spi-nor/
 obj-$(CONFIG_MTD_UBI)		+= ubi/
diff --git a/drivers/mtd/chips/Kconfig b/drivers/mtd/chips/Kconfig
index e4696b37f3d..9f02c28c020 100644
--- a/drivers/mtd/chips/Kconfig
+++ b/drivers/mtd/chips/Kconfig
@@ -169,33 +169,33 @@ config MTD_OTP
 	  in the programming of OTP bits will waste them.
 
 config MTD_CFI_INTELEXT
-	tristate "Support for Intel/Sharp flash chips"
+	tristate "Support for CFI command set 0001 (Intel/Sharp chips)"
 	depends on MTD_GEN_PROBE
 	select MTD_CFI_UTIL
 	help
 	  The Common Flash Interface defines a number of different command
 	  sets which a CFI-compliant chip may claim to implement. This code
-	  provides support for one of those command sets, used on Intel
-	  StrataFlash and other parts.
+	  provides support for command set 0001, used on Intel StrataFlash
+	  and other parts.
 
 config MTD_CFI_AMDSTD
-	tristate "Support for AMD/Fujitsu/Spansion flash chips"
+	tristate "Support for CFI command set 0002 (AMD/Fujitsu/Spansion chips)"
 	depends on MTD_GEN_PROBE
 	select MTD_CFI_UTIL
 	help
 	  The Common Flash Interface defines a number of different command
 	  sets which a CFI-compliant chip may claim to implement. This code
-	  provides support for one of those command sets, used on chips
-	  including the AMD Am29LV320.
+	  provides support for command set 0002, used on chips including
+	  the AMD Am29LV320.
 
 config MTD_CFI_STAA
-	tristate "Support for ST (Advanced Architecture) flash chips"
+	tristate "Support for CFI command set 0020 (ST (Advanced Architecture) chips)"
 	depends on MTD_GEN_PROBE
 	select MTD_CFI_UTIL
 	help
 	  The Common Flash Interface defines a number of different command
 	  sets which a CFI-compliant chip may claim to implement. This code
-	  provides support for one of those command sets.
+	  provides support for command set 0020.
 
 config MTD_CFI_UTIL
 	tristate
diff --git a/drivers/mtd/chips/cfi_cmdset_0020.c b/drivers/mtd/chips/cfi_cmdset_0020.c
index 6293855fb5e..423666b51ef 100644
--- a/drivers/mtd/chips/cfi_cmdset_0020.c
+++ b/drivers/mtd/chips/cfi_cmdset_0020.c
@@ -961,7 +961,7 @@ static int cfi_staa_erase_varsize(struct mtd_info *mtd,
 			chipnum++;
 
 			if (chipnum >= cfi->numchips)
-			break;
+				break;
 		}
 	}
 
@@ -1170,7 +1170,7 @@ static int cfi_staa_lock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
 			chipnum++;
 
 			if (chipnum >= cfi->numchips)
-			break;
+				break;
 		}
 	}
 	return 0;
diff --git a/drivers/mtd/chips/cfi_util.c b/drivers/mtd/chips/cfi_util.c
index 08049f6eea6..09c79bd0b4f 100644
--- a/drivers/mtd/chips/cfi_util.c
+++ b/drivers/mtd/chips/cfi_util.c
@@ -239,7 +239,7 @@ int cfi_varsize_frob(struct mtd_info *mtd, varsize_frob_t frob,
 			chipnum++;
 
 			if (chipnum >= cfi->numchips)
-			break;
+				break;
 		}
 	}
 
diff --git a/drivers/mtd/devices/Kconfig b/drivers/mtd/devices/Kconfig
index 1210bc2923b..c49d0b127fe 100644
--- a/drivers/mtd/devices/Kconfig
+++ b/drivers/mtd/devices/Kconfig
@@ -80,7 +80,7 @@ config MTD_DATAFLASH_OTP
 
 config MTD_M25P80
 	tristate "Support most SPI Flash chips (AT26DF, M25P, W25X, ...)"
-	depends on SPI_MASTER
+	depends on SPI_MASTER && MTD_SPI_NOR
 	help
 	  This enables access to most modern SPI flash chips, used for
 	  program and data storage.   Series supported include Atmel AT26DF,
@@ -212,7 +212,7 @@ config MTD_DOCG3
 
 config MTD_ST_SPI_FSM
 	tristate "ST Microelectronics SPI FSM Serial Flash Controller"
-	depends on ARM || SH
+	depends on ARCH_STI
 	help
 	  This provides an MTD device driver for the ST Microelectronics
 	  SPI Fast Sequence Mode (FSM) Serial Flash Controller and support
diff --git a/drivers/mtd/devices/elm.c b/drivers/mtd/devices/elm.c
index 1fd4a0f7796..7df86948e6d 100644
--- a/drivers/mtd/devices/elm.c
+++ b/drivers/mtd/devices/elm.c
@@ -213,6 +213,28 @@ static void elm_load_syndrome(struct elm_info *info,
 				val = cpu_to_be32(*(u32 *) &ecc[0]) >> 12;
 				elm_write_reg(info, offset, val);
 				break;
+			case BCH16_ECC:
+				val = cpu_to_be32(*(u32 *) &ecc[22]);
+				elm_write_reg(info, offset, val);
+				offset += 4;
+				val = cpu_to_be32(*(u32 *) &ecc[18]);
+				elm_write_reg(info, offset, val);
+				offset += 4;
+				val = cpu_to_be32(*(u32 *) &ecc[14]);
+				elm_write_reg(info, offset, val);
+				offset += 4;
+				val = cpu_to_be32(*(u32 *) &ecc[10]);
+				elm_write_reg(info, offset, val);
+				offset += 4;
+				val = cpu_to_be32(*(u32 *) &ecc[6]);
+				elm_write_reg(info, offset, val);
+				offset += 4;
+				val = cpu_to_be32(*(u32 *) &ecc[2]);
+				elm_write_reg(info, offset, val);
+				offset += 4;
+				val = cpu_to_be32(*(u32 *) &ecc[0]) >> 16;
+				elm_write_reg(info, offset, val);
+				break;
 			default:
 				pr_err("invalid config bch_type\n");
 			}
@@ -418,6 +440,7 @@ static int elm_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
 /**
  * elm_context_save
  * saves ELM configurations to preserve them across Hardware powered-down
@@ -435,6 +458,13 @@ static int elm_context_save(struct elm_info *info)
 	for (i = 0; i < ERROR_VECTOR_MAX; i++) {
 		offset = i * SYNDROME_FRAGMENT_REG_SIZE;
 		switch (bch_type) {
+		case BCH16_ECC:
+			regs->elm_syndrome_fragment_6[i] = elm_read_reg(info,
+					ELM_SYNDROME_FRAGMENT_6 + offset);
+			regs->elm_syndrome_fragment_5[i] = elm_read_reg(info,
+					ELM_SYNDROME_FRAGMENT_5 + offset);
+			regs->elm_syndrome_fragment_4[i] = elm_read_reg(info,
+					ELM_SYNDROME_FRAGMENT_4 + offset);
 		case BCH8_ECC:
 			regs->elm_syndrome_fragment_3[i] = elm_read_reg(info,
 					ELM_SYNDROME_FRAGMENT_3 + offset);
@@ -473,6 +503,13 @@ static int elm_context_restore(struct elm_info *info)
 	for (i = 0; i < ERROR_VECTOR_MAX; i++) {
 		offset = i * SYNDROME_FRAGMENT_REG_SIZE;
 		switch (bch_type) {
+		case BCH16_ECC:
+			elm_write_reg(info, ELM_SYNDROME_FRAGMENT_6 + offset,
+					regs->elm_syndrome_fragment_6[i]);
+			elm_write_reg(info, ELM_SYNDROME_FRAGMENT_5 + offset,
+					regs->elm_syndrome_fragment_5[i]);
+			elm_write_reg(info, ELM_SYNDROME_FRAGMENT_4 + offset,
+					regs->elm_syndrome_fragment_4[i]);
 		case BCH8_ECC:
 			elm_write_reg(info, ELM_SYNDROME_FRAGMENT_3 + offset,
 					regs->elm_syndrome_fragment_3[i]);
@@ -509,6 +546,7 @@ static int elm_resume(struct device *dev)
 	elm_context_restore(info);
 	return 0;
 }
+#endif
 
 static SIMPLE_DEV_PM_OPS(elm_pm_ops, elm_suspend, elm_resume);
 
diff --git a/drivers/mtd/devices/m25p80.c b/drivers/mtd/devices/m25p80.c
index 524dab3ac93..ed7e0a1bed3 100644
--- a/drivers/mtd/devices/m25p80.c
+++ b/drivers/mtd/devices/m25p80.c
@@ -19,485 +19,98 @@
 #include <linux/errno.h>
 #include <linux/module.h>
 #include <linux/device.h>
-#include <linux/interrupt.h>
-#include <linux/mutex.h>
-#include <linux/math64.h>
-#include <linux/slab.h>
-#include <linux/sched.h>
-#include <linux/mod_devicetable.h>
 
-#include <linux/mtd/cfi.h>
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/partitions.h>
-#include <linux/of_platform.h>
 
 #include <linux/spi/spi.h>
 #include <linux/spi/flash.h>
+#include <linux/mtd/spi-nor.h>
 
-/* Flash opcodes. */
-#define	OPCODE_WREN		0x06	/* Write enable */
-#define	OPCODE_RDSR		0x05	/* Read status register */
-#define	OPCODE_WRSR		0x01	/* Write status register 1 byte */
-#define	OPCODE_NORM_READ	0x03	/* Read data bytes (low frequency) */
-#define	OPCODE_FAST_READ	0x0b	/* Read data bytes (high frequency) */
-#define	OPCODE_DUAL_READ        0x3b    /* Read data bytes (Dual SPI) */
-#define	OPCODE_QUAD_READ        0x6b    /* Read data bytes (Quad SPI) */
-#define	OPCODE_PP		0x02	/* Page program (up to 256 bytes) */
-#define	OPCODE_BE_4K		0x20	/* Erase 4KiB block */
-#define	OPCODE_BE_4K_PMC	0xd7	/* Erase 4KiB block on PMC chips */
-#define	OPCODE_BE_32K		0x52	/* Erase 32KiB block */
-#define	OPCODE_CHIP_ERASE	0xc7	/* Erase whole flash chip */
-#define	OPCODE_SE		0xd8	/* Sector erase (usually 64KiB) */
-#define	OPCODE_RDID		0x9f	/* Read JEDEC ID */
-#define	OPCODE_RDCR             0x35    /* Read configuration register */
-
-/* 4-byte address opcodes - used on Spansion and some Macronix flashes. */
-#define	OPCODE_NORM_READ_4B	0x13	/* Read data bytes (low frequency) */
-#define	OPCODE_FAST_READ_4B	0x0c	/* Read data bytes (high frequency) */
-#define	OPCODE_DUAL_READ_4B	0x3c    /* Read data bytes (Dual SPI) */
-#define	OPCODE_QUAD_READ_4B	0x6c    /* Read data bytes (Quad SPI) */
-#define	OPCODE_PP_4B		0x12	/* Page program (up to 256 bytes) */
-#define	OPCODE_SE_4B		0xdc	/* Sector erase (usually 64KiB) */
-
-/* Used for SST flashes only. */
-#define	OPCODE_BP		0x02	/* Byte program */
-#define	OPCODE_WRDI		0x04	/* Write disable */
-#define	OPCODE_AAI_WP		0xad	/* Auto address increment word program */
-
-/* Used for Macronix and Winbond flashes. */
-#define	OPCODE_EN4B		0xb7	/* Enter 4-byte mode */
-#define	OPCODE_EX4B		0xe9	/* Exit 4-byte mode */
-
-/* Used for Spansion flashes only. */
-#define	OPCODE_BRWR		0x17	/* Bank register write */
-
-/* Status Register bits. */
-#define	SR_WIP			1	/* Write in progress */
-#define	SR_WEL			2	/* Write enable latch */
-/* meaning of other SR_* bits may differ between vendors */
-#define	SR_BP0			4	/* Block protect 0 */
-#define	SR_BP1			8	/* Block protect 1 */
-#define	SR_BP2			0x10	/* Block protect 2 */
-#define	SR_SRWD			0x80	/* SR write protect */
-
-#define SR_QUAD_EN_MX           0x40    /* Macronix Quad I/O */
-
-/* Configuration Register bits. */
-#define CR_QUAD_EN_SPAN		0x2     /* Spansion Quad I/O */
-
-/* Define max times to check status register before we give up. */
-#define	MAX_READY_WAIT_JIFFIES	(40 * HZ)	/* M25P16 specs 40s max chip erase */
 #define	MAX_CMD_SIZE		6
-
-#define JEDEC_MFR(_jedec_id)	((_jedec_id) >> 16)
-
-/****************************************************************************/
-
-enum read_type {
-	M25P80_NORMAL = 0,
-	M25P80_FAST,
-	M25P80_DUAL,
-	M25P80_QUAD,
-};
-
 struct m25p {
 	struct spi_device	*spi;
-	struct mutex		lock;
+	struct spi_nor		spi_nor;
 	struct mtd_info		mtd;
-	u16			page_size;
-	u16			addr_width;
-	u8			erase_opcode;
-	u8			read_opcode;
-	u8			program_opcode;
-	u8			*command;
-	enum read_type		flash_read;
+	u8			command[MAX_CMD_SIZE];
 };
 
-static inline struct m25p *mtd_to_m25p(struct mtd_info *mtd)
-{
-	return container_of(mtd, struct m25p, mtd);
-}
-
-/****************************************************************************/
-
-/*
- * Internal helper functions
- */
-
-/*
- * Read the status register, returning its value in the location
- * Return the status register value.
- * Returns negative if error occurred.
- */
-static int read_sr(struct m25p *flash)
-{
-	ssize_t retval;
-	u8 code = OPCODE_RDSR;
-	u8 val;
-
-	retval = spi_write_then_read(flash->spi, &code, 1, &val, 1);
-
-	if (retval < 0) {
-		dev_err(&flash->spi->dev, "error %d reading SR\n",
-				(int) retval);
-		return retval;
-	}
-
-	return val;
-}
-
-/*
- * Read configuration register, returning its value in the
- * location. Return the configuration register value.
- * Returns negative if error occured.
- */
-static int read_cr(struct m25p *flash)
-{
-	u8 code = OPCODE_RDCR;
-	int ret;
-	u8 val;
-
-	ret = spi_write_then_read(flash->spi, &code, 1, &val, 1);
-	if (ret < 0) {
-		dev_err(&flash->spi->dev, "error %d reading CR\n", ret);
-		return ret;
-	}
-
-	return val;
-}
-
-/*
- * Write status register 1 byte
- * Returns negative if error occurred.
- */
-static int write_sr(struct m25p *flash, u8 val)
-{
-	flash->command[0] = OPCODE_WRSR;
-	flash->command[1] = val;
-
-	return spi_write(flash->spi, flash->command, 2);
-}
-
-/*
- * Set write enable latch with Write Enable command.
- * Returns negative if error occurred.
- */
-static inline int write_enable(struct m25p *flash)
-{
-	u8	code = OPCODE_WREN;
-
-	return spi_write_then_read(flash->spi, &code, 1, NULL, 0);
-}
-
-/*
- * Send write disble instruction to the chip.
- */
-static inline int write_disable(struct m25p *flash)
-{
-	u8	code = OPCODE_WRDI;
-
-	return spi_write_then_read(flash->spi, &code, 1, NULL, 0);
-}
-
-/*
- * Enable/disable 4-byte addressing mode.
- */
-static inline int set_4byte(struct m25p *flash, u32 jedec_id, int enable)
-{
-	int status;
-	bool need_wren = false;
-
-	switch (JEDEC_MFR(jedec_id)) {
-	case CFI_MFR_ST: /* Micron, actually */
-		/* Some Micron need WREN command; all will accept it */
-		need_wren = true;
-	case CFI_MFR_MACRONIX:
-	case 0xEF /* winbond */:
-		if (need_wren)
-			write_enable(flash);
-
-		flash->command[0] = enable ? OPCODE_EN4B : OPCODE_EX4B;
-		status = spi_write(flash->spi, flash->command, 1);
-
-		if (need_wren)
-			write_disable(flash);
-
-		return status;
-	default:
-		/* Spansion style */
-		flash->command[0] = OPCODE_BRWR;
-		flash->command[1] = enable << 7;
-		return spi_write(flash->spi, flash->command, 2);
-	}
-}
-
-/*
- * Service routine to read status register until ready, or timeout occurs.
- * Returns non-zero if error.
- */
-static int wait_till_ready(struct m25p *flash)
-{
-	unsigned long deadline;
-	int sr;
-
-	deadline = jiffies + MAX_READY_WAIT_JIFFIES;
-
-	do {
-		if ((sr = read_sr(flash)) < 0)
-			break;
-		else if (!(sr & SR_WIP))
-			return 0;
-
-		cond_resched();
-
-	} while (!time_after_eq(jiffies, deadline));
-
-	return 1;
-}
-
-/*
- * Write status Register and configuration register with 2 bytes
- * The first byte will be written to the status register, while the
- * second byte will be written to the configuration register.
- * Return negative if error occured.
- */
-static int write_sr_cr(struct m25p *flash, u16 val)
-{
-	flash->command[0] = OPCODE_WRSR;
-	flash->command[1] = val & 0xff;
-	flash->command[2] = (val >> 8);
-
-	return spi_write(flash->spi, flash->command, 3);
-}
-
-static int macronix_quad_enable(struct m25p *flash)
-{
-	int ret, val;
-	u8 cmd[2];
-	cmd[0] = OPCODE_WRSR;
-
-	val = read_sr(flash);
-	cmd[1] = val | SR_QUAD_EN_MX;
-	write_enable(flash);
-
-	spi_write(flash->spi, &cmd, 2);
-
-	if (wait_till_ready(flash))
-		return 1;
-
-	ret = read_sr(flash);
-	if (!(ret > 0 && (ret & SR_QUAD_EN_MX))) {
-		dev_err(&flash->spi->dev, "Macronix Quad bit not set\n");
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int spansion_quad_enable(struct m25p *flash)
+static int m25p80_read_reg(struct spi_nor *nor, u8 code, u8 *val, int len)
 {
+	struct m25p *flash = nor->priv;
+	struct spi_device *spi = flash->spi;
 	int ret;
-	int quad_en = CR_QUAD_EN_SPAN << 8;
-
-	write_enable(flash);
 
-	ret = write_sr_cr(flash, quad_en);
-	if (ret < 0) {
-		dev_err(&flash->spi->dev,
-			"error while writing configuration register\n");
-		return -EINVAL;
-	}
-
-	/* read back and check it */
-	ret = read_cr(flash);
-	if (!(ret > 0 && (ret & CR_QUAD_EN_SPAN))) {
-		dev_err(&flash->spi->dev, "Spansion Quad bit not set\n");
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int set_quad_mode(struct m25p *flash, u32 jedec_id)
-{
-	int status;
-
-	switch (JEDEC_MFR(jedec_id)) {
-	case CFI_MFR_MACRONIX:
-		status = macronix_quad_enable(flash);
-		if (status) {
-			dev_err(&flash->spi->dev,
-				"Macronix quad-read not enabled\n");
-			return -EINVAL;
-		}
-		return status;
-	default:
-		status = spansion_quad_enable(flash);
-		if (status) {
-			dev_err(&flash->spi->dev,
-				"Spansion quad-read not enabled\n");
-			return -EINVAL;
-		}
-		return status;
-	}
-}
-
-/*
- * Erase the whole flash memory
- *
- * Returns 0 if successful, non-zero otherwise.
- */
-static int erase_chip(struct m25p *flash)
-{
-	pr_debug("%s: %s %lldKiB\n", dev_name(&flash->spi->dev), __func__,
-			(long long)(flash->mtd.size >> 10));
+	ret = spi_write_then_read(spi, &code, 1, val, len);
+	if (ret < 0)
+		dev_err(&spi->dev, "error %d reading %x\n", ret, code);
 
-	/* Wait until finished previous write command. */
-	if (wait_till_ready(flash))
-		return 1;
-
-	/* Send write enable, then erase commands. */
-	write_enable(flash);
-
-	/* Set up command buffer. */
-	flash->command[0] = OPCODE_CHIP_ERASE;
-
-	spi_write(flash->spi, flash->command, 1);
-
-	return 0;
+	return ret;
 }
 
-static void m25p_addr2cmd(struct m25p *flash, unsigned int addr, u8 *cmd)
+static void m25p_addr2cmd(struct spi_nor *nor, unsigned int addr, u8 *cmd)
 {
 	/* opcode is in cmd[0] */
-	cmd[1] = addr >> (flash->addr_width * 8 -  8);
-	cmd[2] = addr >> (flash->addr_width * 8 - 16);
-	cmd[3] = addr >> (flash->addr_width * 8 - 24);
-	cmd[4] = addr >> (flash->addr_width * 8 - 32);
+	cmd[1] = addr >> (nor->addr_width * 8 -  8);
+	cmd[2] = addr >> (nor->addr_width * 8 - 16);
+	cmd[3] = addr >> (nor->addr_width * 8 - 24);
+	cmd[4] = addr >> (nor->addr_width * 8 - 32);
 }
 
-static int m25p_cmdsz(struct m25p *flash)
+static int m25p_cmdsz(struct spi_nor *nor)
 {
-	return 1 + flash->addr_width;
+	return 1 + nor->addr_width;
 }
 
-/*
- * Erase one sector of flash memory at offset ``offset'' which is any
- * address within the sector which should be erased.
- *
- * Returns 0 if successful, non-zero otherwise.
- */
-static int erase_sector(struct m25p *flash, u32 offset)
+static int m25p80_write_reg(struct spi_nor *nor, u8 opcode, u8 *buf, int len,
+			int wr_en)
 {
-	pr_debug("%s: %s %dKiB at 0x%08x\n", dev_name(&flash->spi->dev),
-			__func__, flash->mtd.erasesize / 1024, offset);
-
-	/* Wait until finished previous write command. */
-	if (wait_till_ready(flash))
-		return 1;
+	struct m25p *flash = nor->priv;
+	struct spi_device *spi = flash->spi;
 
-	/* Send write enable, then erase commands. */
-	write_enable(flash);
-
-	/* Set up command buffer. */
-	flash->command[0] = flash->erase_opcode;
-	m25p_addr2cmd(flash, offset, flash->command);
-
-	spi_write(flash->spi, flash->command, m25p_cmdsz(flash));
+	flash->command[0] = opcode;
+	if (buf)
+		memcpy(&flash->command[1], buf, len);
 
-	return 0;
+	return spi_write(spi, flash->command, len + 1);
 }
 
-/****************************************************************************/
-
-/*
- * MTD implementation
- */
-
-/*
- * Erase an address range on the flash chip.  The address range may extend
- * one or more erase sectors.  Return an error is there is a problem erasing.
- */
-static int m25p80_erase(struct mtd_info *mtd, struct erase_info *instr)
+static void m25p80_write(struct spi_nor *nor, loff_t to, size_t len,
+			size_t *retlen, const u_char *buf)
 {
-	struct m25p *flash = mtd_to_m25p(mtd);
-	u32 addr,len;
-	uint32_t rem;
-
-	pr_debug("%s: %s at 0x%llx, len %lld\n", dev_name(&flash->spi->dev),
-			__func__, (long long)instr->addr,
-			(long long)instr->len);
-
-	div_u64_rem(instr->len, mtd->erasesize, &rem);
-	if (rem)
-		return -EINVAL;
-
-	addr = instr->addr;
-	len = instr->len;
-
-	mutex_lock(&flash->lock);
-
-	/* whole-chip erase? */
-	if (len == flash->mtd.size) {
-		if (erase_chip(flash)) {
-			instr->state = MTD_ERASE_FAILED;
-			mutex_unlock(&flash->lock);
-			return -EIO;
-		}
+	struct m25p *flash = nor->priv;
+	struct spi_device *spi = flash->spi;
+	struct spi_transfer t[2] = {};
+	struct spi_message m;
+	int cmd_sz = m25p_cmdsz(nor);
 
-	/* REVISIT in some cases we could speed up erasing large regions
-	 * by using OPCODE_SE instead of OPCODE_BE_4K.  We may have set up
-	 * to use "small sector erase", but that's not always optimal.
-	 */
+	spi_message_init(&m);
 
-	/* "sector"-at-a-time erase */
-	} else {
-		while (len) {
-			if (erase_sector(flash, addr)) {
-				instr->state = MTD_ERASE_FAILED;
-				mutex_unlock(&flash->lock);
-				return -EIO;
-			}
+	if (nor->program_opcode == SPINOR_OP_AAI_WP && nor->sst_write_second)
+		cmd_sz = 1;
 
-			addr += mtd->erasesize;
-			len -= mtd->erasesize;
-		}
-	}
+	flash->command[0] = nor->program_opcode;
+	m25p_addr2cmd(nor, to, flash->command);
 
-	mutex_unlock(&flash->lock);
+	t[0].tx_buf = flash->command;
+	t[0].len = cmd_sz;
+	spi_message_add_tail(&t[0], &m);
 
-	instr->state = MTD_ERASE_DONE;
-	mtd_erase_callback(instr);
+	t[1].tx_buf = buf;
+	t[1].len = len;
+	spi_message_add_tail(&t[1], &m);
 
-	return 0;
-}
+	spi_sync(spi, &m);
 
-/*
- * Dummy Cycle calculation for different type of read.
- * It can be used to support more commands with
- * different dummy cycle requirements.
- */
-static inline int m25p80_dummy_cycles_read(struct m25p *flash)
-{
-	switch (flash->flash_read) {
-	case M25P80_FAST:
-	case M25P80_DUAL:
-	case M25P80_QUAD:
-		return 1;
-	case M25P80_NORMAL:
-		return 0;
-	default:
-		dev_err(&flash->spi->dev, "No valid read type supported\n");
-		return -1;
-	}
+	*retlen += m.actual_length - cmd_sz;
 }
 
-static inline unsigned int m25p80_rx_nbits(const struct m25p *flash)
+static inline unsigned int m25p80_rx_nbits(struct spi_nor *nor)
 {
-	switch (flash->flash_read) {
-	case M25P80_DUAL:
+	switch (nor->flash_read) {
+	case SPI_NOR_DUAL:
 		return 2;
-	case M25P80_QUAD:
+	case SPI_NOR_QUAD:
 		return 4;
 	default:
 		return 0;
@@ -505,590 +118,72 @@ static inline unsigned int m25p80_rx_nbits(const struct m25p *flash)
 }
 
 /*
- * Read an address range from the flash chip.  The address range
+ * Read an address range from the nor chip.  The address range
  * may be any size provided it is within the physical boundaries.
  */
-static int m25p80_read(struct mtd_info *mtd, loff_t from, size_t len,
-	size_t *retlen, u_char *buf)
+static int m25p80_read(struct spi_nor *nor, loff_t from, size_t len,
+			size_t *retlen, u_char *buf)
 {
-	struct m25p *flash = mtd_to_m25p(mtd);
+	struct m25p *flash = nor->priv;
+	struct spi_device *spi = flash->spi;
 	struct spi_transfer t[2];
 	struct spi_message m;
-	uint8_t opcode;
-	int dummy;
+	int dummy = nor->read_dummy;
+	int ret;
 
-	pr_debug("%s: %s from 0x%08x, len %zd\n", dev_name(&flash->spi->dev),
-			__func__, (u32)from, len);
+	/* Wait till previous write/erase is done. */
+	ret = nor->wait_till_ready(nor);
+	if (ret)
+		return ret;
 
 	spi_message_init(&m);
 	memset(t, 0, (sizeof t));
 
-	dummy =  m25p80_dummy_cycles_read(flash);
-	if (dummy < 0) {
-		dev_err(&flash->spi->dev, "No valid read command supported\n");
-		return -EINVAL;
-	}
+	flash->command[0] = nor->read_opcode;
+	m25p_addr2cmd(nor, from, flash->command);
 
 	t[0].tx_buf = flash->command;
-	t[0].len = m25p_cmdsz(flash) + dummy;
+	t[0].len = m25p_cmdsz(nor) + dummy;
 	spi_message_add_tail(&t[0], &m);
 
 	t[1].rx_buf = buf;
-	t[1].rx_nbits = m25p80_rx_nbits(flash);
+	t[1].rx_nbits = m25p80_rx_nbits(nor);
 	t[1].len = len;
 	spi_message_add_tail(&t[1], &m);
 
-	mutex_lock(&flash->lock);
-
-	/* Wait till previous write/erase is done. */
-	if (wait_till_ready(flash)) {
-		/* REVISIT status return?? */
-		mutex_unlock(&flash->lock);
-		return 1;
-	}
-
-	/* Set up the write data buffer. */
-	opcode = flash->read_opcode;
-	flash->command[0] = opcode;
-	m25p_addr2cmd(flash, from, flash->command);
-
-	spi_sync(flash->spi, &m);
-
-	*retlen = m.actual_length - m25p_cmdsz(flash) - dummy;
-
-	mutex_unlock(&flash->lock);
+	spi_sync(spi, &m);
 
+	*retlen = m.actual_length - m25p_cmdsz(nor) - dummy;
 	return 0;
 }
 
-/*
- * Write an address range to the flash chip.  Data must be written in
- * FLASH_PAGESIZE chunks.  The address range may be any size provided
- * it is within the physical boundaries.
- */
-static int m25p80_write(struct mtd_info *mtd, loff_t to, size_t len,
-	size_t *retlen, const u_char *buf)
+static int m25p80_erase(struct spi_nor *nor, loff_t offset)
 {
-	struct m25p *flash = mtd_to_m25p(mtd);
-	u32 page_offset, page_size;
-	struct spi_transfer t[2];
-	struct spi_message m;
-
-	pr_debug("%s: %s to 0x%08x, len %zd\n", dev_name(&flash->spi->dev),
-			__func__, (u32)to, len);
-
-	spi_message_init(&m);
-	memset(t, 0, (sizeof t));
-
-	t[0].tx_buf = flash->command;
-	t[0].len = m25p_cmdsz(flash);
-	spi_message_add_tail(&t[0], &m);
-
-	t[1].tx_buf = buf;
-	spi_message_add_tail(&t[1], &m);
-
-	mutex_lock(&flash->lock);
-
-	/* Wait until finished previous write command. */
-	if (wait_till_ready(flash)) {
-		mutex_unlock(&flash->lock);
-		return 1;
-	}
-
-	write_enable(flash);
-
-	/* Set up the opcode in the write buffer. */
-	flash->command[0] = flash->program_opcode;
-	m25p_addr2cmd(flash, to, flash->command);
-
-	page_offset = to & (flash->page_size - 1);
-
-	/* do all the bytes fit onto one page? */
-	if (page_offset + len <= flash->page_size) {
-		t[1].len = len;
-
-		spi_sync(flash->spi, &m);
-
-		*retlen = m.actual_length - m25p_cmdsz(flash);
-	} else {
-		u32 i;
-
-		/* the size of data remaining on the first page */
-		page_size = flash->page_size - page_offset;
-
-		t[1].len = page_size;
-		spi_sync(flash->spi, &m);
-
-		*retlen = m.actual_length - m25p_cmdsz(flash);
-
-		/* write everything in flash->page_size chunks */
-		for (i = page_size; i < len; i += page_size) {
-			page_size = len - i;
-			if (page_size > flash->page_size)
-				page_size = flash->page_size;
-
-			/* write the next page to flash */
-			m25p_addr2cmd(flash, to + i, flash->command);
-
-			t[1].tx_buf = buf + i;
-			t[1].len = page_size;
-
-			wait_till_ready(flash);
-
-			write_enable(flash);
-
-			spi_sync(flash->spi, &m);
-
-			*retlen += m.actual_length - m25p_cmdsz(flash);
-		}
-	}
-
-	mutex_unlock(&flash->lock);
-
-	return 0;
-}
-
-static int sst_write(struct mtd_info *mtd, loff_t to, size_t len,
-		size_t *retlen, const u_char *buf)
-{
-	struct m25p *flash = mtd_to_m25p(mtd);
-	struct spi_transfer t[2];
-	struct spi_message m;
-	size_t actual;
-	int cmd_sz, ret;
-
-	pr_debug("%s: %s to 0x%08x, len %zd\n", dev_name(&flash->spi->dev),
-			__func__, (u32)to, len);
-
-	spi_message_init(&m);
-	memset(t, 0, (sizeof t));
-
-	t[0].tx_buf = flash->command;
-	t[0].len = m25p_cmdsz(flash);
-	spi_message_add_tail(&t[0], &m);
-
-	t[1].tx_buf = buf;
-	spi_message_add_tail(&t[1], &m);
+	struct m25p *flash = nor->priv;
+	int ret;
 
-	mutex_lock(&flash->lock);
+	dev_dbg(nor->dev, "%dKiB at 0x%08x\n",
+		flash->mtd.erasesize / 1024, (u32)offset);
 
 	/* Wait until finished previous write command. */
-	ret = wait_till_ready(flash);
+	ret = nor->wait_till_ready(nor);
 	if (ret)
-		goto time_out;
-
-	write_enable(flash);
-
-	actual = to % 2;
-	/* Start write from odd address. */
-	if (actual) {
-		flash->command[0] = OPCODE_BP;
-		m25p_addr2cmd(flash, to, flash->command);
-
-		/* write one byte. */
-		t[1].len = 1;
-		spi_sync(flash->spi, &m);
-		ret = wait_till_ready(flash);
-		if (ret)
-			goto time_out;
-		*retlen += m.actual_length - m25p_cmdsz(flash);
-	}
-	to += actual;
-
-	flash->command[0] = OPCODE_AAI_WP;
-	m25p_addr2cmd(flash, to, flash->command);
-
-	/* Write out most of the data here. */
-	cmd_sz = m25p_cmdsz(flash);
-	for (; actual < len - 1; actual += 2) {
-		t[0].len = cmd_sz;
-		/* write two bytes. */
-		t[1].len = 2;
-		t[1].tx_buf = buf + actual;
+		return ret;
 
-		spi_sync(flash->spi, &m);
-		ret = wait_till_ready(flash);
-		if (ret)
-			goto time_out;
-		*retlen += m.actual_length - cmd_sz;
-		cmd_sz = 1;
-		to += 2;
-	}
-	write_disable(flash);
-	ret = wait_till_ready(flash);
+	/* Send write enable, then erase commands. */
+	ret = nor->write_reg(nor, SPINOR_OP_WREN, NULL, 0, 0);
 	if (ret)
-		goto time_out;
-
-	/* Write out trailing byte if it exists. */
-	if (actual != len) {
-		write_enable(flash);
-		flash->command[0] = OPCODE_BP;
-		m25p_addr2cmd(flash, to, flash->command);
-		t[0].len = m25p_cmdsz(flash);
-		t[1].len = 1;
-		t[1].tx_buf = buf + actual;
-
-		spi_sync(flash->spi, &m);
-		ret = wait_till_ready(flash);
-		if (ret)
-			goto time_out;
-		*retlen += m.actual_length - m25p_cmdsz(flash);
-		write_disable(flash);
-	}
-
-time_out:
-	mutex_unlock(&flash->lock);
-	return ret;
-}
-
-static int m25p80_lock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
-{
-	struct m25p *flash = mtd_to_m25p(mtd);
-	uint32_t offset = ofs;
-	uint8_t status_old, status_new;
-	int res = 0;
-
-	mutex_lock(&flash->lock);
-	/* Wait until finished previous command */
-	if (wait_till_ready(flash)) {
-		res = 1;
-		goto err;
-	}
-
-	status_old = read_sr(flash);
-
-	if (offset < flash->mtd.size-(flash->mtd.size/2))
-		status_new = status_old | SR_BP2 | SR_BP1 | SR_BP0;
-	else if (offset < flash->mtd.size-(flash->mtd.size/4))
-		status_new = (status_old & ~SR_BP0) | SR_BP2 | SR_BP1;
-	else if (offset < flash->mtd.size-(flash->mtd.size/8))
-		status_new = (status_old & ~SR_BP1) | SR_BP2 | SR_BP0;
-	else if (offset < flash->mtd.size-(flash->mtd.size/16))
-		status_new = (status_old & ~(SR_BP0|SR_BP1)) | SR_BP2;
-	else if (offset < flash->mtd.size-(flash->mtd.size/32))
-		status_new = (status_old & ~SR_BP2) | SR_BP1 | SR_BP0;
-	else if (offset < flash->mtd.size-(flash->mtd.size/64))
-		status_new = (status_old & ~(SR_BP2|SR_BP0)) | SR_BP1;
-	else
-		status_new = (status_old & ~(SR_BP2|SR_BP1)) | SR_BP0;
-
-	/* Only modify protection if it will not unlock other areas */
-	if ((status_new&(SR_BP2|SR_BP1|SR_BP0)) >
-					(status_old&(SR_BP2|SR_BP1|SR_BP0))) {
-		write_enable(flash);
-		if (write_sr(flash, status_new) < 0) {
-			res = 1;
-			goto err;
-		}
-	}
-
-err:	mutex_unlock(&flash->lock);
-	return res;
-}
-
-static int m25p80_unlock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
-{
-	struct m25p *flash = mtd_to_m25p(mtd);
-	uint32_t offset = ofs;
-	uint8_t status_old, status_new;
-	int res = 0;
-
-	mutex_lock(&flash->lock);
-	/* Wait until finished previous command */
-	if (wait_till_ready(flash)) {
-		res = 1;
-		goto err;
-	}
-
-	status_old = read_sr(flash);
-
-	if (offset+len > flash->mtd.size-(flash->mtd.size/64))
-		status_new = status_old & ~(SR_BP2|SR_BP1|SR_BP0);
-	else if (offset+len > flash->mtd.size-(flash->mtd.size/32))
-		status_new = (status_old & ~(SR_BP2|SR_BP1)) | SR_BP0;
-	else if (offset+len > flash->mtd.size-(flash->mtd.size/16))
-		status_new = (status_old & ~(SR_BP2|SR_BP0)) | SR_BP1;
-	else if (offset+len > flash->mtd.size-(flash->mtd.size/8))
-		status_new = (status_old & ~SR_BP2) | SR_BP1 | SR_BP0;
-	else if (offset+len > flash->mtd.size-(flash->mtd.size/4))
-		status_new = (status_old & ~(SR_BP0|SR_BP1)) | SR_BP2;
-	else if (offset+len > flash->mtd.size-(flash->mtd.size/2))
-		status_new = (status_old & ~SR_BP1) | SR_BP2 | SR_BP0;
-	else
-		status_new = (status_old & ~SR_BP0) | SR_BP2 | SR_BP1;
-
-	/* Only modify protection if it will not lock other areas */
-	if ((status_new&(SR_BP2|SR_BP1|SR_BP0)) <
-					(status_old&(SR_BP2|SR_BP1|SR_BP0))) {
-		write_enable(flash);
-		if (write_sr(flash, status_new) < 0) {
-			res = 1;
-			goto err;
-		}
-	}
-
-err:	mutex_unlock(&flash->lock);
-	return res;
-}
-
-/****************************************************************************/
-
-/*
- * SPI device driver setup and teardown
- */
-
-struct flash_info {
-	/* JEDEC id zero means "no ID" (most older chips); otherwise it has
-	 * a high byte of zero plus three data bytes: the manufacturer id,
-	 * then a two byte device id.
-	 */
-	u32		jedec_id;
-	u16             ext_id;
-
-	/* The size listed here is what works with OPCODE_SE, which isn't
-	 * necessarily called a "sector" by the vendor.
-	 */
-	unsigned	sector_size;
-	u16		n_sectors;
-
-	u16		page_size;
-	u16		addr_width;
-
-	u16		flags;
-#define	SECT_4K		0x01		/* OPCODE_BE_4K works uniformly */
-#define	M25P_NO_ERASE	0x02		/* No erase command needed */
-#define	SST_WRITE	0x04		/* use SST byte programming */
-#define	M25P_NO_FR	0x08		/* Can't do fastread */
-#define	SECT_4K_PMC	0x10		/* OPCODE_BE_4K_PMC works uniformly */
-#define	M25P80_DUAL_READ	0x20    /* Flash supports Dual Read */
-#define	M25P80_QUAD_READ	0x40    /* Flash supports Quad Read */
-};
-
-#define INFO(_jedec_id, _ext_id, _sector_size, _n_sectors, _flags)	\
-	((kernel_ulong_t)&(struct flash_info) {				\
-		.jedec_id = (_jedec_id),				\
-		.ext_id = (_ext_id),					\
-		.sector_size = (_sector_size),				\
-		.n_sectors = (_n_sectors),				\
-		.page_size = 256,					\
-		.flags = (_flags),					\
-	})
-
-#define CAT25_INFO(_sector_size, _n_sectors, _page_size, _addr_width, _flags)	\
-	((kernel_ulong_t)&(struct flash_info) {				\
-		.sector_size = (_sector_size),				\
-		.n_sectors = (_n_sectors),				\
-		.page_size = (_page_size),				\
-		.addr_width = (_addr_width),				\
-		.flags = (_flags),					\
-	})
-
-/* NOTE: double check command sets and memory organization when you add
- * more flash chips.  This current list focusses on newer chips, which
- * have been converging on command sets which including JEDEC ID.
- */
-static const struct spi_device_id m25p_ids[] = {
-	/* Atmel -- some are (confusingly) marketed as "DataFlash" */
-	{ "at25fs010",  INFO(0x1f6601, 0, 32 * 1024,   4, SECT_4K) },
-	{ "at25fs040",  INFO(0x1f6604, 0, 64 * 1024,   8, SECT_4K) },
-
-	{ "at25df041a", INFO(0x1f4401, 0, 64 * 1024,   8, SECT_4K) },
-	{ "at25df321a", INFO(0x1f4701, 0, 64 * 1024,  64, SECT_4K) },
-	{ "at25df641",  INFO(0x1f4800, 0, 64 * 1024, 128, SECT_4K) },
-
-	{ "at26f004",   INFO(0x1f0400, 0, 64 * 1024,  8, SECT_4K) },
-	{ "at26df081a", INFO(0x1f4501, 0, 64 * 1024, 16, SECT_4K) },
-	{ "at26df161a", INFO(0x1f4601, 0, 64 * 1024, 32, SECT_4K) },
-	{ "at26df321",  INFO(0x1f4700, 0, 64 * 1024, 64, SECT_4K) },
-
-	{ "at45db081d", INFO(0x1f2500, 0, 64 * 1024, 16, SECT_4K) },
-
-	/* EON -- en25xxx */
-	{ "en25f32",    INFO(0x1c3116, 0, 64 * 1024,   64, SECT_4K) },
-	{ "en25p32",    INFO(0x1c2016, 0, 64 * 1024,   64, 0) },
-	{ "en25q32b",   INFO(0x1c3016, 0, 64 * 1024,   64, 0) },
-	{ "en25p64",    INFO(0x1c2017, 0, 64 * 1024,  128, 0) },
-	{ "en25q64",    INFO(0x1c3017, 0, 64 * 1024,  128, SECT_4K) },
-	{ "en25qh256",  INFO(0x1c7019, 0, 64 * 1024,  512, 0) },
-
-	/* ESMT */
-	{ "f25l32pa", INFO(0x8c2016, 0, 64 * 1024, 64, SECT_4K) },
-
-	/* Everspin */
-	{ "mr25h256", CAT25_INFO( 32 * 1024, 1, 256, 2, M25P_NO_ERASE | M25P_NO_FR) },
-	{ "mr25h10",  CAT25_INFO(128 * 1024, 1, 256, 3, M25P_NO_ERASE | M25P_NO_FR) },
-
-	/* GigaDevice */
-	{ "gd25q32", INFO(0xc84016, 0, 64 * 1024,  64, SECT_4K) },
-	{ "gd25q64", INFO(0xc84017, 0, 64 * 1024, 128, SECT_4K) },
-
-	/* Intel/Numonyx -- xxxs33b */
-	{ "160s33b",  INFO(0x898911, 0, 64 * 1024,  32, 0) },
-	{ "320s33b",  INFO(0x898912, 0, 64 * 1024,  64, 0) },
-	{ "640s33b",  INFO(0x898913, 0, 64 * 1024, 128, 0) },
-
-	/* Macronix */
-	{ "mx25l2005a",  INFO(0xc22012, 0, 64 * 1024,   4, SECT_4K) },
-	{ "mx25l4005a",  INFO(0xc22013, 0, 64 * 1024,   8, SECT_4K) },
-	{ "mx25l8005",   INFO(0xc22014, 0, 64 * 1024,  16, 0) },
-	{ "mx25l1606e",  INFO(0xc22015, 0, 64 * 1024,  32, SECT_4K) },
-	{ "mx25l3205d",  INFO(0xc22016, 0, 64 * 1024,  64, 0) },
-	{ "mx25l3255e",  INFO(0xc29e16, 0, 64 * 1024,  64, SECT_4K) },
-	{ "mx25l6405d",  INFO(0xc22017, 0, 64 * 1024, 128, 0) },
-	{ "mx25l12805d", INFO(0xc22018, 0, 64 * 1024, 256, 0) },
-	{ "mx25l12855e", INFO(0xc22618, 0, 64 * 1024, 256, 0) },
-	{ "mx25l25635e", INFO(0xc22019, 0, 64 * 1024, 512, 0) },
-	{ "mx25l25655e", INFO(0xc22619, 0, 64 * 1024, 512, 0) },
-	{ "mx66l51235l", INFO(0xc2201a, 0, 64 * 1024, 1024, M25P80_QUAD_READ) },
-	{ "mx66l1g55g",  INFO(0xc2261b, 0, 64 * 1024, 2048, M25P80_QUAD_READ) },
-
-	/* Micron */
-	{ "n25q064",     INFO(0x20ba17, 0, 64 * 1024,  128, 0) },
-	{ "n25q128a11",  INFO(0x20bb18, 0, 64 * 1024,  256, 0) },
-	{ "n25q128a13",  INFO(0x20ba18, 0, 64 * 1024,  256, 0) },
-	{ "n25q256a",    INFO(0x20ba19, 0, 64 * 1024,  512, SECT_4K) },
-	{ "n25q512a",    INFO(0x20bb20, 0, 64 * 1024, 1024, SECT_4K) },
-
-	/* PMC */
-	{ "pm25lv512",   INFO(0,        0, 32 * 1024,    2, SECT_4K_PMC) },
-	{ "pm25lv010",   INFO(0,        0, 32 * 1024,    4, SECT_4K_PMC) },
-	{ "pm25lq032",   INFO(0x7f9d46, 0, 64 * 1024,   64, SECT_4K) },
-
-	/* Spansion -- single (large) sector size only, at least
-	 * for the chips listed here (without boot sectors).
-	 */
-	{ "s25sl032p",  INFO(0x010215, 0x4d00,  64 * 1024,  64, 0) },
-	{ "s25sl064p",  INFO(0x010216, 0x4d00,  64 * 1024, 128, 0) },
-	{ "s25fl256s0", INFO(0x010219, 0x4d00, 256 * 1024, 128, 0) },
-	{ "s25fl256s1", INFO(0x010219, 0x4d01,  64 * 1024, 512, M25P80_DUAL_READ | M25P80_QUAD_READ) },
-	{ "s25fl512s",  INFO(0x010220, 0x4d00, 256 * 1024, 256, M25P80_DUAL_READ | M25P80_QUAD_READ) },
-	{ "s70fl01gs",  INFO(0x010221, 0x4d00, 256 * 1024, 256, 0) },
-	{ "s25sl12800", INFO(0x012018, 0x0300, 256 * 1024,  64, 0) },
-	{ "s25sl12801", INFO(0x012018, 0x0301,  64 * 1024, 256, 0) },
-	{ "s25fl129p0", INFO(0x012018, 0x4d00, 256 * 1024,  64, 0) },
-	{ "s25fl129p1", INFO(0x012018, 0x4d01,  64 * 1024, 256, 0) },
-	{ "s25sl004a",  INFO(0x010212,      0,  64 * 1024,   8, 0) },
-	{ "s25sl008a",  INFO(0x010213,      0,  64 * 1024,  16, 0) },
-	{ "s25sl016a",  INFO(0x010214,      0,  64 * 1024,  32, 0) },
-	{ "s25sl032a",  INFO(0x010215,      0,  64 * 1024,  64, 0) },
-	{ "s25sl064a",  INFO(0x010216,      0,  64 * 1024, 128, 0) },
-	{ "s25fl008k",  INFO(0xef4014,      0,  64 * 1024,  16, SECT_4K) },
-	{ "s25fl016k",  INFO(0xef4015,      0,  64 * 1024,  32, SECT_4K) },
-	{ "s25fl064k",  INFO(0xef4017,      0,  64 * 1024, 128, SECT_4K) },
-
-	/* SST -- large erase sizes are "overlays", "sectors" are 4K */
-	{ "sst25vf040b", INFO(0xbf258d, 0, 64 * 1024,  8, SECT_4K | SST_WRITE) },
-	{ "sst25vf080b", INFO(0xbf258e, 0, 64 * 1024, 16, SECT_4K | SST_WRITE) },
-	{ "sst25vf016b", INFO(0xbf2541, 0, 64 * 1024, 32, SECT_4K | SST_WRITE) },
-	{ "sst25vf032b", INFO(0xbf254a, 0, 64 * 1024, 64, SECT_4K | SST_WRITE) },
-	{ "sst25vf064c", INFO(0xbf254b, 0, 64 * 1024, 128, SECT_4K) },
-	{ "sst25wf512",  INFO(0xbf2501, 0, 64 * 1024,  1, SECT_4K | SST_WRITE) },
-	{ "sst25wf010",  INFO(0xbf2502, 0, 64 * 1024,  2, SECT_4K | SST_WRITE) },
-	{ "sst25wf020",  INFO(0xbf2503, 0, 64 * 1024,  4, SECT_4K | SST_WRITE) },
-	{ "sst25wf040",  INFO(0xbf2504, 0, 64 * 1024,  8, SECT_4K | SST_WRITE) },
-
-	/* ST Microelectronics -- newer production may have feature updates */
-	{ "m25p05",  INFO(0x202010,  0,  32 * 1024,   2, 0) },
-	{ "m25p10",  INFO(0x202011,  0,  32 * 1024,   4, 0) },
-	{ "m25p20",  INFO(0x202012,  0,  64 * 1024,   4, 0) },
-	{ "m25p40",  INFO(0x202013,  0,  64 * 1024,   8, 0) },
-	{ "m25p80",  INFO(0x202014,  0,  64 * 1024,  16, 0) },
-	{ "m25p16",  INFO(0x202015,  0,  64 * 1024,  32, 0) },
-	{ "m25p32",  INFO(0x202016,  0,  64 * 1024,  64, 0) },
-	{ "m25p64",  INFO(0x202017,  0,  64 * 1024, 128, 0) },
-	{ "m25p128", INFO(0x202018,  0, 256 * 1024,  64, 0) },
-	{ "n25q032", INFO(0x20ba16,  0,  64 * 1024,  64, 0) },
-
-	{ "m25p05-nonjedec",  INFO(0, 0,  32 * 1024,   2, 0) },
-	{ "m25p10-nonjedec",  INFO(0, 0,  32 * 1024,   4, 0) },
-	{ "m25p20-nonjedec",  INFO(0, 0,  64 * 1024,   4, 0) },
-	{ "m25p40-nonjedec",  INFO(0, 0,  64 * 1024,   8, 0) },
-	{ "m25p80-nonjedec",  INFO(0, 0,  64 * 1024,  16, 0) },
-	{ "m25p16-nonjedec",  INFO(0, 0,  64 * 1024,  32, 0) },
-	{ "m25p32-nonjedec",  INFO(0, 0,  64 * 1024,  64, 0) },
-	{ "m25p64-nonjedec",  INFO(0, 0,  64 * 1024, 128, 0) },
-	{ "m25p128-nonjedec", INFO(0, 0, 256 * 1024,  64, 0) },
-
-	{ "m45pe10", INFO(0x204011,  0, 64 * 1024,    2, 0) },
-	{ "m45pe80", INFO(0x204014,  0, 64 * 1024,   16, 0) },
-	{ "m45pe16", INFO(0x204015,  0, 64 * 1024,   32, 0) },
-
-	{ "m25pe20", INFO(0x208012,  0, 64 * 1024,  4,       0) },
-	{ "m25pe80", INFO(0x208014,  0, 64 * 1024, 16,       0) },
-	{ "m25pe16", INFO(0x208015,  0, 64 * 1024, 32, SECT_4K) },
-
-	{ "m25px16",    INFO(0x207115,  0, 64 * 1024, 32, SECT_4K) },
-	{ "m25px32",    INFO(0x207116,  0, 64 * 1024, 64, SECT_4K) },
-	{ "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) },
-
-	/* Winbond -- w25x "blocks" are 64K, "sectors" are 4KiB */
-	{ "w25x10", INFO(0xef3011, 0, 64 * 1024,  2,  SECT_4K) },
-	{ "w25x20", INFO(0xef3012, 0, 64 * 1024,  4,  SECT_4K) },
-	{ "w25x40", INFO(0xef3013, 0, 64 * 1024,  8,  SECT_4K) },
-	{ "w25x80", INFO(0xef3014, 0, 64 * 1024,  16, SECT_4K) },
-	{ "w25x16", INFO(0xef3015, 0, 64 * 1024,  32, SECT_4K) },
-	{ "w25x32", INFO(0xef3016, 0, 64 * 1024,  64, SECT_4K) },
-	{ "w25q32", INFO(0xef4016, 0, 64 * 1024,  64, SECT_4K) },
-	{ "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) },
-	{ "w25q256", INFO(0xef4019, 0, 64 * 1024, 512, SECT_4K) },
-
-	/* Catalyst / On Semiconductor -- non-JEDEC */
-	{ "cat25c11", CAT25_INFO(  16, 8, 16, 1, M25P_NO_ERASE | M25P_NO_FR) },
-	{ "cat25c03", CAT25_INFO(  32, 8, 16, 2, M25P_NO_ERASE | M25P_NO_FR) },
-	{ "cat25c09", CAT25_INFO( 128, 8, 32, 2, M25P_NO_ERASE | M25P_NO_FR) },
-	{ "cat25c17", CAT25_INFO( 256, 8, 32, 2, M25P_NO_ERASE | M25P_NO_FR) },
-	{ "cat25128", CAT25_INFO(2048, 8, 64, 2, M25P_NO_ERASE | M25P_NO_FR) },
-	{ },
-};
-MODULE_DEVICE_TABLE(spi, m25p_ids);
-
-static const struct spi_device_id *jedec_probe(struct spi_device *spi)
-{
-	int			tmp;
-	u8			code = OPCODE_RDID;
-	u8			id[5];
-	u32			jedec;
-	u16                     ext_jedec;
-	struct flash_info	*info;
+		return ret;
 
-	/* JEDEC also defines an optional "extended device information"
-	 * string for after vendor-specific data, after the three bytes
-	 * we use here.  Supporting some chips might require using it.
-	 */
-	tmp = spi_write_then_read(spi, &code, 1, id, 5);
-	if (tmp < 0) {
-		pr_debug("%s: error %d reading JEDEC ID\n",
-				dev_name(&spi->dev), tmp);
-		return ERR_PTR(tmp);
-	}
-	jedec = id[0];
-	jedec = jedec << 8;
-	jedec |= id[1];
-	jedec = jedec << 8;
-	jedec |= id[2];
+	/* Set up command buffer. */
+	flash->command[0] = nor->erase_opcode;
+	m25p_addr2cmd(nor, offset, flash->command);
 
-	ext_jedec = id[3] << 8 | id[4];
+	spi_write(flash->spi, flash->command, m25p_cmdsz(nor));
 
-	for (tmp = 0; tmp < ARRAY_SIZE(m25p_ids) - 1; tmp++) {
-		info = (void *)m25p_ids[tmp].driver_data;
-		if (info->jedec_id == jedec) {
-			if (info->ext_id == 0 || info->ext_id == ext_jedec)
-				return &m25p_ids[tmp];
-		}
-	}
-	dev_err(&spi->dev, "unrecognized JEDEC id %06x\n", jedec);
-	return ERR_PTR(-ENODEV);
+	return 0;
 }
 
-
 /*
  * board specific setup should have ensured the SPI clock used here
  * matches what the READ command supports, at least until this driver
@@ -1096,231 +191,45 @@ static const struct spi_device_id *jedec_probe(struct spi_device *spi)
  */
 static int m25p_probe(struct spi_device *spi)
 {
-	const struct spi_device_id	*id = spi_get_device_id(spi);
-	struct flash_platform_data	*data;
-	struct m25p			*flash;
-	struct flash_info		*info;
-	unsigned			i;
 	struct mtd_part_parser_data	ppdata;
-	struct device_node *np = spi->dev.of_node;
+	struct flash_platform_data	*data;
+	struct m25p *flash;
+	struct spi_nor *nor;
+	enum read_mode mode = SPI_NOR_NORMAL;
 	int 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(&spi->dev);
-	if (data && data->type) {
-		const struct spi_device_id *plat_id;
-
-		for (i = 0; i < ARRAY_SIZE(m25p_ids) - 1; i++) {
-			plat_id = &m25p_ids[i];
-			if (strcmp(data->type, plat_id->name))
-				continue;
-			break;
-		}
-
-		if (i < ARRAY_SIZE(m25p_ids) - 1)
-			id = plat_id;
-		else
-			dev_warn(&spi->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(spi);
-		if (IS_ERR(jid)) {
-			return PTR_ERR(jid);
-		} else if (jid != id) {
-			/*
-			 * JEDEC knows better, so overwrite platform ID. We
-			 * can't trust partitions any longer, but we'll let
-			 * mtd apply them anyway, since some partitions may be
-			 * marked read-only, and we don't want to lose that
-			 * information, even if it's not 100% accurate.
-			 */
-			dev_warn(&spi->dev, "found %s, expected %s\n",
-				 jid->name, id->name);
-			id = jid;
-			info = (void *)jid->driver_data;
-		}
-	}
-
 	flash = devm_kzalloc(&spi->dev, sizeof(*flash), GFP_KERNEL);
 	if (!flash)
 		return -ENOMEM;
 
-	flash->command = devm_kzalloc(&spi->dev, MAX_CMD_SIZE, GFP_KERNEL);
-	if (!flash->command)
-		return -ENOMEM;
-
-	flash->spi = spi;
-	mutex_init(&flash->lock);
-	spi_set_drvdata(spi, flash);
-
-	/*
-	 * Atmel, SST and Intel/Numonyx serial flash tend to power
-	 * up with the software protection bits set
-	 */
+	nor = &flash->spi_nor;
 
-	if (JEDEC_MFR(info->jedec_id) == CFI_MFR_ATMEL ||
-	    JEDEC_MFR(info->jedec_id) == CFI_MFR_INTEL ||
-	    JEDEC_MFR(info->jedec_id) == CFI_MFR_SST) {
-		write_enable(flash);
-		write_sr(flash, 0);
-	}
-
-	if (data && data->name)
-		flash->mtd.name = data->name;
-	else
-		flash->mtd.name = dev_name(&spi->dev);
-
-	flash->mtd.type = MTD_NORFLASH;
-	flash->mtd.writesize = 1;
-	flash->mtd.flags = MTD_CAP_NORFLASH;
-	flash->mtd.size = info->sector_size * info->n_sectors;
-	flash->mtd._erase = m25p80_erase;
-	flash->mtd._read = m25p80_read;
-
-	/* flash protection support for STmicro chips */
-	if (JEDEC_MFR(info->jedec_id) == CFI_MFR_ST) {
-		flash->mtd._lock = m25p80_lock;
-		flash->mtd._unlock = m25p80_unlock;
-	}
+	/* install the hooks */
+	nor->read = m25p80_read;
+	nor->write = m25p80_write;
+	nor->erase = m25p80_erase;
+	nor->write_reg = m25p80_write_reg;
+	nor->read_reg = m25p80_read_reg;
 
-	/* sst flash chips use AAI word program */
-	if (info->flags & SST_WRITE)
-		flash->mtd._write = sst_write;
-	else
-		flash->mtd._write = m25p80_write;
+	nor->dev = &spi->dev;
+	nor->mtd = &flash->mtd;
+	nor->priv = flash;
 
-	/* prefer "small sector" erase if possible */
-	if (info->flags & SECT_4K) {
-		flash->erase_opcode = OPCODE_BE_4K;
-		flash->mtd.erasesize = 4096;
-	} else if (info->flags & SECT_4K_PMC) {
-		flash->erase_opcode = OPCODE_BE_4K_PMC;
-		flash->mtd.erasesize = 4096;
-	} else {
-		flash->erase_opcode = OPCODE_SE;
-		flash->mtd.erasesize = info->sector_size;
-	}
+	spi_set_drvdata(spi, flash);
+	flash->mtd.priv = nor;
+	flash->spi = spi;
 
-	if (info->flags & M25P_NO_ERASE)
-		flash->mtd.flags |= MTD_NO_ERASE;
+	if (spi->mode & SPI_RX_QUAD)
+		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 (ret)
+		return ret;
 
+	data = dev_get_platdata(&spi->dev);
 	ppdata.of_node = spi->dev.of_node;
-	flash->mtd.dev.parent = &spi->dev;
-	flash->page_size = info->page_size;
-	flash->mtd.writebufsize = flash->page_size;
-
-	if (np) {
-		/* If we were instantiated by DT, use it */
-		if (of_property_read_bool(np, "m25p,fast-read"))
-			flash->flash_read = M25P80_FAST;
-		else
-			flash->flash_read = M25P80_NORMAL;
-	} else {
-		/* If we weren't instantiated by DT, default to fast-read */
-		flash->flash_read = M25P80_FAST;
-	}
-
-	/* Some devices cannot do fast-read, no matter what DT tells us */
-	if (info->flags & M25P_NO_FR)
-		flash->flash_read = M25P80_NORMAL;
-
-	/* Quad/Dual-read mode takes precedence over fast/normal */
-	if (spi->mode & SPI_RX_QUAD && info->flags & M25P80_QUAD_READ) {
-		ret = set_quad_mode(flash, info->jedec_id);
-		if (ret) {
-			dev_err(&flash->spi->dev, "quad mode not supported\n");
-			return ret;
-		}
-		flash->flash_read = M25P80_QUAD;
-	} else if (spi->mode & SPI_RX_DUAL && info->flags & M25P80_DUAL_READ) {
-		flash->flash_read = M25P80_DUAL;
-	}
 
-	/* Default commands */
-	switch (flash->flash_read) {
-	case M25P80_QUAD:
-		flash->read_opcode = OPCODE_QUAD_READ;
-		break;
-	case M25P80_DUAL:
-		flash->read_opcode = OPCODE_DUAL_READ;
-		break;
-	case M25P80_FAST:
-		flash->read_opcode = OPCODE_FAST_READ;
-		break;
-	case M25P80_NORMAL:
-		flash->read_opcode = OPCODE_NORM_READ;
-		break;
-	default:
-		dev_err(&flash->spi->dev, "No Read opcode defined\n");
-		return -EINVAL;
-	}
-
-	flash->program_opcode = OPCODE_PP;
-
-	if (info->addr_width)
-		flash->addr_width = info->addr_width;
-	else if (flash->mtd.size > 0x1000000) {
-		/* enable 4-byte addressing if the device exceeds 16MiB */
-		flash->addr_width = 4;
-		if (JEDEC_MFR(info->jedec_id) == CFI_MFR_AMD) {
-			/* Dedicated 4-byte command set */
-			switch (flash->flash_read) {
-			case M25P80_QUAD:
-				flash->read_opcode = OPCODE_QUAD_READ_4B;
-				break;
-			case M25P80_DUAL:
-				flash->read_opcode = OPCODE_DUAL_READ_4B;
-				break;
-			case M25P80_FAST:
-				flash->read_opcode = OPCODE_FAST_READ_4B;
-				break;
-			case M25P80_NORMAL:
-				flash->read_opcode = OPCODE_NORM_READ_4B;
-				break;
-			}
-			flash->program_opcode = OPCODE_PP_4B;
-			/* No small sector erase for 4-byte command set */
-			flash->erase_opcode = OPCODE_SE_4B;
-			flash->mtd.erasesize = info->sector_size;
-		} else
-			set_4byte(flash, info->jedec_id, 1);
-	} else {
-		flash->addr_width = 3;
-	}
-
-	dev_info(&spi->dev, "%s (%lld Kbytes)\n", id->name,
-			(long long)flash->mtd.size >> 10);
-
-	pr_debug("mtd .name = %s, .size = 0x%llx (%lldMiB) "
-			".erasesize = 0x%.8x (%uKiB) .numeraseregions = %d\n",
-		flash->mtd.name,
-		(long long)flash->mtd.size, (long long)(flash->mtd.size >> 20),
-		flash->mtd.erasesize, flash->mtd.erasesize / 1024,
-		flash->mtd.numeraseregions);
-
-	if (flash->mtd.numeraseregions)
-		for (i = 0; i < flash->mtd.numeraseregions; i++)
-			pr_debug("mtd.eraseregions[%d] = { .offset = 0x%llx, "
-				".erasesize = 0x%.8x (%uKiB), "
-				".numblocks = %d }\n",
-				i, (long long)flash->mtd.eraseregions[i].offset,
-				flash->mtd.eraseregions[i].erasesize,
-				flash->mtd.eraseregions[i].erasesize / 1024,
-				flash->mtd.eraseregions[i].numblocks);
-
-
-	/* partitions should match sector boundaries; and it may be good to
-	 * use readonly partitions for writeprotected sectors (BP2..BP0).
-	 */
 	return mtd_device_parse_register(&flash->mtd, NULL, &ppdata,
 			data ? data->parts : NULL,
 			data ? data->nr_parts : 0);
@@ -1341,7 +250,7 @@ static struct spi_driver m25p80_driver = {
 		.name	= "m25p80",
 		.owner	= THIS_MODULE,
 	},
-	.id_table	= m25p_ids,
+	.id_table	= spi_nor_ids,
 	.probe	= m25p_probe,
 	.remove	= m25p_remove,
 
diff --git a/drivers/mtd/devices/serial_flash_cmds.h b/drivers/mtd/devices/serial_flash_cmds.h
index 4f0c2c7c898..f59a125295d 100644
--- a/drivers/mtd/devices/serial_flash_cmds.h
+++ b/drivers/mtd/devices/serial_flash_cmds.h
@@ -13,43 +13,23 @@
 #define _MTD_SERIAL_FLASH_CMDS_H
 
 /* Generic Flash Commands/OPCODEs */
-#define FLASH_CMD_WREN		0x06
-#define FLASH_CMD_WRDI		0x04
-#define FLASH_CMD_RDID		0x9f
-#define FLASH_CMD_RDSR		0x05
-#define FLASH_CMD_RDSR2		0x35
-#define FLASH_CMD_WRSR		0x01
-#define FLASH_CMD_SE_4K		0x20
-#define FLASH_CMD_SE_32K	0x52
-#define FLASH_CMD_SE		0xd8
-#define FLASH_CMD_CHIPERASE	0xc7
-#define FLASH_CMD_WRVCR		0x81
-#define FLASH_CMD_RDVCR		0x85
+#define SPINOR_OP_RDSR2		0x35
+#define SPINOR_OP_WRVCR		0x81
+#define SPINOR_OP_RDVCR		0x85
 
 /* JEDEC Standard - Serial Flash Discoverable Parmeters (SFDP) Commands */
-#define FLASH_CMD_READ		0x03	/* READ */
-#define FLASH_CMD_READ_FAST	0x0b	/* FAST READ */
-#define FLASH_CMD_READ_1_1_2	0x3b	/* DUAL OUTPUT READ */
-#define FLASH_CMD_READ_1_2_2	0xbb	/* DUAL I/O READ */
-#define FLASH_CMD_READ_1_1_4	0x6b	/* QUAD OUTPUT READ */
-#define FLASH_CMD_READ_1_4_4	0xeb	/* QUAD I/O READ */
+#define SPINOR_OP_READ_1_2_2	0xbb	/* DUAL I/O READ */
+#define SPINOR_OP_READ_1_4_4	0xeb	/* QUAD I/O READ */
 
-#define FLASH_CMD_WRITE		0x02	/* PAGE PROGRAM */
-#define FLASH_CMD_WRITE_1_1_2	0xa2	/* DUAL INPUT PROGRAM */
-#define FLASH_CMD_WRITE_1_2_2	0xd2	/* DUAL INPUT EXT PROGRAM */
-#define FLASH_CMD_WRITE_1_1_4	0x32	/* QUAD INPUT PROGRAM */
-#define FLASH_CMD_WRITE_1_4_4	0x12	/* QUAD INPUT EXT PROGRAM */
-
-#define FLASH_CMD_EN4B_ADDR	0xb7	/* Enter 4-byte address mode */
-#define FLASH_CMD_EX4B_ADDR	0xe9	/* Exit 4-byte address mode */
+#define SPINOR_OP_WRITE		0x02	/* PAGE PROGRAM */
+#define SPINOR_OP_WRITE_1_1_2	0xa2	/* DUAL INPUT PROGRAM */
+#define SPINOR_OP_WRITE_1_2_2	0xd2	/* DUAL INPUT EXT PROGRAM */
+#define SPINOR_OP_WRITE_1_1_4	0x32	/* QUAD INPUT PROGRAM */
+#define SPINOR_OP_WRITE_1_4_4	0x12	/* QUAD INPUT EXT PROGRAM */
 
 /* READ commands with 32-bit addressing */
-#define FLASH_CMD_READ4		0x13
-#define FLASH_CMD_READ4_FAST	0x0c
-#define FLASH_CMD_READ4_1_1_2	0x3c
-#define FLASH_CMD_READ4_1_2_2	0xbc
-#define FLASH_CMD_READ4_1_1_4	0x6c
-#define FLASH_CMD_READ4_1_4_4	0xec
+#define SPINOR_OP_READ4_1_2_2	0xbc
+#define SPINOR_OP_READ4_1_4_4	0xec
 
 /* Configuration flags */
 #define FLASH_FLAG_SINGLE	0x000000ff
diff --git a/drivers/mtd/devices/slram.c b/drivers/mtd/devices/slram.c
index 5a5cd2ace4a..2fc4957cbe7 100644
--- a/drivers/mtd/devices/slram.c
+++ b/drivers/mtd/devices/slram.c
@@ -280,14 +280,11 @@ __setup("slram=", mtd_slram_setup);
 static int __init init_slram(void)
 {
 	char *devname;
-	int i;
 
 #ifndef MODULE
 	char *devstart;
 	char *devlength;
 
-	i = 0;
-
 	if (!map) {
 		E("slram: not enough parameters.\n");
 		return(-EINVAL);
@@ -314,6 +311,7 @@ static int __init init_slram(void)
 	}
 #else
 	int count;
+	int i;
 
 	for (count = 0; count < SLRAM_MAX_DEVICES_PARAMS && map[count];
 			count++) {
diff --git a/drivers/mtd/devices/st_spi_fsm.c b/drivers/mtd/devices/st_spi_fsm.c
index 1957d7c8e18..d252514d3e9 100644
--- a/drivers/mtd/devices/st_spi_fsm.c
+++ b/drivers/mtd/devices/st_spi_fsm.c
@@ -19,6 +19,7 @@
 #include <linux/mfd/syscon.h>
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/partitions.h>
+#include <linux/mtd/spi-nor.h>
 #include <linux/sched.h>
 #include <linux/delay.h>
 #include <linux/io.h>
@@ -201,44 +202,6 @@
 
 #define STFSM_MAX_WAIT_SEQ_MS  1000     /* FSM execution time */
 
-/* Flash Commands */
-#define FLASH_CMD_WREN         0x06
-#define FLASH_CMD_WRDI         0x04
-#define FLASH_CMD_RDID         0x9f
-#define FLASH_CMD_RDSR         0x05
-#define FLASH_CMD_RDSR2                0x35
-#define FLASH_CMD_WRSR         0x01
-#define FLASH_CMD_SE_4K                0x20
-#define FLASH_CMD_SE_32K       0x52
-#define FLASH_CMD_SE           0xd8
-#define FLASH_CMD_CHIPERASE    0xc7
-#define FLASH_CMD_WRVCR                0x81
-#define FLASH_CMD_RDVCR                0x85
-
-#define FLASH_CMD_READ         0x03    /* READ */
-#define FLASH_CMD_READ_FAST    0x0b    /* FAST READ */
-#define FLASH_CMD_READ_1_1_2   0x3b    /* DUAL OUTPUT READ */
-#define FLASH_CMD_READ_1_2_2   0xbb    /* DUAL I/O READ */
-#define FLASH_CMD_READ_1_1_4   0x6b    /* QUAD OUTPUT READ */
-#define FLASH_CMD_READ_1_4_4   0xeb    /* QUAD I/O READ */
-
-#define FLASH_CMD_WRITE                0x02    /* PAGE PROGRAM */
-#define FLASH_CMD_WRITE_1_1_2  0xa2    /* DUAL INPUT PROGRAM */
-#define FLASH_CMD_WRITE_1_2_2  0xd2    /* DUAL INPUT EXT PROGRAM */
-#define FLASH_CMD_WRITE_1_1_4  0x32    /* QUAD INPUT PROGRAM */
-#define FLASH_CMD_WRITE_1_4_4  0x12    /* QUAD INPUT EXT PROGRAM */
-
-#define FLASH_CMD_EN4B_ADDR    0xb7    /* Enter 4-byte address mode */
-#define FLASH_CMD_EX4B_ADDR    0xe9    /* Exit 4-byte address mode */
-
-/* READ commands with 32-bit addressing (N25Q256 and S25FLxxxS) */
-#define FLASH_CMD_READ4                0x13
-#define FLASH_CMD_READ4_FAST   0x0c
-#define FLASH_CMD_READ4_1_1_2  0x3c
-#define FLASH_CMD_READ4_1_2_2  0xbc
-#define FLASH_CMD_READ4_1_1_4  0x6c
-#define FLASH_CMD_READ4_1_4_4  0xec
-
 /* S25FLxxxS commands */
 #define S25FL_CMD_WRITE4_1_1_4 0x34
 #define S25FL_CMD_SE4          0xdc
@@ -246,7 +209,7 @@
 #define S25FL_CMD_DYBWR                0xe1
 #define S25FL_CMD_DYBRD                0xe0
 #define S25FL_CMD_WRITE4       0x12    /* Note, opcode clashes with
-					* 'FLASH_CMD_WRITE_1_4_4'
+					* 'SPINOR_OP_WRITE_1_4_4'
 					* as found on N25Qxxx devices! */
 
 /* Status register */
@@ -261,6 +224,12 @@
 #define S25FL_STATUS_E_ERR     0x20
 #define S25FL_STATUS_P_ERR     0x40
 
+#define N25Q_CMD_WRVCR         0x81
+#define N25Q_CMD_RDVCR         0x85
+#define N25Q_CMD_RDVECR        0x65
+#define N25Q_CMD_RDNVCR        0xb5
+#define N25Q_CMD_WRNVCR        0xb1
+
 #define FLASH_PAGESIZE         256			/* In Bytes    */
 #define FLASH_PAGESIZE_32      (FLASH_PAGESIZE / 4)	/* In uint32_t */
 #define FLASH_MAX_BUSY_WAIT    (300 * HZ)	/* Maximum 'CHIPERASE' time */
@@ -270,7 +239,6 @@
  */
 #define CFG_READ_TOGGLE_32BIT_ADDR     0x00000001
 #define CFG_WRITE_TOGGLE_32BIT_ADDR    0x00000002
-#define CFG_WRITE_EX_32BIT_ADDR_DELAY  0x00000004
 #define CFG_ERASESEC_TOGGLE_32BIT_ADDR 0x00000008
 #define CFG_S25FL_CHECK_ERROR_FLAGS    0x00000010
 
@@ -329,7 +297,7 @@ struct flash_info {
 	u32             jedec_id;
 	u16             ext_id;
 	/*
-	 * The size listed here is what works with FLASH_CMD_SE, which isn't
+	 * The size listed here is what works with SPINOR_OP_SE, which isn't
 	 * necessarily called a "sector" by the vendor.
 	 */
 	unsigned        sector_size;
@@ -369,17 +337,26 @@ static struct flash_info flash_types[] = {
 	{ "m25px32", 0x207116, 0,  64 * 1024,  64, M25PX_FLAG, 75, NULL },
 	{ "m25px64", 0x207117, 0,  64 * 1024, 128, M25PX_FLAG, 75, NULL },
 
+	/* Macronix MX25xxx
+	 *     - Support for 'FLASH_FLAG_WRITE_1_4_4' is omitted for devices
+	 *       where operating frequency must be reduced.
+	 */
 #define MX25_FLAG (FLASH_FLAG_READ_WRITE       |	\
 		   FLASH_FLAG_READ_FAST         |	\
 		   FLASH_FLAG_READ_1_1_2        |	\
 		   FLASH_FLAG_READ_1_2_2        |	\
 		   FLASH_FLAG_READ_1_1_4        |	\
-		   FLASH_FLAG_READ_1_4_4        |	\
 		   FLASH_FLAG_SE_4K             |	\
 		   FLASH_FLAG_SE_32K)
+	{ "mx25l3255e",  0xc29e16, 0, 64 * 1024, 64,
+	  (MX25_FLAG | FLASH_FLAG_WRITE_1_4_4), 86,
+	  stfsm_mx25_config},
 	{ "mx25l25635e", 0xc22019, 0, 64*1024, 512,
 	  (MX25_FLAG | FLASH_FLAG_32BIT_ADDR | FLASH_FLAG_RESET), 70,
 	  stfsm_mx25_config },
+	{ "mx25l25655e", 0xc22619, 0, 64*1024, 512,
+	  (MX25_FLAG | FLASH_FLAG_32BIT_ADDR | FLASH_FLAG_RESET), 70,
+	  stfsm_mx25_config},
 
 #define N25Q_FLAG (FLASH_FLAG_READ_WRITE       |	\
 		   FLASH_FLAG_READ_FAST         |	\
@@ -407,6 +384,8 @@ static struct flash_info flash_types[] = {
 			FLASH_FLAG_READ_1_4_4   |	\
 			FLASH_FLAG_WRITE_1_1_4  |	\
 			FLASH_FLAG_READ_FAST)
+	{ "s25fl032p",  0x010215, 0x4d00,  64 * 1024,  64, S25FLXXXP_FLAG, 80,
+	  stfsm_s25fl_config},
 	{ "s25fl129p0", 0x012018, 0x4d00, 256 * 1024,  64, S25FLXXXP_FLAG, 80,
 	  stfsm_s25fl_config },
 	{ "s25fl129p1", 0x012018, 0x4d01,  64 * 1024, 256, S25FLXXXP_FLAG, 80,
@@ -473,22 +452,22 @@ static struct flash_info flash_types[] = {
 
 /* Default READ configurations, in order of preference */
 static struct seq_rw_config default_read_configs[] = {
-	{FLASH_FLAG_READ_1_4_4, FLASH_CMD_READ_1_4_4,	0, 4, 4, 0x00, 2, 4},
-	{FLASH_FLAG_READ_1_1_4, FLASH_CMD_READ_1_1_4,	0, 1, 4, 0x00, 4, 0},
-	{FLASH_FLAG_READ_1_2_2, FLASH_CMD_READ_1_2_2,	0, 2, 2, 0x00, 4, 0},
-	{FLASH_FLAG_READ_1_1_2, FLASH_CMD_READ_1_1_2,	0, 1, 2, 0x00, 0, 8},
-	{FLASH_FLAG_READ_FAST,	FLASH_CMD_READ_FAST,	0, 1, 1, 0x00, 0, 8},
-	{FLASH_FLAG_READ_WRITE, FLASH_CMD_READ,		0, 1, 1, 0x00, 0, 0},
+	{FLASH_FLAG_READ_1_4_4, SPINOR_OP_READ_1_4_4,	0, 4, 4, 0x00, 2, 4},
+	{FLASH_FLAG_READ_1_1_4, SPINOR_OP_READ_1_1_4,	0, 1, 4, 0x00, 4, 0},
+	{FLASH_FLAG_READ_1_2_2, SPINOR_OP_READ_1_2_2,	0, 2, 2, 0x00, 4, 0},
+	{FLASH_FLAG_READ_1_1_2, SPINOR_OP_READ_1_1_2,	0, 1, 2, 0x00, 0, 8},
+	{FLASH_FLAG_READ_FAST,	SPINOR_OP_READ_FAST,	0, 1, 1, 0x00, 0, 8},
+	{FLASH_FLAG_READ_WRITE, SPINOR_OP_READ,		0, 1, 1, 0x00, 0, 0},
 	{0x00,			0,			0, 0, 0, 0x00, 0, 0},
 };
 
 /* Default WRITE configurations */
 static struct seq_rw_config default_write_configs[] = {
-	{FLASH_FLAG_WRITE_1_4_4, FLASH_CMD_WRITE_1_4_4, 1, 4, 4, 0x00, 0, 0},
-	{FLASH_FLAG_WRITE_1_1_4, FLASH_CMD_WRITE_1_1_4, 1, 1, 4, 0x00, 0, 0},
-	{FLASH_FLAG_WRITE_1_2_2, FLASH_CMD_WRITE_1_2_2, 1, 2, 2, 0x00, 0, 0},
-	{FLASH_FLAG_WRITE_1_1_2, FLASH_CMD_WRITE_1_1_2, 1, 1, 2, 0x00, 0, 0},
-	{FLASH_FLAG_READ_WRITE,  FLASH_CMD_WRITE,       1, 1, 1, 0x00, 0, 0},
+	{FLASH_FLAG_WRITE_1_4_4, SPINOR_OP_WRITE_1_4_4, 1, 4, 4, 0x00, 0, 0},
+	{FLASH_FLAG_WRITE_1_1_4, SPINOR_OP_WRITE_1_1_4, 1, 1, 4, 0x00, 0, 0},
+	{FLASH_FLAG_WRITE_1_2_2, SPINOR_OP_WRITE_1_2_2, 1, 2, 2, 0x00, 0, 0},
+	{FLASH_FLAG_WRITE_1_1_2, SPINOR_OP_WRITE_1_1_2, 1, 1, 2, 0x00, 0, 0},
+	{FLASH_FLAG_READ_WRITE,  SPINOR_OP_WRITE,       1, 1, 1, 0x00, 0, 0},
 	{0x00,			 0,			0, 0, 0, 0x00, 0, 0},
 };
 
@@ -511,12 +490,12 @@ static struct seq_rw_config default_write_configs[] = {
  * cycles.
  */
 static struct seq_rw_config n25q_read3_configs[] = {
-	{FLASH_FLAG_READ_1_4_4, FLASH_CMD_READ_1_4_4,	0, 4, 4, 0x00, 0, 8},
-	{FLASH_FLAG_READ_1_1_4, FLASH_CMD_READ_1_1_4,	0, 1, 4, 0x00, 0, 8},
-	{FLASH_FLAG_READ_1_2_2, FLASH_CMD_READ_1_2_2,	0, 2, 2, 0x00, 0, 8},
-	{FLASH_FLAG_READ_1_1_2, FLASH_CMD_READ_1_1_2,	0, 1, 2, 0x00, 0, 8},
-	{FLASH_FLAG_READ_FAST,	FLASH_CMD_READ_FAST,	0, 1, 1, 0x00, 0, 8},
-	{FLASH_FLAG_READ_WRITE, FLASH_CMD_READ,	        0, 1, 1, 0x00, 0, 0},
+	{FLASH_FLAG_READ_1_4_4, SPINOR_OP_READ_1_4_4,	0, 4, 4, 0x00, 0, 8},
+	{FLASH_FLAG_READ_1_1_4, SPINOR_OP_READ_1_1_4,	0, 1, 4, 0x00, 0, 8},
+	{FLASH_FLAG_READ_1_2_2, SPINOR_OP_READ_1_2_2,	0, 2, 2, 0x00, 0, 8},
+	{FLASH_FLAG_READ_1_1_2, SPINOR_OP_READ_1_1_2,	0, 1, 2, 0x00, 0, 8},
+	{FLASH_FLAG_READ_FAST,	SPINOR_OP_READ_FAST,	0, 1, 1, 0x00, 0, 8},
+	{FLASH_FLAG_READ_WRITE, SPINOR_OP_READ,	        0, 1, 1, 0x00, 0, 0},
 	{0x00,			0,			0, 0, 0, 0x00, 0, 0},
 };
 
@@ -526,12 +505,12 @@ static struct seq_rw_config n25q_read3_configs[] = {
  *	- 'FAST' variants configured for 8 dummy cycles (see note above.)
  */
 static struct seq_rw_config n25q_read4_configs[] = {
-	{FLASH_FLAG_READ_1_4_4, FLASH_CMD_READ4_1_4_4,	0, 4, 4, 0x00, 0, 8},
-	{FLASH_FLAG_READ_1_1_4, FLASH_CMD_READ4_1_1_4,	0, 1, 4, 0x00, 0, 8},
-	{FLASH_FLAG_READ_1_2_2, FLASH_CMD_READ4_1_2_2,	0, 2, 2, 0x00, 0, 8},
-	{FLASH_FLAG_READ_1_1_2, FLASH_CMD_READ4_1_1_2,	0, 1, 2, 0x00, 0, 8},
-	{FLASH_FLAG_READ_FAST,	FLASH_CMD_READ4_FAST,	0, 1, 1, 0x00, 0, 8},
-	{FLASH_FLAG_READ_WRITE, FLASH_CMD_READ4,	0, 1, 1, 0x00, 0, 0},
+	{FLASH_FLAG_READ_1_4_4, SPINOR_OP_READ4_1_4_4,	0, 4, 4, 0x00, 0, 8},
+	{FLASH_FLAG_READ_1_1_4, SPINOR_OP_READ4_1_1_4,	0, 1, 4, 0x00, 0, 8},
+	{FLASH_FLAG_READ_1_2_2, SPINOR_OP_READ4_1_2_2,	0, 2, 2, 0x00, 0, 8},
+	{FLASH_FLAG_READ_1_1_2, SPINOR_OP_READ4_1_1_2,	0, 1, 2, 0x00, 0, 8},
+	{FLASH_FLAG_READ_FAST,	SPINOR_OP_READ4_FAST,	0, 1, 1, 0x00, 0, 8},
+	{FLASH_FLAG_READ_WRITE, SPINOR_OP_READ4,	0, 1, 1, 0x00, 0, 0},
 	{0x00,			0,			0, 0, 0, 0x00, 0, 0},
 };
 
@@ -544,7 +523,7 @@ static int stfsm_mx25_en_32bit_addr_seq(struct stfsm_seq *seq)
 {
 	seq->seq_opc[0] = (SEQ_OPC_PADS_1 |
 			   SEQ_OPC_CYCLES(8) |
-			   SEQ_OPC_OPCODE(FLASH_CMD_EN4B_ADDR) |
+			   SEQ_OPC_OPCODE(SPINOR_OP_EN4B) |
 			   SEQ_OPC_CSDEASSERT);
 
 	seq->seq[0] = STFSM_INST_CMD1;
@@ -572,12 +551,12 @@ static int stfsm_mx25_en_32bit_addr_seq(struct stfsm_seq *seq)
  * entering a state that is incompatible with the SPIBoot Controller.
  */
 static struct seq_rw_config stfsm_s25fl_read4_configs[] = {
-	{FLASH_FLAG_READ_1_4_4,  FLASH_CMD_READ4_1_4_4,  0, 4, 4, 0x00, 2, 4},
-	{FLASH_FLAG_READ_1_1_4,  FLASH_CMD_READ4_1_1_4,  0, 1, 4, 0x00, 0, 8},
-	{FLASH_FLAG_READ_1_2_2,  FLASH_CMD_READ4_1_2_2,  0, 2, 2, 0x00, 4, 0},
-	{FLASH_FLAG_READ_1_1_2,  FLASH_CMD_READ4_1_1_2,  0, 1, 2, 0x00, 0, 8},
-	{FLASH_FLAG_READ_FAST,   FLASH_CMD_READ4_FAST,   0, 1, 1, 0x00, 0, 8},
-	{FLASH_FLAG_READ_WRITE,  FLASH_CMD_READ4,        0, 1, 1, 0x00, 0, 0},
+	{FLASH_FLAG_READ_1_4_4,  SPINOR_OP_READ4_1_4_4,  0, 4, 4, 0x00, 2, 4},
+	{FLASH_FLAG_READ_1_1_4,  SPINOR_OP_READ4_1_1_4,  0, 1, 4, 0x00, 0, 8},
+	{FLASH_FLAG_READ_1_2_2,  SPINOR_OP_READ4_1_2_2,  0, 2, 2, 0x00, 4, 0},
+	{FLASH_FLAG_READ_1_1_2,  SPINOR_OP_READ4_1_1_2,  0, 1, 2, 0x00, 0, 8},
+	{FLASH_FLAG_READ_FAST,   SPINOR_OP_READ4_FAST,   0, 1, 1, 0x00, 0, 8},
+	{FLASH_FLAG_READ_WRITE,  SPINOR_OP_READ4,        0, 1, 1, 0x00, 0, 0},
 	{0x00,                   0,                      0, 0, 0, 0x00, 0, 0},
 };
 
@@ -590,13 +569,13 @@ static struct seq_rw_config stfsm_s25fl_write4_configs[] = {
 /*
  * [W25Qxxx] Configuration
  */
-#define W25Q_STATUS_QE			(0x1 << 9)
+#define W25Q_STATUS_QE			(0x1 << 1)
 
 static struct stfsm_seq stfsm_seq_read_jedec = {
 	.data_size = TRANSFER_SIZE(8),
 	.seq_opc[0] = (SEQ_OPC_PADS_1 |
 		       SEQ_OPC_CYCLES(8) |
-		       SEQ_OPC_OPCODE(FLASH_CMD_RDID)),
+		       SEQ_OPC_OPCODE(SPINOR_OP_RDID)),
 	.seq = {
 		STFSM_INST_CMD1,
 		STFSM_INST_DATA_READ,
@@ -612,7 +591,7 @@ static struct stfsm_seq stfsm_seq_read_status_fifo = {
 	.data_size = TRANSFER_SIZE(4),
 	.seq_opc[0] = (SEQ_OPC_PADS_1 |
 		       SEQ_OPC_CYCLES(8) |
-		       SEQ_OPC_OPCODE(FLASH_CMD_RDSR)),
+		       SEQ_OPC_OPCODE(SPINOR_OP_RDSR)),
 	.seq = {
 		STFSM_INST_CMD1,
 		STFSM_INST_DATA_READ,
@@ -628,10 +607,10 @@ static struct stfsm_seq stfsm_seq_erase_sector = {
 	/* 'addr_cfg' configured during initialisation */
 	.seq_opc = {
 		(SEQ_OPC_PADS_1 | SEQ_OPC_CYCLES(8) |
-		 SEQ_OPC_OPCODE(FLASH_CMD_WREN) | SEQ_OPC_CSDEASSERT),
+		 SEQ_OPC_OPCODE(SPINOR_OP_WREN) | SEQ_OPC_CSDEASSERT),
 
 		(SEQ_OPC_PADS_1 | SEQ_OPC_CYCLES(8) |
-		 SEQ_OPC_OPCODE(FLASH_CMD_SE)),
+		 SEQ_OPC_OPCODE(SPINOR_OP_SE)),
 	},
 	.seq = {
 		STFSM_INST_CMD1,
@@ -649,10 +628,10 @@ static struct stfsm_seq stfsm_seq_erase_sector = {
 static struct stfsm_seq stfsm_seq_erase_chip = {
 	.seq_opc = {
 		(SEQ_OPC_PADS_1 | SEQ_OPC_CYCLES(8) |
-		 SEQ_OPC_OPCODE(FLASH_CMD_WREN) | SEQ_OPC_CSDEASSERT),
+		 SEQ_OPC_OPCODE(SPINOR_OP_WREN) | SEQ_OPC_CSDEASSERT),
 
 		(SEQ_OPC_PADS_1 | SEQ_OPC_CYCLES(8) |
-		 SEQ_OPC_OPCODE(FLASH_CMD_CHIPERASE) | SEQ_OPC_CSDEASSERT),
+		 SEQ_OPC_OPCODE(SPINOR_OP_CHIP_ERASE) | SEQ_OPC_CSDEASSERT),
 	},
 	.seq = {
 		STFSM_INST_CMD1,
@@ -669,26 +648,9 @@ static struct stfsm_seq stfsm_seq_erase_chip = {
 
 static struct stfsm_seq stfsm_seq_write_status = {
 	.seq_opc[0] = (SEQ_OPC_PADS_1 | SEQ_OPC_CYCLES(8) |
-		       SEQ_OPC_OPCODE(FLASH_CMD_WREN) | SEQ_OPC_CSDEASSERT),
-	.seq_opc[1] = (SEQ_OPC_PADS_1 | SEQ_OPC_CYCLES(8) |
-		       SEQ_OPC_OPCODE(FLASH_CMD_WRSR)),
-	.seq = {
-		STFSM_INST_CMD1,
-		STFSM_INST_CMD2,
-		STFSM_INST_STA_WR1,
-		STFSM_INST_STOP,
-	},
-	.seq_cfg = (SEQ_CFG_PADS_1 |
-		    SEQ_CFG_READNOTWRITE |
-		    SEQ_CFG_CSDEASSERT |
-		    SEQ_CFG_STARTSEQ),
-};
-
-static struct stfsm_seq stfsm_seq_wrvcr = {
-	.seq_opc[0] = (SEQ_OPC_PADS_1 | SEQ_OPC_CYCLES(8) |
-		       SEQ_OPC_OPCODE(FLASH_CMD_WREN) | SEQ_OPC_CSDEASSERT),
+		       SEQ_OPC_OPCODE(SPINOR_OP_WREN) | SEQ_OPC_CSDEASSERT),
 	.seq_opc[1] = (SEQ_OPC_PADS_1 | SEQ_OPC_CYCLES(8) |
-		       SEQ_OPC_OPCODE(FLASH_CMD_WRVCR)),
+		       SEQ_OPC_OPCODE(SPINOR_OP_WRSR)),
 	.seq = {
 		STFSM_INST_CMD1,
 		STFSM_INST_CMD2,
@@ -704,9 +666,9 @@ static struct stfsm_seq stfsm_seq_wrvcr = {
 static int stfsm_n25q_en_32bit_addr_seq(struct stfsm_seq *seq)
 {
 	seq->seq_opc[0] = (SEQ_OPC_PADS_1 | SEQ_OPC_CYCLES(8) |
-			   SEQ_OPC_OPCODE(FLASH_CMD_EN4B_ADDR));
+			   SEQ_OPC_OPCODE(SPINOR_OP_EN4B));
 	seq->seq_opc[1] = (SEQ_OPC_PADS_1 | SEQ_OPC_CYCLES(8) |
-			   SEQ_OPC_OPCODE(FLASH_CMD_WREN) |
+			   SEQ_OPC_OPCODE(SPINOR_OP_WREN) |
 			   SEQ_OPC_CSDEASSERT);
 
 	seq->seq[0] = STFSM_INST_CMD2;
@@ -793,7 +755,7 @@ static void stfsm_read_fifo(struct stfsm *fsm, uint32_t *buf, uint32_t size)
 
 	dev_dbg(fsm->dev, "Reading %d bytes from FIFO\n", size);
 
-	BUG_ON((((uint32_t)buf) & 0x3) || (size & 0x3));
+	BUG_ON((((uintptr_t)buf) & 0x3) || (size & 0x3));
 
 	while (remaining) {
 		for (;;) {
@@ -817,7 +779,7 @@ static int stfsm_write_fifo(struct stfsm *fsm, const uint32_t *buf,
 
 	dev_dbg(fsm->dev, "writing %d bytes to FIFO\n", size);
 
-	BUG_ON((((uint32_t)buf) & 0x3) || (size & 0x3));
+	BUG_ON((((uintptr_t)buf) & 0x3) || (size & 0x3));
 
 	writesl(fsm->base + SPI_FAST_SEQ_DATA_REG, buf, words);
 
@@ -827,7 +789,7 @@ static int stfsm_write_fifo(struct stfsm *fsm, const uint32_t *buf,
 static int stfsm_enter_32bit_addr(struct stfsm *fsm, int enter)
 {
 	struct stfsm_seq *seq = &fsm->stfsm_seq_en_32bit_addr;
-	uint32_t cmd = enter ? FLASH_CMD_EN4B_ADDR : FLASH_CMD_EX4B_ADDR;
+	uint32_t cmd = enter ? SPINOR_OP_EN4B : SPINOR_OP_EX4B;
 
 	seq->seq_opc[0] = (SEQ_OPC_PADS_1 |
 			   SEQ_OPC_CYCLES(8) |
@@ -851,7 +813,7 @@ static uint8_t stfsm_wait_busy(struct stfsm *fsm)
 	/* Use RDRS1 */
 	seq->seq_opc[0] = (SEQ_OPC_PADS_1 |
 			   SEQ_OPC_CYCLES(8) |
-			   SEQ_OPC_OPCODE(FLASH_CMD_RDSR));
+			   SEQ_OPC_OPCODE(SPINOR_OP_RDSR));
 
 	/* Load read_status sequence */
 	stfsm_load_seq(fsm, seq);
@@ -889,60 +851,57 @@ static uint8_t stfsm_wait_busy(struct stfsm *fsm)
 }
 
 static int stfsm_read_status(struct stfsm *fsm, uint8_t cmd,
-			   uint8_t *status)
+			     uint8_t *data, int bytes)
 {
 	struct stfsm_seq *seq = &stfsm_seq_read_status_fifo;
 	uint32_t tmp;
+	uint8_t *t = (uint8_t *)&tmp;
+	int i;
 
-	dev_dbg(fsm->dev, "reading STA[%s]\n",
-		(cmd == FLASH_CMD_RDSR) ? "1" : "2");
+	dev_dbg(fsm->dev, "read 'status' register [0x%02x], %d byte(s)\n",
+		cmd, bytes);
 
-	seq->seq_opc[0] = (SEQ_OPC_PADS_1 |
-			   SEQ_OPC_CYCLES(8) |
+	BUG_ON(bytes != 1 && bytes != 2);
+
+	seq->seq_opc[0] = (SEQ_OPC_PADS_1 | SEQ_OPC_CYCLES(8) |
 			   SEQ_OPC_OPCODE(cmd)),
 
 	stfsm_load_seq(fsm, seq);
 
 	stfsm_read_fifo(fsm, &tmp, 4);
 
-	*status = (uint8_t)(tmp >> 24);
+	for (i = 0; i < bytes; i++)
+		data[i] = t[i];
 
 	stfsm_wait_seq(fsm);
 
 	return 0;
 }
 
-static int stfsm_write_status(struct stfsm *fsm, uint16_t status,
-			       int sta_bytes)
+static int stfsm_write_status(struct stfsm *fsm, uint8_t cmd,
+			    uint16_t data, int bytes, int wait_busy)
 {
 	struct stfsm_seq *seq = &stfsm_seq_write_status;
 
-	dev_dbg(fsm->dev, "writing STA[%s] 0x%04x\n",
-		(sta_bytes == 1) ? "1" : "1+2", status);
-
-	seq->status = (uint32_t)status | STA_PADS_1 | STA_CSDEASSERT;
-	seq->seq[2] = (sta_bytes == 1) ?
-		STFSM_INST_STA_WR1 : STFSM_INST_STA_WR1_2;
-
-	stfsm_load_seq(fsm, seq);
-
-	stfsm_wait_seq(fsm);
+	dev_dbg(fsm->dev,
+		"write 'status' register [0x%02x], %d byte(s), 0x%04x\n"
+		" %s wait-busy\n", cmd, bytes, data, wait_busy ? "with" : "no");
 
-	return 0;
-};
+	BUG_ON(bytes != 1 && bytes != 2);
 
-static int stfsm_wrvcr(struct stfsm *fsm, uint8_t data)
-{
-	struct stfsm_seq *seq = &stfsm_seq_wrvcr;
-
-	dev_dbg(fsm->dev, "writing VCR 0x%02x\n", data);
+	seq->seq_opc[1] = (SEQ_OPC_PADS_1 | SEQ_OPC_CYCLES(8) |
+			   SEQ_OPC_OPCODE(cmd));
 
-	seq->status = (STA_DATA_BYTE1(data) | STA_PADS_1 | STA_CSDEASSERT);
+	seq->status = (uint32_t)data | STA_PADS_1 | STA_CSDEASSERT;
+	seq->seq[2] = (bytes == 1) ? STFSM_INST_STA_WR1 : STFSM_INST_STA_WR1_2;
 
 	stfsm_load_seq(fsm, seq);
 
 	stfsm_wait_seq(fsm);
 
+	if (wait_busy)
+		stfsm_wait_busy(fsm);
+
 	return 0;
 }
 
@@ -1027,7 +986,7 @@ static void stfsm_prepare_rw_seq(struct stfsm *fsm,
 	if (cfg->write)
 		seq->seq_opc[i++] = (SEQ_OPC_PADS_1 |
 				     SEQ_OPC_CYCLES(8) |
-				     SEQ_OPC_OPCODE(FLASH_CMD_WREN) |
+				     SEQ_OPC_OPCODE(SPINOR_OP_WREN) |
 				     SEQ_OPC_CSDEASSERT);
 
 	/* Address configuration (24 or 32-bit addresses) */
@@ -1149,31 +1108,36 @@ static int stfsm_mx25_config(struct stfsm *fsm)
 		stfsm_mx25_en_32bit_addr_seq(&fsm->stfsm_seq_en_32bit_addr);
 
 		soc_reset = stfsm_can_handle_soc_reset(fsm);
-		if (soc_reset || !fsm->booted_from_spi) {
+		if (soc_reset || !fsm->booted_from_spi)
 			/* If we can handle SoC resets, we enable 32-bit address
 			 * mode pervasively */
 			stfsm_enter_32bit_addr(fsm, 1);
 
-		} else {
+		else
 			/* Else, enable/disable 32-bit addressing before/after
 			 * each operation */
 			fsm->configuration = (CFG_READ_TOGGLE_32BIT_ADDR |
 					      CFG_WRITE_TOGGLE_32BIT_ADDR |
 					      CFG_ERASESEC_TOGGLE_32BIT_ADDR);
-			/* It seems a small delay is required after exiting
-			 * 32-bit mode following a write operation.  The issue
-			 * is under investigation.
-			 */
-			fsm->configuration |= CFG_WRITE_EX_32BIT_ADDR_DELAY;
-		}
 	}
 
-	/* For QUAD mode, set 'QE' STATUS bit */
+	/* Check status of 'QE' bit, update if required. */
+	stfsm_read_status(fsm, SPINOR_OP_RDSR, &sta, 1);
 	data_pads = ((fsm->stfsm_seq_read.seq_cfg >> 16) & 0x3) + 1;
 	if (data_pads == 4) {
-		stfsm_read_status(fsm, FLASH_CMD_RDSR, &sta);
-		sta |= MX25_STATUS_QE;
-		stfsm_write_status(fsm, sta, 1);
+		if (!(sta & MX25_STATUS_QE)) {
+			/* Set 'QE' */
+			sta |= MX25_STATUS_QE;
+
+			stfsm_write_status(fsm, SPINOR_OP_WRSR, sta, 1, 1);
+		}
+	} else {
+		if (sta & MX25_STATUS_QE) {
+			/* Clear 'QE' */
+			sta &= ~MX25_STATUS_QE;
+
+			stfsm_write_status(fsm, SPINOR_OP_WRSR, sta, 1, 1);
+		}
 	}
 
 	return 0;
@@ -1239,7 +1203,7 @@ static int stfsm_n25q_config(struct stfsm *fsm)
 	 */
 	vcr = (N25Q_VCR_DUMMY_CYCLES(8) | N25Q_VCR_XIP_DISABLED |
 	       N25Q_VCR_WRAP_CONT);
-	stfsm_wrvcr(fsm, vcr);
+	stfsm_write_status(fsm, N25Q_CMD_WRVCR, vcr, 1, 0);
 
 	return 0;
 }
@@ -1297,7 +1261,7 @@ static void stfsm_s25fl_write_dyb(struct stfsm *fsm, uint32_t offs, uint8_t dby)
 {
 	struct stfsm_seq seq = {
 		.seq_opc[0] = (SEQ_OPC_PADS_1 | SEQ_OPC_CYCLES(8) |
-			       SEQ_OPC_OPCODE(FLASH_CMD_WREN) |
+			       SEQ_OPC_OPCODE(SPINOR_OP_WREN) |
 			       SEQ_OPC_CSDEASSERT),
 		.seq_opc[1] = (SEQ_OPC_PADS_1 | SEQ_OPC_CYCLES(8) |
 			       SEQ_OPC_OPCODE(S25FL_CMD_DYBWR)),
@@ -1337,7 +1301,7 @@ static int stfsm_s25fl_clear_status_reg(struct stfsm *fsm)
 			       SEQ_OPC_CSDEASSERT),
 		.seq_opc[1] = (SEQ_OPC_PADS_1 |
 			       SEQ_OPC_CYCLES(8) |
-			       SEQ_OPC_OPCODE(FLASH_CMD_WRDI) |
+			       SEQ_OPC_OPCODE(SPINOR_OP_WRDI) |
 			       SEQ_OPC_CSDEASSERT),
 		.seq = {
 			STFSM_INST_CMD1,
@@ -1367,6 +1331,7 @@ static int stfsm_s25fl_config(struct stfsm *fsm)
 	uint32_t offs;
 	uint16_t sta_wr;
 	uint8_t sr1, cr1, dyb;
+	int update_sr = 0;
 	int ret;
 
 	if (flags & FLASH_FLAG_32BIT_ADDR) {
@@ -1414,34 +1379,28 @@ static int stfsm_s25fl_config(struct stfsm *fsm)
 		}
 	}
 
-	/* Check status of 'QE' bit */
+	/* Check status of 'QE' bit, update if required. */
+	stfsm_read_status(fsm, SPINOR_OP_RDSR2, &cr1, 1);
 	data_pads = ((fsm->stfsm_seq_read.seq_cfg >> 16) & 0x3) + 1;
-	stfsm_read_status(fsm, FLASH_CMD_RDSR2, &cr1);
 	if (data_pads == 4) {
 		if (!(cr1 & STFSM_S25FL_CONFIG_QE)) {
 			/* Set 'QE' */
 			cr1 |= STFSM_S25FL_CONFIG_QE;
 
-			stfsm_read_status(fsm, FLASH_CMD_RDSR, &sr1);
-			sta_wr = ((uint16_t)cr1  << 8) | sr1;
-
-			stfsm_write_status(fsm, sta_wr, 2);
-
-			stfsm_wait_busy(fsm);
+			update_sr = 1;
 		}
 	} else {
-		if ((cr1 & STFSM_S25FL_CONFIG_QE)) {
+		if (cr1 & STFSM_S25FL_CONFIG_QE) {
 			/* Clear 'QE' */
 			cr1 &= ~STFSM_S25FL_CONFIG_QE;
 
-			stfsm_read_status(fsm, FLASH_CMD_RDSR, &sr1);
-			sta_wr = ((uint16_t)cr1  << 8) | sr1;
-
-			stfsm_write_status(fsm, sta_wr, 2);
-
-			stfsm_wait_busy(fsm);
+			update_sr = 1;
 		}
-
+	}
+	if (update_sr) {
+		stfsm_read_status(fsm, SPINOR_OP_RDSR, &sr1, 1);
+		sta_wr = ((uint16_t)cr1  << 8) | sr1;
+		stfsm_write_status(fsm, SPINOR_OP_WRSR, sta_wr, 2, 1);
 	}
 
 	/*
@@ -1456,27 +1415,36 @@ static int stfsm_s25fl_config(struct stfsm *fsm)
 static int stfsm_w25q_config(struct stfsm *fsm)
 {
 	uint32_t data_pads;
-	uint16_t sta_wr;
-	uint8_t sta1, sta2;
+	uint8_t sr1, sr2;
+	uint16_t sr_wr;
+	int update_sr = 0;
 	int ret;
 
 	ret = stfsm_prepare_rwe_seqs_default(fsm);
 	if (ret)
 		return ret;
 
-	/* If using QUAD mode, set QE STATUS bit */
+	/* Check status of 'QE' bit, update if required. */
+	stfsm_read_status(fsm, SPINOR_OP_RDSR2, &sr2, 1);
 	data_pads = ((fsm->stfsm_seq_read.seq_cfg >> 16) & 0x3) + 1;
 	if (data_pads == 4) {
-		stfsm_read_status(fsm, FLASH_CMD_RDSR, &sta1);
-		stfsm_read_status(fsm, FLASH_CMD_RDSR2, &sta2);
-
-		sta_wr = ((uint16_t)sta2 << 8) | sta1;
-
-		sta_wr |= W25Q_STATUS_QE;
-
-		stfsm_write_status(fsm, sta_wr, 2);
-
-		stfsm_wait_busy(fsm);
+		if (!(sr2 & W25Q_STATUS_QE)) {
+			/* Set 'QE' */
+			sr2 |= W25Q_STATUS_QE;
+			update_sr = 1;
+		}
+	} else {
+		if (sr2 & W25Q_STATUS_QE) {
+			/* Clear 'QE' */
+			sr2 &= ~W25Q_STATUS_QE;
+			update_sr = 1;
+		}
+	}
+	if (update_sr) {
+		/* Write status register */
+		stfsm_read_status(fsm, SPINOR_OP_RDSR, &sr1, 1);
+		sr_wr = ((uint16_t)sr2 << 8) | sr1;
+		stfsm_write_status(fsm, SPINOR_OP_WRSR, sr_wr, 2, 1);
 	}
 
 	return 0;
@@ -1506,7 +1474,7 @@ static int stfsm_read(struct stfsm *fsm, uint8_t *buf, uint32_t size,
 	read_mask = (data_pads << 2) - 1;
 
 	/* Handle non-aligned buf */
-	p = ((uint32_t)buf & 0x3) ? (uint8_t *)page_buf : buf;
+	p = ((uintptr_t)buf & 0x3) ? (uint8_t *)page_buf : buf;
 
 	/* Handle non-aligned size */
 	size_ub = (size + read_mask) & ~read_mask;
@@ -1528,7 +1496,7 @@ static int stfsm_read(struct stfsm *fsm, uint8_t *buf, uint32_t size,
 	}
 
 	/* Handle non-aligned buf */
-	if ((uint32_t)buf & 0x3)
+	if ((uintptr_t)buf & 0x3)
 		memcpy(buf, page_buf, size);
 
 	/* Wait for sequence to finish */
@@ -1570,7 +1538,7 @@ static int stfsm_write(struct stfsm *fsm, const uint8_t *buf,
 	write_mask = (data_pads << 2) - 1;
 
 	/* Handle non-aligned buf */
-	if ((uint32_t)buf & 0x3) {
+	if ((uintptr_t)buf & 0x3) {
 		memcpy(page_buf, buf, size);
 		p = (uint8_t *)page_buf;
 	} else {
@@ -1628,11 +1596,8 @@ static int stfsm_write(struct stfsm *fsm, const uint8_t *buf,
 		stfsm_s25fl_clear_status_reg(fsm);
 
 	/* Exit 32-bit address mode, if required */
-	if (fsm->configuration & CFG_WRITE_TOGGLE_32BIT_ADDR) {
+	if (fsm->configuration & CFG_WRITE_TOGGLE_32BIT_ADDR)
 		stfsm_enter_32bit_addr(fsm, 0);
-		if (fsm->configuration & CFG_WRITE_EX_32BIT_ADDR_DELAY)
-			udelay(1);
-	}
 
 	return 0;
 }
@@ -1736,7 +1701,7 @@ static int stfsm_mtd_write(struct mtd_info *mtd, loff_t to, size_t len,
 
 	while (len) {
 		/* Write up to page boundary */
-		bytes = min(FLASH_PAGESIZE - page_offs, len);
+		bytes = min_t(size_t, FLASH_PAGESIZE - page_offs, len);
 
 		ret = stfsm_write(fsm, b, bytes, to);
 		if (ret)
@@ -1935,6 +1900,13 @@ static int stfsm_init(struct stfsm *fsm)
 	       fsm->base + SPI_CONFIGDATA);
 	writel(STFSM_DEFAULT_WR_TIME, fsm->base + SPI_STATUS_WR_TIME_REG);
 
+	/*
+	 * Set the FSM 'WAIT' delay to the minimum workable value.  Note, for
+	 * our purposes, the WAIT instruction is used purely to achieve
+	 * "sequence validity" rather than actually implement a delay.
+	 */
+	writel(0x00000001, fsm->base + SPI_PROGRAM_ERASE_TIME);
+
 	/* Clear FIFO, just in case */
 	stfsm_clear_fifo(fsm);
 
@@ -2086,7 +2058,7 @@ static int stfsm_remove(struct platform_device *pdev)
 	return mtd_device_unregister(&fsm->mtd);
 }
 
-static struct of_device_id stfsm_match[] = {
+static const struct of_device_id stfsm_match[] = {
 	{ .compatible = "st,spi-fsm", },
 	{},
 };
diff --git a/drivers/mtd/lpddr/Kconfig b/drivers/mtd/lpddr/Kconfig
index 265f969817e..3a19cbee24d 100644
--- a/drivers/mtd/lpddr/Kconfig
+++ b/drivers/mtd/lpddr/Kconfig
@@ -1,5 +1,5 @@
-menu "LPDDR flash memory drivers"
-	depends on MTD!=n
+menu "LPDDR & LPDDR2 PCM memory drivers"
+	depends on MTD
 
 config MTD_LPDDR
 	tristate "Support for LPDDR flash chips"
@@ -17,4 +17,13 @@ config MTD_QINFO_PROBE
 	    Window QINFO interface, permits software to be used for entire
 	    families of devices. This serves similar purpose of CFI on legacy
 	    Flash products
+
+config MTD_LPDDR2_NVM
+	# ARM dependency is only for writel_relaxed()
+	depends on MTD && ARM
+	tristate "Support for LPDDR2-NVM flash chips"
+	help
+	  This option enables support of PCM memories with a LPDDR2-NVM
+	  (Low power double data rate 2) interface.
+
 endmenu
diff --git a/drivers/mtd/lpddr/Makefile b/drivers/mtd/lpddr/Makefile
index da48e46b581..881d440d483 100644
--- a/drivers/mtd/lpddr/Makefile
+++ b/drivers/mtd/lpddr/Makefile
@@ -4,3 +4,4 @@
 
 obj-$(CONFIG_MTD_QINFO_PROBE)	+= qinfo_probe.o
 obj-$(CONFIG_MTD_LPDDR)	+= lpddr_cmds.o
+obj-$(CONFIG_MTD_LPDDR2_NVM) += lpddr2_nvm.o
diff --git a/drivers/mtd/lpddr/lpddr2_nvm.c b/drivers/mtd/lpddr/lpddr2_nvm.c
new file mode 100644
index 00000000000..063cec40d0a
--- /dev/null
+++ b/drivers/mtd/lpddr/lpddr2_nvm.c
@@ -0,0 +1,507 @@
+/*
+ * LPDDR2-NVM MTD driver. This module provides read, write, erase, lock/unlock
+ * support for LPDDR2-NVM PCM memories
+ *
+ * Copyright © 2012 Micron Technology, Inc.
+ *
+ * Vincenzo Aliberti <vincenzo.aliberti@gmail.com>
+ * Domenico Manna <domenico.manna@gmail.com>
+ * Many thanks to Andrea Vigilante for initial enabling
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the 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 pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__
+
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/mtd/map.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/partitions.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/ioport.h>
+#include <linux/err.h>
+
+/* Parameters */
+#define ERASE_BLOCKSIZE			(0x00020000/2)	/* in Word */
+#define WRITE_BUFFSIZE			(0x00000400/2)	/* in Word */
+#define OW_BASE_ADDRESS			0x00000000	/* OW offset */
+#define BUS_WIDTH			0x00000020	/* x32 devices */
+
+/* PFOW symbols address offset */
+#define PFOW_QUERY_STRING_P		(0x0000/2)	/* in Word */
+#define PFOW_QUERY_STRING_F		(0x0002/2)	/* in Word */
+#define PFOW_QUERY_STRING_O		(0x0004/2)	/* in Word */
+#define PFOW_QUERY_STRING_W		(0x0006/2)	/* in Word */
+
+/* OW registers address */
+#define CMD_CODE_OFS			(0x0080/2)	/* in Word */
+#define CMD_DATA_OFS			(0x0084/2)	/* in Word */
+#define CMD_ADD_L_OFS			(0x0088/2)	/* in Word */
+#define CMD_ADD_H_OFS			(0x008A/2)	/* in Word */
+#define MPR_L_OFS			(0x0090/2)	/* in Word */
+#define MPR_H_OFS			(0x0092/2)	/* in Word */
+#define CMD_EXEC_OFS			(0x00C0/2)	/* in Word */
+#define STATUS_REG_OFS			(0x00CC/2)	/* in Word */
+#define PRG_BUFFER_OFS			(0x0010/2)	/* in Word */
+
+/* Datamask */
+#define MR_CFGMASK			0x8000
+#define SR_OK_DATAMASK			0x0080
+
+/* LPDDR2-NVM Commands */
+#define LPDDR2_NVM_LOCK			0x0061
+#define LPDDR2_NVM_UNLOCK		0x0062
+#define LPDDR2_NVM_SW_PROGRAM		0x0041
+#define LPDDR2_NVM_SW_OVERWRITE		0x0042
+#define LPDDR2_NVM_BUF_PROGRAM		0x00E9
+#define LPDDR2_NVM_BUF_OVERWRITE	0x00EA
+#define LPDDR2_NVM_ERASE		0x0020
+
+/* LPDDR2-NVM Registers offset */
+#define LPDDR2_MODE_REG_DATA		0x0040
+#define LPDDR2_MODE_REG_CFG		0x0050
+
+/*
+ * Internal Type Definitions
+ * pcm_int_data contains memory controller details:
+ * @reg_data : LPDDR2_MODE_REG_DATA register address after remapping
+ * @reg_cfg  : LPDDR2_MODE_REG_CFG register address after remapping
+ * &bus_width: memory bus-width (eg: x16 2 Bytes, x32 4 Bytes)
+ */
+struct pcm_int_data {
+	void __iomem *ctl_regs;
+	int bus_width;
+};
+
+static DEFINE_MUTEX(lpdd2_nvm_mutex);
+
+/*
+ * Build a map_word starting from an u_long
+ */
+static inline map_word build_map_word(u_long myword)
+{
+	map_word val = { {0} };
+	val.x[0] = myword;
+	return val;
+}
+
+/*
+ * Build Mode Register Configuration DataMask based on device bus-width
+ */
+static inline u_int build_mr_cfgmask(u_int bus_width)
+{
+	u_int val = MR_CFGMASK;
+
+	if (bus_width == 0x0004)		/* x32 device */
+		val = val << 16;
+
+	return val;
+}
+
+/*
+ * Build Status Register OK DataMask based on device bus-width
+ */
+static inline u_int build_sr_ok_datamask(u_int bus_width)
+{
+	u_int val = SR_OK_DATAMASK;
+
+	if (bus_width == 0x0004)		/* x32 device */
+		val = (val << 16)+val;
+
+	return val;
+}
+
+/*
+ * Evaluates Overlay Window Control Registers address
+ */
+static inline u_long ow_reg_add(struct map_info *map, u_long offset)
+{
+	u_long val = 0;
+	struct pcm_int_data *pcm_data = map->fldrv_priv;
+
+	val = map->pfow_base + offset*pcm_data->bus_width;
+
+	return val;
+}
+
+/*
+ * Enable lpddr2-nvm Overlay Window
+ * Overlay Window is a memory mapped area containing all LPDDR2-NVM registers
+ * used by device commands as well as uservisible resources like Device Status
+ * Register, Device ID, etc
+ */
+static inline void ow_enable(struct map_info *map)
+{
+	struct pcm_int_data *pcm_data = map->fldrv_priv;
+
+	writel_relaxed(build_mr_cfgmask(pcm_data->bus_width) | 0x18,
+		pcm_data->ctl_regs + LPDDR2_MODE_REG_CFG);
+	writel_relaxed(0x01, pcm_data->ctl_regs + LPDDR2_MODE_REG_DATA);
+}
+
+/*
+ * Disable lpddr2-nvm Overlay Window
+ * Overlay Window is a memory mapped area containing all LPDDR2-NVM registers
+ * used by device commands as well as uservisible resources like Device Status
+ * Register, Device ID, etc
+ */
+static inline void ow_disable(struct map_info *map)
+{
+	struct pcm_int_data *pcm_data = map->fldrv_priv;
+
+	writel_relaxed(build_mr_cfgmask(pcm_data->bus_width) | 0x18,
+		pcm_data->ctl_regs + LPDDR2_MODE_REG_CFG);
+	writel_relaxed(0x02, pcm_data->ctl_regs + LPDDR2_MODE_REG_DATA);
+}
+
+/*
+ * Execute lpddr2-nvm operations
+ */
+static int lpddr2_nvm_do_op(struct map_info *map, u_long cmd_code,
+	u_long cmd_data, u_long cmd_add, u_long cmd_mpr, u_char *buf)
+{
+	map_word add_l = { {0} }, add_h = { {0} }, mpr_l = { {0} },
+		mpr_h = { {0} }, data_l = { {0} }, cmd = { {0} },
+		exec_cmd = { {0} }, sr;
+	map_word data_h = { {0} };	/* only for 2x x16 devices stacked */
+	u_long i, status_reg, prg_buff_ofs;
+	struct pcm_int_data *pcm_data = map->fldrv_priv;
+	u_int sr_ok_datamask = build_sr_ok_datamask(pcm_data->bus_width);
+
+	/* Builds low and high words for OW Control Registers */
+	add_l.x[0]	= cmd_add & 0x0000FFFF;
+	add_h.x[0]	= (cmd_add >> 16) & 0x0000FFFF;
+	mpr_l.x[0]	= cmd_mpr & 0x0000FFFF;
+	mpr_h.x[0]	= (cmd_mpr >> 16) & 0x0000FFFF;
+	cmd.x[0]	= cmd_code & 0x0000FFFF;
+	exec_cmd.x[0]	= 0x0001;
+	data_l.x[0]	= cmd_data & 0x0000FFFF;
+	data_h.x[0]	= (cmd_data >> 16) & 0x0000FFFF; /* only for 2x x16 */
+
+	/* Set Overlay Window Control Registers */
+	map_write(map, cmd, ow_reg_add(map, CMD_CODE_OFS));
+	map_write(map, data_l, ow_reg_add(map, CMD_DATA_OFS));
+	map_write(map, add_l, ow_reg_add(map, CMD_ADD_L_OFS));
+	map_write(map, add_h, ow_reg_add(map, CMD_ADD_H_OFS));
+	map_write(map, mpr_l, ow_reg_add(map, MPR_L_OFS));
+	map_write(map, mpr_h, ow_reg_add(map, MPR_H_OFS));
+	if (pcm_data->bus_width == 0x0004) {	/* 2x16 devices stacked */
+		map_write(map, cmd, ow_reg_add(map, CMD_CODE_OFS) + 2);
+		map_write(map, data_h, ow_reg_add(map, CMD_DATA_OFS) + 2);
+		map_write(map, add_l, ow_reg_add(map, CMD_ADD_L_OFS) + 2);
+		map_write(map, add_h, ow_reg_add(map, CMD_ADD_H_OFS) + 2);
+		map_write(map, mpr_l, ow_reg_add(map, MPR_L_OFS) + 2);
+		map_write(map, mpr_h, ow_reg_add(map, MPR_H_OFS) + 2);
+	}
+
+	/* Fill Program Buffer */
+	if ((cmd_code == LPDDR2_NVM_BUF_PROGRAM) ||
+		(cmd_code == LPDDR2_NVM_BUF_OVERWRITE)) {
+		prg_buff_ofs = (map_read(map,
+			ow_reg_add(map, PRG_BUFFER_OFS))).x[0];
+		for (i = 0; i < cmd_mpr; i++) {
+			map_write(map, build_map_word(buf[i]), map->pfow_base +
+			prg_buff_ofs + i);
+		}
+	}
+
+	/* Command Execute */
+	map_write(map, exec_cmd, ow_reg_add(map, CMD_EXEC_OFS));
+	if (pcm_data->bus_width == 0x0004)	/* 2x16 devices stacked */
+		map_write(map, exec_cmd, ow_reg_add(map, CMD_EXEC_OFS) + 2);
+
+	/* Status Register Check */
+	do {
+		sr = map_read(map, ow_reg_add(map, STATUS_REG_OFS));
+		status_reg = sr.x[0];
+		if (pcm_data->bus_width == 0x0004) {/* 2x16 devices stacked */
+			sr = map_read(map, ow_reg_add(map,
+				STATUS_REG_OFS) + 2);
+			status_reg += sr.x[0] << 16;
+		}
+	} while ((status_reg & sr_ok_datamask) != sr_ok_datamask);
+
+	return (((status_reg & sr_ok_datamask) == sr_ok_datamask) ? 0 : -EIO);
+}
+
+/*
+ * Execute lpddr2-nvm operations @ block level
+ */
+static int lpddr2_nvm_do_block_op(struct mtd_info *mtd, loff_t start_add,
+	uint64_t len, u_char block_op)
+{
+	struct map_info *map = mtd->priv;
+	u_long add, end_add;
+	int ret = 0;
+
+	mutex_lock(&lpdd2_nvm_mutex);
+
+	ow_enable(map);
+
+	add = start_add;
+	end_add = add + len;
+
+	do {
+		ret = lpddr2_nvm_do_op(map, block_op, 0x00, add, add, NULL);
+		if (ret)
+			goto out;
+		add += mtd->erasesize;
+	} while (add < end_add);
+
+out:
+	ow_disable(map);
+	mutex_unlock(&lpdd2_nvm_mutex);
+	return ret;
+}
+
+/*
+ * verify presence of PFOW string
+ */
+static int lpddr2_nvm_pfow_present(struct map_info *map)
+{
+	map_word pfow_val[4];
+	unsigned int found = 1;
+
+	mutex_lock(&lpdd2_nvm_mutex);
+
+	ow_enable(map);
+
+	/* Load string from array */
+	pfow_val[0] = map_read(map, ow_reg_add(map, PFOW_QUERY_STRING_P));
+	pfow_val[1] = map_read(map, ow_reg_add(map, PFOW_QUERY_STRING_F));
+	pfow_val[2] = map_read(map, ow_reg_add(map, PFOW_QUERY_STRING_O));
+	pfow_val[3] = map_read(map, ow_reg_add(map, PFOW_QUERY_STRING_W));
+
+	/* Verify the string loaded vs expected */
+	if (!map_word_equal(map, build_map_word('P'), pfow_val[0]))
+		found = 0;
+	if (!map_word_equal(map, build_map_word('F'), pfow_val[1]))
+		found = 0;
+	if (!map_word_equal(map, build_map_word('O'), pfow_val[2]))
+		found = 0;
+	if (!map_word_equal(map, build_map_word('W'), pfow_val[3]))
+		found = 0;
+
+	ow_disable(map);
+
+	mutex_unlock(&lpdd2_nvm_mutex);
+
+	return found;
+}
+
+/*
+ * lpddr2_nvm driver read method
+ */
+static int lpddr2_nvm_read(struct mtd_info *mtd, loff_t start_add,
+				size_t len, size_t *retlen, u_char *buf)
+{
+	struct map_info *map = mtd->priv;
+
+	mutex_lock(&lpdd2_nvm_mutex);
+
+	*retlen = len;
+
+	map_copy_from(map, buf, start_add, *retlen);
+
+	mutex_unlock(&lpdd2_nvm_mutex);
+	return 0;
+}
+
+/*
+ * lpddr2_nvm driver write method
+ */
+static int lpddr2_nvm_write(struct mtd_info *mtd, loff_t start_add,
+				size_t len, size_t *retlen, const u_char *buf)
+{
+	struct map_info *map = mtd->priv;
+	struct pcm_int_data *pcm_data = map->fldrv_priv;
+	u_long add, current_len, tot_len, target_len, my_data;
+	u_char *write_buf = (u_char *)buf;
+	int ret = 0;
+
+	mutex_lock(&lpdd2_nvm_mutex);
+
+	ow_enable(map);
+
+	/* Set start value for the variables */
+	add = start_add;
+	target_len = len;
+	tot_len = 0;
+
+	while (tot_len < target_len) {
+		if (!(IS_ALIGNED(add, mtd->writesize))) { /* do sw program */
+			my_data = write_buf[tot_len];
+			my_data += (write_buf[tot_len+1]) << 8;
+			if (pcm_data->bus_width == 0x0004) {/* 2x16 devices */
+				my_data += (write_buf[tot_len+2]) << 16;
+				my_data += (write_buf[tot_len+3]) << 24;
+			}
+			ret = lpddr2_nvm_do_op(map, LPDDR2_NVM_SW_OVERWRITE,
+				my_data, add, 0x00, NULL);
+			if (ret)
+				goto out;
+
+			add += pcm_data->bus_width;
+			tot_len += pcm_data->bus_width;
+		} else {		/* do buffer program */
+			current_len = min(target_len - tot_len,
+				(u_long) mtd->writesize);
+			ret = lpddr2_nvm_do_op(map, LPDDR2_NVM_BUF_OVERWRITE,
+				0x00, add, current_len, write_buf + tot_len);
+			if (ret)
+				goto out;
+
+			add += current_len;
+			tot_len += current_len;
+		}
+	}
+
+out:
+	*retlen = tot_len;
+	ow_disable(map);
+	mutex_unlock(&lpdd2_nvm_mutex);
+	return ret;
+}
+
+/*
+ * lpddr2_nvm driver erase method
+ */
+static int lpddr2_nvm_erase(struct mtd_info *mtd, struct erase_info *instr)
+{
+	int ret = lpddr2_nvm_do_block_op(mtd, instr->addr, instr->len,
+		LPDDR2_NVM_ERASE);
+	if (!ret) {
+		instr->state = MTD_ERASE_DONE;
+		mtd_erase_callback(instr);
+	}
+
+	return ret;
+}
+
+/*
+ * lpddr2_nvm driver unlock method
+ */
+static int lpddr2_nvm_unlock(struct mtd_info *mtd, loff_t start_add,
+	uint64_t len)
+{
+	return lpddr2_nvm_do_block_op(mtd, start_add, len, LPDDR2_NVM_UNLOCK);
+}
+
+/*
+ * lpddr2_nvm driver lock method
+ */
+static int lpddr2_nvm_lock(struct mtd_info *mtd, loff_t start_add,
+	uint64_t len)
+{
+	return lpddr2_nvm_do_block_op(mtd, start_add, len, LPDDR2_NVM_LOCK);
+}
+
+/*
+ * lpddr2_nvm driver probe method
+ */
+static int lpddr2_nvm_probe(struct platform_device *pdev)
+{
+	struct map_info *map;
+	struct mtd_info *mtd;
+	struct resource *add_range;
+	struct resource *control_regs;
+	struct pcm_int_data *pcm_data;
+
+	/* Allocate memory control_regs data structures */
+	pcm_data = devm_kzalloc(&pdev->dev, sizeof(*pcm_data), GFP_KERNEL);
+	if (!pcm_data)
+		return -ENOMEM;
+
+	pcm_data->bus_width = BUS_WIDTH;
+
+	/* Allocate memory for map_info & mtd_info data structures */
+	map = devm_kzalloc(&pdev->dev, sizeof(*map), GFP_KERNEL);
+	if (!map)
+		return -ENOMEM;
+
+	mtd = devm_kzalloc(&pdev->dev, sizeof(*mtd), GFP_KERNEL);
+	if (!mtd)
+		return -ENOMEM;
+
+	/* lpddr2_nvm address range */
+	add_range = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	/* Populate map_info data structure */
+	*map = (struct map_info) {
+		.virt		= devm_ioremap_resource(&pdev->dev, add_range),
+		.name		= pdev->dev.init_name,
+		.phys		= add_range->start,
+		.size		= resource_size(add_range),
+		.bankwidth	= pcm_data->bus_width / 2,
+		.pfow_base	= OW_BASE_ADDRESS,
+		.fldrv_priv	= pcm_data,
+	};
+	if (IS_ERR(map->virt))
+		return PTR_ERR(map->virt);
+
+	simple_map_init(map);	/* fill with default methods */
+
+	control_regs = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	pcm_data->ctl_regs = devm_ioremap_resource(&pdev->dev, control_regs);
+	if (IS_ERR(pcm_data->ctl_regs))
+		return PTR_ERR(pcm_data->ctl_regs);
+
+	/* Populate mtd_info data structure */
+	*mtd = (struct mtd_info) {
+		.name		= pdev->dev.init_name,
+		.type		= MTD_RAM,
+		.priv		= map,
+		.size		= resource_size(add_range),
+		.erasesize	= ERASE_BLOCKSIZE * pcm_data->bus_width,
+		.writesize	= 1,
+		.writebufsize	= WRITE_BUFFSIZE * pcm_data->bus_width,
+		.flags		= (MTD_CAP_NVRAM | MTD_POWERUP_LOCK),
+		._read		= lpddr2_nvm_read,
+		._write		= lpddr2_nvm_write,
+		._erase		= lpddr2_nvm_erase,
+		._unlock	= lpddr2_nvm_unlock,
+		._lock		= lpddr2_nvm_lock,
+	};
+
+	/* Verify the presence of the device looking for PFOW string */
+	if (!lpddr2_nvm_pfow_present(map)) {
+		pr_err("device not recognized\n");
+		return -EINVAL;
+	}
+	/* Parse partitions and register the MTD device */
+	return mtd_device_parse_register(mtd, NULL, NULL, NULL, 0);
+}
+
+/*
+ * lpddr2_nvm driver remove method
+ */
+static int lpddr2_nvm_remove(struct platform_device *pdev)
+{
+	return mtd_device_unregister(dev_get_drvdata(&pdev->dev));
+}
+
+/* Initialize platform_driver data structure for lpddr2_nvm */
+static struct platform_driver lpddr2_nvm_drv = {
+	.driver		= {
+		.name	= "lpddr2_nvm",
+	},
+	.probe		= lpddr2_nvm_probe,
+	.remove		= lpddr2_nvm_remove,
+};
+
+module_platform_driver(lpddr2_nvm_drv);
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Vincenzo Aliberti <vincenzo.aliberti@gmail.com>");
+MODULE_DESCRIPTION("MTD driver for LPDDR2-NVM PCM memories");
diff --git a/drivers/mtd/maps/Kconfig b/drivers/mtd/maps/Kconfig
index fce23fe043f..21b2874a303 100644
--- a/drivers/mtd/maps/Kconfig
+++ b/drivers/mtd/maps/Kconfig
@@ -108,7 +108,7 @@ config MTD_SUN_UFLASH
 
 config MTD_SC520CDP
 	tristate "CFI Flash device mapped on AMD SC520 CDP"
-	depends on X86 && MTD_CFI
+	depends on (MELAN || COMPILE_TEST) && MTD_CFI
 	help
 	  The SC520 CDP board has two banks of CFI-compliant chips and one
 	  Dual-in-line JEDEC chip. This 'mapping' driver supports that
@@ -116,7 +116,7 @@ config MTD_SC520CDP
 
 config MTD_NETSC520
 	tristate "CFI Flash device mapped on AMD NetSc520"
-	depends on X86 && MTD_CFI
+	depends on (MELAN || COMPILE_TEST) && MTD_CFI
 	help
 	  This enables access routines for the flash chips on the AMD NetSc520
 	  demonstration board. If you have one of these boards and would like
diff --git a/drivers/mtd/maps/sc520cdp.c b/drivers/mtd/maps/sc520cdp.c
index 8fead8e46bc..093edd51bdc 100644
--- a/drivers/mtd/maps/sc520cdp.c
+++ b/drivers/mtd/maps/sc520cdp.c
@@ -183,7 +183,7 @@ static const struct sc520_par_table par_table[NUM_FLASH_BANKS] =
 
 static void sc520cdp_setup_par(void)
 {
-	volatile unsigned long __iomem *mmcr;
+	unsigned long __iomem *mmcr;
 	unsigned long mmcr_val;
 	int i, j;
 
@@ -203,11 +203,11 @@ static void sc520cdp_setup_par(void)
 	*/
 	for(i = 0; i < NUM_FLASH_BANKS; i++) {		/* for each par_table entry  */
 		for(j = 0; j < NUM_SC520_PAR; j++) {	/* for each PAR register     */
-			mmcr_val = mmcr[SC520_PAR(j)];
+			mmcr_val = readl(&mmcr[SC520_PAR(j)]);
 			/* if target device field matches, reprogram the PAR */
 			if((mmcr_val & SC520_PAR_TRGDEV) == par_table[i].trgdev)
 			{
-				mmcr[SC520_PAR(j)] = par_table[i].new_par;
+				writel(par_table[i].new_par, &mmcr[SC520_PAR(j)]);
 				break;
 			}
 		}
diff --git a/drivers/mtd/maps/solutionengine.c b/drivers/mtd/maps/solutionengine.c
index 83a7a709156..bb580bc1644 100644
--- a/drivers/mtd/maps/solutionengine.c
+++ b/drivers/mtd/maps/solutionengine.c
@@ -33,28 +33,6 @@ struct map_info soleng_flash_map = {
 
 static const char * const probes[] = { "RedBoot", "cmdlinepart", NULL };
 
-#ifdef CONFIG_MTD_SUPERH_RESERVE
-static struct mtd_partition superh_se_partitions[] = {
-	/* Reserved for boot code, read-only */
-	{
-		.name = "flash_boot",
-		.offset = 0x00000000,
-		.size = CONFIG_MTD_SUPERH_RESERVE,
-		.mask_flags = MTD_WRITEABLE,
-	},
-	/* All else is writable (e.g. JFFS) */
-	{
-		.name = "Flash FS",
-		.offset = MTDPART_OFS_NXTBLK,
-		.size = MTDPART_SIZ_FULL,
-	}
-};
-#define NUM_PARTITIONS ARRAY_SIZE(superh_se_partitions)
-#else
-#define superh_se_partitions NULL
-#define NUM_PARTITIONS 0
-#endif /* CONFIG_MTD_SUPERH_RESERVE */
-
 static int __init init_soleng_maps(void)
 {
 	/* First probe at offset 0 */
@@ -92,8 +70,7 @@ static int __init init_soleng_maps(void)
 		mtd_device_register(eprom_mtd, NULL, 0);
 	}
 
-	mtd_device_parse_register(flash_mtd, probes, NULL,
-				  superh_se_partitions, NUM_PARTITIONS);
+	mtd_device_parse_register(flash_mtd, probes, NULL, NULL, 0);
 
 	return 0;
 }
diff --git a/drivers/mtd/mtd_blkdevs.c b/drivers/mtd/mtd_blkdevs.c
index 0b2ccb68c0d..43e30992a36 100644
--- a/drivers/mtd/mtd_blkdevs.c
+++ b/drivers/mtd/mtd_blkdevs.c
@@ -82,12 +82,14 @@ static int do_blktrans_request(struct mtd_blktrans_ops *tr,
 
 	block = blk_rq_pos(req) << 9 >> tr->blkshift;
 	nsect = blk_rq_cur_bytes(req) >> tr->blkshift;
-
-	buf = req->buffer;
+	buf = bio_data(req->bio);
 
 	if (req->cmd_type != REQ_TYPE_FS)
 		return -EIO;
 
+	if (req->cmd_flags & REQ_FLUSH)
+		return tr->flush(dev);
+
 	if (blk_rq_pos(req) + blk_rq_cur_sectors(req) >
 	    get_capacity(req->rq_disk))
 		return -EIO;
@@ -408,6 +410,9 @@ int add_mtd_blktrans_dev(struct mtd_blktrans_dev *new)
 	if (!new->rq)
 		goto error3;
 
+	if (tr->flush)
+		blk_queue_flush(new->rq, REQ_FLUSH);
+
 	new->rq->queuedata = new;
 	blk_queue_logical_block_size(new->rq, tr->blksize);
 
diff --git a/drivers/mtd/mtdchar.c b/drivers/mtd/mtdchar.c
index 7d4e7b9da3a..a0f54e80670 100644
--- a/drivers/mtd/mtdchar.c
+++ b/drivers/mtd/mtdchar.c
@@ -568,13 +568,18 @@ static int mtdchar_write_ioctl(struct mtd_info *mtd,
 {
 	struct mtd_write_req req;
 	struct mtd_oob_ops ops;
-	void __user *usr_data, *usr_oob;
+	const void __user *usr_data, *usr_oob;
 	int ret;
 
-	if (copy_from_user(&req, argp, sizeof(req)) ||
-			!access_ok(VERIFY_READ, req.usr_data, req.len) ||
-			!access_ok(VERIFY_READ, req.usr_oob, req.ooblen))
+	if (copy_from_user(&req, argp, sizeof(req)))
 		return -EFAULT;
+
+	usr_data = (const void __user *)(uintptr_t)req.usr_data;
+	usr_oob = (const void __user *)(uintptr_t)req.usr_oob;
+	if (!access_ok(VERIFY_READ, usr_data, req.len) ||
+	    !access_ok(VERIFY_READ, usr_oob, req.ooblen))
+		return -EFAULT;
+
 	if (!mtd->_write_oob)
 		return -EOPNOTSUPP;
 
@@ -583,10 +588,7 @@ static int mtdchar_write_ioctl(struct mtd_info *mtd,
 	ops.ooblen = (size_t)req.ooblen;
 	ops.ooboffs = 0;
 
-	usr_data = (void __user *)(uintptr_t)req.usr_data;
-	usr_oob = (void __user *)(uintptr_t)req.usr_oob;
-
-	if (req.usr_data) {
+	if (usr_data) {
 		ops.datbuf = memdup_user(usr_data, ops.len);
 		if (IS_ERR(ops.datbuf))
 			return PTR_ERR(ops.datbuf);
@@ -594,7 +596,7 @@ static int mtdchar_write_ioctl(struct mtd_info *mtd,
 		ops.datbuf = NULL;
 	}
 
-	if (req.usr_oob) {
+	if (usr_oob) {
 		ops.oobbuf = memdup_user(usr_oob, ops.ooblen);
 		if (IS_ERR(ops.oobbuf)) {
 			kfree(ops.datbuf);
diff --git a/drivers/mtd/nand/bf5xx_nand.c b/drivers/mtd/nand/bf5xx_nand.c
index b7a24946ca2..722898aea7a 100644
--- a/drivers/mtd/nand/bf5xx_nand.c
+++ b/drivers/mtd/nand/bf5xx_nand.c
@@ -679,9 +679,6 @@ static int bf5xx_nand_remove(struct platform_device *pdev)
 	peripheral_free_list(bfin_nfc_pin_req);
 	bf5xx_nand_dma_remove(info);
 
-	/* free the common resources */
-	kfree(info);
-
 	return 0;
 }
 
@@ -742,10 +739,10 @@ static int bf5xx_nand_probe(struct platform_device *pdev)
 		return -EFAULT;
 	}
 
-	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	info = devm_kzalloc(&pdev->dev, sizeof(*info), GFP_KERNEL);
 	if (info == NULL) {
 		err = -ENOMEM;
-		goto out_err_kzalloc;
+		goto out_err;
 	}
 
 	platform_set_drvdata(pdev, info);
@@ -790,7 +787,7 @@ static int bf5xx_nand_probe(struct platform_device *pdev)
 	/* initialise the hardware */
 	err = bf5xx_nand_hw_init(info);
 	if (err)
-		goto out_err_hw_init;
+		goto out_err;
 
 	/* setup hardware ECC data struct */
 	if (hardware_ecc) {
@@ -827,9 +824,7 @@ static int bf5xx_nand_probe(struct platform_device *pdev)
 
 out_err_nand_scan:
 	bf5xx_nand_dma_remove(info);
-out_err_hw_init:
-	kfree(info);
-out_err_kzalloc:
+out_err:
 	peripheral_free_list(bfin_nfc_pin_req);
 
 	return err;
diff --git a/drivers/mtd/nand/denali.c b/drivers/mtd/nand/denali.c
index c07cd573ad3..9f2012a3e76 100644
--- a/drivers/mtd/nand/denali.c
+++ b/drivers/mtd/nand/denali.c
@@ -1233,7 +1233,7 @@ static int denali_waitfunc(struct mtd_info *mtd, struct nand_chip *chip)
 	return status;
 }
 
-static void denali_erase(struct mtd_info *mtd, int page)
+static int denali_erase(struct mtd_info *mtd, int page)
 {
 	struct denali_nand_info *denali = mtd_to_denali(mtd);
 
@@ -1250,8 +1250,7 @@ static void denali_erase(struct mtd_info *mtd, int page)
 	irq_status = wait_for_irq(denali, INTR_STATUS__ERASE_COMP |
 					INTR_STATUS__ERASE_FAIL);
 
-	denali->status = (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,
@@ -1584,7 +1583,7 @@ int denali_init(struct denali_nand_info *denali)
 	denali->nand.ecc.write_page_raw = denali_write_page_raw;
 	denali->nand.ecc.read_oob = denali_read_oob;
 	denali->nand.ecc.write_oob = denali_write_oob;
-	denali->nand.erase_cmd = denali_erase;
+	denali->nand.erase = denali_erase;
 
 	if (nand_scan_tail(&denali->mtd)) {
 		ret = -ENXIO;
diff --git a/drivers/mtd/nand/docg4.c b/drivers/mtd/nand/docg4.c
index 1b0265e85a0..ce24637e14f 100644
--- a/drivers/mtd/nand/docg4.c
+++ b/drivers/mtd/nand/docg4.c
@@ -872,7 +872,7 @@ static int docg4_read_oob(struct mtd_info *mtd, struct nand_chip *nand,
 	return 0;
 }
 
-static void docg4_erase_block(struct mtd_info *mtd, int page)
+static int docg4_erase_block(struct mtd_info *mtd, int page)
 {
 	struct nand_chip *nand = mtd->priv;
 	struct docg4_priv *doc = nand->priv;
@@ -916,6 +916,8 @@ static void docg4_erase_block(struct mtd_info *mtd, int page)
 	write_nop(docptr);
 	poll_status(doc);
 	write_nop(docptr);
+
+	return nand->waitfunc(mtd, nand);
 }
 
 static int write_page(struct mtd_info *mtd, struct nand_chip *nand,
@@ -1236,7 +1238,7 @@ static void __init init_mtd_structs(struct mtd_info *mtd)
 	nand->block_markbad = docg4_block_markbad;
 	nand->read_buf = docg4_read_buf;
 	nand->write_buf = docg4_write_buf16;
-	nand->erase_cmd = docg4_erase_block;
+	nand->erase = docg4_erase_block;
 	nand->ecc.read_page = docg4_read_page;
 	nand->ecc.write_page = docg4_write_page;
 	nand->ecc.read_page_raw = docg4_read_page_raw;
diff --git a/drivers/mtd/nand/fsl_elbc_nand.c b/drivers/mtd/nand/fsl_elbc_nand.c
index ec549cd9849..545a5c002f0 100644
--- a/drivers/mtd/nand/fsl_elbc_nand.c
+++ b/drivers/mtd/nand/fsl_elbc_nand.c
@@ -723,6 +723,19 @@ static int fsl_elbc_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 	return 0;
 }
 
+/* ECC will be calculated automatically, and errors will be detected in
+ * waitfunc.
+ */
+static int fsl_elbc_write_subpage(struct mtd_info *mtd, struct nand_chip *chip,
+				uint32_t offset, uint32_t data_len,
+				const uint8_t *buf, int oob_required)
+{
+	fsl_elbc_write_buf(mtd, buf, mtd->writesize);
+	fsl_elbc_write_buf(mtd, chip->oob_poi, mtd->oobsize);
+
+	return 0;
+}
+
 static int fsl_elbc_chip_init(struct fsl_elbc_mtd *priv)
 {
 	struct fsl_lbc_ctrl *ctrl = priv->ctrl;
@@ -761,6 +774,7 @@ static int fsl_elbc_chip_init(struct fsl_elbc_mtd *priv)
 
 	chip->ecc.read_page = fsl_elbc_read_page;
 	chip->ecc.write_page = fsl_elbc_write_page;
+	chip->ecc.write_subpage = fsl_elbc_write_subpage;
 
 	/* If CS Base Register selects full hardware ECC then use it */
 	if ((in_be32(&lbc->bank[priv->bank].br) & BR_DECC) ==
diff --git a/drivers/mtd/nand/fsl_ifc_nand.c b/drivers/mtd/nand/fsl_ifc_nand.c
index cb45d2f8e20..2338124dd05 100644
--- a/drivers/mtd/nand/fsl_ifc_nand.c
+++ b/drivers/mtd/nand/fsl_ifc_nand.c
@@ -56,7 +56,7 @@ struct fsl_ifc_nand_ctrl {
 	struct nand_hw_control controller;
 	struct fsl_ifc_mtd *chips[FSL_IFC_BANK_COUNT];
 
-	u8 __iomem *addr;	/* Address of assigned IFC buffer	*/
+	void __iomem *addr;	/* Address of assigned IFC buffer	*/
 	unsigned int page;	/* Last page written to / read from	*/
 	unsigned int read_bytes;/* Number of bytes read during command	*/
 	unsigned int column;	/* Saved column from SEQIN		*/
@@ -591,7 +591,10 @@ static void fsl_ifc_cmdfunc(struct mtd_info *mtd, unsigned int command,
 		 * The chip always seems to report that it is
 		 * write-protected, even when it is not.
 		 */
-		setbits8(ifc_nand_ctrl->addr, NAND_STATUS_WP);
+		if (chip->options & NAND_BUSWIDTH_16)
+			setbits16(ifc_nand_ctrl->addr, NAND_STATUS_WP);
+		else
+			setbits8(ifc_nand_ctrl->addr, NAND_STATUS_WP);
 		return;
 
 	case NAND_CMD_RESET:
@@ -636,7 +639,7 @@ static void fsl_ifc_write_buf(struct mtd_info *mtd, const u8 *buf, int len)
 		len = bufsize - ifc_nand_ctrl->index;
 	}
 
-	memcpy_toio(&ifc_nand_ctrl->addr[ifc_nand_ctrl->index], buf, len);
+	memcpy_toio(ifc_nand_ctrl->addr + ifc_nand_ctrl->index, buf, len);
 	ifc_nand_ctrl->index += len;
 }
 
@@ -648,13 +651,16 @@ static uint8_t fsl_ifc_read_byte(struct mtd_info *mtd)
 {
 	struct nand_chip *chip = mtd->priv;
 	struct fsl_ifc_mtd *priv = chip->priv;
+	unsigned int offset;
 
 	/*
 	 * If there are still bytes in the IFC buffer, then use the
 	 * next byte.
 	 */
-	if (ifc_nand_ctrl->index < ifc_nand_ctrl->read_bytes)
-		return in_8(&ifc_nand_ctrl->addr[ifc_nand_ctrl->index++]);
+	if (ifc_nand_ctrl->index < ifc_nand_ctrl->read_bytes) {
+		offset = ifc_nand_ctrl->index++;
+		return in_8(ifc_nand_ctrl->addr + offset);
+	}
 
 	dev_err(priv->dev, "%s: beyond end of buffer\n", __func__);
 	return ERR_BYTE;
@@ -675,8 +681,7 @@ static uint8_t fsl_ifc_read_byte16(struct mtd_info *mtd)
 	 * next byte.
 	 */
 	if (ifc_nand_ctrl->index < ifc_nand_ctrl->read_bytes) {
-		data = in_be16((uint16_t __iomem *)&ifc_nand_ctrl->
-			       addr[ifc_nand_ctrl->index]);
+		data = in_be16(ifc_nand_ctrl->addr + ifc_nand_ctrl->index);
 		ifc_nand_ctrl->index += 2;
 		return (uint8_t) data;
 	}
@@ -701,7 +706,7 @@ static void fsl_ifc_read_buf(struct mtd_info *mtd, u8 *buf, int len)
 
 	avail = min((unsigned int)len,
 			ifc_nand_ctrl->read_bytes - ifc_nand_ctrl->index);
-	memcpy_fromio(buf, &ifc_nand_ctrl->addr[ifc_nand_ctrl->index], avail);
+	memcpy_fromio(buf, ifc_nand_ctrl->addr + ifc_nand_ctrl->index, avail);
 	ifc_nand_ctrl->index += avail;
 
 	if (len > avail)
diff --git a/drivers/mtd/nand/gpmi-nand/bch-regs.h b/drivers/mtd/nand/gpmi-nand/bch-regs.h
index 588f5374047..05bb91f2f4c 100644
--- a/drivers/mtd/nand/gpmi-nand/bch-regs.h
+++ b/drivers/mtd/nand/gpmi-nand/bch-regs.h
@@ -54,7 +54,7 @@
 #define MX6Q_BP_BCH_FLASH0LAYOUT0_ECC0		11
 #define MX6Q_BM_BCH_FLASH0LAYOUT0_ECC0	(0x1f << MX6Q_BP_BCH_FLASH0LAYOUT0_ECC0)
 #define BF_BCH_FLASH0LAYOUT0_ECC0(v, x)				\
-	(GPMI_IS_MX6Q(x)					\
+	(GPMI_IS_MX6(x)					\
 		? (((v) << MX6Q_BP_BCH_FLASH0LAYOUT0_ECC0)	\
 			& MX6Q_BM_BCH_FLASH0LAYOUT0_ECC0)	\
 		: (((v) << BP_BCH_FLASH0LAYOUT0_ECC0)		\
@@ -65,7 +65,7 @@
 #define MX6Q_BM_BCH_FLASH0LAYOUT0_GF_13_14			\
 				(0x1 << MX6Q_BP_BCH_FLASH0LAYOUT0_GF_13_14)
 #define BF_BCH_FLASH0LAYOUT0_GF(v, x)				\
-	((GPMI_IS_MX6Q(x) && ((v) == 14))			\
+	((GPMI_IS_MX6(x) && ((v) == 14))			\
 		? (((1) << MX6Q_BP_BCH_FLASH0LAYOUT0_GF_13_14)	\
 			& MX6Q_BM_BCH_FLASH0LAYOUT0_GF_13_14)	\
 		: 0						\
@@ -77,7 +77,7 @@
 #define MX6Q_BM_BCH_FLASH0LAYOUT0_DATA0_SIZE	\
 			(0x3ff << BP_BCH_FLASH0LAYOUT0_DATA0_SIZE)
 #define BF_BCH_FLASH0LAYOUT0_DATA0_SIZE(v, x)				\
-	(GPMI_IS_MX6Q(x)						\
+	(GPMI_IS_MX6(x)						\
 		? (((v) >> 2) & MX6Q_BM_BCH_FLASH0LAYOUT0_DATA0_SIZE)	\
 		: ((v) & BM_BCH_FLASH0LAYOUT0_DATA0_SIZE)		\
 	)
@@ -96,7 +96,7 @@
 #define MX6Q_BP_BCH_FLASH0LAYOUT1_ECCN		11
 #define MX6Q_BM_BCH_FLASH0LAYOUT1_ECCN	(0x1f << MX6Q_BP_BCH_FLASH0LAYOUT1_ECCN)
 #define BF_BCH_FLASH0LAYOUT1_ECCN(v, x)				\
-	(GPMI_IS_MX6Q(x)					\
+	(GPMI_IS_MX6(x)					\
 		? (((v) << MX6Q_BP_BCH_FLASH0LAYOUT1_ECCN)	\
 			& MX6Q_BM_BCH_FLASH0LAYOUT1_ECCN)	\
 		: (((v) << BP_BCH_FLASH0LAYOUT1_ECCN)		\
@@ -107,7 +107,7 @@
 #define MX6Q_BM_BCH_FLASH0LAYOUT1_GF_13_14			\
 				(0x1 << MX6Q_BP_BCH_FLASH0LAYOUT1_GF_13_14)
 #define BF_BCH_FLASH0LAYOUT1_GF(v, x)				\
-	((GPMI_IS_MX6Q(x) && ((v) == 14))			\
+	((GPMI_IS_MX6(x) && ((v) == 14))			\
 		? (((1) << MX6Q_BP_BCH_FLASH0LAYOUT1_GF_13_14)	\
 			& MX6Q_BM_BCH_FLASH0LAYOUT1_GF_13_14)	\
 		: 0						\
@@ -119,7 +119,7 @@
 #define MX6Q_BM_BCH_FLASH0LAYOUT1_DATAN_SIZE	\
 			(0x3ff << BP_BCH_FLASH0LAYOUT1_DATAN_SIZE)
 #define BF_BCH_FLASH0LAYOUT1_DATAN_SIZE(v, x)				\
-	(GPMI_IS_MX6Q(x)						\
+	(GPMI_IS_MX6(x)						\
 		? (((v) >> 2) & MX6Q_BM_BCH_FLASH0LAYOUT1_DATAN_SIZE)	\
 		: ((v) & BM_BCH_FLASH0LAYOUT1_DATAN_SIZE)		\
 	)
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-lib.c b/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
index dd1df605a1d..87e658ce23e 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
@@ -861,7 +861,7 @@ static void gpmi_compute_edo_timing(struct gpmi_nand_data *this,
 	struct resources *r = &this->resources;
 	unsigned long rate = clk_get_rate(r->clock[0]);
 	int mode = this->timing_mode;
-	int dll_threshold = 16; /* in ns */
+	int dll_threshold = this->devdata->max_chain_delay;
 	unsigned long delay;
 	unsigned long clk_period;
 	int t_rea;
@@ -886,9 +886,6 @@ static void gpmi_compute_edo_timing(struct gpmi_nand_data *this,
 	/* [3] for GPMI_HW_GPMI_CTRL1 */
 	hw->wrn_dly_sel = BV_GPMI_CTRL1_WRN_DLY_SEL_NO_DELAY;
 
-	if (GPMI_IS_MX6Q(this))
-		dll_threshold = 12;
-
 	/*
 	 * Enlarge 10 times for the numerator and denominator in {3}.
 	 * This make us to get more accurate result.
@@ -974,7 +971,7 @@ int gpmi_extra_init(struct gpmi_nand_data *this)
 	struct nand_chip *chip = &this->nand;
 
 	/* Enable the asynchronous EDO feature. */
-	if (GPMI_IS_MX6Q(this) && chip->onfi_version) {
+	if (GPMI_IS_MX6(this) && chip->onfi_version) {
 		int mode = onfi_get_async_timing_mode(chip);
 
 		/* We only support the timing mode 4 and mode 5. */
@@ -1096,12 +1093,12 @@ int gpmi_is_ready(struct gpmi_nand_data *this, unsigned chip)
 	if (GPMI_IS_MX23(this)) {
 		mask = MX23_BM_GPMI_DEBUG_READY0 << chip;
 		reg = readl(r->gpmi_regs + HW_GPMI_DEBUG);
-	} else if (GPMI_IS_MX28(this) || GPMI_IS_MX6Q(this)) {
+	} else if (GPMI_IS_MX28(this) || GPMI_IS_MX6(this)) {
 		/*
 		 * In the imx6, all the ready/busy pins are bound
 		 * together. So we only need to check chip 0.
 		 */
-		if (GPMI_IS_MX6Q(this))
+		if (GPMI_IS_MX6(this))
 			chip = 0;
 
 		/* MX28 shares the same R/B register as MX6Q. */
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-nand.c b/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
index bb77f750e75..f638cd8077c 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
@@ -53,6 +53,30 @@ static struct nand_ecclayout gpmi_hw_ecclayout = {
 	.oobfree = { {.offset = 0, .length = 0} }
 };
 
+static const struct gpmi_devdata gpmi_devdata_imx23 = {
+	.type = IS_MX23,
+	.bch_max_ecc_strength = 20,
+	.max_chain_delay = 16,
+};
+
+static const struct gpmi_devdata gpmi_devdata_imx28 = {
+	.type = IS_MX28,
+	.bch_max_ecc_strength = 20,
+	.max_chain_delay = 16,
+};
+
+static const struct gpmi_devdata gpmi_devdata_imx6q = {
+	.type = IS_MX6Q,
+	.bch_max_ecc_strength = 40,
+	.max_chain_delay = 12,
+};
+
+static const struct gpmi_devdata gpmi_devdata_imx6sx = {
+	.type = IS_MX6SX,
+	.bch_max_ecc_strength = 62,
+	.max_chain_delay = 12,
+};
+
 static irqreturn_t bch_irq(int irq, void *cookie)
 {
 	struct gpmi_nand_data *this = cookie;
@@ -102,14 +126,8 @@ static inline bool gpmi_check_ecc(struct gpmi_nand_data *this)
 		/* The mx23/mx28 only support the GF13. */
 		if (geo->gf_len == 14)
 			return false;
-
-		if (geo->ecc_strength > MXS_ECC_STRENGTH_MAX)
-			return false;
-	} else if (GPMI_IS_MX6Q(this)) {
-		if (geo->ecc_strength > MX6_ECC_STRENGTH_MAX)
-			return false;
 	}
-	return true;
+	return geo->ecc_strength <= this->devdata->bch_max_ecc_strength;
 }
 
 /*
@@ -270,8 +288,7 @@ static int legacy_set_geometry(struct gpmi_nand_data *this)
 			"We can not support this nand chip."
 			" Its required ecc strength(%d) is beyond our"
 			" capability(%d).\n", geo->ecc_strength,
-			(GPMI_IS_MX6Q(this) ? MX6_ECC_STRENGTH_MAX
-					: MXS_ECC_STRENGTH_MAX));
+			this->devdata->bch_max_ecc_strength);
 		return -EINVAL;
 	}
 
@@ -572,7 +589,7 @@ static int gpmi_get_clks(struct gpmi_nand_data *this)
 	}
 
 	/* Get extra clocks */
-	if (GPMI_IS_MX6Q(this))
+	if (GPMI_IS_MX6(this))
 		extra_clks = extra_clks_for_mx6q;
 	if (!extra_clks)
 		return 0;
@@ -590,9 +607,9 @@ static int gpmi_get_clks(struct gpmi_nand_data *this)
 		r->clock[i] = clk;
 	}
 
-	if (GPMI_IS_MX6Q(this))
+	if (GPMI_IS_MX6(this))
 		/*
-		 * Set the default value for the gpmi clock in mx6q:
+		 * Set the default value for the gpmi clock.
 		 *
 		 * If you want to use the ONFI nand which is in the
 		 * Synchronous Mode, you should change the clock as you need.
@@ -1655,7 +1672,7 @@ static int gpmi_init_last(struct gpmi_nand_data *this)
 	 *  (1) the chip is imx6, and
 	 *  (2) the size of the ECC parity is byte aligned.
 	 */
-	if (GPMI_IS_MX6Q(this) &&
+	if (GPMI_IS_MX6(this) &&
 		((bch_geo->gf_len * bch_geo->ecc_strength) % 8) == 0) {
 		ecc->read_subpage = gpmi_ecc_read_subpage;
 		chip->options |= NAND_SUBPAGE_READ;
@@ -1711,7 +1728,7 @@ static int gpmi_nand_init(struct gpmi_nand_data *this)
 	if (ret)
 		goto err_out;
 
-	ret = nand_scan_ident(mtd, GPMI_IS_MX6Q(this) ? 2 : 1, NULL);
+	ret = nand_scan_ident(mtd, GPMI_IS_MX6(this) ? 2 : 1, NULL);
 	if (ret)
 		goto err_out;
 
@@ -1740,23 +1757,19 @@ err_out:
 	return ret;
 }
 
-static const struct platform_device_id gpmi_ids[] = {
-	{ .name = "imx23-gpmi-nand", .driver_data = IS_MX23, },
-	{ .name = "imx28-gpmi-nand", .driver_data = IS_MX28, },
-	{ .name = "imx6q-gpmi-nand", .driver_data = IS_MX6Q, },
-	{}
-};
-
 static const struct of_device_id gpmi_nand_id_table[] = {
 	{
 		.compatible = "fsl,imx23-gpmi-nand",
-		.data = (void *)&gpmi_ids[IS_MX23],
+		.data = (void *)&gpmi_devdata_imx23,
 	}, {
 		.compatible = "fsl,imx28-gpmi-nand",
-		.data = (void *)&gpmi_ids[IS_MX28],
+		.data = (void *)&gpmi_devdata_imx28,
 	}, {
 		.compatible = "fsl,imx6q-gpmi-nand",
-		.data = (void *)&gpmi_ids[IS_MX6Q],
+		.data = (void *)&gpmi_devdata_imx6q,
+	}, {
+		.compatible = "fsl,imx6sx-gpmi-nand",
+		.data = (void *)&gpmi_devdata_imx6sx,
 	}, {}
 };
 MODULE_DEVICE_TABLE(of, gpmi_nand_id_table);
@@ -1767,18 +1780,18 @@ static int gpmi_nand_probe(struct platform_device *pdev)
 	const struct of_device_id *of_id;
 	int ret;
 
+	this = devm_kzalloc(&pdev->dev, sizeof(*this), GFP_KERNEL);
+	if (!this)
+		return -ENOMEM;
+
 	of_id = of_match_device(gpmi_nand_id_table, &pdev->dev);
 	if (of_id) {
-		pdev->id_entry = of_id->data;
+		this->devdata = of_id->data;
 	} else {
 		dev_err(&pdev->dev, "Failed to find the right device id.\n");
 		return -ENODEV;
 	}
 
-	this = devm_kzalloc(&pdev->dev, sizeof(*this), GFP_KERNEL);
-	if (!this)
-		return -ENOMEM;
-
 	platform_set_drvdata(pdev, this);
 	this->pdev  = pdev;
 	this->dev   = &pdev->dev;
@@ -1823,7 +1836,6 @@ static struct platform_driver gpmi_nand_driver = {
 	},
 	.probe   = gpmi_nand_probe,
 	.remove  = gpmi_nand_remove,
-	.id_table = gpmi_ids,
 };
 module_platform_driver(gpmi_nand_driver);
 
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-nand.h b/drivers/mtd/nand/gpmi-nand/gpmi-nand.h
index 4c801fa1872..32c6ba49f98 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-nand.h
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-nand.h
@@ -119,11 +119,25 @@ struct nand_timing {
 	int8_t  tRHOH_in_ns;
 };
 
+enum gpmi_type {
+	IS_MX23,
+	IS_MX28,
+	IS_MX6Q,
+	IS_MX6SX
+};
+
+struct gpmi_devdata {
+	enum gpmi_type type;
+	int bch_max_ecc_strength;
+	int max_chain_delay; /* See the async EDO mode */
+};
+
 struct gpmi_nand_data {
 	/* flags */
 #define GPMI_ASYNC_EDO_ENABLED	(1 << 0)
 #define GPMI_TIMING_INIT_OK	(1 << 1)
 	int			flags;
+	const struct gpmi_devdata *devdata;
 
 	/* System Interface */
 	struct device		*dev;
@@ -281,15 +295,11 @@ extern int gpmi_read_page(struct gpmi_nand_data *,
 #define STATUS_ERASED		0xff
 #define STATUS_UNCORRECTABLE	0xfe
 
-/* BCH's bit correction capability. */
-#define MXS_ECC_STRENGTH_MAX	20	/* mx23 and mx28 */
-#define MX6_ECC_STRENGTH_MAX	40
-
-/* Use the platform_id to distinguish different Archs. */
-#define IS_MX23			0x0
-#define IS_MX28			0x1
-#define IS_MX6Q			0x2
-#define GPMI_IS_MX23(x)		((x)->pdev->id_entry->driver_data == IS_MX23)
-#define GPMI_IS_MX28(x)		((x)->pdev->id_entry->driver_data == IS_MX28)
-#define GPMI_IS_MX6Q(x)		((x)->pdev->id_entry->driver_data == IS_MX6Q)
+/* Use the devdata to distinguish different Archs. */
+#define GPMI_IS_MX23(x)		((x)->devdata->type == IS_MX23)
+#define GPMI_IS_MX28(x)		((x)->devdata->type == IS_MX28)
+#define GPMI_IS_MX6Q(x)		((x)->devdata->type == IS_MX6Q)
+#define GPMI_IS_MX6SX(x)	((x)->devdata->type == IS_MX6SX)
+
+#define GPMI_IS_MX6(x)		(GPMI_IS_MX6Q(x) || GPMI_IS_MX6SX(x))
 #endif
diff --git a/drivers/mtd/nand/nand_base.c b/drivers/mtd/nand/nand_base.c
index 9d01c4df838..41167e9e991 100644
--- a/drivers/mtd/nand/nand_base.c
+++ b/drivers/mtd/nand/nand_base.c
@@ -37,6 +37,7 @@
 #include <linux/err.h>
 #include <linux/sched.h>
 #include <linux/slab.h>
+#include <linux/mm.h>
 #include <linux/types.h>
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/nand.h>
@@ -1204,8 +1205,7 @@ static int nand_read_subpage(struct mtd_info *mtd, struct nand_chip *chip,
 	 * ecc.pos. Let's make sure that there are no gaps in ECC positions.
 	 */
 	for (i = 0; i < eccfrag_len - 1; i++) {
-		if (eccpos[i + start_step * chip->ecc.bytes] + 1 !=
-			eccpos[i + start_step * chip->ecc.bytes + 1]) {
+		if (eccpos[i + index] + 1 != eccpos[i + index + 1]) {
 			gaps = 1;
 			break;
 		}
@@ -1501,6 +1501,7 @@ static int nand_do_read_ops(struct mtd_info *mtd, loff_t from,
 		mtd->oobavail : mtd->oobsize;
 
 	uint8_t *bufpoi, *oob, *buf;
+	int use_bufpoi;
 	unsigned int max_bitflips = 0;
 	int retry_mode = 0;
 	bool ecc_fail = false;
@@ -1523,9 +1524,20 @@ static int nand_do_read_ops(struct mtd_info *mtd, loff_t from,
 		bytes = min(mtd->writesize - col, readlen);
 		aligned = (bytes == mtd->writesize);
 
+		if (!aligned)
+			use_bufpoi = 1;
+		else if (chip->options & NAND_USE_BOUNCE_BUFFER)
+			use_bufpoi = !virt_addr_valid(buf);
+		else
+			use_bufpoi = 0;
+
 		/* Is the current page in the buffer? */
 		if (realpage != chip->pagebuf || oob) {
-			bufpoi = aligned ? buf : chip->buffers->databuf;
+			bufpoi = use_bufpoi ? chip->buffers->databuf : buf;
+
+			if (use_bufpoi && aligned)
+				pr_debug("%s: using read bounce buffer for buf@%p\n",
+						 __func__, buf);
 
 read_retry:
 			chip->cmdfunc(mtd, NAND_CMD_READ0, 0x00, page);
@@ -1547,7 +1559,7 @@ read_retry:
 				ret = chip->ecc.read_page(mtd, chip, bufpoi,
 							  oob_required, page);
 			if (ret < 0) {
-				if (!aligned)
+				if (use_bufpoi)
 					/* Invalidate page cache */
 					chip->pagebuf = -1;
 				break;
@@ -1556,7 +1568,7 @@ read_retry:
 			max_bitflips = max_t(unsigned int, max_bitflips, ret);
 
 			/* Transfer not aligned data */
-			if (!aligned) {
+			if (use_bufpoi) {
 				if (!NAND_HAS_SUBPAGE_READ(chip) && !oob &&
 				    !(mtd->ecc_stats.failed - ecc_failures) &&
 				    (ops->mode != MTD_OPS_RAW)) {
@@ -2376,11 +2388,23 @@ static int nand_do_write_ops(struct mtd_info *mtd, loff_t to,
 		int bytes = mtd->writesize;
 		int cached = writelen > bytes && page != blockmask;
 		uint8_t *wbuf = buf;
+		int use_bufpoi;
+		int part_pagewr = (column || writelen < (mtd->writesize - 1));
+
+		if (part_pagewr)
+			use_bufpoi = 1;
+		else if (chip->options & NAND_USE_BOUNCE_BUFFER)
+			use_bufpoi = !virt_addr_valid(buf);
+		else
+			use_bufpoi = 0;
 
-		/* Partial page write? */
-		if (unlikely(column || writelen < (mtd->writesize - 1))) {
+		/* Partial page write?, or need to use bounce buffer */
+		if (use_bufpoi) {
+			pr_debug("%s: using write bounce buffer for buf@%p\n",
+					 __func__, buf);
 			cached = 0;
-			bytes = min_t(int, bytes - column, (int) writelen);
+			if (part_pagewr)
+				bytes = min_t(int, bytes - column, writelen);
 			chip->pagebuf = -1;
 			memset(chip->buffers->databuf, 0xff, mtd->writesize);
 			memcpy(&chip->buffers->databuf[column], buf, bytes);
@@ -2618,18 +2642,20 @@ out:
 }
 
 /**
- * single_erase_cmd - [GENERIC] NAND standard block erase command function
+ * single_erase - [GENERIC] NAND standard block erase command function
  * @mtd: MTD device structure
  * @page: the page address of the block which will be erased
  *
- * Standard erase command for NAND chips.
+ * Standard erase command for NAND chips. Returns NAND status.
  */
-static void single_erase_cmd(struct mtd_info *mtd, int page)
+static int single_erase(struct mtd_info *mtd, int page)
 {
 	struct nand_chip *chip = mtd->priv;
 	/* Send commands to erase a block */
 	chip->cmdfunc(mtd, NAND_CMD_ERASE1, -1, page);
 	chip->cmdfunc(mtd, NAND_CMD_ERASE2, -1, -1);
+
+	return chip->waitfunc(mtd, chip);
 }
 
 /**
@@ -2710,9 +2736,7 @@ int nand_erase_nand(struct mtd_info *mtd, struct erase_info *instr,
 		    (page + pages_per_block))
 			chip->pagebuf = -1;
 
-		chip->erase_cmd(mtd, page & chip->pagemask);
-
-		status = chip->waitfunc(mtd, chip);
+		status = chip->erase(mtd, page & chip->pagemask);
 
 		/*
 		 * See if operation failed and additional status checks are
@@ -3607,7 +3631,7 @@ static struct nand_flash_dev *nand_get_flash_type(struct mtd_info *mtd,
 
 	chip->onfi_version = 0;
 	if (!type->name || !type->pagesize) {
-		/* Check is chip is ONFI compliant */
+		/* Check if the chip is ONFI compliant */
 		if (nand_flash_detect_onfi(mtd, chip, &busw))
 			goto ident_done;
 
@@ -3685,7 +3709,7 @@ ident_done:
 	}
 
 	chip->badblockbits = 8;
-	chip->erase_cmd = single_erase_cmd;
+	chip->erase = single_erase;
 
 	/* Do not replace user supplied command function! */
 	if (mtd->writesize > 512 && chip->cmdfunc == nand_command)
@@ -3770,6 +3794,39 @@ int nand_scan_ident(struct mtd_info *mtd, int maxchips,
 }
 EXPORT_SYMBOL(nand_scan_ident);
 
+/*
+ * Check if the chip configuration meet the datasheet requirements.
+
+ * If our configuration corrects A bits per B bytes and the minimum
+ * required correction level is X bits per Y bytes, then we must ensure
+ * both of the following are true:
+ *
+ * (1) A / B >= X / Y
+ * (2) A >= X
+ *
+ * Requirement (1) ensures we can correct for the required bitflip density.
+ * Requirement (2) ensures we can correct even when all bitflips are clumped
+ * in the same sector.
+ */
+static bool nand_ecc_strength_good(struct mtd_info *mtd)
+{
+	struct nand_chip *chip = mtd->priv;
+	struct nand_ecc_ctrl *ecc = &chip->ecc;
+	int corr, ds_corr;
+
+	if (ecc->size == 0 || chip->ecc_step_ds == 0)
+		/* Not enough information */
+		return true;
+
+	/*
+	 * We get the number of corrected bits per page to compare
+	 * the correction density.
+	 */
+	corr = (mtd->writesize * ecc->strength) / ecc->size;
+	ds_corr = (mtd->writesize * chip->ecc_strength_ds) / chip->ecc_step_ds;
+
+	return corr >= ds_corr && ecc->strength >= chip->ecc_strength_ds;
+}
 
 /**
  * nand_scan_tail - [NAND Interface] Scan for the NAND device
@@ -3990,6 +4047,9 @@ int nand_scan_tail(struct mtd_info *mtd)
 		ecc->layout->oobavail += ecc->layout->oobfree[i].length;
 	mtd->oobavail = ecc->layout->oobavail;
 
+	/* ECC sanity check: warn noisily if it's too weak */
+	WARN_ON(!nand_ecc_strength_good(mtd));
+
 	/*
 	 * Set the number of read / write steps for one page depending on ECC
 	 * mode.
@@ -4023,8 +4083,16 @@ int nand_scan_tail(struct mtd_info *mtd)
 	chip->pagebuf = -1;
 
 	/* Large page NAND with SOFT_ECC should support subpage reads */
-	if ((ecc->mode == NAND_ECC_SOFT) && (chip->page_shift > 9))
-		chip->options |= NAND_SUBPAGE_READ;
+	switch (ecc->mode) {
+	case NAND_ECC_SOFT:
+	case NAND_ECC_SOFT_BCH:
+		if (chip->page_shift > 9)
+			chip->options |= NAND_SUBPAGE_READ;
+		break;
+
+	default:
+		break;
+	}
 
 	/* Fill in remaining MTD driver data */
 	mtd->type = nand_is_slc(chip) ? MTD_NANDFLASH : MTD_MLCNANDFLASH;
diff --git a/drivers/mtd/nand/nand_bbt.c b/drivers/mtd/nand/nand_bbt.c
index c0615d1526f..7f0c3b4c2a4 100644
--- a/drivers/mtd/nand/nand_bbt.c
+++ b/drivers/mtd/nand/nand_bbt.c
@@ -528,7 +528,7 @@ static int search_bbt(struct mtd_info *mtd, uint8_t *buf, struct nand_bbt_descr
 {
 	struct nand_chip *this = mtd->priv;
 	int i, chips;
-	int bits, startblock, block, dir;
+	int startblock, block, dir;
 	int scanlen = mtd->writesize + mtd->oobsize;
 	int bbtblocks;
 	int blocktopage = this->bbt_erase_shift - this->page_shift;
@@ -552,9 +552,6 @@ static int search_bbt(struct mtd_info *mtd, uint8_t *buf, struct nand_bbt_descr
 		bbtblocks = mtd->size >> this->bbt_erase_shift;
 	}
 
-	/* Number of bits for each erase block in the bbt */
-	bits = td->options & NAND_BBT_NRBITS_MSK;
-
 	for (i = 0; i < chips; i++) {
 		/* Reset version information */
 		td->version[i] = 0;
@@ -1285,6 +1282,7 @@ static int nand_create_badblock_pattern(struct nand_chip *this)
 int nand_default_bbt(struct mtd_info *mtd)
 {
 	struct nand_chip *this = mtd->priv;
+	int ret;
 
 	/* Is a flash based bad block table requested? */
 	if (this->bbt_options & NAND_BBT_USE_FLASH) {
@@ -1303,8 +1301,11 @@ int nand_default_bbt(struct mtd_info *mtd)
 		this->bbt_md = NULL;
 	}
 
-	if (!this->badblock_pattern)
-		nand_create_badblock_pattern(this);
+	if (!this->badblock_pattern) {
+		ret = nand_create_badblock_pattern(this);
+		if (ret)
+			return ret;
+	}
 
 	return nand_scan_bbt(mtd, this->badblock_pattern);
 }
diff --git a/drivers/mtd/nand/nand_ecc.c b/drivers/mtd/nand/nand_ecc.c
index 053c9a2d47c..97c4c0216c9 100644
--- a/drivers/mtd/nand/nand_ecc.c
+++ b/drivers/mtd/nand/nand_ecc.c
@@ -506,7 +506,7 @@ int __nand_correct_data(unsigned char *buf,
 	if ((bitsperbyte[b0] + bitsperbyte[b1] + bitsperbyte[b2]) == 1)
 		return 1;	/* error in ECC data; no action needed */
 
-	pr_err("%s: uncorrectable ECC error", __func__);
+	pr_err("%s: uncorrectable ECC error\n", __func__);
 	return -1;
 }
 EXPORT_SYMBOL(__nand_correct_data);
diff --git a/drivers/mtd/nand/omap2.c b/drivers/mtd/nand/omap2.c
index 1ff49b80bda..f0ed92e210a 100644
--- a/drivers/mtd/nand/omap2.c
+++ b/drivers/mtd/nand/omap2.c
@@ -137,6 +137,10 @@
 #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,
+				0x07, 0x0e};
 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};
@@ -1114,6 +1118,19 @@ static void __maybe_unused omap_enable_hwecc_bch(struct mtd_info *mtd, int mode)
 			ecc_size1 = BCH_ECC_SIZE1;
 		}
 		break;
+	case OMAP_ECC_BCH16_CODE_HW:
+		bch_type = 0x2;
+		nsectors = chip->ecc.steps;
+		if (mode == NAND_ECC_READ) {
+			wr_mode	  = 0x01;
+			ecc_size0 = 52; /* ECC bits in nibbles per sector */
+			ecc_size1 = 0;  /* non-ECC bits in nibbles per sector */
+		} else {
+			wr_mode	  = 0x01;
+			ecc_size0 = 0;  /* extra bits in nibbles per sector */
+			ecc_size1 = 52; /* OOB bits in nibbles per sector */
+		}
+		break;
 	default:
 		return;
 	}
@@ -1162,7 +1179,8 @@ static int __maybe_unused omap_calculate_ecc_bch(struct mtd_info *mtd,
 	struct gpmc_nand_regs	*gpmc_regs = &info->reg;
 	u8 *ecc_code;
 	unsigned long nsectors, bch_val1, bch_val2, bch_val3, bch_val4;
-	int i;
+	u32 val;
+	int i, j;
 
 	nsectors = ((readl(info->reg.gpmc_ecc_config) >> 4) & 0x7) + 1;
 	for (i = 0; i < nsectors; i++) {
@@ -1201,6 +1219,41 @@ static int __maybe_unused omap_calculate_ecc_bch(struct mtd_info *mtd,
 			*ecc_code++ = ((bch_val1 >> 4) & 0xFF);
 			*ecc_code++ = ((bch_val1 & 0xF) << 4);
 			break;
+		case OMAP_ECC_BCH16_CODE_HW:
+			val = readl(gpmc_regs->gpmc_bch_result6[i]);
+			ecc_code[0]  = ((val >>  8) & 0xFF);
+			ecc_code[1]  = ((val >>  0) & 0xFF);
+			val = readl(gpmc_regs->gpmc_bch_result5[i]);
+			ecc_code[2]  = ((val >> 24) & 0xFF);
+			ecc_code[3]  = ((val >> 16) & 0xFF);
+			ecc_code[4]  = ((val >>  8) & 0xFF);
+			ecc_code[5]  = ((val >>  0) & 0xFF);
+			val = readl(gpmc_regs->gpmc_bch_result4[i]);
+			ecc_code[6]  = ((val >> 24) & 0xFF);
+			ecc_code[7]  = ((val >> 16) & 0xFF);
+			ecc_code[8]  = ((val >>  8) & 0xFF);
+			ecc_code[9]  = ((val >>  0) & 0xFF);
+			val = readl(gpmc_regs->gpmc_bch_result3[i]);
+			ecc_code[10] = ((val >> 24) & 0xFF);
+			ecc_code[11] = ((val >> 16) & 0xFF);
+			ecc_code[12] = ((val >>  8) & 0xFF);
+			ecc_code[13] = ((val >>  0) & 0xFF);
+			val = readl(gpmc_regs->gpmc_bch_result2[i]);
+			ecc_code[14] = ((val >> 24) & 0xFF);
+			ecc_code[15] = ((val >> 16) & 0xFF);
+			ecc_code[16] = ((val >>  8) & 0xFF);
+			ecc_code[17] = ((val >>  0) & 0xFF);
+			val = readl(gpmc_regs->gpmc_bch_result1[i]);
+			ecc_code[18] = ((val >> 24) & 0xFF);
+			ecc_code[19] = ((val >> 16) & 0xFF);
+			ecc_code[20] = ((val >>  8) & 0xFF);
+			ecc_code[21] = ((val >>  0) & 0xFF);
+			val = readl(gpmc_regs->gpmc_bch_result0[i]);
+			ecc_code[22] = ((val >> 24) & 0xFF);
+			ecc_code[23] = ((val >> 16) & 0xFF);
+			ecc_code[24] = ((val >>  8) & 0xFF);
+			ecc_code[25] = ((val >>  0) & 0xFF);
+			break;
 		default:
 			return -EINVAL;
 		}
@@ -1210,8 +1263,8 @@ static int __maybe_unused omap_calculate_ecc_bch(struct mtd_info *mtd,
 		case OMAP_ECC_BCH4_CODE_HW_DETECTION_SW:
 			/* Add constant polynomial to remainder, so that
 			 * ECC of blank pages results in 0x0 on reading back */
-			for (i = 0; i < eccbytes; i++)
-				ecc_calc[i] ^= bch4_polynomial[i];
+			for (j = 0; j < eccbytes; j++)
+				ecc_calc[j] ^= bch4_polynomial[j];
 			break;
 		case OMAP_ECC_BCH4_CODE_HW:
 			/* Set  8th ECC byte as 0x0 for ROM compatibility */
@@ -1220,13 +1273,15 @@ static int __maybe_unused omap_calculate_ecc_bch(struct mtd_info *mtd,
 		case OMAP_ECC_BCH8_CODE_HW_DETECTION_SW:
 			/* Add constant polynomial to remainder, so that
 			 * ECC of blank pages results in 0x0 on reading back */
-			for (i = 0; i < eccbytes; i++)
-				ecc_calc[i] ^= bch8_polynomial[i];
+			for (j = 0; j < eccbytes; j++)
+				ecc_calc[j] ^= bch8_polynomial[j];
 			break;
 		case OMAP_ECC_BCH8_CODE_HW:
 			/* Set 14th ECC byte as 0x0 for ROM compatibility */
 			ecc_calc[eccbytes - 1] = 0x0;
 			break;
+		case OMAP_ECC_BCH16_CODE_HW:
+			break;
 		default:
 			return -EINVAL;
 		}
@@ -1237,6 +1292,7 @@ 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
@@ -1276,7 +1332,6 @@ static int erased_sector_bitflips(u_char *data, u_char *oob,
 	return flip_bits;
 }
 
-#ifdef CONFIG_MTD_NAND_OMAP_BCH
 /**
  * omap_elm_correct_data - corrects page data area in case error reported
  * @mtd:	MTD device structure
@@ -1318,6 +1373,10 @@ static int omap_elm_correct_data(struct mtd_info *mtd, u_char *data,
 		actual_eccbytes = ecc->bytes - 1;
 		erased_ecc_vec = bch8_vector;
 		break;
+	case OMAP_ECC_BCH16_CODE_HW:
+		actual_eccbytes = ecc->bytes;
+		erased_ecc_vec = bch16_vector;
+		break;
 	default:
 		pr_err("invalid driver configuration\n");
 		return -EINVAL;
@@ -1382,7 +1441,7 @@ static int omap_elm_correct_data(struct mtd_info *mtd, u_char *data,
 
 	/* Check if any error reported */
 	if (!is_error_reported)
-		return 0;
+		return stat;
 
 	/* Decode BCH error using ELM module */
 	elm_decode_bch_error_page(info->elm_dev, ecc_vec, err_vec);
@@ -1401,6 +1460,7 @@ static int omap_elm_correct_data(struct mtd_info *mtd, u_char *data,
 						BCH4_BIT_PAD;
 					break;
 				case OMAP_ECC_BCH8_CODE_HW:
+				case OMAP_ECC_BCH16_CODE_HW:
 					pos = err_vec[i].error_loc[j];
 					break;
 				default:
@@ -1912,6 +1972,40 @@ static int omap_nand_probe(struct platform_device *pdev)
 		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;
+		nand_chip->ecc.bytes		= 26;
+		nand_chip->ecc.strength		= 16;
+		nand_chip->ecc.hwctl		= omap_enable_hwecc_bch;
+		nand_chip->ecc.correct		= omap_elm_correct_data;
+		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");
+			goto return_error;
+		}
+		/* define ECC layout */
+		ecclayout->eccbytes		= nand_chip->ecc.bytes *
+							(mtd->writesize /
+							nand_chip->ecc.size);
+		oob_index			= BADBLOCK_MARKER_LENGTH;
+		for (i = 0; i < ecclayout->eccbytes; i++, oob_index++)
+			ecclayout->eccpos[i]	= oob_index;
+		/* reserved marker already included in ecclayout->eccbytes */
+		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");
 		err = -EINVAL;
diff --git a/drivers/mtd/nand/orion_nand.c b/drivers/mtd/nand/orion_nand.c
index dd7fe817eaf..471b4df3a5a 100644
--- a/drivers/mtd/nand/orion_nand.c
+++ b/drivers/mtd/nand/orion_nand.c
@@ -214,7 +214,7 @@ static int orion_nand_remove(struct platform_device *pdev)
 }
 
 #ifdef CONFIG_OF
-static struct of_device_id orion_nand_of_match_table[] = {
+static const struct of_device_id orion_nand_of_match_table[] = {
 	{ .compatible = "marvell,orion-nand", },
 	{},
 };
diff --git a/drivers/mtd/nand/pxa3xx_nand.c b/drivers/mtd/nand/pxa3xx_nand.c
index 7588fe2c127..96b0b1d27df 100644
--- a/drivers/mtd/nand/pxa3xx_nand.c
+++ b/drivers/mtd/nand/pxa3xx_nand.c
@@ -127,10 +127,10 @@
 
 /* macros for registers read/write */
 #define nand_writel(info, off, val)	\
-	__raw_writel((val), (info)->mmio_base + (off))
+	writel_relaxed((val), (info)->mmio_base + (off))
 
 #define nand_readl(info, off)		\
-	__raw_readl((info)->mmio_base + (off))
+	readl_relaxed((info)->mmio_base + (off))
 
 /* error code and state */
 enum {
@@ -337,7 +337,7 @@ static struct nand_ecclayout ecc_layout_4KB_bch8bit = {
 /* convert nano-seconds to nand flash controller clock cycles */
 #define ns2cycle(ns, clk)	(int)((ns) * (clk / 1000000) / 1000)
 
-static struct of_device_id pxa3xx_nand_dt_ids[] = {
+static const struct of_device_id pxa3xx_nand_dt_ids[] = {
 	{
 		.compatible = "marvell,pxa3xx-nand",
 		.data       = (void *)PXA3XX_NAND_VARIANT_PXA,
@@ -1354,7 +1354,6 @@ static int pxa_ecc_init(struct pxa3xx_nand_info *info,
 		ecc->mode = NAND_ECC_HW;
 		ecc->size = 512;
 		ecc->strength = 1;
-		return 1;
 
 	} else if (strength == 1 && ecc_stepsize == 512 && page_size == 512) {
 		info->chunk_size = 512;
@@ -1363,7 +1362,6 @@ static int pxa_ecc_init(struct pxa3xx_nand_info *info,
 		ecc->mode = NAND_ECC_HW;
 		ecc->size = 512;
 		ecc->strength = 1;
-		return 1;
 
 	/*
 	 * Required ECC: 4-bit correction per 512 bytes
@@ -1378,7 +1376,6 @@ static int pxa_ecc_init(struct pxa3xx_nand_info *info,
 		ecc->size = info->chunk_size;
 		ecc->layout = &ecc_layout_2KB_bch4bit;
 		ecc->strength = 16;
-		return 1;
 
 	} else if (strength == 4 && ecc_stepsize == 512 && page_size == 4096) {
 		info->ecc_bch = 1;
@@ -1389,7 +1386,6 @@ static int pxa_ecc_init(struct pxa3xx_nand_info *info,
 		ecc->size = info->chunk_size;
 		ecc->layout = &ecc_layout_4KB_bch4bit;
 		ecc->strength = 16;
-		return 1;
 
 	/*
 	 * Required ECC: 8-bit correction per 512 bytes
@@ -1404,8 +1400,15 @@ static int pxa_ecc_init(struct pxa3xx_nand_info *info,
 		ecc->size = info->chunk_size;
 		ecc->layout = &ecc_layout_4KB_bch8bit;
 		ecc->strength = 16;
-		return 1;
+	} else {
+		dev_err(&info->pdev->dev,
+			"ECC strength %d at page size %d is not supported\n",
+			strength, page_size);
+		return -ENODEV;
 	}
+
+	dev_info(&info->pdev->dev, "ECC strength %d, ECC step size %d\n",
+		 ecc->strength, ecc->size);
 	return 0;
 }
 
@@ -1516,8 +1519,13 @@ KEEP_CONFIG:
 		}
 	}
 
-	ecc_strength = chip->ecc_strength_ds;
-	ecc_step = chip->ecc_step_ds;
+	if (pdata->ecc_strength && pdata->ecc_step_size) {
+		ecc_strength = pdata->ecc_strength;
+		ecc_step = pdata->ecc_step_size;
+	} else {
+		ecc_strength = chip->ecc_strength_ds;
+		ecc_step = chip->ecc_step_ds;
+	}
 
 	/* Set default ECC strength requirements on non-ONFI devices */
 	if (ecc_strength < 1 && ecc_step < 1) {
@@ -1527,12 +1535,8 @@ KEEP_CONFIG:
 
 	ret = pxa_ecc_init(info, &chip->ecc, ecc_strength,
 			   ecc_step, mtd->writesize);
-	if (!ret) {
-		dev_err(&info->pdev->dev,
-			"ECC strength %d at page size %d is not supported\n",
-			ecc_strength, mtd->writesize);
-		return -ENODEV;
-	}
+	if (ret)
+		return ret;
 
 	/* calculate addressing information */
 	if (mtd->writesize >= 2048)
@@ -1730,6 +1734,14 @@ static int pxa3xx_nand_probe_dt(struct platform_device *pdev)
 	of_property_read_u32(np, "num-cs", &pdata->num_cs);
 	pdata->flash_bbt = of_get_nand_on_flash_bbt(np);
 
+	pdata->ecc_strength = of_get_nand_ecc_strength(np);
+	if (pdata->ecc_strength < 0)
+		pdata->ecc_strength = 0;
+
+	pdata->ecc_step_size = of_get_nand_ecc_step_size(np);
+	if (pdata->ecc_step_size < 0)
+		pdata->ecc_step_size = 0;
+
 	pdev->dev.platform_data = pdata;
 
 	return 0;
diff --git a/drivers/mtd/nand/r852.c b/drivers/mtd/nand/r852.c
index 325930db3f0..baea83f4dea 100644
--- a/drivers/mtd/nand/r852.c
+++ b/drivers/mtd/nand/r852.c
@@ -245,7 +245,7 @@ static void r852_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
 	}
 
 	/* write DWORD chinks - faster */
-	while (len) {
+	while (len >= 4) {
 		reg = buf[0] | buf[1] << 8 | buf[2] << 16 | buf[3] << 24;
 		r852_write_reg_dword(dev, R852_DATALINE, reg);
 		buf += 4;
@@ -254,8 +254,10 @@ static void r852_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
 	}
 
 	/* write rest */
-	while (len)
+	while (len > 0) {
 		r852_write_reg(dev, R852_DATALINE, *buf++);
+		len--;
+	}
 }
 
 /*
diff --git a/drivers/mtd/onenand/samsung.c b/drivers/mtd/onenand/samsung.c
index b1a792fd1c2..efb819c3df2 100644
--- a/drivers/mtd/onenand/samsung.c
+++ b/drivers/mtd/onenand/samsung.c
@@ -537,9 +537,9 @@ static int onenand_write_bufferram(struct mtd_info *mtd, int area,
 	return 0;
 }
 
-static int (*s5pc110_dma_ops)(void *dst, void *src, size_t count, int direction);
+static int (*s5pc110_dma_ops)(dma_addr_t dst, dma_addr_t src, size_t count, int direction);
 
-static int s5pc110_dma_poll(void *dst, void *src, size_t count, int direction)
+static int s5pc110_dma_poll(dma_addr_t dst, dma_addr_t src, size_t count, int direction)
 {
 	void __iomem *base = onenand->dma_addr;
 	int status;
@@ -605,7 +605,7 @@ static irqreturn_t s5pc110_onenand_irq(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
-static int s5pc110_dma_irq(void *dst, void *src, size_t count, int direction)
+static int s5pc110_dma_irq(dma_addr_t dst, dma_addr_t src, size_t count, int direction)
 {
 	void __iomem *base = onenand->dma_addr;
 	int status;
@@ -686,7 +686,7 @@ static int s5pc110_read_bufferram(struct mtd_info *mtd, int area,
 		dev_err(dev, "Couldn't map a %d byte buffer for DMA\n", count);
 		goto normal;
 	}
-	err = s5pc110_dma_ops((void *) dma_dst, (void *) dma_src,
+	err = s5pc110_dma_ops(dma_dst, dma_src,
 			count, S5PC110_DMA_DIR_READ);
 
 	if (page_dma)
diff --git a/drivers/mtd/spi-nor/Kconfig b/drivers/mtd/spi-nor/Kconfig
new file mode 100644
index 00000000000..f8acfa4310e
--- /dev/null
+++ b/drivers/mtd/spi-nor/Kconfig
@@ -0,0 +1,17 @@
+menuconfig MTD_SPI_NOR
+	tristate "SPI-NOR device support"
+	depends on MTD
+	help
+	  This is the framework for the SPI NOR which can be used by the SPI
+	  device drivers and the SPI-NOR device driver.
+
+if MTD_SPI_NOR
+
+config SPI_FSL_QUADSPI
+	tristate "Freescale Quad SPI controller"
+	depends on ARCH_MXC
+	help
+	  This enables support for the Quad SPI controller in master mode.
+	  We only connect the NOR to this controller now.
+
+endif # MTD_SPI_NOR
diff --git a/drivers/mtd/spi-nor/Makefile b/drivers/mtd/spi-nor/Makefile
new file mode 100644
index 00000000000..6a7ce146224
--- /dev/null
+++ b/drivers/mtd/spi-nor/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_MTD_SPI_NOR)	+= spi-nor.o
+obj-$(CONFIG_SPI_FSL_QUADSPI)	+= fsl-quadspi.o
diff --git a/drivers/mtd/spi-nor/fsl-quadspi.c b/drivers/mtd/spi-nor/fsl-quadspi.c
new file mode 100644
index 00000000000..8d659a2888d
--- /dev/null
+++ b/drivers/mtd/spi-nor/fsl-quadspi.c
@@ -0,0 +1,1009 @@
+/*
+ * Freescale QuadSPI driver.
+ *
+ * Copyright (C) 2013 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.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/errno.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/timer.h>
+#include <linux/jiffies.h>
+#include <linux/completion.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/partitions.h>
+#include <linux/mtd/spi-nor.h>
+
+/* The registers */
+#define QUADSPI_MCR			0x00
+#define QUADSPI_MCR_RESERVED_SHIFT	16
+#define QUADSPI_MCR_RESERVED_MASK	(0xF << QUADSPI_MCR_RESERVED_SHIFT)
+#define QUADSPI_MCR_MDIS_SHIFT		14
+#define QUADSPI_MCR_MDIS_MASK		(1 << QUADSPI_MCR_MDIS_SHIFT)
+#define QUADSPI_MCR_CLR_TXF_SHIFT	11
+#define QUADSPI_MCR_CLR_TXF_MASK	(1 << QUADSPI_MCR_CLR_TXF_SHIFT)
+#define QUADSPI_MCR_CLR_RXF_SHIFT	10
+#define QUADSPI_MCR_CLR_RXF_MASK	(1 << QUADSPI_MCR_CLR_RXF_SHIFT)
+#define QUADSPI_MCR_DDR_EN_SHIFT	7
+#define QUADSPI_MCR_DDR_EN_MASK		(1 << QUADSPI_MCR_DDR_EN_SHIFT)
+#define QUADSPI_MCR_END_CFG_SHIFT	2
+#define QUADSPI_MCR_END_CFG_MASK	(3 << QUADSPI_MCR_END_CFG_SHIFT)
+#define QUADSPI_MCR_SWRSTHD_SHIFT	1
+#define QUADSPI_MCR_SWRSTHD_MASK	(1 << QUADSPI_MCR_SWRSTHD_SHIFT)
+#define QUADSPI_MCR_SWRSTSD_SHIFT	0
+#define QUADSPI_MCR_SWRSTSD_MASK	(1 << QUADSPI_MCR_SWRSTSD_SHIFT)
+
+#define QUADSPI_IPCR			0x08
+#define QUADSPI_IPCR_SEQID_SHIFT	24
+#define QUADSPI_IPCR_SEQID_MASK		(0xF << QUADSPI_IPCR_SEQID_SHIFT)
+
+#define QUADSPI_BUF0CR			0x10
+#define QUADSPI_BUF1CR			0x14
+#define QUADSPI_BUF2CR			0x18
+#define QUADSPI_BUFXCR_INVALID_MSTRID	0xe
+
+#define QUADSPI_BUF3CR			0x1c
+#define QUADSPI_BUF3CR_ALLMST_SHIFT	31
+#define QUADSPI_BUF3CR_ALLMST		(1 << QUADSPI_BUF3CR_ALLMST_SHIFT)
+
+#define QUADSPI_BFGENCR			0x20
+#define QUADSPI_BFGENCR_PAR_EN_SHIFT	16
+#define QUADSPI_BFGENCR_PAR_EN_MASK	(1 << (QUADSPI_BFGENCR_PAR_EN_SHIFT))
+#define QUADSPI_BFGENCR_SEQID_SHIFT	12
+#define QUADSPI_BFGENCR_SEQID_MASK	(0xF << QUADSPI_BFGENCR_SEQID_SHIFT)
+
+#define QUADSPI_BUF0IND			0x30
+#define QUADSPI_BUF1IND			0x34
+#define QUADSPI_BUF2IND			0x38
+#define QUADSPI_SFAR			0x100
+
+#define QUADSPI_SMPR			0x108
+#define QUADSPI_SMPR_DDRSMP_SHIFT	16
+#define QUADSPI_SMPR_DDRSMP_MASK	(7 << QUADSPI_SMPR_DDRSMP_SHIFT)
+#define QUADSPI_SMPR_FSDLY_SHIFT	6
+#define QUADSPI_SMPR_FSDLY_MASK		(1 << QUADSPI_SMPR_FSDLY_SHIFT)
+#define QUADSPI_SMPR_FSPHS_SHIFT	5
+#define QUADSPI_SMPR_FSPHS_MASK		(1 << QUADSPI_SMPR_FSPHS_SHIFT)
+#define QUADSPI_SMPR_HSENA_SHIFT	0
+#define QUADSPI_SMPR_HSENA_MASK		(1 << QUADSPI_SMPR_HSENA_SHIFT)
+
+#define QUADSPI_RBSR			0x10c
+#define QUADSPI_RBSR_RDBFL_SHIFT	8
+#define QUADSPI_RBSR_RDBFL_MASK		(0x3F << QUADSPI_RBSR_RDBFL_SHIFT)
+
+#define QUADSPI_RBCT			0x110
+#define QUADSPI_RBCT_WMRK_MASK		0x1F
+#define QUADSPI_RBCT_RXBRD_SHIFT	8
+#define QUADSPI_RBCT_RXBRD_USEIPS	(0x1 << QUADSPI_RBCT_RXBRD_SHIFT)
+
+#define QUADSPI_TBSR			0x150
+#define QUADSPI_TBDR			0x154
+#define QUADSPI_SR			0x15c
+#define QUADSPI_SR_IP_ACC_SHIFT		1
+#define QUADSPI_SR_IP_ACC_MASK		(0x1 << QUADSPI_SR_IP_ACC_SHIFT)
+#define QUADSPI_SR_AHB_ACC_SHIFT	2
+#define QUADSPI_SR_AHB_ACC_MASK		(0x1 << QUADSPI_SR_AHB_ACC_SHIFT)
+
+#define QUADSPI_FR			0x160
+#define QUADSPI_FR_TFF_MASK		0x1
+
+#define QUADSPI_SFA1AD			0x180
+#define QUADSPI_SFA2AD			0x184
+#define QUADSPI_SFB1AD			0x188
+#define QUADSPI_SFB2AD			0x18c
+#define QUADSPI_RBDR			0x200
+
+#define QUADSPI_LUTKEY			0x300
+#define QUADSPI_LUTKEY_VALUE		0x5AF05AF0
+
+#define QUADSPI_LCKCR			0x304
+#define QUADSPI_LCKER_LOCK		0x1
+#define QUADSPI_LCKER_UNLOCK		0x2
+
+#define QUADSPI_RSER			0x164
+#define QUADSPI_RSER_TFIE		(0x1 << 0)
+
+#define QUADSPI_LUT_BASE		0x310
+
+/*
+ * The definition of the LUT register shows below:
+ *
+ *  ---------------------------------------------------
+ *  | INSTR1 | PAD1 | OPRND1 | INSTR0 | PAD0 | OPRND0 |
+ *  ---------------------------------------------------
+ */
+#define OPRND0_SHIFT		0
+#define PAD0_SHIFT		8
+#define INSTR0_SHIFT		10
+#define OPRND1_SHIFT		16
+
+/* Instruction set for the LUT register. */
+#define LUT_STOP		0
+#define LUT_CMD			1
+#define LUT_ADDR		2
+#define LUT_DUMMY		3
+#define LUT_MODE		4
+#define LUT_MODE2		5
+#define LUT_MODE4		6
+#define LUT_READ		7
+#define LUT_WRITE		8
+#define LUT_JMP_ON_CS		9
+#define LUT_ADDR_DDR		10
+#define LUT_MODE_DDR		11
+#define LUT_MODE2_DDR		12
+#define LUT_MODE4_DDR		13
+#define LUT_READ_DDR		14
+#define LUT_WRITE_DDR		15
+#define LUT_DATA_LEARN		16
+
+/*
+ * The PAD definitions for LUT register.
+ *
+ * The pad stands for the lines number of IO[0:3].
+ * For example, the Quad read need four IO lines, so you should
+ * set LUT_PAD4 which means we use four IO lines.
+ */
+#define LUT_PAD1		0
+#define LUT_PAD2		1
+#define LUT_PAD4		2
+
+/* Oprands for the LUT register. */
+#define ADDR24BIT		0x18
+#define ADDR32BIT		0x20
+
+/* Macros for constructing the LUT register. */
+#define LUT0(ins, pad, opr)						\
+		(((opr) << OPRND0_SHIFT) | ((LUT_##pad) << PAD0_SHIFT) | \
+		((LUT_##ins) << INSTR0_SHIFT))
+
+#define LUT1(ins, pad, opr)	(LUT0(ins, pad, opr) << OPRND1_SHIFT)
+
+/* other macros for LUT register. */
+#define QUADSPI_LUT(x)          (QUADSPI_LUT_BASE + (x) * 4)
+#define QUADSPI_LUT_NUM		64
+
+/* SEQID -- we can have 16 seqids at most. */
+#define SEQID_QUAD_READ		0
+#define SEQID_WREN		1
+#define SEQID_WRDI		2
+#define SEQID_RDSR		3
+#define SEQID_SE		4
+#define SEQID_CHIP_ERASE	5
+#define SEQID_PP		6
+#define SEQID_RDID		7
+#define SEQID_WRSR		8
+#define SEQID_RDCR		9
+#define SEQID_EN4B		10
+#define SEQID_BRWR		11
+
+enum fsl_qspi_devtype {
+	FSL_QUADSPI_VYBRID,
+	FSL_QUADSPI_IMX6SX,
+};
+
+struct fsl_qspi_devtype_data {
+	enum fsl_qspi_devtype devtype;
+	int rxfifo;
+	int txfifo;
+};
+
+static struct fsl_qspi_devtype_data vybrid_data = {
+	.devtype = FSL_QUADSPI_VYBRID,
+	.rxfifo = 128,
+	.txfifo = 64
+};
+
+static struct fsl_qspi_devtype_data imx6sx_data = {
+	.devtype = FSL_QUADSPI_IMX6SX,
+	.rxfifo = 128,
+	.txfifo = 512
+};
+
+#define FSL_QSPI_MAX_CHIP	4
+struct fsl_qspi {
+	struct mtd_info mtd[FSL_QSPI_MAX_CHIP];
+	struct spi_nor nor[FSL_QSPI_MAX_CHIP];
+	void __iomem *iobase;
+	void __iomem *ahb_base; /* Used when read from AHB bus */
+	u32 memmap_phy;
+	struct clk *clk, *clk_en;
+	struct device *dev;
+	struct completion c;
+	struct fsl_qspi_devtype_data *devtype_data;
+	u32 nor_size;
+	u32 nor_num;
+	u32 clk_rate;
+	unsigned int chip_base_addr; /* We may support two chips. */
+};
+
+static inline int is_vybrid_qspi(struct fsl_qspi *q)
+{
+	return q->devtype_data->devtype == FSL_QUADSPI_VYBRID;
+}
+
+static inline int is_imx6sx_qspi(struct fsl_qspi *q)
+{
+	return q->devtype_data->devtype == FSL_QUADSPI_IMX6SX;
+}
+
+/*
+ * An IC bug makes us to re-arrange the 32-bit data.
+ * The following chips, such as IMX6SLX, have fixed this bug.
+ */
+static inline u32 fsl_qspi_endian_xchg(struct fsl_qspi *q, u32 a)
+{
+	return is_vybrid_qspi(q) ? __swab32(a) : a;
+}
+
+static inline void fsl_qspi_unlock_lut(struct fsl_qspi *q)
+{
+	writel(QUADSPI_LUTKEY_VALUE, q->iobase + QUADSPI_LUTKEY);
+	writel(QUADSPI_LCKER_UNLOCK, q->iobase + QUADSPI_LCKCR);
+}
+
+static inline void fsl_qspi_lock_lut(struct fsl_qspi *q)
+{
+	writel(QUADSPI_LUTKEY_VALUE, q->iobase + QUADSPI_LUTKEY);
+	writel(QUADSPI_LCKER_LOCK, q->iobase + QUADSPI_LCKCR);
+}
+
+static irqreturn_t fsl_qspi_irq_handler(int irq, void *dev_id)
+{
+	struct fsl_qspi *q = dev_id;
+	u32 reg;
+
+	/* clear interrupt */
+	reg = readl(q->iobase + QUADSPI_FR);
+	writel(reg, q->iobase + QUADSPI_FR);
+
+	if (reg & QUADSPI_FR_TFF_MASK)
+		complete(&q->c);
+
+	dev_dbg(q->dev, "QUADSPI_FR : 0x%.8x:0x%.8x\n", q->chip_base_addr, reg);
+	return IRQ_HANDLED;
+}
+
+static void fsl_qspi_init_lut(struct fsl_qspi *q)
+{
+	void __iomem *base = q->iobase;
+	int rxfifo = q->devtype_data->rxfifo;
+	u32 lut_base;
+	u8 cmd, addrlen, dummy;
+	int i;
+
+	fsl_qspi_unlock_lut(q);
+
+	/* Clear all the LUT table */
+	for (i = 0; i < QUADSPI_LUT_NUM; i++)
+		writel(0, base + QUADSPI_LUT_BASE + i * 4);
+
+	/* Quad Read */
+	lut_base = SEQID_QUAD_READ * 4;
+
+	if (q->nor_size <= SZ_16M) {
+		cmd = SPINOR_OP_READ_1_1_4;
+		addrlen = ADDR24BIT;
+		dummy = 8;
+	} else {
+		/* use the 4-byte address */
+		cmd = SPINOR_OP_READ_1_1_4;
+		addrlen = ADDR32BIT;
+		dummy = 8;
+	}
+
+	writel(LUT0(CMD, PAD1, cmd) | LUT1(ADDR, PAD1, addrlen),
+			base + QUADSPI_LUT(lut_base));
+	writel(LUT0(DUMMY, PAD1, dummy) | LUT1(READ, PAD4, rxfifo),
+			base + QUADSPI_LUT(lut_base + 1));
+
+	/* Write enable */
+	lut_base = SEQID_WREN * 4;
+	writel(LUT0(CMD, PAD1, SPINOR_OP_WREN), base + QUADSPI_LUT(lut_base));
+
+	/* Page Program */
+	lut_base = SEQID_PP * 4;
+
+	if (q->nor_size <= SZ_16M) {
+		cmd = SPINOR_OP_PP;
+		addrlen = ADDR24BIT;
+	} else {
+		/* use the 4-byte address */
+		cmd = SPINOR_OP_PP;
+		addrlen = ADDR32BIT;
+	}
+
+	writel(LUT0(CMD, PAD1, cmd) | LUT1(ADDR, PAD1, addrlen),
+			base + QUADSPI_LUT(lut_base));
+	writel(LUT0(WRITE, PAD1, 0), base + QUADSPI_LUT(lut_base + 1));
+
+	/* Read Status */
+	lut_base = SEQID_RDSR * 4;
+	writel(LUT0(CMD, PAD1, SPINOR_OP_RDSR) | LUT1(READ, PAD1, 0x1),
+			base + QUADSPI_LUT(lut_base));
+
+	/* Erase a sector */
+	lut_base = SEQID_SE * 4;
+
+	if (q->nor_size <= SZ_16M) {
+		cmd = SPINOR_OP_SE;
+		addrlen = ADDR24BIT;
+	} else {
+		/* use the 4-byte address */
+		cmd = SPINOR_OP_SE;
+		addrlen = ADDR32BIT;
+	}
+
+	writel(LUT0(CMD, PAD1, cmd) | LUT1(ADDR, PAD1, addrlen),
+			base + QUADSPI_LUT(lut_base));
+
+	/* Erase the whole chip */
+	lut_base = SEQID_CHIP_ERASE * 4;
+	writel(LUT0(CMD, PAD1, SPINOR_OP_CHIP_ERASE),
+			base + QUADSPI_LUT(lut_base));
+
+	/* READ ID */
+	lut_base = SEQID_RDID * 4;
+	writel(LUT0(CMD, PAD1, SPINOR_OP_RDID) | LUT1(READ, PAD1, 0x8),
+			base + QUADSPI_LUT(lut_base));
+
+	/* Write Register */
+	lut_base = SEQID_WRSR * 4;
+	writel(LUT0(CMD, PAD1, SPINOR_OP_WRSR) | LUT1(WRITE, PAD1, 0x2),
+			base + QUADSPI_LUT(lut_base));
+
+	/* Read Configuration Register */
+	lut_base = SEQID_RDCR * 4;
+	writel(LUT0(CMD, PAD1, SPINOR_OP_RDCR) | LUT1(READ, PAD1, 0x1),
+			base + QUADSPI_LUT(lut_base));
+
+	/* Write disable */
+	lut_base = SEQID_WRDI * 4;
+	writel(LUT0(CMD, PAD1, SPINOR_OP_WRDI), base + QUADSPI_LUT(lut_base));
+
+	/* Enter 4 Byte Mode (Micron) */
+	lut_base = SEQID_EN4B * 4;
+	writel(LUT0(CMD, PAD1, SPINOR_OP_EN4B), base + QUADSPI_LUT(lut_base));
+
+	/* Enter 4 Byte Mode (Spansion) */
+	lut_base = SEQID_BRWR * 4;
+	writel(LUT0(CMD, PAD1, SPINOR_OP_BRWR), base + QUADSPI_LUT(lut_base));
+
+	fsl_qspi_lock_lut(q);
+}
+
+/* Get the SEQID for the command */
+static int fsl_qspi_get_seqid(struct fsl_qspi *q, u8 cmd)
+{
+	switch (cmd) {
+	case SPINOR_OP_READ_1_1_4:
+		return SEQID_QUAD_READ;
+	case SPINOR_OP_WREN:
+		return SEQID_WREN;
+	case SPINOR_OP_WRDI:
+		return SEQID_WRDI;
+	case SPINOR_OP_RDSR:
+		return SEQID_RDSR;
+	case SPINOR_OP_SE:
+		return SEQID_SE;
+	case SPINOR_OP_CHIP_ERASE:
+		return SEQID_CHIP_ERASE;
+	case SPINOR_OP_PP:
+		return SEQID_PP;
+	case SPINOR_OP_RDID:
+		return SEQID_RDID;
+	case SPINOR_OP_WRSR:
+		return SEQID_WRSR;
+	case SPINOR_OP_RDCR:
+		return SEQID_RDCR;
+	case SPINOR_OP_EN4B:
+		return SEQID_EN4B;
+	case SPINOR_OP_BRWR:
+		return SEQID_BRWR;
+	default:
+		dev_err(q->dev, "Unsupported cmd 0x%.2x\n", cmd);
+		break;
+	}
+	return -EINVAL;
+}
+
+static int
+fsl_qspi_runcmd(struct fsl_qspi *q, u8 cmd, unsigned int addr, int len)
+{
+	void __iomem *base = q->iobase;
+	int seqid;
+	u32 reg, reg2;
+	int err;
+
+	init_completion(&q->c);
+	dev_dbg(q->dev, "to 0x%.8x:0x%.8x, len:%d, cmd:%.2x\n",
+			q->chip_base_addr, addr, len, cmd);
+
+	/* save the reg */
+	reg = readl(base + QUADSPI_MCR);
+
+	writel(q->memmap_phy + q->chip_base_addr + addr, base + QUADSPI_SFAR);
+	writel(QUADSPI_RBCT_WMRK_MASK | QUADSPI_RBCT_RXBRD_USEIPS,
+			base + QUADSPI_RBCT);
+	writel(reg | QUADSPI_MCR_CLR_RXF_MASK, base + QUADSPI_MCR);
+
+	do {
+		reg2 = readl(base + QUADSPI_SR);
+		if (reg2 & (QUADSPI_SR_IP_ACC_MASK | QUADSPI_SR_AHB_ACC_MASK)) {
+			udelay(1);
+			dev_dbg(q->dev, "The controller is busy, 0x%x\n", reg2);
+			continue;
+		}
+		break;
+	} while (1);
+
+	/* trigger the LUT now */
+	seqid = fsl_qspi_get_seqid(q, cmd);
+	writel((seqid << QUADSPI_IPCR_SEQID_SHIFT) | len, base + QUADSPI_IPCR);
+
+	/* Wait for the interrupt. */
+	err = wait_for_completion_timeout(&q->c, msecs_to_jiffies(1000));
+	if (!err) {
+		dev_err(q->dev,
+			"cmd 0x%.2x timeout, addr@%.8x, FR:0x%.8x, SR:0x%.8x\n",
+			cmd, addr, readl(base + QUADSPI_FR),
+			readl(base + QUADSPI_SR));
+		err = -ETIMEDOUT;
+	} else {
+		err = 0;
+	}
+
+	/* restore the MCR */
+	writel(reg, base + QUADSPI_MCR);
+
+	return err;
+}
+
+/* Read out the data from the QUADSPI_RBDR buffer registers. */
+static void fsl_qspi_read_data(struct fsl_qspi *q, int len, u8 *rxbuf)
+{
+	u32 tmp;
+	int i = 0;
+
+	while (len > 0) {
+		tmp = readl(q->iobase + QUADSPI_RBDR + i * 4);
+		tmp = fsl_qspi_endian_xchg(q, tmp);
+		dev_dbg(q->dev, "chip addr:0x%.8x, rcv:0x%.8x\n",
+				q->chip_base_addr, tmp);
+
+		if (len >= 4) {
+			*((u32 *)rxbuf) = tmp;
+			rxbuf += 4;
+		} else {
+			memcpy(rxbuf, &tmp, len);
+			break;
+		}
+
+		len -= 4;
+		i++;
+	}
+}
+
+/*
+ * If we have changed the content of the flash by writing or erasing,
+ * we need to invalidate the AHB buffer. If we do not do so, we may read out
+ * the wrong data. The spec tells us reset the AHB domain and Serial Flash
+ * domain at the same time.
+ */
+static inline void fsl_qspi_invalid(struct fsl_qspi *q)
+{
+	u32 reg;
+
+	reg = readl(q->iobase + QUADSPI_MCR);
+	reg |= QUADSPI_MCR_SWRSTHD_MASK | QUADSPI_MCR_SWRSTSD_MASK;
+	writel(reg, q->iobase + QUADSPI_MCR);
+
+	/*
+	 * The minimum delay : 1 AHB + 2 SFCK clocks.
+	 * Delay 1 us is enough.
+	 */
+	udelay(1);
+
+	reg &= ~(QUADSPI_MCR_SWRSTHD_MASK | QUADSPI_MCR_SWRSTSD_MASK);
+	writel(reg, q->iobase + QUADSPI_MCR);
+}
+
+static int fsl_qspi_nor_write(struct fsl_qspi *q, struct spi_nor *nor,
+				u8 opcode, unsigned int to, u32 *txbuf,
+				unsigned count, size_t *retlen)
+{
+	int ret, i, j;
+	u32 tmp;
+
+	dev_dbg(q->dev, "to 0x%.8x:0x%.8x, len : %d\n",
+		q->chip_base_addr, to, count);
+
+	/* clear the TX FIFO. */
+	tmp = readl(q->iobase + QUADSPI_MCR);
+	writel(tmp | QUADSPI_MCR_CLR_RXF_MASK, q->iobase + QUADSPI_MCR);
+
+	/* fill the TX data to the FIFO */
+	for (j = 0, i = ((count + 3) / 4); j < i; j++) {
+		tmp = fsl_qspi_endian_xchg(q, *txbuf);
+		writel(tmp, q->iobase + QUADSPI_TBDR);
+		txbuf++;
+	}
+
+	/* Trigger it */
+	ret = fsl_qspi_runcmd(q, opcode, to, count);
+
+	if (ret == 0 && retlen)
+		*retlen += count;
+
+	return ret;
+}
+
+static void fsl_qspi_set_map_addr(struct fsl_qspi *q)
+{
+	int nor_size = q->nor_size;
+	void __iomem *base = q->iobase;
+
+	writel(nor_size + q->memmap_phy, base + QUADSPI_SFA1AD);
+	writel(nor_size * 2 + q->memmap_phy, base + QUADSPI_SFA2AD);
+	writel(nor_size * 3 + q->memmap_phy, base + QUADSPI_SFB1AD);
+	writel(nor_size * 4 + q->memmap_phy, base + QUADSPI_SFB2AD);
+}
+
+/*
+ * There are two different ways to read out the data from the flash:
+ *  the "IP Command Read" and the "AHB Command Read".
+ *
+ * The IC guy suggests we use the "AHB Command Read" which is faster
+ * then the "IP Command Read". (What's more is that there is a bug in
+ * the "IP Command Read" in the Vybrid.)
+ *
+ * After we set up the registers for the "AHB Command Read", we can use
+ * the memcpy to read the data directly. A "missed" access to the buffer
+ * causes the controller to clear the buffer, and use the sequence pointed
+ * by the QUADSPI_BFGENCR[SEQID] to initiate a read from the flash.
+ */
+static void fsl_qspi_init_abh_read(struct fsl_qspi *q)
+{
+	void __iomem *base = q->iobase;
+	int seqid;
+
+	/* AHB configuration for access buffer 0/1/2 .*/
+	writel(QUADSPI_BUFXCR_INVALID_MSTRID, base + QUADSPI_BUF0CR);
+	writel(QUADSPI_BUFXCR_INVALID_MSTRID, base + QUADSPI_BUF1CR);
+	writel(QUADSPI_BUFXCR_INVALID_MSTRID, base + QUADSPI_BUF2CR);
+	writel(QUADSPI_BUF3CR_ALLMST, base + QUADSPI_BUF3CR);
+
+	/* We only use the buffer3 */
+	writel(0, base + QUADSPI_BUF0IND);
+	writel(0, base + QUADSPI_BUF1IND);
+	writel(0, base + QUADSPI_BUF2IND);
+
+	/* Set the default lut sequence for AHB Read. */
+	seqid = fsl_qspi_get_seqid(q, q->nor[0].read_opcode);
+	writel(seqid << QUADSPI_BFGENCR_SEQID_SHIFT,
+		q->iobase + QUADSPI_BFGENCR);
+}
+
+/* We use this function to do some basic init for spi_nor_scan(). */
+static int fsl_qspi_nor_setup(struct fsl_qspi *q)
+{
+	void __iomem *base = q->iobase;
+	u32 reg;
+	int ret;
+
+	/* the default frequency, we will change it in the future.*/
+	ret = clk_set_rate(q->clk, 66000000);
+	if (ret)
+		return ret;
+
+	/* Init the LUT table. */
+	fsl_qspi_init_lut(q);
+
+	/* Disable the module */
+	writel(QUADSPI_MCR_MDIS_MASK | QUADSPI_MCR_RESERVED_MASK,
+			base + QUADSPI_MCR);
+
+	reg = readl(base + QUADSPI_SMPR);
+	writel(reg & ~(QUADSPI_SMPR_FSDLY_MASK
+			| QUADSPI_SMPR_FSPHS_MASK
+			| QUADSPI_SMPR_HSENA_MASK
+			| QUADSPI_SMPR_DDRSMP_MASK), base + QUADSPI_SMPR);
+
+	/* Enable the module */
+	writel(QUADSPI_MCR_RESERVED_MASK | QUADSPI_MCR_END_CFG_MASK,
+			base + QUADSPI_MCR);
+
+	/* enable the interrupt */
+	writel(QUADSPI_RSER_TFIE, q->iobase + QUADSPI_RSER);
+
+	return 0;
+}
+
+static int fsl_qspi_nor_setup_last(struct fsl_qspi *q)
+{
+	unsigned long rate = q->clk_rate;
+	int ret;
+
+	if (is_imx6sx_qspi(q))
+		rate *= 4;
+
+	ret = clk_set_rate(q->clk, rate);
+	if (ret)
+		return ret;
+
+	/* Init the LUT table again. */
+	fsl_qspi_init_lut(q);
+
+	/* Init for AHB read */
+	fsl_qspi_init_abh_read(q);
+
+	return 0;
+}
+
+static struct of_device_id fsl_qspi_dt_ids[] = {
+	{ .compatible = "fsl,vf610-qspi", .data = (void *)&vybrid_data, },
+	{ .compatible = "fsl,imx6sx-qspi", .data = (void *)&imx6sx_data, },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, fsl_qspi_dt_ids);
+
+static void fsl_qspi_set_base_addr(struct fsl_qspi *q, struct spi_nor *nor)
+{
+	q->chip_base_addr = q->nor_size * (nor - q->nor);
+}
+
+static int fsl_qspi_read_reg(struct spi_nor *nor, u8 opcode, u8 *buf, int len)
+{
+	int ret;
+	struct fsl_qspi *q = nor->priv;
+
+	ret = fsl_qspi_runcmd(q, opcode, 0, len);
+	if (ret)
+		return ret;
+
+	fsl_qspi_read_data(q, len, buf);
+	return 0;
+}
+
+static int fsl_qspi_write_reg(struct spi_nor *nor, u8 opcode, u8 *buf, int len,
+			int write_enable)
+{
+	struct fsl_qspi *q = nor->priv;
+	int ret;
+
+	if (!buf) {
+		ret = fsl_qspi_runcmd(q, opcode, 0, 1);
+		if (ret)
+			return ret;
+
+		if (opcode == SPINOR_OP_CHIP_ERASE)
+			fsl_qspi_invalid(q);
+
+	} else if (len > 0) {
+		ret = fsl_qspi_nor_write(q, nor, opcode, 0,
+					(u32 *)buf, len, NULL);
+	} else {
+		dev_err(q->dev, "invalid cmd %d\n", opcode);
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static void fsl_qspi_write(struct spi_nor *nor, loff_t to,
+		size_t len, size_t *retlen, const u_char *buf)
+{
+	struct fsl_qspi *q = nor->priv;
+
+	fsl_qspi_nor_write(q, nor, nor->program_opcode, to,
+				(u32 *)buf, len, retlen);
+
+	/* invalid the data in the AHB buffer. */
+	fsl_qspi_invalid(q);
+}
+
+static int fsl_qspi_read(struct spi_nor *nor, loff_t from,
+		size_t len, size_t *retlen, u_char *buf)
+{
+	struct fsl_qspi *q = nor->priv;
+	u8 cmd = nor->read_opcode;
+	int ret;
+
+	dev_dbg(q->dev, "cmd [%x],read from (0x%p, 0x%.8x, 0x%.8x),len:%d\n",
+		cmd, q->ahb_base, q->chip_base_addr, (unsigned int)from, len);
+
+	/* Wait until the previous command is finished. */
+	ret = nor->wait_till_ready(nor);
+	if (ret)
+		return ret;
+
+	/* Read out the data directly from the AHB buffer.*/
+	memcpy(buf, q->ahb_base + q->chip_base_addr + from, len);
+
+	*retlen += len;
+	return 0;
+}
+
+static int fsl_qspi_erase(struct spi_nor *nor, loff_t offs)
+{
+	struct fsl_qspi *q = nor->priv;
+	int ret;
+
+	dev_dbg(nor->dev, "%dKiB at 0x%08x:0x%08x\n",
+		nor->mtd->erasesize / 1024, q->chip_base_addr, (u32)offs);
+
+	/* Wait until finished previous write command. */
+	ret = nor->wait_till_ready(nor);
+	if (ret)
+		return ret;
+
+	/* Send write enable, then erase commands. */
+	ret = nor->write_reg(nor, SPINOR_OP_WREN, NULL, 0, 0);
+	if (ret)
+		return ret;
+
+	ret = fsl_qspi_runcmd(q, nor->erase_opcode, offs, 0);
+	if (ret)
+		return ret;
+
+	fsl_qspi_invalid(q);
+	return 0;
+}
+
+static int fsl_qspi_prep(struct spi_nor *nor, enum spi_nor_ops ops)
+{
+	struct fsl_qspi *q = nor->priv;
+	int ret;
+
+	ret = clk_enable(q->clk_en);
+	if (ret)
+		return ret;
+
+	ret = clk_enable(q->clk);
+	if (ret) {
+		clk_disable(q->clk_en);
+		return ret;
+	}
+
+	fsl_qspi_set_base_addr(q, nor);
+	return 0;
+}
+
+static void fsl_qspi_unprep(struct spi_nor *nor, enum spi_nor_ops ops)
+{
+	struct fsl_qspi *q = nor->priv;
+
+	clk_disable(q->clk);
+	clk_disable(q->clk_en);
+}
+
+static int fsl_qspi_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct mtd_part_parser_data ppdata;
+	struct device *dev = &pdev->dev;
+	struct fsl_qspi *q;
+	struct resource *res;
+	struct spi_nor *nor;
+	struct mtd_info *mtd;
+	int ret, i = 0;
+	bool has_second_chip = false;
+	const struct of_device_id *of_id =
+			of_match_device(fsl_qspi_dt_ids, &pdev->dev);
+
+	q = devm_kzalloc(dev, sizeof(*q), GFP_KERNEL);
+	if (!q)
+		return -ENOMEM;
+
+	q->nor_num = of_get_child_count(dev->of_node);
+	if (!q->nor_num || q->nor_num > FSL_QSPI_MAX_CHIP)
+		return -ENODEV;
+
+	/* find the resources */
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "QuadSPI");
+	q->iobase = devm_ioremap_resource(dev, res);
+	if (IS_ERR(q->iobase)) {
+		ret = PTR_ERR(q->iobase);
+		goto map_failed;
+	}
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+					"QuadSPI-memory");
+	q->ahb_base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(q->ahb_base)) {
+		ret = PTR_ERR(q->ahb_base);
+		goto map_failed;
+	}
+	q->memmap_phy = res->start;
+
+	/* find the clocks */
+	q->clk_en = devm_clk_get(dev, "qspi_en");
+	if (IS_ERR(q->clk_en)) {
+		ret = PTR_ERR(q->clk_en);
+		goto map_failed;
+	}
+
+	q->clk = devm_clk_get(dev, "qspi");
+	if (IS_ERR(q->clk)) {
+		ret = PTR_ERR(q->clk);
+		goto map_failed;
+	}
+
+	ret = clk_prepare_enable(q->clk_en);
+	if (ret) {
+		dev_err(dev, "can not enable the qspi_en clock\n");
+		goto map_failed;
+	}
+
+	ret = clk_prepare_enable(q->clk);
+	if (ret) {
+		clk_disable_unprepare(q->clk_en);
+		dev_err(dev, "can not enable the qspi clock\n");
+		goto map_failed;
+	}
+
+	/* find the irq */
+	ret = platform_get_irq(pdev, 0);
+	if (ret < 0) {
+		dev_err(dev, "failed to get the irq\n");
+		goto irq_failed;
+	}
+
+	ret = devm_request_irq(dev, ret,
+			fsl_qspi_irq_handler, 0, pdev->name, q);
+	if (ret) {
+		dev_err(dev, "failed to request irq.\n");
+		goto irq_failed;
+	}
+
+	q->dev = dev;
+	q->devtype_data = (struct fsl_qspi_devtype_data *)of_id->data;
+	platform_set_drvdata(pdev, q);
+
+	ret = fsl_qspi_nor_setup(q);
+	if (ret)
+		goto irq_failed;
+
+	if (of_get_property(np, "fsl,qspi-has-second-chip", NULL))
+		has_second_chip = true;
+
+	/* iterate the subnodes. */
+	for_each_available_child_of_node(dev->of_node, np) {
+		const struct spi_device_id *id;
+		char modalias[40];
+
+		/* skip the holes */
+		if (!has_second_chip)
+			i *= 2;
+
+		nor = &q->nor[i];
+		mtd = &q->mtd[i];
+
+		nor->mtd = mtd;
+		nor->dev = dev;
+		nor->priv = q;
+		mtd->priv = nor;
+
+		/* fill the hooks */
+		nor->read_reg = fsl_qspi_read_reg;
+		nor->write_reg = fsl_qspi_write_reg;
+		nor->read = fsl_qspi_read;
+		nor->write = fsl_qspi_write;
+		nor->erase = fsl_qspi_erase;
+
+		nor->prepare = fsl_qspi_prep;
+		nor->unprepare = fsl_qspi_unprep;
+
+		if (of_modalias_node(np, modalias, sizeof(modalias)) < 0)
+			goto map_failed;
+
+		id = spi_nor_match_id(modalias);
+		if (!id)
+			goto map_failed;
+
+		ret = of_property_read_u32(np, "spi-max-frequency",
+				&q->clk_rate);
+		if (ret < 0)
+			goto map_failed;
+
+		/* set the chip address for READID */
+		fsl_qspi_set_base_addr(q, nor);
+
+		ret = spi_nor_scan(nor, id, SPI_NOR_QUAD);
+		if (ret)
+			goto map_failed;
+
+		ppdata.of_node = np;
+		ret = mtd_device_parse_register(mtd, NULL, &ppdata, NULL, 0);
+		if (ret)
+			goto map_failed;
+
+		/* Set the correct NOR size now. */
+		if (q->nor_size == 0) {
+			q->nor_size = mtd->size;
+
+			/* Map the SPI NOR to accessiable address */
+			fsl_qspi_set_map_addr(q);
+		}
+
+		/*
+		 * The TX FIFO is 64 bytes in the Vybrid, but the Page Program
+		 * may writes 265 bytes per time. The write is working in the
+		 * unit of the TX FIFO, not in the unit of the SPI NOR's page
+		 * size.
+		 *
+		 * So shrink the spi_nor->page_size if it is larger then the
+		 * TX FIFO.
+		 */
+		if (nor->page_size > q->devtype_data->txfifo)
+			nor->page_size = q->devtype_data->txfifo;
+
+		i++;
+	}
+
+	/* finish the rest init. */
+	ret = fsl_qspi_nor_setup_last(q);
+	if (ret)
+		goto last_init_failed;
+
+	clk_disable(q->clk);
+	clk_disable(q->clk_en);
+	dev_info(dev, "QuadSPI SPI NOR flash driver\n");
+	return 0;
+
+last_init_failed:
+	for (i = 0; i < q->nor_num; i++)
+		mtd_device_unregister(&q->mtd[i]);
+
+irq_failed:
+	clk_disable_unprepare(q->clk);
+	clk_disable_unprepare(q->clk_en);
+map_failed:
+	dev_err(dev, "Freescale QuadSPI probe failed\n");
+	return ret;
+}
+
+static int fsl_qspi_remove(struct platform_device *pdev)
+{
+	struct fsl_qspi *q = platform_get_drvdata(pdev);
+	int i;
+
+	for (i = 0; i < q->nor_num; i++)
+		mtd_device_unregister(&q->mtd[i]);
+
+	/* disable the hardware */
+	writel(QUADSPI_MCR_MDIS_MASK, q->iobase + QUADSPI_MCR);
+	writel(0x0, q->iobase + QUADSPI_RSER);
+
+	clk_unprepare(q->clk);
+	clk_unprepare(q->clk_en);
+	return 0;
+}
+
+static struct platform_driver fsl_qspi_driver = {
+	.driver = {
+		.name	= "fsl-quadspi",
+		.bus	= &platform_bus_type,
+		.owner	= THIS_MODULE,
+		.of_match_table = fsl_qspi_dt_ids,
+	},
+	.probe          = fsl_qspi_probe,
+	.remove		= fsl_qspi_remove,
+};
+module_platform_driver(fsl_qspi_driver);
+
+MODULE_DESCRIPTION("Freescale QuadSPI Controller Driver");
+MODULE_AUTHOR("Freescale Semiconductor Inc.");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mtd/spi-nor/spi-nor.c b/drivers/mtd/spi-nor/spi-nor.c
new file mode 100644
index 00000000000..c713c865671
--- /dev/null
+++ b/drivers/mtd/spi-nor/spi-nor.c
@@ -0,0 +1,1107 @@
+/*
+ * Based on m25p80.c, by Mike Lavender (mike@steroidmicros.com), with
+ * influence from lart.c (Abraham Van Der Merwe) and mtd_dataflash.c
+ *
+ * Copyright (C) 2005, Intec Automation Inc.
+ * Copyright (C) 2014, Freescale Semiconductor, Inc.
+ *
+ * This code 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/errno.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/mutex.h>
+#include <linux/math64.h>
+
+#include <linux/mtd/cfi.h>
+#include <linux/mtd/mtd.h>
+#include <linux/of_platform.h>
+#include <linux/spi/flash.h>
+#include <linux/mtd/spi-nor.h>
+
+/* Define max times to check status register before we give up. */
+#define	MAX_READY_WAIT_JIFFIES	(40 * HZ) /* M25P16 specs 40s max chip erase */
+
+#define JEDEC_MFR(_jedec_id)	((_jedec_id) >> 16)
+
+/*
+ * Read the status register, returning its value in the location
+ * Return the status register value.
+ * Returns negative if error occurred.
+ */
+static int read_sr(struct spi_nor *nor)
+{
+	int ret;
+	u8 val;
+
+	ret = nor->read_reg(nor, SPINOR_OP_RDSR, &val, 1);
+	if (ret < 0) {
+		pr_err("error %d reading SR\n", (int) ret);
+		return ret;
+	}
+
+	return val;
+}
+
+/*
+ * Read configuration register, returning its value in the
+ * location. Return the configuration register value.
+ * Returns negative if error occured.
+ */
+static int read_cr(struct spi_nor *nor)
+{
+	int ret;
+	u8 val;
+
+	ret = nor->read_reg(nor, SPINOR_OP_RDCR, &val, 1);
+	if (ret < 0) {
+		dev_err(nor->dev, "error %d reading CR\n", ret);
+		return ret;
+	}
+
+	return val;
+}
+
+/*
+ * Dummy Cycle calculation for different type of read.
+ * It can be used to support more commands with
+ * different dummy cycle requirements.
+ */
+static inline int spi_nor_read_dummy_cycles(struct spi_nor *nor)
+{
+	switch (nor->flash_read) {
+	case SPI_NOR_FAST:
+	case SPI_NOR_DUAL:
+	case SPI_NOR_QUAD:
+		return 1;
+	case SPI_NOR_NORMAL:
+		return 0;
+	}
+	return 0;
+}
+
+/*
+ * Write status register 1 byte
+ * Returns negative if error occurred.
+ */
+static inline int write_sr(struct spi_nor *nor, u8 val)
+{
+	nor->cmd_buf[0] = val;
+	return nor->write_reg(nor, SPINOR_OP_WRSR, nor->cmd_buf, 1, 0);
+}
+
+/*
+ * Set write enable latch with Write Enable command.
+ * Returns negative if error occurred.
+ */
+static inline int write_enable(struct spi_nor *nor)
+{
+	return nor->write_reg(nor, SPINOR_OP_WREN, NULL, 0, 0);
+}
+
+/*
+ * Send write disble instruction to the chip.
+ */
+static inline int write_disable(struct spi_nor *nor)
+{
+	return nor->write_reg(nor, SPINOR_OP_WRDI, NULL, 0, 0);
+}
+
+static inline struct spi_nor *mtd_to_spi_nor(struct mtd_info *mtd)
+{
+	return mtd->priv;
+}
+
+/* Enable/disable 4-byte addressing mode. */
+static inline int set_4byte(struct spi_nor *nor, u32 jedec_id, int enable)
+{
+	int status;
+	bool need_wren = false;
+	u8 cmd;
+
+	switch (JEDEC_MFR(jedec_id)) {
+	case CFI_MFR_ST: /* Micron, actually */
+		/* Some Micron need WREN command; all will accept it */
+		need_wren = true;
+	case CFI_MFR_MACRONIX:
+	case 0xEF /* winbond */:
+		if (need_wren)
+			write_enable(nor);
+
+		cmd = enable ? SPINOR_OP_EN4B : SPINOR_OP_EX4B;
+		status = nor->write_reg(nor, cmd, NULL, 0, 0);
+		if (need_wren)
+			write_disable(nor);
+
+		return status;
+	default:
+		/* Spansion style */
+		nor->cmd_buf[0] = enable << 7;
+		return nor->write_reg(nor, SPINOR_OP_BRWR, nor->cmd_buf, 1, 0);
+	}
+}
+
+static int spi_nor_wait_till_ready(struct spi_nor *nor)
+{
+	unsigned long deadline;
+	int sr;
+
+	deadline = jiffies + MAX_READY_WAIT_JIFFIES;
+
+	do {
+		cond_resched();
+
+		sr = read_sr(nor);
+		if (sr < 0)
+			break;
+		else if (!(sr & SR_WIP))
+			return 0;
+	} while (!time_after_eq(jiffies, deadline));
+
+	return -ETIMEDOUT;
+}
+
+/*
+ * Service routine to read status register until ready, or timeout occurs.
+ * Returns non-zero if error.
+ */
+static int wait_till_ready(struct spi_nor *nor)
+{
+	return nor->wait_till_ready(nor);
+}
+
+/*
+ * Erase the whole flash memory
+ *
+ * Returns 0 if successful, non-zero otherwise.
+ */
+static int erase_chip(struct spi_nor *nor)
+{
+	int ret;
+
+	dev_dbg(nor->dev, " %lldKiB\n", (long long)(nor->mtd->size >> 10));
+
+	/* Wait until finished previous write command. */
+	ret = wait_till_ready(nor);
+	if (ret)
+		return ret;
+
+	/* Send write enable, then erase commands. */
+	write_enable(nor);
+
+	return nor->write_reg(nor, SPINOR_OP_CHIP_ERASE, NULL, 0, 0);
+}
+
+static int spi_nor_lock_and_prep(struct spi_nor *nor, enum spi_nor_ops ops)
+{
+	int ret = 0;
+
+	mutex_lock(&nor->lock);
+
+	if (nor->prepare) {
+		ret = nor->prepare(nor, ops);
+		if (ret) {
+			dev_err(nor->dev, "failed in the preparation.\n");
+			mutex_unlock(&nor->lock);
+			return ret;
+		}
+	}
+	return ret;
+}
+
+static void spi_nor_unlock_and_unprep(struct spi_nor *nor, enum spi_nor_ops ops)
+{
+	if (nor->unprepare)
+		nor->unprepare(nor, ops);
+	mutex_unlock(&nor->lock);
+}
+
+/*
+ * Erase an address range on the nor chip.  The address range may extend
+ * one or more erase sectors.  Return an error is there is a problem erasing.
+ */
+static int spi_nor_erase(struct mtd_info *mtd, struct erase_info *instr)
+{
+	struct spi_nor *nor = mtd_to_spi_nor(mtd);
+	u32 addr, len;
+	uint32_t rem;
+	int ret;
+
+	dev_dbg(nor->dev, "at 0x%llx, len %lld\n", (long long)instr->addr,
+			(long long)instr->len);
+
+	div_u64_rem(instr->len, mtd->erasesize, &rem);
+	if (rem)
+		return -EINVAL;
+
+	addr = instr->addr;
+	len = instr->len;
+
+	ret = spi_nor_lock_and_prep(nor, SPI_NOR_OPS_ERASE);
+	if (ret)
+		return ret;
+
+	/* whole-chip erase? */
+	if (len == mtd->size) {
+		if (erase_chip(nor)) {
+			ret = -EIO;
+			goto erase_err;
+		}
+
+	/* REVISIT in some cases we could speed up erasing large regions
+	 * by using SPINOR_OP_SE instead of SPINOR_OP_BE_4K.  We may have set up
+	 * to use "small sector erase", but that's not always optimal.
+	 */
+
+	/* "sector"-at-a-time erase */
+	} else {
+		while (len) {
+			if (nor->erase(nor, addr)) {
+				ret = -EIO;
+				goto erase_err;
+			}
+
+			addr += mtd->erasesize;
+			len -= mtd->erasesize;
+		}
+	}
+
+	spi_nor_unlock_and_unprep(nor, SPI_NOR_OPS_ERASE);
+
+	instr->state = MTD_ERASE_DONE;
+	mtd_erase_callback(instr);
+
+	return ret;
+
+erase_err:
+	spi_nor_unlock_and_unprep(nor, SPI_NOR_OPS_ERASE);
+	instr->state = MTD_ERASE_FAILED;
+	return ret;
+}
+
+static int spi_nor_lock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
+{
+	struct spi_nor *nor = mtd_to_spi_nor(mtd);
+	uint32_t offset = ofs;
+	uint8_t status_old, status_new;
+	int ret = 0;
+
+	ret = spi_nor_lock_and_prep(nor, SPI_NOR_OPS_LOCK);
+	if (ret)
+		return ret;
+
+	/* Wait until finished previous command */
+	ret = wait_till_ready(nor);
+	if (ret)
+		goto err;
+
+	status_old = read_sr(nor);
+
+	if (offset < mtd->size - (mtd->size / 2))
+		status_new = status_old | SR_BP2 | SR_BP1 | SR_BP0;
+	else if (offset < mtd->size - (mtd->size / 4))
+		status_new = (status_old & ~SR_BP0) | SR_BP2 | SR_BP1;
+	else if (offset < mtd->size - (mtd->size / 8))
+		status_new = (status_old & ~SR_BP1) | SR_BP2 | SR_BP0;
+	else if (offset < mtd->size - (mtd->size / 16))
+		status_new = (status_old & ~(SR_BP0 | SR_BP1)) | SR_BP2;
+	else if (offset < mtd->size - (mtd->size / 32))
+		status_new = (status_old & ~SR_BP2) | SR_BP1 | SR_BP0;
+	else if (offset < mtd->size - (mtd->size / 64))
+		status_new = (status_old & ~(SR_BP2 | SR_BP0)) | SR_BP1;
+	else
+		status_new = (status_old & ~(SR_BP2 | SR_BP1)) | SR_BP0;
+
+	/* Only modify protection if it will not unlock other areas */
+	if ((status_new & (SR_BP2 | SR_BP1 | SR_BP0)) >
+				(status_old & (SR_BP2 | SR_BP1 | SR_BP0))) {
+		write_enable(nor);
+		ret = write_sr(nor, status_new);
+		if (ret)
+			goto err;
+	}
+
+err:
+	spi_nor_unlock_and_unprep(nor, SPI_NOR_OPS_LOCK);
+	return ret;
+}
+
+static int spi_nor_unlock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
+{
+	struct spi_nor *nor = mtd_to_spi_nor(mtd);
+	uint32_t offset = ofs;
+	uint8_t status_old, status_new;
+	int ret = 0;
+
+	ret = spi_nor_lock_and_prep(nor, SPI_NOR_OPS_UNLOCK);
+	if (ret)
+		return ret;
+
+	/* Wait until finished previous command */
+	ret = wait_till_ready(nor);
+	if (ret)
+		goto err;
+
+	status_old = read_sr(nor);
+
+	if (offset+len > mtd->size - (mtd->size / 64))
+		status_new = status_old & ~(SR_BP2 | SR_BP1 | SR_BP0);
+	else if (offset+len > mtd->size - (mtd->size / 32))
+		status_new = (status_old & ~(SR_BP2 | SR_BP1)) | SR_BP0;
+	else if (offset+len > mtd->size - (mtd->size / 16))
+		status_new = (status_old & ~(SR_BP2 | SR_BP0)) | SR_BP1;
+	else if (offset+len > mtd->size - (mtd->size / 8))
+		status_new = (status_old & ~SR_BP2) | SR_BP1 | SR_BP0;
+	else if (offset+len > mtd->size - (mtd->size / 4))
+		status_new = (status_old & ~(SR_BP0 | SR_BP1)) | SR_BP2;
+	else if (offset+len > mtd->size - (mtd->size / 2))
+		status_new = (status_old & ~SR_BP1) | SR_BP2 | SR_BP0;
+	else
+		status_new = (status_old & ~SR_BP0) | SR_BP2 | SR_BP1;
+
+	/* Only modify protection if it will not lock other areas */
+	if ((status_new & (SR_BP2 | SR_BP1 | SR_BP0)) <
+				(status_old & (SR_BP2 | SR_BP1 | SR_BP0))) {
+		write_enable(nor);
+		ret = write_sr(nor, status_new);
+		if (ret)
+			goto err;
+	}
+
+err:
+	spi_nor_unlock_and_unprep(nor, SPI_NOR_OPS_UNLOCK);
+	return ret;
+}
+
+struct flash_info {
+	/* JEDEC id zero means "no ID" (most older chips); otherwise it has
+	 * a high byte of zero plus three data bytes: the manufacturer id,
+	 * then a two byte device id.
+	 */
+	u32		jedec_id;
+	u16             ext_id;
+
+	/* The size listed here is what works with SPINOR_OP_SE, which isn't
+	 * necessarily called a "sector" by the vendor.
+	 */
+	unsigned	sector_size;
+	u16		n_sectors;
+
+	u16		page_size;
+	u16		addr_width;
+
+	u16		flags;
+#define	SECT_4K			0x01	/* SPINOR_OP_BE_4K works uniformly */
+#define	SPI_NOR_NO_ERASE	0x02	/* No erase command needed */
+#define	SST_WRITE		0x04	/* use SST byte programming */
+#define	SPI_NOR_NO_FR		0x08	/* Can't do fastread */
+#define	SECT_4K_PMC		0x10	/* SPINOR_OP_BE_4K_PMC works uniformly */
+#define	SPI_NOR_DUAL_READ	0x20    /* Flash supports Dual Read */
+#define	SPI_NOR_QUAD_READ	0x40    /* Flash supports Quad Read */
+};
+
+#define INFO(_jedec_id, _ext_id, _sector_size, _n_sectors, _flags)	\
+	((kernel_ulong_t)&(struct flash_info) {				\
+		.jedec_id = (_jedec_id),				\
+		.ext_id = (_ext_id),					\
+		.sector_size = (_sector_size),				\
+		.n_sectors = (_n_sectors),				\
+		.page_size = 256,					\
+		.flags = (_flags),					\
+	})
+
+#define CAT25_INFO(_sector_size, _n_sectors, _page_size, _addr_width, _flags)	\
+	((kernel_ulong_t)&(struct flash_info) {				\
+		.sector_size = (_sector_size),				\
+		.n_sectors = (_n_sectors),				\
+		.page_size = (_page_size),				\
+		.addr_width = (_addr_width),				\
+		.flags = (_flags),					\
+	})
+
+/* NOTE: double check command sets and memory organization when you add
+ * more nor chips.  This current list focusses on newer chips, which
+ * have been converging on command sets which including JEDEC ID.
+ */
+const struct spi_device_id spi_nor_ids[] = {
+	/* Atmel -- some are (confusingly) marketed as "DataFlash" */
+	{ "at25fs010",  INFO(0x1f6601, 0, 32 * 1024,   4, SECT_4K) },
+	{ "at25fs040",  INFO(0x1f6604, 0, 64 * 1024,   8, SECT_4K) },
+
+	{ "at25df041a", INFO(0x1f4401, 0, 64 * 1024,   8, SECT_4K) },
+	{ "at25df321a", INFO(0x1f4701, 0, 64 * 1024,  64, SECT_4K) },
+	{ "at25df641",  INFO(0x1f4800, 0, 64 * 1024, 128, SECT_4K) },
+
+	{ "at26f004",   INFO(0x1f0400, 0, 64 * 1024,  8, SECT_4K) },
+	{ "at26df081a", INFO(0x1f4501, 0, 64 * 1024, 16, SECT_4K) },
+	{ "at26df161a", INFO(0x1f4601, 0, 64 * 1024, 32, SECT_4K) },
+	{ "at26df321",  INFO(0x1f4700, 0, 64 * 1024, 64, SECT_4K) },
+
+	{ "at45db081d", INFO(0x1f2500, 0, 64 * 1024, 16, SECT_4K) },
+
+	/* EON -- en25xxx */
+	{ "en25f32",    INFO(0x1c3116, 0, 64 * 1024,   64, SECT_4K) },
+	{ "en25p32",    INFO(0x1c2016, 0, 64 * 1024,   64, 0) },
+	{ "en25q32b",   INFO(0x1c3016, 0, 64 * 1024,   64, 0) },
+	{ "en25p64",    INFO(0x1c2017, 0, 64 * 1024,  128, 0) },
+	{ "en25q64",    INFO(0x1c3017, 0, 64 * 1024,  128, SECT_4K) },
+	{ "en25qh256",  INFO(0x1c7019, 0, 64 * 1024,  512, 0) },
+
+	/* ESMT */
+	{ "f25l32pa", INFO(0x8c2016, 0, 64 * 1024, 64, SECT_4K) },
+
+	/* Everspin */
+	{ "mr25h256", CAT25_INFO( 32 * 1024, 1, 256, 2, SPI_NOR_NO_ERASE | SPI_NOR_NO_FR) },
+	{ "mr25h10",  CAT25_INFO(128 * 1024, 1, 256, 3, SPI_NOR_NO_ERASE | SPI_NOR_NO_FR) },
+
+	/* GigaDevice */
+	{ "gd25q32", INFO(0xc84016, 0, 64 * 1024,  64, SECT_4K) },
+	{ "gd25q64", INFO(0xc84017, 0, 64 * 1024, 128, SECT_4K) },
+
+	/* Intel/Numonyx -- xxxs33b */
+	{ "160s33b",  INFO(0x898911, 0, 64 * 1024,  32, 0) },
+	{ "320s33b",  INFO(0x898912, 0, 64 * 1024,  64, 0) },
+	{ "640s33b",  INFO(0x898913, 0, 64 * 1024, 128, 0) },
+
+	/* Macronix */
+	{ "mx25l2005a",  INFO(0xc22012, 0, 64 * 1024,   4, SECT_4K) },
+	{ "mx25l4005a",  INFO(0xc22013, 0, 64 * 1024,   8, SECT_4K) },
+	{ "mx25l8005",   INFO(0xc22014, 0, 64 * 1024,  16, 0) },
+	{ "mx25l1606e",  INFO(0xc22015, 0, 64 * 1024,  32, SECT_4K) },
+	{ "mx25l3205d",  INFO(0xc22016, 0, 64 * 1024,  64, 0) },
+	{ "mx25l3255e",  INFO(0xc29e16, 0, 64 * 1024,  64, SECT_4K) },
+	{ "mx25l6405d",  INFO(0xc22017, 0, 64 * 1024, 128, 0) },
+	{ "mx25l12805d", INFO(0xc22018, 0, 64 * 1024, 256, 0) },
+	{ "mx25l12855e", INFO(0xc22618, 0, 64 * 1024, 256, 0) },
+	{ "mx25l25635e", INFO(0xc22019, 0, 64 * 1024, 512, 0) },
+	{ "mx25l25655e", INFO(0xc22619, 0, 64 * 1024, 512, 0) },
+	{ "mx66l51235l", INFO(0xc2201a, 0, 64 * 1024, 1024, SPI_NOR_QUAD_READ) },
+	{ "mx66l1g55g",  INFO(0xc2261b, 0, 64 * 1024, 2048, SPI_NOR_QUAD_READ) },
+
+	/* Micron */
+	{ "n25q064",     INFO(0x20ba17, 0, 64 * 1024,  128, 0) },
+	{ "n25q128a11",  INFO(0x20bb18, 0, 64 * 1024,  256, 0) },
+	{ "n25q128a13",  INFO(0x20ba18, 0, 64 * 1024,  256, 0) },
+	{ "n25q256a",    INFO(0x20ba19, 0, 64 * 1024,  512, SECT_4K) },
+	{ "n25q512a",    INFO(0x20bb20, 0, 64 * 1024, 1024, SECT_4K) },
+
+	/* PMC */
+	{ "pm25lv512",   INFO(0,        0, 32 * 1024,    2, SECT_4K_PMC) },
+	{ "pm25lv010",   INFO(0,        0, 32 * 1024,    4, SECT_4K_PMC) },
+	{ "pm25lq032",   INFO(0x7f9d46, 0, 64 * 1024,   64, SECT_4K) },
+
+	/* Spansion -- single (large) sector size only, at least
+	 * for the chips listed here (without boot sectors).
+	 */
+	{ "s25sl032p",  INFO(0x010215, 0x4d00,  64 * 1024,  64, SPI_NOR_DUAL_READ | SPI_NOR_QUAD_READ) },
+	{ "s25sl064p",  INFO(0x010216, 0x4d00,  64 * 1024, 128, 0) },
+	{ "s25fl256s0", INFO(0x010219, 0x4d00, 256 * 1024, 128, 0) },
+	{ "s25fl256s1", INFO(0x010219, 0x4d01,  64 * 1024, 512, SPI_NOR_DUAL_READ | SPI_NOR_QUAD_READ) },
+	{ "s25fl512s",  INFO(0x010220, 0x4d00, 256 * 1024, 256, SPI_NOR_DUAL_READ | SPI_NOR_QUAD_READ) },
+	{ "s70fl01gs",  INFO(0x010221, 0x4d00, 256 * 1024, 256, 0) },
+	{ "s25sl12800", INFO(0x012018, 0x0300, 256 * 1024,  64, 0) },
+	{ "s25sl12801", INFO(0x012018, 0x0301,  64 * 1024, 256, 0) },
+	{ "s25fl129p0", INFO(0x012018, 0x4d00, 256 * 1024,  64, 0) },
+	{ "s25fl129p1", INFO(0x012018, 0x4d01,  64 * 1024, 256, 0) },
+	{ "s25sl004a",  INFO(0x010212,      0,  64 * 1024,   8, 0) },
+	{ "s25sl008a",  INFO(0x010213,      0,  64 * 1024,  16, 0) },
+	{ "s25sl016a",  INFO(0x010214,      0,  64 * 1024,  32, 0) },
+	{ "s25sl032a",  INFO(0x010215,      0,  64 * 1024,  64, 0) },
+	{ "s25sl064a",  INFO(0x010216,      0,  64 * 1024, 128, 0) },
+	{ "s25fl008k",  INFO(0xef4014,      0,  64 * 1024,  16, SECT_4K) },
+	{ "s25fl016k",  INFO(0xef4015,      0,  64 * 1024,  32, SECT_4K) },
+	{ "s25fl064k",  INFO(0xef4017,      0,  64 * 1024, 128, SECT_4K) },
+
+	/* SST -- large erase sizes are "overlays", "sectors" are 4K */
+	{ "sst25vf040b", INFO(0xbf258d, 0, 64 * 1024,  8, SECT_4K | SST_WRITE) },
+	{ "sst25vf080b", INFO(0xbf258e, 0, 64 * 1024, 16, SECT_4K | SST_WRITE) },
+	{ "sst25vf016b", INFO(0xbf2541, 0, 64 * 1024, 32, SECT_4K | SST_WRITE) },
+	{ "sst25vf032b", INFO(0xbf254a, 0, 64 * 1024, 64, SECT_4K | SST_WRITE) },
+	{ "sst25vf064c", INFO(0xbf254b, 0, 64 * 1024, 128, SECT_4K) },
+	{ "sst25wf512",  INFO(0xbf2501, 0, 64 * 1024,  1, SECT_4K | SST_WRITE) },
+	{ "sst25wf010",  INFO(0xbf2502, 0, 64 * 1024,  2, SECT_4K | SST_WRITE) },
+	{ "sst25wf020",  INFO(0xbf2503, 0, 64 * 1024,  4, SECT_4K | SST_WRITE) },
+	{ "sst25wf040",  INFO(0xbf2504, 0, 64 * 1024,  8, SECT_4K | SST_WRITE) },
+
+	/* ST Microelectronics -- newer production may have feature updates */
+	{ "m25p05",  INFO(0x202010,  0,  32 * 1024,   2, 0) },
+	{ "m25p10",  INFO(0x202011,  0,  32 * 1024,   4, 0) },
+	{ "m25p20",  INFO(0x202012,  0,  64 * 1024,   4, 0) },
+	{ "m25p40",  INFO(0x202013,  0,  64 * 1024,   8, 0) },
+	{ "m25p80",  INFO(0x202014,  0,  64 * 1024,  16, 0) },
+	{ "m25p16",  INFO(0x202015,  0,  64 * 1024,  32, 0) },
+	{ "m25p32",  INFO(0x202016,  0,  64 * 1024,  64, 0) },
+	{ "m25p64",  INFO(0x202017,  0,  64 * 1024, 128, 0) },
+	{ "m25p128", INFO(0x202018,  0, 256 * 1024,  64, 0) },
+	{ "n25q032", INFO(0x20ba16,  0,  64 * 1024,  64, 0) },
+
+	{ "m25p05-nonjedec",  INFO(0, 0,  32 * 1024,   2, 0) },
+	{ "m25p10-nonjedec",  INFO(0, 0,  32 * 1024,   4, 0) },
+	{ "m25p20-nonjedec",  INFO(0, 0,  64 * 1024,   4, 0) },
+	{ "m25p40-nonjedec",  INFO(0, 0,  64 * 1024,   8, 0) },
+	{ "m25p80-nonjedec",  INFO(0, 0,  64 * 1024,  16, 0) },
+	{ "m25p16-nonjedec",  INFO(0, 0,  64 * 1024,  32, 0) },
+	{ "m25p32-nonjedec",  INFO(0, 0,  64 * 1024,  64, 0) },
+	{ "m25p64-nonjedec",  INFO(0, 0,  64 * 1024, 128, 0) },
+	{ "m25p128-nonjedec", INFO(0, 0, 256 * 1024,  64, 0) },
+
+	{ "m45pe10", INFO(0x204011,  0, 64 * 1024,    2, 0) },
+	{ "m45pe80", INFO(0x204014,  0, 64 * 1024,   16, 0) },
+	{ "m45pe16", INFO(0x204015,  0, 64 * 1024,   32, 0) },
+
+	{ "m25pe20", INFO(0x208012,  0, 64 * 1024,  4,       0) },
+	{ "m25pe80", INFO(0x208014,  0, 64 * 1024, 16,       0) },
+	{ "m25pe16", INFO(0x208015,  0, 64 * 1024, 32, SECT_4K) },
+
+	{ "m25px16",    INFO(0x207115,  0, 64 * 1024, 32, SECT_4K) },
+	{ "m25px32",    INFO(0x207116,  0, 64 * 1024, 64, SECT_4K) },
+	{ "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) },
+
+	/* Winbond -- w25x "blocks" are 64K, "sectors" are 4KiB */
+	{ "w25x10", INFO(0xef3011, 0, 64 * 1024,  2,  SECT_4K) },
+	{ "w25x20", INFO(0xef3012, 0, 64 * 1024,  4,  SECT_4K) },
+	{ "w25x40", INFO(0xef3013, 0, 64 * 1024,  8,  SECT_4K) },
+	{ "w25x80", INFO(0xef3014, 0, 64 * 1024,  16, SECT_4K) },
+	{ "w25x16", INFO(0xef3015, 0, 64 * 1024,  32, SECT_4K) },
+	{ "w25x32", INFO(0xef3016, 0, 64 * 1024,  64, SECT_4K) },
+	{ "w25q32", INFO(0xef4016, 0, 64 * 1024,  64, SECT_4K) },
+	{ "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) },
+	{ "w25q256", INFO(0xef4019, 0, 64 * 1024, 512, SECT_4K) },
+
+	/* Catalyst / On Semiconductor -- non-JEDEC */
+	{ "cat25c11", CAT25_INFO(  16, 8, 16, 1, SPI_NOR_NO_ERASE | SPI_NOR_NO_FR) },
+	{ "cat25c03", CAT25_INFO(  32, 8, 16, 2, SPI_NOR_NO_ERASE | SPI_NOR_NO_FR) },
+	{ "cat25c09", CAT25_INFO( 128, 8, 32, 2, SPI_NOR_NO_ERASE | SPI_NOR_NO_FR) },
+	{ "cat25c17", CAT25_INFO( 256, 8, 32, 2, SPI_NOR_NO_ERASE | SPI_NOR_NO_FR) },
+	{ "cat25128", CAT25_INFO(2048, 8, 64, 2, SPI_NOR_NO_ERASE | SPI_NOR_NO_FR) },
+	{ },
+};
+EXPORT_SYMBOL_GPL(spi_nor_ids);
+
+static const struct spi_device_id *spi_nor_read_id(struct spi_nor *nor)
+{
+	int			tmp;
+	u8			id[5];
+	u32			jedec;
+	u16                     ext_jedec;
+	struct flash_info	*info;
+
+	tmp = nor->read_reg(nor, SPINOR_OP_RDID, id, 5);
+	if (tmp < 0) {
+		dev_dbg(nor->dev, " error %d reading JEDEC ID\n", tmp);
+		return ERR_PTR(tmp);
+	}
+	jedec = id[0];
+	jedec = jedec << 8;
+	jedec |= id[1];
+	jedec = jedec << 8;
+	jedec |= id[2];
+
+	ext_jedec = id[3] << 8 | id[4];
+
+	for (tmp = 0; tmp < ARRAY_SIZE(spi_nor_ids) - 1; tmp++) {
+		info = (void *)spi_nor_ids[tmp].driver_data;
+		if (info->jedec_id == jedec) {
+			if (info->ext_id == 0 || info->ext_id == ext_jedec)
+				return &spi_nor_ids[tmp];
+		}
+	}
+	dev_err(nor->dev, "unrecognized JEDEC id %06x\n", jedec);
+	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)
+{
+	struct spi_nor *nor = mtd_to_spi_nor(mtd);
+	int ret;
+
+	dev_dbg(nor->dev, "from 0x%08x, len %zd\n", (u32)from, len);
+
+	ret = spi_nor_lock_and_prep(nor, SPI_NOR_OPS_READ);
+	if (ret)
+		return ret;
+
+	ret = nor->read(nor, from, len, retlen, buf);
+
+	spi_nor_unlock_and_unprep(nor, SPI_NOR_OPS_READ);
+	return ret;
+}
+
+static int sst_write(struct mtd_info *mtd, loff_t to, size_t len,
+		size_t *retlen, const u_char *buf)
+{
+	struct spi_nor *nor = mtd_to_spi_nor(mtd);
+	size_t actual;
+	int ret;
+
+	dev_dbg(nor->dev, "to 0x%08x, len %zd\n", (u32)to, len);
+
+	ret = spi_nor_lock_and_prep(nor, SPI_NOR_OPS_WRITE);
+	if (ret)
+		return ret;
+
+	/* Wait until finished previous write command. */
+	ret = wait_till_ready(nor);
+	if (ret)
+		goto time_out;
+
+	write_enable(nor);
+
+	nor->sst_write_second = false;
+
+	actual = to % 2;
+	/* Start write from odd address. */
+	if (actual) {
+		nor->program_opcode = SPINOR_OP_BP;
+
+		/* write one byte. */
+		nor->write(nor, to, 1, retlen, buf);
+		ret = wait_till_ready(nor);
+		if (ret)
+			goto time_out;
+	}
+	to += actual;
+
+	/* Write out most of the data here. */
+	for (; actual < len - 1; actual += 2) {
+		nor->program_opcode = SPINOR_OP_AAI_WP;
+
+		/* write two bytes. */
+		nor->write(nor, to, 2, retlen, buf + actual);
+		ret = wait_till_ready(nor);
+		if (ret)
+			goto time_out;
+		to += 2;
+		nor->sst_write_second = true;
+	}
+	nor->sst_write_second = false;
+
+	write_disable(nor);
+	ret = wait_till_ready(nor);
+	if (ret)
+		goto time_out;
+
+	/* Write out trailing byte if it exists. */
+	if (actual != len) {
+		write_enable(nor);
+
+		nor->program_opcode = SPINOR_OP_BP;
+		nor->write(nor, to, 1, retlen, buf + actual);
+
+		ret = wait_till_ready(nor);
+		if (ret)
+			goto time_out;
+		write_disable(nor);
+	}
+time_out:
+	spi_nor_unlock_and_unprep(nor, SPI_NOR_OPS_WRITE);
+	return ret;
+}
+
+/*
+ * Write an address range to the nor chip.  Data must be written in
+ * FLASH_PAGESIZE chunks.  The address range may be any size provided
+ * it is within the physical boundaries.
+ */
+static int spi_nor_write(struct mtd_info *mtd, loff_t to, size_t len,
+	size_t *retlen, const u_char *buf)
+{
+	struct spi_nor *nor = mtd_to_spi_nor(mtd);
+	u32 page_offset, page_size, i;
+	int ret;
+
+	dev_dbg(nor->dev, "to 0x%08x, len %zd\n", (u32)to, len);
+
+	ret = spi_nor_lock_and_prep(nor, SPI_NOR_OPS_WRITE);
+	if (ret)
+		return ret;
+
+	/* Wait until finished previous write command. */
+	ret = wait_till_ready(nor);
+	if (ret)
+		goto write_err;
+
+	write_enable(nor);
+
+	page_offset = to & (nor->page_size - 1);
+
+	/* do all the bytes fit onto one page? */
+	if (page_offset + len <= nor->page_size) {
+		nor->write(nor, to, len, retlen, buf);
+	} else {
+		/* the size of data remaining on the first page */
+		page_size = nor->page_size - page_offset;
+		nor->write(nor, to, page_size, retlen, buf);
+
+		/* write everything in nor->page_size chunks */
+		for (i = page_size; i < len; i += page_size) {
+			page_size = len - i;
+			if (page_size > nor->page_size)
+				page_size = nor->page_size;
+
+			wait_till_ready(nor);
+			write_enable(nor);
+
+			nor->write(nor, to + i, page_size, retlen, buf + i);
+		}
+	}
+
+write_err:
+	spi_nor_unlock_and_unprep(nor, SPI_NOR_OPS_WRITE);
+	return 0;
+}
+
+static int macronix_quad_enable(struct spi_nor *nor)
+{
+	int ret, val;
+
+	val = read_sr(nor);
+	write_enable(nor);
+
+	nor->cmd_buf[0] = val | SR_QUAD_EN_MX;
+	nor->write_reg(nor, SPINOR_OP_WRSR, nor->cmd_buf, 1, 0);
+
+	if (wait_till_ready(nor))
+		return 1;
+
+	ret = read_sr(nor);
+	if (!(ret > 0 && (ret & SR_QUAD_EN_MX))) {
+		dev_err(nor->dev, "Macronix Quad bit not set\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ * Write status Register and configuration register with 2 bytes
+ * The first byte will be written to the status register, while the
+ * second byte will be written to the configuration register.
+ * Return negative if error occured.
+ */
+static int write_sr_cr(struct spi_nor *nor, u16 val)
+{
+	nor->cmd_buf[0] = val & 0xff;
+	nor->cmd_buf[1] = (val >> 8);
+
+	return nor->write_reg(nor, SPINOR_OP_WRSR, nor->cmd_buf, 2, 0);
+}
+
+static int spansion_quad_enable(struct spi_nor *nor)
+{
+	int ret;
+	int quad_en = CR_QUAD_EN_SPAN << 8;
+
+	write_enable(nor);
+
+	ret = write_sr_cr(nor, quad_en);
+	if (ret < 0) {
+		dev_err(nor->dev,
+			"error while writing configuration register\n");
+		return -EINVAL;
+	}
+
+	/* read back and check it */
+	ret = read_cr(nor);
+	if (!(ret > 0 && (ret & CR_QUAD_EN_SPAN))) {
+		dev_err(nor->dev, "Spansion Quad bit not set\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int set_quad_mode(struct spi_nor *nor, u32 jedec_id)
+{
+	int status;
+
+	switch (JEDEC_MFR(jedec_id)) {
+	case CFI_MFR_MACRONIX:
+		status = macronix_quad_enable(nor);
+		if (status) {
+			dev_err(nor->dev, "Macronix quad-read not enabled\n");
+			return -EINVAL;
+		}
+		return status;
+	default:
+		status = spansion_quad_enable(nor);
+		if (status) {
+			dev_err(nor->dev, "Spansion quad-read not enabled\n");
+			return -EINVAL;
+		}
+		return status;
+	}
+}
+
+static int spi_nor_check(struct spi_nor *nor)
+{
+	if (!nor->dev || !nor->read || !nor->write ||
+		!nor->read_reg || !nor->write_reg || !nor->erase) {
+		pr_err("spi-nor: please fill all the necessary fields!\n");
+		return -EINVAL;
+	}
+
+	if (!nor->read_id)
+		nor->read_id = spi_nor_read_id;
+	if (!nor->wait_till_ready)
+		nor->wait_till_ready = spi_nor_wait_till_ready;
+
+	return 0;
+}
+
+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;
+	int ret;
+	int i;
+
+	ret = spi_nor_check(nor);
+	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);
+		if (IS_ERR(jid)) {
+			return PTR_ERR(jid);
+		} else if (jid != id) {
+			/*
+			 * JEDEC knows better, so overwrite platform ID. We
+			 * can't trust partitions any longer, but we'll let
+			 * mtd apply them anyway, since some partitions may be
+			 * marked read-only, and we don't want to lose that
+			 * information, even if it's not 100% accurate.
+			 */
+			dev_warn(dev, "found %s, expected %s\n",
+				 jid->name, id->name);
+			id = jid;
+			info = (void *)jid->driver_data;
+		}
+	}
+
+	mutex_init(&nor->lock);
+
+	/*
+	 * Atmel, SST and Intel/Numonyx serial nor tend to power
+	 * up with the software protection bits set
+	 */
+
+	if (JEDEC_MFR(info->jedec_id) == CFI_MFR_ATMEL ||
+	    JEDEC_MFR(info->jedec_id) == CFI_MFR_INTEL ||
+	    JEDEC_MFR(info->jedec_id) == CFI_MFR_SST) {
+		write_enable(nor);
+		write_sr(nor, 0);
+	}
+
+	if (data && data->name)
+		mtd->name = data->name;
+	else
+		mtd->name = dev_name(dev);
+
+	mtd->type = MTD_NORFLASH;
+	mtd->writesize = 1;
+	mtd->flags = MTD_CAP_NORFLASH;
+	mtd->size = info->sector_size * info->n_sectors;
+	mtd->_erase = spi_nor_erase;
+	mtd->_read = spi_nor_read;
+
+	/* nor protection support for STmicro chips */
+	if (JEDEC_MFR(info->jedec_id) == CFI_MFR_ST) {
+		mtd->_lock = spi_nor_lock;
+		mtd->_unlock = spi_nor_unlock;
+	}
+
+	/* sst nor chips use AAI word program */
+	if (info->flags & SST_WRITE)
+		mtd->_write = sst_write;
+	else
+		mtd->_write = spi_nor_write;
+
+	/* prefer "small sector" erase if possible */
+	if (info->flags & SECT_4K) {
+		nor->erase_opcode = SPINOR_OP_BE_4K;
+		mtd->erasesize = 4096;
+	} else if (info->flags & SECT_4K_PMC) {
+		nor->erase_opcode = SPINOR_OP_BE_4K_PMC;
+		mtd->erasesize = 4096;
+	} else {
+		nor->erase_opcode = SPINOR_OP_SE;
+		mtd->erasesize = info->sector_size;
+	}
+
+	if (info->flags & SPI_NOR_NO_ERASE)
+		mtd->flags |= MTD_NO_ERASE;
+
+	mtd->dev.parent = dev;
+	nor->page_size = info->page_size;
+	mtd->writebufsize = nor->page_size;
+
+	if (np) {
+		/* If we were instantiated by DT, use it */
+		if (of_property_read_bool(np, "m25p,fast-read"))
+			nor->flash_read = SPI_NOR_FAST;
+		else
+			nor->flash_read = SPI_NOR_NORMAL;
+	} else {
+		/* If we weren't instantiated by DT, default to fast-read */
+		nor->flash_read = SPI_NOR_FAST;
+	}
+
+	/* Some devices cannot do fast-read, no matter what DT tells us */
+	if (info->flags & SPI_NOR_NO_FR)
+		nor->flash_read = SPI_NOR_NORMAL;
+
+	/* Quad/Dual-read mode takes precedence over fast/normal */
+	if (mode == SPI_NOR_QUAD && info->flags & SPI_NOR_QUAD_READ) {
+		ret = set_quad_mode(nor, info->jedec_id);
+		if (ret) {
+			dev_err(dev, "quad mode not supported\n");
+			return ret;
+		}
+		nor->flash_read = SPI_NOR_QUAD;
+	} else if (mode == SPI_NOR_DUAL && info->flags & SPI_NOR_DUAL_READ) {
+		nor->flash_read = SPI_NOR_DUAL;
+	}
+
+	/* Default commands */
+	switch (nor->flash_read) {
+	case SPI_NOR_QUAD:
+		nor->read_opcode = SPINOR_OP_READ_1_1_4;
+		break;
+	case SPI_NOR_DUAL:
+		nor->read_opcode = SPINOR_OP_READ_1_1_2;
+		break;
+	case SPI_NOR_FAST:
+		nor->read_opcode = SPINOR_OP_READ_FAST;
+		break;
+	case SPI_NOR_NORMAL:
+		nor->read_opcode = SPINOR_OP_READ;
+		break;
+	default:
+		dev_err(dev, "No Read opcode defined\n");
+		return -EINVAL;
+	}
+
+	nor->program_opcode = SPINOR_OP_PP;
+
+	if (info->addr_width)
+		nor->addr_width = info->addr_width;
+	else if (mtd->size > 0x1000000) {
+		/* enable 4-byte addressing if the device exceeds 16MiB */
+		nor->addr_width = 4;
+		if (JEDEC_MFR(info->jedec_id) == CFI_MFR_AMD) {
+			/* Dedicated 4-byte command set */
+			switch (nor->flash_read) {
+			case SPI_NOR_QUAD:
+				nor->read_opcode = SPINOR_OP_READ4_1_1_4;
+				break;
+			case SPI_NOR_DUAL:
+				nor->read_opcode = SPINOR_OP_READ4_1_1_2;
+				break;
+			case SPI_NOR_FAST:
+				nor->read_opcode = SPINOR_OP_READ4_FAST;
+				break;
+			case SPI_NOR_NORMAL:
+				nor->read_opcode = SPINOR_OP_READ4;
+				break;
+			}
+			nor->program_opcode = SPINOR_OP_PP_4B;
+			/* No small sector erase for 4-byte command set */
+			nor->erase_opcode = SPINOR_OP_SE_4B;
+			mtd->erasesize = info->sector_size;
+		} else
+			set_4byte(nor, info->jedec_id, 1);
+	} else {
+		nor->addr_width = 3;
+	}
+
+	nor->read_dummy = spi_nor_read_dummy_cycles(nor);
+
+	dev_info(dev, "%s (%lld Kbytes)\n", id->name,
+			(long long)mtd->size >> 10);
+
+	dev_dbg(dev,
+		"mtd .name = %s, .size = 0x%llx (%lldMiB), "
+		".erasesize = 0x%.8x (%uKiB) .numeraseregions = %d\n",
+		mtd->name, (long long)mtd->size, (long long)(mtd->size >> 20),
+		mtd->erasesize, mtd->erasesize / 1024, mtd->numeraseregions);
+
+	if (mtd->numeraseregions)
+		for (i = 0; i < mtd->numeraseregions; i++)
+			dev_dbg(dev,
+				"mtd.eraseregions[%d] = { .offset = 0x%llx, "
+				".erasesize = 0x%.8x (%uKiB), "
+				".numblocks = %d }\n",
+				i, (long long)mtd->eraseregions[i].offset,
+				mtd->eraseregions[i].erasesize,
+				mtd->eraseregions[i].erasesize / 1024,
+				mtd->eraseregions[i].numblocks);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(spi_nor_scan);
+
+const struct spi_device_id *spi_nor_match_id(char *name)
+{
+	const struct spi_device_id *id = spi_nor_ids;
+
+	while (id->name[0]) {
+		if (!strcmp(name, id->name))
+			return id;
+		id++;
+	}
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(spi_nor_match_id);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Huang Shijie <shijie8@gmail.com>");
+MODULE_AUTHOR("Mike Lavender");
+MODULE_DESCRIPTION("framework for SPI NOR");
diff --git a/drivers/mtd/tests/oobtest.c b/drivers/mtd/tests/oobtest.c
index 2e9e2d11f20..f19ab1acde1 100644
--- a/drivers/mtd/tests/oobtest.c
+++ b/drivers/mtd/tests/oobtest.c
@@ -69,8 +69,8 @@ static int write_eraseblock(int ebnum)
 	int err = 0;
 	loff_t addr = ebnum * mtd->erasesize;
 
+	prandom_bytes_state(&rnd_state, writebuf, use_len_max * pgcnt);
 	for (i = 0; i < pgcnt; ++i, addr += mtd->writesize) {
-		prandom_bytes_state(&rnd_state, writebuf, use_len);
 		ops.mode      = MTD_OPS_AUTO_OOB;
 		ops.len       = 0;
 		ops.retlen    = 0;
@@ -78,7 +78,7 @@ static int write_eraseblock(int ebnum)
 		ops.oobretlen = 0;
 		ops.ooboffs   = use_offset;
 		ops.datbuf    = NULL;
-		ops.oobbuf    = writebuf;
+		ops.oobbuf    = writebuf + (use_len_max * i) + use_offset;
 		err = mtd_write_oob(mtd, addr, &ops);
 		if (err || ops.oobretlen != use_len) {
 			pr_err("error: writeoob failed at %#llx\n",
@@ -122,8 +122,8 @@ static int verify_eraseblock(int ebnum)
 	int err = 0;
 	loff_t addr = ebnum * mtd->erasesize;
 
+	prandom_bytes_state(&rnd_state, writebuf, use_len_max * pgcnt);
 	for (i = 0; i < pgcnt; ++i, addr += mtd->writesize) {
-		prandom_bytes_state(&rnd_state, writebuf, use_len);
 		ops.mode      = MTD_OPS_AUTO_OOB;
 		ops.len       = 0;
 		ops.retlen    = 0;
@@ -139,7 +139,8 @@ static int verify_eraseblock(int ebnum)
 			errcnt += 1;
 			return err ? err : -1;
 		}
-		if (memcmp(readbuf, writebuf, use_len)) {
+		if (memcmp(readbuf, writebuf + (use_len_max * i) + use_offset,
+			   use_len)) {
 			pr_err("error: verify failed at %#llx\n",
 			       (long long)addr);
 			errcnt += 1;
@@ -166,7 +167,9 @@ static int verify_eraseblock(int ebnum)
 				errcnt += 1;
 				return err ? err : -1;
 			}
-			if (memcmp(readbuf + use_offset, writebuf, use_len)) {
+			if (memcmp(readbuf + use_offset,
+				   writebuf + (use_len_max * i) + use_offset,
+				   use_len)) {
 				pr_err("error: verify failed at %#llx\n",
 						(long long)addr);
 				errcnt += 1;
@@ -566,8 +569,8 @@ static int __init mtd_oobtest_init(void)
 		if (bbt[i] || bbt[i + 1])
 			continue;
 		addr = (i + 1) * mtd->erasesize - mtd->writesize;
+		prandom_bytes_state(&rnd_state, writebuf, sz * cnt);
 		for (pg = 0; pg < cnt; ++pg) {
-			prandom_bytes_state(&rnd_state, writebuf, sz);
 			ops.mode      = MTD_OPS_AUTO_OOB;
 			ops.len       = 0;
 			ops.retlen    = 0;
@@ -575,7 +578,7 @@ static int __init mtd_oobtest_init(void)
 			ops.oobretlen = 0;
 			ops.ooboffs   = 0;
 			ops.datbuf    = NULL;
-			ops.oobbuf    = writebuf;
+			ops.oobbuf    = writebuf + pg * sz;
 			err = mtd_write_oob(mtd, addr, &ops);
 			if (err)
 				goto out;
diff --git a/drivers/mtd/ubi/block.c b/drivers/mtd/ubi/block.c
index 8d659e6a1b4..8457df7ec5a 100644
--- a/drivers/mtd/ubi/block.c
+++ b/drivers/mtd/ubi/block.c
@@ -253,7 +253,7 @@ static int do_ubiblock_request(struct ubiblock *dev, struct request *req)
 	 * flash access anyway.
 	 */
 	mutex_lock(&dev->dev_mutex);
-	ret = ubiblock_read(dev, req->buffer, sec, len);
+	ret = ubiblock_read(dev, bio_data(req->bio), sec, len);
 	mutex_unlock(&dev->dev_mutex);
 
 	return ret;
@@ -432,8 +432,10 @@ int ubiblock_create(struct ubi_volume_info *vi)
 	 * Rembember workqueues are cheap, they're not threads.
 	 */
 	dev->wq = alloc_workqueue("%s", 0, 0, gd->disk_name);
-	if (!dev->wq)
+	if (!dev->wq) {
+		ret = -ENOMEM;
 		goto out_free_queue;
+	}
 	INIT_WORK(&dev->work, ubiblock_do_work);
 
 	mutex_lock(&devices_mutex);
diff --git a/drivers/mtd/ubi/cdev.c b/drivers/mtd/ubi/cdev.c
index f54562a5998..7646220ca6e 100644
--- a/drivers/mtd/ubi/cdev.c
+++ b/drivers/mtd/ubi/cdev.c
@@ -731,7 +731,7 @@ static int rename_volumes(struct ubi_device *ubi,
 			goto out_free;
 		}
 
-		re->desc = ubi_open_volume(ubi->ubi_num, vol_id, UBI_EXCLUSIVE);
+		re->desc = ubi_open_volume(ubi->ubi_num, vol_id, UBI_READWRITE);
 		if (IS_ERR(re->desc)) {
 			err = PTR_ERR(re->desc);
 			ubi_err("cannot open volume %d, error %d", vol_id, err);
diff --git a/drivers/mtd/ubi/fastmap.c b/drivers/mtd/ubi/fastmap.c
index c5dad652614..b04e7d05988 100644
--- a/drivers/mtd/ubi/fastmap.c
+++ b/drivers/mtd/ubi/fastmap.c
@@ -125,9 +125,9 @@ static struct ubi_ainf_volume *add_vol(struct ubi_attach_info *ai, int vol_id,
 		parent = *p;
 		av = rb_entry(parent, struct ubi_ainf_volume, rb);
 
-		if (vol_id > av->vol_id)
+		if (vol_id < av->vol_id)
 			p = &(*p)->rb_left;
-		else if (vol_id > av->vol_id)
+		else
 			p = &(*p)->rb_right;
 	}
 
diff --git a/drivers/net/bonding/bond_alb.c b/drivers/net/bonding/bond_alb.c
index 9f69e818b00..93580a47cc5 100644
--- a/drivers/net/bonding/bond_alb.c
+++ b/drivers/net/bonding/bond_alb.c
@@ -82,7 +82,8 @@ static inline struct arp_pkt *arp_pkt(const struct sk_buff *skb)
 }
 
 /* Forward declaration */
-static void alb_send_learning_packets(struct slave *slave, u8 mac_addr[]);
+static void alb_send_learning_packets(struct slave *slave, u8 mac_addr[],
+				      bool strict_match);
 static void rlb_purge_src_ip(struct bonding *bond, struct arp_pkt *arp);
 static void rlb_src_unlink(struct bonding *bond, u32 index);
 static void rlb_src_link(struct bonding *bond, u32 ip_src_hash,
@@ -459,7 +460,7 @@ static void rlb_teach_disabled_mac_on_primary(struct bonding *bond, u8 addr[])
 
 	bond->alb_info.rlb_promisc_timeout_counter = 0;
 
-	alb_send_learning_packets(bond->curr_active_slave, addr);
+	alb_send_learning_packets(bond->curr_active_slave, addr, true);
 }
 
 /* slave being removed should not be active at this point
@@ -995,7 +996,7 @@ static void rlb_clear_vlan(struct bonding *bond, unsigned short vlan_id)
 /*********************** tlb/rlb shared functions *********************/
 
 static void alb_send_lp_vid(struct slave *slave, u8 mac_addr[],
-			    u16 vid)
+			    __be16 vlan_proto, u16 vid)
 {
 	struct learning_pkt pkt;
 	struct sk_buff *skb;
@@ -1021,7 +1022,7 @@ static void alb_send_lp_vid(struct slave *slave, u8 mac_addr[],
 	skb->dev = slave->dev;
 
 	if (vid) {
-		skb = vlan_put_tag(skb, htons(ETH_P_8021Q), vid);
+		skb = vlan_put_tag(skb, vlan_proto, vid);
 		if (!skb) {
 			pr_err("%s: Error: failed to insert VLAN tag\n",
 			       slave->bond->dev->name);
@@ -1032,22 +1033,32 @@ static void alb_send_lp_vid(struct slave *slave, u8 mac_addr[],
 	dev_queue_xmit(skb);
 }
 
-
-static void alb_send_learning_packets(struct slave *slave, u8 mac_addr[])
+static void alb_send_learning_packets(struct slave *slave, u8 mac_addr[],
+				      bool strict_match)
 {
 	struct bonding *bond = bond_get_bond_by_slave(slave);
 	struct net_device *upper;
 	struct list_head *iter;
 
 	/* send untagged */
-	alb_send_lp_vid(slave, mac_addr, 0);
+	alb_send_lp_vid(slave, mac_addr, 0, 0);
 
 	/* loop through vlans and send one packet for each */
 	rcu_read_lock();
 	netdev_for_each_all_upper_dev_rcu(bond->dev, upper, iter) {
-		if (upper->priv_flags & IFF_802_1Q_VLAN)
-			alb_send_lp_vid(slave, mac_addr,
-					vlan_dev_vlan_id(upper));
+		if (is_vlan_dev(upper) && vlan_get_encap_level(upper) == 0) {
+			if (strict_match &&
+			    ether_addr_equal_64bits(mac_addr,
+						    upper->dev_addr)) {
+				alb_send_lp_vid(slave, mac_addr,
+						vlan_dev_vlan_proto(upper),
+						vlan_dev_vlan_id(upper));
+			} else if (!strict_match) {
+				alb_send_lp_vid(slave, upper->dev_addr,
+						vlan_dev_vlan_proto(upper),
+						vlan_dev_vlan_id(upper));
+			}
+		}
 	}
 	rcu_read_unlock();
 }
@@ -1107,7 +1118,7 @@ static void alb_fasten_mac_swap(struct bonding *bond, struct slave *slave1,
 
 	/* fasten the change in the switch */
 	if (SLAVE_IS_OK(slave1)) {
-		alb_send_learning_packets(slave1, slave1->dev->dev_addr);
+		alb_send_learning_packets(slave1, slave1->dev->dev_addr, false);
 		if (bond->alb_info.rlb_enabled) {
 			/* inform the clients that the mac address
 			 * has changed
@@ -1119,7 +1130,7 @@ static void alb_fasten_mac_swap(struct bonding *bond, struct slave *slave1,
 	}
 
 	if (SLAVE_IS_OK(slave2)) {
-		alb_send_learning_packets(slave2, slave2->dev->dev_addr);
+		alb_send_learning_packets(slave2, slave2->dev->dev_addr, false);
 		if (bond->alb_info.rlb_enabled) {
 			/* inform the clients that the mac address
 			 * has changed
@@ -1490,6 +1501,8 @@ void bond_alb_monitor(struct work_struct *work)
 
 	/* send learning packets */
 	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
@@ -1497,8 +1510,15 @@ void bond_alb_monitor(struct work_struct *work)
 		 */
 		read_lock(&bond->curr_slave_lock);
 
-		bond_for_each_slave_rcu(bond, slave, iter)
-			alb_send_learning_packets(slave, slave->dev->dev_addr);
+		bond_for_each_slave_rcu(bond, slave, iter) {
+			/* If updating current_active, use all currently
+			 * user mac addreses (!strict_match).  Otherwise, only
+			 * use mac of the slave device.
+			 */
+			strict_match = (slave != bond->curr_active_slave);
+			alb_send_learning_packets(slave, slave->dev->dev_addr,
+						  strict_match);
+		}
 
 		read_unlock(&bond->curr_slave_lock);
 
@@ -1721,7 +1741,8 @@ void bond_alb_handle_active_change(struct bonding *bond, struct slave *new_slave
 	} else {
 		/* set the new_slave to the bond mac address */
 		alb_set_slave_mac_addr(new_slave, bond->dev->dev_addr);
-		alb_send_learning_packets(new_slave, bond->dev->dev_addr);
+		alb_send_learning_packets(new_slave, bond->dev->dev_addr,
+					  false);
 	}
 
 	write_lock_bh(&bond->curr_slave_lock);
@@ -1764,7 +1785,8 @@ int bond_alb_set_mac_address(struct net_device *bond_dev, void *addr)
 		alb_set_slave_mac_addr(bond->curr_active_slave, bond_dev->dev_addr);
 
 		read_lock(&bond->lock);
-		alb_send_learning_packets(bond->curr_active_slave, bond_dev->dev_addr);
+		alb_send_learning_packets(bond->curr_active_slave,
+					  bond_dev->dev_addr, false);
 		if (bond->alb_info.rlb_enabled) {
 			/* inform clients mac address has changed */
 			rlb_req_update_slave_clients(bond, bond->curr_active_slave);
diff --git a/drivers/net/bonding/bond_main.c b/drivers/net/bonding/bond_main.c
index 69aff72c895..d3a67896d43 100644
--- a/drivers/net/bonding/bond_main.c
+++ b/drivers/net/bonding/bond_main.c
@@ -2126,10 +2126,10 @@ static bool bond_has_this_ip(struct bonding *bond, __be32 ip)
  */
 static void bond_arp_send(struct net_device *slave_dev, int arp_op,
 			  __be32 dest_ip, __be32 src_ip,
-			  struct bond_vlan_tag *inner,
-			  struct bond_vlan_tag *outer)
+			  struct bond_vlan_tag *tags)
 {
 	struct sk_buff *skb;
+	int i;
 
 	pr_debug("arp %d on slave %s: dst %pI4 src %pI4\n",
 		 arp_op, slave_dev->name, &dest_ip, &src_ip);
@@ -2141,21 +2141,26 @@ static void bond_arp_send(struct net_device *slave_dev, int arp_op,
 		net_err_ratelimited("ARP packet allocation failed\n");
 		return;
 	}
-	if (outer->vlan_id) {
-		if (inner->vlan_id) {
-			pr_debug("inner tag: proto %X vid %X\n",
-				 ntohs(inner->vlan_proto), inner->vlan_id);
-			skb = __vlan_put_tag(skb, inner->vlan_proto,
-					     inner->vlan_id);
-			if (!skb) {
-				net_err_ratelimited("failed to insert inner VLAN tag\n");
-				return;
-			}
-		}
 
-		pr_debug("outer reg: proto %X vid %X\n",
-			 ntohs(outer->vlan_proto), outer->vlan_id);
-		skb = vlan_put_tag(skb, outer->vlan_proto, outer->vlan_id);
+	/* Go through all the tags backwards and add them to the packet */
+	for (i = BOND_MAX_VLAN_ENCAP - 1; i > 0; i--) {
+		if (!tags[i].vlan_id)
+			continue;
+
+		pr_debug("inner tag: proto %X vid %X\n",
+			 ntohs(tags[i].vlan_proto), tags[i].vlan_id);
+		skb = __vlan_put_tag(skb, tags[i].vlan_proto,
+				     tags[i].vlan_id);
+		if (!skb) {
+			net_err_ratelimited("failed to insert inner VLAN tag\n");
+			return;
+		}
+	}
+	/* Set the outer tag */
+	if (tags[0].vlan_id) {
+		pr_debug("outer tag: proto %X vid %X\n",
+			 ntohs(tags[0].vlan_proto), tags[0].vlan_id);
+		skb = vlan_put_tag(skb, tags[0].vlan_proto, tags[0].vlan_id);
 		if (!skb) {
 			net_err_ratelimited("failed to insert outer VLAN tag\n");
 			return;
@@ -2164,22 +2169,52 @@ static void bond_arp_send(struct net_device *slave_dev, int arp_op,
 	arp_xmit(skb);
 }
 
+/* Validate the device path between the @start_dev and the @end_dev.
+ * The path is valid if the @end_dev is reachable through device
+ * stacking.
+ * When the path is validated, collect any vlan information in the
+ * path.
+ */
+static bool bond_verify_device_path(struct net_device *start_dev,
+				    struct net_device *end_dev,
+				    struct bond_vlan_tag *tags)
+{
+	struct net_device *upper;
+	struct list_head  *iter;
+	int  idx;
+
+	if (start_dev == end_dev)
+		return true;
+
+	netdev_for_each_upper_dev_rcu(start_dev, upper, iter) {
+		if (bond_verify_device_path(upper, end_dev, tags)) {
+			if (is_vlan_dev(upper)) {
+				idx = vlan_get_encap_level(upper);
+				if (idx >= BOND_MAX_VLAN_ENCAP)
+					return false;
+
+				tags[idx].vlan_proto =
+						    vlan_dev_vlan_proto(upper);
+				tags[idx].vlan_id = vlan_dev_vlan_id(upper);
+			}
+			return true;
+		}
+	}
+
+	return false;
+}
 
 static void bond_arp_send_all(struct bonding *bond, struct slave *slave)
 {
-	struct net_device *upper, *vlan_upper;
-	struct list_head *iter, *vlan_iter;
 	struct rtable *rt;
-	struct bond_vlan_tag inner, outer;
+	struct bond_vlan_tag tags[BOND_MAX_VLAN_ENCAP];
 	__be32 *targets = bond->params.arp_targets, addr;
 	int i;
+	bool ret;
 
 	for (i = 0; i < BOND_MAX_ARP_TARGETS && targets[i]; i++) {
 		pr_debug("basa: target %pI4\n", &targets[i]);
-		inner.vlan_proto = 0;
-		inner.vlan_id = 0;
-		outer.vlan_proto = 0;
-		outer.vlan_id = 0;
+		memset(tags, 0, sizeof(tags));
 
 		/* Find out through which dev should the packet go */
 		rt = ip_route_output(dev_net(bond->dev), targets[i], 0,
@@ -2192,7 +2227,8 @@ static void bond_arp_send_all(struct bonding *bond, struct slave *slave)
 				net_warn_ratelimited("%s: no route to arp_ip_target %pI4 and arp_validate is set\n",
 						     bond->dev->name,
 						     &targets[i]);
-			bond_arp_send(slave->dev, ARPOP_REQUEST, targets[i], 0, &inner, &outer);
+			bond_arp_send(slave->dev, ARPOP_REQUEST, targets[i],
+				      0, tags);
 			continue;
 		}
 
@@ -2201,52 +2237,12 @@ static void bond_arp_send_all(struct bonding *bond, struct slave *slave)
 			goto found;
 
 		rcu_read_lock();
-		/* first we search only for vlan devices. for every vlan
-		 * found we verify its upper dev list, searching for the
-		 * rt->dst.dev. If found we save the tag of the vlan and
-		 * proceed to send the packet.
-		 */
-		netdev_for_each_all_upper_dev_rcu(bond->dev, vlan_upper,
-						  vlan_iter) {
-			if (!is_vlan_dev(vlan_upper))
-				continue;
-
-			if (vlan_upper == rt->dst.dev) {
-				outer.vlan_proto = vlan_dev_vlan_proto(vlan_upper);
-				outer.vlan_id = vlan_dev_vlan_id(vlan_upper);
-				rcu_read_unlock();
-				goto found;
-			}
-			netdev_for_each_all_upper_dev_rcu(vlan_upper, upper,
-							  iter) {
-				if (upper == rt->dst.dev) {
-					/* If the upper dev is a vlan dev too,
-					 *  set the vlan tag to inner tag.
-					 */
-					if (is_vlan_dev(upper)) {
-						inner.vlan_proto = vlan_dev_vlan_proto(upper);
-						inner.vlan_id = vlan_dev_vlan_id(upper);
-					}
-					outer.vlan_proto = vlan_dev_vlan_proto(vlan_upper);
-					outer.vlan_id = vlan_dev_vlan_id(vlan_upper);
-					rcu_read_unlock();
-					goto found;
-				}
-			}
-		}
-
-		/* if the device we're looking for is not on top of any of
-		 * our upper vlans, then just search for any dev that
-		 * matches, and in case it's a vlan - save the id
-		 */
-		netdev_for_each_all_upper_dev_rcu(bond->dev, upper, iter) {
-			if (upper == rt->dst.dev) {
-				rcu_read_unlock();
-				goto found;
-			}
-		}
+		ret = bond_verify_device_path(bond->dev, rt->dst.dev, tags);
 		rcu_read_unlock();
 
+		if (ret)
+			goto found;
+
 		/* Not our device - skip */
 		pr_debug("%s: no path to arp_ip_target %pI4 via rt.dev %s\n",
 			 bond->dev->name, &targets[i],
@@ -2259,7 +2255,7 @@ found:
 		addr = bond_confirm_addr(rt->dst.dev, targets[i], 0);
 		ip_rt_put(rt);
 		bond_arp_send(slave->dev, ARPOP_REQUEST, targets[i],
-			      addr, &inner, &outer);
+			      addr, tags);
 	}
 }
 
diff --git a/drivers/net/bonding/bond_options.c b/drivers/net/bonding/bond_options.c
index 724e30fa20b..83207029844 100644
--- a/drivers/net/bonding/bond_options.c
+++ b/drivers/net/bonding/bond_options.c
@@ -125,6 +125,7 @@ static const struct bond_opt_value bond_fail_over_mac_tbl[] = {
 static const struct bond_opt_value bond_intmax_tbl[] = {
 	{ "off",     0,       BOND_VALFLAG_DEFAULT},
 	{ "maxval",  INT_MAX, BOND_VALFLAG_MAX},
+	{ NULL,      -1,      0}
 };
 
 static const struct bond_opt_value bond_lacp_rate_tbl[] = {
diff --git a/drivers/net/bonding/bonding.h b/drivers/net/bonding/bonding.h
index b8bdd0acc8f..00bea320e3b 100644
--- a/drivers/net/bonding/bonding.h
+++ b/drivers/net/bonding/bonding.h
@@ -36,6 +36,7 @@
 
 #define bond_version DRV_DESCRIPTION ": v" DRV_VERSION " (" DRV_RELDATE ")\n"
 
+#define BOND_MAX_VLAN_ENCAP	2
 #define BOND_MAX_ARP_TARGETS	16
 
 #define BOND_DEFAULT_MIIMON	100
diff --git a/drivers/net/can/c_can/Kconfig b/drivers/net/can/c_can/Kconfig
index 8ab7103d4f4..61ffc12d8fd 100644
--- a/drivers/net/can/c_can/Kconfig
+++ b/drivers/net/can/c_can/Kconfig
@@ -14,13 +14,6 @@ config CAN_C_CAN_PLATFORM
 	  SPEAr1310 and SPEAr320 evaluation boards & TI (www.ti.com)
 	  boards like am335x, dm814x, dm813x and dm811x.
 
-config CAN_C_CAN_STRICT_FRAME_ORDERING
-	bool "Force a strict RX CAN frame order (may cause frame loss)"
-	---help---
-	  The RX split buffer prevents packet reordering but can cause packet
-	  loss. Only enable this option when you accept to lose CAN frames
-	  in favour of getting the received CAN frames in the correct order.
-
 config CAN_C_CAN_PCI
 	tristate "Generic PCI Bus based C_CAN/D_CAN driver"
 	depends on PCI
diff --git a/drivers/net/can/c_can/c_can.c b/drivers/net/can/c_can/c_can.c
index a2ca820b537..95e04e2002d 100644
--- a/drivers/net/can/c_can/c_can.c
+++ b/drivers/net/can/c_can/c_can.c
@@ -732,26 +732,12 @@ static u32 c_can_adjust_pending(u32 pend)
 static inline void c_can_rx_object_get(struct net_device *dev,
 				       struct c_can_priv *priv, u32 obj)
 {
-#ifdef CONFIG_CAN_C_CAN_STRICT_FRAME_ORDERING
-	if (obj < C_CAN_MSG_RX_LOW_LAST)
-		c_can_object_get(dev, IF_RX, obj, IF_COMM_RCV_LOW);
-	else
-#endif
 		c_can_object_get(dev, IF_RX, obj, priv->comm_rcv_high);
 }
 
 static inline void c_can_rx_finalize(struct net_device *dev,
 				     struct c_can_priv *priv, u32 obj)
 {
-#ifdef CONFIG_CAN_C_CAN_STRICT_FRAME_ORDERING
-	if (obj < C_CAN_MSG_RX_LOW_LAST)
-		priv->rxmasked |= BIT(obj - 1);
-	else if (obj == C_CAN_MSG_RX_LOW_LAST) {
-		priv->rxmasked = 0;
-		/* activate all lower message objects */
-		c_can_activate_all_lower_rx_msg_obj(dev, IF_RX);
-	}
-#endif
 	if (priv->type != BOSCH_D_CAN)
 		c_can_object_get(dev, IF_RX, obj, IF_COMM_CLR_NEWDAT);
 }
@@ -799,9 +785,6 @@ static inline u32 c_can_get_pending(struct c_can_priv *priv)
 {
 	u32 pend = priv->read_reg(priv, C_CAN_NEWDAT1_REG);
 
-#ifdef CONFIG_CAN_C_CAN_STRICT_FRAME_ORDERING
-	pend &= ~priv->rxmasked;
-#endif
 	return pend;
 }
 
@@ -814,25 +797,6 @@ static inline u32 c_can_get_pending(struct c_can_priv *priv)
  * has arrived. To work-around this issue, we keep two groups of message
  * objects whose partitioning is defined by C_CAN_MSG_OBJ_RX_SPLIT.
  *
- * If CONFIG_CAN_C_CAN_STRICT_FRAME_ORDERING = y
- *
- * To ensure in-order frame reception we use the following
- * approach while re-activating a message object to receive further
- * frames:
- * - if the current message object number is lower than
- *   C_CAN_MSG_RX_LOW_LAST, do not clear the NEWDAT bit while clearing
- *   the INTPND bit.
- * - if the current message object number is equal to
- *   C_CAN_MSG_RX_LOW_LAST then clear the NEWDAT bit of all lower
- *   receive message objects.
- * - if the current message object number is greater than
- *   C_CAN_MSG_RX_LOW_LAST then clear the NEWDAT bit of
- *   only this message object.
- *
- * This can cause packet loss!
- *
- * If CONFIG_CAN_C_CAN_STRICT_FRAME_ORDERING = n
- *
  * We clear the newdat bit right away.
  *
  * This can result in packet reordering when the readout is slow.
diff --git a/drivers/net/can/led.c b/drivers/net/can/led.c
index a3d99a8fd2d..ab7f1b01be4 100644
--- a/drivers/net/can/led.c
+++ b/drivers/net/can/led.c
@@ -97,6 +97,9 @@ static int can_led_notifier(struct notifier_block *nb, unsigned long msg,
 	if (!priv)
 		return NOTIFY_DONE;
 
+	if (!priv->tx_led_trig || !priv->rx_led_trig)
+		return NOTIFY_DONE;
+
 	if (msg == NETDEV_CHANGENAME) {
 		snprintf(name, sizeof(name), "%s-tx", netdev->name);
 		led_trigger_rename_static(name, priv->tx_led_trig);
diff --git a/drivers/net/can/sja1000/peak_pci.c b/drivers/net/can/sja1000/peak_pci.c
index c540e3d12e3..564933ae218 100644
--- a/drivers/net/can/sja1000/peak_pci.c
+++ b/drivers/net/can/sja1000/peak_pci.c
@@ -551,7 +551,7 @@ static int peak_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
 	struct sja1000_priv *priv;
 	struct peak_pci_chan *chan;
-	struct net_device *dev;
+	struct net_device *dev, *prev_dev;
 	void __iomem *cfg_base, *reg_base;
 	u16 sub_sys_id, icr;
 	int i, err, channels;
@@ -688,11 +688,13 @@ failure_remove_channels:
 	writew(0x0, cfg_base + PITA_ICR + 2);
 
 	chan = NULL;
-	for (dev = pci_get_drvdata(pdev); dev; dev = chan->prev_dev) {
-		unregister_sja1000dev(dev);
-		free_sja1000dev(dev);
+	for (dev = pci_get_drvdata(pdev); dev; dev = prev_dev) {
 		priv = netdev_priv(dev);
 		chan = priv->priv;
+		prev_dev = chan->prev_dev;
+
+		unregister_sja1000dev(dev);
+		free_sja1000dev(dev);
 	}
 
 	/* free any PCIeC resources too */
@@ -726,10 +728,12 @@ static void peak_pci_remove(struct pci_dev *pdev)
 
 	/* Loop over all registered devices */
 	while (1) {
+		struct net_device *prev_dev = chan->prev_dev;
+
 		dev_info(&pdev->dev, "removing device %s\n", dev->name);
 		unregister_sja1000dev(dev);
 		free_sja1000dev(dev);
-		dev = chan->prev_dev;
+		dev = prev_dev;
 
 		if (!dev) {
 			/* do that only for first channel */
diff --git a/drivers/net/ethernet/Kconfig b/drivers/net/ethernet/Kconfig
index 39b26fe28d1..05134945846 100644
--- a/drivers/net/ethernet/Kconfig
+++ b/drivers/net/ethernet/Kconfig
@@ -35,6 +35,19 @@ source "drivers/net/ethernet/calxeda/Kconfig"
 source "drivers/net/ethernet/chelsio/Kconfig"
 source "drivers/net/ethernet/cirrus/Kconfig"
 source "drivers/net/ethernet/cisco/Kconfig"
+
+config CX_ECAT
+	tristate "Beckhoff CX5020 EtherCAT master support"
+	depends on PCI
+	depends on X86 || COMPILE_TEST
+	---help---
+	  Driver for EtherCAT master module located on CCAT FPGA
+	  that can be found on Beckhoff CX5020, and possibly other of CX
+	  Beckhoff CX series industrial PCs.
+
+	  To compile this driver as a module, choose M here. The module
+	  will be called ec_bhf.
+
 source "drivers/net/ethernet/davicom/Kconfig"
 
 config DNET
diff --git a/drivers/net/ethernet/Makefile b/drivers/net/ethernet/Makefile
index 545d0b3b9cb..35190e36c45 100644
--- a/drivers/net/ethernet/Makefile
+++ b/drivers/net/ethernet/Makefile
@@ -21,6 +21,7 @@ obj-$(CONFIG_NET_CALXEDA_XGMAC) += calxeda/
 obj-$(CONFIG_NET_VENDOR_CHELSIO) += chelsio/
 obj-$(CONFIG_NET_VENDOR_CIRRUS) += cirrus/
 obj-$(CONFIG_NET_VENDOR_CISCO) += cisco/
+obj-$(CONFIG_CX_ECAT) += ec_bhf.o
 obj-$(CONFIG_DM9000) += davicom/
 obj-$(CONFIG_DNET) += dnet.o
 obj-$(CONFIG_NET_VENDOR_DEC) += dec/
diff --git a/drivers/net/ethernet/altera/Makefile b/drivers/net/ethernet/altera/Makefile
index d4a187e4536..3eff2fd3997 100644
--- a/drivers/net/ethernet/altera/Makefile
+++ b/drivers/net/ethernet/altera/Makefile
@@ -5,3 +5,4 @@
 obj-$(CONFIG_ALTERA_TSE) += altera_tse.o
 altera_tse-objs := altera_tse_main.o altera_tse_ethtool.o \
 altera_msgdma.o altera_sgdma.o altera_utils.o
+ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/net/ethernet/altera/altera_msgdma.c b/drivers/net/ethernet/altera/altera_msgdma.c
index 4d1f2fdd5c3..0fb986ba329 100644
--- a/drivers/net/ethernet/altera/altera_msgdma.c
+++ b/drivers/net/ethernet/altera/altera_msgdma.c
@@ -37,18 +37,16 @@ void msgdma_start_rxdma(struct altera_tse_private *priv)
 void msgdma_reset(struct altera_tse_private *priv)
 {
 	int counter;
-	struct msgdma_csr *txcsr =
-		(struct msgdma_csr *)priv->tx_dma_csr;
-	struct msgdma_csr *rxcsr =
-		(struct msgdma_csr *)priv->rx_dma_csr;
 
 	/* Reset Rx mSGDMA */
-	iowrite32(MSGDMA_CSR_STAT_MASK, &rxcsr->status);
-	iowrite32(MSGDMA_CSR_CTL_RESET, &rxcsr->control);
+	csrwr32(MSGDMA_CSR_STAT_MASK, priv->rx_dma_csr,
+		msgdma_csroffs(status));
+	csrwr32(MSGDMA_CSR_CTL_RESET, priv->rx_dma_csr,
+		msgdma_csroffs(control));
 
 	counter = 0;
 	while (counter++ < ALTERA_TSE_SW_RESET_WATCHDOG_CNTR) {
-		if (tse_bit_is_clear(&rxcsr->status,
+		if (tse_bit_is_clear(priv->rx_dma_csr, msgdma_csroffs(status),
 				     MSGDMA_CSR_STAT_RESETTING))
 			break;
 		udelay(1);
@@ -59,15 +57,18 @@ void msgdma_reset(struct altera_tse_private *priv)
 			   "TSE Rx mSGDMA resetting bit never cleared!\n");
 
 	/* clear all status bits */
-	iowrite32(MSGDMA_CSR_STAT_MASK, &rxcsr->status);
+	csrwr32(MSGDMA_CSR_STAT_MASK, priv->rx_dma_csr, msgdma_csroffs(status));
 
 	/* Reset Tx mSGDMA */
-	iowrite32(MSGDMA_CSR_STAT_MASK, &txcsr->status);
-	iowrite32(MSGDMA_CSR_CTL_RESET, &txcsr->control);
+	csrwr32(MSGDMA_CSR_STAT_MASK, priv->tx_dma_csr,
+		msgdma_csroffs(status));
+
+	csrwr32(MSGDMA_CSR_CTL_RESET, priv->tx_dma_csr,
+		msgdma_csroffs(control));
 
 	counter = 0;
 	while (counter++ < ALTERA_TSE_SW_RESET_WATCHDOG_CNTR) {
-		if (tse_bit_is_clear(&txcsr->status,
+		if (tse_bit_is_clear(priv->tx_dma_csr, msgdma_csroffs(status),
 				     MSGDMA_CSR_STAT_RESETTING))
 			break;
 		udelay(1);
@@ -78,58 +79,58 @@ void msgdma_reset(struct altera_tse_private *priv)
 			   "TSE Tx mSGDMA resetting bit never cleared!\n");
 
 	/* clear all status bits */
-	iowrite32(MSGDMA_CSR_STAT_MASK, &txcsr->status);
+	csrwr32(MSGDMA_CSR_STAT_MASK, priv->tx_dma_csr, msgdma_csroffs(status));
 }
 
 void msgdma_disable_rxirq(struct altera_tse_private *priv)
 {
-	struct msgdma_csr *csr = priv->rx_dma_csr;
-	tse_clear_bit(&csr->control, MSGDMA_CSR_CTL_GLOBAL_INTR);
+	tse_clear_bit(priv->rx_dma_csr, msgdma_csroffs(control),
+		      MSGDMA_CSR_CTL_GLOBAL_INTR);
 }
 
 void msgdma_enable_rxirq(struct altera_tse_private *priv)
 {
-	struct msgdma_csr *csr = priv->rx_dma_csr;
-	tse_set_bit(&csr->control, MSGDMA_CSR_CTL_GLOBAL_INTR);
+	tse_set_bit(priv->rx_dma_csr, msgdma_csroffs(control),
+		    MSGDMA_CSR_CTL_GLOBAL_INTR);
 }
 
 void msgdma_disable_txirq(struct altera_tse_private *priv)
 {
-	struct msgdma_csr *csr = priv->tx_dma_csr;
-	tse_clear_bit(&csr->control, MSGDMA_CSR_CTL_GLOBAL_INTR);
+	tse_clear_bit(priv->tx_dma_csr, msgdma_csroffs(control),
+		      MSGDMA_CSR_CTL_GLOBAL_INTR);
 }
 
 void msgdma_enable_txirq(struct altera_tse_private *priv)
 {
-	struct msgdma_csr *csr = priv->tx_dma_csr;
-	tse_set_bit(&csr->control, MSGDMA_CSR_CTL_GLOBAL_INTR);
+	tse_set_bit(priv->tx_dma_csr, msgdma_csroffs(control),
+		    MSGDMA_CSR_CTL_GLOBAL_INTR);
 }
 
 void msgdma_clear_rxirq(struct altera_tse_private *priv)
 {
-	struct msgdma_csr *csr = priv->rx_dma_csr;
-	iowrite32(MSGDMA_CSR_STAT_IRQ, &csr->status);
+	csrwr32(MSGDMA_CSR_STAT_IRQ, priv->rx_dma_csr, msgdma_csroffs(status));
 }
 
 void msgdma_clear_txirq(struct altera_tse_private *priv)
 {
-	struct msgdma_csr *csr = priv->tx_dma_csr;
-	iowrite32(MSGDMA_CSR_STAT_IRQ, &csr->status);
+	csrwr32(MSGDMA_CSR_STAT_IRQ, priv->tx_dma_csr, msgdma_csroffs(status));
 }
 
 /* return 0 to indicate transmit is pending */
 int msgdma_tx_buffer(struct altera_tse_private *priv, struct tse_buffer *buffer)
 {
-	struct msgdma_extended_desc *desc = priv->tx_dma_desc;
-
-	iowrite32(lower_32_bits(buffer->dma_addr), &desc->read_addr_lo);
-	iowrite32(upper_32_bits(buffer->dma_addr), &desc->read_addr_hi);
-	iowrite32(0, &desc->write_addr_lo);
-	iowrite32(0, &desc->write_addr_hi);
-	iowrite32(buffer->len, &desc->len);
-	iowrite32(0, &desc->burst_seq_num);
-	iowrite32(MSGDMA_DESC_TX_STRIDE, &desc->stride);
-	iowrite32(MSGDMA_DESC_CTL_TX_SINGLE, &desc->control);
+	csrwr32(lower_32_bits(buffer->dma_addr), priv->tx_dma_desc,
+		msgdma_descroffs(read_addr_lo));
+	csrwr32(upper_32_bits(buffer->dma_addr), priv->tx_dma_desc,
+		msgdma_descroffs(read_addr_hi));
+	csrwr32(0, priv->tx_dma_desc, msgdma_descroffs(write_addr_lo));
+	csrwr32(0, priv->tx_dma_desc, msgdma_descroffs(write_addr_hi));
+	csrwr32(buffer->len, priv->tx_dma_desc, msgdma_descroffs(len));
+	csrwr32(0, priv->tx_dma_desc, msgdma_descroffs(burst_seq_num));
+	csrwr32(MSGDMA_DESC_TX_STRIDE, priv->tx_dma_desc,
+		msgdma_descroffs(stride));
+	csrwr32(MSGDMA_DESC_CTL_TX_SINGLE, priv->tx_dma_desc,
+		msgdma_descroffs(control));
 	return 0;
 }
 
@@ -138,17 +139,16 @@ u32 msgdma_tx_completions(struct altera_tse_private *priv)
 	u32 ready = 0;
 	u32 inuse;
 	u32 status;
-	struct msgdma_csr *txcsr =
-		(struct msgdma_csr *)priv->tx_dma_csr;
 
 	/* Get number of sent descriptors */
-	inuse = ioread32(&txcsr->rw_fill_level) & 0xffff;
+	inuse = csrrd32(priv->tx_dma_csr, msgdma_csroffs(rw_fill_level))
+			& 0xffff;
 
 	if (inuse) { /* Tx FIFO is not empty */
 		ready = priv->tx_prod - priv->tx_cons - inuse - 1;
 	} else {
 		/* Check for buffered last packet */
-		status = ioread32(&txcsr->status);
+		status = csrrd32(priv->tx_dma_csr, msgdma_csroffs(status));
 		if (status & MSGDMA_CSR_STAT_BUSY)
 			ready = priv->tx_prod - priv->tx_cons - 1;
 		else
@@ -162,7 +162,6 @@ u32 msgdma_tx_completions(struct altera_tse_private *priv)
 void msgdma_add_rx_desc(struct altera_tse_private *priv,
 			struct tse_buffer *rxbuffer)
 {
-	struct msgdma_extended_desc *desc = priv->rx_dma_desc;
 	u32 len = priv->rx_dma_buf_sz;
 	dma_addr_t dma_addr = rxbuffer->dma_addr;
 	u32 control = (MSGDMA_DESC_CTL_END_ON_EOP
@@ -172,14 +171,16 @@ void msgdma_add_rx_desc(struct altera_tse_private *priv,
 			| MSGDMA_DESC_CTL_TR_ERR_IRQ
 			| MSGDMA_DESC_CTL_GO);
 
-	iowrite32(0, &desc->read_addr_lo);
-	iowrite32(0, &desc->read_addr_hi);
-	iowrite32(lower_32_bits(dma_addr), &desc->write_addr_lo);
-	iowrite32(upper_32_bits(dma_addr), &desc->write_addr_hi);
-	iowrite32(len, &desc->len);
-	iowrite32(0, &desc->burst_seq_num);
-	iowrite32(0x00010001, &desc->stride);
-	iowrite32(control, &desc->control);
+	csrwr32(0, priv->rx_dma_desc, msgdma_descroffs(read_addr_lo));
+	csrwr32(0, priv->rx_dma_desc, msgdma_descroffs(read_addr_hi));
+	csrwr32(lower_32_bits(dma_addr), priv->rx_dma_desc,
+		msgdma_descroffs(write_addr_lo));
+	csrwr32(upper_32_bits(dma_addr), priv->rx_dma_desc,
+		msgdma_descroffs(write_addr_hi));
+	csrwr32(len, priv->rx_dma_desc, msgdma_descroffs(len));
+	csrwr32(0, priv->rx_dma_desc, msgdma_descroffs(burst_seq_num));
+	csrwr32(0x00010001, priv->rx_dma_desc, msgdma_descroffs(stride));
+	csrwr32(control, priv->rx_dma_desc, msgdma_descroffs(control));
 }
 
 /* status is returned on upper 16 bits,
@@ -190,14 +191,13 @@ u32 msgdma_rx_status(struct altera_tse_private *priv)
 	u32 rxstatus = 0;
 	u32 pktlength;
 	u32 pktstatus;
-	struct msgdma_csr *rxcsr =
-		(struct msgdma_csr *)priv->rx_dma_csr;
-	struct msgdma_response *rxresp =
-		(struct msgdma_response *)priv->rx_dma_resp;
-
-	if (ioread32(&rxcsr->resp_fill_level) & 0xffff) {
-		pktlength = ioread32(&rxresp->bytes_transferred);
-		pktstatus = ioread32(&rxresp->status);
+
+	if (csrrd32(priv->rx_dma_csr, msgdma_csroffs(resp_fill_level))
+	    & 0xffff) {
+		pktlength = csrrd32(priv->rx_dma_resp,
+				    msgdma_respoffs(bytes_transferred));
+		pktstatus = csrrd32(priv->rx_dma_resp,
+				    msgdma_respoffs(status));
 		rxstatus = pktstatus;
 		rxstatus = rxstatus << 16;
 		rxstatus |= (pktlength & 0xffff);
diff --git a/drivers/net/ethernet/altera/altera_msgdmahw.h b/drivers/net/ethernet/altera/altera_msgdmahw.h
index d7b59ba4019..e335626e1b6 100644
--- a/drivers/net/ethernet/altera/altera_msgdmahw.h
+++ b/drivers/net/ethernet/altera/altera_msgdmahw.h
@@ -17,15 +17,6 @@
 #ifndef __ALTERA_MSGDMAHW_H__
 #define __ALTERA_MSGDMAHW_H__
 
-/* mSGDMA standard descriptor format
- */
-struct msgdma_desc {
-	u32 read_addr;	/* data buffer source address */
-	u32 write_addr;	/* data buffer destination address */
-	u32 len;	/* the number of bytes to transfer per descriptor */
-	u32 control;	/* characteristics of the transfer */
-};
-
 /* mSGDMA extended descriptor format
  */
 struct msgdma_extended_desc {
@@ -159,6 +150,10 @@ struct msgdma_response {
 	u32 status;
 };
 
+#define msgdma_respoffs(a) (offsetof(struct msgdma_response, a))
+#define msgdma_csroffs(a) (offsetof(struct msgdma_csr, a))
+#define msgdma_descroffs(a) (offsetof(struct msgdma_extended_desc, a))
+
 /* mSGDMA response register bit definitions
  */
 #define MSGDMA_RESP_EARLY_TERM	BIT(8)
diff --git a/drivers/net/ethernet/altera/altera_sgdma.c b/drivers/net/ethernet/altera/altera_sgdma.c
index 9ce8630692b..99cc56f451c 100644
--- a/drivers/net/ethernet/altera/altera_sgdma.c
+++ b/drivers/net/ethernet/altera/altera_sgdma.c
@@ -20,8 +20,8 @@
 #include "altera_sgdmahw.h"
 #include "altera_sgdma.h"
 
-static void sgdma_setup_descrip(struct sgdma_descrip *desc,
-				struct sgdma_descrip *ndesc,
+static void sgdma_setup_descrip(struct sgdma_descrip __iomem *desc,
+				struct sgdma_descrip __iomem *ndesc,
 				dma_addr_t ndesc_phys,
 				dma_addr_t raddr,
 				dma_addr_t waddr,
@@ -31,17 +31,17 @@ static void sgdma_setup_descrip(struct sgdma_descrip *desc,
 				int wfixed);
 
 static int sgdma_async_write(struct altera_tse_private *priv,
-			      struct sgdma_descrip *desc);
+			      struct sgdma_descrip __iomem *desc);
 
 static int sgdma_async_read(struct altera_tse_private *priv);
 
 static dma_addr_t
 sgdma_txphysaddr(struct altera_tse_private *priv,
-		 struct sgdma_descrip *desc);
+		 struct sgdma_descrip __iomem *desc);
 
 static dma_addr_t
 sgdma_rxphysaddr(struct altera_tse_private *priv,
-		 struct sgdma_descrip *desc);
+		 struct sgdma_descrip __iomem *desc);
 
 static int sgdma_txbusy(struct altera_tse_private *priv);
 
@@ -79,7 +79,8 @@ int sgdma_initialize(struct altera_tse_private *priv)
 	priv->rxdescphys = (dma_addr_t) 0;
 	priv->txdescphys = (dma_addr_t) 0;
 
-	priv->rxdescphys = dma_map_single(priv->device, priv->rx_dma_desc,
+	priv->rxdescphys = dma_map_single(priv->device,
+					  (void __force *)priv->rx_dma_desc,
 					  priv->rxdescmem, DMA_BIDIRECTIONAL);
 
 	if (dma_mapping_error(priv->device, priv->rxdescphys)) {
@@ -88,7 +89,8 @@ int sgdma_initialize(struct altera_tse_private *priv)
 		return -EINVAL;
 	}
 
-	priv->txdescphys = dma_map_single(priv->device, priv->tx_dma_desc,
+	priv->txdescphys = dma_map_single(priv->device,
+					  (void __force *)priv->tx_dma_desc,
 					  priv->txdescmem, DMA_TO_DEVICE);
 
 	if (dma_mapping_error(priv->device, priv->txdescphys)) {
@@ -98,8 +100,8 @@ int sgdma_initialize(struct altera_tse_private *priv)
 	}
 
 	/* Initialize descriptor memory to all 0's, sync memory to cache */
-	memset(priv->tx_dma_desc, 0, priv->txdescmem);
-	memset(priv->rx_dma_desc, 0, priv->rxdescmem);
+	memset_io(priv->tx_dma_desc, 0, priv->txdescmem);
+	memset_io(priv->rx_dma_desc, 0, priv->rxdescmem);
 
 	dma_sync_single_for_device(priv->device, priv->txdescphys,
 				   priv->txdescmem, DMA_TO_DEVICE);
@@ -126,22 +128,15 @@ void sgdma_uninitialize(struct altera_tse_private *priv)
  */
 void sgdma_reset(struct altera_tse_private *priv)
 {
-	u32 *ptxdescripmem = (u32 *)priv->tx_dma_desc;
-	u32 txdescriplen   = priv->txdescmem;
-	u32 *prxdescripmem = (u32 *)priv->rx_dma_desc;
-	u32 rxdescriplen   = priv->rxdescmem;
-	struct sgdma_csr *ptxsgdma = (struct sgdma_csr *)priv->tx_dma_csr;
-	struct sgdma_csr *prxsgdma = (struct sgdma_csr *)priv->rx_dma_csr;
-
 	/* Initialize descriptor memory to 0 */
-	memset(ptxdescripmem, 0, txdescriplen);
-	memset(prxdescripmem, 0, rxdescriplen);
+	memset_io(priv->tx_dma_desc, 0, priv->txdescmem);
+	memset_io(priv->rx_dma_desc, 0, priv->rxdescmem);
 
-	iowrite32(SGDMA_CTRLREG_RESET, &ptxsgdma->control);
-	iowrite32(0, &ptxsgdma->control);
+	csrwr32(SGDMA_CTRLREG_RESET, priv->tx_dma_csr, sgdma_csroffs(control));
+	csrwr32(0, priv->tx_dma_csr, sgdma_csroffs(control));
 
-	iowrite32(SGDMA_CTRLREG_RESET, &prxsgdma->control);
-	iowrite32(0, &prxsgdma->control);
+	csrwr32(SGDMA_CTRLREG_RESET, priv->rx_dma_csr, sgdma_csroffs(control));
+	csrwr32(0, priv->rx_dma_csr, sgdma_csroffs(control));
 }
 
 /* For SGDMA, interrupts remain enabled after initially enabling,
@@ -167,14 +162,14 @@ void sgdma_disable_txirq(struct altera_tse_private *priv)
 
 void sgdma_clear_rxirq(struct altera_tse_private *priv)
 {
-	struct sgdma_csr *csr = (struct sgdma_csr *)priv->rx_dma_csr;
-	tse_set_bit(&csr->control, SGDMA_CTRLREG_CLRINT);
+	tse_set_bit(priv->rx_dma_csr, sgdma_csroffs(control),
+		    SGDMA_CTRLREG_CLRINT);
 }
 
 void sgdma_clear_txirq(struct altera_tse_private *priv)
 {
-	struct sgdma_csr *csr = (struct sgdma_csr *)priv->tx_dma_csr;
-	tse_set_bit(&csr->control, SGDMA_CTRLREG_CLRINT);
+	tse_set_bit(priv->tx_dma_csr, sgdma_csroffs(control),
+		    SGDMA_CTRLREG_CLRINT);
 }
 
 /* transmits buffer through SGDMA. Returns number of buffers
@@ -184,12 +179,11 @@ void sgdma_clear_txirq(struct altera_tse_private *priv)
  */
 int sgdma_tx_buffer(struct altera_tse_private *priv, struct tse_buffer *buffer)
 {
-	int pktstx = 0;
-	struct sgdma_descrip *descbase =
-		(struct sgdma_descrip *)priv->tx_dma_desc;
+	struct sgdma_descrip __iomem *descbase =
+		(struct sgdma_descrip __iomem *)priv->tx_dma_desc;
 
-	struct sgdma_descrip *cdesc = &descbase[0];
-	struct sgdma_descrip *ndesc = &descbase[1];
+	struct sgdma_descrip __iomem *cdesc = &descbase[0];
+	struct sgdma_descrip __iomem *ndesc = &descbase[1];
 
 	/* wait 'til the tx sgdma is ready for the next transmit request */
 	if (sgdma_txbusy(priv))
@@ -205,7 +199,7 @@ int sgdma_tx_buffer(struct altera_tse_private *priv, struct tse_buffer *buffer)
 			    0,				/* read fixed */
 			    SGDMA_CONTROL_WR_FIXED);	/* Generate SOP */
 
-	pktstx = sgdma_async_write(priv, cdesc);
+	sgdma_async_write(priv, cdesc);
 
 	/* enqueue the request to the pending transmit queue */
 	queue_tx(priv, buffer);
@@ -219,10 +213,10 @@ int sgdma_tx_buffer(struct altera_tse_private *priv, struct tse_buffer *buffer)
 u32 sgdma_tx_completions(struct altera_tse_private *priv)
 {
 	u32 ready = 0;
-	struct sgdma_descrip *desc = (struct sgdma_descrip *)priv->tx_dma_desc;
 
 	if (!sgdma_txbusy(priv) &&
-	    ((desc->control & SGDMA_CONTROL_HW_OWNED) == 0) &&
+	    ((csrrd8(priv->tx_dma_desc, sgdma_descroffs(control))
+	     & SGDMA_CONTROL_HW_OWNED) == 0) &&
 	    (dequeue_tx(priv))) {
 		ready = 1;
 	}
@@ -246,32 +240,31 @@ void sgdma_add_rx_desc(struct altera_tse_private *priv,
  */
 u32 sgdma_rx_status(struct altera_tse_private *priv)
 {
-	struct sgdma_csr *csr = (struct sgdma_csr *)priv->rx_dma_csr;
-	struct sgdma_descrip *base = (struct sgdma_descrip *)priv->rx_dma_desc;
-	struct sgdma_descrip *desc = NULL;
-	int pktsrx;
-	unsigned int rxstatus = 0;
-	unsigned int pktlength = 0;
-	unsigned int pktstatus = 0;
+	struct sgdma_descrip __iomem *base =
+		(struct sgdma_descrip __iomem *)priv->rx_dma_desc;
+	struct sgdma_descrip __iomem *desc = NULL;
 	struct tse_buffer *rxbuffer = NULL;
+	unsigned int rxstatus = 0;
 
-	u32 sts = ioread32(&csr->status);
+	u32 sts = csrrd32(priv->rx_dma_csr, sgdma_csroffs(status));
 
 	desc = &base[0];
 	if (sts & SGDMA_STSREG_EOP) {
+		unsigned int pktlength = 0;
+		unsigned int pktstatus = 0;
 		dma_sync_single_for_cpu(priv->device,
 					priv->rxdescphys,
 					priv->sgdmadesclen,
 					DMA_FROM_DEVICE);
 
-		pktlength = desc->bytes_xferred;
-		pktstatus = desc->status & 0x3f;
-		rxstatus = pktstatus;
+		pktlength = csrrd16(desc, sgdma_descroffs(bytes_xferred));
+		pktstatus = csrrd8(desc, sgdma_descroffs(status));
+		rxstatus = pktstatus & ~SGDMA_STATUS_EOP;
 		rxstatus = rxstatus << 16;
 		rxstatus |= (pktlength & 0xffff);
 
 		if (rxstatus) {
-			desc->status = 0;
+			csrwr8(0, desc, sgdma_descroffs(status));
 
 			rxbuffer = dequeue_rx(priv);
 			if (rxbuffer == NULL)
@@ -279,12 +272,12 @@ u32 sgdma_rx_status(struct altera_tse_private *priv)
 					    "sgdma rx and rx queue empty!\n");
 
 			/* Clear control */
-			iowrite32(0, &csr->control);
+			csrwr32(0, priv->rx_dma_csr, sgdma_csroffs(control));
 			/* clear status */
-			iowrite32(0xf, &csr->status);
+			csrwr32(0xf, priv->rx_dma_csr, sgdma_csroffs(status));
 
 			/* kick the rx sgdma after reaping this descriptor */
-			pktsrx = sgdma_async_read(priv);
+			sgdma_async_read(priv);
 
 		} else {
 			/* If the SGDMA indicated an end of packet on recv,
@@ -298,10 +291,11 @@ u32 sgdma_rx_status(struct altera_tse_private *priv)
 			 */
 			netdev_err(priv->dev,
 				   "SGDMA RX Error Info: %x, %x, %x\n",
-				   sts, desc->status, rxstatus);
+				   sts, csrrd8(desc, sgdma_descroffs(status)),
+				   rxstatus);
 		}
 	} else if (sts == 0) {
-		pktsrx = sgdma_async_read(priv);
+		sgdma_async_read(priv);
 	}
 
 	return rxstatus;
@@ -309,8 +303,8 @@ u32 sgdma_rx_status(struct altera_tse_private *priv)
 
 
 /* Private functions */
-static void sgdma_setup_descrip(struct sgdma_descrip *desc,
-				struct sgdma_descrip *ndesc,
+static void sgdma_setup_descrip(struct sgdma_descrip __iomem *desc,
+				struct sgdma_descrip __iomem *ndesc,
 				dma_addr_t ndesc_phys,
 				dma_addr_t raddr,
 				dma_addr_t waddr,
@@ -320,27 +314,30 @@ static void sgdma_setup_descrip(struct sgdma_descrip *desc,
 				int wfixed)
 {
 	/* Clear the next descriptor as not owned by hardware */
-	u32 ctrl = ndesc->control;
+
+	u32 ctrl = csrrd8(ndesc, sgdma_descroffs(control));
 	ctrl &= ~SGDMA_CONTROL_HW_OWNED;
-	ndesc->control = ctrl;
+	csrwr8(ctrl, ndesc, sgdma_descroffs(control));
 
-	ctrl = 0;
 	ctrl = SGDMA_CONTROL_HW_OWNED;
 	ctrl |= generate_eop;
 	ctrl |= rfixed;
 	ctrl |= wfixed;
 
 	/* Channel is implicitly zero, initialized to 0 by default */
-
-	desc->raddr = raddr;
-	desc->waddr = waddr;
-	desc->next = lower_32_bits(ndesc_phys);
-	desc->control = ctrl;
-	desc->status = 0;
-	desc->rburst = 0;
-	desc->wburst = 0;
-	desc->bytes = length;
-	desc->bytes_xferred = 0;
+	csrwr32(lower_32_bits(raddr), desc, sgdma_descroffs(raddr));
+	csrwr32(lower_32_bits(waddr), desc, sgdma_descroffs(waddr));
+
+	csrwr32(0, desc, sgdma_descroffs(pad1));
+	csrwr32(0, desc, sgdma_descroffs(pad2));
+	csrwr32(lower_32_bits(ndesc_phys), desc, sgdma_descroffs(next));
+
+	csrwr8(ctrl, desc, sgdma_descroffs(control));
+	csrwr8(0, desc, sgdma_descroffs(status));
+	csrwr8(0, desc, sgdma_descroffs(wburst));
+	csrwr8(0, desc, sgdma_descroffs(rburst));
+	csrwr16(length, desc, sgdma_descroffs(bytes));
+	csrwr16(0, desc, sgdma_descroffs(bytes_xferred));
 }
 
 /* If hardware is busy, don't restart async read.
@@ -351,12 +348,11 @@ static void sgdma_setup_descrip(struct sgdma_descrip *desc,
  */
 static int sgdma_async_read(struct altera_tse_private *priv)
 {
-	struct sgdma_csr *csr = (struct sgdma_csr *)priv->rx_dma_csr;
-	struct sgdma_descrip *descbase =
-		(struct sgdma_descrip *)priv->rx_dma_desc;
+	struct sgdma_descrip __iomem *descbase =
+		(struct sgdma_descrip __iomem *)priv->rx_dma_desc;
 
-	struct sgdma_descrip *cdesc = &descbase[0];
-	struct sgdma_descrip *ndesc = &descbase[1];
+	struct sgdma_descrip __iomem *cdesc = &descbase[0];
+	struct sgdma_descrip __iomem *ndesc = &descbase[1];
 
 	struct tse_buffer *rxbuffer = NULL;
 
@@ -382,11 +378,13 @@ static int sgdma_async_read(struct altera_tse_private *priv)
 					   priv->sgdmadesclen,
 					   DMA_TO_DEVICE);
 
-		iowrite32(lower_32_bits(sgdma_rxphysaddr(priv, cdesc)),
-			  &csr->next_descrip);
+		csrwr32(lower_32_bits(sgdma_rxphysaddr(priv, cdesc)),
+			priv->rx_dma_csr,
+			sgdma_csroffs(next_descrip));
 
-		iowrite32((priv->rxctrlreg | SGDMA_CTRLREG_START),
-			  &csr->control);
+		csrwr32((priv->rxctrlreg | SGDMA_CTRLREG_START),
+			priv->rx_dma_csr,
+			sgdma_csroffs(control));
 
 		return 1;
 	}
@@ -395,32 +393,32 @@ static int sgdma_async_read(struct altera_tse_private *priv)
 }
 
 static int sgdma_async_write(struct altera_tse_private *priv,
-			     struct sgdma_descrip *desc)
+			     struct sgdma_descrip __iomem *desc)
 {
-	struct sgdma_csr *csr = (struct sgdma_csr *)priv->tx_dma_csr;
-
 	if (sgdma_txbusy(priv))
 		return 0;
 
 	/* clear control and status */
-	iowrite32(0, &csr->control);
-	iowrite32(0x1f, &csr->status);
+	csrwr32(0, priv->tx_dma_csr, sgdma_csroffs(control));
+	csrwr32(0x1f, priv->tx_dma_csr, sgdma_csroffs(status));
 
 	dma_sync_single_for_device(priv->device, priv->txdescphys,
 				   priv->sgdmadesclen, DMA_TO_DEVICE);
 
-	iowrite32(lower_32_bits(sgdma_txphysaddr(priv, desc)),
-		  &csr->next_descrip);
+	csrwr32(lower_32_bits(sgdma_txphysaddr(priv, desc)),
+		priv->tx_dma_csr,
+		sgdma_csroffs(next_descrip));
 
-	iowrite32((priv->txctrlreg | SGDMA_CTRLREG_START),
-		  &csr->control);
+	csrwr32((priv->txctrlreg | SGDMA_CTRLREG_START),
+		priv->tx_dma_csr,
+		sgdma_csroffs(control));
 
 	return 1;
 }
 
 static dma_addr_t
 sgdma_txphysaddr(struct altera_tse_private *priv,
-		 struct sgdma_descrip *desc)
+		 struct sgdma_descrip __iomem *desc)
 {
 	dma_addr_t paddr = priv->txdescmem_busaddr;
 	uintptr_t offs = (uintptr_t)desc - (uintptr_t)priv->tx_dma_desc;
@@ -429,7 +427,7 @@ sgdma_txphysaddr(struct altera_tse_private *priv,
 
 static dma_addr_t
 sgdma_rxphysaddr(struct altera_tse_private *priv,
-		 struct sgdma_descrip *desc)
+		 struct sgdma_descrip __iomem *desc)
 {
 	dma_addr_t paddr = priv->rxdescmem_busaddr;
 	uintptr_t offs = (uintptr_t)desc - (uintptr_t)priv->rx_dma_desc;
@@ -518,8 +516,8 @@ queue_rx_peekhead(struct altera_tse_private *priv)
  */
 static int sgdma_rxbusy(struct altera_tse_private *priv)
 {
-	struct sgdma_csr *csr = (struct sgdma_csr *)priv->rx_dma_csr;
-	return ioread32(&csr->status) & SGDMA_STSREG_BUSY;
+	return csrrd32(priv->rx_dma_csr, sgdma_csroffs(status))
+		       & SGDMA_STSREG_BUSY;
 }
 
 /* waits for the tx sgdma to finish it's current operation, returns 0
@@ -528,13 +526,14 @@ static int sgdma_rxbusy(struct altera_tse_private *priv)
 static int sgdma_txbusy(struct altera_tse_private *priv)
 {
 	int delay = 0;
-	struct sgdma_csr *csr = (struct sgdma_csr *)priv->tx_dma_csr;
 
 	/* if DMA is busy, wait for current transactino to finish */
-	while ((ioread32(&csr->status) & SGDMA_STSREG_BUSY) && (delay++ < 100))
+	while ((csrrd32(priv->tx_dma_csr, sgdma_csroffs(status))
+		& SGDMA_STSREG_BUSY) && (delay++ < 100))
 		udelay(1);
 
-	if (ioread32(&csr->status) & SGDMA_STSREG_BUSY) {
+	if (csrrd32(priv->tx_dma_csr, sgdma_csroffs(status))
+	    & SGDMA_STSREG_BUSY) {
 		netdev_err(priv->dev, "timeout waiting for tx dma\n");
 		return 1;
 	}
diff --git a/drivers/net/ethernet/altera/altera_sgdmahw.h b/drivers/net/ethernet/altera/altera_sgdmahw.h
index ba3334f3538..85bc33b218d 100644
--- a/drivers/net/ethernet/altera/altera_sgdmahw.h
+++ b/drivers/net/ethernet/altera/altera_sgdmahw.h
@@ -19,16 +19,16 @@
 
 /* SGDMA descriptor structure */
 struct sgdma_descrip {
-	unsigned int	raddr; /* address of data to be read */
-	unsigned int	pad1;
-	unsigned int	waddr;
-	unsigned int    pad2;
-	unsigned int	next;
-	unsigned int	pad3;
-	unsigned short  bytes;
-	unsigned char   rburst;
-	unsigned char	wburst;
-	unsigned short	bytes_xferred;	/* 16 bits, bytes xferred */
+	u32	raddr; /* address of data to be read */
+	u32	pad1;
+	u32	waddr;
+	u32	pad2;
+	u32	next;
+	u32	pad3;
+	u16	bytes;
+	u8	rburst;
+	u8	wburst;
+	u16	bytes_xferred;	/* 16 bits, bytes xferred */
 
 	/* bit 0: error
 	 * bit 1: length error
@@ -39,7 +39,7 @@ struct sgdma_descrip {
 	 * bit 6: reserved
 	 * bit 7: status eop for recv case
 	 */
-	unsigned char	status;
+	u8	status;
 
 	/* bit 0: eop
 	 * bit 1: read_fixed
@@ -47,7 +47,7 @@ struct sgdma_descrip {
 	 * bits 3,4,5,6: Channel (always 0)
 	 * bit 7: hardware owned
 	 */
-	unsigned char	control;
+	u8	control;
 } __packed;
 
 
@@ -101,6 +101,8 @@ struct sgdma_csr {
 	u32	pad3[3];
 };
 
+#define sgdma_csroffs(a) (offsetof(struct sgdma_csr, a))
+#define sgdma_descroffs(a) (offsetof(struct sgdma_descrip, a))
 
 #define SGDMA_STSREG_ERR	BIT(0) /* Error */
 #define SGDMA_STSREG_EOP	BIT(1) /* EOP */
diff --git a/drivers/net/ethernet/altera/altera_tse.h b/drivers/net/ethernet/altera/altera_tse.h
index 465c4aabebb..2adb24d4523 100644
--- a/drivers/net/ethernet/altera/altera_tse.h
+++ b/drivers/net/ethernet/altera/altera_tse.h
@@ -357,6 +357,8 @@ struct altera_tse_mac {
 	u32 reserved5[42];
 };
 
+#define tse_csroffs(a) (offsetof(struct altera_tse_mac, a))
+
 /* Transmit and Receive Command Registers Bit Definitions
  */
 #define ALTERA_TSE_TX_CMD_STAT_OMIT_CRC		BIT(17)
@@ -487,4 +489,49 @@ struct altera_tse_private {
  */
 void altera_tse_set_ethtool_ops(struct net_device *);
 
+static inline
+u32 csrrd32(void __iomem *mac, size_t offs)
+{
+	void __iomem *paddr = (void __iomem *)((uintptr_t)mac + offs);
+	return readl(paddr);
+}
+
+static inline
+u16 csrrd16(void __iomem *mac, size_t offs)
+{
+	void __iomem *paddr = (void __iomem *)((uintptr_t)mac + offs);
+	return readw(paddr);
+}
+
+static inline
+u8 csrrd8(void __iomem *mac, size_t offs)
+{
+	void __iomem *paddr = (void __iomem *)((uintptr_t)mac + offs);
+	return readb(paddr);
+}
+
+static inline
+void csrwr32(u32 val, void __iomem *mac, size_t offs)
+{
+	void __iomem *paddr = (void __iomem *)((uintptr_t)mac + offs);
+
+	writel(val, paddr);
+}
+
+static inline
+void csrwr16(u16 val, void __iomem *mac, size_t offs)
+{
+	void __iomem *paddr = (void __iomem *)((uintptr_t)mac + offs);
+
+	writew(val, paddr);
+}
+
+static inline
+void csrwr8(u8 val, void __iomem *mac, size_t offs)
+{
+	void __iomem *paddr = (void __iomem *)((uintptr_t)mac + offs);
+
+	writeb(val, paddr);
+}
+
 #endif /* __ALTERA_TSE_H__ */
diff --git a/drivers/net/ethernet/altera/altera_tse_ethtool.c b/drivers/net/ethernet/altera/altera_tse_ethtool.c
index 76133caffa7..54c25eff795 100644
--- a/drivers/net/ethernet/altera/altera_tse_ethtool.c
+++ b/drivers/net/ethernet/altera/altera_tse_ethtool.c
@@ -96,54 +96,89 @@ static void tse_fill_stats(struct net_device *dev, struct ethtool_stats *dummy,
 			   u64 *buf)
 {
 	struct altera_tse_private *priv = netdev_priv(dev);
-	struct altera_tse_mac *mac = priv->mac_dev;
 	u64 ext;
 
-	buf[0] = ioread32(&mac->frames_transmitted_ok);
-	buf[1] = ioread32(&mac->frames_received_ok);
-	buf[2] = ioread32(&mac->frames_check_sequence_errors);
-	buf[3] = ioread32(&mac->alignment_errors);
+	buf[0] = csrrd32(priv->mac_dev,
+			 tse_csroffs(frames_transmitted_ok));
+	buf[1] = csrrd32(priv->mac_dev,
+			 tse_csroffs(frames_received_ok));
+	buf[2] = csrrd32(priv->mac_dev,
+			 tse_csroffs(frames_check_sequence_errors));
+	buf[3] = csrrd32(priv->mac_dev,
+			 tse_csroffs(alignment_errors));
 
 	/* Extended aOctetsTransmittedOK counter */
-	ext = (u64) ioread32(&mac->msb_octets_transmitted_ok) << 32;
-	ext |= ioread32(&mac->octets_transmitted_ok);
+	ext = (u64) csrrd32(priv->mac_dev,
+			    tse_csroffs(msb_octets_transmitted_ok)) << 32;
+
+	ext |= csrrd32(priv->mac_dev,
+		       tse_csroffs(octets_transmitted_ok));
 	buf[4] = ext;
 
 	/* Extended aOctetsReceivedOK counter */
-	ext = (u64) ioread32(&mac->msb_octets_received_ok) << 32;
-	ext |= ioread32(&mac->octets_received_ok);
+	ext = (u64) csrrd32(priv->mac_dev,
+			    tse_csroffs(msb_octets_received_ok)) << 32;
+
+	ext |= csrrd32(priv->mac_dev,
+		       tse_csroffs(octets_received_ok));
 	buf[5] = ext;
 
-	buf[6] = ioread32(&mac->tx_pause_mac_ctrl_frames);
-	buf[7] = ioread32(&mac->rx_pause_mac_ctrl_frames);
-	buf[8] = ioread32(&mac->if_in_errors);
-	buf[9] = ioread32(&mac->if_out_errors);
-	buf[10] = ioread32(&mac->if_in_ucast_pkts);
-	buf[11] = ioread32(&mac->if_in_multicast_pkts);
-	buf[12] = ioread32(&mac->if_in_broadcast_pkts);
-	buf[13] = ioread32(&mac->if_out_discards);
-	buf[14] = ioread32(&mac->if_out_ucast_pkts);
-	buf[15] = ioread32(&mac->if_out_multicast_pkts);
-	buf[16] = ioread32(&mac->if_out_broadcast_pkts);
-	buf[17] = ioread32(&mac->ether_stats_drop_events);
+	buf[6] = csrrd32(priv->mac_dev,
+			 tse_csroffs(tx_pause_mac_ctrl_frames));
+	buf[7] = csrrd32(priv->mac_dev,
+			 tse_csroffs(rx_pause_mac_ctrl_frames));
+	buf[8] = csrrd32(priv->mac_dev,
+			 tse_csroffs(if_in_errors));
+	buf[9] = csrrd32(priv->mac_dev,
+			 tse_csroffs(if_out_errors));
+	buf[10] = csrrd32(priv->mac_dev,
+			  tse_csroffs(if_in_ucast_pkts));
+	buf[11] = csrrd32(priv->mac_dev,
+			  tse_csroffs(if_in_multicast_pkts));
+	buf[12] = csrrd32(priv->mac_dev,
+			  tse_csroffs(if_in_broadcast_pkts));
+	buf[13] = csrrd32(priv->mac_dev,
+			  tse_csroffs(if_out_discards));
+	buf[14] = csrrd32(priv->mac_dev,
+			  tse_csroffs(if_out_ucast_pkts));
+	buf[15] = csrrd32(priv->mac_dev,
+			  tse_csroffs(if_out_multicast_pkts));
+	buf[16] = csrrd32(priv->mac_dev,
+			  tse_csroffs(if_out_broadcast_pkts));
+	buf[17] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_drop_events));
 
 	/* Extended etherStatsOctets counter */
-	ext = (u64) ioread32(&mac->msb_ether_stats_octets) << 32;
-	ext |= ioread32(&mac->ether_stats_octets);
+	ext = (u64) csrrd32(priv->mac_dev,
+			    tse_csroffs(msb_ether_stats_octets)) << 32;
+	ext |= csrrd32(priv->mac_dev,
+		       tse_csroffs(ether_stats_octets));
 	buf[18] = ext;
 
-	buf[19] = ioread32(&mac->ether_stats_pkts);
-	buf[20] = ioread32(&mac->ether_stats_undersize_pkts);
-	buf[21] = ioread32(&mac->ether_stats_oversize_pkts);
-	buf[22] = ioread32(&mac->ether_stats_pkts_64_octets);
-	buf[23] = ioread32(&mac->ether_stats_pkts_65to127_octets);
-	buf[24] = ioread32(&mac->ether_stats_pkts_128to255_octets);
-	buf[25] = ioread32(&mac->ether_stats_pkts_256to511_octets);
-	buf[26] = ioread32(&mac->ether_stats_pkts_512to1023_octets);
-	buf[27] = ioread32(&mac->ether_stats_pkts_1024to1518_octets);
-	buf[28] = ioread32(&mac->ether_stats_pkts_1519tox_octets);
-	buf[29] = ioread32(&mac->ether_stats_jabbers);
-	buf[30] = ioread32(&mac->ether_stats_fragments);
+	buf[19] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_pkts));
+	buf[20] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_undersize_pkts));
+	buf[21] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_oversize_pkts));
+	buf[22] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_pkts_64_octets));
+	buf[23] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_pkts_65to127_octets));
+	buf[24] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_pkts_128to255_octets));
+	buf[25] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_pkts_256to511_octets));
+	buf[26] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_pkts_512to1023_octets));
+	buf[27] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_pkts_1024to1518_octets));
+	buf[28] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_pkts_1519tox_octets));
+	buf[29] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_jabbers));
+	buf[30] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_fragments));
 }
 
 static int tse_sset_count(struct net_device *dev, int sset)
@@ -178,7 +213,6 @@ static void tse_get_regs(struct net_device *dev, struct ethtool_regs *regs,
 {
 	int i;
 	struct altera_tse_private *priv = netdev_priv(dev);
-	u32 *tse_mac_regs = (u32 *)priv->mac_dev;
 	u32 *buf = regbuf;
 
 	/* Set version to a known value, so ethtool knows
@@ -196,7 +230,7 @@ static void tse_get_regs(struct net_device *dev, struct ethtool_regs *regs,
 	regs->version = 1;
 
 	for (i = 0; i < TSE_NUM_REGS; i++)
-		buf[i] = ioread32(&tse_mac_regs[i]);
+		buf[i] = csrrd32(priv->mac_dev, i * 4);
 }
 
 static int tse_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
diff --git a/drivers/net/ethernet/altera/altera_tse_main.c b/drivers/net/ethernet/altera/altera_tse_main.c
index e44a4aeb970..7330681574d 100644
--- a/drivers/net/ethernet/altera/altera_tse_main.c
+++ b/drivers/net/ethernet/altera/altera_tse_main.c
@@ -100,29 +100,30 @@ static inline u32 tse_tx_avail(struct altera_tse_private *priv)
  */
 static int altera_tse_mdio_read(struct mii_bus *bus, int mii_id, int regnum)
 {
-	struct altera_tse_mac *mac = (struct altera_tse_mac *)bus->priv;
-	unsigned int *mdio_regs = (unsigned int *)&mac->mdio_phy0;
-	u32 data;
+	struct net_device *ndev = bus->priv;
+	struct altera_tse_private *priv = netdev_priv(ndev);
 
 	/* set MDIO address */
-	iowrite32((mii_id & 0x1f), &mac->mdio_phy0_addr);
+	csrwr32((mii_id & 0x1f), priv->mac_dev,
+		tse_csroffs(mdio_phy0_addr));
 
 	/* get the data */
-	data = ioread32(&mdio_regs[regnum]) & 0xffff;
-	return data;
+	return csrrd32(priv->mac_dev,
+		       tse_csroffs(mdio_phy0) + regnum * 4) & 0xffff;
 }
 
 static int altera_tse_mdio_write(struct mii_bus *bus, int mii_id, int regnum,
 				 u16 value)
 {
-	struct altera_tse_mac *mac = (struct altera_tse_mac *)bus->priv;
-	unsigned int *mdio_regs = (unsigned int *)&mac->mdio_phy0;
+	struct net_device *ndev = bus->priv;
+	struct altera_tse_private *priv = netdev_priv(ndev);
 
 	/* set MDIO address */
-	iowrite32((mii_id & 0x1f), &mac->mdio_phy0_addr);
+	csrwr32((mii_id & 0x1f), priv->mac_dev,
+		tse_csroffs(mdio_phy0_addr));
 
 	/* write the data */
-	iowrite32((u32) value, &mdio_regs[regnum]);
+	csrwr32(value, priv->mac_dev, tse_csroffs(mdio_phy0) + regnum * 4);
 	return 0;
 }
 
@@ -168,7 +169,7 @@ static int altera_tse_mdio_create(struct net_device *dev, unsigned int id)
 	for (i = 0; i < PHY_MAX_ADDR; i++)
 		mdio->irq[i] = PHY_POLL;
 
-	mdio->priv = priv->mac_dev;
+	mdio->priv = dev;
 	mdio->parent = priv->device;
 
 	ret = of_mdiobus_register(mdio, mdio_node);
@@ -563,7 +564,6 @@ static int tse_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	unsigned int nopaged_len = skb_headlen(skb);
 	enum netdev_tx ret = NETDEV_TX_OK;
 	dma_addr_t dma_addr;
-	int txcomplete = 0;
 
 	spin_lock_bh(&priv->tx_lock);
 
@@ -599,7 +599,7 @@ static int tse_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	dma_sync_single_for_device(priv->device, buffer->dma_addr,
 				   buffer->len, DMA_TO_DEVICE);
 
-	txcomplete = priv->dmaops->tx_buffer(priv, buffer);
+	priv->dmaops->tx_buffer(priv, buffer);
 
 	skb_tx_timestamp(skb);
 
@@ -698,7 +698,6 @@ static struct phy_device *connect_local_phy(struct net_device *dev)
 	struct altera_tse_private *priv = netdev_priv(dev);
 	struct phy_device *phydev = NULL;
 	char phy_id_fmt[MII_BUS_ID_SIZE + 3];
-	int ret;
 
 	if (priv->phy_addr != POLL_PHY) {
 		snprintf(phy_id_fmt, MII_BUS_ID_SIZE + 3, PHY_ID_FMT,
@@ -712,6 +711,7 @@ static struct phy_device *connect_local_phy(struct net_device *dev)
 			netdev_err(dev, "Could not attach to PHY\n");
 
 	} else {
+		int ret;
 		phydev = phy_find_first(priv->mdio);
 		if (phydev == NULL) {
 			netdev_err(dev, "No PHY found\n");
@@ -791,7 +791,6 @@ static int init_phy(struct net_device *dev)
 
 static void tse_update_mac_addr(struct altera_tse_private *priv, u8 *addr)
 {
-	struct altera_tse_mac *mac = priv->mac_dev;
 	u32 msb;
 	u32 lsb;
 
@@ -799,8 +798,8 @@ static void tse_update_mac_addr(struct altera_tse_private *priv, u8 *addr)
 	lsb = ((addr[5] << 8) | addr[4]) & 0xffff;
 
 	/* Set primary MAC address */
-	iowrite32(msb, &mac->mac_addr_0);
-	iowrite32(lsb, &mac->mac_addr_1);
+	csrwr32(msb, priv->mac_dev, tse_csroffs(mac_addr_0));
+	csrwr32(lsb, priv->mac_dev, tse_csroffs(mac_addr_1));
 }
 
 /* MAC software reset.
@@ -811,26 +810,26 @@ static void tse_update_mac_addr(struct altera_tse_private *priv, u8 *addr)
  */
 static int reset_mac(struct altera_tse_private *priv)
 {
-	void __iomem *cmd_cfg_reg = &priv->mac_dev->command_config;
 	int counter;
 	u32 dat;
 
-	dat = ioread32(cmd_cfg_reg);
+	dat = csrrd32(priv->mac_dev, tse_csroffs(command_config));
 	dat &= ~(MAC_CMDCFG_TX_ENA | MAC_CMDCFG_RX_ENA);
 	dat |= MAC_CMDCFG_SW_RESET | MAC_CMDCFG_CNT_RESET;
-	iowrite32(dat, cmd_cfg_reg);
+	csrwr32(dat, priv->mac_dev, tse_csroffs(command_config));
 
 	counter = 0;
 	while (counter++ < ALTERA_TSE_SW_RESET_WATCHDOG_CNTR) {
-		if (tse_bit_is_clear(cmd_cfg_reg, MAC_CMDCFG_SW_RESET))
+		if (tse_bit_is_clear(priv->mac_dev, tse_csroffs(command_config),
+				     MAC_CMDCFG_SW_RESET))
 			break;
 		udelay(1);
 	}
 
 	if (counter >= ALTERA_TSE_SW_RESET_WATCHDOG_CNTR) {
-		dat = ioread32(cmd_cfg_reg);
+		dat = csrrd32(priv->mac_dev, tse_csroffs(command_config));
 		dat &= ~MAC_CMDCFG_SW_RESET;
-		iowrite32(dat, cmd_cfg_reg);
+		csrwr32(dat, priv->mac_dev, tse_csroffs(command_config));
 		return -1;
 	}
 	return 0;
@@ -840,41 +839,57 @@ static int reset_mac(struct altera_tse_private *priv)
 */
 static int init_mac(struct altera_tse_private *priv)
 {
-	struct altera_tse_mac *mac = priv->mac_dev;
 	unsigned int cmd = 0;
 	u32 frm_length;
 
 	/* Setup Rx FIFO */
-	iowrite32(priv->rx_fifo_depth - ALTERA_TSE_RX_SECTION_EMPTY,
-		  &mac->rx_section_empty);
-	iowrite32(ALTERA_TSE_RX_SECTION_FULL, &mac->rx_section_full);
-	iowrite32(ALTERA_TSE_RX_ALMOST_EMPTY, &mac->rx_almost_empty);
-	iowrite32(ALTERA_TSE_RX_ALMOST_FULL, &mac->rx_almost_full);
+	csrwr32(priv->rx_fifo_depth - ALTERA_TSE_RX_SECTION_EMPTY,
+		priv->mac_dev, tse_csroffs(rx_section_empty));
+
+	csrwr32(ALTERA_TSE_RX_SECTION_FULL, priv->mac_dev,
+		tse_csroffs(rx_section_full));
+
+	csrwr32(ALTERA_TSE_RX_ALMOST_EMPTY, priv->mac_dev,
+		tse_csroffs(rx_almost_empty));
+
+	csrwr32(ALTERA_TSE_RX_ALMOST_FULL, priv->mac_dev,
+		tse_csroffs(rx_almost_full));
 
 	/* Setup Tx FIFO */
-	iowrite32(priv->tx_fifo_depth - ALTERA_TSE_TX_SECTION_EMPTY,
-		  &mac->tx_section_empty);
-	iowrite32(ALTERA_TSE_TX_SECTION_FULL, &mac->tx_section_full);
-	iowrite32(ALTERA_TSE_TX_ALMOST_EMPTY, &mac->tx_almost_empty);
-	iowrite32(ALTERA_TSE_TX_ALMOST_FULL, &mac->tx_almost_full);
+	csrwr32(priv->tx_fifo_depth - ALTERA_TSE_TX_SECTION_EMPTY,
+		priv->mac_dev, tse_csroffs(tx_section_empty));
+
+	csrwr32(ALTERA_TSE_TX_SECTION_FULL, priv->mac_dev,
+		tse_csroffs(tx_section_full));
+
+	csrwr32(ALTERA_TSE_TX_ALMOST_EMPTY, priv->mac_dev,
+		tse_csroffs(tx_almost_empty));
+
+	csrwr32(ALTERA_TSE_TX_ALMOST_FULL, priv->mac_dev,
+		tse_csroffs(tx_almost_full));
 
 	/* MAC Address Configuration */
 	tse_update_mac_addr(priv, priv->dev->dev_addr);
 
 	/* MAC Function Configuration */
 	frm_length = ETH_HLEN + priv->dev->mtu + ETH_FCS_LEN;
-	iowrite32(frm_length, &mac->frm_length);
-	iowrite32(ALTERA_TSE_TX_IPG_LENGTH, &mac->tx_ipg_length);
+	csrwr32(frm_length, priv->mac_dev, tse_csroffs(frm_length));
+
+	csrwr32(ALTERA_TSE_TX_IPG_LENGTH, priv->mac_dev,
+		tse_csroffs(tx_ipg_length));
 
 	/* Disable RX/TX shift 16 for alignment of all received frames on 16-bit
 	 * start address
 	 */
-	tse_set_bit(&mac->rx_cmd_stat, ALTERA_TSE_RX_CMD_STAT_RX_SHIFT16);
-	tse_clear_bit(&mac->tx_cmd_stat, ALTERA_TSE_TX_CMD_STAT_TX_SHIFT16 |
-					 ALTERA_TSE_TX_CMD_STAT_OMIT_CRC);
+	tse_set_bit(priv->mac_dev, tse_csroffs(rx_cmd_stat),
+		    ALTERA_TSE_RX_CMD_STAT_RX_SHIFT16);
+
+	tse_clear_bit(priv->mac_dev, tse_csroffs(tx_cmd_stat),
+		      ALTERA_TSE_TX_CMD_STAT_TX_SHIFT16 |
+		      ALTERA_TSE_TX_CMD_STAT_OMIT_CRC);
 
 	/* Set the MAC options */
-	cmd = ioread32(&mac->command_config);
+	cmd = csrrd32(priv->mac_dev, tse_csroffs(command_config));
 	cmd &= ~MAC_CMDCFG_PAD_EN;	/* No padding Removal on Receive */
 	cmd &= ~MAC_CMDCFG_CRC_FWD;	/* CRC Removal */
 	cmd |= MAC_CMDCFG_RX_ERR_DISC;	/* Automatically discard frames
@@ -889,9 +904,10 @@ static int init_mac(struct altera_tse_private *priv)
 	cmd &= ~MAC_CMDCFG_ETH_SPEED;
 	cmd &= ~MAC_CMDCFG_ENA_10;
 
-	iowrite32(cmd, &mac->command_config);
+	csrwr32(cmd, priv->mac_dev, tse_csroffs(command_config));
 
-	iowrite32(ALTERA_TSE_PAUSE_QUANTA, &mac->pause_quanta);
+	csrwr32(ALTERA_TSE_PAUSE_QUANTA, priv->mac_dev,
+		tse_csroffs(pause_quanta));
 
 	if (netif_msg_hw(priv))
 		dev_dbg(priv->device,
@@ -904,15 +920,14 @@ static int init_mac(struct altera_tse_private *priv)
  */
 static void tse_set_mac(struct altera_tse_private *priv, bool enable)
 {
-	struct altera_tse_mac *mac = priv->mac_dev;
-	u32 value = ioread32(&mac->command_config);
+	u32 value = csrrd32(priv->mac_dev, tse_csroffs(command_config));
 
 	if (enable)
 		value |= MAC_CMDCFG_TX_ENA | MAC_CMDCFG_RX_ENA;
 	else
 		value &= ~(MAC_CMDCFG_TX_ENA | MAC_CMDCFG_RX_ENA);
 
-	iowrite32(value, &mac->command_config);
+	csrwr32(value, priv->mac_dev, tse_csroffs(command_config));
 }
 
 /* Change the MTU
@@ -942,13 +957,12 @@ static int tse_change_mtu(struct net_device *dev, int new_mtu)
 static void altera_tse_set_mcfilter(struct net_device *dev)
 {
 	struct altera_tse_private *priv = netdev_priv(dev);
-	struct altera_tse_mac *mac = priv->mac_dev;
 	int i;
 	struct netdev_hw_addr *ha;
 
 	/* clear the hash filter */
 	for (i = 0; i < 64; i++)
-		iowrite32(0, &(mac->hash_table[i]));
+		csrwr32(0, priv->mac_dev, tse_csroffs(hash_table) + i * 4);
 
 	netdev_for_each_mc_addr(ha, dev) {
 		unsigned int hash = 0;
@@ -964,7 +978,7 @@ static void altera_tse_set_mcfilter(struct net_device *dev)
 
 			hash = (hash << 1) | xor_bit;
 		}
-		iowrite32(1, &(mac->hash_table[hash]));
+		csrwr32(1, priv->mac_dev, tse_csroffs(hash_table) + hash * 4);
 	}
 }
 
@@ -972,12 +986,11 @@ static void altera_tse_set_mcfilter(struct net_device *dev)
 static void altera_tse_set_mcfilterall(struct net_device *dev)
 {
 	struct altera_tse_private *priv = netdev_priv(dev);
-	struct altera_tse_mac *mac = priv->mac_dev;
 	int i;
 
 	/* set the hash filter */
 	for (i = 0; i < 64; i++)
-		iowrite32(1, &(mac->hash_table[i]));
+		csrwr32(1, priv->mac_dev, tse_csroffs(hash_table) + i * 4);
 }
 
 /* Set or clear the multicast filter for this adaptor
@@ -985,12 +998,12 @@ static void altera_tse_set_mcfilterall(struct net_device *dev)
 static void tse_set_rx_mode_hashfilter(struct net_device *dev)
 {
 	struct altera_tse_private *priv = netdev_priv(dev);
-	struct altera_tse_mac *mac = priv->mac_dev;
 
 	spin_lock(&priv->mac_cfg_lock);
 
 	if (dev->flags & IFF_PROMISC)
-		tse_set_bit(&mac->command_config, MAC_CMDCFG_PROMIS_EN);
+		tse_set_bit(priv->mac_dev, tse_csroffs(command_config),
+			    MAC_CMDCFG_PROMIS_EN);
 
 	if (dev->flags & IFF_ALLMULTI)
 		altera_tse_set_mcfilterall(dev);
@@ -1005,15 +1018,16 @@ static void tse_set_rx_mode_hashfilter(struct net_device *dev)
 static void tse_set_rx_mode(struct net_device *dev)
 {
 	struct altera_tse_private *priv = netdev_priv(dev);
-	struct altera_tse_mac *mac = priv->mac_dev;
 
 	spin_lock(&priv->mac_cfg_lock);
 
 	if ((dev->flags & IFF_PROMISC) || (dev->flags & IFF_ALLMULTI) ||
 	    !netdev_mc_empty(dev) || !netdev_uc_empty(dev))
-		tse_set_bit(&mac->command_config, MAC_CMDCFG_PROMIS_EN);
+		tse_set_bit(priv->mac_dev, tse_csroffs(command_config),
+			    MAC_CMDCFG_PROMIS_EN);
 	else
-		tse_clear_bit(&mac->command_config, MAC_CMDCFG_PROMIS_EN);
+		tse_clear_bit(priv->mac_dev, tse_csroffs(command_config),
+			      MAC_CMDCFG_PROMIS_EN);
 
 	spin_unlock(&priv->mac_cfg_lock);
 }
@@ -1362,6 +1376,11 @@ static int altera_tse_probe(struct platform_device *pdev)
 		of_property_read_bool(pdev->dev.of_node,
 				      "altr,has-hash-multicast-filter");
 
+	/* Set hash filter to not set for now until the
+	 * multicast filter receive issue is debugged
+	 */
+	priv->hash_filter = 0;
+
 	/* get supplemental address settings for this instance */
 	priv->added_unicast =
 		of_property_read_bool(pdev->dev.of_node,
@@ -1493,7 +1512,7 @@ static int altera_tse_remove(struct platform_device *pdev)
 	return 0;
 }
 
-struct altera_dmaops altera_dtype_sgdma = {
+static const struct altera_dmaops altera_dtype_sgdma = {
 	.altera_dtype = ALTERA_DTYPE_SGDMA,
 	.dmamask = 32,
 	.reset_dma = sgdma_reset,
@@ -1512,7 +1531,7 @@ struct altera_dmaops altera_dtype_sgdma = {
 	.start_rxdma = sgdma_start_rxdma,
 };
 
-struct altera_dmaops altera_dtype_msgdma = {
+static const struct altera_dmaops altera_dtype_msgdma = {
 	.altera_dtype = ALTERA_DTYPE_MSGDMA,
 	.dmamask = 64,
 	.reset_dma = msgdma_reset,
diff --git a/drivers/net/ethernet/altera/altera_utils.c b/drivers/net/ethernet/altera/altera_utils.c
index 70fa13f486b..d7eeb1713ad 100644
--- a/drivers/net/ethernet/altera/altera_utils.c
+++ b/drivers/net/ethernet/altera/altera_utils.c
@@ -17,28 +17,28 @@
 #include "altera_tse.h"
 #include "altera_utils.h"
 
-void tse_set_bit(void __iomem *ioaddr, u32 bit_mask)
+void tse_set_bit(void __iomem *ioaddr, size_t offs, u32 bit_mask)
 {
-	u32 value = ioread32(ioaddr);
+	u32 value = csrrd32(ioaddr, offs);
 	value |= bit_mask;
-	iowrite32(value, ioaddr);
+	csrwr32(value, ioaddr, offs);
 }
 
-void tse_clear_bit(void __iomem *ioaddr, u32 bit_mask)
+void tse_clear_bit(void __iomem *ioaddr, size_t offs, u32 bit_mask)
 {
-	u32 value = ioread32(ioaddr);
+	u32 value = csrrd32(ioaddr, offs);
 	value &= ~bit_mask;
-	iowrite32(value, ioaddr);
+	csrwr32(value, ioaddr, offs);
 }
 
-int tse_bit_is_set(void __iomem *ioaddr, u32 bit_mask)
+int tse_bit_is_set(void __iomem *ioaddr, size_t offs, u32 bit_mask)
 {
-	u32 value = ioread32(ioaddr);
+	u32 value = csrrd32(ioaddr, offs);
 	return (value & bit_mask) ? 1 : 0;
 }
 
-int tse_bit_is_clear(void __iomem *ioaddr, u32 bit_mask)
+int tse_bit_is_clear(void __iomem *ioaddr, size_t offs, u32 bit_mask)
 {
-	u32 value = ioread32(ioaddr);
+	u32 value = csrrd32(ioaddr, offs);
 	return (value & bit_mask) ? 0 : 1;
 }
diff --git a/drivers/net/ethernet/altera/altera_utils.h b/drivers/net/ethernet/altera/altera_utils.h
index ce1db36d358..baf100ccf58 100644
--- a/drivers/net/ethernet/altera/altera_utils.h
+++ b/drivers/net/ethernet/altera/altera_utils.h
@@ -19,9 +19,9 @@
 #ifndef __ALTERA_UTILS_H__
 #define __ALTERA_UTILS_H__
 
-void tse_set_bit(void __iomem *ioaddr, u32 bit_mask);
-void tse_clear_bit(void __iomem *ioaddr, u32 bit_mask);
-int tse_bit_is_set(void __iomem *ioaddr, u32 bit_mask);
-int tse_bit_is_clear(void __iomem *ioaddr, u32 bit_mask);
+void tse_set_bit(void __iomem *ioaddr, size_t offs, u32 bit_mask);
+void tse_clear_bit(void __iomem *ioaddr, size_t offs, u32 bit_mask);
+int tse_bit_is_set(void __iomem *ioaddr, size_t offs, u32 bit_mask);
+int tse_bit_is_clear(void __iomem *ioaddr, size_t offs, u32 bit_mask);
 
 #endif /* __ALTERA_UTILS_H__*/
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
index 9261d5313b5..dd57c7c5a3d 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
@@ -2781,7 +2781,7 @@ int bnx2x_nic_load(struct bnx2x *bp, int load_mode)
 
 	case LOAD_OPEN:
 		netif_tx_start_all_queues(bp->dev);
-		smp_mb__after_clear_bit();
+		smp_mb__after_atomic();
 		break;
 
 	case LOAD_DIAG:
@@ -4939,9 +4939,9 @@ void bnx2x_update_coalesce_sb_index(struct bnx2x *bp, u8 fw_sb_id,
 void bnx2x_schedule_sp_rtnl(struct bnx2x *bp, enum sp_rtnl_flag flag,
 			    u32 verbose)
 {
-	smp_mb__before_clear_bit();
+	smp_mb__before_atomic();
 	set_bit(flag, &bp->sp_rtnl_state);
-	smp_mb__after_clear_bit();
+	smp_mb__after_atomic();
 	DP((BNX2X_MSG_SP | verbose), "Scheduling sp_rtnl task [Flag: %d]\n",
 	   flag);
 	schedule_delayed_work(&bp->sp_rtnl_task, 0);
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
index b260913db23..3a8e51ed5be 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
@@ -1858,10 +1858,10 @@ void bnx2x_sp_event(struct bnx2x_fastpath *fp, union eth_rx_cqe *rr_cqe)
 		return;
 #endif
 
-	smp_mb__before_atomic_inc();
+	smp_mb__before_atomic();
 	atomic_inc(&bp->cq_spq_left);
 	/* push the change in bp->spq_left and towards the memory */
-	smp_mb__after_atomic_inc();
+	smp_mb__after_atomic();
 
 	DP(BNX2X_MSG_SP, "bp->cq_spq_left %x\n", atomic_read(&bp->cq_spq_left));
 
@@ -1876,11 +1876,11 @@ void bnx2x_sp_event(struct bnx2x_fastpath *fp, union eth_rx_cqe *rr_cqe)
 		 * sp_state is cleared, and this order prevents
 		 * races
 		 */
-		smp_mb__before_clear_bit();
+		smp_mb__before_atomic();
 		set_bit(BNX2X_AFEX_PENDING_VIFSET_MCP_ACK, &bp->sp_state);
 		wmb();
 		clear_bit(BNX2X_AFEX_FCOE_Q_UPDATE_PENDING, &bp->sp_state);
-		smp_mb__after_clear_bit();
+		smp_mb__after_atomic();
 
 		/* schedule the sp task as mcp ack is required */
 		bnx2x_schedule_sp_task(bp);
@@ -5272,9 +5272,9 @@ static void bnx2x_after_function_update(struct bnx2x *bp)
 		__clear_bit(RAMROD_COMP_WAIT, &queue_params.ramrod_flags);
 
 		/* mark latest Q bit */
-		smp_mb__before_clear_bit();
+		smp_mb__before_atomic();
 		set_bit(BNX2X_AFEX_FCOE_Q_UPDATE_PENDING, &bp->sp_state);
-		smp_mb__after_clear_bit();
+		smp_mb__after_atomic();
 
 		/* send Q update ramrod for FCoE Q */
 		rc = bnx2x_queue_state_change(bp, &queue_params);
@@ -5500,7 +5500,7 @@ next_spqe:
 		spqe_cnt++;
 	} /* for */
 
-	smp_mb__before_atomic_inc();
+	smp_mb__before_atomic();
 	atomic_add(spqe_cnt, &bp->eq_spq_left);
 
 	bp->eq_cons = sw_cons;
@@ -10051,8 +10051,8 @@ 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)	(0x1a150c + ((p) << 4))
-#define BNX2X_PREV_UNDI_MF_FUNC(f)	(0x1a184c + ((f) << 4))
+#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_unload_undi_fw_supports_mf(struct bnx2x *bp)
 {
 	u8 major, minor, version;
@@ -10352,6 +10352,7 @@ 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);
@@ -10398,7 +10399,10 @@ static int bnx2x_prev_unload_common(struct bnx2x *bp)
 			 * cleaning methods - might be redundant but harmless.
 			 */
 			if (bnx2x_prev_unload_undi_fw_supports_mf(bp)) {
-				bnx2x_prev_unload_undi_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
@@ -13871,9 +13875,9 @@ static int bnx2x_drv_ctl(struct net_device *dev, struct drv_ctl_info *ctl)
 	case DRV_CTL_RET_L2_SPQ_CREDIT_CMD: {
 		int count = ctl->data.credit.credit_count;
 
-		smp_mb__before_atomic_inc();
+		smp_mb__before_atomic();
 		atomic_add(count, &bp->cq_spq_left);
-		smp_mb__after_atomic_inc();
+		smp_mb__after_atomic();
 		break;
 	}
 	case DRV_CTL_ULP_REGISTER_CMD: {
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
index 31297266b74..d725317c427 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
@@ -258,16 +258,16 @@ static bool bnx2x_raw_check_pending(struct bnx2x_raw_obj *o)
 
 static void bnx2x_raw_clear_pending(struct bnx2x_raw_obj *o)
 {
-	smp_mb__before_clear_bit();
+	smp_mb__before_atomic();
 	clear_bit(o->state, o->pstate);
-	smp_mb__after_clear_bit();
+	smp_mb__after_atomic();
 }
 
 static void bnx2x_raw_set_pending(struct bnx2x_raw_obj *o)
 {
-	smp_mb__before_clear_bit();
+	smp_mb__before_atomic();
 	set_bit(o->state, o->pstate);
-	smp_mb__after_clear_bit();
+	smp_mb__after_atomic();
 }
 
 /**
@@ -2131,7 +2131,7 @@ static int bnx2x_set_rx_mode_e1x(struct bnx2x *bp,
 
 	/* The operation is completed */
 	clear_bit(p->state, p->pstate);
-	smp_mb__after_clear_bit();
+	smp_mb__after_atomic();
 
 	return 0;
 }
@@ -3576,16 +3576,16 @@ error_exit1:
 
 static void bnx2x_mcast_clear_sched(struct bnx2x_mcast_obj *o)
 {
-	smp_mb__before_clear_bit();
+	smp_mb__before_atomic();
 	clear_bit(o->sched_state, o->raw.pstate);
-	smp_mb__after_clear_bit();
+	smp_mb__after_atomic();
 }
 
 static void bnx2x_mcast_set_sched(struct bnx2x_mcast_obj *o)
 {
-	smp_mb__before_clear_bit();
+	smp_mb__before_atomic();
 	set_bit(o->sched_state, o->raw.pstate);
-	smp_mb__after_clear_bit();
+	smp_mb__after_atomic();
 }
 
 static bool bnx2x_mcast_check_sched(struct bnx2x_mcast_obj *o)
@@ -4200,7 +4200,7 @@ int bnx2x_queue_state_change(struct bnx2x *bp,
 		if (rc) {
 			o->next_state = BNX2X_Q_STATE_MAX;
 			clear_bit(pending_bit, pending);
-			smp_mb__after_clear_bit();
+			smp_mb__after_atomic();
 			return rc;
 		}
 
@@ -4288,7 +4288,7 @@ static int bnx2x_queue_comp_cmd(struct bnx2x *bp,
 	wmb();
 
 	clear_bit(cmd, &o->pending);
-	smp_mb__after_clear_bit();
+	smp_mb__after_atomic();
 
 	return 0;
 }
@@ -5279,7 +5279,7 @@ static inline int bnx2x_func_state_change_comp(struct bnx2x *bp,
 	wmb();
 
 	clear_bit(cmd, &o->pending);
-	smp_mb__after_clear_bit();
+	smp_mb__after_atomic();
 
 	return 0;
 }
@@ -5926,7 +5926,7 @@ int bnx2x_func_state_change(struct bnx2x *bp,
 		if (rc) {
 			o->next_state = BNX2X_F_STATE_MAX;
 			clear_bit(cmd, pending);
-			smp_mb__after_clear_bit();
+			smp_mb__after_atomic();
 			return rc;
 		}
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
index 81cc2d9831c..faf01488d26 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
@@ -1650,9 +1650,9 @@ static
 void bnx2x_vf_handle_filters_eqe(struct bnx2x *bp,
 				 struct bnx2x_virtf *vf)
 {
-	smp_mb__before_clear_bit();
+	smp_mb__before_atomic();
 	clear_bit(BNX2X_FILTER_RX_MODE_PENDING, &vf->filter_state);
-	smp_mb__after_clear_bit();
+	smp_mb__after_atomic();
 }
 
 static void bnx2x_vf_handle_rss_update_eqe(struct bnx2x *bp,
@@ -2695,7 +2695,7 @@ out:
 		bnx2x_unlock_vf_pf_channel(bp, vf, CHANNEL_TLV_PF_SET_MAC);
 	}
 
-	return 0;
+	return rc;
 }
 
 int bnx2x_set_vf_vlan(struct net_device *dev, int vfidx, u16 vlan, u8 qos)
@@ -2990,9 +2990,9 @@ void bnx2x_iov_task(struct work_struct *work)
 
 void bnx2x_schedule_iov_task(struct bnx2x *bp, enum bnx2x_iov_flag flag)
 {
-	smp_mb__before_clear_bit();
+	smp_mb__before_atomic();
 	set_bit(flag, &bp->iov_task_state);
-	smp_mb__after_clear_bit();
+	smp_mb__after_atomic();
 	DP(BNX2X_MSG_IOV, "Scheduling iov task [Flag: %d]\n", flag);
 	queue_delayed_work(bnx2x_iov_wq, &bp->iov_task, 0);
 }
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.c
index 0c067e8564d..784c7155b98 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.c
@@ -747,7 +747,7 @@ int bnx2x_vfpf_config_mac(struct bnx2x *bp, u8 *addr, u8 vf_qid, bool set)
 out:
 	bnx2x_vfpf_finalize(bp, &req->first_tlv);
 
-	return 0;
+	return rc;
 }
 
 /* request pf to config rss table for vf queues*/
diff --git a/drivers/net/ethernet/broadcom/cnic.c b/drivers/net/ethernet/broadcom/cnic.c
index 09f3fefcbf9..4dd48d2fa80 100644
--- a/drivers/net/ethernet/broadcom/cnic.c
+++ b/drivers/net/ethernet/broadcom/cnic.c
@@ -436,7 +436,7 @@ static int cnic_offld_prep(struct cnic_sock *csk)
 static int cnic_close_prep(struct cnic_sock *csk)
 {
 	clear_bit(SK_F_CONNECT_START, &csk->flags);
-	smp_mb__after_clear_bit();
+	smp_mb__after_atomic();
 
 	if (test_and_clear_bit(SK_F_OFFLD_COMPLETE, &csk->flags)) {
 		while (test_and_set_bit(SK_F_OFFLD_SCHED, &csk->flags))
@@ -450,7 +450,7 @@ static int cnic_close_prep(struct cnic_sock *csk)
 static int cnic_abort_prep(struct cnic_sock *csk)
 {
 	clear_bit(SK_F_CONNECT_START, &csk->flags);
-	smp_mb__after_clear_bit();
+	smp_mb__after_atomic();
 
 	while (test_and_set_bit(SK_F_OFFLD_SCHED, &csk->flags))
 		msleep(1);
@@ -3646,7 +3646,7 @@ static int cnic_cm_destroy(struct cnic_sock *csk)
 
 	csk_hold(csk);
 	clear_bit(SK_F_INUSE, &csk->flags);
-	smp_mb__after_clear_bit();
+	smp_mb__after_atomic();
 	while (atomic_read(&csk->ref_count) != 1)
 		msleep(1);
 	cnic_cm_cleanup(csk);
@@ -4026,7 +4026,7 @@ static void cnic_cm_process_kcqe(struct cnic_dev *dev, struct kcqe *kcqe)
 			 L4_KCQE_COMPLETION_STATUS_PARITY_ERROR)
 			set_bit(SK_F_HW_ERR, &csk->flags);
 
-		smp_mb__before_clear_bit();
+		smp_mb__before_atomic();
 		clear_bit(SK_F_OFFLD_SCHED, &csk->flags);
 		cnic_cm_upcall(cp, csk, opcode);
 		break;
diff --git a/drivers/net/ethernet/brocade/bna/bnad.c b/drivers/net/ethernet/brocade/bna/bnad.c
index 675550fe8ee..3a77f9ead00 100644
--- a/drivers/net/ethernet/brocade/bna/bnad.c
+++ b/drivers/net/ethernet/brocade/bna/bnad.c
@@ -249,7 +249,7 @@ bnad_tx_complete(struct bnad *bnad, struct bna_tcb *tcb)
 	if (likely(test_bit(BNAD_TXQ_TX_STARTED, &tcb->flags)))
 		bna_ib_ack(tcb->i_dbell, sent);
 
-	smp_mb__before_clear_bit();
+	smp_mb__before_atomic();
 	clear_bit(BNAD_TXQ_FREE_SENT, &tcb->flags);
 
 	return sent;
@@ -1126,7 +1126,7 @@ bnad_tx_cleanup(struct delayed_work *work)
 
 		bnad_txq_cleanup(bnad, tcb);
 
-		smp_mb__before_clear_bit();
+		smp_mb__before_atomic();
 		clear_bit(BNAD_TXQ_FREE_SENT, &tcb->flags);
 	}
 
@@ -2992,7 +2992,7 @@ bnad_start_xmit(struct sk_buff *skb, struct net_device *netdev)
 			sent = bnad_txcmpl_process(bnad, tcb);
 			if (likely(test_bit(BNAD_TXQ_TX_STARTED, &tcb->flags)))
 				bna_ib_ack(tcb->i_dbell, sent);
-			smp_mb__before_clear_bit();
+			smp_mb__before_atomic();
 			clear_bit(BNAD_TXQ_FREE_SENT, &tcb->flags);
 		} else {
 			netif_stop_queue(netdev);
diff --git a/drivers/net/ethernet/chelsio/cxgb/cxgb2.c b/drivers/net/ethernet/chelsio/cxgb/cxgb2.c
index 0fe7ff750d7..05613a85ce6 100644
--- a/drivers/net/ethernet/chelsio/cxgb/cxgb2.c
+++ b/drivers/net/ethernet/chelsio/cxgb/cxgb2.c
@@ -281,7 +281,7 @@ static int cxgb_close(struct net_device *dev)
 	if (adapter->params.stats_update_period &&
 	    !(adapter->open_device_map & PORT_MASK)) {
 		/* Stop statistics accumulation. */
-		smp_mb__after_clear_bit();
+		smp_mb__after_atomic();
 		spin_lock(&adapter->work_lock);   /* sync with update task */
 		spin_unlock(&adapter->work_lock);
 		cancel_mac_stats_update(adapter);
diff --git a/drivers/net/ethernet/chelsio/cxgb3/sge.c b/drivers/net/ethernet/chelsio/cxgb3/sge.c
index 8b069f96e92..3dfcf600fcc 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/sge.c
@@ -1379,7 +1379,7 @@ static inline int check_desc_avail(struct adapter *adap, struct sge_txq *q,
 		struct sge_qset *qs = txq_to_qset(q, qid);
 
 		set_bit(qid, &qs->txq_stopped);
-		smp_mb__after_clear_bit();
+		smp_mb__after_atomic();
 
 		if (should_restart_tx(q) &&
 		    test_and_clear_bit(qid, &qs->txq_stopped))
@@ -1492,7 +1492,7 @@ static void restart_ctrlq(unsigned long data)
 
 	if (!skb_queue_empty(&q->sendq)) {
 		set_bit(TXQ_CTRL, &qs->txq_stopped);
-		smp_mb__after_clear_bit();
+		smp_mb__after_atomic();
 
 		if (should_restart_tx(q) &&
 		    test_and_clear_bit(TXQ_CTRL, &qs->txq_stopped))
@@ -1697,7 +1697,7 @@ again:	reclaim_completed_tx(adap, q, TX_RECLAIM_CHUNK);
 
 		if (unlikely(q->size - q->in_use < ndesc)) {
 			set_bit(TXQ_OFLD, &qs->txq_stopped);
-			smp_mb__after_clear_bit();
+			smp_mb__after_atomic();
 
 			if (should_restart_tx(q) &&
 			    test_and_clear_bit(TXQ_OFLD, &qs->txq_stopped))
diff --git a/drivers/net/ethernet/chelsio/cxgb4/sge.c b/drivers/net/ethernet/chelsio/cxgb4/sge.c
index ca95cf2954e..e249528c8e6 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/sge.c
@@ -2031,7 +2031,7 @@ static void sge_rx_timer_cb(unsigned long data)
 			struct sge_fl *fl = s->egr_map[id];
 
 			clear_bit(id, s->starving_fl);
-			smp_mb__after_clear_bit();
+			smp_mb__after_atomic();
 
 			if (fl_starving(fl)) {
 				rxq = container_of(fl, struct sge_eth_rxq, fl);
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/sge.c b/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
index 9cfa4b4bb08..9d88c1d50b4 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
@@ -1951,7 +1951,7 @@ static void sge_rx_timer_cb(unsigned long data)
 			struct sge_fl *fl = s->egr_map[id];
 
 			clear_bit(id, s->starving_fl);
-			smp_mb__after_clear_bit();
+			smp_mb__after_atomic();
 
 			/*
 			 * Since we are accessing fl without a lock there's a
diff --git a/drivers/net/ethernet/ec_bhf.c b/drivers/net/ethernet/ec_bhf.c
new file mode 100644
index 00000000000..4884205e56e
--- /dev/null
+++ b/drivers/net/ethernet/ec_bhf.c
@@ -0,0 +1,706 @@
+ /*
+ * drivers/net/ethernet/beckhoff/ec_bhf.c
+ *
+ * Copyright (C) 2014 Darek Marcinkiewicz <reksio@newterm.pl>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+/* This is a driver for EtherCAT master module present on CCAT FPGA.
+ * 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>
+#include <linux/pci.h>
+#include <linux/init.h>
+
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/ip.h>
+#include <linux/skbuff.h>
+#include <linux/hrtimer.h>
+#include <linux/interrupt.h>
+#include <linux/stat.h>
+
+#define TIMER_INTERVAL_NSEC	20000
+
+#define INFO_BLOCK_SIZE		0x10
+#define INFO_BLOCK_TYPE		0x0
+#define INFO_BLOCK_REV		0x2
+#define INFO_BLOCK_BLK_CNT	0x4
+#define INFO_BLOCK_TX_CHAN	0x4
+#define INFO_BLOCK_RX_CHAN	0x5
+#define INFO_BLOCK_OFFSET	0x8
+
+#define EC_MII_OFFSET		0x4
+#define EC_FIFO_OFFSET		0x8
+#define EC_MAC_OFFSET		0xc
+
+#define MAC_FRAME_ERR_CNT	0x0
+#define MAC_RX_ERR_CNT		0x1
+#define MAC_CRC_ERR_CNT		0x2
+#define MAC_LNK_LST_ERR_CNT	0x3
+#define MAC_TX_FRAME_CNT	0x10
+#define MAC_RX_FRAME_CNT	0x14
+#define MAC_TX_FIFO_LVL		0x20
+#define MAC_DROPPED_FRMS	0x28
+#define MAC_CONNECTED_CCAT_FLAG	0x78
+
+#define MII_MAC_ADDR		0x8
+#define MII_MAC_FILT_FLAG	0xe
+#define MII_LINK_STATUS		0xf
+
+#define FIFO_TX_REG		0x0
+#define FIFO_TX_RESET		0x8
+#define FIFO_RX_REG		0x10
+#define FIFO_RX_ADDR_VALID	(1u << 31)
+#define FIFO_RX_RESET		0x18
+
+#define DMA_CHAN_OFFSET		0x1000
+#define DMA_CHAN_SIZE		0x8
+
+#define DMA_WINDOW_SIZE_MASK	0xfffffffc
+
+static struct pci_device_id ids[] = {
+	{ PCI_DEVICE(0x15ec, 0x5000), },
+	{ 0, }
+};
+MODULE_DEVICE_TABLE(pci, ids);
+
+struct rx_header {
+#define RXHDR_NEXT_ADDR_MASK	0xffffffu
+#define RXHDR_NEXT_VALID	(1u << 31)
+	__le32 next;
+#define RXHDR_NEXT_RECV_FLAG	0x1
+	__le32 recv;
+#define RXHDR_LEN_MASK		0xfffu
+	__le16 len;
+	__le16 port;
+	__le32 reserved;
+	u8 timestamp[8];
+} __packed;
+
+#define PKT_PAYLOAD_SIZE	0x7e8
+struct rx_desc {
+	struct rx_header header;
+	u8 data[PKT_PAYLOAD_SIZE];
+} __packed;
+
+struct tx_header {
+	__le16 len;
+#define TX_HDR_PORT_0		0x1
+#define TX_HDR_PORT_1		0x2
+	u8 port;
+	u8 ts_enable;
+#define TX_HDR_SENT		0x1
+	__le32 sent;
+	u8 timestamp[8];
+} __packed;
+
+struct tx_desc {
+	struct tx_header header;
+	u8 data[PKT_PAYLOAD_SIZE];
+} __packed;
+
+#define FIFO_SIZE		64
+
+static long polling_frequency = TIMER_INTERVAL_NSEC;
+
+struct bhf_dma {
+	u8 *buf;
+	size_t len;
+	dma_addr_t buf_phys;
+
+	u8 *alloc;
+	size_t alloc_len;
+	dma_addr_t alloc_phys;
+};
+
+struct ec_bhf_priv {
+	struct net_device *net_dev;
+
+	struct pci_dev *dev;
+
+	void * __iomem io;
+	void * __iomem dma_io;
+
+	struct hrtimer hrtimer;
+
+	int tx_dma_chan;
+	int rx_dma_chan;
+	void * __iomem ec_io;
+	void * __iomem fifo_io;
+	void * __iomem mii_io;
+	void * __iomem mac_io;
+
+	struct bhf_dma rx_buf;
+	struct rx_desc *rx_descs;
+	int rx_dnext;
+	int rx_dcount;
+
+	struct bhf_dma tx_buf;
+	struct tx_desc *tx_descs;
+	int tx_dcount;
+	int tx_dnext;
+
+	u64 stat_rx_bytes;
+	u64 stat_tx_bytes;
+};
+
+#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);
+	iowrite8(0, priv->mac_io + MAC_RX_ERR_CNT);
+	iowrite8(0, priv->mac_io + MAC_CRC_ERR_CNT);
+	iowrite8(0, priv->mac_io + MAC_LNK_LST_ERR_CNT);
+	iowrite32(0, priv->mac_io + MAC_TX_FRAME_CNT);
+	iowrite32(0, priv->mac_io + MAC_RX_FRAME_CNT);
+	iowrite8(0, priv->mac_io + MAC_DROPPED_FRMS);
+
+	iowrite8(0, priv->fifo_io + FIFO_TX_RESET);
+	iowrite8(0, priv->fifo_io + FIFO_RX_RESET);
+
+	iowrite8(0, priv->mac_io + MAC_TX_FIFO_LVL);
+}
+
+static void ec_bhf_send_packet(struct ec_bhf_priv *priv, struct tx_desc *desc)
+{
+	u32 len = le16_to_cpu(desc->header.len) + sizeof(desc->header);
+	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)
+{
+	return le32_to_cpu(desc->header.sent) & TX_HDR_SENT;
+}
+
+static void ec_bhf_process_tx(struct ec_bhf_priv *priv)
+{
+	if (unlikely(netif_queue_stopped(priv->net_dev))) {
+		/* Make sure that we perceive changes to tx_dnext. */
+		smp_rmb();
+
+		if (ec_bhf_desc_sent(&priv->tx_descs[priv->tx_dnext]))
+			netif_wake_queue(priv->net_dev);
+	}
+}
+
+static int ec_bhf_pkt_received(struct rx_desc *desc)
+{
+	return le32_to_cpu(desc->header.recv) & RXHDR_NEXT_RECV_FLAG;
+}
+
+static void ec_bhf_add_rx_desc(struct ec_bhf_priv *priv, struct rx_desc *desc)
+{
+	iowrite32(FIFO_RX_ADDR_VALID | ((u8 *)(desc) - priv->rx_buf.buf),
+		  priv->fifo_io + FIFO_RX_REG);
+}
+
+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) &
+			       RXHDR_LEN_MASK) - sizeof(struct rx_header) - 4;
+		u8 *data = desc->data;
+		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);
+		}
+
+		desc->header.recv = 0;
+
+		ec_bhf_add_rx_desc(priv, desc);
+
+		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)
+{
+	struct ec_bhf_priv *priv = container_of(timer, struct ec_bhf_priv,
+						hrtimer);
+	ec_bhf_process_rx(priv);
+	ec_bhf_process_tx(priv);
+
+	if (!netif_running(priv->net_dev))
+		return HRTIMER_NORESTART;
+
+	hrtimer_forward_now(timer, ktime_set(0, polling_frequency));
+	return HRTIMER_RESTART;
+}
+
+static int ec_bhf_setup_offsets(struct ec_bhf_priv *priv)
+{
+	struct device *dev = PRIV_TO_DEV(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);
+		if (type == ETHERCAT_MASTER_ID)
+			break;
+	}
+	if (i == block_count) {
+		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;
+}
+
+static netdev_tx_t ec_bhf_start_xmit(struct sk_buff *skb,
+				     struct net_device *net_dev)
+{
+	struct ec_bhf_priv *priv = netdev_priv(net_dev);
+	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);
+	len = skb->len;
+
+	memset(&desc->header, 0, sizeof(desc->header));
+	desc->header.len = cpu_to_le16(len);
+	desc->header.port = TX_HDR_PORT_0;
+
+	ec_bhf_send_packet(priv, desc);
+
+	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
+		 * 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;
+
+	dev_kfree_skb(skb);
+
+	return NETDEV_TX_OK;
+}
+
+static int ec_bhf_alloc_dma_mem(struct ec_bhf_priv *priv,
+				struct bhf_dma *buf,
+				int channel,
+				int size)
+{
+	int offset = channel * DMA_CHAN_SIZE + DMA_CHAN_OFFSET;
+	struct device *dev = PRIV_TO_DEV(priv);
+	u32 mask;
+
+	iowrite32(0xffffffff, priv->dma_io + offset);
+
+	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
+	 * - is of size 2^mask bytes (at most)
+	 * In order to ensure that we will allocate buffer of
+	 * 2 * 2^mask bytes.
+	 */
+	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");
+		return -ENOMEM;
+	}
+
+	buf->buf_phys = (buf->alloc_phys + buf->len) & mask;
+	buf->buf = buf->alloc + (buf->buf_phys - buf->alloc_phys);
+
+	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;
+}
+
+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_dnext = 0;
+
+	for (i = 0; i < priv->tx_dcount; i++)
+		priv->tx_descs[i].header.sent = cpu_to_le32(TX_HDR_SENT);
+}
+
+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_dnext = 0;
+
+	for (i = 0; i < priv->rx_dcount; i++) {
+		struct rx_desc *desc = &priv->rx_descs[i];
+		u32 next;
+
+		if (i != priv->rx_dcount - 1)
+			next = (u8 *)(desc + 1) - priv->rx_buf.buf;
+		else
+			next = 0;
+		next |= RXHDR_NEXT_VALID;
+		desc->header.next = cpu_to_le32(next);
+		desc->header.recv = 0;
+		ec_bhf_add_rx_desc(priv, desc);
+	}
+}
+
+static int ec_bhf_open(struct net_device *net_dev)
+{
+	struct ec_bhf_priv *priv = netdev_priv(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,
+				   FIFO_SIZE * sizeof(struct rx_desc));
+	if (err) {
+		dev_err(dev, "Failed to allocate rx buffer\n");
+		goto out;
+	}
+	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);
+
+	hrtimer_init(&priv->hrtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	priv->hrtimer.function = ec_bhf_timer_fun;
+	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:
+	dma_free_coherent(dev, priv->rx_buf.alloc_len, priv->rx_buf.alloc,
+			  priv->rx_buf.alloc_len);
+out:
+	return err;
+}
+
+static int ec_bhf_stop(struct net_device *net_dev)
+{
+	struct ec_bhf_priv *priv = netdev_priv(net_dev);
+	struct device *dev = PRIV_TO_DEV(priv);
+
+	hrtimer_cancel(&priv->hrtimer);
+
+	ec_bhf_reset(priv);
+
+	netif_tx_disable(net_dev);
+
+	dma_free_coherent(dev, priv->tx_buf.alloc_len,
+			  priv->tx_buf.alloc, priv->tx_buf.alloc_phys);
+	dma_free_coherent(dev, priv->rx_buf.alloc_len,
+			  priv->rx_buf.alloc, priv->rx_buf.alloc_phys);
+
+	return 0;
+}
+
+static struct rtnl_link_stats64 *
+ec_bhf_get_stats(struct net_device *net_dev,
+		 struct rtnl_link_stats64 *stats)
+{
+	struct ec_bhf_priv *priv = netdev_priv(net_dev);
+
+	stats->rx_errors = ioread8(priv->mac_io + MAC_RX_ERR_CNT) +
+				ioread8(priv->mac_io + MAC_CRC_ERR_CNT) +
+				ioread8(priv->mac_io + MAC_FRAME_ERR_CNT);
+	stats->rx_packets = ioread32(priv->mac_io + MAC_RX_FRAME_CNT);
+	stats->tx_packets = ioread32(priv->mac_io + MAC_TX_FRAME_CNT);
+	stats->rx_dropped = ioread8(priv->mac_io + MAC_DROPPED_FRMS);
+
+	stats->tx_bytes = priv->stat_tx_bytes;
+	stats->rx_bytes = priv->stat_rx_bytes;
+
+	return stats;
+}
+
+static const struct net_device_ops ec_bhf_netdev_ops = {
+	.ndo_start_xmit		= ec_bhf_start_xmit,
+	.ndo_open		= ec_bhf_open,
+	.ndo_stop		= ec_bhf_stop,
+	.ndo_get_stats64	= ec_bhf_get_stats,
+	.ndo_change_mtu		= eth_change_mtu,
+	.ndo_validate_addr	= eth_validate_addr,
+	.ndo_set_mac_address	= eth_mac_addr
+};
+
+static int ec_bhf_probe(struct pci_dev *dev, const struct pci_device_id *id)
+{
+	struct net_device *net_dev;
+	struct ec_bhf_priv *priv;
+	void * __iomem dma_io;
+	void * __iomem io;
+	int err = 0;
+
+	err = pci_enable_device(dev);
+	if (err)
+		return err;
+
+	pci_set_master(dev);
+
+	err = pci_set_dma_mask(dev, DMA_BIT_MASK(32));
+	if (err) {
+		dev_err(&dev->dev,
+			"Required dma mask not supported, failed to initialize device\n");
+		err = -EIO;
+		goto err_disable_dev;
+	}
+
+	err = pci_set_consistent_dma_mask(dev, DMA_BIT_MASK(32));
+	if (err) {
+		dev_err(&dev->dev,
+			"Required dma mask not supported, failed to initialize device\n");
+		goto err_disable_dev;
+	}
+
+	err = pci_request_regions(dev, "ec_bhf");
+	if (err) {
+		dev_err(&dev->dev, "Failed to request pci memory regions\n");
+		goto err_disable_dev;
+	}
+
+	io = pci_iomap(dev, 0, 0);
+	if (!io) {
+		dev_err(&dev->dev, "Failed to map pci card memory bar 0");
+		err = -EIO;
+		goto err_release_regions;
+	}
+
+	dma_io = pci_iomap(dev, 2, 0);
+	if (!dma_io) {
+		dev_err(&dev->dev, "Failed to map pci card memory bar 2");
+		err = -EIO;
+		goto err_unmap;
+	}
+
+	net_dev = alloc_etherdev(sizeof(struct ec_bhf_priv));
+	if (net_dev == 0) {
+		err = -ENOMEM;
+		goto err_unmap_dma_io;
+	}
+
+	pci_set_drvdata(dev, net_dev);
+	SET_NETDEV_DEV(net_dev, &dev->dev);
+
+	net_dev->features = 0;
+	net_dev->flags |= IFF_NOARP;
+
+	net_dev->netdev_ops = &ec_bhf_netdev_ops;
+
+	priv = netdev_priv(net_dev);
+	priv->net_dev = net_dev;
+	priv->io = io;
+	priv->dma_io = dma_io;
+	priv->dev = dev;
+
+	err = ec_bhf_setup_offsets(priv);
+	if (err < 0)
+		goto err_free_net_dev;
+
+	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;
+
+	return 0;
+
+err_free_net_dev:
+	free_netdev(net_dev);
+err_unmap_dma_io:
+	pci_iounmap(dev, dma_io);
+err_unmap:
+	pci_iounmap(dev, io);
+err_release_regions:
+	pci_release_regions(dev);
+err_disable_dev:
+	pci_clear_master(dev);
+	pci_disable_device(dev);
+
+	return err;
+}
+
+static void ec_bhf_remove(struct pci_dev *dev)
+{
+	struct net_device *net_dev = pci_get_drvdata(dev);
+	struct ec_bhf_priv *priv = netdev_priv(net_dev);
+
+	unregister_netdev(net_dev);
+	free_netdev(net_dev);
+
+	pci_iounmap(dev, priv->dma_io);
+	pci_iounmap(dev, priv->io);
+	pci_release_regions(dev);
+	pci_clear_master(dev);
+	pci_disable_device(dev);
+}
+
+static struct pci_driver pci_driver = {
+	.name		= "ec_bhf",
+	.id_table	= ids,
+	.probe		= ec_bhf_probe,
+	.remove		= ec_bhf_remove,
+};
+
+static int __init ec_bhf_init(void)
+{
+	return pci_register_driver(&pci_driver);
+}
+
+static void __exit ec_bhf_exit(void)
+{
+	pci_unregister_driver(&pci_driver);
+}
+
+module_init(ec_bhf_init);
+module_exit(ec_bhf_exit);
+
+module_param(polling_frequency, long, S_IRUGO);
+MODULE_PARM_DESC(polling_frequency, "Polling timer frequency in ns");
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Dariusz Marcinkiewicz <reksio@newterm.pl>");
diff --git a/drivers/net/ethernet/emulex/benet/be_main.c b/drivers/net/ethernet/emulex/benet/be_main.c
index a18645407d2..dc19bc5dec7 100644
--- a/drivers/net/ethernet/emulex/benet/be_main.c
+++ b/drivers/net/ethernet/emulex/benet/be_main.c
@@ -4949,6 +4949,12 @@ static void be_eeh_resume(struct pci_dev *pdev)
 	if (status)
 		goto err;
 
+	/* On some BE3 FW versions, after a HW reset,
+	 * interrupts will remain disabled for each function.
+	 * So, explicitly enable interrupts
+	 */
+	be_intr_set(adapter, true);
+
 	/* tell fw we're ready to fire cmds */
 	status = be_cmd_fw_init(adapter);
 	if (status)
diff --git a/drivers/net/ethernet/freescale/gianfar.c b/drivers/net/ethernet/freescale/gianfar.c
index e2d42475b00..ee6ddbd4f25 100644
--- a/drivers/net/ethernet/freescale/gianfar.c
+++ b/drivers/net/ethernet/freescale/gianfar.c
@@ -1798,9 +1798,9 @@ void stop_gfar(struct net_device *dev)
 
 	netif_tx_stop_all_queues(dev);
 
-	smp_mb__before_clear_bit();
+	smp_mb__before_atomic();
 	set_bit(GFAR_DOWN, &priv->state);
-	smp_mb__after_clear_bit();
+	smp_mb__after_atomic();
 
 	disable_napi(priv);
 
@@ -2043,9 +2043,9 @@ int startup_gfar(struct net_device *ndev)
 
 	gfar_init_tx_rx_base(priv);
 
-	smp_mb__before_clear_bit();
+	smp_mb__before_atomic();
 	clear_bit(GFAR_DOWN, &priv->state);
-	smp_mb__after_clear_bit();
+	smp_mb__after_atomic();
 
 	/* Start Rx/Tx DMA and enable the interrupts */
 	gfar_start(priv);
diff --git a/drivers/net/ethernet/ibm/emac/mal.c b/drivers/net/ethernet/ibm/emac/mal.c
index 9d75fef6396..63eb959a28a 100644
--- a/drivers/net/ethernet/ibm/emac/mal.c
+++ b/drivers/net/ethernet/ibm/emac/mal.c
@@ -682,10 +682,7 @@ static int mal_probe(struct platform_device *ofdev)
 		goto fail6;
 
 	/* Enable all MAL SERR interrupt sources */
-	if (mal->version == 2)
-		set_mal_dcrn(mal, MAL_IER, MAL2_IER_EVENTS);
-	else
-		set_mal_dcrn(mal, MAL_IER, MAL1_IER_EVENTS);
+	set_mal_dcrn(mal, MAL_IER, MAL_IER_EVENTS);
 
 	/* Enable EOB interrupt */
 	mal_enable_eob_irq(mal);
diff --git a/drivers/net/ethernet/ibm/emac/mal.h b/drivers/net/ethernet/ibm/emac/mal.h
index e431a32e3d6..eeade2ea833 100644
--- a/drivers/net/ethernet/ibm/emac/mal.h
+++ b/drivers/net/ethernet/ibm/emac/mal.h
@@ -95,24 +95,20 @@
 
 
 #define MAL_IER			0x02
+/* MAL IER bits */
 #define   MAL_IER_DE		0x00000010
 #define   MAL_IER_OTE		0x00000004
 #define   MAL_IER_OE		0x00000002
 #define   MAL_IER_PE		0x00000001
-/* MAL V1 IER bits */
-#define   MAL1_IER_NWE		0x00000008
-#define   MAL1_IER_SOC_EVENTS	MAL1_IER_NWE
-#define   MAL1_IER_EVENTS	(MAL1_IER_SOC_EVENTS | MAL_IER_DE | \
-				 MAL_IER_OTE | MAL_IER_OE | MAL_IER_PE)
 
-/* MAL V2 IER bits */
-#define   MAL2_IER_PT		0x00000080
-#define   MAL2_IER_PRE		0x00000040
-#define   MAL2_IER_PWE		0x00000020
-#define   MAL2_IER_SOC_EVENTS	(MAL2_IER_PT | MAL2_IER_PRE | MAL2_IER_PWE)
-#define   MAL2_IER_EVENTS	(MAL2_IER_SOC_EVENTS | MAL_IER_DE | \
-				 MAL_IER_OTE | MAL_IER_OE | MAL_IER_PE)
+/* PLB read/write/timeout errors */
+#define   MAL_IER_PTE		0x00000080
+#define   MAL_IER_PRE		0x00000040
+#define   MAL_IER_PWE		0x00000020
 
+#define   MAL_IER_SOC_EVENTS	(MAL_IER_PTE | MAL_IER_PRE | MAL_IER_PWE)
+#define   MAL_IER_EVENTS	(MAL_IER_SOC_EVENTS | MAL_IER_DE | \
+				 MAL_IER_OTE | MAL_IER_OE | MAL_IER_PE)
 
 #define MAL_TXCASR		0x04
 #define MAL_TXCARR		0x05
diff --git a/drivers/net/ethernet/ibm/emac/rgmii.c b/drivers/net/ethernet/ibm/emac/rgmii.c
index 4fb2f96da23..a01182cce96 100644
--- a/drivers/net/ethernet/ibm/emac/rgmii.c
+++ b/drivers/net/ethernet/ibm/emac/rgmii.c
@@ -45,6 +45,7 @@
 
 /* RGMIIx_SSR */
 #define RGMII_SSR_MASK(idx)	(0x7 << ((idx) * 8))
+#define RGMII_SSR_10(idx)	(0x1 << ((idx) * 8))
 #define RGMII_SSR_100(idx)	(0x2 << ((idx) * 8))
 #define RGMII_SSR_1000(idx)	(0x4 << ((idx) * 8))
 
@@ -139,6 +140,8 @@ void rgmii_set_speed(struct platform_device *ofdev, int input, int speed)
 		ssr |= RGMII_SSR_1000(input);
 	else if (speed == SPEED_100)
 		ssr |= RGMII_SSR_100(input);
+	else if (speed == SPEED_10)
+		ssr |= RGMII_SSR_10(input);
 
 	out_be32(&p->ssr, ssr);
 
diff --git a/drivers/net/ethernet/intel/i40e/i40e_main.c b/drivers/net/ethernet/intel/i40e/i40e_main.c
index cf0761f0891..2e72449f126 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_main.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_main.c
@@ -4676,7 +4676,7 @@ static void i40e_service_event_complete(struct i40e_pf *pf)
 	BUG_ON(!test_bit(__I40E_SERVICE_SCHED, &pf->state));
 
 	/* flush memory to make sure state is correct before next watchog */
-	smp_mb__before_clear_bit();
+	smp_mb__before_atomic();
 	clear_bit(__I40E_SERVICE_SCHED, &pf->state);
 }
 
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
index d62e7a25cf9..c047c3ef8d7 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
@@ -376,7 +376,7 @@ static void ixgbe_service_event_complete(struct ixgbe_adapter *adapter)
 	BUG_ON(!test_bit(__IXGBE_SERVICE_SCHED, &adapter->state));
 
 	/* flush memory to make sure state is correct before next watchdog */
-	smp_mb__before_clear_bit();
+	smp_mb__before_atomic();
 	clear_bit(__IXGBE_SERVICE_SCHED, &adapter->state);
 }
 
@@ -4672,7 +4672,7 @@ static void ixgbe_up_complete(struct ixgbe_adapter *adapter)
 	if (hw->mac.ops.enable_tx_laser)
 		hw->mac.ops.enable_tx_laser(hw);
 
-	smp_mb__before_clear_bit();
+	smp_mb__before_atomic();
 	clear_bit(__IXGBE_DOWN, &adapter->state);
 	ixgbe_napi_enable_all(adapter);
 
@@ -5568,7 +5568,7 @@ static int ixgbe_resume(struct pci_dev *pdev)
 		e_dev_err("Cannot enable PCI device from suspend\n");
 		return err;
 	}
-	smp_mb__before_clear_bit();
+	smp_mb__before_atomic();
 	clear_bit(__IXGBE_DISABLED, &adapter->state);
 	pci_set_master(pdev);
 
@@ -8542,7 +8542,7 @@ static pci_ers_result_t ixgbe_io_slot_reset(struct pci_dev *pdev)
 		e_err(probe, "Cannot re-enable PCI device after reset.\n");
 		result = PCI_ERS_RESULT_DISCONNECT;
 	} else {
-		smp_mb__before_clear_bit();
+		smp_mb__before_atomic();
 		clear_bit(__IXGBE_DISABLED, &adapter->state);
 		adapter->hw.hw_addr = adapter->io_addr;
 		pci_set_master(pdev);
diff --git a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
index d0799e8e31e..de2793b0630 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
+++ b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
@@ -1668,7 +1668,7 @@ static void ixgbevf_up_complete(struct ixgbevf_adapter *adapter)
 
 	spin_unlock_bh(&adapter->mbx_lock);
 
-	smp_mb__before_clear_bit();
+	smp_mb__before_atomic();
 	clear_bit(__IXGBEVF_DOWN, &adapter->state);
 	ixgbevf_napi_enable_all(adapter);
 
@@ -3354,7 +3354,7 @@ static int ixgbevf_resume(struct pci_dev *pdev)
 		dev_err(&pdev->dev, "Cannot enable PCI device from suspend\n");
 		return err;
 	}
-	smp_mb__before_clear_bit();
+	smp_mb__before_atomic();
 	clear_bit(__IXGBEVF_DISABLED, &adapter->state);
 	pci_set_master(pdev);
 
@@ -3712,7 +3712,7 @@ static pci_ers_result_t ixgbevf_io_slot_reset(struct pci_dev *pdev)
 		return PCI_ERS_RESULT_DISCONNECT;
 	}
 
-	smp_mb__before_clear_bit();
+	smp_mb__before_atomic();
 	clear_bit(__IXGBEVF_DISABLED, &adapter->state);
 	pci_set_master(pdev);
 
diff --git a/drivers/net/ethernet/jme.c b/drivers/net/ethernet/jme.c
index b0c6050479e..b78378cea5e 100644
--- a/drivers/net/ethernet/jme.c
+++ b/drivers/net/ethernet/jme.c
@@ -1988,7 +1988,7 @@ jme_alloc_txdesc(struct jme_adapter *jme,
 	return idx;
 }
 
-static void
+static int
 jme_fill_tx_map(struct pci_dev *pdev,
 		struct txdesc *txdesc,
 		struct jme_buffer_info *txbi,
@@ -2005,6 +2005,9 @@ jme_fill_tx_map(struct pci_dev *pdev,
 				len,
 				PCI_DMA_TODEVICE);
 
+	if (unlikely(pci_dma_mapping_error(pdev, dmaaddr)))
+		return -EINVAL;
+
 	pci_dma_sync_single_for_device(pdev,
 				       dmaaddr,
 				       len,
@@ -2021,9 +2024,30 @@ jme_fill_tx_map(struct pci_dev *pdev,
 
 	txbi->mapping = dmaaddr;
 	txbi->len = len;
+	return 0;
 }
 
-static void
+static void jme_drop_tx_map(struct jme_adapter *jme, int startidx, int count)
+{
+	struct jme_ring *txring = &(jme->txring[0]);
+	struct jme_buffer_info *txbi = txring->bufinf, *ctxbi;
+	int mask = jme->tx_ring_mask;
+	int j;
+
+	for (j = 0 ; j < count ; j++) {
+		ctxbi = txbi + ((startidx + j + 2) & (mask));
+		pci_unmap_page(jme->pdev,
+				ctxbi->mapping,
+				ctxbi->len,
+				PCI_DMA_TODEVICE);
+
+				ctxbi->mapping = 0;
+				ctxbi->len = 0;
+	}
+
+}
+
+static int
 jme_map_tx_skb(struct jme_adapter *jme, struct sk_buff *skb, int idx)
 {
 	struct jme_ring *txring = &(jme->txring[0]);
@@ -2034,25 +2058,37 @@ jme_map_tx_skb(struct jme_adapter *jme, struct sk_buff *skb, int idx)
 	int mask = jme->tx_ring_mask;
 	const struct skb_frag_struct *frag;
 	u32 len;
+	int ret = 0;
 
 	for (i = 0 ; i < nr_frags ; ++i) {
 		frag = &skb_shinfo(skb)->frags[i];
 		ctxdesc = txdesc + ((idx + i + 2) & (mask));
 		ctxbi = txbi + ((idx + i + 2) & (mask));
 
-		jme_fill_tx_map(jme->pdev, ctxdesc, ctxbi,
+		ret = jme_fill_tx_map(jme->pdev, ctxdesc, ctxbi,
 				skb_frag_page(frag),
 				frag->page_offset, skb_frag_size(frag), hidma);
+		if (ret) {
+			jme_drop_tx_map(jme, idx, i);
+			goto out;
+		}
+
 	}
 
 	len = skb_is_nonlinear(skb) ? skb_headlen(skb) : skb->len;
 	ctxdesc = txdesc + ((idx + 1) & (mask));
 	ctxbi = txbi + ((idx + 1) & (mask));
-	jme_fill_tx_map(jme->pdev, ctxdesc, ctxbi, virt_to_page(skb->data),
+	ret = jme_fill_tx_map(jme->pdev, ctxdesc, ctxbi, virt_to_page(skb->data),
 			offset_in_page(skb->data), len, hidma);
+	if (ret)
+		jme_drop_tx_map(jme, idx, i);
+
+out:
+	return ret;
 
 }
 
+
 static int
 jme_tx_tso(struct sk_buff *skb, __le16 *mss, u8 *flags)
 {
@@ -2131,6 +2167,7 @@ jme_fill_tx_desc(struct jme_adapter *jme, struct sk_buff *skb, int idx)
 	struct txdesc *txdesc;
 	struct jme_buffer_info *txbi;
 	u8 flags;
+	int ret = 0;
 
 	txdesc = (struct txdesc *)txring->desc + idx;
 	txbi = txring->bufinf + idx;
@@ -2155,7 +2192,10 @@ jme_fill_tx_desc(struct jme_adapter *jme, struct sk_buff *skb, int idx)
 	if (jme_tx_tso(skb, &txdesc->desc1.mss, &flags))
 		jme_tx_csum(jme, skb, &flags);
 	jme_tx_vlan(skb, &txdesc->desc1.vlan, &flags);
-	jme_map_tx_skb(jme, skb, idx);
+	ret = jme_map_tx_skb(jme, skb, idx);
+	if (ret)
+		return ret;
+
 	txdesc->desc1.flags = flags;
 	/*
 	 * Set tx buffer info after telling NIC to send
@@ -2228,7 +2268,8 @@ jme_start_xmit(struct sk_buff *skb, struct net_device *netdev)
 		return NETDEV_TX_BUSY;
 	}
 
-	jme_fill_tx_desc(jme, skb, idx);
+	if (jme_fill_tx_desc(jme, skb, idx))
+		return NETDEV_TX_OK;
 
 	jwrite32(jme, JME_TXCS, jme->reg_txcs |
 				TXCS_SELECT_QUEUE0 |
diff --git a/drivers/net/ethernet/mellanox/mlx4/alloc.c b/drivers/net/ethernet/mellanox/mlx4/alloc.c
index c3ad464d062..b0297da5030 100644
--- a/drivers/net/ethernet/mellanox/mlx4/alloc.c
+++ b/drivers/net/ethernet/mellanox/mlx4/alloc.c
@@ -171,7 +171,7 @@ void mlx4_bitmap_cleanup(struct mlx4_bitmap *bitmap)
  */
 
 int mlx4_buf_alloc(struct mlx4_dev *dev, int size, int max_direct,
-		   struct mlx4_buf *buf)
+		   struct mlx4_buf *buf, gfp_t gfp)
 {
 	dma_addr_t t;
 
@@ -180,7 +180,7 @@ int mlx4_buf_alloc(struct mlx4_dev *dev, int size, int max_direct,
 		buf->npages       = 1;
 		buf->page_shift   = get_order(size) + PAGE_SHIFT;
 		buf->direct.buf   = dma_alloc_coherent(&dev->pdev->dev,
-						       size, &t, GFP_KERNEL);
+						       size, &t, gfp);
 		if (!buf->direct.buf)
 			return -ENOMEM;
 
@@ -200,14 +200,14 @@ int mlx4_buf_alloc(struct mlx4_dev *dev, int size, int max_direct,
 		buf->npages      = buf->nbufs;
 		buf->page_shift  = PAGE_SHIFT;
 		buf->page_list   = kcalloc(buf->nbufs, sizeof(*buf->page_list),
-					   GFP_KERNEL);
+					   gfp);
 		if (!buf->page_list)
 			return -ENOMEM;
 
 		for (i = 0; i < buf->nbufs; ++i) {
 			buf->page_list[i].buf =
 				dma_alloc_coherent(&dev->pdev->dev, PAGE_SIZE,
-						   &t, GFP_KERNEL);
+						   &t, gfp);
 			if (!buf->page_list[i].buf)
 				goto err_free;
 
@@ -218,7 +218,7 @@ int mlx4_buf_alloc(struct mlx4_dev *dev, int size, int max_direct,
 
 		if (BITS_PER_LONG == 64) {
 			struct page **pages;
-			pages = kmalloc(sizeof *pages * buf->nbufs, GFP_KERNEL);
+			pages = kmalloc(sizeof *pages * buf->nbufs, gfp);
 			if (!pages)
 				goto err_free;
 			for (i = 0; i < buf->nbufs; ++i)
@@ -260,11 +260,12 @@ void mlx4_buf_free(struct mlx4_dev *dev, int size, struct mlx4_buf *buf)
 }
 EXPORT_SYMBOL_GPL(mlx4_buf_free);
 
-static struct mlx4_db_pgdir *mlx4_alloc_db_pgdir(struct device *dma_device)
+static struct mlx4_db_pgdir *mlx4_alloc_db_pgdir(struct device *dma_device,
+						 gfp_t gfp)
 {
 	struct mlx4_db_pgdir *pgdir;
 
-	pgdir = kzalloc(sizeof *pgdir, GFP_KERNEL);
+	pgdir = kzalloc(sizeof *pgdir, gfp);
 	if (!pgdir)
 		return NULL;
 
@@ -272,7 +273,7 @@ static struct mlx4_db_pgdir *mlx4_alloc_db_pgdir(struct device *dma_device)
 	pgdir->bits[0] = pgdir->order0;
 	pgdir->bits[1] = pgdir->order1;
 	pgdir->db_page = dma_alloc_coherent(dma_device, PAGE_SIZE,
-					    &pgdir->db_dma, GFP_KERNEL);
+					    &pgdir->db_dma, gfp);
 	if (!pgdir->db_page) {
 		kfree(pgdir);
 		return NULL;
@@ -312,7 +313,7 @@ found:
 	return 0;
 }
 
-int mlx4_db_alloc(struct mlx4_dev *dev, struct mlx4_db *db, int order)
+int mlx4_db_alloc(struct mlx4_dev *dev, struct mlx4_db *db, int order, gfp_t gfp)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
 	struct mlx4_db_pgdir *pgdir;
@@ -324,7 +325,7 @@ int mlx4_db_alloc(struct mlx4_dev *dev, struct mlx4_db *db, int order)
 		if (!mlx4_alloc_db_from_pgdir(pgdir, db, order))
 			goto out;
 
-	pgdir = mlx4_alloc_db_pgdir(&(dev->pdev->dev));
+	pgdir = mlx4_alloc_db_pgdir(&(dev->pdev->dev), gfp);
 	if (!pgdir) {
 		ret = -ENOMEM;
 		goto out;
@@ -376,13 +377,13 @@ int mlx4_alloc_hwq_res(struct mlx4_dev *dev, struct mlx4_hwq_resources *wqres,
 {
 	int err;
 
-	err = mlx4_db_alloc(dev, &wqres->db, 1);
+	err = mlx4_db_alloc(dev, &wqres->db, 1, GFP_KERNEL);
 	if (err)
 		return err;
 
 	*wqres->db.db = 0;
 
-	err = mlx4_buf_alloc(dev, size, max_direct, &wqres->buf);
+	err = mlx4_buf_alloc(dev, size, max_direct, &wqres->buf, GFP_KERNEL);
 	if (err)
 		goto err_db;
 
@@ -391,7 +392,7 @@ int mlx4_alloc_hwq_res(struct mlx4_dev *dev, struct mlx4_hwq_resources *wqres,
 	if (err)
 		goto err_buf;
 
-	err = mlx4_buf_write_mtt(dev, &wqres->mtt, &wqres->buf);
+	err = mlx4_buf_write_mtt(dev, &wqres->mtt, &wqres->buf, GFP_KERNEL);
 	if (err)
 		goto err_mtt;
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/cmd.c b/drivers/net/ethernet/mellanox/mlx4/cmd.c
index 78099eab767..29b616990e5 100644
--- a/drivers/net/ethernet/mellanox/mlx4/cmd.c
+++ b/drivers/net/ethernet/mellanox/mlx4/cmd.c
@@ -705,20 +705,28 @@ static int mlx4_MAD_IFC_wrapper(struct mlx4_dev *dev, int slave,
 	struct ib_smp *smp = inbox->buf;
 	u32 index;
 	u8 port;
+	u8 opcode_modifier;
 	u16 *table;
 	int err;
 	int vidx, pidx;
+	int network_view;
 	struct mlx4_priv *priv = mlx4_priv(dev);
 	struct ib_smp *outsmp = outbox->buf;
 	__be16 *outtab = (__be16 *)(outsmp->data);
 	__be32 slave_cap_mask;
 	__be64 slave_node_guid;
+
 	port = vhcr->in_modifier;
 
+	/* network-view bit is for driver use only, and should not be passed to FW */
+	opcode_modifier = vhcr->op_modifier & ~0x8; /* clear netw view bit */
+	network_view = !!(vhcr->op_modifier & 0x8);
+
 	if (smp->base_version == 1 &&
 	    smp->mgmt_class == IB_MGMT_CLASS_SUBN_LID_ROUTED &&
 	    smp->class_version == 1) {
-		if (smp->method	== IB_MGMT_METHOD_GET) {
+		/* host view is paravirtualized */
+		if (!network_view && smp->method == IB_MGMT_METHOD_GET) {
 			if (smp->attr_id == IB_SMP_ATTR_PKEY_TABLE) {
 				index = be32_to_cpu(smp->attr_mod);
 				if (port < 1 || port > dev->caps.num_ports)
@@ -743,7 +751,7 @@ static int mlx4_MAD_IFC_wrapper(struct mlx4_dev *dev, int slave,
 				/*get the slave specific caps:*/
 				/*do the command */
 				err = mlx4_cmd_box(dev, inbox->dma, outbox->dma,
-					    vhcr->in_modifier, vhcr->op_modifier,
+					    vhcr->in_modifier, opcode_modifier,
 					    vhcr->op, MLX4_CMD_TIME_CLASS_C, MLX4_CMD_NATIVE);
 				/* modify the response for slaves */
 				if (!err && slave != mlx4_master_func_num(dev)) {
@@ -760,7 +768,7 @@ static int mlx4_MAD_IFC_wrapper(struct mlx4_dev *dev, int slave,
 				smp->attr_mod = cpu_to_be32(slave / 8);
 				/* execute cmd */
 				err = mlx4_cmd_box(dev, inbox->dma, outbox->dma,
-					     vhcr->in_modifier, vhcr->op_modifier,
+					     vhcr->in_modifier, opcode_modifier,
 					     vhcr->op, MLX4_CMD_TIME_CLASS_C, MLX4_CMD_NATIVE);
 				if (!err) {
 					/* if needed, move slave gid to index 0 */
@@ -774,7 +782,7 @@ static int mlx4_MAD_IFC_wrapper(struct mlx4_dev *dev, int slave,
 			}
 			if (smp->attr_id == IB_SMP_ATTR_NODE_INFO) {
 				err = mlx4_cmd_box(dev, inbox->dma, outbox->dma,
-					     vhcr->in_modifier, vhcr->op_modifier,
+					     vhcr->in_modifier, opcode_modifier,
 					     vhcr->op, MLX4_CMD_TIME_CLASS_C, MLX4_CMD_NATIVE);
 				if (!err) {
 					slave_node_guid =  mlx4_get_slave_node_guid(dev, slave);
@@ -784,19 +792,24 @@ static int mlx4_MAD_IFC_wrapper(struct mlx4_dev *dev, int slave,
 			}
 		}
 	}
+
+	/* Non-privileged VFs are only allowed "host" view LID-routed 'Get' MADs.
+	 * These are the MADs used by ib verbs (such as ib_query_gids).
+	 */
 	if (slave != mlx4_master_func_num(dev) &&
-	    ((smp->mgmt_class == IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE) ||
-	     (smp->mgmt_class == IB_MGMT_CLASS_SUBN_LID_ROUTED &&
-	      smp->method == IB_MGMT_METHOD_SET))) {
-		mlx4_err(dev, "slave %d is trying to execute a Subnet MGMT MAD, "
-			 "class 0x%x, method 0x%x for attr 0x%x. Rejecting\n",
-			 slave, smp->method, smp->mgmt_class,
-			 be16_to_cpu(smp->attr_id));
-		return -EPERM;
+	    !mlx4_vf_smi_enabled(dev, slave, port)) {
+		if (!(smp->mgmt_class == IB_MGMT_CLASS_SUBN_LID_ROUTED &&
+		      smp->method == IB_MGMT_METHOD_GET) || network_view) {
+			mlx4_err(dev, "Unprivileged slave %d is trying to execute a Subnet MGMT MAD, class 0x%x, method 0x%x, view=%s for attr 0x%x. Rejecting\n",
+				 slave, smp->method, smp->mgmt_class,
+				 network_view ? "Network" : "Host",
+				 be16_to_cpu(smp->attr_id));
+			return -EPERM;
+		}
 	}
-	/*default:*/
+
 	return mlx4_cmd_box(dev, inbox->dma, outbox->dma,
-				    vhcr->in_modifier, vhcr->op_modifier,
+				    vhcr->in_modifier, opcode_modifier,
 				    vhcr->op, MLX4_CMD_TIME_CLASS_C, MLX4_CMD_NATIVE);
 }
 
@@ -1253,12 +1266,12 @@ static struct mlx4_cmd_info cmd_info[] = {
 	},
 	{
 		.opcode = MLX4_CMD_UPDATE_QP,
-		.has_inbox = false,
+		.has_inbox = true,
 		.has_outbox = false,
 		.out_is_imm = false,
 		.encode_slave_id = false,
 		.verify = NULL,
-		.wrapper = mlx4_CMD_EPERM_wrapper
+		.wrapper = mlx4_UPDATE_QP_wrapper
 	},
 	{
 		.opcode = MLX4_CMD_GET_OP_REQ,
@@ -1653,6 +1666,8 @@ static int mlx4_master_activate_admin_state(struct mlx4_priv *priv, int slave)
 	for (port = min_port; port <= max_port; port++) {
 		if (!test_bit(port - 1, actv_ports.ports))
 			continue;
+		priv->mfunc.master.vf_oper[slave].smi_enabled[port] =
+			priv->mfunc.master.vf_admin[slave].enable_smi[port];
 		vp_oper = &priv->mfunc.master.vf_oper[slave].vport[port];
 		vp_admin = &priv->mfunc.master.vf_admin[slave].vport[port];
 		vp_oper->state = *vp_admin;
@@ -1704,6 +1719,8 @@ static void mlx4_master_deactivate_admin_state(struct mlx4_priv *priv, int slave
 	for (port = min_port; port <= max_port; port++) {
 		if (!test_bit(port - 1, actv_ports.ports))
 			continue;
+		priv->mfunc.master.vf_oper[slave].smi_enabled[port] =
+			MLX4_VF_SMI_DISABLED;
 		vp_oper = &priv->mfunc.master.vf_oper[slave].vport[port];
 		if (NO_INDX != vp_oper->vlan_idx) {
 			__mlx4_unregister_vlan(&priv->dev,
@@ -2537,3 +2554,50 @@ int mlx4_set_vf_link_state(struct mlx4_dev *dev, int port, int vf, int link_stat
 	return 0;
 }
 EXPORT_SYMBOL_GPL(mlx4_set_vf_link_state);
+
+int mlx4_vf_smi_enabled(struct mlx4_dev *dev, int slave, int port)
+{
+	struct mlx4_priv *priv = mlx4_priv(dev);
+
+	if (slave < 1 || slave >= dev->num_slaves ||
+	    port < 1 || port > MLX4_MAX_PORTS)
+		return 0;
+
+	return priv->mfunc.master.vf_oper[slave].smi_enabled[port] ==
+		MLX4_VF_SMI_ENABLED;
+}
+EXPORT_SYMBOL_GPL(mlx4_vf_smi_enabled);
+
+int mlx4_vf_get_enable_smi_admin(struct mlx4_dev *dev, int slave, int port)
+{
+	struct mlx4_priv *priv = mlx4_priv(dev);
+
+	if (slave == mlx4_master_func_num(dev))
+		return 1;
+
+	if (slave < 1 || slave >= dev->num_slaves ||
+	    port < 1 || port > MLX4_MAX_PORTS)
+		return 0;
+
+	return priv->mfunc.master.vf_admin[slave].enable_smi[port] ==
+		MLX4_VF_SMI_ENABLED;
+}
+EXPORT_SYMBOL_GPL(mlx4_vf_get_enable_smi_admin);
+
+int mlx4_vf_set_enable_smi_admin(struct mlx4_dev *dev, int slave, int port,
+				 int enabled)
+{
+	struct mlx4_priv *priv = mlx4_priv(dev);
+
+	if (slave == mlx4_master_func_num(dev))
+		return 0;
+
+	if (slave < 1 || slave >= dev->num_slaves ||
+	    port < 1 || port > MLX4_MAX_PORTS ||
+	    enabled < 0 || enabled > 1)
+		return -EINVAL;
+
+	priv->mfunc.master.vf_admin[slave].enable_smi[port] = enabled;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mlx4_vf_set_enable_smi_admin);
diff --git a/drivers/net/ethernet/mellanox/mlx4/cq.c b/drivers/net/ethernet/mellanox/mlx4/cq.c
index 0487121e4a0..c90cde5b4ae 100644
--- a/drivers/net/ethernet/mellanox/mlx4/cq.c
+++ b/drivers/net/ethernet/mellanox/mlx4/cq.c
@@ -173,11 +173,11 @@ int __mlx4_cq_alloc_icm(struct mlx4_dev *dev, int *cqn)
 	if (*cqn == -1)
 		return -ENOMEM;
 
-	err = mlx4_table_get(dev, &cq_table->table, *cqn);
+	err = mlx4_table_get(dev, &cq_table->table, *cqn, GFP_KERNEL);
 	if (err)
 		goto err_out;
 
-	err = mlx4_table_get(dev, &cq_table->cmpt_table, *cqn);
+	err = mlx4_table_get(dev, &cq_table->cmpt_table, *cqn, GFP_KERNEL);
 	if (err)
 		goto err_put;
 	return 0;
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_rx.c b/drivers/net/ethernet/mellanox/mlx4/en_rx.c
index ba049ae8874..87857a6463e 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_rx.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_rx.c
@@ -972,7 +972,7 @@ static int mlx4_en_config_rss_qp(struct mlx4_en_priv *priv, int qpn,
 	if (!context)
 		return -ENOMEM;
 
-	err = mlx4_qp_alloc(mdev->dev, qpn, qp);
+	err = mlx4_qp_alloc(mdev->dev, qpn, qp, GFP_KERNEL);
 	if (err) {
 		en_err(priv, "Failed to allocate qp #%x\n", qpn);
 		goto out;
@@ -1012,7 +1012,7 @@ int mlx4_en_create_drop_qp(struct mlx4_en_priv *priv)
 		en_err(priv, "Failed reserving drop qpn\n");
 		return err;
 	}
-	err = mlx4_qp_alloc(priv->mdev->dev, qpn, &priv->drop_qp);
+	err = mlx4_qp_alloc(priv->mdev->dev, qpn, &priv->drop_qp, GFP_KERNEL);
 	if (err) {
 		en_err(priv, "Failed allocating drop qp\n");
 		mlx4_qp_release_range(priv->mdev->dev, qpn, 1);
@@ -1071,7 +1071,7 @@ int mlx4_en_config_rss_steer(struct mlx4_en_priv *priv)
 	}
 
 	/* Configure RSS indirection qp */
-	err = mlx4_qp_alloc(mdev->dev, priv->base_qpn, &rss_map->indir_qp);
+	err = mlx4_qp_alloc(mdev->dev, priv->base_qpn, &rss_map->indir_qp, GFP_KERNEL);
 	if (err) {
 		en_err(priv, "Failed to allocate RSS indirection QP\n");
 		goto rss_err;
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_tx.c b/drivers/net/ethernet/mellanox/mlx4/en_tx.c
index dd1f6d34645..bc0cc1eb214 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_tx.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_tx.c
@@ -113,7 +113,7 @@ int mlx4_en_create_tx_ring(struct mlx4_en_priv *priv,
 	       ring->buf_size, (unsigned long long) ring->wqres.buf.direct.map);
 
 	ring->qpn = qpn;
-	err = mlx4_qp_alloc(mdev->dev, ring->qpn, &ring->qp);
+	err = mlx4_qp_alloc(mdev->dev, ring->qpn, &ring->qp, GFP_KERNEL);
 	if (err) {
 		en_err(priv, "Failed allocating qp %d\n", ring->qpn);
 		goto err_map;
diff --git a/drivers/net/ethernet/mellanox/mlx4/fw.c b/drivers/net/ethernet/mellanox/mlx4/fw.c
index d16a4d11890..01e6dd61ee3 100644
--- a/drivers/net/ethernet/mellanox/mlx4/fw.c
+++ b/drivers/net/ethernet/mellanox/mlx4/fw.c
@@ -178,8 +178,8 @@ int mlx4_QUERY_FUNC_CAP_wrapper(struct mlx4_dev *dev, int slave,
 				struct mlx4_cmd_info *cmd)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
-	u8	field;
-	u32	size;
+	u8	field, port;
+	u32	size, proxy_qp, qkey;
 	int	err = 0;
 
 #define QUERY_FUNC_CAP_FLAGS_OFFSET		0x0
@@ -209,6 +209,7 @@ int mlx4_QUERY_FUNC_CAP_wrapper(struct mlx4_dev *dev, int slave,
 
 /* when opcode modifier = 1 */
 #define QUERY_FUNC_CAP_PHYS_PORT_OFFSET		0x3
+#define QUERY_FUNC_CAP_PRIV_VF_QKEY_OFFSET	0x4
 #define QUERY_FUNC_CAP_FLAGS0_OFFSET		0x8
 #define QUERY_FUNC_CAP_FLAGS1_OFFSET		0xc
 
@@ -221,6 +222,7 @@ int mlx4_QUERY_FUNC_CAP_wrapper(struct mlx4_dev *dev, int slave,
 #define QUERY_FUNC_CAP_FLAGS1_FORCE_MAC		0x40
 #define QUERY_FUNC_CAP_FLAGS1_FORCE_VLAN	0x80
 #define QUERY_FUNC_CAP_FLAGS1_NIC_INFO			0x10
+#define QUERY_FUNC_CAP_VF_ENABLE_QP0		0x08
 
 #define QUERY_FUNC_CAP_FLAGS0_FORCE_PHY_WQE_GID 0x80
 
@@ -234,28 +236,35 @@ int mlx4_QUERY_FUNC_CAP_wrapper(struct mlx4_dev *dev, int slave,
 			return -EINVAL;
 
 		vhcr->in_modifier = converted_port;
-		/* Set nic_info bit to mark new fields support */
-		field  = QUERY_FUNC_CAP_FLAGS1_NIC_INFO;
-		MLX4_PUT(outbox->buf, field, QUERY_FUNC_CAP_FLAGS1_OFFSET);
-
 		/* phys-port = logical-port */
 		field = vhcr->in_modifier -
 			find_first_bit(actv_ports.ports, dev->caps.num_ports);
 		MLX4_PUT(outbox->buf, field, QUERY_FUNC_CAP_PHYS_PORT_OFFSET);
 
-		field = vhcr->in_modifier;
+		port = vhcr->in_modifier;
+		proxy_qp = dev->phys_caps.base_proxy_sqpn + 8 * slave + port - 1;
+
+		/* Set nic_info bit to mark new fields support */
+		field  = QUERY_FUNC_CAP_FLAGS1_NIC_INFO;
+
+		if (mlx4_vf_smi_enabled(dev, slave, port) &&
+		    !mlx4_get_parav_qkey(dev, proxy_qp, &qkey)) {
+			field |= QUERY_FUNC_CAP_VF_ENABLE_QP0;
+			MLX4_PUT(outbox->buf, qkey,
+				 QUERY_FUNC_CAP_PRIV_VF_QKEY_OFFSET);
+		}
+		MLX4_PUT(outbox->buf, field, QUERY_FUNC_CAP_FLAGS1_OFFSET);
+
 		/* size is now the QP number */
-		size = dev->phys_caps.base_tunnel_sqpn + 8 * slave + field - 1;
+		size = dev->phys_caps.base_tunnel_sqpn + 8 * slave + port - 1;
 		MLX4_PUT(outbox->buf, size, QUERY_FUNC_CAP_QP0_TUNNEL);
 
 		size += 2;
 		MLX4_PUT(outbox->buf, size, QUERY_FUNC_CAP_QP1_TUNNEL);
 
-		size = dev->phys_caps.base_proxy_sqpn + 8 * slave + field - 1;
-		MLX4_PUT(outbox->buf, size, QUERY_FUNC_CAP_QP0_PROXY);
-
-		size += 2;
-		MLX4_PUT(outbox->buf, size, QUERY_FUNC_CAP_QP1_PROXY);
+		MLX4_PUT(outbox->buf, proxy_qp, QUERY_FUNC_CAP_QP0_PROXY);
+		proxy_qp += 2;
+		MLX4_PUT(outbox->buf, proxy_qp, QUERY_FUNC_CAP_QP1_PROXY);
 
 		MLX4_PUT(outbox->buf, dev->caps.phys_port_id[vhcr->in_modifier],
 			 QUERY_FUNC_CAP_PHYS_PORT_ID);
@@ -326,7 +335,7 @@ int mlx4_QUERY_FUNC_CAP(struct mlx4_dev *dev, u32 gen_or_port,
 	struct mlx4_cmd_mailbox *mailbox;
 	u32			*outbox;
 	u8			field, op_modifier;
-	u32			size;
+	u32			size, qkey;
 	int			err = 0, quotas = 0;
 
 	op_modifier = !!gen_or_port; /* 0 = general, 1 = logical port */
@@ -414,7 +423,7 @@ int mlx4_QUERY_FUNC_CAP(struct mlx4_dev *dev, u32 gen_or_port,
 
 	MLX4_GET(func_cap->flags1, outbox, QUERY_FUNC_CAP_FLAGS1_OFFSET);
 	if (dev->caps.port_type[gen_or_port] == MLX4_PORT_TYPE_ETH) {
-		if (func_cap->flags1 & QUERY_FUNC_CAP_FLAGS1_OFFSET) {
+		if (func_cap->flags1 & QUERY_FUNC_CAP_FLAGS1_FORCE_VLAN) {
 			mlx4_err(dev, "VLAN is enforced on this port\n");
 			err = -EPROTONOSUPPORT;
 			goto out;
@@ -442,6 +451,13 @@ int mlx4_QUERY_FUNC_CAP(struct mlx4_dev *dev, u32 gen_or_port,
 		goto out;
 	}
 
+	if (func_cap->flags1 & QUERY_FUNC_CAP_VF_ENABLE_QP0) {
+		MLX4_GET(qkey, outbox, QUERY_FUNC_CAP_PRIV_VF_QKEY_OFFSET);
+		func_cap->qp0_qkey = qkey;
+	} else {
+		func_cap->qp0_qkey = 0;
+	}
+
 	MLX4_GET(size, outbox, QUERY_FUNC_CAP_QP0_TUNNEL);
 	func_cap->qp0_tunnel_qpn = size & 0xFFFFFF;
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/fw.h b/drivers/net/ethernet/mellanox/mlx4/fw.h
index 6811ee00ba7..1fce03ebe5c 100644
--- a/drivers/net/ethernet/mellanox/mlx4/fw.h
+++ b/drivers/net/ethernet/mellanox/mlx4/fw.h
@@ -134,6 +134,7 @@ struct mlx4_func_cap {
 	int	max_eq;
 	int	reserved_eq;
 	int	mcg_quota;
+	u32	qp0_qkey;
 	u32	qp0_tunnel_qpn;
 	u32	qp0_proxy_qpn;
 	u32	qp1_tunnel_qpn;
diff --git a/drivers/net/ethernet/mellanox/mlx4/icm.c b/drivers/net/ethernet/mellanox/mlx4/icm.c
index 5fbf4924c27..97c9b1db1d2 100644
--- a/drivers/net/ethernet/mellanox/mlx4/icm.c
+++ b/drivers/net/ethernet/mellanox/mlx4/icm.c
@@ -245,7 +245,8 @@ int mlx4_UNMAP_ICM_AUX(struct mlx4_dev *dev)
 			MLX4_CMD_TIME_CLASS_B, MLX4_CMD_NATIVE);
 }
 
-int mlx4_table_get(struct mlx4_dev *dev, struct mlx4_icm_table *table, u32 obj)
+int mlx4_table_get(struct mlx4_dev *dev, struct mlx4_icm_table *table, u32 obj,
+		   gfp_t gfp)
 {
 	u32 i = (obj & (table->num_obj - 1)) /
 			(MLX4_TABLE_CHUNK_SIZE / table->obj_size);
@@ -259,7 +260,7 @@ int mlx4_table_get(struct mlx4_dev *dev, struct mlx4_icm_table *table, u32 obj)
 	}
 
 	table->icm[i] = mlx4_alloc_icm(dev, MLX4_TABLE_CHUNK_SIZE >> PAGE_SHIFT,
-				       (table->lowmem ? GFP_KERNEL : GFP_HIGHUSER) |
+				       (table->lowmem ? gfp : GFP_HIGHUSER) |
 				       __GFP_NOWARN, table->coherent);
 	if (!table->icm[i]) {
 		ret = -ENOMEM;
@@ -356,7 +357,7 @@ int mlx4_table_get_range(struct mlx4_dev *dev, struct mlx4_icm_table *table,
 	u32 i;
 
 	for (i = start; i <= end; i += inc) {
-		err = mlx4_table_get(dev, table, i);
+		err = mlx4_table_get(dev, table, i, GFP_KERNEL);
 		if (err)
 			goto fail;
 	}
diff --git a/drivers/net/ethernet/mellanox/mlx4/icm.h b/drivers/net/ethernet/mellanox/mlx4/icm.h
index dee67fa3910..0c736455015 100644
--- a/drivers/net/ethernet/mellanox/mlx4/icm.h
+++ b/drivers/net/ethernet/mellanox/mlx4/icm.h
@@ -71,7 +71,8 @@ struct mlx4_icm *mlx4_alloc_icm(struct mlx4_dev *dev, int npages,
 				gfp_t gfp_mask, int coherent);
 void mlx4_free_icm(struct mlx4_dev *dev, struct mlx4_icm *icm, int coherent);
 
-int mlx4_table_get(struct mlx4_dev *dev, struct mlx4_icm_table *table, u32 obj);
+int mlx4_table_get(struct mlx4_dev *dev, struct mlx4_icm_table *table, u32 obj,
+		   gfp_t gfp);
 void mlx4_table_put(struct mlx4_dev *dev, struct mlx4_icm_table *table, u32 obj);
 int mlx4_table_get_range(struct mlx4_dev *dev, struct mlx4_icm_table *table,
 			 u32 start, u32 end);
diff --git a/drivers/net/ethernet/mellanox/mlx4/main.c b/drivers/net/ethernet/mellanox/mlx4/main.c
index 7cf9dadcb47..26169b3eaed 100644
--- a/drivers/net/ethernet/mellanox/mlx4/main.c
+++ b/drivers/net/ethernet/mellanox/mlx4/main.c
@@ -666,13 +666,15 @@ static int mlx4_slave_cap(struct mlx4_dev *dev)
 		return -ENODEV;
 	}
 
+	dev->caps.qp0_qkey = kcalloc(dev->caps.num_ports, sizeof(u32), GFP_KERNEL);
 	dev->caps.qp0_tunnel = kcalloc(dev->caps.num_ports, sizeof (u32), GFP_KERNEL);
 	dev->caps.qp0_proxy = kcalloc(dev->caps.num_ports, sizeof (u32), GFP_KERNEL);
 	dev->caps.qp1_tunnel = kcalloc(dev->caps.num_ports, sizeof (u32), GFP_KERNEL);
 	dev->caps.qp1_proxy = kcalloc(dev->caps.num_ports, sizeof (u32), GFP_KERNEL);
 
 	if (!dev->caps.qp0_tunnel || !dev->caps.qp0_proxy ||
-	    !dev->caps.qp1_tunnel || !dev->caps.qp1_proxy) {
+	    !dev->caps.qp1_tunnel || !dev->caps.qp1_proxy ||
+	    !dev->caps.qp0_qkey) {
 		err = -ENOMEM;
 		goto err_mem;
 	}
@@ -684,6 +686,7 @@ static int mlx4_slave_cap(struct mlx4_dev *dev)
 				 " port %d, aborting (%d).\n", i, err);
 			goto err_mem;
 		}
+		dev->caps.qp0_qkey[i - 1] = func_cap.qp0_qkey;
 		dev->caps.qp0_tunnel[i - 1] = func_cap.qp0_tunnel_qpn;
 		dev->caps.qp0_proxy[i - 1] = func_cap.qp0_proxy_qpn;
 		dev->caps.qp1_tunnel[i - 1] = func_cap.qp1_tunnel_qpn;
@@ -729,12 +732,16 @@ static int mlx4_slave_cap(struct mlx4_dev *dev)
 	return 0;
 
 err_mem:
+	kfree(dev->caps.qp0_qkey);
 	kfree(dev->caps.qp0_tunnel);
 	kfree(dev->caps.qp0_proxy);
 	kfree(dev->caps.qp1_tunnel);
 	kfree(dev->caps.qp1_proxy);
-	dev->caps.qp0_tunnel = dev->caps.qp0_proxy =
-		dev->caps.qp1_tunnel = dev->caps.qp1_proxy = NULL;
+	dev->caps.qp0_qkey = NULL;
+	dev->caps.qp0_tunnel = NULL;
+	dev->caps.qp0_proxy = NULL;
+	dev->caps.qp1_tunnel = NULL;
+	dev->caps.qp1_proxy = NULL;
 
 	return err;
 }
@@ -1696,6 +1703,14 @@ unmap_bf:
 	unmap_internal_clock(dev);
 	unmap_bf_area(dev);
 
+	if (mlx4_is_slave(dev)) {
+		kfree(dev->caps.qp0_qkey);
+		kfree(dev->caps.qp0_tunnel);
+		kfree(dev->caps.qp0_proxy);
+		kfree(dev->caps.qp1_tunnel);
+		kfree(dev->caps.qp1_proxy);
+	}
+
 err_close:
 	if (mlx4_is_slave(dev))
 		mlx4_slave_exit(dev);
@@ -2044,6 +2059,7 @@ static int mlx4_init_port_info(struct mlx4_dev *dev, int port)
 	if (!mlx4_is_slave(dev)) {
 		mlx4_init_mac_table(dev, &info->mac_table);
 		mlx4_init_vlan_table(dev, &info->vlan_table);
+		mlx4_init_roce_gid_table(dev, &info->gid_table);
 		info->base_qpn = mlx4_get_base_qpn(dev, port);
 	}
 
@@ -2565,6 +2581,14 @@ err_master_mfunc:
 	if (mlx4_is_master(dev))
 		mlx4_multi_func_cleanup(dev);
 
+	if (mlx4_is_slave(dev)) {
+		kfree(dev->caps.qp0_qkey);
+		kfree(dev->caps.qp0_tunnel);
+		kfree(dev->caps.qp0_proxy);
+		kfree(dev->caps.qp1_tunnel);
+		kfree(dev->caps.qp1_proxy);
+	}
+
 err_close:
 	if (dev->flags & MLX4_FLAG_MSI_X)
 		pci_disable_msix(pdev);
@@ -2688,6 +2712,7 @@ static void __mlx4_remove_one(struct pci_dev *pdev)
 	if (!mlx4_is_slave(dev))
 		mlx4_free_ownership(dev);
 
+	kfree(dev->caps.qp0_qkey);
 	kfree(dev->caps.qp0_tunnel);
 	kfree(dev->caps.qp0_proxy);
 	kfree(dev->caps.qp1_tunnel);
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4.h b/drivers/net/ethernet/mellanox/mlx4/mlx4.h
index f9c46510196..7a0665beebb 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4.h
@@ -133,6 +133,11 @@ enum {
 	MLX4_COMM_CMD_FLR = 254
 };
 
+enum {
+	MLX4_VF_SMI_DISABLED,
+	MLX4_VF_SMI_ENABLED
+};
+
 /*The flag indicates that the slave should delay the RESET cmd*/
 #define MLX4_DELAY_RESET_SLAVE 0xbbbbbbb
 /*indicates how many retries will be done if we are in the middle of FLR*/
@@ -488,6 +493,7 @@ struct mlx4_vport_state {
 
 struct mlx4_vf_admin_state {
 	struct mlx4_vport_state vport[MLX4_MAX_PORTS + 1];
+	u8 enable_smi[MLX4_MAX_PORTS + 1];
 };
 
 struct mlx4_vport_oper_state {
@@ -495,8 +501,10 @@ struct mlx4_vport_oper_state {
 	int mac_idx;
 	int vlan_idx;
 };
+
 struct mlx4_vf_oper_state {
 	struct mlx4_vport_oper_state vport[MLX4_MAX_PORTS + 1];
+	u8 smi_enabled[MLX4_MAX_PORTS + 1];
 };
 
 struct slave_list {
@@ -695,6 +703,17 @@ struct mlx4_mac_table {
 	int			max;
 };
 
+#define MLX4_ROCE_GID_ENTRY_SIZE	16
+
+struct mlx4_roce_gid_entry {
+	u8 raw[MLX4_ROCE_GID_ENTRY_SIZE];
+};
+
+struct mlx4_roce_gid_table {
+	struct mlx4_roce_gid_entry	roce_gids[MLX4_ROCE_MAX_GIDS];
+	struct mutex			mutex;
+};
+
 #define MLX4_MAX_VLAN_NUM	128
 #define MLX4_VLAN_TABLE_SIZE	(MLX4_MAX_VLAN_NUM << 2)
 
@@ -758,6 +777,7 @@ struct mlx4_port_info {
 	struct device_attribute port_mtu_attr;
 	struct mlx4_mac_table	mac_table;
 	struct mlx4_vlan_table	vlan_table;
+	struct mlx4_roce_gid_table gid_table;
 	int			base_qpn;
 };
 
@@ -788,10 +808,6 @@ enum {
 	MLX4_USE_RR	= 1,
 };
 
-struct mlx4_roce_gid_entry {
-	u8 raw[16];
-};
-
 struct mlx4_priv {
 	struct mlx4_dev		dev;
 
@@ -839,7 +855,6 @@ struct mlx4_priv {
 	int			fs_hash_mode;
 	u8 virt2phys_pkey[MLX4_MFUNC_MAX][MLX4_MAX_PORTS][MLX4_MAX_PORT_PKEYS];
 	__be64			slave_node_guids[MLX4_MFUNC_MAX];
-	struct mlx4_roce_gid_entry roce_gids[MLX4_MAX_PORTS][MLX4_ROCE_MAX_GIDS];
 
 	atomic_t		opreq_count;
 	struct work_struct	opreq_task;
@@ -888,7 +903,7 @@ void mlx4_cleanup_cq_table(struct mlx4_dev *dev);
 void mlx4_cleanup_qp_table(struct mlx4_dev *dev);
 void mlx4_cleanup_srq_table(struct mlx4_dev *dev);
 void mlx4_cleanup_mcg_table(struct mlx4_dev *dev);
-int __mlx4_qp_alloc_icm(struct mlx4_dev *dev, int qpn);
+int __mlx4_qp_alloc_icm(struct mlx4_dev *dev, int qpn, gfp_t gfp);
 void __mlx4_qp_free_icm(struct mlx4_dev *dev, int qpn);
 int __mlx4_cq_alloc_icm(struct mlx4_dev *dev, int *cqn);
 void __mlx4_cq_free_icm(struct mlx4_dev *dev, int cqn);
@@ -896,7 +911,7 @@ int __mlx4_srq_alloc_icm(struct mlx4_dev *dev, int *srqn);
 void __mlx4_srq_free_icm(struct mlx4_dev *dev, int srqn);
 int __mlx4_mpt_reserve(struct mlx4_dev *dev);
 void __mlx4_mpt_release(struct mlx4_dev *dev, u32 index);
-int __mlx4_mpt_alloc_icm(struct mlx4_dev *dev, u32 index);
+int __mlx4_mpt_alloc_icm(struct mlx4_dev *dev, u32 index, gfp_t gfp);
 void __mlx4_mpt_free_icm(struct mlx4_dev *dev, u32 index);
 u32 __mlx4_alloc_mtt_range(struct mlx4_dev *dev, int order);
 void __mlx4_free_mtt_range(struct mlx4_dev *dev, u32 first_seg, int order);
@@ -1140,6 +1155,8 @@ int mlx4_change_port_types(struct mlx4_dev *dev,
 
 void mlx4_init_mac_table(struct mlx4_dev *dev, struct mlx4_mac_table *table);
 void mlx4_init_vlan_table(struct mlx4_dev *dev, struct mlx4_vlan_table *table);
+void mlx4_init_roce_gid_table(struct mlx4_dev *dev,
+			      struct mlx4_roce_gid_table *table);
 void __mlx4_unregister_vlan(struct mlx4_dev *dev, u8 port, u16 vlan);
 int __mlx4_register_vlan(struct mlx4_dev *dev, u8 port, u16 vlan, int *index);
 
@@ -1149,6 +1166,7 @@ int mlx4_get_slave_from_resource_id(struct mlx4_dev *dev,
 				    enum mlx4_resource resource_type,
 				    u64 resource_id, int *slave);
 void mlx4_delete_all_resources_for_slave(struct mlx4_dev *dev, int slave_id);
+void mlx4_reset_roce_gids(struct mlx4_dev *dev, int slave);
 int mlx4_init_resource_tracker(struct mlx4_dev *dev);
 
 void mlx4_free_resource_tracker(struct mlx4_dev *dev,
@@ -1195,6 +1213,12 @@ int mlx4_QP_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
 			   struct mlx4_cmd_mailbox *outbox,
 			   struct mlx4_cmd_info *cmd);
 
+int mlx4_UPDATE_QP_wrapper(struct mlx4_dev *dev, int slave,
+			   struct mlx4_vhcr *vhcr,
+			   struct mlx4_cmd_mailbox *inbox,
+			   struct mlx4_cmd_mailbox *outbox,
+			   struct mlx4_cmd_info *cmd);
+
 int mlx4_PROMISC_wrapper(struct mlx4_dev *dev, int slave,
 			 struct mlx4_vhcr *vhcr,
 			 struct mlx4_cmd_mailbox *inbox,
diff --git a/drivers/net/ethernet/mellanox/mlx4/mr.c b/drivers/net/ethernet/mellanox/mlx4/mr.c
index 24835853b75..4c71dafad21 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mr.c
+++ b/drivers/net/ethernet/mellanox/mlx4/mr.c
@@ -364,14 +364,14 @@ static void mlx4_mpt_release(struct mlx4_dev *dev, u32 index)
 	__mlx4_mpt_release(dev, index);
 }
 
-int __mlx4_mpt_alloc_icm(struct mlx4_dev *dev, u32 index)
+int __mlx4_mpt_alloc_icm(struct mlx4_dev *dev, u32 index, gfp_t gfp)
 {
 	struct mlx4_mr_table *mr_table = &mlx4_priv(dev)->mr_table;
 
-	return mlx4_table_get(dev, &mr_table->dmpt_table, index);
+	return mlx4_table_get(dev, &mr_table->dmpt_table, index, gfp);
 }
 
-static int mlx4_mpt_alloc_icm(struct mlx4_dev *dev, u32 index)
+static int mlx4_mpt_alloc_icm(struct mlx4_dev *dev, u32 index, gfp_t gfp)
 {
 	u64 param = 0;
 
@@ -382,7 +382,7 @@ static int mlx4_mpt_alloc_icm(struct mlx4_dev *dev, u32 index)
 							MLX4_CMD_TIME_CLASS_A,
 							MLX4_CMD_WRAPPED);
 	}
-	return __mlx4_mpt_alloc_icm(dev, index);
+	return __mlx4_mpt_alloc_icm(dev, index, gfp);
 }
 
 void __mlx4_mpt_free_icm(struct mlx4_dev *dev, u32 index)
@@ -469,7 +469,7 @@ int mlx4_mr_enable(struct mlx4_dev *dev, struct mlx4_mr *mr)
 	struct mlx4_mpt_entry *mpt_entry;
 	int err;
 
-	err = mlx4_mpt_alloc_icm(dev, key_to_hw_index(mr->key));
+	err = mlx4_mpt_alloc_icm(dev, key_to_hw_index(mr->key), GFP_KERNEL);
 	if (err)
 		return err;
 
@@ -627,13 +627,14 @@ int mlx4_write_mtt(struct mlx4_dev *dev, struct mlx4_mtt *mtt,
 EXPORT_SYMBOL_GPL(mlx4_write_mtt);
 
 int mlx4_buf_write_mtt(struct mlx4_dev *dev, struct mlx4_mtt *mtt,
-		       struct mlx4_buf *buf)
+		       struct mlx4_buf *buf, gfp_t gfp)
 {
 	u64 *page_list;
 	int err;
 	int i;
 
-	page_list = kmalloc(buf->npages * sizeof *page_list, GFP_KERNEL);
+	page_list = kmalloc(buf->npages * sizeof *page_list,
+			    gfp);
 	if (!page_list)
 		return -ENOMEM;
 
@@ -680,7 +681,7 @@ int mlx4_mw_enable(struct mlx4_dev *dev, struct mlx4_mw *mw)
 	struct mlx4_mpt_entry *mpt_entry;
 	int err;
 
-	err = mlx4_mpt_alloc_icm(dev, key_to_hw_index(mw->key));
+	err = mlx4_mpt_alloc_icm(dev, key_to_hw_index(mw->key), GFP_KERNEL);
 	if (err)
 		return err;
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/port.c b/drivers/net/ethernet/mellanox/mlx4/port.c
index b5b3549b0c8..5ec6f203c6e 100644
--- a/drivers/net/ethernet/mellanox/mlx4/port.c
+++ b/drivers/net/ethernet/mellanox/mlx4/port.c
@@ -75,6 +75,16 @@ void mlx4_init_vlan_table(struct mlx4_dev *dev, struct mlx4_vlan_table *table)
 	table->total = 0;
 }
 
+void mlx4_init_roce_gid_table(struct mlx4_dev *dev,
+			      struct mlx4_roce_gid_table *table)
+{
+	int i;
+
+	mutex_init(&table->mutex);
+	for (i = 0; i < MLX4_ROCE_MAX_GIDS; i++)
+		memset(table->roce_gids[i].raw, 0, MLX4_ROCE_GID_ENTRY_SIZE);
+}
+
 static int validate_index(struct mlx4_dev *dev,
 			  struct mlx4_mac_table *table, int index)
 {
@@ -584,6 +594,84 @@ int mlx4_get_base_gid_ix(struct mlx4_dev *dev, int slave, int port)
 }
 EXPORT_SYMBOL_GPL(mlx4_get_base_gid_ix);
 
+static int mlx4_reset_roce_port_gids(struct mlx4_dev *dev, int slave,
+				     int port, struct mlx4_cmd_mailbox *mailbox)
+{
+	struct mlx4_roce_gid_entry *gid_entry_mbox;
+	struct mlx4_priv *priv = mlx4_priv(dev);
+	int num_gids, base, offset;
+	int i, err;
+
+	num_gids = mlx4_get_slave_num_gids(dev, slave, port);
+	base = mlx4_get_base_gid_ix(dev, slave, port);
+
+	memset(mailbox->buf, 0, MLX4_MAILBOX_SIZE);
+
+	mutex_lock(&(priv->port[port].gid_table.mutex));
+	/* Zero-out gids belonging to that slave in the port GID table */
+	for (i = 0, offset = base; i < num_gids; offset++, i++)
+		memcpy(priv->port[port].gid_table.roce_gids[offset].raw,
+		       zgid_entry.raw, MLX4_ROCE_GID_ENTRY_SIZE);
+
+	/* Now, copy roce port gids table to mailbox for passing to FW */
+	gid_entry_mbox = (struct mlx4_roce_gid_entry *)mailbox->buf;
+	for (i = 0; i < MLX4_ROCE_MAX_GIDS; gid_entry_mbox++, i++)
+		memcpy(gid_entry_mbox->raw,
+		       priv->port[port].gid_table.roce_gids[i].raw,
+		       MLX4_ROCE_GID_ENTRY_SIZE);
+
+	err = mlx4_cmd(dev, mailbox->dma,
+		       ((u32)port) | (MLX4_SET_PORT_GID_TABLE << 8), 1,
+		       MLX4_CMD_SET_PORT, MLX4_CMD_TIME_CLASS_B,
+		       MLX4_CMD_NATIVE);
+	mutex_unlock(&(priv->port[port].gid_table.mutex));
+	return err;
+}
+
+
+void mlx4_reset_roce_gids(struct mlx4_dev *dev, int slave)
+{
+	struct mlx4_active_ports actv_ports;
+	struct mlx4_cmd_mailbox *mailbox;
+	int num_eth_ports, err;
+	int i;
+
+	if (slave < 0 || slave > dev->num_vfs)
+		return;
+
+	actv_ports = mlx4_get_active_ports(dev, slave);
+
+	for (i = 0, num_eth_ports = 0; i < dev->caps.num_ports; i++) {
+		if (test_bit(i, actv_ports.ports)) {
+			if (dev->caps.port_type[i + 1] != MLX4_PORT_TYPE_ETH)
+				continue;
+			num_eth_ports++;
+		}
+	}
+
+	if (!num_eth_ports)
+		return;
+
+	/* have ETH ports.  Alloc mailbox for SET_PORT command */
+	mailbox = mlx4_alloc_cmd_mailbox(dev);
+	if (IS_ERR(mailbox))
+		return;
+
+	for (i = 0; i < dev->caps.num_ports; i++) {
+		if (test_bit(i, actv_ports.ports)) {
+			if (dev->caps.port_type[i + 1] != MLX4_PORT_TYPE_ETH)
+				continue;
+			err = mlx4_reset_roce_port_gids(dev, slave, i + 1, mailbox);
+			if (err)
+				mlx4_warn(dev, "Could not reset ETH port GID table for slave %d, port %d (%d)\n",
+					  slave, i + 1, err);
+		}
+	}
+
+	mlx4_free_cmd_mailbox(dev, mailbox);
+	return;
+}
+
 static int mlx4_common_set_port(struct mlx4_dev *dev, int slave, u32 in_mod,
 				u8 op_mod, struct mlx4_cmd_mailbox *inbox)
 {
@@ -692,10 +780,12 @@ static int mlx4_common_set_port(struct mlx4_dev *dev, int slave, u32 in_mod,
 			/* 2. Check that do not have duplicates in OTHER
 			 *    entries in the port GID table
 			 */
+
+			mutex_lock(&(priv->port[port].gid_table.mutex));
 			for (i = 0; i < MLX4_ROCE_MAX_GIDS; i++) {
 				if (i >= base && i < base + num_gids)
 					continue; /* don't compare to slave's current gids */
-				gid_entry_tbl = &priv->roce_gids[port - 1][i];
+				gid_entry_tbl = &priv->port[port].gid_table.roce_gids[i];
 				if (!memcmp(gid_entry_tbl->raw, zgid_entry.raw, sizeof(zgid_entry)))
 					continue;
 				gid_entry_mbox = (struct mlx4_roce_gid_entry *)(inbox->buf);
@@ -709,6 +799,7 @@ static int mlx4_common_set_port(struct mlx4_dev *dev, int slave, u32 in_mod,
 						mlx4_warn(dev, "requested gid entry for slave:%d "
 							  "is a duplicate of gid at index %d\n",
 							  slave, i);
+						mutex_unlock(&(priv->port[port].gid_table.mutex));
 						return -EINVAL;
 					}
 				}
@@ -717,16 +808,24 @@ static int mlx4_common_set_port(struct mlx4_dev *dev, int slave, u32 in_mod,
 			/* insert slave GIDs with memcpy, starting at slave's base index */
 			gid_entry_mbox = (struct mlx4_roce_gid_entry *)(inbox->buf);
 			for (i = 0, offset = base; i < num_gids; gid_entry_mbox++, offset++, i++)
-				memcpy(priv->roce_gids[port - 1][offset].raw, gid_entry_mbox->raw, 16);
+				memcpy(priv->port[port].gid_table.roce_gids[offset].raw,
+				       gid_entry_mbox->raw, MLX4_ROCE_GID_ENTRY_SIZE);
 
 			/* Now, copy roce port gids table to current mailbox for passing to FW */
 			gid_entry_mbox = (struct mlx4_roce_gid_entry *)(inbox->buf);
 			for (i = 0; i < MLX4_ROCE_MAX_GIDS; gid_entry_mbox++, i++)
-				memcpy(gid_entry_mbox->raw, priv->roce_gids[port - 1][i].raw, 16);
-
-			break;
+				memcpy(gid_entry_mbox->raw,
+				       priv->port[port].gid_table.roce_gids[i].raw,
+				       MLX4_ROCE_GID_ENTRY_SIZE);
+
+			err = mlx4_cmd(dev, inbox->dma, in_mod & 0xffff, op_mod,
+				       MLX4_CMD_SET_PORT, MLX4_CMD_TIME_CLASS_B,
+				       MLX4_CMD_NATIVE);
+			mutex_unlock(&(priv->port[port].gid_table.mutex));
+			return err;
 		}
-		return mlx4_cmd(dev, inbox->dma, in_mod, op_mod,
+
+		return mlx4_cmd(dev, inbox->dma, in_mod & 0xffff, op_mod,
 				MLX4_CMD_SET_PORT, MLX4_CMD_TIME_CLASS_B,
 				MLX4_CMD_NATIVE);
 	}
@@ -1099,7 +1198,8 @@ int mlx4_get_slave_from_roce_gid(struct mlx4_dev *dev, int port, u8 *gid,
 	num_vfs = bitmap_weight(slaves_pport.slaves, dev->num_vfs + 1) - 1;
 
 	for (i = 0; i < MLX4_ROCE_MAX_GIDS; i++) {
-		if (!memcmp(priv->roce_gids[port - 1][i].raw, gid, 16)) {
+		if (!memcmp(priv->port[port].gid_table.roce_gids[i].raw, gid,
+			    MLX4_ROCE_GID_ENTRY_SIZE)) {
 			found_ix = i;
 			break;
 		}
@@ -1187,7 +1287,8 @@ int mlx4_get_roce_gid_from_slave(struct mlx4_dev *dev, int port, int slave_id,
 	if (!mlx4_is_master(dev))
 		return -EINVAL;
 
-	memcpy(gid, priv->roce_gids[port - 1][slave_id].raw, 16);
+	memcpy(gid, priv->port[port].gid_table.roce_gids[slave_id].raw,
+	       MLX4_ROCE_GID_ENTRY_SIZE);
 	return 0;
 }
 EXPORT_SYMBOL(mlx4_get_roce_gid_from_slave);
diff --git a/drivers/net/ethernet/mellanox/mlx4/qp.c b/drivers/net/ethernet/mellanox/mlx4/qp.c
index 61d64ebffd5..40af6194792 100644
--- a/drivers/net/ethernet/mellanox/mlx4/qp.c
+++ b/drivers/net/ethernet/mellanox/mlx4/qp.c
@@ -272,29 +272,29 @@ void mlx4_qp_release_range(struct mlx4_dev *dev, int base_qpn, int cnt)
 }
 EXPORT_SYMBOL_GPL(mlx4_qp_release_range);
 
-int __mlx4_qp_alloc_icm(struct mlx4_dev *dev, int qpn)
+int __mlx4_qp_alloc_icm(struct mlx4_dev *dev, int qpn, gfp_t gfp)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
 	struct mlx4_qp_table *qp_table = &priv->qp_table;
 	int err;
 
-	err = mlx4_table_get(dev, &qp_table->qp_table, qpn);
+	err = mlx4_table_get(dev, &qp_table->qp_table, qpn, gfp);
 	if (err)
 		goto err_out;
 
-	err = mlx4_table_get(dev, &qp_table->auxc_table, qpn);
+	err = mlx4_table_get(dev, &qp_table->auxc_table, qpn, gfp);
 	if (err)
 		goto err_put_qp;
 
-	err = mlx4_table_get(dev, &qp_table->altc_table, qpn);
+	err = mlx4_table_get(dev, &qp_table->altc_table, qpn, gfp);
 	if (err)
 		goto err_put_auxc;
 
-	err = mlx4_table_get(dev, &qp_table->rdmarc_table, qpn);
+	err = mlx4_table_get(dev, &qp_table->rdmarc_table, qpn, gfp);
 	if (err)
 		goto err_put_altc;
 
-	err = mlx4_table_get(dev, &qp_table->cmpt_table, qpn);
+	err = mlx4_table_get(dev, &qp_table->cmpt_table, qpn, gfp);
 	if (err)
 		goto err_put_rdmarc;
 
@@ -316,7 +316,7 @@ err_out:
 	return err;
 }
 
-static int mlx4_qp_alloc_icm(struct mlx4_dev *dev, int qpn)
+static int mlx4_qp_alloc_icm(struct mlx4_dev *dev, int qpn, gfp_t gfp)
 {
 	u64 param = 0;
 
@@ -326,7 +326,7 @@ static int mlx4_qp_alloc_icm(struct mlx4_dev *dev, int qpn)
 				    MLX4_CMD_ALLOC_RES, MLX4_CMD_TIME_CLASS_A,
 				    MLX4_CMD_WRAPPED);
 	}
-	return __mlx4_qp_alloc_icm(dev, qpn);
+	return __mlx4_qp_alloc_icm(dev, qpn, gfp);
 }
 
 void __mlx4_qp_free_icm(struct mlx4_dev *dev, int qpn)
@@ -355,7 +355,7 @@ static void mlx4_qp_free_icm(struct mlx4_dev *dev, int qpn)
 		__mlx4_qp_free_icm(dev, qpn);
 }
 
-int mlx4_qp_alloc(struct mlx4_dev *dev, int qpn, struct mlx4_qp *qp)
+int mlx4_qp_alloc(struct mlx4_dev *dev, int qpn, struct mlx4_qp *qp, gfp_t gfp)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
 	struct mlx4_qp_table *qp_table = &priv->qp_table;
@@ -366,7 +366,7 @@ int mlx4_qp_alloc(struct mlx4_dev *dev, int qpn, struct mlx4_qp *qp)
 
 	qp->qpn = qpn;
 
-	err = mlx4_qp_alloc_icm(dev, qpn);
+	err = mlx4_qp_alloc_icm(dev, qpn, gfp);
 	if (err)
 		return err;
 
@@ -389,6 +389,41 @@ 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,
+		   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;
+	int err = 0;
+
+	mailbox = mlx4_alloc_cmd_mailbox(dev);
+	if (IS_ERR(mailbox))
+		return PTR_ERR(mailbox);
+
+	cmd = (struct mlx4_update_qp_context *)mailbox->buf;
+
+	if (!attr || (attr & ~MLX4_UPDATE_QP_SUPPORTED_ATTRS))
+		return -EINVAL;
+
+	if (attr & MLX4_UPDATE_QP_SMAC) {
+		pri_addr_path_mask |= 1ULL << MLX4_UPD_QP_PATH_MASK_MAC_INDEX;
+		cmd->qp_context.pri_path.grh_mylmc = params->smac_index;
+	}
+
+	cmd->primary_addr_path_mask = cpu_to_be64(pri_addr_path_mask);
+
+	err = mlx4_cmd(dev, mailbox->dma, qp->qpn & 0xffffff, 0,
+		       MLX4_CMD_UPDATE_QP, MLX4_CMD_TIME_CLASS_A,
+		       MLX4_CMD_NATIVE);
+
+	mlx4_free_cmd_mailbox(dev, mailbox);
+	return err;
+}
+EXPORT_SYMBOL_GPL(mlx4_update_qp);
+
 void mlx4_qp_remove(struct mlx4_dev *dev, struct mlx4_qp *qp)
 {
 	struct mlx4_qp_table *qp_table = &mlx4_priv(dev)->qp_table;
diff --git a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
index 1c3fdd4a1f7..2ba3b762396 100644
--- a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
+++ b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
@@ -586,6 +586,7 @@ void mlx4_free_resource_tracker(struct mlx4_dev *dev,
 			}
 			/* free master's vlans */
 			i = dev->caps.function;
+			mlx4_reset_roce_gids(dev, i);
 			mutex_lock(&priv->mfunc.master.res_tracker.slave_list[i].mutex);
 			rem_slave_vlans(dev, i);
 			mutex_unlock(&priv->mfunc.master.res_tracker.slave_list[i].mutex);
@@ -1532,7 +1533,7 @@ static int qp_alloc_res(struct mlx4_dev *dev, int slave, int op, int cmd,
 			return err;
 
 		if (!fw_reserved(dev, qpn)) {
-			err = __mlx4_qp_alloc_icm(dev, qpn);
+			err = __mlx4_qp_alloc_icm(dev, qpn, GFP_KERNEL);
 			if (err) {
 				res_abort_move(dev, slave, RES_QP, qpn);
 				return err;
@@ -1619,7 +1620,7 @@ static int mpt_alloc_res(struct mlx4_dev *dev, int slave, int op, int cmd,
 		if (err)
 			return err;
 
-		err = __mlx4_mpt_alloc_icm(dev, mpt->key);
+		err = __mlx4_mpt_alloc_icm(dev, mpt->key, GFP_KERNEL);
 		if (err) {
 			res_abort_move(dev, slave, RES_MPT, id);
 			return err;
@@ -2827,10 +2828,12 @@ static int get_containing_mtt(struct mlx4_dev *dev, int slave, int start,
 }
 
 static int verify_qp_parameters(struct mlx4_dev *dev,
+				struct mlx4_vhcr *vhcr,
 				struct mlx4_cmd_mailbox *inbox,
 				enum qp_transition transition, u8 slave)
 {
 	u32			qp_type;
+	u32			qpn;
 	struct mlx4_qp_context	*qp_ctx;
 	enum mlx4_qp_optpar	optpar;
 	int port;
@@ -2873,8 +2876,22 @@ static int verify_qp_parameters(struct mlx4_dev *dev,
 		default:
 			break;
 		}
+		break;
 
+	case MLX4_QP_ST_MLX:
+		qpn = vhcr->in_modifier & 0x7fffff;
+		port = (qp_ctx->pri_path.sched_queue >> 6 & 1) + 1;
+		if (transition == QP_TRANS_INIT2RTR &&
+		    slave != mlx4_master_func_num(dev) &&
+		    mlx4_is_qp_reserved(dev, qpn) &&
+		    !mlx4_vf_smi_enabled(dev, slave, port)) {
+			/* only enabled VFs may create MLX proxy QPs */
+			mlx4_err(dev, "%s: unprivileged slave %d attempting to create an MLX proxy special QP on port %d\n",
+				 __func__, slave, port);
+			return -EPERM;
+		}
 		break;
+
 	default:
 		break;
 	}
@@ -3454,7 +3471,7 @@ int mlx4_INIT2RTR_QP_wrapper(struct mlx4_dev *dev, int slave,
 	err = adjust_qp_sched_queue(dev, slave, qpc, inbox);
 	if (err)
 		return err;
-	err = verify_qp_parameters(dev, inbox, QP_TRANS_INIT2RTR, slave);
+	err = verify_qp_parameters(dev, vhcr, inbox, QP_TRANS_INIT2RTR, slave);
 	if (err)
 		return err;
 
@@ -3508,7 +3525,7 @@ int mlx4_RTR2RTS_QP_wrapper(struct mlx4_dev *dev, int slave,
 	err = adjust_qp_sched_queue(dev, slave, context, inbox);
 	if (err)
 		return err;
-	err = verify_qp_parameters(dev, inbox, QP_TRANS_RTR2RTS, slave);
+	err = verify_qp_parameters(dev, vhcr, inbox, QP_TRANS_RTR2RTS, slave);
 	if (err)
 		return err;
 
@@ -3530,7 +3547,7 @@ int mlx4_RTS2RTS_QP_wrapper(struct mlx4_dev *dev, int slave,
 	err = adjust_qp_sched_queue(dev, slave, context, inbox);
 	if (err)
 		return err;
-	err = verify_qp_parameters(dev, inbox, QP_TRANS_RTS2RTS, slave);
+	err = verify_qp_parameters(dev, vhcr, inbox, QP_TRANS_RTS2RTS, slave);
 	if (err)
 		return err;
 
@@ -3567,7 +3584,7 @@ int mlx4_SQD2SQD_QP_wrapper(struct mlx4_dev *dev, int slave,
 	err = adjust_qp_sched_queue(dev, slave, context, inbox);
 	if (err)
 		return err;
-	err = verify_qp_parameters(dev, inbox, QP_TRANS_SQD2SQD, slave);
+	err = verify_qp_parameters(dev, vhcr, inbox, QP_TRANS_SQD2SQD, slave);
 	if (err)
 		return err;
 
@@ -3589,7 +3606,7 @@ int mlx4_SQD2RTS_QP_wrapper(struct mlx4_dev *dev, int slave,
 	err = adjust_qp_sched_queue(dev, slave, context, inbox);
 	if (err)
 		return err;
-	err = verify_qp_parameters(dev, inbox, QP_TRANS_SQD2RTS, slave);
+	err = verify_qp_parameters(dev, vhcr, inbox, QP_TRANS_SQD2RTS, slave);
 	if (err)
 		return err;
 
@@ -3895,6 +3912,60 @@ static int add_eth_header(struct mlx4_dev *dev, int slave,
 
 }
 
+#define MLX4_UPD_QP_PATH_MASK_SUPPORTED (1ULL << MLX4_UPD_QP_PATH_MASK_MAC_INDEX)
+int mlx4_UPDATE_QP_wrapper(struct mlx4_dev *dev, int slave,
+			   struct mlx4_vhcr *vhcr,
+			   struct mlx4_cmd_mailbox *inbox,
+			   struct mlx4_cmd_mailbox *outbox,
+			   struct mlx4_cmd_info *cmd_info)
+{
+	int err;
+	u32 qpn = vhcr->in_modifier & 0xffffff;
+	struct res_qp *rqp;
+	u64 mac;
+	unsigned port;
+	u64 pri_addr_path_mask;
+	struct mlx4_update_qp_context *cmd;
+	int smac_index;
+
+	cmd = (struct mlx4_update_qp_context *)inbox->buf;
+
+	pri_addr_path_mask = be64_to_cpu(cmd->primary_addr_path_mask);
+	if (cmd->qp_mask || cmd->secondary_addr_path_mask ||
+	    (pri_addr_path_mask & ~MLX4_UPD_QP_PATH_MASK_SUPPORTED))
+		return -EPERM;
+
+	/* Just change the smac for the QP */
+	err = get_res(dev, slave, qpn, RES_QP, &rqp);
+	if (err) {
+		mlx4_err(dev, "Updating qpn 0x%x for slave %d rejected\n", qpn, slave);
+		return err;
+	}
+
+	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;
+	}
+
+	err = mlx4_cmd(dev, inbox->dma,
+		       vhcr->in_modifier, 0,
+		       MLX4_CMD_UPDATE_QP, MLX4_CMD_TIME_CLASS_A,
+		       MLX4_CMD_NATIVE);
+	if (err) {
+		mlx4_err(dev, "Failed to update qpn on qpn 0x%x, command failed\n", qpn);
+		goto err_mac;
+	}
+
+err_mac:
+	put_res(dev, slave, qpn, RES_QP);
+	return err;
+}
+
 int mlx4_QP_FLOW_STEERING_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
 					 struct mlx4_vhcr *vhcr,
 					 struct mlx4_cmd_mailbox *inbox,
@@ -4627,7 +4698,7 @@ static void rem_slave_xrcdns(struct mlx4_dev *dev, int slave)
 void mlx4_delete_all_resources_for_slave(struct mlx4_dev *dev, int slave)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
-
+	mlx4_reset_roce_gids(dev, slave);
 	mutex_lock(&priv->mfunc.master.res_tracker.slave_list[slave].mutex);
 	rem_slave_vlans(dev, slave);
 	rem_slave_macs(dev, slave);
diff --git a/drivers/net/ethernet/mellanox/mlx4/srq.c b/drivers/net/ethernet/mellanox/mlx4/srq.c
index 98faf870b0b..67146624eb5 100644
--- a/drivers/net/ethernet/mellanox/mlx4/srq.c
+++ b/drivers/net/ethernet/mellanox/mlx4/srq.c
@@ -103,11 +103,11 @@ int __mlx4_srq_alloc_icm(struct mlx4_dev *dev, int *srqn)
 	if (*srqn == -1)
 		return -ENOMEM;
 
-	err = mlx4_table_get(dev, &srq_table->table, *srqn);
+	err = mlx4_table_get(dev, &srq_table->table, *srqn, GFP_KERNEL);
 	if (err)
 		goto err_out;
 
-	err = mlx4_table_get(dev, &srq_table->cmpt_table, *srqn);
+	err = mlx4_table_get(dev, &srq_table->cmpt_table, *srqn, GFP_KERNEL);
 	if (err)
 		goto err_put;
 	return 0;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/mr.c b/drivers/net/ethernet/mellanox/mlx5/core/mr.c
index 4cc92764940..ac52a0fe2d3 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/mr.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/mr.c
@@ -82,7 +82,11 @@ int mlx5_core_create_mkey(struct mlx5_core_dev *dev, struct mlx5_core_mr *mr,
 		return mlx5_cmd_status_to_err(&lout.hdr);
 	}
 
+	mr->iova = be64_to_cpu(in->seg.start_addr);
+	mr->size = be64_to_cpu(in->seg.len);
 	mr->key = mlx5_idx_to_mkey(be32_to_cpu(lout.mkey) & 0xffffff) | key;
+	mr->pd = be32_to_cpu(in->seg.flags_pd) & 0xffffff;
+
 	mlx5_core_dbg(dev, "out 0x%x, key 0x%x, mkey 0x%x\n",
 		      be32_to_cpu(lout.mkey), key, mr->key);
 
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h b/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
index 7b52a88923e..f785d01c7d1 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
@@ -1719,22 +1719,6 @@ static inline u32 qlcnic_tx_avail(struct qlcnic_host_tx_ring *tx_ring)
 				tx_ring->producer;
 }
 
-static inline int qlcnic_set_real_num_queues(struct qlcnic_adapter *adapter,
-					     struct net_device *netdev)
-{
-	int err;
-
-	netdev->num_tx_queues = adapter->drv_tx_rings;
-	netdev->real_num_tx_queues = adapter->drv_tx_rings;
-
-	err = netif_set_real_num_tx_queues(netdev, adapter->drv_tx_rings);
-	if (err)
-		netdev_err(netdev, "failed to set %d Tx queues\n",
-			   adapter->drv_tx_rings);
-
-	return err;
-}
-
 struct qlcnic_nic_template {
 	int (*config_bridged_mode) (struct qlcnic_adapter *, u32);
 	int (*config_led) (struct qlcnic_adapter *, u32, u32);
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_dcb.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_dcb.c
index a51fe18f09a..561cb11ca58 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_dcb.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_dcb.c
@@ -1020,6 +1020,7 @@ static int qlcnic_dcb_peer_app_info(struct net_device *netdev,
 	struct qlcnic_dcb_cee *peer;
 	int i;
 
+	memset(info, 0, sizeof(*info));
 	*app_count = 0;
 
 	if (!test_bit(QLCNIC_DCB_STATE, &adapter->dcb->state))
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
index 0bc914859e3..7e55e88a81b 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
@@ -2206,6 +2206,31 @@ static void qlcnic_82xx_set_mac_filter_count(struct qlcnic_adapter *adapter)
 	ahw->max_uc_count = count;
 }
 
+static int qlcnic_set_real_num_queues(struct qlcnic_adapter *adapter,
+				      u8 tx_queues, u8 rx_queues)
+{
+	struct net_device *netdev = adapter->netdev;
+	int err = 0;
+
+	if (tx_queues) {
+		err = netif_set_real_num_tx_queues(netdev, tx_queues);
+		if (err) {
+			netdev_err(netdev, "failed to set %d Tx queues\n",
+				   tx_queues);
+			return err;
+		}
+	}
+
+	if (rx_queues) {
+		err = netif_set_real_num_rx_queues(netdev, rx_queues);
+		if (err)
+			netdev_err(netdev, "failed to set %d Rx queues\n",
+				   rx_queues);
+	}
+
+	return err;
+}
+
 int
 qlcnic_setup_netdev(struct qlcnic_adapter *adapter, struct net_device *netdev,
 		    int pci_using_dac)
@@ -2269,7 +2294,8 @@ qlcnic_setup_netdev(struct qlcnic_adapter *adapter, struct net_device *netdev,
 	netdev->priv_flags |= IFF_UNICAST_FLT;
 	netdev->irq = adapter->msix_entries[0].vector;
 
-	err = qlcnic_set_real_num_queues(adapter, netdev);
+	err = qlcnic_set_real_num_queues(adapter, adapter->drv_tx_rings,
+					 adapter->drv_sds_rings);
 	if (err)
 		return err;
 
@@ -2943,9 +2969,13 @@ static void qlcnic_dump_tx_rings(struct qlcnic_adapter *adapter)
 			    tx_ring->tx_stats.xmit_called,
 			    tx_ring->tx_stats.xmit_on,
 			    tx_ring->tx_stats.xmit_off);
+
+		if (tx_ring->crb_intr_mask)
+			netdev_info(netdev, "crb_intr_mask=%d\n",
+				    readl(tx_ring->crb_intr_mask));
+
 		netdev_info(netdev,
-			    "crb_intr_mask=%d, hw_producer=%d, sw_producer=%d sw_consumer=%d, hw_consumer=%d\n",
-			    readl(tx_ring->crb_intr_mask),
+			    "hw_producer=%d, sw_producer=%d sw_consumer=%d, hw_consumer=%d\n",
 			    readl(tx_ring->crb_cmd_producer),
 			    tx_ring->producer, tx_ring->sw_consumer,
 			    le32_to_cpu(*(tx_ring->hw_consumer)));
@@ -3978,12 +4008,21 @@ int qlcnic_validate_rings(struct qlcnic_adapter *adapter, __u32 ring_cnt,
 int qlcnic_setup_rings(struct qlcnic_adapter *adapter)
 {
 	struct net_device *netdev = adapter->netdev;
+	u8 tx_rings, rx_rings;
 	int err;
 
 	if (test_bit(__QLCNIC_RESETTING, &adapter->state))
 		return -EBUSY;
 
+	tx_rings = adapter->drv_tss_rings;
+	rx_rings = adapter->drv_rss_rings;
+
 	netif_device_detach(netdev);
+
+	err = qlcnic_set_real_num_queues(adapter, tx_rings, rx_rings);
+	if (err)
+		goto done;
+
 	if (netif_running(netdev))
 		__qlcnic_down(adapter, netdev);
 
@@ -4003,7 +4042,17 @@ int qlcnic_setup_rings(struct qlcnic_adapter *adapter)
 		return err;
 	}
 
-	netif_set_real_num_tx_queues(netdev, adapter->drv_tx_rings);
+	/* Check if we need to update real_num_{tx|rx}_queues because
+	 * qlcnic_setup_intr() may change Tx/Rx rings size
+	 */
+	if ((tx_rings != adapter->drv_tx_rings) ||
+	    (rx_rings != adapter->drv_sds_rings)) {
+		err = qlcnic_set_real_num_queues(adapter,
+						 adapter->drv_tx_rings,
+						 adapter->drv_sds_rings);
+		if (err)
+			goto done;
+	}
 
 	if (qlcnic_83xx_check(adapter)) {
 		qlcnic_83xx_initialize_nic(adapter, 1);
diff --git a/drivers/net/ethernet/sfc/nic.c b/drivers/net/ethernet/sfc/nic.c
index 32d969e857f..89b83e59e1d 100644
--- a/drivers/net/ethernet/sfc/nic.c
+++ b/drivers/net/ethernet/sfc/nic.c
@@ -156,13 +156,15 @@ void efx_nic_fini_interrupt(struct efx_nic *efx)
 	efx->net_dev->rx_cpu_rmap = NULL;
 #endif
 
-	/* Disable MSI/MSI-X interrupts */
-	efx_for_each_channel(channel, efx)
-		free_irq(channel->irq, &efx->msi_context[channel->channel]);
-
-	/* Disable legacy interrupt */
-	if (efx->legacy_irq)
+	if (EFX_INT_MODE_USE_MSI(efx)) {
+		/* Disable MSI/MSI-X interrupts */
+		efx_for_each_channel(channel, efx)
+			free_irq(channel->irq,
+				 &efx->msi_context[channel->channel]);
+	} else {
+		/* Disable legacy interrupt */
 		free_irq(efx->legacy_irq, efx);
+	}
 }
 
 /* Register dump */
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index d940034acdd..0f4841d2e8d 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -1704,7 +1704,7 @@ static int stmmac_open(struct net_device *dev)
 		if (ret) {
 			pr_err("%s: Cannot attach to PHY (error: %d)\n",
 			       __func__, ret);
-			goto phy_error;
+			return ret;
 		}
 	}
 
@@ -1779,8 +1779,6 @@ init_error:
 dma_desc_error:
 	if (priv->phydev)
 		phy_disconnect(priv->phydev);
-phy_error:
-	clk_disable_unprepare(priv->stmmac_clk);
 
 	return ret;
 }
diff --git a/drivers/net/ethernet/sun/cassini.c b/drivers/net/ethernet/sun/cassini.c
index df8d383acf4..b9ac20f4265 100644
--- a/drivers/net/ethernet/sun/cassini.c
+++ b/drivers/net/ethernet/sun/cassini.c
@@ -246,7 +246,7 @@ static inline void cas_lock_tx(struct cas *cp)
 	int i;
 
 	for (i = 0; i < N_TX_RINGS; i++)
-		spin_lock(&cp->tx_lock[i]);
+		spin_lock_nested(&cp->tx_lock[i], i);
 }
 
 static inline void cas_lock_all(struct cas *cp)
diff --git a/drivers/net/ethernet/ti/cpsw.c b/drivers/net/ethernet/ti/cpsw.c
index 36aa109416c..c331b7ebc81 100644
--- a/drivers/net/ethernet/ti/cpsw.c
+++ b/drivers/net/ethernet/ti/cpsw.c
@@ -1871,18 +1871,13 @@ static int cpsw_probe_dt(struct cpsw_platform_data *data,
 		mdio_node = of_find_node_by_phandle(be32_to_cpup(parp));
 		phyid = be32_to_cpup(parp+1);
 		mdio = of_find_device_by_node(mdio_node);
-
-		if (strncmp(mdio->name, "gpio", 4) == 0) {
-			/* GPIO bitbang MDIO driver attached */
-			struct mii_bus *bus = dev_get_drvdata(&mdio->dev);
-
-			snprintf(slave_data->phy_id, sizeof(slave_data->phy_id),
-				 PHY_ID_FMT, bus->id, phyid);
-		} else {
-			/* davinci MDIO driver attached */
-			snprintf(slave_data->phy_id, sizeof(slave_data->phy_id),
-				 PHY_ID_FMT, mdio->name, phyid);
+		of_node_put(mdio_node);
+		if (!mdio) {
+			pr_err("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)
diff --git a/drivers/net/ethernet/tile/tilegx.c b/drivers/net/ethernet/tile/tilegx.c
index 7e1c91d41a8..449011b0e00 100644
--- a/drivers/net/ethernet/tile/tilegx.c
+++ b/drivers/net/ethernet/tile/tilegx.c
@@ -1208,8 +1208,8 @@ static int tile_net_setup_interrupts(struct net_device *dev)
 
 	irq = md->ingress_irq;
 	if (irq < 0) {
-		irq = create_irq();
-		if (irq < 0) {
+		irq = irq_alloc_hwirq(-1);
+		if (!irq) {
 			netdev_err(dev,
 				   "create_irq failed: mpipe[%d] %d\n",
 				   instance, irq);
@@ -1223,7 +1223,7 @@ static int tile_net_setup_interrupts(struct net_device *dev)
 		if (rc != 0) {
 			netdev_err(dev, "request_irq failed: mpipe[%d] %d\n",
 				   instance, rc);
-			destroy_irq(irq);
+			irq_free_hwirq(irq);
 			return rc;
 		}
 		md->ingress_irq = irq;
diff --git a/drivers/net/irda/sh_sir.c b/drivers/net/irda/sh_sir.c
index cadf52e2246..e3fe9a28613 100644
--- a/drivers/net/irda/sh_sir.c
+++ b/drivers/net/irda/sh_sir.c
@@ -217,21 +217,17 @@ crc_init_out:
 static u32 sh_sir_find_sclk(struct clk *irda_clk)
 {
 	struct cpufreq_frequency_table *freq_table = irda_clk->freq_table;
+	struct cpufreq_frequency_table *pos;
 	struct clk *pclk = clk_get(NULL, "peripheral_clk");
 	u32 limit, min = 0xffffffff, tmp;
-	int i, index = 0;
+	int index = 0;
 
 	limit = clk_get_rate(pclk);
 	clk_put(pclk);
 
 	/* IrDA can not set over peripheral_clk */
-	for (i = 0;
-	     freq_table[i].frequency != CPUFREQ_TABLE_END;
-	     i++) {
-		u32 freq = freq_table[i].frequency;
-
-		if (freq == CPUFREQ_ENTRY_INVALID)
-			continue;
+	cpufreq_for_each_valid_entry(pos, freq_table) {
+		u32 freq = pos->frequency;
 
 		/* IrDA should not over peripheral_clk */
 		if (freq > limit)
@@ -240,7 +236,7 @@ static u32 sh_sir_find_sclk(struct clk *irda_clk)
 		tmp = freq % SCLK_BASE;
 		if (tmp < min) {
 			min = tmp;
-			index = i;
+			index = pos - freq_table;
 		}
 	}
 
diff --git a/drivers/net/macvlan.c b/drivers/net/macvlan.c
index b0e2865a681..d53e299ae1d 100644
--- a/drivers/net/macvlan.c
+++ b/drivers/net/macvlan.c
@@ -458,8 +458,10 @@ static void macvlan_change_rx_flags(struct net_device *dev, int change)
 	struct macvlan_dev *vlan = netdev_priv(dev);
 	struct net_device *lowerdev = vlan->lowerdev;
 
-	if (change & IFF_ALLMULTI)
-		dev_set_allmulti(lowerdev, dev->flags & IFF_ALLMULTI ? 1 : -1);
+	if (dev->flags & IFF_UP) {
+		if (change & IFF_ALLMULTI)
+			dev_set_allmulti(lowerdev, dev->flags & IFF_ALLMULTI ? 1 : -1);
+	}
 }
 
 static void macvlan_set_mac_lists(struct net_device *dev)
@@ -515,6 +517,11 @@ static struct lock_class_key macvlan_netdev_addr_lock_key;
 #define MACVLAN_STATE_MASK \
 	((1<<__LINK_STATE_NOCARRIER) | (1<<__LINK_STATE_DORMANT))
 
+static int macvlan_get_nest_level(struct net_device *dev)
+{
+	return ((struct macvlan_dev *)netdev_priv(dev))->nest_level;
+}
+
 static void macvlan_set_lockdep_class_one(struct net_device *dev,
 					  struct netdev_queue *txq,
 					  void *_unused)
@@ -525,8 +532,9 @@ static void macvlan_set_lockdep_class_one(struct net_device *dev,
 
 static void macvlan_set_lockdep_class(struct net_device *dev)
 {
-	lockdep_set_class(&dev->addr_list_lock,
-			  &macvlan_netdev_addr_lock_key);
+	lockdep_set_class_and_subclass(&dev->addr_list_lock,
+				       &macvlan_netdev_addr_lock_key,
+				       macvlan_get_nest_level(dev));
 	netdev_for_each_tx_queue(dev, macvlan_set_lockdep_class_one, NULL);
 }
 
@@ -721,6 +729,7 @@ static const struct net_device_ops macvlan_netdev_ops = {
 	.ndo_fdb_add		= macvlan_fdb_add,
 	.ndo_fdb_del		= macvlan_fdb_del,
 	.ndo_fdb_dump		= ndo_dflt_fdb_dump,
+	.ndo_get_lock_subclass  = macvlan_get_nest_level,
 };
 
 void macvlan_common_setup(struct net_device *dev)
@@ -849,6 +858,7 @@ int macvlan_common_newlink(struct net *src_net, struct net_device *dev,
 	vlan->dev      = dev;
 	vlan->port     = port;
 	vlan->set_features = MACVLAN_FEATURES;
+	vlan->nest_level = dev_get_nest_level(lowerdev, netif_is_macvlan) + 1;
 
 	vlan->mode     = MACVLAN_MODE_VEPA;
 	if (data && data[IFLA_MACVLAN_MODE])
diff --git a/drivers/net/phy/mdio-gpio.c b/drivers/net/phy/mdio-gpio.c
index 9c4defdec67..5f1a2250018 100644
--- a/drivers/net/phy/mdio-gpio.c
+++ b/drivers/net/phy/mdio-gpio.c
@@ -215,6 +215,10 @@ static int mdio_gpio_probe(struct platform_device *pdev)
 	if (pdev->dev.of_node) {
 		pdata = mdio_gpio_of_get_data(pdev);
 		bus_id = of_alias_get_id(pdev->dev.of_node, "mdio-gpio");
+		if (bus_id < 0) {
+			dev_warn(&pdev->dev, "failed to get alias id\n");
+			bus_id = 0;
+		}
 	} else {
 		pdata = dev_get_platdata(&pdev->dev);
 		bus_id = pdev->id;
diff --git a/drivers/net/phy/phy.c b/drivers/net/phy/phy.c
index a972056b224..3bc079a67a3 100644
--- a/drivers/net/phy/phy.c
+++ b/drivers/net/phy/phy.c
@@ -715,7 +715,7 @@ void phy_state_machine(struct work_struct *work)
 	struct delayed_work *dwork = to_delayed_work(work);
 	struct phy_device *phydev =
 			container_of(dwork, struct phy_device, state_queue);
-	int needs_aneg = 0, do_suspend = 0;
+	bool needs_aneg = false, do_suspend = false, do_resume = false;
 	int err = 0;
 
 	mutex_lock(&phydev->lock);
@@ -727,7 +727,7 @@ void phy_state_machine(struct work_struct *work)
 	case PHY_PENDING:
 		break;
 	case PHY_UP:
-		needs_aneg = 1;
+		needs_aneg = true;
 
 		phydev->link_timeout = PHY_AN_TIMEOUT;
 
@@ -757,7 +757,7 @@ void phy_state_machine(struct work_struct *work)
 			phydev->adjust_link(phydev->attached_dev);
 
 		} else if (0 == phydev->link_timeout--)
-			needs_aneg = 1;
+			needs_aneg = true;
 		break;
 	case PHY_NOLINK:
 		err = phy_read_status(phydev);
@@ -791,7 +791,7 @@ void phy_state_machine(struct work_struct *work)
 			netif_carrier_on(phydev->attached_dev);
 		} else {
 			if (0 == phydev->link_timeout--)
-				needs_aneg = 1;
+				needs_aneg = true;
 		}
 
 		phydev->adjust_link(phydev->attached_dev);
@@ -827,7 +827,7 @@ void phy_state_machine(struct work_struct *work)
 			phydev->link = 0;
 			netif_carrier_off(phydev->attached_dev);
 			phydev->adjust_link(phydev->attached_dev);
-			do_suspend = 1;
+			do_suspend = true;
 		}
 		break;
 	case PHY_RESUMING:
@@ -876,6 +876,7 @@ void phy_state_machine(struct work_struct *work)
 			}
 			phydev->adjust_link(phydev->attached_dev);
 		}
+		do_resume = true;
 		break;
 	}
 
@@ -883,9 +884,10 @@ void phy_state_machine(struct work_struct *work)
 
 	if (needs_aneg)
 		err = phy_start_aneg(phydev);
-
-	if (do_suspend)
+	else if (do_suspend)
 		phy_suspend(phydev);
+	else if (do_resume)
+		phy_resume(phydev);
 
 	if (err < 0)
 		phy_error(phydev);
diff --git a/drivers/net/phy/phy_device.c b/drivers/net/phy/phy_device.c
index 0ce60662429..4987a1c6dc5 100644
--- a/drivers/net/phy/phy_device.c
+++ b/drivers/net/phy/phy_device.c
@@ -614,8 +614,8 @@ int phy_attach_direct(struct net_device *dev, struct phy_device *phydev,
 	err = phy_init_hw(phydev);
 	if (err)
 		phy_detach(phydev);
-
-	phy_resume(phydev);
+	else
+		phy_resume(phydev);
 
 	return err;
 }
diff --git a/drivers/net/team/team.c b/drivers/net/team/team.c
index 767fe61b5ac..ce4989be86d 100644
--- a/drivers/net/team/team.c
+++ b/drivers/net/team/team.c
@@ -1724,6 +1724,7 @@ static int team_change_mtu(struct net_device *dev, int new_mtu)
 	 * to traverse list in reverse under rcu_read_lock
 	 */
 	mutex_lock(&team->lock);
+	team->port_mtu_change_allowed = true;
 	list_for_each_entry(port, &team->port_list, list) {
 		err = dev_set_mtu(port->dev, new_mtu);
 		if (err) {
@@ -1732,6 +1733,7 @@ static int team_change_mtu(struct net_device *dev, int new_mtu)
 			goto unwind;
 		}
 	}
+	team->port_mtu_change_allowed = false;
 	mutex_unlock(&team->lock);
 
 	dev->mtu = new_mtu;
@@ -1741,6 +1743,7 @@ static int team_change_mtu(struct net_device *dev, int new_mtu)
 unwind:
 	list_for_each_entry_continue_reverse(port, &team->port_list, list)
 		dev_set_mtu(port->dev, dev->mtu);
+	team->port_mtu_change_allowed = false;
 	mutex_unlock(&team->lock);
 
 	return err;
@@ -2851,7 +2854,9 @@ static int team_device_event(struct notifier_block *unused,
 		break;
 	case NETDEV_PRECHANGEMTU:
 		/* Forbid to change mtu of underlaying device */
-		return NOTIFY_BAD;
+		if (!port->team->port_mtu_change_allowed)
+			return NOTIFY_BAD;
+		break;
 	case NETDEV_PRE_TYPE_CHANGE:
 		/* Forbid to change type of underlaying device */
 		return NOTIFY_BAD;
diff --git a/drivers/net/usb/cdc_mbim.c b/drivers/net/usb/cdc_mbim.c
index c9f3281506a..2e025ddcef2 100644
--- a/drivers/net/usb/cdc_mbim.c
+++ b/drivers/net/usb/cdc_mbim.c
@@ -120,6 +120,16 @@ static void cdc_mbim_unbind(struct usbnet *dev, struct usb_interface *intf)
 	cdc_ncm_unbind(dev, intf);
 }
 
+/* verify that the ethernet protocol is IPv4 or IPv6 */
+static bool is_ip_proto(__be16 proto)
+{
+	switch (proto) {
+	case htons(ETH_P_IP):
+	case htons(ETH_P_IPV6):
+		return true;
+	}
+	return false;
+}
 
 static struct sk_buff *cdc_mbim_tx_fixup(struct usbnet *dev, struct sk_buff *skb, gfp_t flags)
 {
@@ -128,6 +138,7 @@ static struct sk_buff *cdc_mbim_tx_fixup(struct usbnet *dev, struct sk_buff *skb
 	struct cdc_ncm_ctx *ctx = info->ctx;
 	__le32 sign = cpu_to_le32(USB_CDC_MBIM_NDP16_IPS_SIGN);
 	u16 tci = 0;
+	bool is_ip;
 	u8 *c;
 
 	if (!ctx)
@@ -137,25 +148,32 @@ static struct sk_buff *cdc_mbim_tx_fixup(struct usbnet *dev, struct sk_buff *skb
 		if (skb->len <= ETH_HLEN)
 			goto error;
 
+		/* Some applications using e.g. packet sockets will
+		 * bypass the VLAN acceleration and create tagged
+		 * ethernet frames directly.  We primarily look for
+		 * the accelerated out-of-band tag, but fall back if
+		 * required
+		 */
+		skb_reset_mac_header(skb);
+		if (vlan_get_tag(skb, &tci) < 0 && skb->len > VLAN_ETH_HLEN &&
+		    __vlan_get_tag(skb, &tci) == 0) {
+			is_ip = is_ip_proto(vlan_eth_hdr(skb)->h_vlan_encapsulated_proto);
+			skb_pull(skb, VLAN_ETH_HLEN);
+		} else {
+			is_ip = is_ip_proto(eth_hdr(skb)->h_proto);
+			skb_pull(skb, ETH_HLEN);
+		}
+
 		/* mapping VLANs to MBIM sessions:
 		 *   no tag     => IPS session <0>
 		 *   1 - 255    => IPS session <vlanid>
 		 *   256 - 511  => DSS session <vlanid - 256>
 		 *   512 - 4095 => unsupported, drop
 		 */
-		vlan_get_tag(skb, &tci);
-
 		switch (tci & 0x0f00) {
 		case 0x0000: /* VLAN ID 0 - 255 */
-			/* verify that datagram is IPv4 or IPv6 */
-			skb_reset_mac_header(skb);
-			switch (eth_hdr(skb)->h_proto) {
-			case htons(ETH_P_IP):
-			case htons(ETH_P_IPV6):
-				break;
-			default:
+			if (!is_ip)
 				goto error;
-			}
 			c = (u8 *)&sign;
 			c[3] = tci;
 			break;
@@ -169,7 +187,6 @@ static struct sk_buff *cdc_mbim_tx_fixup(struct usbnet *dev, struct sk_buff *skb
 				  "unsupported tci=0x%04x\n", tci);
 			goto error;
 		}
-		skb_pull(skb, ETH_HLEN);
 	}
 
 	spin_lock_bh(&ctx->mtx);
@@ -204,17 +221,23 @@ static void do_neigh_solicit(struct usbnet *dev, u8 *buf, u16 tci)
 		return;
 
 	/* need to send the NA on the VLAN dev, if any */
-	if (tci)
+	rcu_read_lock();
+	if (tci) {
 		netdev = __vlan_find_dev_deep(dev->net, htons(ETH_P_8021Q),
 					      tci);
-	else
+		if (!netdev) {
+			rcu_read_unlock();
+			return;
+		}
+	} else {
 		netdev = dev->net;
-	if (!netdev)
-		return;
+	}
+	dev_hold(netdev);
+	rcu_read_unlock();
 
 	in6_dev = in6_dev_get(netdev);
 	if (!in6_dev)
-		return;
+		goto out;
 	is_router = !!in6_dev->cnf.forwarding;
 	in6_dev_put(in6_dev);
 
@@ -224,6 +247,8 @@ static void do_neigh_solicit(struct usbnet *dev, u8 *buf, u16 tci)
 				 true /* solicited */,
 				 false /* override */,
 				 true /* inc_opt */);
+out:
+	dev_put(netdev);
 }
 
 static bool is_neigh_solicit(u8 *buf, size_t len)
diff --git a/drivers/net/usb/ipheth.c b/drivers/net/usb/ipheth.c
index 421934c83f1..973275fef25 100644
--- a/drivers/net/usb/ipheth.c
+++ b/drivers/net/usb/ipheth.c
@@ -59,6 +59,8 @@
 #define USB_PRODUCT_IPHONE_3GS  0x1294
 #define USB_PRODUCT_IPHONE_4	0x1297
 #define USB_PRODUCT_IPAD 0x129a
+#define USB_PRODUCT_IPAD_2	0x12a2
+#define USB_PRODUCT_IPAD_3	0x12a6
 #define USB_PRODUCT_IPAD_MINI    0x12ab
 #define USB_PRODUCT_IPHONE_4_VZW 0x129c
 #define USB_PRODUCT_IPHONE_4S	0x12a0
@@ -107,6 +109,14 @@ static struct usb_device_id ipheth_table[] = {
 		IPHETH_USBINTF_CLASS, IPHETH_USBINTF_SUBCLASS,
 		IPHETH_USBINTF_PROTO) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(
+		USB_VENDOR_APPLE, USB_PRODUCT_IPAD_2,
+		IPHETH_USBINTF_CLASS, IPHETH_USBINTF_SUBCLASS,
+		IPHETH_USBINTF_PROTO) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(
+		USB_VENDOR_APPLE, USB_PRODUCT_IPAD_3,
+		IPHETH_USBINTF_CLASS, IPHETH_USBINTF_SUBCLASS,
+		IPHETH_USBINTF_PROTO) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(
 		USB_VENDOR_APPLE, USB_PRODUCT_IPAD_MINI,
 		IPHETH_USBINTF_CLASS, IPHETH_USBINTF_SUBCLASS,
 		IPHETH_USBINTF_PROTO) },
diff --git a/drivers/net/usb/qmi_wwan.c b/drivers/net/usb/qmi_wwan.c
index 83208d4fdc5..dc4bf06948c 100644
--- a/drivers/net/usb/qmi_wwan.c
+++ b/drivers/net/usb/qmi_wwan.c
@@ -748,11 +748,15 @@ static const struct usb_device_id products[] = {
 	{QMI_FIXED_INTF(0x1199, 0x68a2, 19)},	/* Sierra Wireless MC7710 in QMI mode */
 	{QMI_FIXED_INTF(0x1199, 0x68c0, 8)},	/* Sierra Wireless MC73xx */
 	{QMI_FIXED_INTF(0x1199, 0x68c0, 10)},	/* Sierra Wireless MC73xx */
-	{QMI_FIXED_INTF(0x1199, 0x68c0, 11)},	/* Sierra Wireless MC73xx */
 	{QMI_FIXED_INTF(0x1199, 0x901c, 8)},    /* Sierra Wireless EM7700 */
 	{QMI_FIXED_INTF(0x1199, 0x901f, 8)},    /* Sierra Wireless EM7355 */
 	{QMI_FIXED_INTF(0x1199, 0x9041, 8)},	/* Sierra Wireless MC7305/MC7355 */
 	{QMI_FIXED_INTF(0x1199, 0x9051, 8)},	/* Netgear AirCard 340U */
+	{QMI_FIXED_INTF(0x1199, 0x9053, 8)},	/* Sierra Wireless Modem */
+	{QMI_FIXED_INTF(0x1199, 0x9054, 8)},	/* Sierra Wireless Modem */
+	{QMI_FIXED_INTF(0x1199, 0x9055, 8)},	/* Netgear AirCard 341U */
+	{QMI_FIXED_INTF(0x1199, 0x9056, 8)},	/* Sierra Wireless Modem */
+	{QMI_FIXED_INTF(0x1199, 0x9061, 8)},	/* Sierra Wireless Modem */
 	{QMI_FIXED_INTF(0x1bbb, 0x011e, 4)},	/* Telekom Speedstick LTE II (Alcatel One Touch L100V LTE) */
 	{QMI_FIXED_INTF(0x1bbb, 0x0203, 2)},	/* Alcatel L800MA */
 	{QMI_FIXED_INTF(0x2357, 0x0201, 4)},	/* TP-LINK HSUPA Modem MA180 */
diff --git a/drivers/net/wireless/ath/ath10k/wmi.h b/drivers/net/wireless/ath/ath10k/wmi.h
index 4fcc96aa951..f51d5ca0141 100644
--- a/drivers/net/wireless/ath/ath10k/wmi.h
+++ b/drivers/net/wireless/ath/ath10k/wmi.h
@@ -1265,7 +1265,7 @@ struct wmi_resource_config {
 	 */
 	__le32 rx_decap_mode;
 
-	/* what is the maximum scan requests than can be queued */
+	/* what is the maximum number of scan requests that can be queued */
 	__le32 scan_max_pending_reqs;
 
 	/* maximum VDEV that could use BMISS offload */
@@ -1450,7 +1450,7 @@ struct wmi_resource_config_10x {
 	 */
 	__le32 rx_decap_mode;
 
-	/* what is the maximum scan requests than can be queued */
+	/* what is the maximum number of scan requests that can be queued */
 	__le32 scan_max_pending_reqs;
 
 	/* maximum VDEV that could use BMISS offload */
diff --git a/drivers/net/wireless/ath/ath6kl/wmi.h b/drivers/net/wireless/ath/ath6kl/wmi.h
index b5f226503ba..5c702ae4d9f 100644
--- a/drivers/net/wireless/ath/ath6kl/wmi.h
+++ b/drivers/net/wireless/ath/ath6kl/wmi.h
@@ -1068,7 +1068,7 @@ struct wmi_power_mode_cmd {
 } __packed;
 
 /*
- * Policy to determnine whether power save failure event should be sent to
+ * Policy to determine whether power save failure event should be sent to
  * host during scanning
  */
 enum power_save_fail_event_policy {
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_main.c b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
index f46cd0250e4..5627917c5ff 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_main.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
@@ -95,8 +95,10 @@ static void ath9k_htc_vif_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
 
 	if ((vif->type == NL80211_IFTYPE_AP ||
 	     vif->type == NL80211_IFTYPE_MESH_POINT) &&
-	    bss_conf->enable_beacon)
+	    bss_conf->enable_beacon) {
 		priv->reconfig_beacon = true;
+		priv->rearm_ani = true;
+	}
 
 	if (bss_conf->assoc) {
 		priv->rearm_ani = true;
@@ -257,6 +259,7 @@ static int ath9k_htc_set_channel(struct ath9k_htc_priv *priv,
 
 	ath9k_htc_ps_wakeup(priv);
 
+	ath9k_htc_stop_ani(priv);
 	del_timer_sync(&priv->tx.cleanup_timer);
 	ath9k_htc_tx_drain(priv);
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
index afb3d15e38f..be1985296bd 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
@@ -4948,7 +4948,7 @@ static int brcmf_enable_bw40_2g(struct brcmf_if *ifp)
 	if (!err) {
 		/* only set 2G bandwidth using bw_cap command */
 		band_bwcap.band = cpu_to_le32(WLC_BAND_2G);
-		band_bwcap.bw_cap = cpu_to_le32(WLC_BW_40MHZ_BIT);
+		band_bwcap.bw_cap = cpu_to_le32(WLC_BW_CAP_40MHZ);
 		err = brcmf_fil_iovar_data_set(ifp, "bw_cap", &band_bwcap,
 					       sizeof(band_bwcap));
 	} else {
diff --git a/drivers/net/wireless/iwlwifi/mvm/coex.c b/drivers/net/wireless/iwlwifi/mvm/coex.c
index fa858d548d1..0489314425c 100644
--- a/drivers/net/wireless/iwlwifi/mvm/coex.c
+++ b/drivers/net/wireless/iwlwifi/mvm/coex.c
@@ -611,14 +611,14 @@ int iwl_send_bt_init_conf(struct iwl_mvm *mvm)
 		bt_cmd->flags |= cpu_to_le32(BT_COEX_SYNC2SCO);
 
 	if (IWL_MVM_BT_COEX_CORUNNING) {
-		bt_cmd->valid_bit_msk = cpu_to_le32(BT_VALID_CORUN_LUT_20 |
-						    BT_VALID_CORUN_LUT_40);
+		bt_cmd->valid_bit_msk |= cpu_to_le32(BT_VALID_CORUN_LUT_20 |
+						     BT_VALID_CORUN_LUT_40);
 		bt_cmd->flags |= cpu_to_le32(BT_COEX_CORUNNING);
 	}
 
 	if (IWL_MVM_BT_COEX_MPLUT) {
 		bt_cmd->flags |= cpu_to_le32(BT_COEX_MPLUT);
-		bt_cmd->valid_bit_msk = cpu_to_le32(BT_VALID_MULTI_PRIO_LUT);
+		bt_cmd->valid_bit_msk |= cpu_to_le32(BT_VALID_MULTI_PRIO_LUT);
 	}
 
 	if (mvm->cfg->bt_shared_single_ant)
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
index 9426905de6b..d73a89ecd78 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
@@ -183,9 +183,9 @@ enum iwl_scan_type {
  *	this number of packets were received (typically 1)
  * @passive2active: is auto switching from passive to active during scan allowed
  * @rxchain_sel_flags: RXON_RX_CHAIN_*
- * @max_out_time: in usecs, max out of serving channel time
+ * @max_out_time: in TUs, max out of serving channel time
  * @suspend_time: how long to pause scan when returning to service channel:
- *	bits 0-19: beacon interal in usecs (suspend before executing)
+ *	bits 0-19: beacon interal in TUs (suspend before executing)
  *	bits 20-23: reserved
  *	bits 24-31: number of beacons (suspend between channels)
  * @rxon_flags: RXON_FLG_*
@@ -383,8 +383,8 @@ enum scan_framework_client {
  * @quiet_plcp_th:	quiet channel num of packets threshold
  * @good_CRC_th:	passive to active promotion threshold
  * @rx_chain:		RXON rx chain.
- * @max_out_time:	max uSec to be out of assoceated channel
- * @suspend_time:	pause scan this long when returning to service channel
+ * @max_out_time:	max TUs to be out of assoceated channel
+ * @suspend_time:	pause scan this TUs when returning to service channel
  * @flags:		RXON flags
  * @filter_flags:	RXONfilter
  * @tx_cmd:		tx command for active scan; for 2GHz and for 5GHz.
diff --git a/drivers/net/wireless/iwlwifi/mvm/mac80211.c b/drivers/net/wireless/iwlwifi/mvm/mac80211.c
index f0cebf12c7b..8735ef1f44a 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mac80211.c
+++ b/drivers/net/wireless/iwlwifi/mvm/mac80211.c
@@ -826,7 +826,7 @@ static int iwl_mvm_mac_add_interface(struct ieee80211_hw *hw,
 	if (ret)
 		goto out_remove_mac;
 
-	if (!mvm->bf_allowed_vif &&
+	if (!mvm->bf_allowed_vif && false &&
 	    vif->type == NL80211_IFTYPE_STATION && !vif->p2p &&
 	    mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_BF_UPDATED){
 		mvm->bf_allowed_vif = mvmvif;
@@ -1007,7 +1007,7 @@ static void iwl_mvm_mc_iface_iterator(void *_data, u8 *mac,
 	memcpy(cmd->bssid, vif->bss_conf.bssid, ETH_ALEN);
 	len = roundup(sizeof(*cmd) + cmd->count * ETH_ALEN, 4);
 
-	ret = iwl_mvm_send_cmd_pdu(mvm, MCAST_FILTER_CMD, CMD_SYNC, len, cmd);
+	ret = iwl_mvm_send_cmd_pdu(mvm, MCAST_FILTER_CMD, CMD_ASYNC, len, cmd);
 	if (ret)
 		IWL_ERR(mvm, "mcast filter cmd error. ret=%d\n", ret);
 }
@@ -1023,7 +1023,7 @@ static void iwl_mvm_recalc_multicast(struct iwl_mvm *mvm)
 	if (WARN_ON_ONCE(!mvm->mcast_filter_cmd))
 		return;
 
-	ieee80211_iterate_active_interfaces(
+	ieee80211_iterate_active_interfaces_atomic(
 		mvm->hw, IEEE80211_IFACE_ITER_NORMAL,
 		iwl_mvm_mc_iface_iterator, &iter_data);
 }
@@ -1807,6 +1807,11 @@ static int iwl_mvm_mac_sched_scan_start(struct ieee80211_hw *hw,
 
 	mutex_lock(&mvm->mutex);
 
+	if (!iwl_mvm_is_idle(mvm)) {
+		ret = -EBUSY;
+		goto out;
+	}
+
 	switch (mvm->scan_status) {
 	case IWL_MVM_SCAN_OS:
 		IWL_DEBUG_SCAN(mvm, "Stopping previous scan for sched_scan\n");
diff --git a/drivers/net/wireless/iwlwifi/mvm/mvm.h b/drivers/net/wireless/iwlwifi/mvm/mvm.h
index d564233a65d..f1ec0986c3c 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mvm.h
+++ b/drivers/net/wireless/iwlwifi/mvm/mvm.h
@@ -1003,6 +1003,9 @@ static inline bool iwl_mvm_vif_low_latency(struct iwl_mvm_vif *mvmvif)
 	return mvmvif->low_latency;
 }
 
+/* Assoc status */
+bool iwl_mvm_is_idle(struct iwl_mvm *mvm);
+
 /* Thermal management and CT-kill */
 void iwl_mvm_tt_tx_backoff(struct iwl_mvm *mvm, u32 backoff);
 void iwl_mvm_tt_handler(struct iwl_mvm *mvm);
diff --git a/drivers/net/wireless/iwlwifi/mvm/rs.c b/drivers/net/wireless/iwlwifi/mvm/rs.c
index 9f52c5b3f0e..e1c83889936 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rs.c
+++ b/drivers/net/wireless/iwlwifi/mvm/rs.c
@@ -1010,7 +1010,7 @@ static void rs_tx_status(void *mvm_r, struct ieee80211_supported_band *sband,
 		return;
 	}
 
-#ifdef CPTCFG_MAC80211_DEBUGFS
+#ifdef CONFIG_MAC80211_DEBUGFS
 	/* Disable last tx check if we are debugging with fixed rate */
 	if (lq_sta->dbg_fixed_rate) {
 		IWL_DEBUG_RATE(mvm, "Fixed rate. avoid rate scaling\n");
diff --git a/drivers/net/wireless/iwlwifi/mvm/scan.c b/drivers/net/wireless/iwlwifi/mvm/scan.c
index c91dc849885..c28de54c75d 100644
--- a/drivers/net/wireless/iwlwifi/mvm/scan.c
+++ b/drivers/net/wireless/iwlwifi/mvm/scan.c
@@ -277,51 +277,22 @@ static void iwl_mvm_scan_calc_params(struct iwl_mvm *mvm,
 					    IEEE80211_IFACE_ITER_NORMAL,
 					    iwl_mvm_scan_condition_iterator,
 					    &global_bound);
-	/*
-	 * Under low latency traffic passive scan is fragmented meaning
-	 * that dwell on a particular channel will be fragmented. Each fragment
-	 * dwell time is 20ms and fragments period is 105ms. Skipping to next
-	 * channel will be delayed by the same period - 105ms. So suspend_time
-	 * parameter describing both fragments and channels skipping periods is
-	 * set to 105ms. This value is chosen so that overall passive scan
-	 * duration will not be too long. Max_out_time in this case is set to
-	 * 70ms, so for active scanning operating channel will be left for 70ms
-	 * while for passive still for 20ms (fragment dwell).
-	 */
-	if (global_bound) {
-		if (!iwl_mvm_low_latency(mvm)) {
-			params->suspend_time = ieee80211_tu_to_usec(100);
-			params->max_out_time = ieee80211_tu_to_usec(600);
-		} else {
-			params->suspend_time = ieee80211_tu_to_usec(105);
-			/* P2P doesn't support fragmented passive scan, so
-			 * configure max_out_time to be at least longest dwell
-			 * time for passive scan.
-			 */
-			if (vif->type == NL80211_IFTYPE_STATION && !vif->p2p) {
-				params->max_out_time = ieee80211_tu_to_usec(70);
-				params->passive_fragmented = true;
-			} else {
-				u32 passive_dwell;
 
-				/*
-				 * Use band G so that passive channel dwell time
-				 * will be assigned with maximum value.
-				 */
-				band = IEEE80211_BAND_2GHZ;
-				passive_dwell = iwl_mvm_get_passive_dwell(band);
-				params->max_out_time =
-					ieee80211_tu_to_usec(passive_dwell);
-			}
-		}
+	if (!global_bound)
+		goto not_bound;
+
+	params->suspend_time = 100;
+	params->max_out_time = 600;
+
+	if (iwl_mvm_low_latency(mvm)) {
+		params->suspend_time = 250;
+		params->max_out_time = 250;
 	}
 
+not_bound:
+
 	for (band = IEEE80211_BAND_2GHZ; band < IEEE80211_NUM_BANDS; band++) {
-		if (params->passive_fragmented)
-			params->dwell[band].passive = 20;
-		else
-			params->dwell[band].passive =
-				iwl_mvm_get_passive_dwell(band);
+		params->dwell[band].passive = iwl_mvm_get_passive_dwell(band);
 		params->dwell[band].active = iwl_mvm_get_active_dwell(band,
 								      n_ssids);
 	}
@@ -761,7 +732,7 @@ int iwl_mvm_config_sched_scan(struct iwl_mvm *mvm,
 	int band_2ghz = mvm->nvm_data->bands[IEEE80211_BAND_2GHZ].n_channels;
 	int band_5ghz = mvm->nvm_data->bands[IEEE80211_BAND_5GHZ].n_channels;
 	int head = 0;
-	int tail = band_2ghz + band_5ghz;
+	int tail = band_2ghz + band_5ghz - 1;
 	u32 ssid_bitmap;
 	int cmd_len;
 	int ret;
diff --git a/drivers/net/wireless/iwlwifi/mvm/utils.c b/drivers/net/wireless/iwlwifi/mvm/utils.c
index d619851745a..2180902266a 100644
--- a/drivers/net/wireless/iwlwifi/mvm/utils.c
+++ b/drivers/net/wireless/iwlwifi/mvm/utils.c
@@ -644,3 +644,22 @@ bool iwl_mvm_low_latency(struct iwl_mvm *mvm)
 
 	return result;
 }
+
+static void iwl_mvm_idle_iter(void *_data, u8 *mac, struct ieee80211_vif *vif)
+{
+	bool *idle = _data;
+
+	if (!vif->bss_conf.idle)
+		*idle = false;
+}
+
+bool iwl_mvm_is_idle(struct iwl_mvm *mvm)
+{
+	bool idle = true;
+
+	ieee80211_iterate_active_interfaces_atomic(
+			mvm->hw, IEEE80211_IFACE_ITER_NORMAL,
+			iwl_mvm_idle_iter, &idle);
+
+	return idle;
+}
diff --git a/drivers/net/wireless/iwlwifi/pcie/trans.c b/drivers/net/wireless/iwlwifi/pcie/trans.c
index dcfd6d866d0..2365553f1ef 100644
--- a/drivers/net/wireless/iwlwifi/pcie/trans.c
+++ b/drivers/net/wireless/iwlwifi/pcie/trans.c
@@ -1749,6 +1749,10 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 	 * PCI Tx retries from interfering with C3 CPU state */
 	pci_write_config_byte(pdev, PCI_CFG_RETRY_TIMEOUT, 0x00);
 
+	trans->dev = &pdev->dev;
+	trans_pcie->pci_dev = pdev;
+	iwl_disable_interrupts(trans);
+
 	err = pci_enable_msi(pdev);
 	if (err) {
 		dev_err(&pdev->dev, "pci_enable_msi failed(0X%x)\n", err);
@@ -1760,8 +1764,6 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 		}
 	}
 
-	trans->dev = &pdev->dev;
-	trans_pcie->pci_dev = pdev;
 	trans->hw_rev = iwl_read32(trans, CSR_HW_REV);
 	trans->hw_id = (pdev->device << 16) + pdev->subsystem_device;
 	snprintf(trans->hw_id_str, sizeof(trans->hw_id_str),
@@ -1787,8 +1789,6 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 		goto out_pci_disable_msi;
 	}
 
-	trans_pcie->inta_mask = CSR_INI_SET_MASK;
-
 	if (iwl_pcie_alloc_ict(trans))
 		goto out_free_cmd_pool;
 
@@ -1800,6 +1800,8 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 		goto out_free_ict;
 	}
 
+	trans_pcie->inta_mask = CSR_INI_SET_MASK;
+
 	return trans;
 
 out_free_ict:
diff --git a/drivers/net/wireless/orinoco/orinoco_usb.c b/drivers/net/wireless/orinoco/orinoco_usb.c
index f9805c9353d..3ac71339d04 100644
--- a/drivers/net/wireless/orinoco/orinoco_usb.c
+++ b/drivers/net/wireless/orinoco/orinoco_usb.c
@@ -100,25 +100,11 @@ static struct ez_usb_fw firmware = {
 	.code = NULL,
 };
 
-#ifdef CONFIG_USB_DEBUG
-static int debug = 1;
-#else
-static int debug;
-#endif
-
 /* Debugging macros */
-#undef dbg
-#define dbg(format, arg...) \
-	do { if (debug) printk(KERN_DEBUG PFX "%s: " format "\n", \
-			       __func__ , ## arg); } while (0)
 #undef err
 #define err(format, arg...) \
 	do { printk(KERN_ERR PFX format "\n", ## arg); } while (0)
 
-/* Module paramaters */
-module_param(debug, int, 0644);
-MODULE_PARM_DESC(debug, "Debug enabled or not");
-
 MODULE_FIRMWARE("orinoco_ezusb_fw");
 
 /*
@@ -341,7 +327,7 @@ static void ezusb_request_timerfn(u_long _ctx)
 		ctx->state = EZUSB_CTX_REQ_TIMEOUT;
 	} else {
 		ctx->state = EZUSB_CTX_RESP_TIMEOUT;
-		dbg("couldn't unlink");
+		dev_dbg(&ctx->outurb->dev->dev, "couldn't unlink\n");
 		atomic_inc(&ctx->refcount);
 		ctx->killed = 1;
 		ezusb_ctx_complete(ctx);
@@ -634,9 +620,9 @@ static void ezusb_request_in_callback(struct ezusb_priv *upriv,
 				ctx = c;
 				break;
 			}
-			dbg("Skipped (0x%x/0x%x) (%d/%d)",
-			    le16_to_cpu(ans->hermes_rid),
-			    c->in_rid, ans->ans_reply_count, reply_count);
+			netdev_dbg(upriv->dev, "Skipped (0x%x/0x%x) (%d/%d)\n",
+				   le16_to_cpu(ans->hermes_rid), c->in_rid,
+				   ans->ans_reply_count, reply_count);
 		}
 	}
 
@@ -768,7 +754,7 @@ static int ezusb_submit_in_urb(struct ezusb_priv *upriv)
 	void *cur_buf = upriv->read_urb->transfer_buffer;
 
 	if (upriv->read_urb->status == -EINPROGRESS) {
-		dbg("urb busy, not resubmiting");
+		netdev_dbg(upriv->dev, "urb busy, not resubmiting\n");
 		retval = -EBUSY;
 		goto exit;
 	}
@@ -838,8 +824,9 @@ static int ezusb_firmware_download(struct ezusb_priv *upriv,
 		memcpy(fw_buffer, &fw->code[addr], FW_BUF_SIZE);
 		if (variant_offset >= addr &&
 		    variant_offset < addr + FW_BUF_SIZE) {
-			dbg("Patching card_variant byte at 0x%04X",
-			    variant_offset);
+			netdev_dbg(upriv->dev,
+				   "Patching card_variant byte at 0x%04X\n",
+				   variant_offset);
 			fw_buffer[variant_offset - addr] = FW_VAR_VALUE;
 		}
 		retval = usb_control_msg(upriv->udev,
@@ -879,7 +866,6 @@ static int ezusb_access_ltv(struct ezusb_priv *upriv,
 	BUG_ON(in_irq());
 
 	if (!upriv->udev) {
-		dbg("Device disconnected");
 		retval = -ENODEV;
 		goto exit;
 	}
@@ -1023,8 +1009,9 @@ static int ezusb_doicmd_wait(struct hermes *hw, u16 cmd, u16 parm0, u16 parm1,
 		cpu_to_le16(parm1),
 		cpu_to_le16(parm2),
 	};
-	dbg("0x%04X, parm0 0x%04X, parm1 0x%04X, parm2 0x%04X",
-	    cmd, parm0, parm1, parm2);
+	netdev_dbg(upriv->dev,
+		   "0x%04X, parm0 0x%04X, parm1 0x%04X, parm2 0x%04X\n", cmd,
+		   parm0, parm1, parm2);
 	ctx = ezusb_alloc_ctx(upriv, EZUSB_RID_DOCMD, EZUSB_RID_ACK);
 	if (!ctx)
 		return -ENOMEM;
@@ -1045,7 +1032,7 @@ static int ezusb_docmd_wait(struct hermes *hw, u16 cmd, u16 parm0,
 		0,
 		0,
 	};
-	dbg("0x%04X, parm0 0x%04X", cmd, parm0);
+	netdev_dbg(upriv->dev, "0x%04X, parm0 0x%04X\n", cmd, parm0);
 	ctx = ezusb_alloc_ctx(upriv, EZUSB_RID_DOCMD, EZUSB_RID_ACK);
 	if (!ctx)
 		return -ENOMEM;
@@ -1332,7 +1319,7 @@ static int ezusb_hard_reset(struct orinoco_private *priv)
 		return retval;
 	}
 
-	dbg("sending control message");
+	netdev_dbg(upriv->dev, "sending control message\n");
 	retval = usb_control_msg(upriv->udev,
 				 usb_sndctrlpipe(upriv->udev, 0),
 				 EZUSB_REQUEST_TRIGER,
@@ -1401,10 +1388,8 @@ static void ezusb_bulk_in_callback(struct urb *urb)
 	u16 crc;
 	u16 hermes_rid;
 
-	if (upriv->udev == NULL) {
-		dbg("disconnected");
+	if (upriv->udev == NULL)
 		return;
-	}
 
 	if (urb->status == -ETIMEDOUT) {
 		/* When a device gets unplugged we get this every time
@@ -1421,12 +1406,13 @@ static void ezusb_bulk_in_callback(struct urb *urb)
 	if ((urb->status == -EILSEQ)
 	    || (urb->status == -ENOENT)
 	    || (urb->status == -ECONNRESET)) {
-		dbg("status %d, not resubmiting", urb->status);
+		netdev_dbg(upriv->dev, "status %d, not resubmiting\n",
+			   urb->status);
 		return;
 	}
 	if (urb->status)
-		dbg("status: %d length: %d",
-		    urb->status, urb->actual_length);
+		netdev_dbg(upriv->dev, "status: %d length: %d\n",
+			   urb->status, urb->actual_length);
 	if (urb->actual_length < sizeof(*ans)) {
 		err("%s: short read, ignoring", __func__);
 		goto resubmit;
diff --git a/drivers/net/wireless/rsi/rsi_91x_sdio.c b/drivers/net/wireless/rsi/rsi_91x_sdio.c
index 2e39d38d6a9..46e7af446f0 100644
--- a/drivers/net/wireless/rsi/rsi_91x_sdio.c
+++ b/drivers/net/wireless/rsi/rsi_91x_sdio.c
@@ -285,7 +285,6 @@ static void rsi_reset_card(struct sdio_func *pfunction)
 		if (err) {
 			rsi_dbg(ERR_ZONE, "%s: CCCR speed reg read failed: %d\n",
 				__func__, err);
-			card->state &= ~MMC_STATE_HIGHSPEED;
 		} else {
 			err = rsi_cmd52writebyte(card,
 						 SDIO_CCCR_SPEED,
@@ -296,14 +295,13 @@ static void rsi_reset_card(struct sdio_func *pfunction)
 					__func__, err);
 				return;
 			}
-			mmc_card_set_highspeed(card);
 			host->ios.timing = MMC_TIMING_SD_HS;
 			host->ops->set_ios(host, &host->ios);
 		}
 	}
 
 	/* Set clock */
-	if (mmc_card_highspeed(card))
+	if (mmc_card_hs(card))
 		clock = 50000000;
 	else
 		clock = card->cis.max_dtr;
diff --git a/drivers/net/wireless/ti/wlcore/main.c b/drivers/net/wireless/ti/wlcore/main.c
index ed88d391348..e71eae35336 100644
--- a/drivers/net/wireless/ti/wlcore/main.c
+++ b/drivers/net/wireless/ti/wlcore/main.c
@@ -543,7 +543,7 @@ static int wlcore_irq_locked(struct wl1271 *wl)
 		 * wl1271_ps_elp_wakeup cannot be called concurrently.
 		 */
 		clear_bit(WL1271_FLAG_IRQ_RUNNING, &wl->flags);
-		smp_mb__after_clear_bit();
+		smp_mb__after_atomic();
 
 		ret = wlcore_fw_status(wl, wl->fw_status);
 		if (ret < 0)
diff --git a/drivers/net/xen-netback/common.h b/drivers/net/xen-netback/common.h
index 630a3fcf65b..0d4a285cbd7 100644
--- a/drivers/net/xen-netback/common.h
+++ b/drivers/net/xen-netback/common.h
@@ -226,7 +226,7 @@ int xenvif_map_frontend_rings(struct xenvif *vif,
 			      grant_ref_t rx_ring_ref);
 
 /* Check for SKBs from frontend and schedule backend processing */
-void xenvif_check_rx_xenvif(struct xenvif *vif);
+void xenvif_napi_schedule_or_enable_events(struct xenvif *vif);
 
 /* Prevent the device from generating any further traffic. */
 void xenvif_carrier_off(struct xenvif *vif);
diff --git a/drivers/net/xen-netback/interface.c b/drivers/net/xen-netback/interface.c
index ef05c5c49d4..20e9defa106 100644
--- a/drivers/net/xen-netback/interface.c
+++ b/drivers/net/xen-netback/interface.c
@@ -75,32 +75,8 @@ static int xenvif_poll(struct napi_struct *napi, int budget)
 	work_done = xenvif_tx_action(vif, budget);
 
 	if (work_done < budget) {
-		int more_to_do = 0;
-		unsigned long flags;
-
-		/* It is necessary to disable IRQ before calling
-		 * RING_HAS_UNCONSUMED_REQUESTS. Otherwise we might
-		 * lose event from the frontend.
-		 *
-		 * Consider:
-		 *   RING_HAS_UNCONSUMED_REQUESTS
-		 *   <frontend generates event to trigger napi_schedule>
-		 *   __napi_complete
-		 *
-		 * This handler is still in scheduled state so the
-		 * event has no effect at all. After __napi_complete
-		 * this handler is descheduled and cannot get
-		 * scheduled again. We lose event in this case and the ring
-		 * will be completely stalled.
-		 */
-
-		local_irq_save(flags);
-
-		RING_FINAL_CHECK_FOR_REQUESTS(&vif->tx, more_to_do);
-		if (!more_to_do)
-			__napi_complete(napi);
-
-		local_irq_restore(flags);
+		napi_complete(napi);
+		xenvif_napi_schedule_or_enable_events(vif);
 	}
 
 	return work_done;
@@ -194,7 +170,7 @@ static void xenvif_up(struct xenvif *vif)
 	enable_irq(vif->tx_irq);
 	if (vif->tx_irq != vif->rx_irq)
 		enable_irq(vif->rx_irq);
-	xenvif_check_rx_xenvif(vif);
+	xenvif_napi_schedule_or_enable_events(vif);
 }
 
 static void xenvif_down(struct xenvif *vif)
diff --git a/drivers/net/xen-netback/netback.c b/drivers/net/xen-netback/netback.c
index 76665405c5a..7367208ee8c 100644
--- a/drivers/net/xen-netback/netback.c
+++ b/drivers/net/xen-netback/netback.c
@@ -104,7 +104,7 @@ static inline unsigned long idx_to_kaddr(struct xenvif *vif,
 
 /* Find the containing VIF's structure from a pointer in pending_tx_info array
  */
-static inline struct xenvif* ubuf_to_vif(struct ubuf_info *ubuf)
+static inline struct xenvif *ubuf_to_vif(const struct ubuf_info *ubuf)
 {
 	u16 pending_idx = ubuf->desc;
 	struct pending_tx_info *temp =
@@ -323,6 +323,35 @@ static void xenvif_gop_frag_copy(struct xenvif *vif, struct sk_buff *skb,
 }
 
 /*
+ * Find the grant ref for a given frag in a chain of struct ubuf_info's
+ * skb: the skb itself
+ * i: the frag's number
+ * ubuf: a pointer to an element in the chain. It should not be NULL
+ *
+ * Returns a pointer to the element in the chain where the page were found. If
+ * not found, returns NULL.
+ * See the definition of callback_struct in common.h for more details about
+ * the chain.
+ */
+static const struct ubuf_info *xenvif_find_gref(const struct sk_buff *const skb,
+						const int i,
+						const struct ubuf_info *ubuf)
+{
+	struct xenvif *foreign_vif = ubuf_to_vif(ubuf);
+
+	do {
+		u16 pending_idx = ubuf->desc;
+
+		if (skb_shinfo(skb)->frags[i].page.p ==
+		    foreign_vif->mmap_pages[pending_idx])
+			break;
+		ubuf = (struct ubuf_info *) ubuf->ctx;
+	} while (ubuf);
+
+	return ubuf;
+}
+
+/*
  * Prepare an SKB to be transmitted to the frontend.
  *
  * This function is responsible for allocating grant operations, meta
@@ -346,9 +375,8 @@ static int xenvif_gop_skb(struct sk_buff *skb,
 	int head = 1;
 	int old_meta_prod;
 	int gso_type;
-	struct ubuf_info *ubuf = skb_shinfo(skb)->destructor_arg;
-	grant_ref_t foreign_grefs[MAX_SKB_FRAGS];
-	struct xenvif *foreign_vif = NULL;
+	const struct ubuf_info *ubuf = skb_shinfo(skb)->destructor_arg;
+	const struct ubuf_info *const head_ubuf = ubuf;
 
 	old_meta_prod = npo->meta_prod;
 
@@ -386,19 +414,6 @@ static int xenvif_gop_skb(struct sk_buff *skb,
 	npo->copy_off = 0;
 	npo->copy_gref = req->gref;
 
-	if ((skb_shinfo(skb)->tx_flags & SKBTX_DEV_ZEROCOPY) &&
-		 (ubuf->callback == &xenvif_zerocopy_callback)) {
-		int i = 0;
-		foreign_vif = ubuf_to_vif(ubuf);
-
-		do {
-			u16 pending_idx = ubuf->desc;
-			foreign_grefs[i++] =
-				foreign_vif->pending_tx_info[pending_idx].req.gref;
-			ubuf = (struct ubuf_info *) ubuf->ctx;
-		} while (ubuf);
-	}
-
 	data = skb->data;
 	while (data < skb_tail_pointer(skb)) {
 		unsigned int offset = offset_in_page(data);
@@ -415,13 +430,60 @@ static int xenvif_gop_skb(struct sk_buff *skb,
 	}
 
 	for (i = 0; i < nr_frags; i++) {
+		/* This variable also signals whether foreign_gref has a real
+		 * value or not.
+		 */
+		struct xenvif *foreign_vif = NULL;
+		grant_ref_t foreign_gref;
+
+		if ((skb_shinfo(skb)->tx_flags & SKBTX_DEV_ZEROCOPY) &&
+			(ubuf->callback == &xenvif_zerocopy_callback)) {
+			const struct ubuf_info *const startpoint = ubuf;
+
+			/* Ideally ubuf points to the chain element which
+			 * belongs to this frag. Or if frags were removed from
+			 * the beginning, then shortly before it.
+			 */
+			ubuf = xenvif_find_gref(skb, i, ubuf);
+
+			/* Try again from the beginning of the list, if we
+			 * haven't tried from there. This only makes sense in
+			 * the unlikely event of reordering the original frags.
+			 * For injected local pages it's an unnecessary second
+			 * run.
+			 */
+			if (unlikely(!ubuf) && startpoint != head_ubuf)
+				ubuf = xenvif_find_gref(skb, i, head_ubuf);
+
+			if (likely(ubuf)) {
+				u16 pending_idx = ubuf->desc;
+
+				foreign_vif = ubuf_to_vif(ubuf);
+				foreign_gref = foreign_vif->pending_tx_info[pending_idx].req.gref;
+				/* Just a safety measure. If this was the last
+				 * element on the list, the for loop will
+				 * iterate again if a local page were added to
+				 * the end. Using head_ubuf here prevents the
+				 * second search on the chain. Or the original
+				 * frags changed order, but that's less likely.
+				 * In any way, ubuf shouldn't be NULL.
+				 */
+				ubuf = ubuf->ctx ?
+					(struct ubuf_info *) ubuf->ctx :
+					head_ubuf;
+			} else
+				/* This frag was a local page, added to the
+				 * array after the skb left netback.
+				 */
+				ubuf = head_ubuf;
+		}
 		xenvif_gop_frag_copy(vif, skb, npo,
 				     skb_frag_page(&skb_shinfo(skb)->frags[i]),
 				     skb_frag_size(&skb_shinfo(skb)->frags[i]),
 				     skb_shinfo(skb)->frags[i].page_offset,
 				     &head,
 				     foreign_vif,
-				     foreign_grefs[i]);
+				     foreign_vif ? foreign_gref : UINT_MAX);
 	}
 
 	return npo->meta_prod - old_meta_prod;
@@ -654,7 +716,7 @@ done:
 		notify_remote_via_irq(vif->rx_irq);
 }
 
-void xenvif_check_rx_xenvif(struct xenvif *vif)
+void xenvif_napi_schedule_or_enable_events(struct xenvif *vif)
 {
 	int more_to_do;
 
@@ -688,7 +750,7 @@ static void tx_credit_callback(unsigned long data)
 {
 	struct xenvif *vif = (struct xenvif *)data;
 	tx_add_credit(vif);
-	xenvif_check_rx_xenvif(vif);
+	xenvif_napi_schedule_or_enable_events(vif);
 }
 
 static void xenvif_tx_err(struct xenvif *vif,
diff --git a/drivers/nubus/nubus.c b/drivers/nubus/nubus.c
index 43926cd25ae..5066a7ef7b6 100644
--- a/drivers/nubus/nubus.c
+++ b/drivers/nubus/nubus.c
@@ -473,7 +473,7 @@ static struct nubus_dev* __init
 	if (slot == 0 && (unsigned long)dir.base % 2)
 		dir.base += 1;
 	
-	if (console_loglevel >= 10)
+	if (console_loglevel >= CONSOLE_LOGLEVEL_DEBUG)
 		printk(KERN_DEBUG "nubus_get_functional_resource: parent is 0x%p, dir is 0x%p\n",
 		       parent->base, dir.base);
 
@@ -568,7 +568,7 @@ static int __init nubus_get_vidnames(struct nubus_board* board,
 
 	printk(KERN_INFO "    video modes supported:\n");
 	nubus_get_subdir(parent, &dir);
-	if (console_loglevel >= 10)
+	if (console_loglevel >= CONSOLE_LOGLEVEL_DEBUG)
 		printk(KERN_DEBUG "nubus_get_vidnames: parent is 0x%p, dir is 0x%p\n",
 		       parent->base, dir.base);
 
@@ -629,7 +629,7 @@ static int __init nubus_get_vendorinfo(struct nubus_board* board,
 
 	printk(KERN_INFO "    vendor info:\n");
 	nubus_get_subdir(parent, &dir);
-	if (console_loglevel >= 10)
+	if (console_loglevel >= CONSOLE_LOGLEVEL_DEBUG)
 		printk(KERN_DEBUG "nubus_get_vendorinfo: parent is 0x%p, dir is 0x%p\n",
 		       parent->base, dir.base);
 
@@ -654,7 +654,7 @@ static int __init nubus_get_board_resource(struct nubus_board* board, int slot,
 	struct nubus_dirent ent;
 	
 	nubus_get_subdir(parent, &dir);
-	if (console_loglevel >= 10)
+	if (console_loglevel >= CONSOLE_LOGLEVEL_DEBUG)
 		printk(KERN_DEBUG "nubus_get_board_resource: parent is 0x%p, dir is 0x%p\n",
 		       parent->base, dir.base);
 
@@ -753,19 +753,19 @@ static void __init nubus_find_rom_dir(struct nubus_board* board)
 	if (nubus_readdir(&dir, &ent) == -1)
 		goto badrom;
 
-	if (console_loglevel >= 10)
+	if (console_loglevel >= CONSOLE_LOGLEVEL_DEBUG)
 		printk(KERN_INFO "nubus_get_rom_dir: entry %02x %06x\n", ent.type, ent.data);
 	/* This one takes us to where we want to go. */
 	if (nubus_readdir(&dir, &ent) == -1) 
 		goto badrom;
-	if (console_loglevel >= 10)
+	if (console_loglevel >= CONSOLE_LOGLEVEL_DEBUG)
 		printk(KERN_DEBUG "nubus_get_rom_dir: entry %02x %06x\n", ent.type, ent.data);
 	nubus_get_subdir(&ent, &dir);
 
 	/* Resource ID 01, also an "Unknown Macintosh" */
 	if (nubus_readdir(&dir, &ent) == -1) 
 		goto badrom;
-	if (console_loglevel >= 10)
+	if (console_loglevel >= CONSOLE_LOGLEVEL_DEBUG)
 		printk(KERN_DEBUG "nubus_get_rom_dir: entry %02x %06x\n", ent.type, ent.data);
 
 	/* FIXME: the first one is *not* always the right one.  We
@@ -780,7 +780,7 @@ static void __init nubus_find_rom_dir(struct nubus_board* board)
 	   path to that address... */
 	if (nubus_readdir(&dir, &ent) == -1)
 		goto badrom;
-	if (console_loglevel >= 10)
+	if (console_loglevel >= CONSOLE_LOGLEVEL_DEBUG)
 		printk(KERN_DEBUG "nubus_get_rom_dir: entry %02x %06x\n", ent.type, ent.data);
 	
 	/* Bwahahahaha... */
@@ -816,7 +816,7 @@ static struct nubus_board* __init nubus_add_board(int slot, int bytelanes)
 	board->fblock = rp;
 
 	/* Dump the format block for debugging purposes */
-	if (console_loglevel >= 10) {
+	if (console_loglevel >= CONSOLE_LOGLEVEL_DEBUG) {
 		int i;
 		printk(KERN_DEBUG "Slot %X, format block at 0x%p\n",
 		       slot, rp);
diff --git a/drivers/of/Kconfig b/drivers/of/Kconfig
index 889005fa4d0..2dcb0541012 100644
--- a/drivers/of/Kconfig
+++ b/drivers/of/Kconfig
@@ -20,6 +20,7 @@ config OF_SELFTEST
 config OF_FLATTREE
 	bool
 	select DTC
+	select LIBFDT
 
 config OF_EARLY_FLATTREE
 	bool
diff --git a/drivers/of/Makefile b/drivers/of/Makefile
index ed9660adad7..099b1fb00af 100644
--- a/drivers/of/Makefile
+++ b/drivers/of/Makefile
@@ -1,5 +1,6 @@
 obj-y = base.o device.o platform.o
 obj-$(CONFIG_OF_FLATTREE) += fdt.o
+obj-$(CONFIG_OF_EARLY_FLATTREE) += fdt_address.o
 obj-$(CONFIG_OF_PROMTREE) += pdt.o
 obj-$(CONFIG_OF_ADDRESS)  += address.o
 obj-$(CONFIG_OF_IRQ)    += irq.o
@@ -10,3 +11,6 @@ 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
+
+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 cb4242a69cd..5edfcb0da37 100644
--- a/drivers/of/address.c
+++ b/drivers/of/address.c
@@ -498,8 +498,7 @@ static u64 __of_translate_address(struct device_node *dev,
 	/* Count address cells & copy address locally */
 	bus->count_cells(dev, &na, &ns);
 	if (!OF_CHECK_COUNTS(na, ns)) {
-		printk(KERN_ERR "prom_parse: Bad cell count for %s\n",
-		       of_node_full_name(dev));
+		pr_debug("OF: Bad cell count for %s\n", of_node_full_name(dev));
 		goto bail;
 	}
 	memcpy(addr, in_addr, na * 4);
@@ -564,25 +563,6 @@ u64 of_translate_dma_address(struct device_node *dev, const __be32 *in_addr)
 }
 EXPORT_SYMBOL(of_translate_dma_address);
 
-bool of_can_translate_address(struct device_node *dev)
-{
-	struct device_node *parent;
-	struct of_bus *bus;
-	int na, ns;
-
-	parent = of_get_parent(dev);
-	if (parent == NULL)
-		return false;
-
-	bus = of_match_bus(parent);
-	bus->count_cells(dev, &na, &ns);
-
-	of_node_put(parent);
-
-	return OF_CHECK_COUNTS(na, ns);
-}
-EXPORT_SYMBOL(of_can_translate_address);
-
 const __be32 *of_get_address(struct device_node *dev, int index, u64 *size,
 		    unsigned int *flags)
 {
@@ -721,3 +701,113 @@ void __iomem *of_iomap(struct device_node *np, int index)
 	return ioremap(res.start, resource_size(&res));
 }
 EXPORT_SYMBOL(of_iomap);
+
+/**
+ * of_dma_get_range - Get DMA range info
+ * @np:		device node to get DMA range info
+ * @dma_addr:	pointer to store initial DMA address of DMA range
+ * @paddr:	pointer to store initial CPU address of DMA range
+ * @size:	pointer to store size of DMA range
+ *
+ * Look in bottom up direction for the first "dma-ranges" property
+ * and parse it.
+ *  dma-ranges format:
+ *	DMA addr (dma_addr)	: naddr cells
+ *	CPU addr (phys_addr_t)	: pna cells
+ *	size			: nsize cells
+ *
+ * It returns -ENODEV if "dma-ranges" property was not found
+ * for this device in DT.
+ */
+int of_dma_get_range(struct device_node *np, u64 *dma_addr, u64 *paddr, u64 *size)
+{
+	struct device_node *node = of_node_get(np);
+	const __be32 *ranges = NULL;
+	int len, naddr, nsize, pna;
+	int ret = 0;
+	u64 dmaaddr;
+
+	if (!node)
+		return -EINVAL;
+
+	while (1) {
+		naddr = of_n_addr_cells(node);
+		nsize = of_n_size_cells(node);
+		node = of_get_next_parent(node);
+		if (!node)
+			break;
+
+		ranges = of_get_property(node, "dma-ranges", &len);
+
+		/* Ignore empty ranges, they imply no translation required */
+		if (ranges && len > 0)
+			break;
+
+		/*
+		 * At least empty ranges has to be defined for parent node if
+		 * DMA is supported
+		 */
+		if (!ranges)
+			break;
+	}
+
+	if (!ranges) {
+		pr_debug("%s: no dma-ranges found for node(%s)\n",
+			 __func__, np->full_name);
+		ret = -ENODEV;
+		goto out;
+	}
+
+	len /= sizeof(u32);
+
+	pna = of_n_addr_cells(node);
+
+	/* dma-ranges format:
+	 * DMA addr	: naddr cells
+	 * CPU addr	: pna cells
+	 * size		: nsize cells
+	 */
+	dmaaddr = of_read_number(ranges, naddr);
+	*paddr = of_translate_dma_address(np, ranges);
+	if (*paddr == OF_BAD_ADDR) {
+		pr_err("%s: translation of DMA address(%pad) to CPU address failed node(%s)\n",
+		       __func__, dma_addr, np->full_name);
+		ret = -EINVAL;
+		goto out;
+	}
+	*dma_addr = dmaaddr;
+
+	*size = of_read_number(ranges + naddr + pna, nsize);
+
+	pr_debug("dma_addr(%llx) cpu_addr(%llx) size(%llx)\n",
+		 *dma_addr, *paddr, *size);
+
+out:
+	of_node_put(node);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(of_dma_get_range);
+
+/**
+ * of_dma_is_coherent - Check if device is coherent
+ * @np:	device node
+ *
+ * It returns true if "dma-coherent" property was found
+ * for this device in DT.
+ */
+bool of_dma_is_coherent(struct device_node *np)
+{
+	struct device_node *node = of_node_get(np);
+
+	while (node) {
+		if (of_property_read_bool(node, "dma-coherent")) {
+			of_node_put(node);
+			return true;
+		}
+		node = of_get_next_parent(node);
+	}
+	of_node_put(node);
+	return false;
+}
+EXPORT_SYMBOL_GPL(of_dma_is_coherent);
diff --git a/drivers/of/base.c b/drivers/of/base.c
index 6d4ee22708c..8368d96ae7b 100644
--- a/drivers/of/base.c
+++ b/drivers/of/base.c
@@ -695,6 +695,25 @@ struct device_node *of_get_next_parent(struct device_node *node)
 }
 EXPORT_SYMBOL(of_get_next_parent);
 
+static struct device_node *__of_get_next_child(const struct device_node *node,
+						struct device_node *prev)
+{
+	struct device_node *next;
+
+	if (!node)
+		return NULL;
+
+	next = prev ? prev->sibling : node->child;
+	for (; next; next = next->sibling)
+		if (of_node_get(next))
+			break;
+	of_node_put(prev);
+	return next;
+}
+#define __for_each_child_of_node(parent, child) \
+	for (child = __of_get_next_child(parent, NULL); child != NULL; \
+	     child = __of_get_next_child(parent, child))
+
 /**
  *	of_get_next_child - Iterate a node childs
  *	@node:	parent node
@@ -710,11 +729,7 @@ struct device_node *of_get_next_child(const struct device_node *node,
 	unsigned long flags;
 
 	raw_spin_lock_irqsave(&devtree_lock, flags);
-	next = prev ? prev->sibling : node->child;
-	for (; next; next = next->sibling)
-		if (of_node_get(next))
-			break;
-	of_node_put(prev);
+	next = __of_get_next_child(node, prev);
 	raw_spin_unlock_irqrestore(&devtree_lock, flags);
 	return next;
 }
@@ -734,6 +749,9 @@ struct device_node *of_get_next_available_child(const struct device_node *node,
 	struct device_node *next;
 	unsigned long flags;
 
+	if (!node)
+		return NULL;
+
 	raw_spin_lock_irqsave(&devtree_lock, flags);
 	next = prev ? prev->sibling : node->child;
 	for (; next; next = next->sibling) {
@@ -771,23 +789,78 @@ struct device_node *of_get_child_by_name(const struct device_node *node,
 }
 EXPORT_SYMBOL(of_get_child_by_name);
 
+static struct device_node *__of_find_node_by_path(struct device_node *parent,
+						const char *path)
+{
+	struct device_node *child;
+	int len = strchrnul(path, '/') - path;
+
+	if (!len)
+		return NULL;
+
+	__for_each_child_of_node(parent, child) {
+		const char *name = strrchr(child->full_name, '/');
+		if (WARN(!name, "malformed device_node %s\n", child->full_name))
+			continue;
+		name++;
+		if (strncmp(path, name, len) == 0 && (strlen(name) == len))
+			return child;
+	}
+	return NULL;
+}
+
 /**
  *	of_find_node_by_path - Find a node matching a full OF path
- *	@path:	The full path to match
+ *	@path: Either the full path to match, or if the path does not
+ *	       start with '/', the name of a property of the /aliases
+ *	       node (an alias).  In the case of an alias, the node
+ *	       matching the alias' value will be returned.
+ *
+ *	Valid paths:
+ *		/foo/bar	Full path
+ *		foo		Valid alias
+ *		foo/bar		Valid alias + relative path
  *
  *	Returns a node pointer with refcount incremented, use
  *	of_node_put() on it when done.
  */
 struct device_node *of_find_node_by_path(const char *path)
 {
-	struct device_node *np = of_allnodes;
+	struct device_node *np = NULL;
+	struct property *pp;
 	unsigned long flags;
 
+	if (strcmp(path, "/") == 0)
+		return of_node_get(of_allnodes);
+
+	/* The path could begin with an alias */
+	if (*path != '/') {
+		char *p = strchrnul(path, '/');
+		int len = p - path;
+
+		/* of_aliases must not be NULL */
+		if (!of_aliases)
+			return NULL;
+
+		for_each_property_of_node(of_aliases, pp) {
+			if (strlen(pp->name) == len && !strncmp(pp->name, path, len)) {
+				np = of_find_node_by_path(pp->value);
+				break;
+			}
+		}
+		if (!np)
+			return NULL;
+		path = p;
+	}
+
+	/* Step down the tree matching path components */
 	raw_spin_lock_irqsave(&devtree_lock, flags);
-	for (; np; np = np->allnext) {
-		if (np->full_name && (of_node_cmp(np->full_name, path) == 0)
-		    && of_node_get(np))
-			break;
+	if (!np)
+		np = of_node_get(of_allnodes);
+	while (np && *path == '/') {
+		path++; /* Increment past '/' delimiter */
+		np = __of_find_node_by_path(np, path);
+		path = strchrnul(path, '/');
 	}
 	raw_spin_unlock_irqrestore(&devtree_lock, flags);
 	return np;
@@ -1800,7 +1873,7 @@ int of_update_property(struct device_node *np, struct property *newprop)
 {
 	struct property **next, *oldprop;
 	unsigned long flags;
-	int rc, found = 0;
+	int rc;
 
 	rc = of_property_notify(OF_RECONFIG_UPDATE_PROPERTY, np, newprop);
 	if (rc)
@@ -1809,30 +1882,34 @@ int of_update_property(struct device_node *np, struct property *newprop)
 	if (!newprop->name)
 		return -EINVAL;
 
-	oldprop = of_find_property(np, newprop->name, NULL);
-	if (!oldprop)
-		return of_add_property(np, newprop);
-
 	raw_spin_lock_irqsave(&devtree_lock, flags);
 	next = &np->properties;
-	while (*next) {
+	oldprop = __of_find_property(np, newprop->name, NULL);
+	if (!oldprop) {
+		/* add the new node */
+		rc = __of_add_property(np, newprop);
+	} else while (*next) {
+		/* replace the node */
 		if (*next == oldprop) {
-			/* found the node */
 			newprop->next = oldprop->next;
 			*next = newprop;
 			oldprop->next = np->deadprops;
 			np->deadprops = oldprop;
-			found = 1;
 			break;
 		}
 		next = &(*next)->next;
 	}
 	raw_spin_unlock_irqrestore(&devtree_lock, flags);
-	if (!found)
-		return -ENODEV;
+	if (rc)
+		return rc;
+
+	/* At early boot, bail out and defer setup to of_init() */
+	if (!of_kset)
+		return 0;
 
 	/* Update the sysfs attribute */
-	sysfs_remove_bin_file(&np->kobj, &oldprop->attr);
+	if (oldprop)
+		sysfs_remove_bin_file(&np->kobj, &oldprop->attr);
 	__of_add_property_sysfs(np, newprop);
 
 	return 0;
@@ -2036,8 +2113,8 @@ void of_alias_scan(void * (*dt_alloc)(u64 size, u64 align))
  * @np:		Pointer to the given device_node
  * @stem:	Alias stem of the given device_node
  *
- * The function travels the lookup table to get alias id for the given
- * device_node and alias stem.  It returns the alias id if find it.
+ * The function travels the lookup table to get the alias id for the given
+ * device_node and alias stem.  It returns the alias id if found.
  */
 int of_alias_get_id(struct device_node *np, const char *stem)
 {
diff --git a/drivers/of/fdt.c b/drivers/of/fdt.c
index 7a2ef7bb802..c4cddf0cd96 100644
--- a/drivers/of/fdt.c
+++ b/drivers/of/fdt.c
@@ -12,7 +12,6 @@
 #include <linux/kernel.h>
 #include <linux/initrd.h>
 #include <linux/memblock.h>
-#include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_fdt.h>
 #include <linux/of_reserved_mem.h>
@@ -20,62 +19,13 @@
 #include <linux/string.h>
 #include <linux/errno.h>
 #include <linux/slab.h>
+#include <linux/libfdt.h>
+#include <linux/debugfs.h>
+#include <linux/serial_core.h>
 
 #include <asm/setup.h>  /* for COMMAND_LINE_SIZE */
-#ifdef CONFIG_PPC
-#include <asm/machdep.h>
-#endif /* CONFIG_PPC */
-
 #include <asm/page.h>
 
-char *of_fdt_get_string(struct boot_param_header *blob, u32 offset)
-{
-	return ((char *)blob) +
-		be32_to_cpu(blob->off_dt_strings) + offset;
-}
-
-/**
- * of_fdt_get_property - Given a node in the given flat blob, return
- * the property ptr
- */
-void *of_fdt_get_property(struct boot_param_header *blob,
-		       unsigned long node, const char *name,
-		       unsigned long *size)
-{
-	unsigned long p = node;
-
-	do {
-		u32 tag = be32_to_cpup((__be32 *)p);
-		u32 sz, noff;
-		const char *nstr;
-
-		p += 4;
-		if (tag == OF_DT_NOP)
-			continue;
-		if (tag != OF_DT_PROP)
-			return NULL;
-
-		sz = be32_to_cpup((__be32 *)p);
-		noff = be32_to_cpup((__be32 *)(p + 4));
-		p += 8;
-		if (be32_to_cpu(blob->version) < 0x10)
-			p = ALIGN(p, sz >= 8 ? 8 : 4);
-
-		nstr = of_fdt_get_string(blob, noff);
-		if (nstr == NULL) {
-			pr_warning("Can't find property index name !\n");
-			return NULL;
-		}
-		if (strcmp(name, nstr) == 0) {
-			if (size)
-				*size = sz;
-			return (void *)p;
-		}
-		p += sz;
-		p = ALIGN(p, 4);
-	} while (1);
-}
-
 /**
  * of_fdt_is_compatible - Return true if given node from the given blob has
  * compat in its compatible list
@@ -86,13 +36,14 @@ void *of_fdt_get_property(struct boot_param_header *blob,
  * On match, returns a non-zero value with smaller values returned for more
  * specific compatible values.
  */
-int of_fdt_is_compatible(struct boot_param_header *blob,
+int of_fdt_is_compatible(const void *blob,
 		      unsigned long node, const char *compat)
 {
 	const char *cp;
-	unsigned long cplen, l, score = 0;
+	int cplen;
+	unsigned long l, score = 0;
 
-	cp = of_fdt_get_property(blob, node, "compatible", &cplen);
+	cp = fdt_getprop(blob, node, "compatible", &cplen);
 	if (cp == NULL)
 		return 0;
 	while (cplen > 0) {
@@ -110,7 +61,7 @@ int of_fdt_is_compatible(struct boot_param_header *blob,
 /**
  * of_fdt_match - Return true if node matches a list of compatible values
  */
-int of_fdt_match(struct boot_param_header *blob, unsigned long node,
+int of_fdt_match(const void *blob, unsigned long node,
                  const char *const *compat)
 {
 	unsigned int tmp, score = 0;
@@ -149,30 +100,29 @@ static void *unflatten_dt_alloc(void **mem, unsigned long size,
  * @allnextpp: pointer to ->allnext from last allocated device_node
  * @fpsize: Size of the node path up at the current depth.
  */
-static void * unflatten_dt_node(struct boot_param_header *blob,
+static void * unflatten_dt_node(void *blob,
 				void *mem,
-				void **p,
+				int *poffset,
 				struct device_node *dad,
 				struct device_node ***allnextpp,
 				unsigned long fpsize)
 {
+	const __be32 *p;
 	struct device_node *np;
 	struct property *pp, **prev_pp = NULL;
-	char *pathp;
-	u32 tag;
+	const char *pathp;
 	unsigned int l, allocl;
+	static int depth = 0;
+	int old_depth;
+	int offset;
 	int has_name = 0;
 	int new_format = 0;
 
-	tag = be32_to_cpup(*p);
-	if (tag != OF_DT_BEGIN_NODE) {
-		pr_err("Weird tag at start of node: %x\n", tag);
+	pathp = fdt_get_name(blob, *poffset, &l);
+	if (!pathp)
 		return mem;
-	}
-	*p += 4;
-	pathp = *p;
-	l = allocl = strlen(pathp) + 1;
-	*p = PTR_ALIGN(*p + l, 4);
+
+	allocl = l++;
 
 	/* version 0x10 has a more compact unit name here instead of the full
 	 * path. we accumulate the full path size using "fpsize", we'll rebuild
@@ -190,7 +140,7 @@ static void * unflatten_dt_node(struct boot_param_header *blob,
 			fpsize = 1;
 			allocl = 2;
 			l = 1;
-			*pathp = '\0';
+			pathp = "";
 		} else {
 			/* account for '/' and path size minus terminal 0
 			 * already in 'l'
@@ -237,32 +187,23 @@ static void * unflatten_dt_node(struct boot_param_header *blob,
 		}
 	}
 	/* process properties */
-	while (1) {
-		u32 sz, noff;
-		char *pname;
-
-		tag = be32_to_cpup(*p);
-		if (tag == OF_DT_NOP) {
-			*p += 4;
-			continue;
-		}
-		if (tag != OF_DT_PROP)
+	for (offset = fdt_first_property_offset(blob, *poffset);
+	     (offset >= 0);
+	     (offset = fdt_next_property_offset(blob, offset))) {
+		const char *pname;
+		u32 sz;
+
+		if (!(p = fdt_getprop_by_offset(blob, offset, &pname, &sz))) {
+			offset = -FDT_ERR_INTERNAL;
 			break;
-		*p += 4;
-		sz = be32_to_cpup(*p);
-		noff = be32_to_cpup(*p + 4);
-		*p += 8;
-		if (be32_to_cpu(blob->version) < 0x10)
-			*p = PTR_ALIGN(*p, sz >= 8 ? 8 : 4);
-
-		pname = of_fdt_get_string(blob, noff);
+		}
+
 		if (pname == NULL) {
 			pr_info("Can't find property name in list !\n");
 			break;
 		}
 		if (strcmp(pname, "name") == 0)
 			has_name = 1;
-		l = strlen(pname) + 1;
 		pp = unflatten_dt_alloc(&mem, sizeof(struct property),
 					__alignof__(struct property));
 		if (allnextpp) {
@@ -274,26 +215,25 @@ static void * unflatten_dt_node(struct boot_param_header *blob,
 			if ((strcmp(pname, "phandle") == 0) ||
 			    (strcmp(pname, "linux,phandle") == 0)) {
 				if (np->phandle == 0)
-					np->phandle = be32_to_cpup((__be32*)*p);
+					np->phandle = be32_to_cpup(p);
 			}
 			/* And we process the "ibm,phandle" property
 			 * used in pSeries dynamic device tree
 			 * stuff */
 			if (strcmp(pname, "ibm,phandle") == 0)
-				np->phandle = be32_to_cpup((__be32 *)*p);
-			pp->name = pname;
+				np->phandle = be32_to_cpup(p);
+			pp->name = (char *)pname;
 			pp->length = sz;
-			pp->value = *p;
+			pp->value = (__be32 *)p;
 			*prev_pp = pp;
 			prev_pp = &pp->next;
 		}
-		*p = PTR_ALIGN((*p) + sz, 4);
 	}
 	/* with version 0x10 we may not have the name property, recreate
 	 * it here from the unit name if absent
 	 */
 	if (!has_name) {
-		char *p1 = pathp, *ps = pathp, *pa = NULL;
+		const char *p1 = pathp, *ps = pathp, *pa = NULL;
 		int sz;
 
 		while (*p1) {
@@ -330,19 +270,18 @@ static void * unflatten_dt_node(struct boot_param_header *blob,
 		if (!np->type)
 			np->type = "<NULL>";
 	}
-	while (tag == OF_DT_BEGIN_NODE || tag == OF_DT_NOP) {
-		if (tag == OF_DT_NOP)
-			*p += 4;
-		else
-			mem = unflatten_dt_node(blob, mem, p, np, allnextpp,
-						fpsize);
-		tag = be32_to_cpup(*p);
-	}
-	if (tag != OF_DT_END_NODE) {
-		pr_err("Weird tag at end of node: %x\n", tag);
-		return mem;
-	}
-	*p += 4;
+
+	old_depth = depth;
+	*poffset = fdt_next_node(blob, *poffset, &depth);
+	if (depth < 0)
+		depth = 0;
+	while (*poffset > 0 && depth > old_depth)
+		mem = unflatten_dt_node(blob, mem, poffset, np, allnextpp,
+					fpsize);
+
+	if (*poffset < 0 && *poffset != -FDT_ERR_NOTFOUND)
+		pr_err("unflatten: error %d processing FDT\n", *poffset);
+
 	return mem;
 }
 
@@ -358,12 +297,13 @@ static void * unflatten_dt_node(struct boot_param_header *blob,
  * @dt_alloc: An allocator that provides a virtual address to memory
  * for the resulting tree
  */
-static void __unflatten_device_tree(struct boot_param_header *blob,
+static void __unflatten_device_tree(void *blob,
 			     struct device_node **mynodes,
 			     void * (*dt_alloc)(u64 size, u64 align))
 {
 	unsigned long size;
-	void *start, *mem;
+	int start;
+	void *mem;
 	struct device_node **allnextp = mynodes;
 
 	pr_debug(" -> unflatten_device_tree()\n");
@@ -374,18 +314,18 @@ static void __unflatten_device_tree(struct boot_param_header *blob,
 	}
 
 	pr_debug("Unflattening device tree:\n");
-	pr_debug("magic: %08x\n", be32_to_cpu(blob->magic));
-	pr_debug("size: %08x\n", be32_to_cpu(blob->totalsize));
-	pr_debug("version: %08x\n", be32_to_cpu(blob->version));
+	pr_debug("magic: %08x\n", fdt_magic(blob));
+	pr_debug("size: %08x\n", fdt_totalsize(blob));
+	pr_debug("version: %08x\n", fdt_version(blob));
 
-	if (be32_to_cpu(blob->magic) != OF_DT_HEADER) {
+	if (fdt_check_header(blob)) {
 		pr_err("Invalid device tree blob header\n");
 		return;
 	}
 
 	/* First pass, scan for size */
-	start = ((void *)blob) + be32_to_cpu(blob->off_dt_struct);
-	size = (unsigned long)unflatten_dt_node(blob, 0, &start, NULL, NULL, 0);
+	start = 0;
+	size = (unsigned long)unflatten_dt_node(blob, NULL, &start, NULL, NULL, 0);
 	size = ALIGN(size, 4);
 
 	pr_debug("  size is %lx, allocating...\n", size);
@@ -399,10 +339,8 @@ static void __unflatten_device_tree(struct boot_param_header *blob,
 	pr_debug("  unflattening %p...\n", mem);
 
 	/* Second pass, do actual unflattening */
-	start = ((void *)blob) + be32_to_cpu(blob->off_dt_struct);
+	start = 0;
 	unflatten_dt_node(blob, mem, &start, NULL, &allnextp, 0);
-	if (be32_to_cpup(start) != OF_DT_END)
-		pr_warning("Weird tag at end of tree: %08x\n", be32_to_cpup(start));
 	if (be32_to_cpup(mem + size) != 0xdeadbeef)
 		pr_warning("End of tree marker overwritten: %08x\n",
 			   be32_to_cpup(mem + size));
@@ -427,9 +365,7 @@ static void *kernel_tree_alloc(u64 size, u64 align)
 void of_fdt_unflatten_tree(unsigned long *blob,
 			struct device_node **mynodes)
 {
-	struct boot_param_header *device_tree =
-		(struct boot_param_header *)blob;
-	__unflatten_device_tree(device_tree, mynodes, &kernel_tree_alloc);
+	__unflatten_device_tree(blob, mynodes, &kernel_tree_alloc);
 }
 EXPORT_SYMBOL_GPL(of_fdt_unflatten_tree);
 
@@ -437,7 +373,7 @@ EXPORT_SYMBOL_GPL(of_fdt_unflatten_tree);
 int __initdata dt_root_addr_cells;
 int __initdata dt_root_size_cells;
 
-struct boot_param_header *initial_boot_params;
+void *initial_boot_params;
 
 #ifdef CONFIG_OF_EARLY_FLATTREE
 
@@ -449,8 +385,8 @@ static int __init __reserved_mem_reserve_reg(unsigned long node,
 {
 	int t_len = (dt_root_addr_cells + dt_root_size_cells) * sizeof(__be32);
 	phys_addr_t base, size;
-	unsigned long len;
-	__be32 *prop;
+	int len;
+	const __be32 *prop;
 	int nomap, first = 1;
 
 	prop = of_get_flat_dt_prop(node, "reg", &len);
@@ -493,7 +429,7 @@ static int __init __reserved_mem_reserve_reg(unsigned long node,
  */
 static int __init __reserved_mem_check_root(unsigned long node)
 {
-	__be32 *prop;
+	const __be32 *prop;
 
 	prop = of_get_flat_dt_prop(node, "#size-cells", NULL);
 	if (!prop || be32_to_cpup(prop) != dt_root_size_cells)
@@ -557,9 +493,25 @@ static int __init __fdt_scan_reserved_mem(unsigned long node, const char *uname,
  */
 void __init early_init_fdt_scan_reserved_mem(void)
 {
+	int n;
+	u64 base, size;
+
 	if (!initial_boot_params)
 		return;
 
+	/* Reserve the dtb region */
+	early_init_dt_reserve_memory_arch(__pa(initial_boot_params),
+					  fdt_totalsize(initial_boot_params),
+					  0);
+
+	/* Process header /memreserve/ fields */
+	for (n = 0; ; n++) {
+		fdt_get_mem_rsv(initial_boot_params, n, &base, &size);
+		if (!size)
+			break;
+		early_init_dt_reserve_memory_arch(base, size, 0);
+	}
+
 	of_scan_flat_dt(__fdt_scan_reserved_mem, NULL);
 	fdt_init_reserved_mem();
 }
@@ -578,47 +530,19 @@ int __init of_scan_flat_dt(int (*it)(unsigned long node,
 				     void *data),
 			   void *data)
 {
-	unsigned long p = ((unsigned long)initial_boot_params) +
-		be32_to_cpu(initial_boot_params->off_dt_struct);
-	int rc = 0;
-	int depth = -1;
-
-	do {
-		u32 tag = be32_to_cpup((__be32 *)p);
-		const char *pathp;
-
-		p += 4;
-		if (tag == OF_DT_END_NODE) {
-			depth--;
-			continue;
-		}
-		if (tag == OF_DT_NOP)
-			continue;
-		if (tag == OF_DT_END)
-			break;
-		if (tag == OF_DT_PROP) {
-			u32 sz = be32_to_cpup((__be32 *)p);
-			p += 8;
-			if (be32_to_cpu(initial_boot_params->version) < 0x10)
-				p = ALIGN(p, sz >= 8 ? 8 : 4);
-			p += sz;
-			p = ALIGN(p, 4);
-			continue;
-		}
-		if (tag != OF_DT_BEGIN_NODE) {
-			pr_err("Invalid tag %x in flat device tree!\n", tag);
-			return -EINVAL;
-		}
-		depth++;
-		pathp = (char *)p;
-		p = ALIGN(p + strlen(pathp) + 1, 4);
+	const void *blob = initial_boot_params;
+	const char *pathp;
+	int offset, rc = 0, depth = -1;
+
+        for (offset = fdt_next_node(blob, -1, &depth);
+             offset >= 0 && depth >= 0 && !rc;
+             offset = fdt_next_node(blob, offset, &depth)) {
+
+		pathp = fdt_get_name(blob, offset, NULL);
 		if (*pathp == '/')
 			pathp = kbasename(pathp);
-		rc = it(p, pathp, depth, data);
-		if (rc != 0)
-			break;
-	} while (1);
-
+		rc = it(offset, pathp, depth, data);
+	}
 	return rc;
 }
 
@@ -627,14 +551,15 @@ int __init of_scan_flat_dt(int (*it)(unsigned long node,
  */
 unsigned long __init of_get_flat_dt_root(void)
 {
-	unsigned long p = ((unsigned long)initial_boot_params) +
-		be32_to_cpu(initial_boot_params->off_dt_struct);
-
-	while (be32_to_cpup((__be32 *)p) == OF_DT_NOP)
-		p += 4;
-	BUG_ON(be32_to_cpup((__be32 *)p) != OF_DT_BEGIN_NODE);
-	p += 4;
-	return ALIGN(p + strlen((char *)p) + 1, 4);
+	return 0;
+}
+
+/**
+ * of_get_flat_dt_size - Return the total size of the FDT
+ */
+int __init of_get_flat_dt_size(void)
+{
+	return fdt_totalsize(initial_boot_params);
 }
 
 /**
@@ -643,10 +568,10 @@ unsigned long __init of_get_flat_dt_root(void)
  * This function can be used within scan_flattened_dt callback to get
  * access to properties
  */
-void *__init of_get_flat_dt_prop(unsigned long node, const char *name,
-				 unsigned long *size)
+const void *__init of_get_flat_dt_prop(unsigned long node, const char *name,
+				       int *size)
 {
-	return of_fdt_get_property(initial_boot_params, node, name, size);
+	return fdt_getprop(initial_boot_params, node, name, size);
 }
 
 /**
@@ -676,73 +601,6 @@ struct fdt_scan_status {
 	void *data;
 };
 
-/**
- * fdt_scan_node_by_path - iterator for of_scan_flat_dt_by_path function
- */
-static int __init fdt_scan_node_by_path(unsigned long node, const char *uname,
-					int depth, void *data)
-{
-	struct fdt_scan_status *st = data;
-
-	/*
-	 * if scan at the requested fdt node has been completed,
-	 * return -ENXIO to abort further scanning
-	 */
-	if (depth <= st->depth)
-		return -ENXIO;
-
-	/* requested fdt node has been found, so call iterator function */
-	if (st->found)
-		return st->iterator(node, uname, depth, st->data);
-
-	/* check if scanning automata is entering next level of fdt nodes */
-	if (depth == st->depth + 1 &&
-	    strncmp(st->name, uname, st->namelen) == 0 &&
-	    uname[st->namelen] == 0) {
-		st->depth += 1;
-		if (st->name[st->namelen] == 0) {
-			st->found = 1;
-		} else {
-			const char *next = st->name + st->namelen + 1;
-			st->name = next;
-			st->namelen = strcspn(next, "/");
-		}
-		return 0;
-	}
-
-	/* scan next fdt node */
-	return 0;
-}
-
-/**
- * of_scan_flat_dt_by_path - scan flattened tree blob and call callback on each
- *			     child of the given path.
- * @path: path to start searching for children
- * @it: callback function
- * @data: context data pointer
- *
- * This function is used to scan the flattened device-tree starting from the
- * node given by path. It is used to extract information (like reserved
- * memory), which is required on ealy boot before we can unflatten the tree.
- */
-int __init of_scan_flat_dt_by_path(const char *path,
-	int (*it)(unsigned long node, const char *name, int depth, void *data),
-	void *data)
-{
-	struct fdt_scan_status st = {path, 0, -1, 0, it, data};
-	int ret = 0;
-
-	if (initial_boot_params)
-                ret = of_scan_flat_dt(fdt_scan_node_by_path, &st);
-
-	if (!st.found)
-		return -ENOENT;
-	else if (ret == -ENXIO)	/* scan has been completed */
-		return 0;
-	else
-		return ret;
-}
-
 const char * __init of_flat_dt_get_machine_name(void)
 {
 	const char *name;
@@ -782,7 +640,7 @@ const void * __init of_flat_dt_match_machine(const void *default_match,
 	}
 	if (!best_data) {
 		const char *prop;
-		long size;
+		int size;
 
 		pr_err("\n unrecognized device tree list:\n[ ");
 
@@ -811,8 +669,8 @@ const void * __init of_flat_dt_match_machine(const void *default_match,
 static void __init early_init_dt_check_for_initrd(unsigned long node)
 {
 	u64 start, end;
-	unsigned long len;
-	__be32 *prop;
+	int len;
+	const __be32 *prop;
 
 	pr_debug("Looking for initrd properties... ");
 
@@ -839,13 +697,68 @@ static inline void early_init_dt_check_for_initrd(unsigned long node)
 }
 #endif /* CONFIG_BLK_DEV_INITRD */
 
+#ifdef CONFIG_SERIAL_EARLYCON
+extern struct of_device_id __earlycon_of_table[];
+
+int __init early_init_dt_scan_chosen_serial(void)
+{
+	int offset;
+	const char *p;
+	int l;
+	const struct of_device_id *match = __earlycon_of_table;
+	const void *fdt = initial_boot_params;
+
+	offset = fdt_path_offset(fdt, "/chosen");
+	if (offset < 0)
+		offset = fdt_path_offset(fdt, "/chosen@0");
+	if (offset < 0)
+		return -ENOENT;
+
+	p = fdt_getprop(fdt, offset, "stdout-path", &l);
+	if (!p)
+		p = fdt_getprop(fdt, offset, "linux,stdout-path", &l);
+	if (!p || !l)
+		return -ENOENT;
+
+	/* Get the node specified by stdout-path */
+	offset = fdt_path_offset(fdt, p);
+	if (offset < 0)
+		return -ENODEV;
+
+	while (match->compatible) {
+		unsigned long addr;
+		if (fdt_node_check_compatible(fdt, offset, match->compatible)) {
+			match++;
+			continue;
+		}
+
+		addr = fdt_translate_address(fdt, offset);
+		if (!addr)
+			return -ENXIO;
+
+		of_setup_earlycon(addr, match->data);
+		return 0;
+	}
+	return -ENODEV;
+}
+
+static int __init setup_of_earlycon(char *buf)
+{
+	if (buf)
+		return 0;
+
+	return early_init_dt_scan_chosen_serial();
+}
+early_param("earlycon", setup_of_earlycon);
+#endif
+
 /**
  * early_init_dt_scan_root - fetch the top level address and size cells
  */
 int __init early_init_dt_scan_root(unsigned long node, const char *uname,
 				   int depth, void *data)
 {
-	__be32 *prop;
+	const __be32 *prop;
 
 	if (depth != 0)
 		return 0;
@@ -867,9 +780,9 @@ int __init early_init_dt_scan_root(unsigned long node, const char *uname,
 	return 1;
 }
 
-u64 __init dt_mem_next_cell(int s, __be32 **cellp)
+u64 __init dt_mem_next_cell(int s, const __be32 **cellp)
 {
-	__be32 *p = *cellp;
+	const __be32 *p = *cellp;
 
 	*cellp = p + s;
 	return of_read_number(p, s);
@@ -881,9 +794,9 @@ u64 __init dt_mem_next_cell(int s, __be32 **cellp)
 int __init early_init_dt_scan_memory(unsigned long node, const char *uname,
 				     int depth, void *data)
 {
-	char *type = of_get_flat_dt_prop(node, "device_type", NULL);
-	__be32 *reg, *endp;
-	unsigned long l;
+	const char *type = of_get_flat_dt_prop(node, "device_type", NULL);
+	const __be32 *reg, *endp;
+	int l;
 
 	/* We are scanning "memory" nodes only */
 	if (type == NULL) {
@@ -891,7 +804,7 @@ int __init early_init_dt_scan_memory(unsigned long node, const char *uname,
 		 * The longtrail doesn't have a device_type on the
 		 * /memory node, so look for the node called /memory@0.
 		 */
-		if (depth != 1 || strcmp(uname, "memory@0") != 0)
+		if (!IS_ENABLED(CONFIG_PPC32) || depth != 1 || strcmp(uname, "memory@0") != 0)
 			return 0;
 	} else if (strcmp(type, "memory") != 0)
 		return 0;
@@ -904,7 +817,7 @@ int __init early_init_dt_scan_memory(unsigned long node, const char *uname,
 
 	endp = reg + (l / sizeof(__be32));
 
-	pr_debug("memory scan node %s, reg size %ld, data: %x %x %x %x,\n",
+	pr_debug("memory scan node %s, reg size %d, data: %x %x %x %x,\n",
 	    uname, l, reg[0], reg[1], reg[2], reg[3]);
 
 	while ((endp - reg) >= (dt_root_addr_cells + dt_root_size_cells)) {
@@ -927,8 +840,8 @@ int __init early_init_dt_scan_memory(unsigned long node, const char *uname,
 int __init early_init_dt_scan_chosen(unsigned long node, const char *uname,
 				     int depth, void *data)
 {
-	unsigned long l;
-	char *p;
+	int l;
+	const char *p;
 
 	pr_debug("search \"chosen\", depth: %d, uname: %s\n", depth, uname);
 
@@ -1003,8 +916,8 @@ void * __init __weak early_init_dt_alloc_memory_arch(u64 size, u64 align)
 int __init __weak early_init_dt_reserve_memory_arch(phys_addr_t base,
 					phys_addr_t size, bool nomap)
 {
-	pr_err("Reserved memory not supported, ignoring range 0x%llx - 0x%llx%s\n",
-		  base, size, nomap ? " (nomap)" : "");
+	pr_err("Reserved memory not supported, ignoring range 0x%pa - 0x%pa%s\n",
+		  &base, &size, nomap ? " (nomap)" : "");
 	return -ENOSYS;
 }
 #endif
@@ -1018,7 +931,7 @@ bool __init early_init_dt_scan(void *params)
 	initial_boot_params = params;
 
 	/* check device tree validity */
-	if (be32_to_cpu(initial_boot_params->magic) != OF_DT_HEADER) {
+	if (fdt_check_header(params)) {
 		initial_boot_params = NULL;
 		return false;
 	}
@@ -1073,9 +986,9 @@ void __init unflatten_and_copy_device_tree(void)
 		return;
 	}
 
-	size = __be32_to_cpu(initial_boot_params->totalsize);
+	size = fdt_totalsize(initial_boot_params);
 	dt = early_init_dt_alloc_memory_arch(size,
-		__alignof__(struct boot_param_header));
+					     roundup_pow_of_two(FDT_V17_SIZE));
 
 	if (dt) {
 		memcpy(dt, initial_boot_params, size);
@@ -1084,4 +997,27 @@ void __init unflatten_and_copy_device_tree(void)
 	unflatten_device_tree();
 }
 
+#if defined(CONFIG_DEBUG_FS) && defined(DEBUG)
+static struct debugfs_blob_wrapper flat_dt_blob;
+
+static int __init of_flat_dt_debugfs_export_fdt(void)
+{
+	struct dentry *d = debugfs_create_dir("device-tree", NULL);
+
+	if (!d)
+		return -ENOENT;
+
+	flat_dt_blob.data = initial_boot_params;
+	flat_dt_blob.size = fdt_totalsize(initial_boot_params);
+
+	d = debugfs_create_blob("flat-device-tree", S_IFREG | S_IRUSR,
+				d, &flat_dt_blob);
+	if (!d)
+		return -ENOENT;
+
+	return 0;
+}
+module_init(of_flat_dt_debugfs_export_fdt);
+#endif
+
 #endif /* CONFIG_OF_EARLY_FLATTREE */
diff --git a/drivers/of/fdt_address.c b/drivers/of/fdt_address.c
new file mode 100644
index 00000000000..8d3dc6fbdb7
--- /dev/null
+++ b/drivers/of/fdt_address.c
@@ -0,0 +1,241 @@
+/*
+ * FDT Address translation based on u-boot fdt_support.c which in turn was
+ * based on the kernel unflattened DT address translation code.
+ *
+ * (C) Copyright 2007
+ * Gerald Van Baren, Custom IDEAS, vanbaren@cideas.com
+ *
+ * Copyright 2010-2011 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, or (at your option)
+ * any later version.
+ */
+#include <linux/kernel.h>
+#include <linux/libfdt.h>
+#include <linux/of.h>
+#include <linux/of_fdt.h>
+#include <linux/sizes.h>
+
+/* Max address size we deal with */
+#define OF_MAX_ADDR_CELLS	4
+#define OF_CHECK_COUNTS(na, ns)	((na) > 0 && (na) <= OF_MAX_ADDR_CELLS && \
+			(ns) > 0)
+
+/* Debug utility */
+#ifdef DEBUG
+static void __init of_dump_addr(const char *s, const __be32 *addr, int na)
+{
+	pr_debug("%s", s);
+	while(na--)
+		pr_cont(" %08x", *(addr++));
+	pr_debug("\n");
+}
+#else
+static void __init of_dump_addr(const char *s, const __be32 *addr, int na) { }
+#endif
+
+/* Callbacks for bus specific translators */
+struct of_bus {
+	void		(*count_cells)(const void *blob, int parentoffset,
+				int *addrc, int *sizec);
+	u64		(*map)(__be32 *addr, const __be32 *range,
+				int na, int ns, int pna);
+	int		(*translate)(__be32 *addr, u64 offset, int na);
+};
+
+/* Default translator (generic bus) */
+static void __init fdt_bus_default_count_cells(const void *blob, int parentoffset,
+					       int *addrc, int *sizec)
+{
+	const __be32 *prop;
+
+	if (addrc) {
+		prop = fdt_getprop(blob, parentoffset, "#address-cells", NULL);
+		if (prop)
+			*addrc = be32_to_cpup(prop);
+		else
+			*addrc = dt_root_addr_cells;
+	}
+
+	if (sizec) {
+		prop = fdt_getprop(blob, parentoffset, "#size-cells", NULL);
+		if (prop)
+			*sizec = be32_to_cpup(prop);
+		else
+			*sizec = dt_root_size_cells;
+	}
+}
+
+static u64 __init fdt_bus_default_map(__be32 *addr, const __be32 *range,
+				      int na, int ns, int pna)
+{
+	u64 cp, s, da;
+
+	cp = of_read_number(range, na);
+	s  = of_read_number(range + na + pna, ns);
+	da = of_read_number(addr, na);
+
+	pr_debug("FDT: default map, cp=%llx, s=%llx, da=%llx\n",
+	    cp, s, da);
+
+	if (da < cp || da >= (cp + s))
+		return OF_BAD_ADDR;
+	return da - cp;
+}
+
+static int __init fdt_bus_default_translate(__be32 *addr, u64 offset, int na)
+{
+	u64 a = of_read_number(addr, na);
+	memset(addr, 0, na * 4);
+	a += offset;
+	if (na > 1)
+		addr[na - 2] = cpu_to_fdt32(a >> 32);
+	addr[na - 1] = cpu_to_fdt32(a & 0xffffffffu);
+
+	return 0;
+}
+
+/* Array of bus specific translators */
+static const struct of_bus of_busses[] __initconst = {
+	/* Default */
+	{
+		.count_cells = fdt_bus_default_count_cells,
+		.map = fdt_bus_default_map,
+		.translate = fdt_bus_default_translate,
+	},
+};
+
+static int __init fdt_translate_one(const void *blob, int parent,
+				    const struct of_bus *bus,
+				    const struct of_bus *pbus, __be32 *addr,
+				    int na, int ns, int pna, const char *rprop)
+{
+	const __be32 *ranges;
+	int rlen;
+	int rone;
+	u64 offset = OF_BAD_ADDR;
+
+	ranges = fdt_getprop(blob, parent, rprop, &rlen);
+	if (!ranges)
+		return 1;
+	if (rlen == 0) {
+		offset = of_read_number(addr, na);
+		memset(addr, 0, pna * 4);
+		pr_debug("FDT: empty ranges, 1:1 translation\n");
+		goto finish;
+	}
+
+	pr_debug("FDT: walking ranges...\n");
+
+	/* Now walk through the ranges */
+	rlen /= 4;
+	rone = na + pna + ns;
+	for (; rlen >= rone; rlen -= rone, ranges += rone) {
+		offset = bus->map(addr, ranges, na, ns, pna);
+		if (offset != OF_BAD_ADDR)
+			break;
+	}
+	if (offset == OF_BAD_ADDR) {
+		pr_debug("FDT: not found !\n");
+		return 1;
+	}
+	memcpy(addr, ranges + na, 4 * pna);
+
+ finish:
+	of_dump_addr("FDT: parent translation for:", addr, pna);
+	pr_debug("FDT: with offset: %llx\n", offset);
+
+	/* Translate it into parent bus space */
+	return pbus->translate(addr, offset, pna);
+}
+
+/*
+ * Translate an address from the device-tree into a CPU physical address,
+ * this walks up the tree and applies the various bus mappings on the
+ * way.
+ *
+ * Note: We consider that crossing any level with #size-cells == 0 to mean
+ * that translation is impossible (that is we are not dealing with a value
+ * that can be mapped to a cpu physical address). This is not really specified
+ * that way, but this is traditionally the way IBM at least do things
+ */
+u64 __init fdt_translate_address(const void *blob, int node_offset)
+{
+	int parent, len;
+	const struct of_bus *bus, *pbus;
+	const __be32 *reg;
+	__be32 addr[OF_MAX_ADDR_CELLS];
+	int na, ns, pna, pns;
+	u64 result = OF_BAD_ADDR;
+
+	pr_debug("FDT: ** translation for device %s **\n",
+		 fdt_get_name(blob, node_offset, NULL));
+
+	reg = fdt_getprop(blob, node_offset, "reg", &len);
+	if (!reg) {
+		pr_err("FDT: warning: device tree node '%s' has no address.\n",
+			fdt_get_name(blob, node_offset, NULL));
+		goto bail;
+	}
+
+	/* Get parent & match bus type */
+	parent = fdt_parent_offset(blob, node_offset);
+	if (parent < 0)
+		goto bail;
+	bus = &of_busses[0];
+
+	/* Cound address cells & copy address locally */
+	bus->count_cells(blob, parent, &na, &ns);
+	if (!OF_CHECK_COUNTS(na, ns)) {
+		pr_err("FDT: Bad cell count for %s\n",
+		       fdt_get_name(blob, node_offset, NULL));
+		goto bail;
+	}
+	memcpy(addr, reg, na * 4);
+
+	pr_debug("FDT: bus (na=%d, ns=%d) on %s\n",
+		 na, ns, fdt_get_name(blob, parent, NULL));
+	of_dump_addr("OF: translating address:", addr, na);
+
+	/* Translate */
+	for (;;) {
+		/* Switch to parent bus */
+		node_offset = parent;
+		parent = fdt_parent_offset(blob, node_offset);
+
+		/* If root, we have finished */
+		if (parent < 0) {
+			pr_debug("FDT: reached root node\n");
+			result = of_read_number(addr, na);
+			break;
+		}
+
+		/* Get new parent bus and counts */
+		pbus = &of_busses[0];
+		pbus->count_cells(blob, parent, &pna, &pns);
+		if (!OF_CHECK_COUNTS(pna, pns)) {
+			pr_err("FDT: Bad cell count for %s\n",
+				fdt_get_name(blob, node_offset, NULL));
+			break;
+		}
+
+		pr_debug("FDT: parent bus (na=%d, ns=%d) on %s\n",
+			 pna, pns, fdt_get_name(blob, parent, NULL));
+
+		/* Apply bus translation */
+		if (fdt_translate_one(blob, node_offset, bus, pbus,
+					addr, na, ns, pna, "ranges"))
+			break;
+
+		/* Complete the move up one level */
+		na = pna;
+		ns = pns;
+		bus = pbus;
+
+		of_dump_addr("FDT: one level translation:", addr, na);
+	}
+ bail:
+	return result;
+}
diff --git a/drivers/of/irq.c b/drivers/of/irq.c
index 5aeb8941135..3e06a699352 100644
--- a/drivers/of/irq.c
+++ b/drivers/of/irq.c
@@ -406,6 +406,28 @@ int of_irq_get(struct device_node *dev, int index)
 }
 
 /**
+ * of_irq_get_byname - Decode a node's IRQ and return it as a Linux irq number
+ * @dev: pointer to device tree node
+ * @name: irq name
+ *
+ * Returns Linux irq number on success, or -EPROBE_DEFER if the irq domain
+ * is not yet created, or error code in case of any other failure.
+ */
+int of_irq_get_byname(struct device_node *dev, const char *name)
+{
+	int index;
+
+	if (unlikely(!name))
+		return -EINVAL;
+
+	index = of_property_match_string(dev, "interrupt-names", name);
+	if (index < 0)
+		return index;
+
+	return of_irq_get(dev, index);
+}
+
+/**
  * of_irq_count - Count the number of IRQs a node uses
  * @dev: pointer to device tree node
  */
diff --git a/drivers/of/of_pci_irq.c b/drivers/of/of_pci_irq.c
index 8736bc7676c..1710d9dc7fc 100644
--- a/drivers/of/of_pci_irq.c
+++ b/drivers/of/of_pci_irq.c
@@ -18,8 +18,6 @@ int of_irq_parse_pci(const struct pci_dev *pdev, struct of_phandle_args *out_irq
 {
 	struct device_node *dn, *ppnode;
 	struct pci_dev *ppdev;
-	u32 lspec;
-	__be32 lspec_be;
 	__be32 laddr[3];
 	u8 pin;
 	int rc;
@@ -46,7 +44,6 @@ int of_irq_parse_pci(const struct pci_dev *pdev, struct of_phandle_args *out_irq
 		return -ENODEV;
 
 	/* Now we walk up the PCI tree */
-	lspec = pin;
 	for (;;) {
 		/* Get the pci_dev of our parent */
 		ppdev = pdev->bus->self;
@@ -80,14 +77,13 @@ int of_irq_parse_pci(const struct pci_dev *pdev, struct of_phandle_args *out_irq
 		/* We can only get here if we hit a P2P bridge with no node,
 		 * let's do standard swizzling and try again
 		 */
-		lspec = pci_swizzle_interrupt_pin(pdev, lspec);
+		pin = pci_swizzle_interrupt_pin(pdev, pin);
 		pdev = ppdev;
 	}
 
 	out_irq->np = ppnode;
 	out_irq->args_count = 1;
-	out_irq->args[0] = lspec;
-	lspec_be = cpu_to_be32(lspec);
+	out_irq->args[0] = pin;
 	laddr[0] = cpu_to_be32((pdev->bus->number << 16) | (pdev->devfn << 8));
 	laddr[1] = laddr[2] = cpu_to_be32(0);
 	return of_irq_parse_raw(laddr, out_irq);
diff --git a/drivers/of/of_reserved_mem.c b/drivers/of/of_reserved_mem.c
index daaaf935911..632aae86137 100644
--- a/drivers/of/of_reserved_mem.c
+++ b/drivers/of/of_reserved_mem.c
@@ -95,8 +95,8 @@ static int __init __reserved_mem_alloc_size(unsigned long node,
 	int t_len = (dt_root_addr_cells + dt_root_size_cells) * sizeof(__be32);
 	phys_addr_t start = 0, end = 0;
 	phys_addr_t base = 0, align = 0, size;
-	unsigned long len;
-	__be32 *prop;
+	int len;
+	const __be32 *prop;
 	int nomap;
 	int ret;
 
@@ -188,7 +188,7 @@ static int __init __reserved_mem_init_node(struct reserved_mem *rmem)
 		if (!of_flat_dt_is_compatible(rmem->fdt_node, compat))
 			continue;
 
-		if (initfn(rmem, rmem->fdt_node, rmem->name) == 0) {
+		if (initfn(rmem) == 0) {
 			pr_info("Reserved memory: initialized node %s, compatible id %s\n",
 				rmem->name, compat);
 			return 0;
diff --git a/drivers/of/platform.c b/drivers/of/platform.c
index bd47fbc53dc..6c48d73a7fd 100644
--- a/drivers/of/platform.c
+++ b/drivers/of/platform.c
@@ -51,10 +51,6 @@ struct platform_device *of_find_device_by_node(struct device_node *np)
 }
 EXPORT_SYMBOL(of_find_device_by_node);
 
-#if defined(CONFIG_PPC_DCR)
-#include <asm/dcr.h>
-#endif
-
 #ifdef CONFIG_OF_ADDRESS
 /*
  * The following routines scan a subtree and registers a device for
@@ -68,66 +64,35 @@ EXPORT_SYMBOL(of_find_device_by_node);
  * of_device_make_bus_id - Use the device node data to assign a unique name
  * @dev: pointer to device structure that is linked to a device tree node
  *
- * This routine will first try using either the dcr-reg or the reg property
- * value to derive a unique name.  As a last resort it will use the node
- * name followed by a unique number.
+ * This routine will first try using the translated bus address to
+ * derive a unique name. If it cannot, then it will prepend names from
+ * parent nodes until a unique name can be derived.
  */
 void of_device_make_bus_id(struct device *dev)
 {
-	static atomic_t bus_no_reg_magic;
 	struct device_node *node = dev->of_node;
 	const __be32 *reg;
 	u64 addr;
-	const __be32 *addrp;
-	int magic;
 
-#ifdef CONFIG_PPC_DCR
-	/*
-	 * If it's a DCR based device, use 'd' for native DCRs
-	 * and 'D' for MMIO DCRs.
-	 */
-	reg = of_get_property(node, "dcr-reg", NULL);
-	if (reg) {
-#ifdef CONFIG_PPC_DCR_NATIVE
-		dev_set_name(dev, "d%x.%s", *reg, node->name);
-#else /* CONFIG_PPC_DCR_NATIVE */
-		u64 addr = of_translate_dcr_address(node, *reg, NULL);
-		if (addr != OF_BAD_ADDR) {
-			dev_set_name(dev, "D%llx.%s",
-				     (unsigned long long)addr, node->name);
+	/* Construct the name, using parent nodes if necessary to ensure uniqueness */
+	while (node->parent) {
+		/*
+		 * If the address can be translated, then that is as much
+		 * uniqueness as we need. Make it the first component and return
+		 */
+		reg = of_get_property(node, "reg", NULL);
+		if (reg && (addr = of_translate_address(node, reg)) != OF_BAD_ADDR) {
+			dev_set_name(dev, dev_name(dev) ? "%llx.%s:%s" : "%llx.%s",
+				     (unsigned long long)addr, node->name,
+				     dev_name(dev));
 			return;
 		}
-#endif /* !CONFIG_PPC_DCR_NATIVE */
-	}
-#endif /* CONFIG_PPC_DCR */
 
-	/*
-	 * For MMIO, get the physical address
-	 */
-	reg = of_get_property(node, "reg", NULL);
-	if (reg) {
-		if (of_can_translate_address(node)) {
-			addr = of_translate_address(node, reg);
-		} else {
-			addrp = of_get_address(node, 0, NULL, NULL);
-			if (addrp)
-				addr = of_read_number(addrp, 1);
-			else
-				addr = OF_BAD_ADDR;
-		}
-		if (addr != OF_BAD_ADDR) {
-			dev_set_name(dev, "%llx.%s",
-				     (unsigned long long)addr, node->name);
-			return;
-		}
+		/* format arguments only used if dev_name() resolves to NULL */
+		dev_set_name(dev, dev_name(dev) ? "%s:%s" : "%s",
+			     strrchr(node->full_name, '/') + 1, dev_name(dev));
+		node = node->parent;
 	}
-
-	/*
-	 * No BusID, use the node name and add a globally incremented
-	 * counter (and pray...)
-	 */
-	magic = atomic_add_return(1, &bus_no_reg_magic);
-	dev_set_name(dev, "%s.%d", node->name, magic - 1);
 }
 
 /**
@@ -149,9 +114,8 @@ struct platform_device *of_device_alloc(struct device_node *np,
 		return NULL;
 
 	/* count the io and irq resources */
-	if (of_can_translate_address(np))
-		while (of_address_to_resource(np, num_reg, &temp_res) == 0)
-			num_reg++;
+	while (of_address_to_resource(np, num_reg, &temp_res) == 0)
+		num_reg++;
 	num_irq = of_irq_count(np);
 
 	/* Populate the resource table */
@@ -174,9 +138,6 @@ struct platform_device *of_device_alloc(struct device_node *np,
 	}
 
 	dev->dev.of_node = of_node_get(np);
-#if defined(CONFIG_MICROBLAZE)
-	dev->dev.dma_mask = &dev->archdata.dma_mask;
-#endif
 	dev->dev.parent = parent;
 
 	if (bus_id)
@@ -189,6 +150,64 @@ struct platform_device *of_device_alloc(struct device_node *np,
 EXPORT_SYMBOL(of_device_alloc);
 
 /**
+ * of_dma_configure - Setup DMA configuration
+ * @dev:	Device to apply DMA configuration
+ *
+ * Try to get devices's DMA configuration from DT and update it
+ * accordingly.
+ *
+ * In case if platform code need to use own special DMA configuration,it
+ * 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)
+{
+	u64 dma_addr, paddr, size;
+	int ret;
+	struct device *dev = &pdev->dev;
+
+#if defined(CONFIG_MICROBLAZE)
+	pdev->archdata.dma_mask = 0xffffffffUL;
+#endif
+
+	/*
+	 * Set default dma-mask to 32 bit. Drivers are expected to setup
+	 * the correct supported dma_mask.
+	 */
+	dev->coherent_dma_mask = DMA_BIT_MASK(32);
+
+	/*
+	 * Set it to coherent_dma_mask by default if the architecture
+	 * code has not set it.
+	 */
+	if (!dev->dma_mask)
+		dev->dma_mask = &dev->coherent_dma_mask;
+
+	/*
+	 * if dma-coherent property exist, call arch hook to setup
+	 * dma coherent operations.
+	 */
+	if (of_dma_is_coherent(dev->of_node)) {
+		set_arch_dma_coherent_ops(dev);
+		dev_dbg(dev, "device is dma coherent\n");
+	}
+
+	/*
+	 * if dma-ranges property doesn't exist - just return else
+	 * setup the dma offset
+	 */
+	ret = of_dma_get_range(dev->of_node, &dma_addr, &paddr, &size);
+	if (ret < 0) {
+		dev_dbg(dev, "no dma range information to setup\n");
+		return;
+	}
+
+	/* DMA ranges found. Calculate and set dma_pfn_offset */
+	dev->dma_pfn_offset = PFN_DOWN(paddr - dma_addr);
+	dev_dbg(dev, "dma_pfn_offset(%#08lx)\n", dev->dma_pfn_offset);
+}
+
+/**
  * of_platform_device_create_pdata - Alloc, initialize and register an of_device
  * @np: pointer to node to create device for
  * @bus_id: name to assign device
@@ -206,19 +225,15 @@ static struct platform_device *of_platform_device_create_pdata(
 {
 	struct platform_device *dev;
 
-	if (!of_device_is_available(np))
+	if (!of_device_is_available(np) ||
+	    of_node_test_and_set_flag(np, OF_POPULATED))
 		return NULL;
 
 	dev = of_device_alloc(np, bus_id, parent);
 	if (!dev)
-		return NULL;
+		goto err_clear_flag;
 
-#if defined(CONFIG_MICROBLAZE)
-	dev->archdata.dma_mask = 0xffffffffUL;
-#endif
-	dev->dev.coherent_dma_mask = DMA_BIT_MASK(32);
-	if (!dev->dev.dma_mask)
-		dev->dev.dma_mask = &dev->dev.coherent_dma_mask;
+	of_dma_configure(dev);
 	dev->dev.bus = &platform_bus_type;
 	dev->dev.platform_data = platform_data;
 
@@ -229,10 +244,14 @@ static struct platform_device *of_platform_device_create_pdata(
 
 	if (of_device_add(dev) != 0) {
 		platform_device_put(dev);
-		return NULL;
+		goto err_clear_flag;
 	}
 
 	return dev;
+
+err_clear_flag:
+	of_node_clear_flag(np, OF_POPULATED);
+	return NULL;
 }
 
 /**
@@ -264,14 +283,15 @@ static struct amba_device *of_amba_device_create(struct device_node *node,
 
 	pr_debug("Creating amba device %s\n", node->full_name);
 
-	if (!of_device_is_available(node))
+	if (!of_device_is_available(node) ||
+	    of_node_test_and_set_flag(node, OF_POPULATED))
 		return NULL;
 
 	dev = amba_device_alloc(NULL, 0, 0);
 	if (!dev) {
 		pr_err("%s(): amba_device_alloc() failed for %s\n",
 		       __func__, node->full_name);
-		return NULL;
+		goto err_clear_flag;
 	}
 
 	/* setup generic device info */
@@ -311,6 +331,8 @@ static struct amba_device *of_amba_device_create(struct device_node *node,
 
 err_free:
 	amba_device_put(dev);
+err_clear_flag:
+	of_node_clear_flag(node, OF_POPULATED);
 	return NULL;
 }
 #else /* CONFIG_ARM_AMBA */
@@ -487,4 +509,60 @@ int of_platform_populate(struct device_node *root,
 	return rc;
 }
 EXPORT_SYMBOL_GPL(of_platform_populate);
+
+static int of_platform_device_destroy(struct device *dev, void *data)
+{
+	bool *children_left = data;
+
+	/* Do not touch devices not populated from the device tree */
+	if (!dev->of_node || !of_node_check_flag(dev->of_node, OF_POPULATED)) {
+		*children_left = true;
+		return 0;
+	}
+
+	/* Recurse, but don't touch this device if it has any children left */
+	if (of_platform_depopulate(dev) != 0) {
+		*children_left = true;
+		return 0;
+	}
+
+	if (dev->bus == &platform_bus_type)
+		platform_device_unregister(to_platform_device(dev));
+#ifdef CONFIG_ARM_AMBA
+	else if (dev->bus == &amba_bustype)
+		amba_device_unregister(to_amba_device(dev));
+#endif
+	else {
+		*children_left = true;
+		return 0;
+	}
+
+	of_node_clear_flag(dev->of_node, OF_POPULATED);
+
+	return 0;
+}
+
+/**
+ * of_platform_depopulate() - Remove devices populated from device tree
+ * @parent: device which childred will be removed
+ *
+ * Complementary to of_platform_populate(), this function removes children
+ * of the given device (and, recurrently, their children) that have been
+ * created from their respective device tree nodes (and only those,
+ * leaving others - eg. manually created - unharmed).
+ *
+ * Returns 0 when all children devices have been removed or
+ * -EBUSY when some children remained.
+ */
+int of_platform_depopulate(struct device *parent)
+{
+	bool children_left = false;
+
+	device_for_each_child(parent, &children_left,
+			      of_platform_device_destroy);
+
+	return children_left ? -EBUSY : 0;
+}
+EXPORT_SYMBOL_GPL(of_platform_depopulate);
+
 #endif /* CONFIG_OF_ADDRESS */
diff --git a/drivers/of/selftest.c b/drivers/of/selftest.c
index fe70b86bcff..077314eebb9 100644
--- a/drivers/of/selftest.c
+++ b/drivers/of/selftest.c
@@ -31,6 +31,51 @@ static struct selftest_results {
 	} \
 }
 
+static void __init of_selftest_find_node_by_name(void)
+{
+	struct device_node *np;
+
+	np = of_find_node_by_path("/testcase-data");
+	selftest(np && !strcmp("/testcase-data", np->full_name),
+		"find /testcase-data failed\n");
+	of_node_put(np);
+
+	/* Test if trailing '/' works */
+	np = of_find_node_by_path("/testcase-data/");
+	selftest(!np, "trailing '/' on /testcase-data/ should fail\n");
+
+	np = of_find_node_by_path("/testcase-data/phandle-tests/consumer-a");
+	selftest(np && !strcmp("/testcase-data/phandle-tests/consumer-a", np->full_name),
+		"find /testcase-data/phandle-tests/consumer-a failed\n");
+	of_node_put(np);
+
+	np = of_find_node_by_path("testcase-alias");
+	selftest(np && !strcmp("/testcase-data", np->full_name),
+		"find testcase-alias failed\n");
+	of_node_put(np);
+
+	/* Test if trailing '/' works on aliases */
+	np = of_find_node_by_path("testcase-alias/");
+	selftest(!np, "trailing '/' on testcase-alias/ should fail\n");
+
+	np = of_find_node_by_path("testcase-alias/phandle-tests/consumer-a");
+	selftest(np && !strcmp("/testcase-data/phandle-tests/consumer-a", np->full_name),
+		"find testcase-alias/phandle-tests/consumer-a failed\n");
+	of_node_put(np);
+
+	np = of_find_node_by_path("/testcase-data/missing-path");
+	selftest(!np, "non-existent path returned node %s\n", np->full_name);
+	of_node_put(np);
+
+	np = of_find_node_by_path("missing-alias");
+	selftest(!np, "non-existent alias returned node %s\n", np->full_name);
+	of_node_put(np);
+
+	np = of_find_node_by_path("testcase-alias/missing-path");
+	selftest(!np, "non-existent alias with relative path returned node %s\n", np->full_name);
+	of_node_put(np);
+}
+
 static void __init of_selftest_dynamic(void)
 {
 	struct device_node *np;
@@ -431,8 +476,12 @@ static void __init of_selftest_match_node(void)
 static void __init of_selftest_platform_populate(void)
 {
 	int irq;
-	struct device_node *np;
+	struct device_node *np, *child;
 	struct platform_device *pdev;
+	struct of_device_id match[] = {
+		{ .compatible = "test-device", },
+		{}
+	};
 
 	np = of_find_node_by_path("/testcase-data");
 	of_platform_populate(np, of_default_bus_match_table, NULL, NULL);
@@ -440,22 +489,32 @@ static void __init of_selftest_platform_populate(void)
 	/* Test that a missing irq domain returns -EPROBE_DEFER */
 	np = of_find_node_by_path("/testcase-data/testcase-device1");
 	pdev = of_find_device_by_node(np);
-	if (!pdev)
-		selftest(0, "device 1 creation failed\n");
+	selftest(pdev, "device 1 creation failed\n");
+
 	irq = platform_get_irq(pdev, 0);
-	if (irq != -EPROBE_DEFER)
-		selftest(0, "device deferred probe failed - %d\n", irq);
+	selftest(irq == -EPROBE_DEFER, "device deferred probe failed - %d\n", irq);
 
 	/* Test that a parsing failure does not return -EPROBE_DEFER */
 	np = of_find_node_by_path("/testcase-data/testcase-device2");
 	pdev = of_find_device_by_node(np);
-	if (!pdev)
-		selftest(0, "device 2 creation failed\n");
+	selftest(pdev, "device 2 creation failed\n");
 	irq = platform_get_irq(pdev, 0);
-	if (irq >= 0 || irq == -EPROBE_DEFER)
-		selftest(0, "device parsing error failed - %d\n", irq);
+	selftest(irq < 0 && irq != -EPROBE_DEFER, "device parsing error failed - %d\n", irq);
 
-	selftest(1, "passed");
+	np = of_find_node_by_path("/testcase-data/platform-tests");
+	if (!np) {
+		pr_err("No testcase data in device tree\n");
+		return;
+	}
+
+	for_each_child_of_node(np, child) {
+		struct device_node *grandchild;
+		of_platform_populate(child, match, NULL, NULL);
+		for_each_child_of_node(child, grandchild)
+			selftest(of_find_device_by_node(grandchild),
+				 "Could not create device for node '%s'\n",
+				 grandchild->name);
+	}
 }
 
 static int __init of_selftest(void)
@@ -470,6 +529,7 @@ static int __init of_selftest(void)
 	of_node_put(np);
 
 	pr_info("start of selftest - you will see error messages\n");
+	of_selftest_find_node_by_name();
 	of_selftest_dynamic();
 	of_selftest_parse_phandle_with_args();
 	of_selftest_property_match_string();
diff --git a/drivers/of/testcase-data/testcases.dtsi b/drivers/of/testcase-data/testcases.dtsi
index 3a5b75a8e4d..6d8d980ac85 100644
--- a/drivers/of/testcase-data/testcases.dtsi
+++ b/drivers/of/testcase-data/testcases.dtsi
@@ -1,3 +1,4 @@
 #include "tests-phandle.dtsi"
 #include "tests-interrupts.dtsi"
 #include "tests-match.dtsi"
+#include "tests-platform.dtsi"
diff --git a/drivers/of/testcase-data/tests-phandle.dtsi b/drivers/of/testcase-data/tests-phandle.dtsi
index 788a4c24b8f..ce0fe083d40 100644
--- a/drivers/of/testcase-data/tests-phandle.dtsi
+++ b/drivers/of/testcase-data/tests-phandle.dtsi
@@ -1,6 +1,10 @@
 
 / {
-	testcase-data {
+	aliases {
+		testcase-alias = &testcase;
+	};
+
+	testcase: testcase-data {
 		security-password = "password";
 		duplicate-name = "duplicate";
 		duplicate-name { };
diff --git a/drivers/of/testcase-data/tests-platform.dtsi b/drivers/of/testcase-data/tests-platform.dtsi
new file mode 100644
index 00000000000..eb20eeb2b06
--- /dev/null
+++ b/drivers/of/testcase-data/tests-platform.dtsi
@@ -0,0 +1,35 @@
+
+/ {
+	testcase-data {
+		platform-tests {
+			#address-cells = <1>;
+			#size-cells = <0>;
+
+			test-device@0 {
+				compatible = "test-device";
+				reg = <0x0>;
+
+				#address-cells = <1>;
+				#size-cells = <0>;
+
+				dev@100 {
+					compatible = "test-sub-device";
+					reg = <0x100>;
+				};
+			};
+
+			test-device@1 {
+				compatible = "test-device";
+				reg = <0x1>;
+
+				#address-cells = <1>;
+				#size-cells = <0>;
+
+				dev@100 {
+					compatible = "test-sub-device";
+					reg = <0x100>;
+				};
+			};
+		};
+	};
+};
diff --git a/drivers/parport/parport_serial.c b/drivers/parport/parport_serial.c
index ff53314100f..ee932004724 100644
--- a/drivers/parport/parport_serial.c
+++ b/drivers/parport/parport_serial.c
@@ -62,6 +62,7 @@ enum parport_pc_pci_cards {
 	timedia_9079a,
 	timedia_9079b,
 	timedia_9079c,
+	wch_ch353_1s1p,
 	wch_ch353_2s1p,
 	sunix_2s1p,
 };
@@ -148,6 +149,7 @@ static struct parport_pc_pci cards[] = {
 	/* timedia_9079a */             { 1, { { 2, 3 }, } },
 	/* timedia_9079b */             { 1, { { 2, 3 }, } },
 	/* timedia_9079c */             { 1, { { 2, 3 }, } },
+	/* wch_ch353_1s1p*/             { 1, { { 1, -1}, } },
 	/* wch_ch353_2s1p*/             { 1, { { 2, -1}, } },
 	/* sunix_2s1p */                { 1, { { 3, -1 }, } },
 };
@@ -253,6 +255,7 @@ static struct pci_device_id parport_serial_pci_tbl[] = {
 	{ 0x1409, 0x7168, 0x1409, 0xd079, 0, 0, timedia_9079c },
 
 	/* WCH CARDS */
+	{ 0x4348, 0x5053, PCI_ANY_ID, PCI_ANY_ID, 0, 0, wch_ch353_1s1p},
 	{ 0x4348, 0x7053, 0x4348, 0x3253, 0, 0, wch_ch353_2s1p},
 
 	/*
@@ -479,6 +482,12 @@ static struct pciserial_board pci_parport_serial_boards[] = {
 		.base_baud	= 921600,
 		.uart_offset	= 8,
 	},
+	[wch_ch353_1s1p] = {
+		.flags          = FL_BASE0|FL_BASE_BARS,
+		.num_ports      = 1,
+		.base_baud      = 115200,
+		.uart_offset    = 8,
+	},
 	[wch_ch353_2s1p] = {
 		.flags          = FL_BASE0|FL_BASE_BARS,
 		.num_ports      = 2,
diff --git a/drivers/parport/procfs.c b/drivers/parport/procfs.c
index 92ed045a5f9..3b470801a04 100644
--- a/drivers/parport/procfs.c
+++ b/drivers/parport/procfs.c
@@ -31,7 +31,7 @@
 #define PARPORT_MIN_SPINTIME_VALUE 1
 #define PARPORT_MAX_SPINTIME_VALUE 1000
 
-static int do_active_device(ctl_table *table, int write,
+static int do_active_device(struct ctl_table *table, int write,
 		      void __user *result, size_t *lenp, loff_t *ppos)
 {
 	struct parport *port = (struct parport *)table->extra1;
@@ -68,7 +68,7 @@ static int do_active_device(ctl_table *table, int write,
 }
 
 #ifdef CONFIG_PARPORT_1284
-static int do_autoprobe(ctl_table *table, int write,
+static int do_autoprobe(struct ctl_table *table, int write,
 			void __user *result, size_t *lenp, loff_t *ppos)
 {
 	struct parport_device_info *info = table->extra2;
@@ -110,9 +110,9 @@ static int do_autoprobe(ctl_table *table, int write,
 }
 #endif /* IEEE1284.3 support. */
 
-static int do_hardware_base_addr (ctl_table *table, int write,
-				  void __user *result,
-				  size_t *lenp, loff_t *ppos)
+static int do_hardware_base_addr(struct ctl_table *table, int write,
+				 void __user *result,
+				 size_t *lenp, loff_t *ppos)
 {
 	struct parport *port = (struct parport *)table->extra1;
 	char buffer[20];
@@ -138,9 +138,9 @@ static int do_hardware_base_addr (ctl_table *table, int write,
 	return copy_to_user(result, buffer, len) ? -EFAULT : 0;
 }
 
-static int do_hardware_irq (ctl_table *table, int write,
-			    void __user *result,
-			    size_t *lenp, loff_t *ppos)
+static int do_hardware_irq(struct ctl_table *table, int write,
+			   void __user *result,
+			   size_t *lenp, loff_t *ppos)
 {
 	struct parport *port = (struct parport *)table->extra1;
 	char buffer[20];
@@ -166,9 +166,9 @@ static int do_hardware_irq (ctl_table *table, int write,
 	return copy_to_user(result, buffer, len) ? -EFAULT : 0;
 }
 
-static int do_hardware_dma (ctl_table *table, int write,
-			    void __user *result,
-			    size_t *lenp, loff_t *ppos)
+static int do_hardware_dma(struct ctl_table *table, int write,
+			   void __user *result,
+			   size_t *lenp, loff_t *ppos)
 {
 	struct parport *port = (struct parport *)table->extra1;
 	char buffer[20];
@@ -194,9 +194,9 @@ static int do_hardware_dma (ctl_table *table, int write,
 	return copy_to_user(result, buffer, len) ? -EFAULT : 0;
 }
 
-static int do_hardware_modes (ctl_table *table, int write,
-			      void __user *result,
-			      size_t *lenp, loff_t *ppos)
+static int do_hardware_modes(struct ctl_table *table, int write,
+			     void __user *result,
+			     size_t *lenp, loff_t *ppos)
 {
 	struct parport *port = (struct parport *)table->extra1;
 	char buffer[40];
@@ -255,11 +255,11 @@ PARPORT_MAX_SPINTIME_VALUE;
 
 struct parport_sysctl_table {
 	struct ctl_table_header *sysctl_header;
-	ctl_table vars[12];
-	ctl_table device_dir[2];
-	ctl_table port_dir[2];
-	ctl_table parport_dir[2];
-	ctl_table dev_dir[2];
+	struct ctl_table vars[12];
+	struct ctl_table device_dir[2];
+	struct ctl_table port_dir[2];
+	struct ctl_table parport_dir[2];
+	struct ctl_table dev_dir[2];
 };
 
 static const struct parport_sysctl_table parport_sysctl_template = {
@@ -369,12 +369,12 @@ static const struct parport_sysctl_table parport_sysctl_template = {
 struct parport_device_sysctl_table
 {
 	struct ctl_table_header *sysctl_header;
-	ctl_table vars[2];
-	ctl_table device_dir[2];
-	ctl_table devices_root_dir[2];
-	ctl_table port_dir[2];
-	ctl_table parport_dir[2];
-	ctl_table dev_dir[2];
+	struct ctl_table vars[2];
+	struct ctl_table device_dir[2];
+	struct ctl_table devices_root_dir[2];
+	struct ctl_table port_dir[2];
+	struct ctl_table parport_dir[2];
+	struct ctl_table dev_dir[2];
 };
 
 static const struct parport_device_sysctl_table
@@ -422,10 +422,10 @@ parport_device_sysctl_template = {
 struct parport_default_sysctl_table
 {
 	struct ctl_table_header *sysctl_header;
-	ctl_table vars[3];
-        ctl_table default_dir[2];
-	ctl_table parport_dir[2];
-	ctl_table dev_dir[2];
+	struct ctl_table vars[3];
+	struct ctl_table default_dir[2];
+	struct ctl_table parport_dir[2];
+	struct ctl_table dev_dir[2];
 };
 
 static struct parport_default_sysctl_table
diff --git a/drivers/pci/access.c b/drivers/pci/access.c
index 7f8b78c0887..8c148f39e8d 100644
--- a/drivers/pci/access.c
+++ b/drivers/pci/access.c
@@ -148,7 +148,7 @@ static noinline void pci_wait_cfg(struct pci_dev *dev)
 int pci_user_read_config_##size						\
 	(struct pci_dev *dev, int pos, type *val)			\
 {									\
-	int ret = 0;							\
+	int ret = PCIBIOS_SUCCESSFUL;					\
 	u32 data = -1;							\
 	if (PCI_##size##_BAD)						\
 		return -EINVAL;						\
@@ -159,9 +159,7 @@ int pci_user_read_config_##size						\
 					pos, sizeof(type), &data);	\
 	raw_spin_unlock_irq(&pci_lock);				\
 	*val = (type)data;						\
-	if (ret > 0)							\
-		ret = -EINVAL;						\
-	return ret;							\
+	return pcibios_err_to_errno(ret);				\
 }									\
 EXPORT_SYMBOL_GPL(pci_user_read_config_##size);
 
@@ -170,7 +168,7 @@ EXPORT_SYMBOL_GPL(pci_user_read_config_##size);
 int pci_user_write_config_##size					\
 	(struct pci_dev *dev, int pos, type val)			\
 {									\
-	int ret = -EIO;							\
+	int ret = PCIBIOS_SUCCESSFUL;					\
 	if (PCI_##size##_BAD)						\
 		return -EINVAL;						\
 	raw_spin_lock_irq(&pci_lock);				\
@@ -179,9 +177,7 @@ int pci_user_write_config_##size					\
 	ret = dev->bus->ops->write(dev->bus, dev->devfn,		\
 					pos, sizeof(type), val);	\
 	raw_spin_unlock_irq(&pci_lock);				\
-	if (ret > 0)							\
-		ret = -EINVAL;						\
-	return ret;							\
+	return pcibios_err_to_errno(ret);				\
 }									\
 EXPORT_SYMBOL_GPL(pci_user_write_config_##size);
 
diff --git a/drivers/pci/bus.c b/drivers/pci/bus.c
index fb8aed307c2..447d393725e 100644
--- a/drivers/pci/bus.c
+++ b/drivers/pci/bus.c
@@ -13,7 +13,6 @@
 #include <linux/errno.h>
 #include <linux/ioport.h>
 #include <linux/proc_fs.h>
-#include <linux/init.h>
 #include <linux/slab.h>
 
 #include "pci.h"
@@ -236,7 +235,7 @@ void __weak pcibios_resource_survey_bus(struct pci_bus *bus) { }
  *
  * This adds add sysfs entries and start device drivers
  */
-int pci_bus_add_device(struct pci_dev *dev)
+void pci_bus_add_device(struct pci_dev *dev)
 {
 	int retval;
 
@@ -253,8 +252,6 @@ int pci_bus_add_device(struct pci_dev *dev)
 	WARN_ON(retval < 0);
 
 	dev->is_added = 1;
-
-	return 0;
 }
 
 /**
@@ -267,16 +264,12 @@ void pci_bus_add_devices(const struct pci_bus *bus)
 {
 	struct pci_dev *dev;
 	struct pci_bus *child;
-	int retval;
 
 	list_for_each_entry(dev, &bus->devices, bus_list) {
 		/* Skip already-added devices */
 		if (dev->is_added)
 			continue;
-		retval = pci_bus_add_device(dev);
-		if (retval)
-			dev_err(&dev->dev, "Error adding device (%d)\n",
-				retval);
+		pci_bus_add_device(dev);
 	}
 
 	list_for_each_entry(dev, &bus->devices, bus_list) {
diff --git a/drivers/pci/host-bridge.c b/drivers/pci/host-bridge.c
index 47aaf22d814..0e5f3c95af5 100644
--- a/drivers/pci/host-bridge.c
+++ b/drivers/pci/host-bridge.c
@@ -3,7 +3,6 @@
  */
 
 #include <linux/kernel.h>
-#include <linux/init.h>
 #include <linux/pci.h>
 #include <linux/module.h>
 
diff --git a/drivers/pci/host/Kconfig b/drivers/pci/host/Kconfig
index a6f67ec8882..21df477be0c 100644
--- a/drivers/pci/host/Kconfig
+++ b/drivers/pci/host/Kconfig
@@ -33,4 +33,17 @@ config PCI_RCAR_GEN2
 	  There are 3 internal PCI controllers available with a single
 	  built-in EHCI/OHCI host controller present on each one.
 
+config PCI_RCAR_GEN2_PCIE
+	bool "Renesas R-Car PCIe controller"
+	depends on ARCH_SHMOBILE || (ARM && COMPILE_TEST)
+	help
+	  Say Y here if you want PCIe controller support on R-Car Gen2 SoCs.
+
+config PCI_HOST_GENERIC
+	bool "Generic PCI host controller"
+	depends on ARM && OF
+	help
+	  Say Y here if you want to support a simple generic PCI host
+	  controller, such as the one emulated by kvmtool.
+
 endmenu
diff --git a/drivers/pci/host/Makefile b/drivers/pci/host/Makefile
index 13fb3333aa0..611ba4b48c9 100644
--- a/drivers/pci/host/Makefile
+++ b/drivers/pci/host/Makefile
@@ -4,3 +4,5 @@ obj-$(CONFIG_PCI_IMX6) += pci-imx6.o
 obj-$(CONFIG_PCI_MVEBU) += pci-mvebu.o
 obj-$(CONFIG_PCI_TEGRA) += pci-tegra.o
 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
diff --git a/drivers/pci/host/pci-exynos.c b/drivers/pci/host/pci-exynos.c
index 3de6bfbbe8e..1632661c5b7 100644
--- a/drivers/pci/host/pci-exynos.c
+++ b/drivers/pci/host/pci-exynos.c
@@ -415,9 +415,7 @@ static irqreturn_t exynos_pcie_msi_irq_handler(int irq, void *arg)
 {
 	struct pcie_port *pp = arg;
 
-	dw_handle_msi_irq(pp);
-
-	return IRQ_HANDLED;
+	return dw_handle_msi_irq(pp);
 }
 
 static void exynos_pcie_msi_init(struct pcie_port *pp)
@@ -511,7 +509,8 @@ static struct pcie_host_ops exynos_pcie_host_ops = {
 	.host_init = exynos_pcie_host_init,
 };
 
-static int add_pcie_port(struct pcie_port *pp, struct platform_device *pdev)
+static int __init add_pcie_port(struct pcie_port *pp,
+				struct platform_device *pdev)
 {
 	int ret;
 
@@ -568,10 +567,8 @@ static int __init exynos_pcie_probe(struct platform_device *pdev)
 
 	exynos_pcie = devm_kzalloc(&pdev->dev, sizeof(*exynos_pcie),
 				GFP_KERNEL);
-	if (!exynos_pcie) {
-		dev_err(&pdev->dev, "no memory for exynos pcie\n");
+	if (!exynos_pcie)
 		return -ENOMEM;
-	}
 
 	pp = &exynos_pcie->pp;
 
diff --git a/drivers/pci/host/pci-host-generic.c b/drivers/pci/host/pci-host-generic.c
new file mode 100644
index 00000000000..44fe6aa6a43
--- /dev/null
+++ b/drivers/pci/host/pci-host-generic.c
@@ -0,0 +1,388 @@
+/*
+ * Simple, generic PCI host controller driver targetting firmware-initialised
+ * systems and virtual machines (e.g. the PCI emulation provided by kvmtool).
+ *
+ * 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/>.
+ *
+ * Copyright (C) 2014 ARM Limited
+ *
+ * Author: Will Deacon <will.deacon@arm.com>
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/of_pci.h>
+#include <linux/platform_device.h>
+
+struct gen_pci_cfg_bus_ops {
+	u32 bus_shift;
+	void __iomem *(*map_bus)(struct pci_bus *, unsigned int, int);
+};
+
+struct gen_pci_cfg_windows {
+	struct resource				res;
+	struct resource				bus_range;
+	void __iomem				**win;
+
+	const struct gen_pci_cfg_bus_ops	*ops;
+};
+
+struct gen_pci {
+	struct pci_host_bridge			host;
+	struct gen_pci_cfg_windows		cfg;
+	struct list_head			resources;
+};
+
+static void __iomem *gen_pci_map_cfg_bus_cam(struct pci_bus *bus,
+					     unsigned int devfn,
+					     int where)
+{
+	struct pci_sys_data *sys = bus->sysdata;
+	struct gen_pci *pci = sys->private_data;
+	resource_size_t idx = bus->number - pci->cfg.bus_range.start;
+
+	return pci->cfg.win[idx] + ((devfn << 8) | where);
+}
+
+static struct gen_pci_cfg_bus_ops gen_pci_cfg_cam_bus_ops = {
+	.bus_shift	= 16,
+	.map_bus	= gen_pci_map_cfg_bus_cam,
+};
+
+static void __iomem *gen_pci_map_cfg_bus_ecam(struct pci_bus *bus,
+					      unsigned int devfn,
+					      int where)
+{
+	struct pci_sys_data *sys = bus->sysdata;
+	struct gen_pci *pci = sys->private_data;
+	resource_size_t idx = bus->number - pci->cfg.bus_range.start;
+
+	return pci->cfg.win[idx] + ((devfn << 12) | where);
+}
+
+static struct gen_pci_cfg_bus_ops gen_pci_cfg_ecam_bus_ops = {
+	.bus_shift	= 20,
+	.map_bus	= gen_pci_map_cfg_bus_ecam,
+};
+
+static int gen_pci_config_read(struct pci_bus *bus, unsigned int devfn,
+				int where, int size, u32 *val)
+{
+	void __iomem *addr;
+	struct pci_sys_data *sys = bus->sysdata;
+	struct gen_pci *pci = sys->private_data;
+
+	addr = pci->cfg.ops->map_bus(bus, devfn, where);
+
+	switch (size) {
+	case 1:
+		*val = readb(addr);
+		break;
+	case 2:
+		*val = readw(addr);
+		break;
+	default:
+		*val = readl(addr);
+	}
+
+	return PCIBIOS_SUCCESSFUL;
+}
+
+static int gen_pci_config_write(struct pci_bus *bus, unsigned int devfn,
+				 int where, int size, u32 val)
+{
+	void __iomem *addr;
+	struct pci_sys_data *sys = bus->sysdata;
+	struct gen_pci *pci = sys->private_data;
+
+	addr = pci->cfg.ops->map_bus(bus, devfn, where);
+
+	switch (size) {
+	case 1:
+		writeb(val, addr);
+		break;
+	case 2:
+		writew(val, addr);
+		break;
+	default:
+		writel(val, addr);
+	}
+
+	return PCIBIOS_SUCCESSFUL;
+}
+
+static struct pci_ops gen_pci_ops = {
+	.read	= gen_pci_config_read,
+	.write	= gen_pci_config_write,
+};
+
+static const struct of_device_id gen_pci_of_match[] = {
+	{ .compatible = "pci-host-cam-generic",
+	  .data = &gen_pci_cfg_cam_bus_ops },
+
+	{ .compatible = "pci-host-ecam-generic",
+	  .data = &gen_pci_cfg_ecam_bus_ops },
+
+	{ },
+};
+MODULE_DEVICE_TABLE(of, gen_pci_of_match);
+
+static int gen_pci_calc_io_offset(struct device *dev,
+				  struct of_pci_range *range,
+				  struct resource *res,
+				  resource_size_t *offset)
+{
+	static atomic_t wins = ATOMIC_INIT(0);
+	int err, idx, max_win;
+	unsigned int window;
+
+	if (!PAGE_ALIGNED(range->cpu_addr))
+		return -EINVAL;
+
+	max_win = (IO_SPACE_LIMIT + 1) / SZ_64K;
+	idx = atomic_inc_return(&wins);
+	if (idx > max_win)
+		return -ENOSPC;
+
+	window = (idx - 1) * SZ_64K;
+	err = pci_ioremap_io(window, range->cpu_addr);
+	if (err)
+		return err;
+
+	of_pci_range_to_resource(range, dev->of_node, res);
+	res->start = window;
+	res->end = res->start + range->size - 1;
+	*offset = window - range->pci_addr;
+	return 0;
+}
+
+static int gen_pci_calc_mem_offset(struct device *dev,
+				   struct of_pci_range *range,
+				   struct resource *res,
+				   resource_size_t *offset)
+{
+	of_pci_range_to_resource(range, dev->of_node, res);
+	*offset = range->cpu_addr - range->pci_addr;
+	return 0;
+}
+
+static void gen_pci_release_of_pci_ranges(struct gen_pci *pci)
+{
+	struct pci_host_bridge_window *win;
+
+	list_for_each_entry(win, &pci->resources, list)
+		release_resource(win->res);
+
+	pci_free_resource_list(&pci->resources);
+}
+
+static int gen_pci_parse_request_of_pci_ranges(struct gen_pci *pci)
+{
+	struct of_pci_range range;
+	struct of_pci_range_parser parser;
+	int err, res_valid = 0;
+	struct device *dev = pci->host.dev.parent;
+	struct device_node *np = dev->of_node;
+
+	if (of_pci_range_parser_init(&parser, np)) {
+		dev_err(dev, "missing \"ranges\" property\n");
+		return -EINVAL;
+	}
+
+	for_each_of_pci_range(&parser, &range) {
+		struct resource *parent, *res;
+		resource_size_t offset;
+		u32 restype = range.flags & IORESOURCE_TYPE_BITS;
+
+		res = devm_kmalloc(dev, sizeof(*res), GFP_KERNEL);
+		if (!res) {
+			err = -ENOMEM;
+			goto out_release_res;
+		}
+
+		switch (restype) {
+		case IORESOURCE_IO:
+			parent = &ioport_resource;
+			err = gen_pci_calc_io_offset(dev, &range, res, &offset);
+			break;
+		case IORESOURCE_MEM:
+			parent = &iomem_resource;
+			err = gen_pci_calc_mem_offset(dev, &range, res, &offset);
+			res_valid |= !(res->flags & IORESOURCE_PREFETCH || err);
+			break;
+		default:
+			err = -EINVAL;
+			continue;
+		}
+
+		if (err) {
+			dev_warn(dev,
+				 "error %d: failed to add resource [type 0x%x, %lld bytes]\n",
+				 err, restype, range.size);
+			continue;
+		}
+
+		err = request_resource(parent, res);
+		if (err)
+			goto out_release_res;
+
+		pci_add_resource_offset(&pci->resources, res, offset);
+	}
+
+	if (!res_valid) {
+		dev_err(dev, "non-prefetchable memory resource required\n");
+		err = -EINVAL;
+		goto out_release_res;
+	}
+
+	return 0;
+
+out_release_res:
+	gen_pci_release_of_pci_ranges(pci);
+	return err;
+}
+
+static int gen_pci_parse_map_cfg_windows(struct gen_pci *pci)
+{
+	int err;
+	u8 bus_max;
+	resource_size_t busn;
+	struct resource *bus_range;
+	struct device *dev = pci->host.dev.parent;
+	struct device_node *np = dev->of_node;
+
+	if (of_pci_parse_bus_range(np, &pci->cfg.bus_range))
+		pci->cfg.bus_range = (struct resource) {
+			.name	= np->name,
+			.start	= 0,
+			.end	= 0xff,
+			.flags	= IORESOURCE_BUS,
+		};
+
+	err = of_address_to_resource(np, 0, &pci->cfg.res);
+	if (err) {
+		dev_err(dev, "missing \"reg\" property\n");
+		return err;
+	}
+
+	pci->cfg.win = devm_kcalloc(dev, resource_size(&pci->cfg.bus_range),
+				    sizeof(*pci->cfg.win), GFP_KERNEL);
+	if (!pci->cfg.win)
+		return -ENOMEM;
+
+	/* Limit the bus-range to fit within reg */
+	bus_max = pci->cfg.bus_range.start +
+		  (resource_size(&pci->cfg.res) >> pci->cfg.ops->bus_shift) - 1;
+	pci->cfg.bus_range.end = min_t(resource_size_t, pci->cfg.bus_range.end,
+				       bus_max);
+
+	/* Map our Configuration Space windows */
+	if (!devm_request_mem_region(dev, pci->cfg.res.start,
+				     resource_size(&pci->cfg.res),
+				     "Configuration Space"))
+		return -ENOMEM;
+
+	bus_range = &pci->cfg.bus_range;
+	for (busn = bus_range->start; busn <= bus_range->end; ++busn) {
+		u32 idx = busn - bus_range->start;
+		u32 sz = 1 << pci->cfg.ops->bus_shift;
+
+		pci->cfg.win[idx] = devm_ioremap(dev,
+						 pci->cfg.res.start + busn * sz,
+						 sz);
+		if (!pci->cfg.win[idx])
+			return -ENOMEM;
+	}
+
+	/* Register bus resource */
+	pci_add_resource(&pci->resources, bus_range);
+	return 0;
+}
+
+static int gen_pci_setup(int nr, struct pci_sys_data *sys)
+{
+	struct gen_pci *pci = sys->private_data;
+	list_splice_init(&pci->resources, &sys->resources);
+	return 1;
+}
+
+static int gen_pci_probe(struct platform_device *pdev)
+{
+	int err;
+	const char *type;
+	const struct of_device_id *of_id;
+	const int *prop;
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	struct gen_pci *pci = devm_kzalloc(dev, sizeof(*pci), GFP_KERNEL);
+	struct hw_pci hw = {
+		.nr_controllers	= 1,
+		.private_data	= (void **)&pci,
+		.setup		= gen_pci_setup,
+		.map_irq	= of_irq_parse_and_map_pci,
+		.ops		= &gen_pci_ops,
+	};
+
+	if (!pci)
+		return -ENOMEM;
+
+	type = of_get_property(np, "device_type", NULL);
+	if (!type || strcmp(type, "pci")) {
+		dev_err(dev, "invalid \"device_type\" %s\n", type);
+		return -EINVAL;
+	}
+
+	prop = of_get_property(of_chosen, "linux,pci-probe-only", NULL);
+	if (prop) {
+		if (*prop)
+			pci_add_flags(PCI_PROBE_ONLY);
+		else
+			pci_clear_flags(PCI_PROBE_ONLY);
+	}
+
+	of_id = of_match_node(gen_pci_of_match, np);
+	pci->cfg.ops = of_id->data;
+	pci->host.dev.parent = dev;
+	INIT_LIST_HEAD(&pci->host.windows);
+	INIT_LIST_HEAD(&pci->resources);
+
+	/* Parse our PCI ranges and request their resources */
+	err = gen_pci_parse_request_of_pci_ranges(pci);
+	if (err)
+		return err;
+
+	/* Parse and map our Configuration Space windows */
+	err = gen_pci_parse_map_cfg_windows(pci);
+	if (err) {
+		gen_pci_release_of_pci_ranges(pci);
+		return err;
+	}
+
+	pci_common_init_dev(dev, &hw);
+	return 0;
+}
+
+static struct platform_driver gen_pci_driver = {
+	.driver = {
+		.name = "pci-host-generic",
+		.owner = THIS_MODULE,
+		.of_match_table = gen_pci_of_match,
+	},
+	.probe = gen_pci_probe,
+};
+module_platform_driver(gen_pci_driver);
+
+MODULE_DESCRIPTION("Generic PCI host driver");
+MODULE_AUTHOR("Will Deacon <will.deacon@arm.com>");
+MODULE_LICENSE("GPLv2");
diff --git a/drivers/pci/host/pci-imx6.c b/drivers/pci/host/pci-imx6.c
index ee082509b0b..a5645ae4aef 100644
--- a/drivers/pci/host/pci-imx6.c
+++ b/drivers/pci/host/pci-imx6.c
@@ -25,6 +25,7 @@
 #include <linux/resource.h>
 #include <linux/signal.h>
 #include <linux/types.h>
+#include <linux/interrupt.h>
 
 #include "pcie-designware.h"
 
@@ -32,13 +33,9 @@
 
 struct imx6_pcie {
 	int			reset_gpio;
-	int			power_on_gpio;
-	int			wake_up_gpio;
-	int			disable_gpio;
-	struct clk		*lvds_gate;
-	struct clk		*sata_ref_100m;
-	struct clk		*pcie_ref_125m;
-	struct clk		*pcie_axi;
+	struct clk		*pcie_bus;
+	struct clk		*pcie_phy;
+	struct clk		*pcie;
 	struct pcie_port	pp;
 	struct regmap		*iomuxc_gpr;
 	void __iomem		*mem_base;
@@ -231,36 +228,27 @@ static int imx6_pcie_deassert_core_reset(struct pcie_port *pp)
 	struct imx6_pcie *imx6_pcie = to_imx6_pcie(pp);
 	int ret;
 
-	if (gpio_is_valid(imx6_pcie->power_on_gpio))
-		gpio_set_value(imx6_pcie->power_on_gpio, 1);
-
 	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->sata_ref_100m);
-	if (ret) {
-		dev_err(pp->dev, "unable to enable sata_ref_100m\n");
-		goto err_sata_ref;
-	}
-
-	ret = clk_prepare_enable(imx6_pcie->pcie_ref_125m);
+	ret = clk_prepare_enable(imx6_pcie->pcie_phy);
 	if (ret) {
-		dev_err(pp->dev, "unable to enable pcie_ref_125m\n");
-		goto err_pcie_ref;
+		dev_err(pp->dev, "unable to enable pcie_phy clock\n");
+		goto err_pcie_phy;
 	}
 
-	ret = clk_prepare_enable(imx6_pcie->lvds_gate);
+	ret = clk_prepare_enable(imx6_pcie->pcie_bus);
 	if (ret) {
-		dev_err(pp->dev, "unable to enable lvds_gate\n");
-		goto err_lvds_gate;
+		dev_err(pp->dev, "unable to enable pcie_bus clock\n");
+		goto err_pcie_bus;
 	}
 
-	ret = clk_prepare_enable(imx6_pcie->pcie_axi);
+	ret = clk_prepare_enable(imx6_pcie->pcie);
 	if (ret) {
-		dev_err(pp->dev, "unable to enable pcie_axi\n");
-		goto err_pcie_axi;
+		dev_err(pp->dev, "unable to enable pcie clock\n");
+		goto err_pcie;
 	}
 
 	/* allow the clocks to stabilize */
@@ -274,13 +262,11 @@ static int imx6_pcie_deassert_core_reset(struct pcie_port *pp)
 	}
 	return 0;
 
-err_pcie_axi:
-	clk_disable_unprepare(imx6_pcie->lvds_gate);
-err_lvds_gate:
-	clk_disable_unprepare(imx6_pcie->pcie_ref_125m);
-err_pcie_ref:
-	clk_disable_unprepare(imx6_pcie->sata_ref_100m);
-err_sata_ref:
+err_pcie:
+	clk_disable_unprepare(imx6_pcie->pcie_bus);
+err_pcie_bus:
+	clk_disable_unprepare(imx6_pcie->pcie_phy);
+err_pcie_phy:
 	return ret;
 
 }
@@ -329,6 +315,13 @@ static int imx6_pcie_wait_for_link(struct pcie_port *pp)
 	return 0;
 }
 
+static irqreturn_t imx6_pcie_msi_handler(int irq, void *arg)
+{
+	struct pcie_port *pp = arg;
+
+	return dw_handle_msi_irq(pp);
+}
+
 static int imx6_pcie_start_link(struct pcie_port *pp)
 {
 	struct imx6_pcie *imx6_pcie = to_imx6_pcie(pp);
@@ -403,6 +396,9 @@ static void imx6_pcie_host_init(struct pcie_port *pp)
 	dw_pcie_setup_rc(pp);
 
 	imx6_pcie_start_link(pp);
+
+	if (IS_ENABLED(CONFIG_PCI_MSI))
+		dw_pcie_msi_init(pp);
 }
 
 static void imx6_pcie_reset_phy(struct pcie_port *pp)
@@ -487,15 +483,25 @@ static struct pcie_host_ops imx6_pcie_host_ops = {
 	.host_init = imx6_pcie_host_init,
 };
 
-static int imx6_add_pcie_port(struct pcie_port *pp,
+static int __init imx6_add_pcie_port(struct pcie_port *pp,
 			struct platform_device *pdev)
 {
 	int ret;
 
-	pp->irq = platform_get_irq(pdev, 0);
-	if (!pp->irq) {
-		dev_err(&pdev->dev, "failed to get irq\n");
-		return -ENODEV;
+	if (IS_ENABLED(CONFIG_PCI_MSI)) {
+		pp->msi_irq = platform_get_irq_byname(pdev, "msi");
+		if (pp->msi_irq <= 0) {
+			dev_err(&pdev->dev, "failed to get MSI irq\n");
+			return -ENODEV;
+		}
+
+		ret = devm_request_irq(&pdev->dev, pp->msi_irq,
+		                       imx6_pcie_msi_handler,
+		                       IRQF_SHARED, "mx6-pcie-msi", pp);
+		if (ret) {
+			dev_err(&pdev->dev, "failed to request MSI irq\n");
+			return -ENODEV;
+		}
 	}
 
 	pp->root_bus_nr = -1;
@@ -546,69 +552,26 @@ static int __init imx6_pcie_probe(struct platform_device *pdev)
 		}
 	}
 
-	imx6_pcie->power_on_gpio = of_get_named_gpio(np, "power-on-gpio", 0);
-	if (gpio_is_valid(imx6_pcie->power_on_gpio)) {
-		ret = devm_gpio_request_one(&pdev->dev,
-					imx6_pcie->power_on_gpio,
-					GPIOF_OUT_INIT_LOW,
-					"PCIe power enable");
-		if (ret) {
-			dev_err(&pdev->dev, "unable to get power-on gpio\n");
-			return ret;
-		}
-	}
-
-	imx6_pcie->wake_up_gpio = of_get_named_gpio(np, "wake-up-gpio", 0);
-	if (gpio_is_valid(imx6_pcie->wake_up_gpio)) {
-		ret = devm_gpio_request_one(&pdev->dev,
-					imx6_pcie->wake_up_gpio,
-					GPIOF_IN,
-					"PCIe wake up");
-		if (ret) {
-			dev_err(&pdev->dev, "unable to get wake-up gpio\n");
-			return ret;
-		}
-	}
-
-	imx6_pcie->disable_gpio = of_get_named_gpio(np, "disable-gpio", 0);
-	if (gpio_is_valid(imx6_pcie->disable_gpio)) {
-		ret = devm_gpio_request_one(&pdev->dev,
-					imx6_pcie->disable_gpio,
-					GPIOF_OUT_INIT_HIGH,
-					"PCIe disable endpoint");
-		if (ret) {
-			dev_err(&pdev->dev, "unable to get disable-ep gpio\n");
-			return ret;
-		}
-	}
-
 	/* Fetch clocks */
-	imx6_pcie->lvds_gate = devm_clk_get(&pdev->dev, "lvds_gate");
-	if (IS_ERR(imx6_pcie->lvds_gate)) {
-		dev_err(&pdev->dev,
-			"lvds_gate clock select missing or invalid\n");
-		return PTR_ERR(imx6_pcie->lvds_gate);
-	}
-
-	imx6_pcie->sata_ref_100m = devm_clk_get(&pdev->dev, "sata_ref_100m");
-	if (IS_ERR(imx6_pcie->sata_ref_100m)) {
+	imx6_pcie->pcie_phy = devm_clk_get(&pdev->dev, "pcie_phy");
+	if (IS_ERR(imx6_pcie->pcie_phy)) {
 		dev_err(&pdev->dev,
-			"sata_ref_100m clock source missing or invalid\n");
-		return PTR_ERR(imx6_pcie->sata_ref_100m);
+			"pcie_phy clock source missing or invalid\n");
+		return PTR_ERR(imx6_pcie->pcie_phy);
 	}
 
-	imx6_pcie->pcie_ref_125m = devm_clk_get(&pdev->dev, "pcie_ref_125m");
-	if (IS_ERR(imx6_pcie->pcie_ref_125m)) {
+	imx6_pcie->pcie_bus = devm_clk_get(&pdev->dev, "pcie_bus");
+	if (IS_ERR(imx6_pcie->pcie_bus)) {
 		dev_err(&pdev->dev,
-			"pcie_ref_125m clock source missing or invalid\n");
-		return PTR_ERR(imx6_pcie->pcie_ref_125m);
+			"pcie_bus clock source missing or invalid\n");
+		return PTR_ERR(imx6_pcie->pcie_bus);
 	}
 
-	imx6_pcie->pcie_axi = devm_clk_get(&pdev->dev, "pcie_axi");
-	if (IS_ERR(imx6_pcie->pcie_axi)) {
+	imx6_pcie->pcie = devm_clk_get(&pdev->dev, "pcie");
+	if (IS_ERR(imx6_pcie->pcie)) {
 		dev_err(&pdev->dev,
-			"pcie_axi clock source missing or invalid\n");
-		return PTR_ERR(imx6_pcie->pcie_axi);
+			"pcie clock source missing or invalid\n");
+		return PTR_ERR(imx6_pcie->pcie);
 	}
 
 	/* Grab GPR config register range */
diff --git a/drivers/pci/host/pci-mvebu.c b/drivers/pci/host/pci-mvebu.c
index d3d1cfd51e0..e384e253459 100644
--- a/drivers/pci/host/pci-mvebu.c
+++ b/drivers/pci/host/pci-mvebu.c
@@ -293,6 +293,58 @@ static int mvebu_pcie_hw_wr_conf(struct mvebu_pcie_port *port,
 	return PCIBIOS_SUCCESSFUL;
 }
 
+/*
+ * Remove windows, starting from the largest ones to the smallest
+ * ones.
+ */
+static void mvebu_pcie_del_windows(struct mvebu_pcie_port *port,
+				   phys_addr_t base, size_t size)
+{
+	while (size) {
+		size_t sz = 1 << (fls(size) - 1);
+
+		mvebu_mbus_del_window(base, sz);
+		base += sz;
+		size -= sz;
+	}
+}
+
+/*
+ * MBus windows can only have a power of two size, but PCI BARs do not
+ * have this constraint. Therefore, we have to split the PCI BAR into
+ * areas each having a power of two size. We start from the largest
+ * one (i.e highest order bit set in the size).
+ */
+static void mvebu_pcie_add_windows(struct mvebu_pcie_port *port,
+				   unsigned int target, unsigned int attribute,
+				   phys_addr_t base, size_t size,
+				   phys_addr_t remap)
+{
+	size_t size_mapped = 0;
+
+	while (size) {
+		size_t sz = 1 << (fls(size) - 1);
+		int ret;
+
+		ret = mvebu_mbus_add_window_remap_by_id(target, attribute, base,
+							sz, remap);
+		if (ret) {
+			dev_err(&port->pcie->pdev->dev,
+				"Could not create MBus window at 0x%x, size 0x%x: %d\n",
+				base, sz, ret);
+			mvebu_pcie_del_windows(port, base - size_mapped,
+					       size_mapped);
+			return;
+		}
+
+		size -= sz;
+		size_mapped += sz;
+		base += sz;
+		if (remap != MVEBU_MBUS_NO_REMAP)
+			remap += sz;
+	}
+}
+
 static void mvebu_pcie_handle_iobase_change(struct mvebu_pcie_port *port)
 {
 	phys_addr_t iobase;
@@ -304,8 +356,8 @@ static void mvebu_pcie_handle_iobase_change(struct mvebu_pcie_port *port)
 
 		/* If a window was configured, remove it */
 		if (port->iowin_base) {
-			mvebu_mbus_del_window(port->iowin_base,
-					      port->iowin_size);
+			mvebu_pcie_del_windows(port, port->iowin_base,
+					       port->iowin_size);
 			port->iowin_base = 0;
 			port->iowin_size = 0;
 		}
@@ -331,11 +383,11 @@ static void mvebu_pcie_handle_iobase_change(struct mvebu_pcie_port *port)
 	port->iowin_base = port->pcie->io.start + iobase;
 	port->iowin_size = ((0xFFF | ((port->bridge.iolimit & 0xF0) << 8) |
 			    (port->bridge.iolimitupper << 16)) -
-			    iobase);
+			    iobase) + 1;
 
-	mvebu_mbus_add_window_remap_by_id(port->io_target, port->io_attr,
-					  port->iowin_base, port->iowin_size,
-					  iobase);
+	mvebu_pcie_add_windows(port, port->io_target, port->io_attr,
+			       port->iowin_base, port->iowin_size,
+			       iobase);
 }
 
 static void mvebu_pcie_handle_membase_change(struct mvebu_pcie_port *port)
@@ -346,8 +398,8 @@ static void mvebu_pcie_handle_membase_change(struct mvebu_pcie_port *port)
 
 		/* If a window was configured, remove it */
 		if (port->memwin_base) {
-			mvebu_mbus_del_window(port->memwin_base,
-					      port->memwin_size);
+			mvebu_pcie_del_windows(port, port->memwin_base,
+					       port->memwin_size);
 			port->memwin_base = 0;
 			port->memwin_size = 0;
 		}
@@ -364,10 +416,11 @@ static void mvebu_pcie_handle_membase_change(struct mvebu_pcie_port *port)
 	port->memwin_base  = ((port->bridge.membase & 0xFFF0) << 16);
 	port->memwin_size  =
 		(((port->bridge.memlimit & 0xFFF0) << 16) | 0xFFFFF) -
-		port->memwin_base;
+		port->memwin_base + 1;
 
-	mvebu_mbus_add_window_by_id(port->mem_target, port->mem_attr,
-				    port->memwin_base, port->memwin_size);
+	mvebu_pcie_add_windows(port, port->mem_target, port->mem_attr,
+			       port->memwin_base, port->memwin_size,
+			       MVEBU_MBUS_NO_REMAP);
 }
 
 /*
@@ -743,14 +796,21 @@ static resource_size_t mvebu_pcie_align_resource(struct pci_dev *dev,
 
 	/*
 	 * On the PCI-to-PCI bridge side, the I/O windows must have at
-	 * least a 64 KB size and be aligned on their size, and the
-	 * memory windows must have at least a 1 MB size and be
-	 * aligned on their size
+	 * least a 64 KB size and the memory windows must have at
+	 * least a 1 MB size. Moreover, MBus windows need to have a
+	 * base address aligned on their size, and their size must be
+	 * a power of two. This means that if the BAR doesn't have a
+	 * power of two size, several MBus windows will actually be
+	 * created. We need to ensure that the biggest MBus window
+	 * (which will be the first one) is aligned on its size, which
+	 * explains the rounddown_pow_of_two() being done here.
 	 */
 	if (res->flags & IORESOURCE_IO)
-		return round_up(start, max_t(resource_size_t, SZ_64K, size));
+		return round_up(start, max_t(resource_size_t, SZ_64K,
+					     rounddown_pow_of_two(size)));
 	else if (res->flags & IORESOURCE_MEM)
-		return round_up(start, max_t(resource_size_t, SZ_1M, size));
+		return round_up(start, max_t(resource_size_t, SZ_1M,
+					     rounddown_pow_of_two(size)));
 	else
 		return start;
 }
diff --git a/drivers/pci/host/pci-rcar-gen2.c b/drivers/pci/host/pci-rcar-gen2.c
index 4fe349dcaf5..3ef854f5a5b 100644
--- a/drivers/pci/host/pci-rcar-gen2.c
+++ b/drivers/pci/host/pci-rcar-gen2.c
@@ -99,6 +99,7 @@ struct rcar_pci_priv {
 	struct resource io_res;
 	struct resource mem_res;
 	struct resource *cfg_res;
+	unsigned busnr;
 	int irq;
 	unsigned long window_size;
 };
@@ -318,8 +319,8 @@ static int rcar_pci_setup(int nr, struct pci_sys_data *sys)
 	pci_add_resource(&sys->resources, &priv->io_res);
 	pci_add_resource(&sys->resources, &priv->mem_res);
 
-	/* Setup bus number based on platform device id */
-	sys->busnr = to_platform_device(priv->dev)->id;
+	/* Setup bus number based on platform device id / of bus-range */
+	sys->busnr = priv->busnr;
 	return 1;
 }
 
@@ -372,6 +373,23 @@ static int rcar_pci_probe(struct platform_device *pdev)
 
 	priv->window_size = SZ_1G;
 
+	if (pdev->dev.of_node) {
+		struct resource busnr;
+		int ret;
+
+		ret = of_pci_parse_bus_range(pdev->dev.of_node, &busnr);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "failed to parse bus-range\n");
+			return ret;
+		}
+
+		priv->busnr = busnr.start;
+		if (busnr.end != busnr.start)
+			dev_warn(&pdev->dev, "only one bus number supported\n");
+	} else {
+		priv->busnr = pdev->id;
+	}
+
 	hw_private[0] = priv;
 	memset(&hw, 0, sizeof(hw));
 	hw.nr_controllers = ARRAY_SIZE(hw_private);
@@ -383,11 +401,20 @@ static int rcar_pci_probe(struct platform_device *pdev)
 	return 0;
 }
 
+static struct of_device_id rcar_pci_of_match[] = {
+	{ .compatible = "renesas,pci-r8a7790", },
+	{ .compatible = "renesas,pci-r8a7791", },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(of, rcar_pci_of_match);
+
 static struct platform_driver rcar_pci_driver = {
 	.driver = {
 		.name = "pci-rcar-gen2",
 		.owner = THIS_MODULE,
 		.suppress_bind_attrs = true,
+		.of_match_table = rcar_pci_of_match,
 	},
 	.probe = rcar_pci_probe,
 };
diff --git a/drivers/pci/host/pcie-designware.c b/drivers/pci/host/pcie-designware.c
index c4e37329447..e3bf9e6d5d9 100644
--- a/drivers/pci/host/pcie-designware.c
+++ b/drivers/pci/host/pcie-designware.c
@@ -156,15 +156,17 @@ static struct irq_chip dw_msi_irq_chip = {
 };
 
 /* MSI int handler */
-void dw_handle_msi_irq(struct pcie_port *pp)
+irqreturn_t dw_handle_msi_irq(struct pcie_port *pp)
 {
 	unsigned long val;
 	int i, pos, irq;
+	irqreturn_t ret = IRQ_NONE;
 
 	for (i = 0; i < MAX_MSI_CTRLS; i++) {
 		dw_pcie_rd_own_conf(pp, PCIE_MSI_INTR0_STATUS + i * 12, 4,
 				(u32 *)&val);
 		if (val) {
+			ret = IRQ_HANDLED;
 			pos = 0;
 			while ((pos = find_next_bit(&val, 32, pos)) != 32) {
 				irq = irq_find_mapping(pp->irq_domain,
@@ -177,6 +179,8 @@ void dw_handle_msi_irq(struct pcie_port *pp)
 			}
 		}
 	}
+
+	return ret;
 }
 
 void dw_pcie_msi_init(struct pcie_port *pp)
diff --git a/drivers/pci/host/pcie-designware.h b/drivers/pci/host/pcie-designware.h
index 3063b3594d8..a169d22d517 100644
--- a/drivers/pci/host/pcie-designware.h
+++ b/drivers/pci/host/pcie-designware.h
@@ -68,7 +68,7 @@ struct pcie_host_ops {
 
 int dw_pcie_cfg_read(void __iomem *addr, int where, int size, u32 *val);
 int dw_pcie_cfg_write(void __iomem *addr, int where, int size, u32 val);
-void dw_handle_msi_irq(struct pcie_port *pp);
+irqreturn_t dw_handle_msi_irq(struct pcie_port *pp);
 void dw_pcie_msi_init(struct pcie_port *pp);
 int dw_pcie_link_up(struct pcie_port *pp);
 void dw_pcie_setup_rc(struct pcie_port *pp);
diff --git a/drivers/pci/host/pcie-rcar.c b/drivers/pci/host/pcie-rcar.c
new file mode 100644
index 00000000000..8e06124aa80
--- /dev/null
+++ b/drivers/pci/host/pcie-rcar.c
@@ -0,0 +1,1008 @@
+/*
+ * PCIe driver for Renesas R-Car SoCs
+ *  Copyright (C) 2014 Renesas Electronics Europe Ltd
+ *
+ * Based on:
+ *  arch/sh/drivers/pci/pcie-sh7786.c
+ *  arch/sh/drivers/pci/ops-sh7786.c
+ *  Copyright (C) 2009 - 2011  Paul Mundt
+ *
+ * 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/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_irq.h>
+#include <linux/of_pci.h>
+#include <linux/of_platform.h>
+#include <linux/pci.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#define DRV_NAME "rcar-pcie"
+
+#define PCIECAR			0x000010
+#define PCIECCTLR		0x000018
+#define  CONFIG_SEND_ENABLE	(1 << 31)
+#define  TYPE0			(0 << 8)
+#define  TYPE1			(1 << 8)
+#define PCIECDR			0x000020
+#define PCIEMSR			0x000028
+#define PCIEINTXR		0x000400
+#define PCIEMSITXR		0x000840
+
+/* Transfer control */
+#define PCIETCTLR		0x02000
+#define  CFINIT			1
+#define PCIETSTR		0x02004
+#define  DATA_LINK_ACTIVE	1
+#define PCIEERRFR		0x02020
+#define  UNSUPPORTED_REQUEST	(1 << 4)
+#define PCIEMSIFR		0x02044
+#define PCIEMSIALR		0x02048
+#define  MSIFE			1
+#define PCIEMSIAUR		0x0204c
+#define PCIEMSIIER		0x02050
+
+/* root port address */
+#define PCIEPRAR(x)		(0x02080 + ((x) * 0x4))
+
+/* local address reg & mask */
+#define PCIELAR(x)		(0x02200 + ((x) * 0x20))
+#define PCIELAMR(x)		(0x02208 + ((x) * 0x20))
+#define  LAM_PREFETCH		(1 << 3)
+#define  LAM_64BIT		(1 << 2)
+#define  LAR_ENABLE		(1 << 1)
+
+/* PCIe address reg & mask */
+#define PCIEPARL(x)		(0x03400 + ((x) * 0x20))
+#define PCIEPARH(x)		(0x03404 + ((x) * 0x20))
+#define PCIEPAMR(x)		(0x03408 + ((x) * 0x20))
+#define PCIEPTCTLR(x)		(0x0340c + ((x) * 0x20))
+#define  PAR_ENABLE		(1 << 31)
+#define  IO_SPACE		(1 << 8)
+
+/* Configuration */
+#define PCICONF(x)		(0x010000 + ((x) * 0x4))
+#define PMCAP(x)		(0x010040 + ((x) * 0x4))
+#define EXPCAP(x)		(0x010070 + ((x) * 0x4))
+#define VCCAP(x)		(0x010100 + ((x) * 0x4))
+
+/* link layer */
+#define IDSETR1			0x011004
+#define TLCTLR			0x011048
+#define MACSR			0x011054
+#define MACCTLR			0x011058
+#define  SCRAMBLE_DISABLE	(1 << 27)
+
+/* R-Car H1 PHY */
+#define H1_PCIEPHYADRR		0x04000c
+#define  WRITE_CMD		(1 << 16)
+#define  PHY_ACK		(1 << 24)
+#define  RATE_POS		12
+#define  LANE_POS		8
+#define  ADR_POS		0
+#define H1_PCIEPHYDOUTR		0x040014
+#define H1_PCIEPHYSR		0x040018
+
+#define INT_PCI_MSI_NR	32
+
+#define RCONF(x)	(PCICONF(0)+(x))
+#define RPMCAP(x)	(PMCAP(0)+(x))
+#define REXPCAP(x)	(EXPCAP(0)+(x))
+#define RVCCAP(x)	(VCCAP(0)+(x))
+
+#define  PCIE_CONF_BUS(b)	(((b) & 0xff) << 24)
+#define  PCIE_CONF_DEV(d)	(((d) & 0x1f) << 19)
+#define  PCIE_CONF_FUNC(f)	(((f) & 0x7) << 16)
+
+#define PCI_MAX_RESOURCES 4
+#define MAX_NR_INBOUND_MAPS 6
+
+struct rcar_msi {
+	DECLARE_BITMAP(used, INT_PCI_MSI_NR);
+	struct irq_domain *domain;
+	struct msi_chip chip;
+	unsigned long pages;
+	struct mutex lock;
+	int irq1;
+	int irq2;
+};
+
+static inline struct rcar_msi *to_rcar_msi(struct msi_chip *chip)
+{
+	return container_of(chip, struct rcar_msi, chip);
+}
+
+/* Structure representing the PCIe interface */
+struct rcar_pcie {
+	struct device		*dev;
+	void __iomem		*base;
+	struct resource		res[PCI_MAX_RESOURCES];
+	struct resource		busn;
+	int			root_bus_nr;
+	struct clk		*clk;
+	struct clk		*bus_clk;
+	struct			rcar_msi msi;
+};
+
+static inline struct rcar_pcie *sys_to_pcie(struct pci_sys_data *sys)
+{
+	return sys->private_data;
+}
+
+static void pci_write_reg(struct rcar_pcie *pcie, unsigned long val,
+			  unsigned long reg)
+{
+	writel(val, pcie->base + reg);
+}
+
+static unsigned long pci_read_reg(struct rcar_pcie *pcie, unsigned long reg)
+{
+	return readl(pcie->base + reg);
+}
+
+enum {
+	PCI_ACCESS_READ,
+	PCI_ACCESS_WRITE,
+};
+
+static void rcar_rmw32(struct rcar_pcie *pcie, int where, u32 mask, u32 data)
+{
+	int shift = 8 * (where & 3);
+	u32 val = pci_read_reg(pcie, where & ~3);
+
+	val &= ~(mask << shift);
+	val |= data << shift;
+	pci_write_reg(pcie, val, where & ~3);
+}
+
+static u32 rcar_read_conf(struct rcar_pcie *pcie, int where)
+{
+	int shift = 8 * (where & 3);
+	u32 val = pci_read_reg(pcie, where & ~3);
+
+	return val >> shift;
+}
+
+/* Serialization is provided by 'pci_lock' in drivers/pci/access.c */
+static int rcar_pcie_config_access(struct rcar_pcie *pcie,
+		unsigned char access_type, struct pci_bus *bus,
+		unsigned int devfn, int where, u32 *data)
+{
+	int dev, func, reg, index;
+
+	dev = PCI_SLOT(devfn);
+	func = PCI_FUNC(devfn);
+	reg = where & ~3;
+	index = reg / 4;
+
+	/*
+	 * While each channel has its own memory-mapped extended config
+	 * space, it's generally only accessible when in endpoint mode.
+	 * When in root complex mode, the controller is unable to target
+	 * itself with either type 0 or type 1 accesses, and indeed, any
+	 * controller initiated target transfer to its own config space
+	 * result in a completer abort.
+	 *
+	 * Each channel effectively only supports a single device, but as
+	 * the same channel <-> device access works for any PCI_SLOT()
+	 * value, we cheat a bit here and bind the controller's config
+	 * space to devfn 0 in order to enable self-enumeration. In this
+	 * case the regular ECAR/ECDR path is sidelined and the mangled
+	 * config access itself is initiated as an internal bus transaction.
+	 */
+	if (pci_is_root_bus(bus)) {
+		if (dev != 0)
+			return PCIBIOS_DEVICE_NOT_FOUND;
+
+		if (access_type == PCI_ACCESS_READ) {
+			*data = pci_read_reg(pcie, PCICONF(index));
+		} else {
+			/* Keep an eye out for changes to the root bus number */
+			if (pci_is_root_bus(bus) && (reg == PCI_PRIMARY_BUS))
+				pcie->root_bus_nr = *data & 0xff;
+
+			pci_write_reg(pcie, *data, PCICONF(index));
+		}
+
+		return PCIBIOS_SUCCESSFUL;
+	}
+
+	if (pcie->root_bus_nr < 0)
+		return PCIBIOS_DEVICE_NOT_FOUND;
+
+	/* Clear errors */
+	pci_write_reg(pcie, pci_read_reg(pcie, PCIEERRFR), PCIEERRFR);
+
+	/* Set the PIO address */
+	pci_write_reg(pcie, PCIE_CONF_BUS(bus->number) | PCIE_CONF_DEV(dev) |
+				PCIE_CONF_FUNC(func) | reg, PCIECAR);
+
+	/* Enable the configuration access */
+	if (bus->parent->number == pcie->root_bus_nr)
+		pci_write_reg(pcie, CONFIG_SEND_ENABLE | TYPE0, PCIECCTLR);
+	else
+		pci_write_reg(pcie, CONFIG_SEND_ENABLE | TYPE1, PCIECCTLR);
+
+	/* Check for errors */
+	if (pci_read_reg(pcie, PCIEERRFR) & UNSUPPORTED_REQUEST)
+		return PCIBIOS_DEVICE_NOT_FOUND;
+
+	/* Check for master and target aborts */
+	if (rcar_read_conf(pcie, RCONF(PCI_STATUS)) &
+		(PCI_STATUS_REC_MASTER_ABORT | PCI_STATUS_REC_TARGET_ABORT))
+		return PCIBIOS_DEVICE_NOT_FOUND;
+
+	if (access_type == PCI_ACCESS_READ)
+		*data = pci_read_reg(pcie, PCIECDR);
+	else
+		pci_write_reg(pcie, *data, PCIECDR);
+
+	/* Disable the configuration access */
+	pci_write_reg(pcie, 0, PCIECCTLR);
+
+	return PCIBIOS_SUCCESSFUL;
+}
+
+static int rcar_pcie_read_conf(struct pci_bus *bus, unsigned int devfn,
+			       int where, int size, u32 *val)
+{
+	struct rcar_pcie *pcie = sys_to_pcie(bus->sysdata);
+	int ret;
+
+	if ((size == 2) && (where & 1))
+		return PCIBIOS_BAD_REGISTER_NUMBER;
+	else if ((size == 4) && (where & 3))
+		return PCIBIOS_BAD_REGISTER_NUMBER;
+
+	ret = rcar_pcie_config_access(pcie, PCI_ACCESS_READ,
+				      bus, devfn, where, val);
+	if (ret != PCIBIOS_SUCCESSFUL) {
+		*val = 0xffffffff;
+		return ret;
+	}
+
+	if (size == 1)
+		*val = (*val >> (8 * (where & 3))) & 0xff;
+	else if (size == 2)
+		*val = (*val >> (8 * (where & 2))) & 0xffff;
+
+	dev_dbg(&bus->dev, "pcie-config-read: bus=%3d devfn=0x%04x "
+		"where=0x%04x size=%d val=0x%08lx\n", bus->number,
+		devfn, where, size, (unsigned long)*val);
+
+	return ret;
+}
+
+/* Serialization is provided by 'pci_lock' in drivers/pci/access.c */
+static int rcar_pcie_write_conf(struct pci_bus *bus, unsigned int devfn,
+				int where, int size, u32 val)
+{
+	struct rcar_pcie *pcie = sys_to_pcie(bus->sysdata);
+	int shift, ret;
+	u32 data;
+
+	if ((size == 2) && (where & 1))
+		return PCIBIOS_BAD_REGISTER_NUMBER;
+	else if ((size == 4) && (where & 3))
+		return PCIBIOS_BAD_REGISTER_NUMBER;
+
+	ret = rcar_pcie_config_access(pcie, PCI_ACCESS_READ,
+				      bus, devfn, where, &data);
+	if (ret != PCIBIOS_SUCCESSFUL)
+		return ret;
+
+	dev_dbg(&bus->dev, "pcie-config-write: bus=%3d devfn=0x%04x "
+		"where=0x%04x size=%d val=0x%08lx\n", bus->number,
+		devfn, where, size, (unsigned long)val);
+
+	if (size == 1) {
+		shift = 8 * (where & 3);
+		data &= ~(0xff << shift);
+		data |= ((val & 0xff) << shift);
+	} else if (size == 2) {
+		shift = 8 * (where & 2);
+		data &= ~(0xffff << shift);
+		data |= ((val & 0xffff) << shift);
+	} else
+		data = val;
+
+	ret = rcar_pcie_config_access(pcie, PCI_ACCESS_WRITE,
+				      bus, devfn, where, &data);
+
+	return ret;
+}
+
+static struct pci_ops rcar_pcie_ops = {
+	.read	= rcar_pcie_read_conf,
+	.write	= rcar_pcie_write_conf,
+};
+
+static void rcar_pcie_setup_window(int win, struct resource *res,
+				   struct rcar_pcie *pcie)
+{
+	/* Setup PCIe address space mappings for each resource */
+	resource_size_t size;
+	u32 mask;
+
+	pci_write_reg(pcie, 0x00000000, PCIEPTCTLR(win));
+
+	/*
+	 * The PAMR mask is calculated in units of 128Bytes, which
+	 * keeps things pretty simple.
+	 */
+	size = resource_size(res);
+	mask = (roundup_pow_of_two(size) / SZ_128) - 1;
+	pci_write_reg(pcie, mask << 7, PCIEPAMR(win));
+
+	pci_write_reg(pcie, upper_32_bits(res->start), PCIEPARH(win));
+	pci_write_reg(pcie, lower_32_bits(res->start), PCIEPARL(win));
+
+	/* First resource is for IO */
+	mask = PAR_ENABLE;
+	if (res->flags & IORESOURCE_IO)
+		mask |= IO_SPACE;
+
+	pci_write_reg(pcie, mask, PCIEPTCTLR(win));
+}
+
+static int rcar_pcie_setup(int nr, struct pci_sys_data *sys)
+{
+	struct rcar_pcie *pcie = sys_to_pcie(sys);
+	struct resource *res;
+	int i;
+
+	pcie->root_bus_nr = -1;
+
+	/* Setup PCI resources */
+	for (i = 0; i < PCI_MAX_RESOURCES; i++) {
+
+		res = &pcie->res[i];
+		if (!res->flags)
+			continue;
+
+		rcar_pcie_setup_window(i, res, pcie);
+
+		if (res->flags & IORESOURCE_IO)
+			pci_ioremap_io(nr * SZ_64K, res->start);
+		else
+			pci_add_resource(&sys->resources, res);
+	}
+	pci_add_resource(&sys->resources, &pcie->busn);
+
+	return 1;
+}
+
+static void rcar_pcie_add_bus(struct pci_bus *bus)
+{
+	if (IS_ENABLED(CONFIG_PCI_MSI)) {
+		struct rcar_pcie *pcie = sys_to_pcie(bus->sysdata);
+
+		bus->msi = &pcie->msi.chip;
+	}
+}
+
+struct hw_pci rcar_pci = {
+	.setup          = rcar_pcie_setup,
+	.map_irq        = of_irq_parse_and_map_pci,
+	.ops            = &rcar_pcie_ops,
+	.add_bus        = rcar_pcie_add_bus,
+};
+
+static void rcar_pcie_enable(struct rcar_pcie *pcie)
+{
+	struct platform_device *pdev = to_platform_device(pcie->dev);
+
+	rcar_pci.nr_controllers = 1;
+	rcar_pci.private_data = (void **)&pcie;
+
+	pci_common_init_dev(&pdev->dev, &rcar_pci);
+#ifdef CONFIG_PCI_DOMAINS
+	rcar_pci.domain++;
+#endif
+}
+
+static int phy_wait_for_ack(struct rcar_pcie *pcie)
+{
+	unsigned int timeout = 100;
+
+	while (timeout--) {
+		if (pci_read_reg(pcie, H1_PCIEPHYADRR) & PHY_ACK)
+			return 0;
+
+		udelay(100);
+	}
+
+	dev_err(pcie->dev, "Access to PCIe phy timed out\n");
+
+	return -ETIMEDOUT;
+}
+
+static void phy_write_reg(struct rcar_pcie *pcie,
+				 unsigned int rate, unsigned int addr,
+				 unsigned int lane, unsigned int data)
+{
+	unsigned long phyaddr;
+
+	phyaddr = WRITE_CMD |
+		((rate & 1) << RATE_POS) |
+		((lane & 0xf) << LANE_POS) |
+		((addr & 0xff) << ADR_POS);
+
+	/* Set write data */
+	pci_write_reg(pcie, data, H1_PCIEPHYDOUTR);
+	pci_write_reg(pcie, phyaddr, H1_PCIEPHYADRR);
+
+	/* Ignore errors as they will be dealt with if the data link is down */
+	phy_wait_for_ack(pcie);
+
+	/* Clear command */
+	pci_write_reg(pcie, 0, H1_PCIEPHYDOUTR);
+	pci_write_reg(pcie, 0, H1_PCIEPHYADRR);
+
+	/* Ignore errors as they will be dealt with if the data link is down */
+	phy_wait_for_ack(pcie);
+}
+
+static int rcar_pcie_wait_for_dl(struct rcar_pcie *pcie)
+{
+	unsigned int timeout = 10;
+
+	while (timeout--) {
+		if ((pci_read_reg(pcie, PCIETSTR) & DATA_LINK_ACTIVE))
+			return 0;
+
+		msleep(5);
+	}
+
+	return -ETIMEDOUT;
+}
+
+static int rcar_pcie_hw_init(struct rcar_pcie *pcie)
+{
+	int err;
+
+	/* Begin initialization */
+	pci_write_reg(pcie, 0, PCIETCTLR);
+
+	/* Set mode */
+	pci_write_reg(pcie, 1, PCIEMSR);
+
+	/*
+	 * Initial header for port config space is type 1, set the device
+	 * class to match. Hardware takes care of propagating the IDSETR
+	 * settings, so there is no need to bother with a quirk.
+	 */
+	pci_write_reg(pcie, PCI_CLASS_BRIDGE_PCI << 16, IDSETR1);
+
+	/*
+	 * Setup Secondary Bus Number & Subordinate Bus Number, even though
+	 * they aren't used, to avoid bridge being detected as broken.
+	 */
+	rcar_rmw32(pcie, RCONF(PCI_SECONDARY_BUS), 0xff, 1);
+	rcar_rmw32(pcie, RCONF(PCI_SUBORDINATE_BUS), 0xff, 1);
+
+	/* Initialize default capabilities. */
+	rcar_rmw32(pcie, REXPCAP(0), 0, PCI_CAP_ID_EXP);
+	rcar_rmw32(pcie, REXPCAP(PCI_EXP_FLAGS),
+		PCI_EXP_FLAGS_TYPE, PCI_EXP_TYPE_ROOT_PORT << 4);
+	rcar_rmw32(pcie, RCONF(PCI_HEADER_TYPE), 0x7f,
+		PCI_HEADER_TYPE_BRIDGE);
+
+	/* Enable data link layer active state reporting */
+	rcar_rmw32(pcie, REXPCAP(PCI_EXP_LNKCAP), 0, PCI_EXP_LNKCAP_DLLLARC);
+
+	/* Write out the physical slot number = 0 */
+	rcar_rmw32(pcie, REXPCAP(PCI_EXP_SLTCAP), PCI_EXP_SLTCAP_PSN, 0);
+
+	/* Set the completion timer timeout to the maximum 50ms. */
+	rcar_rmw32(pcie, TLCTLR+1, 0x3f, 50);
+
+	/* Terminate list of capabilities (Next Capability Offset=0) */
+	rcar_rmw32(pcie, RVCCAP(0), 0xfff0, 0);
+
+	/* Enable MAC data scrambling. */
+	rcar_rmw32(pcie, MACCTLR, SCRAMBLE_DISABLE, 0);
+
+	/* Enable MSI */
+	if (IS_ENABLED(CONFIG_PCI_MSI))
+		pci_write_reg(pcie, 0x101f0000, PCIEMSITXR);
+
+	/* Finish initialization - establish a PCI Express link */
+	pci_write_reg(pcie, CFINIT, PCIETCTLR);
+
+	/* This will timeout if we don't have a link. */
+	err = rcar_pcie_wait_for_dl(pcie);
+	if (err)
+		return err;
+
+	/* Enable INTx interrupts */
+	rcar_rmw32(pcie, PCIEINTXR, 0, 0xF << 8);
+
+	/* Enable slave Bus Mastering */
+	rcar_rmw32(pcie, RCONF(PCI_STATUS), PCI_STATUS_DEVSEL_MASK,
+		PCI_COMMAND_IO | PCI_COMMAND_MEMORY | PCI_COMMAND_MASTER |
+		PCI_STATUS_CAP_LIST | PCI_STATUS_DEVSEL_FAST);
+
+	wmb();
+
+	return 0;
+}
+
+static int rcar_pcie_hw_init_h1(struct rcar_pcie *pcie)
+{
+	unsigned int timeout = 10;
+
+	/* Initialize the phy */
+	phy_write_reg(pcie, 0, 0x42, 0x1, 0x0EC34191);
+	phy_write_reg(pcie, 1, 0x42, 0x1, 0x0EC34180);
+	phy_write_reg(pcie, 0, 0x43, 0x1, 0x00210188);
+	phy_write_reg(pcie, 1, 0x43, 0x1, 0x00210188);
+	phy_write_reg(pcie, 0, 0x44, 0x1, 0x015C0014);
+	phy_write_reg(pcie, 1, 0x44, 0x1, 0x015C0014);
+	phy_write_reg(pcie, 1, 0x4C, 0x1, 0x786174A0);
+	phy_write_reg(pcie, 1, 0x4D, 0x1, 0x048000BB);
+	phy_write_reg(pcie, 0, 0x51, 0x1, 0x079EC062);
+	phy_write_reg(pcie, 0, 0x52, 0x1, 0x20000000);
+	phy_write_reg(pcie, 1, 0x52, 0x1, 0x20000000);
+	phy_write_reg(pcie, 1, 0x56, 0x1, 0x00003806);
+
+	phy_write_reg(pcie, 0, 0x60, 0x1, 0x004B03A5);
+	phy_write_reg(pcie, 0, 0x64, 0x1, 0x3F0F1F0F);
+	phy_write_reg(pcie, 0, 0x66, 0x1, 0x00008000);
+
+	while (timeout--) {
+		if (pci_read_reg(pcie, H1_PCIEPHYSR))
+			return rcar_pcie_hw_init(pcie);
+
+		msleep(5);
+	}
+
+	return -ETIMEDOUT;
+}
+
+static int rcar_msi_alloc(struct rcar_msi *chip)
+{
+	int msi;
+
+	mutex_lock(&chip->lock);
+
+	msi = find_first_zero_bit(chip->used, INT_PCI_MSI_NR);
+	if (msi < INT_PCI_MSI_NR)
+		set_bit(msi, chip->used);
+	else
+		msi = -ENOSPC;
+
+	mutex_unlock(&chip->lock);
+
+	return msi;
+}
+
+static void rcar_msi_free(struct rcar_msi *chip, unsigned long irq)
+{
+	mutex_lock(&chip->lock);
+	clear_bit(irq, chip->used);
+	mutex_unlock(&chip->lock);
+}
+
+static irqreturn_t rcar_pcie_msi_irq(int irq, void *data)
+{
+	struct rcar_pcie *pcie = data;
+	struct rcar_msi *msi = &pcie->msi;
+	unsigned long reg;
+
+	reg = pci_read_reg(pcie, PCIEMSIFR);
+
+	/* MSI & INTx share an interrupt - we only handle MSI here */
+	if (!reg)
+		return IRQ_NONE;
+
+	while (reg) {
+		unsigned int index = find_first_bit(&reg, 32);
+		unsigned int irq;
+
+		/* clear the interrupt */
+		pci_write_reg(pcie, 1 << index, PCIEMSIFR);
+
+		irq = irq_find_mapping(msi->domain, index);
+		if (irq) {
+			if (test_bit(index, msi->used))
+				generic_handle_irq(irq);
+			else
+				dev_info(pcie->dev, "unhandled MSI\n");
+		} else {
+			/* Unknown MSI, just clear it */
+			dev_dbg(pcie->dev, "unexpected MSI\n");
+		}
+
+		/* see if there's any more pending in this vector */
+		reg = pci_read_reg(pcie, PCIEMSIFR);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int rcar_msi_setup_irq(struct msi_chip *chip, struct pci_dev *pdev,
+			      struct msi_desc *desc)
+{
+	struct rcar_msi *msi = to_rcar_msi(chip);
+	struct rcar_pcie *pcie = container_of(chip, struct rcar_pcie, msi.chip);
+	struct msi_msg msg;
+	unsigned int irq;
+	int hwirq;
+
+	hwirq = rcar_msi_alloc(msi);
+	if (hwirq < 0)
+		return hwirq;
+
+	irq = irq_create_mapping(msi->domain, hwirq);
+	if (!irq) {
+		rcar_msi_free(msi, hwirq);
+		return -EINVAL;
+	}
+
+	irq_set_msi_desc(irq, desc);
+
+	msg.address_lo = pci_read_reg(pcie, PCIEMSIALR) & ~MSIFE;
+	msg.address_hi = pci_read_reg(pcie, PCIEMSIAUR);
+	msg.data = hwirq;
+
+	write_msi_msg(irq, &msg);
+
+	return 0;
+}
+
+static void rcar_msi_teardown_irq(struct msi_chip *chip, unsigned int irq)
+{
+	struct rcar_msi *msi = to_rcar_msi(chip);
+	struct irq_data *d = irq_get_irq_data(irq);
+
+	rcar_msi_free(msi, d->hwirq);
+}
+
+static struct irq_chip rcar_msi_irq_chip = {
+	.name = "R-Car PCIe MSI",
+	.irq_enable = unmask_msi_irq,
+	.irq_disable = mask_msi_irq,
+	.irq_mask = mask_msi_irq,
+	.irq_unmask = unmask_msi_irq,
+};
+
+static int rcar_msi_map(struct irq_domain *domain, unsigned int irq,
+			irq_hw_number_t hwirq)
+{
+	irq_set_chip_and_handler(irq, &rcar_msi_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 msi_domain_ops = {
+	.map = rcar_msi_map,
+};
+
+static int rcar_pcie_enable_msi(struct rcar_pcie *pcie)
+{
+	struct platform_device *pdev = to_platform_device(pcie->dev);
+	struct rcar_msi *msi = &pcie->msi;
+	unsigned long base;
+	int err;
+
+	mutex_init(&msi->lock);
+
+	msi->chip.dev = pcie->dev;
+	msi->chip.setup_irq = rcar_msi_setup_irq;
+	msi->chip.teardown_irq = rcar_msi_teardown_irq;
+
+	msi->domain = irq_domain_add_linear(pcie->dev->of_node, INT_PCI_MSI_NR,
+					    &msi_domain_ops, &msi->chip);
+	if (!msi->domain) {
+		dev_err(&pdev->dev, "failed to create IRQ domain\n");
+		return -ENOMEM;
+	}
+
+	/* Two irqs are for MSI, but they are also used for non-MSI irqs */
+	err = devm_request_irq(&pdev->dev, msi->irq1, rcar_pcie_msi_irq,
+			       IRQF_SHARED, rcar_msi_irq_chip.name, pcie);
+	if (err < 0) {
+		dev_err(&pdev->dev, "failed to request IRQ: %d\n", err);
+		goto err;
+	}
+
+	err = devm_request_irq(&pdev->dev, msi->irq2, rcar_pcie_msi_irq,
+			       IRQF_SHARED, rcar_msi_irq_chip.name, pcie);
+	if (err < 0) {
+		dev_err(&pdev->dev, "failed to request IRQ: %d\n", err);
+		goto err;
+	}
+
+	/* setup MSI data target */
+	msi->pages = __get_free_pages(GFP_KERNEL, 0);
+	base = virt_to_phys((void *)msi->pages);
+
+	pci_write_reg(pcie, base | MSIFE, PCIEMSIALR);
+	pci_write_reg(pcie, 0, PCIEMSIAUR);
+
+	/* enable all MSI interrupts */
+	pci_write_reg(pcie, 0xffffffff, PCIEMSIIER);
+
+	return 0;
+
+err:
+	irq_domain_remove(msi->domain);
+	return err;
+}
+
+static int rcar_pcie_get_resources(struct platform_device *pdev,
+				   struct rcar_pcie *pcie)
+{
+	struct resource res;
+	int err, i;
+
+	err = of_address_to_resource(pdev->dev.of_node, 0, &res);
+	if (err)
+		return err;
+
+	pcie->clk = devm_clk_get(&pdev->dev, "pcie");
+	if (IS_ERR(pcie->clk)) {
+		dev_err(pcie->dev, "cannot get platform clock\n");
+		return PTR_ERR(pcie->clk);
+	}
+	err = clk_prepare_enable(pcie->clk);
+	if (err)
+		goto fail_clk;
+
+	pcie->bus_clk = devm_clk_get(&pdev->dev, "pcie_bus");
+	if (IS_ERR(pcie->bus_clk)) {
+		dev_err(pcie->dev, "cannot get pcie bus clock\n");
+		err = PTR_ERR(pcie->bus_clk);
+		goto fail_clk;
+	}
+	err = clk_prepare_enable(pcie->bus_clk);
+	if (err)
+		goto err_map_reg;
+
+	i = irq_of_parse_and_map(pdev->dev.of_node, 0);
+	if (i < 0) {
+		dev_err(pcie->dev, "cannot get platform resources for msi interrupt\n");
+		err = -ENOENT;
+		goto err_map_reg;
+	}
+	pcie->msi.irq1 = i;
+
+	i = irq_of_parse_and_map(pdev->dev.of_node, 1);
+	if (i < 0) {
+		dev_err(pcie->dev, "cannot get platform resources for msi interrupt\n");
+		err = -ENOENT;
+		goto err_map_reg;
+	}
+	pcie->msi.irq2 = i;
+
+	pcie->base = devm_ioremap_resource(&pdev->dev, &res);
+	if (IS_ERR(pcie->base)) {
+		err = PTR_ERR(pcie->base);
+		goto err_map_reg;
+	}
+
+	return 0;
+
+err_map_reg:
+	clk_disable_unprepare(pcie->bus_clk);
+fail_clk:
+	clk_disable_unprepare(pcie->clk);
+
+	return err;
+}
+
+static int rcar_pcie_inbound_ranges(struct rcar_pcie *pcie,
+				    struct of_pci_range *range,
+				    int *index)
+{
+	u64 restype = range->flags;
+	u64 cpu_addr = range->cpu_addr;
+	u64 cpu_end = range->cpu_addr + range->size;
+	u64 pci_addr = range->pci_addr;
+	u32 flags = LAM_64BIT | LAR_ENABLE;
+	u64 mask;
+	u64 size;
+	int idx = *index;
+
+	if (restype & IORESOURCE_PREFETCH)
+		flags |= LAM_PREFETCH;
+
+	/*
+	 * If the size of the range is larger than the alignment of the start
+	 * address, we have to use multiple entries to perform the mapping.
+	 */
+	if (cpu_addr > 0) {
+		unsigned long nr_zeros = __ffs64(cpu_addr);
+		u64 alignment = 1ULL << nr_zeros;
+		size = min(range->size, alignment);
+	} else {
+		size = range->size;
+	}
+	/* Hardware supports max 4GiB inbound region */
+	size = min(size, 1ULL << 32);
+
+	mask = roundup_pow_of_two(size) - 1;
+	mask &= ~0xf;
+
+	while (cpu_addr < cpu_end) {
+		/*
+		 * Set up 64-bit inbound regions as the range parser doesn't
+		 * distinguish between 32 and 64-bit types.
+		 */
+		pci_write_reg(pcie, lower_32_bits(pci_addr), PCIEPRAR(idx));
+		pci_write_reg(pcie, lower_32_bits(cpu_addr), PCIELAR(idx));
+		pci_write_reg(pcie, lower_32_bits(mask) | flags, PCIELAMR(idx));
+
+		pci_write_reg(pcie, upper_32_bits(pci_addr), PCIEPRAR(idx+1));
+		pci_write_reg(pcie, upper_32_bits(cpu_addr), PCIELAR(idx+1));
+		pci_write_reg(pcie, 0, PCIELAMR(idx+1));
+
+		pci_addr += size;
+		cpu_addr += size;
+		idx += 2;
+
+		if (idx > MAX_NR_INBOUND_MAPS) {
+			dev_err(pcie->dev, "Failed to map inbound regions!\n");
+			return -EINVAL;
+		}
+	}
+	*index = idx;
+
+	return 0;
+}
+
+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 rcar_pcie_parse_map_dma_ranges(struct rcar_pcie *pcie,
+					  struct device_node *np)
+{
+	struct of_pci_range range;
+	struct of_pci_range_parser parser;
+	int index = 0;
+	int err;
+
+	if (pci_dma_range_parser_init(&parser, np))
+		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(pcie->dev, "0x%08x 0x%016llx..0x%016llx -> 0x%016llx\n",
+			range.flags, range.cpu_addr, end, range.pci_addr);
+
+		err = rcar_pcie_inbound_ranges(pcie, &range, &index);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+static const struct of_device_id rcar_pcie_of_match[] = {
+	{ .compatible = "renesas,pcie-r8a7779", .data = rcar_pcie_hw_init_h1 },
+	{ .compatible = "renesas,pcie-r8a7790", .data = rcar_pcie_hw_init },
+	{ .compatible = "renesas,pcie-r8a7791", .data = rcar_pcie_hw_init },
+	{},
+};
+MODULE_DEVICE_TABLE(of, rcar_pcie_of_match);
+
+static int rcar_pcie_probe(struct platform_device *pdev)
+{
+	struct rcar_pcie *pcie;
+	unsigned int data;
+	struct of_pci_range range;
+	struct of_pci_range_parser parser;
+	const struct of_device_id *of_id;
+	int err, win = 0;
+	int (*hw_init_fn)(struct rcar_pcie *);
+
+	pcie = devm_kzalloc(&pdev->dev, sizeof(*pcie), GFP_KERNEL);
+	if (!pcie)
+		return -ENOMEM;
+
+	pcie->dev = &pdev->dev;
+	platform_set_drvdata(pdev, pcie);
+
+	/* Get the bus range */
+	if (of_pci_parse_bus_range(pdev->dev.of_node, &pcie->busn)) {
+		dev_err(&pdev->dev, "failed to parse bus-range property\n");
+		return -EINVAL;
+	}
+
+	if (of_pci_range_parser_init(&parser, pdev->dev.of_node)) {
+		dev_err(&pdev->dev, "missing ranges property\n");
+		return -EINVAL;
+	}
+
+	err = rcar_pcie_get_resources(pdev, pcie);
+	if (err < 0) {
+		dev_err(&pdev->dev, "failed to request resources: %d\n", err);
+		return err;
+	}
+
+	for_each_of_pci_range(&parser, &range) {
+		of_pci_range_to_resource(&range, pdev->dev.of_node,
+						&pcie->res[win++]);
+
+		if (win > PCI_MAX_RESOURCES)
+			break;
+	}
+
+	 err = rcar_pcie_parse_map_dma_ranges(pcie, pdev->dev.of_node);
+	 if (err)
+		return err;
+
+	if (IS_ENABLED(CONFIG_PCI_MSI)) {
+		err = rcar_pcie_enable_msi(pcie);
+		if (err < 0) {
+			dev_err(&pdev->dev,
+				"failed to enable MSI support: %d\n",
+				err);
+			return err;
+		}
+	}
+
+	of_id = of_match_device(rcar_pcie_of_match, pcie->dev);
+	if (!of_id || !of_id->data)
+		return -EINVAL;
+	hw_init_fn = of_id->data;
+
+	/* Failure to get a link might just be that no cards are inserted */
+	err = hw_init_fn(pcie);
+	if (err) {
+		dev_info(&pdev->dev, "PCIe link down\n");
+		return 0;
+	}
+
+	data = pci_read_reg(pcie, MACSR);
+	dev_info(&pdev->dev, "PCIe x%d: link up\n", (data >> 20) & 0x3f);
+
+	rcar_pcie_enable(pcie);
+
+	return 0;
+}
+
+static struct platform_driver rcar_pcie_driver = {
+	.driver = {
+		.name = DRV_NAME,
+		.owner = THIS_MODULE,
+		.of_match_table = rcar_pcie_of_match,
+		.suppress_bind_attrs = true,
+	},
+	.probe = rcar_pcie_probe,
+};
+module_platform_driver(rcar_pcie_driver);
+
+MODULE_AUTHOR("Phil Edworthy <phil.edworthy@renesas.com>");
+MODULE_DESCRIPTION("Renesas R-Car PCIe driver");
+MODULE_LICENSE("GPLv2");
diff --git a/drivers/pci/hotplug-pci.c b/drivers/pci/hotplug-pci.c
index 6258dc260d9..c68366cee6b 100644
--- a/drivers/pci/hotplug-pci.c
+++ b/drivers/pci/hotplug-pci.c
@@ -4,7 +4,7 @@
 #include <linux/export.h>
 #include "pci.h"
 
-int __ref pci_hp_add_bridge(struct pci_dev *dev)
+int pci_hp_add_bridge(struct pci_dev *dev)
 {
 	struct pci_bus *parent = dev->bus;
 	int pass, busnr, start = parent->busn_res.start;
diff --git a/drivers/pci/hotplug/acpiphp_glue.c b/drivers/pci/hotplug/acpiphp_glue.c
index bccc27ee103..75e17833021 100644
--- a/drivers/pci/hotplug/acpiphp_glue.c
+++ b/drivers/pci/hotplug/acpiphp_glue.c
@@ -41,7 +41,6 @@
 
 #define pr_fmt(fmt) "acpiphp_glue: " fmt
 
-#include <linux/init.h>
 #include <linux/module.h>
 
 #include <linux/kernel.h>
@@ -501,7 +500,7 @@ static int acpiphp_rescan_slot(struct acpiphp_slot *slot)
  * This function should be called per *physical slot*,
  * not per each slot object in ACPI namespace.
  */
-static void __ref enable_slot(struct acpiphp_slot *slot)
+static void enable_slot(struct acpiphp_slot *slot)
 {
 	struct pci_dev *dev;
 	struct pci_bus *bus = slot->bus;
@@ -516,8 +515,7 @@ static void __ref enable_slot(struct acpiphp_slot *slot)
 			if (PCI_SLOT(dev->devfn) != slot->device)
 				continue;
 
-			if (dev->hdr_type == PCI_HEADER_TYPE_BRIDGE ||
-			    dev->hdr_type == PCI_HEADER_TYPE_CARDBUS) {
+			if (pci_is_bridge(dev)) {
 				max = pci_scan_bridge(bus, dev, max, pass);
 				if (pass && dev->subordinate) {
 					check_hotplug_bridge(slot, dev);
diff --git a/drivers/pci/hotplug/cpci_hotplug_pci.c b/drivers/pci/hotplug/cpci_hotplug_pci.c
index 8c146485176..f6ef64c2ccb 100644
--- a/drivers/pci/hotplug/cpci_hotplug_pci.c
+++ b/drivers/pci/hotplug/cpci_hotplug_pci.c
@@ -250,7 +250,7 @@ int cpci_led_off(struct slot* slot)
  * Device configuration functions
  */
 
-int __ref cpci_configure_slot(struct slot *slot)
+int cpci_configure_slot(struct slot *slot)
 {
 	struct pci_dev *dev;
 	struct pci_bus *parent;
@@ -289,8 +289,7 @@ int __ref cpci_configure_slot(struct slot *slot)
 	list_for_each_entry(dev, &parent->devices, bus_list)
 		if (PCI_SLOT(dev->devfn) != PCI_SLOT(slot->devfn))
 			continue;
-		if ((dev->hdr_type == PCI_HEADER_TYPE_BRIDGE) ||
-		    (dev->hdr_type == PCI_HEADER_TYPE_CARDBUS))
+		if (pci_is_bridge(dev))
 			pci_hp_add_bridge(dev);
 
 
diff --git a/drivers/pci/hotplug/cpqphp_ctrl.c b/drivers/pci/hotplug/cpqphp_ctrl.c
index 11845b79679..f593585f278 100644
--- a/drivers/pci/hotplug/cpqphp_ctrl.c
+++ b/drivers/pci/hotplug/cpqphp_ctrl.c
@@ -709,7 +709,8 @@ static struct pci_resource *get_max_resource(struct pci_resource **head, u32 siz
 				temp = temp->next;
 			}
 
-			temp->next = max->next;
+			if (temp)
+				temp->next = max->next;
 		}
 
 		max->next = NULL;
diff --git a/drivers/pci/hotplug/cpqphp_nvram.c b/drivers/pci/hotplug/cpqphp_nvram.c
index 76ba8a1c774..9600a392eaa 100644
--- a/drivers/pci/hotplug/cpqphp_nvram.c
+++ b/drivers/pci/hotplug/cpqphp_nvram.c
@@ -34,7 +34,6 @@
 #include <linux/workqueue.h>
 #include <linux/pci.h>
 #include <linux/pci_hotplug.h>
-#include <linux/init.h>
 #include <asm/uaccess.h>
 #include "cpqphp.h"
 #include "cpqphp_nvram.h"
diff --git a/drivers/pci/hotplug/pciehp.h b/drivers/pci/hotplug/pciehp.h
index 8a66866b8cf..8e9012dca45 100644
--- a/drivers/pci/hotplug/pciehp.h
+++ b/drivers/pci/hotplug/pciehp.h
@@ -127,7 +127,7 @@ struct controller {
 #define HP_SUPR_RM(ctrl)	((ctrl)->slot_cap & PCI_EXP_SLTCAP_HPS)
 #define EMI(ctrl)		((ctrl)->slot_cap & PCI_EXP_SLTCAP_EIP)
 #define NO_CMD_CMPL(ctrl)	((ctrl)->slot_cap & PCI_EXP_SLTCAP_NCCS)
-#define PSN(ctrl)		((ctrl)->slot_cap >> 19)
+#define PSN(ctrl)		(((ctrl)->slot_cap & PCI_EXP_SLTCAP_PSN) >> 19)
 
 int pciehp_sysfs_enable_slot(struct slot *slot);
 int pciehp_sysfs_disable_slot(struct slot *slot);
diff --git a/drivers/pci/hotplug/pciehp_hpc.c b/drivers/pci/hotplug/pciehp_hpc.c
index d7d058fa19a..1463412cf7f 100644
--- a/drivers/pci/hotplug/pciehp_hpc.c
+++ b/drivers/pci/hotplug/pciehp_hpc.c
@@ -159,6 +159,8 @@ static void pcie_write_cmd(struct controller *ctrl, u16 cmd, u16 mask)
 
 	pcie_capability_read_word(pdev, PCI_EXP_SLTSTA, &slot_status);
 	if (slot_status & PCI_EXP_SLTSTA_CC) {
+		pcie_capability_write_word(pdev, PCI_EXP_SLTSTA,
+					   PCI_EXP_SLTSTA_CC);
 		if (!ctrl->no_cmd_complete) {
 			/*
 			 * After 1 sec and CMD_COMPLETED still not set, just
diff --git a/drivers/pci/hotplug/pciehp_pci.c b/drivers/pci/hotplug/pciehp_pci.c
index 1b533060ce6..b6cb1df6709 100644
--- a/drivers/pci/hotplug/pciehp_pci.c
+++ b/drivers/pci/hotplug/pciehp_pci.c
@@ -62,8 +62,7 @@ int pciehp_configure_device(struct slot *p_slot)
 	}
 
 	list_for_each_entry(dev, &parent->devices, bus_list)
-		if ((dev->hdr_type == PCI_HEADER_TYPE_BRIDGE) ||
-				(dev->hdr_type == PCI_HEADER_TYPE_CARDBUS))
+		if (pci_is_bridge(dev))
 			pci_hp_add_bridge(dev);
 
 	pci_assign_unassigned_bridge_resources(bridge);
diff --git a/drivers/pci/hotplug/pcihp_slot.c b/drivers/pci/hotplug/pcihp_slot.c
index 16f92035231..e246a10a0d2 100644
--- a/drivers/pci/hotplug/pcihp_slot.c
+++ b/drivers/pci/hotplug/pcihp_slot.c
@@ -160,8 +160,7 @@ void pci_configure_slot(struct pci_dev *dev)
 			(dev->class >> 8) == PCI_CLASS_BRIDGE_PCI)))
 		return;
 
-	if (dev->bus)
-		pcie_bus_configure_settings(dev->bus);
+	pcie_bus_configure_settings(dev->bus);
 
 	memset(&hpp, 0, sizeof(hpp));
 	ret = pci_get_hp_params(dev, &hpp);
diff --git a/drivers/pci/hotplug/rpadlpar_core.c b/drivers/pci/hotplug/rpadlpar_core.c
index 4fcdeedda31..7660232ef46 100644
--- a/drivers/pci/hotplug/rpadlpar_core.c
+++ b/drivers/pci/hotplug/rpadlpar_core.c
@@ -157,8 +157,7 @@ static void dlpar_pci_add_bus(struct device_node *dn)
 	}
 
 	/* Scan below the new bridge */
-	if (dev->hdr_type == PCI_HEADER_TYPE_BRIDGE ||
-	    dev->hdr_type == PCI_HEADER_TYPE_CARDBUS)
+	if (pci_is_bridge(dev))
 		of_scan_pci_bridge(dev);
 
 	/* Map IO space for child bus, which may or may not succeed */
diff --git a/drivers/pci/hotplug/rpaphp_core.c b/drivers/pci/hotplug/rpaphp_core.c
index 4796c15fba9..984d708552f 100644
--- a/drivers/pci/hotplug/rpaphp_core.c
+++ b/drivers/pci/hotplug/rpaphp_core.c
@@ -223,16 +223,16 @@ int rpaphp_get_drc_props(struct device_node *dn, int *drc_index,
 	type_tmp = (char *) &types[1];
 
 	/* Iterate through parent properties, looking for my-drc-index */
-	for (i = 0; i < indexes[0]; i++) {
+	for (i = 0; i < be32_to_cpu(indexes[0]); i++) {
 		if ((unsigned int) indexes[i + 1] == *my_index) {
 			if (drc_name)
 				*drc_name = name_tmp;
 			if (drc_type)
 				*drc_type = type_tmp;
 			if (drc_index)
-				*drc_index = *my_index;
+				*drc_index = be32_to_cpu(*my_index);
 			if (drc_power_domain)
-				*drc_power_domain = domains[i+1];
+				*drc_power_domain = be32_to_cpu(domains[i+1]);
 			return 0;
 		}
 		name_tmp += (strlen(name_tmp) + 1);
@@ -321,16 +321,19 @@ int rpaphp_add_slot(struct device_node *dn)
 	/* register PCI devices */
 	name = (char *) &names[1];
 	type = (char *) &types[1];
-	for (i = 0; i < indexes[0]; i++) {
+	for (i = 0; i < be32_to_cpu(indexes[0]); i++) {
+		int index;
 
-		slot = alloc_slot_struct(dn, indexes[i + 1], name, power_domains[i + 1]);
+		index = be32_to_cpu(indexes[i + 1]);
+		slot = alloc_slot_struct(dn, index, name,
+					 be32_to_cpu(power_domains[i + 1]));
 		if (!slot)
 			return -ENOMEM;
 
 		slot->type = simple_strtoul(type, NULL, 10);
 
 		dbg("Found drc-index:0x%x drc-name:%s drc-type:%s\n",
-				indexes[i + 1], name, type);
+				index, name, type);
 
 		retval = rpaphp_enable_slot(slot);
 		if (!retval)
diff --git a/drivers/pci/hotplug/s390_pci_hpc.c b/drivers/pci/hotplug/s390_pci_hpc.c
index 8d2ce22151e..d1332d2f873 100644
--- a/drivers/pci/hotplug/s390_pci_hpc.c
+++ b/drivers/pci/hotplug/s390_pci_hpc.c
@@ -15,7 +15,6 @@
 #include <linux/slab.h>
 #include <linux/pci.h>
 #include <linux/pci_hotplug.h>
-#include <linux/init.h>
 #include <asm/pci_debug.h>
 #include <asm/sclp.h>
 
diff --git a/drivers/pci/hotplug/shpchp_ctrl.c b/drivers/pci/hotplug/shpchp_ctrl.c
index 58499277903..6efc2ec5e4d 100644
--- a/drivers/pci/hotplug/shpchp_ctrl.c
+++ b/drivers/pci/hotplug/shpchp_ctrl.c
@@ -282,8 +282,8 @@ static int board_added(struct slot *p_slot)
 		return WRONG_BUS_FREQUENCY;
 	}
 
-	bsp = ctrl->pci_dev->bus->cur_bus_speed;
-	msp = ctrl->pci_dev->bus->max_bus_speed;
+	bsp = ctrl->pci_dev->subordinate->cur_bus_speed;
+	msp = ctrl->pci_dev->subordinate->max_bus_speed;
 
 	/* Check if there are other slots or devices on the same bus */
 	if (!list_empty(&ctrl->pci_dev->subordinate->devices))
diff --git a/drivers/pci/hotplug/shpchp_pci.c b/drivers/pci/hotplug/shpchp_pci.c
index 2bf69fe1926..9202d133485 100644
--- a/drivers/pci/hotplug/shpchp_pci.c
+++ b/drivers/pci/hotplug/shpchp_pci.c
@@ -34,7 +34,7 @@
 #include "../pci.h"
 #include "shpchp.h"
 
-int __ref shpchp_configure_device(struct slot *p_slot)
+int shpchp_configure_device(struct slot *p_slot)
 {
 	struct pci_dev *dev;
 	struct controller *ctrl = p_slot->ctrl;
@@ -64,8 +64,7 @@ int __ref shpchp_configure_device(struct slot *p_slot)
 	list_for_each_entry(dev, &parent->devices, bus_list) {
 		if (PCI_SLOT(dev->devfn) != p_slot->device)
 			continue;
-		if ((dev->hdr_type == PCI_HEADER_TYPE_BRIDGE) ||
-		    (dev->hdr_type == PCI_HEADER_TYPE_CARDBUS))
+		if (pci_is_bridge(dev))
 			pci_hp_add_bridge(dev);
 	}
 
diff --git a/drivers/pci/htirq.c b/drivers/pci/htirq.c
index 6e373ea57b3..d68b030ab53 100644
--- a/drivers/pci/htirq.c
+++ b/drivers/pci/htirq.c
@@ -87,12 +87,9 @@ void unmask_ht_irq(struct irq_data *data)
 int __ht_create_irq(struct pci_dev *dev, int idx, ht_irq_update_t *update)
 {
 	struct ht_irq_cfg *cfg;
+	int max_irq, pos, irq;
 	unsigned long flags;
 	u32 data;
-	int max_irq;
-	int pos;
-	int irq;
-	int node;
 
 	pos = pci_find_ht_capability(dev, HT_CAPTYPE_IRQ);
 	if (!pos)
@@ -120,10 +117,8 @@ int __ht_create_irq(struct pci_dev *dev, int idx, ht_irq_update_t *update)
 	cfg->msg.address_lo = 0xffffffff;
 	cfg->msg.address_hi = 0xffffffff;
 
-	node = dev_to_node(&dev->dev);
-	irq = create_irq_nr(0, node);
-
-	if (irq <= 0) {
+	irq = irq_alloc_hwirq(dev_to_node(&dev->dev));
+	if (!irq) {
 		kfree(cfg);
 		return -EBUSY;
 	}
@@ -166,7 +161,7 @@ void ht_destroy_irq(unsigned int irq)
 	cfg = irq_get_handler_data(irq);
 	irq_set_chip(irq, NULL);
 	irq_set_handler_data(irq, NULL);
-	destroy_irq(irq);
+	irq_free_hwirq(irq);
 
 	kfree(cfg);
 }
diff --git a/drivers/pci/iov.c b/drivers/pci/iov.c
index de7a74782f9..cb6f24740ee 100644
--- a/drivers/pci/iov.c
+++ b/drivers/pci/iov.c
@@ -106,7 +106,7 @@ static int virtfn_add(struct pci_dev *dev, int id, int reset)
 	pci_device_add(virtfn, virtfn->bus);
 	mutex_unlock(&iov->dev->sriov->lock);
 
-	rc = pci_bus_add_device(virtfn);
+	pci_bus_add_device(virtfn);
 	sprintf(buf, "virtfn%u", id);
 	rc = sysfs_create_link(&dev->dev.kobj, &virtfn->dev.kobj, buf);
 	if (rc)
diff --git a/drivers/pci/msi.c b/drivers/pci/msi.c
index 955ab7990c5..27a7e67ddfe 100644
--- a/drivers/pci/msi.c
+++ b/drivers/pci/msi.c
@@ -10,7 +10,6 @@
 #include <linux/mm.h>
 #include <linux/irq.h>
 #include <linux/interrupt.h>
-#include <linux/init.h>
 #include <linux/export.h>
 #include <linux/ioport.h>
 #include <linux/pci.h>
@@ -544,22 +543,18 @@ static int populate_msi_sysfs(struct pci_dev *pdev)
 	if (!msi_attrs)
 		return -ENOMEM;
 	list_for_each_entry(entry, &pdev->msi_list, list) {
-		char *name = kmalloc(20, GFP_KERNEL);
-		if (!name)
-			goto error_attrs;
-
 		msi_dev_attr = kzalloc(sizeof(*msi_dev_attr), GFP_KERNEL);
-		if (!msi_dev_attr) {
-			kfree(name);
+		if (!msi_dev_attr)
 			goto error_attrs;
-		}
+		msi_attrs[count] = &msi_dev_attr->attr;
 
-		sprintf(name, "%d", entry->irq);
 		sysfs_attr_init(&msi_dev_attr->attr);
-		msi_dev_attr->attr.name = name;
+		msi_dev_attr->attr.name = kasprintf(GFP_KERNEL, "%d",
+						    entry->irq);
+		if (!msi_dev_attr->attr.name)
+			goto error_attrs;
 		msi_dev_attr->attr.mode = S_IRUGO;
 		msi_dev_attr->show = msi_mode_show;
-		msi_attrs[count] = &msi_dev_attr->attr;
 		++count;
 	}
 
@@ -883,50 +878,6 @@ int pci_msi_vec_count(struct pci_dev *dev)
 }
 EXPORT_SYMBOL(pci_msi_vec_count);
 
-/**
- * pci_enable_msi_block - configure device's MSI capability structure
- * @dev: device to configure
- * @nvec: number of interrupts to configure
- *
- * Allocate IRQs for a device with the MSI capability.
- * This function returns a negative errno if an error occurs.  If it
- * is unable to allocate the number of interrupts requested, it returns
- * the number of interrupts it might be able to allocate.  If it successfully
- * allocates at least the number of interrupts requested, it returns 0 and
- * updates the @dev's irq member to the lowest new interrupt number; the
- * other interrupt numbers allocated to this device are consecutive.
- */
-int pci_enable_msi_block(struct pci_dev *dev, int nvec)
-{
-	int status, maxvec;
-
-	if (dev->current_state != PCI_D0)
-		return -EINVAL;
-
-	maxvec = pci_msi_vec_count(dev);
-	if (maxvec < 0)
-		return maxvec;
-	if (nvec > maxvec)
-		return maxvec;
-
-	status = pci_msi_check_device(dev, nvec, PCI_CAP_ID_MSI);
-	if (status)
-		return status;
-
-	WARN_ON(!!dev->msi_enabled);
-
-	/* Check whether driver already requested MSI-X irqs */
-	if (dev->msix_enabled) {
-		dev_info(&dev->dev, "can't enable MSI "
-			 "(MSI-X already enabled)\n");
-		return -EINVAL;
-	}
-
-	status = msi_capability_init(dev, nvec);
-	return status;
-}
-EXPORT_SYMBOL(pci_enable_msi_block);
-
 void pci_msi_shutdown(struct pci_dev *dev)
 {
 	struct msi_desc *desc;
@@ -1132,14 +1083,45 @@ void pci_msi_init_pci_dev(struct pci_dev *dev)
  **/
 int pci_enable_msi_range(struct pci_dev *dev, int minvec, int maxvec)
 {
-	int nvec = maxvec;
+	int nvec;
 	int rc;
 
+	if (dev->current_state != PCI_D0)
+		return -EINVAL;
+
+	WARN_ON(!!dev->msi_enabled);
+
+	/* Check whether driver already requested MSI-X irqs */
+	if (dev->msix_enabled) {
+		dev_info(&dev->dev,
+			 "can't enable MSI (MSI-X already enabled)\n");
+		return -EINVAL;
+	}
+
 	if (maxvec < minvec)
 		return -ERANGE;
 
+	nvec = pci_msi_vec_count(dev);
+	if (nvec < 0)
+		return nvec;
+	else if (nvec < minvec)
+		return -EINVAL;
+	else if (nvec > 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 = pci_enable_msi_block(dev, nvec);
+		rc = msi_capability_init(dev, nvec);
 		if (rc < 0) {
 			return rc;
 		} else if (rc > 0) {
diff --git a/drivers/pci/pci-acpi.c b/drivers/pci/pci-acpi.c
index f49abef8848..ca4927ba843 100644
--- a/drivers/pci/pci-acpi.c
+++ b/drivers/pci/pci-acpi.c
@@ -309,13 +309,7 @@ static struct acpi_device *acpi_pci_find_companion(struct device *dev)
 	bool check_children;
 	u64 addr;
 
-	/*
-	 * pci_is_bridge() is not suitable here, because pci_dev->subordinate
-	 * is set only after acpi_pci_find_device() has been called for the
-	 * given device.
-	 */
-	check_children = pci_dev->hdr_type == PCI_HEADER_TYPE_BRIDGE
-			|| pci_dev->hdr_type == PCI_HEADER_TYPE_CARDBUS;
+	check_children = pci_is_bridge(pci_dev);
 	/* Please ref to ACPI spec for the syntax of _ADR */
 	addr = (PCI_SLOT(pci_dev->devfn) << 16) | PCI_FUNC(pci_dev->devfn);
 	return acpi_find_child_device(ACPI_COMPANION(dev->parent), addr,
diff --git a/drivers/pci/pci-driver.c b/drivers/pci/pci-driver.c
index d911e0c1f35..837d71f5390 100644
--- a/drivers/pci/pci-driver.c
+++ b/drivers/pci/pci-driver.c
@@ -107,7 +107,7 @@ store_new_id(struct device_driver *driver, const char *buf, size_t count)
 		subdevice=PCI_ANY_ID, class=0, class_mask=0;
 	unsigned long driver_data=0;
 	int fields=0;
-	int retval;
+	int retval = 0;
 
 	fields = sscanf(buf, "%x %x %x %x %x %x %lx",
 			&vendor, &device, &subvendor, &subdevice,
@@ -115,6 +115,26 @@ store_new_id(struct device_driver *driver, const char *buf, size_t count)
 	if (fields < 2)
 		return -EINVAL;
 
+	if (fields != 7) {
+		struct pci_dev *pdev = kzalloc(sizeof(*pdev), GFP_KERNEL);
+		if (!pdev)
+			return -ENOMEM;
+
+		pdev->vendor = vendor;
+		pdev->device = device;
+		pdev->subsystem_vendor = subvendor;
+		pdev->subsystem_device = subdevice;
+		pdev->class = class;
+
+		if (pci_match_id(pdrv->id_table, pdev))
+			retval = -EEXIST;
+
+		kfree(pdev);
+
+		if (retval)
+			return retval;
+	}
+
 	/* Only accept driver_data values that match an existing id_table
 	   entry */
 	if (ids) {
@@ -216,6 +236,13 @@ const struct pci_device_id *pci_match_id(const struct pci_device_id *ids,
 	return NULL;
 }
 
+static const struct pci_device_id pci_device_id_any = {
+	.vendor = PCI_ANY_ID,
+	.device = PCI_ANY_ID,
+	.subvendor = PCI_ANY_ID,
+	.subdevice = PCI_ANY_ID,
+};
+
 /**
  * pci_match_device - Tell if a PCI device structure has a matching PCI device id structure
  * @drv: the PCI driver to match against
@@ -229,18 +256,30 @@ static const struct pci_device_id *pci_match_device(struct pci_driver *drv,
 						    struct pci_dev *dev)
 {
 	struct pci_dynid *dynid;
+	const struct pci_device_id *found_id = NULL;
+
+	/* When driver_override is set, only bind to the matching driver */
+	if (dev->driver_override && strcmp(dev->driver_override, drv->name))
+		return NULL;
 
 	/* Look at the dynamic ids first, before the static ones */
 	spin_lock(&drv->dynids.lock);
 	list_for_each_entry(dynid, &drv->dynids.list, node) {
 		if (pci_match_one_device(&dynid->id, dev)) {
-			spin_unlock(&drv->dynids.lock);
-			return &dynid->id;
+			found_id = &dynid->id;
+			break;
 		}
 	}
 	spin_unlock(&drv->dynids.lock);
 
-	return pci_match_id(drv->id_table, dev);
+	if (!found_id)
+		found_id = pci_match_id(drv->id_table, dev);
+
+	/* driver_override will always match, send a dummy id */
+	if (!found_id && dev->driver_override)
+		found_id = &pci_device_id_any;
+
+	return found_id;
 }
 
 struct drv_dev_and_id {
@@ -580,14 +619,14 @@ static void pci_pm_default_resume(struct pci_dev *pci_dev)
 {
 	pci_fixup_device(pci_fixup_resume, pci_dev);
 
-	if (!pci_is_bridge(pci_dev))
+	if (!pci_has_subordinate(pci_dev))
 		pci_enable_wake(pci_dev, PCI_D0, false);
 }
 
 static void pci_pm_default_suspend(struct pci_dev *pci_dev)
 {
 	/* Disable non-bridge devices without PM support */
-	if (!pci_is_bridge(pci_dev))
+	if (!pci_has_subordinate(pci_dev))
 		pci_disable_enabled_device(pci_dev);
 }
 
@@ -717,7 +756,7 @@ static int pci_pm_suspend_noirq(struct device *dev)
 
 	if (!pci_dev->state_saved) {
 		pci_save_state(pci_dev);
-		if (!pci_is_bridge(pci_dev))
+		if (!pci_has_subordinate(pci_dev))
 			pci_prepare_to_sleep(pci_dev);
 	}
 
@@ -971,7 +1010,7 @@ static int pci_pm_poweroff_noirq(struct device *dev)
 			return error;
 	}
 
-	if (!pci_dev->state_saved && !pci_is_bridge(pci_dev))
+	if (!pci_dev->state_saved && !pci_has_subordinate(pci_dev))
 		pci_prepare_to_sleep(pci_dev);
 
 	/*
@@ -1325,8 +1364,6 @@ static int pci_uevent(struct device *dev, struct kobj_uevent_env *env)
 		return -ENODEV;
 
 	pdev = to_pci_dev(dev);
-	if (!pdev)
-		return -ENODEV;
 
 	if (add_uevent_var(env, "PCI_CLASS=%04X", pdev->class))
 		return -ENOMEM;
@@ -1347,6 +1384,7 @@ static int pci_uevent(struct device *dev, struct kobj_uevent_env *env)
 			   (u8)(pdev->class >> 16), (u8)(pdev->class >> 8),
 			   (u8)(pdev->class)))
 		return -ENOMEM;
+
 	return 0;
 }
 
diff --git a/drivers/pci/pci-sysfs.c b/drivers/pci/pci-sysfs.c
index 4e0acefb756..84c350994b0 100644
--- a/drivers/pci/pci-sysfs.c
+++ b/drivers/pci/pci-sysfs.c
@@ -29,6 +29,7 @@
 #include <linux/slab.h>
 #include <linux/vgaarb.h>
 #include <linux/pm_runtime.h>
+#include <linux/of.h>
 #include "pci.h"
 
 static int sysfs_initialized;	/* = 0 */
@@ -416,6 +417,20 @@ static ssize_t d3cold_allowed_show(struct device *dev,
 static DEVICE_ATTR_RW(d3cold_allowed);
 #endif
 
+#ifdef CONFIG_OF
+static ssize_t devspec_show(struct device *dev,
+			    struct device_attribute *attr, char *buf)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct device_node *np = pci_device_to_OF_node(pdev);
+
+	if (np == NULL || np->full_name == NULL)
+		return 0;
+	return sprintf(buf, "%s", np->full_name);
+}
+static DEVICE_ATTR_RO(devspec);
+#endif
+
 #ifdef CONFIG_PCI_IOV
 static ssize_t sriov_totalvfs_show(struct device *dev,
 				   struct device_attribute *attr,
@@ -499,6 +514,45 @@ static struct device_attribute sriov_numvfs_attr =
 		       sriov_numvfs_show, sriov_numvfs_store);
 #endif /* CONFIG_PCI_IOV */
 
+static ssize_t driver_override_store(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t count)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	char *driver_override, *old = pdev->driver_override, *cp;
+
+	if (count > PATH_MAX)
+		return -EINVAL;
+
+	driver_override = kstrndup(buf, count, GFP_KERNEL);
+	if (!driver_override)
+		return -ENOMEM;
+
+	cp = strchr(driver_override, '\n');
+	if (cp)
+		*cp = '\0';
+
+	if (strlen(driver_override)) {
+		pdev->driver_override = driver_override;
+	} else {
+		kfree(driver_override);
+		pdev->driver_override = NULL;
+	}
+
+	kfree(old);
+
+	return count;
+}
+
+static ssize_t driver_override_show(struct device *dev,
+				    struct device_attribute *attr, char *buf)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+
+	return sprintf(buf, "%s\n", pdev->driver_override);
+}
+static DEVICE_ATTR_RW(driver_override);
+
 static struct attribute *pci_dev_attrs[] = {
 	&dev_attr_resource.attr,
 	&dev_attr_vendor.attr,
@@ -521,6 +575,10 @@ static struct attribute *pci_dev_attrs[] = {
 #if defined(CONFIG_PM_RUNTIME) && defined(CONFIG_ACPI)
 	&dev_attr_d3cold_allowed.attr,
 #endif
+#ifdef CONFIG_OF
+	&dev_attr_devspec.attr,
+#endif
+	&dev_attr_driver_override.attr,
 	NULL,
 };
 
@@ -1255,11 +1313,6 @@ static struct bin_attribute pcie_config_attr = {
 	.write = pci_write_config,
 };
 
-int __weak pcibios_add_platform_entries(struct pci_dev *dev)
-{
-	return 0;
-}
-
 static ssize_t reset_store(struct device *dev,
 			   struct device_attribute *attr, const char *buf,
 			   size_t count)
@@ -1375,11 +1428,6 @@ int __must_check pci_create_sysfs_dev_files (struct pci_dev *pdev)
 		pdev->rom_attr = attr;
 	}
 
-	/* add platform-specific attributes */
-	retval = pcibios_add_platform_entries(pdev);
-	if (retval)
-		goto err_rom_file;
-
 	/* add sysfs entries for various capabilities */
 	retval = pci_create_capabilities_sysfs(pdev);
 	if (retval)
diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index 7325d43bf03..212c63d780e 100644
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -106,7 +106,7 @@ static bool pcie_ari_disabled;
  * Given a PCI bus, returns the highest PCI bus number present in the set
  * including the given PCI bus and its list of child PCI buses.
  */
-unsigned char pci_bus_max_busnr(struct pci_bus* bus)
+unsigned char pci_bus_max_busnr(struct pci_bus *bus)
 {
 	struct pci_bus *tmp;
 	unsigned char max, n;
@@ -1371,7 +1371,7 @@ static void pcim_release(struct device *gendev, void *res)
 		pci_disable_device(dev);
 }
 
-static struct pci_devres * get_pci_dr(struct pci_dev *pdev)
+static struct pci_devres *get_pci_dr(struct pci_dev *pdev)
 {
 	struct pci_devres *dr, *new_dr;
 
@@ -1385,7 +1385,7 @@ static struct pci_devres * get_pci_dr(struct pci_dev *pdev)
 	return devres_get(&pdev->dev, new_dr, NULL, NULL);
 }
 
-static struct pci_devres * find_pci_dr(struct pci_dev *pdev)
+static struct pci_devres *find_pci_dr(struct pci_dev *pdev)
 {
 	if (pci_is_managed(pdev))
 		return devres_find(&pdev->dev, pcim_release, NULL, NULL);
@@ -1468,6 +1468,17 @@ void __weak pcibios_release_device(struct pci_dev *dev) {}
  */
 void __weak pcibios_disable_device (struct pci_dev *dev) {}
 
+/**
+ * pcibios_penalize_isa_irq - penalize an ISA IRQ
+ * @irq: ISA IRQ to penalize
+ * @active: IRQ active or not
+ *
+ * Permits the platform to provide architecture-specific functionality when
+ * penalizing ISA IRQs. This is the default implementation. Architecture
+ * implementations can override this.
+ */
+void __weak pcibios_penalize_isa_irq(int irq, int active) {}
+
 static void do_pci_disable_device(struct pci_dev *dev)
 {
 	u16 pci_command;
@@ -3067,7 +3078,8 @@ int pci_wait_for_pending_transaction(struct pci_dev *dev)
 	if (!pci_is_pcie(dev))
 		return 1;
 
-	return pci_wait_for_pending(dev, PCI_EXP_DEVSTA, PCI_EXP_DEVSTA_TRPND);
+	return pci_wait_for_pending(dev, pci_pcie_cap(dev) + PCI_EXP_DEVSTA,
+				    PCI_EXP_DEVSTA_TRPND);
 }
 EXPORT_SYMBOL(pci_wait_for_pending_transaction);
 
@@ -3109,7 +3121,7 @@ static int pci_af_flr(struct pci_dev *dev, int probe)
 		return 0;
 
 	/* Wait for Transaction Pending bit clean */
-	if (pci_wait_for_pending(dev, PCI_AF_STATUS, PCI_AF_STATUS_TP))
+	if (pci_wait_for_pending(dev, pos + PCI_AF_STATUS, PCI_AF_STATUS_TP))
 		goto clear;
 
 	dev_err(&dev->dev, "transaction is not cleared; "
@@ -3167,14 +3179,7 @@ static int pci_pm_reset(struct pci_dev *dev, int probe)
 	return 0;
 }
 
-/**
- * pci_reset_bridge_secondary_bus - Reset the secondary bus on a PCI bridge.
- * @dev: Bridge device
- *
- * Use the bridge control register to assert reset on the secondary bus.
- * Devices on the secondary bus are left in power-on state.
- */
-void pci_reset_bridge_secondary_bus(struct pci_dev *dev)
+void __weak pcibios_reset_secondary_bus(struct pci_dev *dev)
 {
 	u16 ctrl;
 
@@ -3199,6 +3204,18 @@ void pci_reset_bridge_secondary_bus(struct pci_dev *dev)
 	 */
 	ssleep(1);
 }
+
+/**
+ * pci_reset_bridge_secondary_bus - Reset the secondary bus on a PCI bridge.
+ * @dev: Bridge device
+ *
+ * Use the bridge control register to assert reset on the secondary bus.
+ * Devices on the secondary bus are left in power-on state.
+ */
+void pci_reset_bridge_secondary_bus(struct pci_dev *dev)
+{
+	pcibios_reset_secondary_bus(dev);
+}
 EXPORT_SYMBOL_GPL(pci_reset_bridge_secondary_bus);
 
 static int pci_parent_bus_reset(struct pci_dev *dev, int probe)
@@ -3305,8 +3322,27 @@ static void pci_dev_unlock(struct pci_dev *dev)
 	pci_cfg_access_unlock(dev);
 }
 
+/**
+ * pci_reset_notify - notify device driver of reset
+ * @dev: device to be notified of reset
+ * @prepare: 'true' if device is about to be reset; 'false' if reset attempt
+ *           completed
+ *
+ * Must be called prior to device access being disabled and after device
+ * access is restored.
+ */
+static void pci_reset_notify(struct pci_dev *dev, bool prepare)
+{
+	const struct pci_error_handlers *err_handler =
+			dev->driver ? dev->driver->err_handler : NULL;
+	if (err_handler && err_handler->reset_notify)
+		err_handler->reset_notify(dev, prepare);
+}
+
 static void pci_dev_save_and_disable(struct pci_dev *dev)
 {
+	pci_reset_notify(dev, true);
+
 	/*
 	 * Wake-up device prior to save.  PM registers default to D0 after
 	 * reset and a simple register restore doesn't reliably return
@@ -3328,6 +3364,7 @@ static void pci_dev_save_and_disable(struct pci_dev *dev)
 static void pci_dev_restore(struct pci_dev *dev)
 {
 	pci_restore_state(dev);
+	pci_reset_notify(dev, false);
 }
 
 static int pci_dev_reset(struct pci_dev *dev, int probe)
@@ -3344,6 +3381,7 @@ static int pci_dev_reset(struct pci_dev *dev, int probe)
 
 	return rc;
 }
+
 /**
  * __pci_reset_function - reset a PCI device function
  * @dev: PCI device to reset
@@ -4125,7 +4163,7 @@ int pci_set_vga_state(struct pci_dev *dev, bool decode,
 	u16 cmd;
 	int rc;
 
-	WARN_ON((flags & PCI_VGA_STATE_CHANGE_DECODES) & (command_bits & ~(PCI_COMMAND_IO|PCI_COMMAND_MEMORY)));
+	WARN_ON((flags & PCI_VGA_STATE_CHANGE_DECODES) && (command_bits & ~(PCI_COMMAND_IO|PCI_COMMAND_MEMORY)));
 
 	/* ARCH specific VGA enables */
 	rc = pci_set_vga_state_arch(dev, decode, command_bits, flags);
diff --git a/drivers/pci/pci.h b/drivers/pci/pci.h
index 6bd082299e3..0601890db22 100644
--- a/drivers/pci/pci.h
+++ b/drivers/pci/pci.h
@@ -77,7 +77,7 @@ static inline void pci_wakeup_event(struct pci_dev *dev)
 	pm_wakeup_event(&dev->dev, 100);
 }
 
-static inline bool pci_is_bridge(struct pci_dev *pci_dev)
+static inline bool pci_has_subordinate(struct pci_dev *pci_dev)
 {
 	return !!(pci_dev->subordinate);
 }
@@ -201,11 +201,11 @@ int __pci_read_base(struct pci_dev *dev, enum pci_bar_type type,
 		    struct resource *res, unsigned int reg);
 int pci_resource_bar(struct pci_dev *dev, int resno, enum pci_bar_type *type);
 void pci_configure_ari(struct pci_dev *dev);
-void __ref __pci_bus_size_bridges(struct pci_bus *bus,
+void __pci_bus_size_bridges(struct pci_bus *bus,
 			struct list_head *realloc_head);
-void __ref __pci_bus_assign_resources(const struct pci_bus *bus,
-				      struct list_head *realloc_head,
-				      struct list_head *fail_head);
+void __pci_bus_assign_resources(const struct pci_bus *bus,
+				struct list_head *realloc_head,
+				struct list_head *fail_head);
 
 /**
  * pci_ari_enabled - query ARI forwarding status
diff --git a/drivers/pci/pcie/portdrv_core.c b/drivers/pci/pcie/portdrv_core.c
index 986f8eadfd3..2f0ce668a77 100644
--- a/drivers/pci/pcie/portdrv_core.c
+++ b/drivers/pci/pcie/portdrv_core.c
@@ -99,7 +99,7 @@ static int pcie_port_enable_msix(struct pci_dev *dev, int *vectors, int mask)
 	for (i = 0; i < nr_entries; i++)
 		msix_entries[i].entry = i;
 
-	status = pci_enable_msix(dev, msix_entries, nr_entries);
+	status = pci_enable_msix_exact(dev, msix_entries, nr_entries);
 	if (status)
 		goto Exit;
 
@@ -171,7 +171,7 @@ static int pcie_port_enable_msix(struct pci_dev *dev, int *vectors, int mask)
 		pci_disable_msix(dev);
 
 		/* Now allocate the MSI-X vectors for real */
-		status = pci_enable_msix(dev, msix_entries, nvec);
+		status = pci_enable_msix_exact(dev, msix_entries, nvec);
 		if (status)
 			goto Exit;
 	}
@@ -379,10 +379,13 @@ int pcie_port_device_register(struct pci_dev *dev)
 	/*
 	 * Initialize service irqs. Don't use service devices that
 	 * require interrupts if there is no way to generate them.
+	 * However, some drivers may have a polling mode (e.g. pciehp_poll_mode)
+	 * that can be used in the absence of irqs.  Allow them to determine
+	 * if that is to be used.
 	 */
 	status = init_service_irqs(dev, irqs, capabilities);
 	if (status) {
-		capabilities &= PCIE_PORT_SERVICE_VC;
+		capabilities &= PCIE_PORT_SERVICE_VC | PCIE_PORT_SERVICE_HP;
 		if (!capabilities)
 			goto error_disable;
 	}
diff --git a/drivers/pci/probe.c b/drivers/pci/probe.c
index ef09f5f2fe6..2bbf5221afb 100644
--- a/drivers/pci/probe.c
+++ b/drivers/pci/probe.c
@@ -171,9 +171,10 @@ int __pci_read_base(struct pci_dev *dev, enum pci_bar_type type,
 			struct resource *res, unsigned int pos)
 {
 	u32 l, sz, mask;
+	u64 l64, sz64, mask64;
 	u16 orig_cmd;
 	struct pci_bus_region region, inverted_region;
-	bool bar_too_big = false, bar_disabled = false;
+	bool bar_too_big = false, bar_too_high = false, bar_invalid = false;
 
 	mask = type ? PCI_ROM_ADDRESS_MASK : ~0;
 
@@ -226,9 +227,9 @@ int __pci_read_base(struct pci_dev *dev, enum pci_bar_type type,
 	}
 
 	if (res->flags & IORESOURCE_MEM_64) {
-		u64 l64 = l;
-		u64 sz64 = sz;
-		u64 mask64 = mask | (u64)~0 << 32;
+		l64 = l;
+		sz64 = sz;
+		mask64 = mask | (u64)~0 << 32;
 
 		pci_read_config_dword(dev, pos + 4, &l);
 		pci_write_config_dword(dev, pos + 4, ~0);
@@ -243,19 +244,22 @@ int __pci_read_base(struct pci_dev *dev, enum pci_bar_type type,
 		if (!sz64)
 			goto fail;
 
-		if ((sizeof(resource_size_t) < 8) && (sz64 > 0x100000000ULL)) {
+		if ((sizeof(dma_addr_t) < 8 || sizeof(resource_size_t) < 8) &&
+		    sz64 > 0x100000000ULL) {
+			res->flags |= IORESOURCE_UNSET | IORESOURCE_DISABLED;
+			res->start = 0;
+			res->end = 0;
 			bar_too_big = true;
-			goto fail;
+			goto out;
 		}
 
-		if ((sizeof(resource_size_t) < 8) && l) {
-			/* Address above 32-bit boundary; disable the BAR */
-			pci_write_config_dword(dev, pos, 0);
-			pci_write_config_dword(dev, pos + 4, 0);
+		if ((sizeof(dma_addr_t) < 8) && l) {
+			/* Above 32-bit boundary; try to reallocate */
 			res->flags |= IORESOURCE_UNSET;
-			region.start = 0;
-			region.end = sz64;
-			bar_disabled = true;
+			res->start = 0;
+			res->end = sz64;
+			bar_too_high = true;
+			goto out;
 		} else {
 			region.start = l64;
 			region.end = l64 + sz64;
@@ -285,11 +289,10 @@ int __pci_read_base(struct pci_dev *dev, enum pci_bar_type type,
 	 * be claimed by the device.
 	 */
 	if (inverted_region.start != region.start) {
-		dev_info(&dev->dev, "reg 0x%x: initial BAR value %pa invalid; forcing reassignment\n",
-			 pos, &region.start);
 		res->flags |= IORESOURCE_UNSET;
-		res->end -= res->start;
 		res->start = 0;
+		res->end = region.end - region.start;
+		bar_invalid = true;
 	}
 
 	goto out;
@@ -303,8 +306,15 @@ out:
 		pci_write_config_word(dev, PCI_COMMAND, orig_cmd);
 
 	if (bar_too_big)
-		dev_err(&dev->dev, "reg 0x%x: can't handle 64-bit BAR\n", pos);
-	if (res->flags && !bar_disabled)
+		dev_err(&dev->dev, "reg 0x%x: can't handle BAR larger than 4GB (size %#010llx)\n",
+			pos, (unsigned long long) sz64);
+	if (bar_too_high)
+		dev_info(&dev->dev, "reg 0x%x: can't handle BAR above 4G (bus address %#010llx)\n",
+			 pos, (unsigned long long) l64);
+	if (bar_invalid)
+		dev_info(&dev->dev, "reg 0x%x: initial BAR value %#010llx invalid\n",
+			 pos, (unsigned long long) region.start);
+	if (res->flags)
 		dev_printk(KERN_DEBUG, &dev->dev, "reg 0x%x: %pR\n", pos, res);
 
 	return (res->flags & IORESOURCE_MEM_64) ? 1 : 0;
@@ -465,7 +475,7 @@ void pci_read_bridge_bases(struct pci_bus *child)
 
 	if (dev->transparent) {
 		pci_bus_for_each_resource(child->parent, res, i) {
-			if (res) {
+			if (res && res->flags) {
 				pci_bus_add_resource(child, res,
 						     PCI_SUBTRACTIVE_DECODE);
 				dev_printk(KERN_DEBUG, &dev->dev,
@@ -719,7 +729,7 @@ add_dev:
 	return child;
 }
 
-struct pci_bus *__ref pci_add_new_bus(struct pci_bus *parent, struct pci_dev *dev, int busnr)
+struct pci_bus *pci_add_new_bus(struct pci_bus *parent, struct pci_dev *dev, int busnr)
 {
 	struct pci_bus *child;
 
@@ -984,6 +994,43 @@ void set_pcie_hotplug_bridge(struct pci_dev *pdev)
 
 
 /**
+ * pci_ext_cfg_is_aliased - is ext config space just an alias of std config?
+ * @dev: PCI device
+ *
+ * PCI Express to PCI/PCI-X Bridge Specification, rev 1.0, 4.1.4 says that
+ * when forwarding a type1 configuration request the bridge must check that
+ * the extended register address field is zero.  The bridge is not permitted
+ * to forward the transactions and must handle it as an Unsupported Request.
+ * Some bridges do not follow this rule and simply drop the extended register
+ * bits, resulting in the standard config space being aliased, every 256
+ * bytes across the entire configuration space.  Test for this condition by
+ * comparing the first dword of each potential alias to the vendor/device ID.
+ * Known offenders:
+ *   ASM1083/1085 PCIe-to-PCI Reversible Bridge (1b21:1080, rev 01 & 03)
+ *   AMD/ATI SBx00 PCI to PCI Bridge (1002:4384, rev 40)
+ */
+static bool pci_ext_cfg_is_aliased(struct pci_dev *dev)
+{
+#ifdef CONFIG_PCI_QUIRKS
+	int pos;
+	u32 header, tmp;
+
+	pci_read_config_dword(dev, PCI_VENDOR_ID, &header);
+
+	for (pos = PCI_CFG_SPACE_SIZE;
+	     pos < PCI_CFG_SPACE_EXP_SIZE; pos += PCI_CFG_SPACE_SIZE) {
+		if (pci_read_config_dword(dev, pos, &tmp) != PCIBIOS_SUCCESSFUL
+		    || header != tmp)
+			return false;
+	}
+
+	return true;
+#else
+	return false;
+#endif
+}
+
+/**
  * pci_cfg_space_size - get the configuration space size of the PCI device.
  * @dev: PCI device
  *
@@ -1001,7 +1048,7 @@ static int pci_cfg_space_size_ext(struct pci_dev *dev)
 
 	if (pci_read_config_dword(dev, pos, &status) != PCIBIOS_SUCCESSFUL)
 		goto fail;
-	if (status == 0xffffffff)
+	if (status == 0xffffffff || pci_ext_cfg_is_aliased(dev))
 		goto fail;
 
 	return PCI_CFG_SPACE_EXP_SIZE;
@@ -1215,6 +1262,7 @@ static void pci_release_dev(struct device *dev)
 	pci_release_of_node(pci_dev);
 	pcibios_release_device(pci_dev);
 	pci_bus_put(pci_dev->bus);
+	kfree(pci_dev->driver_override);
 	kfree(pci_dev);
 }
 
@@ -1369,7 +1417,7 @@ void pci_device_add(struct pci_dev *dev, struct pci_bus *bus)
 	WARN_ON(ret < 0);
 }
 
-struct pci_dev *__ref pci_scan_single_device(struct pci_bus *bus, int devfn)
+struct pci_dev *pci_scan_single_device(struct pci_bus *bus, int devfn)
 {
 	struct pci_dev *dev;
 
@@ -1617,7 +1665,7 @@ static int pcie_bus_configure_set(struct pci_dev *dev, void *data)
  */
 void pcie_bus_configure_settings(struct pci_bus *bus)
 {
-	u8 smpss;
+	u8 smpss = 0;
 
 	if (!bus->self)
 		return;
@@ -1670,8 +1718,7 @@ unsigned int pci_scan_child_bus(struct pci_bus *bus)
 
 	for (pass=0; pass < 2; pass++)
 		list_for_each_entry(dev, &bus->devices, bus_list) {
-			if (dev->hdr_type == PCI_HEADER_TYPE_BRIDGE ||
-			    dev->hdr_type == PCI_HEADER_TYPE_CARDBUS)
+			if (pci_is_bridge(dev))
 				max = pci_scan_bridge(bus, dev, max, pass);
 		}
 
@@ -1958,7 +2005,7 @@ EXPORT_SYMBOL(pci_scan_bus);
  *
  * Returns the max number of subordinate bus discovered.
  */
-unsigned int __ref pci_rescan_bus_bridge_resize(struct pci_dev *bridge)
+unsigned int pci_rescan_bus_bridge_resize(struct pci_dev *bridge)
 {
 	unsigned int max;
 	struct pci_bus *bus = bridge->subordinate;
@@ -1981,7 +2028,7 @@ unsigned int __ref pci_rescan_bus_bridge_resize(struct pci_dev *bridge)
  *
  * Returns the max number of subordinate bus discovered.
  */
-unsigned int __ref pci_rescan_bus(struct pci_bus *bus)
+unsigned int pci_rescan_bus(struct pci_bus *bus)
 {
 	unsigned int max;
 
diff --git a/drivers/pci/quirks.c b/drivers/pci/quirks.c
index e7292065a1b..92e68c7747f 100644
--- a/drivers/pci/quirks.c
+++ b/drivers/pci/quirks.c
@@ -2954,6 +2954,7 @@ static void disable_igfx_irq(struct pci_dev *dev)
 }
 DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_INTEL, 0x0102, disable_igfx_irq);
 DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_INTEL, 0x010a, disable_igfx_irq);
+DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_INTEL, 0x0152, disable_igfx_irq);
 
 /*
  * PCI devices which are on Intel chips can skip the 10ms delay
@@ -2991,6 +2992,14 @@ DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_CHELSIO, 0x0030,
 			 quirk_broken_intx_masking);
 DECLARE_PCI_FIXUP_HEADER(0x1814, 0x0601, /* Ralink RT2800 802.11n PCI */
 			 quirk_broken_intx_masking);
+/*
+ * Realtek RTL8169 PCI Gigabit Ethernet Controller (rev 10)
+ * Subsystem: Realtek RTL8169/8110 Family PCI Gigabit Ethernet NIC
+ *
+ * RTL8110SC - Fails under PCI device assignment using DisINTx masking.
+ */
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_REALTEK, 0x8169,
+			 quirk_broken_intx_masking);
 
 static void pci_do_fixups(struct pci_dev *dev, struct pci_fixup *f,
 			  struct pci_fixup *end)
@@ -3453,6 +3462,8 @@ static const u16 pci_quirk_intel_pch_acs_ids[] = {
 	/* Wildcat PCH */
 	0x9c90, 0x9c91, 0x9c92, 0x9c93, 0x9c94, 0x9c95, 0x9c96, 0x9c97,
 	0x9c98, 0x9c99, 0x9c9a, 0x9c9b,
+	/* Patsburg (X79) PCH */
+	0x1d10, 0x1d12, 0x1d14, 0x1d16, 0x1d18, 0x1d1a, 0x1d1c, 0x1d1e,
 };
 
 static bool pci_quirk_intel_pch_acs_match(struct pci_dev *dev)
diff --git a/drivers/pci/search.c b/drivers/pci/search.c
index 4a1b972efe7..8e495bda678 100644
--- a/drivers/pci/search.c
+++ b/drivers/pci/search.c
@@ -7,7 +7,6 @@
  *	Copyright (C) 2003 -- 2004 Greg Kroah-Hartman <greg@kroah.com>
  */
 
-#include <linux/init.h>
 #include <linux/pci.h>
 #include <linux/slab.h>
 #include <linux/module.h>
diff --git a/drivers/pci/setup-bus.c b/drivers/pci/setup-bus.c
index 138bdd6393b..fd9b545c3cf 100644
--- a/drivers/pci/setup-bus.c
+++ b/drivers/pci/setup-bus.c
@@ -713,12 +713,11 @@ static void pci_bridge_check_ranges(struct pci_bus *bus)
    bus resource of a given type. Note: we intentionally skip
    the bus resources which have already been assigned (that is,
    have non-NULL parent resource). */
-static struct resource *find_free_bus_resource(struct pci_bus *bus, unsigned long type)
+static struct resource *find_free_bus_resource(struct pci_bus *bus,
+			 unsigned long type_mask, unsigned long type)
 {
 	int i;
 	struct resource *r;
-	unsigned long type_mask = IORESOURCE_IO | IORESOURCE_MEM |
-				  IORESOURCE_PREFETCH;
 
 	pci_bus_for_each_resource(bus, r, i) {
 		if (r == &ioport_resource || r == &iomem_resource)
@@ -815,7 +814,8 @@ static void pbus_size_io(struct pci_bus *bus, resource_size_t min_size,
 		resource_size_t add_size, struct list_head *realloc_head)
 {
 	struct pci_dev *dev;
-	struct resource *b_res = find_free_bus_resource(bus, IORESOURCE_IO);
+	struct resource *b_res = find_free_bus_resource(bus, IORESOURCE_IO,
+							IORESOURCE_IO);
 	resource_size_t size = 0, size0 = 0, size1 = 0;
 	resource_size_t children_add_size = 0;
 	resource_size_t min_align, align;
@@ -907,36 +907,40 @@ static inline resource_size_t calculate_mem_align(resource_size_t *aligns,
  * @bus : the bus
  * @mask: mask the resource flag, then compare it with type
  * @type: the type of free resource from bridge
+ * @type2: second match type
+ * @type3: third match type
  * @min_size : the minimum memory window that must to be allocated
  * @add_size : additional optional memory window
  * @realloc_head : track the additional memory window on this list
  *
  * Calculate the size of the bus and minimal alignment which
  * guarantees that all child resources fit in this size.
+ *
+ * Returns -ENOSPC if there's no available bus resource of the desired type.
+ * Otherwise, sets the bus resource start/end to indicate the required
+ * size, adds things to realloc_head (if supplied), and returns 0.
  */
 static int pbus_size_mem(struct pci_bus *bus, unsigned long mask,
-			 unsigned long type, resource_size_t min_size,
-			resource_size_t add_size,
-			struct list_head *realloc_head)
+			 unsigned long type, unsigned long type2,
+			 unsigned long type3,
+			 resource_size_t min_size, resource_size_t add_size,
+			 struct list_head *realloc_head)
 {
 	struct pci_dev *dev;
 	resource_size_t min_align, align, size, size0, size1;
-	resource_size_t aligns[12];	/* Alignments from 1Mb to 2Gb */
+	resource_size_t aligns[14];	/* Alignments from 1Mb to 8Gb */
 	int order, max_order;
-	struct resource *b_res = find_free_bus_resource(bus, type);
-	unsigned int mem64_mask = 0;
+	struct resource *b_res = find_free_bus_resource(bus,
+					mask | IORESOURCE_PREFETCH, type);
 	resource_size_t children_add_size = 0;
 
 	if (!b_res)
-		return 0;
+		return -ENOSPC;
 
 	memset(aligns, 0, sizeof(aligns));
 	max_order = 0;
 	size = 0;
 
-	mem64_mask = b_res->flags & IORESOURCE_MEM_64;
-	b_res->flags &= ~IORESOURCE_MEM_64;
-
 	list_for_each_entry(dev, &bus->devices, bus_list) {
 		int i;
 
@@ -944,7 +948,9 @@ static int pbus_size_mem(struct pci_bus *bus, unsigned long mask,
 			struct resource *r = &dev->resource[i];
 			resource_size_t r_size;
 
-			if (r->parent || (r->flags & mask) != type)
+			if (r->parent || ((r->flags & mask) != type &&
+					  (r->flags & mask) != type2 &&
+					  (r->flags & mask) != type3))
 				continue;
 			r_size = resource_size(r);
 #ifdef CONFIG_PCI_IOV
@@ -957,10 +963,17 @@ static int pbus_size_mem(struct pci_bus *bus, unsigned long mask,
 				continue;
 			}
 #endif
-			/* For bridges size != alignment */
+			/*
+			 * aligns[0] is for 1MB (since bridge memory
+			 * windows are always at least 1MB aligned), so
+			 * keep "order" from being negative for smaller
+			 * resources.
+			 */
 			align = pci_resource_alignment(dev, r);
 			order = __ffs(align) - 20;
-			if (order > 11) {
+			if (order < 0)
+				order = 0;
+			if (order >= ARRAY_SIZE(aligns)) {
 				dev_warn(&dev->dev, "disabling BAR %d: %pR "
 					 "(bad alignment %#llx)\n", i, r,
 					 (unsigned long long) align);
@@ -968,15 +981,12 @@ static int pbus_size_mem(struct pci_bus *bus, unsigned long mask,
 				continue;
 			}
 			size += r_size;
-			if (order < 0)
-				order = 0;
 			/* Exclude ranges with size > align from
 			   calculation of the alignment. */
 			if (r_size == align)
 				aligns[order] += align;
 			if (order > max_order)
 				max_order = order;
-			mem64_mask &= r->flags & IORESOURCE_MEM_64;
 
 			if (realloc_head)
 				children_add_size += get_res_add_size(realloc_head, r);
@@ -997,18 +1007,18 @@ static int pbus_size_mem(struct pci_bus *bus, unsigned long mask,
 				 "%pR to %pR (unused)\n", b_res,
 				 &bus->busn_res);
 		b_res->flags = 0;
-		return 1;
+		return 0;
 	}
 	b_res->start = min_align;
 	b_res->end = size0 + min_align - 1;
-	b_res->flags |= IORESOURCE_STARTALIGN | mem64_mask;
+	b_res->flags |= IORESOURCE_STARTALIGN;
 	if (size1 > size0 && realloc_head) {
 		add_to_list(realloc_head, bus->self, b_res, size1-size0, min_align);
 		dev_printk(KERN_DEBUG, &bus->self->dev, "bridge window "
 				 "%pR to %pR add_size %llx\n", b_res,
 				 &bus->busn_res, (unsigned long long)size1-size0);
 	}
-	return 1;
+	return 0;
 }
 
 unsigned long pci_cardbus_resource_alignment(struct resource *res)
@@ -1113,12 +1123,13 @@ handle_done:
 	;
 }
 
-void __ref __pci_bus_size_bridges(struct pci_bus *bus,
-			struct list_head *realloc_head)
+void __pci_bus_size_bridges(struct pci_bus *bus, struct list_head *realloc_head)
 {
 	struct pci_dev *dev;
-	unsigned long mask, prefmask;
+	unsigned long mask, prefmask, type2 = 0, type3 = 0;
 	resource_size_t additional_mem_size = 0, additional_io_size = 0;
+	struct resource *b_res;
+	int ret;
 
 	list_for_each_entry(dev, &bus->devices, bus_list) {
 		struct pci_bus *b = dev->subordinate;
@@ -1152,41 +1163,93 @@ void __ref __pci_bus_size_bridges(struct pci_bus *bus,
 			additional_io_size  = pci_hotplug_io_size;
 			additional_mem_size = pci_hotplug_mem_size;
 		}
-		/*
-		 * Follow thru
-		 */
+		/* Fall through */
 	default:
 		pbus_size_io(bus, realloc_head ? 0 : additional_io_size,
 			     additional_io_size, realloc_head);
-		/* If the bridge supports prefetchable range, size it
-		   separately. If it doesn't, or its prefetchable window
-		   has already been allocated by arch code, try
-		   non-prefetchable range for both types of PCI memory
-		   resources. */
+
+		/*
+		 * If there's a 64-bit prefetchable MMIO window, compute
+		 * the size required to put all 64-bit prefetchable
+		 * resources in it.
+		 */
+		b_res = &bus->self->resource[PCI_BRIDGE_RESOURCES];
 		mask = IORESOURCE_MEM;
 		prefmask = IORESOURCE_MEM | IORESOURCE_PREFETCH;
-		if (pbus_size_mem(bus, prefmask, prefmask,
+		if (b_res[2].flags & IORESOURCE_MEM_64) {
+			prefmask |= IORESOURCE_MEM_64;
+			ret = pbus_size_mem(bus, prefmask, prefmask,
+				  prefmask, prefmask,
 				  realloc_head ? 0 : additional_mem_size,
-				  additional_mem_size, realloc_head))
-			mask = prefmask; /* Success, size non-prefetch only. */
-		else
-			additional_mem_size += additional_mem_size;
-		pbus_size_mem(bus, mask, IORESOURCE_MEM,
+				  additional_mem_size, realloc_head);
+
+			/*
+			 * If successful, all non-prefetchable resources
+			 * and any 32-bit prefetchable resources will go in
+			 * the non-prefetchable window.
+			 */
+			if (ret == 0) {
+				mask = prefmask;
+				type2 = prefmask & ~IORESOURCE_MEM_64;
+				type3 = prefmask & ~IORESOURCE_PREFETCH;
+			}
+		}
+
+		/*
+		 * If there is no 64-bit prefetchable window, compute the
+		 * size required to put all prefetchable resources in the
+		 * 32-bit prefetchable window (if there is one).
+		 */
+		if (!type2) {
+			prefmask &= ~IORESOURCE_MEM_64;
+			ret = pbus_size_mem(bus, prefmask, prefmask,
+					 prefmask, prefmask,
+					 realloc_head ? 0 : additional_mem_size,
+					 additional_mem_size, realloc_head);
+
+			/*
+			 * If successful, only non-prefetchable resources
+			 * will go in the non-prefetchable window.
+			 */
+			if (ret == 0)
+				mask = prefmask;
+			else
+				additional_mem_size += additional_mem_size;
+
+			type2 = type3 = IORESOURCE_MEM;
+		}
+
+		/*
+		 * Compute the size required to put everything else in the
+		 * non-prefetchable window.  This includes:
+		 *
+		 *   - all non-prefetchable resources
+		 *   - 32-bit prefetchable resources if there's a 64-bit
+		 *     prefetchable window or no prefetchable window at all
+		 *   - 64-bit prefetchable resources if there's no
+		 *     prefetchable window at all
+		 *
+		 * Note that the strategy in __pci_assign_resource() must
+		 * match that used here.  Specifically, we cannot put a
+		 * 32-bit prefetchable resource in a 64-bit prefetchable
+		 * window.
+		 */
+		pbus_size_mem(bus, mask, IORESOURCE_MEM, type2, type3,
 				realloc_head ? 0 : additional_mem_size,
 				additional_mem_size, realloc_head);
 		break;
 	}
 }
 
-void __ref pci_bus_size_bridges(struct pci_bus *bus)
+void pci_bus_size_bridges(struct pci_bus *bus)
 {
 	__pci_bus_size_bridges(bus, NULL);
 }
 EXPORT_SYMBOL(pci_bus_size_bridges);
 
-void __ref __pci_bus_assign_resources(const struct pci_bus *bus,
-				      struct list_head *realloc_head,
-				      struct list_head *fail_head)
+void __pci_bus_assign_resources(const struct pci_bus *bus,
+				struct list_head *realloc_head,
+				struct list_head *fail_head)
 {
 	struct pci_bus *b;
 	struct pci_dev *dev;
@@ -1218,15 +1281,15 @@ void __ref __pci_bus_assign_resources(const struct pci_bus *bus,
 	}
 }
 
-void __ref pci_bus_assign_resources(const struct pci_bus *bus)
+void pci_bus_assign_resources(const struct pci_bus *bus)
 {
 	__pci_bus_assign_resources(bus, NULL, NULL);
 }
 EXPORT_SYMBOL(pci_bus_assign_resources);
 
-static void __ref __pci_bridge_assign_resources(const struct pci_dev *bridge,
-					 struct list_head *add_head,
-					 struct list_head *fail_head)
+static void __pci_bridge_assign_resources(const struct pci_dev *bridge,
+					  struct list_head *add_head,
+					  struct list_head *fail_head)
 {
 	struct pci_bus *b;
 
@@ -1257,42 +1320,66 @@ static void __ref __pci_bridge_assign_resources(const struct pci_dev *bridge,
 static void pci_bridge_release_resources(struct pci_bus *bus,
 					  unsigned long type)
 {
-	int idx;
-	bool changed = false;
-	struct pci_dev *dev;
+	struct pci_dev *dev = bus->self;
 	struct resource *r;
 	unsigned long type_mask = IORESOURCE_IO | IORESOURCE_MEM |
-				  IORESOURCE_PREFETCH;
+				  IORESOURCE_PREFETCH | IORESOURCE_MEM_64;
+	unsigned old_flags = 0;
+	struct resource *b_res;
+	int idx = 1;
 
-	dev = bus->self;
-	for (idx = PCI_BRIDGE_RESOURCES; idx <= PCI_BRIDGE_RESOURCE_END;
-	     idx++) {
-		r = &dev->resource[idx];
-		if ((r->flags & type_mask) != type)
-			continue;
-		if (!r->parent)
-			continue;
-		/*
-		 * if there are children under that, we should release them
-		 *  all
-		 */
-		release_child_resources(r);
-		if (!release_resource(r)) {
-			dev_printk(KERN_DEBUG, &dev->dev,
-				 "resource %d %pR released\n", idx, r);
-			/* keep the old size */
-			r->end = resource_size(r) - 1;
-			r->start = 0;
-			r->flags = 0;
-			changed = true;
-		}
-	}
+	b_res = &dev->resource[PCI_BRIDGE_RESOURCES];
+
+	/*
+	 *     1. if there is io port assign fail, will release bridge
+	 *	  io port.
+	 *     2. if there is non pref mmio assign fail, release bridge
+	 *	  nonpref mmio.
+	 *     3. if there is 64bit pref mmio assign fail, and bridge pref
+	 *	  is 64bit, release bridge pref mmio.
+	 *     4. if there is pref mmio assign fail, and bridge pref is
+	 *	  32bit mmio, release bridge pref mmio
+	 *     5. if there is pref mmio assign fail, and bridge pref is not
+	 *	  assigned, release bridge nonpref mmio.
+	 */
+	if (type & IORESOURCE_IO)
+		idx = 0;
+	else if (!(type & IORESOURCE_PREFETCH))
+		idx = 1;
+	else if ((type & IORESOURCE_MEM_64) &&
+		 (b_res[2].flags & IORESOURCE_MEM_64))
+		idx = 2;
+	else if (!(b_res[2].flags & IORESOURCE_MEM_64) &&
+		 (b_res[2].flags & IORESOURCE_PREFETCH))
+		idx = 2;
+	else
+		idx = 1;
+
+	r = &b_res[idx];
+
+	if (!r->parent)
+		return;
+
+	/*
+	 * if there are children under that, we should release them
+	 *  all
+	 */
+	release_child_resources(r);
+	if (!release_resource(r)) {
+		type = old_flags = r->flags & type_mask;
+		dev_printk(KERN_DEBUG, &dev->dev, "resource %d %pR released\n",
+					PCI_BRIDGE_RESOURCES + idx, r);
+		/* keep the old size */
+		r->end = resource_size(r) - 1;
+		r->start = 0;
+		r->flags = 0;
 
-	if (changed) {
 		/* avoiding touch the one without PREF */
 		if (type & IORESOURCE_PREFETCH)
 			type = IORESOURCE_PREFETCH;
 		__pci_setup_bridge(bus, type);
+		/* for next child res under same bridge */
+		r->flags = old_flags;
 	}
 }
 
@@ -1304,9 +1391,9 @@ enum release_type {
  * try to release pci bridge resources that is from leaf bridge,
  * so we can allocate big new one later
  */
-static void __ref pci_bus_release_bridge_resources(struct pci_bus *bus,
-						   unsigned long type,
-						   enum release_type rel_type)
+static void pci_bus_release_bridge_resources(struct pci_bus *bus,
+					     unsigned long type,
+					     enum release_type rel_type)
 {
 	struct pci_dev *dev;
 	bool is_leaf_bridge = true;
@@ -1471,7 +1558,7 @@ void pci_assign_unassigned_root_bus_resources(struct pci_bus *bus)
 	LIST_HEAD(fail_head);
 	struct pci_dev_resource *fail_res;
 	unsigned long type_mask = IORESOURCE_IO | IORESOURCE_MEM |
-				  IORESOURCE_PREFETCH;
+				  IORESOURCE_PREFETCH | IORESOURCE_MEM_64;
 	int pci_try_num = 1;
 	enum enable_type enable_local;
 
@@ -1629,9 +1716,7 @@ void pci_assign_unassigned_bus_resources(struct pci_bus *bus)
 
 	down_read(&pci_bus_sem);
 	list_for_each_entry(dev, &bus->devices, bus_list)
-		if (dev->hdr_type == PCI_HEADER_TYPE_BRIDGE ||
-		    dev->hdr_type == PCI_HEADER_TYPE_CARDBUS)
-			if (dev->subordinate)
+		if (pci_is_bridge(dev) && pci_has_subordinate(dev))
 				__pci_bus_size_bridges(dev->subordinate,
 							 &add_list);
 	up_read(&pci_bus_sem);
diff --git a/drivers/pci/setup-irq.c b/drivers/pci/setup-irq.c
index 9bd6864ec5d..dbc4ffcf42d 100644
--- a/drivers/pci/setup-irq.c
+++ b/drivers/pci/setup-irq.c
@@ -10,7 +10,6 @@
  */
 
 
-#include <linux/init.h>
 #include <linux/kernel.h>
 #include <linux/pci.h>
 #include <linux/errno.h>
diff --git a/drivers/pci/setup-res.c b/drivers/pci/setup-res.c
index 7eed671d558..33f9e32d94d 100644
--- a/drivers/pci/setup-res.c
+++ b/drivers/pci/setup-res.c
@@ -16,7 +16,6 @@
  *	     Resource sorting
  */
 
-#include <linux/init.h>
 #include <linux/kernel.h>
 #include <linux/export.h>
 #include <linux/pci.h>
@@ -209,21 +208,42 @@ static int __pci_assign_resource(struct pci_bus *bus, struct pci_dev *dev,
 
 	min = (res->flags & IORESOURCE_IO) ? PCIBIOS_MIN_IO : PCIBIOS_MIN_MEM;
 
-	/* First, try exact prefetching match.. */
+	/*
+	 * First, try exact prefetching match.  Even if a 64-bit
+	 * prefetchable bridge window is below 4GB, we can't put a 32-bit
+	 * prefetchable resource in it because pbus_size_mem() assumes a
+	 * 64-bit window will contain no 32-bit resources.  If we assign
+	 * things differently than they were sized, not everything will fit.
+	 */
 	ret = pci_bus_alloc_resource(bus, res, size, align, min,
-				     IORESOURCE_PREFETCH,
+				     IORESOURCE_PREFETCH | IORESOURCE_MEM_64,
 				     pcibios_align_resource, dev);
+	if (ret == 0)
+		return 0;
 
-	if (ret < 0 && (res->flags & IORESOURCE_PREFETCH)) {
-		/*
-		 * That failed.
-		 *
-		 * But a prefetching area can handle a non-prefetching
-		 * window (it will just not perform as well).
-		 */
-		ret = pci_bus_alloc_resource(bus, res, size, align, min, 0,
+	/*
+	 * If the prefetchable window is only 32 bits wide, we can put
+	 * 64-bit prefetchable resources in it.
+	 */
+	if ((res->flags & (IORESOURCE_PREFETCH | IORESOURCE_MEM_64)) ==
+	     (IORESOURCE_PREFETCH | IORESOURCE_MEM_64)) {
+		ret = pci_bus_alloc_resource(bus, res, size, align, min,
+					     IORESOURCE_PREFETCH,
 					     pcibios_align_resource, dev);
+		if (ret == 0)
+			return 0;
 	}
+
+	/*
+	 * If we didn't find a better match, we can put any memory resource
+	 * in a non-prefetchable window.  If this resource is 32 bits and
+	 * non-prefetchable, the first call already tried the only possibility
+	 * so we don't need to try again.
+	 */
+	if (res->flags & (IORESOURCE_PREFETCH | IORESOURCE_MEM_64))
+		ret = pci_bus_alloc_resource(bus, res, size, align, min, 0,
+					     pcibios_align_resource, dev);
+
 	return ret;
 }
 
diff --git a/drivers/pci/xen-pcifront.c b/drivers/pci/xen-pcifront.c
index 179b8edc226..53df39a22c8 100644
--- a/drivers/pci/xen-pcifront.c
+++ b/drivers/pci/xen-pcifront.c
@@ -662,9 +662,9 @@ static void pcifront_do_aer(struct work_struct *data)
 	notify_remote_via_evtchn(pdev->evtchn);
 
 	/*in case of we lost an aer request in four lines time_window*/
-	smp_mb__before_clear_bit();
+	smp_mb__before_atomic();
 	clear_bit(_PDEVB_op_active, &pdev->flags);
-	smp_mb__after_clear_bit();
+	smp_mb__after_atomic();
 
 	schedule_pcifront_aer_op(pdev);
 
diff --git a/drivers/pcmcia/cardbus.c b/drivers/pcmcia/cardbus.c
index 8bde61952d2..4fe4cc4ae19 100644
--- a/drivers/pcmcia/cardbus.c
+++ b/drivers/pcmcia/cardbus.c
@@ -78,8 +78,7 @@ int __ref cb_alloc(struct pcmcia_socket *s)
 	max = bus->busn_res.start;
 	for (pass = 0; pass < 2; pass++)
 		list_for_each_entry(dev, &bus->devices, bus_list)
-			if (dev->hdr_type == PCI_HEADER_TYPE_BRIDGE ||
-			    dev->hdr_type == PCI_HEADER_TYPE_CARDBUS)
+			if (pci_is_bridge(dev))
 				max = pci_scan_bridge(bus, dev, max, pass);
 
 	/*
diff --git a/drivers/phy/Kconfig b/drivers/phy/Kconfig
index 4906c27fa3b..16a2f067c24 100644
--- a/drivers/phy/Kconfig
+++ b/drivers/phy/Kconfig
@@ -89,8 +89,8 @@ config PHY_EXYNOS_DP_VIDEO
 
 config BCM_KONA_USB2_PHY
 	tristate "Broadcom Kona USB2 PHY Driver"
-	depends on GENERIC_PHY
 	depends on HAS_IOMEM
+	select GENERIC_PHY
 	help
 	  Enable this to support the Broadcom Kona USB 2.0 PHY.
 
@@ -160,6 +160,17 @@ config PHY_EXYNOS5250_USB2
 	  particular SoC is compiled in the driver. In case of Exynos 5250 four
 	  phys are available - device, host, HSIC0 and HSIC.
 
+config PHY_EXYNOS5_USBDRD
+	tristate "Exynos5 SoC series USB DRD PHY driver"
+	depends on ARCH_EXYNOS5 && OF
+	depends on HAS_IOMEM
+	select GENERIC_PHY
+	select MFD_SYSCON
+	help
+	  Enable USB DRD PHY support for Exynos 5 SoC series.
+	  This driver provides PHY interface for USB 3.0 DRD controller
+	  present on Exynos5 SoC series.
+
 config PHY_XGENE
 	tristate "APM X-Gene 15Gbps PHY support"
 	depends on HAS_IOMEM && OF && (ARM64 || COMPILE_TEST)
diff --git a/drivers/phy/Makefile b/drivers/phy/Makefile
index 7728518572a..b4f1d577060 100644
--- a/drivers/phy/Makefile
+++ b/drivers/phy/Makefile
@@ -18,4 +18,5 @@ phy-exynos-usb2-y			+= phy-samsung-usb2.o
 phy-exynos-usb2-$(CONFIG_PHY_EXYNOS4210_USB2)	+= phy-exynos4210-usb2.o
 phy-exynos-usb2-$(CONFIG_PHY_EXYNOS4X12_USB2)	+= phy-exynos4x12-usb2.o
 phy-exynos-usb2-$(CONFIG_PHY_EXYNOS5250_USB2)	+= phy-exynos5250-usb2.o
+obj-$(CONFIG_PHY_EXYNOS5_USBDRD)	+= phy-exynos5-usbdrd.o
 obj-$(CONFIG_PHY_XGENE)			+= phy-xgene.o
diff --git a/drivers/phy/phy-exynos-mipi-video.c b/drivers/phy/phy-exynos-mipi-video.c
index 7f139326a64..ff026689358 100644
--- a/drivers/phy/phy-exynos-mipi-video.c
+++ b/drivers/phy/phy-exynos-mipi-video.c
@@ -101,7 +101,7 @@ static struct phy *exynos_mipi_video_phy_xlate(struct device *dev,
 {
 	struct exynos_mipi_video_phy *state = dev_get_drvdata(dev);
 
-	if (WARN_ON(args->args[0] > EXYNOS_MIPI_PHYS_NUM))
+	if (WARN_ON(args->args[0] >= EXYNOS_MIPI_PHYS_NUM))
 		return ERR_PTR(-ENODEV);
 
 	return state->phys[args->args[0]].phy;
diff --git a/drivers/phy/phy-exynos5-usbdrd.c b/drivers/phy/phy-exynos5-usbdrd.c
new file mode 100644
index 00000000000..76d862b2202
--- /dev/null
+++ b/drivers/phy/phy-exynos5-usbdrd.c
@@ -0,0 +1,676 @@
+/*
+ * Samsung EXYNOS5 SoC series USB DRD PHY driver
+ *
+ * Phy provider for USB 3.0 DRD controller on Exynos5 SoC series
+ *
+ * Copyright (C) 2014 Samsung Electronics Co., Ltd.
+ * Author: Vivek Gautam <gautam.vivek@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.
+ */
+
+#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/of_address.h>
+#include <linux/phy/phy.h>
+#include <linux/platform_device.h>
+#include <linux/mutex.h>
+#include <linux/mfd/syscon.h>
+#include <linux/mfd/syscon/exynos5-pmu.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+
+/* Exynos USB PHY registers */
+#define EXYNOS5_FSEL_9MHZ6		0x0
+#define EXYNOS5_FSEL_10MHZ		0x1
+#define EXYNOS5_FSEL_12MHZ		0x2
+#define EXYNOS5_FSEL_19MHZ2		0x3
+#define EXYNOS5_FSEL_20MHZ		0x4
+#define EXYNOS5_FSEL_24MHZ		0x5
+#define EXYNOS5_FSEL_50MHZ		0x7
+
+/* EXYNOS5: USB 3.0 DRD PHY registers */
+#define EXYNOS5_DRD_LINKSYSTEM			0x04
+
+#define LINKSYSTEM_FLADJ_MASK			(0x3f << 1)
+#define LINKSYSTEM_FLADJ(_x)			((_x) << 1)
+#define LINKSYSTEM_XHCI_VERSION_CONTROL		BIT(27)
+
+#define EXYNOS5_DRD_PHYUTMI			0x08
+
+#define PHYUTMI_OTGDISABLE			BIT(6)
+#define PHYUTMI_FORCESUSPEND			BIT(1)
+#define PHYUTMI_FORCESLEEP			BIT(0)
+
+#define EXYNOS5_DRD_PHYPIPE			0x0c
+
+#define EXYNOS5_DRD_PHYCLKRST			0x10
+
+#define PHYCLKRST_EN_UTMISUSPEND		BIT(31)
+
+#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			BIT(20)
+#define PHYCLKRST_REF_SSP_EN			BIT(19)
+#define PHYCLKRST_REF_CLKDIV2			BIT(18)
+
+#define PHYCLKRST_MPLL_MULTIPLIER_MASK		(0x7f << 11)
+#define PHYCLKRST_MPLL_MULTIPLIER_100MHZ_REF	(0x19 << 11)
+#define PHYCLKRST_MPLL_MULTIPLIER_50M_REF	(0x32 << 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_UTMI_MASK		(0x7 << 5)
+#define PHYCLKRST_FSEL_PIPE_MASK		(0x7 << 8)
+#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			BIT(4)
+
+#define PHYCLKRST_REFCLKSEL_MASK		(0x03 << 2)
+#define PHYCLKRST_REFCLKSEL_PAD_REFCLK		(0x2 << 2)
+#define PHYCLKRST_REFCLKSEL_EXT_REFCLK		(0x3 << 2)
+
+#define PHYCLKRST_PORTRESET			BIT(1)
+#define PHYCLKRST_COMMONONN			BIT(0)
+
+#define EXYNOS5_DRD_PHYREG0			0x14
+#define EXYNOS5_DRD_PHYREG1			0x18
+
+#define EXYNOS5_DRD_PHYPARAM0			0x1c
+
+#define PHYPARAM0_REF_USE_PAD			BIT(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			BIT(3)
+#define PHYTEST_POWERDOWN_HSP			BIT(2)
+
+#define EXYNOS5_DRD_PHYADP			0x2c
+
+#define EXYNOS5_DRD_PHYUTMICLKSEL		0x30
+
+#define PHYUTMICLKSEL_UTMI_CLKSEL		BIT(2)
+
+#define EXYNOS5_DRD_PHYRESUME			0x34
+#define EXYNOS5_DRD_LINKPORT			0x44
+
+#define KHZ	1000
+#define MHZ	(KHZ * KHZ)
+
+enum exynos5_usbdrd_phy_id {
+	EXYNOS5_DRDPHY_UTMI,
+	EXYNOS5_DRDPHY_PIPE3,
+	EXYNOS5_DRDPHYS_NUM,
+};
+
+struct phy_usb_instance;
+struct exynos5_usbdrd_phy;
+
+struct exynos5_usbdrd_phy_config {
+	u32 id;
+	void (*phy_isol)(struct phy_usb_instance *inst, u32 on);
+	void (*phy_init)(struct exynos5_usbdrd_phy *phy_drd);
+	unsigned int (*set_refclk)(struct phy_usb_instance *inst);
+};
+
+struct exynos5_usbdrd_phy_drvdata {
+	const struct exynos5_usbdrd_phy_config *phy_cfg;
+	u32 pmu_offset_usbdrd0_phy;
+	u32 pmu_offset_usbdrd1_phy;
+};
+
+/**
+ * struct exynos5_usbdrd_phy - driver data for USB 3.0 PHY
+ * @dev: pointer to device instance of this platform device
+ * @reg_phy: usb phy controller register memory base
+ * @clk: phy clock for register access
+ * @drv_data: pointer to SoC level driver data structure
+ * @phys[]: array for 'EXYNOS5_DRDPHYS_NUM' number of PHY
+ *	    instances each with its 'phy' and 'phy_cfg'.
+ * @extrefclk: frequency select settings when using 'separate
+ *	       reference clocks' for SS and HS operations
+ * @ref_clk: reference clock to PHY block from which PHY's
+ *	     operational clocks are derived
+ * @ref_rate: rate of above reference clock
+ */
+struct exynos5_usbdrd_phy {
+	struct device *dev;
+	void __iomem *reg_phy;
+	struct clk *clk;
+	const struct exynos5_usbdrd_phy_drvdata *drv_data;
+	struct phy_usb_instance {
+		struct phy *phy;
+		u32 index;
+		struct regmap *reg_pmu;
+		u32 pmu_offset;
+		const struct exynos5_usbdrd_phy_config *phy_cfg;
+	} phys[EXYNOS5_DRDPHYS_NUM];
+	u32 extrefclk;
+	struct clk *ref_clk;
+	struct regulator *vbus;
+};
+
+static inline
+struct exynos5_usbdrd_phy *to_usbdrd_phy(struct phy_usb_instance *inst)
+{
+	return container_of((inst), struct exynos5_usbdrd_phy,
+			    phys[(inst)->index]);
+}
+
+/*
+ * exynos5_rate_to_clk() converts the supplied clock rate to the value that
+ * can be written to the phy register.
+ */
+static unsigned int exynos5_rate_to_clk(unsigned long rate, u32 *reg)
+{
+	/* EXYNOS5_FSEL_MASK */
+
+	switch (rate) {
+	case 9600 * KHZ:
+		*reg = EXYNOS5_FSEL_9MHZ6;
+		break;
+	case 10 * MHZ:
+		*reg = EXYNOS5_FSEL_10MHZ;
+		break;
+	case 12 * MHZ:
+		*reg = EXYNOS5_FSEL_12MHZ;
+		break;
+	case 19200 * KHZ:
+		*reg = EXYNOS5_FSEL_19MHZ2;
+		break;
+	case 20 * MHZ:
+		*reg = EXYNOS5_FSEL_20MHZ;
+		break;
+	case 24 * MHZ:
+		*reg = EXYNOS5_FSEL_24MHZ;
+		break;
+	case 50 * MHZ:
+		*reg = EXYNOS5_FSEL_50MHZ;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void exynos5_usbdrd_phy_isol(struct phy_usb_instance *inst,
+						unsigned int on)
+{
+	unsigned int val;
+
+	if (!inst->reg_pmu)
+		return;
+
+	val = on ? 0 : EXYNOS5_PHY_ENABLE;
+
+	regmap_update_bits(inst->reg_pmu, inst->pmu_offset,
+			   EXYNOS5_PHY_ENABLE, val);
+}
+
+/*
+ * Sets the pipe3 phy's clk as EXTREFCLK (XXTI) which is internal clock
+ * from clock core. Further sets multiplier values and spread spectrum
+ * clock settings for SuperSpeed operations.
+ */
+static unsigned int
+exynos5_usbdrd_pipe3_set_refclk(struct phy_usb_instance *inst)
+{
+	static u32 reg;
+	struct exynos5_usbdrd_phy *phy_drd = to_usbdrd_phy(inst);
+
+	/* restore any previous reference clock settings */
+	reg = readl(phy_drd->reg_phy + EXYNOS5_DRD_PHYCLKRST);
+
+	/* Use EXTREFCLK as ref clock */
+	reg &= ~PHYCLKRST_REFCLKSEL_MASK;
+	reg |=	PHYCLKRST_REFCLKSEL_EXT_REFCLK;
+
+	/* FSEL settings corresponding to reference clock */
+	reg &= ~PHYCLKRST_FSEL_PIPE_MASK |
+		PHYCLKRST_MPLL_MULTIPLIER_MASK |
+		PHYCLKRST_SSC_REFCLKSEL_MASK;
+	switch (phy_drd->extrefclk) {
+	case EXYNOS5_FSEL_50MHZ:
+		reg |= (PHYCLKRST_MPLL_MULTIPLIER_50M_REF |
+			PHYCLKRST_SSC_REFCLKSEL(0x00));
+		break;
+	case EXYNOS5_FSEL_24MHZ:
+		reg |= (PHYCLKRST_MPLL_MULTIPLIER_24MHZ_REF |
+			PHYCLKRST_SSC_REFCLKSEL(0x88));
+		break;
+	case EXYNOS5_FSEL_20MHZ:
+		reg |= (PHYCLKRST_MPLL_MULTIPLIER_20MHZ_REF |
+			PHYCLKRST_SSC_REFCLKSEL(0x00));
+		break;
+	case EXYNOS5_FSEL_19MHZ2:
+		reg |= (PHYCLKRST_MPLL_MULTIPLIER_19200KHZ_REF |
+			PHYCLKRST_SSC_REFCLKSEL(0x88));
+		break;
+	default:
+		dev_dbg(phy_drd->dev, "unsupported ref clk\n");
+		break;
+	}
+
+	return reg;
+}
+
+/*
+ * Sets the utmi phy's clk as EXTREFCLK (XXTI) which is internal clock
+ * from clock core. Further sets the FSEL values for HighSpeed operations.
+ */
+static unsigned int
+exynos5_usbdrd_utmi_set_refclk(struct phy_usb_instance *inst)
+{
+	static u32 reg;
+	struct exynos5_usbdrd_phy *phy_drd = to_usbdrd_phy(inst);
+
+	/* restore any previous reference clock settings */
+	reg = readl(phy_drd->reg_phy + EXYNOS5_DRD_PHYCLKRST);
+
+	reg &= ~PHYCLKRST_REFCLKSEL_MASK;
+	reg |=	PHYCLKRST_REFCLKSEL_EXT_REFCLK;
+
+	reg &= ~PHYCLKRST_FSEL_UTMI_MASK |
+		PHYCLKRST_MPLL_MULTIPLIER_MASK |
+		PHYCLKRST_SSC_REFCLKSEL_MASK;
+	reg |= PHYCLKRST_FSEL(phy_drd->extrefclk);
+
+	return reg;
+}
+
+static void exynos5_usbdrd_pipe3_init(struct exynos5_usbdrd_phy *phy_drd)
+{
+	u32 reg;
+
+	reg = readl(phy_drd->reg_phy + EXYNOS5_DRD_PHYPARAM1);
+	/* Set Tx De-Emphasis level */
+	reg &= ~PHYPARAM1_PCS_TXDEEMPH_MASK;
+	reg |=	PHYPARAM1_PCS_TXDEEMPH;
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYPARAM1);
+
+	reg = readl(phy_drd->reg_phy + EXYNOS5_DRD_PHYTEST);
+	reg &= ~PHYTEST_POWERDOWN_SSP;
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYTEST);
+}
+
+static void exynos5_usbdrd_utmi_init(struct exynos5_usbdrd_phy *phy_drd)
+{
+	u32 reg;
+
+	reg = readl(phy_drd->reg_phy + EXYNOS5_DRD_PHYPARAM0);
+	/* Set Loss-of-Signal Detector sensitivity */
+	reg &= ~PHYPARAM0_REF_LOSLEVEL_MASK;
+	reg |=	PHYPARAM0_REF_LOSLEVEL;
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYPARAM0);
+
+	reg = readl(phy_drd->reg_phy + EXYNOS5_DRD_PHYPARAM1);
+	/* Set Tx De-Emphasis level */
+	reg &= ~PHYPARAM1_PCS_TXDEEMPH_MASK;
+	reg |=	PHYPARAM1_PCS_TXDEEMPH;
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYPARAM1);
+
+	/* UTMI Power Control */
+	writel(PHYUTMI_OTGDISABLE, phy_drd->reg_phy + EXYNOS5_DRD_PHYUTMI);
+
+	reg = readl(phy_drd->reg_phy + EXYNOS5_DRD_PHYTEST);
+	reg &= ~PHYTEST_POWERDOWN_HSP;
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYTEST);
+}
+
+static int exynos5_usbdrd_phy_init(struct phy *phy)
+{
+	int ret;
+	u32 reg;
+	struct phy_usb_instance *inst = phy_get_drvdata(phy);
+	struct exynos5_usbdrd_phy *phy_drd = to_usbdrd_phy(inst);
+
+	ret = clk_prepare_enable(phy_drd->clk);
+	if (ret)
+		return ret;
+
+	/* Reset USB 3.0 PHY */
+	writel(0x0, phy_drd->reg_phy + EXYNOS5_DRD_PHYREG0);
+	writel(0x0, phy_drd->reg_phy + EXYNOS5_DRD_PHYRESUME);
+
+	/*
+	 * Setting the Frame length Adj value[6:1] to default 0x20
+	 * See xHCI 1.0 spec, 5.2.4
+	 */
+	reg =	LINKSYSTEM_XHCI_VERSION_CONTROL |
+		LINKSYSTEM_FLADJ(0x20);
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_LINKSYSTEM);
+
+	reg = readl(phy_drd->reg_phy + EXYNOS5_DRD_PHYPARAM0);
+	/* Select PHY CLK source */
+	reg &= ~PHYPARAM0_REF_USE_PAD;
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYPARAM0);
+
+	/* This bit must be set for both HS and SS operations */
+	reg = readl(phy_drd->reg_phy + EXYNOS5_DRD_PHYUTMICLKSEL);
+	reg |= PHYUTMICLKSEL_UTMI_CLKSEL;
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYUTMICLKSEL);
+
+	/* UTMI or PIPE3 specific init */
+	inst->phy_cfg->phy_init(phy_drd);
+
+	/* reference clock settings */
+	reg = inst->phy_cfg->set_refclk(inst);
+
+		/* Digital power supply in normal operating mode */
+	reg |=	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 |
+		/* Reset the port */
+		PHYCLKRST_PORTRESET;
+
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYCLKRST);
+
+	udelay(10);
+
+	reg &= ~PHYCLKRST_PORTRESET;
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYCLKRST);
+
+	clk_disable_unprepare(phy_drd->clk);
+
+	return 0;
+}
+
+static int exynos5_usbdrd_phy_exit(struct phy *phy)
+{
+	int ret;
+	u32 reg;
+	struct phy_usb_instance *inst = phy_get_drvdata(phy);
+	struct exynos5_usbdrd_phy *phy_drd = to_usbdrd_phy(inst);
+
+	ret = clk_prepare_enable(phy_drd->clk);
+	if (ret)
+		return ret;
+
+	reg =	PHYUTMI_OTGDISABLE |
+		PHYUTMI_FORCESUSPEND |
+		PHYUTMI_FORCESLEEP;
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYUTMI);
+
+	/* Resetting the PHYCLKRST enable bits to reduce leakage current */
+	reg = readl(phy_drd->reg_phy + EXYNOS5_DRD_PHYCLKRST);
+	reg &= ~(PHYCLKRST_REF_SSP_EN |
+		 PHYCLKRST_SSC_EN |
+		 PHYCLKRST_COMMONONN);
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYCLKRST);
+
+	/* Control PHYTEST to remove leakage current */
+	reg = readl(phy_drd->reg_phy + EXYNOS5_DRD_PHYTEST);
+	reg |=	PHYTEST_POWERDOWN_SSP |
+		PHYTEST_POWERDOWN_HSP;
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYTEST);
+
+	clk_disable_unprepare(phy_drd->clk);
+
+	return 0;
+}
+
+static int exynos5_usbdrd_phy_power_on(struct phy *phy)
+{
+	int ret;
+	struct phy_usb_instance *inst = phy_get_drvdata(phy);
+	struct exynos5_usbdrd_phy *phy_drd = to_usbdrd_phy(inst);
+
+	dev_dbg(phy_drd->dev, "Request to power_on usbdrd_phy phy\n");
+
+	clk_prepare_enable(phy_drd->ref_clk);
+
+	/* Enable VBUS supply */
+	if (phy_drd->vbus) {
+		ret = regulator_enable(phy_drd->vbus);
+		if (ret) {
+			dev_err(phy_drd->dev, "Failed to enable VBUS supply\n");
+			goto fail_vbus;
+		}
+	}
+
+	/* Power-on PHY*/
+	inst->phy_cfg->phy_isol(inst, 0);
+
+	return 0;
+
+fail_vbus:
+	clk_disable_unprepare(phy_drd->ref_clk);
+
+	return ret;
+}
+
+static int exynos5_usbdrd_phy_power_off(struct phy *phy)
+{
+	struct phy_usb_instance *inst = phy_get_drvdata(phy);
+	struct exynos5_usbdrd_phy *phy_drd = to_usbdrd_phy(inst);
+
+	dev_dbg(phy_drd->dev, "Request to power_off usbdrd_phy phy\n");
+
+	/* Power-off the PHY */
+	inst->phy_cfg->phy_isol(inst, 1);
+
+	/* Disable VBUS supply */
+	if (phy_drd->vbus)
+		regulator_disable(phy_drd->vbus);
+
+	clk_disable_unprepare(phy_drd->ref_clk);
+
+	return 0;
+}
+
+static struct phy *exynos5_usbdrd_phy_xlate(struct device *dev,
+					struct of_phandle_args *args)
+{
+	struct exynos5_usbdrd_phy *phy_drd = dev_get_drvdata(dev);
+
+	if (WARN_ON(args->args[0] > EXYNOS5_DRDPHYS_NUM))
+		return ERR_PTR(-ENODEV);
+
+	return phy_drd->phys[args->args[0]].phy;
+}
+
+static struct phy_ops exynos5_usbdrd_phy_ops = {
+	.init		= exynos5_usbdrd_phy_init,
+	.exit		= exynos5_usbdrd_phy_exit,
+	.power_on	= exynos5_usbdrd_phy_power_on,
+	.power_off	= exynos5_usbdrd_phy_power_off,
+	.owner		= THIS_MODULE,
+};
+
+const struct exynos5_usbdrd_phy_config phy_cfg_exynos5[] = {
+	{
+		.id		= EXYNOS5_DRDPHY_UTMI,
+		.phy_isol	= exynos5_usbdrd_phy_isol,
+		.phy_init	= exynos5_usbdrd_utmi_init,
+		.set_refclk	= exynos5_usbdrd_utmi_set_refclk,
+	},
+	{
+		.id		= EXYNOS5_DRDPHY_PIPE3,
+		.phy_isol	= exynos5_usbdrd_phy_isol,
+		.phy_init	= exynos5_usbdrd_pipe3_init,
+		.set_refclk	= exynos5_usbdrd_pipe3_set_refclk,
+	},
+};
+
+const struct exynos5_usbdrd_phy_drvdata exynos5420_usbdrd_phy = {
+	.phy_cfg		= phy_cfg_exynos5,
+	.pmu_offset_usbdrd0_phy	= EXYNOS5_USBDRD_PHY_CONTROL,
+	.pmu_offset_usbdrd1_phy	= EXYNOS5420_USBDRD1_PHY_CONTROL,
+};
+
+const struct exynos5_usbdrd_phy_drvdata exynos5250_usbdrd_phy = {
+	.phy_cfg		= phy_cfg_exynos5,
+	.pmu_offset_usbdrd0_phy	= EXYNOS5_USBDRD_PHY_CONTROL,
+};
+
+static const struct of_device_id exynos5_usbdrd_phy_of_match[] = {
+	{
+		.compatible = "samsung,exynos5250-usbdrd-phy",
+		.data = &exynos5250_usbdrd_phy
+	}, {
+		.compatible = "samsung,exynos5420-usbdrd-phy",
+		.data = &exynos5420_usbdrd_phy
+	},
+	{ },
+};
+
+static int exynos5_usbdrd_phy_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *node = dev->of_node;
+	struct exynos5_usbdrd_phy *phy_drd;
+	struct phy_provider *phy_provider;
+	struct resource *res;
+	const struct of_device_id *match;
+	const struct exynos5_usbdrd_phy_drvdata *drv_data;
+	struct regmap *reg_pmu;
+	u32 pmu_offset;
+	unsigned long ref_rate;
+	int i, ret;
+	int channel;
+
+	phy_drd = devm_kzalloc(dev, sizeof(*phy_drd), GFP_KERNEL);
+	if (!phy_drd)
+		return -ENOMEM;
+
+	dev_set_drvdata(dev, phy_drd);
+	phy_drd->dev = dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	phy_drd->reg_phy = devm_ioremap_resource(dev, res);
+	if (IS_ERR(phy_drd->reg_phy))
+		return PTR_ERR(phy_drd->reg_phy);
+
+	match = of_match_node(exynos5_usbdrd_phy_of_match, pdev->dev.of_node);
+
+	drv_data = match->data;
+	phy_drd->drv_data = drv_data;
+
+	phy_drd->clk = devm_clk_get(dev, "phy");
+	if (IS_ERR(phy_drd->clk)) {
+		dev_err(dev, "Failed to get clock of phy controller\n");
+		return PTR_ERR(phy_drd->clk);
+	}
+
+	phy_drd->ref_clk = devm_clk_get(dev, "ref");
+	if (IS_ERR(phy_drd->ref_clk)) {
+		dev_err(dev, "Failed to get reference clock of usbdrd phy\n");
+		return PTR_ERR(phy_drd->ref_clk);
+	}
+	ref_rate = clk_get_rate(phy_drd->ref_clk);
+
+	ret = exynos5_rate_to_clk(ref_rate, &phy_drd->extrefclk);
+	if (ret) {
+		dev_err(phy_drd->dev, "Clock rate (%ld) not supported\n",
+			ref_rate);
+		return ret;
+	}
+
+	reg_pmu = syscon_regmap_lookup_by_phandle(dev->of_node,
+						   "samsung,pmu-syscon");
+	if (IS_ERR(reg_pmu)) {
+		dev_err(dev, "Failed to lookup PMU regmap\n");
+		return PTR_ERR(reg_pmu);
+	}
+
+	/*
+	 * Exynos5420 SoC has multiple channels for USB 3.0 PHY, with
+	 * each having separate power control registers.
+	 * 'channel' facilitates to set such registers.
+	 */
+	channel = of_alias_get_id(node, "usbdrdphy");
+	if (channel < 0)
+		dev_dbg(dev, "Not a multi-controller usbdrd phy\n");
+
+	switch (channel) {
+	case 1:
+		pmu_offset = phy_drd->drv_data->pmu_offset_usbdrd1_phy;
+		break;
+	case 0:
+	default:
+		pmu_offset = phy_drd->drv_data->pmu_offset_usbdrd0_phy;
+		break;
+	}
+
+	/* Get Vbus regulator */
+	phy_drd->vbus = devm_regulator_get(dev, "vbus");
+	if (IS_ERR(phy_drd->vbus)) {
+		ret = PTR_ERR(phy_drd->vbus);
+		if (ret == -EPROBE_DEFER)
+			return ret;
+
+		dev_warn(dev, "Failed to get VBUS supply regulator\n");
+		phy_drd->vbus = NULL;
+	}
+
+	dev_vdbg(dev, "Creating usbdrd_phy phy\n");
+
+	for (i = 0; i < EXYNOS5_DRDPHYS_NUM; i++) {
+		struct phy *phy = devm_phy_create(dev, &exynos5_usbdrd_phy_ops,
+						  NULL);
+		if (IS_ERR(phy)) {
+			dev_err(dev, "Failed to create usbdrd_phy phy\n");
+			return PTR_ERR(phy);
+		}
+
+		phy_drd->phys[i].phy = phy;
+		phy_drd->phys[i].index = i;
+		phy_drd->phys[i].reg_pmu = reg_pmu;
+		phy_drd->phys[i].pmu_offset = pmu_offset;
+		phy_drd->phys[i].phy_cfg = &drv_data->phy_cfg[i];
+		phy_set_drvdata(phy, &phy_drd->phys[i]);
+	}
+
+	phy_provider = devm_of_phy_provider_register(dev,
+						     exynos5_usbdrd_phy_xlate);
+	if (IS_ERR(phy_provider)) {
+		dev_err(phy_drd->dev, "Failed to register phy provider\n");
+		return PTR_ERR(phy_provider);
+	}
+
+	return 0;
+}
+
+static struct platform_driver exynos5_usb3drd_phy = {
+	.probe	= exynos5_usbdrd_phy_probe,
+	.driver = {
+		.of_match_table	= exynos5_usbdrd_phy_of_match,
+		.name		= "exynos5_usb3drd_phy",
+		.owner		= THIS_MODULE,
+	}
+};
+
+module_platform_driver(exynos5_usb3drd_phy);
+MODULE_DESCRIPTION("Samsung EXYNOS5 SoCs USB 3.0 DRD controller PHY driver");
+MODULE_AUTHOR("Vivek Gautam <gautam.vivek@samsung.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:exynos5_usb3drd_phy");
diff --git a/drivers/phy/phy-exynos5250-sata.c b/drivers/phy/phy-exynos5250-sata.c
index c9361b731fa..05689450f93 100644
--- a/drivers/phy/phy-exynos5250-sata.c
+++ b/drivers/phy/phy-exynos5250-sata.c
@@ -246,6 +246,6 @@ static struct platform_driver exynos_sata_phy_driver = {
 module_platform_driver(exynos_sata_phy_driver);
 
 MODULE_DESCRIPTION("Samsung SerDes PHY driver");
-MODULE_LICENSE("GPL V2");
+MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Girish K S <ks.giri@samsung.com>");
 MODULE_AUTHOR("Yuvaraj C D <yuvaraj.cd@samsung.com>");
diff --git a/drivers/phy/phy-omap-usb2.c b/drivers/phy/phy-omap-usb2.c
index a2205a841e5..7007c11fe07 100644
--- a/drivers/phy/phy-omap-usb2.c
+++ b/drivers/phy/phy-omap-usb2.c
@@ -275,18 +275,34 @@ static int omap_usb2_probe(struct platform_device *pdev)
 	if (IS_ERR(phy_provider))
 		return PTR_ERR(phy_provider);
 
-	phy->wkupclk = devm_clk_get(phy->dev, "usb_phy_cm_clk32k");
+	phy->wkupclk = devm_clk_get(phy->dev, "wkupclk");
 	if (IS_ERR(phy->wkupclk)) {
-		dev_err(&pdev->dev, "unable to get usb_phy_cm_clk32k\n");
-		return PTR_ERR(phy->wkupclk);
+		dev_warn(&pdev->dev, "unable to get wkupclk, trying old name\n");
+		phy->wkupclk = devm_clk_get(phy->dev, "usb_phy_cm_clk32k");
+		if (IS_ERR(phy->wkupclk)) {
+			dev_err(&pdev->dev, "unable to get usb_phy_cm_clk32k\n");
+			return PTR_ERR(phy->wkupclk);
+		} else {
+			dev_warn(&pdev->dev,
+				 "found usb_phy_cm_clk32k, please fix DTS\n");
+		}
 	}
 	clk_prepare(phy->wkupclk);
 
-	phy->optclk = devm_clk_get(phy->dev, "usb_otg_ss_refclk960m");
-	if (IS_ERR(phy->optclk))
-		dev_vdbg(&pdev->dev, "unable to get refclk960m\n");
-	else
+	phy->optclk = devm_clk_get(phy->dev, "refclk");
+	if (IS_ERR(phy->optclk)) {
+		dev_dbg(&pdev->dev, "unable to get refclk, trying old name\n");
+		phy->optclk = devm_clk_get(phy->dev, "usb_otg_ss_refclk960m");
+		if (IS_ERR(phy->optclk)) {
+			dev_dbg(&pdev->dev,
+				"unable to get usb_otg_ss_refclk960m\n");
+		} else {
+			dev_warn(&pdev->dev,
+				 "found usb_otg_ss_refclk960m, please fix DTS\n");
+		}
+	} else {
 		clk_prepare(phy->optclk);
+	}
 
 	usb_add_phy_dev(&phy->phy);
 
diff --git a/drivers/phy/phy-sun4i-usb.c b/drivers/phy/phy-sun4i-usb.c
index e6e6c4ba714..115d8d5190d 100644
--- a/drivers/phy/phy-sun4i-usb.c
+++ b/drivers/phy/phy-sun4i-usb.c
@@ -61,7 +61,6 @@
 #define MAX_PHYS			3
 
 struct sun4i_usb_phy_data {
-	struct clk *clk;
 	void __iomem *base;
 	struct mutex mutex;
 	int num_phys;
@@ -71,6 +70,7 @@ struct sun4i_usb_phy_data {
 		void __iomem *pmu;
 		struct regulator *vbus;
 		struct reset_control *reset;
+		struct clk *clk;
 		int index;
 	} phys[MAX_PHYS];
 };
@@ -146,13 +146,13 @@ static int sun4i_usb_phy_init(struct phy *_phy)
 	struct sun4i_usb_phy_data *data = to_sun4i_usb_phy_data(phy);
 	int ret;
 
-	ret = clk_prepare_enable(data->clk);
+	ret = clk_prepare_enable(phy->clk);
 	if (ret)
 		return ret;
 
 	ret = reset_control_deassert(phy->reset);
 	if (ret) {
-		clk_disable_unprepare(data->clk);
+		clk_disable_unprepare(phy->clk);
 		return ret;
 	}
 
@@ -170,11 +170,10 @@ static int sun4i_usb_phy_init(struct phy *_phy)
 static int sun4i_usb_phy_exit(struct phy *_phy)
 {
 	struct sun4i_usb_phy *phy = phy_get_drvdata(_phy);
-	struct sun4i_usb_phy_data *data = to_sun4i_usb_phy_data(phy);
 
 	sun4i_usb_phy_passby(phy, 0);
 	reset_control_assert(phy->reset);
-	clk_disable_unprepare(data->clk);
+	clk_disable_unprepare(phy->clk);
 
 	return 0;
 }
@@ -224,13 +223,9 @@ static int sun4i_usb_phy_probe(struct platform_device *pdev)
 	struct sun4i_usb_phy_data *data;
 	struct device *dev = &pdev->dev;
 	struct device_node *np = dev->of_node;
-	void __iomem *pmu = NULL;
 	struct phy_provider *phy_provider;
-	struct reset_control *reset;
-	struct regulator *vbus;
+	bool dedicated_clocks;
 	struct resource *res;
-	struct phy *phy;
-	char name[16];
 	int i;
 
 	data = devm_kzalloc(dev, sizeof(*data), GFP_KERNEL);
@@ -249,55 +244,64 @@ static int sun4i_usb_phy_probe(struct platform_device *pdev)
 	else
 		data->disc_thresh = 2;
 
+	if (of_device_is_compatible(np, "allwinner,sun6i-a31-usb-phy"))
+		dedicated_clocks = true;
+	else
+		dedicated_clocks = false;
+
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "phy_ctrl");
 	data->base = devm_ioremap_resource(dev, res);
 	if (IS_ERR(data->base))
 		return PTR_ERR(data->base);
 
-	data->clk = devm_clk_get(dev, "usb_phy");
-	if (IS_ERR(data->clk)) {
-		dev_err(dev, "could not get usb_phy clock\n");
-		return PTR_ERR(data->clk);
-	}
-
 	/* Skip 0, 0 is the phy for otg which is not yet supported. */
 	for (i = 1; i < data->num_phys; i++) {
+		struct sun4i_usb_phy *phy = data->phys + i;
+		char name[16];
+
 		snprintf(name, sizeof(name), "usb%d_vbus", i);
-		vbus = devm_regulator_get_optional(dev, name);
-		if (IS_ERR(vbus)) {
-			if (PTR_ERR(vbus) == -EPROBE_DEFER)
+		phy->vbus = devm_regulator_get_optional(dev, name);
+		if (IS_ERR(phy->vbus)) {
+			if (PTR_ERR(phy->vbus) == -EPROBE_DEFER)
 				return -EPROBE_DEFER;
-			vbus = NULL;
+			phy->vbus = NULL;
+		}
+
+		if (dedicated_clocks)
+			snprintf(name, sizeof(name), "usb%d_phy", i);
+		else
+			strlcpy(name, "usb_phy", sizeof(name));
+
+		phy->clk = devm_clk_get(dev, name);
+		if (IS_ERR(phy->clk)) {
+			dev_err(dev, "failed to get clock %s\n", name);
+			return PTR_ERR(phy->clk);
 		}
 
 		snprintf(name, sizeof(name), "usb%d_reset", i);
-		reset = devm_reset_control_get(dev, name);
-		if (IS_ERR(reset)) {
+		phy->reset = devm_reset_control_get(dev, name);
+		if (IS_ERR(phy->reset)) {
 			dev_err(dev, "failed to get reset %s\n", name);
-			return PTR_ERR(reset);
+			return PTR_ERR(phy->reset);
 		}
 
 		if (i) { /* No pmu for usbc0 */
 			snprintf(name, sizeof(name), "pmu%d", i);
 			res = platform_get_resource_byname(pdev,
 							IORESOURCE_MEM, name);
-			pmu = devm_ioremap_resource(dev, res);
-			if (IS_ERR(pmu))
-				return PTR_ERR(pmu);
+			phy->pmu = devm_ioremap_resource(dev, res);
+			if (IS_ERR(phy->pmu))
+				return PTR_ERR(phy->pmu);
 		}
 
-		phy = devm_phy_create(dev, &sun4i_usb_phy_ops, NULL);
-		if (IS_ERR(phy)) {
+		phy->phy = devm_phy_create(dev, &sun4i_usb_phy_ops, NULL);
+		if (IS_ERR(phy->phy)) {
 			dev_err(dev, "failed to create PHY %d\n", i);
-			return PTR_ERR(phy);
+			return PTR_ERR(phy->phy);
 		}
 
-		data->phys[i].phy = phy;
-		data->phys[i].pmu = pmu;
-		data->phys[i].vbus = vbus;
-		data->phys[i].reset = reset;
-		data->phys[i].index = i;
-		phy_set_drvdata(phy, &data->phys[i]);
+		phy->index = i;
+		phy_set_drvdata(phy->phy, &data->phys[i]);
 	}
 
 	dev_set_drvdata(dev, data);
@@ -311,6 +315,7 @@ static int sun4i_usb_phy_probe(struct platform_device *pdev)
 static const struct of_device_id sun4i_usb_phy_of_match[] = {
 	{ .compatible = "allwinner,sun4i-a10-usb-phy" },
 	{ .compatible = "allwinner,sun5i-a13-usb-phy" },
+	{ .compatible = "allwinner,sun6i-a31-usb-phy" },
 	{ .compatible = "allwinner,sun7i-a20-usb-phy" },
 	{ },
 };
diff --git a/drivers/pinctrl/Kconfig b/drivers/pinctrl/Kconfig
index e00c02d0a09..0042ccb46b9 100644
--- a/drivers/pinctrl/Kconfig
+++ b/drivers/pinctrl/Kconfig
@@ -84,6 +84,9 @@ config PINCTRL_AT91
 	depends on ARCH_AT91
 	select PINMUX
 	select PINCONF
+	select GPIOLIB
+	select OF_GPIO
+	select GPIOLIB_IRQCHIP
 	help
 	  Say Y here to enable the at91 pinctrl driver
 
@@ -185,6 +188,13 @@ config PINCTRL_IMX6SL
 	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
@@ -221,10 +231,27 @@ config PINCTRL_MSM
 	select PINMUX
 	select PINCONF
 	select GENERIC_PINCONF
+	select GPIOLIB_IRQCHIP
+
+config PINCTRL_APQ8064
+	tristate "Qualcomm APQ8064 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 APQ8064 platform.
+
+config PINCTRL_IPQ8064
+	tristate "Qualcomm IPQ8064 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 IPQ8064 platform.
 
 config PINCTRL_MSM8X74
 	tristate "Qualcomm 8x74 pin controller driver"
-	depends on GPIOLIB && OF
+	depends on GPIOLIB && OF && (ARCH_QCOM || COMPILE_TEST)
 	select PINCTRL_MSM
 	help
 	  This is the pinctrl, pinmux, pinconf and gpiolib driver for the
@@ -256,6 +283,7 @@ config PINCTRL_ROCKCHIP
 	select PINMUX
 	select GENERIC_PINCONF
 	select GENERIC_IRQ_CHIP
+	select MFD_SYSCON
 
 config PINCTRL_SINGLE
 	tristate "One-register-per-pin type device tree based pinctrl driver"
@@ -270,17 +298,14 @@ config PINCTRL_SIRF
 	bool "CSR SiRFprimaII/SiRFmarco pin controller driver"
 	depends on ARCH_SIRF
 	select PINMUX
-
-config PINCTRL_SUNXI
-	bool
-	select PINMUX
-	select GENERIC_PINCONF
+	select GPIOLIB_IRQCHIP
 
 config PINCTRL_ST
 	bool
 	depends on OF
 	select PINMUX
 	select PINCONF
+	select GPIOLIB_IRQCHIP
 
 config PINCTRL_TEGRA
 	bool
@@ -368,9 +393,11 @@ config PINCTRL_S3C64XX
 	depends on ARCH_S3C64XX
 	select PINCTRL_SAMSUNG
 
+source "drivers/pinctrl/berlin/Kconfig"
 source "drivers/pinctrl/mvebu/Kconfig"
 source "drivers/pinctrl/sh-pfc/Kconfig"
 source "drivers/pinctrl/spear/Kconfig"
+source "drivers/pinctrl/sunxi/Kconfig"
 source "drivers/pinctrl/vt8500/Kconfig"
 
 config PINCTRL_XWAY
diff --git a/drivers/pinctrl/Makefile b/drivers/pinctrl/Makefile
index 6d3fd62b9ae..c4b5d405b8f 100644
--- a/drivers/pinctrl/Makefile
+++ b/drivers/pinctrl/Makefile
@@ -32,12 +32,15 @@ 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_MSM)	+= pinctrl-msm.o
+obj-$(CONFIG_PINCTRL_APQ8064)	+= pinctrl-apq8064.o
+obj-$(CONFIG_PINCTRL_IPQ8064)	+= pinctrl-ipq8064.o
 obj-$(CONFIG_PINCTRL_MSM8X74)	+= pinctrl-msm8x74.o
 obj-$(CONFIG_PINCTRL_NOMADIK)	+= pinctrl-nomadik.o
 obj-$(CONFIG_PINCTRL_STN8815)	+= pinctrl-nomadik-stn8815.o
@@ -47,7 +50,6 @@ obj-$(CONFIG_PINCTRL_PALMAS)	+= pinctrl-palmas.o
 obj-$(CONFIG_PINCTRL_ROCKCHIP)	+= pinctrl-rockchip.o
 obj-$(CONFIG_PINCTRL_SINGLE)	+= pinctrl-single.o
 obj-$(CONFIG_PINCTRL_SIRF)	+= sirf/
-obj-$(CONFIG_PINCTRL_SUNXI)	+= pinctrl-sunxi.o
 obj-$(CONFIG_PINCTRL_TEGRA)	+= pinctrl-tegra.o
 obj-$(CONFIG_PINCTRL_TEGRA20)	+= pinctrl-tegra20.o
 obj-$(CONFIG_PINCTRL_TEGRA30)	+= pinctrl-tegra30.o
@@ -68,8 +70,10 @@ 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-$(CONFIG_PLAT_ORION)        += mvebu/
 obj-$(CONFIG_ARCH_SHMOBILE)	+= sh-pfc/
 obj-$(CONFIG_SUPERH)		+= sh-pfc/
 obj-$(CONFIG_PLAT_SPEAR)	+= spear/
 obj-$(CONFIG_ARCH_VT8500)	+= vt8500/
+obj-$(CONFIG_ARCH_SUNXI)	+= sunxi/
diff --git a/drivers/pinctrl/berlin/Kconfig b/drivers/pinctrl/berlin/Kconfig
new file mode 100644
index 00000000000..b18322bc7bf
--- /dev/null
+++ b/drivers/pinctrl/berlin/Kconfig
@@ -0,0 +1,20 @@
+if ARCH_BERLIN
+
+config PINCTRL_BERLIN
+	bool
+	select PINMUX
+	select REGMAP_MMIO
+
+config PINCTRL_BERLIN_BG2
+	bool
+	select PINCTRL_BERLIN
+
+config PINCTRL_BERLIN_BG2CD
+	bool
+	select PINCTRL_BERLIN
+
+config PINCTRL_BERLIN_BG2Q
+	bool
+	select PINCTRL_BERLIN
+
+endif
diff --git a/drivers/pinctrl/berlin/Makefile b/drivers/pinctrl/berlin/Makefile
new file mode 100644
index 00000000000..deb0c6baf31
--- /dev/null
+++ b/drivers/pinctrl/berlin/Makefile
@@ -0,0 +1,4 @@
+obj-$(CONFIG_PINCTRL_BERLIN)		+= berlin.o
+obj-$(CONFIG_PINCTRL_BERLIN_BG2)	+= berlin-bg2.o
+obj-$(CONFIG_PINCTRL_BERLIN_BG2CD)	+= berlin-bg2cd.o
+obj-$(CONFIG_PINCTRL_BERLIN_BG2Q)	+= berlin-bg2q.o
diff --git a/drivers/pinctrl/berlin/berlin-bg2.c b/drivers/pinctrl/berlin/berlin-bg2.c
new file mode 100644
index 00000000000..dcd4f6a4fc5
--- /dev/null
+++ b/drivers/pinctrl/berlin/berlin-bg2.c
@@ -0,0 +1,274 @@
+/*
+ * Marvell Berlin BG2 pinctrl driver.
+ *
+ * Copyright (C) 2014 Marvell Technology Group Ltd.
+ *
+ * Antoine Ténart <antoine.tenart@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+
+#include "berlin.h"
+
+static const struct berlin_desc_group berlin2_soc_pinctrl_groups[] = {
+	/* G */
+	BERLIN_PINCTRL_GROUP("G0", 0x00, 0x1, 0x00,
+		BERLIN_PINCTRL_FUNCTION(0x0, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
+	BERLIN_PINCTRL_GROUP("G1", 0x00, 0x2, 0x01,
+		BERLIN_PINCTRL_FUNCTION(0x0, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "usb1")),
+	BERLIN_PINCTRL_GROUP("G2", 0x00, 0x2, 0x02,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "pwm"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "i2s0")),
+	BERLIN_PINCTRL_GROUP("G3", 0x00, 0x2, 0x04,
+		BERLIN_PINCTRL_FUNCTION(0x0, "soc"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "i2s1")),
+	BERLIN_PINCTRL_GROUP("G4", 0x00, 0x2, 0x06,
+		BERLIN_PINCTRL_FUNCTION(0x0, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "pwm")),
+	BERLIN_PINCTRL_GROUP("G5", 0x00, 0x3, 0x08,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sts1"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "et"),
+		/*
+		 * Mode 0x3 mux i2s2 mclk *and* i2s3 mclk:
+		 * add two functions so it can be used with other groups
+		 * within the same subnode in the device tree
+		 */
+		BERLIN_PINCTRL_FUNCTION(0x3, "i2s2"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "i2s3")),
+	BERLIN_PINCTRL_GROUP("G6", 0x00, 0x2, 0x0b,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sts0"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "et")),
+	BERLIN_PINCTRL_GROUP("G7", 0x00, 0x3, 0x0d,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sts0"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "et"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "vdac")),
+	BERLIN_PINCTRL_GROUP("G8", 0x00, 0x3, 0x10,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sd0"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "et"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "usb0_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x4, "sata_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G9", 0x00, 0x3, 0x13,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sd0"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "et"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "usb0_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x4, "sata_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G10", 0x00, 0x2, 0x16,
+		BERLIN_PINCTRL_FUNCTION(0x0, "soc"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "twsi0"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "ptp")),
+	BERLIN_PINCTRL_GROUP("G11", 0x00, 0x2, 0x18,
+		BERLIN_PINCTRL_FUNCTION(0x0, "soc"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "twsi1"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "eddc")),
+	BERLIN_PINCTRL_GROUP("G12", 0x00, 0x3, 0x1a,
+		BERLIN_PINCTRL_FUNCTION(0x0, "sts2"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sata"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "sd1"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "usb0_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x4, "sts1"),
+		BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G13", 0x04, 0x3, 0x00,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sata"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "sd1"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "usb0_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x4, "sts1"),
+		BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G14", 0x04, 0x1, 0x03,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G15", 0x04, 0x2, 0x04,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "et"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "osco")),
+	BERLIN_PINCTRL_GROUP("G16", 0x04, 0x3, 0x06,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "dvio"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "fp")),
+	BERLIN_PINCTRL_GROUP("G17", 0x04, 0x3, 0x09,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "dvio"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "fp")),
+	BERLIN_PINCTRL_GROUP("G18", 0x04, 0x1, 0x0c,
+		BERLIN_PINCTRL_FUNCTION(0x0, "pll"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "i2s0")),
+	BERLIN_PINCTRL_GROUP("G19", 0x04, 0x1, 0x0d,
+		BERLIN_PINCTRL_FUNCTION(0x0, "i2s0"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "pwm")),
+	BERLIN_PINCTRL_GROUP("G20", 0x04, 0x1, 0x0e,
+		BERLIN_PINCTRL_FUNCTION(0x0, "spdif"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "arc")),
+	BERLIN_PINCTRL_GROUP("G21", 0x04, 0x3, 0x0f,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "dvio"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "adac_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x4, "pdm_a"),	/* gpio17..19,pdm */
+		BERLIN_PINCTRL_FUNCTION(0x7, "pdm_b")),	/* gpio12..14,pdm */
+	BERLIN_PINCTRL_GROUP("G22", 0x04, 0x3, 0x12,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "dv0"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "twsi0"),
+		BERLIN_PINCTRL_FUNCTION(0x4, "pwm")),
+	BERLIN_PINCTRL_GROUP("G23", 0x04, 0x3, 0x15,
+		BERLIN_PINCTRL_FUNCTION(0x0, "vclki"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "dv0"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "i2s0"),
+		BERLIN_PINCTRL_FUNCTION(0x4, "pwm"),
+		BERLIN_PINCTRL_FUNCTION(0x7, "pdm")),
+	BERLIN_PINCTRL_GROUP("G24", 0x04, 0x2, 0x18,
+		BERLIN_PINCTRL_FUNCTION(0x0, "i2s2"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "i2s1")),
+	BERLIN_PINCTRL_GROUP("G25", 0x04, 0x2, 0x1a,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "nand"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "i2s2")),
+	BERLIN_PINCTRL_GROUP("G26", 0x04, 0x1, 0x1c,
+		BERLIN_PINCTRL_FUNCTION(0x0, "nand"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "emmc")),
+	BERLIN_PINCTRL_GROUP("G27", 0x04, 0x1, 0x1d,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "nand")),
+	BERLIN_PINCTRL_GROUP("G28", 0x04, 0x2, 0x1e,
+		BERLIN_PINCTRL_FUNCTION(0x0, "dvo"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "sp")),
+};
+
+static const struct berlin_desc_group berlin2_sysmgr_pinctrl_groups[] = {
+	/* GSM */
+	BERLIN_PINCTRL_GROUP("GSM0", 0x40, 0x2, 0x00,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "spi2"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "eth1")),
+	BERLIN_PINCTRL_GROUP("GSM1", 0x40, 0x2, 0x02,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "spi2"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "eth1")),
+	BERLIN_PINCTRL_GROUP("GSM2", 0x40, 0x2, 0x04,
+		BERLIN_PINCTRL_FUNCTION(0x0, "twsi2"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "spi2")),
+	BERLIN_PINCTRL_GROUP("GSM3", 0x40, 0x2, 0x06,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "uart0"),	/* CTS/RTS */
+		BERLIN_PINCTRL_FUNCTION(0x2, "uart2"),	/* RX/TX */
+		BERLIN_PINCTRL_FUNCTION(0x3, "twsi2")),
+	BERLIN_PINCTRL_GROUP("GSM4", 0x40, 0x2, 0x08,
+		BERLIN_PINCTRL_FUNCTION(0x0, "uart0"),	/* RX/TX */
+		BERLIN_PINCTRL_FUNCTION(0x1, "irda0")),
+	BERLIN_PINCTRL_GROUP("GSM5", 0x40, 0x2, 0x0a,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "uart1"),	/* RX/TX */
+		BERLIN_PINCTRL_FUNCTION(0x2, "irda1"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "twsi3")),
+	BERLIN_PINCTRL_GROUP("GSM6", 0x40, 0x2, 0x0c,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "spi2"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "clki")),
+	BERLIN_PINCTRL_GROUP("GSM7", 0x40, 0x1, 0x0e,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "hdmi")),
+	BERLIN_PINCTRL_GROUP("GSM8", 0x40, 0x1, 0x0f,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "hdmi")),
+	BERLIN_PINCTRL_GROUP("GSM9", 0x40, 0x1, 0x10,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "led")),
+	BERLIN_PINCTRL_GROUP("GSM10", 0x40, 0x1, 0x11,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "led")),
+	BERLIN_PINCTRL_GROUP("GSM11", 0x40, 0x1, 0x12,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "led")),
+};
+
+static const struct berlin_pinctrl_desc berlin2_soc_pinctrl_data = {
+	.groups = berlin2_soc_pinctrl_groups,
+	.ngroups = ARRAY_SIZE(berlin2_soc_pinctrl_groups),
+};
+
+static const struct berlin_pinctrl_desc berlin2_sysmgr_pinctrl_data = {
+	.groups = berlin2_sysmgr_pinctrl_groups,
+	.ngroups = ARRAY_SIZE(berlin2_sysmgr_pinctrl_groups),
+};
+
+static const struct of_device_id berlin2_pinctrl_match[] = {
+	{
+		.compatible = "marvell,berlin2-chip-ctrl",
+		.data = &berlin2_soc_pinctrl_data
+	},
+	{
+		.compatible = "marvell,berlin2-system-ctrl",
+		.data = &berlin2_sysmgr_pinctrl_data
+	},
+	{}
+};
+MODULE_DEVICE_TABLE(of, berlin2_pinctrl_match);
+
+static int berlin2_pinctrl_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *match =
+		of_match_device(berlin2_pinctrl_match, &pdev->dev);
+	struct regmap_config *rmconfig;
+	struct regmap *regmap;
+	struct resource *res;
+	void __iomem *base;
+
+	rmconfig = devm_kzalloc(&pdev->dev, sizeof(*rmconfig), GFP_KERNEL);
+	if (!rmconfig)
+		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);
+
+	rmconfig->reg_bits = 32,
+	rmconfig->val_bits = 32,
+	rmconfig->reg_stride = 4,
+	rmconfig->max_register = resource_size(res);
+
+	regmap = devm_regmap_init_mmio(&pdev->dev, base, rmconfig);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	return berlin_pinctrl_probe(pdev, match->data);
+}
+
+static struct platform_driver berlin2_pinctrl_driver = {
+	.probe	= berlin2_pinctrl_probe,
+	.driver	= {
+		.name = "berlin-bg2-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = berlin2_pinctrl_match,
+	},
+};
+module_platform_driver(berlin2_pinctrl_driver);
+
+MODULE_AUTHOR("Antoine Ténart <antoine.tenart@free-electrons.com>");
+MODULE_DESCRIPTION("Marvell Berlin BG2 pinctrl driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/berlin/berlin-bg2cd.c b/drivers/pinctrl/berlin/berlin-bg2cd.c
new file mode 100644
index 00000000000..89d585ef7da
--- /dev/null
+++ b/drivers/pinctrl/berlin/berlin-bg2cd.c
@@ -0,0 +1,217 @@
+/*
+ * Marvell Berlin BG2CD pinctrl driver.
+ *
+ * Copyright (C) 2014 Marvell Technology Group Ltd.
+ *
+ * Antoine Ténart <antoine.tenart@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+
+#include "berlin.h"
+
+static const struct berlin_desc_group berlin2cd_soc_pinctrl_groups[] = {
+	/* G */
+	BERLIN_PINCTRL_GROUP("G0", 0x00, 0x1, 0x00,
+		BERLIN_PINCTRL_FUNCTION(0x0, "jtag"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "led"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "pwm")),
+	BERLIN_PINCTRL_GROUP("G1", 0x00, 0x2, 0x01,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sd0"),
+		BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G2", 0x00, 0x2, 0x02,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sd0"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "fe"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "pll"),
+		BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G3", 0x00, 0x2, 0x04,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sd0"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "twsi2"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "pll"),
+		BERLIN_PINCTRL_FUNCTION(0x4, "fe"),
+		BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G4", 0x00, 0x2, 0x06,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sd0"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "twsi3"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "pll"),
+		BERLIN_PINCTRL_FUNCTION(0x4, "pwm"),
+		BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G5", 0x00, 0x3, 0x08,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sd0"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "twsi3"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "arc"),
+		BERLIN_PINCTRL_FUNCTION(0x4, "pwm"),
+		BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G6", 0x00, 0x2, 0x0b,
+		BERLIN_PINCTRL_FUNCTION(0x0, "uart0"),	/* RX/TX */
+		BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
+	BERLIN_PINCTRL_GROUP("G7", 0x00, 0x3, 0x0d,
+		BERLIN_PINCTRL_FUNCTION(0x0, "eddc"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "twsi1"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("G8", 0x00, 0x3, 0x10,
+		BERLIN_PINCTRL_FUNCTION(0x0, "ss0"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
+	BERLIN_PINCTRL_GROUP("G9", 0x00, 0x3, 0x13,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "twsi0")),
+	BERLIN_PINCTRL_GROUP("G10", 0x00, 0x2, 0x16,
+		BERLIN_PINCTRL_FUNCTION(0x0, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
+	BERLIN_PINCTRL_GROUP("G11", 0x00, 0x2, 0x18,
+		BERLIN_PINCTRL_FUNCTION(0x0, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
+	BERLIN_PINCTRL_GROUP("G12", 0x00, 0x3, 0x1a,
+		BERLIN_PINCTRL_FUNCTION(0x0, "usb1"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
+	BERLIN_PINCTRL_GROUP("G13", 0x04, 0x3, 0x00,
+		BERLIN_PINCTRL_FUNCTION(0x0, "nand"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "usb0_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G14", 0x04, 0x1, 0x03,
+		BERLIN_PINCTRL_FUNCTION(0x0, "nand"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
+	BERLIN_PINCTRL_GROUP("G15", 0x04, 0x2, 0x04,
+		BERLIN_PINCTRL_FUNCTION(0x0, "jtag"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
+	BERLIN_PINCTRL_GROUP("G16", 0x04, 0x3, 0x06,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G17", 0x04, 0x3, 0x09,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G18", 0x04, 0x1, 0x0c,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G19", 0x04, 0x1, 0x0d,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G20", 0x04, 0x1, 0x0e,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G21", 0x04, 0x3, 0x0f,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G22", 0x04, 0x3, 0x12,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G23", 0x04, 0x3, 0x15,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G24", 0x04, 0x2, 0x18,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G25", 0x04, 0x2, 0x1a,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G26", 0x04, 0x1, 0x1c,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G27", 0x04, 0x1, 0x1d,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G28", 0x04, 0x2, 0x1e,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+};
+
+static const struct berlin_desc_group berlin2cd_sysmgr_pinctrl_groups[] = {
+	/* GSM */
+	BERLIN_PINCTRL_GROUP("GSM0", 0x40, 0x2, 0x00,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("GSM1", 0x40, 0x2, 0x02,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("GSM2", 0x40, 0x2, 0x04,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("GSM3", 0x40, 0x2, 0x06,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("GSM4", 0x40, 0x2, 0x08,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("GSM5", 0x40, 0x2, 0x0a,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("GSM6", 0x40, 0x2, 0x0c,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("GSM7", 0x40, 0x1, 0x0e,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("GSM8", 0x40, 0x1, 0x0f,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("GSM9", 0x40, 0x1, 0x10,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("GSM10", 0x40, 0x1, 0x11,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("GSM11", 0x40, 0x1, 0x12,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+};
+
+static const struct berlin_pinctrl_desc berlin2cd_soc_pinctrl_data = {
+	.groups = berlin2cd_soc_pinctrl_groups,
+	.ngroups = ARRAY_SIZE(berlin2cd_soc_pinctrl_groups),
+};
+
+static const struct berlin_pinctrl_desc berlin2cd_sysmgr_pinctrl_data = {
+	.groups = berlin2cd_sysmgr_pinctrl_groups,
+	.ngroups = ARRAY_SIZE(berlin2cd_sysmgr_pinctrl_groups),
+};
+
+static const struct of_device_id berlin2cd_pinctrl_match[] = {
+	{
+		.compatible = "marvell,berlin2cd-chip-ctrl",
+		.data = &berlin2cd_soc_pinctrl_data
+	},
+	{
+		.compatible = "marvell,berlin2cd-system-ctrl",
+		.data = &berlin2cd_sysmgr_pinctrl_data
+	},
+	{}
+};
+MODULE_DEVICE_TABLE(of, berlin2cd_pinctrl_match);
+
+static int berlin2cd_pinctrl_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *match =
+		of_match_device(berlin2cd_pinctrl_match, &pdev->dev);
+	struct regmap_config *rmconfig;
+	struct regmap *regmap;
+	struct resource *res;
+	void __iomem *base;
+
+	rmconfig = devm_kzalloc(&pdev->dev, sizeof(*rmconfig), GFP_KERNEL);
+	if (!rmconfig)
+		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);
+
+	rmconfig->reg_bits = 32,
+	rmconfig->val_bits = 32,
+	rmconfig->reg_stride = 4,
+	rmconfig->max_register = resource_size(res);
+
+	regmap = devm_regmap_init_mmio(&pdev->dev, base, rmconfig);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	return berlin_pinctrl_probe(pdev, match->data);
+}
+
+static struct platform_driver berlin2cd_pinctrl_driver = {
+	.probe	= berlin2cd_pinctrl_probe,
+	.driver	= {
+		.name = "berlin-bg2cd-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = berlin2cd_pinctrl_match,
+	},
+};
+module_platform_driver(berlin2cd_pinctrl_driver);
+
+MODULE_AUTHOR("Antoine Ténart <antoine.tenart@free-electrons.com>");
+MODULE_DESCRIPTION("Marvell Berlin BG2CD pinctrl driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/berlin/berlin-bg2q.c b/drivers/pinctrl/berlin/berlin-bg2q.c
new file mode 100644
index 00000000000..9fcf9836045
--- /dev/null
+++ b/drivers/pinctrl/berlin/berlin-bg2q.c
@@ -0,0 +1,436 @@
+/*
+ * Marvell Berlin BG2Q pinctrl driver
+ *
+ * Copyright (C) 2014 Marvell Technology Group Ltd.
+ *
+ * Antoine Ténart <antoine.tenart@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+
+#include "berlin.h"
+
+static const struct berlin_desc_group berlin2q_soc_pinctrl_groups[] = {
+	/* G */
+	BERLIN_PINCTRL_GROUP("G0", 0x18, 0x3, 0x00,
+			BERLIN_PINCTRL_FUNCTION(0x0, "nand"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "mmc"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("G1", 0x18, 0x3, 0x03,
+			BERLIN_PINCTRL_FUNCTION(0x0, "nand"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("G2", 0x18, 0x3, 0x06,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "arc"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "lvds")),
+	BERLIN_PINCTRL_GROUP("G3", 0x18, 0x3, 0x09,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "i2s2"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "lvds")),
+	BERLIN_PINCTRL_GROUP("G4", 0x18, 0x3, 0x0c,
+			BERLIN_PINCTRL_FUNCTION(0x0, "pll"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "sd0"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "rgmii"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "sata_dbg"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+			BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G5", 0x18, 0x3, 0x0f,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "sd0"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "rgmii"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "sata_dbg"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+			BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G6", 0x18, 0x3, 0x12,
+			BERLIN_PINCTRL_FUNCTION(0x0, "jtag"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "twsi0"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("G7", 0x18, 0x3, 0x15,
+			BERLIN_PINCTRL_FUNCTION(0x0, "jtag"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "twsi1"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "eddc")),
+	BERLIN_PINCTRL_GROUP("G8", 0x18, 0x3, 0x18,
+			BERLIN_PINCTRL_FUNCTION(0x0, "spi1"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
+	BERLIN_PINCTRL_GROUP("G9", 0x18, 0x3, 0x1b,
+			BERLIN_PINCTRL_FUNCTION(0x0, "spi1"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "sata")),
+	BERLIN_PINCTRL_GROUP("G10", 0x1c, 0x3, 0x00,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "spi1"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "i2s0"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "pwm"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "sata")),
+	BERLIN_PINCTRL_GROUP("G11", 0x1c, 0x3, 0x03,
+			BERLIN_PINCTRL_FUNCTION(0x0, "jtag"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "spi1"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "i2s1"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "pwm"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "sata")),
+	BERLIN_PINCTRL_GROUP("G12", 0x1c, 0x3, 0x06,
+			BERLIN_PINCTRL_FUNCTION(0x0, "agc"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
+	BERLIN_PINCTRL_GROUP("G13", 0x1c, 0x3, 0x09,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "sts1"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "rgmii"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+			BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G14", 0x1c, 0x3, 0x0c,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "sts0"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "sts1"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+			BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G15", 0x1c, 0x3, 0x0f,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "sts0"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "sts1"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "vdac"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+			BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G16", 0x1c, 0x3, 0x12,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "sts0"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "sts1"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "osco"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+			BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G17", 0x1c, 0x3, 0x15,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "rgmii"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "spdif"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "sts1"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+			BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G18", 0x1c, 0x3, 0x18,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "rgmii"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "i2s2"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "sts1")),
+	BERLIN_PINCTRL_GROUP("G19", 0x1c, 0x3, 0x1b,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "rgmii"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "i2s3"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "sts1"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "osco")),
+	BERLIN_PINCTRL_GROUP("G20", 0x20, 0x3, 0x00,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "demod"),
+			/*
+			 * Mode 0x4 mux usb2_dbg *and* usb3_dbg:
+			 * add two functions so it can be used with other groups
+			 * within the same subnode in the device tree
+			 */
+			BERLIN_PINCTRL_FUNCTION(0x4, "usb2_dbg"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "usb3_dbg")),
+	BERLIN_PINCTRL_GROUP("G21", 0x20, 0x3, 0x03,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "sts2"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "demod")),
+	BERLIN_PINCTRL_GROUP("G22", 0x20, 0x3, 0x06,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("G23", 0x20, 0x3, 0x09,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "avif"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "usb2_dbg")),
+	BERLIN_PINCTRL_GROUP("G24", 0x20, 0x3, 0x0c,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "demod"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "usb2_dbg")),
+	BERLIN_PINCTRL_GROUP("G25", 0x20, 0x3, 0x0f,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "vga"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "avif"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "usb2_dbg")),
+	BERLIN_PINCTRL_GROUP("G26", 0x20, 0x3, 0x12,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "lvds"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("G27", 0x20, 0x3, 0x15,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "agc"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("G28", 0x20, 0x3, 0x18,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "avif"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "usb2_dbg")),
+	BERLIN_PINCTRL_GROUP("G29", 0x20, 0x3, 0x1b,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "scrd0"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("G30", 0x24, 0x3, 0x00,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "scrd1"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("G31", 0x24, 0x3, 0x03,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "sd1"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("G32", 0x24, 0x3, 0x06,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "sd1"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	/* GAV */
+	BERLIN_PINCTRL_GROUP("GAV0", 0x24, 0x3, 0x09,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dvio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "lvds")),
+	BERLIN_PINCTRL_GROUP("GAV1", 0x24, 0x3, 0x0c,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dvio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "vga")),
+	BERLIN_PINCTRL_GROUP("GAV2", 0x24, 0x3, 0x0f,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dvio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "i2s3"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "pdm"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "adac")),
+	BERLIN_PINCTRL_GROUP("GAV3", 0x24, 0x3, 0x12,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dvio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "i2s3"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "adac")),
+	BERLIN_PINCTRL_GROUP("GAV4", 0x24, 0x3, 0x15,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dvio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "i2s1"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "adac")),
+	BERLIN_PINCTRL_GROUP("GAV5", 0x24, 0x3, 0x18,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dvio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "spdif")),
+	BERLIN_PINCTRL_GROUP("GAV6", 0x24, 0x3, 0x1b,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dvio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "i2s2")),
+	BERLIN_PINCTRL_GROUP("GAV7", 0x28, 0x3, 0x00,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dvio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "i2s3")),
+	BERLIN_PINCTRL_GROUP("GAV8", 0x28, 0x3, 0x03,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dv0"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "pwm")),
+	BERLIN_PINCTRL_GROUP("GAV9", 0x28, 0x3, 0x06,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dv0"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "pwm")),
+	BERLIN_PINCTRL_GROUP("GAV10", 0x28, 0x3, 0x09,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dv0"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "agc")),
+	BERLIN_PINCTRL_GROUP("GAV11", 0x28, 0x3, 0x0c,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dv0"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "i2s0"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "pwm"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "vclki")),
+	BERLIN_PINCTRL_GROUP("GAV12", 0x28, 0x3, 0x0f,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "i2s2"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "i2s1")),
+	BERLIN_PINCTRL_GROUP("GAV13", 0x28, 0x3, 0x12,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "i2s2")),
+	BERLIN_PINCTRL_GROUP("GAV14", 0x28, 0x3, 0x15,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "i2s2"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "i2s1")),
+	BERLIN_PINCTRL_GROUP("GAV15", 0x28, 0x3, 0x18,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "i2s1"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "dac_dbg")),
+	BERLIN_PINCTRL_GROUP("GAV16", 0x28, 0x3, 0x1b,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "i2s0"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "i2s1"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "i2s3"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "pdm"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "dac_dbg")),
+	BERLIN_PINCTRL_GROUP("GAV17", 0x2c, 0x3, 0x00,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "i2s0"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "i2s1"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "pwm"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "i2s3"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "pdm"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "dac_dbg")),
+	BERLIN_PINCTRL_GROUP("GAV18", 0x2c, 0x3, 0x03,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "spdif"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "arc")),
+	BERLIN_PINCTRL_GROUP("GAV19", 0x2c, 0x3, 0x06,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "spdif"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "i2s3"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "pdm")),
+};
+
+static const struct berlin_desc_group berlin2q_sysmgr_pinctrl_groups[] = {
+	/* GSM */
+	BERLIN_PINCTRL_GROUP("GSM0", 0x40, 0x2, 0x00,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "spi2"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "eth1")),
+	BERLIN_PINCTRL_GROUP("GSM1", 0x40, 0x2, 0x02,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "spi2"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "eth1")),
+	BERLIN_PINCTRL_GROUP("GSM2", 0x40, 0x2, 0x04,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "spi2"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "eddc")),
+	BERLIN_PINCTRL_GROUP("GSM3", 0x40, 0x2, 0x06,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "spi2"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "eddc")),
+	BERLIN_PINCTRL_GROUP("GSM4", 0x40, 0x1, 0x08,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "hdmi")),
+	BERLIN_PINCTRL_GROUP("GSM5", 0x40, 0x1, 0x09,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "hdmi")),
+	BERLIN_PINCTRL_GROUP("GSM6", 0x40, 0x1, 0x0a,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "hdmi")),
+	BERLIN_PINCTRL_GROUP("GSM7", 0x40, 0x1, 0x0b,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "hdmi")),
+	BERLIN_PINCTRL_GROUP("GSM8", 0x40, 0x1, 0x0c,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "hdmi")),
+	BERLIN_PINCTRL_GROUP("GSM9", 0x40, 0x1, 0x0d,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "hdmi")),
+	BERLIN_PINCTRL_GROUP("GSM10", 0x40, 0x1, 0x0e,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "led")),
+	BERLIN_PINCTRL_GROUP("GSM11", 0x40, 0x1, 0x0f,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "led")),
+	BERLIN_PINCTRL_GROUP("GSM12", 0x40, 0x2, 0x10,
+			BERLIN_PINCTRL_FUNCTION(0x0, "uart0"), /* RX/TX */
+			BERLIN_PINCTRL_FUNCTION(0x1, "irda0"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("GSM13", 0x40, 0x2, 0x12,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "uart0"), /* CTS/RTS */
+			BERLIN_PINCTRL_FUNCTION(0x2, "uart1"), /* RX/TX */
+			BERLIN_PINCTRL_FUNCTION(0x3, "twsi2")),
+	BERLIN_PINCTRL_GROUP("GSM14", 0x40, 0x2, 0x14,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "uart1"), /* RX/TX */
+			BERLIN_PINCTRL_FUNCTION(0x2, "irda1"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "twsi3")),
+	BERLIN_PINCTRL_GROUP("GSM15", 0x40, 0x2, 0x16,
+			BERLIN_PINCTRL_FUNCTION(0x0, "pwr"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "led"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("GSM16", 0x40, 0x1, 0x18,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "eddc")),
+	BERLIN_PINCTRL_GROUP("GSM17", 0x40, 0x1, 0x19,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "eddc")),
+	BERLIN_PINCTRL_GROUP("GSM18", 0x40, 0x1, 0x1a,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "eddc")),
+};
+
+static const struct berlin_pinctrl_desc berlin2q_soc_pinctrl_data = {
+	.groups = berlin2q_soc_pinctrl_groups,
+	.ngroups = ARRAY_SIZE(berlin2q_soc_pinctrl_groups),
+};
+
+static const struct berlin_pinctrl_desc berlin2q_sysmgr_pinctrl_data = {
+	.groups = berlin2q_sysmgr_pinctrl_groups,
+	.ngroups = ARRAY_SIZE(berlin2q_sysmgr_pinctrl_groups),
+};
+
+static const struct of_device_id berlin2q_pinctrl_match[] = {
+	{
+		.compatible = "marvell,berlin2q-chip-ctrl",
+		.data = &berlin2q_soc_pinctrl_data,
+	},
+	{
+		.compatible = "marvell,berlin2q-system-ctrl",
+		.data = &berlin2q_sysmgr_pinctrl_data,
+	},
+	{}
+};
+MODULE_DEVICE_TABLE(of, berlin2q_pinctrl_match);
+
+static int berlin2q_pinctrl_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *match =
+		of_match_device(berlin2q_pinctrl_match, &pdev->dev);
+	struct regmap_config *rmconfig;
+	struct regmap *regmap;
+	struct resource *res;
+	void __iomem *base;
+
+	rmconfig = devm_kzalloc(&pdev->dev, sizeof(*rmconfig), GFP_KERNEL);
+	if (!rmconfig)
+		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);
+
+	rmconfig->reg_bits = 32,
+	rmconfig->val_bits = 32,
+	rmconfig->reg_stride = 4,
+	rmconfig->max_register = resource_size(res);
+
+	regmap = devm_regmap_init_mmio(&pdev->dev, base, rmconfig);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	return berlin_pinctrl_probe(pdev, match->data);
+}
+
+static struct platform_driver berlin2q_pinctrl_driver = {
+	.probe	= berlin2q_pinctrl_probe,
+	.driver	= {
+		.name = "berlin-bg2q-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = berlin2q_pinctrl_match,
+	},
+};
+module_platform_driver(berlin2q_pinctrl_driver);
+
+MODULE_AUTHOR("Antoine Ténart <antoine.tenart@free-electrons.com>");
+MODULE_DESCRIPTION("Marvell Berlin BG2Q pinctrl driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/berlin/berlin.c b/drivers/pinctrl/berlin/berlin.c
new file mode 100644
index 00000000000..edf5d2fd2b2
--- /dev/null
+++ b/drivers/pinctrl/berlin/berlin.c
@@ -0,0 +1,348 @@
+/*
+ * Marvell Berlin SoC pinctrl core driver
+ *
+ * Copyright (C) 2014 Marvell Technology Group Ltd.
+ *
+ * Antoine Ténart <antoine.tenart@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinmux.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+
+#include "../core.h"
+#include "../pinctrl-utils.h"
+#include "berlin.h"
+
+struct berlin_pinctrl {
+	struct regmap *regmap;
+	struct device *dev;
+	const struct berlin_pinctrl_desc *desc;
+	struct berlin_pinctrl_function *functions;
+	unsigned nfunctions;
+	struct pinctrl_dev *pctrl_dev;
+};
+
+static int berlin_pinctrl_get_group_count(struct pinctrl_dev *pctrl_dev)
+{
+	struct berlin_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctrl_dev);
+
+	return pctrl->desc->ngroups;
+}
+
+static const char *berlin_pinctrl_get_group_name(struct pinctrl_dev *pctrl_dev,
+						 unsigned group)
+{
+	struct berlin_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctrl_dev);
+
+	return pctrl->desc->groups[group].name;
+}
+
+static int berlin_pinctrl_dt_node_to_map(struct pinctrl_dev *pctrl_dev,
+					 struct device_node *node,
+					 struct pinctrl_map **map,
+					 unsigned *num_maps)
+{
+	struct berlin_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctrl_dev);
+	struct property *prop;
+	const char *function_name, *group_name;
+	unsigned reserved_maps = 0;
+	int ret, ngroups;
+
+	*map = NULL;
+	*num_maps = 0;
+
+	ret = of_property_read_string(node, "function", &function_name);
+	if (ret) {
+		dev_err(pctrl->dev,
+			"missing function property in node %s\n",
+			node->name);
+		return -EINVAL;
+	}
+
+	ngroups = of_property_count_strings(node, "groups");
+	if (ngroups < 0) {
+		dev_err(pctrl->dev,
+			"missing groups property in node %s\n",
+			node->name);
+		return -EINVAL;
+	}
+
+	ret = pinctrl_utils_reserve_map(pctrl_dev, map, &reserved_maps,
+					num_maps, ngroups);
+	if (ret) {
+		dev_err(pctrl->dev, "can't reserve map: %d\n", ret);
+		return ret;
+	}
+
+	of_property_for_each_string(node, "groups", prop, group_name) {
+		ret = pinctrl_utils_add_map_mux(pctrl_dev, map, &reserved_maps,
+						num_maps, group_name,
+						function_name);
+		if (ret) {
+			dev_err(pctrl->dev, "can't add map: %d\n", ret);
+			return ret;
+		}
+	}
+
+	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,
+};
+
+static int berlin_pinmux_get_functions_count(struct pinctrl_dev *pctrl_dev)
+{
+	struct berlin_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctrl_dev);
+
+	return pctrl->nfunctions;
+}
+
+static const char *berlin_pinmux_get_function_name(struct pinctrl_dev *pctrl_dev,
+						   unsigned function)
+{
+	struct berlin_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctrl_dev);
+
+	return pctrl->functions[function].name;
+}
+
+static int berlin_pinmux_get_function_groups(struct pinctrl_dev *pctrl_dev,
+					     unsigned function,
+					     const char * const **groups,
+					     unsigned * const num_groups)
+{
+	struct berlin_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctrl_dev);
+
+	*groups = pctrl->functions[function].groups;
+	*num_groups = pctrl->functions[function].ngroups;
+
+	return 0;
+}
+
+static struct berlin_desc_function *
+berlin_pinctrl_find_function_by_name(struct berlin_pinctrl *pctrl,
+				     const struct berlin_desc_group *group,
+				     const char *fname)
+{
+	struct berlin_desc_function *function = group->functions;
+
+	while (function->name) {
+		if (!strcmp(function->name, fname))
+			return function;
+
+		function++;
+	}
+
+	return NULL;
+}
+
+static int berlin_pinmux_enable(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;
+	struct berlin_pinctrl_function *func = pctrl->functions + function;
+	struct berlin_desc_function *function_desc =
+		berlin_pinctrl_find_function_by_name(pctrl, group_desc,
+						     func->name);
+	u32 mask, val;
+
+	if (!function_desc)
+		return -EINVAL;
+
+	mask = GENMASK(group_desc->lsb + group_desc->bit_width - 1,
+		       group_desc->lsb);
+	val = function_desc->muxval << group_desc->lsb;
+	regmap_update_bits(pctrl->regmap, group_desc->offset, mask, val);
+
+	return 0;
+}
+
+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,
+};
+
+static int berlin_pinctrl_add_function(struct berlin_pinctrl *pctrl,
+				       const char *name)
+{
+	struct berlin_pinctrl_function *function = pctrl->functions;
+
+	while (function->name) {
+		if (!strcmp(function->name, name)) {
+			function->ngroups++;
+			return -EEXIST;
+		}
+		function++;
+	}
+
+	function->name = name;
+	function->ngroups = 1;
+
+	pctrl->nfunctions++;
+
+	return 0;
+}
+
+static int berlin_pinctrl_build_state(struct platform_device *pdev)
+{
+	struct berlin_pinctrl *pctrl = platform_get_drvdata(pdev);
+	struct berlin_desc_group const *desc_group;
+	struct berlin_desc_function const *desc_function;
+	int i, max_functions = 0;
+
+	pctrl->nfunctions = 0;
+
+	for (i = 0; i < pctrl->desc->ngroups; i++) {
+		desc_group = pctrl->desc->groups + i;
+		/* compute the maxiumum number of functions a group can have */
+		max_functions += 1 << (desc_group->bit_width + 1);
+	}
+
+	/* we will reallocate later */
+	pctrl->functions = devm_kzalloc(&pdev->dev,
+					max_functions * sizeof(*pctrl->functions),
+					GFP_KERNEL);
+	if (!pctrl->functions)
+		return -ENOMEM;
+
+	/* register all functions */
+	for (i = 0; i < pctrl->desc->ngroups; i++) {
+		desc_group = pctrl->desc->groups + i;
+		desc_function = desc_group->functions;
+
+		while (desc_function->name) {
+			berlin_pinctrl_add_function(pctrl, desc_function->name);
+			desc_function++;
+		}
+	}
+
+	pctrl->functions = krealloc(pctrl->functions,
+				    pctrl->nfunctions * sizeof(*pctrl->functions),
+				    GFP_KERNEL);
+
+	/* map functions to theirs groups */
+	for (i = 0; i < pctrl->desc->ngroups; i++) {
+		desc_group = pctrl->desc->groups + i;
+		desc_function = desc_group->functions;
+
+		while (desc_function->name) {
+			struct berlin_pinctrl_function
+				*function = pctrl->functions;
+			const char **groups;
+			bool found = false;
+
+			while (function->name) {
+				if (!strcmp(desc_function->name, function->name)) {
+					found = true;
+					break;
+				}
+				function++;
+			}
+
+			if (!found)
+				return -EINVAL;
+
+			if (!function->groups) {
+				function->groups =
+					devm_kzalloc(&pdev->dev,
+						     function->ngroups * sizeof(char *),
+						     GFP_KERNEL);
+
+				if (!function->groups)
+					return -ENOMEM;
+			}
+
+			groups = function->groups;
+			while (*groups)
+				groups++;
+
+			*groups = desc_group->name;
+
+			desc_function++;
+		}
+	}
+
+	return 0;
+}
+
+static struct pinctrl_desc berlin_pctrl_desc = {
+	.name		= "berlin-pinctrl",
+	.pctlops	= &berlin_pinctrl_ops,
+	.pmxops		= &berlin_pinmux_ops,
+	.owner		= THIS_MODULE,
+};
+
+int berlin_pinctrl_probe(struct platform_device *pdev,
+			 const struct berlin_pinctrl_desc *desc)
+{
+	struct device *dev = &pdev->dev;
+	struct berlin_pinctrl *pctrl;
+	struct regmap *regmap;
+	int ret;
+
+	regmap = dev_get_regmap(&pdev->dev, NULL);
+	if (!regmap)
+		return PTR_ERR(regmap);
+
+	pctrl = devm_kzalloc(dev, sizeof(*pctrl), GFP_KERNEL);
+	if (!pctrl)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, pctrl);
+
+	pctrl->regmap = regmap;
+	pctrl->dev = &pdev->dev;
+	pctrl->desc = desc;
+
+	ret = berlin_pinctrl_build_state(pdev);
+	if (ret) {
+		dev_err(dev, "cannot build driver state: %d\n", ret);
+		return ret;
+	}
+
+	pctrl->pctrl_dev = pinctrl_register(&berlin_pctrl_desc, dev, pctrl);
+	if (!pctrl->pctrl_dev) {
+		dev_err(dev, "failed to register pinctrl driver\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
diff --git a/drivers/pinctrl/berlin/berlin.h b/drivers/pinctrl/berlin/berlin.h
new file mode 100644
index 00000000000..e1aa8414519
--- /dev/null
+++ b/drivers/pinctrl/berlin/berlin.h
@@ -0,0 +1,61 @@
+/*
+ * Marvell Berlin SoC pinctrl driver.
+ *
+ * Copyright (C) 2014 Marvell Technology Group Ltd.
+ *
+ * Antoine Ténart <antoine.tenart@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#ifndef __PINCTRL_BERLIN_H
+#define __PINCTRL_BERLIN_H
+
+struct berlin_desc_function {
+	const char	*name;
+	u8		muxval;
+};
+
+struct berlin_desc_group {
+	const char			*name;
+	u8				offset;
+	u8				bit_width;
+	u8				lsb;
+	struct berlin_desc_function	*functions;
+};
+
+struct berlin_pinctrl_desc {
+	const struct berlin_desc_group	*groups;
+	unsigned			ngroups;
+};
+
+struct berlin_pinctrl_function {
+	const char	*name;
+	const char	**groups;
+	unsigned	ngroups;
+};
+
+#define BERLIN_PINCTRL_GROUP(_name, _offset, _width, _lsb, ...)		\
+	{								\
+		.name = _name,						\
+		.offset = _offset,					\
+		.bit_width = _width,					\
+		.lsb = _lsb,						\
+		.functions = (struct berlin_desc_function[]){		\
+			__VA_ARGS__, { } },				\
+	}
+
+#define BERLIN_PINCTRL_FUNCTION(_muxval, _name)		\
+	{						\
+		.name = _name,				\
+		.muxval = _muxval,			\
+	}
+
+#define BERLIN_PINCTRL_FUNCTION_UNKNOWN		{}
+
+int berlin_pinctrl_probe(struct platform_device *pdev,
+			 const struct berlin_pinctrl_desc *desc);
+
+#endif /* __PINCTRL_BERLIN_H */
diff --git a/drivers/pinctrl/core.c b/drivers/pinctrl/core.c
index c0fe6091566..e09474ecde2 100644
--- a/drivers/pinctrl/core.c
+++ b/drivers/pinctrl/core.c
@@ -468,6 +468,9 @@ int pinctrl_get_group_pins(struct pinctrl_dev *pctldev, const char *pin_group,
 	const struct pinctrl_ops *pctlops = pctldev->desc->pctlops;
 	int gs;
 
+	if (!pctlops->get_group_pins)
+		return -EINVAL;
+
 	gs = pinctrl_get_group_selector(pctldev, pin_group);
 	if (gs < 0)
 		return gs;
@@ -1362,15 +1365,16 @@ static int pinctrl_groups_show(struct seq_file *s, void *what)
 
 	seq_puts(s, "registered pin groups:\n");
 	while (selector < ngroups) {
-		const unsigned *pins;
-		unsigned num_pins;
+		const unsigned *pins = NULL;
+		unsigned num_pins = 0;
 		const char *gname = ops->get_group_name(pctldev, selector);
 		const char *pname;
-		int ret;
+		int ret = 0;
 		int i;
 
-		ret = ops->get_group_pins(pctldev, selector,
-					  &pins, &num_pins);
+		if (ops->get_group_pins)
+			ret = ops->get_group_pins(pctldev, selector,
+						  &pins, &num_pins);
 		if (ret)
 			seq_printf(s, "%s [ERROR GETTING PINS]\n",
 				   gname);
@@ -1694,8 +1698,7 @@ static int pinctrl_check_ops(struct pinctrl_dev *pctldev)
 
 	if (!ops ||
 	    !ops->get_groups_count ||
-	    !ops->get_group_name ||
-	    !ops->get_group_pins)
+	    !ops->get_group_name)
 		return -EINVAL;
 
 	if (ops->dt_node_to_map && !ops->dt_free_map)
diff --git a/drivers/pinctrl/mvebu/Kconfig b/drivers/pinctrl/mvebu/Kconfig
index cc298fade93..d6dd8358a6f 100644
--- a/drivers/pinctrl/mvebu/Kconfig
+++ b/drivers/pinctrl/mvebu/Kconfig
@@ -30,4 +30,8 @@ config PINCTRL_ARMADA_XP
 	bool
 	select PINCTRL_MVEBU
 
+config PINCTRL_ORION
+	bool
+	select PINCTRL_MVEBU
+
 endif
diff --git a/drivers/pinctrl/mvebu/Makefile b/drivers/pinctrl/mvebu/Makefile
index bc1b9f14f53..a0818e96374 100644
--- a/drivers/pinctrl/mvebu/Makefile
+++ b/drivers/pinctrl/mvebu/Makefile
@@ -5,3 +5,4 @@ obj-$(CONFIG_PINCTRL_ARMADA_370) += pinctrl-armada-370.o
 obj-$(CONFIG_PINCTRL_ARMADA_375) += pinctrl-armada-375.o
 obj-$(CONFIG_PINCTRL_ARMADA_38X) += pinctrl-armada-38x.o
 obj-$(CONFIG_PINCTRL_ARMADA_XP)  += pinctrl-armada-xp.o
+obj-$(CONFIG_PINCTRL_ORION)  += pinctrl-orion.o
diff --git a/drivers/pinctrl/mvebu/pinctrl-orion.c b/drivers/pinctrl/mvebu/pinctrl-orion.c
new file mode 100644
index 00000000000..dda1e7254e1
--- /dev/null
+++ b/drivers/pinctrl/mvebu/pinctrl-orion.c
@@ -0,0 +1,261 @@
+/*
+ * Marvell Orion pinctrl driver based on mvebu pinctrl core
+ *
+ * Author: Thomas Petazzoni <thomas.petazzoni@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.
+ *
+ * The first 16 MPP pins on Orion are easy to handle: they are
+ * configured through 2 consecutive registers, located at the base
+ * address of the MPP device.
+ *
+ * However the last 4 MPP pins are handled by a register at offset
+ * 0x50 from the base address, so it is not consecutive with the first
+ * two registers.
+ */
+
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-mvebu.h"
+
+static void __iomem *mpp_base;
+static void __iomem *high_mpp_base;
+
+static int orion_mpp_ctrl_get(unsigned pid, unsigned long *config)
+{
+	unsigned shift = (pid % MVEBU_MPPS_PER_REG) * MVEBU_MPP_BITS;
+
+	if (pid < 16) {
+		unsigned off = (pid / MVEBU_MPPS_PER_REG) * MVEBU_MPP_BITS;
+		*config = (readl(mpp_base + off) >> shift) & MVEBU_MPP_MASK;
+	}
+	else {
+		*config = (readl(high_mpp_base) >> shift) & MVEBU_MPP_MASK;
+	}
+
+	return 0;
+}
+
+static int orion_mpp_ctrl_set(unsigned pid, unsigned long config)
+{
+	unsigned shift = (pid % MVEBU_MPPS_PER_REG) * MVEBU_MPP_BITS;
+
+	if (pid < 16) {
+		unsigned off = (pid / MVEBU_MPPS_PER_REG) * MVEBU_MPP_BITS;
+		u32 reg = readl(mpp_base + off) & ~(MVEBU_MPP_MASK << shift);
+		writel(reg | (config << shift), mpp_base + off);
+	}
+	else {
+		u32 reg = readl(high_mpp_base) & ~(MVEBU_MPP_MASK << shift);
+		writel(reg | (config << shift), high_mpp_base);
+	}
+
+	return 0;
+}
+
+#define V(f5181l, f5182, f5281) \
+	((f5181l << 0) | (f5182 << 1) | (f5281 << 2))
+
+enum orion_variant {
+	V_5181L = V(1, 0, 0),
+	V_5182  = V(0, 1, 0),
+	V_5281  = V(0, 0, 1),
+	V_ALL   = V(1, 1, 1),
+};
+
+static struct mvebu_mpp_mode orion_mpp_modes[] = {
+	MPP_MODE(0,
+		 MPP_VAR_FUNCTION(0x0, "pcie", "rstout",    V_ALL),
+		 MPP_VAR_FUNCTION(0x2, "pci", "req2",       V_ALL),
+		 MPP_VAR_FUNCTION(0x3, "gpio", NULL,        V_ALL)),
+	MPP_MODE(1,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x2, "pci", "gnt2",       V_ALL)),
+	MPP_MODE(2,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x2, "pci", "req3",       V_ALL),
+		 MPP_VAR_FUNCTION(0x3, "pci-1", "pme",      V_ALL)),
+	MPP_MODE(3,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x2, "pci", "gnt3",       V_ALL)),
+	MPP_MODE(4,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x2, "pci", "req4",       V_ALL),
+		 MPP_VAR_FUNCTION(0x4, "bootnand", "re",    V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x5, "sata0", "prsnt",    V_5182)),
+	MPP_MODE(5,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x2, "pci", "gnt4",       V_ALL),
+		 MPP_VAR_FUNCTION(0x4, "bootnand", "we",    V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x5, "sata1", "prsnt",    V_5182)),
+	MPP_MODE(6,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x2, "pci", "req5",       V_ALL),
+		 MPP_VAR_FUNCTION(0x4, "nand", "re0",       V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x5, "pci-1", "clk",      V_5181L),
+		 MPP_VAR_FUNCTION(0x5, "sata0", "act",      V_5182)),
+	MPP_MODE(7,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x2, "pci", "gnt5",       V_ALL),
+		 MPP_VAR_FUNCTION(0x4, "nand", "we0",       V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x5, "pci-1", "clk",      V_5181L),
+		 MPP_VAR_FUNCTION(0x5, "sata1", "act",      V_5182)),
+	MPP_MODE(8,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x1, "ge", "col",         V_ALL)),
+	MPP_MODE(9,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x1, "ge", "rxerr",       V_ALL)),
+	MPP_MODE(10,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x1, "ge", "crs",         V_ALL)),
+	MPP_MODE(11,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x1, "ge", "txerr",       V_ALL)),
+	MPP_MODE(12,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x1, "ge", "txd4",        V_ALL),
+		 MPP_VAR_FUNCTION(0x4, "nand", "re1",       V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x5, "sata0", "ledprsnt", V_5182)),
+	MPP_MODE(13,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x1, "ge", "txd5",        V_ALL),
+		 MPP_VAR_FUNCTION(0x4, "nand", "we1",       V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x5, "sata1", "ledprsnt", V_5182)),
+	MPP_MODE(14,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x1, "ge", "txd6",        V_ALL),
+		 MPP_VAR_FUNCTION(0x4, "nand", "re2",       V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x5, "sata0", "ledact",   V_5182)),
+	MPP_MODE(15,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x1, "ge", "txd7",        V_ALL),
+		 MPP_VAR_FUNCTION(0x4, "nand", "we2",       V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x5, "sata1", "ledact",   V_5182)),
+	MPP_MODE(16,
+		 MPP_VAR_FUNCTION(0x0, "uart1", "rxd",      V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x1, "ge", "rxd4",        V_ALL),
+		 MPP_VAR_FUNCTION(0x5, "gpio", NULL,        V_5182)),
+	MPP_MODE(17,
+		 MPP_VAR_FUNCTION(0x0, "uart1", "txd",      V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x1, "ge", "rxd5",        V_ALL),
+		 MPP_VAR_FUNCTION(0x5, "gpio", NULL,        V_5182)),
+	MPP_MODE(18,
+		 MPP_VAR_FUNCTION(0x0, "uart1", "cts",      V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x1, "ge", "rxd6",        V_ALL),
+		 MPP_VAR_FUNCTION(0x5, "gpio", NULL,        V_5182)),
+	MPP_MODE(19,
+		 MPP_VAR_FUNCTION(0x0, "uart1", "rts",      V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x1, "ge", "rxd7",        V_ALL),
+		 MPP_VAR_FUNCTION(0x5, "gpio", NULL,        V_5182)),
+};
+
+static struct mvebu_mpp_ctrl orion_mpp_controls[] = {
+	MPP_FUNC_CTRL(0, 19, NULL, orion_mpp_ctrl),
+};
+
+static struct pinctrl_gpio_range mv88f5181l_gpio_ranges[] = {
+	MPP_GPIO_RANGE(0, 0, 0, 16),
+};
+
+static struct pinctrl_gpio_range mv88f5182_gpio_ranges[] = {
+	MPP_GPIO_RANGE(0, 0, 0, 19),
+};
+
+static struct pinctrl_gpio_range mv88f5281_gpio_ranges[] = {
+	MPP_GPIO_RANGE(0, 0, 0, 16),
+};
+
+static struct mvebu_pinctrl_soc_info mv88f5181l_info = {
+	.variant = V_5181L,
+	.controls = orion_mpp_controls,
+	.ncontrols = ARRAY_SIZE(orion_mpp_controls),
+	.modes = orion_mpp_modes,
+	.nmodes = ARRAY_SIZE(orion_mpp_modes),
+	.gpioranges = mv88f5181l_gpio_ranges,
+	.ngpioranges = ARRAY_SIZE(mv88f5181l_gpio_ranges),
+};
+
+static struct mvebu_pinctrl_soc_info mv88f5182_info = {
+	.variant = V_5182,
+	.controls = orion_mpp_controls,
+	.ncontrols = ARRAY_SIZE(orion_mpp_controls),
+	.modes = orion_mpp_modes,
+	.nmodes = ARRAY_SIZE(orion_mpp_modes),
+	.gpioranges = mv88f5182_gpio_ranges,
+	.ngpioranges = ARRAY_SIZE(mv88f5182_gpio_ranges),
+};
+
+static struct mvebu_pinctrl_soc_info mv88f5281_info = {
+	.variant = V_5281,
+	.controls = orion_mpp_controls,
+	.ncontrols = ARRAY_SIZE(orion_mpp_controls),
+	.modes = orion_mpp_modes,
+	.nmodes = ARRAY_SIZE(orion_mpp_modes),
+	.gpioranges = mv88f5281_gpio_ranges,
+	.ngpioranges = ARRAY_SIZE(mv88f5281_gpio_ranges),
+};
+
+/*
+ * There are multiple variants of the Orion SoCs, but in terms of pin
+ * muxing, they are identical.
+ */
+static struct of_device_id orion_pinctrl_of_match[] = {
+	{ .compatible = "marvell,88f5181l-pinctrl", .data = &mv88f5181l_info },
+	{ .compatible = "marvell,88f5182-pinctrl", .data = &mv88f5182_info },
+	{ .compatible = "marvell,88f5281-pinctrl", .data = &mv88f5281_info },
+	{ }
+};
+
+static int orion_pinctrl_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *match =
+		of_match_device(orion_pinctrl_of_match, &pdev->dev);
+	struct resource *res;
+
+	pdev->dev.platform_data = (void*)match->data;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	mpp_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(mpp_base))
+		return PTR_ERR(mpp_base);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	high_mpp_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(high_mpp_base))
+		return PTR_ERR(high_mpp_base);
+
+	return mvebu_pinctrl_probe(pdev);
+}
+
+static int orion_pinctrl_remove(struct platform_device *pdev)
+{
+	return mvebu_pinctrl_remove(pdev);
+}
+
+static struct platform_driver orion_pinctrl_driver = {
+	.driver = {
+		.name = "orion-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = of_match_ptr(orion_pinctrl_of_match),
+	},
+	.probe = orion_pinctrl_probe,
+	.remove = orion_pinctrl_remove,
+};
+
+module_platform_driver(orion_pinctrl_driver);
+
+MODULE_AUTHOR("Thomas Petazzoni <thomas.petazzoni@free-electrons.com>");
+MODULE_DESCRIPTION("Marvell Orion pinctrl driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/pinconf-generic.c b/drivers/pinctrl/pinconf-generic.c
index 3d9a999fb69..29ff77f90fc 100644
--- a/drivers/pinctrl/pinconf-generic.c
+++ b/drivers/pinctrl/pinconf-generic.c
@@ -166,6 +166,7 @@ static struct pinconf_generic_dt_params dt_params[] = {
 	{ "input-schmitt-enable", PIN_CONFIG_INPUT_SCHMITT_ENABLE, 1 },
 	{ "input-schmitt-disable", PIN_CONFIG_INPUT_SCHMITT_ENABLE, 0 },
 	{ "input-debounce", PIN_CONFIG_INPUT_DEBOUNCE, 0 },
+	{ "power-source", PIN_CONFIG_POWER_SOURCE, 0 },
 	{ "low-power-enable", PIN_CONFIG_LOW_POWER_MODE, 1 },
 	{ "low-power-disable", PIN_CONFIG_LOW_POWER_MODE, 0 },
 	{ "output-low", PIN_CONFIG_OUTPUT, 0, },
@@ -228,13 +229,12 @@ int pinconf_generic_parse_dt_config(struct device_node *np,
 	 * Now limit the number of configs to the real number of
 	 * found properties.
 	 */
-	*configs = kzalloc(ncfg * sizeof(unsigned long), GFP_KERNEL);
+	*configs = kmemdup(cfg, ncfg * sizeof(unsigned long), GFP_KERNEL);
 	if (!*configs) {
 		ret = -ENOMEM;
 		goto out;
 	}
 
-	memcpy(*configs, cfg, ncfg * sizeof(unsigned long));
 	*nconfigs = ncfg;
 
 out:
diff --git a/drivers/pinctrl/pinctrl-adi2.c b/drivers/pinctrl/pinctrl-adi2.c
index 0cc0eec8339..5c44feb54eb 100644
--- a/drivers/pinctrl/pinctrl-adi2.c
+++ b/drivers/pinctrl/pinctrl-adi2.c
@@ -927,7 +927,7 @@ static int adi_gpio_irq_map(struct irq_domain *d, unsigned int irq,
 	return 0;
 }
 
-const struct irq_domain_ops adi_gpio_irq_domain_ops = {
+static const struct irq_domain_ops adi_gpio_irq_domain_ops = {
 	.map = adi_gpio_irq_map,
 	.xlate = irq_domain_xlate_onecell,
 };
diff --git a/drivers/pinctrl/pinctrl-apq8064.c b/drivers/pinctrl/pinctrl-apq8064.c
new file mode 100644
index 00000000000..519f7886b0f
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-apq8064.c
@@ -0,0 +1,613 @@
+/*
+ * Copyright (c) 2014, Sony Mobile Communications AB.
+ *
+ * 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 apq8064_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, "SDC1_CLK"),
+	PINCTRL_PIN(91, "SDC1_CMD"),
+	PINCTRL_PIN(92, "SDC1_DATA"),
+	PINCTRL_PIN(93, "SDC3_CLK"),
+	PINCTRL_PIN(94, "SDC3_CMD"),
+	PINCTRL_PIN(95, "SDC3_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);
+
+static const unsigned int sdc1_clk_pins[] = { 90 };
+static const unsigned int sdc1_cmd_pins[] = { 91 };
+static const unsigned int sdc1_data_pins[] = { 92 };
+static const unsigned int sdc3_clk_pins[] = { 93 };
+static const unsigned int sdc3_cmd_pins[] = { 94 };
+static const unsigned int sdc3_data_pins[] = { 95 };
+
+#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, f8, f9, f10) \
+	{						\
+		.name = "gpio" #id,			\
+		.pins = gpio##id##_pins,		\
+		.npins = ARRAY_SIZE(gpio##id##_pins),	\
+		.funcs = (int[]){			\
+			APQ_MUX_NA, /* gpio mode */	\
+			APQ_MUX_##f1,			\
+			APQ_MUX_##f2,			\
+			APQ_MUX_##f3,			\
+			APQ_MUX_##f4,			\
+			APQ_MUX_##f5,			\
+			APQ_MUX_##f6,			\
+			APQ_MUX_##f7,			\
+			APQ_MUX_##f8,			\
+			APQ_MUX_##f9,			\
+			APQ_MUX_##f10,			\
+		},					\
+		.nfuncs = 11,				\
+		.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 = 0x400 + 0x4 * 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 = 1,			\
+		.intr_target_bit = 0,			\
+		.intr_raw_status_bit = 3,		\
+		.intr_polarity_bit = 1,			\
+		.intr_detection_bit = 2,		\
+		.intr_detection_width = 1,		\
+	}
+
+#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_raw_status_bit = -1,		\
+		.intr_polarity_bit = -1,		\
+		.intr_detection_bit = -1,		\
+		.intr_detection_width = -1,		\
+	}
+
+enum apq8064_functions {
+	APQ_MUX_cam_mclk,
+	APQ_MUX_codec_mic_i2s,
+	APQ_MUX_codec_spkr_i2s,
+	APQ_MUX_gsbi1,
+	APQ_MUX_gsbi2,
+	APQ_MUX_gsbi3,
+	APQ_MUX_gsbi4,
+	APQ_MUX_gsbi4_cam_i2c,
+	APQ_MUX_gsbi5,
+	APQ_MUX_gsbi5_spi_cs1,
+	APQ_MUX_gsbi5_spi_cs2,
+	APQ_MUX_gsbi5_spi_cs3,
+	APQ_MUX_gsbi6,
+	APQ_MUX_gsbi6_spi_cs1,
+	APQ_MUX_gsbi6_spi_cs2,
+	APQ_MUX_gsbi6_spi_cs3,
+	APQ_MUX_gsbi7,
+	APQ_MUX_gsbi7_spi_cs1,
+	APQ_MUX_gsbi7_spi_cs2,
+	APQ_MUX_gsbi7_spi_cs3,
+	APQ_MUX_gsbi_cam_i2c,
+	APQ_MUX_hdmi,
+	APQ_MUX_mi2s,
+	APQ_MUX_riva_bt,
+	APQ_MUX_riva_fm,
+	APQ_MUX_riva_wlan,
+	APQ_MUX_sdc2,
+	APQ_MUX_sdc4,
+	APQ_MUX_slimbus,
+	APQ_MUX_spkr_i2s,
+	APQ_MUX_tsif1,
+	APQ_MUX_tsif2,
+	APQ_MUX_usb2_hsic,
+	APQ_MUX_NA,
+};
+
+static const char * const cam_mclk_groups[] = {
+	"gpio4" "gpio5"
+};
+static const char * const codec_mic_i2s_groups[] = {
+	"gpio34", "gpio35", "gpio36", "gpio37", "gpio38"
+};
+static const char * const codec_spkr_i2s_groups[] = {
+	"gpio39", "gpio40", "gpio41", "gpio42"
+};
+static const char * const gsbi1_groups[] = {
+	"gpio18", "gpio19", "gpio20", "gpio21"
+};
+static const char * const gsbi2_groups[] = {
+	"gpio22", "gpio23", "gpio24", "gpio25"
+};
+static const char * const gsbi3_groups[] = {
+	"gpio6", "gpio7", "gpio8", "gpio9"
+};
+static const char * const gsbi4_groups[] = {
+	"gpio10", "gpio11", "gpio12", "gpio13"
+};
+static const char * const gsbi4_cam_i2c_groups[] = {
+	"gpio10", "gpio11", "gpio12", "gpio13"
+};
+static const char * const gsbi5_groups[] = {
+	"gpio51", "gpio52", "gpio53", "gpio54"
+};
+static const char * const gsbi5_spi_cs1_groups[] = {
+	"gpio47"
+};
+static const char * const gsbi5_spi_cs2_groups[] = {
+	"gpio31"
+};
+static const char * const gsbi5_spi_cs3_groups[] = {
+	"gpio32"
+};
+static const char * const gsbi6_groups[] = {
+	"gpio14", "gpio15", "gpio16", "gpio17"
+};
+static const char * const gsbi6_spi_cs1_groups[] = {
+	"gpio47"
+};
+static const char * const gsbi6_spi_cs2_groups[] = {
+	"gpio31"
+};
+static const char * const gsbi6_spi_cs3_groups[] = {
+	"gpio32"
+};
+static const char * const gsbi7_groups[] = {
+	"gpio82", "gpio83", "gpio84", "gpio85"
+};
+static const char * const gsbi7_spi_cs1_groups[] = {
+	"gpio47"
+};
+static const char * const gsbi7_spi_cs2_groups[] = {
+	"gpio31"
+};
+static const char * const gsbi7_spi_cs3_groups[] = {
+	"gpio32"
+};
+static const char * const gsbi_cam_i2c_groups[] = {
+	"gpio10", "gpio11", "gpio12", "gpio13"
+};
+static const char * const hdmi_groups[] = {
+	"gpio69", "gpio70", "gpio71", "gpio72"
+};
+static const char * const mi2s_groups[] = {
+	"gpio27", "gpio28", "gpio29", "gpio30", "gpio31", "gpio32", "gpio33"
+};
+static const char * const riva_bt_groups[] = {
+	"gpio16", "gpio17"
+};
+static const char * const riva_fm_groups[] = {
+	"gpio14", "gpio15"
+};
+static const char * const riva_wlan_groups[] = {
+	"gpio64", "gpio65", "gpio66", "gpio67", "gpio68"
+};
+static const char * const sdc2_groups[] = {
+	"gpio57", "gpio58", "gpio59", "gpio60", "gpio61", "gpio62"
+};
+static const char * const sdc4_groups[] = {
+	"gpio63", "gpio64", "gpio65", "gpio66", "gpio67", "gpio68"
+};
+static const char * const slimbus_groups[] = {
+	"gpio40", "gpio41"
+};
+static const char * const spkr_i2s_groups[] = {
+	"gpio47", "gpio48", "gpio49", "gpio50"
+};
+static const char * const tsif1_groups[] = {
+	"gpio55", "gpio56", "gpio57"
+};
+static const char * const tsif2_groups[] = {
+	"gpio58", "gpio59", "gpio60"
+};
+static const char * const usb2_hsic_groups[] = {
+	"gpio88", "gpio89"
+};
+
+static const struct msm_function apq8064_functions[] = {
+	FUNCTION(cam_mclk),
+	FUNCTION(codec_mic_i2s),
+	FUNCTION(codec_spkr_i2s),
+	FUNCTION(gsbi1),
+	FUNCTION(gsbi2),
+	FUNCTION(gsbi3),
+	FUNCTION(gsbi4),
+	FUNCTION(gsbi4_cam_i2c),
+	FUNCTION(gsbi5),
+	FUNCTION(gsbi5_spi_cs1),
+	FUNCTION(gsbi5_spi_cs2),
+	FUNCTION(gsbi5_spi_cs3),
+	FUNCTION(gsbi6),
+	FUNCTION(gsbi6_spi_cs1),
+	FUNCTION(gsbi6_spi_cs2),
+	FUNCTION(gsbi6_spi_cs3),
+	FUNCTION(gsbi7),
+	FUNCTION(gsbi7_spi_cs1),
+	FUNCTION(gsbi7_spi_cs2),
+	FUNCTION(gsbi7_spi_cs3),
+	FUNCTION(gsbi_cam_i2c),
+	FUNCTION(hdmi),
+	FUNCTION(mi2s),
+	FUNCTION(riva_bt),
+	FUNCTION(riva_fm),
+	FUNCTION(riva_wlan),
+	FUNCTION(sdc2),
+	FUNCTION(sdc4),
+	FUNCTION(slimbus),
+	FUNCTION(spkr_i2s),
+	FUNCTION(tsif1),
+	FUNCTION(tsif2),
+	FUNCTION(usb2_hsic),
+};
+
+static const struct msm_pingroup apq8064_groups[] = {
+	PINGROUP(0, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(1, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(2, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(3, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(4, NA, NA, cam_mclk, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(5, NA, cam_mclk, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(6, gsbi3, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(7, gsbi3, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(8, gsbi3, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(9, gsbi3, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(10, gsbi4, NA, NA, NA, NA, NA, NA, NA, gsbi4_cam_i2c, NA),
+	PINGROUP(11, gsbi4, NA, NA, NA, NA, NA, NA, NA, NA, gsbi4_cam_i2c),
+	PINGROUP(12, gsbi4, NA, NA, NA, NA, gsbi4_cam_i2c, NA, NA, NA, NA),
+	PINGROUP(13, gsbi4, NA, NA, NA, NA, gsbi4_cam_i2c, NA, NA, NA, NA),
+	PINGROUP(14, riva_fm, gsbi6, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(15, riva_fm, gsbi6, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(16, riva_bt, gsbi6, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(17, riva_bt, gsbi6, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(18, gsbi1, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(19, gsbi1, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(20, gsbi1, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(21, gsbi1, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(22, gsbi2, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(23, gsbi2, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(24, gsbi2, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(25, gsbi2, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(26, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(27, mi2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(28, mi2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(29, mi2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(30, mi2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(31, mi2s, NA, gsbi5_spi_cs2, gsbi6_spi_cs2, gsbi7_spi_cs2, NA, NA, NA, NA, NA),
+	PINGROUP(32, mi2s, NA, NA, NA, NA, gsbi5_spi_cs3, gsbi6_spi_cs3, gsbi7_spi_cs3, NA, NA),
+	PINGROUP(33, mi2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(34, codec_mic_i2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(35, codec_mic_i2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(36, codec_mic_i2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(37, codec_mic_i2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(38, codec_mic_i2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(39, codec_spkr_i2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(40, slimbus, codec_spkr_i2s, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(41, slimbus, codec_spkr_i2s, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(42, codec_spkr_i2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(43, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(44, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(45, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(46, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(47, spkr_i2s, gsbi5_spi_cs1, gsbi6_spi_cs1, gsbi7_spi_cs1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(48, spkr_i2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(49, spkr_i2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(50, spkr_i2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(51, NA, gsbi5, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(52, NA, gsbi5, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(53, NA, gsbi5, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(54, NA, gsbi5, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(55, tsif1, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(56, tsif1, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(57, tsif1, sdc2, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(58, tsif2, sdc2, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(59, tsif2, sdc2, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(60, tsif2, sdc2, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(61, NA, sdc2, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(62, NA, sdc2, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(63, NA, sdc4, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(64, riva_wlan, sdc4, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(65, riva_wlan, sdc4, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(66, riva_wlan, sdc4, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(67, riva_wlan, sdc4, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(68, riva_wlan, sdc4, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(69, hdmi, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(70, hdmi, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(71, hdmi, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(72, hdmi, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(73, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(74, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	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(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),
+	PINGROUP(82, NA, gsbi7, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(83, gsbi7, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(84, NA, gsbi7, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(85, NA, NA, gsbi7, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(86, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(87, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(88, usb2_hsic, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(89, usb2_hsic, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+
+	SDC_PINGROUP(sdc1_clk, 0x20a0, 13, 6),
+	SDC_PINGROUP(sdc1_cmd, 0x20a0, 11, 3),
+	SDC_PINGROUP(sdc1_data, 0x20a0, 9, 0),
+
+	SDC_PINGROUP(sdc3_clk, 0x20a4, 14, 6),
+	SDC_PINGROUP(sdc3_cmd, 0x20a4, 11, 3),
+	SDC_PINGROUP(sdc3_data, 0x20a4, 9, 0),
+};
+
+#define NUM_GPIO_PINGROUPS 90
+
+static const struct msm_pinctrl_soc_data apq8064_pinctrl = {
+	.pins = apq8064_pins,
+	.npins = ARRAY_SIZE(apq8064_pins),
+	.functions = apq8064_functions,
+	.nfunctions = ARRAY_SIZE(apq8064_functions),
+	.groups = apq8064_groups,
+	.ngroups = ARRAY_SIZE(apq8064_groups),
+	.ngpios = NUM_GPIO_PINGROUPS,
+};
+
+static int apq8064_pinctrl_probe(struct platform_device *pdev)
+{
+	return msm_pinctrl_probe(pdev, &apq8064_pinctrl);
+}
+
+static const struct of_device_id apq8064_pinctrl_of_match[] = {
+	{ .compatible = "qcom,apq8064-pinctrl", },
+	{ },
+};
+
+static struct platform_driver apq8064_pinctrl_driver = {
+	.driver = {
+		.name = "apq8064-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = apq8064_pinctrl_of_match,
+	},
+	.probe = apq8064_pinctrl_probe,
+	.remove = msm_pinctrl_remove,
+};
+
+static int __init apq8064_pinctrl_init(void)
+{
+	return platform_driver_register(&apq8064_pinctrl_driver);
+}
+arch_initcall(apq8064_pinctrl_init);
+
+static void __exit apq8064_pinctrl_exit(void)
+{
+	platform_driver_unregister(&apq8064_pinctrl_driver);
+}
+module_exit(apq8064_pinctrl_exit);
+
+MODULE_AUTHOR("Bjorn Andersson <bjorn.andersson@sonymobile.com>");
+MODULE_DESCRIPTION("Qualcomm APQ8064 pinctrl driver");
+MODULE_LICENSE("GPL v2");
+MODULE_DEVICE_TABLE(of, apq8064_pinctrl_of_match);
diff --git a/drivers/pinctrl/pinctrl-at91.c b/drivers/pinctrl/pinctrl-at91.c
index 5d24aaec5db..421493cb490 100644
--- a/drivers/pinctrl/pinctrl-at91.c
+++ b/drivers/pinctrl/pinctrl-at91.c
@@ -16,9 +16,6 @@
 #include <linux/of_irq.h>
 #include <linux/slab.h>
 #include <linux/interrupt.h>
-#include <linux/irq.h>
-#include <linux/irqdomain.h>
-#include <linux/irqchip/chained_irq.h>
 #include <linux/io.h>
 #include <linux/gpio.h>
 #include <linux/pinctrl/machine.h>
@@ -47,7 +44,6 @@ struct at91_gpio_chip {
 	int			pioc_idx;	/* PIO bank index */
 	void __iomem		*regbase;	/* PIO bank virtual address */
 	struct clk		*clock;		/* associated clock */
-	struct irq_domain	*domain;	/* associated irq domain */
 	struct at91_pinctrl_mux_ops *ops;	/* ops */
 };
 
@@ -1192,21 +1188,6 @@ static int at91_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
 	return 0;
 }
 
-static int at91_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
-{
-	struct at91_gpio_chip *at91_gpio = to_at91_gpio_chip(chip);
-	int virq;
-
-	if (offset < chip->ngpio)
-		virq = irq_create_mapping(at91_gpio->domain, offset);
-	else
-		virq = -ENXIO;
-
-	dev_dbg(chip->dev, "%s: request IRQ for GPIO %d, return %d\n",
-				chip->label, offset + chip->base, virq);
-	return virq;
-}
-
 #ifdef CONFIG_DEBUG_FS
 static void at91_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 {
@@ -1216,8 +1197,7 @@ static void at91_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 	void __iomem *pio = at91_gpio->regbase;
 
 	for (i = 0; i < chip->ngpio; i++) {
-		unsigned pin = chip->base + i;
-		unsigned mask = pin_to_mask(pin);
+		unsigned mask = pin_to_mask(i);
 		const char *gpio_label;
 		u32 pdsr;
 
@@ -1336,6 +1316,11 @@ static int alt_gpio_irq_type(struct irq_data *d, unsigned type)
 	return 0;
 }
 
+static void gpio_irq_ack(struct irq_data *d)
+{
+	/* the interrupt is already cleared before by reading ISR */
+}
+
 static unsigned int gpio_irq_startup(struct irq_data *d)
 {
 	struct at91_gpio_chip *at91_gpio = irq_data_get_irq_chip_data(d);
@@ -1435,6 +1420,7 @@ void at91_pinctrl_gpio_resume(void)
 
 static struct irq_chip gpio_irqchip = {
 	.name		= "GPIO",
+	.irq_ack	= gpio_irq_ack,
 	.irq_startup	= gpio_irq_startup,
 	.irq_shutdown	= gpio_irq_shutdown,
 	.irq_disable	= gpio_irq_mask,
@@ -1446,9 +1432,11 @@ static struct irq_chip gpio_irqchip = {
 
 static void gpio_irq_handler(unsigned irq, struct irq_desc *desc)
 {
-	struct irq_chip *chip = irq_desc_get_chip(desc);
-	struct irq_data *idata = irq_desc_get_irq_data(desc);
-	struct at91_gpio_chip *at91_gpio = irq_data_get_irq_chip_data(idata);
+	struct irq_chip *chip = irq_get_chip(irq);
+	struct gpio_chip *gpio_chip = irq_desc_get_handler_data(desc);
+	struct at91_gpio_chip *at91_gpio = container_of(gpio_chip,
+					   struct at91_gpio_chip, chip);
+
 	void __iomem	*pio = at91_gpio->regbase;
 	unsigned long	isr;
 	int		n;
@@ -1465,85 +1453,25 @@ static void gpio_irq_handler(unsigned irq, struct irq_desc *desc)
 				break;
 			at91_gpio = at91_gpio->next;
 			pio = at91_gpio->regbase;
+			gpio_chip = &at91_gpio->chip;
 			continue;
 		}
 
 		for_each_set_bit(n, &isr, BITS_PER_LONG) {
-			generic_handle_irq(irq_find_mapping(at91_gpio->domain, n));
+			generic_handle_irq(irq_find_mapping(
+					   gpio_chip->irqdomain, n));
 		}
 	}
 	chained_irq_exit(chip, desc);
 	/* now it may re-trigger */
 }
 
-/*
- * This lock class tells lockdep that GPIO irqs are in a different
- * category than their parents, so it won't report false recursion.
- */
-static struct lock_class_key gpio_lock_class;
-
-static int at91_gpio_irq_map(struct irq_domain *h, unsigned int virq,
-							irq_hw_number_t hw)
-{
-	struct at91_gpio_chip	*at91_gpio = h->host_data;
-	void __iomem		*pio = at91_gpio->regbase;
-	u32			mask = 1 << hw;
-
-	irq_set_lockdep_class(virq, &gpio_lock_class);
-
-	/*
-	 * Can use the "simple" and not "edge" handler since it's
-	 * shorter, and the AIC handles interrupts sanely.
-	 */
-	irq_set_chip(virq, &gpio_irqchip);
-	if ((at91_gpio->ops == &at91sam9x5_ops) &&
-	    (readl_relaxed(pio + PIO_AIMMR) & mask) &&
-	    (readl_relaxed(pio + PIO_ELSR) & mask))
-		irq_set_handler(virq, handle_level_irq);
-	else
-		irq_set_handler(virq, handle_simple_irq);
-	set_irq_flags(virq, IRQF_VALID);
-	irq_set_chip_data(virq, at91_gpio);
-
-	return 0;
-}
-
-static int at91_gpio_irq_domain_xlate(struct irq_domain *d,
-				      struct device_node *ctrlr,
-				      const u32 *intspec, unsigned int intsize,
-				      irq_hw_number_t *out_hwirq,
-				      unsigned int *out_type)
-{
-	struct at91_gpio_chip *at91_gpio = d->host_data;
-	int ret;
-	int pin = at91_gpio->chip.base + intspec[0];
-
-	if (WARN_ON(intsize < 2))
-		return -EINVAL;
-	*out_hwirq = intspec[0];
-	*out_type = intspec[1] & IRQ_TYPE_SENSE_MASK;
-
-	ret = gpio_request(pin, ctrlr->full_name);
-	if (ret)
-		return ret;
-
-	ret = gpio_direction_input(pin);
-	if (ret)
-		return ret;
-
-	return 0;
-}
-
-static struct irq_domain_ops at91_gpio_ops = {
-	.map	= at91_gpio_irq_map,
-	.xlate	= at91_gpio_irq_domain_xlate,
-};
-
 static int at91_gpio_of_irq_setup(struct device_node *node,
 				  struct at91_gpio_chip *at91_gpio)
 {
-	struct at91_gpio_chip	*prev = NULL;
+	struct at91_gpio_chip   *prev = NULL;
 	struct irq_data		*d = irq_get_irq_data(at91_gpio->pioc_virq);
+	int ret;
 
 	at91_gpio->pioc_hwirq = irqd_to_hwirq(d);
 
@@ -1553,10 +1481,17 @@ static int at91_gpio_of_irq_setup(struct device_node *node,
 	/* Disable irqs of this PIO controller */
 	writel_relaxed(~0, at91_gpio->regbase + PIO_IDR);
 
-	/* Setup irq domain */
-	at91_gpio->domain = irq_domain_add_linear(node, at91_gpio->chip.ngpio,
-						&at91_gpio_ops, at91_gpio);
-	if (!at91_gpio->domain)
+	/*
+	 * Let the generic code handle this edge IRQ, the the chained
+	 * handler will perform the actual work of handling the parent
+	 * interrupt.
+	 */
+	ret = gpiochip_irqchip_add(&at91_gpio->chip,
+				   &gpio_irqchip,
+				   0,
+				   handle_edge_irq,
+				   IRQ_TYPE_EDGE_BOTH);
+	if (ret)
 		panic("at91_gpio.%d: couldn't allocate irq domain (DT).\n",
 			at91_gpio->pioc_idx);
 
@@ -1571,8 +1506,11 @@ static int at91_gpio_of_irq_setup(struct device_node *node,
 	if (prev && prev->next == at91_gpio)
 		return 0;
 
-	irq_set_chip_data(at91_gpio->pioc_virq, at91_gpio);
-	irq_set_chained_handler(at91_gpio->pioc_virq, gpio_irq_handler);
+	/* Then register the chain on the parent IRQ */
+	gpiochip_set_chained_irqchip(&at91_gpio->chip,
+				     &gpio_irqchip,
+				     at91_gpio->pioc_virq,
+				     gpio_irq_handler);
 
 	return 0;
 }
@@ -1586,7 +1524,6 @@ static struct gpio_chip at91_gpio_template = {
 	.get			= at91_gpio_get,
 	.direction_output	= at91_gpio_direction_output,
 	.set			= at91_gpio_set,
-	.to_irq			= at91_gpio_to_irq,
 	.dbg_show		= at91_gpio_dbg_show,
 	.can_sleep		= false,
 	.ngpio			= MAX_NB_GPIO_PER_BANK,
diff --git a/drivers/pinctrl/pinctrl-baytrail.c b/drivers/pinctrl/pinctrl-baytrail.c
index 6e8301f7718..975572e2f26 100644
--- a/drivers/pinctrl/pinctrl-baytrail.c
+++ b/drivers/pinctrl/pinctrl-baytrail.c
@@ -43,9 +43,20 @@
 #define BYT_INT_STAT_REG	0x800
 
 /* BYT_CONF0_REG register bits */
+#define BYT_IODEN		BIT(31)
 #define BYT_TRIG_NEG		BIT(26)
 #define BYT_TRIG_POS		BIT(25)
 #define BYT_TRIG_LVL		BIT(24)
+#define BYT_PULL_STR_SHIFT	9
+#define BYT_PULL_STR_MASK	(3 << BYT_PULL_STR_SHIFT)
+#define BYT_PULL_STR_2K		(0 << BYT_PULL_STR_SHIFT)
+#define BYT_PULL_STR_10K	(1 << BYT_PULL_STR_SHIFT)
+#define BYT_PULL_STR_20K	(2 << BYT_PULL_STR_SHIFT)
+#define BYT_PULL_STR_40K	(3 << BYT_PULL_STR_SHIFT)
+#define BYT_PULL_ASSIGN_SHIFT	7
+#define BYT_PULL_ASSIGN_MASK	(3 << BYT_PULL_ASSIGN_SHIFT)
+#define BYT_PULL_ASSIGN_UP	(1 << BYT_PULL_ASSIGN_SHIFT)
+#define BYT_PULL_ASSIGN_DOWN	(2 << BYT_PULL_ASSIGN_SHIFT)
 #define BYT_PIN_MUX		0x07
 
 /* BYT_VAL_REG register bits */
@@ -321,6 +332,8 @@ static void byt_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 	spin_lock_irqsave(&vg->lock, flags);
 
 	for (i = 0; i < vg->chip.ngpio; i++) {
+		const char *pull_str = NULL;
+		const char *pull = NULL;
 		const char *label;
 		offs = vg->range->pins[i] * 16;
 		conf0 = readl(vg->reg_base + offs + BYT_CONF0_REG);
@@ -330,8 +343,32 @@ static void byt_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 		if (!label)
 			label = "Unrequested";
 
+		switch (conf0 & BYT_PULL_ASSIGN_MASK) {
+		case BYT_PULL_ASSIGN_UP:
+			pull = "up";
+			break;
+		case BYT_PULL_ASSIGN_DOWN:
+			pull = "down";
+			break;
+		}
+
+		switch (conf0 & BYT_PULL_STR_MASK) {
+		case BYT_PULL_STR_2K:
+			pull_str = "2k";
+			break;
+		case BYT_PULL_STR_10K:
+			pull_str = "10k";
+			break;
+		case BYT_PULL_STR_20K:
+			pull_str = "20k";
+			break;
+		case BYT_PULL_STR_40K:
+			pull_str = "40k";
+			break;
+		}
+
 		seq_printf(s,
-			   " gpio-%-3d (%-20.20s) %s %s %s pad-%-3d offset:0x%03x mux:%d %s%s%s\n",
+			   " gpio-%-3d (%-20.20s) %s %s %s pad-%-3d offset:0x%03x mux:%d %s%s%s",
 			   i,
 			   label,
 			   val & BYT_INPUT_EN ? "  " : "in",
@@ -339,9 +376,19 @@ static void byt_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 			   val & BYT_LEVEL ? "hi" : "lo",
 			   vg->range->pins[i], offs,
 			   conf0 & 0x7,
-			   conf0 & BYT_TRIG_NEG ? " fall" : "",
-			   conf0 & BYT_TRIG_POS ? " rise" : "",
-			   conf0 & BYT_TRIG_LVL ? " level" : "");
+			   conf0 & BYT_TRIG_NEG ? " fall" : "     ",
+			   conf0 & BYT_TRIG_POS ? " rise" : "     ",
+			   conf0 & BYT_TRIG_LVL ? " level" : "      ");
+
+		if (pull && pull_str)
+			seq_printf(s, " %-4s %-3s", pull, pull_str);
+		else
+			seq_puts(s, "          ");
+
+		if (conf0 & BYT_IODEN)
+			seq_puts(s, " open-drain");
+
+		seq_puts(s, "\n");
 	}
 	spin_unlock_irqrestore(&vg->lock, flags);
 }
@@ -527,12 +574,6 @@ static int byt_gpio_probe(struct platform_device *pdev)
 	gc->can_sleep = false;
 	gc->dev = dev;
 
-	ret = gpiochip_add(gc);
-	if (ret) {
-		dev_err(&pdev->dev, "failed adding byt-gpio chip\n");
-		return ret;
-	}
-
 	/* set up interrupts  */
 	irq_rc = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (irq_rc && irq_rc->start) {
@@ -550,6 +591,12 @@ static int byt_gpio_probe(struct platform_device *pdev)
 		irq_set_chained_handler(hwirq, byt_gpio_irq_handler);
 	}
 
+	ret = gpiochip_add(gc);
+	if (ret) {
+		dev_err(&pdev->dev, "failed adding byt-gpio chip\n");
+		return ret;
+	}
+
 	pm_runtime_enable(dev);
 
 	return 0;
@@ -572,6 +619,7 @@ static const struct dev_pm_ops byt_gpio_pm_ops = {
 
 static const struct acpi_device_id byt_gpio_acpi_match[] = {
 	{ "INT33B2", 0 },
+	{ "INT33FC", 0 },
 	{ }
 };
 MODULE_DEVICE_TABLE(acpi, byt_gpio_acpi_match);
diff --git a/drivers/pinctrl/pinctrl-exynos.c b/drivers/pinctrl/pinctrl-exynos.c
index 07c81306f2f..9609c23834c 100644
--- a/drivers/pinctrl/pinctrl-exynos.c
+++ b/drivers/pinctrl/pinctrl-exynos.c
@@ -718,6 +718,73 @@ struct samsung_pin_ctrl s5pv210_pin_ctrl[] = {
 	},
 };
 
+/* pin banks of exynos3250 pin-controller 0 */
+static struct samsung_pin_bank exynos3250_pin_banks0[] = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpa0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(6, 0x020, "gpa1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpb",  0x08),
+	EXYNOS_PIN_BANK_EINTG(5, 0x060, "gpc0", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(5, 0x080, "gpc1", 0x10),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0a0, "gpd0", 0x14),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0c0, "gpd1", 0x18),
+};
+
+/* pin banks of exynos3250 pin-controller 1 */
+static struct samsung_pin_bank exynos3250_pin_banks1[] = {
+	EXYNOS_PIN_BANK_EINTN(8, 0x120, "gpe0"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x140, "gpe1"),
+	EXYNOS_PIN_BANK_EINTN(3, 0x180, "gpe2"),
+	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpk0", 0x08),
+	EXYNOS_PIN_BANK_EINTG(7, 0x060, "gpk1", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(7, 0x080, "gpk2", 0x10),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0c0, "gpl0", 0x18),
+	EXYNOS_PIN_BANK_EINTG(8, 0x260, "gpm0", 0x24),
+	EXYNOS_PIN_BANK_EINTG(7, 0x280, "gpm1", 0x28),
+	EXYNOS_PIN_BANK_EINTG(5, 0x2a0, "gpm2", 0x2c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x2c0, "gpm3", 0x30),
+	EXYNOS_PIN_BANK_EINTG(8, 0x2e0, "gpm4", 0x34),
+	EXYNOS_PIN_BANK_EINTW(8, 0xc00, "gpx0", 0x00),
+	EXYNOS_PIN_BANK_EINTW(8, 0xc20, "gpx1", 0x04),
+	EXYNOS_PIN_BANK_EINTW(8, 0xc40, "gpx2", 0x08),
+	EXYNOS_PIN_BANK_EINTW(8, 0xc60, "gpx3", 0x0c),
+};
+
+/*
+ * Samsung pinctrl driver data for Exynos3250 SoC. Exynos3250 SoC includes
+ * two gpio/pin-mux/pinconfig controllers.
+ */
+struct samsung_pin_ctrl exynos3250_pin_ctrl[] = {
+	{
+		/* pin-controller instance 0 data */
+		.pin_banks	= exynos3250_pin_banks0,
+		.nr_banks	= ARRAY_SIZE(exynos3250_pin_banks0),
+		.geint_con	= EXYNOS_GPIO_ECON_OFFSET,
+		.geint_mask	= EXYNOS_GPIO_EMASK_OFFSET,
+		.geint_pend	= EXYNOS_GPIO_EPEND_OFFSET,
+		.svc		= EXYNOS_SVC_OFFSET,
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.label		= "exynos3250-gpio-ctrl0",
+	}, {
+		/* pin-controller instance 1 data */
+		.pin_banks	= exynos3250_pin_banks1,
+		.nr_banks	= ARRAY_SIZE(exynos3250_pin_banks1),
+		.geint_con	= EXYNOS_GPIO_ECON_OFFSET,
+		.geint_mask	= EXYNOS_GPIO_EMASK_OFFSET,
+		.geint_pend	= EXYNOS_GPIO_EPEND_OFFSET,
+		.weint_con	= EXYNOS_WKUP_ECON_OFFSET,
+		.weint_mask	= EXYNOS_WKUP_EMASK_OFFSET,
+		.weint_pend	= EXYNOS_WKUP_EPEND_OFFSET,
+		.svc		= EXYNOS_SVC_OFFSET,
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.eint_wkup_init = exynos_eint_wkup_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.label		= "exynos3250-gpio-ctrl1",
+	},
+};
+
 /* pin banks of exynos4210 pin-controller 0 */
 static struct samsung_pin_bank exynos4210_pin_banks0[] = {
 	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpa0", 0x00),
diff --git a/drivers/pinctrl/pinctrl-imx.c b/drivers/pinctrl/pinctrl-imx.c
index e118fb121e0..a24448e5d39 100644
--- a/drivers/pinctrl/pinctrl-imx.c
+++ b/drivers/pinctrl/pinctrl-imx.c
@@ -491,7 +491,7 @@ static int imx_pinctrl_parse_groups(struct device_node *np,
 			pin->mux_mode |= IOMUXC_CONFIG_SION;
 		pin->config = config & ~IMX_PAD_SION;
 
-		dev_dbg(info->dev, "%s: %d 0x%08lx", info->pins[pin_id].name,
+		dev_dbg(info->dev, "%s: 0x%x 0x%08lx", info->pins[pin_id].name,
 				pin->mux_mode, pin->config);
 	}
 
diff --git a/drivers/pinctrl/pinctrl-imx6sx.c b/drivers/pinctrl/pinctrl-imx6sx.c
new file mode 100644
index 00000000000..09758a56b9d
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-imx6sx.c
@@ -0,0 +1,407 @@
+/*
+ * Copyright (C) 2014 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 version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-imx.h"
+
+enum imx6sx_pads {
+	MX6Sx_PAD_RESERVE0 = 0,
+	MX6Sx_PAD_RESERVE1 = 1,
+	MX6Sx_PAD_RESERVE2 = 2,
+	MX6Sx_PAD_RESERVE3 = 3,
+	MX6Sx_PAD_RESERVE4 = 4,
+	MX6SX_PAD_GPIO1_IO00 = 5,
+	MX6SX_PAD_GPIO1_IO01 = 6,
+	MX6SX_PAD_GPIO1_IO02 = 7,
+	MX6SX_PAD_GPIO1_IO03 = 8,
+	MX6SX_PAD_GPIO1_IO04 = 9,
+	MX6SX_PAD_GPIO1_IO05 = 10,
+	MX6SX_PAD_GPIO1_IO06 = 11,
+	MX6SX_PAD_GPIO1_IO07 = 12,
+	MX6SX_PAD_GPIO1_IO08 = 13,
+	MX6SX_PAD_GPIO1_IO09 = 14,
+	MX6SX_PAD_GPIO1_IO10 = 15,
+	MX6SX_PAD_GPIO1_IO11 = 16,
+	MX6SX_PAD_GPIO1_IO12 = 17,
+	MX6SX_PAD_GPIO1_IO13 = 18,
+	MX6SX_PAD_CSI_DATA00 = 19,
+	MX6SX_PAD_CSI_DATA01 = 20,
+	MX6SX_PAD_CSI_DATA02 = 21,
+	MX6SX_PAD_CSI_DATA03 = 22,
+	MX6SX_PAD_CSI_DATA04 = 23,
+	MX6SX_PAD_CSI_DATA05 = 24,
+	MX6SX_PAD_CSI_DATA06 = 25,
+	MX6SX_PAD_CSI_DATA07 = 26,
+	MX6SX_PAD_CSI_HSYNC = 27,
+	MX6SX_PAD_CSI_MCLK = 28,
+	MX6SX_PAD_CSI_PIXCLK = 29,
+	MX6SX_PAD_CSI_VSYNC = 30,
+	MX6SX_PAD_ENET1_COL = 31,
+	MX6SX_PAD_ENET1_CRS = 32,
+	MX6SX_PAD_ENET1_MDC = 33,
+	MX6SX_PAD_ENET1_MDIO = 34,
+	MX6SX_PAD_ENET1_RX_CLK = 35,
+	MX6SX_PAD_ENET1_TX_CLK = 36,
+	MX6SX_PAD_ENET2_COL = 37,
+	MX6SX_PAD_ENET2_CRS = 38,
+	MX6SX_PAD_ENET2_RX_CLK = 39,
+	MX6SX_PAD_ENET2_TX_CLK = 40,
+	MX6SX_PAD_KEY_COL0 = 41,
+	MX6SX_PAD_KEY_COL1 = 42,
+	MX6SX_PAD_KEY_COL2 = 43,
+	MX6SX_PAD_KEY_COL3 = 44,
+	MX6SX_PAD_KEY_COL4 = 45,
+	MX6SX_PAD_KEY_ROW0 = 46,
+	MX6SX_PAD_KEY_ROW1 = 47,
+	MX6SX_PAD_KEY_ROW2 = 48,
+	MX6SX_PAD_KEY_ROW3 = 49,
+	MX6SX_PAD_KEY_ROW4 = 50,
+	MX6SX_PAD_LCD1_CLK = 51,
+	MX6SX_PAD_LCD1_DATA00 = 52,
+	MX6SX_PAD_LCD1_DATA01 = 53,
+	MX6SX_PAD_LCD1_DATA02 = 54,
+	MX6SX_PAD_LCD1_DATA03 = 55,
+	MX6SX_PAD_LCD1_DATA04 = 56,
+	MX6SX_PAD_LCD1_DATA05 = 57,
+	MX6SX_PAD_LCD1_DATA06 = 58,
+	MX6SX_PAD_LCD1_DATA07 = 59,
+	MX6SX_PAD_LCD1_DATA08 = 60,
+	MX6SX_PAD_LCD1_DATA09 = 61,
+	MX6SX_PAD_LCD1_DATA10 = 62,
+	MX6SX_PAD_LCD1_DATA11 = 63,
+	MX6SX_PAD_LCD1_DATA12 = 64,
+	MX6SX_PAD_LCD1_DATA13 = 65,
+	MX6SX_PAD_LCD1_DATA14 = 66,
+	MX6SX_PAD_LCD1_DATA15 = 67,
+	MX6SX_PAD_LCD1_DATA16 = 68,
+	MX6SX_PAD_LCD1_DATA17 = 69,
+	MX6SX_PAD_LCD1_DATA18 = 70,
+	MX6SX_PAD_LCD1_DATA19 = 71,
+	MX6SX_PAD_LCD1_DATA20 = 72,
+	MX6SX_PAD_LCD1_DATA21 = 73,
+	MX6SX_PAD_LCD1_DATA22 = 74,
+	MX6SX_PAD_LCD1_DATA23 = 75,
+	MX6SX_PAD_LCD1_ENABLE = 76,
+	MX6SX_PAD_LCD1_HSYNC = 77,
+	MX6SX_PAD_LCD1_RESET = 78,
+	MX6SX_PAD_LCD1_VSYNC = 79,
+	MX6SX_PAD_NAND_ALE = 80,
+	MX6SX_PAD_NAND_CE0_B = 81,
+	MX6SX_PAD_NAND_CE1_B = 82,
+	MX6SX_PAD_NAND_CLE = 83,
+	MX6SX_PAD_NAND_DATA00 = 84 ,
+	MX6SX_PAD_NAND_DATA01 = 85,
+	MX6SX_PAD_NAND_DATA02 = 86,
+	MX6SX_PAD_NAND_DATA03 = 87,
+	MX6SX_PAD_NAND_DATA04 = 88,
+	MX6SX_PAD_NAND_DATA05 = 89,
+	MX6SX_PAD_NAND_DATA06 = 90,
+	MX6SX_PAD_NAND_DATA07 = 91,
+	MX6SX_PAD_NAND_RE_B = 92,
+	MX6SX_PAD_NAND_READY_B = 93,
+	MX6SX_PAD_NAND_WE_B = 94,
+	MX6SX_PAD_NAND_WP_B = 95,
+	MX6SX_PAD_QSPI1A_DATA0 = 96,
+	MX6SX_PAD_QSPI1A_DATA1 = 97,
+	MX6SX_PAD_QSPI1A_DATA2 = 98,
+	MX6SX_PAD_QSPI1A_DATA3 = 99,
+	MX6SX_PAD_QSPI1A_DQS = 100,
+	MX6SX_PAD_QSPI1A_SCLK = 101,
+	MX6SX_PAD_QSPI1A_SS0_B = 102,
+	MX6SX_PAD_QSPI1A_SS1_B = 103,
+	MX6SX_PAD_QSPI1B_DATA0 = 104,
+	MX6SX_PAD_QSPI1B_DATA1 = 105,
+	MX6SX_PAD_QSPI1B_DATA2 = 106,
+	MX6SX_PAD_QSPI1B_DATA3 = 107,
+	MX6SX_PAD_QSPI1B_DQS = 108,
+	MX6SX_PAD_QSPI1B_SCLK = 109,
+	MX6SX_PAD_QSPI1B_SS0_B = 110,
+	MX6SX_PAD_QSPI1B_SS1_B = 111,
+	MX6SX_PAD_RGMII1_RD0 = 112,
+	MX6SX_PAD_RGMII1_RD1 = 113,
+	MX6SX_PAD_RGMII1_RD2 = 114,
+	MX6SX_PAD_RGMII1_RD3 = 115,
+	MX6SX_PAD_RGMII1_RX_CTL = 116,
+	MX6SX_PAD_RGMII1_RXC = 117,
+	MX6SX_PAD_RGMII1_TD0 = 118,
+	MX6SX_PAD_RGMII1_TD1 = 119,
+	MX6SX_PAD_RGMII1_TD2 = 120,
+	MX6SX_PAD_RGMII1_TD3 = 121,
+	MX6SX_PAD_RGMII1_TX_CTL = 122,
+	MX6SX_PAD_RGMII1_TXC = 123,
+	MX6SX_PAD_RGMII2_RD0 = 124,
+	MX6SX_PAD_RGMII2_RD1 = 125,
+	MX6SX_PAD_RGMII2_RD2 = 126,
+	MX6SX_PAD_RGMII2_RD3 = 127,
+	MX6SX_PAD_RGMII2_RX_CTL = 128,
+	MX6SX_PAD_RGMII2_RXC = 129,
+	MX6SX_PAD_RGMII2_TD0 = 130,
+	MX6SX_PAD_RGMII2_TD1 = 131,
+	MX6SX_PAD_RGMII2_TD2 = 132,
+	MX6SX_PAD_RGMII2_TD3 = 133,
+	MX6SX_PAD_RGMII2_TX_CTL = 134,
+	MX6SX_PAD_RGMII2_TXC = 135,
+	MX6SX_PAD_SD1_CLK = 136,
+	MX6SX_PAD_SD1_CMD = 137,
+	MX6SX_PAD_SD1_DATA0 = 138,
+	MX6SX_PAD_SD1_DATA1 = 139,
+	MX6SX_PAD_SD1_DATA2 = 140,
+	MX6SX_PAD_SD1_DATA3 = 141,
+	MX6SX_PAD_SD2_CLK = 142,
+	MX6SX_PAD_SD2_CMD = 143,
+	MX6SX_PAD_SD2_DATA0 = 144,
+	MX6SX_PAD_SD2_DATA1 = 145,
+	MX6SX_PAD_SD2_DATA2 = 146,
+	MX6SX_PAD_SD2_DATA3 = 147,
+	MX6SX_PAD_SD3_CLK = 148,
+	MX6SX_PAD_SD3_CMD = 149,
+	MX6SX_PAD_SD3_DATA0 = 150,
+	MX6SX_PAD_SD3_DATA1 = 151,
+	MX6SX_PAD_SD3_DATA2 = 152,
+	MX6SX_PAD_SD3_DATA3 = 153,
+	MX6SX_PAD_SD3_DATA4 = 154,
+	MX6SX_PAD_SD3_DATA5 = 155,
+	MX6SX_PAD_SD3_DATA6 = 156,
+	MX6SX_PAD_SD3_DATA7 = 157,
+	MX6SX_PAD_SD4_CLK = 158,
+	MX6SX_PAD_SD4_CMD = 159,
+	MX6SX_PAD_SD4_DATA0 = 160,
+	MX6SX_PAD_SD4_DATA1 = 161,
+	MX6SX_PAD_SD4_DATA2 = 162,
+	MX6SX_PAD_SD4_DATA3 = 163,
+	MX6SX_PAD_SD4_DATA4 = 164,
+	MX6SX_PAD_SD4_DATA5 = 165,
+	MX6SX_PAD_SD4_DATA6 = 166,
+	MX6SX_PAD_SD4_DATA7 = 167,
+	MX6SX_PAD_SD4_RESET_B = 168,
+	MX6SX_PAD_USB_H_DATA = 169,
+	MX6SX_PAD_USB_H_STROBE = 170,
+};
+
+/* Pad names for the pinmux subsystem */
+static const struct pinctrl_pin_desc imx6sx_pinctrl_pads[] = {
+	IMX_PINCTRL_PIN(MX6Sx_PAD_RESERVE0),
+	IMX_PINCTRL_PIN(MX6Sx_PAD_RESERVE1),
+	IMX_PINCTRL_PIN(MX6Sx_PAD_RESERVE2),
+	IMX_PINCTRL_PIN(MX6Sx_PAD_RESERVE3),
+	IMX_PINCTRL_PIN(MX6Sx_PAD_RESERVE4),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO00),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO01),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO02),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO03),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO04),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO05),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO06),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO07),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO08),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO09),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO10),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO11),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO12),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO13),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_DATA00),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_DATA01),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_DATA02),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_DATA03),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_DATA04),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_DATA05),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_DATA06),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_DATA07),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_HSYNC),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_MCLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_PIXCLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_VSYNC),
+	IMX_PINCTRL_PIN(MX6SX_PAD_ENET1_COL),
+	IMX_PINCTRL_PIN(MX6SX_PAD_ENET1_CRS),
+	IMX_PINCTRL_PIN(MX6SX_PAD_ENET1_MDC),
+	IMX_PINCTRL_PIN(MX6SX_PAD_ENET1_MDIO),
+	IMX_PINCTRL_PIN(MX6SX_PAD_ENET1_RX_CLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_ENET1_TX_CLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_ENET2_COL),
+	IMX_PINCTRL_PIN(MX6SX_PAD_ENET2_CRS),
+	IMX_PINCTRL_PIN(MX6SX_PAD_ENET2_RX_CLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_ENET2_TX_CLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_KEY_COL0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_KEY_COL1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_KEY_COL2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_KEY_COL3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_KEY_COL4),
+	IMX_PINCTRL_PIN(MX6SX_PAD_KEY_ROW0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_KEY_ROW1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_KEY_ROW2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_KEY_ROW3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_KEY_ROW4),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_CLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA00),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA01),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA02),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA03),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA04),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA05),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA06),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA07),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA08),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA09),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA10),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA11),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA12),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA13),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA14),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA15),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA16),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA17),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA18),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA19),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA20),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA21),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA22),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA23),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_ENABLE),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_HSYNC),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_RESET),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_VSYNC),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_ALE),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_CE0_B),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_CE1_B),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_CLE),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_DATA00),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_DATA01),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_DATA02),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_DATA03),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_DATA04),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_DATA05),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_DATA06),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_DATA07),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_RE_B),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_READY_B),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_WE_B),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_WP_B),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1A_DATA0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1A_DATA1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1A_DATA2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1A_DATA3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1A_DQS),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1A_SCLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1A_SS0_B),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1A_SS1_B),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1B_DATA0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1B_DATA1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1B_DATA2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1B_DATA3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1B_DQS),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1B_SCLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1B_SS0_B),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1B_SS1_B),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_RD0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_RD1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_RD2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_RD3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_RX_CTL),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_RXC),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_TD0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_TD1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_TD2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_TD3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_TX_CTL),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_TXC),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_RD0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_RD1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_RD2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_RD3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_RX_CTL),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_RXC),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_TD0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_TD1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_TD2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_TD3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_TX_CTL),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_TXC),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD1_CLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD1_CMD),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD1_DATA0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD1_DATA1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD1_DATA2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD1_DATA3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD2_CLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD2_CMD),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD2_DATA0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD2_DATA1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD2_DATA2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD2_DATA3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD3_CLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD3_CMD),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD3_DATA0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD3_DATA1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD3_DATA2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD3_DATA3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD3_DATA4),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD3_DATA5),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD3_DATA6),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD3_DATA7),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD4_CLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD4_CMD),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD4_DATA0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD4_DATA1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD4_DATA2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD4_DATA3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD4_DATA4),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD4_DATA5),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD4_DATA6),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD4_DATA7),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD4_RESET_B),
+	IMX_PINCTRL_PIN(MX6SX_PAD_USB_H_DATA),
+	IMX_PINCTRL_PIN(MX6SX_PAD_USB_H_STROBE),
+};
+
+static struct imx_pinctrl_soc_info imx6sx_pinctrl_info = {
+	.pins = imx6sx_pinctrl_pads,
+	.npins = ARRAY_SIZE(imx6sx_pinctrl_pads),
+};
+
+static struct of_device_id imx6sx_pinctrl_of_match[] = {
+	{ .compatible = "fsl,imx6sx-iomuxc", },
+	{ /* sentinel */ }
+};
+
+static int imx6sx_pinctrl_probe(struct platform_device *pdev)
+{
+	return imx_pinctrl_probe(pdev, &imx6sx_pinctrl_info);
+}
+
+static struct platform_driver imx6sx_pinctrl_driver = {
+	.driver = {
+		.name = "imx6sx-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = of_match_ptr(imx6sx_pinctrl_of_match),
+	},
+	.probe = imx6sx_pinctrl_probe,
+	.remove = imx_pinctrl_remove,
+};
+
+static int __init imx6sx_pinctrl_init(void)
+{
+	return platform_driver_register(&imx6sx_pinctrl_driver);
+}
+arch_initcall(imx6sx_pinctrl_init);
+
+static void __exit imx6sx_pinctrl_exit(void)
+{
+	platform_driver_unregister(&imx6sx_pinctrl_driver);
+}
+module_exit(imx6sx_pinctrl_exit);
+
+MODULE_AUTHOR("Anson Huang <Anson.Huang@freescale.com>");
+MODULE_DESCRIPTION("Freescale imx6sx pinctrl driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/pinctrl-ipq8064.c b/drivers/pinctrl/pinctrl-ipq8064.c
new file mode 100644
index 00000000000..acafea4c3a3
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-ipq8064.c
@@ -0,0 +1,653 @@
+/*
+ * 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 ipq8064_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, "SDC3_CLK"),
+	PINCTRL_PIN(70, "SDC3_CMD"),
+	PINCTRL_PIN(71, "SDC3_DATA"),
+};
+
+#define DECLARE_IPQ_GPIO_PINS(pin) static const unsigned int gpio##pin##_pins[] = { pin }
+DECLARE_IPQ_GPIO_PINS(0);
+DECLARE_IPQ_GPIO_PINS(1);
+DECLARE_IPQ_GPIO_PINS(2);
+DECLARE_IPQ_GPIO_PINS(3);
+DECLARE_IPQ_GPIO_PINS(4);
+DECLARE_IPQ_GPIO_PINS(5);
+DECLARE_IPQ_GPIO_PINS(6);
+DECLARE_IPQ_GPIO_PINS(7);
+DECLARE_IPQ_GPIO_PINS(8);
+DECLARE_IPQ_GPIO_PINS(9);
+DECLARE_IPQ_GPIO_PINS(10);
+DECLARE_IPQ_GPIO_PINS(11);
+DECLARE_IPQ_GPIO_PINS(12);
+DECLARE_IPQ_GPIO_PINS(13);
+DECLARE_IPQ_GPIO_PINS(14);
+DECLARE_IPQ_GPIO_PINS(15);
+DECLARE_IPQ_GPIO_PINS(16);
+DECLARE_IPQ_GPIO_PINS(17);
+DECLARE_IPQ_GPIO_PINS(18);
+DECLARE_IPQ_GPIO_PINS(19);
+DECLARE_IPQ_GPIO_PINS(20);
+DECLARE_IPQ_GPIO_PINS(21);
+DECLARE_IPQ_GPIO_PINS(22);
+DECLARE_IPQ_GPIO_PINS(23);
+DECLARE_IPQ_GPIO_PINS(24);
+DECLARE_IPQ_GPIO_PINS(25);
+DECLARE_IPQ_GPIO_PINS(26);
+DECLARE_IPQ_GPIO_PINS(27);
+DECLARE_IPQ_GPIO_PINS(28);
+DECLARE_IPQ_GPIO_PINS(29);
+DECLARE_IPQ_GPIO_PINS(30);
+DECLARE_IPQ_GPIO_PINS(31);
+DECLARE_IPQ_GPIO_PINS(32);
+DECLARE_IPQ_GPIO_PINS(33);
+DECLARE_IPQ_GPIO_PINS(34);
+DECLARE_IPQ_GPIO_PINS(35);
+DECLARE_IPQ_GPIO_PINS(36);
+DECLARE_IPQ_GPIO_PINS(37);
+DECLARE_IPQ_GPIO_PINS(38);
+DECLARE_IPQ_GPIO_PINS(39);
+DECLARE_IPQ_GPIO_PINS(40);
+DECLARE_IPQ_GPIO_PINS(41);
+DECLARE_IPQ_GPIO_PINS(42);
+DECLARE_IPQ_GPIO_PINS(43);
+DECLARE_IPQ_GPIO_PINS(44);
+DECLARE_IPQ_GPIO_PINS(45);
+DECLARE_IPQ_GPIO_PINS(46);
+DECLARE_IPQ_GPIO_PINS(47);
+DECLARE_IPQ_GPIO_PINS(48);
+DECLARE_IPQ_GPIO_PINS(49);
+DECLARE_IPQ_GPIO_PINS(50);
+DECLARE_IPQ_GPIO_PINS(51);
+DECLARE_IPQ_GPIO_PINS(52);
+DECLARE_IPQ_GPIO_PINS(53);
+DECLARE_IPQ_GPIO_PINS(54);
+DECLARE_IPQ_GPIO_PINS(55);
+DECLARE_IPQ_GPIO_PINS(56);
+DECLARE_IPQ_GPIO_PINS(57);
+DECLARE_IPQ_GPIO_PINS(58);
+DECLARE_IPQ_GPIO_PINS(59);
+DECLARE_IPQ_GPIO_PINS(60);
+DECLARE_IPQ_GPIO_PINS(61);
+DECLARE_IPQ_GPIO_PINS(62);
+DECLARE_IPQ_GPIO_PINS(63);
+DECLARE_IPQ_GPIO_PINS(64);
+DECLARE_IPQ_GPIO_PINS(65);
+DECLARE_IPQ_GPIO_PINS(66);
+DECLARE_IPQ_GPIO_PINS(67);
+DECLARE_IPQ_GPIO_PINS(68);
+
+static const unsigned int sdc3_clk_pins[] = { 69 };
+static const unsigned int sdc3_cmd_pins[] = { 70 };
+static const unsigned int sdc3_data_pins[] = { 71 };
+
+#define FUNCTION(fname)					\
+	[IPQ_MUX_##fname] = {				\
+		.name = #fname,				\
+		.groups = fname##_groups,		\
+		.ngroups = ARRAY_SIZE(fname##_groups),	\
+	}
+
+#define PINGROUP(id, f1, f2, f3, f4, f5, f6, f7, f8, f9, f10) \
+	{						\
+		.name = "gpio" #id,			\
+		.pins = gpio##id##_pins,		\
+		.npins = ARRAY_SIZE(gpio##id##_pins),	\
+		.funcs = (int[]){			\
+			IPQ_MUX_NA, /* gpio mode */	\
+			IPQ_MUX_##f1,			\
+			IPQ_MUX_##f2,			\
+			IPQ_MUX_##f3,			\
+			IPQ_MUX_##f4,			\
+			IPQ_MUX_##f5,			\
+			IPQ_MUX_##f6,			\
+			IPQ_MUX_##f7,			\
+			IPQ_MUX_##f8,			\
+			IPQ_MUX_##f9,			\
+			IPQ_MUX_##f10,			\
+		},					\
+		.nfuncs = 11,				\
+		.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 = 0x400 + 0x4 * 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 = 1,			\
+		.intr_target_bit = 0,			\
+		.intr_raw_status_bit = 3,		\
+		.intr_polarity_bit = 1,			\
+		.intr_detection_bit = 2,		\
+		.intr_detection_width = 1,		\
+	}
+
+#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_raw_status_bit = -1,              \
+		.intr_polarity_bit = -1,                \
+		.intr_detection_bit = -1,               \
+		.intr_detection_width = -1,             \
+	}
+
+enum ipq8064_functions {
+	IPQ_MUX_mdio,
+	IPQ_MUX_mi2s,
+	IPQ_MUX_pdm,
+	IPQ_MUX_ssbi,
+	IPQ_MUX_spmi,
+	IPQ_MUX_audio_pcm,
+	IPQ_MUX_gsbi1,
+	IPQ_MUX_gsbi2,
+	IPQ_MUX_gsbi4,
+	IPQ_MUX_gsbi5,
+	IPQ_MUX_gsbi5_spi_cs1,
+	IPQ_MUX_gsbi5_spi_cs2,
+	IPQ_MUX_gsbi5_spi_cs3,
+	IPQ_MUX_gsbi6,
+	IPQ_MUX_gsbi7,
+	IPQ_MUX_nss_spi,
+	IPQ_MUX_sdc1,
+	IPQ_MUX_spdif,
+	IPQ_MUX_nand,
+	IPQ_MUX_tsif1,
+	IPQ_MUX_tsif2,
+	IPQ_MUX_usb_fs_n,
+	IPQ_MUX_usb_fs,
+	IPQ_MUX_usb2_hsic,
+	IPQ_MUX_rgmii2,
+	IPQ_MUX_sata,
+	IPQ_MUX_pcie1_rst,
+	IPQ_MUX_pcie1_prsnt,
+	IPQ_MUX_pcie1_pwrflt,
+	IPQ_MUX_pcie1_pwren_n,
+	IPQ_MUX_pcie1_pwren,
+	IPQ_MUX_pcie1_clk_req,
+	IPQ_MUX_pcie2_rst,
+	IPQ_MUX_pcie2_prsnt,
+	IPQ_MUX_pcie2_pwrflt,
+	IPQ_MUX_pcie2_pwren_n,
+	IPQ_MUX_pcie2_pwren,
+	IPQ_MUX_pcie2_clk_req,
+	IPQ_MUX_pcie3_rst,
+	IPQ_MUX_pcie3_prsnt,
+	IPQ_MUX_pcie3_pwrflt,
+	IPQ_MUX_pcie3_pwren_n,
+	IPQ_MUX_pcie3_pwren,
+	IPQ_MUX_pcie3_clk_req,
+	IPQ_MUX_ps_hold,
+	IPQ_MUX_NA,
+};
+
+static const char * const mdio_groups[] = {
+	"gpio0", "gpio1", "gpio10", "gpio11",
+};
+
+static const char * const mi2s_groups[] = {
+	"gpio27", "gpio28", "gpio29", "gpio30", "gpio31", "gpio32",
+	"gpio33", "gpio55", "gpio56", "gpio57", "gpio58",
+};
+
+static const char * const pdm_groups[] = {
+	"gpio3", "gpio16", "gpio17", "gpio22", "gpio30", "gpio31",
+	"gpio34", "gpio35", "gpio52", "gpio55", "gpio56", "gpio58",
+	"gpio59",
+};
+
+static const char * const ssbi_groups[] = {
+	"gpio10", "gpio11",
+};
+
+static const char * const spmi_groups[] = {
+	"gpio10", "gpio11",
+};
+
+static const char * const audio_pcm_groups[] = {
+	"gpio14", "gpio15", "gpio16", "gpio17",
+};
+
+static const char * const gsbi1_groups[] = {
+	"gpio51", "gpio52", "gpio53", "gpio54",
+};
+
+static const char * const gsbi2_groups[] = {
+	"gpio22", "gpio23", "gpio24", "gpio25",
+};
+
+static const char * const gsbi4_groups[] = {
+	"gpio10", "gpio11", "gpio12", "gpio13",
+};
+
+static const char * const gsbi5_groups[] = {
+	"gpio18", "gpio19", "gpio20", "gpio21",
+};
+
+static const char * const gsbi5_spi_cs1_groups[] = {
+	"gpio6", "gpio61",
+};
+
+static const char * const gsbi5_spi_cs2_groups[] = {
+	"gpio7", "gpio62",
+};
+
+static const char * const gsbi5_spi_cs3_groups[] = {
+	"gpio2",
+};
+
+static const char * const gsbi6_groups[] = {
+	"gpio27", "gpio28", "gpio29", "gpio30", "gpio55", "gpio56",
+	"gpio57", "gpio58",
+};
+
+static const char * const gsbi7_groups[] = {
+	"gpio6", "gpio7", "gpio8", "gpio9",
+};
+
+static const char * const nss_spi_groups[] = {
+	"gpio14", "gpio15", "gpio16", "gpio17", "gpio55", "gpio56",
+	"gpio57", "gpio58",
+};
+
+static const char * const sdc1_groups[] = {
+	"gpio38", "gpio39", "gpio40", "gpio41", "gpio42", "gpio43",
+	"gpio44", "gpio45", "gpio46", "gpio47",
+};
+
+static const char * const spdif_groups[] = {
+	"gpio10", "gpio48",
+};
+
+static const char * const nand_groups[] = {
+	"gpio34", "gpio35", "gpio36", "gpio37", "gpio38", "gpio39",
+	"gpio40", "gpio41", "gpio42", "gpio43", "gpio44", "gpio45",
+	"gpio46", "gpio47",
+};
+
+static const char * const tsif1_groups[] = {
+	"gpio55", "gpio56", "gpio57", "gpio58",
+};
+
+static const char * const tsif2_groups[] = {
+	"gpio59", "gpio60", "gpio61", "gpio62",
+};
+
+static const char * const usb_fs_n_groups[] = {
+	"gpio6",
+};
+
+static const char * const usb_fs_groups[] = {
+	"gpio6", "gpio7", "gpio8",
+};
+
+static const char * const usb2_hsic_groups[] = {
+	"gpio67", "gpio68",
+};
+
+static const char * const rgmii2_groups[] = {
+	"gpio27", "gpio28", "gpio29", "gpio30", "gpio31", "gpio32",
+	"gpio51", "gpio52", "gpio59", "gpio60", "gpio61", "gpio62",
+};
+
+static const char * const sata_groups[] = {
+	"gpio10",
+};
+
+static const char * const pcie1_rst_groups[] = {
+	"gpio3",
+};
+
+static const char * const pcie1_prsnt_groups[] = {
+	"gpio3", "gpio11",
+};
+
+static const char * const pcie1_pwren_n_groups[] = {
+	"gpio4", "gpio12",
+};
+
+static const char * const pcie1_pwren_groups[] = {
+	"gpio4", "gpio12",
+};
+
+static const char * const pcie1_pwrflt_groups[] = {
+	"gpio5", "gpio13",
+};
+
+static const char * const pcie1_clk_req_groups[] = {
+	"gpio5",
+};
+
+static const char * const pcie2_rst_groups[] = {
+	"gpio48",
+};
+
+static const char * const pcie2_prsnt_groups[] = {
+	"gpio11", "gpio48",
+};
+
+static const char * const pcie2_pwren_n_groups[] = {
+	"gpio12", "gpio49",
+};
+
+static const char * const pcie2_pwren_groups[] = {
+	"gpio12", "gpio49",
+};
+
+static const char * const pcie2_pwrflt_groups[] = {
+	"gpio13", "gpio50",
+};
+
+static const char * const pcie2_clk_req_groups[] = {
+	"gpio50",
+};
+
+static const char * const pcie3_rst_groups[] = {
+	"gpio63",
+};
+
+static const char * const pcie3_prsnt_groups[] = {
+	"gpio11",
+};
+
+static const char * const pcie3_pwren_n_groups[] = {
+	"gpio12",
+};
+
+static const char * const pcie3_pwren_groups[] = {
+	"gpio12",
+};
+
+static const char * const pcie3_pwrflt_groups[] = {
+	"gpio13",
+};
+
+static const char * const pcie3_clk_req_groups[] = {
+	"gpio65",
+};
+
+static const char * const ps_hold_groups[] = {
+	"gpio26",
+};
+
+static const struct msm_function ipq8064_functions[] = {
+	FUNCTION(mdio),
+	FUNCTION(ssbi),
+	FUNCTION(spmi),
+	FUNCTION(mi2s),
+	FUNCTION(pdm),
+	FUNCTION(audio_pcm),
+	FUNCTION(gsbi1),
+	FUNCTION(gsbi2),
+	FUNCTION(gsbi4),
+	FUNCTION(gsbi5),
+	FUNCTION(gsbi5_spi_cs1),
+	FUNCTION(gsbi5_spi_cs2),
+	FUNCTION(gsbi5_spi_cs3),
+	FUNCTION(gsbi6),
+	FUNCTION(gsbi7),
+	FUNCTION(nss_spi),
+	FUNCTION(sdc1),
+	FUNCTION(spdif),
+	FUNCTION(nand),
+	FUNCTION(tsif1),
+	FUNCTION(tsif2),
+	FUNCTION(usb_fs_n),
+	FUNCTION(usb_fs),
+	FUNCTION(usb2_hsic),
+	FUNCTION(rgmii2),
+	FUNCTION(sata),
+	FUNCTION(pcie1_rst),
+	FUNCTION(pcie1_prsnt),
+	FUNCTION(pcie1_pwren_n),
+	FUNCTION(pcie1_pwren),
+	FUNCTION(pcie1_pwrflt),
+	FUNCTION(pcie1_clk_req),
+	FUNCTION(pcie2_rst),
+	FUNCTION(pcie2_prsnt),
+	FUNCTION(pcie2_pwren_n),
+	FUNCTION(pcie2_pwren),
+	FUNCTION(pcie2_pwrflt),
+	FUNCTION(pcie2_clk_req),
+	FUNCTION(pcie3_rst),
+	FUNCTION(pcie3_prsnt),
+	FUNCTION(pcie3_pwren_n),
+	FUNCTION(pcie3_pwren),
+	FUNCTION(pcie3_pwrflt),
+	FUNCTION(pcie3_clk_req),
+	FUNCTION(ps_hold),
+};
+
+static const struct msm_pingroup ipq8064_groups[] = {
+	PINGROUP(0, mdio, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(1, mdio, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(2, gsbi5_spi_cs3, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(3, pcie1_rst, pcie1_prsnt, pdm, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(4, pcie1_pwren_n, pcie1_pwren, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(5, pcie1_clk_req, pcie1_pwrflt, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(6, gsbi7, usb_fs, gsbi5_spi_cs1, usb_fs_n, NA, NA, NA, NA, NA, NA),
+	PINGROUP(7, gsbi7, usb_fs, gsbi5_spi_cs2, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(8, gsbi7, usb_fs, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(9, gsbi7, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(10, gsbi4, spdif, sata, ssbi, mdio, spmi, NA, NA, NA, NA),
+	PINGROUP(11, gsbi4, pcie2_prsnt, pcie1_prsnt, pcie3_prsnt, ssbi, mdio, spmi, NA, NA, NA),
+	PINGROUP(12, gsbi4, pcie2_pwren_n, pcie1_pwren_n, pcie3_pwren_n, pcie2_pwren, pcie1_pwren, pcie3_pwren, NA, NA, NA),
+	PINGROUP(13, gsbi4, pcie2_pwrflt, pcie1_pwrflt, pcie3_pwrflt, NA, NA, NA, NA, NA, NA),
+	PINGROUP(14, audio_pcm, nss_spi, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(15, audio_pcm, nss_spi, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(16, audio_pcm, nss_spi, pdm, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(17, audio_pcm, nss_spi, pdm, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(18, gsbi5, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(19, gsbi5, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(20, gsbi5, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(21, gsbi5, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(22, gsbi2, pdm, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(23, gsbi2, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(24, gsbi2, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(25, gsbi2, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(26, ps_hold, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(27, mi2s, rgmii2, gsbi6, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(28, mi2s, rgmii2, gsbi6, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(29, mi2s, rgmii2, gsbi6, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(30, mi2s, rgmii2, gsbi6, pdm, NA, NA, NA, NA, NA, NA),
+	PINGROUP(31, mi2s, rgmii2, pdm, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(32, mi2s, rgmii2, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(33, mi2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(34, nand, pdm, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(35, nand, pdm, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(36, nand, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(37, nand, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(38, nand, sdc1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(39, nand, sdc1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(40, nand, sdc1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(41, nand, sdc1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(42, nand, sdc1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(43, nand, sdc1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(44, nand, sdc1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(45, nand, sdc1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(46, nand, sdc1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(47, nand, sdc1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(48, pcie2_rst, spdif, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(49, pcie2_pwren_n, pcie2_pwren, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(50, pcie2_clk_req, pcie2_pwrflt, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(51, gsbi1, rgmii2, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(52, gsbi1, rgmii2, pdm, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(53, gsbi1, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(54, gsbi1, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(55, tsif1, mi2s, gsbi6, pdm, nss_spi, NA, NA, NA, NA, NA),
+	PINGROUP(56, tsif1, mi2s, gsbi6, pdm, nss_spi, NA, NA, NA, NA, NA),
+	PINGROUP(57, tsif1, mi2s, gsbi6, nss_spi, NA, NA, NA, NA, NA, NA),
+	PINGROUP(58, tsif1, mi2s, gsbi6, pdm, nss_spi, NA, NA, NA, NA, NA),
+	PINGROUP(59, tsif2, rgmii2, pdm, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(60, tsif2, rgmii2, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(61, tsif2, rgmii2, gsbi5_spi_cs1, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(62, tsif2, rgmii2, gsbi5_spi_cs2, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(63, pcie3_rst, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(64, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(65, pcie3_clk_req, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(66, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(67, usb2_hsic, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(68, usb2_hsic, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	SDC_PINGROUP(sdc3_clk, 0x204a, 14, 6),
+	SDC_PINGROUP(sdc3_cmd, 0x204a, 11, 3),
+	SDC_PINGROUP(sdc3_data, 0x204a, 9, 0),
+};
+
+#define NUM_GPIO_PINGROUPS 69
+
+static const struct msm_pinctrl_soc_data ipq8064_pinctrl = {
+	.pins = ipq8064_pins,
+	.npins = ARRAY_SIZE(ipq8064_pins),
+	.functions = ipq8064_functions,
+	.nfunctions = ARRAY_SIZE(ipq8064_functions),
+	.groups = ipq8064_groups,
+	.ngroups = ARRAY_SIZE(ipq8064_groups),
+	.ngpios = NUM_GPIO_PINGROUPS,
+};
+
+static int ipq8064_pinctrl_probe(struct platform_device *pdev)
+{
+	return msm_pinctrl_probe(pdev, &ipq8064_pinctrl);
+}
+
+static const struct of_device_id ipq8064_pinctrl_of_match[] = {
+	{ .compatible = "qcom,ipq8064-pinctrl", },
+	{ },
+};
+
+static struct platform_driver ipq8064_pinctrl_driver = {
+	.driver = {
+		.name = "ipq8064-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = ipq8064_pinctrl_of_match,
+	},
+	.probe = ipq8064_pinctrl_probe,
+	.remove = msm_pinctrl_remove,
+};
+
+static int __init ipq8064_pinctrl_init(void)
+{
+	return platform_driver_register(&ipq8064_pinctrl_driver);
+}
+arch_initcall(ipq8064_pinctrl_init);
+
+static void __exit ipq8064_pinctrl_exit(void)
+{
+	platform_driver_unregister(&ipq8064_pinctrl_driver);
+}
+module_exit(ipq8064_pinctrl_exit);
+
+MODULE_AUTHOR("Andy Gross <agross@codeaurora.org>");
+MODULE_DESCRIPTION("Qualcomm IPQ8064 pinctrl driver");
+MODULE_LICENSE("GPL v2");
+MODULE_DEVICE_TABLE(of, ipq8064_pinctrl_of_match);
diff --git a/drivers/pinctrl/pinctrl-lantiq.h b/drivers/pinctrl/pinctrl-lantiq.h
index 6d07f023853..c7cfad5527d 100644
--- a/drivers/pinctrl/pinctrl-lantiq.h
+++ b/drivers/pinctrl/pinctrl-lantiq.h
@@ -10,6 +10,7 @@
  */
 
 #ifndef __PINCTRL_LANTIQ_H
+#define __PINCTRL_LANTIQ_H
 
 #include <linux/clkdev.h>
 #include <linux/pinctrl/pinctrl.h>
diff --git a/drivers/pinctrl/pinctrl-msm.c b/drivers/pinctrl/pinctrl-msm.c
index e43fbce5659..df6dda4ce80 100644
--- a/drivers/pinctrl/pinctrl-msm.c
+++ b/drivers/pinctrl/pinctrl-msm.c
@@ -13,7 +13,6 @@
  */
 
 #include <linux/err.h>
-#include <linux/irqdomain.h>
 #include <linux/io.h>
 #include <linux/module.h>
 #include <linux/of.h>
@@ -26,8 +25,6 @@
 #include <linux/slab.h>
 #include <linux/gpio.h>
 #include <linux/interrupt.h>
-#include <linux/irq.h>
-#include <linux/irqchip/chained_irq.h>
 #include <linux/spinlock.h>
 
 #include "core.h"
@@ -41,7 +38,6 @@
  * struct msm_pinctrl - state for a pinctrl-msm device
  * @dev:            device handle.
  * @pctrl:          pinctrl handle.
- * @domain:         irqdomain handle.
  * @chip:           gpiochip handle.
  * @irq:            parent irq for the TLMM irq_chip.
  * @lock:           Spinlock to protect register resources as well
@@ -55,7 +51,6 @@
 struct msm_pinctrl {
 	struct device *dev;
 	struct pinctrl_dev *pctrl;
-	struct irq_domain *domain;
 	struct gpio_chip chip;
 	int irq;
 
@@ -68,6 +63,11 @@ struct msm_pinctrl {
 	void __iomem *regs;
 };
 
+static inline struct msm_pinctrl *to_msm_pinctrl(struct gpio_chip *gc)
+{
+	return container_of(gc, struct msm_pinctrl, chip);
+}
+
 static int msm_get_groups_count(struct pinctrl_dev *pctldev)
 {
 	struct msm_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctldev);
@@ -145,12 +145,12 @@ static int msm_pinmux_enable(struct pinctrl_dev *pctldev,
 	if (WARN_ON(g->mux_bit < 0))
 		return -EINVAL;
 
-	for (i = 0; i < ARRAY_SIZE(g->funcs); i++) {
+	for (i = 0; i < g->nfuncs; i++) {
 		if (g->funcs[i] == function)
 			break;
 	}
 
-	if (WARN_ON(i == ARRAY_SIZE(g->funcs)))
+	if (WARN_ON(i == g->nfuncs))
 		return -EINVAL;
 
 	spin_lock_irqsave(&pctrl->lock, flags);
@@ -480,13 +480,6 @@ static void msm_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 	spin_unlock_irqrestore(&pctrl->lock, flags);
 }
 
-static int msm_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
-{
-	struct msm_pinctrl *pctrl = container_of(chip, struct msm_pinctrl, chip);
-
-	return irq_find_mapping(pctrl->domain, offset);
-}
-
 static int msm_gpio_request(struct gpio_chip *chip, unsigned offset)
 {
 	int gpio = chip->base + offset;
@@ -556,7 +549,6 @@ static struct gpio_chip msm_gpio_template = {
 	.direction_output = msm_gpio_direction_output,
 	.get              = msm_gpio_get,
 	.set              = msm_gpio_set,
-	.to_irq           = msm_gpio_to_irq,
 	.request          = msm_gpio_request,
 	.free             = msm_gpio_free,
 	.dbg_show         = msm_gpio_dbg_show,
@@ -608,12 +600,12 @@ static void msm_gpio_update_dual_edge_pos(struct msm_pinctrl *pctrl,
 
 static void msm_gpio_irq_mask(struct irq_data *d)
 {
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct msm_pinctrl *pctrl = to_msm_pinctrl(gc);
 	const struct msm_pingroup *g;
-	struct msm_pinctrl *pctrl;
 	unsigned long flags;
 	u32 val;
 
-	pctrl = irq_data_get_irq_chip_data(d);
 	g = &pctrl->soc->groups[d->hwirq];
 
 	spin_lock_irqsave(&pctrl->lock, flags);
@@ -629,12 +621,12 @@ static void msm_gpio_irq_mask(struct irq_data *d)
 
 static void msm_gpio_irq_unmask(struct irq_data *d)
 {
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct msm_pinctrl *pctrl = to_msm_pinctrl(gc);
 	const struct msm_pingroup *g;
-	struct msm_pinctrl *pctrl;
 	unsigned long flags;
 	u32 val;
 
-	pctrl = irq_data_get_irq_chip_data(d);
 	g = &pctrl->soc->groups[d->hwirq];
 
 	spin_lock_irqsave(&pctrl->lock, flags);
@@ -654,12 +646,12 @@ static void msm_gpio_irq_unmask(struct irq_data *d)
 
 static void msm_gpio_irq_ack(struct irq_data *d)
 {
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct msm_pinctrl *pctrl = to_msm_pinctrl(gc);
 	const struct msm_pingroup *g;
-	struct msm_pinctrl *pctrl;
 	unsigned long flags;
 	u32 val;
 
-	pctrl = irq_data_get_irq_chip_data(d);
 	g = &pctrl->soc->groups[d->hwirq];
 
 	spin_lock_irqsave(&pctrl->lock, flags);
@@ -681,12 +673,12 @@ static void msm_gpio_irq_ack(struct irq_data *d)
 
 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);
+	struct msm_pinctrl *pctrl = to_msm_pinctrl(gc);
 	const struct msm_pingroup *g;
-	struct msm_pinctrl *pctrl;
 	unsigned long flags;
 	u32 val;
 
-	pctrl = irq_data_get_irq_chip_data(d);
 	g = &pctrl->soc->groups[d->hwirq];
 
 	spin_lock_irqsave(&pctrl->lock, flags);
@@ -775,11 +767,10 @@ static int msm_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 
 static int msm_gpio_irq_set_wake(struct irq_data *d, unsigned int on)
 {
-	struct msm_pinctrl *pctrl;
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct msm_pinctrl *pctrl = to_msm_pinctrl(gc);
 	unsigned long flags;
 
-	pctrl = irq_data_get_irq_chip_data(d);
-
 	spin_lock_irqsave(&pctrl->lock, flags);
 
 	irq_set_irq_wake(pctrl->irq, on);
@@ -789,25 +780,6 @@ static int msm_gpio_irq_set_wake(struct irq_data *d, unsigned int on)
 	return 0;
 }
 
-static int msm_gpio_irq_reqres(struct irq_data *d)
-{
-	struct msm_pinctrl *pctrl = irq_data_get_irq_chip_data(d);
-
-	if (gpio_lock_as_irq(&pctrl->chip, d->hwirq)) {
-		dev_err(pctrl->dev, "unable to lock HW IRQ %lu for IRQ\n",
-			d->hwirq);
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static void msm_gpio_irq_relres(struct irq_data *d)
-{
-	struct msm_pinctrl *pctrl = irq_data_get_irq_chip_data(d);
-
-	gpio_unlock_as_irq(&pctrl->chip, d->hwirq);
-}
-
 static struct irq_chip msm_gpio_irq_chip = {
 	.name           = "msmgpio",
 	.irq_mask       = msm_gpio_irq_mask,
@@ -815,14 +787,13 @@ static struct irq_chip msm_gpio_irq_chip = {
 	.irq_ack        = msm_gpio_irq_ack,
 	.irq_set_type   = msm_gpio_irq_set_type,
 	.irq_set_wake   = msm_gpio_irq_set_wake,
-	.irq_request_resources = msm_gpio_irq_reqres,
-	.irq_release_resources = msm_gpio_irq_relres,
 };
 
 static void msm_gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 {
+	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
 	const struct msm_pingroup *g;
-	struct msm_pinctrl *pctrl = irq_desc_get_handler_data(desc);
+	struct msm_pinctrl *pctrl = to_msm_pinctrl(gc);
 	struct irq_chip *chip = irq_get_chip(irq);
 	int irq_pin;
 	int handled = 0;
@@ -839,7 +810,7 @@ static void msm_gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 		g = &pctrl->soc->groups[i];
 		val = readl(pctrl->regs + g->intr_status_reg);
 		if (val & BIT(g->intr_status_bit)) {
-			irq_pin = irq_find_mapping(pctrl->domain, i);
+			irq_pin = irq_find_mapping(gc->irqdomain, i);
 			generic_handle_irq(irq_pin);
 			handled++;
 		}
@@ -852,19 +823,10 @@ static void msm_gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 	chained_irq_exit(chip, desc);
 }
 
-/*
- * This lock class tells lockdep that GPIO irqs are in a different
- * category than their parents, so it won't report false recursion.
- */
-static struct lock_class_key gpio_lock_class;
-
 static int msm_gpio_init(struct msm_pinctrl *pctrl)
 {
 	struct gpio_chip *chip;
-	int irq;
 	int ret;
-	int i;
-	int r;
 	unsigned ngpio = pctrl->soc->ngpios;
 
 	if (WARN_ON(ngpio > MAX_NR_GPIO))
@@ -890,23 +852,18 @@ static int msm_gpio_init(struct msm_pinctrl *pctrl)
 		return ret;
 	}
 
-	pctrl->domain = irq_domain_add_linear(pctrl->dev->of_node, chip->ngpio,
-					      &irq_domain_simple_ops, NULL);
-	if (!pctrl->domain) {
-		dev_err(pctrl->dev, "Failed to register irq domain\n");
-		r = gpiochip_remove(&pctrl->chip);
+	ret = gpiochip_irqchip_add(chip,
+				   &msm_gpio_irq_chip,
+				   0,
+				   handle_edge_irq,
+				   IRQ_TYPE_NONE);
+	if (ret) {
+		dev_err(pctrl->dev, "Failed to add irqchip to gpiochip\n");
 		return -ENOSYS;
 	}
 
-	for (i = 0; i < chip->ngpio; i++) {
-		irq = irq_create_mapping(pctrl->domain, i);
-		irq_set_lockdep_class(irq, &gpio_lock_class);
-		irq_set_chip_and_handler(irq, &msm_gpio_irq_chip, handle_edge_irq);
-		irq_set_chip_data(irq, pctrl);
-	}
-
-	irq_set_handler_data(pctrl->irq, pctrl);
-	irq_set_chained_handler(pctrl->irq, msm_gpio_irq_handler);
+	gpiochip_set_chained_irqchip(chip, &msm_gpio_irq_chip, pctrl->irq,
+				     msm_gpio_irq_handler);
 
 	return 0;
 }
@@ -974,8 +931,6 @@ int msm_pinctrl_remove(struct platform_device *pdev)
 		return ret;
 	}
 
-	irq_set_chained_handler(pctrl->irq, NULL);
-	irq_domain_remove(pctrl->domain);
 	pinctrl_unregister(pctrl->pctrl);
 
 	return 0;
diff --git a/drivers/pinctrl/pinctrl-msm.h b/drivers/pinctrl/pinctrl-msm.h
index 6e26f1b676d..7b2a227a590 100644
--- a/drivers/pinctrl/pinctrl-msm.h
+++ b/drivers/pinctrl/pinctrl-msm.h
@@ -65,7 +65,8 @@ struct msm_pingroup {
 	const unsigned *pins;
 	unsigned npins;
 
-	unsigned funcs[8];
+	unsigned *funcs;
+	unsigned nfuncs;
 
 	s16 ctl_reg;
 	s16 io_reg;
diff --git a/drivers/pinctrl/pinctrl-msm8x74.c b/drivers/pinctrl/pinctrl-msm8x74.c
index dde5529807a..418306911a6 100644
--- a/drivers/pinctrl/pinctrl-msm8x74.c
+++ b/drivers/pinctrl/pinctrl-msm8x74.c
@@ -341,7 +341,7 @@ static const unsigned int sdc2_data_pins[] = { 151 };
 		.name = "gpio" #id,			\
 		.pins = gpio##id##_pins,		\
 		.npins = ARRAY_SIZE(gpio##id##_pins),	\
-		.funcs = {				\
+		.funcs = (int[]){			\
 			MSM_MUX_NA, /* gpio mode */	\
 			MSM_MUX_##f1,			\
 			MSM_MUX_##f2,			\
@@ -351,6 +351,7 @@ static const unsigned int sdc2_data_pins[] = { 151 };
 			MSM_MUX_##f6,			\
 			MSM_MUX_##f7			\
 		},					\
+		.nfuncs = 8,				\
 		.ctl_reg = 0x1000 + 0x10 * id,		\
 		.io_reg = 0x1004 + 0x10 * id,		\
 		.intr_cfg_reg = 0x1008 + 0x10 * id,	\
@@ -401,169 +402,568 @@ static const unsigned int sdc2_data_pins[] = { 151 };
  * the pingroup table below.
  */
 enum msm8x74_functions {
+	MSM_MUX_cci_i2c0,
+	MSM_MUX_cci_i2c1,
+	MSM_MUX_blsp_i2c1,
 	MSM_MUX_blsp_i2c2,
+	MSM_MUX_blsp_i2c3,
+	MSM_MUX_blsp_i2c4,
+	MSM_MUX_blsp_i2c5,
 	MSM_MUX_blsp_i2c6,
+	MSM_MUX_blsp_i2c7,
+	MSM_MUX_blsp_i2c8,
+	MSM_MUX_blsp_i2c9,
+	MSM_MUX_blsp_i2c10,
 	MSM_MUX_blsp_i2c11,
+	MSM_MUX_blsp_i2c12,
 	MSM_MUX_blsp_spi1,
+	MSM_MUX_blsp_spi1_cs1,
+	MSM_MUX_blsp_spi1_cs2,
+	MSM_MUX_blsp_spi1_cs3,
+	MSM_MUX_blsp_spi2,
+	MSM_MUX_blsp_spi2_cs1,
+	MSM_MUX_blsp_spi2_cs2,
+	MSM_MUX_blsp_spi2_cs3,
+	MSM_MUX_blsp_spi3,
+	MSM_MUX_blsp_spi4,
+	MSM_MUX_blsp_spi5,
+	MSM_MUX_blsp_spi6,
+	MSM_MUX_blsp_spi7,
 	MSM_MUX_blsp_spi8,
+	MSM_MUX_blsp_spi9,
+	MSM_MUX_blsp_spi10,
+	MSM_MUX_blsp_spi10_cs1,
+	MSM_MUX_blsp_spi10_cs2,
+	MSM_MUX_blsp_spi10_cs3,
+	MSM_MUX_blsp_spi11,
+	MSM_MUX_blsp_spi12,
+	MSM_MUX_blsp_uart1,
 	MSM_MUX_blsp_uart2,
+	MSM_MUX_blsp_uart3,
+	MSM_MUX_blsp_uart4,
+	MSM_MUX_blsp_uart5,
+	MSM_MUX_blsp_uart6,
+	MSM_MUX_blsp_uart7,
 	MSM_MUX_blsp_uart8,
+	MSM_MUX_blsp_uart9,
+	MSM_MUX_blsp_uart10,
+	MSM_MUX_blsp_uart11,
+	MSM_MUX_blsp_uart12,
+	MSM_MUX_blsp_uim1,
+	MSM_MUX_blsp_uim2,
+	MSM_MUX_blsp_uim3,
+	MSM_MUX_blsp_uim4,
+	MSM_MUX_blsp_uim5,
+	MSM_MUX_blsp_uim6,
+	MSM_MUX_blsp_uim7,
+	MSM_MUX_blsp_uim8,
+	MSM_MUX_blsp_uim9,
+	MSM_MUX_blsp_uim10,
+	MSM_MUX_blsp_uim11,
+	MSM_MUX_blsp_uim12,
+	MSM_MUX_uim1,
+	MSM_MUX_uim2,
+	MSM_MUX_uim_batt_alarm,
+	MSM_MUX_sdc3,
+	MSM_MUX_sdc4,
+	MSM_MUX_gcc_gp_clk1,
+	MSM_MUX_gcc_gp_clk2,
+	MSM_MUX_gcc_gp_clk3,
+	MSM_MUX_qua_mi2s,
+	MSM_MUX_pri_mi2s,
+	MSM_MUX_spkr_mi2s,
+	MSM_MUX_ter_mi2s,
+	MSM_MUX_sec_mi2s,
+	MSM_MUX_hdmi_cec,
+	MSM_MUX_hdmi_ddc,
+	MSM_MUX_hdmi_hpd,
+	MSM_MUX_edp_hpd,
+	MSM_MUX_mdp_vsync,
+	MSM_MUX_cam_mclk0,
+	MSM_MUX_cam_mclk1,
+	MSM_MUX_cam_mclk2,
+	MSM_MUX_cam_mclk3,
+	MSM_MUX_cci_timer0,
+	MSM_MUX_cci_timer1,
+	MSM_MUX_cci_timer2,
+	MSM_MUX_cci_timer3,
+	MSM_MUX_cci_timer4,
+	MSM_MUX_cci_async_in0,
+	MSM_MUX_cci_async_in1,
+	MSM_MUX_cci_async_in2,
+	MSM_MUX_gp_pdm0,
+	MSM_MUX_gp_pdm1,
+	MSM_MUX_gp_pdm2,
+	MSM_MUX_gp0_clk,
+	MSM_MUX_gp1_clk,
+	MSM_MUX_gp_mn,
+	MSM_MUX_tsif1,
+	MSM_MUX_tsif2,
+	MSM_MUX_hsic,
+	MSM_MUX_grfc,
+	MSM_MUX_audio_ref_clk,
+	MSM_MUX_bt,
+	MSM_MUX_fm,
+	MSM_MUX_wlan,
 	MSM_MUX_slimbus,
 	MSM_MUX_NA,
 };
 
+static const char * const blsp_uart1_groups[] = {
+	"gpio0", "gpio1", "gpio2", "gpio3"
+};
+static const char * const blsp_uim1_groups[] = { "gpio0", "gpio1" };
+static const char * const blsp_i2c1_groups[] = { "gpio2", "gpio3" };
+static const char * const blsp_spi1_groups[] = {
+	"gpio0", "gpio1", "gpio2", "gpio3"
+};
+static const char * const blsp_spi1_cs1_groups[] = { "gpio8" };
+static const char * const blsp_spi1_cs2_groups[] = { "gpio9", "gpio11" };
+static const char * const blsp_spi1_cs3_groups[] = { "gpio10" };
+
+static const char * const blsp_uart2_groups[] = {
+	"gpio4", "gpio5", "gpio6", "gpio7"
+};
+static const char * const blsp_uim2_groups[] = { "gpio4", "gpio5" };
 static const char * const blsp_i2c2_groups[] = { "gpio6", "gpio7" };
+static const char * const blsp_spi2_groups[] = {
+	"gpio4", "gpio5", "gpio6", "gpio7"
+};
+static const char * const blsp_spi2_cs1_groups[] = { "gpio53", "gpio62" };
+static const char * const blsp_spi2_cs2_groups[] = { "gpio54", "gpio63" };
+static const char * const blsp_spi2_cs3_groups[] = { "gpio66" };
+
+static const char * const blsp_uart3_groups[] = {
+	"gpio8", "gpio9", "gpio10", "gpio11"
+};
+static const char * const blsp_uim3_groups[] = { "gpio8", "gpio9" };
+static const char * const blsp_i2c3_groups[] = { "gpio10", "gpio11" };
+static const char * const blsp_spi3_groups[] = {
+	"gpio8", "gpio9", "gpio10", "gpio11"
+};
+
+static const char * const cci_i2c0_groups[] = { "gpio19", "gpio20" };
+static const char * const cci_i2c1_groups[] = { "gpio21", "gpio22" };
+
+static const char * const blsp_uart4_groups[] = {
+	"gpio19", "gpio20", "gpio21", "gpio22"
+};
+static const char * const blsp_uim4_groups[] = { "gpio19", "gpio20" };
+static const char * const blsp_i2c4_groups[] = { "gpio21", "gpio22" };
+static const char * const blsp_spi4_groups[] = {
+	"gpio19", "gpio20", "gpio21", "gpio22"
+};
+
+static const char * const blsp_uart5_groups[] = {
+	"gpio23", "gpio24", "gpio25", "gpio26"
+};
+static const char * const blsp_uim5_groups[] = { "gpio23", "gpio24" };
+static const char * const blsp_i2c5_groups[] = { "gpio25", "gpio26" };
+static const char * const blsp_spi5_groups[] = {
+	"gpio23", "gpio24", "gpio25", "gpio26"
+};
+
+static const char * const blsp_uart6_groups[] = {
+	"gpio27", "gpio28", "gpio29", "gpio30"
+};
+static const char * const blsp_uim6_groups[] = { "gpio27", "gpio28" };
 static const char * const blsp_i2c6_groups[] = { "gpio29", "gpio30" };
-static const char * const blsp_i2c11_groups[] = { "gpio83", "gpio84" };
-static const char * const blsp_spi1_groups[] = { "gpio0", "gpio1", "gpio2", "gpio3" };
+static const char * const blsp_spi6_groups[] = {
+	"gpio27", "gpio28", "gpio29", "gpio30"
+};
+
+static const char * const blsp_uart7_groups[] = {
+	"gpio41", "gpio42", "gpio43", "gpio44"
+};
+static const char * const blsp_uim7_groups[] = { "gpio41", "gpio42" };
+static const char * const blsp_i2c7_groups[] = { "gpio43", "gpio44" };
+static const char * const blsp_spi7_groups[] = {
+	"gpio41", "gpio42", "gpio43", "gpio44"
+};
+
+static const char * const blsp_uart8_groups[] = {
+	"gpio45", "gpio46", "gpio47", "gpio48"
+};
+static const char * const blsp_uim8_groups[] = { "gpio45", "gpio46" };
+static const char * const blsp_i2c8_groups[] = { "gpio47", "gpio48" };
 static const char * const blsp_spi8_groups[] = {
 	"gpio45", "gpio46", "gpio47", "gpio48"
 };
-static const char * const blsp_uart2_groups[] = { "gpio4", "gpio5" };
-static const char * const blsp_uart8_groups[] = { "gpio45", "gpio46" };
+
+static const char * const blsp_uart9_groups[] = {
+	"gpio49", "gpio50", "gpio51", "gpio52"
+};
+static const char * const blsp_uim9_groups[] = { "gpio49", "gpio50" };
+static const char * const blsp_i2c9_groups[] = { "gpio51", "gpio52" };
+static const char * const blsp_spi9_groups[] = {
+	"gpio49", "gpio50", "gpio51", "gpio52"
+};
+
+static const char * const blsp_uart10_groups[] = {
+	"gpio53", "gpio54", "gpio55", "gpio56"
+};
+static const char * const blsp_uim10_groups[] = { "gpio53", "gpio54" };
+static const char * const blsp_i2c10_groups[] = { "gpio55", "gpio56" };
+static const char * const blsp_spi10_groups[] = {
+	"gpio53", "gpio54", "gpio55", "gpio56"
+};
+static const char * const blsp_spi10_cs1_groups[] = { "gpio47", "gpio67" };
+static const char * const blsp_spi10_cs2_groups[] = { "gpio48", "gpio68" };
+static const char * const blsp_spi10_cs3_groups[] = { "gpio90" };
+
+static const char * const blsp_uart11_groups[] = {
+	"gpio81", "gpio82", "gpio83", "gpio84"
+};
+static const char * const blsp_uim11_groups[] = { "gpio81", "gpio82" };
+static const char * const blsp_i2c11_groups[] = { "gpio83", "gpio84" };
+static const char * const blsp_spi11_groups[] = {
+	"gpio81", "gpio82", "gpio83", "gpio84"
+};
+
+static const char * const blsp_uart12_groups[] = {
+	"gpio85", "gpio86", "gpio87", "gpio88"
+};
+static const char * const blsp_uim12_groups[] = { "gpio85", "gpio86" };
+static const char * const blsp_i2c12_groups[] = { "gpio87", "gpio88" };
+static const char * const blsp_spi12_groups[] = {
+	"gpio85", "gpio86", "gpio87", "gpio88"
+};
+
+static const char * const uim1_groups[] = {
+	"gpio97", "gpio98", "gpio99", "gpio100"
+};
+
+static const char * const uim2_groups[] = {
+	"gpio49", "gpio50", "gpio51", "gpio52"
+};
+
+static const char * const uim_batt_alarm_groups[] = { "gpio101" };
+
+static const char * const sdc3_groups[] = {
+	"gpio35", "gpio36", "gpio37", "gpio38", "gpio39", "gpio40"
+};
+
+static const char * const sdc4_groups[] = {
+	"gpio91", "gpio92", "gpio93", "gpio94", "gpio95", "gpio96"
+};
+
+static const char * const gp0_clk_groups[] = { "gpio26" };
+static const char * const gp1_clk_groups[] = { "gpio27", "gpio57", "gpio78" };
+static const char * const gp_mn_groups[] = { "gpio29" };
+static const char * const gcc_gp_clk1_groups[] = { "gpio57", "gpio78" };
+static const char * const gcc_gp_clk2_groups[] = { "gpio58", "gpio81" };
+static const char * const gcc_gp_clk3_groups[] = { "gpio59", "gpio82" };
+
+static const char * const qua_mi2s_groups[] = {
+	"gpio57", "gpio58", "gpio59", "gpio60", "gpio61", "gpio62", "gpio63",
+};
+
+static const char * const pri_mi2s_groups[] = {
+	"gpio64", "gpio65", "gpio66", "gpio67", "gpio68"
+};
+
+static const char * const spkr_mi2s_groups[] = {
+	"gpio69", "gpio70", "gpio71", "gpio72"
+};
+
+static const char * const ter_mi2s_groups[] = {
+	"gpio73", "gpio74", "gpio75", "gpio76", "gpio77"
+};
+
+static const char * const sec_mi2s_groups[] = {
+	"gpio78", "gpio79", "gpio80", "gpio81", "gpio82"
+};
+
+static const char * const hdmi_cec_groups[] = { "gpio31" };
+static const char * const hdmi_ddc_groups[] = { "gpio32", "gpio33" };
+static const char * const hdmi_hpd_groups[] = { "gpio34" };
+static const char * const edp_hpd_groups[] = { "gpio102" };
+
+static const char * const mdp_vsync_groups[] = { "gpio12", "gpio13", "gpio14" };
+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_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[] = { "gpio27" };
+static const char * const cci_async_in0_groups[] = { "gpio28" };
+static const char * const cci_async_in1_groups[] = { "gpio26" };
+static const char * const cci_async_in2_groups[] = { "gpio27" };
+
+static const char * const gp_pdm0_groups[] = { "gpio54", "gpio68" };
+static const char * const gp_pdm1_groups[] = { "gpio74", "gpio86" };
+static const char * const gp_pdm2_groups[] = { "gpio63", "gpio79" };
+
+static const char * const tsif1_groups[] = {
+	"gpio89", "gpio90", "gpio91", "gpio92"
+};
+
+static const char * const tsif2_groups[] = {
+	"gpio93", "gpio94", "gpio95", "gpio96"
+};
+
+static const char * const hsic_groups[] = { "gpio144", "gpio145" };
+static const char * const grfc_groups[] = {
+	"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", "gpio136", "gpio137", "gpio141", "gpio143"
+};
+
+static const char * const audio_ref_clk_groups[] = { "gpio69" };
+
+static const char * const bt_groups[] = { "gpio35", "gpio43", "gpio44" };
+
+static const char * const fm_groups[] = { "gpio41", "gpio42" };
+
+static const char * const wlan_groups[] = {
+	"gpio36", "gpio37", "gpio38", "gpio39", "gpio40"
+};
+
 static const char * const slimbus_groups[] = { "gpio70", "gpio71" };
 
 static const struct msm_function msm8x74_functions[] = {
+	FUNCTION(cci_i2c0),
+	FUNCTION(cci_i2c1),
+	FUNCTION(uim1),
+	FUNCTION(uim2),
+	FUNCTION(uim_batt_alarm),
+	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(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_spi2_cs1),
+	FUNCTION(blsp_spi2_cs2),
+	FUNCTION(blsp_spi2_cs3),
+	FUNCTION(blsp_spi3),
+	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(sdc3),
+	FUNCTION(sdc4),
+	FUNCTION(gcc_gp_clk1),
+	FUNCTION(gcc_gp_clk2),
+	FUNCTION(gcc_gp_clk3),
+	FUNCTION(qua_mi2s),
+	FUNCTION(pri_mi2s),
+	FUNCTION(spkr_mi2s),
+	FUNCTION(ter_mi2s),
+	FUNCTION(sec_mi2s),
+	FUNCTION(mdp_vsync),
+	FUNCTION(cam_mclk0),
+	FUNCTION(cam_mclk1),
+	FUNCTION(cam_mclk2),
+	FUNCTION(cam_mclk3),
+	FUNCTION(cci_timer0),
+	FUNCTION(cci_timer1),
+	FUNCTION(cci_timer2),
+	FUNCTION(cci_timer3),
+	FUNCTION(cci_timer4),
+	FUNCTION(cci_async_in0),
+	FUNCTION(cci_async_in1),
+	FUNCTION(cci_async_in2),
+	FUNCTION(hdmi_cec),
+	FUNCTION(hdmi_ddc),
+	FUNCTION(hdmi_hpd),
+	FUNCTION(edp_hpd),
+	FUNCTION(gp_pdm0),
+	FUNCTION(gp_pdm1),
+	FUNCTION(gp_pdm2),
+	FUNCTION(gp0_clk),
+	FUNCTION(gp1_clk),
+	FUNCTION(gp_mn),
+	FUNCTION(tsif1),
+	FUNCTION(tsif2),
+	FUNCTION(hsic),
+	FUNCTION(grfc),
+	FUNCTION(audio_ref_clk),
+	FUNCTION(bt),
+	FUNCTION(fm),
+	FUNCTION(wlan),
 	FUNCTION(slimbus),
 };
 
 static const struct msm_pingroup msm8x74_groups[] = {
-	PINGROUP(0,   blsp_spi1, NA, NA, NA, NA, NA, NA),
-	PINGROUP(1,   blsp_spi1, NA, NA, NA, NA, NA, NA),
-	PINGROUP(2,   blsp_spi1, NA, NA, NA, NA, NA, NA),
-	PINGROUP(3,   blsp_spi1, NA, NA, NA, NA, NA, NA),
-	PINGROUP(4,   NA, blsp_uart2, NA, NA, NA, NA, NA),
-	PINGROUP(5,   NA, blsp_uart2, NA, NA, NA, NA, NA),
-	PINGROUP(6,   NA, NA, blsp_i2c2, NA, NA, NA, NA),
-	PINGROUP(7,   NA, NA, blsp_i2c2, NA, NA, NA, NA),
-	PINGROUP(8,   NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(9,   NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(10,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(11,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(12,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(13,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(14,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(15,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(16,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(17,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(18,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(19,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(20,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(21,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(22,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(23,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(24,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(25,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(26,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(27,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(28,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(29,  NA, NA, blsp_i2c6, NA, NA, NA, NA),
-	PINGROUP(30,  NA, NA, blsp_i2c6, NA, NA, NA, NA),
-	PINGROUP(31,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(32,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(33,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(34,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(35,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(36,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(37,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(38,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(39,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(40,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(41,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(42,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(43,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(44,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(45,  blsp_spi8, blsp_uart8, NA, NA, NA, NA, NA),
-	PINGROUP(46,  blsp_spi8, blsp_uart8, NA, NA, NA, NA, NA),
-	PINGROUP(47,  blsp_spi8, NA, NA, NA, NA, NA, NA),
-	PINGROUP(48,  blsp_spi8, NA, NA, NA, NA, NA, NA),
-	PINGROUP(49,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(50,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(51,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(52,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(53,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(54,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(55,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(56,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(57,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(58,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(59,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(60,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(61,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(62,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(63,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(64,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(65,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(66,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(67,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(68,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(69,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(70,  slimbus, NA, NA, NA, NA, NA, NA),
-	PINGROUP(71,  slimbus, NA, NA, NA, NA, NA, NA),
-	PINGROUP(72,  NA, NA, 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,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(76,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(77,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(78,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(79,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(80,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(81,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(82,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(83,  NA, NA, blsp_i2c11, NA, NA, NA, NA),
-	PINGROUP(84,  NA, NA, blsp_i2c11, NA, NA, NA, NA),
-	PINGROUP(85,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(86,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(87,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(88,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(89,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(90,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(91,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(92,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(93,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(94,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(95,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(96,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(97,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(98,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(99,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(100, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(101, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(102, NA, NA, NA, NA, NA, NA, NA),
+	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, blsp_spi1_cs1, NA, NA, NA),
+	PINGROUP(9,   blsp_spi3, blsp_uart3, blsp_uim3, blsp_spi1_cs2, NA, NA, NA),
+	PINGROUP(10,  blsp_spi3, blsp_uart3, blsp_i2c3, blsp_spi1_cs3, NA, NA, NA),
+	PINGROUP(11,  blsp_spi3, blsp_uart3, blsp_i2c3, blsp_spi1_cs2, 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, blsp_spi4, blsp_uart4, blsp_uim4, NA, NA, NA),
+	PINGROUP(20,  cci_i2c0, blsp_spi4, blsp_uart4, blsp_uim4, NA, NA, NA),
+	PINGROUP(21,  cci_i2c1, blsp_spi4, blsp_uart4, blsp_i2c4, NA, NA, NA),
+	PINGROUP(22,  cci_i2c1, blsp_spi4, blsp_uart4, blsp_i2c4, NA, NA, NA),
+	PINGROUP(23,  cci_timer0, blsp_spi5, blsp_uart5, blsp_uim5, NA, NA, NA),
+	PINGROUP(24,  cci_timer1, blsp_spi5, blsp_uart5, blsp_uim5, NA, NA, NA),
+	PINGROUP(25,  cci_timer2, blsp_spi5, blsp_uart5, blsp_i2c5, NA, NA, NA),
+	PINGROUP(26,  cci_timer3, cci_async_in1, blsp_spi5, blsp_uart5, blsp_i2c5, gp0_clk, NA),
+	PINGROUP(27,  cci_timer4, cci_async_in2, blsp_spi6, blsp_uart6, blsp_i2c6, gp1_clk, NA),
+	PINGROUP(28,  cci_async_in0, blsp_spi6, blsp_uart6, blsp_uim6, NA, NA, NA),
+	PINGROUP(29,  blsp_spi6, blsp_uart6, blsp_i2c6, gp_mn, NA, NA, NA),
+	PINGROUP(30,  blsp_spi6, blsp_uart6, blsp_i2c6, 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, NA, NA, NA, NA, NA),
+	PINGROUP(35,  bt, sdc3, NA, NA, NA, NA, NA),
+	PINGROUP(36,  wlan, sdc3, NA, NA, NA, NA, NA),
+	PINGROUP(37,  wlan, sdc3, NA, NA, NA, NA, NA),
+	PINGROUP(38,  wlan, sdc3, NA, NA, NA, NA, NA),
+	PINGROUP(39,  wlan, sdc3, NA, NA, NA, NA, NA),
+	PINGROUP(40,  wlan, sdc3, NA, NA, NA, NA, NA),
+	PINGROUP(41,  fm, blsp_spi7, blsp_uart7, blsp_uim7, NA, NA, NA),
+	PINGROUP(42,  fm, blsp_spi7, blsp_uart7, blsp_uim7, NA, NA, NA),
+	PINGROUP(43,  bt, blsp_spi7, blsp_uart7, blsp_i2c7, NA, NA, NA),
+	PINGROUP(44,  bt, blsp_spi7, blsp_uart7, blsp_i2c7, NA, NA, NA),
+	PINGROUP(45,  blsp_spi8, blsp_uart8, blsp_uim8, NA, NA, NA, NA),
+	PINGROUP(46,  blsp_spi8, blsp_uart8, blsp_uim8, NA, NA, NA, NA),
+	PINGROUP(47,  blsp_spi8, blsp_uart8, blsp_i2c8, blsp_spi10_cs1, NA, NA, NA),
+	PINGROUP(48,  blsp_spi8, blsp_uart8, blsp_i2c8, blsp_spi10_cs2, NA, NA, NA),
+	PINGROUP(49,  uim2, blsp_spi9, blsp_uart9, blsp_uim9, NA, NA, NA),
+	PINGROUP(50,  uim2, blsp_spi9, blsp_uart9, blsp_uim9, NA, NA, NA),
+	PINGROUP(51,  uim2, blsp_spi9, blsp_uart9, blsp_i2c9, NA, NA, NA),
+	PINGROUP(52,  uim2, blsp_spi9, blsp_uart9, blsp_i2c9, NA, NA, NA),
+	PINGROUP(53,  blsp_spi10, blsp_uart10, blsp_uim10, blsp_spi2_cs1, NA, NA, NA),
+	PINGROUP(54,  blsp_spi10, blsp_uart10, blsp_uim10, blsp_spi2_cs2, gp_pdm0, NA, NA),
+	PINGROUP(55,  blsp_spi10, blsp_uart10, blsp_i2c10, NA, NA, NA, NA),
+	PINGROUP(56,  blsp_spi10, blsp_uart10, blsp_i2c10, NA, NA, NA, NA),
+	PINGROUP(57,  qua_mi2s, gcc_gp_clk1, NA, NA, NA, NA, NA),
+	PINGROUP(58,  qua_mi2s, gcc_gp_clk2, NA, NA, NA, NA, NA),
+	PINGROUP(59,  qua_mi2s, gcc_gp_clk3, NA, NA, NA, NA, NA),
+	PINGROUP(60,  qua_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(61,  qua_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(62,  qua_mi2s, blsp_spi2_cs1, NA, NA, NA, NA, NA),
+	PINGROUP(63,  qua_mi2s, blsp_spi2_cs2, gp_pdm2, NA, NA, NA, NA),
+	PINGROUP(64,  pri_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(65,  pri_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(66,  pri_mi2s, blsp_spi2_cs3, NA, NA, NA, NA, NA),
+	PINGROUP(67,  pri_mi2s, blsp_spi10_cs1, NA, NA, NA, NA, NA),
+	PINGROUP(68,  pri_mi2s, blsp_spi10_cs2, gp_pdm0, NA, NA, NA, NA),
+	PINGROUP(69,  spkr_mi2s, audio_ref_clk, NA, NA, NA, NA, NA),
+	PINGROUP(70,  slimbus, spkr_mi2s, NA, NA, NA, NA, NA),
+	PINGROUP(71,  slimbus, spkr_mi2s, NA, NA, NA, NA, NA),
+	PINGROUP(72,  spkr_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(73,  ter_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(74,  ter_mi2s, gp_pdm1, NA, NA, NA, NA, NA),
+	PINGROUP(75,  ter_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(76,  ter_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(77,  ter_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(78,  sec_mi2s, gcc_gp_clk1, NA, NA, NA, NA, NA),
+	PINGROUP(79,  sec_mi2s, gp_pdm2, NA, NA, NA, NA, NA),
+	PINGROUP(80,  sec_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(81,  sec_mi2s, blsp_spi11, blsp_uart11, blsp_uim11, gcc_gp_clk2, NA, NA),
+	PINGROUP(82,  sec_mi2s, blsp_spi11, blsp_uart11, blsp_uim11, gcc_gp_clk3, NA, NA),
+	PINGROUP(83,  blsp_spi11, blsp_uart11, blsp_i2c11, NA, NA, NA, NA),
+	PINGROUP(84,  blsp_spi11, blsp_uart11, blsp_i2c11, NA, NA, NA, NA),
+	PINGROUP(85,  blsp_spi12, blsp_uart12, blsp_uim12, NA, NA, NA, NA),
+	PINGROUP(86,  blsp_spi12, blsp_uart12, blsp_uim12, gp_pdm1, NA, NA, NA),
+	PINGROUP(87,  blsp_spi12, blsp_uart12, blsp_i2c12, NA, NA, NA, NA),
+	PINGROUP(88,  blsp_spi12, blsp_uart12, blsp_i2c12, NA, NA, NA, NA),
+	PINGROUP(89,  tsif1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(90,  tsif1, blsp_spi10_cs3, NA, NA, NA, NA, NA),
+	PINGROUP(91,  tsif1, sdc4, NA, NA, NA, NA, NA),
+	PINGROUP(92,  tsif1, sdc4, NA, NA, NA, NA, NA),
+	PINGROUP(93,  tsif2, sdc4, NA, NA, NA, NA, NA),
+	PINGROUP(94,  tsif2, sdc4, NA, NA, NA, NA, NA),
+	PINGROUP(95,  tsif2, sdc4, NA, NA, NA, NA, NA),
+	PINGROUP(96,  tsif2, sdc4, NA, NA, NA, NA, NA),
+	PINGROUP(97,  uim1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(98,  uim1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(99,  uim1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(100, uim1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(101, uim_batt_alarm, NA, NA, NA, NA, NA, NA),
+	PINGROUP(102, edp_hpd, NA, NA, NA, NA, NA, NA),
 	PINGROUP(103, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(104, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(105, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(106, NA, 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, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(111, NA, 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, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(117, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(118, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(119, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(120, NA, 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, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(124, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(125, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(126, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(127, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(128, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(104, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(105, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(106, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(107, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(108, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(109, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(110, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(111, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(112, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(113, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(114, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(115, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(116, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(117, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(118, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(119, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(120, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(121, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(122, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(123, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(124, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(125, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(126, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(127, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(128, NA, grfc, NA, NA, NA, NA, NA),
 	PINGROUP(129, NA, NA, NA, NA, NA, NA, NA),
 	PINGROUP(130, NA, NA, NA, NA, NA, NA, NA),
 	PINGROUP(131, NA, NA, NA, NA, NA, NA, NA),
@@ -571,16 +971,16 @@ static const struct msm_pingroup msm8x74_groups[] = {
 	PINGROUP(133, NA, NA, NA, NA, NA, NA, NA),
 	PINGROUP(134, NA, NA, NA, NA, NA, NA, NA),
 	PINGROUP(135, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(136, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(137, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(136, NA, grfc, NA, NA, NA, NA, NA),
+	PINGROUP(137, NA, grfc, 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, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(141, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(143, NA, 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(141, NA, grfc, NA, NA, NA, NA, NA),
+	PINGROUP(142, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(143, NA, grfc, NA, NA, NA, NA, NA),
+	PINGROUP(144, hsic, NA, NA, NA, NA, NA, NA),
+	PINGROUP(145, hsic, 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),
diff --git a/drivers/pinctrl/pinctrl-rockchip.c b/drivers/pinctrl/pinctrl-rockchip.c
index 96c60d230c1..bb805d5e9ff 100644
--- a/drivers/pinctrl/pinctrl-rockchip.c
+++ b/drivers/pinctrl/pinctrl-rockchip.c
@@ -37,6 +37,8 @@
 #include <linux/pinctrl/pinconf-generic.h>
 #include <linux/irqchip/chained_irq.h>
 #include <linux/clk.h>
+#include <linux/regmap.h>
+#include <linux/mfd/syscon.h>
 #include <dt-bindings/pinctrl/rockchip.h>
 
 #include "core.h"
@@ -86,7 +88,7 @@ enum rockchip_pin_bank_type {
  */
 struct rockchip_pin_bank {
 	void __iomem			*reg_base;
-	void __iomem			*reg_pull;
+	struct regmap			*regmap_pull;
 	struct clk			*clk;
 	int				irq;
 	u32				pin_base;
@@ -120,8 +122,9 @@ struct rockchip_pin_ctrl {
 	char				*label;
 	enum rockchip_pinctrl_type	type;
 	int				mux_offset;
-	void	(*pull_calc_reg)(struct rockchip_pin_bank *bank, int pin_num,
-				 void __iomem **reg, u8 *bit);
+	void	(*pull_calc_reg)(struct rockchip_pin_bank *bank,
+				    int pin_num, struct regmap **regmap,
+				    int *reg, u8 *bit);
 };
 
 struct rockchip_pin_config {
@@ -159,8 +162,10 @@ struct rockchip_pmx_func {
 };
 
 struct rockchip_pinctrl {
-	void __iomem			*reg_base;
-	void __iomem			*reg_pull;
+	struct regmap			*regmap_base;
+	int				reg_size;
+	struct regmap			*regmap_pull;
+	struct regmap			*regmap_pmu;
 	struct device			*dev;
 	struct rockchip_pin_ctrl	*ctrl;
 	struct pinctrl_desc		pctl;
@@ -171,6 +176,12 @@ struct rockchip_pinctrl {
 	unsigned int			nfunctions;
 };
 
+static struct regmap_config rockchip_regmap_config = {
+	.reg_bits = 32,
+	.val_bits = 32,
+	.reg_stride = 4,
+};
+
 static inline struct rockchip_pin_bank *gc_to_pin_bank(struct gpio_chip *gc)
 {
 	return container_of(gc, struct rockchip_pin_bank, gpio_chip);
@@ -329,6 +340,29 @@ static const struct pinctrl_ops rockchip_pctrl_ops = {
  * Hardware access
  */
 
+static int rockchip_get_mux(struct rockchip_pin_bank *bank, int pin)
+{
+	struct rockchip_pinctrl *info = bank->drvdata;
+	unsigned int val;
+	int reg, ret;
+	u8 bit;
+
+	if (bank->bank_type == RK3188_BANK0 && pin < 16)
+		return RK_FUNC_GPIO;
+
+	/* get basic quadrupel of mux registers and the correct reg inside */
+	reg = info->ctrl->mux_offset;
+	reg += bank->bank_num * 0x10;
+	reg += (pin / 8) * 4;
+	bit = (pin % 8) * 2;
+
+	ret = regmap_read(info->regmap_base, reg, &val);
+	if (ret)
+		return ret;
+
+	return ((val >> bit) & 3);
+}
+
 /*
  * Set a new mux function for a pin.
  *
@@ -345,7 +379,7 @@ static const struct pinctrl_ops rockchip_pctrl_ops = {
 static int rockchip_set_mux(struct rockchip_pin_bank *bank, int pin, int mux)
 {
 	struct rockchip_pinctrl *info = bank->drvdata;
-	void __iomem *reg = info->reg_base + info->ctrl->mux_offset;
+	int reg, ret;
 	unsigned long flags;
 	u8 bit;
 	u32 data;
@@ -368,6 +402,7 @@ static int rockchip_set_mux(struct rockchip_pin_bank *bank, int pin, int mux)
 						bank->bank_num, pin, mux);
 
 	/* get basic quadrupel of mux registers and the correct reg inside */
+	reg = info->ctrl->mux_offset;
 	reg += bank->bank_num * 0x10;
 	reg += (pin / 8) * 4;
 	bit = (pin % 8) * 2;
@@ -376,11 +411,11 @@ static int rockchip_set_mux(struct rockchip_pin_bank *bank, int pin, int mux)
 
 	data = (3 << (bit + 16));
 	data |= (mux & 3) << bit;
-	writel(data, reg);
+	ret = regmap_write(info->regmap_base, reg, data);
 
 	spin_unlock_irqrestore(&bank->slock, flags);
 
-	return 0;
+	return ret;
 }
 
 #define RK2928_PULL_OFFSET		0x118
@@ -388,34 +423,46 @@ static int rockchip_set_mux(struct rockchip_pin_bank *bank, int pin, int mux)
 #define RK2928_PULL_BANK_STRIDE		8
 
 static void rk2928_calc_pull_reg_and_bit(struct rockchip_pin_bank *bank,
-				    int pin_num, void __iomem **reg, u8 *bit)
+				    int pin_num, struct regmap **regmap,
+				    int *reg, u8 *bit)
 {
 	struct rockchip_pinctrl *info = bank->drvdata;
 
-	*reg = info->reg_base + RK2928_PULL_OFFSET;
+	*regmap = info->regmap_base;
+	*reg = RK2928_PULL_OFFSET;
 	*reg += bank->bank_num * RK2928_PULL_BANK_STRIDE;
 	*reg += (pin_num / RK2928_PULL_PINS_PER_REG) * 4;
 
 	*bit = pin_num % RK2928_PULL_PINS_PER_REG;
 };
 
+#define RK3188_PULL_OFFSET		0x164
 #define RK3188_PULL_BITS_PER_PIN	2
 #define RK3188_PULL_PINS_PER_REG	8
 #define RK3188_PULL_BANK_STRIDE		16
+#define RK3188_PULL_PMU_OFFSET		0x64
 
 static void rk3188_calc_pull_reg_and_bit(struct rockchip_pin_bank *bank,
-				    int pin_num, void __iomem **reg, u8 *bit)
+				    int pin_num, struct regmap **regmap,
+				    int *reg, u8 *bit)
 {
 	struct rockchip_pinctrl *info = bank->drvdata;
 
 	/* The first 12 pins of the first bank are located elsewhere */
 	if (bank->bank_type == RK3188_BANK0 && pin_num < 12) {
-		*reg = bank->reg_pull +
-				((pin_num / RK3188_PULL_PINS_PER_REG) * 4);
+		*regmap = info->regmap_pmu ? info->regmap_pmu
+					   : bank->regmap_pull;
+		*reg = info->regmap_pmu ? RK3188_PULL_PMU_OFFSET : 0;
+		*reg += ((pin_num / RK3188_PULL_PINS_PER_REG) * 4);
 		*bit = pin_num % RK3188_PULL_PINS_PER_REG;
 		*bit *= RK3188_PULL_BITS_PER_PIN;
 	} else {
-		*reg = info->reg_pull - 4;
+		*regmap = info->regmap_pull ? info->regmap_pull
+					    : info->regmap_base;
+		*reg = info->regmap_pull ? 0 : RK3188_PULL_OFFSET;
+
+		/* correct the offset, as it is the 2nd pull register */
+		*reg -= 4;
 		*reg += bank->bank_num * RK3188_PULL_BANK_STRIDE;
 		*reg += ((pin_num / RK3188_PULL_PINS_PER_REG) * 4);
 
@@ -433,7 +480,8 @@ static int rockchip_get_pull(struct rockchip_pin_bank *bank, int pin_num)
 {
 	struct rockchip_pinctrl *info = bank->drvdata;
 	struct rockchip_pin_ctrl *ctrl = info->ctrl;
-	void __iomem *reg;
+	struct regmap *regmap;
+	int reg, ret;
 	u8 bit;
 	u32 data;
 
@@ -441,15 +489,19 @@ static int rockchip_get_pull(struct rockchip_pin_bank *bank, int pin_num)
 	if (ctrl->type == RK3066B)
 		return PIN_CONFIG_BIAS_DISABLE;
 
-	ctrl->pull_calc_reg(bank, pin_num, &reg, &bit);
+	ctrl->pull_calc_reg(bank, pin_num, &regmap, &reg, &bit);
+
+	ret = regmap_read(regmap, reg, &data);
+	if (ret)
+		return ret;
 
 	switch (ctrl->type) {
 	case RK2928:
-		return !(readl_relaxed(reg) & BIT(bit))
+		return !(data & BIT(bit))
 				? PIN_CONFIG_BIAS_PULL_PIN_DEFAULT
 				: PIN_CONFIG_BIAS_DISABLE;
 	case RK3188:
-		data = readl_relaxed(reg) >> bit;
+		data >>= bit;
 		data &= (1 << RK3188_PULL_BITS_PER_PIN) - 1;
 
 		switch (data) {
@@ -476,7 +528,8 @@ static int rockchip_set_pull(struct rockchip_pin_bank *bank,
 {
 	struct rockchip_pinctrl *info = bank->drvdata;
 	struct rockchip_pin_ctrl *ctrl = info->ctrl;
-	void __iomem *reg;
+	struct regmap *regmap;
+	int reg, ret;
 	unsigned long flags;
 	u8 bit;
 	u32 data;
@@ -488,7 +541,7 @@ static int rockchip_set_pull(struct rockchip_pin_bank *bank,
 	if (ctrl->type == RK3066B)
 		return pull ? -EINVAL : 0;
 
-	ctrl->pull_calc_reg(bank, pin_num, &reg, &bit);
+	ctrl->pull_calc_reg(bank, pin_num, &regmap, &reg, &bit);
 
 	switch (ctrl->type) {
 	case RK2928:
@@ -497,7 +550,7 @@ static int rockchip_set_pull(struct rockchip_pin_bank *bank,
 		data = BIT(bit + 16);
 		if (pull == PIN_CONFIG_BIAS_DISABLE)
 			data |= BIT(bit);
-		writel(data, reg);
+		ret = regmap_write(regmap, reg, data);
 
 		spin_unlock_irqrestore(&bank->slock, flags);
 		break;
@@ -526,7 +579,7 @@ static int rockchip_set_pull(struct rockchip_pin_bank *bank,
 			return -EINVAL;
 		}
 
-		writel(data, reg);
+		ret = regmap_write(regmap, reg, data);
 
 		spin_unlock_irqrestore(&bank->slock, flags);
 		break;
@@ -535,7 +588,7 @@ static int rockchip_set_pull(struct rockchip_pin_bank *bank,
 		return -EINVAL;
 	}
 
-	return 0;
+	return ret;
 }
 
 /*
@@ -687,6 +740,10 @@ static bool rockchip_pinconf_pull_valid(struct rockchip_pin_ctrl *ctrl,
 	return false;
 }
 
+static int rockchip_gpio_direction_output(struct gpio_chip *gc,
+					  unsigned offset, int value);
+static int rockchip_gpio_get(struct gpio_chip *gc, unsigned offset);
+
 /* set the pin config settings for a specified pin */
 static int rockchip_pinconf_set(struct pinctrl_dev *pctldev, unsigned int pin,
 				unsigned long *configs, unsigned num_configs)
@@ -724,6 +781,13 @@ static int rockchip_pinconf_set(struct pinctrl_dev *pctldev, unsigned int pin,
 			if (rc)
 				return rc;
 			break;
+		case PIN_CONFIG_OUTPUT:
+			rc = rockchip_gpio_direction_output(&bank->gpio_chip,
+							    pin - bank->pin_base,
+							    arg);
+			if (rc)
+				return rc;
+			break;
 		default:
 			return -ENOTSUPP;
 			break;
@@ -740,13 +804,15 @@ static int rockchip_pinconf_get(struct pinctrl_dev *pctldev, unsigned int pin,
 	struct rockchip_pinctrl *info = pinctrl_dev_get_drvdata(pctldev);
 	struct rockchip_pin_bank *bank = pin_to_bank(info, pin);
 	enum pin_config_param param = pinconf_to_config_param(*config);
+	u16 arg;
+	int rc;
 
 	switch (param) {
 	case PIN_CONFIG_BIAS_DISABLE:
 		if (rockchip_get_pull(bank, pin - bank->pin_base) != param)
 			return -EINVAL;
 
-		*config = 0;
+		arg = 0;
 		break;
 	case PIN_CONFIG_BIAS_PULL_UP:
 	case PIN_CONFIG_BIAS_PULL_DOWN:
@@ -758,13 +824,26 @@ static int rockchip_pinconf_get(struct pinctrl_dev *pctldev, unsigned int pin,
 		if (rockchip_get_pull(bank, pin - bank->pin_base) != param)
 			return -EINVAL;
 
-		*config = 1;
+		arg = 1;
+		break;
+	case PIN_CONFIG_OUTPUT:
+		rc = rockchip_get_mux(bank, pin - bank->pin_base);
+		if (rc != RK_FUNC_GPIO)
+			return -EINVAL;
+
+		rc = rockchip_gpio_get(&bank->gpio_chip, pin - bank->pin_base);
+		if (rc < 0)
+			return rc;
+
+		arg = rc ? 1 : 0;
 		break;
 	default:
 		return -ENOTSUPP;
 		break;
 	}
 
+	*config = pinconf_to_config_packed(param, arg);
+
 	return 0;
 }
 
@@ -1365,16 +1444,17 @@ static int rockchip_gpiolib_unregister(struct platform_device *pdev,
 }
 
 static int rockchip_get_bank_data(struct rockchip_pin_bank *bank,
-				  struct device *dev)
+				  struct rockchip_pinctrl *info)
 {
 	struct resource res;
+	void __iomem *base;
 
 	if (of_address_to_resource(bank->of_node, 0, &res)) {
-		dev_err(dev, "cannot find IO resource for bank\n");
+		dev_err(info->dev, "cannot find IO resource for bank\n");
 		return -ENOENT;
 	}
 
-	bank->reg_base = devm_ioremap_resource(dev, &res);
+	bank->reg_base = devm_ioremap_resource(info->dev, &res);
 	if (IS_ERR(bank->reg_base))
 		return PTR_ERR(bank->reg_base);
 
@@ -1384,16 +1464,30 @@ static int rockchip_get_bank_data(struct rockchip_pin_bank *bank,
 	 */
 	if (of_device_is_compatible(bank->of_node,
 				    "rockchip,rk3188-gpio-bank0")) {
+		struct device_node *node;
+
 		bank->bank_type = RK3188_BANK0;
 
-		if (of_address_to_resource(bank->of_node, 1, &res)) {
-			dev_err(dev, "cannot find IO resource for bank\n");
-			return -ENOENT;
+		node = of_parse_phandle(bank->of_node->parent,
+					"rockchip,pmu", 0);
+		if (!node) {
+			if (of_address_to_resource(bank->of_node, 1, &res)) {
+				dev_err(info->dev, "cannot find IO resource for bank\n");
+				return -ENOENT;
+			}
+
+			base = devm_ioremap_resource(info->dev, &res);
+			if (IS_ERR(base))
+				return PTR_ERR(base);
+			rockchip_regmap_config.max_register =
+						    resource_size(&res) - 4;
+			rockchip_regmap_config.name =
+					    "rockchip,rk3188-gpio-bank0-pull";
+			bank->regmap_pull = devm_regmap_init_mmio(info->dev,
+						    base,
+						    &rockchip_regmap_config);
 		}
 
-		bank->reg_pull = devm_ioremap_resource(dev, &res);
-		if (IS_ERR(bank->reg_pull))
-			return PTR_ERR(bank->reg_pull);
 	} else {
 		bank->bank_type = COMMON_BANK;
 	}
@@ -1433,7 +1527,7 @@ static struct rockchip_pin_ctrl *rockchip_pinctrl_get_soc_data(
 			if (!strcmp(bank->name, np->name)) {
 				bank->of_node = np;
 
-				if (!rockchip_get_bank_data(bank, &pdev->dev))
+				if (!rockchip_get_bank_data(bank, d))
 					bank->valid = true;
 
 				break;
@@ -1457,7 +1551,9 @@ static int rockchip_pinctrl_probe(struct platform_device *pdev)
 	struct rockchip_pinctrl *info;
 	struct device *dev = &pdev->dev;
 	struct rockchip_pin_ctrl *ctrl;
+	struct device_node *np = pdev->dev.of_node, *node;
 	struct resource *res;
+	void __iomem *base;
 	int ret;
 
 	if (!dev->of_node) {
@@ -1469,25 +1565,56 @@ static int rockchip_pinctrl_probe(struct platform_device *pdev)
 	if (!info)
 		return -ENOMEM;
 
+	info->dev = dev;
+
 	ctrl = rockchip_pinctrl_get_soc_data(info, pdev);
 	if (!ctrl) {
 		dev_err(dev, "driver data not available\n");
 		return -EINVAL;
 	}
 	info->ctrl = ctrl;
-	info->dev = dev;
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	info->reg_base = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(info->reg_base))
-		return PTR_ERR(info->reg_base);
-
-	/* The RK3188 has its pull registers in a separate place */
-	if (ctrl->type == RK3188) {
-		res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-		info->reg_pull = devm_ioremap_resource(&pdev->dev, res);
-		if (IS_ERR(info->reg_pull))
-			return PTR_ERR(info->reg_pull);
+	node = of_parse_phandle(np, "rockchip,grf", 0);
+	if (node) {
+		info->regmap_base = syscon_node_to_regmap(node);
+		if (IS_ERR(info->regmap_base))
+			return PTR_ERR(info->regmap_base);
+	} else {
+		res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+		base = devm_ioremap_resource(&pdev->dev, res);
+		if (IS_ERR(base))
+			return PTR_ERR(base);
+
+		rockchip_regmap_config.max_register = resource_size(res) - 4;
+		rockchip_regmap_config.name = "rockchip,pinctrl";
+		info->regmap_base = devm_regmap_init_mmio(&pdev->dev, base,
+						    &rockchip_regmap_config);
+
+		/* to check for the old dt-bindings */
+		info->reg_size = resource_size(res);
+
+		/* Honor the old binding, with pull registers as 2nd resource */
+		if (ctrl->type == RK3188 && info->reg_size < 0x200) {
+			res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+			base = devm_ioremap_resource(&pdev->dev, res);
+			if (IS_ERR(base))
+				return PTR_ERR(base);
+
+			rockchip_regmap_config.max_register =
+							resource_size(res) - 4;
+			rockchip_regmap_config.name = "rockchip,pinctrl-pull";
+			info->regmap_pull = devm_regmap_init_mmio(&pdev->dev,
+						    base,
+						    &rockchip_regmap_config);
+		}
+	}
+
+	/* try to find the optional reference to the pmu syscon */
+	node = of_parse_phandle(np, "rockchip,pmu", 0);
+	if (node) {
+		info->regmap_pmu = syscon_node_to_regmap(node);
+		if (IS_ERR(info->regmap_pmu))
+			return PTR_ERR(info->regmap_pmu);
 	}
 
 	ret = rockchip_gpiolib_register(pdev, info);
diff --git a/drivers/pinctrl/pinctrl-samsung.c b/drivers/pinctrl/pinctrl-samsung.c
index 0324d4cb19b..3e61d0f8f14 100644
--- a/drivers/pinctrl/pinctrl-samsung.c
+++ b/drivers/pinctrl/pinctrl-samsung.c
@@ -1114,6 +1114,8 @@ static struct syscore_ops samsung_pinctrl_syscore_ops = {
 
 static const struct of_device_id samsung_pinctrl_dt_match[] = {
 #ifdef CONFIG_PINCTRL_EXYNOS
+	{ .compatible = "samsung,exynos3250-pinctrl",
+		.data = (void *)exynos3250_pin_ctrl },
 	{ .compatible = "samsung,exynos4210-pinctrl",
 		.data = (void *)exynos4210_pin_ctrl },
 	{ .compatible = "samsung,exynos4x12-pinctrl",
diff --git a/drivers/pinctrl/pinctrl-samsung.h b/drivers/pinctrl/pinctrl-samsung.h
index bab9c212255..b3e41fa5798 100644
--- a/drivers/pinctrl/pinctrl-samsung.h
+++ b/drivers/pinctrl/pinctrl-samsung.h
@@ -251,6 +251,7 @@ struct samsung_pmx_func {
 };
 
 /* list of all exported SoC specific data */
+extern struct samsung_pin_ctrl exynos3250_pin_ctrl[];
 extern struct samsung_pin_ctrl exynos4210_pin_ctrl[];
 extern struct samsung_pin_ctrl exynos4x12_pin_ctrl[];
 extern struct samsung_pin_ctrl exynos5250_pin_ctrl[];
diff --git a/drivers/pinctrl/pinctrl-st.c b/drivers/pinctrl/pinctrl-st.c
index bd725b0a434..1bd6363bc95 100644
--- a/drivers/pinctrl/pinctrl-st.c
+++ b/drivers/pinctrl/pinctrl-st.c
@@ -13,10 +13,6 @@
 #include <linux/slab.h>
 #include <linux/err.h>
 #include <linux/io.h>
-#include <linux/irq.h>
-#include <linux/irqdesc.h>
-#include <linux/irqdomain.h>
-#include <linux/irqchip/chained_irq.h>
 #include <linux/of.h>
 #include <linux/of_irq.h>
 #include <linux/of_gpio.h>
@@ -242,13 +238,13 @@ struct st_pio_control {
 };
 
 struct st_pctl_data {
-	enum st_retime_style rt_style;
-	unsigned int	*input_delays;
-	int		ninput_delays;
-	unsigned int	*output_delays;
-	int		noutput_delays;
+	const enum st_retime_style	rt_style;
+	const unsigned int		*input_delays;
+	const int			ninput_delays;
+	const unsigned int		*output_delays;
+	const int			noutput_delays;
 	/* register offset information */
-	int alt, oe, pu, od, rt;
+	const int alt, oe, pu, od, rt;
 };
 
 struct st_pinconf {
@@ -321,7 +317,6 @@ struct st_gpio_bank {
 	struct pinctrl_gpio_range	range;
 	void __iomem			*base;
 	struct st_pio_control		pc;
-	struct	irq_domain		*domain;
 	unsigned long			irq_edge_conf;
 	spinlock_t                      lock;
 };
@@ -342,15 +337,15 @@ struct st_pinctrl {
 
 /* SOC specific data */
 /* STiH415 data */
-static unsigned int stih415_input_delays[] = {0, 500, 1000, 1500};
-static unsigned int stih415_output_delays[] = {0, 1000, 2000, 3000};
+static const unsigned int stih415_input_delays[] = {0, 500, 1000, 1500};
+static const unsigned int stih415_output_delays[] = {0, 1000, 2000, 3000};
 
 #define STIH415_PCTRL_COMMON_DATA				\
 	.rt_style	= st_retime_style_packed,		\
 	.input_delays	= stih415_input_delays,			\
-	.ninput_delays	= 4,					\
+	.ninput_delays	= ARRAY_SIZE(stih415_input_delays),	\
 	.output_delays = stih415_output_delays,			\
-	.noutput_delays = 4
+	.noutput_delays = ARRAY_SIZE(stih415_output_delays)
 
 static const struct st_pctl_data  stih415_sbc_data = {
 	STIH415_PCTRL_COMMON_DATA,
@@ -378,8 +373,8 @@ static const struct st_pctl_data  stih415_right_data = {
 };
 
 /* STiH416 data */
-static unsigned int stih416_delays[] = {0, 300, 500, 750, 1000, 1250, 1500,
-			1750, 2000, 2250, 2500, 2750, 3000, 3250 };
+static const unsigned int stih416_delays[] = {0, 300, 500, 750, 1000, 1250,
+			1500, 1750, 2000, 2250, 2500, 2750, 3000, 3250 };
 
 static const struct st_pctl_data  stih416_data = {
 	.rt_style	= st_retime_style_dedicated,
@@ -468,7 +463,7 @@ static void st_pctl_set_function(struct st_pio_control *pc,
 static unsigned long st_pinconf_delay_to_bit(unsigned int delay,
 	const struct st_pctl_data *data, unsigned long config)
 {
-	unsigned int *delay_times;
+	const unsigned int *delay_times;
 	int num_delay_times, i, closest_index = -1;
 	unsigned int closest_divergence = UINT_MAX;
 
@@ -501,7 +496,7 @@ static unsigned long st_pinconf_delay_to_bit(unsigned int delay,
 static unsigned long st_pinconf_bit_to_delay(unsigned int index,
 	const struct st_pctl_data *data, unsigned long output)
 {
-	unsigned int *delay_times;
+	const unsigned int *delay_times;
 	int num_delay_times;
 
 	if (output) {
@@ -1285,58 +1280,26 @@ static int st_pctl_parse_functions(struct device_node *np,
 	return 0;
 }
 
-static int st_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
-{
-	struct st_gpio_bank *bank = gpio_chip_to_bank(chip);
-	int irq = -ENXIO;
-
-	if (offset < chip->ngpio)
-		irq = irq_find_mapping(bank->domain, offset);
-
-	dev_info(chip->dev, "%s: request IRQ for GPIO %d, return %d\n",
-				chip->label, offset + chip->base, irq);
-	return irq;
-}
-
 static void st_gpio_irq_mask(struct irq_data *d)
 {
-	struct st_gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct st_gpio_bank *bank = gpio_chip_to_bank(gc);
 
 	writel(BIT(d->hwirq), bank->base + REG_PIO_CLR_PMASK);
 }
 
 static void st_gpio_irq_unmask(struct irq_data *d)
 {
-	struct st_gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct st_gpio_bank *bank = gpio_chip_to_bank(gc);
 
 	writel(BIT(d->hwirq), bank->base + REG_PIO_SET_PMASK);
 }
 
-static unsigned int st_gpio_irq_startup(struct irq_data *d)
-{
-	struct st_gpio_bank *bank = irq_data_get_irq_chip_data(d);
-
-	if (gpio_lock_as_irq(&bank->gpio_chip, d->hwirq))
-		dev_err(bank->gpio_chip.dev,
-			"unable to lock HW IRQ %lu for IRQ\n",
-			d->hwirq);
-
-	st_gpio_irq_unmask(d);
-
-	return 0;
-}
-
-static void st_gpio_irq_shutdown(struct irq_data *d)
-{
-	struct st_gpio_bank *bank = irq_data_get_irq_chip_data(d);
-
-	st_gpio_irq_mask(d);
-	gpio_unlock_as_irq(&bank->gpio_chip, d->hwirq);
-}
-
 static int st_gpio_irq_set_type(struct irq_data *d, unsigned type)
 {
-	struct st_gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct st_gpio_bank *bank = gpio_chip_to_bank(gc);
 	unsigned long flags;
 	int comp, pin = d->hwirq;
 	u32 val;
@@ -1440,7 +1403,7 @@ static void __gpio_irq_handler(struct st_gpio_bank *bank)
 					continue;
 			}
 
-			generic_handle_irq(irq_find_mapping(bank->domain, n));
+			generic_handle_irq(irq_find_mapping(bank->gpio_chip.irqdomain, n));
 		}
 	}
 }
@@ -1449,7 +1412,8 @@ static void st_gpio_irq_handler(unsigned irq, struct irq_desc *desc)
 {
 	/* interrupt dedicated per bank */
 	struct irq_chip *chip = irq_get_chip(irq);
-	struct st_gpio_bank *bank = irq_get_handler_data(irq);
+	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
+	struct st_gpio_bank *bank = gpio_chip_to_bank(gc);
 
 	chained_irq_enter(chip, desc);
 	__gpio_irq_handler(bank);
@@ -1483,7 +1447,6 @@ static struct gpio_chip st_gpio_template = {
 	.ngpio			= ST_GPIO_PINS_PER_BANK,
 	.of_gpio_n_cells	= 1,
 	.of_xlate		= st_gpio_xlate,
-	.to_irq			= st_gpio_to_irq,
 };
 
 static struct irq_chip st_gpio_irqchip = {
@@ -1491,26 +1454,6 @@ static struct irq_chip st_gpio_irqchip = {
 	.irq_mask	= st_gpio_irq_mask,
 	.irq_unmask	= st_gpio_irq_unmask,
 	.irq_set_type	= st_gpio_irq_set_type,
-	.irq_startup	= st_gpio_irq_startup,
-	.irq_shutdown	= st_gpio_irq_shutdown,
-};
-
-static int st_gpio_irq_domain_map(struct irq_domain *h,
-			unsigned int virq, irq_hw_number_t hw)
-{
-	struct st_gpio_bank *bank = h->host_data;
-
-	irq_set_chip(virq, &st_gpio_irqchip);
-	irq_set_handler(virq, handle_simple_irq);
-	set_irq_flags(virq, IRQF_VALID);
-	irq_set_chip_data(virq, bank);
-
-	return 0;
-}
-
-static struct irq_domain_ops st_gpio_irq_ops = {
-	.map	= st_gpio_irq_domain_map,
-	.xlate	= irq_domain_xlate_twocell,
 };
 
 static int st_gpiolib_register_bank(struct st_pinctrl *info,
@@ -1521,7 +1464,7 @@ static int st_gpiolib_register_bank(struct st_pinctrl *info,
 	struct device *dev = info->dev;
 	int bank_num = of_alias_get_id(np, "gpio");
 	struct resource res, irq_res;
-	int gpio_irq = 0, err, i;
+	int gpio_irq = 0, err;
 
 	if (of_address_to_resource(np, 0, &res))
 		return -ENODEV;
@@ -1534,6 +1477,7 @@ static int st_gpiolib_register_bank(struct st_pinctrl *info,
 	bank->gpio_chip.base = bank_num * ST_GPIO_PINS_PER_BANK;
 	bank->gpio_chip.ngpio = ST_GPIO_PINS_PER_BANK;
 	bank->gpio_chip.of_node = np;
+	bank->gpio_chip.dev = dev;
 	spin_lock_init(&bank->lock);
 
 	of_property_read_string(np, "st,bank-name", &range->name);
@@ -1571,26 +1515,18 @@ static int st_gpiolib_register_bank(struct st_pinctrl *info,
 
 	if (of_irq_to_resource(np, 0, &irq_res)) {
 		gpio_irq = irq_res.start;
-		irq_set_chained_handler(gpio_irq, st_gpio_irq_handler);
-		irq_set_handler_data(gpio_irq, bank);
+		gpiochip_set_chained_irqchip(&bank->gpio_chip, &st_gpio_irqchip,
+					     gpio_irq, st_gpio_irq_handler);
 	}
 
 	if (info->irqmux_base > 0 || gpio_irq > 0) {
-		/* Setup IRQ domain */
-		bank->domain  = irq_domain_add_linear(np,
-						ST_GPIO_PINS_PER_BANK,
-						&st_gpio_irq_ops, bank);
-		if (!bank->domain) {
-			dev_err(dev, "Failed to add irq domain for %s\n",
-				np->full_name);
-		} else  {
-			for (i = 0; i < ST_GPIO_PINS_PER_BANK; i++) {
-				if (irq_create_mapping(bank->domain, i) < 0)
-					dev_err(dev,
-						"Failed to map IRQ %i\n", i);
-			}
+		err = gpiochip_irqchip_add(&bank->gpio_chip, &st_gpio_irqchip,
+					   0, handle_simple_irq,
+					   IRQ_TYPE_LEVEL_LOW);
+		if (err) {
+			dev_info(dev, "could not add irqchip\n");
+			return err;
 		}
-
 	} else {
 		dev_info(dev, "No IRQ support for %s bank\n", np->full_name);
 	}
diff --git a/drivers/pinctrl/pinctrl-sunxi-pins.h b/drivers/pinctrl/pinctrl-sunxi-pins.h
deleted file mode 100644
index 3d6066988a7..00000000000
--- a/drivers/pinctrl/pinctrl-sunxi-pins.h
+++ /dev/null
@@ -1,3863 +0,0 @@
-/*
- * Allwinner A1X SoCs pinctrl driver.
- *
- * Copyright (C) 2012 Maxime Ripard
- *
- * Maxime Ripard <maxime.ripard@free-electrons.com>
- *
- * This file is licensed under the terms of the GNU General Public
- * License version 2.  This program is licensed "as is" without any
- * warranty of any kind, whether express or implied.
- */
-
-#ifndef __PINCTRL_SUNXI_PINS_H
-#define __PINCTRL_SUNXI_PINS_H
-
-#include "pinctrl-sunxi.h"
-
-static const struct sunxi_desc_pin sun4i_a10_pins[] = {
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD3 */
-		  SUNXI_FUNCTION(0x3, "spi1"),		/* CS0 */
-		  SUNXI_FUNCTION(0x4, "uart2")),	/* RTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD2 */
-		  SUNXI_FUNCTION(0x3, "spi1"),		/* CLK */
-		  SUNXI_FUNCTION(0x4, "uart2")),	/* CTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD1 */
-		  SUNXI_FUNCTION(0x3, "spi1"),		/* MOSI */
-		  SUNXI_FUNCTION(0x4, "uart2")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD0 */
-		  SUNXI_FUNCTION(0x3, "spi1"),		/* MISO */
-		  SUNXI_FUNCTION(0x4, "uart2")),	/* RX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD3 */
-		  SUNXI_FUNCTION(0x3, "spi1")),		/* CS1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD2 */
-		  SUNXI_FUNCTION(0x3, "spi3")),		/* CS0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD1 */
-		  SUNXI_FUNCTION(0x3, "spi3")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD0 */
-		  SUNXI_FUNCTION(0x3, "spi3")),		/* MOSI */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXCK */
-		  SUNXI_FUNCTION(0x3, "spi3")),		/* MISO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXERR */
-		  SUNXI_FUNCTION(0x3, "spi3")),		/* CS1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXDV */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* EMDC */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* RX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* EMDIO */
-		  SUNXI_FUNCTION(0x3, "uart6"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* RTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXEN */
-		  SUNXI_FUNCTION(0x3, "uart6"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* CTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXCK */
-		  SUNXI_FUNCTION(0x3, "uart7"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* DTR */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ECRS */
-		  SUNXI_FUNCTION(0x3, "uart7"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* DSR */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ECOL */
-		  SUNXI_FUNCTION(0x3, "can"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* DCD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXERR */
-		  SUNXI_FUNCTION(0x3, "can"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* RING */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "pwm")),		/* PWM0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ir0")),		/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ir0")),		/* RX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s"),		/* MCLK */
-		  SUNXI_FUNCTION(0x3, "ac97")),		/* MCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s"),		/* BCLK */
-		  SUNXI_FUNCTION(0x3, "ac97")),		/* BCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s"),		/* LRCK */
-		  SUNXI_FUNCTION(0x3, "ac97")),		/* SYNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s"),		/* DO0 */
-		  SUNXI_FUNCTION(0x3, "ac97")),		/* DO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s")),		/* DO1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s")),		/* DO2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s")),		/* DO3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s"),		/* DI */
-		  SUNXI_FUNCTION(0x3, "ac97")),		/* DI */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2")),		/* CS1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS0 */
-		  SUNXI_FUNCTION(0x3, "jtag")),		/* MS0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "jtag")),		/* CK0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* MOSI */
-		  SUNXI_FUNCTION(0x3, "jtag")),		/* DO0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* MISO */
-		  SUNXI_FUNCTION(0x3, "jtag")),		/* DI0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB22,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart0"),		/* TX */
-		  SUNXI_FUNCTION(0x3, "ir1")),		/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB23,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart0"),		/* RX */
-		  SUNXI_FUNCTION(0x3, "ir1")),		/* RX */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NWE */
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* MOSI */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NALE */
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* MISO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCLE */
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NRE# */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB0 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CMD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB1 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ0 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ1 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ2 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ3 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ7 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NWP */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE4 */
-		  SUNXI_FUNCTION(0x3, "spi2")),		/* CS0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE5 */
-		  SUNXI_FUNCTION(0x3, "spi2")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE6 */
-		  SUNXI_FUNCTION(0x3, "spi2")),		/* MOSI */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC22,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE7 */
-		  SUNXI_FUNCTION(0x3, "spi2")),		/* MISO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC23,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* CS0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC24,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQS */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D0 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D1 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D2 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D3 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D4 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D5 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D6 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VPC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D7 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D8 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D9 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VM3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D10 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D11 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D12 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D13 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D14 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D15 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D16 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VPC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D17 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D18 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D19 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D20 */
-		  SUNXI_FUNCTION(0x3, "csi1")),		/* MCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D21 */
-		  SUNXI_FUNCTION(0x3, "sim")),		/* VPPEN */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD22,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D22 */
-		  SUNXI_FUNCTION(0x3, "sim")),		/* VPPPP */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD23,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D23 */
-		  SUNXI_FUNCTION(0x3, "sim")),		/* DET */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD24,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "sim")),		/* VCCEN */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD25,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* DE */
-		  SUNXI_FUNCTION(0x3, "sim")),		/* RST */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD26,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* HSYNC */
-		  SUNXI_FUNCTION(0x3, "sim")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD27,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* VSYNC */
-		  SUNXI_FUNCTION(0x3, "sim")),		/* SDA */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* PCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* ERR */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* CK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* SYNC */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* HSYNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* DVLD */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* VSYNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D0 */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D1 */
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "sim")),		/* VPPEN */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D2 */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D3 */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D4 */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* D4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D5 */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* D5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D6 */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* D6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D7 */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* D7 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "jtag")),		/* MSI */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D0 */
-		  SUNXI_FUNCTION(0x4, "jtag")),		/* DI1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CLK */
-		  SUNXI_FUNCTION(0x4, "uart0")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CMD */
-		  SUNXI_FUNCTION(0x4, "jtag")),		/* DO1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D3 */
-		  SUNXI_FUNCTION(0x4, "uart0")),	/* RX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D2 */
-		  SUNXI_FUNCTION(0x4, "jtag")),		/* CK1 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* PCK */
-		  SUNXI_FUNCTION(0x4, "mmc1")),		/* CMD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* ERR */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* CK */
-		  SUNXI_FUNCTION(0x4, "mmc1")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* SYNC */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* HSYNC */
-		  SUNXI_FUNCTION(0x4, "mmc1")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* DVLD */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* VSYNC */
-		  SUNXI_FUNCTION(0x4, "mmc1")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D0 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D0 */
-		  SUNXI_FUNCTION(0x4, "mmc1"),		/* D2 */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D8 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D1 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "mmc1"),		/* D3 */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D9 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D2 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D2 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* TX */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D10 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D3 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D3 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* RX */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D11 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D4 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D4 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* RTS */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D12 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D5 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D5 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* CTS */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D13 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D6 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D6 */
-		  SUNXI_FUNCTION(0x4, "uart4"),		/* TX */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D14 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D7 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D7 */
-		  SUNXI_FUNCTION(0x4, "uart4"),		/* RX */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D15 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D0 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAA0 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 0),		/* EINT0 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D1 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAA1 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 1),		/* EINT1 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D2 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAA2 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* RTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 2),		/* EINT2 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D3 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAIRQ */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* CTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 3),		/* EINT3 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D4 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD0 */
-		  SUNXI_FUNCTION(0x4, "uart4"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 4),		/* EINT4 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D5 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD1 */
-		  SUNXI_FUNCTION(0x4, "uart4"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 5),		/* EINT5 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D6 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD2 */
-		  SUNXI_FUNCTION(0x4, "uart5"),		/* TX */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* BS */
-		  SUNXI_FUNCTION_IRQ(0x6, 6),		/* EINT6 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D7 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD3 */
-		  SUNXI_FUNCTION(0x4, "uart5"),		/* RX */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* CLK */
-		  SUNXI_FUNCTION_IRQ(0x6, 7),		/* EINT7 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D7 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D8 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD4 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN0 */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* D0 */
-		  SUNXI_FUNCTION_IRQ(0x6, 8),		/* EINT8 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D8 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D9 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD5 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN1 */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* D1 */
-		  SUNXI_FUNCTION_IRQ(0x6, 9),		/* EINT9 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D9 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D10 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD6 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN2 */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* D2 */
-		  SUNXI_FUNCTION_IRQ(0x6, 10),		/* EINT10 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D10 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D11 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD7 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN3 */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* D3 */
-		  SUNXI_FUNCTION_IRQ(0x6, 11),		/* EINT11 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D11 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D12 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD8 */
-		  SUNXI_FUNCTION(0x4, "ps2"),		/* SCK1 */
-		  SUNXI_FUNCTION_IRQ(0x6, 12),		/* EINT12 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D12 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D13 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD9 */
-		  SUNXI_FUNCTION(0x4, "ps2"),		/* SDA1 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* RST */
-		  SUNXI_FUNCTION_IRQ(0x6, 13),		/* EINT13 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D13 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D14 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD10 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN4 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* VPPEN */
-		  SUNXI_FUNCTION_IRQ(0x6, 14),		/* EINT14 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D14 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D15 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD11 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN5 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* VPPPP */
-		  SUNXI_FUNCTION_IRQ(0x6, 15),		/* EINT15 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D15 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D16 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD12 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN6 */
-		  SUNXI_FUNCTION_IRQ(0x6, 16),		/* EINT16 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D16 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D17 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD13 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN7 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* VCCEN */
-		  SUNXI_FUNCTION_IRQ(0x6, 17),		/* EINT17 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D17 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D18 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD14 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT0 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* SCK */
-		  SUNXI_FUNCTION_IRQ(0x6, 18),		/* EINT18 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D18 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D19 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD15 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT1 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* SDA */
-		  SUNXI_FUNCTION_IRQ(0x6, 19),		/* EINT19 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D19 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D20 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAOE */
-		  SUNXI_FUNCTION(0x4, "can"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 20),		/* EINT20 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D20 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D21 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATADREQ */
-		  SUNXI_FUNCTION(0x4, "can"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 21),		/* EINT21 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D21 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH22,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D22 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATADACK */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT2 */
-		  SUNXI_FUNCTION(0x5, "mmc1"),		/* CMD */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D22 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH23,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D23 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATACS0 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT3 */
-		  SUNXI_FUNCTION(0x5, "mmc1"),		/* CLK */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D23 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH24,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATACS1 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT4 */
-		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D0 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* PCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH25,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* DE */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAIORDY */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT5 */
-		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D1 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* FIELD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH26,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* HSYNC */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAIOR */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT6 */
-		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D2 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* HSYNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH27,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* VSYNC */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAIOW */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT7 */
-		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D3 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* VSYNC */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out")),
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out")),
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out")),
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "pwm")),		/* PWM1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc3")),		/* CMD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc3")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* CS0 */
-		  SUNXI_FUNCTION(0x3, "uart5"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 22)),		/* EINT22 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "uart5"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 23)),		/* EINT23 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* MOSI */
-		  SUNXI_FUNCTION(0x3, "uart6"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 24)),		/* EINT24 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* MISO */
-		  SUNXI_FUNCTION(0x3, "uart6"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 25)),		/* EINT25 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* CS1 */
-		  SUNXI_FUNCTION(0x3, "ps2"),		/* SCK1 */
-		  SUNXI_FUNCTION(0x4, "timer4"),	/* TCLKIN0 */
-		  SUNXI_FUNCTION_IRQ(0x6, 26)),		/* EINT26 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS1 */
-		  SUNXI_FUNCTION(0x3, "ps2"),		/* SDA1 */
-		  SUNXI_FUNCTION(0x4, "timer5"),	/* TCLKIN1 */
-		  SUNXI_FUNCTION_IRQ(0x6, 27)),		/* EINT27 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS0 */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* RTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 28)),		/* EINT28 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* CTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 29)),		/* EINT29 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 30)),		/* EINT30 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 31)),		/* EINT31 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ps2"),		/* SCK0 */
-		  SUNXI_FUNCTION(0x3, "uart7"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "hdmi")),		/* HSCL */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ps2"),		/* SDA0 */
-		  SUNXI_FUNCTION(0x3, "uart7"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "hdmi")),		/* HSDA */
-};
-
-static const struct sunxi_desc_pin sun5i_a10s_pins[] = {
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD3 */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* CLK */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD2 */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* ERR */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD1 */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* SYNC */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD0 */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* DLVD */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD3 */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* D0 */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD2 */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* D1 */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD1 */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* D2 */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD0 */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* D3 */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN7 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXCK */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* D4 */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* DTR */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXERR */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* D5 */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* DSR */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXDV */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* D6 */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* DCD */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* EMDC */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* D7 */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* RING */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* EMDIO */
-		  SUNXI_FUNCTION(0x3, "uart1"),		/* TX */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXEN */
-		  SUNXI_FUNCTION(0x3, "uart1"),		/* RX */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXCK */
-		  SUNXI_FUNCTION(0x3, "uart1"),		/* CTS */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* TX */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ECRS */
-		  SUNXI_FUNCTION(0x3, "uart1"),		/* RTS */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* RX */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT7 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ECOL */
-		  SUNXI_FUNCTION(0x3, "uart2")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXERR */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 31)),		/* EINT31 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "pwm"),		/* PWM0 */
-		  SUNXI_FUNCTION_IRQ(0x6, 16)),		/* EINT16 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ir0"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 17)),		/* EINT17 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ir0"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 18)),		/* EINT18 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s"),		/* MCLK */
-		  SUNXI_FUNCTION_IRQ(0x6, 19)),		/* EINT19 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s"),		/* BCLK */
-		  SUNXI_FUNCTION_IRQ(0x6, 20)),		/* EINT20 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s"),		/* LRCK */
-		  SUNXI_FUNCTION_IRQ(0x6, 21)),		/* EINT21 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s"),		/* DO */
-		  SUNXI_FUNCTION_IRQ(0x6, 22)),		/* EINT22 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s"),		/* DI */
-		  SUNXI_FUNCTION_IRQ(0x6, 23)),		/* EINT23 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS1 */
-		  SUNXI_FUNCTION_IRQ(0x6, 24)),		/* EINT24 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS0 */
-		  SUNXI_FUNCTION(0x3, "jtag"),		/* MS0 */
-		  SUNXI_FUNCTION_IRQ(0x6, 25)),		/* EINT25 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "jtag"),		/* CK0 */
-		  SUNXI_FUNCTION_IRQ(0x6, 26)),		/* EINT26 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* MOSI */
-		  SUNXI_FUNCTION(0x3, "jtag"),		/* DO0 */
-		  SUNXI_FUNCTION_IRQ(0x6, 27)),		/* EINT27 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* MISO */
-		  SUNXI_FUNCTION(0x3, "jtag"),		/* DI0 */
-		  SUNXI_FUNCTION_IRQ(0x6, 28)),		/* EINT28 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart0"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 29)),		/* EINT29 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart0"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 30)),		/* EINT30 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NWE */
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* MOSI */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NALE */
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* MISO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCLE */
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE1 */
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* CS0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NRE */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB0 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CMD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB1 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ0 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ1 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ2 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ3 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ4 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ5 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ6 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ7 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D7 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NWP */
-		  SUNXI_FUNCTION(0x4, "uart3")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE2 */
-		  SUNXI_FUNCTION(0x4, "uart3")),	/* RX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE3 */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "uart3")),	/* CTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE4 */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "uart3")),	/* RTS */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D2 */
-		  SUNXI_FUNCTION(0x3, "uart2")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D3 */
-		  SUNXI_FUNCTION(0x3, "uart2")),	/* RX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D4 */
-		  SUNXI_FUNCTION(0x3, "uart2")),	/* CTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D5 */
-		  SUNXI_FUNCTION(0x3, "uart2")),	/* RTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D6 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ECRS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D7 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ECOL */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D8 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D9 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D10 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXD0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D11 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXD1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D12 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXD2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D13 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXD3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D14 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D15 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXERR */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D16 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D17 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D18 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXDV */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D19 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXD0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D20 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXD1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D21 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXD2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD22,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D22 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXD3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD23,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D23 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXEN */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD24,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD25,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* DE */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXERR */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD26,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* HSYNC */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* EMDC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD27,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* VSYNC */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* EMDIO */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* PCK */
-		  SUNXI_FUNCTION(0x4, "spi2"),		/* CS0 */
-		  SUNXI_FUNCTION_IRQ(0x6, 14)),		/* EINT14 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* ERR */
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* CK */
-		  SUNXI_FUNCTION(0x4, "spi2"),		/* CLK */
-		  SUNXI_FUNCTION_IRQ(0x6, 15)),		/* EINT15 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* SYNC */
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* HSYNC */
-		  SUNXI_FUNCTION(0x4, "spi2")),		/* MOSI */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* DVLD */
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* VSYNC */
-		  SUNXI_FUNCTION(0x4, "spi2")),		/* MISO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D0 */
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* D0 */
-		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D1 */
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D2 */
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* D2 */
-		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D3 */
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* D3 */
-		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D4 */
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* D4 */
-		  SUNXI_FUNCTION(0x4, "mmc2")),		/* CMD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D5 */
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* D5 */
-		  SUNXI_FUNCTION(0x4, "mmc2")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D6 */
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* D6 */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D7 */
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* D7 */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* RX */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "jtag")),		/* MS1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D0 */
-		  SUNXI_FUNCTION(0x4, "jtag")),		/* DI1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CLK */
-		  SUNXI_FUNCTION(0x4, "uart0")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CMD */
-		  SUNXI_FUNCTION(0x4, "jtag")),		/* DO1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D3 */
-		  SUNXI_FUNCTION(0x4, "uart0")),	/* RX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D2 */
-		  SUNXI_FUNCTION(0x4, "jtag")),		/* CK1 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x2, "gps"),		/* CLK */
-		  SUNXI_FUNCTION_IRQ(0x6, 0)),		/* EINT0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x2, "gps"),		/* SIGN */
-		  SUNXI_FUNCTION_IRQ(0x6, 1)),		/* EINT1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x2, "gps"),		/* MAG */
-		  SUNXI_FUNCTION_IRQ(0x6, 2)),		/* EINT2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc1"),		/* CMD */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 3)),		/* EINT3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc1"),		/* CLK */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 4)),		/* EINT4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc1"),		/* DO */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* CTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 5)),		/* EINT5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc1"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* RTS */
-		  SUNXI_FUNCTION(0x5, "uart2"),		/* RTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 6)),		/* EINT6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc1"),		/* D2 */
-		  SUNXI_FUNCTION(0x5, "uart2"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 7)),		/* EINT7 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc1"),		/* D3 */
-		  SUNXI_FUNCTION(0x5, "uart2"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 8)),		/* EINT8 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS0 */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 9)),		/* EINT9 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 10)),		/* EINT10 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* CTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 11)),		/* EINT11 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* RTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 12)),		/* EINT12 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS1 */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* PWM1 */
-		  SUNXI_FUNCTION(0x5, "uart2"),		/* CTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 13)),		/* EINT13 */
-};
-
-static const struct sunxi_desc_pin sun5i_a13_pins[] = {
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "pwm"),
-		  SUNXI_FUNCTION_IRQ(0x6, 16)),		/* EINT16 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ir0"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 17)),		/* EINT17 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ir0"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 18)),		/* EINT18 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS1 */
-		  SUNXI_FUNCTION_IRQ(0x6, 24)),		/* EINT24 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SDA */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NWE */
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* MOSI */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NALE */
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* MISO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCLE */
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE1 */
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* CS0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NRE */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB0 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CMD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB1 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ0 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ1 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ2 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ3 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ4 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ5 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ6 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ7 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D7 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQS */
-		  SUNXI_FUNCTION(0x4, "uart3")),	/* RTS */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D7 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D10 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D11 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D12 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D13 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D14 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D15 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D18 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D19 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D20 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D21 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD22,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D22 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD23,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D23 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD24,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD25,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* DE */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD26,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* HSYNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD27,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* VSYNC */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* PCLK */
-		  SUNXI_FUNCTION(0x4, "spi2"),		/* CS0 */
-		  SUNXI_FUNCTION_IRQ(0x6, 14)),		/* EINT14 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* MCLK */
-		  SUNXI_FUNCTION(0x4, "spi2"),		/* CLK */
-		  SUNXI_FUNCTION_IRQ(0x6, 15)),		/* EINT15 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* HSYNC */
-		  SUNXI_FUNCTION(0x4, "spi2")),		/* MOSI */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* VSYNC */
-		  SUNXI_FUNCTION(0x4, "spi2")),		/* MISO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* D0 */
-		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* D2 */
-		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* D3 */
-		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* D4 */
-		  SUNXI_FUNCTION(0x4, "mmc2")),		/* CMD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* D5 */
-		  SUNXI_FUNCTION(0x4, "mmc2")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* D6 */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* D7 */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* RX */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0")),		/* CMD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0")),		/* D2 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION_IRQ(0x6, 0)),		/* EINT0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION_IRQ(0x6, 1)),		/* EINT1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION_IRQ(0x6, 2)),		/* EINT2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc1"),		/* CMD */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 3)),		/* EINT3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc1"),		/* CLK */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 4)),		/* EINT4 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS0 */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 9)),		/* EINT9 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 10)),		/* EINT10 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* CTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 11)),		/* EINT11 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* RTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 12)),		/* EINT12 */
-};
-
-static const struct sunxi_desc_pin sun6i_a31_pins[] = {
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD0 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D0 */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* DTR */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD1 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* DSR */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD2 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D2 */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* DCD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD3 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D3 */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* RING */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD4 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D4 */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD5 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D5 */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* RX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD6 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D6 */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* RTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD7 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D7 */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* CTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXCLK */
-		  SUNXI_FUNCTION(0x3, "lcd1")),		/* D8 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXEN */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D9 */
-		  SUNXI_FUNCTION(0x4, "mmc3"),		/* CMD */
-		  SUNXI_FUNCTION(0x5, "mmc2")),		/* CMD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* GTXCLK */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D10 */
-		  SUNXI_FUNCTION(0x4, "mmc3"),		/* CLK */
-		  SUNXI_FUNCTION(0x5, "mmc2")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD0 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D11 */
-		  SUNXI_FUNCTION(0x4, "mmc3"),		/* D0 */
-		  SUNXI_FUNCTION(0x5, "mmc2")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD1 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D12 */
-		  SUNXI_FUNCTION(0x4, "mmc3"),		/* D1 */
-		  SUNXI_FUNCTION(0x5, "mmc2")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD2 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D13 */
-		  SUNXI_FUNCTION(0x4, "mmc3"),		/* D2 */
-		  SUNXI_FUNCTION(0x5, "mmc2")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD3 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D14 */
-		  SUNXI_FUNCTION(0x4, "mmc3"),		/* D3 */
-		  SUNXI_FUNCTION(0x5, "mmc2")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD4 */
-		  SUNXI_FUNCTION(0x3, "lcd1")),		/* D15 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD5 */
-		  SUNXI_FUNCTION(0x3, "lcd1")),		/* D16 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD6 */
-		  SUNXI_FUNCTION(0x3, "lcd1")),		/* D17 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD7 */
-		  SUNXI_FUNCTION(0x3, "lcd1")),		/* D18 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXDV */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D19 */
-		  SUNXI_FUNCTION(0x4, "pwm3")),		/* Positive */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXCLK */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D20 */
-		  SUNXI_FUNCTION(0x4, "pwm3")),		/* Negative */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXERR */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D21 */
-		  SUNXI_FUNCTION(0x4, "spi3")),		/* CS0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA22,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXERR */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D22 */
-		  SUNXI_FUNCTION(0x4, "spi3")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA23,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* COL */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D23 */
-		  SUNXI_FUNCTION(0x4, "spi3")),		/* MOSI */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA24,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* CRS */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* CLK */
-		  SUNXI_FUNCTION(0x4, "spi3")),		/* MISO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA25,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* CLKIN */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* DE */
-		  SUNXI_FUNCTION(0x4, "spi3")),		/* CS1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA26,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* MDC */
-		  SUNXI_FUNCTION(0x3, "lcd1")),		/* HSYNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA27,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* MDIO */
-		  SUNXI_FUNCTION(0x3, "lcd1")),		/* VSYNC */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0"),		/* MCLK */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* CTS */
-		  SUNXI_FUNCTION(0x4, "csi")),		/* MCLK1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0")),		/* BCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0")),		/* LRCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0")),		/* DO0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DO1 */
-		  SUNXI_FUNCTION(0x3, "uart3")),	/* RTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DO2 */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "i2c3")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DO3 */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "i2c3")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "i2s0")),		/* DI */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* WE */
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* MOSI */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* ALE */
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* MISO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* CLE */
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* CE1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* CE0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* RE */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* RB0 */
-		  SUNXI_FUNCTION(0x3, "mmc2"),		/* CMD */
-		  SUNXI_FUNCTION(0x4, "mmc3")),		/* CMD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* RB1 */
-		  SUNXI_FUNCTION(0x3, "mmc2"),		/* CLK */
-		  SUNXI_FUNCTION(0x4, "mmc3")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ0 */
-		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D0 */
-		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ1 */
-		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ2 */
-		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D2 */
-		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ3 */
-		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D3 */
-		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ4 */
-		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D4 */
-		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ5 */
-		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D5 */
-		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ6 */
-		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D6 */
-		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ7 */
-		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D7 */
-		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D7 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ8 */
-		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ9 */
-		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ10 */
-		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ11 */
-		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ12 */
-		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ13 */
-		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC22,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ14 */
-		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC23,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ15 */
-		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ7 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC24,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQS */
-		  SUNXI_FUNCTION(0x3, "mmc2"),		/* RST */
-		  SUNXI_FUNCTION(0x4, "mmc3")),		/* RST */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC25,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* CE2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC26,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* CE3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC27,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* CS0 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D0 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D1 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D2 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D3 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D4 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D5 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D6 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VPC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D7 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D8 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D9 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D10 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D11 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D12 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D13 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D14 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D15 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D16 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VPC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D17 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D18 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D19 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D20 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D21 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD22,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D22 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD23,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D23 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD24,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD25,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* DE */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD26,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* HSYNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD27,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* VSYNC */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* PCLK */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* MCLK */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* ERR */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* HSYNC */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* SYNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* VSYNC */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* DVLD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D0 */
-		  SUNXI_FUNCTION(0x3, "uart5")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D1 */
-		  SUNXI_FUNCTION(0x3, "uart5")),	/* RX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D2 */
-		  SUNXI_FUNCTION(0x3, "uart5")),	/* RTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D3 */
-		  SUNXI_FUNCTION(0x3, "uart5")),	/* CTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D4 */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D5 */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D6 */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D7 */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D8 */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* D4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D9 */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* D5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D10 */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* D6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D11 */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* D7 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi")),		/* MIPI CSI MCLK */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "jtag")),		/* MS1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D0 */
-		  SUNXI_FUNCTION(0x4, "jtag")),		/* DI1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CLK */
-		  SUNXI_FUNCTION(0x4, "uart0")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CMD */
-		  SUNXI_FUNCTION(0x4, "jtag")),		/* DO1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D3 */
-		  SUNXI_FUNCTION(0x4, "uart0")),	/* RX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D2 */
-		  SUNXI_FUNCTION(0x4, "jtag")),		/* CK1 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc1")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc1")),		/* CMD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc1")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc1")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc1")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc1")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart2")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart2")),	/* RX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart2")),	/* RTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart2")),	/* CTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c3"),		/* SCK */
-		  SUNXI_FUNCTION(0x3, "usb")),		/* DP3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c3"),		/* SDA */
-		  SUNXI_FUNCTION(0x3, "usb")),		/* DM3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS1 */
-		  SUNXI_FUNCTION(0x3, "i2s1")),		/* MCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS0 */
-		  SUNXI_FUNCTION(0x3, "i2s1")),		/* BCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "i2s1")),		/* LRCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
-		  SUNXI_FUNCTION(0x3, "i2s1")),		/* DIN */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
-		  SUNXI_FUNCTION(0x3, "i2s1")),		/* DOUT */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart4")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart4")),	/* RX */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand1")),	/* WE */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand1")),	/* ALE */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand1")),	/* CLE */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand1")),	/* CE1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand1")),	/* CE0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand1")),	/* RE */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand1")),	/* RB0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand1")),	/* RB1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand1")),	/* DQS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS0 */
-		  SUNXI_FUNCTION(0x3, "jtag"),		/* MS0 */
-		  SUNXI_FUNCTION(0x4, "pwm1")),		/* Positive */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "jtag"),		/* CK0 */
-		  SUNXI_FUNCTION(0x4, "pwm1")),		/* Negative */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* MOSI */
-		  SUNXI_FUNCTION(0x3, "jtag"),		/* DO0 */
-		  SUNXI_FUNCTION(0x4, "pwm2")),		/* Positive */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* MISO */
-		  SUNXI_FUNCTION(0x3, "jtag"),		/* DI0 */
-		  SUNXI_FUNCTION(0x4, "pwm2")),		/* Negative */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "pwm0")),
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart0")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart0")),	/* RX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH22,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out")),
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH23,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out")),
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH24,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out")),
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH25,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out")),
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH26,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out")),
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH27,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out")),
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH28,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out")),
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH29,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand1")),	/* CE2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH30,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand1")),	/* CE3 */
-};
-
-static const struct sunxi_desc_pin sun7i_a20_pins[] = {
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD3 */
-		  SUNXI_FUNCTION(0x3, "spi1"),		/* CS0 */
-		  SUNXI_FUNCTION(0x4, "uart2"),		/* RTS */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXD3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD2 */
-		  SUNXI_FUNCTION(0x3, "spi1"),		/* CLK */
-		  SUNXI_FUNCTION(0x4, "uart2"),		/* CTS */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXD2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD1 */
-		  SUNXI_FUNCTION(0x3, "spi1"),		/* MOSI */
-		  SUNXI_FUNCTION(0x4, "uart2"),		/* TX */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXD1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD0 */
-		  SUNXI_FUNCTION(0x3, "spi1"),		/* MISO */
-		  SUNXI_FUNCTION(0x4, "uart2"),		/* RX */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXD0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD3 */
-		  SUNXI_FUNCTION(0x3, "spi1"),		/* CS1 */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GTXD3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD2 */
-		  SUNXI_FUNCTION(0x3, "spi3"),		/* CS0 */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GTXD2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD1 */
-		  SUNXI_FUNCTION(0x3, "spi3"),		/* CLK */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GTXD1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD0 */
-		  SUNXI_FUNCTION(0x3, "spi3"),		/* MOSI */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GTXD0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXCK */
-		  SUNXI_FUNCTION(0x3, "spi3"),		/* MISO */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXERR */
-		  SUNXI_FUNCTION(0x3, "spi3"),		/* CS1 */
-		  SUNXI_FUNCTION(0x5, "gmac"),		/* GNULL / ERXERR */
-		  SUNXI_FUNCTION(0x6, "i2s1")),		/* MCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXDV */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* TX */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXCTL / ERXDV */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* EMDC */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* RX */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* EMDC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* EMDIO */
-		  SUNXI_FUNCTION(0x3, "uart6"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* RTS */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* EMDIO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXEN */
-		  SUNXI_FUNCTION(0x3, "uart6"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* CTS */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GTXCTL / ETXEN */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXCK */
-		  SUNXI_FUNCTION(0x3, "uart7"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* DTR */
-		  SUNXI_FUNCTION(0x5, "gmac"),		/* GNULL / ETXCK */
-		  SUNXI_FUNCTION(0x6, "i2s1")),		/* BCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ECRS */
-		  SUNXI_FUNCTION(0x3, "uart7"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* DSR */
-		  SUNXI_FUNCTION(0x5, "gmac"),		/* GTXCK / ECRS */
-		  SUNXI_FUNCTION(0x6, "i2s1")),		/* LRCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ECOL */
-		  SUNXI_FUNCTION(0x3, "can"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* DCD */
-		  SUNXI_FUNCTION(0x5, "gmac"),		/* GCLKIN / ECOL */
-		  SUNXI_FUNCTION(0x6, "i2s1")),		/* DO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXERR */
-		  SUNXI_FUNCTION(0x3, "can"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* RING */
-		  SUNXI_FUNCTION(0x5, "gmac"),		/* GNULL / ETXERR */
-		  SUNXI_FUNCTION(0x6, "i2s1")),		/* LRCK */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "pwm")),		/* PWM0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ir0"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "spdif")),	/* MCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ir0")),		/* RX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0"),		/* MCLK */
-		  SUNXI_FUNCTION(0x3, "ac97")),		/* MCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0"),		/* BCLK */
-		  SUNXI_FUNCTION(0x3, "ac97")),		/* BCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0"),		/* LRCK */
-		  SUNXI_FUNCTION(0x3, "ac97")),		/* SYNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DO0 */
-		  SUNXI_FUNCTION(0x3, "ac97")),		/* DO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0")),		/* DO1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0")),		/* DO2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0")),		/* DO3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DI */
-		  SUNXI_FUNCTION(0x3, "ac97"),		/* DI */
-		  SUNXI_FUNCTION(0x4, "spdif")),	/* DI */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS1 */
-		  SUNXI_FUNCTION(0x4, "spdif")),	/* DO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS0 */
-		  SUNXI_FUNCTION(0x3, "jtag")),		/* MS0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "jtag")),		/* CK0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* MOSI */
-		  SUNXI_FUNCTION(0x3, "jtag")),		/* DO0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* MISO */
-		  SUNXI_FUNCTION(0x3, "jtag")),		/* DI0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB22,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart0"),		/* TX */
-		  SUNXI_FUNCTION(0x3, "ir1")),		/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB23,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart0"),		/* RX */
-		  SUNXI_FUNCTION(0x3, "ir1")),		/* RX */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NWE */
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* MOSI */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NALE */
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* MISO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCLE */
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NRE# */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB0 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CMD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB1 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ0 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ1 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ2 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ3 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ7 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NWP */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE4 */
-		  SUNXI_FUNCTION(0x3, "spi2"),		/* CS0 */
-		  SUNXI_FUNCTION_IRQ(0x6, 12)),		/* EINT12 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE5 */
-		  SUNXI_FUNCTION(0x3, "spi2"),		/* CLK */
-		  SUNXI_FUNCTION_IRQ(0x6, 13)),		/* EINT13 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE6 */
-		  SUNXI_FUNCTION(0x3, "spi2"),		/* MOSI */
-		  SUNXI_FUNCTION_IRQ(0x6, 14)),		/* EINT14 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC22,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE7 */
-		  SUNXI_FUNCTION(0x3, "spi2"),		/* MISO */
-		  SUNXI_FUNCTION_IRQ(0x6, 15)),		/* EINT15 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC23,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* CS0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC24,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQS */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D0 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D1 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D2 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D3 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D4 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D5 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D6 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VPC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D7 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D8 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D9 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VM3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D10 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D11 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D12 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D13 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D14 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D15 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D16 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VPC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D17 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D18 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D19 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D20 */
-		  SUNXI_FUNCTION(0x3, "csi1")),		/* MCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D21 */
-		  SUNXI_FUNCTION(0x3, "sim")),		/* VPPEN */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD22,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D22 */
-		  SUNXI_FUNCTION(0x3, "sim")),		/* VPPPP */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD23,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D23 */
-		  SUNXI_FUNCTION(0x3, "sim")),		/* DET */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD24,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "sim")),		/* VCCEN */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD25,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* DE */
-		  SUNXI_FUNCTION(0x3, "sim")),		/* RST */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD26,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* HSYNC */
-		  SUNXI_FUNCTION(0x3, "sim")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD27,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* VSYNC */
-		  SUNXI_FUNCTION(0x3, "sim")),		/* SDA */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* PCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* ERR */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* CK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* SYNC */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* HSYNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* DVLD */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* VSYNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D0 */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D1 */
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "sim")),		/* VPPEN */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D2 */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D3 */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D4 */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* D4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D5 */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* D5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D6 */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* D6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D7 */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* D7 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "jtag")),		/* MSI */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D0 */
-		  SUNXI_FUNCTION(0x4, "jtag")),		/* DI1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CLK */
-		  SUNXI_FUNCTION(0x4, "uart0")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CMD */
-		  SUNXI_FUNCTION(0x4, "jtag")),		/* DO1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D3 */
-		  SUNXI_FUNCTION(0x4, "uart0")),	/* RX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D2 */
-		  SUNXI_FUNCTION(0x4, "jtag")),		/* CK1 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* PCK */
-		  SUNXI_FUNCTION(0x4, "mmc1")),		/* CMD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* ERR */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* CK */
-		  SUNXI_FUNCTION(0x4, "mmc1")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* SYNC */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* HSYNC */
-		  SUNXI_FUNCTION(0x4, "mmc1")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* DVLD */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* VSYNC */
-		  SUNXI_FUNCTION(0x4, "mmc1")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D0 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D0 */
-		  SUNXI_FUNCTION(0x4, "mmc1"),		/* D2 */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D8 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D1 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "mmc1"),		/* D3 */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D9 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D2 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D2 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* TX */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D10 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D3 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D3 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* RX */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D11 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D4 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D4 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* RTS */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D12 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D5 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D5 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* CTS */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D13 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D6 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D6 */
-		  SUNXI_FUNCTION(0x4, "uart4"),		/* TX */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D14 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D7 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D7 */
-		  SUNXI_FUNCTION(0x4, "uart4"),		/* RX */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D15 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D0 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 0),		/* EINT0 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 1),		/* EINT1 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D2 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* RTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 2),		/* EINT2 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D3 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* CTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 3),		/* EINT3 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D4 */
-		  SUNXI_FUNCTION(0x4, "uart4"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 4),		/* EINT4 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D5 */
-		  SUNXI_FUNCTION(0x4, "uart4"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 5),		/* EINT5 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D6 */
-		  SUNXI_FUNCTION(0x4, "uart5"),		/* TX */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* BS */
-		  SUNXI_FUNCTION_IRQ(0x6, 6),		/* EINT6 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D7 */
-		  SUNXI_FUNCTION(0x4, "uart5"),		/* RX */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* CLK */
-		  SUNXI_FUNCTION_IRQ(0x6, 7),		/* EINT7 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D7 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D8 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXD3 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN0 */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* D0 */
-		  SUNXI_FUNCTION_IRQ(0x6, 8),		/* EINT8 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D8 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D9 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXD2 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN1 */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* D1 */
-		  SUNXI_FUNCTION_IRQ(0x6, 9),		/* EINT9 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D9 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D10 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXD1 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN2 */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* D2 */
-		  SUNXI_FUNCTION_IRQ(0x6, 10),		/* EINT10 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D10 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D11 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXD0 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN3 */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* D3 */
-		  SUNXI_FUNCTION_IRQ(0x6, 11),		/* EINT11 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D11 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D12 */
-		  SUNXI_FUNCTION(0x4, "ps2"),		/* SCK1 */
-		  SUNXI_FUNCTION_IRQ(0x6, 12),		/* EINT12 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D12 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D13 */
-		  SUNXI_FUNCTION(0x4, "ps2"),		/* SDA1 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* RST */
-		  SUNXI_FUNCTION_IRQ(0x6, 13),		/* EINT13 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D13 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D14 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD3 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN4 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* VPPEN */
-		  SUNXI_FUNCTION_IRQ(0x6, 14),		/* EINT14 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D14 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D15 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD3 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN5 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* VPPPP */
-		  SUNXI_FUNCTION_IRQ(0x6, 15),		/* EINT15 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D15 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D16 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD2 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN6 */
-		  SUNXI_FUNCTION_IRQ(0x6, 16),		/* EINT16 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D16 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D17 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD1 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN7 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* VCCEN */
-		  SUNXI_FUNCTION_IRQ(0x6, 17),		/* EINT17 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D17 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D18 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD0 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT0 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* SCK */
-		  SUNXI_FUNCTION_IRQ(0x6, 18),		/* EINT18 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D18 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D19 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXERR */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT1 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* SDA */
-		  SUNXI_FUNCTION_IRQ(0x6, 19),		/* EINT19 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D19 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D20 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXDV */
-		  SUNXI_FUNCTION(0x4, "can"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 20),		/* EINT20 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D20 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D21 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* EMDC */
-		  SUNXI_FUNCTION(0x4, "can"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 21),		/* EINT21 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D21 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH22,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D22 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* EMDIO */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT2 */
-		  SUNXI_FUNCTION(0x5, "mmc1"),		/* CMD */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D22 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH23,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D23 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXEN */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT3 */
-		  SUNXI_FUNCTION(0x5, "mmc1"),		/* CLK */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D23 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH24,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXCK */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT4 */
-		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D0 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* PCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH25,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* DE */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ECRS */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT5 */
-		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D1 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* FIELD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH26,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* HSYNC */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ECOL */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT6 */
-		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D2 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* HSYNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH27,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* VSYNC */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXERR */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT7 */
-		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D3 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* VSYNC */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "i2c3")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "i2c3")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "i2c4")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "pwm"),		/* PWM1 */
-		  SUNXI_FUNCTION(0x3, "i2c4")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc3")),		/* CMD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc3")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* CS0 */
-		  SUNXI_FUNCTION(0x3, "uart5"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x5, 22)),		/* EINT22 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "uart5"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x5, 23)),		/* EINT23 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* MOSI */
-		  SUNXI_FUNCTION(0x3, "uart6"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "clk_out_a"),	/* CLK_OUT_A */
-		  SUNXI_FUNCTION_IRQ(0x5, 24)),		/* EINT24 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* MISO */
-		  SUNXI_FUNCTION(0x3, "uart6"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "clk_out_b"),	/* CLK_OUT_B */
-		  SUNXI_FUNCTION_IRQ(0x5, 25)),		/* EINT25 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* CS1 */
-		  SUNXI_FUNCTION(0x3, "ps2"),		/* SCK1 */
-		  SUNXI_FUNCTION(0x4, "timer4"),	/* TCLKIN0 */
-		  SUNXI_FUNCTION_IRQ(0x5, 26)),		/* EINT26 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS1 */
-		  SUNXI_FUNCTION(0x3, "ps2"),		/* SDA1 */
-		  SUNXI_FUNCTION(0x4, "timer5"),	/* TCLKIN1 */
-		  SUNXI_FUNCTION_IRQ(0x5, 27)),		/* EINT27 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS0 */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* RTS */
-		  SUNXI_FUNCTION_IRQ(0x5, 28)),		/* EINT28 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* CTS */
-		  SUNXI_FUNCTION_IRQ(0x5, 29)),		/* EINT29 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x5, 30)),		/* EINT30 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x5, 31)),		/* EINT31 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ps2"),		/* SCK0 */
-		  SUNXI_FUNCTION(0x3, "uart7"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "hdmi")),		/* HSCL */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ps2"),		/* SDA0 */
-		  SUNXI_FUNCTION(0x3, "uart7"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "hdmi")),		/* HSDA */
-};
-
-static const struct sunxi_pinctrl_desc sun4i_a10_pinctrl_data = {
-	.pins = sun4i_a10_pins,
-	.npins = ARRAY_SIZE(sun4i_a10_pins),
-};
-
-static const struct sunxi_pinctrl_desc sun5i_a10s_pinctrl_data = {
-	.pins = sun5i_a10s_pins,
-	.npins = ARRAY_SIZE(sun5i_a10s_pins),
-};
-
-static const struct sunxi_pinctrl_desc sun5i_a13_pinctrl_data = {
-	.pins = sun5i_a13_pins,
-	.npins = ARRAY_SIZE(sun5i_a13_pins),
-};
-
-static const struct sunxi_pinctrl_desc sun6i_a31_pinctrl_data = {
-	.pins = sun6i_a31_pins,
-	.npins = ARRAY_SIZE(sun6i_a31_pins),
-};
-
-static const struct sunxi_pinctrl_desc sun7i_a20_pinctrl_data = {
-	.pins = sun7i_a20_pins,
-	.npins = ARRAY_SIZE(sun7i_a20_pins),
-};
-
-#endif /* __PINCTRL_SUNXI_PINS_H */
diff --git a/drivers/pinctrl/pinctrl-sunxi.h b/drivers/pinctrl/pinctrl-sunxi.h
deleted file mode 100644
index 552b0e97077..00000000000
--- a/drivers/pinctrl/pinctrl-sunxi.h
+++ /dev/null
@@ -1,548 +0,0 @@
-/*
- * Allwinner A1X SoCs pinctrl driver.
- *
- * Copyright (C) 2012 Maxime Ripard
- *
- * Maxime Ripard <maxime.ripard@free-electrons.com>
- *
- * This file is licensed under the terms of the GNU General Public
- * License version 2.  This program is licensed "as is" without any
- * warranty of any kind, whether express or implied.
- */
-
-#ifndef __PINCTRL_SUNXI_H
-#define __PINCTRL_SUNXI_H
-
-#include <linux/kernel.h>
-#include <linux/spinlock.h>
-
-#define PA_BASE	0
-#define PB_BASE	32
-#define PC_BASE	64
-#define PD_BASE	96
-#define PE_BASE	128
-#define PF_BASE	160
-#define PG_BASE	192
-#define PH_BASE	224
-#define PI_BASE	256
-
-#define SUNXI_PINCTRL_PIN_PA0	PINCTRL_PIN(PA_BASE + 0, "PA0")
-#define SUNXI_PINCTRL_PIN_PA1	PINCTRL_PIN(PA_BASE + 1, "PA1")
-#define SUNXI_PINCTRL_PIN_PA2	PINCTRL_PIN(PA_BASE + 2, "PA2")
-#define SUNXI_PINCTRL_PIN_PA3	PINCTRL_PIN(PA_BASE + 3, "PA3")
-#define SUNXI_PINCTRL_PIN_PA4	PINCTRL_PIN(PA_BASE + 4, "PA4")
-#define SUNXI_PINCTRL_PIN_PA5	PINCTRL_PIN(PA_BASE + 5, "PA5")
-#define SUNXI_PINCTRL_PIN_PA6	PINCTRL_PIN(PA_BASE + 6, "PA6")
-#define SUNXI_PINCTRL_PIN_PA7	PINCTRL_PIN(PA_BASE + 7, "PA7")
-#define SUNXI_PINCTRL_PIN_PA8	PINCTRL_PIN(PA_BASE + 8, "PA8")
-#define SUNXI_PINCTRL_PIN_PA9	PINCTRL_PIN(PA_BASE + 9, "PA9")
-#define SUNXI_PINCTRL_PIN_PA10	PINCTRL_PIN(PA_BASE + 10, "PA10")
-#define SUNXI_PINCTRL_PIN_PA11	PINCTRL_PIN(PA_BASE + 11, "PA11")
-#define SUNXI_PINCTRL_PIN_PA12	PINCTRL_PIN(PA_BASE + 12, "PA12")
-#define SUNXI_PINCTRL_PIN_PA13	PINCTRL_PIN(PA_BASE + 13, "PA13")
-#define SUNXI_PINCTRL_PIN_PA14	PINCTRL_PIN(PA_BASE + 14, "PA14")
-#define SUNXI_PINCTRL_PIN_PA15	PINCTRL_PIN(PA_BASE + 15, "PA15")
-#define SUNXI_PINCTRL_PIN_PA16	PINCTRL_PIN(PA_BASE + 16, "PA16")
-#define SUNXI_PINCTRL_PIN_PA17	PINCTRL_PIN(PA_BASE + 17, "PA17")
-#define SUNXI_PINCTRL_PIN_PA18	PINCTRL_PIN(PA_BASE + 18, "PA18")
-#define SUNXI_PINCTRL_PIN_PA19	PINCTRL_PIN(PA_BASE + 19, "PA19")
-#define SUNXI_PINCTRL_PIN_PA20	PINCTRL_PIN(PA_BASE + 20, "PA20")
-#define SUNXI_PINCTRL_PIN_PA21	PINCTRL_PIN(PA_BASE + 21, "PA21")
-#define SUNXI_PINCTRL_PIN_PA22	PINCTRL_PIN(PA_BASE + 22, "PA22")
-#define SUNXI_PINCTRL_PIN_PA23	PINCTRL_PIN(PA_BASE + 23, "PA23")
-#define SUNXI_PINCTRL_PIN_PA24	PINCTRL_PIN(PA_BASE + 24, "PA24")
-#define SUNXI_PINCTRL_PIN_PA25	PINCTRL_PIN(PA_BASE + 25, "PA25")
-#define SUNXI_PINCTRL_PIN_PA26	PINCTRL_PIN(PA_BASE + 26, "PA26")
-#define SUNXI_PINCTRL_PIN_PA27	PINCTRL_PIN(PA_BASE + 27, "PA27")
-#define SUNXI_PINCTRL_PIN_PA28	PINCTRL_PIN(PA_BASE + 28, "PA28")
-#define SUNXI_PINCTRL_PIN_PA29	PINCTRL_PIN(PA_BASE + 29, "PA29")
-#define SUNXI_PINCTRL_PIN_PA30	PINCTRL_PIN(PA_BASE + 30, "PA30")
-#define SUNXI_PINCTRL_PIN_PA31	PINCTRL_PIN(PA_BASE + 31, "PA31")
-
-#define SUNXI_PINCTRL_PIN_PB0	PINCTRL_PIN(PB_BASE + 0, "PB0")
-#define SUNXI_PINCTRL_PIN_PB1	PINCTRL_PIN(PB_BASE + 1, "PB1")
-#define SUNXI_PINCTRL_PIN_PB2	PINCTRL_PIN(PB_BASE + 2, "PB2")
-#define SUNXI_PINCTRL_PIN_PB3	PINCTRL_PIN(PB_BASE + 3, "PB3")
-#define SUNXI_PINCTRL_PIN_PB4	PINCTRL_PIN(PB_BASE + 4, "PB4")
-#define SUNXI_PINCTRL_PIN_PB5	PINCTRL_PIN(PB_BASE + 5, "PB5")
-#define SUNXI_PINCTRL_PIN_PB6	PINCTRL_PIN(PB_BASE + 6, "PB6")
-#define SUNXI_PINCTRL_PIN_PB7	PINCTRL_PIN(PB_BASE + 7, "PB7")
-#define SUNXI_PINCTRL_PIN_PB8	PINCTRL_PIN(PB_BASE + 8, "PB8")
-#define SUNXI_PINCTRL_PIN_PB9	PINCTRL_PIN(PB_BASE + 9, "PB9")
-#define SUNXI_PINCTRL_PIN_PB10	PINCTRL_PIN(PB_BASE + 10, "PB10")
-#define SUNXI_PINCTRL_PIN_PB11	PINCTRL_PIN(PB_BASE + 11, "PB11")
-#define SUNXI_PINCTRL_PIN_PB12	PINCTRL_PIN(PB_BASE + 12, "PB12")
-#define SUNXI_PINCTRL_PIN_PB13	PINCTRL_PIN(PB_BASE + 13, "PB13")
-#define SUNXI_PINCTRL_PIN_PB14	PINCTRL_PIN(PB_BASE + 14, "PB14")
-#define SUNXI_PINCTRL_PIN_PB15	PINCTRL_PIN(PB_BASE + 15, "PB15")
-#define SUNXI_PINCTRL_PIN_PB16	PINCTRL_PIN(PB_BASE + 16, "PB16")
-#define SUNXI_PINCTRL_PIN_PB17	PINCTRL_PIN(PB_BASE + 17, "PB17")
-#define SUNXI_PINCTRL_PIN_PB18	PINCTRL_PIN(PB_BASE + 18, "PB18")
-#define SUNXI_PINCTRL_PIN_PB19	PINCTRL_PIN(PB_BASE + 19, "PB19")
-#define SUNXI_PINCTRL_PIN_PB20	PINCTRL_PIN(PB_BASE + 20, "PB20")
-#define SUNXI_PINCTRL_PIN_PB21	PINCTRL_PIN(PB_BASE + 21, "PB21")
-#define SUNXI_PINCTRL_PIN_PB22	PINCTRL_PIN(PB_BASE + 22, "PB22")
-#define SUNXI_PINCTRL_PIN_PB23	PINCTRL_PIN(PB_BASE + 23, "PB23")
-#define SUNXI_PINCTRL_PIN_PB24	PINCTRL_PIN(PB_BASE + 24, "PB24")
-#define SUNXI_PINCTRL_PIN_PB25	PINCTRL_PIN(PB_BASE + 25, "PB25")
-#define SUNXI_PINCTRL_PIN_PB26	PINCTRL_PIN(PB_BASE + 26, "PB26")
-#define SUNXI_PINCTRL_PIN_PB27	PINCTRL_PIN(PB_BASE + 27, "PB27")
-#define SUNXI_PINCTRL_PIN_PB28	PINCTRL_PIN(PB_BASE + 28, "PB28")
-#define SUNXI_PINCTRL_PIN_PB29	PINCTRL_PIN(PB_BASE + 29, "PB29")
-#define SUNXI_PINCTRL_PIN_PB30	PINCTRL_PIN(PB_BASE + 30, "PB30")
-#define SUNXI_PINCTRL_PIN_PB31	PINCTRL_PIN(PB_BASE + 31, "PB31")
-
-#define SUNXI_PINCTRL_PIN_PC0	PINCTRL_PIN(PC_BASE + 0, "PC0")
-#define SUNXI_PINCTRL_PIN_PC1	PINCTRL_PIN(PC_BASE + 1, "PC1")
-#define SUNXI_PINCTRL_PIN_PC2	PINCTRL_PIN(PC_BASE + 2, "PC2")
-#define SUNXI_PINCTRL_PIN_PC3	PINCTRL_PIN(PC_BASE + 3, "PC3")
-#define SUNXI_PINCTRL_PIN_PC4	PINCTRL_PIN(PC_BASE + 4, "PC4")
-#define SUNXI_PINCTRL_PIN_PC5	PINCTRL_PIN(PC_BASE + 5, "PC5")
-#define SUNXI_PINCTRL_PIN_PC6	PINCTRL_PIN(PC_BASE + 6, "PC6")
-#define SUNXI_PINCTRL_PIN_PC7	PINCTRL_PIN(PC_BASE + 7, "PC7")
-#define SUNXI_PINCTRL_PIN_PC8	PINCTRL_PIN(PC_BASE + 8, "PC8")
-#define SUNXI_PINCTRL_PIN_PC9	PINCTRL_PIN(PC_BASE + 9, "PC9")
-#define SUNXI_PINCTRL_PIN_PC10	PINCTRL_PIN(PC_BASE + 10, "PC10")
-#define SUNXI_PINCTRL_PIN_PC11	PINCTRL_PIN(PC_BASE + 11, "PC11")
-#define SUNXI_PINCTRL_PIN_PC12	PINCTRL_PIN(PC_BASE + 12, "PC12")
-#define SUNXI_PINCTRL_PIN_PC13	PINCTRL_PIN(PC_BASE + 13, "PC13")
-#define SUNXI_PINCTRL_PIN_PC14	PINCTRL_PIN(PC_BASE + 14, "PC14")
-#define SUNXI_PINCTRL_PIN_PC15	PINCTRL_PIN(PC_BASE + 15, "PC15")
-#define SUNXI_PINCTRL_PIN_PC16	PINCTRL_PIN(PC_BASE + 16, "PC16")
-#define SUNXI_PINCTRL_PIN_PC17	PINCTRL_PIN(PC_BASE + 17, "PC17")
-#define SUNXI_PINCTRL_PIN_PC18	PINCTRL_PIN(PC_BASE + 18, "PC18")
-#define SUNXI_PINCTRL_PIN_PC19	PINCTRL_PIN(PC_BASE + 19, "PC19")
-#define SUNXI_PINCTRL_PIN_PC20	PINCTRL_PIN(PC_BASE + 20, "PC20")
-#define SUNXI_PINCTRL_PIN_PC21	PINCTRL_PIN(PC_BASE + 21, "PC21")
-#define SUNXI_PINCTRL_PIN_PC22	PINCTRL_PIN(PC_BASE + 22, "PC22")
-#define SUNXI_PINCTRL_PIN_PC23	PINCTRL_PIN(PC_BASE + 23, "PC23")
-#define SUNXI_PINCTRL_PIN_PC24	PINCTRL_PIN(PC_BASE + 24, "PC24")
-#define SUNXI_PINCTRL_PIN_PC25	PINCTRL_PIN(PC_BASE + 25, "PC25")
-#define SUNXI_PINCTRL_PIN_PC26	PINCTRL_PIN(PC_BASE + 26, "PC26")
-#define SUNXI_PINCTRL_PIN_PC27	PINCTRL_PIN(PC_BASE + 27, "PC27")
-#define SUNXI_PINCTRL_PIN_PC28	PINCTRL_PIN(PC_BASE + 28, "PC28")
-#define SUNXI_PINCTRL_PIN_PC29	PINCTRL_PIN(PC_BASE + 29, "PC29")
-#define SUNXI_PINCTRL_PIN_PC30	PINCTRL_PIN(PC_BASE + 30, "PC30")
-#define SUNXI_PINCTRL_PIN_PC31	PINCTRL_PIN(PC_BASE + 31, "PC31")
-
-#define SUNXI_PINCTRL_PIN_PD0	PINCTRL_PIN(PD_BASE + 0, "PD0")
-#define SUNXI_PINCTRL_PIN_PD1	PINCTRL_PIN(PD_BASE + 1, "PD1")
-#define SUNXI_PINCTRL_PIN_PD2	PINCTRL_PIN(PD_BASE + 2, "PD2")
-#define SUNXI_PINCTRL_PIN_PD3	PINCTRL_PIN(PD_BASE + 3, "PD3")
-#define SUNXI_PINCTRL_PIN_PD4	PINCTRL_PIN(PD_BASE + 4, "PD4")
-#define SUNXI_PINCTRL_PIN_PD5	PINCTRL_PIN(PD_BASE + 5, "PD5")
-#define SUNXI_PINCTRL_PIN_PD6	PINCTRL_PIN(PD_BASE + 6, "PD6")
-#define SUNXI_PINCTRL_PIN_PD7	PINCTRL_PIN(PD_BASE + 7, "PD7")
-#define SUNXI_PINCTRL_PIN_PD8	PINCTRL_PIN(PD_BASE + 8, "PD8")
-#define SUNXI_PINCTRL_PIN_PD9	PINCTRL_PIN(PD_BASE + 9, "PD9")
-#define SUNXI_PINCTRL_PIN_PD10	PINCTRL_PIN(PD_BASE + 10, "PD10")
-#define SUNXI_PINCTRL_PIN_PD11	PINCTRL_PIN(PD_BASE + 11, "PD11")
-#define SUNXI_PINCTRL_PIN_PD12	PINCTRL_PIN(PD_BASE + 12, "PD12")
-#define SUNXI_PINCTRL_PIN_PD13	PINCTRL_PIN(PD_BASE + 13, "PD13")
-#define SUNXI_PINCTRL_PIN_PD14	PINCTRL_PIN(PD_BASE + 14, "PD14")
-#define SUNXI_PINCTRL_PIN_PD15	PINCTRL_PIN(PD_BASE + 15, "PD15")
-#define SUNXI_PINCTRL_PIN_PD16	PINCTRL_PIN(PD_BASE + 16, "PD16")
-#define SUNXI_PINCTRL_PIN_PD17	PINCTRL_PIN(PD_BASE + 17, "PD17")
-#define SUNXI_PINCTRL_PIN_PD18	PINCTRL_PIN(PD_BASE + 18, "PD18")
-#define SUNXI_PINCTRL_PIN_PD19	PINCTRL_PIN(PD_BASE + 19, "PD19")
-#define SUNXI_PINCTRL_PIN_PD20	PINCTRL_PIN(PD_BASE + 20, "PD20")
-#define SUNXI_PINCTRL_PIN_PD21	PINCTRL_PIN(PD_BASE + 21, "PD21")
-#define SUNXI_PINCTRL_PIN_PD22	PINCTRL_PIN(PD_BASE + 22, "PD22")
-#define SUNXI_PINCTRL_PIN_PD23	PINCTRL_PIN(PD_BASE + 23, "PD23")
-#define SUNXI_PINCTRL_PIN_PD24	PINCTRL_PIN(PD_BASE + 24, "PD24")
-#define SUNXI_PINCTRL_PIN_PD25	PINCTRL_PIN(PD_BASE + 25, "PD25")
-#define SUNXI_PINCTRL_PIN_PD26	PINCTRL_PIN(PD_BASE + 26, "PD26")
-#define SUNXI_PINCTRL_PIN_PD27	PINCTRL_PIN(PD_BASE + 27, "PD27")
-#define SUNXI_PINCTRL_PIN_PD28	PINCTRL_PIN(PD_BASE + 28, "PD28")
-#define SUNXI_PINCTRL_PIN_PD29	PINCTRL_PIN(PD_BASE + 29, "PD29")
-#define SUNXI_PINCTRL_PIN_PD30	PINCTRL_PIN(PD_BASE + 30, "PD30")
-#define SUNXI_PINCTRL_PIN_PD31	PINCTRL_PIN(PD_BASE + 31, "PD31")
-
-#define SUNXI_PINCTRL_PIN_PE0	PINCTRL_PIN(PE_BASE + 0, "PE0")
-#define SUNXI_PINCTRL_PIN_PE1	PINCTRL_PIN(PE_BASE + 1, "PE1")
-#define SUNXI_PINCTRL_PIN_PE2	PINCTRL_PIN(PE_BASE + 2, "PE2")
-#define SUNXI_PINCTRL_PIN_PE3	PINCTRL_PIN(PE_BASE + 3, "PE3")
-#define SUNXI_PINCTRL_PIN_PE4	PINCTRL_PIN(PE_BASE + 4, "PE4")
-#define SUNXI_PINCTRL_PIN_PE5	PINCTRL_PIN(PE_BASE + 5, "PE5")
-#define SUNXI_PINCTRL_PIN_PE6	PINCTRL_PIN(PE_BASE + 6, "PE6")
-#define SUNXI_PINCTRL_PIN_PE7	PINCTRL_PIN(PE_BASE + 7, "PE7")
-#define SUNXI_PINCTRL_PIN_PE8	PINCTRL_PIN(PE_BASE + 8, "PE8")
-#define SUNXI_PINCTRL_PIN_PE9	PINCTRL_PIN(PE_BASE + 9, "PE9")
-#define SUNXI_PINCTRL_PIN_PE10	PINCTRL_PIN(PE_BASE + 10, "PE10")
-#define SUNXI_PINCTRL_PIN_PE11	PINCTRL_PIN(PE_BASE + 11, "PE11")
-#define SUNXI_PINCTRL_PIN_PE12	PINCTRL_PIN(PE_BASE + 12, "PE12")
-#define SUNXI_PINCTRL_PIN_PE13	PINCTRL_PIN(PE_BASE + 13, "PE13")
-#define SUNXI_PINCTRL_PIN_PE14	PINCTRL_PIN(PE_BASE + 14, "PE14")
-#define SUNXI_PINCTRL_PIN_PE15	PINCTRL_PIN(PE_BASE + 15, "PE15")
-#define SUNXI_PINCTRL_PIN_PE16	PINCTRL_PIN(PE_BASE + 16, "PE16")
-#define SUNXI_PINCTRL_PIN_PE17	PINCTRL_PIN(PE_BASE + 17, "PE17")
-#define SUNXI_PINCTRL_PIN_PE18	PINCTRL_PIN(PE_BASE + 18, "PE18")
-#define SUNXI_PINCTRL_PIN_PE19	PINCTRL_PIN(PE_BASE + 19, "PE19")
-#define SUNXI_PINCTRL_PIN_PE20	PINCTRL_PIN(PE_BASE + 20, "PE20")
-#define SUNXI_PINCTRL_PIN_PE21	PINCTRL_PIN(PE_BASE + 21, "PE21")
-#define SUNXI_PINCTRL_PIN_PE22	PINCTRL_PIN(PE_BASE + 22, "PE22")
-#define SUNXI_PINCTRL_PIN_PE23	PINCTRL_PIN(PE_BASE + 23, "PE23")
-#define SUNXI_PINCTRL_PIN_PE24	PINCTRL_PIN(PE_BASE + 24, "PE24")
-#define SUNXI_PINCTRL_PIN_PE25	PINCTRL_PIN(PE_BASE + 25, "PE25")
-#define SUNXI_PINCTRL_PIN_PE26	PINCTRL_PIN(PE_BASE + 26, "PE26")
-#define SUNXI_PINCTRL_PIN_PE27	PINCTRL_PIN(PE_BASE + 27, "PE27")
-#define SUNXI_PINCTRL_PIN_PE28	PINCTRL_PIN(PE_BASE + 28, "PE28")
-#define SUNXI_PINCTRL_PIN_PE29	PINCTRL_PIN(PE_BASE + 29, "PE29")
-#define SUNXI_PINCTRL_PIN_PE30	PINCTRL_PIN(PE_BASE + 30, "PE30")
-#define SUNXI_PINCTRL_PIN_PE31	PINCTRL_PIN(PE_BASE + 31, "PE31")
-
-#define SUNXI_PINCTRL_PIN_PF0	PINCTRL_PIN(PF_BASE + 0, "PF0")
-#define SUNXI_PINCTRL_PIN_PF1	PINCTRL_PIN(PF_BASE + 1, "PF1")
-#define SUNXI_PINCTRL_PIN_PF2	PINCTRL_PIN(PF_BASE + 2, "PF2")
-#define SUNXI_PINCTRL_PIN_PF3	PINCTRL_PIN(PF_BASE + 3, "PF3")
-#define SUNXI_PINCTRL_PIN_PF4	PINCTRL_PIN(PF_BASE + 4, "PF4")
-#define SUNXI_PINCTRL_PIN_PF5	PINCTRL_PIN(PF_BASE + 5, "PF5")
-#define SUNXI_PINCTRL_PIN_PF6	PINCTRL_PIN(PF_BASE + 6, "PF6")
-#define SUNXI_PINCTRL_PIN_PF7	PINCTRL_PIN(PF_BASE + 7, "PF7")
-#define SUNXI_PINCTRL_PIN_PF8	PINCTRL_PIN(PF_BASE + 8, "PF8")
-#define SUNXI_PINCTRL_PIN_PF9	PINCTRL_PIN(PF_BASE + 9, "PF9")
-#define SUNXI_PINCTRL_PIN_PF10	PINCTRL_PIN(PF_BASE + 10, "PF10")
-#define SUNXI_PINCTRL_PIN_PF11	PINCTRL_PIN(PF_BASE + 11, "PF11")
-#define SUNXI_PINCTRL_PIN_PF12	PINCTRL_PIN(PF_BASE + 12, "PF12")
-#define SUNXI_PINCTRL_PIN_PF13	PINCTRL_PIN(PF_BASE + 13, "PF13")
-#define SUNXI_PINCTRL_PIN_PF14	PINCTRL_PIN(PF_BASE + 14, "PF14")
-#define SUNXI_PINCTRL_PIN_PF15	PINCTRL_PIN(PF_BASE + 15, "PF15")
-#define SUNXI_PINCTRL_PIN_PF16	PINCTRL_PIN(PF_BASE + 16, "PF16")
-#define SUNXI_PINCTRL_PIN_PF17	PINCTRL_PIN(PF_BASE + 17, "PF17")
-#define SUNXI_PINCTRL_PIN_PF18	PINCTRL_PIN(PF_BASE + 18, "PF18")
-#define SUNXI_PINCTRL_PIN_PF19	PINCTRL_PIN(PF_BASE + 19, "PF19")
-#define SUNXI_PINCTRL_PIN_PF20	PINCTRL_PIN(PF_BASE + 20, "PF20")
-#define SUNXI_PINCTRL_PIN_PF21	PINCTRL_PIN(PF_BASE + 21, "PF21")
-#define SUNXI_PINCTRL_PIN_PF22	PINCTRL_PIN(PF_BASE + 22, "PF22")
-#define SUNXI_PINCTRL_PIN_PF23	PINCTRL_PIN(PF_BASE + 23, "PF23")
-#define SUNXI_PINCTRL_PIN_PF24	PINCTRL_PIN(PF_BASE + 24, "PF24")
-#define SUNXI_PINCTRL_PIN_PF25	PINCTRL_PIN(PF_BASE + 25, "PF25")
-#define SUNXI_PINCTRL_PIN_PF26	PINCTRL_PIN(PF_BASE + 26, "PF26")
-#define SUNXI_PINCTRL_PIN_PF27	PINCTRL_PIN(PF_BASE + 27, "PF27")
-#define SUNXI_PINCTRL_PIN_PF28	PINCTRL_PIN(PF_BASE + 28, "PF28")
-#define SUNXI_PINCTRL_PIN_PF29	PINCTRL_PIN(PF_BASE + 29, "PF29")
-#define SUNXI_PINCTRL_PIN_PF30	PINCTRL_PIN(PF_BASE + 30, "PF30")
-#define SUNXI_PINCTRL_PIN_PF31	PINCTRL_PIN(PF_BASE + 31, "PF31")
-
-#define SUNXI_PINCTRL_PIN_PG0	PINCTRL_PIN(PG_BASE + 0, "PG0")
-#define SUNXI_PINCTRL_PIN_PG1	PINCTRL_PIN(PG_BASE + 1, "PG1")
-#define SUNXI_PINCTRL_PIN_PG2	PINCTRL_PIN(PG_BASE + 2, "PG2")
-#define SUNXI_PINCTRL_PIN_PG3	PINCTRL_PIN(PG_BASE + 3, "PG3")
-#define SUNXI_PINCTRL_PIN_PG4	PINCTRL_PIN(PG_BASE + 4, "PG4")
-#define SUNXI_PINCTRL_PIN_PG5	PINCTRL_PIN(PG_BASE + 5, "PG5")
-#define SUNXI_PINCTRL_PIN_PG6	PINCTRL_PIN(PG_BASE + 6, "PG6")
-#define SUNXI_PINCTRL_PIN_PG7	PINCTRL_PIN(PG_BASE + 7, "PG7")
-#define SUNXI_PINCTRL_PIN_PG8	PINCTRL_PIN(PG_BASE + 8, "PG8")
-#define SUNXI_PINCTRL_PIN_PG9	PINCTRL_PIN(PG_BASE + 9, "PG9")
-#define SUNXI_PINCTRL_PIN_PG10	PINCTRL_PIN(PG_BASE + 10, "PG10")
-#define SUNXI_PINCTRL_PIN_PG11	PINCTRL_PIN(PG_BASE + 11, "PG11")
-#define SUNXI_PINCTRL_PIN_PG12	PINCTRL_PIN(PG_BASE + 12, "PG12")
-#define SUNXI_PINCTRL_PIN_PG13	PINCTRL_PIN(PG_BASE + 13, "PG13")
-#define SUNXI_PINCTRL_PIN_PG14	PINCTRL_PIN(PG_BASE + 14, "PG14")
-#define SUNXI_PINCTRL_PIN_PG15	PINCTRL_PIN(PG_BASE + 15, "PG15")
-#define SUNXI_PINCTRL_PIN_PG16	PINCTRL_PIN(PG_BASE + 16, "PG16")
-#define SUNXI_PINCTRL_PIN_PG17	PINCTRL_PIN(PG_BASE + 17, "PG17")
-#define SUNXI_PINCTRL_PIN_PG18	PINCTRL_PIN(PG_BASE + 18, "PG18")
-#define SUNXI_PINCTRL_PIN_PG19	PINCTRL_PIN(PG_BASE + 19, "PG19")
-#define SUNXI_PINCTRL_PIN_PG20	PINCTRL_PIN(PG_BASE + 20, "PG20")
-#define SUNXI_PINCTRL_PIN_PG21	PINCTRL_PIN(PG_BASE + 21, "PG21")
-#define SUNXI_PINCTRL_PIN_PG22	PINCTRL_PIN(PG_BASE + 22, "PG22")
-#define SUNXI_PINCTRL_PIN_PG23	PINCTRL_PIN(PG_BASE + 23, "PG23")
-#define SUNXI_PINCTRL_PIN_PG24	PINCTRL_PIN(PG_BASE + 24, "PG24")
-#define SUNXI_PINCTRL_PIN_PG25	PINCTRL_PIN(PG_BASE + 25, "PG25")
-#define SUNXI_PINCTRL_PIN_PG26	PINCTRL_PIN(PG_BASE + 26, "PG26")
-#define SUNXI_PINCTRL_PIN_PG27	PINCTRL_PIN(PG_BASE + 27, "PG27")
-#define SUNXI_PINCTRL_PIN_PG28	PINCTRL_PIN(PG_BASE + 28, "PG28")
-#define SUNXI_PINCTRL_PIN_PG29	PINCTRL_PIN(PG_BASE + 29, "PG29")
-#define SUNXI_PINCTRL_PIN_PG30	PINCTRL_PIN(PG_BASE + 30, "PG30")
-#define SUNXI_PINCTRL_PIN_PG31	PINCTRL_PIN(PG_BASE + 31, "PG31")
-
-#define SUNXI_PINCTRL_PIN_PH0	PINCTRL_PIN(PH_BASE + 0, "PH0")
-#define SUNXI_PINCTRL_PIN_PH1	PINCTRL_PIN(PH_BASE + 1, "PH1")
-#define SUNXI_PINCTRL_PIN_PH2	PINCTRL_PIN(PH_BASE + 2, "PH2")
-#define SUNXI_PINCTRL_PIN_PH3	PINCTRL_PIN(PH_BASE + 3, "PH3")
-#define SUNXI_PINCTRL_PIN_PH4	PINCTRL_PIN(PH_BASE + 4, "PH4")
-#define SUNXI_PINCTRL_PIN_PH5	PINCTRL_PIN(PH_BASE + 5, "PH5")
-#define SUNXI_PINCTRL_PIN_PH6	PINCTRL_PIN(PH_BASE + 6, "PH6")
-#define SUNXI_PINCTRL_PIN_PH7	PINCTRL_PIN(PH_BASE + 7, "PH7")
-#define SUNXI_PINCTRL_PIN_PH8	PINCTRL_PIN(PH_BASE + 8, "PH8")
-#define SUNXI_PINCTRL_PIN_PH9	PINCTRL_PIN(PH_BASE + 9, "PH9")
-#define SUNXI_PINCTRL_PIN_PH10	PINCTRL_PIN(PH_BASE + 10, "PH10")
-#define SUNXI_PINCTRL_PIN_PH11	PINCTRL_PIN(PH_BASE + 11, "PH11")
-#define SUNXI_PINCTRL_PIN_PH12	PINCTRL_PIN(PH_BASE + 12, "PH12")
-#define SUNXI_PINCTRL_PIN_PH13	PINCTRL_PIN(PH_BASE + 13, "PH13")
-#define SUNXI_PINCTRL_PIN_PH14	PINCTRL_PIN(PH_BASE + 14, "PH14")
-#define SUNXI_PINCTRL_PIN_PH15	PINCTRL_PIN(PH_BASE + 15, "PH15")
-#define SUNXI_PINCTRL_PIN_PH16	PINCTRL_PIN(PH_BASE + 16, "PH16")
-#define SUNXI_PINCTRL_PIN_PH17	PINCTRL_PIN(PH_BASE + 17, "PH17")
-#define SUNXI_PINCTRL_PIN_PH18	PINCTRL_PIN(PH_BASE + 18, "PH18")
-#define SUNXI_PINCTRL_PIN_PH19	PINCTRL_PIN(PH_BASE + 19, "PH19")
-#define SUNXI_PINCTRL_PIN_PH20	PINCTRL_PIN(PH_BASE + 20, "PH20")
-#define SUNXI_PINCTRL_PIN_PH21	PINCTRL_PIN(PH_BASE + 21, "PH21")
-#define SUNXI_PINCTRL_PIN_PH22	PINCTRL_PIN(PH_BASE + 22, "PH22")
-#define SUNXI_PINCTRL_PIN_PH23	PINCTRL_PIN(PH_BASE + 23, "PH23")
-#define SUNXI_PINCTRL_PIN_PH24	PINCTRL_PIN(PH_BASE + 24, "PH24")
-#define SUNXI_PINCTRL_PIN_PH25	PINCTRL_PIN(PH_BASE + 25, "PH25")
-#define SUNXI_PINCTRL_PIN_PH26	PINCTRL_PIN(PH_BASE + 26, "PH26")
-#define SUNXI_PINCTRL_PIN_PH27	PINCTRL_PIN(PH_BASE + 27, "PH27")
-#define SUNXI_PINCTRL_PIN_PH28	PINCTRL_PIN(PH_BASE + 28, "PH28")
-#define SUNXI_PINCTRL_PIN_PH29	PINCTRL_PIN(PH_BASE + 29, "PH29")
-#define SUNXI_PINCTRL_PIN_PH30	PINCTRL_PIN(PH_BASE + 30, "PH30")
-#define SUNXI_PINCTRL_PIN_PH31	PINCTRL_PIN(PH_BASE + 31, "PH31")
-
-#define SUNXI_PINCTRL_PIN_PI0	PINCTRL_PIN(PI_BASE + 0, "PI0")
-#define SUNXI_PINCTRL_PIN_PI1	PINCTRL_PIN(PI_BASE + 1, "PI1")
-#define SUNXI_PINCTRL_PIN_PI2	PINCTRL_PIN(PI_BASE + 2, "PI2")
-#define SUNXI_PINCTRL_PIN_PI3	PINCTRL_PIN(PI_BASE + 3, "PI3")
-#define SUNXI_PINCTRL_PIN_PI4	PINCTRL_PIN(PI_BASE + 4, "PI4")
-#define SUNXI_PINCTRL_PIN_PI5	PINCTRL_PIN(PI_BASE + 5, "PI5")
-#define SUNXI_PINCTRL_PIN_PI6	PINCTRL_PIN(PI_BASE + 6, "PI6")
-#define SUNXI_PINCTRL_PIN_PI7	PINCTRL_PIN(PI_BASE + 7, "PI7")
-#define SUNXI_PINCTRL_PIN_PI8	PINCTRL_PIN(PI_BASE + 8, "PI8")
-#define SUNXI_PINCTRL_PIN_PI9	PINCTRL_PIN(PI_BASE + 9, "PI9")
-#define SUNXI_PINCTRL_PIN_PI10	PINCTRL_PIN(PI_BASE + 10, "PI10")
-#define SUNXI_PINCTRL_PIN_PI11	PINCTRL_PIN(PI_BASE + 11, "PI11")
-#define SUNXI_PINCTRL_PIN_PI12	PINCTRL_PIN(PI_BASE + 12, "PI12")
-#define SUNXI_PINCTRL_PIN_PI13	PINCTRL_PIN(PI_BASE + 13, "PI13")
-#define SUNXI_PINCTRL_PIN_PI14	PINCTRL_PIN(PI_BASE + 14, "PI14")
-#define SUNXI_PINCTRL_PIN_PI15	PINCTRL_PIN(PI_BASE + 15, "PI15")
-#define SUNXI_PINCTRL_PIN_PI16	PINCTRL_PIN(PI_BASE + 16, "PI16")
-#define SUNXI_PINCTRL_PIN_PI17	PINCTRL_PIN(PI_BASE + 17, "PI17")
-#define SUNXI_PINCTRL_PIN_PI18	PINCTRL_PIN(PI_BASE + 18, "PI18")
-#define SUNXI_PINCTRL_PIN_PI19	PINCTRL_PIN(PI_BASE + 19, "PI19")
-#define SUNXI_PINCTRL_PIN_PI20	PINCTRL_PIN(PI_BASE + 20, "PI20")
-#define SUNXI_PINCTRL_PIN_PI21	PINCTRL_PIN(PI_BASE + 21, "PI21")
-#define SUNXI_PINCTRL_PIN_PI22	PINCTRL_PIN(PI_BASE + 22, "PI22")
-#define SUNXI_PINCTRL_PIN_PI23	PINCTRL_PIN(PI_BASE + 23, "PI23")
-#define SUNXI_PINCTRL_PIN_PI24	PINCTRL_PIN(PI_BASE + 24, "PI24")
-#define SUNXI_PINCTRL_PIN_PI25	PINCTRL_PIN(PI_BASE + 25, "PI25")
-#define SUNXI_PINCTRL_PIN_PI26	PINCTRL_PIN(PI_BASE + 26, "PI26")
-#define SUNXI_PINCTRL_PIN_PI27	PINCTRL_PIN(PI_BASE + 27, "PI27")
-#define SUNXI_PINCTRL_PIN_PI28	PINCTRL_PIN(PI_BASE + 28, "PI28")
-#define SUNXI_PINCTRL_PIN_PI29	PINCTRL_PIN(PI_BASE + 29, "PI29")
-#define SUNXI_PINCTRL_PIN_PI30	PINCTRL_PIN(PI_BASE + 30, "PI30")
-#define SUNXI_PINCTRL_PIN_PI31	PINCTRL_PIN(PI_BASE + 31, "PI31")
-
-#define SUNXI_PIN_NAME_MAX_LEN	5
-
-#define BANK_MEM_SIZE		0x24
-#define MUX_REGS_OFFSET		0x0
-#define DATA_REGS_OFFSET	0x10
-#define DLEVEL_REGS_OFFSET	0x14
-#define PULL_REGS_OFFSET	0x1c
-
-#define PINS_PER_BANK		32
-#define MUX_PINS_PER_REG	8
-#define MUX_PINS_BITS		4
-#define MUX_PINS_MASK		0x0f
-#define DATA_PINS_PER_REG	32
-#define DATA_PINS_BITS		1
-#define DATA_PINS_MASK		0x01
-#define DLEVEL_PINS_PER_REG	16
-#define DLEVEL_PINS_BITS	2
-#define DLEVEL_PINS_MASK	0x03
-#define PULL_PINS_PER_REG	16
-#define PULL_PINS_BITS		2
-#define PULL_PINS_MASK		0x03
-
-#define SUNXI_IRQ_NUMBER	32
-
-#define IRQ_CFG_REG		0x200
-#define IRQ_CFG_IRQ_PER_REG		8
-#define IRQ_CFG_IRQ_BITS		4
-#define IRQ_CFG_IRQ_MASK		((1 << IRQ_CFG_IRQ_BITS) - 1)
-#define IRQ_CTRL_REG		0x210
-#define IRQ_CTRL_IRQ_PER_REG		32
-#define IRQ_CTRL_IRQ_BITS		1
-#define IRQ_CTRL_IRQ_MASK		((1 << IRQ_CTRL_IRQ_BITS) - 1)
-#define IRQ_STATUS_REG		0x214
-#define IRQ_STATUS_IRQ_PER_REG		32
-#define IRQ_STATUS_IRQ_BITS		1
-#define IRQ_STATUS_IRQ_MASK		((1 << IRQ_STATUS_IRQ_BITS) - 1)
-
-#define IRQ_EDGE_RISING		0x00
-#define IRQ_EDGE_FALLING	0x01
-#define IRQ_LEVEL_HIGH		0x02
-#define IRQ_LEVEL_LOW		0x03
-#define IRQ_EDGE_BOTH		0x04
-
-struct sunxi_desc_function {
-	const char	*name;
-	u8		muxval;
-	u8		irqnum;
-};
-
-struct sunxi_desc_pin {
-	struct pinctrl_pin_desc		pin;
-	struct sunxi_desc_function	*functions;
-};
-
-struct sunxi_pinctrl_desc {
-	const struct sunxi_desc_pin	*pins;
-	int				npins;
-	struct pinctrl_gpio_range	*ranges;
-	int				nranges;
-};
-
-struct sunxi_pinctrl_function {
-	const char	*name;
-	const char	**groups;
-	unsigned	ngroups;
-};
-
-struct sunxi_pinctrl_group {
-	const char	*name;
-	unsigned long	config;
-	unsigned	pin;
-};
-
-struct sunxi_pinctrl {
-	void __iomem			*membase;
-	struct gpio_chip		*chip;
-	struct sunxi_pinctrl_desc	*desc;
-	struct device			*dev;
-	struct irq_domain		*domain;
-	struct sunxi_pinctrl_function	*functions;
-	unsigned			nfunctions;
-	struct sunxi_pinctrl_group	*groups;
-	unsigned			ngroups;
-	int				irq;
-	int				irq_array[SUNXI_IRQ_NUMBER];
-	spinlock_t			lock;
-	struct pinctrl_dev		*pctl_dev;
-};
-
-#define SUNXI_PIN(_pin, ...)					\
-	{							\
-		.pin = _pin,					\
-		.functions = (struct sunxi_desc_function[]){	\
-			__VA_ARGS__, { } },			\
-	}
-
-#define SUNXI_FUNCTION(_val, _name)				\
-	{							\
-		.name = _name,					\
-		.muxval = _val,					\
-	}
-
-#define SUNXI_FUNCTION_IRQ(_val, _irq)				\
-	{							\
-		.name = "irq",					\
-		.muxval = _val,					\
-		.irqnum = _irq,					\
-	}
-
-/*
- * The sunXi PIO registers are organized as is:
- * 0x00 - 0x0c	Muxing values.
- *		8 pins per register, each pin having a 4bits value
- * 0x10		Pin values
- *		32 bits per register, each pin corresponding to one bit
- * 0x14 - 0x18	Drive level
- *		16 pins per register, each pin having a 2bits value
- * 0x1c - 0x20	Pull-Up values
- *		16 pins per register, each pin having a 2bits value
- *
- * This is for the first bank. Each bank will have the same layout,
- * with an offset being a multiple of 0x24.
- *
- * The following functions calculate from the pin number the register
- * and the bit offset that we should access.
- */
-static inline u32 sunxi_mux_reg(u16 pin)
-{
-	u8 bank = pin / PINS_PER_BANK;
-	u32 offset = bank * BANK_MEM_SIZE;
-	offset += MUX_REGS_OFFSET;
-	offset += pin % PINS_PER_BANK / MUX_PINS_PER_REG * 0x04;
-	return round_down(offset, 4);
-}
-
-static inline u32 sunxi_mux_offset(u16 pin)
-{
-	u32 pin_num = pin % MUX_PINS_PER_REG;
-	return pin_num * MUX_PINS_BITS;
-}
-
-static inline u32 sunxi_data_reg(u16 pin)
-{
-	u8 bank = pin / PINS_PER_BANK;
-	u32 offset = bank * BANK_MEM_SIZE;
-	offset += DATA_REGS_OFFSET;
-	offset += pin % PINS_PER_BANK / DATA_PINS_PER_REG * 0x04;
-	return round_down(offset, 4);
-}
-
-static inline u32 sunxi_data_offset(u16 pin)
-{
-	u32 pin_num = pin % DATA_PINS_PER_REG;
-	return pin_num * DATA_PINS_BITS;
-}
-
-static inline u32 sunxi_dlevel_reg(u16 pin)
-{
-	u8 bank = pin / PINS_PER_BANK;
-	u32 offset = bank * BANK_MEM_SIZE;
-	offset += DLEVEL_REGS_OFFSET;
-	offset += pin % PINS_PER_BANK / DLEVEL_PINS_PER_REG * 0x04;
-	return round_down(offset, 4);
-}
-
-static inline u32 sunxi_dlevel_offset(u16 pin)
-{
-	u32 pin_num = pin % DLEVEL_PINS_PER_REG;
-	return pin_num * DLEVEL_PINS_BITS;
-}
-
-static inline u32 sunxi_pull_reg(u16 pin)
-{
-	u8 bank = pin / PINS_PER_BANK;
-	u32 offset = bank * BANK_MEM_SIZE;
-	offset += PULL_REGS_OFFSET;
-	offset += pin % PINS_PER_BANK / PULL_PINS_PER_REG * 0x04;
-	return round_down(offset, 4);
-}
-
-static inline u32 sunxi_pull_offset(u16 pin)
-{
-	u32 pin_num = pin % PULL_PINS_PER_REG;
-	return pin_num * PULL_PINS_BITS;
-}
-
-static inline u32 sunxi_irq_cfg_reg(u16 irq)
-{
-	u8 reg = irq / IRQ_CFG_IRQ_PER_REG * 0x04;
-	return reg + IRQ_CFG_REG;
-}
-
-static inline u32 sunxi_irq_cfg_offset(u16 irq)
-{
-	u32 irq_num = irq % IRQ_CFG_IRQ_PER_REG;
-	return irq_num * IRQ_CFG_IRQ_BITS;
-}
-
-static inline u32 sunxi_irq_ctrl_reg(u16 irq)
-{
-	u8 reg = irq / IRQ_CTRL_IRQ_PER_REG * 0x04;
-	return reg + IRQ_CTRL_REG;
-}
-
-static inline u32 sunxi_irq_ctrl_offset(u16 irq)
-{
-	u32 irq_num = irq % IRQ_CTRL_IRQ_PER_REG;
-	return irq_num * IRQ_CTRL_IRQ_BITS;
-}
-
-static inline u32 sunxi_irq_status_reg(u16 irq)
-{
-	u8 reg = irq / IRQ_STATUS_IRQ_PER_REG * 0x04;
-	return reg + IRQ_STATUS_REG;
-}
-
-static inline u32 sunxi_irq_status_offset(u16 irq)
-{
-	u32 irq_num = irq % IRQ_STATUS_IRQ_PER_REG;
-	return irq_num * IRQ_STATUS_IRQ_BITS;
-}
-
-#endif /* __PINCTRL_SUNXI_H */
diff --git a/drivers/pinctrl/pinctrl-tegra.c b/drivers/pinctrl/pinctrl-tegra.c
index 65458096f41..2d43bff74f5 100644
--- a/drivers/pinctrl/pinctrl-tegra.c
+++ b/drivers/pinctrl/pinctrl-tegra.c
@@ -295,17 +295,11 @@ static void tegra_pinctrl_disable(struct pinctrl_dev *pctldev,
 {
 	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
 	const struct tegra_pingroup *g;
-	u32 val;
 
 	g = &pmx->soc->groups[group];
 
 	if (WARN_ON(g->mux_reg < 0))
 		return;
-
-	val = pmx_readl(pmx, g->mux_bank, g->mux_reg);
-	val &= ~(0x3 << g->mux_bit);
-	val |= g->func_safe << g->mux_bit;
-	pmx_writel(pmx, val, g->mux_bank, g->mux_reg);
 }
 
 static const struct pinmux_ops tegra_pinmux_ops = {
@@ -336,32 +330,32 @@ static int tegra_pinconf_reg(struct tegra_pmx *pmx,
 		*width = 1;
 		break;
 	case TEGRA_PINCONF_PARAM_ENABLE_INPUT:
-		*bank = g->einput_bank;
-		*reg = g->einput_reg;
+		*bank = g->mux_bank;
+		*reg = g->mux_reg;
 		*bit = g->einput_bit;
 		*width = 1;
 		break;
 	case TEGRA_PINCONF_PARAM_OPEN_DRAIN:
-		*bank = g->odrain_bank;
-		*reg = g->odrain_reg;
+		*bank = g->mux_bank;
+		*reg = g->mux_reg;
 		*bit = g->odrain_bit;
 		*width = 1;
 		break;
 	case TEGRA_PINCONF_PARAM_LOCK:
-		*bank = g->lock_bank;
-		*reg = g->lock_reg;
+		*bank = g->mux_bank;
+		*reg = g->mux_reg;
 		*bit = g->lock_bit;
 		*width = 1;
 		break;
 	case TEGRA_PINCONF_PARAM_IORESET:
-		*bank = g->ioreset_bank;
-		*reg = g->ioreset_reg;
+		*bank = g->mux_bank;
+		*reg = g->mux_reg;
 		*bit = g->ioreset_bit;
 		*width = 1;
 		break;
 	case TEGRA_PINCONF_PARAM_RCV_SEL:
-		*bank = g->rcv_sel_bank;
-		*reg = g->rcv_sel_reg;
+		*bank = g->mux_bank;
+		*reg = g->mux_reg;
 		*bit = g->rcv_sel_bit;
 		*width = 1;
 		break;
@@ -408,8 +402,8 @@ static int tegra_pinconf_reg(struct tegra_pmx *pmx,
 		*width = g->slwr_width;
 		break;
 	case TEGRA_PINCONF_PARAM_DRIVE_TYPE:
-		*bank = g->drvtype_bank;
-		*reg = g->drvtype_reg;
+		*bank = g->drv_bank;
+		*reg = g->drv_reg;
 		*bit = g->drvtype_bit;
 		*width = 2;
 		break;
@@ -418,11 +412,22 @@ static int tegra_pinconf_reg(struct tegra_pmx *pmx,
 		return -ENOTSUPP;
 	}
 
-	if (*reg < 0) {
-		if (report_err)
+	if (*reg < 0 || *bit > 31) {
+		if (report_err) {
+			const char *prop = "unknown";
+			int i;
+
+			for (i = 0; i < ARRAY_SIZE(cfg_params); i++) {
+				if (cfg_params[i].param == param) {
+					prop = cfg_params[i].property;
+					break;
+				}
+			}
+
 			dev_err(pmx->dev,
-				"Config param %04x not supported on group %s\n",
-				param, g->name);
+				"Config param %04x (%s) not supported on group %s\n",
+				param, prop, g->name);
+		}
 		return -ENOTSUPP;
 	}
 
diff --git a/drivers/pinctrl/pinctrl-tegra.h b/drivers/pinctrl/pinctrl-tegra.h
index 6053832d433..8d94d1332e7 100644
--- a/drivers/pinctrl/pinctrl-tegra.h
+++ b/drivers/pinctrl/pinctrl-tegra.h
@@ -78,98 +78,83 @@ struct tegra_function {
 
 /**
  * struct tegra_pingroup - Tegra pin group
- * @mux_reg:		Mux register offset. -1 if unsupported.
- * @mux_bank:		Mux register bank. 0 if unsupported.
- * @mux_bit:		Mux register bit. 0 if unsupported.
- * @pupd_reg:		Pull-up/down register offset. -1 if unsupported.
- * @pupd_bank:		Pull-up/down register bank. 0 if unsupported.
- * @pupd_bit:		Pull-up/down register bit. 0 if unsupported.
- * @tri_reg:		Tri-state register offset. -1 if unsupported.
- * @tri_bank:		Tri-state register bank. 0 if unsupported.
- * @tri_bit:		Tri-state register bit. 0 if unsupported.
- * @einput_reg:		Enable-input register offset. -1 if unsupported.
- * @einput_bank:	Enable-input register bank. 0 if unsupported.
- * @einput_bit:		Enable-input register bit. 0 if unsupported.
- * @odrain_reg:		Open-drain register offset. -1 if unsupported.
- * @odrain_bank:	Open-drain register bank. 0 if unsupported.
- * @odrain_bit:		Open-drain register bit. 0 if unsupported.
- * @lock_reg:		Lock register offset. -1 if unsupported.
- * @lock_bank:		Lock register bank. 0 if unsupported.
- * @lock_bit:		Lock register bit. 0 if unsupported.
- * @ioreset_reg:	IO reset register offset. -1 if unsupported.
- * @ioreset_bank:	IO reset register bank. 0 if unsupported.
- * @ioreset_bit:	IO reset register bit. 0 if unsupported.
- * @rcv_sel_reg:	Receiver select offset. -1 if unsupported.
- * @rcv_sel_bank:	Receiver select bank. 0 if unsupported.
- * @rcv_sel_bit:	Receiver select bit. 0 if unsupported.
- * @drv_reg:		Drive fields register offset. -1 if unsupported.
- *			This register contains the hsm, schmitt, lpmd, drvdn,
- *			drvup, slwr, and slwf parameters.
- * @drv_bank:		Drive fields register bank. 0 if unsupported.
- * @hsm_bit:		High Speed Mode register bit. 0 if unsupported.
- * @schmitt_bit:	Scmitt register bit. 0 if unsupported.
- * @lpmd_bit:		Low Power Mode register bit. 0 if unsupported.
- * @drvdn_bit:		Drive Down register bit. 0 if unsupported.
- * @drvdn_width:	Drive Down field width. 0 if unsupported.
- * @drvup_bit:		Drive Up register bit. 0 if unsupported.
- * @drvup_width:	Drive Up field width. 0 if unsupported.
- * @slwr_bit:		Slew Rising register bit. 0 if unsupported.
- * @slwr_width:		Slew Rising field width. 0 if unsupported.
- * @slwf_bit:		Slew Falling register bit. 0 if unsupported.
- * @slwf_width:		Slew Falling field width. 0 if unsupported.
- * @drvtype_reg:	Drive type fields register offset. -1 if unsupported.
- * @drvtype_bank:	Drive type fields register bank. 0 if unsupported.
- * @drvtype_bit:	Drive type register bit. 0 if unsupported.
+ * @name		The name of the pin group.
+ * @pins		An array of pin IDs included in this pin group.
+ * @npins		The number of entries in @pins.
+ * @funcs		The mux functions which can be muxed onto this group.
+ * @mux_reg:		Mux register offset.
+ *			This register contains the mux, einput, odrain, lock,
+ *			ioreset, rcv_sel parameters.
+ * @mux_bank:		Mux register bank.
+ * @mux_bit:		Mux register bit.
+ * @pupd_reg:		Pull-up/down register offset.
+ * @pupd_bank:		Pull-up/down register bank.
+ * @pupd_bit:		Pull-up/down register bit.
+ * @tri_reg:		Tri-state register offset.
+ * @tri_bank:		Tri-state register bank.
+ * @tri_bit:		Tri-state register bit.
+ * @einput_bit:		Enable-input register bit.
+ * @odrain_bit:		Open-drain register bit.
+ * @lock_bit:		Lock register bit.
+ * @ioreset_bit:	IO reset register bit.
+ * @rcv_sel_bit:	Receiver select bit.
+ * @drv_reg:		Drive fields register offset.
+ *			This register contains hsm, schmitt, lpmd, drvdn,
+ *			drvup, slwr, slwf, and drvtype parameters.
+ * @drv_bank:		Drive fields register bank.
+ * @hsm_bit:		High Speed Mode register bit.
+ * @schmitt_bit:	Scmitt register bit.
+ * @lpmd_bit:		Low Power Mode register bit.
+ * @drvdn_bit:		Drive Down register bit.
+ * @drvdn_width:	Drive Down field width.
+ * @drvup_bit:		Drive Up register bit.
+ * @drvup_width:	Drive Up field width.
+ * @slwr_bit:		Slew Rising register bit.
+ * @slwr_width:		Slew Rising field width.
+ * @slwf_bit:		Slew Falling register bit.
+ * @slwf_width:		Slew Falling field width.
+ * @drvtype_bit:	Drive type register bit.
+ *
+ * -1 in a *_reg field means that feature is unsupported for this group.
+ * *_bank and *_reg values are irrelevant when *_reg is -1.
+ * When *_reg is valid, *_bit may be -1 to indicate an unsupported feature.
  *
  * A representation of a group of pins (possibly just one pin) in the Tegra
  * pin controller. Each group allows some parameter or parameters to be
  * configured. The most common is mux function selection. Many others exist
  * such as pull-up/down, tri-state, etc. Tegra's pin controller is complex;
  * certain groups may only support configuring certain parameters, hence
- * each parameter is optional, represented by a -1 "reg" value.
+ * each parameter is optional.
  */
 struct tegra_pingroup {
 	const char *name;
 	const unsigned *pins;
-	unsigned npins;
-	unsigned funcs[4];
-	unsigned func_safe;
+	u8 npins;
+	u8 funcs[4];
 	s16 mux_reg;
 	s16 pupd_reg;
 	s16 tri_reg;
-	s16 einput_reg;
-	s16 odrain_reg;
-	s16 lock_reg;
-	s16 ioreset_reg;
-	s16 rcv_sel_reg;
 	s16 drv_reg;
-	s16 drvtype_reg;
 	u32 mux_bank:2;
 	u32 pupd_bank:2;
 	u32 tri_bank:2;
-	u32 einput_bank:2;
-	u32 odrain_bank:2;
-	u32 ioreset_bank:2;
-	u32 rcv_sel_bank:2;
-	u32 lock_bank:2;
 	u32 drv_bank:2;
-	u32 drvtype_bank:2;
-	u32 mux_bit:5;
-	u32 pupd_bit:5;
-	u32 tri_bit:5;
-	u32 einput_bit:5;
-	u32 odrain_bit:5;
-	u32 lock_bit:5;
-	u32 ioreset_bit:5;
-	u32 rcv_sel_bit:5;
-	u32 hsm_bit:5;
-	u32 schmitt_bit:5;
-	u32 lpmd_bit:5;
-	u32 drvdn_bit:5;
-	u32 drvup_bit:5;
-	u32 slwr_bit:5;
-	u32 slwf_bit:5;
-	u32 drvtype_bit:5;
+	u32 mux_bit:6;
+	u32 pupd_bit:6;
+	u32 tri_bit:6;
+	u32 einput_bit:6;
+	u32 odrain_bit:6;
+	u32 lock_bit:6;
+	u32 ioreset_bit:6;
+	u32 rcv_sel_bit:6;
+	u32 hsm_bit:6;
+	u32 schmitt_bit:6;
+	u32 lpmd_bit:6;
+	u32 drvdn_bit:6;
+	u32 drvup_bit:6;
+	u32 slwr_bit:6;
+	u32 slwf_bit:6;
+	u32 drvtype_bit:6;
 	u32 drvdn_width:6;
 	u32 drvup_width:6;
 	u32 slwr_width:6;
diff --git a/drivers/pinctrl/pinctrl-tegra114.c b/drivers/pinctrl/pinctrl-tegra114.c
index 63fe7619d3f..33614baab4c 100644
--- a/drivers/pinctrl/pinctrl-tegra114.c
+++ b/drivers/pinctrl/pinctrl-tegra114.c
@@ -1547,10 +1547,12 @@ static struct tegra_function tegra114_functions[] = {
 #define DRV_PINGROUP_REG_A		0x868	/* bank 0 */
 #define PINGROUP_REG_A			0x3000	/* bank 1 */
 
-#define PINGROUP_REG_Y(r)		((r) - PINGROUP_REG_A)
-#define PINGROUP_REG_N(r)		-1
+#define PINGROUP_REG(r)			((r) - PINGROUP_REG_A)
 
-#define PINGROUP(pg_name, f0, f1, f2, f3, f_safe, r, od, ior, rcv_sel)	\
+#define PINGROUP_BIT_Y(b)		(b)
+#define PINGROUP_BIT_N(b)		(-1)
+
+#define PINGROUP(pg_name, f0, f1, f2, f3, r, od, ior, rcv_sel)		\
 	{								\
 		.name = #pg_name,					\
 		.pins = pg_name##_pins,					\
@@ -1561,38 +1563,24 @@ static struct tegra_function tegra114_functions[] = {
 			TEGRA_MUX_##f2,					\
 			TEGRA_MUX_##f3,					\
 		},							\
-		.func_safe = TEGRA_MUX_##f_safe,			\
-		.mux_reg = PINGROUP_REG_Y(r),				\
+		.mux_reg = PINGROUP_REG(r),				\
 		.mux_bank = 1,						\
 		.mux_bit = 0,						\
-		.pupd_reg = PINGROUP_REG_Y(r),				\
+		.pupd_reg = PINGROUP_REG(r),				\
 		.pupd_bank = 1,						\
 		.pupd_bit = 2,						\
-		.tri_reg = PINGROUP_REG_Y(r),				\
+		.tri_reg = PINGROUP_REG(r),				\
 		.tri_bank = 1,						\
 		.tri_bit = 4,						\
-		.einput_reg = PINGROUP_REG_Y(r),			\
-		.einput_bank = 1,					\
-		.einput_bit = 5,					\
-		.odrain_reg = PINGROUP_REG_##od(r),			\
-		.odrain_bank = 1,					\
-		.odrain_bit = 6,					\
-		.lock_reg = PINGROUP_REG_Y(r),				\
-		.lock_bank = 1,						\
-		.lock_bit = 7,						\
-		.ioreset_reg = PINGROUP_REG_##ior(r),			\
-		.ioreset_bank = 1,					\
-		.ioreset_bit = 8,					\
-		.rcv_sel_reg = PINGROUP_REG_##rcv_sel(r),		\
-		.rcv_sel_bank = 1,					\
-		.rcv_sel_bit = 9,					\
+		.einput_bit = PINGROUP_BIT_Y(5),			\
+		.odrain_bit = PINGROUP_BIT_##od(6),			\
+		.lock_bit = PINGROUP_BIT_Y(7),				\
+		.ioreset_bit = PINGROUP_BIT_##ior(8),			\
+		.rcv_sel_bit = PINGROUP_BIT_##rcv_sel(9),		\
 		.drv_reg = -1,						\
-		.drvtype_reg = -1,					\
 	}
 
-#define DRV_PINGROUP_REG_Y(r)		((r) - DRV_PINGROUP_REG_A)
-#define DRV_PINGROUP_REG_N(r)		-1
-
+#define DRV_PINGROUP_REG(r)		((r) - DRV_PINGROUP_REG_A)
 
 #define DRV_PINGROUP(pg_name, r, hsm_b, schmitt_b, lpmd_b,		\
 		     drvdn_b, drvdn_w, drvup_b, drvup_w,		\
@@ -1605,12 +1593,12 @@ static struct tegra_function tegra114_functions[] = {
 		.mux_reg = -1,						\
 		.pupd_reg = -1,						\
 		.tri_reg = -1,						\
-		.einput_reg = -1,					\
-		.odrain_reg = -1,					\
-		.lock_reg = -1,						\
-		.ioreset_reg = -1,					\
-		.rcv_sel_reg = -1,					\
-		.drv_reg = DRV_PINGROUP_REG_Y(r),			\
+		.einput_bit = -1,					\
+		.odrain_bit = -1,					\
+		.lock_bit = -1,						\
+		.ioreset_bit = -1,					\
+		.rcv_sel_bit = -1,					\
+		.drv_reg = DRV_PINGROUP_REG(r),				\
 		.drv_bank = 0,						\
 		.hsm_bit = hsm_b,					\
 		.schmitt_bit = schmitt_b,				\
@@ -1623,190 +1611,188 @@ static struct tegra_function tegra114_functions[] = {
 		.slwr_width = slwr_w,					\
 		.slwf_bit = slwf_b,					\
 		.slwf_width = slwf_w,					\
-		.drvtype_reg = DRV_PINGROUP_REG_##drvtype(r),		\
-		.drvtype_bank = 0,					\
-		.drvtype_bit = 6,					\
+		.drvtype_bit = PINGROUP_BIT_##drvtype(6),		\
 	}
 
 static const struct tegra_pingroup tegra114_groups[] = {
-	/*       pg_name,                f0,         f1,         f2,           f3,          safe,     r,      od, ior, rcv_sel */
-	PINGROUP(ulpi_data0_po1,         SPI3,       HSI,        UARTA,        ULPI,        ULPI,     0x3000,  N,  N,  N),
-	PINGROUP(ulpi_data1_po2,         SPI3,       HSI,        UARTA,        ULPI,        ULPI,     0x3004,  N,  N,  N),
-	PINGROUP(ulpi_data2_po3,         SPI3,       HSI,        UARTA,        ULPI,        ULPI,     0x3008,  N,  N,  N),
-	PINGROUP(ulpi_data3_po4,         SPI3,       HSI,        UARTA,        ULPI,        ULPI,     0x300c,  N,  N,  N),
-	PINGROUP(ulpi_data4_po5,         SPI2,       HSI,        UARTA,        ULPI,        ULPI,     0x3010,  N,  N,  N),
-	PINGROUP(ulpi_data5_po6,         SPI2,       HSI,        UARTA,        ULPI,        ULPI,     0x3014,  N,  N,  N),
-	PINGROUP(ulpi_data6_po7,         SPI2,       HSI,        UARTA,        ULPI,        ULPI,     0x3018,  N,  N,  N),
-	PINGROUP(ulpi_data7_po0,         SPI2,       HSI,        UARTA,        ULPI,        ULPI,     0x301c,  N,  N,  N),
-	PINGROUP(ulpi_clk_py0,           SPI1,       SPI5,       UARTD,        ULPI,        ULPI,     0x3020,  N,  N,  N),
-	PINGROUP(ulpi_dir_py1,           SPI1,       SPI5,       UARTD,        ULPI,        ULPI,     0x3024,  N,  N,  N),
-	PINGROUP(ulpi_nxt_py2,           SPI1,       SPI5,       UARTD,        ULPI,        ULPI,     0x3028,  N,  N,  N),
-	PINGROUP(ulpi_stp_py3,           SPI1,       SPI5,       UARTD,        ULPI,        ULPI,     0x302c,  N,  N,  N),
-	PINGROUP(dap3_fs_pp0,            I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    I2S2,     0x3030,  N,  N,  N),
-	PINGROUP(dap3_din_pp1,           I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    I2S2,     0x3034,  N,  N,  N),
-	PINGROUP(dap3_dout_pp2,          I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    I2S2,     0x3038,  N,  N,  N),
-	PINGROUP(dap3_sclk_pp3,          I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    I2S2,     0x303c,  N,  N,  N),
-	PINGROUP(pv0,                    USB,        RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3040,  N,  N,  N),
-	PINGROUP(pv1,                    RSVD1,      RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3044,  N,  N,  N),
-	PINGROUP(sdmmc1_clk_pz0,         SDMMC1,     CLK12,      RSVD3,        RSVD4,       RSVD4,    0x3048,  N,  N,  N),
-	PINGROUP(sdmmc1_cmd_pz1,         SDMMC1,     SPDIF,      SPI4,         UARTA,       SDMMC1,   0x304c,  N,  N,  N),
-	PINGROUP(sdmmc1_dat3_py4,        SDMMC1,     SPDIF,      SPI4,         UARTA,       SDMMC1,   0x3050,  N,  N,  N),
-	PINGROUP(sdmmc1_dat2_py5,        SDMMC1,     PWM0,       SPI4,         UARTA,       SDMMC1,   0x3054,  N,  N,  N),
-	PINGROUP(sdmmc1_dat1_py6,        SDMMC1,     PWM1,       SPI4,         UARTA,       SDMMC1,   0x3058,  N,  N,  N),
-	PINGROUP(sdmmc1_dat0_py7,        SDMMC1,     RSVD2,      SPI4,         UARTA,       RSVD2,    0x305c,  N,  N,  N),
-	PINGROUP(clk2_out_pw5,           EXTPERIPH2, RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3068,  N,  N,  N),
-	PINGROUP(clk2_req_pcc5,          DAP,        RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x306c,  N,  N,  N),
-	PINGROUP(hdmi_int_pn7,           RSVD1,      RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3110,  N,  N,  Y),
-	PINGROUP(ddc_scl_pv4,            I2C4,       RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3114,  N,  N,  Y),
-	PINGROUP(ddc_sda_pv5,            I2C4,       RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3118,  N,  N,  Y),
-	PINGROUP(uart2_rxd_pc3,          IRDA,       SPDIF,      UARTA,        SPI4,        IRDA,     0x3164,  N,  N,  N),
-	PINGROUP(uart2_txd_pc2,          IRDA,       SPDIF,      UARTA,        SPI4,        IRDA,     0x3168,  N,  N,  N),
-	PINGROUP(uart2_rts_n_pj6,        UARTA,      UARTB,      RSVD3,        SPI4,        RSVD3,    0x316c,  N,  N,  N),
-	PINGROUP(uart2_cts_n_pj5,        UARTA,      UARTB,      RSVD3,        SPI4,        RSVD3,    0x3170,  N,  N,  N),
-	PINGROUP(uart3_txd_pw6,          UARTC,      RSVD2,      RSVD3,        SPI4,        RSVD3,    0x3174,  N,  N,  N),
-	PINGROUP(uart3_rxd_pw7,          UARTC,      RSVD2,      RSVD3,        SPI4,        RSVD3,    0x3178,  N,  N,  N),
-	PINGROUP(uart3_cts_n_pa1,        UARTC,      SDMMC1,     DTV,          SPI4,        UARTC,    0x317c,  N,  N,  N),
-	PINGROUP(uart3_rts_n_pc0,        UARTC,      PWM0,       DTV,          DISPLAYA,    UARTC,    0x3180,  N,  N,  N),
-	PINGROUP(pu0,                    OWR,        UARTA,      RSVD3,        RSVD4,       RSVD4,    0x3184,  N,  N,  N),
-	PINGROUP(pu1,                    RSVD1,      UARTA,      RSVD3,        RSVD4,       RSVD4,    0x3188,  N,  N,  N),
-	PINGROUP(pu2,                    RSVD1,      UARTA,      RSVD3,        RSVD4,       RSVD4,    0x318c,  N,  N,  N),
-	PINGROUP(pu3,                    PWM0,       UARTA,      DISPLAYA,     DISPLAYB,    PWM0,     0x3190,  N,  N,  N),
-	PINGROUP(pu4,                    PWM1,       UARTA,      DISPLAYA,     DISPLAYB,    PWM1,     0x3194,  N,  N,  N),
-	PINGROUP(pu5,                    PWM2,       UARTA,      DISPLAYA,     DISPLAYB,    PWM2,     0x3198,  N,  N,  N),
-	PINGROUP(pu6,                    PWM3,       UARTA,      USB,          DISPLAYB,    PWM3,     0x319c,  N,  N,  N),
-	PINGROUP(gen1_i2c_sda_pc5,       I2C1,       RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x31a0,  Y,  N,  N),
-	PINGROUP(gen1_i2c_scl_pc4,       I2C1,       RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x31a4,  Y,  N,  N),
-	PINGROUP(dap4_fs_pp4,            I2S3,       RSVD2,      DTV,          RSVD4,       RSVD4,    0x31a8,  N,  N,  N),
-	PINGROUP(dap4_din_pp5,           I2S3,       RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x31ac,  N,  N,  N),
-	PINGROUP(dap4_dout_pp6,          I2S3,       RSVD2,      DTV,          RSVD4,       RSVD4,    0x31b0,  N,  N,  N),
-	PINGROUP(dap4_sclk_pp7,          I2S3,       RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x31b4,  N,  N,  N),
-	PINGROUP(clk3_out_pee0,          EXTPERIPH3, RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x31b8,  N,  N,  N),
-	PINGROUP(clk3_req_pee1,          DEV3,       RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x31bc,  N,  N,  N),
-	PINGROUP(gmi_wp_n_pc7,           RSVD1,      NAND,       GMI,          GMI_ALT,     RSVD1,    0x31c0,  N,  N,  N),
-	PINGROUP(gmi_iordy_pi5,          SDMMC2,     RSVD2,      GMI,          TRACE,       RSVD2,    0x31c4,  N,  N,  N),
-	PINGROUP(gmi_wait_pi7,           SPI4,       NAND,       GMI,          DTV,         NAND,     0x31c8,  N,  N,  N),
-	PINGROUP(gmi_adv_n_pk0,          RSVD1,      NAND,       GMI,          TRACE,       RSVD1,    0x31cc,  N,  N,  N),
-	PINGROUP(gmi_clk_pk1,            SDMMC2,     NAND,       GMI,          TRACE,       GMI,      0x31d0,  N,  N,  N),
-	PINGROUP(gmi_cs0_n_pj0,          RSVD1,      NAND,       GMI,          USB,         RSVD1,    0x31d4,  N,  N,  N),
-	PINGROUP(gmi_cs1_n_pj2,          RSVD1,      NAND,       GMI,          SOC,         RSVD1,    0x31d8,  N,  N,  N),
-	PINGROUP(gmi_cs2_n_pk3,          SDMMC2,     NAND,       GMI,          TRACE,       GMI,      0x31dc,  N,  N,  N),
-	PINGROUP(gmi_cs3_n_pk4,          SDMMC2,     NAND,       GMI,          GMI_ALT,     GMI,      0x31e0,  N,  N,  N),
-	PINGROUP(gmi_cs4_n_pk2,          USB,        NAND,       GMI,          TRACE,       GMI,      0x31e4,  N,  N,  N),
-	PINGROUP(gmi_cs6_n_pi3,          NAND,       NAND_ALT,   GMI,          SPI4,        NAND,     0x31e8,  N,  N,  N),
-	PINGROUP(gmi_cs7_n_pi6,          NAND,       NAND_ALT,   GMI,          SDMMC2,      NAND,     0x31ec,  N,  N,  N),
-	PINGROUP(gmi_ad0_pg0,            RSVD1,      NAND,       GMI,          RSVD4,       RSVD4,    0x31f0,  N,  N,  N),
-	PINGROUP(gmi_ad1_pg1,            RSVD1,      NAND,       GMI,          RSVD4,       RSVD4,    0x31f4,  N,  N,  N),
-	PINGROUP(gmi_ad2_pg2,            RSVD1,      NAND,       GMI,          RSVD4,       RSVD4,    0x31f8,  N,  N,  N),
-	PINGROUP(gmi_ad3_pg3,            RSVD1,      NAND,       GMI,          RSVD4,       RSVD4,    0x31fc,  N,  N,  N),
-	PINGROUP(gmi_ad4_pg4,            RSVD1,      NAND,       GMI,          RSVD4,       RSVD4,    0x3200,  N,  N,  N),
-	PINGROUP(gmi_ad5_pg5,            RSVD1,      NAND,       GMI,          SPI4,        RSVD1,    0x3204,  N,  N,  N),
-	PINGROUP(gmi_ad6_pg6,            RSVD1,      NAND,       GMI,          SPI4,        RSVD1,    0x3208,  N,  N,  N),
-	PINGROUP(gmi_ad7_pg7,            RSVD1,      NAND,       GMI,          SPI4,        RSVD1,    0x320c,  N,  N,  N),
-	PINGROUP(gmi_ad8_ph0,            PWM0,       NAND,       GMI,          DTV,         GMI,      0x3210,  N,  N,  N),
-	PINGROUP(gmi_ad9_ph1,            PWM1,       NAND,       GMI,          CLDVFS,      GMI,      0x3214,  N,  N,  N),
-	PINGROUP(gmi_ad10_ph2,           PWM2,       NAND,       GMI,          CLDVFS,      GMI,      0x3218,  N,  N,  N),
-	PINGROUP(gmi_ad11_ph3,           PWM3,       NAND,       GMI,          USB,         GMI,      0x321c,  N,  N,  N),
-	PINGROUP(gmi_ad12_ph4,           SDMMC2,     NAND,       GMI,          RSVD4,       RSVD4,    0x3220,  N,  N,  N),
-	PINGROUP(gmi_ad13_ph5,           SDMMC2,     NAND,       GMI,          RSVD4,       RSVD4,    0x3224,  N,  N,  N),
-	PINGROUP(gmi_ad14_ph6,           SDMMC2,     NAND,       GMI,          DTV,         GMI,      0x3228,  N,  N,  N),
-	PINGROUP(gmi_ad15_ph7,           SDMMC2,     NAND,       GMI,          DTV,         GMI,      0x322c,  N,  N,  N),
-	PINGROUP(gmi_a16_pj7,            UARTD,      TRACE,      GMI,          GMI_ALT,     GMI,      0x3230,  N,  N,  N),
-	PINGROUP(gmi_a17_pb0,            UARTD,      RSVD2,      GMI,          TRACE,       RSVD2,    0x3234,  N,  N,  N),
-	PINGROUP(gmi_a18_pb1,            UARTD,      RSVD2,      GMI,          TRACE,       RSVD2,    0x3238,  N,  N,  N),
-	PINGROUP(gmi_a19_pk7,            UARTD,      SPI4,       GMI,          TRACE,       GMI,      0x323c,  N,  N,  N),
-	PINGROUP(gmi_wr_n_pi0,           RSVD1,      NAND,       GMI,          SPI4,        RSVD1,    0x3240,  N,  N,  N),
-	PINGROUP(gmi_oe_n_pi1,           RSVD1,      NAND,       GMI,          SOC,         RSVD1,    0x3244,  N,  N,  N),
-	PINGROUP(gmi_dqs_p_pj3,          SDMMC2,     NAND,       GMI,          TRACE,       NAND,     0x3248,  N,  N,  N),
-	PINGROUP(gmi_rst_n_pi4,          NAND,       NAND_ALT,   GMI,          RSVD4,       RSVD4,    0x324c,  N,  N,  N),
-	PINGROUP(gen2_i2c_scl_pt5,       I2C2,       RSVD2,      GMI,          RSVD4,       RSVD4,    0x3250,  Y,  N,  N),
-	PINGROUP(gen2_i2c_sda_pt6,       I2C2,       RSVD2,      GMI,          RSVD4,       RSVD4,    0x3254,  Y,  N,  N),
-	PINGROUP(sdmmc4_clk_pcc4,        SDMMC4,     RSVD2,      GMI,          RSVD4,       RSVD4,    0x3258,  N,  Y,  N),
-	PINGROUP(sdmmc4_cmd_pt7,         SDMMC4,     RSVD2,      GMI,          RSVD4,       RSVD4,    0x325c,  N,  Y,  N),
-	PINGROUP(sdmmc4_dat0_paa0,       SDMMC4,     SPI3,       GMI,          RSVD4,       RSVD4,    0x3260,  N,  Y,  N),
-	PINGROUP(sdmmc4_dat1_paa1,       SDMMC4,     SPI3,       GMI,          RSVD4,       RSVD4,    0x3264,  N,  Y,  N),
-	PINGROUP(sdmmc4_dat2_paa2,       SDMMC4,     SPI3,       GMI,          RSVD4,       RSVD4,    0x3268,  N,  Y,  N),
-	PINGROUP(sdmmc4_dat3_paa3,       SDMMC4,     SPI3,       GMI,          RSVD4,       RSVD4,    0x326c,  N,  Y,  N),
-	PINGROUP(sdmmc4_dat4_paa4,       SDMMC4,     SPI3,       GMI,          RSVD4,       RSVD4,    0x3270,  N,  Y,  N),
-	PINGROUP(sdmmc4_dat5_paa5,       SDMMC4,     SPI3,       GMI,          RSVD4,       RSVD4,    0x3274,  N,  Y,  N),
-	PINGROUP(sdmmc4_dat6_paa6,       SDMMC4,     SPI3,       GMI,          RSVD4,       RSVD4,    0x3278,  N,  Y,  N),
-	PINGROUP(sdmmc4_dat7_paa7,       SDMMC4,     RSVD2,      GMI,          RSVD4,       RSVD4,    0x327c,  N,  Y,  N),
-	PINGROUP(cam_mclk_pcc0,          VI,         VI_ALT1,    VI_ALT3,      RSVD4,       RSVD4,    0x3284,  N,  N,  N),
-	PINGROUP(pcc1,                   I2S4,       RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3288,  N,  N,  N),
-	PINGROUP(pbb0,                   I2S4,       VI,         VI_ALT1,      VI_ALT3,     I2S4,     0x328c,  N,  N,  N),
-	PINGROUP(cam_i2c_scl_pbb1,       VGP1,       I2C3,       RSVD3,        RSVD4,       RSVD4,    0x3290,  Y,  N,  N),
-	PINGROUP(cam_i2c_sda_pbb2,       VGP2,       I2C3,       RSVD3,        RSVD4,       RSVD4,    0x3294,  Y,  N,  N),
-	PINGROUP(pbb3,                   VGP3,       DISPLAYA,   DISPLAYB,     RSVD4,       RSVD4,    0x3298,  N,  N,  N),
-	PINGROUP(pbb4,                   VGP4,       DISPLAYA,   DISPLAYB,     RSVD4,       RSVD4,    0x329c,  N,  N,  N),
-	PINGROUP(pbb5,                   VGP5,       DISPLAYA,   DISPLAYB,     RSVD4,       RSVD4,    0x32a0,  N,  N,  N),
-	PINGROUP(pbb6,                   VGP6,       DISPLAYA,   DISPLAYB,     RSVD4,       RSVD4,    0x32a4,  N,  N,  N),
-	PINGROUP(pbb7,                   I2S4,       RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x32a8,  N,  N,  N),
-	PINGROUP(pcc2,                   I2S4,       RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x32ac,  N,  N,  N),
-	PINGROUP(jtag_rtck,              RTCK,       RSVD2,      RSVD3,        RSVD4,       RTCK,     0x32b0,  N,  N,  N),
-	PINGROUP(pwr_i2c_scl_pz6,        I2CPWR,     RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x32b4,  Y,  N,  N),
-	PINGROUP(pwr_i2c_sda_pz7,        I2CPWR,     RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x32b8,  Y,  N,  N),
-	PINGROUP(kb_row0_pr0,            KBC,        RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x32bc,  N,  N,  N),
-	PINGROUP(kb_row1_pr1,            KBC,        RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x32c0,  N,  N,  N),
-	PINGROUP(kb_row2_pr2,            KBC,        RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x32c4,  N,  N,  N),
-	PINGROUP(kb_row3_pr3,            KBC,        DISPLAYA,   RSVD3,        DISPLAYB,    RSVD3,    0x32c8,  N,  N,  N),
-	PINGROUP(kb_row4_pr4,            KBC,        DISPLAYA,   SPI2,         DISPLAYB,    KBC,      0x32cc,  N,  N,  N),
-	PINGROUP(kb_row5_pr5,            KBC,        DISPLAYA,   SPI2,         DISPLAYB,    KBC,      0x32d0,  N,  N,  N),
-	PINGROUP(kb_row6_pr6,            KBC,        DISPLAYA,   DISPLAYA_ALT, DISPLAYB,    KBC,      0x32d4,  N,  N,  N),
-	PINGROUP(kb_row7_pr7,            KBC,        RSVD2,      CLDVFS,       UARTA,       RSVD2,    0x32d8,  N,  N,  N),
-	PINGROUP(kb_row8_ps0,            KBC,        RSVD2,      CLDVFS,       UARTA,       RSVD2,    0x32dc,  N,  N,  N),
-	PINGROUP(kb_row9_ps1,            KBC,        RSVD2,      RSVD3,        UARTA,       RSVD3,    0x32e0,  N,  N,  N),
-	PINGROUP(kb_row10_ps2,           KBC,        RSVD2,      RSVD3,        UARTA,       RSVD3,    0x32e4,  N,  N,  N),
-	PINGROUP(kb_col0_pq0,            KBC,        USB,        SPI2,         EMC_DLL,     KBC,      0x32fc,  N,  N,  N),
-	PINGROUP(kb_col1_pq1,            KBC,        RSVD2,      SPI2,         EMC_DLL,     RSVD2,    0x3300,  N,  N,  N),
-	PINGROUP(kb_col2_pq2,            KBC,        RSVD2,      SPI2,         RSVD4,       RSVD2,    0x3304,  N,  N,  N),
-	PINGROUP(kb_col3_pq3,            KBC,        DISPLAYA,   PWM2,         UARTA,       KBC,      0x3308,  N,  N,  N),
-	PINGROUP(kb_col4_pq4,            KBC,        OWR,        SDMMC3,       UARTA,       KBC,      0x330c,  N,  N,  N),
-	PINGROUP(kb_col5_pq5,            KBC,        RSVD2,      SDMMC1,       RSVD4,       RSVD4,    0x3310,  N,  N,  N),
-	PINGROUP(kb_col6_pq6,            KBC,        RSVD2,      SPI2,         RSVD4,       RSVD4,    0x3314,  N,  N,  N),
-	PINGROUP(kb_col7_pq7,            KBC,        RSVD2,      SPI2,         RSVD4,       RSVD4,    0x3318,  N,  N,  N),
-	PINGROUP(clk_32k_out_pa0,        BLINK,      SOC,        RSVD3,        RSVD4,       RSVD4,    0x331c,  N,  N,  N),
-	PINGROUP(sys_clk_req_pz5,        SYSCLK,     RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3320,  N,  N,  N),
-	PINGROUP(core_pwr_req,           PWRON,      RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3324,  N,  N,  N),
-	PINGROUP(cpu_pwr_req,            CPU,        RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3328,  N,  N,  N),
-	PINGROUP(pwr_int_n,              PMI,        RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x332c,  N,  N,  N),
-	PINGROUP(clk_32k_in,             CLK,        RSVD2,      RSVD3,        RSVD4,       CLK,      0x3330,  N,  N,  N),
-	PINGROUP(owr,                    OWR,        RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3334,  N,  N,  Y),
-	PINGROUP(dap1_fs_pn0,            I2S0,       HDA,        GMI,          RSVD4,       RSVD4,    0x3338,  N,  N,  N),
-	PINGROUP(dap1_din_pn1,           I2S0,       HDA,        GMI,          RSVD4,       RSVD4,    0x333c,  N,  N,  N),
-	PINGROUP(dap1_dout_pn2,          I2S0,       HDA,        GMI,          RSVD4,       RSVD4,    0x3340,  N,  N,  N),
-	PINGROUP(dap1_sclk_pn3,          I2S0,       HDA,        GMI,          RSVD4,       RSVD4,    0x3344,  N,  N,  N),
-	PINGROUP(clk1_req_pee2,          DAP,        DAP1,       RSVD3,        RSVD4,       RSVD4,    0x3348,  N,  N,  N),
-	PINGROUP(clk1_out_pw4,           EXTPERIPH1, DAP2,       RSVD3,        RSVD4,       RSVD4,    0x334c,  N,  N,  N),
-	PINGROUP(spdif_in_pk6,           SPDIF,      USB,        RSVD3,        RSVD4,       RSVD4,    0x3350,  N,  N,  N),
-	PINGROUP(spdif_out_pk5,          SPDIF,      RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3354,  N,  N,  N),
-	PINGROUP(dap2_fs_pa2,            I2S1,       HDA,        RSVD3,        RSVD4,       RSVD4,    0x3358,  N,  N,  N),
-	PINGROUP(dap2_din_pa4,           I2S1,       HDA,        RSVD3,        RSVD4,       RSVD4,    0x335c,  N,  N,  N),
-	PINGROUP(dap2_dout_pa5,          I2S1,       HDA,        RSVD3,        RSVD4,       RSVD4,    0x3360,  N,  N,  N),
-	PINGROUP(dap2_sclk_pa3,          I2S1,       HDA,        RSVD3,        RSVD4,       RSVD4,    0x3364,  N,  N,  N),
-	PINGROUP(dvfs_pwm_px0,           SPI6,       CLDVFS,     RSVD3,        RSVD4,       RSVD4,    0x3368,  N,  N,  N),
-	PINGROUP(gpio_x1_aud_px1,        SPI6,       RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x336c,  N,  N,  N),
-	PINGROUP(gpio_x3_aud_px3,        SPI6,       SPI1,       RSVD3,        RSVD4,       RSVD4,    0x3370,  N,  N,  N),
-	PINGROUP(dvfs_clk_px2,           SPI6,       CLDVFS,     RSVD3,        RSVD4,       RSVD4,    0x3374,  N,  N,  N),
-	PINGROUP(gpio_x4_aud_px4,        RSVD1,      SPI1,       SPI2,         DAP2,        RSVD1,    0x3378,  N,  N,  N),
-	PINGROUP(gpio_x5_aud_px5,        RSVD1,      SPI1,       SPI2,         RSVD4,       RSVD1,    0x337c,  N,  N,  N),
-	PINGROUP(gpio_x6_aud_px6,        SPI6,       SPI1,       SPI2,         RSVD4,       RSVD4,    0x3380,  N,  N,  N),
-	PINGROUP(gpio_x7_aud_px7,        RSVD1,      SPI1,       SPI2,         RSVD4,       RSVD4,    0x3384,  N,  N,  N),
-	PINGROUP(sdmmc3_clk_pa6,         SDMMC3,     RSVD2,      RSVD3,        SPI3,        RSVD3,    0x3390,  N,  N,  N),
-	PINGROUP(sdmmc3_cmd_pa7,         SDMMC3,     PWM3,       UARTA,        SPI3,        SDMMC3,   0x3394,  N,  N,  N),
-	PINGROUP(sdmmc3_dat0_pb7,        SDMMC3,     RSVD2,      RSVD3,        SPI3,        RSVD3,    0x3398,  N,  N,  N),
-	PINGROUP(sdmmc3_dat1_pb6,        SDMMC3,     PWM2,       UARTA,        SPI3,        SDMMC3,   0x339c,  N,  N,  N),
-	PINGROUP(sdmmc3_dat2_pb5,        SDMMC3,     PWM1,       DISPLAYA,     SPI3,        SDMMC3,   0x33a0,  N,  N,  N),
-	PINGROUP(sdmmc3_dat3_pb4,        SDMMC3,     PWM0,       DISPLAYB,     SPI3,        SDMMC3,   0x33a4,  N,  N,  N),
-	PINGROUP(hdmi_cec_pee3,          CEC,        SDMMC3,     RSVD3,        SOC,         RSVD3,    0x33e0,  Y,  N,  N),
-	PINGROUP(sdmmc1_wp_n_pv3,        SDMMC1,     CLK12,      SPI4,         UARTA,       SDMMC1,   0x33e4,  N,  N,  N),
-	PINGROUP(sdmmc3_cd_n_pv2,        SDMMC3,     OWR,        RSVD3,        RSVD4,       RSVD4,    0x33e8,  N,  N,  N),
-	PINGROUP(gpio_w2_aud_pw2,        SPI6,       RSVD2,      SPI2,         I2C1,        RSVD2,    0x33ec,  N,  N,  N),
-	PINGROUP(gpio_w3_aud_pw3,        SPI6,       SPI1,       SPI2,         I2C1,        SPI6,     0x33f0,  N,  N,  N),
-	PINGROUP(usb_vbus_en0_pn4,       USB,        RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x33f4,  Y,  N,  N),
-	PINGROUP(usb_vbus_en1_pn5,       USB,        RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x33f8,  Y,  N,  N),
-	PINGROUP(sdmmc3_clk_lb_in_pee5,  SDMMC3,     RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x33fc,  N,  N,  N),
-	PINGROUP(sdmmc3_clk_lb_out_pee4, SDMMC3,     RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3400,  N,  N,  N),
-	PINGROUP(gmi_clk_lb,             SDMMC2,     NAND,       GMI,          RSVD4,       GMI,      0x3404,  N,  N,  N),
-	PINGROUP(reset_out_n,            RSVD1,      RSVD2,      RSVD3,        RESET_OUT_N, RSVD3,    0x3408,  N,  N,  N),
+	/*       pg_name,                f0,         f1,         f2,           f3,          r,      od, ior, rcv_sel */
+	PINGROUP(ulpi_data0_po1,         SPI3,       HSI,        UARTA,        ULPI,        0x3000, N,   N,  N),
+	PINGROUP(ulpi_data1_po2,         SPI3,       HSI,        UARTA,        ULPI,        0x3004, N,   N,  N),
+	PINGROUP(ulpi_data2_po3,         SPI3,       HSI,        UARTA,        ULPI,        0x3008, N,   N,  N),
+	PINGROUP(ulpi_data3_po4,         SPI3,       HSI,        UARTA,        ULPI,        0x300c, N,   N,  N),
+	PINGROUP(ulpi_data4_po5,         SPI2,       HSI,        UARTA,        ULPI,        0x3010, N,   N,  N),
+	PINGROUP(ulpi_data5_po6,         SPI2,       HSI,        UARTA,        ULPI,        0x3014, N,   N,  N),
+	PINGROUP(ulpi_data6_po7,         SPI2,       HSI,        UARTA,        ULPI,        0x3018, N,   N,  N),
+	PINGROUP(ulpi_data7_po0,         SPI2,       HSI,        UARTA,        ULPI,        0x301c, N,   N,  N),
+	PINGROUP(ulpi_clk_py0,           SPI1,       SPI5,       UARTD,        ULPI,        0x3020, N,   N,  N),
+	PINGROUP(ulpi_dir_py1,           SPI1,       SPI5,       UARTD,        ULPI,        0x3024, N,   N,  N),
+	PINGROUP(ulpi_nxt_py2,           SPI1,       SPI5,       UARTD,        ULPI,        0x3028, N,   N,  N),
+	PINGROUP(ulpi_stp_py3,           SPI1,       SPI5,       UARTD,        ULPI,        0x302c, N,   N,  N),
+	PINGROUP(dap3_fs_pp0,            I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    0x3030, N,   N,  N),
+	PINGROUP(dap3_din_pp1,           I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    0x3034, N,   N,  N),
+	PINGROUP(dap3_dout_pp2,          I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    0x3038, N,   N,  N),
+	PINGROUP(dap3_sclk_pp3,          I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    0x303c, N,   N,  N),
+	PINGROUP(pv0,                    USB,        RSVD2,      RSVD3,        RSVD4,       0x3040, N,   N,  N),
+	PINGROUP(pv1,                    RSVD1,      RSVD2,      RSVD3,        RSVD4,       0x3044, N,   N,  N),
+	PINGROUP(sdmmc1_clk_pz0,         SDMMC1,     CLK12,      RSVD3,        RSVD4,       0x3048, N,   N,  N),
+	PINGROUP(sdmmc1_cmd_pz1,         SDMMC1,     SPDIF,      SPI4,         UARTA,       0x304c, N,   N,  N),
+	PINGROUP(sdmmc1_dat3_py4,        SDMMC1,     SPDIF,      SPI4,         UARTA,       0x3050, N,   N,  N),
+	PINGROUP(sdmmc1_dat2_py5,        SDMMC1,     PWM0,       SPI4,         UARTA,       0x3054, N,   N,  N),
+	PINGROUP(sdmmc1_dat1_py6,        SDMMC1,     PWM1,       SPI4,         UARTA,       0x3058, N,   N,  N),
+	PINGROUP(sdmmc1_dat0_py7,        SDMMC1,     RSVD2,      SPI4,         UARTA,       0x305c, N,   N,  N),
+	PINGROUP(clk2_out_pw5,           EXTPERIPH2, RSVD2,      RSVD3,        RSVD4,       0x3068, N,   N,  N),
+	PINGROUP(clk2_req_pcc5,          DAP,        RSVD2,      RSVD3,        RSVD4,       0x306c, N,   N,  N),
+	PINGROUP(hdmi_int_pn7,           RSVD1,      RSVD2,      RSVD3,        RSVD4,       0x3110, N,   N,  Y),
+	PINGROUP(ddc_scl_pv4,            I2C4,       RSVD2,      RSVD3,        RSVD4,       0x3114, N,   N,  Y),
+	PINGROUP(ddc_sda_pv5,            I2C4,       RSVD2,      RSVD3,        RSVD4,       0x3118, N,   N,  Y),
+	PINGROUP(uart2_rxd_pc3,          IRDA,       SPDIF,      UARTA,        SPI4,        0x3164, N,   N,  N),
+	PINGROUP(uart2_txd_pc2,          IRDA,       SPDIF,      UARTA,        SPI4,        0x3168, N,   N,  N),
+	PINGROUP(uart2_rts_n_pj6,        UARTA,      UARTB,      RSVD3,        SPI4,        0x316c, N,   N,  N),
+	PINGROUP(uart2_cts_n_pj5,        UARTA,      UARTB,      RSVD3,        SPI4,        0x3170, N,   N,  N),
+	PINGROUP(uart3_txd_pw6,          UARTC,      RSVD2,      RSVD3,        SPI4,        0x3174, N,   N,  N),
+	PINGROUP(uart3_rxd_pw7,          UARTC,      RSVD2,      RSVD3,        SPI4,        0x3178, N,   N,  N),
+	PINGROUP(uart3_cts_n_pa1,        UARTC,      SDMMC1,     DTV,          SPI4,        0x317c, N,   N,  N),
+	PINGROUP(uart3_rts_n_pc0,        UARTC,      PWM0,       DTV,          DISPLAYA,    0x3180, N,   N,  N),
+	PINGROUP(pu0,                    OWR,        UARTA,      RSVD3,        RSVD4,       0x3184, N,   N,  N),
+	PINGROUP(pu1,                    RSVD1,      UARTA,      RSVD3,        RSVD4,       0x3188, N,   N,  N),
+	PINGROUP(pu2,                    RSVD1,      UARTA,      RSVD3,        RSVD4,       0x318c, N,   N,  N),
+	PINGROUP(pu3,                    PWM0,       UARTA,      DISPLAYA,     DISPLAYB,    0x3190, N,   N,  N),
+	PINGROUP(pu4,                    PWM1,       UARTA,      DISPLAYA,     DISPLAYB,    0x3194, N,   N,  N),
+	PINGROUP(pu5,                    PWM2,       UARTA,      DISPLAYA,     DISPLAYB,    0x3198, N,   N,  N),
+	PINGROUP(pu6,                    PWM3,       UARTA,      USB,          DISPLAYB,    0x319c, N,   N,  N),
+	PINGROUP(gen1_i2c_sda_pc5,       I2C1,       RSVD2,      RSVD3,        RSVD4,       0x31a0, Y,   N,  N),
+	PINGROUP(gen1_i2c_scl_pc4,       I2C1,       RSVD2,      RSVD3,        RSVD4,       0x31a4, Y,   N,  N),
+	PINGROUP(dap4_fs_pp4,            I2S3,       RSVD2,      DTV,          RSVD4,       0x31a8, N,   N,  N),
+	PINGROUP(dap4_din_pp5,           I2S3,       RSVD2,      RSVD3,        RSVD4,       0x31ac, N,   N,  N),
+	PINGROUP(dap4_dout_pp6,          I2S3,       RSVD2,      DTV,          RSVD4,       0x31b0, N,   N,  N),
+	PINGROUP(dap4_sclk_pp7,          I2S3,       RSVD2,      RSVD3,        RSVD4,       0x31b4, N,   N,  N),
+	PINGROUP(clk3_out_pee0,          EXTPERIPH3, RSVD2,      RSVD3,        RSVD4,       0x31b8, N,   N,  N),
+	PINGROUP(clk3_req_pee1,          DEV3,       RSVD2,      RSVD3,        RSVD4,       0x31bc, N,   N,  N),
+	PINGROUP(gmi_wp_n_pc7,           RSVD1,      NAND,       GMI,          GMI_ALT,     0x31c0, N,   N,  N),
+	PINGROUP(gmi_iordy_pi5,          SDMMC2,     RSVD2,      GMI,          TRACE,       0x31c4, N,   N,  N),
+	PINGROUP(gmi_wait_pi7,           SPI4,       NAND,       GMI,          DTV,         0x31c8, N,   N,  N),
+	PINGROUP(gmi_adv_n_pk0,          RSVD1,      NAND,       GMI,          TRACE,       0x31cc, N,   N,  N),
+	PINGROUP(gmi_clk_pk1,            SDMMC2,     NAND,       GMI,          TRACE,       0x31d0, N,   N,  N),
+	PINGROUP(gmi_cs0_n_pj0,          RSVD1,      NAND,       GMI,          USB,         0x31d4, N,   N,  N),
+	PINGROUP(gmi_cs1_n_pj2,          RSVD1,      NAND,       GMI,          SOC,         0x31d8, N,   N,  N),
+	PINGROUP(gmi_cs2_n_pk3,          SDMMC2,     NAND,       GMI,          TRACE,       0x31dc, N,   N,  N),
+	PINGROUP(gmi_cs3_n_pk4,          SDMMC2,     NAND,       GMI,          GMI_ALT,     0x31e0, N,   N,  N),
+	PINGROUP(gmi_cs4_n_pk2,          USB,        NAND,       GMI,          TRACE,       0x31e4, N,   N,  N),
+	PINGROUP(gmi_cs6_n_pi3,          NAND,       NAND_ALT,   GMI,          SPI4,        0x31e8, N,   N,  N),
+	PINGROUP(gmi_cs7_n_pi6,          NAND,       NAND_ALT,   GMI,          SDMMC2,      0x31ec, N,   N,  N),
+	PINGROUP(gmi_ad0_pg0,            RSVD1,      NAND,       GMI,          RSVD4,       0x31f0, N,   N,  N),
+	PINGROUP(gmi_ad1_pg1,            RSVD1,      NAND,       GMI,          RSVD4,       0x31f4, N,   N,  N),
+	PINGROUP(gmi_ad2_pg2,            RSVD1,      NAND,       GMI,          RSVD4,       0x31f8, N,   N,  N),
+	PINGROUP(gmi_ad3_pg3,            RSVD1,      NAND,       GMI,          RSVD4,       0x31fc, N,   N,  N),
+	PINGROUP(gmi_ad4_pg4,            RSVD1,      NAND,       GMI,          RSVD4,       0x3200, N,   N,  N),
+	PINGROUP(gmi_ad5_pg5,            RSVD1,      NAND,       GMI,          SPI4,        0x3204, N,   N,  N),
+	PINGROUP(gmi_ad6_pg6,            RSVD1,      NAND,       GMI,          SPI4,        0x3208, N,   N,  N),
+	PINGROUP(gmi_ad7_pg7,            RSVD1,      NAND,       GMI,          SPI4,        0x320c, N,   N,  N),
+	PINGROUP(gmi_ad8_ph0,            PWM0,       NAND,       GMI,          DTV,         0x3210, N,   N,  N),
+	PINGROUP(gmi_ad9_ph1,            PWM1,       NAND,       GMI,          CLDVFS,      0x3214, N,   N,  N),
+	PINGROUP(gmi_ad10_ph2,           PWM2,       NAND,       GMI,          CLDVFS,      0x3218, N,   N,  N),
+	PINGROUP(gmi_ad11_ph3,           PWM3,       NAND,       GMI,          USB,         0x321c, N,   N,  N),
+	PINGROUP(gmi_ad12_ph4,           SDMMC2,     NAND,       GMI,          RSVD4,       0x3220, N,   N,  N),
+	PINGROUP(gmi_ad13_ph5,           SDMMC2,     NAND,       GMI,          RSVD4,       0x3224, N,   N,  N),
+	PINGROUP(gmi_ad14_ph6,           SDMMC2,     NAND,       GMI,          DTV,         0x3228, N,   N,  N),
+	PINGROUP(gmi_ad15_ph7,           SDMMC2,     NAND,       GMI,          DTV,         0x322c, N,   N,  N),
+	PINGROUP(gmi_a16_pj7,            UARTD,      TRACE,      GMI,          GMI_ALT,     0x3230, N,   N,  N),
+	PINGROUP(gmi_a17_pb0,            UARTD,      RSVD2,      GMI,          TRACE,       0x3234, N,   N,  N),
+	PINGROUP(gmi_a18_pb1,            UARTD,      RSVD2,      GMI,          TRACE,       0x3238, N,   N,  N),
+	PINGROUP(gmi_a19_pk7,            UARTD,      SPI4,       GMI,          TRACE,       0x323c, N,   N,  N),
+	PINGROUP(gmi_wr_n_pi0,           RSVD1,      NAND,       GMI,          SPI4,        0x3240, N,   N,  N),
+	PINGROUP(gmi_oe_n_pi1,           RSVD1,      NAND,       GMI,          SOC,         0x3244, N,   N,  N),
+	PINGROUP(gmi_dqs_p_pj3,          SDMMC2,     NAND,       GMI,          TRACE,       0x3248, N,   N,  N),
+	PINGROUP(gmi_rst_n_pi4,          NAND,       NAND_ALT,   GMI,          RSVD4,       0x324c, N,   N,  N),
+	PINGROUP(gen2_i2c_scl_pt5,       I2C2,       RSVD2,      GMI,          RSVD4,       0x3250, Y,   N,  N),
+	PINGROUP(gen2_i2c_sda_pt6,       I2C2,       RSVD2,      GMI,          RSVD4,       0x3254, Y,   N,  N),
+	PINGROUP(sdmmc4_clk_pcc4,        SDMMC4,     RSVD2,      GMI,          RSVD4,       0x3258, N,   Y,  N),
+	PINGROUP(sdmmc4_cmd_pt7,         SDMMC4,     RSVD2,      GMI,          RSVD4,       0x325c, N,   Y,  N),
+	PINGROUP(sdmmc4_dat0_paa0,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x3260, N,   Y,  N),
+	PINGROUP(sdmmc4_dat1_paa1,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x3264, N,   Y,  N),
+	PINGROUP(sdmmc4_dat2_paa2,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x3268, N,   Y,  N),
+	PINGROUP(sdmmc4_dat3_paa3,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x326c, N,   Y,  N),
+	PINGROUP(sdmmc4_dat4_paa4,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x3270, N,   Y,  N),
+	PINGROUP(sdmmc4_dat5_paa5,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x3274, N,   Y,  N),
+	PINGROUP(sdmmc4_dat6_paa6,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x3278, N,   Y,  N),
+	PINGROUP(sdmmc4_dat7_paa7,       SDMMC4,     RSVD2,      GMI,          RSVD4,       0x327c, N,   Y,  N),
+	PINGROUP(cam_mclk_pcc0,          VI,         VI_ALT1,    VI_ALT3,      RSVD4,       0x3284, N,   N,  N),
+	PINGROUP(pcc1,                   I2S4,       RSVD2,      RSVD3,        RSVD4,       0x3288, N,   N,  N),
+	PINGROUP(pbb0,                   I2S4,       VI,         VI_ALT1,      VI_ALT3,     0x328c, N,   N,  N),
+	PINGROUP(cam_i2c_scl_pbb1,       VGP1,       I2C3,       RSVD3,        RSVD4,       0x3290, Y,   N,  N),
+	PINGROUP(cam_i2c_sda_pbb2,       VGP2,       I2C3,       RSVD3,        RSVD4,       0x3294, Y,   N,  N),
+	PINGROUP(pbb3,                   VGP3,       DISPLAYA,   DISPLAYB,     RSVD4,       0x3298, N,   N,  N),
+	PINGROUP(pbb4,                   VGP4,       DISPLAYA,   DISPLAYB,     RSVD4,       0x329c, N,   N,  N),
+	PINGROUP(pbb5,                   VGP5,       DISPLAYA,   DISPLAYB,     RSVD4,       0x32a0, N,   N,  N),
+	PINGROUP(pbb6,                   VGP6,       DISPLAYA,   DISPLAYB,     RSVD4,       0x32a4, N,   N,  N),
+	PINGROUP(pbb7,                   I2S4,       RSVD2,      RSVD3,        RSVD4,       0x32a8, N,   N,  N),
+	PINGROUP(pcc2,                   I2S4,       RSVD2,      RSVD3,        RSVD4,       0x32ac, N,   N,  N),
+	PINGROUP(jtag_rtck,              RTCK,       RSVD2,      RSVD3,        RSVD4,       0x32b0, N,   N,  N),
+	PINGROUP(pwr_i2c_scl_pz6,        I2CPWR,     RSVD2,      RSVD3,        RSVD4,       0x32b4, Y,   N,  N),
+	PINGROUP(pwr_i2c_sda_pz7,        I2CPWR,     RSVD2,      RSVD3,        RSVD4,       0x32b8, Y,   N,  N),
+	PINGROUP(kb_row0_pr0,            KBC,        RSVD2,      RSVD3,        RSVD4,       0x32bc, N,   N,  N),
+	PINGROUP(kb_row1_pr1,            KBC,        RSVD2,      RSVD3,        RSVD4,       0x32c0, N,   N,  N),
+	PINGROUP(kb_row2_pr2,            KBC,        RSVD2,      RSVD3,        RSVD4,       0x32c4, N,   N,  N),
+	PINGROUP(kb_row3_pr3,            KBC,        DISPLAYA,   RSVD3,        DISPLAYB,    0x32c8, N,   N,  N),
+	PINGROUP(kb_row4_pr4,            KBC,        DISPLAYA,   SPI2,         DISPLAYB,    0x32cc, N,   N,  N),
+	PINGROUP(kb_row5_pr5,            KBC,        DISPLAYA,   SPI2,         DISPLAYB,    0x32d0, N,   N,  N),
+	PINGROUP(kb_row6_pr6,            KBC,        DISPLAYA,   DISPLAYA_ALT, DISPLAYB,    0x32d4, N,   N,  N),
+	PINGROUP(kb_row7_pr7,            KBC,        RSVD2,      CLDVFS,       UARTA,       0x32d8, N,   N,  N),
+	PINGROUP(kb_row8_ps0,            KBC,        RSVD2,      CLDVFS,       UARTA,       0x32dc, N,   N,  N),
+	PINGROUP(kb_row9_ps1,            KBC,        RSVD2,      RSVD3,        UARTA,       0x32e0, N,   N,  N),
+	PINGROUP(kb_row10_ps2,           KBC,        RSVD2,      RSVD3,        UARTA,       0x32e4, N,   N,  N),
+	PINGROUP(kb_col0_pq0,            KBC,        USB,        SPI2,         EMC_DLL,     0x32fc, N,   N,  N),
+	PINGROUP(kb_col1_pq1,            KBC,        RSVD2,      SPI2,         EMC_DLL,     0x3300, N,   N,  N),
+	PINGROUP(kb_col2_pq2,            KBC,        RSVD2,      SPI2,         RSVD4,       0x3304, N,   N,  N),
+	PINGROUP(kb_col3_pq3,            KBC,        DISPLAYA,   PWM2,         UARTA,       0x3308, N,   N,  N),
+	PINGROUP(kb_col4_pq4,            KBC,        OWR,        SDMMC3,       UARTA,       0x330c, N,   N,  N),
+	PINGROUP(kb_col5_pq5,            KBC,        RSVD2,      SDMMC1,       RSVD4,       0x3310, N,   N,  N),
+	PINGROUP(kb_col6_pq6,            KBC,        RSVD2,      SPI2,         RSVD4,       0x3314, N,   N,  N),
+	PINGROUP(kb_col7_pq7,            KBC,        RSVD2,      SPI2,         RSVD4,       0x3318, N,   N,  N),
+	PINGROUP(clk_32k_out_pa0,        BLINK,      SOC,        RSVD3,        RSVD4,       0x331c, N,   N,  N),
+	PINGROUP(sys_clk_req_pz5,        SYSCLK,     RSVD2,      RSVD3,        RSVD4,       0x3320, N,   N,  N),
+	PINGROUP(core_pwr_req,           PWRON,      RSVD2,      RSVD3,        RSVD4,       0x3324, N,   N,  N),
+	PINGROUP(cpu_pwr_req,            CPU,        RSVD2,      RSVD3,        RSVD4,       0x3328, N,   N,  N),
+	PINGROUP(pwr_int_n,              PMI,        RSVD2,      RSVD3,        RSVD4,       0x332c, N,   N,  N),
+	PINGROUP(clk_32k_in,             CLK,        RSVD2,      RSVD3,        RSVD4,       0x3330, N,   N,  N),
+	PINGROUP(owr,                    OWR,        RSVD2,      RSVD3,        RSVD4,       0x3334, N,   N,  Y),
+	PINGROUP(dap1_fs_pn0,            I2S0,       HDA,        GMI,          RSVD4,       0x3338, N,   N,  N),
+	PINGROUP(dap1_din_pn1,           I2S0,       HDA,        GMI,          RSVD4,       0x333c, N,   N,  N),
+	PINGROUP(dap1_dout_pn2,          I2S0,       HDA,        GMI,          RSVD4,       0x3340, N,   N,  N),
+	PINGROUP(dap1_sclk_pn3,          I2S0,       HDA,        GMI,          RSVD4,       0x3344, N,   N,  N),
+	PINGROUP(clk1_req_pee2,          DAP,        DAP1,       RSVD3,        RSVD4,       0x3348, N,   N,  N),
+	PINGROUP(clk1_out_pw4,           EXTPERIPH1, DAP2,       RSVD3,        RSVD4,       0x334c, N,   N,  N),
+	PINGROUP(spdif_in_pk6,           SPDIF,      USB,        RSVD3,        RSVD4,       0x3350, N,   N,  N),
+	PINGROUP(spdif_out_pk5,          SPDIF,      RSVD2,      RSVD3,        RSVD4,       0x3354, N,   N,  N),
+	PINGROUP(dap2_fs_pa2,            I2S1,       HDA,        RSVD3,        RSVD4,       0x3358, N,   N,  N),
+	PINGROUP(dap2_din_pa4,           I2S1,       HDA,        RSVD3,        RSVD4,       0x335c, N,   N,  N),
+	PINGROUP(dap2_dout_pa5,          I2S1,       HDA,        RSVD3,        RSVD4,       0x3360, N,   N,  N),
+	PINGROUP(dap2_sclk_pa3,          I2S1,       HDA,        RSVD3,        RSVD4,       0x3364, N,   N,  N),
+	PINGROUP(dvfs_pwm_px0,           SPI6,       CLDVFS,     RSVD3,        RSVD4,       0x3368, N,   N,  N),
+	PINGROUP(gpio_x1_aud_px1,        SPI6,       RSVD2,      RSVD3,        RSVD4,       0x336c, N,   N,  N),
+	PINGROUP(gpio_x3_aud_px3,        SPI6,       SPI1,       RSVD3,        RSVD4,       0x3370, N,   N,  N),
+	PINGROUP(dvfs_clk_px2,           SPI6,       CLDVFS,     RSVD3,        RSVD4,       0x3374, N,   N,  N),
+	PINGROUP(gpio_x4_aud_px4,        RSVD1,      SPI1,       SPI2,         DAP2,        0x3378, N,   N,  N),
+	PINGROUP(gpio_x5_aud_px5,        RSVD1,      SPI1,       SPI2,         RSVD4,       0x337c, N,   N,  N),
+	PINGROUP(gpio_x6_aud_px6,        SPI6,       SPI1,       SPI2,         RSVD4,       0x3380, N,   N,  N),
+	PINGROUP(gpio_x7_aud_px7,        RSVD1,      SPI1,       SPI2,         RSVD4,       0x3384, N,   N,  N),
+	PINGROUP(sdmmc3_clk_pa6,         SDMMC3,     RSVD2,      RSVD3,        SPI3,        0x3390, N,   N,  N),
+	PINGROUP(sdmmc3_cmd_pa7,         SDMMC3,     PWM3,       UARTA,        SPI3,        0x3394, N,   N,  N),
+	PINGROUP(sdmmc3_dat0_pb7,        SDMMC3,     RSVD2,      RSVD3,        SPI3,        0x3398, N,   N,  N),
+	PINGROUP(sdmmc3_dat1_pb6,        SDMMC3,     PWM2,       UARTA,        SPI3,        0x339c, N,   N,  N),
+	PINGROUP(sdmmc3_dat2_pb5,        SDMMC3,     PWM1,       DISPLAYA,     SPI3,        0x33a0, N,   N,  N),
+	PINGROUP(sdmmc3_dat3_pb4,        SDMMC3,     PWM0,       DISPLAYB,     SPI3,        0x33a4, N,   N,  N),
+	PINGROUP(hdmi_cec_pee3,          CEC,        SDMMC3,     RSVD3,        SOC,         0x33e0, Y,   N,  N),
+	PINGROUP(sdmmc1_wp_n_pv3,        SDMMC1,     CLK12,      SPI4,         UARTA,       0x33e4, N,   N,  N),
+	PINGROUP(sdmmc3_cd_n_pv2,        SDMMC3,     OWR,        RSVD3,        RSVD4,       0x33e8, N,   N,  N),
+	PINGROUP(gpio_w2_aud_pw2,        SPI6,       RSVD2,      SPI2,         I2C1,        0x33ec, N,   N,  N),
+	PINGROUP(gpio_w3_aud_pw3,        SPI6,       SPI1,       SPI2,         I2C1,        0x33f0, N,   N,  N),
+	PINGROUP(usb_vbus_en0_pn4,       USB,        RSVD2,      RSVD3,        RSVD4,       0x33f4, Y,   N,  N),
+	PINGROUP(usb_vbus_en1_pn5,       USB,        RSVD2,      RSVD3,        RSVD4,       0x33f8, Y,   N,  N),
+	PINGROUP(sdmmc3_clk_lb_in_pee5,  SDMMC3,     RSVD2,      RSVD3,        RSVD4,       0x33fc, N,   N,  N),
+	PINGROUP(sdmmc3_clk_lb_out_pee4, SDMMC3,     RSVD2,      RSVD3,        RSVD4,       0x3400, N,   N,  N),
+	PINGROUP(gmi_clk_lb,             SDMMC2,     NAND,       GMI,          RSVD4,       0x3404, N,   N,  N),
+	PINGROUP(reset_out_n,            RSVD1,      RSVD2,      RSVD3,        RESET_OUT_N, 0x3408, N,   N,  N),
 
 	/* pg_name, r, hsm_b, schmitt_b, lpmd_b, drvdn_b, drvdn_w, drvup_b, drvup_w, slwr_b, slwr_w, slwf_b, slwf_w, drvtype */
 	DRV_PINGROUP(ao1,         0x868,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
diff --git a/drivers/pinctrl/pinctrl-tegra124.c b/drivers/pinctrl/pinctrl-tegra124.c
index 73773706755..e80797e2001 100644
--- a/drivers/pinctrl/pinctrl-tegra124.c
+++ b/drivers/pinctrl/pinctrl-tegra124.c
@@ -1677,10 +1677,12 @@ static struct tegra_function tegra124_functions[] = {
 #define DRV_PINGROUP_REG_A		0x868	/* bank 0 */
 #define PINGROUP_REG_A			0x3000	/* bank 1 */
 
-#define PINGROUP_REG_Y(r)		((r) - PINGROUP_REG_A)
-#define PINGROUP_REG_N(r)		-1
+#define PINGROUP_REG(r)			((r) - PINGROUP_REG_A)
 
-#define PINGROUP(pg_name, f0, f1, f2, f3, f_safe, r, od, ior, rcv_sel)	\
+#define PINGROUP_BIT_Y(b)		(b)
+#define PINGROUP_BIT_N(b)		(-1)
+
+#define PINGROUP(pg_name, f0, f1, f2, f3, r, od, ior, rcv_sel)		\
 	{								\
 		.name = #pg_name,					\
 		.pins = pg_name##_pins,					\
@@ -1691,38 +1693,24 @@ static struct tegra_function tegra124_functions[] = {
 			TEGRA_MUX_##f2,					\
 			TEGRA_MUX_##f3,					\
 		},							\
-		.func_safe = TEGRA_MUX_##f_safe,			\
-		.mux_reg = PINGROUP_REG_Y(r),				\
+		.mux_reg = PINGROUP_REG(r),				\
 		.mux_bank = 1,						\
 		.mux_bit = 0,						\
-		.pupd_reg = PINGROUP_REG_Y(r),				\
+		.pupd_reg = PINGROUP_REG(r),				\
 		.pupd_bank = 1,						\
 		.pupd_bit = 2,						\
-		.tri_reg = PINGROUP_REG_Y(r),				\
+		.tri_reg = PINGROUP_REG(r),				\
 		.tri_bank = 1,						\
 		.tri_bit = 4,						\
-		.einput_reg = PINGROUP_REG_Y(r),			\
-		.einput_bank = 1,					\
-		.einput_bit = 5,					\
-		.odrain_reg = PINGROUP_REG_##od(r),			\
-		.odrain_bank = 1,					\
-		.odrain_bit = 6,					\
-		.lock_reg = PINGROUP_REG_Y(r),				\
-		.lock_bank = 1,						\
-		.lock_bit = 7,						\
-		.ioreset_reg = PINGROUP_REG_##ior(r),			\
-		.ioreset_bank = 1,					\
-		.ioreset_bit = 8,					\
-		.rcv_sel_reg = PINGROUP_REG_##rcv_sel(r),		\
-		.rcv_sel_bank = 1,					\
-		.rcv_sel_bit = 9,					\
+		.einput_bit = PINGROUP_BIT_Y(5),			\
+		.odrain_bit = PINGROUP_BIT_##od(6),			\
+		.lock_bit = PINGROUP_BIT_Y(7),				\
+		.ioreset_bit = PINGROUP_BIT_##ior(8),			\
+		.rcv_sel_bit = PINGROUP_BIT_##rcv_sel(9),		\
 		.drv_reg = -1,						\
-		.drvtype_reg = -1,					\
 	}
 
-#define DRV_PINGROUP_REG_Y(r)		((r) - DRV_PINGROUP_REG_A)
-#define DRV_PINGROUP_REG_N(r)		-1
-
+#define DRV_PINGROUP_REG(r)		((r) - DRV_PINGROUP_REG_A)
 
 #define DRV_PINGROUP(pg_name, r, hsm_b, schmitt_b, lpmd_b,		\
 		     drvdn_b, drvdn_w, drvup_b, drvup_w,		\
@@ -1735,12 +1723,12 @@ static struct tegra_function tegra124_functions[] = {
 		.mux_reg = -1,						\
 		.pupd_reg = -1,						\
 		.tri_reg = -1,						\
-		.einput_reg = -1,					\
-		.odrain_reg = -1,					\
-		.lock_reg = -1,						\
-		.ioreset_reg = -1,					\
-		.rcv_sel_reg = -1,					\
-		.drv_reg = DRV_PINGROUP_REG_Y(r),			\
+		.einput_bit = -1,					\
+		.odrain_bit = -1,					\
+		.lock_bit = -1,						\
+		.ioreset_bit = -1,					\
+		.rcv_sel_bit = -1,					\
+		.drv_reg = DRV_PINGROUP_REG(r),				\
 		.drv_bank = 0,						\
 		.hsm_bit = hsm_b,					\
 		.schmitt_bit = schmitt_b,				\
@@ -1753,246 +1741,244 @@ static struct tegra_function tegra124_functions[] = {
 		.slwr_width = slwr_w,					\
 		.slwf_bit = slwf_b,					\
 		.slwf_width = slwf_w,					\
-		.drvtype_reg = DRV_PINGROUP_REG_##drvtype(r),		\
-		.drvtype_bank = 0,					\
-		.drvtype_bit = 6,					\
+		.drvtype_bit = PINGROUP_BIT_##drvtype(6),		\
 	}
 
 static const struct tegra_pingroup tegra124_groups[] = {
-       /*       pg_name,                f0,         f1,         f2,           f3,          safe,       r,      od, ior, rcv_sel */
-       PINGROUP(ulpi_data0_po1,         SPI3,       HSI,        UARTA,        ULPI,        SPI3,       0x3000,  N,  N,  N),
-       PINGROUP(ulpi_data1_po2,         SPI3,       HSI,        UARTA,        ULPI,        SPI3,       0x3004,  N,  N,  N),
-       PINGROUP(ulpi_data2_po3,         SPI3,       HSI,        UARTA,        ULPI,        SPI3,       0x3008,  N,  N,  N),
-       PINGROUP(ulpi_data3_po4,         SPI3,       HSI,        UARTA,        ULPI,        SPI3,       0x300c,  N,  N,  N),
-       PINGROUP(ulpi_data4_po5,         SPI2,       HSI,        UARTA,        ULPI,        SPI2,       0x3010,  N,  N,  N),
-       PINGROUP(ulpi_data5_po6,         SPI2,       HSI,        UARTA,        ULPI,        SPI2,       0x3014,  N,  N,  N),
-       PINGROUP(ulpi_data6_po7,         SPI2,       HSI,        UARTA,        ULPI,        SPI2,       0x3018,  N,  N,  N),
-       PINGROUP(ulpi_data7_po0,         SPI2,       HSI,        UARTA,        ULPI,        SPI2,       0x301c,  N,  N,  N),
-       PINGROUP(ulpi_clk_py0,           SPI1,       SPI5,       UARTD,        ULPI,        SPI1,       0x3020,  N,  N,  N),
-       PINGROUP(ulpi_dir_py1,           SPI1,       SPI5,       UARTD,        ULPI,        SPI1,       0x3024,  N,  N,  N),
-       PINGROUP(ulpi_nxt_py2,           SPI1,       SPI5,       UARTD,        ULPI,        SPI1,       0x3028,  N,  N,  N),
-       PINGROUP(ulpi_stp_py3,           SPI1,       SPI5,       UARTD,        ULPI,        SPI1,       0x302c,  N,  N,  N),
-       PINGROUP(dap3_fs_pp0,            I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    I2S2,       0x3030,  N,  N,  N),
-       PINGROUP(dap3_din_pp1,           I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    I2S2,       0x3034,  N,  N,  N),
-       PINGROUP(dap3_dout_pp2,          I2S2,       SPI5,       DISPLAYA,     RSVD4,       I2S2,       0x3038,  N,  N,  N),
-       PINGROUP(dap3_sclk_pp3,          I2S2,       SPI5,       RSVD3,        DISPLAYB,    I2S2,       0x303c,  N,  N,  N),
-       PINGROUP(pv0,                    RSVD1,      RSVD2,      RSVD3,        RSVD4,       RSVD1,      0x3040,  N,  N,  N),
-       PINGROUP(pv1,                    RSVD1,      RSVD2,      RSVD3,        RSVD4,       RSVD1,      0x3044,  N,  N,  N),
-       PINGROUP(sdmmc1_clk_pz0,         SDMMC1,     CLK12,      RSVD3,        RSVD4,       RSVD3,      0x3048,  N,  N,  N),
-       PINGROUP(sdmmc1_cmd_pz1,         SDMMC1,     SPDIF,      SPI4,         UARTA,       SDMMC1,     0x304c,  N,  N,  N),
-       PINGROUP(sdmmc1_dat3_py4,        SDMMC1,     SPDIF,      SPI4,         UARTA,       SDMMC1,     0x3050,  N,  N,  N),
-       PINGROUP(sdmmc1_dat2_py5,        SDMMC1,     PWM0,       SPI4,         UARTA,       SDMMC1,     0x3054,  N,  N,  N),
-       PINGROUP(sdmmc1_dat1_py6,        SDMMC1,     PWM1,       SPI4,         UARTA,       SDMMC1,     0x3058,  N,  N,  N),
-       PINGROUP(sdmmc1_dat0_py7,        SDMMC1,     RSVD2,      SPI4,         UARTA,       SDMMC1,     0x305c,  N,  N,  N),
-       PINGROUP(clk2_out_pw5,           EXTPERIPH2, RSVD2,      RSVD3,        RSVD4,       EXTPERIPH2, 0x3068,  N,  N,  N),
-       PINGROUP(clk2_req_pcc5,          DAP,        RSVD2,      RSVD3,        RSVD4,       DAP,        0x306c,  N,  N,  N),
-       PINGROUP(hdmi_int_pn7,           RSVD1,      RSVD2,      RSVD3,        RSVD4,       RSVD1,      0x3110,  N,  N,  Y),
-       PINGROUP(ddc_scl_pv4,            I2C4,       RSVD2,      RSVD3,        RSVD4,       I2C4,       0x3114,  N,  N,  Y),
-       PINGROUP(ddc_sda_pv5,            I2C4,       RSVD2,      RSVD3,        RSVD4,       I2C4,       0x3118,  N,  N,  Y),
-       PINGROUP(uart2_rxd_pc3,          IRDA,       SPDIF,      UARTA,        SPI4,        IRDA,       0x3164,  N,  N,  N),
-       PINGROUP(uart2_txd_pc2,          IRDA,       SPDIF,      UARTA,        SPI4,        IRDA,       0x3168,  N,  N,  N),
-       PINGROUP(uart2_rts_n_pj6,        UARTA,      UARTB,      GMI,          SPI4,        UARTA,      0x316c,  N,  N,  N),
-       PINGROUP(uart2_cts_n_pj5,        UARTA,      UARTB,      GMI,          SPI4,        UARTA,      0x3170,  N,  N,  N),
-       PINGROUP(uart3_txd_pw6,          UARTC,      RSVD2,      GMI,          SPI4,        UARTC,      0x3174,  N,  N,  N),
-       PINGROUP(uart3_rxd_pw7,          UARTC,      RSVD2,      GMI,          SPI4,        UARTC,      0x3178,  N,  N,  N),
-       PINGROUP(uart3_cts_n_pa1,        UARTC,      SDMMC1,     DTV,          GMI,         UARTC,      0x317c,  N,  N,  N),
-       PINGROUP(uart3_rts_n_pc0,        UARTC,      PWM0,       DTV,          GMI,         UARTC,      0x3180,  N,  N,  N),
-       PINGROUP(pu0,                    OWR,        UARTA,      GMI,          RSVD4,       RSVD4,      0x3184,  N,  N,  N),
-       PINGROUP(pu1,                    RSVD1,      UARTA,      GMI,          RSVD4,       RSVD4,      0x3188,  N,  N,  N),
-       PINGROUP(pu2,                    RSVD1,      UARTA,      GMI,          RSVD4,       RSVD4,      0x318c,  N,  N,  N),
-       PINGROUP(pu3,                    PWM0,       UARTA,      GMI,          DISPLAYB,    PWM0,       0x3190,  N,  N,  N),
-       PINGROUP(pu4,                    PWM1,       UARTA,      GMI,          DISPLAYB,    PWM1,       0x3194,  N,  N,  N),
-       PINGROUP(pu5,                    PWM2,       UARTA,      GMI,          DISPLAYB,    PWM2,       0x3198,  N,  N,  N),
-       PINGROUP(pu6,                    PWM3,       UARTA,      RSVD3,        GMI,         RSVD3,      0x319c,  N,  N,  N),
-       PINGROUP(gen1_i2c_sda_pc5,       I2C1,       RSVD2,      RSVD3,        RSVD4,       I2C1,       0x31a0,  Y,  N,  N),
-       PINGROUP(gen1_i2c_scl_pc4,       I2C1,       RSVD2,      RSVD3,        RSVD4,       I2C1,       0x31a4,  Y,  N,  N),
-       PINGROUP(dap4_fs_pp4,            I2S3,       GMI,        DTV,          RSVD4,       I2S3,       0x31a8,  N,  N,  N),
-       PINGROUP(dap4_din_pp5,           I2S3,       GMI,        RSVD3,        RSVD4,       I2S3,       0x31ac,  N,  N,  N),
-       PINGROUP(dap4_dout_pp6,          I2S3,       GMI,        DTV,          RSVD4,       I2S3,       0x31b0,  N,  N,  N),
-       PINGROUP(dap4_sclk_pp7,          I2S3,       GMI,        RSVD3,        RSVD4,       I2S3,       0x31b4,  N,  N,  N),
-       PINGROUP(clk3_out_pee0,          EXTPERIPH3, RSVD2,      RSVD3,        RSVD4,       RSVD3,      0x31b8,  N,  N,  N),
-       PINGROUP(clk3_req_pee1,          DEV3,       RSVD2,      RSVD3,        RSVD4,       RSVD4,      0x31bc,  N,  N,  N),
-       PINGROUP(pc7,                    RSVD1,      RSVD2,      GMI,          GMI_ALT,     RSVD1,      0x31c0,  N,  N,  N),
-       PINGROUP(pi5,                    SDMMC2,     RSVD2,      GMI,          RSVD4,       GMI,        0x31c4,  N,  N,  N),
-       PINGROUP(pi7,                    RSVD1,      TRACE,      GMI,          DTV,         RSVD1,      0x31c8,  N,  N,  N),
-       PINGROUP(pk0,                    RSVD1,      SDMMC3,     GMI,          SOC,         RSVD1,      0x31cc,  N,  N,  N),
-       PINGROUP(pk1,                    SDMMC2,     TRACE,      GMI,          RSVD4,       GMI,        0x31d0,  N,  N,  N),
-       PINGROUP(pj0,                    RSVD1,      RSVD2,      GMI,          USB,         RSVD1,      0x31d4,  N,  N,  N),
-       PINGROUP(pj2,                    RSVD1,      RSVD2,      GMI,          SOC,         RSVD1,      0x31d8,  N,  N,  N),
-       PINGROUP(pk3,                    SDMMC2,     TRACE,      GMI,          CCLA,        GMI,        0x31dc,  N,  N,  N),
-       PINGROUP(pk4,                    SDMMC2,     RSVD2,      GMI,          GMI_ALT,     GMI,        0x31e0,  N,  N,  N),
-       PINGROUP(pk2,                    RSVD1,      RSVD2,      GMI,          RSVD4,       RSVD4,      0x31e4,  N,  N,  N),
-       PINGROUP(pi3,                    RSVD1,      RSVD2,      GMI,          SPI4,        RSVD1,      0x31e8,  N,  N,  N),
-       PINGROUP(pi6,                    RSVD1,      RSVD2,      GMI,          SDMMC2,      RSVD1,      0x31ec,  N,  N,  N),
-       PINGROUP(pg0,                    RSVD1,      RSVD2,      GMI,          RSVD4,       RSVD4,      0x31f0,  N,  N,  N),
-       PINGROUP(pg1,                    RSVD1,      RSVD2,      GMI,          RSVD4,       RSVD4,      0x31f4,  N,  N,  N),
-       PINGROUP(pg2,                    RSVD1,      TRACE,      GMI,          RSVD4,       RSVD4,      0x31f8,  N,  N,  N),
-       PINGROUP(pg3,                    RSVD1,      TRACE,      GMI,          RSVD4,       RSVD4,      0x31fc,  N,  N,  N),
-       PINGROUP(pg4,                    RSVD1,      TMDS,       GMI,          SPI4,        RSVD1,      0x3200,  N,  N,  N),
-       PINGROUP(pg5,                    RSVD1,      RSVD2,      GMI,          SPI4,        RSVD1,      0x3204,  N,  N,  N),
-       PINGROUP(pg6,                    RSVD1,      RSVD2,      GMI,          SPI4,        RSVD1,      0x3208,  N,  N,  N),
-       PINGROUP(pg7,                    RSVD1,      RSVD2,      GMI,          SPI4,        RSVD1,      0x320c,  N,  N,  N),
-       PINGROUP(ph0,                    PWM0,       TRACE,      GMI,          DTV,         GMI,        0x3210,  N,  N,  N),
-       PINGROUP(ph1,                    PWM1,       TMDS,       GMI,          DISPLAYA,    GMI,        0x3214,  N,  N,  N),
-       PINGROUP(ph2,                    PWM2,       TMDS,       GMI,          CLDVFS,      GMI,        0x3218,  N,  N,  N),
-       PINGROUP(ph3,                    PWM3,       SPI4,       GMI,          CLDVFS,      GMI,        0x321c,  N,  N,  N),
-       PINGROUP(ph4,                    SDMMC2,     RSVD2,      GMI,          RSVD4,       GMI,        0x3220,  N,  N,  N),
-       PINGROUP(ph5,                    SDMMC2,     RSVD2,      GMI,          RSVD4,       GMI,        0x3224,  N,  N,  N),
-       PINGROUP(ph6,                    SDMMC2,     TRACE,      GMI,          DTV,         GMI,        0x3228,  N,  N,  N),
-       PINGROUP(ph7,                    SDMMC2,     TRACE,      GMI,          DTV,         GMI,        0x322c,  N,  N,  N),
-       PINGROUP(pj7,                    UARTD,      RSVD2,      GMI,          GMI_ALT,     RSVD2,      0x3230,  N,  N,  N),
-       PINGROUP(pb0,                    UARTD,      RSVD2,      GMI,          RSVD4,       RSVD2,      0x3234,  N,  N,  N),
-       PINGROUP(pb1,                    UARTD,      RSVD2,      GMI,          RSVD4,       RSVD2,      0x3238,  N,  N,  N),
-       PINGROUP(pk7,                    UARTD,      RSVD2,      GMI,          RSVD4,       RSVD2,      0x323c,  N,  N,  N),
-       PINGROUP(pi0,                    RSVD1,      RSVD2,      GMI,          RSVD4,       RSVD4,      0x3240,  N,  N,  N),
-       PINGROUP(pi1,                    RSVD1,      RSVD2,      GMI,          RSVD4,       RSVD1,      0x3244,  N,  N,  N),
-       PINGROUP(pi2,                    SDMMC2,     TRACE,      GMI,          RSVD4,       GMI,        0x3248,  N,  N,  N),
-       PINGROUP(pi4,                    SPI4,       TRACE,      GMI,          DISPLAYA,    GMI,        0x324c,  N,  N,  N),
-       PINGROUP(gen2_i2c_scl_pt5,       I2C2,       RSVD2,      GMI,          RSVD4,       RSVD2,      0x3250,  Y,  N,  N),
-       PINGROUP(gen2_i2c_sda_pt6,       I2C2,       RSVD2,      GMI,          RSVD4,       RSVD2,      0x3254,  Y,  N,  N),
-       PINGROUP(sdmmc4_clk_pcc4,        SDMMC4,     RSVD2,      GMI,          RSVD4,       RSVD2,      0x3258,  N,  Y,  N),
-       PINGROUP(sdmmc4_cmd_pt7,         SDMMC4,     RSVD2,      GMI,          RSVD4,       RSVD2,      0x325c,  N,  Y,  N),
-       PINGROUP(sdmmc4_dat0_paa0,       SDMMC4,     SPI3,       GMI,          RSVD4,       SDMMC4,     0x3260,  N,  Y,  N),
-       PINGROUP(sdmmc4_dat1_paa1,       SDMMC4,     SPI3,       GMI,          RSVD4,       SDMMC4,     0x3264,  N,  Y,  N),
-       PINGROUP(sdmmc4_dat2_paa2,       SDMMC4,     SPI3,       GMI,          RSVD4,       SDMMC4,     0x3268,  N,  Y,  N),
-       PINGROUP(sdmmc4_dat3_paa3,       SDMMC4,     SPI3,       GMI,          RSVD4,       SDMMC4,     0x326c,  N,  Y,  N),
-       PINGROUP(sdmmc4_dat4_paa4,       SDMMC4,     SPI3,       GMI,          RSVD4,       SDMMC4,     0x3270,  N,  Y,  N),
-       PINGROUP(sdmmc4_dat5_paa5,       SDMMC4,     SPI3,       RSVD3,        RSVD4,       SDMMC4,     0x3274,  N,  Y,  N),
-       PINGROUP(sdmmc4_dat6_paa6,       SDMMC4,     SPI3,       GMI,          RSVD4,       SDMMC4,     0x3278,  N,  Y,  N),
-       PINGROUP(sdmmc4_dat7_paa7,       SDMMC4,     RSVD2,      GMI,          RSVD4,       SDMMC4,     0x327c,  N,  Y,  N),
-       PINGROUP(cam_mclk_pcc0,          VI,         VI_ALT1,    VI_ALT3,      SDMMC2,      VI,         0x3284,  N,  N,  N),
-       PINGROUP(pcc1,                   I2S4,       RSVD2,      RSVD3,        SDMMC2,      I2S4,       0x3288,  N,  N,  N),
-       PINGROUP(pbb0,                   VGP6,       VIMCLK2,    SDMMC2,       VIMCLK2_ALT, VGP6,       0x328c,  N,  N,  N),
-       PINGROUP(cam_i2c_scl_pbb1,       VGP1,       I2C3,       RSVD3,        SDMMC2,      VGP1,       0x3290,  Y,  N,  N),
-       PINGROUP(cam_i2c_sda_pbb2,       VGP2,       I2C3,       RSVD3,        SDMMC2,      VGP2,       0x3294,  Y,  N,  N),
-       PINGROUP(pbb3,                   VGP3,       DISPLAYA,   DISPLAYB,     SDMMC2,      VGP3,       0x3298,  N,  N,  N),
-       PINGROUP(pbb4,                   VGP4,       DISPLAYA,   DISPLAYB,     SDMMC2,      VGP4,       0x329c,  N,  N,  N),
-       PINGROUP(pbb5,                   VGP5,       DISPLAYA,   RSVD3,        SDMMC2,      VGP5,       0x32a0,  N,  N,  N),
-       PINGROUP(pbb6,                   I2S4,       RSVD2,      DISPLAYB,     SDMMC2,      I2S4,       0x32a4,  N,  N,  N),
-       PINGROUP(pbb7,                   I2S4,       RSVD2,      RSVD3,        SDMMC2,      I2S4,       0x32a8,  N,  N,  N),
-       PINGROUP(pcc2,                   I2S4,       RSVD2,      SDMMC3,       SDMMC2,      I2S4,       0x32ac,  N,  N,  N),
-       PINGROUP(jtag_rtck,              RTCK,       RSVD2,      RSVD3,        RSVD4,       RTCK,       0x32b0,  N,  N,  N),
-       PINGROUP(pwr_i2c_scl_pz6,        I2CPWR,     RSVD2,      RSVD3,        RSVD4,       RSVD2,      0x32b4,  Y,  N,  N),
-       PINGROUP(pwr_i2c_sda_pz7,        I2CPWR,     RSVD2,      RSVD3,        RSVD4,       RSVD2,      0x32b8,  Y,  N,  N),
-       PINGROUP(kb_row0_pr0,            KBC,        RSVD2,      RSVD3,        RSVD4,       RSVD4,      0x32bc,  N,  N,  N),
-       PINGROUP(kb_row1_pr1,            KBC,        RSVD2,      RSVD3,        RSVD4,       RSVD4,      0x32c0,  N,  N,  N),
-       PINGROUP(kb_row2_pr2,            KBC,        RSVD2,      RSVD3,        RSVD4,       RSVD4,      0x32c4,  N,  N,  N),
-       PINGROUP(kb_row3_pr3,            KBC,        DISPLAYA,   SYS,          DISPLAYB,    KBC,        0x32c8,  N,  N,  N),
-       PINGROUP(kb_row4_pr4,            KBC,        DISPLAYA,   RSVD3,        DISPLAYB,    RSVD3,      0x32cc,  N,  N,  N),
-       PINGROUP(kb_row5_pr5,            KBC,        DISPLAYA,   RSVD3,        DISPLAYB,    RSVD3,      0x32d0,  N,  N,  N),
-       PINGROUP(kb_row6_pr6,            KBC,        DISPLAYA,   DISPLAYA_ALT, DISPLAYB,    KBC,        0x32d4,  N,  N,  N),
-       PINGROUP(kb_row7_pr7,            KBC,        RSVD2,      CLDVFS,       UARTA,       RSVD2,      0x32d8,  N,  N,  N),
-       PINGROUP(kb_row8_ps0,            KBC,        RSVD2,      CLDVFS,       UARTA,       RSVD2,      0x32dc,  N,  N,  N),
-       PINGROUP(kb_row9_ps1,            KBC,        RSVD2,      RSVD3,        UARTA,       KBC,        0x32e0,  N,  N,  N),
-       PINGROUP(kb_row10_ps2,           KBC,        RSVD2,      RSVD3,        UARTA,       KBC,        0x32e4,  N,  N,  N),
-       PINGROUP(kb_row11_ps3,           KBC,        RSVD2,      RSVD3,        IRDA,        RSVD3,      0x32e8,  N,  N,  N),
-       PINGROUP(kb_row12_ps4,           KBC,        RSVD2,      RSVD3,        IRDA,        RSVD3,      0x32ec,  N,  N,  N),
-       PINGROUP(kb_row13_ps5,           KBC,        RSVD2,      SPI2,         RSVD4,       RSVD4,      0x32f0,  N,  N,  N),
-       PINGROUP(kb_row14_ps6,           KBC,        RSVD2,      SPI2,         RSVD4,       RSVD4,      0x32f4,  N,  N,  N),
-       PINGROUP(kb_row15_ps7,           KBC,        SOC,        RSVD3,        RSVD4,       KBC,        0x32f8,  N,  N,  N),
-       PINGROUP(kb_col0_pq0,            KBC,        RSVD2,      SPI2,         RSVD4,       RSVD4,      0x32fc,  N,  N,  N),
-       PINGROUP(kb_col1_pq1,            KBC,        RSVD2,      SPI2,         RSVD4,       RSVD4,      0x3300,  N,  N,  N),
-       PINGROUP(kb_col2_pq2,            KBC,        RSVD2,      SPI2,         RSVD4,       RSVD4,      0x3304,  N,  N,  N),
-       PINGROUP(kb_col3_pq3,            KBC,        DISPLAYA,   PWM2,         UARTA,       KBC,        0x3308,  N,  N,  N),
-       PINGROUP(kb_col4_pq4,            KBC,        OWR,        SDMMC3,       UARTA,       KBC,        0x330c,  N,  N,  N),
-       PINGROUP(kb_col5_pq5,            KBC,        RSVD2,      SDMMC3,       RSVD4,       RSVD4,      0x3310,  N,  N,  N),
-       PINGROUP(kb_col6_pq6,            KBC,        RSVD2,      SPI2,         UARTD,       RSVD2,      0x3314,  N,  N,  N),
-       PINGROUP(kb_col7_pq7,            KBC,        RSVD2,      SPI2,         UARTD,       RSVD2,      0x3318,  N,  N,  N),
-       PINGROUP(clk_32k_out_pa0,        BLINK,      SOC,        RSVD3,        RSVD4,       RSVD3,      0x331c,  N,  N,  N),
-       PINGROUP(core_pwr_req,           PWRON,      RSVD2,      RSVD3,        RSVD4,       RSVD2,      0x3324,  N,  N,  N),
-       PINGROUP(cpu_pwr_req,            CPU,        RSVD2,      RSVD3,        RSVD4,       RSVD2,      0x3328,  N,  N,  N),
-       PINGROUP(pwr_int_n,              PMI,        RSVD2,      RSVD3,        RSVD4,       RSVD2,      0x332c,  N,  N,  N),
-       PINGROUP(clk_32k_in,             CLK,        RSVD2,      RSVD3,        RSVD4,       RSVD2,      0x3330,  N,  N,  N),
-       PINGROUP(owr,                    OWR,        RSVD2,      RSVD3,        RSVD4,       RSVD2,      0x3334,  N,  N,  Y),
-       PINGROUP(dap1_fs_pn0,            I2S0,       HDA,        GMI,          RSVD4,       RSVD4,      0x3338,  N,  N,  N),
-       PINGROUP(dap1_din_pn1,           I2S0,       HDA,        GMI,          RSVD4,       RSVD4,      0x333c,  N,  N,  N),
-       PINGROUP(dap1_dout_pn2,          I2S0,       HDA,        GMI,          SATA,        I2S0,       0x3340,  N,  N,  N),
-       PINGROUP(dap1_sclk_pn3,          I2S0,       HDA,        GMI,          RSVD4,       I2S0,       0x3344,  N,  N,  N),
-       PINGROUP(dap_mclk1_req_pee2,     DAP,        DAP1,       SATA,         RSVD4,       DAP,        0x3348,  N,  N,  N),
-       PINGROUP(dap_mclk1_pw4,          EXTPERIPH1, DAP2,       RSVD3,        RSVD4,       RSVD3,      0x334c,  N,  N,  N),
-       PINGROUP(spdif_in_pk6,           SPDIF,      RSVD2,      RSVD3,        I2C3,        RSVD3,      0x3350,  N,  N,  N),
-       PINGROUP(spdif_out_pk5,          SPDIF,      RSVD2,      RSVD3,        I2C3,        RSVD3,      0x3354,  N,  N,  N),
-       PINGROUP(dap2_fs_pa2,            I2S1,       HDA,        GMI,          RSVD4,       I2S1,       0x3358,  N,  N,  N),
-       PINGROUP(dap2_din_pa4,           I2S1,       HDA,        GMI,          RSVD4,       I2S1,       0x335c,  N,  N,  N),
-       PINGROUP(dap2_dout_pa5,          I2S1,       HDA,        GMI,          RSVD4,       I2S1,       0x3360,  N,  N,  N),
-       PINGROUP(dap2_sclk_pa3,          I2S1,       HDA,        GMI,          RSVD4,       I2S1,       0x3364,  N,  N,  N),
-       PINGROUP(dvfs_pwm_px0,           SPI6,       CLDVFS,     GMI,          RSVD4,       SPI6,       0x3368,  N,  N,  N),
-       PINGROUP(gpio_x1_aud_px1,        SPI6,       RSVD2,      GMI,          RSVD4,       SPI6,       0x336c,  N,  N,  N),
-       PINGROUP(gpio_x3_aud_px3,        SPI6,       SPI1,       GMI,          RSVD4,       SPI6,       0x3370,  N,  N,  N),
-       PINGROUP(dvfs_clk_px2,           SPI6,       CLDVFS,     GMI,          RSVD4,       SPI6,       0x3374,  N,  N,  N),
-       PINGROUP(gpio_x4_aud_px4,        GMI,        SPI1,       SPI2,         DAP2,        SPI1,       0x3378,  N,  N,  N),
-       PINGROUP(gpio_x5_aud_px5,        GMI,        SPI1,       SPI2,         RSVD4,       SPI1,       0x337c,  N,  N,  N),
-       PINGROUP(gpio_x6_aud_px6,        SPI6,       SPI1,       SPI2,         GMI,         SPI1,       0x3380,  N,  N,  N),
-       PINGROUP(gpio_x7_aud_px7,        RSVD1,      SPI1,       SPI2,         RSVD4,       SPI1,       0x3384,  N,  N,  N),
-       PINGROUP(sdmmc3_clk_pa6,         SDMMC3,     RSVD2,      RSVD3,        SPI3,        SDMMC3,     0x3390,  N,  N,  N),
-       PINGROUP(sdmmc3_cmd_pa7,         SDMMC3,     PWM3,       UARTA,        SPI3,        SDMMC3,     0x3394,  N,  N,  N),
-       PINGROUP(sdmmc3_dat0_pb7,        SDMMC3,     RSVD2,      RSVD3,        SPI3,        SDMMC3,     0x3398,  N,  N,  N),
-       PINGROUP(sdmmc3_dat1_pb6,        SDMMC3,     PWM2,       UARTA,        SPI3,        SDMMC3,     0x339c,  N,  N,  N),
-       PINGROUP(sdmmc3_dat2_pb5,        SDMMC3,     PWM1,       DISPLAYA,     SPI3,        SDMMC3,     0x33a0,  N,  N,  N),
-       PINGROUP(sdmmc3_dat3_pb4,        SDMMC3,     PWM0,       DISPLAYB,     SPI3,        SDMMC3,     0x33a4,  N,  N,  N),
-       PINGROUP(pex_l0_rst_n_pdd1,      PE0,        RSVD2,      RSVD3,        RSVD4,       PE0,        0x33bc,  N,  N,  N),
-       PINGROUP(pex_l0_clkreq_n_pdd2,   PE0,        RSVD2,      RSVD3,        RSVD4,       PE0,        0x33c0,  N,  N,  N),
-       PINGROUP(pex_wake_n_pdd3,        PE,         RSVD2,      RSVD3,        RSVD4,       PE,         0x33c4,  N,  N,  N),
-       PINGROUP(pex_l1_rst_n_pdd5,      PE1,        RSVD2,      RSVD3,        RSVD4,       PE1,        0x33cc,  N,  N,  N),
-       PINGROUP(pex_l1_clkreq_n_pdd6,   PE1,        RSVD2,      RSVD3,        RSVD4,       PE1,        0x33d0,  N,  N,  N),
-       PINGROUP(hdmi_cec_pee3,          CEC,        RSVD2,      RSVD3,        RSVD4,       CEC,        0x33e0,  Y,  N,  N),
-       PINGROUP(sdmmc1_wp_n_pv3,        SDMMC1,     CLK12,      SPI4,         UARTA,       SDMMC1,     0x33e4,  N,  N,  N),
-       PINGROUP(sdmmc3_cd_n_pv2,        SDMMC3,     OWR,        RSVD3,        RSVD4,       SDMMC3,     0x33e8,  N,  N,  N),
-       PINGROUP(gpio_w2_aud_pw2,        SPI6,       RSVD2,      SPI2,         I2C1,        RSVD2,      0x33ec,  N,  N,  N),
-       PINGROUP(gpio_w3_aud_pw3,        SPI6,       SPI1,       SPI2,         I2C1,        SPI1,       0x33f0,  N,  N,  N),
-       PINGROUP(usb_vbus_en0_pn4,       USB,        RSVD2,      RSVD3,        RSVD4,       USB,        0x33f4,  Y,  N,  N),
-       PINGROUP(usb_vbus_en1_pn5,       USB,        RSVD2,      RSVD3,        RSVD4,       USB,        0x33f8,  Y,  N,  N),
-       PINGROUP(sdmmc3_clk_lb_in_pee5,  SDMMC3,     RSVD2,      RSVD3,        RSVD4,       SDMMC3,     0x33fc,  N,  N,  N),
-       PINGROUP(sdmmc3_clk_lb_out_pee4, SDMMC3,     RSVD2,      RSVD3,        RSVD4,       SDMMC3,     0x3400,  N,  N,  N),
-       PINGROUP(gmi_clk_lb,             SDMMC2,     RSVD2,      GMI,          RSVD4,       SDMMC2,     0x3404,  N,  N,  N),
-       PINGROUP(reset_out_n,            RSVD1,      RSVD2,      RSVD3,        RESET_OUT_N, RSVD1,      0x3408,  N,  N,  N),
-       PINGROUP(kb_row16_pt0,           KBC,        RSVD2,      RSVD3,        UARTC,       KBC,        0x340c,  N,  N,  N),
-       PINGROUP(kb_row17_pt1,           KBC,        RSVD2,      RSVD3,        UARTC,       KBC,        0x3410,  N,  N,  N),
-       PINGROUP(usb_vbus_en2_pff1,      USB,        RSVD2,      RSVD3,        RSVD4,       USB,        0x3414,  Y,  N,  N),
-       PINGROUP(pff2,                   SATA,       RSVD2,      RSVD3,        RSVD4,       RSVD2,      0x3418,  Y,  N,  N),
-       PINGROUP(dp_hpd_pff0,            DP,         RSVD2,      RSVD3,        RSVD4,       DP,         0x3430,  N,  N,  N),
+	/*       pg_name,                f0,         f1,         f2,           f3,          r,      od, ior, rcv_sel */
+	PINGROUP(ulpi_data0_po1,         SPI3,       HSI,        UARTA,        ULPI,        0x3000, N,   N,  N),
+	PINGROUP(ulpi_data1_po2,         SPI3,       HSI,        UARTA,        ULPI,        0x3004, N,   N,  N),
+	PINGROUP(ulpi_data2_po3,         SPI3,       HSI,        UARTA,        ULPI,        0x3008, N,   N,  N),
+	PINGROUP(ulpi_data3_po4,         SPI3,       HSI,        UARTA,        ULPI,        0x300c, N,   N,  N),
+	PINGROUP(ulpi_data4_po5,         SPI2,       HSI,        UARTA,        ULPI,        0x3010, N,   N,  N),
+	PINGROUP(ulpi_data5_po6,         SPI2,       HSI,        UARTA,        ULPI,        0x3014, N,   N,  N),
+	PINGROUP(ulpi_data6_po7,         SPI2,       HSI,        UARTA,        ULPI,        0x3018, N,   N,  N),
+	PINGROUP(ulpi_data7_po0,         SPI2,       HSI,        UARTA,        ULPI,        0x301c, N,   N,  N),
+	PINGROUP(ulpi_clk_py0,           SPI1,       SPI5,       UARTD,        ULPI,        0x3020, N,   N,  N),
+	PINGROUP(ulpi_dir_py1,           SPI1,       SPI5,       UARTD,        ULPI,        0x3024, N,   N,  N),
+	PINGROUP(ulpi_nxt_py2,           SPI1,       SPI5,       UARTD,        ULPI,        0x3028, N,   N,  N),
+	PINGROUP(ulpi_stp_py3,           SPI1,       SPI5,       UARTD,        ULPI,        0x302c, N,   N,  N),
+	PINGROUP(dap3_fs_pp0,            I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    0x3030, N,   N,  N),
+	PINGROUP(dap3_din_pp1,           I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    0x3034, N,   N,  N),
+	PINGROUP(dap3_dout_pp2,          I2S2,       SPI5,       DISPLAYA,     RSVD4,       0x3038, N,   N,  N),
+	PINGROUP(dap3_sclk_pp3,          I2S2,       SPI5,       RSVD3,        DISPLAYB,    0x303c, N,   N,  N),
+	PINGROUP(pv0,                    RSVD1,      RSVD2,      RSVD3,        RSVD4,       0x3040, N,   N,  N),
+	PINGROUP(pv1,                    RSVD1,      RSVD2,      RSVD3,        RSVD4,       0x3044, N,   N,  N),
+	PINGROUP(sdmmc1_clk_pz0,         SDMMC1,     CLK12,      RSVD3,        RSVD4,       0x3048, N,   N,  N),
+	PINGROUP(sdmmc1_cmd_pz1,         SDMMC1,     SPDIF,      SPI4,         UARTA,       0x304c, N,   N,  N),
+	PINGROUP(sdmmc1_dat3_py4,        SDMMC1,     SPDIF,      SPI4,         UARTA,       0x3050, N,   N,  N),
+	PINGROUP(sdmmc1_dat2_py5,        SDMMC1,     PWM0,       SPI4,         UARTA,       0x3054, N,   N,  N),
+	PINGROUP(sdmmc1_dat1_py6,        SDMMC1,     PWM1,       SPI4,         UARTA,       0x3058, N,   N,  N),
+	PINGROUP(sdmmc1_dat0_py7,        SDMMC1,     RSVD2,      SPI4,         UARTA,       0x305c, N,   N,  N),
+	PINGROUP(clk2_out_pw5,           EXTPERIPH2, RSVD2,      RSVD3,        RSVD4,       0x3068, N,   N,  N),
+	PINGROUP(clk2_req_pcc5,          DAP,        RSVD2,      RSVD3,        RSVD4,       0x306c, N,   N,  N),
+	PINGROUP(hdmi_int_pn7,           RSVD1,      RSVD2,      RSVD3,        RSVD4,       0x3110, N,   N,  Y),
+	PINGROUP(ddc_scl_pv4,            I2C4,       RSVD2,      RSVD3,        RSVD4,       0x3114, N,   N,  Y),
+	PINGROUP(ddc_sda_pv5,            I2C4,       RSVD2,      RSVD3,        RSVD4,       0x3118, N,   N,  Y),
+	PINGROUP(uart2_rxd_pc3,          IRDA,       SPDIF,      UARTA,        SPI4,        0x3164, N,   N,  N),
+	PINGROUP(uart2_txd_pc2,          IRDA,       SPDIF,      UARTA,        SPI4,        0x3168, N,   N,  N),
+	PINGROUP(uart2_rts_n_pj6,        UARTA,      UARTB,      GMI,          SPI4,        0x316c, N,   N,  N),
+	PINGROUP(uart2_cts_n_pj5,        UARTA,      UARTB,      GMI,          SPI4,        0x3170, N,   N,  N),
+	PINGROUP(uart3_txd_pw6,          UARTC,      RSVD2,      GMI,          SPI4,        0x3174, N,   N,  N),
+	PINGROUP(uart3_rxd_pw7,          UARTC,      RSVD2,      GMI,          SPI4,        0x3178, N,   N,  N),
+	PINGROUP(uart3_cts_n_pa1,        UARTC,      SDMMC1,     DTV,          GMI,         0x317c, N,   N,  N),
+	PINGROUP(uart3_rts_n_pc0,        UARTC,      PWM0,       DTV,          GMI,         0x3180, N,   N,  N),
+	PINGROUP(pu0,                    OWR,        UARTA,      GMI,          RSVD4,       0x3184, N,   N,  N),
+	PINGROUP(pu1,                    RSVD1,      UARTA,      GMI,          RSVD4,       0x3188, N,   N,  N),
+	PINGROUP(pu2,                    RSVD1,      UARTA,      GMI,          RSVD4,       0x318c, N,   N,  N),
+	PINGROUP(pu3,                    PWM0,       UARTA,      GMI,          DISPLAYB,    0x3190, N,   N,  N),
+	PINGROUP(pu4,                    PWM1,       UARTA,      GMI,          DISPLAYB,    0x3194, N,   N,  N),
+	PINGROUP(pu5,                    PWM2,       UARTA,      GMI,          DISPLAYB,    0x3198, N,   N,  N),
+	PINGROUP(pu6,                    PWM3,       UARTA,      RSVD3,        GMI,         0x319c, N,   N,  N),
+	PINGROUP(gen1_i2c_sda_pc5,       I2C1,       RSVD2,      RSVD3,        RSVD4,       0x31a0, Y,   N,  N),
+	PINGROUP(gen1_i2c_scl_pc4,       I2C1,       RSVD2,      RSVD3,        RSVD4,       0x31a4, Y,   N,  N),
+	PINGROUP(dap4_fs_pp4,            I2S3,       GMI,        DTV,          RSVD4,       0x31a8, N,   N,  N),
+	PINGROUP(dap4_din_pp5,           I2S3,       GMI,        RSVD3,        RSVD4,       0x31ac, N,   N,  N),
+	PINGROUP(dap4_dout_pp6,          I2S3,       GMI,        DTV,          RSVD4,       0x31b0, N,   N,  N),
+	PINGROUP(dap4_sclk_pp7,          I2S3,       GMI,        RSVD3,        RSVD4,       0x31b4, N,   N,  N),
+	PINGROUP(clk3_out_pee0,          EXTPERIPH3, RSVD2,      RSVD3,        RSVD4,       0x31b8, N,   N,  N),
+	PINGROUP(clk3_req_pee1,          DEV3,       RSVD2,      RSVD3,        RSVD4,       0x31bc, N,   N,  N),
+	PINGROUP(pc7,                    RSVD1,      RSVD2,      GMI,          GMI_ALT,     0x31c0, N,   N,  N),
+	PINGROUP(pi5,                    SDMMC2,     RSVD2,      GMI,          RSVD4,       0x31c4, N,   N,  N),
+	PINGROUP(pi7,                    RSVD1,      TRACE,      GMI,          DTV,         0x31c8, N,   N,  N),
+	PINGROUP(pk0,                    RSVD1,      SDMMC3,     GMI,          SOC,         0x31cc, N,   N,  N),
+	PINGROUP(pk1,                    SDMMC2,     TRACE,      GMI,          RSVD4,       0x31d0, N,   N,  N),
+	PINGROUP(pj0,                    RSVD1,      RSVD2,      GMI,          USB,         0x31d4, N,   N,  N),
+	PINGROUP(pj2,                    RSVD1,      RSVD2,      GMI,          SOC,         0x31d8, N,   N,  N),
+	PINGROUP(pk3,                    SDMMC2,     TRACE,      GMI,          CCLA,        0x31dc, N,   N,  N),
+	PINGROUP(pk4,                    SDMMC2,     RSVD2,      GMI,          GMI_ALT,     0x31e0, N,   N,  N),
+	PINGROUP(pk2,                    RSVD1,      RSVD2,      GMI,          RSVD4,       0x31e4, N,   N,  N),
+	PINGROUP(pi3,                    RSVD1,      RSVD2,      GMI,          SPI4,        0x31e8, N,   N,  N),
+	PINGROUP(pi6,                    RSVD1,      RSVD2,      GMI,          SDMMC2,      0x31ec, N,   N,  N),
+	PINGROUP(pg0,                    RSVD1,      RSVD2,      GMI,          RSVD4,       0x31f0, N,   N,  N),
+	PINGROUP(pg1,                    RSVD1,      RSVD2,      GMI,          RSVD4,       0x31f4, N,   N,  N),
+	PINGROUP(pg2,                    RSVD1,      TRACE,      GMI,          RSVD4,       0x31f8, N,   N,  N),
+	PINGROUP(pg3,                    RSVD1,      TRACE,      GMI,          RSVD4,       0x31fc, N,   N,  N),
+	PINGROUP(pg4,                    RSVD1,      TMDS,       GMI,          SPI4,        0x3200, N,   N,  N),
+	PINGROUP(pg5,                    RSVD1,      RSVD2,      GMI,          SPI4,        0x3204, N,   N,  N),
+	PINGROUP(pg6,                    RSVD1,      RSVD2,      GMI,          SPI4,        0x3208, N,   N,  N),
+	PINGROUP(pg7,                    RSVD1,      RSVD2,      GMI,          SPI4,        0x320c, N,   N,  N),
+	PINGROUP(ph0,                    PWM0,       TRACE,      GMI,          DTV,         0x3210, N,   N,  N),
+	PINGROUP(ph1,                    PWM1,       TMDS,       GMI,          DISPLAYA,    0x3214, N,   N,  N),
+	PINGROUP(ph2,                    PWM2,       TMDS,       GMI,          CLDVFS,      0x3218, N,   N,  N),
+	PINGROUP(ph3,                    PWM3,       SPI4,       GMI,          CLDVFS,      0x321c, N,   N,  N),
+	PINGROUP(ph4,                    SDMMC2,     RSVD2,      GMI,          RSVD4,       0x3220, N,   N,  N),
+	PINGROUP(ph5,                    SDMMC2,     RSVD2,      GMI,          RSVD4,       0x3224, N,   N,  N),
+	PINGROUP(ph6,                    SDMMC2,     TRACE,      GMI,          DTV,         0x3228, N,   N,  N),
+	PINGROUP(ph7,                    SDMMC2,     TRACE,      GMI,          DTV,         0x322c, N,   N,  N),
+	PINGROUP(pj7,                    UARTD,      RSVD2,      GMI,          GMI_ALT,     0x3230, N,   N,  N),
+	PINGROUP(pb0,                    UARTD,      RSVD2,      GMI,          RSVD4,       0x3234, N,   N,  N),
+	PINGROUP(pb1,                    UARTD,      RSVD2,      GMI,          RSVD4,       0x3238, N,   N,  N),
+	PINGROUP(pk7,                    UARTD,      RSVD2,      GMI,          RSVD4,       0x323c, N,   N,  N),
+	PINGROUP(pi0,                    RSVD1,      RSVD2,      GMI,          RSVD4,       0x3240, N,   N,  N),
+	PINGROUP(pi1,                    RSVD1,      RSVD2,      GMI,          RSVD4,       0x3244, N,   N,  N),
+	PINGROUP(pi2,                    SDMMC2,     TRACE,      GMI,          RSVD4,       0x3248, N,   N,  N),
+	PINGROUP(pi4,                    SPI4,       TRACE,      GMI,          DISPLAYA,    0x324c, N,   N,  N),
+	PINGROUP(gen2_i2c_scl_pt5,       I2C2,       RSVD2,      GMI,          RSVD4,       0x3250, Y,   N,  N),
+	PINGROUP(gen2_i2c_sda_pt6,       I2C2,       RSVD2,      GMI,          RSVD4,       0x3254, Y,   N,  N),
+	PINGROUP(sdmmc4_clk_pcc4,        SDMMC4,     RSVD2,      GMI,          RSVD4,       0x3258, N,   Y,  N),
+	PINGROUP(sdmmc4_cmd_pt7,         SDMMC4,     RSVD2,      GMI,          RSVD4,       0x325c, N,   Y,  N),
+	PINGROUP(sdmmc4_dat0_paa0,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x3260, N,   Y,  N),
+	PINGROUP(sdmmc4_dat1_paa1,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x3264, N,   Y,  N),
+	PINGROUP(sdmmc4_dat2_paa2,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x3268, N,   Y,  N),
+	PINGROUP(sdmmc4_dat3_paa3,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x326c, N,   Y,  N),
+	PINGROUP(sdmmc4_dat4_paa4,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x3270, N,   Y,  N),
+	PINGROUP(sdmmc4_dat5_paa5,       SDMMC4,     SPI3,       RSVD3,        RSVD4,       0x3274, N,   Y,  N),
+	PINGROUP(sdmmc4_dat6_paa6,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x3278, N,   Y,  N),
+	PINGROUP(sdmmc4_dat7_paa7,       SDMMC4,     RSVD2,      GMI,          RSVD4,       0x327c, N,   Y,  N),
+	PINGROUP(cam_mclk_pcc0,          VI,         VI_ALT1,    VI_ALT3,      SDMMC2,      0x3284, N,   N,  N),
+	PINGROUP(pcc1,                   I2S4,       RSVD2,      RSVD3,        SDMMC2,      0x3288, N,   N,  N),
+	PINGROUP(pbb0,                   VGP6,       VIMCLK2,    SDMMC2,       VIMCLK2_ALT, 0x328c, N,   N,  N),
+	PINGROUP(cam_i2c_scl_pbb1,       VGP1,       I2C3,       RSVD3,        SDMMC2,      0x3290, Y,   N,  N),
+	PINGROUP(cam_i2c_sda_pbb2,       VGP2,       I2C3,       RSVD3,        SDMMC2,      0x3294, Y,   N,  N),
+	PINGROUP(pbb3,                   VGP3,       DISPLAYA,   DISPLAYB,     SDMMC2,      0x3298, N,   N,  N),
+	PINGROUP(pbb4,                   VGP4,       DISPLAYA,   DISPLAYB,     SDMMC2,      0x329c, N,   N,  N),
+	PINGROUP(pbb5,                   VGP5,       DISPLAYA,   RSVD3,        SDMMC2,      0x32a0, N,   N,  N),
+	PINGROUP(pbb6,                   I2S4,       RSVD2,      DISPLAYB,     SDMMC2,      0x32a4, N,   N,  N),
+	PINGROUP(pbb7,                   I2S4,       RSVD2,      RSVD3,        SDMMC2,      0x32a8, N,   N,  N),
+	PINGROUP(pcc2,                   I2S4,       RSVD2,      SDMMC3,       SDMMC2,      0x32ac, N,   N,  N),
+	PINGROUP(jtag_rtck,              RTCK,       RSVD2,      RSVD3,        RSVD4,       0x32b0, N,   N,  N),
+	PINGROUP(pwr_i2c_scl_pz6,        I2CPWR,     RSVD2,      RSVD3,        RSVD4,       0x32b4, Y,   N,  N),
+	PINGROUP(pwr_i2c_sda_pz7,        I2CPWR,     RSVD2,      RSVD3,        RSVD4,       0x32b8, Y,   N,  N),
+	PINGROUP(kb_row0_pr0,            KBC,        RSVD2,      RSVD3,        RSVD4,       0x32bc, N,   N,  N),
+	PINGROUP(kb_row1_pr1,            KBC,        RSVD2,      RSVD3,        RSVD4,       0x32c0, N,   N,  N),
+	PINGROUP(kb_row2_pr2,            KBC,        RSVD2,      RSVD3,        RSVD4,       0x32c4, N,   N,  N),
+	PINGROUP(kb_row3_pr3,            KBC,        DISPLAYA,   SYS,          DISPLAYB,    0x32c8, N,   N,  N),
+	PINGROUP(kb_row4_pr4,            KBC,        DISPLAYA,   RSVD3,        DISPLAYB,    0x32cc, N,   N,  N),
+	PINGROUP(kb_row5_pr5,            KBC,        DISPLAYA,   RSVD3,        DISPLAYB,    0x32d0, N,   N,  N),
+	PINGROUP(kb_row6_pr6,            KBC,        DISPLAYA,   DISPLAYA_ALT, DISPLAYB,    0x32d4, N,   N,  N),
+	PINGROUP(kb_row7_pr7,            KBC,        RSVD2,      CLDVFS,       UARTA,       0x32d8, N,   N,  N),
+	PINGROUP(kb_row8_ps0,            KBC,        RSVD2,      CLDVFS,       UARTA,       0x32dc, N,   N,  N),
+	PINGROUP(kb_row9_ps1,            KBC,        RSVD2,      RSVD3,        UARTA,       0x32e0, N,   N,  N),
+	PINGROUP(kb_row10_ps2,           KBC,        RSVD2,      RSVD3,        UARTA,       0x32e4, N,   N,  N),
+	PINGROUP(kb_row11_ps3,           KBC,        RSVD2,      RSVD3,        IRDA,        0x32e8, N,   N,  N),
+	PINGROUP(kb_row12_ps4,           KBC,        RSVD2,      RSVD3,        IRDA,        0x32ec, N,   N,  N),
+	PINGROUP(kb_row13_ps5,           KBC,        RSVD2,      SPI2,         RSVD4,       0x32f0, N,   N,  N),
+	PINGROUP(kb_row14_ps6,           KBC,        RSVD2,      SPI2,         RSVD4,       0x32f4, N,   N,  N),
+	PINGROUP(kb_row15_ps7,           KBC,        SOC,        RSVD3,        RSVD4,       0x32f8, N,   N,  N),
+	PINGROUP(kb_col0_pq0,            KBC,        RSVD2,      SPI2,         RSVD4,       0x32fc, N,   N,  N),
+	PINGROUP(kb_col1_pq1,            KBC,        RSVD2,      SPI2,         RSVD4,       0x3300, N,   N,  N),
+	PINGROUP(kb_col2_pq2,            KBC,        RSVD2,      SPI2,         RSVD4,       0x3304, N,   N,  N),
+	PINGROUP(kb_col3_pq3,            KBC,        DISPLAYA,   PWM2,         UARTA,       0x3308, N,   N,  N),
+	PINGROUP(kb_col4_pq4,            KBC,        OWR,        SDMMC3,       UARTA,       0x330c, N,   N,  N),
+	PINGROUP(kb_col5_pq5,            KBC,        RSVD2,      SDMMC3,       RSVD4,       0x3310, N,   N,  N),
+	PINGROUP(kb_col6_pq6,            KBC,        RSVD2,      SPI2,         UARTD,       0x3314, N,   N,  N),
+	PINGROUP(kb_col7_pq7,            KBC,        RSVD2,      SPI2,         UARTD,       0x3318, N,   N,  N),
+	PINGROUP(clk_32k_out_pa0,        BLINK,      SOC,        RSVD3,        RSVD4,       0x331c, N,   N,  N),
+	PINGROUP(core_pwr_req,           PWRON,      RSVD2,      RSVD3,        RSVD4,       0x3324, N,   N,  N),
+	PINGROUP(cpu_pwr_req,            CPU,        RSVD2,      RSVD3,        RSVD4,       0x3328, N,   N,  N),
+	PINGROUP(pwr_int_n,              PMI,        RSVD2,      RSVD3,        RSVD4,       0x332c, N,   N,  N),
+	PINGROUP(clk_32k_in,             CLK,        RSVD2,      RSVD3,        RSVD4,       0x3330, N,   N,  N),
+	PINGROUP(owr,                    OWR,        RSVD2,      RSVD3,        RSVD4,       0x3334, N,   N,  Y),
+	PINGROUP(dap1_fs_pn0,            I2S0,       HDA,        GMI,          RSVD4,       0x3338, N,   N,  N),
+	PINGROUP(dap1_din_pn1,           I2S0,       HDA,        GMI,          RSVD4,       0x333c, N,   N,  N),
+	PINGROUP(dap1_dout_pn2,          I2S0,       HDA,        GMI,          SATA,        0x3340, N,   N,  N),
+	PINGROUP(dap1_sclk_pn3,          I2S0,       HDA,        GMI,          RSVD4,       0x3344, N,   N,  N),
+	PINGROUP(dap_mclk1_req_pee2,     DAP,        DAP1,       SATA,         RSVD4,       0x3348, N,   N,  N),
+	PINGROUP(dap_mclk1_pw4,          EXTPERIPH1, DAP2,       RSVD3,        RSVD4,       0x334c, N,   N,  N),
+	PINGROUP(spdif_in_pk6,           SPDIF,      RSVD2,      RSVD3,        I2C3,        0x3350, N,   N,  N),
+	PINGROUP(spdif_out_pk5,          SPDIF,      RSVD2,      RSVD3,        I2C3,        0x3354, N,   N,  N),
+	PINGROUP(dap2_fs_pa2,            I2S1,       HDA,        GMI,          RSVD4,       0x3358, N,   N,  N),
+	PINGROUP(dap2_din_pa4,           I2S1,       HDA,        GMI,          RSVD4,       0x335c, N,   N,  N),
+	PINGROUP(dap2_dout_pa5,          I2S1,       HDA,        GMI,          RSVD4,       0x3360, N,   N,  N),
+	PINGROUP(dap2_sclk_pa3,          I2S1,       HDA,        GMI,          RSVD4,       0x3364, N,   N,  N),
+	PINGROUP(dvfs_pwm_px0,           SPI6,       CLDVFS,     GMI,          RSVD4,       0x3368, N,   N,  N),
+	PINGROUP(gpio_x1_aud_px1,        SPI6,       RSVD2,      GMI,          RSVD4,       0x336c, N,   N,  N),
+	PINGROUP(gpio_x3_aud_px3,        SPI6,       SPI1,       GMI,          RSVD4,       0x3370, N,   N,  N),
+	PINGROUP(dvfs_clk_px2,           SPI6,       CLDVFS,     GMI,          RSVD4,       0x3374, N,   N,  N),
+	PINGROUP(gpio_x4_aud_px4,        GMI,        SPI1,       SPI2,         DAP2,        0x3378, N,   N,  N),
+	PINGROUP(gpio_x5_aud_px5,        GMI,        SPI1,       SPI2,         RSVD4,       0x337c, N,   N,  N),
+	PINGROUP(gpio_x6_aud_px6,        SPI6,       SPI1,       SPI2,         GMI,         0x3380, N,   N,  N),
+	PINGROUP(gpio_x7_aud_px7,        RSVD1,      SPI1,       SPI2,         RSVD4,       0x3384, N,   N,  N),
+	PINGROUP(sdmmc3_clk_pa6,         SDMMC3,     RSVD2,      RSVD3,        SPI3,        0x3390, N,   N,  N),
+	PINGROUP(sdmmc3_cmd_pa7,         SDMMC3,     PWM3,       UARTA,        SPI3,        0x3394, N,   N,  N),
+	PINGROUP(sdmmc3_dat0_pb7,        SDMMC3,     RSVD2,      RSVD3,        SPI3,        0x3398, N,   N,  N),
+	PINGROUP(sdmmc3_dat1_pb6,        SDMMC3,     PWM2,       UARTA,        SPI3,        0x339c, N,   N,  N),
+	PINGROUP(sdmmc3_dat2_pb5,        SDMMC3,     PWM1,       DISPLAYA,     SPI3,        0x33a0, N,   N,  N),
+	PINGROUP(sdmmc3_dat3_pb4,        SDMMC3,     PWM0,       DISPLAYB,     SPI3,        0x33a4, N,   N,  N),
+	PINGROUP(pex_l0_rst_n_pdd1,      PE0,        RSVD2,      RSVD3,        RSVD4,       0x33bc, N,   N,  N),
+	PINGROUP(pex_l0_clkreq_n_pdd2,   PE0,        RSVD2,      RSVD3,        RSVD4,       0x33c0, N,   N,  N),
+	PINGROUP(pex_wake_n_pdd3,        PE,         RSVD2,      RSVD3,        RSVD4,       0x33c4, N,   N,  N),
+	PINGROUP(pex_l1_rst_n_pdd5,      PE1,        RSVD2,      RSVD3,        RSVD4,       0x33cc, N,   N,  N),
+	PINGROUP(pex_l1_clkreq_n_pdd6,   PE1,        RSVD2,      RSVD3,        RSVD4,       0x33d0, N,   N,  N),
+	PINGROUP(hdmi_cec_pee3,          CEC,        RSVD2,      RSVD3,        RSVD4,       0x33e0, Y,   N,  N),
+	PINGROUP(sdmmc1_wp_n_pv3,        SDMMC1,     CLK12,      SPI4,         UARTA,       0x33e4, N,   N,  N),
+	PINGROUP(sdmmc3_cd_n_pv2,        SDMMC3,     OWR,        RSVD3,        RSVD4,       0x33e8, N,   N,  N),
+	PINGROUP(gpio_w2_aud_pw2,        SPI6,       RSVD2,      SPI2,         I2C1,        0x33ec, N,   N,  N),
+	PINGROUP(gpio_w3_aud_pw3,        SPI6,       SPI1,       SPI2,         I2C1,        0x33f0, N,   N,  N),
+	PINGROUP(usb_vbus_en0_pn4,       USB,        RSVD2,      RSVD3,        RSVD4,       0x33f4, Y,   N,  N),
+	PINGROUP(usb_vbus_en1_pn5,       USB,        RSVD2,      RSVD3,        RSVD4,       0x33f8, Y,   N,  N),
+	PINGROUP(sdmmc3_clk_lb_in_pee5,  SDMMC3,     RSVD2,      RSVD3,        RSVD4,       0x33fc, N,   N,  N),
+	PINGROUP(sdmmc3_clk_lb_out_pee4, SDMMC3,     RSVD2,      RSVD3,        RSVD4,       0x3400, N,   N,  N),
+	PINGROUP(gmi_clk_lb,             SDMMC2,     RSVD2,      GMI,          RSVD4,       0x3404, N,   N,  N),
+	PINGROUP(reset_out_n,            RSVD1,      RSVD2,      RSVD3,        RESET_OUT_N, 0x3408, N,   N,  N),
+	PINGROUP(kb_row16_pt0,           KBC,        RSVD2,      RSVD3,        UARTC,       0x340c, N,   N,  N),
+	PINGROUP(kb_row17_pt1,           KBC,        RSVD2,      RSVD3,        UARTC,       0x3410, N,   N,  N),
+	PINGROUP(usb_vbus_en2_pff1,      USB,        RSVD2,      RSVD3,        RSVD4,       0x3414, Y,   N,  N),
+	PINGROUP(pff2,                   SATA,       RSVD2,      RSVD3,        RSVD4,       0x3418, Y,   N,  N),
+	PINGROUP(dp_hpd_pff0,            DP,         RSVD2,      RSVD3,        RSVD4,       0x3430, N,   N,  N),
 
 	/* pg_name, r, hsm_b, schmitt_b, lpmd_b, drvdn_b, drvdn_w, drvup_b, drvup_w, slwr_b, slwr_w, slwf_b, slwf_w, drvtype */
-       DRV_PINGROUP(ao1,         0x868,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(ao2,         0x86c,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(at1,         0x870,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
-       DRV_PINGROUP(at2,         0x874,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
-       DRV_PINGROUP(at3,         0x878,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
-       DRV_PINGROUP(at4,         0x87c,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
-       DRV_PINGROUP(at5,         0x880,  2,  3,  4,  14,  5,  19,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(cdev1,       0x884,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(cdev2,       0x888,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(dap1,        0x890,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(dap2,        0x894,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(dap3,        0x898,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(dap4,        0x89c,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(dbg,         0x8a0,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(sdio3,       0x8b0,  2,  3, -1,  12,  7,  20,  7,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(spi,         0x8b4,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(uaa,         0x8b8,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(uab,         0x8bc,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(uart2,       0x8c0,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(uart3,       0x8c4,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(sdio1,       0x8ec,  2,  3, -1,  12,  7,  20,  7,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(ddc,         0x8fc,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(gma,         0x900,  2,  3,  4,  14,  5,  20,  5,  28,  2,  30,  2,  Y),
-       DRV_PINGROUP(gme,         0x910,  2,  3,  4,  14,  5,  19,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(gmf,         0x914,  2,  3,  4,  14,  5,  19,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(gmg,         0x918,  2,  3,  4,  14,  5,  19,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(gmh,         0x91c,  2,  3,  4,  14,  5,  19,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(owr,         0x920,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(uda,         0x924,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(gpv,         0x928,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(dev3,        0x92c,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(cec,         0x938,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(at6,         0x994,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
-       DRV_PINGROUP(dap5,        0x998,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(usb_vbus_en, 0x99c,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(ao3,         0x9a8,  2,  3,  4,  12,  5,  -1, -1,  28,  2,  -1, -1,  N),
-       DRV_PINGROUP(ao0,         0x9b0,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       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),
+	DRV_PINGROUP(ao1,         0x868,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(ao2,         0x86c,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(at1,         0x870,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
+	DRV_PINGROUP(at2,         0x874,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
+	DRV_PINGROUP(at3,         0x878,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
+	DRV_PINGROUP(at4,         0x87c,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
+	DRV_PINGROUP(at5,         0x880,  2,  3,  4,  14,  5,  19,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(cdev1,       0x884,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(cdev2,       0x888,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(dap1,        0x890,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(dap2,        0x894,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(dap3,        0x898,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(dap4,        0x89c,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(dbg,         0x8a0,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(sdio3,       0x8b0,  2,  3, -1,  12,  7,  20,  7,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(spi,         0x8b4,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(uaa,         0x8b8,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(uab,         0x8bc,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(uart2,       0x8c0,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(uart3,       0x8c4,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(sdio1,       0x8ec,  2,  3, -1,  12,  7,  20,  7,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(ddc,         0x8fc,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(gma,         0x900,  2,  3,  4,  14,  5,  20,  5,  28,  2,  30,  2,  Y),
+	DRV_PINGROUP(gme,         0x910,  2,  3,  4,  14,  5,  19,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(gmf,         0x914,  2,  3,  4,  14,  5,  19,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(gmg,         0x918,  2,  3,  4,  14,  5,  19,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(gmh,         0x91c,  2,  3,  4,  14,  5,  19,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(owr,         0x920,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(uda,         0x924,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(gpv,         0x928,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(dev3,        0x92c,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(cec,         0x938,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(at6,         0x994,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
+	DRV_PINGROUP(dap5,        0x998,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(usb_vbus_en, 0x99c,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(ao3,         0x9a8,  2,  3,  4,  12,  5,  -1, -1,  28,  2,  -1, -1,  N),
+	DRV_PINGROUP(ao0,         0x9b0,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	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),
 };
 
 static const struct tegra_pinctrl_soc_data tegra124_pinctrl = {
diff --git a/drivers/pinctrl/pinctrl-tegra20.c b/drivers/pinctrl/pinctrl-tegra20.c
index e0b50408838..7563ebc9c79 100644
--- a/drivers/pinctrl/pinctrl-tegra20.c
+++ b/drivers/pinctrl/pinctrl-tegra20.c
@@ -1973,7 +1973,7 @@ static struct tegra_function tegra20_functions[] = {
 #define PINGROUP_REG_A		0x868
 
 /* Pin group with mux control, and typically tri-state and pull-up/down too */
-#define MUX_PG(pg_name, f0, f1, f2, f3, f_safe,			\
+#define MUX_PG(pg_name, f0, f1, f2, f3,				\
 	       tri_r, tri_b, mux_r, mux_b, pupd_r, pupd_b)	\
 	{							\
 		.name = #pg_name,				\
@@ -1985,7 +1985,6 @@ static struct tegra_function tegra20_functions[] = {
 			TEGRA_MUX_ ## f2,			\
 			TEGRA_MUX_ ## f3,			\
 		},						\
-		.func_safe = TEGRA_MUX_ ## f_safe,		\
 		.mux_reg = ((mux_r) - PIN_MUX_CTL_REG_A),	\
 		.mux_bank = 1,					\
 		.mux_bit = mux_b,				\
@@ -1995,13 +1994,12 @@ static struct tegra_function tegra20_functions[] = {
 		.tri_reg = ((tri_r) - TRISTATE_REG_A),		\
 		.tri_bank = 0,					\
 		.tri_bit = tri_b,				\
-		.einput_reg = -1,				\
-		.odrain_reg = -1,				\
-		.lock_reg = -1,					\
-		.ioreset_reg = -1,				\
-		.rcv_sel_reg = -1,				\
+		.einput_bit = -1,				\
+		.odrain_bit = -1,				\
+		.lock_bit = -1,					\
+		.ioreset_bit = -1,				\
+		.rcv_sel_bit = -1,				\
 		.drv_reg = -1,					\
-		.drvtype_reg = -1,				\
 	}
 
 /* Pin groups with only pull up and pull down control */
@@ -2014,14 +2012,7 @@ static struct tegra_function tegra20_functions[] = {
 		.pupd_reg = ((pupd_r) - PULLUPDOWN_REG_A),	\
 		.pupd_bank = 2,					\
 		.pupd_bit = pupd_b,				\
-		.tri_reg = -1,					\
-		.einput_reg = -1,				\
-		.odrain_reg = -1,				\
-		.lock_reg = -1,					\
-		.ioreset_reg = -1,				\
-		.rcv_sel_reg = -1,				\
 		.drv_reg = -1,					\
-		.drvtype_reg = -1,				\
 	}
 
 /* Pin groups for drive strength registers (configurable version) */
@@ -2035,11 +2026,6 @@ static struct tegra_function tegra20_functions[] = {
 		.mux_reg = -1,					\
 		.pupd_reg = -1,					\
 		.tri_reg = -1,					\
-		.einput_reg = -1,				\
-		.odrain_reg = -1,				\
-		.lock_reg = -1,					\
-		.ioreset_reg = -1,				\
-		.rcv_sel_reg = -1,				\
 		.drv_reg = ((r) - PINGROUP_REG_A),		\
 		.drv_bank = 3,					\
 		.hsm_bit = hsm_b,				\
@@ -2053,7 +2039,7 @@ static struct tegra_function tegra20_functions[] = {
 		.slwr_width = slwr_w,				\
 		.slwf_bit = slwf_b,				\
 		.slwf_width = slwf_w,				\
-		.drvtype_reg = -1,				\
+		.drvtype_bit = -1,				\
 	}
 
 /* Pin groups for drive strength registers (simple version) */
@@ -2061,114 +2047,114 @@ static struct tegra_function tegra20_functions[] = {
 	DRV_PG_EXT(pg_name, r, 2,  3,  4, 12, 20, 28, 2, 30, 2)
 
 static const struct tegra_pingroup tegra20_groups[] = {
-	/*     name,   f0,        f1,        f2,        f3,            f_safe,    tri r/b,  mux r/b,  pupd r/b */
-	MUX_PG(ata,    IDE,       NAND,      GMI,       RSVD4,         IDE,       0x14, 0,  0x80, 24, 0xa0, 0),
-	MUX_PG(atb,    IDE,       NAND,      GMI,       SDIO4,         IDE,       0x14, 1,  0x80, 16, 0xa0, 2),
-	MUX_PG(atc,    IDE,       NAND,      GMI,       SDIO4,         IDE,       0x14, 2,  0x80, 22, 0xa0, 4),
-	MUX_PG(atd,    IDE,       NAND,      GMI,       SDIO4,         IDE,       0x14, 3,  0x80, 20, 0xa0, 6),
-	MUX_PG(ate,    IDE,       NAND,      GMI,       RSVD4,         IDE,       0x18, 25, 0x80, 12, 0xa0, 8),
-	MUX_PG(cdev1,  OSC,       PLLA_OUT,  PLLM_OUT1, AUDIO_SYNC,    OSC,       0x14, 4,  0x88, 2,  0xa8, 0),
-	MUX_PG(cdev2,  OSC,       AHB_CLK,   APB_CLK,   PLLP_OUT4,     OSC,       0x14, 5,  0x88, 4,  0xa8, 2),
-	MUX_PG(crtp,   CRT,       RSVD2,     RSVD3,     RSVD4,         RSVD2,     0x20, 14, 0x98, 20, 0xa4, 24),
-	MUX_PG(csus,   PLLC_OUT1, PLLP_OUT2, PLLP_OUT3, VI_SENSOR_CLK, PLLC_OUT1, 0x14, 6,  0x88, 6,  0xac, 24),
-	MUX_PG(dap1,   DAP1,      RSVD2,     GMI,       SDIO2,         DAP1,      0x14, 7,  0x88, 20, 0xa0, 10),
-	MUX_PG(dap2,   DAP2,      TWC,       RSVD3,     GMI,           DAP2,      0x14, 8,  0x88, 22, 0xa0, 12),
-	MUX_PG(dap3,   DAP3,      RSVD2,     RSVD3,     RSVD4,         DAP3,      0x14, 9,  0x88, 24, 0xa0, 14),
-	MUX_PG(dap4,   DAP4,      RSVD2,     GMI,       RSVD4,         DAP4,      0x14, 10, 0x88, 26, 0xa0, 16),
-	MUX_PG(ddc,    I2C2,      RSVD2,     RSVD3,     RSVD4,         RSVD4,     0x18, 31, 0x88, 0,  0xb0, 28),
-	MUX_PG(dta,    RSVD1,     SDIO2,     VI,        RSVD4,         RSVD4,     0x14, 11, 0x84, 20, 0xa0, 18),
-	MUX_PG(dtb,    RSVD1,     RSVD2,     VI,        SPI1,          RSVD1,     0x14, 12, 0x84, 22, 0xa0, 20),
-	MUX_PG(dtc,    RSVD1,     RSVD2,     VI,        RSVD4,         RSVD1,     0x14, 13, 0x84, 26, 0xa0, 22),
-	MUX_PG(dtd,    RSVD1,     SDIO2,     VI,        RSVD4,         RSVD1,     0x14, 14, 0x84, 28, 0xa0, 24),
-	MUX_PG(dte,    RSVD1,     RSVD2,     VI,        SPI1,          RSVD1,     0x14, 15, 0x84, 30, 0xa0, 26),
-	MUX_PG(dtf,    I2C3,      RSVD2,     VI,        RSVD4,         RSVD4,     0x20, 12, 0x98, 30, 0xa0, 28),
-	MUX_PG(gma,    UARTE,     SPI3,      GMI,       SDIO4,         SPI3,      0x14, 28, 0x84, 0,  0xb0, 20),
-	MUX_PG(gmb,    IDE,       NAND,      GMI,       GMI_INT,       GMI,       0x18, 29, 0x88, 28, 0xb0, 22),
-	MUX_PG(gmc,    UARTD,     SPI4,      GMI,       SFLASH,        SPI4,      0x14, 29, 0x84, 2,  0xb0, 24),
-	MUX_PG(gmd,    RSVD1,     NAND,      GMI,       SFLASH,        GMI,       0x18, 30, 0x88, 30, 0xb0, 26),
-	MUX_PG(gme,    RSVD1,     DAP5,      GMI,       SDIO4,         GMI,       0x18, 0,  0x8c, 0,  0xa8, 24),
-	MUX_PG(gpu,    PWM,       UARTA,     GMI,       RSVD4,         RSVD4,     0x14, 16, 0x8c, 4,  0xa4, 20),
-	MUX_PG(gpu7,   RTCK,      RSVD2,     RSVD3,     RSVD4,         RTCK,      0x20, 11, 0x98, 28, 0xa4, 6),
-	MUX_PG(gpv,    PCIE,      RSVD2,     RSVD3,     RSVD4,         PCIE,      0x14, 17, 0x8c, 2,  0xa0, 30),
-	MUX_PG(hdint,  HDMI,      RSVD2,     RSVD3,     RSVD4,         HDMI,      0x1c, 23, 0x84, 4,  -1,   -1),
-	MUX_PG(i2cp,   I2CP,      RSVD2,     RSVD3,     RSVD4,         RSVD4,     0x14, 18, 0x88, 8,  0xa4, 2),
-	MUX_PG(irrx,   UARTA,     UARTB,     GMI,       SPI4,          UARTB,     0x14, 20, 0x88, 18, 0xa8, 22),
-	MUX_PG(irtx,   UARTA,     UARTB,     GMI,       SPI4,          UARTB,     0x14, 19, 0x88, 16, 0xa8, 20),
-	MUX_PG(kbca,   KBC,       NAND,      SDIO2,     EMC_TEST0_DLL, KBC,       0x14, 22, 0x88, 10, 0xa4, 8),
-	MUX_PG(kbcb,   KBC,       NAND,      SDIO2,     MIO,           KBC,       0x14, 21, 0x88, 12, 0xa4, 10),
-	MUX_PG(kbcc,   KBC,       NAND,      TRACE,     EMC_TEST1_DLL, KBC,       0x18, 26, 0x88, 14, 0xa4, 12),
-	MUX_PG(kbcd,   KBC,       NAND,      SDIO2,     MIO,           KBC,       0x20, 10, 0x98, 26, 0xa4, 14),
-	MUX_PG(kbce,   KBC,       NAND,      OWR,       RSVD4,         KBC,       0x14, 26, 0x80, 28, 0xb0, 2),
-	MUX_PG(kbcf,   KBC,       NAND,      TRACE,     MIO,           KBC,       0x14, 27, 0x80, 26, 0xb0, 0),
-	MUX_PG(lcsn,   DISPLAYA,  DISPLAYB,  SPI3,      RSVD4,         RSVD4,     0x1c, 31, 0x90, 12, -1,   -1),
-	MUX_PG(ld0,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 0,  0x94, 0,  -1,   -1),
-	MUX_PG(ld1,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 1,  0x94, 2,  -1,   -1),
-	MUX_PG(ld2,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 2,  0x94, 4,  -1,   -1),
-	MUX_PG(ld3,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 3,  0x94, 6,  -1,   -1),
-	MUX_PG(ld4,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 4,  0x94, 8,  -1,   -1),
-	MUX_PG(ld5,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 5,  0x94, 10, -1,   -1),
-	MUX_PG(ld6,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 6,  0x94, 12, -1,   -1),
-	MUX_PG(ld7,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 7,  0x94, 14, -1,   -1),
-	MUX_PG(ld8,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 8,  0x94, 16, -1,   -1),
-	MUX_PG(ld9,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 9,  0x94, 18, -1,   -1),
-	MUX_PG(ld10,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 10, 0x94, 20, -1,   -1),
-	MUX_PG(ld11,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 11, 0x94, 22, -1,   -1),
-	MUX_PG(ld12,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 12, 0x94, 24, -1,   -1),
-	MUX_PG(ld13,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 13, 0x94, 26, -1,   -1),
-	MUX_PG(ld14,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 14, 0x94, 28, -1,   -1),
-	MUX_PG(ld15,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 15, 0x94, 30, -1,   -1),
-	MUX_PG(ld16,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 16, 0x98, 0,  -1,   -1),
-	MUX_PG(ld17,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         RSVD4,     0x1c, 17, 0x98, 2,  -1,   -1),
-	MUX_PG(ldc,    DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         RSVD4,     0x1c, 30, 0x90, 14, -1,   -1),
-	MUX_PG(ldi,    DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         RSVD4,     0x20, 6,  0x98, 16, -1,   -1),
-	MUX_PG(lhp0,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         RSVD4,     0x1c, 18, 0x98, 10, -1,   -1),
-	MUX_PG(lhp1,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         RSVD4,     0x1c, 19, 0x98, 4,  -1,   -1),
-	MUX_PG(lhp2,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         RSVD4,     0x1c, 20, 0x98, 6,  -1,   -1),
-	MUX_PG(lhs,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x20, 7,  0x90, 22, -1,   -1),
-	MUX_PG(lm0,    DISPLAYA,  DISPLAYB,  SPI3,      RSVD4,         RSVD4,     0x1c, 24, 0x90, 26, -1,   -1),
-	MUX_PG(lm1,    DISPLAYA,  DISPLAYB,  RSVD3,     CRT,           RSVD3,     0x1c, 25, 0x90, 28, -1,   -1),
-	MUX_PG(lpp,    DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         RSVD4,     0x20, 8,  0x98, 14, -1,   -1),
-	MUX_PG(lpw0,   DISPLAYA,  DISPLAYB,  SPI3,      HDMI,          DISPLAYA,  0x20, 3,  0x90, 0,  -1,   -1),
-	MUX_PG(lpw1,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         RSVD4,     0x20, 4,  0x90, 2,  -1,   -1),
-	MUX_PG(lpw2,   DISPLAYA,  DISPLAYB,  SPI3,      HDMI,          DISPLAYA,  0x20, 5,  0x90, 4,  -1,   -1),
-	MUX_PG(lsc0,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 27, 0x90, 18, -1,   -1),
-	MUX_PG(lsc1,   DISPLAYA,  DISPLAYB,  SPI3,      HDMI,          DISPLAYA,  0x1c, 28, 0x90, 20, -1,   -1),
-	MUX_PG(lsck,   DISPLAYA,  DISPLAYB,  SPI3,      HDMI,          DISPLAYA,  0x1c, 29, 0x90, 16, -1,   -1),
-	MUX_PG(lsda,   DISPLAYA,  DISPLAYB,  SPI3,      HDMI,          DISPLAYA,  0x20, 1,  0x90, 8,  -1,   -1),
-	MUX_PG(lsdi,   DISPLAYA,  DISPLAYB,  SPI3,      RSVD4,         DISPLAYA,  0x20, 2,  0x90, 6,  -1,   -1),
-	MUX_PG(lspi,   DISPLAYA,  DISPLAYB,  XIO,       HDMI,          DISPLAYA,  0x20, 0,  0x90, 10, -1,   -1),
-	MUX_PG(lvp0,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         RSVD4,     0x1c, 21, 0x90, 30, -1,   -1),
-	MUX_PG(lvp1,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         RSVD4,     0x1c, 22, 0x98, 8,  -1,   -1),
-	MUX_PG(lvs,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 26, 0x90, 24, -1,   -1),
-	MUX_PG(owc,    OWR,       RSVD2,     RSVD3,     RSVD4,         OWR,       0x14, 31, 0x84, 8,  0xb0, 30),
-	MUX_PG(pmc,    PWR_ON,    PWR_INTR,  RSVD3,     RSVD4,         PWR_ON,    0x14, 23, 0x98, 18, -1,   -1),
-	MUX_PG(pta,    I2C2,      HDMI,      GMI,       RSVD4,         RSVD4,     0x14, 24, 0x98, 22, 0xa4, 4),
-	MUX_PG(rm,     I2C1,      RSVD2,     RSVD3,     RSVD4,         RSVD4,     0x14, 25, 0x80, 14, 0xa4, 0),
-	MUX_PG(sdb,    UARTA,     PWM,       SDIO3,     SPI2,          PWM,       0x20, 15, 0x8c, 10, -1,   -1),
-	MUX_PG(sdc,    PWM,       TWC,       SDIO3,     SPI3,          TWC,       0x18, 1,  0x8c, 12, 0xac, 28),
-	MUX_PG(sdd,    UARTA,     PWM,       SDIO3,     SPI3,          PWM,       0x18, 2,  0x8c, 14, 0xac, 30),
-	MUX_PG(sdio1,  SDIO1,     RSVD2,     UARTE,     UARTA,         RSVD2,     0x14, 30, 0x80, 30, 0xb0, 18),
-	MUX_PG(slxa,   PCIE,      SPI4,      SDIO3,     SPI2,          PCIE,      0x18, 3,  0x84, 6,  0xa4, 22),
-	MUX_PG(slxc,   SPDIF,     SPI4,      SDIO3,     SPI2,          SPI4,      0x18, 5,  0x84, 10, 0xa4, 26),
-	MUX_PG(slxd,   SPDIF,     SPI4,      SDIO3,     SPI2,          SPI4,      0x18, 6,  0x84, 12, 0xa4, 28),
-	MUX_PG(slxk,   PCIE,      SPI4,      SDIO3,     SPI2,          PCIE,      0x18, 7,  0x84, 14, 0xa4, 30),
-	MUX_PG(spdi,   SPDIF,     RSVD2,     I2C1,      SDIO2,         RSVD2,     0x18, 8,  0x8c, 8,  0xa4, 16),
-	MUX_PG(spdo,   SPDIF,     RSVD2,     I2C1,      SDIO2,         RSVD2,     0x18, 9,  0x8c, 6,  0xa4, 18),
-	MUX_PG(spia,   SPI1,      SPI2,      SPI3,      GMI,           GMI,       0x18, 10, 0x8c, 30, 0xa8, 4),
-	MUX_PG(spib,   SPI1,      SPI2,      SPI3,      GMI,           GMI,       0x18, 11, 0x8c, 28, 0xa8, 6),
-	MUX_PG(spic,   SPI1,      SPI2,      SPI3,      GMI,           GMI,       0x18, 12, 0x8c, 26, 0xa8, 8),
-	MUX_PG(spid,   SPI2,      SPI1,      SPI2_ALT,  GMI,           GMI,       0x18, 13, 0x8c, 24, 0xa8, 10),
-	MUX_PG(spie,   SPI2,      SPI1,      SPI2_ALT,  GMI,           GMI,       0x18, 14, 0x8c, 22, 0xa8, 12),
-	MUX_PG(spif,   SPI3,      SPI1,      SPI2,      RSVD4,         RSVD4,     0x18, 15, 0x8c, 20, 0xa8, 14),
-	MUX_PG(spig,   SPI3,      SPI2,      SPI2_ALT,  I2C1,          SPI2_ALT,  0x18, 16, 0x8c, 18, 0xa8, 16),
-	MUX_PG(spih,   SPI3,      SPI2,      SPI2_ALT,  I2C1,          SPI2_ALT,  0x18, 17, 0x8c, 16, 0xa8, 18),
-	MUX_PG(uaa,    SPI3,      MIPI_HS,   UARTA,     ULPI,          MIPI_HS,   0x18, 18, 0x80, 0,  0xac, 0),
-	MUX_PG(uab,    SPI2,      MIPI_HS,   UARTA,     ULPI,          MIPI_HS,   0x18, 19, 0x80, 2,  0xac, 2),
-	MUX_PG(uac,    OWR,       RSVD2,     RSVD3,     RSVD4,         RSVD4,     0x18, 20, 0x80, 4,  0xac, 4),
-	MUX_PG(uad,    IRDA,      SPDIF,     UARTA,     SPI4,          SPDIF,     0x18, 21, 0x80, 6,  0xac, 6),
-	MUX_PG(uca,    UARTC,     RSVD2,     GMI,       RSVD4,         RSVD4,     0x18, 22, 0x84, 16, 0xac, 8),
-	MUX_PG(ucb,    UARTC,     PWM,       GMI,       RSVD4,         RSVD4,     0x18, 23, 0x84, 18, 0xac, 10),
-	MUX_PG(uda,    SPI1,      RSVD2,     UARTD,     ULPI,          RSVD2,     0x20, 13, 0x80, 8,  0xb0, 16),
+	/*     name,   f0,        f1,        f2,        f3,            tri r/b,  mux r/b,  pupd r/b */
+	MUX_PG(ata,    IDE,       NAND,      GMI,       RSVD4,         0x14, 0,  0x80, 24, 0xa0, 0),
+	MUX_PG(atb,    IDE,       NAND,      GMI,       SDIO4,         0x14, 1,  0x80, 16, 0xa0, 2),
+	MUX_PG(atc,    IDE,       NAND,      GMI,       SDIO4,         0x14, 2,  0x80, 22, 0xa0, 4),
+	MUX_PG(atd,    IDE,       NAND,      GMI,       SDIO4,         0x14, 3,  0x80, 20, 0xa0, 6),
+	MUX_PG(ate,    IDE,       NAND,      GMI,       RSVD4,         0x18, 25, 0x80, 12, 0xa0, 8),
+	MUX_PG(cdev1,  OSC,       PLLA_OUT,  PLLM_OUT1, AUDIO_SYNC,    0x14, 4,  0x88, 2,  0xa8, 0),
+	MUX_PG(cdev2,  OSC,       AHB_CLK,   APB_CLK,   PLLP_OUT4,     0x14, 5,  0x88, 4,  0xa8, 2),
+	MUX_PG(crtp,   CRT,       RSVD2,     RSVD3,     RSVD4,         0x20, 14, 0x98, 20, 0xa4, 24),
+	MUX_PG(csus,   PLLC_OUT1, PLLP_OUT2, PLLP_OUT3, VI_SENSOR_CLK, 0x14, 6,  0x88, 6,  0xac, 24),
+	MUX_PG(dap1,   DAP1,      RSVD2,     GMI,       SDIO2,         0x14, 7,  0x88, 20, 0xa0, 10),
+	MUX_PG(dap2,   DAP2,      TWC,       RSVD3,     GMI,           0x14, 8,  0x88, 22, 0xa0, 12),
+	MUX_PG(dap3,   DAP3,      RSVD2,     RSVD3,     RSVD4,         0x14, 9,  0x88, 24, 0xa0, 14),
+	MUX_PG(dap4,   DAP4,      RSVD2,     GMI,       RSVD4,         0x14, 10, 0x88, 26, 0xa0, 16),
+	MUX_PG(ddc,    I2C2,      RSVD2,     RSVD3,     RSVD4,         0x18, 31, 0x88, 0,  0xb0, 28),
+	MUX_PG(dta,    RSVD1,     SDIO2,     VI,        RSVD4,         0x14, 11, 0x84, 20, 0xa0, 18),
+	MUX_PG(dtb,    RSVD1,     RSVD2,     VI,        SPI1,          0x14, 12, 0x84, 22, 0xa0, 20),
+	MUX_PG(dtc,    RSVD1,     RSVD2,     VI,        RSVD4,         0x14, 13, 0x84, 26, 0xa0, 22),
+	MUX_PG(dtd,    RSVD1,     SDIO2,     VI,        RSVD4,         0x14, 14, 0x84, 28, 0xa0, 24),
+	MUX_PG(dte,    RSVD1,     RSVD2,     VI,        SPI1,          0x14, 15, 0x84, 30, 0xa0, 26),
+	MUX_PG(dtf,    I2C3,      RSVD2,     VI,        RSVD4,         0x20, 12, 0x98, 30, 0xa0, 28),
+	MUX_PG(gma,    UARTE,     SPI3,      GMI,       SDIO4,         0x14, 28, 0x84, 0,  0xb0, 20),
+	MUX_PG(gmb,    IDE,       NAND,      GMI,       GMI_INT,       0x18, 29, 0x88, 28, 0xb0, 22),
+	MUX_PG(gmc,    UARTD,     SPI4,      GMI,       SFLASH,        0x14, 29, 0x84, 2,  0xb0, 24),
+	MUX_PG(gmd,    RSVD1,     NAND,      GMI,       SFLASH,        0x18, 30, 0x88, 30, 0xb0, 26),
+	MUX_PG(gme,    RSVD1,     DAP5,      GMI,       SDIO4,         0x18, 0,  0x8c, 0,  0xa8, 24),
+	MUX_PG(gpu,    PWM,       UARTA,     GMI,       RSVD4,         0x14, 16, 0x8c, 4,  0xa4, 20),
+	MUX_PG(gpu7,   RTCK,      RSVD2,     RSVD3,     RSVD4,         0x20, 11, 0x98, 28, 0xa4, 6),
+	MUX_PG(gpv,    PCIE,      RSVD2,     RSVD3,     RSVD4,         0x14, 17, 0x8c, 2,  0xa0, 30),
+	MUX_PG(hdint,  HDMI,      RSVD2,     RSVD3,     RSVD4,         0x1c, 23, 0x84, 4,  -1,   -1),
+	MUX_PG(i2cp,   I2CP,      RSVD2,     RSVD3,     RSVD4,         0x14, 18, 0x88, 8,  0xa4, 2),
+	MUX_PG(irrx,   UARTA,     UARTB,     GMI,       SPI4,          0x14, 20, 0x88, 18, 0xa8, 22),
+	MUX_PG(irtx,   UARTA,     UARTB,     GMI,       SPI4,          0x14, 19, 0x88, 16, 0xa8, 20),
+	MUX_PG(kbca,   KBC,       NAND,      SDIO2,     EMC_TEST0_DLL, 0x14, 22, 0x88, 10, 0xa4, 8),
+	MUX_PG(kbcb,   KBC,       NAND,      SDIO2,     MIO,           0x14, 21, 0x88, 12, 0xa4, 10),
+	MUX_PG(kbcc,   KBC,       NAND,      TRACE,     EMC_TEST1_DLL, 0x18, 26, 0x88, 14, 0xa4, 12),
+	MUX_PG(kbcd,   KBC,       NAND,      SDIO2,     MIO,           0x20, 10, 0x98, 26, 0xa4, 14),
+	MUX_PG(kbce,   KBC,       NAND,      OWR,       RSVD4,         0x14, 26, 0x80, 28, 0xb0, 2),
+	MUX_PG(kbcf,   KBC,       NAND,      TRACE,     MIO,           0x14, 27, 0x80, 26, 0xb0, 0),
+	MUX_PG(lcsn,   DISPLAYA,  DISPLAYB,  SPI3,      RSVD4,         0x1c, 31, 0x90, 12, -1,   -1),
+	MUX_PG(ld0,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 0,  0x94, 0,  -1,   -1),
+	MUX_PG(ld1,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 1,  0x94, 2,  -1,   -1),
+	MUX_PG(ld2,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 2,  0x94, 4,  -1,   -1),
+	MUX_PG(ld3,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 3,  0x94, 6,  -1,   -1),
+	MUX_PG(ld4,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 4,  0x94, 8,  -1,   -1),
+	MUX_PG(ld5,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 5,  0x94, 10, -1,   -1),
+	MUX_PG(ld6,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 6,  0x94, 12, -1,   -1),
+	MUX_PG(ld7,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 7,  0x94, 14, -1,   -1),
+	MUX_PG(ld8,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 8,  0x94, 16, -1,   -1),
+	MUX_PG(ld9,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 9,  0x94, 18, -1,   -1),
+	MUX_PG(ld10,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 10, 0x94, 20, -1,   -1),
+	MUX_PG(ld11,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 11, 0x94, 22, -1,   -1),
+	MUX_PG(ld12,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 12, 0x94, 24, -1,   -1),
+	MUX_PG(ld13,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 13, 0x94, 26, -1,   -1),
+	MUX_PG(ld14,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 14, 0x94, 28, -1,   -1),
+	MUX_PG(ld15,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 15, 0x94, 30, -1,   -1),
+	MUX_PG(ld16,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 16, 0x98, 0,  -1,   -1),
+	MUX_PG(ld17,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         0x1c, 17, 0x98, 2,  -1,   -1),
+	MUX_PG(ldc,    DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         0x1c, 30, 0x90, 14, -1,   -1),
+	MUX_PG(ldi,    DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         0x20, 6,  0x98, 16, -1,   -1),
+	MUX_PG(lhp0,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         0x1c, 18, 0x98, 10, -1,   -1),
+	MUX_PG(lhp1,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         0x1c, 19, 0x98, 4,  -1,   -1),
+	MUX_PG(lhp2,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         0x1c, 20, 0x98, 6,  -1,   -1),
+	MUX_PG(lhs,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x20, 7,  0x90, 22, -1,   -1),
+	MUX_PG(lm0,    DISPLAYA,  DISPLAYB,  SPI3,      RSVD4,         0x1c, 24, 0x90, 26, -1,   -1),
+	MUX_PG(lm1,    DISPLAYA,  DISPLAYB,  RSVD3,     CRT,           0x1c, 25, 0x90, 28, -1,   -1),
+	MUX_PG(lpp,    DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         0x20, 8,  0x98, 14, -1,   -1),
+	MUX_PG(lpw0,   DISPLAYA,  DISPLAYB,  SPI3,      HDMI,          0x20, 3,  0x90, 0,  -1,   -1),
+	MUX_PG(lpw1,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         0x20, 4,  0x90, 2,  -1,   -1),
+	MUX_PG(lpw2,   DISPLAYA,  DISPLAYB,  SPI3,      HDMI,          0x20, 5,  0x90, 4,  -1,   -1),
+	MUX_PG(lsc0,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 27, 0x90, 18, -1,   -1),
+	MUX_PG(lsc1,   DISPLAYA,  DISPLAYB,  SPI3,      HDMI,          0x1c, 28, 0x90, 20, -1,   -1),
+	MUX_PG(lsck,   DISPLAYA,  DISPLAYB,  SPI3,      HDMI,          0x1c, 29, 0x90, 16, -1,   -1),
+	MUX_PG(lsda,   DISPLAYA,  DISPLAYB,  SPI3,      HDMI,          0x20, 1,  0x90, 8,  -1,   -1),
+	MUX_PG(lsdi,   DISPLAYA,  DISPLAYB,  SPI3,      RSVD4,         0x20, 2,  0x90, 6,  -1,   -1),
+	MUX_PG(lspi,   DISPLAYA,  DISPLAYB,  XIO,       HDMI,          0x20, 0,  0x90, 10, -1,   -1),
+	MUX_PG(lvp0,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         0x1c, 21, 0x90, 30, -1,   -1),
+	MUX_PG(lvp1,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         0x1c, 22, 0x98, 8,  -1,   -1),
+	MUX_PG(lvs,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 26, 0x90, 24, -1,   -1),
+	MUX_PG(owc,    OWR,       RSVD2,     RSVD3,     RSVD4,         0x14, 31, 0x84, 8,  0xb0, 30),
+	MUX_PG(pmc,    PWR_ON,    PWR_INTR,  RSVD3,     RSVD4,         0x14, 23, 0x98, 18, -1,   -1),
+	MUX_PG(pta,    I2C2,      HDMI,      GMI,       RSVD4,         0x14, 24, 0x98, 22, 0xa4, 4),
+	MUX_PG(rm,     I2C1,      RSVD2,     RSVD3,     RSVD4,         0x14, 25, 0x80, 14, 0xa4, 0),
+	MUX_PG(sdb,    UARTA,     PWM,       SDIO3,     SPI2,          0x20, 15, 0x8c, 10, -1,   -1),
+	MUX_PG(sdc,    PWM,       TWC,       SDIO3,     SPI3,          0x18, 1,  0x8c, 12, 0xac, 28),
+	MUX_PG(sdd,    UARTA,     PWM,       SDIO3,     SPI3,          0x18, 2,  0x8c, 14, 0xac, 30),
+	MUX_PG(sdio1,  SDIO1,     RSVD2,     UARTE,     UARTA,         0x14, 30, 0x80, 30, 0xb0, 18),
+	MUX_PG(slxa,   PCIE,      SPI4,      SDIO3,     SPI2,          0x18, 3,  0x84, 6,  0xa4, 22),
+	MUX_PG(slxc,   SPDIF,     SPI4,      SDIO3,     SPI2,          0x18, 5,  0x84, 10, 0xa4, 26),
+	MUX_PG(slxd,   SPDIF,     SPI4,      SDIO3,     SPI2,          0x18, 6,  0x84, 12, 0xa4, 28),
+	MUX_PG(slxk,   PCIE,      SPI4,      SDIO3,     SPI2,          0x18, 7,  0x84, 14, 0xa4, 30),
+	MUX_PG(spdi,   SPDIF,     RSVD2,     I2C1,      SDIO2,         0x18, 8,  0x8c, 8,  0xa4, 16),
+	MUX_PG(spdo,   SPDIF,     RSVD2,     I2C1,      SDIO2,         0x18, 9,  0x8c, 6,  0xa4, 18),
+	MUX_PG(spia,   SPI1,      SPI2,      SPI3,      GMI,           0x18, 10, 0x8c, 30, 0xa8, 4),
+	MUX_PG(spib,   SPI1,      SPI2,      SPI3,      GMI,           0x18, 11, 0x8c, 28, 0xa8, 6),
+	MUX_PG(spic,   SPI1,      SPI2,      SPI3,      GMI,           0x18, 12, 0x8c, 26, 0xa8, 8),
+	MUX_PG(spid,   SPI2,      SPI1,      SPI2_ALT,  GMI,           0x18, 13, 0x8c, 24, 0xa8, 10),
+	MUX_PG(spie,   SPI2,      SPI1,      SPI2_ALT,  GMI,           0x18, 14, 0x8c, 22, 0xa8, 12),
+	MUX_PG(spif,   SPI3,      SPI1,      SPI2,      RSVD4,         0x18, 15, 0x8c, 20, 0xa8, 14),
+	MUX_PG(spig,   SPI3,      SPI2,      SPI2_ALT,  I2C1,          0x18, 16, 0x8c, 18, 0xa8, 16),
+	MUX_PG(spih,   SPI3,      SPI2,      SPI2_ALT,  I2C1,          0x18, 17, 0x8c, 16, 0xa8, 18),
+	MUX_PG(uaa,    SPI3,      MIPI_HS,   UARTA,     ULPI,          0x18, 18, 0x80, 0,  0xac, 0),
+	MUX_PG(uab,    SPI2,      MIPI_HS,   UARTA,     ULPI,          0x18, 19, 0x80, 2,  0xac, 2),
+	MUX_PG(uac,    OWR,       RSVD2,     RSVD3,     RSVD4,         0x18, 20, 0x80, 4,  0xac, 4),
+	MUX_PG(uad,    IRDA,      SPDIF,     UARTA,     SPI4,          0x18, 21, 0x80, 6,  0xac, 6),
+	MUX_PG(uca,    UARTC,     RSVD2,     GMI,       RSVD4,         0x18, 22, 0x84, 16, 0xac, 8),
+	MUX_PG(ucb,    UARTC,     PWM,       GMI,       RSVD4,         0x18, 23, 0x84, 18, 0xac, 10),
+	MUX_PG(uda,    SPI1,      RSVD2,     UARTD,     ULPI,          0x20, 13, 0x80, 8,  0xb0, 16),
 	/*      pg_name, pupd_r/b */
 	PULL_PG(ck32,    0xb0, 14),
 	PULL_PG(ddrc,    0xac, 26),
diff --git a/drivers/pinctrl/pinctrl-tegra30.c b/drivers/pinctrl/pinctrl-tegra30.c
index 41d24f5c285..fe2d2cf78ad 100644
--- a/drivers/pinctrl/pinctrl-tegra30.c
+++ b/drivers/pinctrl/pinctrl-tegra30.c
@@ -2108,10 +2108,12 @@ static struct tegra_function tegra30_functions[] = {
 #define DRV_PINGROUP_REG_A		0x868	/* bank 0 */
 #define PINGROUP_REG_A			0x3000	/* bank 1 */
 
-#define PINGROUP_REG_Y(r)		((r) - PINGROUP_REG_A)
-#define PINGROUP_REG_N(r)		-1
+#define PINGROUP_REG(r)			((r) - PINGROUP_REG_A)
 
-#define PINGROUP(pg_name, f0, f1, f2, f3, f_safe, r, od, ior)	\
+#define PINGROUP_BIT_Y(b)		(b)
+#define PINGROUP_BIT_N(b)		(-1)
+
+#define PINGROUP(pg_name, f0, f1, f2, f3, r, od, ior)		\
 	{							\
 		.name = #pg_name,				\
 		.pins = pg_name##_pins,				\
@@ -2122,35 +2124,24 @@ static struct tegra_function tegra30_functions[] = {
 			TEGRA_MUX_##f2,				\
 			TEGRA_MUX_##f3,				\
 		},						\
-		.func_safe = TEGRA_MUX_##f_safe,		\
-		.mux_reg = PINGROUP_REG_Y(r),			\
+		.mux_reg = PINGROUP_REG(r),			\
 		.mux_bank = 1,					\
 		.mux_bit = 0,					\
-		.pupd_reg = PINGROUP_REG_Y(r),			\
+		.pupd_reg = PINGROUP_REG(r),			\
 		.pupd_bank = 1,					\
 		.pupd_bit = 2,					\
-		.tri_reg = PINGROUP_REG_Y(r),			\
+		.tri_reg = PINGROUP_REG(r),			\
 		.tri_bank = 1,					\
 		.tri_bit = 4,					\
-		.einput_reg = PINGROUP_REG_Y(r),		\
-		.einput_bank = 1,				\
-		.einput_bit = 5,				\
-		.odrain_reg = PINGROUP_REG_##od(r),		\
-		.odrain_bank = 1,				\
-		.odrain_bit = 6,				\
-		.lock_reg = PINGROUP_REG_Y(r),			\
-		.lock_bank = 1,					\
-		.lock_bit = 7,					\
-		.ioreset_reg = PINGROUP_REG_##ior(r),		\
-		.ioreset_bank = 1,				\
-		.ioreset_bit = 8,				\
-		.rcv_sel_reg = -1,				\
+		.einput_bit = PINGROUP_BIT_Y(5),		\
+		.odrain_bit = PINGROUP_BIT_##od(6),		\
+		.lock_bit = PINGROUP_BIT_Y(7),			\
+		.ioreset_bit = PINGROUP_BIT_##ior(8),		\
+		.rcv_sel_bit = -1,				\
 		.drv_reg = -1,					\
-		.drvtype_reg = -1,				\
 	}
 
-#define DRV_PINGROUP_REG_Y(r)		((r) - DRV_PINGROUP_REG_A)
-#define DRV_PINGROUP_REG_N(r)		-1
+#define DRV_PINGROUP_REG(r)		((r) - DRV_PINGROUP_REG_A)
 
 #define DRV_PINGROUP(pg_name, r, hsm_b, schmitt_b, lpmd_b,	\
 		     drvdn_b, drvdn_w, drvup_b, drvup_w,	\
@@ -2162,12 +2153,12 @@ static struct tegra_function tegra30_functions[] = {
 		.mux_reg = -1,					\
 		.pupd_reg = -1,					\
 		.tri_reg = -1,					\
-		.einput_reg = -1,				\
-		.odrain_reg = -1,				\
-		.lock_reg = -1,					\
-		.ioreset_reg = -1,				\
-		.rcv_sel_reg = -1,				\
-		.drv_reg = DRV_PINGROUP_REG_Y(r),		\
+		.einput_bit = -1,				\
+		.odrain_bit = -1,				\
+		.lock_bit = -1,					\
+		.ioreset_bit = -1,				\
+		.rcv_sel_bit = -1,				\
+		.drv_reg = DRV_PINGROUP_REG(r),			\
 		.drv_bank = 0,					\
 		.hsm_bit = hsm_b,				\
 		.schmitt_bit = schmitt_b,			\
@@ -2180,260 +2171,260 @@ static struct tegra_function tegra30_functions[] = {
 		.slwr_width = slwr_w,				\
 		.slwf_bit = slwf_b,				\
 		.slwf_width = slwf_w,				\
-		.drvtype_reg = -1,				\
+		.drvtype_bit = -1,				\
 	}
 
 static const struct tegra_pingroup tegra30_groups[] = {
-	/*       pg_name,              f0,           f1,           f2,           f3,           safe,         r,      od, ior */
-	PINGROUP(clk_32k_out_pa0,      BLINK,        RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x331c, N, N),
-	PINGROUP(uart3_cts_n_pa1,      UARTC,        RSVD2,        GMI,          RSVD4,        RSVD4,        0x317c, N, N),
-	PINGROUP(dap2_fs_pa2,          I2S1,         HDA,          RSVD3,        GMI,          RSVD3,        0x3358, N, N),
-	PINGROUP(dap2_sclk_pa3,        I2S1,         HDA,          RSVD3,        GMI,          RSVD3,        0x3364, N, N),
-	PINGROUP(dap2_din_pa4,         I2S1,         HDA,          RSVD3,        GMI,          RSVD3,        0x335c, N, N),
-	PINGROUP(dap2_dout_pa5,        I2S1,         HDA,          RSVD3,        GMI,          RSVD3,        0x3360, N, N),
-	PINGROUP(sdmmc3_clk_pa6,       UARTA,        PWM2,         SDMMC3,       SPI3,         SPI3,         0x3390, N, N),
-	PINGROUP(sdmmc3_cmd_pa7,       UARTA,        PWM3,         SDMMC3,       SPI2,         SPI2,         0x3394, N, N),
-	PINGROUP(gmi_a17_pb0,          UARTD,        SPI4,         GMI,          DTV,          DTV,          0x3234, N, N),
-	PINGROUP(gmi_a18_pb1,          UARTD,        SPI4,         GMI,          DTV,          DTV,          0x3238, N, N),
-	PINGROUP(lcd_pwr0_pb2,         DISPLAYA,     DISPLAYB,     SPI5,         HDCP,         HDCP,         0x3090, N, N),
-	PINGROUP(lcd_pclk_pb3,         DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x3094, N, N),
-	PINGROUP(sdmmc3_dat3_pb4,      RSVD1,        PWM0,         SDMMC3,       SPI3,         RSVD1,        0x33a4, N, N),
-	PINGROUP(sdmmc3_dat2_pb5,      RSVD1,        PWM1,         SDMMC3,       SPI3,         RSVD1,        0x33a0, N, N),
-	PINGROUP(sdmmc3_dat1_pb6,      RSVD1,        RSVD2,        SDMMC3,       SPI3,         RSVD2,        0x339c, N, N),
-	PINGROUP(sdmmc3_dat0_pb7,      RSVD1,        RSVD2,        SDMMC3,       SPI3,         RSVD2,        0x3398, N, N),
-	PINGROUP(uart3_rts_n_pc0,      UARTC,        PWM0,         GMI,          RSVD4,        RSVD4,        0x3180, N, N),
-	PINGROUP(lcd_pwr1_pc1,         DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x3070, N, N),
-	PINGROUP(uart2_txd_pc2,        UARTB,        SPDIF,        UARTA,        SPI4,         SPI4,         0x3168, N, N),
-	PINGROUP(uart2_rxd_pc3,        UARTB,        SPDIF,        UARTA,        SPI4,         SPI4,         0x3164, N, N),
-	PINGROUP(gen1_i2c_scl_pc4,     I2C1,         RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x31a4, Y, N),
-	PINGROUP(gen1_i2c_sda_pc5,     I2C1,         RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x31a0, Y, N),
-	PINGROUP(lcd_pwr2_pc6,         DISPLAYA,     DISPLAYB,     SPI5,         HDCP,         HDCP,         0x3074, N, N),
-	PINGROUP(gmi_wp_n_pc7,         RSVD1,        NAND,         GMI,          GMI_ALT,      RSVD1,        0x31c0, N, N),
-	PINGROUP(sdmmc3_dat5_pd0,      PWM0,         SPI4,         SDMMC3,       SPI2,         SPI2,         0x33ac, N, N),
-	PINGROUP(sdmmc3_dat4_pd1,      PWM1,         SPI4,         SDMMC3,       SPI2,         SPI2,         0x33a8, N, N),
-	PINGROUP(lcd_dc1_pd2,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x310c, N, N),
-	PINGROUP(sdmmc3_dat6_pd3,      SPDIF,        SPI4,         SDMMC3,       SPI2,         SPI2,         0x33b0, N, N),
-	PINGROUP(sdmmc3_dat7_pd4,      SPDIF,        SPI4,         SDMMC3,       SPI2,         SPI2,         0x33b4, N, N),
-	PINGROUP(vi_d1_pd5,            DDR,          SDMMC2,       VI,           RSVD4,        RSVD4,        0x3128, N, Y),
-	PINGROUP(vi_vsync_pd6,         DDR,          RSVD2,        VI,           RSVD4,        RSVD4,        0x315c, N, Y),
-	PINGROUP(vi_hsync_pd7,         DDR,          RSVD2,        VI,           RSVD4,        RSVD4,        0x3160, N, Y),
-	PINGROUP(lcd_d0_pe0,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30a4, N, N),
-	PINGROUP(lcd_d1_pe1,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30a8, N, N),
-	PINGROUP(lcd_d2_pe2,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30ac, N, N),
-	PINGROUP(lcd_d3_pe3,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30b0, N, N),
-	PINGROUP(lcd_d4_pe4,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30b4, N, N),
-	PINGROUP(lcd_d5_pe5,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30b8, N, N),
-	PINGROUP(lcd_d6_pe6,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30bc, N, N),
-	PINGROUP(lcd_d7_pe7,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30c0, N, N),
-	PINGROUP(lcd_d8_pf0,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30c4, N, N),
-	PINGROUP(lcd_d9_pf1,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30c8, N, N),
-	PINGROUP(lcd_d10_pf2,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30cc, N, N),
-	PINGROUP(lcd_d11_pf3,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30d0, N, N),
-	PINGROUP(lcd_d12_pf4,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30d4, N, N),
-	PINGROUP(lcd_d13_pf5,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30d8, N, N),
-	PINGROUP(lcd_d14_pf6,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30dc, N, N),
-	PINGROUP(lcd_d15_pf7,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30e0, N, N),
-	PINGROUP(gmi_ad0_pg0,          RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x31f0, N, N),
-	PINGROUP(gmi_ad1_pg1,          RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x31f4, N, N),
-	PINGROUP(gmi_ad2_pg2,          RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x31f8, N, N),
-	PINGROUP(gmi_ad3_pg3,          RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x31fc, N, N),
-	PINGROUP(gmi_ad4_pg4,          RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x3200, N, N),
-	PINGROUP(gmi_ad5_pg5,          RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x3204, N, N),
-	PINGROUP(gmi_ad6_pg6,          RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x3208, N, N),
-	PINGROUP(gmi_ad7_pg7,          RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x320c, N, N),
-	PINGROUP(gmi_ad8_ph0,          PWM0,         NAND,         GMI,          RSVD4,        RSVD4,        0x3210, N, N),
-	PINGROUP(gmi_ad9_ph1,          PWM1,         NAND,         GMI,          RSVD4,        RSVD4,        0x3214, N, N),
-	PINGROUP(gmi_ad10_ph2,         PWM2,         NAND,         GMI,          RSVD4,        RSVD4,        0x3218, N, N),
-	PINGROUP(gmi_ad11_ph3,         PWM3,         NAND,         GMI,          RSVD4,        RSVD4,        0x321c, N, N),
-	PINGROUP(gmi_ad12_ph4,         RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x3220, N, N),
-	PINGROUP(gmi_ad13_ph5,         RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x3224, N, N),
-	PINGROUP(gmi_ad14_ph6,         RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x3228, N, N),
-	PINGROUP(gmi_ad15_ph7,         RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x322c, N, N),
-	PINGROUP(gmi_wr_n_pi0,         RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x3240, N, N),
-	PINGROUP(gmi_oe_n_pi1,         RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x3244, N, N),
-	PINGROUP(gmi_dqs_pi2,          RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x3248, N, N),
-	PINGROUP(gmi_cs6_n_pi3,        NAND,         NAND_ALT,     GMI,          SATA,         SATA,         0x31e8, N, N),
-	PINGROUP(gmi_rst_n_pi4,        NAND,         NAND_ALT,     GMI,          RSVD4,        RSVD4,        0x324c, N, N),
-	PINGROUP(gmi_iordy_pi5,        RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x31c4, N, N),
-	PINGROUP(gmi_cs7_n_pi6,        NAND,         NAND_ALT,     GMI,          GMI_ALT,      GMI_ALT,      0x31ec, N, N),
-	PINGROUP(gmi_wait_pi7,         RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x31c8, N, N),
-	PINGROUP(gmi_cs0_n_pj0,        RSVD1,        NAND,         GMI,          DTV,          RSVD1,        0x31d4, N, N),
-	PINGROUP(lcd_de_pj1,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x3098, N, N),
-	PINGROUP(gmi_cs1_n_pj2,        RSVD1,        NAND,         GMI,          DTV,          RSVD1,        0x31d8, N, N),
-	PINGROUP(lcd_hsync_pj3,        DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x309c, N, N),
-	PINGROUP(lcd_vsync_pj4,        DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30a0, N, N),
-	PINGROUP(uart2_cts_n_pj5,      UARTA,        UARTB,        GMI,          SPI4,         SPI4,         0x3170, N, N),
-	PINGROUP(uart2_rts_n_pj6,      UARTA,        UARTB,        GMI,          SPI4,         SPI4,         0x316c, N, N),
-	PINGROUP(gmi_a16_pj7,          UARTD,        SPI4,         GMI,          GMI_ALT,      GMI_ALT,      0x3230, N, N),
-	PINGROUP(gmi_adv_n_pk0,        RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x31cc, N, N),
-	PINGROUP(gmi_clk_pk1,          RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x31d0, N, N),
-	PINGROUP(gmi_cs4_n_pk2,        RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x31e4, N, N),
-	PINGROUP(gmi_cs2_n_pk3,        RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x31dc, N, N),
-	PINGROUP(gmi_cs3_n_pk4,        RSVD1,        NAND,         GMI,          GMI_ALT,      RSVD1,        0x31e0, N, N),
-	PINGROUP(spdif_out_pk5,        SPDIF,        RSVD2,        I2C1,         SDMMC2,       RSVD2,        0x3354, N, N),
-	PINGROUP(spdif_in_pk6,         SPDIF,        HDA,          I2C1,         SDMMC2,       SDMMC2,       0x3350, N, N),
-	PINGROUP(gmi_a19_pk7,          UARTD,        SPI4,         GMI,          RSVD4,        RSVD4,        0x323c, N, N),
-	PINGROUP(vi_d2_pl0,            DDR,          SDMMC2,       VI,           RSVD4,        RSVD4,        0x312c, N, Y),
-	PINGROUP(vi_d3_pl1,            DDR,          SDMMC2,       VI,           RSVD4,        RSVD4,        0x3130, N, Y),
-	PINGROUP(vi_d4_pl2,            DDR,          SDMMC2,       VI,           RSVD4,        RSVD4,        0x3134, N, Y),
-	PINGROUP(vi_d5_pl3,            DDR,          SDMMC2,       VI,           RSVD4,        RSVD4,        0x3138, N, Y),
-	PINGROUP(vi_d6_pl4,            DDR,          SDMMC2,       VI,           RSVD4,        RSVD4,        0x313c, N, Y),
-	PINGROUP(vi_d7_pl5,            DDR,          SDMMC2,       VI,           RSVD4,        RSVD4,        0x3140, N, Y),
-	PINGROUP(vi_d8_pl6,            DDR,          SDMMC2,       VI,           RSVD4,        RSVD4,        0x3144, N, Y),
-	PINGROUP(vi_d9_pl7,            DDR,          SDMMC2,       VI,           RSVD4,        RSVD4,        0x3148, N, Y),
-	PINGROUP(lcd_d16_pm0,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30e4, N, N),
-	PINGROUP(lcd_d17_pm1,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30e8, N, N),
-	PINGROUP(lcd_d18_pm2,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30ec, N, N),
-	PINGROUP(lcd_d19_pm3,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30f0, N, N),
-	PINGROUP(lcd_d20_pm4,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30f4, N, N),
-	PINGROUP(lcd_d21_pm5,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30f8, N, N),
-	PINGROUP(lcd_d22_pm6,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30fc, N, N),
-	PINGROUP(lcd_d23_pm7,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x3100, N, N),
-	PINGROUP(dap1_fs_pn0,          I2S0,         HDA,          GMI,          SDMMC2,       SDMMC2,       0x3338, N, N),
-	PINGROUP(dap1_din_pn1,         I2S0,         HDA,          GMI,          SDMMC2,       SDMMC2,       0x333c, N, N),
-	PINGROUP(dap1_dout_pn2,        I2S0,         HDA,          GMI,          SDMMC2,       SDMMC2,       0x3340, N, N),
-	PINGROUP(dap1_sclk_pn3,        I2S0,         HDA,          GMI,          SDMMC2,       SDMMC2,       0x3344, N, N),
-	PINGROUP(lcd_cs0_n_pn4,        DISPLAYA,     DISPLAYB,     SPI5,         RSVD4,        RSVD4,        0x3084, N, N),
-	PINGROUP(lcd_sdout_pn5,        DISPLAYA,     DISPLAYB,     SPI5,         HDCP,         HDCP,         0x307c, N, N),
-	PINGROUP(lcd_dc0_pn6,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x3088, N, N),
-	PINGROUP(hdmi_int_pn7,         HDMI,         RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3110, N, N),
-	PINGROUP(ulpi_data7_po0,       SPI2,         HSI,          UARTA,        ULPI,         ULPI,         0x301c, N, N),
-	PINGROUP(ulpi_data0_po1,       SPI3,         HSI,          UARTA,        ULPI,         ULPI,         0x3000, N, N),
-	PINGROUP(ulpi_data1_po2,       SPI3,         HSI,          UARTA,        ULPI,         ULPI,         0x3004, N, N),
-	PINGROUP(ulpi_data2_po3,       SPI3,         HSI,          UARTA,        ULPI,         ULPI,         0x3008, N, N),
-	PINGROUP(ulpi_data3_po4,       SPI3,         HSI,          UARTA,        ULPI,         ULPI,         0x300c, N, N),
-	PINGROUP(ulpi_data4_po5,       SPI2,         HSI,          UARTA,        ULPI,         ULPI,         0x3010, N, N),
-	PINGROUP(ulpi_data5_po6,       SPI2,         HSI,          UARTA,        ULPI,         ULPI,         0x3014, N, N),
-	PINGROUP(ulpi_data6_po7,       SPI2,         HSI,          UARTA,        ULPI,         ULPI,         0x3018, N, N),
-	PINGROUP(dap3_fs_pp0,          I2S2,         RSVD2,        DISPLAYA,     DISPLAYB,     RSVD2,        0x3030, N, N),
-	PINGROUP(dap3_din_pp1,         I2S2,         RSVD2,        DISPLAYA,     DISPLAYB,     RSVD2,        0x3034, N, N),
-	PINGROUP(dap3_dout_pp2,        I2S2,         RSVD2,        DISPLAYA,     DISPLAYB,     RSVD2,        0x3038, N, N),
-	PINGROUP(dap3_sclk_pp3,        I2S2,         RSVD2,        DISPLAYA,     DISPLAYB,     RSVD2,        0x303c, N, N),
-	PINGROUP(dap4_fs_pp4,          I2S3,         RSVD2,        GMI,          RSVD4,        RSVD4,        0x31a8, N, N),
-	PINGROUP(dap4_din_pp5,         I2S3,         RSVD2,        GMI,          RSVD4,        RSVD4,        0x31ac, N, N),
-	PINGROUP(dap4_dout_pp6,        I2S3,         RSVD2,        GMI,          RSVD4,        RSVD4,        0x31b0, N, N),
-	PINGROUP(dap4_sclk_pp7,        I2S3,         RSVD2,        GMI,          RSVD4,        RSVD4,        0x31b4, N, N),
-	PINGROUP(kb_col0_pq0,          KBC,          NAND,         TRACE,        TEST,         TEST,         0x32fc, N, N),
-	PINGROUP(kb_col1_pq1,          KBC,          NAND,         TRACE,        TEST,         TEST,         0x3300, N, N),
-	PINGROUP(kb_col2_pq2,          KBC,          NAND,         TRACE,        RSVD4,        RSVD4,        0x3304, N, N),
-	PINGROUP(kb_col3_pq3,          KBC,          NAND,         TRACE,        RSVD4,        RSVD4,        0x3308, N, N),
-	PINGROUP(kb_col4_pq4,          KBC,          NAND,         TRACE,        RSVD4,        RSVD4,        0x330c, N, N),
-	PINGROUP(kb_col5_pq5,          KBC,          NAND,         TRACE,        RSVD4,        RSVD4,        0x3310, N, N),
-	PINGROUP(kb_col6_pq6,          KBC,          NAND,         TRACE,        MIO,          MIO,          0x3314, N, N),
-	PINGROUP(kb_col7_pq7,          KBC,          NAND,         TRACE,        MIO,          MIO,          0x3318, N, N),
-	PINGROUP(kb_row0_pr0,          KBC,          NAND,         RSVD3,        RSVD4,        RSVD4,        0x32bc, N, N),
-	PINGROUP(kb_row1_pr1,          KBC,          NAND,         RSVD3,        RSVD4,        RSVD4,        0x32c0, N, N),
-	PINGROUP(kb_row2_pr2,          KBC,          NAND,         RSVD3,        RSVD4,        RSVD4,        0x32c4, N, N),
-	PINGROUP(kb_row3_pr3,          KBC,          NAND,         RSVD3,        INVALID,      RSVD3,        0x32c8, N, N),
-	PINGROUP(kb_row4_pr4,          KBC,          NAND,         TRACE,        RSVD4,        RSVD4,        0x32cc, N, N),
-	PINGROUP(kb_row5_pr5,          KBC,          NAND,         TRACE,        OWR,          OWR,          0x32d0, N, N),
-	PINGROUP(kb_row6_pr6,          KBC,          NAND,         SDMMC2,       MIO,          MIO,          0x32d4, N, N),
-	PINGROUP(kb_row7_pr7,          KBC,          NAND,         SDMMC2,       MIO,          MIO,          0x32d8, N, N),
-	PINGROUP(kb_row8_ps0,          KBC,          NAND,         SDMMC2,       MIO,          MIO,          0x32dc, N, N),
-	PINGROUP(kb_row9_ps1,          KBC,          NAND,         SDMMC2,       MIO,          MIO,          0x32e0, N, N),
-	PINGROUP(kb_row10_ps2,         KBC,          NAND,         SDMMC2,       MIO,          MIO,          0x32e4, N, N),
-	PINGROUP(kb_row11_ps3,         KBC,          NAND,         SDMMC2,       MIO,          MIO,          0x32e8, N, N),
-	PINGROUP(kb_row12_ps4,         KBC,          NAND,         SDMMC2,       MIO,          MIO,          0x32ec, N, N),
-	PINGROUP(kb_row13_ps5,         KBC,          NAND,         SDMMC2,       MIO,          MIO,          0x32f0, N, N),
-	PINGROUP(kb_row14_ps6,         KBC,          NAND,         SDMMC2,       MIO,          MIO,          0x32f4, N, N),
-	PINGROUP(kb_row15_ps7,         KBC,          NAND,         SDMMC2,       MIO,          MIO,          0x32f8, N, N),
-	PINGROUP(vi_pclk_pt0,          RSVD1,        SDMMC2,       VI,           RSVD4,        RSVD4,        0x3154, N, Y),
-	PINGROUP(vi_mclk_pt1,          VI,           VI_ALT1,      VI_ALT2,      VI_ALT3,      VI_ALT3,      0x3158, N, Y),
-	PINGROUP(vi_d10_pt2,           DDR,          RSVD2,        VI,           RSVD4,        RSVD4,        0x314c, N, Y),
-	PINGROUP(vi_d11_pt3,           DDR,          RSVD2,        VI,           RSVD4,        RSVD4,        0x3150, N, Y),
-	PINGROUP(vi_d0_pt4,            DDR,          RSVD2,        VI,           RSVD4,        RSVD4,        0x3124, N, Y),
-	PINGROUP(gen2_i2c_scl_pt5,     I2C2,         HDCP,         GMI,          RSVD4,        RSVD4,        0x3250, Y, N),
-	PINGROUP(gen2_i2c_sda_pt6,     I2C2,         HDCP,         GMI,          RSVD4,        RSVD4,        0x3254, Y, N),
-	PINGROUP(sdmmc4_cmd_pt7,       I2C3,         NAND,         GMI,          SDMMC4,       SDMMC4,       0x325c, N, Y),
-	PINGROUP(pu0,                  OWR,          UARTA,        GMI,          RSVD4,        RSVD4,        0x3184, N, N),
-	PINGROUP(pu1,                  RSVD1,        UARTA,        GMI,          RSVD4,        RSVD4,        0x3188, N, N),
-	PINGROUP(pu2,                  RSVD1,        UARTA,        GMI,          RSVD4,        RSVD4,        0x318c, N, N),
-	PINGROUP(pu3,                  PWM0,         UARTA,        GMI,          RSVD4,        RSVD4,        0x3190, N, N),
-	PINGROUP(pu4,                  PWM1,         UARTA,        GMI,          RSVD4,        RSVD4,        0x3194, N, N),
-	PINGROUP(pu5,                  PWM2,         UARTA,        GMI,          RSVD4,        RSVD4,        0x3198, N, N),
-	PINGROUP(pu6,                  PWM3,         UARTA,        GMI,          RSVD4,        RSVD4,        0x319c, N, N),
-	PINGROUP(jtag_rtck_pu7,        RTCK,         RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x32b0, N, N),
-	PINGROUP(pv0,                  RSVD1,        RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3040, N, N),
-	PINGROUP(pv1,                  RSVD1,        RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3044, N, N),
-	PINGROUP(pv2,                  OWR,          RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3060, N, N),
-	PINGROUP(pv3,                  CLK_12M_OUT,  RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3064, N, N),
-	PINGROUP(ddc_scl_pv4,          I2C4,         RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3114, N, N),
-	PINGROUP(ddc_sda_pv5,          I2C4,         RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3118, N, N),
-	PINGROUP(crt_hsync_pv6,        CRT,          RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x311c, N, N),
-	PINGROUP(crt_vsync_pv7,        CRT,          RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3120, N, N),
-	PINGROUP(lcd_cs1_n_pw0,        DISPLAYA,     DISPLAYB,     SPI5,         RSVD4,        RSVD4,        0x3104, N, N),
-	PINGROUP(lcd_m1_pw1,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x3108, N, N),
-	PINGROUP(spi2_cs1_n_pw2,       SPI3,         SPI2,         SPI2_ALT,     I2C1,         I2C1,         0x3388, N, N),
-	PINGROUP(spi2_cs2_n_pw3,       SPI3,         SPI2,         SPI2_ALT,     I2C1,         I2C1,         0x338c, N, N),
-	PINGROUP(clk1_out_pw4,         EXTPERIPH1,   RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x334c, N, N),
-	PINGROUP(clk2_out_pw5,         EXTPERIPH2,   RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3068, N, N),
-	PINGROUP(uart3_txd_pw6,        UARTC,        RSVD2,        GMI,          RSVD4,        RSVD4,        0x3174, N, N),
-	PINGROUP(uart3_rxd_pw7,        UARTC,        RSVD2,        GMI,          RSVD4,        RSVD4,        0x3178, N, N),
-	PINGROUP(spi2_mosi_px0,        SPI6,         SPI2,         SPI3,         GMI,          GMI,          0x3368, N, N),
-	PINGROUP(spi2_miso_px1,        SPI6,         SPI2,         SPI3,         GMI,          GMI,          0x336c, N, N),
-	PINGROUP(spi2_sck_px2,         SPI6,         SPI2,         SPI3,         GMI,          GMI,          0x3374, N, N),
-	PINGROUP(spi2_cs0_n_px3,       SPI6,         SPI2,         SPI3,         GMI,          GMI,          0x3370, N, N),
-	PINGROUP(spi1_mosi_px4,        SPI2,         SPI1,         SPI2_ALT,     GMI,          GMI,          0x3378, N, N),
-	PINGROUP(spi1_sck_px5,         SPI2,         SPI1,         SPI2_ALT,     GMI,          GMI,          0x337c, N, N),
-	PINGROUP(spi1_cs0_n_px6,       SPI2,         SPI1,         SPI2_ALT,     GMI,          GMI,          0x3380, N, N),
-	PINGROUP(spi1_miso_px7,        SPI3,         SPI1,         SPI2_ALT,     RSVD4,        RSVD4,        0x3384, N, N),
-	PINGROUP(ulpi_clk_py0,         SPI1,         RSVD2,        UARTD,        ULPI,         RSVD2,        0x3020, N, N),
-	PINGROUP(ulpi_dir_py1,         SPI1,         RSVD2,        UARTD,        ULPI,         RSVD2,        0x3024, N, N),
-	PINGROUP(ulpi_nxt_py2,         SPI1,         RSVD2,        UARTD,        ULPI,         RSVD2,        0x3028, N, N),
-	PINGROUP(ulpi_stp_py3,         SPI1,         RSVD2,        UARTD,        ULPI,         RSVD2,        0x302c, N, N),
-	PINGROUP(sdmmc1_dat3_py4,      SDMMC1,       RSVD2,        UARTE,        UARTA,        RSVD2,        0x3050, N, N),
-	PINGROUP(sdmmc1_dat2_py5,      SDMMC1,       RSVD2,        UARTE,        UARTA,        RSVD2,        0x3054, N, N),
-	PINGROUP(sdmmc1_dat1_py6,      SDMMC1,       RSVD2,        UARTE,        UARTA,        RSVD2,        0x3058, N, N),
-	PINGROUP(sdmmc1_dat0_py7,      SDMMC1,       RSVD2,        UARTE,        UARTA,        RSVD2,        0x305c, N, N),
-	PINGROUP(sdmmc1_clk_pz0,       SDMMC1,       RSVD2,        RSVD3,        UARTA,        RSVD3,        0x3048, N, N),
-	PINGROUP(sdmmc1_cmd_pz1,       SDMMC1,       RSVD2,        RSVD3,        UARTA,        RSVD3,        0x304c, N, N),
-	PINGROUP(lcd_sdin_pz2,         DISPLAYA,     DISPLAYB,     SPI5,         RSVD4,        RSVD4,        0x3078, N, N),
-	PINGROUP(lcd_wr_n_pz3,         DISPLAYA,     DISPLAYB,     SPI5,         HDCP,         HDCP,         0x3080, N, N),
-	PINGROUP(lcd_sck_pz4,          DISPLAYA,     DISPLAYB,     SPI5,         HDCP,         HDCP,         0x308c, N, N),
-	PINGROUP(sys_clk_req_pz5,      SYSCLK,       RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3320, N, N),
-	PINGROUP(pwr_i2c_scl_pz6,      I2CPWR,       RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x32b4, Y, N),
-	PINGROUP(pwr_i2c_sda_pz7,      I2CPWR,       RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x32b8, Y, N),
-	PINGROUP(sdmmc4_dat0_paa0,     UARTE,        SPI3,         GMI,          SDMMC4,       SDMMC4,       0x3260, N, Y),
-	PINGROUP(sdmmc4_dat1_paa1,     UARTE,        SPI3,         GMI,          SDMMC4,       SDMMC4,       0x3264, N, Y),
-	PINGROUP(sdmmc4_dat2_paa2,     UARTE,        SPI3,         GMI,          SDMMC4,       SDMMC4,       0x3268, N, Y),
-	PINGROUP(sdmmc4_dat3_paa3,     UARTE,        SPI3,         GMI,          SDMMC4,       SDMMC4,       0x326c, N, Y),
-	PINGROUP(sdmmc4_dat4_paa4,     I2C3,         I2S4,         GMI,          SDMMC4,       SDMMC4,       0x3270, N, Y),
-	PINGROUP(sdmmc4_dat5_paa5,     VGP3,         I2S4,         GMI,          SDMMC4,       SDMMC4,       0x3274, N, Y),
-	PINGROUP(sdmmc4_dat6_paa6,     VGP4,         I2S4,         GMI,          SDMMC4,       SDMMC4,       0x3278, N, Y),
-	PINGROUP(sdmmc4_dat7_paa7,     VGP5,         I2S4,         GMI,          SDMMC4,       SDMMC4,       0x327c, N, Y),
-	PINGROUP(pbb0,                 I2S4,         RSVD2,        RSVD3,        SDMMC4,       RSVD3,        0x328c, N, N),
-	PINGROUP(cam_i2c_scl_pbb1,     VGP1,         I2C3,         RSVD3,        SDMMC4,       RSVD3,        0x3290, Y, N),
-	PINGROUP(cam_i2c_sda_pbb2,     VGP2,         I2C3,         RSVD3,        SDMMC4,       RSVD3,        0x3294, Y, N),
-	PINGROUP(pbb3,                 VGP3,         DISPLAYA,     DISPLAYB,     SDMMC4,       SDMMC4,       0x3298, N, N),
-	PINGROUP(pbb4,                 VGP4,         DISPLAYA,     DISPLAYB,     SDMMC4,       SDMMC4,       0x329c, N, N),
-	PINGROUP(pbb5,                 VGP5,         DISPLAYA,     DISPLAYB,     SDMMC4,       SDMMC4,       0x32a0, N, N),
-	PINGROUP(pbb6,                 VGP6,         DISPLAYA,     DISPLAYB,     SDMMC4,       SDMMC4,       0x32a4, N, N),
-	PINGROUP(pbb7,                 I2S4,         RSVD2,        RSVD3,        SDMMC4,       RSVD3,        0x32a8, N, N),
-	PINGROUP(cam_mclk_pcc0,        VI,           VI_ALT1,      VI_ALT3,      SDMMC4,       SDMMC4,       0x3284, N, N),
-	PINGROUP(pcc1,                 I2S4,         RSVD2,        RSVD3,        SDMMC4,       RSVD3,        0x3288, N, N),
-	PINGROUP(pcc2,                 I2S4,         RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x32ac, N, N),
-	PINGROUP(sdmmc4_rst_n_pcc3,    VGP6,         RSVD2,        RSVD3,        SDMMC4,       RSVD3,        0x3280, N, Y),
-	PINGROUP(sdmmc4_clk_pcc4,      INVALID,      NAND,         GMI,          SDMMC4,       SDMMC4,       0x3258, N, Y),
-	PINGROUP(clk2_req_pcc5,        DAP,          RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x306c, N, N),
-	PINGROUP(pex_l2_rst_n_pcc6,    PCIE,         HDA,          RSVD3,        RSVD4,        RSVD4,        0x33d8, N, N),
-	PINGROUP(pex_l2_clkreq_n_pcc7, PCIE,         HDA,          RSVD3,        RSVD4,        RSVD4,        0x33dc, N, N),
-	PINGROUP(pex_l0_prsnt_n_pdd0,  PCIE,         HDA,          RSVD3,        RSVD4,        RSVD4,        0x33b8, N, N),
-	PINGROUP(pex_l0_rst_n_pdd1,    PCIE,         HDA,          RSVD3,        RSVD4,        RSVD4,        0x33bc, N, N),
-	PINGROUP(pex_l0_clkreq_n_pdd2, PCIE,         HDA,          RSVD3,        RSVD4,        RSVD4,        0x33c0, N, N),
-	PINGROUP(pex_wake_n_pdd3,      PCIE,         HDA,          RSVD3,        RSVD4,        RSVD4,        0x33c4, N, N),
-	PINGROUP(pex_l1_prsnt_n_pdd4,  PCIE,         HDA,          RSVD3,        RSVD4,        RSVD4,        0x33c8, N, N),
-	PINGROUP(pex_l1_rst_n_pdd5,    PCIE,         HDA,          RSVD3,        RSVD4,        RSVD4,        0x33cc, N, N),
-	PINGROUP(pex_l1_clkreq_n_pdd6, PCIE,         HDA,          RSVD3,        RSVD4,        RSVD4,        0x33d0, N, N),
-	PINGROUP(pex_l2_prsnt_n_pdd7,  PCIE,         HDA,          RSVD3,        RSVD4,        RSVD4,        0x33d4, N, N),
-	PINGROUP(clk3_out_pee0,        EXTPERIPH3,   RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x31b8, N, N),
-	PINGROUP(clk3_req_pee1,        DEV3,         RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x31bc, N, N),
-	PINGROUP(clk1_req_pee2,        DAP,          HDA,          RSVD3,        RSVD4,        RSVD4,        0x3348, N, N),
-	PINGROUP(hdmi_cec_pee3,        CEC,          RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x33e0, Y, N),
-	PINGROUP(clk_32k_in,           CLK_32K_IN,   RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3330, N, N),
-	PINGROUP(core_pwr_req,         CORE_PWR_REQ, RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3324, N, N),
-	PINGROUP(cpu_pwr_req,          CPU_PWR_REQ,  RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3328, N, N),
-	PINGROUP(owr,                  OWR,          CEC,          RSVD3,        RSVD4,        RSVD4,        0x3334, N, N),
-	PINGROUP(pwr_int_n,            PWR_INT_N,    RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x332c, N, N),
+	/*       pg_name,              f0,           f1,           f2,           f3,           r,      od, ior */
+	PINGROUP(clk_32k_out_pa0,      BLINK,        RSVD2,        RSVD3,        RSVD4,        0x331c, N, N),
+	PINGROUP(uart3_cts_n_pa1,      UARTC,        RSVD2,        GMI,          RSVD4,        0x317c, N, N),
+	PINGROUP(dap2_fs_pa2,          I2S1,         HDA,          RSVD3,        GMI,          0x3358, N, N),
+	PINGROUP(dap2_sclk_pa3,        I2S1,         HDA,          RSVD3,        GMI,          0x3364, N, N),
+	PINGROUP(dap2_din_pa4,         I2S1,         HDA,          RSVD3,        GMI,          0x335c, N, N),
+	PINGROUP(dap2_dout_pa5,        I2S1,         HDA,          RSVD3,        GMI,          0x3360, N, N),
+	PINGROUP(sdmmc3_clk_pa6,       UARTA,        PWM2,         SDMMC3,       SPI3,         0x3390, N, N),
+	PINGROUP(sdmmc3_cmd_pa7,       UARTA,        PWM3,         SDMMC3,       SPI2,         0x3394, N, N),
+	PINGROUP(gmi_a17_pb0,          UARTD,        SPI4,         GMI,          DTV,          0x3234, N, N),
+	PINGROUP(gmi_a18_pb1,          UARTD,        SPI4,         GMI,          DTV,          0x3238, N, N),
+	PINGROUP(lcd_pwr0_pb2,         DISPLAYA,     DISPLAYB,     SPI5,         HDCP,         0x3090, N, N),
+	PINGROUP(lcd_pclk_pb3,         DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x3094, N, N),
+	PINGROUP(sdmmc3_dat3_pb4,      RSVD1,        PWM0,         SDMMC3,       SPI3,         0x33a4, N, N),
+	PINGROUP(sdmmc3_dat2_pb5,      RSVD1,        PWM1,         SDMMC3,       SPI3,         0x33a0, N, N),
+	PINGROUP(sdmmc3_dat1_pb6,      RSVD1,        RSVD2,        SDMMC3,       SPI3,         0x339c, N, N),
+	PINGROUP(sdmmc3_dat0_pb7,      RSVD1,        RSVD2,        SDMMC3,       SPI3,         0x3398, N, N),
+	PINGROUP(uart3_rts_n_pc0,      UARTC,        PWM0,         GMI,          RSVD4,        0x3180, N, N),
+	PINGROUP(lcd_pwr1_pc1,         DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x3070, N, N),
+	PINGROUP(uart2_txd_pc2,        UARTB,        SPDIF,        UARTA,        SPI4,         0x3168, N, N),
+	PINGROUP(uart2_rxd_pc3,        UARTB,        SPDIF,        UARTA,        SPI4,         0x3164, N, N),
+	PINGROUP(gen1_i2c_scl_pc4,     I2C1,         RSVD2,        RSVD3,        RSVD4,        0x31a4, Y, N),
+	PINGROUP(gen1_i2c_sda_pc5,     I2C1,         RSVD2,        RSVD3,        RSVD4,        0x31a0, Y, N),
+	PINGROUP(lcd_pwr2_pc6,         DISPLAYA,     DISPLAYB,     SPI5,         HDCP,         0x3074, N, N),
+	PINGROUP(gmi_wp_n_pc7,         RSVD1,        NAND,         GMI,          GMI_ALT,      0x31c0, N, N),
+	PINGROUP(sdmmc3_dat5_pd0,      PWM0,         SPI4,         SDMMC3,       SPI2,         0x33ac, N, N),
+	PINGROUP(sdmmc3_dat4_pd1,      PWM1,         SPI4,         SDMMC3,       SPI2,         0x33a8, N, N),
+	PINGROUP(lcd_dc1_pd2,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x310c, N, N),
+	PINGROUP(sdmmc3_dat6_pd3,      SPDIF,        SPI4,         SDMMC3,       SPI2,         0x33b0, N, N),
+	PINGROUP(sdmmc3_dat7_pd4,      SPDIF,        SPI4,         SDMMC3,       SPI2,         0x33b4, N, N),
+	PINGROUP(vi_d1_pd5,            DDR,          SDMMC2,       VI,           RSVD4,        0x3128, N, Y),
+	PINGROUP(vi_vsync_pd6,         DDR,          RSVD2,        VI,           RSVD4,        0x315c, N, Y),
+	PINGROUP(vi_hsync_pd7,         DDR,          RSVD2,        VI,           RSVD4,        0x3160, N, Y),
+	PINGROUP(lcd_d0_pe0,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30a4, N, N),
+	PINGROUP(lcd_d1_pe1,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30a8, N, N),
+	PINGROUP(lcd_d2_pe2,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30ac, N, N),
+	PINGROUP(lcd_d3_pe3,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30b0, N, N),
+	PINGROUP(lcd_d4_pe4,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30b4, N, N),
+	PINGROUP(lcd_d5_pe5,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30b8, N, N),
+	PINGROUP(lcd_d6_pe6,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30bc, N, N),
+	PINGROUP(lcd_d7_pe7,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30c0, N, N),
+	PINGROUP(lcd_d8_pf0,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30c4, N, N),
+	PINGROUP(lcd_d9_pf1,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30c8, N, N),
+	PINGROUP(lcd_d10_pf2,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30cc, N, N),
+	PINGROUP(lcd_d11_pf3,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30d0, N, N),
+	PINGROUP(lcd_d12_pf4,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30d4, N, N),
+	PINGROUP(lcd_d13_pf5,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30d8, N, N),
+	PINGROUP(lcd_d14_pf6,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30dc, N, N),
+	PINGROUP(lcd_d15_pf7,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30e0, N, N),
+	PINGROUP(gmi_ad0_pg0,          RSVD1,        NAND,         GMI,          RSVD4,        0x31f0, N, N),
+	PINGROUP(gmi_ad1_pg1,          RSVD1,        NAND,         GMI,          RSVD4,        0x31f4, N, N),
+	PINGROUP(gmi_ad2_pg2,          RSVD1,        NAND,         GMI,          RSVD4,        0x31f8, N, N),
+	PINGROUP(gmi_ad3_pg3,          RSVD1,        NAND,         GMI,          RSVD4,        0x31fc, N, N),
+	PINGROUP(gmi_ad4_pg4,          RSVD1,        NAND,         GMI,          RSVD4,        0x3200, N, N),
+	PINGROUP(gmi_ad5_pg5,          RSVD1,        NAND,         GMI,          RSVD4,        0x3204, N, N),
+	PINGROUP(gmi_ad6_pg6,          RSVD1,        NAND,         GMI,          RSVD4,        0x3208, N, N),
+	PINGROUP(gmi_ad7_pg7,          RSVD1,        NAND,         GMI,          RSVD4,        0x320c, N, N),
+	PINGROUP(gmi_ad8_ph0,          PWM0,         NAND,         GMI,          RSVD4,        0x3210, N, N),
+	PINGROUP(gmi_ad9_ph1,          PWM1,         NAND,         GMI,          RSVD4,        0x3214, N, N),
+	PINGROUP(gmi_ad10_ph2,         PWM2,         NAND,         GMI,          RSVD4,        0x3218, N, N),
+	PINGROUP(gmi_ad11_ph3,         PWM3,         NAND,         GMI,          RSVD4,        0x321c, N, N),
+	PINGROUP(gmi_ad12_ph4,         RSVD1,        NAND,         GMI,          RSVD4,        0x3220, N, N),
+	PINGROUP(gmi_ad13_ph5,         RSVD1,        NAND,         GMI,          RSVD4,        0x3224, N, N),
+	PINGROUP(gmi_ad14_ph6,         RSVD1,        NAND,         GMI,          RSVD4,        0x3228, N, N),
+	PINGROUP(gmi_ad15_ph7,         RSVD1,        NAND,         GMI,          RSVD4,        0x322c, N, N),
+	PINGROUP(gmi_wr_n_pi0,         RSVD1,        NAND,         GMI,          RSVD4,        0x3240, N, N),
+	PINGROUP(gmi_oe_n_pi1,         RSVD1,        NAND,         GMI,          RSVD4,        0x3244, N, N),
+	PINGROUP(gmi_dqs_pi2,          RSVD1,        NAND,         GMI,          RSVD4,        0x3248, N, N),
+	PINGROUP(gmi_cs6_n_pi3,        NAND,         NAND_ALT,     GMI,          SATA,         0x31e8, N, N),
+	PINGROUP(gmi_rst_n_pi4,        NAND,         NAND_ALT,     GMI,          RSVD4,        0x324c, N, N),
+	PINGROUP(gmi_iordy_pi5,        RSVD1,        NAND,         GMI,          RSVD4,        0x31c4, N, N),
+	PINGROUP(gmi_cs7_n_pi6,        NAND,         NAND_ALT,     GMI,          GMI_ALT,      0x31ec, N, N),
+	PINGROUP(gmi_wait_pi7,         RSVD1,        NAND,         GMI,          RSVD4,        0x31c8, N, N),
+	PINGROUP(gmi_cs0_n_pj0,        RSVD1,        NAND,         GMI,          DTV,          0x31d4, N, N),
+	PINGROUP(lcd_de_pj1,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x3098, N, N),
+	PINGROUP(gmi_cs1_n_pj2,        RSVD1,        NAND,         GMI,          DTV,          0x31d8, N, N),
+	PINGROUP(lcd_hsync_pj3,        DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x309c, N, N),
+	PINGROUP(lcd_vsync_pj4,        DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30a0, N, N),
+	PINGROUP(uart2_cts_n_pj5,      UARTA,        UARTB,        GMI,          SPI4,         0x3170, N, N),
+	PINGROUP(uart2_rts_n_pj6,      UARTA,        UARTB,        GMI,          SPI4,         0x316c, N, N),
+	PINGROUP(gmi_a16_pj7,          UARTD,        SPI4,         GMI,          GMI_ALT,      0x3230, N, N),
+	PINGROUP(gmi_adv_n_pk0,        RSVD1,        NAND,         GMI,          RSVD4,        0x31cc, N, N),
+	PINGROUP(gmi_clk_pk1,          RSVD1,        NAND,         GMI,          RSVD4,        0x31d0, N, N),
+	PINGROUP(gmi_cs4_n_pk2,        RSVD1,        NAND,         GMI,          RSVD4,        0x31e4, N, N),
+	PINGROUP(gmi_cs2_n_pk3,        RSVD1,        NAND,         GMI,          RSVD4,        0x31dc, N, N),
+	PINGROUP(gmi_cs3_n_pk4,        RSVD1,        NAND,         GMI,          GMI_ALT,      0x31e0, N, N),
+	PINGROUP(spdif_out_pk5,        SPDIF,        RSVD2,        I2C1,         SDMMC2,       0x3354, N, N),
+	PINGROUP(spdif_in_pk6,         SPDIF,        HDA,          I2C1,         SDMMC2,       0x3350, N, N),
+	PINGROUP(gmi_a19_pk7,          UARTD,        SPI4,         GMI,          RSVD4,        0x323c, N, N),
+	PINGROUP(vi_d2_pl0,            DDR,          SDMMC2,       VI,           RSVD4,        0x312c, N, Y),
+	PINGROUP(vi_d3_pl1,            DDR,          SDMMC2,       VI,           RSVD4,        0x3130, N, Y),
+	PINGROUP(vi_d4_pl2,            DDR,          SDMMC2,       VI,           RSVD4,        0x3134, N, Y),
+	PINGROUP(vi_d5_pl3,            DDR,          SDMMC2,       VI,           RSVD4,        0x3138, N, Y),
+	PINGROUP(vi_d6_pl4,            DDR,          SDMMC2,       VI,           RSVD4,        0x313c, N, Y),
+	PINGROUP(vi_d7_pl5,            DDR,          SDMMC2,       VI,           RSVD4,        0x3140, N, Y),
+	PINGROUP(vi_d8_pl6,            DDR,          SDMMC2,       VI,           RSVD4,        0x3144, N, Y),
+	PINGROUP(vi_d9_pl7,            DDR,          SDMMC2,       VI,           RSVD4,        0x3148, N, Y),
+	PINGROUP(lcd_d16_pm0,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30e4, N, N),
+	PINGROUP(lcd_d17_pm1,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30e8, N, N),
+	PINGROUP(lcd_d18_pm2,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30ec, N, N),
+	PINGROUP(lcd_d19_pm3,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30f0, N, N),
+	PINGROUP(lcd_d20_pm4,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30f4, N, N),
+	PINGROUP(lcd_d21_pm5,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30f8, N, N),
+	PINGROUP(lcd_d22_pm6,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30fc, N, N),
+	PINGROUP(lcd_d23_pm7,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x3100, N, N),
+	PINGROUP(dap1_fs_pn0,          I2S0,         HDA,          GMI,          SDMMC2,       0x3338, N, N),
+	PINGROUP(dap1_din_pn1,         I2S0,         HDA,          GMI,          SDMMC2,       0x333c, N, N),
+	PINGROUP(dap1_dout_pn2,        I2S0,         HDA,          GMI,          SDMMC2,       0x3340, N, N),
+	PINGROUP(dap1_sclk_pn3,        I2S0,         HDA,          GMI,          SDMMC2,       0x3344, N, N),
+	PINGROUP(lcd_cs0_n_pn4,        DISPLAYA,     DISPLAYB,     SPI5,         RSVD4,        0x3084, N, N),
+	PINGROUP(lcd_sdout_pn5,        DISPLAYA,     DISPLAYB,     SPI5,         HDCP,         0x307c, N, N),
+	PINGROUP(lcd_dc0_pn6,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x3088, N, N),
+	PINGROUP(hdmi_int_pn7,         HDMI,         RSVD2,        RSVD3,        RSVD4,        0x3110, N, N),
+	PINGROUP(ulpi_data7_po0,       SPI2,         HSI,          UARTA,        ULPI,         0x301c, N, N),
+	PINGROUP(ulpi_data0_po1,       SPI3,         HSI,          UARTA,        ULPI,         0x3000, N, N),
+	PINGROUP(ulpi_data1_po2,       SPI3,         HSI,          UARTA,        ULPI,         0x3004, N, N),
+	PINGROUP(ulpi_data2_po3,       SPI3,         HSI,          UARTA,        ULPI,         0x3008, N, N),
+	PINGROUP(ulpi_data3_po4,       SPI3,         HSI,          UARTA,        ULPI,         0x300c, N, N),
+	PINGROUP(ulpi_data4_po5,       SPI2,         HSI,          UARTA,        ULPI,         0x3010, N, N),
+	PINGROUP(ulpi_data5_po6,       SPI2,         HSI,          UARTA,        ULPI,         0x3014, N, N),
+	PINGROUP(ulpi_data6_po7,       SPI2,         HSI,          UARTA,        ULPI,         0x3018, N, N),
+	PINGROUP(dap3_fs_pp0,          I2S2,         RSVD2,        DISPLAYA,     DISPLAYB,     0x3030, N, N),
+	PINGROUP(dap3_din_pp1,         I2S2,         RSVD2,        DISPLAYA,     DISPLAYB,     0x3034, N, N),
+	PINGROUP(dap3_dout_pp2,        I2S2,         RSVD2,        DISPLAYA,     DISPLAYB,     0x3038, N, N),
+	PINGROUP(dap3_sclk_pp3,        I2S2,         RSVD2,        DISPLAYA,     DISPLAYB,     0x303c, N, N),
+	PINGROUP(dap4_fs_pp4,          I2S3,         RSVD2,        GMI,          RSVD4,        0x31a8, N, N),
+	PINGROUP(dap4_din_pp5,         I2S3,         RSVD2,        GMI,          RSVD4,        0x31ac, N, N),
+	PINGROUP(dap4_dout_pp6,        I2S3,         RSVD2,        GMI,          RSVD4,        0x31b0, N, N),
+	PINGROUP(dap4_sclk_pp7,        I2S3,         RSVD2,        GMI,          RSVD4,        0x31b4, N, N),
+	PINGROUP(kb_col0_pq0,          KBC,          NAND,         TRACE,        TEST,         0x32fc, N, N),
+	PINGROUP(kb_col1_pq1,          KBC,          NAND,         TRACE,        TEST,         0x3300, N, N),
+	PINGROUP(kb_col2_pq2,          KBC,          NAND,         TRACE,        RSVD4,        0x3304, N, N),
+	PINGROUP(kb_col3_pq3,          KBC,          NAND,         TRACE,        RSVD4,        0x3308, N, N),
+	PINGROUP(kb_col4_pq4,          KBC,          NAND,         TRACE,        RSVD4,        0x330c, N, N),
+	PINGROUP(kb_col5_pq5,          KBC,          NAND,         TRACE,        RSVD4,        0x3310, N, N),
+	PINGROUP(kb_col6_pq6,          KBC,          NAND,         TRACE,        MIO,          0x3314, N, N),
+	PINGROUP(kb_col7_pq7,          KBC,          NAND,         TRACE,        MIO,          0x3318, N, N),
+	PINGROUP(kb_row0_pr0,          KBC,          NAND,         RSVD3,        RSVD4,        0x32bc, N, N),
+	PINGROUP(kb_row1_pr1,          KBC,          NAND,         RSVD3,        RSVD4,        0x32c0, N, N),
+	PINGROUP(kb_row2_pr2,          KBC,          NAND,         RSVD3,        RSVD4,        0x32c4, N, N),
+	PINGROUP(kb_row3_pr3,          KBC,          NAND,         RSVD3,        INVALID,      0x32c8, N, N),
+	PINGROUP(kb_row4_pr4,          KBC,          NAND,         TRACE,        RSVD4,        0x32cc, N, N),
+	PINGROUP(kb_row5_pr5,          KBC,          NAND,         TRACE,        OWR,          0x32d0, N, N),
+	PINGROUP(kb_row6_pr6,          KBC,          NAND,         SDMMC2,       MIO,          0x32d4, N, N),
+	PINGROUP(kb_row7_pr7,          KBC,          NAND,         SDMMC2,       MIO,          0x32d8, N, N),
+	PINGROUP(kb_row8_ps0,          KBC,          NAND,         SDMMC2,       MIO,          0x32dc, N, N),
+	PINGROUP(kb_row9_ps1,          KBC,          NAND,         SDMMC2,       MIO,          0x32e0, N, N),
+	PINGROUP(kb_row10_ps2,         KBC,          NAND,         SDMMC2,       MIO,          0x32e4, N, N),
+	PINGROUP(kb_row11_ps3,         KBC,          NAND,         SDMMC2,       MIO,          0x32e8, N, N),
+	PINGROUP(kb_row12_ps4,         KBC,          NAND,         SDMMC2,       MIO,          0x32ec, N, N),
+	PINGROUP(kb_row13_ps5,         KBC,          NAND,         SDMMC2,       MIO,          0x32f0, N, N),
+	PINGROUP(kb_row14_ps6,         KBC,          NAND,         SDMMC2,       MIO,          0x32f4, N, N),
+	PINGROUP(kb_row15_ps7,         KBC,          NAND,         SDMMC2,       MIO,          0x32f8, N, N),
+	PINGROUP(vi_pclk_pt0,          RSVD1,        SDMMC2,       VI,           RSVD4,        0x3154, N, Y),
+	PINGROUP(vi_mclk_pt1,          VI,           VI_ALT1,      VI_ALT2,      VI_ALT3,      0x3158, N, Y),
+	PINGROUP(vi_d10_pt2,           DDR,          RSVD2,        VI,           RSVD4,        0x314c, N, Y),
+	PINGROUP(vi_d11_pt3,           DDR,          RSVD2,        VI,           RSVD4,        0x3150, N, Y),
+	PINGROUP(vi_d0_pt4,            DDR,          RSVD2,        VI,           RSVD4,        0x3124, N, Y),
+	PINGROUP(gen2_i2c_scl_pt5,     I2C2,         HDCP,         GMI,          RSVD4,        0x3250, Y, N),
+	PINGROUP(gen2_i2c_sda_pt6,     I2C2,         HDCP,         GMI,          RSVD4,        0x3254, Y, N),
+	PINGROUP(sdmmc4_cmd_pt7,       I2C3,         NAND,         GMI,          SDMMC4,       0x325c, N, Y),
+	PINGROUP(pu0,                  OWR,          UARTA,        GMI,          RSVD4,        0x3184, N, N),
+	PINGROUP(pu1,                  RSVD1,        UARTA,        GMI,          RSVD4,        0x3188, N, N),
+	PINGROUP(pu2,                  RSVD1,        UARTA,        GMI,          RSVD4,        0x318c, N, N),
+	PINGROUP(pu3,                  PWM0,         UARTA,        GMI,          RSVD4,        0x3190, N, N),
+	PINGROUP(pu4,                  PWM1,         UARTA,        GMI,          RSVD4,        0x3194, N, N),
+	PINGROUP(pu5,                  PWM2,         UARTA,        GMI,          RSVD4,        0x3198, N, N),
+	PINGROUP(pu6,                  PWM3,         UARTA,        GMI,          RSVD4,        0x319c, N, N),
+	PINGROUP(jtag_rtck_pu7,        RTCK,         RSVD2,        RSVD3,        RSVD4,        0x32b0, N, N),
+	PINGROUP(pv0,                  RSVD1,        RSVD2,        RSVD3,        RSVD4,        0x3040, N, N),
+	PINGROUP(pv1,                  RSVD1,        RSVD2,        RSVD3,        RSVD4,        0x3044, N, N),
+	PINGROUP(pv2,                  OWR,          RSVD2,        RSVD3,        RSVD4,        0x3060, N, N),
+	PINGROUP(pv3,                  CLK_12M_OUT,  RSVD2,        RSVD3,        RSVD4,        0x3064, N, N),
+	PINGROUP(ddc_scl_pv4,          I2C4,         RSVD2,        RSVD3,        RSVD4,        0x3114, N, N),
+	PINGROUP(ddc_sda_pv5,          I2C4,         RSVD2,        RSVD3,        RSVD4,        0x3118, N, N),
+	PINGROUP(crt_hsync_pv6,        CRT,          RSVD2,        RSVD3,        RSVD4,        0x311c, N, N),
+	PINGROUP(crt_vsync_pv7,        CRT,          RSVD2,        RSVD3,        RSVD4,        0x3120, N, N),
+	PINGROUP(lcd_cs1_n_pw0,        DISPLAYA,     DISPLAYB,     SPI5,         RSVD4,        0x3104, N, N),
+	PINGROUP(lcd_m1_pw1,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x3108, N, N),
+	PINGROUP(spi2_cs1_n_pw2,       SPI3,         SPI2,         SPI2_ALT,     I2C1,         0x3388, N, N),
+	PINGROUP(spi2_cs2_n_pw3,       SPI3,         SPI2,         SPI2_ALT,     I2C1,         0x338c, N, N),
+	PINGROUP(clk1_out_pw4,         EXTPERIPH1,   RSVD2,        RSVD3,        RSVD4,        0x334c, N, N),
+	PINGROUP(clk2_out_pw5,         EXTPERIPH2,   RSVD2,        RSVD3,        RSVD4,        0x3068, N, N),
+	PINGROUP(uart3_txd_pw6,        UARTC,        RSVD2,        GMI,          RSVD4,        0x3174, N, N),
+	PINGROUP(uart3_rxd_pw7,        UARTC,        RSVD2,        GMI,          RSVD4,        0x3178, N, N),
+	PINGROUP(spi2_mosi_px0,        SPI6,         SPI2,         SPI3,         GMI,          0x3368, N, N),
+	PINGROUP(spi2_miso_px1,        SPI6,         SPI2,         SPI3,         GMI,          0x336c, N, N),
+	PINGROUP(spi2_sck_px2,         SPI6,         SPI2,         SPI3,         GMI,          0x3374, N, N),
+	PINGROUP(spi2_cs0_n_px3,       SPI6,         SPI2,         SPI3,         GMI,          0x3370, N, N),
+	PINGROUP(spi1_mosi_px4,        SPI2,         SPI1,         SPI2_ALT,     GMI,          0x3378, N, N),
+	PINGROUP(spi1_sck_px5,         SPI2,         SPI1,         SPI2_ALT,     GMI,          0x337c, N, N),
+	PINGROUP(spi1_cs0_n_px6,       SPI2,         SPI1,         SPI2_ALT,     GMI,          0x3380, N, N),
+	PINGROUP(spi1_miso_px7,        SPI3,         SPI1,         SPI2_ALT,     RSVD4,        0x3384, N, N),
+	PINGROUP(ulpi_clk_py0,         SPI1,         RSVD2,        UARTD,        ULPI,         0x3020, N, N),
+	PINGROUP(ulpi_dir_py1,         SPI1,         RSVD2,        UARTD,        ULPI,         0x3024, N, N),
+	PINGROUP(ulpi_nxt_py2,         SPI1,         RSVD2,        UARTD,        ULPI,         0x3028, N, N),
+	PINGROUP(ulpi_stp_py3,         SPI1,         RSVD2,        UARTD,        ULPI,         0x302c, N, N),
+	PINGROUP(sdmmc1_dat3_py4,      SDMMC1,       RSVD2,        UARTE,        UARTA,        0x3050, N, N),
+	PINGROUP(sdmmc1_dat2_py5,      SDMMC1,       RSVD2,        UARTE,        UARTA,        0x3054, N, N),
+	PINGROUP(sdmmc1_dat1_py6,      SDMMC1,       RSVD2,        UARTE,        UARTA,        0x3058, N, N),
+	PINGROUP(sdmmc1_dat0_py7,      SDMMC1,       RSVD2,        UARTE,        UARTA,        0x305c, N, N),
+	PINGROUP(sdmmc1_clk_pz0,       SDMMC1,       RSVD2,        RSVD3,        UARTA,        0x3048, N, N),
+	PINGROUP(sdmmc1_cmd_pz1,       SDMMC1,       RSVD2,        RSVD3,        UARTA,        0x304c, N, N),
+	PINGROUP(lcd_sdin_pz2,         DISPLAYA,     DISPLAYB,     SPI5,         RSVD4,        0x3078, N, N),
+	PINGROUP(lcd_wr_n_pz3,         DISPLAYA,     DISPLAYB,     SPI5,         HDCP,         0x3080, N, N),
+	PINGROUP(lcd_sck_pz4,          DISPLAYA,     DISPLAYB,     SPI5,         HDCP,         0x308c, N, N),
+	PINGROUP(sys_clk_req_pz5,      SYSCLK,       RSVD2,        RSVD3,        RSVD4,        0x3320, N, N),
+	PINGROUP(pwr_i2c_scl_pz6,      I2CPWR,       RSVD2,        RSVD3,        RSVD4,        0x32b4, Y, N),
+	PINGROUP(pwr_i2c_sda_pz7,      I2CPWR,       RSVD2,        RSVD3,        RSVD4,        0x32b8, Y, N),
+	PINGROUP(sdmmc4_dat0_paa0,     UARTE,        SPI3,         GMI,          SDMMC4,       0x3260, N, Y),
+	PINGROUP(sdmmc4_dat1_paa1,     UARTE,        SPI3,         GMI,          SDMMC4,       0x3264, N, Y),
+	PINGROUP(sdmmc4_dat2_paa2,     UARTE,        SPI3,         GMI,          SDMMC4,       0x3268, N, Y),
+	PINGROUP(sdmmc4_dat3_paa3,     UARTE,        SPI3,         GMI,          SDMMC4,       0x326c, N, Y),
+	PINGROUP(sdmmc4_dat4_paa4,     I2C3,         I2S4,         GMI,          SDMMC4,       0x3270, N, Y),
+	PINGROUP(sdmmc4_dat5_paa5,     VGP3,         I2S4,         GMI,          SDMMC4,       0x3274, N, Y),
+	PINGROUP(sdmmc4_dat6_paa6,     VGP4,         I2S4,         GMI,          SDMMC4,       0x3278, N, Y),
+	PINGROUP(sdmmc4_dat7_paa7,     VGP5,         I2S4,         GMI,          SDMMC4,       0x327c, N, Y),
+	PINGROUP(pbb0,                 I2S4,         RSVD2,        RSVD3,        SDMMC4,       0x328c, N, N),
+	PINGROUP(cam_i2c_scl_pbb1,     VGP1,         I2C3,         RSVD3,        SDMMC4,       0x3290, Y, N),
+	PINGROUP(cam_i2c_sda_pbb2,     VGP2,         I2C3,         RSVD3,        SDMMC4,       0x3294, Y, N),
+	PINGROUP(pbb3,                 VGP3,         DISPLAYA,     DISPLAYB,     SDMMC4,       0x3298, N, N),
+	PINGROUP(pbb4,                 VGP4,         DISPLAYA,     DISPLAYB,     SDMMC4,       0x329c, N, N),
+	PINGROUP(pbb5,                 VGP5,         DISPLAYA,     DISPLAYB,     SDMMC4,       0x32a0, N, N),
+	PINGROUP(pbb6,                 VGP6,         DISPLAYA,     DISPLAYB,     SDMMC4,       0x32a4, N, N),
+	PINGROUP(pbb7,                 I2S4,         RSVD2,        RSVD3,        SDMMC4,       0x32a8, N, N),
+	PINGROUP(cam_mclk_pcc0,        VI,           VI_ALT1,      VI_ALT3,      SDMMC4,       0x3284, N, N),
+	PINGROUP(pcc1,                 I2S4,         RSVD2,        RSVD3,        SDMMC4,       0x3288, N, N),
+	PINGROUP(pcc2,                 I2S4,         RSVD2,        RSVD3,        RSVD4,        0x32ac, N, N),
+	PINGROUP(sdmmc4_rst_n_pcc3,    VGP6,         RSVD2,        RSVD3,        SDMMC4,       0x3280, N, Y),
+	PINGROUP(sdmmc4_clk_pcc4,      INVALID,      NAND,         GMI,          SDMMC4,       0x3258, N, Y),
+	PINGROUP(clk2_req_pcc5,        DAP,          RSVD2,        RSVD3,        RSVD4,        0x306c, N, N),
+	PINGROUP(pex_l2_rst_n_pcc6,    PCIE,         HDA,          RSVD3,        RSVD4,        0x33d8, N, N),
+	PINGROUP(pex_l2_clkreq_n_pcc7, PCIE,         HDA,          RSVD3,        RSVD4,        0x33dc, N, N),
+	PINGROUP(pex_l0_prsnt_n_pdd0,  PCIE,         HDA,          RSVD3,        RSVD4,        0x33b8, N, N),
+	PINGROUP(pex_l0_rst_n_pdd1,    PCIE,         HDA,          RSVD3,        RSVD4,        0x33bc, N, N),
+	PINGROUP(pex_l0_clkreq_n_pdd2, PCIE,         HDA,          RSVD3,        RSVD4,        0x33c0, N, N),
+	PINGROUP(pex_wake_n_pdd3,      PCIE,         HDA,          RSVD3,        RSVD4,        0x33c4, N, N),
+	PINGROUP(pex_l1_prsnt_n_pdd4,  PCIE,         HDA,          RSVD3,        RSVD4,        0x33c8, N, N),
+	PINGROUP(pex_l1_rst_n_pdd5,    PCIE,         HDA,          RSVD3,        RSVD4,        0x33cc, N, N),
+	PINGROUP(pex_l1_clkreq_n_pdd6, PCIE,         HDA,          RSVD3,        RSVD4,        0x33d0, N, N),
+	PINGROUP(pex_l2_prsnt_n_pdd7,  PCIE,         HDA,          RSVD3,        RSVD4,        0x33d4, N, N),
+	PINGROUP(clk3_out_pee0,        EXTPERIPH3,   RSVD2,        RSVD3,        RSVD4,        0x31b8, N, N),
+	PINGROUP(clk3_req_pee1,        DEV3,         RSVD2,        RSVD3,        RSVD4,        0x31bc, N, N),
+	PINGROUP(clk1_req_pee2,        DAP,          HDA,          RSVD3,        RSVD4,        0x3348, N, N),
+	PINGROUP(hdmi_cec_pee3,        CEC,          RSVD2,        RSVD3,        RSVD4,        0x33e0, Y, N),
+	PINGROUP(clk_32k_in,           CLK_32K_IN,   RSVD2,        RSVD3,        RSVD4,        0x3330, N, N),
+	PINGROUP(core_pwr_req,         CORE_PWR_REQ, RSVD2,        RSVD3,        RSVD4,        0x3324, N, N),
+	PINGROUP(cpu_pwr_req,          CPU_PWR_REQ,  RSVD2,        RSVD3,        RSVD4,        0x3328, N, N),
+	PINGROUP(owr,                  OWR,          CEC,          RSVD3,        RSVD4,        0x3334, N, N),
+	PINGROUP(pwr_int_n,            PWR_INT_N,    RSVD2,        RSVD3,        RSVD4,        0x332c, N, N),
 	/* pg_name, r, hsm_b, schmitt_b, lpmd_b, drvdn_b, drvdn_w, drvup_b, drvup_w, slwr_b, slwr_w, slwf_b, slwf_w */
 	DRV_PINGROUP(ao1,   0x868,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2),
 	DRV_PINGROUP(ao2,   0x86c,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2),
diff --git a/drivers/pinctrl/pinmux.c b/drivers/pinctrl/pinmux.c
index 9248ce4efed..051e8592990 100644
--- a/drivers/pinctrl/pinmux.c
+++ b/drivers/pinctrl/pinmux.c
@@ -391,14 +391,16 @@ int pinmux_enable_setting(struct pinctrl_setting const *setting)
 	struct pinctrl_dev *pctldev = setting->pctldev;
 	const struct pinctrl_ops *pctlops = pctldev->desc->pctlops;
 	const struct pinmux_ops *ops = pctldev->desc->pmxops;
-	int ret;
-	const unsigned *pins;
-	unsigned num_pins;
+	int ret = 0;
+	const unsigned *pins = NULL;
+	unsigned num_pins = 0;
 	int i;
 	struct pin_desc *desc;
 
-	ret = pctlops->get_group_pins(pctldev, setting->data.mux.group,
-				      &pins, &num_pins);
+	if (pctlops->get_group_pins)
+		ret = pctlops->get_group_pins(pctldev, setting->data.mux.group,
+					      &pins, &num_pins);
+
 	if (ret) {
 		const char *gname;
 
@@ -470,14 +472,15 @@ void pinmux_disable_setting(struct pinctrl_setting const *setting)
 	struct pinctrl_dev *pctldev = setting->pctldev;
 	const struct pinctrl_ops *pctlops = pctldev->desc->pctlops;
 	const struct pinmux_ops *ops = pctldev->desc->pmxops;
-	int ret;
-	const unsigned *pins;
-	unsigned num_pins;
+	int ret = 0;
+	const unsigned *pins = NULL;
+	unsigned num_pins = 0;
 	int i;
 	struct pin_desc *desc;
 
-	ret = pctlops->get_group_pins(pctldev, setting->data.mux.group,
-				      &pins, &num_pins);
+	if (pctlops->get_group_pins)
+		ret = pctlops->get_group_pins(pctldev, setting->data.mux.group,
+					      &pins, &num_pins);
 	if (ret) {
 		const char *gname;
 
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a73a4.c b/drivers/pinctrl/sh-pfc/pfc-r8a73a4.c
index d39ca87353e..ce9fb7aa8ba 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a73a4.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a73a4.c
@@ -20,7 +20,10 @@
 #include <linux/io.h>
 #include <linux/kernel.h>
 #include <linux/pinctrl/pinconf-generic.h>
+
+#ifndef CONFIG_ARCH_MULTIPLATFORM
 #include <mach/irqs.h>
+#endif
 
 #include "core.h"
 #include "sh_pfc.h"
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7740.c b/drivers/pinctrl/sh-pfc/pfc-r8a7740.c
index 6c83ce43a94..e4c1ef47705 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7740.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7740.c
@@ -22,7 +22,9 @@
 #include <linux/kernel.h>
 #include <linux/pinctrl/pinconf-generic.h>
 
+#ifndef CONFIG_ARCH_MULTIPLATFORM
 #include <mach/irqs.h>
+#endif
 
 #include "core.h"
 #include "sh_pfc.h"
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7790.c b/drivers/pinctrl/sh-pfc/pfc-r8a7790.c
index f5cd3f96180..9a179c94b4d 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7790.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7790.c
@@ -782,7 +782,8 @@ enum {
 	USB1_PWEN_MARK, AUDIO_CLKOUT_D_MARK, USB1_OVC_MARK,
 	TCLK1_B_MARK,
 
-	I2C3_SCL_MARK, I2C3_SDA_MARK,
+	IIC0_SCL_MARK, IIC0_SDA_MARK, I2C0_SCL_MARK, I2C0_SDA_MARK,
+	IIC3_SCL_MARK, IIC3_SDA_MARK, I2C3_SCL_MARK, I2C3_SDA_MARK,
 	PINMUX_MARK_END,
 };
 
@@ -1722,6 +1723,13 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_DATA(IP16_7, USB1_OVC),
 	PINMUX_IPSR_MODSEL_DATA(IP16_7, TCLK1_B, SEL_TMU1_1),
 
+	PINMUX_DATA(IIC0_SCL_MARK, FN_SEL_IIC0_0),
+	PINMUX_DATA(IIC0_SDA_MARK, FN_SEL_IIC0_0),
+	PINMUX_DATA(I2C0_SCL_MARK, FN_SEL_IIC0_1),
+	PINMUX_DATA(I2C0_SDA_MARK, FN_SEL_IIC0_1),
+
+	PINMUX_DATA(IIC3_SCL_MARK, FN_SEL_IICDVFS_0),
+	PINMUX_DATA(IIC3_SDA_MARK, FN_SEL_IICDVFS_0),
 	PINMUX_DATA(I2C3_SCL_MARK, FN_SEL_IICDVFS_1),
 	PINMUX_DATA(I2C3_SDA_MARK, FN_SEL_IICDVFS_1),
 };
@@ -1735,8 +1743,10 @@ static const struct sh_pfc_pin pinmux_pins[] = {
 	PINMUX_GPIO_GP_ALL(),
 
 	/* Pins not associated with a GPIO port */
-	SH_PFC_PIN_NAMED(ROW_GROUP_A('J'), 15, AJ15),
+	SH_PFC_PIN_NAMED(ROW_GROUP_A('F'), 15, AF15),
+	SH_PFC_PIN_NAMED(ROW_GROUP_A('G'), 15, AG15),
 	SH_PFC_PIN_NAMED(ROW_GROUP_A('H'), 15, AH15),
+	SH_PFC_PIN_NAMED(ROW_GROUP_A('J'), 15, AJ15),
 };
 
 /* - AUDIO CLOCK ------------------------------------------------------------ */
@@ -2054,6 +2064,14 @@ static const unsigned int hscif1_ctrl_b_pins[] = {
 static const unsigned int hscif1_ctrl_b_mux[] = {
 	HRTS1_N_B_MARK, HCTS1_N_B_MARK,
 };
+/* - I2C0 ------------------------------------------------------------------- */
+static const unsigned int i2c0_pins[] = {
+	/* SCL, SDA */
+	PIN_A_NUMBER('G', 15), PIN_A_NUMBER('F', 15),
+};
+static const unsigned int i2c0_mux[] = {
+	I2C0_SCL_MARK, I2C0_SDA_MARK,
+};
 /* - I2C1 ------------------------------------------------------------------- */
 static const unsigned int i2c1_pins[] = {
 	/* SCL, SDA */
@@ -2120,6 +2138,80 @@ static const unsigned int i2c3_pins[] = {
 static const unsigned int i2c3_mux[] = {
 	I2C3_SCL_MARK, I2C3_SDA_MARK,
 };
+/* - IIC0 (I2C4) ------------------------------------------------------------ */
+static const unsigned int iic0_pins[] = {
+	/* SCL, SDA */
+	PIN_A_NUMBER('G', 15), PIN_A_NUMBER('F', 15),
+};
+static const unsigned int iic0_mux[] = {
+	IIC0_SCL_MARK, IIC0_SDA_MARK,
+};
+/* - IIC1 (I2C5) ------------------------------------------------------------ */
+static const unsigned int iic1_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(1, 16), RCAR_GP_PIN(1, 17),
+};
+static const unsigned int iic1_mux[] = {
+	IIC1_SCL_MARK, IIC1_SDA_MARK,
+};
+static const unsigned int iic1_b_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(3, 6), RCAR_GP_PIN(3, 7),
+};
+static const unsigned int iic1_b_mux[] = {
+	IIC1_SCL_B_MARK, IIC1_SDA_B_MARK,
+};
+static const unsigned int iic1_c_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(4, 30), RCAR_GP_PIN(4, 27),
+};
+static const unsigned int iic1_c_mux[] = {
+	IIC1_SCL_C_MARK, IIC1_SDA_C_MARK,
+};
+/* - IIC2 (I2C6) ------------------------------------------------------------ */
+static const unsigned int iic2_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(5, 5), RCAR_GP_PIN(5, 6),
+};
+static const unsigned int iic2_mux[] = {
+	IIC2_SCL_MARK, IIC2_SDA_MARK,
+};
+static const unsigned int iic2_b_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(4, 0), RCAR_GP_PIN(4, 1),
+};
+static const unsigned int iic2_b_mux[] = {
+	IIC2_SCL_B_MARK, IIC2_SDA_B_MARK,
+};
+static const unsigned int iic2_c_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(0, 6), RCAR_GP_PIN(0, 7),
+};
+static const unsigned int iic2_c_mux[] = {
+	IIC2_SCL_C_MARK, IIC2_SDA_C_MARK,
+};
+static const unsigned int iic2_d_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(3, 14), RCAR_GP_PIN(3, 15),
+};
+static const unsigned int iic2_d_mux[] = {
+	IIC2_SCL_D_MARK, IIC2_SDA_D_MARK,
+};
+static const unsigned int iic2_e_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(2, 18), RCAR_GP_PIN(2, 19),
+};
+static const unsigned int iic2_e_mux[] = {
+	IIC2_SCL_E_MARK, IIC2_SDA_E_MARK,
+};
+/* - IIC3 (I2C7) ------------------------------------------------------------ */
+static const unsigned int iic3_pins[] = {
+/* SCL, SDA */
+	PIN_A_NUMBER('J', 15), PIN_A_NUMBER('H', 15),
+};
+static const unsigned int iic3_mux[] = {
+	IIC3_SCL_MARK, IIC3_SDA_MARK,
+};
 /* - INTC ------------------------------------------------------------------- */
 static const unsigned int intc_irq0_pins[] = {
 	/* IRQ */
@@ -3757,6 +3849,7 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(hscif1_data_b),
 	SH_PFC_PIN_GROUP(hscif1_clk_b),
 	SH_PFC_PIN_GROUP(hscif1_ctrl_b),
+	SH_PFC_PIN_GROUP(i2c0),
 	SH_PFC_PIN_GROUP(i2c1),
 	SH_PFC_PIN_GROUP(i2c1_b),
 	SH_PFC_PIN_GROUP(i2c1_c),
@@ -3766,6 +3859,16 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(i2c2_d),
 	SH_PFC_PIN_GROUP(i2c2_e),
 	SH_PFC_PIN_GROUP(i2c3),
+	SH_PFC_PIN_GROUP(iic0),
+	SH_PFC_PIN_GROUP(iic1),
+	SH_PFC_PIN_GROUP(iic1_b),
+	SH_PFC_PIN_GROUP(iic1_c),
+	SH_PFC_PIN_GROUP(iic2),
+	SH_PFC_PIN_GROUP(iic2_b),
+	SH_PFC_PIN_GROUP(iic2_c),
+	SH_PFC_PIN_GROUP(iic2_d),
+	SH_PFC_PIN_GROUP(iic2_e),
+	SH_PFC_PIN_GROUP(iic3),
 	SH_PFC_PIN_GROUP(intc_irq0),
 	SH_PFC_PIN_GROUP(intc_irq1),
 	SH_PFC_PIN_GROUP(intc_irq2),
@@ -4044,6 +4147,10 @@ static const char * const hscif1_groups[] = {
 	"hscif1_ctrl_b",
 };
 
+static const char * const i2c0_groups[] = {
+	"i2c0",
+};
+
 static const char * const i2c1_groups[] = {
 	"i2c1",
 	"i2c1_b",
@@ -4062,6 +4169,28 @@ static const char * const i2c3_groups[] = {
 	"i2c3",
 };
 
+static const char * const iic0_groups[] = {
+	"iic0",
+};
+
+static const char * const iic1_groups[] = {
+	"iic1",
+	"iic1_b",
+	"iic1_c",
+};
+
+static const char * const iic2_groups[] = {
+	"iic2",
+	"iic2_b",
+	"iic2_c",
+	"iic2_d",
+	"iic2_e",
+};
+
+static const char * const iic3_groups[] = {
+	"iic3",
+};
+
 static const char * const intc_groups[] = {
 	"intc_irq0",
 	"intc_irq1",
@@ -4373,9 +4502,14 @@ static const struct sh_pfc_function pinmux_functions[] = {
 	SH_PFC_FUNCTION(eth),
 	SH_PFC_FUNCTION(hscif0),
 	SH_PFC_FUNCTION(hscif1),
+	SH_PFC_FUNCTION(i2c0),
 	SH_PFC_FUNCTION(i2c1),
 	SH_PFC_FUNCTION(i2c2),
 	SH_PFC_FUNCTION(i2c3),
+	SH_PFC_FUNCTION(iic0),
+	SH_PFC_FUNCTION(iic1),
+	SH_PFC_FUNCTION(iic2),
+	SH_PFC_FUNCTION(iic3),
 	SH_PFC_FUNCTION(intc),
 	SH_PFC_FUNCTION(mmc0),
 	SH_PFC_FUNCTION(mmc1),
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7791.c b/drivers/pinctrl/sh-pfc/pfc-r8a7791.c
index 7868bf3a0f9..2e688dc4a3c 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7791.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7791.c
@@ -1680,6 +1680,53 @@ static const struct sh_pfc_pin pinmux_pins[] = {
 	PINMUX_GPIO_GP_ALL(),
 };
 
+/* - Audio Clock ------------------------------------------------------------ */
+static const unsigned int audio_clk_a_pins[] = {
+	/* CLK */
+	RCAR_GP_PIN(2, 28),
+};
+
+static const unsigned int audio_clk_a_mux[] = {
+	AUDIO_CLKA_MARK,
+};
+
+static const unsigned int audio_clk_b_pins[] = {
+	/* CLK */
+	RCAR_GP_PIN(2, 29),
+};
+
+static const unsigned int audio_clk_b_mux[] = {
+	AUDIO_CLKB_MARK,
+};
+
+static const unsigned int audio_clk_b_b_pins[] = {
+	/* CLK */
+	RCAR_GP_PIN(7, 20),
+};
+
+static const unsigned int audio_clk_b_b_mux[] = {
+	AUDIO_CLKB_B_MARK,
+};
+
+static const unsigned int audio_clk_c_pins[] = {
+	/* CLK */
+	RCAR_GP_PIN(2, 30),
+};
+
+static const unsigned int audio_clk_c_mux[] = {
+	AUDIO_CLKC_MARK,
+};
+
+static const unsigned int audio_clkout_pins[] = {
+	/* CLK */
+	RCAR_GP_PIN(2, 31),
+};
+
+static const unsigned int audio_clkout_mux[] = {
+	AUDIO_CLKOUT_MARK,
+};
+
+
 /* - DU --------------------------------------------------------------------- */
 static const unsigned int du_rgb666_pins[] = {
 	/* R[7:2], G[7:2], B[7:2] */
@@ -1733,19 +1780,32 @@ static const unsigned int du_clk_out_1_mux[] = {
 	DU1_DOTCLKOUT1_MARK
 };
 static const unsigned int du_sync_pins[] = {
-	/* EXVSYNC/VSYNC, EXHSYNC/HSYNC, EXDISP/EXODDF/EXCDE */
-	RCAR_GP_PIN(3, 29), RCAR_GP_PIN(3, 28), RCAR_GP_PIN(3, 27),
+	/* EXVSYNC/VSYNC, EXHSYNC/HSYNC */
+	RCAR_GP_PIN(3, 28), RCAR_GP_PIN(3, 27),
 };
 static const unsigned int du_sync_mux[] = {
-	DU1_EXODDF_DU1_ODDF_DISP_CDE_MARK,
 	DU1_EXVSYNC_DU1_VSYNC_MARK, DU1_EXHSYNC_DU1_HSYNC_MARK
 };
-static const unsigned int du_cde_disp_pins[] = {
-	/* CDE DISP */
-	RCAR_GP_PIN(3, 31), RCAR_GP_PIN(3, 30),
+static const unsigned int du_oddf_pins[] = {
+	/* EXDISP/EXODDF/EXCDE */
+	RCAR_GP_PIN(3, 29),
+};
+static const unsigned int du_oddf_mux[] = {
+	DU1_EXODDF_DU1_ODDF_DISP_CDE_MARK,
+};
+static const unsigned int du_cde_pins[] = {
+	/* CDE */
+	RCAR_GP_PIN(3, 31),
 };
-static const unsigned int du_cde_disp_mux[] = {
-	DU1_CDE_MARK, DU1_DISP_MARK
+static const unsigned int du_cde_mux[] = {
+	DU1_CDE_MARK,
+};
+static const unsigned int du_disp_pins[] = {
+	/* DISP */
+	RCAR_GP_PIN(3, 30),
+};
+static const unsigned int du_disp_mux[] = {
+	DU1_DISP_MARK,
 };
 static const unsigned int du0_clk_in_pins[] = {
 	/* CLKIN */
@@ -3246,6 +3306,260 @@ static const unsigned int sdhi2_wp_pins[] = {
 static const unsigned int sdhi2_wp_mux[] = {
 	SD2_WP_MARK,
 };
+
+/* - SSI -------------------------------------------------------------------- */
+static const unsigned int ssi0_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(2, 2),
+};
+
+static const unsigned int ssi0_data_mux[] = {
+	SSI_SDATA0_MARK,
+};
+
+static const unsigned int ssi0_data_b_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(3, 4),
+};
+
+static const unsigned int ssi0_data_b_mux[] = {
+	SSI_SDATA0_B_MARK,
+};
+
+static const unsigned int ssi0129_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(2, 0), RCAR_GP_PIN(2, 1),
+};
+
+static const unsigned int ssi0129_ctrl_mux[] = {
+	SSI_SCK0129_MARK, SSI_WS0129_MARK,
+};
+
+static const unsigned int ssi0129_ctrl_b_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(3, 2), RCAR_GP_PIN(3, 3),
+};
+
+static const unsigned int ssi0129_ctrl_b_mux[] = {
+	SSI_SCK0129_B_MARK, SSI_WS0129_B_MARK,
+};
+
+static const unsigned int ssi1_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(2, 5),
+};
+
+static const unsigned int ssi1_data_mux[] = {
+	SSI_SDATA1_MARK,
+};
+
+static const unsigned int ssi1_data_b_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(3, 7),
+};
+
+static const unsigned int ssi1_data_b_mux[] = {
+	SSI_SDATA1_B_MARK,
+};
+
+static const unsigned int ssi1_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(2, 3), RCAR_GP_PIN(2, 4),
+};
+
+static const unsigned int ssi1_ctrl_mux[] = {
+	SSI_SCK1_MARK, SSI_WS1_MARK,
+};
+
+static const unsigned int ssi1_ctrl_b_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(3, 5), RCAR_GP_PIN(3, 6),
+};
+
+static const unsigned int ssi1_ctrl_b_mux[] = {
+	SSI_SCK1_B_MARK, SSI_WS1_B_MARK,
+};
+
+static const unsigned int ssi2_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(2, 8),
+};
+
+static const unsigned int ssi2_data_mux[] = {
+	SSI_SDATA2_MARK,
+};
+
+static const unsigned int ssi2_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(2, 6), RCAR_GP_PIN(2, 7),
+};
+
+static const unsigned int ssi2_ctrl_mux[] = {
+	SSI_SCK2_MARK, SSI_WS2_MARK,
+};
+
+static const unsigned int ssi3_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(2, 11),
+};
+
+static const unsigned int ssi3_data_mux[] = {
+	SSI_SDATA3_MARK,
+};
+
+static const unsigned int ssi34_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(2, 9), RCAR_GP_PIN(2, 10),
+};
+
+static const unsigned int ssi34_ctrl_mux[] = {
+	SSI_SCK34_MARK, SSI_WS34_MARK,
+};
+
+static const unsigned int ssi4_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(2, 14),
+};
+
+static const unsigned int ssi4_data_mux[] = {
+	SSI_SDATA4_MARK,
+};
+
+static const unsigned int ssi4_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(2, 12), RCAR_GP_PIN(2, 13),
+};
+
+static const unsigned int ssi4_ctrl_mux[] = {
+	SSI_SCK4_MARK, SSI_WS4_MARK,
+};
+
+static const unsigned int ssi5_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(2, 17),
+};
+
+static const unsigned int ssi5_data_mux[] = {
+	SSI_SDATA5_MARK,
+};
+
+static const unsigned int ssi5_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(2, 15), RCAR_GP_PIN(2, 16),
+};
+
+static const unsigned int ssi5_ctrl_mux[] = {
+	SSI_SCK5_MARK, SSI_WS5_MARK,
+};
+
+static const unsigned int ssi6_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(2, 20),
+};
+
+static const unsigned int ssi6_data_mux[] = {
+	SSI_SDATA6_MARK,
+};
+
+static const unsigned int ssi6_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(2, 18), RCAR_GP_PIN(2, 19),
+};
+
+static const unsigned int ssi6_ctrl_mux[] = {
+	SSI_SCK6_MARK, SSI_WS6_MARK,
+};
+
+static const unsigned int ssi7_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(2, 23),
+};
+
+static const unsigned int ssi7_data_mux[] = {
+	SSI_SDATA7_MARK,
+};
+
+static const unsigned int ssi7_data_b_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(3, 12),
+};
+
+static const unsigned int ssi7_data_b_mux[] = {
+	SSI_SDATA7_B_MARK,
+};
+
+static const unsigned int ssi78_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(2, 21), RCAR_GP_PIN(2, 22),
+};
+
+static const unsigned int ssi78_ctrl_mux[] = {
+	SSI_SCK78_MARK, SSI_WS78_MARK,
+};
+
+static const unsigned int ssi78_ctrl_b_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(3, 10), RCAR_GP_PIN(3, 11),
+};
+
+static const unsigned int ssi78_ctrl_b_mux[] = {
+	SSI_SCK78_B_MARK, SSI_WS78_B_MARK,
+};
+
+static const unsigned int ssi8_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(2, 24),
+};
+
+static const unsigned int ssi8_data_mux[] = {
+	SSI_SDATA8_MARK,
+};
+
+static const unsigned int ssi8_data_b_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(3, 13),
+};
+
+static const unsigned int ssi8_data_b_mux[] = {
+	SSI_SDATA8_B_MARK,
+};
+
+static const unsigned int ssi9_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(2, 27),
+};
+
+static const unsigned int ssi9_data_mux[] = {
+	SSI_SDATA9_MARK,
+};
+
+static const unsigned int ssi9_data_b_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(3, 18),
+};
+
+static const unsigned int ssi9_data_b_mux[] = {
+	SSI_SDATA9_B_MARK,
+};
+
+static const unsigned int ssi9_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(2, 25), RCAR_GP_PIN(2, 26),
+};
+
+static const unsigned int ssi9_ctrl_mux[] = {
+	SSI_SCK9_MARK, SSI_WS9_MARK,
+};
+
+static const unsigned int ssi9_ctrl_b_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(3, 14), RCAR_GP_PIN(3, 15),
+};
+
+static const unsigned int ssi9_ctrl_b_mux[] = {
+	SSI_SCK9_B_MARK, SSI_WS9_B_MARK,
+};
+
 /* - USB0 ------------------------------------------------------------------- */
 static const unsigned int usb0_pins[] = {
 	RCAR_GP_PIN(7, 23), /* PWEN */
@@ -3550,12 +3864,19 @@ static const unsigned int vin2_clk_mux[] = {
 };
 
 static const struct sh_pfc_pin_group pinmux_groups[] = {
+	SH_PFC_PIN_GROUP(audio_clk_a),
+	SH_PFC_PIN_GROUP(audio_clk_b),
+	SH_PFC_PIN_GROUP(audio_clk_b_b),
+	SH_PFC_PIN_GROUP(audio_clk_c),
+	SH_PFC_PIN_GROUP(audio_clkout),
 	SH_PFC_PIN_GROUP(du_rgb666),
 	SH_PFC_PIN_GROUP(du_rgb888),
 	SH_PFC_PIN_GROUP(du_clk_out_0),
 	SH_PFC_PIN_GROUP(du_clk_out_1),
 	SH_PFC_PIN_GROUP(du_sync),
-	SH_PFC_PIN_GROUP(du_cde_disp),
+	SH_PFC_PIN_GROUP(du_oddf),
+	SH_PFC_PIN_GROUP(du_cde),
+	SH_PFC_PIN_GROUP(du_disp),
 	SH_PFC_PIN_GROUP(du0_clk_in),
 	SH_PFC_PIN_GROUP(du1_clk_in),
 	SH_PFC_PIN_GROUP(du1_clk_in_b),
@@ -3762,6 +4083,34 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(sdhi2_ctrl),
 	SH_PFC_PIN_GROUP(sdhi2_cd),
 	SH_PFC_PIN_GROUP(sdhi2_wp),
+	SH_PFC_PIN_GROUP(ssi0_data),
+	SH_PFC_PIN_GROUP(ssi0_data_b),
+	SH_PFC_PIN_GROUP(ssi0129_ctrl),
+	SH_PFC_PIN_GROUP(ssi0129_ctrl_b),
+	SH_PFC_PIN_GROUP(ssi1_data),
+	SH_PFC_PIN_GROUP(ssi1_data_b),
+	SH_PFC_PIN_GROUP(ssi1_ctrl),
+	SH_PFC_PIN_GROUP(ssi1_ctrl_b),
+	SH_PFC_PIN_GROUP(ssi2_data),
+	SH_PFC_PIN_GROUP(ssi2_ctrl),
+	SH_PFC_PIN_GROUP(ssi3_data),
+	SH_PFC_PIN_GROUP(ssi34_ctrl),
+	SH_PFC_PIN_GROUP(ssi4_data),
+	SH_PFC_PIN_GROUP(ssi4_ctrl),
+	SH_PFC_PIN_GROUP(ssi5_data),
+	SH_PFC_PIN_GROUP(ssi5_ctrl),
+	SH_PFC_PIN_GROUP(ssi6_data),
+	SH_PFC_PIN_GROUP(ssi6_ctrl),
+	SH_PFC_PIN_GROUP(ssi7_data),
+	SH_PFC_PIN_GROUP(ssi7_data_b),
+	SH_PFC_PIN_GROUP(ssi78_ctrl),
+	SH_PFC_PIN_GROUP(ssi78_ctrl_b),
+	SH_PFC_PIN_GROUP(ssi8_data),
+	SH_PFC_PIN_GROUP(ssi8_data_b),
+	SH_PFC_PIN_GROUP(ssi9_data),
+	SH_PFC_PIN_GROUP(ssi9_data_b),
+	SH_PFC_PIN_GROUP(ssi9_ctrl),
+	SH_PFC_PIN_GROUP(ssi9_ctrl_b),
 	SH_PFC_PIN_GROUP(usb0),
 	SH_PFC_PIN_GROUP(usb1),
 	VIN_DATA_PIN_GROUP(vin0_data, 24),
@@ -3798,13 +4147,23 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(vin2_clk),
 };
 
+static const char * const audio_clk_groups[] = {
+	"audio_clk_a",
+	"audio_clk_b",
+	"audio_clk_b_b",
+	"audio_clk_c",
+	"audio_clkout",
+};
+
 static const char * const du_groups[] = {
 	"du_rgb666",
 	"du_rgb888",
 	"du_clk_out_0",
 	"du_clk_out_1",
 	"du_sync",
-	"du_cde_disp",
+	"du_oddf",
+	"du_cde",
+	"du_disp",
 };
 
 static const char * const du0_groups[] = {
@@ -4103,6 +4462,37 @@ static const char * const sdhi2_groups[] = {
 	"sdhi2_wp",
 };
 
+static const char * const ssi_groups[] = {
+	"ssi0_data",
+	"ssi0_data_b",
+	"ssi0129_ctrl",
+	"ssi0129_ctrl_b",
+	"ssi1_data",
+	"ssi1_data_b",
+	"ssi1_ctrl",
+	"ssi1_ctrl_b",
+	"ssi2_data",
+	"ssi2_ctrl",
+	"ssi3_data",
+	"ssi34_ctrl",
+	"ssi4_data",
+	"ssi4_ctrl",
+	"ssi5_data",
+	"ssi5_ctrl",
+	"ssi6_data",
+	"ssi6_ctrl",
+	"ssi7_data",
+	"ssi7_data_b",
+	"ssi78_ctrl",
+	"ssi78_ctrl_b",
+	"ssi8_data",
+	"ssi8_data_b",
+	"ssi9_data",
+	"ssi9_data_b",
+	"ssi9_ctrl",
+	"ssi9_ctrl_b",
+};
+
 static const char * const usb0_groups[] = {
 	"usb0",
 };
@@ -4152,6 +4542,7 @@ static const char * const vin2_groups[] = {
 };
 
 static const struct sh_pfc_function pinmux_functions[] = {
+	SH_PFC_FUNCTION(audio_clk),
 	SH_PFC_FUNCTION(du),
 	SH_PFC_FUNCTION(du0),
 	SH_PFC_FUNCTION(du1),
@@ -4187,6 +4578,7 @@ static const struct sh_pfc_function pinmux_functions[] = {
 	SH_PFC_FUNCTION(sdhi0),
 	SH_PFC_FUNCTION(sdhi1),
 	SH_PFC_FUNCTION(sdhi2),
+	SH_PFC_FUNCTION(ssi),
 	SH_PFC_FUNCTION(usb0),
 	SH_PFC_FUNCTION(usb1),
 	SH_PFC_FUNCTION(vin0),
diff --git a/drivers/pinctrl/sh-pfc/pfc-sh73a0.c b/drivers/pinctrl/sh-pfc/pfc-sh73a0.c
index 6f6ba100994..ee370de4609 100644
--- a/drivers/pinctrl/sh-pfc/pfc-sh73a0.c
+++ b/drivers/pinctrl/sh-pfc/pfc-sh73a0.c
@@ -26,7 +26,9 @@
 #include <linux/regulator/machine.h>
 #include <linux/slab.h>
 
+#ifndef CONFIG_ARCH_MULTIPLATFORM
 #include <mach/irqs.h>
+#endif
 
 #include "core.h"
 #include "sh_pfc.h"
diff --git a/drivers/pinctrl/sh-pfc/sh_pfc.h b/drivers/pinctrl/sh-pfc/sh_pfc.h
index ab8fd258d9e..d482c40b012 100644
--- a/drivers/pinctrl/sh-pfc/sh_pfc.h
+++ b/drivers/pinctrl/sh-pfc/sh_pfc.h
@@ -98,8 +98,13 @@ struct pinmux_irq {
 	const short *gpios;
 };
 
+#ifdef CONFIG_ARCH_MULTIPLATFORM
+#define PINMUX_IRQ(irq_nr, ids...)			   \
+	{ .gpios = (const short []) { ids, -1 } }
+#else
 #define PINMUX_IRQ(irq_nr, ids...)			   \
 	{ .irq = irq_nr, .gpios = (const short []) { ids, -1 } }
+#endif
 
 struct pinmux_range {
 	u16 begin;
diff --git a/drivers/pinctrl/sirf/pinctrl-sirf.c b/drivers/pinctrl/sirf/pinctrl-sirf.c
index 76502aab2cb..014f5b1fee5 100644
--- a/drivers/pinctrl/sirf/pinctrl-sirf.c
+++ b/drivers/pinctrl/sirf/pinctrl-sirf.c
@@ -14,8 +14,6 @@
 #include <linux/io.h>
 #include <linux/slab.h>
 #include <linux/err.h>
-#include <linux/irqdomain.h>
-#include <linux/irqchip/chained_irq.h>
 #include <linux/pinctrl/pinctrl.h>
 #include <linux/pinctrl/pinmux.h>
 #include <linux/pinctrl/consumer.h>
@@ -27,22 +25,23 @@
 #include <linux/bitops.h>
 #include <linux/gpio.h>
 #include <linux/of_gpio.h>
-#include <asm/mach/irq.h>
 
 #include "pinctrl-sirf.h"
 
 #define DRIVER_NAME "pinmux-sirf"
 
 struct sirfsoc_gpio_bank {
-	struct of_mm_gpio_chip chip;
-	struct irq_domain *domain;
 	int id;
 	int parent_irq;
 	spinlock_t lock;
+};
+
+struct sirfsoc_gpio_chip {
+	struct of_mm_gpio_chip chip;
 	bool is_marco; /* for marco, some registers are different with prima2 */
+	struct sirfsoc_gpio_bank sgpio_bank[SIRFSOC_GPIO_NO_OF_BANKS];
 };
 
-static struct sirfsoc_gpio_bank sgpio_bank[SIRFSOC_GPIO_NO_OF_BANKS];
 static DEFINE_SPINLOCK(sgpio_lock);
 
 static struct sirfsoc_pin_group *sirfsoc_pin_groups;
@@ -255,37 +254,6 @@ static struct pinctrl_desc sirfsoc_pinmux_desc = {
 	.owner = THIS_MODULE,
 };
 
-/*
- * Todo: bind irq_chip to every pinctrl_gpio_range
- */
-static struct pinctrl_gpio_range sirfsoc_gpio_ranges[] = {
-	{
-		.name = "sirfsoc-gpio*",
-		.id = 0,
-		.base = 0,
-		.pin_base = 0,
-		.npins = 32,
-	}, {
-		.name = "sirfsoc-gpio*",
-		.id = 1,
-		.base = 32,
-		.pin_base = 32,
-		.npins = 32,
-	}, {
-		.name = "sirfsoc-gpio*",
-		.id = 2,
-		.base = 64,
-		.pin_base = 64,
-		.npins = 32,
-	}, {
-		.name = "sirfsoc-gpio*",
-		.id = 3,
-		.base = 96,
-		.pin_base = 96,
-		.npins = 19,
-	},
-};
-
 static void __iomem *sirfsoc_rsc_of_iomap(void)
 {
 	const struct of_device_id rsc_ids[]  = {
@@ -303,19 +271,16 @@ static void __iomem *sirfsoc_rsc_of_iomap(void)
 }
 
 static int sirfsoc_gpio_of_xlate(struct gpio_chip *gc,
-       const struct of_phandle_args *gpiospec,
-       u32 *flags)
+	const struct of_phandle_args *gpiospec,
+	u32 *flags)
 {
-       if (gpiospec->args[0] > SIRFSOC_GPIO_NO_OF_BANKS * SIRFSOC_GPIO_BANK_SIZE)
-		return -EINVAL;
-
-       if (gc != &sgpio_bank[gpiospec->args[0] / SIRFSOC_GPIO_BANK_SIZE].chip.gc)
+	if (gpiospec->args[0] > SIRFSOC_GPIO_NO_OF_BANKS * SIRFSOC_GPIO_BANK_SIZE)
 		return -EINVAL;
 
-       if (flags)
+	if (flags)
 		*flags = gpiospec->args[1];
 
-       return gpiospec->args[0] % SIRFSOC_GPIO_BANK_SIZE;
+	return gpiospec->args[0];
 }
 
 static const struct of_device_id pinmux_ids[] = {
@@ -331,7 +296,6 @@ static int sirfsoc_pinmux_probe(struct platform_device *pdev)
 	struct sirfsoc_pmx *spmx;
 	struct device_node *np = pdev->dev.of_node;
 	const struct sirfsoc_pinctrl_data *pdata;
-	int i;
 
 	/* Create state holders etc for this driver */
 	spmx = devm_kzalloc(&pdev->dev, sizeof(*spmx), GFP_KERNEL);
@@ -375,11 +339,6 @@ static int sirfsoc_pinmux_probe(struct platform_device *pdev)
 		goto out_no_pmx;
 	}
 
-	for (i = 0; i < ARRAY_SIZE(sirfsoc_gpio_ranges); i++) {
-		sirfsoc_gpio_ranges[i].gc = &sgpio_bank[i].chip.gc;
-		pinctrl_add_gpio_range(spmx->pmx, &sirfsoc_gpio_ranges[i]);
-	}
-
 	dev_info(&pdev->dev, "initialized SIRFSOC pinmux driver\n");
 
 	return 0;
@@ -464,34 +423,28 @@ static int __init sirfsoc_pinmux_init(void)
 }
 arch_initcall(sirfsoc_pinmux_init);
 
-static inline int sirfsoc_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
+static inline struct sirfsoc_gpio_chip *to_sirfsoc_gpio(struct gpio_chip *gc)
 {
-	struct sirfsoc_gpio_bank *bank = container_of(to_of_mm_gpio_chip(chip),
-		struct sirfsoc_gpio_bank, chip);
-
-	return irq_create_mapping(bank->domain, offset + bank->id *
-		SIRFSOC_GPIO_BANK_SIZE);
-}
-
-static inline int sirfsoc_gpio_to_offset(unsigned int gpio)
-{
-	return gpio % SIRFSOC_GPIO_BANK_SIZE;
+	return container_of(gc, struct sirfsoc_gpio_chip, chip.gc);
 }
 
-static inline struct sirfsoc_gpio_bank *sirfsoc_gpio_to_bank(unsigned int gpio)
+static inline struct sirfsoc_gpio_bank *
+sirfsoc_gpio_to_bank(struct sirfsoc_gpio_chip *sgpio, unsigned int offset)
 {
-	return &sgpio_bank[gpio / SIRFSOC_GPIO_BANK_SIZE];
+	return &sgpio->sgpio_bank[offset / SIRFSOC_GPIO_BANK_SIZE];
 }
 
-static inline struct sirfsoc_gpio_bank *sirfsoc_irqchip_to_bank(struct gpio_chip *chip)
+static inline int sirfsoc_gpio_to_bankoff(unsigned int offset)
 {
-	return container_of(to_of_mm_gpio_chip(chip), struct sirfsoc_gpio_bank, chip);
+	return offset % SIRFSOC_GPIO_BANK_SIZE;
 }
 
 static void sirfsoc_gpio_irq_ack(struct irq_data *d)
 {
-	struct sirfsoc_gpio_bank *bank = irq_data_get_irq_chip_data(d);
-	int idx = d->hwirq % SIRFSOC_GPIO_BANK_SIZE;
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(gc);
+	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, d->hwirq);
+	int idx = sirfsoc_gpio_to_bankoff(d->hwirq);
 	u32 val, offset;
 	unsigned long flags;
 
@@ -499,14 +452,16 @@ static void sirfsoc_gpio_irq_ack(struct irq_data *d)
 
 	spin_lock_irqsave(&sgpio_lock, flags);
 
-	val = readl(bank->chip.regs + offset);
+	val = readl(sgpio->chip.regs + offset);
 
-	writel(val, bank->chip.regs + offset);
+	writel(val, sgpio->chip.regs + offset);
 
 	spin_unlock_irqrestore(&sgpio_lock, flags);
 }
 
-static void __sirfsoc_gpio_irq_mask(struct sirfsoc_gpio_bank *bank, int idx)
+static void __sirfsoc_gpio_irq_mask(struct sirfsoc_gpio_chip *sgpio,
+				    struct sirfsoc_gpio_bank *bank,
+				    int idx)
 {
 	u32 val, offset;
 	unsigned long flags;
@@ -515,25 +470,29 @@ static void __sirfsoc_gpio_irq_mask(struct sirfsoc_gpio_bank *bank, int idx)
 
 	spin_lock_irqsave(&sgpio_lock, flags);
 
-	val = readl(bank->chip.regs + offset);
+	val = readl(sgpio->chip.regs + offset);
 	val &= ~SIRFSOC_GPIO_CTL_INTR_EN_MASK;
 	val &= ~SIRFSOC_GPIO_CTL_INTR_STS_MASK;
-	writel(val, bank->chip.regs + offset);
+	writel(val, sgpio->chip.regs + offset);
 
 	spin_unlock_irqrestore(&sgpio_lock, flags);
 }
 
 static void sirfsoc_gpio_irq_mask(struct irq_data *d)
 {
-	struct sirfsoc_gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(gc);
+	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, d->hwirq);
 
-	__sirfsoc_gpio_irq_mask(bank, d->hwirq % SIRFSOC_GPIO_BANK_SIZE);
+	__sirfsoc_gpio_irq_mask(sgpio, bank, d->hwirq % SIRFSOC_GPIO_BANK_SIZE);
 }
 
 static void sirfsoc_gpio_irq_unmask(struct irq_data *d)
 {
-	struct sirfsoc_gpio_bank *bank = irq_data_get_irq_chip_data(d);
-	int idx = d->hwirq % SIRFSOC_GPIO_BANK_SIZE;
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(gc);
+	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, d->hwirq);
+	int idx = sirfsoc_gpio_to_bankoff(d->hwirq);
 	u32 val, offset;
 	unsigned long flags;
 
@@ -541,18 +500,20 @@ static void sirfsoc_gpio_irq_unmask(struct irq_data *d)
 
 	spin_lock_irqsave(&sgpio_lock, flags);
 
-	val = readl(bank->chip.regs + offset);
+	val = readl(sgpio->chip.regs + offset);
 	val &= ~SIRFSOC_GPIO_CTL_INTR_STS_MASK;
 	val |= SIRFSOC_GPIO_CTL_INTR_EN_MASK;
-	writel(val, bank->chip.regs + offset);
+	writel(val, sgpio->chip.regs + offset);
 
 	spin_unlock_irqrestore(&sgpio_lock, flags);
 }
 
 static int sirfsoc_gpio_irq_type(struct irq_data *d, unsigned type)
 {
-	struct sirfsoc_gpio_bank *bank = irq_data_get_irq_chip_data(d);
-	int idx = d->hwirq % SIRFSOC_GPIO_BANK_SIZE;
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(gc);
+	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, d->hwirq);
+	int idx = sirfsoc_gpio_to_bankoff(d->hwirq);
 	u32 val, offset;
 	unsigned long flags;
 
@@ -560,7 +521,7 @@ static int sirfsoc_gpio_irq_type(struct irq_data *d, unsigned type)
 
 	spin_lock_irqsave(&sgpio_lock, flags);
 
-	val = readl(bank->chip.regs + offset);
+	val = readl(sgpio->chip.regs + offset);
 	val &= ~(SIRFSOC_GPIO_CTL_INTR_STS_MASK | SIRFSOC_GPIO_CTL_OUT_EN_MASK);
 
 	switch (type) {
@@ -588,53 +549,41 @@ static int sirfsoc_gpio_irq_type(struct irq_data *d, unsigned type)
 		break;
 	}
 
-	writel(val, bank->chip.regs + offset);
+	writel(val, sgpio->chip.regs + offset);
 
 	spin_unlock_irqrestore(&sgpio_lock, flags);
 
 	return 0;
 }
 
-static int sirfsoc_gpio_irq_reqres(struct irq_data *d)
-{
-	struct sirfsoc_gpio_bank *bank = irq_data_get_irq_chip_data(d);
-
-	if (gpio_lock_as_irq(&bank->chip.gc, d->hwirq % SIRFSOC_GPIO_BANK_SIZE)) {
-		dev_err(bank->chip.gc.dev,
-			"unable to lock HW IRQ %lu for IRQ\n",
-			d->hwirq);
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static void sirfsoc_gpio_irq_relres(struct irq_data *d)
-{
-	struct sirfsoc_gpio_bank *bank = irq_data_get_irq_chip_data(d);
-
-	gpio_unlock_as_irq(&bank->chip.gc, d->hwirq % SIRFSOC_GPIO_BANK_SIZE);
-}
-
 static struct irq_chip sirfsoc_irq_chip = {
 	.name = "sirf-gpio-irq",
 	.irq_ack = sirfsoc_gpio_irq_ack,
 	.irq_mask = sirfsoc_gpio_irq_mask,
 	.irq_unmask = sirfsoc_gpio_irq_unmask,
 	.irq_set_type = sirfsoc_gpio_irq_type,
-	.irq_request_resources = sirfsoc_gpio_irq_reqres,
-	.irq_release_resources = sirfsoc_gpio_irq_relres,
 };
 
 static void sirfsoc_gpio_handle_irq(unsigned int irq, struct irq_desc *desc)
 {
-	struct sirfsoc_gpio_bank *bank = irq_get_handler_data(irq);
+	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
+	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(gc);
+	struct sirfsoc_gpio_bank *bank;
 	u32 status, ctrl;
 	int idx = 0;
 	struct irq_chip *chip = irq_get_chip(irq);
+	int i;
+
+	for (i = 0; i < SIRFSOC_GPIO_NO_OF_BANKS; i++) {
+		bank = &sgpio->sgpio_bank[i];
+		if (bank->parent_irq == irq)
+			break;
+	}
+	BUG_ON(i == SIRFSOC_GPIO_NO_OF_BANKS);
 
 	chained_irq_enter(chip, desc);
 
-	status = readl(bank->chip.regs + SIRFSOC_GPIO_INT_STATUS(bank->id));
+	status = readl(sgpio->chip.regs + SIRFSOC_GPIO_INT_STATUS(bank->id));
 	if (!status) {
 		printk(KERN_WARNING
 			"%s: gpio id %d status %#x no interrupt is flaged\n",
@@ -644,7 +593,7 @@ static void sirfsoc_gpio_handle_irq(unsigned int irq, struct irq_desc *desc)
 	}
 
 	while (status) {
-		ctrl = readl(bank->chip.regs + SIRFSOC_GPIO_CTRL(bank->id, idx));
+		ctrl = readl(sgpio->chip.regs + SIRFSOC_GPIO_CTRL(bank->id, idx));
 
 		/*
 		 * Here we must check whether the corresponding GPIO's interrupt
@@ -653,7 +602,7 @@ static void sirfsoc_gpio_handle_irq(unsigned int irq, struct irq_desc *desc)
 		if ((status & 0x1) && (ctrl & SIRFSOC_GPIO_CTL_INTR_EN_MASK)) {
 			pr_debug("%s: gpio id %d idx %d happens\n",
 				__func__, bank->id, idx);
-			generic_handle_irq(irq_find_mapping(bank->domain, idx +
+			generic_handle_irq(irq_find_mapping(gc->irqdomain, idx +
 					bank->id * SIRFSOC_GPIO_BANK_SIZE));
 		}
 
@@ -664,18 +613,20 @@ static void sirfsoc_gpio_handle_irq(unsigned int irq, struct irq_desc *desc)
 	chained_irq_exit(chip, desc);
 }
 
-static inline void sirfsoc_gpio_set_input(struct sirfsoc_gpio_bank *bank, unsigned ctrl_offset)
+static inline void sirfsoc_gpio_set_input(struct sirfsoc_gpio_chip *sgpio,
+					  unsigned ctrl_offset)
 {
 	u32 val;
 
-	val = readl(bank->chip.regs + ctrl_offset);
+	val = readl(sgpio->chip.regs + ctrl_offset);
 	val &= ~SIRFSOC_GPIO_CTL_OUT_EN_MASK;
-	writel(val, bank->chip.regs + ctrl_offset);
+	writel(val, sgpio->chip.regs + ctrl_offset);
 }
 
 static int sirfsoc_gpio_request(struct gpio_chip *chip, unsigned offset)
 {
-	struct sirfsoc_gpio_bank *bank = sirfsoc_irqchip_to_bank(chip);
+	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(chip);
+	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, offset);
 	unsigned long flags;
 
 	if (pinctrl_request_gpio(chip->base + offset))
@@ -687,8 +638,8 @@ static int sirfsoc_gpio_request(struct gpio_chip *chip, unsigned offset)
 	 * default status:
 	 * set direction as input and mask irq
 	 */
-	sirfsoc_gpio_set_input(bank, SIRFSOC_GPIO_CTRL(bank->id, offset));
-	__sirfsoc_gpio_irq_mask(bank, offset);
+	sirfsoc_gpio_set_input(sgpio, SIRFSOC_GPIO_CTRL(bank->id, offset));
+	__sirfsoc_gpio_irq_mask(sgpio, bank, offset);
 
 	spin_unlock_irqrestore(&bank->lock, flags);
 
@@ -697,13 +648,14 @@ static int sirfsoc_gpio_request(struct gpio_chip *chip, unsigned offset)
 
 static void sirfsoc_gpio_free(struct gpio_chip *chip, unsigned offset)
 {
-	struct sirfsoc_gpio_bank *bank = sirfsoc_irqchip_to_bank(chip);
+	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(chip);
+	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, offset);
 	unsigned long flags;
 
 	spin_lock_irqsave(&bank->lock, flags);
 
-	__sirfsoc_gpio_irq_mask(bank, offset);
-	sirfsoc_gpio_set_input(bank, SIRFSOC_GPIO_CTRL(bank->id, offset));
+	__sirfsoc_gpio_irq_mask(sgpio, bank, offset);
+	sirfsoc_gpio_set_input(sgpio, SIRFSOC_GPIO_CTRL(bank->id, offset));
 
 	spin_unlock_irqrestore(&bank->lock, flags);
 
@@ -712,8 +664,9 @@ static void sirfsoc_gpio_free(struct gpio_chip *chip, unsigned offset)
 
 static int sirfsoc_gpio_direction_input(struct gpio_chip *chip, unsigned gpio)
 {
-	struct sirfsoc_gpio_bank *bank = sirfsoc_irqchip_to_bank(chip);
-	int idx = sirfsoc_gpio_to_offset(gpio);
+	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(chip);
+	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, gpio);
+	int idx = sirfsoc_gpio_to_bankoff(gpio);
 	unsigned long flags;
 	unsigned offset;
 
@@ -721,22 +674,24 @@ static int sirfsoc_gpio_direction_input(struct gpio_chip *chip, unsigned gpio)
 
 	spin_lock_irqsave(&bank->lock, flags);
 
-	sirfsoc_gpio_set_input(bank, offset);
+	sirfsoc_gpio_set_input(sgpio, offset);
 
 	spin_unlock_irqrestore(&bank->lock, flags);
 
 	return 0;
 }
 
-static inline void sirfsoc_gpio_set_output(struct sirfsoc_gpio_bank *bank, unsigned offset,
-	int value)
+static inline void sirfsoc_gpio_set_output(struct sirfsoc_gpio_chip *sgpio,
+					   struct sirfsoc_gpio_bank *bank,
+					   unsigned offset,
+					   int value)
 {
 	u32 out_ctrl;
 	unsigned long flags;
 
 	spin_lock_irqsave(&bank->lock, flags);
 
-	out_ctrl = readl(bank->chip.regs + offset);
+	out_ctrl = readl(sgpio->chip.regs + offset);
 	if (value)
 		out_ctrl |= SIRFSOC_GPIO_CTL_DATAOUT_MASK;
 	else
@@ -744,15 +699,16 @@ static inline void sirfsoc_gpio_set_output(struct sirfsoc_gpio_bank *bank, unsig
 
 	out_ctrl &= ~SIRFSOC_GPIO_CTL_INTR_EN_MASK;
 	out_ctrl |= SIRFSOC_GPIO_CTL_OUT_EN_MASK;
-	writel(out_ctrl, bank->chip.regs + offset);
+	writel(out_ctrl, sgpio->chip.regs + offset);
 
 	spin_unlock_irqrestore(&bank->lock, flags);
 }
 
 static int sirfsoc_gpio_direction_output(struct gpio_chip *chip, unsigned gpio, int value)
 {
-	struct sirfsoc_gpio_bank *bank = sirfsoc_irqchip_to_bank(chip);
-	int idx = sirfsoc_gpio_to_offset(gpio);
+	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(chip);
+	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, gpio);
+	int idx = sirfsoc_gpio_to_bankoff(gpio);
 	u32 offset;
 	unsigned long flags;
 
@@ -760,7 +716,7 @@ static int sirfsoc_gpio_direction_output(struct gpio_chip *chip, unsigned gpio,
 
 	spin_lock_irqsave(&sgpio_lock, flags);
 
-	sirfsoc_gpio_set_output(bank, offset, value);
+	sirfsoc_gpio_set_output(sgpio, bank, offset, value);
 
 	spin_unlock_irqrestore(&sgpio_lock, flags);
 
@@ -769,13 +725,14 @@ static int sirfsoc_gpio_direction_output(struct gpio_chip *chip, unsigned gpio,
 
 static int sirfsoc_gpio_get_value(struct gpio_chip *chip, unsigned offset)
 {
-	struct sirfsoc_gpio_bank *bank = sirfsoc_irqchip_to_bank(chip);
+	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(chip);
+	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, offset);
 	u32 val;
 	unsigned long flags;
 
 	spin_lock_irqsave(&bank->lock, flags);
 
-	val = readl(bank->chip.regs + SIRFSOC_GPIO_CTRL(bank->id, offset));
+	val = readl(sgpio->chip.regs + SIRFSOC_GPIO_CTRL(bank->id, offset));
 
 	spin_unlock_irqrestore(&bank->lock, flags);
 
@@ -785,44 +742,25 @@ static int sirfsoc_gpio_get_value(struct gpio_chip *chip, unsigned offset)
 static void sirfsoc_gpio_set_value(struct gpio_chip *chip, unsigned offset,
 	int value)
 {
-	struct sirfsoc_gpio_bank *bank = sirfsoc_irqchip_to_bank(chip);
+	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(chip);
+	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, offset);
 	u32 ctrl;
 	unsigned long flags;
 
 	spin_lock_irqsave(&bank->lock, flags);
 
-	ctrl = readl(bank->chip.regs + SIRFSOC_GPIO_CTRL(bank->id, offset));
+	ctrl = readl(sgpio->chip.regs + SIRFSOC_GPIO_CTRL(bank->id, offset));
 	if (value)
 		ctrl |= SIRFSOC_GPIO_CTL_DATAOUT_MASK;
 	else
 		ctrl &= ~SIRFSOC_GPIO_CTL_DATAOUT_MASK;
-	writel(ctrl, bank->chip.regs + SIRFSOC_GPIO_CTRL(bank->id, offset));
+	writel(ctrl, sgpio->chip.regs + SIRFSOC_GPIO_CTRL(bank->id, offset));
 
 	spin_unlock_irqrestore(&bank->lock, flags);
 }
 
-static int sirfsoc_gpio_irq_map(struct irq_domain *d, unsigned int irq,
-				irq_hw_number_t hwirq)
-{
-	struct sirfsoc_gpio_bank *bank = d->host_data;
-
-	if (!bank)
-		return -EINVAL;
-
-	irq_set_chip(irq, &sirfsoc_irq_chip);
-	irq_set_handler(irq, handle_level_irq);
-	irq_set_chip_data(irq, bank + hwirq / SIRFSOC_GPIO_BANK_SIZE);
-	set_irq_flags(irq, IRQF_VALID);
-
-	return 0;
-}
-
-static const struct irq_domain_ops sirfsoc_gpio_irq_simple_ops = {
-	.map = sirfsoc_gpio_irq_map,
-	.xlate = irq_domain_xlate_twocell,
-};
-
-static void sirfsoc_gpio_set_pullup(const u32 *pullups)
+static void sirfsoc_gpio_set_pullup(struct sirfsoc_gpio_chip *sgpio,
+				    const u32 *pullups)
 {
 	int i, n;
 	const unsigned long *p = (const unsigned long *)pullups;
@@ -830,15 +768,16 @@ static void sirfsoc_gpio_set_pullup(const u32 *pullups)
 	for (i = 0; i < SIRFSOC_GPIO_NO_OF_BANKS; i++) {
 		for_each_set_bit(n, p + i, BITS_PER_LONG) {
 			u32 offset = SIRFSOC_GPIO_CTRL(i, n);
-			u32 val = readl(sgpio_bank[i].chip.regs + offset);
+			u32 val = readl(sgpio->chip.regs + offset);
 			val |= SIRFSOC_GPIO_CTL_PULL_MASK;
 			val |= SIRFSOC_GPIO_CTL_PULL_HIGH;
-			writel(val, sgpio_bank[i].chip.regs + offset);
+			writel(val, sgpio->chip.regs + offset);
 		}
 	}
 }
 
-static void sirfsoc_gpio_set_pulldown(const u32 *pulldowns)
+static void sirfsoc_gpio_set_pulldown(struct sirfsoc_gpio_chip *sgpio,
+				      const u32 *pulldowns)
 {
 	int i, n;
 	const unsigned long *p = (const unsigned long *)pulldowns;
@@ -846,10 +785,10 @@ static void sirfsoc_gpio_set_pulldown(const u32 *pulldowns)
 	for (i = 0; i < SIRFSOC_GPIO_NO_OF_BANKS; i++) {
 		for_each_set_bit(n, p + i, BITS_PER_LONG) {
 			u32 offset = SIRFSOC_GPIO_CTRL(i, n);
-			u32 val = readl(sgpio_bank[i].chip.regs + offset);
+			u32 val = readl(sgpio->chip.regs + offset);
 			val |= SIRFSOC_GPIO_CTL_PULL_MASK;
 			val &= ~SIRFSOC_GPIO_CTL_PULL_HIGH;
-			writel(val, sgpio_bank[i].chip.regs + offset);
+			writel(val, sgpio->chip.regs + offset);
 		}
 	}
 }
@@ -857,10 +796,10 @@ static void sirfsoc_gpio_set_pulldown(const u32 *pulldowns)
 static int sirfsoc_gpio_probe(struct device_node *np)
 {
 	int i, err = 0;
+	static struct sirfsoc_gpio_chip *sgpio;
 	struct sirfsoc_gpio_bank *bank;
 	void __iomem *regs;
 	struct platform_device *pdev;
-	struct irq_domain *domain;
 	bool is_marco = false;
 
 	u32 pullups[SIRFSOC_GPIO_NO_OF_BANKS], pulldowns[SIRFSOC_GPIO_NO_OF_BANKS];
@@ -869,6 +808,10 @@ static int sirfsoc_gpio_probe(struct device_node *np)
 	if (!pdev)
 		return -ENODEV;
 
+	sgpio = devm_kzalloc(&pdev->dev, sizeof(*sgpio), GFP_KERNEL);
+	if (!sgpio)
+		return -ENOMEM;
+
 	regs = of_iomap(np, 0);
 	if (!regs)
 		return -ENOMEM;
@@ -876,63 +819,76 @@ static int sirfsoc_gpio_probe(struct device_node *np)
 	if (of_device_is_compatible(np, "sirf,marco-pinctrl"))
 		is_marco = 1;
 
-	domain = irq_domain_add_linear(np, SIRFSOC_GPIO_BANK_SIZE * SIRFSOC_GPIO_NO_OF_BANKS,
-		&sirfsoc_gpio_irq_simple_ops, sgpio_bank);
-	if (!domain) {
-		pr_err("%s: Failed to create irqdomain\n", np->full_name);
-			err = -ENOSYS;
+	sgpio->chip.gc.request = sirfsoc_gpio_request;
+	sgpio->chip.gc.free = sirfsoc_gpio_free;
+	sgpio->chip.gc.direction_input = sirfsoc_gpio_direction_input;
+	sgpio->chip.gc.get = sirfsoc_gpio_get_value;
+	sgpio->chip.gc.direction_output = sirfsoc_gpio_direction_output;
+	sgpio->chip.gc.set = sirfsoc_gpio_set_value;
+	sgpio->chip.gc.base = 0;
+	sgpio->chip.gc.ngpio = SIRFSOC_GPIO_BANK_SIZE * SIRFSOC_GPIO_NO_OF_BANKS;
+	sgpio->chip.gc.label = kstrdup(np->full_name, GFP_KERNEL);
+	sgpio->chip.gc.of_node = np;
+	sgpio->chip.gc.of_xlate = sirfsoc_gpio_of_xlate;
+	sgpio->chip.gc.of_gpio_n_cells = 2;
+	sgpio->chip.gc.dev = &pdev->dev;
+	sgpio->chip.regs = regs;
+	sgpio->is_marco = is_marco;
+
+	err = gpiochip_add(&sgpio->chip.gc);
+	if (err) {
+		dev_err(&pdev->dev, "%s: error in probe function with status %d\n",
+			np->full_name, err);
+		goto out;
+	}
+
+	err =  gpiochip_irqchip_add(&sgpio->chip.gc,
+		&sirfsoc_irq_chip,
+		0, handle_level_irq,
+		IRQ_TYPE_NONE);
+	if (err) {
+		dev_err(&pdev->dev,
+			"could not connect irqchip to gpiochip\n");
 		goto out;
 	}
 
 	for (i = 0; i < SIRFSOC_GPIO_NO_OF_BANKS; i++) {
-		bank = &sgpio_bank[i];
+		bank = &sgpio->sgpio_bank[i];
 		spin_lock_init(&bank->lock);
-		bank->chip.gc.request = sirfsoc_gpio_request;
-		bank->chip.gc.free = sirfsoc_gpio_free;
-		bank->chip.gc.direction_input = sirfsoc_gpio_direction_input;
-		bank->chip.gc.get = sirfsoc_gpio_get_value;
-		bank->chip.gc.direction_output = sirfsoc_gpio_direction_output;
-		bank->chip.gc.set = sirfsoc_gpio_set_value;
-		bank->chip.gc.to_irq = sirfsoc_gpio_to_irq;
-		bank->chip.gc.base = i * SIRFSOC_GPIO_BANK_SIZE;
-		bank->chip.gc.ngpio = SIRFSOC_GPIO_BANK_SIZE;
-		bank->chip.gc.label = kstrdup(np->full_name, GFP_KERNEL);
-		bank->chip.gc.of_node = np;
-		bank->chip.gc.of_xlate = sirfsoc_gpio_of_xlate;
-		bank->chip.gc.of_gpio_n_cells = 2;
-		bank->chip.gc.dev = &pdev->dev;
-		bank->chip.regs = regs;
-		bank->id = i;
-		bank->is_marco = is_marco;
 		bank->parent_irq = platform_get_irq(pdev, i);
 		if (bank->parent_irq < 0) {
 			err = bank->parent_irq;
-			goto out;
+			goto out_banks;
 		}
 
-		err = gpiochip_add(&bank->chip.gc);
-		if (err) {
-			pr_err("%s: error in probe function with status %d\n",
-				np->full_name, err);
-			goto out;
-		}
-
-		bank->domain = domain;
+		gpiochip_set_chained_irqchip(&sgpio->chip.gc,
+			&sirfsoc_irq_chip,
+			bank->parent_irq,
+			sirfsoc_gpio_handle_irq);
+	}
 
-		irq_set_chained_handler(bank->parent_irq, sirfsoc_gpio_handle_irq);
-		irq_set_handler_data(bank->parent_irq, bank);
+	err = gpiochip_add_pin_range(&sgpio->chip.gc, dev_name(&pdev->dev),
+		0, 0, SIRFSOC_GPIO_BANK_SIZE * SIRFSOC_GPIO_NO_OF_BANKS);
+	if (err) {
+		dev_err(&pdev->dev,
+			"could not add gpiochip pin range\n");
+		goto out_no_range;
 	}
 
 	if (!of_property_read_u32_array(np, "sirf,pullups", pullups,
 		SIRFSOC_GPIO_NO_OF_BANKS))
-		sirfsoc_gpio_set_pullup(pullups);
+		sirfsoc_gpio_set_pullup(sgpio, pullups);
 
 	if (!of_property_read_u32_array(np, "sirf,pulldowns", pulldowns,
 		SIRFSOC_GPIO_NO_OF_BANKS))
-		sirfsoc_gpio_set_pulldown(pulldowns);
+		sirfsoc_gpio_set_pulldown(sgpio, pulldowns);
 
 	return 0;
 
+out_no_range:
+out_banks:
+	if (gpiochip_remove(&sgpio->chip.gc))
+		dev_err(&pdev->dev, "could not remove gpio chip\n");
 out:
 	iounmap(regs);
 	return err;
diff --git a/drivers/pinctrl/sunxi/Kconfig b/drivers/pinctrl/sunxi/Kconfig
new file mode 100644
index 00000000000..73e0a305ea1
--- /dev/null
+++ b/drivers/pinctrl/sunxi/Kconfig
@@ -0,0 +1,36 @@
+if ARCH_SUNXI
+
+config PINCTRL_SUNXI
+	bool
+
+config PINCTRL_SUNXI_COMMON
+	bool
+	select PINMUX
+	select GENERIC_PINCONF
+
+config PINCTRL_SUN4I_A10
+	def_bool PINCTRL_SUNXI || MACH_SUN4I
+	select PINCTRL_SUNXI_COMMON
+
+config PINCTRL_SUN5I_A10S
+	def_bool PINCTRL_SUNXI || MACH_SUN5I
+	select PINCTRL_SUNXI_COMMON
+
+config PINCTRL_SUN5I_A13
+	def_bool PINCTRL_SUNXI || MACH_SUN5I
+	select PINCTRL_SUNXI_COMMON
+
+config PINCTRL_SUN6I_A31
+	def_bool PINCTRL_SUNXI || MACH_SUN6I
+	select PINCTRL_SUNXI_COMMON
+
+config PINCTRL_SUN6I_A31_R
+	def_bool PINCTRL_SUNXI || MACH_SUN6I
+	depends on RESET_CONTROLLER
+	select PINCTRL_SUNXI_COMMON
+
+config PINCTRL_SUN7I_A20
+	def_bool PINCTRL_SUNXI || MACH_SUN7I
+	select PINCTRL_SUNXI_COMMON
+
+endif
diff --git a/drivers/pinctrl/sunxi/Makefile b/drivers/pinctrl/sunxi/Makefile
new file mode 100644
index 00000000000..0f4461cbe11
--- /dev/null
+++ b/drivers/pinctrl/sunxi/Makefile
@@ -0,0 +1,10 @@
+# Core
+obj-$(CONFIG_PINCTRL_SUNXI_COMMON)	+= pinctrl-sunxi.o
+
+# SoC Drivers
+obj-$(CONFIG_PINCTRL_SUN4I_A10)		+= pinctrl-sun4i-a10.o
+obj-$(CONFIG_PINCTRL_SUN5I_A10S)	+= pinctrl-sun5i-a10s.o
+obj-$(CONFIG_PINCTRL_SUN5I_A13)		+= pinctrl-sun5i-a13.o
+obj-$(CONFIG_PINCTRL_SUN6I_A31)		+= pinctrl-sun6i-a31.o
+obj-$(CONFIG_PINCTRL_SUN6I_A31_R)	+= pinctrl-sun6i-a31-r.o
+obj-$(CONFIG_PINCTRL_SUN7I_A20)		+= pinctrl-sun7i-a20.o
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun4i-a10.c b/drivers/pinctrl/sunxi/pinctrl-sun4i-a10.c
new file mode 100644
index 00000000000..fa1ff7c7e35
--- /dev/null
+++ b/drivers/pinctrl/sunxi/pinctrl-sun4i-a10.c
@@ -0,0 +1,1039 @@
+/*
+ * Allwinner A10 SoCs pinctrl driver.
+ *
+ * Copyright (C) 2014 Maxime Ripard
+ *
+ * Maxime Ripard <maxime.ripard@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-sunxi.h"
+
+static const struct sunxi_desc_pin sun4i_a10_pins[] = {
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD3 */
+		  SUNXI_FUNCTION(0x3, "spi1"),		/* CS0 */
+		  SUNXI_FUNCTION(0x4, "uart2")),	/* RTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD2 */
+		  SUNXI_FUNCTION(0x3, "spi1"),		/* CLK */
+		  SUNXI_FUNCTION(0x4, "uart2")),	/* CTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD1 */
+		  SUNXI_FUNCTION(0x3, "spi1"),		/* MOSI */
+		  SUNXI_FUNCTION(0x4, "uart2")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD0 */
+		  SUNXI_FUNCTION(0x3, "spi1"),		/* MISO */
+		  SUNXI_FUNCTION(0x4, "uart2")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD3 */
+		  SUNXI_FUNCTION(0x3, "spi1")),		/* CS1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD2 */
+		  SUNXI_FUNCTION(0x3, "spi3")),		/* CS0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD1 */
+		  SUNXI_FUNCTION(0x3, "spi3")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD0 */
+		  SUNXI_FUNCTION(0x3, "spi3")),		/* MOSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXCK */
+		  SUNXI_FUNCTION(0x3, "spi3")),		/* MISO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXERR */
+		  SUNXI_FUNCTION(0x3, "spi3")),		/* CS1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXDV */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* EMDC */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* EMDIO */
+		  SUNXI_FUNCTION(0x3, "uart6"),		/* TX */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* RTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXEN */
+		  SUNXI_FUNCTION(0x3, "uart6"),		/* RX */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* CTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXCK */
+		  SUNXI_FUNCTION(0x3, "uart7"),		/* TX */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* DTR */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ECRS */
+		  SUNXI_FUNCTION(0x3, "uart7"),		/* RX */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* DSR */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ECOL */
+		  SUNXI_FUNCTION(0x3, "can"),		/* TX */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* DCD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXERR */
+		  SUNXI_FUNCTION(0x3, "can"),		/* RX */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* RING */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "pwm")),		/* PWM0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ir0")),		/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ir0")),		/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s"),		/* MCLK */
+		  SUNXI_FUNCTION(0x3, "ac97")),		/* MCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s"),		/* BCLK */
+		  SUNXI_FUNCTION(0x3, "ac97")),		/* BCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s"),		/* LRCK */
+		  SUNXI_FUNCTION(0x3, "ac97")),		/* SYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s"),		/* DO0 */
+		  SUNXI_FUNCTION(0x3, "ac97")),		/* DO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s")),		/* DO1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s")),		/* DO2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s")),		/* DO3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s"),		/* DI */
+		  SUNXI_FUNCTION(0x3, "ac97")),		/* DI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2")),		/* CS1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS0 */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* MS0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* CK0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* DO0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* DI0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart0"),		/* TX */
+		  SUNXI_FUNCTION(0x3, "ir1")),		/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart0"),		/* RX */
+		  SUNXI_FUNCTION(0x3, "ir1")),		/* RX */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NWE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MOSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NALE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MISO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCLE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NRE# */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB0 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB1 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ0 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ1 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ2 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ3 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NWP */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE4 */
+		  SUNXI_FUNCTION(0x3, "spi2")),		/* CS0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE5 */
+		  SUNXI_FUNCTION(0x3, "spi2")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE6 */
+		  SUNXI_FUNCTION(0x3, "spi2")),		/* MOSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE7 */
+		  SUNXI_FUNCTION(0x3, "spi2")),		/* MISO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* CS0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQS */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D0 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D1 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D4 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D5 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D6 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VPC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D7 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D8 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D9 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VM3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D10 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D11 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D12 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D13 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D14 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D15 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D16 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VPC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D17 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D18 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D19 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D20 */
+		  SUNXI_FUNCTION(0x3, "csi1")),		/* MCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D21 */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* VPPEN */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D22 */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* VPPPP */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D23 */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* DET */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* VCCEN */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 25),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* DE */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* RST */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 26),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* HSYNC */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 27),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* VSYNC */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* SDA */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* PCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* ERR */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* CK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* SYNC */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* HSYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* DVLD */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* VSYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D0 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D1 */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "sim")),		/* VPPEN */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D4 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D5 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D6 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D7 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D7 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* MSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D0 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* DI1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CLK */
+		  SUNXI_FUNCTION(0x4, "uart0")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CMD */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* DO1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D3 */
+		  SUNXI_FUNCTION(0x4, "uart0")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D2 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* CK1 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* PCK */
+		  SUNXI_FUNCTION(0x4, "mmc1")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* ERR */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* CK */
+		  SUNXI_FUNCTION(0x4, "mmc1")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* SYNC */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* HSYNC */
+		  SUNXI_FUNCTION(0x4, "mmc1")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* DVLD */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* VSYNC */
+		  SUNXI_FUNCTION(0x4, "mmc1")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* D0 */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* D0 */
+		  SUNXI_FUNCTION(0x4, "mmc1"),		/* D2 */
+		  SUNXI_FUNCTION(0x5, "csi0")),		/* D8 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* D1 */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "mmc1"),		/* D3 */
+		  SUNXI_FUNCTION(0x5, "csi0")),		/* D9 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* D2 */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* TX */
+		  SUNXI_FUNCTION(0x5, "csi0")),		/* D10 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* D3 */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* RX */
+		  SUNXI_FUNCTION(0x5, "csi0")),		/* D11 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* D4 */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* D4 */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* RTS */
+		  SUNXI_FUNCTION(0x5, "csi0")),		/* D12 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* D5 */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* D5 */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* CTS */
+		  SUNXI_FUNCTION(0x5, "csi0")),		/* D13 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* D6 */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* D6 */
+		  SUNXI_FUNCTION(0x4, "uart4"),		/* TX */
+		  SUNXI_FUNCTION(0x5, "csi0")),		/* D14 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* D7 */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* D7 */
+		  SUNXI_FUNCTION(0x4, "uart4"),		/* RX */
+		  SUNXI_FUNCTION(0x5, "csi0")),		/* D15 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D0 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAA0 */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 0),		/* EINT0 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D1 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAA1 */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 1),		/* EINT1 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAA2 */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* RTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 2),		/* EINT2 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAIRQ */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* CTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 3),		/* EINT3 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D4 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD0 */
+		  SUNXI_FUNCTION(0x4, "uart4"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 4),		/* EINT4 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D5 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD1 */
+		  SUNXI_FUNCTION(0x4, "uart4"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 5),		/* EINT5 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D6 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD2 */
+		  SUNXI_FUNCTION(0x4, "uart5"),		/* TX */
+		  SUNXI_FUNCTION(0x5, "ms"),		/* BS */
+		  SUNXI_FUNCTION_IRQ(0x6, 6),		/* EINT6 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D7 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD3 */
+		  SUNXI_FUNCTION(0x4, "uart5"),		/* RX */
+		  SUNXI_FUNCTION(0x5, "ms"),		/* CLK */
+		  SUNXI_FUNCTION_IRQ(0x6, 7),		/* EINT7 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D8 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD4 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN0 */
+		  SUNXI_FUNCTION(0x5, "ms"),		/* D0 */
+		  SUNXI_FUNCTION_IRQ(0x6, 8),		/* EINT8 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D8 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D9 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD5 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN1 */
+		  SUNXI_FUNCTION(0x5, "ms"),		/* D1 */
+		  SUNXI_FUNCTION_IRQ(0x6, 9),		/* EINT9 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D9 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D10 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD6 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN2 */
+		  SUNXI_FUNCTION(0x5, "ms"),		/* D2 */
+		  SUNXI_FUNCTION_IRQ(0x6, 10),		/* EINT10 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D10 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D11 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD7 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN3 */
+		  SUNXI_FUNCTION(0x5, "ms"),		/* D3 */
+		  SUNXI_FUNCTION_IRQ(0x6, 11),		/* EINT11 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D11 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D12 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD8 */
+		  SUNXI_FUNCTION(0x4, "ps2"),		/* SCK1 */
+		  SUNXI_FUNCTION_IRQ(0x6, 12),		/* EINT12 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D12 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D13 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD9 */
+		  SUNXI_FUNCTION(0x4, "ps2"),		/* SDA1 */
+		  SUNXI_FUNCTION(0x5, "sim"),		/* RST */
+		  SUNXI_FUNCTION_IRQ(0x6, 13),		/* EINT13 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D13 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D14 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD10 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN4 */
+		  SUNXI_FUNCTION(0x5, "sim"),		/* VPPEN */
+		  SUNXI_FUNCTION_IRQ(0x6, 14),		/* EINT14 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D14 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D15 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD11 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN5 */
+		  SUNXI_FUNCTION(0x5, "sim"),		/* VPPPP */
+		  SUNXI_FUNCTION_IRQ(0x6, 15),		/* EINT15 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D15 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D16 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD12 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN6 */
+		  SUNXI_FUNCTION_IRQ(0x6, 16),		/* EINT16 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D16 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D17 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD13 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN7 */
+		  SUNXI_FUNCTION(0x5, "sim"),		/* VCCEN */
+		  SUNXI_FUNCTION_IRQ(0x6, 17),		/* EINT17 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D17 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D18 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD14 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT0 */
+		  SUNXI_FUNCTION(0x5, "sim"),		/* SCK */
+		  SUNXI_FUNCTION_IRQ(0x6, 18),		/* EINT18 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D18 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D19 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD15 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT1 */
+		  SUNXI_FUNCTION(0x5, "sim"),		/* SDA */
+		  SUNXI_FUNCTION_IRQ(0x6, 19),		/* EINT19 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D19 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D20 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAOE */
+		  SUNXI_FUNCTION(0x4, "can"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 20),		/* EINT20 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D20 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D21 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATADREQ */
+		  SUNXI_FUNCTION(0x4, "can"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 21),		/* EINT21 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D21 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D22 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATADACK */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT2 */
+		  SUNXI_FUNCTION(0x5, "mmc1"),		/* CMD */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D22 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D23 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATACS0 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT3 */
+		  SUNXI_FUNCTION(0x5, "mmc1"),		/* CLK */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D23 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATACS1 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT4 */
+		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D0 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* PCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 25),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* DE */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAIORDY */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT5 */
+		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D1 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* FIELD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 26),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* HSYNC */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAIOR */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT6 */
+		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D2 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* HSYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 27),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* VSYNC */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAIOW */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT7 */
+		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D3 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* VSYNC */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "pwm")),		/* PWM1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* CS0 */
+		  SUNXI_FUNCTION(0x3, "uart5"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 22)),		/* EINT22 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "uart5"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 23)),		/* EINT23 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "uart6"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 24)),		/* EINT24 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "uart6"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 25)),		/* EINT25 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* CS1 */
+		  SUNXI_FUNCTION(0x3, "ps2"),		/* SCK1 */
+		  SUNXI_FUNCTION(0x4, "timer4"),	/* TCLKIN0 */
+		  SUNXI_FUNCTION_IRQ(0x6, 26)),		/* EINT26 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS1 */
+		  SUNXI_FUNCTION(0x3, "ps2"),		/* SDA1 */
+		  SUNXI_FUNCTION(0x4, "timer5"),	/* TCLKIN1 */
+		  SUNXI_FUNCTION_IRQ(0x6, 27)),		/* EINT27 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS0 */
+		  SUNXI_FUNCTION(0x3, "uart2"),		/* RTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 28)),		/* EINT28 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "uart2"),		/* CTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 29)),		/* EINT29 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "uart2"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 30)),		/* EINT30 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "uart2"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 31)),		/* EINT31 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ps2"),		/* SCK0 */
+		  SUNXI_FUNCTION(0x3, "uart7"),		/* TX */
+		  SUNXI_FUNCTION(0x4, "hdmi")),		/* HSCL */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ps2"),		/* SDA0 */
+		  SUNXI_FUNCTION(0x3, "uart7"),		/* RX */
+		  SUNXI_FUNCTION(0x4, "hdmi")),		/* HSDA */
+};
+
+static const struct sunxi_pinctrl_desc sun4i_a10_pinctrl_data = {
+	.pins = sun4i_a10_pins,
+	.npins = ARRAY_SIZE(sun4i_a10_pins),
+};
+
+static int sun4i_a10_pinctrl_probe(struct platform_device *pdev)
+{
+	return sunxi_pinctrl_init(pdev,
+				  &sun4i_a10_pinctrl_data);
+}
+
+static struct of_device_id sun4i_a10_pinctrl_match[] = {
+	{ .compatible = "allwinner,sun4i-a10-pinctrl", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, sun4i_a10_pinctrl_match);
+
+static struct platform_driver sun4i_a10_pinctrl_driver = {
+	.probe	= sun4i_a10_pinctrl_probe,
+	.driver	= {
+		.name		= "sun4i-pinctrl",
+		.owner		= THIS_MODULE,
+		.of_match_table	= sun4i_a10_pinctrl_match,
+	},
+};
+module_platform_driver(sun4i_a10_pinctrl_driver);
+
+MODULE_AUTHOR("Maxime Ripard <maxime.ripard@free-electrons.com");
+MODULE_DESCRIPTION("Allwinner A10 pinctrl driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun5i-a10s.c b/drivers/pinctrl/sunxi/pinctrl-sun5i-a10s.c
new file mode 100644
index 00000000000..164d743f526
--- /dev/null
+++ b/drivers/pinctrl/sunxi/pinctrl-sun5i-a10s.c
@@ -0,0 +1,690 @@
+/*
+ * Allwinner A10s SoCs pinctrl driver.
+ *
+ * Copyright (C) 2014 Maxime Ripard
+ *
+ * Maxime Ripard <maxime.ripard@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-sunxi.h"
+
+static const struct sunxi_desc_pin sun5i_a10s_pins[] = {
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD3 */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* CLK */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD2 */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* ERR */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD1 */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* SYNC */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD0 */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* DLVD */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD3 */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* D0 */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD2 */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* D1 */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD1 */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* D2 */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD0 */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* D3 */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXCK */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* D4 */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* DTR */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXERR */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* D5 */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* DSR */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXDV */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* D6 */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* DCD */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* EMDC */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* D7 */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* RING */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* EMDIO */
+		  SUNXI_FUNCTION(0x3, "uart1"),		/* TX */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXEN */
+		  SUNXI_FUNCTION(0x3, "uart1"),		/* RX */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXCK */
+		  SUNXI_FUNCTION(0x3, "uart1"),		/* CTS */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* TX */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ECRS */
+		  SUNXI_FUNCTION(0x3, "uart1"),		/* RTS */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* RX */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ECOL */
+		  SUNXI_FUNCTION(0x3, "uart2")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXERR */
+		  SUNXI_FUNCTION(0x3, "uart2"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 31)),		/* EINT31 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "pwm"),		/* PWM0 */
+		  SUNXI_FUNCTION_IRQ(0x6, 16)),		/* EINT16 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ir0"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 17)),		/* EINT17 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ir0"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 18)),		/* EINT18 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s"),		/* MCLK */
+		  SUNXI_FUNCTION_IRQ(0x6, 19)),		/* EINT19 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s"),		/* BCLK */
+		  SUNXI_FUNCTION_IRQ(0x6, 20)),		/* EINT20 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s"),		/* LRCK */
+		  SUNXI_FUNCTION_IRQ(0x6, 21)),		/* EINT21 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s"),		/* DO */
+		  SUNXI_FUNCTION_IRQ(0x6, 22)),		/* EINT22 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s"),		/* DI */
+		  SUNXI_FUNCTION_IRQ(0x6, 23)),		/* EINT23 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS1 */
+		  SUNXI_FUNCTION_IRQ(0x6, 24)),		/* EINT24 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS0 */
+		  SUNXI_FUNCTION(0x3, "jtag"),		/* MS0 */
+		  SUNXI_FUNCTION_IRQ(0x6, 25)),		/* EINT25 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "jtag"),		/* CK0 */
+		  SUNXI_FUNCTION_IRQ(0x6, 26)),		/* EINT26 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "jtag"),		/* DO0 */
+		  SUNXI_FUNCTION_IRQ(0x6, 27)),		/* EINT27 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "jtag"),		/* DI0 */
+		  SUNXI_FUNCTION_IRQ(0x6, 28)),		/* EINT28 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart0"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 29)),		/* EINT29 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart0"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 30)),		/* EINT30 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NWE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MOSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NALE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MISO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCLE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE1 */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* CS0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NRE */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB0 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB1 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ0 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ1 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ2 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ3 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ4 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ5 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ6 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ7 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NWP */
+		  SUNXI_FUNCTION(0x4, "uart3")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE2 */
+		  SUNXI_FUNCTION(0x4, "uart3")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE3 */
+		  SUNXI_FUNCTION(0x3, "uart2"),		/* TX */
+		  SUNXI_FUNCTION(0x4, "uart3")),	/* CTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE4 */
+		  SUNXI_FUNCTION(0x3, "uart2"),		/* RX */
+		  SUNXI_FUNCTION(0x4, "uart3")),	/* RTS */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "uart2")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "uart2")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D4 */
+		  SUNXI_FUNCTION(0x3, "uart2")),	/* CTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D5 */
+		  SUNXI_FUNCTION(0x3, "uart2")),	/* RTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D6 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ECRS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D7 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ECOL */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D8 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D9 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D10 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXD0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D11 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXD1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D12 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXD2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D13 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXD3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D14 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D15 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXERR */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D16 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D17 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D18 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXDV */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D19 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXD0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D20 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXD1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D21 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXD2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D22 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXD3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D23 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXEN */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 25),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* DE */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXERR */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 26),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* HSYNC */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* EMDC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 27),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* VSYNC */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* EMDIO */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* PCK */
+		  SUNXI_FUNCTION(0x4, "spi2"),		/* CS0 */
+		  SUNXI_FUNCTION_IRQ(0x6, 14)),		/* EINT14 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* ERR */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* CK */
+		  SUNXI_FUNCTION(0x4, "spi2"),		/* CLK */
+		  SUNXI_FUNCTION_IRQ(0x6, 15)),		/* EINT15 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* SYNC */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* HSYNC */
+		  SUNXI_FUNCTION(0x4, "spi2")),		/* MOSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* DVLD */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* VSYNC */
+		  SUNXI_FUNCTION(0x4, "spi2")),		/* MISO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D0 */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D0 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D1 */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D2 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D3 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D4 */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D4 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D5 */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D5 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D6 */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D6 */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D7 */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D7 */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* RX */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* MS1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D0 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* DI1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CLK */
+		  SUNXI_FUNCTION(0x4, "uart0")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CMD */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* DO1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D3 */
+		  SUNXI_FUNCTION(0x4, "uart0")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D2 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* CK1 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x2, "gps"),		/* CLK */
+		  SUNXI_FUNCTION_IRQ(0x6, 0)),		/* EINT0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x2, "gps"),		/* SIGN */
+		  SUNXI_FUNCTION_IRQ(0x6, 1)),		/* EINT1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x2, "gps"),		/* MAG */
+		  SUNXI_FUNCTION_IRQ(0x6, 2)),		/* EINT2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* CMD */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 3)),		/* EINT3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* CLK */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 4)),		/* EINT4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* DO */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* CTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 5)),		/* EINT5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* RTS */
+		  SUNXI_FUNCTION(0x5, "uart2"),		/* RTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 6)),		/* EINT6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* D2 */
+		  SUNXI_FUNCTION(0x5, "uart2"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 7)),		/* EINT7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* D3 */
+		  SUNXI_FUNCTION(0x5, "uart2"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 8)),		/* EINT8 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS0 */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 9)),		/* EINT9 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 10)),		/* EINT10 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* CTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 11)),		/* EINT11 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* RTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 12)),		/* EINT12 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS1 */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* PWM1 */
+		  SUNXI_FUNCTION(0x5, "uart2"),		/* CTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 13)),		/* EINT13 */
+};
+
+static const struct sunxi_pinctrl_desc sun5i_a10s_pinctrl_data = {
+	.pins = sun5i_a10s_pins,
+	.npins = ARRAY_SIZE(sun5i_a10s_pins),
+};
+
+static int sun5i_a10s_pinctrl_probe(struct platform_device *pdev)
+{
+	return sunxi_pinctrl_init(pdev,
+				  &sun5i_a10s_pinctrl_data);
+}
+
+static struct of_device_id sun5i_a10s_pinctrl_match[] = {
+	{ .compatible = "allwinner,sun5i-a10s-pinctrl", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, sun5i_a10s_pinctrl_match);
+
+static struct platform_driver sun5i_a10s_pinctrl_driver = {
+	.probe	= sun5i_a10s_pinctrl_probe,
+	.driver	= {
+		.name		= "sun5i-a10s-pinctrl",
+		.owner		= THIS_MODULE,
+		.of_match_table	= sun5i_a10s_pinctrl_match,
+	},
+};
+module_platform_driver(sun5i_a10s_pinctrl_driver);
+
+MODULE_AUTHOR("Maxime Ripard <maxime.ripard@free-electrons.com");
+MODULE_DESCRIPTION("Allwinner A10s pinctrl driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun5i-a13.c b/drivers/pinctrl/sunxi/pinctrl-sun5i-a13.c
new file mode 100644
index 00000000000..1188a2b7b98
--- /dev/null
+++ b/drivers/pinctrl/sunxi/pinctrl-sun5i-a13.c
@@ -0,0 +1,411 @@
+/*
+ * Allwinner A13 SoCs pinctrl driver.
+ *
+ * Copyright (C) 2014 Maxime Ripard
+ *
+ * Maxime Ripard <maxime.ripard@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-sunxi.h"
+
+static const struct sunxi_desc_pin sun5i_a13_pins[] = {
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "pwm"),
+		  SUNXI_FUNCTION_IRQ(0x6, 16)),		/* EINT16 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ir0"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 17)),		/* EINT17 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ir0"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 18)),		/* EINT18 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS1 */
+		  SUNXI_FUNCTION_IRQ(0x6, 24)),		/* EINT24 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SDA */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NWE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MOSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NALE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MISO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCLE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE1 */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* CS0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NRE */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB0 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB1 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ0 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ1 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ2 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ3 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ4 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ5 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ6 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ7 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D7 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQS */
+		  SUNXI_FUNCTION(0x4, "uart3")),	/* RTS */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D7 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D10 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D11 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D12 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D13 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D14 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D15 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D18 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D19 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D20 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D21 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D22 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D23 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 25),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* DE */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 26),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* HSYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 27),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* VSYNC */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* PCLK */
+		  SUNXI_FUNCTION(0x4, "spi2"),		/* CS0 */
+		  SUNXI_FUNCTION_IRQ(0x6, 14)),		/* EINT14 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* MCLK */
+		  SUNXI_FUNCTION(0x4, "spi2"),		/* CLK */
+		  SUNXI_FUNCTION_IRQ(0x6, 15)),		/* EINT15 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* HSYNC */
+		  SUNXI_FUNCTION(0x4, "spi2")),		/* MOSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* VSYNC */
+		  SUNXI_FUNCTION(0x4, "spi2")),		/* MISO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D0 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D2 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D3 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D4 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D5 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D6 */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D7 */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* RX */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0")),		/* D2 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION_IRQ(0x6, 0)),		/* EINT0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION_IRQ(0x6, 1)),		/* EINT1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION_IRQ(0x6, 2)),		/* EINT2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* CMD */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 3)),		/* EINT3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* CLK */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 4)),		/* EINT4 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS0 */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 9)),		/* EINT9 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 10)),		/* EINT10 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* CTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 11)),		/* EINT11 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* RTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 12)),		/* EINT12 */
+};
+
+static const struct sunxi_pinctrl_desc sun5i_a13_pinctrl_data = {
+	.pins = sun5i_a13_pins,
+	.npins = ARRAY_SIZE(sun5i_a13_pins),
+};
+
+static int sun5i_a13_pinctrl_probe(struct platform_device *pdev)
+{
+	return sunxi_pinctrl_init(pdev,
+				  &sun5i_a13_pinctrl_data);
+}
+
+static struct of_device_id sun5i_a13_pinctrl_match[] = {
+	{ .compatible = "allwinner,sun5i-a13-pinctrl", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, sun5i_a13_pinctrl_match);
+
+static struct platform_driver sun5i_a13_pinctrl_driver = {
+	.probe	= sun5i_a13_pinctrl_probe,
+	.driver	= {
+		.name		= "sun5i-a13-pinctrl",
+		.owner		= THIS_MODULE,
+		.of_match_table	= sun5i_a13_pinctrl_match,
+	},
+};
+module_platform_driver(sun5i_a13_pinctrl_driver);
+
+MODULE_AUTHOR("Maxime Ripard <maxime.ripard@free-electrons.com");
+MODULE_DESCRIPTION("Allwinner A13 pinctrl driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun6i-a31-r.c b/drivers/pinctrl/sunxi/pinctrl-sun6i-a31-r.c
new file mode 100644
index 00000000000..8fcba48e0a4
--- /dev/null
+++ b/drivers/pinctrl/sunxi/pinctrl-sun6i-a31-r.c
@@ -0,0 +1,141 @@
+/*
+ * Allwinner A31 SoCs special pins pinctrl driver.
+ *
+ * Copyright (C) 2014 Boris Brezillon
+ * Boris Brezillon <boris.brezillon@free-electrons.com>
+ *
+ * Copyright (C) 2014 Maxime Ripard
+ * Maxime Ripard <maxime.ripard@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/reset.h>
+
+#include "pinctrl-sunxi.h"
+
+static const struct sunxi_desc_pin sun6i_a31_r_pins[] = {
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_twi"),		/* SCK */
+		  SUNXI_FUNCTION(0x3, "s_p2wi")),	/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_twi"),		/* SDA */
+		  SUNXI_FUNCTION(0x3, "s_p2wi")),	/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_uart")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_uart")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_ir")),		/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "s_jtag")),	/* MS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "s_jtag")),	/* CK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "s_jtag")),	/* DO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "s_jtag")),	/* DI */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "1wire")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "rtc")),		/* CLKO */
+};
+
+static const struct sunxi_pinctrl_desc sun6i_a31_r_pinctrl_data = {
+	.pins = sun6i_a31_r_pins,
+	.npins = ARRAY_SIZE(sun6i_a31_r_pins),
+	.pin_base = PL_BASE,
+};
+
+static int sun6i_a31_r_pinctrl_probe(struct platform_device *pdev)
+{
+	struct reset_control *rstc;
+	int ret;
+
+	rstc = devm_reset_control_get(&pdev->dev, NULL);
+	if (IS_ERR(rstc)) {
+		dev_err(&pdev->dev, "Reset controller missing\n");
+		return PTR_ERR(rstc);
+	}
+
+	ret = reset_control_deassert(rstc);
+	if (ret)
+		return ret;
+
+	ret = sunxi_pinctrl_init(pdev,
+				 &sun6i_a31_r_pinctrl_data);
+
+	if (ret)
+		reset_control_assert(rstc);
+
+	return ret;
+}
+
+static struct of_device_id sun6i_a31_r_pinctrl_match[] = {
+	{ .compatible = "allwinner,sun6i-a31-r-pinctrl", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, sun6i_a31_r_pinctrl_match);
+
+static struct platform_driver sun6i_a31_r_pinctrl_driver = {
+	.probe	= sun6i_a31_r_pinctrl_probe,
+	.driver	= {
+		.name		= "sun6i-a31-r-pinctrl",
+		.owner		= THIS_MODULE,
+		.of_match_table	= sun6i_a31_r_pinctrl_match,
+	},
+};
+module_platform_driver(sun6i_a31_r_pinctrl_driver);
+
+MODULE_AUTHOR("Boris Brezillon <boris.brezillon@free-electrons.com");
+MODULE_AUTHOR("Maxime Ripard <maxime.ripard@free-electrons.com");
+MODULE_DESCRIPTION("Allwinner A31 R_PIO pinctrl driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun6i-a31.c b/drivers/pinctrl/sunxi/pinctrl-sun6i-a31.c
new file mode 100644
index 00000000000..8dea5856458
--- /dev/null
+++ b/drivers/pinctrl/sunxi/pinctrl-sun6i-a31.c
@@ -0,0 +1,865 @@
+/*
+ * Allwinner A31 SoCs pinctrl driver.
+ *
+ * Copyright (C) 2014 Maxime Ripard
+ *
+ * Maxime Ripard <maxime.ripard@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-sunxi.h"
+
+static const struct sunxi_desc_pin sun6i_a31_pins[] = {
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD0 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D0 */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* DTR */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD1 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* DSR */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD2 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D2 */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* DCD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD3 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D3 */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* RING */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD4 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D4 */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD5 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D5 */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD6 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D6 */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* RTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD7 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D7 */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* CTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXCLK */
+		  SUNXI_FUNCTION(0x3, "lcd1")),		/* D8 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXEN */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D9 */
+		  SUNXI_FUNCTION(0x4, "mmc3"),		/* CMD */
+		  SUNXI_FUNCTION(0x5, "mmc2")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* GTXCLK */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D10 */
+		  SUNXI_FUNCTION(0x4, "mmc3"),		/* CLK */
+		  SUNXI_FUNCTION(0x5, "mmc2")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD0 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D11 */
+		  SUNXI_FUNCTION(0x4, "mmc3"),		/* D0 */
+		  SUNXI_FUNCTION(0x5, "mmc2")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD1 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D12 */
+		  SUNXI_FUNCTION(0x4, "mmc3"),		/* D1 */
+		  SUNXI_FUNCTION(0x5, "mmc2")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD2 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D13 */
+		  SUNXI_FUNCTION(0x4, "mmc3"),		/* D2 */
+		  SUNXI_FUNCTION(0x5, "mmc2")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD3 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D14 */
+		  SUNXI_FUNCTION(0x4, "mmc3"),		/* D3 */
+		  SUNXI_FUNCTION(0x5, "mmc2")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD4 */
+		  SUNXI_FUNCTION(0x3, "lcd1")),		/* D15 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD5 */
+		  SUNXI_FUNCTION(0x3, "lcd1")),		/* D16 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD6 */
+		  SUNXI_FUNCTION(0x3, "lcd1")),		/* D17 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD7 */
+		  SUNXI_FUNCTION(0x3, "lcd1")),		/* D18 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXDV */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D19 */
+		  SUNXI_FUNCTION(0x4, "pwm3")),		/* Positive */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXCLK */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D20 */
+		  SUNXI_FUNCTION(0x4, "pwm3")),		/* Negative */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXERR */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D21 */
+		  SUNXI_FUNCTION(0x4, "spi3")),		/* CS0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXERR */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D22 */
+		  SUNXI_FUNCTION(0x4, "spi3")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* COL */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D23 */
+		  SUNXI_FUNCTION(0x4, "spi3")),		/* MOSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* CRS */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* CLK */
+		  SUNXI_FUNCTION(0x4, "spi3")),		/* MISO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 25),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* CLKIN */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* DE */
+		  SUNXI_FUNCTION(0x4, "spi3")),		/* CS1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 26),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* MDC */
+		  SUNXI_FUNCTION(0x3, "lcd1")),		/* HSYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 27),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* MDIO */
+		  SUNXI_FUNCTION(0x3, "lcd1")),		/* VSYNC */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* MCLK */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* CTS */
+		  SUNXI_FUNCTION(0x4, "csi")),		/* MCLK1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0")),		/* BCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0")),		/* LRCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0")),		/* DO0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DO1 */
+		  SUNXI_FUNCTION(0x3, "uart3")),	/* RTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DO2 */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* TX */
+		  SUNXI_FUNCTION(0x4, "i2c3")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DO3 */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* RX */
+		  SUNXI_FUNCTION(0x4, "i2c3")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "i2s0")),		/* DI */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* WE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MOSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* ALE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MISO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* CLE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* CE1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* CE0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* RE */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* RB0 */
+		  SUNXI_FUNCTION(0x3, "mmc2"),		/* CMD */
+		  SUNXI_FUNCTION(0x4, "mmc3")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* RB1 */
+		  SUNXI_FUNCTION(0x3, "mmc2"),		/* CLK */
+		  SUNXI_FUNCTION(0x4, "mmc3")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ0 */
+		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D0 */
+		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ1 */
+		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ2 */
+		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D2 */
+		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ3 */
+		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D3 */
+		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ4 */
+		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D4 */
+		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ5 */
+		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D5 */
+		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ6 */
+		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D6 */
+		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ7 */
+		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D7 */
+		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ8 */
+		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ9 */
+		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ10 */
+		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ11 */
+		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ12 */
+		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ13 */
+		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ14 */
+		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ15 */
+		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQS */
+		  SUNXI_FUNCTION(0x3, "mmc2"),		/* RST */
+		  SUNXI_FUNCTION(0x4, "mmc3")),		/* RST */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 25),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* CE2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 26),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* CE3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 27),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* CS0 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D0 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D1 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D4 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D5 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D6 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VPC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D7 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D8 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D9 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D10 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D11 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D12 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D13 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D14 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D15 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D16 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VPC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D17 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D18 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D19 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D20 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D21 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D22 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D23 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 25),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* DE */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 26),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* HSYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 27),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* VSYNC */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* PCLK */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* MCLK */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* ERR */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* HSYNC */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* SYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* VSYNC */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* DVLD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D0 */
+		  SUNXI_FUNCTION(0x3, "uart5")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D1 */
+		  SUNXI_FUNCTION(0x3, "uart5")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "uart5")),	/* RTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "uart5")),	/* CTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D4 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D5 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D6 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D7 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D8 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D9 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D10 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D11 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi")),		/* MIPI CSI MCLK */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* MS1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D0 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* DI1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CLK */
+		  SUNXI_FUNCTION(0x4, "uart0")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CMD */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* DO1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D3 */
+		  SUNXI_FUNCTION(0x4, "uart0")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D2 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* CK1 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart2")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart2")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart2")),	/* RTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart2")),	/* CTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c3"),		/* SCK */
+		  SUNXI_FUNCTION(0x3, "usb")),		/* DP3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c3"),		/* SDA */
+		  SUNXI_FUNCTION(0x3, "usb")),		/* DM3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS1 */
+		  SUNXI_FUNCTION(0x3, "i2s1")),		/* MCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS0 */
+		  SUNXI_FUNCTION(0x3, "i2s1")),		/* BCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "i2s1")),		/* LRCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "i2s1")),		/* DIN */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "i2s1")),		/* DOUT */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart4")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart4")),	/* RX */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand1")),	/* WE */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand1")),	/* ALE */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand1")),	/* CLE */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand1")),	/* CE1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand1")),	/* CE0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand1")),	/* RE */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand1")),	/* RB0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand1")),	/* RB1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand1")),	/* DQS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS0 */
+		  SUNXI_FUNCTION(0x3, "jtag"),		/* MS0 */
+		  SUNXI_FUNCTION(0x4, "pwm1")),		/* Positive */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "jtag"),		/* CK0 */
+		  SUNXI_FUNCTION(0x4, "pwm1")),		/* Negative */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "jtag"),		/* DO0 */
+		  SUNXI_FUNCTION(0x4, "pwm2")),		/* Positive */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "jtag"),		/* DI0 */
+		  SUNXI_FUNCTION(0x4, "pwm2")),		/* Negative */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "pwm0")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart0")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart0")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 25),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 26),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 27),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 28),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 29),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand1")),	/* CE2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 30),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand1")),	/* CE3 */
+};
+
+static const struct sunxi_pinctrl_desc sun6i_a31_pinctrl_data = {
+	.pins = sun6i_a31_pins,
+	.npins = ARRAY_SIZE(sun6i_a31_pins),
+};
+
+static int sun6i_a31_pinctrl_probe(struct platform_device *pdev)
+{
+	return sunxi_pinctrl_init(pdev,
+				  &sun6i_a31_pinctrl_data);
+}
+
+static struct of_device_id sun6i_a31_pinctrl_match[] = {
+	{ .compatible = "allwinner,sun6i-a31-pinctrl", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, sun6i_a31_pinctrl_match);
+
+static struct platform_driver sun6i_a31_pinctrl_driver = {
+	.probe	= sun6i_a31_pinctrl_probe,
+	.driver	= {
+		.name		= "sun6i-a31-pinctrl",
+		.owner		= THIS_MODULE,
+		.of_match_table	= sun6i_a31_pinctrl_match,
+	},
+};
+module_platform_driver(sun6i_a31_pinctrl_driver);
+
+MODULE_AUTHOR("Maxime Ripard <maxime.ripard@free-electrons.com");
+MODULE_DESCRIPTION("Allwinner A31 pinctrl driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun7i-a20.c b/drivers/pinctrl/sunxi/pinctrl-sun7i-a20.c
new file mode 100644
index 00000000000..d8577ce5f1a
--- /dev/null
+++ b/drivers/pinctrl/sunxi/pinctrl-sun7i-a20.c
@@ -0,0 +1,1065 @@
+/*
+ * Allwinner A20 SoCs pinctrl driver.
+ *
+ * Copyright (C) 2014 Maxime Ripard
+ *
+ * Maxime Ripard <maxime.ripard@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-sunxi.h"
+
+static const struct sunxi_desc_pin sun7i_a20_pins[] = {
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD3 */
+		  SUNXI_FUNCTION(0x3, "spi1"),		/* CS0 */
+		  SUNXI_FUNCTION(0x4, "uart2"),		/* RTS */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXD3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD2 */
+		  SUNXI_FUNCTION(0x3, "spi1"),		/* CLK */
+		  SUNXI_FUNCTION(0x4, "uart2"),		/* CTS */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXD2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD1 */
+		  SUNXI_FUNCTION(0x3, "spi1"),		/* MOSI */
+		  SUNXI_FUNCTION(0x4, "uart2"),		/* TX */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXD1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD0 */
+		  SUNXI_FUNCTION(0x3, "spi1"),		/* MISO */
+		  SUNXI_FUNCTION(0x4, "uart2"),		/* RX */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXD0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD3 */
+		  SUNXI_FUNCTION(0x3, "spi1"),		/* CS1 */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* GTXD3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD2 */
+		  SUNXI_FUNCTION(0x3, "spi3"),		/* CS0 */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* GTXD2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD1 */
+		  SUNXI_FUNCTION(0x3, "spi3"),		/* CLK */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* GTXD1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD0 */
+		  SUNXI_FUNCTION(0x3, "spi3"),		/* MOSI */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* GTXD0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXCK */
+		  SUNXI_FUNCTION(0x3, "spi3"),		/* MISO */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXERR */
+		  SUNXI_FUNCTION(0x3, "spi3"),		/* CS1 */
+		  SUNXI_FUNCTION(0x5, "gmac"),		/* GNULL / ERXERR */
+		  SUNXI_FUNCTION(0x6, "i2s1")),		/* MCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXDV */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* TX */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXCTL / ERXDV */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* EMDC */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* RX */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* EMDC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* EMDIO */
+		  SUNXI_FUNCTION(0x3, "uart6"),		/* TX */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* RTS */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* EMDIO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXEN */
+		  SUNXI_FUNCTION(0x3, "uart6"),		/* RX */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* CTS */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* GTXCTL / ETXEN */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXCK */
+		  SUNXI_FUNCTION(0x3, "uart7"),		/* TX */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* DTR */
+		  SUNXI_FUNCTION(0x5, "gmac"),		/* GNULL / ETXCK */
+		  SUNXI_FUNCTION(0x6, "i2s1")),		/* BCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ECRS */
+		  SUNXI_FUNCTION(0x3, "uart7"),		/* RX */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* DSR */
+		  SUNXI_FUNCTION(0x5, "gmac"),		/* GTXCK / ECRS */
+		  SUNXI_FUNCTION(0x6, "i2s1")),		/* LRCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ECOL */
+		  SUNXI_FUNCTION(0x3, "can"),		/* TX */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* DCD */
+		  SUNXI_FUNCTION(0x5, "gmac"),		/* GCLKIN / ECOL */
+		  SUNXI_FUNCTION(0x6, "i2s1")),		/* DO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXERR */
+		  SUNXI_FUNCTION(0x3, "can"),		/* RX */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* RING */
+		  SUNXI_FUNCTION(0x5, "gmac"),		/* GNULL / ETXERR */
+		  SUNXI_FUNCTION(0x6, "i2s1")),		/* LRCK */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "pwm")),		/* PWM0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ir0"),		/* TX */
+		  SUNXI_FUNCTION(0x4, "spdif")),	/* MCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ir0")),		/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* MCLK */
+		  SUNXI_FUNCTION(0x3, "ac97")),		/* MCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* BCLK */
+		  SUNXI_FUNCTION(0x3, "ac97")),		/* BCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* LRCK */
+		  SUNXI_FUNCTION(0x3, "ac97")),		/* SYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DO0 */
+		  SUNXI_FUNCTION(0x3, "ac97")),		/* DO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0")),		/* DO1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0")),		/* DO2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0")),		/* DO3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DI */
+		  SUNXI_FUNCTION(0x3, "ac97"),		/* DI */
+		  SUNXI_FUNCTION(0x4, "spdif")),	/* DI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS1 */
+		  SUNXI_FUNCTION(0x4, "spdif")),	/* DO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS0 */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* MS0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* CK0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* DO0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* DI0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart0"),		/* TX */
+		  SUNXI_FUNCTION(0x3, "ir1")),		/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart0"),		/* RX */
+		  SUNXI_FUNCTION(0x3, "ir1")),		/* RX */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NWE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MOSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NALE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MISO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCLE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NRE# */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB0 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB1 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ0 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ1 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ2 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ3 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NWP */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE4 */
+		  SUNXI_FUNCTION(0x3, "spi2"),		/* CS0 */
+		  SUNXI_FUNCTION_IRQ(0x6, 12)),		/* EINT12 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE5 */
+		  SUNXI_FUNCTION(0x3, "spi2"),		/* CLK */
+		  SUNXI_FUNCTION_IRQ(0x6, 13)),		/* EINT13 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE6 */
+		  SUNXI_FUNCTION(0x3, "spi2"),		/* MOSI */
+		  SUNXI_FUNCTION_IRQ(0x6, 14)),		/* EINT14 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE7 */
+		  SUNXI_FUNCTION(0x3, "spi2"),		/* MISO */
+		  SUNXI_FUNCTION_IRQ(0x6, 15)),		/* EINT15 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* CS0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQS */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D0 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D1 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D4 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D5 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D6 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VPC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D7 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D8 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D9 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VM3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D10 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D11 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D12 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D13 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D14 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D15 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D16 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VPC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D17 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D18 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D19 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D20 */
+		  SUNXI_FUNCTION(0x3, "csi1")),		/* MCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D21 */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* VPPEN */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D22 */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* VPPPP */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D23 */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* DET */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* VCCEN */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 25),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* DE */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* RST */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 26),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* HSYNC */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 27),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* VSYNC */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* SDA */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* PCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* ERR */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* CK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* SYNC */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* HSYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* DVLD */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* VSYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D0 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D1 */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "sim")),		/* VPPEN */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D4 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D5 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D6 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D7 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D7 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* MSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D0 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* DI1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CLK */
+		  SUNXI_FUNCTION(0x4, "uart0")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CMD */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* DO1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D3 */
+		  SUNXI_FUNCTION(0x4, "uart0")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D2 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* CK1 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* PCK */
+		  SUNXI_FUNCTION(0x4, "mmc1")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* ERR */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* CK */
+		  SUNXI_FUNCTION(0x4, "mmc1")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* SYNC */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* HSYNC */
+		  SUNXI_FUNCTION(0x4, "mmc1")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* DVLD */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* VSYNC */
+		  SUNXI_FUNCTION(0x4, "mmc1")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* D0 */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* D0 */
+		  SUNXI_FUNCTION(0x4, "mmc1"),		/* D2 */
+		  SUNXI_FUNCTION(0x5, "csi0")),		/* D8 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* D1 */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "mmc1"),		/* D3 */
+		  SUNXI_FUNCTION(0x5, "csi0")),		/* D9 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* D2 */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* TX */
+		  SUNXI_FUNCTION(0x5, "csi0")),		/* D10 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* D3 */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* RX */
+		  SUNXI_FUNCTION(0x5, "csi0")),		/* D11 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* D4 */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* D4 */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* RTS */
+		  SUNXI_FUNCTION(0x5, "csi0")),		/* D12 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* D5 */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* D5 */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* CTS */
+		  SUNXI_FUNCTION(0x5, "csi0")),		/* D13 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* D6 */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* D6 */
+		  SUNXI_FUNCTION(0x4, "uart4"),		/* TX */
+		  SUNXI_FUNCTION(0x5, "csi0")),		/* D14 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts1"),		/* D7 */
+		  SUNXI_FUNCTION(0x3, "csi1"),		/* D7 */
+		  SUNXI_FUNCTION(0x4, "uart4"),		/* RX */
+		  SUNXI_FUNCTION(0x5, "csi0")),		/* D15 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D0 */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 0),		/* EINT0 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 1),		/* EINT1 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D2 */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* RTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 2),		/* EINT2 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D3 */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* CTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 3),		/* EINT3 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D4 */
+		  SUNXI_FUNCTION(0x4, "uart4"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 4),		/* EINT4 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D5 */
+		  SUNXI_FUNCTION(0x4, "uart4"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 5),		/* EINT5 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D6 */
+		  SUNXI_FUNCTION(0x4, "uart5"),		/* TX */
+		  SUNXI_FUNCTION(0x5, "ms"),		/* BS */
+		  SUNXI_FUNCTION_IRQ(0x6, 6),		/* EINT6 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D7 */
+		  SUNXI_FUNCTION(0x4, "uart5"),		/* RX */
+		  SUNXI_FUNCTION(0x5, "ms"),		/* CLK */
+		  SUNXI_FUNCTION_IRQ(0x6, 7),		/* EINT7 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D8 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXD3 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN0 */
+		  SUNXI_FUNCTION(0x5, "ms"),		/* D0 */
+		  SUNXI_FUNCTION_IRQ(0x6, 8),		/* EINT8 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D8 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D9 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXD2 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN1 */
+		  SUNXI_FUNCTION(0x5, "ms"),		/* D1 */
+		  SUNXI_FUNCTION_IRQ(0x6, 9),		/* EINT9 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D9 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D10 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXD1 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN2 */
+		  SUNXI_FUNCTION(0x5, "ms"),		/* D2 */
+		  SUNXI_FUNCTION_IRQ(0x6, 10),		/* EINT10 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D10 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D11 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXD0 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN3 */
+		  SUNXI_FUNCTION(0x5, "ms"),		/* D3 */
+		  SUNXI_FUNCTION_IRQ(0x6, 11),		/* EINT11 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D11 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D12 */
+		  SUNXI_FUNCTION(0x4, "ps2"),		/* SCK1 */
+		  SUNXI_FUNCTION_IRQ(0x6, 12),		/* EINT12 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D12 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D13 */
+		  SUNXI_FUNCTION(0x4, "ps2"),		/* SDA1 */
+		  SUNXI_FUNCTION(0x5, "sim"),		/* RST */
+		  SUNXI_FUNCTION_IRQ(0x6, 13),		/* EINT13 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D13 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D14 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD3 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN4 */
+		  SUNXI_FUNCTION(0x5, "sim"),		/* VPPEN */
+		  SUNXI_FUNCTION_IRQ(0x6, 14),		/* EINT14 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D14 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D15 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD3 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN5 */
+		  SUNXI_FUNCTION(0x5, "sim"),		/* VPPPP */
+		  SUNXI_FUNCTION_IRQ(0x6, 15),		/* EINT15 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D15 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D16 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD2 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN6 */
+		  SUNXI_FUNCTION_IRQ(0x6, 16),		/* EINT16 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D16 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D17 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD1 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN7 */
+		  SUNXI_FUNCTION(0x5, "sim"),		/* VCCEN */
+		  SUNXI_FUNCTION_IRQ(0x6, 17),		/* EINT17 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D17 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D18 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD0 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT0 */
+		  SUNXI_FUNCTION(0x5, "sim"),		/* SCK */
+		  SUNXI_FUNCTION_IRQ(0x6, 18),		/* EINT18 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D18 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D19 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXERR */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT1 */
+		  SUNXI_FUNCTION(0x5, "sim"),		/* SDA */
+		  SUNXI_FUNCTION_IRQ(0x6, 19),		/* EINT19 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D19 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D20 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXDV */
+		  SUNXI_FUNCTION(0x4, "can"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 20),		/* EINT20 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D20 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D21 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* EMDC */
+		  SUNXI_FUNCTION(0x4, "can"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 21),		/* EINT21 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D21 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D22 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* EMDIO */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT2 */
+		  SUNXI_FUNCTION(0x5, "mmc1"),		/* CMD */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D22 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D23 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXEN */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT3 */
+		  SUNXI_FUNCTION(0x5, "mmc1"),		/* CLK */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D23 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXCK */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT4 */
+		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D0 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* PCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 25),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* DE */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ECRS */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT5 */
+		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D1 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* FIELD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 26),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* HSYNC */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ECOL */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT6 */
+		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D2 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* HSYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 27),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* VSYNC */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXERR */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT7 */
+		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D3 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* VSYNC */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "i2c3")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "i2c3")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "i2c4")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "pwm"),		/* PWM1 */
+		  SUNXI_FUNCTION(0x3, "i2c4")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* CS0 */
+		  SUNXI_FUNCTION(0x3, "uart5"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x5, 22)),		/* EINT22 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "uart5"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x5, 23)),		/* EINT23 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "uart6"),		/* TX */
+		  SUNXI_FUNCTION(0x4, "clk_out_a"),	/* CLK_OUT_A */
+		  SUNXI_FUNCTION_IRQ(0x5, 24)),		/* EINT24 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "uart6"),		/* RX */
+		  SUNXI_FUNCTION(0x4, "clk_out_b"),	/* CLK_OUT_B */
+		  SUNXI_FUNCTION_IRQ(0x5, 25)),		/* EINT25 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* CS1 */
+		  SUNXI_FUNCTION(0x3, "ps2"),		/* SCK1 */
+		  SUNXI_FUNCTION(0x4, "timer4"),	/* TCLKIN0 */
+		  SUNXI_FUNCTION_IRQ(0x5, 26)),		/* EINT26 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS1 */
+		  SUNXI_FUNCTION(0x3, "ps2"),		/* SDA1 */
+		  SUNXI_FUNCTION(0x4, "timer5"),	/* TCLKIN1 */
+		  SUNXI_FUNCTION_IRQ(0x5, 27)),		/* EINT27 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS0 */
+		  SUNXI_FUNCTION(0x3, "uart2"),		/* RTS */
+		  SUNXI_FUNCTION_IRQ(0x5, 28)),		/* EINT28 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "uart2"),		/* CTS */
+		  SUNXI_FUNCTION_IRQ(0x5, 29)),		/* EINT29 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "uart2"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x5, 30)),		/* EINT30 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "uart2"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x5, 31)),		/* EINT31 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ps2"),		/* SCK0 */
+		  SUNXI_FUNCTION(0x3, "uart7"),		/* TX */
+		  SUNXI_FUNCTION(0x4, "hdmi")),		/* HSCL */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ps2"),		/* SDA0 */
+		  SUNXI_FUNCTION(0x3, "uart7"),		/* RX */
+		  SUNXI_FUNCTION(0x4, "hdmi")),		/* HSDA */
+};
+
+static const struct sunxi_pinctrl_desc sun7i_a20_pinctrl_data = {
+	.pins = sun7i_a20_pins,
+	.npins = ARRAY_SIZE(sun7i_a20_pins),
+};
+
+static int sun7i_a20_pinctrl_probe(struct platform_device *pdev)
+{
+	return sunxi_pinctrl_init(pdev,
+				  &sun7i_a20_pinctrl_data);
+}
+
+static struct of_device_id sun7i_a20_pinctrl_match[] = {
+	{ .compatible = "allwinner,sun7i-a20-pinctrl", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, sun7i_a20_pinctrl_match);
+
+static struct platform_driver sun7i_a20_pinctrl_driver = {
+	.probe	= sun7i_a20_pinctrl_probe,
+	.driver	= {
+		.name		= "sun7i-a20-pinctrl",
+		.owner		= THIS_MODULE,
+		.of_match_table	= sun7i_a20_pinctrl_match,
+	},
+};
+module_platform_driver(sun7i_a20_pinctrl_driver);
+
+MODULE_AUTHOR("Maxime Ripard <maxime.ripard@free-electrons.com");
+MODULE_DESCRIPTION("Allwinner A20 pinctrl driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/pinctrl-sunxi.c b/drivers/pinctrl/sunxi/pinctrl-sunxi.c
index f9fabe9bf47..f1ca75e6d7b 100644
--- a/drivers/pinctrl/pinctrl-sunxi.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sunxi.c
@@ -28,9 +28,8 @@
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 
-#include "core.h"
+#include "../core.h"
 #include "pinctrl-sunxi.h"
-#include "pinctrl-sunxi-pins.h"
 
 static struct sunxi_pinctrl_group *
 sunxi_pinctrl_find_group_by_name(struct sunxi_pinctrl *pctl, const char *group)
@@ -281,6 +280,7 @@ static int sunxi_pconf_group_set(struct pinctrl_dev *pctldev,
 	struct sunxi_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
 	struct sunxi_pinctrl_group *g = &pctl->groups[group];
 	unsigned long flags;
+	unsigned pin = g->pin - pctl->desc->pin_base;
 	u32 val, mask;
 	u16 strength;
 	u8 dlevel;
@@ -304,23 +304,23 @@ static int sunxi_pconf_group_set(struct pinctrl_dev *pctldev,
 			 *   3: 40mA
 			 */
 			dlevel = strength / 10 - 1;
-			val = readl(pctl->membase + sunxi_dlevel_reg(g->pin));
-			mask = DLEVEL_PINS_MASK << sunxi_dlevel_offset(g->pin);
+			val = readl(pctl->membase + sunxi_dlevel_reg(pin));
+			mask = DLEVEL_PINS_MASK << sunxi_dlevel_offset(pin);
 			writel((val & ~mask)
-				| dlevel << sunxi_dlevel_offset(g->pin),
-				pctl->membase + sunxi_dlevel_reg(g->pin));
+				| dlevel << sunxi_dlevel_offset(pin),
+				pctl->membase + sunxi_dlevel_reg(pin));
 			break;
 		case PIN_CONFIG_BIAS_PULL_UP:
-			val = readl(pctl->membase + sunxi_pull_reg(g->pin));
-			mask = PULL_PINS_MASK << sunxi_pull_offset(g->pin);
-			writel((val & ~mask) | 1 << sunxi_pull_offset(g->pin),
-				pctl->membase + sunxi_pull_reg(g->pin));
+			val = readl(pctl->membase + sunxi_pull_reg(pin));
+			mask = PULL_PINS_MASK << sunxi_pull_offset(pin);
+			writel((val & ~mask) | 1 << sunxi_pull_offset(pin),
+				pctl->membase + sunxi_pull_reg(pin));
 			break;
 		case PIN_CONFIG_BIAS_PULL_DOWN:
-			val = readl(pctl->membase + sunxi_pull_reg(g->pin));
-			mask = PULL_PINS_MASK << sunxi_pull_offset(g->pin);
-			writel((val & ~mask) | 2 << sunxi_pull_offset(g->pin),
-				pctl->membase + sunxi_pull_reg(g->pin));
+			val = readl(pctl->membase + sunxi_pull_reg(pin));
+			mask = PULL_PINS_MASK << sunxi_pull_offset(pin);
+			writel((val & ~mask) | 2 << sunxi_pull_offset(pin),
+				pctl->membase + sunxi_pull_reg(pin));
 			break;
 		default:
 			break;
@@ -377,6 +377,7 @@ static void sunxi_pmx_set(struct pinctrl_dev *pctldev,
 
 	spin_lock_irqsave(&pctl->lock, flags);
 
+	pin -= pctl->desc->pin_base;
 	val = readl(pctl->membase + sunxi_mux_reg(pin));
 	mask = MUX_PINS_MASK << sunxi_mux_offset(pin);
 	writel((val & ~mask) | config << sunxi_mux_offset(pin),
@@ -437,12 +438,6 @@ static const struct pinmux_ops sunxi_pmx_ops = {
 	.gpio_set_direction	= sunxi_pmx_gpio_set_direction,
 };
 
-static struct pinctrl_desc sunxi_pctrl_desc = {
-	.confops	= &sunxi_pconf_ops,
-	.pctlops	= &sunxi_pctrl_ops,
-	.pmxops		= &sunxi_pmx_ops,
-};
-
 static int sunxi_pinctrl_gpio_request(struct gpio_chip *chip, unsigned offset)
 {
 	return pinctrl_request_gpio(chip->base + offset);
@@ -530,27 +525,12 @@ static int sunxi_pinctrl_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 	if (!desc)
 		return -EINVAL;
 
-	pctl->irq_array[desc->irqnum] = offset;
-
 	dev_dbg(chip->dev, "%s: request IRQ for GPIO %d, return %d\n",
 		chip->label, offset + chip->base, desc->irqnum);
 
 	return irq_find_mapping(pctl->domain, desc->irqnum);
 }
 
-static struct gpio_chip sunxi_pinctrl_gpio_chip = {
-	.owner			= THIS_MODULE,
-	.request		= sunxi_pinctrl_gpio_request,
-	.free			= sunxi_pinctrl_gpio_free,
-	.direction_input	= sunxi_pinctrl_gpio_direction_input,
-	.direction_output	= sunxi_pinctrl_gpio_direction_output,
-	.get			= sunxi_pinctrl_gpio_get,
-	.set			= sunxi_pinctrl_gpio_set,
-	.of_xlate		= sunxi_pinctrl_gpio_of_xlate,
-	.to_irq			= sunxi_pinctrl_gpio_to_irq,
-	.of_gpio_n_cells	= 3,
-	.can_sleep		= false,
-};
 
 static int sunxi_pinctrl_irq_set_type(struct irq_data *d,
 				      unsigned int type)
@@ -685,16 +665,6 @@ static void sunxi_pinctrl_irq_handler(unsigned irq, struct irq_desc *desc)
 	}
 }
 
-static struct of_device_id sunxi_pinctrl_match[] = {
-	{ .compatible = "allwinner,sun4i-a10-pinctrl", .data = (void *)&sun4i_a10_pinctrl_data },
-	{ .compatible = "allwinner,sun5i-a10s-pinctrl", .data = (void *)&sun5i_a10s_pinctrl_data },
-	{ .compatible = "allwinner,sun5i-a13-pinctrl", .data = (void *)&sun5i_a13_pinctrl_data },
-	{ .compatible = "allwinner,sun6i-a31-pinctrl", .data = (void *)&sun6i_a31_pinctrl_data },
-	{ .compatible = "allwinner,sun7i-a20-pinctrl", .data = (void *)&sun7i_a20_pinctrl_data },
-	{}
-};
-MODULE_DEVICE_TABLE(of, sunxi_pinctrl_match);
-
 static int sunxi_pinctrl_add_function(struct sunxi_pinctrl *pctl,
 					const char *name)
 {
@@ -755,6 +725,9 @@ static int sunxi_pinctrl_build_state(struct platform_device *pdev)
 		struct sunxi_desc_function *func = pin->functions;
 
 		while (func->name) {
+			/* Create interrupt mapping while we're at it */
+			if (!strcmp(func->name, "irq"))
+				pctl->irq_array[func->irqnum] = pin->pin.number;
 			sunxi_pinctrl_add_function(pctl, func->name);
 			func++;
 		}
@@ -798,12 +771,14 @@ static int sunxi_pinctrl_build_state(struct platform_device *pdev)
 	return 0;
 }
 
-static int sunxi_pinctrl_probe(struct platform_device *pdev)
+int sunxi_pinctrl_init(struct platform_device *pdev,
+		       const struct sunxi_pinctrl_desc *desc)
 {
 	struct device_node *node = pdev->dev.of_node;
-	const struct of_device_id *device;
+	struct pinctrl_desc *pctrl_desc;
 	struct pinctrl_pin_desc *pins;
 	struct sunxi_pinctrl *pctl;
+	struct resource *res;
 	int i, ret, last_pin;
 	struct clk *clk;
 
@@ -814,15 +789,13 @@ static int sunxi_pinctrl_probe(struct platform_device *pdev)
 
 	spin_lock_init(&pctl->lock);
 
-	pctl->membase = of_iomap(node, 0);
-	if (!pctl->membase)
-		return -ENOMEM;
-
-	device = of_match_device(sunxi_pinctrl_match, &pdev->dev);
-	if (!device)
-		return -ENODEV;
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	pctl->membase = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(pctl->membase))
+		return PTR_ERR(pctl->membase);
 
-	pctl->desc = (struct sunxi_pinctrl_desc *)device->data;
+	pctl->dev = &pdev->dev;
+	pctl->desc = desc;
 
 	ret = sunxi_pinctrl_build_state(pdev);
 	if (ret) {
@@ -839,12 +812,21 @@ static int sunxi_pinctrl_probe(struct platform_device *pdev)
 	for (i = 0; i < pctl->desc->npins; i++)
 		pins[i] = pctl->desc->pins[i].pin;
 
-	sunxi_pctrl_desc.name = dev_name(&pdev->dev);
-	sunxi_pctrl_desc.owner = THIS_MODULE;
-	sunxi_pctrl_desc.pins = pins;
-	sunxi_pctrl_desc.npins = pctl->desc->npins;
-	pctl->dev = &pdev->dev;
-	pctl->pctl_dev = pinctrl_register(&sunxi_pctrl_desc,
+	pctrl_desc = devm_kzalloc(&pdev->dev,
+				  sizeof(*pctrl_desc),
+				  GFP_KERNEL);
+	if (!pctrl_desc)
+		return -ENOMEM;
+
+	pctrl_desc->name = dev_name(&pdev->dev);
+	pctrl_desc->owner = THIS_MODULE;
+	pctrl_desc->pins = pins;
+	pctrl_desc->npins = pctl->desc->npins;
+	pctrl_desc->confops = &sunxi_pconf_ops;
+	pctrl_desc->pctlops = &sunxi_pctrl_ops;
+	pctrl_desc->pmxops =  &sunxi_pmx_ops;
+
+	pctl->pctl_dev = pinctrl_register(pctrl_desc,
 					  &pdev->dev, pctl);
 	if (!pctl->pctl_dev) {
 		dev_err(&pdev->dev, "couldn't register pinctrl driver\n");
@@ -858,11 +840,22 @@ static int sunxi_pinctrl_probe(struct platform_device *pdev)
 	}
 
 	last_pin = pctl->desc->pins[pctl->desc->npins - 1].pin.number;
-	pctl->chip = &sunxi_pinctrl_gpio_chip;
-	pctl->chip->ngpio = round_up(last_pin, PINS_PER_BANK);
+	pctl->chip->owner = THIS_MODULE;
+	pctl->chip->request = sunxi_pinctrl_gpio_request,
+	pctl->chip->free = sunxi_pinctrl_gpio_free,
+	pctl->chip->direction_input = sunxi_pinctrl_gpio_direction_input,
+	pctl->chip->direction_output = sunxi_pinctrl_gpio_direction_output,
+	pctl->chip->get = sunxi_pinctrl_gpio_get,
+	pctl->chip->set = sunxi_pinctrl_gpio_set,
+	pctl->chip->of_xlate = sunxi_pinctrl_gpio_of_xlate,
+	pctl->chip->to_irq = sunxi_pinctrl_gpio_to_irq,
+	pctl->chip->of_gpio_n_cells = 3,
+	pctl->chip->can_sleep = false,
+	pctl->chip->ngpio = round_up(last_pin, PINS_PER_BANK) -
+			    pctl->desc->pin_base;
 	pctl->chip->label = dev_name(&pdev->dev);
 	pctl->chip->dev = &pdev->dev;
-	pctl->chip->base = 0;
+	pctl->chip->base = pctl->desc->pin_base;
 
 	ret = gpiochip_add(pctl->chip);
 	if (ret)
@@ -884,12 +877,14 @@ static int sunxi_pinctrl_probe(struct platform_device *pdev)
 		goto gpiochip_error;
 	}
 
-	clk_prepare_enable(clk);
+	ret = clk_prepare_enable(clk);
+	if (ret)
+		goto gpiochip_error;
 
 	pctl->irq = irq_of_parse_and_map(node, 0);
 	if (!pctl->irq) {
 		ret = -EINVAL;
-		goto gpiochip_error;
+		goto clk_error;
 	}
 
 	pctl->domain = irq_domain_add_linear(node, SUNXI_IRQ_NUMBER,
@@ -897,7 +892,7 @@ static int sunxi_pinctrl_probe(struct platform_device *pdev)
 	if (!pctl->domain) {
 		dev_err(&pdev->dev, "Couldn't register IRQ domain\n");
 		ret = -ENOMEM;
-		goto gpiochip_error;
+		goto clk_error;
 	}
 
 	for (i = 0; i < SUNXI_IRQ_NUMBER; i++) {
@@ -915,6 +910,8 @@ static int sunxi_pinctrl_probe(struct platform_device *pdev)
 
 	return 0;
 
+clk_error:
+	clk_disable_unprepare(clk);
 gpiochip_error:
 	if (gpiochip_remove(pctl->chip))
 		dev_err(&pdev->dev, "failed to remove gpio chip\n");
@@ -922,17 +919,3 @@ pinctrl_error:
 	pinctrl_unregister(pctl->pctl_dev);
 	return ret;
 }
-
-static struct platform_driver sunxi_pinctrl_driver = {
-	.probe = sunxi_pinctrl_probe,
-	.driver = {
-		.name = "sunxi-pinctrl",
-		.owner = THIS_MODULE,
-		.of_match_table = sunxi_pinctrl_match,
-	},
-};
-module_platform_driver(sunxi_pinctrl_driver);
-
-MODULE_AUTHOR("Maxime Ripard <maxime.ripard@free-electrons.com");
-MODULE_DESCRIPTION("Allwinner A1X pinctrl driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/sunxi/pinctrl-sunxi.h b/drivers/pinctrl/sunxi/pinctrl-sunxi.h
new file mode 100644
index 00000000000..8169ba59887
--- /dev/null
+++ b/drivers/pinctrl/sunxi/pinctrl-sunxi.h
@@ -0,0 +1,258 @@
+/*
+ * Allwinner A1X SoCs pinctrl driver.
+ *
+ * Copyright (C) 2012 Maxime Ripard
+ *
+ * Maxime Ripard <maxime.ripard@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#ifndef __PINCTRL_SUNXI_H
+#define __PINCTRL_SUNXI_H
+
+#include <linux/kernel.h>
+#include <linux/spinlock.h>
+
+#define PA_BASE	0
+#define PB_BASE	32
+#define PC_BASE	64
+#define PD_BASE	96
+#define PE_BASE	128
+#define PF_BASE	160
+#define PG_BASE	192
+#define PH_BASE	224
+#define PI_BASE	256
+#define PL_BASE	352
+#define PM_BASE	384
+
+#define SUNXI_PINCTRL_PIN(bank, pin)		\
+	PINCTRL_PIN(P ## bank ## _BASE + (pin), "P" #bank #pin)
+
+#define SUNXI_PIN_NAME_MAX_LEN	5
+
+#define BANK_MEM_SIZE		0x24
+#define MUX_REGS_OFFSET		0x0
+#define DATA_REGS_OFFSET	0x10
+#define DLEVEL_REGS_OFFSET	0x14
+#define PULL_REGS_OFFSET	0x1c
+
+#define PINS_PER_BANK		32
+#define MUX_PINS_PER_REG	8
+#define MUX_PINS_BITS		4
+#define MUX_PINS_MASK		0x0f
+#define DATA_PINS_PER_REG	32
+#define DATA_PINS_BITS		1
+#define DATA_PINS_MASK		0x01
+#define DLEVEL_PINS_PER_REG	16
+#define DLEVEL_PINS_BITS	2
+#define DLEVEL_PINS_MASK	0x03
+#define PULL_PINS_PER_REG	16
+#define PULL_PINS_BITS		2
+#define PULL_PINS_MASK		0x03
+
+#define SUNXI_IRQ_NUMBER	32
+
+#define IRQ_CFG_REG		0x200
+#define IRQ_CFG_IRQ_PER_REG		8
+#define IRQ_CFG_IRQ_BITS		4
+#define IRQ_CFG_IRQ_MASK		((1 << IRQ_CFG_IRQ_BITS) - 1)
+#define IRQ_CTRL_REG		0x210
+#define IRQ_CTRL_IRQ_PER_REG		32
+#define IRQ_CTRL_IRQ_BITS		1
+#define IRQ_CTRL_IRQ_MASK		((1 << IRQ_CTRL_IRQ_BITS) - 1)
+#define IRQ_STATUS_REG		0x214
+#define IRQ_STATUS_IRQ_PER_REG		32
+#define IRQ_STATUS_IRQ_BITS		1
+#define IRQ_STATUS_IRQ_MASK		((1 << IRQ_STATUS_IRQ_BITS) - 1)
+
+#define IRQ_EDGE_RISING		0x00
+#define IRQ_EDGE_FALLING	0x01
+#define IRQ_LEVEL_HIGH		0x02
+#define IRQ_LEVEL_LOW		0x03
+#define IRQ_EDGE_BOTH		0x04
+
+struct sunxi_desc_function {
+	const char	*name;
+	u8		muxval;
+	u8		irqnum;
+};
+
+struct sunxi_desc_pin {
+	struct pinctrl_pin_desc		pin;
+	struct sunxi_desc_function	*functions;
+};
+
+struct sunxi_pinctrl_desc {
+	const struct sunxi_desc_pin	*pins;
+	int				npins;
+	unsigned			pin_base;
+};
+
+struct sunxi_pinctrl_function {
+	const char	*name;
+	const char	**groups;
+	unsigned	ngroups;
+};
+
+struct sunxi_pinctrl_group {
+	const char	*name;
+	unsigned long	config;
+	unsigned	pin;
+};
+
+struct sunxi_pinctrl {
+	void __iomem			*membase;
+	struct gpio_chip		*chip;
+	const struct sunxi_pinctrl_desc	*desc;
+	struct device			*dev;
+	struct irq_domain		*domain;
+	struct sunxi_pinctrl_function	*functions;
+	unsigned			nfunctions;
+	struct sunxi_pinctrl_group	*groups;
+	unsigned			ngroups;
+	int				irq;
+	int				irq_array[SUNXI_IRQ_NUMBER];
+	spinlock_t			lock;
+	struct pinctrl_dev		*pctl_dev;
+};
+
+#define SUNXI_PIN(_pin, ...)					\
+	{							\
+		.pin = _pin,					\
+		.functions = (struct sunxi_desc_function[]){	\
+			__VA_ARGS__, { } },			\
+	}
+
+#define SUNXI_FUNCTION(_val, _name)				\
+	{							\
+		.name = _name,					\
+		.muxval = _val,					\
+	}
+
+#define SUNXI_FUNCTION_IRQ(_val, _irq)				\
+	{							\
+		.name = "irq",					\
+		.muxval = _val,					\
+		.irqnum = _irq,					\
+	}
+
+/*
+ * The sunXi PIO registers are organized as is:
+ * 0x00 - 0x0c	Muxing values.
+ *		8 pins per register, each pin having a 4bits value
+ * 0x10		Pin values
+ *		32 bits per register, each pin corresponding to one bit
+ * 0x14 - 0x18	Drive level
+ *		16 pins per register, each pin having a 2bits value
+ * 0x1c - 0x20	Pull-Up values
+ *		16 pins per register, each pin having a 2bits value
+ *
+ * This is for the first bank. Each bank will have the same layout,
+ * with an offset being a multiple of 0x24.
+ *
+ * The following functions calculate from the pin number the register
+ * and the bit offset that we should access.
+ */
+static inline u32 sunxi_mux_reg(u16 pin)
+{
+	u8 bank = pin / PINS_PER_BANK;
+	u32 offset = bank * BANK_MEM_SIZE;
+	offset += MUX_REGS_OFFSET;
+	offset += pin % PINS_PER_BANK / MUX_PINS_PER_REG * 0x04;
+	return round_down(offset, 4);
+}
+
+static inline u32 sunxi_mux_offset(u16 pin)
+{
+	u32 pin_num = pin % MUX_PINS_PER_REG;
+	return pin_num * MUX_PINS_BITS;
+}
+
+static inline u32 sunxi_data_reg(u16 pin)
+{
+	u8 bank = pin / PINS_PER_BANK;
+	u32 offset = bank * BANK_MEM_SIZE;
+	offset += DATA_REGS_OFFSET;
+	offset += pin % PINS_PER_BANK / DATA_PINS_PER_REG * 0x04;
+	return round_down(offset, 4);
+}
+
+static inline u32 sunxi_data_offset(u16 pin)
+{
+	u32 pin_num = pin % DATA_PINS_PER_REG;
+	return pin_num * DATA_PINS_BITS;
+}
+
+static inline u32 sunxi_dlevel_reg(u16 pin)
+{
+	u8 bank = pin / PINS_PER_BANK;
+	u32 offset = bank * BANK_MEM_SIZE;
+	offset += DLEVEL_REGS_OFFSET;
+	offset += pin % PINS_PER_BANK / DLEVEL_PINS_PER_REG * 0x04;
+	return round_down(offset, 4);
+}
+
+static inline u32 sunxi_dlevel_offset(u16 pin)
+{
+	u32 pin_num = pin % DLEVEL_PINS_PER_REG;
+	return pin_num * DLEVEL_PINS_BITS;
+}
+
+static inline u32 sunxi_pull_reg(u16 pin)
+{
+	u8 bank = pin / PINS_PER_BANK;
+	u32 offset = bank * BANK_MEM_SIZE;
+	offset += PULL_REGS_OFFSET;
+	offset += pin % PINS_PER_BANK / PULL_PINS_PER_REG * 0x04;
+	return round_down(offset, 4);
+}
+
+static inline u32 sunxi_pull_offset(u16 pin)
+{
+	u32 pin_num = pin % PULL_PINS_PER_REG;
+	return pin_num * PULL_PINS_BITS;
+}
+
+static inline u32 sunxi_irq_cfg_reg(u16 irq)
+{
+	u8 reg = irq / IRQ_CFG_IRQ_PER_REG * 0x04;
+	return reg + IRQ_CFG_REG;
+}
+
+static inline u32 sunxi_irq_cfg_offset(u16 irq)
+{
+	u32 irq_num = irq % IRQ_CFG_IRQ_PER_REG;
+	return irq_num * IRQ_CFG_IRQ_BITS;
+}
+
+static inline u32 sunxi_irq_ctrl_reg(u16 irq)
+{
+	u8 reg = irq / IRQ_CTRL_IRQ_PER_REG * 0x04;
+	return reg + IRQ_CTRL_REG;
+}
+
+static inline u32 sunxi_irq_ctrl_offset(u16 irq)
+{
+	u32 irq_num = irq % IRQ_CTRL_IRQ_PER_REG;
+	return irq_num * IRQ_CTRL_IRQ_BITS;
+}
+
+static inline u32 sunxi_irq_status_reg(u16 irq)
+{
+	u8 reg = irq / IRQ_STATUS_IRQ_PER_REG * 0x04;
+	return reg + IRQ_STATUS_REG;
+}
+
+static inline u32 sunxi_irq_status_offset(u16 irq)
+{
+	u32 irq_num = irq % IRQ_STATUS_IRQ_PER_REG;
+	return irq_num * IRQ_STATUS_IRQ_BITS;
+}
+
+int sunxi_pinctrl_init(struct platform_device *pdev,
+		       const struct sunxi_pinctrl_desc *desc);
+
+#endif /* __PINCTRL_SUNXI_H */
diff --git a/drivers/pinctrl/vt8500/pinctrl-wmt.c b/drivers/pinctrl/vt8500/pinctrl-wmt.c
index 9802b67040c..2c61281bebd 100644
--- a/drivers/pinctrl/vt8500/pinctrl-wmt.c
+++ b/drivers/pinctrl/vt8500/pinctrl-wmt.c
@@ -523,17 +523,6 @@ static int wmt_gpio_get_direction(struct gpio_chip *chip, unsigned offset)
 		return GPIOF_DIR_IN;
 }
 
-static int wmt_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
-{
-	return pinctrl_gpio_direction_input(chip->base + offset);
-}
-
-static int wmt_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
-				     int value)
-{
-	return pinctrl_gpio_direction_output(chip->base + offset);
-}
-
 static int wmt_gpio_get_value(struct gpio_chip *chip, unsigned offset)
 {
 	struct wmt_pinctrl_data *data = dev_get_drvdata(chip->dev);
@@ -568,6 +557,18 @@ static void wmt_gpio_set_value(struct gpio_chip *chip, unsigned offset,
 		wmt_clearbits(data, reg_data_out, BIT(bit));
 }
 
+static int wmt_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
+{
+	return pinctrl_gpio_direction_input(chip->base + offset);
+}
+
+static int wmt_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
+				     int value)
+{
+	wmt_gpio_set_value(chip, offset, value);
+	return pinctrl_gpio_direction_output(chip->base + offset);
+}
+
 static struct gpio_chip wmt_gpio_chip = {
 	.label = "gpio-wmt",
 	.owner = THIS_MODULE,
diff --git a/drivers/platform/chrome/chromeos_laptop.c b/drivers/platform/chrome/chromeos_laptop.c
index 7f3aad0e115..7f1a2e2711b 100644
--- a/drivers/platform/chrome/chromeos_laptop.c
+++ b/drivers/platform/chrome/chromeos_laptop.c
@@ -84,21 +84,19 @@ static struct i2c_board_info tsl2563_als_device = {
 	I2C_BOARD_INFO("tsl2563", TAOS_ALS_I2C_ADDR),
 };
 
+static int mxt_t19_keys[] = {
+	KEY_RESERVED,
+	KEY_RESERVED,
+	KEY_RESERVED,
+	KEY_RESERVED,
+	KEY_RESERVED,
+	BTN_LEFT
+};
+
 static struct mxt_platform_data atmel_224s_tp_platform_data = {
-	.x_line			= 18,
-	.y_line			= 12,
-	.x_size			= 102*20,
-	.y_size			= 68*20,
-	.blen			= 0x80,	/* Gain setting is in upper 4 bits */
-	.threshold		= 0x32,
-	.voltage		= 0,	/* 3.3V */
-	.orient			= MXT_VERTICAL_FLIP,
 	.irqflags		= IRQF_TRIGGER_FALLING,
-	.is_tp			= true,
-	.key_map		= { KEY_RESERVED,
-				    KEY_RESERVED,
-				    KEY_RESERVED,
-				    BTN_LEFT },
+	.t19_num_keys		= ARRAY_SIZE(mxt_t19_keys),
+	.t19_keymap		= mxt_t19_keys,
 	.config			= NULL,
 	.config_length		= 0,
 };
@@ -110,16 +108,7 @@ static struct i2c_board_info atmel_224s_tp_device = {
 };
 
 static struct mxt_platform_data atmel_1664s_platform_data = {
-	.x_line			= 32,
-	.y_line			= 50,
-	.x_size			= 1700,
-	.y_size			= 2560,
-	.blen			= 0x89,	/* Gain setting is in upper 4 bits */
-	.threshold		= 0x28,
-	.voltage		= 0,	/* 3.3V */
-	.orient			= MXT_ROTATED_90_COUNTER,
 	.irqflags		= IRQF_TRIGGER_FALLING,
-	.is_tp			= false,
 	.config			= NULL,
 	.config_length		= 0,
 };
diff --git a/drivers/platform/goldfish/goldfish_pipe.c b/drivers/platform/goldfish/goldfish_pipe.c
index 4f5aa831f54..d9a09d9637d 100644
--- a/drivers/platform/goldfish/goldfish_pipe.c
+++ b/drivers/platform/goldfish/goldfish_pipe.c
@@ -56,6 +56,7 @@
 #include <linux/bitops.h>
 #include <linux/slab.h>
 #include <linux/io.h>
+#include <linux/goldfish.h>
 
 /*
  * IMPORTANT: The following constants must match the ones used and defined
@@ -66,8 +67,10 @@
 #define PIPE_REG_COMMAND		0x00  /* write: value = command */
 #define PIPE_REG_STATUS			0x04  /* read */
 #define PIPE_REG_CHANNEL		0x08  /* read/write: channel id */
+#define PIPE_REG_CHANNEL_HIGH	        0x30  /* read/write: channel id */
 #define PIPE_REG_SIZE			0x0c  /* read/write: buffer size */
 #define PIPE_REG_ADDRESS		0x10  /* write: physical address */
+#define PIPE_REG_ADDRESS_HIGH	        0x34  /* write: physical address */
 #define PIPE_REG_WAKES			0x14  /* read: wake flags */
 #define PIPE_REG_PARAMS_ADDR_LOW	0x18  /* read/write: batch data address */
 #define PIPE_REG_PARAMS_ADDR_HIGH	0x1c  /* read/write: batch data address */
@@ -109,9 +112,9 @@
 #define PIPE_WAKE_WRITE        (1 << 2)  /* pipe can now be written to */
 
 struct access_params {
-	u32 channel;
+	unsigned long channel;
 	u32 size;
-	u32 address;
+	unsigned long address;
 	u32 cmd;
 	u32 result;
 	/* reserved for future extension */
@@ -149,13 +152,14 @@ enum {
 
 
 static u32 goldfish_cmd_status(struct goldfish_pipe *pipe, u32 cmd)
-{ 
+{
 	unsigned long flags;
 	u32 status;
 	struct goldfish_pipe_dev *dev = pipe->dev;
 
 	spin_lock_irqsave(&dev->lock, flags);
-	writel((u32)pipe, dev->base + PIPE_REG_CHANNEL);
+	gf_write64((u64)(unsigned long)pipe, dev->base + PIPE_REG_CHANNEL,
+				dev->base + PIPE_REG_CHANNEL_HIGH);
 	writel(cmd, dev->base + PIPE_REG_COMMAND);
 	status = readl(dev->base + PIPE_REG_STATUS);
 	spin_unlock_irqrestore(&dev->lock, flags);
@@ -163,12 +167,13 @@ static u32 goldfish_cmd_status(struct goldfish_pipe *pipe, u32 cmd)
 }
 
 static void goldfish_cmd(struct goldfish_pipe *pipe, u32 cmd)
-{ 
+{
 	unsigned long flags;
 	struct goldfish_pipe_dev *dev = pipe->dev;
 
 	spin_lock_irqsave(&dev->lock, flags);
-	writel((u32)pipe, dev->base + PIPE_REG_CHANNEL);
+	gf_write64((u64)(unsigned long)pipe, dev->base + PIPE_REG_CHANNEL,
+				dev->base + PIPE_REG_CHANNEL_HIGH);
 	writel(cmd, dev->base + PIPE_REG_COMMAND);
 	spin_unlock_irqrestore(&dev->lock, flags);
 }
@@ -322,9 +327,12 @@ static ssize_t goldfish_pipe_read_write(struct file *filp, char __user *buffer,
 		spin_lock_irqsave(&dev->lock, irq_flags);
 		if (access_with_param(dev, CMD_WRITE_BUFFER + cmd_offset,
 				address, avail, pipe, &status)) {
-			writel((u32)pipe, dev->base + PIPE_REG_CHANNEL);
+			gf_write64((u64)(unsigned long)pipe,
+				   dev->base + PIPE_REG_CHANNEL,
+				   dev->base + PIPE_REG_CHANNEL_HIGH);
 			writel(avail, dev->base + PIPE_REG_SIZE);
-			writel(address, dev->base + PIPE_REG_ADDRESS);
+			gf_write64(address, dev->base + PIPE_REG_ADDRESS,
+				dev->base + PIPE_REG_ADDRESS_HIGH);
 			writel(CMD_WRITE_BUFFER + cmd_offset,
 					dev->base + PIPE_REG_COMMAND);
 			status = readl(dev->base + PIPE_REG_STATUS);
@@ -447,7 +455,15 @@ static irqreturn_t goldfish_pipe_interrupt(int irq, void *dev_id)
 		/* First read the channel, 0 means the end of the list */
 		struct goldfish_pipe *pipe;
 		unsigned long wakes;
-		unsigned long channel = readl(dev->base + PIPE_REG_CHANNEL);
+		unsigned long channel = 0;
+
+#ifdef CONFIG_64BIT
+		channel = (u64)readl(dev->base + PIPE_REG_CHANNEL_HIGH) << 32;
+
+		if (channel == 0)
+			break;
+#endif
+		channel |= readl(dev->base + PIPE_REG_CHANNEL);
 
 		if (channel == 0)
 			break;
diff --git a/drivers/platform/goldfish/pdev_bus.c b/drivers/platform/goldfish/pdev_bus.c
index 92cc4cfafde..8c43589c3ed 100644
--- a/drivers/platform/goldfish/pdev_bus.c
+++ b/drivers/platform/goldfish/pdev_bus.c
@@ -36,6 +36,7 @@
 #define PDEV_BUS_IO_SIZE        (0x14)
 #define PDEV_BUS_IRQ            (0x18)
 #define PDEV_BUS_IRQ_COUNT      (0x1c)
+#define PDEV_BUS_GET_NAME_HIGH  (0x20)
 
 struct pdev_bus_dev {
 	struct list_head list;
@@ -129,7 +130,10 @@ static int goldfish_new_pdev(void)
 	dev->pdev.dev.dma_mask = (void *)(dev->pdev.name + name_len + 1);
 	*dev->pdev.dev.dma_mask = ~0;
 
-	writel((unsigned long)name, pdev_bus_base + PDEV_BUS_GET_NAME);
+#ifdef CONFIG_64BIT
+	writel((u32)((u64)name>>32), pdev_bus_base + PDEV_BUS_GET_NAME_HIGH);
+#endif
+	writel((u32)(unsigned long)name, pdev_bus_base + PDEV_BUS_GET_NAME);
 	name[name_len] = '\0';
 	dev->pdev.id = readl(pdev_bus_base + PDEV_BUS_ID);
 	dev->pdev.resource[0].start = base;
@@ -184,11 +188,6 @@ static int goldfish_pdev_bus_probe(struct platform_device *pdev)
 	pdev_bus_addr = r->start;
 	pdev_bus_len = resource_size(r);
 
-	if (request_mem_region(pdev_bus_addr, pdev_bus_len, "goldfish")) {
-		dev_err(&pdev->dev, "unable to reserve Goldfish MMIO.\n");
-		return -EBUSY;
-	}
-
 	pdev_bus_base = ioremap(pdev_bus_addr, pdev_bus_len);
 	if (pdev_bus_base == NULL) {
 		ret = -ENOMEM;
diff --git a/drivers/platform/x86/Kconfig b/drivers/platform/x86/Kconfig
index 27df2c533b0..172f26ce59a 100644
--- a/drivers/platform/x86/Kconfig
+++ b/drivers/platform/x86/Kconfig
@@ -102,7 +102,7 @@ config DELL_LAPTOP
 	default n
 	---help---
 	This driver adds support for rfkill and backlight control to Dell
-	laptops.
+	laptops (except for some models covered by the Compal driver).
 
 config DELL_WMI
 	tristate "Dell WMI extras"
@@ -127,6 +127,16 @@ config DELL_WMI_AIO
 	  To compile this driver as a module, choose M here: the module will
 	  be called dell-wmi-aio.
 
+config DELL_SMO8800
+	tristate "Dell Latitude freefall driver (ACPI SMO8800/SMO8810)"
+	depends on ACPI
+	---help---
+	  Say Y here if you want to support SMO8800/SMO8810 freefall device
+	  on Dell Latitude laptops.
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called dell-smo8800.
+
 
 config FUJITSU_LAPTOP
 	tristate "Fujitsu Laptop Extras"
@@ -265,23 +275,21 @@ config PANASONIC_LAPTOP
 	  R2, R3, R5, T2, W2 and Y2 series), say Y.
 
 config COMPAL_LAPTOP
-	tristate "Compal Laptop Extras"
+	tristate "Compal (and others) Laptop Extras"
 	depends on ACPI
 	depends on BACKLIGHT_CLASS_DEVICE
 	depends on RFKILL
 	depends on HWMON
 	depends on POWER_SUPPLY
 	---help---
-	  This is a driver for laptops built by Compal:
-
-	  Compal FL90/IFL90
-	  Compal FL91/IFL91
-	  Compal FL92/JFL92
-	  Compal FT00/IFT00
+	  This is a driver for laptops built by Compal, and some models by
+	  other brands (e.g. Dell, Toshiba).
 
-	  It adds support for Bluetooth, WLAN and LCD brightness control.
+	  It adds support for rfkill, Bluetooth, WLAN and LCD brightness
+	  control.
 
-	  If you have an Compal FL9x/IFL9x/FT00 laptop, say Y or M here.
+	  For a (possibly incomplete) list of supported laptops, please refer
+	  to: Documentation/platform/x86-laptop-drivers.txt
 
 config SONY_LAPTOP
 	tristate "Sony Laptop Extras"
@@ -724,7 +732,7 @@ config IBM_RTL
 
 config XO1_RFKILL
 	tristate "OLPC XO-1 software RF kill switch"
-	depends on OLPC
+	depends on OLPC || COMPILE_TEST
 	depends on RFKILL
 	---help---
 	  Support for enabling/disabling the WLAN interface on the OLPC XO-1
@@ -732,6 +740,7 @@ config XO1_RFKILL
 
 config XO15_EBOOK
 	tristate "OLPC XO-1.5 ebook switch"
+	depends on OLPC || COMPILE_TEST
 	depends on ACPI && INPUT
 	---help---
 	  Support for the ebook switch on the OLPC XO-1.5 laptop.
diff --git a/drivers/platform/x86/Makefile b/drivers/platform/x86/Makefile
index 1a2eafc9d48..c4ca428fd3d 100644
--- a/drivers/platform/x86/Makefile
+++ b/drivers/platform/x86/Makefile
@@ -13,6 +13,7 @@ obj-$(CONFIG_COMPAL_LAPTOP)	+= compal-laptop.o
 obj-$(CONFIG_DELL_LAPTOP)	+= dell-laptop.o
 obj-$(CONFIG_DELL_WMI)		+= dell-wmi.o
 obj-$(CONFIG_DELL_WMI_AIO)	+= dell-wmi-aio.o
+obj-$(CONFIG_DELL_SMO8800)	+= dell-smo8800.o
 obj-$(CONFIG_ACER_WMI)		+= acer-wmi.o
 obj-$(CONFIG_ACERHDF)		+= acerhdf.o
 obj-$(CONFIG_HP_ACCEL)		+= hp_accel.o
diff --git a/drivers/platform/x86/acer-wmi.c b/drivers/platform/x86/acer-wmi.c
index c91f69b39db..bbf78b2d6d9 100644
--- a/drivers/platform/x86/acer-wmi.c
+++ b/drivers/platform/x86/acer-wmi.c
@@ -570,6 +570,14 @@ static const struct dmi_system_id video_vendor_dmi_table[] = {
 			DMI_MATCH(DMI_PRODUCT_NAME, "Aspire 5750"),
 		},
 	},
+	{
+		.callback = video_set_backlight_video_vendor,
+		.ident = "Acer Aspire 5741",
+		.matches = {
+			DMI_MATCH(DMI_BOARD_VENDOR, "Acer"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "Aspire 5741"),
+		},
+	},
 	{}
 };
 
@@ -2228,7 +2236,7 @@ static int __init acer_wmi_init(void)
 		pr_info("Brightness must be controlled by acpi video driver\n");
 	} else {
 		pr_info("Disabling ACPI video driver\n");
-		acpi_video_unregister();
+		acpi_video_unregister_backlight();
 	}
 
 	if (wmi_has_guid(WMID_GUID3)) {
diff --git a/drivers/platform/x86/alienware-wmi.c b/drivers/platform/x86/alienware-wmi.c
index 541f9514f76..297b6640213 100644
--- a/drivers/platform/x86/alienware-wmi.c
+++ b/drivers/platform/x86/alienware-wmi.c
@@ -32,6 +32,7 @@
 #define WMAX_METHOD_HDMI_STATUS		0x2
 #define WMAX_METHOD_BRIGHTNESS		0x3
 #define WMAX_METHOD_ZONE_CONTROL	0x4
+#define WMAX_METHOD_HDMI_CABLE		0x5
 
 MODULE_AUTHOR("Mario Limonciello <mario_limonciello@dell.com>");
 MODULE_DESCRIPTION("Alienware special feature control");
@@ -350,12 +351,11 @@ static int alienware_zone_init(struct platform_device *dev)
 	char *name;
 
 	if (interface == WMAX) {
-		global_led.max_brightness = 100;
 		lighting_control_state = WMAX_RUNNING;
 	} else if (interface == LEGACY) {
-		global_led.max_brightness = 0x0F;
 		lighting_control_state = LEGACY_RUNNING;
 	}
+	global_led.max_brightness = 0x0F;
 	global_brightness = global_led.max_brightness;
 
 	/*
@@ -423,41 +423,85 @@ static void alienware_zone_exit(struct platform_device *dev)
 	The HDMI mux sysfs node indicates the status of the HDMI input mux.
 	It can toggle between standard system GPU output and HDMI input.
 */
-static ssize_t show_hdmi(struct device *dev, struct device_attribute *attr,
-			 char *buf)
+static acpi_status alienware_hdmi_command(struct hdmi_args *in_args,
+					  u32 command, int *out_data)
 {
 	acpi_status status;
-	struct acpi_buffer input;
 	union acpi_object *obj;
-	u32 tmp = 0;
-	struct acpi_buffer output = { ACPI_ALLOCATE_BUFFER, NULL };
+	struct acpi_buffer input;
+	struct acpi_buffer output;
+
+	input.length = (acpi_size) sizeof(*in_args);
+	input.pointer = in_args;
+	if (out_data != NULL) {
+		output.length = ACPI_ALLOCATE_BUFFER;
+		output.pointer = NULL;
+		status = wmi_evaluate_method(WMAX_CONTROL_GUID, 1,
+					     command, &input, &output);
+	} else
+		status = wmi_evaluate_method(WMAX_CONTROL_GUID, 1,
+					     command, &input, NULL);
+
+	if (ACPI_SUCCESS(status) && out_data != NULL) {
+		obj = (union acpi_object *)output.pointer;
+		if (obj && obj->type == ACPI_TYPE_INTEGER)
+			*out_data = (u32) obj->integer.value;
+	}
+	return status;
+
+}
+
+static ssize_t show_hdmi_cable(struct device *dev,
+			       struct device_attribute *attr, char *buf)
+{
+	acpi_status status;
+	u32 out_data;
 	struct hdmi_args in_args = {
 		.arg = 0,
 	};
-	input.length = (acpi_size) sizeof(in_args);
-	input.pointer = &in_args;
-	status = wmi_evaluate_method(WMAX_CONTROL_GUID, 1,
-				     WMAX_METHOD_HDMI_STATUS, &input, &output);
+	status =
+	    alienware_hdmi_command(&in_args, WMAX_METHOD_HDMI_CABLE,
+				   (u32 *) &out_data);
+	if (ACPI_SUCCESS(status)) {
+		if (out_data == 0)
+			return scnprintf(buf, PAGE_SIZE,
+					 "[unconnected] connected unknown\n");
+		else if (out_data == 1)
+			return scnprintf(buf, PAGE_SIZE,
+					 "unconnected [connected] unknown\n");
+	}
+	pr_err("alienware-wmi: unknown HDMI cable status: %d\n", status);
+	return scnprintf(buf, PAGE_SIZE, "unconnected connected [unknown]\n");
+}
+
+static ssize_t show_hdmi_source(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	acpi_status status;
+	u32 out_data;
+	struct hdmi_args in_args = {
+		.arg = 0,
+	};
+	status =
+	    alienware_hdmi_command(&in_args, WMAX_METHOD_HDMI_STATUS,
+				   (u32 *) &out_data);
 
 	if (ACPI_SUCCESS(status)) {
-		obj = (union acpi_object *)output.pointer;
-		if (obj && obj->type == ACPI_TYPE_INTEGER)
-			tmp = (u32) obj->integer.value;
-		if (tmp == 1)
+		if (out_data == 1)
 			return scnprintf(buf, PAGE_SIZE,
 					 "[input] gpu unknown\n");
-		else if (tmp == 2)
+		else if (out_data == 2)
 			return scnprintf(buf, PAGE_SIZE,
 					 "input [gpu] unknown\n");
 	}
-	pr_err("alienware-wmi: unknown HDMI status: %d\n", status);
+	pr_err("alienware-wmi: unknown HDMI source status: %d\n", out_data);
 	return scnprintf(buf, PAGE_SIZE, "input gpu [unknown]\n");
 }
 
-static ssize_t toggle_hdmi(struct device *dev, struct device_attribute *attr,
-			   const char *buf, size_t count)
+static ssize_t toggle_hdmi_source(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf, size_t count)
 {
-	struct acpi_buffer input;
 	acpi_status status;
 	struct hdmi_args args;
 	if (strcmp(buf, "gpu\n") == 0)
@@ -467,33 +511,46 @@ static ssize_t toggle_hdmi(struct device *dev, struct device_attribute *attr,
 	else
 		args.arg = 3;
 	pr_debug("alienware-wmi: setting hdmi to %d : %s", args.arg, buf);
-	input.length = (acpi_size) sizeof(args);
-	input.pointer = &args;
-	status = wmi_evaluate_method(WMAX_CONTROL_GUID, 1,
-				     WMAX_METHOD_HDMI_SOURCE, &input, NULL);
+
+	status = alienware_hdmi_command(&args, WMAX_METHOD_HDMI_SOURCE, NULL);
+
 	if (ACPI_FAILURE(status))
 		pr_err("alienware-wmi: HDMI toggle failed: results: %u\n",
 		       status);
 	return count;
 }
 
-static DEVICE_ATTR(hdmi, S_IRUGO | S_IWUSR, show_hdmi, toggle_hdmi);
+static DEVICE_ATTR(cable, S_IRUGO, show_hdmi_cable, NULL);
+static DEVICE_ATTR(source, S_IRUGO | S_IWUSR, show_hdmi_source,
+		   toggle_hdmi_source);
+
+static struct attribute *hdmi_attrs[] = {
+	&dev_attr_cable.attr,
+	&dev_attr_source.attr,
+	NULL,
+};
 
-static void remove_hdmi(struct platform_device *device)
+static struct attribute_group hdmi_attribute_group = {
+	.name = "hdmi",
+	.attrs = hdmi_attrs,
+};
+
+static void remove_hdmi(struct platform_device *dev)
 {
-	device_remove_file(&device->dev, &dev_attr_hdmi);
+	sysfs_remove_group(&dev->dev.kobj, &hdmi_attribute_group);
 }
 
-static int create_hdmi(void)
+static int create_hdmi(struct platform_device *dev)
 {
-	int ret = -ENOMEM;
-	ret = device_create_file(&platform_device->dev, &dev_attr_hdmi);
+	int ret;
+
+	ret = sysfs_create_group(&dev->dev.kobj, &hdmi_attribute_group);
 	if (ret)
 		goto error_create_hdmi;
 	return 0;
 
 error_create_hdmi:
-	remove_hdmi(platform_device);
+	remove_hdmi(dev);
 	return ret;
 }
 
@@ -527,7 +584,7 @@ static int __init alienware_wmi_init(void)
 		goto fail_platform_device2;
 
 	if (interface == WMAX) {
-		ret = create_hdmi();
+		ret = create_hdmi(platform_device);
 		if (ret)
 			goto fail_prep_hdmi;
 	}
diff --git a/drivers/platform/x86/asus-nb-wmi.c b/drivers/platform/x86/asus-nb-wmi.c
index 563f59efa66..ddf0eefd862 100644
--- a/drivers/platform/x86/asus-nb-wmi.c
+++ b/drivers/platform/x86/asus-nb-wmi.c
@@ -137,6 +137,15 @@ static struct dmi_system_id asus_quirks[] = {
 	},
 	{
 		.callback = dmi_matched,
+		.ident = "ASUSTeK COMPUTER INC. X550CA",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "X550CA"),
+		},
+		.driver_data = &quirk_asus_x401u,
+	},
+	{
+		.callback = dmi_matched,
 		.ident = "ASUSTeK COMPUTER INC. X55A",
 		.matches = {
 			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
diff --git a/drivers/platform/x86/asus-wmi.c b/drivers/platform/x86/asus-wmi.c
index c5e082fb82f..3c6ccedc82b 100644
--- a/drivers/platform/x86/asus-wmi.c
+++ b/drivers/platform/x86/asus-wmi.c
@@ -266,7 +266,7 @@ static int asus_wmi_evaluate_method(u32 method_id, u32 arg0, u32 arg1,
 	struct acpi_buffer output = { ACPI_ALLOCATE_BUFFER, NULL };
 	acpi_status status;
 	union acpi_object *obj;
-	u32 tmp;
+	u32 tmp = 0;
 
 	status = wmi_evaluate_method(ASUS_WMI_MGMT_GUID, 1, method_id,
 				     &input, &output);
@@ -277,8 +277,6 @@ static int asus_wmi_evaluate_method(u32 method_id, u32 arg0, u32 arg1,
 	obj = (union acpi_object *)output.pointer;
 	if (obj && obj->type == ACPI_TYPE_INTEGER)
 		tmp = (u32) obj->integer.value;
-	else
-		tmp = 0;
 
 	if (retval)
 		*retval = tmp;
@@ -642,8 +640,7 @@ static void asus_rfkill_hotplug(struct asus_wmi *asus)
 			dev = pci_scan_single_device(bus, 0);
 			if (dev) {
 				pci_bus_assign_resources(bus);
-				if (pci_bus_add_device(dev))
-					pr_err("Unable to hotplug wifi\n");
+				pci_bus_add_device(dev);
 			}
 		} else {
 			dev = pci_get_slot(bus, 0);
diff --git a/drivers/platform/x86/dell-smo8800.c b/drivers/platform/x86/dell-smo8800.c
new file mode 100644
index 00000000000..a653716055d
--- /dev/null
+++ b/drivers/platform/x86/dell-smo8800.c
@@ -0,0 +1,233 @@
+/*
+ *  dell-smo8800.c - Dell Latitude ACPI SMO8800/SMO8810 freefall sensor driver
+ *
+ *  Copyright (C) 2012 Sonal Santan <sonal.santan@gmail.com>
+ *  Copyright (C) 2014 Pali Rohár <pali.rohar@gmail.com>
+ *
+ *  This is loosely based on lis3lv02d 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.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT 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 DRIVER_NAME "smo8800"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/acpi.h>
+#include <linux/interrupt.h>
+#include <linux/miscdevice.h>
+
+struct smo8800_device {
+	u32 irq;                     /* acpi device irq */
+	atomic_t counter;            /* count after last read */
+	struct miscdevice miscdev;   /* for /dev/freefall */
+	unsigned long misc_opened;   /* whether the device is open */
+	wait_queue_head_t misc_wait; /* Wait queue for the misc dev */
+	struct device *dev;          /* acpi device */
+};
+
+static irqreturn_t smo8800_interrupt_quick(int irq, void *data)
+{
+	struct smo8800_device *smo8800 = data;
+
+	atomic_inc(&smo8800->counter);
+	wake_up_interruptible(&smo8800->misc_wait);
+	return IRQ_WAKE_THREAD;
+}
+
+static irqreturn_t smo8800_interrupt_thread(int irq, void *data)
+{
+	struct smo8800_device *smo8800 = data;
+
+	dev_info(smo8800->dev, "detected free fall\n");
+	return IRQ_HANDLED;
+}
+
+static acpi_status smo8800_get_resource(struct acpi_resource *resource,
+					void *context)
+{
+	struct acpi_resource_extended_irq *irq;
+
+	if (resource->type != ACPI_RESOURCE_TYPE_EXTENDED_IRQ)
+		return AE_OK;
+
+	irq = &resource->data.extended_irq;
+	if (!irq || !irq->interrupt_count)
+		return AE_OK;
+
+	*((u32 *)context) = irq->interrupts[0];
+	return AE_CTRL_TERMINATE;
+}
+
+static u32 smo8800_get_irq(struct acpi_device *device)
+{
+	u32 irq = 0;
+	acpi_status status;
+
+	status = acpi_walk_resources(device->handle, METHOD_NAME__CRS,
+				     smo8800_get_resource, &irq);
+	if (ACPI_FAILURE(status)) {
+		dev_err(&device->dev, "acpi_walk_resources failed\n");
+		return 0;
+	}
+
+	return irq;
+}
+
+static ssize_t smo8800_misc_read(struct file *file, char __user *buf,
+				 size_t count, loff_t *pos)
+{
+	struct smo8800_device *smo8800 = container_of(file->private_data,
+					 struct smo8800_device, miscdev);
+
+	u32 data = 0;
+	unsigned char byte_data = 0;
+	ssize_t retval = 1;
+
+	if (count < 1)
+		return -EINVAL;
+
+	atomic_set(&smo8800->counter, 0);
+	retval = wait_event_interruptible(smo8800->misc_wait,
+				(data = atomic_xchg(&smo8800->counter, 0)));
+
+	if (retval)
+		return retval;
+
+	byte_data = 1;
+	retval = 1;
+
+	if (data < 255)
+		byte_data = data;
+	else
+		byte_data = 255;
+
+	if (put_user(byte_data, buf))
+		retval = -EFAULT;
+
+	return retval;
+}
+
+static int smo8800_misc_open(struct inode *inode, struct file *file)
+{
+	struct smo8800_device *smo8800 = container_of(file->private_data,
+					 struct smo8800_device, miscdev);
+
+	if (test_and_set_bit(0, &smo8800->misc_opened))
+		return -EBUSY; /* already open */
+
+	atomic_set(&smo8800->counter, 0);
+	return 0;
+}
+
+static int smo8800_misc_release(struct inode *inode, struct file *file)
+{
+	struct smo8800_device *smo8800 = container_of(file->private_data,
+					 struct smo8800_device, miscdev);
+
+	clear_bit(0, &smo8800->misc_opened); /* release the device */
+	return 0;
+}
+
+static const struct file_operations smo8800_misc_fops = {
+	.owner = THIS_MODULE,
+	.read = smo8800_misc_read,
+	.open = smo8800_misc_open,
+	.release = smo8800_misc_release,
+};
+
+static int smo8800_add(struct acpi_device *device)
+{
+	int err;
+	struct smo8800_device *smo8800;
+
+	smo8800 = devm_kzalloc(&device->dev, sizeof(*smo8800), GFP_KERNEL);
+	if (!smo8800) {
+		dev_err(&device->dev, "failed to allocate device data\n");
+		return -ENOMEM;
+	}
+
+	smo8800->dev = &device->dev;
+	smo8800->miscdev.minor = MISC_DYNAMIC_MINOR;
+	smo8800->miscdev.name = "freefall";
+	smo8800->miscdev.fops = &smo8800_misc_fops;
+
+	init_waitqueue_head(&smo8800->misc_wait);
+
+	err = misc_register(&smo8800->miscdev);
+	if (err) {
+		dev_err(&device->dev, "failed to register misc dev: %d\n", err);
+		return err;
+	}
+
+	device->driver_data = smo8800;
+
+	smo8800->irq = smo8800_get_irq(device);
+	if (!smo8800->irq) {
+		dev_err(&device->dev, "failed to obtain IRQ\n");
+		err = -EINVAL;
+		goto error;
+	}
+
+	err = request_threaded_irq(smo8800->irq, smo8800_interrupt_quick,
+				   smo8800_interrupt_thread,
+				   IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+				   DRIVER_NAME, smo8800);
+	if (err) {
+		dev_err(&device->dev,
+			"failed to request thread for IRQ %d: %d\n",
+			smo8800->irq, err);
+		goto error;
+	}
+
+	dev_dbg(&device->dev, "device /dev/freefall registered with IRQ %d\n",
+		 smo8800->irq);
+	return 0;
+
+error:
+	misc_deregister(&smo8800->miscdev);
+	return err;
+}
+
+static int smo8800_remove(struct acpi_device *device)
+{
+	struct smo8800_device *smo8800 = device->driver_data;
+
+	free_irq(smo8800->irq, smo8800);
+	misc_deregister(&smo8800->miscdev);
+	dev_dbg(&device->dev, "device /dev/freefall unregistered\n");
+	return 0;
+}
+
+static const struct acpi_device_id smo8800_ids[] = {
+	{ "SMO8800", 0 },
+	{ "SMO8810", 0 },
+	{ "", 0 },
+};
+
+MODULE_DEVICE_TABLE(acpi, smo8800_ids);
+
+static struct acpi_driver smo8800_driver = {
+	.name = DRIVER_NAME,
+	.class = "Latitude",
+	.ids = smo8800_ids,
+	.ops = {
+		.add = smo8800_add,
+		.remove = smo8800_remove,
+	},
+	.owner = THIS_MODULE,
+};
+
+module_acpi_driver(smo8800_driver);
+
+MODULE_DESCRIPTION("Dell Latitude freefall driver (ACPI SMO8800/SMO8810)");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Sonal Santan, Pali Rohár");
diff --git a/drivers/platform/x86/eeepc-laptop.c b/drivers/platform/x86/eeepc-laptop.c
index 399e8c56219..9b0c57cd1d4 100644
--- a/drivers/platform/x86/eeepc-laptop.c
+++ b/drivers/platform/x86/eeepc-laptop.c
@@ -633,8 +633,7 @@ static void eeepc_rfkill_hotplug(struct eeepc_laptop *eeepc, acpi_handle handle)
 			dev = pci_scan_single_device(bus, 0);
 			if (dev) {
 				pci_bus_assign_resources(bus);
-				if (pci_bus_add_device(dev))
-					pr_err("Unable to hotplug wifi\n");
+				pci_bus_add_device(dev);
 			}
 		} else {
 			dev = pci_get_slot(bus, 0);
diff --git a/drivers/platform/x86/hp-wmi.c b/drivers/platform/x86/hp-wmi.c
index 8ba8956b5a4..484a8673b83 100644
--- a/drivers/platform/x86/hp-wmi.c
+++ b/drivers/platform/x86/hp-wmi.c
@@ -53,6 +53,7 @@ MODULE_ALIAS("wmi:5FB7F034-2C63-45e9-BE91-3D44E2C707E4");
 #define HPWMI_ALS_QUERY 0x3
 #define HPWMI_HARDWARE_QUERY 0x4
 #define HPWMI_WIRELESS_QUERY 0x5
+#define HPWMI_BIOS_QUERY 0x9
 #define HPWMI_HOTKEY_QUERY 0xc
 #define HPWMI_FEATURE_QUERY 0xd
 #define HPWMI_WIRELESS2_QUERY 0x1b
@@ -144,6 +145,7 @@ static const struct key_entry hp_wmi_keymap[] = {
 	{ KE_KEY, 0x2142, { KEY_MEDIA } },
 	{ KE_KEY, 0x213b, { KEY_INFO } },
 	{ KE_KEY, 0x2169, { KEY_DIRECTION } },
+	{ KE_KEY, 0x216a, { KEY_SETUP } },
 	{ KE_KEY, 0x231b, { KEY_HELP } },
 	{ KE_END, 0 }
 };
@@ -304,6 +306,19 @@ static int hp_wmi_bios_2009_later(void)
 	return (state & 0x10) ? 1 : 0;
 }
 
+static int hp_wmi_enable_hotkeys(void)
+{
+	int ret;
+	int query = 0x6e;
+
+	ret = hp_wmi_perform_query(HPWMI_BIOS_QUERY, 1, &query, sizeof(query),
+				   0);
+
+	if (ret)
+		return -EINVAL;
+	return 0;
+}
+
 static int hp_wmi_set_block(void *data, bool blocked)
 {
 	enum hp_wmi_radio r = (enum hp_wmi_radio) data;
@@ -648,6 +663,9 @@ static int __init hp_wmi_input_setup(void)
 			    hp_wmi_tablet_state());
 	input_sync(hp_wmi_input_dev);
 
+	if (hp_wmi_bios_2009_later() == 4)
+		hp_wmi_enable_hotkeys();
+
 	status = wmi_install_notify_handler(HPWMI_EVENT_GUID, hp_wmi_notify, NULL);
 	if (ACPI_FAILURE(status)) {
 		err = -EIO;
diff --git a/drivers/platform/x86/ideapad-laptop.c b/drivers/platform/x86/ideapad-laptop.c
index 6dd060a0bb6..b4c495a62ee 100644
--- a/drivers/platform/x86/ideapad-laptop.c
+++ b/drivers/platform/x86/ideapad-laptop.c
@@ -36,6 +36,8 @@
 #include <linux/debugfs.h>
 #include <linux/seq_file.h>
 #include <linux/i8042.h>
+#include <linux/dmi.h>
+#include <linux/device.h>
 
 #define IDEAPAD_RFKILL_DEV_NUM	(3)
 
@@ -819,6 +821,19 @@ static void ideapad_acpi_notify(acpi_handle handle, u32 event, void *data)
 	}
 }
 
+/* Blacklist for devices where the ideapad rfkill interface does not work */
+static struct dmi_system_id rfkill_blacklist[] = {
+	/* The Lenovo Yoga 2 11 always reports everything as blocked */
+	{
+		.ident = "Lenovo Yoga 2 11",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
+			DMI_MATCH(DMI_PRODUCT_VERSION, "Lenovo Yoga 2 11"),
+		},
+	},
+	{}
+};
+
 static int ideapad_acpi_add(struct platform_device *pdev)
 {
 	int ret, i;
@@ -833,7 +848,7 @@ static int ideapad_acpi_add(struct platform_device *pdev)
 	if (read_method_int(adev->handle, "_CFG", &cfg))
 		return -ENODEV;
 
-	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
 
@@ -844,7 +859,7 @@ static int ideapad_acpi_add(struct platform_device *pdev)
 
 	ret = ideapad_sysfs_init(priv);
 	if (ret)
-		goto sysfs_failed;
+		return ret;
 
 	ret = ideapad_debugfs_init(priv);
 	if (ret)
@@ -854,11 +869,10 @@ static int ideapad_acpi_add(struct platform_device *pdev)
 	if (ret)
 		goto input_failed;
 
-	for (i = 0; i < IDEAPAD_RFKILL_DEV_NUM; i++) {
-		if (test_bit(ideapad_rfk_data[i].cfgbit, &priv->cfg))
-			ideapad_register_rfkill(priv, i);
-		else
-			priv->rfk[i] = NULL;
+	if (!dmi_check_system(rfkill_blacklist)) {
+		for (i = 0; i < IDEAPAD_RFKILL_DEV_NUM; i++)
+			if (test_bit(ideapad_rfk_data[i].cfgbit, &priv->cfg))
+				ideapad_register_rfkill(priv, i);
 	}
 	ideapad_sync_rfk_state(priv);
 	ideapad_sync_touchpad_state(priv);
@@ -884,8 +898,6 @@ input_failed:
 	ideapad_debugfs_exit(priv);
 debugfs_failed:
 	ideapad_sysfs_exit(priv);
-sysfs_failed:
-	kfree(priv);
 	return ret;
 }
 
@@ -903,7 +915,6 @@ static int ideapad_acpi_remove(struct platform_device *pdev)
 	ideapad_debugfs_exit(priv);
 	ideapad_sysfs_exit(priv);
 	dev_set_drvdata(&pdev->dev, NULL);
-	kfree(priv);
 
 	return 0;
 }
diff --git a/drivers/platform/x86/intel_mid_thermal.c b/drivers/platform/x86/intel_mid_thermal.c
index 93fab8b70ce..ab7860a21a2 100644
--- a/drivers/platform/x86/intel_mid_thermal.c
+++ b/drivers/platform/x86/intel_mid_thermal.c
@@ -481,7 +481,8 @@ static int mid_thermal_probe(struct platform_device *pdev)
 	int i;
 	struct platform_info *pinfo;
 
-	pinfo = kzalloc(sizeof(struct platform_info), GFP_KERNEL);
+	pinfo = devm_kzalloc(&pdev->dev, sizeof(struct platform_info),
+			     GFP_KERNEL);
 	if (!pinfo)
 		return -ENOMEM;
 
@@ -489,7 +490,6 @@ static int mid_thermal_probe(struct platform_device *pdev)
 	ret = mid_initialize_adc(&pdev->dev);
 	if (ret) {
 		dev_err(&pdev->dev, "ADC init failed");
-		kfree(pinfo);
 		return ret;
 	}
 
@@ -520,7 +520,6 @@ err:
 		thermal_zone_device_unregister(pinfo->tzd[i]);
 	}
 	configure_adc(0);
-	kfree(pinfo);
 	return ret;
 }
 
@@ -541,8 +540,6 @@ static int mid_thermal_remove(struct platform_device *pdev)
 		thermal_zone_device_unregister(pinfo->tzd[i]);
 	}
 
-	kfree(pinfo);
-
 	/* Stop the ADC */
 	return configure_adc(0);
 }
diff --git a/drivers/platform/x86/intel_pmic_gpio.c b/drivers/platform/x86/intel_pmic_gpio.c
index 2805988485f..40929e4f7ad 100644
--- a/drivers/platform/x86/intel_pmic_gpio.c
+++ b/drivers/platform/x86/intel_pmic_gpio.c
@@ -91,7 +91,7 @@ static void pmic_program_irqtype(int gpio, int type)
 
 static int pmic_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 {
-	if (offset > 8) {
+	if (offset >= 8) {
 		pr_err("only pin 0-7 support input\n");
 		return -1;/* we only have 8 GPIO can use as input */
 	}
@@ -130,7 +130,7 @@ static int pmic_gpio_get(struct gpio_chip *chip, unsigned offset)
 	int ret;
 
 	/* we only have 8 GPIO pins we can use as input */
-	if (offset > 8)
+	if (offset >= 8)
 		return -EOPNOTSUPP;
 	ret = intel_scu_ipc_ioread8(GPIO0 + offset, &r);
 	if (ret < 0)
diff --git a/drivers/platform/x86/pvpanic.c b/drivers/platform/x86/pvpanic.c
index c9f6e511daa..073a90a63db 100644
--- a/drivers/platform/x86/pvpanic.c
+++ b/drivers/platform/x86/pvpanic.c
@@ -70,6 +70,7 @@ pvpanic_panic_notify(struct notifier_block *nb, unsigned long code,
 
 static struct notifier_block pvpanic_panic_nb = {
 	.notifier_call = pvpanic_panic_notify,
+	.priority = 1, /* let this called before broken drm_fb_helper */
 };
 
 
diff --git a/drivers/platform/x86/samsung-laptop.c b/drivers/platform/x86/samsung-laptop.c
index d1f03005317..5a596651227 100644
--- a/drivers/platform/x86/samsung-laptop.c
+++ b/drivers/platform/x86/samsung-laptop.c
@@ -27,6 +27,7 @@
 #include <linux/debugfs.h>
 #include <linux/ctype.h>
 #include <linux/efi.h>
+#include <linux/suspend.h>
 #include <acpi/video.h>
 
 /*
@@ -340,6 +341,8 @@ struct samsung_laptop {
 	struct samsung_laptop_debug debug;
 	struct samsung_quirks *quirks;
 
+	struct notifier_block pm_nb;
+
 	bool handle_backlight;
 	bool has_stepping_quirk;
 
@@ -348,6 +351,8 @@ struct samsung_laptop {
 
 struct samsung_quirks {
 	bool broken_acpi_video;
+	bool four_kbd_backlight_levels;
+	bool enable_kbd_backlight;
 };
 
 static struct samsung_quirks samsung_unknown = {};
@@ -356,6 +361,11 @@ static struct samsung_quirks samsung_broken_acpi_video = {
 	.broken_acpi_video = true,
 };
 
+static struct samsung_quirks samsung_np740u3e = {
+	.four_kbd_backlight_levels = true,
+	.enable_kbd_backlight = true,
+};
+
 static bool force;
 module_param(force, bool, 0);
 MODULE_PARM_DESC(force,
@@ -1051,6 +1061,8 @@ static int __init samsung_leds_init(struct samsung_laptop *samsung)
 		samsung->kbd_led.brightness_set = kbd_led_set;
 		samsung->kbd_led.brightness_get = kbd_led_get;
 		samsung->kbd_led.max_brightness = 8;
+		if (samsung->quirks->four_kbd_backlight_levels)
+			samsung->kbd_led.max_brightness = 4;
 
 		ret = led_classdev_register(&samsung->platform_device->dev,
 					   &samsung->kbd_led);
@@ -1414,6 +1426,19 @@ static void samsung_platform_exit(struct samsung_laptop *samsung)
 	}
 }
 
+static int samsung_pm_notification(struct notifier_block *nb,
+				   unsigned long val, void *ptr)
+{
+	struct samsung_laptop *samsung;
+
+	samsung = container_of(nb, struct samsung_laptop, pm_nb);
+	if (val == PM_POST_HIBERNATION &&
+	    samsung->quirks->enable_kbd_backlight)
+		kbd_backlight_enable(samsung);
+
+	return 0;
+}
+
 static int __init samsung_platform_init(struct samsung_laptop *samsung)
 {
 	struct platform_device *pdev;
@@ -1534,6 +1559,15 @@ static struct dmi_system_id __initdata samsung_dmi_table[] = {
 		},
 	 .driver_data = &samsung_broken_acpi_video,
 	},
+	{
+	 .callback = samsung_dmi_matched,
+	 .ident = "730U3E/740U3E",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "SAMSUNG ELECTRONICS CO., LTD."),
+		DMI_MATCH(DMI_PRODUCT_NAME, "730U3E/740U3E"),
+		},
+	 .driver_data = &samsung_np740u3e,
+	},
 	{ },
 };
 MODULE_DEVICE_TABLE(dmi, samsung_dmi_table);
@@ -1608,6 +1642,9 @@ static int __init samsung_init(void)
 	if (ret)
 		goto error_debugfs;
 
+	samsung->pm_nb.notifier_call = samsung_pm_notification;
+	register_pm_notifier(&samsung->pm_nb);
+
 	samsung_platform_device = samsung->platform_device;
 	return ret;
 
@@ -1633,6 +1670,7 @@ static void __exit samsung_exit(void)
 	struct samsung_laptop *samsung;
 
 	samsung = platform_get_drvdata(samsung_platform_device);
+	unregister_pm_notifier(&samsung->pm_nb);
 
 	samsung_debugfs_exit(samsung);
 	samsung_leds_exit(samsung);
diff --git a/drivers/platform/x86/thinkpad_acpi.c b/drivers/platform/x86/thinkpad_acpi.c
index 15e61c16736..d82f196e3cf 100644
--- a/drivers/platform/x86/thinkpad_acpi.c
+++ b/drivers/platform/x86/thinkpad_acpi.c
@@ -3171,8 +3171,10 @@ static int __init hotkey_init(struct ibm_init_struct *iibm)
 		KEY_MICMUTE,	/* 0x1a: Mic mute (since ?400 or so) */
 
 		/* (assignments unknown, please report if found) */
-		KEY_UNKNOWN, KEY_UNKNOWN, KEY_UNKNOWN, KEY_UNKNOWN,
 		KEY_UNKNOWN,
+
+		/* Extra keys in use since the X240 / T440 / T540 */
+		KEY_CONFIG, KEY_SEARCH, KEY_SCALE, KEY_COMPUTER,
 		},
 	};
 
diff --git a/drivers/platform/x86/toshiba_acpi.c b/drivers/platform/x86/toshiba_acpi.c
index 46473ca7566..76441dcbe5f 100644
--- a/drivers/platform/x86/toshiba_acpi.c
+++ b/drivers/platform/x86/toshiba_acpi.c
@@ -56,6 +56,7 @@
 #include <linux/workqueue.h>
 #include <linux/i8042.h>
 #include <linux/acpi.h>
+#include <linux/dmi.h>
 #include <asm/uaccess.h>
 
 MODULE_AUTHOR("John Belmonte");
@@ -213,6 +214,30 @@ static const struct key_entry toshiba_acpi_keymap[] = {
 	{ KE_END, 0 },
 };
 
+/* alternative keymap */
+static const struct dmi_system_id toshiba_alt_keymap_dmi[] = {
+	{
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "TOSHIBA"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "Satellite M840"),
+		},
+	},
+	{}
+};
+
+static const struct key_entry toshiba_acpi_alt_keymap[] = {
+	{ KE_KEY, 0x157, { KEY_MUTE } },
+	{ KE_KEY, 0x102, { KEY_ZOOMOUT } },
+	{ KE_KEY, 0x103, { KEY_ZOOMIN } },
+	{ KE_KEY, 0x139, { KEY_ZOOMRESET } },
+	{ KE_KEY, 0x13e, { KEY_SWITCHVIDEOMODE } },
+	{ KE_KEY, 0x13c, { KEY_BRIGHTNESSDOWN } },
+	{ KE_KEY, 0x13d, { KEY_BRIGHTNESSUP } },
+	{ KE_KEY, 0x158, { KEY_WLAN } },
+	{ KE_KEY, 0x13f, { KEY_TOUCHPAD_TOGGLE } },
+	{ KE_END, 0 },
+};
+
 /* utility
  */
 
@@ -1440,6 +1465,7 @@ static int toshiba_acpi_setup_keyboard(struct toshiba_acpi_dev *dev)
 	acpi_handle ec_handle;
 	int error;
 	u32 hci_result;
+	const struct key_entry *keymap = toshiba_acpi_keymap;
 
 	dev->hotkey_dev = input_allocate_device();
 	if (!dev->hotkey_dev)
@@ -1449,7 +1475,9 @@ static int toshiba_acpi_setup_keyboard(struct toshiba_acpi_dev *dev)
 	dev->hotkey_dev->phys = "toshiba_acpi/input0";
 	dev->hotkey_dev->id.bustype = BUS_HOST;
 
-	error = sparse_keymap_setup(dev->hotkey_dev, toshiba_acpi_keymap, NULL);
+	if (dmi_check_system(toshiba_alt_keymap_dmi))
+		keymap = toshiba_acpi_alt_keymap;
+	error = sparse_keymap_setup(dev->hotkey_dev, keymap, NULL);
 	if (error)
 		goto err_free_dev;
 
diff --git a/drivers/pnp/pnpacpi/core.c b/drivers/pnp/pnpacpi/core.c
index c31aa07b3ba..b81448b2c75 100644
--- a/drivers/pnp/pnpacpi/core.c
+++ b/drivers/pnp/pnpacpi/core.c
@@ -30,26 +30,6 @@
 
 static int num;
 
-/* We need only to blacklist devices that have already an acpi driver that
- * can't use pnp layer. We don't need to blacklist device that are directly
- * used by the kernel (PCI root, ...), as it is harmless and there were
- * already present in pnpbios. But there is an exception for devices that
- * have irqs (PIC, Timer) because we call acpi_register_gsi.
- * Finally, only devices that have a CRS method need to be in this list.
- */
-static struct acpi_device_id excluded_id_list[] __initdata = {
-	{"PNP0C09", 0},		/* EC */
-	{"PNP0C0F", 0},		/* Link device */
-	{"PNP0000", 0},		/* PIC */
-	{"PNP0100", 0},		/* Timer */
-	{"", 0},
-};
-
-static inline int __init is_exclusive_device(struct acpi_device *dev)
-{
-	return (!acpi_match_device_ids(dev, excluded_id_list));
-}
-
 /*
  * Compatible Device IDs
  */
@@ -266,7 +246,7 @@ static int __init pnpacpi_add_device(struct acpi_device *device)
 	if (!pnpid)
 		return 0;
 
-	if (is_exclusive_device(device) || !device->status.present)
+	if (!device->status.present)
 		return 0;
 
 	dev = pnp_alloc_dev(&pnpacpi_protocol, num, pnpid);
@@ -326,10 +306,10 @@ static acpi_status __init pnpacpi_add_device_handler(acpi_handle handle,
 {
 	struct acpi_device *device;
 
-	if (!acpi_bus_get_device(handle, &device))
-		pnpacpi_add_device(device);
-	else
+	if (acpi_bus_get_device(handle, &device))
 		return AE_CTRL_DEPTH;
+	if (acpi_is_pnp_device(device))
+		pnpacpi_add_device(device);
 	return AE_OK;
 }
 
diff --git a/drivers/pnp/resource.c b/drivers/pnp/resource.c
index 01712cbfd92..782e8228957 100644
--- a/drivers/pnp/resource.c
+++ b/drivers/pnp/resource.c
@@ -360,7 +360,7 @@ int pnp_check_irq(struct pnp_dev *dev, struct resource *res)
 		return 1;
 
 	/* check if the resource is valid */
-	if (*irq < 0 || *irq > 15)
+	if (*irq > 15)
 		return 0;
 
 	/* check if the resource is reserved */
@@ -424,7 +424,7 @@ int pnp_check_dma(struct pnp_dev *dev, struct resource *res)
 		return 1;
 
 	/* check if the resource is valid */
-	if (*dma < 0 || *dma == 4 || *dma > 7)
+	if (*dma == 4 || *dma > 7)
 		return 0;
 
 	/* check if the resource is reserved */
diff --git a/drivers/power/power_supply_core.c b/drivers/power/power_supply_core.c
index 26606641fe4..5a5a24e7d43 100644
--- a/drivers/power/power_supply_core.c
+++ b/drivers/power/power_supply_core.c
@@ -537,7 +537,7 @@ static void psy_unregister_cooler(struct power_supply *psy)
 }
 #endif
 
-int power_supply_register(struct device *parent, struct power_supply *psy)
+int __power_supply_register(struct device *parent, struct power_supply *psy, bool ws)
 {
 	struct device *dev;
 	int rc;
@@ -568,7 +568,7 @@ int power_supply_register(struct device *parent, struct power_supply *psy)
 	}
 
 	spin_lock_init(&psy->changed_lock);
-	rc = device_init_wakeup(dev, true);
+	rc = device_init_wakeup(dev, ws);
 	if (rc)
 		goto wakeup_init_failed;
 
@@ -606,8 +606,19 @@ dev_set_name_failed:
 success:
 	return rc;
 }
+
+int power_supply_register(struct device *parent, struct power_supply *psy)
+{
+	return __power_supply_register(parent, psy, true);
+}
 EXPORT_SYMBOL_GPL(power_supply_register);
 
+int power_supply_register_no_ws(struct device *parent, struct power_supply *psy)
+{
+	return __power_supply_register(parent, psy, false);
+}
+EXPORT_SYMBOL_GPL(power_supply_register_no_ws);
+
 void power_supply_unregister(struct power_supply *psy)
 {
 	cancel_work_sync(&psy->changed_work);
diff --git a/drivers/power/reset/Kconfig b/drivers/power/reset/Kconfig
index fa0e4e057b9..bdcf5173e37 100644
--- a/drivers/power/reset/Kconfig
+++ b/drivers/power/reset/Kconfig
@@ -12,6 +12,14 @@ config POWER_RESET_AS3722
 	help
 	  This driver supports turning off board via a ams AS3722 power-off.
 
+config POWER_RESET_AXXIA
+	bool "LSI Axxia reset driver"
+	depends on POWER_RESET && ARCH_AXXIA
+	help
+	  This driver supports restart for Axxia SoC.
+
+	  Say Y if you have an Axxia family SoC.
+
 config POWER_RESET_GPIO
 	bool "GPIO power-off driver"
 	depends on OF_GPIO && POWER_RESET
@@ -43,6 +51,13 @@ config POWER_RESET_RESTART
 	  Instead they restart, and u-boot holds the SoC until the
 	  user presses a key. u-boot then boots into Linux.
 
+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_VEXPRESS
 	bool "ARM Versatile Express power-off and reset driver"
 	depends on ARM || ARM64
@@ -57,3 +72,11 @@ config POWER_RESET_XGENE
 	depends on POWER_RESET
 	help
 	  Reboot support for the APM SoC X-Gene Eval boards.
+
+config POWER_RESET_KEYSTONE
+	bool "Keystone reset driver"
+	depends on ARCH_KEYSTONE
+	select MFD_SYSCON
+	help
+	  Reboot support for the KEYSTONE SoCs.
+
diff --git a/drivers/power/reset/Makefile b/drivers/power/reset/Makefile
index a5b4a77d1a4..dde2e8bbac5 100644
--- a/drivers/power/reset/Makefile
+++ b/drivers/power/reset/Makefile
@@ -1,7 +1,10 @@
 obj-$(CONFIG_POWER_RESET_AS3722) += as3722-poweroff.o
+obj-$(CONFIG_POWER_RESET_AXXIA) += axxia-reset.o
 obj-$(CONFIG_POWER_RESET_GPIO) += gpio-poweroff.o
 obj-$(CONFIG_POWER_RESET_MSM) += msm-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_VEXPRESS) += vexpress-poweroff.o
 obj-$(CONFIG_POWER_RESET_XGENE) += xgene-reboot.o
+obj-$(CONFIG_POWER_RESET_KEYSTONE) += keystone-reset.o
diff --git a/drivers/power/reset/axxia-reset.c b/drivers/power/reset/axxia-reset.c
new file mode 100644
index 00000000000..3b1f8d60178
--- /dev/null
+++ b/drivers/power/reset/axxia-reset.c
@@ -0,0 +1,88 @@
+/*
+ * Reset driver for Axxia devices
+ *
+ * Copyright (C) 2014 LSI
+ *
+ * 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/init.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/reboot.h>
+#include <linux/regmap.h>
+
+#include <asm/system_misc.h>
+
+
+#define SC_CRIT_WRITE_KEY	0x1000
+#define SC_LATCH_ON_RESET	0x1004
+#define SC_RESET_CONTROL	0x1008
+#define   RSTCTL_RST_ZERO	(1<<3)
+#define   RSTCTL_RST_FAB	(1<<2)
+#define   RSTCTL_RST_CHIP	(1<<1)
+#define   RSTCTL_RST_SYS	(1<<0)
+#define SC_EFUSE_INT_STATUS	0x180c
+#define   EFUSE_READ_DONE	(1<<31)
+
+static struct regmap *syscon;
+
+static void do_axxia_restart(enum reboot_mode reboot_mode, const char *cmd)
+{
+	/* Access Key (0xab) */
+	regmap_write(syscon, SC_CRIT_WRITE_KEY, 0xab);
+	/* Select internal boot from 0xffff0000 */
+	regmap_write(syscon, SC_LATCH_ON_RESET, 0x00000040);
+	/* Assert ResetReadDone (to avoid hanging in boot ROM) */
+	regmap_write(syscon, SC_EFUSE_INT_STATUS, EFUSE_READ_DONE);
+	/* Assert chip reset */
+	regmap_update_bits(syscon, SC_RESET_CONTROL,
+			   RSTCTL_RST_CHIP, RSTCTL_RST_CHIP);
+}
+
+static int axxia_reset_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+
+	syscon = syscon_regmap_lookup_by_phandle(dev->of_node, "syscon");
+	if (IS_ERR(syscon)) {
+		pr_err("%s: syscon lookup failed\n", dev->of_node->name);
+		return PTR_ERR(syscon);
+	}
+
+	arm_pm_restart = do_axxia_restart;
+
+	return 0;
+}
+
+static const struct of_device_id of_axxia_reset_match[] = {
+	{ .compatible = "lsi,axm55xx-reset", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, of_axxia_reset_match);
+
+static struct platform_driver axxia_reset_driver = {
+	.probe = axxia_reset_probe,
+	.driver = {
+		.name = "axxia-reset",
+		.of_match_table = of_match_ptr(of_axxia_reset_match),
+	},
+};
+
+static int __init axxia_reset_init(void)
+{
+	return platform_driver_register(&axxia_reset_driver);
+}
+device_initcall(axxia_reset_init);
diff --git a/drivers/power/reset/keystone-reset.c b/drivers/power/reset/keystone-reset.c
new file mode 100644
index 00000000000..408a18fd91c
--- /dev/null
+++ b/drivers/power/reset/keystone-reset.c
@@ -0,0 +1,166 @@
+/*
+ * TI keystone reboot driver
+ *
+ * Copyright (C) 2014 Texas Instruments Incorporated. http://www.ti.com/
+ *
+ * Author: Ivan Khoronzhuk <ivan.khoronzhuk@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/io.h>
+#include <linux/module.h>
+#include <linux/reboot.h>
+#include <linux/regmap.h>
+#include <asm/system_misc.h>
+#include <linux/mfd/syscon.h>
+#include <linux/of_platform.h>
+
+#define RSTYPE_RG			0x0
+#define RSCTRL_RG			0x4
+#define RSCFG_RG			0x8
+#define RSISO_RG			0xc
+
+#define RSCTRL_KEY_MASK			0x0000ffff
+#define RSCTRL_RESET_MASK		BIT(16)
+#define RSCTRL_KEY			0x5a69
+
+#define RSMUX_OMODE_MASK		0xe
+#define RSMUX_OMODE_RESET_ON		0xa
+#define RSMUX_OMODE_RESET_OFF		0x0
+#define RSMUX_LOCK_MASK			0x1
+#define RSMUX_LOCK_SET			0x1
+
+#define RSCFG_RSTYPE_SOFT		0x300f
+#define RSCFG_RSTYPE_HARD		0x0
+
+#define WDT_MUX_NUMBER			0x4
+
+static int rspll_offset;
+static struct regmap *pllctrl_regs;
+
+/**
+ * rsctrl_enable_rspll_write - enable access to RSCTRL, RSCFG
+ * To be able to access to RSCTRL, RSCFG registers
+ * we have to write a key before
+ */
+static inline int rsctrl_enable_rspll_write(void)
+{
+	return regmap_update_bits(pllctrl_regs, rspll_offset + RSCTRL_RG,
+				  RSCTRL_KEY_MASK, RSCTRL_KEY);
+}
+
+static void rsctrl_restart(enum reboot_mode mode, const char *cmd)
+{
+	/* enable write access to RSTCTRL */
+	rsctrl_enable_rspll_write();
+
+	/* reset the SOC */
+	regmap_update_bits(pllctrl_regs, rspll_offset + RSCTRL_RG,
+			   RSCTRL_RESET_MASK, 0);
+}
+
+static struct of_device_id rsctrl_of_match[] = {
+	{.compatible = "ti,keystone-reset", },
+	{},
+};
+
+static int rsctrl_probe(struct platform_device *pdev)
+{
+	int i;
+	int ret;
+	u32 val;
+	unsigned int rg;
+	u32 rsmux_offset;
+	struct regmap *devctrl_regs;
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+
+	if (!np)
+		return -ENODEV;
+
+	/* get regmaps */
+	pllctrl_regs = syscon_regmap_lookup_by_phandle(np, "ti,syscon-pll");
+	if (IS_ERR(pllctrl_regs))
+		return PTR_ERR(pllctrl_regs);
+
+	devctrl_regs = syscon_regmap_lookup_by_phandle(np, "ti,syscon-dev");
+	if (IS_ERR(devctrl_regs))
+		return PTR_ERR(devctrl_regs);
+
+	ret = of_property_read_u32_index(np, "ti,syscon-pll", 1, &rspll_offset);
+	if (ret) {
+		dev_err(dev, "couldn't read the reset pll offset!\n");
+		return -EINVAL;
+	}
+
+	ret = of_property_read_u32_index(np, "ti,syscon-dev", 1, &rsmux_offset);
+	if (ret) {
+		dev_err(dev, "couldn't read the rsmux offset!\n");
+		return -EINVAL;
+	}
+
+	/* set soft/hard reset */
+	val = of_property_read_bool(np, "ti,soft-reset");
+	val = val ? RSCFG_RSTYPE_SOFT : RSCFG_RSTYPE_HARD;
+
+	ret = rsctrl_enable_rspll_write();
+	if (ret)
+		return ret;
+
+	ret = regmap_write(pllctrl_regs, rspll_offset + RSCFG_RG, val);
+	if (ret)
+		return ret;
+
+	arm_pm_restart = rsctrl_restart;
+
+	/* disable a reset isolation for all module clocks */
+	ret = regmap_write(pllctrl_regs, rspll_offset + RSISO_RG, 0);
+	if (ret)
+		return ret;
+
+	/* enable a reset for watchdogs from wdt-list */
+	for (i = 0; i < WDT_MUX_NUMBER; i++) {
+		ret = of_property_read_u32_index(np, "ti,wdt-list", i, &val);
+		if (ret == -EOVERFLOW && !i) {
+			dev_err(dev, "ti,wdt-list property has to contain at"
+				"least one entry\n");
+			return -EINVAL;
+		} else if (ret) {
+			break;
+		}
+
+		if (val >= WDT_MUX_NUMBER) {
+			dev_err(dev, "ti,wdt-list property can contain"
+				"only numbers < 4\n");
+			return -EINVAL;
+		}
+
+		rg = rsmux_offset + val * 4;
+
+		ret = regmap_update_bits(devctrl_regs, rg, RSMUX_OMODE_MASK,
+					 RSMUX_OMODE_RESET_ON |
+					 RSMUX_LOCK_SET);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static struct platform_driver rsctrl_driver = {
+	.probe = rsctrl_probe,
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = KBUILD_MODNAME,
+		.of_match_table = rsctrl_of_match,
+	},
+};
+module_platform_driver(rsctrl_driver);
+
+MODULE_AUTHOR("Ivan Khoronzhuk <ivan.khoronzhuk@ti.com>");
+MODULE_DESCRIPTION("Texas Instruments keystone reset driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:" KBUILD_MODNAME);
diff --git a/drivers/power/reset/sun6i-reboot.c b/drivers/power/reset/sun6i-reboot.c
new file mode 100644
index 00000000000..af2cd7ff2fe
--- /dev/null
+++ b/drivers/power/reset/sun6i-reboot.c
@@ -0,0 +1,85 @@
+/*
+ * Allwinner A31 SoCs reset code
+ *
+ * Copyright (C) 2012-2014 Maxime Ripard
+ *
+ * Maxime Ripard <maxime.ripard@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/delay.h>
+#include <linux/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>
+
+#define SUN6I_WATCHDOG1_IRQ_REG		0x00
+#define SUN6I_WATCHDOG1_CTRL_REG	0x10
+#define SUN6I_WATCHDOG1_CTRL_RESTART		BIT(0)
+#define SUN6I_WATCHDOG1_CONFIG_REG	0x14
+#define SUN6I_WATCHDOG1_CONFIG_RESTART		BIT(0)
+#define SUN6I_WATCHDOG1_CONFIG_IRQ		BIT(1)
+#define SUN6I_WATCHDOG1_MODE_REG	0x18
+#define SUN6I_WATCHDOG1_MODE_ENABLE		BIT(0)
+
+static void __iomem *wdt_base;
+
+static void sun6i_wdt_restart(enum reboot_mode mode, const char *cmd)
+{
+	if (!wdt_base)
+		return;
+
+	/* Disable interrupts */
+	writel(0, wdt_base + SUN6I_WATCHDOG1_IRQ_REG);
+
+	/* We want to disable the IRQ and just reset the whole system */
+	writel(SUN6I_WATCHDOG1_CONFIG_RESTART,
+		wdt_base + SUN6I_WATCHDOG1_CONFIG_REG);
+
+	/* Enable timer. The default and lowest interval value is 0.5s */
+	writel(SUN6I_WATCHDOG1_MODE_ENABLE,
+		wdt_base + SUN6I_WATCHDOG1_MODE_REG);
+
+	/* Restart the watchdog. */
+	writel(SUN6I_WATCHDOG1_CTRL_RESTART,
+		wdt_base + SUN6I_WATCHDOG1_CTRL_REG);
+
+	while (1) {
+		mdelay(5);
+		writel(SUN6I_WATCHDOG1_MODE_ENABLE,
+			wdt_base + SUN6I_WATCHDOG1_MODE_REG);
+	}
+}
+
+static int sun6i_reboot_probe(struct platform_device *pdev)
+{
+	wdt_base = of_iomap(pdev->dev.of_node, 0);
+	if (!wdt_base) {
+		WARN(1, "failed to map watchdog base address");
+		return -ENODEV;
+	}
+
+	arm_pm_restart = sun6i_wdt_restart;
+
+	return 0;
+}
+
+static struct of_device_id sun6i_reboot_of_match[] = {
+	{ .compatible = "allwinner,sun6i-a31-wdt" },
+	{}
+};
+
+static struct platform_driver sun6i_reboot_driver = {
+	.probe = sun6i_reboot_probe,
+	.driver = {
+		.name = "sun6i-reboot",
+		.of_match_table = sun6i_reboot_of_match,
+	},
+};
+module_platform_driver(sun6i_reboot_driver);
diff --git a/drivers/power/reset/vexpress-poweroff.c b/drivers/power/reset/vexpress-poweroff.c
index b95cf71ed69..4dc102e2b23 100644
--- a/drivers/power/reset/vexpress-poweroff.c
+++ b/drivers/power/reset/vexpress-poweroff.c
@@ -23,10 +23,10 @@
 static void vexpress_reset_do(struct device *dev, const char *what)
 {
 	int err = -ENOENT;
-	struct vexpress_config_func *func = dev_get_drvdata(dev);
+	struct regmap *reg = dev_get_drvdata(dev);
 
-	if (func) {
-		err = vexpress_config_write(func, 0, 0);
+	if (reg) {
+		err = regmap_write(reg, 0, 0);
 		if (!err)
 			mdelay(1000);
 	}
@@ -91,17 +91,17 @@ static int vexpress_reset_probe(struct platform_device *pdev)
 	enum vexpress_reset_func func;
 	const struct of_device_id *match =
 			of_match_device(vexpress_reset_of_match, &pdev->dev);
-	struct vexpress_config_func *config_func;
+	struct regmap *regmap;
 
 	if (match)
 		func = (enum vexpress_reset_func)match->data;
 	else
 		func = pdev->id_entry->driver_data;
 
-	config_func = vexpress_config_func_get_by_dev(&pdev->dev);
-	if (!config_func)
-		return -EINVAL;
-	dev_set_drvdata(&pdev->dev, config_func);
+	regmap = devm_regmap_init_vexpress_config(&pdev->dev);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+	dev_set_drvdata(&pdev->dev, regmap);
 
 	switch (func) {
 	case FUNC_SHUTDOWN:
diff --git a/drivers/power/tps65090-charger.c b/drivers/power/tps65090-charger.c
index 8fc9d6df87f..1685f63b9e5 100644
--- a/drivers/power/tps65090-charger.c
+++ b/drivers/power/tps65090-charger.c
@@ -28,17 +28,6 @@
 
 #include <linux/mfd/tps65090.h>
 
-#define TPS65090_REG_INTR_STS	0x00
-#define TPS65090_REG_INTR_MASK	0x02
-#define TPS65090_REG_CG_CTRL0	0x04
-#define TPS65090_REG_CG_CTRL1	0x05
-#define TPS65090_REG_CG_CTRL2	0x06
-#define TPS65090_REG_CG_CTRL3	0x07
-#define TPS65090_REG_CG_CTRL4	0x08
-#define TPS65090_REG_CG_CTRL5	0x09
-#define TPS65090_REG_CG_STATUS1	0x0a
-#define TPS65090_REG_CG_STATUS2	0x0b
-
 #define TPS65090_CHARGER_ENABLE	BIT(0)
 #define TPS65090_VACG		BIT(1)
 #define TPS65090_NOITERM	BIT(5)
diff --git a/drivers/powercap/intel_rapl.c b/drivers/powercap/intel_rapl.c
index d9a0770b6c7..b1cda6ffdbc 100644
--- a/drivers/powercap/intel_rapl.c
+++ b/drivers/powercap/intel_rapl.c
@@ -951,7 +951,9 @@ static const struct x86_cpu_id rapl_ids[] = {
 	{ X86_VENDOR_INTEL, 6, 0x2d},/* Sandy Bridge EP */
 	{ X86_VENDOR_INTEL, 6, 0x37},/* Valleyview */
 	{ X86_VENDOR_INTEL, 6, 0x3a},/* Ivy Bridge */
-	{ X86_VENDOR_INTEL, 6, 0x45},/* Haswell */
+	{ X86_VENDOR_INTEL, 6, 0x3c},/* Haswell */
+	{ X86_VENDOR_INTEL, 6, 0x3d},/* Broadwell */
+	{ X86_VENDOR_INTEL, 6, 0x45},/* Haswell ULT */
 	/* TODO: Add more CPU IDs after testing */
 	{}
 };
@@ -1124,8 +1126,7 @@ err_cleanup_package:
 static int rapl_check_domain(int cpu, int domain)
 {
 	unsigned msr;
-	u64 val1, val2 = 0;
-	int retry = 0;
+	u64 val = 0;
 
 	switch (domain) {
 	case RAPL_DOMAIN_PACKAGE:
@@ -1144,26 +1145,13 @@ static int rapl_check_domain(int cpu, int domain)
 		pr_err("invalid domain id %d\n", domain);
 		return -EINVAL;
 	}
-	if (rdmsrl_safe_on_cpu(cpu, msr, &val1))
-		return -ENODEV;
-
-	/* PP1/uncore/graphics domain may not be active at the time of
-	 * driver loading. So skip further checks.
+	/* make sure domain counters are available and contains non-zero
+	 * values, otherwise skip it.
 	 */
-	if (domain == RAPL_DOMAIN_PP1)
-		return 0;
-	/* energy counters roll slowly on some domains */
-	while (++retry < 10) {
-		usleep_range(10000, 15000);
-		rdmsrl_safe_on_cpu(cpu, msr, &val2);
-		if ((val1 & ENERGY_STATUS_MASK) != (val2 & ENERGY_STATUS_MASK))
-			return 0;
-	}
-	/* if energy counter does not change, report as bad domain */
-	pr_info("domain %s energy ctr %llu:%llu not working, skip\n",
-		rapl_domain_names[domain], val1, val2);
+	if (rdmsrl_safe_on_cpu(cpu, msr, &val) || !val)
+		return -ENODEV;
 
-	return -ENODEV;
+	return 0;
 }
 
 /* Detect active and valid domains for the given CPU, caller must
@@ -1180,6 +1168,9 @@ static int rapl_detect_domains(struct rapl_package *rp, int cpu)
 		/* use physical package id to read counters */
 		if (!rapl_check_domain(cpu, i))
 			rp->domain_map |= 1 << i;
+		else
+			pr_warn("RAPL domain %s detection failed\n",
+				rapl_domain_names[i]);
 	}
 	rp->nr_domains = bitmap_weight(&rp->domain_map,	RAPL_DOMAIN_MAX);
 	if (!rp->nr_domains) {
diff --git a/drivers/ptp/Kconfig b/drivers/ptp/Kconfig
index 6963bdf5417..6aea373547f 100644
--- a/drivers/ptp/Kconfig
+++ b/drivers/ptp/Kconfig
@@ -6,6 +6,7 @@ menu "PTP clock support"
 
 config PTP_1588_CLOCK
 	tristate "PTP clock support"
+	depends on NET
 	select PPS
 	select NET_PTP_CLASSIFY
 	help
@@ -74,7 +75,7 @@ config DP83640_PHY
 config PTP_1588_CLOCK_PCH
 	tristate "Intel PCH EG20T as PTP clock"
 	depends on X86 || COMPILE_TEST
-	depends on HAS_IOMEM
+	depends on HAS_IOMEM && NET
 	select PTP_1588_CLOCK
 	help
 	  This driver adds support for using the PCH EG20T as a PTP
diff --git a/drivers/pwm/Kconfig b/drivers/pwm/Kconfig
index 5b34ff29ea3..4ad7b89a4cb 100644
--- a/drivers/pwm/Kconfig
+++ b/drivers/pwm/Kconfig
@@ -62,6 +62,15 @@ config PWM_ATMEL_TCB
 	  To compile this driver as a module, choose M here: the module
 	  will be called pwm-atmel-tcb.
 
+config PWM_BCM_KONA
+	tristate "Kona PWM support"
+	depends on ARCH_BCM_MOBILE
+	help
+	  Generic PWM framework driver for Broadcom Kona PWM block.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called pwm-bcm-kona.
+
 config PWM_BFIN
 	tristate "Blackfin PWM support"
 	depends on BFIN_GPTIMERS
diff --git a/drivers/pwm/Makefile b/drivers/pwm/Makefile
index e57d2c38a79..5c86a19d5d3 100644
--- a/drivers/pwm/Makefile
+++ b/drivers/pwm/Makefile
@@ -3,6 +3,7 @@ obj-$(CONFIG_PWM_SYSFS)		+= sysfs.o
 obj-$(CONFIG_PWM_AB8500)	+= pwm-ab8500.o
 obj-$(CONFIG_PWM_ATMEL)		+= pwm-atmel.o
 obj-$(CONFIG_PWM_ATMEL_TCB)	+= pwm-atmel-tcb.o
+obj-$(CONFIG_PWM_BCM_KONA)	+= pwm-bcm-kona.o
 obj-$(CONFIG_PWM_BFIN)		+= pwm-bfin.o
 obj-$(CONFIG_PWM_CLPS711X)	+= pwm-clps711x.o
 obj-$(CONFIG_PWM_EP93XX)	+= pwm-ep93xx.o
diff --git a/drivers/pwm/core.c b/drivers/pwm/core.c
index a80471399c2..4b66bf09ee5 100644
--- a/drivers/pwm/core.c
+++ b/drivers/pwm/core.c
@@ -661,10 +661,16 @@ struct pwm_device *pwm_get(struct device *dev, const char *con_id)
 		}
 	}
 
+	mutex_unlock(&pwm_lookup_lock);
+
 	if (chip)
 		pwm = pwm_request_from_chip(chip, index, con_id ?: dev_id);
+	if (IS_ERR(pwm))
+		return pwm;
+
+	pwm_set_period(pwm, p->period);
+	pwm_set_polarity(pwm, p->polarity);
 
-	mutex_unlock(&pwm_lookup_lock);
 
 	return pwm;
 }
diff --git a/drivers/pwm/pwm-ab8500.c b/drivers/pwm/pwm-ab8500.c
index 1d07a6f9937..4c07a8420b3 100644
--- a/drivers/pwm/pwm-ab8500.c
+++ b/drivers/pwm/pwm-ab8500.c
@@ -20,10 +20,6 @@
 #define AB8500_PWM_OUT_CTRL2_REG	0x61
 #define AB8500_PWM_OUT_CTRL7_REG	0x66
 
-/* backlight driver constants */
-#define ENABLE_PWM			1
-#define DISABLE_PWM			0
-
 struct ab8500_pwm_chip {
 	struct pwm_chip chip;
 };
@@ -64,7 +60,7 @@ static int ab8500_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 
 	ret = abx500_mask_and_set_register_interruptible(chip->dev,
 				AB8500_MISC, AB8500_PWM_OUT_CTRL7_REG,
-				1 << (chip->base - 1), ENABLE_PWM);
+				1 << (chip->base - 1), 1 << (chip->base - 1));
 	if (ret < 0)
 		dev_err(chip->dev, "%s: Failed to enable PWM, Error %d\n",
 							pwm->label, ret);
@@ -77,11 +73,10 @@ static void ab8500_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
 
 	ret = abx500_mask_and_set_register_interruptible(chip->dev,
 				AB8500_MISC, AB8500_PWM_OUT_CTRL7_REG,
-				1 << (chip->base - 1), DISABLE_PWM);
+				1 << (chip->base - 1), 0);
 	if (ret < 0)
 		dev_err(chip->dev, "%s: Failed to disable PWM, Error %d\n",
 							pwm->label, ret);
-	return;
 }
 
 static const struct pwm_ops ab8500_pwm_ops = {
@@ -101,10 +96,8 @@ static int ab8500_pwm_probe(struct platform_device *pdev)
 	 * device which is required for ab8500 read and write
 	 */
 	ab8500 = devm_kzalloc(&pdev->dev, sizeof(*ab8500), GFP_KERNEL);
-	if (ab8500 == NULL) {
-		dev_err(&pdev->dev, "failed to allocate memory\n");
+	if (ab8500 == NULL)
 		return -ENOMEM;
-	}
 
 	ab8500->chip.dev = &pdev->dev;
 	ab8500->chip.ops = &ab8500_pwm_ops;
diff --git a/drivers/pwm/pwm-atmel.c b/drivers/pwm/pwm-atmel.c
index 0adc952cc4e..6e700a541ca 100644
--- a/drivers/pwm/pwm-atmel.c
+++ b/drivers/pwm/pwm-atmel.c
@@ -357,6 +357,7 @@ static int atmel_pwm_probe(struct platform_device *pdev)
 
 	atmel_pwm->chip.base = -1;
 	atmel_pwm->chip.npwm = 4;
+	atmel_pwm->chip.can_sleep = true;
 	atmel_pwm->config = data->config;
 
 	ret = pwmchip_add(&atmel_pwm->chip);
diff --git a/drivers/pwm/pwm-bcm-kona.c b/drivers/pwm/pwm-bcm-kona.c
new file mode 100644
index 00000000000..02bc048892a
--- /dev/null
+++ b/drivers/pwm/pwm-bcm-kona.c
@@ -0,0 +1,318 @@
+/*
+ * 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 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/clk.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/math64.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/pwm.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+
+/*
+ * The Kona PWM has some unusual characteristics.  Here are the main points.
+ *
+ * 1) There is no disable bit and the hardware docs advise programming a zero
+ *    duty to achieve output equivalent to that of a normal disable operation.
+ *
+ * 2) Changes to prescale, duty, period, and polarity do not take effect until
+ *    a subsequent rising edge of the trigger bit.
+ *
+ * 3) If the smooth bit and trigger bit are both low, the output is a constant
+ *    high signal.  Otherwise, the earlier waveform continues to be output.
+ *
+ * 4) If the smooth bit is set on the rising edge of the trigger bit, output
+ *    will transition to the new settings on a period boundary (which could be
+ *    seconds away).  If the smooth bit is clear, new settings will be applied
+ *    as soon as possible (the hardware always has a 400ns delay).
+ *
+ * 5) When the external clock that feeds the PWM is disabled, output is pegged
+ *    high or low depending on its state at that exact instant.
+ */
+
+#define PWM_CONTROL_OFFSET			(0x00000000)
+#define PWM_CONTROL_SMOOTH_SHIFT(chan)		(24 + (chan))
+#define PWM_CONTROL_TYPE_SHIFT(chan)		(16 + (chan))
+#define PWM_CONTROL_POLARITY_SHIFT(chan)	(8 + (chan))
+#define PWM_CONTROL_TRIGGER_SHIFT(chan)		(chan)
+
+#define PRESCALE_OFFSET				(0x00000004)
+#define PRESCALE_SHIFT(chan)			((chan) << 2)
+#define PRESCALE_MASK(chan)			(0x7 << PRESCALE_SHIFT(chan))
+#define PRESCALE_MIN				(0x00000000)
+#define PRESCALE_MAX				(0x00000007)
+
+#define PERIOD_COUNT_OFFSET(chan)		(0x00000008 + ((chan) << 3))
+#define PERIOD_COUNT_MIN			(0x00000002)
+#define PERIOD_COUNT_MAX			(0x00ffffff)
+
+#define DUTY_CYCLE_HIGH_OFFSET(chan)		(0x0000000c + ((chan) << 3))
+#define DUTY_CYCLE_HIGH_MIN			(0x00000000)
+#define DUTY_CYCLE_HIGH_MAX			(0x00ffffff)
+
+struct kona_pwmc {
+	struct pwm_chip chip;
+	void __iomem *base;
+	struct clk *clk;
+};
+
+static inline struct kona_pwmc *to_kona_pwmc(struct pwm_chip *_chip)
+{
+	return container_of(_chip, struct kona_pwmc, chip);
+}
+
+static void kona_pwmc_apply_settings(struct kona_pwmc *kp, unsigned int chan)
+{
+	unsigned int value = readl(kp->base + PWM_CONTROL_OFFSET);
+
+	/* Clear trigger bit but set smooth bit to maintain old output */
+	value |= 1 << PWM_CONTROL_SMOOTH_SHIFT(chan);
+	value &= ~(1 << PWM_CONTROL_TRIGGER_SHIFT(chan));
+	writel(value, kp->base + PWM_CONTROL_OFFSET);
+
+	/* Set trigger bit and clear smooth bit to apply new settings */
+	value &= ~(1 << PWM_CONTROL_SMOOTH_SHIFT(chan));
+	value |= 1 << PWM_CONTROL_TRIGGER_SHIFT(chan);
+	writel(value, kp->base + PWM_CONTROL_OFFSET);
+}
+
+static int kona_pwmc_config(struct pwm_chip *chip, struct pwm_device *pwm,
+			    int duty_ns, int period_ns)
+{
+	struct kona_pwmc *kp = to_kona_pwmc(chip);
+	u64 val, div, rate;
+	unsigned long prescale = PRESCALE_MIN, pc, dc;
+	unsigned int value, chan = pwm->hwpwm;
+
+	/*
+	 * Find period count, duty count and prescale to suit duty_ns and
+	 * period_ns. This is done according to formulas described below:
+	 *
+	 * period_ns = 10^9 * (PRESCALE + 1) * PC / PWM_CLK_RATE
+	 * duty_ns = 10^9 * (PRESCALE + 1) * DC / PWM_CLK_RATE
+	 *
+	 * PC = (PWM_CLK_RATE * period_ns) / (10^9 * (PRESCALE + 1))
+	 * DC = (PWM_CLK_RATE * duty_ns) / (10^9 * (PRESCALE + 1))
+	 */
+
+	rate = clk_get_rate(kp->clk);
+
+	while (1) {
+		div = 1000000000;
+		div *= 1 + prescale;
+		val = rate * period_ns;
+		pc = div64_u64(val, div);
+		val = rate * duty_ns;
+		dc = div64_u64(val, div);
+
+		/* If duty_ns or period_ns are not achievable then return */
+		if (pc < PERIOD_COUNT_MIN || dc < DUTY_CYCLE_HIGH_MIN)
+			return -EINVAL;
+
+		/* If pc and dc are in bounds, the calculation is done */
+		if (pc <= PERIOD_COUNT_MAX && dc <= DUTY_CYCLE_HIGH_MAX)
+			break;
+
+		/* Otherwise, increase prescale and recalculate pc and dc */
+		if (++prescale > PRESCALE_MAX)
+			return -EINVAL;
+	}
+
+	/* If the PWM channel is enabled, write the settings to the HW */
+	if (test_bit(PWMF_ENABLED, &pwm->flags)) {
+		value = readl(kp->base + PRESCALE_OFFSET);
+		value &= ~PRESCALE_MASK(chan);
+		value |= prescale << PRESCALE_SHIFT(chan);
+		writel(value, kp->base + PRESCALE_OFFSET);
+
+		writel(pc, kp->base + PERIOD_COUNT_OFFSET(chan));
+
+		writel(dc, kp->base + DUTY_CYCLE_HIGH_OFFSET(chan));
+
+		kona_pwmc_apply_settings(kp, chan);
+	}
+
+	return 0;
+}
+
+static int kona_pwmc_set_polarity(struct pwm_chip *chip, struct pwm_device *pwm,
+				  enum pwm_polarity polarity)
+{
+	struct kona_pwmc *kp = to_kona_pwmc(chip);
+	unsigned int chan = pwm->hwpwm;
+	unsigned int value;
+	int ret;
+
+	ret = clk_prepare_enable(kp->clk);
+	if (ret < 0) {
+		dev_err(chip->dev, "failed to enable clock: %d\n", ret);
+		return ret;
+	}
+
+	value = readl(kp->base + PWM_CONTROL_OFFSET);
+
+	if (polarity == PWM_POLARITY_NORMAL)
+		value |= 1 << PWM_CONTROL_POLARITY_SHIFT(chan);
+	else
+		value &= ~(1 << PWM_CONTROL_POLARITY_SHIFT(chan));
+
+	writel(value, kp->base + PWM_CONTROL_OFFSET);
+
+	kona_pwmc_apply_settings(kp, chan);
+
+	/* Wait for waveform to settle before gating off the clock */
+	ndelay(400);
+
+	clk_disable_unprepare(kp->clk);
+
+	return 0;
+}
+
+static int kona_pwmc_enable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct kona_pwmc *kp = to_kona_pwmc(chip);
+	int ret;
+
+	ret = clk_prepare_enable(kp->clk);
+	if (ret < 0) {
+		dev_err(chip->dev, "failed to enable clock: %d\n", ret);
+		return ret;
+	}
+
+	ret = kona_pwmc_config(chip, pwm, pwm->duty_cycle, pwm->period);
+	if (ret < 0) {
+		clk_disable_unprepare(kp->clk);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void kona_pwmc_disable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct kona_pwmc *kp = to_kona_pwmc(chip);
+	unsigned int chan = pwm->hwpwm;
+
+	/* Simulate a disable by configuring for zero duty */
+	writel(0, kp->base + DUTY_CYCLE_HIGH_OFFSET(chan));
+	kona_pwmc_apply_settings(kp, chan);
+
+	/* Wait for waveform to settle before gating off the clock */
+	ndelay(400);
+
+	clk_disable_unprepare(kp->clk);
+}
+
+static const struct pwm_ops kona_pwm_ops = {
+	.config = kona_pwmc_config,
+	.set_polarity = kona_pwmc_set_polarity,
+	.enable = kona_pwmc_enable,
+	.disable = kona_pwmc_disable,
+	.owner = THIS_MODULE,
+};
+
+static int kona_pwmc_probe(struct platform_device *pdev)
+{
+	struct kona_pwmc *kp;
+	struct resource *res;
+	unsigned int chan;
+	unsigned int value = 0;
+	int ret = 0;
+
+	kp = devm_kzalloc(&pdev->dev, sizeof(*kp), GFP_KERNEL);
+	if (kp == NULL)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, kp);
+
+	kp->chip.dev = &pdev->dev;
+	kp->chip.ops = &kona_pwm_ops;
+	kp->chip.base = -1;
+	kp->chip.npwm = 6;
+	kp->chip.of_xlate = of_pwm_xlate_with_flags;
+	kp->chip.of_pwm_n_cells = 3;
+	kp->chip.can_sleep = true;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	kp->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(kp->base))
+		return PTR_ERR(kp->base);
+
+	kp->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(kp->clk)) {
+		dev_err(&pdev->dev, "failed to get clock: %ld\n",
+			PTR_ERR(kp->clk));
+		return PTR_ERR(kp->clk);
+	}
+
+	ret = clk_prepare_enable(kp->clk);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to enable clock: %d\n", ret);
+		return ret;
+	}
+
+	/* Set smooth mode, push/pull, and normal polarity for all channels */
+	for (chan = 0; chan < kp->chip.npwm; chan++) {
+		value |= (1 << PWM_CONTROL_SMOOTH_SHIFT(chan));
+		value |= (1 << PWM_CONTROL_TYPE_SHIFT(chan));
+		value |= (1 << PWM_CONTROL_POLARITY_SHIFT(chan));
+	}
+
+	writel(value, kp->base + PWM_CONTROL_OFFSET);
+
+	clk_disable_unprepare(kp->clk);
+
+	ret = pwmchip_add(&kp->chip);
+	if (ret < 0)
+		dev_err(&pdev->dev, "failed to add PWM chip: %d\n", ret);
+
+	return ret;
+}
+
+static int kona_pwmc_remove(struct platform_device *pdev)
+{
+	struct kona_pwmc *kp = platform_get_drvdata(pdev);
+	unsigned int chan;
+
+	for (chan = 0; chan < kp->chip.npwm; chan++)
+		if (test_bit(PWMF_ENABLED, &kp->chip.pwms[chan].flags))
+			clk_disable_unprepare(kp->clk);
+
+	return pwmchip_remove(&kp->chip);
+}
+
+static const struct of_device_id bcm_kona_pwmc_dt[] = {
+	{ .compatible = "brcm,kona-pwm" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, bcm_kona_pwmc_dt);
+
+static struct platform_driver kona_pwmc_driver = {
+	.driver = {
+		.name = "bcm-kona-pwm",
+		.of_match_table = bcm_kona_pwmc_dt,
+	},
+	.probe = kona_pwmc_probe,
+	.remove = kona_pwmc_remove,
+};
+module_platform_driver(kona_pwmc_driver);
+
+MODULE_AUTHOR("Broadcom Corporation <bcm-kernel-feedback-list@broadcom.com>");
+MODULE_AUTHOR("Tim Kryger <tkryger@broadcom.com>");
+MODULE_DESCRIPTION("Broadcom Kona PWM driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pwm/pwm-fsl-ftm.c b/drivers/pwm/pwm-fsl-ftm.c
index 420169e96b5..a18bc8fea38 100644
--- a/drivers/pwm/pwm-fsl-ftm.c
+++ b/drivers/pwm/pwm-fsl-ftm.c
@@ -454,6 +454,7 @@ static int fsl_pwm_probe(struct platform_device *pdev)
 	fpc->chip.of_pwm_n_cells = 3;
 	fpc->chip.base = -1;
 	fpc->chip.npwm = 8;
+	fpc->chip.can_sleep = true;
 
 	ret = pwmchip_add(&fpc->chip);
 	if (ret < 0) {
diff --git a/drivers/pwm/pwm-imx.c b/drivers/pwm/pwm-imx.c
index cc477334487..d797c7b84c3 100644
--- a/drivers/pwm/pwm-imx.c
+++ b/drivers/pwm/pwm-imx.c
@@ -241,10 +241,8 @@ static int imx_pwm_probe(struct platform_device *pdev)
 		return -ENODEV;
 
 	imx = devm_kzalloc(&pdev->dev, sizeof(*imx), GFP_KERNEL);
-	if (imx == NULL) {
-		dev_err(&pdev->dev, "failed to allocate memory\n");
+	if (imx == NULL)
 		return -ENOMEM;
-	}
 
 	imx->clk_per = devm_clk_get(&pdev->dev, "per");
 	if (IS_ERR(imx->clk_per)) {
diff --git a/drivers/pwm/pwm-lp3943.c b/drivers/pwm/pwm-lp3943.c
index a40b9c34e9f..2c39b0e50fa 100644
--- a/drivers/pwm/pwm-lp3943.c
+++ b/drivers/pwm/pwm-lp3943.c
@@ -278,6 +278,7 @@ static int lp3943_pwm_probe(struct platform_device *pdev)
 	lp3943_pwm->chip.dev = &pdev->dev;
 	lp3943_pwm->chip.ops = &lp3943_pwm_ops;
 	lp3943_pwm->chip.npwm = LP3943_NUM_PWMS;
+	lp3943_pwm->chip.can_sleep = true;
 
 	platform_set_drvdata(pdev, lp3943_pwm);
 
diff --git a/drivers/pwm/pwm-lpss.c b/drivers/pwm/pwm-lpss.c
index 449e372050a..44ce6c6103a 100644
--- a/drivers/pwm/pwm-lpss.c
+++ b/drivers/pwm/pwm-lpss.c
@@ -6,6 +6,7 @@
  * Author: Chew Kean Ho <kean.ho.chew@intel.com>
  * Author: Chang Rebecca Swee Fun <rebecca.swee.fun.chang@intel.com>
  * Author: Chew Chiau Ee <chiau.ee.chew@intel.com>
+ * Author: Alan Cox <alan@linux.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
@@ -19,6 +20,9 @@
 #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 */
 
 #define PWM				0x00000000
 #define PWM_ENABLE			BIT(31)
@@ -34,6 +38,16 @@ struct pwm_lpss_chip {
 	struct pwm_chip chip;
 	void __iomem *regs;
 	struct clk *clk;
+	unsigned long clk_rate;
+};
+
+struct pwm_lpss_boardinfo {
+	unsigned long clk_rate;
+};
+
+/* BayTrail */
+static const struct pwm_lpss_boardinfo byt_info = {
+	25000000
 };
 
 static inline struct pwm_lpss_chip *to_lpwm(struct pwm_chip *chip)
@@ -55,7 +69,7 @@ static int pwm_lpss_config(struct pwm_chip *chip, struct pwm_device *pwm,
 	/* The equation is: base_unit = ((freq / c) * 65536) + correction */
 	base_unit = freq * 65536;
 
-	c = clk_get_rate(lpwm->clk);
+	c = lpwm->clk_rate;
 	if (!c)
 		return -EINVAL;
 
@@ -113,52 +127,48 @@ static const struct pwm_ops pwm_lpss_ops = {
 	.owner = THIS_MODULE,
 };
 
-static const struct acpi_device_id pwm_lpss_acpi_match[] = {
-	{ "80860F09", 0 },
-	{ },
-};
-MODULE_DEVICE_TABLE(acpi, pwm_lpss_acpi_match);
-
-static int pwm_lpss_probe(struct platform_device *pdev)
+static struct pwm_lpss_chip *pwm_lpss_probe(struct device *dev,
+					    struct resource *r,
+					    const struct pwm_lpss_boardinfo *info)
 {
 	struct pwm_lpss_chip *lpwm;
-	struct resource *r;
 	int ret;
 
-	lpwm = devm_kzalloc(&pdev->dev, sizeof(*lpwm), GFP_KERNEL);
+	lpwm = devm_kzalloc(dev, sizeof(*lpwm), GFP_KERNEL);
 	if (!lpwm)
-		return -ENOMEM;
-
-	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+		return ERR_PTR(-ENOMEM);
 
-	lpwm->regs = devm_ioremap_resource(&pdev->dev, r);
+	lpwm->regs = devm_ioremap_resource(dev, r);
 	if (IS_ERR(lpwm->regs))
-		return PTR_ERR(lpwm->regs);
-
-	lpwm->clk = devm_clk_get(&pdev->dev, NULL);
-	if (IS_ERR(lpwm->clk)) {
-		dev_err(&pdev->dev, "failed to get PWM clock\n");
-		return PTR_ERR(lpwm->clk);
+		return ERR_CAST(lpwm->regs);
+
+	if (info) {
+		lpwm->clk_rate = info->clk_rate;
+	} else {
+		lpwm->clk = devm_clk_get(dev, NULL);
+		if (IS_ERR(lpwm->clk)) {
+			dev_err(dev, "failed to get PWM clock\n");
+			return ERR_CAST(lpwm->clk);
+		}
+		lpwm->clk_rate = clk_get_rate(lpwm->clk);
 	}
 
-	lpwm->chip.dev = &pdev->dev;
+	lpwm->chip.dev = dev;
 	lpwm->chip.ops = &pwm_lpss_ops;
 	lpwm->chip.base = -1;
 	lpwm->chip.npwm = 1;
 
 	ret = pwmchip_add(&lpwm->chip);
 	if (ret) {
-		dev_err(&pdev->dev, "failed to add PWM chip: %d\n", ret);
-		return ret;
+		dev_err(dev, "failed to add PWM chip: %d\n", ret);
+		return ERR_PTR(ret);
 	}
 
-	platform_set_drvdata(pdev, lpwm);
-	return 0;
+	return lpwm;
 }
 
-static int pwm_lpss_remove(struct platform_device *pdev)
+static int pwm_lpss_remove(struct pwm_lpss_chip *lpwm)
 {
-	struct pwm_lpss_chip *lpwm = platform_get_drvdata(pdev);
 	u32 ctrl;
 
 	ctrl = readl(lpwm->regs + PWM);
@@ -167,15 +177,104 @@ static int pwm_lpss_remove(struct platform_device *pdev)
 	return pwmchip_remove(&lpwm->chip);
 }
 
-static struct platform_driver pwm_lpss_driver = {
+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)
+{
+	struct pwm_lpss_chip *lpwm;
+	struct resource *r;
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	lpwm = pwm_lpss_probe(&pdev->dev, r, NULL);
+	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", 0 },
+	{ },
+};
+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,
-	.remove = pwm_lpss_remove,
+	.probe = pwm_lpss_probe_platform,
+	.remove = pwm_lpss_remove_platform,
 };
-module_platform_driver(pwm_lpss_driver);
+
+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);
 
 MODULE_DESCRIPTION("PWM driver for Intel LPSS");
 MODULE_AUTHOR("Mika Westerberg <mika.westerberg@linux.intel.com>");
diff --git a/drivers/pwm/pwm-mxs.c b/drivers/pwm/pwm-mxs.c
index 9475bc7a6f9..4f1bb4e0a42 100644
--- a/drivers/pwm/pwm-mxs.c
+++ b/drivers/pwm/pwm-mxs.c
@@ -147,6 +147,7 @@ static int mxs_pwm_probe(struct platform_device *pdev)
 	mxs->chip.dev = &pdev->dev;
 	mxs->chip.ops = &mxs_pwm_ops;
 	mxs->chip.base = -1;
+	mxs->chip.can_sleep = true;
 	ret = of_property_read_u32(np, "fsl,pwm-number", &mxs->chip.npwm);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "failed to get pwm number: %d\n", ret);
diff --git a/drivers/pwm/pwm-pxa.c b/drivers/pwm/pwm-pxa.c
index cd356d87024..0b312ec420b 100644
--- a/drivers/pwm/pwm-pxa.c
+++ b/drivers/pwm/pwm-pxa.c
@@ -179,10 +179,8 @@ static int pwm_probe(struct platform_device *pdev)
 		return -EINVAL;
 
 	pwm = devm_kzalloc(&pdev->dev, sizeof(*pwm), GFP_KERNEL);
-	if (pwm == NULL) {
-		dev_err(&pdev->dev, "failed to allocate memory\n");
+	if (pwm == NULL)
 		return -ENOMEM;
-	}
 
 	pwm->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(pwm->clk))
diff --git a/drivers/pwm/pwm-renesas-tpu.c b/drivers/pwm/pwm-renesas-tpu.c
index aff6ba9b49e..3b71b42e89d 100644
--- a/drivers/pwm/pwm-renesas-tpu.c
+++ b/drivers/pwm/pwm-renesas-tpu.c
@@ -21,13 +21,14 @@
 #include <linux/module.h>
 #include <linux/mutex.h>
 #include <linux/of.h>
-#include <linux/platform_data/pwm-renesas-tpu.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/pwm.h>
 #include <linux/slab.h>
 #include <linux/spinlock.h>
 
+#define TPU_CHANNEL_MAX		4
+
 #define TPU_TSTR		0x00	/* Timer start register (shared) */
 
 #define TPU_TCRn		0x00	/* Timer control register */
@@ -87,7 +88,6 @@ struct tpu_pwm_device {
 
 struct tpu_device {
 	struct platform_device *pdev;
-	enum pwm_polarity polarities[TPU_CHANNEL_MAX];
 	struct pwm_chip chip;
 	spinlock_t lock;
 
@@ -229,7 +229,7 @@ static int tpu_pwm_request(struct pwm_chip *chip, struct pwm_device *_pwm)
 
 	pwm->tpu = tpu;
 	pwm->channel = _pwm->hwpwm;
-	pwm->polarity = tpu->polarities[pwm->channel];
+	pwm->polarity = PWM_POLARITY_NORMAL;
 	pwm->prescaler = 0;
 	pwm->period = 0;
 	pwm->duty = 0;
@@ -388,16 +388,6 @@ static const struct pwm_ops tpu_pwm_ops = {
  * Probe and remove
  */
 
-static void tpu_parse_pdata(struct tpu_device *tpu)
-{
-	struct tpu_pwm_platform_data *pdata = tpu->pdev->dev.platform_data;
-	unsigned int i;
-
-	for (i = 0; i < ARRAY_SIZE(tpu->polarities); ++i)
-		tpu->polarities[i] = pdata ? pdata->channels[i].polarity
-				   : PWM_POLARITY_NORMAL;
-}
-
 static int tpu_probe(struct platform_device *pdev)
 {
 	struct tpu_device *tpu;
@@ -405,17 +395,12 @@ static int tpu_probe(struct platform_device *pdev)
 	int ret;
 
 	tpu = devm_kzalloc(&pdev->dev, sizeof(*tpu), GFP_KERNEL);
-	if (tpu == NULL) {
-		dev_err(&pdev->dev, "failed to allocate driver data\n");
+	if (tpu == NULL)
 		return -ENOMEM;
-	}
 
 	spin_lock_init(&tpu->lock);
 	tpu->pdev = pdev;
 
-	/* Initialize device configuration from platform data. */
-	tpu_parse_pdata(tpu);
-
 	/* Map memory, get clock and pin control. */
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	tpu->base = devm_ioremap_resource(&pdev->dev, res);
diff --git a/drivers/pwm/pwm-samsung.c b/drivers/pwm/pwm-samsung.c
index d66529a995a..ba6b650cf8d 100644
--- a/drivers/pwm/pwm-samsung.c
+++ b/drivers/pwm/pwm-samsung.c
@@ -335,9 +335,6 @@ static int pwm_samsung_config(struct pwm_chip *chip, struct pwm_device *pwm,
 	writel(tcnt, our_chip->base + REG_TCNTB(pwm->hwpwm));
 	writel(tcmp, our_chip->base + REG_TCMPB(pwm->hwpwm));
 
-	if (test_bit(PWMF_ENABLED, &pwm->flags))
-		pwm_samsung_enable(chip, pwm);
-
 	chan->period_ns = period_ns;
 	chan->tin_ns = tin_ns;
 	chan->duty_ns = duty_ns;
diff --git a/drivers/pwm/pwm-spear.c b/drivers/pwm/pwm-spear.c
index cb2d4f0f971..6fd93e6a412 100644
--- a/drivers/pwm/pwm-spear.c
+++ b/drivers/pwm/pwm-spear.c
@@ -179,10 +179,8 @@ static int spear_pwm_probe(struct platform_device *pdev)
 	u32 val;
 
 	pc = devm_kzalloc(&pdev->dev, sizeof(*pc), GFP_KERNEL);
-	if (!pc) {
-		dev_err(&pdev->dev, "failed to allocate memory\n");
+	if (!pc)
 		return -ENOMEM;
-	}
 
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	pc->mmio_base = devm_ioremap_resource(&pdev->dev, r);
@@ -222,7 +220,7 @@ static int spear_pwm_probe(struct platform_device *pdev)
 	}
 
 	ret = pwmchip_add(&pc->chip);
-	if (!ret) {
+	if (ret < 0) {
 		clk_unprepare(pc->clk);
 		dev_err(&pdev->dev, "pwmchip_add() failed: %d\n", ret);
 	}
diff --git a/drivers/pwm/pwm-tegra.c b/drivers/pwm/pwm-tegra.c
index 74298c561c4..61d86b9498c 100644
--- a/drivers/pwm/pwm-tegra.c
+++ b/drivers/pwm/pwm-tegra.c
@@ -173,10 +173,8 @@ static int tegra_pwm_probe(struct platform_device *pdev)
 	int ret;
 
 	pwm = devm_kzalloc(&pdev->dev, sizeof(*pwm), GFP_KERNEL);
-	if (!pwm) {
-		dev_err(&pdev->dev, "failed to allocate memory\n");
+	if (!pwm)
 		return -ENOMEM;
-	}
 
 	pwm->dev = &pdev->dev;
 
diff --git a/drivers/pwm/pwm-tiecap.c b/drivers/pwm/pwm-tiecap.c
index 032092c7a6a..74efbe7f20c 100644
--- a/drivers/pwm/pwm-tiecap.c
+++ b/drivers/pwm/pwm-tiecap.c
@@ -209,10 +209,8 @@ static int ecap_pwm_probe(struct platform_device *pdev)
 	u16 status;
 
 	pc = devm_kzalloc(&pdev->dev, sizeof(*pc), GFP_KERNEL);
-	if (!pc) {
-		dev_err(&pdev->dev, "failed to allocate memory\n");
+	if (!pc)
 		return -ENOMEM;
-	}
 
 	clk = devm_clk_get(&pdev->dev, "fck");
 	if (IS_ERR(clk)) {
diff --git a/drivers/pwm/pwm-tiehrpwm.c b/drivers/pwm/pwm-tiehrpwm.c
index aee4471424d..cb75133085a 100644
--- a/drivers/pwm/pwm-tiehrpwm.c
+++ b/drivers/pwm/pwm-tiehrpwm.c
@@ -138,12 +138,12 @@ static inline struct ehrpwm_pwm_chip *to_ehrpwm_pwm_chip(struct pwm_chip *chip)
 	return container_of(chip, struct ehrpwm_pwm_chip, chip);
 }
 
-static u16 ehrpwm_read(void __iomem *base, int offset)
+static inline u16 ehrpwm_read(void __iomem *base, int offset)
 {
 	return readw(base + offset);
 }
 
-static void ehrpwm_write(void __iomem *base, int offset, unsigned int val)
+static inline void ehrpwm_write(void __iomem *base, int offset, unsigned int val)
 {
 	writew(val & 0xFFFF, base + offset);
 }
@@ -440,10 +440,8 @@ static int ehrpwm_pwm_probe(struct platform_device *pdev)
 	u16 status;
 
 	pc = devm_kzalloc(&pdev->dev, sizeof(*pc), GFP_KERNEL);
-	if (!pc) {
-		dev_err(&pdev->dev, "failed to allocate memory\n");
+	if (!pc)
 		return -ENOMEM;
-	}
 
 	clk = devm_clk_get(&pdev->dev, "fck");
 	if (IS_ERR(clk)) {
@@ -531,6 +529,7 @@ static int ehrpwm_pwm_remove(struct platform_device *pdev)
 	return pwmchip_remove(&pc->chip);
 }
 
+#ifdef CONFIG_PM_SLEEP
 static void ehrpwm_pwm_save_context(struct ehrpwm_pwm_chip *pc)
 {
 	pm_runtime_get_sync(pc->chip.dev);
@@ -557,7 +556,6 @@ static void ehrpwm_pwm_restore_context(struct ehrpwm_pwm_chip *pc)
 	ehrpwm_write(pc->mmio_base, TBCTL, pc->ctx.tbctl);
 }
 
-#ifdef CONFIG_PM_SLEEP
 static int ehrpwm_pwm_suspend(struct device *dev)
 {
 	struct ehrpwm_pwm_chip *pc = dev_get_drvdata(dev);
diff --git a/drivers/pwm/pwm-twl.c b/drivers/pwm/pwm-twl.c
index b99a50e626a..04f76725d59 100644
--- a/drivers/pwm/pwm-twl.c
+++ b/drivers/pwm/pwm-twl.c
@@ -265,14 +265,6 @@ static void twl6030_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
 
 	ret = twl_i2c_write_u8(TWL6030_MODULE_ID1, val, TWL6030_TOGGLE3_REG);
 	if (ret < 0) {
-		dev_err(chip->dev, "%s: Failed to read TOGGLE3\n", pwm->label);
-		goto out;
-	}
-
-	val |= TWL6030_PWM_TOGGLE(pwm->hwpwm, TWL6030_PWMXS | TWL6030_PWMXEN);
-
-	ret = twl_i2c_write_u8(TWL6030_MODULE_ID1, val, TWL6030_TOGGLE3_REG);
-	if (ret < 0) {
 		dev_err(chip->dev, "%s: Failed to disable PWM\n", pwm->label);
 		goto out;
 	}
diff --git a/drivers/pwm/pwm-vt8500.c b/drivers/pwm/pwm-vt8500.c
index 323125abf3f..652e6b5b859 100644
--- a/drivers/pwm/pwm-vt8500.c
+++ b/drivers/pwm/pwm-vt8500.c
@@ -211,10 +211,8 @@ static int vt8500_pwm_probe(struct platform_device *pdev)
 	}
 
 	chip = devm_kzalloc(&pdev->dev, sizeof(*chip), GFP_KERNEL);
-	if (chip == NULL) {
-		dev_err(&pdev->dev, "failed to allocate memory\n");
+	if (chip == NULL)
 		return -ENOMEM;
-	}
 
 	chip->chip.dev = &pdev->dev;
 	chip->chip.ops = &vt8500_pwm_ops;
diff --git a/drivers/rapidio/devices/tsi721.c b/drivers/rapidio/devices/tsi721.c
index 1753dc693c1..2ca1a0b3ad5 100644
--- a/drivers/rapidio/devices/tsi721.c
+++ b/drivers/rapidio/devices/tsi721.c
@@ -768,15 +768,10 @@ static int tsi721_enable_msix(struct tsi721_device *priv)
 	}
 #endif /* CONFIG_RAPIDIO_DMA_ENGINE */
 
-	err = pci_enable_msix(priv->pdev, entries, ARRAY_SIZE(entries));
+	err = pci_enable_msix_exact(priv->pdev, entries, ARRAY_SIZE(entries));
 	if (err) {
-		if (err > 0)
-			dev_info(&priv->pdev->dev,
-				 "Only %d MSI-X vectors available, "
-				 "not using MSI-X\n", err);
-		else
-			dev_err(&priv->pdev->dev,
-				"Failed to enable MSI-X (err=%d)\n", err);
+		dev_err(&priv->pdev->dev,
+			"Failed to enable MSI-X (err=%d)\n", err);
 		return err;
 	}
 
diff --git a/drivers/regulator/Kconfig b/drivers/regulator/Kconfig
index 903eb37f047..789eb46090e 100644
--- a/drivers/regulator/Kconfig
+++ b/drivers/regulator/Kconfig
@@ -139,6 +139,13 @@ config REGULATOR_AS3722
 	  AS3722 PMIC. This will enable support for all the software
 	  controllable DCDC/LDO regulators.
 
+config REGULATOR_AXP20X
+	tristate "X-POWERS AXP20X PMIC Regulators"
+	depends on MFD_AXP20X
+	help
+	  This driver provides support for the voltage regulators on the
+	  AXP20X PMIC.
+
 config REGULATOR_BCM590XX
 	tristate "Broadcom BCM590xx PMU Regulators"
 	depends on MFD_BCM590XX
@@ -265,12 +272,21 @@ config REGULATOR_LP8788
 	help
 	  This driver supports LP8788 voltage regulator chip.
 
+config REGULATOR_LTC3589
+	tristate "LTC3589 8-output voltage regulator"
+	depends on I2C
+	select REGMAP_I2C
+	help
+	  This enables support for the LTC3589, LTC3589-1, and LTC3589-2
+	  8-output regulators controlled via I2C.
+
 config REGULATOR_MAX14577
-	tristate "Maxim 14577 regulator"
+	tristate "Maxim 14577/77836 regulator"
 	depends on MFD_MAX14577
 	help
-	  This driver controls a Maxim 14577 regulator via I2C bus.
-	  The regulators include safeout LDO and current regulator 'CHARGER'.
+	  This driver controls a Maxim MAX14577/77836 regulator via I2C bus.
+	  The MAX14577 regulators include safeout LDO and charger current
+	  regulator. The MAX77836 has two additional LDOs.
 
 config REGULATOR_MAX1586
 	tristate "Maxim 1586/1587 voltage regulator"
diff --git a/drivers/regulator/Makefile b/drivers/regulator/Makefile
index 12ef277a48b..d461110f446 100644
--- a/drivers/regulator/Makefile
+++ b/drivers/regulator/Makefile
@@ -20,6 +20,7 @@ obj-$(CONFIG_REGULATOR_ANATOP) += anatop-regulator.o
 obj-$(CONFIG_REGULATOR_ARIZONA) += arizona-micsupp.o arizona-ldo1.o
 obj-$(CONFIG_REGULATOR_AS3711) += as3711-regulator.o
 obj-$(CONFIG_REGULATOR_AS3722) += as3722-regulator.o
+obj-$(CONFIG_REGULATOR_AXP20X) += axp20x-regulator.o
 obj-$(CONFIG_REGULATOR_BCM590XX) += bcm590xx-regulator.o
 obj-$(CONFIG_REGULATOR_DA903X)	+= da903x.o
 obj-$(CONFIG_REGULATOR_DA9052)	+= da9052-regulator.o
@@ -37,6 +38,7 @@ obj-$(CONFIG_REGULATOR_LP872X) += lp872x.o
 obj-$(CONFIG_REGULATOR_LP8788) += lp8788-buck.o
 obj-$(CONFIG_REGULATOR_LP8788) += lp8788-ldo.o
 obj-$(CONFIG_REGULATOR_LP8755) += lp8755.o
+obj-$(CONFIG_REGULATOR_LTC3589) += ltc3589.o
 obj-$(CONFIG_REGULATOR_MAX14577) += max14577.o
 obj-$(CONFIG_REGULATOR_MAX1586) += max1586.o
 obj-$(CONFIG_REGULATOR_MAX8649)	+= max8649.o
diff --git a/drivers/regulator/anatop-regulator.c b/drivers/regulator/anatop-regulator.c
index 7c397bb81e0..4f730af70e7 100644
--- a/drivers/regulator/anatop-regulator.c
+++ b/drivers/regulator/anatop-regulator.c
@@ -300,7 +300,7 @@ static int anatop_regulator_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id of_anatop_regulator_match_tbl[] = {
+static const struct of_device_id of_anatop_regulator_match_tbl[] = {
 	{ .compatible = "fsl,anatop-regulator", },
 	{ /* end */ }
 };
diff --git a/drivers/regulator/arizona-ldo1.c b/drivers/regulator/arizona-ldo1.c
index b1033d30b50..04f262a836b 100644
--- a/drivers/regulator/arizona-ldo1.c
+++ b/drivers/regulator/arizona-ldo1.c
@@ -16,9 +16,11 @@
 #include <linux/init.h>
 #include <linux/bitops.h>
 #include <linux/err.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
+#include <linux/regulator/of_regulator.h>
 #include <linux/gpio.h>
 #include <linux/slab.h>
 
@@ -178,6 +180,42 @@ static const struct regulator_init_data arizona_ldo1_default = {
 	.num_consumer_supplies = 1,
 };
 
+static int arizona_ldo1_of_get_pdata(struct arizona *arizona,
+				     struct regulator_config *config)
+{
+	struct arizona_pdata *pdata = &arizona->pdata;
+	struct arizona_ldo1 *ldo1 = config->driver_data;
+	struct device_node *init_node, *dcvdd_node;
+	struct regulator_init_data *init_data;
+
+	pdata->ldoena = arizona_of_get_named_gpio(arizona, "wlf,ldoena", true);
+
+	init_node = of_get_child_by_name(arizona->dev->of_node, "ldo1");
+	dcvdd_node = of_parse_phandle(arizona->dev->of_node, "DCVDD-supply", 0);
+
+	if (init_node) {
+		config->of_node = init_node;
+
+		init_data = of_get_regulator_init_data(arizona->dev, init_node);
+
+		if (init_data) {
+			init_data->consumer_supplies = &ldo1->supply;
+			init_data->num_consumer_supplies = 1;
+
+			if (dcvdd_node && dcvdd_node != init_node)
+				arizona->external_dcvdd = true;
+
+			pdata->ldo1 = init_data;
+		}
+	} else if (dcvdd_node) {
+		arizona->external_dcvdd = true;
+	}
+
+	of_node_put(dcvdd_node);
+
+	return 0;
+}
+
 static int arizona_ldo1_probe(struct platform_device *pdev)
 {
 	struct arizona *arizona = dev_get_drvdata(pdev->dev.parent);
@@ -186,6 +224,8 @@ static int arizona_ldo1_probe(struct platform_device *pdev)
 	struct arizona_ldo1 *ldo1;
 	int ret;
 
+	arizona->external_dcvdd = false;
+
 	ldo1 = devm_kzalloc(&pdev->dev, sizeof(*ldo1), GFP_KERNEL);
 	if (!ldo1)
 		return -ENOMEM;
@@ -216,6 +256,15 @@ static int arizona_ldo1_probe(struct platform_device *pdev)
 	config.dev = arizona->dev;
 	config.driver_data = ldo1;
 	config.regmap = arizona->regmap;
+
+	if (IS_ENABLED(CONFIG_OF)) {
+		if (!dev_get_platdata(arizona->dev)) {
+			ret = arizona_ldo1_of_get_pdata(arizona, &config);
+			if (ret < 0)
+				return ret;
+		}
+	}
+
 	config.ena_gpio = arizona->pdata.ldoena;
 
 	if (arizona->pdata.ldo1)
@@ -223,6 +272,13 @@ static int arizona_ldo1_probe(struct platform_device *pdev)
 	else
 		config.init_data = &ldo1->init_data;
 
+	/*
+	 * LDO1 can only be used to supply DCVDD so if it has no
+	 * consumers then DCVDD is supplied externally.
+	 */
+	if (config.init_data->num_consumer_supplies == 0)
+		arizona->external_dcvdd = true;
+
 	ldo1->regulator = devm_regulator_register(&pdev->dev, desc, &config);
 	if (IS_ERR(ldo1->regulator)) {
 		ret = PTR_ERR(ldo1->regulator);
@@ -231,6 +287,8 @@ static int arizona_ldo1_probe(struct platform_device *pdev)
 		return ret;
 	}
 
+	of_node_put(config.of_node);
+
 	platform_set_drvdata(pdev, ldo1);
 
 	return 0;
diff --git a/drivers/regulator/arizona-micsupp.c b/drivers/regulator/arizona-micsupp.c
index 6fdd9bf6927..ce9aca5f8ee 100644
--- a/drivers/regulator/arizona-micsupp.c
+++ b/drivers/regulator/arizona-micsupp.c
@@ -16,9 +16,11 @@
 #include <linux/init.h>
 #include <linux/bitops.h>
 #include <linux/err.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
+#include <linux/regulator/of_regulator.h>
 #include <linux/gpio.h>
 #include <linux/slab.h>
 #include <linux/workqueue.h>
@@ -195,6 +197,32 @@ static const struct regulator_init_data arizona_micsupp_ext_default = {
 	.num_consumer_supplies = 1,
 };
 
+static int arizona_micsupp_of_get_pdata(struct arizona *arizona,
+					struct regulator_config *config)
+{
+	struct arizona_pdata *pdata = &arizona->pdata;
+	struct arizona_micsupp *micsupp = config->driver_data;
+	struct device_node *np;
+	struct regulator_init_data *init_data;
+
+	np = of_get_child_by_name(arizona->dev->of_node, "micvdd");
+
+	if (np) {
+		config->of_node = np;
+
+		init_data = of_get_regulator_init_data(arizona->dev, np);
+
+		if (init_data) {
+			init_data->consumer_supplies = &micsupp->supply;
+			init_data->num_consumer_supplies = 1;
+
+			pdata->micvdd = init_data;
+		}
+	}
+
+	return 0;
+}
+
 static int arizona_micsupp_probe(struct platform_device *pdev)
 {
 	struct arizona *arizona = dev_get_drvdata(pdev->dev.parent);
@@ -234,6 +262,14 @@ static int arizona_micsupp_probe(struct platform_device *pdev)
 	config.driver_data = micsupp;
 	config.regmap = arizona->regmap;
 
+	if (IS_ENABLED(CONFIG_OF)) {
+		if (!dev_get_platdata(arizona->dev)) {
+			ret = arizona_micsupp_of_get_pdata(arizona, &config);
+			if (ret < 0)
+				return ret;
+		}
+	}
+
 	if (arizona->pdata.micvdd)
 		config.init_data = arizona->pdata.micvdd;
 	else
@@ -253,6 +289,8 @@ static int arizona_micsupp_probe(struct platform_device *pdev)
 		return ret;
 	}
 
+	of_node_put(config.of_node);
+
 	platform_set_drvdata(pdev, micsupp);
 
 	return 0;
diff --git a/drivers/regulator/axp20x-regulator.c b/drivers/regulator/axp20x-regulator.c
new file mode 100644
index 00000000000..004aadb7bcc
--- /dev/null
+++ b/drivers/regulator/axp20x-regulator.c
@@ -0,0 +1,286 @@
+/*
+ * AXP20x regulators driver.
+ *
+ * Copyright (C) 2013 Carlo Caione <carlo@caione.org>
+ *
+ * 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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/init.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/mfd/axp20x.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/of_regulator.h>
+
+#define AXP20X_IO_ENABLED		0x03
+#define AXP20X_IO_DISABLED		0x07
+
+#define AXP20X_WORKMODE_DCDC2_MASK	BIT(2)
+#define AXP20X_WORKMODE_DCDC3_MASK	BIT(1)
+
+#define AXP20X_FREQ_DCDC_MASK		0x0f
+
+#define AXP20X_DESC_IO(_id, _supply, _min, _max, _step, _vreg, _vmask, _ereg,   \
+		       _emask, _enable_val, _disable_val)			\
+	[AXP20X_##_id] = {							\
+		.name		= #_id,						\
+		.supply_name	= (_supply),					\
+		.type		= REGULATOR_VOLTAGE,				\
+		.id		= AXP20X_##_id,					\
+		.n_voltages	= (((_max) - (_min)) / (_step) + 1),		\
+		.owner		= THIS_MODULE,					\
+		.min_uV		= (_min) * 1000,				\
+		.uV_step	= (_step) * 1000,				\
+		.vsel_reg	= (_vreg),					\
+		.vsel_mask	= (_vmask),					\
+		.enable_reg	= (_ereg),					\
+		.enable_mask	= (_emask),					\
+		.enable_val	= (_enable_val),				\
+		.disable_val	= (_disable_val),				\
+		.ops		= &axp20x_ops,					\
+	}
+
+#define AXP20X_DESC(_id, _supply, _min, _max, _step, _vreg, _vmask, _ereg,	\
+		    _emask) 							\
+	[AXP20X_##_id] = {							\
+		.name		= #_id,						\
+		.supply_name	= (_supply),					\
+		.type		= REGULATOR_VOLTAGE,				\
+		.id		= AXP20X_##_id,					\
+		.n_voltages	= (((_max) - (_min)) / (_step) + 1),		\
+		.owner		= THIS_MODULE,					\
+		.min_uV		= (_min) * 1000,				\
+		.uV_step	= (_step) * 1000,				\
+		.vsel_reg	= (_vreg),					\
+		.vsel_mask	= (_vmask),					\
+		.enable_reg	= (_ereg),					\
+		.enable_mask	= (_emask),					\
+		.ops		= &axp20x_ops,					\
+	}
+
+#define AXP20X_DESC_FIXED(_id, _supply, _volt)					\
+	[AXP20X_##_id] = {							\
+		.name		= #_id,						\
+		.supply_name	= (_supply),					\
+		.type		= REGULATOR_VOLTAGE,				\
+		.id		= AXP20X_##_id,					\
+		.n_voltages	= 1,						\
+		.owner		= THIS_MODULE,					\
+		.min_uV		= (_volt) * 1000,				\
+		.ops		= &axp20x_ops_fixed				\
+	}
+
+#define AXP20X_DESC_TABLE(_id, _supply, _table, _vreg, _vmask, _ereg, _emask)	\
+	[AXP20X_##_id] = {							\
+		.name		= #_id,						\
+		.supply_name	= (_supply),					\
+		.type		= REGULATOR_VOLTAGE,				\
+		.id		= AXP20X_##_id,					\
+		.n_voltages	= ARRAY_SIZE(_table),				\
+		.owner		= THIS_MODULE,					\
+		.vsel_reg	= (_vreg),					\
+		.vsel_mask	= (_vmask),					\
+		.enable_reg	= (_ereg),					\
+		.enable_mask	= (_emask),					\
+		.volt_table	= (_table),					\
+		.ops		= &axp20x_ops_table,				\
+	}
+
+static const int axp20x_ldo4_data[] = { 1250000, 1300000, 1400000, 1500000, 1600000,
+					1700000, 1800000, 1900000, 2000000, 2500000,
+					2700000, 2800000, 3000000, 3100000, 3200000,
+					3300000 };
+
+static struct regulator_ops axp20x_ops_fixed = {
+	.list_voltage		= regulator_list_voltage_linear,
+};
+
+static struct regulator_ops axp20x_ops_table = {
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.list_voltage		= regulator_list_voltage_table,
+	.map_voltage		= regulator_map_voltage_ascend,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.is_enabled		= regulator_is_enabled_regmap,
+};
+
+static struct regulator_ops axp20x_ops = {
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.list_voltage		= regulator_list_voltage_linear,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.is_enabled		= regulator_is_enabled_regmap,
+};
+
+static const struct regulator_desc axp20x_regulators[] = {
+	AXP20X_DESC(DCDC2, "vin2", 700, 2275, 25, AXP20X_DCDC2_V_OUT, 0x3f,
+		    AXP20X_PWR_OUT_CTRL, 0x10),
+	AXP20X_DESC(DCDC3, "vin3", 700, 3500, 25, AXP20X_DCDC3_V_OUT, 0x7f,
+		    AXP20X_PWR_OUT_CTRL, 0x02),
+	AXP20X_DESC_FIXED(LDO1, "acin", 1300),
+	AXP20X_DESC(LDO2, "ldo24in", 1800, 3300, 100, AXP20X_LDO24_V_OUT, 0xf0,
+		    AXP20X_PWR_OUT_CTRL, 0x04),
+	AXP20X_DESC(LDO3, "ldo3in", 700, 3500, 25, AXP20X_LDO3_V_OUT, 0x7f,
+		    AXP20X_PWR_OUT_CTRL, 0x40),
+	AXP20X_DESC_TABLE(LDO4, "ldo24in", axp20x_ldo4_data, AXP20X_LDO24_V_OUT, 0x0f,
+			  AXP20X_PWR_OUT_CTRL, 0x08),
+	AXP20X_DESC_IO(LDO5, "ldo5in", 1800, 3300, 100, AXP20X_LDO5_V_OUT, 0xf0,
+		       AXP20X_GPIO0_CTRL, 0x07, AXP20X_IO_ENABLED,
+		       AXP20X_IO_DISABLED),
+};
+
+#define AXP_MATCH(_name, _id) \
+	[AXP20X_##_id] = { \
+		.name		= #_name, \
+		.driver_data	= (void *) &axp20x_regulators[AXP20X_##_id], \
+	}
+
+static struct of_regulator_match axp20x_matches[] = {
+	AXP_MATCH(dcdc2, DCDC2),
+	AXP_MATCH(dcdc3, DCDC3),
+	AXP_MATCH(ldo1, LDO1),
+	AXP_MATCH(ldo2, LDO2),
+	AXP_MATCH(ldo3, LDO3),
+	AXP_MATCH(ldo4, LDO4),
+	AXP_MATCH(ldo5, LDO5),
+};
+
+static int axp20x_set_dcdc_freq(struct platform_device *pdev, u32 dcdcfreq)
+{
+	struct axp20x_dev *axp20x = dev_get_drvdata(pdev->dev.parent);
+
+	if (dcdcfreq < 750) {
+		dcdcfreq = 750;
+		dev_warn(&pdev->dev, "DCDC frequency too low. Set to 750kHz\n");
+	}
+
+	if (dcdcfreq > 1875) {
+		dcdcfreq = 1875;
+		dev_warn(&pdev->dev, "DCDC frequency too high. Set to 1875kHz\n");
+	}
+
+	dcdcfreq = (dcdcfreq - 750) / 75;
+
+	return regmap_update_bits(axp20x->regmap, AXP20X_DCDC_FREQ,
+				  AXP20X_FREQ_DCDC_MASK, dcdcfreq);
+}
+
+static int axp20x_regulator_parse_dt(struct platform_device *pdev)
+{
+	struct device_node *np, *regulators;
+	int ret;
+	u32 dcdcfreq;
+
+	np = of_node_get(pdev->dev.parent->of_node);
+	if (!np)
+		return 0;
+
+	regulators = of_get_child_by_name(np, "regulators");
+	if (!regulators) {
+		dev_warn(&pdev->dev, "regulators node not found\n");
+	} else {
+		ret = of_regulator_match(&pdev->dev, regulators, axp20x_matches,
+					 ARRAY_SIZE(axp20x_matches));
+		if (ret < 0) {
+			dev_err(&pdev->dev, "Error parsing regulator init data: %d\n", ret);
+			return ret;
+		}
+
+		dcdcfreq = 1500;
+		of_property_read_u32(regulators, "x-powers,dcdc-freq", &dcdcfreq);
+		ret = axp20x_set_dcdc_freq(pdev, dcdcfreq);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "Error setting dcdc frequency: %d\n", ret);
+			return ret;
+		}
+
+		of_node_put(regulators);
+	}
+
+	return 0;
+}
+
+static int axp20x_set_dcdc_workmode(struct regulator_dev *rdev, int id, u32 workmode)
+{
+	unsigned int mask = AXP20X_WORKMODE_DCDC2_MASK;
+
+	if ((id != AXP20X_DCDC2) && (id != AXP20X_DCDC3))
+		return -EINVAL;
+
+	if (id == AXP20X_DCDC3)
+		mask = AXP20X_WORKMODE_DCDC3_MASK;
+
+	workmode <<= ffs(mask) - 1;
+
+	return regmap_update_bits(rdev->regmap, AXP20X_DCDC_MODE, mask, workmode);
+}
+
+static int axp20x_regulator_probe(struct platform_device *pdev)
+{
+	struct regulator_dev *rdev;
+	struct axp20x_dev *axp20x = dev_get_drvdata(pdev->dev.parent);
+	struct regulator_config config = { };
+	struct regulator_init_data *init_data;
+	int ret, i;
+	u32 workmode;
+
+	ret = axp20x_regulator_parse_dt(pdev);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < AXP20X_REG_ID_MAX; i++) {
+		init_data = axp20x_matches[i].init_data;
+
+		config.dev = &pdev->dev;
+		config.init_data = init_data;
+		config.regmap = axp20x->regmap;
+		config.of_node = axp20x_matches[i].of_node;
+
+		rdev = devm_regulator_register(&pdev->dev, &axp20x_regulators[i],
+					       &config);
+		if (IS_ERR(rdev)) {
+			dev_err(&pdev->dev, "Failed to register %s\n",
+				axp20x_regulators[i].name);
+
+			return PTR_ERR(rdev);
+		}
+
+		ret = of_property_read_u32(axp20x_matches[i].of_node, "x-powers,dcdc-workmode",
+					   &workmode);
+		if (!ret) {
+			if (axp20x_set_dcdc_workmode(rdev, i, workmode))
+				dev_err(&pdev->dev, "Failed to set workmode on %s\n",
+					axp20x_regulators[i].name);
+		}
+	}
+
+	return 0;
+}
+
+static struct platform_driver axp20x_regulator_driver = {
+	.probe	= axp20x_regulator_probe,
+	.driver	= {
+		.name		= "axp20x-regulator",
+		.owner		= THIS_MODULE,
+	},
+};
+
+module_platform_driver(axp20x_regulator_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Carlo Caione <carlo@caione.org>");
+MODULE_DESCRIPTION("Regulator Driver for AXP20X PMIC");
diff --git a/drivers/regulator/bcm590xx-regulator.c b/drivers/regulator/bcm590xx-regulator.c
index c3750c5b382..57544e254a7 100644
--- a/drivers/regulator/bcm590xx-regulator.c
+++ b/drivers/regulator/bcm590xx-regulator.c
@@ -22,7 +22,7 @@
 #include <linux/regulator/of_regulator.h>
 #include <linux/slab.h>
 
-/* Register defs */
+/* I2C slave 0 registers */
 #define BCM590XX_RFLDOPMCTRL1	0x60
 #define BCM590XX_IOSR1PMCTRL1	0x7a
 #define BCM590XX_IOSR2PMCTRL1	0x7c
@@ -31,13 +31,34 @@
 #define BCM590XX_SDSR2PMCTRL1	0x86
 #define BCM590XX_MSRPMCTRL1	0x8a
 #define BCM590XX_VSRPMCTRL1	0x8e
-#define BCM590XX_REG_ENABLE	BIT(7)
-
 #define BCM590XX_RFLDOCTRL	0x96
 #define BCM590XX_CSRVOUT1	0xc0
+
+/* I2C slave 1 registers */
+#define BCM590XX_GPLDO5PMCTRL1	0x16
+#define BCM590XX_GPLDO6PMCTRL1	0x18
+#define BCM590XX_GPLDO1CTRL	0x1a
+#define BCM590XX_GPLDO2CTRL	0x1b
+#define BCM590XX_GPLDO3CTRL	0x1c
+#define BCM590XX_GPLDO4CTRL	0x1d
+#define BCM590XX_GPLDO5CTRL	0x1e
+#define BCM590XX_GPLDO6CTRL	0x1f
+#define BCM590XX_OTG_CTRL	0x40
+#define BCM590XX_GPLDO1PMCTRL1	0x57
+#define BCM590XX_GPLDO2PMCTRL1	0x59
+#define BCM590XX_GPLDO3PMCTRL1	0x5b
+#define BCM590XX_GPLDO4PMCTRL1	0x5d
+
+#define BCM590XX_REG_ENABLE	BIT(7)
+#define BCM590XX_VBUS_ENABLE	BIT(2)
 #define BCM590XX_LDO_VSEL_MASK	GENMASK(5, 3)
 #define BCM590XX_SR_VSEL_MASK	GENMASK(5, 0)
 
+/*
+ * RFLDO to VSR regulators are
+ * accessed via I2C slave 0
+ */
+
 /* LDO regulator IDs */
 #define BCM590XX_REG_RFLDO	0
 #define BCM590XX_REG_CAMLDO1	1
@@ -62,9 +83,25 @@
 #define BCM590XX_REG_SDSR2	18
 #define BCM590XX_REG_VSR	19
 
-#define BCM590XX_NUM_REGS	20
+/*
+ * GPLDO1 to VBUS regulators are
+ * accessed via I2C slave 1
+ */
+
+#define BCM590XX_REG_GPLDO1	20
+#define BCM590XX_REG_GPLDO2	21
+#define BCM590XX_REG_GPLDO3	22
+#define BCM590XX_REG_GPLDO4	23
+#define BCM590XX_REG_GPLDO5	24
+#define BCM590XX_REG_GPLDO6	25
+#define BCM590XX_REG_VBUS	26
+
+#define BCM590XX_NUM_REGS	27
 
 #define BCM590XX_REG_IS_LDO(n)	(n < BCM590XX_REG_CSR)
+#define BCM590XX_REG_IS_GPLDO(n) \
+	((n > BCM590XX_REG_VSR) && (n < BCM590XX_REG_VBUS))
+#define BCM590XX_REG_IS_VBUS(n)	(n == BCM590XX_REG_VBUS)
 
 struct bcm590xx_board {
 	struct regulator_init_data *bcm590xx_pmu_init_data[BCM590XX_NUM_REGS];
@@ -149,6 +186,12 @@ static struct bcm590xx_info bcm590xx_regs[] = {
 	BCM590XX_REG_RANGES(sdsr1, dcdc_sdsr1_ranges),
 	BCM590XX_REG_RANGES(sdsr2, dcdc_iosr1_ranges),
 	BCM590XX_REG_RANGES(vsr, dcdc_iosr1_ranges),
+	BCM590XX_REG_TABLE(gpldo1, ldo_a_table),
+	BCM590XX_REG_TABLE(gpldo2, ldo_a_table),
+	BCM590XX_REG_TABLE(gpldo3, ldo_a_table),
+	BCM590XX_REG_TABLE(gpldo4, ldo_a_table),
+	BCM590XX_REG_TABLE(gpldo5, ldo_a_table),
+	BCM590XX_REG_TABLE(gpldo6, ldo_a_table),
 };
 
 struct bcm590xx_reg {
@@ -161,6 +204,8 @@ static int bcm590xx_get_vsel_register(int id)
 {
 	if (BCM590XX_REG_IS_LDO(id))
 		return BCM590XX_RFLDOCTRL + id;
+	else if (BCM590XX_REG_IS_GPLDO(id))
+		return BCM590XX_GPLDO1CTRL + id;
 	else
 		return BCM590XX_CSRVOUT1 + (id - BCM590XX_REG_CSR) * 3;
 }
@@ -171,6 +216,8 @@ static int bcm590xx_get_enable_register(int id)
 
 	if (BCM590XX_REG_IS_LDO(id))
 		reg = BCM590XX_RFLDOPMCTRL1 + id * 2;
+	else if (BCM590XX_REG_IS_GPLDO(id))
+		reg = BCM590XX_GPLDO1PMCTRL1 + id * 2;
 	else
 		switch (id) {
 		case BCM590XX_REG_CSR:
@@ -191,8 +238,11 @@ static int bcm590xx_get_enable_register(int id)
 		case BCM590XX_REG_SDSR2:
 			reg = BCM590XX_SDSR2PMCTRL1;
 			break;
+		case BCM590XX_REG_VBUS:
+			reg = BCM590XX_OTG_CTRL;
 		};
 
+
 	return reg;
 }
 
@@ -216,6 +266,12 @@ static struct regulator_ops bcm590xx_ops_dcdc = {
 	.map_voltage		= regulator_map_voltage_linear_range,
 };
 
+static struct regulator_ops bcm590xx_ops_vbus = {
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+};
+
 #define BCM590XX_MATCH(_name, _id) \
 	{ \
 		.name = #_name, \
@@ -243,6 +299,13 @@ static struct of_regulator_match bcm590xx_matches[] = {
 	BCM590XX_MATCH(sdsr1, SDSR1),
 	BCM590XX_MATCH(sdsr2, SDSR2),
 	BCM590XX_MATCH(vsr, VSR),
+	BCM590XX_MATCH(gpldo1, GPLDO1),
+	BCM590XX_MATCH(gpldo2, GPLDO2),
+	BCM590XX_MATCH(gpldo3, GPLDO3),
+	BCM590XX_MATCH(gpldo4, GPLDO4),
+	BCM590XX_MATCH(gpldo5, GPLDO5),
+	BCM590XX_MATCH(gpldo6, GPLDO6),
+	BCM590XX_MATCH(vbus, VBUS),
 };
 
 static struct bcm590xx_board *bcm590xx_parse_dt_reg_data(
@@ -353,17 +416,23 @@ static int bcm590xx_probe(struct platform_device *pdev)
 		pmu->desc[i].linear_ranges = info->linear_ranges;
 		pmu->desc[i].n_linear_ranges = info->n_linear_ranges;
 
-		if (BCM590XX_REG_IS_LDO(i)) {
+		if ((BCM590XX_REG_IS_LDO(i)) || (BCM590XX_REG_IS_GPLDO(i))) {
 			pmu->desc[i].ops = &bcm590xx_ops_ldo;
 			pmu->desc[i].vsel_mask = BCM590XX_LDO_VSEL_MASK;
-		} else {
+		} else if (BCM590XX_REG_IS_VBUS(i))
+			pmu->desc[i].ops = &bcm590xx_ops_vbus;
+		else {
 			pmu->desc[i].ops = &bcm590xx_ops_dcdc;
 			pmu->desc[i].vsel_mask = BCM590XX_SR_VSEL_MASK;
 		}
 
-		pmu->desc[i].vsel_reg = bcm590xx_get_vsel_register(i);
-		pmu->desc[i].enable_is_inverted = true;
-		pmu->desc[i].enable_mask = BCM590XX_REG_ENABLE;
+		if (BCM590XX_REG_IS_VBUS(i))
+			pmu->desc[i].enable_mask = BCM590XX_VBUS_ENABLE;
+		else {
+			pmu->desc[i].vsel_reg = bcm590xx_get_vsel_register(i);
+			pmu->desc[i].enable_is_inverted = true;
+			pmu->desc[i].enable_mask = BCM590XX_REG_ENABLE;
+		}
 		pmu->desc[i].enable_reg = bcm590xx_get_enable_register(i);
 		pmu->desc[i].type = REGULATOR_VOLTAGE;
 		pmu->desc[i].owner = THIS_MODULE;
@@ -371,7 +440,10 @@ static int bcm590xx_probe(struct platform_device *pdev)
 		config.dev = bcm590xx->dev;
 		config.init_data = reg_data;
 		config.driver_data = pmu;
-		config.regmap = bcm590xx->regmap;
+		if (BCM590XX_REG_IS_GPLDO(i) || BCM590XX_REG_IS_VBUS(i))
+			config.regmap = bcm590xx->regmap_sec;
+		else
+			config.regmap = bcm590xx->regmap_pri;
 
 		if (bcm590xx_reg_matches)
 			config.of_node = bcm590xx_reg_matches[i].of_node;
diff --git a/drivers/regulator/core.c b/drivers/regulator/core.c
index 9a09f3cdbab..4c1f999041d 100644
--- a/drivers/regulator/core.c
+++ b/drivers/regulator/core.c
@@ -844,13 +844,22 @@ static int machine_constraints_voltage(struct regulator_dev *rdev,
 	/* do we need to apply the constraint voltage */
 	if (rdev->constraints->apply_uV &&
 	    rdev->constraints->min_uV == rdev->constraints->max_uV) {
-		ret = _regulator_do_set_voltage(rdev,
-						rdev->constraints->min_uV,
-						rdev->constraints->max_uV);
-		if (ret < 0) {
-			rdev_err(rdev, "failed to apply %duV constraint\n",
-				 rdev->constraints->min_uV);
-			return ret;
+		int current_uV = _regulator_get_voltage(rdev);
+		if (current_uV < 0) {
+			rdev_err(rdev, "failed to get the current voltage\n");
+			return current_uV;
+		}
+		if (current_uV < rdev->constraints->min_uV ||
+		    current_uV > rdev->constraints->max_uV) {
+			ret = _regulator_do_set_voltage(
+				rdev, rdev->constraints->min_uV,
+				rdev->constraints->max_uV);
+			if (ret < 0) {
+				rdev_err(rdev,
+					"failed to apply %duV constraint\n",
+					rdev->constraints->min_uV);
+				return ret;
+			}
 		}
 	}
 
@@ -1430,9 +1439,9 @@ EXPORT_SYMBOL_GPL(regulator_get);
  *
  * Returns a struct regulator corresponding to the regulator producer,
  * or IS_ERR() condition containing errno.  Other consumers will be
- * unable to obtain this reference is held and the use count for the
- * regulator will be initialised to reflect the current state of the
- * regulator.
+ * unable to obtain this regulator while this reference is held and the
+ * use count for the regulator will be initialised to reflect the current
+ * state of the regulator.
  *
  * This is intended for use by consumers which cannot tolerate shared
  * use of the regulator such as those which need to force the
@@ -1456,10 +1465,7 @@ EXPORT_SYMBOL_GPL(regulator_get_exclusive);
  * @id: Supply name or regulator ID.
  *
  * Returns a struct regulator corresponding to the regulator producer,
- * or IS_ERR() condition containing errno.  Other consumers will be
- * unable to obtain this reference is held and the use count for the
- * regulator will be initialised to reflect the current state of the
- * regulator.
+ * or IS_ERR() condition containing errno.
  *
  * This is intended for use by consumers for devices which can have
  * some supplies unconnected in normal use, such as some MMC devices.
@@ -1597,9 +1603,10 @@ EXPORT_SYMBOL_GPL(regulator_unregister_supply_alias);
  * registered any aliases that were registered will be removed
  * before returning to the caller.
  */
-int regulator_bulk_register_supply_alias(struct device *dev, const char **id,
+int regulator_bulk_register_supply_alias(struct device *dev,
+					 const char *const *id,
 					 struct device *alias_dev,
-					 const char **alias_id,
+					 const char *const *alias_id,
 					 int num_id)
 {
 	int i;
@@ -1637,7 +1644,7 @@ EXPORT_SYMBOL_GPL(regulator_bulk_register_supply_alias);
  * aliases in one operation.
  */
 void regulator_bulk_unregister_supply_alias(struct device *dev,
-					    const char **id,
+					    const char *const *id,
 					    int num_id)
 {
 	int i;
@@ -2321,6 +2328,10 @@ static int _regulator_do_set_voltage(struct regulator_dev *rdev,
 			    regulator_list_voltage_linear)
 				ret = regulator_map_voltage_linear(rdev,
 								min_uV, max_uV);
+			else if (rdev->desc->ops->list_voltage ==
+				 regulator_list_voltage_linear_range)
+				ret = regulator_map_voltage_linear_range(rdev,
+								min_uV, max_uV);
 			else
 				ret = regulator_map_voltage_iterate(rdev,
 								min_uV, max_uV);
@@ -3447,7 +3458,7 @@ regulator_register(const struct regulator_desc *regulator_desc,
 
 	/* register with sysfs */
 	rdev->dev.class = &regulator_class;
-	rdev->dev.of_node = config->of_node;
+	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);
@@ -3589,6 +3600,7 @@ void regulator_unregister(struct regulator_dev *rdev)
 	list_del(&rdev->list);
 	kfree(rdev->constraints);
 	regulator_ena_gpio_free(rdev);
+	of_node_put(rdev->dev.of_node);
 	device_unregister(&rdev->dev);
 	mutex_unlock(&regulator_list_mutex);
 }
@@ -3819,8 +3831,9 @@ static int __init regulator_init_complete(void)
 	mutex_lock(&regulator_list_mutex);
 
 	/* If we have a full configuration then disable any regulators
-	 * which are not in use or always_on.  This will become the
-	 * default behaviour in the future.
+	 * we have permission to change the status for and which are
+	 * not in use or always_on.  This is effectively the default
+	 * for DT and ACPI as they have full constraints.
 	 */
 	list_for_each_entry(rdev, &regulator_list, list) {
 		ops = rdev->desc->ops;
@@ -3829,6 +3842,9 @@ static int __init regulator_init_complete(void)
 		if (c && c->always_on)
 			continue;
 
+		if (c && !(c->valid_ops_mask & REGULATOR_CHANGE_STATUS))
+			continue;
+
 		mutex_lock(&rdev->mutex);
 
 		if (rdev->use_count)
@@ -3867,4 +3883,4 @@ unlock:
 
 	return 0;
 }
-late_initcall(regulator_init_complete);
+late_initcall_sync(regulator_init_complete);
diff --git a/drivers/regulator/devres.c b/drivers/regulator/devres.c
index f44818b838d..8f785bc9e51 100644
--- a/drivers/regulator/devres.c
+++ b/drivers/regulator/devres.c
@@ -360,9 +360,9 @@ EXPORT_SYMBOL_GPL(devm_regulator_unregister_supply_alias);
  * will be removed before returning to the caller.
  */
 int devm_regulator_bulk_register_supply_alias(struct device *dev,
-					      const char **id,
+					      const char *const *id,
 					      struct device *alias_dev,
-					      const char **alias_id,
+					      const char *const *alias_id,
 					      int num_id)
 {
 	int i;
@@ -404,7 +404,7 @@ EXPORT_SYMBOL_GPL(devm_regulator_bulk_register_supply_alias);
  * will ensure that the resource is freed.
  */
 void devm_regulator_bulk_unregister_supply_alias(struct device *dev,
-						 const char **id,
+						 const char *const *id,
 						 int num_id)
 {
 	int i;
diff --git a/drivers/regulator/fixed.c b/drivers/regulator/fixed.c
index c61f7e97e4f..354105eff1f 100644
--- a/drivers/regulator/fixed.c
+++ b/drivers/regulator/fixed.c
@@ -50,7 +50,6 @@ of_get_fixed_voltage_config(struct device *dev)
 {
 	struct fixed_voltage_config *config;
 	struct device_node *np = dev->of_node;
-	const __be32 *delay;
 	struct regulator_init_data *init_data;
 
 	config = devm_kzalloc(dev, sizeof(struct fixed_voltage_config),
@@ -91,15 +90,11 @@ of_get_fixed_voltage_config(struct device *dev)
 	if ((config->gpio == -ENODEV) || (config->gpio == -EPROBE_DEFER))
 		return ERR_PTR(-EPROBE_DEFER);
 
-	delay = of_get_property(np, "startup-delay-us", NULL);
-	if (delay)
-		config->startup_delay = be32_to_cpu(*delay);
+	of_property_read_u32(np, "startup-delay-us", &config->startup_delay);
 
-	if (of_find_property(np, "enable-active-high", NULL))
-		config->enable_high = true;
-
-	if (of_find_property(np, "gpio-open-drain", NULL))
-		config->gpio_is_open_drain = true;
+	config->enable_high = of_property_read_bool(np, "enable-active-high");
+	config->gpio_is_open_drain = of_property_read_bool(np,
+							   "gpio-open-drain");
 
 	if (of_find_property(np, "vin-supply", NULL))
 		config->input_supply = "vin";
diff --git a/drivers/regulator/ltc3589.c b/drivers/regulator/ltc3589.c
new file mode 100644
index 00000000000..110a99ee116
--- /dev/null
+++ b/drivers/regulator/ltc3589.c
@@ -0,0 +1,554 @@
+/*
+ * Linear Technology LTC3589,LTC3589-1 regulator support
+ *
+ * Copyright (c) 2014 Philipp Zabel <p.zabel@pengutronix.de>, Pengutronix
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * 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/init.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/regmap.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/of_regulator.h>
+
+#define DRIVER_NAME		"ltc3589"
+
+#define LTC3589_IRQSTAT		0x02
+#define LTC3589_SCR1		0x07
+#define LTC3589_OVEN		0x10
+#define LTC3589_SCR2		0x12
+#define LTC3589_PGSTAT		0x13
+#define LTC3589_VCCR		0x20
+#define LTC3589_CLIRQ		0x21
+#define LTC3589_B1DTV1		0x23
+#define LTC3589_B1DTV2		0x24
+#define LTC3589_VRRCR		0x25
+#define LTC3589_B2DTV1		0x26
+#define LTC3589_B2DTV2		0x27
+#define LTC3589_B3DTV1		0x29
+#define LTC3589_B3DTV2		0x2a
+#define LTC3589_L2DTV1		0x32
+#define LTC3589_L2DTV2		0x33
+
+#define LTC3589_IRQSTAT_PGOOD_TIMEOUT	BIT(3)
+#define LTC3589_IRQSTAT_UNDERVOLT_WARN	BIT(4)
+#define LTC3589_IRQSTAT_UNDERVOLT_FAULT	BIT(5)
+#define LTC3589_IRQSTAT_THERMAL_WARN	BIT(6)
+#define LTC3589_IRQSTAT_THERMAL_FAULT	BIT(7)
+
+#define LTC3589_OVEN_SW1		BIT(0)
+#define LTC3589_OVEN_SW2		BIT(1)
+#define LTC3589_OVEN_SW3		BIT(2)
+#define LTC3589_OVEN_BB_OUT		BIT(3)
+#define LTC3589_OVEN_LDO2		BIT(4)
+#define LTC3589_OVEN_LDO3		BIT(5)
+#define LTC3589_OVEN_LDO4		BIT(6)
+#define LTC3589_OVEN_SW_CTRL		BIT(7)
+
+#define LTC3589_VCCR_SW1_GO		BIT(0)
+#define LTC3589_VCCR_SW2_GO		BIT(2)
+#define LTC3589_VCCR_SW3_GO		BIT(4)
+#define LTC3589_VCCR_LDO2_GO		BIT(6)
+
+enum ltc3589_variant {
+	LTC3589,
+	LTC3589_1,
+	LTC3589_2,
+};
+
+enum ltc3589_reg {
+	LTC3589_SW1,
+	LTC3589_SW2,
+	LTC3589_SW3,
+	LTC3589_BB_OUT,
+	LTC3589_LDO1,
+	LTC3589_LDO2,
+	LTC3589_LDO3,
+	LTC3589_LDO4,
+	LTC3589_NUM_REGULATORS,
+};
+
+struct ltc3589_regulator {
+	struct regulator_desc desc;
+
+	/* External feedback voltage divider */
+	unsigned int r1;
+	unsigned int r2;
+};
+
+struct ltc3589 {
+	struct regmap *regmap;
+	struct device *dev;
+	enum ltc3589_variant variant;
+	struct ltc3589_regulator regulator_descs[LTC3589_NUM_REGULATORS];
+	struct regulator_dev *regulators[LTC3589_NUM_REGULATORS];
+};
+
+static const int ltc3589_ldo4[] = {
+	2800000, 2500000, 1800000, 3300000,
+};
+
+static const int ltc3589_12_ldo4[] = {
+	1200000, 1800000, 2500000, 3200000,
+};
+
+static int ltc3589_set_ramp_delay(struct regulator_dev *rdev, int ramp_delay)
+{
+	struct ltc3589 *ltc3589 = rdev_get_drvdata(rdev);
+	int sel, shift;
+
+	if (unlikely(ramp_delay <= 0))
+		return -EINVAL;
+
+	/* VRRCR slew rate offsets are the same as VCCR go bit offsets */
+	shift = ffs(rdev->desc->apply_bit) - 1;
+
+	/* The slew rate can be set to 0.88, 1.75, 3.5, or 7 mV/uS */
+	for (sel = 0; sel < 4; sel++) {
+		if ((880 << sel) >= ramp_delay) {
+			return regmap_update_bits(ltc3589->regmap,
+						  LTC3589_VRRCR,
+						  0x3 << shift, sel << shift);
+		}
+	}
+	return -EINVAL;
+}
+
+static int ltc3589_set_suspend_voltage(struct regulator_dev *rdev, int uV)
+{
+	struct ltc3589 *ltc3589 = rdev_get_drvdata(rdev);
+	int sel;
+
+	sel = regulator_map_voltage_linear(rdev, uV, uV);
+	if (sel < 0)
+		return sel;
+
+	/* DTV2 register follows right after the corresponding DTV1 register */
+	return regmap_update_bits(ltc3589->regmap, rdev->desc->vsel_reg + 1,
+				  rdev->desc->vsel_mask, sel);
+}
+
+static int ltc3589_set_suspend_mode(struct regulator_dev *rdev,
+				    unsigned int mode)
+{
+	struct ltc3589 *ltc3589 = rdev_get_drvdata(rdev);
+	int mask, bit = 0;
+
+	/* VCCR reference selects are right next to the VCCR go bits */
+	mask = rdev->desc->apply_bit << 1;
+
+	if (mode == REGULATOR_MODE_STANDBY)
+		bit = mask;	/* Select DTV2 */
+
+	mask |= rdev->desc->apply_bit;
+	bit |= rdev->desc->apply_bit;
+	return regmap_update_bits(ltc3589->regmap, LTC3589_VCCR, mask, bit);
+}
+
+/* SW1, SW2, SW3, LDO2 */
+static struct regulator_ops ltc3589_linear_regulator_ops = {
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
+	.list_voltage = regulator_list_voltage_linear,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.set_ramp_delay = ltc3589_set_ramp_delay,
+	.set_voltage_time_sel = regulator_set_voltage_time_sel,
+	.set_suspend_voltage = ltc3589_set_suspend_voltage,
+	.set_suspend_mode = ltc3589_set_suspend_mode,
+};
+
+/* BB_OUT, LDO3 */
+static struct regulator_ops ltc3589_fixed_regulator_ops = {
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
+};
+
+/* LDO1 */
+static struct regulator_ops ltc3589_fixed_standby_regulator_ops = {
+};
+
+/* LDO4 */
+static struct regulator_ops ltc3589_table_regulator_ops = {
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
+	.list_voltage = regulator_list_voltage_table,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+};
+
+
+#define LTC3589_REG(_name, _ops, en_bit, dtv1_reg, dtv_mask, go_bit)	\
+	[LTC3589_ ## _name] = {						\
+		.desc = {						\
+			.name = #_name,					\
+			.n_voltages = (dtv_mask) + 1,			\
+			.min_uV = (go_bit) ? 362500 : 0,		\
+			.uV_step = (go_bit) ? 12500 : 0,		\
+			.ramp_delay = (go_bit) ? 1750 : 0,		\
+			.fixed_uV = (dtv_mask) ? 0 : 800000,		\
+			.ops = &ltc3589_ ## _ops ## _regulator_ops,	\
+			.type = REGULATOR_VOLTAGE,			\
+			.id = LTC3589_ ## _name,			\
+			.owner = THIS_MODULE,				\
+			.vsel_reg = (dtv1_reg),			\
+			.vsel_mask = (dtv_mask),			\
+			.apply_reg = (go_bit) ? LTC3589_VCCR : 0,	\
+			.apply_bit = (go_bit),				\
+			.enable_reg = (en_bit) ? LTC3589_OVEN : 0,	\
+			.enable_mask = (en_bit),			\
+		},							\
+	}
+
+#define LTC3589_LINEAR_REG(_name, _dtv1)				\
+	LTC3589_REG(_name, linear, LTC3589_OVEN_ ## _name,		\
+		    LTC3589_ ## _dtv1, 0x1f,				\
+		    LTC3589_VCCR_ ## _name ## _GO)
+
+#define LTC3589_FIXED_REG(_name) \
+	LTC3589_REG(_name, fixed, LTC3589_OVEN_ ## _name, 0, 0, 0)
+
+static struct ltc3589_regulator ltc3589_regulators[LTC3589_NUM_REGULATORS] = {
+	LTC3589_LINEAR_REG(SW1, B1DTV1),
+	LTC3589_LINEAR_REG(SW2, B2DTV1),
+	LTC3589_LINEAR_REG(SW3, B3DTV1),
+	LTC3589_FIXED_REG(BB_OUT),
+	LTC3589_REG(LDO1, fixed_standby, 0, 0, 0, 0),
+	LTC3589_LINEAR_REG(LDO2, L2DTV1),
+	LTC3589_FIXED_REG(LDO3),
+	LTC3589_REG(LDO4, table, LTC3589_OVEN_LDO4, LTC3589_L2DTV2, 0x60, 0),
+};
+
+#ifdef CONFIG_OF
+static struct of_regulator_match ltc3589_matches[LTC3589_NUM_REGULATORS] = {
+	{ .name = "sw1",    },
+	{ .name = "sw2",    },
+	{ .name = "sw3",    },
+	{ .name = "bb-out", },
+	{ .name = "ldo1",   }, /* standby */
+	{ .name = "ldo2",   },
+	{ .name = "ldo3",   },
+	{ .name = "ldo4",   },
+};
+
+static int ltc3589_parse_regulators_dt(struct ltc3589 *ltc3589)
+{
+	struct device *dev = ltc3589->dev;
+	struct device_node *node;
+	int i, ret;
+
+	node = of_find_node_by_name(dev->of_node, "regulators");
+	if (!node) {
+		dev_err(dev, "regulators node not found\n");
+		return -EINVAL;
+	}
+
+	ret = of_regulator_match(dev, node, ltc3589_matches,
+				 ARRAY_SIZE(ltc3589_matches));
+	of_node_put(node);
+	if (ret < 0) {
+		dev_err(dev, "Error parsing regulator init data: %d\n", ret);
+		return ret;
+	}
+	if (ret != LTC3589_NUM_REGULATORS) {
+		dev_err(dev, "Only %d regulators described in device tree\n",
+			ret);
+		return -EINVAL;
+	}
+
+	/* Parse feedback voltage dividers. LDO3 and LDO4 don't have them */
+	for (i = 0; i < LTC3589_LDO3; i++) {
+		struct ltc3589_regulator *desc = &ltc3589->regulator_descs[i];
+		struct device_node *np = ltc3589_matches[i].of_node;
+		u32 vdiv[2];
+
+		ret = of_property_read_u32_array(np, "lltc,fb-voltage-divider",
+						 vdiv, 2);
+		if (ret) {
+			dev_err(dev, "Failed to parse voltage divider: %d\n",
+				ret);
+			return ret;
+		}
+
+		desc->r1 = vdiv[0];
+		desc->r2 = vdiv[1];
+	}
+
+	return 0;
+}
+
+static inline struct regulator_init_data *match_init_data(int index)
+{
+	return ltc3589_matches[index].init_data;
+}
+
+static inline struct device_node *match_of_node(int index)
+{
+	return ltc3589_matches[index].of_node;
+}
+#else
+static inline int ltc3589_parse_regulators_dt(struct ltc3589 *ltc3589)
+{
+	return 0;
+}
+
+static inline struct regulator_init_data *match_init_data(int index)
+{
+	return NULL;
+}
+
+static inline struct device_node *match_of_node(int index)
+{
+	return NULL;
+}
+#endif
+
+static bool ltc3589_writeable_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case LTC3589_IRQSTAT:
+	case LTC3589_SCR1:
+	case LTC3589_OVEN:
+	case LTC3589_SCR2:
+	case LTC3589_VCCR:
+	case LTC3589_CLIRQ:
+	case LTC3589_B1DTV1:
+	case LTC3589_B1DTV2:
+	case LTC3589_VRRCR:
+	case LTC3589_B2DTV1:
+	case LTC3589_B2DTV2:
+	case LTC3589_B3DTV1:
+	case LTC3589_B3DTV2:
+	case LTC3589_L2DTV1:
+	case LTC3589_L2DTV2:
+		return true;
+	}
+	return false;
+}
+
+static bool ltc3589_readable_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case LTC3589_IRQSTAT:
+	case LTC3589_SCR1:
+	case LTC3589_OVEN:
+	case LTC3589_SCR2:
+	case LTC3589_PGSTAT:
+	case LTC3589_VCCR:
+	case LTC3589_B1DTV1:
+	case LTC3589_B1DTV2:
+	case LTC3589_VRRCR:
+	case LTC3589_B2DTV1:
+	case LTC3589_B2DTV2:
+	case LTC3589_B3DTV1:
+	case LTC3589_B3DTV2:
+	case LTC3589_L2DTV1:
+	case LTC3589_L2DTV2:
+		return true;
+	}
+	return false;
+}
+
+static bool ltc3589_volatile_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case LTC3589_IRQSTAT:
+	case LTC3589_PGSTAT:
+		return true;
+	}
+	return false;
+}
+
+struct reg_default ltc3589_reg_defaults[] = {
+	{ LTC3589_SCR1,   0x00 },
+	{ LTC3589_OVEN,   0x00 },
+	{ LTC3589_SCR2,   0x00 },
+	{ LTC3589_VCCR,   0x00 },
+	{ LTC3589_B1DTV1, 0x19 },
+	{ LTC3589_B1DTV2, 0x19 },
+	{ LTC3589_VRRCR,  0xff },
+	{ LTC3589_B2DTV1, 0x19 },
+	{ LTC3589_B2DTV2, 0x19 },
+	{ LTC3589_B3DTV1, 0x19 },
+	{ LTC3589_B3DTV2, 0x19 },
+	{ LTC3589_L2DTV1, 0x19 },
+	{ LTC3589_L2DTV2, 0x19 },
+};
+
+static const struct regmap_config ltc3589_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.writeable_reg = ltc3589_writeable_reg,
+	.readable_reg = ltc3589_readable_reg,
+	.volatile_reg = ltc3589_volatile_reg,
+	.max_register = LTC3589_L2DTV2,
+	.reg_defaults = ltc3589_reg_defaults,
+	.num_reg_defaults = ARRAY_SIZE(ltc3589_reg_defaults),
+	.use_single_rw = true,
+	.cache_type = REGCACHE_RBTREE,
+};
+
+
+static irqreturn_t ltc3589_isr(int irq, void *dev_id)
+{
+	struct ltc3589 *ltc3589 = dev_id;
+	unsigned int i, irqstat, event;
+
+	regmap_read(ltc3589->regmap, LTC3589_IRQSTAT, &irqstat);
+
+	if (irqstat & LTC3589_IRQSTAT_THERMAL_WARN) {
+		event = REGULATOR_EVENT_OVER_TEMP;
+		for (i = 0; i < LTC3589_NUM_REGULATORS; i++)
+			regulator_notifier_call_chain(ltc3589->regulators[i],
+						      event, NULL);
+	}
+
+	if (irqstat & LTC3589_IRQSTAT_UNDERVOLT_WARN) {
+		event = REGULATOR_EVENT_UNDER_VOLTAGE;
+		for (i = 0; i < LTC3589_NUM_REGULATORS; i++)
+			regulator_notifier_call_chain(ltc3589->regulators[i],
+						      event, NULL);
+	}
+
+	/* Clear warning condition */
+	regmap_write(ltc3589->regmap, LTC3589_CLIRQ, 0);
+
+	return IRQ_HANDLED;
+}
+
+static inline unsigned int ltc3589_scale(unsigned int uV, u32 r1, u32 r2)
+{
+	uint64_t tmp;
+	if (uV == 0)
+		return 0;
+	tmp = (uint64_t)uV * r1;
+	do_div(tmp, r2);
+	return uV + (unsigned int)tmp;
+}
+
+static void ltc3589_apply_fb_voltage_divider(struct ltc3589_regulator *rdesc)
+{
+	struct regulator_desc *desc = &rdesc->desc;
+
+	if (!rdesc->r1 || !rdesc->r2)
+		return;
+
+	desc->min_uV = ltc3589_scale(desc->min_uV, rdesc->r1, rdesc->r2);
+	desc->uV_step = ltc3589_scale(desc->uV_step, rdesc->r1, rdesc->r2);
+	desc->fixed_uV = ltc3589_scale(desc->fixed_uV, rdesc->r1, rdesc->r2);
+}
+
+static int ltc3589_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct device *dev = &client->dev;
+	struct ltc3589_regulator *descs;
+	struct ltc3589 *ltc3589;
+	int i, ret;
+
+	ltc3589 = devm_kzalloc(dev, sizeof(*ltc3589), GFP_KERNEL);
+	if (!ltc3589)
+		return -ENOMEM;
+
+	i2c_set_clientdata(client, ltc3589);
+	ltc3589->variant = id->driver_data;
+	ltc3589->dev = dev;
+
+	descs = ltc3589->regulator_descs;
+	memcpy(descs, ltc3589_regulators, sizeof(ltc3589_regulators));
+	if (ltc3589->variant == LTC3589) {
+		descs[LTC3589_LDO3].desc.fixed_uV = 1800000;
+		descs[LTC3589_LDO4].desc.volt_table = ltc3589_ldo4;
+	} else {
+		descs[LTC3589_LDO3].desc.fixed_uV = 2800000;
+		descs[LTC3589_LDO4].desc.volt_table = ltc3589_12_ldo4;
+	}
+
+	ltc3589->regmap = devm_regmap_init_i2c(client, &ltc3589_regmap_config);
+	if (IS_ERR(ltc3589->regmap)) {
+		ret = PTR_ERR(ltc3589->regmap);
+		dev_err(dev, "failed to initialize regmap: %d\n", ret);
+		return ret;
+	}
+
+	ret = ltc3589_parse_regulators_dt(ltc3589);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < LTC3589_NUM_REGULATORS; i++) {
+		struct ltc3589_regulator *rdesc = &ltc3589->regulator_descs[i];
+		struct regulator_desc *desc = &rdesc->desc;
+		struct regulator_init_data *init_data;
+		struct regulator_config config = { };
+
+		init_data = match_init_data(i);
+
+		if (i < LTC3589_LDO3)
+			ltc3589_apply_fb_voltage_divider(rdesc);
+
+		config.dev = dev;
+		config.init_data = init_data;
+		config.driver_data = ltc3589;
+		config.of_node = match_of_node(i);
+
+		ltc3589->regulators[i] = devm_regulator_register(dev, desc,
+								 &config);
+		if (IS_ERR(ltc3589->regulators[i])) {
+			ret = PTR_ERR(ltc3589->regulators[i]);
+			dev_err(dev, "failed to register regulator %s: %d\n",
+				desc->name, ret);
+			return ret;
+		}
+	}
+
+	ret = devm_request_threaded_irq(dev, client->irq, NULL, ltc3589_isr,
+					IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+					client->name, ltc3589);
+	if (ret) {
+		dev_err(dev, "Failed to request IRQ: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static struct i2c_device_id ltc3589_i2c_id[] = {
+	{ "ltc3589",   LTC3589   },
+	{ "ltc3589-1", LTC3589_1 },
+	{ "ltc3589-2", LTC3589_2 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ltc3589_i2c_id);
+
+static struct i2c_driver ltc3589_driver = {
+	.driver = {
+		.name = DRIVER_NAME,
+		.owner = THIS_MODULE,
+	},
+	.probe = ltc3589_probe,
+	.id_table = ltc3589_i2c_id,
+};
+module_i2c_driver(ltc3589_driver);
+
+MODULE_AUTHOR("Philipp Zabel <p.zabel@pengutronix.de>");
+MODULE_DESCRIPTION("Regulator driver for Linear Technology LTC3589(-1,2)");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("i2c:ltc3589");
diff --git a/drivers/regulator/max14577.c b/drivers/regulator/max14577.c
index ed60baaecee..5d9c605cf53 100644
--- a/drivers/regulator/max14577.c
+++ b/drivers/regulator/max14577.c
@@ -1,5 +1,5 @@
 /*
- * max14577.c - Regulator driver for the Maxim 14577
+ * max14577.c - Regulator driver for the Maxim 14577/77836
  *
  * Copyright (C) 2013,2014 Samsung Electronics
  * Krzysztof Kozlowski <k.kozlowski@samsung.com>
@@ -22,6 +22,42 @@
 #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);
@@ -47,6 +83,9 @@ static int max14577_reg_get_current_limit(struct regulator_dev *rdev)
 {
 	u8 reg_data;
 	struct regmap *rmap = rdev->regmap;
+	struct max14577 *max14577 = rdev_get_drvdata(rdev);
+	const struct maxim_charger_current *limits =
+		&maxim_charger_currents[max14577->dev_type];
 
 	if (rdev_get_id(rdev) != MAX14577_CHARGER)
 		return -EINVAL;
@@ -54,12 +93,11 @@ static int max14577_reg_get_current_limit(struct regulator_dev *rdev)
 	max14577_read_reg(rmap, MAX14577_CHG_REG_CHG_CTRL4, &reg_data);
 
 	if ((reg_data & CHGCTRL4_MBCICHWRCL_MASK) == 0)
-		return MAX14577_REGULATOR_CURRENT_LIMIT_MIN;
+		return limits->min;
 
 	reg_data = ((reg_data & CHGCTRL4_MBCICHWRCH_MASK) >>
 			CHGCTRL4_MBCICHWRCH_SHIFT);
-	return MAX14577_REGULATOR_CURRENT_LIMIT_HIGH_START +
-		reg_data * MAX14577_REGULATOR_CURRENT_LIMIT_HIGH_STEP;
+	return limits->high_start + reg_data * limits->high_step;
 }
 
 static int max14577_reg_set_current_limit(struct regulator_dev *rdev,
@@ -67,33 +105,39 @@ static int max14577_reg_set_current_limit(struct regulator_dev *rdev,
 {
 	int i, current_bits = 0xf;
 	u8 reg_data;
+	struct max14577 *max14577 = rdev_get_drvdata(rdev);
+	const struct maxim_charger_current *limits =
+		&maxim_charger_currents[max14577->dev_type];
 
 	if (rdev_get_id(rdev) != MAX14577_CHARGER)
 		return -EINVAL;
 
-	if (min_uA > MAX14577_REGULATOR_CURRENT_LIMIT_MAX ||
-			max_uA < MAX14577_REGULATOR_CURRENT_LIMIT_MIN)
+	if (min_uA > limits->max || max_uA < limits->min)
 		return -EINVAL;
 
-	if (max_uA < MAX14577_REGULATOR_CURRENT_LIMIT_HIGH_START) {
-		/* Less than 200 mA, so set 90mA (turn only Low Bit off) */
+	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: <LIMIT_HIGH_START, inifinite>, so search for
-	 * valid current starting from LIMIT_MAX. */
-	for (i = MAX14577_REGULATOR_CURRENT_LIMIT_MAX;
-			i >= MAX14577_REGULATOR_CURRENT_LIMIT_HIGH_START;
-			i -= MAX14577_REGULATOR_CURRENT_LIMIT_HIGH_STEP) {
+	/*
+	 * 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 200mA-950 mA) */
+
+	/* 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;
@@ -118,7 +162,7 @@ static struct regulator_ops max14577_charger_ops = {
 	.set_current_limit	= max14577_reg_set_current_limit,
 };
 
-static const struct regulator_desc supported_regulators[] = {
+static const struct regulator_desc max14577_supported_regulators[] = {
 	[MAX14577_SAFEOUT] = {
 		.name		= "SAFEOUT",
 		.id		= MAX14577_SAFEOUT,
@@ -141,16 +185,88 @@ static const struct regulator_desc supported_regulators[] = {
 	},
 };
 
+static struct regulator_ops max77836_ldo_ops = {
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= regulator_enable_regmap,
+	.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,
+	/* TODO: add .set_suspend_mode */
+};
+
+static const struct regulator_desc max77836_supported_regulators[] = {
+	[MAX14577_SAFEOUT] = {
+		.name		= "SAFEOUT",
+		.id		= MAX14577_SAFEOUT,
+		.ops		= &max14577_safeout_ops,
+		.type		= REGULATOR_VOLTAGE,
+		.owner		= THIS_MODULE,
+		.n_voltages	= 1,
+		.min_uV		= MAX14577_REGULATOR_SAFEOUT_VOLTAGE,
+		.enable_reg	= MAX14577_REG_CONTROL2,
+		.enable_mask	= CTRL2_SFOUTORD_MASK,
+	},
+	[MAX14577_CHARGER] = {
+		.name		= "CHARGER",
+		.id		= MAX14577_CHARGER,
+		.ops		= &max14577_charger_ops,
+		.type		= REGULATOR_CURRENT,
+		.owner		= THIS_MODULE,
+		.enable_reg	= MAX14577_CHG_REG_CHG_CTRL2,
+		.enable_mask	= CHGCTRL2_MBCHOSTEN_MASK,
+	},
+	[MAX77836_LDO1] = {
+		.name		= "LDO1",
+		.id		= MAX77836_LDO1,
+		.ops		= &max77836_ldo_ops,
+		.type		= REGULATOR_VOLTAGE,
+		.owner		= THIS_MODULE,
+		.n_voltages	= MAX77836_REGULATOR_LDO_VOLTAGE_STEPS_NUM,
+		.min_uV		= MAX77836_REGULATOR_LDO_VOLTAGE_MIN,
+		.uV_step	= MAX77836_REGULATOR_LDO_VOLTAGE_STEP,
+		.enable_reg	= MAX77836_LDO_REG_CNFG1_LDO1,
+		.enable_mask	= MAX77836_CNFG1_LDO_PWRMD_MASK,
+		.vsel_reg	= MAX77836_LDO_REG_CNFG1_LDO1,
+		.vsel_mask	= MAX77836_CNFG1_LDO_TV_MASK,
+	},
+	[MAX77836_LDO2] = {
+		.name		= "LDO2",
+		.id		= MAX77836_LDO2,
+		.ops		= &max77836_ldo_ops,
+		.type		= REGULATOR_VOLTAGE,
+		.owner		= THIS_MODULE,
+		.n_voltages	= MAX77836_REGULATOR_LDO_VOLTAGE_STEPS_NUM,
+		.min_uV		= MAX77836_REGULATOR_LDO_VOLTAGE_MIN,
+		.uV_step	= MAX77836_REGULATOR_LDO_VOLTAGE_STEP,
+		.enable_reg	= MAX77836_LDO_REG_CNFG1_LDO2,
+		.enable_mask	= MAX77836_CNFG1_LDO_PWRMD_MASK,
+		.vsel_reg	= MAX77836_LDO_REG_CNFG1_LDO2,
+		.vsel_mask	= MAX77836_CNFG1_LDO_TV_MASK,
+	},
+};
+
 #ifdef CONFIG_OF
 static struct of_regulator_match max14577_regulator_matches[] = {
 	{ .name	= "SAFEOUT", },
 	{ .name = "CHARGER", },
 };
 
-static int max14577_regulator_dt_parse_pdata(struct platform_device *pdev)
+static struct of_regulator_match max77836_regulator_matches[] = {
+	{ .name	= "SAFEOUT", },
+	{ .name = "CHARGER", },
+	{ .name = "LDO1", },
+	{ .name = "LDO2", },
+};
+
+static int max14577_regulator_dt_parse_pdata(struct platform_device *pdev,
+		enum maxim_device_type dev_type)
 {
 	int ret;
 	struct device_node *np;
+	struct of_regulator_match *regulator_matches;
+	unsigned int regulator_matches_size;
 
 	np = of_get_child_by_name(pdev->dev.parent->of_node, "regulators");
 	if (!np) {
@@ -158,8 +274,19 @@ static int max14577_regulator_dt_parse_pdata(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	ret = of_regulator_match(&pdev->dev, np, max14577_regulator_matches,
-			MAX14577_REG_MAX);
+	switch (dev_type) {
+	case MAXIM_DEVICE_TYPE_MAX77836:
+		regulator_matches = max77836_regulator_matches;
+		regulator_matches_size = ARRAY_SIZE(max77836_regulator_matches);
+		break;
+	case MAXIM_DEVICE_TYPE_MAX14577:
+	default:
+		regulator_matches = max14577_regulator_matches;
+		regulator_matches_size = ARRAY_SIZE(max14577_regulator_matches);
+	}
+
+	ret = of_regulator_match(&pdev->dev, np, regulator_matches,
+			regulator_matches_size);
 	if (ret < 0)
 		dev_err(&pdev->dev, "Error parsing regulator init data: %d\n", ret);
 	else
@@ -170,31 +297,74 @@ static int max14577_regulator_dt_parse_pdata(struct platform_device *pdev)
 	return ret;
 }
 
-static inline struct regulator_init_data *match_init_data(int index)
+static inline struct regulator_init_data *match_init_data(int index,
+		enum maxim_device_type dev_type)
 {
-	return max14577_regulator_matches[index].init_data;
+	switch (dev_type) {
+	case MAXIM_DEVICE_TYPE_MAX77836:
+		return max77836_regulator_matches[index].init_data;
+
+	case MAXIM_DEVICE_TYPE_MAX14577:
+	default:
+		return max14577_regulator_matches[index].init_data;
+	}
 }
 
-static inline struct device_node *match_of_node(int index)
+static inline struct device_node *match_of_node(int index,
+		enum maxim_device_type dev_type)
 {
-	return max14577_regulator_matches[index].of_node;
+	switch (dev_type) {
+	case MAXIM_DEVICE_TYPE_MAX77836:
+		return max77836_regulator_matches[index].of_node;
+
+	case MAXIM_DEVICE_TYPE_MAX14577:
+	default:
+		return max14577_regulator_matches[index].of_node;
+	}
 }
 #else /* CONFIG_OF */
-static int max14577_regulator_dt_parse_pdata(struct platform_device *pdev)
+static int max14577_regulator_dt_parse_pdata(struct platform_device *pdev,
+		enum maxim_device_type dev_type)
 {
 	return 0;
 }
-static inline struct regulator_init_data *match_init_data(int index)
+static inline struct regulator_init_data *match_init_data(int index,
+		enum maxim_device_type dev_type)
 {
 	return NULL;
 }
 
-static inline struct device_node *match_of_node(int index)
+static inline struct device_node *match_of_node(int index,
+		enum maxim_device_type dev_type)
 {
 	return NULL;
 }
 #endif /* CONFIG_OF */
 
+/**
+ * Registers for regulators of max77836 use different I2C slave addresses so
+ * different regmaps must be used for them.
+ *
+ * Returns proper regmap for accessing regulator passed by id.
+ */
+static struct regmap *max14577_get_regmap(struct max14577 *max14577,
+		int reg_id)
+{
+	switch (max14577->dev_type) {
+	case MAXIM_DEVICE_TYPE_MAX77836:
+		switch (reg_id) {
+		case MAX77836_SAFEOUT ... MAX77836_CHARGER:
+			return max14577->regmap;
+		default:
+			/* MAX77836_LDO1 ... MAX77836_LDO2 */
+			return max14577->regmap_pmic;
+		}
+
+	case MAXIM_DEVICE_TYPE_MAX14577:
+	default:
+		return max14577->regmap;
+	}
+}
 
 static int max14577_regulator_probe(struct platform_device *pdev)
 {
@@ -202,15 +372,29 @@ static int max14577_regulator_probe(struct platform_device *pdev)
 	struct max14577_platform_data *pdata = dev_get_platdata(max14577->dev);
 	int i, ret;
 	struct regulator_config config = {};
+	const struct regulator_desc *supported_regulators;
+	unsigned int supported_regulators_size;
+	enum maxim_device_type dev_type = max14577->dev_type;
 
-	ret = max14577_regulator_dt_parse_pdata(pdev);
+	ret = max14577_regulator_dt_parse_pdata(pdev, dev_type);
 	if (ret)
 		return ret;
 
+	switch (dev_type) {
+	case MAXIM_DEVICE_TYPE_MAX77836:
+		supported_regulators = max77836_supported_regulators;
+		supported_regulators_size = ARRAY_SIZE(max77836_supported_regulators);
+		break;
+	case MAXIM_DEVICE_TYPE_MAX14577:
+	default:
+		supported_regulators = max14577_supported_regulators;
+		supported_regulators_size = ARRAY_SIZE(max14577_supported_regulators);
+	}
+
 	config.dev = &pdev->dev;
-	config.regmap = max14577->regmap;
+	config.driver_data = max14577;
 
-	for (i = 0; i < ARRAY_SIZE(supported_regulators); i++) {
+	for (i = 0; i < supported_regulators_size; i++) {
 		struct regulator_dev *regulator;
 		/*
 		 * Index of supported_regulators[] is also the id and must
@@ -220,17 +404,19 @@ static int max14577_regulator_probe(struct platform_device *pdev)
 			config.init_data = pdata->regulators[i].initdata;
 			config.of_node = pdata->regulators[i].of_node;
 		} else {
-			config.init_data = match_init_data(i);
-			config.of_node = match_of_node(i);
+			config.init_data = match_init_data(i, dev_type);
+			config.of_node = match_of_node(i, dev_type);
 		}
+		config.regmap = max14577_get_regmap(max14577,
+				supported_regulators[i].id);
 
 		regulator = devm_regulator_register(&pdev->dev,
 				&supported_regulators[i], &config);
 		if (IS_ERR(regulator)) {
 			ret = PTR_ERR(regulator);
 			dev_err(&pdev->dev,
-					"Regulator init failed for ID %d with error: %d\n",
-					i, ret);
+					"Regulator init failed for %d/%s with error: %d\n",
+					i, supported_regulators[i].name, ret);
 			return ret;
 		}
 	}
@@ -238,20 +424,41 @@ static int max14577_regulator_probe(struct platform_device *pdev)
 	return ret;
 }
 
+static const struct platform_device_id max14577_regulator_id[] = {
+	{ "max14577-regulator", MAXIM_DEVICE_TYPE_MAX14577, },
+	{ "max77836-regulator", MAXIM_DEVICE_TYPE_MAX77836, },
+	{ }
+};
+MODULE_DEVICE_TABLE(platform, max14577_regulator_id);
+
 static struct platform_driver max14577_regulator_driver = {
 	.driver = {
 		   .owner = THIS_MODULE,
 		   .name = "max14577-regulator",
 		   },
-	.probe	= max14577_regulator_probe,
+	.probe		= max14577_regulator_probe,
+	.id_table	= max14577_regulator_id,
 };
 
 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(ARRAY_SIZE(supported_regulators) != MAX14577_REG_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);
+
+	BUILD_BUG_ON(MAX77836_REGULATOR_LDO_VOLTAGE_MIN +
+			(MAX77836_REGULATOR_LDO_VOLTAGE_STEP *
+			  (MAX77836_REGULATOR_LDO_VOLTAGE_STEPS_NUM - 1)) !=
+			MAX77836_REGULATOR_LDO_VOLTAGE_MAX);
 
 	return platform_driver_register(&max14577_regulator_driver);
 }
@@ -264,6 +471,6 @@ static void __exit max14577_regulator_exit(void)
 module_exit(max14577_regulator_exit);
 
 MODULE_AUTHOR("Krzysztof Kozlowski <k.kozlowski@samsung.com>");
-MODULE_DESCRIPTION("MAXIM 14577 regulator driver");
+MODULE_DESCRIPTION("Maxim 14577/77836 regulator driver");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("platform:max14577-regulator");
diff --git a/drivers/regulator/max8649.c b/drivers/regulator/max8649.c
index 3172da847d2..c8bddcc8f91 100644
--- a/drivers/regulator/max8649.c
+++ b/drivers/regulator/max8649.c
@@ -161,10 +161,8 @@ static int max8649_regulator_probe(struct i2c_client *client,
 
 	info = devm_kzalloc(&client->dev, sizeof(struct max8649_regulator_info),
 			    GFP_KERNEL);
-	if (!info) {
-		dev_err(&client->dev, "No enough memory\n");
+	if (!info)
 		return -ENOMEM;
-	}
 
 	info->regmap = devm_regmap_init_i2c(client, &max8649_regmap_config);
 	if (IS_ERR(info->regmap)) {
diff --git a/drivers/regulator/max8952.c b/drivers/regulator/max8952.c
index d920f5a32ec..c2792f0271a 100644
--- a/drivers/regulator/max8952.c
+++ b/drivers/regulator/max8952.c
@@ -129,7 +129,7 @@ static const struct regulator_desc regulator = {
 };
 
 #ifdef CONFIG_OF
-static struct of_device_id max8952_dt_match[] = {
+static const struct of_device_id max8952_dt_match[] = {
 	{ .compatible = "maxim,max8952" },
 	{},
 };
diff --git a/drivers/regulator/of_regulator.c b/drivers/regulator/of_regulator.c
index ea4f36f2cbe..ee5e67bc8d5 100644
--- a/drivers/regulator/of_regulator.c
+++ b/drivers/regulator/of_regulator.c
@@ -19,9 +19,7 @@
 static void of_get_regulation_constraints(struct device_node *np,
 					struct regulator_init_data **init_data)
 {
-	const __be32 *min_uV, *max_uV, *uV_offset;
-	const __be32 *min_uA, *max_uA, *ramp_delay;
-	struct property *prop;
+	const __be32 *min_uV, *max_uV;
 	struct regulation_constraints *constraints = &(*init_data)->constraints;
 	int ret;
 	u32 pval;
@@ -42,36 +40,29 @@ static void of_get_regulation_constraints(struct device_node *np,
 	if (min_uV && max_uV && constraints->min_uV == constraints->max_uV)
 		constraints->apply_uV = true;
 
-	uV_offset = of_get_property(np, "regulator-microvolt-offset", NULL);
-	if (uV_offset)
-		constraints->uV_offset = be32_to_cpu(*uV_offset);
-	min_uA = of_get_property(np, "regulator-min-microamp", NULL);
-	if (min_uA)
-		constraints->min_uA = be32_to_cpu(*min_uA);
-	max_uA = of_get_property(np, "regulator-max-microamp", NULL);
-	if (max_uA)
-		constraints->max_uA = be32_to_cpu(*max_uA);
+	if (!of_property_read_u32(np, "regulator-microvolt-offset", &pval))
+		constraints->uV_offset = pval;
+	if (!of_property_read_u32(np, "regulator-min-microamp", &pval))
+		constraints->min_uA = pval;
+	if (!of_property_read_u32(np, "regulator-max-microamp", &pval))
+		constraints->max_uA = pval;
 
 	/* Current change possible? */
 	if (constraints->min_uA != constraints->max_uA)
 		constraints->valid_ops_mask |= REGULATOR_CHANGE_CURRENT;
 
-	if (of_find_property(np, "regulator-boot-on", NULL))
-		constraints->boot_on = true;
-
-	if (of_find_property(np, "regulator-always-on", NULL))
-		constraints->always_on = true;
-	else /* status change should be possible if not always on. */
+	constraints->boot_on = of_property_read_bool(np, "regulator-boot-on");
+	constraints->always_on = of_property_read_bool(np, "regulator-always-on");
+	if (!constraints->always_on) /* status change should be possible. */
 		constraints->valid_ops_mask |= REGULATOR_CHANGE_STATUS;
 
 	if (of_property_read_bool(np, "regulator-allow-bypass"))
 		constraints->valid_ops_mask |= REGULATOR_CHANGE_BYPASS;
 
-	prop = of_find_property(np, "regulator-ramp-delay", NULL);
-	if (prop && prop->value) {
-		ramp_delay = prop->value;
-		if (*ramp_delay)
-			constraints->ramp_delay = be32_to_cpu(*ramp_delay);
+	ret = of_property_read_u32(np, "regulator-ramp-delay", &pval);
+	if (!ret) {
+		if (pval)
+			constraints->ramp_delay = pval;
 		else
 			constraints->ramp_disable = true;
 	}
@@ -106,6 +97,20 @@ struct regulator_init_data *of_get_regulator_init_data(struct device *dev,
 }
 EXPORT_SYMBOL_GPL(of_get_regulator_init_data);
 
+struct devm_of_regulator_matches {
+	struct of_regulator_match *matches;
+	unsigned int num_matches;
+};
+
+static void devm_of_regulator_put_matches(struct device *dev, void *res)
+{
+	struct devm_of_regulator_matches *devm_matches = res;
+	int i;
+
+	for (i = 0; i < devm_matches->num_matches; i++)
+		of_node_put(devm_matches->matches[i].of_node);
+}
+
 /**
  * of_regulator_match - extract multiple regulator init data from device tree.
  * @dev: device requesting the data
@@ -119,7 +124,8 @@ EXPORT_SYMBOL_GPL(of_get_regulator_init_data);
  * regulator. The data parsed from a child node will be matched to a regulator
  * based on either the deprecated property regulator-compatible if present,
  * or otherwise the child node's name. Note that the match table is modified
- * in place.
+ * in place and an additional of_node reference is taken for each matched
+ * regulator.
  *
  * Returns the number of matches found or a negative error code on failure.
  */
@@ -131,10 +137,22 @@ int of_regulator_match(struct device *dev, struct device_node *node,
 	unsigned int i;
 	const char *name;
 	struct device_node *child;
+	struct devm_of_regulator_matches *devm_matches;
 
 	if (!dev || !node)
 		return -EINVAL;
 
+	devm_matches = devres_alloc(devm_of_regulator_put_matches,
+				    sizeof(struct devm_of_regulator_matches),
+				    GFP_KERNEL);
+	if (!devm_matches)
+		return -ENOMEM;
+
+	devm_matches->matches = matches;
+	devm_matches->num_matches = num_matches;
+
+	devres_add(dev, devm_matches);
+
 	for (i = 0; i < num_matches; i++) {
 		struct of_regulator_match *match = &matches[i];
 		match->init_data = NULL;
@@ -162,7 +180,7 @@ int of_regulator_match(struct device *dev, struct device_node *node,
 					child->name);
 				return -EINVAL;
 			}
-			match->of_node = child;
+			match->of_node = of_node_get(child);
 			count++;
 			break;
 		}
diff --git a/drivers/regulator/palmas-regulator.c b/drivers/regulator/palmas-regulator.c
index 9c62b1d3468..864ed02ce4b 100644
--- a/drivers/regulator/palmas-regulator.c
+++ b/drivers/regulator/palmas-regulator.c
@@ -36,6 +36,18 @@ struct regs_info {
 	int	sleep_id;
 };
 
+static const struct regulator_linear_range smps_low_ranges[] = {
+	REGULATOR_LINEAR_RANGE(500000, 0x1, 0x6, 0),
+	REGULATOR_LINEAR_RANGE(510000, 0x7, 0x79, 10000),
+	REGULATOR_LINEAR_RANGE(1650000, 0x7A, 0x7f, 0),
+};
+
+static const struct regulator_linear_range smps_high_ranges[] = {
+	REGULATOR_LINEAR_RANGE(1000000, 0x1, 0x6, 0),
+	REGULATOR_LINEAR_RANGE(1020000, 0x7, 0x79, 20000),
+	REGULATOR_LINEAR_RANGE(3300000, 0x7A, 0x7f, 0),
+};
+
 static const struct regs_info palmas_regs_info[] = {
 	{
 		.name		= "SMPS12",
@@ -280,54 +292,6 @@ static int palmas_ldo_write(struct palmas *palmas, unsigned int reg,
 	return regmap_write(palmas->regmap[REGULATOR_SLAVE], addr, value);
 }
 
-static int palmas_is_enabled_smps(struct regulator_dev *dev)
-{
-	struct palmas_pmic *pmic = rdev_get_drvdata(dev);
-	int id = rdev_get_id(dev);
-	unsigned int reg;
-
-	palmas_smps_read(pmic->palmas, palmas_regs_info[id].ctrl_addr, &reg);
-
-	reg &= PALMAS_SMPS12_CTRL_STATUS_MASK;
-	reg >>= PALMAS_SMPS12_CTRL_STATUS_SHIFT;
-
-	return !!(reg);
-}
-
-static int palmas_enable_smps(struct regulator_dev *dev)
-{
-	struct palmas_pmic *pmic = rdev_get_drvdata(dev);
-	int id = rdev_get_id(dev);
-	unsigned int reg;
-
-	palmas_smps_read(pmic->palmas, palmas_regs_info[id].ctrl_addr, &reg);
-
-	reg &= ~PALMAS_SMPS12_CTRL_MODE_ACTIVE_MASK;
-	if (pmic->current_reg_mode[id])
-		reg |= pmic->current_reg_mode[id];
-	else
-		reg |= SMPS_CTRL_MODE_ON;
-
-	palmas_smps_write(pmic->palmas, palmas_regs_info[id].ctrl_addr, reg);
-
-	return 0;
-}
-
-static int palmas_disable_smps(struct regulator_dev *dev)
-{
-	struct palmas_pmic *pmic = rdev_get_drvdata(dev);
-	int id = rdev_get_id(dev);
-	unsigned int reg;
-
-	palmas_smps_read(pmic->palmas, palmas_regs_info[id].ctrl_addr, &reg);
-
-	reg &= ~PALMAS_SMPS12_CTRL_MODE_ACTIVE_MASK;
-
-	palmas_smps_write(pmic->palmas, palmas_regs_info[id].ctrl_addr, reg);
-
-	return 0;
-}
-
 static int palmas_set_mode_smps(struct regulator_dev *dev, unsigned int mode)
 {
 	struct palmas_pmic *pmic = rdev_get_drvdata(dev);
@@ -382,81 +346,6 @@ static unsigned int palmas_get_mode_smps(struct regulator_dev *dev)
 	return 0;
 }
 
-static int palmas_list_voltage_smps(struct regulator_dev *dev,
-					unsigned selector)
-{
-	struct palmas_pmic *pmic = rdev_get_drvdata(dev);
-	int id = rdev_get_id(dev);
-	int mult = 1;
-
-	/* Read the multiplier set in VSEL register to return
-	 * the correct voltage.
-	 */
-	if (pmic->range[id])
-		mult = 2;
-
-	if (selector == 0)
-		return 0;
-	else if (selector < 6)
-		return 500000 * mult;
-	else
-		/* Voltage is linear mapping starting from selector 6,
-		 * volt = (0.49V + ((selector - 5) * 0.01V)) * RANGE
-		 * RANGE is either x1 or x2
-		 */
-		return (490000 + ((selector - 5) * 10000)) * mult;
-}
-
-static int palmas_map_voltage_smps(struct regulator_dev *rdev,
-		int min_uV, int max_uV)
-{
-	struct palmas_pmic *pmic = rdev_get_drvdata(rdev);
-	int id = rdev_get_id(rdev);
-	int ret, voltage;
-
-	if (min_uV == 0)
-		return 0;
-
-	if (pmic->range[id]) { /* RANGE is x2 */
-		if (min_uV < 1000000)
-			min_uV = 1000000;
-		ret = DIV_ROUND_UP(min_uV - 1000000, 20000) + 6;
-	} else {		/* RANGE is x1 */
-		if (min_uV < 500000)
-			min_uV = 500000;
-		ret = DIV_ROUND_UP(min_uV - 500000, 10000) + 6;
-	}
-
-	/* Map back into a voltage to verify we're still in bounds */
-	voltage = palmas_list_voltage_smps(rdev, ret);
-	if (voltage < min_uV || voltage > max_uV)
-		return -EINVAL;
-
-	return ret;
-}
-
-static int palma_smps_set_voltage_smps_time_sel(struct regulator_dev *rdev,
-	unsigned int old_selector, unsigned int new_selector)
-{
-	struct palmas_pmic *pmic = rdev_get_drvdata(rdev);
-	int id = rdev_get_id(rdev);
-	int old_uv, new_uv;
-	unsigned int ramp_delay = pmic->ramp_delay[id];
-
-	if (!ramp_delay)
-		return 0;
-
-	old_uv = palmas_list_voltage_smps(rdev, old_selector);
-	if (old_uv < 0)
-		return old_uv;
-
-	new_uv = palmas_list_voltage_smps(rdev, new_selector);
-	if (new_uv < 0)
-		return new_uv;
-
-	return DIV_ROUND_UP(abs(old_uv - new_uv), ramp_delay);
-}
-
 static int palmas_smps_set_ramp_delay(struct regulator_dev *rdev,
 		 int ramp_delay)
 {
@@ -493,16 +382,16 @@ static int palmas_smps_set_ramp_delay(struct regulator_dev *rdev,
 }
 
 static struct regulator_ops palmas_ops_smps = {
-	.is_enabled		= palmas_is_enabled_smps,
-	.enable			= palmas_enable_smps,
-	.disable		= palmas_disable_smps,
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
 	.set_mode		= palmas_set_mode_smps,
 	.get_mode		= palmas_get_mode_smps,
 	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
 	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
-	.list_voltage		= palmas_list_voltage_smps,
-	.map_voltage		= palmas_map_voltage_smps,
-	.set_voltage_time_sel	= palma_smps_set_voltage_smps_time_sel,
+	.list_voltage		= regulator_list_voltage_linear_range,
+	.map_voltage		= regulator_map_voltage_linear_range,
+	.set_voltage_time_sel	= regulator_set_voltage_time_sel,
 	.set_ramp_delay		= palmas_smps_set_ramp_delay,
 };
 
@@ -511,9 +400,9 @@ static struct regulator_ops palmas_ops_ext_control_smps = {
 	.get_mode		= palmas_get_mode_smps,
 	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
 	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
-	.list_voltage		= palmas_list_voltage_smps,
-	.map_voltage		= palmas_map_voltage_smps,
-	.set_voltage_time_sel	= palma_smps_set_voltage_smps_time_sel,
+	.list_voltage		= regulator_list_voltage_linear_range,
+	.map_voltage		= regulator_map_voltage_linear_range,
+	.set_voltage_time_sel	= regulator_set_voltage_time_sel,
 	.set_ramp_delay		= palmas_smps_set_ramp_delay,
 };
 
@@ -1042,12 +931,17 @@ static int palmas_regulators_probe(struct platform_device *pdev)
 			 * ranges. Read the current smps mode for later use.
 			 */
 			addr = palmas_regs_info[id].vsel_addr;
+			pmic->desc[id].n_linear_ranges = 3;
 
 			ret = palmas_smps_read(pmic->palmas, addr, &reg);
 			if (ret)
 				return ret;
 			if (reg & PALMAS_SMPS12_VOLTAGE_RANGE)
 				pmic->range[id] = 1;
+			if (pmic->range[id])
+				pmic->desc[id].linear_ranges = smps_high_ranges;
+			else
+				pmic->desc[id].linear_ranges = smps_low_ranges;
 
 			if (reg_init && reg_init->roof_floor)
 				pmic->desc[id].ops =
@@ -1199,7 +1093,7 @@ static int palmas_regulators_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id of_palmas_match_tbl[] = {
+static const struct of_device_id of_palmas_match_tbl[] = {
 	{ .compatible = "ti,palmas-pmic", },
 	{ .compatible = "ti,twl6035-pmic", },
 	{ .compatible = "ti,twl6036-pmic", },
diff --git a/drivers/regulator/pbias-regulator.c b/drivers/regulator/pbias-regulator.c
index 6d38be3d970..6d02d68dfb4 100644
--- a/drivers/regulator/pbias-regulator.c
+++ b/drivers/regulator/pbias-regulator.c
@@ -49,33 +49,13 @@ static const unsigned int pbias_volt_table[] = {
 	3000000
 };
 
-static int pbias_regulator_enable(struct regulator_dev *rdev)
-{
-	struct pbias_regulator_data *data = rdev_get_drvdata(rdev);
-	const struct pbias_reg_info *info = data->info;
-
-	return regmap_update_bits(data->syscon, rdev->desc->enable_reg,
-				  info->enable_mask, info->enable);
-}
-
-static int pbias_regulator_is_enable(struct regulator_dev *rdev)
-{
-	struct pbias_regulator_data *data = rdev_get_drvdata(rdev);
-	const struct pbias_reg_info *info = data->info;
-	int value;
-
-	regmap_read(data->syscon, rdev->desc->enable_reg, &value);
-
-	return (value & info->enable_mask) == info->enable;
-}
-
 static struct regulator_ops pbias_regulator_voltage_ops = {
 	.list_voltage = regulator_list_voltage_table,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
 	.set_voltage_sel = regulator_set_voltage_sel_regmap,
-	.enable = pbias_regulator_enable,
+	.enable = regulator_enable_regmap,
 	.disable = regulator_disable_regmap,
-	.is_enabled = pbias_regulator_is_enable,
+	.is_enabled = regulator_is_enabled_regmap,
 };
 
 static const struct pbias_reg_info pbias_mmc_omap2430 = {
@@ -142,10 +122,8 @@ static int pbias_regulator_probe(struct platform_device *pdev)
 
 	drvdata = devm_kzalloc(&pdev->dev, sizeof(struct pbias_regulator_data)
 			       * count, GFP_KERNEL);
-	if (drvdata == NULL) {
-		dev_err(&pdev->dev, "Failed to allocate device data\n");
+	if (!drvdata)
 		return -ENOMEM;
-	}
 
 	syscon = syscon_regmap_lookup_by_phandle(np, "syscon");
 	if (IS_ERR(syscon))
@@ -180,6 +158,7 @@ static int pbias_regulator_probe(struct platform_device *pdev)
 		drvdata[data_idx].desc.vsel_mask = info->vmode;
 		drvdata[data_idx].desc.enable_reg = res->start;
 		drvdata[data_idx].desc.enable_mask = info->enable_mask;
+		drvdata[data_idx].desc.enable_val = info->enable;
 
 		cfg.init_data = pbias_matches[idx].init_data;
 		cfg.driver_data = &drvdata[data_idx];
diff --git a/drivers/regulator/pfuze100-regulator.c b/drivers/regulator/pfuze100-regulator.c
index 67e678c4301..c879dff597e 100644
--- a/drivers/regulator/pfuze100-regulator.c
+++ b/drivers/regulator/pfuze100-regulator.c
@@ -125,6 +125,9 @@ static struct regulator_ops pfuze100_ldo_regulator_ops = {
 };
 
 static struct regulator_ops pfuze100_fixed_regulator_ops = {
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
 	.list_voltage = regulator_list_voltage_linear,
 };
 
@@ -137,6 +140,8 @@ static struct regulator_ops pfuze100_sw_regulator_ops = {
 };
 
 static struct regulator_ops pfuze100_swb_regulator_ops = {
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
 	.list_voltage = regulator_list_voltage_table,
 	.map_voltage = regulator_map_voltage_ascend,
 	.set_voltage_sel = regulator_set_voltage_sel_regmap,
@@ -189,6 +194,8 @@ static struct regulator_ops pfuze100_swb_regulator_ops = {
 			.volt_table = voltages,	\
 			.vsel_reg = (base),	\
 			.vsel_mask = (mask),	\
+			.enable_reg = (base),	\
+			.enable_mask = 0x48,	\
 		},	\
 	}
 
@@ -502,6 +509,7 @@ static int pfuze100_regulator_probe(struct i2c_client *client,
 		config.init_data = init_data;
 		config.driver_data = pfuze_chip;
 		config.of_node = match_of_node(i);
+		config.ena_gpio = -EINVAL;
 
 		pfuze_chip->regulators[i] =
 			devm_regulator_register(&client->dev, desc, &config);
diff --git a/drivers/regulator/s2mpa01.c b/drivers/regulator/s2mpa01.c
index f19a30f0fb4..ee83b487642 100644
--- a/drivers/regulator/s2mpa01.c
+++ b/drivers/regulator/s2mpa01.c
@@ -61,7 +61,7 @@ static int s2mpa01_regulator_set_voltage_time_sel(struct regulator_dev *rdev,
 	unsigned int ramp_delay = 0;
 	int old_volt, new_volt;
 
-	switch (rdev->desc->id) {
+	switch (rdev_get_id(rdev)) {
 	case S2MPA01_BUCK2:
 	case S2MPA01_BUCK4:
 		ramp_delay = s2mpa01->ramp_delay24;
@@ -102,7 +102,7 @@ static int s2mpa01_set_ramp_delay(struct regulator_dev *rdev, int ramp_delay)
 	unsigned int ramp_enable = 1, enable_shift = 0;
 	int ret;
 
-	switch (rdev->desc->id) {
+	switch (rdev_get_id(rdev)) {
 	case S2MPA01_BUCK1:
 		enable_shift = S2MPA01_BUCK1_RAMP_EN_SHIFT;
 		if (!ramp_delay) {
@@ -116,7 +116,6 @@ static int s2mpa01_set_ramp_delay(struct regulator_dev *rdev, int ramp_delay)
 			ramp_delay = s2mpa01->ramp_delay16;
 
 		ramp_shift = S2MPA01_BUCK16_RAMP_SHIFT;
-		ramp_reg = S2MPA01_REG_RAMP1;
 		break;
 	case S2MPA01_BUCK2:
 		enable_shift = S2MPA01_BUCK2_RAMP_EN_SHIFT;
@@ -192,11 +191,15 @@ static int s2mpa01_set_ramp_delay(struct regulator_dev *rdev, int ramp_delay)
 	if (!ramp_enable)
 		goto ramp_disable;
 
-	ret = regmap_update_bits(rdev->regmap, S2MPA01_REG_RAMP1,
-				 1 << enable_shift, 1 << enable_shift);
-	if (ret) {
-		dev_err(&rdev->dev, "failed to enable ramp rate\n");
-		return ret;
+	/* Ramp delay can be enabled/disabled only for buck[1234] */
+	if (rdev_get_id(rdev) >= S2MPA01_BUCK1 &&
+			rdev_get_id(rdev) <= S2MPA01_BUCK4) {
+		ret = regmap_update_bits(rdev->regmap, S2MPA01_REG_RAMP1,
+					 1 << enable_shift, 1 << enable_shift);
+		if (ret) {
+			dev_err(&rdev->dev, "failed to enable ramp rate\n");
+			return ret;
+		}
 	}
 
 	ramp_val = get_ramp_delay(ramp_delay);
diff --git a/drivers/regulator/s2mps11.c b/drivers/regulator/s2mps11.c
index e713c162fbd..02e2fb2fca6 100644
--- a/drivers/regulator/s2mps11.c
+++ b/drivers/regulator/s2mps11.c
@@ -27,6 +27,7 @@
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
 #include <linux/regulator/of_regulator.h>
+#include <linux/of_gpio.h>
 #include <linux/mfd/samsung/core.h>
 #include <linux/mfd/samsung/s2mps11.h>
 #include <linux/mfd/samsung/s2mps14.h>
@@ -44,6 +45,8 @@ struct s2mps11_info {
 	 * was enabled.
 	 */
 	unsigned int s2mps14_suspend_state:30;
+	/* Array of size rdev_num with GPIO-s for external sleep control */
+	int *ext_control_gpio;
 };
 
 static int get_ramp_delay(int ramp_delay)
@@ -202,11 +205,16 @@ static int s2mps11_set_ramp_delay(struct regulator_dev *rdev, int ramp_delay)
 	if (!ramp_enable)
 		goto ramp_disable;
 
-	ret = regmap_update_bits(rdev->regmap, S2MPS11_REG_RAMP,
-				 1 << enable_shift, 1 << enable_shift);
-	if (ret) {
-		dev_err(&rdev->dev, "failed to enable ramp rate\n");
-		return ret;
+	/* Ramp delay can be enabled/disabled only for buck[2346] */
+	if ((rdev_get_id(rdev) >= S2MPS11_BUCK2 &&
+			rdev_get_id(rdev) <= S2MPS11_BUCK4) ||
+			rdev_get_id(rdev) == S2MPS11_BUCK6)  {
+		ret = regmap_update_bits(rdev->regmap, S2MPS11_REG_RAMP,
+					 1 << enable_shift, 1 << enable_shift);
+		if (ret) {
+			dev_err(&rdev->dev, "failed to enable ramp rate\n");
+			return ret;
+		}
 	}
 
 	ramp_val = get_ramp_delay(ramp_delay);
@@ -409,6 +417,8 @@ static int s2mps14_regulator_enable(struct regulator_dev *rdev)
 
 	if (s2mps11->s2mps14_suspend_state & (1 << rdev_get_id(rdev)))
 		val = S2MPS14_ENABLE_SUSPEND;
+	else if (gpio_is_valid(s2mps11->ext_control_gpio[rdev_get_id(rdev)]))
+		val = S2MPS14_ENABLE_EXT_CONTROL;
 	else
 		val = rdev->desc->enable_mask;
 
@@ -565,12 +575,61 @@ static const struct regulator_desc s2mps14_regulators[] = {
 	regulator_desc_s2mps14_buck1235(5),
 };
 
+static int s2mps14_pmic_enable_ext_control(struct s2mps11_info *s2mps11,
+		struct regulator_dev *rdev)
+{
+	return regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
+			rdev->desc->enable_mask, S2MPS14_ENABLE_EXT_CONTROL);
+}
+
+static void s2mps14_pmic_dt_parse_ext_control_gpio(struct platform_device *pdev,
+		struct of_regulator_match *rdata, struct s2mps11_info *s2mps11)
+{
+	int *gpio = s2mps11->ext_control_gpio;
+	unsigned int i;
+	unsigned int valid_regulators[3] = { S2MPS14_LDO10, S2MPS14_LDO11,
+		S2MPS14_LDO12 };
+
+	for (i = 0; i < ARRAY_SIZE(valid_regulators); i++) {
+		unsigned int reg = valid_regulators[i];
+
+		if (!rdata[reg].init_data || !rdata[reg].of_node)
+			continue;
+
+		gpio[reg] = of_get_named_gpio(rdata[reg].of_node,
+				"samsung,ext-control-gpios", 0);
+		if (gpio_is_valid(gpio[reg]))
+			dev_dbg(&pdev->dev, "Using GPIO %d for ext-control over %d/%s\n",
+					gpio[reg], reg, rdata[reg].name);
+	}
+}
+
+static int s2mps11_pmic_dt_parse(struct platform_device *pdev,
+		struct of_regulator_match *rdata, struct s2mps11_info *s2mps11,
+		enum sec_device_type dev_type)
+{
+	struct device_node *reg_np;
+
+	reg_np = of_get_child_by_name(pdev->dev.parent->of_node, "regulators");
+	if (!reg_np) {
+		dev_err(&pdev->dev, "could not find regulators sub-node\n");
+		return -EINVAL;
+	}
+
+	of_regulator_match(&pdev->dev, reg_np, rdata, s2mps11->rdev_num);
+	if (dev_type == S2MPS14X)
+		s2mps14_pmic_dt_parse_ext_control_gpio(pdev, rdata, s2mps11);
+
+	of_node_put(reg_np);
+
+	return 0;
+}
+
 static int s2mps11_pmic_probe(struct platform_device *pdev)
 {
 	struct sec_pmic_dev *iodev = dev_get_drvdata(pdev->dev.parent);
-	struct sec_platform_data *pdata = iodev->pdata;
+	struct sec_platform_data *pdata = NULL;
 	struct of_regulator_match *rdata = NULL;
-	struct device_node *reg_np = NULL;
 	struct regulator_config config = { };
 	struct s2mps11_info *s2mps11;
 	int i, ret = 0;
@@ -597,8 +656,21 @@ static int s2mps11_pmic_probe(struct platform_device *pdev)
 		return -EINVAL;
 	};
 
+	s2mps11->ext_control_gpio = devm_kzalloc(&pdev->dev,
+			sizeof(*s2mps11->ext_control_gpio) * s2mps11->rdev_num,
+			GFP_KERNEL);
+	if (!s2mps11->ext_control_gpio)
+		return -ENOMEM;
+	/*
+	 * 0 is a valid GPIO so initialize all GPIO-s to negative value
+	 * to indicate that external control won't be used for this regulator.
+	 */
+	for (i = 0; i < s2mps11->rdev_num; i++)
+		s2mps11->ext_control_gpio[i] = -EINVAL;
+
 	if (!iodev->dev->of_node) {
-		if (pdata) {
+		if (iodev->pdata) {
+			pdata = iodev->pdata;
 			goto common_reg;
 		} else {
 			dev_err(pdev->dev.parent,
@@ -614,15 +686,9 @@ static int s2mps11_pmic_probe(struct platform_device *pdev)
 	for (i = 0; i < s2mps11->rdev_num; i++)
 		rdata[i].name = regulators[i].name;
 
-	reg_np = of_get_child_by_name(iodev->dev->of_node, "regulators");
-	if (!reg_np) {
-		dev_err(&pdev->dev, "could not find regulators sub-node\n");
-		ret = -EINVAL;
+	ret = s2mps11_pmic_dt_parse(pdev, rdata, s2mps11, dev_type);
+	if (ret)
 		goto out;
-	}
-
-	of_regulator_match(&pdev->dev, reg_np, rdata, s2mps11->rdev_num);
-	of_node_put(reg_np);
 
 common_reg:
 	platform_set_drvdata(pdev, s2mps11);
@@ -630,16 +696,18 @@ common_reg:
 	config.dev = &pdev->dev;
 	config.regmap = iodev->regmap_pmic;
 	config.driver_data = s2mps11;
+	config.ena_gpio_flags = GPIOF_OUT_INIT_HIGH;
 	for (i = 0; i < s2mps11->rdev_num; i++) {
 		struct regulator_dev *regulator;
 
-		if (!reg_np) {
+		if (pdata) {
 			config.init_data = pdata->regulators[i].initdata;
 			config.of_node = pdata->regulators[i].reg_node;
 		} else {
 			config.init_data = rdata[i].init_data;
 			config.of_node = rdata[i].of_node;
 		}
+		config.ena_gpio = s2mps11->ext_control_gpio[i];
 
 		regulator = devm_regulator_register(&pdev->dev,
 						&regulators[i], &config);
@@ -649,6 +717,17 @@ common_reg:
 				i);
 			goto out;
 		}
+
+		if (gpio_is_valid(s2mps11->ext_control_gpio[i])) {
+			ret = s2mps14_pmic_enable_ext_control(s2mps11,
+					regulator);
+			if (ret < 0) {
+				dev_err(&pdev->dev,
+						"failed to enable GPIO control over %s: %d\n",
+						regulator->desc->name, ret);
+				goto out;
+			}
+		}
 	}
 
 out:
diff --git a/drivers/regulator/s5m8767.c b/drivers/regulator/s5m8767.c
index 92f19a005dc..c79af943a5c 100644
--- a/drivers/regulator/s5m8767.c
+++ b/drivers/regulator/s5m8767.c
@@ -28,7 +28,6 @@ struct s5m8767_info {
 	struct device *dev;
 	struct sec_pmic_dev *iodev;
 	int num_regulators;
-	struct regulator_dev **rdev;
 	struct sec_opmode_data *opmode;
 
 	int ramp_delay;
@@ -529,16 +528,6 @@ static int s5m8767_pmic_dt_parse_ds_gpio(struct sec_pmic_dev *iodev,
 	return 0;
 }
 
-static void s5m8767_pmic_dt_parse_ext_control_gpio(struct sec_pmic_dev *iodev,
-		struct sec_regulator_data *rdata,
-		struct device_node *reg_np)
-{
-	rdata->ext_control_gpio = of_get_named_gpio(reg_np,
-			"s5m8767,pmic-ext-control-gpios", 0);
-	if (!gpio_is_valid(rdata->ext_control_gpio))
-		rdata->ext_control_gpio = 0;
-}
-
 static int s5m8767_pmic_dt_parse_pdata(struct platform_device *pdev,
 					struct sec_platform_data *pdata)
 {
@@ -587,7 +576,8 @@ static int s5m8767_pmic_dt_parse_pdata(struct platform_device *pdev,
 			continue;
 		}
 
-		s5m8767_pmic_dt_parse_ext_control_gpio(iodev, rdata, reg_np);
+		rdata->ext_control_gpio = of_get_named_gpio(reg_np,
+			"s5m8767,pmic-ext-control-gpios", 0);
 
 		rdata->id = i;
 		rdata->initdata = of_get_regulator_init_data(
@@ -695,7 +685,6 @@ static int s5m8767_pmic_probe(struct platform_device *pdev)
 	struct sec_pmic_dev *iodev = dev_get_drvdata(pdev->dev.parent);
 	struct sec_platform_data *pdata = iodev->pdata;
 	struct regulator_config config = { };
-	struct regulator_dev **rdev;
 	struct s5m8767_info *s5m8767;
 	int i, ret, size, buck_init;
 
@@ -737,11 +726,7 @@ static int s5m8767_pmic_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	size = sizeof(struct regulator_dev *) * (S5M8767_REG_MAX - 2);
-	s5m8767->rdev = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
-	if (!s5m8767->rdev)
-		return -ENOMEM;
 
-	rdev = s5m8767->rdev;
 	s5m8767->dev = &pdev->dev;
 	s5m8767->iodev = iodev;
 	s5m8767->num_regulators = pdata->num_regulators;
@@ -938,6 +923,7 @@ static int s5m8767_pmic_probe(struct platform_device *pdev)
 		const struct sec_voltage_desc *desc;
 		int id = pdata->regulators[i].id;
 		int enable_reg, enable_val;
+		struct regulator_dev *rdev;
 
 		desc = reg_voltage_map[id];
 		if (desc) {
@@ -964,26 +950,27 @@ static int s5m8767_pmic_probe(struct platform_device *pdev)
 		config.driver_data = s5m8767;
 		config.regmap = iodev->regmap_pmic;
 		config.of_node = pdata->regulators[i].reg_node;
-		config.ena_gpio = config.ena_gpio_flags = 0;
-		if (pdata->regulators[i].ext_control_gpio)
+		config.ena_gpio = -EINVAL;
+		config.ena_gpio_flags = 0;
+		if (gpio_is_valid(pdata->regulators[i].ext_control_gpio))
 			s5m8767_regulator_config_ext_control(s5m8767,
 					&pdata->regulators[i], &config);
 
-		rdev[i] = devm_regulator_register(&pdev->dev, &regulators[id],
+		rdev = devm_regulator_register(&pdev->dev, &regulators[id],
 						  &config);
-		if (IS_ERR(rdev[i])) {
-			ret = PTR_ERR(rdev[i]);
+		if (IS_ERR(rdev)) {
+			ret = PTR_ERR(rdev);
 			dev_err(s5m8767->dev, "regulator init failed for %d\n",
 					id);
 			return ret;
 		}
 
-		if (pdata->regulators[i].ext_control_gpio) {
-			ret = s5m8767_enable_ext_control(s5m8767, rdev[i]);
+		if (gpio_is_valid(pdata->regulators[i].ext_control_gpio)) {
+			ret = s5m8767_enable_ext_control(s5m8767, rdev);
 			if (ret < 0) {
 				dev_err(s5m8767->dev,
 						"failed to enable gpio control over %s: %d\n",
-						rdev[i]->desc->name, ret);
+						rdev->desc->name, ret);
 				return ret;
 			}
 		}
diff --git a/drivers/regulator/st-pwm.c b/drivers/regulator/st-pwm.c
index e367af1c5f9..5ea78df449f 100644
--- a/drivers/regulator/st-pwm.c
+++ b/drivers/regulator/st-pwm.c
@@ -118,7 +118,7 @@ static const struct st_pwm_regulator_pdata b2105_info = {
 	.duty_cycle_table = b2105_duty_cycle_table,
 };
 
-static struct of_device_id st_pwm_of_match[] = {
+static const struct of_device_id st_pwm_of_match[] = {
 	{ .compatible = "st,b2105-pwm-regulator", .data = &b2105_info, },
 	{ },
 };
diff --git a/drivers/regulator/tps65090-regulator.c b/drivers/regulator/tps65090-regulator.c
index 2e92ef68574..2064b3fd45f 100644
--- a/drivers/regulator/tps65090-regulator.c
+++ b/drivers/regulator/tps65090-regulator.c
@@ -17,6 +17,7 @@
  */
 
 #include <linux/module.h>
+#include <linux/delay.h>
 #include <linux/init.h>
 #include <linux/gpio.h>
 #include <linux/of_gpio.h>
@@ -28,49 +29,216 @@
 #include <linux/regulator/of_regulator.h>
 #include <linux/mfd/tps65090.h>
 
+#define MAX_CTRL_READ_TRIES	5
+#define MAX_FET_ENABLE_TRIES	1000
+
+#define CTRL_EN_BIT		0 /* Regulator enable bit, active high */
+#define CTRL_WT_BIT		2 /* Regulator wait time 0 bit */
+#define CTRL_PG_BIT		4 /* Regulator power good bit, 1=good */
+#define CTRL_TO_BIT		7 /* Regulator timeout bit, 1=wait */
+
+#define MAX_OVERCURRENT_WAIT	3 /* Overcurrent wait must be <= this */
+
+/**
+ * struct tps65090_regulator - Per-regulator data for a tps65090 regulator
+ *
+ * @dev: Pointer to our device.
+ * @desc: The struct regulator_desc for the regulator.
+ * @rdev: The struct regulator_dev for the regulator.
+ * @overcurrent_wait_valid: True if overcurrent_wait is valid.
+ * @overcurrent_wait: For FETs, the value to put in the WTFET bitfield.
+ */
+
 struct tps65090_regulator {
 	struct device		*dev;
 	struct regulator_desc	*desc;
 	struct regulator_dev	*rdev;
+	bool			overcurrent_wait_valid;
+	int			overcurrent_wait;
 };
 
 static struct regulator_ops tps65090_ext_control_ops = {
 };
 
-static struct regulator_ops tps65090_reg_contol_ops = {
+/**
+ * tps65090_reg_set_overcurrent_wait - Setup overcurrent wait
+ *
+ * This will set the overcurrent wait time based on what's in the regulator
+ * info.
+ *
+ * @ri:		Overall regulator data
+ * @rdev:	Regulator device
+ *
+ * Return: 0 if no error, non-zero if there was an error writing the register.
+ */
+static int tps65090_reg_set_overcurrent_wait(struct tps65090_regulator *ri,
+					     struct regulator_dev *rdev)
+{
+	int ret;
+
+	ret = regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
+				 MAX_OVERCURRENT_WAIT << CTRL_WT_BIT,
+				 ri->overcurrent_wait << CTRL_WT_BIT);
+	if (ret) {
+		dev_err(&rdev->dev, "Error updating overcurrent wait %#x\n",
+			rdev->desc->enable_reg);
+	}
+
+	return ret;
+}
+
+/**
+ * tps65090_try_enable_fet - Try to enable a FET
+ *
+ * @rdev:	Regulator device
+ *
+ * Return: 0 if ok, -ENOTRECOVERABLE if the FET power good bit did not get
+ * set, or some other -ve value if another error occurred (e.g. i2c error)
+ */
+static int tps65090_try_enable_fet(struct regulator_dev *rdev)
+{
+	unsigned int control;
+	int ret, i;
+
+	ret = regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
+				 rdev->desc->enable_mask,
+				 rdev->desc->enable_mask);
+	if (ret < 0) {
+		dev_err(&rdev->dev, "Error in updating reg %#x\n",
+			rdev->desc->enable_reg);
+		return ret;
+	}
+
+	for (i = 0; i < MAX_CTRL_READ_TRIES; i++) {
+		ret = regmap_read(rdev->regmap, rdev->desc->enable_reg,
+				  &control);
+		if (ret < 0)
+			return ret;
+
+		if (!(control & BIT(CTRL_TO_BIT)))
+			break;
+
+		usleep_range(1000, 1500);
+	}
+	if (!(control & BIT(CTRL_PG_BIT)))
+		return -ENOTRECOVERABLE;
+
+	return 0;
+}
+
+/**
+ * tps65090_fet_enable - Enable a FET, trying a few times if it fails
+ *
+ * Some versions of the tps65090 have issues when turning on the FETs.
+ * This function goes through several steps to ensure the best chance of the
+ * FET going on.  Specifically:
+ * - We'll make sure that we bump the "overcurrent wait" to the maximum, which
+ *   increases the chances that we'll turn on properly.
+ * - We'll retry turning the FET on multiple times (turning off in between).
+ *
+ * @rdev:	Regulator device
+ *
+ * Return: 0 if ok, non-zero if it fails.
+ */
+static int tps65090_fet_enable(struct regulator_dev *rdev)
+{
+	int ret, tries;
+
+	/*
+	 * Try enabling multiple times until we succeed since sometimes the
+	 * first try times out.
+	 */
+	tries = 0;
+	while (true) {
+		ret = tps65090_try_enable_fet(rdev);
+		if (!ret)
+			break;
+		if (ret != -ENOTRECOVERABLE || tries == MAX_FET_ENABLE_TRIES)
+			goto err;
+
+		/* Try turning the FET off (and then on again) */
+		ret = regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
+					 rdev->desc->enable_mask, 0);
+		if (ret)
+			goto err;
+
+		tries++;
+	}
+
+	if (tries)
+		dev_warn(&rdev->dev, "reg %#x enable ok after %d tries\n",
+			 rdev->desc->enable_reg, tries);
+
+	return 0;
+err:
+	dev_warn(&rdev->dev, "reg %#x enable failed\n", rdev->desc->enable_reg);
+	WARN_ON(1);
+
+	return ret;
+}
+
+static struct regulator_ops tps65090_reg_control_ops = {
 	.enable		= regulator_enable_regmap,
 	.disable	= regulator_disable_regmap,
 	.is_enabled	= regulator_is_enabled_regmap,
 };
 
+static struct regulator_ops tps65090_fet_control_ops = {
+	.enable		= tps65090_fet_enable,
+	.disable	= regulator_disable_regmap,
+	.is_enabled	= regulator_is_enabled_regmap,
+};
+
 static struct regulator_ops tps65090_ldo_ops = {
 };
 
-#define tps65090_REG_DESC(_id, _sname, _en_reg, _ops)	\
+#define tps65090_REG_DESC(_id, _sname, _en_reg, _en_bits, _ops)	\
 {							\
 	.name = "TPS65090_RAILS"#_id,			\
 	.supply_name = _sname,				\
 	.id = TPS65090_REGULATOR_##_id,			\
 	.ops = &_ops,					\
 	.enable_reg = _en_reg,				\
-	.enable_mask = BIT(0),				\
+	.enable_val = _en_bits,				\
+	.enable_mask = _en_bits,			\
 	.type = REGULATOR_VOLTAGE,			\
 	.owner = THIS_MODULE,				\
 }
 
 static struct regulator_desc tps65090_regulator_desc[] = {
-	tps65090_REG_DESC(DCDC1, "vsys1",   0x0C, tps65090_reg_contol_ops),
-	tps65090_REG_DESC(DCDC2, "vsys2",   0x0D, tps65090_reg_contol_ops),
-	tps65090_REG_DESC(DCDC3, "vsys3",   0x0E, tps65090_reg_contol_ops),
-	tps65090_REG_DESC(FET1,  "infet1",  0x0F, tps65090_reg_contol_ops),
-	tps65090_REG_DESC(FET2,  "infet2",  0x10, tps65090_reg_contol_ops),
-	tps65090_REG_DESC(FET3,  "infet3",  0x11, tps65090_reg_contol_ops),
-	tps65090_REG_DESC(FET4,  "infet4",  0x12, tps65090_reg_contol_ops),
-	tps65090_REG_DESC(FET5,  "infet5",  0x13, tps65090_reg_contol_ops),
-	tps65090_REG_DESC(FET6,  "infet6",  0x14, tps65090_reg_contol_ops),
-	tps65090_REG_DESC(FET7,  "infet7",  0x15, tps65090_reg_contol_ops),
-	tps65090_REG_DESC(LDO1,  "vsys-l1", 0,    tps65090_ldo_ops),
-	tps65090_REG_DESC(LDO2,  "vsys-l2", 0,    tps65090_ldo_ops),
+	tps65090_REG_DESC(DCDC1, "vsys1",   0x0C, BIT(CTRL_EN_BIT),
+			  tps65090_reg_control_ops),
+	tps65090_REG_DESC(DCDC2, "vsys2",   0x0D, BIT(CTRL_EN_BIT),
+			  tps65090_reg_control_ops),
+	tps65090_REG_DESC(DCDC3, "vsys3",   0x0E, BIT(CTRL_EN_BIT),
+			  tps65090_reg_control_ops),
+
+	tps65090_REG_DESC(FET1,  "infet1",  0x0F,
+			  BIT(CTRL_EN_BIT) | BIT(CTRL_PG_BIT),
+			  tps65090_fet_control_ops),
+	tps65090_REG_DESC(FET2,  "infet2",  0x10,
+			  BIT(CTRL_EN_BIT) | BIT(CTRL_PG_BIT),
+			  tps65090_fet_control_ops),
+	tps65090_REG_DESC(FET3,  "infet3",  0x11,
+			  BIT(CTRL_EN_BIT) | BIT(CTRL_PG_BIT),
+			  tps65090_fet_control_ops),
+	tps65090_REG_DESC(FET4,  "infet4",  0x12,
+			  BIT(CTRL_EN_BIT) | BIT(CTRL_PG_BIT),
+			  tps65090_fet_control_ops),
+	tps65090_REG_DESC(FET5,  "infet5",  0x13,
+			  BIT(CTRL_EN_BIT) | BIT(CTRL_PG_BIT),
+			  tps65090_fet_control_ops),
+	tps65090_REG_DESC(FET6,  "infet6",  0x14,
+			  BIT(CTRL_EN_BIT) | BIT(CTRL_PG_BIT),
+			  tps65090_fet_control_ops),
+	tps65090_REG_DESC(FET7,  "infet7",  0x15,
+			  BIT(CTRL_EN_BIT) | BIT(CTRL_PG_BIT),
+			  tps65090_fet_control_ops),
+
+	tps65090_REG_DESC(LDO1,  "vsys-l1", 0, 0,
+			  tps65090_ldo_ops),
+	tps65090_REG_DESC(LDO2,  "vsys-l2", 0, 0,
+			  tps65090_ldo_ops),
 };
 
 static inline bool is_dcdc(int id)
@@ -209,6 +377,11 @@ static struct tps65090_platform_data *tps65090_parse_dt_reg_data(
 			rpdata->gpio = of_get_named_gpio(np,
 					"dcdc-ext-control-gpios", 0);
 
+		if (of_property_read_u32(tps65090_matches[idx].of_node,
+					 "ti,overcurrent-wait",
+					 &rpdata->overcurrent_wait) == 0)
+			rpdata->overcurrent_wait_valid = true;
+
 		tps65090_pdata->reg_pdata[idx] = rpdata;
 	}
 	return tps65090_pdata;
@@ -258,6 +431,11 @@ static int tps65090_regulator_probe(struct platform_device *pdev)
 		ri = &pmic[num];
 		ri->dev = &pdev->dev;
 		ri->desc = &tps65090_regulator_desc[num];
+		if (tps_pdata) {
+			ri->overcurrent_wait_valid =
+				tps_pdata->overcurrent_wait_valid;
+			ri->overcurrent_wait = tps_pdata->overcurrent_wait;
+		}
 
 		/*
 		 * TPS5090 DCDC support the control from external digital input.
@@ -299,6 +477,12 @@ static int tps65090_regulator_probe(struct platform_device *pdev)
 		}
 		ri->rdev = rdev;
 
+		if (ri->overcurrent_wait_valid) {
+			ret = tps65090_reg_set_overcurrent_wait(ri, rdev);
+			if (ret < 0)
+				return ret;
+		}
+
 		/* Enable external control if it is require */
 		if (tps_pdata && is_dcdc(num) && tps_pdata->reg_init_data &&
 				tps_pdata->enable_ext_control) {
diff --git a/drivers/regulator/tps65217-regulator.c b/drivers/regulator/tps65217-regulator.c
index 10b78d2b766..f7ed20a5a8b 100644
--- a/drivers/regulator/tps65217-regulator.c
+++ b/drivers/regulator/tps65217-regulator.c
@@ -134,6 +134,7 @@ static struct regulator_ops tps65217_pmic_ldo1_ops = {
 	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
 	.set_voltage_sel	= tps65217_pmic_set_voltage_sel,
 	.list_voltage		= regulator_list_voltage_table,
+	.map_voltage		= regulator_map_voltage_ascend,
 };
 
 static const struct regulator_desc regulators[] = {
@@ -257,9 +258,6 @@ static int tps65217_regulator_probe(struct platform_device *pdev)
 				pdev->name);
 			return PTR_ERR(rdev);
 		}
-
-		/* Save regulator for cleanup */
-		tps->rdev[i] = rdev;
 	}
 	return 0;
 }
diff --git a/drivers/regulator/tps65218-regulator.c b/drivers/regulator/tps65218-regulator.c
index cec72fa71d1..69b4b775041 100644
--- a/drivers/regulator/tps65218-regulator.c
+++ b/drivers/regulator/tps65218-regulator.c
@@ -27,12 +27,10 @@
 #include <linux/regulator/machine.h>
 #include <linux/mfd/tps65218.h>
 
-static unsigned int tps65218_ramp_delay = 4000;
-
 enum tps65218_regulators { DCDC1, DCDC2, DCDC3, DCDC4, DCDC5, DCDC6, LDO1 };
 
 #define TPS65218_REGULATOR(_name, _id, _ops, _n, _vr, _vm, _er, _em, _t, \
-			    _lr, _nlr)				\
+			    _lr, _nlr, _delay)			\
 	{							\
 		.name			= _name,		\
 		.id			= _id,			\
@@ -47,6 +45,7 @@ enum tps65218_regulators { DCDC1, DCDC2, DCDC3, DCDC4, DCDC5, DCDC6, LDO1 };
 		.volt_table		= _t,			\
 		.linear_ranges		= _lr,			\
 		.n_linear_ranges	= _nlr,			\
+		.ramp_delay		= _delay,		\
 	}							\
 
 #define TPS65218_INFO(_id, _nm, _min, _max)	\
@@ -152,22 +151,6 @@ static int tps65218_pmic_disable(struct regulator_dev *dev)
 				   dev->desc->enable_mask, TPS65218_PROTECT_L1);
 }
 
-static int tps65218_set_voltage_time_sel(struct regulator_dev *rdev,
-	unsigned int old_selector, unsigned int new_selector)
-{
-	int old_uv, new_uv;
-
-	old_uv = regulator_list_voltage_linear_range(rdev, old_selector);
-	if (old_uv < 0)
-		return old_uv;
-
-	new_uv = regulator_list_voltage_linear_range(rdev, new_selector);
-	if (new_uv < 0)
-		return new_uv;
-
-	return DIV_ROUND_UP(abs(old_uv - new_uv), tps65218_ramp_delay);
-}
-
 /* Operations permitted on DCDC1, DCDC2 */
 static struct regulator_ops tps65218_dcdc12_ops = {
 	.is_enabled		= regulator_is_enabled_regmap,
@@ -177,7 +160,7 @@ static struct regulator_ops tps65218_dcdc12_ops = {
 	.set_voltage_sel	= tps65218_pmic_set_voltage_sel,
 	.list_voltage		= regulator_list_voltage_linear_range,
 	.map_voltage		= regulator_map_voltage_linear_range,
-	.set_voltage_time_sel	= tps65218_set_voltage_time_sel,
+	.set_voltage_time_sel	= regulator_set_voltage_time_sel,
 };
 
 /* Operations permitted on DCDC3, DCDC4 and LDO1 */
@@ -203,33 +186,33 @@ static const struct regulator_desc regulators[] = {
 			   TPS65218_REG_CONTROL_DCDC1,
 			   TPS65218_CONTROL_DCDC1_MASK,
 			   TPS65218_REG_ENABLE1, TPS65218_ENABLE1_DC1_EN, NULL,
-			   dcdc1_dcdc2_ranges, 2),
+			   dcdc1_dcdc2_ranges, 2, 4000),
 	TPS65218_REGULATOR("DCDC2", TPS65218_DCDC_2, tps65218_dcdc12_ops, 64,
 			   TPS65218_REG_CONTROL_DCDC2,
 			   TPS65218_CONTROL_DCDC2_MASK,
 			   TPS65218_REG_ENABLE1, TPS65218_ENABLE1_DC2_EN, NULL,
-			   dcdc1_dcdc2_ranges, 2),
+			   dcdc1_dcdc2_ranges, 2, 4000),
 	TPS65218_REGULATOR("DCDC3", TPS65218_DCDC_3, tps65218_ldo1_dcdc34_ops,
 			   64, TPS65218_REG_CONTROL_DCDC3,
 			   TPS65218_CONTROL_DCDC3_MASK, TPS65218_REG_ENABLE1,
 			   TPS65218_ENABLE1_DC3_EN, NULL,
-			   ldo1_dcdc3_ranges, 2),
+			   ldo1_dcdc3_ranges, 2, 0),
 	TPS65218_REGULATOR("DCDC4", TPS65218_DCDC_4, tps65218_ldo1_dcdc34_ops,
 			   53, TPS65218_REG_CONTROL_DCDC4,
 			   TPS65218_CONTROL_DCDC4_MASK,
 			   TPS65218_REG_ENABLE1, TPS65218_ENABLE1_DC4_EN, NULL,
-			   dcdc4_ranges, 2),
+			   dcdc4_ranges, 2, 0),
 	TPS65218_REGULATOR("DCDC5", TPS65218_DCDC_5, tps65218_dcdc56_pmic_ops,
 			   1, -1, -1, TPS65218_REG_ENABLE1,
-			   TPS65218_ENABLE1_DC5_EN, NULL, NULL, 0),
+			   TPS65218_ENABLE1_DC5_EN, NULL, NULL, 0, 0),
 	TPS65218_REGULATOR("DCDC6", TPS65218_DCDC_6, tps65218_dcdc56_pmic_ops,
 			   1, -1, -1, TPS65218_REG_ENABLE1,
-			   TPS65218_ENABLE1_DC6_EN, NULL, NULL, 0),
+			   TPS65218_ENABLE1_DC6_EN, NULL, NULL, 0, 0),
 	TPS65218_REGULATOR("LDO1", TPS65218_LDO_1, tps65218_ldo1_dcdc34_ops, 64,
 			   TPS65218_REG_CONTROL_DCDC4,
 			   TPS65218_CONTROL_LDO1_MASK, TPS65218_REG_ENABLE2,
 			   TPS65218_ENABLE2_LDO1_EN, NULL, ldo1_dcdc3_ranges,
-			   2),
+			   2, 0),
 };
 
 static int tps65218_regulator_probe(struct platform_device *pdev)
diff --git a/drivers/regulator/tps6586x-regulator.c b/drivers/regulator/tps6586x-regulator.c
index 32f38a63d94..0a3bb3aecd9 100644
--- a/drivers/regulator/tps6586x-regulator.c
+++ b/drivers/regulator/tps6586x-regulator.c
@@ -63,12 +63,7 @@ struct tps6586x_regulator {
 	int enable_reg[2];
 };
 
-static inline struct device *to_tps6586x_dev(struct regulator_dev *rdev)
-{
-	return rdev_get_dev(rdev)->parent;
-}
-
-static struct regulator_ops tps6586x_regulator_ops = {
+static struct regulator_ops tps6586x_rw_regulator_ops = {
 	.list_voltage = regulator_list_voltage_table,
 	.map_voltage = regulator_map_voltage_ascend,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
@@ -79,6 +74,16 @@ static struct regulator_ops tps6586x_regulator_ops = {
 	.disable = regulator_disable_regmap,
 };
 
+static struct regulator_ops tps6586x_ro_regulator_ops = {
+	.list_voltage = regulator_list_voltage_table,
+	.map_voltage = regulator_map_voltage_ascend,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+};
+
 static struct regulator_ops tps6586x_sys_regulator_ops = {
 };
 
@@ -106,6 +111,13 @@ static const unsigned int tps6586x_sm2_voltages[] = {
 	4200000, 4250000, 4300000, 4350000, 4400000, 4450000, 4500000, 4550000,
 };
 
+static int tps658640_sm2_voltages[] = {
+	2150000, 2200000, 2250000, 2300000, 2350000, 2400000, 2450000, 2500000,
+	2550000, 2600000, 2650000, 2700000, 2750000, 2800000, 2850000, 2900000,
+	2950000, 3000000, 3050000, 3100000, 3150000, 3200000, 3250000, 3300000,
+	3350000, 3400000, 3450000, 3500000, 3550000, 3600000, 3650000, 3700000,
+};
+
 static const unsigned int tps658643_sm2_voltages[] = {
 	1025000, 1050000, 1075000, 1100000, 1125000, 1150000, 1175000, 1200000,
 	1225000, 1250000, 1275000, 1300000, 1325000, 1350000, 1375000, 1400000,
@@ -120,12 +132,16 @@ static const unsigned int tps6586x_dvm_voltages[] = {
 	1325000, 1350000, 1375000, 1400000, 1425000, 1450000, 1475000, 1500000,
 };
 
-#define TPS6586X_REGULATOR(_id, _pin_name, vdata, vreg, shift, nbits,	\
+static int tps658640_rtc_voltages[] = {
+	2500000, 2850000, 3100000, 3300000,
+};
+
+#define TPS6586X_REGULATOR(_id, _ops, _pin_name, vdata, vreg, shift, nbits, \
 			   ereg0, ebit0, ereg1, ebit1, goreg, gobit)	\
 	.desc	= {							\
 		.supply_name = _pin_name,				\
 		.name	= "REG-" #_id,					\
-		.ops	= &tps6586x_regulator_ops,			\
+		.ops	= &tps6586x_## _ops ## _regulator_ops,		\
 		.type	= REGULATOR_VOLTAGE,				\
 		.id	= TPS6586X_ID_##_id,				\
 		.n_voltages = ARRAY_SIZE(vdata##_voltages),		\
@@ -146,14 +162,21 @@ static const unsigned int tps6586x_dvm_voltages[] = {
 #define TPS6586X_LDO(_id, _pname, vdata, vreg, shift, nbits,		\
 		     ereg0, ebit0, ereg1, ebit1)			\
 {									\
-	TPS6586X_REGULATOR(_id, _pname, vdata, vreg, shift, nbits,	\
+	TPS6586X_REGULATOR(_id, rw, _pname, vdata, vreg, shift, nbits,	\
+			   ereg0, ebit0, ereg1, ebit1, 0, 0)		\
+}
+
+#define TPS6586X_FIXED_LDO(_id, _pname, vdata, vreg, shift, nbits,	\
+			  ereg0, ebit0, ereg1, ebit1)			\
+{									\
+	TPS6586X_REGULATOR(_id, ro, _pname, vdata, vreg, shift, nbits,	\
 			   ereg0, ebit0, ereg1, ebit1, 0, 0)		\
 }
 
 #define TPS6586X_DVM(_id, _pname, vdata, vreg, shift, nbits,		\
 		     ereg0, ebit0, ereg1, ebit1, goreg, gobit)		\
 {									\
-	TPS6586X_REGULATOR(_id, _pname, vdata, vreg, shift, nbits,	\
+	TPS6586X_REGULATOR(_id, rw, _pname, vdata, vreg, shift, nbits,	\
 			   ereg0, ebit0, ereg1, ebit1, goreg, gobit)	\
 }
 
@@ -207,6 +230,26 @@ static struct tps6586x_regulator tps658623_regulator[] = {
 					END, 7),
 };
 
+static struct tps6586x_regulator tps658640_regulator[] = {
+	TPS6586X_LDO(LDO_3, "vinldo23", tps6586x_ldo0, SUPPLYV4, 0, 3,
+					ENC, 2, END, 2),
+	TPS6586X_LDO(LDO_5, "REG-SYS", tps6586x_ldo0, SUPPLYV6, 0, 3,
+					ENE, 6, ENE, 6),
+	TPS6586X_LDO(LDO_6, "vinldo678", tps6586x_ldo0, SUPPLYV3, 0, 3,
+					ENC, 4, END, 4),
+	TPS6586X_LDO(LDO_7, "vinldo678", tps6586x_ldo0, SUPPLYV3, 3, 3,
+					ENC, 5, END, 5),
+	TPS6586X_LDO(LDO_8, "vinldo678", tps6586x_ldo0, SUPPLYV2, 5, 3,
+					ENC, 6, END, 6),
+	TPS6586X_LDO(LDO_9, "vinldo9", tps6586x_ldo0, SUPPLYV6, 3, 3,
+					ENE, 7, ENE, 7),
+	TPS6586X_LDO(SM_2, "vin-sm2", tps658640_sm2, SUPPLYV2, 0, 5,
+					ENC, 7, END, 7),
+
+	TPS6586X_FIXED_LDO(LDO_RTC, "REG-SYS", tps658640_rtc, SUPPLYV4, 3, 2,
+					V4, 7, V4, 7),
+};
+
 static struct tps6586x_regulator tps658643_regulator[] = {
 	TPS6586X_LDO(SM_2, "vin-sm2", tps658643_sm2, SUPPLYV2, 0, 5, ENC, 7,
 					END, 7),
@@ -295,6 +338,11 @@ static struct tps6586x_regulator *find_regulator_info(int id, int version)
 		table = tps658623_regulator;
 		num = ARRAY_SIZE(tps658623_regulator);
 		break;
+	case TPS658640:
+	case TPS658640v2:
+		table = tps658640_regulator;
+		num = ARRAY_SIZE(tps658640_regulator);
+		break;
 	case TPS658643:
 		table = tps658643_regulator;
 		num = ARRAY_SIZE(tps658643_regulator);
diff --git a/drivers/regulator/vexpress.c b/drivers/regulator/vexpress.c
index f3ae28a7e66..02e7267ccf9 100644
--- a/drivers/regulator/vexpress.c
+++ b/drivers/regulator/vexpress.c
@@ -26,14 +26,14 @@
 struct vexpress_regulator {
 	struct regulator_desc desc;
 	struct regulator_dev *regdev;
-	struct vexpress_config_func *func;
+	struct regmap *regmap;
 };
 
 static int vexpress_regulator_get_voltage(struct regulator_dev *regdev)
 {
 	struct vexpress_regulator *reg = rdev_get_drvdata(regdev);
 	u32 uV;
-	int err = vexpress_config_read(reg->func, 0, &uV);
+	int err = regmap_read(reg->regmap, 0, &uV);
 
 	return err ? err : uV;
 }
@@ -43,7 +43,7 @@ static int vexpress_regulator_set_voltage(struct regulator_dev *regdev,
 {
 	struct vexpress_regulator *reg = rdev_get_drvdata(regdev);
 
-	return vexpress_config_write(reg->func, 0, min_uV);
+	return regmap_write(reg->regmap, 0, min_uV);
 }
 
 static struct regulator_ops vexpress_regulator_ops_ro = {
@@ -57,22 +57,17 @@ static struct regulator_ops vexpress_regulator_ops = {
 
 static int vexpress_regulator_probe(struct platform_device *pdev)
 {
-	int err;
 	struct vexpress_regulator *reg;
 	struct regulator_init_data *init_data;
 	struct regulator_config config = { };
 
 	reg = devm_kzalloc(&pdev->dev, sizeof(*reg), GFP_KERNEL);
-	if (!reg) {
-		err = -ENOMEM;
-		goto error_kzalloc;
-	}
+	if (!reg)
+		return -ENOMEM;
 
-	reg->func = vexpress_config_func_get_by_dev(&pdev->dev);
-	if (!reg->func) {
-		err = -ENXIO;
-		goto error_get_func;
-	}
+	reg->regmap = devm_regmap_init_vexpress_config(&pdev->dev);
+	if (IS_ERR(reg->regmap))
+		return PTR_ERR(reg->regmap);
 
 	reg->desc.name = dev_name(&pdev->dev);
 	reg->desc.type = REGULATOR_VOLTAGE;
@@ -80,10 +75,8 @@ static int vexpress_regulator_probe(struct platform_device *pdev)
 	reg->desc.continuous_voltage_range = true;
 
 	init_data = of_get_regulator_init_data(&pdev->dev, pdev->dev.of_node);
-	if (!init_data) {
-		err = -EINVAL;
-		goto error_get_regulator_init_data;
-	}
+	if (!init_data)
+		return -EINVAL;
 
 	init_data->constraints.apply_uV = 0;
 	if (init_data->constraints.min_uV && init_data->constraints.max_uV)
@@ -97,40 +90,21 @@ static int vexpress_regulator_probe(struct platform_device *pdev)
 	config.of_node = pdev->dev.of_node;
 
 	reg->regdev = devm_regulator_register(&pdev->dev, &reg->desc, &config);
-	if (IS_ERR(reg->regdev)) {
-		err = PTR_ERR(reg->regdev);
-		goto error_regulator_register;
-	}
+	if (IS_ERR(reg->regdev))
+		return PTR_ERR(reg->regdev);
 
 	platform_set_drvdata(pdev, reg);
 
 	return 0;
-
-error_regulator_register:
-error_get_regulator_init_data:
-	vexpress_config_func_put(reg->func);
-error_get_func:
-error_kzalloc:
-	return err;
-}
-
-static int vexpress_regulator_remove(struct platform_device *pdev)
-{
-	struct vexpress_regulator *reg = platform_get_drvdata(pdev);
-
-	vexpress_config_func_put(reg->func);
-
-	return 0;
 }
 
-static struct of_device_id vexpress_regulator_of_match[] = {
+static const struct of_device_id vexpress_regulator_of_match[] = {
 	{ .compatible = "arm,vexpress-volt", },
 	{ }
 };
 
 static struct platform_driver vexpress_regulator_driver = {
 	.probe = vexpress_regulator_probe,
-	.remove = vexpress_regulator_remove,
 	.driver	= {
 		.name = DRVNAME,
 		.owner = THIS_MODULE,
diff --git a/drivers/reset/Makefile b/drivers/reset/Makefile
index 4f60caf750c..60fed3d7820 100644
--- a/drivers/reset/Makefile
+++ b/drivers/reset/Makefile
@@ -1,3 +1,4 @@
 obj-$(CONFIG_RESET_CONTROLLER) += core.o
+obj-$(CONFIG_ARCH_SOCFPGA) += reset-socfpga.o
 obj-$(CONFIG_ARCH_SUNXI) += reset-sunxi.o
 obj-$(CONFIG_ARCH_STI) += sti/
diff --git a/drivers/reset/reset-socfpga.c b/drivers/reset/reset-socfpga.c
new file mode 100644
index 00000000000..79c32ca84ef
--- /dev/null
+++ b/drivers/reset/reset-socfpga.c
@@ -0,0 +1,146 @@
+/*
+ * Copyright 2014 Steffen Trumtrar <s.trumtrar@pengutronix.de>
+ *
+ * based on
+ * Allwinner SoCs Reset Controller driver
+ *
+ * Copyright 2013 Maxime Ripard
+ *
+ * Maxime Ripard <maxime.ripard@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/err.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/reset-controller.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+
+#define NR_BANKS		4
+#define OFFSET_MODRST		0x10
+
+struct socfpga_reset_data {
+	spinlock_t			lock;
+	void __iomem			*membase;
+	struct reset_controller_dev	rcdev;
+};
+
+static int socfpga_reset_assert(struct reset_controller_dev *rcdev,
+				unsigned long id)
+{
+	struct socfpga_reset_data *data = container_of(rcdev,
+						     struct socfpga_reset_data,
+						     rcdev);
+	int bank = id / BITS_PER_LONG;
+	int offset = id % BITS_PER_LONG;
+	unsigned long flags;
+	u32 reg;
+
+	spin_lock_irqsave(&data->lock, flags);
+
+	reg = readl(data->membase + OFFSET_MODRST + (bank * NR_BANKS));
+	writel(reg | BIT(offset), data->membase + OFFSET_MODRST +
+				 (bank * NR_BANKS));
+	spin_unlock_irqrestore(&data->lock, flags);
+
+	return 0;
+}
+
+static int socfpga_reset_deassert(struct reset_controller_dev *rcdev,
+				  unsigned long id)
+{
+	struct socfpga_reset_data *data = container_of(rcdev,
+						     struct socfpga_reset_data,
+						     rcdev);
+
+	int bank = id / BITS_PER_LONG;
+	int offset = id % BITS_PER_LONG;
+	unsigned long flags;
+	u32 reg;
+
+	spin_lock_irqsave(&data->lock, flags);
+
+	reg = readl(data->membase + OFFSET_MODRST + (bank * NR_BANKS));
+	writel(reg & ~BIT(offset), data->membase + OFFSET_MODRST +
+				  (bank * NR_BANKS));
+
+	spin_unlock_irqrestore(&data->lock, flags);
+
+	return 0;
+}
+
+static struct reset_control_ops socfpga_reset_ops = {
+	.assert		= socfpga_reset_assert,
+	.deassert	= socfpga_reset_deassert,
+};
+
+static int socfpga_reset_probe(struct platform_device *pdev)
+{
+	struct socfpga_reset_data *data;
+	struct resource *res;
+
+	/*
+	 * The binding was mainlined without the required property.
+	 * Do not continue, when we encounter an old DT.
+	 */
+	if (!of_find_property(pdev->dev.of_node, "#reset-cells", NULL)) {
+		dev_err(&pdev->dev, "%s missing #reset-cells property\n",
+			pdev->dev.of_node->full_name);
+		return -EINVAL;
+	}
+
+	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	data->membase = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(data->membase))
+		return PTR_ERR(data->membase);
+
+	spin_lock_init(&data->lock);
+
+	data->rcdev.owner = THIS_MODULE;
+	data->rcdev.nr_resets = NR_BANKS * BITS_PER_LONG;
+	data->rcdev.ops = &socfpga_reset_ops;
+	data->rcdev.of_node = pdev->dev.of_node;
+	reset_controller_register(&data->rcdev);
+
+	return 0;
+}
+
+static int socfpga_reset_remove(struct platform_device *pdev)
+{
+	struct socfpga_reset_data *data = platform_get_drvdata(pdev);
+
+	reset_controller_unregister(&data->rcdev);
+
+	return 0;
+}
+
+static const struct of_device_id socfpga_reset_dt_ids[] = {
+	{ .compatible = "altr,rst-mgr", },
+	{ /* sentinel */ },
+};
+
+static struct platform_driver socfpga_reset_driver = {
+	.probe	= socfpga_reset_probe,
+	.remove	= socfpga_reset_remove,
+	.driver = {
+		.name		= "socfpga-reset",
+		.owner		= THIS_MODULE,
+		.of_match_table	= socfpga_reset_dt_ids,
+	},
+};
+module_platform_driver(socfpga_reset_driver);
+
+MODULE_AUTHOR("Steffen Trumtrar <s.trumtrar@pengutronix.de");
+MODULE_DESCRIPTION("Socfpga Reset Controller Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/reset/reset-sunxi.c b/drivers/reset/reset-sunxi.c
index 695bd3496eb..a94e7a7820b 100644
--- a/drivers/reset/reset-sunxi.c
+++ b/drivers/reset/reset-sunxi.c
@@ -145,7 +145,24 @@ MODULE_DEVICE_TABLE(of, sunxi_reset_dt_ids);
 
 static int sunxi_reset_probe(struct platform_device *pdev)
 {
-	return sunxi_reset_init(pdev->dev.of_node);
+	struct sunxi_reset_data *data;
+	struct resource *res;
+
+	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	data->membase = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(data->membase))
+		return PTR_ERR(data->membase);
+
+	data->rcdev.owner = THIS_MODULE;
+	data->rcdev.nr_resets = resource_size(res) * 32;
+	data->rcdev.ops = &sunxi_reset_ops;
+	data->rcdev.of_node = pdev->dev.of_node;
+
+	return reset_controller_register(&data->rcdev);
 }
 
 static int sunxi_reset_remove(struct platform_device *pdev)
@@ -153,8 +170,6 @@ static int sunxi_reset_remove(struct platform_device *pdev)
 	struct sunxi_reset_data *data = platform_get_drvdata(pdev);
 
 	reset_controller_unregister(&data->rcdev);
-	iounmap(data->membase);
-	kfree(data);
 
 	return 0;
 }
diff --git a/drivers/reset/sti/reset-stih415.c b/drivers/reset/sti/reset-stih415.c
index e6f6c41abe1..c93fd260447 100644
--- a/drivers/reset/sti/reset-stih415.c
+++ b/drivers/reset/sti/reset-stih415.c
@@ -73,6 +73,7 @@ static const struct syscfg_reset_channel_data stih415_softresets[] = {
 	[STIH415_USB0_SOFTRESET] = STIH415_SRST_REAR(SYSCFG_376, 9),
 	[STIH415_USB1_SOFTRESET] = STIH415_SRST_REAR(SYSCFG_376, 10),
 	[STIH415_USB2_SOFTRESET] = STIH415_SRST_REAR(SYSCFG_376, 11),
+	[STIH415_KEYSCAN_SOFTRESET] = STIH415_SRST_LPM(LPM_SYSCFG_1, 8),
 };
 
 static struct syscfg_reset_controller_data stih415_powerdown_controller = {
diff --git a/drivers/reset/sti/reset-stih416.c b/drivers/reset/sti/reset-stih416.c
index fe3bf02bdc8..5fc987076a9 100644
--- a/drivers/reset/sti/reset-stih416.c
+++ b/drivers/reset/sti/reset-stih416.c
@@ -104,6 +104,7 @@ static const struct syscfg_reset_channel_data stih416_softresets[] = {
 	[STIH416_COMPO_A_SOFTRESET] = STIH416_SRST_CPU(SYSCFG_7564, 4),
 	[STIH416_VP8_DEC_SOFTRESET] = STIH416_SRST_CPU(SYSCFG_7564, 10),
 	[STIH416_VTG_MAIN_SOFTRESET] = STIH416_SRST_CPU(SYSCFG_7564, 16),
+	[STIH416_KEYSCAN_SOFTRESET] = STIH416_SRST_LPM(LPM_SYSCFG_1, 8),
 };
 
 static struct syscfg_reset_controller_data stih416_powerdown_controller = {
diff --git a/drivers/rtc/Kconfig b/drivers/rtc/Kconfig
index 2e565f8e516..0754f5c7cb3 100644
--- a/drivers/rtc/Kconfig
+++ b/drivers/rtc/Kconfig
@@ -386,12 +386,12 @@ config RTC_DRV_PCF8583
 	  will be called rtc-pcf8583.
 
 config RTC_DRV_M41T80
-	tristate "ST M41T62/65/M41T80/81/82/83/84/85/87"
+	tristate "ST M41T62/65/M41T80/81/82/83/84/85/87 and compatible"
 	help
 	  If you say Y here you will get support for the ST M41T60
 	  and M41T80 RTC chips series. Currently, the following chips are
 	  supported: M41T62, M41T65, M41T80, M41T81, M41T82, M41T83, M41ST84,
-	  M41ST85, and M41ST87.
+	  M41ST85, M41ST87, and MicroCrystal RV4162.
 
 	  This driver can also be built as a module. If so, the module
 	  will be called rtc-m41t80.
@@ -530,11 +530,11 @@ config RTC_DRV_RV3029C2
 	  will be called rtc-rv3029c2.
 
 config RTC_DRV_S5M
-	tristate "Samsung S5M series"
+	tristate "Samsung S2M/S5M series"
 	depends on MFD_SEC_CORE
 	help
 	  If you say yes here you will get support for the
-	  RTC of Samsung S5M PMIC series.
+	  RTC of Samsung S2MPS14 and S5M PMIC series.
 
 	  This driver can also be built as a module. If so, the module
 	  will be called rtc-s5m.
@@ -573,6 +573,17 @@ config RTC_DRV_DS1305
 	  This driver can also be built as a module. If so, the module
 	  will be called rtc-ds1305.
 
+config RTC_DRV_DS1343
+	select REGMAP_SPI
+	tristate "Dallas/Maxim DS1343/DS1344"
+	help
+	  If you say yes here you get support for the
+	  Dallas/Maxim DS1343 and DS1344 real time clock chips.
+	  Support for trickle charger, alarm is provided.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called rtc-ds1343.
+
 config RTC_DRV_DS1347
 	tristate "Dallas/Maxim DS1347"
 	help
@@ -650,6 +661,14 @@ config RTC_DRV_RX4581
 	  This driver can also be built as a module. If so the module
 	  will be called rtc-rx4581.
 
+config RTC_DRV_MCP795
+	tristate "Microchip MCP795"
+	help
+	  If you say yes here you will get support for the Microchip MCP795.
+
+	  This driver can also be built as a module. If so the module
+	  will be called rtc-mcp795.
+
 endif # SPI_MASTER
 
 comment "Platform RTC drivers"
@@ -758,6 +777,16 @@ config RTC_DRV_DA9055
 	  This driver can also be built as a module. If so, the module
 	  will be called rtc-da9055
 
+config RTC_DRV_DA9063
+	tristate "Dialog Semiconductor DA9063 RTC"
+	depends on MFD_DA9063
+	help
+	  If you say yes here you will get support for the RTC subsystem
+	  of the Dialog Semiconductor DA9063.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called "rtc-da9063".
+
 config RTC_DRV_EFI
 	tristate "EFI RTC"
 	depends on IA64
@@ -1327,6 +1356,15 @@ config RTC_DRV_MOXART
 	   This driver can also be built as a module. If so, the module
 	   will be called rtc-moxart
 
+config RTC_DRV_XGENE
+	tristate "APM X-Gene RTC"
+	help
+	  If you say yes here you get support for the APM X-Gene SoC real time
+	  clock.
+
+	  This driver can also be built as a module, if so, the module
+	  will be called "rtc-xgene".
+
 comment "HID Sensor RTC drivers"
 
 config RTC_DRV_HID_SENSOR_TIME
diff --git a/drivers/rtc/Makefile b/drivers/rtc/Makefile
index 40a09915c8f..70347d041d1 100644
--- a/drivers/rtc/Makefile
+++ b/drivers/rtc/Makefile
@@ -32,6 +32,7 @@ obj-$(CONFIG_RTC_DRV_CMOS)	+= rtc-cmos.o
 obj-$(CONFIG_RTC_DRV_COH901331)	+= rtc-coh901331.o
 obj-$(CONFIG_RTC_DRV_DA9052)	+= rtc-da9052.o
 obj-$(CONFIG_RTC_DRV_DA9055)	+= rtc-da9055.o
+obj-$(CONFIG_RTC_DRV_DA9063)	+= rtc-da9063.o
 obj-$(CONFIG_RTC_DRV_DAVINCI)	+= rtc-davinci.o
 obj-$(CONFIG_RTC_DRV_DM355EVM)	+= rtc-dm355evm.o
 obj-$(CONFIG_RTC_DRV_VRTC)	+= rtc-mrst.o
@@ -40,6 +41,7 @@ obj-$(CONFIG_RTC_DRV_DS1286)	+= rtc-ds1286.o
 obj-$(CONFIG_RTC_DRV_DS1302)	+= rtc-ds1302.o
 obj-$(CONFIG_RTC_DRV_DS1305)	+= rtc-ds1305.o
 obj-$(CONFIG_RTC_DRV_DS1307)	+= rtc-ds1307.o
+obj-$(CONFIG_RTC_DRV_DS1343)	+= rtc-ds1343.o
 obj-$(CONFIG_RTC_DRV_DS1347)	+= rtc-ds1347.o
 obj-$(CONFIG_RTC_DRV_DS1374)	+= rtc-ds1374.o
 obj-$(CONFIG_RTC_DRV_DS1390)	+= rtc-ds1390.o
@@ -80,6 +82,7 @@ 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_MC13XXX)	+= rtc-mc13xxx.o
+obj-$(CONFIG_RTC_DRV_MCP795)	+= rtc-mcp795.o
 obj-$(CONFIG_RTC_DRV_MSM6242)	+= rtc-msm6242.o
 obj-$(CONFIG_RTC_DRV_MPC5121)	+= rtc-mpc5121.o
 obj-$(CONFIG_RTC_DRV_MV)	+= rtc-mv.o
@@ -135,5 +138,6 @@ obj-$(CONFIG_RTC_DRV_VT8500)	+= rtc-vt8500.o
 obj-$(CONFIG_RTC_DRV_WM831X)	+= rtc-wm831x.o
 obj-$(CONFIG_RTC_DRV_WM8350)	+= rtc-wm8350.o
 obj-$(CONFIG_RTC_DRV_X1205)	+= rtc-x1205.o
+obj-$(CONFIG_RTC_DRV_XGENE)	+= rtc-xgene.o
 obj-$(CONFIG_RTC_DRV_SIRFSOC)	+= rtc-sirfsoc.o
 obj-$(CONFIG_RTC_DRV_MOXART)	+= rtc-moxart.o
diff --git a/drivers/rtc/interface.c b/drivers/rtc/interface.c
index c2eff608236..5813fa52c3d 100644
--- a/drivers/rtc/interface.c
+++ b/drivers/rtc/interface.c
@@ -292,7 +292,8 @@ int __rtc_read_alarm(struct rtc_device *rtc, struct rtc_wkalrm *alarm)
 		dev_dbg(&rtc->dev, "alarm rollover: %s\n", "year");
 		do {
 			alarm->time.tm_year++;
-		} while (rtc_valid_tm(&alarm->time) != 0);
+		} while (!is_leap_year(alarm->time.tm_year + 1900)
+			&& rtc_valid_tm(&alarm->time) != 0);
 		break;
 
 	default:
@@ -300,7 +301,16 @@ int __rtc_read_alarm(struct rtc_device *rtc, struct rtc_wkalrm *alarm)
 	}
 
 done:
-	return 0;
+	err = rtc_valid_tm(&alarm->time);
+
+	if (err) {
+		dev_warn(&rtc->dev, "invalid alarm value: %d-%d-%d %d:%d:%d\n",
+			alarm->time.tm_year + 1900, alarm->time.tm_mon + 1,
+			alarm->time.tm_mday, alarm->time.tm_hour, alarm->time.tm_min,
+			alarm->time.tm_sec);
+	}
+
+	return err;
 }
 
 int rtc_read_alarm(struct rtc_device *rtc, struct rtc_wkalrm *alarm)
diff --git a/drivers/rtc/rtc-88pm860x.c b/drivers/rtc/rtc-88pm860x.c
index 816504846cd..0c6add1a38d 100644
--- a/drivers/rtc/rtc-88pm860x.c
+++ b/drivers/rtc/rtc-88pm860x.c
@@ -293,7 +293,7 @@ static int pm860x_rtc_dt_init(struct platform_device *pdev,
 	int ret;
 	if (!np)
 		return -ENODEV;
-	np = of_find_node_by_name(np, "rtc");
+	np = of_get_child_by_name(np, "rtc");
 	if (!np) {
 		dev_err(&pdev->dev, "failed to find rtc node\n");
 		return -ENODEV;
@@ -301,6 +301,7 @@ static int pm860x_rtc_dt_init(struct platform_device *pdev,
 	ret = of_property_read_u32(np, "marvell,88pm860x-vrtc", &info->vrtc);
 	if (ret)
 		info->vrtc = 0;
+	of_node_put(np);
 	return 0;
 }
 #else
diff --git a/drivers/rtc/rtc-at91rm9200.c b/drivers/rtc/rtc-at91rm9200.c
index 3281c90691c..44fe83ee9be 100644
--- a/drivers/rtc/rtc-at91rm9200.c
+++ b/drivers/rtc/rtc-at91rm9200.c
@@ -48,6 +48,7 @@ struct at91_rtc_config {
 
 static const struct at91_rtc_config *at91_rtc_config;
 static DECLARE_COMPLETION(at91_rtc_updated);
+static DECLARE_COMPLETION(at91_rtc_upd_rdy);
 static unsigned int at91_alarm_year = AT91_RTC_EPOCH;
 static void __iomem *at91_rtc_regs;
 static int irq;
@@ -161,6 +162,8 @@ static int at91_rtc_settime(struct device *dev, struct rtc_time *tm)
 		1900 + tm->tm_year, tm->tm_mon, tm->tm_mday,
 		tm->tm_hour, tm->tm_min, tm->tm_sec);
 
+	wait_for_completion(&at91_rtc_upd_rdy);
+
 	/* Stop Time/Calendar from counting */
 	cr = at91_rtc_read(AT91_RTC_CR);
 	at91_rtc_write(AT91_RTC_CR, cr | AT91_RTC_UPDCAL | AT91_RTC_UPDTIM);
@@ -183,7 +186,9 @@ static int at91_rtc_settime(struct device *dev, struct rtc_time *tm)
 
 	/* Restart Time/Calendar */
 	cr = at91_rtc_read(AT91_RTC_CR);
+	at91_rtc_write(AT91_RTC_SCCR, AT91_RTC_SECEV);
 	at91_rtc_write(AT91_RTC_CR, cr & ~(AT91_RTC_UPDCAL | AT91_RTC_UPDTIM));
+	at91_rtc_write_ier(AT91_RTC_SECEV);
 
 	return 0;
 }
@@ -290,8 +295,10 @@ static irqreturn_t at91_rtc_interrupt(int irq, void *dev_id)
 	if (rtsr) {		/* this interrupt is shared!  Is it ours? */
 		if (rtsr & AT91_RTC_ALARM)
 			events |= (RTC_AF | RTC_IRQF);
-		if (rtsr & AT91_RTC_SECEV)
-			events |= (RTC_UF | RTC_IRQF);
+		if (rtsr & AT91_RTC_SECEV) {
+			complete(&at91_rtc_upd_rdy);
+			at91_rtc_write_idr(AT91_RTC_SECEV);
+		}
 		if (rtsr & AT91_RTC_ACKUPD)
 			complete(&at91_rtc_updated);
 
@@ -413,6 +420,11 @@ static int __init at91_rtc_probe(struct platform_device *pdev)
 		return PTR_ERR(rtc);
 	platform_set_drvdata(pdev, rtc);
 
+	/* enable SECEV interrupt in order to initialize at91_rtc_upd_rdy
+	 * completion.
+	 */
+	at91_rtc_write_ier(AT91_RTC_SECEV);
+
 	dev_info(&pdev->dev, "AT91 Real Time Clock driver.\n");
 	return 0;
 }
diff --git a/drivers/rtc/rtc-bfin.c b/drivers/rtc/rtc-bfin.c
index 0c53f452849..fe4bdb06a55 100644
--- a/drivers/rtc/rtc-bfin.c
+++ b/drivers/rtc/rtc-bfin.c
@@ -346,7 +346,7 @@ static int bfin_rtc_probe(struct platform_device *pdev)
 {
 	struct bfin_rtc *rtc;
 	struct device *dev = &pdev->dev;
-	int ret = 0;
+	int ret;
 	unsigned long timeout = jiffies + HZ;
 
 	dev_dbg_stamp(dev);
@@ -361,16 +361,17 @@ static int bfin_rtc_probe(struct platform_device *pdev)
 	/* Register our RTC with the RTC framework */
 	rtc->rtc_dev = devm_rtc_device_register(dev, pdev->name, &bfin_rtc_ops,
 						THIS_MODULE);
-	if (unlikely(IS_ERR(rtc->rtc_dev))) {
-		ret = PTR_ERR(rtc->rtc_dev);
-		goto err;
-	}
+	if (unlikely(IS_ERR(rtc->rtc_dev)))
+		return PTR_ERR(rtc->rtc_dev);
 
 	/* Grab the IRQ and init the hardware */
 	ret = devm_request_irq(dev, IRQ_RTC, bfin_rtc_interrupt, 0,
 				pdev->name, dev);
 	if (unlikely(ret))
-		goto err;
+		dev_err(&pdev->dev,
+			"unable to request IRQ; alarm won't work, "
+			"and writes will be delayed\n");
+
 	/* sometimes the bootloader touched things, but the write complete was not
 	 * enabled, so let's just do a quick timeout here since the IRQ will not fire ...
 	 */
@@ -381,9 +382,6 @@ static int bfin_rtc_probe(struct platform_device *pdev)
 	bfin_write_RTC_SWCNT(0);
 
 	return 0;
-
-err:
-	return ret;
 }
 
 static int bfin_rtc_remove(struct platform_device *pdev)
diff --git a/drivers/rtc/rtc-cmos.c b/drivers/rtc/rtc-cmos.c
index 0963c9309c7..b0e4a3eb33c 100644
--- a/drivers/rtc/rtc-cmos.c
+++ b/drivers/rtc/rtc-cmos.c
@@ -647,6 +647,7 @@ cmos_do_probe(struct device *dev, struct resource *ports, int rtc_irq)
 	int				retval = 0;
 	unsigned char			rtc_control;
 	unsigned			address_space;
+	u32				flags = 0;
 
 	/* there can be only one ... */
 	if (cmos_rtc.dev)
@@ -660,9 +661,12 @@ cmos_do_probe(struct device *dev, struct resource *ports, int rtc_irq)
 	 * REVISIT non-x86 systems may instead use memory space resources
 	 * (needing ioremap etc), not i/o space resources like this ...
 	 */
-	ports = request_region(ports->start,
-			resource_size(ports),
-			driver_name);
+	if (RTC_IOMAPPED)
+		ports = request_region(ports->start, resource_size(ports),
+				       driver_name);
+	else
+		ports = request_mem_region(ports->start, resource_size(ports),
+					   driver_name);
 	if (!ports) {
 		dev_dbg(dev, "i/o registers already in use\n");
 		return -EBUSY;
@@ -699,6 +703,11 @@ cmos_do_probe(struct device *dev, struct resource *ports, int rtc_irq)
 	 * expect CMOS_READ and friends to handle.
 	 */
 	if (info) {
+		if (info->flags)
+			flags = info->flags;
+		if (info->address_space)
+			address_space = info->address_space;
+
 		if (info->rtc_day_alarm && info->rtc_day_alarm < 128)
 			cmos_rtc.day_alrm = info->rtc_day_alarm;
 		if (info->rtc_mon_alarm && info->rtc_mon_alarm < 128)
@@ -726,18 +735,21 @@ cmos_do_probe(struct device *dev, struct resource *ports, int rtc_irq)
 
 	spin_lock_irq(&rtc_lock);
 
-	/* force periodic irq to CMOS reset default of 1024Hz;
-	 *
-	 * REVISIT it's been reported that at least one x86_64 ALI mobo
-	 * doesn't use 32KHz here ... for portability we might need to
-	 * do something about other clock frequencies.
-	 */
-	cmos_rtc.rtc->irq_freq = 1024;
-	hpet_set_periodic_freq(cmos_rtc.rtc->irq_freq);
-	CMOS_WRITE(RTC_REF_CLCK_32KHZ | 0x06, RTC_FREQ_SELECT);
+	if (!(flags & CMOS_RTC_FLAGS_NOFREQ)) {
+		/* force periodic irq to CMOS reset default of 1024Hz;
+		 *
+		 * REVISIT it's been reported that at least one x86_64 ALI
+		 * mobo doesn't use 32KHz here ... for portability we might
+		 * need to do something about other clock frequencies.
+		 */
+		cmos_rtc.rtc->irq_freq = 1024;
+		hpet_set_periodic_freq(cmos_rtc.rtc->irq_freq);
+		CMOS_WRITE(RTC_REF_CLCK_32KHZ | 0x06, RTC_FREQ_SELECT);
+	}
 
 	/* disable irqs */
-	cmos_irq_disable(&cmos_rtc, RTC_PIE | RTC_AIE | RTC_UIE);
+	if (is_valid_irq(rtc_irq))
+		cmos_irq_disable(&cmos_rtc, RTC_PIE | RTC_AIE | RTC_UIE);
 
 	rtc_control = CMOS_READ(RTC_CONTROL);
 
@@ -802,14 +814,18 @@ cleanup1:
 	cmos_rtc.dev = NULL;
 	rtc_device_unregister(cmos_rtc.rtc);
 cleanup0:
-	release_region(ports->start, resource_size(ports));
+	if (RTC_IOMAPPED)
+		release_region(ports->start, resource_size(ports));
+	else
+		release_mem_region(ports->start, resource_size(ports));
 	return retval;
 }
 
-static void cmos_do_shutdown(void)
+static void cmos_do_shutdown(int rtc_irq)
 {
 	spin_lock_irq(&rtc_lock);
-	cmos_irq_disable(&cmos_rtc, RTC_IRQMASK);
+	if (is_valid_irq(rtc_irq))
+		cmos_irq_disable(&cmos_rtc, RTC_IRQMASK);
 	spin_unlock_irq(&rtc_lock);
 }
 
@@ -818,7 +834,7 @@ static void __exit cmos_do_remove(struct device *dev)
 	struct cmos_rtc	*cmos = dev_get_drvdata(dev);
 	struct resource *ports;
 
-	cmos_do_shutdown();
+	cmos_do_shutdown(cmos->irq);
 
 	sysfs_remove_bin_file(&dev->kobj, &nvram);
 
@@ -831,7 +847,10 @@ static void __exit cmos_do_remove(struct device *dev)
 	cmos->rtc = NULL;
 
 	ports = cmos->iomem;
-	release_region(ports->start, resource_size(ports));
+	if (RTC_IOMAPPED)
+		release_region(ports->start, resource_size(ports));
+	else
+		release_mem_region(ports->start, resource_size(ports));
 	cmos->iomem = NULL;
 
 	cmos->dev = NULL;
@@ -1065,10 +1084,13 @@ static void __exit cmos_pnp_remove(struct pnp_dev *pnp)
 
 static void cmos_pnp_shutdown(struct pnp_dev *pnp)
 {
-	if (system_state == SYSTEM_POWER_OFF && !cmos_poweroff(&pnp->dev))
+	struct device *dev = &pnp->dev;
+	struct cmos_rtc	*cmos = dev_get_drvdata(dev);
+
+	if (system_state == SYSTEM_POWER_OFF && !cmos_poweroff(dev))
 		return;
 
-	cmos_do_shutdown();
+	cmos_do_shutdown(cmos->irq);
 }
 
 static const struct pnp_device_id rtc_ids[] = {
@@ -1143,11 +1165,21 @@ static inline void cmos_of_init(struct platform_device *pdev) {}
 
 static int __init cmos_platform_probe(struct platform_device *pdev)
 {
+	struct resource *resource;
+	int irq;
+
 	cmos_of_init(pdev);
 	cmos_wake_setup(&pdev->dev);
-	return cmos_do_probe(&pdev->dev,
-			platform_get_resource(pdev, IORESOURCE_IO, 0),
-			platform_get_irq(pdev, 0));
+
+	if (RTC_IOMAPPED)
+		resource = platform_get_resource(pdev, IORESOURCE_IO, 0);
+	else
+		resource = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		irq = -1;
+
+	return cmos_do_probe(&pdev->dev, resource, irq);
 }
 
 static int __exit cmos_platform_remove(struct platform_device *pdev)
@@ -1158,10 +1190,13 @@ static int __exit cmos_platform_remove(struct platform_device *pdev)
 
 static void cmos_platform_shutdown(struct platform_device *pdev)
 {
-	if (system_state == SYSTEM_POWER_OFF && !cmos_poweroff(&pdev->dev))
+	struct device *dev = &pdev->dev;
+	struct cmos_rtc	*cmos = dev_get_drvdata(dev);
+
+	if (system_state == SYSTEM_POWER_OFF && !cmos_poweroff(dev))
 		return;
 
-	cmos_do_shutdown();
+	cmos_do_shutdown(cmos->irq);
 }
 
 /* work with hotplug and coldplug */
diff --git a/drivers/rtc/rtc-da9052.c b/drivers/rtc/rtc-da9052.c
index a1cbf64242a..e5c9486cf45 100644
--- a/drivers/rtc/rtc-da9052.c
+++ b/drivers/rtc/rtc-da9052.c
@@ -20,28 +20,28 @@
 #include <linux/mfd/da9052/da9052.h>
 #include <linux/mfd/da9052/reg.h>
 
-#define rtc_err(da9052, fmt, ...) \
-		dev_err(da9052->dev, "%s: " fmt, __func__, ##__VA_ARGS__)
+#define rtc_err(rtc, fmt, ...) \
+		dev_err(rtc->da9052->dev, "%s: " fmt, __func__, ##__VA_ARGS__)
 
 struct da9052_rtc {
 	struct rtc_device *rtc;
 	struct da9052 *da9052;
 };
 
-static int da9052_rtc_enable_alarm(struct da9052 *da9052, bool enable)
+static int da9052_rtc_enable_alarm(struct da9052_rtc *rtc, bool enable)
 {
 	int ret;
 	if (enable) {
-		ret = da9052_reg_update(da9052, DA9052_ALARM_Y_REG,
-					DA9052_ALARM_Y_ALARM_ON,
-					DA9052_ALARM_Y_ALARM_ON);
+		ret = da9052_reg_update(rtc->da9052, DA9052_ALARM_Y_REG,
+				DA9052_ALARM_Y_ALARM_ON|DA9052_ALARM_Y_TICK_ON,
+				DA9052_ALARM_Y_ALARM_ON);
 		if (ret != 0)
-			rtc_err(da9052, "Failed to enable ALM: %d\n", ret);
+			rtc_err(rtc, "Failed to enable ALM: %d\n", ret);
 	} else {
-		ret = da9052_reg_update(da9052, DA9052_ALARM_Y_REG,
-					DA9052_ALARM_Y_ALARM_ON, 0);
+		ret = da9052_reg_update(rtc->da9052, DA9052_ALARM_Y_REG,
+			DA9052_ALARM_Y_ALARM_ON|DA9052_ALARM_Y_TICK_ON, 0);
 		if (ret != 0)
-			rtc_err(da9052, "Write error: %d\n", ret);
+			rtc_err(rtc, "Write error: %d\n", ret);
 	}
 	return ret;
 }
@@ -49,31 +49,20 @@ static int da9052_rtc_enable_alarm(struct da9052 *da9052, bool enable)
 static irqreturn_t da9052_rtc_irq(int irq, void *data)
 {
 	struct da9052_rtc *rtc = data;
-	int ret;
 
-	ret = da9052_reg_read(rtc->da9052, DA9052_ALARM_MI_REG);
-	if (ret < 0) {
-		rtc_err(rtc->da9052, "Read error: %d\n", ret);
-		return IRQ_NONE;
-	}
-
-	if (ret & DA9052_ALARMMI_ALARMTYPE) {
-		da9052_rtc_enable_alarm(rtc->da9052, 0);
-		rtc_update_irq(rtc->rtc, 1, RTC_IRQF | RTC_AF);
-	} else
-		rtc_update_irq(rtc->rtc, 1, RTC_IRQF | RTC_PF);
+	rtc_update_irq(rtc->rtc, 1, RTC_IRQF | RTC_AF);
 
 	return IRQ_HANDLED;
 }
 
-static int da9052_read_alarm(struct da9052 *da9052, struct rtc_time *rtc_tm)
+static int da9052_read_alarm(struct da9052_rtc *rtc, struct rtc_time *rtc_tm)
 {
 	int ret;
 	uint8_t v[5];
 
-	ret = da9052_group_read(da9052, DA9052_ALARM_MI_REG, 5, v);
+	ret = da9052_group_read(rtc->da9052, DA9052_ALARM_MI_REG, 5, v);
 	if (ret != 0) {
-		rtc_err(da9052, "Failed to group read ALM: %d\n", ret);
+		rtc_err(rtc, "Failed to group read ALM: %d\n", ret);
 		return ret;
 	}
 
@@ -84,23 +73,33 @@ static int da9052_read_alarm(struct da9052 *da9052, struct rtc_time *rtc_tm)
 	rtc_tm->tm_min  = v[0] & DA9052_RTC_MIN;
 
 	ret = rtc_valid_tm(rtc_tm);
-	if (ret != 0)
-		return ret;
 	return ret;
 }
 
-static int da9052_set_alarm(struct da9052 *da9052, struct rtc_time *rtc_tm)
+static int da9052_set_alarm(struct da9052_rtc *rtc, struct rtc_time *rtc_tm)
 {
+	struct da9052 *da9052 = rtc->da9052;
+	unsigned long alm_time;
 	int ret;
 	uint8_t v[3];
 
+	ret = rtc_tm_to_time(rtc_tm, &alm_time);
+	if (ret != 0)
+		return ret;
+
+	if (rtc_tm->tm_sec > 0) {
+		alm_time += 60 - rtc_tm->tm_sec;
+		rtc_time_to_tm(alm_time, rtc_tm);
+	}
+	BUG_ON(rtc_tm->tm_sec); /* it will cause repeated irqs if not zero */
+
 	rtc_tm->tm_year -= 100;
 	rtc_tm->tm_mon += 1;
 
 	ret = da9052_reg_update(da9052, DA9052_ALARM_MI_REG,
 				DA9052_RTC_MIN, rtc_tm->tm_min);
 	if (ret != 0) {
-		rtc_err(da9052, "Failed to write ALRM MIN: %d\n", ret);
+		rtc_err(rtc, "Failed to write ALRM MIN: %d\n", ret);
 		return ret;
 	}
 
@@ -115,22 +114,22 @@ static int da9052_set_alarm(struct da9052 *da9052, struct rtc_time *rtc_tm)
 	ret = da9052_reg_update(da9052, DA9052_ALARM_Y_REG,
 				DA9052_RTC_YEAR, rtc_tm->tm_year);
 	if (ret != 0)
-		rtc_err(da9052, "Failed to write ALRM YEAR: %d\n", ret);
+		rtc_err(rtc, "Failed to write ALRM YEAR: %d\n", ret);
 
 	return ret;
 }
 
-static int da9052_rtc_get_alarm_status(struct da9052 *da9052)
+static int da9052_rtc_get_alarm_status(struct da9052_rtc *rtc)
 {
 	int ret;
 
-	ret = da9052_reg_read(da9052, DA9052_ALARM_Y_REG);
+	ret = da9052_reg_read(rtc->da9052, DA9052_ALARM_Y_REG);
 	if (ret < 0) {
-		rtc_err(da9052, "Failed to read ALM: %d\n", ret);
+		rtc_err(rtc, "Failed to read ALM: %d\n", ret);
 		return ret;
 	}
-	ret &= DA9052_ALARM_Y_ALARM_ON;
-	return (ret > 0) ? 1 : 0;
+
+	return !!(ret&DA9052_ALARM_Y_ALARM_ON);
 }
 
 static int da9052_rtc_read_time(struct device *dev, struct rtc_time *rtc_tm)
@@ -141,7 +140,7 @@ static int da9052_rtc_read_time(struct device *dev, struct rtc_time *rtc_tm)
 
 	ret = da9052_group_read(rtc->da9052, DA9052_COUNT_S_REG, 6, v);
 	if (ret < 0) {
-		rtc_err(rtc->da9052, "Failed to read RTC time : %d\n", ret);
+		rtc_err(rtc, "Failed to read RTC time : %d\n", ret);
 		return ret;
 	}
 
@@ -153,18 +152,14 @@ static int da9052_rtc_read_time(struct device *dev, struct rtc_time *rtc_tm)
 	rtc_tm->tm_sec  = v[0] & DA9052_RTC_SEC;
 
 	ret = rtc_valid_tm(rtc_tm);
-	if (ret != 0) {
-		rtc_err(rtc->da9052, "rtc_valid_tm failed: %d\n", ret);
-		return ret;
-	}
-
-	return 0;
+	return ret;
 }
 
 static int da9052_rtc_set_time(struct device *dev, struct rtc_time *tm)
 {
 	struct da9052_rtc *rtc;
 	uint8_t v[6];
+	int ret;
 
 	rtc = dev_get_drvdata(dev);
 
@@ -175,7 +170,10 @@ static int da9052_rtc_set_time(struct device *dev, struct rtc_time *tm)
 	v[4] = tm->tm_mon + 1;
 	v[5] = tm->tm_year - 100;
 
-	return da9052_group_write(rtc->da9052, DA9052_COUNT_S_REG, 6, v);
+	ret = da9052_group_write(rtc->da9052, DA9052_COUNT_S_REG, 6, v);
+	if (ret < 0)
+		rtc_err(rtc, "failed to set RTC time: %d\n", ret);
+	return ret;
 }
 
 static int da9052_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
@@ -184,13 +182,13 @@ static int da9052_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 	struct rtc_time *tm = &alrm->time;
 	struct da9052_rtc *rtc = dev_get_drvdata(dev);
 
-	ret = da9052_read_alarm(rtc->da9052, tm);
-
-	if (ret)
+	ret = da9052_read_alarm(rtc, tm);
+	if (ret < 0) {
+		rtc_err(rtc, "failed to read RTC alarm: %d\n", ret);
 		return ret;
+	}
 
-	alrm->enabled = da9052_rtc_get_alarm_status(rtc->da9052);
-
+	alrm->enabled = da9052_rtc_get_alarm_status(rtc);
 	return 0;
 }
 
@@ -200,16 +198,15 @@ static int da9052_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 	struct rtc_time *tm = &alrm->time;
 	struct da9052_rtc *rtc = dev_get_drvdata(dev);
 
-	ret = da9052_rtc_enable_alarm(rtc->da9052, 0);
+	ret = da9052_rtc_enable_alarm(rtc, 0);
 	if (ret < 0)
 		return ret;
 
-	ret = da9052_set_alarm(rtc->da9052, tm);
-	if (ret)
+	ret = da9052_set_alarm(rtc, tm);
+	if (ret < 0)
 		return ret;
 
-	ret = da9052_rtc_enable_alarm(rtc->da9052, 1);
-
+	ret = da9052_rtc_enable_alarm(rtc, 1);
 	return ret;
 }
 
@@ -217,7 +214,7 @@ static int da9052_rtc_alarm_irq_enable(struct device *dev, unsigned int enabled)
 {
 	struct da9052_rtc *rtc = dev_get_drvdata(dev);
 
-	return da9052_rtc_enable_alarm(rtc->da9052, enabled);
+	return da9052_rtc_enable_alarm(rtc, enabled);
 }
 
 static const struct rtc_class_ops da9052_rtc_ops = {
@@ -239,10 +236,23 @@ static int da9052_rtc_probe(struct platform_device *pdev)
 
 	rtc->da9052 = dev_get_drvdata(pdev->dev.parent);
 	platform_set_drvdata(pdev, rtc);
+
+	ret = da9052_reg_write(rtc->da9052, DA9052_BBAT_CONT_REG, 0xFE);
+	if (ret < 0) {
+		rtc_err(rtc,
+			"Failed to setup RTC battery charging: %d\n", ret);
+		return ret;
+	}
+
+	ret = da9052_reg_update(rtc->da9052, DA9052_ALARM_Y_REG,
+				DA9052_ALARM_Y_TICK_ON, 0);
+	if (ret != 0)
+		rtc_err(rtc, "Failed to disable TICKS: %d\n", ret);
+
 	ret = da9052_request_irq(rtc->da9052, DA9052_IRQ_ALARM, "ALM",
 				da9052_rtc_irq, rtc);
 	if (ret != 0) {
-		rtc_err(rtc->da9052, "irq registration failed: %d\n", ret);
+		rtc_err(rtc, "irq registration failed: %d\n", ret);
 		return ret;
 	}
 
@@ -261,7 +271,7 @@ static struct platform_driver da9052_rtc_driver = {
 
 module_platform_driver(da9052_rtc_driver);
 
-MODULE_AUTHOR("David Dajun Chen <dchen@diasemi.com>");
+MODULE_AUTHOR("Anthony Olech <Anthony.Olech@diasemi.com>");
 MODULE_DESCRIPTION("RTC driver for Dialog DA9052 PMIC");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("platform:da9052-rtc");
diff --git a/drivers/rtc/rtc-da9063.c b/drivers/rtc/rtc-da9063.c
new file mode 100644
index 00000000000..595393098b0
--- /dev/null
+++ b/drivers/rtc/rtc-da9063.c
@@ -0,0 +1,333 @@
+/* rtc-da9063.c - Real time clock device driver for DA9063
+ * Copyright (C) 2013-14  Dialog Semiconductor Ltd.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Library General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Library General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/rtc.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/regmap.h>
+#include <linux/mfd/da9063/registers.h>
+#include <linux/mfd/da9063/core.h>
+
+#define YEARS_TO_DA9063(year)		((year) - 100)
+#define MONTHS_TO_DA9063(month)		((month) + 1)
+#define YEARS_FROM_DA9063(year)		((year) + 100)
+#define MONTHS_FROM_DA9063(month)	((month) - 1)
+
+#define RTC_DATA_LEN	(DA9063_REG_COUNT_Y - DA9063_REG_COUNT_S + 1)
+#define RTC_SEC		0
+#define RTC_MIN		1
+#define RTC_HOUR	2
+#define RTC_DAY		3
+#define RTC_MONTH	4
+#define RTC_YEAR	5
+
+struct da9063_rtc {
+	struct rtc_device	*rtc_dev;
+	struct da9063		*hw;
+	struct rtc_time		alarm_time;
+	bool			rtc_sync;
+};
+
+static void da9063_data_to_tm(u8 *data, struct rtc_time *tm)
+{
+	tm->tm_sec  = data[RTC_SEC]  & DA9063_COUNT_SEC_MASK;
+	tm->tm_min  = data[RTC_MIN]  & DA9063_COUNT_MIN_MASK;
+	tm->tm_hour = data[RTC_HOUR] & DA9063_COUNT_HOUR_MASK;
+	tm->tm_mday = data[RTC_DAY]  & DA9063_COUNT_DAY_MASK;
+	tm->tm_mon  = MONTHS_FROM_DA9063(data[RTC_MONTH] &
+					 DA9063_COUNT_MONTH_MASK);
+	tm->tm_year = YEARS_FROM_DA9063(data[RTC_YEAR] &
+					DA9063_COUNT_YEAR_MASK);
+}
+
+static void da9063_tm_to_data(struct rtc_time *tm, u8 *data)
+{
+	data[RTC_SEC] &= ~DA9063_COUNT_SEC_MASK;
+	data[RTC_SEC] |= tm->tm_sec & DA9063_COUNT_SEC_MASK;
+
+	data[RTC_MIN] &= ~DA9063_COUNT_MIN_MASK;
+	data[RTC_MIN] |= tm->tm_min & DA9063_COUNT_MIN_MASK;
+
+	data[RTC_HOUR] &= ~DA9063_COUNT_HOUR_MASK;
+	data[RTC_HOUR] |= tm->tm_hour & DA9063_COUNT_HOUR_MASK;
+
+	data[RTC_DAY] &= ~DA9063_COUNT_DAY_MASK;
+	data[RTC_DAY] |= tm->tm_mday & DA9063_COUNT_DAY_MASK;
+
+	data[RTC_MONTH] &= ~DA9063_COUNT_MONTH_MASK;
+	data[RTC_MONTH] |= MONTHS_TO_DA9063(tm->tm_mon) &
+				DA9063_COUNT_MONTH_MASK;
+
+	data[RTC_YEAR] &= ~DA9063_COUNT_YEAR_MASK;
+	data[RTC_YEAR] |= YEARS_TO_DA9063(tm->tm_year) &
+				DA9063_COUNT_YEAR_MASK;
+}
+
+static int da9063_rtc_stop_alarm(struct device *dev)
+{
+	struct da9063_rtc *rtc = dev_get_drvdata(dev);
+
+	return regmap_update_bits(rtc->hw->regmap, DA9063_REG_ALARM_Y,
+				  DA9063_ALARM_ON, 0);
+}
+
+static int da9063_rtc_start_alarm(struct device *dev)
+{
+	struct da9063_rtc *rtc = dev_get_drvdata(dev);
+
+	return regmap_update_bits(rtc->hw->regmap, DA9063_REG_ALARM_Y,
+				  DA9063_ALARM_ON, DA9063_ALARM_ON);
+}
+
+static int da9063_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	struct da9063_rtc *rtc = dev_get_drvdata(dev);
+	unsigned long tm_secs;
+	unsigned long al_secs;
+	u8 data[RTC_DATA_LEN];
+	int ret;
+
+	ret = regmap_bulk_read(rtc->hw->regmap, DA9063_REG_COUNT_S,
+			       data, RTC_DATA_LEN);
+	if (ret < 0) {
+		dev_err(dev, "Failed to read RTC time data: %d\n", ret);
+		return ret;
+	}
+
+	if (!(data[RTC_SEC] & DA9063_RTC_READ)) {
+		dev_dbg(dev, "RTC not yet ready to be read by the host\n");
+		return -EINVAL;
+	}
+
+	da9063_data_to_tm(data, tm);
+
+	rtc_tm_to_time(tm, &tm_secs);
+	rtc_tm_to_time(&rtc->alarm_time, &al_secs);
+
+	/* handle the rtc synchronisation delay */
+	if (rtc->rtc_sync == true && al_secs - tm_secs == 1)
+		memcpy(tm, &rtc->alarm_time, sizeof(struct rtc_time));
+	else
+		rtc->rtc_sync = false;
+
+	return rtc_valid_tm(tm);
+}
+
+static int da9063_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	struct da9063_rtc *rtc = dev_get_drvdata(dev);
+	u8 data[RTC_DATA_LEN];
+	int ret;
+
+	da9063_tm_to_data(tm, data);
+	ret = regmap_bulk_write(rtc->hw->regmap, DA9063_REG_COUNT_S,
+				data, RTC_DATA_LEN);
+	if (ret < 0)
+		dev_err(dev, "Failed to set RTC time data: %d\n", ret);
+
+	return ret;
+}
+
+static int da9063_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct da9063_rtc *rtc = dev_get_drvdata(dev);
+	u8 data[RTC_DATA_LEN];
+	int ret;
+	unsigned int val;
+
+	ret = regmap_bulk_read(rtc->hw->regmap, DA9063_REG_ALARM_S,
+			       &data[RTC_SEC], RTC_DATA_LEN);
+	if (ret < 0)
+		return ret;
+
+	da9063_data_to_tm(data, &alrm->time);
+
+	alrm->enabled = !!(data[RTC_YEAR] & DA9063_ALARM_ON);
+
+	ret = regmap_read(rtc->hw->regmap, DA9063_REG_EVENT_A, &val);
+	if (ret < 0)
+		return ret;
+
+	if (val & (DA9063_E_ALARM))
+		alrm->pending = 1;
+	else
+		alrm->pending = 0;
+
+	return 0;
+}
+
+static int da9063_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct da9063_rtc *rtc = dev_get_drvdata(dev);
+	u8 data[RTC_DATA_LEN];
+	int ret;
+
+	da9063_tm_to_data(&alrm->time, data);
+
+	ret = da9063_rtc_stop_alarm(dev);
+	if (ret < 0) {
+		dev_err(dev, "Failed to stop alarm: %d\n", ret);
+		return ret;
+	}
+
+	ret = regmap_bulk_write(rtc->hw->regmap, DA9063_REG_ALARM_S,
+				data, RTC_DATA_LEN);
+	if (ret < 0) {
+		dev_err(dev, "Failed to write alarm: %d\n", ret);
+		return ret;
+	}
+
+	rtc->alarm_time = alrm->time;
+
+	if (alrm->enabled) {
+		ret = da9063_rtc_start_alarm(dev);
+		if (ret < 0) {
+			dev_err(dev, "Failed to start alarm: %d\n", ret);
+			return ret;
+		}
+	}
+
+	return ret;
+}
+
+static int da9063_rtc_alarm_irq_enable(struct device *dev, unsigned int enabled)
+{
+	if (enabled)
+		return da9063_rtc_start_alarm(dev);
+	else
+		return da9063_rtc_stop_alarm(dev);
+}
+
+static irqreturn_t da9063_alarm_event(int irq, void *data)
+{
+	struct da9063_rtc *rtc = data;
+
+	regmap_update_bits(rtc->hw->regmap, DA9063_REG_ALARM_Y,
+			   DA9063_ALARM_ON, 0);
+
+	rtc->rtc_sync = true;
+	rtc_update_irq(rtc->rtc_dev, 1, RTC_IRQF | RTC_AF);
+
+	return IRQ_HANDLED;
+}
+
+static const struct rtc_class_ops da9063_rtc_ops = {
+	.read_time = da9063_rtc_read_time,
+	.set_time = da9063_rtc_set_time,
+	.read_alarm = da9063_rtc_read_alarm,
+	.set_alarm = da9063_rtc_set_alarm,
+	.alarm_irq_enable = da9063_rtc_alarm_irq_enable,
+};
+
+static int da9063_rtc_probe(struct platform_device *pdev)
+{
+	struct da9063 *da9063 = dev_get_drvdata(pdev->dev.parent);
+	struct da9063_rtc *rtc;
+	int irq_alarm;
+	u8 data[RTC_DATA_LEN];
+	int ret;
+
+	ret = regmap_update_bits(da9063->regmap, DA9063_REG_CONTROL_E,
+				 DA9063_RTC_EN, DA9063_RTC_EN);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to enable RTC\n");
+		goto err;
+	}
+
+	ret = regmap_update_bits(da9063->regmap, DA9063_REG_EN_32K,
+				 DA9063_CRYSTAL, DA9063_CRYSTAL);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to run 32kHz oscillator\n");
+		goto err;
+	}
+
+	ret = regmap_update_bits(da9063->regmap, DA9063_REG_ALARM_S,
+			DA9063_ALARM_STATUS_TICK | DA9063_ALARM_STATUS_ALARM,
+			0);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to access RTC alarm register\n");
+		goto err;
+	}
+
+	ret = regmap_update_bits(da9063->regmap, DA9063_REG_ALARM_S,
+				 DA9063_ALARM_STATUS_ALARM,
+				 DA9063_ALARM_STATUS_ALARM);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to access RTC alarm register\n");
+		goto err;
+	}
+
+	ret = regmap_update_bits(da9063->regmap, DA9063_REG_ALARM_Y,
+				 DA9063_TICK_ON, 0);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to disable TICKs\n");
+		goto err;
+	}
+
+	ret = regmap_bulk_read(da9063->regmap, DA9063_REG_ALARM_S,
+			       data, RTC_DATA_LEN);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to read initial alarm data: %d\n",
+			ret);
+		goto err;
+	}
+
+	rtc = devm_kzalloc(&pdev->dev, sizeof(*rtc), GFP_KERNEL);
+	if (!rtc)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, rtc);
+
+	irq_alarm = platform_get_irq_byname(pdev, "ALARM");
+	ret = devm_request_threaded_irq(&pdev->dev, irq_alarm, NULL,
+					da9063_alarm_event,
+					IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+					"ALARM", rtc);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to request ALARM IRQ %d: %d\n",
+			irq_alarm, ret);
+		goto err;
+	}
+
+	rtc->hw = da9063;
+	rtc->rtc_dev = devm_rtc_device_register(&pdev->dev, DA9063_DRVNAME_RTC,
+					   &da9063_rtc_ops, THIS_MODULE);
+	if (IS_ERR(rtc->rtc_dev))
+		return PTR_ERR(rtc->rtc_dev);
+
+	da9063_data_to_tm(data, &rtc->alarm_time);
+	rtc->rtc_sync = false;
+err:
+	return ret;
+}
+
+static struct platform_driver da9063_rtc_driver = {
+	.probe		= da9063_rtc_probe,
+	.driver		= {
+		.name	= DA9063_DRVNAME_RTC,
+		.owner	= THIS_MODULE,
+	},
+};
+
+module_platform_driver(da9063_rtc_driver);
+
+MODULE_AUTHOR("S Twiss <stwiss.opensource@diasemi.com>");
+MODULE_DESCRIPTION("Real time clock device driver for Dialog DA9063");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:" DA9063_DRVNAME_RTC);
diff --git a/drivers/rtc/rtc-ds1343.c b/drivers/rtc/rtc-ds1343.c
new file mode 100644
index 00000000000..c3719189dd9
--- /dev/null
+++ b/drivers/rtc/rtc-ds1343.c
@@ -0,0 +1,689 @@
+/* rtc-ds1343.c
+ *
+ * Driver for Dallas Semiconductor DS1343 Low Current, SPI Compatible
+ * Real Time Clock
+ *
+ * Author : Raghavendra Chandra Ganiga <ravi23ganiga@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/init.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/device.h>
+#include <linux/spi/spi.h>
+#include <linux/regmap.h>
+#include <linux/rtc.h>
+#include <linux/bcd.h>
+#include <linux/pm.h>
+#include <linux/slab.h>
+
+#define DS1343_DRV_VERSION	"01.00"
+#define DALLAS_MAXIM_DS1343	0
+#define DALLAS_MAXIM_DS1344	1
+
+/* RTC DS1343 Registers */
+#define DS1343_SECONDS_REG	0x00
+#define DS1343_MINUTES_REG	0x01
+#define DS1343_HOURS_REG	0x02
+#define DS1343_DAY_REG		0x03
+#define DS1343_DATE_REG		0x04
+#define DS1343_MONTH_REG	0x05
+#define DS1343_YEAR_REG		0x06
+#define DS1343_ALM0_SEC_REG	0x07
+#define DS1343_ALM0_MIN_REG	0x08
+#define DS1343_ALM0_HOUR_REG	0x09
+#define DS1343_ALM0_DAY_REG	0x0A
+#define DS1343_ALM1_SEC_REG	0x0B
+#define DS1343_ALM1_MIN_REG	0x0C
+#define DS1343_ALM1_HOUR_REG	0x0D
+#define DS1343_ALM1_DAY_REG	0x0E
+#define DS1343_CONTROL_REG	0x0F
+#define DS1343_STATUS_REG	0x10
+#define DS1343_TRICKLE_REG	0x11
+
+/* DS1343 Control Registers bits */
+#define DS1343_EOSC		0x80
+#define DS1343_DOSF		0x20
+#define DS1343_EGFIL		0x10
+#define DS1343_SQW		0x08
+#define DS1343_INTCN		0x04
+#define DS1343_A1IE		0x02
+#define DS1343_A0IE		0x01
+
+/* DS1343 Status Registers bits */
+#define DS1343_OSF		0x80
+#define DS1343_IRQF1		0x02
+#define DS1343_IRQF0		0x01
+
+/* DS1343 Trickle Charger Registers bits */
+#define DS1343_TRICKLE_MAGIC	0xa0
+#define DS1343_TRICKLE_DS1	0x08
+#define DS1343_TRICKLE_1K	0x01
+#define DS1343_TRICKLE_2K	0x02
+#define DS1343_TRICKLE_4K	0x03
+
+static const struct spi_device_id ds1343_id[] = {
+	{ "ds1343", DALLAS_MAXIM_DS1343 },
+	{ "ds1344", DALLAS_MAXIM_DS1344 },
+	{ }
+};
+MODULE_DEVICE_TABLE(spi, ds1343_id);
+
+struct ds1343_priv {
+	struct spi_device *spi;
+	struct rtc_device *rtc;
+	struct regmap *map;
+	struct mutex mutex;
+	unsigned int irqen;
+	int irq;
+	int alarm_sec;
+	int alarm_min;
+	int alarm_hour;
+	int alarm_mday;
+};
+
+static int ds1343_ioctl(struct device *dev, unsigned int cmd, unsigned long arg)
+{
+	switch (cmd) {
+#ifdef RTC_SET_CHARGE
+	case RTC_SET_CHARGE:
+	{
+		int val;
+
+		if (copy_from_user(&val, (int __user *)arg, sizeof(int)))
+			return -EFAULT;
+
+		return regmap_write(priv->map, DS1343_TRICKLE_REG, val);
+	}
+	break;
+#endif
+	}
+
+	return -ENOIOCTLCMD;
+}
+
+static ssize_t ds1343_show_glitchfilter(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct ds1343_priv *priv = dev_get_drvdata(dev);
+	int glitch_filt_status, data;
+
+	regmap_read(priv->map, DS1343_CONTROL_REG, &data);
+
+	glitch_filt_status = !!(data & DS1343_EGFIL);
+
+	if (glitch_filt_status)
+		return sprintf(buf, "enabled\n");
+	else
+		return sprintf(buf, "disabled\n");
+}
+
+static ssize_t ds1343_store_glitchfilter(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct ds1343_priv *priv = dev_get_drvdata(dev);
+	int data;
+
+	regmap_read(priv->map, DS1343_CONTROL_REG, &data);
+
+	if (strncmp(buf, "enabled", 7) == 0)
+		data |= DS1343_EGFIL;
+
+	else if (strncmp(buf, "disabled", 8) == 0)
+		data &= ~(DS1343_EGFIL);
+
+	else
+		return -EINVAL;
+
+	regmap_write(priv->map, DS1343_CONTROL_REG, data);
+
+	return count;
+}
+
+static DEVICE_ATTR(glitch_filter, S_IRUGO | S_IWUSR, ds1343_show_glitchfilter,
+			ds1343_store_glitchfilter);
+
+static ssize_t ds1343_show_alarmstatus(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct ds1343_priv *priv = dev_get_drvdata(dev);
+	int alarmstatus, data;
+
+	regmap_read(priv->map, DS1343_CONTROL_REG, &data);
+
+	alarmstatus = !!(data & DS1343_A0IE);
+
+	if (alarmstatus)
+		return sprintf(buf, "enabled\n");
+	else
+		return sprintf(buf, "disabled\n");
+}
+
+static DEVICE_ATTR(alarm_status, S_IRUGO, ds1343_show_alarmstatus, NULL);
+
+static ssize_t ds1343_show_alarmmode(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct ds1343_priv *priv = dev_get_drvdata(dev);
+	int alarm_mode, data;
+	char *alarm_str;
+
+	regmap_read(priv->map, DS1343_ALM0_SEC_REG, &data);
+	alarm_mode = (data & 0x80) >> 4;
+
+	regmap_read(priv->map, DS1343_ALM0_MIN_REG, &data);
+	alarm_mode |= (data & 0x80) >> 5;
+
+	regmap_read(priv->map, DS1343_ALM0_HOUR_REG, &data);
+	alarm_mode |= (data & 0x80) >> 6;
+
+	regmap_read(priv->map, DS1343_ALM0_DAY_REG, &data);
+	alarm_mode |= (data & 0x80) >> 7;
+
+	switch (alarm_mode) {
+	case 15:
+		alarm_str = "each second";
+		break;
+
+	case 7:
+		alarm_str = "seconds match";
+		break;
+
+	case 3:
+		alarm_str = "minutes and seconds match";
+		break;
+
+	case 1:
+		alarm_str = "hours, minutes and seconds match";
+		break;
+
+	case 0:
+		alarm_str = "day, hours, minutes and seconds match";
+		break;
+
+	default:
+		alarm_str = "invalid";
+		break;
+	}
+
+	return sprintf(buf, "%s\n", alarm_str);
+}
+
+static DEVICE_ATTR(alarm_mode, S_IRUGO, ds1343_show_alarmmode, NULL);
+
+static ssize_t ds1343_show_tricklecharger(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct ds1343_priv *priv = dev_get_drvdata(dev);
+	int data;
+	char *diodes = "disabled", *resistors = " ";
+
+	regmap_read(priv->map, DS1343_TRICKLE_REG, &data);
+
+	if ((data & 0xf0) == DS1343_TRICKLE_MAGIC) {
+		switch (data & 0x0c) {
+		case DS1343_TRICKLE_DS1:
+			diodes = "one diode,";
+			break;
+
+		default:
+			diodes = "no diode,";
+			break;
+		}
+
+		switch (data & 0x03) {
+		case DS1343_TRICKLE_1K:
+			resistors = "1k Ohm";
+			break;
+
+		case DS1343_TRICKLE_2K:
+			resistors = "2k Ohm";
+			break;
+
+		case DS1343_TRICKLE_4K:
+			resistors = "4k Ohm";
+			break;
+
+		default:
+			diodes = "disabled";
+			break;
+		}
+	}
+
+	return sprintf(buf, "%s %s\n", diodes, resistors);
+}
+
+static DEVICE_ATTR(trickle_charger, S_IRUGO, ds1343_show_tricklecharger, NULL);
+
+static int ds1343_sysfs_register(struct device *dev)
+{
+	struct ds1343_priv *priv = dev_get_drvdata(dev);
+	int err;
+
+	err = device_create_file(dev, &dev_attr_glitch_filter);
+	if (err)
+		return err;
+
+	err = device_create_file(dev, &dev_attr_trickle_charger);
+	if (err)
+		goto error1;
+
+	if (priv->irq <= 0)
+		return err;
+
+	err = device_create_file(dev, &dev_attr_alarm_mode);
+	if (err)
+		goto error2;
+
+	err = device_create_file(dev, &dev_attr_alarm_status);
+	if (!err)
+		return err;
+
+	device_remove_file(dev, &dev_attr_alarm_mode);
+
+error2:
+	device_remove_file(dev, &dev_attr_trickle_charger);
+
+error1:
+	device_remove_file(dev, &dev_attr_glitch_filter);
+
+	return err;
+}
+
+static void ds1343_sysfs_unregister(struct device *dev)
+{
+	struct ds1343_priv *priv = dev_get_drvdata(dev);
+
+	device_remove_file(dev, &dev_attr_glitch_filter);
+	device_remove_file(dev, &dev_attr_trickle_charger);
+
+	if (priv->irq <= 0)
+		return;
+
+	device_remove_file(dev, &dev_attr_alarm_status);
+	device_remove_file(dev, &dev_attr_alarm_mode);
+}
+
+static int ds1343_read_time(struct device *dev, struct rtc_time *dt)
+{
+	struct ds1343_priv *priv = dev_get_drvdata(dev);
+	unsigned char buf[7];
+	int res;
+
+	res = regmap_bulk_read(priv->map, DS1343_SECONDS_REG, buf, 7);
+	if (res)
+		return res;
+
+	dt->tm_sec	= bcd2bin(buf[0]);
+	dt->tm_min	= bcd2bin(buf[1]);
+	dt->tm_hour	= bcd2bin(buf[2] & 0x3F);
+	dt->tm_wday	= bcd2bin(buf[3]) - 1;
+	dt->tm_mday	= bcd2bin(buf[4]);
+	dt->tm_mon	= bcd2bin(buf[5] & 0x1F) - 1;
+	dt->tm_year	= bcd2bin(buf[6]) + 100; /* year offset from 1900 */
+
+	return rtc_valid_tm(dt);
+}
+
+static int ds1343_set_time(struct device *dev, struct rtc_time *dt)
+{
+	struct ds1343_priv *priv = dev_get_drvdata(dev);
+	int res;
+
+	res = regmap_write(priv->map, DS1343_SECONDS_REG,
+				bin2bcd(dt->tm_sec));
+	if (res)
+		return res;
+
+	res = regmap_write(priv->map, DS1343_MINUTES_REG,
+				bin2bcd(dt->tm_min));
+	if (res)
+		return res;
+
+	res = regmap_write(priv->map, DS1343_HOURS_REG,
+				bin2bcd(dt->tm_hour) & 0x3F);
+	if (res)
+		return res;
+
+	res = regmap_write(priv->map, DS1343_DAY_REG,
+				bin2bcd(dt->tm_wday + 1));
+	if (res)
+		return res;
+
+	res = regmap_write(priv->map, DS1343_DATE_REG,
+				bin2bcd(dt->tm_mday));
+	if (res)
+		return res;
+
+	res = regmap_write(priv->map, DS1343_MONTH_REG,
+				bin2bcd(dt->tm_mon + 1));
+	if (res)
+		return res;
+
+	dt->tm_year %= 100;
+
+	res = regmap_write(priv->map, DS1343_YEAR_REG,
+				bin2bcd(dt->tm_year));
+	if (res)
+		return res;
+
+	return 0;
+}
+
+static int ds1343_update_alarm(struct device *dev)
+{
+	struct ds1343_priv *priv = dev_get_drvdata(dev);
+	unsigned int control, stat;
+	unsigned char buf[4];
+	int res = 0;
+
+	res = regmap_read(priv->map, DS1343_CONTROL_REG, &control);
+	if (res)
+		return res;
+
+	res = regmap_read(priv->map, DS1343_STATUS_REG, &stat);
+	if (res)
+		return res;
+
+	control &= ~(DS1343_A0IE);
+	stat &= ~(DS1343_IRQF0);
+
+	res = regmap_write(priv->map, DS1343_CONTROL_REG, control);
+	if (res)
+		return res;
+
+	res = regmap_write(priv->map, DS1343_STATUS_REG, stat);
+	if (res)
+		return res;
+
+	buf[0] = priv->alarm_sec < 0 || (priv->irqen & RTC_UF) ?
+		0x80 : bin2bcd(priv->alarm_sec) & 0x7F;
+	buf[1] = priv->alarm_min < 0 || (priv->irqen & RTC_UF) ?
+		0x80 : bin2bcd(priv->alarm_min) & 0x7F;
+	buf[2] = priv->alarm_hour < 0 || (priv->irqen & RTC_UF) ?
+		0x80 : bin2bcd(priv->alarm_hour) & 0x3F;
+	buf[3] = priv->alarm_mday < 0 || (priv->irqen & RTC_UF) ?
+		0x80 : bin2bcd(priv->alarm_mday) & 0x7F;
+
+	res = regmap_bulk_write(priv->map, DS1343_ALM0_SEC_REG, buf, 4);
+	if (res)
+		return res;
+
+	if (priv->irqen) {
+		control |= DS1343_A0IE;
+		res = regmap_write(priv->map, DS1343_CONTROL_REG, control);
+	}
+
+	return res;
+}
+
+static int ds1343_read_alarm(struct device *dev, struct rtc_wkalrm *alarm)
+{
+	struct ds1343_priv *priv = dev_get_drvdata(dev);
+	int res = 0;
+	unsigned int stat;
+
+	if (priv->irq <= 0)
+		return -EINVAL;
+
+	mutex_lock(&priv->mutex);
+
+	res = regmap_read(priv->map, DS1343_STATUS_REG, &stat);
+	if (res)
+		goto out;
+
+	alarm->enabled = !!(priv->irqen & RTC_AF);
+	alarm->pending = !!(stat & DS1343_IRQF0);
+
+	alarm->time.tm_sec = priv->alarm_sec < 0 ? 0 : priv->alarm_sec;
+	alarm->time.tm_min = priv->alarm_min < 0 ? 0 : priv->alarm_min;
+	alarm->time.tm_hour = priv->alarm_hour < 0 ? 0 : priv->alarm_hour;
+	alarm->time.tm_mday = priv->alarm_mday < 0 ? 0 : priv->alarm_mday;
+
+	alarm->time.tm_mon = -1;
+	alarm->time.tm_year = -1;
+	alarm->time.tm_wday = -1;
+	alarm->time.tm_yday = -1;
+	alarm->time.tm_isdst = -1;
+
+out:
+	mutex_unlock(&priv->mutex);
+	return res;
+}
+
+static int ds1343_set_alarm(struct device *dev, struct rtc_wkalrm *alarm)
+{
+	struct ds1343_priv *priv = dev_get_drvdata(dev);
+	int res = 0;
+
+	if (priv->irq <= 0)
+		return -EINVAL;
+
+	mutex_lock(&priv->mutex);
+
+	priv->alarm_sec = alarm->time.tm_sec;
+	priv->alarm_min = alarm->time.tm_min;
+	priv->alarm_hour = alarm->time.tm_hour;
+	priv->alarm_mday = alarm->time.tm_mday;
+
+	if (alarm->enabled)
+		priv->irqen |= RTC_AF;
+
+	res = ds1343_update_alarm(dev);
+
+	mutex_unlock(&priv->mutex);
+
+	return res;
+}
+
+static int ds1343_alarm_irq_enable(struct device *dev, unsigned int enabled)
+{
+	struct ds1343_priv *priv = dev_get_drvdata(dev);
+	int res = 0;
+
+	if (priv->irq <= 0)
+		return -EINVAL;
+
+	mutex_lock(&priv->mutex);
+
+	if (enabled)
+		priv->irqen |= RTC_AF;
+	else
+		priv->irqen &= ~RTC_AF;
+
+	res = ds1343_update_alarm(dev);
+
+	mutex_unlock(&priv->mutex);
+
+	return res;
+}
+
+static irqreturn_t ds1343_thread(int irq, void *dev_id)
+{
+	struct ds1343_priv *priv = dev_id;
+	unsigned int stat, control;
+	int res = 0;
+
+	mutex_lock(&priv->mutex);
+
+	res = regmap_read(priv->map, DS1343_STATUS_REG, &stat);
+	if (res)
+		goto out;
+
+	if (stat & DS1343_IRQF0) {
+		stat &= ~DS1343_IRQF0;
+		regmap_write(priv->map, DS1343_STATUS_REG, stat);
+
+		res = regmap_read(priv->map, DS1343_CONTROL_REG, &control);
+		if (res)
+			goto out;
+
+		control &= ~DS1343_A0IE;
+		regmap_write(priv->map, DS1343_CONTROL_REG, control);
+
+		rtc_update_irq(priv->rtc, 1, RTC_AF | RTC_IRQF);
+	}
+
+out:
+	mutex_unlock(&priv->mutex);
+	return IRQ_HANDLED;
+}
+
+static const struct rtc_class_ops ds1343_rtc_ops = {
+	.ioctl		= ds1343_ioctl,
+	.read_time	= ds1343_read_time,
+	.set_time	= ds1343_set_time,
+	.read_alarm	= ds1343_read_alarm,
+	.set_alarm	= ds1343_set_alarm,
+	.alarm_irq_enable = ds1343_alarm_irq_enable,
+};
+
+static int ds1343_probe(struct spi_device *spi)
+{
+	struct ds1343_priv *priv;
+	struct regmap_config config;
+	unsigned int data;
+	int res;
+
+	memset(&config, 0, sizeof(config));
+	config.reg_bits = 8;
+	config.val_bits = 8;
+	config.write_flag_mask = 0x80;
+
+	priv = devm_kzalloc(&spi->dev, sizeof(struct ds1343_priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->spi = spi;
+	mutex_init(&priv->mutex);
+
+	/* RTC DS1347 works in spi mode 3 and
+	 * its chip select is active high
+	 */
+	spi->mode = SPI_MODE_3 | SPI_CS_HIGH;
+	spi->bits_per_word = 8;
+	res = spi_setup(spi);
+	if (res)
+		return res;
+
+	spi_set_drvdata(spi, priv);
+
+	priv->map = devm_regmap_init_spi(spi, &config);
+
+	if (IS_ERR(priv->map)) {
+		dev_err(&spi->dev, "spi regmap init failed for rtc ds1343\n");
+		return PTR_ERR(priv->map);
+	}
+
+	res = regmap_read(priv->map, DS1343_SECONDS_REG, &data);
+	if (res)
+		return res;
+
+	regmap_read(priv->map, DS1343_CONTROL_REG, &data);
+	data |= DS1343_INTCN;
+	data &= ~(DS1343_EOSC | DS1343_A1IE | DS1343_A0IE);
+	regmap_write(priv->map, DS1343_CONTROL_REG, data);
+
+	regmap_read(priv->map, DS1343_STATUS_REG, &data);
+	data &= ~(DS1343_OSF | DS1343_IRQF1 | DS1343_IRQF0);
+	regmap_write(priv->map, DS1343_STATUS_REG, data);
+
+	priv->rtc = devm_rtc_device_register(&spi->dev, "ds1343",
+					&ds1343_rtc_ops, THIS_MODULE);
+	if (IS_ERR(priv->rtc)) {
+		dev_err(&spi->dev, "unable to register rtc ds1343\n");
+		return PTR_ERR(priv->rtc);
+	}
+
+	priv->irq = spi->irq;
+
+	if (priv->irq >= 0) {
+		res = devm_request_threaded_irq(&spi->dev, spi->irq, NULL,
+						ds1343_thread,
+						IRQF_NO_SUSPEND | IRQF_ONESHOT,
+						"ds1343", priv);
+		if (res) {
+			priv->irq = -1;
+			dev_err(&spi->dev,
+				"unable to request irq for rtc ds1343\n");
+		} else {
+			device_set_wakeup_capable(&spi->dev, 1);
+		}
+	}
+
+	res = ds1343_sysfs_register(&spi->dev);
+	if (res)
+		dev_err(&spi->dev,
+			"unable to create sysfs entries for rtc ds1343\n");
+
+	return 0;
+}
+
+static int ds1343_remove(struct spi_device *spi)
+{
+	struct ds1343_priv *priv = spi_get_drvdata(spi);
+
+	if (spi->irq) {
+		mutex_lock(&priv->mutex);
+		priv->irqen &= ~RTC_AF;
+		mutex_unlock(&priv->mutex);
+
+		devm_free_irq(&spi->dev, spi->irq, priv);
+	}
+
+	spi_set_drvdata(spi, NULL);
+
+	ds1343_sysfs_unregister(&spi->dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+
+static int ds1343_suspend(struct device *dev)
+{
+	struct spi_device *spi = to_spi_device(dev);
+
+	if (spi->irq >= 0 && device_may_wakeup(dev))
+		enable_irq_wake(spi->irq);
+
+	return 0;
+}
+
+static int ds1343_resume(struct device *dev)
+{
+	struct spi_device *spi = to_spi_device(dev);
+
+	if (spi->irq >= 0 && device_may_wakeup(dev))
+		disable_irq_wake(spi->irq);
+
+	return 0;
+}
+
+#endif
+
+static SIMPLE_DEV_PM_OPS(ds1343_pm, ds1343_suspend, ds1343_resume);
+
+static struct spi_driver ds1343_driver = {
+	.driver = {
+		.name = "ds1343",
+		.owner = THIS_MODULE,
+		.pm = &ds1343_pm,
+	},
+	.probe = ds1343_probe,
+	.remove = ds1343_remove,
+	.id_table = ds1343_id,
+};
+
+module_spi_driver(ds1343_driver);
+
+MODULE_DESCRIPTION("DS1343 RTC SPI Driver");
+MODULE_AUTHOR("Raghavendra Chandra Ganiga <ravi23ganiga@gmail.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_VERSION(DS1343_DRV_VERSION);
diff --git a/drivers/rtc/rtc-ds1742.c b/drivers/rtc/rtc-ds1742.c
index 942103dac30..c6b2191a412 100644
--- a/drivers/rtc/rtc-ds1742.c
+++ b/drivers/rtc/rtc-ds1742.c
@@ -219,7 +219,7 @@ static int ds1742_rtc_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id __maybe_unused ds1742_rtc_of_match[] = {
+static const struct of_device_id __maybe_unused ds1742_rtc_of_match[] = {
 	{ .compatible = "maxim,ds1742", },
 	{ }
 };
diff --git a/drivers/rtc/rtc-efi.c b/drivers/rtc/rtc-efi.c
index 797aa0252ba..c4c38431012 100644
--- a/drivers/rtc/rtc-efi.c
+++ b/drivers/rtc/rtc-efi.c
@@ -35,7 +35,7 @@ static inline int
 compute_yday(efi_time_t *eft)
 {
 	/* efi_time_t.month is in the [1-12] so, we need -1 */
-	return rtc_year_days(eft->day - 1, eft->month - 1, eft->year);
+	return rtc_year_days(eft->day, eft->month - 1, eft->year);
 }
 /*
  * returns day of the week [0-6] 0=Sunday
diff --git a/drivers/rtc/rtc-hym8563.c b/drivers/rtc/rtc-hym8563.c
index bd628a6f981..b936bb4096b 100644
--- a/drivers/rtc/rtc-hym8563.c
+++ b/drivers/rtc/rtc-hym8563.c
@@ -418,6 +418,9 @@ static struct clk *hym8563_clkout_register_clk(struct hym8563 *hym8563)
 	init.num_parents = 0;
 	hym8563->clkout_hw.init = &init;
 
+	/* optional override of the clockname */
+	of_property_read_string(node, "clock-output-names", &init.name);
+
 	/* register the clock */
 	clk = clk_register(&client->dev, &hym8563->clkout_hw);
 
@@ -569,6 +572,9 @@ static int hym8563_probe(struct i2c_client *client,
 	if (IS_ERR(hym8563->rtc))
 		return PTR_ERR(hym8563->rtc);
 
+	/* the hym8563 alarm only supports a minute accuracy */
+	hym8563->rtc->uie_unsupported = 1;
+
 #ifdef CONFIG_COMMON_CLK
 	hym8563_clkout_register_clk(hym8563);
 #endif
@@ -582,7 +588,7 @@ static const struct i2c_device_id hym8563_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, hym8563_id);
 
-static struct of_device_id hym8563_dt_idtable[] = {
+static const struct of_device_id hym8563_dt_idtable[] = {
 	{ .compatible = "haoyu,hym8563" },
 	{},
 };
diff --git a/drivers/rtc/rtc-isl12057.c b/drivers/rtc/rtc-isl12057.c
index 41bd76aaff7..455b601d731 100644
--- a/drivers/rtc/rtc-isl12057.c
+++ b/drivers/rtc/rtc-isl12057.c
@@ -278,7 +278,7 @@ static int isl12057_probe(struct i2c_client *client,
 }
 
 #ifdef CONFIG_OF
-static struct of_device_id isl12057_dt_match[] = {
+static const struct of_device_id isl12057_dt_match[] = {
 	{ .compatible = "isl,isl12057" },
 	{ },
 };
diff --git a/drivers/rtc/rtc-m41t80.c b/drivers/rtc/rtc-m41t80.c
index a5248aa1abf..7ff7427c2e6 100644
--- a/drivers/rtc/rtc-m41t80.c
+++ b/drivers/rtc/rtc-m41t80.c
@@ -66,8 +66,6 @@
 #define M41T80_FEATURE_WD	(1 << 3)	/* Extra watchdog resolution */
 #define M41T80_FEATURE_SQ_ALT	(1 << 4)	/* RSx bits are in reg 4 */
 
-#define DRV_VERSION "0.05"
-
 static DEFINE_MUTEX(m41t80_rtc_mutex);
 static const struct i2c_device_id m41t80_id[] = {
 	{ "m41t62", M41T80_FEATURE_SQ | M41T80_FEATURE_SQ_ALT },
@@ -80,6 +78,7 @@ static const struct i2c_device_id m41t80_id[] = {
 	{ "m41st84", M41T80_FEATURE_HT | M41T80_FEATURE_BL | M41T80_FEATURE_SQ },
 	{ "m41st85", M41T80_FEATURE_HT | M41T80_FEATURE_BL | M41T80_FEATURE_SQ },
 	{ "m41st87", M41T80_FEATURE_HT | M41T80_FEATURE_BL | M41T80_FEATURE_SQ },
+	{ "rv4162", M41T80_FEATURE_SQ | M41T80_FEATURE_WD | M41T80_FEATURE_SQ_ALT },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, m41t80_id);
@@ -232,7 +231,7 @@ static ssize_t m41t80_sysfs_show_flags(struct device *dev,
 
 	val = i2c_smbus_read_byte_data(client, M41T80_REG_FLAGS);
 	if (val < 0)
-		return -EIO;
+		return val;
 	return sprintf(buf, "%#x\n", val);
 }
 static DEVICE_ATTR(flags, S_IRUGO, m41t80_sysfs_show_flags, NULL);
@@ -252,7 +251,7 @@ static ssize_t m41t80_sysfs_show_sqwfreq(struct device *dev,
 		reg_sqw = M41T80_REG_WDAY;
 	val = i2c_smbus_read_byte_data(client, reg_sqw);
 	if (val < 0)
-		return -EIO;
+		return val;
 	val = (val >> 4) & 0xf;
 	switch (val) {
 	case 0:
@@ -271,7 +270,7 @@ static ssize_t m41t80_sysfs_set_sqwfreq(struct device *dev,
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct m41t80_data *clientdata = i2c_get_clientdata(client);
-	int almon, sqw, reg_sqw;
+	int almon, sqw, reg_sqw, rc;
 	int val = simple_strtoul(buf, NULL, 0);
 
 	if (!(clientdata->features & M41T80_FEATURE_SQ))
@@ -291,21 +290,30 @@ static ssize_t m41t80_sysfs_set_sqwfreq(struct device *dev,
 	/* disable SQW, set SQW frequency & re-enable */
 	almon = i2c_smbus_read_byte_data(client, M41T80_REG_ALARM_MON);
 	if (almon < 0)
-		return -EIO;
+		return almon;
 	reg_sqw = M41T80_REG_SQW;
 	if (clientdata->features & M41T80_FEATURE_SQ_ALT)
 		reg_sqw = M41T80_REG_WDAY;
 	sqw = i2c_smbus_read_byte_data(client, reg_sqw);
 	if (sqw < 0)
-		return -EIO;
+		return sqw;
 	sqw = (sqw & 0x0f) | (val << 4);
-	if (i2c_smbus_write_byte_data(client, M41T80_REG_ALARM_MON,
-				      almon & ~M41T80_ALMON_SQWE) < 0 ||
-	    i2c_smbus_write_byte_data(client, reg_sqw, sqw) < 0)
-		return -EIO;
-	if (val && i2c_smbus_write_byte_data(client, M41T80_REG_ALARM_MON,
-					     almon | M41T80_ALMON_SQWE) < 0)
-		return -EIO;
+
+	rc = i2c_smbus_write_byte_data(client, M41T80_REG_ALARM_MON,
+				      almon & ~M41T80_ALMON_SQWE);
+	if (rc < 0)
+		return rc;
+
+	if (val) {
+		rc = i2c_smbus_write_byte_data(client, reg_sqw, sqw);
+		if (rc < 0)
+			return rc;
+
+		rc = i2c_smbus_write_byte_data(client, M41T80_REG_ALARM_MON,
+					     almon | M41T80_ALMON_SQWE);
+		if (rc <0)
+			return rc;
+	}
 	return count;
 }
 static DEVICE_ATTR(sqwfreq, S_IRUGO | S_IWUSR,
@@ -629,40 +637,28 @@ static int m41t80_probe(struct i2c_client *client,
 	struct m41t80_data *clientdata = NULL;
 
 	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C
-				     | I2C_FUNC_SMBUS_BYTE_DATA)) {
-		rc = -ENODEV;
-		goto exit;
-	}
-
-	dev_info(&client->dev,
-		 "chip found, driver version " DRV_VERSION "\n");
+				     | I2C_FUNC_SMBUS_BYTE_DATA))
+		return -ENODEV;
 
 	clientdata = devm_kzalloc(&client->dev, sizeof(*clientdata),
 				GFP_KERNEL);
-	if (!clientdata) {
-		rc = -ENOMEM;
-		goto exit;
-	}
+	if (!clientdata)
+		return -ENOMEM;
 
 	clientdata->features = id->driver_data;
 	i2c_set_clientdata(client, clientdata);
 
 	rtc = devm_rtc_device_register(&client->dev, client->name,
 					&m41t80_rtc_ops, THIS_MODULE);
-	if (IS_ERR(rtc)) {
-		rc = PTR_ERR(rtc);
-		rtc = NULL;
-		goto exit;
-	}
+	if (IS_ERR(rtc))
+		return PTR_ERR(rtc);
 
 	clientdata->rtc = rtc;
 
 	/* Make sure HT (Halt Update) bit is cleared */
 	rc = i2c_smbus_read_byte_data(client, M41T80_REG_ALARM_HOUR);
-	if (rc < 0)
-		goto ht_err;
 
-	if (rc & M41T80_ALHOUR_HT) {
+	if (rc >= 0 && rc & M41T80_ALHOUR_HT) {
 		if (clientdata->features & M41T80_FEATURE_HT) {
 			m41t80_get_datetime(client, &tm);
 			dev_info(&client->dev, "HT bit was set!\n");
@@ -673,53 +669,44 @@ static int m41t80_probe(struct i2c_client *client,
 				 tm.tm_mon + 1, tm.tm_mday, tm.tm_hour,
 				 tm.tm_min, tm.tm_sec);
 		}
-		if (i2c_smbus_write_byte_data(client,
-					      M41T80_REG_ALARM_HOUR,
-					      rc & ~M41T80_ALHOUR_HT) < 0)
-			goto ht_err;
+		rc = i2c_smbus_write_byte_data(client, M41T80_REG_ALARM_HOUR,
+					      rc & ~M41T80_ALHOUR_HT);
+	}
+
+	if (rc < 0) {
+		dev_err(&client->dev, "Can't clear HT bit\n");
+		return rc;
 	}
 
 	/* Make sure ST (stop) bit is cleared */
 	rc = i2c_smbus_read_byte_data(client, M41T80_REG_SEC);
-	if (rc < 0)
-		goto st_err;
 
-	if (rc & M41T80_SEC_ST) {
-		if (i2c_smbus_write_byte_data(client, M41T80_REG_SEC,
-					      rc & ~M41T80_SEC_ST) < 0)
-			goto st_err;
+	if (rc >= 0 && rc & M41T80_SEC_ST)
+		rc = i2c_smbus_write_byte_data(client, M41T80_REG_SEC,
+					      rc & ~M41T80_SEC_ST);
+	if (rc < 0) {
+		dev_err(&client->dev, "Can't clear ST bit\n");
+		return rc;
 	}
 
 	rc = m41t80_sysfs_register(&client->dev);
 	if (rc)
-		goto exit;
+		return rc;
 
 #ifdef CONFIG_RTC_DRV_M41T80_WDT
 	if (clientdata->features & M41T80_FEATURE_HT) {
 		save_client = client;
 		rc = misc_register(&wdt_dev);
 		if (rc)
-			goto exit;
+			return rc;
 		rc = register_reboot_notifier(&wdt_notifier);
 		if (rc) {
 			misc_deregister(&wdt_dev);
-			goto exit;
+			return rc;
 		}
 	}
 #endif
 	return 0;
-
-st_err:
-	rc = -EIO;
-	dev_err(&client->dev, "Can't clear ST bit\n");
-	goto exit;
-ht_err:
-	rc = -EIO;
-	dev_err(&client->dev, "Can't clear HT bit\n");
-	goto exit;
-
-exit:
-	return rc;
 }
 
 static int m41t80_remove(struct i2c_client *client)
@@ -750,4 +737,3 @@ module_i2c_driver(m41t80_driver);
 MODULE_AUTHOR("Alexander Bigga <ab@mycable.de>");
 MODULE_DESCRIPTION("ST Microelectronics M41T80 series RTC I2C Client Driver");
 MODULE_LICENSE("GPL");
-MODULE_VERSION(DRV_VERSION);
diff --git a/drivers/rtc/rtc-mcp795.c b/drivers/rtc/rtc-mcp795.c
new file mode 100644
index 00000000000..34295bf0041
--- /dev/null
+++ b/drivers/rtc/rtc-mcp795.c
@@ -0,0 +1,199 @@
+/*
+ * SPI Driver for Microchip MCP795 RTC
+ *
+ * Copyright (C) Josef Gajdusek <atx@atx.name>
+ *
+ * based on other Linux RTC drivers
+ *
+ * Device datasheet:
+ * http://ww1.microchip.com/downloads/en/DeviceDoc/22280A.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/module.h>
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/printk.h>
+#include <linux/spi/spi.h>
+#include <linux/rtc.h>
+
+/* MCP795 Instructions, see datasheet table 3-1 */
+#define MCP795_EEREAD	0x03
+#define MCP795_EEWRITE	0x02
+#define MCP795_EEWRDI	0x04
+#define MCP795_EEWREN	0x06
+#define MCP795_SRREAD	0x05
+#define MCP795_SRWRITE	0x01
+#define MCP795_READ		0x13
+#define MCP795_WRITE	0x12
+#define MCP795_UNLOCK	0x14
+#define MCP795_IDWRITE	0x32
+#define MCP795_IDREAD	0x33
+#define MCP795_CLRWDT	0x44
+#define MCP795_CLRRAM	0x54
+
+#define MCP795_ST_BIT	0x80
+#define MCP795_24_BIT	0x40
+
+static int mcp795_rtcc_read(struct device *dev, u8 addr, u8 *buf, u8 count)
+{
+	struct spi_device *spi = to_spi_device(dev);
+	int ret;
+	u8 tx[2];
+
+	tx[0] = MCP795_READ;
+	tx[1] = addr;
+	ret = spi_write_then_read(spi, tx, sizeof(tx), buf, count);
+
+	if (ret)
+		dev_err(dev, "Failed reading %d bytes from address %x.\n",
+					count, addr);
+
+	return ret;
+}
+
+static int mcp795_rtcc_write(struct device *dev, u8 addr, u8 *data, u8 count)
+{
+	struct spi_device *spi = to_spi_device(dev);
+	int ret;
+	u8 tx[2 + count];
+
+	tx[0] = MCP795_WRITE;
+	tx[1] = addr;
+	memcpy(&tx[2], data, count);
+
+	ret = spi_write(spi, tx, 2 + count);
+
+	if (ret)
+		dev_err(dev, "Failed to write %d bytes to address %x.\n",
+					count, addr);
+
+	return ret;
+}
+
+static int mcp795_rtcc_set_bits(struct device *dev, u8 addr, u8 mask, u8 state)
+{
+	int ret;
+	u8 tmp;
+
+	ret = mcp795_rtcc_read(dev, addr, &tmp, 1);
+	if (ret)
+		return ret;
+
+	if ((tmp & mask) != state) {
+		tmp = (tmp & ~mask) | state;
+		ret = mcp795_rtcc_write(dev, addr, &tmp, 1);
+	}
+
+	return ret;
+}
+
+static int mcp795_set_time(struct device *dev, struct rtc_time *tim)
+{
+	int ret;
+	u8 data[7];
+
+	/* Read first, so we can leave config bits untouched */
+	ret = mcp795_rtcc_read(dev, 0x01, data, sizeof(data));
+
+	if (ret)
+		return ret;
+
+	data[0] = (data[0] & 0x80) | ((tim->tm_sec / 10) << 4) | (tim->tm_sec % 10);
+	data[1] = (data[1] & 0x80) | ((tim->tm_min / 10) << 4) | (tim->tm_min % 10);
+	data[2] = ((tim->tm_hour / 10) << 4) | (tim->tm_hour % 10);
+	data[4] = ((tim->tm_mday / 10) << 4) | ((tim->tm_mday) % 10);
+	data[5] = (data[5] & 0x10) | (tim->tm_mon / 10) | (tim->tm_mon % 10);
+
+	if (tim->tm_year > 100)
+		tim->tm_year -= 100;
+
+	data[6] = ((tim->tm_year / 10) << 4) | (tim->tm_year % 10);
+
+	ret = mcp795_rtcc_write(dev, 0x01, data, sizeof(data));
+
+	if (ret)
+		return ret;
+
+	dev_dbg(dev, "Set mcp795: %04d-%02d-%02d %02d:%02d:%02d\n",
+			tim->tm_year + 1900, tim->tm_mon, tim->tm_mday,
+			tim->tm_hour, tim->tm_min, tim->tm_sec);
+
+	return 0;
+}
+
+static int mcp795_read_time(struct device *dev, struct rtc_time *tim)
+{
+	int ret;
+	u8 data[7];
+
+	ret = mcp795_rtcc_read(dev, 0x01, data, sizeof(data));
+
+	if (ret)
+		return ret;
+
+	tim->tm_sec		= ((data[0] & 0x70) >> 4) * 10 + (data[0] & 0x0f);
+	tim->tm_min		= ((data[1] & 0x70) >> 4) * 10 + (data[1] & 0x0f);
+	tim->tm_hour	= ((data[2] & 0x30) >> 4) * 10 + (data[2] & 0x0f);
+	tim->tm_mday	= ((data[4] & 0x30) >> 4) * 10 + (data[4] & 0x0f);
+	tim->tm_mon		= ((data[5] & 0x10) >> 4) * 10 + (data[5] & 0x0f);
+	tim->tm_year	= ((data[6] & 0xf0) >> 4) * 10 + (data[6] & 0x0f) + 100; /* Assume we are in 20xx */
+
+	dev_dbg(dev, "Read from mcp795: %04d-%02d-%02d %02d:%02d:%02d\n",
+				tim->tm_year + 1900, tim->tm_mon, tim->tm_mday,
+				tim->tm_hour, tim->tm_min, tim->tm_sec);
+
+	return rtc_valid_tm(tim);
+}
+
+static struct rtc_class_ops mcp795_rtc_ops = {
+		.read_time = mcp795_read_time,
+		.set_time = mcp795_set_time
+};
+
+static int mcp795_probe(struct spi_device *spi)
+{
+	struct rtc_device *rtc;
+	int ret;
+
+	spi->mode = SPI_MODE_0;
+	spi->bits_per_word = 8;
+	ret = spi_setup(spi);
+	if (ret) {
+		dev_err(&spi->dev, "Unable to setup SPI\n");
+		return ret;
+	}
+
+	/* Start the oscillator */
+	mcp795_rtcc_set_bits(&spi->dev, 0x01, MCP795_ST_BIT, MCP795_ST_BIT);
+	/* Clear the 12 hour mode flag*/
+	mcp795_rtcc_set_bits(&spi->dev, 0x03, MCP795_24_BIT, 0);
+
+	rtc = devm_rtc_device_register(&spi->dev, "rtc-mcp795",
+								&mcp795_rtc_ops, THIS_MODULE);
+	if (IS_ERR(rtc))
+		return PTR_ERR(rtc);
+
+	spi_set_drvdata(spi, rtc);
+
+	return 0;
+}
+
+static struct spi_driver mcp795_driver = {
+		.driver = {
+				.name = "rtc-mcp795",
+				.owner = THIS_MODULE,
+		},
+		.probe = mcp795_probe,
+};
+
+module_spi_driver(mcp795_driver);
+
+MODULE_DESCRIPTION("MCP795 RTC SPI Driver");
+MODULE_AUTHOR("Josef Gajdusek <atx@atx.name>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("spi:mcp795");
diff --git a/drivers/rtc/rtc-mv.c b/drivers/rtc/rtc-mv.c
index d15a999363f..6aaec2fc7c0 100644
--- a/drivers/rtc/rtc-mv.c
+++ b/drivers/rtc/rtc-mv.c
@@ -319,7 +319,7 @@ static int __exit mv_rtc_remove(struct platform_device *pdev)
 }
 
 #ifdef CONFIG_OF
-static struct of_device_id rtc_mv_of_match_table[] = {
+static const struct of_device_id rtc_mv_of_match_table[] = {
 	{ .compatible = "marvell,orion-rtc", },
 	{}
 };
diff --git a/drivers/rtc/rtc-omap.c b/drivers/rtc/rtc-omap.c
index 26de5f8c2ae..21142e6574a 100644
--- a/drivers/rtc/rtc-omap.c
+++ b/drivers/rtc/rtc-omap.c
@@ -73,43 +73,52 @@
 #define OMAP_RTC_IRQWAKEEN		0x7c
 
 /* OMAP_RTC_CTRL_REG bit fields: */
-#define OMAP_RTC_CTRL_SPLIT		(1<<7)
-#define OMAP_RTC_CTRL_DISABLE		(1<<6)
-#define OMAP_RTC_CTRL_SET_32_COUNTER	(1<<5)
-#define OMAP_RTC_CTRL_TEST		(1<<4)
-#define OMAP_RTC_CTRL_MODE_12_24	(1<<3)
-#define OMAP_RTC_CTRL_AUTO_COMP		(1<<2)
-#define OMAP_RTC_CTRL_ROUND_30S		(1<<1)
-#define OMAP_RTC_CTRL_STOP		(1<<0)
+#define OMAP_RTC_CTRL_SPLIT		BIT(7)
+#define OMAP_RTC_CTRL_DISABLE		BIT(6)
+#define OMAP_RTC_CTRL_SET_32_COUNTER	BIT(5)
+#define OMAP_RTC_CTRL_TEST		BIT(4)
+#define OMAP_RTC_CTRL_MODE_12_24	BIT(3)
+#define OMAP_RTC_CTRL_AUTO_COMP		BIT(2)
+#define OMAP_RTC_CTRL_ROUND_30S		BIT(1)
+#define OMAP_RTC_CTRL_STOP		BIT(0)
 
 /* OMAP_RTC_STATUS_REG bit fields: */
-#define OMAP_RTC_STATUS_POWER_UP        (1<<7)
-#define OMAP_RTC_STATUS_ALARM           (1<<6)
-#define OMAP_RTC_STATUS_1D_EVENT        (1<<5)
-#define OMAP_RTC_STATUS_1H_EVENT        (1<<4)
-#define OMAP_RTC_STATUS_1M_EVENT        (1<<3)
-#define OMAP_RTC_STATUS_1S_EVENT        (1<<2)
-#define OMAP_RTC_STATUS_RUN             (1<<1)
-#define OMAP_RTC_STATUS_BUSY            (1<<0)
+#define OMAP_RTC_STATUS_POWER_UP	BIT(7)
+#define OMAP_RTC_STATUS_ALARM		BIT(6)
+#define OMAP_RTC_STATUS_1D_EVENT	BIT(5)
+#define OMAP_RTC_STATUS_1H_EVENT	BIT(4)
+#define OMAP_RTC_STATUS_1M_EVENT	BIT(3)
+#define OMAP_RTC_STATUS_1S_EVENT	BIT(2)
+#define OMAP_RTC_STATUS_RUN		BIT(1)
+#define OMAP_RTC_STATUS_BUSY		BIT(0)
 
 /* OMAP_RTC_INTERRUPTS_REG bit fields: */
-#define OMAP_RTC_INTERRUPTS_IT_ALARM    (1<<3)
-#define OMAP_RTC_INTERRUPTS_IT_TIMER    (1<<2)
+#define OMAP_RTC_INTERRUPTS_IT_ALARM	BIT(3)
+#define OMAP_RTC_INTERRUPTS_IT_TIMER	BIT(2)
+
+/* OMAP_RTC_OSC_REG bit fields: */
+#define OMAP_RTC_OSC_32KCLK_EN		BIT(6)
 
 /* OMAP_RTC_IRQWAKEEN bit fields: */
-#define OMAP_RTC_IRQWAKEEN_ALARM_WAKEEN    (1<<1)
+#define OMAP_RTC_IRQWAKEEN_ALARM_WAKEEN	BIT(1)
 
 /* OMAP_RTC_KICKER values */
 #define	KICK0_VALUE			0x83e70b13
 #define	KICK1_VALUE			0x95a4f1e0
 
-#define	OMAP_RTC_HAS_KICKER		0x1
+#define	OMAP_RTC_HAS_KICKER		BIT(0)
 
 /*
  * Few RTC IP revisions has special WAKE-EN Register to enable Wakeup
  * generation for event Alarm.
  */
-#define	OMAP_RTC_HAS_IRQWAKEEN		0x2
+#define	OMAP_RTC_HAS_IRQWAKEEN		BIT(1)
+
+/*
+ * Some RTC IP revisions (like those in AM335x and DRA7x) need
+ * the 32KHz clock to be explicitly enabled.
+ */
+#define OMAP_RTC_HAS_32KCLK_EN		BIT(2)
 
 static void __iomem	*rtc_base;
 
@@ -162,17 +171,28 @@ static irqreturn_t rtc_irq(int irq, void *rtc)
 
 static int omap_rtc_alarm_irq_enable(struct device *dev, unsigned int enabled)
 {
-	u8 reg;
+	u8 reg, irqwake_reg = 0;
+	struct platform_device *pdev = to_platform_device(dev);
+	const struct platform_device_id *id_entry =
+					platform_get_device_id(pdev);
 
 	local_irq_disable();
 	rtc_wait_not_busy();
 	reg = rtc_read(OMAP_RTC_INTERRUPTS_REG);
-	if (enabled)
+	if (id_entry->driver_data & OMAP_RTC_HAS_IRQWAKEEN)
+		irqwake_reg = rtc_read(OMAP_RTC_IRQWAKEEN);
+
+	if (enabled) {
 		reg |= OMAP_RTC_INTERRUPTS_IT_ALARM;
-	else
+		irqwake_reg |= OMAP_RTC_IRQWAKEEN_ALARM_WAKEEN;
+	} else {
 		reg &= ~OMAP_RTC_INTERRUPTS_IT_ALARM;
+		irqwake_reg &= ~OMAP_RTC_IRQWAKEEN_ALARM_WAKEEN;
+	}
 	rtc_wait_not_busy();
 	rtc_write(reg, OMAP_RTC_INTERRUPTS_REG);
+	if (id_entry->driver_data & OMAP_RTC_HAS_IRQWAKEEN)
+		rtc_write(irqwake_reg, OMAP_RTC_IRQWAKEEN);
 	local_irq_enable();
 
 	return 0;
@@ -272,7 +292,10 @@ static int omap_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alm)
 
 static int omap_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alm)
 {
-	u8 reg;
+	u8 reg, irqwake_reg = 0;
+	struct platform_device *pdev = to_platform_device(dev);
+	const struct platform_device_id *id_entry =
+					platform_get_device_id(pdev);
 
 	if (tm2bcd(&alm->time) < 0)
 		return -EINVAL;
@@ -288,11 +311,19 @@ static int omap_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alm)
 	rtc_write(alm->time.tm_sec, OMAP_RTC_ALARM_SECONDS_REG);
 
 	reg = rtc_read(OMAP_RTC_INTERRUPTS_REG);
-	if (alm->enabled)
+	if (id_entry->driver_data & OMAP_RTC_HAS_IRQWAKEEN)
+		irqwake_reg = rtc_read(OMAP_RTC_IRQWAKEEN);
+
+	if (alm->enabled) {
 		reg |= OMAP_RTC_INTERRUPTS_IT_ALARM;
-	else
+		irqwake_reg |= OMAP_RTC_IRQWAKEEN_ALARM_WAKEEN;
+	} else {
 		reg &= ~OMAP_RTC_INTERRUPTS_IT_ALARM;
+		irqwake_reg &= ~OMAP_RTC_IRQWAKEEN_ALARM_WAKEEN;
+	}
 	rtc_write(reg, OMAP_RTC_INTERRUPTS_REG);
+	if (id_entry->driver_data & OMAP_RTC_HAS_IRQWAKEEN)
+		rtc_write(irqwake_reg, OMAP_RTC_IRQWAKEEN);
 
 	local_irq_enable();
 
@@ -319,7 +350,8 @@ static struct platform_device_id omap_rtc_devtype[] = {
 	},
 	[OMAP_RTC_DATA_AM3352_IDX] = {
 		.name	= "am3352-rtc",
-		.driver_data = OMAP_RTC_HAS_KICKER | OMAP_RTC_HAS_IRQWAKEEN,
+		.driver_data = OMAP_RTC_HAS_KICKER | OMAP_RTC_HAS_IRQWAKEEN |
+			       OMAP_RTC_HAS_32KCLK_EN,
 	},
 	[OMAP_RTC_DATA_DA830_IDX] = {
 		.name	= "da830-rtc",
@@ -352,6 +384,12 @@ static int __init omap_rtc_probe(struct platform_device *pdev)
 	if (of_id)
 		pdev->id_entry = of_id->data;
 
+	id_entry = platform_get_device_id(pdev);
+	if (!id_entry) {
+		dev_err(&pdev->dev, "no matching device entry\n");
+		return -ENODEV;
+	}
+
 	omap_rtc_timer = platform_get_irq(pdev, 0);
 	if (omap_rtc_timer <= 0) {
 		pr_debug("%s: no update irq?\n", pdev->name);
@@ -373,8 +411,7 @@ static int __init omap_rtc_probe(struct platform_device *pdev)
 	pm_runtime_enable(&pdev->dev);
 	pm_runtime_get_sync(&pdev->dev);
 
-	id_entry = platform_get_device_id(pdev);
-	if (id_entry && (id_entry->driver_data & OMAP_RTC_HAS_KICKER)) {
+	if (id_entry->driver_data & OMAP_RTC_HAS_KICKER) {
 		rtc_writel(KICK0_VALUE, OMAP_RTC_KICK0_REG);
 		rtc_writel(KICK1_VALUE, OMAP_RTC_KICK1_REG);
 	}
@@ -393,6 +430,10 @@ static int __init omap_rtc_probe(struct platform_device *pdev)
 	 */
 	rtc_write(0, OMAP_RTC_INTERRUPTS_REG);
 
+	/* enable RTC functional clock */
+	if (id_entry->driver_data & OMAP_RTC_HAS_32KCLK_EN)
+		rtc_writel(OMAP_RTC_OSC_32KCLK_EN, OMAP_RTC_OSC_REG);
+
 	/* clear old status */
 	reg = rtc_read(OMAP_RTC_STATUS_REG);
 	if (reg & (u8) OMAP_RTC_STATUS_POWER_UP) {
@@ -452,7 +493,7 @@ static int __init omap_rtc_probe(struct platform_device *pdev)
 	return 0;
 
 fail0:
-	if (id_entry && (id_entry->driver_data & OMAP_RTC_HAS_KICKER))
+	if (id_entry->driver_data & OMAP_RTC_HAS_KICKER)
 		rtc_writel(0, OMAP_RTC_KICK0_REG);
 	pm_runtime_put_sync(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
@@ -469,7 +510,7 @@ static int __exit omap_rtc_remove(struct platform_device *pdev)
 	/* leave rtc running, but disable irqs */
 	rtc_write(0, OMAP_RTC_INTERRUPTS_REG);
 
-	if (id_entry && (id_entry->driver_data & OMAP_RTC_HAS_KICKER))
+	if (id_entry->driver_data & OMAP_RTC_HAS_KICKER)
 		rtc_writel(0, OMAP_RTC_KICK0_REG);
 
 	/* Disable the clock/module */
@@ -484,28 +525,16 @@ static u8 irqstat;
 
 static int omap_rtc_suspend(struct device *dev)
 {
-	u8 irqwake_stat;
-	struct platform_device *pdev = to_platform_device(dev);
-	const struct platform_device_id *id_entry =
-					platform_get_device_id(pdev);
-
 	irqstat = rtc_read(OMAP_RTC_INTERRUPTS_REG);
 
 	/* FIXME the RTC alarm is not currently acting as a wakeup event
 	 * source on some platforms, and in fact this enable() call is just
 	 * saving a flag that's never used...
 	 */
-	if (device_may_wakeup(dev)) {
+	if (device_may_wakeup(dev))
 		enable_irq_wake(omap_rtc_alarm);
-
-		if (id_entry->driver_data & OMAP_RTC_HAS_IRQWAKEEN) {
-			irqwake_stat = rtc_read(OMAP_RTC_IRQWAKEEN);
-			irqwake_stat |= OMAP_RTC_IRQWAKEEN_ALARM_WAKEEN;
-			rtc_write(irqwake_stat, OMAP_RTC_IRQWAKEEN);
-		}
-	} else {
+	else
 		rtc_write(0, OMAP_RTC_INTERRUPTS_REG);
-	}
 
 	/* Disable the clock/module */
 	pm_runtime_put_sync(dev);
@@ -515,25 +544,14 @@ static int omap_rtc_suspend(struct device *dev)
 
 static int omap_rtc_resume(struct device *dev)
 {
-	u8 irqwake_stat;
-	struct platform_device *pdev = to_platform_device(dev);
-	const struct platform_device_id *id_entry =
-				platform_get_device_id(pdev);
-
 	/* Enable the clock/module so that we can access the registers */
 	pm_runtime_get_sync(dev);
 
-	if (device_may_wakeup(dev)) {
+	if (device_may_wakeup(dev))
 		disable_irq_wake(omap_rtc_alarm);
-
-		if (id_entry->driver_data & OMAP_RTC_HAS_IRQWAKEEN) {
-			irqwake_stat = rtc_read(OMAP_RTC_IRQWAKEEN);
-			irqwake_stat &= ~OMAP_RTC_IRQWAKEEN_ALARM_WAKEEN;
-			rtc_write(irqwake_stat, OMAP_RTC_IRQWAKEEN);
-		}
-	} else {
+	else
 		rtc_write(irqstat, OMAP_RTC_INTERRUPTS_REG);
-	}
+
 	return 0;
 }
 #endif
diff --git a/drivers/rtc/rtc-palmas.c b/drivers/rtc/rtc-palmas.c
index c360d62fb3f..4dfe2d793fa 100644
--- a/drivers/rtc/rtc-palmas.c
+++ b/drivers/rtc/rtc-palmas.c
@@ -352,7 +352,7 @@ static SIMPLE_DEV_PM_OPS(palmas_rtc_pm_ops, palmas_rtc_suspend,
 			 palmas_rtc_resume);
 
 #ifdef CONFIG_OF
-static struct of_device_id of_palmas_rtc_match[] = {
+static const struct of_device_id of_palmas_rtc_match[] = {
 	{ .compatible = "ti,palmas-rtc"},
 	{ },
 };
diff --git a/drivers/rtc/rtc-pxa.c b/drivers/rtc/rtc-pxa.c
index cccbf9d8972..4561f375327 100644
--- a/drivers/rtc/rtc-pxa.c
+++ b/drivers/rtc/rtc-pxa.c
@@ -389,7 +389,7 @@ static int __exit pxa_rtc_remove(struct platform_device *pdev)
 }
 
 #ifdef CONFIG_OF
-static struct of_device_id pxa_rtc_dt_ids[] = {
+static const struct of_device_id pxa_rtc_dt_ids[] = {
 	{ .compatible = "marvell,pxa-rtc" },
 	{}
 };
diff --git a/drivers/rtc/rtc-s5m.c b/drivers/rtc/rtc-s5m.c
index 476af93543f..8f06250a038 100644
--- a/drivers/rtc/rtc-s5m.c
+++ b/drivers/rtc/rtc-s5m.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013 Samsung Electronics Co., Ltd
+ * Copyright (c) 2013-2014 Samsung Electronics Co., Ltd
  *	http://www.samsung.com
  *
  *  Copyright (C) 2013 Google, Inc
@@ -17,29 +17,79 @@
 
 #include <linux/module.h>
 #include <linux/i2c.h>
-#include <linux/slab.h>
 #include <linux/bcd.h>
-#include <linux/bitops.h>
 #include <linux/regmap.h>
 #include <linux/rtc.h>
-#include <linux/delay.h>
 #include <linux/platform_device.h>
 #include <linux/mfd/samsung/core.h>
 #include <linux/mfd/samsung/irq.h>
 #include <linux/mfd/samsung/rtc.h>
+#include <linux/mfd/samsung/s2mps14.h>
 
 /*
  * Maximum number of retries for checking changes in UDR field
- * of SEC_RTC_UDR_CON register (to limit possible endless loop).
+ * of S5M_RTC_UDR_CON register (to limit possible endless loop).
  *
  * After writing to RTC registers (setting time or alarm) read the UDR field
- * in SEC_RTC_UDR_CON register. UDR is auto-cleared when data have
+ * in S5M_RTC_UDR_CON register. UDR is auto-cleared when data have
  * been transferred.
  */
 #define UDR_READ_RETRY_CNT	5
 
+/* Registers used by the driver which are different between chipsets. */
+struct s5m_rtc_reg_config {
+	/* Number of registers used for setting time/alarm0/alarm1 */
+	unsigned int regs_count;
+	/* First register for time, seconds */
+	unsigned int time;
+	/* RTC control register */
+	unsigned int ctrl;
+	/* First register for alarm 0, seconds */
+	unsigned int alarm0;
+	/* First register for alarm 1, seconds */
+	unsigned int alarm1;
+	/* SMPL/WTSR register */
+	unsigned int smpl_wtsr;
+	/*
+	 * Register for update flag (UDR). Typically setting UDR field to 1
+	 * will enable update of time or alarm register. Then it will be
+	 * auto-cleared after successful update.
+	 */
+	unsigned int rtc_udr_update;
+	/* Mask for UDR field in 'rtc_udr_update' register */
+	unsigned int rtc_udr_mask;
+};
+
+/* Register map for S5M8763 and S5M8767 */
+static const struct s5m_rtc_reg_config s5m_rtc_regs = {
+	.regs_count		= 8,
+	.time			= S5M_RTC_SEC,
+	.ctrl			= S5M_ALARM1_CONF,
+	.alarm0			= S5M_ALARM0_SEC,
+	.alarm1			= S5M_ALARM1_SEC,
+	.smpl_wtsr		= S5M_WTSR_SMPL_CNTL,
+	.rtc_udr_update		= S5M_RTC_UDR_CON,
+	.rtc_udr_mask		= S5M_RTC_UDR_MASK,
+};
+
+/*
+ * Register map for S2MPS14.
+ * It may be also suitable for S2MPS11 but this was not tested.
+ */
+static const struct s5m_rtc_reg_config s2mps_rtc_regs = {
+	.regs_count		= 7,
+	.time			= S2MPS_RTC_SEC,
+	.ctrl			= S2MPS_RTC_CTRL,
+	.alarm0			= S2MPS_ALARM0_SEC,
+	.alarm1			= S2MPS_ALARM1_SEC,
+	.smpl_wtsr		= S2MPS_WTSR_SMPL_CNTL,
+	.rtc_udr_update		= S2MPS_RTC_UDR_CON,
+	.rtc_udr_mask		= S2MPS_RTC_WUDR_MASK,
+};
+
 struct s5m_rtc_info {
 	struct device *dev;
+	struct i2c_client *i2c;
 	struct sec_pmic_dev *s5m87xx;
 	struct regmap *regmap;
 	struct rtc_device *rtc_dev;
@@ -47,6 +97,21 @@ struct s5m_rtc_info {
 	int device_type;
 	int rtc_24hr_mode;
 	bool wtsr_smpl;
+	const struct s5m_rtc_reg_config	*regs;
+};
+
+static const struct regmap_config s5m_rtc_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+
+	.max_register = S5M_RTC_REG_MAX,
+};
+
+static const struct regmap_config s2mps14_rtc_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+
+	.max_register = S2MPS_RTC_REG_MAX,
 };
 
 static void s5m8767_data_to_tm(u8 *data, struct rtc_time *tm,
@@ -104,8 +169,9 @@ static inline int s5m8767_wait_for_udr_update(struct s5m_rtc_info *info)
 	unsigned int data;
 
 	do {
-		ret = regmap_read(info->regmap, SEC_RTC_UDR_CON, &data);
-	} while (--retry && (data & RTC_UDR_MASK) && !ret);
+		ret = regmap_read(info->regmap, info->regs->rtc_udr_update,
+				&data);
+	} while (--retry && (data & info->regs->rtc_udr_mask) && !ret);
 
 	if (!retry)
 		dev_err(info->dev, "waiting for UDR update, reached max number of retries\n");
@@ -113,21 +179,53 @@ static inline int s5m8767_wait_for_udr_update(struct s5m_rtc_info *info)
 	return ret;
 }
 
+static inline int s5m_check_peding_alarm_interrupt(struct s5m_rtc_info *info,
+		struct rtc_wkalrm *alarm)
+{
+	int ret;
+	unsigned int val;
+
+	switch (info->device_type) {
+	case S5M8767X:
+	case S5M8763X:
+		ret = regmap_read(info->regmap, S5M_RTC_STATUS, &val);
+		val &= S5M_ALARM0_STATUS;
+		break;
+	case S2MPS14X:
+		ret = regmap_read(info->s5m87xx->regmap_pmic, S2MPS14_REG_ST2,
+				&val);
+		val &= S2MPS_ALARM0_STATUS;
+		break;
+	default:
+		return -EINVAL;
+	}
+	if (ret < 0)
+		return ret;
+
+	if (val)
+		alarm->pending = 1;
+	else
+		alarm->pending = 0;
+
+	return 0;
+}
+
 static inline int s5m8767_rtc_set_time_reg(struct s5m_rtc_info *info)
 {
 	int ret;
 	unsigned int data;
 
-	ret = regmap_read(info->regmap, SEC_RTC_UDR_CON, &data);
+	ret = regmap_read(info->regmap, info->regs->rtc_udr_update, &data);
 	if (ret < 0) {
 		dev_err(info->dev, "failed to read update reg(%d)\n", ret);
 		return ret;
 	}
 
-	data |= RTC_TIME_EN_MASK;
-	data |= RTC_UDR_MASK;
+	data |= info->regs->rtc_udr_mask;
+	if (info->device_type == S5M8763X || info->device_type == S5M8767X)
+		data |= S5M_RTC_TIME_EN_MASK;
 
-	ret = regmap_write(info->regmap, SEC_RTC_UDR_CON, data);
+	ret = regmap_write(info->regmap, info->regs->rtc_udr_update, data);
 	if (ret < 0) {
 		dev_err(info->dev, "failed to write update reg(%d)\n", ret);
 		return ret;
@@ -143,17 +241,27 @@ static inline int s5m8767_rtc_set_alarm_reg(struct s5m_rtc_info *info)
 	int ret;
 	unsigned int data;
 
-	ret = regmap_read(info->regmap, SEC_RTC_UDR_CON, &data);
+	ret = regmap_read(info->regmap, info->regs->rtc_udr_update, &data);
 	if (ret < 0) {
 		dev_err(info->dev, "%s: fail to read update reg(%d)\n",
 			__func__, ret);
 		return ret;
 	}
 
-	data &= ~RTC_TIME_EN_MASK;
-	data |= RTC_UDR_MASK;
+	data |= info->regs->rtc_udr_mask;
+	switch (info->device_type) {
+	case S5M8763X:
+	case S5M8767X:
+		data &= ~S5M_RTC_TIME_EN_MASK;
+		break;
+	case S2MPS14X:
+		data |= S2MPS_RTC_RUDR_MASK;
+		break;
+	default:
+		return -EINVAL;
+	}
 
-	ret = regmap_write(info->regmap, SEC_RTC_UDR_CON, data);
+	ret = regmap_write(info->regmap, info->regs->rtc_udr_update, data);
 	if (ret < 0) {
 		dev_err(info->dev, "%s: fail to write update reg(%d)\n",
 			__func__, ret);
@@ -200,10 +308,22 @@ static void s5m8763_tm_to_data(struct rtc_time *tm, u8 *data)
 static int s5m_rtc_read_time(struct device *dev, struct rtc_time *tm)
 {
 	struct s5m_rtc_info *info = dev_get_drvdata(dev);
-	u8 data[8];
+	u8 data[info->regs->regs_count];
 	int ret;
 
-	ret = regmap_bulk_read(info->regmap, SEC_RTC_SEC, data, 8);
+	if (info->device_type == S2MPS14X) {
+		ret = regmap_update_bits(info->regmap,
+				info->regs->rtc_udr_update,
+				S2MPS_RTC_RUDR_MASK, S2MPS_RTC_RUDR_MASK);
+		if (ret) {
+			dev_err(dev,
+				"Failed to prepare registers for time reading: %d\n",
+				ret);
+			return ret;
+		}
+	}
+	ret = regmap_bulk_read(info->regmap, info->regs->time, data,
+			info->regs->regs_count);
 	if (ret < 0)
 		return ret;
 
@@ -213,6 +333,7 @@ static int s5m_rtc_read_time(struct device *dev, struct rtc_time *tm)
 		break;
 
 	case S5M8767X:
+	case S2MPS14X:
 		s5m8767_data_to_tm(data, tm, info->rtc_24hr_mode);
 		break;
 
@@ -230,7 +351,7 @@ static int s5m_rtc_read_time(struct device *dev, struct rtc_time *tm)
 static int s5m_rtc_set_time(struct device *dev, struct rtc_time *tm)
 {
 	struct s5m_rtc_info *info = dev_get_drvdata(dev);
-	u8 data[8];
+	u8 data[info->regs->regs_count];
 	int ret = 0;
 
 	switch (info->device_type) {
@@ -238,6 +359,7 @@ static int s5m_rtc_set_time(struct device *dev, struct rtc_time *tm)
 		s5m8763_tm_to_data(tm, data);
 		break;
 	case S5M8767X:
+	case S2MPS14X:
 		ret = s5m8767_tm_to_data(tm, data);
 		break;
 	default:
@@ -251,7 +373,8 @@ static int s5m_rtc_set_time(struct device *dev, struct rtc_time *tm)
 		1900 + tm->tm_year, 1 + tm->tm_mon, tm->tm_mday,
 		tm->tm_hour, tm->tm_min, tm->tm_sec, tm->tm_wday);
 
-	ret = regmap_raw_write(info->regmap, SEC_RTC_SEC, data, 8);
+	ret = regmap_raw_write(info->regmap, info->regs->time, data,
+			info->regs->regs_count);
 	if (ret < 0)
 		return ret;
 
@@ -263,70 +386,60 @@ static int s5m_rtc_set_time(struct device *dev, struct rtc_time *tm)
 static int s5m_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 {
 	struct s5m_rtc_info *info = dev_get_drvdata(dev);
-	u8 data[8];
+	u8 data[info->regs->regs_count];
 	unsigned int val;
 	int ret, i;
 
-	ret = regmap_bulk_read(info->regmap, SEC_ALARM0_SEC, data, 8);
+	ret = regmap_bulk_read(info->regmap, info->regs->alarm0, data,
+			info->regs->regs_count);
 	if (ret < 0)
 		return ret;
 
 	switch (info->device_type) {
 	case S5M8763X:
 		s5m8763_data_to_tm(data, &alrm->time);
-		ret = regmap_read(info->regmap, SEC_ALARM0_CONF, &val);
+		ret = regmap_read(info->regmap, S5M_ALARM0_CONF, &val);
 		if (ret < 0)
 			return ret;
 
 		alrm->enabled = !!val;
-
-		ret = regmap_read(info->regmap, SEC_RTC_STATUS, &val);
-		if (ret < 0)
-			return ret;
-
 		break;
 
 	case S5M8767X:
+	case S2MPS14X:
 		s5m8767_data_to_tm(data, &alrm->time, info->rtc_24hr_mode);
-		dev_dbg(dev, "%s: %d/%d/%d %d:%d:%d(%d)\n", __func__,
-			1900 + alrm->time.tm_year, 1 + alrm->time.tm_mon,
-			alrm->time.tm_mday, alrm->time.tm_hour,
-			alrm->time.tm_min, alrm->time.tm_sec,
-			alrm->time.tm_wday);
-
 		alrm->enabled = 0;
-		for (i = 0; i < 7; i++) {
+		for (i = 0; i < info->regs->regs_count; i++) {
 			if (data[i] & ALARM_ENABLE_MASK) {
 				alrm->enabled = 1;
 				break;
 			}
 		}
-
-		alrm->pending = 0;
-		ret = regmap_read(info->regmap, SEC_RTC_STATUS, &val);
-		if (ret < 0)
-			return ret;
 		break;
 
 	default:
 		return -EINVAL;
 	}
 
-	if (val & ALARM0_STATUS)
-		alrm->pending = 1;
-	else
-		alrm->pending = 0;
+	dev_dbg(dev, "%s: %d/%d/%d %d:%d:%d(%d)\n", __func__,
+		1900 + alrm->time.tm_year, 1 + alrm->time.tm_mon,
+		alrm->time.tm_mday, alrm->time.tm_hour,
+		alrm->time.tm_min, alrm->time.tm_sec,
+		alrm->time.tm_wday);
+
+	ret = s5m_check_peding_alarm_interrupt(info, alrm);
 
 	return 0;
 }
 
 static int s5m_rtc_stop_alarm(struct s5m_rtc_info *info)
 {
-	u8 data[8];
+	u8 data[info->regs->regs_count];
 	int ret, i;
 	struct rtc_time tm;
 
-	ret = regmap_bulk_read(info->regmap, SEC_ALARM0_SEC, data, 8);
+	ret = regmap_bulk_read(info->regmap, info->regs->alarm0, data,
+			info->regs->regs_count);
 	if (ret < 0)
 		return ret;
 
@@ -337,14 +450,16 @@ static int s5m_rtc_stop_alarm(struct s5m_rtc_info *info)
 
 	switch (info->device_type) {
 	case S5M8763X:
-		ret = regmap_write(info->regmap, SEC_ALARM0_CONF, 0);
+		ret = regmap_write(info->regmap, S5M_ALARM0_CONF, 0);
 		break;
 
 	case S5M8767X:
-		for (i = 0; i < 7; i++)
+	case S2MPS14X:
+		for (i = 0; i < info->regs->regs_count; i++)
 			data[i] &= ~ALARM_ENABLE_MASK;
 
-		ret = regmap_raw_write(info->regmap, SEC_ALARM0_SEC, data, 8);
+		ret = regmap_raw_write(info->regmap, info->regs->alarm0, data,
+				info->regs->regs_count);
 		if (ret < 0)
 			return ret;
 
@@ -362,11 +477,12 @@ static int s5m_rtc_stop_alarm(struct s5m_rtc_info *info)
 static int s5m_rtc_start_alarm(struct s5m_rtc_info *info)
 {
 	int ret;
-	u8 data[8];
+	u8 data[info->regs->regs_count];
 	u8 alarm0_conf;
 	struct rtc_time tm;
 
-	ret = regmap_bulk_read(info->regmap, SEC_ALARM0_SEC, data, 8);
+	ret = regmap_bulk_read(info->regmap, info->regs->alarm0, data,
+			info->regs->regs_count);
 	if (ret < 0)
 		return ret;
 
@@ -378,10 +494,11 @@ static int s5m_rtc_start_alarm(struct s5m_rtc_info *info)
 	switch (info->device_type) {
 	case S5M8763X:
 		alarm0_conf = 0x77;
-		ret = regmap_write(info->regmap, SEC_ALARM0_CONF, alarm0_conf);
+		ret = regmap_write(info->regmap, S5M_ALARM0_CONF, alarm0_conf);
 		break;
 
 	case S5M8767X:
+	case S2MPS14X:
 		data[RTC_SEC] |= ALARM_ENABLE_MASK;
 		data[RTC_MIN] |= ALARM_ENABLE_MASK;
 		data[RTC_HOUR] |= ALARM_ENABLE_MASK;
@@ -393,7 +510,8 @@ static int s5m_rtc_start_alarm(struct s5m_rtc_info *info)
 		if (data[RTC_YEAR1] & 0x7f)
 			data[RTC_YEAR1] |= ALARM_ENABLE_MASK;
 
-		ret = regmap_raw_write(info->regmap, SEC_ALARM0_SEC, data, 8);
+		ret = regmap_raw_write(info->regmap, info->regs->alarm0, data,
+				info->regs->regs_count);
 		if (ret < 0)
 			return ret;
 		ret = s5m8767_rtc_set_alarm_reg(info);
@@ -410,7 +528,7 @@ static int s5m_rtc_start_alarm(struct s5m_rtc_info *info)
 static int s5m_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 {
 	struct s5m_rtc_info *info = dev_get_drvdata(dev);
-	u8 data[8];
+	u8 data[info->regs->regs_count];
 	int ret;
 
 	switch (info->device_type) {
@@ -419,6 +537,7 @@ static int s5m_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 		break;
 
 	case S5M8767X:
+	case S2MPS14X:
 		s5m8767_tm_to_data(&alrm->time, data);
 		break;
 
@@ -435,7 +554,8 @@ static int s5m_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 	if (ret < 0)
 		return ret;
 
-	ret = regmap_raw_write(info->regmap, SEC_ALARM0_SEC, data, 8);
+	ret = regmap_raw_write(info->regmap, info->regs->alarm0, data,
+			info->regs->regs_count);
 	if (ret < 0)
 		return ret;
 
@@ -480,7 +600,7 @@ static const struct rtc_class_ops s5m_rtc_ops = {
 static void s5m_rtc_enable_wtsr(struct s5m_rtc_info *info, bool enable)
 {
 	int ret;
-	ret = regmap_update_bits(info->regmap, SEC_WTSR_SMPL_CNTL,
+	ret = regmap_update_bits(info->regmap, info->regs->smpl_wtsr,
 				 WTSR_ENABLE_MASK,
 				 enable ? WTSR_ENABLE_MASK : 0);
 	if (ret < 0)
@@ -491,7 +611,7 @@ static void s5m_rtc_enable_wtsr(struct s5m_rtc_info *info, bool enable)
 static void s5m_rtc_enable_smpl(struct s5m_rtc_info *info, bool enable)
 {
 	int ret;
-	ret = regmap_update_bits(info->regmap, SEC_WTSR_SMPL_CNTL,
+	ret = regmap_update_bits(info->regmap, info->regs->smpl_wtsr,
 				 SMPL_ENABLE_MASK,
 				 enable ? SMPL_ENABLE_MASK : 0);
 	if (ret < 0)
@@ -502,50 +622,41 @@ static void s5m_rtc_enable_smpl(struct s5m_rtc_info *info, bool enable)
 static int s5m8767_rtc_init_reg(struct s5m_rtc_info *info)
 {
 	u8 data[2];
-	unsigned int tp_read;
 	int ret;
-	struct rtc_time tm;
 
-	ret = regmap_read(info->regmap, SEC_RTC_UDR_CON, &tp_read);
-	if (ret < 0) {
-		dev_err(info->dev, "%s: fail to read control reg(%d)\n",
-			__func__, ret);
-		return ret;
-	}
+	switch (info->device_type) {
+	case S5M8763X:
+	case S5M8767X:
+		/* UDR update time. Default of 7.32 ms is too long. */
+		ret = regmap_update_bits(info->regmap, S5M_RTC_UDR_CON,
+				S5M_RTC_UDR_T_MASK, S5M_RTC_UDR_T_450_US);
+		if (ret < 0)
+			dev_err(info->dev, "%s: fail to change UDR time: %d\n",
+					__func__, ret);
 
-	/* Set RTC control register : Binary mode, 24hour mode */
-	data[0] = (1 << BCD_EN_SHIFT) | (1 << MODEL24_SHIFT);
-	data[1] = (0 << BCD_EN_SHIFT) | (1 << MODEL24_SHIFT);
+		/* Set RTC control register : Binary mode, 24hour mode */
+		data[0] = (1 << BCD_EN_SHIFT) | (1 << MODEL24_SHIFT);
+		data[1] = (0 << BCD_EN_SHIFT) | (1 << MODEL24_SHIFT);
+
+		ret = regmap_raw_write(info->regmap, S5M_ALARM0_CONF, data, 2);
+		break;
+
+	case S2MPS14X:
+		data[0] = (0 << BCD_EN_SHIFT) | (1 << MODEL24_SHIFT);
+		ret = regmap_write(info->regmap, info->regs->ctrl, data[0]);
+		break;
+
+	default:
+		return -EINVAL;
+	}
 
 	info->rtc_24hr_mode = 1;
-	ret = regmap_raw_write(info->regmap, SEC_ALARM0_CONF, data, 2);
 	if (ret < 0) {
 		dev_err(info->dev, "%s: fail to write controlm reg(%d)\n",
 			__func__, ret);
 		return ret;
 	}
 
-	/* In first boot time, Set rtc time to 1/1/2012 00:00:00(SUN) */
-	if ((tp_read & RTC_TCON_MASK) == 0) {
-		dev_dbg(info->dev, "rtc init\n");
-		tm.tm_sec = 0;
-		tm.tm_min = 0;
-		tm.tm_hour = 0;
-		tm.tm_wday = 0;
-		tm.tm_mday = 1;
-		tm.tm_mon = 0;
-		tm.tm_year = 112;
-		tm.tm_yday = 0;
-		tm.tm_isdst = 0;
-		ret = s5m_rtc_set_time(info->dev, &tm);
-	}
-
-	ret = regmap_update_bits(info->regmap, SEC_RTC_UDR_CON,
-				 RTC_TCON_MASK, tp_read | RTC_TCON_MASK);
-	if (ret < 0)
-		dev_err(info->dev, "%s: fail to update TCON reg(%d)\n",
-			__func__, ret);
-
 	return ret;
 }
 
@@ -554,7 +665,8 @@ static int s5m_rtc_probe(struct platform_device *pdev)
 	struct sec_pmic_dev *s5m87xx = dev_get_drvdata(pdev->dev.parent);
 	struct sec_platform_data *pdata = s5m87xx->pdata;
 	struct s5m_rtc_info *info;
-	int ret;
+	const struct regmap_config *regmap_cfg;
+	int ret, alarm_irq;
 
 	if (!pdata) {
 		dev_err(pdev->dev.parent, "Platform data not supplied\n");
@@ -565,27 +677,52 @@ static int s5m_rtc_probe(struct platform_device *pdev)
 	if (!info)
 		return -ENOMEM;
 
-	info->dev = &pdev->dev;
-	info->s5m87xx = s5m87xx;
-	info->regmap = s5m87xx->regmap_rtc;
-	info->device_type = s5m87xx->device_type;
-	info->wtsr_smpl = s5m87xx->wtsr_smpl;
-
 	switch (pdata->device_type) {
+	case S2MPS14X:
+		regmap_cfg = &s2mps14_rtc_regmap_config;
+		info->regs = &s2mps_rtc_regs;
+		alarm_irq = S2MPS14_IRQ_RTCA0;
+		break;
 	case S5M8763X:
-		info->irq = regmap_irq_get_virq(s5m87xx->irq_data,
-				S5M8763_IRQ_ALARM0);
+		regmap_cfg = &s5m_rtc_regmap_config;
+		info->regs = &s5m_rtc_regs;
+		alarm_irq = S5M8763_IRQ_ALARM0;
 		break;
-
 	case S5M8767X:
-		info->irq = regmap_irq_get_virq(s5m87xx->irq_data,
-				S5M8767_IRQ_RTCA1);
+		regmap_cfg = &s5m_rtc_regmap_config;
+		info->regs = &s5m_rtc_regs;
+		alarm_irq = S5M8767_IRQ_RTCA1;
 		break;
-
 	default:
+		dev_err(&pdev->dev, "Device type is not supported by RTC driver\n");
+		return -ENODEV;
+	}
+
+	info->i2c = i2c_new_dummy(s5m87xx->i2c->adapter, RTC_I2C_ADDR);
+	if (!info->i2c) {
+		dev_err(&pdev->dev, "Failed to allocate I2C for RTC\n");
+		return -ENODEV;
+	}
+
+	info->regmap = devm_regmap_init_i2c(info->i2c, regmap_cfg);
+	if (IS_ERR(info->regmap)) {
+		ret = PTR_ERR(info->regmap);
+		dev_err(&pdev->dev, "Failed to allocate RTC register map: %d\n",
+				ret);
+		goto err;
+	}
+
+	info->dev = &pdev->dev;
+	info->s5m87xx = s5m87xx;
+	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, "Unsupported device type: %d\n", ret);
-		return ret;
+		dev_err(&pdev->dev, "Failed to get virtual IRQ %d\n",
+				alarm_irq);
+		goto err;
 	}
 
 	platform_set_drvdata(pdev, info);
@@ -602,15 +739,24 @@ static int s5m_rtc_probe(struct platform_device *pdev)
 	info->rtc_dev = devm_rtc_device_register(&pdev->dev, "s5m-rtc",
 						 &s5m_rtc_ops, THIS_MODULE);
 
-	if (IS_ERR(info->rtc_dev))
-		return PTR_ERR(info->rtc_dev);
+	if (IS_ERR(info->rtc_dev)) {
+		ret = PTR_ERR(info->rtc_dev);
+		goto err;
+	}
 
 	ret = devm_request_threaded_irq(&pdev->dev, info->irq, NULL,
 					s5m_rtc_alarm_irq, 0, "rtc-alarm0",
 					info);
-	if (ret < 0)
+	if (ret < 0) {
 		dev_err(&pdev->dev, "Failed to request alarm IRQ: %d: %d\n",
 			info->irq, ret);
+		goto err;
+	}
+
+	return 0;
+
+err:
+	i2c_unregister_device(info->i2c);
 
 	return ret;
 }
@@ -623,7 +769,7 @@ static void s5m_rtc_shutdown(struct platform_device *pdev)
 	if (info->wtsr_smpl) {
 		for (i = 0; i < 3; i++) {
 			s5m_rtc_enable_wtsr(info, false);
-			regmap_read(info->regmap, SEC_WTSR_SMPL_CNTL, &val);
+			regmap_read(info->regmap, info->regs->smpl_wtsr, &val);
 			pr_debug("%s: WTSR_SMPL reg(0x%02x)\n", __func__, val);
 			if (val & WTSR_ENABLE_MASK)
 				pr_emerg("%s: fail to disable WTSR\n",
@@ -639,6 +785,17 @@ static void s5m_rtc_shutdown(struct platform_device *pdev)
 	s5m_rtc_enable_smpl(info, false);
 }
 
+static int s5m_rtc_remove(struct platform_device *pdev)
+{
+	struct s5m_rtc_info *info = platform_get_drvdata(pdev);
+
+	/* Perform also all shutdown steps when removing */
+	s5m_rtc_shutdown(pdev);
+	i2c_unregister_device(info->i2c);
+
+	return 0;
+}
+
 #ifdef CONFIG_PM_SLEEP
 static int s5m_rtc_resume(struct device *dev)
 {
@@ -666,7 +823,8 @@ static int s5m_rtc_suspend(struct device *dev)
 static SIMPLE_DEV_PM_OPS(s5m_rtc_pm_ops, s5m_rtc_suspend, s5m_rtc_resume);
 
 static const struct platform_device_id s5m_rtc_id[] = {
-	{ "s5m-rtc", 0 },
+	{ "s5m-rtc",		S5M8767X },
+	{ "s2mps14-rtc",	S2MPS14X },
 };
 
 static struct platform_driver s5m_rtc_driver = {
@@ -676,6 +834,7 @@ static struct platform_driver s5m_rtc_driver = {
 		.pm	= &s5m_rtc_pm_ops,
 	},
 	.probe		= s5m_rtc_probe,
+	.remove		= s5m_rtc_remove,
 	.shutdown	= s5m_rtc_shutdown,
 	.id_table	= s5m_rtc_id,
 };
@@ -684,6 +843,6 @@ module_platform_driver(s5m_rtc_driver);
 
 /* Module information */
 MODULE_AUTHOR("Sangbeom Kim <sbkim73@samsung.com>");
-MODULE_DESCRIPTION("Samsung S5M RTC driver");
+MODULE_DESCRIPTION("Samsung S5M/S2MPS14 RTC driver");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("platform:s5m-rtc");
diff --git a/drivers/rtc/rtc-sa1100.c b/drivers/rtc/rtc-sa1100.c
index 0f7adeb1944..b6e1ca08c2c 100644
--- a/drivers/rtc/rtc-sa1100.c
+++ b/drivers/rtc/rtc-sa1100.c
@@ -338,7 +338,7 @@ static SIMPLE_DEV_PM_OPS(sa1100_rtc_pm_ops, sa1100_rtc_suspend,
 			sa1100_rtc_resume);
 
 #ifdef CONFIG_OF
-static struct of_device_id sa1100_rtc_dt_ids[] = {
+static const struct of_device_id sa1100_rtc_dt_ids[] = {
 	{ .compatible = "mrvl,sa1100-rtc", },
 	{ .compatible = "mrvl,mmp-rtc", },
 	{}
diff --git a/drivers/rtc/rtc-xgene.c b/drivers/rtc/rtc-xgene.c
new file mode 100644
index 00000000000..14129cc85bd
--- /dev/null
+++ b/drivers/rtc/rtc-xgene.c
@@ -0,0 +1,278 @@
+/*
+ * APM X-Gene SoC Real Time Clock Driver
+ *
+ * Copyright (c) 2014, Applied Micro Circuits Corporation
+ * Author: Rameshwar Prasad Sahu <rsahu@apm.com>
+ *         Loc Ho <lho@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/init.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/rtc.h>
+
+/* RTC CSR Registers */
+#define RTC_CCVR		0x00
+#define RTC_CMR			0x04
+#define RTC_CLR			0x08
+#define RTC_CCR			0x0C
+#define  RTC_CCR_IE		BIT(0)
+#define  RTC_CCR_MASK		BIT(1)
+#define  RTC_CCR_EN		BIT(2)
+#define  RTC_CCR_WEN		BIT(3)
+#define RTC_STAT		0x10
+#define  RTC_STAT_BIT		BIT(0)
+#define RTC_RSTAT		0x14
+#define RTC_EOI			0x18
+#define RTC_VER			0x1C
+
+struct xgene_rtc_dev {
+	struct rtc_device *rtc;
+	struct device *dev;
+	unsigned long alarm_time;
+	void __iomem *csr_base;
+	struct clk *clk;
+	unsigned int irq_wake;
+};
+
+static int xgene_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	struct xgene_rtc_dev *pdata = dev_get_drvdata(dev);
+
+	rtc_time_to_tm(readl(pdata->csr_base + RTC_CCVR), tm);
+	return rtc_valid_tm(tm);
+}
+
+static int xgene_rtc_set_mmss(struct device *dev, unsigned long secs)
+{
+	struct xgene_rtc_dev *pdata = dev_get_drvdata(dev);
+
+	/*
+	 * NOTE: After the following write, the RTC_CCVR is only reflected
+	 *       after the update cycle of 1 seconds.
+	 */
+	writel((u32) secs, pdata->csr_base + RTC_CLR);
+	readl(pdata->csr_base + RTC_CLR); /* Force a barrier */
+
+	return 0;
+}
+
+static int xgene_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct xgene_rtc_dev *pdata = dev_get_drvdata(dev);
+
+	rtc_time_to_tm(pdata->alarm_time, &alrm->time);
+	alrm->enabled = readl(pdata->csr_base + RTC_CCR) & RTC_CCR_IE;
+
+	return 0;
+}
+
+static int xgene_rtc_alarm_irq_enable(struct device *dev, u32 enabled)
+{
+	struct xgene_rtc_dev *pdata = dev_get_drvdata(dev);
+	u32 ccr;
+
+	ccr = readl(pdata->csr_base + RTC_CCR);
+	if (enabled) {
+		ccr &= ~RTC_CCR_MASK;
+		ccr |= RTC_CCR_IE;
+	} else {
+		ccr &= ~RTC_CCR_IE;
+		ccr |= RTC_CCR_MASK;
+	}
+	writel(ccr, pdata->csr_base + RTC_CCR);
+
+	return 0;
+}
+
+static int xgene_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct xgene_rtc_dev *pdata = dev_get_drvdata(dev);
+	unsigned long rtc_time;
+	unsigned long alarm_time;
+
+	rtc_time = readl(pdata->csr_base + RTC_CCVR);
+	rtc_tm_to_time(&alrm->time, &alarm_time);
+
+	pdata->alarm_time = alarm_time;
+	writel((u32) pdata->alarm_time, pdata->csr_base + RTC_CMR);
+
+	xgene_rtc_alarm_irq_enable(dev, alrm->enabled);
+
+	return 0;
+}
+
+static const struct rtc_class_ops xgene_rtc_ops = {
+	.read_time	= xgene_rtc_read_time,
+	.set_mmss	= xgene_rtc_set_mmss,
+	.read_alarm	= xgene_rtc_read_alarm,
+	.set_alarm	= xgene_rtc_set_alarm,
+	.alarm_irq_enable = xgene_rtc_alarm_irq_enable,
+};
+
+static irqreturn_t xgene_rtc_interrupt(int irq, void *id)
+{
+	struct xgene_rtc_dev *pdata = (struct xgene_rtc_dev *) id;
+
+	/* Check if interrupt asserted */
+	if (!(readl(pdata->csr_base + RTC_STAT) & RTC_STAT_BIT))
+		return IRQ_NONE;
+
+	/* Clear interrupt */
+	readl(pdata->csr_base + RTC_EOI);
+
+	rtc_update_irq(pdata->rtc, 1, RTC_IRQF | RTC_AF);
+
+	return IRQ_HANDLED;
+}
+
+static int xgene_rtc_probe(struct platform_device *pdev)
+{
+	struct xgene_rtc_dev *pdata;
+	struct resource *res;
+	int ret;
+	int irq;
+
+	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return -ENOMEM;
+	platform_set_drvdata(pdev, pdata);
+	pdata->dev = &pdev->dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	pdata->csr_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(pdata->csr_base))
+		return PTR_ERR(pdata->csr_base);
+
+	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, xgene_rtc_interrupt, 0,
+			       dev_name(&pdev->dev), pdata);
+	if (ret) {
+		dev_err(&pdev->dev, "Could not request IRQ\n");
+		return ret;
+	}
+
+	pdata->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(pdata->clk)) {
+		dev_err(&pdev->dev, "Couldn't get the clock for RTC\n");
+		return -ENODEV;
+	}
+	clk_prepare_enable(pdata->clk);
+
+	/* Turn on the clock and the crystal */
+	writel(RTC_CCR_EN, pdata->csr_base + RTC_CCR);
+
+	device_init_wakeup(&pdev->dev, 1);
+
+	pdata->rtc = devm_rtc_device_register(&pdev->dev, pdev->name,
+					 &xgene_rtc_ops, THIS_MODULE);
+	if (IS_ERR(pdata->rtc)) {
+		clk_disable_unprepare(pdata->clk);
+		return PTR_ERR(pdata->rtc);
+	}
+
+	/* HW does not support update faster than 1 seconds */
+	pdata->rtc->uie_unsupported = 1;
+
+	return 0;
+}
+
+static int xgene_rtc_remove(struct platform_device *pdev)
+{
+	struct xgene_rtc_dev *pdata = platform_get_drvdata(pdev);
+
+	xgene_rtc_alarm_irq_enable(&pdev->dev, 0);
+	device_init_wakeup(&pdev->dev, 0);
+	clk_disable_unprepare(pdata->clk);
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int xgene_rtc_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct xgene_rtc_dev *pdata = platform_get_drvdata(pdev);
+	int irq;
+
+	irq = platform_get_irq(pdev, 0);
+	if (device_may_wakeup(&pdev->dev)) {
+		if (!enable_irq_wake(irq))
+			pdata->irq_wake = 1;
+	} else {
+		xgene_rtc_alarm_irq_enable(dev, 0);
+		clk_disable(pdata->clk);
+	}
+
+	return 0;
+}
+
+static int xgene_rtc_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct xgene_rtc_dev *pdata = platform_get_drvdata(pdev);
+	int irq;
+
+	irq = platform_get_irq(pdev, 0);
+	if (device_may_wakeup(&pdev->dev)) {
+		if (pdata->irq_wake) {
+			disable_irq_wake(irq);
+			pdata->irq_wake = 0;
+		}
+	} else {
+		clk_enable(pdata->clk);
+		xgene_rtc_alarm_irq_enable(dev, 1);
+	}
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(xgene_rtc_pm_ops, xgene_rtc_suspend, xgene_rtc_resume);
+
+#ifdef CONFIG_OF
+static const struct of_device_id xgene_rtc_of_match[] = {
+	{.compatible = "apm,xgene-rtc" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, xgene_rtc_of_match);
+#endif
+
+static struct platform_driver xgene_rtc_driver = {
+	.probe		= xgene_rtc_probe,
+	.remove		= xgene_rtc_remove,
+	.driver		= {
+		.owner	= THIS_MODULE,
+		.name	= "xgene-rtc",
+		.pm = &xgene_rtc_pm_ops,
+		.of_match_table	= of_match_ptr(xgene_rtc_of_match),
+	},
+};
+
+module_platform_driver(xgene_rtc_driver);
+
+MODULE_DESCRIPTION("APM X-Gene SoC RTC driver");
+MODULE_AUTHOR("Rameshwar Sahu <rsahu@apm.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/s390/char/Makefile b/drivers/s390/char/Makefile
index b69ab17f13f..629fcc275e9 100644
--- a/drivers/s390/char/Makefile
+++ b/drivers/s390/char/Makefile
@@ -33,4 +33,4 @@ obj-$(CONFIG_MONWRITER) += monwriter.o
 obj-$(CONFIG_S390_VMUR) += vmur.o
 
 zcore_mod-objs := sclp_sdias.o zcore.o
-obj-$(CONFIG_ZFCPDUMP) += zcore_mod.o
+obj-$(CONFIG_CRASH_DUMP) += zcore_mod.o
diff --git a/drivers/s390/char/sclp_early.c b/drivers/s390/char/sclp_early.c
index 14196ea0fdf..1918d9dff45 100644
--- a/drivers/s390/char/sclp_early.c
+++ b/drivers/s390/char/sclp_early.c
@@ -22,11 +22,14 @@ struct read_info_sccb {
 	u8	rnsize;			/* 10 */
 	u8	_reserved0[16 - 11];	/* 11-15 */
 	u16	ncpurl;			/* 16-17 */
-	u8	_reserved7[24 - 18];	/* 18-23 */
+	u16	cpuoff;			/* 18-19 */
+	u8	_reserved7[24 - 20];	/* 20-23 */
 	u8	loadparm[8];		/* 24-31 */
 	u8	_reserved1[48 - 32];	/* 32-47 */
 	u64	facilities;		/* 48-55 */
-	u8	_reserved2[84 - 56];	/* 56-83 */
+	u8	_reserved2a[76 - 56];	/* 56-75 */
+	u32	ibc;			/* 76-79 */
+	u8	_reserved2b[84 - 80];	/* 80-83 */
 	u8	fac84;			/* 84 */
 	u8	fac85;			/* 85 */
 	u8	_reserved3[91 - 86];	/* 86-90 */
@@ -45,6 +48,8 @@ static unsigned int sclp_con_has_linemode __initdata;
 static unsigned long sclp_hsa_size;
 static unsigned int sclp_max_cpu;
 static struct sclp_ipl_info sclp_ipl_info;
+static unsigned char sclp_siif;
+static u32 sclp_ibc;
 
 u64 sclp_facilities;
 u8 sclp_fac84;
@@ -96,6 +101,9 @@ static int __init sclp_read_info_early(struct read_info_sccb *sccb)
 
 static void __init sclp_facilities_detect(struct read_info_sccb *sccb)
 {
+	struct sclp_cpu_entry *cpue;
+	u16 boot_cpu_address, cpu;
+
 	if (sclp_read_info_early(sccb))
 		return;
 
@@ -106,6 +114,7 @@ static void __init sclp_facilities_detect(struct read_info_sccb *sccb)
 	sclp_rnmax = sccb->rnmax ? sccb->rnmax : sccb->rnmax2;
 	sclp_rzm = sccb->rnsize ? sccb->rnsize : sccb->rnsize2;
 	sclp_rzm <<= 20;
+	sclp_ibc = sccb->ibc;
 
 	if (!sccb->hcpua) {
 		if (MACHINE_IS_VM)
@@ -116,6 +125,15 @@ static void __init sclp_facilities_detect(struct read_info_sccb *sccb)
 		sclp_max_cpu = sccb->hcpua + 1;
 	}
 
+	boot_cpu_address = stap();
+	cpue = (void *)sccb + sccb->cpuoff;
+	for (cpu = 0; cpu < sccb->ncpurl; cpue++, cpu++) {
+		if (boot_cpu_address != cpue->address)
+			continue;
+		sclp_siif = cpue->siif;
+		break;
+	}
+
 	/* Save IPL information */
 	sclp_ipl_info.is_valid = 1;
 	if (sccb->flags & 0x2)
@@ -148,6 +166,18 @@ unsigned int sclp_get_max_cpu(void)
 	return sclp_max_cpu;
 }
 
+int sclp_has_siif(void)
+{
+	return sclp_siif;
+}
+EXPORT_SYMBOL(sclp_has_siif);
+
+unsigned int sclp_get_ibc(void)
+{
+	return sclp_ibc;
+}
+EXPORT_SYMBOL(sclp_get_ibc);
+
 /*
  * This function will be called after sclp_facilities_detect(), which gets
  * called from early.c code. The sclp_facilities_detect() function retrieves
diff --git a/drivers/s390/char/zcore.c b/drivers/s390/char/zcore.c
index 3d8e4d63f51..1884653e447 100644
--- a/drivers/s390/char/zcore.c
+++ b/drivers/s390/char/zcore.c
@@ -17,6 +17,8 @@
 #include <linux/miscdevice.h>
 #include <linux/debugfs.h>
 #include <linux/module.h>
+#include <linux/memblock.h>
+
 #include <asm/asm-offsets.h>
 #include <asm/ipl.h>
 #include <asm/sclp.h>
@@ -411,33 +413,24 @@ static ssize_t zcore_memmap_read(struct file *filp, char __user *buf,
 				 size_t count, loff_t *ppos)
 {
 	return simple_read_from_buffer(buf, count, ppos, filp->private_data,
-				       MEMORY_CHUNKS * CHUNK_INFO_SIZE);
+				       memblock.memory.cnt * CHUNK_INFO_SIZE);
 }
 
 static int zcore_memmap_open(struct inode *inode, struct file *filp)
 {
-	int i;
+	struct memblock_region *reg;
 	char *buf;
-	struct mem_chunk *chunk_array;
+	int i = 0;
 
-	chunk_array = kzalloc(MEMORY_CHUNKS * sizeof(struct mem_chunk),
-			      GFP_KERNEL);
-	if (!chunk_array)
-		return -ENOMEM;
-	detect_memory_layout(chunk_array, 0);
-	buf = kzalloc(MEMORY_CHUNKS * CHUNK_INFO_SIZE, GFP_KERNEL);
+	buf = kzalloc(memblock.memory.cnt * CHUNK_INFO_SIZE, GFP_KERNEL);
 	if (!buf) {
-		kfree(chunk_array);
 		return -ENOMEM;
 	}
-	for (i = 0; i < MEMORY_CHUNKS; i++) {
-		sprintf(buf + (i * CHUNK_INFO_SIZE), "%016llx %016llx ",
-			(unsigned long long) chunk_array[i].addr,
-			(unsigned long long) chunk_array[i].size);
-		if (chunk_array[i].size == 0)
-			break;
+	for_each_memblock(memory, reg) {
+		sprintf(buf + (i++ * CHUNK_INFO_SIZE), "%016llx %016llx ",
+			(unsigned long long) reg->base,
+			(unsigned long long) reg->size);
 	}
-	kfree(chunk_array);
 	filp->private_data = buf;
 	return nonseekable_open(inode, filp);
 }
@@ -593,21 +586,12 @@ static int __init check_sdias(void)
 
 static int __init get_mem_info(unsigned long *mem, unsigned long *end)
 {
-	int i;
-	struct mem_chunk *chunk_array;
+	struct memblock_region *reg;
 
-	chunk_array = kzalloc(MEMORY_CHUNKS * sizeof(struct mem_chunk),
-			      GFP_KERNEL);
-	if (!chunk_array)
-		return -ENOMEM;
-	detect_memory_layout(chunk_array, 0);
-	for (i = 0; i < MEMORY_CHUNKS; i++) {
-		if (chunk_array[i].size == 0)
-			break;
-		*mem += chunk_array[i].size;
-		*end = max(*end, chunk_array[i].addr + chunk_array[i].size);
+	for_each_memblock(memory, reg) {
+		*mem += reg->size;
+		*end = max_t(unsigned long, *end, reg->base + reg->size);
 	}
-	kfree(chunk_array);
 	return 0;
 }
 
diff --git a/drivers/s390/cio/ccwreq.c b/drivers/s390/cio/ccwreq.c
index 5156264d0c7..07676c22d51 100644
--- a/drivers/s390/cio/ccwreq.c
+++ b/drivers/s390/cio/ccwreq.c
@@ -46,7 +46,7 @@ static u16 ccwreq_next_path(struct ccw_device *cdev)
 		goto out;
 	}
 	req->retries	= req->maxretries;
-	req->mask	= lpm_adjust(req->mask >>= 1, req->lpm);
+	req->mask	= lpm_adjust(req->mask >> 1, req->lpm);
 out:
 	return req->mask;
 }
@@ -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 = (struct irb *)&S390_lowcore.irb;
+	struct irb *irb = &__get_cpu_var(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 6c440d4349d..d497aa05a72 100644
--- a/drivers/s390/cio/chp.c
+++ b/drivers/s390/cio/chp.c
@@ -509,7 +509,7 @@ out:
  * On success return a newly allocated copy of the channel-path description
  * data associated with the given channel-path ID. Return %NULL on error.
  */
-void *chp_get_chp_desc(struct chp_id chpid)
+struct channel_path_desc *chp_get_chp_desc(struct chp_id chpid)
 {
 	struct channel_path *chp;
 	struct channel_path_desc *desc;
diff --git a/drivers/s390/cio/chp.h b/drivers/s390/cio/chp.h
index 9284b785a06..4efd5b867cc 100644
--- a/drivers/s390/cio/chp.h
+++ b/drivers/s390/cio/chp.h
@@ -60,7 +60,7 @@ static inline struct channel_path *chpid_to_chp(struct chp_id chpid)
 int chp_get_status(struct chp_id chpid);
 u8 chp_get_sch_opm(struct subchannel *sch);
 int chp_is_registered(struct chp_id chpid);
-void *chp_get_chp_desc(struct chp_id chpid);
+struct channel_path_desc *chp_get_chp_desc(struct chp_id chpid);
 void chp_remove_cmg_attr(struct channel_path *chp);
 int chp_add_cmg_attr(struct channel_path *chp);
 int chp_update_desc(struct channel_path *chp);
diff --git a/drivers/s390/cio/chsc.h b/drivers/s390/cio/chsc.h
index 7e53a9c8b0b..76c9b50700b 100644
--- a/drivers/s390/cio/chsc.h
+++ b/drivers/s390/cio/chsc.h
@@ -21,17 +21,6 @@ struct cmg_entry {
 	u32 values[NR_MEASUREMENT_ENTRIES];
 } __attribute__ ((packed));
 
-struct channel_path_desc {
-	u8 flags;
-	u8 lsn;
-	u8 desc;
-	u8 chpid;
-	u8 swla;
-	u8 zeroes;
-	u8 chla;
-	u8 chpp;
-} __attribute__ ((packed));
-
 struct channel_path_desc_fmt1 {
 	u8 flags;
 	u8 lsn;
diff --git a/drivers/s390/cio/chsc_sch.c b/drivers/s390/cio/chsc_sch.c
index 1d3661af7bd..3d22d2a4ce1 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 = (struct irb *)&S390_lowcore.irb;
+	struct irb *irb = &__get_cpu_var(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 9e058c4657a..77f9c92df4b 100644
--- a/drivers/s390/cio/cio.c
+++ b/drivers/s390/cio/cio.c
@@ -46,6 +46,9 @@ debug_info_t *cio_debug_msg_id;
 debug_info_t *cio_debug_trace_id;
 debug_info_t *cio_debug_crw_id;
 
+DEFINE_PER_CPU_ALIGNED(struct irb, cio_irb);
+EXPORT_PER_CPU_SYMBOL(cio_irb);
+
 /*
  * Function: cio_debug_init
  * Initializes three debug logs for common I/O:
@@ -560,7 +563,7 @@ static irqreturn_t do_cio_interrupt(int irq, void *dummy)
 
 	__this_cpu_write(s390_idle.nohz_delay, 1);
 	tpi_info = (struct tpi_info *) &get_irq_regs()->int_code;
-	irb = (struct irb *) &S390_lowcore.irb;
+	irb = &__get_cpu_var(cio_irb);
 	sch = (struct subchannel *)(unsigned long) tpi_info->intparm;
 	if (!sch) {
 		/* Clear pending interrupt condition. */
@@ -609,7 +612,7 @@ void cio_tsch(struct subchannel *sch)
 	struct irb *irb;
 	int irq_context;
 
-	irb = (struct irb *)&S390_lowcore.irb;
+	irb = &__get_cpu_var(cio_irb);
 	/* Store interrupt response block to lowcore. */
 	if (tsch(sch->schid, irb) != 0)
 		/* Not status pending or not operational. */
@@ -746,7 +749,7 @@ __clear_io_subchannel_easy(struct subchannel_id schid)
 		struct tpi_info ti;
 
 		if (tpi(&ti)) {
-			tsch(ti.schid, (struct irb *)&S390_lowcore.irb);
+			tsch(ti.schid, &__get_cpu_var(cio_irb));
 			if (schid_equal(&ti.schid, &schid))
 				return 0;
 		}
diff --git a/drivers/s390/cio/cio.h b/drivers/s390/cio/cio.h
index d42f67412bd..a01376ae174 100644
--- a/drivers/s390/cio/cio.h
+++ b/drivers/s390/cio/cio.h
@@ -102,6 +102,8 @@ struct subchannel {
 	struct schib_config config;
 } __attribute__ ((aligned(8)));
 
+DECLARE_PER_CPU(struct irb, cio_irb);
+
 #define to_subchannel(n) container_of(n, struct subchannel, dev)
 
 extern int cio_validate_subchannel (struct subchannel *, struct subchannel_id);
diff --git a/drivers/s390/cio/device_fsm.c b/drivers/s390/cio/device_fsm.c
index c7638c54325..0bc902b3cd8 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 = (struct irb *)&S390_lowcore.irb;
+	irb = &__get_cpu_var(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 = (struct irb *)&S390_lowcore.irb;
+	irb = &__get_cpu_var(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/device_ops.c b/drivers/s390/cio/device_ops.c
index 4845d64f284..f3c417943da 100644
--- a/drivers/s390/cio/device_ops.c
+++ b/drivers/s390/cio/device_ops.c
@@ -563,14 +563,23 @@ out_unlock:
 	return rc;
 }
 
-void *ccw_device_get_chp_desc(struct ccw_device *cdev, int chp_no)
+/**
+ * chp_get_chp_desc - return newly allocated channel-path descriptor
+ * @cdev: device to obtain the descriptor for
+ * @chp_idx: index of the channel path
+ *
+ * On success return a newly allocated copy of the channel-path description
+ * data associated with the given channel path. Return %NULL on error.
+ */
+struct channel_path_desc *ccw_device_get_chp_desc(struct ccw_device *cdev,
+						  int chp_idx)
 {
 	struct subchannel *sch;
 	struct chp_id chpid;
 
 	sch = to_subchannel(cdev->dev.parent);
 	chp_id_init(&chpid);
-	chpid.id = sch->schib.pmcw.chpid[chp_no];
+	chpid.id = sch->schib.pmcw.chpid[chp_idx];
 	return chp_get_chp_desc(chpid);
 }
 
diff --git a/drivers/s390/cio/eadm_sch.c b/drivers/s390/cio/eadm_sch.c
index 3a2ee4a740b..c4f7bf3e24c 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 = (struct irb *)&S390_lowcore.irb;
+	struct irb *irb = &__get_cpu_var(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 ab3baa7f950..8eec1653c9c 100644
--- a/drivers/s390/crypto/ap_bus.c
+++ b/drivers/s390/crypto/ap_bus.c
@@ -1803,7 +1803,7 @@ static int ap_poll_thread(void *data)
 	int requests;
 	struct ap_device *ap_dev;
 
-	set_user_nice(current, 19);
+	set_user_nice(current, MAX_NICE);
 	while (1) {
 		if (ap_suspend_flag)
 			return 0;
diff --git a/drivers/s390/net/qeth_core_main.c b/drivers/s390/net/qeth_core_main.c
index 22470a3b182..e89f38c3117 100644
--- a/drivers/s390/net/qeth_core_main.c
+++ b/drivers/s390/net/qeth_core_main.c
@@ -22,6 +22,7 @@
 #include <net/iucv/af_iucv.h>
 
 #include <asm/ebcdic.h>
+#include <asm/chpid.h>
 #include <asm/io.h>
 #include <asm/sysinfo.h>
 #include <asm/compat.h>
@@ -1344,16 +1345,7 @@ static void qeth_set_multiple_write_queues(struct qeth_card *card)
 static void qeth_update_from_chp_desc(struct qeth_card *card)
 {
 	struct ccw_device *ccwdev;
-	struct channelPath_dsc {
-		u8 flags;
-		u8 lsn;
-		u8 desc;
-		u8 chpid;
-		u8 swla;
-		u8 zeroes;
-		u8 chla;
-		u8 chpp;
-	} *chp_dsc;
+	struct channel_path_desc *chp_dsc;
 
 	QETH_DBF_TEXT(SETUP, 2, "chp_desc");
 
diff --git a/drivers/sbus/char/jsflash.c b/drivers/sbus/char/jsflash.c
index 4ccb5d86938..a40ee1e3748 100644
--- a/drivers/sbus/char/jsflash.c
+++ b/drivers/sbus/char/jsflash.c
@@ -207,7 +207,7 @@ static void jsfd_do_request(struct request_queue *q)
 			goto end;
 		}
 
-		jsfd_read(req->buffer, jdp->dbase + offset, len);
+		jsfd_read(bio_data(req->bio), jdp->dbase + offset, len);
 		err = 0;
 	end:
 		if (!__blk_end_request_cur(req, err))
diff --git a/drivers/scsi/NCR5380.c b/drivers/scsi/NCR5380.c
index bcd22386822..93d13fc9a29 100644
--- a/drivers/scsi/NCR5380.c
+++ b/drivers/scsi/NCR5380.c
@@ -27,8 +27,6 @@
  */
 
 /*
- * $Log: NCR5380.c,v $
-
  * Revision 1.10 1998/9/2	Alan Cox
  *				(alan@lxorguk.ukuu.org.uk)
  * Fixed up the timer lockups reported so far. Things still suck. Looking 
@@ -89,13 +87,6 @@
 #include <scsi/scsi_dbg.h>
 #include <scsi/scsi_transport_spi.h>
 
-#ifndef NDEBUG
-#define NDEBUG 0
-#endif
-#ifndef NDEBUG_ABORT
-#define NDEBUG_ABORT 0
-#endif
-
 #if (NDEBUG & NDEBUG_LISTS)
 #define LIST(x,y) {printk("LINE:%d   Adding %p to %p\n", __LINE__, (void*)(x), (void*)(y)); if ((x)==(y)) udelay(5); }
 #define REMOVE(w,x,y,z) {printk("LINE:%d   Removing: %p->%p  %p->%p \n", __LINE__, (void*)(w), (void*)(x), (void*)(y), (void*)(z)); if ((x)==(y)) udelay(5); }
@@ -1005,7 +996,7 @@ static int NCR5380_queue_command_lck(Scsi_Cmnd * cmd, void (*done) (Scsi_Cmnd *)
 		LIST(cmd, tmp);
 		tmp->host_scribble = (unsigned char *) cmd;
 	}
-	dprintk(NDEBUG_QUEUES, ("scsi%d : command added to %s of queue\n", instance->host_no, (cmd->cmnd[0] == REQUEST_SENSE) ? "head" : "tail"));
+	dprintk(NDEBUG_QUEUES, "scsi%d : command added to %s of queue\n", instance->host_no, (cmd->cmnd[0] == REQUEST_SENSE) ? "head" : "tail");
 
 	/* Run the coroutine if it isn't already running. */
 	/* Kick off command processing */
@@ -1040,7 +1031,7 @@ static void NCR5380_main(struct work_struct *work)
 		/* Lock held here */
 		done = 1;
 		if (!hostdata->connected && !hostdata->selecting) {
-			dprintk(NDEBUG_MAIN, ("scsi%d : not connected\n", instance->host_no));
+			dprintk(NDEBUG_MAIN, "scsi%d : not connected\n", instance->host_no);
 			/*
 			 * Search through the issue_queue for a command destined
 			 * for a target that's not busy.
@@ -1048,7 +1039,7 @@ static void NCR5380_main(struct work_struct *work)
 			for (tmp = (Scsi_Cmnd *) hostdata->issue_queue, prev = NULL; tmp; prev = tmp, tmp = (Scsi_Cmnd *) tmp->host_scribble) 
 			{
 				if (prev != tmp)
-					dprintk(NDEBUG_LISTS, ("MAIN tmp=%p   target=%d   busy=%d lun=%d\n", tmp, tmp->target, hostdata->busy[tmp->target], tmp->lun));
+					dprintk(NDEBUG_LISTS, "MAIN tmp=%p   target=%d   busy=%d lun=%d\n", tmp, tmp->device->id, hostdata->busy[tmp->device->id], tmp->device->lun);
 				/*  When we find one, remove it from the issue queue. */
 				if (!(hostdata->busy[tmp->device->id] & (1 << tmp->device->lun))) {
 					if (prev) {
@@ -1066,7 +1057,7 @@ static void NCR5380_main(struct work_struct *work)
 					 * On failure, we must add the command back to the
 					 *   issue queue so we can keep trying. 
 					 */
-					dprintk(NDEBUG_MAIN|NDEBUG_QUEUES, ("scsi%d : main() : command for target %d lun %d removed from issue_queue\n", instance->host_no, tmp->target, tmp->lun));
+					dprintk(NDEBUG_MAIN|NDEBUG_QUEUES, "scsi%d : main() : command for target %d lun %d removed from issue_queue\n", instance->host_no, tmp->device->id, tmp->device->lun);
 	
 					/*
 					 * A successful selection is defined as one that 
@@ -1095,7 +1086,7 @@ static void NCR5380_main(struct work_struct *work)
 						tmp->host_scribble = (unsigned char *) hostdata->issue_queue;
 						hostdata->issue_queue = tmp;
 						done = 0;
-						dprintk(NDEBUG_MAIN|NDEBUG_QUEUES, ("scsi%d : main(): select() failed, returned to issue_queue\n", instance->host_no));
+						dprintk(NDEBUG_MAIN|NDEBUG_QUEUES, "scsi%d : main(): select() failed, returned to issue_queue\n", instance->host_no);
 					}
 					/* lock held here still */
 				}	/* if target/lun is not busy */
@@ -1125,9 +1116,9 @@ static void NCR5380_main(struct work_struct *work)
 #endif
 		    && (!hostdata->time_expires || time_before_eq(hostdata->time_expires, jiffies))
 		    ) {
-			dprintk(NDEBUG_MAIN, ("scsi%d : main() : performing information transfer\n", instance->host_no));
+			dprintk(NDEBUG_MAIN, "scsi%d : main() : performing information transfer\n", instance->host_no);
 			NCR5380_information_transfer(instance);
-			dprintk(NDEBUG_MAIN, ("scsi%d : main() : done set false\n", instance->host_no));
+			dprintk(NDEBUG_MAIN, "scsi%d : main() : done set false\n", instance->host_no);
 			done = 0;
 		} else
 			break;
@@ -1159,8 +1150,8 @@ static irqreturn_t NCR5380_intr(int dummy, void *dev_id)
 	unsigned char basr;
 	unsigned long flags;
 
-	dprintk(NDEBUG_INTR, ("scsi : NCR5380 irq %d triggered\n",
-		instance->irq));
+	dprintk(NDEBUG_INTR, "scsi : NCR5380 irq %d triggered\n",
+		instance->irq);
 
 	do {
 		done = 1;
@@ -1173,14 +1164,14 @@ static irqreturn_t NCR5380_intr(int dummy, void *dev_id)
 			NCR5380_dprint(NDEBUG_INTR, instance);
 			if ((NCR5380_read(STATUS_REG) & (SR_SEL | SR_IO)) == (SR_SEL | SR_IO)) {
 				done = 0;
-				dprintk(NDEBUG_INTR, ("scsi%d : SEL interrupt\n", instance->host_no));
+				dprintk(NDEBUG_INTR, "scsi%d : SEL interrupt\n", instance->host_no);
 				NCR5380_reselect(instance);
 				(void) NCR5380_read(RESET_PARITY_INTERRUPT_REG);
 			} else if (basr & BASR_PARITY_ERROR) {
-				dprintk(NDEBUG_INTR, ("scsi%d : PARITY interrupt\n", instance->host_no));
+				dprintk(NDEBUG_INTR, "scsi%d : PARITY interrupt\n", instance->host_no);
 				(void) NCR5380_read(RESET_PARITY_INTERRUPT_REG);
 			} else if ((NCR5380_read(STATUS_REG) & SR_RST) == SR_RST) {
-				dprintk(NDEBUG_INTR, ("scsi%d : RESET interrupt\n", instance->host_no));
+				dprintk(NDEBUG_INTR, "scsi%d : RESET interrupt\n", instance->host_no);
 				(void) NCR5380_read(RESET_PARITY_INTERRUPT_REG);
 			} else {
 #if defined(REAL_DMA)
@@ -1210,7 +1201,7 @@ static irqreturn_t NCR5380_intr(int dummy, void *dev_id)
 					NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
 				}
 #else
-				dprintk(NDEBUG_INTR, ("scsi : unknown interrupt, BASR 0x%X, MR 0x%X, SR 0x%x\n", basr, NCR5380_read(MODE_REG), NCR5380_read(STATUS_REG)));
+				dprintk(NDEBUG_INTR, "scsi : unknown interrupt, BASR 0x%X, MR 0x%X, SR 0x%x\n", basr, NCR5380_read(MODE_REG), NCR5380_read(STATUS_REG));
 				(void) NCR5380_read(RESET_PARITY_INTERRUPT_REG);
 #endif
 			}
@@ -1304,7 +1295,7 @@ static int NCR5380_select(struct Scsi_Host *instance, Scsi_Cmnd * cmd, int tag)
 	hostdata->restart_select = 0;
 
 	NCR5380_dprint(NDEBUG_ARBITRATION, instance);
-	dprintk(NDEBUG_ARBITRATION, ("scsi%d : starting arbitration, id = %d\n", instance->host_no, instance->this_id));
+	dprintk(NDEBUG_ARBITRATION, "scsi%d : starting arbitration, id = %d\n", instance->host_no, instance->this_id);
 
 	/* 
 	 * Set the phase bits to 0, otherwise the NCR5380 won't drive the 
@@ -1333,7 +1324,7 @@ static int NCR5380_select(struct Scsi_Host *instance, Scsi_Cmnd * cmd, int tag)
 		goto failed;
 	}
 
-	dprintk(NDEBUG_ARBITRATION, ("scsi%d : arbitration complete\n", instance->host_no));
+	dprintk(NDEBUG_ARBITRATION, "scsi%d : arbitration complete\n", instance->host_no);
 
 	/* 
 	 * The arbitration delay is 2.2us, but this is a minimum and there is 
@@ -1347,7 +1338,7 @@ static int NCR5380_select(struct Scsi_Host *instance, Scsi_Cmnd * cmd, int tag)
 	/* Check for lost arbitration */
 	if ((NCR5380_read(INITIATOR_COMMAND_REG) & ICR_ARBITRATION_LOST) || (NCR5380_read(CURRENT_SCSI_DATA_REG) & hostdata->id_higher_mask) || (NCR5380_read(INITIATOR_COMMAND_REG) & ICR_ARBITRATION_LOST)) {
 		NCR5380_write(MODE_REG, MR_BASE);
-		dprintk(NDEBUG_ARBITRATION, ("scsi%d : lost arbitration, deasserting MR_ARBITRATE\n", instance->host_no));
+		dprintk(NDEBUG_ARBITRATION, "scsi%d : lost arbitration, deasserting MR_ARBITRATE\n", instance->host_no);
 		goto failed;
 	}
 	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_SEL);
@@ -1360,7 +1351,7 @@ static int NCR5380_select(struct Scsi_Host *instance, Scsi_Cmnd * cmd, int tag)
 	    (NCR5380_read(INITIATOR_COMMAND_REG) & ICR_ARBITRATION_LOST)) {
 		NCR5380_write(MODE_REG, MR_BASE);
 		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-		dprintk(NDEBUG_ARBITRATION, ("scsi%d : lost arbitration, deasserting ICR_ASSERT_SEL\n", instance->host_no));
+		dprintk(NDEBUG_ARBITRATION, "scsi%d : lost arbitration, deasserting ICR_ASSERT_SEL\n", instance->host_no);
 		goto failed;
 	}
 	/* 
@@ -1370,7 +1361,7 @@ static int NCR5380_select(struct Scsi_Host *instance, Scsi_Cmnd * cmd, int tag)
 
 	udelay(2);
 
-	dprintk(NDEBUG_ARBITRATION, ("scsi%d : won arbitration\n", instance->host_no));
+	dprintk(NDEBUG_ARBITRATION, "scsi%d : won arbitration\n", instance->host_no);
 
 	/* 
 	 * Now that we have won arbitration, start Selection process, asserting 
@@ -1422,7 +1413,7 @@ static int NCR5380_select(struct Scsi_Host *instance, Scsi_Cmnd * cmd, int tag)
 
 	udelay(1);
 
-	dprintk(NDEBUG_SELECTION, ("scsi%d : selecting target %d\n", instance->host_no, scmd_id(cmd)));
+	dprintk(NDEBUG_SELECTION, "scsi%d : selecting target %d\n", instance->host_no, scmd_id(cmd));
 
 	/* 
 	 * The SCSI specification calls for a 250 ms timeout for the actual 
@@ -1487,7 +1478,7 @@ part2:
 		collect_stats(hostdata, cmd);
 		cmd->scsi_done(cmd);
 		NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
-		dprintk(NDEBUG_SELECTION, ("scsi%d : target did not respond within 250ms\n", instance->host_no));
+		dprintk(NDEBUG_SELECTION, "scsi%d : target did not respond within 250ms\n", instance->host_no);
 		NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
 		return 0;
 	}
@@ -1520,7 +1511,7 @@ part2:
 		goto failed;
 	}
 
-	dprintk(NDEBUG_SELECTION, ("scsi%d : target %d selected, going into MESSAGE OUT phase.\n", instance->host_no, cmd->device->id));
+	dprintk(NDEBUG_SELECTION, "scsi%d : target %d selected, going into MESSAGE OUT phase.\n", instance->host_no, cmd->device->id);
 	tmp[0] = IDENTIFY(((instance->irq == SCSI_IRQ_NONE) ? 0 : 1), cmd->device->lun);
 
 	len = 1;
@@ -1530,7 +1521,7 @@ part2:
 	data = tmp;
 	phase = PHASE_MSGOUT;
 	NCR5380_transfer_pio(instance, &phase, &len, &data);
-	dprintk(NDEBUG_SELECTION, ("scsi%d : nexus established.\n", instance->host_no));
+	dprintk(NDEBUG_SELECTION, "scsi%d : nexus established.\n", instance->host_no);
 	/* XXX need to handle errors here */
 	hostdata->connected = cmd;
 	hostdata->busy[cmd->device->id] |= (1 << cmd->device->lun);
@@ -1583,9 +1574,9 @@ static int NCR5380_transfer_pio(struct Scsi_Host *instance, unsigned char *phase
 	NCR5380_setup(instance);
 
 	if (!(p & SR_IO))
-		dprintk(NDEBUG_PIO, ("scsi%d : pio write %d bytes\n", instance->host_no, c));
+		dprintk(NDEBUG_PIO, "scsi%d : pio write %d bytes\n", instance->host_no, c);
 	else
-		dprintk(NDEBUG_PIO, ("scsi%d : pio read %d bytes\n", instance->host_no, c));
+		dprintk(NDEBUG_PIO, "scsi%d : pio read %d bytes\n", instance->host_no, c);
 
 	/* 
 	 * The NCR5380 chip will only drive the SCSI bus when the 
@@ -1620,11 +1611,11 @@ static int NCR5380_transfer_pio(struct Scsi_Host *instance, unsigned char *phase
 			break;
 		}
 
-		dprintk(NDEBUG_HANDSHAKE, ("scsi%d : REQ detected\n", instance->host_no));
+		dprintk(NDEBUG_HANDSHAKE, "scsi%d : REQ detected\n", instance->host_no);
 
 		/* Check for phase mismatch */
 		if ((tmp & PHASE_MASK) != p) {
-			dprintk(NDEBUG_HANDSHAKE, ("scsi%d : phase mismatch\n", instance->host_no));
+			dprintk(NDEBUG_HANDSHAKE, "scsi%d : phase mismatch\n", instance->host_no);
 			NCR5380_dprint_phase(NDEBUG_HANDSHAKE, instance);
 			break;
 		}
@@ -1660,7 +1651,7 @@ static int NCR5380_transfer_pio(struct Scsi_Host *instance, unsigned char *phase
 
 		/* FIXME - if this fails bus reset ?? */
 		NCR5380_poll_politely(instance, STATUS_REG, SR_REQ, 0, 5*HZ);
-		dprintk(NDEBUG_HANDSHAKE, ("scsi%d : req false, handshake complete\n", instance->host_no));
+		dprintk(NDEBUG_HANDSHAKE, "scsi%d : req false, handshake complete\n", instance->host_no);
 
 /*
  * We have several special cases to consider during REQ/ACK handshaking : 
@@ -1681,7 +1672,7 @@ static int NCR5380_transfer_pio(struct Scsi_Host *instance, unsigned char *phase
 		}
 	} while (--c);
 
-	dprintk(NDEBUG_PIO, ("scsi%d : residual %d\n", instance->host_no, c));
+	dprintk(NDEBUG_PIO, "scsi%d : residual %d\n", instance->host_no, c);
 
 	*count = c;
 	*data = d;
@@ -1828,7 +1819,7 @@ static int NCR5380_transfer_dma(struct Scsi_Host *instance, unsigned char *phase
 		c -= 2;
 	}
 #endif
-	dprintk(NDEBUG_DMA, ("scsi%d : initializing DMA channel %d for %s, %d bytes %s %0x\n", instance->host_no, instance->dma_channel, (p & SR_IO) ? "reading" : "writing", c, (p & SR_IO) ? "to" : "from", (unsigned) d));
+	dprintk(NDEBUG_DMA, "scsi%d : initializing DMA channel %d for %s, %d bytes %s %0x\n", instance->host_no, instance->dma_channel, (p & SR_IO) ? "reading" : "writing", c, (p & SR_IO) ? "to" : "from", (unsigned) d);
 	hostdata->dma_len = (p & SR_IO) ? NCR5380_dma_read_setup(instance, d, c) : NCR5380_dma_write_setup(instance, d, c);
 #endif
 
@@ -1857,7 +1848,7 @@ static int NCR5380_transfer_dma(struct Scsi_Host *instance, unsigned char *phase
 		NCR5380_write(MODE_REG, MR_BASE | MR_DMA_MODE);
 #endif				/* def REAL_DMA */
 
-	dprintk(NDEBUG_DMA, ("scsi%d : mode reg = 0x%X\n", instance->host_no, NCR5380_read(MODE_REG)));
+	dprintk(NDEBUG_DMA, "scsi%d : mode reg = 0x%X\n", instance->host_no, NCR5380_read(MODE_REG));
 
 	/* 
 	 *	On the PAS16 at least I/O recovery delays are not needed here.
@@ -1934,7 +1925,7 @@ static int NCR5380_transfer_dma(struct Scsi_Host *instance, unsigned char *phase
 		}
 	}
 
-	dprintk(NDEBUG_DMA, ("scsi%d : polled DMA transfer complete, basr 0x%X, sr 0x%X\n", instance->host_no, tmp, NCR5380_read(STATUS_REG)));
+	dprintk(NDEBUG_DMA, "scsi%d : polled DMA transfer complete, basr 0x%X, sr 0x%X\n", instance->host_no, tmp, NCR5380_read(STATUS_REG));
 
 	NCR5380_write(MODE_REG, MR_BASE);
 	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
@@ -1948,7 +1939,7 @@ static int NCR5380_transfer_dma(struct Scsi_Host *instance, unsigned char *phase
 #ifdef READ_OVERRUNS
 	if (*phase == p && (p & SR_IO) && residue == 0) {
 		if (overrun) {
-			dprintk(NDEBUG_DMA, ("Got an input overrun, using saved byte\n"));
+			dprintk(NDEBUG_DMA, "Got an input overrun, using saved byte\n");
 			**data = saved_data;
 			*data += 1;
 			*count -= 1;
@@ -1957,13 +1948,13 @@ static int NCR5380_transfer_dma(struct Scsi_Host *instance, unsigned char *phase
 			printk("No overrun??\n");
 			cnt = toPIO = 2;
 		}
-		dprintk(NDEBUG_DMA, ("Doing %d-byte PIO to 0x%X\n", cnt, *data));
+		dprintk(NDEBUG_DMA, "Doing %d-byte PIO to 0x%X\n", cnt, *data);
 		NCR5380_transfer_pio(instance, phase, &cnt, data);
 		*count -= toPIO - cnt;
 	}
 #endif
 
-	dprintk(NDEBUG_DMA, ("Return with data ptr = 0x%X, count %d, last 0x%X, next 0x%X\n", *data, *count, *(*data + *count - 1), *(*data + *count)));
+	dprintk(NDEBUG_DMA, "Return with data ptr = 0x%X, count %d, last 0x%X, next 0x%X\n", *data, *count, *(*data + *count - 1), *(*data + *count));
 	return 0;
 
 #elif defined(REAL_DMA)
@@ -2013,7 +2004,7 @@ static int NCR5380_transfer_dma(struct Scsi_Host *instance, unsigned char *phase
 		foo = NCR5380_pwrite(instance, d, c);
 #else
 		int timeout;
-		dprintk(NDEBUG_C400_PWRITE, ("About to pwrite %d bytes\n", c));
+		dprintk(NDEBUG_C400_PWRITE, "About to pwrite %d bytes\n", c);
 		if (!(foo = NCR5380_pwrite(instance, d, c))) {
 			/*
 			 * Wait for the last byte to be sent.  If REQ is being asserted for 
@@ -2024,19 +2015,19 @@ static int NCR5380_transfer_dma(struct Scsi_Host *instance, unsigned char *phase
 				while (!(NCR5380_read(BUS_AND_STATUS_REG) & BASR_DRQ) && (NCR5380_read(BUS_AND_STATUS_REG) & BASR_PHASE_MATCH));
 
 				if (!timeout)
-					dprintk(NDEBUG_LAST_BYTE_SENT, ("scsi%d : timed out on last byte\n", instance->host_no));
+					dprintk(NDEBUG_LAST_BYTE_SENT, "scsi%d : timed out on last byte\n", instance->host_no);
 
 				if (hostdata->flags & FLAG_CHECK_LAST_BYTE_SENT) {
 					hostdata->flags &= ~FLAG_CHECK_LAST_BYTE_SENT;
 					if (NCR5380_read(TARGET_COMMAND_REG) & TCR_LAST_BYTE_SENT) {
 						hostdata->flags |= FLAG_HAS_LAST_BYTE_SENT;
-						dprintk(NDEBUG_LAST_WRITE_SENT, ("scsi%d : last bit sent works\n", instance->host_no));
+						dprintk(NDEBUG_LAST_BYTE_SENT, "scsi%d : last byte sent works\n", instance->host_no);
 					}
 				}
 			} else {
-				dprintk(NDEBUG_C400_PWRITE, ("Waiting for LASTBYTE\n"));
+				dprintk(NDEBUG_C400_PWRITE, "Waiting for LASTBYTE\n");
 				while (!(NCR5380_read(TARGET_COMMAND_REG) & TCR_LAST_BYTE_SENT));
-				dprintk(NDEBUG_C400_PWRITE, ("Got LASTBYTE\n"));
+				dprintk(NDEBUG_C400_PWRITE, "Got LASTBYTE\n");
 			}
 		}
 #endif
@@ -2045,9 +2036,9 @@ static int NCR5380_transfer_dma(struct Scsi_Host *instance, unsigned char *phase
 	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
 
 	if ((!(p & SR_IO)) && (hostdata->flags & FLAG_NCR53C400)) {
-		dprintk(NDEBUG_C400_PWRITE, ("53C400w: Checking for IRQ\n"));
+		dprintk(NDEBUG_C400_PWRITE, "53C400w: Checking for IRQ\n");
 		if (NCR5380_read(BUS_AND_STATUS_REG) & BASR_IRQ) {
-			dprintk(NDEBUG_C400_PWRITE, ("53C400w:    got it, reading reset interrupt reg\n"));
+			dprintk(NDEBUG_C400_PWRITE, "53C400w:    got it, reading reset interrupt reg\n");
 			NCR5380_read(RESET_PARITY_INTERRUPT_REG);
 		} else {
 			printk("53C400w:    IRQ NOT THERE!\n");
@@ -2139,7 +2130,7 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance) {
 					--cmd->SCp.buffers_residual;
 					cmd->SCp.this_residual = cmd->SCp.buffer->length;
 					cmd->SCp.ptr = sg_virt(cmd->SCp.buffer);
-					dprintk(NDEBUG_INFORMATION, ("scsi%d : %d bytes and %d buffers left\n", instance->host_no, cmd->SCp.this_residual, cmd->SCp.buffers_residual));
+					dprintk(NDEBUG_INFORMATION, "scsi%d : %d bytes and %d buffers left\n", instance->host_no, cmd->SCp.this_residual, cmd->SCp.buffers_residual);
 				}
 				/*
 				 * The preferred transfer method is going to be 
@@ -2219,7 +2210,7 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance) {
 				case LINKED_FLG_CMD_COMPLETE:
 					/* Accept message by clearing ACK */
 					NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-					dprintk(NDEBUG_LINKED, ("scsi%d : target %d lun %d linked command complete.\n", instance->host_no, cmd->device->id, cmd->device->lun));
+					dprintk(NDEBUG_LINKED, "scsi%d : target %d lun %d linked command complete.\n", instance->host_no, cmd->device->id, cmd->device->lun);
 					/* 
 					 * Sanity check : A linked command should only terminate with
 					 * one of these messages if there are more linked commands
@@ -2235,7 +2226,7 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance) {
 					/* The next command is still part of this process */
 					cmd->next_link->tag = cmd->tag;
 					cmd->result = cmd->SCp.Status | (cmd->SCp.Message << 8);
-					dprintk(NDEBUG_LINKED, ("scsi%d : target %d lun %d linked request done, calling scsi_done().\n", instance->host_no, cmd->device->id, cmd->device->lun));
+					dprintk(NDEBUG_LINKED, "scsi%d : target %d lun %d linked request done, calling scsi_done().\n", instance->host_no, cmd->device->id, cmd->device->lun);
 					collect_stats(hostdata, cmd);
 					cmd->scsi_done(cmd);
 					cmd = hostdata->connected;
@@ -2247,7 +2238,7 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance) {
 					sink = 1;
 					NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
 					hostdata->connected = NULL;
-					dprintk(NDEBUG_QUEUES, ("scsi%d : command for target %d, lun %d completed\n", instance->host_no, cmd->device->id, cmd->device->lun));
+					dprintk(NDEBUG_QUEUES, "scsi%d : command for target %d, lun %d completed\n", instance->host_no, cmd->device->id, cmd->device->lun);
 					hostdata->busy[cmd->device->id] &= ~(1 << cmd->device->lun);
 
 					/* 
@@ -2281,13 +2272,13 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance) {
 					if ((cmd->cmnd[0] != REQUEST_SENSE) && (status_byte(cmd->SCp.Status) == CHECK_CONDITION)) {
 						scsi_eh_prep_cmnd(cmd, &hostdata->ses, NULL, 0, ~0);
 
-						dprintk(NDEBUG_AUTOSENSE, ("scsi%d : performing request sense\n", instance->host_no));
+						dprintk(NDEBUG_AUTOSENSE, "scsi%d : performing request sense\n", instance->host_no);
 
 						LIST(cmd, hostdata->issue_queue);
 						cmd->host_scribble = (unsigned char *)
 						    hostdata->issue_queue;
 						hostdata->issue_queue = (Scsi_Cmnd *) cmd;
-						dprintk(NDEBUG_QUEUES, ("scsi%d : REQUEST SENSE added to head of issue queue\n", instance->host_no));
+						dprintk(NDEBUG_QUEUES, "scsi%d : REQUEST SENSE added to head of issue queue\n", instance->host_no);
 					} else
 #endif				/* def AUTOSENSE */
 					{
@@ -2327,7 +2318,7 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance) {
 						    hostdata->disconnected_queue;
 						hostdata->connected = NULL;
 						hostdata->disconnected_queue = cmd;
-						dprintk(NDEBUG_QUEUES, ("scsi%d : command for target %d lun %d was moved from connected to" "  the disconnected_queue\n", instance->host_no, cmd->device->id, cmd->device->lun));
+						dprintk(NDEBUG_QUEUES, "scsi%d : command for target %d lun %d was moved from connected to" "  the disconnected_queue\n", instance->host_no, cmd->device->id, cmd->device->lun);
 						/* 
 						 * Restore phase bits to 0 so an interrupted selection, 
 						 * arbitration can resume.
@@ -2373,14 +2364,14 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance) {
 					extended_msg[0] = EXTENDED_MESSAGE;
 					/* Accept first byte by clearing ACK */
 					NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-					dprintk(NDEBUG_EXTENDED, ("scsi%d : receiving extended message\n", instance->host_no));
+					dprintk(NDEBUG_EXTENDED, "scsi%d : receiving extended message\n", instance->host_no);
 
 					len = 2;
 					data = extended_msg + 1;
 					phase = PHASE_MSGIN;
 					NCR5380_transfer_pio(instance, &phase, &len, &data);
 
-					dprintk(NDEBUG_EXTENDED, ("scsi%d : length=%d, code=0x%02x\n", instance->host_no, (int) extended_msg[1], (int) extended_msg[2]));
+					dprintk(NDEBUG_EXTENDED, "scsi%d : length=%d, code=0x%02x\n", instance->host_no, (int) extended_msg[1], (int) extended_msg[2]);
 
 					if (!len && extended_msg[1] <= (sizeof(extended_msg) - 1)) {
 						/* Accept third byte by clearing ACK */
@@ -2390,7 +2381,7 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance) {
 						phase = PHASE_MSGIN;
 
 						NCR5380_transfer_pio(instance, &phase, &len, &data);
-						dprintk(NDEBUG_EXTENDED, ("scsi%d : message received, residual %d\n", instance->host_no, len));
+						dprintk(NDEBUG_EXTENDED, "scsi%d : message received, residual %d\n", instance->host_no, len);
 
 						switch (extended_msg[2]) {
 						case EXTENDED_SDTR:
@@ -2456,7 +2447,7 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance) {
 				NCR5380_transfer_pio(instance, &phase, &len, &data);
 				if (!cmd->device->disconnect && should_disconnect(cmd->cmnd[0])) {
 					NCR5380_set_timer(hostdata, USLEEP_SLEEP);
-					dprintk(NDEBUG_USLEEP, ("scsi%d : issued command, sleeping until %ul\n", instance->host_no, hostdata->time_expires));
+					dprintk(NDEBUG_USLEEP, "scsi%d : issued command, sleeping until %lu\n", instance->host_no, hostdata->time_expires);
 					return;
 				}
 				break;
@@ -2468,7 +2459,7 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance) {
 				break;
 			default:
 				printk("scsi%d : unknown phase\n", instance->host_no);
-				NCR5380_dprint(NDEBUG_ALL, instance);
+				NCR5380_dprint(NDEBUG_ANY, instance);
 			}	/* switch(phase) */
 		}		/* if (tmp * SR_REQ) */
 		else {
@@ -2476,7 +2467,7 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance) {
 			 */
 			if (!cmd->device->disconnect && time_after_eq(jiffies, poll_time)) {
 				NCR5380_set_timer(hostdata, USLEEP_SLEEP);
-				dprintk(NDEBUG_USLEEP, ("scsi%d : poll timed out, sleeping until %ul\n", instance->host_no, hostdata->time_expires));
+				dprintk(NDEBUG_USLEEP, "scsi%d : poll timed out, sleeping until %lu\n", instance->host_no, hostdata->time_expires);
 				return;
 			}
 		}
@@ -2517,7 +2508,7 @@ static void NCR5380_reselect(struct Scsi_Host *instance) {
 	hostdata->restart_select = 1;
 
 	target_mask = NCR5380_read(CURRENT_SCSI_DATA_REG) & ~(hostdata->id_mask);
-	dprintk(NDEBUG_SELECTION, ("scsi%d : reselect\n", instance->host_no));
+	dprintk(NDEBUG_SELECTION, "scsi%d : reselect\n", instance->host_no);
 
 	/* 
 	 * At this point, we have detected that our SCSI ID is on the bus,
@@ -2597,7 +2588,7 @@ static void NCR5380_reselect(struct Scsi_Host *instance) {
 		do_abort(instance);
 	} else {
 		hostdata->connected = tmp;
-		dprintk(NDEBUG_RESELECTION, ("scsi%d : nexus established, target = %d, lun = %d, tag = %d\n", instance->host_no, tmp->target, tmp->lun, tmp->tag));
+		dprintk(NDEBUG_RESELECTION, "scsi%d : nexus established, target = %d, lun = %d, tag = %d\n", instance->host_no, tmp->device->id, tmp->device->lun, tmp->tag);
 	}
 }
 
@@ -2682,8 +2673,8 @@ static int NCR5380_abort(Scsi_Cmnd * cmd) {
 
 	NCR5380_setup(instance);
 
-	dprintk(NDEBUG_ABORT, ("scsi%d : abort called\n", instance->host_no));
-	dprintk(NDEBUG_ABORT, ("        basr 0x%X, sr 0x%X\n", NCR5380_read(BUS_AND_STATUS_REG), NCR5380_read(STATUS_REG)));
+	dprintk(NDEBUG_ABORT, "scsi%d : abort called\n", instance->host_no);
+	dprintk(NDEBUG_ABORT, "        basr 0x%X, sr 0x%X\n", NCR5380_read(BUS_AND_STATUS_REG), NCR5380_read(STATUS_REG));
 
 #if 0
 /*
@@ -2693,7 +2684,7 @@ static int NCR5380_abort(Scsi_Cmnd * cmd) {
  */
 
 	if (hostdata->connected == cmd) {
-		dprintk(NDEBUG_ABORT, ("scsi%d : aborting connected command\n", instance->host_no));
+		dprintk(NDEBUG_ABORT, "scsi%d : aborting connected command\n", instance->host_no);
 		hostdata->aborted = 1;
 /*
  * We should perform BSY checking, and make sure we haven't slipped
@@ -2721,14 +2712,14 @@ static int NCR5380_abort(Scsi_Cmnd * cmd) {
  *          from the issue queue.
  */
  
-	dprintk(NDEBUG_ABORT, ("scsi%d : abort going into loop.\n", instance->host_no));
+	dprintk(NDEBUG_ABORT, "scsi%d : abort going into loop.\n", instance->host_no);
 	for (prev = (Scsi_Cmnd **) & (hostdata->issue_queue), tmp = (Scsi_Cmnd *) hostdata->issue_queue; tmp; prev = (Scsi_Cmnd **) & (tmp->host_scribble), tmp = (Scsi_Cmnd *) tmp->host_scribble)
 		if (cmd == tmp) {
 			REMOVE(5, *prev, tmp, tmp->host_scribble);
 			(*prev) = (Scsi_Cmnd *) tmp->host_scribble;
 			tmp->host_scribble = NULL;
 			tmp->result = DID_ABORT << 16;
-			dprintk(NDEBUG_ABORT, ("scsi%d : abort removed command from issue queue.\n", instance->host_no));
+			dprintk(NDEBUG_ABORT, "scsi%d : abort removed command from issue queue.\n", instance->host_no);
 			tmp->scsi_done(tmp);
 			return SUCCESS;
 		}
@@ -2750,7 +2741,7 @@ static int NCR5380_abort(Scsi_Cmnd * cmd) {
  */
 
 	if (hostdata->connected) {
-		dprintk(NDEBUG_ABORT, ("scsi%d : abort failed, command connected.\n", instance->host_no));
+		dprintk(NDEBUG_ABORT, "scsi%d : abort failed, command connected.\n", instance->host_no);
 		return FAILED;
 	}
 /*
@@ -2780,11 +2771,11 @@ static int NCR5380_abort(Scsi_Cmnd * cmd) {
 
 	for (tmp = (Scsi_Cmnd *) hostdata->disconnected_queue; tmp; tmp = (Scsi_Cmnd *) tmp->host_scribble)
 		if (cmd == tmp) {
-			dprintk(NDEBUG_ABORT, ("scsi%d : aborting disconnected command.\n", instance->host_no));
+			dprintk(NDEBUG_ABORT, "scsi%d : aborting disconnected command.\n", instance->host_no);
 
 			if (NCR5380_select(instance, cmd, (int) cmd->tag))
 				return FAILED;
-			dprintk(NDEBUG_ABORT, ("scsi%d : nexus reestablished.\n", instance->host_no));
+			dprintk(NDEBUG_ABORT, "scsi%d : nexus reestablished.\n", instance->host_no);
 
 			do_abort(instance);
 
diff --git a/drivers/scsi/NCR5380.h b/drivers/scsi/NCR5380.h
index 14964d0a0e9..c79ddfa6f53 100644
--- a/drivers/scsi/NCR5380.h
+++ b/drivers/scsi/NCR5380.h
@@ -21,10 +21,6 @@
  * 1+ (800) 334-5454
  */
 
-/*
- * $Log: NCR5380.h,v $
- */
-
 #ifndef NCR5380_H
 #define NCR5380_H
 
@@ -60,6 +56,9 @@
 #define NDEBUG_C400_PREAD	0x100000
 #define NDEBUG_C400_PWRITE	0x200000
 #define NDEBUG_LISTS		0x400000
+#define NDEBUG_ABORT		0x800000
+#define NDEBUG_TAGS		0x1000000
+#define NDEBUG_MERGING		0x2000000
 
 #define NDEBUG_ANY		0xFFFFFFFFUL
 
@@ -292,9 +291,24 @@ struct NCR5380_hostdata {
 
 #ifdef __KERNEL__
 
-#define dprintk(a,b)			do {} while(0)
-#define NCR5380_dprint(a,b)		do {} while(0)
-#define NCR5380_dprint_phase(a,b)	do {} while(0)
+#ifndef NDEBUG
+#define NDEBUG (0)
+#endif
+
+#define dprintk(flg, fmt, ...) \
+	do { if ((NDEBUG) & (flg)) pr_debug(fmt, ## __VA_ARGS__); } while (0)
+
+#if NDEBUG
+#define NCR5380_dprint(flg, arg) \
+	do { if ((NDEBUG) & (flg)) NCR5380_print(arg); } while (0)
+#define NCR5380_dprint_phase(flg, arg) \
+	do { if ((NDEBUG) & (flg)) NCR5380_print_phase(arg); } while (0)
+static void NCR5380_print_phase(struct Scsi_Host *instance);
+static void NCR5380_print(struct Scsi_Host *instance);
+#else
+#define NCR5380_dprint(flg, arg)       do {} while (0)
+#define NCR5380_dprint_phase(flg, arg) do {} while (0)
+#endif
 
 #if defined(AUTOPROBE_IRQ)
 static int NCR5380_probe_irq(struct Scsi_Host *instance, int possible);
@@ -307,10 +321,6 @@ static irqreturn_t NCR5380_intr(int irq, void *dev_id);
 #endif
 static void NCR5380_main(struct work_struct *work);
 static void __maybe_unused NCR5380_print_options(struct Scsi_Host *instance);
-#ifdef NDEBUG
-static void NCR5380_print_phase(struct Scsi_Host *instance);
-static void NCR5380_print(struct Scsi_Host *instance);
-#endif
 static int NCR5380_abort(Scsi_Cmnd * cmd);
 static int NCR5380_bus_reset(Scsi_Cmnd * cmd);
 static int NCR5380_queue_command(struct Scsi_Host *, struct scsi_cmnd *);
diff --git a/drivers/scsi/aic7xxx/aic79xx.h b/drivers/scsi/aic7xxx/aic79xx.h
index 9b059422aac..113874c1284 100644
--- a/drivers/scsi/aic7xxx/aic79xx.h
+++ b/drivers/scsi/aic7xxx/aic79xx.h
@@ -911,7 +911,7 @@ struct vpd_config {
 	uint8_t  length;
 	uint8_t  revision;
 	uint8_t  device_flags;
-	uint8_t  termnation_menus[2];
+	uint8_t  termination_menus[2];
 	uint8_t  fifo_threshold;
 	uint8_t  end_tag;
 	uint8_t  vpd_checksum;
diff --git a/drivers/scsi/aic7xxx/aic79xx_pci.c b/drivers/scsi/aic7xxx/aic79xx_pci.c
index 14b5f8d0e7f..cc9bd26f5d1 100644
--- a/drivers/scsi/aic7xxx/aic79xx_pci.c
+++ b/drivers/scsi/aic7xxx/aic79xx_pci.c
@@ -827,7 +827,7 @@ ahd_pci_intr(struct ahd_softc *ahd)
 		for (bit = 0; bit < 8; bit++) {
 
 			if ((pci_status[i] & (0x1 << bit)) != 0) {
-				static const char *s;
+				const char *s;
 
 				s = pci_status_strings[bit];
 				if (i == 7/*TARG*/ && bit == 3)
@@ -887,23 +887,15 @@ ahd_pci_split_intr(struct ahd_softc *ahd, u_int intstat)
 
 		for (bit = 0; bit < 8; bit++) {
 
-			if ((split_status[i] & (0x1 << bit)) != 0) {
-				static const char *s;
-
-				s = split_status_strings[bit];
-				printk(s, ahd_name(ahd),
+			if ((split_status[i] & (0x1 << bit)) != 0)
+				printk(split_status_strings[bit], ahd_name(ahd),
 				       split_status_source[i]);
-			}
 
 			if (i > 1)
 				continue;
 
-			if ((sg_split_status[i] & (0x1 << bit)) != 0) {
-				static const char *s;
-
-				s = split_status_strings[bit];
-				printk(s, ahd_name(ahd), "SG");
-			}
+			if ((sg_split_status[i] & (0x1 << bit)) != 0)
+				printk(split_status_strings[bit], ahd_name(ahd), "SG");
 		}
 	}
 	/*
diff --git a/drivers/scsi/aic7xxx/aic7xxx_osm.c b/drivers/scsi/aic7xxx/aic7xxx_osm.c
index c0c62583b54..114ff0c6e31 100644
--- a/drivers/scsi/aic7xxx/aic7xxx_osm.c
+++ b/drivers/scsi/aic7xxx/aic7xxx_osm.c
@@ -145,16 +145,6 @@ static struct scsi_transport_template *ahc_linux_transport_template = NULL;
 #endif
 
 /*
- * Control collection of SCSI transfer statistics for the /proc filesystem.
- *
- * NOTE: Do NOT enable this when running on kernels version 1.2.x and below.
- * NOTE: This does affect performance since it has to maintain statistics.
- */
-#ifdef CONFIG_AIC7XXX_PROC_STATS
-#define AIC7XXX_PROC_STATS
-#endif
-
-/*
  * To change the default number of tagged transactions allowed per-device,
  * add a line to the lilo.conf file like:
  * append="aic7xxx=verbose,tag_info:{{32,32,32,32},{32,32,32,32}}"
diff --git a/drivers/scsi/arm/acornscsi.c b/drivers/scsi/arm/acornscsi.c
index 059ff477a39..2e797a36760 100644
--- a/drivers/scsi/arm/acornscsi.c
+++ b/drivers/scsi/arm/acornscsi.c
@@ -62,13 +62,6 @@
  */
 #undef CONFIG_SCSI_ACORNSCSI_TAGGED_QUEUE
 /*
- * SCSI-II Linked command support.
- *
- * The higher level code doesn't support linked commands yet, and so the option
- * is undef'd here.
- */
-#undef CONFIG_SCSI_ACORNSCSI_LINK
-/*
  * SCSI-II Synchronous transfer support.
  *
  * Tried and tested...
@@ -160,10 +153,6 @@
 #error "Yippee!  ABORT TAG is now defined!  Remove this error!"
 #endif
 
-#ifdef CONFIG_SCSI_ACORNSCSI_LINK
-#error SCSI2 LINKed commands not supported (yet)!
-#endif
-
 #ifdef USE_DMAC
 /*
  * DMAC setup parameters
@@ -1668,42 +1657,6 @@ void acornscsi_message(AS_Host *host)
 	}
 	break;
 
-#ifdef CONFIG_SCSI_ACORNSCSI_LINK
-    case LINKED_CMD_COMPLETE:
-    case LINKED_FLG_CMD_COMPLETE:
-	/*
-	 * We don't support linked commands yet
-	 */
-	if (0) {
-#if (DEBUG & DEBUG_LINK)
-	    printk("scsi%d.%c: lun %d tag %d linked command complete\n",
-		    host->host->host_no, acornscsi_target(host), host->SCpnt->tag);
-#endif
-	    /*
-	     * A linked command should only terminate with one of these messages
-	     * if there are more linked commands available.
-	     */
-	    if (!host->SCpnt->next_link) {
-		printk(KERN_WARNING "scsi%d.%c: lun %d tag %d linked command complete, but no next_link\n",
-			instance->host_no, acornscsi_target(host), host->SCpnt->tag);
-		acornscsi_sbic_issuecmd(host, CMND_ASSERTATN);
-		msgqueue_addmsg(&host->scsi.msgs, 1, ABORT);
-	    } else {
-		struct scsi_cmnd *SCpnt = host->SCpnt;
-
-		acornscsi_dma_cleanup(host);
-
-		host->SCpnt = host->SCpnt->next_link;
-		host->SCpnt->tag = SCpnt->tag;
-		SCpnt->result = DID_OK | host->scsi.SCp.Message << 8 | host->Scsi.SCp.Status;
-		SCpnt->done(SCpnt);
-
-		/* initialise host->SCpnt->SCp */
-	    }
-	    break;
-	}
-#endif
-
     default: /* reject message */
 	printk(KERN_ERR "scsi%d.%c: unrecognised message %02X, rejecting\n",
 		host->host->host_no, acornscsi_target(host),
@@ -2825,9 +2778,6 @@ char *acornscsi_info(struct Scsi_Host *host)
 #ifdef CONFIG_SCSI_ACORNSCSI_TAGGED_QUEUE
     " TAG"
 #endif
-#ifdef CONFIG_SCSI_ACORNSCSI_LINK
-    " LINK"
-#endif
 #if (DEBUG & DEBUG_NO_WRITE)
     " NOWRITE (" __stringify(NO_WRITE) ")"
 #endif
@@ -2851,9 +2801,6 @@ static int acornscsi_show_info(struct seq_file *m, struct Scsi_Host *instance)
 #ifdef CONFIG_SCSI_ACORNSCSI_TAGGED_QUEUE
     " TAG"
 #endif
-#ifdef CONFIG_SCSI_ACORNSCSI_LINK
-    " LINK"
-#endif
 #if (DEBUG & DEBUG_NO_WRITE)
     " NOWRITE (" __stringify(NO_WRITE) ")"
 #endif
diff --git a/drivers/scsi/arm/cumana_1.c b/drivers/scsi/arm/cumana_1.c
index f8e06090005..8ef810a4476 100644
--- a/drivers/scsi/arm/cumana_1.c
+++ b/drivers/scsi/arm/cumana_1.c
@@ -36,9 +36,6 @@
 	void __iomem *base;		\
 	void __iomem *dma
 
-#define BOARD_NORMAL	0
-#define BOARD_NCR53C400	1
-
 #include "../NCR5380.h"
 
 void cumanascsi_setup(char *str, int *ints)
diff --git a/drivers/scsi/arm/oak.c b/drivers/scsi/arm/oak.c
index 4266eef8aca..188e734c7ff 100644
--- a/drivers/scsi/arm/oak.c
+++ b/drivers/scsi/arm/oak.c
@@ -37,9 +37,6 @@
 #define NCR5380_implementation_fields	\
 	void __iomem *base
 
-#define BOARD_NORMAL	0
-#define BOARD_NCR53C400	1
-
 #include "../NCR5380.h"
 
 #undef START_DMA_INITIATOR_RECEIVE_REG
diff --git a/drivers/scsi/atari_NCR5380.c b/drivers/scsi/atari_NCR5380.c
index 0f3cdbc80ba..1814aa20b72 100644
--- a/drivers/scsi/atari_NCR5380.c
+++ b/drivers/scsi/atari_NCR5380.c
@@ -370,7 +370,7 @@ static int is_lun_busy(Scsi_Cmnd *cmd, int should_be_tagged)
 		return 0;
 	if (TagAlloc[cmd->device->id][cmd->device->lun].nr_allocated >=
 	    TagAlloc[cmd->device->id][cmd->device->lun].queue_size) {
-		TAG_PRINTK("scsi%d: target %d lun %d: no free tags\n",
+		dprintk(NDEBUG_TAGS, "scsi%d: target %d lun %d: no free tags\n",
 			   H_NO(cmd), cmd->device->id, cmd->device->lun);
 		return 1;
 	}
@@ -394,7 +394,7 @@ static void cmd_get_tag(Scsi_Cmnd *cmd, int should_be_tagged)
 	    !setup_use_tagged_queuing || !cmd->device->tagged_supported) {
 		cmd->tag = TAG_NONE;
 		hostdata->busy[cmd->device->id] |= (1 << cmd->device->lun);
-		TAG_PRINTK("scsi%d: target %d lun %d now allocated by untagged "
+		dprintk(NDEBUG_TAGS, "scsi%d: target %d lun %d now allocated by untagged "
 			   "command\n", H_NO(cmd), cmd->device->id, cmd->device->lun);
 	} else {
 		TAG_ALLOC *ta = &TagAlloc[cmd->device->id][cmd->device->lun];
@@ -402,7 +402,7 @@ static void cmd_get_tag(Scsi_Cmnd *cmd, int should_be_tagged)
 		cmd->tag = find_first_zero_bit(ta->allocated, MAX_TAGS);
 		set_bit(cmd->tag, ta->allocated);
 		ta->nr_allocated++;
-		TAG_PRINTK("scsi%d: using tag %d for target %d lun %d "
+		dprintk(NDEBUG_TAGS, "scsi%d: using tag %d for target %d lun %d "
 			   "(now %d tags in use)\n",
 			   H_NO(cmd), cmd->tag, cmd->device->id,
 			   cmd->device->lun, ta->nr_allocated);
@@ -420,7 +420,7 @@ static void cmd_free_tag(Scsi_Cmnd *cmd)
 
 	if (cmd->tag == TAG_NONE) {
 		hostdata->busy[cmd->device->id] &= ~(1 << cmd->device->lun);
-		TAG_PRINTK("scsi%d: target %d lun %d untagged cmd finished\n",
+		dprintk(NDEBUG_TAGS, "scsi%d: target %d lun %d untagged cmd finished\n",
 			   H_NO(cmd), cmd->device->id, cmd->device->lun);
 	} else if (cmd->tag >= MAX_TAGS) {
 		printk(KERN_NOTICE "scsi%d: trying to free bad tag %d!\n",
@@ -429,7 +429,7 @@ static void cmd_free_tag(Scsi_Cmnd *cmd)
 		TAG_ALLOC *ta = &TagAlloc[cmd->device->id][cmd->device->lun];
 		clear_bit(cmd->tag, ta->allocated);
 		ta->nr_allocated--;
-		TAG_PRINTK("scsi%d: freed tag %d for target %d lun %d\n",
+		dprintk(NDEBUG_TAGS, "scsi%d: freed tag %d for target %d lun %d\n",
 			   H_NO(cmd), cmd->tag, cmd->device->id, cmd->device->lun);
 	}
 }
@@ -478,7 +478,7 @@ static void merge_contiguous_buffers(Scsi_Cmnd *cmd)
 	for (endaddr = virt_to_phys(cmd->SCp.ptr + cmd->SCp.this_residual - 1) + 1;
 	     cmd->SCp.buffers_residual &&
 	     virt_to_phys(sg_virt(&cmd->SCp.buffer[1])) == endaddr;) {
-		MER_PRINTK("VTOP(%p) == %08lx -> merging\n",
+		dprintk(NDEBUG_MERGING, "VTOP(%p) == %08lx -> merging\n",
 			   page_address(sg_page(&cmd->SCp.buffer[1])), endaddr);
 #if (NDEBUG & NDEBUG_MERGING)
 		++cnt;
@@ -490,7 +490,7 @@ static void merge_contiguous_buffers(Scsi_Cmnd *cmd)
 	}
 #if (NDEBUG & NDEBUG_MERGING)
 	if (oldlen != cmd->SCp.this_residual)
-		MER_PRINTK("merged %d buffers from %p, new length %08x\n",
+		dprintk(NDEBUG_MERGING, "merged %d buffers from %p, new length %08x\n",
 			   cnt, cmd->SCp.ptr, cmd->SCp.this_residual);
 #endif
 }
@@ -626,16 +626,6 @@ static void NCR5380_print_phase(struct Scsi_Host *instance)
 	}
 }
 
-#else /* !NDEBUG */
-
-/* dummies... */
-static inline void NCR5380_print(struct Scsi_Host *instance)
-{
-};
-static inline void NCR5380_print_phase(struct Scsi_Host *instance)
-{
-};
-
 #endif
 
 /*
@@ -676,7 +666,7 @@ static inline void NCR5380_all_init(void)
 {
 	static int done = 0;
 	if (!done) {
-		INI_PRINTK("scsi : NCR5380_all_init()\n");
+		dprintk(NDEBUG_INIT, "scsi : NCR5380_all_init()\n");
 		done = 1;
 	}
 }
@@ -739,8 +729,8 @@ static void NCR5380_print_status(struct Scsi_Host *instance)
 	Scsi_Cmnd *ptr;
 	unsigned long flags;
 
-	NCR_PRINT(NDEBUG_ANY);
-	NCR_PRINT_PHASE(NDEBUG_ANY);
+	NCR5380_dprint(NDEBUG_ANY, instance);
+	NCR5380_dprint_phase(NDEBUG_ANY, instance);
 
 	hostdata = (struct NCR5380_hostdata *)instance->hostdata;
 
@@ -984,7 +974,7 @@ static int NCR5380_queue_command_lck(Scsi_Cmnd *cmd, void (*done)(Scsi_Cmnd *))
 	}
 	local_irq_restore(flags);
 
-	QU_PRINTK("scsi%d: command added to %s of queue\n", H_NO(cmd),
+	dprintk(NDEBUG_QUEUES, "scsi%d: command added to %s of queue\n", H_NO(cmd),
 		  (cmd->cmnd[0] == REQUEST_SENSE) ? "head" : "tail");
 
 	/* If queue_command() is called from an interrupt (real one or bottom
@@ -1054,7 +1044,7 @@ static void NCR5380_main(struct work_struct *work)
 		done = 1;
 
 		if (!hostdata->connected) {
-			MAIN_PRINTK("scsi%d: not connected\n", HOSTNO);
+			dprintk(NDEBUG_MAIN, "scsi%d: not connected\n", HOSTNO);
 			/*
 			 * Search through the issue_queue for a command destined
 			 * for a target that's not busy.
@@ -1107,7 +1097,7 @@ static void NCR5380_main(struct work_struct *work)
 					 * On failure, we must add the command back to the
 					 *   issue queue so we can keep trying.
 					 */
-					MAIN_PRINTK("scsi%d: main(): command for target %d "
+					dprintk(NDEBUG_MAIN, "scsi%d: main(): command for target %d "
 						    "lun %d removed from issue_queue\n",
 						    HOSTNO, tmp->device->id, tmp->device->lun);
 					/*
@@ -1140,7 +1130,7 @@ static void NCR5380_main(struct work_struct *work)
 #endif
 						falcon_dont_release--;
 						local_irq_restore(flags);
-						MAIN_PRINTK("scsi%d: main(): select() failed, "
+						dprintk(NDEBUG_MAIN, "scsi%d: main(): select() failed, "
 							    "returned to issue_queue\n", HOSTNO);
 						if (hostdata->connected)
 							break;
@@ -1155,10 +1145,10 @@ static void NCR5380_main(struct work_struct *work)
 #endif
 		    ) {
 			local_irq_restore(flags);
-			MAIN_PRINTK("scsi%d: main: performing information transfer\n",
+			dprintk(NDEBUG_MAIN, "scsi%d: main: performing information transfer\n",
 				    HOSTNO);
 			NCR5380_information_transfer(instance);
-			MAIN_PRINTK("scsi%d: main: done set false\n", HOSTNO);
+			dprintk(NDEBUG_MAIN, "scsi%d: main: done set false\n", HOSTNO);
 			done = 0;
 		}
 	} while (!done);
@@ -1204,12 +1194,12 @@ static void NCR5380_dma_complete(struct Scsi_Host *instance)
 			    (BASR_PHASE_MATCH|BASR_ACK)) {
 				saved_data = NCR5380_read(INPUT_DATA_REG);
 				overrun = 1;
-				DMA_PRINTK("scsi%d: read overrun handled\n", HOSTNO);
+				dprintk(NDEBUG_DMA, "scsi%d: read overrun handled\n", HOSTNO);
 			}
 		}
 	}
 
-	DMA_PRINTK("scsi%d: real DMA transfer complete, basr 0x%X, sr 0x%X\n",
+	dprintk(NDEBUG_DMA, "scsi%d: real DMA transfer complete, basr 0x%X, sr 0x%X\n",
 		   HOSTNO, NCR5380_read(BUS_AND_STATUS_REG),
 		   NCR5380_read(STATUS_REG));
 
@@ -1229,13 +1219,13 @@ static void NCR5380_dma_complete(struct Scsi_Host *instance)
 		if ((NCR5380_read(STATUS_REG) & PHASE_MASK) == p && (p & SR_IO)) {
 			cnt = toPIO = atari_read_overruns;
 			if (overrun) {
-				DMA_PRINTK("Got an input overrun, using saved byte\n");
+				dprintk(NDEBUG_DMA, "Got an input overrun, using saved byte\n");
 				*(*data)++ = saved_data;
 				(*count)--;
 				cnt--;
 				toPIO--;
 			}
-			DMA_PRINTK("Doing %d-byte PIO to 0x%08lx\n", cnt, (long)*data);
+			dprintk(NDEBUG_DMA, "Doing %d-byte PIO to 0x%08lx\n", cnt, (long)*data);
 			NCR5380_transfer_pio(instance, &p, &cnt, data);
 			*count -= toPIO - cnt;
 		}
@@ -1261,25 +1251,25 @@ static irqreturn_t NCR5380_intr(int irq, void *dev_id)
 	int done = 1, handled = 0;
 	unsigned char basr;
 
-	INT_PRINTK("scsi%d: NCR5380 irq triggered\n", HOSTNO);
+	dprintk(NDEBUG_INTR, "scsi%d: NCR5380 irq triggered\n", HOSTNO);
 
 	/* Look for pending interrupts */
 	basr = NCR5380_read(BUS_AND_STATUS_REG);
-	INT_PRINTK("scsi%d: BASR=%02x\n", HOSTNO, basr);
+	dprintk(NDEBUG_INTR, "scsi%d: BASR=%02x\n", HOSTNO, basr);
 	/* dispatch to appropriate routine if found and done=0 */
 	if (basr & BASR_IRQ) {
-		NCR_PRINT(NDEBUG_INTR);
+		NCR5380_dprint(NDEBUG_INTR, instance);
 		if ((NCR5380_read(STATUS_REG) & (SR_SEL|SR_IO)) == (SR_SEL|SR_IO)) {
 			done = 0;
 			ENABLE_IRQ();
-			INT_PRINTK("scsi%d: SEL interrupt\n", HOSTNO);
+			dprintk(NDEBUG_INTR, "scsi%d: SEL interrupt\n", HOSTNO);
 			NCR5380_reselect(instance);
 			(void)NCR5380_read(RESET_PARITY_INTERRUPT_REG);
 		} else if (basr & BASR_PARITY_ERROR) {
-			INT_PRINTK("scsi%d: PARITY interrupt\n", HOSTNO);
+			dprintk(NDEBUG_INTR, "scsi%d: PARITY interrupt\n", HOSTNO);
 			(void)NCR5380_read(RESET_PARITY_INTERRUPT_REG);
 		} else if ((NCR5380_read(STATUS_REG) & SR_RST) == SR_RST) {
-			INT_PRINTK("scsi%d: RESET interrupt\n", HOSTNO);
+			dprintk(NDEBUG_INTR, "scsi%d: RESET interrupt\n", HOSTNO);
 			(void)NCR5380_read(RESET_PARITY_INTERRUPT_REG);
 		} else {
 			/*
@@ -1298,7 +1288,7 @@ static irqreturn_t NCR5380_intr(int irq, void *dev_id)
 			    ((basr & BASR_END_DMA_TRANSFER) ||
 			     !(basr & BASR_PHASE_MATCH))) {
 
-				INT_PRINTK("scsi%d: PHASE MISM or EOP interrupt\n", HOSTNO);
+				dprintk(NDEBUG_INTR, "scsi%d: PHASE MISM or EOP interrupt\n", HOSTNO);
 				NCR5380_dma_complete( instance );
 				done = 0;
 				ENABLE_IRQ();
@@ -1323,7 +1313,7 @@ static irqreturn_t NCR5380_intr(int irq, void *dev_id)
 	}
 
 	if (!done) {
-		INT_PRINTK("scsi%d: in int routine, calling main\n", HOSTNO);
+		dprintk(NDEBUG_INTR, "scsi%d: in int routine, calling main\n", HOSTNO);
 		/* Put a call to NCR5380_main() on the queue... */
 		queue_main();
 	}
@@ -1396,8 +1386,8 @@ static int NCR5380_select(struct Scsi_Host *instance, Scsi_Cmnd *cmd, int tag)
 	unsigned long flags;
 
 	hostdata->restart_select = 0;
-	NCR_PRINT(NDEBUG_ARBITRATION);
-	ARB_PRINTK("scsi%d: starting arbitration, id = %d\n", HOSTNO,
+	NCR5380_dprint(NDEBUG_ARBITRATION, instance);
+	dprintk(NDEBUG_ARBITRATION, "scsi%d: starting arbitration, id = %d\n", HOSTNO,
 		   instance->this_id);
 
 	/*
@@ -1442,7 +1432,7 @@ static int NCR5380_select(struct Scsi_Host *instance, Scsi_Cmnd *cmd, int tag)
 		;
 #endif
 
-	ARB_PRINTK("scsi%d: arbitration complete\n", HOSTNO);
+	dprintk(NDEBUG_ARBITRATION, "scsi%d: arbitration complete\n", HOSTNO);
 
 	if (hostdata->connected) {
 		NCR5380_write(MODE_REG, MR_BASE);
@@ -1463,7 +1453,7 @@ static int NCR5380_select(struct Scsi_Host *instance, Scsi_Cmnd *cmd, int tag)
 	    (NCR5380_read(INITIATOR_COMMAND_REG) & ICR_ARBITRATION_LOST) ||
 	    hostdata->connected) {
 		NCR5380_write(MODE_REG, MR_BASE);
-		ARB_PRINTK("scsi%d: lost arbitration, deasserting MR_ARBITRATE\n",
+		dprintk(NDEBUG_ARBITRATION, "scsi%d: lost arbitration, deasserting MR_ARBITRATE\n",
 			   HOSTNO);
 		return -1;
 	}
@@ -1478,7 +1468,7 @@ static int NCR5380_select(struct Scsi_Host *instance, Scsi_Cmnd *cmd, int tag)
 	    hostdata->connected) {
 		NCR5380_write(MODE_REG, MR_BASE);
 		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-		ARB_PRINTK("scsi%d: lost arbitration, deasserting ICR_ASSERT_SEL\n",
+		dprintk(NDEBUG_ARBITRATION, "scsi%d: lost arbitration, deasserting ICR_ASSERT_SEL\n",
 			   HOSTNO);
 		return -1;
 	}
@@ -1501,7 +1491,7 @@ static int NCR5380_select(struct Scsi_Host *instance, Scsi_Cmnd *cmd, int tag)
 		return -1;
 	}
 
-	ARB_PRINTK("scsi%d: won arbitration\n", HOSTNO);
+	dprintk(NDEBUG_ARBITRATION, "scsi%d: won arbitration\n", HOSTNO);
 
 	/*
 	 * Now that we have won arbitration, start Selection process, asserting
@@ -1561,7 +1551,7 @@ static int NCR5380_select(struct Scsi_Host *instance, Scsi_Cmnd *cmd, int tag)
 
 	udelay(1);
 
-	SEL_PRINTK("scsi%d: selecting target %d\n", HOSTNO, cmd->device->id);
+	dprintk(NDEBUG_SELECTION, "scsi%d: selecting target %d\n", HOSTNO, cmd->device->id);
 
 	/*
 	 * The SCSI specification calls for a 250 ms timeout for the actual
@@ -1617,7 +1607,7 @@ static int NCR5380_select(struct Scsi_Host *instance, Scsi_Cmnd *cmd, int tag)
 			printk(KERN_ERR "scsi%d: weirdness\n", HOSTNO);
 			if (hostdata->restart_select)
 				printk(KERN_NOTICE "\trestart select\n");
-			NCR_PRINT(NDEBUG_ANY);
+			NCR5380_dprint(NDEBUG_ANY, instance);
 			NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
 			return -1;
 		}
@@ -1630,7 +1620,7 @@ static int NCR5380_select(struct Scsi_Host *instance, Scsi_Cmnd *cmd, int tag)
 #endif
 		cmd->scsi_done(cmd);
 		NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
-		SEL_PRINTK("scsi%d: target did not respond within 250ms\n", HOSTNO);
+		dprintk(NDEBUG_SELECTION, "scsi%d: target did not respond within 250ms\n", HOSTNO);
 		NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
 		return 0;
 	}
@@ -1656,7 +1646,7 @@ static int NCR5380_select(struct Scsi_Host *instance, Scsi_Cmnd *cmd, int tag)
 	while (!(NCR5380_read(STATUS_REG) & SR_REQ))
 		;
 
-	SEL_PRINTK("scsi%d: target %d selected, going into MESSAGE OUT phase.\n",
+	dprintk(NDEBUG_SELECTION, "scsi%d: target %d selected, going into MESSAGE OUT phase.\n",
 		   HOSTNO, cmd->device->id);
 	tmp[0] = IDENTIFY(1, cmd->device->lun);
 
@@ -1676,7 +1666,7 @@ static int NCR5380_select(struct Scsi_Host *instance, Scsi_Cmnd *cmd, int tag)
 	data = tmp;
 	phase = PHASE_MSGOUT;
 	NCR5380_transfer_pio(instance, &phase, &len, &data);
-	SEL_PRINTK("scsi%d: nexus established.\n", HOSTNO);
+	dprintk(NDEBUG_SELECTION, "scsi%d: nexus established.\n", HOSTNO);
 	/* XXX need to handle errors here */
 	hostdata->connected = cmd;
 #ifndef SUPPORT_TAGS
@@ -1737,12 +1727,12 @@ static int NCR5380_transfer_pio(struct Scsi_Host *instance,
 		while (!((tmp = NCR5380_read(STATUS_REG)) & SR_REQ))
 			;
 
-		HSH_PRINTK("scsi%d: REQ detected\n", HOSTNO);
+		dprintk(NDEBUG_HANDSHAKE, "scsi%d: REQ detected\n", HOSTNO);
 
 		/* Check for phase mismatch */
 		if ((tmp & PHASE_MASK) != p) {
-			PIO_PRINTK("scsi%d: phase mismatch\n", HOSTNO);
-			NCR_PRINT_PHASE(NDEBUG_PIO);
+			dprintk(NDEBUG_PIO, "scsi%d: phase mismatch\n", HOSTNO);
+			NCR5380_dprint_phase(NDEBUG_PIO, instance);
 			break;
 		}
 
@@ -1764,25 +1754,25 @@ static int NCR5380_transfer_pio(struct Scsi_Host *instance,
 		if (!(p & SR_IO)) {
 			if (!((p & SR_MSG) && c > 1)) {
 				NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_DATA);
-				NCR_PRINT(NDEBUG_PIO);
+				NCR5380_dprint(NDEBUG_PIO, instance);
 				NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE |
 					      ICR_ASSERT_DATA | ICR_ASSERT_ACK);
 			} else {
 				NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE |
 					      ICR_ASSERT_DATA | ICR_ASSERT_ATN);
-				NCR_PRINT(NDEBUG_PIO);
+				NCR5380_dprint(NDEBUG_PIO, instance);
 				NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE |
 					      ICR_ASSERT_DATA | ICR_ASSERT_ATN | ICR_ASSERT_ACK);
 			}
 		} else {
-			NCR_PRINT(NDEBUG_PIO);
+			NCR5380_dprint(NDEBUG_PIO, instance);
 			NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_ACK);
 		}
 
 		while (NCR5380_read(STATUS_REG) & SR_REQ)
 			;
 
-		HSH_PRINTK("scsi%d: req false, handshake complete\n", HOSTNO);
+		dprintk(NDEBUG_HANDSHAKE, "scsi%d: req false, handshake complete\n", HOSTNO);
 
 		/*
 		 * We have several special cases to consider during REQ/ACK handshaking :
@@ -1803,7 +1793,7 @@ static int NCR5380_transfer_pio(struct Scsi_Host *instance,
 		}
 	} while (--c);
 
-	PIO_PRINTK("scsi%d: residual %d\n", HOSTNO, c);
+	dprintk(NDEBUG_PIO, "scsi%d: residual %d\n", HOSTNO, c);
 
 	*count = c;
 	*data = d;
@@ -1917,7 +1907,7 @@ static int NCR5380_transfer_dma(struct Scsi_Host *instance,
 	if (atari_read_overruns && (p & SR_IO))
 		c -= atari_read_overruns;
 
-	DMA_PRINTK("scsi%d: initializing DMA for %s, %d bytes %s %p\n",
+	dprintk(NDEBUG_DMA, "scsi%d: initializing DMA for %s, %d bytes %s %p\n",
 		   HOSTNO, (p & SR_IO) ? "reading" : "writing",
 		   c, (p & SR_IO) ? "to" : "from", d);
 
@@ -1997,7 +1987,7 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 			phase = (tmp & PHASE_MASK);
 			if (phase != old_phase) {
 				old_phase = phase;
-				NCR_PRINT_PHASE(NDEBUG_INFORMATION);
+				NCR5380_dprint_phase(NDEBUG_INFORMATION, instance);
 			}
 
 			if (sink && (phase != PHASE_MSGOUT)) {
@@ -2039,7 +2029,7 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 					 * they are at contiguous physical addresses.
 					 */
 					merge_contiguous_buffers(cmd);
-					INF_PRINTK("scsi%d: %d bytes and %d buffers left\n",
+					dprintk(NDEBUG_INFORMATION, "scsi%d: %d bytes and %d buffers left\n",
 						   HOSTNO, cmd->SCp.this_residual,
 						   cmd->SCp.buffers_residual);
 				}
@@ -2123,7 +2113,7 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 					/* Accept message by clearing ACK */
 					NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
 
-					LNK_PRINTK("scsi%d: target %d lun %d linked command "
+					dprintk(NDEBUG_LINKED, "scsi%d: target %d lun %d linked command "
 						   "complete.\n", HOSTNO, cmd->device->id, cmd->device->lun);
 
 					/* Enable reselect interrupts */
@@ -2148,7 +2138,7 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 					 * and don't free it! */
 					cmd->next_link->tag = cmd->tag;
 					cmd->result = cmd->SCp.Status | (cmd->SCp.Message << 8);
-					LNK_PRINTK("scsi%d: target %d lun %d linked request "
+					dprintk(NDEBUG_LINKED, "scsi%d: target %d lun %d linked request "
 						   "done, calling scsi_done().\n",
 						   HOSTNO, cmd->device->id, cmd->device->lun);
 #ifdef NCR5380_STATS
@@ -2165,7 +2155,7 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 					/* ++guenther: possible race with Falcon locking */
 					falcon_dont_release++;
 					hostdata->connected = NULL;
-					QU_PRINTK("scsi%d: command for target %d, lun %d "
+					dprintk(NDEBUG_QUEUES, "scsi%d: command for target %d, lun %d "
 						  "completed\n", HOSTNO, cmd->device->id, cmd->device->lun);
 #ifdef SUPPORT_TAGS
 					cmd_free_tag(cmd);
@@ -2179,7 +2169,7 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 						/* ++Andreas: the mid level code knows about
 						   QUEUE_FULL now. */
 						TAG_ALLOC *ta = &TagAlloc[cmd->device->id][cmd->device->lun];
-						TAG_PRINTK("scsi%d: target %d lun %d returned "
+						dprintk(NDEBUG_TAGS, "scsi%d: target %d lun %d returned "
 							   "QUEUE_FULL after %d commands\n",
 							   HOSTNO, cmd->device->id, cmd->device->lun,
 							   ta->nr_allocated);
@@ -2224,14 +2214,14 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 					    (status_byte(cmd->SCp.Status) == CHECK_CONDITION)) {
 						scsi_eh_prep_cmnd(cmd, &hostdata->ses, NULL, 0, ~0);
 
-						ASEN_PRINTK("scsi%d: performing request sense\n", HOSTNO);
+						dprintk(NDEBUG_AUTOSENSE, "scsi%d: performing request sense\n", HOSTNO);
 
 						local_irq_save(flags);
 						LIST(cmd,hostdata->issue_queue);
 						SET_NEXT(cmd, hostdata->issue_queue);
 						hostdata->issue_queue = (Scsi_Cmnd *) cmd;
 						local_irq_restore(flags);
-						QU_PRINTK("scsi%d: REQUEST SENSE added to head of "
+						dprintk(NDEBUG_QUEUES, "scsi%d: REQUEST SENSE added to head of "
 							  "issue queue\n", H_NO(cmd));
 					} else
 #endif /* def AUTOSENSE */
@@ -2277,7 +2267,7 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 						cmd->device->tagged_supported = 0;
 						hostdata->busy[cmd->device->id] |= (1 << cmd->device->lun);
 						cmd->tag = TAG_NONE;
-						TAG_PRINTK("scsi%d: target %d lun %d rejected "
+						dprintk(NDEBUG_TAGS, "scsi%d: target %d lun %d rejected "
 							   "QUEUE_TAG message; tagged queuing "
 							   "disabled\n",
 							   HOSTNO, cmd->device->id, cmd->device->lun);
@@ -2294,7 +2284,7 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 					hostdata->connected = NULL;
 					hostdata->disconnected_queue = cmd;
 					local_irq_restore(flags);
-					QU_PRINTK("scsi%d: command for target %d lun %d was "
+					dprintk(NDEBUG_QUEUES, "scsi%d: command for target %d lun %d was "
 						  "moved from connected to the "
 						  "disconnected_queue\n", HOSTNO,
 						  cmd->device->id, cmd->device->lun);
@@ -2344,13 +2334,13 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 					/* Accept first byte by clearing ACK */
 					NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
 
-					EXT_PRINTK("scsi%d: receiving extended message\n", HOSTNO);
+					dprintk(NDEBUG_EXTENDED, "scsi%d: receiving extended message\n", HOSTNO);
 
 					len = 2;
 					data = extended_msg + 1;
 					phase = PHASE_MSGIN;
 					NCR5380_transfer_pio(instance, &phase, &len, &data);
-					EXT_PRINTK("scsi%d: length=%d, code=0x%02x\n", HOSTNO,
+					dprintk(NDEBUG_EXTENDED, "scsi%d: length=%d, code=0x%02x\n", HOSTNO,
 						   (int)extended_msg[1], (int)extended_msg[2]);
 
 					if (!len && extended_msg[1] <=
@@ -2362,7 +2352,7 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 						phase = PHASE_MSGIN;
 
 						NCR5380_transfer_pio(instance, &phase, &len, &data);
-						EXT_PRINTK("scsi%d: message received, residual %d\n",
+						dprintk(NDEBUG_EXTENDED, "scsi%d: message received, residual %d\n",
 							   HOSTNO, len);
 
 						switch (extended_msg[2]) {
@@ -2451,7 +2441,7 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 				break;
 			default:
 				printk("scsi%d: unknown phase\n", HOSTNO);
-				NCR_PRINT(NDEBUG_ANY);
+				NCR5380_dprint(NDEBUG_ANY, instance);
 			} /* switch(phase) */
 		} /* if (tmp * SR_REQ) */
 	} /* while (1) */
@@ -2493,7 +2483,7 @@ static void NCR5380_reselect(struct Scsi_Host *instance)
 
 	target_mask = NCR5380_read(CURRENT_SCSI_DATA_REG) & ~(hostdata->id_mask);
 
-	RSL_PRINTK("scsi%d: reselect\n", HOSTNO);
+	dprintk(NDEBUG_RESELECTION, "scsi%d: reselect\n", HOSTNO);
 
 	/*
 	 * At this point, we have detected that our SCSI ID is on the bus,
@@ -2544,7 +2534,7 @@ static void NCR5380_reselect(struct Scsi_Host *instance)
 		if (!NCR5380_transfer_pio(instance, &phase, &len, &data) &&
 		    msg[1] == SIMPLE_QUEUE_TAG)
 			tag = msg[2];
-		TAG_PRINTK("scsi%d: target mask %02x, lun %d sent tag %d at "
+		dprintk(NDEBUG_TAGS, "scsi%d: target mask %02x, lun %d sent tag %d at "
 			   "reselection\n", HOSTNO, target_mask, lun, tag);
 	}
 #endif
@@ -2598,7 +2588,7 @@ static void NCR5380_reselect(struct Scsi_Host *instance)
 	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
 
 	hostdata->connected = tmp;
-	RSL_PRINTK("scsi%d: nexus established, target = %d, lun = %d, tag = %d\n",
+	dprintk(NDEBUG_RESELECTION, "scsi%d: nexus established, target = %d, lun = %d, tag = %d\n",
 		   HOSTNO, tmp->device->id, tmp->device->lun, tmp->tag);
 	falcon_dont_release--;
 }
@@ -2640,7 +2630,7 @@ int NCR5380_abort(Scsi_Cmnd *cmd)
 		printk(KERN_ERR "scsi%d: !!BINGO!! Falcon has no lock in NCR5380_abort\n",
 		       HOSTNO);
 
-	ABRT_PRINTK("scsi%d: abort called basr 0x%02x, sr 0x%02x\n", HOSTNO,
+	dprintk(NDEBUG_ABORT, "scsi%d: abort called basr 0x%02x, sr 0x%02x\n", HOSTNO,
 		    NCR5380_read(BUS_AND_STATUS_REG),
 		    NCR5380_read(STATUS_REG));
 
@@ -2653,7 +2643,7 @@ int NCR5380_abort(Scsi_Cmnd *cmd)
 
 	if (hostdata->connected == cmd) {
 
-		ABRT_PRINTK("scsi%d: aborting connected command\n", HOSTNO);
+		dprintk(NDEBUG_ABORT, "scsi%d: aborting connected command\n", HOSTNO);
 		/*
 		 * We should perform BSY checking, and make sure we haven't slipped
 		 * into BUS FREE.
@@ -2683,11 +2673,11 @@ int NCR5380_abort(Scsi_Cmnd *cmd)
 			local_irq_restore(flags);
 			cmd->scsi_done(cmd);
 			falcon_release_lock_if_possible(hostdata);
-			return SCSI_ABORT_SUCCESS;
+			return SUCCESS;
 		} else {
 /*			local_irq_restore(flags); */
 			printk("scsi%d: abort of connected command failed!\n", HOSTNO);
-			return SCSI_ABORT_ERROR;
+			return FAILED;
 		}
 	}
 #endif
@@ -2705,13 +2695,13 @@ int NCR5380_abort(Scsi_Cmnd *cmd)
 			SET_NEXT(tmp, NULL);
 			tmp->result = DID_ABORT << 16;
 			local_irq_restore(flags);
-			ABRT_PRINTK("scsi%d: abort removed command from issue queue.\n",
+			dprintk(NDEBUG_ABORT, "scsi%d: abort removed command from issue queue.\n",
 				    HOSTNO);
 			/* Tagged queuing note: no tag to free here, hasn't been assigned
 			 * yet... */
 			tmp->scsi_done(tmp);
 			falcon_release_lock_if_possible(hostdata);
-			return SCSI_ABORT_SUCCESS;
+			return SUCCESS;
 		}
 	}
 
@@ -2728,8 +2718,8 @@ int NCR5380_abort(Scsi_Cmnd *cmd)
 
 	if (hostdata->connected) {
 		local_irq_restore(flags);
-		ABRT_PRINTK("scsi%d: abort failed, command connected.\n", HOSTNO);
-		return SCSI_ABORT_SNOOZE;
+		dprintk(NDEBUG_ABORT, "scsi%d: abort failed, command connected.\n", HOSTNO);
+		return FAILED;
 	}
 
 	/*
@@ -2761,12 +2751,12 @@ int NCR5380_abort(Scsi_Cmnd *cmd)
 	     tmp = NEXT(tmp)) {
 		if (cmd == tmp) {
 			local_irq_restore(flags);
-			ABRT_PRINTK("scsi%d: aborting disconnected command.\n", HOSTNO);
+			dprintk(NDEBUG_ABORT, "scsi%d: aborting disconnected command.\n", HOSTNO);
 
 			if (NCR5380_select(instance, cmd, (int)cmd->tag))
-				return SCSI_ABORT_BUSY;
+				return FAILED;
 
-			ABRT_PRINTK("scsi%d: nexus reestablished.\n", HOSTNO);
+			dprintk(NDEBUG_ABORT, "scsi%d: nexus reestablished.\n", HOSTNO);
 
 			do_abort(instance);
 
@@ -2791,7 +2781,7 @@ int NCR5380_abort(Scsi_Cmnd *cmd)
 					local_irq_restore(flags);
 					tmp->scsi_done(tmp);
 					falcon_release_lock_if_possible(hostdata);
-					return SCSI_ABORT_SUCCESS;
+					return SUCCESS;
 				}
 			}
 		}
@@ -2816,7 +2806,7 @@ int NCR5380_abort(Scsi_Cmnd *cmd)
 	 */
 	falcon_release_lock_if_possible(hostdata);
 
-	return SCSI_ABORT_NOT_RUNNING;
+	return FAILED;
 }
 
 
@@ -2825,7 +2815,7 @@ int NCR5380_abort(Scsi_Cmnd *cmd)
  *
  * Purpose : reset the SCSI bus.
  *
- * Returns : SCSI_RESET_WAKEUP
+ * Returns : SUCCESS or FAILURE
  *
  */
 
@@ -2834,7 +2824,7 @@ static int NCR5380_bus_reset(Scsi_Cmnd *cmd)
 	SETUP_HOSTDATA(cmd->device->host);
 	int i;
 	unsigned long flags;
-#if 1
+#if defined(RESET_RUN_DONE)
 	Scsi_Cmnd *connected, *disconnected_queue;
 #endif
 
@@ -2859,7 +2849,14 @@ static int NCR5380_bus_reset(Scsi_Cmnd *cmd)
 	 * through anymore ... */
 	(void)NCR5380_read(RESET_PARITY_INTERRUPT_REG);
 
-#if 1	/* XXX Should now be done by midlevel code, but it's broken XXX */
+	/* MSch 20140115 - looking at the generic NCR5380 driver, all of this
+	 * should go.
+	 * Catch-22: if we don't clear all queues, the SCSI driver lock will
+	 * not be reset by atari_scsi_reset()!
+	 */
+
+#if defined(RESET_RUN_DONE)
+	/* XXX Should now be done by midlevel code, but it's broken XXX */
 	/* XXX see below                                            XXX */
 
 	/* MSch: old-style reset: actually abort all command processing here */
@@ -2890,7 +2887,7 @@ static int NCR5380_bus_reset(Scsi_Cmnd *cmd)
 	 */
 
 	if ((cmd = connected)) {
-		ABRT_PRINTK("scsi%d: reset aborted a connected command\n", H_NO(cmd));
+		dprintk(NDEBUG_ABORT, "scsi%d: reset aborted a connected command\n", H_NO(cmd));
 		cmd->result = (cmd->result & 0xffff) | (DID_RESET << 16);
 		cmd->scsi_done(cmd);
 	}
@@ -2902,7 +2899,7 @@ static int NCR5380_bus_reset(Scsi_Cmnd *cmd)
 		cmd->scsi_done(cmd);
 	}
 	if (i > 0)
-		ABRT_PRINTK("scsi: reset aborted %d disconnected command(s)\n", i);
+		dprintk(NDEBUG_ABORT, "scsi: reset aborted %d disconnected command(s)\n", i);
 
 	/* The Falcon lock should be released after a reset...
 	 */
@@ -2915,7 +2912,7 @@ static int NCR5380_bus_reset(Scsi_Cmnd *cmd)
 	 * the midlevel code that the reset was SUCCESSFUL, and there is no
 	 * need to 'wake up' the commands by a request_sense
 	 */
-	return SCSI_RESET_SUCCESS | SCSI_RESET_BUS_RESET;
+	return SUCCESS;
 #else /* 1 */
 
 	/* MSch: new-style reset handling: let the mid-level do what it can */
@@ -2942,11 +2939,11 @@ static int NCR5380_bus_reset(Scsi_Cmnd *cmd)
 	 */
 
 	if (hostdata->issue_queue)
-		ABRT_PRINTK("scsi%d: reset aborted issued command(s)\n", H_NO(cmd));
+		dprintk(NDEBUG_ABORT, "scsi%d: reset aborted issued command(s)\n", H_NO(cmd));
 	if (hostdata->connected)
-		ABRT_PRINTK("scsi%d: reset aborted a connected command\n", H_NO(cmd));
+		dprintk(NDEBUG_ABORT, "scsi%d: reset aborted a connected command\n", H_NO(cmd));
 	if (hostdata->disconnected_queue)
-		ABRT_PRINTK("scsi%d: reset aborted disconnected command(s)\n", H_NO(cmd));
+		dprintk(NDEBUG_ABORT, "scsi%d: reset aborted disconnected command(s)\n", H_NO(cmd));
 
 	local_irq_save(flags);
 	hostdata->issue_queue = NULL;
@@ -2963,6 +2960,6 @@ static int NCR5380_bus_reset(Scsi_Cmnd *cmd)
 	local_irq_restore(flags);
 
 	/* we did no complete reset of all commands, so a wakeup is required */
-	return SCSI_RESET_WAKEUP | SCSI_RESET_BUS_RESET;
+	return SUCCESS;
 #endif /* 1 */
 }
diff --git a/drivers/scsi/atari_scsi.c b/drivers/scsi/atari_scsi.c
index 296c936cc03..b522134528d 100644
--- a/drivers/scsi/atari_scsi.c
+++ b/drivers/scsi/atari_scsi.c
@@ -67,12 +67,6 @@
 
 #include <linux/module.h>
 
-#define NDEBUG (0)
-
-#define NDEBUG_ABORT		0x00100000
-#define NDEBUG_TAGS		0x00200000
-#define NDEBUG_MERGING		0x00400000
-
 #define AUTOSENSE
 /* For the Atari version, use only polled IO or REAL_DMA */
 #define	REAL_DMA
@@ -314,7 +308,7 @@ static irqreturn_t scsi_tt_intr(int irq, void *dummy)
 
 	dma_stat = tt_scsi_dma.dma_ctrl;
 
-	INT_PRINTK("scsi%d: NCR5380 interrupt, DMA status = %02x\n",
+	dprintk(NDEBUG_INTR, "scsi%d: NCR5380 interrupt, DMA status = %02x\n",
 		   atari_scsi_host->host_no, dma_stat & 0xff);
 
 	/* Look if it was the DMA that has interrupted: First possibility
@@ -340,7 +334,7 @@ static irqreturn_t scsi_tt_intr(int irq, void *dummy)
 	if ((dma_stat & 0x02) && !(dma_stat & 0x40)) {
 		atari_dma_residual = HOSTDATA_DMALEN - (SCSI_DMA_READ_P(dma_addr) - atari_dma_startaddr);
 
-		DMA_PRINTK("SCSI DMA: There are %ld residual bytes.\n",
+		dprintk(NDEBUG_DMA, "SCSI DMA: There are %ld residual bytes.\n",
 			   atari_dma_residual);
 
 		if ((signed int)atari_dma_residual < 0)
@@ -371,7 +365,7 @@ static irqreturn_t scsi_tt_intr(int irq, void *dummy)
 			 * other command.  These shouldn't disconnect anyway.
 			 */
 			if (atari_dma_residual & 0x1ff) {
-				DMA_PRINTK("SCSI DMA: DMA bug corrected, "
+				dprintk(NDEBUG_DMA, "SCSI DMA: DMA bug corrected, "
 					   "difference %ld bytes\n",
 					   512 - (atari_dma_residual & 0x1ff));
 				atari_dma_residual = (atari_dma_residual + 511) & ~0x1ff;
@@ -438,7 +432,7 @@ static irqreturn_t scsi_falcon_intr(int irq, void *dummy)
 			       "ST-DMA fifo\n", transferred & 15);
 
 		atari_dma_residual = HOSTDATA_DMALEN - transferred;
-		DMA_PRINTK("SCSI DMA: There are %ld residual bytes.\n",
+		dprintk(NDEBUG_DMA, "SCSI DMA: There are %ld residual bytes.\n",
 			   atari_dma_residual);
 	} else
 		atari_dma_residual = 0;
@@ -474,11 +468,11 @@ static void atari_scsi_fetch_restbytes(void)
 		/* there are 'nr' bytes left for the last long address
 		   before the DMA pointer */
 		phys_dst ^= nr;
-		DMA_PRINTK("SCSI DMA: there are %d rest bytes for phys addr 0x%08lx",
+		dprintk(NDEBUG_DMA, "SCSI DMA: there are %d rest bytes for phys addr 0x%08lx",
 			   nr, phys_dst);
 		/* The content of the DMA pointer is a physical address!  */
 		dst = phys_to_virt(phys_dst);
-		DMA_PRINTK(" = virt addr %p\n", dst);
+		dprintk(NDEBUG_DMA, " = virt addr %p\n", dst);
 		for (src = (char *)&tt_scsi_dma.dma_restdata; nr != 0; --nr)
 			*dst++ = *src++;
 	}
@@ -639,7 +633,7 @@ static int __init atari_scsi_detect(struct scsi_host_template *host)
 					"double buffer\n");
 			return 0;
 		}
-		atari_dma_phys_buffer = virt_to_phys(atari_dma_buffer);
+		atari_dma_phys_buffer = atari_stram_to_phys(atari_dma_buffer);
 		atari_dma_orig_addr = 0;
 	}
 #endif
@@ -827,7 +821,7 @@ static int atari_scsi_bus_reset(Scsi_Cmnd *cmd)
 	} else {
 		atari_turnon_irq(IRQ_MFP_FSCSI);
 	}
-	if ((rv & SCSI_RESET_ACTION) == SCSI_RESET_SUCCESS)
+	if (rv == SUCCESS)
 		falcon_release_lock_if_possible(hostdata);
 
 	return rv;
@@ -883,7 +877,7 @@ static unsigned long atari_scsi_dma_setup(struct Scsi_Host *instance,
 {
 	unsigned long addr = virt_to_phys(data);
 
-	DMA_PRINTK("scsi%d: setting up dma, data = %p, phys = %lx, count = %ld, "
+	dprintk(NDEBUG_DMA, "scsi%d: setting up dma, data = %p, phys = %lx, count = %ld, "
 		   "dir = %d\n", instance->host_no, data, addr, count, dir);
 
 	if (!IS_A_TT() && !STRAM_ADDR(addr)) {
@@ -1063,7 +1057,7 @@ static unsigned long atari_dma_xfer_len(unsigned long wanted_len,
 		possible_len = limit;
 
 	if (possible_len != wanted_len)
-		DMA_PRINTK("Sorry, must cut DMA transfer size to %ld bytes "
+		dprintk(NDEBUG_DMA, "Sorry, must cut DMA transfer size to %ld bytes "
 			   "instead of %ld\n", possible_len, wanted_len);
 
 	return possible_len;
diff --git a/drivers/scsi/atari_scsi.h b/drivers/scsi/atari_scsi.h
index 11c624bb122..3299d91d733 100644
--- a/drivers/scsi/atari_scsi.h
+++ b/drivers/scsi/atari_scsi.h
@@ -54,125 +54,6 @@
 #define	NCR5380_dma_xfer_len(i,cmd,phase) \
 	atari_dma_xfer_len(cmd->SCp.this_residual,cmd,((phase) & SR_IO) ? 0 : 1)
 
-/* former generic SCSI error handling stuff */
-
-#define SCSI_ABORT_SNOOZE 0
-#define SCSI_ABORT_SUCCESS 1
-#define SCSI_ABORT_PENDING 2
-#define SCSI_ABORT_BUSY 3
-#define SCSI_ABORT_NOT_RUNNING 4
-#define SCSI_ABORT_ERROR 5
-
-#define SCSI_RESET_SNOOZE 0
-#define SCSI_RESET_PUNT 1
-#define SCSI_RESET_SUCCESS 2
-#define SCSI_RESET_PENDING 3
-#define SCSI_RESET_WAKEUP 4
-#define SCSI_RESET_NOT_RUNNING 5
-#define SCSI_RESET_ERROR 6
-
-#define SCSI_RESET_SYNCHRONOUS		0x01
-#define SCSI_RESET_ASYNCHRONOUS		0x02
-#define SCSI_RESET_SUGGEST_BUS_RESET	0x04
-#define SCSI_RESET_SUGGEST_HOST_RESET	0x08
-
-#define SCSI_RESET_BUS_RESET 0x100
-#define SCSI_RESET_HOST_RESET 0x200
-#define SCSI_RESET_ACTION   0xff
-
-/* Debugging printk definitions:
- *
- *  ARB  -> arbitration
- *  ASEN -> auto-sense
- *  DMA  -> DMA
- *  HSH  -> PIO handshake
- *  INF  -> information transfer
- *  INI  -> initialization
- *  INT  -> interrupt
- *  LNK  -> linked commands
- *  MAIN -> NCR5380_main() control flow
- *  NDAT -> no data-out phase
- *  NWR  -> no write commands
- *  PIO  -> PIO transfers
- *  PDMA -> pseudo DMA (unused on Atari)
- *  QU   -> queues
- *  RSL  -> reselections
- *  SEL  -> selections
- *  USL  -> usleep cpde (unused on Atari)
- *  LBS  -> last byte sent (unused on Atari)
- *  RSS  -> restarting of selections
- *  EXT  -> extended messages
- *  ABRT -> aborting and resetting
- *  TAG  -> queue tag handling
- *  MER  -> merging of consec. buffers
- *
- */
-
-#define dprint(flg, format...)			\
-({						\
-	if (NDEBUG & (flg))			\
-		printk(KERN_DEBUG format);	\
-})
-
-#define ARB_PRINTK(format, args...) \
-	dprint(NDEBUG_ARBITRATION, format , ## args)
-#define ASEN_PRINTK(format, args...) \
-	dprint(NDEBUG_AUTOSENSE, format , ## args)
-#define DMA_PRINTK(format, args...) \
-	dprint(NDEBUG_DMA, format , ## args)
-#define HSH_PRINTK(format, args...) \
-	dprint(NDEBUG_HANDSHAKE, format , ## args)
-#define INF_PRINTK(format, args...) \
-	dprint(NDEBUG_INFORMATION, format , ## args)
-#define INI_PRINTK(format, args...) \
-	dprint(NDEBUG_INIT, format , ## args)
-#define INT_PRINTK(format, args...) \
-	dprint(NDEBUG_INTR, format , ## args)
-#define LNK_PRINTK(format, args...) \
-	dprint(NDEBUG_LINKED, format , ## args)
-#define MAIN_PRINTK(format, args...) \
-	dprint(NDEBUG_MAIN, format , ## args)
-#define NDAT_PRINTK(format, args...) \
-	dprint(NDEBUG_NO_DATAOUT, format , ## args)
-#define NWR_PRINTK(format, args...) \
-	dprint(NDEBUG_NO_WRITE, format , ## args)
-#define PIO_PRINTK(format, args...) \
-	dprint(NDEBUG_PIO, format , ## args)
-#define PDMA_PRINTK(format, args...) \
-	dprint(NDEBUG_PSEUDO_DMA, format , ## args)
-#define QU_PRINTK(format, args...) \
-	dprint(NDEBUG_QUEUES, format , ## args)
-#define RSL_PRINTK(format, args...) \
-	dprint(NDEBUG_RESELECTION, format , ## args)
-#define SEL_PRINTK(format, args...) \
-	dprint(NDEBUG_SELECTION, format , ## args)
-#define USL_PRINTK(format, args...) \
-	dprint(NDEBUG_USLEEP, format , ## args)
-#define LBS_PRINTK(format, args...) \
-	dprint(NDEBUG_LAST_BYTE_SENT, format , ## args)
-#define RSS_PRINTK(format, args...) \
-	dprint(NDEBUG_RESTART_SELECT, format , ## args)
-#define EXT_PRINTK(format, args...) \
-	dprint(NDEBUG_EXTENDED, format , ## args)
-#define ABRT_PRINTK(format, args...) \
-	dprint(NDEBUG_ABORT, format , ## args)
-#define TAG_PRINTK(format, args...) \
-	dprint(NDEBUG_TAGS, format , ## args)
-#define MER_PRINTK(format, args...) \
-	dprint(NDEBUG_MERGING, format , ## args)
-
-/* conditional macros for NCR5380_print_{,phase,status} */
-
-#define NCR_PRINT(mask)	\
-	((NDEBUG & (mask)) ? NCR5380_print(instance) : (void)0)
-
-#define NCR_PRINT_PHASE(mask) \
-	((NDEBUG & (mask)) ? NCR5380_print_phase(instance) : (void)0)
-
-#define NCR_PRINT_STATUS(mask) \
-	((NDEBUG & (mask)) ? NCR5380_print_status(instance) : (void)0)
-
-
 #endif /* ndef ASM */
 #endif /* ATARI_SCSI_H */
 
diff --git a/drivers/scsi/be2iscsi/be.h b/drivers/scsi/be2iscsi/be.h
index 1bfb0bd0119..860f527d8f2 100644
--- a/drivers/scsi/be2iscsi/be.h
+++ b/drivers/scsi/be2iscsi/be.h
@@ -83,9 +83,20 @@ static inline void queue_tail_inc(struct be_queue_info *q)
 
 /*ISCSI */
 
+struct be_aic_obj {		/* Adaptive interrupt coalescing (AIC) info */
+	bool enable;
+	u32 min_eqd;		/* in usecs */
+	u32 max_eqd;		/* in usecs */
+	u32 prev_eqd;		/* in usecs */
+	u32 et_eqd;		/* configured val when aic is off */
+	ulong jiffs;
+	u64 eq_prev;		/* Used to calculate eqe */
+};
+
 struct be_eq_obj {
 	bool todo_mcc_cq;
 	bool todo_cq;
+	u32 cq_count;
 	struct be_queue_info q;
 	struct beiscsi_hba *phba;
 	struct be_queue_info *cq;
diff --git a/drivers/scsi/be2iscsi/be_cmds.h b/drivers/scsi/be2iscsi/be_cmds.h
index 7cf7f99ee44..cc7405c0eca 100644
--- a/drivers/scsi/be2iscsi/be_cmds.h
+++ b/drivers/scsi/be2iscsi/be_cmds.h
@@ -71,6 +71,7 @@ struct be_mcc_wrb {
 #define BEISCSI_FW_MBX_TIMEOUT	100
 
 /* MBOX Command VER */
+#define MBX_CMD_VER1	0x01
 #define MBX_CMD_VER2	0x02
 
 struct be_mcc_compl {
@@ -271,6 +272,12 @@ struct be_cmd_resp_eq_create {
 	u16 rsvd0;		/* sword */
 } __packed;
 
+struct be_set_eqd {
+	u32 eq_id;
+	u32 phase;
+	u32 delay_multiplier;
+} __packed;
+
 struct mgmt_chap_format {
 	u32 flags;
 	u8  intr_chap_name[256];
@@ -622,7 +629,7 @@ struct be_cmd_req_modify_eq_delay {
 		u32 eq_id;
 		u32 phase;
 		u32 delay_multiplier;
-	} delay[8];
+	} delay[MAX_CPUS];
 } __packed;
 
 /******************** Get MAC ADDR *******************/
@@ -708,6 +715,8 @@ unsigned int be_cmd_get_port_speed(struct beiscsi_hba *phba);
 
 void free_mcc_tag(struct be_ctrl_info *ctrl, unsigned int tag);
 
+int be_cmd_modify_eq_delay(struct beiscsi_hba *phba, struct be_set_eqd *,
+			    int num);
 int beiscsi_mccq_compl(struct beiscsi_hba *phba,
 			uint32_t tag, struct be_mcc_wrb **wrb,
 			struct be_dma_mem *mbx_cmd_mem);
@@ -1005,6 +1014,26 @@ struct tcp_connect_and_offload_in {
 	u8 rsvd0[3];
 } __packed;
 
+struct tcp_connect_and_offload_in_v1 {
+	struct be_cmd_req_hdr hdr;
+	struct ip_addr_format ip_address;
+	u16 tcp_port;
+	u16 cid;
+	u16 cq_id;
+	u16 defq_id;
+	struct phys_addr dataout_template_pa;
+	u16 hdr_ring_id;
+	u16 data_ring_id;
+	u8 do_offload;
+	u8 ifd_state;
+	u8 rsvd0[2];
+	u16 tcp_window_size;
+	u8 tcp_window_scale_count;
+	u8 rsvd1;
+	u32 tcp_mss:24;
+	u8 rsvd2;
+} __packed;
+
 struct tcp_connect_and_offload_out {
 	struct be_cmd_resp_hdr hdr;
 	u32 connection_handle;
diff --git a/drivers/scsi/be2iscsi/be_iscsi.c b/drivers/scsi/be2iscsi/be_iscsi.c
index a3df43324c9..fd284ff36ec 100644
--- a/drivers/scsi/be2iscsi/be_iscsi.c
+++ b/drivers/scsi/be2iscsi/be_iscsi.c
@@ -1106,7 +1106,7 @@ static int beiscsi_open_conn(struct iscsi_endpoint *ep,
 	struct beiscsi_hba *phba = beiscsi_ep->phba;
 	struct tcp_connect_and_offload_out *ptcpcnct_out;
 	struct be_dma_mem nonemb_cmd;
-	unsigned int tag;
+	unsigned int tag, req_memsize;
 	int ret = -ENOMEM;
 
 	beiscsi_log(phba, KERN_INFO, BEISCSI_LOG_CONFIG,
@@ -1127,8 +1127,14 @@ static int beiscsi_open_conn(struct iscsi_endpoint *ep,
 		       (beiscsi_ep->ep_cid)] = ep;
 
 	beiscsi_ep->cid_vld = 0;
+
+	if (is_chip_be2_be3r(phba))
+		req_memsize = sizeof(struct tcp_connect_and_offload_in);
+	else
+		req_memsize = sizeof(struct tcp_connect_and_offload_in_v1);
+
 	nonemb_cmd.va = pci_alloc_consistent(phba->ctrl.pdev,
-				sizeof(struct tcp_connect_and_offload_in),
+				req_memsize,
 				&nonemb_cmd.dma);
 	if (nonemb_cmd.va == NULL) {
 
@@ -1139,7 +1145,7 @@ static int beiscsi_open_conn(struct iscsi_endpoint *ep,
 		beiscsi_free_ep(beiscsi_ep);
 		return -ENOMEM;
 	}
-	nonemb_cmd.size = sizeof(struct tcp_connect_and_offload_in);
+	nonemb_cmd.size = req_memsize;
 	memset(nonemb_cmd.va, 0, nonemb_cmd.size);
 	tag = mgmt_open_connection(phba, dst_addr, beiscsi_ep, &nonemb_cmd);
 	if (tag <= 0) {
diff --git a/drivers/scsi/be2iscsi/be_main.c b/drivers/scsi/be2iscsi/be_main.c
index 0d822297aa8..55434902962 100644
--- a/drivers/scsi/be2iscsi/be_main.c
+++ b/drivers/scsi/be2iscsi/be_main.c
@@ -599,15 +599,7 @@ static struct beiscsi_hba *beiscsi_hba_alloc(struct pci_dev *pcidev)
 	pci_set_drvdata(pcidev, phba);
 	phba->interface_handle = 0xFFFFFFFF;
 
-	if (iscsi_host_add(shost, &phba->pcidev->dev))
-		goto free_devices;
-
 	return phba;
-
-free_devices:
-	pci_dev_put(phba->pcidev);
-	iscsi_host_free(phba->shost);
-	return NULL;
 }
 
 static void beiscsi_unmap_pci_function(struct beiscsi_hba *phba)
@@ -2279,6 +2271,7 @@ static int be_iopoll(struct blk_iopoll *iop, int budget)
 
 	pbe_eq = container_of(iop, struct be_eq_obj, iopoll);
 	ret = beiscsi_process_cq(pbe_eq);
+	pbe_eq->cq_count += ret;
 	if (ret < budget) {
 		phba = pbe_eq->phba;
 		blk_iopoll_complete(iop);
@@ -3692,7 +3685,7 @@ static void hwi_cleanup(struct beiscsi_hba *phba)
 	struct hwi_controller *phwi_ctrlr;
 	struct hwi_context_memory *phwi_context;
 	struct hwi_async_pdu_context *pasync_ctx;
-	int i, eq_num, ulp_num;
+	int i, eq_for_mcc, ulp_num;
 
 	phwi_ctrlr = phba->phwi_ctrlr;
 	phwi_context = phwi_ctrlr->phwi_ctxt;
@@ -3729,16 +3722,17 @@ static void hwi_cleanup(struct beiscsi_hba *phba)
 		if (q->created)
 			beiscsi_cmd_q_destroy(ctrl, q, QTYPE_CQ);
 	}
+
+	be_mcc_queues_destroy(phba);
 	if (phba->msix_enabled)
-		eq_num = 1;
+		eq_for_mcc = 1;
 	else
-		eq_num = 0;
-	for (i = 0; i < (phba->num_cpus + eq_num); i++) {
+		eq_for_mcc = 0;
+	for (i = 0; i < (phba->num_cpus + eq_for_mcc); i++) {
 		q = &phwi_context->be_eq[i].q;
 		if (q->created)
 			beiscsi_cmd_q_destroy(ctrl, q, QTYPE_EQ);
 	}
-	be_mcc_queues_destroy(phba);
 	be_cmd_fw_uninit(ctrl);
 }
 
@@ -3833,9 +3827,9 @@ static int hwi_init_port(struct beiscsi_hba *phba)
 
 	phwi_ctrlr = phba->phwi_ctrlr;
 	phwi_context = phwi_ctrlr->phwi_ctxt;
-	phwi_context->max_eqd = 0;
+	phwi_context->max_eqd = 128;
 	phwi_context->min_eqd = 0;
-	phwi_context->cur_eqd = 64;
+	phwi_context->cur_eqd = 0;
 	be_cmd_fw_initialize(&phba->ctrl);
 
 	status = beiscsi_create_eqs(phba, phwi_context);
@@ -5290,6 +5284,57 @@ static void beiscsi_msix_enable(struct beiscsi_hba *phba)
 	return;
 }
 
+static void be_eqd_update(struct beiscsi_hba *phba)
+{
+	struct be_set_eqd set_eqd[MAX_CPUS];
+	struct be_aic_obj *aic;
+	struct be_eq_obj *pbe_eq;
+	struct hwi_controller *phwi_ctrlr;
+	struct hwi_context_memory *phwi_context;
+	int eqd, i, num = 0;
+	ulong now;
+	u32 pps, delta;
+	unsigned int tag;
+
+	phwi_ctrlr = phba->phwi_ctrlr;
+	phwi_context = phwi_ctrlr->phwi_ctxt;
+
+	for (i = 0; i <= phba->num_cpus; i++) {
+		aic = &phba->aic_obj[i];
+		pbe_eq = &phwi_context->be_eq[i];
+		now = jiffies;
+		if (!aic->jiffs || time_before(now, aic->jiffs) ||
+		    pbe_eq->cq_count < aic->eq_prev) {
+			aic->jiffs = now;
+			aic->eq_prev = pbe_eq->cq_count;
+			continue;
+		}
+		delta = jiffies_to_msecs(now - aic->jiffs);
+		pps = (((u32)(pbe_eq->cq_count - aic->eq_prev) * 1000) / delta);
+		eqd = (pps / 1500) << 2;
+
+		if (eqd < 8)
+			eqd = 0;
+		eqd = min_t(u32, eqd, phwi_context->max_eqd);
+		eqd = max_t(u32, eqd, phwi_context->min_eqd);
+
+		aic->jiffs = now;
+		aic->eq_prev = pbe_eq->cq_count;
+
+		if (eqd != aic->prev_eqd) {
+			set_eqd[num].delay_multiplier = (eqd * 65)/100;
+			set_eqd[num].eq_id = pbe_eq->q.id;
+			aic->prev_eqd = eqd;
+			num++;
+		}
+	}
+	if (num) {
+		tag = be_cmd_modify_eq_delay(phba, set_eqd, num);
+		if (tag)
+			beiscsi_mccq_compl(phba, tag, NULL, NULL);
+	}
+}
+
 /*
  * beiscsi_hw_health_check()- Check adapter health
  * @work: work item to check HW health
@@ -5303,6 +5348,8 @@ beiscsi_hw_health_check(struct work_struct *work)
 		container_of(work, struct beiscsi_hba,
 			     beiscsi_hw_check_task.work);
 
+	be_eqd_update(phba);
+
 	beiscsi_ue_detect(phba);
 
 	schedule_delayed_work(&phba->beiscsi_hw_check_task,
@@ -5579,7 +5626,7 @@ static int beiscsi_dev_probe(struct pci_dev *pcidev,
 		phba->ctrl.mcc_numtag[i + 1] = 0;
 		phba->ctrl.mcc_tag_available++;
 		memset(&phba->ctrl.ptag_state[i].tag_mem_state, 0,
-		       sizeof(struct beiscsi_mcc_tag_state));
+		       sizeof(struct be_dma_mem));
 	}
 
 	phba->ctrl.mcc_alloc_index = phba->ctrl.mcc_free_index = 0;
@@ -5621,6 +5668,9 @@ static int beiscsi_dev_probe(struct pci_dev *pcidev,
 	}
 	hwi_enable_intr(phba);
 
+	if (iscsi_host_add(phba->shost, &phba->pcidev->dev))
+		goto free_blkenbld;
+
 	if (beiscsi_setup_boot_info(phba))
 		/*
 		 * log error but continue, because we may not be using
diff --git a/drivers/scsi/be2iscsi/be_main.h b/drivers/scsi/be2iscsi/be_main.h
index 9380b55bdea..9ceab426eec 100644
--- a/drivers/scsi/be2iscsi/be_main.h
+++ b/drivers/scsi/be2iscsi/be_main.h
@@ -36,7 +36,7 @@
 #include <scsi/scsi_transport_iscsi.h>
 
 #define DRV_NAME		"be2iscsi"
-#define BUILD_STR		"10.2.125.0"
+#define BUILD_STR		"10.2.273.0"
 #define BE_NAME			"Emulex OneConnect" \
 				"Open-iSCSI Driver version" BUILD_STR
 #define DRV_DESC		BE_NAME " " "Driver"
@@ -71,8 +71,8 @@
 
 #define BEISCSI_SGLIST_ELEMENTS	30
 
-#define BEISCSI_CMD_PER_LUN	128	/* scsi_host->cmd_per_lun */
-#define BEISCSI_MAX_SECTORS	2048	/* scsi_host->max_sectors */
+#define BEISCSI_CMD_PER_LUN	128 /* scsi_host->cmd_per_lun */
+#define BEISCSI_MAX_SECTORS	1024 /* scsi_host->max_sectors */
 #define BEISCSI_TEMPLATE_HDR_PER_CXN_SIZE 128 /* Template size per cxn */
 
 #define BEISCSI_MAX_CMD_LEN	16	/* scsi_host->max_cmd_len */
@@ -427,6 +427,7 @@ struct beiscsi_hba {
 	struct mgmt_session_info boot_sess;
 	struct invalidate_command_table inv_tbl[128];
 
+	struct be_aic_obj aic_obj[MAX_CPUS];
 	unsigned int attr_log_enable;
 	int (*iotask_fn)(struct iscsi_task *,
 			struct scatterlist *sg,
diff --git a/drivers/scsi/be2iscsi/be_mgmt.c b/drivers/scsi/be2iscsi/be_mgmt.c
index 088bdf752cf..6045aa78986 100644
--- a/drivers/scsi/be2iscsi/be_mgmt.c
+++ b/drivers/scsi/be2iscsi/be_mgmt.c
@@ -155,6 +155,43 @@ void beiscsi_ue_detect(struct beiscsi_hba *phba)
 	}
 }
 
+int be_cmd_modify_eq_delay(struct beiscsi_hba *phba,
+		 struct be_set_eqd *set_eqd, int num)
+{
+	struct be_ctrl_info *ctrl = &phba->ctrl;
+	struct be_mcc_wrb *wrb;
+	struct be_cmd_req_modify_eq_delay *req;
+	unsigned int tag = 0;
+	int i;
+
+	spin_lock(&ctrl->mbox_lock);
+	tag = alloc_mcc_tag(phba);
+	if (!tag) {
+		spin_unlock(&ctrl->mbox_lock);
+		return tag;
+	}
+
+	wrb = wrb_from_mccq(phba);
+	req = embedded_payload(wrb);
+
+	wrb->tag0 |= tag;
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
+		OPCODE_COMMON_MODIFY_EQ_DELAY, sizeof(*req));
+
+	req->num_eq = cpu_to_le32(num);
+	for (i = 0; i < num; i++) {
+		req->delay[i].eq_id = cpu_to_le32(set_eqd[i].eq_id);
+		req->delay[i].phase = 0;
+		req->delay[i].delay_multiplier =
+				cpu_to_le32(set_eqd[i].delay_multiplier);
+	}
+
+	be_mcc_notify(phba);
+	spin_unlock(&ctrl->mbox_lock);
+	return tag;
+}
+
 /**
  * mgmt_reopen_session()- Reopen a session based on reopen_type
  * @phba: Device priv structure instance
@@ -447,8 +484,8 @@ unsigned int mgmt_vendor_specific_fw_cmd(struct be_ctrl_info *ctrl,
 					 struct be_dma_mem *nonemb_cmd)
 {
 	struct be_cmd_resp_hdr *resp;
-	struct be_mcc_wrb *wrb = wrb_from_mccq(phba);
-	struct be_sge *mcc_sge = nonembedded_sgl(wrb);
+	struct be_mcc_wrb *wrb;
+	struct be_sge *mcc_sge;
 	unsigned int tag = 0;
 	struct iscsi_bsg_request *bsg_req = job->request;
 	struct be_bsg_vendor_cmd *req = nonemb_cmd->va;
@@ -465,7 +502,6 @@ unsigned int mgmt_vendor_specific_fw_cmd(struct be_ctrl_info *ctrl,
 	req->sector = sector;
 	req->offset = offset;
 	spin_lock(&ctrl->mbox_lock);
-	memset(wrb, 0, sizeof(*wrb));
 
 	switch (bsg_req->rqst_data.h_vendor.vendor_cmd[0]) {
 	case BEISCSI_WRITE_FLASH:
@@ -495,6 +531,8 @@ unsigned int mgmt_vendor_specific_fw_cmd(struct be_ctrl_info *ctrl,
 		return tag;
 	}
 
+	wrb = wrb_from_mccq(phba);
+	mcc_sge = nonembedded_sgl(wrb);
 	be_wrb_hdr_prepare(wrb, nonemb_cmd->size, false,
 			   job->request_payload.sg_cnt);
 	mcc_sge->pa_hi = cpu_to_le32(upper_32_bits(nonemb_cmd->dma));
@@ -525,7 +563,6 @@ int mgmt_epfw_cleanup(struct beiscsi_hba *phba, unsigned short ulp_num)
 	int status = 0;
 
 	spin_lock(&ctrl->mbox_lock);
-	memset(wrb, 0, sizeof(*wrb));
 
 	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_ISCSI,
@@ -675,7 +712,7 @@ int mgmt_open_connection(struct beiscsi_hba *phba,
 	struct sockaddr_in6 *daddr_in6 = (struct sockaddr_in6 *)dst_addr;
 	struct be_ctrl_info *ctrl = &phba->ctrl;
 	struct be_mcc_wrb *wrb;
-	struct tcp_connect_and_offload_in *req;
+	struct tcp_connect_and_offload_in_v1 *req;
 	unsigned short def_hdr_id;
 	unsigned short def_data_id;
 	struct phys_addr template_address = { 0, 0 };
@@ -702,17 +739,16 @@ int mgmt_open_connection(struct beiscsi_hba *phba,
 		return tag;
 	}
 	wrb = wrb_from_mccq(phba);
-	memset(wrb, 0, sizeof(*wrb));
 	sge = nonembedded_sgl(wrb);
 
 	req = nonemb_cmd->va;
 	memset(req, 0, sizeof(*req));
 	wrb->tag0 |= tag;
 
-	be_wrb_hdr_prepare(wrb, sizeof(*req), false, 1);
+	be_wrb_hdr_prepare(wrb, nonemb_cmd->size, false, 1);
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_ISCSI,
 			   OPCODE_COMMON_ISCSI_TCP_CONNECT_AND_OFFLOAD,
-			   sizeof(*req));
+			   nonemb_cmd->size);
 	if (dst_addr->sa_family == PF_INET) {
 		__be32 s_addr = daddr_in->sin_addr.s_addr;
 		req->ip_address.ip_type = BE2_IPV4;
@@ -758,6 +794,13 @@ int mgmt_open_connection(struct beiscsi_hba *phba,
 	sge->pa_hi = cpu_to_le32(upper_32_bits(nonemb_cmd->dma));
 	sge->pa_lo = cpu_to_le32(nonemb_cmd->dma & 0xFFFFFFFF);
 	sge->len = cpu_to_le32(nonemb_cmd->size);
+
+	if (!is_chip_be2_be3r(phba)) {
+		req->hdr.version = MBX_CMD_VER1;
+		req->tcp_window_size = 0;
+		req->tcp_window_scale_count = 2;
+	}
+
 	be_mcc_notify(phba);
 	spin_unlock(&ctrl->mbox_lock);
 	return tag;
@@ -804,7 +847,7 @@ static int mgmt_exec_nonemb_cmd(struct beiscsi_hba *phba,
 				int resp_buf_len)
 {
 	struct be_ctrl_info *ctrl = &phba->ctrl;
-	struct be_mcc_wrb *wrb = wrb_from_mccq(phba);
+	struct be_mcc_wrb *wrb;
 	struct be_sge *sge;
 	unsigned int tag;
 	int rc = 0;
@@ -816,7 +859,8 @@ static int mgmt_exec_nonemb_cmd(struct beiscsi_hba *phba,
 		rc = -ENOMEM;
 		goto free_cmd;
 	}
-	memset(wrb, 0, sizeof(*wrb));
+
+	wrb = wrb_from_mccq(phba);
 	wrb->tag0 |= tag;
 	sge = nonembedded_sgl(wrb);
 
diff --git a/drivers/scsi/be2iscsi/be_mgmt.h b/drivers/scsi/be2iscsi/be_mgmt.h
index 01b8c97284c..24a8fc57747 100644
--- a/drivers/scsi/be2iscsi/be_mgmt.h
+++ b/drivers/scsi/be2iscsi/be_mgmt.h
@@ -335,5 +335,7 @@ void beiscsi_offload_cxn_v0(struct beiscsi_offload_params *params,
 void beiscsi_offload_cxn_v2(struct beiscsi_offload_params *params,
 			     struct wrb_handle *pwrb_handle);
 void beiscsi_ue_detect(struct beiscsi_hba *phba);
+int be_cmd_modify_eq_delay(struct beiscsi_hba *phba,
+			 struct be_set_eqd *, int num);
 
 #endif
diff --git a/drivers/scsi/bfa/bfad.c b/drivers/scsi/bfa/bfad.c
index cc0fbcdc519..7593b7c1d33 100644
--- a/drivers/scsi/bfa/bfad.c
+++ b/drivers/scsi/bfa/bfad.c
@@ -507,7 +507,7 @@ bfa_fcb_pbc_vport_create(struct bfad_s *bfad, struct bfi_pbc_vport_s pbc_vport)
 	struct bfad_vport_s   *vport;
 	int rc;
 
-	vport = kzalloc(sizeof(struct bfad_vport_s), GFP_KERNEL);
+	vport = kzalloc(sizeof(struct bfad_vport_s), GFP_ATOMIC);
 	if (!vport) {
 		bfa_trc(bfad, 0);
 		return;
diff --git a/drivers/scsi/bnx2fc/bnx2fc_fcoe.c b/drivers/scsi/bnx2fc/bnx2fc_fcoe.c
index 1d41f4b9114..f5484302346 100644
--- a/drivers/scsi/bnx2fc/bnx2fc_fcoe.c
+++ b/drivers/scsi/bnx2fc/bnx2fc_fcoe.c
@@ -464,7 +464,7 @@ static int bnx2fc_l2_rcv_thread(void *arg)
 	struct fcoe_percpu_s *bg = arg;
 	struct sk_buff *skb;
 
-	set_user_nice(current, -20);
+	set_user_nice(current, MIN_NICE);
 	set_current_state(TASK_INTERRUPTIBLE);
 	while (!kthread_should_stop()) {
 		schedule();
@@ -602,7 +602,7 @@ int bnx2fc_percpu_io_thread(void *arg)
 	struct bnx2fc_work *work, *tmp;
 	LIST_HEAD(work_list);
 
-	set_user_nice(current, -20);
+	set_user_nice(current, MIN_NICE);
 	set_current_state(TASK_INTERRUPTIBLE);
 	while (!kthread_should_stop()) {
 		schedule();
diff --git a/drivers/scsi/bnx2fc/bnx2fc_hwi.c b/drivers/scsi/bnx2fc/bnx2fc_hwi.c
index 46a37657307..512aed3ae4f 100644
--- a/drivers/scsi/bnx2fc/bnx2fc_hwi.c
+++ b/drivers/scsi/bnx2fc/bnx2fc_hwi.c
@@ -1966,26 +1966,29 @@ static void bnx2fc_free_hash_table(struct bnx2fc_hba *hba)
 {
 	int i;
 	int segment_count;
-	int hash_table_size;
 	u32 *pbl;
 
-	segment_count = hba->hash_tbl_segment_count;
-	hash_table_size = BNX2FC_NUM_MAX_SESS * BNX2FC_MAX_ROWS_IN_HASH_TBL *
-		sizeof(struct fcoe_hash_table_entry);
+	if (hba->hash_tbl_segments) {
 
-	pbl = hba->hash_tbl_pbl;
-	for (i = 0; i < segment_count; ++i) {
-		dma_addr_t dma_address;
+		pbl = hba->hash_tbl_pbl;
+		if (pbl) {
+			segment_count = hba->hash_tbl_segment_count;
+			for (i = 0; i < segment_count; ++i) {
+				dma_addr_t dma_address;
 
-		dma_address = le32_to_cpu(*pbl);
-		++pbl;
-		dma_address += ((u64)le32_to_cpu(*pbl)) << 32;
-		++pbl;
-		dma_free_coherent(&hba->pcidev->dev,
-				  BNX2FC_HASH_TBL_CHUNK_SIZE,
-				  hba->hash_tbl_segments[i],
-				  dma_address);
+				dma_address = le32_to_cpu(*pbl);
+				++pbl;
+				dma_address += ((u64)le32_to_cpu(*pbl)) << 32;
+				++pbl;
+				dma_free_coherent(&hba->pcidev->dev,
+						  BNX2FC_HASH_TBL_CHUNK_SIZE,
+						  hba->hash_tbl_segments[i],
+						  dma_address);
+			}
+		}
 
+		kfree(hba->hash_tbl_segments);
+		hba->hash_tbl_segments = NULL;
 	}
 
 	if (hba->hash_tbl_pbl) {
@@ -2023,7 +2026,7 @@ static int bnx2fc_allocate_hash_table(struct bnx2fc_hba *hba)
 	dma_segment_array = kzalloc(dma_segment_array_size, GFP_KERNEL);
 	if (!dma_segment_array) {
 		printk(KERN_ERR PFX "hash table pointers (dma) alloc failed\n");
-		return -ENOMEM;
+		goto cleanup_ht;
 	}
 
 	for (i = 0; i < segment_count; ++i) {
@@ -2034,15 +2037,7 @@ static int bnx2fc_allocate_hash_table(struct bnx2fc_hba *hba)
 					   GFP_KERNEL);
 		if (!hba->hash_tbl_segments[i]) {
 			printk(KERN_ERR PFX "hash segment alloc failed\n");
-			while (--i >= 0) {
-				dma_free_coherent(&hba->pcidev->dev,
-						    BNX2FC_HASH_TBL_CHUNK_SIZE,
-						    hba->hash_tbl_segments[i],
-						    dma_segment_array[i]);
-				hba->hash_tbl_segments[i] = NULL;
-			}
-			kfree(dma_segment_array);
-			return -ENOMEM;
+			goto cleanup_dma;
 		}
 		memset(hba->hash_tbl_segments[i], 0,
 		       BNX2FC_HASH_TBL_CHUNK_SIZE);
@@ -2054,8 +2049,7 @@ static int bnx2fc_allocate_hash_table(struct bnx2fc_hba *hba)
 					       GFP_KERNEL);
 	if (!hba->hash_tbl_pbl) {
 		printk(KERN_ERR PFX "hash table pbl alloc failed\n");
-		kfree(dma_segment_array);
-		return -ENOMEM;
+		goto cleanup_dma;
 	}
 	memset(hba->hash_tbl_pbl, 0, PAGE_SIZE);
 
@@ -2080,6 +2074,22 @@ static int bnx2fc_allocate_hash_table(struct bnx2fc_hba *hba)
 	}
 	kfree(dma_segment_array);
 	return 0;
+
+cleanup_dma:
+	for (i = 0; i < segment_count; ++i) {
+		if (hba->hash_tbl_segments[i])
+			dma_free_coherent(&hba->pcidev->dev,
+					    BNX2FC_HASH_TBL_CHUNK_SIZE,
+					    hba->hash_tbl_segments[i],
+					    dma_segment_array[i]);
+	}
+
+	kfree(dma_segment_array);
+
+cleanup_ht:
+	kfree(hba->hash_tbl_segments);
+	hba->hash_tbl_segments = NULL;
+	return -ENOMEM;
 }
 
 /**
diff --git a/drivers/scsi/bnx2i/bnx2i_hwi.c b/drivers/scsi/bnx2i/bnx2i_hwi.c
index b5ffd280a1a..d6d491c2f00 100644
--- a/drivers/scsi/bnx2i/bnx2i_hwi.c
+++ b/drivers/scsi/bnx2i/bnx2i_hwi.c
@@ -1870,7 +1870,7 @@ int bnx2i_percpu_io_thread(void *arg)
 	struct bnx2i_work *work, *tmp;
 	LIST_HEAD(work_list);
 
-	set_user_nice(current, -20);
+	set_user_nice(current, MIN_NICE);
 
 	while (!kthread_should_stop()) {
 		spin_lock_bh(&p->p_work_lock);
diff --git a/drivers/scsi/device_handler/scsi_dh_alua.c b/drivers/scsi/device_handler/scsi_dh_alua.c
index 5248c888552..7bcf67eec92 100644
--- a/drivers/scsi/device_handler/scsi_dh_alua.c
+++ b/drivers/scsi/device_handler/scsi_dh_alua.c
@@ -120,6 +120,7 @@ static struct request *get_alua_req(struct scsi_device *sdev,
 			    "%s: blk_get_request failed\n", __func__);
 		return NULL;
 	}
+	blk_rq_set_block_pc(rq);
 
 	if (buflen && blk_rq_map_kern(q, rq, buffer, buflen, GFP_NOIO)) {
 		blk_put_request(rq);
@@ -128,7 +129,6 @@ static struct request *get_alua_req(struct scsi_device *sdev,
 		return NULL;
 	}
 
-	rq->cmd_type = REQ_TYPE_BLOCK_PC;
 	rq->cmd_flags |= REQ_FAILFAST_DEV | REQ_FAILFAST_TRANSPORT |
 			 REQ_FAILFAST_DRIVER;
 	rq->retries = ALUA_FAILOVER_RETRIES;
diff --git a/drivers/scsi/device_handler/scsi_dh_emc.c b/drivers/scsi/device_handler/scsi_dh_emc.c
index e1c8be06de9..6f07f7fe3aa 100644
--- a/drivers/scsi/device_handler/scsi_dh_emc.c
+++ b/drivers/scsi/device_handler/scsi_dh_emc.c
@@ -280,6 +280,7 @@ static struct request *get_req(struct scsi_device *sdev, int cmd,
 		return NULL;
 	}
 
+	blk_rq_set_block_pc(rq);
 	rq->cmd_len = COMMAND_SIZE(cmd);
 	rq->cmd[0] = cmd;
 
@@ -304,7 +305,6 @@ static struct request *get_req(struct scsi_device *sdev, int cmd,
 		break;
 	}
 
-	rq->cmd_type = REQ_TYPE_BLOCK_PC;
 	rq->cmd_flags |= REQ_FAILFAST_DEV | REQ_FAILFAST_TRANSPORT |
 			 REQ_FAILFAST_DRIVER;
 	rq->timeout = CLARIION_TIMEOUT;
diff --git a/drivers/scsi/device_handler/scsi_dh_hp_sw.c b/drivers/scsi/device_handler/scsi_dh_hp_sw.c
index 084062bb8ee..e9d9fea9e27 100644
--- a/drivers/scsi/device_handler/scsi_dh_hp_sw.c
+++ b/drivers/scsi/device_handler/scsi_dh_hp_sw.c
@@ -120,7 +120,7 @@ retry:
 	if (!req)
 		return SCSI_DH_RES_TEMP_UNAVAIL;
 
-	req->cmd_type = REQ_TYPE_BLOCK_PC;
+	blk_rq_set_block_pc(req);
 	req->cmd_flags |= REQ_FAILFAST_DEV | REQ_FAILFAST_TRANSPORT |
 			  REQ_FAILFAST_DRIVER;
 	req->cmd_len = COMMAND_SIZE(TEST_UNIT_READY);
@@ -250,7 +250,7 @@ static int hp_sw_start_stop(struct hp_sw_dh_data *h)
 	if (!req)
 		return SCSI_DH_RES_TEMP_UNAVAIL;
 
-	req->cmd_type = REQ_TYPE_BLOCK_PC;
+	blk_rq_set_block_pc(req);
 	req->cmd_flags |= REQ_FAILFAST_DEV | REQ_FAILFAST_TRANSPORT |
 			  REQ_FAILFAST_DRIVER;
 	req->cmd_len = COMMAND_SIZE(START_STOP);
diff --git a/drivers/scsi/device_handler/scsi_dh_rdac.c b/drivers/scsi/device_handler/scsi_dh_rdac.c
index 4b9cf93f3fb..826069db984 100644
--- a/drivers/scsi/device_handler/scsi_dh_rdac.c
+++ b/drivers/scsi/device_handler/scsi_dh_rdac.c
@@ -279,6 +279,7 @@ static struct request *get_rdac_req(struct scsi_device *sdev,
 				"get_rdac_req: blk_get_request failed.\n");
 		return NULL;
 	}
+	blk_rq_set_block_pc(rq);
 
 	if (buflen && blk_rq_map_kern(q, rq, buffer, buflen, GFP_NOIO)) {
 		blk_put_request(rq);
@@ -287,7 +288,6 @@ static struct request *get_rdac_req(struct scsi_device *sdev,
 		return NULL;
 	}
 
-	rq->cmd_type = REQ_TYPE_BLOCK_PC;
 	rq->cmd_flags |= REQ_FAILFAST_DEV | REQ_FAILFAST_TRANSPORT |
 			 REQ_FAILFAST_DRIVER;
 	rq->retries = RDAC_RETRIES;
diff --git a/drivers/scsi/dtc.c b/drivers/scsi/dtc.c
index eb29fe7eaf4..0a667fe0500 100644
--- a/drivers/scsi/dtc.c
+++ b/drivers/scsi/dtc.c
@@ -3,8 +3,6 @@
 #define PSEUDO_DMA
 #define DONT_USE_INTR
 #define UNSAFE			/* Leave interrupts enabled during pseudo-dma I/O */
-#define xNDEBUG (NDEBUG_INTR+NDEBUG_RESELECTION+\
-		 NDEBUG_SELECTION+NDEBUG_ARBITRATION)
 #define DMA_WORKS_RIGHT
 
 
diff --git a/drivers/scsi/esas2r/esas2r_main.c b/drivers/scsi/esas2r/esas2r_main.c
index f37f3e3dd5d..6504a195c87 100644
--- a/drivers/scsi/esas2r/esas2r_main.c
+++ b/drivers/scsi/esas2r/esas2r_main.c
@@ -390,7 +390,7 @@ static int esas2r_probe(struct pci_dev *pcid,
 	esas2r_log_dev(ESAS2R_LOG_INFO, &(pcid->dev),
 		       "pci_enable_device() OK");
 	esas2r_log_dev(ESAS2R_LOG_INFO, &(pcid->dev),
-		       "after pci_device_enable() enable_cnt: %d",
+		       "after pci_enable_device() enable_cnt: %d",
 		       pcid->enable_cnt.counter);
 
 	host = scsi_host_alloc(&driver_template, host_alloc_size);
diff --git a/drivers/scsi/fcoe/fcoe.c b/drivers/scsi/fcoe/fcoe.c
index d5e105b173f..00ee0ed642a 100644
--- a/drivers/scsi/fcoe/fcoe.c
+++ b/drivers/scsi/fcoe/fcoe.c
@@ -1872,7 +1872,7 @@ static int fcoe_percpu_receive_thread(void *arg)
 
 	skb_queue_head_init(&tmp);
 
-	set_user_nice(current, -20);
+	set_user_nice(current, MIN_NICE);
 
 retry:
 	while (!kthread_should_stop()) {
diff --git a/drivers/scsi/fnic/fnic.h b/drivers/scsi/fnic/fnic.h
index 528d43b7b56..1d3521e13d7 100644
--- a/drivers/scsi/fnic/fnic.h
+++ b/drivers/scsi/fnic/fnic.h
@@ -39,14 +39,15 @@
 
 #define DRV_NAME		"fnic"
 #define DRV_DESCRIPTION		"Cisco FCoE HBA Driver"
-#define DRV_VERSION		"1.5.0.45"
+#define DRV_VERSION		"1.6.0.10"
 #define PFX			DRV_NAME ": "
 #define DFX                     DRV_NAME "%d: "
 
 #define DESC_CLEAN_LOW_WATERMARK 8
 #define FNIC_UCSM_DFLT_THROTTLE_CNT_BLD	16 /* UCSM default throttle count */
 #define FNIC_MIN_IO_REQ			256 /* Min IO throttle count */
-#define FNIC_MAX_IO_REQ		2048 /* scsi_cmnd tag map entries */
+#define FNIC_MAX_IO_REQ		1024 /* scsi_cmnd tag map entries */
+#define FNIC_DFLT_IO_REQ        256 /* Default scsi_cmnd tag map entries */
 #define	FNIC_IO_LOCKS		64 /* IO locks: power of 2 */
 #define FNIC_DFLT_QUEUE_DEPTH	32
 #define	FNIC_STATS_RATE_LIMIT	4 /* limit rate at which stats are pulled up */
diff --git a/drivers/scsi/fnic/fnic_debugfs.c b/drivers/scsi/fnic/fnic_debugfs.c
index b6073f87576..2c613bdea78 100644
--- a/drivers/scsi/fnic/fnic_debugfs.c
+++ b/drivers/scsi/fnic/fnic_debugfs.c
@@ -25,6 +25,21 @@ static struct dentry *fnic_trace_debugfs_file;
 static struct dentry *fnic_trace_enable;
 static struct dentry *fnic_stats_debugfs_root;
 
+static struct dentry *fnic_fc_trace_debugfs_file;
+static struct dentry *fnic_fc_rdata_trace_debugfs_file;
+static struct dentry *fnic_fc_trace_enable;
+static struct dentry *fnic_fc_trace_clear;
+
+struct fc_trace_flag_type {
+	u8 fc_row_file;
+	u8 fc_normal_file;
+	u8 fnic_trace;
+	u8 fc_trace;
+	u8 fc_clear;
+};
+
+static struct fc_trace_flag_type *fc_trc_flag;
+
 /*
  * fnic_debugfs_init - Initialize debugfs for fnic debug logging
  *
@@ -56,6 +71,18 @@ int fnic_debugfs_init(void)
 		return rc;
 	}
 
+	/* Allocate memory to structure */
+	fc_trc_flag = (struct fc_trace_flag_type *)
+		vmalloc(sizeof(struct fc_trace_flag_type));
+
+	if (fc_trc_flag) {
+		fc_trc_flag->fc_row_file = 0;
+		fc_trc_flag->fc_normal_file = 1;
+		fc_trc_flag->fnic_trace = 2;
+		fc_trc_flag->fc_trace = 3;
+		fc_trc_flag->fc_clear = 4;
+	}
+
 	rc = 0;
 	return rc;
 }
@@ -74,15 +101,19 @@ void fnic_debugfs_terminate(void)
 
 	debugfs_remove(fnic_trace_debugfs_root);
 	fnic_trace_debugfs_root = NULL;
+
+	if (fc_trc_flag)
+		vfree(fc_trc_flag);
 }
 
 /*
- * fnic_trace_ctrl_open - Open the trace_enable file
+ * fnic_trace_ctrl_open - Open the trace_enable file for fnic_trace
+ *               Or Open fc_trace_enable file for fc_trace
  * @inode: The inode pointer.
  * @file: The file pointer to attach the trace enable/disable flag.
  *
  * Description:
- * This routine opens a debugsfs file trace_enable.
+ * This routine opens a debugsfs file trace_enable or fc_trace_enable.
  *
  * Returns:
  * This function returns zero if successful.
@@ -94,15 +125,19 @@ static int fnic_trace_ctrl_open(struct inode *inode, struct file *filp)
 }
 
 /*
- * fnic_trace_ctrl_read - Read a trace_enable debugfs file
+ * fnic_trace_ctrl_read -
+ *          Read  trace_enable ,fc_trace_enable
+ *              or fc_trace_clear debugfs file
  * @filp: The file pointer to read from.
  * @ubuf: The buffer to copy the data to.
  * @cnt: The number of bytes to read.
  * @ppos: The position in the file to start reading from.
  *
  * Description:
- * This routine reads value of variable fnic_tracing_enabled
- * and stores into local @buf. It will start reading file at @ppos and
+ * This routine reads value of variable fnic_tracing_enabled or
+ * fnic_fc_tracing_enabled or fnic_fc_trace_cleared
+ * and stores into local @buf.
+ * It will start reading file at @ppos and
  * copy up to @cnt of data to @ubuf from @buf.
  *
  * Returns:
@@ -114,13 +149,25 @@ static ssize_t fnic_trace_ctrl_read(struct file *filp,
 {
 	char buf[64];
 	int len;
-	len = sprintf(buf, "%u\n", fnic_tracing_enabled);
+	u8 *trace_type;
+	len = 0;
+	trace_type = (u8 *)filp->private_data;
+	if (*trace_type == fc_trc_flag->fnic_trace)
+		len = sprintf(buf, "%u\n", fnic_tracing_enabled);
+	else if (*trace_type == fc_trc_flag->fc_trace)
+		len = sprintf(buf, "%u\n", fnic_fc_tracing_enabled);
+	else if (*trace_type == fc_trc_flag->fc_clear)
+		len = sprintf(buf, "%u\n", fnic_fc_trace_cleared);
+	else
+		pr_err("fnic: Cannot read to any debugfs file\n");
 
 	return simple_read_from_buffer(ubuf, cnt, ppos, buf, len);
 }
 
 /*
- * fnic_trace_ctrl_write - Write to trace_enable debugfs file
+ * fnic_trace_ctrl_write -
+ * Write to trace_enable, fc_trace_enable or
+ *         fc_trace_clear debugfs file
  * @filp: The file pointer to write from.
  * @ubuf: The buffer to copy the data from.
  * @cnt: The number of bytes to write.
@@ -128,7 +175,8 @@ static ssize_t fnic_trace_ctrl_read(struct file *filp,
  *
  * Description:
  * This routine writes data from user buffer @ubuf to buffer @buf and
- * sets fnic_tracing_enabled value as per user input.
+ * sets fc_trace_enable ,tracing_enable or fnic_fc_trace_cleared
+ * value as per user input.
  *
  * Returns:
  * This function returns the amount of data that was written.
@@ -140,6 +188,8 @@ static ssize_t fnic_trace_ctrl_write(struct file *filp,
 	char buf[64];
 	unsigned long val;
 	int ret;
+	u8 *trace_type;
+	trace_type = (u8 *)filp->private_data;
 
 	if (cnt >= sizeof(buf))
 		return -EINVAL;
@@ -153,12 +203,27 @@ static ssize_t fnic_trace_ctrl_write(struct file *filp,
 	if (ret < 0)
 		return ret;
 
-	fnic_tracing_enabled = val;
+	if (*trace_type == fc_trc_flag->fnic_trace)
+		fnic_tracing_enabled = val;
+	else if (*trace_type == fc_trc_flag->fc_trace)
+		fnic_fc_tracing_enabled = val;
+	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");
+
 	(*ppos)++;
 
 	return cnt;
 }
 
+static const struct file_operations fnic_trace_ctrl_fops = {
+	.owner = THIS_MODULE,
+	.open = fnic_trace_ctrl_open,
+	.read = fnic_trace_ctrl_read,
+	.write = fnic_trace_ctrl_write,
+};
+
 /*
  * fnic_trace_debugfs_open - Open the fnic trace log
  * @inode: The inode pointer
@@ -178,19 +243,36 @@ static int fnic_trace_debugfs_open(struct inode *inode,
 				  struct file *file)
 {
 	fnic_dbgfs_t *fnic_dbg_prt;
+	u8 *rdata_ptr;
+	rdata_ptr = (u8 *)inode->i_private;
 	fnic_dbg_prt = kzalloc(sizeof(fnic_dbgfs_t), GFP_KERNEL);
 	if (!fnic_dbg_prt)
 		return -ENOMEM;
 
-	fnic_dbg_prt->buffer = vmalloc((3*(trace_max_pages * PAGE_SIZE)));
-	if (!fnic_dbg_prt->buffer) {
-		kfree(fnic_dbg_prt);
-		return -ENOMEM;
+	if (*rdata_ptr == fc_trc_flag->fnic_trace) {
+		fnic_dbg_prt->buffer = vmalloc(3 *
+					(trace_max_pages * PAGE_SIZE));
+		if (!fnic_dbg_prt->buffer) {
+			kfree(fnic_dbg_prt);
+			return -ENOMEM;
+		}
+		memset((void *)fnic_dbg_prt->buffer, 0,
+		3 * (trace_max_pages * PAGE_SIZE));
+		fnic_dbg_prt->buffer_len = fnic_get_trace_data(fnic_dbg_prt);
+	} else {
+		fnic_dbg_prt->buffer =
+			vmalloc(3 * (fnic_fc_trace_max_pages * PAGE_SIZE));
+		if (!fnic_dbg_prt->buffer) {
+			kfree(fnic_dbg_prt);
+			return -ENOMEM;
+		}
+		memset((void *)fnic_dbg_prt->buffer, 0,
+			3 * (fnic_fc_trace_max_pages * PAGE_SIZE));
+		fnic_dbg_prt->buffer_len =
+			fnic_fc_trace_get_data(fnic_dbg_prt, *rdata_ptr);
 	}
-	memset((void *)fnic_dbg_prt->buffer, 0,
-			  (3*(trace_max_pages * PAGE_SIZE)));
-	fnic_dbg_prt->buffer_len = fnic_get_trace_data(fnic_dbg_prt);
 	file->private_data = fnic_dbg_prt;
+
 	return 0;
 }
 
@@ -272,13 +354,6 @@ static int fnic_trace_debugfs_release(struct inode *inode,
 	return 0;
 }
 
-static const struct file_operations fnic_trace_ctrl_fops = {
-	.owner = THIS_MODULE,
-	.open = fnic_trace_ctrl_open,
-	.read = fnic_trace_ctrl_read,
-	.write = fnic_trace_ctrl_write,
-};
-
 static const struct file_operations fnic_trace_debugfs_fops = {
 	.owner = THIS_MODULE,
 	.open = fnic_trace_debugfs_open,
@@ -306,9 +381,10 @@ int fnic_trace_debugfs_init(void)
 		return rc;
 	}
 	fnic_trace_enable = debugfs_create_file("tracing_enable",
-					  S_IFREG|S_IRUGO|S_IWUSR,
-					  fnic_trace_debugfs_root,
-					  NULL, &fnic_trace_ctrl_fops);
+					S_IFREG|S_IRUGO|S_IWUSR,
+					fnic_trace_debugfs_root,
+					&(fc_trc_flag->fnic_trace),
+					&fnic_trace_ctrl_fops);
 
 	if (!fnic_trace_enable) {
 		printk(KERN_DEBUG
@@ -317,10 +393,10 @@ int fnic_trace_debugfs_init(void)
 	}
 
 	fnic_trace_debugfs_file = debugfs_create_file("trace",
-						  S_IFREG|S_IRUGO|S_IWUSR,
-						  fnic_trace_debugfs_root,
-						  NULL,
-						  &fnic_trace_debugfs_fops);
+					S_IFREG|S_IRUGO|S_IWUSR,
+					fnic_trace_debugfs_root,
+					&(fc_trc_flag->fnic_trace),
+					&fnic_trace_debugfs_fops);
 
 	if (!fnic_trace_debugfs_file) {
 		printk(KERN_DEBUG
@@ -340,14 +416,104 @@ int fnic_trace_debugfs_init(void)
  */
 void fnic_trace_debugfs_terminate(void)
 {
-	if (fnic_trace_debugfs_file) {
-		debugfs_remove(fnic_trace_debugfs_file);
-		fnic_trace_debugfs_file = NULL;
+	debugfs_remove(fnic_trace_debugfs_file);
+	fnic_trace_debugfs_file = NULL;
+
+	debugfs_remove(fnic_trace_enable);
+	fnic_trace_enable = NULL;
+}
+
+/*
+ * fnic_fc_trace_debugfs_init -
+ * Initialize debugfs for fnic control frame trace logging
+ *
+ * Description:
+ * When Debugfs is configured this routine sets up the fnic_fc debugfs
+ * file system. If not already created, this routine will create the
+ * create file trace to log fnic fc trace buffer output into debugfs and
+ * it will also create file fc_trace_enable to control enable/disable of
+ * trace logging into trace buffer.
+ */
+
+int fnic_fc_trace_debugfs_init(void)
+{
+	int rc = -1;
+
+	if (!fnic_trace_debugfs_root) {
+		pr_err("fnic:Debugfs root directory doesn't exist\n");
+		return rc;
+	}
+
+	fnic_fc_trace_enable = debugfs_create_file("fc_trace_enable",
+					S_IFREG|S_IRUGO|S_IWUSR,
+					fnic_trace_debugfs_root,
+					&(fc_trc_flag->fc_trace),
+					&fnic_trace_ctrl_fops);
+
+	if (!fnic_fc_trace_enable) {
+		pr_err("fnic: Failed create fc_trace_enable file\n");
+		return rc;
+	}
+
+	fnic_fc_trace_clear = debugfs_create_file("fc_trace_clear",
+					S_IFREG|S_IRUGO|S_IWUSR,
+					fnic_trace_debugfs_root,
+					&(fc_trc_flag->fc_clear),
+					&fnic_trace_ctrl_fops);
+
+	if (!fnic_fc_trace_clear) {
+		pr_err("fnic: Failed to create fc_trace_enable file\n");
+		return rc;
+	}
+
+	fnic_fc_rdata_trace_debugfs_file =
+		debugfs_create_file("fc_trace_rdata",
+				    S_IFREG|S_IRUGO|S_IWUSR,
+				    fnic_trace_debugfs_root,
+				    &(fc_trc_flag->fc_normal_file),
+				    &fnic_trace_debugfs_fops);
+
+	if (!fnic_fc_rdata_trace_debugfs_file) {
+		pr_err("fnic: Failed create fc_rdata_trace file\n");
+		return rc;
 	}
-	if (fnic_trace_enable) {
-		debugfs_remove(fnic_trace_enable);
-		fnic_trace_enable = NULL;
+
+	fnic_fc_trace_debugfs_file =
+		debugfs_create_file("fc_trace",
+				    S_IFREG|S_IRUGO|S_IWUSR,
+				    fnic_trace_debugfs_root,
+				    &(fc_trc_flag->fc_row_file),
+				    &fnic_trace_debugfs_fops);
+
+	if (!fnic_fc_trace_debugfs_file) {
+		pr_err("fnic: Failed to create fc_trace file\n");
+		return rc;
 	}
+	rc = 0;
+	return rc;
+}
+
+/*
+ * fnic_fc_trace_debugfs_terminate - Tear down debugfs infrastructure
+ *
+ * Description:
+ * When Debugfs is configured this routine removes debugfs file system
+ * elements that are specific to fnic_fc trace logging.
+ */
+
+void fnic_fc_trace_debugfs_terminate(void)
+{
+	debugfs_remove(fnic_fc_trace_debugfs_file);
+	fnic_fc_trace_debugfs_file = NULL;
+
+	debugfs_remove(fnic_fc_rdata_trace_debugfs_file);
+	fnic_fc_rdata_trace_debugfs_file = NULL;
+
+	debugfs_remove(fnic_fc_trace_enable);
+	fnic_fc_trace_enable = NULL;
+
+	debugfs_remove(fnic_fc_trace_clear);
+	fnic_fc_trace_clear = NULL;
 }
 
 /*
diff --git a/drivers/scsi/fnic/fnic_fcs.c b/drivers/scsi/fnic/fnic_fcs.c
index 1671325aec7..1b948f633fc 100644
--- a/drivers/scsi/fnic/fnic_fcs.c
+++ b/drivers/scsi/fnic/fnic_fcs.c
@@ -66,19 +66,35 @@ void fnic_handle_link(struct work_struct *work)
 	fnic->link_down_cnt = vnic_dev_link_down_cnt(fnic->vdev);
 
 	if (old_link_status == fnic->link_status) {
-		if (!fnic->link_status)
+		if (!fnic->link_status) {
 			/* DOWN -> DOWN */
 			spin_unlock_irqrestore(&fnic->fnic_lock, flags);
-		else {
+			fnic_fc_trace_set_data(fnic->lport->host->host_no,
+				FNIC_FC_LE, "Link Status: DOWN->DOWN",
+				strlen("Link Status: DOWN->DOWN"));
+		} else {
 			if (old_link_down_cnt != fnic->link_down_cnt) {
 				/* UP -> DOWN -> UP */
 				fnic->lport->host_stats.link_failure_count++;
 				spin_unlock_irqrestore(&fnic->fnic_lock, flags);
+				fnic_fc_trace_set_data(
+					fnic->lport->host->host_no,
+					FNIC_FC_LE,
+					"Link Status:UP_DOWN_UP",
+					strlen("Link_Status:UP_DOWN_UP")
+					);
 				FNIC_FCS_DBG(KERN_DEBUG, fnic->lport->host,
 					     "link down\n");
 				fcoe_ctlr_link_down(&fnic->ctlr);
 				if (fnic->config.flags & VFCF_FIP_CAPABLE) {
 					/* start FCoE VLAN discovery */
+					fnic_fc_trace_set_data(
+						fnic->lport->host->host_no,
+						FNIC_FC_LE,
+						"Link Status: UP_DOWN_UP_VLAN",
+						strlen(
+						"Link Status: UP_DOWN_UP_VLAN")
+						);
 					fnic_fcoe_send_vlan_req(fnic);
 					return;
 				}
@@ -88,22 +104,36 @@ void fnic_handle_link(struct work_struct *work)
 			} 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 */
 		spin_unlock_irqrestore(&fnic->fnic_lock, flags);
 		if (fnic->config.flags & VFCF_FIP_CAPABLE) {
 			/* start FCoE VLAN discovery */
+				fnic_fc_trace_set_data(
+				fnic->lport->host->host_no,
+				FNIC_FC_LE, "Link Status: DOWN_UP_VLAN",
+				strlen("Link Status: DOWN_UP_VLAN"));
 			fnic_fcoe_send_vlan_req(fnic);
 			return;
 		}
 		FNIC_FCS_DBG(KERN_DEBUG, fnic->lport->host, "link up\n");
+		fnic_fc_trace_set_data(fnic->lport->host->host_no, FNIC_FC_LE,
+			"Link Status: DOWN_UP", strlen("Link Status: DOWN_UP"));
 		fcoe_ctlr_link_up(&fnic->ctlr);
 	} else {
 		/* UP -> DOWN */
 		fnic->lport->host_stats.link_failure_count++;
 		spin_unlock_irqrestore(&fnic->fnic_lock, flags);
 		FNIC_FCS_DBG(KERN_DEBUG, fnic->lport->host, "link down\n");
+		fnic_fc_trace_set_data(
+			fnic->lport->host->host_no, FNIC_FC_LE,
+			"Link Status: UP_DOWN",
+			strlen("Link Status: UP_DOWN"));
 		fcoe_ctlr_link_down(&fnic->ctlr);
 	}
 
@@ -267,11 +297,6 @@ static inline int is_fnic_fip_flogi_reject(struct fcoe_ctlr *fip,
 
 	if (desc->fip_dtype == FIP_DT_FLOGI) {
 
-		shost_printk(KERN_DEBUG, lport->host,
-			  " FIP TYPE FLOGI: fab name:%llx "
-			  "vfid:%d map:%x\n",
-			  fip->sel_fcf->fabric_name, fip->sel_fcf->vfid,
-			  fip->sel_fcf->fc_map);
 		if (dlen < sizeof(*els) + sizeof(*fh) + 1)
 			return 0;
 
@@ -616,6 +641,10 @@ static inline int fnic_import_rq_eth_pkt(struct fnic *fnic, struct sk_buff *skb)
 					"using UCSM\n");
 			goto drop;
 		}
+		if ((fnic_fc_trace_set_data(fnic->lport->host->host_no,
+			FNIC_FC_RECV|0x80, (char *)skb->data, skb->len)) != 0) {
+			printk(KERN_ERR "fnic ctlr frame trace error!!!");
+		}
 		skb_queue_tail(&fnic->fip_frame_queue, skb);
 		queue_work(fnic_fip_queue, &fnic->fip_frame_work);
 		return 1;		/* let caller know packet was used */
@@ -844,6 +873,10 @@ static void fnic_rq_cmpl_frame_recv(struct vnic_rq *rq, struct cq_desc
 	}
 	fr_dev(fp) = fnic->lport;
 	spin_unlock_irqrestore(&fnic->fnic_lock, flags);
+	if ((fnic_fc_trace_set_data(fnic->lport->host->host_no, FNIC_FC_RECV,
+					(char *)skb->data, skb->len)) != 0) {
+		printk(KERN_ERR "fnic ctlr frame trace error!!!");
+	}
 
 	skb_queue_tail(&fnic->frame_queue, skb);
 	queue_work(fnic_event_queue, &fnic->frame_work);
@@ -951,6 +984,15 @@ void fnic_eth_send(struct fcoe_ctlr *fip, struct sk_buff *skb)
 		vlan_hdr->h_vlan_proto = htons(ETH_P_8021Q);
 		vlan_hdr->h_vlan_encapsulated_proto = eth_hdr->h_proto;
 		vlan_hdr->h_vlan_TCI = htons(fnic->vlan_id);
+		if ((fnic_fc_trace_set_data(fnic->lport->host->host_no,
+			FNIC_FC_SEND|0x80, (char *)eth_hdr, skb->len)) != 0) {
+			printk(KERN_ERR "fnic ctlr frame trace error!!!");
+		}
+	} else {
+		if ((fnic_fc_trace_set_data(fnic->lport->host->host_no,
+			FNIC_FC_SEND|0x80, (char *)skb->data, skb->len)) != 0) {
+			printk(KERN_ERR "fnic ctlr frame trace error!!!");
+		}
 	}
 
 	pa = pci_map_single(fnic->pdev, skb->data, skb->len, PCI_DMA_TODEVICE);
@@ -1023,6 +1065,11 @@ static int fnic_send_frame(struct fnic *fnic, struct fc_frame *fp)
 
 	pa = pci_map_single(fnic->pdev, eth_hdr, tot_len, PCI_DMA_TODEVICE);
 
+	if ((fnic_fc_trace_set_data(fnic->lport->host->host_no, FNIC_FC_SEND,
+				(char *)eth_hdr, tot_len)) != 0) {
+		printk(KERN_ERR "fnic ctlr frame trace error!!!");
+	}
+
 	spin_lock_irqsave(&fnic->wq_lock[0], flags);
 
 	if (!vnic_wq_desc_avail(wq)) {
diff --git a/drivers/scsi/fnic/fnic_main.c b/drivers/scsi/fnic/fnic_main.c
index 33e4ec2bfe7..8c56fdc3a45 100644
--- a/drivers/scsi/fnic/fnic_main.c
+++ b/drivers/scsi/fnic/fnic_main.c
@@ -74,6 +74,11 @@ module_param(fnic_trace_max_pages, uint, S_IRUGO|S_IWUSR);
 MODULE_PARM_DESC(fnic_trace_max_pages, "Total allocated memory pages "
 					"for fnic trace buffer");
 
+unsigned int fnic_fc_trace_max_pages = 64;
+module_param(fnic_fc_trace_max_pages, uint, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(fnic_fc_trace_max_pages,
+		 "Total allocated memory pages for fc trace buffer");
+
 static unsigned int fnic_max_qdepth = FNIC_DFLT_QUEUE_DEPTH;
 module_param(fnic_max_qdepth, uint, S_IRUGO|S_IWUSR);
 MODULE_PARM_DESC(fnic_max_qdepth, "Queue depth to report for each LUN");
@@ -111,7 +116,7 @@ static struct scsi_host_template fnic_host_template = {
 	.change_queue_type = fc_change_queue_type,
 	.this_id = -1,
 	.cmd_per_lun = 3,
-	.can_queue = FNIC_MAX_IO_REQ,
+	.can_queue = FNIC_DFLT_IO_REQ,
 	.use_clustering = ENABLE_CLUSTERING,
 	.sg_tablesize = FNIC_MAX_SG_DESC_CNT,
 	.max_sectors = 0xffff,
@@ -773,6 +778,7 @@ static int fnic_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		shost_printk(KERN_INFO, fnic->lport->host,
 			     "firmware uses non-FIP mode\n");
 		fcoe_ctlr_init(&fnic->ctlr, FIP_MODE_NON_FIP);
+		fnic->ctlr.state = FIP_ST_NON_FIP;
 	}
 	fnic->state = FNIC_IN_FC_MODE;
 
@@ -1033,11 +1039,20 @@ static int __init fnic_init_module(void)
 	/* Allocate memory for trace buffer */
 	err = fnic_trace_buf_init();
 	if (err < 0) {
-		printk(KERN_ERR PFX "Trace buffer initialization Failed "
-				  "Fnic Tracing utility is disabled\n");
+		printk(KERN_ERR PFX
+		       "Trace buffer initialization Failed. "
+		       "Fnic Tracing utility is disabled\n");
 		fnic_trace_free();
 	}
 
+    /* Allocate memory for fc trace buffer */
+	err = fnic_fc_trace_init();
+	if (err < 0) {
+		printk(KERN_ERR PFX "FC trace buffer initialization Failed "
+		       "FC frame tracing utility is disabled\n");
+		fnic_fc_trace_free();
+	}
+
 	/* Create a cache for allocation of default size sgls */
 	len = sizeof(struct fnic_dflt_sgl_list);
 	fnic_sgl_cache[FNIC_SGL_CACHE_DFLT] = kmem_cache_create
@@ -1118,6 +1133,7 @@ err_create_fnic_sgl_slab_max:
 	kmem_cache_destroy(fnic_sgl_cache[FNIC_SGL_CACHE_DFLT]);
 err_create_fnic_sgl_slab_dflt:
 	fnic_trace_free();
+	fnic_fc_trace_free();
 	fnic_debugfs_terminate();
 	return err;
 }
@@ -1135,6 +1151,7 @@ static void __exit fnic_cleanup_module(void)
 	kmem_cache_destroy(fnic_io_req_cache);
 	fc_release_transport(fnic_fc_transport);
 	fnic_trace_free();
+	fnic_fc_trace_free();
 	fnic_debugfs_terminate();
 }
 
diff --git a/drivers/scsi/fnic/fnic_scsi.c b/drivers/scsi/fnic/fnic_scsi.c
index 0521436d05d..ea28b5ca4c7 100644
--- a/drivers/scsi/fnic/fnic_scsi.c
+++ b/drivers/scsi/fnic/fnic_scsi.c
@@ -1312,8 +1312,9 @@ static void fnic_cleanup_io(struct fnic *fnic, int exclude_id)
 
 cleanup_scsi_cmd:
 		sc->result = DID_TRANSPORT_DISRUPTED << 16;
-		FNIC_SCSI_DBG(KERN_DEBUG, fnic->lport->host, "fnic_cleanup_io:"
-			      " DID_TRANSPORT_DISRUPTED\n");
+		FNIC_SCSI_DBG(KERN_DEBUG, fnic->lport->host,
+			      "%s: sc duration = %lu DID_TRANSPORT_DISRUPTED\n",
+			      __func__, (jiffies - start_time));
 
 		if (atomic64_read(&fnic->io_cmpl_skip))
 			atomic64_dec(&fnic->io_cmpl_skip);
@@ -1733,6 +1734,7 @@ int fnic_abort_cmd(struct scsi_cmnd *sc)
 	struct fnic_stats *fnic_stats;
 	struct abort_stats *abts_stats;
 	struct terminate_stats *term_stats;
+	enum fnic_ioreq_state old_ioreq_state;
 	int tag;
 	DECLARE_COMPLETION_ONSTACK(tm_done);
 
@@ -1793,6 +1795,7 @@ int fnic_abort_cmd(struct scsi_cmnd *sc)
 	 * the completion wont be done till mid-layer, since abort
 	 * has already started.
 	 */
+	old_ioreq_state = CMD_STATE(sc);
 	CMD_STATE(sc) = FNIC_IOREQ_ABTS_PENDING;
 	CMD_ABTS_STATUS(sc) = FCPIO_INVALID_CODE;
 
@@ -1816,6 +1819,8 @@ int fnic_abort_cmd(struct scsi_cmnd *sc)
 	if (fnic_queue_abort_io_req(fnic, sc->request->tag, task_req,
 				    fc_lun.scsi_lun, io_req)) {
 		spin_lock_irqsave(io_lock, flags);
+		if (CMD_STATE(sc) == FNIC_IOREQ_ABTS_PENDING)
+			CMD_STATE(sc) = old_ioreq_state;
 		io_req = (struct fnic_io_req *)CMD_SP(sc);
 		if (io_req)
 			io_req->abts_done = NULL;
@@ -1859,12 +1864,8 @@ int fnic_abort_cmd(struct scsi_cmnd *sc)
 	if (CMD_ABTS_STATUS(sc) == FCPIO_INVALID_CODE) {
 		spin_unlock_irqrestore(io_lock, flags);
 		if (task_req == FCPIO_ITMF_ABT_TASK) {
-			FNIC_SCSI_DBG(KERN_INFO,
-				fnic->lport->host, "Abort Driver Timeout\n");
 			atomic64_inc(&abts_stats->abort_drv_timeouts);
 		} else {
-			FNIC_SCSI_DBG(KERN_INFO, fnic->lport->host,
-				"Terminate Driver Timeout\n");
 			atomic64_inc(&term_stats->terminate_drv_timeouts);
 		}
 		CMD_FLAGS(sc) |= FNIC_IO_ABT_TERM_TIMED_OUT;
diff --git a/drivers/scsi/fnic/fnic_trace.c b/drivers/scsi/fnic/fnic_trace.c
index e002e7187dc..c7728592682 100644
--- a/drivers/scsi/fnic/fnic_trace.c
+++ b/drivers/scsi/fnic/fnic_trace.c
@@ -20,6 +20,7 @@
 #include <linux/errno.h>
 #include <linux/spinlock.h>
 #include <linux/kallsyms.h>
+#include <linux/time.h>
 #include "fnic_io.h"
 #include "fnic.h"
 
@@ -32,6 +33,16 @@ static DEFINE_SPINLOCK(fnic_trace_lock);
 static fnic_trace_dbg_t fnic_trace_entries;
 int fnic_tracing_enabled = 1;
 
+/* static char *fnic_fc_ctlr_trace_buf_p; */
+
+static int fc_trace_max_entries;
+static unsigned long fnic_fc_ctlr_trace_buf_p;
+static fnic_trace_dbg_t fc_trace_entries;
+int fnic_fc_tracing_enabled = 1;
+int fnic_fc_trace_cleared = 1;
+static DEFINE_SPINLOCK(fnic_fc_trace_lock);
+
+
 /*
  * fnic_trace_get_buf - Give buffer pointer to user to fill up trace information
  *
@@ -428,10 +439,10 @@ int fnic_trace_buf_init(void)
 	}
 	err = fnic_trace_debugfs_init();
 	if (err < 0) {
-		printk(KERN_ERR PFX "Failed to initialize debugfs for tracing\n");
+		pr_err("fnic: Failed to initialize debugfs for tracing\n");
 		goto err_fnic_trace_debugfs_init;
 	}
-	printk(KERN_INFO PFX "Successfully Initialized Trace Buffer\n");
+	pr_info("fnic: Successfully Initialized Trace Buffer\n");
 	return err;
 err_fnic_trace_debugfs_init:
 	fnic_trace_free();
@@ -456,3 +467,314 @@ void fnic_trace_free(void)
 	}
 	printk(KERN_INFO PFX "Successfully Freed Trace Buffer\n");
 }
+
+/*
+ * fnic_fc_ctlr_trace_buf_init -
+ * Initialize trace buffer to log fnic control frames
+ * Description:
+ * Initialize trace buffer data structure by allocating
+ * required memory for trace data as well as for Indexes.
+ * Frame size is 256 bytes and
+ * memory is allocated for 1024 entries of 256 bytes.
+ * Page_offset(Index) is set to the address of trace entry
+ * and page_offset is initialized by adding frame size
+ * to the previous page_offset entry.
+ */
+
+int fnic_fc_trace_init(void)
+{
+	unsigned long fc_trace_buf_head;
+	int err = 0;
+	int i;
+
+	fc_trace_max_entries = (fnic_fc_trace_max_pages * PAGE_SIZE)/
+				FC_TRC_SIZE_BYTES;
+	fnic_fc_ctlr_trace_buf_p = (unsigned long)vmalloc(
+					fnic_fc_trace_max_pages * PAGE_SIZE);
+	if (!fnic_fc_ctlr_trace_buf_p) {
+		pr_err("fnic: Failed to allocate memory for "
+		       "FC Control Trace Buf\n");
+		err = -ENOMEM;
+		goto err_fnic_fc_ctlr_trace_buf_init;
+	}
+
+	memset((void *)fnic_fc_ctlr_trace_buf_p, 0,
+			fnic_fc_trace_max_pages * PAGE_SIZE);
+
+	/* Allocate memory for page offset */
+	fc_trace_entries.page_offset = vmalloc(fc_trace_max_entries *
+						sizeof(unsigned long));
+	if (!fc_trace_entries.page_offset) {
+		pr_err("fnic:Failed to allocate memory for page_offset\n");
+		if (fnic_fc_ctlr_trace_buf_p) {
+			pr_err("fnic: Freeing FC Control Trace Buf\n");
+			vfree((void *)fnic_fc_ctlr_trace_buf_p);
+			fnic_fc_ctlr_trace_buf_p = 0;
+		}
+		err = -ENOMEM;
+		goto err_fnic_fc_ctlr_trace_buf_init;
+	}
+	memset((void *)fc_trace_entries.page_offset, 0,
+	       (fc_trace_max_entries * sizeof(unsigned long)));
+
+	fc_trace_entries.rd_idx = fc_trace_entries.wr_idx = 0;
+	fc_trace_buf_head = fnic_fc_ctlr_trace_buf_p;
+
+	/*
+	* Set up fc_trace_entries.page_offset field with memory location
+	* for every trace entry
+	*/
+	for (i = 0; i < fc_trace_max_entries; i++) {
+		fc_trace_entries.page_offset[i] = fc_trace_buf_head;
+		fc_trace_buf_head += FC_TRC_SIZE_BYTES;
+	}
+	err = fnic_fc_trace_debugfs_init();
+	if (err < 0) {
+		pr_err("fnic: Failed to initialize FC_CTLR tracing.\n");
+		goto err_fnic_fc_ctlr_trace_debugfs_init;
+	}
+	pr_info("fnic: Successfully Initialized FC_CTLR Trace Buffer\n");
+	return err;
+
+err_fnic_fc_ctlr_trace_debugfs_init:
+	fnic_fc_trace_free();
+err_fnic_fc_ctlr_trace_buf_init:
+	return err;
+}
+
+/*
+ * Fnic_fc_ctlr_trace_free - Free memory of fnic_fc_ctlr trace data structures.
+ */
+void fnic_fc_trace_free(void)
+{
+	fnic_fc_tracing_enabled = 0;
+	fnic_fc_trace_debugfs_terminate();
+	if (fc_trace_entries.page_offset) {
+		vfree((void *)fc_trace_entries.page_offset);
+		fc_trace_entries.page_offset = NULL;
+	}
+	if (fnic_fc_ctlr_trace_buf_p) {
+		vfree((void *)fnic_fc_ctlr_trace_buf_p);
+		fnic_fc_ctlr_trace_buf_p = 0;
+	}
+	pr_info("fnic:Successfully FC_CTLR Freed Trace Buffer\n");
+}
+
+/*
+ * fnic_fc_ctlr_set_trace_data:
+ *       Maintain rd & wr idx accordingly and set data
+ * Passed parameters:
+ *       host_no: host number accociated with fnic
+ *       frame_type: send_frame, rece_frame or link event
+ *       fc_frame: pointer to fc_frame
+ *       frame_len: Length of the fc_frame
+ * Description:
+ *   This routine will get next available wr_idx and
+ *   copy all passed trace data to the buffer pointed by wr_idx
+ *   and increment wr_idx. It will also make sure that we dont
+ *   overwrite the entry which we are reading and also
+ *   wrap around if we reach the maximum entries.
+ * Returned Value:
+ *   It will return 0 for success or -1 for failure
+ */
+int fnic_fc_trace_set_data(u32 host_no, u8 frame_type,
+				char *frame, u32 fc_trc_frame_len)
+{
+	unsigned long flags;
+	struct fc_trace_hdr *fc_buf;
+	unsigned long eth_fcoe_hdr_len;
+	char *fc_trace;
+
+	if (fnic_fc_tracing_enabled == 0)
+		return 0;
+
+	spin_lock_irqsave(&fnic_fc_trace_lock, flags);
+
+	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");
+		memset((void *)fnic_fc_ctlr_trace_buf_p, 0,
+				fnic_fc_trace_max_pages * PAGE_SIZE);
+		fnic_fc_trace_cleared = 0;
+	}
+
+	fc_buf = (struct fc_trace_hdr *)
+		fc_trace_entries.page_offset[fc_trace_entries.wr_idx];
+
+	fc_trace_entries.wr_idx++;
+
+	if (fc_trace_entries.wr_idx >= fc_trace_max_entries)
+		fc_trace_entries.wr_idx = 0;
+
+	if (fc_trace_entries.wr_idx == fc_trace_entries.rd_idx) {
+		fc_trace_entries.rd_idx++;
+		if (fc_trace_entries.rd_idx >= fc_trace_max_entries)
+			fc_trace_entries.rd_idx = 0;
+	}
+
+	fc_buf->time_stamp = CURRENT_TIME;
+	fc_buf->host_no = host_no;
+	fc_buf->frame_type = frame_type;
+
+	fc_trace = (char *)FC_TRACE_ADDRESS(fc_buf);
+
+	/* During the receive path, we do not have eth hdr as well as fcoe hdr
+	 * at trace entry point so we will stuff 0xff just to make it generic.
+	 */
+	if (frame_type == FNIC_FC_RECV) {
+		eth_fcoe_hdr_len = sizeof(struct ethhdr) +
+					sizeof(struct fcoe_hdr);
+		fc_trc_frame_len = fc_trc_frame_len + eth_fcoe_hdr_len;
+		memset((char *)fc_trace, 0xff, eth_fcoe_hdr_len);
+		/* Copy the rest of data frame */
+		memcpy((char *)(fc_trace + eth_fcoe_hdr_len), (void *)frame,
+		min_t(u8, fc_trc_frame_len,
+			(u8)(FC_TRC_SIZE_BYTES - FC_TRC_HEADER_SIZE)));
+	} else {
+		memcpy((char *)fc_trace, (void *)frame,
+		min_t(u8, fc_trc_frame_len,
+			(u8)(FC_TRC_SIZE_BYTES - FC_TRC_HEADER_SIZE)));
+	}
+
+	/* Store the actual received length */
+	fc_buf->frame_len = fc_trc_frame_len;
+
+	spin_unlock_irqrestore(&fnic_fc_trace_lock, flags);
+	return 0;
+}
+
+/*
+ * fnic_fc_ctlr_get_trace_data: Copy trace buffer to a memory file
+ * Passed parameter:
+ *       @fnic_dbgfs_t: pointer to debugfs trace buffer
+ *       rdata_flag: 1 => Unformated file
+ *                   0 => formated file
+ * Description:
+ *       This routine will copy the trace data to memory file with
+ *       proper formatting and also copy to another memory
+ *       file without formatting for further procesing.
+ * Retrun Value:
+ *       Number of bytes that were dumped into fnic_dbgfs_t
+ */
+
+int fnic_fc_trace_get_data(fnic_dbgfs_t *fnic_dbgfs_prt, u8 rdata_flag)
+{
+	int rd_idx, wr_idx;
+	unsigned long flags;
+	int len = 0, j;
+	struct fc_trace_hdr *tdata;
+	char *fc_trace;
+
+	spin_lock_irqsave(&fnic_fc_trace_lock, flags);
+	if (fc_trace_entries.wr_idx == fc_trace_entries.rd_idx) {
+		spin_unlock_irqrestore(&fnic_fc_trace_lock, flags);
+		pr_info("fnic: Buffer is empty\n");
+		return 0;
+	}
+	rd_idx = fc_trace_entries.rd_idx;
+	wr_idx = fc_trace_entries.wr_idx;
+	if (rdata_flag == 0) {
+		len += snprintf(fnic_dbgfs_prt->buffer + len,
+			(fnic_fc_trace_max_pages * PAGE_SIZE * 3) - len,
+			"Time Stamp (UTC)\t\t"
+			"Host No:   F Type:  len:     FCoE_FRAME:\n");
+	}
+
+	while (rd_idx != wr_idx) {
+		tdata = (struct fc_trace_hdr *)
+			fc_trace_entries.page_offset[rd_idx];
+		if (!tdata) {
+			pr_info("fnic: Rd data is NULL\n");
+			spin_unlock_irqrestore(&fnic_fc_trace_lock, flags);
+			return 0;
+		}
+		if (rdata_flag == 0) {
+			copy_and_format_trace_data(tdata,
+				fnic_dbgfs_prt, &len, rdata_flag);
+		} else {
+			fc_trace = (char *)tdata;
+			for (j = 0; j < FC_TRC_SIZE_BYTES; j++) {
+				len += snprintf(fnic_dbgfs_prt->buffer + len,
+				(fnic_fc_trace_max_pages * PAGE_SIZE * 3)
+				- len, "%02x", fc_trace[j] & 0xff);
+			} /* for loop */
+			len += snprintf(fnic_dbgfs_prt->buffer + len,
+				(fnic_fc_trace_max_pages * PAGE_SIZE * 3) - len,
+				"\n");
+		}
+		rd_idx++;
+		if (rd_idx > (fc_trace_max_entries - 1))
+			rd_idx = 0;
+	}
+
+	spin_unlock_irqrestore(&fnic_fc_trace_lock, flags);
+	return len;
+}
+
+/*
+ * copy_and_format_trace_data: Copy formatted data to char * buffer
+ * Passed Parameter:
+ *      @fc_trace_hdr_t: pointer to trace data
+ *      @fnic_dbgfs_t: pointer to debugfs trace buffer
+ *      @orig_len: pointer to len
+ *      rdata_flag: 0 => Formated file, 1 => Unformated file
+ * Description:
+ *      This routine will format and copy the passed trace data
+ *      for formated file or unformated file accordingly.
+ */
+
+void copy_and_format_trace_data(struct fc_trace_hdr *tdata,
+				fnic_dbgfs_t *fnic_dbgfs_prt, int *orig_len,
+				u8 rdata_flag)
+{
+	struct tm tm;
+	int j, i = 1, len;
+	char *fc_trace, *fmt;
+	int ethhdr_len = sizeof(struct ethhdr) - 1;
+	int fcoehdr_len = sizeof(struct fcoe_hdr);
+	int fchdr_len = sizeof(struct fc_frame_header);
+	int max_size = fnic_fc_trace_max_pages * PAGE_SIZE * 3;
+
+	tdata->frame_type = tdata->frame_type & 0x7F;
+
+	len = *orig_len;
+
+	time_to_tm(tdata->time_stamp.tv_sec, 0, &tm);
+
+	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,
+		fmt,
+		tm.tm_mon + 1, tm.tm_mday, tm.tm_year + 1900,
+		tm.tm_hour, tm.tm_min, tm.tm_sec,
+		tdata->time_stamp.tv_nsec, tdata->host_no,
+		tdata->frame_type, tdata->frame_len);
+
+	fc_trace = (char *)FC_TRACE_ADDRESS(tdata);
+
+	for (j = 0; j < min_t(u8, tdata->frame_len,
+		(u8)(FC_TRC_SIZE_BYTES - FC_TRC_HEADER_SIZE)); j++) {
+		if (tdata->frame_type == FNIC_FC_LE) {
+			len += snprintf(fnic_dbgfs_prt->buffer + len,
+				max_size - len, "%c", fc_trace[j]);
+		} else {
+			len += snprintf(fnic_dbgfs_prt->buffer + len,
+				max_size - len, "%02x", fc_trace[j] & 0xff);
+			len += snprintf(fnic_dbgfs_prt->buffer + len,
+				max_size - len, " ");
+			if (j == ethhdr_len ||
+				j == ethhdr_len + fcoehdr_len ||
+				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,
+					"\n\t\t\t\t\t\t\t\t");
+				i++;
+			}
+		} /* end of else*/
+	} /* End of for loop*/
+	len += snprintf(fnic_dbgfs_prt->buffer + len,
+		max_size - len, "\n");
+	*orig_len = len;
+}
diff --git a/drivers/scsi/fnic/fnic_trace.h b/drivers/scsi/fnic/fnic_trace.h
index d412f2ee3c4..a8aa0578fcb 100644
--- a/drivers/scsi/fnic/fnic_trace.h
+++ b/drivers/scsi/fnic/fnic_trace.h
@@ -19,6 +19,17 @@
 #define __FNIC_TRACE_H__
 
 #define FNIC_ENTRY_SIZE_BYTES 64
+#define FC_TRC_SIZE_BYTES 256
+#define FC_TRC_HEADER_SIZE sizeof(struct fc_trace_hdr)
+
+/*
+ * Fisrt bit of FNIC_FC_RECV and FNIC_FC_SEND is used to represent the type
+ * of frame 1 => Eth frame, 0=> FC frame
+ */
+
+#define FNIC_FC_RECV 0x52 /* Character R */
+#define FNIC_FC_SEND 0x54 /* Character T */
+#define FNIC_FC_LE 0x4C /* Character L */
 
 extern ssize_t simple_read_from_buffer(void __user *to,
 					  size_t count,
@@ -30,6 +41,10 @@ extern unsigned int fnic_trace_max_pages;
 extern int fnic_tracing_enabled;
 extern unsigned int trace_max_pages;
 
+extern unsigned int fnic_fc_trace_max_pages;
+extern int fnic_fc_tracing_enabled;
+extern int fnic_fc_trace_cleared;
+
 typedef struct fnic_trace_dbg {
 	int wr_idx;
 	int rd_idx;
@@ -56,6 +71,16 @@ struct fnic_trace_data {
 
 typedef struct fnic_trace_data fnic_trace_data_t;
 
+struct fc_trace_hdr {
+	struct timespec time_stamp;
+	u32 host_no;
+	u8 frame_type;
+	u8 frame_len;
+} __attribute__((__packed__));
+
+#define FC_TRACE_ADDRESS(a) \
+	((unsigned long)(a) + sizeof(struct fc_trace_hdr))
+
 #define FNIC_TRACE_ENTRY_SIZE \
 		  (FNIC_ENTRY_SIZE_BYTES - sizeof(fnic_trace_data_t))
 
@@ -88,4 +113,17 @@ int fnic_debugfs_init(void);
 void fnic_debugfs_terminate(void);
 int fnic_trace_debugfs_init(void);
 void fnic_trace_debugfs_terminate(void);
+
+/* Fnic FC CTLR Trace releated function */
+int fnic_fc_trace_init(void);
+void fnic_fc_trace_free(void);
+int fnic_fc_trace_set_data(u32 host_no, u8 frame_type,
+				char *frame, u32 fc_frame_len);
+int fnic_fc_trace_get_data(fnic_dbgfs_t *fnic_dbgfs_prt, u8 rdata_flag);
+void copy_and_format_trace_data(struct fc_trace_hdr *tdata,
+				fnic_dbgfs_t *fnic_dbgfs_prt,
+				int *len, u8 rdata_flag);
+int fnic_fc_trace_debugfs_init(void);
+void fnic_fc_trace_debugfs_terminate(void);
+
 #endif
diff --git a/drivers/scsi/g_NCR5380.c b/drivers/scsi/g_NCR5380.c
index 7176365e916..a1bc8ca958e 100644
--- a/drivers/scsi/g_NCR5380.c
+++ b/drivers/scsi/g_NCR5380.c
@@ -78,10 +78,6 @@
  *     
  */
 
-/*
- * $Log: generic_NCR5380.c,v $
- */
-
 /* settings for DTC3181E card with only Mustek scanner attached */
 #define USLEEP
 #define USLEEP_POLL	1
diff --git a/drivers/scsi/g_NCR5380.h b/drivers/scsi/g_NCR5380.h
index 1bcdb7beb77..703adf78e0b 100644
--- a/drivers/scsi/g_NCR5380.h
+++ b/drivers/scsi/g_NCR5380.h
@@ -25,10 +25,6 @@
  * 1+ (800) 334-5454
  */
 
-/*
- * $Log: generic_NCR5380.h,v $
- */
-
 #ifndef GENERIC_NCR5380_H
 #define GENERIC_NCR5380_H
 
@@ -58,8 +54,6 @@ static const char* generic_NCR5380_info(struct Scsi_Host *);
 #define CAN_QUEUE 16
 #endif
 
-#ifndef HOSTS_C
-
 #define __STRVAL(x) #x
 #define STRVAL(x) __STRVAL(x)
 
@@ -131,7 +125,6 @@ static const char* generic_NCR5380_info(struct Scsi_Host *);
 #define BOARD_NCR53C400A 2
 #define BOARD_DTC3181E	3
 
-#endif /* else def HOSTS_C */
 #endif /* ndef ASM */
 #endif /* GENERIC_NCR5380_H */
 
diff --git a/drivers/scsi/hpsa.c b/drivers/scsi/hpsa.c
index 9a6e4a2cd07..5858600bfe5 100644
--- a/drivers/scsi/hpsa.c
+++ b/drivers/scsi/hpsa.c
@@ -115,9 +115,15 @@ static const struct pci_device_id hpsa_pci_device_id[] = {
 	{PCI_VENDOR_ID_HP,     PCI_DEVICE_ID_HP_CISSI,     0x103C, 0x21C3},
 	{PCI_VENDOR_ID_HP,     PCI_DEVICE_ID_HP_CISSI,     0x103C, 0x21C4},
 	{PCI_VENDOR_ID_HP,     PCI_DEVICE_ID_HP_CISSI,     0x103C, 0x21C5},
+	{PCI_VENDOR_ID_HP,     PCI_DEVICE_ID_HP_CISSI,     0x103C, 0x21C6},
 	{PCI_VENDOR_ID_HP,     PCI_DEVICE_ID_HP_CISSI,     0x103C, 0x21C7},
 	{PCI_VENDOR_ID_HP,     PCI_DEVICE_ID_HP_CISSI,     0x103C, 0x21C8},
 	{PCI_VENDOR_ID_HP,     PCI_DEVICE_ID_HP_CISSI,     0x103C, 0x21C9},
+	{PCI_VENDOR_ID_HP,     PCI_DEVICE_ID_HP_CISSI,     0x103C, 0x21CA},
+	{PCI_VENDOR_ID_HP,     PCI_DEVICE_ID_HP_CISSI,     0x103C, 0x21CB},
+	{PCI_VENDOR_ID_HP,     PCI_DEVICE_ID_HP_CISSI,     0x103C, 0x21CC},
+	{PCI_VENDOR_ID_HP,     PCI_DEVICE_ID_HP_CISSI,     0x103C, 0x21CD},
+	{PCI_VENDOR_ID_HP,     PCI_DEVICE_ID_HP_CISSI,     0x103C, 0x21CE},
 	{PCI_VENDOR_ID_HP_3PAR, 0x0075, 0x1590, 0x0076},
 	{PCI_VENDOR_ID_HP_3PAR, 0x0075, 0x1590, 0x0087},
 	{PCI_VENDOR_ID_HP_3PAR, 0x0075, 0x1590, 0x007D},
@@ -165,9 +171,15 @@ static struct board_type products[] = {
 	{0x21C3103C, "Smart Array", &SA5_access},
 	{0x21C4103C, "Smart Array", &SA5_access},
 	{0x21C5103C, "Smart Array", &SA5_access},
+	{0x21C6103C, "Smart Array", &SA5_access},
 	{0x21C7103C, "Smart Array", &SA5_access},
 	{0x21C8103C, "Smart Array", &SA5_access},
 	{0x21C9103C, "Smart Array", &SA5_access},
+	{0x21CA103C, "Smart Array", &SA5_access},
+	{0x21CB103C, "Smart Array", &SA5_access},
+	{0x21CC103C, "Smart Array", &SA5_access},
+	{0x21CD103C, "Smart Array", &SA5_access},
+	{0x21CE103C, "Smart Array", &SA5_access},
 	{0x00761590, "HP Storage P1224 Array Controller", &SA5_access},
 	{0x00871590, "HP Storage P1224e Array Controller", &SA5_access},
 	{0x007D1590, "HP Storage P1228 Array Controller", &SA5_access},
@@ -2836,6 +2848,8 @@ static int hpsa_get_pdisk_of_ioaccel2(struct ctlr_info *h,
 
 	/* Get the list of physical devices */
 	physicals = kzalloc(reportsize, GFP_KERNEL);
+	if (physicals == NULL)
+		return 0;
 	if (hpsa_scsi_do_report_phys_luns(h, (struct ReportLUNdata *) physicals,
 		reportsize, extended)) {
 		dev_err(&h->pdev->dev,
@@ -2963,19 +2977,24 @@ u8 *figure_lunaddrbytes(struct ctlr_info *h, int raid_ctlr_position, int i,
 static int hpsa_hba_mode_enabled(struct ctlr_info *h)
 {
 	int rc;
+	int hba_mode_enabled;
 	struct bmic_controller_parameters *ctlr_params;
 	ctlr_params = kzalloc(sizeof(struct bmic_controller_parameters),
 		GFP_KERNEL);
 
 	if (!ctlr_params)
-		return 0;
+		return -ENOMEM;
 	rc = hpsa_bmic_ctrl_mode_sense(h, RAID_CTLR_LUNID, 0, ctlr_params,
 		sizeof(struct bmic_controller_parameters));
-	if (rc != 0) {
+	if (rc) {
 		kfree(ctlr_params);
-		return 0;
+		return rc;
 	}
-	return ctlr_params->nvram_flags & (1 << 3) ? 1 : 0;
+
+	hba_mode_enabled =
+		((ctlr_params->nvram_flags & HBA_MODE_ENABLED_FLAG) != 0);
+	kfree(ctlr_params);
+	return hba_mode_enabled;
 }
 
 static void hpsa_update_scsi_devices(struct ctlr_info *h, int hostno)
@@ -3001,7 +3020,7 @@ static void hpsa_update_scsi_devices(struct ctlr_info *h, int hostno)
 	int reportlunsize = sizeof(*physdev_list) + HPSA_MAX_PHYS_LUN * 24;
 	int i, n_ext_target_devs, ndevs_to_allocate;
 	int raid_ctlr_position;
-	u8 rescan_hba_mode;
+	int rescan_hba_mode;
 	DECLARE_BITMAP(lunzerobits, MAX_EXT_TARGETS);
 
 	currentsd = kzalloc(sizeof(*currentsd) * HPSA_MAX_DEVICES, GFP_KERNEL);
@@ -3016,6 +3035,8 @@ static void hpsa_update_scsi_devices(struct ctlr_info *h, int hostno)
 	memset(lunzerobits, 0, sizeof(lunzerobits));
 
 	rescan_hba_mode = hpsa_hba_mode_enabled(h);
+	if (rescan_hba_mode < 0)
+		goto out;
 
 	if (!h->hba_mode_enabled && rescan_hba_mode)
 		dev_warn(&h->pdev->dev, "HBA mode enabled\n");
diff --git a/drivers/scsi/hpsa.h b/drivers/scsi/hpsa.h
index 44235a27e1b..1e3cf33a82c 100644
--- a/drivers/scsi/hpsa.h
+++ b/drivers/scsi/hpsa.h
@@ -90,6 +90,7 @@ struct bmic_controller_parameters {
 	u8   automatic_drive_slamming;
 	u8   reserved1;
 	u8   nvram_flags;
+#define HBA_MODE_ENABLED_FLAG (1 << 3)
 	u8   cache_nvram_flags;
 	u8   drive_config_flags;
 	u16  reserved2;
diff --git a/drivers/scsi/ibmvscsi/ibmvfc.c b/drivers/scsi/ibmvscsi/ibmvfc.c
index 23f5ba5e647..8dd47689d58 100644
--- a/drivers/scsi/ibmvscsi/ibmvfc.c
+++ b/drivers/scsi/ibmvscsi/ibmvfc.c
@@ -4515,7 +4515,7 @@ static int ibmvfc_work(void *data)
 	struct ibmvfc_host *vhost = data;
 	int rc;
 
-	set_user_nice(current, -20);
+	set_user_nice(current, MIN_NICE);
 
 	while (1) {
 		rc = wait_event_interruptible(vhost->work_wait_q,
diff --git a/drivers/scsi/ibmvscsi/ibmvscsi.c b/drivers/scsi/ibmvscsi/ibmvscsi.c
index fa764406df6..2ebfb2bb0f4 100644
--- a/drivers/scsi/ibmvscsi/ibmvscsi.c
+++ b/drivers/scsi/ibmvscsi/ibmvscsi.c
@@ -2213,7 +2213,7 @@ static int ibmvscsi_work(void *data)
 	struct ibmvscsi_host_data *hostdata = data;
 	int rc;
 
-	set_user_nice(current, -20);
+	set_user_nice(current, MIN_NICE);
 
 	while (1) {
 		rc = wait_event_interruptible(hostdata->work_wait_q,
diff --git a/drivers/scsi/isci/remote_device.c b/drivers/scsi/isci/remote_device.c
index 96a26f45467..cc51f38b116 100644
--- a/drivers/scsi/isci/remote_device.c
+++ b/drivers/scsi/isci/remote_device.c
@@ -1541,7 +1541,7 @@ void isci_remote_device_release(struct kref *kref)
 	clear_bit(IDEV_STOP_PENDING, &idev->flags);
 	clear_bit(IDEV_IO_READY, &idev->flags);
 	clear_bit(IDEV_GONE, &idev->flags);
-	smp_mb__before_clear_bit();
+	smp_mb__before_atomic();
 	clear_bit(IDEV_ALLOCATED, &idev->flags);
 	wake_up(&ihost->eventq);
 }
diff --git a/drivers/scsi/libiscsi.c b/drivers/scsi/libiscsi.c
index 26dc005bb0f..ecd7bd304ef 100644
--- a/drivers/scsi/libiscsi.c
+++ b/drivers/scsi/libiscsi.c
@@ -1442,9 +1442,9 @@ static int iscsi_xmit_task(struct iscsi_conn *conn)
 		conn->task = NULL;
 	}
 	/* regular RX path uses back_lock */
-	spin_lock_bh(&conn->session->back_lock);
+	spin_lock(&conn->session->back_lock);
 	__iscsi_put_task(task);
-	spin_unlock_bh(&conn->session->back_lock);
+	spin_unlock(&conn->session->back_lock);
 	return rc;
 }
 
diff --git a/drivers/scsi/lpfc/lpfc_hbadisc.c b/drivers/scsi/lpfc/lpfc_hbadisc.c
index 59b51c529ba..294c072e908 100644
--- a/drivers/scsi/lpfc/lpfc_hbadisc.c
+++ b/drivers/scsi/lpfc/lpfc_hbadisc.c
@@ -731,7 +731,7 @@ lpfc_do_work(void *p)
 	struct lpfc_hba *phba = p;
 	int rc;
 
-	set_user_nice(current, -20);
+	set_user_nice(current, MIN_NICE);
 	current->flags |= PF_NOFREEZE;
 	phba->data_flags = 0;
 
diff --git a/drivers/scsi/lpfc/lpfc_sli.c b/drivers/scsi/lpfc/lpfc_sli.c
index 6bb51f8e3c1..393662c24df 100644
--- a/drivers/scsi/lpfc/lpfc_sli.c
+++ b/drivers/scsi/lpfc/lpfc_sli.c
@@ -265,6 +265,16 @@ lpfc_sli4_eq_get(struct lpfc_queue *q)
 		return NULL;
 
 	q->hba_index = idx;
+
+	/*
+	 * insert barrier for instruction interlock : data from the hardware
+	 * must have the valid bit checked before it can be copied and acted
+	 * upon. Given what was seen in lpfc_sli4_cq_get() of speculative
+	 * instructions allowing action on content before valid bit checked,
+	 * add barrier here as well. May not be needed as "content" is a
+	 * single 32-bit entity here (vs multi word structure for cq's).
+	 */
+	mb();
 	return eqe;
 }
 
@@ -370,6 +380,17 @@ lpfc_sli4_cq_get(struct lpfc_queue *q)
 
 	cqe = q->qe[q->hba_index].cqe;
 	q->hba_index = idx;
+
+	/*
+	 * insert barrier for instruction interlock : data from the hardware
+	 * must have the valid bit checked before it can be copied and acted
+	 * upon. Speculative instructions were allowing a bcopy at the start
+	 * of lpfc_sli4_fp_handle_wcqe(), which is called immediately
+	 * after our return, to copy data before the valid bit check above
+	 * was done. As such, some of the copied data was stale. The barrier
+	 * ensures the check is before any data is copied.
+	 */
+	mb();
 	return cqe;
 }
 
diff --git a/drivers/scsi/mac_scsi.c b/drivers/scsi/mac_scsi.c
index f5cdc68cd5b..6a039eb1cbc 100644
--- a/drivers/scsi/mac_scsi.c
+++ b/drivers/scsi/mac_scsi.c
@@ -25,10 +25,6 @@
  * 1+ (800) 334-5454
  */
 
-/*
- * $Log: mac_NCR5380.c,v $
- */
-
 #include <linux/types.h>
 #include <linux/stddef.h>
 #include <linux/ctype.h>
@@ -58,12 +54,6 @@
 
 #include "NCR5380.h"
 
-#if 0
-#define NDEBUG (NDEBUG_INTR | NDEBUG_PSEUDO_DMA | NDEBUG_ARBITRATION | NDEBUG_SELECTION | NDEBUG_RESELECTION)
-#else
-#define NDEBUG (NDEBUG_ABORT)
-#endif
-
 #define RESET_BOOT
 #define DRIVER_SETUP
 
diff --git a/drivers/scsi/mac_scsi.h b/drivers/scsi/mac_scsi.h
index 7dc62fce1c4..06969b06e54 100644
--- a/drivers/scsi/mac_scsi.h
+++ b/drivers/scsi/mac_scsi.h
@@ -22,10 +22,6 @@
  * 1+ (800) 334-5454
  */
 
-/*
- * $Log: cumana_NCR5380.h,v $
- */
-
 #ifndef MAC_NCR5380_H
 #define MAC_NCR5380_H
 
@@ -51,8 +47,6 @@
 
 #include <scsi/scsicam.h>
 
-#ifndef HOSTS_C
-
 #define NCR5380_implementation_fields \
     int port, ctrl
 
@@ -75,10 +69,6 @@
 #define NCR5380_show_info macscsi_show_info
 #define NCR5380_write_info macscsi_write_info
 
-#define BOARD_NORMAL	0
-#define BOARD_NCR53C400	1
-
-#endif /* ndef HOSTS_C */
 #endif /* ndef ASM */
 #endif /* MAC_NCR5380_H */
 
diff --git a/drivers/scsi/megaraid/megaraid_sas_base.c b/drivers/scsi/megaraid/megaraid_sas_base.c
index d84d02c2aad..112799b131a 100644
--- a/drivers/scsi/megaraid/megaraid_sas_base.c
+++ b/drivers/scsi/megaraid/megaraid_sas_base.c
@@ -3061,7 +3061,8 @@ megasas_transition_to_ready(struct megasas_instance *instance, int ocr)
 	u32 cur_state;
 	u32 abs_state, curr_abs_state;
 
-	fw_state = instance->instancet->read_fw_status_reg(instance->reg_set) & MFI_STATE_MASK;
+	abs_state = instance->instancet->read_fw_status_reg(instance->reg_set);
+	fw_state = abs_state & MFI_STATE_MASK;
 
 	if (fw_state != MFI_STATE_READY)
 		printk(KERN_INFO "megasas: Waiting for FW to come to ready"
@@ -3069,9 +3070,6 @@ megasas_transition_to_ready(struct megasas_instance *instance, int ocr)
 
 	while (fw_state != MFI_STATE_READY) {
 
-		abs_state =
-		instance->instancet->read_fw_status_reg(instance->reg_set);
-
 		switch (fw_state) {
 
 		case MFI_STATE_FAULT:
@@ -3223,10 +3221,8 @@ megasas_transition_to_ready(struct megasas_instance *instance, int ocr)
 		 * The cur_state should not last for more than max_wait secs
 		 */
 		for (i = 0; i < (max_wait * 1000); i++) {
-			fw_state = instance->instancet->read_fw_status_reg(instance->reg_set) &
-					MFI_STATE_MASK ;
-		curr_abs_state =
-		instance->instancet->read_fw_status_reg(instance->reg_set);
+			curr_abs_state = instance->instancet->
+				read_fw_status_reg(instance->reg_set);
 
 			if (abs_state == curr_abs_state) {
 				msleep(1);
@@ -3242,6 +3238,9 @@ megasas_transition_to_ready(struct megasas_instance *instance, int ocr)
 			       "in %d secs\n", fw_state, max_wait);
 			return -ENODEV;
 		}
+
+		abs_state = curr_abs_state;
+		fw_state = curr_abs_state & MFI_STATE_MASK;
 	}
 	printk(KERN_INFO "megasas: FW now in Ready state\n");
 
diff --git a/drivers/scsi/mpt2sas/mpt2sas_base.c b/drivers/scsi/mpt2sas/mpt2sas_base.c
index bde63f7452b..8b88118e20e 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_base.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_base.c
@@ -1739,14 +1739,14 @@ mpt2sas_base_free_smid(struct MPT2SAS_ADAPTER *ioc, u16 smid)
 			list_for_each_entry_safe(chain_req, next,
 			    &ioc->scsi_lookup[i].chain_list, tracker_list) {
 				list_del_init(&chain_req->tracker_list);
-				list_add_tail(&chain_req->tracker_list,
+				list_add(&chain_req->tracker_list,
 				    &ioc->free_chain_list);
 			}
 		}
 		ioc->scsi_lookup[i].cb_idx = 0xFF;
 		ioc->scsi_lookup[i].scmd = NULL;
 		ioc->scsi_lookup[i].direct_io = 0;
-		list_add_tail(&ioc->scsi_lookup[i].tracker_list,
+		list_add(&ioc->scsi_lookup[i].tracker_list,
 		    &ioc->free_list);
 		spin_unlock_irqrestore(&ioc->scsi_lookup_lock, flags);
 
@@ -1764,13 +1764,13 @@ mpt2sas_base_free_smid(struct MPT2SAS_ADAPTER *ioc, u16 smid)
 		/* hi-priority */
 		i = smid - ioc->hi_priority_smid;
 		ioc->hpr_lookup[i].cb_idx = 0xFF;
-		list_add_tail(&ioc->hpr_lookup[i].tracker_list,
+		list_add(&ioc->hpr_lookup[i].tracker_list,
 		    &ioc->hpr_free_list);
 	} else if (smid <= ioc->hba_queue_depth) {
 		/* internal queue */
 		i = smid - ioc->internal_smid;
 		ioc->internal_lookup[i].cb_idx = 0xFF;
-		list_add_tail(&ioc->internal_lookup[i].tracker_list,
+		list_add(&ioc->internal_lookup[i].tracker_list,
 		    &ioc->internal_free_list);
 	}
 	spin_unlock_irqrestore(&ioc->scsi_lookup_lock, flags);
diff --git a/drivers/scsi/mpt2sas/mpt2sas_base.h b/drivers/scsi/mpt2sas/mpt2sas_base.h
index 1f2ac3a2862..fd3b998c75b 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_base.h
+++ b/drivers/scsi/mpt2sas/mpt2sas_base.h
@@ -1065,7 +1065,7 @@ void mpt2sas_scsih_event_callback(struct MPT2SAS_ADAPTER *ioc, u8 msix_index,
     u32 reply);
 int mpt2sas_scsih_issue_tm(struct MPT2SAS_ADAPTER *ioc, u16 handle,
 	uint channel, uint id, uint lun, u8 type, u16 smid_task,
-	ulong timeout, unsigned long serial_number, enum mutex_type m_type);
+	ulong timeout, enum mutex_type m_type);
 void mpt2sas_scsih_set_tm_flag(struct MPT2SAS_ADAPTER *ioc, u16 handle);
 void mpt2sas_scsih_clear_tm_flag(struct MPT2SAS_ADAPTER *ioc, u16 handle);
 void mpt2sas_expander_remove(struct MPT2SAS_ADAPTER *ioc, u64 sas_address);
diff --git a/drivers/scsi/mpt2sas/mpt2sas_ctl.c b/drivers/scsi/mpt2sas/mpt2sas_ctl.c
index b7f887c9b0b..62df8f9d427 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_ctl.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_ctl.c
@@ -987,7 +987,7 @@ _ctl_do_mpt_command(struct MPT2SAS_ADAPTER *ioc, struct mpt2_ioctl_command karg,
 			mpt2sas_scsih_issue_tm(ioc,
 			    le16_to_cpu(mpi_request->FunctionDependent1), 0, 0,
 			    0, MPI2_SCSITASKMGMT_TASKTYPE_TARGET_RESET, 0, 10,
-			    0, TM_MUTEX_ON);
+			    TM_MUTEX_ON);
 			ioc->tm_cmds.status = MPT2_CMD_NOT_USED;
 		} else
 			mpt2sas_base_hard_reset_handler(ioc, CAN_SLEEP,
diff --git a/drivers/scsi/mpt2sas/mpt2sas_scsih.c b/drivers/scsi/mpt2sas/mpt2sas_scsih.c
index 6fd7d40b2c4..5055f925d2c 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_scsih.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_scsih.c
@@ -2368,7 +2368,6 @@ mpt2sas_scsih_clear_tm_flag(struct MPT2SAS_ADAPTER *ioc, u16 handle)
  * @type: MPI2_SCSITASKMGMT_TASKTYPE__XXX (defined in mpi2_init.h)
  * @smid_task: smid assigned to the task
  * @timeout: timeout in seconds
- * @serial_number: the serial_number from scmd
  * @m_type: TM_MUTEX_ON or TM_MUTEX_OFF
  * Context: user
  *
@@ -2381,7 +2380,7 @@ mpt2sas_scsih_clear_tm_flag(struct MPT2SAS_ADAPTER *ioc, u16 handle)
 int
 mpt2sas_scsih_issue_tm(struct MPT2SAS_ADAPTER *ioc, u16 handle, uint channel,
     uint id, uint lun, u8 type, u16 smid_task, ulong timeout,
-	unsigned long serial_number, enum mutex_type m_type)
+	enum mutex_type m_type)
 {
 	Mpi2SCSITaskManagementRequest_t *mpi_request;
 	Mpi2SCSITaskManagementReply_t *mpi_reply;
@@ -2634,8 +2633,7 @@ _scsih_abort(struct scsi_cmnd *scmd)
 	handle = sas_device_priv_data->sas_target->handle;
 	r = mpt2sas_scsih_issue_tm(ioc, handle, scmd->device->channel,
 	    scmd->device->id, scmd->device->lun,
-	    MPI2_SCSITASKMGMT_TASKTYPE_ABORT_TASK, smid, 30,
-	    scmd->serial_number, TM_MUTEX_ON);
+	    MPI2_SCSITASKMGMT_TASKTYPE_ABORT_TASK, smid, 30, TM_MUTEX_ON);
 
  out:
 	sdev_printk(KERN_INFO, scmd->device, "task abort: %s scmd(%p)\n",
@@ -2696,8 +2694,7 @@ _scsih_dev_reset(struct scsi_cmnd *scmd)
 
 	r = mpt2sas_scsih_issue_tm(ioc, handle, scmd->device->channel,
 	    scmd->device->id, scmd->device->lun,
-	    MPI2_SCSITASKMGMT_TASKTYPE_LOGICAL_UNIT_RESET, 0, 30, 0,
-	    TM_MUTEX_ON);
+	    MPI2_SCSITASKMGMT_TASKTYPE_LOGICAL_UNIT_RESET, 0, 30, TM_MUTEX_ON);
 
  out:
 	sdev_printk(KERN_INFO, scmd->device, "device reset: %s scmd(%p)\n",
@@ -2757,7 +2754,7 @@ _scsih_target_reset(struct scsi_cmnd *scmd)
 
 	r = mpt2sas_scsih_issue_tm(ioc, handle, scmd->device->channel,
 	    scmd->device->id, 0, MPI2_SCSITASKMGMT_TASKTYPE_TARGET_RESET, 0,
-	    30, 0, TM_MUTEX_ON);
+	    30, TM_MUTEX_ON);
 
  out:
 	starget_printk(KERN_INFO, starget, "target reset: %s scmd(%p)\n",
@@ -3953,9 +3950,9 @@ _scsih_setup_direct_io(struct MPT2SAS_ADAPTER *ioc, struct scsi_cmnd *scmd,
  * SCSI_MLQUEUE_HOST_BUSY if the entire host queue is full
  */
 static int
-_scsih_qcmd_lck(struct scsi_cmnd *scmd, void (*done)(struct scsi_cmnd *))
+_scsih_qcmd(struct Scsi_Host *shost, struct scsi_cmnd *scmd)
 {
-	struct MPT2SAS_ADAPTER *ioc = shost_priv(scmd->device->host);
+	struct MPT2SAS_ADAPTER *ioc = shost_priv(shost);
 	struct MPT2SAS_DEVICE *sas_device_priv_data;
 	struct MPT2SAS_TARGET *sas_target_priv_data;
 	struct _raid_device *raid_device;
@@ -3963,7 +3960,6 @@ _scsih_qcmd_lck(struct scsi_cmnd *scmd, void (*done)(struct scsi_cmnd *))
 	u32 mpi_control;
 	u16 smid;
 
-	scmd->scsi_done = done;
 	sas_device_priv_data = scmd->device->hostdata;
 	if (!sas_device_priv_data || !sas_device_priv_data->sas_target) {
 		scmd->result = DID_NO_CONNECT << 16;
@@ -4039,7 +4035,7 @@ _scsih_qcmd_lck(struct scsi_cmnd *scmd, void (*done)(struct scsi_cmnd *))
 	    MPT_TARGET_FLAGS_RAID_COMPONENT)
 		mpi_request->Function = MPI2_FUNCTION_RAID_SCSI_IO_PASSTHROUGH;
 	else
-		mpi_request->Function = MPI2_FUNCTION_SCSI_IO_REQUEST;
+	mpi_request->Function = MPI2_FUNCTION_SCSI_IO_REQUEST;
 	mpi_request->DevHandle =
 	    cpu_to_le16(sas_device_priv_data->sas_target->handle);
 	mpi_request->DataLength = cpu_to_le32(scsi_bufflen(scmd));
@@ -4083,8 +4079,6 @@ _scsih_qcmd_lck(struct scsi_cmnd *scmd, void (*done)(struct scsi_cmnd *))
 	return SCSI_MLQUEUE_HOST_BUSY;
 }
 
-static DEF_SCSI_QCMD(_scsih_qcmd)
-
 /**
  * _scsih_normalize_sense - normalize descriptor and fixed format sense data
  * @sense_buffer: sense data returned by target
@@ -5880,7 +5874,7 @@ broadcast_aen_retry:
 
 		spin_unlock_irqrestore(&ioc->scsi_lookup_lock, flags);
 		r = mpt2sas_scsih_issue_tm(ioc, handle, 0, 0, lun,
-		    MPI2_SCSITASKMGMT_TASKTYPE_QUERY_TASK, smid, 30, 0,
+		    MPI2_SCSITASKMGMT_TASKTYPE_QUERY_TASK, smid, 30,
 		    TM_MUTEX_OFF);
 		if (r == FAILED) {
 			sdev_printk(KERN_WARNING, sdev,
@@ -5922,7 +5916,7 @@ broadcast_aen_retry:
 
 		r = mpt2sas_scsih_issue_tm(ioc, handle, sdev->channel, sdev->id,
 		    sdev->lun, MPI2_SCSITASKMGMT_TASKTYPE_ABORT_TASK, smid, 30,
-		    scmd->serial_number, TM_MUTEX_OFF);
+		    TM_MUTEX_OFF);
 		if (r == FAILED) {
 			sdev_printk(KERN_WARNING, sdev,
 			    "mpt2sas_scsih_issue_tm: ABORT_TASK: FAILED : "
diff --git a/drivers/scsi/mpt3sas/mpt3sas_base.h b/drivers/scsi/mpt3sas/mpt3sas_base.h
index 0ebf5d913c8..9b90a6fef70 100644
--- a/drivers/scsi/mpt3sas/mpt3sas_base.h
+++ b/drivers/scsi/mpt3sas/mpt3sas_base.h
@@ -993,7 +993,7 @@ void mpt3sas_scsih_reset_handler(struct MPT3SAS_ADAPTER *ioc, int reset_phase);
 
 int mpt3sas_scsih_issue_tm(struct MPT3SAS_ADAPTER *ioc, u16 handle,
 	uint channel, uint id, uint lun, u8 type, u16 smid_task,
-	ulong timeout, unsigned long serial_number,  enum mutex_type m_type);
+	ulong timeout, enum mutex_type m_type);
 void mpt3sas_scsih_set_tm_flag(struct MPT3SAS_ADAPTER *ioc, u16 handle);
 void mpt3sas_scsih_clear_tm_flag(struct MPT3SAS_ADAPTER *ioc, u16 handle);
 void mpt3sas_expander_remove(struct MPT3SAS_ADAPTER *ioc, u64 sas_address);
diff --git a/drivers/scsi/mpt3sas/mpt3sas_ctl.c b/drivers/scsi/mpt3sas/mpt3sas_ctl.c
index 9b89de14a0a..ba9cbe598a9 100644
--- a/drivers/scsi/mpt3sas/mpt3sas_ctl.c
+++ b/drivers/scsi/mpt3sas/mpt3sas_ctl.c
@@ -980,7 +980,7 @@ _ctl_do_mpt_command(struct MPT3SAS_ADAPTER *ioc, struct mpt3_ioctl_command karg,
 			mpt3sas_scsih_issue_tm(ioc,
 			    le16_to_cpu(mpi_request->FunctionDependent1), 0, 0,
 			    0, MPI2_SCSITASKMGMT_TASKTYPE_TARGET_RESET, 0, 30,
-			    0, TM_MUTEX_ON);
+			    TM_MUTEX_ON);
 		} else
 			mpt3sas_base_hard_reset_handler(ioc, CAN_SLEEP,
 			    FORCE_BIG_HAMMER);
diff --git a/drivers/scsi/mpt3sas/mpt3sas_scsih.c b/drivers/scsi/mpt3sas/mpt3sas_scsih.c
index a961fe11b52..18e713db1d3 100644
--- a/drivers/scsi/mpt3sas/mpt3sas_scsih.c
+++ b/drivers/scsi/mpt3sas/mpt3sas_scsih.c
@@ -2029,7 +2029,6 @@ mpt3sas_scsih_clear_tm_flag(struct MPT3SAS_ADAPTER *ioc, u16 handle)
  * @type: MPI2_SCSITASKMGMT_TASKTYPE__XXX (defined in mpi2_init.h)
  * @smid_task: smid assigned to the task
  * @timeout: timeout in seconds
- * @serial_number: the serial_number from scmd
  * @m_type: TM_MUTEX_ON or TM_MUTEX_OFF
  * Context: user
  *
@@ -2042,7 +2041,7 @@ mpt3sas_scsih_clear_tm_flag(struct MPT3SAS_ADAPTER *ioc, u16 handle)
 int
 mpt3sas_scsih_issue_tm(struct MPT3SAS_ADAPTER *ioc, u16 handle, uint channel,
 	uint id, uint lun, u8 type, u16 smid_task, ulong timeout,
-	unsigned long serial_number, enum mutex_type m_type)
+	enum mutex_type m_type)
 {
 	Mpi2SCSITaskManagementRequest_t *mpi_request;
 	Mpi2SCSITaskManagementReply_t *mpi_reply;
@@ -2293,8 +2292,7 @@ _scsih_abort(struct scsi_cmnd *scmd)
 	handle = sas_device_priv_data->sas_target->handle;
 	r = mpt3sas_scsih_issue_tm(ioc, handle, scmd->device->channel,
 	    scmd->device->id, scmd->device->lun,
-	    MPI2_SCSITASKMGMT_TASKTYPE_ABORT_TASK, smid, 30,
-	    scmd->serial_number, TM_MUTEX_ON);
+	    MPI2_SCSITASKMGMT_TASKTYPE_ABORT_TASK, smid, 30, TM_MUTEX_ON);
 
  out:
 	sdev_printk(KERN_INFO, scmd->device, "task abort: %s scmd(%p)\n",
@@ -2353,8 +2351,7 @@ _scsih_dev_reset(struct scsi_cmnd *scmd)
 
 	r = mpt3sas_scsih_issue_tm(ioc, handle, scmd->device->channel,
 	    scmd->device->id, scmd->device->lun,
-	    MPI2_SCSITASKMGMT_TASKTYPE_LOGICAL_UNIT_RESET, 0, 30, 0,
-	    TM_MUTEX_ON);
+	    MPI2_SCSITASKMGMT_TASKTYPE_LOGICAL_UNIT_RESET, 0, 30, TM_MUTEX_ON);
 
  out:
 	sdev_printk(KERN_INFO, scmd->device, "device reset: %s scmd(%p)\n",
@@ -2414,7 +2411,7 @@ _scsih_target_reset(struct scsi_cmnd *scmd)
 
 	r = mpt3sas_scsih_issue_tm(ioc, handle, scmd->device->channel,
 	    scmd->device->id, 0, MPI2_SCSITASKMGMT_TASKTYPE_TARGET_RESET, 0,
-	    30, 0, TM_MUTEX_ON);
+	    30, TM_MUTEX_ON);
 
  out:
 	starget_printk(KERN_INFO, starget, "target reset: %s scmd(%p)\n",
@@ -3518,7 +3515,7 @@ _scsih_eedp_error_handling(struct scsi_cmnd *scmd, u16 ioc_status)
 
 
 /**
- * _scsih_qcmd_lck - main scsi request entry point
+ * _scsih_qcmd - main scsi request entry point
  * @scmd: pointer to scsi command object
  * @done: function pointer to be invoked on completion
  *
@@ -3529,9 +3526,9 @@ _scsih_eedp_error_handling(struct scsi_cmnd *scmd, u16 ioc_status)
  * SCSI_MLQUEUE_HOST_BUSY if the entire host queue is full
  */
 static int
-_scsih_qcmd_lck(struct scsi_cmnd *scmd, void (*done)(struct scsi_cmnd *))
+_scsih_qcmd(struct Scsi_Host *shost, struct scsi_cmnd *scmd)
 {
-	struct MPT3SAS_ADAPTER *ioc = shost_priv(scmd->device->host);
+	struct MPT3SAS_ADAPTER *ioc = shost_priv(shost);
 	struct MPT3SAS_DEVICE *sas_device_priv_data;
 	struct MPT3SAS_TARGET *sas_target_priv_data;
 	Mpi2SCSIIORequest_t *mpi_request;
@@ -3544,7 +3541,6 @@ _scsih_qcmd_lck(struct scsi_cmnd *scmd, void (*done)(struct scsi_cmnd *))
 		scsi_print_command(scmd);
 #endif
 
-	scmd->scsi_done = done;
 	sas_device_priv_data = scmd->device->hostdata;
 	if (!sas_device_priv_data || !sas_device_priv_data->sas_target) {
 		scmd->result = DID_NO_CONNECT << 16;
@@ -3659,8 +3655,6 @@ _scsih_qcmd_lck(struct scsi_cmnd *scmd, void (*done)(struct scsi_cmnd *))
  out:
 	return SCSI_MLQUEUE_HOST_BUSY;
 }
-static DEF_SCSI_QCMD(_scsih_qcmd)
-
 
 /**
  * _scsih_normalize_sense - normalize descriptor and fixed format sense data
@@ -5425,7 +5419,7 @@ _scsih_sas_broadcast_primitive_event(struct MPT3SAS_ADAPTER *ioc,
 
 		spin_unlock_irqrestore(&ioc->scsi_lookup_lock, flags);
 		r = mpt3sas_scsih_issue_tm(ioc, handle, 0, 0, lun,
-		    MPI2_SCSITASKMGMT_TASKTYPE_QUERY_TASK, smid, 30, 0,
+		    MPI2_SCSITASKMGMT_TASKTYPE_QUERY_TASK, smid, 30,
 		    TM_MUTEX_OFF);
 		if (r == FAILED) {
 			sdev_printk(KERN_WARNING, sdev,
@@ -5467,7 +5461,7 @@ _scsih_sas_broadcast_primitive_event(struct MPT3SAS_ADAPTER *ioc,
 
 		r = mpt3sas_scsih_issue_tm(ioc, handle, sdev->channel, sdev->id,
 		    sdev->lun, MPI2_SCSITASKMGMT_TASKTYPE_ABORT_TASK, smid, 30,
-		    scmd->serial_number, TM_MUTEX_OFF);
+		    TM_MUTEX_OFF);
 		if (r == FAILED) {
 			sdev_printk(KERN_WARNING, sdev,
 			    "mpt3sas_scsih_issue_tm: ABORT_TASK: FAILED : "
diff --git a/drivers/scsi/mvsas/mv_init.c b/drivers/scsi/mvsas/mv_init.c
index 5ff978be249..eacee48a955 100644
--- a/drivers/scsi/mvsas/mv_init.c
+++ b/drivers/scsi/mvsas/mv_init.c
@@ -728,6 +728,15 @@ static struct pci_device_id mvs_pci_table[] = {
 		.class_mask	= 0,
 		.driver_data	= chip_9485,
 	},
+	{
+		.vendor		= PCI_VENDOR_ID_MARVELL_EXT,
+		.device		= 0x9485,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= 0x9485,
+		.class		= 0,
+		.class_mask	= 0,
+		.driver_data	= chip_9485,
+	},
 	{ PCI_VDEVICE(OCZ, 0x1021), chip_9485}, /* OCZ RevoDrive3 */
 	{ PCI_VDEVICE(OCZ, 0x1022), chip_9485}, /* OCZ RevoDrive3/zDriveR4 (exact model unknown) */
 	{ PCI_VDEVICE(OCZ, 0x1040), chip_9485}, /* OCZ RevoDrive3/zDriveR4 (exact model unknown) */
diff --git a/drivers/scsi/osd/osd_initiator.c b/drivers/scsi/osd/osd_initiator.c
index bac04c2335a..5f4cbf0c475 100644
--- a/drivers/scsi/osd/osd_initiator.c
+++ b/drivers/scsi/osd/osd_initiator.c
@@ -1570,6 +1570,7 @@ static struct request *_make_request(struct request_queue *q, bool has_write,
 		if (unlikely(!req))
 			return ERR_PTR(-ENOMEM);
 
+		blk_rq_set_block_pc(req);
 		return req;
 	}
 }
@@ -1590,7 +1591,6 @@ static int _init_blk_request(struct osd_request *or,
 	}
 
 	or->request = req;
-	req->cmd_type = REQ_TYPE_BLOCK_PC;
 	req->cmd_flags |= REQ_QUIET;
 
 	req->timeout = or->timeout;
@@ -1608,7 +1608,7 @@ static int _init_blk_request(struct osd_request *or,
 				ret = PTR_ERR(req);
 				goto out;
 			}
-			req->cmd_type = REQ_TYPE_BLOCK_PC;
+			blk_rq_set_block_pc(req);
 			or->in.req = or->request->next_rq = req;
 		}
 	} else if (has_in)
diff --git a/drivers/scsi/osst.c b/drivers/scsi/osst.c
index 21883a2d632..0727ea7cc38 100644
--- a/drivers/scsi/osst.c
+++ b/drivers/scsi/osst.c
@@ -365,7 +365,7 @@ static int osst_execute(struct osst_request *SRpnt, const unsigned char *cmd,
 	if (!req)
 		return DRIVER_ERROR << 24;
 
-	req->cmd_type = REQ_TYPE_BLOCK_PC;
+	blk_rq_set_block_pc(req);
 	req->cmd_flags |= REQ_QUIET;
 
 	SRpnt->bio = NULL;
diff --git a/drivers/scsi/pas16.h b/drivers/scsi/pas16.h
index 3721342835e..aa528f53c53 100644
--- a/drivers/scsi/pas16.h
+++ b/drivers/scsi/pas16.h
@@ -129,8 +129,6 @@ static int pas16_bus_reset(Scsi_Cmnd *);
 #define CAN_QUEUE 32 
 #endif
 
-#ifndef HOSTS_C
-
 #define NCR5380_implementation_fields \
     volatile unsigned short io_port
 
@@ -171,6 +169,5 @@ static int pas16_bus_reset(Scsi_Cmnd *);
    
 #define PAS16_IRQS 0xd4a8 
 
-#endif /* else def HOSTS_C */
 #endif /* ndef ASM */
 #endif /* PAS16_H */
diff --git a/drivers/scsi/pm8001/pm8001_ctl.c b/drivers/scsi/pm8001/pm8001_ctl.c
index 28b4e813915..fe5eee4d0a1 100644
--- a/drivers/scsi/pm8001/pm8001_ctl.c
+++ b/drivers/scsi/pm8001/pm8001_ctl.c
@@ -395,6 +395,8 @@ static ssize_t pm8001_ctl_bios_version_show(struct device *cdev,
 	payload.offset = 0;
 	payload.length = 4096;
 	payload.func_specific = kzalloc(4096, GFP_KERNEL);
+	if (!payload.func_specific)
+		return -ENOMEM;
 	PM8001_CHIP_DISP->get_nvmd_req(pm8001_ha, &payload);
 	wait_for_completion(&completion);
 	virt_addr = pm8001_ha->memoryMap.region[NVMD].virt_ptr;
@@ -402,6 +404,7 @@ static ssize_t pm8001_ctl_bios_version_show(struct device *cdev,
 		bios_index++)
 		str += sprintf(str, "%c",
 			*((u8 *)((u8 *)virt_addr+bios_index)));
+	kfree(payload.func_specific);
 	return str - buf;
 }
 static DEVICE_ATTR(bios_version, S_IRUGO, pm8001_ctl_bios_version_show, NULL);
diff --git a/drivers/scsi/qla2xxx/qla_attr.c b/drivers/scsi/qla2xxx/qla_attr.c
index 07befcf365b..16fe5196e6d 100644
--- a/drivers/scsi/qla2xxx/qla_attr.c
+++ b/drivers/scsi/qla2xxx/qla_attr.c
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
@@ -664,7 +664,7 @@ do_read:
 		}
 
 		rval = qla2x00_read_sfp(vha, ha->sfp_data_dma, ha->sfp_data,
-		    addr, offset, SFP_BLOCK_SIZE, 0);
+		    addr, offset, SFP_BLOCK_SIZE, BIT_1);
 		if (rval != QLA_SUCCESS) {
 			ql_log(ql_log_warn, vha, 0x706d,
 			    "Unable to read SFP data (%x/%x/%x).\n", rval,
@@ -1495,7 +1495,7 @@ qla2x00_fw_dump_size_show(struct device *dev, struct device_attribute *attr,
 
 	if (!ha->fw_dumped)
 		size = 0;
-	else if (IS_QLA82XX(ha))
+	else if (IS_P3P_TYPE(ha))
 		size = ha->md_template_size + ha->md_dump_size;
 	else
 		size = ha->fw_dump_len;
diff --git a/drivers/scsi/qla2xxx/qla_bsg.c b/drivers/scsi/qla2xxx/qla_bsg.c
index 71ff340f6de..524f9eb7fcd 100644
--- a/drivers/scsi/qla2xxx/qla_bsg.c
+++ b/drivers/scsi/qla2xxx/qla_bsg.c
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2012 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
@@ -2054,9 +2054,49 @@ qla26xx_serdes_op(struct fc_bsg_job *bsg_job)
 		bsg_job->reply->reply_payload_rcv_len = sizeof(sr);
 		break;
 	default:
-		ql_log(ql_log_warn, vha, 0x708c,
+		ql_dbg(ql_dbg_user, vha, 0x708c,
 		    "Unknown serdes cmd %x.\n", sr.cmd);
-		rval = -EDOM;
+		rval = -EINVAL;
+		break;
+	}
+
+	bsg_job->reply->reply_data.vendor_reply.vendor_rsp[0] =
+	    rval ? EXT_STATUS_MAILBOX : 0;
+
+	bsg_job->reply_len = sizeof(struct fc_bsg_reply);
+	bsg_job->reply->result = DID_OK << 16;
+	bsg_job->job_done(bsg_job);
+	return 0;
+}
+
+static int
+qla8044_serdes_op(struct fc_bsg_job *bsg_job)
+{
+	struct Scsi_Host *host = bsg_job->shost;
+	scsi_qla_host_t *vha = shost_priv(host);
+	int rval = 0;
+	struct qla_serdes_reg_ex sr;
+
+	memset(&sr, 0, sizeof(sr));
+
+	sg_copy_to_buffer(bsg_job->request_payload.sg_list,
+	    bsg_job->request_payload.sg_cnt, &sr, sizeof(sr));
+
+	switch (sr.cmd) {
+	case INT_SC_SERDES_WRITE_REG:
+		rval = qla8044_write_serdes_word(vha, sr.addr, sr.val);
+		bsg_job->reply->reply_payload_rcv_len = 0;
+		break;
+	case INT_SC_SERDES_READ_REG:
+		rval = qla8044_read_serdes_word(vha, sr.addr, &sr.val);
+		sg_copy_from_buffer(bsg_job->reply_payload.sg_list,
+		    bsg_job->reply_payload.sg_cnt, &sr, sizeof(sr));
+		bsg_job->reply->reply_payload_rcv_len = sizeof(sr);
+		break;
+	default:
+		ql_dbg(ql_dbg_user, vha, 0x70cf,
+		    "Unknown serdes cmd %x.\n", sr.cmd);
+		rval = -EINVAL;
 		break;
 	}
 
@@ -2121,6 +2161,9 @@ qla2x00_process_vendor_specific(struct fc_bsg_job *bsg_job)
 	case QL_VND_SERDES_OP:
 		return qla26xx_serdes_op(bsg_job);
 
+	case QL_VND_SERDES_OP_EX:
+		return qla8044_serdes_op(bsg_job);
+
 	default:
 		return -ENOSYS;
 	}
diff --git a/drivers/scsi/qla2xxx/qla_bsg.h b/drivers/scsi/qla2xxx/qla_bsg.h
index e5c2126221e..d38f9efa56f 100644
--- a/drivers/scsi/qla2xxx/qla_bsg.h
+++ b/drivers/scsi/qla2xxx/qla_bsg.h
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
@@ -24,6 +24,7 @@
 #define QL_VND_READ_I2C		0x11
 #define QL_VND_FX00_MGMT_CMD	0x12
 #define QL_VND_SERDES_OP	0x13
+#define	QL_VND_SERDES_OP_EX	0x14
 
 /* BSG Vendor specific subcode returns */
 #define EXT_STATUS_OK			0
@@ -225,4 +226,10 @@ struct qla_serdes_reg {
 	uint16_t val;
 } __packed;
 
+struct qla_serdes_reg_ex {
+	uint16_t cmd;
+	uint32_t addr;
+	uint32_t val;
+} __packed;
+
 #endif
diff --git a/drivers/scsi/qla2xxx/qla_dbg.c b/drivers/scsi/qla2xxx/qla_dbg.c
index 97255f7c397..c72ee97bf3f 100644
--- a/drivers/scsi/qla2xxx/qla_dbg.c
+++ b/drivers/scsi/qla2xxx/qla_dbg.c
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
@@ -15,7 +15,7 @@
  * |                              |                    | 0x0144,0x0146	|
  * |                              |                    | 0x015b-0x0160	|
  * |                              |                    | 0x016e-0x0170	|
- * | Mailbox commands             |       0x1187       | 0x1018-0x1019	|
+ * | Mailbox commands             |       0x118d       | 0x1018-0x1019	|
  * |                              |                    | 0x10ca         |
  * |                              |                    | 0x1115-0x1116  |
  * |                              |                    | 0x111a-0x111b	|
@@ -45,12 +45,16 @@
  * |                              |                    | 0x70ad-0x70ae  |
  * |                              |                    | 0x70d7-0x70db  |
  * |                              |                    | 0x70de-0x70df  |
- * | Task Management              |       0x803d       | 0x8025-0x8026  |
- * |                              |                    | 0x800b,0x8039  |
+ * | Task Management              |       0x803d       | 0x8000,0x800b  |
+ * |                              |                    | 0x8019         |
+ * |                              |                    | 0x8025,0x8026  |
+ * |                              |                    | 0x8031,0x8032  |
+ * |                              |                    | 0x8039,0x803c  |
  * | AER/EEH                      |       0x9011       |		|
  * | Virtual Port                 |       0xa007       |		|
- * | ISP82XX Specific             |       0xb14c       | 0xb002,0xb024  |
+ * | ISP82XX Specific             |       0xb157       | 0xb002,0xb024  |
  * |                              |                    | 0xb09e,0xb0ae  |
+ * |				  |		       | 0xb0c3,0xb0c6  |
  * |                              |                    | 0xb0e0-0xb0ef  |
  * |                              |                    | 0xb085,0xb0dc  |
  * |                              |                    | 0xb107,0xb108  |
@@ -60,12 +64,12 @@
  * |                              |                    | 0xb13c-0xb140  |
  * |                              |                    | 0xb149		|
  * | MultiQ                       |       0xc00c       |		|
- * | Misc                         |       0xd2ff       | 0xd017-0xd019	|
+ * | Misc                         |       0xd212       | 0xd017-0xd019	|
  * |                              |                    | 0xd020		|
- * |                              |                    | 0xd02e-0xd0ff	|
+ * |                              |                    | 0xd030-0xd0ff	|
  * |                              |                    | 0xd101-0xd1fe	|
- * |                              |                    | 0xd212-0xd2fe	|
- * | Target Mode		  |	  0xe070       | 0xe021		|
+ * |                              |                    | 0xd213-0xd2fe	|
+ * | Target Mode		  |	  0xe078       |		|
  * | Target Mode Management	  |	  0xf072       | 0xf002-0xf003	|
  * |                              |                    | 0xf046-0xf049  |
  * | Target Mode Task Management  |	  0x1000b      |		|
@@ -277,9 +281,15 @@ qla24xx_dump_memory(struct qla_hw_data *ha, uint32_t *code_ram,
 	if (rval != QLA_SUCCESS)
 		return rval;
 
+	set_bit(RISC_SRAM_DUMP_CMPL, &ha->fw_dump_cap_flags);
+
 	/* External Memory. */
-	return qla24xx_dump_ram(ha, 0x100000, *nxt,
+	rval = qla24xx_dump_ram(ha, 0x100000, *nxt,
 	    ha->fw_memory_size - 0x100000 + 1, nxt);
+	if (rval == QLA_SUCCESS)
+		set_bit(RISC_EXT_MEM_DUMP_CMPL, &ha->fw_dump_cap_flags);
+
+	return rval;
 }
 
 static uint32_t *
@@ -296,23 +306,15 @@ qla24xx_read_window(struct device_reg_24xx __iomem *reg, uint32_t iobase,
 	return buf;
 }
 
-int
-qla24xx_pause_risc(struct device_reg_24xx __iomem *reg)
+void
+qla24xx_pause_risc(struct device_reg_24xx __iomem *reg, struct qla_hw_data *ha)
 {
-	int rval = QLA_SUCCESS;
-	uint32_t cnt;
-
 	WRT_REG_DWORD(&reg->hccr, HCCRX_SET_RISC_PAUSE);
-	for (cnt = 30000;
-	    ((RD_REG_DWORD(&reg->host_status) & HSRX_RISC_PAUSED) == 0) &&
-	    rval == QLA_SUCCESS; cnt--) {
-		if (cnt)
-			udelay(100);
-		else
-			rval = QLA_FUNCTION_TIMEOUT;
-	}
 
-	return rval;
+	/* 100 usec delay is sufficient enough for hardware to pause RISC */
+	udelay(100);
+	if (RD_REG_DWORD(&reg->host_status) & HSRX_RISC_PAUSED)
+		set_bit(RISC_PAUSE_CMPL, &ha->fw_dump_cap_flags);
 }
 
 int
@@ -320,10 +322,14 @@ qla24xx_soft_reset(struct qla_hw_data *ha)
 {
 	int rval = QLA_SUCCESS;
 	uint32_t cnt;
-	uint16_t mb0, wd;
+	uint16_t wd;
 	struct device_reg_24xx __iomem *reg = &ha->iobase->isp24;
 
-	/* Reset RISC. */
+	/*
+	 * Reset RISC. The delay is dependent on system architecture.
+	 * Driver can proceed with the reset sequence after waiting
+	 * for a timeout period.
+	 */
 	WRT_REG_DWORD(&reg->ctrl_status, CSRX_DMA_SHUTDOWN|MWB_4096_BYTES);
 	for (cnt = 0; cnt < 30000; cnt++) {
 		if ((RD_REG_DWORD(&reg->ctrl_status) & CSRX_DMA_ACTIVE) == 0)
@@ -331,19 +337,14 @@ qla24xx_soft_reset(struct qla_hw_data *ha)
 
 		udelay(10);
 	}
+	if (!(RD_REG_DWORD(&reg->ctrl_status) & CSRX_DMA_ACTIVE))
+		set_bit(DMA_SHUTDOWN_CMPL, &ha->fw_dump_cap_flags);
 
 	WRT_REG_DWORD(&reg->ctrl_status,
 	    CSRX_ISP_SOFT_RESET|CSRX_DMA_SHUTDOWN|MWB_4096_BYTES);
 	pci_read_config_word(ha->pdev, PCI_COMMAND, &wd);
 
 	udelay(100);
-	/* Wait for firmware to complete NVRAM accesses. */
-	mb0 = (uint32_t) RD_REG_WORD(&reg->mailbox0);
-	for (cnt = 10000 ; cnt && mb0; cnt--) {
-		udelay(5);
-		mb0 = (uint32_t) RD_REG_WORD(&reg->mailbox0);
-		barrier();
-	}
 
 	/* Wait for soft-reset to complete. */
 	for (cnt = 0; cnt < 30000; cnt++) {
@@ -353,16 +354,21 @@ qla24xx_soft_reset(struct qla_hw_data *ha)
 
 		udelay(10);
 	}
+	if (!(RD_REG_DWORD(&reg->ctrl_status) & CSRX_ISP_SOFT_RESET))
+		set_bit(ISP_RESET_CMPL, &ha->fw_dump_cap_flags);
+
 	WRT_REG_DWORD(&reg->hccr, HCCRX_CLR_RISC_RESET);
 	RD_REG_DWORD(&reg->hccr);             /* PCI Posting. */
 
-	for (cnt = 30000; RD_REG_WORD(&reg->mailbox0) != 0 &&
+	for (cnt = 10000; RD_REG_WORD(&reg->mailbox0) != 0 &&
 	    rval == QLA_SUCCESS; cnt--) {
 		if (cnt)
-			udelay(100);
+			udelay(10);
 		else
 			rval = QLA_FUNCTION_TIMEOUT;
 	}
+	if (rval == QLA_SUCCESS)
+		set_bit(RISC_RDY_AFT_RESET, &ha->fw_dump_cap_flags);
 
 	return rval;
 }
@@ -659,12 +665,13 @@ qla2xxx_dump_post_process(scsi_qla_host_t *vha, int rval)
 
 	if (rval != QLA_SUCCESS) {
 		ql_log(ql_log_warn, vha, 0xd000,
-		    "Failed to dump firmware (%x).\n", rval);
+		    "Failed to dump firmware (%x), dump status flags (0x%lx).\n",
+		    rval, ha->fw_dump_cap_flags);
 		ha->fw_dumped = 0;
 	} else {
 		ql_log(ql_log_info, vha, 0xd001,
-		    "Firmware dump saved to temp buffer (%ld/%p).\n",
-		    vha->host_no, ha->fw_dump);
+		    "Firmware dump saved to temp buffer (%ld/%p), dump status flags (0x%lx).\n",
+		    vha->host_no, ha->fw_dump, ha->fw_dump_cap_flags);
 		ha->fw_dumped = 1;
 		qla2x00_post_uevent_work(vha, QLA_UEVENT_CODE_FW_DUMP);
 	}
@@ -1053,6 +1060,7 @@ qla24xx_fw_dump(scsi_qla_host_t *vha, int hardware_locked)
 
 	risc_address = ext_mem_cnt = 0;
 	flags = 0;
+	ha->fw_dump_cap_flags = 0;
 
 	if (!hardware_locked)
 		spin_lock_irqsave(&ha->hardware_lock, flags);
@@ -1075,10 +1083,11 @@ qla24xx_fw_dump(scsi_qla_host_t *vha, int hardware_locked)
 
 	fw->host_status = htonl(RD_REG_DWORD(&reg->host_status));
 
-	/* Pause RISC. */
-	rval = qla24xx_pause_risc(reg);
-	if (rval != QLA_SUCCESS)
-		goto qla24xx_fw_dump_failed_0;
+	/*
+	 * Pause RISC. No need to track timeout, as resetting the chip
+	 * is the right approach incase of pause timeout
+	 */
+	qla24xx_pause_risc(reg, ha);
 
 	/* Host interface registers. */
 	dmp_reg = &reg->flash_addr;
@@ -1302,6 +1311,7 @@ qla25xx_fw_dump(scsi_qla_host_t *vha, int hardware_locked)
 
 	risc_address = ext_mem_cnt = 0;
 	flags = 0;
+	ha->fw_dump_cap_flags = 0;
 
 	if (!hardware_locked)
 		spin_lock_irqsave(&ha->hardware_lock, flags);
@@ -1325,10 +1335,11 @@ qla25xx_fw_dump(scsi_qla_host_t *vha, int hardware_locked)
 
 	fw->host_status = htonl(RD_REG_DWORD(&reg->host_status));
 
-	/* Pause RISC. */
-	rval = qla24xx_pause_risc(reg);
-	if (rval != QLA_SUCCESS)
-		goto qla25xx_fw_dump_failed_0;
+	/*
+	 * Pause RISC. No need to track timeout, as resetting the chip
+	 * is the right approach incase of pause timeout
+	 */
+	qla24xx_pause_risc(reg, ha);
 
 	/* Host/Risc registers. */
 	iter_reg = fw->host_risc_reg;
@@ -1619,6 +1630,7 @@ qla81xx_fw_dump(scsi_qla_host_t *vha, int hardware_locked)
 
 	risc_address = ext_mem_cnt = 0;
 	flags = 0;
+	ha->fw_dump_cap_flags = 0;
 
 	if (!hardware_locked)
 		spin_lock_irqsave(&ha->hardware_lock, flags);
@@ -1641,10 +1653,11 @@ qla81xx_fw_dump(scsi_qla_host_t *vha, int hardware_locked)
 
 	fw->host_status = htonl(RD_REG_DWORD(&reg->host_status));
 
-	/* Pause RISC. */
-	rval = qla24xx_pause_risc(reg);
-	if (rval != QLA_SUCCESS)
-		goto qla81xx_fw_dump_failed_0;
+	/*
+	 * Pause RISC. No need to track timeout, as resetting the chip
+	 * is the right approach incase of pause timeout
+	 */
+	qla24xx_pause_risc(reg, ha);
 
 	/* Host/Risc registers. */
 	iter_reg = fw->host_risc_reg;
@@ -1938,6 +1951,7 @@ qla83xx_fw_dump(scsi_qla_host_t *vha, int hardware_locked)
 
 	risc_address = ext_mem_cnt = 0;
 	flags = 0;
+	ha->fw_dump_cap_flags = 0;
 
 	if (!hardware_locked)
 		spin_lock_irqsave(&ha->hardware_lock, flags);
@@ -1959,10 +1973,11 @@ qla83xx_fw_dump(scsi_qla_host_t *vha, int hardware_locked)
 
 	fw->host_status = htonl(RD_REG_DWORD(&reg->host_status));
 
-	/* Pause RISC. */
-	rval = qla24xx_pause_risc(reg);
-	if (rval != QLA_SUCCESS)
-		goto qla83xx_fw_dump_failed_0;
+	/*
+	 * Pause RISC. No need to track timeout, as resetting the chip
+	 * is the right approach incase of pause timeout
+	 */
+	qla24xx_pause_risc(reg, ha);
 
 	WRT_REG_DWORD(&reg->iobase_addr, 0x6000);
 	dmp_reg = &reg->iobase_window;
@@ -2385,9 +2400,11 @@ qla83xx_fw_dump(scsi_qla_host_t *vha, int hardware_locked)
 			nxt += sizeof(fw->code_ram);
 			nxt += (ha->fw_memory_size - 0x100000 + 1);
 			goto copy_queue;
-		} else
+		} else {
+			set_bit(RISC_RDY_AFT_RESET, &ha->fw_dump_cap_flags);
 			ql_log(ql_log_warn, vha, 0xd010,
 			    "bigger hammer success?\n");
+		}
 	}
 
 	rval = qla24xx_dump_memory(ha, fw->code_ram, sizeof(fw->code_ram),
diff --git a/drivers/scsi/qla2xxx/qla_dbg.h b/drivers/scsi/qla2xxx/qla_dbg.h
index cc961040f8b..e1fc4e66966 100644
--- a/drivers/scsi/qla2xxx/qla_dbg.h
+++ b/drivers/scsi/qla2xxx/qla_dbg.h
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
@@ -353,5 +353,6 @@ extern int qla27xx_dump_mpi_ram(struct qla_hw_data *, uint32_t, uint32_t *,
 	uint32_t, void **);
 extern int qla24xx_dump_ram(struct qla_hw_data *, uint32_t, uint32_t *,
 	uint32_t, void **);
-extern int qla24xx_pause_risc(struct device_reg_24xx __iomem *);
+extern void qla24xx_pause_risc(struct device_reg_24xx __iomem *,
+	struct qla_hw_data *);
 extern int qla24xx_soft_reset(struct qla_hw_data *);
diff --git a/drivers/scsi/qla2xxx/qla_def.h b/drivers/scsi/qla2xxx/qla_def.h
index 6a106136716..1fa01044866 100644
--- a/drivers/scsi/qla2xxx/qla_def.h
+++ b/drivers/scsi/qla2xxx/qla_def.h
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
@@ -965,6 +965,13 @@ struct mbx_cmd_32 {
  */
 #define MBC_WRITE_MPI_REGISTER		0x01    /* Write MPI Register. */
 
+/*
+ * ISP8044 mailbox commands
+ */
+#define MBC_SET_GET_ETH_SERDES_REG	0x150
+#define HCS_WRITE_SERDES		0x3
+#define HCS_READ_SERDES			0x4
+
 /* Firmware return data sizes */
 #define FCAL_MAP_SIZE	128
 
@@ -1622,10 +1629,20 @@ typedef struct {
 #define PO_MODE_DIF_PASS	2
 #define PO_MODE_DIF_REPLACE	3
 #define PO_MODE_DIF_TCP_CKSUM	6
-#define PO_ENABLE_DIF_BUNDLING	BIT_8
 #define PO_ENABLE_INCR_GUARD_SEED	BIT_3
-#define PO_DISABLE_INCR_REF_TAG	BIT_5
 #define PO_DISABLE_GUARD_CHECK	BIT_4
+#define PO_DISABLE_INCR_REF_TAG	BIT_5
+#define PO_DIS_HEADER_MODE	BIT_7
+#define PO_ENABLE_DIF_BUNDLING	BIT_8
+#define PO_DIS_FRAME_MODE	BIT_9
+#define PO_DIS_VALD_APP_ESC	BIT_10 /* Dis validation for escape tag/ffffh */
+#define PO_DIS_VALD_APP_REF_ESC BIT_11
+
+#define PO_DIS_APP_TAG_REPL	BIT_12 /* disable REG Tag replacement */
+#define PO_DIS_REF_TAG_REPL	BIT_13
+#define PO_DIS_APP_TAG_VALD	BIT_14 /* disable REF Tag validation */
+#define PO_DIS_REF_TAG_VALD	BIT_15
+
 /*
  * ISP queue - 64-Bit addressing, continuation crc entry structure definition.
  */
@@ -1748,6 +1765,8 @@ typedef struct {
 #define CS_PORT_CONFIG_CHG	0x2A	/* Port Configuration Changed */
 #define CS_PORT_BUSY		0x2B	/* Port Busy */
 #define CS_COMPLETE_CHKCOND	0x30	/* Error? */
+#define CS_IOCB_ERROR		0x31	/* Generic error for IOCB request
+					   failure */
 #define CS_BAD_PAYLOAD		0x80	/* Driver defined */
 #define CS_UNKNOWN		0x81	/* Driver defined */
 #define CS_RETRY		0x82	/* Driver defined */
@@ -2676,6 +2695,7 @@ struct rsp_que {
 	uint32_t __iomem *rsp_q_out;
 	uint16_t  ring_index;
 	uint16_t  out_ptr;
+	uint16_t  *in_ptr;		/* queue shadow in index */
 	uint16_t  length;
 	uint16_t  options;
 	uint16_t  rid;
@@ -2702,6 +2722,7 @@ struct req_que {
 	uint32_t __iomem *req_q_out;
 	uint16_t  ring_index;
 	uint16_t  in_ptr;
+	uint16_t  *out_ptr;		/* queue shadow out index */
 	uint16_t  cnt;
 	uint16_t  length;
 	uint16_t  options;
@@ -2907,6 +2928,8 @@ struct qla_hw_data {
 #define PCI_DEVICE_ID_QLOGIC_ISP8031	0x8031
 #define PCI_DEVICE_ID_QLOGIC_ISP2031	0x2031
 #define PCI_DEVICE_ID_QLOGIC_ISP2071	0x2071
+#define PCI_DEVICE_ID_QLOGIC_ISP2271	0x2271
+
 	uint32_t	device_type;
 #define DT_ISP2100                      BIT_0
 #define DT_ISP2200                      BIT_1
@@ -2928,7 +2951,8 @@ struct qla_hw_data {
 #define DT_ISPFX00			BIT_17
 #define DT_ISP8044			BIT_18
 #define DT_ISP2071			BIT_19
-#define DT_ISP_LAST			(DT_ISP2071 << 1)
+#define DT_ISP2271			BIT_20
+#define DT_ISP_LAST			(DT_ISP2271 << 1)
 
 #define DT_T10_PI                       BIT_25
 #define DT_IIDMA                        BIT_26
@@ -2959,6 +2983,7 @@ struct qla_hw_data {
 #define IS_QLA8031(ha)	(DT_MASK(ha) & DT_ISP8031)
 #define IS_QLAFX00(ha)	(DT_MASK(ha) & DT_ISPFX00)
 #define IS_QLA2071(ha)	(DT_MASK(ha) & DT_ISP2071)
+#define IS_QLA2271(ha)	(DT_MASK(ha) & DT_ISP2271)
 
 #define IS_QLA23XX(ha)  (IS_QLA2300(ha) || IS_QLA2312(ha) || IS_QLA2322(ha) || \
 			IS_QLA6312(ha) || IS_QLA6322(ha))
@@ -2967,7 +2992,7 @@ struct qla_hw_data {
 #define IS_QLA25XX(ha)  (IS_QLA2532(ha))
 #define IS_QLA83XX(ha)	(IS_QLA2031(ha) || IS_QLA8031(ha))
 #define IS_QLA84XX(ha)  (IS_QLA8432(ha))
-#define IS_QLA27XX(ha)  (IS_QLA2071(ha))
+#define IS_QLA27XX(ha)  (IS_QLA2071(ha) || IS_QLA2271(ha))
 #define IS_QLA24XX_TYPE(ha)     (IS_QLA24XX(ha) || IS_QLA54XX(ha) || \
 				IS_QLA84XX(ha))
 #define IS_CNA_CAPABLE(ha)	(IS_QLA81XX(ha) || IS_QLA82XX(ha) || \
@@ -3006,6 +3031,7 @@ struct qla_hw_data {
     (((ha)->fw_attributes_h << 16 | (ha)->fw_attributes) & BIT_22))
 #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))
 
 	/* HBA serial number */
 	uint8_t		serial0;
@@ -3136,7 +3162,15 @@ struct qla_hw_data {
 	struct qla2xxx_fw_dump *fw_dump;
 	uint32_t	fw_dump_len;
 	int		fw_dumped;
+	unsigned long	fw_dump_cap_flags;
+#define RISC_PAUSE_CMPL		0
+#define DMA_SHUTDOWN_CMPL	1
+#define ISP_RESET_CMPL		2
+#define RISC_RDY_AFT_RESET	3
+#define RISC_SRAM_DUMP_CMPL	4
+#define RISC_EXT_MEM_DUMP_CMPL	5
 	int		fw_dump_reading;
+	int		prev_minidump_failed;
 	dma_addr_t	eft_dma;
 	void		*eft;
 /* Current size of mctp dump is 0x086064 bytes */
diff --git a/drivers/scsi/qla2xxx/qla_dfs.c b/drivers/scsi/qla2xxx/qla_dfs.c
index 32ab8095768..2ca39b8e716 100644
--- a/drivers/scsi/qla2xxx/qla_dfs.c
+++ b/drivers/scsi/qla2xxx/qla_dfs.c
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
diff --git a/drivers/scsi/qla2xxx/qla_fw.h b/drivers/scsi/qla2xxx/qla_fw.h
index 3a7353eaccb..eb8f57249f1 100644
--- a/drivers/scsi/qla2xxx/qla_fw.h
+++ b/drivers/scsi/qla2xxx/qla_fw.h
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
@@ -371,7 +371,10 @@ struct init_cb_24xx {
 	 * BIT 14 = Data Rate bit 1
 	 * BIT 15 = Data Rate bit 2
 	 * BIT 16 = Enable 75 ohm Termination Select
-	 * BIT 17-31 = Reserved
+	 * BIT 17-28 = Reserved
+	 * BIT 29 = Enable response queue 0 in index shadowing
+	 * BIT 30 = Enable request queue 0 out index shadowing
+	 * BIT 31 = Reserved
 	 */
 	uint32_t firmware_options_3;
 	uint16_t qos;
@@ -1134,13 +1137,6 @@ struct device_reg_24xx {
 #define MIN_MULTI_ID_FABRIC	64	/* Must be power-of-2. */
 #define MAX_MULTI_ID_FABRIC	256	/* ... */
 
-#define for_each_mapped_vp_idx(_ha, _idx)		\
-	for (_idx = find_next_bit((_ha)->vp_idx_map,	\
-		(_ha)->max_npiv_vports + 1, 1);		\
-	    _idx <= (_ha)->max_npiv_vports;		\
-	    _idx = find_next_bit((_ha)->vp_idx_map,	\
-		(_ha)->max_npiv_vports + 1, _idx + 1))	\
-
 struct mid_conf_entry_24xx {
 	uint16_t reserved_1;
 
diff --git a/drivers/scsi/qla2xxx/qla_gbl.h b/drivers/scsi/qla2xxx/qla_gbl.h
index e665e810993..d48dea8fab1 100644
--- a/drivers/scsi/qla2xxx/qla_gbl.h
+++ b/drivers/scsi/qla2xxx/qla_gbl.h
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
@@ -220,6 +220,13 @@ extern unsigned long qla2x00_get_async_timeout(struct scsi_qla_host *);
 
 extern void *qla2x00_alloc_iocbs(scsi_qla_host_t *, srb_t *);
 extern int qla2x00_issue_marker(scsi_qla_host_t *, int);
+extern int qla24xx_walk_and_build_sglist_no_difb(struct qla_hw_data *, srb_t *,
+	uint32_t *, uint16_t, struct qla_tgt_cmd *);
+extern int qla24xx_walk_and_build_sglist(struct qla_hw_data *, srb_t *,
+	uint32_t *, uint16_t, struct qla_tgt_cmd *);
+extern int qla24xx_walk_and_build_prot_sglist(struct qla_hw_data *, srb_t *,
+	uint32_t *, uint16_t, struct qla_tgt_cmd *);
+
 
 /*
  * Global Function Prototypes in qla_mbx.c source file.
@@ -347,6 +354,11 @@ extern int
 qla2x00_read_serdes_word(scsi_qla_host_t *, uint16_t, uint16_t *);
 
 extern int
+qla8044_write_serdes_word(scsi_qla_host_t *, uint32_t, uint32_t);
+extern int
+qla8044_read_serdes_word(scsi_qla_host_t *, uint32_t, uint32_t *);
+
+extern int
 qla2x00_set_serdes_params(scsi_qla_host_t *, uint16_t, uint16_t, uint16_t);
 
 extern int
diff --git a/drivers/scsi/qla2xxx/qla_gs.c b/drivers/scsi/qla2xxx/qla_gs.c
index e377f9d2f92..a0df3b1b382 100644
--- a/drivers/scsi/qla2xxx/qla_gs.c
+++ b/drivers/scsi/qla2xxx/qla_gs.c
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
diff --git a/drivers/scsi/qla2xxx/qla_init.c b/drivers/scsi/qla2xxx/qla_init.c
index 38aeb54cd9d..e2184412617 100644
--- a/drivers/scsi/qla2xxx/qla_init.c
+++ b/drivers/scsi/qla2xxx/qla_init.c
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
@@ -1476,6 +1476,7 @@ qla2x00_alloc_fw_dump(scsi_qla_host_t *vha)
 	}
 
 	ha->fw_dumped = 0;
+	ha->fw_dump_cap_flags = 0;
 	dump_size = fixed_size = mem_size = eft_size = fce_size = mq_size = 0;
 	req_q_size = rsp_q_size = 0;
 
@@ -2061,6 +2062,10 @@ qla24xx_config_rings(struct scsi_qla_host *vha)
 	icb->atio_q_address[0] = cpu_to_le32(LSD(ha->tgt.atio_dma));
 	icb->atio_q_address[1] = cpu_to_le32(MSD(ha->tgt.atio_dma));
 
+	if (IS_SHADOW_REG_CAPABLE(ha))
+		icb->firmware_options_2 |=
+		    __constant_cpu_to_le32(BIT_30|BIT_29);
+
 	if (ha->mqenable || IS_QLA83XX(ha) || IS_QLA27XX(ha)) {
 		icb->qos = __constant_cpu_to_le16(QLA_DEFAULT_QUE_QOS);
 		icb->rid = __constant_cpu_to_le16(rid);
@@ -2138,6 +2143,8 @@ qla2x00_init_rings(scsi_qla_host_t *vha)
 		req = ha->req_q_map[que];
 		if (!req)
 			continue;
+		req->out_ptr = (void *)(req->ring + req->length);
+		*req->out_ptr = 0;
 		for (cnt = 1; cnt < req->num_outstanding_cmds; cnt++)
 			req->outstanding_cmds[cnt] = NULL;
 
@@ -2153,6 +2160,8 @@ qla2x00_init_rings(scsi_qla_host_t *vha)
 		rsp = ha->rsp_q_map[que];
 		if (!rsp)
 			continue;
+		rsp->in_ptr = (void *)(rsp->ring + rsp->length);
+		*rsp->in_ptr = 0;
 		/* Initialize response queue entries */
 		if (IS_QLAFX00(ha))
 			qlafx00_init_response_q_entries(rsp);
@@ -3406,7 +3415,7 @@ qla2x00_configure_fabric(scsi_qla_host_t *vha)
 					    fcport->d_id.b.domain,
 					    fcport->d_id.b.area,
 					    fcport->d_id.b.al_pa);
-					fcport->loop_id = FC_NO_LOOP_ID;
+					qla2x00_clear_loop_id(fcport);
 				}
 			}
 		}
@@ -4727,7 +4736,6 @@ static int
 qla2x00_restart_isp(scsi_qla_host_t *vha)
 {
 	int status = 0;
-	uint32_t wait_time;
 	struct qla_hw_data *ha = vha->hw;
 	struct req_que *req = ha->req_q_map[0];
 	struct rsp_que *rsp = ha->rsp_q_map[0];
@@ -4744,14 +4752,12 @@ qla2x00_restart_isp(scsi_qla_host_t *vha)
 	if (!status && !(status = qla2x00_init_rings(vha))) {
 		clear_bit(RESET_MARKER_NEEDED, &vha->dpc_flags);
 		ha->flags.chip_reset_done = 1;
+
 		/* Initialize the queues in use */
 		qla25xx_init_queues(ha);
 
 		status = qla2x00_fw_ready(vha);
 		if (!status) {
-			ql_dbg(ql_dbg_taskm, vha, 0x8031,
-			    "Start configure loop status = %d.\n", status);
-
 			/* Issue a marker after FW becomes ready. */
 			qla2x00_marker(vha, req, rsp, 0, 0, MK_SYNC_ALL);
 
@@ -4766,24 +4772,12 @@ qla2x00_restart_isp(scsi_qla_host_t *vha)
 				qlt_24xx_process_atio_queue(vha);
 			spin_unlock_irqrestore(&ha->hardware_lock, flags);
 
-			/* Wait at most MAX_TARGET RSCNs for a stable link. */
-			wait_time = 256;
-			do {
-				clear_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags);
-				qla2x00_configure_loop(vha);
-				wait_time--;
-			} while (!atomic_read(&vha->loop_down_timer) &&
-				!(test_bit(ISP_ABORT_NEEDED, &vha->dpc_flags))
-				&& wait_time && (test_bit(LOOP_RESYNC_NEEDED,
-				&vha->dpc_flags)));
+			set_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags);
 		}
 
 		/* if no cable then assume it's good */
 		if ((vha->device_flags & DFLG_NO_CABLE))
 			status = 0;
-
-		ql_dbg(ql_dbg_taskm, vha, 0x8032,
-		    "Configure loop done, status = 0x%x.\n", status);
 	}
 	return (status);
 }
@@ -6130,7 +6124,6 @@ int
 qla82xx_restart_isp(scsi_qla_host_t *vha)
 {
 	int status, rval;
-	uint32_t wait_time;
 	struct qla_hw_data *ha = vha->hw;
 	struct req_que *req = ha->req_q_map[0];
 	struct rsp_que *rsp = ha->rsp_q_map[0];
@@ -6144,31 +6137,15 @@ qla82xx_restart_isp(scsi_qla_host_t *vha)
 
 		status = qla2x00_fw_ready(vha);
 		if (!status) {
-			ql_log(ql_log_info, vha, 0x803c,
-			    "Start configure loop, status =%d.\n", status);
-
 			/* Issue a marker after FW becomes ready. */
 			qla2x00_marker(vha, req, rsp, 0, 0, MK_SYNC_ALL);
-
 			vha->flags.online = 1;
-			/* Wait at most MAX_TARGET RSCNs for a stable link. */
-			wait_time = 256;
-			do {
-				clear_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags);
-				qla2x00_configure_loop(vha);
-				wait_time--;
-			} while (!atomic_read(&vha->loop_down_timer) &&
-			    !(test_bit(ISP_ABORT_NEEDED, &vha->dpc_flags)) &&
-			    wait_time &&
-			    (test_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags)));
+			set_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags);
 		}
 
 		/* if no cable then assume it's good */
 		if ((vha->device_flags & DFLG_NO_CABLE))
 			status = 0;
-
-		ql_log(ql_log_info, vha, 0x8000,
-		    "Configure loop done, status = 0x%x.\n", status);
 	}
 
 	if (!status) {
@@ -6182,8 +6159,6 @@ qla82xx_restart_isp(scsi_qla_host_t *vha)
 			vha->marker_needed = 1;
 		}
 
-		vha->flags.online = 1;
-
 		ha->isp_ops->enable_intrs(ha);
 
 		ha->isp_abort_cnt = 0;
diff --git a/drivers/scsi/qla2xxx/qla_inline.h b/drivers/scsi/qla2xxx/qla_inline.h
index ce8b5fb0f34..b3b1d6fc2d6 100644
--- a/drivers/scsi/qla2xxx/qla_inline.h
+++ b/drivers/scsi/qla2xxx/qla_inline.h
@@ -1,10 +1,11 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
 
+#include "qla_target.h"
 /**
  * qla24xx_calc_iocbs() - Determine number of Command Type 3 and
  * Continuation Type 1 IOCBs to allocate.
@@ -128,12 +129,20 @@ qla2x00_clear_loop_id(fc_port_t *fcport) {
 }
 
 static inline void
-qla2x00_clean_dsd_pool(struct qla_hw_data *ha, srb_t *sp)
+qla2x00_clean_dsd_pool(struct qla_hw_data *ha, srb_t *sp,
+	struct qla_tgt_cmd *tc)
 {
 	struct dsd_dma *dsd_ptr, *tdsd_ptr;
 	struct crc_context *ctx;
 
-	ctx = (struct crc_context *)GET_CMD_CTX_SP(sp);
+	if (sp)
+		ctx = (struct crc_context *)GET_CMD_CTX_SP(sp);
+	else if (tc)
+		ctx = (struct crc_context *)tc->ctx;
+	else {
+		BUG();
+		return;
+	}
 
 	/* clean up allocated prev pool */
 	list_for_each_entry_safe(dsd_ptr, tdsd_ptr,
diff --git a/drivers/scsi/qla2xxx/qla_iocb.c b/drivers/scsi/qla2xxx/qla_iocb.c
index e607568bce4..76093152959 100644
--- a/drivers/scsi/qla2xxx/qla_iocb.c
+++ b/drivers/scsi/qla2xxx/qla_iocb.c
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
@@ -936,9 +936,9 @@ qla24xx_get_one_block_sg(uint32_t blk_sz, struct qla2_sgx *sgx,
 	return 1;
 }
 
-static int
+int
 qla24xx_walk_and_build_sglist_no_difb(struct qla_hw_data *ha, srb_t *sp,
-	uint32_t *dsd, uint16_t tot_dsds)
+	uint32_t *dsd, uint16_t tot_dsds, struct qla_tgt_cmd *tc)
 {
 	void *next_dsd;
 	uint8_t avail_dsds = 0;
@@ -948,21 +948,35 @@ qla24xx_walk_and_build_sglist_no_difb(struct qla_hw_data *ha, srb_t *sp,
 	uint32_t *cur_dsd = dsd;
 	uint16_t	used_dsds = tot_dsds;
 
-	uint32_t	prot_int;
+	uint32_t	prot_int; /* protection interval */
 	uint32_t	partial;
 	struct qla2_sgx sgx;
 	dma_addr_t	sle_dma;
 	uint32_t	sle_dma_len, tot_prot_dma_len = 0;
-	struct scsi_cmnd *cmd = GET_CMD_SP(sp);
-
-	prot_int = cmd->device->sector_size;
+	struct scsi_cmnd *cmd;
+	struct scsi_qla_host *vha;
 
 	memset(&sgx, 0, sizeof(struct qla2_sgx));
-	sgx.tot_bytes = scsi_bufflen(cmd);
-	sgx.cur_sg = scsi_sglist(cmd);
-	sgx.sp = sp;
-
-	sg_prot = scsi_prot_sglist(cmd);
+	if (sp) {
+		vha = sp->fcport->vha;
+		cmd = GET_CMD_SP(sp);
+		prot_int = cmd->device->sector_size;
+
+		sgx.tot_bytes = scsi_bufflen(cmd);
+		sgx.cur_sg = scsi_sglist(cmd);
+		sgx.sp = sp;
+
+		sg_prot = scsi_prot_sglist(cmd);
+	} else if (tc) {
+		vha = tc->vha;
+		prot_int      = tc->blk_sz;
+		sgx.tot_bytes = tc->bufflen;
+		sgx.cur_sg    = tc->sg;
+		sg_prot	      = tc->prot_sg;
+	} else {
+		BUG();
+		return 1;
+	}
 
 	while (qla24xx_get_one_block_sg(prot_int, &sgx, &partial)) {
 
@@ -995,10 +1009,18 @@ alloc_and_fill:
 				return 1;
 			}
 
-			list_add_tail(&dsd_ptr->list,
-			    &((struct crc_context *)sp->u.scmd.ctx)->dsd_list);
+			if (sp) {
+				list_add_tail(&dsd_ptr->list,
+				    &((struct crc_context *)
+					    sp->u.scmd.ctx)->dsd_list);
+
+				sp->flags |= SRB_CRC_CTX_DSD_VALID;
+			} else {
+				list_add_tail(&dsd_ptr->list,
+				    &(tc->ctx->dsd_list));
+				tc->ctx_dsd_alloced = 1;
+			}
 
-			sp->flags |= SRB_CRC_CTX_DSD_VALID;
 
 			/* add new list to cmd iocb or last list */
 			*cur_dsd++ = cpu_to_le32(LSD(dsd_ptr->dsd_list_dma));
@@ -1033,21 +1055,35 @@ alloc_and_fill:
 	return 0;
 }
 
-static int
+int
 qla24xx_walk_and_build_sglist(struct qla_hw_data *ha, srb_t *sp, uint32_t *dsd,
-	uint16_t tot_dsds)
+	uint16_t tot_dsds, struct qla_tgt_cmd *tc)
 {
 	void *next_dsd;
 	uint8_t avail_dsds = 0;
 	uint32_t dsd_list_len;
 	struct dsd_dma *dsd_ptr;
-	struct scatterlist *sg;
+	struct scatterlist *sg, *sgl;
 	uint32_t *cur_dsd = dsd;
 	int	i;
 	uint16_t	used_dsds = tot_dsds;
-	struct scsi_cmnd *cmd = GET_CMD_SP(sp);
+	struct scsi_cmnd *cmd;
+	struct scsi_qla_host *vha;
+
+	if (sp) {
+		cmd = GET_CMD_SP(sp);
+		sgl = scsi_sglist(cmd);
+		vha = sp->fcport->vha;
+	} else if (tc) {
+		sgl = tc->sg;
+		vha = tc->vha;
+	} else {
+		BUG();
+		return 1;
+	}
 
-	scsi_for_each_sg(cmd, sg, tot_dsds, i) {
+
+	for_each_sg(sgl, sg, tot_dsds, i) {
 		dma_addr_t	sle_dma;
 
 		/* Allocate additional continuation packets? */
@@ -1076,10 +1112,17 @@ qla24xx_walk_and_build_sglist(struct qla_hw_data *ha, srb_t *sp, uint32_t *dsd,
 				return 1;
 			}
 
-			list_add_tail(&dsd_ptr->list,
-			    &((struct crc_context *)sp->u.scmd.ctx)->dsd_list);
+			if (sp) {
+				list_add_tail(&dsd_ptr->list,
+				    &((struct crc_context *)
+					    sp->u.scmd.ctx)->dsd_list);
 
-			sp->flags |= SRB_CRC_CTX_DSD_VALID;
+				sp->flags |= SRB_CRC_CTX_DSD_VALID;
+			} else {
+				list_add_tail(&dsd_ptr->list,
+				    &(tc->ctx->dsd_list));
+				tc->ctx_dsd_alloced = 1;
+			}
 
 			/* add new list to cmd iocb or last list */
 			*cur_dsd++ = cpu_to_le32(LSD(dsd_ptr->dsd_list_dma));
@@ -1102,23 +1145,37 @@ qla24xx_walk_and_build_sglist(struct qla_hw_data *ha, srb_t *sp, uint32_t *dsd,
 	return 0;
 }
 
-static int
+int
 qla24xx_walk_and_build_prot_sglist(struct qla_hw_data *ha, srb_t *sp,
-							uint32_t *dsd,
-	uint16_t tot_dsds)
+	uint32_t *dsd, uint16_t tot_dsds, struct qla_tgt_cmd *tc)
 {
 	void *next_dsd;
 	uint8_t avail_dsds = 0;
 	uint32_t dsd_list_len;
 	struct dsd_dma *dsd_ptr;
-	struct scatterlist *sg;
+	struct scatterlist *sg, *sgl;
 	int	i;
 	struct scsi_cmnd *cmd;
 	uint32_t *cur_dsd = dsd;
-	uint16_t	used_dsds = tot_dsds;
+	uint16_t used_dsds = tot_dsds;
+	struct scsi_qla_host *vha;
+
+	if (sp) {
+		cmd = GET_CMD_SP(sp);
+		sgl = scsi_prot_sglist(cmd);
+		vha = sp->fcport->vha;
+	} else if (tc) {
+		vha = tc->vha;
+		sgl = tc->prot_sg;
+	} else {
+		BUG();
+		return 1;
+	}
 
-	cmd = GET_CMD_SP(sp);
-	scsi_for_each_prot_sg(cmd, sg, tot_dsds, i) {
+	ql_dbg(ql_dbg_tgt, vha, 0xe021,
+		"%s: enter\n", __func__);
+
+	for_each_sg(sgl, sg, tot_dsds, i) {
 		dma_addr_t	sle_dma;
 
 		/* Allocate additional continuation packets? */
@@ -1147,10 +1204,17 @@ qla24xx_walk_and_build_prot_sglist(struct qla_hw_data *ha, srb_t *sp,
 				return 1;
 			}
 
-			list_add_tail(&dsd_ptr->list,
-			    &((struct crc_context *)sp->u.scmd.ctx)->dsd_list);
+			if (sp) {
+				list_add_tail(&dsd_ptr->list,
+				    &((struct crc_context *)
+					    sp->u.scmd.ctx)->dsd_list);
 
-			sp->flags |= SRB_CRC_CTX_DSD_VALID;
+				sp->flags |= SRB_CRC_CTX_DSD_VALID;
+			} else {
+				list_add_tail(&dsd_ptr->list,
+				    &(tc->ctx->dsd_list));
+				tc->ctx_dsd_alloced = 1;
+			}
 
 			/* add new list to cmd iocb or last list */
 			*cur_dsd++ = cpu_to_le32(LSD(dsd_ptr->dsd_list_dma));
@@ -1386,10 +1450,10 @@ qla24xx_build_scsi_crc_2_iocbs(srb_t *sp, struct cmd_type_crc_2 *cmd_pkt,
 
 	if (!bundling && tot_prot_dsds) {
 		if (qla24xx_walk_and_build_sglist_no_difb(ha, sp,
-		    cur_dsd, tot_dsds))
+			cur_dsd, tot_dsds, NULL))
 			goto crc_queuing_error;
 	} else if (qla24xx_walk_and_build_sglist(ha, sp, cur_dsd,
-	    (tot_dsds - tot_prot_dsds)))
+			(tot_dsds - tot_prot_dsds), NULL))
 		goto crc_queuing_error;
 
 	if (bundling && tot_prot_dsds) {
@@ -1398,7 +1462,7 @@ qla24xx_build_scsi_crc_2_iocbs(srb_t *sp, struct cmd_type_crc_2 *cmd_pkt,
 			__constant_cpu_to_le16(CF_DIF_SEG_DESCR_ENABLE);
 		cur_dsd = (uint32_t *) &crc_ctx_pkt->u.bundling.dif_address;
 		if (qla24xx_walk_and_build_prot_sglist(ha, sp, cur_dsd,
-		    tot_prot_dsds))
+				tot_prot_dsds, NULL))
 			goto crc_queuing_error;
 	}
 	return QLA_SUCCESS;
@@ -1478,8 +1542,8 @@ qla24xx_start_scsi(srb_t *sp)
 	tot_dsds = nseg;
 	req_cnt = qla24xx_calc_iocbs(vha, tot_dsds);
 	if (req->cnt < (req_cnt + 2)) {
-		cnt = RD_REG_DWORD_RELAXED(req->req_q_out);
-
+		cnt = IS_SHADOW_REG_CAPABLE(ha) ? *req->out_ptr :
+		    RD_REG_DWORD_RELAXED(req->req_q_out);
 		if (req->ring_index < cnt)
 			req->cnt = cnt - req->ring_index;
 		else
@@ -1697,8 +1761,8 @@ qla24xx_dif_start_scsi(srb_t *sp)
 	tot_prot_dsds = nseg;
 	tot_dsds += nseg;
 	if (req->cnt < (req_cnt + 2)) {
-		cnt = RD_REG_DWORD_RELAXED(req->req_q_out);
-
+		cnt = IS_SHADOW_REG_CAPABLE(ha) ? *req->out_ptr :
+		    RD_REG_DWORD_RELAXED(req->req_q_out);
 		if (req->ring_index < cnt)
 			req->cnt = cnt - req->ring_index;
 		else
@@ -2825,8 +2889,8 @@ qla2x00_start_bidir(srb_t *sp, struct scsi_qla_host *vha, uint32_t tot_dsds)
 
 	/* Check for room on request queue. */
 	if (req->cnt < req_cnt + 2) {
-		cnt = RD_REG_DWORD_RELAXED(req->req_q_out);
-
+		cnt = IS_SHADOW_REG_CAPABLE(ha) ? *req->out_ptr :
+		    RD_REG_DWORD_RELAXED(req->req_q_out);
 		if  (req->ring_index < cnt)
 			req->cnt = cnt - req->ring_index;
 		else
diff --git a/drivers/scsi/qla2xxx/qla_isr.c b/drivers/scsi/qla2xxx/qla_isr.c
index 95314ef2e50..a56825c73c3 100644
--- a/drivers/scsi/qla2xxx/qla_isr.c
+++ b/drivers/scsi/qla2xxx/qla_isr.c
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
@@ -2009,11 +2009,13 @@ qla2x00_status_entry(scsi_qla_host_t *vha, struct rsp_que *rsp, void *pkt)
 		ql_dbg(ql_dbg_io, vha, 0x3017,
 		    "Invalid status handle (0x%x).\n", sts->handle);
 
-		if (IS_P3P_TYPE(ha))
-			set_bit(FCOE_CTX_RESET_NEEDED, &vha->dpc_flags);
-		else
-			set_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
-		qla2xxx_wake_dpc(vha);
+		if (!test_bit(ABORT_ISP_ACTIVE, &vha->dpc_flags)) {
+			if (IS_P3P_TYPE(ha))
+				set_bit(FCOE_CTX_RESET_NEEDED, &vha->dpc_flags);
+			else
+				set_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
+			qla2xxx_wake_dpc(vha);
+		}
 		return;
 	}
 
@@ -2472,12 +2474,14 @@ void qla24xx_process_response_queue(struct scsi_qla_host *vha,
 		if (pkt->entry_status != 0) {
 			qla2x00_error_entry(vha, rsp, (sts_entry_t *) pkt);
 
-			(void)qlt_24xx_process_response_error(vha, pkt);
+			if (qlt_24xx_process_response_error(vha, pkt))
+				goto process_err;
 
 			((response_t *)pkt)->signature = RESPONSE_PROCESSED;
 			wmb();
 			continue;
 		}
+process_err:
 
 		switch (pkt->entry_type) {
 		case STATUS_TYPE:
@@ -2494,10 +2498,10 @@ void qla24xx_process_response_queue(struct scsi_qla_host *vha,
 			qla24xx_logio_entry(vha, rsp->req,
 			    (struct logio_entry_24xx *)pkt);
 			break;
-                case CT_IOCB_TYPE:
+		case CT_IOCB_TYPE:
 			qla24xx_els_ct_entry(vha, rsp->req, pkt, CT_IOCB_TYPE);
 			break;
-                case ELS_IOCB_TYPE:
+		case ELS_IOCB_TYPE:
 			qla24xx_els_ct_entry(vha, rsp->req, pkt, ELS_IOCB_TYPE);
 			break;
 		case ABTS_RECV_24XX:
@@ -2506,6 +2510,7 @@ void qla24xx_process_response_queue(struct scsi_qla_host *vha,
 		case ABTS_RESP_24XX:
 		case CTIO_TYPE7:
 		case NOTIFY_ACK_TYPE:
+		case CTIO_CRC2:
 			qlt_response_pkt_all_vps(vha, (response_t *)pkt);
 			break;
 		case MARKER_TYPE:
diff --git a/drivers/scsi/qla2xxx/qla_mbx.c b/drivers/scsi/qla2xxx/qla_mbx.c
index 2528709c4ad..1c33a77db5c 100644
--- a/drivers/scsi/qla2xxx/qla_mbx.c
+++ b/drivers/scsi/qla2xxx/qla_mbx.c
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
@@ -1319,7 +1319,7 @@ qla2x00_get_node_name_list(scsi_qla_host_t *vha, void **out_data, int *out_len)
 
 		left = 0;
 
-		list = kzalloc(dma_size, GFP_KERNEL);
+		list = kmemdup(pmap, dma_size, GFP_KERNEL);
 		if (!list) {
 			ql_log(ql_log_warn, vha, 0x1140,
 			    "%s(%ld): failed to allocate node names list "
@@ -1328,7 +1328,6 @@ qla2x00_get_node_name_list(scsi_qla_host_t *vha, void **out_data, int *out_len)
 			goto out_free;
 		}
 
-		memcpy(list, pmap, dma_size);
 restart:
 		dma_free_coherent(&ha->pdev->dev, dma_size, pmap, pmap_dma);
 	}
@@ -2644,7 +2643,10 @@ qla24xx_abort_command(srb_t *sp)
 		ql_dbg(ql_dbg_mbx, vha, 0x1090,
 		    "Failed to complete IOCB -- completion status (%x).\n",
 		    le16_to_cpu(abt->nport_handle));
-		rval = QLA_FUNCTION_FAILED;
+		if (abt->nport_handle == CS_IOCB_ERROR)
+			rval = QLA_FUNCTION_PARAMETER_ERROR;
+		else
+			rval = QLA_FUNCTION_FAILED;
 	} else {
 		ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x1091,
 		    "Done %s.\n", __func__);
@@ -2879,6 +2881,78 @@ qla2x00_read_serdes_word(scsi_qla_host_t *vha, uint16_t addr, uint16_t *data)
 	return rval;
 }
 
+int
+qla8044_write_serdes_word(scsi_qla_host_t *vha, uint32_t addr, uint32_t data)
+{
+	int rval;
+	mbx_cmd_t mc;
+	mbx_cmd_t *mcp = &mc;
+
+	if (!IS_QLA8044(vha->hw))
+		return QLA_FUNCTION_FAILED;
+
+	ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x1186,
+	    "Entered %s.\n", __func__);
+
+	mcp->mb[0] = MBC_SET_GET_ETH_SERDES_REG;
+	mcp->mb[1] = HCS_WRITE_SERDES;
+	mcp->mb[3] = LSW(addr);
+	mcp->mb[4] = MSW(addr);
+	mcp->mb[5] = LSW(data);
+	mcp->mb[6] = MSW(data);
+	mcp->out_mb = MBX_6|MBX_5|MBX_4|MBX_3|MBX_1|MBX_0;
+	mcp->in_mb = MBX_0;
+	mcp->tov = MBX_TOV_SECONDS;
+	mcp->flags = 0;
+	rval = qla2x00_mailbox_command(vha, mcp);
+
+	if (rval != QLA_SUCCESS) {
+		ql_dbg(ql_dbg_mbx, vha, 0x1187,
+		    "Failed=%x mb[0]=%x.\n", rval, mcp->mb[0]);
+	} else {
+		ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x1188,
+		    "Done %s.\n", __func__);
+	}
+
+	return rval;
+}
+
+int
+qla8044_read_serdes_word(scsi_qla_host_t *vha, uint32_t addr, uint32_t *data)
+{
+	int rval;
+	mbx_cmd_t mc;
+	mbx_cmd_t *mcp = &mc;
+
+	if (!IS_QLA8044(vha->hw))
+		return QLA_FUNCTION_FAILED;
+
+	ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x1189,
+	    "Entered %s.\n", __func__);
+
+	mcp->mb[0] = MBC_SET_GET_ETH_SERDES_REG;
+	mcp->mb[1] = HCS_READ_SERDES;
+	mcp->mb[3] = LSW(addr);
+	mcp->mb[4] = MSW(addr);
+	mcp->out_mb = MBX_4|MBX_3|MBX_1|MBX_0;
+	mcp->in_mb = MBX_2|MBX_1|MBX_0;
+	mcp->tov = MBX_TOV_SECONDS;
+	mcp->flags = 0;
+	rval = qla2x00_mailbox_command(vha, mcp);
+
+	*data = mcp->mb[2] << 16 | mcp->mb[1];
+
+	if (rval != QLA_SUCCESS) {
+		ql_dbg(ql_dbg_mbx, vha, 0x118a,
+		    "Failed=%x mb[0]=%x.\n", rval, mcp->mb[0]);
+	} else {
+		ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x118b,
+		    "Done %s.\n", __func__);
+	}
+
+	return rval;
+}
+
 /**
  * qla2x00_set_serdes_params() -
  * @ha: HA context
@@ -3660,6 +3734,9 @@ qla25xx_init_req_que(struct scsi_qla_host *vha, struct req_que *req)
 	ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x10d3,
 	    "Entered %s.\n", __func__);
 
+	if (IS_SHADOW_REG_CAPABLE(ha))
+		req->options |= BIT_13;
+
 	mcp->mb[0] = MBC_INITIALIZE_MULTIQ;
 	mcp->mb[1] = req->options;
 	mcp->mb[2] = MSW(LSD(req->dma));
@@ -3679,7 +3756,7 @@ qla25xx_init_req_que(struct scsi_qla_host *vha, struct req_que *req)
 	/* que in ptr index */
 	mcp->mb[8] = 0;
 	/* que out ptr index */
-	mcp->mb[9] = 0;
+	mcp->mb[9] = *req->out_ptr = 0;
 	mcp->out_mb = MBX_14|MBX_13|MBX_12|MBX_11|MBX_10|MBX_9|MBX_8|MBX_7|
 			MBX_6|MBX_5|MBX_4|MBX_3|MBX_2|MBX_1|MBX_0;
 	mcp->in_mb = MBX_0;
@@ -3688,7 +3765,7 @@ qla25xx_init_req_que(struct scsi_qla_host *vha, struct req_que *req)
 
 	if (IS_QLA81XX(ha) || IS_QLA83XX(ha) || IS_QLA27XX(ha))
 		mcp->in_mb |= MBX_1;
-	if (IS_QLA83XX(ha) || !IS_QLA27XX(ha)) {
+	if (IS_QLA83XX(ha) || IS_QLA27XX(ha)) {
 		mcp->out_mb |= MBX_15;
 		/* debug q create issue in SR-IOV */
 		mcp->in_mb |= MBX_9 | MBX_8 | MBX_7;
@@ -3697,7 +3774,7 @@ qla25xx_init_req_que(struct scsi_qla_host *vha, struct req_que *req)
 	spin_lock_irqsave(&ha->hardware_lock, flags);
 	if (!(req->options & BIT_0)) {
 		WRT_REG_DWORD(req->req_q_in, 0);
-		if (!IS_QLA83XX(ha) || !IS_QLA27XX(ha))
+		if (!IS_QLA83XX(ha) && !IS_QLA27XX(ha))
 			WRT_REG_DWORD(req->req_q_out, 0);
 	}
 	spin_unlock_irqrestore(&ha->hardware_lock, flags);
@@ -3726,6 +3803,9 @@ qla25xx_init_rsp_que(struct scsi_qla_host *vha, struct rsp_que *rsp)
 	ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x10d6,
 	    "Entered %s.\n", __func__);
 
+	if (IS_SHADOW_REG_CAPABLE(ha))
+		rsp->options |= BIT_13;
+
 	mcp->mb[0] = MBC_INITIALIZE_MULTIQ;
 	mcp->mb[1] = rsp->options;
 	mcp->mb[2] = MSW(LSD(rsp->dma));
@@ -3740,7 +3820,7 @@ qla25xx_init_rsp_que(struct scsi_qla_host *vha, struct rsp_que *rsp)
 
 	mcp->mb[4] = rsp->id;
 	/* que in ptr index */
-	mcp->mb[8] = 0;
+	mcp->mb[8] = *rsp->in_ptr = 0;
 	/* que out ptr index */
 	mcp->mb[9] = 0;
 	mcp->out_mb = MBX_14|MBX_13|MBX_9|MBX_8|MBX_7
diff --git a/drivers/scsi/qla2xxx/qla_mid.c b/drivers/scsi/qla2xxx/qla_mid.c
index f0a852257f9..89998244f48 100644
--- a/drivers/scsi/qla2xxx/qla_mid.c
+++ b/drivers/scsi/qla2xxx/qla_mid.c
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
diff --git a/drivers/scsi/qla2xxx/qla_mr.c b/drivers/scsi/qla2xxx/qla_mr.c
index 0aaf6a9c87d..abeb3901498 100644
--- a/drivers/scsi/qla2xxx/qla_mr.c
+++ b/drivers/scsi/qla2xxx/qla_mr.c
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
@@ -527,21 +527,63 @@ qlafx00_soc_cpu_reset(scsi_qla_host_t *vha)
 	struct qla_hw_data *ha = vha->hw;
 	int i, core;
 	uint32_t cnt;
+	uint32_t reg_val;
+
+	spin_lock_irqsave(&ha->hardware_lock, flags);
+
+	QLAFX00_SET_HBA_SOC_REG(ha, 0x80004, 0);
+	QLAFX00_SET_HBA_SOC_REG(ha, 0x82004, 0);
+
+	/* stop the XOR DMA engines */
+	QLAFX00_SET_HBA_SOC_REG(ha, 0x60920, 0x02);
+	QLAFX00_SET_HBA_SOC_REG(ha, 0x60924, 0x02);
+	QLAFX00_SET_HBA_SOC_REG(ha, 0xf0920, 0x02);
+	QLAFX00_SET_HBA_SOC_REG(ha, 0xf0924, 0x02);
+
+	/* stop the IDMA engines */
+	reg_val = QLAFX00_GET_HBA_SOC_REG(ha, 0x60840);
+	reg_val &= ~(1<<12);
+	QLAFX00_SET_HBA_SOC_REG(ha, 0x60840, reg_val);
+
+	reg_val = QLAFX00_GET_HBA_SOC_REG(ha, 0x60844);
+	reg_val &= ~(1<<12);
+	QLAFX00_SET_HBA_SOC_REG(ha, 0x60844, reg_val);
+
+	reg_val = QLAFX00_GET_HBA_SOC_REG(ha, 0x60848);
+	reg_val &= ~(1<<12);
+	QLAFX00_SET_HBA_SOC_REG(ha, 0x60848, reg_val);
+
+	reg_val = QLAFX00_GET_HBA_SOC_REG(ha, 0x6084C);
+	reg_val &= ~(1<<12);
+	QLAFX00_SET_HBA_SOC_REG(ha, 0x6084C, reg_val);
+
+	for (i = 0; i < 100000; i++) {
+		if ((QLAFX00_GET_HBA_SOC_REG(ha, 0xd0000) & 0x10000000) == 0 &&
+		    (QLAFX00_GET_HBA_SOC_REG(ha, 0x10600) & 0x1) == 0)
+			break;
+		udelay(100);
+	}
 
 	/* Set all 4 cores in reset */
 	for (i = 0; i < 4; i++) {
 		QLAFX00_SET_HBA_SOC_REG(ha,
 		    (SOC_SW_RST_CONTROL_REG_CORE0 + 8*i), (0xF01));
-	}
-
-	/* Set all 4 core Clock gating control */
-	for (i = 0; i < 4; i++) {
 		QLAFX00_SET_HBA_SOC_REG(ha,
 		    (SOC_SW_RST_CONTROL_REG_CORE0 + 4 + 8*i), (0x01010101));
 	}
 
 	/* Reset all units in Fabric */
-	QLAFX00_SET_HBA_SOC_REG(ha, SOC_FABRIC_RST_CONTROL_REG, (0x11F0101));
+	QLAFX00_SET_HBA_SOC_REG(ha, SOC_FABRIC_RST_CONTROL_REG, (0x011f0101));
+
+	/* */
+	QLAFX00_SET_HBA_SOC_REG(ha, 0x10610, 1);
+	QLAFX00_SET_HBA_SOC_REG(ha, 0x10600, 0);
+
+	/* Set all 4 core Memory Power Down Registers */
+	for (i = 0; i < 5; i++) {
+		QLAFX00_SET_HBA_SOC_REG(ha,
+		    (SOC_PWR_MANAGEMENT_PWR_DOWN_REG + 4*i), (0x0));
+	}
 
 	/* Reset all interrupt control registers */
 	for (i = 0; i < 115; i++) {
@@ -564,20 +606,19 @@ qlafx00_soc_cpu_reset(scsi_qla_host_t *vha)
 	QLAFX00_SET_HBA_SOC_REG(ha, SOC_FABRIC_CONTROL_REG, (0x2));
 	QLAFX00_SET_HBA_SOC_REG(ha, SOC_FABRIC_CONFIG_REG, (0x3));
 
-	spin_lock_irqsave(&ha->hardware_lock, flags);
-
 	/* Kick in Fabric units */
 	QLAFX00_SET_HBA_SOC_REG(ha, SOC_FABRIC_RST_CONTROL_REG, (0x0));
 
 	/* Kick in Core0 to start boot process */
 	QLAFX00_SET_HBA_SOC_REG(ha, SOC_SW_RST_CONTROL_REG_CORE0, (0xF00));
 
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+
 	/* Wait 10secs for soft-reset to complete. */
 	for (cnt = 10; cnt; cnt--) {
 		msleep(1000);
 		barrier();
 	}
-	spin_unlock_irqrestore(&ha->hardware_lock, flags);
 }
 
 /**
@@ -597,7 +638,6 @@ qlafx00_soft_reset(scsi_qla_host_t *vha)
 
 	ha->isp_ops->disable_intrs(ha);
 	qlafx00_soc_cpu_reset(vha);
-	ha->isp_ops->enable_intrs(ha);
 }
 
 /**
@@ -2675,7 +2715,7 @@ qlafx00_process_response_queue(struct scsi_qla_host *vha,
 	uint16_t lreq_q_out = 0;
 
 	lreq_q_in = RD_REG_DWORD(rsp->rsp_q_in);
-	lreq_q_out = RD_REG_DWORD(rsp->rsp_q_out);
+	lreq_q_out = rsp->ring_index;
 
 	while (lreq_q_in != lreq_q_out) {
 		lptr = rsp->ring_ptr;
@@ -3426,7 +3466,7 @@ qlafx00_fxdisc_iocb(srb_t *sp, struct fxdisc_entry_fx00 *pfxiocb)
 	    sp->fcport->vha, 0x3047,
 	    (uint8_t *)&fx_iocb, sizeof(struct fxdisc_entry_fx00));
 
-	memcpy((void *)pfxiocb, &fx_iocb,
+	memcpy_toio((void __iomem *)pfxiocb, &fx_iocb,
 	    sizeof(struct fxdisc_entry_fx00));
 	wmb();
 }
diff --git a/drivers/scsi/qla2xxx/qla_mr.h b/drivers/scsi/qla2xxx/qla_mr.h
index e529dfaeb85..aeaa1b40b1f 100644
--- a/drivers/scsi/qla2xxx/qla_mr.h
+++ b/drivers/scsi/qla2xxx/qla_mr.h
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
@@ -351,6 +351,7 @@ struct config_info_data {
 #define SOC_FABRIC_RST_CONTROL_REG       0x0020840
 #define SOC_FABRIC_CONTROL_REG           0x0020200
 #define SOC_FABRIC_CONFIG_REG            0x0020204
+#define SOC_PWR_MANAGEMENT_PWR_DOWN_REG  0x001820C
 
 #define SOC_INTERRUPT_SOURCE_I_CONTROL_REG     0x0020B00
 #define SOC_CORE_TIMER_REG                     0x0021850
diff --git a/drivers/scsi/qla2xxx/qla_nx.c b/drivers/scsi/qla2xxx/qla_nx.c
index 5511e24b1f1..58f3c912d96 100644
--- a/drivers/scsi/qla2xxx/qla_nx.c
+++ b/drivers/scsi/qla2xxx/qla_nx.c
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
@@ -848,6 +848,7 @@ qla82xx_rom_lock(struct qla_hw_data *ha)
 {
 	int done = 0, timeout = 0;
 	uint32_t lock_owner = 0;
+	scsi_qla_host_t *vha = pci_get_drvdata(ha->pdev);
 
 	while (!done) {
 		/* acquire semaphore2 from PCI HW block */
@@ -856,17 +857,21 @@ 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,
+			    "%s: Simultaneous flash access by following ports, active port = %d: accessing port = %d",
+			    __func__, ha->portnum, lock_owner);
 			return -1;
 		}
 		timeout++;
 	}
-	qla82xx_wr_32(ha, QLA82XX_ROM_LOCK_ID, ROM_LOCK_DRIVER);
+	qla82xx_wr_32(ha, QLA82XX_ROM_LOCK_ID, ha->portnum);
 	return 0;
 }
 
 static void
 qla82xx_rom_unlock(struct qla_hw_data *ha)
 {
+	qla82xx_wr_32(ha, QLA82XX_ROM_LOCK_ID, 0xffffffff);
 	qla82xx_rd_32(ha, QLA82XX_PCIE_REG(PCIE_SEM2_UNLOCK));
 }
 
@@ -950,6 +955,7 @@ static int
 qla82xx_rom_fast_read(struct qla_hw_data *ha, int addr, int *valp)
 {
 	int ret, loops = 0;
+	uint32_t lock_owner = 0;
 	scsi_qla_host_t *vha = pci_get_drvdata(ha->pdev);
 
 	while ((qla82xx_rom_lock(ha) != 0) && (loops < 50000)) {
@@ -958,8 +964,10 @@ qla82xx_rom_fast_read(struct qla_hw_data *ha, int addr, int *valp)
 		loops++;
 	}
 	if (loops >= 50000) {
+		lock_owner = qla82xx_rd_32(ha, QLA82XX_ROM_LOCK_ID);
 		ql_log(ql_log_fatal, vha, 0x00b9,
-		    "Failed to acquire SEM2 lock.\n");
+		    "Failed to acquire SEM2 lock, Lock Owner %u.\n",
+		    lock_owner);
 		return -1;
 	}
 	ret = qla82xx_do_rom_fast_read(ha, addr, valp);
@@ -1057,6 +1065,7 @@ static int
 ql82xx_rom_lock_d(struct qla_hw_data *ha)
 {
 	int loops = 0;
+	uint32_t lock_owner = 0;
 	scsi_qla_host_t *vha = pci_get_drvdata(ha->pdev);
 
 	while ((qla82xx_rom_lock(ha) != 0) && (loops < 50000)) {
@@ -1065,8 +1074,9 @@ ql82xx_rom_lock_d(struct qla_hw_data *ha)
 		loops++;
 	}
 	if (loops >= 50000) {
+		lock_owner = qla82xx_rd_32(ha, QLA82XX_ROM_LOCK_ID);
 		ql_log(ql_log_warn, vha, 0xb010,
-		    "ROM lock failed.\n");
+		    "ROM lock failed, Lock Owner %u.\n", lock_owner);
 		return -1;
 	}
 	return 0;
@@ -2811,12 +2821,14 @@ static void
 qla82xx_rom_lock_recovery(struct qla_hw_data *ha)
 {
 	scsi_qla_host_t *vha = pci_get_drvdata(ha->pdev);
+	uint32_t lock_owner = 0;
 
-	if (qla82xx_rom_lock(ha))
+	if (qla82xx_rom_lock(ha)) {
+		lock_owner = qla82xx_rd_32(ha, QLA82XX_ROM_LOCK_ID);
 		/* Someone else is holding the lock. */
 		ql_log(ql_log_info, vha, 0xb022,
-		    "Resetting rom_lock.\n");
-
+		    "Resetting rom_lock, Lock Owner %u.\n", lock_owner);
+	}
 	/*
 	 * Either we got the lock, or someone
 	 * else died while holding it.
@@ -2840,47 +2852,30 @@ static int
 qla82xx_device_bootstrap(scsi_qla_host_t *vha)
 {
 	int rval = QLA_SUCCESS;
-	int i, timeout;
+	int i;
 	uint32_t old_count, count;
 	struct qla_hw_data *ha = vha->hw;
-	int need_reset = 0, peg_stuck = 1;
+	int need_reset = 0;
 
 	need_reset = qla82xx_need_reset(ha);
 
-	old_count = qla82xx_rd_32(ha, QLA82XX_PEG_ALIVE_COUNTER);
-
-	for (i = 0; i < 10; i++) {
-		timeout = msleep_interruptible(200);
-		if (timeout) {
-			qla82xx_wr_32(ha, QLA82XX_CRB_DEV_STATE,
-				QLA8XXX_DEV_FAILED);
-			return QLA_FUNCTION_FAILED;
-		}
-
-		count = qla82xx_rd_32(ha, QLA82XX_PEG_ALIVE_COUNTER);
-		if (count != old_count)
-			peg_stuck = 0;
-	}
-
 	if (need_reset) {
 		/* We are trying to perform a recovery here. */
-		if (peg_stuck)
+		if (ha->flags.isp82xx_fw_hung)
 			qla82xx_rom_lock_recovery(ha);
-		goto dev_initialize;
 	} else  {
-		/* Start of day for this ha context. */
-		if (peg_stuck) {
-			/* Either we are the first or recovery in progress. */
-			qla82xx_rom_lock_recovery(ha);
-			goto dev_initialize;
-		} else
-			/* Firmware already running. */
-			goto dev_ready;
+		old_count = qla82xx_rd_32(ha, QLA82XX_PEG_ALIVE_COUNTER);
+		for (i = 0; i < 10; i++) {
+			msleep(200);
+			count = qla82xx_rd_32(ha, QLA82XX_PEG_ALIVE_COUNTER);
+			if (count != old_count) {
+				rval = QLA_SUCCESS;
+				goto dev_ready;
+			}
+		}
+		qla82xx_rom_lock_recovery(ha);
 	}
 
-	return rval;
-
-dev_initialize:
 	/* set to DEV_INITIALIZING */
 	ql_log(ql_log_info, vha, 0x009e,
 	    "HW State: INITIALIZING.\n");
@@ -3142,18 +3137,18 @@ qla82xx_check_md_needed(scsi_qla_host_t *vha)
 
 	if (ql2xmdenable) {
 		if (!ha->fw_dumped) {
-			if (fw_major_version != ha->fw_major_version ||
+			if ((fw_major_version != ha->fw_major_version ||
 			    fw_minor_version != ha->fw_minor_version ||
-			    fw_subminor_version != ha->fw_subminor_version) {
+			    fw_subminor_version != ha->fw_subminor_version) ||
+			    (ha->prev_minidump_failed)) {
 				ql_dbg(ql_dbg_p3p, vha, 0xb02d,
-				    "Firmware version differs "
-				    "Previous version: %d:%d:%d - "
-				    "New version: %d:%d:%d\n",
+				    "Firmware version differs Previous version: %d:%d:%d - New version: %d:%d:%d, prev_minidump_failed: %d.\n",
 				    fw_major_version, fw_minor_version,
 				    fw_subminor_version,
 				    ha->fw_major_version,
 				    ha->fw_minor_version,
-				    ha->fw_subminor_version);
+				    ha->fw_subminor_version,
+				    ha->prev_minidump_failed);
 				/* Release MiniDump resources */
 				qla82xx_md_free(vha);
 				/* ALlocate MiniDump resources */
@@ -3682,8 +3677,10 @@ qla82xx_chip_reset_cleanup(scsi_qla_host_t *vha)
 			for (cnt = 1; cnt < req->num_outstanding_cmds; cnt++) {
 				sp = req->outstanding_cmds[cnt];
 				if (sp) {
-					if (!sp->u.scmd.ctx ||
-					    (sp->flags & SRB_FCP_CMND_DMA_VALID)) {
+					if ((!sp->u.scmd.ctx ||
+					    (sp->flags &
+						SRB_FCP_CMND_DMA_VALID)) &&
+						!ha->flags.isp82xx_fw_hung) {
 						spin_unlock_irqrestore(
 						    &ha->hardware_lock, flags);
 						if (ha->isp_ops->abort_command(sp)) {
diff --git a/drivers/scsi/qla2xxx/qla_nx.h b/drivers/scsi/qla2xxx/qla_nx.h
index 1bb93dbbccb..59c477883a7 100644
--- a/drivers/scsi/qla2xxx/qla_nx.h
+++ b/drivers/scsi/qla2xxx/qla_nx.h
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
@@ -333,9 +333,6 @@
 #define QLA82XX_ROMUSB_ROM_INSTR_OPCODE		(ROMUSB_ROM + 0x0004)
 #define QLA82XX_ROMUSB_GLB_CAS_RST		(ROMUSB_GLB + 0x0038)
 
-/* Lock IDs for ROM lock */
-#define ROM_LOCK_DRIVER       0x0d417340
-
 #define QLA82XX_PCI_CRB_WINDOWSIZE 0x00100000	 /* all are 1MB windows */
 #define QLA82XX_PCI_CRB_WINDOW(A) \
 	(QLA82XX_PCI_CRBSPACE + (A)*QLA82XX_PCI_CRB_WINDOWSIZE)
@@ -1186,6 +1183,7 @@ static const int MD_MIU_TEST_AGT_RDDATA[] = { 0x410000A8, 0x410000AC,
 #define CRB_NIU_XG_PAUSE_CTL_P1        0x8
 
 #define qla82xx_get_temp_val(x)          ((x) >> 16)
+#define qla82xx_get_temp_val1(x)          ((x) && 0x0000FFFF)
 #define qla82xx_get_temp_state(x)        ((x) & 0xffff)
 #define qla82xx_encode_temp(val, state)  (((val) << 16) | (state))
 
diff --git a/drivers/scsi/qla2xxx/qla_nx2.c b/drivers/scsi/qla2xxx/qla_nx2.c
index 86cf10815db..da9e3902f21 100644
--- a/drivers/scsi/qla2xxx/qla_nx2.c
+++ b/drivers/scsi/qla2xxx/qla_nx2.c
@@ -1,17 +1,20 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
 
 #include <linux/vmalloc.h>
+#include <linux/delay.h>
 
 #include "qla_def.h"
 #include "qla_gbl.h"
 
 #include <linux/delay.h>
 
+#define TIMEOUT_100_MS 100
+
 /* 8044 Flash Read/Write functions */
 uint32_t
 qla8044_rd_reg(struct qla_hw_data *ha, ulong addr)
@@ -117,6 +120,95 @@ qla8044_read_write_crb_reg(struct scsi_qla_host *vha,
 	qla8044_wr_reg_indirect(vha, waddr, value);
 }
 
+static int
+qla8044_poll_wait_for_ready(struct scsi_qla_host *vha, uint32_t addr1,
+	uint32_t mask)
+{
+	unsigned long timeout;
+	uint32_t temp;
+
+	/* jiffies after 100ms */
+	timeout = jiffies + msecs_to_jiffies(TIMEOUT_100_MS);
+	do {
+		qla8044_rd_reg_indirect(vha, addr1, &temp);
+		if ((temp & mask) != 0)
+			break;
+		if (time_after_eq(jiffies, timeout)) {
+			ql_log(ql_log_warn, vha, 0xb151,
+				"Error in processing rdmdio entry\n");
+			return -1;
+		}
+	} while (1);
+
+	return 0;
+}
+
+static uint32_t
+qla8044_ipmdio_rd_reg(struct scsi_qla_host *vha,
+	uint32_t addr1, uint32_t addr3, uint32_t mask, uint32_t addr)
+{
+	uint32_t temp;
+	int ret = 0;
+
+	ret = qla8044_poll_wait_for_ready(vha, addr1, mask);
+	if (ret == -1)
+		return -1;
+
+	temp = (0x40000000 | addr);
+	qla8044_wr_reg_indirect(vha, addr1, temp);
+
+	ret = qla8044_poll_wait_for_ready(vha, addr1, mask);
+	if (ret == -1)
+		return 0;
+
+	qla8044_rd_reg_indirect(vha, addr3, &ret);
+
+	return ret;
+}
+
+
+static int
+qla8044_poll_wait_ipmdio_bus_idle(struct scsi_qla_host *vha,
+	uint32_t addr1, uint32_t addr2, uint32_t addr3, uint32_t mask)
+{
+	unsigned long timeout;
+	uint32_t temp;
+
+	/* jiffies after 100 msecs */
+	timeout = jiffies + msecs_to_jiffies(TIMEOUT_100_MS);
+	do {
+		temp = qla8044_ipmdio_rd_reg(vha, addr1, addr3, mask, addr2);
+		if ((temp & 0x1) != 1)
+			break;
+		if (time_after_eq(jiffies, timeout)) {
+			ql_log(ql_log_warn, vha, 0xb152,
+			    "Error in processing mdiobus idle\n");
+			return -1;
+		}
+	} while (1);
+
+	return 0;
+}
+
+static int
+qla8044_ipmdio_wr_reg(struct scsi_qla_host *vha, uint32_t addr1,
+	uint32_t addr3, uint32_t mask, uint32_t addr, uint32_t value)
+{
+	int ret = 0;
+
+	ret = qla8044_poll_wait_for_ready(vha, addr1, mask);
+	if (ret == -1)
+		return -1;
+
+	qla8044_wr_reg_indirect(vha, addr3, value);
+	qla8044_wr_reg_indirect(vha, addr1, addr);
+
+	ret = qla8044_poll_wait_for_ready(vha, addr1, mask);
+	if (ret == -1)
+		return -1;
+
+	return 0;
+}
 /*
  * qla8044_rmw_crb_reg - Read value from raddr, AND with test_mask,
  * Shift Left,Right/OR/XOR with values RMW header and write value to waddr.
@@ -356,8 +448,8 @@ qla8044_flash_lock(scsi_qla_host_t *vha)
 			lock_owner = qla8044_rd_reg(ha,
 			    QLA8044_FLASH_LOCK_ID);
 			ql_log(ql_log_warn, vha, 0xb113,
-			    "%s: flash lock by %d failed, held by %d\n",
-				__func__, ha->portnum, lock_owner);
+			    "%s: Simultaneous flash access by following ports, active port = %d: accessing port = %d",
+			    __func__, ha->portnum, lock_owner);
 			ret_val = QLA_FUNCTION_FAILED;
 			break;
 		}
@@ -1541,7 +1633,7 @@ static void
 qla8044_need_reset_handler(struct scsi_qla_host *vha)
 {
 	uint32_t dev_state = 0, drv_state, drv_active;
-	unsigned long reset_timeout, dev_init_timeout;
+	unsigned long reset_timeout;
 	struct qla_hw_data *ha = vha->hw;
 
 	ql_log(ql_log_fatal, vha, 0xb0c2,
@@ -1555,84 +1647,78 @@ qla8044_need_reset_handler(struct scsi_qla_host *vha)
 		qla8044_idc_lock(ha);
 	}
 
+	dev_state = qla8044_rd_direct(vha,
+	    QLA8044_CRB_DEV_STATE_INDEX);
 	drv_state = qla8044_rd_direct(vha,
 	    QLA8044_CRB_DRV_STATE_INDEX);
 	drv_active = qla8044_rd_direct(vha,
 	    QLA8044_CRB_DRV_ACTIVE_INDEX);
 
 	ql_log(ql_log_info, vha, 0xb0c5,
-	    "%s(%ld): drv_state = 0x%x, drv_active = 0x%x\n",
-	    __func__, vha->host_no, drv_state, drv_active);
+	    "%s(%ld): drv_state = 0x%x, drv_active = 0x%x dev_state = 0x%x\n",
+	    __func__, vha->host_no, drv_state, drv_active, dev_state);
 
-	if (!ha->flags.nic_core_reset_owner) {
-		ql_dbg(ql_dbg_p3p, vha, 0xb0c3,
-		    "%s(%ld): reset acknowledged\n",
-		    __func__, vha->host_no);
-		qla8044_set_rst_ready(vha);
+	qla8044_set_rst_ready(vha);
 
-		/* Non-reset owners ACK Reset and wait for device INIT state
-		 * as part of Reset Recovery by Reset Owner
-		 */
-		dev_init_timeout = jiffies + (ha->fcoe_reset_timeout * HZ);
+	/* wait for 10 seconds for reset ack from all functions */
+	reset_timeout = jiffies + (ha->fcoe_reset_timeout * HZ);
 
-		do {
-			if (time_after_eq(jiffies, dev_init_timeout)) {
-				ql_log(ql_log_info, vha, 0xb0c4,
-				    "%s: Non Reset owner: Reset Ack Timeout!\n",
-				    __func__);
-				break;
-			}
+	do {
+		if (time_after_eq(jiffies, reset_timeout)) {
+			ql_log(ql_log_info, vha, 0xb0c4,
+			    "%s: Function %d: Reset Ack Timeout!, drv_state: 0x%08x, drv_active: 0x%08x\n",
+			    __func__, ha->portnum, drv_state, drv_active);
+			break;
+		}
 
-			qla8044_idc_unlock(ha);
-			msleep(1000);
-			qla8044_idc_lock(ha);
+		qla8044_idc_unlock(ha);
+		msleep(1000);
+		qla8044_idc_lock(ha);
 
-			dev_state = qla8044_rd_direct(vha,
-					QLA8044_CRB_DEV_STATE_INDEX);
-		} while (((drv_state & drv_active) != drv_active) &&
-		    (dev_state == QLA8XXX_DEV_NEED_RESET));
+		dev_state = qla8044_rd_direct(vha,
+		    QLA8044_CRB_DEV_STATE_INDEX);
+		drv_state = qla8044_rd_direct(vha,
+		    QLA8044_CRB_DRV_STATE_INDEX);
+		drv_active = qla8044_rd_direct(vha,
+		    QLA8044_CRB_DRV_ACTIVE_INDEX);
+	} while (((drv_state & drv_active) != drv_active) &&
+	    (dev_state == QLA8XXX_DEV_NEED_RESET));
+
+	/* Remove IDC participation of functions not acknowledging */
+	if (drv_state != drv_active) {
+		ql_log(ql_log_info, vha, 0xb0c7,
+		    "%s(%ld): Function %d turning off drv_active of non-acking function 0x%x\n",
+		    __func__, vha->host_no, ha->portnum,
+		    (drv_active ^ drv_state));
+		drv_active = drv_active & drv_state;
+		qla8044_wr_direct(vha, QLA8044_CRB_DRV_ACTIVE_INDEX,
+		    drv_active);
 	} else {
-		qla8044_set_rst_ready(vha);
-
-		/* wait for 10 seconds for reset ack from all functions */
-		reset_timeout = jiffies + (ha->fcoe_reset_timeout * HZ);
-
-		while ((drv_state & drv_active) != drv_active) {
-			if (time_after_eq(jiffies, reset_timeout)) {
-				ql_log(ql_log_info, vha, 0xb0c6,
-				    "%s: RESET TIMEOUT!"
-				    "drv_state: 0x%08x, drv_active: 0x%08x\n",
-				    QLA2XXX_DRIVER_NAME, drv_state, drv_active);
-				break;
-			}
-
-			qla8044_idc_unlock(ha);
-			msleep(1000);
-			qla8044_idc_lock(ha);
-
-			drv_state = qla8044_rd_direct(vha,
-			    QLA8044_CRB_DRV_STATE_INDEX);
-			drv_active = qla8044_rd_direct(vha,
-			    QLA8044_CRB_DRV_ACTIVE_INDEX);
-		}
-
-		if (drv_state != drv_active) {
-			ql_log(ql_log_info, vha, 0xb0c7,
-			    "%s(%ld): Reset_owner turning off drv_active "
-			    "of non-acking function 0x%x\n", __func__,
-			    vha->host_no, (drv_active ^ drv_state));
-			drv_active = drv_active & drv_state;
-			qla8044_wr_direct(vha, QLA8044_CRB_DRV_ACTIVE_INDEX,
-			    drv_active);
+		/*
+		 * Reset owner should execute reset recovery,
+		 * if all functions acknowledged
+		 */
+		if ((ha->flags.nic_core_reset_owner) &&
+		    (dev_state == QLA8XXX_DEV_NEED_RESET)) {
+			ha->flags.nic_core_reset_owner = 0;
+			qla8044_device_bootstrap(vha);
+			return;
 		}
+	}
 
-		/*
-		* Clear RESET OWNER, will be set at next reset
-		* by next RST_OWNER
-		*/
+	/* Exit if non active function */
+	if (!(drv_active & (1 << ha->portnum))) {
 		ha->flags.nic_core_reset_owner = 0;
+		return;
+	}
 
-		/* Start Reset Recovery */
+	/*
+	 * Execute Reset Recovery if Reset Owner or Function 7
+	 * is the only active function
+	 */
+	if (ha->flags.nic_core_reset_owner ||
+	    ((drv_state & drv_active) == QLA8044_FUN7_ACTIVE_INDEX)) {
+		ha->flags.nic_core_reset_owner = 0;
 		qla8044_device_bootstrap(vha);
 	}
 }
@@ -1655,6 +1741,19 @@ qla8044_set_drv_active(struct scsi_qla_host *vha)
 	qla8044_wr_direct(vha, QLA8044_CRB_DRV_ACTIVE_INDEX, drv_active);
 }
 
+static int
+qla8044_check_drv_active(struct scsi_qla_host *vha)
+{
+	uint32_t drv_active;
+	struct qla_hw_data *ha = vha->hw;
+
+	drv_active = qla8044_rd_direct(vha, QLA8044_CRB_DRV_ACTIVE_INDEX);
+	if (drv_active & (1 << ha->portnum))
+		return QLA_SUCCESS;
+	else
+		return QLA_TEST_FAILED;
+}
+
 static void
 qla8044_clear_idc_dontreset(struct scsi_qla_host *vha)
 {
@@ -1837,14 +1936,16 @@ qla8044_device_state_handler(struct scsi_qla_host *vha)
 
 	while (1) {
 		if (time_after_eq(jiffies, dev_init_timeout)) {
-			ql_log(ql_log_warn, vha, 0xb0cf,
-			    "%s: Device Init Failed 0x%x = %s\n",
-			    QLA2XXX_DRIVER_NAME, dev_state,
-			    dev_state < MAX_STATES ?
-			    qdev_state(dev_state) : "Unknown");
-
-			qla8044_wr_direct(vha, QLA8044_CRB_DEV_STATE_INDEX,
-			    QLA8XXX_DEV_FAILED);
+			if (qla8044_check_drv_active(vha) == QLA_SUCCESS) {
+				ql_log(ql_log_warn, vha, 0xb0cf,
+				    "%s: Device Init Failed 0x%x = %s\n",
+				    QLA2XXX_DRIVER_NAME, dev_state,
+				    dev_state < MAX_STATES ?
+				    qdev_state(dev_state) : "Unknown");
+				qla8044_wr_direct(vha,
+				    QLA8044_CRB_DEV_STATE_INDEX,
+				    QLA8XXX_DEV_FAILED);
+			}
 		}
 
 		dev_state = qla8044_rd_direct(vha, QLA8044_CRB_DEV_STATE_INDEX);
@@ -2017,6 +2118,13 @@ qla8044_watchdog(struct scsi_qla_host *vha)
 	    test_bit(FCOE_CTX_RESET_NEEDED, &vha->dpc_flags))) {
 		dev_state = qla8044_rd_direct(vha, QLA8044_CRB_DEV_STATE_INDEX);
 
+		if (qla8044_check_fw_alive(vha)) {
+			ha->flags.isp82xx_fw_hung = 1;
+			ql_log(ql_log_warn, vha, 0xb10a,
+			    "Firmware hung.\n");
+			qla82xx_clear_pending_mbx(vha);
+		}
+
 		if (qla8044_check_temp(vha)) {
 			set_bit(ISP_UNRECOVERABLE, &vha->dpc_flags);
 			ha->flags.isp82xx_fw_hung = 1;
@@ -2037,7 +2145,7 @@ qla8044_watchdog(struct scsi_qla_host *vha)
 			qla2xxx_wake_dpc(vha);
 		} else  {
 			/* Check firmware health */
-			if (qla8044_check_fw_alive(vha)) {
+			if (ha->flags.isp82xx_fw_hung) {
 				halt_status = qla8044_rd_direct(vha,
 					QLA8044_PEG_HALT_STATUS1_INDEX);
 				if (halt_status &
@@ -2073,12 +2181,8 @@ qla8044_watchdog(struct scsi_qla_host *vha)
 						    __func__);
 						set_bit(ISP_ABORT_NEEDED,
 						    &vha->dpc_flags);
-						qla82xx_clear_pending_mbx(vha);
 					}
 				}
-				ha->flags.isp82xx_fw_hung = 1;
-				ql_log(ql_log_warn, vha, 0xb10a,
-				    "Firmware hung.\n");
 				qla2xxx_wake_dpc(vha);
 			}
 		}
@@ -2286,8 +2390,6 @@ qla8044_minidump_process_rdmem(struct scsi_qla_host *vha,
 		}
 
 		if (j >= MAX_CTL_CHECK) {
-			printk_ratelimited(KERN_ERR
-			    "%s: failed to read through agent\n", __func__);
 			write_unlock_irqrestore(&ha->hw_lock, flags);
 			return QLA_SUCCESS;
 		}
@@ -2882,6 +2984,231 @@ error_exit:
 	return rval;
 }
 
+static uint32_t
+qla8044_minidump_process_rddfe(struct scsi_qla_host *vha,
+	struct qla8044_minidump_entry_hdr *entry_hdr, uint32_t **d_ptr)
+{
+	int loop_cnt;
+	uint32_t addr1, addr2, value, data, temp, wrVal;
+	uint8_t stride, stride2;
+	uint16_t count;
+	uint32_t poll, mask, data_size, modify_mask;
+	uint32_t wait_count = 0;
+
+	uint32_t *data_ptr = *d_ptr;
+
+	struct qla8044_minidump_entry_rddfe *rddfe;
+	rddfe = (struct qla8044_minidump_entry_rddfe *) entry_hdr;
+
+	addr1 = rddfe->addr_1;
+	value = rddfe->value;
+	stride = rddfe->stride;
+	stride2 = rddfe->stride2;
+	count = rddfe->count;
+
+	poll = rddfe->poll;
+	mask = rddfe->mask;
+	modify_mask = rddfe->modify_mask;
+	data_size = rddfe->data_size;
+
+	addr2 = addr1 + stride;
+
+	for (loop_cnt = 0x0; loop_cnt < count; loop_cnt++) {
+		qla8044_wr_reg_indirect(vha, addr1, (0x40000000 | value));
+
+		wait_count = 0;
+		while (wait_count < poll) {
+			qla8044_rd_reg_indirect(vha, addr1, &temp);
+			if ((temp & mask) != 0)
+				break;
+			wait_count++;
+		}
+
+		if (wait_count == poll) {
+			ql_log(ql_log_warn, vha, 0xb153,
+			    "%s: TIMEOUT\n", __func__);
+			goto error;
+		} else {
+			qla8044_rd_reg_indirect(vha, addr2, &temp);
+			temp = temp & modify_mask;
+			temp = (temp | ((loop_cnt << 16) | loop_cnt));
+			wrVal = ((temp << 16) | temp);
+
+			qla8044_wr_reg_indirect(vha, addr2, wrVal);
+			qla8044_wr_reg_indirect(vha, addr1, value);
+
+			wait_count = 0;
+			while (wait_count < poll) {
+				qla8044_rd_reg_indirect(vha, addr1, &temp);
+				if ((temp & mask) != 0)
+					break;
+				wait_count++;
+			}
+			if (wait_count == poll) {
+				ql_log(ql_log_warn, vha, 0xb154,
+				    "%s: TIMEOUT\n", __func__);
+				goto error;
+			}
+
+			qla8044_wr_reg_indirect(vha, addr1,
+			    ((0x40000000 | value) + stride2));
+			wait_count = 0;
+			while (wait_count < poll) {
+				qla8044_rd_reg_indirect(vha, addr1, &temp);
+				if ((temp & mask) != 0)
+					break;
+				wait_count++;
+			}
+
+			if (wait_count == poll) {
+				ql_log(ql_log_warn, vha, 0xb155,
+				    "%s: TIMEOUT\n", __func__);
+				goto error;
+			}
+
+			qla8044_rd_reg_indirect(vha, addr2, &data);
+
+			*data_ptr++ = wrVal;
+			*data_ptr++ = data;
+		}
+
+	}
+
+	*d_ptr = data_ptr;
+	return QLA_SUCCESS;
+
+error:
+	return -1;
+
+}
+
+static uint32_t
+qla8044_minidump_process_rdmdio(struct scsi_qla_host *vha,
+	struct qla8044_minidump_entry_hdr *entry_hdr, uint32_t **d_ptr)
+{
+	int ret = 0;
+	uint32_t addr1, addr2, value1, value2, data, selVal;
+	uint8_t stride1, stride2;
+	uint32_t addr3, addr4, addr5, addr6, addr7;
+	uint16_t count, loop_cnt;
+	uint32_t poll, mask;
+	uint32_t *data_ptr = *d_ptr;
+
+	struct qla8044_minidump_entry_rdmdio *rdmdio;
+
+	rdmdio = (struct qla8044_minidump_entry_rdmdio *) entry_hdr;
+
+	addr1 = rdmdio->addr_1;
+	addr2 = rdmdio->addr_2;
+	value1 = rdmdio->value_1;
+	stride1 = rdmdio->stride_1;
+	stride2 = rdmdio->stride_2;
+	count = rdmdio->count;
+
+	poll = rdmdio->poll;
+	mask = rdmdio->mask;
+	value2 = rdmdio->value_2;
+
+	addr3 = addr1 + stride1;
+
+	for (loop_cnt = 0; loop_cnt < count; loop_cnt++) {
+		ret = qla8044_poll_wait_ipmdio_bus_idle(vha, addr1, addr2,
+		    addr3, mask);
+		if (ret == -1)
+			goto error;
+
+		addr4 = addr2 - stride1;
+		ret = qla8044_ipmdio_wr_reg(vha, addr1, addr3, mask, addr4,
+		    value2);
+		if (ret == -1)
+			goto error;
+
+		addr5 = addr2 - (2 * stride1);
+		ret = qla8044_ipmdio_wr_reg(vha, addr1, addr3, mask, addr5,
+		    value1);
+		if (ret == -1)
+			goto error;
+
+		addr6 = addr2 - (3 * stride1);
+		ret = qla8044_ipmdio_wr_reg(vha, addr1, addr3, mask,
+		    addr6, 0x2);
+		if (ret == -1)
+			goto error;
+
+		ret = qla8044_poll_wait_ipmdio_bus_idle(vha, addr1, addr2,
+		    addr3, mask);
+		if (ret == -1)
+			goto error;
+
+		addr7 = addr2 - (4 * stride1);
+			data = qla8044_ipmdio_rd_reg(vha, addr1, addr3,
+			    mask, addr7);
+		if (data == -1)
+			goto error;
+
+		selVal = (value2 << 18) | (value1 << 2) | 2;
+
+		stride2 = rdmdio->stride_2;
+		*data_ptr++ = selVal;
+		*data_ptr++ = data;
+
+		value1 = value1 + stride2;
+		*d_ptr = data_ptr;
+	}
+
+	return 0;
+
+error:
+	return -1;
+}
+
+static uint32_t qla8044_minidump_process_pollwr(struct scsi_qla_host *vha,
+		struct qla8044_minidump_entry_hdr *entry_hdr, uint32_t **d_ptr)
+{
+	uint32_t addr1, addr2, value1, value2, poll, mask, r_value;
+	uint32_t wait_count = 0;
+	struct qla8044_minidump_entry_pollwr *pollwr_hdr;
+
+	pollwr_hdr = (struct qla8044_minidump_entry_pollwr *)entry_hdr;
+	addr1 = pollwr_hdr->addr_1;
+	addr2 = pollwr_hdr->addr_2;
+	value1 = pollwr_hdr->value_1;
+	value2 = pollwr_hdr->value_2;
+
+	poll = pollwr_hdr->poll;
+	mask = pollwr_hdr->mask;
+
+	while (wait_count < poll) {
+		qla8044_rd_reg_indirect(vha, addr1, &r_value);
+
+		if ((r_value & poll) != 0)
+			break;
+		wait_count++;
+	}
+
+	if (wait_count == poll) {
+		ql_log(ql_log_warn, vha, 0xb156, "%s: TIMEOUT\n", __func__);
+		goto error;
+	}
+
+	qla8044_wr_reg_indirect(vha, addr2, value2);
+	qla8044_wr_reg_indirect(vha, addr1, value1);
+
+	wait_count = 0;
+	while (wait_count < poll) {
+		qla8044_rd_reg_indirect(vha, addr1, &r_value);
+
+		if ((r_value & poll) != 0)
+			break;
+		wait_count++;
+	}
+
+	return QLA_SUCCESS;
+
+error:
+	return -1;
+}
+
 /*
  *
  * qla8044_collect_md_data - Retrieve firmware minidump data.
@@ -3089,6 +3416,24 @@ qla8044_collect_md_data(struct scsi_qla_host *vha)
 			if (rval != QLA_SUCCESS)
 				qla8044_mark_entry_skipped(vha, entry_hdr, i);
 			break;
+		case QLA8044_RDDFE:
+			rval = qla8044_minidump_process_rddfe(vha, entry_hdr,
+			    &data_ptr);
+			if (rval != QLA_SUCCESS)
+				qla8044_mark_entry_skipped(vha, entry_hdr, i);
+			break;
+		case QLA8044_RDMDIO:
+			rval = qla8044_minidump_process_rdmdio(vha, entry_hdr,
+			    &data_ptr);
+			if (rval != QLA_SUCCESS)
+				qla8044_mark_entry_skipped(vha, entry_hdr, i);
+			break;
+		case QLA8044_POLLWR:
+			rval = qla8044_minidump_process_pollwr(vha, entry_hdr,
+			    &data_ptr);
+			if (rval != QLA_SUCCESS)
+				qla8044_mark_entry_skipped(vha, entry_hdr, i);
+			break;
 		case QLA82XX_RDNOP:
 		default:
 			qla8044_mark_entry_skipped(vha, entry_hdr, i);
@@ -3110,6 +3455,7 @@ skip_nxt_entry:
 		    "Dump data mismatch: Data collected: "
 		    "[0x%x], total_data_size:[0x%x]\n",
 		    data_collected, ha->md_dump_size);
+		rval = QLA_FUNCTION_FAILED;
 		goto md_failed;
 	}
 
@@ -3134,10 +3480,12 @@ qla8044_get_minidump(struct scsi_qla_host *vha)
 
 	if (!qla8044_collect_md_data(vha)) {
 		ha->fw_dumped = 1;
+		ha->prev_minidump_failed = 0;
 	} else {
 		ql_log(ql_log_fatal, vha, 0xb0db,
 		    "%s: Unable to collect minidump\n",
 		    __func__);
+		ha->prev_minidump_failed = 1;
 	}
 }
 
diff --git a/drivers/scsi/qla2xxx/qla_nx2.h b/drivers/scsi/qla2xxx/qla_nx2.h
index 2ab2eabab90..ada36057d7c 100644
--- a/drivers/scsi/qla2xxx/qla_nx2.h
+++ b/drivers/scsi/qla2xxx/qla_nx2.h
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
@@ -133,6 +133,7 @@
 #define QLA8044_LINK_SPEED(f)		(0x36E0+(((f) >> 2) * 4))
 #define QLA8044_MAX_LINK_SPEED(f)       (0x36F0+(((f) / 4) * 4))
 #define QLA8044_LINK_SPEED_FACTOR	10
+#define QLA8044_FUN7_ACTIVE_INDEX	0x80
 
 /* FLASH API Defines */
 #define QLA8044_FLASH_MAX_WAIT_USEC	100
@@ -431,6 +432,50 @@ struct qla8044_minidump_entry_pollrd {
 	uint32_t rsvd_1;
 } __packed;
 
+struct qla8044_minidump_entry_rddfe {
+	struct qla8044_minidump_entry_hdr h;
+	uint32_t addr_1;
+	uint32_t value;
+	uint8_t stride;
+	uint8_t stride2;
+	uint16_t count;
+	uint32_t poll;
+	uint32_t mask;
+	uint32_t modify_mask;
+	uint32_t data_size;
+	uint32_t rsvd;
+
+} __packed;
+
+struct qla8044_minidump_entry_rdmdio {
+	struct qla8044_minidump_entry_hdr h;
+
+	uint32_t addr_1;
+	uint32_t addr_2;
+	uint32_t value_1;
+	uint8_t stride_1;
+	uint8_t stride_2;
+	uint16_t count;
+	uint32_t poll;
+	uint32_t mask;
+	uint32_t value_2;
+	uint32_t data_size;
+
+} __packed;
+
+struct qla8044_minidump_entry_pollwr {
+	struct qla8044_minidump_entry_hdr h;
+	uint32_t addr_1;
+	uint32_t addr_2;
+	uint32_t value_1;
+	uint32_t value_2;
+	uint32_t poll;
+	uint32_t mask;
+	uint32_t data_size;
+	uint32_t rsvd;
+
+}  __packed;
+
 /* RDMUX2 Entry */
 struct qla8044_minidump_entry_rdmux2 {
 	struct qla8044_minidump_entry_hdr h;
@@ -516,6 +561,9 @@ static const uint32_t qla8044_reg_tbl[] = {
 #define QLA8044_DBG_RSVD_ARRAY_LEN              8
 #define QLA8044_DBG_OCM_WNDREG_ARRAY_LEN        16
 #define QLA8044_SS_PCI_INDEX                    0
+#define QLA8044_RDDFE          38
+#define QLA8044_RDMDIO         39
+#define QLA8044_POLLWR         40
 
 struct qla8044_minidump_template_hdr {
 	uint32_t entry_type;
diff --git a/drivers/scsi/qla2xxx/qla_os.c b/drivers/scsi/qla2xxx/qla_os.c
index 19e99cc3372..d96bfb55e57 100644
--- a/drivers/scsi/qla2xxx/qla_os.c
+++ b/drivers/scsi/qla2xxx/qla_os.c
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
@@ -616,7 +616,7 @@ qla2x00_sp_free_dma(void *vha, void *ptr)
 
 	if (sp->flags & SRB_CRC_CTX_DSD_VALID) {
 		/* List assured to be having elements */
-		qla2x00_clean_dsd_pool(ha, sp);
+		qla2x00_clean_dsd_pool(ha, sp, NULL);
 		sp->flags &= ~SRB_CRC_CTX_DSD_VALID;
 	}
 
@@ -781,7 +781,7 @@ static int
 qla2x00_eh_wait_on_command(struct scsi_cmnd *cmd)
 {
 #define ABORT_POLLING_PERIOD	1000
-#define ABORT_WAIT_ITER		((10 * 1000) / (ABORT_POLLING_PERIOD))
+#define ABORT_WAIT_ITER		((2 * 1000) / (ABORT_POLLING_PERIOD))
 	unsigned long wait_iter = ABORT_WAIT_ITER;
 	scsi_qla_host_t *vha = shost_priv(cmd->device->host);
 	struct qla_hw_data *ha = vha->hw;
@@ -844,11 +844,8 @@ qla2x00_wait_for_hba_online(scsi_qla_host_t *vha)
 }
 
 /*
- * qla2x00_wait_for_reset_ready
- *    Wait till the HBA is online after going through
- *    <= MAX_RETRIES_OF_ISP_ABORT  or
- *    finally HBA is disabled ie marked offline or flash
- *    operations are in progress.
+ * qla2x00_wait_for_hba_ready
+ * Wait till the HBA is ready before doing driver unload
  *
  * Input:
  *     ha - pointer to host adapter structure
@@ -857,35 +854,15 @@ qla2x00_wait_for_hba_online(scsi_qla_host_t *vha)
  *    Does context switching-Release SPIN_LOCK
  *    (if any) before calling this routine.
  *
- * Return:
- *    Success (Adapter is online/no flash ops) : 0
- *    Failed  (Adapter is offline/disabled/flash ops in progress) : 1
  */
-static int
-qla2x00_wait_for_reset_ready(scsi_qla_host_t *vha)
+static void
+qla2x00_wait_for_hba_ready(scsi_qla_host_t *vha)
 {
-	int		return_status;
-	unsigned long	wait_online;
 	struct qla_hw_data *ha = vha->hw;
-	scsi_qla_host_t *base_vha = pci_get_drvdata(ha->pdev);
 
-	wait_online = jiffies + (MAX_LOOP_TIMEOUT * HZ);
-	while (((test_bit(ISP_ABORT_NEEDED, &base_vha->dpc_flags)) ||
-	    test_bit(ABORT_ISP_ACTIVE, &base_vha->dpc_flags) ||
-	    test_bit(ISP_ABORT_RETRY, &base_vha->dpc_flags) ||
-	    ha->optrom_state != QLA_SWAITING ||
-	    ha->dpc_active) && time_before(jiffies, wait_online))
+	while ((!(vha->flags.online) || ha->dpc_active ||
+	    ha->flags.mbox_busy))
 		msleep(1000);
-
-	if (base_vha->flags.online &&  ha->optrom_state == QLA_SWAITING)
-		return_status = QLA_SUCCESS;
-	else
-		return_status = QLA_FUNCTION_FAILED;
-
-	ql_dbg(ql_dbg_taskm, vha, 0x8019,
-	    "%s return status=%d.\n", __func__, return_status);
-
-	return return_status;
 }
 
 int
@@ -945,7 +922,7 @@ qla2xxx_eh_abort(struct scsi_cmnd *cmd)
 	int ret;
 	unsigned int id, lun;
 	unsigned long flags;
-	int wait = 0;
+	int rval, wait = 0;
 	struct qla_hw_data *ha = vha->hw;
 
 	if (!CMD_SP(cmd))
@@ -974,10 +951,20 @@ qla2xxx_eh_abort(struct scsi_cmnd *cmd)
 	sp_get(sp);
 
 	spin_unlock_irqrestore(&ha->hardware_lock, flags);
-	if (ha->isp_ops->abort_command(sp)) {
-		ret = FAILED;
+	rval = ha->isp_ops->abort_command(sp);
+	if (rval) {
+		if (rval == QLA_FUNCTION_PARAMETER_ERROR) {
+			/*
+			 * Decrement the ref_count since we can't find the
+			 * command
+			 */
+			atomic_dec(&sp->ref_count);
+			ret = SUCCESS;
+		} else
+			ret = FAILED;
+
 		ql_dbg(ql_dbg_taskm, vha, 0x8003,
-		    "Abort command mbx failed cmd=%p.\n", cmd);
+		    "Abort command mbx failed cmd=%p, rval=%x.\n", cmd, rval);
 	} else {
 		ql_dbg(ql_dbg_taskm, vha, 0x8004,
 		    "Abort command mbx success cmd=%p.\n", cmd);
@@ -985,6 +972,12 @@ qla2xxx_eh_abort(struct scsi_cmnd *cmd)
 	}
 
 	spin_lock_irqsave(&ha->hardware_lock, flags);
+	/*
+	 * Clear the slot in the oustanding_cmds array if we can't find the
+	 * command to reclaim the resources.
+	 */
+	if (rval == QLA_FUNCTION_PARAMETER_ERROR)
+		vha->req->outstanding_cmds[sp->handle] = NULL;
 	sp->done(ha, sp, 0);
 	spin_unlock_irqrestore(&ha->hardware_lock, flags);
 
@@ -1236,7 +1229,11 @@ qla2xxx_eh_host_reset(struct scsi_cmnd *cmd)
 	ql_log(ql_log_info, vha, 0x8018,
 	    "ADAPTER RESET ISSUED nexus=%ld:%d:%d.\n", vha->host_no, id, lun);
 
-	if (qla2x00_wait_for_reset_ready(vha) != QLA_SUCCESS)
+	/*
+	 * No point in issuing another reset if one is active.  Also do not
+	 * attempt a reset if we are updating flash.
+	 */
+	if (qla2x00_reset_active(vha) || ha->optrom_state != QLA_SWAITING)
 		goto eh_host_reset_lock;
 
 	if (vha != base_vha) {
@@ -2270,6 +2267,13 @@ qla2x00_set_isp_flags(struct qla_hw_data *ha)
 		ha->device_type |= DT_IIDMA;
 		ha->fw_srisc_address = RISC_START_ADDRESS_2400;
 		break;
+	case PCI_DEVICE_ID_QLOGIC_ISP2271:
+		ha->device_type |= DT_ISP2271;
+		ha->device_type |= DT_ZIO_SUPPORTED;
+		ha->device_type |= DT_FWI2;
+		ha->device_type |= DT_IIDMA;
+		ha->fw_srisc_address = RISC_START_ADDRESS_2400;
+		break;
 	}
 
 	if (IS_QLA82XX(ha))
@@ -2346,7 +2350,8 @@ qla2x00_probe_one(struct pci_dev *pdev, const struct pci_device_id *id)
 	    pdev->device == PCI_DEVICE_ID_QLOGIC_ISP8031 ||
 	    pdev->device == PCI_DEVICE_ID_QLOGIC_ISPF001 ||
 	    pdev->device == PCI_DEVICE_ID_QLOGIC_ISP8044 ||
-	    pdev->device == PCI_DEVICE_ID_QLOGIC_ISP2071) {
+	    pdev->device == PCI_DEVICE_ID_QLOGIC_ISP2071 ||
+	    pdev->device == PCI_DEVICE_ID_QLOGIC_ISP2271) {
 		bars = pci_select_bars(pdev, IORESOURCE_MEM);
 		mem_only = 1;
 		ql_dbg_pci(ql_dbg_init, pdev, 0x0007,
@@ -2877,6 +2882,7 @@ skip_dpc:
 
 	base_vha->flags.init_done = 1;
 	base_vha->flags.online = 1;
+	ha->prev_minidump_failed = 0;
 
 	ql_dbg(ql_dbg_init, base_vha, 0x00f2,
 	    "Init done and hba is online.\n");
@@ -3136,6 +3142,8 @@ qla2x00_remove_one(struct pci_dev *pdev)
 	base_vha = pci_get_drvdata(pdev);
 	ha = base_vha->hw;
 
+	qla2x00_wait_for_hba_ready(base_vha);
+
 	set_bit(UNLOADING, &base_vha->dpc_flags);
 
 	if (IS_QLAFX00(ha))
@@ -3645,6 +3653,7 @@ qla2x00_free_fw_dump(struct qla_hw_data *ha)
 	ha->eft = NULL;
 	ha->eft_dma = 0;
 	ha->fw_dumped = 0;
+	ha->fw_dump_cap_flags = 0;
 	ha->fw_dump_reading = 0;
 	ha->fw_dump = NULL;
 	ha->fw_dump_len = 0;
@@ -4828,7 +4837,7 @@ qla2x00_do_dpc(void *data)
 	ha = (struct qla_hw_data *)data;
 	base_vha = pci_get_drvdata(ha->pdev);
 
-	set_user_nice(current, -20);
+	set_user_nice(current, MIN_NICE);
 
 	set_current_state(TASK_INTERRUPTIBLE);
 	while (!kthread_should_stop()) {
@@ -4913,12 +4922,13 @@ qla2x00_do_dpc(void *data)
 				if (qlafx00_reset_initialize(base_vha)) {
 					/* Failed. Abort isp later. */
 					if (!test_bit(UNLOADING,
-					    &base_vha->dpc_flags))
+					    &base_vha->dpc_flags)) {
 						set_bit(ISP_UNRECOVERABLE,
 						    &base_vha->dpc_flags);
 						ql_dbg(ql_dbg_dpc, base_vha,
 						    0x4021,
 						    "Reset Recovery Failed\n");
+					}
 				}
 			}
 
@@ -5077,8 +5087,10 @@ intr_on_check:
 			ha->isp_ops->enable_intrs(ha);
 
 		if (test_and_clear_bit(BEACON_BLINK_NEEDED,
-					&base_vha->dpc_flags))
-			ha->isp_ops->beacon_blink(base_vha);
+					&base_vha->dpc_flags)) {
+			if (ha->beacon_blink_led == 1)
+				ha->isp_ops->beacon_blink(base_vha);
+		}
 
 		if (!IS_QLAFX00(ha))
 			qla2x00_do_dpc_all_vps(base_vha);
@@ -5325,7 +5337,7 @@ qla2x00_timer(scsi_qla_host_t *vha)
 #define FW_ISP82XX	7
 #define FW_ISP2031	8
 #define FW_ISP8031	9
-#define FW_ISP2071	10
+#define FW_ISP27XX	10
 
 #define FW_FILE_ISP21XX	"ql2100_fw.bin"
 #define FW_FILE_ISP22XX	"ql2200_fw.bin"
@@ -5337,7 +5349,7 @@ qla2x00_timer(scsi_qla_host_t *vha)
 #define FW_FILE_ISP82XX	"ql8200_fw.bin"
 #define FW_FILE_ISP2031	"ql2600_fw.bin"
 #define FW_FILE_ISP8031	"ql8300_fw.bin"
-#define FW_FILE_ISP2071	"ql2700_fw.bin"
+#define FW_FILE_ISP27XX	"ql2700_fw.bin"
 
 
 static DEFINE_MUTEX(qla_fw_lock);
@@ -5353,7 +5365,7 @@ static struct fw_blob qla_fw_blobs[FW_BLOBS] = {
 	{ .name = FW_FILE_ISP82XX, },
 	{ .name = FW_FILE_ISP2031, },
 	{ .name = FW_FILE_ISP8031, },
-	{ .name = FW_FILE_ISP2071, },
+	{ .name = FW_FILE_ISP27XX, },
 };
 
 struct fw_blob *
@@ -5382,8 +5394,8 @@ qla2x00_request_firmware(scsi_qla_host_t *vha)
 		blob = &qla_fw_blobs[FW_ISP2031];
 	} else if (IS_QLA8031(ha)) {
 		blob = &qla_fw_blobs[FW_ISP8031];
-	} else if (IS_QLA2071(ha)) {
-		blob = &qla_fw_blobs[FW_ISP2071];
+	} else if (IS_QLA27XX(ha)) {
+		blob = &qla_fw_blobs[FW_ISP27XX];
 	} else {
 		return NULL;
 	}
@@ -5714,6 +5726,7 @@ static struct pci_device_id qla2xxx_pci_tbl[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_QLOGIC, PCI_DEVICE_ID_QLOGIC_ISPF001) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_QLOGIC, PCI_DEVICE_ID_QLOGIC_ISP8044) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_QLOGIC, PCI_DEVICE_ID_QLOGIC_ISP2071) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_QLOGIC, PCI_DEVICE_ID_QLOGIC_ISP2271) },
 	{ 0 },
 };
 MODULE_DEVICE_TABLE(pci, qla2xxx_pci_tbl);
diff --git a/drivers/scsi/qla2xxx/qla_settings.h b/drivers/scsi/qla2xxx/qla_settings.h
index 46ef0ac48f4..2fb7ebfbbc3 100644
--- a/drivers/scsi/qla2xxx/qla_settings.h
+++ b/drivers/scsi/qla2xxx/qla_settings.h
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
diff --git a/drivers/scsi/qla2xxx/qla_sup.c b/drivers/scsi/qla2xxx/qla_sup.c
index f28123e8ed6..bca173e56f1 100644
--- a/drivers/scsi/qla2xxx/qla_sup.c
+++ b/drivers/scsi/qla2xxx/qla_sup.c
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
@@ -1727,11 +1727,8 @@ qla83xx_beacon_blink(struct scsi_qla_host *vha)
 	if (IS_QLA2031(ha)) {
 		led_select_value = qla83xx_select_led_port(ha);
 
-		qla83xx_wr_reg(vha, led_select_value, 0x40002000);
-		qla83xx_wr_reg(vha, led_select_value + 4, 0x40002000);
-		msleep(1000);
-		qla83xx_wr_reg(vha, led_select_value, 0x40004000);
-		qla83xx_wr_reg(vha, led_select_value + 4, 0x40004000);
+		qla83xx_wr_reg(vha, led_select_value, 0x40000230);
+		qla83xx_wr_reg(vha, led_select_value + 4, 0x40000230);
 	} else if (IS_QLA8031(ha)) {
 		led_select_value = qla83xx_select_led_port(ha);
 
diff --git a/drivers/scsi/qla2xxx/qla_target.c b/drivers/scsi/qla2xxx/qla_target.c
index 0cb73074c19..b1d10f9935c 100644
--- a/drivers/scsi/qla2xxx/qla_target.c
+++ b/drivers/scsi/qla2xxx/qla_target.c
@@ -182,6 +182,11 @@ struct scsi_qla_host *qlt_find_host_by_vp_idx(struct scsi_qla_host *vha,
 void qlt_24xx_atio_pkt_all_vps(struct scsi_qla_host *vha,
 	struct atio_from_isp *atio)
 {
+	ql_dbg(ql_dbg_tgt, vha, 0xe072,
+		"%s: qla_target(%d): type %x ox_id %04x\n",
+		__func__, vha->vp_idx, atio->u.raw.entry_type,
+		be16_to_cpu(atio->u.isp24.fcp_hdr.ox_id));
+
 	switch (atio->u.raw.entry_type) {
 	case ATIO_TYPE7:
 	{
@@ -236,6 +241,10 @@ void qlt_24xx_atio_pkt_all_vps(struct scsi_qla_host *vha,
 void qlt_response_pkt_all_vps(struct scsi_qla_host *vha, response_t *pkt)
 {
 	switch (pkt->entry_type) {
+	case CTIO_CRC2:
+		ql_dbg(ql_dbg_tgt, vha, 0xe073,
+			"qla_target(%d):%s: CRC2 Response pkt\n",
+			vha->vp_idx, __func__);
 	case CTIO_TYPE7:
 	{
 		struct ctio7_from_24xx *entry = (struct ctio7_from_24xx *)pkt;
@@ -1350,13 +1359,42 @@ static int qlt_pci_map_calc_cnt(struct qla_tgt_prm *prm)
 
 	prm->cmd->sg_mapped = 1;
 
-	/*
-	 * If greater than four sg entries then we need to allocate
-	 * the continuation entries
-	 */
-	if (prm->seg_cnt > prm->tgt->datasegs_per_cmd)
-		prm->req_cnt += DIV_ROUND_UP(prm->seg_cnt -
-		    prm->tgt->datasegs_per_cmd, prm->tgt->datasegs_per_cont);
+	if (cmd->se_cmd.prot_op == TARGET_PROT_NORMAL) {
+		/*
+		 * If greater than four sg entries then we need to allocate
+		 * the continuation entries
+		 */
+		if (prm->seg_cnt > prm->tgt->datasegs_per_cmd)
+			prm->req_cnt += DIV_ROUND_UP(prm->seg_cnt -
+			prm->tgt->datasegs_per_cmd,
+			prm->tgt->datasegs_per_cont);
+	} else {
+		/* DIF */
+		if ((cmd->se_cmd.prot_op == TARGET_PROT_DIN_INSERT) ||
+		    (cmd->se_cmd.prot_op == TARGET_PROT_DOUT_STRIP)) {
+			prm->seg_cnt = DIV_ROUND_UP(cmd->bufflen, cmd->blk_sz);
+			prm->tot_dsds = prm->seg_cnt;
+		} else
+			prm->tot_dsds = prm->seg_cnt;
+
+		if (cmd->prot_sg_cnt) {
+			prm->prot_sg      = cmd->prot_sg;
+			prm->prot_seg_cnt = pci_map_sg(prm->tgt->ha->pdev,
+				cmd->prot_sg, cmd->prot_sg_cnt,
+				cmd->dma_data_direction);
+			if (unlikely(prm->prot_seg_cnt == 0))
+				goto out_err;
+
+			if ((cmd->se_cmd.prot_op == TARGET_PROT_DIN_INSERT) ||
+			    (cmd->se_cmd.prot_op == TARGET_PROT_DOUT_STRIP)) {
+				/* Dif Bundling not support here */
+				prm->prot_seg_cnt = DIV_ROUND_UP(cmd->bufflen,
+								cmd->blk_sz);
+				prm->tot_dsds += prm->prot_seg_cnt;
+			} else
+				prm->tot_dsds += prm->prot_seg_cnt;
+		}
+	}
 
 	ql_dbg(ql_dbg_tgt, prm->cmd->vha, 0xe009, "seg_cnt=%d, req_cnt=%d\n",
 	    prm->seg_cnt, prm->req_cnt);
@@ -1377,6 +1415,16 @@ static inline void qlt_unmap_sg(struct scsi_qla_host *vha,
 	BUG_ON(!cmd->sg_mapped);
 	pci_unmap_sg(ha->pdev, cmd->sg, cmd->sg_cnt, cmd->dma_data_direction);
 	cmd->sg_mapped = 0;
+
+	if (cmd->prot_sg_cnt)
+		pci_unmap_sg(ha->pdev, cmd->prot_sg, cmd->prot_sg_cnt,
+			cmd->dma_data_direction);
+
+	if (cmd->ctx_dsd_alloced)
+		qla2x00_clean_dsd_pool(ha, NULL, cmd);
+
+	if (cmd->ctx)
+		dma_pool_free(ha->dl_dma_pool, cmd->ctx, cmd->ctx->crc_ctx_dma);
 }
 
 static int qlt_check_reserve_free_req(struct scsi_qla_host *vha,
@@ -1665,8 +1713,9 @@ 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\n",
-	    vha->vp_idx, cmd->tag);
+	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;
@@ -1902,6 +1951,323 @@ skip_explict_conf:
 	/* Sense with len > 24, is it possible ??? */
 }
 
+
+
+/* diff  */
+static inline int
+qlt_hba_err_chk_enabled(struct se_cmd *se_cmd)
+{
+	/*
+	 * Uncomment when corresponding SCSI changes are done.
+	 *
+	 if (!sp->cmd->prot_chk)
+	 return 0;
+	 *
+	 */
+	switch (se_cmd->prot_op) {
+	case TARGET_PROT_DOUT_INSERT:
+	case TARGET_PROT_DIN_STRIP:
+		if (ql2xenablehba_err_chk >= 1)
+			return 1;
+		break;
+	case TARGET_PROT_DOUT_PASS:
+	case TARGET_PROT_DIN_PASS:
+		if (ql2xenablehba_err_chk >= 2)
+			return 1;
+		break;
+	case TARGET_PROT_DIN_INSERT:
+	case TARGET_PROT_DOUT_STRIP:
+		return 1;
+	default:
+		break;
+	}
+	return 0;
+}
+
+/*
+ * qla24xx_set_t10dif_tags_from_cmd - Extract Ref and App tags from SCSI command
+ *
+ */
+static inline void
+qlt_set_t10dif_tags(struct se_cmd *se_cmd, struct crc_context *ctx)
+{
+	uint32_t lba = 0xffffffff & se_cmd->t_task_lba;
+
+	/* wait til Mode Sense/Select cmd, modepage Ah, subpage 2
+	 * have been immplemented by TCM, before AppTag is avail.
+	 * Look for modesense_handlers[]
+	 */
+	ctx->app_tag = __constant_cpu_to_le16(0);
+	ctx->app_tag_mask[0] = 0x0;
+	ctx->app_tag_mask[1] = 0x0;
+
+	switch (se_cmd->prot_type) {
+	case TARGET_DIF_TYPE0_PROT:
+		/*
+		 * No check for ql2xenablehba_err_chk, as it would be an
+		 * I/O error if hba tag generation is not done.
+		 */
+		ctx->ref_tag = cpu_to_le32(lba);
+
+		if (!qlt_hba_err_chk_enabled(se_cmd))
+			break;
+
+		/* enable ALL bytes of the ref tag */
+		ctx->ref_tag_mask[0] = 0xff;
+		ctx->ref_tag_mask[1] = 0xff;
+		ctx->ref_tag_mask[2] = 0xff;
+		ctx->ref_tag_mask[3] = 0xff;
+		break;
+	/*
+	 * For TYpe 1 protection: 16 bit GUARD tag, 32 bit REF tag, and
+	 * 16 bit app tag.
+	 */
+	case TARGET_DIF_TYPE1_PROT:
+		ctx->ref_tag = cpu_to_le32(lba);
+
+		if (!qlt_hba_err_chk_enabled(se_cmd))
+			break;
+
+		/* enable ALL bytes of the ref tag */
+		ctx->ref_tag_mask[0] = 0xff;
+		ctx->ref_tag_mask[1] = 0xff;
+		ctx->ref_tag_mask[2] = 0xff;
+		ctx->ref_tag_mask[3] = 0xff;
+		break;
+	/*
+	 * For TYPE 2 protection: 16 bit GUARD + 32 bit REF tag has to
+	 * match LBA in CDB + N
+	 */
+	case TARGET_DIF_TYPE2_PROT:
+		ctx->ref_tag = cpu_to_le32(lba);
+
+		if (!qlt_hba_err_chk_enabled(se_cmd))
+			break;
+
+		/* enable ALL bytes of the ref tag */
+		ctx->ref_tag_mask[0] = 0xff;
+		ctx->ref_tag_mask[1] = 0xff;
+		ctx->ref_tag_mask[2] = 0xff;
+		ctx->ref_tag_mask[3] = 0xff;
+		break;
+
+	/* For Type 3 protection: 16 bit GUARD only */
+	case TARGET_DIF_TYPE3_PROT:
+		ctx->ref_tag_mask[0] = ctx->ref_tag_mask[1] =
+			ctx->ref_tag_mask[2] = ctx->ref_tag_mask[3] = 0x00;
+		break;
+	}
+}
+
+
+static inline int
+qlt_build_ctio_crc2_pkt(struct qla_tgt_prm *prm, scsi_qla_host_t *vha)
+{
+	uint32_t		*cur_dsd;
+	int			sgc;
+	uint32_t		transfer_length = 0;
+	uint32_t		data_bytes;
+	uint32_t		dif_bytes;
+	uint8_t			bundling = 1;
+	uint8_t			*clr_ptr;
+	struct crc_context	*crc_ctx_pkt = NULL;
+	struct qla_hw_data	*ha;
+	struct ctio_crc2_to_fw	*pkt;
+	dma_addr_t		crc_ctx_dma;
+	uint16_t		fw_prot_opts = 0;
+	struct qla_tgt_cmd	*cmd = prm->cmd;
+	struct se_cmd		*se_cmd = &cmd->se_cmd;
+	uint32_t h;
+	struct atio_from_isp *atio = &prm->cmd->atio;
+
+	sgc = 0;
+	ha = vha->hw;
+
+	pkt = (struct ctio_crc2_to_fw *)vha->req->ring_ptr;
+	prm->pkt = pkt;
+	memset(pkt, 0, sizeof(*pkt));
+
+	ql_dbg(ql_dbg_tgt, vha, 0xe071,
+		"qla_target(%d):%s: se_cmd[%p] CRC2 prot_op[0x%x] cmd prot sg:cnt[%p:%x] lba[%llu]\n",
+		vha->vp_idx, __func__, se_cmd, se_cmd->prot_op,
+		prm->prot_sg, prm->prot_seg_cnt, se_cmd->t_task_lba);
+
+	if ((se_cmd->prot_op == TARGET_PROT_DIN_INSERT) ||
+	    (se_cmd->prot_op == TARGET_PROT_DOUT_STRIP))
+		bundling = 0;
+
+	/* Compute dif len and adjust data len to incude protection */
+	data_bytes = cmd->bufflen;
+	dif_bytes  = (data_bytes / cmd->blk_sz) * 8;
+
+	switch (se_cmd->prot_op) {
+	case TARGET_PROT_DIN_INSERT:
+	case TARGET_PROT_DOUT_STRIP:
+		transfer_length = data_bytes;
+		data_bytes += dif_bytes;
+		break;
+
+	case TARGET_PROT_DIN_STRIP:
+	case TARGET_PROT_DOUT_INSERT:
+	case TARGET_PROT_DIN_PASS:
+	case TARGET_PROT_DOUT_PASS:
+		transfer_length = data_bytes + dif_bytes;
+		break;
+
+	default:
+		BUG();
+		break;
+	}
+
+	if (!qlt_hba_err_chk_enabled(se_cmd))
+		fw_prot_opts |= 0x10; /* Disable Guard tag checking */
+	/* HBA error checking enabled */
+	else if (IS_PI_UNINIT_CAPABLE(ha)) {
+		if ((se_cmd->prot_type == TARGET_DIF_TYPE1_PROT) ||
+		    (se_cmd->prot_type == TARGET_DIF_TYPE2_PROT))
+			fw_prot_opts |= PO_DIS_VALD_APP_ESC;
+		else if (se_cmd->prot_type == TARGET_DIF_TYPE3_PROT)
+			fw_prot_opts |= PO_DIS_VALD_APP_REF_ESC;
+	}
+
+	switch (se_cmd->prot_op) {
+	case TARGET_PROT_DIN_INSERT:
+	case TARGET_PROT_DOUT_INSERT:
+		fw_prot_opts |= PO_MODE_DIF_INSERT;
+		break;
+	case TARGET_PROT_DIN_STRIP:
+	case TARGET_PROT_DOUT_STRIP:
+		fw_prot_opts |= PO_MODE_DIF_REMOVE;
+		break;
+	case TARGET_PROT_DIN_PASS:
+	case TARGET_PROT_DOUT_PASS:
+		fw_prot_opts |= PO_MODE_DIF_PASS;
+		/* FUTURE: does tcm require T10CRC<->IPCKSUM conversion? */
+		break;
+	default:/* Normal Request */
+		fw_prot_opts |= PO_MODE_DIF_PASS;
+		break;
+	}
+
+
+	/* ---- PKT ---- */
+	/* Update entry type to indicate Command Type CRC_2 IOCB */
+	pkt->entry_type  = CTIO_CRC2;
+	pkt->entry_count = 1;
+	pkt->vp_index = vha->vp_idx;
+
+	h = qlt_make_handle(vha);
+	if (unlikely(h == QLA_TGT_NULL_HANDLE)) {
+		/*
+		 * CTIO type 7 from the firmware doesn't provide a way to
+		 * know the initiator's LOOP ID, hence we can't find
+		 * the session and, so, the command.
+		 */
+		return -EAGAIN;
+	} else
+		ha->tgt.cmds[h-1] = prm->cmd;
+
+
+	pkt->handle  = h | CTIO_COMPLETION_HANDLE_MARK;
+	pkt->nport_handle = prm->cmd->loop_id;
+	pkt->timeout = __constant_cpu_to_le16(QLA_TGT_TIMEOUT);
+	pkt->initiator_id[0] = atio->u.isp24.fcp_hdr.s_id[2];
+	pkt->initiator_id[1] = atio->u.isp24.fcp_hdr.s_id[1];
+	pkt->initiator_id[2] = atio->u.isp24.fcp_hdr.s_id[0];
+	pkt->exchange_addr   = atio->u.isp24.exchange_addr;
+	pkt->ox_id  = swab16(atio->u.isp24.fcp_hdr.ox_id);
+	pkt->flags |= (atio->u.isp24.attr << 9);
+	pkt->relative_offset = cpu_to_le32(prm->cmd->offset);
+
+	/* Set transfer direction */
+	if (cmd->dma_data_direction == DMA_TO_DEVICE)
+		pkt->flags = __constant_cpu_to_le16(CTIO7_FLAGS_DATA_IN);
+	else if (cmd->dma_data_direction == DMA_FROM_DEVICE)
+		pkt->flags = __constant_cpu_to_le16(CTIO7_FLAGS_DATA_OUT);
+
+
+	pkt->dseg_count = prm->tot_dsds;
+	/* Fibre channel byte count */
+	pkt->transfer_length = cpu_to_le32(transfer_length);
+
+
+	/* ----- CRC context -------- */
+
+	/* Allocate CRC context from global pool */
+	crc_ctx_pkt = cmd->ctx =
+	    dma_pool_alloc(ha->dl_dma_pool, GFP_ATOMIC, &crc_ctx_dma);
+
+	if (!crc_ctx_pkt)
+		goto crc_queuing_error;
+
+	/* Zero out CTX area. */
+	clr_ptr = (uint8_t *)crc_ctx_pkt;
+	memset(clr_ptr, 0, sizeof(*crc_ctx_pkt));
+
+	crc_ctx_pkt->crc_ctx_dma = crc_ctx_dma;
+	INIT_LIST_HEAD(&crc_ctx_pkt->dsd_list);
+
+	/* Set handle */
+	crc_ctx_pkt->handle = pkt->handle;
+
+	qlt_set_t10dif_tags(se_cmd, crc_ctx_pkt);
+
+	pkt->crc_context_address[0] = cpu_to_le32(LSD(crc_ctx_dma));
+	pkt->crc_context_address[1] = cpu_to_le32(MSD(crc_ctx_dma));
+	pkt->crc_context_len = CRC_CONTEXT_LEN_FW;
+
+
+	if (!bundling) {
+		cur_dsd = (uint32_t *) &crc_ctx_pkt->u.nobundling.data_address;
+	} else {
+		/*
+		 * Configure Bundling if we need to fetch interlaving
+		 * protection PCI accesses
+		 */
+		fw_prot_opts |= PO_ENABLE_DIF_BUNDLING;
+		crc_ctx_pkt->u.bundling.dif_byte_count = cpu_to_le32(dif_bytes);
+		crc_ctx_pkt->u.bundling.dseg_count =
+			cpu_to_le16(prm->tot_dsds - prm->prot_seg_cnt);
+		cur_dsd = (uint32_t *) &crc_ctx_pkt->u.bundling.data_address;
+	}
+
+	/* Finish the common fields of CRC pkt */
+	crc_ctx_pkt->blk_size   = cpu_to_le16(cmd->blk_sz);
+	crc_ctx_pkt->prot_opts  = cpu_to_le16(fw_prot_opts);
+	crc_ctx_pkt->byte_count = cpu_to_le32(data_bytes);
+	crc_ctx_pkt->guard_seed = __constant_cpu_to_le16(0);
+
+
+	/* Walks data segments */
+	pkt->flags |= __constant_cpu_to_le16(CTIO7_FLAGS_DSD_PTR);
+
+	if (!bundling && prm->prot_seg_cnt) {
+		if (qla24xx_walk_and_build_sglist_no_difb(ha, NULL, cur_dsd,
+			prm->tot_dsds, cmd))
+			goto crc_queuing_error;
+	} else if (qla24xx_walk_and_build_sglist(ha, NULL, cur_dsd,
+		(prm->tot_dsds - prm->prot_seg_cnt), cmd))
+		goto crc_queuing_error;
+
+	if (bundling && prm->prot_seg_cnt) {
+		/* Walks dif segments */
+		pkt->add_flags |=
+			__constant_cpu_to_le16(CTIO_CRC2_AF_DIF_DSD_ENA);
+
+		cur_dsd = (uint32_t *) &crc_ctx_pkt->u.bundling.dif_address;
+		if (qla24xx_walk_and_build_prot_sglist(ha, NULL, cur_dsd,
+			prm->prot_seg_cnt, cmd))
+			goto crc_queuing_error;
+	}
+	return QLA_SUCCESS;
+
+crc_queuing_error:
+	/* Cleanup will be performed by the caller */
+
+	return QLA_FUNCTION_FAILED;
+}
+
+
 /*
  * Callback to setup response of xmit_type of QLA_TGT_XMIT_DATA and *
  * QLA_TGT_XMIT_STATUS for >= 24xx silicon
@@ -1921,9 +2287,10 @@ int qlt_xmit_response(struct qla_tgt_cmd *cmd, int xmit_type,
 	qlt_check_srr_debug(cmd, &xmit_type);
 
 	ql_dbg(ql_dbg_tgt, cmd->vha, 0xe018,
-	    "is_send_status=%d, cmd->bufflen=%d, cmd->sg_cnt=%d, "
-	    "cmd->dma_data_direction=%d\n", (xmit_type & QLA_TGT_XMIT_STATUS) ?
-	    1 : 0, cmd->bufflen, cmd->sg_cnt, cmd->dma_data_direction);
+	    "is_send_status=%d, cmd->bufflen=%d, cmd->sg_cnt=%d, cmd->dma_data_direction=%d se_cmd[%p]\n",
+	    (xmit_type & QLA_TGT_XMIT_STATUS) ?
+	    1 : 0, cmd->bufflen, cmd->sg_cnt, cmd->dma_data_direction,
+	    &cmd->se_cmd);
 
 	res = qlt_pre_xmit_response(cmd, &prm, xmit_type, scsi_status,
 	    &full_req_cnt);
@@ -1941,7 +2308,10 @@ int qlt_xmit_response(struct qla_tgt_cmd *cmd, int xmit_type,
 	if (unlikely(res))
 		goto out_unmap_unlock;
 
-	res = qlt_24xx_build_ctio_pkt(&prm, vha);
+	if (cmd->se_cmd.prot_op && (xmit_type & QLA_TGT_XMIT_DATA))
+		res = qlt_build_ctio_crc2_pkt(&prm, vha);
+	else
+		res = qlt_24xx_build_ctio_pkt(&prm, vha);
 	if (unlikely(res != 0))
 		goto out_unmap_unlock;
 
@@ -1953,7 +2323,8 @@ int qlt_xmit_response(struct qla_tgt_cmd *cmd, int xmit_type,
 		    __constant_cpu_to_le16(CTIO7_FLAGS_DATA_IN |
 			CTIO7_FLAGS_STATUS_MODE_0);
 
-		qlt_load_data_segments(&prm, vha);
+		if (cmd->se_cmd.prot_op == TARGET_PROT_NORMAL)
+			qlt_load_data_segments(&prm, vha);
 
 		if (prm.add_status_pkt == 0) {
 			if (xmit_type & QLA_TGT_XMIT_STATUS) {
@@ -1983,8 +2354,14 @@ int qlt_xmit_response(struct qla_tgt_cmd *cmd, int xmit_type,
 			ql_dbg(ql_dbg_tgt, vha, 0xe019,
 			    "Building additional status packet\n");
 
+			/*
+			 * T10Dif: ctio_crc2_to_fw overlay ontop of
+			 * ctio7_to_24xx
+			 */
 			memcpy(ctio, pkt, sizeof(*ctio));
+			/* reset back to CTIO7 */
 			ctio->entry_count = 1;
+			ctio->entry_type = CTIO_TYPE7;
 			ctio->dseg_count = 0;
 			ctio->u.status1.flags &= ~__constant_cpu_to_le16(
 			    CTIO7_FLAGS_DATA_IN);
@@ -1993,6 +2370,11 @@ int qlt_xmit_response(struct qla_tgt_cmd *cmd, int xmit_type,
 			pkt->handle |= CTIO_INTERMEDIATE_HANDLE_MARK;
 			pkt->u.status0.flags |= __constant_cpu_to_le16(
 			    CTIO7_FLAGS_DONT_RET_CTIO);
+
+			/* qlt_24xx_init_ctio_to_isp will correct
+			 * all neccessary fields that's part of CTIO7.
+			 * There should be no residual of CTIO-CRC2 data.
+			 */
 			qlt_24xx_init_ctio_to_isp((struct ctio7_to_24xx *)ctio,
 			    &prm);
 			pr_debug("Status CTIO7: %p\n", ctio);
@@ -2041,8 +2423,10 @@ 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, "CTIO_start: vha(%d)",
-	    (int)vha->vp_idx);
+	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)
@@ -2054,14 +2438,19 @@ int qlt_rdy_to_xfer(struct qla_tgt_cmd *cmd)
 	res = qlt_check_reserve_free_req(vha, prm.req_cnt);
 	if (res != 0)
 		goto out_unlock_free_unmap;
+	if (cmd->se_cmd.prot_op)
+		res = qlt_build_ctio_crc2_pkt(&prm, vha);
+	else
+		res = qlt_24xx_build_ctio_pkt(&prm, vha);
 
-	res = qlt_24xx_build_ctio_pkt(&prm, vha);
 	if (unlikely(res != 0))
 		goto out_unlock_free_unmap;
 	pkt = (struct ctio7_to_24xx *)prm.pkt;
 	pkt->u.status0.flags |= __constant_cpu_to_le16(CTIO7_FLAGS_DATA_OUT |
 	    CTIO7_FLAGS_STATUS_MODE_0);
-	qlt_load_data_segments(&prm, vha);
+
+	if (cmd->se_cmd.prot_op == TARGET_PROT_NORMAL)
+		qlt_load_data_segments(&prm, vha);
 
 	cmd->state = QLA_TGT_STATE_NEED_DATA;
 
@@ -2079,6 +2468,143 @@ out_unlock_free_unmap:
 }
 EXPORT_SYMBOL(qlt_rdy_to_xfer);
 
+
+/*
+ * Checks the guard or meta-data for the type of error
+ * detected by the HBA.
+ */
+static inline int
+qlt_handle_dif_error(struct scsi_qla_host *vha, struct qla_tgt_cmd *cmd,
+		struct ctio_crc_from_fw *sts)
+{
+	uint8_t		*ap = &sts->actual_dif[0];
+	uint8_t		*ep = &sts->expected_dif[0];
+	uint32_t	e_ref_tag, a_ref_tag;
+	uint16_t	e_app_tag, a_app_tag;
+	uint16_t	e_guard, a_guard;
+	uint64_t	lba = cmd->se_cmd.t_task_lba;
+
+	a_guard   = be16_to_cpu(*(uint16_t *)(ap + 0));
+	a_app_tag = be16_to_cpu(*(uint16_t *)(ap + 2));
+	a_ref_tag = be32_to_cpu(*(uint32_t *)(ap + 4));
+
+	e_guard   = be16_to_cpu(*(uint16_t *)(ep + 0));
+	e_app_tag = be16_to_cpu(*(uint16_t *)(ep + 2));
+	e_ref_tag = be32_to_cpu(*(uint32_t *)(ep + 4));
+
+	ql_dbg(ql_dbg_tgt, vha, 0xe075,
+	    "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",
+	    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);
+
+	/*
+	 * Ignore sector if:
+	 * For type     3: ref & app tag is all 'f's
+	 * For type 0,1,2: app tag is all 'f's
+	 */
+	if ((a_app_tag == 0xffff) &&
+	    ((cmd->se_cmd.prot_type != TARGET_DIF_TYPE3_PROT) ||
+	     (a_ref_tag == 0xffffffff))) {
+		uint32_t blocks_done;
+
+		/* 2TB boundary case covered automatically with this */
+		blocks_done = e_ref_tag - (uint32_t)lba + 1;
+		cmd->se_cmd.bad_sector = e_ref_tag;
+		cmd->se_cmd.pi_err = 0;
+		ql_dbg(ql_dbg_tgt, vha, 0xf074,
+			"need to return scsi good\n");
+
+		/* Update protection tag */
+		if (cmd->prot_sg_cnt) {
+			uint32_t i, j = 0, k = 0, num_ent;
+			struct scatterlist *sg, *sgl;
+
+
+			sgl = cmd->prot_sg;
+
+			/* Patch the corresponding protection tags */
+			for_each_sg(sgl, sg, cmd->prot_sg_cnt, i) {
+				num_ent = sg_dma_len(sg) / 8;
+				if (k + num_ent < blocks_done) {
+					k += num_ent;
+					continue;
+				}
+				j = blocks_done - k - 1;
+				k = blocks_done;
+				break;
+			}
+
+			if (k != blocks_done) {
+				ql_log(ql_log_warn, vha, 0xf076,
+				    "unexpected tag values tag:lba=%u:%llu)\n",
+				    e_ref_tag, (unsigned long long)lba);
+				goto out;
+			}
+
+#if 0
+			struct sd_dif_tuple *spt;
+			/* TODO:
+			 * This section came from initiator. Is it valid here?
+			 * should ulp be override with actual val???
+			 */
+			spt = page_address(sg_page(sg)) + sg->offset;
+			spt += j;
+
+			spt->app_tag = 0xffff;
+			if (cmd->se_cmd.prot_type == SCSI_PROT_DIF_TYPE3)
+				spt->ref_tag = 0xffffffff;
+#endif
+		}
+
+		return 0;
+	}
+
+	/* check guard */
+	if (e_guard != a_guard) {
+		cmd->se_cmd.pi_err = TCM_LOGICAL_BLOCK_GUARD_CHECK_FAILED;
+		cmd->se_cmd.bad_sector = cmd->se_cmd.t_task_lba;
+
+		ql_log(ql_log_warn, vha, 0xe076,
+		    "Guard ERR: 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] cmd=%p\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, cmd);
+		goto out;
+	}
+
+	/* check ref tag */
+	if (e_ref_tag != a_ref_tag) {
+		cmd->se_cmd.pi_err = TCM_LOGICAL_BLOCK_REF_TAG_CHECK_FAILED;
+		cmd->se_cmd.bad_sector = e_ref_tag;
+
+		ql_log(ql_log_warn, vha, 0xe077,
+			"Ref Tag ERR: 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] cmd=%p\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, cmd);
+		goto out;
+	}
+
+	/* check appl tag */
+	if (e_app_tag != a_app_tag) {
+		cmd->se_cmd.pi_err = TCM_LOGICAL_BLOCK_APP_TAG_CHECK_FAILED;
+		cmd->se_cmd.bad_sector = cmd->se_cmd.t_task_lba;
+
+		ql_log(ql_log_warn, vha, 0xe078,
+			"App Tag ERR: 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] cmd=%p\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, cmd);
+		goto out;
+	}
+out:
+	return 1;
+}
+
+
 /* If hardware_lock held on entry, might drop it, then reaquire */
 /* This function sends the appropriate CTIO to ISP 2xxx or 24xx */
 static int __qlt_send_term_exchange(struct scsi_qla_host *vha,
@@ -2155,18 +2681,36 @@ static void qlt_send_term_exchange(struct scsi_qla_host *vha,
 	rc = __qlt_send_term_exchange(vha, cmd, atio);
 	spin_unlock_irqrestore(&vha->hw->hardware_lock, flags);
 done:
-	if (rc == 1) {
+	/*
+	 * 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 (!ha_locked && !in_interrupt())
 			msleep(250); /* just in case */
 
+		if (cmd->sg_mapped)
+			qlt_unmap_sg(vha, cmd);
 		vha->hw->tgt.tgt_ops->free_cmd(cmd);
 	}
+	return;
 }
 
 void qlt_free_cmd(struct qla_tgt_cmd *cmd)
 {
-	BUG_ON(cmd->sg_mapped);
+	ql_dbg(ql_dbg_tgt, cmd->vha, 0xe074,
+	    "%s: se_cmd[%p] ox_id %04x\n",
+	    __func__, &cmd->se_cmd,
+	    be16_to_cpu(cmd->atio.u.isp24.fcp_hdr.ox_id));
 
+	BUG_ON(cmd->sg_mapped);
 	if (unlikely(cmd->free_sg))
 		kfree(cmd->sg);
 	kmem_cache_free(qla_tgt_cmd_cachep, cmd);
@@ -2374,6 +2918,7 @@ static void qlt_do_ctio_completion(struct scsi_qla_host *vha, uint32_t handle,
 		case CTIO_LIP_RESET:
 		case CTIO_TARGET_RESET:
 		case CTIO_ABORTED:
+			/* driver request abort via Terminate exchange */
 		case CTIO_TIMEOUT:
 		case CTIO_INVALID_RX_ID:
 			/* They are OK */
@@ -2404,18 +2949,58 @@ static void qlt_do_ctio_completion(struct scsi_qla_host *vha, uint32_t handle,
 			else
 				return;
 
+		case CTIO_DIF_ERROR: {
+			struct ctio_crc_from_fw *crc =
+				(struct ctio_crc_from_fw *)ctio;
+			ql_dbg(ql_dbg_tgt_mgt, vha, 0xf073,
+			    "qla_target(%d): CTIO with DIF_ERROR status %x received (state %x, se_cmd %p) actual_dif[0x%llx] expect_dif[0x%llx]\n",
+			    vha->vp_idx, status, cmd->state, se_cmd,
+			    *((u64 *)&crc->actual_dif[0]),
+			    *((u64 *)&crc->expected_dif[0]));
+
+			if (qlt_handle_dif_error(vha, cmd, ctio)) {
+				if (cmd->state == QLA_TGT_STATE_NEED_DATA) {
+					/* scsi Write/xfer rdy complete */
+					goto skip_term;
+				} else {
+					/* scsi read/xmit respond complete
+					 * call handle dif to send scsi status
+					 * rather than terminate exchange.
+					 */
+					cmd->state = QLA_TGT_STATE_PROCESSED;
+					ha->tgt.tgt_ops->handle_dif_err(cmd);
+					return;
+				}
+			} else {
+				/* Need to generate a SCSI good completion.
+				 * because FW did not send scsi status.
+				 */
+				status = 0;
+				goto skip_term;
+			}
+			break;
+		}
 		default:
 			ql_dbg(ql_dbg_tgt_mgt, vha, 0xf05b,
-			    "qla_target(%d): CTIO with error status "
-			    "0x%x received (state %x, se_cmd %p\n",
+			    "qla_target(%d): CTIO with error status 0x%x received (state %x, se_cmd %p\n",
 			    vha->vp_idx, status, cmd->state, se_cmd);
 			break;
 		}
 
-		if (cmd->state != QLA_TGT_STATE_NEED_DATA)
+
+		/* "cmd->state == QLA_TGT_STATE_ABORTED" means
+		 * cmd is already aborted/terminated, we don't
+		 * need to terminate again.  The exchange is already
+		 * cleaned up/freed at FW level.  Just cleanup at driver
+		 * level.
+		 */
+		if ((cmd->state != QLA_TGT_STATE_NEED_DATA) &&
+			(cmd->state != QLA_TGT_STATE_ABORTED)) {
 			if (qlt_term_ctio_exchange(vha, ctio, cmd, status))
 				return;
+		}
 	}
+skip_term:
 
 	if (cmd->state == QLA_TGT_STATE_PROCESSED) {
 		ql_dbg(ql_dbg_tgt, vha, 0xe01f, "Command %p finished\n", cmd);
@@ -2444,7 +3029,8 @@ static void qlt_do_ctio_completion(struct scsi_qla_host *vha, uint32_t handle,
 		    "not return a CTIO complete\n", vha->vp_idx, cmd->state);
 	}
 
-	if (unlikely(status != CTIO_SUCCESS)) {
+	if (unlikely(status != CTIO_SUCCESS) &&
+		(cmd->state != QLA_TGT_STATE_ABORTED)) {
 		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf01f, "Finishing failed CTIO\n");
 		dump_stack();
 	}
@@ -2563,8 +3149,9 @@ static void qlt_do_work(struct work_struct *work)
 	    atio->u.isp24.fcp_cmnd.add_cdb_len]));
 
 	ql_dbg(ql_dbg_tgt, vha, 0xe022,
-	    "qla_target: START qla command: %p lun: 0x%04x (tag %d)\n",
-	    cmd, cmd->unpacked_lun, cmd->tag);
+		"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 = vha->hw->tgt.tgt_ops->handle_cmd(vha, cmd, cdb, data_length,
 	    fcp_task_attr, data_dir, bidi);
@@ -3527,11 +4114,11 @@ 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, "
-		    "add_cdb_len %d, data_length %04x, s_id %x:%x:%x\n",
+		    "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[
@@ -3629,11 +4216,13 @@ static void qlt_response_pkt(struct scsi_qla_host *vha, response_t *pkt)
 	tgt->irq_cmd_count++;
 
 	switch (pkt->entry_type) {
+	case CTIO_CRC2:
 	case CTIO_TYPE7:
 	{
 		struct ctio7_from_24xx *entry = (struct ctio7_from_24xx *)pkt;
-		ql_dbg(ql_dbg_tgt, vha, 0xe030, "CTIO_TYPE7: instance %d\n",
-		    vha->vp_idx);
+		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);
@@ -4768,6 +5357,7 @@ qlt_24xx_process_response_error(struct scsi_qla_host *vha,
 	case ABTS_RESP_24XX:
 	case CTIO_TYPE7:
 	case NOTIFY_ACK_TYPE:
+	case CTIO_CRC2:
 		return 1;
 	default:
 		return 0;
diff --git a/drivers/scsi/qla2xxx/qla_target.h b/drivers/scsi/qla2xxx/qla_target.h
index ce33d8c2640..f873e10451d 100644
--- a/drivers/scsi/qla2xxx/qla_target.h
+++ b/drivers/scsi/qla2xxx/qla_target.h
@@ -293,6 +293,7 @@ struct ctio_to_2xxx {
 #define CTIO_ABORTED			0x02
 #define CTIO_INVALID_RX_ID		0x08
 #define CTIO_TIMEOUT			0x0B
+#define CTIO_DIF_ERROR			0x0C     /* DIF error detected  */
 #define CTIO_LIP_RESET			0x0E
 #define CTIO_TARGET_RESET		0x17
 #define CTIO_PORT_UNAVAILABLE		0x28
@@ -498,11 +499,12 @@ struct ctio7_from_24xx {
 #define CTIO7_FLAGS_DONT_RET_CTIO	BIT_8
 #define CTIO7_FLAGS_STATUS_MODE_0	0
 #define CTIO7_FLAGS_STATUS_MODE_1	BIT_6
+#define CTIO7_FLAGS_STATUS_MODE_2	BIT_7
 #define CTIO7_FLAGS_EXPLICIT_CONFORM	BIT_5
 #define CTIO7_FLAGS_CONFIRM_SATISF	BIT_4
 #define CTIO7_FLAGS_DSD_PTR		BIT_2
-#define CTIO7_FLAGS_DATA_IN		BIT_1
-#define CTIO7_FLAGS_DATA_OUT		BIT_0
+#define CTIO7_FLAGS_DATA_IN		BIT_1 /* data to initiator */
+#define CTIO7_FLAGS_DATA_OUT		BIT_0 /* data from initiator */
 
 #define ELS_PLOGI			0x3
 #define ELS_FLOGI			0x4
@@ -514,6 +516,68 @@ struct ctio7_from_24xx {
 #define ELS_ADISC			0x52
 
 /*
+ *CTIO Type CRC_2 IOCB
+ */
+struct ctio_crc2_to_fw {
+	uint8_t entry_type;		/* Entry type. */
+#define CTIO_CRC2 0x7A
+	uint8_t entry_count;		/* Entry count. */
+	uint8_t sys_define;		/* System defined. */
+	uint8_t entry_status;		/* Entry Status. */
+
+	uint32_t handle;		/* System handle. */
+	uint16_t nport_handle;		/* N_PORT handle. */
+	uint16_t timeout;		/* Command timeout. */
+
+	uint16_t dseg_count;		/* Data segment count. */
+	uint8_t  vp_index;
+	uint8_t  add_flags;		/* additional flags */
+#define CTIO_CRC2_AF_DIF_DSD_ENA BIT_3
+
+	uint8_t  initiator_id[3];	/* initiator ID */
+	uint8_t  reserved1;
+	uint32_t exchange_addr;		/* rcv exchange address */
+	uint16_t reserved2;
+	uint16_t flags;			/* refer to CTIO7 flags values */
+	uint32_t residual;
+	uint16_t ox_id;
+	uint16_t scsi_status;
+	uint32_t relative_offset;
+	uint32_t reserved5;
+	uint32_t transfer_length;		/* total fc transfer length */
+	uint32_t reserved6;
+	uint32_t crc_context_address[2];/* Data segment address. */
+	uint16_t crc_context_len;	/* Data segment length. */
+	uint16_t reserved_1;		/* MUST be set to 0. */
+} __packed;
+
+/* CTIO Type CRC_x Status IOCB */
+struct ctio_crc_from_fw {
+	uint8_t entry_type;		/* Entry type. */
+	uint8_t entry_count;		/* Entry count. */
+	uint8_t sys_define;		/* System defined. */
+	uint8_t entry_status;		/* Entry Status. */
+
+	uint32_t handle;		/* System handle. */
+	uint16_t status;
+	uint16_t timeout;		/* Command timeout. */
+	uint16_t dseg_count;		/* Data segment count. */
+	uint32_t reserved1;
+	uint16_t state_flags;
+#define CTIO_CRC_SF_DIF_CHOPPED BIT_4
+
+	uint32_t exchange_address;	/* rcv exchange address */
+	uint16_t reserved2;
+	uint16_t flags;
+	uint32_t resid_xfer_length;
+	uint16_t ox_id;
+	uint8_t  reserved3[12];
+	uint16_t runt_guard;		/* reported runt blk guard */
+	uint8_t  actual_dif[8];
+	uint8_t  expected_dif[8];
+} __packed;
+
+/*
  * ISP queue - ABTS received/response entries structure definition for 24xx.
  */
 #define ABTS_RECV_24XX		0x54 /* ABTS received (for 24xx) */
@@ -641,6 +705,7 @@ struct qla_tgt_func_tmpl {
 	int (*handle_cmd)(struct scsi_qla_host *, struct qla_tgt_cmd *,
 			unsigned char *, uint32_t, int, int, int);
 	void (*handle_data)(struct qla_tgt_cmd *);
+	void (*handle_dif_err)(struct qla_tgt_cmd *);
 	int (*handle_tmr)(struct qla_tgt_mgmt_cmd *, uint32_t, uint8_t,
 			uint32_t);
 	void (*free_cmd)(struct qla_tgt_cmd *);
@@ -829,9 +894,9 @@ struct qla_tgt_sess {
 };
 
 struct qla_tgt_cmd {
+	struct se_cmd se_cmd;
 	struct qla_tgt_sess *sess;
 	int state;
-	struct se_cmd se_cmd;
 	struct work_struct free_work;
 	struct work_struct work;
 	/* Sense buffer that will be mapped into outgoing status */
@@ -843,6 +908,7 @@ struct qla_tgt_cmd {
 	unsigned int free_sg:1;
 	unsigned int aborted:1; /* Needed in case of SRR */
 	unsigned int write_data_transferred:1;
+	unsigned int ctx_dsd_alloced:1;
 
 	struct scatterlist *sg;	/* cmd data buffer SG vector */
 	int sg_cnt;		/* SG segments count */
@@ -857,6 +923,12 @@ struct qla_tgt_cmd {
 	struct scsi_qla_host *vha;
 
 	struct atio_from_isp atio;
+	/* t10dif */
+	struct scatterlist *prot_sg;
+	uint32_t prot_sg_cnt;
+	uint32_t blk_sz;
+	struct crc_context *ctx;
+
 };
 
 struct qla_tgt_sess_work_param {
@@ -901,6 +973,10 @@ struct qla_tgt_prm {
 	int sense_buffer_len;
 	int residual;
 	int add_status_pkt;
+	/* dif */
+	struct scatterlist *prot_sg;
+	uint16_t prot_seg_cnt;
+	uint16_t tot_dsds;
 };
 
 struct qla_tgt_srr_imm {
@@ -976,6 +1052,8 @@ extern void qlt_24xx_atio_pkt_all_vps(struct scsi_qla_host *,
 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);
diff --git a/drivers/scsi/qla2xxx/qla_tmpl.c b/drivers/scsi/qla2xxx/qla_tmpl.c
index a804e9b744b..cb9a0c4bc41 100644
--- a/drivers/scsi/qla2xxx/qla_tmpl.c
+++ b/drivers/scsi/qla2xxx/qla_tmpl.c
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
@@ -201,7 +201,6 @@ qla27xx_read_reg(__iomem struct device_reg_24xx *reg,
 		ql_dbg(ql_dbg_misc, NULL, 0xd014,
 		    "%s: @%x\n", __func__, offset);
 	}
-	qla27xx_insert32(offset, buf, len);
 	qla27xx_read32(window, buf, len);
 }
 
@@ -220,7 +219,7 @@ qla27xx_write_reg(__iomem struct device_reg_24xx *reg,
 
 static inline void
 qla27xx_read_window(__iomem struct device_reg_24xx *reg,
-	uint32_t base, uint offset, uint count, uint width, void *buf,
+	uint32_t addr, uint offset, uint count, uint width, void *buf,
 	ulong *len)
 {
 	void *window = (void *)reg + offset;
@@ -229,14 +228,14 @@ qla27xx_read_window(__iomem struct device_reg_24xx *reg,
 	if (buf) {
 		ql_dbg(ql_dbg_misc, NULL, 0xd016,
 		    "%s: base=%x offset=%x count=%x width=%x\n",
-		    __func__, base, offset, count, width);
+		    __func__, addr, offset, count, width);
 	}
-	qla27xx_write_reg(reg, IOBASE_ADDR, base, buf);
+	qla27xx_write_reg(reg, IOBASE_ADDR, addr, buf);
 	while (count--) {
-		qla27xx_insert32(base, buf, len);
+		qla27xx_insert32(addr, buf, len);
 		readn(window, buf, len);
 		window += width;
-		base += width;
+		addr++;
 	}
 }
 
@@ -336,7 +335,8 @@ qla27xx_fwdt_entry_t260(struct scsi_qla_host *vha,
 
 	ql_dbg(ql_dbg_misc, vha, 0xd204,
 	    "%s: rdpci [%lx]\n", __func__, *len);
-	qla27xx_read_reg(reg, ent->t260.pci_addr, buf, len);
+	qla27xx_insert32(ent->t260.pci_offset, buf, len);
+	qla27xx_read_reg(reg, ent->t260.pci_offset, buf, len);
 
 	return false;
 }
@@ -349,7 +349,7 @@ qla27xx_fwdt_entry_t261(struct scsi_qla_host *vha,
 
 	ql_dbg(ql_dbg_misc, vha, 0xd205,
 	    "%s: wrpci [%lx]\n", __func__, *len);
-	qla27xx_write_reg(reg, ent->t261.pci_addr, ent->t261.write_data, buf);
+	qla27xx_write_reg(reg, ent->t261.pci_offset, ent->t261.write_data, buf);
 
 	return false;
 }
@@ -392,9 +392,9 @@ qla27xx_fwdt_entry_t262(struct scsi_qla_host *vha,
 		goto done;
 	}
 
-	if (end < start) {
+	if (end < start || end == 0) {
 		ql_dbg(ql_dbg_misc, vha, 0xd023,
-		    "%s: bad range (start=%x end=%x)\n", __func__,
+		    "%s: unusable range (start=%x end=%x)\n", __func__,
 		    ent->t262.end_addr, ent->t262.start_addr);
 		qla27xx_skip_entry(ent, buf);
 		goto done;
@@ -452,17 +452,15 @@ qla27xx_fwdt_entry_t263(struct scsi_qla_host *vha,
 		ql_dbg(ql_dbg_misc, vha, 0xd025,
 		    "%s: unsupported atio queue\n", __func__);
 		qla27xx_skip_entry(ent, buf);
-		goto done;
 	} else {
 		ql_dbg(ql_dbg_misc, vha, 0xd026,
 		    "%s: unknown queue %u\n", __func__, ent->t263.queue_type);
 		qla27xx_skip_entry(ent, buf);
-		goto done;
 	}
 
 	if (buf)
 		ent->t263.num_queues = count;
-done:
+
 	return false;
 }
 
@@ -503,7 +501,7 @@ qla27xx_fwdt_entry_t265(struct scsi_qla_host *vha,
 	ql_dbg(ql_dbg_misc, vha, 0xd209,
 	    "%s: pause risc [%lx]\n", __func__, *len);
 	if (buf)
-		qla24xx_pause_risc(reg);
+		qla24xx_pause_risc(reg, vha->hw);
 
 	return false;
 }
@@ -590,7 +588,6 @@ qla27xx_fwdt_entry_t270(struct scsi_qla_host *vha,
 	struct qla27xx_fwdt_entry *ent, void *buf, ulong *len)
 {
 	struct device_reg_24xx __iomem *reg = qla27xx_isp_reg(vha);
-	void *window = (void *)reg + 0xc4;
 	ulong dwords = ent->t270.count;
 	ulong addr = ent->t270.addr;
 
@@ -599,10 +596,9 @@ qla27xx_fwdt_entry_t270(struct scsi_qla_host *vha,
 	qla27xx_write_reg(reg, IOBASE_ADDR, 0x40, buf);
 	while (dwords--) {
 		qla27xx_write_reg(reg, 0xc0, addr|0x80000000, buf);
-		qla27xx_read_reg(reg, 0xc4, buf, len);
 		qla27xx_insert32(addr, buf, len);
-		qla27xx_read32(window, buf, len);
-		addr++;
+		qla27xx_read_reg(reg, 0xc4, buf, len);
+		addr += sizeof(uint32_t);
 	}
 
 	return false;
@@ -614,12 +610,12 @@ qla27xx_fwdt_entry_t271(struct scsi_qla_host *vha,
 {
 	struct device_reg_24xx __iomem *reg = qla27xx_isp_reg(vha);
 	ulong addr = ent->t271.addr;
+	ulong data = ent->t271.data;
 
 	ql_dbg(ql_dbg_misc, vha, 0xd20f,
 	    "%s: wrremreg [%lx]\n", __func__, *len);
 	qla27xx_write_reg(reg, IOBASE_ADDR, 0x40, buf);
-	qla27xx_read_reg(reg, 0xc4, buf, len);
-	qla27xx_insert32(addr, buf, len);
+	qla27xx_write_reg(reg, 0xc4, data, buf);
 	qla27xx_write_reg(reg, 0xc0, addr, buf);
 
 	return false;
@@ -662,9 +658,59 @@ qla27xx_fwdt_entry_t273(struct scsi_qla_host *vha,
 			    "%s: failed pcicfg read at %lx\n", __func__, addr);
 		qla27xx_insert32(addr, buf, len);
 		qla27xx_insert32(value, buf, len);
-		addr += 4;
+		addr += sizeof(uint32_t);
+	}
+
+	return false;
+}
+
+static int
+qla27xx_fwdt_entry_t274(struct scsi_qla_host *vha,
+	struct qla27xx_fwdt_entry *ent, void *buf, ulong *len)
+{
+	uint count = 0;
+	uint i;
+
+	ql_dbg(ql_dbg_misc, vha, 0xd212,
+	    "%s: getqsh(%x) [%lx]\n", __func__, ent->t274.queue_type, *len);
+	if (ent->t274.queue_type == T274_QUEUE_TYPE_REQ_SHAD) {
+		for (i = 0; i < vha->hw->max_req_queues; i++) {
+			struct req_que *req = vha->hw->req_q_map[i];
+			if (req || !buf) {
+				qla27xx_insert16(i, buf, len);
+				qla27xx_insert16(1, buf, len);
+				qla27xx_insert32(req && req->out_ptr ?
+				    *req->out_ptr : 0, buf, len);
+				count++;
+			}
+		}
+	} else if (ent->t274.queue_type == T274_QUEUE_TYPE_RSP_SHAD) {
+		for (i = 0; i < vha->hw->max_rsp_queues; i++) {
+			struct rsp_que *rsp = vha->hw->rsp_q_map[i];
+			if (rsp || !buf) {
+				qla27xx_insert16(i, buf, len);
+				qla27xx_insert16(1, buf, len);
+				qla27xx_insert32(rsp && rsp->in_ptr ?
+				    *rsp->in_ptr : 0, buf, len);
+				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);
+		qla27xx_skip_entry(ent, buf);
 	}
 
+	if (buf)
+		ent->t274.num_queues = count;
+
+	if (!count)
+		qla27xx_skip_entry(ent, buf);
+
 	return false;
 }
 
@@ -709,6 +755,7 @@ static struct qla27xx_fwdt_entry_call ql27xx_fwdt_entry_call_list[] = {
 	{ ENTRY_TYPE_WRREMREG		, qla27xx_fwdt_entry_t271  } ,
 	{ ENTRY_TYPE_RDREMRAM		, qla27xx_fwdt_entry_t272  } ,
 	{ ENTRY_TYPE_PCICFG		, qla27xx_fwdt_entry_t273  } ,
+	{ ENTRY_TYPE_GET_SHADOW		, qla27xx_fwdt_entry_t274  } ,
 	{ -1				, qla27xx_fwdt_entry_other }
 };
 
diff --git a/drivers/scsi/qla2xxx/qla_tmpl.h b/drivers/scsi/qla2xxx/qla_tmpl.h
index c9d2fff4d96..1967424c8e6 100644
--- a/drivers/scsi/qla2xxx/qla_tmpl.h
+++ b/drivers/scsi/qla2xxx/qla_tmpl.h
@@ -1,6 +1,6 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
@@ -52,6 +52,7 @@ struct __packed qla27xx_fwdt_template {
 #define ENTRY_TYPE_WRREMREG		271
 #define ENTRY_TYPE_RDREMRAM		272
 #define ENTRY_TYPE_PCICFG		273
+#define ENTRY_TYPE_GET_SHADOW		274
 
 #define CAPTURE_FLAG_PHYS_ONLY		BIT_0
 #define CAPTURE_FLAG_PHYS_VIRT		BIT_1
@@ -109,12 +110,12 @@ struct __packed qla27xx_fwdt_entry {
 		} t259;
 
 		struct __packed {
-			uint8_t pci_addr;
+			uint8_t pci_offset;
 			uint8_t reserved[3];
 		} t260;
 
 		struct __packed {
-			uint8_t pci_addr;
+			uint8_t pci_offset;
 			uint8_t reserved[3];
 			uint32_t write_data;
 		} t261;
@@ -186,6 +187,12 @@ struct __packed qla27xx_fwdt_entry {
 			uint32_t addr;
 			uint32_t count;
 		} t273;
+
+		struct __packed {
+			uint32_t num_queues;
+			uint8_t  queue_type;
+			uint8_t  reserved[3];
+		} t274;
 	};
 };
 
@@ -202,4 +209,8 @@ struct __packed qla27xx_fwdt_entry {
 #define T268_BUF_TYPE_EXCH_BUFOFF	2
 #define T268_BUF_TYPE_EXTD_LOGIN	3
 
+#define T274_QUEUE_TYPE_REQ_SHAD	1
+#define T274_QUEUE_TYPE_RSP_SHAD	2
+#define T274_QUEUE_TYPE_ATIO_SHAD	3
+
 #endif
diff --git a/drivers/scsi/qla2xxx/qla_version.h b/drivers/scsi/qla2xxx/qla_version.h
index e36b9471254..4d2c98cbec4 100644
--- a/drivers/scsi/qla2xxx/qla_version.h
+++ b/drivers/scsi/qla2xxx/qla_version.h
@@ -1,13 +1,13 @@
 /*
  * QLogic Fibre Channel HBA Driver
- * Copyright (c)  2003-2013 QLogic Corporation
+ * Copyright (c)  2003-2014 QLogic Corporation
  *
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
 /*
  * Driver version
  */
-#define QLA2XXX_VERSION      "8.07.00.02-k"
+#define QLA2XXX_VERSION      "8.07.00.08-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 68fb66fdb75..896cb23adb7 100644
--- a/drivers/scsi/qla2xxx/tcm_qla2xxx.c
+++ b/drivers/scsi/qla2xxx/tcm_qla2xxx.c
@@ -472,6 +472,11 @@ static int tcm_qla2xxx_write_pending(struct se_cmd *se_cmd)
 	cmd->sg_cnt = se_cmd->t_data_nents;
 	cmd->sg = se_cmd->t_data_sg;
 
+	cmd->prot_sg_cnt = se_cmd->t_prot_nents;
+	cmd->prot_sg = se_cmd->t_prot_sg;
+	cmd->blk_sz  = se_cmd->se_dev->dev_attrib.block_size;
+	se_cmd->pi_err = 0;
+
 	/*
 	 * qla_target.c:qlt_rdy_to_xfer() will call pci_map_sg() to setup
 	 * the SGL mappings into PCIe memory for incoming FCP WRITE data.
@@ -567,8 +572,13 @@ static void tcm_qla2xxx_handle_data_work(struct work_struct *work)
 			return;
 		}
 
-		transport_generic_request_failure(&cmd->se_cmd,
-						  TCM_CHECK_CONDITION_ABORT_CMD);
+		if (cmd->se_cmd.pi_err)
+			transport_generic_request_failure(&cmd->se_cmd,
+				cmd->se_cmd.pi_err);
+		else
+			transport_generic_request_failure(&cmd->se_cmd,
+				TCM_CHECK_CONDITION_ABORT_CMD);
+
 		return;
 	}
 
@@ -584,6 +594,27 @@ static void tcm_qla2xxx_handle_data(struct qla_tgt_cmd *cmd)
 	queue_work(tcm_qla2xxx_free_wq, &cmd->work);
 }
 
+static void tcm_qla2xxx_handle_dif_work(struct work_struct *work)
+{
+	struct qla_tgt_cmd *cmd = container_of(work, struct qla_tgt_cmd, work);
+
+	/* take an extra kref to prevent cmd free too early.
+	 * need to wait for SCSI status/check condition to
+	 * finish responding generate by transport_generic_request_failure.
+	 */
+	kref_get(&cmd->se_cmd.cmd_kref);
+	transport_generic_request_failure(&cmd->se_cmd, cmd->se_cmd.pi_err);
+}
+
+/*
+ * Called from qla_target.c:qlt_do_ctio_completion()
+ */
+static void tcm_qla2xxx_handle_dif_err(struct qla_tgt_cmd *cmd)
+{
+	INIT_WORK(&cmd->work, tcm_qla2xxx_handle_dif_work);
+	queue_work(tcm_qla2xxx_free_wq, &cmd->work);
+}
+
 /*
  * Called from qla_target.c:qlt_issue_task_mgmt()
  */
@@ -610,6 +641,11 @@ static int tcm_qla2xxx_queue_data_in(struct se_cmd *se_cmd)
 	cmd->sg = se_cmd->t_data_sg;
 	cmd->offset = 0;
 
+	cmd->prot_sg_cnt = se_cmd->t_prot_nents;
+	cmd->prot_sg = se_cmd->t_prot_sg;
+	cmd->blk_sz  = se_cmd->se_dev->dev_attrib.block_size;
+	se_cmd->pi_err = 0;
+
 	/*
 	 * Now queue completed DATA_IN the qla2xxx LLD and response ring
 	 */
@@ -1600,6 +1636,7 @@ static void tcm_qla2xxx_update_sess(struct qla_tgt_sess *sess, port_id_t s_id,
 static struct qla_tgt_func_tmpl tcm_qla2xxx_template = {
 	.handle_cmd		= tcm_qla2xxx_handle_cmd,
 	.handle_data		= tcm_qla2xxx_handle_data,
+	.handle_dif_err		= tcm_qla2xxx_handle_dif_err,
 	.handle_tmr		= tcm_qla2xxx_handle_tmr,
 	.free_cmd		= tcm_qla2xxx_free_cmd,
 	.free_mcmd		= tcm_qla2xxx_free_mcmd,
diff --git a/drivers/scsi/qla4xxx/ql4_83xx.c b/drivers/scsi/qla4xxx/ql4_83xx.c
index 2eba3536592..556c1525f88 100644
--- a/drivers/scsi/qla4xxx/ql4_83xx.c
+++ b/drivers/scsi/qla4xxx/ql4_83xx.c
@@ -249,110 +249,6 @@ void qla4_83xx_rom_lock_recovery(struct scsi_qla_host *ha)
 	qla4_83xx_flash_unlock(ha);
 }
 
-/**
- * qla4_83xx_ms_mem_write_128b - Writes data to MS/off-chip memory
- * @ha: Pointer to adapter structure
- * @addr: Flash address to write to
- * @data: Data to be written
- * @count: word_count to be written
- *
- * Return: On success return QLA_SUCCESS
- *	   On error return QLA_ERROR
- **/
-int qla4_83xx_ms_mem_write_128b(struct scsi_qla_host *ha, uint64_t addr,
-				uint32_t *data, uint32_t count)
-{
-	int i, j;
-	uint32_t agt_ctrl;
-	unsigned long flags;
-	int ret_val = QLA_SUCCESS;
-
-	/* Only 128-bit aligned access */
-	if (addr & 0xF) {
-		ret_val = QLA_ERROR;
-		goto exit_ms_mem_write;
-	}
-
-	write_lock_irqsave(&ha->hw_lock, flags);
-
-	/* Write address */
-	ret_val = qla4_83xx_wr_reg_indirect(ha, MD_MIU_TEST_AGT_ADDR_HI, 0);
-	if (ret_val == QLA_ERROR) {
-		ql4_printk(KERN_ERR, ha, "%s: write to AGT_ADDR_HI failed\n",
-			   __func__);
-		goto exit_ms_mem_write_unlock;
-	}
-
-	for (i = 0; i < count; i++, addr += 16) {
-		if (!((QLA8XXX_ADDR_IN_RANGE(addr, QLA8XXX_ADDR_QDR_NET,
-					     QLA8XXX_ADDR_QDR_NET_MAX)) ||
-		      (QLA8XXX_ADDR_IN_RANGE(addr, QLA8XXX_ADDR_DDR_NET,
-					     QLA8XXX_ADDR_DDR_NET_MAX)))) {
-			ret_val = QLA_ERROR;
-			goto exit_ms_mem_write_unlock;
-		}
-
-		ret_val = qla4_83xx_wr_reg_indirect(ha, MD_MIU_TEST_AGT_ADDR_LO,
-						    addr);
-		/* Write data */
-		ret_val |= qla4_83xx_wr_reg_indirect(ha,
-						     MD_MIU_TEST_AGT_WRDATA_LO,
-						     *data++);
-		ret_val |= qla4_83xx_wr_reg_indirect(ha,
-						     MD_MIU_TEST_AGT_WRDATA_HI,
-						     *data++);
-		ret_val |= qla4_83xx_wr_reg_indirect(ha,
-						     MD_MIU_TEST_AGT_WRDATA_ULO,
-						     *data++);
-		ret_val |= qla4_83xx_wr_reg_indirect(ha,
-						     MD_MIU_TEST_AGT_WRDATA_UHI,
-						     *data++);
-		if (ret_val == QLA_ERROR) {
-			ql4_printk(KERN_ERR, ha, "%s: write to AGT_WRDATA failed\n",
-				   __func__);
-			goto exit_ms_mem_write_unlock;
-		}
-
-		/* Check write status */
-		ret_val = qla4_83xx_wr_reg_indirect(ha, MD_MIU_TEST_AGT_CTRL,
-						    MIU_TA_CTL_WRITE_ENABLE);
-		ret_val |= qla4_83xx_wr_reg_indirect(ha, MD_MIU_TEST_AGT_CTRL,
-						     MIU_TA_CTL_WRITE_START);
-		if (ret_val == QLA_ERROR) {
-			ql4_printk(KERN_ERR, ha, "%s: write to AGT_CTRL failed\n",
-				   __func__);
-			goto exit_ms_mem_write_unlock;
-		}
-
-		for (j = 0; j < MAX_CTL_CHECK; j++) {
-			ret_val = qla4_83xx_rd_reg_indirect(ha,
-							MD_MIU_TEST_AGT_CTRL,
-							&agt_ctrl);
-			if (ret_val == QLA_ERROR) {
-				ql4_printk(KERN_ERR, ha, "%s: failed to read MD_MIU_TEST_AGT_CTRL\n",
-					   __func__);
-				goto exit_ms_mem_write_unlock;
-			}
-			if ((agt_ctrl & MIU_TA_CTL_BUSY) == 0)
-				break;
-		}
-
-		/* Status check failed */
-		if (j >= MAX_CTL_CHECK) {
-			printk_ratelimited(KERN_ERR "%s: MS memory write failed!\n",
-					   __func__);
-			ret_val = QLA_ERROR;
-			goto exit_ms_mem_write_unlock;
-		}
-	}
-
-exit_ms_mem_write_unlock:
-	write_unlock_irqrestore(&ha->hw_lock, flags);
-
-exit_ms_mem_write:
-	return ret_val;
-}
-
 #define INTENT_TO_RECOVER	0x01
 #define PROCEED_TO_RECOVER	0x02
 
@@ -760,7 +656,7 @@ static int qla4_83xx_copy_bootloader(struct scsi_qla_host *ha)
 			  __func__));
 
 	/* 128 bit/16 byte write to MS memory */
-	ret_val = qla4_83xx_ms_mem_write_128b(ha, dest, (uint32_t *)p_cache,
+	ret_val = qla4_8xxx_ms_mem_write_128b(ha, dest, (uint32_t *)p_cache,
 					      count);
 	if (ret_val == QLA_ERROR) {
 		ql4_printk(KERN_ERR, ha, "%s: Error writing firmware to MS\n",
diff --git a/drivers/scsi/qla4xxx/ql4_83xx.h b/drivers/scsi/qla4xxx/ql4_83xx.h
index a0de6e25ea5..775fdf9fcc8 100644
--- a/drivers/scsi/qla4xxx/ql4_83xx.h
+++ b/drivers/scsi/qla4xxx/ql4_83xx.h
@@ -254,6 +254,50 @@ struct qla83xx_minidump_entry_pollrd {
 	uint32_t rsvd_1;
 };
 
+struct qla8044_minidump_entry_rddfe {
+	struct qla8xxx_minidump_entry_hdr h;
+	uint32_t addr_1;
+	uint32_t value;
+	uint8_t stride;
+	uint8_t stride2;
+	uint16_t count;
+	uint32_t poll;
+	uint32_t mask;
+	uint32_t modify_mask;
+	uint32_t data_size;
+	uint32_t rsvd;
+
+} __packed;
+
+struct qla8044_minidump_entry_rdmdio {
+	struct qla8xxx_minidump_entry_hdr h;
+
+	uint32_t addr_1;
+	uint32_t addr_2;
+	uint32_t value_1;
+	uint8_t stride_1;
+	uint8_t stride_2;
+	uint16_t count;
+	uint32_t poll;
+	uint32_t mask;
+	uint32_t value_2;
+	uint32_t data_size;
+
+} __packed;
+
+struct qla8044_minidump_entry_pollwr {
+	struct qla8xxx_minidump_entry_hdr h;
+	uint32_t addr_1;
+	uint32_t addr_2;
+	uint32_t value_1;
+	uint32_t value_2;
+	uint32_t poll;
+	uint32_t mask;
+	uint32_t data_size;
+	uint32_t rsvd;
+
+} __packed;
+
 /* RDMUX2 Entry */
 struct qla83xx_minidump_entry_rdmux2 {
 	struct qla8xxx_minidump_entry_hdr h;
diff --git a/drivers/scsi/qla4xxx/ql4_def.h b/drivers/scsi/qla4xxx/ql4_def.h
index 73a502288bd..8f6d0fb2cd8 100644
--- a/drivers/scsi/qla4xxx/ql4_def.h
+++ b/drivers/scsi/qla4xxx/ql4_def.h
@@ -601,6 +601,7 @@ struct scsi_qla_host {
 #define DPC_HA_NEED_QUIESCENT		22 /* 0x00400000 ISP-82xx only*/
 #define DPC_POST_IDC_ACK		23 /* 0x00800000 */
 #define DPC_RESTORE_ACB			24 /* 0x01000000 */
+#define DPC_SYSFS_DDB_EXPORT		25 /* 0x02000000 */
 
 	struct Scsi_Host *host; /* pointer to host data */
 	uint32_t tot_ddbs;
diff --git a/drivers/scsi/qla4xxx/ql4_fw.h b/drivers/scsi/qla4xxx/ql4_fw.h
index 209853ce0bb..699575efc9b 100644
--- a/drivers/scsi/qla4xxx/ql4_fw.h
+++ b/drivers/scsi/qla4xxx/ql4_fw.h
@@ -1415,6 +1415,9 @@ struct ql_iscsi_stats {
 #define QLA83XX_DBG_OCM_WNDREG_ARRAY_LEN	16
 #define QLA83XX_SS_OCM_WNDREG_INDEX		3
 #define QLA83XX_SS_PCI_INDEX			0
+#define QLA8022_TEMPLATE_CAP_OFFSET		172
+#define QLA83XX_TEMPLATE_CAP_OFFSET		268
+#define QLA80XX_TEMPLATE_RESERVED_BITS		16
 
 struct qla4_8xxx_minidump_template_hdr {
 	uint32_t entry_type;
@@ -1434,6 +1437,7 @@ struct qla4_8xxx_minidump_template_hdr {
 	uint32_t saved_state_array[QLA8XXX_DBG_STATE_ARRAY_LEN];
 	uint32_t capture_size_array[QLA8XXX_DBG_CAP_SIZE_ARRAY_LEN];
 	uint32_t ocm_window_reg[QLA83XX_DBG_OCM_WNDREG_ARRAY_LEN];
+	uint32_t capabilities[QLA80XX_TEMPLATE_RESERVED_BITS];
 };
 
 #endif /*  _QLA4X_FW_H */
diff --git a/drivers/scsi/qla4xxx/ql4_glbl.h b/drivers/scsi/qla4xxx/ql4_glbl.h
index b1a19cd8d5b..5f58b451327 100644
--- a/drivers/scsi/qla4xxx/ql4_glbl.h
+++ b/drivers/scsi/qla4xxx/ql4_glbl.h
@@ -274,13 +274,14 @@ int qla4xxx_set_acb(struct scsi_qla_host *ha, uint32_t *mbox_cmd,
 int qla4xxx_get_acb(struct scsi_qla_host *ha, dma_addr_t acb_dma,
 		    uint32_t acb_type, uint32_t len);
 int qla4_84xx_config_acb(struct scsi_qla_host *ha, int acb_config);
-int qla4_83xx_ms_mem_write_128b(struct scsi_qla_host *ha,
+int qla4_8xxx_ms_mem_write_128b(struct scsi_qla_host *ha,
 				uint64_t addr, uint32_t *data, uint32_t count);
 uint8_t qla4xxx_set_ipaddr_state(uint8_t fw_ipaddr_state);
 int qla4_83xx_get_port_config(struct scsi_qla_host *ha, uint32_t *config);
 int qla4_83xx_set_port_config(struct scsi_qla_host *ha, uint32_t *config);
 int qla4_8xxx_check_init_adapter_retry(struct scsi_qla_host *ha);
 int qla4_83xx_is_detached(struct scsi_qla_host *ha);
+int qla4xxx_sysfs_ddb_export(struct scsi_qla_host *ha);
 
 extern int ql4xextended_error_logging;
 extern int ql4xdontresethba;
diff --git a/drivers/scsi/qla4xxx/ql4_init.c b/drivers/scsi/qla4xxx/ql4_init.c
index 28fbece7e08..6f12f859b11 100644
--- a/drivers/scsi/qla4xxx/ql4_init.c
+++ b/drivers/scsi/qla4xxx/ql4_init.c
@@ -282,6 +282,25 @@ qla4xxx_wait_for_ip_config(struct scsi_qla_host *ha)
 	return ipv4_wait|ipv6_wait;
 }
 
+static int qla4_80xx_is_minidump_dma_capable(struct scsi_qla_host *ha,
+		struct qla4_8xxx_minidump_template_hdr *md_hdr)
+{
+	int offset = (is_qla8022(ha)) ? QLA8022_TEMPLATE_CAP_OFFSET :
+					QLA83XX_TEMPLATE_CAP_OFFSET;
+	int rval = 1;
+	uint32_t *cap_offset;
+
+	cap_offset = (uint32_t *)((char *)md_hdr + offset);
+
+	if (!(le32_to_cpu(*cap_offset) & BIT_0)) {
+		ql4_printk(KERN_INFO, ha, "PEX DMA Not supported %d\n",
+			   *cap_offset);
+		rval = 0;
+	}
+
+	return rval;
+}
+
 /**
  * qla4xxx_alloc_fw_dump - Allocate memory for minidump data.
  * @ha: pointer to host adapter structure.
@@ -294,6 +313,7 @@ void qla4xxx_alloc_fw_dump(struct scsi_qla_host *ha)
 	void *md_tmp;
 	dma_addr_t md_tmp_dma;
 	struct qla4_8xxx_minidump_template_hdr *md_hdr;
+	int dma_capable;
 
 	if (ha->fw_dump) {
 		ql4_printk(KERN_WARNING, ha,
@@ -326,13 +346,19 @@ void qla4xxx_alloc_fw_dump(struct scsi_qla_host *ha)
 
 	md_hdr = (struct qla4_8xxx_minidump_template_hdr *)md_tmp;
 
+	dma_capable = qla4_80xx_is_minidump_dma_capable(ha, md_hdr);
+
 	capture_debug_level = md_hdr->capture_debug_level;
 
 	/* Get capture mask based on module loadtime setting. */
-	if (ql4xmdcapmask >= 0x3 && ql4xmdcapmask <= 0x7F)
+	if ((ql4xmdcapmask >= 0x3 && ql4xmdcapmask <= 0x7F) ||
+	    (ql4xmdcapmask == 0xFF && dma_capable))  {
 		ha->fw_dump_capture_mask = ql4xmdcapmask;
-	else
+	} else {
+		if (ql4xmdcapmask == 0xFF)
+			ql4_printk(KERN_INFO, ha, "Falling back to default capture mask, as PEX DMA is not supported\n");
 		ha->fw_dump_capture_mask = capture_debug_level;
+	}
 
 	md_hdr->driver_capture_mask = ha->fw_dump_capture_mask;
 
@@ -864,6 +890,8 @@ int qla4xxx_start_firmware(struct scsi_qla_host *ha)
 	if (status == QLA_SUCCESS) {
 		if (test_and_clear_bit(AF_GET_CRASH_RECORD, &ha->flags))
 			qla4xxx_get_crash_record(ha);
+
+		qla4xxx_init_rings(ha);
 	} else {
 		DEBUG(printk("scsi%ld: %s: Firmware has NOT started\n",
 			     ha->host_no, __func__));
diff --git a/drivers/scsi/qla4xxx/ql4_isr.c b/drivers/scsi/qla4xxx/ql4_isr.c
index b1925d195f4..081b6b78d2c 100644
--- a/drivers/scsi/qla4xxx/ql4_isr.c
+++ b/drivers/scsi/qla4xxx/ql4_isr.c
@@ -1526,7 +1526,7 @@ void qla4xxx_process_aen(struct scsi_qla_host * ha, uint8_t process_aen)
 
 int qla4xxx_request_irqs(struct scsi_qla_host *ha)
 {
-	int ret;
+	int ret = 0;
 	int rval = QLA_ERROR;
 
 	if (is_qla40XX(ha))
@@ -1580,15 +1580,13 @@ try_msi:
 		}
 	}
 
-	/*
-	 * Prevent interrupts from falling back to INTx mode in cases where
-	 * interrupts cannot get acquired through MSI-X or MSI mode.
-	 */
+try_intx:
 	if (is_qla8022(ha)) {
-		ql4_printk(KERN_WARNING, ha, "IRQ not attached -- %d.\n", ret);
+		ql4_printk(KERN_WARNING, ha, "%s: ISP82xx Legacy interrupt not supported\n",
+			   __func__);
 		goto irq_not_attached;
 	}
-try_intx:
+
 	/* Trying INTx */
 	ret = request_irq(ha->pdev->irq, ha->isp_ops->intr_handler,
 	    IRQF_SHARED, DRIVER_NAME, ha);
diff --git a/drivers/scsi/qla4xxx/ql4_mbx.c b/drivers/scsi/qla4xxx/ql4_mbx.c
index 0a6b782d6fd..0a3312c6dd6 100644
--- a/drivers/scsi/qla4xxx/ql4_mbx.c
+++ b/drivers/scsi/qla4xxx/ql4_mbx.c
@@ -2381,7 +2381,7 @@ int qla4_84xx_config_acb(struct scsi_qla_host *ha, int acb_config)
 			ql4_printk(KERN_ERR, ha, "%s: Unable to alloc acb\n",
 				   __func__);
 			rval = QLA_ERROR;
-			goto exit_config_acb;
+			goto exit_free_acb;
 		}
 		memcpy(ha->saved_acb, acb, acb_len);
 		break;
@@ -2395,8 +2395,6 @@ int qla4_84xx_config_acb(struct scsi_qla_host *ha, int acb_config)
 		}
 
 		memcpy(acb, ha->saved_acb, acb_len);
-		kfree(ha->saved_acb);
-		ha->saved_acb = NULL;
 
 		rval = qla4xxx_set_acb(ha, &mbox_cmd[0], &mbox_sts[0], acb_dma);
 		if (rval != QLA_SUCCESS)
@@ -2412,6 +2410,10 @@ exit_free_acb:
 	dma_free_coherent(&ha->pdev->dev, sizeof(struct addr_ctrl_blk), acb,
 			  acb_dma);
 exit_config_acb:
+	if ((acb_config == ACB_CONFIG_SET) && ha->saved_acb) {
+		kfree(ha->saved_acb);
+		ha->saved_acb = NULL;
+	}
 	DEBUG2(ql4_printk(KERN_INFO, ha,
 			  "%s %s\n", __func__,
 			  rval == QLA_SUCCESS ? "SUCCEEDED" : "FAILED"));
diff --git a/drivers/scsi/qla4xxx/ql4_nx.c b/drivers/scsi/qla4xxx/ql4_nx.c
index 63328c812b7..9dbdb4be2d8 100644
--- a/drivers/scsi/qla4xxx/ql4_nx.c
+++ b/drivers/scsi/qla4xxx/ql4_nx.c
@@ -14,6 +14,7 @@
 
 #include <asm-generic/io-64-nonatomic-lo-hi.h>
 
+#define TIMEOUT_100_MS	100
 #define MASK(n)		DMA_BIT_MASK(n)
 #define MN_WIN(addr)	(((addr & 0x1fc0000) >> 1) | ((addr >> 25) & 0x3ff))
 #define OCM_WIN(addr)	(((addr & 0x1ff0000) >> 1) | ((addr >> 25) & 0x3ff))
@@ -1176,6 +1177,112 @@ qla4_82xx_pinit_from_rom(struct scsi_qla_host *ha, int verbose)
 	return 0;
 }
 
+/**
+ * qla4_8xxx_ms_mem_write_128b - Writes data to MS/off-chip memory
+ * @ha: Pointer to adapter structure
+ * @addr: Flash address to write to
+ * @data: Data to be written
+ * @count: word_count to be written
+ *
+ * Return: On success return QLA_SUCCESS
+ *         On error return QLA_ERROR
+ **/
+int qla4_8xxx_ms_mem_write_128b(struct scsi_qla_host *ha, uint64_t addr,
+				uint32_t *data, uint32_t count)
+{
+	int i, j;
+	uint32_t agt_ctrl;
+	unsigned long flags;
+	int ret_val = QLA_SUCCESS;
+
+	/* Only 128-bit aligned access */
+	if (addr & 0xF) {
+		ret_val = QLA_ERROR;
+		goto exit_ms_mem_write;
+	}
+
+	write_lock_irqsave(&ha->hw_lock, flags);
+
+	/* Write address */
+	ret_val = ha->isp_ops->wr_reg_indirect(ha, MD_MIU_TEST_AGT_ADDR_HI, 0);
+	if (ret_val == QLA_ERROR) {
+		ql4_printk(KERN_ERR, ha, "%s: write to AGT_ADDR_HI failed\n",
+			   __func__);
+		goto exit_ms_mem_write_unlock;
+	}
+
+	for (i = 0; i < count; i++, addr += 16) {
+		if (!((QLA8XXX_ADDR_IN_RANGE(addr, QLA8XXX_ADDR_QDR_NET,
+					     QLA8XXX_ADDR_QDR_NET_MAX)) ||
+		      (QLA8XXX_ADDR_IN_RANGE(addr, QLA8XXX_ADDR_DDR_NET,
+					     QLA8XXX_ADDR_DDR_NET_MAX)))) {
+			ret_val = QLA_ERROR;
+			goto exit_ms_mem_write_unlock;
+		}
+
+		ret_val = ha->isp_ops->wr_reg_indirect(ha,
+						       MD_MIU_TEST_AGT_ADDR_LO,
+						       addr);
+		/* Write data */
+		ret_val |= ha->isp_ops->wr_reg_indirect(ha,
+						MD_MIU_TEST_AGT_WRDATA_LO,
+						*data++);
+		ret_val |= ha->isp_ops->wr_reg_indirect(ha,
+						MD_MIU_TEST_AGT_WRDATA_HI,
+						*data++);
+		ret_val |= ha->isp_ops->wr_reg_indirect(ha,
+						MD_MIU_TEST_AGT_WRDATA_ULO,
+						*data++);
+		ret_val |= ha->isp_ops->wr_reg_indirect(ha,
+						MD_MIU_TEST_AGT_WRDATA_UHI,
+						*data++);
+		if (ret_val == QLA_ERROR) {
+			ql4_printk(KERN_ERR, ha, "%s: write to AGT_WRDATA failed\n",
+				   __func__);
+			goto exit_ms_mem_write_unlock;
+		}
+
+		/* Check write status */
+		ret_val = ha->isp_ops->wr_reg_indirect(ha, MD_MIU_TEST_AGT_CTRL,
+						       MIU_TA_CTL_WRITE_ENABLE);
+		ret_val |= ha->isp_ops->wr_reg_indirect(ha,
+							MD_MIU_TEST_AGT_CTRL,
+							MIU_TA_CTL_WRITE_START);
+		if (ret_val == QLA_ERROR) {
+			ql4_printk(KERN_ERR, ha, "%s: write to AGT_CTRL failed\n",
+				   __func__);
+			goto exit_ms_mem_write_unlock;
+		}
+
+		for (j = 0; j < MAX_CTL_CHECK; j++) {
+			ret_val = ha->isp_ops->rd_reg_indirect(ha,
+							MD_MIU_TEST_AGT_CTRL,
+							&agt_ctrl);
+			if (ret_val == QLA_ERROR) {
+				ql4_printk(KERN_ERR, ha, "%s: failed to read MD_MIU_TEST_AGT_CTRL\n",
+					   __func__);
+				goto exit_ms_mem_write_unlock;
+			}
+			if ((agt_ctrl & MIU_TA_CTL_BUSY) == 0)
+				break;
+		}
+
+		/* Status check failed */
+		if (j >= MAX_CTL_CHECK) {
+			printk_ratelimited(KERN_ERR "%s: MS memory write failed!\n",
+					   __func__);
+			ret_val = QLA_ERROR;
+			goto exit_ms_mem_write_unlock;
+		}
+	}
+
+exit_ms_mem_write_unlock:
+	write_unlock_irqrestore(&ha->hw_lock, flags);
+
+exit_ms_mem_write:
+	return ret_val;
+}
+
 static int
 qla4_82xx_load_from_flash(struct scsi_qla_host *ha, uint32_t image_start)
 {
@@ -1714,6 +1821,101 @@ void qla4_82xx_rom_lock_recovery(struct scsi_qla_host *ha)
 	qla4_82xx_rom_unlock(ha);
 }
 
+static uint32_t ql4_84xx_poll_wait_for_ready(struct scsi_qla_host *ha,
+					     uint32_t addr1, uint32_t mask)
+{
+	unsigned long timeout;
+	uint32_t rval = QLA_SUCCESS;
+	uint32_t temp;
+
+	timeout = jiffies + msecs_to_jiffies(TIMEOUT_100_MS);
+	do {
+		ha->isp_ops->rd_reg_indirect(ha, addr1, &temp);
+		if ((temp & mask) != 0)
+			break;
+
+		if (time_after_eq(jiffies, timeout)) {
+			ql4_printk(KERN_INFO, ha, "Error in processing rdmdio entry\n");
+			return QLA_ERROR;
+		}
+	} while (1);
+
+	return rval;
+}
+
+uint32_t ql4_84xx_ipmdio_rd_reg(struct scsi_qla_host *ha, uint32_t addr1,
+				uint32_t addr3, uint32_t mask, uint32_t addr,
+				uint32_t *data_ptr)
+{
+	int rval = QLA_SUCCESS;
+	uint32_t temp;
+	uint32_t data;
+
+	rval = ql4_84xx_poll_wait_for_ready(ha, addr1, mask);
+	if (rval)
+		goto exit_ipmdio_rd_reg;
+
+	temp = (0x40000000 | addr);
+	ha->isp_ops->wr_reg_indirect(ha, addr1, temp);
+
+	rval = ql4_84xx_poll_wait_for_ready(ha, addr1, mask);
+	if (rval)
+		goto exit_ipmdio_rd_reg;
+
+	ha->isp_ops->rd_reg_indirect(ha, addr3, &data);
+	*data_ptr = data;
+
+exit_ipmdio_rd_reg:
+	return rval;
+}
+
+
+static uint32_t ql4_84xx_poll_wait_ipmdio_bus_idle(struct scsi_qla_host *ha,
+						    uint32_t addr1,
+						    uint32_t addr2,
+						    uint32_t addr3,
+						    uint32_t mask)
+{
+	unsigned long timeout;
+	uint32_t temp;
+	uint32_t rval = QLA_SUCCESS;
+
+	timeout = jiffies + msecs_to_jiffies(TIMEOUT_100_MS);
+	do {
+		ql4_84xx_ipmdio_rd_reg(ha, addr1, addr3, mask, addr2, &temp);
+		if ((temp & 0x1) != 1)
+			break;
+		if (time_after_eq(jiffies, timeout)) {
+			ql4_printk(KERN_INFO, ha, "Error in processing mdiobus idle\n");
+			return QLA_ERROR;
+		}
+	} while (1);
+
+	return rval;
+}
+
+static int ql4_84xx_ipmdio_wr_reg(struct scsi_qla_host *ha,
+				  uint32_t addr1, uint32_t addr3,
+				  uint32_t mask, uint32_t addr,
+				  uint32_t value)
+{
+	int rval = QLA_SUCCESS;
+
+	rval = ql4_84xx_poll_wait_for_ready(ha, addr1, mask);
+	if (rval)
+		goto exit_ipmdio_wr_reg;
+
+	ha->isp_ops->wr_reg_indirect(ha, addr3, value);
+	ha->isp_ops->wr_reg_indirect(ha, addr1, addr);
+
+	rval = ql4_84xx_poll_wait_for_ready(ha, addr1, mask);
+	if (rval)
+		goto exit_ipmdio_wr_reg;
+
+exit_ipmdio_wr_reg:
+	return rval;
+}
+
 static void qla4_8xxx_minidump_process_rdcrb(struct scsi_qla_host *ha,
 				struct qla8xxx_minidump_entry_hdr *entry_hdr,
 				uint32_t **d_ptr)
@@ -1822,7 +2024,7 @@ error_exit:
 	return rval;
 }
 
-static int qla4_83xx_minidump_pex_dma_read(struct scsi_qla_host *ha,
+static int qla4_8xxx_minidump_pex_dma_read(struct scsi_qla_host *ha,
 				struct qla8xxx_minidump_entry_hdr *entry_hdr,
 				uint32_t **d_ptr)
 {
@@ -1899,11 +2101,11 @@ static int qla4_83xx_minidump_pex_dma_read(struct scsi_qla_host *ha,
 		dma_desc.cmd.read_data_size = size;
 
 		/* Prepare: Write pex-dma descriptor to MS memory. */
-		rval = qla4_83xx_ms_mem_write_128b(ha,
+		rval = qla4_8xxx_ms_mem_write_128b(ha,
 			      (uint64_t)m_hdr->desc_card_addr,
 			      (uint32_t *)&dma_desc,
 			      (sizeof(struct qla4_83xx_pex_dma_descriptor)/16));
-		if (rval == -1) {
+		if (rval != QLA_SUCCESS) {
 			ql4_printk(KERN_INFO, ha,
 				   "%s: Error writing rdmem-dma-init to MS !!!\n",
 				   __func__);
@@ -2359,17 +2561,10 @@ static int qla4_8xxx_minidump_process_rdmem(struct scsi_qla_host *ha,
 	uint32_t *data_ptr = *d_ptr;
 	int rval = QLA_SUCCESS;
 
-	if (is_qla8032(ha) || is_qla8042(ha)) {
-		rval = qla4_83xx_minidump_pex_dma_read(ha, entry_hdr,
-						       &data_ptr);
-		if (rval != QLA_SUCCESS) {
-			rval = __qla4_8xxx_minidump_process_rdmem(ha, entry_hdr,
-								  &data_ptr);
-		}
-	} else {
+	rval = qla4_8xxx_minidump_pex_dma_read(ha, entry_hdr, &data_ptr);
+	if (rval != QLA_SUCCESS)
 		rval = __qla4_8xxx_minidump_process_rdmem(ha, entry_hdr,
 							  &data_ptr);
-	}
 	*d_ptr = data_ptr;
 	return rval;
 }
@@ -2440,6 +2635,227 @@ exit_process_pollrd:
 	return rval;
 }
 
+static uint32_t qla4_84xx_minidump_process_rddfe(struct scsi_qla_host *ha,
+				struct qla8xxx_minidump_entry_hdr *entry_hdr,
+				uint32_t **d_ptr)
+{
+	int loop_cnt;
+	uint32_t addr1, addr2, value, data, temp, wrval;
+	uint8_t stride, stride2;
+	uint16_t count;
+	uint32_t poll, mask, data_size, modify_mask;
+	uint32_t wait_count = 0;
+	uint32_t *data_ptr = *d_ptr;
+	struct qla8044_minidump_entry_rddfe *rddfe;
+	uint32_t rval = QLA_SUCCESS;
+
+	rddfe = (struct qla8044_minidump_entry_rddfe *)entry_hdr;
+	addr1 = le32_to_cpu(rddfe->addr_1);
+	value = le32_to_cpu(rddfe->value);
+	stride = le32_to_cpu(rddfe->stride);
+	stride2 = le32_to_cpu(rddfe->stride2);
+	count = le32_to_cpu(rddfe->count);
+
+	poll = le32_to_cpu(rddfe->poll);
+	mask = le32_to_cpu(rddfe->mask);
+	modify_mask = le32_to_cpu(rddfe->modify_mask);
+	data_size = le32_to_cpu(rddfe->data_size);
+
+	addr2 = addr1 + stride;
+
+	for (loop_cnt = 0x0; loop_cnt < count; loop_cnt++) {
+		ha->isp_ops->wr_reg_indirect(ha, addr1, (0x40000000 | value));
+
+		wait_count = 0;
+		while (wait_count < poll) {
+			ha->isp_ops->rd_reg_indirect(ha, addr1, &temp);
+			if ((temp & mask) != 0)
+				break;
+			wait_count++;
+		}
+
+		if (wait_count == poll) {
+			ql4_printk(KERN_ERR, ha, "%s: TIMEOUT\n", __func__);
+			rval = QLA_ERROR;
+			goto exit_process_rddfe;
+		} else {
+			ha->isp_ops->rd_reg_indirect(ha, addr2, &temp);
+			temp = temp & modify_mask;
+			temp = (temp | ((loop_cnt << 16) | loop_cnt));
+			wrval = ((temp << 16) | temp);
+
+			ha->isp_ops->wr_reg_indirect(ha, addr2, wrval);
+			ha->isp_ops->wr_reg_indirect(ha, addr1, value);
+
+			wait_count = 0;
+			while (wait_count < poll) {
+				ha->isp_ops->rd_reg_indirect(ha, addr1, &temp);
+				if ((temp & mask) != 0)
+					break;
+				wait_count++;
+			}
+			if (wait_count == poll) {
+				ql4_printk(KERN_ERR, ha, "%s: TIMEOUT\n",
+					   __func__);
+				rval = QLA_ERROR;
+				goto exit_process_rddfe;
+			}
+
+			ha->isp_ops->wr_reg_indirect(ha, addr1,
+						     ((0x40000000 | value) +
+						     stride2));
+			wait_count = 0;
+			while (wait_count < poll) {
+				ha->isp_ops->rd_reg_indirect(ha, addr1, &temp);
+				if ((temp & mask) != 0)
+					break;
+				wait_count++;
+			}
+
+			if (wait_count == poll) {
+				ql4_printk(KERN_ERR, ha, "%s: TIMEOUT\n",
+					   __func__);
+				rval = QLA_ERROR;
+				goto exit_process_rddfe;
+			}
+
+			ha->isp_ops->rd_reg_indirect(ha, addr2, &data);
+
+			*data_ptr++ = cpu_to_le32(wrval);
+			*data_ptr++ = cpu_to_le32(data);
+		}
+	}
+
+	*d_ptr = data_ptr;
+exit_process_rddfe:
+	return rval;
+}
+
+static uint32_t qla4_84xx_minidump_process_rdmdio(struct scsi_qla_host *ha,
+				struct qla8xxx_minidump_entry_hdr *entry_hdr,
+				uint32_t **d_ptr)
+{
+	int rval = QLA_SUCCESS;
+	uint32_t addr1, addr2, value1, value2, data, selval;
+	uint8_t stride1, stride2;
+	uint32_t addr3, addr4, addr5, addr6, addr7;
+	uint16_t count, loop_cnt;
+	uint32_t poll, mask;
+	uint32_t *data_ptr = *d_ptr;
+	struct qla8044_minidump_entry_rdmdio *rdmdio;
+
+	rdmdio = (struct qla8044_minidump_entry_rdmdio *)entry_hdr;
+	addr1 = le32_to_cpu(rdmdio->addr_1);
+	addr2 = le32_to_cpu(rdmdio->addr_2);
+	value1 = le32_to_cpu(rdmdio->value_1);
+	stride1 = le32_to_cpu(rdmdio->stride_1);
+	stride2 = le32_to_cpu(rdmdio->stride_2);
+	count = le32_to_cpu(rdmdio->count);
+
+	poll = le32_to_cpu(rdmdio->poll);
+	mask = le32_to_cpu(rdmdio->mask);
+	value2 = le32_to_cpu(rdmdio->value_2);
+
+	addr3 = addr1 + stride1;
+
+	for (loop_cnt = 0; loop_cnt < count; loop_cnt++) {
+		rval = ql4_84xx_poll_wait_ipmdio_bus_idle(ha, addr1, addr2,
+							 addr3, mask);
+		if (rval)
+			goto exit_process_rdmdio;
+
+		addr4 = addr2 - stride1;
+		rval = ql4_84xx_ipmdio_wr_reg(ha, addr1, addr3, mask, addr4,
+					     value2);
+		if (rval)
+			goto exit_process_rdmdio;
+
+		addr5 = addr2 - (2 * stride1);
+		rval = ql4_84xx_ipmdio_wr_reg(ha, addr1, addr3, mask, addr5,
+					     value1);
+		if (rval)
+			goto exit_process_rdmdio;
+
+		addr6 = addr2 - (3 * stride1);
+		rval = ql4_84xx_ipmdio_wr_reg(ha, addr1, addr3, mask,
+					     addr6, 0x2);
+		if (rval)
+			goto exit_process_rdmdio;
+
+		rval = ql4_84xx_poll_wait_ipmdio_bus_idle(ha, addr1, addr2,
+							 addr3, mask);
+		if (rval)
+			goto exit_process_rdmdio;
+
+		addr7 = addr2 - (4 * stride1);
+		rval = ql4_84xx_ipmdio_rd_reg(ha, addr1, addr3,
+						      mask, addr7, &data);
+		if (rval)
+			goto exit_process_rdmdio;
+
+		selval = (value2 << 18) | (value1 << 2) | 2;
+
+		stride2 = le32_to_cpu(rdmdio->stride_2);
+		*data_ptr++ = cpu_to_le32(selval);
+		*data_ptr++ = cpu_to_le32(data);
+
+		value1 = value1 + stride2;
+		*d_ptr = data_ptr;
+	}
+
+exit_process_rdmdio:
+	return rval;
+}
+
+static uint32_t qla4_84xx_minidump_process_pollwr(struct scsi_qla_host *ha,
+				struct qla8xxx_minidump_entry_hdr *entry_hdr,
+				uint32_t **d_ptr)
+{
+	uint32_t addr1, addr2, value1, value2, poll, mask, r_value;
+	struct qla8044_minidump_entry_pollwr *pollwr_hdr;
+	uint32_t wait_count = 0;
+	uint32_t rval = QLA_SUCCESS;
+
+	pollwr_hdr = (struct qla8044_minidump_entry_pollwr *)entry_hdr;
+	addr1 = le32_to_cpu(pollwr_hdr->addr_1);
+	addr2 = le32_to_cpu(pollwr_hdr->addr_2);
+	value1 = le32_to_cpu(pollwr_hdr->value_1);
+	value2 = le32_to_cpu(pollwr_hdr->value_2);
+
+	poll = le32_to_cpu(pollwr_hdr->poll);
+	mask = le32_to_cpu(pollwr_hdr->mask);
+
+	while (wait_count < poll) {
+		ha->isp_ops->rd_reg_indirect(ha, addr1, &r_value);
+
+		if ((r_value & poll) != 0)
+			break;
+
+		wait_count++;
+	}
+
+	if (wait_count == poll) {
+		ql4_printk(KERN_ERR, ha, "%s: TIMEOUT\n", __func__);
+		rval = QLA_ERROR;
+		goto exit_process_pollwr;
+	}
+
+	ha->isp_ops->wr_reg_indirect(ha, addr2, value2);
+	ha->isp_ops->wr_reg_indirect(ha, addr1, value1);
+
+	wait_count = 0;
+	while (wait_count < poll) {
+		ha->isp_ops->rd_reg_indirect(ha, addr1, &r_value);
+
+		if ((r_value & poll) != 0)
+			break;
+		wait_count++;
+	}
+
+exit_process_pollwr:
+	return rval;
+}
+
 static void qla83xx_minidump_process_rdmux2(struct scsi_qla_host *ha,
 				struct qla8xxx_minidump_entry_hdr *entry_hdr,
 				uint32_t **d_ptr)
@@ -2753,6 +3169,24 @@ static int qla4_8xxx_collect_md_data(struct scsi_qla_host *ha)
 			if (rval != QLA_SUCCESS)
 				qla4_8xxx_mark_entry_skipped(ha, entry_hdr, i);
 			break;
+		case QLA8044_RDDFE:
+			rval = qla4_84xx_minidump_process_rddfe(ha, entry_hdr,
+								&data_ptr);
+			if (rval != QLA_SUCCESS)
+				qla4_8xxx_mark_entry_skipped(ha, entry_hdr, i);
+			break;
+		case QLA8044_RDMDIO:
+			rval = qla4_84xx_minidump_process_rdmdio(ha, entry_hdr,
+								 &data_ptr);
+			if (rval != QLA_SUCCESS)
+				qla4_8xxx_mark_entry_skipped(ha, entry_hdr, i);
+			break;
+		case QLA8044_POLLWR:
+			rval = qla4_84xx_minidump_process_pollwr(ha, entry_hdr,
+								 &data_ptr);
+			if (rval != QLA_SUCCESS)
+				qla4_8xxx_mark_entry_skipped(ha, entry_hdr, i);
+			break;
 		case QLA8XXX_RDNOP:
 		default:
 			qla4_8xxx_mark_entry_skipped(ha, entry_hdr, i);
diff --git a/drivers/scsi/qla4xxx/ql4_nx.h b/drivers/scsi/qla4xxx/ql4_nx.h
index 14500a0f62c..337d9fcf641 100644
--- a/drivers/scsi/qla4xxx/ql4_nx.h
+++ b/drivers/scsi/qla4xxx/ql4_nx.h
@@ -858,6 +858,9 @@ struct crb_addr_pair {
 #define QLA83XX_POLLRD	35
 #define QLA83XX_RDMUX2	36
 #define QLA83XX_POLLRDMWR  37
+#define QLA8044_RDDFE	38
+#define QLA8044_RDMDIO	39
+#define QLA8044_POLLWR	40
 #define QLA8XXX_RDROM	71
 #define QLA8XXX_RDMEM	72
 #define QLA8XXX_CNTRL	98
diff --git a/drivers/scsi/qla4xxx/ql4_os.c b/drivers/scsi/qla4xxx/ql4_os.c
index 459b9f7186f..32020637620 100644
--- a/drivers/scsi/qla4xxx/ql4_os.c
+++ b/drivers/scsi/qla4xxx/ql4_os.c
@@ -83,12 +83,12 @@ MODULE_PARM_DESC(ql4xsess_recovery_tmo,
 		" Target Session Recovery Timeout.\n"
 		"\t\t  Default: 120 sec.");
 
-int ql4xmdcapmask = 0x1F;
+int ql4xmdcapmask = 0;
 module_param(ql4xmdcapmask, int, S_IRUGO);
 MODULE_PARM_DESC(ql4xmdcapmask,
 		 " Set the Minidump driver capture mask level.\n"
-		 "\t\t  Default is 0x1F.\n"
-		 "\t\t  Can be set to 0x3, 0x7, 0xF, 0x1F, 0x3F, 0x7F");
+		 "\t\t  Default is 0 (firmware default capture mask)\n"
+		 "\t\t  Can be set to 0x3, 0x7, 0xF, 0x1F, 0x3F, 0x7F, 0xFF");
 
 int ql4xenablemd = 1;
 module_param(ql4xenablemd, int, S_IRUGO | S_IWUSR);
@@ -1742,6 +1742,9 @@ static int qla4xxx_get_ep_param(struct iscsi_endpoint *ep,
 	struct sockaddr *dst_addr;
 	struct scsi_qla_host *ha;
 
+	if (!qla_ep)
+		return -ENOTCONN;
+
 	ha = to_qla_host(qla_ep->host);
 	DEBUG2(ql4_printk(KERN_INFO, ha, "%s: host: %ld\n", __func__,
 			  ha->host_no));
@@ -1749,9 +1752,6 @@ static int qla4xxx_get_ep_param(struct iscsi_endpoint *ep,
 	switch (param) {
 	case ISCSI_PARAM_CONN_PORT:
 	case ISCSI_PARAM_CONN_ADDRESS:
-		if (!qla_ep)
-			return -ENOTCONN;
-
 		dst_addr = (struct sockaddr *)&qla_ep->dst_addr;
 		if (!dst_addr)
 			return -ENOTCONN;
@@ -2879,7 +2879,6 @@ static int qla4xxx_conn_get_param(struct iscsi_cls_conn *cls_conn,
 	struct iscsi_conn *conn;
 	struct qla_conn *qla_conn;
 	struct sockaddr *dst_addr;
-	int len = 0;
 
 	conn = cls_conn->dd_data;
 	qla_conn = conn->dd_data;
@@ -2893,9 +2892,6 @@ static int qla4xxx_conn_get_param(struct iscsi_cls_conn *cls_conn,
 	default:
 		return iscsi_conn_get_param(cls_conn, param, buf);
 	}
-
-	return len;
-
 }
 
 int qla4xxx_get_ddb_index(struct scsi_qla_host *ha, uint16_t *ddb_index)
@@ -3569,14 +3565,13 @@ static int qla4xxx_copy_from_fwddb_param(struct iscsi_bus_flash_session *sess,
 	if (test_bit(OPT_IPV6_DEVICE, &options)) {
 		conn->ipv6_traffic_class = fw_ddb_entry->ipv4_tos;
 
-		conn->link_local_ipv6_addr = kzalloc(IPv6_ADDR_LEN, GFP_KERNEL);
+		conn->link_local_ipv6_addr = kmemdup(
+					fw_ddb_entry->link_local_ipv6_addr,
+					IPv6_ADDR_LEN, GFP_KERNEL);
 		if (!conn->link_local_ipv6_addr) {
 			rc = -ENOMEM;
 			goto exit_copy;
 		}
-
-		memcpy(conn->link_local_ipv6_addr,
-		       fw_ddb_entry->link_local_ipv6_addr, IPv6_ADDR_LEN);
 	} else {
 		conn->ipv4_tos = fw_ddb_entry->ipv4_tos;
 	}
@@ -4565,6 +4560,7 @@ static void qla4xxx_timer(struct scsi_qla_host *ha)
 	     test_bit(DPC_LINK_CHANGED, &ha->dpc_flags) ||
 	     test_bit(DPC_HA_UNRECOVERABLE, &ha->dpc_flags) ||
 	     test_bit(DPC_HA_NEED_QUIESCENT, &ha->dpc_flags) ||
+	     test_bit(DPC_SYSFS_DDB_EXPORT, &ha->dpc_flags) ||
 	     test_bit(DPC_AEN, &ha->dpc_flags)) {
 		DEBUG2(printk("scsi%ld: %s: scheduling dpc routine"
 			      " - dpc flags = 0x%lx\n",
@@ -4862,9 +4858,6 @@ static int qla4xxx_recover_adapter(struct scsi_qla_host *ha)
 		    ha->host_no, __func__));
 		status = ha->isp_ops->reset_firmware(ha);
 		if (status == QLA_SUCCESS) {
-			if (!test_bit(AF_FW_RECOVERY, &ha->flags))
-				qla4xxx_cmd_wait(ha);
-
 			ha->isp_ops->disable_intrs(ha);
 			qla4xxx_process_aen(ha, FLUSH_DDB_CHANGED_AENS);
 			qla4xxx_abort_active_cmds(ha, DID_RESET << 16);
@@ -5432,6 +5425,11 @@ dpc_post_reset_ha:
 				qla4xxx_relogin_all_devices(ha);
 		}
 	}
+	if (test_and_clear_bit(DPC_SYSFS_DDB_EXPORT, &ha->dpc_flags)) {
+		if (qla4xxx_sysfs_ddb_export(ha))
+			ql4_printk(KERN_ERR, ha, "%s: Error exporting ddb to sysfs\n",
+				   __func__);
+	}
 }
 
 /**
@@ -8409,7 +8407,7 @@ exit_ddb_del:
  *
  * Export the firmware DDB for all send targets and normal targets to sysfs.
  **/
-static int qla4xxx_sysfs_ddb_export(struct scsi_qla_host *ha)
+int qla4xxx_sysfs_ddb_export(struct scsi_qla_host *ha)
 {
 	struct dev_db_entry *fw_ddb_entry = NULL;
 	dma_addr_t fw_ddb_entry_dma;
@@ -8847,11 +8845,8 @@ skip_retry_init:
 		ql4_printk(KERN_ERR, ha,
 			   "%s: No iSCSI boot target configured\n", __func__);
 
-	if (qla4xxx_sysfs_ddb_export(ha))
-		ql4_printk(KERN_ERR, ha,
-			   "%s: Error exporting ddb to sysfs\n", __func__);
-
-		/* Perform the build ddb list and login to each */
+	set_bit(DPC_SYSFS_DDB_EXPORT, &ha->dpc_flags);
+	/* Perform the build ddb list and login to each */
 	qla4xxx_build_ddb_list(ha, INIT_ADAPTER);
 	iscsi_host_for_each_session(ha->host, qla4xxx_login_flash_ddb);
 	qla4xxx_wait_login_resp_boot_tgt(ha);
diff --git a/drivers/scsi/qla4xxx/ql4_version.h b/drivers/scsi/qla4xxx/ql4_version.h
index c6ba0a6b845..f11eaa77333 100644
--- a/drivers/scsi/qla4xxx/ql4_version.h
+++ b/drivers/scsi/qla4xxx/ql4_version.h
@@ -5,4 +5,4 @@
  * See LICENSE.qla4xxx for copyright and licensing details.
  */
 
-#define QLA4XXX_DRIVER_VERSION	"5.04.00-k4"
+#define QLA4XXX_DRIVER_VERSION	"5.04.00-k6"
diff --git a/drivers/scsi/scsi_debug.c b/drivers/scsi/scsi_debug.c
index f3e9cc038d1..1328a262107 100644
--- a/drivers/scsi/scsi_debug.c
+++ b/drivers/scsi/scsi_debug.c
@@ -130,6 +130,7 @@ static const char * scsi_debug_version_date = "20100324";
 #define SCSI_DEBUG_OPT_DIF_ERR   32
 #define SCSI_DEBUG_OPT_DIX_ERR   64
 #define SCSI_DEBUG_OPT_MAC_TIMEOUT  128
+#define SCSI_DEBUG_OPT_SHORT_TRANSFER	256
 /* When "every_nth" > 0 then modulo "every_nth" commands:
  *   - a no response is simulated if SCSI_DEBUG_OPT_TIMEOUT is set
  *   - a RECOVERED_ERROR is simulated on successful read and write
@@ -3583,6 +3584,7 @@ int scsi_debug_queuecommand_lck(struct scsi_cmnd *SCpnt, done_funct_t done)
 	int inj_transport = 0;
 	int inj_dif = 0;
 	int inj_dix = 0;
+	int inj_short = 0;
 	int delay_override = 0;
 	int unmap = 0;
 
@@ -3628,6 +3630,8 @@ int scsi_debug_queuecommand_lck(struct scsi_cmnd *SCpnt, done_funct_t done)
 			inj_dif = 1; /* to reads and writes below */
 		else if (SCSI_DEBUG_OPT_DIX_ERR & scsi_debug_opts)
 			inj_dix = 1; /* to reads and writes below */
+		else if (SCSI_DEBUG_OPT_SHORT_TRANSFER & scsi_debug_opts)
+			inj_short = 1;
 	}
 
 	if (devip->wlun) {
@@ -3744,6 +3748,10 @@ read:
 		if (scsi_debug_fake_rw)
 			break;
 		get_data_transfer_info(cmd, &lba, &num, &ei_lba);
+
+		if (inj_short)
+			num /= 2;
+
 		errsts = resp_read(SCpnt, lba, num, devip, ei_lba);
 		if (inj_recovered && (0 == errsts)) {
 			mk_sense_buffer(devip, RECOVERED_ERROR,
diff --git a/drivers/scsi/scsi_error.c b/drivers/scsi/scsi_error.c
index f17aa7aa787..cbe38e5e795 100644
--- a/drivers/scsi/scsi_error.c
+++ b/drivers/scsi/scsi_error.c
@@ -1029,6 +1029,7 @@ retry:
 		rtn = NEEDS_RETRY;
 	} else {
 		timeleft = wait_for_completion_timeout(&done, timeout);
+		rtn = SUCCESS;
 	}
 
 	shost->eh_action = NULL;
@@ -1951,6 +1952,8 @@ static void scsi_eh_lock_door(struct scsi_device *sdev)
 	 */
 	req = blk_get_request(sdev->request_queue, READ, GFP_KERNEL);
 
+	blk_rq_set_block_pc(req);
+
 	req->cmd[0] = ALLOW_MEDIUM_REMOVAL;
 	req->cmd[1] = 0;
 	req->cmd[2] = 0;
@@ -1960,7 +1963,6 @@ static void scsi_eh_lock_door(struct scsi_device *sdev)
 
 	req->cmd_len = COMMAND_SIZE(req->cmd[0]);
 
-	req->cmd_type = REQ_TYPE_BLOCK_PC;
 	req->cmd_flags |= REQ_QUIET;
 	req->timeout = 10 * HZ;
 	req->retries = 5;
@@ -2306,6 +2308,12 @@ scsi_reset_provider(struct scsi_device *dev, int flag)
 	}
 
 	scmd = scsi_get_command(dev, GFP_KERNEL);
+	if (!scmd) {
+		rtn = FAILED;
+		put_device(&dev->sdev_gendev);
+		goto out_put_autopm_host;
+	}
+
 	blk_rq_init(NULL, &req);
 	scmd->request = &req;
 
diff --git a/drivers/scsi/scsi_lib.c b/drivers/scsi/scsi_lib.c
index 9db097a28a7..f7e316368c9 100644
--- a/drivers/scsi/scsi_lib.c
+++ b/drivers/scsi/scsi_lib.c
@@ -140,7 +140,7 @@ static void __scsi_queue_insert(struct scsi_cmnd *cmd, int reason, int unbusy)
 	cmd->result = 0;
 	spin_lock_irqsave(q->queue_lock, flags);
 	blk_requeue_request(q, cmd->request);
-	kblockd_schedule_work(q, &device->requeue_work);
+	kblockd_schedule_work(&device->requeue_work);
 	spin_unlock_irqrestore(q->queue_lock, flags);
 }
 
@@ -195,6 +195,7 @@ int scsi_execute(struct scsi_device *sdev, const unsigned char *cmd,
 	req = blk_get_request(sdev->request_queue, write, __GFP_WAIT);
 	if (!req)
 		return ret;
+	blk_rq_set_block_pc(req);
 
 	if (bufflen &&	blk_rq_map_kern(sdev->request_queue, req,
 					buffer, bufflen, __GFP_WAIT))
@@ -206,7 +207,6 @@ int scsi_execute(struct scsi_device *sdev, const unsigned char *cmd,
 	req->sense_len = 0;
 	req->retries = retries;
 	req->timeout = timeout;
-	req->cmd_type = REQ_TYPE_BLOCK_PC;
 	req->cmd_flags |= flags | REQ_QUIET | REQ_PREEMPT;
 
 	/*
@@ -512,68 +512,6 @@ void scsi_run_host_queues(struct Scsi_Host *shost)
 		scsi_run_queue(sdev->request_queue);
 }
 
-static void __scsi_release_buffers(struct scsi_cmnd *, int);
-
-/*
- * Function:    scsi_end_request()
- *
- * Purpose:     Post-processing of completed commands (usually invoked at end
- *		of upper level post-processing and scsi_io_completion).
- *
- * Arguments:   cmd	 - command that is complete.
- *              error    - 0 if I/O indicates success, < 0 for I/O error.
- *              bytes    - number of bytes of completed I/O
- *		requeue  - indicates whether we should requeue leftovers.
- *
- * Lock status: Assumed that lock is not held upon entry.
- *
- * Returns:     cmd if requeue required, NULL otherwise.
- *
- * Notes:       This is called for block device requests in order to
- *              mark some number of sectors as complete.
- * 
- *		We are guaranteeing that the request queue will be goosed
- *		at some point during this call.
- * Notes:	If cmd was requeued, upon return it will be a stale pointer.
- */
-static struct scsi_cmnd *scsi_end_request(struct scsi_cmnd *cmd, int error,
-					  int bytes, int requeue)
-{
-	struct request_queue *q = cmd->device->request_queue;
-	struct request *req = cmd->request;
-
-	/*
-	 * If there are blocks left over at the end, set up the command
-	 * to queue the remainder of them.
-	 */
-	if (blk_end_request(req, error, bytes)) {
-		/* kill remainder if no retrys */
-		if (error && scsi_noretry_cmd(cmd))
-			blk_end_request_all(req, error);
-		else {
-			if (requeue) {
-				/*
-				 * Bleah.  Leftovers again.  Stick the
-				 * leftovers in the front of the
-				 * queue, and goose the queue again.
-				 */
-				scsi_release_buffers(cmd);
-				scsi_requeue_command(q, cmd);
-				cmd = NULL;
-			}
-			return cmd;
-		}
-	}
-
-	/*
-	 * This will goose the queue request function at the end, so we don't
-	 * need to worry about launching another command.
-	 */
-	__scsi_release_buffers(cmd, 0);
-	scsi_next_command(cmd);
-	return NULL;
-}
-
 static inline unsigned int scsi_sgtable_index(unsigned short nents)
 {
 	unsigned int index;
@@ -625,30 +563,10 @@ static void scsi_free_sgtable(struct scsi_data_buffer *sdb)
 	__sg_free_table(&sdb->table, SCSI_MAX_SG_SEGMENTS, scsi_sg_free);
 }
 
-static void __scsi_release_buffers(struct scsi_cmnd *cmd, int do_bidi_check)
-{
-
-	if (cmd->sdb.table.nents)
-		scsi_free_sgtable(&cmd->sdb);
-
-	memset(&cmd->sdb, 0, sizeof(cmd->sdb));
-
-	if (do_bidi_check && scsi_bidi_cmnd(cmd)) {
-		struct scsi_data_buffer *bidi_sdb =
-			cmd->request->next_rq->special;
-		scsi_free_sgtable(bidi_sdb);
-		kmem_cache_free(scsi_sdb_cache, bidi_sdb);
-		cmd->request->next_rq->special = NULL;
-	}
-
-	if (scsi_prot_sg_count(cmd))
-		scsi_free_sgtable(cmd->prot_sdb);
-}
-
 /*
  * Function:    scsi_release_buffers()
  *
- * Purpose:     Completion processing for block device I/O requests.
+ * Purpose:     Free resources allocate for a scsi_command.
  *
  * Arguments:   cmd	- command that we are bailing.
  *
@@ -659,15 +577,29 @@ static void __scsi_release_buffers(struct scsi_cmnd *cmd, int do_bidi_check)
  * Notes:       In the event that an upper level driver rejects a
  *		command, we must release resources allocated during
  *		the __init_io() function.  Primarily this would involve
- *		the scatter-gather table, and potentially any bounce
- *		buffers.
+ *		the scatter-gather table.
  */
 void scsi_release_buffers(struct scsi_cmnd *cmd)
 {
-	__scsi_release_buffers(cmd, 1);
+	if (cmd->sdb.table.nents)
+		scsi_free_sgtable(&cmd->sdb);
+
+	memset(&cmd->sdb, 0, sizeof(cmd->sdb));
+
+	if (scsi_prot_sg_count(cmd))
+		scsi_free_sgtable(cmd->prot_sdb);
 }
 EXPORT_SYMBOL(scsi_release_buffers);
 
+static void scsi_release_bidi_buffers(struct scsi_cmnd *cmd)
+{
+	struct scsi_data_buffer *bidi_sdb = cmd->request->next_rq->special;
+
+	scsi_free_sgtable(bidi_sdb);
+	kmem_cache_free(scsi_sdb_cache, bidi_sdb);
+	cmd->request->next_rq->special = NULL;
+}
+
 /**
  * __scsi_error_from_host_byte - translate SCSI error code into errno
  * @cmd:	SCSI command (unused)
@@ -725,16 +657,9 @@ static int __scsi_error_from_host_byte(struct scsi_cmnd *cmd, int result)
  *
  * Returns:     Nothing
  *
- * Notes:       This function is matched in terms of capabilities to
- *              the function that created the scatter-gather list.
- *              In other words, if there are no bounce buffers
- *              (the normal case for most drivers), we don't need
- *              the logic to deal with cleaning up afterwards.
- *
- *		We must call scsi_end_request().  This will finish off
- *		the specified number of sectors.  If we are done, the
- *		command block will be released and the queue function
- *		will be goosed.  If we are not done then we have to
+ * Notes:       We will finish off the specified number of sectors.  If we
+ *		are done, the command block will be released and the queue
+ *		function will be goosed.  If we are not done then we have to
  *		figure out what to do next:
  *
  *		a) We can call scsi_requeue_command().  The request
@@ -743,7 +668,7 @@ static int __scsi_error_from_host_byte(struct scsi_cmnd *cmd, int result)
  *		   be used if we made forward progress, or if we want
  *		   to switch from READ(10) to READ(6) for example.
  *
- *		b) We can call scsi_queue_insert().  The request will
+ *		b) We can call __scsi_queue_insert().  The request will
  *		   be put back on the queue and retried using the same
  *		   command as before, possibly after a delay.
  *
@@ -801,6 +726,8 @@ void scsi_io_completion(struct scsi_cmnd *cmd, unsigned int good_bytes)
 			req->next_rq->resid_len = scsi_in(cmd)->resid;
 
 			scsi_release_buffers(cmd);
+			scsi_release_bidi_buffers(cmd);
+
 			blk_end_request_all(req, 0);
 
 			scsi_next_command(cmd);
@@ -840,12 +767,25 @@ void scsi_io_completion(struct scsi_cmnd *cmd, unsigned int good_bytes)
 	}
 
 	/*
-	 * A number of bytes were successfully read.  If there
-	 * are leftovers and there is some kind of error
-	 * (result != 0), retry the rest.
+	 * If we finished all bytes in the request we are done now.
 	 */
-	if (scsi_end_request(cmd, error, good_bytes, result == 0) == NULL)
-		return;
+	if (!blk_end_request(req, error, good_bytes))
+		goto next_command;
+
+	/*
+	 * Kill remainder if no retrys.
+	 */
+	if (error && scsi_noretry_cmd(cmd)) {
+		blk_end_request_all(req, error);
+		goto next_command;
+	}
+
+	/*
+	 * If there had been no error, but we have leftover bytes in the
+	 * requeues just queue the command up again.
+	 */
+	if (result == 0)
+		goto requeue;
 
 	error = __scsi_error_from_host_byte(cmd, result);
 
@@ -973,7 +913,6 @@ void scsi_io_completion(struct scsi_cmnd *cmd, unsigned int good_bytes)
 	switch (action) {
 	case ACTION_FAIL:
 		/* Give up and fail the remainder of the request */
-		scsi_release_buffers(cmd);
 		if (!(req->cmd_flags & REQ_QUIET)) {
 			if (description)
 				scmd_printk(KERN_INFO, cmd, "%s\n",
@@ -983,12 +922,11 @@ void scsi_io_completion(struct scsi_cmnd *cmd, unsigned int good_bytes)
 				scsi_print_sense("", cmd);
 			scsi_print_command(cmd);
 		}
-		if (blk_end_request_err(req, error))
-			scsi_requeue_command(q, cmd);
-		else
-			scsi_next_command(cmd);
-		break;
+		if (!blk_end_request_err(req, error))
+			goto next_command;
+		/*FALLTHRU*/
 	case ACTION_REPREP:
+	requeue:
 		/* Unprep the request and put it back at the head of the queue.
 		 * A new command will be prepared and issued.
 		 */
@@ -1004,6 +942,11 @@ void scsi_io_completion(struct scsi_cmnd *cmd, unsigned int good_bytes)
 		__scsi_queue_insert(cmd, SCSI_MLQUEUE_DEVICE_BUSY, 0);
 		break;
 	}
+	return;
+
+next_command:
+	scsi_release_buffers(cmd);
+	scsi_next_command(cmd);
 }
 
 static int scsi_init_sgtable(struct request *req, struct scsi_data_buffer *sdb,
@@ -1019,8 +962,6 @@ static int scsi_init_sgtable(struct request *req, struct scsi_data_buffer *sdb,
 		return BLKPREP_DEFER;
 	}
 
-	req->buffer = NULL;
-
 	/* 
 	 * Next, walk the list, and fill in the addresses and sizes of
 	 * each segment.
@@ -1130,15 +1071,7 @@ static struct scsi_cmnd *scsi_get_cmd_from_req(struct scsi_device *sdev,
 
 int scsi_setup_blk_pc_cmnd(struct scsi_device *sdev, struct request *req)
 {
-	struct scsi_cmnd *cmd;
-	int ret = scsi_prep_state_check(sdev, req);
-
-	if (ret != BLKPREP_OK)
-		return ret;
-
-	cmd = scsi_get_cmd_from_req(sdev, req);
-	if (unlikely(!cmd))
-		return BLKPREP_DEFER;
+	struct scsi_cmnd *cmd = req->special;
 
 	/*
 	 * BLOCK_PC requests may transfer data, in which case they must
@@ -1158,7 +1091,6 @@ int scsi_setup_blk_pc_cmnd(struct scsi_device *sdev, struct request *req)
 		BUG_ON(blk_rq_bytes(req));
 
 		memset(&cmd->sdb, 0, sizeof(cmd->sdb));
-		req->buffer = NULL;
 	}
 
 	cmd->cmd_len = req->cmd_len;
@@ -1182,15 +1114,11 @@ EXPORT_SYMBOL(scsi_setup_blk_pc_cmnd);
  */
 int scsi_setup_fs_cmnd(struct scsi_device *sdev, struct request *req)
 {
-	struct scsi_cmnd *cmd;
-	int ret = scsi_prep_state_check(sdev, req);
-
-	if (ret != BLKPREP_OK)
-		return ret;
+	struct scsi_cmnd *cmd = req->special;
 
 	if (unlikely(sdev->scsi_dh_data && sdev->scsi_dh_data->scsi_dh
 			 && sdev->scsi_dh_data->scsi_dh->prep_fn)) {
-		ret = sdev->scsi_dh_data->scsi_dh->prep_fn(sdev, req);
+		int ret = sdev->scsi_dh_data->scsi_dh->prep_fn(sdev, req);
 		if (ret != BLKPREP_OK)
 			return ret;
 	}
@@ -1200,16 +1128,13 @@ int scsi_setup_fs_cmnd(struct scsi_device *sdev, struct request *req)
 	 */
 	BUG_ON(!req->nr_phys_segments);
 
-	cmd = scsi_get_cmd_from_req(sdev, req);
-	if (unlikely(!cmd))
-		return BLKPREP_DEFER;
-
 	memset(cmd->cmnd, 0, BLK_MAX_CDB);
 	return scsi_init_io(cmd, GFP_ATOMIC);
 }
 EXPORT_SYMBOL(scsi_setup_fs_cmnd);
 
-int scsi_prep_state_check(struct scsi_device *sdev, struct request *req)
+static int
+scsi_prep_state_check(struct scsi_device *sdev, struct request *req)
 {
 	int ret = BLKPREP_OK;
 
@@ -1261,9 +1186,9 @@ int scsi_prep_state_check(struct scsi_device *sdev, struct request *req)
 	}
 	return ret;
 }
-EXPORT_SYMBOL(scsi_prep_state_check);
 
-int scsi_prep_return(struct request_queue *q, struct request *req, int ret)
+static int
+scsi_prep_return(struct request_queue *q, struct request *req, int ret)
 {
 	struct scsi_device *sdev = q->queuedata;
 
@@ -1294,18 +1219,44 @@ int scsi_prep_return(struct request_queue *q, struct request *req, int ret)
 
 	return ret;
 }
-EXPORT_SYMBOL(scsi_prep_return);
 
-int scsi_prep_fn(struct request_queue *q, struct request *req)
+static int scsi_prep_fn(struct request_queue *q, struct request *req)
 {
 	struct scsi_device *sdev = q->queuedata;
-	int ret = BLKPREP_KILL;
+	struct scsi_cmnd *cmd;
+	int ret;
+
+	ret = scsi_prep_state_check(sdev, req);
+	if (ret != BLKPREP_OK)
+		goto out;
 
-	if (req->cmd_type == REQ_TYPE_BLOCK_PC)
+	cmd = scsi_get_cmd_from_req(sdev, req);
+	if (unlikely(!cmd)) {
+		ret = BLKPREP_DEFER;
+		goto out;
+	}
+
+	if (req->cmd_type == REQ_TYPE_FS)
+		ret = scsi_cmd_to_driver(cmd)->init_command(cmd);
+	else if (req->cmd_type == REQ_TYPE_BLOCK_PC)
 		ret = scsi_setup_blk_pc_cmnd(sdev, req);
+	else
+		ret = BLKPREP_KILL;
+
+out:
 	return scsi_prep_return(q, req, ret);
 }
-EXPORT_SYMBOL(scsi_prep_fn);
+
+static void scsi_unprep_fn(struct request_queue *q, struct request *req)
+{
+	if (req->cmd_type == REQ_TYPE_FS) {
+		struct scsi_cmnd *cmd = req->special;
+		struct scsi_driver *drv = scsi_cmd_to_driver(cmd);
+
+		if (drv->uninit_command)
+			drv->uninit_command(cmd);
+	}
+}
 
 /*
  * scsi_dev_queue_ready: if we can send requests to sdev, return 1 else
@@ -1726,6 +1677,7 @@ struct request_queue *scsi_alloc_queue(struct scsi_device *sdev)
 		return NULL;
 
 	blk_queue_prep_rq(q, scsi_prep_fn);
+	blk_queue_unprep_rq(q, scsi_unprep_fn);
 	blk_queue_softirq_done(q, scsi_softirq_done);
 	blk_queue_rq_timed_out(q, scsi_times_out);
 	blk_queue_lld_busy(q, scsi_lld_busy);
diff --git a/drivers/scsi/scsi_sysctl.c b/drivers/scsi/scsi_sysctl.c
index 2b6b93f7d8e..546f16299ef 100644
--- a/drivers/scsi/scsi_sysctl.c
+++ b/drivers/scsi/scsi_sysctl.c
@@ -12,7 +12,7 @@
 #include "scsi_priv.h"
 
 
-static ctl_table scsi_table[] = {
+static struct ctl_table scsi_table[] = {
 	{ .procname	= "logging_level",
 	  .data		= &scsi_logging_level,
 	  .maxlen	= sizeof(scsi_logging_level),
@@ -21,14 +21,14 @@ static ctl_table scsi_table[] = {
 	{ }
 };
 
-static ctl_table scsi_dir_table[] = {
+static struct ctl_table scsi_dir_table[] = {
 	{ .procname	= "scsi",
 	  .mode		= 0555,
 	  .child	= scsi_table },
 	{ }
 };
 
-static ctl_table scsi_root_table[] = {
+static struct ctl_table scsi_root_table[] = {
 	{ .procname	= "dev",
 	  .mode		= 0555,
 	  .child	= scsi_dir_table },
diff --git a/drivers/scsi/scsi_transport_sas.c b/drivers/scsi/scsi_transport_sas.c
index 1b681427dde..c341f855fad 100644
--- a/drivers/scsi/scsi_transport_sas.c
+++ b/drivers/scsi/scsi_transport_sas.c
@@ -1621,8 +1621,6 @@ void sas_rphy_free(struct sas_rphy *rphy)
 	list_del(&rphy->list);
 	mutex_unlock(&sas_host->lock);
 
-	sas_bsg_remove(shost, rphy);
-
 	transport_destroy_device(dev);
 
 	put_device(dev);
@@ -1681,6 +1679,7 @@ sas_rphy_remove(struct sas_rphy *rphy)
 	}
 
 	sas_rphy_unlink(rphy);
+	sas_bsg_remove(NULL, rphy);
 	transport_remove_device(dev);
 	device_del(dev);
 }
diff --git a/drivers/scsi/sd.c b/drivers/scsi/sd.c
index efcbcd18286..e9689d57ccb 100644
--- a/drivers/scsi/sd.c
+++ b/drivers/scsi/sd.c
@@ -109,6 +109,8 @@ static int sd_suspend_system(struct device *);
 static int sd_suspend_runtime(struct device *);
 static int sd_resume(struct device *);
 static void sd_rescan(struct device *);
+static int sd_init_command(struct scsi_cmnd *SCpnt);
+static void sd_uninit_command(struct scsi_cmnd *SCpnt);
 static int sd_done(struct scsi_cmnd *);
 static int sd_eh_action(struct scsi_cmnd *, int);
 static void sd_read_capacity(struct scsi_disk *sdkp, unsigned char *buffer);
@@ -503,6 +505,8 @@ static struct scsi_driver sd_template = {
 		.pm		= &sd_pm_ops,
 	},
 	.rescan			= sd_rescan,
+	.init_command		= sd_init_command,
+	.uninit_command		= sd_uninit_command,
 	.done			= sd_done,
 	.eh_action		= sd_eh_action,
 };
@@ -737,16 +741,14 @@ static int sd_setup_discard_cmnd(struct scsi_device *sdp, struct request *rq)
 		goto out;
 	}
 
+	rq->completion_data = page;
 	blk_add_request_payload(rq, page, len);
 	ret = scsi_setup_blk_pc_cmnd(sdp, rq);
-	rq->buffer = page_address(page);
 	rq->__data_len = nr_bytes;
 
 out:
-	if (ret != BLKPREP_OK) {
+	if (ret != BLKPREP_OK)
 		__free_page(page);
-		rq->buffer = NULL;
-	}
 	return ret;
 }
 
@@ -838,14 +840,13 @@ static int scsi_setup_flush_cmnd(struct scsi_device *sdp, struct request *rq)
 	return scsi_setup_blk_pc_cmnd(sdp, rq);
 }
 
-static void sd_unprep_fn(struct request_queue *q, struct request *rq)
+static void sd_uninit_command(struct scsi_cmnd *SCpnt)
 {
-	struct scsi_cmnd *SCpnt = rq->special;
+	struct request *rq = SCpnt->request;
+
+	if (rq->cmd_flags & REQ_DISCARD)
+		__free_page(rq->completion_data);
 
-	if (rq->cmd_flags & REQ_DISCARD) {
-		free_page((unsigned long)rq->buffer);
-		rq->buffer = NULL;
-	}
 	if (SCpnt->cmnd != rq->cmd) {
 		mempool_free(SCpnt->cmnd, sd_cdb_pool);
 		SCpnt->cmnd = NULL;
@@ -853,18 +854,10 @@ static void sd_unprep_fn(struct request_queue *q, struct request *rq)
 	}
 }
 
-/**
- *	sd_prep_fn - build a scsi (read or write) command from
- *	information in the request structure.
- *	@SCpnt: pointer to mid-level's per scsi command structure that
- *	contains request and into which the scsi command is written
- *
- *	Returns 1 if successful and 0 if error (or cannot be done now).
- **/
-static int sd_prep_fn(struct request_queue *q, struct request *rq)
+static int sd_init_command(struct scsi_cmnd *SCpnt)
 {
-	struct scsi_cmnd *SCpnt;
-	struct scsi_device *sdp = q->queuedata;
+	struct request *rq = SCpnt->request;
+	struct scsi_device *sdp = SCpnt->device;
 	struct gendisk *disk = rq->rq_disk;
 	struct scsi_disk *sdkp;
 	sector_t block = blk_rq_pos(rq);
@@ -886,12 +879,6 @@ static int sd_prep_fn(struct request_queue *q, struct request *rq)
 	} else if (rq->cmd_flags & REQ_FLUSH) {
 		ret = scsi_setup_flush_cmnd(sdp, rq);
 		goto out;
-	} else if (rq->cmd_type == REQ_TYPE_BLOCK_PC) {
-		ret = scsi_setup_blk_pc_cmnd(sdp, rq);
-		goto out;
-	} else if (rq->cmd_type != REQ_TYPE_FS) {
-		ret = BLKPREP_KILL;
-		goto out;
 	}
 	ret = scsi_setup_fs_cmnd(sdp, rq);
 	if (ret != BLKPREP_OK)
@@ -903,11 +890,10 @@ static int sd_prep_fn(struct request_queue *q, struct request *rq)
 	 * is used for a killable error condition */
 	ret = BLKPREP_KILL;
 
-	SCSI_LOG_HLQUEUE(1, scmd_printk(KERN_INFO, SCpnt,
-					"sd_prep_fn: block=%llu, "
-					"count=%d\n",
-					(unsigned long long)block,
-					this_count));
+	SCSI_LOG_HLQUEUE(1,
+		scmd_printk(KERN_INFO, SCpnt,
+			"%s: block=%llu, count=%d\n",
+			__func__, (unsigned long long)block, this_count));
 
 	if (!sdp || !scsi_device_online(sdp) ||
 	    block + blk_rq_sectors(rq) > get_capacity(disk)) {
@@ -1127,7 +1113,7 @@ static int sd_prep_fn(struct request_queue *q, struct request *rq)
 	 */
 	ret = BLKPREP_OK;
  out:
-	return scsi_prep_return(q, rq, ret);
+	return ret;
 }
 
 /**
@@ -1689,12 +1675,12 @@ static int sd_done(struct scsi_cmnd *SCpnt)
 						   sshdr.ascq));
 	}
 #endif
+	sdkp->medium_access_timed_out = 0;
+
 	if (driver_byte(result) != DRIVER_SENSE &&
 	    (!sense_valid || sense_deferred))
 		goto out;
 
-	sdkp->medium_access_timed_out = 0;
-
 	switch (sshdr.sense_key) {
 	case HARDWARE_ERROR:
 	case MEDIUM_ERROR:
@@ -2878,9 +2864,6 @@ static void sd_probe_async(void *data, async_cookie_t cookie)
 
 	sd_revalidate_disk(gd);
 
-	blk_queue_prep_rq(sdp->request_queue, sd_prep_fn);
-	blk_queue_unprep_rq(sdp->request_queue, sd_unprep_fn);
-
 	gd->driverfs_dev = &sdp->sdev_gendev;
 	gd->flags = GENHD_FL_EXT_DEVT;
 	if (sdp->removable) {
@@ -3028,8 +3011,6 @@ static int sd_remove(struct device *dev)
 
 	async_synchronize_full_domain(&scsi_sd_pm_domain);
 	async_synchronize_full_domain(&scsi_sd_probe_domain);
-	blk_queue_prep_rq(sdkp->device->request_queue, scsi_prep_fn);
-	blk_queue_unprep_rq(sdkp->device->request_queue, NULL);
 	device_del(&sdkp->dev);
 	del_gendisk(sdkp->disk);
 	sd_shutdown(dev);
diff --git a/drivers/scsi/sg.c b/drivers/scsi/sg.c
index df5e961484e..53268aaba55 100644
--- a/drivers/scsi/sg.c
+++ b/drivers/scsi/sg.c
@@ -1653,10 +1653,9 @@ static int sg_start_req(Sg_request *srp, unsigned char *cmd)
 	if (!rq)
 		return -ENOMEM;
 
+	blk_rq_set_block_pc(rq);
 	memcpy(rq->cmd, cmd, hp->cmd_len);
-
 	rq->cmd_len = hp->cmd_len;
-	rq->cmd_type = REQ_TYPE_BLOCK_PC;
 
 	srp->rq = rq;
 	rq->end_io_data = srp;
diff --git a/drivers/scsi/sr.c b/drivers/scsi/sr.c
index 40d85929aef..93cbd36c990 100644
--- a/drivers/scsi/sr.c
+++ b/drivers/scsi/sr.c
@@ -79,6 +79,7 @@ MODULE_ALIAS_SCSI_DEVICE(TYPE_WORM);
 static DEFINE_MUTEX(sr_mutex);
 static int sr_probe(struct device *);
 static int sr_remove(struct device *);
+static int sr_init_command(struct scsi_cmnd *SCpnt);
 static int sr_done(struct scsi_cmnd *);
 static int sr_runtime_suspend(struct device *dev);
 
@@ -94,6 +95,7 @@ static struct scsi_driver sr_template = {
 		.remove		= sr_remove,
 		.pm		= &sr_pm_ops,
 	},
+	.init_command		= sr_init_command,
 	.done			= sr_done,
 };
 
@@ -378,21 +380,14 @@ static int sr_done(struct scsi_cmnd *SCpnt)
 	return good_bytes;
 }
 
-static int sr_prep_fn(struct request_queue *q, struct request *rq)
+static int sr_init_command(struct scsi_cmnd *SCpnt)
 {
 	int block = 0, this_count, s_size;
 	struct scsi_cd *cd;
-	struct scsi_cmnd *SCpnt;
-	struct scsi_device *sdp = q->queuedata;
+	struct request *rq = SCpnt->request;
+	struct scsi_device *sdp = SCpnt->device;
 	int ret;
 
-	if (rq->cmd_type == REQ_TYPE_BLOCK_PC) {
-		ret = scsi_setup_blk_pc_cmnd(sdp, rq);
-		goto out;
-	} else if (rq->cmd_type != REQ_TYPE_FS) {
-		ret = BLKPREP_KILL;
-		goto out;
-	}
 	ret = scsi_setup_fs_cmnd(sdp, rq);
 	if (ret != BLKPREP_OK)
 		goto out;
@@ -517,7 +512,7 @@ static int sr_prep_fn(struct request_queue *q, struct request *rq)
 	 */
 	ret = BLKPREP_OK;
  out:
-	return scsi_prep_return(q, rq, ret);
+	return ret;
 }
 
 static int sr_block_open(struct block_device *bdev, fmode_t mode)
@@ -718,7 +713,6 @@ static int sr_probe(struct device *dev)
 
 	/* FIXME: need to handle a get_capabilities failure properly ?? */
 	get_capabilities(cd);
-	blk_queue_prep_rq(sdev->request_queue, sr_prep_fn);
 	sr_vendor_init(cd);
 
 	disk->driverfs_dev = &sdev->sdev_gendev;
@@ -993,7 +987,6 @@ static int sr_remove(struct device *dev)
 
 	scsi_autopm_get_device(cd->device);
 
-	blk_queue_prep_rq(cd->device->request_queue, scsi_prep_fn);
 	del_gendisk(cd->disk);
 
 	mutex_lock(&sr_ref_mutex);
diff --git a/drivers/scsi/st.c b/drivers/scsi/st.c
index afc834e172c..14eb4b256a0 100644
--- a/drivers/scsi/st.c
+++ b/drivers/scsi/st.c
@@ -484,7 +484,7 @@ static int st_scsi_execute(struct st_request *SRpnt, const unsigned char *cmd,
 	if (!req)
 		return DRIVER_ERROR << 24;
 
-	req->cmd_type = REQ_TYPE_BLOCK_PC;
+	blk_rq_set_block_pc(req);
 	req->cmd_flags |= REQ_QUIET;
 
 	mdata->null_mapped = 1;
diff --git a/drivers/scsi/sun3_NCR5380.c b/drivers/scsi/sun3_NCR5380.c
index 636bbe0ea84..88220794cc9 100644
--- a/drivers/scsi/sun3_NCR5380.c
+++ b/drivers/scsi/sun3_NCR5380.c
@@ -364,7 +364,7 @@ static int is_lun_busy(struct scsi_cmnd *cmd, int should_be_tagged)
 	return( 0 );
     if (TagAlloc[cmd->device->id][cmd->device->lun].nr_allocated >=
 	TagAlloc[cmd->device->id][cmd->device->lun].queue_size ) {
-	TAG_PRINTK( "scsi%d: target %d lun %d: no free tags\n",
+	dprintk(NDEBUG_TAGS,  "scsi%d: target %d lun %d: no free tags\n",
 		    H_NO(cmd), cmd->device->id, cmd->device->lun );
 	return( 1 );
     }
@@ -388,7 +388,7 @@ static void cmd_get_tag(struct scsi_cmnd *cmd, int should_be_tagged)
 	!setup_use_tagged_queuing || !cmd->device->tagged_supported) {
 	cmd->tag = TAG_NONE;
 	hostdata->busy[cmd->device->id] |= (1 << cmd->device->lun);
-	TAG_PRINTK( "scsi%d: target %d lun %d now allocated by untagged "
+	dprintk(NDEBUG_TAGS,  "scsi%d: target %d lun %d now allocated by untagged "
 		    "command\n", H_NO(cmd), cmd->device->id, cmd->device->lun );
     }
     else {
@@ -397,7 +397,7 @@ static void cmd_get_tag(struct scsi_cmnd *cmd, int should_be_tagged)
 	cmd->tag = find_first_zero_bit( &ta->allocated, MAX_TAGS );
 	set_bit( cmd->tag, &ta->allocated );
 	ta->nr_allocated++;
-	TAG_PRINTK( "scsi%d: using tag %d for target %d lun %d "
+	dprintk(NDEBUG_TAGS,  "scsi%d: using tag %d for target %d lun %d "
 		    "(now %d tags in use)\n",
 		    H_NO(cmd), cmd->tag, cmd->device->id, cmd->device->lun,
 		    ta->nr_allocated );
@@ -415,7 +415,7 @@ static void cmd_free_tag(struct scsi_cmnd *cmd)
 
     if (cmd->tag == TAG_NONE) {
 	hostdata->busy[cmd->device->id] &= ~(1 << cmd->device->lun);
-	TAG_PRINTK( "scsi%d: target %d lun %d untagged cmd finished\n",
+	dprintk(NDEBUG_TAGS,  "scsi%d: target %d lun %d untagged cmd finished\n",
 		    H_NO(cmd), cmd->device->id, cmd->device->lun );
     }
     else if (cmd->tag >= MAX_TAGS) {
@@ -426,7 +426,7 @@ static void cmd_free_tag(struct scsi_cmnd *cmd)
 	TAG_ALLOC *ta = &TagAlloc[cmd->device->id][cmd->device->lun];
 	clear_bit( cmd->tag, &ta->allocated );
 	ta->nr_allocated--;
-	TAG_PRINTK( "scsi%d: freed tag %d for target %d lun %d\n",
+	dprintk(NDEBUG_TAGS,  "scsi%d: freed tag %d for target %d lun %d\n",
 		    H_NO(cmd), cmd->tag, cmd->device->id, cmd->device->lun );
     }
 }
@@ -484,7 +484,7 @@ static __inline__ void initialize_SCp(struct scsi_cmnd *cmd)
 
 #include <linux/delay.h>
 
-#if 1
+#if NDEBUG
 static struct {
     unsigned char mask;
     const char * name;} 
@@ -572,12 +572,6 @@ static void NCR5380_print_phase(struct Scsi_Host *instance)
     }
 }
 
-#else /* !NDEBUG */
-
-/* dummies... */
-__inline__ void NCR5380_print(struct Scsi_Host *instance) { };
-__inline__ void NCR5380_print_phase(struct Scsi_Host *instance) { };
-
 #endif
 
 /*
@@ -618,7 +612,7 @@ static inline void NCR5380_all_init (void)
 {
     static int done = 0;
     if (!done) {
-	INI_PRINTK("scsi : NCR5380_all_init()\n");
+	dprintk(NDEBUG_INIT, "scsi : NCR5380_all_init()\n");
 	done = 1;
     }
 }
@@ -681,8 +675,8 @@ static void NCR5380_print_status(struct Scsi_Host *instance)
 	Scsi_Cmnd *ptr;
 	unsigned long flags;
 
-	NCR_PRINT(NDEBUG_ANY);
-	NCR_PRINT_PHASE(NDEBUG_ANY);
+	NCR5380_dprint(NDEBUG_ANY, instance);
+	NCR5380_dprint_phase(NDEBUG_ANY, instance);
 
 	hostdata = (struct NCR5380_hostdata *)instance->hostdata;
 
@@ -928,7 +922,7 @@ static int NCR5380_queue_command_lck(struct scsi_cmnd *cmd,
 
     local_irq_restore(flags);
 
-    QU_PRINTK("scsi%d: command added to %s of queue\n", H_NO(cmd),
+    dprintk(NDEBUG_QUEUES, "scsi%d: command added to %s of queue\n", H_NO(cmd),
 	      (cmd->cmnd[0] == REQUEST_SENSE) ? "head" : "tail");
 
     /* If queue_command() is called from an interrupt (real one or bottom
@@ -998,7 +992,7 @@ static void NCR5380_main (struct work_struct *bl)
 	done = 1;
 	
 	if (!hostdata->connected) {
-	    MAIN_PRINTK( "scsi%d: not connected\n", HOSTNO );
+	    dprintk(NDEBUG_MAIN,  "scsi%d: not connected\n", HOSTNO );
 	    /*
 	     * Search through the issue_queue for a command destined
 	     * for a target that's not busy.
@@ -1012,12 +1006,8 @@ static void NCR5380_main (struct work_struct *bl)
 	    for (tmp = (struct scsi_cmnd *) hostdata->issue_queue,
 		 prev = NULL; tmp; prev = tmp, tmp = NEXT(tmp) ) {
 
-#if (NDEBUG & NDEBUG_LISTS)
 		if (prev != tmp)
-		    printk("MAIN tmp=%p   target=%d   busy=%d lun=%d\n",
-			   tmp, tmp->target, hostdata->busy[tmp->target],
-			   tmp->lun);
-#endif
+			dprintk(NDEBUG_LISTS, "MAIN tmp=%p   target=%d   busy=%d lun=%d\n", tmp, tmp->device->id, hostdata->busy[tmp->device->id], tmp->device->lun);
 		/*  When we find one, remove it from the issue queue. */
 		/* ++guenther: possible race with Falcon locking */
 		if (
@@ -1047,9 +1037,9 @@ static void NCR5380_main (struct work_struct *bl)
 		     * On failure, we must add the command back to the
 		     *   issue queue so we can keep trying.	
 		     */
-		    MAIN_PRINTK("scsi%d: main(): command for target %d "
+		    dprintk(NDEBUG_MAIN, "scsi%d: main(): command for target %d "
 				"lun %d removed from issue_queue\n",
-				HOSTNO, tmp->target, tmp->lun);
+				HOSTNO, tmp->device->id, tmp->device->lun);
 		    /* 
 		     * REQUEST SENSE commands are issued without tagged
 		     * queueing, even on SCSI-II devices because the 
@@ -1076,7 +1066,7 @@ static void NCR5380_main (struct work_struct *bl)
 			cmd_free_tag( tmp );
 #endif
 			local_irq_restore(flags);
-			MAIN_PRINTK("scsi%d: main(): select() failed, "
+			dprintk(NDEBUG_MAIN, "scsi%d: main(): select() failed, "
 				    "returned to issue_queue\n", HOSTNO);
 			if (hostdata->connected)
 			    break;
@@ -1090,10 +1080,10 @@ static void NCR5380_main (struct work_struct *bl)
 #endif
 	    ) {
 	    local_irq_restore(flags);
-	    MAIN_PRINTK("scsi%d: main: performing information transfer\n",
+	    dprintk(NDEBUG_MAIN, "scsi%d: main: performing information transfer\n",
 			HOSTNO);
 	    NCR5380_information_transfer(instance);
-	    MAIN_PRINTK("scsi%d: main: done set false\n", HOSTNO);
+	    dprintk(NDEBUG_MAIN, "scsi%d: main: done set false\n", HOSTNO);
 	    done = 0;
 	}
     } while (!done);
@@ -1130,7 +1120,7 @@ static void NCR5380_dma_complete( struct Scsi_Host *instance )
 	return;
     }
 
-    DMA_PRINTK("scsi%d: real DMA transfer complete, basr 0x%X, sr 0x%X\n",
+    dprintk(NDEBUG_DMA, "scsi%d: real DMA transfer complete, basr 0x%X, sr 0x%X\n",
 	       HOSTNO, NCR5380_read(BUS_AND_STATUS_REG),
 	       NCR5380_read(STATUS_REG));
 
@@ -1189,27 +1179,27 @@ static irqreturn_t NCR5380_intr (int irq, void *dev_id)
     int done = 1, handled = 0;
     unsigned char basr;
 
-    INT_PRINTK("scsi%d: NCR5380 irq triggered\n", HOSTNO);
+    dprintk(NDEBUG_INTR, "scsi%d: NCR5380 irq triggered\n", HOSTNO);
 
     /* Look for pending interrupts */
     basr = NCR5380_read(BUS_AND_STATUS_REG);
-    INT_PRINTK("scsi%d: BASR=%02x\n", HOSTNO, basr);
+    dprintk(NDEBUG_INTR, "scsi%d: BASR=%02x\n", HOSTNO, basr);
     /* dispatch to appropriate routine if found and done=0 */
     if (basr & BASR_IRQ) {
-	NCR_PRINT(NDEBUG_INTR);
+	NCR5380_dprint(NDEBUG_INTR, instance);
 	if ((NCR5380_read(STATUS_REG) & (SR_SEL|SR_IO)) == (SR_SEL|SR_IO)) {
 	    done = 0;
 //	    ENABLE_IRQ();
-	    INT_PRINTK("scsi%d: SEL interrupt\n", HOSTNO);
+	    dprintk(NDEBUG_INTR, "scsi%d: SEL interrupt\n", HOSTNO);
 	    NCR5380_reselect(instance);
 	    (void) NCR5380_read(RESET_PARITY_INTERRUPT_REG);
 	}
 	else if (basr & BASR_PARITY_ERROR) {
-	    INT_PRINTK("scsi%d: PARITY interrupt\n", HOSTNO);
+	    dprintk(NDEBUG_INTR, "scsi%d: PARITY interrupt\n", HOSTNO);
 	    (void) NCR5380_read(RESET_PARITY_INTERRUPT_REG);
 	}
 	else if ((NCR5380_read(STATUS_REG) & SR_RST) == SR_RST) {
-	    INT_PRINTK("scsi%d: RESET interrupt\n", HOSTNO);
+	    dprintk(NDEBUG_INTR, "scsi%d: RESET interrupt\n", HOSTNO);
 	    (void)NCR5380_read(RESET_PARITY_INTERRUPT_REG);
 	}
 	else {
@@ -1229,7 +1219,7 @@ static irqreturn_t NCR5380_intr (int irq, void *dev_id)
 		((basr & BASR_END_DMA_TRANSFER) || 
 		 !(basr & BASR_PHASE_MATCH))) {
 		    
-		INT_PRINTK("scsi%d: PHASE MISM or EOP interrupt\n", HOSTNO);
+		dprintk(NDEBUG_INTR, "scsi%d: PHASE MISM or EOP interrupt\n", HOSTNO);
 		NCR5380_dma_complete( instance );
 		done = 0;
 //		ENABLE_IRQ();
@@ -1238,7 +1228,7 @@ static irqreturn_t NCR5380_intr (int irq, void *dev_id)
 	    {
 /* MS: Ignore unknown phase mismatch interrupts (caused by EOP interrupt) */
 		if (basr & BASR_PHASE_MATCH)
-		   INT_PRINTK("scsi%d: unknown interrupt, "
+		   dprintk(NDEBUG_INTR, "scsi%d: unknown interrupt, "
 			   "BASR 0x%x, MR 0x%x, SR 0x%x\n",
 			   HOSTNO, basr, NCR5380_read(MODE_REG),
 			   NCR5380_read(STATUS_REG));
@@ -1262,7 +1252,7 @@ static irqreturn_t NCR5380_intr (int irq, void *dev_id)
     }
     
     if (!done) {
-	INT_PRINTK("scsi%d: in int routine, calling main\n", HOSTNO);
+	dprintk(NDEBUG_INTR, "scsi%d: in int routine, calling main\n", HOSTNO);
 	/* Put a call to NCR5380_main() on the queue... */
 	queue_main();
     }
@@ -1338,8 +1328,8 @@ static int NCR5380_select(struct Scsi_Host *instance, struct scsi_cmnd *cmd,
     unsigned long flags;
 
     hostdata->restart_select = 0;
-    NCR_PRINT(NDEBUG_ARBITRATION);
-    ARB_PRINTK("scsi%d: starting arbitration, id = %d\n", HOSTNO,
+    NCR5380_dprint(NDEBUG_ARBITRATION, instance);
+    dprintk(NDEBUG_ARBITRATION, "scsi%d: starting arbitration, id = %d\n", HOSTNO,
 	       instance->this_id);
 
     /* 
@@ -1385,7 +1375,7 @@ static int NCR5380_select(struct Scsi_Host *instance, struct scsi_cmnd *cmd,
 	 && !hostdata->connected);
 #endif
 
-    ARB_PRINTK("scsi%d: arbitration complete\n", HOSTNO);
+    dprintk(NDEBUG_ARBITRATION, "scsi%d: arbitration complete\n", HOSTNO);
 
     if (hostdata->connected) {
 	NCR5380_write(MODE_REG, MR_BASE); 
@@ -1406,7 +1396,7 @@ static int NCR5380_select(struct Scsi_Host *instance, struct scsi_cmnd *cmd,
 	(NCR5380_read(INITIATOR_COMMAND_REG) & ICR_ARBITRATION_LOST) ||
 	hostdata->connected) {
 	NCR5380_write(MODE_REG, MR_BASE); 
-	ARB_PRINTK("scsi%d: lost arbitration, deasserting MR_ARBITRATE\n",
+	dprintk(NDEBUG_ARBITRATION, "scsi%d: lost arbitration, deasserting MR_ARBITRATE\n",
 		   HOSTNO);
 	return -1;
     }
@@ -1421,7 +1411,7 @@ static int NCR5380_select(struct Scsi_Host *instance, struct scsi_cmnd *cmd,
 	hostdata->connected) {
 	NCR5380_write(MODE_REG, MR_BASE);
 	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-	ARB_PRINTK("scsi%d: lost arbitration, deasserting ICR_ASSERT_SEL\n",
+	dprintk(NDEBUG_ARBITRATION, "scsi%d: lost arbitration, deasserting ICR_ASSERT_SEL\n",
 		   HOSTNO);
 	return -1;
     }
@@ -1444,7 +1434,7 @@ static int NCR5380_select(struct Scsi_Host *instance, struct scsi_cmnd *cmd,
 	return -1;
     }
 
-    ARB_PRINTK("scsi%d: won arbitration\n", HOSTNO);
+    dprintk(NDEBUG_ARBITRATION, "scsi%d: won arbitration\n", HOSTNO);
 
     /* 
      * Now that we have won arbitration, start Selection process, asserting 
@@ -1504,7 +1494,7 @@ static int NCR5380_select(struct Scsi_Host *instance, struct scsi_cmnd *cmd,
 
     udelay(1);
 
-    SEL_PRINTK("scsi%d: selecting target %d\n", HOSTNO, cmd->device->id);
+    dprintk(NDEBUG_SELECTION, "scsi%d: selecting target %d\n", HOSTNO, cmd->device->id);
 
     /* 
      * The SCSI specification calls for a 250 ms timeout for the actual 
@@ -1559,7 +1549,7 @@ static int NCR5380_select(struct Scsi_Host *instance, struct scsi_cmnd *cmd,
 	    printk(KERN_ERR "scsi%d: weirdness\n", HOSTNO);
 	    if (hostdata->restart_select)
 		printk(KERN_NOTICE "\trestart select\n");
-	    NCR_PRINT(NDEBUG_ANY);
+	    NCR5380_dprint(NDEBUG_ANY, instance);
 	    NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
 	    return -1;
 	}
@@ -1572,7 +1562,7 @@ static int NCR5380_select(struct Scsi_Host *instance, struct scsi_cmnd *cmd,
 #endif
 	cmd->scsi_done(cmd);
 	NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
-	SEL_PRINTK("scsi%d: target did not respond within 250ms\n", HOSTNO);
+	dprintk(NDEBUG_SELECTION, "scsi%d: target did not respond within 250ms\n", HOSTNO);
 	NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
 	return 0;
     } 
@@ -1597,7 +1587,7 @@ static int NCR5380_select(struct Scsi_Host *instance, struct scsi_cmnd *cmd,
     /* Wait for start of REQ/ACK handshake */
     while (!(NCR5380_read(STATUS_REG) & SR_REQ));
 
-    SEL_PRINTK("scsi%d: target %d selected, going into MESSAGE OUT phase.\n",
+    dprintk(NDEBUG_SELECTION, "scsi%d: target %d selected, going into MESSAGE OUT phase.\n",
 	       HOSTNO, cmd->device->id);
     tmp[0] = IDENTIFY(1, cmd->device->lun);
 
@@ -1617,7 +1607,7 @@ static int NCR5380_select(struct Scsi_Host *instance, struct scsi_cmnd *cmd,
     data = tmp;
     phase = PHASE_MSGOUT;
     NCR5380_transfer_pio(instance, &phase, &len, &data);
-    SEL_PRINTK("scsi%d: nexus established.\n", HOSTNO);
+    dprintk(NDEBUG_SELECTION, "scsi%d: nexus established.\n", HOSTNO);
     /* XXX need to handle errors here */
     hostdata->connected = cmd;
 #ifndef SUPPORT_TAGS
@@ -1680,12 +1670,12 @@ static int NCR5380_transfer_pio( struct Scsi_Host *instance,
 	 */
 	while (!((tmp = NCR5380_read(STATUS_REG)) & SR_REQ));
 
-	HSH_PRINTK("scsi%d: REQ detected\n", HOSTNO);
+	dprintk(NDEBUG_HANDSHAKE, "scsi%d: REQ detected\n", HOSTNO);
 
 	/* Check for phase mismatch */	
 	if ((tmp & PHASE_MASK) != p) {
-	    PIO_PRINTK("scsi%d: phase mismatch\n", HOSTNO);
-	    NCR_PRINT_PHASE(NDEBUG_PIO);
+	    dprintk(NDEBUG_PIO, "scsi%d: phase mismatch\n", HOSTNO);
+	    NCR5380_dprint_phase(NDEBUG_PIO, instance);
 	    break;
 	}
 
@@ -1708,24 +1698,24 @@ static int NCR5380_transfer_pio( struct Scsi_Host *instance,
 	    if (!((p & SR_MSG) && c > 1)) {
 		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | 
 		    ICR_ASSERT_DATA);
-		NCR_PRINT(NDEBUG_PIO);
+		NCR5380_dprint(NDEBUG_PIO, instance);
 		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | 
 			ICR_ASSERT_DATA | ICR_ASSERT_ACK);
 	    } else {
 		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE |
 		    ICR_ASSERT_DATA | ICR_ASSERT_ATN);
-		NCR_PRINT(NDEBUG_PIO);
+		NCR5380_dprint(NDEBUG_PIO, instance);
 		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | 
 		    ICR_ASSERT_DATA | ICR_ASSERT_ATN | ICR_ASSERT_ACK);
 	    }
 	} else {
-	    NCR_PRINT(NDEBUG_PIO);
+	    NCR5380_dprint(NDEBUG_PIO, instance);
 	    NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_ACK);
 	}
 
 	while (NCR5380_read(STATUS_REG) & SR_REQ);
 
-	HSH_PRINTK("scsi%d: req false, handshake complete\n", HOSTNO);
+	dprintk(NDEBUG_HANDSHAKE, "scsi%d: req false, handshake complete\n", HOSTNO);
 
 /*
  * We have several special cases to consider during REQ/ACK handshaking : 
@@ -1746,7 +1736,7 @@ static int NCR5380_transfer_pio( struct Scsi_Host *instance,
 	} 
     } while (--c);
 
-    PIO_PRINTK("scsi%d: residual %d\n", HOSTNO, c);
+    dprintk(NDEBUG_PIO, "scsi%d: residual %d\n", HOSTNO, c);
 
     *count = c;
     *data = d;
@@ -1854,7 +1844,7 @@ static int NCR5380_transfer_dma( struct Scsi_Host *instance,
     }
     hostdata->dma_len = c;
 
-    DMA_PRINTK("scsi%d: initializing DMA for %s, %d bytes %s %p\n",
+    dprintk(NDEBUG_DMA, "scsi%d: initializing DMA for %s, %d bytes %s %p\n",
 	       HOSTNO, (p & SR_IO) ? "reading" : "writing",
 	       c, (p & SR_IO) ? "to" : "from", *data);
 
@@ -1931,7 +1921,7 @@ static void NCR5380_information_transfer (struct Scsi_Host *instance)
 	    phase = (tmp & PHASE_MASK); 
  	    if (phase != old_phase) {
 		old_phase = phase;
-		NCR_PRINT_PHASE(NDEBUG_INFORMATION);
+		NCR5380_dprint_phase(NDEBUG_INFORMATION, instance);
 	    }
 
 	    if(phase == PHASE_CMDOUT) {
@@ -1996,7 +1986,7 @@ static void NCR5380_information_transfer (struct Scsi_Host *instance)
 		    --cmd->SCp.buffers_residual;
 		    cmd->SCp.this_residual = cmd->SCp.buffer->length;
 		    cmd->SCp.ptr = SGADDR(cmd->SCp.buffer);
-		    INF_PRINTK("scsi%d: %d bytes and %d buffers left\n",
+		    dprintk(NDEBUG_INFORMATION, "scsi%d: %d bytes and %d buffers left\n",
 			       HOSTNO, cmd->SCp.this_residual,
 			       cmd->SCp.buffers_residual);
 		}
@@ -2088,7 +2078,7 @@ static void NCR5380_information_transfer (struct Scsi_Host *instance)
 		    /* Accept message by clearing ACK */
 		    NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
 		    
-		    LNK_PRINTK("scsi%d: target %d lun %d linked command "
+		    dprintk(NDEBUG_LINKED, "scsi%d: target %d lun %d linked command "
 			       "complete.\n", HOSTNO, cmd->device->id, cmd->device->lun);
 
 		    /* Enable reselect interrupts */
@@ -2113,7 +2103,7 @@ static void NCR5380_information_transfer (struct Scsi_Host *instance)
 		     * and don't free it! */
 		    cmd->next_link->tag = cmd->tag;
 		    cmd->result = cmd->SCp.Status | (cmd->SCp.Message << 8); 
-		    LNK_PRINTK("scsi%d: target %d lun %d linked request "
+		    dprintk(NDEBUG_LINKED, "scsi%d: target %d lun %d linked request "
 			       "done, calling scsi_done().\n",
 			       HOSTNO, cmd->device->id, cmd->device->lun);
 #ifdef NCR5380_STATS
@@ -2128,7 +2118,7 @@ static void NCR5380_information_transfer (struct Scsi_Host *instance)
 		    /* Accept message by clearing ACK */
 		    NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
 		    hostdata->connected = NULL;
-		    QU_PRINTK("scsi%d: command for target %d, lun %d "
+		    dprintk(NDEBUG_QUEUES, "scsi%d: command for target %d, lun %d "
 			      "completed\n", HOSTNO, cmd->device->id, cmd->device->lun);
 #ifdef SUPPORT_TAGS
 		    cmd_free_tag( cmd );
@@ -2142,7 +2132,7 @@ static void NCR5380_information_transfer (struct Scsi_Host *instance)
 			/* ++Andreas: the mid level code knows about
 			   QUEUE_FULL now. */
 			TAG_ALLOC *ta = &TagAlloc[cmd->device->id][cmd->device->lun];
-			TAG_PRINTK("scsi%d: target %d lun %d returned "
+			dprintk(NDEBUG_TAGS, "scsi%d: target %d lun %d returned "
 				   "QUEUE_FULL after %d commands\n",
 				   HOSTNO, cmd->device->id, cmd->device->lun,
 				   ta->nr_allocated);
@@ -2186,7 +2176,7 @@ static void NCR5380_information_transfer (struct Scsi_Host *instance)
 		    if ((cmd->cmnd[0] != REQUEST_SENSE) && 
 			(status_byte(cmd->SCp.Status) == CHECK_CONDITION)) {
 			scsi_eh_prep_cmnd(cmd, &hostdata->ses, NULL, 0, ~0);
-			ASEN_PRINTK("scsi%d: performing request sense\n",
+			dprintk(NDEBUG_AUTOSENSE, "scsi%d: performing request sense\n",
 				    HOSTNO);
 			/* this is initialized from initialize_SCp 
 			cmd->SCp.buffer = NULL;
@@ -2198,7 +2188,7 @@ static void NCR5380_information_transfer (struct Scsi_Host *instance)
 			SET_NEXT(cmd, hostdata->issue_queue);
 		        hostdata->issue_queue = (struct scsi_cmnd *) cmd;
 		        local_irq_restore(flags);
-			QU_PRINTK("scsi%d: REQUEST SENSE added to head of "
+			dprintk(NDEBUG_QUEUES, "scsi%d: REQUEST SENSE added to head of "
 				  "issue queue\n", H_NO(cmd));
 		   } else
 #endif /* def AUTOSENSE */
@@ -2238,7 +2228,7 @@ static void NCR5380_information_transfer (struct Scsi_Host *instance)
 			cmd->device->tagged_supported = 0;
 			hostdata->busy[cmd->device->id] |= (1 << cmd->device->lun);
 			cmd->tag = TAG_NONE;
-			TAG_PRINTK("scsi%d: target %d lun %d rejected "
+			dprintk(NDEBUG_TAGS, "scsi%d: target %d lun %d rejected "
 				   "QUEUE_TAG message; tagged queuing "
 				   "disabled\n",
 				   HOSTNO, cmd->device->id, cmd->device->lun);
@@ -2255,7 +2245,7 @@ static void NCR5380_information_transfer (struct Scsi_Host *instance)
 		    hostdata->connected = NULL;
 		    hostdata->disconnected_queue = cmd;
 		    local_irq_restore(flags);
-		    QU_PRINTK("scsi%d: command for target %d lun %d was "
+		    dprintk(NDEBUG_QUEUES, "scsi%d: command for target %d lun %d was "
 			      "moved from connected to the "
 			      "disconnected_queue\n", HOSTNO, 
 			      cmd->device->id, cmd->device->lun);
@@ -2308,13 +2298,13 @@ static void NCR5380_information_transfer (struct Scsi_Host *instance)
 		    /* Accept first byte by clearing ACK */
 		    NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
 
-		    EXT_PRINTK("scsi%d: receiving extended message\n", HOSTNO);
+		    dprintk(NDEBUG_EXTENDED, "scsi%d: receiving extended message\n", HOSTNO);
 
 		    len = 2;
 		    data = extended_msg + 1;
 		    phase = PHASE_MSGIN;
 		    NCR5380_transfer_pio(instance, &phase, &len, &data);
-		    EXT_PRINTK("scsi%d: length=%d, code=0x%02x\n", HOSTNO,
+		    dprintk(NDEBUG_EXTENDED, "scsi%d: length=%d, code=0x%02x\n", HOSTNO,
 			       (int)extended_msg[1], (int)extended_msg[2]);
 
 		    if (!len && extended_msg[1] <= 
@@ -2326,7 +2316,7 @@ static void NCR5380_information_transfer (struct Scsi_Host *instance)
 			phase = PHASE_MSGIN;
 
 			NCR5380_transfer_pio(instance, &phase, &len, &data);
-			EXT_PRINTK("scsi%d: message received, residual %d\n",
+			dprintk(NDEBUG_EXTENDED, "scsi%d: message received, residual %d\n",
 				   HOSTNO, len);
 
 			switch (extended_msg[2]) {
@@ -2416,7 +2406,7 @@ static void NCR5380_information_transfer (struct Scsi_Host *instance)
 		break;
 	    default:
 		printk("scsi%d: unknown phase\n", HOSTNO);
-		NCR_PRINT(NDEBUG_ANY);
+		NCR5380_dprint(NDEBUG_ANY, instance);
 	    } /* switch(phase) */
 	} /* if (tmp * SR_REQ) */ 
     } /* while (1) */
@@ -2458,7 +2448,7 @@ static void NCR5380_reselect (struct Scsi_Host *instance)
 
     target_mask = NCR5380_read(CURRENT_SCSI_DATA_REG) & ~(hostdata->id_mask);
 
-    RSL_PRINTK("scsi%d: reselect\n", HOSTNO);
+    dprintk(NDEBUG_RESELECTION, "scsi%d: reselect\n", HOSTNO);
 
     /* 
      * At this point, we have detected that our SCSI ID is on the bus,
@@ -2580,14 +2570,14 @@ static void NCR5380_reselect (struct Scsi_Host *instance)
 	if (!NCR5380_transfer_pio(instance, &phase, &len, &data) &&
 	    msg[1] == SIMPLE_QUEUE_TAG)
 	    tag = msg[2];
-	TAG_PRINTK("scsi%d: target mask %02x, lun %d sent tag %d at "
+	dprintk(NDEBUG_TAGS, "scsi%d: target mask %02x, lun %d sent tag %d at "
 		   "reselection\n", HOSTNO, target_mask, lun, tag);
     }
 #endif
     
     hostdata->connected = tmp;
-    RSL_PRINTK("scsi%d: nexus established, target = %d, lun = %d, tag = %d\n",
-	       HOSTNO, tmp->target, tmp->lun, tmp->tag);
+    dprintk(NDEBUG_RESELECTION, "scsi%d: nexus established, target = %d, lun = %d, tag = %d\n",
+	       HOSTNO, tmp->device->id, tmp->device->lun, tmp->tag);
 }
 
 
@@ -2622,7 +2612,7 @@ static int NCR5380_abort(struct scsi_cmnd *cmd)
 
     local_irq_save(flags);
     
-    ABRT_PRINTK("scsi%d: abort called basr 0x%02x, sr 0x%02x\n", HOSTNO,
+    dprintk(NDEBUG_ABORT, "scsi%d: abort called basr 0x%02x, sr 0x%02x\n", HOSTNO,
 		NCR5380_read(BUS_AND_STATUS_REG),
 		NCR5380_read(STATUS_REG));
 
@@ -2635,7 +2625,7 @@ static int NCR5380_abort(struct scsi_cmnd *cmd)
 
     if (hostdata->connected == cmd) {
 
-	ABRT_PRINTK("scsi%d: aborting connected command\n", HOSTNO);
+	dprintk(NDEBUG_ABORT, "scsi%d: aborting connected command\n", HOSTNO);
 /*
  * We should perform BSY checking, and make sure we haven't slipped
  * into BUS FREE.
@@ -2664,11 +2654,11 @@ static int NCR5380_abort(struct scsi_cmnd *cmd)
 #endif
 	  local_irq_restore(flags);
 	  cmd->scsi_done(cmd);
-	  return SCSI_ABORT_SUCCESS;
+	  return SUCCESS;
 	} else {
 /*	  local_irq_restore(flags); */
 	  printk("scsi%d: abort of connected command failed!\n", HOSTNO);
-	  return SCSI_ABORT_ERROR;
+	  return FAILED;
 	} 
    }
 #endif
@@ -2686,12 +2676,12 @@ static int NCR5380_abort(struct scsi_cmnd *cmd)
 	    SET_NEXT(tmp, NULL);
 	    tmp->result = DID_ABORT << 16;
 	    local_irq_restore(flags);
-	    ABRT_PRINTK("scsi%d: abort removed command from issue queue.\n",
+	    dprintk(NDEBUG_ABORT, "scsi%d: abort removed command from issue queue.\n",
 			HOSTNO);
 	    /* Tagged queuing note: no tag to free here, hasn't been assigned
 	     * yet... */
 	    tmp->scsi_done(tmp);
-	    return SCSI_ABORT_SUCCESS;
+	    return SUCCESS;
 	}
 
 /* 
@@ -2707,8 +2697,8 @@ static int NCR5380_abort(struct scsi_cmnd *cmd)
 
     if (hostdata->connected) {
 	local_irq_restore(flags);
-	ABRT_PRINTK("scsi%d: abort failed, command connected.\n", HOSTNO);
-        return SCSI_ABORT_SNOOZE;
+	dprintk(NDEBUG_ABORT, "scsi%d: abort failed, command connected.\n", HOSTNO);
+        return FAILED;
     }
 
 /*
@@ -2740,12 +2730,12 @@ static int NCR5380_abort(struct scsi_cmnd *cmd)
 	 tmp = NEXT(tmp)) 
         if (cmd == tmp) {
             local_irq_restore(flags);
-	    ABRT_PRINTK("scsi%d: aborting disconnected command.\n", HOSTNO);
+	    dprintk(NDEBUG_ABORT, "scsi%d: aborting disconnected command.\n", HOSTNO);
   
             if (NCR5380_select (instance, cmd, (int) cmd->tag)) 
-		return SCSI_ABORT_BUSY;
+		return FAILED;
 
-	    ABRT_PRINTK("scsi%d: nexus reestablished.\n", HOSTNO);
+	    dprintk(NDEBUG_ABORT, "scsi%d: nexus reestablished.\n", HOSTNO);
 
 	    do_abort (instance);
 
@@ -2769,7 +2759,7 @@ static int NCR5380_abort(struct scsi_cmnd *cmd)
 #endif
 		    local_irq_restore(flags);
 		    tmp->scsi_done(tmp);
-		    return SCSI_ABORT_SUCCESS;
+		    return SUCCESS;
 		}
 	}
 
@@ -2786,7 +2776,7 @@ static int NCR5380_abort(struct scsi_cmnd *cmd)
     local_irq_restore(flags);
     printk(KERN_INFO "scsi%d: warning : SCSI command probably completed successfully before abortion\n", HOSTNO); 
 
-    return SCSI_ABORT_NOT_RUNNING;
+    return FAILED;
 }
 
 
@@ -2795,7 +2785,7 @@ static int NCR5380_abort(struct scsi_cmnd *cmd)
  * 
  * Purpose : reset the SCSI bus.
  *
- * Returns : SCSI_RESET_WAKEUP
+ * Returns : SUCCESS or FAILURE
  *
  */ 
 
@@ -2804,7 +2794,7 @@ static int NCR5380_bus_reset(struct scsi_cmnd *cmd)
     SETUP_HOSTDATA(cmd->device->host);
     int           i;
     unsigned long flags;
-#if 1
+#if defined(RESET_RUN_DONE)
     struct scsi_cmnd *connected, *disconnected_queue;
 #endif
 
@@ -2826,8 +2816,15 @@ static int NCR5380_bus_reset(struct scsi_cmnd *cmd)
      * through anymore ... */
     (void)NCR5380_read( RESET_PARITY_INTERRUPT_REG );
 
-#if 1 /* XXX Should now be done by midlevel code, but it's broken XXX */
-      /* XXX see below                                            XXX */
+	/* MSch 20140115 - looking at the generic NCR5380 driver, all of this
+	 * should go.
+	 * Catch-22: if we don't clear all queues, the SCSI driver lock will
+	 * not be released by atari_scsi_reset()!
+	 */
+
+#if defined(RESET_RUN_DONE)
+	/* XXX Should now be done by midlevel code, but it's broken XXX */
+	/* XXX see below                                            XXX */
 
     /* MSch: old-style reset: actually abort all command processing here */
 
@@ -2857,7 +2854,7 @@ static int NCR5380_bus_reset(struct scsi_cmnd *cmd)
      */
 
     if ((cmd = connected)) {
-	ABRT_PRINTK("scsi%d: reset aborted a connected command\n", H_NO(cmd));
+	dprintk(NDEBUG_ABORT, "scsi%d: reset aborted a connected command\n", H_NO(cmd));
 	cmd->result = (cmd->result & 0xffff) | (DID_RESET << 16);
 	cmd->scsi_done( cmd );
     }
@@ -2869,14 +2866,14 @@ static int NCR5380_bus_reset(struct scsi_cmnd *cmd)
 	cmd->scsi_done( cmd );
     }
     if (i > 0)
-	ABRT_PRINTK("scsi: reset aborted %d disconnected command(s)\n", i);
+	dprintk(NDEBUG_ABORT, "scsi: reset aborted %d disconnected command(s)\n", i);
 
 
     /* since all commands have been explicitly terminated, we need to tell
      * the midlevel code that the reset was SUCCESSFUL, and there is no 
      * need to 'wake up' the commands by a request_sense
      */
-    return SCSI_RESET_SUCCESS | SCSI_RESET_BUS_RESET;
+    return SUCCESS;
 #else /* 1 */
 
     /* MSch: new-style reset handling: let the mid-level do what it can */
@@ -2903,11 +2900,11 @@ static int NCR5380_bus_reset(struct scsi_cmnd *cmd)
      */
 
     if (hostdata->issue_queue)
-	ABRT_PRINTK("scsi%d: reset aborted issued command(s)\n", H_NO(cmd));
+	dprintk(NDEBUG_ABORT, "scsi%d: reset aborted issued command(s)\n", H_NO(cmd));
     if (hostdata->connected) 
-	ABRT_PRINTK("scsi%d: reset aborted a connected command\n", H_NO(cmd));
+	dprintk(NDEBUG_ABORT, "scsi%d: reset aborted a connected command\n", H_NO(cmd));
     if (hostdata->disconnected_queue)
-	ABRT_PRINTK("scsi%d: reset aborted disconnected command(s)\n", H_NO(cmd));
+	dprintk(NDEBUG_ABORT, "scsi%d: reset aborted disconnected command(s)\n", H_NO(cmd));
 
     local_irq_save(flags);
     hostdata->issue_queue = NULL;
@@ -2924,7 +2921,7 @@ static int NCR5380_bus_reset(struct scsi_cmnd *cmd)
     local_irq_restore(flags);
 
     /* we did no complete reset of all commands, so a wakeup is required */
-    return SCSI_RESET_WAKEUP | SCSI_RESET_BUS_RESET;
+    return SUCCESS;
 #endif /* 1 */
 }
 
diff --git a/drivers/scsi/sun3_scsi.c b/drivers/scsi/sun3_scsi.c
index e2c009b033c..9707b7494a8 100644
--- a/drivers/scsi/sun3_scsi.c
+++ b/drivers/scsi/sun3_scsi.c
@@ -3,6 +3,10 @@
  *
  * Sun3 DMA routines added by Sam Creasey (sammy@sammy.net)
  *
+ * VME support added by Sam Creasey
+ *
+ * TODO: modify this driver to support multiple Sun3 SCSI VME boards
+ *
  * Adapted from mac_scsinew.c:
  */
 /*
@@ -45,10 +49,6 @@
  * USLEEP - enable support for devices that don't disconnect.  Untested.
  */
 
-/*
- * $Log: sun3_NCR5380.c,v $
- */
-
 #define AUTOSENSE
 
 #include <linux/types.h>
@@ -69,23 +69,15 @@
 #include <asm/idprom.h>
 #include <asm/machines.h>
 
-#define NDEBUG 0
-
-#define NDEBUG_ABORT		0x00100000
-#define NDEBUG_TAGS		0x00200000
-#define NDEBUG_MERGING		0x00400000
-
 /* dma on! */
 #define REAL_DMA
 
 #include "scsi.h"
-#include "initio.h"
 #include <scsi/scsi_host.h>
 #include "sun3_scsi.h"
+#include "NCR5380.h"
 
-static void NCR5380_print(struct Scsi_Host *instance);
-
-/* #define OLDDMA */
+extern int sun3_map_test(unsigned long, char *);
 
 #define USE_WRAPPER
 /*#define RESET_BOOT */
@@ -101,7 +93,11 @@ static void NCR5380_print(struct Scsi_Host *instance);
 
 /* #define SUPPORT_TAGS */
 
+#ifdef SUN3_SCSI_VME
+#define ENABLE_IRQ()
+#else
 #define	ENABLE_IRQ()	enable_irq( IRQ_SUN3_SCSI ); 
+#endif
 
 
 static irqreturn_t scsi_sun3_intr(int irq, void *dummy);
@@ -123,6 +119,8 @@ module_param(setup_hostid, int, 0);
 
 static struct scsi_cmnd *sun3_dma_setup_done = NULL;
 
+#define	RESET_RUN_DONE
+
 #define	AFTER_RESET_DELAY	(HZ/2)
 
 /* ms to wait after hitting dma regs */
@@ -136,10 +134,9 @@ static struct scsi_cmnd *sun3_dma_setup_done = NULL;
 
 static volatile unsigned char *sun3_scsi_regp;
 static volatile struct sun3_dma_regs *dregs;
-#ifdef OLDDMA
-static unsigned char *dmabuf = NULL; /* dma memory buffer */
-#endif
+#ifndef SUN3_SCSI_VME
 static struct sun3_udc_regs *udc_regs = NULL;
+#endif
 static unsigned char *sun3_dma_orig_addr = NULL;
 static unsigned long sun3_dma_orig_count = 0;
 static int sun3_dma_active = 0;
@@ -159,6 +156,7 @@ static inline void sun3scsi_write(int reg, int value)
 	sun3_scsi_regp[reg] = value;
 }
 
+#ifndef SUN3_SCSI_VME
 /* dma controller register access functions */
 
 static inline unsigned short sun3_udc_read(unsigned char reg)
@@ -180,6 +178,7 @@ static inline void sun3_udc_write(unsigned short val, unsigned char reg)
 	dregs->udc_data = val;
 	udelay(SUN3_DMA_DELAY);
 }
+#endif
 
 /*
  * XXX: status debug
@@ -198,17 +197,32 @@ static struct Scsi_Host *default_instance;
  *
  */
  
-int __init sun3scsi_detect(struct scsi_host_template * tpnt)
+static int __init sun3scsi_detect(struct scsi_host_template *tpnt)
 {
-	unsigned long ioaddr;
+	unsigned long ioaddr, irq;
 	static int called = 0;
 	struct Scsi_Host *instance;
+#ifdef SUN3_SCSI_VME
+	int i;
+	unsigned long addrs[3] = { IOBASE_SUN3_VMESCSI,
+				   IOBASE_SUN3_VMESCSI + 0x4000,
+				   0 };
+	unsigned long vecs[3] = { SUN3_VEC_VMESCSI0,
+				  SUN3_VEC_VMESCSI1,
+				  0 };
+#endif
 
 	/* check that this machine has an onboard 5380 */
 	switch(idprom->id_machtype) {
+#ifdef SUN3_SCSI_VME
+	case SM_SUN3|SM_3_160:
+	case SM_SUN3|SM_3_260:
+		break;
+#else
 	case SM_SUN3|SM_3_50:
 	case SM_SUN3|SM_3_60:
 		break;
+#endif
 
 	default:
 		return 0;
@@ -217,7 +231,11 @@ int __init sun3scsi_detect(struct scsi_host_template * tpnt)
 	if(called)
 		return 0;
 
+#ifdef SUN3_SCSI_VME
+	tpnt->proc_name = "Sun3 5380 VME SCSI";
+#else
 	tpnt->proc_name = "Sun3 5380 SCSI";
+#endif
 
 	/* setup variables */
 	tpnt->can_queue =
@@ -234,6 +252,38 @@ int __init sun3scsi_detect(struct scsi_host_template * tpnt)
 		tpnt->this_id = 7;
 	}
 
+#ifdef SUN3_SCSI_VME
+	ioaddr = 0;
+	for (i = 0; addrs[i] != 0; i++) {
+		unsigned char x;
+
+		ioaddr = (unsigned long)sun3_ioremap(addrs[i], PAGE_SIZE,
+						     SUN3_PAGE_TYPE_VME16);
+		irq = vecs[i];
+		sun3_scsi_regp = (unsigned char *)ioaddr;
+
+		dregs = (struct sun3_dma_regs *)(((unsigned char *)ioaddr) + 8);
+
+		if (sun3_map_test((unsigned long)dregs, &x)) {
+			unsigned short oldcsr;
+
+			oldcsr = dregs->csr;
+			dregs->csr = 0;
+			udelay(SUN3_DMA_DELAY);
+			if (dregs->csr == 0x1400)
+				break;
+
+			dregs->csr = oldcsr;
+		}
+
+		iounmap((void *)ioaddr);
+		ioaddr = 0;
+	}
+
+	if (!ioaddr)
+		return 0;
+#else
+	irq = IRQ_SUN3_SCSI;
 	ioaddr = (unsigned long)ioremap(IOBASE_SUN3_SCSI, PAGE_SIZE);
 	sun3_scsi_regp = (unsigned char *)ioaddr;
 
@@ -244,11 +294,6 @@ int __init sun3scsi_detect(struct scsi_host_template * tpnt)
 	     printk("SUN3 Scsi couldn't allocate DVMA memory!\n");
 	     return 0;
 	}
-#ifdef OLDDMA
-	if((dmabuf = dvma_malloc_align(SUN3_DVMA_BUFSIZE, 0x10000)) == NULL) {
-	     printk("SUN3 Scsi couldn't allocate DVMA memory!\n");
-	     return 0;
-	}
 #endif
 #ifdef SUPPORT_TAGS
 	if (setup_use_tagged_queuing < 0)
@@ -262,7 +307,7 @@ int __init sun3scsi_detect(struct scsi_host_template * tpnt)
 	default_instance = instance;
 
         instance->io_port = (unsigned long) ioaddr;
-	instance->irq = IRQ_SUN3_SCSI;
+	instance->irq = irq;
 
 	NCR5380_init(instance, 0);
 
@@ -283,7 +328,8 @@ int __init sun3scsi_detect(struct scsi_host_template * tpnt)
 #endif
 	}
 	
-	printk("scsi%d: Sun3 5380 at port %lX irq", instance->host_no, instance->io_port);
+	pr_info("scsi%d: %s at port %lX irq", instance->host_no,
+		tpnt->proc_name, instance->io_port);
 	if (instance->irq == SCSI_IRQ_NONE)
 		printk ("s disabled");
 	else
@@ -300,6 +346,15 @@ int __init sun3scsi_detect(struct scsi_host_template * tpnt)
 	dregs->csr = CSR_SCSI | CSR_FIFO | CSR_INTR;
 	udelay(SUN3_DMA_DELAY);
 	dregs->fifo_count = 0;
+#ifdef SUN3_SCSI_VME
+	dregs->fifo_count_hi = 0;
+	dregs->dma_addr_hi = 0;
+	dregs->dma_addr_lo = 0;
+	dregs->dma_count_hi = 0;
+	dregs->dma_count_lo = 0;
+
+	dregs->ivect = VME_DATA24 | (instance->irq & 0xff);
+#endif
 
 	called = 1;
 
@@ -367,7 +422,8 @@ static void sun3_scsi_reset_boot(struct Scsi_Host *instance)
 }
 #endif
 
-const char * sun3scsi_info (struct Scsi_Host *spnt) {
+static const char *sun3scsi_info(struct Scsi_Host *spnt)
+{
     return "";
 }
 
@@ -379,6 +435,10 @@ static irqreturn_t scsi_sun3_intr(int irq, void *dummy)
 	unsigned short csr = dregs->csr;
 	int handled = 0;
 
+#ifdef SUN3_SCSI_VME
+	dregs->csr &= ~CSR_DMA_ENABLE;
+#endif
+
 	if(csr & ~CSR_GOOD) {
 		if(csr & CSR_DMA_BUSERR) {
 			printk("scsi%d: bus error in dma\n", default_instance->host_no);
@@ -422,31 +482,28 @@ void sun3_sun3_debug (void)
 /* sun3scsi_dma_setup() -- initialize the dma controller for a read/write */
 static unsigned long sun3scsi_dma_setup(void *data, unsigned long count, int write_flag)
 {
-#ifdef OLDDMA
-	if(write_flag) 
-		memcpy(dmabuf, data, count);
-	else {
-		sun3_dma_orig_addr = data;
-		sun3_dma_orig_count = count;
-	}
-#else
 	void *addr;
 
 	if(sun3_dma_orig_addr != NULL)
 		dvma_unmap(sun3_dma_orig_addr);
 
-//	addr = sun3_dvma_page((unsigned long)data, (unsigned long)dmabuf);
+#ifdef SUN3_SCSI_VME
+	addr = (void *)dvma_map_vme((unsigned long) data, count);
+#else
 	addr = (void *)dvma_map((unsigned long) data, count);
+#endif
 		
 	sun3_dma_orig_addr = addr;
 	sun3_dma_orig_count = count;
-#endif
+
+#ifndef SUN3_SCSI_VME
 	dregs->fifo_count = 0;
 	sun3_udc_write(UDC_RESET, UDC_CSR);
 	
 	/* reset fifo */
 	dregs->csr &= ~CSR_FIFO;
 	dregs->csr |= CSR_FIFO;
+#endif
 	
 	/* set direction */
 	if(write_flag)
@@ -454,6 +511,17 @@ static unsigned long sun3scsi_dma_setup(void *data, unsigned long count, int wri
 	else
 		dregs->csr &= ~CSR_SEND;
 	
+#ifdef SUN3_SCSI_VME
+	dregs->csr |= CSR_PACK_ENABLE;
+
+	dregs->dma_addr_hi = ((unsigned long)addr >> 16);
+	dregs->dma_addr_lo = ((unsigned long)addr & 0xffff);
+
+	dregs->dma_count_hi = 0;
+	dregs->dma_count_lo = 0;
+	dregs->fifo_count_hi = 0;
+	dregs->fifo_count = 0;
+#else
 	/* byte count for fifo */
 	dregs->fifo_count = count;
 
@@ -467,17 +535,12 @@ static unsigned long sun3scsi_dma_setup(void *data, unsigned long count, int wri
 		printk("scsi%d: fifo_mismatch %04x not %04x\n",
 		       default_instance->host_no, dregs->fifo_count,
 		       (unsigned int) count);
-		NCR5380_print(default_instance);
+		NCR5380_dprint(NDEBUG_DMA, default_instance);
 	}
 
 	/* setup udc */
-#ifdef OLDDMA
-	udc_regs->addr_hi = ((dvma_vtob(dmabuf) & 0xff0000) >> 8);
-	udc_regs->addr_lo = (dvma_vtob(dmabuf) & 0xffff);
-#else
 	udc_regs->addr_hi = (((unsigned long)(addr) & 0xff0000) >> 8);
 	udc_regs->addr_lo = ((unsigned long)(addr) & 0xffff);
-#endif
 	udc_regs->count = count/2; /* count in words */
 	udc_regs->mode_hi = UDC_MODE_HIWORD;
 	if(write_flag) {
@@ -501,11 +564,13 @@ static unsigned long sun3scsi_dma_setup(void *data, unsigned long count, int wri
 
 	/* interrupt enable */
 	sun3_udc_write(UDC_INT_ENABLE, UDC_CSR);
+#endif
 	
        	return count;
 
 }
 
+#ifndef SUN3_SCSI_VME
 static inline unsigned long sun3scsi_dma_count(struct Scsi_Host *instance)
 {
 	unsigned short resid;
@@ -518,6 +583,7 @@ static inline unsigned long sun3scsi_dma_count(struct Scsi_Host *instance)
 
 	return (unsigned long) resid;
 }
+#endif
 
 static inline unsigned long sun3scsi_dma_residual(struct Scsi_Host *instance)
 {
@@ -536,8 +602,23 @@ static inline unsigned long sun3scsi_dma_xfer_len(unsigned long wanted,
 
 static inline int sun3scsi_dma_start(unsigned long count, unsigned char *data)
 {
+#ifdef SUN3_SCSI_VME
+	unsigned short csr;
+
+	csr = dregs->csr;
 
+	dregs->dma_count_hi = (sun3_dma_orig_count >> 16);
+	dregs->dma_count_lo = (sun3_dma_orig_count & 0xffff);
+
+	dregs->fifo_count_hi = (sun3_dma_orig_count >> 16);
+	dregs->fifo_count = (sun3_dma_orig_count & 0xffff);
+
+/*	if(!(csr & CSR_DMA_ENABLE))
+ *		dregs->csr |= CSR_DMA_ENABLE;
+ */
+#else
     sun3_udc_write(UDC_CHN_START, UDC_CSR);
+#endif
     
     return 0;
 }
@@ -545,12 +626,46 @@ static inline int sun3scsi_dma_start(unsigned long count, unsigned char *data)
 /* clean up after our dma is done */
 static int sun3scsi_dma_finish(int write_flag)
 {
-	unsigned short count;
+	unsigned short __maybe_unused count;
 	unsigned short fifo;
 	int ret = 0;
 	
 	sun3_dma_active = 0;
-#if 1
+
+#ifdef SUN3_SCSI_VME
+	dregs->csr &= ~CSR_DMA_ENABLE;
+
+	fifo = dregs->fifo_count;
+	if (write_flag) {
+		if ((fifo > 0) && (fifo < sun3_dma_orig_count))
+			fifo++;
+	}
+
+	last_residual = fifo;
+	/* empty bytes from the fifo which didn't make it */
+	if ((!write_flag) && (dregs->csr & CSR_LEFT)) {
+		unsigned char *vaddr;
+
+		vaddr = (unsigned char *)dvma_vmetov(sun3_dma_orig_addr);
+
+		vaddr += (sun3_dma_orig_count - fifo);
+		vaddr--;
+
+		switch (dregs->csr & CSR_LEFT) {
+		case CSR_LEFT_3:
+			*vaddr = (dregs->bpack_lo & 0xff00) >> 8;
+			vaddr--;
+
+		case CSR_LEFT_2:
+			*vaddr = (dregs->bpack_hi & 0x00ff);
+			vaddr--;
+
+		case CSR_LEFT_1:
+			*vaddr = (dregs->bpack_hi & 0xff00) >> 8;
+			break;
+		}
+	}
+#else
 	// check to empty the fifo on a read
 	if(!write_flag) {
 		int tmo = 20000; /* .2 sec */
@@ -566,28 +681,8 @@ static int sun3scsi_dma_finish(int write_flag)
 			udelay(10);
 		}
 	}
-		
-#endif
 
 	count = sun3scsi_dma_count(default_instance);
-#ifdef OLDDMA
-
-	/* if we've finished a read, copy out the data we read */
- 	if(sun3_dma_orig_addr) {
-		/* check for residual bytes after dma end */
-		if(count && (NCR5380_read(BUS_AND_STATUS_REG) &
-			     (BASR_PHASE_MATCH | BASR_ACK))) {
-			printk("scsi%d: sun3_scsi_finish: read overrun baby... ", default_instance->host_no);
-			printk("basr now %02x\n", NCR5380_read(BUS_AND_STATUS_REG));
-			ret = count;
-		}
-		
-		/* copy in what we dma'd no matter what */
-		memcpy(sun3_dma_orig_addr, dmabuf, sun3_dma_orig_count);
-		sun3_dma_orig_addr = NULL;
-
-	}
-#else
 
 	fifo = dregs->fifo_count;
 	last_residual = fifo;
@@ -605,10 +700,23 @@ static int sun3scsi_dma_finish(int write_flag)
 		vaddr[-2] = (data & 0xff00) >> 8;
 		vaddr[-1] = (data & 0xff);
 	}
+#endif
 
 	dvma_unmap(sun3_dma_orig_addr);
 	sun3_dma_orig_addr = NULL;
-#endif
+
+#ifdef SUN3_SCSI_VME
+	dregs->dma_addr_hi = 0;
+	dregs->dma_addr_lo = 0;
+	dregs->dma_count_hi = 0;
+	dregs->dma_count_lo = 0;
+
+	dregs->fifo_count = 0;
+	dregs->fifo_count_hi = 0;
+
+	dregs->csr &= ~CSR_SEND;
+/*	dregs->csr |= CSR_DMA_ENABLE; */
+#else
 	sun3_udc_write(UDC_RESET, UDC_CSR);
 	dregs->fifo_count = 0;
 	dregs->csr &= ~CSR_SEND;
@@ -616,6 +724,7 @@ static int sun3scsi_dma_finish(int write_flag)
 	/* reset fifo */
 	dregs->csr &= ~CSR_FIFO;
 	dregs->csr |= CSR_FIFO;
+#endif
 	
 	sun3_dma_setup_done = NULL;
 
diff --git a/drivers/scsi/sun3_scsi.h b/drivers/scsi/sun3_scsi.h
index a8da9c710fe..e96a37cf06a 100644
--- a/drivers/scsi/sun3_scsi.h
+++ b/drivers/scsi/sun3_scsi.h
@@ -29,12 +29,8 @@
  * 1+ (800) 334-5454
  */
 
-/*
- * $Log: cumana_NCR5380.h,v $
- */
-
-#ifndef SUN3_NCR5380_H
-#define SUN3_NCR5380_H
+#ifndef SUN3_SCSI_H
+#define SUN3_SCSI_H
 
 #define SUN3SCSI_PUBLIC_RELEASE 1
 
@@ -82,8 +78,6 @@ static int sun3scsi_release (struct Scsi_Host *);
 #define SUN3_SCSI_NAME "Sun3 NCR5380 SCSI"
 #endif
 
-#ifndef HOSTS_C
-
 #define NCR5380_implementation_fields \
     int port, ctrl
 
@@ -108,9 +102,6 @@ static int sun3scsi_release (struct Scsi_Host *);
 #define NCR5380_dma_read_setup(instance, data, count) sun3scsi_dma_setup(data, count, 0)
 #define NCR5380_dma_residual sun3scsi_dma_residual
 
-#define BOARD_NORMAL	0
-#define BOARD_NCR53C400	1
-
 /* additional registers - mainly DMA control regs */
 /* these start at regbase + 8 -- directly after the NCR regs */
 struct sun3_dma_regs {
@@ -191,189 +182,5 @@ struct sun3_udc_regs {
 
 #define VME_DATA24 0x3d00
 
-// debugging printk's, taken from atari_scsi.h 
-/* Debugging printk definitions:
- *
- *  ARB  -> arbitration
- *  ASEN -> auto-sense
- *  DMA  -> DMA
- *  HSH  -> PIO handshake
- *  INF  -> information transfer
- *  INI  -> initialization
- *  INT  -> interrupt
- *  LNK  -> linked commands
- *  MAIN -> NCR5380_main() control flow
- *  NDAT -> no data-out phase
- *  NWR  -> no write commands
- *  PIO  -> PIO transfers
- *  PDMA -> pseudo DMA (unused on Atari)
- *  QU   -> queues
- *  RSL  -> reselections
- *  SEL  -> selections
- *  USL  -> usleep cpde (unused on Atari)
- *  LBS  -> last byte sent (unused on Atari)
- *  RSS  -> restarting of selections
- *  EXT  -> extended messages
- *  ABRT -> aborting and resetting
- *  TAG  -> queue tag handling
- *  MER  -> merging of consec. buffers
- *
- */
-
-#include "NCR5380.h"
-
-#if NDEBUG & NDEBUG_ARBITRATION
-#define ARB_PRINTK(format, args...) \
-	printk(KERN_DEBUG format , ## args)
-#else
-#define ARB_PRINTK(format, args...)
-#endif
-#if NDEBUG & NDEBUG_AUTOSENSE
-#define ASEN_PRINTK(format, args...) \
-	printk(KERN_DEBUG format , ## args)
-#else
-#define ASEN_PRINTK(format, args...)
-#endif
-#if NDEBUG & NDEBUG_DMA
-#define DMA_PRINTK(format, args...) \
-	printk(KERN_DEBUG format , ## args)
-#else
-#define DMA_PRINTK(format, args...)
-#endif
-#if NDEBUG & NDEBUG_HANDSHAKE
-#define HSH_PRINTK(format, args...) \
-	printk(KERN_DEBUG format , ## args)
-#else
-#define HSH_PRINTK(format, args...)
-#endif
-#if NDEBUG & NDEBUG_INFORMATION
-#define INF_PRINTK(format, args...) \
-	printk(KERN_DEBUG format , ## args)
-#else
-#define INF_PRINTK(format, args...)
-#endif
-#if NDEBUG & NDEBUG_INIT
-#define INI_PRINTK(format, args...) \
-	printk(KERN_DEBUG format , ## args)
-#else
-#define INI_PRINTK(format, args...)
-#endif
-#if NDEBUG & NDEBUG_INTR
-#define INT_PRINTK(format, args...) \
-	printk(KERN_DEBUG format , ## args)
-#else
-#define INT_PRINTK(format, args...)
-#endif
-#if NDEBUG & NDEBUG_LINKED
-#define LNK_PRINTK(format, args...) \
-	printk(KERN_DEBUG format , ## args)
-#else
-#define LNK_PRINTK(format, args...)
-#endif
-#if NDEBUG & NDEBUG_MAIN
-#define MAIN_PRINTK(format, args...) \
-	printk(KERN_DEBUG format , ## args)
-#else
-#define MAIN_PRINTK(format, args...)
-#endif
-#if NDEBUG & NDEBUG_NO_DATAOUT
-#define NDAT_PRINTK(format, args...) \
-	printk(KERN_DEBUG format , ## args)
-#else
-#define NDAT_PRINTK(format, args...)
-#endif
-#if NDEBUG & NDEBUG_NO_WRITE
-#define NWR_PRINTK(format, args...) \
-	printk(KERN_DEBUG format , ## args)
-#else
-#define NWR_PRINTK(format, args...)
-#endif
-#if NDEBUG & NDEBUG_PIO
-#define PIO_PRINTK(format, args...) \
-	printk(KERN_DEBUG format , ## args)
-#else
-#define PIO_PRINTK(format, args...)
-#endif
-#if NDEBUG & NDEBUG_PSEUDO_DMA
-#define PDMA_PRINTK(format, args...) \
-	printk(KERN_DEBUG format , ## args)
-#else
-#define PDMA_PRINTK(format, args...)
-#endif
-#if NDEBUG & NDEBUG_QUEUES
-#define QU_PRINTK(format, args...) \
-	printk(KERN_DEBUG format , ## args)
-#else
-#define QU_PRINTK(format, args...)
-#endif
-#if NDEBUG & NDEBUG_RESELECTION
-#define RSL_PRINTK(format, args...) \
-	printk(KERN_DEBUG format , ## args)
-#else
-#define RSL_PRINTK(format, args...)
-#endif
-#if NDEBUG & NDEBUG_SELECTION
-#define SEL_PRINTK(format, args...) \
-	printk(KERN_DEBUG format , ## args)
-#else
-#define SEL_PRINTK(format, args...)
-#endif
-#if NDEBUG & NDEBUG_USLEEP
-#define USL_PRINTK(format, args...) \
-	printk(KERN_DEBUG format , ## args)
-#else
-#define USL_PRINTK(format, args...)
-#endif
-#if NDEBUG & NDEBUG_LAST_BYTE_SENT
-#define LBS_PRINTK(format, args...) \
-	printk(KERN_DEBUG format , ## args)
-#else
-#define LBS_PRINTK(format, args...)
-#endif
-#if NDEBUG & NDEBUG_RESTART_SELECT
-#define RSS_PRINTK(format, args...) \
-	printk(KERN_DEBUG format , ## args)
-#else
-#define RSS_PRINTK(format, args...)
-#endif
-#if NDEBUG & NDEBUG_EXTENDED
-#define EXT_PRINTK(format, args...) \
-	printk(KERN_DEBUG format , ## args)
-#else
-#define EXT_PRINTK(format, args...)
-#endif
-#if NDEBUG & NDEBUG_ABORT
-#define ABRT_PRINTK(format, args...) \
-	printk(KERN_DEBUG format , ## args)
-#else
-#define ABRT_PRINTK(format, args...)
-#endif
-#if NDEBUG & NDEBUG_TAGS
-#define TAG_PRINTK(format, args...) \
-	printk(KERN_DEBUG format , ## args)
-#else
-#define TAG_PRINTK(format, args...)
-#endif
-#if NDEBUG & NDEBUG_MERGING
-#define MER_PRINTK(format, args...) \
-	printk(KERN_DEBUG format , ## args)
-#else
-#define MER_PRINTK(format, args...)
-#endif
-
-/* conditional macros for NCR5380_print_{,phase,status} */
-
-#define NCR_PRINT(mask)	\
-	((NDEBUG & (mask)) ? NCR5380_print(instance) : (void)0)
-
-#define NCR_PRINT_PHASE(mask) \
-	((NDEBUG & (mask)) ? NCR5380_print_phase(instance) : (void)0)
-
-#define NCR_PRINT_STATUS(mask) \
-	((NDEBUG & (mask)) ? NCR5380_print_status(instance) : (void)0)
-
-
-
-#endif /* ndef HOSTS_C */
-#endif /* SUN3_NCR5380_H */
+#endif /* SUN3_SCSI_H */
 
diff --git a/drivers/scsi/sun3_scsi_vme.c b/drivers/scsi/sun3_scsi_vme.c
index a3dd55d1d2f..1eeece6e204 100644
--- a/drivers/scsi/sun3_scsi_vme.c
+++ b/drivers/scsi/sun3_scsi_vme.c
@@ -1,589 +1,3 @@
- /*
- * Sun3 SCSI stuff by Erik Verbruggen (erik@bigmama.xtdnet.nl)
- *
- * Sun3 DMA routines added by Sam Creasey (sammy@sammy.net)
- *
- * VME support added by Sam Creasey
- *
- * Adapted from sun3_scsi.c -- see there for other headers
- *
- * TODO: modify this driver to support multiple Sun3 SCSI VME boards
- *
- */
-
-#define AUTOSENSE
-
-#include <linux/types.h>
-#include <linux/stddef.h>
-#include <linux/ctype.h>
-#include <linux/delay.h>
-
-#include <linux/module.h>
-#include <linux/signal.h>
-#include <linux/ioport.h>
-#include <linux/init.h>
-#include <linux/blkdev.h>
-
-#include <asm/io.h>
-
-#include <asm/sun3ints.h>
-#include <asm/dvma.h>
-#include <asm/idprom.h>
-#include <asm/machines.h>
-
 #define SUN3_SCSI_VME
 
-#undef SUN3_SCSI_DEBUG
-
-/* dma on! */
-#define REAL_DMA
-
-#define NDEBUG 0
-
-#define NDEBUG_ABORT		0x00100000
-#define NDEBUG_TAGS		0x00200000
-#define NDEBUG_MERGING		0x00400000
-
-#include "scsi.h"
-#include "initio.h"
-#include <scsi/scsi_host.h>
-#include "sun3_scsi.h"
-
-extern int sun3_map_test(unsigned long, char *);
-
-#define USE_WRAPPER
-/*#define RESET_BOOT */
-#define DRIVER_SETUP
-
-/*
- * BUG can be used to trigger a strange code-size related hang on 2.1 kernels
- */
-#ifdef BUG
-#undef RESET_BOOT
-#undef DRIVER_SETUP
-#endif
-
-/* #define SUPPORT_TAGS */
-
-//#define	ENABLE_IRQ()	enable_irq( SUN3_VEC_VMESCSI0 ); 
-#define ENABLE_IRQ()
-
-
-static irqreturn_t scsi_sun3_intr(int irq, void *dummy);
-static inline unsigned char sun3scsi_read(int reg);
-static inline void sun3scsi_write(int reg, int value);
-
-static int setup_can_queue = -1;
-module_param(setup_can_queue, int, 0);
-static int setup_cmd_per_lun = -1;
-module_param(setup_cmd_per_lun, int, 0);
-static int setup_sg_tablesize = -1;
-module_param(setup_sg_tablesize, int, 0);
-#ifdef SUPPORT_TAGS
-static int setup_use_tagged_queuing = -1;
-module_param(setup_use_tagged_queuing, int, 0);
-#endif
-static int setup_hostid = -1;
-module_param(setup_hostid, int, 0);
-
-static struct scsi_cmnd *sun3_dma_setup_done = NULL;
-
-#define	AFTER_RESET_DELAY	(HZ/2)
-
-/* ms to wait after hitting dma regs */
-#define SUN3_DMA_DELAY 10
-
-/* dvma buffer to allocate -- 32k should hopefully be more than sufficient */
-#define SUN3_DVMA_BUFSIZE 0xe000
-
-/* minimum number of bytes to do dma on */
-#define SUN3_DMA_MINSIZE 128
-
-static volatile unsigned char *sun3_scsi_regp;
-static volatile struct sun3_dma_regs *dregs;
-#ifdef OLDDMA
-static unsigned char *dmabuf = NULL; /* dma memory buffer */
-#endif
-static unsigned char *sun3_dma_orig_addr = NULL;
-static unsigned long sun3_dma_orig_count = 0;
-static int sun3_dma_active = 0;
-static unsigned long last_residual = 0;
-
-/*
- * NCR 5380 register access functions
- */
-
-static inline unsigned char sun3scsi_read(int reg)
-{
-	return( sun3_scsi_regp[reg] );
-}
-
-static inline void sun3scsi_write(int reg, int value)
-{
-	sun3_scsi_regp[reg] = value;
-}
-
-/*
- * XXX: status debug
- */
-static struct Scsi_Host *default_instance;
-
-/*
- * Function : int sun3scsi_detect(struct scsi_host_template * tpnt)
- *
- * Purpose : initializes mac NCR5380 driver based on the
- *	command line / compile time port and irq definitions.
- *
- * Inputs : tpnt - template for this SCSI adapter.
- *
- * Returns : 1 if a host adapter was found, 0 if not.
- *
- */
- 
-static int __init sun3scsi_detect(struct scsi_host_template * tpnt)
-{
-	unsigned long ioaddr, irq = 0;
-	static int called = 0;
-	struct Scsi_Host *instance;
-	int i;
-	unsigned long addrs[3] = { IOBASE_SUN3_VMESCSI, 
-				   IOBASE_SUN3_VMESCSI + 0x4000,
-				   0 };
-	unsigned long vecs[3] = { SUN3_VEC_VMESCSI0,
-				  SUN3_VEC_VMESCSI1,
-				  0 };
-	/* check that this machine has an onboard 5380 */
-	switch(idprom->id_machtype) {
-	case SM_SUN3|SM_3_160:
-	case SM_SUN3|SM_3_260:
-		break;
-
-	default:
-		return 0;
-	}
-
-	if(called)
-		return 0;
-
-	tpnt->proc_name = "Sun3 5380 VME SCSI";
-
-	/* setup variables */
-	tpnt->can_queue =
-		(setup_can_queue > 0) ? setup_can_queue : CAN_QUEUE;
-	tpnt->cmd_per_lun =
-		(setup_cmd_per_lun > 0) ? setup_cmd_per_lun : CMD_PER_LUN;
-	tpnt->sg_tablesize = 
-		(setup_sg_tablesize >= 0) ? setup_sg_tablesize : SG_TABLESIZE;
-	
-	if (setup_hostid >= 0)
-		tpnt->this_id = setup_hostid;
-	else {
-		/* use 7 as default */
-		tpnt->this_id = 7;
-	}
-	
-	ioaddr = 0;
-	for(i = 0; addrs[i] != 0; i++) {
-		unsigned char x;
-		
-		ioaddr = (unsigned long)sun3_ioremap(addrs[i], PAGE_SIZE,
-						     SUN3_PAGE_TYPE_VME16);
-		irq = vecs[i];
-		sun3_scsi_regp = (unsigned char *)ioaddr;
-		
-		dregs = (struct sun3_dma_regs *)(((unsigned char *)ioaddr) + 8);
-		
-		if(sun3_map_test((unsigned long)dregs, &x)) {
-			unsigned short oldcsr;
-
-			oldcsr = dregs->csr;
-			dregs->csr = 0;
-			udelay(SUN3_DMA_DELAY);
-			if(dregs->csr == 0x1400)
-				break;
-			
-			dregs->csr = oldcsr;
-		}
-
-		iounmap((void *)ioaddr);
-		ioaddr = 0;
-	}
-
-	if(!ioaddr)
-		return 0;
-	
-#ifdef SUPPORT_TAGS
-	if (setup_use_tagged_queuing < 0)
-		setup_use_tagged_queuing = USE_TAGGED_QUEUING;
-#endif
-
-	instance = scsi_register (tpnt, sizeof(struct NCR5380_hostdata));
-	if(instance == NULL)
-		return 0;
-		
-	default_instance = instance;
-
-        instance->io_port = (unsigned long) ioaddr;
-	instance->irq = irq;
-
-	NCR5380_init(instance, 0);
-
-	instance->n_io_port = 32;
-
-        ((struct NCR5380_hostdata *)instance->hostdata)->ctrl = 0;
-
-	if (request_irq(instance->irq, scsi_sun3_intr,
-			0, "Sun3SCSI-5380VME", instance)) {
-#ifndef REAL_DMA
-		printk("scsi%d: IRQ%d not free, interrupts disabled\n",
-		       instance->host_no, instance->irq);
-		instance->irq = SCSI_IRQ_NONE;
-#else
-		printk("scsi%d: IRQ%d not free, bailing out\n",
-		       instance->host_no, instance->irq);
-		return 0;
-#endif
-	}
-
-	printk("scsi%d: Sun3 5380 VME at port %lX irq", instance->host_no, instance->io_port);
-	if (instance->irq == SCSI_IRQ_NONE)
-		printk ("s disabled");
-	else
-		printk (" %d", instance->irq);
-	printk(" options CAN_QUEUE=%d CMD_PER_LUN=%d release=%d",
-	       instance->can_queue, instance->cmd_per_lun,
-	       SUN3SCSI_PUBLIC_RELEASE);
-	printk("\nscsi%d:", instance->host_no);
-	NCR5380_print_options(instance);
-	printk("\n");
-
-	dregs->csr = 0;
-	udelay(SUN3_DMA_DELAY);
-	dregs->csr = CSR_SCSI | CSR_FIFO | CSR_INTR;
-	udelay(SUN3_DMA_DELAY);
-	dregs->fifo_count = 0;
-	dregs->fifo_count_hi = 0;
-	dregs->dma_addr_hi = 0;
-	dregs->dma_addr_lo = 0;
-	dregs->dma_count_hi = 0;
-	dregs->dma_count_lo = 0;
-
-	dregs->ivect = VME_DATA24 | (instance->irq & 0xff);
-
-	called = 1;
-
-#ifdef RESET_BOOT
-	sun3_scsi_reset_boot(instance);
-#endif
-
-	return 1;
-}
-
-int sun3scsi_release (struct Scsi_Host *shpnt)
-{
-	if (shpnt->irq != SCSI_IRQ_NONE)
-		free_irq(shpnt->irq, shpnt);
-
-	iounmap((void *)sun3_scsi_regp);
-
-	NCR5380_exit(shpnt);
-	return 0;
-}
-
-#ifdef RESET_BOOT
-/*
- * Our 'bus reset on boot' function
- */
-
-static void sun3_scsi_reset_boot(struct Scsi_Host *instance)
-{
-	unsigned long end;
-
-	NCR5380_local_declare();
-	NCR5380_setup(instance);
-	
-	/*
-	 * Do a SCSI reset to clean up the bus during initialization. No
-	 * messing with the queues, interrupts, or locks necessary here.
-	 */
-
-	printk( "Sun3 SCSI: resetting the SCSI bus..." );
-
-	/* switch off SCSI IRQ - catch an interrupt without IRQ bit set else */
-//       	sun3_disable_irq( IRQ_SUN3_SCSI );
-
-	/* get in phase */
-	NCR5380_write( TARGET_COMMAND_REG,
-		      PHASE_SR_TO_TCR( NCR5380_read(STATUS_REG) ));
-
-	/* assert RST */
-	NCR5380_write( INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_RST );
-
-	/* The min. reset hold time is 25us, so 40us should be enough */
-	udelay( 50 );
-
-	/* reset RST and interrupt */
-	NCR5380_write( INITIATOR_COMMAND_REG, ICR_BASE );
-	NCR5380_read( RESET_PARITY_INTERRUPT_REG );
-
-	for( end = jiffies + AFTER_RESET_DELAY; time_before(jiffies, end); )
-		barrier();
-
-	/* switch on SCSI IRQ again */
-//       	sun3_enable_irq( IRQ_SUN3_SCSI );
-
-	printk( " done\n" );
-}
-#endif
-
-static const char * sun3scsi_info (struct Scsi_Host *spnt) {
-    return "";
-}
-
-// safe bits for the CSR
-#define CSR_GOOD 0x060f
-
-static irqreturn_t scsi_sun3_intr(int irq, void *dummy)
-{
-	unsigned short csr = dregs->csr;
-	int handled = 0;
-
-	dregs->csr &= ~CSR_DMA_ENABLE;
-
-
-#ifdef SUN3_SCSI_DEBUG
-	printk("scsi_intr csr %x\n", csr);
-#endif
-
-	if(csr & ~CSR_GOOD) {
-		if(csr & CSR_DMA_BUSERR) {
-			printk("scsi%d: bus error in dma\n", default_instance->host_no);
-#ifdef SUN3_SCSI_DEBUG
-			printk("scsi: residual %x count %x addr %p dmaaddr %x\n", 
-			       dregs->fifo_count,
-			       dregs->dma_count_lo | (dregs->dma_count_hi << 16),
-			       sun3_dma_orig_addr,
-			       dregs->dma_addr_lo | (dregs->dma_addr_hi << 16));
-#endif
-		}
-
-		if(csr & CSR_DMA_CONFLICT) {
-			printk("scsi%d: dma conflict\n", default_instance->host_no);
-		}
-		handled = 1;
-	}
-
-	if(csr & (CSR_SDB_INT | CSR_DMA_INT)) {
-		NCR5380_intr(irq, dummy);
-		handled = 1;
-	}
-
-	return IRQ_RETVAL(handled);
-}
-
-/*
- * Debug stuff - to be called on NMI, or sysrq key. Use at your own risk; 
- * reentering NCR5380_print_status seems to have ugly side effects
- */
-
-/* this doesn't seem to get used at all -- sam */
-#if 0
-void sun3_sun3_debug (void)
-{
-	unsigned long flags;
-	NCR5380_local_declare();
-
-	if (default_instance) {
-			local_irq_save(flags);
-			NCR5380_print_status(default_instance);
-			local_irq_restore(flags);
-	}
-}
-#endif
-
-
-/* sun3scsi_dma_setup() -- initialize the dma controller for a read/write */
-static unsigned long sun3scsi_dma_setup(void *data, unsigned long count, int write_flag)
-{
-	void *addr;
-
-	if(sun3_dma_orig_addr != NULL)
-		dvma_unmap(sun3_dma_orig_addr);
-
-//	addr = sun3_dvma_page((unsigned long)data, (unsigned long)dmabuf);
-	addr = (void *)dvma_map_vme((unsigned long) data, count);
-		
-	sun3_dma_orig_addr = addr;
-	sun3_dma_orig_count = count;
-	
-#ifdef SUN3_SCSI_DEBUG
-	printk("scsi: dma_setup addr %p count %x\n", addr, count);
-#endif
-
-//	dregs->fifo_count = 0;
-#if 0	
-	/* reset fifo */
-	dregs->csr &= ~CSR_FIFO;
-	dregs->csr |= CSR_FIFO;
-#endif	
-	/* set direction */
-	if(write_flag)
-		dregs->csr |= CSR_SEND;
-	else
-		dregs->csr &= ~CSR_SEND;
-	
-	/* reset fifo */
-//	dregs->csr &= ~CSR_FIFO;
-//	dregs->csr |= CSR_FIFO;
-
-	dregs->csr |= CSR_PACK_ENABLE;
-
-	dregs->dma_addr_hi = ((unsigned long)addr >> 16);
-	dregs->dma_addr_lo = ((unsigned long)addr & 0xffff);
-	
-	dregs->dma_count_hi = 0;
-	dregs->dma_count_lo = 0;
-	dregs->fifo_count_hi = 0;
-	dregs->fifo_count = 0;
-		
-#ifdef SUN3_SCSI_DEBUG
-	printk("scsi: dma_setup done csr %x\n", dregs->csr);
-#endif
-       	return count;
-
-}
-
-static inline unsigned long sun3scsi_dma_residual(struct Scsi_Host *instance)
-{
-	return last_residual;
-}
-
-static inline unsigned long sun3scsi_dma_xfer_len(unsigned long wanted,
-						  struct scsi_cmnd *cmd,
-						  int write_flag)
-{
-	if (cmd->request->cmd_type == REQ_TYPE_FS)
- 		return wanted;
-	else
-		return 0;
-}
-
-static int sun3scsi_dma_start(unsigned long count, char *data)
-{
-	
-	unsigned short csr;
-
-	csr = dregs->csr;
-#ifdef SUN3_SCSI_DEBUG
-	printk("scsi: dma_start data %p count %x csr %x fifo %x\n", data, count, csr, dregs->fifo_count);
-#endif
-	
-	dregs->dma_count_hi = (sun3_dma_orig_count >> 16);
-	dregs->dma_count_lo = (sun3_dma_orig_count & 0xffff);
-
-	dregs->fifo_count_hi = (sun3_dma_orig_count >> 16);
-	dregs->fifo_count = (sun3_dma_orig_count & 0xffff);
-
-//	if(!(csr & CSR_DMA_ENABLE))
-//		dregs->csr |= CSR_DMA_ENABLE;
-
-	return 0;
-}
-
-/* clean up after our dma is done */
-static int sun3scsi_dma_finish(int write_flag)
-{
-	unsigned short fifo;
-	int ret = 0;
-	
-	sun3_dma_active = 0;
-
-	dregs->csr &= ~CSR_DMA_ENABLE;
-	
-	fifo = dregs->fifo_count;
-	if(write_flag) {
-		if((fifo > 0) && (fifo < sun3_dma_orig_count))
-			fifo++;
-	}
-
-	last_residual = fifo;
-#ifdef SUN3_SCSI_DEBUG
-	printk("scsi: residual %x total %x\n", fifo, sun3_dma_orig_count);
-#endif
-	/* empty bytes from the fifo which didn't make it */
-	if((!write_flag) && (dregs->csr & CSR_LEFT)) {
-		unsigned char *vaddr;
-
-#ifdef SUN3_SCSI_DEBUG
-		printk("scsi: got left over bytes\n");
-#endif
-
-		vaddr = (unsigned char *)dvma_vmetov(sun3_dma_orig_addr);
-		
-		vaddr += (sun3_dma_orig_count - fifo);
-		vaddr--;
-		
-		switch(dregs->csr & CSR_LEFT) {
-		case CSR_LEFT_3:
-			*vaddr = (dregs->bpack_lo & 0xff00) >> 8;
-			vaddr--;
-			
-		case CSR_LEFT_2:
-			*vaddr = (dregs->bpack_hi & 0x00ff);
-			vaddr--;
-			
-		case CSR_LEFT_1:
-			*vaddr = (dregs->bpack_hi & 0xff00) >> 8;
-			break;
-		}
-		
-		
-	}
-
-	dvma_unmap(sun3_dma_orig_addr);
-	sun3_dma_orig_addr = NULL;
-
-	dregs->dma_addr_hi = 0;
-	dregs->dma_addr_lo = 0;
-	dregs->dma_count_hi = 0;
-	dregs->dma_count_lo = 0;
-
-	dregs->fifo_count = 0;
-	dregs->fifo_count_hi = 0;
-
-	dregs->csr &= ~CSR_SEND;
-	
-//	dregs->csr |= CSR_DMA_ENABLE;
-	
-#if 0
-	/* reset fifo */
-	dregs->csr &= ~CSR_FIFO;
-	dregs->csr |= CSR_FIFO;
-#endif	
-	sun3_dma_setup_done = NULL;
-
-	return ret;
-
-}
-
-#include "sun3_NCR5380.c"
-
-static struct scsi_host_template driver_template = {
-	.name			= SUN3_SCSI_NAME,
-	.detect			= sun3scsi_detect,
-	.release		= sun3scsi_release,
-	.info			= sun3scsi_info,
-	.queuecommand		= sun3scsi_queue_command,
-	.eh_abort_handler      	= sun3scsi_abort,
-	.eh_bus_reset_handler  	= sun3scsi_bus_reset,
-	.can_queue		= CAN_QUEUE,
-	.this_id		= 7,
-	.sg_tablesize		= SG_TABLESIZE,
-	.cmd_per_lun		= CMD_PER_LUN,
-	.use_clustering		= DISABLE_CLUSTERING
-};
-
-
-#include "scsi_module.c"
-
-MODULE_LICENSE("GPL");
+#include "sun3_scsi.c"
diff --git a/drivers/scsi/t128.c b/drivers/scsi/t128.c
index a4abce9d526..8cc80931df1 100644
--- a/drivers/scsi/t128.c
+++ b/drivers/scsi/t128.c
@@ -102,10 +102,6 @@
  * 15 9-11
  */
  
-/*
- * $Log: t128.c,v $
- */
-
 #include <linux/signal.h>
 #include <linux/io.h>
 #include <linux/blkdev.h>
diff --git a/drivers/scsi/t128.h b/drivers/scsi/t128.h
index 1df82c28e56..fd68cecc62a 100644
--- a/drivers/scsi/t128.h
+++ b/drivers/scsi/t128.h
@@ -34,10 +34,6 @@
  * 1+ (800) 334-5454
  */
 
-/*
- * $Log: t128.h,v $
- */
-
 #ifndef T128_H
 #define T128_H
 
@@ -107,8 +103,6 @@ static int t128_bus_reset(struct scsi_cmnd *);
 #define CAN_QUEUE 32
 #endif
 
-#ifndef HOSTS_C
-
 #define NCR5380_implementation_fields \
     void __iomem *base
 
@@ -148,6 +142,5 @@ static int t128_bus_reset(struct scsi_cmnd *);
 
 #define T128_IRQS 0xc4a8
 
-#endif /* else def HOSTS_C */
 #endif /* ndef ASM */
 #endif /* T128_H */
diff --git a/drivers/scsi/ufs/ufs.h b/drivers/scsi/ufs/ufs.h
index 72105009052..f42d1cee652 100644
--- a/drivers/scsi/ufs/ufs.h
+++ b/drivers/scsi/ufs/ufs.h
@@ -196,9 +196,9 @@ enum {
  * @dword_2: UPIU header DW-2
  */
 struct utp_upiu_header {
-	u32 dword_0;
-	u32 dword_1;
-	u32 dword_2;
+	__be32 dword_0;
+	__be32 dword_1;
+	__be32 dword_2;
 };
 
 /**
@@ -207,7 +207,7 @@ struct utp_upiu_header {
  * @cdb: Command Descriptor Block CDB DW-4 to DW-7
  */
 struct utp_upiu_cmd {
-	u32 exp_data_transfer_len;
+	__be32 exp_data_transfer_len;
 	u8 cdb[MAX_CDB_SIZE];
 };
 
@@ -228,10 +228,10 @@ struct utp_upiu_query {
 	u8 idn;
 	u8 index;
 	u8 selector;
-	u16 reserved_osf;
-	u16 length;
-	u32 value;
-	u32 reserved[2];
+	__be16 reserved_osf;
+	__be16 length;
+	__be32 value;
+	__be32 reserved[2];
 };
 
 /**
@@ -256,9 +256,9 @@ struct utp_upiu_req {
  * @sense_data: Sense data field DW-8 to DW-12
  */
 struct utp_cmd_rsp {
-	u32 residual_transfer_count;
-	u32 reserved[4];
-	u16 sense_data_len;
+	__be32 residual_transfer_count;
+	__be32 reserved[4];
+	__be16 sense_data_len;
 	u8 sense_data[18];
 };
 
@@ -286,10 +286,10 @@ struct utp_upiu_rsp {
  */
 struct utp_upiu_task_req {
 	struct utp_upiu_header header;
-	u32 input_param1;
-	u32 input_param2;
-	u32 input_param3;
-	u32 reserved[2];
+	__be32 input_param1;
+	__be32 input_param2;
+	__be32 input_param3;
+	__be32 reserved[2];
 };
 
 /**
@@ -301,9 +301,9 @@ struct utp_upiu_task_req {
  */
 struct utp_upiu_task_rsp {
 	struct utp_upiu_header header;
-	u32 output_param1;
-	u32 output_param2;
-	u32 reserved[3];
+	__be32 output_param1;
+	__be32 output_param2;
+	__be32 reserved[3];
 };
 
 /**
diff --git a/drivers/scsi/ufs/ufshcd.c b/drivers/scsi/ufs/ufshcd.c
index 04884d663e4..0c287725125 100644
--- a/drivers/scsi/ufs/ufshcd.c
+++ b/drivers/scsi/ufs/ufshcd.c
@@ -55,6 +55,9 @@
 /* Query request timeout */
 #define QUERY_REQ_TIMEOUT 30 /* msec */
 
+/* Task management command timeout */
+#define TM_CMD_TIMEOUT	100 /* msecs */
+
 /* Expose the flag value from utp_upiu_query.value */
 #define MASK_QUERY_UPIU_FLAG_LOC 0xFF
 
@@ -71,9 +74,22 @@ enum {
 
 /* UFSHCD states */
 enum {
-	UFSHCD_STATE_OPERATIONAL,
 	UFSHCD_STATE_RESET,
 	UFSHCD_STATE_ERROR,
+	UFSHCD_STATE_OPERATIONAL,
+};
+
+/* UFSHCD error handling flags */
+enum {
+	UFSHCD_EH_IN_PROGRESS = (1 << 0),
+};
+
+/* UFSHCD UIC layer error flags */
+enum {
+	UFSHCD_UIC_DL_PA_INIT_ERROR = (1 << 0), /* Data link layer error */
+	UFSHCD_UIC_NL_ERROR = (1 << 1), /* Network layer error */
+	UFSHCD_UIC_TL_ERROR = (1 << 2), /* Transport Layer error */
+	UFSHCD_UIC_DME_ERROR = (1 << 3), /* DME error */
 };
 
 /* Interrupt configuration options */
@@ -83,6 +99,18 @@ enum {
 	UFSHCD_INT_CLEAR,
 };
 
+#define ufshcd_set_eh_in_progress(h) \
+	(h->eh_flags |= UFSHCD_EH_IN_PROGRESS)
+#define ufshcd_eh_in_progress(h) \
+	(h->eh_flags & UFSHCD_EH_IN_PROGRESS)
+#define ufshcd_clear_eh_in_progress(h) \
+	(h->eh_flags &= ~UFSHCD_EH_IN_PROGRESS)
+
+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);
+
 /*
  * ufshcd_wait_for_register - wait for register value to change
  * @hba - per-adapter interface
@@ -163,7 +191,7 @@ static inline int ufshcd_is_device_present(u32 reg_hcs)
  */
 static inline int ufshcd_get_tr_ocs(struct ufshcd_lrb *lrbp)
 {
-	return lrbp->utr_descriptor_ptr->header.dword_2 & MASK_OCS;
+	return le32_to_cpu(lrbp->utr_descriptor_ptr->header.dword_2) & MASK_OCS;
 }
 
 /**
@@ -176,19 +204,41 @@ static inline int ufshcd_get_tr_ocs(struct ufshcd_lrb *lrbp)
 static inline int
 ufshcd_get_tmr_ocs(struct utp_task_req_desc *task_req_descp)
 {
-	return task_req_descp->header.dword_2 & MASK_OCS;
+	return le32_to_cpu(task_req_descp->header.dword_2) & MASK_OCS;
 }
 
 /**
  * ufshcd_get_tm_free_slot - get a free slot for task management request
  * @hba: per adapter instance
+ * @free_slot: pointer to variable with available slot value
  *
- * Returns maximum number of task management request slots in case of
- * task management queue full or returns the free slot number
+ * Get a free tag and lock it until ufshcd_put_tm_slot() is called.
+ * Returns 0 if free slot is not available, else return 1 with tag value
+ * in @free_slot.
  */
-static inline int ufshcd_get_tm_free_slot(struct ufs_hba *hba)
+static bool ufshcd_get_tm_free_slot(struct ufs_hba *hba, int *free_slot)
 {
-	return find_first_zero_bit(&hba->outstanding_tasks, hba->nutmrs);
+	int tag;
+	bool ret = false;
+
+	if (!free_slot)
+		goto out;
+
+	do {
+		tag = find_first_zero_bit(&hba->tm_slots_in_use, hba->nutmrs);
+		if (tag >= hba->nutmrs)
+			goto out;
+	} while (test_and_set_bit_lock(tag, &hba->tm_slots_in_use));
+
+	*free_slot = tag;
+	ret = true;
+out:
+	return ret;
+}
+
+static inline void ufshcd_put_tm_slot(struct ufs_hba *hba, int slot)
+{
+	clear_bit_unlock(slot, &hba->tm_slots_in_use);
 }
 
 /**
@@ -390,26 +440,6 @@ static inline void ufshcd_copy_sense_data(struct ufshcd_lrb *lrbp)
 }
 
 /**
- * ufshcd_query_to_cpu() - formats the buffer to native cpu endian
- * @response: upiu query response to convert
- */
-static inline void ufshcd_query_to_cpu(struct utp_upiu_query *response)
-{
-	response->length = be16_to_cpu(response->length);
-	response->value = be32_to_cpu(response->value);
-}
-
-/**
- * ufshcd_query_to_be() - formats the buffer to big endian
- * @request: upiu query request to convert
- */
-static inline void ufshcd_query_to_be(struct utp_upiu_query *request)
-{
-	request->length = cpu_to_be16(request->length);
-	request->value = cpu_to_be32(request->value);
-}
-
-/**
  * ufshcd_copy_query_response() - Copy the Query Response and the data
  * descriptor
  * @hba: per adapter instance
@@ -425,7 +455,6 @@ void ufshcd_copy_query_response(struct ufs_hba *hba, struct ufshcd_lrb *lrbp)
 			UPIU_RSP_CODE_OFFSET;
 
 	memcpy(&query_res->upiu_res, &lrbp->ucd_rsp_ptr->qr, QUERY_OSF_SIZE);
-	ufshcd_query_to_cpu(&query_res->upiu_res);
 
 
 	/* Get the descriptor */
@@ -749,7 +778,7 @@ static void ufshcd_prepare_utp_query_req_upiu(struct ufs_hba *hba,
 {
 	struct utp_upiu_req *ucd_req_ptr = lrbp->ucd_req_ptr;
 	struct ufs_query *query = &hba->dev_cmd.query;
-	u16 len = query->request.upiu_req.length;
+	u16 len = be16_to_cpu(query->request.upiu_req.length);
 	u8 *descp = (u8 *)lrbp->ucd_req_ptr + GENERAL_UPIU_REQUEST_SIZE;
 
 	/* Query request header */
@@ -766,7 +795,6 @@ static void ufshcd_prepare_utp_query_req_upiu(struct ufs_hba *hba,
 	/* Copy the Query Request buffer as is */
 	memcpy(&ucd_req_ptr->qr, &query->request.upiu_req,
 			QUERY_OSF_SIZE);
-	ufshcd_query_to_be(&ucd_req_ptr->qr);
 
 	/* Copy the Descriptor */
 	if ((len > 0) && (query->request.upiu_req.opcode ==
@@ -853,10 +881,25 @@ static int ufshcd_queuecommand(struct Scsi_Host *host, struct scsi_cmnd *cmd)
 
 	tag = cmd->request->tag;
 
-	if (hba->ufshcd_state != UFSHCD_STATE_OPERATIONAL) {
+	spin_lock_irqsave(hba->host->host_lock, flags);
+	switch (hba->ufshcd_state) {
+	case UFSHCD_STATE_OPERATIONAL:
+		break;
+	case UFSHCD_STATE_RESET:
 		err = SCSI_MLQUEUE_HOST_BUSY;
-		goto out;
+		goto out_unlock;
+	case UFSHCD_STATE_ERROR:
+		set_host_byte(cmd, DID_ERROR);
+		cmd->scsi_done(cmd);
+		goto out_unlock;
+	default:
+		dev_WARN_ONCE(hba->dev, 1, "%s: invalid state %d\n",
+				__func__, hba->ufshcd_state);
+		set_host_byte(cmd, DID_BAD_TARGET);
+		cmd->scsi_done(cmd);
+		goto out_unlock;
 	}
+	spin_unlock_irqrestore(hba->host->host_lock, flags);
 
 	/* acquire the tag to make sure device cmds don't use it */
 	if (test_and_set_bit_lock(tag, &hba->lrb_in_use)) {
@@ -893,6 +936,7 @@ static int ufshcd_queuecommand(struct Scsi_Host *host, struct scsi_cmnd *cmd)
 	/* issue command to the controller */
 	spin_lock_irqsave(hba->host->host_lock, flags);
 	ufshcd_send_command(hba, tag);
+out_unlock:
 	spin_unlock_irqrestore(hba->host->host_lock, flags);
 out:
 	return err;
@@ -1151,7 +1195,7 @@ static int ufshcd_query_flag(struct ufs_hba *hba, enum query_opcode opcode,
 	}
 
 	if (flag_res)
-		*flag_res = (response->upiu_res.value &
+		*flag_res = (be32_to_cpu(response->upiu_res.value) &
 				MASK_QUERY_UPIU_FLAG_LOC) & 0x1;
 
 out_unlock:
@@ -1170,7 +1214,7 @@ out_unlock:
  *
  * Returns 0 for success, non-zero in case of failure
 */
-int ufshcd_query_attr(struct ufs_hba *hba, enum query_opcode opcode,
+static int ufshcd_query_attr(struct ufs_hba *hba, enum query_opcode opcode,
 			enum attr_idn idn, u8 index, u8 selector, u32 *attr_val)
 {
 	struct ufs_query_req *request;
@@ -1195,7 +1239,7 @@ int ufshcd_query_attr(struct ufs_hba *hba, enum query_opcode opcode,
 	switch (opcode) {
 	case UPIU_QUERY_OPCODE_WRITE_ATTR:
 		request->query_func = UPIU_QUERY_FUNC_STANDARD_WRITE_REQUEST;
-		request->upiu_req.value = *attr_val;
+		request->upiu_req.value = cpu_to_be32(*attr_val);
 		break;
 	case UPIU_QUERY_OPCODE_READ_ATTR:
 		request->query_func = UPIU_QUERY_FUNC_STANDARD_READ_REQUEST;
@@ -1222,7 +1266,7 @@ int ufshcd_query_attr(struct ufs_hba *hba, enum query_opcode opcode,
 		goto out_unlock;
 	}
 
-	*attr_val = response->upiu_res.value;
+	*attr_val = be32_to_cpu(response->upiu_res.value);
 
 out_unlock:
 	mutex_unlock(&hba->dev_cmd.lock);
@@ -1481,7 +1525,7 @@ EXPORT_SYMBOL_GPL(ufshcd_dme_get_attr);
  *
  * Returns 0 on success, non-zero value on failure
  */
-int ufshcd_uic_change_pwr_mode(struct ufs_hba *hba, u8 mode)
+static int ufshcd_uic_change_pwr_mode(struct ufs_hba *hba, u8 mode)
 {
 	struct uic_command uic_cmd = {0};
 	struct completion pwr_done;
@@ -1701,11 +1745,6 @@ static int ufshcd_make_hba_operational(struct ufs_hba *hba)
 		goto out;
 	}
 
-	if (hba->ufshcd_state == UFSHCD_STATE_RESET)
-		scsi_unblock_requests(hba->host);
-
-	hba->ufshcd_state = UFSHCD_STATE_OPERATIONAL;
-
 out:
 	return err;
 }
@@ -1831,66 +1870,6 @@ static int ufshcd_verify_dev_init(struct ufs_hba *hba)
 }
 
 /**
- * ufshcd_do_reset - reset the host controller
- * @hba: per adapter instance
- *
- * Returns SUCCESS/FAILED
- */
-static int ufshcd_do_reset(struct ufs_hba *hba)
-{
-	struct ufshcd_lrb *lrbp;
-	unsigned long flags;
-	int tag;
-
-	/* block commands from midlayer */
-	scsi_block_requests(hba->host);
-
-	spin_lock_irqsave(hba->host->host_lock, flags);
-	hba->ufshcd_state = UFSHCD_STATE_RESET;
-
-	/* send controller to reset state */
-	ufshcd_hba_stop(hba);
-	spin_unlock_irqrestore(hba->host->host_lock, flags);
-
-	/* abort outstanding commands */
-	for (tag = 0; tag < hba->nutrs; tag++) {
-		if (test_bit(tag, &hba->outstanding_reqs)) {
-			lrbp = &hba->lrb[tag];
-			if (lrbp->cmd) {
-				scsi_dma_unmap(lrbp->cmd);
-				lrbp->cmd->result = DID_RESET << 16;
-				lrbp->cmd->scsi_done(lrbp->cmd);
-				lrbp->cmd = NULL;
-				clear_bit_unlock(tag, &hba->lrb_in_use);
-			}
-		}
-	}
-
-	/* complete device management command */
-	if (hba->dev_cmd.complete)
-		complete(hba->dev_cmd.complete);
-
-	/* clear outstanding request/task bit maps */
-	hba->outstanding_reqs = 0;
-	hba->outstanding_tasks = 0;
-
-	/* Host controller enable */
-	if (ufshcd_hba_enable(hba)) {
-		dev_err(hba->dev,
-			"Reset: Controller initialization failed\n");
-		return FAILED;
-	}
-
-	if (ufshcd_link_startup(hba)) {
-		dev_err(hba->dev,
-			"Reset: Link start-up failed\n");
-		return FAILED;
-	}
-
-	return SUCCESS;
-}
-
-/**
  * ufshcd_slave_alloc - handle initial SCSI device configurations
  * @sdev: pointer to SCSI device
  *
@@ -1907,6 +1886,9 @@ static int ufshcd_slave_alloc(struct scsi_device *sdev)
 	sdev->use_10_for_ms = 1;
 	scsi_set_tag_type(sdev, MSG_SIMPLE_TAG);
 
+	/* allow SCSI layer to restart the device in case of errors */
+	sdev->allow_restart = 1;
+
 	/*
 	 * Inform SCSI Midlayer that the LUN queue depth is same as the
 	 * controller queue depth. If a LUN queue depth is less than the
@@ -1934,10 +1916,11 @@ static void ufshcd_slave_destroy(struct scsi_device *sdev)
  * ufshcd_task_req_compl - handle task management request completion
  * @hba: per adapter instance
  * @index: index of the completed request
+ * @resp: task management service response
  *
- * Returns SUCCESS/FAILED
+ * Returns non-zero value on error, zero on success
  */
-static int ufshcd_task_req_compl(struct ufs_hba *hba, u32 index)
+static int ufshcd_task_req_compl(struct ufs_hba *hba, u32 index, u8 *resp)
 {
 	struct utp_task_req_desc *task_req_descp;
 	struct utp_upiu_task_rsp *task_rsp_upiup;
@@ -1958,19 +1941,15 @@ static int ufshcd_task_req_compl(struct ufs_hba *hba, u32 index)
 				task_req_descp[index].task_rsp_upiu;
 		task_result = be32_to_cpu(task_rsp_upiup->header.dword_1);
 		task_result = ((task_result & MASK_TASK_RESPONSE) >> 8);
-
-		if (task_result != UPIU_TASK_MANAGEMENT_FUNC_COMPL &&
-		    task_result != UPIU_TASK_MANAGEMENT_FUNC_SUCCEEDED)
-			task_result = FAILED;
-		else
-			task_result = SUCCESS;
+		if (resp)
+			*resp = (u8)task_result;
 	} else {
-		task_result = FAILED;
-		dev_err(hba->dev,
-			"trc: Invalid ocs = %x\n", ocs_value);
+		dev_err(hba->dev, "%s: failed, ocs = 0x%x\n",
+				__func__, ocs_value);
 	}
 	spin_unlock_irqrestore(hba->host->host_lock, flags);
-	return task_result;
+
+	return ocs_value;
 }
 
 /**
@@ -2105,6 +2084,9 @@ ufshcd_transfer_rsp_status(struct ufs_hba *hba, struct ufshcd_lrb *lrbp)
 	case OCS_ABORTED:
 		result |= DID_ABORT << 16;
 		break;
+	case OCS_INVALID_COMMAND_STATUS:
+		result |= DID_REQUEUE << 16;
+		break;
 	case OCS_INVALID_CMD_TABLE_ATTR:
 	case OCS_INVALID_PRDT_ATTR:
 	case OCS_MISMATCH_DATA_BUF_SIZE:
@@ -2422,41 +2404,145 @@ out:
 }
 
 /**
- * ufshcd_fatal_err_handler - handle fatal errors
- * @hba: per adapter instance
+ * ufshcd_err_handler - handle UFS errors that require s/w attention
+ * @work: pointer to work structure
  */
-static void ufshcd_fatal_err_handler(struct work_struct *work)
+static void ufshcd_err_handler(struct work_struct *work)
 {
 	struct ufs_hba *hba;
-	hba = container_of(work, struct ufs_hba, feh_workq);
+	unsigned long flags;
+	u32 err_xfer = 0;
+	u32 err_tm = 0;
+	int err = 0;
+	int tag;
+
+	hba = container_of(work, struct ufs_hba, eh_work);
 
 	pm_runtime_get_sync(hba->dev);
-	/* check if reset is already in progress */
-	if (hba->ufshcd_state != UFSHCD_STATE_RESET)
-		ufshcd_do_reset(hba);
+
+	spin_lock_irqsave(hba->host->host_lock, flags);
+	if (hba->ufshcd_state == UFSHCD_STATE_RESET) {
+		spin_unlock_irqrestore(hba->host->host_lock, flags);
+		goto out;
+	}
+
+	hba->ufshcd_state = UFSHCD_STATE_RESET;
+	ufshcd_set_eh_in_progress(hba);
+
+	/* Complete requests that have door-bell cleared by h/w */
+	ufshcd_transfer_req_compl(hba);
+	ufshcd_tmc_handler(hba);
+	spin_unlock_irqrestore(hba->host->host_lock, flags);
+
+	/* Clear pending transfer requests */
+	for_each_set_bit(tag, &hba->outstanding_reqs, hba->nutrs)
+		if (ufshcd_clear_cmd(hba, tag))
+			err_xfer |= 1 << tag;
+
+	/* Clear pending task management requests */
+	for_each_set_bit(tag, &hba->outstanding_tasks, hba->nutmrs)
+		if (ufshcd_clear_tm_cmd(hba, tag))
+			err_tm |= 1 << tag;
+
+	/* Complete the requests that are cleared by s/w */
+	spin_lock_irqsave(hba->host->host_lock, flags);
+	ufshcd_transfer_req_compl(hba);
+	ufshcd_tmc_handler(hba);
+	spin_unlock_irqrestore(hba->host->host_lock, flags);
+
+	/* Fatal errors need reset */
+	if (err_xfer || err_tm || (hba->saved_err & INT_FATAL_ERRORS) ||
+			((hba->saved_err & UIC_ERROR) &&
+			 (hba->saved_uic_err & UFSHCD_UIC_DL_PA_INIT_ERROR))) {
+		err = ufshcd_reset_and_restore(hba);
+		if (err) {
+			dev_err(hba->dev, "%s: reset and restore failed\n",
+					__func__);
+			hba->ufshcd_state = UFSHCD_STATE_ERROR;
+		}
+		/*
+		 * Inform scsi mid-layer that we did reset and allow to handle
+		 * Unit Attention properly.
+		 */
+		scsi_report_bus_reset(hba->host, 0);
+		hba->saved_err = 0;
+		hba->saved_uic_err = 0;
+	}
+	ufshcd_clear_eh_in_progress(hba);
+
+out:
+	scsi_unblock_requests(hba->host);
 	pm_runtime_put_sync(hba->dev);
 }
 
 /**
- * ufshcd_err_handler - Check for fatal errors
- * @work: pointer to a work queue structure
+ * ufshcd_update_uic_error - check and set fatal UIC error flags.
+ * @hba: per-adapter instance
  */
-static void ufshcd_err_handler(struct ufs_hba *hba)
+static void ufshcd_update_uic_error(struct ufs_hba *hba)
 {
 	u32 reg;
 
+	/* PA_INIT_ERROR is fatal and needs UIC reset */
+	reg = ufshcd_readl(hba, REG_UIC_ERROR_CODE_DATA_LINK_LAYER);
+	if (reg & UIC_DATA_LINK_LAYER_ERROR_PA_INIT)
+		hba->uic_error |= UFSHCD_UIC_DL_PA_INIT_ERROR;
+
+	/* UIC NL/TL/DME errors needs software retry */
+	reg = ufshcd_readl(hba, REG_UIC_ERROR_CODE_NETWORK_LAYER);
+	if (reg)
+		hba->uic_error |= UFSHCD_UIC_NL_ERROR;
+
+	reg = ufshcd_readl(hba, REG_UIC_ERROR_CODE_TRANSPORT_LAYER);
+	if (reg)
+		hba->uic_error |= UFSHCD_UIC_TL_ERROR;
+
+	reg = ufshcd_readl(hba, REG_UIC_ERROR_CODE_DME);
+	if (reg)
+		hba->uic_error |= UFSHCD_UIC_DME_ERROR;
+
+	dev_dbg(hba->dev, "%s: UIC error flags = 0x%08x\n",
+			__func__, hba->uic_error);
+}
+
+/**
+ * ufshcd_check_errors - Check for errors that need s/w attention
+ * @hba: per-adapter instance
+ */
+static void ufshcd_check_errors(struct ufs_hba *hba)
+{
+	bool queue_eh_work = false;
+
 	if (hba->errors & INT_FATAL_ERRORS)
-		goto fatal_eh;
+		queue_eh_work = true;
 
 	if (hba->errors & UIC_ERROR) {
-		reg = ufshcd_readl(hba, REG_UIC_ERROR_CODE_DATA_LINK_LAYER);
-		if (reg & UIC_DATA_LINK_LAYER_ERROR_PA_INIT)
-			goto fatal_eh;
+		hba->uic_error = 0;
+		ufshcd_update_uic_error(hba);
+		if (hba->uic_error)
+			queue_eh_work = true;
 	}
-	return;
-fatal_eh:
-	hba->ufshcd_state = UFSHCD_STATE_ERROR;
-	schedule_work(&hba->feh_workq);
+
+	if (queue_eh_work) {
+		/* handle fatal errors only when link is functional */
+		if (hba->ufshcd_state == UFSHCD_STATE_OPERATIONAL) {
+			/* block commands from scsi mid-layer */
+			scsi_block_requests(hba->host);
+
+			/* transfer error masks to sticky bits */
+			hba->saved_err |= hba->errors;
+			hba->saved_uic_err |= hba->uic_error;
+
+			hba->ufshcd_state = UFSHCD_STATE_ERROR;
+			schedule_work(&hba->eh_work);
+		}
+	}
+	/*
+	 * if (!queue_eh_work) -
+	 * Other errors are either non-fatal where host recovers
+	 * itself without s/w intervention or errors that will be
+	 * handled by the SCSI core layer.
+	 */
 }
 
 /**
@@ -2469,7 +2555,7 @@ static void ufshcd_tmc_handler(struct ufs_hba *hba)
 
 	tm_doorbell = ufshcd_readl(hba, REG_UTP_TASK_REQ_DOOR_BELL);
 	hba->tm_condition = tm_doorbell ^ hba->outstanding_tasks;
-	wake_up_interruptible(&hba->ufshcd_tm_wait_queue);
+	wake_up(&hba->tm_wq);
 }
 
 /**
@@ -2481,7 +2567,7 @@ static void ufshcd_sl_intr(struct ufs_hba *hba, u32 intr_status)
 {
 	hba->errors = UFSHCD_ERROR_MASK & intr_status;
 	if (hba->errors)
-		ufshcd_err_handler(hba);
+		ufshcd_check_errors(hba);
 
 	if (intr_status & UFSHCD_UIC_MASK)
 		ufshcd_uic_cmd_compl(hba, intr_status);
@@ -2519,38 +2605,58 @@ static irqreturn_t ufshcd_intr(int irq, void *__hba)
 	return retval;
 }
 
+static int ufshcd_clear_tm_cmd(struct ufs_hba *hba, int tag)
+{
+	int err = 0;
+	u32 mask = 1 << tag;
+	unsigned long flags;
+
+	if (!test_bit(tag, &hba->outstanding_tasks))
+		goto out;
+
+	spin_lock_irqsave(hba->host->host_lock, flags);
+	ufshcd_writel(hba, ~(1 << tag), REG_UTP_TASK_REQ_LIST_CLEAR);
+	spin_unlock_irqrestore(hba->host->host_lock, flags);
+
+	/* poll for max. 1 sec to clear door bell register by h/w */
+	err = ufshcd_wait_for_register(hba,
+			REG_UTP_TASK_REQ_DOOR_BELL,
+			mask, 0, 1000, 1000);
+out:
+	return err;
+}
+
 /**
  * ufshcd_issue_tm_cmd - issues task management commands to controller
  * @hba: per adapter instance
- * @lrbp: pointer to local reference block
+ * @lun_id: LUN ID to which TM command is sent
+ * @task_id: task ID to which the TM command is applicable
+ * @tm_function: task management function opcode
+ * @tm_response: task management service response return value
  *
- * Returns SUCCESS/FAILED
+ * Returns non-zero value on error, zero on success.
  */
-static int
-ufshcd_issue_tm_cmd(struct ufs_hba *hba,
-		    struct ufshcd_lrb *lrbp,
-		    u8 tm_function)
+static int ufshcd_issue_tm_cmd(struct ufs_hba *hba, int lun_id, int task_id,
+		u8 tm_function, u8 *tm_response)
 {
 	struct utp_task_req_desc *task_req_descp;
 	struct utp_upiu_task_req *task_req_upiup;
 	struct Scsi_Host *host;
 	unsigned long flags;
-	int free_slot = 0;
+	int free_slot;
 	int err;
+	int task_tag;
 
 	host = hba->host;
 
-	spin_lock_irqsave(host->host_lock, flags);
-
-	/* If task management queue is full */
-	free_slot = ufshcd_get_tm_free_slot(hba);
-	if (free_slot >= hba->nutmrs) {
-		spin_unlock_irqrestore(host->host_lock, flags);
-		dev_err(hba->dev, "Task management queue full\n");
-		err = FAILED;
-		goto out;
-	}
+	/*
+	 * Get free slot, sleep if slots are unavailable.
+	 * Even though we use wait_event() which sleeps indefinitely,
+	 * the maximum wait time is bounded by %TM_CMD_TIMEOUT.
+	 */
+	wait_event(hba->tm_tag_wq, ufshcd_get_tm_free_slot(hba, &free_slot));
 
+	spin_lock_irqsave(host->host_lock, flags);
 	task_req_descp = hba->utmrdl_base_addr;
 	task_req_descp += free_slot;
 
@@ -2562,18 +2668,15 @@ ufshcd_issue_tm_cmd(struct ufs_hba *hba,
 	/* Configure task request UPIU */
 	task_req_upiup =
 		(struct utp_upiu_task_req *) task_req_descp->task_req_upiu;
+	task_tag = hba->nutrs + free_slot;
 	task_req_upiup->header.dword_0 =
 		UPIU_HEADER_DWORD(UPIU_TRANSACTION_TASK_REQ, 0,
-					      lrbp->lun, lrbp->task_tag);
+					      lun_id, task_tag);
 	task_req_upiup->header.dword_1 =
 		UPIU_HEADER_DWORD(0, tm_function, 0, 0);
 
-	task_req_upiup->input_param1 = lrbp->lun;
-	task_req_upiup->input_param1 =
-		cpu_to_be32(task_req_upiup->input_param1);
-	task_req_upiup->input_param2 = lrbp->task_tag;
-	task_req_upiup->input_param2 =
-		cpu_to_be32(task_req_upiup->input_param2);
+	task_req_upiup->input_param1 = cpu_to_be32(lun_id);
+	task_req_upiup->input_param2 = cpu_to_be32(task_id);
 
 	/* send command to the controller */
 	__set_bit(free_slot, &hba->outstanding_tasks);
@@ -2582,91 +2685,88 @@ ufshcd_issue_tm_cmd(struct ufs_hba *hba,
 	spin_unlock_irqrestore(host->host_lock, flags);
 
 	/* wait until the task management command is completed */
-	err =
-	wait_event_interruptible_timeout(hba->ufshcd_tm_wait_queue,
-					 (test_bit(free_slot,
-					 &hba->tm_condition) != 0),
-					 60 * HZ);
+	err = wait_event_timeout(hba->tm_wq,
+			test_bit(free_slot, &hba->tm_condition),
+			msecs_to_jiffies(TM_CMD_TIMEOUT));
 	if (!err) {
-		dev_err(hba->dev,
-			"Task management command timed-out\n");
-		err = FAILED;
-		goto out;
+		dev_err(hba->dev, "%s: task management cmd 0x%.2x timed-out\n",
+				__func__, tm_function);
+		if (ufshcd_clear_tm_cmd(hba, free_slot))
+			dev_WARN(hba->dev, "%s: unable clear tm cmd (slot %d) after timeout\n",
+					__func__, free_slot);
+		err = -ETIMEDOUT;
+	} else {
+		err = ufshcd_task_req_compl(hba, free_slot, tm_response);
 	}
+
 	clear_bit(free_slot, &hba->tm_condition);
-	err = ufshcd_task_req_compl(hba, free_slot);
-out:
+	ufshcd_put_tm_slot(hba, free_slot);
+	wake_up(&hba->tm_tag_wq);
+
 	return err;
 }
 
 /**
- * ufshcd_device_reset - reset device and abort all the pending commands
+ * ufshcd_eh_device_reset_handler - device reset handler registered to
+ *                                    scsi layer.
  * @cmd: SCSI command pointer
  *
  * Returns SUCCESS/FAILED
  */
-static int ufshcd_device_reset(struct scsi_cmnd *cmd)
+static int ufshcd_eh_device_reset_handler(struct scsi_cmnd *cmd)
 {
 	struct Scsi_Host *host;
 	struct ufs_hba *hba;
 	unsigned int tag;
 	u32 pos;
 	int err;
+	u8 resp = 0xF;
+	struct ufshcd_lrb *lrbp;
+	unsigned long flags;
 
 	host = cmd->device->host;
 	hba = shost_priv(host);
 	tag = cmd->request->tag;
 
-	err = ufshcd_issue_tm_cmd(hba, &hba->lrb[tag], UFS_LOGICAL_RESET);
-	if (err == FAILED)
+	lrbp = &hba->lrb[tag];
+	err = ufshcd_issue_tm_cmd(hba, lrbp->lun, 0, UFS_LOGICAL_RESET, &resp);
+	if (err || resp != UPIU_TASK_MANAGEMENT_FUNC_COMPL) {
+		if (!err)
+			err = resp;
 		goto out;
+	}
 
-	for (pos = 0; pos < hba->nutrs; pos++) {
-		if (test_bit(pos, &hba->outstanding_reqs) &&
-		    (hba->lrb[tag].lun == hba->lrb[pos].lun)) {
-
-			/* clear the respective UTRLCLR register bit */
-			ufshcd_utrl_clear(hba, pos);
-
-			clear_bit(pos, &hba->outstanding_reqs);
-
-			if (hba->lrb[pos].cmd) {
-				scsi_dma_unmap(hba->lrb[pos].cmd);
-				hba->lrb[pos].cmd->result =
-					DID_ABORT << 16;
-				hba->lrb[pos].cmd->scsi_done(cmd);
-				hba->lrb[pos].cmd = NULL;
-				clear_bit_unlock(pos, &hba->lrb_in_use);
-				wake_up(&hba->dev_cmd.tag_wq);
-			}
+	/* clear the commands that were pending for corresponding LUN */
+	for_each_set_bit(pos, &hba->outstanding_reqs, hba->nutrs) {
+		if (hba->lrb[pos].lun == lrbp->lun) {
+			err = ufshcd_clear_cmd(hba, pos);
+			if (err)
+				break;
 		}
-	} /* end of for */
+	}
+	spin_lock_irqsave(host->host_lock, flags);
+	ufshcd_transfer_req_compl(hba);
+	spin_unlock_irqrestore(host->host_lock, flags);
 out:
+	if (!err) {
+		err = SUCCESS;
+	} else {
+		dev_err(hba->dev, "%s: failed with err %d\n", __func__, err);
+		err = FAILED;
+	}
 	return err;
 }
 
 /**
- * ufshcd_host_reset - Main reset function registered with scsi layer
- * @cmd: SCSI command pointer
- *
- * Returns SUCCESS/FAILED
- */
-static int ufshcd_host_reset(struct scsi_cmnd *cmd)
-{
-	struct ufs_hba *hba;
-
-	hba = shost_priv(cmd->device->host);
-
-	if (hba->ufshcd_state == UFSHCD_STATE_RESET)
-		return SUCCESS;
-
-	return ufshcd_do_reset(hba);
-}
-
-/**
  * ufshcd_abort - abort a specific command
  * @cmd: SCSI command pointer
  *
+ * Abort the pending command in device by sending UFS_ABORT_TASK task management
+ * command, and in host controller by clearing the door-bell register. There can
+ * be race between controller sending the command to the device while abort is
+ * issued. To avoid that, first issue UFS_QUERY_TASK to check if the command is
+ * really issued and then try to abort it.
+ *
  * Returns SUCCESS/FAILED
  */
 static int ufshcd_abort(struct scsi_cmnd *cmd)
@@ -2675,33 +2775,68 @@ static int ufshcd_abort(struct scsi_cmnd *cmd)
 	struct ufs_hba *hba;
 	unsigned long flags;
 	unsigned int tag;
-	int err;
+	int err = 0;
+	int poll_cnt;
+	u8 resp = 0xF;
+	struct ufshcd_lrb *lrbp;
 
 	host = cmd->device->host;
 	hba = shost_priv(host);
 	tag = cmd->request->tag;
 
-	spin_lock_irqsave(host->host_lock, flags);
+	/* If command is already aborted/completed, return SUCCESS */
+	if (!(test_bit(tag, &hba->outstanding_reqs)))
+		goto out;
 
-	/* check if command is still pending */
-	if (!(test_bit(tag, &hba->outstanding_reqs))) {
-		err = FAILED;
-		spin_unlock_irqrestore(host->host_lock, flags);
+	lrbp = &hba->lrb[tag];
+	for (poll_cnt = 100; poll_cnt; poll_cnt--) {
+		err = ufshcd_issue_tm_cmd(hba, lrbp->lun, lrbp->task_tag,
+				UFS_QUERY_TASK, &resp);
+		if (!err && resp == UPIU_TASK_MANAGEMENT_FUNC_SUCCEEDED) {
+			/* cmd pending in the device */
+			break;
+		} else if (!err && resp == UPIU_TASK_MANAGEMENT_FUNC_COMPL) {
+			u32 reg;
+
+			/*
+			 * cmd not pending in the device, check if it is
+			 * in transition.
+			 */
+			reg = ufshcd_readl(hba, REG_UTP_TRANSFER_REQ_DOOR_BELL);
+			if (reg & (1 << tag)) {
+				/* sleep for max. 200us to stabilize */
+				usleep_range(100, 200);
+				continue;
+			}
+			/* command completed already */
+			goto out;
+		} else {
+			if (!err)
+				err = resp; /* service response error */
+			goto out;
+		}
+	}
+
+	if (!poll_cnt) {
+		err = -EBUSY;
 		goto out;
 	}
-	spin_unlock_irqrestore(host->host_lock, flags);
 
-	err = ufshcd_issue_tm_cmd(hba, &hba->lrb[tag], UFS_ABORT_TASK);
-	if (err == FAILED)
+	err = ufshcd_issue_tm_cmd(hba, lrbp->lun, lrbp->task_tag,
+			UFS_ABORT_TASK, &resp);
+	if (err || resp != UPIU_TASK_MANAGEMENT_FUNC_COMPL) {
+		if (!err)
+			err = resp; /* service response error */
+		goto out;
+	}
+
+	err = ufshcd_clear_cmd(hba, tag);
+	if (err)
 		goto out;
 
 	scsi_dma_unmap(cmd);
 
 	spin_lock_irqsave(host->host_lock, flags);
-
-	/* clear the respective UTRLCLR register bit */
-	ufshcd_utrl_clear(hba, tag);
-
 	__clear_bit(tag, &hba->outstanding_reqs);
 	hba->lrb[tag].cmd = NULL;
 	spin_unlock_irqrestore(host->host_lock, flags);
@@ -2709,6 +2844,129 @@ 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;
+	} else {
+		dev_err(hba->dev, "%s: failed with err %d\n", __func__, err);
+		err = FAILED;
+	}
+
+	return err;
+}
+
+/**
+ * ufshcd_host_reset_and_restore - reset and restore host controller
+ * @hba: per-adapter instance
+ *
+ * Note that host controller reset may issue DME_RESET to
+ * local and remote (device) Uni-Pro stack and the attributes
+ * are reset to default state.
+ *
+ * Returns zero on success, non-zero on failure
+ */
+static int ufshcd_host_reset_and_restore(struct ufs_hba *hba)
+{
+	int err;
+	async_cookie_t cookie;
+	unsigned long flags;
+
+	/* Reset the host controller */
+	spin_lock_irqsave(hba->host->host_lock, flags);
+	ufshcd_hba_stop(hba);
+	spin_unlock_irqrestore(hba->host->host_lock, flags);
+
+	err = ufshcd_hba_enable(hba);
+	if (err)
+		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 = -EIO;
+out:
+	if (err)
+		dev_err(hba->dev, "%s: Host init failed %d\n", __func__, err);
+
+	return err;
+}
+
+/**
+ * ufshcd_reset_and_restore - reset and re-initialize host/device
+ * @hba: per-adapter instance
+ *
+ * Reset and recover device, host and re-establish link. This
+ * is helpful to recover the communication in fatal error conditions.
+ *
+ * Returns zero on success, non-zero on failure
+ */
+static int ufshcd_reset_and_restore(struct ufs_hba *hba)
+{
+	int err = 0;
+	unsigned long flags;
+
+	err = ufshcd_host_reset_and_restore(hba);
+
+	/*
+	 * After reset the door-bell might be cleared, complete
+	 * outstanding requests in s/w here.
+	 */
+	spin_lock_irqsave(hba->host->host_lock, flags);
+	ufshcd_transfer_req_compl(hba);
+	ufshcd_tmc_handler(hba);
+	spin_unlock_irqrestore(hba->host->host_lock, flags);
+
+	return err;
+}
+
+/**
+ * ufshcd_eh_host_reset_handler - host reset handler registered to scsi layer
+ * @cmd - SCSI command pointer
+ *
+ * Returns SUCCESS/FAILED
+ */
+static int ufshcd_eh_host_reset_handler(struct scsi_cmnd *cmd)
+{
+	int err;
+	unsigned long flags;
+	struct ufs_hba *hba;
+
+	hba = shost_priv(cmd->device->host);
+
+	/*
+	 * Check if there is any race with fatal error handling.
+	 * If so, wait for it to complete. Even though fatal error
+	 * handling does reset and restore in some cases, don't assume
+	 * anything out of it. We are just avoiding race here.
+	 */
+	do {
+		spin_lock_irqsave(hba->host->host_lock, flags);
+		if (!(work_pending(&hba->eh_work) ||
+				hba->ufshcd_state == UFSHCD_STATE_RESET))
+			break;
+		spin_unlock_irqrestore(hba->host->host_lock, flags);
+		dev_dbg(hba->dev, "%s: reset in progress\n", __func__);
+		flush_work(&hba->eh_work);
+	} while (1);
+
+	hba->ufshcd_state = UFSHCD_STATE_RESET;
+	ufshcd_set_eh_in_progress(hba);
+	spin_unlock_irqrestore(hba->host->host_lock, flags);
+
+	err = ufshcd_reset_and_restore(hba);
+
+	spin_lock_irqsave(hba->host->host_lock, flags);
+	if (!err) {
+		err = SUCCESS;
+		hba->ufshcd_state = UFSHCD_STATE_OPERATIONAL;
+	} else {
+		err = FAILED;
+		hba->ufshcd_state = UFSHCD_STATE_ERROR;
+	}
+	ufshcd_clear_eh_in_progress(hba);
+	spin_unlock_irqrestore(hba->host->host_lock, flags);
+
 	return err;
 }
 
@@ -2737,8 +2995,13 @@ static void ufshcd_async_scan(void *data, async_cookie_t cookie)
 		goto out;
 
 	ufshcd_force_reset_auto_bkops(hba);
-	scsi_scan_host(hba->host);
-	pm_runtime_put_sync(hba->dev);
+	hba->ufshcd_state = UFSHCD_STATE_OPERATIONAL;
+
+	/* 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);
+	}
 out:
 	return;
 }
@@ -2751,8 +3014,8 @@ static struct scsi_host_template ufshcd_driver_template = {
 	.slave_alloc		= ufshcd_slave_alloc,
 	.slave_destroy		= ufshcd_slave_destroy,
 	.eh_abort_handler	= ufshcd_abort,
-	.eh_device_reset_handler = ufshcd_device_reset,
-	.eh_host_reset_handler	= ufshcd_host_reset,
+	.eh_device_reset_handler = ufshcd_eh_device_reset_handler,
+	.eh_host_reset_handler   = ufshcd_eh_host_reset_handler,
 	.this_id		= -1,
 	.sg_tablesize		= SG_ALL,
 	.cmd_per_lun		= UFSHCD_CMD_PER_LUN,
@@ -2916,10 +3179,11 @@ int ufshcd_init(struct device *dev, struct ufs_hba **hba_handle,
 	host->max_cmd_len = MAX_CDB_SIZE;
 
 	/* Initailize wait queue for task management */
-	init_waitqueue_head(&hba->ufshcd_tm_wait_queue);
+	init_waitqueue_head(&hba->tm_wq);
+	init_waitqueue_head(&hba->tm_tag_wq);
 
 	/* Initialize work queues */
-	INIT_WORK(&hba->feh_workq, ufshcd_fatal_err_handler);
+	INIT_WORK(&hba->eh_work, ufshcd_err_handler);
 	INIT_WORK(&hba->eeh_work, ufshcd_exception_event_handler);
 
 	/* Initialize UIC command mutex */
diff --git a/drivers/scsi/ufs/ufshcd.h b/drivers/scsi/ufs/ufshcd.h
index 577679a2d18..acf318e338e 100644
--- a/drivers/scsi/ufs/ufshcd.h
+++ b/drivers/scsi/ufs/ufshcd.h
@@ -174,15 +174,21 @@ struct ufs_dev_cmd {
  * @irq: Irq number of the controller
  * @active_uic_cmd: handle of active UIC command
  * @uic_cmd_mutex: mutex for uic command
- * @ufshcd_tm_wait_queue: wait queue for task management
+ * @tm_wq: wait queue for task management
+ * @tm_tag_wq: wait queue for free task management slots
+ * @tm_slots_in_use: bit map of task management request slots in use
  * @pwr_done: completion for power mode change
  * @tm_condition: condition variable for task management
  * @ufshcd_state: UFSHCD states
+ * @eh_flags: Error handling flags
  * @intr_mask: Interrupt Mask Bits
  * @ee_ctrl_mask: Exception event control mask
- * @feh_workq: Work queue for fatal controller error handling
+ * @eh_work: Worker to handle UFS errors that require s/w attention
  * @eeh_work: Worker to handle exception events
  * @errors: HBA errors
+ * @uic_error: UFS interconnect layer error status
+ * @saved_err: sticky error mask
+ * @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
  */
@@ -217,21 +223,27 @@ struct ufs_hba {
 	struct uic_command *active_uic_cmd;
 	struct mutex uic_cmd_mutex;
 
-	wait_queue_head_t ufshcd_tm_wait_queue;
+	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;
 
 	u32 ufshcd_state;
+	u32 eh_flags;
 	u32 intr_mask;
 	u16 ee_ctrl_mask;
 
 	/* Work Queues */
-	struct work_struct feh_workq;
+	struct work_struct eh_work;
 	struct work_struct eeh_work;
 
 	/* HBA Errors */
 	u32 errors;
+	u32 uic_error;
+	u32 saved_err;
+	u32 saved_uic_err;
 
 	/* Device management request data */
 	struct ufs_dev_cmd dev_cmd;
@@ -263,6 +275,8 @@ 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);
diff --git a/drivers/scsi/ufs/ufshci.h b/drivers/scsi/ufs/ufshci.h
index 0475c6619a6..9abc7e32b43 100644
--- a/drivers/scsi/ufs/ufshci.h
+++ b/drivers/scsi/ufs/ufshci.h
@@ -304,10 +304,10 @@ enum {
  * @size: size of physical segment DW-3
  */
 struct ufshcd_sg_entry {
-	u32    base_addr;
-	u32    upper_addr;
-	u32    reserved;
-	u32    size;
+	__le32    base_addr;
+	__le32    upper_addr;
+	__le32    reserved;
+	__le32    size;
 };
 
 /**
@@ -330,10 +330,10 @@ struct utp_transfer_cmd_desc {
  * @dword3: Descriptor Header DW3
  */
 struct request_desc_header {
-	u32 dword_0;
-	u32 dword_1;
-	u32 dword_2;
-	u32 dword_3;
+	__le32 dword_0;
+	__le32 dword_1;
+	__le32 dword_2;
+	__le32 dword_3;
 };
 
 /**
@@ -352,16 +352,16 @@ struct utp_transfer_req_desc {
 	struct request_desc_header header;
 
 	/* DW 4-5*/
-	u32  command_desc_base_addr_lo;
-	u32  command_desc_base_addr_hi;
+	__le32  command_desc_base_addr_lo;
+	__le32  command_desc_base_addr_hi;
 
 	/* DW 6 */
-	u16  response_upiu_length;
-	u16  response_upiu_offset;
+	__le16  response_upiu_length;
+	__le16  response_upiu_offset;
 
 	/* DW 7 */
-	u16  prd_table_length;
-	u16  prd_table_offset;
+	__le16  prd_table_length;
+	__le16  prd_table_offset;
 };
 
 /**
@@ -376,10 +376,10 @@ struct utp_task_req_desc {
 	struct request_desc_header header;
 
 	/* DW 4-11 */
-	u32 task_req_upiu[TASK_REQ_UPIU_SIZE_DWORDS];
+	__le32 task_req_upiu[TASK_REQ_UPIU_SIZE_DWORDS];
 
 	/* DW 12-19 */
-	u32 task_rsp_upiu[TASK_RSP_UPIU_SIZE_DWORDS];
+	__le32 task_rsp_upiu[TASK_RSP_UPIU_SIZE_DWORDS];
 };
 
 #endif /* End of Header */
diff --git a/drivers/scsi/virtio_scsi.c b/drivers/scsi/virtio_scsi.c
index db3b494e592..d4727b33947 100644
--- a/drivers/scsi/virtio_scsi.c
+++ b/drivers/scsi/virtio_scsi.c
@@ -73,17 +73,12 @@ struct virtio_scsi_vq {
  * queue, and also lets the driver optimize the IRQ affinity for the virtqueues
  * (each virtqueue's affinity is set to the CPU that "owns" the queue).
  *
- * An interesting effect of this policy is that only writes to req_vq need to
- * take the tgt_lock.  Read can be done outside the lock because:
+ * tgt_lock is held to serialize reading and writing req_vq. Reading req_vq
+ * could be done locklessly, but we do not do it yet.
  *
- * - writes of req_vq only occur when atomic_inc_return(&tgt->reqs) returns 1.
- *   In that case, no other CPU is reading req_vq: even if they were in
- *   virtscsi_queuecommand_multi, they would be spinning on tgt_lock.
- *
- * - reads of req_vq only occur when the target is not idle (reqs != 0).
- *   A CPU that enters virtscsi_queuecommand_multi will not modify req_vq.
- *
- * Similarly, decrements of reqs are never concurrent with writes of req_vq.
+ * Decrements of reqs are never concurrent with writes of req_vq: before the
+ * decrement reqs will be != 0; after the decrement the virtqueue completion
+ * routine will not use the req_vq so it can be changed by a new request.
  * Thus they can happen outside the tgt_lock, provided of course we make reqs
  * an atomic_t.
  */
@@ -204,7 +199,6 @@ static void virtscsi_complete_cmd(struct virtio_scsi *vscsi, void *buf)
 			set_driver_byte(sc, DRIVER_SENSE);
 	}
 
-	mempool_free(cmd, virtscsi_cmd_pool);
 	sc->scsi_done(sc);
 
 	atomic_dec(&tgt->reqs);
@@ -238,38 +232,6 @@ static void virtscsi_req_done(struct virtqueue *vq)
 	int index = vq->index - VIRTIO_SCSI_VQ_BASE;
 	struct virtio_scsi_vq *req_vq = &vscsi->req_vqs[index];
 
-	/*
-	 * Read req_vq before decrementing the reqs field in
-	 * virtscsi_complete_cmd.
-	 *
-	 * With barriers:
-	 *
-	 * 	CPU #0			virtscsi_queuecommand_multi (CPU #1)
-	 * 	------------------------------------------------------------
-	 * 	lock vq_lock
-	 * 	read req_vq
-	 * 	read reqs (reqs = 1)
-	 * 	write reqs (reqs = 0)
-	 * 				increment reqs (reqs = 1)
-	 * 				write req_vq
-	 *
-	 * Possible reordering without barriers:
-	 *
-	 * 	CPU #0			virtscsi_queuecommand_multi (CPU #1)
-	 * 	------------------------------------------------------------
-	 * 	lock vq_lock
-	 * 	read reqs (reqs = 1)
-	 * 	write reqs (reqs = 0)
-	 * 				increment reqs (reqs = 1)
-	 * 				write req_vq
-	 * 	read (wrong) req_vq
-	 *
-	 * We do not need a full smp_rmb, because req_vq is required to get
-	 * to tgt->reqs: tgt is &vscsi->tgt[sc->device->id], where sc is stored
-	 * in the virtqueue as the user token.
-	 */
-	smp_read_barrier_depends();
-
 	virtscsi_vq_done(vscsi, req_vq, virtscsi_complete_cmd);
 };
 
@@ -279,8 +241,6 @@ static void virtscsi_complete_free(struct virtio_scsi *vscsi, void *buf)
 
 	if (cmd->comp)
 		complete_all(cmd->comp);
-	else
-		mempool_free(cmd, virtscsi_cmd_pool);
 }
 
 static void virtscsi_ctrl_done(struct virtqueue *vq)
@@ -496,10 +456,9 @@ static int virtscsi_queuecommand(struct virtio_scsi *vscsi,
 				 struct virtio_scsi_vq *req_vq,
 				 struct scsi_cmnd *sc)
 {
-	struct virtio_scsi_cmd *cmd;
-	int ret;
-
 	struct Scsi_Host *shost = virtio_scsi_host(vscsi->vdev);
+	struct virtio_scsi_cmd *cmd = scsi_cmd_priv(sc);
+
 	BUG_ON(scsi_sg_count(sc) > shost->sg_tablesize);
 
 	/* TODO: check feature bit and fail if unsupported?  */
@@ -508,11 +467,6 @@ static int virtscsi_queuecommand(struct virtio_scsi *vscsi,
 	dev_dbg(&sc->device->sdev_gendev,
 		"cmd %p CDB: %#02x\n", sc, sc->cmnd[0]);
 
-	ret = SCSI_MLQUEUE_HOST_BUSY;
-	cmd = mempool_alloc(virtscsi_cmd_pool, GFP_ATOMIC);
-	if (!cmd)
-		goto out;
-
 	memset(cmd, 0, sizeof(*cmd));
 	cmd->sc = sc;
 	cmd->req.cmd = (struct virtio_scsi_cmd_req){
@@ -531,13 +485,9 @@ static int virtscsi_queuecommand(struct virtio_scsi *vscsi,
 
 	if (virtscsi_kick_cmd(req_vq, cmd,
 			      sizeof cmd->req.cmd, sizeof cmd->resp.cmd,
-			      GFP_ATOMIC) == 0)
-		ret = 0;
-	else
-		mempool_free(cmd, virtscsi_cmd_pool);
-
-out:
-	return ret;
+			      GFP_ATOMIC) != 0)
+		return SCSI_MLQUEUE_HOST_BUSY;
+	return 0;
 }
 
 static int virtscsi_queuecommand_single(struct Scsi_Host *sh,
@@ -560,12 +510,8 @@ static struct virtio_scsi_vq *virtscsi_pick_vq(struct virtio_scsi *vscsi,
 
 	spin_lock_irqsave(&tgt->tgt_lock, flags);
 
-	/*
-	 * The memory barrier after atomic_inc_return matches
-	 * the smp_read_barrier_depends() in virtscsi_req_done.
-	 */
 	if (atomic_inc_return(&tgt->reqs) > 1)
-		vq = ACCESS_ONCE(tgt->req_vq);
+		vq = tgt->req_vq;
 	else {
 		queue_num = smp_processor_id();
 		while (unlikely(queue_num >= vscsi->num_queues))
@@ -683,6 +629,7 @@ static struct scsi_host_template virtscsi_host_template_single = {
 	.name = "Virtio SCSI HBA",
 	.proc_name = "virtio_scsi",
 	.this_id = -1,
+	.cmd_size = sizeof(struct virtio_scsi_cmd),
 	.queuecommand = virtscsi_queuecommand_single,
 	.eh_abort_handler = virtscsi_abort,
 	.eh_device_reset_handler = virtscsi_device_reset,
@@ -699,6 +646,7 @@ static struct scsi_host_template virtscsi_host_template_multi = {
 	.name = "Virtio SCSI HBA",
 	.proc_name = "virtio_scsi",
 	.this_id = -1,
+	.cmd_size = sizeof(struct virtio_scsi_cmd),
 	.queuecommand = virtscsi_queuecommand_multi,
 	.eh_abort_handler = virtscsi_abort,
 	.eh_device_reset_handler = virtscsi_device_reset,
diff --git a/drivers/sh/Makefile b/drivers/sh/Makefile
index fc67f564f02..788ed9b59b4 100644
--- a/drivers/sh/Makefile
+++ b/drivers/sh/Makefile
@@ -1,10 +1,12 @@
 #
 # Makefile for the SuperH specific drivers.
 #
-obj-y	:= intc/
+obj-$(CONFIG_SUPERH)			+= intc/
+obj-$(CONFIG_ARCH_SHMOBILE_LEGACY)	+= intc/
+ifneq ($(CONFIG_COMMON_CLK),y)
+obj-$(CONFIG_HAVE_CLK)			+= clk/
+endif
+obj-$(CONFIG_MAPLE)			+= maple/
+obj-$(CONFIG_SUPERHYWAY)		+= superhyway/
 
-obj-$(CONFIG_HAVE_CLK)		+= clk/
-obj-$(CONFIG_MAPLE)		+= maple/
-obj-$(CONFIG_SUPERHYWAY)	+= superhyway/
-
-obj-y				+= pm_runtime.o
+obj-y					+= pm_runtime.o
diff --git a/drivers/sh/clk/core.c b/drivers/sh/clk/core.c
index 74727851820..be56b22ca94 100644
--- a/drivers/sh/clk/core.c
+++ b/drivers/sh/clk/core.c
@@ -196,17 +196,11 @@ int clk_rate_table_find(struct clk *clk,
 			struct cpufreq_frequency_table *freq_table,
 			unsigned long rate)
 {
-	int i;
-
-	for (i = 0; freq_table[i].frequency != CPUFREQ_TABLE_END; i++) {
-		unsigned long freq = freq_table[i].frequency;
+	struct cpufreq_frequency_table *pos;
 
-		if (freq == CPUFREQ_ENTRY_INVALID)
-			continue;
-
-		if (freq == rate)
-			return i;
-	}
+	cpufreq_for_each_valid_entry(pos, freq_table)
+		if (pos->frequency == rate)
+			return pos - freq_table;
 
 	return -ENOENT;
 }
@@ -575,11 +569,7 @@ long clk_round_parent(struct clk *clk, unsigned long target,
 		return abs(target - *best_freq);
 	}
 
-	for (freq = parent->freq_table; freq->frequency != CPUFREQ_TABLE_END;
-	     freq++) {
-		if (freq->frequency == CPUFREQ_ENTRY_INVALID)
-			continue;
-
+	cpufreq_for_each_valid_entry(freq, parent->freq_table) {
 		if (unlikely(freq->frequency / target <= div_min - 1)) {
 			unsigned long freq_max;
 
diff --git a/drivers/sh/intc/Kconfig b/drivers/sh/intc/Kconfig
index f7d90617c9d..60228fae943 100644
--- a/drivers/sh/intc/Kconfig
+++ b/drivers/sh/intc/Kconfig
@@ -6,7 +6,7 @@ comment "Interrupt controller options"
 
 config INTC_USERIMASK
 	bool "Userspace interrupt masking support"
-	depends on ARCH_SHMOBILE || (SUPERH && CPU_SH4A) || COMPILE_TEST
+	depends on (SUPERH && CPU_SH4A) || COMPILE_TEST
 	help
 	  This enables support for hardware-assisted userspace hardirq
 	  masking.
diff --git a/drivers/sh/intc/core.c b/drivers/sh/intc/core.c
index 8f32a1323a7..81f22980b2d 100644
--- a/drivers/sh/intc/core.c
+++ b/drivers/sh/intc/core.c
@@ -80,12 +80,6 @@ static void __init intc_register_irq(struct intc_desc *desc,
 	unsigned int data[2], primary;
 	unsigned long flags;
 
-	/*
-	 * Register the IRQ position with the global IRQ map, then insert
-	 * it in to the radix tree.
-	 */
-	irq_reserve_irq(irq);
-
 	raw_spin_lock_irqsave(&intc_big_lock, flags);
 	radix_tree_insert(&d->tree, enum_id, intc_irq_xlate_get(irq));
 	raw_spin_unlock_irqrestore(&intc_big_lock, flags);
diff --git a/drivers/sh/pm_runtime.c b/drivers/sh/pm_runtime.c
index 8afa5a4589f..72f63817a1a 100644
--- a/drivers/sh/pm_runtime.c
+++ b/drivers/sh/pm_runtime.c
@@ -21,18 +21,43 @@
 #include <linux/slab.h>
 
 #ifdef CONFIG_PM_RUNTIME
-
-static int default_platform_runtime_idle(struct device *dev)
+static int sh_pm_runtime_suspend(struct device *dev)
 {
-	/* suspend synchronously to disable clocks immediately */
+	int ret;
+
+	ret = pm_generic_runtime_suspend(dev);
+	if (ret) {
+		dev_err(dev, "failed to suspend device\n");
+		return ret;
+	}
+
+	ret = pm_clk_suspend(dev);
+	if (ret) {
+		dev_err(dev, "failed to suspend clock\n");
+		pm_generic_runtime_resume(dev);
+		return ret;
+	}
+
 	return 0;
 }
 
+static int sh_pm_runtime_resume(struct device *dev)
+{
+	int ret;
+
+	ret = pm_clk_resume(dev);
+	if (ret) {
+		dev_err(dev, "failed to resume clock\n");
+		return ret;
+	}
+
+	return pm_generic_runtime_resume(dev);
+}
+
 static struct dev_pm_domain default_pm_domain = {
 	.ops = {
-		.runtime_suspend = pm_clk_suspend,
-		.runtime_resume = pm_clk_resume,
-		.runtime_idle = default_platform_runtime_idle,
+		.runtime_suspend = sh_pm_runtime_suspend,
+		.runtime_resume = sh_pm_runtime_resume,
 		USE_PLATFORM_PM_SLEEP_OPS
 	},
 };
@@ -50,8 +75,28 @@ 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)) {
+		if (!of_machine_is_compatible("renesas,emev2") &&
+		    !of_machine_is_compatible("renesas,r7s72100") &&
+		    !of_machine_is_compatible("renesas,r8a73a4") &&
+		    !of_machine_is_compatible("renesas,r8a7740") &&
+		    !of_machine_is_compatible("renesas,r8a7778") &&
+		    !of_machine_is_compatible("renesas,r8a7779") &&
+		    !of_machine_is_compatible("renesas,r8a7790") &&
+		    !of_machine_is_compatible("renesas,r8a7791") &&
+		    !of_machine_is_compatible("renesas,r8a7792") &&
+		    !of_machine_is_compatible("renesas,r8a7793") &&
+		    !of_machine_is_compatible("renesas,r8a7794") &&
+		    !of_machine_is_compatible("renesas,sh7372") &&
+		    !of_machine_is_compatible("renesas,sh73a0"))
+			return 0;
+	}
+
+	default_pm_on = true;
 	pm_clk_add_notifier(&platform_bus_type, &platform_bus_notifier);
 	return 0;
 }
@@ -59,7 +104,8 @@ core_initcall(sh_pm_runtime_init);
 
 static int __init sh_pm_runtime_late_init(void)
 {
-	pm_genpd_poweroff_unused();
+	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
new file mode 100644
index 00000000000..c8543855aa8
--- /dev/null
+++ b/drivers/soc/Kconfig
@@ -0,0 +1,5 @@
+menu "SOC (System On Chip) specific Drivers"
+
+source "drivers/soc/qcom/Kconfig"
+
+endmenu
diff --git a/drivers/soc/Makefile b/drivers/soc/Makefile
new file mode 100644
index 00000000000..0f7c44793b2
--- /dev/null
+++ b/drivers/soc/Makefile
@@ -0,0 +1,5 @@
+#
+# Makefile for the Linux Kernel SOC specific device drivers.
+#
+
+obj-$(CONFIG_ARCH_QCOM)		+= qcom/
diff --git a/drivers/soc/qcom/Kconfig b/drivers/soc/qcom/Kconfig
new file mode 100644
index 00000000000..7bd2c94f54a
--- /dev/null
+++ b/drivers/soc/qcom/Kconfig
@@ -0,0 +1,11 @@
+#
+# QCOM Soc drivers
+#
+config QCOM_GSBI
+        tristate "QCOM General Serial Bus Interface"
+        depends on ARCH_QCOM
+        help
+          Say y here to enable GSBI support.  The GSBI provides control
+          functions for connecting the underlying serial UART, SPI, and I2C
+          devices to the output pins.
+
diff --git a/drivers/soc/qcom/Makefile b/drivers/soc/qcom/Makefile
new file mode 100644
index 00000000000..438901257ac
--- /dev/null
+++ b/drivers/soc/qcom/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_QCOM_GSBI)	+=	qcom_gsbi.o
diff --git a/drivers/soc/qcom/qcom_gsbi.c b/drivers/soc/qcom/qcom_gsbi.c
new file mode 100644
index 00000000000..447458e696a
--- /dev/null
+++ b/drivers/soc/qcom/qcom_gsbi.c
@@ -0,0 +1,85 @@
+/*
+ * 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 rev 2 and
+ * only rev 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>
+
+#define GSBI_CTRL_REG		0x0000
+#define GSBI_PROTOCOL_SHIFT	4
+
+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;
+
+	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)) {
+		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);
+
+	dev_info(&pdev->dev, "GSBI port protocol: %d crci: %d\n", mode, crci);
+
+	hclk = devm_clk_get(&pdev->dev, "iface");
+	if (IS_ERR(hclk))
+		return PTR_ERR(hclk);
+
+	clk_prepare_enable(hclk);
+
+	writel_relaxed((mode << GSBI_PROTOCOL_SHIFT) | crci,
+				base + GSBI_CTRL_REG);
+
+	/* make sure the gsbi control write is not reordered */
+	wmb();
+
+	clk_disable_unprepare(hclk);
+
+	return of_platform_populate(pdev->dev.of_node, NULL, NULL, &pdev->dev);
+}
+
+static const struct of_device_id gsbi_dt_match[] = {
+	{ .compatible = "qcom,gsbi-v1.0.0", },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(of, gsbi_dt_match);
+
+static struct platform_driver gsbi_driver = {
+	.driver = {
+		.name		= "gsbi",
+		.owner		= THIS_MODULE,
+		.of_match_table	= gsbi_dt_match,
+	},
+	.probe = gsbi_probe,
+};
+
+module_platform_driver(gsbi_driver);
+
+MODULE_AUTHOR("Andy Gross <agross@codeaurora.org>");
+MODULE_DESCRIPTION("QCOM GSBI driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index 60f2b41c731..213b5cbb9dc 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -91,8 +91,8 @@ config SPI_BFIN5XX
 	help
 	  This is the SPI controller master driver for Blackfin 5xx processor.
 
-config SPI_BFIN_V3
-	tristate "SPI controller v3 for Blackfin"
+config SPI_ADI_V3
+	tristate "SPI controller v3 for ADI"
 	depends on BF60x
 	help
 	  This is the SPI controller v3 master driver
@@ -148,6 +148,13 @@ config SPI_BUTTERFLY
 	  inexpensive battery powered microcontroller evaluation board.
 	  This same cable can be used to flash new firmware.
 
+config SPI_CADENCE
+	tristate "Cadence SPI controller"
+	depends on ARM
+	help
+	  This selects the Cadence SPI controller master driver
+	  used by Xilinx Zynq.
+
 config SPI_CLPS711X
 	tristate "CLPS711X host SPI controller"
 	depends on ARCH_CLPS711X || COMPILE_TEST
@@ -505,7 +512,7 @@ config SPI_TEGRA20_SLINK
 
 config SPI_TOPCLIFF_PCH
 	tristate "Intel EG20T PCH/LAPIS Semicon IOH(ML7213/ML7223/ML7831) SPI"
-	depends on PCI
+	depends on PCI && (X86_32 || COMPILE_TEST)
 	help
 	  SPI driver for the Topcliff PCH (Platform Controller Hub) SPI bus
 	  used in some x86 embedded processors.
diff --git a/drivers/spi/Makefile b/drivers/spi/Makefile
index bd792669e56..929c9f5eac0 100644
--- a/drivers/spi/Makefile
+++ b/drivers/spi/Makefile
@@ -18,10 +18,11 @@ obj-$(CONFIG_SPI_BCM2835)		+= spi-bcm2835.o
 obj-$(CONFIG_SPI_BCM63XX)		+= spi-bcm63xx.o
 obj-$(CONFIG_SPI_BCM63XX_HSSPI)		+= spi-bcm63xx-hsspi.o
 obj-$(CONFIG_SPI_BFIN5XX)		+= spi-bfin5xx.o
-obj-$(CONFIG_SPI_BFIN_V3)               += spi-bfin-v3.o
+obj-$(CONFIG_SPI_ADI_V3)                += spi-adi-v3.o
 obj-$(CONFIG_SPI_BFIN_SPORT)		+= spi-bfin-sport.o
 obj-$(CONFIG_SPI_BITBANG)		+= spi-bitbang.o
 obj-$(CONFIG_SPI_BUTTERFLY)		+= spi-butterfly.o
+obj-$(CONFIG_SPI_CADENCE)		+= spi-cadence.o
 obj-$(CONFIG_SPI_CLPS711X)		+= spi-clps711x.o
 obj-$(CONFIG_SPI_COLDFIRE_QSPI)		+= spi-coldfire-qspi.o
 obj-$(CONFIG_SPI_DAVINCI)		+= spi-davinci.o
diff --git a/drivers/spi/spi-bfin-v3.c b/drivers/spi/spi-adi-v3.c
index 4089d0e0d84..dcb2287c7f8 100644
--- a/drivers/spi/spi-bfin-v3.c
+++ b/drivers/spi/spi-adi-v3.c
@@ -1,7 +1,7 @@
 /*
  * Analog Devices SPI3 controller driver
  *
- * Copyright (c) 2013 Analog Devices Inc.
+ * Copyright (c) 2014 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
@@ -13,6 +13,7 @@
  * GNU General Public License for more details.
  */
 
+#include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/device.h>
 #include <linux/dma-mapping.h>
@@ -26,35 +27,34 @@
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <linux/spi/spi.h>
+#include <linux/spi/adi_spi3.h>
 #include <linux/types.h>
 
-#include <asm/bfin_spi3.h>
-#include <asm/cacheflush.h>
 #include <asm/dma.h>
 #include <asm/portmux.h>
 
-enum bfin_spi_state {
+enum adi_spi_state {
 	START_STATE,
 	RUNNING_STATE,
 	DONE_STATE,
 	ERROR_STATE
 };
 
-struct bfin_spi_master;
+struct adi_spi_master;
 
-struct bfin_spi_transfer_ops {
-	void (*write) (struct bfin_spi_master *);
-	void (*read) (struct bfin_spi_master *);
-	void (*duplex) (struct bfin_spi_master *);
+struct adi_spi_transfer_ops {
+	void (*write) (struct adi_spi_master *);
+	void (*read) (struct adi_spi_master *);
+	void (*duplex) (struct adi_spi_master *);
 };
 
 /* runtime info for spi master */
-struct bfin_spi_master {
+struct adi_spi_master {
 	/* SPI framework hookup */
 	struct spi_master *master;
 
 	/* Regs base of SPI controller */
-	struct bfin_spi_regs __iomem *regs;
+	struct adi_spi_regs __iomem *regs;
 
 	/* Pin request list */
 	u16 *pin_req;
@@ -65,7 +65,7 @@ struct bfin_spi_master {
 	/* Current message transfer state info */
 	struct spi_message *cur_msg;
 	struct spi_transfer *cur_transfer;
-	struct bfin_spi_device *cur_chip;
+	struct adi_spi_device *cur_chip;
 	unsigned transfer_len;
 
 	/* transfer buffer */
@@ -90,12 +90,12 @@ struct bfin_spi_master {
 	u32 ssel;
 
 	unsigned long sclk;
-	enum bfin_spi_state state;
+	enum adi_spi_state state;
 
-	const struct bfin_spi_transfer_ops *ops;
+	const struct adi_spi_transfer_ops *ops;
 };
 
-struct bfin_spi_device {
+struct adi_spi_device {
 	u32 control;
 	u32 clock;
 	u32 ssel;
@@ -105,17 +105,25 @@ struct bfin_spi_device {
 	u32 cs_gpio;
 	u32 tx_dummy_val; /* tx value for rx only transfer */
 	bool enable_dma;
-	const struct bfin_spi_transfer_ops *ops;
+	const struct adi_spi_transfer_ops *ops;
 };
 
-static void bfin_spi_enable(struct bfin_spi_master *drv_data)
+static void adi_spi_enable(struct adi_spi_master *drv_data)
 {
-	bfin_write_or(&drv_data->regs->control, SPI_CTL_EN);
+	u32 ctl;
+
+	ctl = ioread32(&drv_data->regs->control);
+	ctl |= SPI_CTL_EN;
+	iowrite32(ctl, &drv_data->regs->control);
 }
 
-static void bfin_spi_disable(struct bfin_spi_master *drv_data)
+static void adi_spi_disable(struct adi_spi_master *drv_data)
 {
-	bfin_write_and(&drv_data->regs->control, ~SPI_CTL_EN);
+	u32 ctl;
+
+	ctl = ioread32(&drv_data->regs->control);
+	ctl &= ~SPI_CTL_EN;
+	iowrite32(ctl, &drv_data->regs->control);
 }
 
 /* Caculate the SPI_CLOCK register value based on input HZ */
@@ -128,35 +136,43 @@ static u32 hz_to_spi_clock(u32 sclk, u32 speed_hz)
 	return spi_clock;
 }
 
-static int bfin_spi_flush(struct bfin_spi_master *drv_data)
+static int adi_spi_flush(struct adi_spi_master *drv_data)
 {
 	unsigned long limit = loops_per_jiffy << 1;
 
 	/* wait for stop and clear stat */
-	while (!(bfin_read(&drv_data->regs->status) & SPI_STAT_SPIF) && --limit)
+	while (!(ioread32(&drv_data->regs->status) & SPI_STAT_SPIF) && --limit)
 		cpu_relax();
 
-	bfin_write(&drv_data->regs->status, 0xFFFFFFFF);
+	iowrite32(0xFFFFFFFF, &drv_data->regs->status);
 
 	return limit;
 }
 
 /* Chip select operation functions for cs_change flag */
-static void bfin_spi_cs_active(struct bfin_spi_master *drv_data, struct bfin_spi_device *chip)
+static void adi_spi_cs_active(struct adi_spi_master *drv_data, struct adi_spi_device *chip)
 {
-	if (likely(chip->cs < MAX_CTRL_CS))
-		bfin_write_and(&drv_data->regs->ssel, ~chip->ssel);
-	else
+	if (likely(chip->cs < MAX_CTRL_CS)) {
+		u32 reg;
+		reg = ioread32(&drv_data->regs->ssel);
+		reg &= ~chip->ssel;
+		iowrite32(reg, &drv_data->regs->ssel);
+	} else {
 		gpio_set_value(chip->cs_gpio, 0);
+	}
 }
 
-static void bfin_spi_cs_deactive(struct bfin_spi_master *drv_data,
-				struct bfin_spi_device *chip)
+static void adi_spi_cs_deactive(struct adi_spi_master *drv_data,
+				struct adi_spi_device *chip)
 {
-	if (likely(chip->cs < MAX_CTRL_CS))
-		bfin_write_or(&drv_data->regs->ssel, chip->ssel);
-	else
+	if (likely(chip->cs < MAX_CTRL_CS)) {
+		u32 reg;
+		reg = ioread32(&drv_data->regs->ssel);
+		reg |= chip->ssel;
+		iowrite32(reg, &drv_data->regs->ssel);
+	} else {
 		gpio_set_value(chip->cs_gpio, 1);
+	}
 
 	/* Move delay here for consistency */
 	if (chip->cs_chg_udelay)
@@ -164,187 +180,192 @@ static void bfin_spi_cs_deactive(struct bfin_spi_master *drv_data,
 }
 
 /* enable or disable the pin muxed by GPIO and SPI CS to work as SPI CS */
-static inline void bfin_spi_cs_enable(struct bfin_spi_master *drv_data,
-					struct bfin_spi_device *chip)
+static inline void adi_spi_cs_enable(struct adi_spi_master *drv_data,
+					struct adi_spi_device *chip)
 {
-	if (chip->cs < MAX_CTRL_CS)
-		bfin_write_or(&drv_data->regs->ssel, chip->ssel >> 8);
+	if (chip->cs < MAX_CTRL_CS) {
+		u32 reg;
+		reg = ioread32(&drv_data->regs->ssel);
+		reg |= chip->ssel >> 8;
+		iowrite32(reg, &drv_data->regs->ssel);
+	}
 }
 
-static inline void bfin_spi_cs_disable(struct bfin_spi_master *drv_data,
-					struct bfin_spi_device *chip)
+static inline void adi_spi_cs_disable(struct adi_spi_master *drv_data,
+					struct adi_spi_device *chip)
 {
-	if (chip->cs < MAX_CTRL_CS)
-		bfin_write_and(&drv_data->regs->ssel, ~(chip->ssel >> 8));
+	if (chip->cs < MAX_CTRL_CS) {
+		u32 reg;
+		reg = ioread32(&drv_data->regs->ssel);
+		reg &= ~(chip->ssel >> 8);
+		iowrite32(reg, &drv_data->regs->ssel);
+	}
 }
 
 /* stop controller and re-config current chip*/
-static void bfin_spi_restore_state(struct bfin_spi_master *drv_data)
+static void adi_spi_restore_state(struct adi_spi_master *drv_data)
 {
-	struct bfin_spi_device *chip = drv_data->cur_chip;
+	struct adi_spi_device *chip = drv_data->cur_chip;
 
 	/* Clear status and disable clock */
-	bfin_write(&drv_data->regs->status, 0xFFFFFFFF);
-	bfin_write(&drv_data->regs->rx_control, 0x0);
-	bfin_write(&drv_data->regs->tx_control, 0x0);
-	bfin_spi_disable(drv_data);
-
-	SSYNC();
+	iowrite32(0xFFFFFFFF, &drv_data->regs->status);
+	iowrite32(0x0, &drv_data->regs->rx_control);
+	iowrite32(0x0, &drv_data->regs->tx_control);
+	adi_spi_disable(drv_data);
 
 	/* Load the registers */
-	bfin_write(&drv_data->regs->control, chip->control);
-	bfin_write(&drv_data->regs->clock, chip->clock);
+	iowrite32(chip->control, &drv_data->regs->control);
+	iowrite32(chip->clock, &drv_data->regs->clock);
 
-	bfin_spi_enable(drv_data);
+	adi_spi_enable(drv_data);
 	drv_data->tx_num = drv_data->rx_num = 0;
 	/* we always choose tx transfer initiate */
-	bfin_write(&drv_data->regs->rx_control, SPI_RXCTL_REN);
-	bfin_write(&drv_data->regs->tx_control,
-			SPI_TXCTL_TEN | SPI_TXCTL_TTI);
-	bfin_spi_cs_active(drv_data, chip);
+	iowrite32(SPI_RXCTL_REN, &drv_data->regs->rx_control);
+	iowrite32(SPI_TXCTL_TEN | SPI_TXCTL_TTI, &drv_data->regs->tx_control);
+	adi_spi_cs_active(drv_data, chip);
 }
 
 /* discard invalid rx data and empty rfifo */
-static inline void dummy_read(struct bfin_spi_master *drv_data)
+static inline void dummy_read(struct adi_spi_master *drv_data)
 {
-	while (!(bfin_read(&drv_data->regs->status) & SPI_STAT_RFE))
-		bfin_read(&drv_data->regs->rfifo);
+	while (!(ioread32(&drv_data->regs->status) & SPI_STAT_RFE))
+		ioread32(&drv_data->regs->rfifo);
 }
 
-static void bfin_spi_u8_write(struct bfin_spi_master *drv_data)
+static void adi_spi_u8_write(struct adi_spi_master *drv_data)
 {
 	dummy_read(drv_data);
 	while (drv_data->tx < drv_data->tx_end) {
-		bfin_write(&drv_data->regs->tfifo, (*(u8 *)(drv_data->tx++)));
-		while (bfin_read(&drv_data->regs->status) & SPI_STAT_RFE)
+		iowrite32(*(u8 *)(drv_data->tx++), &drv_data->regs->tfifo);
+		while (ioread32(&drv_data->regs->status) & SPI_STAT_RFE)
 			cpu_relax();
-		bfin_read(&drv_data->regs->rfifo);
+		ioread32(&drv_data->regs->rfifo);
 	}
 }
 
-static void bfin_spi_u8_read(struct bfin_spi_master *drv_data)
+static void adi_spi_u8_read(struct adi_spi_master *drv_data)
 {
 	u32 tx_val = drv_data->cur_chip->tx_dummy_val;
 
 	dummy_read(drv_data);
 	while (drv_data->rx < drv_data->rx_end) {
-		bfin_write(&drv_data->regs->tfifo, tx_val);
-		while (bfin_read(&drv_data->regs->status) & SPI_STAT_RFE)
+		iowrite32(tx_val, &drv_data->regs->tfifo);
+		while (ioread32(&drv_data->regs->status) & SPI_STAT_RFE)
 			cpu_relax();
-		*(u8 *)(drv_data->rx++) = bfin_read(&drv_data->regs->rfifo);
+		*(u8 *)(drv_data->rx++) = ioread32(&drv_data->regs->rfifo);
 	}
 }
 
-static void bfin_spi_u8_duplex(struct bfin_spi_master *drv_data)
+static void adi_spi_u8_duplex(struct adi_spi_master *drv_data)
 {
 	dummy_read(drv_data);
 	while (drv_data->rx < drv_data->rx_end) {
-		bfin_write(&drv_data->regs->tfifo, (*(u8 *)(drv_data->tx++)));
-		while (bfin_read(&drv_data->regs->status) & SPI_STAT_RFE)
+		iowrite32(*(u8 *)(drv_data->tx++), &drv_data->regs->tfifo);
+		while (ioread32(&drv_data->regs->status) & SPI_STAT_RFE)
 			cpu_relax();
-		*(u8 *)(drv_data->rx++) = bfin_read(&drv_data->regs->rfifo);
+		*(u8 *)(drv_data->rx++) = ioread32(&drv_data->regs->rfifo);
 	}
 }
 
-static const struct bfin_spi_transfer_ops bfin_bfin_spi_transfer_ops_u8 = {
-	.write  = bfin_spi_u8_write,
-	.read   = bfin_spi_u8_read,
-	.duplex = bfin_spi_u8_duplex,
+static const struct adi_spi_transfer_ops adi_spi_transfer_ops_u8 = {
+	.write  = adi_spi_u8_write,
+	.read   = adi_spi_u8_read,
+	.duplex = adi_spi_u8_duplex,
 };
 
-static void bfin_spi_u16_write(struct bfin_spi_master *drv_data)
+static void adi_spi_u16_write(struct adi_spi_master *drv_data)
 {
 	dummy_read(drv_data);
 	while (drv_data->tx < drv_data->tx_end) {
-		bfin_write(&drv_data->regs->tfifo, (*(u16 *)drv_data->tx));
+		iowrite32(*(u16 *)drv_data->tx, &drv_data->regs->tfifo);
 		drv_data->tx += 2;
-		while (bfin_read(&drv_data->regs->status) & SPI_STAT_RFE)
+		while (ioread32(&drv_data->regs->status) & SPI_STAT_RFE)
 			cpu_relax();
-		bfin_read(&drv_data->regs->rfifo);
+		ioread32(&drv_data->regs->rfifo);
 	}
 }
 
-static void bfin_spi_u16_read(struct bfin_spi_master *drv_data)
+static void adi_spi_u16_read(struct adi_spi_master *drv_data)
 {
 	u32 tx_val = drv_data->cur_chip->tx_dummy_val;
 
 	dummy_read(drv_data);
 	while (drv_data->rx < drv_data->rx_end) {
-		bfin_write(&drv_data->regs->tfifo, tx_val);
-		while (bfin_read(&drv_data->regs->status) & SPI_STAT_RFE)
+		iowrite32(tx_val, &drv_data->regs->tfifo);
+		while (ioread32(&drv_data->regs->status) & SPI_STAT_RFE)
 			cpu_relax();
-		*(u16 *)drv_data->rx = bfin_read(&drv_data->regs->rfifo);
+		*(u16 *)drv_data->rx = ioread32(&drv_data->regs->rfifo);
 		drv_data->rx += 2;
 	}
 }
 
-static void bfin_spi_u16_duplex(struct bfin_spi_master *drv_data)
+static void adi_spi_u16_duplex(struct adi_spi_master *drv_data)
 {
 	dummy_read(drv_data);
 	while (drv_data->rx < drv_data->rx_end) {
-		bfin_write(&drv_data->regs->tfifo, (*(u16 *)drv_data->tx));
+		iowrite32(*(u16 *)drv_data->tx, &drv_data->regs->tfifo);
 		drv_data->tx += 2;
-		while (bfin_read(&drv_data->regs->status) & SPI_STAT_RFE)
+		while (ioread32(&drv_data->regs->status) & SPI_STAT_RFE)
 			cpu_relax();
-		*(u16 *)drv_data->rx = bfin_read(&drv_data->regs->rfifo);
+		*(u16 *)drv_data->rx = ioread32(&drv_data->regs->rfifo);
 		drv_data->rx += 2;
 	}
 }
 
-static const struct bfin_spi_transfer_ops bfin_bfin_spi_transfer_ops_u16 = {
-	.write  = bfin_spi_u16_write,
-	.read   = bfin_spi_u16_read,
-	.duplex = bfin_spi_u16_duplex,
+static const struct adi_spi_transfer_ops adi_spi_transfer_ops_u16 = {
+	.write  = adi_spi_u16_write,
+	.read   = adi_spi_u16_read,
+	.duplex = adi_spi_u16_duplex,
 };
 
-static void bfin_spi_u32_write(struct bfin_spi_master *drv_data)
+static void adi_spi_u32_write(struct adi_spi_master *drv_data)
 {
 	dummy_read(drv_data);
 	while (drv_data->tx < drv_data->tx_end) {
-		bfin_write(&drv_data->regs->tfifo, (*(u32 *)drv_data->tx));
+		iowrite32(*(u32 *)drv_data->tx, &drv_data->regs->tfifo);
 		drv_data->tx += 4;
-		while (bfin_read(&drv_data->regs->status) & SPI_STAT_RFE)
+		while (ioread32(&drv_data->regs->status) & SPI_STAT_RFE)
 			cpu_relax();
-		bfin_read(&drv_data->regs->rfifo);
+		ioread32(&drv_data->regs->rfifo);
 	}
 }
 
-static void bfin_spi_u32_read(struct bfin_spi_master *drv_data)
+static void adi_spi_u32_read(struct adi_spi_master *drv_data)
 {
 	u32 tx_val = drv_data->cur_chip->tx_dummy_val;
 
 	dummy_read(drv_data);
 	while (drv_data->rx < drv_data->rx_end) {
-		bfin_write(&drv_data->regs->tfifo, tx_val);
-		while (bfin_read(&drv_data->regs->status) & SPI_STAT_RFE)
+		iowrite32(tx_val, &drv_data->regs->tfifo);
+		while (ioread32(&drv_data->regs->status) & SPI_STAT_RFE)
 			cpu_relax();
-		*(u32 *)drv_data->rx = bfin_read(&drv_data->regs->rfifo);
+		*(u32 *)drv_data->rx = ioread32(&drv_data->regs->rfifo);
 		drv_data->rx += 4;
 	}
 }
 
-static void bfin_spi_u32_duplex(struct bfin_spi_master *drv_data)
+static void adi_spi_u32_duplex(struct adi_spi_master *drv_data)
 {
 	dummy_read(drv_data);
 	while (drv_data->rx < drv_data->rx_end) {
-		bfin_write(&drv_data->regs->tfifo, (*(u32 *)drv_data->tx));
+		iowrite32(*(u32 *)drv_data->tx, &drv_data->regs->tfifo);
 		drv_data->tx += 4;
-		while (bfin_read(&drv_data->regs->status) & SPI_STAT_RFE)
+		while (ioread32(&drv_data->regs->status) & SPI_STAT_RFE)
 			cpu_relax();
-		*(u32 *)drv_data->rx = bfin_read(&drv_data->regs->rfifo);
+		*(u32 *)drv_data->rx = ioread32(&drv_data->regs->rfifo);
 		drv_data->rx += 4;
 	}
 }
 
-static const struct bfin_spi_transfer_ops bfin_bfin_spi_transfer_ops_u32 = {
-	.write  = bfin_spi_u32_write,
-	.read   = bfin_spi_u32_read,
-	.duplex = bfin_spi_u32_duplex,
+static const struct adi_spi_transfer_ops adi_spi_transfer_ops_u32 = {
+	.write  = adi_spi_u32_write,
+	.read   = adi_spi_u32_read,
+	.duplex = adi_spi_u32_duplex,
 };
 
 
 /* test if there is more transfer to be done */
-static void bfin_spi_next_transfer(struct bfin_spi_master *drv)
+static void adi_spi_next_transfer(struct adi_spi_master *drv)
 {
 	struct spi_message *msg = drv->cur_msg;
 	struct spi_transfer *t = drv->cur_transfer;
@@ -360,15 +381,15 @@ static void bfin_spi_next_transfer(struct bfin_spi_master *drv)
 	}
 }
 
-static void bfin_spi_giveback(struct bfin_spi_master *drv_data)
+static void adi_spi_giveback(struct adi_spi_master *drv_data)
 {
-	struct bfin_spi_device *chip = drv_data->cur_chip;
+	struct adi_spi_device *chip = drv_data->cur_chip;
 
-	bfin_spi_cs_deactive(drv_data, chip);
+	adi_spi_cs_deactive(drv_data, chip);
 	spi_finalize_current_message(drv_data->master);
 }
 
-static int bfin_spi_setup_transfer(struct bfin_spi_master *drv)
+static int adi_spi_setup_transfer(struct adi_spi_master *drv)
 {
 	struct spi_transfer *t = drv->cur_transfer;
 	u32 cr, cr_width;
@@ -393,34 +414,33 @@ static int bfin_spi_setup_transfer(struct bfin_spi_master *drv)
 	switch (t->bits_per_word) {
 	case 8:
 		cr_width = SPI_CTL_SIZE08;
-		drv->ops = &bfin_bfin_spi_transfer_ops_u8;
+		drv->ops = &adi_spi_transfer_ops_u8;
 		break;
 	case 16:
 		cr_width = SPI_CTL_SIZE16;
-		drv->ops = &bfin_bfin_spi_transfer_ops_u16;
+		drv->ops = &adi_spi_transfer_ops_u16;
 		break;
 	case 32:
 		cr_width = SPI_CTL_SIZE32;
-		drv->ops = &bfin_bfin_spi_transfer_ops_u32;
+		drv->ops = &adi_spi_transfer_ops_u32;
 		break;
 	default:
 		return -EINVAL;
 	}
-	cr = bfin_read(&drv->regs->control) & ~SPI_CTL_SIZE;
+	cr = ioread32(&drv->regs->control) & ~SPI_CTL_SIZE;
 	cr |= cr_width;
-	bfin_write(&drv->regs->control, cr);
+	iowrite32(cr, &drv->regs->control);
 
 	/* speed setup */
-	bfin_write(&drv->regs->clock,
-			hz_to_spi_clock(drv->sclk, t->speed_hz));
+	iowrite32(hz_to_spi_clock(drv->sclk, t->speed_hz), &drv->regs->clock);
 	return 0;
 }
 
-static int bfin_spi_dma_xfer(struct bfin_spi_master *drv_data)
+static int adi_spi_dma_xfer(struct adi_spi_master *drv_data)
 {
 	struct spi_transfer *t = drv_data->cur_transfer;
 	struct spi_message *msg = drv_data->cur_msg;
-	struct bfin_spi_device *chip = drv_data->cur_chip;
+	struct adi_spi_device *chip = drv_data->cur_chip;
 	u32 dma_config;
 	unsigned long word_count, word_size;
 	void *tx_buf, *rx_buf;
@@ -498,17 +518,16 @@ static int bfin_spi_dma_xfer(struct bfin_spi_master *drv_data)
 	set_dma_config(drv_data->rx_dma, dma_config | WNR);
 	enable_dma(drv_data->tx_dma);
 	enable_dma(drv_data->rx_dma);
-	SSYNC();
 
-	bfin_write(&drv_data->regs->rx_control, SPI_RXCTL_REN | SPI_RXCTL_RDR_NE);
-	SSYNC();
-	bfin_write(&drv_data->regs->tx_control,
-			SPI_TXCTL_TEN | SPI_TXCTL_TTI | SPI_TXCTL_TDR_NF);
+	iowrite32(SPI_RXCTL_REN | SPI_RXCTL_RDR_NE,
+			&drv_data->regs->rx_control);
+	iowrite32(SPI_TXCTL_TEN | SPI_TXCTL_TTI | SPI_TXCTL_TDR_NF,
+			&drv_data->regs->tx_control);
 
 	return 0;
 }
 
-static int bfin_spi_pio_xfer(struct bfin_spi_master *drv_data)
+static int adi_spi_pio_xfer(struct adi_spi_master *drv_data)
 {
 	struct spi_message *msg = drv_data->cur_msg;
 
@@ -529,19 +548,19 @@ static int bfin_spi_pio_xfer(struct bfin_spi_master *drv_data)
 			return -EIO;
 	}
 
-	if (!bfin_spi_flush(drv_data))
+	if (!adi_spi_flush(drv_data))
 		return -EIO;
 	msg->actual_length += drv_data->transfer_len;
 	tasklet_schedule(&drv_data->pump_transfers);
 	return 0;
 }
 
-static void bfin_spi_pump_transfers(unsigned long data)
+static void adi_spi_pump_transfers(unsigned long data)
 {
-	struct bfin_spi_master *drv_data = (struct bfin_spi_master *)data;
+	struct adi_spi_master *drv_data = (struct adi_spi_master *)data;
 	struct spi_message *msg = NULL;
 	struct spi_transfer *t = NULL;
-	struct bfin_spi_device *chip = NULL;
+	struct adi_spi_device *chip = NULL;
 	int ret;
 
 	/* Get current state information */
@@ -552,7 +571,7 @@ static void bfin_spi_pump_transfers(unsigned long data)
 	/* Handle for abort */
 	if (drv_data->state == ERROR_STATE) {
 		msg->status = -EIO;
-		bfin_spi_giveback(drv_data);
+		adi_spi_giveback(drv_data);
 		return;
 	}
 
@@ -560,14 +579,14 @@ static void bfin_spi_pump_transfers(unsigned long data)
 		if (t->delay_usecs)
 			udelay(t->delay_usecs);
 		if (t->cs_change)
-			bfin_spi_cs_deactive(drv_data, chip);
-		bfin_spi_next_transfer(drv_data);
+			adi_spi_cs_deactive(drv_data, chip);
+		adi_spi_next_transfer(drv_data);
 		t = drv_data->cur_transfer;
 	}
 	/* Handle end of message */
 	if (drv_data->state == DONE_STATE) {
 		msg->status = 0;
-		bfin_spi_giveback(drv_data);
+		adi_spi_giveback(drv_data);
 		return;
 	}
 
@@ -577,34 +596,34 @@ static void bfin_spi_pump_transfers(unsigned long data)
 		return;
 	}
 
-	ret = bfin_spi_setup_transfer(drv_data);
+	ret = adi_spi_setup_transfer(drv_data);
 	if (ret) {
 		msg->status = ret;
-		bfin_spi_giveback(drv_data);
+		adi_spi_giveback(drv_data);
 	}
 
-	bfin_write(&drv_data->regs->status, 0xFFFFFFFF);
-	bfin_spi_cs_active(drv_data, chip);
+	iowrite32(0xFFFFFFFF, &drv_data->regs->status);
+	adi_spi_cs_active(drv_data, chip);
 	drv_data->state = RUNNING_STATE;
 
 	if (chip->enable_dma)
-		ret = bfin_spi_dma_xfer(drv_data);
+		ret = adi_spi_dma_xfer(drv_data);
 	else
-		ret = bfin_spi_pio_xfer(drv_data);
+		ret = adi_spi_pio_xfer(drv_data);
 	if (ret) {
 		msg->status = ret;
-		bfin_spi_giveback(drv_data);
+		adi_spi_giveback(drv_data);
 	}
 }
 
-static int bfin_spi_transfer_one_message(struct spi_master *master,
+static int adi_spi_transfer_one_message(struct spi_master *master,
 					struct spi_message *m)
 {
-	struct bfin_spi_master *drv_data = spi_master_get_devdata(master);
+	struct adi_spi_master *drv_data = spi_master_get_devdata(master);
 
 	drv_data->cur_msg = m;
 	drv_data->cur_chip = spi_get_ctldata(drv_data->cur_msg->spi);
-	bfin_spi_restore_state(drv_data);
+	adi_spi_restore_state(drv_data);
 
 	drv_data->state = START_STATE;
 	drv_data->cur_transfer = list_entry(drv_data->cur_msg->transfers.next,
@@ -630,15 +649,15 @@ static const u16 ssel[][MAX_SPI_SSEL] = {
 	P_SPI2_SSEL6, P_SPI2_SSEL7},
 };
 
-static int bfin_spi_setup(struct spi_device *spi)
+static int adi_spi_setup(struct spi_device *spi)
 {
-	struct bfin_spi_master *drv_data = spi_master_get_devdata(spi->master);
-	struct bfin_spi_device *chip = spi_get_ctldata(spi);
-	u32 bfin_ctl_reg = SPI_CTL_ODM | SPI_CTL_PSSE;
+	struct adi_spi_master *drv_data = spi_master_get_devdata(spi->master);
+	struct adi_spi_device *chip = spi_get_ctldata(spi);
+	u32 ctl_reg = SPI_CTL_ODM | SPI_CTL_PSSE;
 	int ret = -EINVAL;
 
 	if (!chip) {
-		struct bfin_spi3_chip *chip_info = spi->controller_data;
+		struct adi_spi3_chip *chip_info = spi->controller_data;
 
 		chip = kzalloc(sizeof(*chip), GFP_KERNEL);
 		if (!chip) {
@@ -646,7 +665,7 @@ static int bfin_spi_setup(struct spi_device *spi)
 			return -ENOMEM;
 		}
 		if (chip_info) {
-			if (chip_info->control & ~bfin_ctl_reg) {
+			if (chip_info->control & ~ctl_reg) {
 				dev_err(&spi->dev,
 					"do not set bits that the SPI framework manages\n");
 				goto error;
@@ -657,6 +676,7 @@ static int bfin_spi_setup(struct spi_device *spi)
 			chip->enable_dma = chip_info->enable_dma;
 		}
 		chip->cs = spi->chip_select;
+
 		if (chip->cs < MAX_CTRL_CS) {
 			chip->ssel = (1 << chip->cs) << 8;
 			ret = peripheral_request(ssel[spi->master->bus_num]
@@ -678,7 +698,7 @@ static int bfin_spi_setup(struct spi_device *spi)
 	}
 
 	/* force a default base state */
-	chip->control &= bfin_ctl_reg;
+	chip->control &= ctl_reg;
 
 	if (spi->mode & SPI_CPOL)
 		chip->control |= SPI_CTL_CPOL;
@@ -692,8 +712,8 @@ static int bfin_spi_setup(struct spi_device *spi)
 
 	chip->clock = hz_to_spi_clock(drv_data->sclk, spi->max_speed_hz);
 
-	bfin_spi_cs_enable(drv_data, chip);
-	bfin_spi_cs_deactive(drv_data, chip);
+	adi_spi_cs_enable(drv_data, chip);
+	adi_spi_cs_deactive(drv_data, chip);
 
 	return 0;
 error:
@@ -705,10 +725,10 @@ error:
 	return ret;
 }
 
-static void bfin_spi_cleanup(struct spi_device *spi)
+static void adi_spi_cleanup(struct spi_device *spi)
 {
-	struct bfin_spi_device *chip = spi_get_ctldata(spi);
-	struct bfin_spi_master *drv_data = spi_master_get_devdata(spi->master);
+	struct adi_spi_device *chip = spi_get_ctldata(spi);
+	struct adi_spi_master *drv_data = spi_master_get_devdata(spi->master);
 
 	if (!chip)
 		return;
@@ -716,7 +736,7 @@ static void bfin_spi_cleanup(struct spi_device *spi)
 	if (chip->cs < MAX_CTRL_CS) {
 		peripheral_free(ssel[spi->master->bus_num]
 					[chip->cs-1]);
-		bfin_spi_cs_disable(drv_data, chip);
+		adi_spi_cs_disable(drv_data, chip);
 	} else {
 		gpio_free(chip->cs_gpio);
 	}
@@ -725,10 +745,11 @@ static void bfin_spi_cleanup(struct spi_device *spi)
 	spi_set_ctldata(spi, NULL);
 }
 
-static irqreturn_t bfin_spi_tx_dma_isr(int irq, void *dev_id)
+static irqreturn_t adi_spi_tx_dma_isr(int irq, void *dev_id)
 {
-	struct bfin_spi_master *drv_data = dev_id;
+	struct adi_spi_master *drv_data = dev_id;
 	u32 dma_stat = get_dma_curr_irqstat(drv_data->tx_dma);
+	u32 tx_ctl;
 
 	clear_dma_irqstat(drv_data->tx_dma);
 	if (dma_stat & DMA_DONE) {
@@ -739,13 +760,15 @@ static irqreturn_t bfin_spi_tx_dma_isr(int irq, void *dev_id)
 		if (drv_data->tx)
 			drv_data->state = ERROR_STATE;
 	}
-	bfin_write_and(&drv_data->regs->tx_control, ~SPI_TXCTL_TDR_NF);
+	tx_ctl = ioread32(&drv_data->regs->tx_control);
+	tx_ctl &= ~SPI_TXCTL_TDR_NF;
+	iowrite32(tx_ctl, &drv_data->regs->tx_control);
 	return IRQ_HANDLED;
 }
 
-static irqreturn_t bfin_spi_rx_dma_isr(int irq, void *dev_id)
+static irqreturn_t adi_spi_rx_dma_isr(int irq, void *dev_id)
 {
-	struct bfin_spi_master *drv_data = dev_id;
+	struct adi_spi_master *drv_data = dev_id;
 	struct spi_message *msg = drv_data->cur_msg;
 	u32 dma_stat = get_dma_curr_irqstat(drv_data->rx_dma);
 
@@ -760,8 +783,8 @@ static irqreturn_t bfin_spi_rx_dma_isr(int irq, void *dev_id)
 		dev_err(&drv_data->master->dev,
 				"spi rx dma error: %d\n", dma_stat);
 	}
-	bfin_write(&drv_data->regs->tx_control, 0);
-	bfin_write(&drv_data->regs->rx_control, 0);
+	iowrite32(0, &drv_data->regs->tx_control);
+	iowrite32(0, &drv_data->regs->rx_control);
 	if (drv_data->rx_num != drv_data->tx_num)
 		dev_dbg(&drv_data->master->dev,
 				"dma interrupt missing: tx=%d,rx=%d\n",
@@ -770,15 +793,15 @@ static irqreturn_t bfin_spi_rx_dma_isr(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static int bfin_spi_probe(struct platform_device *pdev)
+static int adi_spi_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
-	struct bfin_spi3_master *info = dev_get_platdata(dev);
+	struct adi_spi3_master *info = dev_get_platdata(dev);
 	struct spi_master *master;
-	struct bfin_spi_master *drv_data;
+	struct adi_spi_master *drv_data;
 	struct resource *mem, *res;
 	unsigned int tx_dma, rx_dma;
-	unsigned long sclk;
+	struct clk *sclk;
 	int ret;
 
 	if (!info) {
@@ -786,10 +809,10 @@ static int bfin_spi_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	sclk = get_sclk1();
-	if (!sclk) {
-		dev_err(dev, "can not get sclk1\n");
-		return -ENXIO;
+	sclk = devm_clk_get(dev, "spi");
+	if (IS_ERR(sclk)) {
+		dev_err(dev, "can not get spi clock\n");
+		return PTR_ERR(sclk);
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_DMA, 0);
@@ -819,9 +842,9 @@ static int bfin_spi_probe(struct platform_device *pdev)
 
 	master->bus_num = pdev->id;
 	master->num_chipselect = info->num_chipselect;
-	master->cleanup = bfin_spi_cleanup;
-	master->setup = bfin_spi_setup;
-	master->transfer_one_message = bfin_spi_transfer_one_message;
+	master->cleanup = adi_spi_cleanup;
+	master->setup = adi_spi_setup;
+	master->transfer_one_message = adi_spi_transfer_one_message;
 	master->bits_per_word_mask = SPI_BPW_MASK(32) | SPI_BPW_MASK(16) |
 				     SPI_BPW_MASK(8);
 
@@ -830,7 +853,7 @@ static int bfin_spi_probe(struct platform_device *pdev)
 	drv_data->tx_dma = tx_dma;
 	drv_data->rx_dma = rx_dma;
 	drv_data->pin_req = info->pin_req;
-	drv_data->sclk = sclk;
+	drv_data->sclk = clk_get_rate(sclk);
 
 	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	drv_data->regs = devm_ioremap_resource(dev, mem);
@@ -845,28 +868,28 @@ static int bfin_spi_probe(struct platform_device *pdev)
 		dev_err(dev, "can not request SPI TX DMA channel\n");
 		goto err_put_master;
 	}
-	set_dma_callback(tx_dma, bfin_spi_tx_dma_isr, drv_data);
+	set_dma_callback(tx_dma, adi_spi_tx_dma_isr, drv_data);
 
 	ret = request_dma(rx_dma, "SPI_RX_DMA");
 	if (ret) {
 		dev_err(dev, "can not request SPI RX DMA channel\n");
 		goto err_free_tx_dma;
 	}
-	set_dma_callback(drv_data->rx_dma, bfin_spi_rx_dma_isr, drv_data);
+	set_dma_callback(drv_data->rx_dma, adi_spi_rx_dma_isr, drv_data);
 
 	/* request CLK, MOSI and MISO */
-	ret = peripheral_request_list(drv_data->pin_req, "bfin-spi3");
+	ret = peripheral_request_list(drv_data->pin_req, "adi-spi3");
 	if (ret < 0) {
 		dev_err(dev, "can not request spi pins\n");
 		goto err_free_rx_dma;
 	}
 
-	bfin_write(&drv_data->regs->control, SPI_CTL_MSTR | SPI_CTL_CPHA);
-	bfin_write(&drv_data->regs->ssel, 0x0000FE00);
-	bfin_write(&drv_data->regs->delay, 0x0);
+	iowrite32(SPI_CTL_MSTR | SPI_CTL_CPHA, &drv_data->regs->control);
+	iowrite32(0x0000FE00, &drv_data->regs->ssel);
+	iowrite32(0x0, &drv_data->regs->delay);
 
 	tasklet_init(&drv_data->pump_transfers,
-			bfin_spi_pump_transfers, (unsigned long)drv_data);
+			adi_spi_pump_transfers, (unsigned long)drv_data);
 	/* register with the SPI framework */
 	ret = devm_spi_register_master(dev, master);
 	if (ret) {
@@ -888,43 +911,41 @@ err_put_master:
 	return ret;
 }
 
-static int bfin_spi_remove(struct platform_device *pdev)
+static int adi_spi_remove(struct platform_device *pdev)
 {
 	struct spi_master *master = platform_get_drvdata(pdev);
-	struct bfin_spi_master *drv_data = spi_master_get_devdata(master);
-
-	bfin_spi_disable(drv_data);
+	struct adi_spi_master *drv_data = spi_master_get_devdata(master);
 
+	adi_spi_disable(drv_data);
 	peripheral_free_list(drv_data->pin_req);
 	free_dma(drv_data->rx_dma);
 	free_dma(drv_data->tx_dma);
-
 	return 0;
 }
 
 #ifdef CONFIG_PM
-static int bfin_spi_suspend(struct device *dev)
+static int adi_spi_suspend(struct device *dev)
 {
 	struct spi_master *master = dev_get_drvdata(dev);
-	struct bfin_spi_master *drv_data = spi_master_get_devdata(master);
+	struct adi_spi_master *drv_data = spi_master_get_devdata(master);
 
 	spi_master_suspend(master);
 
-	drv_data->control = bfin_read(&drv_data->regs->control);
-	drv_data->ssel = bfin_read(&drv_data->regs->ssel);
+	drv_data->control = ioread32(&drv_data->regs->control);
+	drv_data->ssel = ioread32(&drv_data->regs->ssel);
 
-	bfin_write(&drv_data->regs->control, SPI_CTL_MSTR | SPI_CTL_CPHA);
-	bfin_write(&drv_data->regs->ssel, 0x0000FE00);
+	iowrite32(SPI_CTL_MSTR | SPI_CTL_CPHA, &drv_data->regs->control);
+	iowrite32(0x0000FE00, &drv_data->regs->ssel);
 	dma_disable_irq(drv_data->rx_dma);
 	dma_disable_irq(drv_data->tx_dma);
 
 	return 0;
 }
 
-static int bfin_spi_resume(struct device *dev)
+static int adi_spi_resume(struct device *dev)
 {
 	struct spi_master *master = dev_get_drvdata(dev);
-	struct bfin_spi_master *drv_data = spi_master_get_devdata(master);
+	struct adi_spi_master *drv_data = spi_master_get_devdata(master);
 	int ret = 0;
 
 	/* bootrom may modify spi and dma status when resume in spi boot mode */
@@ -932,8 +953,8 @@ static int bfin_spi_resume(struct device *dev)
 
 	dma_enable_irq(drv_data->rx_dma);
 	dma_enable_irq(drv_data->tx_dma);
-	bfin_write(&drv_data->regs->control, drv_data->control);
-	bfin_write(&drv_data->regs->ssel, drv_data->ssel);
+	iowrite32(drv_data->control, &drv_data->regs->control);
+	iowrite32(drv_data->ssel, &drv_data->regs->ssel);
 
 	ret = spi_master_resume(master);
 	if (ret) {
@@ -944,21 +965,21 @@ static int bfin_spi_resume(struct device *dev)
 	return ret;
 }
 #endif
-static const struct dev_pm_ops bfin_spi_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(bfin_spi_suspend, bfin_spi_resume)
+static const struct dev_pm_ops adi_spi_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(adi_spi_suspend, adi_spi_resume)
 };
 
-MODULE_ALIAS("platform:bfin-spi3");
-static struct platform_driver bfin_spi_driver = {
+MODULE_ALIAS("platform:adi-spi3");
+static struct platform_driver adi_spi_driver = {
 	.driver	= {
-		.name	= "bfin-spi3",
+		.name	= "adi-spi3",
 		.owner	= THIS_MODULE,
-		.pm     = &bfin_spi_pm_ops,
+		.pm     = &adi_spi_pm_ops,
 	},
-	.remove		= bfin_spi_remove,
+	.remove		= adi_spi_remove,
 };
 
-module_platform_driver_probe(bfin_spi_driver, bfin_spi_probe);
+module_platform_driver_probe(adi_spi_driver, adi_spi_probe);
 
 MODULE_DESCRIPTION("Analog Devices SPI3 controller driver");
 MODULE_AUTHOR("Scott Jiang <Scott.Jiang.Linux@gmail.com>");
diff --git a/drivers/spi/spi-ath79.c b/drivers/spi/spi-ath79.c
index 3898b0b9ee7..058db0fe8dc 100644
--- a/drivers/spi/spi-ath79.c
+++ b/drivers/spi/spi-ath79.c
@@ -16,7 +16,6 @@
 #include <linux/module.h>
 #include <linux/delay.h>
 #include <linux/spinlock.h>
-#include <linux/workqueue.h>
 #include <linux/platform_device.h>
 #include <linux/io.h>
 #include <linux/spi/spi.h>
diff --git a/drivers/spi/spi-atmel.c b/drivers/spi/spi-atmel.c
index 079e6b1b0cd..92a6f0d9323 100644
--- a/drivers/spi/spi-atmel.c
+++ b/drivers/spi/spi-atmel.c
@@ -224,7 +224,7 @@ struct atmel_spi {
 	struct platform_device	*pdev;
 
 	struct spi_transfer	*current_transfer;
-	unsigned long		current_remaining_bytes;
+	int			current_remaining_bytes;
 	int			done_status;
 
 	struct completion	xfer_completion;
@@ -874,8 +874,9 @@ atmel_spi_pump_pio_data(struct atmel_spi *as, struct spi_transfer *xfer)
 		spi_readl(as, RDR);
 	}
 	if (xfer->bits_per_word > 8) {
-		as->current_remaining_bytes -= 2;
-		if (as->current_remaining_bytes < 0)
+		if (as->current_remaining_bytes > 2)
+			as->current_remaining_bytes -= 2;
+		else
 			as->current_remaining_bytes = 0;
 	} else {
 		as->current_remaining_bytes--;
@@ -1110,6 +1111,8 @@ static int atmel_spi_one_transfer(struct spi_master *master,
 				atmel_spi_next_xfer_pio(master, xfer);
 			} else {
 				as->current_remaining_bytes -= len;
+				if (as->current_remaining_bytes < 0)
+					as->current_remaining_bytes = 0;
 			}
 		} else {
 			atmel_spi_next_xfer_pio(master, xfer);
diff --git a/drivers/spi/spi-bcm63xx-hsspi.c b/drivers/spi/spi-bcm63xx-hsspi.c
index 5a211e98383..86f5a98aa7a 100644
--- a/drivers/spi/spi-bcm63xx-hsspi.c
+++ b/drivers/spi/spi-bcm63xx-hsspi.c
@@ -18,7 +18,6 @@
 #include <linux/err.h>
 #include <linux/interrupt.h>
 #include <linux/spi/spi.h>
-#include <linux/workqueue.h>
 #include <linux/mutex.h>
 
 #define HSSPI_GLOBAL_CTRL_REG			0x0
diff --git a/drivers/spi/spi-bcm63xx.c b/drivers/spi/spi-bcm63xx.c
index 0250fa721ce..8510400e786 100644
--- a/drivers/spi/spi-bcm63xx.c
+++ b/drivers/spi/spi-bcm63xx.c
@@ -29,7 +29,6 @@
 #include <linux/spi/spi.h>
 #include <linux/completion.h>
 #include <linux/err.h>
-#include <linux/workqueue.h>
 #include <linux/pm_runtime.h>
 
 #include <bcm63xx_dev_spi.h>
diff --git a/drivers/spi/spi-cadence.c b/drivers/spi/spi-cadence.c
new file mode 100644
index 00000000000..bb758978465
--- /dev/null
+++ b/drivers/spi/spi-cadence.c
@@ -0,0 +1,673 @@
+/*
+ * Cadence SPI controller driver (master mode only)
+ *
+ * Copyright (C) 2008 - 2014 Xilinx, Inc.
+ *
+ * based on Blackfin On-Chip SPI Driver (spi_bfin5xx.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; either version 2 of the License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of_irq.h>
+#include <linux/of_address.h>
+#include <linux/platform_device.h>
+#include <linux/spi/spi.h>
+
+/* Name of this driver */
+#define CDNS_SPI_NAME		"cdns-spi"
+
+/* Register offset definitions */
+#define CDNS_SPI_CR_OFFSET	0x00 /* Configuration  Register, RW */
+#define CDNS_SPI_ISR_OFFSET	0x04 /* Interrupt Status Register, RO */
+#define CDNS_SPI_IER_OFFSET	0x08 /* Interrupt Enable Register, WO */
+#define CDNS_SPI_IDR_OFFSET	0x0c /* Interrupt Disable Register, WO */
+#define CDNS_SPI_IMR_OFFSET	0x10 /* Interrupt Enabled Mask Register, RO */
+#define CDNS_SPI_ER_OFFSET	0x14 /* Enable/Disable Register, RW */
+#define CDNS_SPI_DR_OFFSET	0x18 /* Delay Register, RW */
+#define CDNS_SPI_TXD_OFFSET	0x1C /* Data Transmit Register, WO */
+#define CDNS_SPI_RXD_OFFSET	0x20 /* Data Receive Register, RO */
+#define CDNS_SPI_SICR_OFFSET	0x24 /* Slave Idle Count Register, RW */
+#define CDNS_SPI_THLD_OFFSET	0x28 /* Transmit FIFO Watermark Register,RW */
+
+/*
+ * SPI Configuration Register bit Masks
+ *
+ * This register contains various control bits that affect the operation
+ * of the SPI controller
+ */
+#define CDNS_SPI_CR_MANSTRT_MASK	0x00010000 /* Manual TX Start */
+#define CDNS_SPI_CR_CPHA_MASK		0x00000004 /* Clock Phase Control */
+#define CDNS_SPI_CR_CPOL_MASK		0x00000002 /* Clock Polarity Control */
+#define CDNS_SPI_CR_SSCTRL_MASK		0x00003C00 /* Slave Select Mask */
+#define CDNS_SPI_CR_BAUD_DIV_MASK	0x00000038 /* Baud Rate Divisor Mask */
+#define CDNS_SPI_CR_MSTREN_MASK		0x00000001 /* Master Enable Mask */
+#define CDNS_SPI_CR_MANSTRTEN_MASK	0x00008000 /* Manual TX Enable Mask */
+#define CDNS_SPI_CR_SSFORCE_MASK	0x00004000 /* Manual SS Enable Mask */
+#define CDNS_SPI_CR_BAUD_DIV_4_MASK	0x00000008 /* Default Baud Div Mask */
+#define CDNS_SPI_CR_DEFAULT_MASK	(CDNS_SPI_CR_MSTREN_MASK | \
+					CDNS_SPI_CR_SSCTRL_MASK | \
+					CDNS_SPI_CR_SSFORCE_MASK | \
+					CDNS_SPI_CR_BAUD_DIV_4_MASK)
+
+/*
+ * SPI Configuration Register - Baud rate and slave select
+ *
+ * These are the values used in the calculation of baud rate divisor and
+ * setting the slave select.
+ */
+
+#define CDNS_SPI_BAUD_DIV_MAX		7 /* Baud rate divisor maximum */
+#define CDNS_SPI_BAUD_DIV_MIN		1 /* Baud rate divisor minimum */
+#define CDNS_SPI_BAUD_DIV_SHIFT		3 /* Baud rate divisor shift in CR */
+#define CDNS_SPI_SS_SHIFT		10 /* Slave Select field shift in CR */
+#define CDNS_SPI_SS0			0x1 /* Slave Select zero */
+
+/*
+ * SPI Interrupt Registers bit Masks
+ *
+ * All the four interrupt registers (Status/Mask/Enable/Disable) have the same
+ * bit definitions.
+ */
+#define CDNS_SPI_IXR_TXOW_MASK	0x00000004 /* SPI TX FIFO Overwater */
+#define CDNS_SPI_IXR_MODF_MASK	0x00000002 /* SPI Mode Fault */
+#define CDNS_SPI_IXR_RXNEMTY_MASK 0x00000010 /* SPI RX FIFO Not Empty */
+#define CDNS_SPI_IXR_DEFAULT_MASK	(CDNS_SPI_IXR_TXOW_MASK | \
+					CDNS_SPI_IXR_MODF_MASK)
+#define CDNS_SPI_IXR_TXFULL_MASK	0x00000008 /* SPI TX Full */
+#define CDNS_SPI_IXR_ALL_MASK	0x0000007F /* SPI all interrupts */
+
+/*
+ * SPI Enable Register bit Masks
+ *
+ * This register is used to enable or disable the SPI controller
+ */
+#define CDNS_SPI_ER_ENABLE_MASK	0x00000001 /* SPI Enable Bit Mask */
+#define CDNS_SPI_ER_DISABLE_MASK	0x0 /* SPI Disable Bit Mask */
+
+/* SPI FIFO depth in bytes */
+#define CDNS_SPI_FIFO_DEPTH	128
+
+/* Default number of chip select lines */
+#define CDNS_SPI_DEFAULT_NUM_CS		4
+
+/**
+ * struct cdns_spi - This definition defines spi driver instance
+ * @regs:		Virtual address of the SPI controller registers
+ * @ref_clk:		Pointer to the peripheral clock
+ * @pclk:		Pointer to the APB clock
+ * @speed_hz:		Current SPI bus clock speed in Hz
+ * @txbuf:		Pointer	to the TX buffer
+ * @rxbuf:		Pointer to the RX buffer
+ * @tx_bytes:		Number of bytes left to transfer
+ * @rx_bytes:		Number of bytes requested
+ * @dev_busy:		Device busy flag
+ * @is_decoded_cs:	Flag for decoder property set or not
+ */
+struct cdns_spi {
+	void __iomem *regs;
+	struct clk *ref_clk;
+	struct clk *pclk;
+	u32 speed_hz;
+	const u8 *txbuf;
+	u8 *rxbuf;
+	int tx_bytes;
+	int rx_bytes;
+	u8 dev_busy;
+	u32 is_decoded_cs;
+};
+
+/* Macros for the SPI controller read/write */
+static inline u32 cdns_spi_read(struct cdns_spi *xspi, u32 offset)
+{
+	return readl_relaxed(xspi->regs + offset);
+}
+
+static inline void cdns_spi_write(struct cdns_spi *xspi, u32 offset, u32 val)
+{
+	writel_relaxed(val, xspi->regs + offset);
+}
+
+/**
+ * cdns_spi_init_hw - Initialize the hardware and configure the SPI controller
+ * @xspi:	Pointer to the cdns_spi structure
+ *
+ * On reset the SPI controller is configured to be in master mode, baud rate
+ * divisor is set to 4, threshold value for TX FIFO not full interrupt is set
+ * to 1 and size of the word to be transferred as 8 bit.
+ * This function initializes the SPI controller to disable and clear all the
+ * interrupts, enable manual slave select and manual start, deselect all the
+ * chip select lines, and enable the SPI controller.
+ */
+static void cdns_spi_init_hw(struct cdns_spi *xspi)
+{
+	cdns_spi_write(xspi, CDNS_SPI_ER_OFFSET,
+		       CDNS_SPI_ER_DISABLE_MASK);
+	cdns_spi_write(xspi, CDNS_SPI_IDR_OFFSET,
+		       CDNS_SPI_IXR_ALL_MASK);
+
+	/* Clear the RX FIFO */
+	while (cdns_spi_read(xspi, CDNS_SPI_ISR_OFFSET) &
+	       CDNS_SPI_IXR_RXNEMTY_MASK)
+		cdns_spi_read(xspi, CDNS_SPI_RXD_OFFSET);
+
+	cdns_spi_write(xspi, CDNS_SPI_ISR_OFFSET,
+		       CDNS_SPI_IXR_ALL_MASK);
+	cdns_spi_write(xspi, CDNS_SPI_CR_OFFSET,
+		       CDNS_SPI_CR_DEFAULT_MASK);
+	cdns_spi_write(xspi, CDNS_SPI_ER_OFFSET,
+		       CDNS_SPI_ER_ENABLE_MASK);
+}
+
+/**
+ * cdns_spi_chipselect - Select or deselect the chip select line
+ * @spi:	Pointer to the spi_device structure
+ * @is_on:	Select(0) or deselect (1) the chip select line
+ */
+static void cdns_spi_chipselect(struct spi_device *spi, bool is_high)
+{
+	struct cdns_spi *xspi = spi_master_get_devdata(spi->master);
+	u32 ctrl_reg;
+
+	ctrl_reg = cdns_spi_read(xspi, CDNS_SPI_CR_OFFSET);
+
+	if (is_high) {
+		/* Deselect the slave */
+		ctrl_reg |= CDNS_SPI_CR_SSCTRL_MASK;
+	} else {
+		/* Select the slave */
+		ctrl_reg &= ~CDNS_SPI_CR_SSCTRL_MASK;
+		if (!(xspi->is_decoded_cs))
+			ctrl_reg |= ((~(CDNS_SPI_SS0 << spi->chip_select)) <<
+				     CDNS_SPI_SS_SHIFT) &
+				     CDNS_SPI_CR_SSCTRL_MASK;
+		else
+			ctrl_reg |= (spi->chip_select << CDNS_SPI_SS_SHIFT) &
+				     CDNS_SPI_CR_SSCTRL_MASK;
+	}
+
+	cdns_spi_write(xspi, CDNS_SPI_CR_OFFSET, ctrl_reg);
+}
+
+/**
+ * cdns_spi_config_clock_mode - Sets clock polarity and phase
+ * @spi:	Pointer to the spi_device structure
+ *
+ * Sets the requested clock polarity and phase.
+ */
+static void cdns_spi_config_clock_mode(struct spi_device *spi)
+{
+	struct cdns_spi *xspi = spi_master_get_devdata(spi->master);
+	u32 ctrl_reg;
+
+	ctrl_reg = cdns_spi_read(xspi, CDNS_SPI_CR_OFFSET);
+
+	/* Set the SPI clock phase and clock polarity */
+	ctrl_reg &= ~(CDNS_SPI_CR_CPHA_MASK | CDNS_SPI_CR_CPOL_MASK);
+	if (spi->mode & SPI_CPHA)
+		ctrl_reg |= CDNS_SPI_CR_CPHA_MASK;
+	if (spi->mode & SPI_CPOL)
+		ctrl_reg |= CDNS_SPI_CR_CPOL_MASK;
+
+	cdns_spi_write(xspi, CDNS_SPI_CR_OFFSET, ctrl_reg);
+}
+
+/**
+ * cdns_spi_config_clock_freq - Sets clock frequency
+ * @spi:	Pointer to the spi_device structure
+ * @transfer:	Pointer to the spi_transfer structure which provides
+ *		information about next transfer setup parameters
+ *
+ * Sets the requested clock frequency.
+ * Note: If the requested frequency is not an exact match with what can be
+ * obtained using the prescalar value the driver sets the clock frequency which
+ * is lower than the requested frequency (maximum lower) for the transfer. If
+ * the requested frequency is higher or lower than that is supported by the SPI
+ * controller the driver will set the highest or lowest frequency supported by
+ * controller.
+ */
+static void cdns_spi_config_clock_freq(struct spi_device *spi,
+				  struct spi_transfer *transfer)
+{
+	struct cdns_spi *xspi = spi_master_get_devdata(spi->master);
+	u32 ctrl_reg, baud_rate_val;
+	unsigned long frequency;
+
+	frequency = clk_get_rate(xspi->ref_clk);
+
+	ctrl_reg = cdns_spi_read(xspi, CDNS_SPI_CR_OFFSET);
+
+	/* Set the clock frequency */
+	if (xspi->speed_hz != transfer->speed_hz) {
+		/* first valid value is 1 */
+		baud_rate_val = CDNS_SPI_BAUD_DIV_MIN;
+		while ((baud_rate_val < CDNS_SPI_BAUD_DIV_MAX) &&
+		       (frequency / (2 << baud_rate_val)) > transfer->speed_hz)
+			baud_rate_val++;
+
+		ctrl_reg &= ~CDNS_SPI_CR_BAUD_DIV_MASK;
+		ctrl_reg |= baud_rate_val << CDNS_SPI_BAUD_DIV_SHIFT;
+
+		xspi->speed_hz = frequency / (2 << baud_rate_val);
+	}
+	cdns_spi_write(xspi, CDNS_SPI_CR_OFFSET, ctrl_reg);
+}
+
+/**
+ * cdns_spi_setup_transfer - Configure SPI controller for specified transfer
+ * @spi:	Pointer to the spi_device structure
+ * @transfer:	Pointer to the spi_transfer structure which provides
+ *		information about next transfer setup parameters
+ *
+ * Sets the operational mode of SPI controller for the next SPI transfer and
+ * sets the requested clock frequency.
+ *
+ * Return:	Always 0
+ */
+static int cdns_spi_setup_transfer(struct spi_device *spi,
+				   struct spi_transfer *transfer)
+{
+	struct cdns_spi *xspi = spi_master_get_devdata(spi->master);
+
+	cdns_spi_config_clock_freq(spi, transfer);
+
+	dev_dbg(&spi->dev, "%s, mode %d, %u bits/w, %u clock speed\n",
+		__func__, spi->mode, spi->bits_per_word,
+		xspi->speed_hz);
+
+	return 0;
+}
+
+/**
+ * cdns_spi_fill_tx_fifo - Fills the TX FIFO with as many bytes as possible
+ * @xspi:	Pointer to the cdns_spi structure
+ */
+static void cdns_spi_fill_tx_fifo(struct cdns_spi *xspi)
+{
+	unsigned long trans_cnt = 0;
+
+	while ((trans_cnt < CDNS_SPI_FIFO_DEPTH) &&
+	       (xspi->tx_bytes > 0)) {
+		if (xspi->txbuf)
+			cdns_spi_write(xspi, CDNS_SPI_TXD_OFFSET,
+				       *xspi->txbuf++);
+		else
+			cdns_spi_write(xspi, CDNS_SPI_TXD_OFFSET, 0);
+
+		xspi->tx_bytes--;
+		trans_cnt++;
+	}
+}
+
+/**
+ * cdns_spi_irq - Interrupt service routine of the SPI controller
+ * @irq:	IRQ number
+ * @dev_id:	Pointer to the xspi structure
+ *
+ * This function handles TX empty and Mode Fault interrupts only.
+ * On TX empty interrupt this function reads the received data from RX FIFO and
+ * fills the TX FIFO if there is any data remaining to be transferred.
+ * On Mode Fault interrupt this function indicates that transfer is completed,
+ * the SPI subsystem will identify the error as the remaining bytes to be
+ * transferred is non-zero.
+ *
+ * Return:	IRQ_HANDLED when handled; IRQ_NONE otherwise.
+ */
+static irqreturn_t cdns_spi_irq(int irq, void *dev_id)
+{
+	struct spi_master *master = dev_id;
+	struct cdns_spi *xspi = spi_master_get_devdata(master);
+	u32 intr_status, status;
+
+	status = IRQ_NONE;
+	intr_status = cdns_spi_read(xspi, CDNS_SPI_ISR_OFFSET);
+	cdns_spi_write(xspi, CDNS_SPI_ISR_OFFSET, intr_status);
+
+	if (intr_status & CDNS_SPI_IXR_MODF_MASK) {
+		/* Indicate that transfer is completed, the SPI subsystem will
+		 * identify the error as the remaining bytes to be
+		 * transferred is non-zero
+		 */
+		cdns_spi_write(xspi, CDNS_SPI_IDR_OFFSET,
+			       CDNS_SPI_IXR_DEFAULT_MASK);
+		spi_finalize_current_transfer(master);
+		status = IRQ_HANDLED;
+	} else if (intr_status & CDNS_SPI_IXR_TXOW_MASK) {
+		unsigned long trans_cnt;
+
+		trans_cnt = xspi->rx_bytes - xspi->tx_bytes;
+
+		/* Read out the data from the RX FIFO */
+		while (trans_cnt) {
+			u8 data;
+
+			data = cdns_spi_read(xspi, CDNS_SPI_RXD_OFFSET);
+			if (xspi->rxbuf)
+				*xspi->rxbuf++ = data;
+
+			xspi->rx_bytes--;
+			trans_cnt--;
+		}
+
+		if (xspi->tx_bytes) {
+			/* There is more data to send */
+			cdns_spi_fill_tx_fifo(xspi);
+		} else {
+			/* Transfer is completed */
+			cdns_spi_write(xspi, CDNS_SPI_IDR_OFFSET,
+				       CDNS_SPI_IXR_DEFAULT_MASK);
+			spi_finalize_current_transfer(master);
+		}
+		status = IRQ_HANDLED;
+	}
+
+	return status;
+}
+
+/**
+ * cdns_transfer_one - Initiates the SPI transfer
+ * @master:	Pointer to spi_master structure
+ * @spi:	Pointer to the spi_device structure
+ * @transfer:	Pointer to the spi_transfer structure which provides
+ *		information about next transfer parameters
+ *
+ * This function fills the TX FIFO, starts the SPI transfer and
+ * returns a positive transfer count so that core will wait for completion.
+ *
+ * Return:	Number of bytes transferred in the last transfer
+ */
+static int cdns_transfer_one(struct spi_master *master,
+			     struct spi_device *spi,
+			     struct spi_transfer *transfer)
+{
+	struct cdns_spi *xspi = spi_master_get_devdata(master);
+
+	xspi->txbuf = transfer->tx_buf;
+	xspi->rxbuf = transfer->rx_buf;
+	xspi->tx_bytes = transfer->len;
+	xspi->rx_bytes = transfer->len;
+
+	cdns_spi_setup_transfer(spi, transfer);
+
+	cdns_spi_fill_tx_fifo(xspi);
+
+	cdns_spi_write(xspi, CDNS_SPI_IER_OFFSET,
+		       CDNS_SPI_IXR_DEFAULT_MASK);
+	return transfer->len;
+}
+
+/**
+ * cdns_prepare_transfer_hardware - Prepares hardware for transfer.
+ * @master:	Pointer to the spi_master structure which provides
+ *		information about the controller.
+ *
+ * This function enables SPI master controller.
+ *
+ * Return:	0 always
+ */
+static int cdns_prepare_transfer_hardware(struct spi_master *master)
+{
+	struct cdns_spi *xspi = spi_master_get_devdata(master);
+
+	cdns_spi_config_clock_mode(master->cur_msg->spi);
+
+	cdns_spi_write(xspi, CDNS_SPI_ER_OFFSET,
+		       CDNS_SPI_ER_ENABLE_MASK);
+
+	return 0;
+}
+
+/**
+ * cdns_unprepare_transfer_hardware - Relaxes hardware after transfer
+ * @master:	Pointer to the spi_master structure which provides
+ *		information about the controller.
+ *
+ * This function disables the SPI master controller.
+ *
+ * Return:	0 always
+ */
+static int cdns_unprepare_transfer_hardware(struct spi_master *master)
+{
+	struct cdns_spi *xspi = spi_master_get_devdata(master);
+
+	cdns_spi_write(xspi, CDNS_SPI_ER_OFFSET,
+		       CDNS_SPI_ER_DISABLE_MASK);
+
+	return 0;
+}
+
+/**
+ * cdns_spi_probe - Probe method for the SPI driver
+ * @pdev:	Pointer to the platform_device structure
+ *
+ * This function initializes the driver data structures and the hardware.
+ *
+ * Return:	0 on success and error value on error
+ */
+static int cdns_spi_probe(struct platform_device *pdev)
+{
+	int ret = 0, irq;
+	struct spi_master *master;
+	struct cdns_spi *xspi;
+	struct resource *res;
+	u32 num_cs;
+
+	master = spi_alloc_master(&pdev->dev, sizeof(*xspi));
+	if (master == NULL)
+		return -ENOMEM;
+
+	xspi = spi_master_get_devdata(master);
+	master->dev.of_node = pdev->dev.of_node;
+	platform_set_drvdata(pdev, master);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	xspi->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(xspi->regs)) {
+		ret = PTR_ERR(xspi->regs);
+		goto remove_master;
+	}
+
+	xspi->pclk = devm_clk_get(&pdev->dev, "pclk");
+	if (IS_ERR(xspi->pclk)) {
+		dev_err(&pdev->dev, "pclk clock not found.\n");
+		ret = PTR_ERR(xspi->pclk);
+		goto remove_master;
+	}
+
+	xspi->ref_clk = devm_clk_get(&pdev->dev, "ref_clk");
+	if (IS_ERR(xspi->ref_clk)) {
+		dev_err(&pdev->dev, "ref_clk clock not found.\n");
+		ret = PTR_ERR(xspi->ref_clk);
+		goto remove_master;
+	}
+
+	ret = clk_prepare_enable(xspi->pclk);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to enable APB clock.\n");
+		goto remove_master;
+	}
+
+	ret = clk_prepare_enable(xspi->ref_clk);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to enable device clock.\n");
+		goto clk_dis_apb;
+	}
+
+	/* SPI controller initializations */
+	cdns_spi_init_hw(xspi);
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq <= 0) {
+		ret = -ENXIO;
+		dev_err(&pdev->dev, "irq number is invalid\n");
+		goto remove_master;
+	}
+
+	ret = devm_request_irq(&pdev->dev, irq, cdns_spi_irq,
+			       0, pdev->name, master);
+	if (ret != 0) {
+		ret = -ENXIO;
+		dev_err(&pdev->dev, "request_irq failed\n");
+		goto remove_master;
+	}
+
+	ret = of_property_read_u32(pdev->dev.of_node, "num-cs", &num_cs);
+
+	if (ret < 0)
+		master->num_chipselect = CDNS_SPI_DEFAULT_NUM_CS;
+	else
+		master->num_chipselect = num_cs;
+
+	ret = of_property_read_u32(pdev->dev.of_node, "is-decoded-cs",
+				   &xspi->is_decoded_cs);
+
+	if (ret < 0)
+		xspi->is_decoded_cs = 0;
+
+	master->prepare_transfer_hardware = cdns_prepare_transfer_hardware;
+	master->transfer_one = cdns_transfer_one;
+	master->unprepare_transfer_hardware = cdns_unprepare_transfer_hardware;
+	master->set_cs = cdns_spi_chipselect;
+	master->mode_bits = SPI_CPOL | SPI_CPHA;
+
+	/* Set to default valid value */
+	master->max_speed_hz = clk_get_rate(xspi->ref_clk) / 4;
+	xspi->speed_hz = master->max_speed_hz;
+
+	master->bits_per_word_mask = SPI_BPW_MASK(8);
+
+	ret = spi_register_master(master);
+	if (ret) {
+		dev_err(&pdev->dev, "spi_register_master failed\n");
+		goto clk_dis_all;
+	}
+
+	return ret;
+
+clk_dis_all:
+	clk_disable_unprepare(xspi->ref_clk);
+clk_dis_apb:
+	clk_disable_unprepare(xspi->pclk);
+remove_master:
+	spi_master_put(master);
+	return ret;
+}
+
+/**
+ * cdns_spi_remove - Remove method for the SPI driver
+ * @pdev:	Pointer to the platform_device structure
+ *
+ * This function is called if a device is physically removed from the system or
+ * if the driver module is being unloaded. It frees all resources allocated to
+ * the device.
+ *
+ * Return:	0 on success and error value on error
+ */
+static int cdns_spi_remove(struct platform_device *pdev)
+{
+	struct spi_master *master = platform_get_drvdata(pdev);
+	struct cdns_spi *xspi = spi_master_get_devdata(master);
+
+	cdns_spi_write(xspi, CDNS_SPI_ER_OFFSET,
+		       CDNS_SPI_ER_DISABLE_MASK);
+
+	clk_disable_unprepare(xspi->ref_clk);
+	clk_disable_unprepare(xspi->pclk);
+
+	spi_unregister_master(master);
+
+	return 0;
+}
+
+/**
+ * cdns_spi_suspend - Suspend method for the SPI driver
+ * @dev:	Address of the platform_device structure
+ *
+ * This function disables the SPI controller and
+ * changes the driver state to "suspend"
+ *
+ * Return:	Always 0
+ */
+static int __maybe_unused cdns_spi_suspend(struct device *dev)
+{
+	struct platform_device *pdev = container_of(dev,
+			struct platform_device, dev);
+	struct spi_master *master = platform_get_drvdata(pdev);
+	struct cdns_spi *xspi = spi_master_get_devdata(master);
+
+	spi_master_suspend(master);
+
+	clk_disable_unprepare(xspi->ref_clk);
+
+	clk_disable_unprepare(xspi->pclk);
+
+	return 0;
+}
+
+/**
+ * cdns_spi_resume - Resume method for the SPI driver
+ * @dev:	Address of the platform_device structure
+ *
+ * This function changes the driver state to "ready"
+ *
+ * Return:	0 on success and error value on error
+ */
+static int __maybe_unused cdns_spi_resume(struct device *dev)
+{
+	struct platform_device *pdev = container_of(dev,
+			struct platform_device, dev);
+	struct spi_master *master = platform_get_drvdata(pdev);
+	struct cdns_spi *xspi = spi_master_get_devdata(master);
+	int ret = 0;
+
+	ret = clk_prepare_enable(xspi->pclk);
+	if (ret) {
+		dev_err(dev, "Cannot enable APB clock.\n");
+		return ret;
+	}
+
+	ret = clk_prepare_enable(xspi->ref_clk);
+	if (ret) {
+		dev_err(dev, "Cannot enable device clock.\n");
+		clk_disable(xspi->pclk);
+		return ret;
+	}
+	spi_master_resume(master);
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(cdns_spi_dev_pm_ops, cdns_spi_suspend,
+			 cdns_spi_resume);
+
+static struct of_device_id cdns_spi_of_match[] = {
+	{ .compatible = "xlnx,zynq-spi-r1p6" },
+	{ .compatible = "cdns,spi-r1p6" },
+	{ /* end of table */ }
+};
+MODULE_DEVICE_TABLE(of, cdns_spi_of_match);
+
+/* cdns_spi_driver - This structure defines the SPI subsystem platform driver */
+static struct platform_driver cdns_spi_driver = {
+	.probe	= cdns_spi_probe,
+	.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,
+	},
+};
+
+module_platform_driver(cdns_spi_driver);
+
+MODULE_AUTHOR("Xilinx, Inc.");
+MODULE_DESCRIPTION("Cadence SPI driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/spi/spi-dw-mmio.c b/drivers/spi/spi-dw-mmio.c
index 1492f5ee9aa..a5cba14ac3d 100644
--- a/drivers/spi/spi-dw-mmio.c
+++ b/drivers/spi/spi-dw-mmio.c
@@ -16,6 +16,7 @@
 #include <linux/spi/spi.h>
 #include <linux/scatterlist.h>
 #include <linux/module.h>
+#include <linux/of_gpio.h>
 
 #include "spi-dw.h"
 
@@ -70,6 +71,27 @@ static int dw_spi_mmio_probe(struct platform_device *pdev)
 	dws->num_cs = 4;
 	dws->max_freq = clk_get_rate(dwsmmio->clk);
 
+	if (pdev->dev.of_node) {
+		int i;
+
+		for (i = 0; i < dws->num_cs; i++) {
+			int cs_gpio = of_get_named_gpio(pdev->dev.of_node,
+					"cs-gpios", i);
+
+			if (cs_gpio == -EPROBE_DEFER) {
+				ret = cs_gpio;
+				goto out;
+			}
+
+			if (gpio_is_valid(cs_gpio)) {
+				ret = devm_gpio_request(&pdev->dev, cs_gpio,
+						dev_name(&pdev->dev));
+				if (ret)
+					goto out;
+			}
+		}
+	}
+
 	ret = dw_spi_add_host(&pdev->dev, dws);
 	if (ret)
 		goto out;
diff --git a/drivers/spi/spi-dw.c b/drivers/spi/spi-dw.c
index 712ac5629cd..29f33143b79 100644
--- a/drivers/spi/spi-dw.c
+++ b/drivers/spi/spi-dw.c
@@ -24,6 +24,7 @@
 #include <linux/delay.h>
 #include <linux/slab.h>
 #include <linux/spi/spi.h>
+#include <linux/gpio.h>
 
 #include "spi-dw.h"
 
@@ -36,12 +37,6 @@
 #define DONE_STATE	((void *)2)
 #define ERROR_STATE	((void *)-1)
 
-#define QUEUE_RUNNING	0
-#define QUEUE_STOPPED	1
-
-#define MRST_SPI_DEASSERT	0
-#define MRST_SPI_ASSERT		1
-
 /* Slave spi_dev related */
 struct chip_data {
 	u16 cr0;
@@ -263,28 +258,22 @@ static int map_dma_buffers(struct dw_spi *dws)
 static void giveback(struct dw_spi *dws)
 {
 	struct spi_transfer *last_transfer;
-	unsigned long flags;
 	struct spi_message *msg;
 
-	spin_lock_irqsave(&dws->lock, flags);
 	msg = dws->cur_msg;
 	dws->cur_msg = NULL;
 	dws->cur_transfer = NULL;
 	dws->prev_chip = dws->cur_chip;
 	dws->cur_chip = NULL;
 	dws->dma_mapped = 0;
-	queue_work(dws->workqueue, &dws->pump_messages);
-	spin_unlock_irqrestore(&dws->lock, flags);
 
 	last_transfer = list_last_entry(&msg->transfers, struct spi_transfer,
 					transfer_list);
 
-	if (!last_transfer->cs_change && dws->cs_control)
-		dws->cs_control(MRST_SPI_DEASSERT);
+	if (!last_transfer->cs_change)
+		spi_chip_sel(dws, dws->cur_msg->spi, 0);
 
-	msg->state = NULL;
-	if (msg->complete)
-		msg->complete(msg->context);
+	spi_finalize_current_message(dws->master);
 }
 
 static void int_error_stop(struct dw_spi *dws, const char *msg)
@@ -502,7 +491,7 @@ static void pump_transfers(unsigned long data)
 			dw_writew(dws, DW_SPI_CTRL0, cr0);
 
 		spi_set_clk(dws, clk_div ? clk_div : chip->clk_div);
-		spi_chip_sel(dws, spi->chip_select);
+		spi_chip_sel(dws, spi, 1);
 
 		/* Set the interrupt mask, for poll mode just disable all int */
 		spi_mask_intr(dws, 0xff);
@@ -529,30 +518,12 @@ early_exit:
 	return;
 }
 
-static void pump_messages(struct work_struct *work)
+static int dw_spi_transfer_one_message(struct spi_master *master,
+		struct spi_message *msg)
 {
-	struct dw_spi *dws =
-		container_of(work, struct dw_spi, pump_messages);
-	unsigned long flags;
-
-	/* Lock queue and check for queue work */
-	spin_lock_irqsave(&dws->lock, flags);
-	if (list_empty(&dws->queue) || dws->run == QUEUE_STOPPED) {
-		dws->busy = 0;
-		spin_unlock_irqrestore(&dws->lock, flags);
-		return;
-	}
-
-	/* Make sure we are not already running a message */
-	if (dws->cur_msg) {
-		spin_unlock_irqrestore(&dws->lock, flags);
-		return;
-	}
-
-	/* Extract head of queue */
-	dws->cur_msg = list_entry(dws->queue.next, struct spi_message, queue);
-	list_del_init(&dws->cur_msg->queue);
+	struct dw_spi *dws = spi_master_get_devdata(master);
 
+	dws->cur_msg = msg;
 	/* Initial message state*/
 	dws->cur_msg->state = START_STATE;
 	dws->cur_transfer = list_entry(dws->cur_msg->transfers.next,
@@ -560,46 +531,9 @@ static void pump_messages(struct work_struct *work)
 						transfer_list);
 	dws->cur_chip = spi_get_ctldata(dws->cur_msg->spi);
 
-	/* Mark as busy and launch transfers */
+	/* Launch transfers */
 	tasklet_schedule(&dws->pump_transfers);
 
-	dws->busy = 1;
-	spin_unlock_irqrestore(&dws->lock, flags);
-}
-
-/* spi_device use this to queue in their spi_msg */
-static int dw_spi_transfer(struct spi_device *spi, struct spi_message *msg)
-{
-	struct dw_spi *dws = spi_master_get_devdata(spi->master);
-	unsigned long flags;
-
-	spin_lock_irqsave(&dws->lock, flags);
-
-	if (dws->run == QUEUE_STOPPED) {
-		spin_unlock_irqrestore(&dws->lock, flags);
-		return -ESHUTDOWN;
-	}
-
-	msg->actual_length = 0;
-	msg->status = -EINPROGRESS;
-	msg->state = START_STATE;
-
-	list_add_tail(&msg->queue, &dws->queue);
-
-	if (dws->run == QUEUE_RUNNING && !dws->busy) {
-
-		if (dws->cur_transfer || dws->cur_msg)
-			queue_work(dws->workqueue,
-					&dws->pump_messages);
-		else {
-			/* If no other data transaction in air, just go */
-			spin_unlock_irqrestore(&dws->lock, flags);
-			pump_messages(&dws->pump_messages);
-			return 0;
-		}
-	}
-
-	spin_unlock_irqrestore(&dws->lock, flags);
 	return 0;
 }
 
@@ -608,6 +542,7 @@ static int dw_spi_setup(struct spi_device *spi)
 {
 	struct dw_spi_chip *chip_info = NULL;
 	struct chip_data *chip;
+	int ret;
 
 	/* Only alloc on first setup */
 	chip = spi_get_ctldata(spi);
@@ -661,81 +596,13 @@ static int dw_spi_setup(struct spi_device *spi)
 			| (spi->mode  << SPI_MODE_OFFSET)
 			| (chip->tmode << SPI_TMOD_OFFSET);
 
-	return 0;
-}
-
-static int init_queue(struct dw_spi *dws)
-{
-	INIT_LIST_HEAD(&dws->queue);
-	spin_lock_init(&dws->lock);
-
-	dws->run = QUEUE_STOPPED;
-	dws->busy = 0;
-
-	tasklet_init(&dws->pump_transfers,
-			pump_transfers,	(unsigned long)dws);
-
-	INIT_WORK(&dws->pump_messages, pump_messages);
-	dws->workqueue = create_singlethread_workqueue(
-					dev_name(dws->master->dev.parent));
-	if (dws->workqueue == NULL)
-		return -EBUSY;
-
-	return 0;
-}
-
-static int start_queue(struct dw_spi *dws)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&dws->lock, flags);
-
-	if (dws->run == QUEUE_RUNNING || dws->busy) {
-		spin_unlock_irqrestore(&dws->lock, flags);
-		return -EBUSY;
+	if (gpio_is_valid(spi->cs_gpio)) {
+		ret = gpio_direction_output(spi->cs_gpio,
+				!(spi->mode & SPI_CS_HIGH));
+		if (ret)
+			return ret;
 	}
 
-	dws->run = QUEUE_RUNNING;
-	dws->cur_msg = NULL;
-	dws->cur_transfer = NULL;
-	dws->cur_chip = NULL;
-	dws->prev_chip = NULL;
-	spin_unlock_irqrestore(&dws->lock, flags);
-
-	queue_work(dws->workqueue, &dws->pump_messages);
-
-	return 0;
-}
-
-static int stop_queue(struct dw_spi *dws)
-{
-	unsigned long flags;
-	unsigned limit = 50;
-	int status = 0;
-
-	spin_lock_irqsave(&dws->lock, flags);
-	dws->run = QUEUE_STOPPED;
-	while ((!list_empty(&dws->queue) || dws->busy) && limit--) {
-		spin_unlock_irqrestore(&dws->lock, flags);
-		msleep(10);
-		spin_lock_irqsave(&dws->lock, flags);
-	}
-
-	if (!list_empty(&dws->queue) || dws->busy)
-		status = -EBUSY;
-	spin_unlock_irqrestore(&dws->lock, flags);
-
-	return status;
-}
-
-static int destroy_queue(struct dw_spi *dws)
-{
-	int status;
-
-	status = stop_queue(dws);
-	if (status != 0)
-		return status;
-	destroy_workqueue(dws->workqueue);
 	return 0;
 }
 
@@ -794,7 +661,7 @@ int dw_spi_add_host(struct device *dev, struct dw_spi *dws)
 	master->bus_num = dws->bus_num;
 	master->num_chipselect = dws->num_cs;
 	master->setup = dw_spi_setup;
-	master->transfer = dw_spi_transfer;
+	master->transfer_one_message = dw_spi_transfer_one_message;
 	master->max_speed_hz = dws->max_freq;
 
 	/* Basic HW init */
@@ -808,33 +675,21 @@ int dw_spi_add_host(struct device *dev, struct dw_spi *dws)
 		}
 	}
 
-	/* Initial and start queue */
-	ret = init_queue(dws);
-	if (ret) {
-		dev_err(&master->dev, "problem initializing queue\n");
-		goto err_diable_hw;
-	}
-	ret = start_queue(dws);
-	if (ret) {
-		dev_err(&master->dev, "problem starting queue\n");
-		goto err_diable_hw;
-	}
+	tasklet_init(&dws->pump_transfers, pump_transfers, (unsigned long)dws);
 
 	spi_master_set_devdata(master, dws);
 	ret = devm_spi_register_master(dev, master);
 	if (ret) {
 		dev_err(&master->dev, "problem registering spi master\n");
-		goto err_queue_alloc;
+		goto err_dma_exit;
 	}
 
 	mrst_spi_debugfs_init(dws);
 	return 0;
 
-err_queue_alloc:
-	destroy_queue(dws);
+err_dma_exit:
 	if (dws->dma_ops && dws->dma_ops->dma_exit)
 		dws->dma_ops->dma_exit(dws);
-err_diable_hw:
 	spi_enable_chip(dws, 0);
 err_free_master:
 	spi_master_put(master);
@@ -844,18 +699,10 @@ EXPORT_SYMBOL_GPL(dw_spi_add_host);
 
 void dw_spi_remove_host(struct dw_spi *dws)
 {
-	int status = 0;
-
 	if (!dws)
 		return;
 	mrst_spi_debugfs_remove(dws);
 
-	/* Remove the queue */
-	status = destroy_queue(dws);
-	if (status != 0)
-		dev_err(&dws->master->dev,
-			"dw_spi_remove: workqueue will not complete, message memory not freed\n");
-
 	if (dws->dma_ops && dws->dma_ops->dma_exit)
 		dws->dma_ops->dma_exit(dws);
 	spi_enable_chip(dws, 0);
@@ -868,7 +715,7 @@ int dw_spi_suspend_host(struct dw_spi *dws)
 {
 	int ret = 0;
 
-	ret = stop_queue(dws);
+	ret = spi_master_suspend(dws->master);
 	if (ret)
 		return ret;
 	spi_enable_chip(dws, 0);
@@ -882,7 +729,7 @@ int dw_spi_resume_host(struct dw_spi *dws)
 	int ret;
 
 	spi_hw_init(dws);
-	ret = start_queue(dws);
+	ret = spi_master_resume(dws->master);
 	if (ret)
 		dev_err(&dws->master->dev, "fail to start queue (%d)\n", ret);
 	return ret;
diff --git a/drivers/spi/spi-dw.h b/drivers/spi/spi-dw.h
index 587643dae11..6d2acad34f6 100644
--- a/drivers/spi/spi-dw.h
+++ b/drivers/spi/spi-dw.h
@@ -3,6 +3,7 @@
 
 #include <linux/io.h>
 #include <linux/scatterlist.h>
+#include <linux/gpio.h>
 
 /* Register offsets */
 #define DW_SPI_CTRL0			0x00
@@ -104,14 +105,6 @@ struct dw_spi {
 	u16			bus_num;
 	u16			num_cs;		/* supported slave numbers */
 
-	/* Driver message queue */
-	struct workqueue_struct	*workqueue;
-	struct work_struct	pump_messages;
-	spinlock_t		lock;
-	struct list_head	queue;
-	int			busy;
-	int			run;
-
 	/* Message Transfer pump */
 	struct tasklet_struct	pump_transfers;
 
@@ -186,15 +179,20 @@ static inline void spi_set_clk(struct dw_spi *dws, u16 div)
 	dw_writel(dws, DW_SPI_BAUDR, div);
 }
 
-static inline void spi_chip_sel(struct dw_spi *dws, u16 cs)
+static inline void spi_chip_sel(struct dw_spi *dws, struct spi_device *spi,
+		int active)
 {
-	if (cs > dws->num_cs)
-		return;
+	u16 cs = spi->chip_select;
+	int gpio_val = active ? (spi->mode & SPI_CS_HIGH) :
+		!(spi->mode & SPI_CS_HIGH);
 
 	if (dws->cs_control)
-		dws->cs_control(1);
+		dws->cs_control(active);
+	if (gpio_is_valid(spi->cs_gpio))
+		gpio_set_value(spi->cs_gpio, gpio_val);
 
-	dw_writel(dws, DW_SPI_SER, 1 << cs);
+	if (active)
+		dw_writel(dws, DW_SPI_SER, 1 << cs);
 }
 
 /* Disable IRQ bits */
diff --git a/drivers/spi/spi-falcon.c b/drivers/spi/spi-falcon.c
index 09965f069a1..ba441ad9a00 100644
--- a/drivers/spi/spi-falcon.c
+++ b/drivers/spi/spi-falcon.c
@@ -11,7 +11,6 @@
 #include <linux/platform_device.h>
 #include <linux/spi/spi.h>
 #include <linux/delay.h>
-#include <linux/workqueue.h>
 #include <linux/of.h>
 #include <linux/of_platform.h>
 
diff --git a/drivers/spi/spi-fsl-dspi.c b/drivers/spi/spi-fsl-dspi.c
index d565eeee3bd..5021ddf03f6 100644
--- a/drivers/spi/spi-fsl-dspi.c
+++ b/drivers/spi/spi-fsl-dspi.c
@@ -406,7 +406,7 @@ static irqreturn_t dspi_interrupt(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static struct of_device_id fsl_dspi_dt_ids[] = {
+static const struct of_device_id fsl_dspi_dt_ids[] = {
 	{ .compatible = "fsl,vf610-dspi", .data = NULL, },
 	{ /* sentinel */ }
 };
diff --git a/drivers/spi/spi-fsl-espi.c b/drivers/spi/spi-fsl-espi.c
index e767f5831b9..8ebd724e4c5 100644
--- a/drivers/spi/spi-fsl-espi.c
+++ b/drivers/spi/spi-fsl-espi.c
@@ -348,7 +348,7 @@ static void fsl_espi_cmd_trans(struct spi_message *m,
 	}
 
 	espi_trans->tx_buf = local_buf;
-	espi_trans->rx_buf = local_buf + espi_trans->n_tx;
+	espi_trans->rx_buf = local_buf;
 	fsl_espi_do_trans(m, espi_trans);
 
 	espi_trans->actual_length = espi_trans->len;
@@ -397,7 +397,7 @@ static void fsl_espi_rw_trans(struct spi_message *m,
 		espi_trans->n_rx = trans_len;
 		espi_trans->len = trans_len + n_tx;
 		espi_trans->tx_buf = local_buf;
-		espi_trans->rx_buf = local_buf + n_tx;
+		espi_trans->rx_buf = local_buf;
 		fsl_espi_do_trans(m, espi_trans);
 
 		memcpy(rx_buf + pos, espi_trans->rx_buf + n_tx, trans_len);
@@ -458,7 +458,7 @@ static int fsl_espi_setup(struct spi_device *spi)
 		return -EINVAL;
 
 	if (!cs) {
-		cs = kzalloc(sizeof *cs, GFP_KERNEL);
+		cs = devm_kzalloc(&spi->dev, sizeof(*cs), GFP_KERNEL);
 		if (!cs)
 			return -ENOMEM;
 		spi->controller_state = cs;
@@ -586,8 +586,10 @@ static struct spi_master * fsl_espi_probe(struct device *dev,
 	struct spi_master *master;
 	struct mpc8xxx_spi *mpc8xxx_spi;
 	struct fsl_espi_reg *reg_base;
-	u32 regval;
-	int i, ret = 0;
+	struct device_node *nc;
+	const __be32 *prop;
+	u32 regval, csmode;
+	int i, len, ret = 0;
 
 	master = spi_alloc_master(dev, sizeof(struct mpc8xxx_spi));
 	if (!master) {
@@ -634,8 +636,32 @@ static struct spi_master * fsl_espi_probe(struct device *dev,
 	mpc8xxx_spi_write_reg(&reg_base->event, 0xffffffff);
 
 	/* Init eSPI CS mode register */
-	for (i = 0; i < pdata->max_chipselect; i++)
-		mpc8xxx_spi_write_reg(&reg_base->csmode[i], CSMODE_INIT_VAL);
+	for_each_available_child_of_node(master->dev.of_node, nc) {
+		/* get chip select */
+		prop = of_get_property(nc, "reg", &len);
+		if (!prop || len < sizeof(*prop))
+			continue;
+		i = be32_to_cpup(prop);
+		if (i < 0 || i >= pdata->max_chipselect)
+			continue;
+
+		csmode = CSMODE_INIT_VAL;
+		/* check if CSBEF is set in device tree */
+		prop = of_get_property(nc, "fsl,csbef", &len);
+		if (prop && len >= sizeof(*prop)) {
+			csmode &= ~(CSMODE_BEF(0xf));
+			csmode |= CSMODE_BEF(be32_to_cpup(prop));
+		}
+		/* check if CSAFT is set in device tree */
+		prop = of_get_property(nc, "fsl,csaft", &len);
+		if (prop && len >= sizeof(*prop)) {
+			csmode &= ~(CSMODE_AFT(0xf));
+			csmode |= CSMODE_AFT(be32_to_cpup(prop));
+		}
+		mpc8xxx_spi_write_reg(&reg_base->csmode[i], csmode);
+
+		dev_info(dev, "cs=%d, init_csmode=0x%x\n", i, csmode);
+	}
 
 	/* Enable SPI interface */
 	regval = pdata->initial_spmode | SPMODE_INIT_VAL | SPMODE_ENABLE;
diff --git a/drivers/spi/spi-fsl-lib.c b/drivers/spi/spi-fsl-lib.c
index e5d45fca355..95212ea96c8 100644
--- a/drivers/spi/spi-fsl-lib.c
+++ b/drivers/spi/spi-fsl-lib.c
@@ -99,11 +99,6 @@ int mpc8xxx_spi_transfer(struct spi_device *spi,
 	return 0;
 }
 
-void mpc8xxx_spi_cleanup(struct spi_device *spi)
-{
-	kfree(spi->controller_state);
-}
-
 const char *mpc8xxx_spi_strmode(unsigned int flags)
 {
 	if (flags & SPI_QE_CPU_MODE) {
@@ -134,7 +129,6 @@ int mpc8xxx_spi_probe(struct device *dev, struct resource *mem,
 			| SPI_LSB_FIRST | SPI_LOOP;
 
 	master->transfer = mpc8xxx_spi_transfer;
-	master->cleanup = mpc8xxx_spi_cleanup;
 	master->dev.of_node = dev->of_node;
 
 	mpc8xxx_spi = spi_master_get_devdata(master);
diff --git a/drivers/spi/spi-fsl-lib.h b/drivers/spi/spi-fsl-lib.h
index 52db6936778..2fcbfd01d10 100644
--- a/drivers/spi/spi-fsl-lib.h
+++ b/drivers/spi/spi-fsl-lib.h
@@ -124,7 +124,6 @@ extern struct mpc8xxx_spi_probe_info *to_of_pinfo(
 extern int mpc8xxx_spi_bufs(struct mpc8xxx_spi *mspi,
 		struct spi_transfer *t, unsigned int len);
 extern int mpc8xxx_spi_transfer(struct spi_device *spi, struct spi_message *m);
-extern void mpc8xxx_spi_cleanup(struct spi_device *spi);
 extern const char *mpc8xxx_spi_strmode(unsigned int flags);
 extern int mpc8xxx_spi_probe(struct device *dev, struct resource *mem,
 		unsigned int irq);
diff --git a/drivers/spi/spi-fsl-spi.c b/drivers/spi/spi-fsl-spi.c
index b3e7775034d..98ccd231bf0 100644
--- a/drivers/spi/spi-fsl-spi.c
+++ b/drivers/spi/spi-fsl-spi.c
@@ -431,7 +431,7 @@ static int fsl_spi_setup(struct spi_device *spi)
 		return -EINVAL;
 
 	if (!cs) {
-		cs = kzalloc(sizeof *cs, GFP_KERNEL);
+		cs = devm_kzalloc(&spi->dev, sizeof(*cs), GFP_KERNEL);
 		if (!cs)
 			return -ENOMEM;
 		spi->controller_state = cs;
diff --git a/drivers/spi/spi-gpio.c b/drivers/spi/spi-gpio.c
index 09823076df8..9f595535cf2 100644
--- a/drivers/spi/spi-gpio.c
+++ b/drivers/spi/spi-gpio.c
@@ -340,7 +340,7 @@ done:
 }
 
 #ifdef CONFIG_OF
-static struct of_device_id spi_gpio_dt_ids[] = {
+static const struct of_device_id spi_gpio_dt_ids[] = {
 	{ .compatible = "spi-gpio" },
 	{}
 };
diff --git a/drivers/spi/spi-nuc900.c b/drivers/spi/spi-nuc900.c
index 16e30de650b..73e91d5a43d 100644
--- a/drivers/spi/spi-nuc900.c
+++ b/drivers/spi/spi-nuc900.c
@@ -10,7 +10,6 @@
 
 #include <linux/module.h>
 #include <linux/spinlock.h>
-#include <linux/workqueue.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/errno.h>
diff --git a/drivers/spi/spi-omap-uwire.c b/drivers/spi/spi-omap-uwire.c
index be2a2e108e2..0f5a0aa3b87 100644
--- a/drivers/spi/spi-omap-uwire.c
+++ b/drivers/spi/spi-omap-uwire.c
@@ -37,7 +37,6 @@
 #include <linux/init.h>
 #include <linux/delay.h>
 #include <linux/platform_device.h>
-#include <linux/workqueue.h>
 #include <linux/interrupt.h>
 #include <linux/err.h>
 #include <linux/clk.h>
diff --git a/drivers/spi/spi-pl022.c b/drivers/spi/spi-pl022.c
index 51d99779682..66d2ae21e78 100644
--- a/drivers/spi/spi-pl022.c
+++ b/drivers/spi/spi-pl022.c
@@ -1111,10 +1111,8 @@ static int pl022_dma_probe(struct pl022 *pl022)
 	}
 
 	pl022->dummypage = kmalloc(PAGE_SIZE, GFP_KERNEL);
-	if (!pl022->dummypage) {
-		dev_dbg(&pl022->adev->dev, "no DMA dummypage!\n");
+	if (!pl022->dummypage)
 		goto err_no_dummypage;
-	}
 
 	dev_info(&pl022->adev->dev, "setup for DMA on RX %s, TX %s\n",
 		 dma_chan_name(pl022->dma_rx_channel),
@@ -1809,11 +1807,8 @@ static int pl022_setup(struct spi_device *spi)
 
 	if (chip == NULL) {
 		chip = kzalloc(sizeof(struct chip_data), GFP_KERNEL);
-		if (!chip) {
-			dev_err(&spi->dev,
-				"cannot allocate controller state\n");
+		if (!chip)
 			return -ENOMEM;
-		}
 		dev_dbg(&spi->dev,
 			"allocated memory for controller's runtime state\n");
 	}
@@ -2050,10 +2045,8 @@ pl022_platform_data_dt_get(struct device *dev)
 	}
 
 	pd = devm_kzalloc(dev, sizeof(struct pl022_ssp_controller), GFP_KERNEL);
-	if (!pd) {
-		dev_err(dev, "cannot allocate platform data memory\n");
+	if (!pd)
 		return NULL;
-	}
 
 	pd->bus_id = -1;
 	pd->enable_dma = 1;
diff --git a/drivers/spi/spi-pxa2xx-dma.c b/drivers/spi/spi-pxa2xx-dma.c
index 713af4806f2..f6759dc0153 100644
--- a/drivers/spi/spi-pxa2xx-dma.c
+++ b/drivers/spi/spi-pxa2xx-dma.c
@@ -29,18 +29,6 @@ static int pxa2xx_spi_map_dma_buffer(struct driver_data *drv_data,
 	struct sg_table *sgt;
 	void *buf, *pbuf;
 
-	/*
-	 * Some DMA controllers have problems transferring buffers that are
-	 * not multiple of 4 bytes. So we truncate the transfer so that it
-	 * is suitable for such controllers, and handle the trailing bytes
-	 * manually after the DMA completes.
-	 *
-	 * REVISIT: It would be better if this information could be
-	 * retrieved directly from the DMA device in a similar way than
-	 * ->copy_align etc. is done.
-	 */
-	len = ALIGN(drv_data->len, 4);
-
 	if (dir == DMA_TO_DEVICE) {
 		dmadev = drv_data->tx_chan->device->dev;
 		sgt = &drv_data->tx_sgt;
@@ -144,12 +132,8 @@ static void pxa2xx_spi_dma_transfer_complete(struct driver_data *drv_data,
 		if (!error) {
 			pxa2xx_spi_unmap_dma_buffers(drv_data);
 
-			/* Handle the last bytes of unaligned transfer */
 			drv_data->tx += drv_data->tx_map_len;
-			drv_data->write(drv_data);
-
 			drv_data->rx += drv_data->rx_map_len;
-			drv_data->read(drv_data);
 
 			msg->actual_length += drv_data->len;
 			msg->state = pxa2xx_spi_next_transfer(drv_data);
diff --git a/drivers/spi/spi-pxa2xx-pci.c b/drivers/spi/spi-pxa2xx-pci.c
index 3f006d3ed2a..c1865c92ccb 100644
--- a/drivers/spi/spi-pxa2xx-pci.c
+++ b/drivers/spi/spi-pxa2xx-pci.c
@@ -8,7 +8,43 @@
 #include <linux/module.h>
 #include <linux/spi/pxa2xx_spi.h>
 
-static int ce4100_spi_probe(struct pci_dev *dev,
+enum {
+	PORT_CE4100,
+	PORT_BYT,
+};
+
+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;
+};
+
+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,
+	},
+	[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,
+	},
+};
+
+static int pxa2xx_spi_pci_probe(struct pci_dev *dev,
 		const struct pci_device_id *ent)
 {
 	struct platform_device_info pi;
@@ -16,6 +52,7 @@ static int ce4100_spi_probe(struct pci_dev *dev,
 	struct platform_device *pdev;
 	struct pxa2xx_spi_master spi_pdata;
 	struct ssp_device *ssp;
+	struct pxa_spi_info *c;
 
 	ret = pcim_enable_device(dev);
 	if (ret)
@@ -25,8 +62,16 @@ static int ce4100_spi_probe(struct pci_dev *dev,
 	if (ret)
 		return ret;
 
+	c = &spi_info_configs[ent->driver_data];
+
 	memset(&spi_pdata, 0, sizeof(spi_pdata));
-	spi_pdata.num_chipselect = dev->devfn;
+	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;
 
 	ssp = &spi_pdata.ssp;
 	ssp->phys_base = pci_resource_start(dev, 0);
@@ -36,8 +81,8 @@ static int ce4100_spi_probe(struct pci_dev *dev,
 		return -EIO;
 	}
 	ssp->irq = dev->irq;
-	ssp->port_id = dev->devfn;
-	ssp->type = PXA25x_SSP;
+	ssp->port_id = (c->port_id >= 0) ? c->port_id : dev->devfn;
+	ssp->type = c->type;
 
 	memset(&pi, 0, sizeof(pi));
 	pi.parent = &dev->dev;
@@ -55,28 +100,29 @@ static int ce4100_spi_probe(struct pci_dev *dev,
 	return 0;
 }
 
-static void ce4100_spi_remove(struct pci_dev *dev)
+static void pxa2xx_spi_pci_remove(struct pci_dev *dev)
 {
 	struct platform_device *pdev = pci_get_drvdata(dev);
 
 	platform_device_unregister(pdev);
 }
 
-static const struct pci_device_id ce4100_spi_devices[] = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x2e6a) },
+static const struct pci_device_id pxa2xx_spi_pci_devices[] = {
+	{ PCI_VDEVICE(INTEL, 0x2e6a), PORT_CE4100 },
+	{ PCI_VDEVICE(INTEL, 0x0f0e), PORT_BYT },
 	{ },
 };
-MODULE_DEVICE_TABLE(pci, ce4100_spi_devices);
+MODULE_DEVICE_TABLE(pci, pxa2xx_spi_pci_devices);
 
-static struct pci_driver ce4100_spi_driver = {
-	.name           = "ce4100_spi",
-	.id_table       = ce4100_spi_devices,
-	.probe          = ce4100_spi_probe,
-	.remove         = ce4100_spi_remove,
+static struct pci_driver pxa2xx_spi_pci_driver = {
+	.name           = "pxa2xx_spi_pci",
+	.id_table       = pxa2xx_spi_pci_devices,
+	.probe          = pxa2xx_spi_pci_probe,
+	.remove         = pxa2xx_spi_pci_remove,
 };
 
-module_pci_driver(ce4100_spi_driver);
+module_pci_driver(pxa2xx_spi_pci_driver);
 
-MODULE_DESCRIPTION("CE4100 PCI-SPI glue code for PXA's driver");
+MODULE_DESCRIPTION("CE4100/LPSS PCI-SPI glue code for PXA's driver");
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Sebastian Andrzej Siewior <bigeasy@linutronix.de>");
diff --git a/drivers/spi/spi-pxa2xx.c b/drivers/spi/spi-pxa2xx.c
index 41185d0557f..a98df7eeb42 100644
--- a/drivers/spi/spi-pxa2xx.c
+++ b/drivers/spi/spi-pxa2xx.c
@@ -27,7 +27,6 @@
 #include <linux/platform_device.h>
 #include <linux/spi/pxa2xx_spi.h>
 #include <linux/spi/spi.h>
-#include <linux/workqueue.h>
 #include <linux/delay.h>
 #include <linux/gpio.h>
 #include <linux/slab.h>
@@ -886,11 +885,8 @@ static int setup(struct spi_device *spi)
 	chip = spi_get_ctldata(spi);
 	if (!chip) {
 		chip = kzalloc(sizeof(struct chip_data), GFP_KERNEL);
-		if (!chip) {
-			dev_err(&spi->dev,
-				"failed setup: can't allocate chip data\n");
+		if (!chip)
 			return -ENOMEM;
-		}
 
 		if (drv_data->ssp_type == CE4100_SSP) {
 			if (spi->chip_select > 4) {
@@ -1037,11 +1033,8 @@ pxa2xx_spi_acpi_get_pdata(struct platform_device *pdev)
 		return NULL;
 
 	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
-	if (!pdata) {
-		dev_err(&pdev->dev,
-			"failed to allocate memory for platform data\n");
+	if (!pdata)
 		return NULL;
-	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res)
@@ -1202,6 +1195,11 @@ static int pxa2xx_spi_probe(struct platform_device *pdev)
 	tasklet_init(&drv_data->pump_transfers, pump_transfers,
 		     (unsigned long)drv_data);
 
+	pm_runtime_set_autosuspend_delay(&pdev->dev, 50);
+	pm_runtime_use_autosuspend(&pdev->dev);
+	pm_runtime_set_active(&pdev->dev);
+	pm_runtime_enable(&pdev->dev);
+
 	/* Register with the SPI framework */
 	platform_set_drvdata(pdev, drv_data);
 	status = devm_spi_register_master(&pdev->dev, master);
@@ -1210,11 +1208,6 @@ static int pxa2xx_spi_probe(struct platform_device *pdev)
 		goto out_error_clock_enabled;
 	}
 
-	pm_runtime_set_autosuspend_delay(&pdev->dev, 50);
-	pm_runtime_use_autosuspend(&pdev->dev);
-	pm_runtime_set_active(&pdev->dev);
-	pm_runtime_enable(&pdev->dev);
-
 	return status;
 
 out_error_clock_enabled:
diff --git a/drivers/spi/spi-qup.c b/drivers/spi/spi-qup.c
index b032e8885e2..fc1de86d3c8 100644
--- a/drivers/spi/spi-qup.c
+++ b/drivers/spi/spi-qup.c
@@ -287,7 +287,7 @@ static irqreturn_t spi_qup_qup_irq(int irq, void *dev_id)
 	writel_relaxed(opflags, controller->base + QUP_OPERATIONAL);
 
 	if (!xfer) {
-		dev_err_ratelimited(controller->dev, "unexpected irq %x08 %x08 %x08\n",
+		dev_err_ratelimited(controller->dev, "unexpected irq %08x %08x %08x\n",
 				    qup_err, spi_err, opflags);
 		return IRQ_HANDLED;
 	}
@@ -366,7 +366,7 @@ static int spi_qup_io_config(struct spi_device *spi, struct spi_transfer *xfer)
 	n_words = xfer->len / w_size;
 	controller->w_size = w_size;
 
-	if (n_words <= controller->in_fifo_sz) {
+	if (n_words <= (controller->in_fifo_sz / sizeof(u32))) {
 		mode = QUP_IO_M_MODE_FIFO;
 		writel_relaxed(n_words, controller->base + QUP_MX_READ_CNT);
 		writel_relaxed(n_words, controller->base + QUP_MX_WRITE_CNT);
@@ -734,7 +734,7 @@ static int spi_qup_remove(struct platform_device *pdev)
 	int ret;
 
 	ret = pm_runtime_get_sync(&pdev->dev);
-	if (ret)
+	if (ret < 0)
 		return ret;
 
 	ret = spi_qup_set_state(controller, QUP_STATE_RESET);
@@ -749,7 +749,7 @@ static int spi_qup_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id spi_qup_dt_match[] = {
+static const struct of_device_id spi_qup_dt_match[] = {
 	{ .compatible = "qcom,spi-qup-v2.1.1", },
 	{ .compatible = "qcom,spi-qup-v2.2.1", },
 	{ }
diff --git a/drivers/spi/spi-rspi.c b/drivers/spi/spi-rspi.c
index 1fb0ad21332..10112745bb1 100644
--- a/drivers/spi/spi-rspi.c
+++ b/drivers/spi/spi-rspi.c
@@ -183,8 +183,6 @@
 #define SPBFCR_TXTRG_MASK	0x30	/* Transmit Buffer Data Triggering Number */
 #define SPBFCR_RXTRG_MASK	0x07	/* Receive Buffer Data Triggering Number */
 
-#define DUMMY_DATA		0x00
-
 struct rspi_data {
 	void __iomem *addr;
 	u32 max_speed_hz;
@@ -197,11 +195,6 @@ struct rspi_data {
 	int rx_irq, tx_irq;
 	const struct spi_ops *ops;
 
-	/* for dmaengine */
-	struct dma_chan *chan_tx;
-	struct dma_chan *chan_rx;
-
-	unsigned dma_width_16bit:1;
 	unsigned dma_callbacked:1;
 	unsigned byte_access:1;
 };
@@ -253,6 +246,8 @@ struct spi_ops {
 	int (*transfer_one)(struct spi_master *master, struct spi_device *spi,
 			    struct spi_transfer *xfer);
 	u16 mode_bits;
+	u16 flags;
+	u16 fifo_size;
 };
 
 /*
@@ -266,7 +261,8 @@ static int rspi_set_config_register(struct rspi_data *rspi, int access_size)
 	rspi_write8(rspi, rspi->sppcr, RSPI_SPPCR);
 
 	/* Sets transfer bit rate */
-	spbr = clk_get_rate(rspi->clk) / (2 * rspi->max_speed_hz) - 1;
+	spbr = DIV_ROUND_UP(clk_get_rate(rspi->clk),
+			    2 * rspi->max_speed_hz) - 1;
 	rspi_write8(rspi, clamp(spbr, 0, 255), RSPI_SPBR);
 
 	/* Disable dummy transmission, set 16-bit word access, 1 frame */
@@ -302,7 +298,8 @@ static int rspi_rz_set_config_register(struct rspi_data *rspi, int access_size)
 	rspi_write8(rspi, rspi->sppcr, RSPI_SPPCR);
 
 	/* Sets transfer bit rate */
-	spbr = clk_get_rate(rspi->clk) / (2 * rspi->max_speed_hz) - 1;
+	spbr = DIV_ROUND_UP(clk_get_rate(rspi->clk),
+			    2 * rspi->max_speed_hz) - 1;
 	rspi_write8(rspi, clamp(spbr, 0, 255), RSPI_SPBR);
 
 	/* Disable dummy transmission, set byte access */
@@ -335,7 +332,7 @@ static int qspi_set_config_register(struct rspi_data *rspi, int access_size)
 	rspi_write8(rspi, rspi->sppcr, RSPI_SPPCR);
 
 	/* Sets transfer bit rate */
-	spbr = clk_get_rate(rspi->clk) / (2 * rspi->max_speed_hz);
+	spbr = DIV_ROUND_UP(clk_get_rate(rspi->clk), 2 * rspi->max_speed_hz);
 	rspi_write8(rspi, clamp(spbr, 0, 255), RSPI_SPBR);
 
 	/* Disable dummy transmission, set byte access */
@@ -403,11 +400,22 @@ static int rspi_wait_for_interrupt(struct rspi_data *rspi, u8 wait_mask,
 	return 0;
 }
 
+static inline int rspi_wait_for_tx_empty(struct rspi_data *rspi)
+{
+	return rspi_wait_for_interrupt(rspi, SPSR_SPTEF, SPCR_SPTIE);
+}
+
+static inline int rspi_wait_for_rx_full(struct rspi_data *rspi)
+{
+	return rspi_wait_for_interrupt(rspi, SPSR_SPRF, SPCR_SPRIE);
+}
+
 static int rspi_data_out(struct rspi_data *rspi, u8 data)
 {
-	if (rspi_wait_for_interrupt(rspi, SPSR_SPTEF, SPCR_SPTIE) < 0) {
+	int error = rspi_wait_for_tx_empty(rspi);
+	if (error < 0) {
 		dev_err(&rspi->master->dev, "transmit timeout\n");
-		return -ETIMEDOUT;
+		return error;
 	}
 	rspi_write_data(rspi, data);
 	return 0;
@@ -415,25 +423,36 @@ static int rspi_data_out(struct rspi_data *rspi, u8 data)
 
 static int rspi_data_in(struct rspi_data *rspi)
 {
+	int error;
 	u8 data;
 
-	if (rspi_wait_for_interrupt(rspi, SPSR_SPRF, SPCR_SPRIE) < 0) {
+	error = rspi_wait_for_rx_full(rspi);
+	if (error < 0) {
 		dev_err(&rspi->master->dev, "receive timeout\n");
-		return -ETIMEDOUT;
+		return error;
 	}
 	data = rspi_read_data(rspi);
 	return data;
 }
 
-static int rspi_data_out_in(struct rspi_data *rspi, u8 data)
+static int rspi_pio_transfer(struct rspi_data *rspi, const u8 *tx, u8 *rx,
+			     unsigned int n)
 {
-	int ret;
-
-	ret = rspi_data_out(rspi, data);
-	if (ret < 0)
-		return ret;
+	while (n-- > 0) {
+		if (tx) {
+			int ret = rspi_data_out(rspi, *tx++);
+			if (ret < 0)
+				return ret;
+		}
+		if (rx) {
+			int ret = rspi_data_in(rspi);
+			if (ret < 0)
+				return ret;
+			*rx++ = ret;
+		}
+	}
 
-	return rspi_data_in(rspi);
+	return 0;
 }
 
 static void rspi_dma_complete(void *arg)
@@ -444,97 +463,67 @@ static void rspi_dma_complete(void *arg)
 	wake_up_interruptible(&rspi->wait);
 }
 
-static int rspi_dma_map_sg(struct scatterlist *sg, const void *buf,
-			   unsigned len, struct dma_chan *chan,
-			   enum dma_transfer_direction dir)
-{
-	sg_init_table(sg, 1);
-	sg_set_buf(sg, buf, len);
-	sg_dma_len(sg) = len;
-	return dma_map_sg(chan->device->dev, sg, 1, dir);
-}
-
-static void rspi_dma_unmap_sg(struct scatterlist *sg, struct dma_chan *chan,
-			      enum dma_transfer_direction dir)
+static int rspi_dma_transfer(struct rspi_data *rspi, struct sg_table *tx,
+			     struct sg_table *rx)
 {
-	dma_unmap_sg(chan->device->dev, sg, 1, dir);
-}
-
-static void rspi_memory_to_8bit(void *buf, const void *data, unsigned len)
-{
-	u16 *dst = buf;
-	const u8 *src = data;
-
-	while (len) {
-		*dst++ = (u16)(*src++);
-		len--;
-	}
-}
-
-static void rspi_memory_from_8bit(void *buf, const void *data, unsigned len)
-{
-	u8 *dst = buf;
-	const u16 *src = data;
-
-	while (len) {
-		*dst++ = (u8)*src++;
-		len--;
-	}
-}
+	struct dma_async_tx_descriptor *desc_tx = NULL, *desc_rx = NULL;
+	u8 irq_mask = 0;
+	unsigned int other_irq = 0;
+	dma_cookie_t cookie;
+	int ret;
 
-static int rspi_send_dma(struct rspi_data *rspi, struct spi_transfer *t)
-{
-	struct scatterlist sg;
-	const void *buf = NULL;
-	struct dma_async_tx_descriptor *desc;
-	unsigned int len;
-	int ret = 0;
-
-	if (rspi->dma_width_16bit) {
-		void *tmp;
-		/*
-		 * If DMAC bus width is 16-bit, the driver allocates a dummy
-		 * buffer. And, the driver converts original data into the
-		 * DMAC data as the following format:
-		 *  original data: 1st byte, 2nd byte ...
-		 *  DMAC data:     1st byte, dummy, 2nd byte, dummy ...
-		 */
-		len = t->len * 2;
-		tmp = kmalloc(len, GFP_KERNEL);
-		if (!tmp)
-			return -ENOMEM;
-		rspi_memory_to_8bit(tmp, t->tx_buf, t->len);
-		buf = tmp;
-	} else {
-		len = t->len;
-		buf = t->tx_buf;
-	}
+	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)
+			return -EIO;
 
-	if (!rspi_dma_map_sg(&sg, buf, len, rspi->chan_tx, DMA_TO_DEVICE)) {
-		ret = -EFAULT;
-		goto end_nomap;
+		irq_mask |= SPCR_SPTIE;
 	}
-	desc = dmaengine_prep_slave_sg(rspi->chan_tx, &sg, 1, DMA_TO_DEVICE,
-				       DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
-	if (!desc) {
-		ret = -EIO;
-		goto end;
+	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)
+			return -EIO;
+
+		irq_mask |= SPCR_SPRIE;
 	}
 
 	/*
-	 * DMAC needs SPTIE, but if SPTIE is set, this IRQ routine will be
+	 * DMAC needs SPxIE, but if SPxIE is set, the IRQ routine will be
 	 * called. So, this driver disables the IRQ while DMA transfer.
 	 */
-	disable_irq(rspi->tx_irq);
+	if (tx)
+		disable_irq(other_irq = rspi->tx_irq);
+	if (rx && rspi->rx_irq != other_irq)
+		disable_irq(rspi->rx_irq);
 
-	rspi_write8(rspi, rspi_read8(rspi, RSPI_SPCR) | SPCR_TXMD, RSPI_SPCR);
-	rspi_enable_irq(rspi, SPCR_SPTIE);
+	rspi_enable_irq(rspi, irq_mask);
 	rspi->dma_callbacked = 0;
 
-	desc->callback = rspi_dma_complete;
-	desc->callback_param = rspi;
-	dmaengine_submit(desc);
-	dma_async_issue_pending(rspi->chan_tx);
+	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;
+		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;
+		dma_async_issue_pending(rspi->master->dma_tx);
+	}
 
 	ret = wait_event_interruptible_timeout(rspi->wait,
 					       rspi->dma_callbacked, HZ);
@@ -542,15 +531,13 @@ static int rspi_send_dma(struct rspi_data *rspi, struct spi_transfer *t)
 		ret = 0;
 	else if (!ret)
 		ret = -ETIMEDOUT;
-	rspi_disable_irq(rspi, SPCR_SPTIE);
 
-	enable_irq(rspi->tx_irq);
+	rspi_disable_irq(rspi, irq_mask);
 
-end:
-	rspi_dma_unmap_sg(&sg, rspi->chan_tx, DMA_TO_DEVICE);
-end_nomap:
-	if (rspi->dma_width_16bit)
-		kfree(buf);
+	if (tx)
+		enable_irq(rspi->tx_irq);
+	if (rx && rspi->rx_irq != other_irq)
+		enable_irq(rspi->rx_irq);
 
 	return ret;
 }
@@ -585,157 +572,37 @@ static void qspi_receive_init(const struct rspi_data *rspi)
 	rspi_write8(rspi, 0, QSPI_SPBFCR);
 }
 
-static int rspi_receive_dma(struct rspi_data *rspi, struct spi_transfer *t)
+static bool __rspi_can_dma(const struct rspi_data *rspi,
+			   const struct spi_transfer *xfer)
 {
-	struct scatterlist sg, sg_dummy;
-	void *dummy = NULL, *rx_buf = NULL;
-	struct dma_async_tx_descriptor *desc, *desc_dummy;
-	unsigned int len;
-	int ret = 0;
-
-	if (rspi->dma_width_16bit) {
-		/*
-		 * If DMAC bus width is 16-bit, the driver allocates a dummy
-		 * buffer. And, finally the driver converts the DMAC data into
-		 * actual data as the following format:
-		 *  DMAC data:   1st byte, dummy, 2nd byte, dummy ...
-		 *  actual data: 1st byte, 2nd byte ...
-		 */
-		len = t->len * 2;
-		rx_buf = kmalloc(len, GFP_KERNEL);
-		if (!rx_buf)
-			return -ENOMEM;
-	 } else {
-		len = t->len;
-		rx_buf = t->rx_buf;
-	}
-
-	/* prepare dummy transfer to generate SPI clocks */
-	dummy = kzalloc(len, GFP_KERNEL);
-	if (!dummy) {
-		ret = -ENOMEM;
-		goto end_nomap;
-	}
-	if (!rspi_dma_map_sg(&sg_dummy, dummy, len, rspi->chan_tx,
-			     DMA_TO_DEVICE)) {
-		ret = -EFAULT;
-		goto end_nomap;
-	}
-	desc_dummy = dmaengine_prep_slave_sg(rspi->chan_tx, &sg_dummy, 1,
-			DMA_TO_DEVICE, DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
-	if (!desc_dummy) {
-		ret = -EIO;
-		goto end_dummy_mapped;
-	}
-
-	/* prepare receive transfer */
-	if (!rspi_dma_map_sg(&sg, rx_buf, len, rspi->chan_rx,
-			     DMA_FROM_DEVICE)) {
-		ret = -EFAULT;
-		goto end_dummy_mapped;
-
-	}
-	desc = dmaengine_prep_slave_sg(rspi->chan_rx, &sg, 1, DMA_FROM_DEVICE,
-				       DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
-	if (!desc) {
-		ret = -EIO;
-		goto end;
-	}
-
-	rspi_receive_init(rspi);
-
-	/*
-	 * DMAC needs SPTIE, but if SPTIE is set, this IRQ routine will be
-	 * called. So, this driver disables the IRQ while DMA transfer.
-	 */
-	disable_irq(rspi->tx_irq);
-	if (rspi->rx_irq != rspi->tx_irq)
-		disable_irq(rspi->rx_irq);
-
-	rspi_write8(rspi, rspi_read8(rspi, RSPI_SPCR) & ~SPCR_TXMD, RSPI_SPCR);
-	rspi_enable_irq(rspi, SPCR_SPTIE | SPCR_SPRIE);
-	rspi->dma_callbacked = 0;
-
-	desc->callback = rspi_dma_complete;
-	desc->callback_param = rspi;
-	dmaengine_submit(desc);
-	dma_async_issue_pending(rspi->chan_rx);
-
-	desc_dummy->callback = NULL;	/* No callback */
-	dmaengine_submit(desc_dummy);
-	dma_async_issue_pending(rspi->chan_tx);
-
-	ret = wait_event_interruptible_timeout(rspi->wait,
-					       rspi->dma_callbacked, HZ);
-	if (ret > 0 && rspi->dma_callbacked)
-		ret = 0;
-	else if (!ret)
-		ret = -ETIMEDOUT;
-	rspi_disable_irq(rspi, SPCR_SPTIE | SPCR_SPRIE);
-
-	enable_irq(rspi->tx_irq);
-	if (rspi->rx_irq != rspi->tx_irq)
-		enable_irq(rspi->rx_irq);
-
-end:
-	rspi_dma_unmap_sg(&sg, rspi->chan_rx, DMA_FROM_DEVICE);
-end_dummy_mapped:
-	rspi_dma_unmap_sg(&sg_dummy, rspi->chan_tx, DMA_TO_DEVICE);
-end_nomap:
-	if (rspi->dma_width_16bit) {
-		if (!ret)
-			rspi_memory_from_8bit(t->rx_buf, rx_buf, t->len);
-		kfree(rx_buf);
-	}
-	kfree(dummy);
-
-	return ret;
+	return xfer->len > rspi->ops->fifo_size;
 }
 
-static int rspi_is_dma(const struct rspi_data *rspi, struct spi_transfer *t)
+static bool rspi_can_dma(struct spi_master *master, struct spi_device *spi,
+			 struct spi_transfer *xfer)
 {
-	if (t->tx_buf && rspi->chan_tx)
-		return 1;
-	/* If the module receives data by DMAC, it also needs TX DMAC */
-	if (t->rx_buf && rspi->chan_tx && rspi->chan_rx)
-		return 1;
+	struct rspi_data *rspi = spi_master_get_devdata(master);
 
-	return 0;
+	return __rspi_can_dma(rspi, xfer);
 }
 
-static int rspi_transfer_out_in(struct rspi_data *rspi,
+static int rspi_common_transfer(struct rspi_data *rspi,
 				struct spi_transfer *xfer)
 {
-	int remain = xfer->len, ret;
-	const u8 *tx_buf = xfer->tx_buf;
-	u8 *rx_buf = xfer->rx_buf;
-	u8 spcr, data;
-
-	rspi_receive_init(rspi);
-
-	spcr = rspi_read8(rspi, RSPI_SPCR);
-	if (rx_buf)
-		spcr &= ~SPCR_TXMD;
-	else
-		spcr |= SPCR_TXMD;
-	rspi_write8(rspi, spcr, RSPI_SPCR);
+	int ret;
 
-	while (remain > 0) {
-		data = tx_buf ? *tx_buf++ : DUMMY_DATA;
-		ret = rspi_data_out(rspi, data);
-		if (ret < 0)
-			return ret;
-		if (rx_buf) {
-			ret = rspi_data_in(rspi);
-			if (ret < 0)
-				return ret;
-			*rx_buf++ = ret;
-		}
-		remain--;
+	if (rspi->master->can_dma && __rspi_can_dma(rspi, xfer)) {
+		/* rx_buf can be NULL on RSPI on SH in TX-only Mode */
+		return rspi_dma_transfer(rspi, &xfer->tx_sg,
+					 xfer->rx_buf ? &xfer->rx_sg : NULL);
 	}
 
+	ret = rspi_pio_transfer(rspi, xfer->tx_buf, xfer->rx_buf, xfer->len);
+	if (ret < 0)
+		return ret;
+
 	/* Wait for the last transmission */
-	rspi_wait_for_interrupt(rspi, SPSR_SPTEF, SPCR_SPTIE);
+	rspi_wait_for_tx_empty(rspi);
 
 	return 0;
 }
@@ -744,46 +611,18 @@ static int rspi_transfer_one(struct spi_master *master, struct spi_device *spi,
 			     struct spi_transfer *xfer)
 {
 	struct rspi_data *rspi = spi_master_get_devdata(master);
-	int ret;
+	u8 spcr;
 
-	if (!rspi_is_dma(rspi, xfer))
-		return rspi_transfer_out_in(rspi, xfer);
-
-	if (xfer->tx_buf) {
-		ret = rspi_send_dma(rspi, xfer);
-		if (ret < 0)
-			return ret;
-	}
-	if (xfer->rx_buf)
-		return rspi_receive_dma(rspi, xfer);
-
-	return 0;
-}
-
-static int rspi_rz_transfer_out_in(struct rspi_data *rspi,
-				   struct spi_transfer *xfer)
-{
-	int remain = xfer->len, ret;
-	const u8 *tx_buf = xfer->tx_buf;
-	u8 *rx_buf = xfer->rx_buf;
-	u8 data;
-
-	rspi_rz_receive_init(rspi);
-
-	while (remain > 0) {
-		data = tx_buf ? *tx_buf++ : DUMMY_DATA;
-		ret = rspi_data_out_in(rspi, data);
-		if (ret < 0)
-			return ret;
-		if (rx_buf)
-			*rx_buf++ = ret;
-		remain--;
+	spcr = rspi_read8(rspi, RSPI_SPCR);
+	if (xfer->rx_buf) {
+		rspi_receive_init(rspi);
+		spcr &= ~SPCR_TXMD;
+	} else {
+		spcr |= SPCR_TXMD;
 	}
+	rspi_write8(rspi, spcr, RSPI_SPCR);
 
-	/* Wait for the last transmission */
-	rspi_wait_for_interrupt(rspi, SPSR_SPTEF, SPCR_SPTIE);
-
-	return 0;
+	return rspi_common_transfer(rspi, xfer);
 }
 
 static int rspi_rz_transfer_one(struct spi_master *master,
@@ -791,68 +630,44 @@ static int rspi_rz_transfer_one(struct spi_master *master,
 				struct spi_transfer *xfer)
 {
 	struct rspi_data *rspi = spi_master_get_devdata(master);
+	int ret;
 
-	return rspi_rz_transfer_out_in(rspi, xfer);
+	rspi_rz_receive_init(rspi);
+
+	return rspi_common_transfer(rspi, xfer);
 }
 
 static int qspi_transfer_out_in(struct rspi_data *rspi,
 				struct spi_transfer *xfer)
 {
-	int remain = xfer->len, ret;
-	const u8 *tx_buf = xfer->tx_buf;
-	u8 *rx_buf = xfer->rx_buf;
-	u8 data;
-
 	qspi_receive_init(rspi);
 
-	while (remain > 0) {
-		data = tx_buf ? *tx_buf++ : DUMMY_DATA;
-		ret = rspi_data_out_in(rspi, data);
-		if (ret < 0)
-			return ret;
-		if (rx_buf)
-			*rx_buf++ = ret;
-		remain--;
-	}
-
-	/* Wait for the last transmission */
-	rspi_wait_for_interrupt(rspi, SPSR_SPTEF, SPCR_SPTIE);
-
-	return 0;
+	return rspi_common_transfer(rspi, xfer);
 }
 
 static int qspi_transfer_out(struct rspi_data *rspi, struct spi_transfer *xfer)
 {
-	const u8 *buf = xfer->tx_buf;
-	unsigned int i;
 	int ret;
 
-	for (i = 0; i < xfer->len; i++) {
-		ret = rspi_data_out(rspi, *buf++);
-		if (ret < 0)
-			return ret;
-	}
+	if (rspi->master->can_dma && __rspi_can_dma(rspi, xfer))
+		return rspi_dma_transfer(rspi, &xfer->tx_sg, NULL);
+
+	ret = rspi_pio_transfer(rspi, xfer->tx_buf, NULL, xfer->len);
+	if (ret < 0)
+		return ret;
 
 	/* Wait for the last transmission */
-	rspi_wait_for_interrupt(rspi, SPSR_SPTEF, SPCR_SPTIE);
+	rspi_wait_for_tx_empty(rspi);
 
 	return 0;
 }
 
 static int qspi_transfer_in(struct rspi_data *rspi, struct spi_transfer *xfer)
 {
-	u8 *buf = xfer->rx_buf;
-	unsigned int i;
-	int ret;
-
-	for (i = 0; i < xfer->len; i++) {
-		ret = rspi_data_in(rspi);
-		if (ret < 0)
-			return ret;
-		*buf++ = ret;
-	}
+	if (rspi->master->can_dma && __rspi_can_dma(rspi, xfer))
+		return rspi_dma_transfer(rspi, NULL, &xfer->rx_sg);
 
-	return 0;
+	return rspi_pio_transfer(rspi, NULL, xfer->rx_buf, xfer->len);
 }
 
 static int qspi_transfer_one(struct spi_master *master, struct spi_device *spi,
@@ -862,10 +677,10 @@ static int qspi_transfer_one(struct spi_master *master, struct spi_device *spi,
 
 	if (spi->mode & SPI_LOOP) {
 		return qspi_transfer_out_in(rspi, xfer);
-	} else if (xfer->tx_buf && xfer->tx_nbits > SPI_NBITS_SINGLE) {
+	} else if (xfer->tx_nbits > SPI_NBITS_SINGLE) {
 		/* Quad or Dual SPI Write */
 		return qspi_transfer_out(rspi, xfer);
-	} else if (xfer->rx_buf && xfer->rx_nbits > SPI_NBITS_SINGLE) {
+	} else if (xfer->rx_nbits > SPI_NBITS_SINGLE) {
 		/* Quad or Dual SPI Read */
 		return qspi_transfer_in(rspi, xfer);
 	} else {
@@ -1046,65 +861,78 @@ static irqreturn_t rspi_irq_tx(int irq, void *_sr)
 	return 0;
 }
 
-static int rspi_request_dma(struct rspi_data *rspi,
-				      struct platform_device *pdev)
+static struct dma_chan *rspi_request_dma_chan(struct device *dev,
+					      enum dma_transfer_direction dir,
+					      unsigned int id,
+					      dma_addr_t port_addr)
 {
-	const struct rspi_plat_data *rspi_pd = dev_get_platdata(&pdev->dev);
-	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	dma_cap_mask_t mask;
+	struct dma_chan *chan;
 	struct dma_slave_config cfg;
 	int ret;
 
-	if (!res || !rspi_pd)
-		return 0;	/* The driver assumes no error. */
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
 
-	rspi->dma_width_16bit = rspi_pd->dma_width_16bit;
-
-	/* If the module receives data by DMAC, it also needs TX DMAC */
-	if (rspi_pd->dma_rx_id && rspi_pd->dma_tx_id) {
-		dma_cap_zero(mask);
-		dma_cap_set(DMA_SLAVE, mask);
-		rspi->chan_rx = dma_request_channel(mask, shdma_chan_filter,
-						    (void *)rspi_pd->dma_rx_id);
-		if (rspi->chan_rx) {
-			cfg.slave_id = rspi_pd->dma_rx_id;
-			cfg.direction = DMA_DEV_TO_MEM;
-			cfg.dst_addr = 0;
-			cfg.src_addr = res->start + RSPI_SPDR;
-			ret = dmaengine_slave_config(rspi->chan_rx, &cfg);
-			if (!ret)
-				dev_info(&pdev->dev, "Use DMA when rx.\n");
-			else
-				return ret;
-		}
+	chan = dma_request_channel(mask, shdma_chan_filter,
+				   (void *)(unsigned long)id);
+	if (!chan) {
+		dev_warn(dev, "dma_request_channel failed\n");
+		return NULL;
 	}
-	if (rspi_pd->dma_tx_id) {
-		dma_cap_zero(mask);
-		dma_cap_set(DMA_SLAVE, mask);
-		rspi->chan_tx = dma_request_channel(mask, shdma_chan_filter,
-						    (void *)rspi_pd->dma_tx_id);
-		if (rspi->chan_tx) {
-			cfg.slave_id = rspi_pd->dma_tx_id;
-			cfg.direction = DMA_MEM_TO_DEV;
-			cfg.dst_addr = res->start + RSPI_SPDR;
-			cfg.src_addr = 0;
-			ret = dmaengine_slave_config(rspi->chan_tx, &cfg);
-			if (!ret)
-				dev_info(&pdev->dev, "Use DMA when tx\n");
-			else
-				return ret;
-		}
+
+	memset(&cfg, 0, sizeof(cfg));
+	cfg.slave_id = id;
+	cfg.direction = dir;
+	if (dir == DMA_MEM_TO_DEV)
+		cfg.dst_addr = port_addr;
+	else
+		cfg.src_addr = port_addr;
+
+	ret = dmaengine_slave_config(chan, &cfg);
+	if (ret) {
+		dev_warn(dev, "dmaengine_slave_config failed %d\n", ret);
+		dma_release_channel(chan);
+		return NULL;
 	}
 
+	return chan;
+}
+
+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);
+
+	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,
+					       res->start + RSPI_SPDR);
+	if (!master->dma_rx)
+		return -ENODEV;
+
+	master->dma_tx = rspi_request_dma_chan(dev, DMA_MEM_TO_DEV,
+					       rspi_pd->dma_tx_id,
+					       res->start + RSPI_SPDR);
+	if (!master->dma_tx) {
+		dma_release_channel(master->dma_rx);
+		master->dma_rx = NULL;
+		return -ENODEV;
+	}
+
+	master->can_dma = rspi_can_dma;
+	dev_info(dev, "DMA available");
 	return 0;
 }
 
 static void rspi_release_dma(struct rspi_data *rspi)
 {
-	if (rspi->chan_tx)
-		dma_release_channel(rspi->chan_tx);
-	if (rspi->chan_rx)
-		dma_release_channel(rspi->chan_rx);
+	if (rspi->master->dma_tx)
+		dma_release_channel(rspi->master->dma_tx);
+	if (rspi->master->dma_rx)
+		dma_release_channel(rspi->master->dma_rx);
 }
 
 static int rspi_remove(struct platform_device *pdev)
@@ -1118,23 +946,29 @@ static int rspi_remove(struct platform_device *pdev)
 }
 
 static const struct spi_ops rspi_ops = {
-	.set_config_register =		rspi_set_config_register,
-	.transfer_one =			rspi_transfer_one,
-	.mode_bits =			SPI_CPHA | SPI_CPOL | SPI_LOOP,
+	.set_config_register =	rspi_set_config_register,
+	.transfer_one =		rspi_transfer_one,
+	.mode_bits =		SPI_CPHA | SPI_CPOL | SPI_LOOP,
+	.flags =		SPI_MASTER_MUST_TX,
+	.fifo_size =		8,
 };
 
 static const struct spi_ops rspi_rz_ops = {
-	.set_config_register =		rspi_rz_set_config_register,
-	.transfer_one =			rspi_rz_transfer_one,
-	.mode_bits =			SPI_CPHA | SPI_CPOL | SPI_LOOP,
+	.set_config_register =	rspi_rz_set_config_register,
+	.transfer_one =		rspi_rz_transfer_one,
+	.mode_bits =		SPI_CPHA | SPI_CPOL | SPI_LOOP,
+	.flags =		SPI_MASTER_MUST_RX | SPI_MASTER_MUST_TX,
+	.fifo_size =		8,	/* 8 for TX, 32 for RX */
 };
 
 static const struct spi_ops qspi_ops = {
-	.set_config_register =		qspi_set_config_register,
-	.transfer_one =			qspi_transfer_one,
-	.mode_bits =			SPI_CPHA | SPI_CPOL | SPI_LOOP |
-					SPI_TX_DUAL | SPI_TX_QUAD |
-					SPI_RX_DUAL | SPI_RX_QUAD,
+	.set_config_register =	qspi_set_config_register,
+	.transfer_one =		qspi_transfer_one,
+	.mode_bits =		SPI_CPHA | SPI_CPOL | SPI_LOOP |
+				SPI_TX_DUAL | SPI_TX_QUAD |
+				SPI_RX_DUAL | SPI_RX_QUAD,
+	.flags =		SPI_MASTER_MUST_RX | SPI_MASTER_MUST_TX,
+	.fifo_size =		32,
 };
 
 #ifdef CONFIG_OF
@@ -1254,6 +1088,7 @@ static int rspi_probe(struct platform_device *pdev)
 	master->prepare_message = rspi_prepare_message;
 	master->unprepare_message = rspi_unprepare_message;
 	master->mode_bits = ops->mode_bits;
+	master->flags = ops->flags;
 	master->dev.of_node = pdev->dev.of_node;
 
 	ret = platform_get_irq_byname(pdev, "rx");
@@ -1291,11 +1126,9 @@ static int rspi_probe(struct platform_device *pdev)
 		goto error2;
 	}
 
-	ret = rspi_request_dma(rspi, pdev);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "rspi_request_dma failed.\n");
-		goto error3;
-	}
+	ret = rspi_request_dma(&pdev->dev, master, res);
+	if (ret < 0)
+		dev_warn(&pdev->dev, "DMA not available, using PIO\n");
 
 	ret = devm_spi_register_master(&pdev->dev, master);
 	if (ret < 0) {
diff --git a/drivers/spi/spi-s3c24xx.c b/drivers/spi/spi-s3c24xx.c
index bed23384dfa..e713737d784 100644
--- a/drivers/spi/spi-s3c24xx.c
+++ b/drivers/spi/spi-s3c24xx.c
@@ -10,7 +10,6 @@
 */
 
 #include <linux/spinlock.h>
-#include <linux/workqueue.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/errno.h>
@@ -183,11 +182,11 @@ static int s3c24xx_spi_setup(struct spi_device *spi)
 
 	/* allocate settings on the first call */
 	if (!cs) {
-		cs = kzalloc(sizeof(struct s3c24xx_spi_devstate), GFP_KERNEL);
-		if (!cs) {
-			dev_err(&spi->dev, "no memory for controller state\n");
+		cs = devm_kzalloc(&spi->dev,
+				  sizeof(struct s3c24xx_spi_devstate),
+				  GFP_KERNEL);
+		if (!cs)
 			return -ENOMEM;
-		}
 
 		cs->spcon = SPCON_DEFAULT;
 		cs->hz = -1;
@@ -209,11 +208,6 @@ static int s3c24xx_spi_setup(struct spi_device *spi)
 	return 0;
 }
 
-static void s3c24xx_spi_cleanup(struct spi_device *spi)
-{
-	kfree(spi->controller_state);
-}
-
 static inline unsigned int hw_txbyte(struct s3c24xx_spi *hw, int count)
 {
 	return hw->tx ? hw->tx[count] : 0;
@@ -543,7 +537,6 @@ static int s3c24xx_spi_probe(struct platform_device *pdev)
 	hw->bitbang.txrx_bufs      = s3c24xx_spi_txrx;
 
 	hw->master->setup  = s3c24xx_spi_setup;
-	hw->master->cleanup = s3c24xx_spi_cleanup;
 
 	dev_dbg(hw->dev, "bitbang at %p\n", &hw->bitbang);
 
diff --git a/drivers/spi/spi-s3c64xx.c b/drivers/spi/spi-s3c64xx.c
index f19cd97855e..75a56968b14 100644
--- a/drivers/spi/spi-s3c64xx.c
+++ b/drivers/spi/spi-s3c64xx.c
@@ -19,7 +19,6 @@
 
 #include <linux/init.h>
 #include <linux/module.h>
-#include <linux/workqueue.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/clk.h>
@@ -773,7 +772,6 @@ static struct s3c64xx_spi_csinfo *s3c64xx_get_slave_ctrldata(
 
 	cs = kzalloc(sizeof(*cs), GFP_KERNEL);
 	if (!cs) {
-		dev_err(&spi->dev, "could not allocate memory for controller data\n");
 		of_node_put(data_np);
 		return ERR_PTR(-ENOMEM);
 	}
@@ -987,10 +985,8 @@ static struct s3c64xx_spi_info *s3c64xx_spi_parse_dt(struct device *dev)
 	u32 temp;
 
 	sci = devm_kzalloc(dev, sizeof(*sci), GFP_KERNEL);
-	if (!sci) {
-		dev_err(dev, "memory allocation for spi_info failed\n");
+	if (!sci)
 		return ERR_PTR(-ENOMEM);
-	}
 
 	if (of_property_read_u32(dev->of_node, "samsung,spi-src-clk", &temp)) {
 		dev_warn(dev, "spi bus clock parent not specified, using clock at index 0 as parent\n");
diff --git a/drivers/spi/spi-sh-msiof.c b/drivers/spi/spi-sh-msiof.c
index e850d03e719..45b09142afe 100644
--- a/drivers/spi/spi-sh-msiof.c
+++ b/drivers/spi/spi-sh-msiof.c
@@ -642,10 +642,8 @@ static struct sh_msiof_spi_info *sh_msiof_spi_parse_dt(struct device *dev)
 	u32 num_cs = 1;
 
 	info = devm_kzalloc(dev, sizeof(struct sh_msiof_spi_info), GFP_KERNEL);
-	if (!info) {
-		dev_err(dev, "failed to allocate setup data\n");
+	if (!info)
 		return NULL;
-	}
 
 	/* Parse the MSIOF properties */
 	of_property_read_u32(np, "num-cs", &num_cs);
diff --git a/drivers/spi/spi-sh-sci.c b/drivers/spi/spi-sh-sci.c
index 8b44b71f502..1f56ef651d1 100644
--- a/drivers/spi/spi-sh-sci.c
+++ b/drivers/spi/spi-sh-sci.c
@@ -16,7 +16,6 @@
 #include <linux/kernel.h>
 #include <linux/delay.h>
 #include <linux/spinlock.h>
-#include <linux/workqueue.h>
 #include <linux/platform_device.h>
 
 #include <linux/spi/spi.h>
diff --git a/drivers/spi/spi-sirf.c b/drivers/spi/spi-sirf.c
index 67d8909dcf3..95ac276eaaf 100644
--- a/drivers/spi/spi-sirf.c
+++ b/drivers/spi/spi-sirf.c
@@ -10,6 +10,7 @@
 #include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/clk.h>
+#include <linux/completion.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/of.h>
@@ -85,6 +86,7 @@
 #define SIRFSOC_SPI_TX_DONE		BIT(1)
 #define SIRFSOC_SPI_RX_OFLOW		BIT(2)
 #define SIRFSOC_SPI_TX_UFLOW		BIT(3)
+#define SIRFSOC_SPI_RX_IO_DMA		BIT(4)
 #define SIRFSOC_SPI_RX_FIFO_FULL	BIT(6)
 #define SIRFSOC_SPI_TXFIFO_EMPTY	BIT(7)
 #define SIRFSOC_SPI_RXFIFO_THD_REACH	BIT(8)
@@ -264,41 +266,34 @@ static irqreturn_t spi_sirfsoc_irq(int irq, void *dev_id)
 {
 	struct sirfsoc_spi *sspi = dev_id;
 	u32 spi_stat = readl(sspi->base + SIRFSOC_SPI_INT_STATUS);
-
-	writel(spi_stat, sspi->base + SIRFSOC_SPI_INT_STATUS);
-
 	if (sspi->tx_by_cmd && (spi_stat & SIRFSOC_SPI_FRM_END)) {
 		complete(&sspi->tx_done);
 		writel(0x0, sspi->base + SIRFSOC_SPI_INT_EN);
+		writel(SIRFSOC_SPI_INT_MASK_ALL,
+				sspi->base + SIRFSOC_SPI_INT_STATUS);
 		return IRQ_HANDLED;
 	}
 
 	/* Error Conditions */
 	if (spi_stat & SIRFSOC_SPI_RX_OFLOW ||
 			spi_stat & SIRFSOC_SPI_TX_UFLOW) {
+		complete(&sspi->tx_done);
 		complete(&sspi->rx_done);
 		writel(0x0, sspi->base + SIRFSOC_SPI_INT_EN);
+		writel(SIRFSOC_SPI_INT_MASK_ALL,
+				sspi->base + SIRFSOC_SPI_INT_STATUS);
+		return IRQ_HANDLED;
 	}
+	if (spi_stat & SIRFSOC_SPI_TXFIFO_EMPTY)
+		complete(&sspi->tx_done);
+	while (!(readl(sspi->base + SIRFSOC_SPI_INT_STATUS) &
+		SIRFSOC_SPI_RX_IO_DMA))
+		cpu_relax();
+	complete(&sspi->rx_done);
+	writel(0x0, sspi->base + SIRFSOC_SPI_INT_EN);
+	writel(SIRFSOC_SPI_INT_MASK_ALL,
+			sspi->base + SIRFSOC_SPI_INT_STATUS);
 
-	if (spi_stat & (SIRFSOC_SPI_FRM_END
-			| SIRFSOC_SPI_RXFIFO_THD_REACH))
-		while (!((readl(sspi->base + SIRFSOC_SPI_RXFIFO_STATUS)
-				& SIRFSOC_SPI_FIFO_EMPTY)) &&
-				sspi->left_rx_word)
-			sspi->rx_word(sspi);
-
-	if (spi_stat & (SIRFSOC_SPI_TXFIFO_EMPTY |
-			SIRFSOC_SPI_TXFIFO_THD_REACH))
-		while (!((readl(sspi->base + SIRFSOC_SPI_TXFIFO_STATUS)
-				& SIRFSOC_SPI_FIFO_FULL)) &&
-				sspi->left_tx_word)
-			sspi->tx_word(sspi);
-
-	/* Received all words */
-	if ((sspi->left_rx_word == 0) && (sspi->left_tx_word == 0)) {
-		complete(&sspi->rx_done);
-		writel(0x0, sspi->base + SIRFSOC_SPI_INT_EN);
-	}
 	return IRQ_HANDLED;
 }
 
@@ -309,59 +304,51 @@ static void spi_sirfsoc_dma_fini_callback(void *data)
 	complete(dma_complete);
 }
 
-static int spi_sirfsoc_transfer(struct spi_device *spi, struct spi_transfer *t)
+static int spi_sirfsoc_cmd_transfer(struct spi_device *spi,
+	struct spi_transfer *t)
 {
 	struct sirfsoc_spi *sspi;
 	int timeout = t->len * 10;
-	sspi = spi_master_get_devdata(spi->master);
+	u32 cmd;
 
-	sspi->tx = t->tx_buf ? t->tx_buf : sspi->dummypage;
-	sspi->rx = t->rx_buf ? t->rx_buf : sspi->dummypage;
-	sspi->left_tx_word = sspi->left_rx_word = t->len / sspi->word_width;
-	reinit_completion(&sspi->rx_done);
-	reinit_completion(&sspi->tx_done);
-
-	writel(SIRFSOC_SPI_INT_MASK_ALL, sspi->base + SIRFSOC_SPI_INT_STATUS);
-
-	/*
-	 * fill tx_buf into command register and wait for its completion
-	 */
-	if (sspi->tx_by_cmd) {
-		u32 cmd;
-		memcpy(&cmd, sspi->tx, t->len);
-
-		if (sspi->word_width == 1 && !(spi->mode & SPI_LSB_FIRST))
-			cmd = cpu_to_be32(cmd) >>
-				((SIRFSOC_MAX_CMD_BYTES - t->len) * 8);
-		if (sspi->word_width == 2 && t->len == 4 &&
-				(!(spi->mode & SPI_LSB_FIRST)))
-			cmd = ((cmd & 0xffff) << 16) | (cmd >> 16);
-
-		writel(cmd, sspi->base + SIRFSOC_SPI_CMD);
-		writel(SIRFSOC_SPI_FRM_END_INT_EN,
-			sspi->base + SIRFSOC_SPI_INT_EN);
-		writel(SIRFSOC_SPI_CMD_TX_EN,
-			sspi->base + SIRFSOC_SPI_TX_RX_EN);
+	sspi = spi_master_get_devdata(spi->master);
+	memcpy(&cmd, sspi->tx, t->len);
+	if (sspi->word_width == 1 && !(spi->mode & SPI_LSB_FIRST))
+		cmd = cpu_to_be32(cmd) >>
+			((SIRFSOC_MAX_CMD_BYTES - t->len) * 8);
+	if (sspi->word_width == 2 && t->len == 4 &&
+			(!(spi->mode & SPI_LSB_FIRST)))
+		cmd = ((cmd & 0xffff) << 16) | (cmd >> 16);
+	writel(cmd, sspi->base + SIRFSOC_SPI_CMD);
+	writel(SIRFSOC_SPI_FRM_END_INT_EN,
+		sspi->base + SIRFSOC_SPI_INT_EN);
+	writel(SIRFSOC_SPI_CMD_TX_EN,
+		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;
+	}
 
-		if (wait_for_completion_timeout(&sspi->tx_done, timeout) == 0) {
-			dev_err(&spi->dev, "transfer timeout\n");
-			return 0;
-		}
+	return t->len;
+}
 
-		return t->len;
-	}
+static void spi_sirfsoc_dma_transfer(struct spi_device *spi,
+	struct spi_transfer *t)
+{
+	struct sirfsoc_spi *sspi;
+	struct dma_async_tx_descriptor *rx_desc, *tx_desc;
+	int timeout = t->len * 10;
 
-	if (sspi->left_tx_word == 1) {
-		writel(readl(sspi->base + SIRFSOC_SPI_CTRL) |
-			SIRFSOC_SPI_ENA_AUTO_CLR,
-			sspi->base + SIRFSOC_SPI_CTRL);
-		writel(0, sspi->base + SIRFSOC_SPI_TX_DMA_IO_LEN);
-		writel(0, sspi->base + SIRFSOC_SPI_RX_DMA_IO_LEN);
-	} else if ((sspi->left_tx_word > 1) && (sspi->left_tx_word <
-				SIRFSOC_SPI_DAT_FRM_LEN_MAX)) {
+	sspi = spi_master_get_devdata(spi->master);
+	writel(SIRFSOC_SPI_FIFO_RESET, sspi->base + SIRFSOC_SPI_RXFIFO_OP);
+	writel(SIRFSOC_SPI_FIFO_RESET, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
+	writel(SIRFSOC_SPI_FIFO_START, sspi->base + SIRFSOC_SPI_RXFIFO_OP);
+	writel(SIRFSOC_SPI_FIFO_START, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
+	writel(0, sspi->base + SIRFSOC_SPI_INT_EN);
+	writel(SIRFSOC_SPI_INT_MASK_ALL, sspi->base + SIRFSOC_SPI_INT_STATUS);
+	if (sspi->left_tx_word < SIRFSOC_SPI_DAT_FRM_LEN_MAX) {
 		writel(readl(sspi->base + SIRFSOC_SPI_CTRL) |
-				SIRFSOC_SPI_MUL_DAT_MODE |
-				SIRFSOC_SPI_ENA_AUTO_CLR,
+			SIRFSOC_SPI_ENA_AUTO_CLR | SIRFSOC_SPI_MUL_DAT_MODE,
 			sspi->base + SIRFSOC_SPI_CTRL);
 		writel(sspi->left_tx_word - 1,
 				sspi->base + SIRFSOC_SPI_TX_DMA_IO_LEN);
@@ -373,76 +360,122 @@ static int spi_sirfsoc_transfer(struct spi_device *spi, struct spi_transfer *t)
 		writel(0, sspi->base + SIRFSOC_SPI_TX_DMA_IO_LEN);
 		writel(0, sspi->base + SIRFSOC_SPI_RX_DMA_IO_LEN);
 	}
-
-	writel(SIRFSOC_SPI_FIFO_RESET, sspi->base + SIRFSOC_SPI_RXFIFO_OP);
-	writel(SIRFSOC_SPI_FIFO_RESET, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
-	writel(SIRFSOC_SPI_FIFO_START, sspi->base + SIRFSOC_SPI_RXFIFO_OP);
-	writel(SIRFSOC_SPI_FIFO_START, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
-
-	if (IS_DMA_VALID(t)) {
-		struct dma_async_tx_descriptor *rx_desc, *tx_desc;
-
-		sspi->dst_start = dma_map_single(&spi->dev, sspi->rx, t->len, DMA_FROM_DEVICE);
-		rx_desc = dmaengine_prep_slave_single(sspi->rx_chan,
-			sspi->dst_start, t->len, DMA_DEV_TO_MEM,
-			DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
-		rx_desc->callback = spi_sirfsoc_dma_fini_callback;
-		rx_desc->callback_param = &sspi->rx_done;
-
-		sspi->src_start = dma_map_single(&spi->dev, (void *)sspi->tx, t->len, DMA_TO_DEVICE);
-		tx_desc = dmaengine_prep_slave_single(sspi->tx_chan,
-			sspi->src_start, t->len, DMA_MEM_TO_DEV,
-			DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
-		tx_desc->callback = spi_sirfsoc_dma_fini_callback;
-		tx_desc->callback_param = &sspi->tx_done;
-
-		dmaengine_submit(tx_desc);
-		dmaengine_submit(rx_desc);
-		dma_async_issue_pending(sspi->tx_chan);
-		dma_async_issue_pending(sspi->rx_chan);
-	} else {
-		/* Send the first word to trigger the whole tx/rx process */
-		sspi->tx_word(sspi);
-
-		writel(SIRFSOC_SPI_RX_OFLOW_INT_EN | SIRFSOC_SPI_TX_UFLOW_INT_EN |
-			SIRFSOC_SPI_RXFIFO_THD_INT_EN | SIRFSOC_SPI_TXFIFO_THD_INT_EN |
-			SIRFSOC_SPI_FRM_END_INT_EN | SIRFSOC_SPI_RXFIFO_FULL_INT_EN |
-			SIRFSOC_SPI_TXFIFO_EMPTY_INT_EN, sspi->base + SIRFSOC_SPI_INT_EN);
-	}
-
-	writel(SIRFSOC_SPI_RX_EN | SIRFSOC_SPI_TX_EN, sspi->base + SIRFSOC_SPI_TX_RX_EN);
-
-	if (!IS_DMA_VALID(t)) { /* for PIO */
-		if (wait_for_completion_timeout(&sspi->rx_done, timeout) == 0)
-			dev_err(&spi->dev, "transfer timeout\n");
-	} else if (wait_for_completion_timeout(&sspi->rx_done, timeout) == 0) {
+	sspi->dst_start = dma_map_single(&spi->dev, sspi->rx, t->len,
+					(t->tx_buf != t->rx_buf) ?
+					DMA_FROM_DEVICE : DMA_BIDIRECTIONAL);
+	rx_desc = dmaengine_prep_slave_single(sspi->rx_chan,
+		sspi->dst_start, t->len, DMA_DEV_TO_MEM,
+		DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	rx_desc->callback = spi_sirfsoc_dma_fini_callback;
+	rx_desc->callback_param = &sspi->rx_done;
+
+	sspi->src_start = dma_map_single(&spi->dev, (void *)sspi->tx, t->len,
+					(t->tx_buf != t->rx_buf) ?
+					DMA_TO_DEVICE : DMA_BIDIRECTIONAL);
+	tx_desc = dmaengine_prep_slave_single(sspi->tx_chan,
+		sspi->src_start, t->len, DMA_MEM_TO_DEV,
+		DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	tx_desc->callback = spi_sirfsoc_dma_fini_callback;
+	tx_desc->callback_param = &sspi->tx_done;
+
+	dmaengine_submit(tx_desc);
+	dmaengine_submit(rx_desc);
+	dma_async_issue_pending(sspi->tx_chan);
+	dma_async_issue_pending(sspi->rx_chan);
+	writel(SIRFSOC_SPI_RX_EN | SIRFSOC_SPI_TX_EN,
+			sspi->base + SIRFSOC_SPI_TX_RX_EN);
+	if (wait_for_completion_timeout(&sspi->rx_done, timeout) == 0) {
 		dev_err(&spi->dev, "transfer timeout\n");
 		dmaengine_terminate_all(sspi->rx_chan);
 	} else
 		sspi->left_rx_word = 0;
-
 	/*
 	 * we only wait tx-done event if transferring by DMA. for PIO,
 	 * we get rx data by writing tx data, so if rx is done, tx has
 	 * done earlier
 	 */
-	if (IS_DMA_VALID(t)) {
-		if (wait_for_completion_timeout(&sspi->tx_done, timeout) == 0) {
-			dev_err(&spi->dev, "transfer timeout\n");
-			dmaengine_terminate_all(sspi->tx_chan);
-		}
-	}
-
-	if (IS_DMA_VALID(t)) {
-		dma_unmap_single(&spi->dev, sspi->src_start, t->len, DMA_TO_DEVICE);
-		dma_unmap_single(&spi->dev, sspi->dst_start, t->len, DMA_FROM_DEVICE);
+	if (wait_for_completion_timeout(&sspi->tx_done, timeout) == 0) {
+		dev_err(&spi->dev, "transfer timeout\n");
+		dmaengine_terminate_all(sspi->tx_chan);
 	}
-
+	dma_unmap_single(&spi->dev, sspi->src_start, t->len, DMA_TO_DEVICE);
+	dma_unmap_single(&spi->dev, sspi->dst_start, t->len, DMA_FROM_DEVICE);
 	/* TX, RX FIFO stop */
 	writel(0, sspi->base + SIRFSOC_SPI_RXFIFO_OP);
 	writel(0, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
-	writel(0, sspi->base + SIRFSOC_SPI_TX_RX_EN);
-	writel(0, sspi->base + SIRFSOC_SPI_INT_EN);
+	if (sspi->left_tx_word >= SIRFSOC_SPI_DAT_FRM_LEN_MAX)
+		writel(0, sspi->base + SIRFSOC_SPI_TX_RX_EN);
+}
+
+static void spi_sirfsoc_pio_transfer(struct spi_device *spi,
+		struct spi_transfer *t)
+{
+	struct sirfsoc_spi *sspi;
+	int timeout = t->len * 10;
+
+	sspi = spi_master_get_devdata(spi->master);
+	do {
+		writel(SIRFSOC_SPI_FIFO_RESET,
+			sspi->base + SIRFSOC_SPI_RXFIFO_OP);
+		writel(SIRFSOC_SPI_FIFO_RESET,
+			sspi->base + SIRFSOC_SPI_TXFIFO_OP);
+		writel(SIRFSOC_SPI_FIFO_START,
+			sspi->base + SIRFSOC_SPI_RXFIFO_OP);
+		writel(SIRFSOC_SPI_FIFO_START,
+			sspi->base + SIRFSOC_SPI_TXFIFO_OP);
+		writel(0, sspi->base + SIRFSOC_SPI_INT_EN);
+		writel(SIRFSOC_SPI_INT_MASK_ALL,
+			sspi->base + SIRFSOC_SPI_INT_STATUS);
+		writel(readl(sspi->base + SIRFSOC_SPI_CTRL) |
+			SIRFSOC_SPI_MUL_DAT_MODE | SIRFSOC_SPI_ENA_AUTO_CLR,
+			sspi->base + SIRFSOC_SPI_CTRL);
+		writel(min(sspi->left_tx_word, (u32)(256 / sspi->word_width))
+				- 1, sspi->base + SIRFSOC_SPI_TX_DMA_IO_LEN);
+		writel(min(sspi->left_rx_word, (u32)(256 / sspi->word_width))
+				- 1, sspi->base + SIRFSOC_SPI_RX_DMA_IO_LEN);
+		while (!((readl(sspi->base + SIRFSOC_SPI_TXFIFO_STATUS)
+			& SIRFSOC_SPI_FIFO_FULL)) && sspi->left_tx_word)
+			sspi->tx_word(sspi);
+		writel(SIRFSOC_SPI_TXFIFO_EMPTY_INT_EN |
+			SIRFSOC_SPI_TX_UFLOW_INT_EN |
+			SIRFSOC_SPI_RX_OFLOW_INT_EN,
+			sspi->base + SIRFSOC_SPI_INT_EN);
+		writel(SIRFSOC_SPI_RX_EN | SIRFSOC_SPI_TX_EN,
+			sspi->base + SIRFSOC_SPI_TX_RX_EN);
+		if (!wait_for_completion_timeout(&sspi->tx_done, timeout) ||
+			!wait_for_completion_timeout(&sspi->rx_done, timeout)) {
+			dev_err(&spi->dev, "transfer timeout\n");
+			break;
+		}
+		while (!((readl(sspi->base + SIRFSOC_SPI_RXFIFO_STATUS)
+			& SIRFSOC_SPI_FIFO_EMPTY)) && sspi->left_rx_word)
+			sspi->rx_word(sspi);
+		writel(0, sspi->base + SIRFSOC_SPI_RXFIFO_OP);
+		writel(0, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
+	} while (sspi->left_tx_word != 0 || sspi->left_rx_word != 0);
+}
+
+static int spi_sirfsoc_transfer(struct spi_device *spi, struct spi_transfer *t)
+{
+	struct sirfsoc_spi *sspi;
+	sspi = spi_master_get_devdata(spi->master);
+
+	sspi->tx = t->tx_buf ? t->tx_buf : sspi->dummypage;
+	sspi->rx = t->rx_buf ? t->rx_buf : sspi->dummypage;
+	sspi->left_tx_word = sspi->left_rx_word = t->len / sspi->word_width;
+	reinit_completion(&sspi->rx_done);
+	reinit_completion(&sspi->tx_done);
+	/*
+	 * in the transfer, if transfer data using command register with rx_buf
+	 * null, just fill command data into command register and wait for its
+	 * completion.
+	 */
+	if (sspi->tx_by_cmd)
+		spi_sirfsoc_cmd_transfer(spi, t);
+	else if (IS_DMA_VALID(t))
+		spi_sirfsoc_dma_transfer(spi, t);
+	else
+		spi_sirfsoc_pio_transfer(spi, t);
 
 	return t->len - sspi->left_rx_word * sspi->word_width;
 }
@@ -512,7 +545,8 @@ spi_sirfsoc_setup_transfer(struct spi_device *spi, struct spi_transfer *t)
 		break;
 	case 12:
 	case 16:
-		regval |= (bits_per_word ==  12) ? SIRFSOC_SPI_TRAN_DAT_FORMAT_12 :
+		regval |= (bits_per_word ==  12) ?
+			SIRFSOC_SPI_TRAN_DAT_FORMAT_12 :
 			SIRFSOC_SPI_TRAN_DAT_FORMAT_16;
 		sspi->rx_word = spi_sirfsoc_rx_word_u16;
 		sspi->tx_word = spi_sirfsoc_tx_word_u16;
@@ -540,8 +574,8 @@ spi_sirfsoc_setup_transfer(struct spi_device *spi, struct spi_transfer *t)
 		regval |= SIRFSOC_SPI_CLK_IDLE_STAT;
 
 	/*
-	 * Data should be driven at least 1/2 cycle before the fetch edge to make
-	 * sure that data gets stable at the fetch edge.
+	 * Data should be driven at least 1/2 cycle before the fetch edge
+	 * to make sure that data gets stable at the fetch edge.
 	 */
 	if (((spi->mode & SPI_CPOL) && (spi->mode & SPI_CPHA)) ||
 	    (!(spi->mode & SPI_CPOL) && !(spi->mode & SPI_CPHA)))
@@ -578,11 +612,14 @@ spi_sirfsoc_setup_transfer(struct spi_device *spi, struct spi_transfer *t)
 	if (IS_DMA_VALID(t)) {
 		/* Enable DMA mode for RX, TX */
 		writel(0, sspi->base + SIRFSOC_SPI_TX_DMA_IO_CTRL);
-		writel(SIRFSOC_SPI_RX_DMA_FLUSH, sspi->base + SIRFSOC_SPI_RX_DMA_IO_CTRL);
+		writel(SIRFSOC_SPI_RX_DMA_FLUSH,
+			sspi->base + SIRFSOC_SPI_RX_DMA_IO_CTRL);
 	} else {
 		/* Enable IO mode for RX, TX */
-		writel(SIRFSOC_SPI_IO_MODE_SEL, sspi->base + SIRFSOC_SPI_TX_DMA_IO_CTRL);
-		writel(SIRFSOC_SPI_IO_MODE_SEL, sspi->base + SIRFSOC_SPI_RX_DMA_IO_CTRL);
+		writel(SIRFSOC_SPI_IO_MODE_SEL,
+			sspi->base + SIRFSOC_SPI_TX_DMA_IO_CTRL);
+		writel(SIRFSOC_SPI_IO_MODE_SEL,
+			sspi->base + SIRFSOC_SPI_RX_DMA_IO_CTRL);
 	}
 
 	return 0;
@@ -612,7 +649,8 @@ static int spi_sirfsoc_probe(struct platform_device *pdev)
 		goto err_cs;
 	}
 
-	master = spi_alloc_master(&pdev->dev, sizeof(*sspi) + sizeof(int) * num_cs);
+	master = spi_alloc_master(&pdev->dev,
+			sizeof(*sspi) + sizeof(int) * num_cs);
 	if (!master) {
 		dev_err(&pdev->dev, "Unable to allocate SPI master\n");
 		return -ENOMEM;
@@ -808,8 +846,7 @@ static struct platform_driver spi_sirfsoc_driver = {
 	.remove = spi_sirfsoc_remove,
 };
 module_platform_driver(spi_sirfsoc_driver);
-
 MODULE_DESCRIPTION("SiRF SoC SPI master driver");
-MODULE_AUTHOR("Zhiwu Song <Zhiwu.Song@csr.com>, "
-		"Barry Song <Baohua.Song@csr.com>");
+MODULE_AUTHOR("Zhiwu Song <Zhiwu.Song@csr.com>");
+MODULE_AUTHOR("Barry Song <Baohua.Song@csr.com>");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/spi/spi-sun4i.c b/drivers/spi/spi-sun4i.c
index d266a870206..85204c93f3d 100644
--- a/drivers/spi/spi-sun4i.c
+++ b/drivers/spi/spi-sun4i.c
@@ -19,7 +19,6 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
-#include <linux/workqueue.h>
 
 #include <linux/spi/spi.h>
 
diff --git a/drivers/spi/spi-sun6i.c b/drivers/spi/spi-sun6i.c
index b3e3498a7e6..bd24093f403 100644
--- a/drivers/spi/spi-sun6i.c
+++ b/drivers/spi/spi-sun6i.c
@@ -20,7 +20,6 @@
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/reset.h>
-#include <linux/workqueue.h>
 
 #include <linux/spi/spi.h>
 
diff --git a/drivers/spi/spi-tegra114.c b/drivers/spi/spi-tegra114.c
index 40064959550..e4a85ada861 100644
--- a/drivers/spi/spi-tegra114.c
+++ b/drivers/spi/spi-tegra114.c
@@ -1012,7 +1012,7 @@ static irqreturn_t tegra_spi_isr(int irq, void *context_data)
 	return IRQ_WAKE_THREAD;
 }
 
-static struct of_device_id tegra_spi_of_match[] = {
+static const struct of_device_id tegra_spi_of_match[] = {
 	{ .compatible = "nvidia,tegra114-spi", },
 	{}
 };
diff --git a/drivers/spi/spi-tegra20-sflash.c b/drivers/spi/spi-tegra20-sflash.c
index 47869ea636e..3548ce25c08 100644
--- a/drivers/spi/spi-tegra20-sflash.c
+++ b/drivers/spi/spi-tegra20-sflash.c
@@ -419,7 +419,7 @@ static irqreturn_t tegra_sflash_isr(int irq, void *context_data)
 	return handle_cpu_based_xfer(tsd);
 }
 
-static struct of_device_id tegra_sflash_of_match[] = {
+static const struct of_device_id tegra_sflash_of_match[] = {
 	{ .compatible = "nvidia,tegra20-sflash", },
 	{}
 };
diff --git a/drivers/spi/spi-tegra20-slink.c b/drivers/spi/spi-tegra20-slink.c
index e3c1b93e45d..0b9e32e9f49 100644
--- a/drivers/spi/spi-tegra20-slink.c
+++ b/drivers/spi/spi-tegra20-slink.c
@@ -1001,7 +1001,7 @@ static const struct tegra_slink_chip_data tegra20_spi_cdata = {
 	.cs_hold_time = false,
 };
 
-static struct of_device_id tegra_slink_of_match[] = {
+static const struct of_device_id tegra_slink_of_match[] = {
 	{ .compatible = "nvidia,tegra30-slink", .data = &tegra30_spi_cdata, },
 	{ .compatible = "nvidia,tegra20-slink", .data = &tegra20_spi_cdata, },
 	{}
diff --git a/drivers/spi/spi-tle62x0.c b/drivers/spi/spi-tle62x0.c
index 2d4010d8082..daf5aa1c24c 100644
--- a/drivers/spi/spi-tle62x0.c
+++ b/drivers/spi/spi-tle62x0.c
@@ -253,10 +253,8 @@ static int tle62x0_probe(struct spi_device *spi)
 	}
 
 	st = kzalloc(sizeof(struct tle62x0_state), GFP_KERNEL);
-	if (st == NULL) {
-		dev_err(&spi->dev, "no memory for device state\n");
+	if (st == NULL)
 		return -ENOMEM;
-	}
 
 	st->us = spi;
 	st->nr_gpio = pdata->gpio_count;
diff --git a/drivers/spi/spi-topcliff-pch.c b/drivers/spi/spi-topcliff-pch.c
index f406b30af96..f05abf89c06 100644
--- a/drivers/spi/spi-topcliff-pch.c
+++ b/drivers/spi/spi-topcliff-pch.c
@@ -1578,14 +1578,11 @@ static int pch_spi_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	struct pch_pd_dev_save *pd_dev_save;
 
 	pd_dev_save = kzalloc(sizeof(struct pch_pd_dev_save), GFP_KERNEL);
-	if (!pd_dev_save) {
-		dev_err(&pdev->dev, "%s Can't allocate pd_dev_sav\n", __func__);
+	if (!pd_dev_save)
 		return -ENOMEM;
-	}
 
 	board_dat = kzalloc(sizeof(struct pch_spi_board_data), GFP_KERNEL);
 	if (!board_dat) {
-		dev_err(&pdev->dev, "%s Can't allocate board_dat\n", __func__);
 		retval = -ENOMEM;
 		goto err_no_mem;
 	}
diff --git a/drivers/spi/spi.c b/drivers/spi/spi.c
index 4eb9bf02996..d4f9670b51b 100644
--- a/drivers/spi/spi.c
+++ b/drivers/spi/spi.c
@@ -580,6 +580,7 @@ static void spi_set_cs(struct spi_device *spi, bool enable)
 		spi->master->set_cs(spi, !enable);
 }
 
+#ifdef CONFIG_HAS_DMA
 static int spi_map_buf(struct spi_master *master, struct device *dev,
 		       struct sg_table *sgt, void *buf, size_t len,
 		       enum dma_data_direction dir)
@@ -637,55 +638,12 @@ static void spi_unmap_buf(struct spi_master *master, struct device *dev,
 	}
 }
 
-static int spi_map_msg(struct spi_master *master, struct spi_message *msg)
+static int __spi_map_msg(struct spi_master *master, struct spi_message *msg)
 {
 	struct device *tx_dev, *rx_dev;
 	struct spi_transfer *xfer;
-	void *tmp;
-	unsigned int max_tx, max_rx;
 	int ret;
 
-	if (master->flags & (SPI_MASTER_MUST_RX | SPI_MASTER_MUST_TX)) {
-		max_tx = 0;
-		max_rx = 0;
-
-		list_for_each_entry(xfer, &msg->transfers, transfer_list) {
-			if ((master->flags & SPI_MASTER_MUST_TX) &&
-			    !xfer->tx_buf)
-				max_tx = max(xfer->len, max_tx);
-			if ((master->flags & SPI_MASTER_MUST_RX) &&
-			    !xfer->rx_buf)
-				max_rx = max(xfer->len, max_rx);
-		}
-
-		if (max_tx) {
-			tmp = krealloc(master->dummy_tx, max_tx,
-				       GFP_KERNEL | GFP_DMA);
-			if (!tmp)
-				return -ENOMEM;
-			master->dummy_tx = tmp;
-			memset(tmp, 0, max_tx);
-		}
-
-		if (max_rx) {
-			tmp = krealloc(master->dummy_rx, max_rx,
-				       GFP_KERNEL | GFP_DMA);
-			if (!tmp)
-				return -ENOMEM;
-			master->dummy_rx = tmp;
-		}
-
-		if (max_tx || max_rx) {
-			list_for_each_entry(xfer, &msg->transfers,
-					    transfer_list) {
-				if (!xfer->tx_buf)
-					xfer->tx_buf = master->dummy_tx;
-				if (!xfer->rx_buf)
-					xfer->rx_buf = master->dummy_rx;
-			}
-		}
-	}
-
 	if (!master->can_dma)
 		return 0;
 
@@ -742,6 +700,69 @@ static int spi_unmap_msg(struct spi_master *master, struct spi_message *msg)
 
 	return 0;
 }
+#else /* !CONFIG_HAS_DMA */
+static inline int __spi_map_msg(struct spi_master *master,
+				struct spi_message *msg)
+{
+	return 0;
+}
+
+static inline int spi_unmap_msg(struct spi_master *master,
+				struct spi_message *msg)
+{
+	return 0;
+}
+#endif /* !CONFIG_HAS_DMA */
+
+static int spi_map_msg(struct spi_master *master, struct spi_message *msg)
+{
+	struct spi_transfer *xfer;
+	void *tmp;
+	unsigned int max_tx, max_rx;
+
+	if (master->flags & (SPI_MASTER_MUST_RX | SPI_MASTER_MUST_TX)) {
+		max_tx = 0;
+		max_rx = 0;
+
+		list_for_each_entry(xfer, &msg->transfers, transfer_list) {
+			if ((master->flags & SPI_MASTER_MUST_TX) &&
+			    !xfer->tx_buf)
+				max_tx = max(xfer->len, max_tx);
+			if ((master->flags & SPI_MASTER_MUST_RX) &&
+			    !xfer->rx_buf)
+				max_rx = max(xfer->len, max_rx);
+		}
+
+		if (max_tx) {
+			tmp = krealloc(master->dummy_tx, max_tx,
+				       GFP_KERNEL | GFP_DMA);
+			if (!tmp)
+				return -ENOMEM;
+			master->dummy_tx = tmp;
+			memset(tmp, 0, max_tx);
+		}
+
+		if (max_rx) {
+			tmp = krealloc(master->dummy_rx, max_rx,
+				       GFP_KERNEL | GFP_DMA);
+			if (!tmp)
+				return -ENOMEM;
+			master->dummy_rx = tmp;
+		}
+
+		if (max_tx || max_rx) {
+			list_for_each_entry(xfer, &msg->transfers,
+					    transfer_list) {
+				if (!xfer->tx_buf)
+					xfer->tx_buf = master->dummy_tx;
+				if (!xfer->rx_buf)
+					xfer->rx_buf = master->dummy_rx;
+			}
+		}
+	}
+
+	return __spi_map_msg(master, msg);
+}
 
 /*
  * spi_transfer_one_message - Default implementation of transfer_one_message()
@@ -775,7 +796,7 @@ static int spi_transfer_one_message(struct spi_master *master,
 		if (ret > 0) {
 			ret = 0;
 			ms = xfer->len * 8 * 1000 / xfer->speed_hz;
-			ms += 10; /* some tolerance */
+			ms += ms + 100; /* some tolerance */
 
 			ms = wait_for_completion_timeout(&master->xfer_completion,
 							 msecs_to_jiffies(ms));
@@ -1151,7 +1172,6 @@ static int spi_master_initialize_queue(struct spi_master *master)
 {
 	int ret;
 
-	master->queued = true;
 	master->transfer = spi_queued_transfer;
 	if (!master->transfer_one_message)
 		master->transfer_one_message = spi_transfer_one_message;
@@ -1162,6 +1182,7 @@ static int spi_master_initialize_queue(struct spi_master *master)
 		dev_err(&master->dev, "problem initializing queue\n");
 		goto err_init_queue;
 	}
+	master->queued = true;
 	ret = spi_start_queue(master);
 	if (ret) {
 		dev_err(&master->dev, "problem starting queue\n");
@@ -1171,8 +1192,8 @@ static int spi_master_initialize_queue(struct spi_master *master)
 	return 0;
 
 err_start_queue:
-err_init_queue:
 	spi_destroy_queue(master);
+err_init_queue:
 	return ret;
 }
 
@@ -1234,6 +1255,8 @@ static void of_register_spi_devices(struct spi_master *master)
 			spi->mode |= SPI_CS_HIGH;
 		if (of_find_property(nc, "spi-3wire", NULL))
 			spi->mode |= SPI_3WIRE;
+		if (of_find_property(nc, "spi-lsb-first", NULL))
+			spi->mode |= SPI_LSB_FIRST;
 
 		/* Device DUAL/QUAD mode */
 		if (!of_property_read_u32(nc, "spi-tx-bus-width", &value)) {
@@ -1247,11 +1270,10 @@ static void of_register_spi_devices(struct spi_master *master)
 				spi->mode |= SPI_TX_QUAD;
 				break;
 			default:
-				dev_err(&master->dev,
-					"spi-tx-bus-width %d not supported\n",
-					value);
-				spi_dev_put(spi);
-				continue;
+				dev_warn(&master->dev,
+					 "spi-tx-bus-width %d not supported\n",
+					 value);
+				break;
 			}
 		}
 
@@ -1266,11 +1288,10 @@ static void of_register_spi_devices(struct spi_master *master)
 				spi->mode |= SPI_RX_QUAD;
 				break;
 			default:
-				dev_err(&master->dev,
-					"spi-rx-bus-width %d not supported\n",
-					value);
-				spi_dev_put(spi);
-				continue;
+				dev_warn(&master->dev,
+					 "spi-rx-bus-width %d not supported\n",
+					 value);
+				break;
 			}
 		}
 
@@ -1756,7 +1777,7 @@ EXPORT_SYMBOL_GPL(spi_busnum_to_master);
  */
 int spi_setup(struct spi_device *spi)
 {
-	unsigned	bad_bits;
+	unsigned	bad_bits, ugly_bits;
 	int		status = 0;
 
 	/* check mode to prevent that DUAL and QUAD set at the same time
@@ -1776,6 +1797,15 @@ int spi_setup(struct spi_device *spi)
 	 * that aren't supported with their current master
 	 */
 	bad_bits = spi->mode & ~spi->master->mode_bits;
+	ugly_bits = bad_bits &
+		    (SPI_TX_DUAL | SPI_TX_QUAD | SPI_RX_DUAL | SPI_RX_QUAD);
+	if (ugly_bits) {
+		dev_warn(&spi->dev,
+			 "setup: ignoring unsupported mode bits %x\n",
+			 ugly_bits);
+		spi->mode &= ~ugly_bits;
+		bad_bits &= ~ugly_bits;
+	}
 	if (bad_bits) {
 		dev_err(&spi->dev, "setup: unsupported mode bits %x\n",
 			bad_bits);
diff --git a/drivers/staging/Kconfig b/drivers/staging/Kconfig
index 22365f140be..4f38fc000a3 100644
--- a/drivers/staging/Kconfig
+++ b/drivers/staging/Kconfig
@@ -48,12 +48,12 @@ 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/rts5139/Kconfig"
-
 source "drivers/staging/rts5208/Kconfig"
 
 source "drivers/staging/frontier/Kconfig"
@@ -90,8 +90,6 @@ source "drivers/staging/tidspbridge/Kconfig"
 
 source "drivers/staging/quickstart/Kconfig"
 
-source "drivers/staging/sbe-2t3e3/Kconfig"
-
 source "drivers/staging/keucr/Kconfig"
 
 source "drivers/staging/bcm/Kconfig"
@@ -144,6 +142,8 @@ source "drivers/staging/gs_fpgaboot/Kconfig"
 
 source "drivers/staging/nokia_h4p/Kconfig"
 
+source "drivers/staging/skein/Kconfig"
+
 source "drivers/staging/unisys/Kconfig"
 
 endif # STAGING
diff --git a/drivers/staging/Makefile b/drivers/staging/Makefile
index fbe84ed2d04..1e97ad2177d 100644
--- a/drivers/staging/Makefile
+++ b/drivers/staging/Makefile
@@ -16,9 +16,9 @@ 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_RTS5139)		+= rts5139/
 obj-$(CONFIG_RTS5208)		+= rts5208/
 obj-$(CONFIG_TRANZPORT)		+= frontier/
 obj-$(CONFIG_IDE_PHISON)	+= phison/
@@ -39,7 +39,6 @@ obj-$(CONFIG_CXT1E1)		+= cxt1e1/
 obj-$(CONFIG_FB_XGI)		+= xgifb/
 obj-$(CONFIG_TIDSPBRIDGE)	+= tidspbridge/
 obj-$(CONFIG_ACPI_QUICKSTART)	+= quickstart/
-obj-$(CONFIG_SBE_2T3E3)		+= sbe-2t3e3/
 obj-$(CONFIG_USB_ENESTORAGE)	+= keucr/
 obj-$(CONFIG_BCM_WIMAX)		+= bcm/
 obj-$(CONFIG_FT1000)		+= ft1000/
@@ -64,4 +63,5 @@ 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/alarm-dev.c b/drivers/staging/android/alarm-dev.c
index 2fc7cdd4c4e..f200e8a8432 100644
--- a/drivers/staging/android/alarm-dev.c
+++ b/drivers/staging/android/alarm-dev.c
@@ -329,6 +329,7 @@ static int alarm_release(struct inode *inode, struct file *file)
 	if (file->private_data) {
 		for (i = 0; i < ANDROID_ALARM_TYPE_COUNT; i++) {
 			uint32_t alarm_type_mask = 1U << i;
+
 			if (alarm_enabled & alarm_type_mask) {
 				alarm_dbg(INFO,
 					  "%s: clear alarm, pending %d\n",
diff --git a/drivers/staging/android/binder.c b/drivers/staging/android/binder.c
index cfe4bc8f05c..a741da77828 100644
--- a/drivers/staging/android/binder.c
+++ b/drivers/staging/android/binder.c
@@ -118,6 +118,7 @@ static int binder_set_stop_on_user_error(const char *val,
 					 struct kernel_param *kp)
 {
 	int ret;
+
 	ret = param_set_int(val, kp);
 	if (binder_stop_on_user_error < 2)
 		wake_up(&binder_user_error_wait);
@@ -194,6 +195,7 @@ static struct binder_transaction_log_entry *binder_transaction_log_add(
 	struct binder_transaction_log *log)
 {
 	struct binder_transaction_log_entry *e;
+
 	e = &log->entry[log->next];
 	memset(e, 0, sizeof(*e));
 	log->next++;
@@ -432,16 +434,17 @@ static inline void binder_unlock(const char *tag)
 static void binder_set_nice(long nice)
 {
 	long min_nice;
+
 	if (can_nice(current, nice)) {
 		set_user_nice(current, nice);
 		return;
 	}
-	min_nice = 20 - current->signal->rlim[RLIMIT_NICE].rlim_cur;
+	min_nice = rlimit_to_nice(current->signal->rlim[RLIMIT_NICE].rlim_cur);
 	binder_debug(BINDER_DEBUG_PRIORITY_CAP,
 		     "%d: nice value %ld not allowed use %ld instead\n",
 		      current->pid, nice, min_nice);
 	set_user_nice(current, min_nice);
-	if (min_nice < 20)
+	if (min_nice <= MAX_NICE)
 		return;
 	binder_user_error("%d RLIMIT_NICE not set\n", current->pid);
 }
@@ -584,6 +587,7 @@ static int binder_update_page_range(struct binder_proc *proc, int allocate,
 	for (page_addr = start; page_addr < end; page_addr += PAGE_SIZE) {
 		int ret;
 		struct page **page_array_ptr;
+
 		page = &proc->pages[(page_addr - proc->buffer) / PAGE_SIZE];
 
 		BUG_ON(*page);
@@ -726,6 +730,7 @@ static struct binder_buffer *binder_alloc_buf(struct binder_proc *proc,
 	binder_insert_allocated_buffer(proc, buffer);
 	if (buffer_size != size) {
 		struct binder_buffer *new_buffer = (void *)buffer->data + size;
+
 		list_add(&new_buffer->entry, &buffer->entry);
 		new_buffer->free = 1;
 		binder_insert_free_buffer(proc, new_buffer);
@@ -838,6 +843,7 @@ static void binder_free_buf(struct binder_proc *proc,
 	if (!list_is_last(&buffer->entry, &proc->buffers)) {
 		struct binder_buffer *next = list_entry(buffer->entry.next,
 						struct binder_buffer, entry);
+
 		if (next->free) {
 			rb_erase(&next->rb_node, &proc->free_buffers);
 			binder_delete_free_buffer(proc, next);
@@ -846,6 +852,7 @@ static void binder_free_buf(struct binder_proc *proc,
 	if (proc->buffers.next != &buffer->entry) {
 		struct binder_buffer *prev = list_entry(buffer->entry.prev,
 						struct binder_buffer, entry);
+
 		if (prev->free) {
 			binder_delete_free_buffer(proc, buffer);
 			rb_erase(&prev->rb_node, &proc->free_buffers);
@@ -1107,6 +1114,7 @@ static int binder_inc_ref(struct binder_ref *ref, int strong,
 			  struct list_head *target_list)
 {
 	int ret;
+
 	if (strong) {
 		if (ref->strong == 0) {
 			ret = binder_inc_node(ref->node, 1, 1, target_list);
@@ -1138,6 +1146,7 @@ static int binder_dec_ref(struct binder_ref *ref, int strong)
 		ref->strong--;
 		if (ref->strong == 0) {
 			int ret;
+
 			ret = binder_dec_node(ref->node, strong, 1);
 			if (ret)
 				return ret;
@@ -1177,6 +1186,7 @@ static void binder_send_failed_reply(struct binder_transaction *t,
 				     uint32_t error_code)
 {
 	struct binder_thread *target_thread;
+
 	BUG_ON(t->flags & TF_ONE_WAY);
 	while (1) {
 		target_thread = t->from;
@@ -1247,6 +1257,7 @@ static void binder_transaction_buffer_release(struct binder_proc *proc,
 		off_end = (void *)offp + buffer->offsets_size;
 	for (; offp < off_end; offp++) {
 		struct flat_binder_object *fp;
+
 		if (*offp > buffer->data_size - sizeof(*fp) ||
 		    buffer->data_size < sizeof(*fp) ||
 		    !IS_ALIGNED(*offp, sizeof(u32))) {
@@ -1259,6 +1270,7 @@ static void binder_transaction_buffer_release(struct binder_proc *proc,
 		case BINDER_TYPE_BINDER:
 		case BINDER_TYPE_WEAK_BINDER: {
 			struct binder_node *node = binder_get_node(proc, fp->binder);
+
 			if (node == NULL) {
 				pr_err("transaction release %d bad node %016llx\n",
 				       debug_id, (u64)fp->binder);
@@ -1272,6 +1284,7 @@ static void binder_transaction_buffer_release(struct binder_proc *proc,
 		case BINDER_TYPE_HANDLE:
 		case BINDER_TYPE_WEAK_HANDLE: {
 			struct binder_ref *ref = binder_get_ref(proc, fp->handle);
+
 			if (ref == NULL) {
 				pr_err("transaction release %d bad handle %d\n",
 				 debug_id, fp->handle);
@@ -1363,6 +1376,7 @@ static void binder_transaction(struct binder_proc *proc,
 	} else {
 		if (tr->target.handle) {
 			struct binder_ref *ref;
+
 			ref = binder_get_ref(proc, tr->target.handle);
 			if (ref == NULL) {
 				binder_user_error("%d:%d got transaction to invalid handle\n",
@@ -1386,6 +1400,7 @@ static void binder_transaction(struct binder_proc *proc,
 		}
 		if (!(tr->flags & TF_ONE_WAY) && thread->transaction_stack) {
 			struct binder_transaction *tmp;
+
 			tmp = thread->transaction_stack;
 			if (tmp->to_thread != thread) {
 				binder_user_error("%d:%d got new transaction with bad transaction stack, transaction %d has target %d:%d\n",
@@ -1452,7 +1467,7 @@ static void binder_transaction(struct binder_proc *proc,
 		t->from = thread;
 	else
 		t->from = NULL;
-	t->sender_euid = proc->tsk->cred->euid;
+	t->sender_euid = task_euid(proc->tsk);
 	t->to_proc = target_proc;
 	t->to_thread = target_thread;
 	t->code = tr->code;
@@ -1501,6 +1516,7 @@ static void binder_transaction(struct binder_proc *proc,
 	off_end = (void *)offp + tr->offsets_size;
 	for (; offp < off_end; offp++) {
 		struct flat_binder_object *fp;
+
 		if (*offp > t->buffer->data_size - sizeof(*fp) ||
 		    t->buffer->data_size < sizeof(*fp) ||
 		    !IS_ALIGNED(*offp, sizeof(u32))) {
@@ -1515,6 +1531,7 @@ static void binder_transaction(struct binder_proc *proc,
 		case BINDER_TYPE_WEAK_BINDER: {
 			struct binder_ref *ref;
 			struct binder_node *node = binder_get_node(proc, fp->binder);
+
 			if (node == NULL) {
 				node = binder_new_node(proc, fp->binder, fp->cookie);
 				if (node == NULL) {
@@ -1529,6 +1546,7 @@ static void binder_transaction(struct binder_proc *proc,
 					proc->pid, thread->pid,
 					(u64)fp->binder, node->debug_id,
 					(u64)fp->cookie, (u64)node->cookie);
+				return_error = BR_FAILED_REPLY;
 				goto err_binder_get_ref_for_node_failed;
 			}
 			ref = binder_get_ref_for_node(target_proc, node);
@@ -1553,6 +1571,7 @@ static void binder_transaction(struct binder_proc *proc,
 		case BINDER_TYPE_HANDLE:
 		case BINDER_TYPE_WEAK_HANDLE: {
 			struct binder_ref *ref = binder_get_ref(proc, fp->handle);
+
 			if (ref == NULL) {
 				binder_user_error("%d:%d got transaction with invalid handle, %d\n",
 						proc->pid,
@@ -1575,6 +1594,7 @@ static void binder_transaction(struct binder_proc *proc,
 					     (u64)ref->node->ptr);
 			} else {
 				struct binder_ref *new_ref;
+
 				new_ref = binder_get_ref_for_node(target_proc, ref->node);
 				if (new_ref == NULL) {
 					return_error = BR_FAILED_REPLY;
@@ -1694,6 +1714,7 @@ err_no_context_mgr_node:
 
 	{
 		struct binder_transaction_log_entry *fe;
+
 		fe = binder_transaction_log_add(&binder_transaction_log_failed);
 		*fe = *e;
 	}
@@ -2024,12 +2045,14 @@ static int binder_thread_write(struct binder_proc *proc,
 			struct binder_work *w;
 			binder_uintptr_t cookie;
 			struct binder_ref_death *death = NULL;
+
 			if (get_user(cookie, (binder_uintptr_t __user *)ptr))
 				return -EFAULT;
 
 			ptr += sizeof(void *);
 			list_for_each_entry(w, &proc->delivered_death, entry) {
 				struct binder_ref_death *tmp_death = container_of(w, struct binder_ref_death, work);
+
 				if (tmp_death->cookie == cookie) {
 					death = tmp_death;
 					break;
@@ -2216,6 +2239,7 @@ retry:
 			const char *cmd_name;
 			int strong = node->internal_strong_refs || node->local_strong_refs;
 			int weak = !hlist_empty(&node->refs) || node->local_weak_refs || strong;
+
 			if (weak && !node->has_weak_ref) {
 				cmd = BR_INCREFS;
 				cmd_name = "BR_INCREFS";
@@ -2322,6 +2346,7 @@ retry:
 		BUG_ON(t->buffer == NULL);
 		if (t->buffer->target_node) {
 			struct binder_node *target_node = t->buffer->target_node;
+
 			tr.target.ptr = target_node->ptr;
 			tr.cookie =  target_node->cookie;
 			t->saved_priority = task_nice(current);
@@ -2343,6 +2368,7 @@ retry:
 
 		if (t->from) {
 			struct task_struct *sender = t->from->proc->tsk;
+
 			tr.sender_pid = task_tgid_nr_ns(sender,
 							task_active_pid_ns(current));
 		} else {
@@ -2413,6 +2439,7 @@ done:
 static void binder_release_work(struct list_head *list)
 {
 	struct binder_work *w;
+
 	while (!list_empty(list)) {
 		w = list_first_entry(list, struct binder_work, entry);
 		list_del_init(&w->entry);
@@ -2574,6 +2601,7 @@ static long binder_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 	struct binder_thread *thread;
 	unsigned int size = _IOC_SIZE(cmd);
 	void __user *ubuf = (void __user *)arg;
+	kuid_t curr_euid = current_euid();
 
 	/*pr_info("binder_ioctl: %d:%d %x %lx\n", proc->pid, current->pid, cmd, arg);*/
 
@@ -2593,6 +2621,7 @@ static long binder_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 	switch (cmd) {
 	case BINDER_WRITE_READ: {
 		struct binder_write_read bwr;
+
 		if (size != sizeof(struct binder_write_read)) {
 			ret = -EINVAL;
 			goto err;
@@ -2658,15 +2687,16 @@ static long binder_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 			goto err;
 		}
 		if (uid_valid(binder_context_mgr_uid)) {
-			if (!uid_eq(binder_context_mgr_uid, current->cred->euid)) {
+			if (!uid_eq(binder_context_mgr_uid, curr_euid)) {
 				pr_err("BINDER_SET_CONTEXT_MGR bad uid %d != %d\n",
-				       from_kuid(&init_user_ns, current->cred->euid),
+				       from_kuid(&init_user_ns, curr_euid),
 				       from_kuid(&init_user_ns, binder_context_mgr_uid));
 				ret = -EPERM;
 				goto err;
 			}
-		} else
-			binder_context_mgr_uid = current->cred->euid;
+		} else {
+			binder_context_mgr_uid = curr_euid;
+		}
 		binder_context_mgr_node = binder_new_node(proc, 0, 0);
 		if (binder_context_mgr_node == NULL) {
 			ret = -ENOMEM;
@@ -2683,16 +2713,20 @@ static long binder_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 		binder_free_thread(proc, thread);
 		thread = NULL;
 		break;
-	case BINDER_VERSION:
+	case BINDER_VERSION: {
+		struct binder_version __user *ver = ubuf;
+
 		if (size != sizeof(struct binder_version)) {
 			ret = -EINVAL;
 			goto err;
 		}
-		if (put_user(BINDER_CURRENT_PROTOCOL_VERSION, &((struct binder_version *)ubuf)->protocol_version)) {
+		if (put_user(BINDER_CURRENT_PROTOCOL_VERSION,
+			     &ver->protocol_version)) {
 			ret = -EINVAL;
 			goto err;
 		}
 		break;
+	}
 	default:
 		ret = -EINVAL;
 		goto err;
@@ -2713,6 +2747,7 @@ err_unlocked:
 static void binder_vma_open(struct vm_area_struct *vma)
 {
 	struct binder_proc *proc = vma->vm_private_data;
+
 	binder_debug(BINDER_DEBUG_OPEN_CLOSE,
 		     "%d open vm area %lx-%lx (%ld K) vma %lx pagep %lx\n",
 		     proc->pid, vma->vm_start, vma->vm_end,
@@ -2723,6 +2758,7 @@ static void binder_vma_open(struct vm_area_struct *vma)
 static void binder_vma_close(struct vm_area_struct *vma)
 {
 	struct binder_proc *proc = vma->vm_private_data;
+
 	binder_debug(BINDER_DEBUG_OPEN_CLOSE,
 		     "%d close vm area %lx-%lx (%ld K) vma %lx pagep %lx\n",
 		     proc->pid, vma->vm_start, vma->vm_end,
@@ -2865,6 +2901,7 @@ static int binder_open(struct inode *nodp, struct file *filp)
 
 	if (binder_debugfs_dir_entry_proc) {
 		char strbuf[11];
+
 		snprintf(strbuf, sizeof(strbuf), "%u", proc->pid);
 		proc->debugfs_entry = debugfs_create_file(strbuf, S_IRUGO,
 			binder_debugfs_dir_entry_proc, proc, &binder_proc_fops);
@@ -2886,8 +2923,10 @@ static void binder_deferred_flush(struct binder_proc *proc)
 {
 	struct rb_node *n;
 	int wake_count = 0;
+
 	for (n = rb_first(&proc->threads); n != NULL; n = rb_next(n)) {
 		struct binder_thread *thread = rb_entry(n, struct binder_thread, rb_node);
+
 		thread->looper |= BINDER_LOOPER_STATE_NEED_RETURN;
 		if (thread->looper & BINDER_LOOPER_STATE_WAITING) {
 			wake_up_interruptible(&thread->wait);
@@ -2904,6 +2943,7 @@ static void binder_deferred_flush(struct binder_proc *proc)
 static int binder_release(struct inode *nodp, struct file *filp)
 {
 	struct binder_proc *proc = filp->private_data;
+
 	debugfs_remove(proc->debugfs_entry);
 	binder_defer_work(proc, BINDER_DEFERRED_RELEASE);
 
@@ -3065,6 +3105,7 @@ static void binder_deferred_func(struct work_struct *work)
 	struct files_struct *files;
 
 	int defer;
+
 	do {
 		binder_lock(__func__);
 		mutex_lock(&binder_deferred_lock);
diff --git a/drivers/staging/android/ion/ion.c b/drivers/staging/android/ion/ion.c
index 3d5bf147223..389b8f67a2e 100644
--- a/drivers/staging/android/ion/ion.c
+++ b/drivers/staging/android/ion/ion.c
@@ -408,6 +408,7 @@ static struct ion_handle *ion_handle_lookup(struct ion_client *client,
 
 	while (n) {
 		struct ion_handle *entry = rb_entry(n, struct ion_handle, node);
+
 		if (buffer < entry->buffer)
 			n = n->rb_left;
 		else if (buffer > entry->buffer)
@@ -626,6 +627,10 @@ static void ion_handle_kmap_put(struct ion_handle *handle)
 {
 	struct ion_buffer *buffer = handle->buffer;
 
+	if (!handle->kmap_cnt) {
+		WARN(1, "%s: Double unmap detected! bailing...\n", __func__);
+		return;
+	}
 	handle->kmap_cnt--;
 	if (!handle->kmap_cnt)
 		ion_buffer_kmap_put(buffer);
@@ -720,9 +725,11 @@ static int ion_get_client_serial(const struct rb_root *root,
 {
 	int serial = -1;
 	struct rb_node *node;
+
 	for (node = rb_first(root); node; node = rb_next(node)) {
 		struct ion_client *client = rb_entry(node, struct ion_client,
 						node);
+
 		if (strcmp(client->name, name))
 			continue;
 		serial = max(serial, client->display_serial);
@@ -1035,12 +1042,14 @@ static int ion_mmap(struct dma_buf *dmabuf, struct vm_area_struct *vma)
 static void ion_dma_buf_release(struct dma_buf *dmabuf)
 {
 	struct ion_buffer *buffer = dmabuf->priv;
+
 	ion_buffer_put(buffer);
 }
 
 static void *ion_dma_buf_kmap(struct dma_buf *dmabuf, unsigned long offset)
 {
 	struct ion_buffer *buffer = dmabuf->priv;
+
 	return buffer->vaddr + offset * PAGE_SIZE;
 }
 
@@ -1292,6 +1301,7 @@ static long ion_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 	case ION_IOC_IMPORT:
 	{
 		struct ion_handle *handle;
+
 		handle = ion_import_dma_buf(client, data.fd.fd);
 		if (IS_ERR(handle))
 			ret = PTR_ERR(handle);
@@ -1393,6 +1403,7 @@ static int ion_debug_heap_show(struct seq_file *s, void *unused)
 		struct ion_client *client = rb_entry(n, struct ion_client,
 						     node);
 		size_t size = ion_debug_heap_total(client, heap->id);
+
 		if (!size)
 			continue;
 		if (client->task) {
@@ -1516,6 +1527,7 @@ void ion_device_add_heap(struct ion_device *dev, struct ion_heap *heap)
 
 	if (!debug_file) {
 		char buf[256], *path;
+
 		path = dentry_path(dev->heaps_debug_root, buf, 256);
 		pr_err("Failed to create heap debugfs at %s/%s\n",
 			path, heap->name);
@@ -1531,6 +1543,7 @@ void ion_device_add_heap(struct ion_device *dev, struct ion_heap *heap)
 			&debug_shrink_fops);
 		if (!debug_file) {
 			char buf[256], *path;
+
 			path = dentry_path(dev->heaps_debug_root, buf, 256);
 			pr_err("Failed to create heap shrinker debugfs at %s/%s\n",
 				path, debug_name);
@@ -1606,6 +1619,7 @@ void __init ion_reserve(struct ion_platform_data *data)
 
 		if (data->heaps[i].base == 0) {
 			phys_addr_t paddr;
+
 			paddr = memblock_alloc_base(data->heaps[i].size,
 						    data->heaps[i].align,
 						    MEMBLOCK_ALLOC_ANYWHERE);
diff --git a/drivers/staging/android/ion/ion_carveout_heap.c b/drivers/staging/android/ion/ion_carveout_heap.c
index 3cb05b9b0e9..dcb6f2196c8 100644
--- a/drivers/staging/android/ion/ion_carveout_heap.c
+++ b/drivers/staging/android/ion/ion_carveout_heap.c
@@ -81,7 +81,7 @@ static int ion_carveout_heap_allocate(struct ion_heap *heap,
 	if (align > PAGE_SIZE)
 		return -EINVAL;
 
-	table = kzalloc(sizeof(struct sg_table), GFP_KERNEL);
+	table = kmalloc(sizeof(struct sg_table), GFP_KERNEL);
 	if (!table)
 		return -ENOMEM;
 	ret = sg_alloc_table(table, 1, GFP_KERNEL);
diff --git a/drivers/staging/android/ion/ion_chunk_heap.c b/drivers/staging/android/ion/ion_chunk_heap.c
index d40f5f83180..3f2c12ba4d1 100644
--- a/drivers/staging/android/ion/ion_chunk_heap.c
+++ b/drivers/staging/android/ion/ion_chunk_heap.c
@@ -55,7 +55,7 @@ static int ion_chunk_heap_allocate(struct ion_heap *heap,
 	if (allocated_size > chunk_heap->size - chunk_heap->allocated)
 		return -ENOMEM;
 
-	table = kzalloc(sizeof(struct sg_table), GFP_KERNEL);
+	table = kmalloc(sizeof(struct sg_table), GFP_KERNEL);
 	if (!table)
 		return -ENOMEM;
 	ret = sg_alloc_table(table, num_chunks, GFP_KERNEL);
diff --git a/drivers/staging/android/ion/ion_heap.c b/drivers/staging/android/ion/ion_heap.c
index bdc6a28ba8c..4605e04712a 100644
--- a/drivers/staging/android/ion/ion_heap.c
+++ b/drivers/staging/android/ion/ion_heap.c
@@ -48,6 +48,7 @@ void *ion_heap_map_kernel(struct ion_heap *heap,
 	for_each_sg(table->sgl, sg, table->nents, i) {
 		int npages_this_entry = PAGE_ALIGN(sg->length) / PAGE_SIZE;
 		struct page *page = sg_page(sg);
+
 		BUG_ON(i >= npages);
 		for (j = 0; j < npages_this_entry; j++)
 			*(tmp++) = page++;
@@ -105,6 +106,7 @@ int ion_heap_map_user(struct ion_heap *heap, struct ion_buffer *buffer,
 static int ion_heap_clear_pages(struct page **pages, int num, pgprot_t pgprot)
 {
 	void *addr = vm_map_ram(pages, num, -1, pgprot);
+
 	if (!addr)
 		return -ENOMEM;
 	memset(addr, 0, PAGE_SIZE * num);
diff --git a/drivers/staging/android/ion/ion_page_pool.c b/drivers/staging/android/ion/ion_page_pool.c
index ecb5fc34ec5..5864f3dfcbc 100644
--- a/drivers/staging/android/ion/ion_page_pool.c
+++ b/drivers/staging/android/ion/ion_page_pool.c
@@ -21,13 +21,9 @@
 #include <linux/list.h>
 #include <linux/module.h>
 #include <linux/slab.h>
+#include <linux/swap.h>
 #include "ion_priv.h"
 
-struct ion_page_pool_item {
-	struct page *page;
-	struct list_head list;
-};
-
 static void *ion_page_pool_alloc_pages(struct ion_page_pool *pool)
 {
 	struct page *page = alloc_pages(pool->gfp_mask, pool->order);
@@ -47,19 +43,12 @@ static void ion_page_pool_free_pages(struct ion_page_pool *pool,
 
 static int ion_page_pool_add(struct ion_page_pool *pool, struct page *page)
 {
-	struct ion_page_pool_item *item;
-
-	item = kmalloc(sizeof(struct ion_page_pool_item), GFP_KERNEL);
-	if (!item)
-		return -ENOMEM;
-
 	mutex_lock(&pool->mutex);
-	item->page = page;
 	if (PageHighMem(page)) {
-		list_add_tail(&item->list, &pool->high_items);
+		list_add_tail(&page->lru, &pool->high_items);
 		pool->high_count++;
 	} else {
-		list_add_tail(&item->list, &pool->low_items);
+		list_add_tail(&page->lru, &pool->low_items);
 		pool->low_count++;
 	}
 	mutex_unlock(&pool->mutex);
@@ -68,28 +57,23 @@ static int ion_page_pool_add(struct ion_page_pool *pool, struct page *page)
 
 static struct page *ion_page_pool_remove(struct ion_page_pool *pool, bool high)
 {
-	struct ion_page_pool_item *item;
 	struct page *page;
 
 	if (high) {
 		BUG_ON(!pool->high_count);
-		item = list_first_entry(&pool->high_items,
-					struct ion_page_pool_item, list);
+		page = list_first_entry(&pool->high_items, struct page, lru);
 		pool->high_count--;
 	} else {
 		BUG_ON(!pool->low_count);
-		item = list_first_entry(&pool->low_items,
-					struct ion_page_pool_item, list);
+		page = list_first_entry(&pool->low_items, struct page, lru);
 		pool->low_count--;
 	}
 
-	list_del(&item->list);
-	page = item->page;
-	kfree(item);
+	list_del(&page->lru);
 	return page;
 }
 
-void *ion_page_pool_alloc(struct ion_page_pool *pool)
+struct page *ion_page_pool_alloc(struct ion_page_pool *pool)
 {
 	struct page *page = NULL;
 
@@ -112,6 +96,8 @@ void ion_page_pool_free(struct ion_page_pool *pool, struct page *page)
 {
 	int ret;
 
+	BUG_ON(pool->order != compound_order(page));
+
 	ret = ion_page_pool_add(pool, page);
 	if (ret)
 		ion_page_pool_free_pages(pool, page);
@@ -119,12 +105,12 @@ void ion_page_pool_free(struct ion_page_pool *pool, struct page *page)
 
 static int ion_page_pool_total(struct ion_page_pool *pool, bool high)
 {
-	int total = 0;
+	int count = pool->low_count;
+
+	if (high)
+		count += pool->high_count;
 
-	total += high ? (pool->high_count + pool->low_count) *
-		(1 << pool->order) :
-			pool->low_count * (1 << pool->order);
-	return total;
+	return count << pool->order;
 }
 
 int ion_page_pool_shrink(struct ion_page_pool *pool, gfp_t gfp_mask,
@@ -133,7 +119,10 @@ int ion_page_pool_shrink(struct ion_page_pool *pool, gfp_t gfp_mask,
 	int freed;
 	bool high;
 
-	high = !!(gfp_mask & __GFP_HIGHMEM);
+	if (current_is_kswapd())
+		high = 1;
+	else
+		high = !!(gfp_mask & __GFP_HIGHMEM);
 
 	if (nr_to_scan == 0)
 		return ion_page_pool_total(pool, high);
@@ -167,7 +156,7 @@ struct ion_page_pool *ion_page_pool_create(gfp_t gfp_mask, unsigned int order)
 	pool->low_count = 0;
 	INIT_LIST_HEAD(&pool->low_items);
 	INIT_LIST_HEAD(&pool->high_items);
-	pool->gfp_mask = gfp_mask;
+	pool->gfp_mask = gfp_mask | __GFP_COMP;
 	pool->order = order;
 	mutex_init(&pool->mutex);
 	plist_node_init(&pool->list, order);
diff --git a/drivers/staging/android/ion/ion_priv.h b/drivers/staging/android/ion/ion_priv.h
index 1eba3f2076a..c8f01757abf 100644
--- a/drivers/staging/android/ion/ion_priv.h
+++ b/drivers/staging/android/ion/ion_priv.h
@@ -178,6 +178,7 @@ struct ion_heap {
 	spinlock_t free_lock;
 	wait_queue_head_t waitqueue;
 	struct task_struct *task;
+
 	int (*debug_show)(struct ion_heap *heap, struct seq_file *, void *);
 };
 
@@ -377,7 +378,7 @@ struct ion_page_pool {
 
 struct ion_page_pool *ion_page_pool_create(gfp_t gfp_mask, unsigned int order);
 void ion_page_pool_destroy(struct ion_page_pool *);
-void *ion_page_pool_alloc(struct ion_page_pool *);
+struct page *ion_page_pool_alloc(struct ion_page_pool *);
 void ion_page_pool_free(struct ion_page_pool *, struct page *);
 
 /** ion_page_pool_shrink - shrinks the size of the memory cached in the pool
diff --git a/drivers/staging/android/ion/ion_system_heap.c b/drivers/staging/android/ion/ion_system_heap.c
index c92363356ae..cb7ae08a5e2 100644
--- a/drivers/staging/android/ion/ion_system_heap.c
+++ b/drivers/staging/android/ion/ion_system_heap.c
@@ -34,6 +34,7 @@ static const int num_orders = ARRAY_SIZE(orders);
 static int order_to_index(unsigned int order)
 {
 	int i;
+
 	for (i = 0; i < num_orders; i++)
 		if (order == orders[i])
 			return i;
@@ -41,7 +42,7 @@ static int order_to_index(unsigned int order)
 	return -1;
 }
 
-static unsigned int order_to_size(int order)
+static inline unsigned int order_to_size(int order)
 {
 	return PAGE_SIZE << order;
 }
@@ -72,14 +73,12 @@ static struct page *alloc_buffer_page(struct ion_system_heap *heap,
 
 		if (order > 4)
 			gfp_flags = high_order_gfp_flags;
-		page = alloc_pages(gfp_flags, order);
+		page = alloc_pages(gfp_flags | __GFP_COMP, order);
 		if (!page)
 			return NULL;
 		ion_pages_sync_for_device(NULL, page, PAGE_SIZE << order,
 						DMA_BIDIRECTIONAL);
 	}
-	if (!page)
-		return NULL;
 
 	return page;
 }
@@ -92,6 +91,7 @@ static void free_buffer_page(struct ion_system_heap *heap,
 
 	if (!cached && !(buffer->private_flags & ION_PRIV_FLAG_SHRINKER_FREE)) {
 		struct ion_page_pool *pool = heap->pools[order_to_index(order)];
+
 		ion_page_pool_free(pool, page);
 	} else {
 		__free_pages(page, order);
@@ -124,7 +124,6 @@ static struct page_info *alloc_largest_available(struct ion_system_heap *heap,
 
 		info->page = page;
 		info->order = orders[i];
-		INIT_LIST_HEAD(&info->list);
 		return info;
 	}
 	kfree(info);
@@ -142,7 +141,6 @@ static int ion_system_heap_allocate(struct ion_heap *heap,
 							heap);
 	struct sg_table *table;
 	struct scatterlist *sg;
-	int ret;
 	struct list_head pages;
 	struct page_info *info, *tmp_info;
 	int i = 0;
@@ -160,24 +158,23 @@ static int ion_system_heap_allocate(struct ion_heap *heap,
 		info = alloc_largest_available(sys_heap, buffer, size_remaining,
 						max_order);
 		if (!info)
-			goto err;
+			goto free_pages;
 		list_add_tail(&info->list, &pages);
-		size_remaining -= (1 << info->order) * PAGE_SIZE;
+		size_remaining -= PAGE_SIZE << info->order;
 		max_order = info->order;
 		i++;
 	}
-	table = kzalloc(sizeof(struct sg_table), GFP_KERNEL);
+	table = kmalloc(sizeof(struct sg_table), GFP_KERNEL);
 	if (!table)
-		goto err;
+		goto free_pages;
 
-	ret = sg_alloc_table(table, i, GFP_KERNEL);
-	if (ret)
-		goto err1;
+	if (sg_alloc_table(table, i, GFP_KERNEL))
+		goto free_table;
 
 	sg = table->sgl;
 	list_for_each_entry_safe(info, tmp_info, &pages, list) {
 		struct page *page = info->page;
-		sg_set_page(sg, page, (1 << info->order) * PAGE_SIZE, 0);
+		sg_set_page(sg, page, PAGE_SIZE << info->order, 0);
 		sg = sg_next(sg);
 		list_del(&info->list);
 		kfree(info);
@@ -185,9 +182,10 @@ static int ion_system_heap_allocate(struct ion_heap *heap,
 
 	buffer->priv_virt = table;
 	return 0;
-err1:
+
+free_table:
 	kfree(table);
-err:
+free_pages:
 	list_for_each_entry_safe(info, tmp_info, &pages, list) {
 		free_buffer_page(sys_heap, buffer, info->page, info->order);
 		kfree(info);
@@ -197,14 +195,12 @@ err:
 
 static void ion_system_heap_free(struct ion_buffer *buffer)
 {
-	struct ion_heap *heap = buffer->heap;
-	struct ion_system_heap *sys_heap = container_of(heap,
+	struct ion_system_heap *sys_heap = container_of(buffer->heap,
 							struct ion_system_heap,
 							heap);
 	struct sg_table *table = buffer->sg_table;
 	bool cached = ion_buffer_cached(buffer);
 	struct scatterlist *sg;
-	LIST_HEAD(pages);
 	int i;
 
 	/* uncached pages come from the page pools, zero them before returning
@@ -242,6 +238,7 @@ static int ion_system_heap_shrink(struct ion_heap *heap, gfp_t gfp_mask,
 
 	for (i = 0; i < num_orders; i++) {
 		struct ion_page_pool *pool = sys_heap->pools[i];
+
 		nr_total += ion_page_pool_shrink(pool, gfp_mask, nr_to_scan);
 	}
 
@@ -267,14 +264,16 @@ static int ion_system_heap_debug_show(struct ion_heap *heap, struct seq_file *s,
 							struct ion_system_heap,
 							heap);
 	int i;
+
 	for (i = 0; i < num_orders; i++) {
 		struct ion_page_pool *pool = sys_heap->pools[i];
+
 		seq_printf(s, "%d order %u highmem pages in pool = %lu total\n",
 			   pool->high_count, pool->order,
-			   (1 << pool->order) * PAGE_SIZE * pool->high_count);
+			   (PAGE_SIZE << pool->order) * pool->high_count);
 		seq_printf(s, "%d order %u lowmem pages in pool = %lu total\n",
 			   pool->low_count, pool->order,
-			   (1 << pool->order) * PAGE_SIZE * pool->low_count);
+			   (PAGE_SIZE << pool->order) * pool->low_count);
 	}
 	return 0;
 }
@@ -293,7 +292,7 @@ struct ion_heap *ion_system_heap_create(struct ion_platform_heap *unused)
 	heap->pools = kzalloc(sizeof(struct ion_page_pool *) * num_orders,
 			      GFP_KERNEL);
 	if (!heap->pools)
-		goto err_alloc_pools;
+		goto free_heap;
 	for (i = 0; i < num_orders; i++) {
 		struct ion_page_pool *pool;
 		gfp_t gfp_flags = low_order_gfp_flags;
@@ -302,18 +301,18 @@ struct ion_heap *ion_system_heap_create(struct ion_platform_heap *unused)
 			gfp_flags = high_order_gfp_flags;
 		pool = ion_page_pool_create(gfp_flags, orders[i]);
 		if (!pool)
-			goto err_create_pool;
+			goto destroy_pools;
 		heap->pools[i] = pool;
 	}
 
 	heap->heap.debug_show = ion_system_heap_debug_show;
 	return &heap->heap;
-err_create_pool:
-	for (i = 0; i < num_orders; i++)
-		if (heap->pools[i])
-			ion_page_pool_destroy(heap->pools[i]);
+
+destroy_pools:
+	while (i--)
+		ion_page_pool_destroy(heap->pools[i]);
 	kfree(heap->pools);
-err_alloc_pools:
+free_heap:
 	kfree(heap);
 	return ERR_PTR(-ENOMEM);
 }
@@ -356,15 +355,15 @@ static int ion_system_contig_heap_allocate(struct ion_heap *heap,
 	for (i = len >> PAGE_SHIFT; i < (1 << order); i++)
 		__free_page(page + i);
 
-	table = kzalloc(sizeof(struct sg_table), GFP_KERNEL);
+	table = kmalloc(sizeof(struct sg_table), GFP_KERNEL);
 	if (!table) {
 		ret = -ENOMEM;
-		goto out;
+		goto free_pages;
 	}
 
 	ret = sg_alloc_table(table, 1, GFP_KERNEL);
 	if (ret)
-		goto out;
+		goto free_table;
 
 	sg_set_page(table->sgl, page, len, 0);
 
@@ -374,10 +373,12 @@ static int ion_system_contig_heap_allocate(struct ion_heap *heap,
 
 	return 0;
 
-out:
+free_table:
+	kfree(table);
+free_pages:
 	for (i = 0; i < len >> PAGE_SHIFT; i++)
 		__free_page(page + i);
-	kfree(table);
+
 	return ret;
 }
 
@@ -443,4 +444,3 @@ void ion_system_contig_heap_destroy(struct ion_heap *heap)
 {
 	kfree(heap);
 }
-
diff --git a/drivers/staging/android/logger.c b/drivers/staging/android/logger.c
index d42f5785f09..2772e01b37f 100644
--- a/drivers/staging/android/logger.c
+++ b/drivers/staging/android/logger.c
@@ -108,6 +108,7 @@ static inline struct logger_log *file_get_log(struct file *file)
 {
 	if (file->f_mode & FMODE_READ) {
 		struct logger_reader *reader = file->private_data;
+
 		return reader->log;
 	} else
 		return file->private_data;
@@ -124,6 +125,7 @@ static struct logger_entry *get_entry_header(struct logger_log *log,
 		size_t off, struct logger_entry *scratch)
 {
 	size_t len = min(sizeof(struct logger_entry), log->size - off);
+
 	if (len != sizeof(struct logger_entry)) {
 		memcpy(((void *) scratch), log->buffer + off, len);
 		memcpy(((void *) scratch) + len, log->buffer,
@@ -642,6 +644,7 @@ static unsigned int logger_poll(struct file *file, poll_table *wait)
 static long logger_set_version(struct logger_reader *reader, void __user *arg)
 {
 	int version;
+
 	if (copy_from_user(&version, arg, sizeof(int)))
 		return -EFAULT;
 
diff --git a/drivers/staging/android/ram_console.h b/drivers/staging/android/ram_console.h
deleted file mode 100644
index 9f1125c1106..00000000000
--- a/drivers/staging/android/ram_console.h
+++ /dev/null
@@ -1,22 +0,0 @@
-/*
- * Copyright (C) 2010 Google, Inc.
- *
- * This software is licensed under the terms of the GNU General Public
- * License version 2, as published by the Free Software Foundation, and
- * may be copied, distributed, and modified under those terms.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- */
-
-#ifndef _INCLUDE_LINUX_PLATFORM_DATA_RAM_CONSOLE_H_
-#define _INCLUDE_LINUX_PLATFORM_DATA_RAM_CONSOLE_H_
-
-struct ram_console_platform_data {
-	const char *bootinfo;
-};
-
-#endif /* _INCLUDE_LINUX_PLATFORM_DATA_RAM_CONSOLE_H_ */
diff --git a/drivers/staging/android/sw_sync.c b/drivers/staging/android/sw_sync.c
index f24493ac65e..12a136ec1ce 100644
--- a/drivers/staging/android/sw_sync.c
+++ b/drivers/staging/android/sw_sync.c
@@ -97,6 +97,7 @@ static void sw_sync_pt_value_str(struct sync_pt *sync_pt,
 				       char *str, int size)
 {
 	struct sw_sync_pt *pt = (struct sw_sync_pt *)sync_pt;
+
 	snprintf(str, size, "%d", pt->value);
 }
 
@@ -156,6 +157,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/sync.c b/drivers/staging/android/sync.c
index 3d05f662110..18174f7c871 100644
--- a/drivers/staging/android/sync.c
+++ b/drivers/staging/android/sync.c
@@ -92,6 +92,10 @@ static void sync_timeline_free(struct kref *kref)
 void sync_timeline_destroy(struct sync_timeline *obj)
 {
 	obj->destroyed = true;
+	/*
+	 * Ensure timeline is marked as destroyed before
+	 * changing timeline's fences status.
+	 */
 	smp_wmb();
 
 	/*
@@ -384,6 +388,7 @@ static void sync_fence_detach_pts(struct sync_fence *fence)
 
 	list_for_each_safe(pos, n, &fence->pt_list_head) {
 		struct sync_pt *pt = container_of(pos, struct sync_pt, pt_list);
+
 		sync_timeline_remove_pt(pt);
 	}
 }
@@ -394,6 +399,7 @@ static void sync_fence_free_pts(struct sync_fence *fence)
 
 	list_for_each_safe(pos, n, &fence->pt_list_head) {
 		struct sync_pt *pt = container_of(pos, struct sync_pt, pt_list);
+
 		sync_pt_free(pt);
 	}
 }
@@ -827,6 +833,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);
@@ -856,18 +863,21 @@ static const char *sync_status_str(int status)
 static void sync_print_pt(struct seq_file *s, struct sync_pt *pt, bool fence)
 {
 	int status = pt->status;
+
 	seq_printf(s, "  %s%spt %s",
 		   fence ? pt->parent->name : "",
 		   fence ? "_" : "",
 		   sync_status_str(status));
 	if (pt->status) {
 		struct timeval tv = ktime_to_timeval(pt->timestamp);
+
 		seq_printf(s, "@%ld.%06ld", tv.tv_sec, tv.tv_usec);
 	}
 
 	if (pt->parent->ops->timeline_value_str &&
 	    pt->parent->ops->pt_value_str) {
 		char value[64];
+
 		pt->parent->ops->pt_value_str(pt, value, sizeof(value));
 		seq_printf(s, ": %s", value);
 		if (fence) {
@@ -892,6 +902,7 @@ static void sync_print_obj(struct seq_file *s, struct sync_timeline *obj)
 
 	if (obj->ops->timeline_value_str) {
 		char value[64];
+
 		obj->ops->timeline_value_str(obj, value, sizeof(value));
 		seq_printf(s, ": %s", value);
 	} else if (obj->ops->print_obj) {
@@ -1001,6 +1012,7 @@ static void sync_dump(void)
 	for (i = 0; i < s.count; i += DUMP_CHUNK) {
 		if ((s.count - i) > DUMP_CHUNK) {
 			char c = s.buf[i + DUMP_CHUNK];
+
 			s.buf[i + DUMP_CHUNK] = 0;
 			pr_cont("%s", s.buf + i);
 			s.buf[i + DUMP_CHUNK] = c;
diff --git a/drivers/staging/android/timed_gpio.c b/drivers/staging/android/timed_gpio.c
index 0c7fdc83b33..180c209a009 100644
--- a/drivers/staging/android/timed_gpio.c
+++ b/drivers/staging/android/timed_gpio.c
@@ -51,6 +51,7 @@ static int gpio_get_time(struct timed_output_dev *dev)
 	if (hrtimer_active(&data->timer)) {
 		ktime_t r = hrtimer_get_remaining(&data->timer);
 		struct timeval t = ktime_to_timeval(r);
+
 		return t.tv_sec * 1000 + t.tv_usec / 1000;
 	} else
 		return 0;
@@ -91,8 +92,8 @@ static int timed_gpio_probe(struct platform_device *pdev)
 		return -EBUSY;
 
 	gpio_data = devm_kzalloc(&pdev->dev,
-				sizeof(struct timed_gpio_data) * pdata->num_gpios,
-				GFP_KERNEL);
+			sizeof(struct timed_gpio_data) * pdata->num_gpios,
+			GFP_KERNEL);
 	if (!gpio_data)
 		return -ENOMEM;
 
diff --git a/drivers/staging/android/uapi/ion.h b/drivers/staging/android/uapi/ion.h
index f09e7c154d6..6aa49567337 100644
--- a/drivers/staging/android/uapi/ion.h
+++ b/drivers/staging/android/uapi/ion.h
@@ -27,12 +27,12 @@ typedef int ion_user_handle_t;
  * @ION_HEAP_TYPE_SYSTEM:	 memory allocated via vmalloc
  * @ION_HEAP_TYPE_SYSTEM_CONTIG: memory allocated via kmalloc
  * @ION_HEAP_TYPE_CARVEOUT:	 memory allocated from a prereserved
- * 				 carveout heap, allocations are physically
- * 				 contiguous
+ *				 carveout heap, allocations are physically
+ *				 contiguous
  * @ION_HEAP_TYPE_DMA:		 memory allocated via DMA API
  * @ION_NUM_HEAPS:		 helper for iterating over heaps, a bit mask
- * 				 is used to identify the heaps, so only 32
- * 				 total heap types are supported
+ *				 is used to identify the heaps, so only 32
+ *				 total heap types are supported
  */
 enum ion_heap_type {
 	ION_HEAP_TYPE_SYSTEM,
@@ -50,7 +50,7 @@ enum ion_heap_type {
 #define ION_HEAP_CARVEOUT_MASK		(1 << ION_HEAP_TYPE_CARVEOUT)
 #define ION_HEAP_TYPE_DMA_MASK		(1 << ION_HEAP_TYPE_DMA)
 
-#define ION_NUM_HEAP_IDS		sizeof(unsigned int) * 8
+#define ION_NUM_HEAP_IDS		(sizeof(unsigned int) * 8)
 
 /**
  * allocation flags - the lower 16 bits are used by core ion, the upper 16
@@ -78,7 +78,7 @@ enum ion_heap_type {
  * @align:		required alignment of the allocation
  * @heap_id_mask:	mask of heap ids to allocate from
  * @flags:		flags passed to heap
- * @handle:		pointer that will be populated with a cookie to use to 
+ * @handle:		pointer that will be populated with a cookie to use to
  *			refer to this allocation
  *
  * Provided by userspace as an argument to the ioctl
diff --git a/drivers/staging/bcm/Bcmchar.c b/drivers/staging/bcm/Bcmchar.c
index ae7490b4d70..606d5f5e921 100644
--- a/drivers/staging/bcm/Bcmchar.c
+++ b/drivers/staging/bcm/Bcmchar.c
@@ -1,6 +1,94 @@
 #include <linux/fs.h>
 
 #include "headers.h"
+
+static int bcm_handle_nvm_read_cmd(struct bcm_mini_adapter *Adapter,
+	PUCHAR pReadData, struct bcm_nvm_readwrite *stNVMReadWrite)
+{
+	INT Status = STATUS_FAILURE;
+
+	down(&Adapter->NVMRdmWrmLock);
+
+	if ((Adapter->IdleMode == TRUE) || (Adapter->bShutStatus == TRUE) ||
+			(Adapter->bPreparingForLowPowerMode == TRUE)) {
+
+		BCM_DEBUG_PRINT(Adapter,
+			DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+			"Device is in Idle/Shutdown Mode\n");
+		up(&Adapter->NVMRdmWrmLock);
+		kfree(pReadData);
+		return -EACCES;
+	}
+
+	Status = BeceemNVMRead(Adapter, (PUINT)pReadData,
+			       stNVMReadWrite->uiOffset,
+			       stNVMReadWrite->uiNumBytes);
+	up(&Adapter->NVMRdmWrmLock);
+
+	if (Status != STATUS_SUCCESS) {
+		kfree(pReadData);
+		return Status;
+	}
+
+	if (copy_to_user(stNVMReadWrite->pBuffer, pReadData,
+			stNVMReadWrite->uiNumBytes)) {
+		kfree(pReadData);
+		return -EFAULT;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int handle_flash2x_adapter(struct bcm_mini_adapter *Adapter,
+	PUCHAR pReadData, struct bcm_nvm_readwrite *stNVMReadWrite)
+{
+	/*
+	 * New Requirement:-
+	 * DSD section updation will be allowed in two case:-
+	 * 1.  if DSD sig is present in DSD header means dongle
+	 * is ok and updation is fruitfull
+	 * 2.  if point 1 failes then user buff should have
+	 * DSD sig. this point ensures that if dongle is
+	 * corrupted then user space program first modify
+	 * the DSD header with valid DSD sig so that this
+	 * as well as further write may be worthwhile.
+	 *
+	 * This restriction has been put assuming that
+	 * if DSD sig is corrupted, DSD data won't be
+	 * considered valid.
+	 */
+	INT Status;
+	ULONG ulDSDMagicNumInUsrBuff = 0;
+
+	Status = BcmFlash2xCorruptSig(Adapter, Adapter->eActiveDSD);
+	if (Status == STATUS_SUCCESS)
+		return STATUS_SUCCESS;
+
+	if (((stNVMReadWrite->uiOffset + stNVMReadWrite->uiNumBytes) !=
+			Adapter->uiNVMDSDSize) ||
+			(stNVMReadWrite->uiNumBytes < SIGNATURE_SIZE)) {
+
+		BCM_DEBUG_PRINT(Adapter, 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;
+	}
+
+	ulDSDMagicNumInUsrBuff =
+		ntohl(*(PUINT)(pReadData + stNVMReadWrite->uiNumBytes -
+		      SIGNATURE_SIZE));
+	if (ulDSDMagicNumInUsrBuff != DSD_IMAGE_MAGIC_NUMBER) {
+		BCM_DEBUG_PRINT(Adapter, 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;
+	}
+
+	return STATUS_SUCCESS;
+}
+
 /***************************************************************
 * Function	  - bcm_char_open()
 *
@@ -101,9 +189,11 @@ static ssize_t bcm_char_read(struct file *filp, char __user *buf, size_t size,
 	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));
+	wait_ret_val = wait_event_interruptible(
+				Adapter->process_read_wait_queue,
+				(pTarang->RxAppControlHead ||
+				Adapter->device_removed));
+
 	if ((wait_ret_val == -ERESTARTSYS)) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 				"Exiting as i've been asked to exit!!!\n");
@@ -151,7 +241,7 @@ static ssize_t bcm_char_read(struct file *filp, char __user *buf, size_t size,
 }
 
 static int bcm_char_ioctl_reg_read_private(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+					   struct bcm_mini_adapter *Adapter)
 {
 	struct bcm_rdm_buffer sRdmBuffer = {0};
 	struct bcm_ioctl_buffer IoBuffer;
@@ -202,7 +292,7 @@ static int bcm_char_ioctl_reg_read_private(void __user *argp,
 }
 
 static int bcm_char_ioctl_reg_write_private(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+					    struct bcm_mini_adapter *Adapter)
 {
 	struct bcm_wrm_buffer sWrmBuffer = {0};
 	struct bcm_ioctl_buffer IoBuffer;
@@ -249,7 +339,7 @@ static int bcm_char_ioctl_reg_write_private(void __user *argp,
 }
 
 static int bcm_char_ioctl_eeprom_reg_read(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+					  struct bcm_mini_adapter *Adapter)
 {
 	struct bcm_rdm_buffer sRdmBuffer = {0};
 	struct bcm_ioctl_buffer IoBuffer;
@@ -317,7 +407,8 @@ static int bcm_char_ioctl_eeprom_reg_read(void __user *argp,
 }
 
 static int bcm_char_ioctl_eeprom_reg_write(void __user *argp,
-	struct bcm_mini_adapter *Adapter, UINT cmd)
+					   struct bcm_mini_adapter *Adapter,
+					   UINT cmd)
 {
 	struct bcm_wrm_buffer sWrmBuffer = {0};
 	struct bcm_ioctl_buffer IoBuffer;
@@ -383,7 +474,7 @@ static int bcm_char_ioctl_eeprom_reg_write(void __user *argp,
 }
 
 static int bcm_char_ioctl_gpio_set_request(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+					   struct bcm_mini_adapter *Adapter)
 {
 	struct bcm_gpio_info gpio_info = {0};
 	struct bcm_ioctl_buffer IoBuffer;
@@ -410,7 +501,8 @@ static int bcm_char_ioctl_gpio_set_request(void __user *argp,
 	if (IoBuffer.InputLength > sizeof(gpio_info))
 		return -EINVAL;
 
-	if (copy_from_user(&gpio_info, IoBuffer.InputBuffer, IoBuffer.InputLength))
+	if (copy_from_user(&gpio_info, IoBuffer.InputBuffer,
+			   IoBuffer.InputLength))
 		return -EFAULT;
 
 	uiBit  = gpio_info.uiGpioNumber;
@@ -492,7 +584,7 @@ static int bcm_char_ioctl_gpio_set_request(void __user *argp,
 }
 
 static int bcm_char_ioctl_led_thread_state_change_req(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+		struct bcm_mini_adapter *Adapter)
 {
 	struct bcm_user_thread_req threadReq = {0};
 	struct bcm_ioctl_buffer IoBuffer;
@@ -516,7 +608,8 @@ static int bcm_char_ioctl_led_thread_state_change_req(void __user *argp,
 	if (IoBuffer.InputLength > sizeof(threadReq))
 		return -EINVAL;
 
-	if (copy_from_user(&threadReq, IoBuffer.InputBuffer, IoBuffer.InputLength))
+	if (copy_from_user(&threadReq, IoBuffer.InputBuffer,
+			   IoBuffer.InputLength))
 		return -EFAULT;
 
 	/* if LED thread is running(Actively or Inactively)
@@ -542,7 +635,7 @@ static int bcm_char_ioctl_led_thread_state_change_req(void __user *argp,
 }
 
 static int bcm_char_ioctl_gpio_status_request(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+		struct bcm_mini_adapter *Adapter)
 {
 	struct bcm_gpio_info gpio_info = {0};
 	struct bcm_ioctl_buffer IoBuffer;
@@ -584,7 +677,7 @@ static int bcm_char_ioctl_gpio_status_request(void __user *argp,
 }
 
 static int bcm_char_ioctl_gpio_multi_request(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+		struct bcm_mini_adapter *Adapter)
 {
 	struct bcm_gpio_multi_info gpio_multi_info[MAX_IDX];
 	struct bcm_gpio_multi_info *pgpio_multi_info =
@@ -594,7 +687,8 @@ static int bcm_char_ioctl_gpio_multi_request(void __user *argp,
 	INT Status = STATUS_FAILURE;
 	int bytes;
 
-	memset(pgpio_multi_info, 0, MAX_IDX * sizeof(struct bcm_gpio_multi_info));
+	memset(pgpio_multi_info, 0,
+	       MAX_IDX * sizeof(struct bcm_gpio_multi_info));
 
 	if ((Adapter->IdleMode == TRUE) ||
 		(Adapter->bShutStatus == TRUE) ||
@@ -610,11 +704,11 @@ static int bcm_char_ioctl_gpio_multi_request(void __user *argp,
 		IoBuffer.OutputLength = sizeof(gpio_multi_info);
 
 	if (copy_from_user(&gpio_multi_info, IoBuffer.InputBuffer,
-		IoBuffer.InputLength))
+			   IoBuffer.InputLength))
 		return -EFAULT;
 
-	if (IsReqGpioIsLedInNVM(Adapter,
-		pgpio_multi_info[WIMAX_IDX].uiGPIOMask) == false) {
+	if (IsReqGpioIsLedInNVM(Adapter, pgpio_multi_info[WIMAX_IDX].uiGPIOMask)
+			== false) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
 				DBG_LVL_ALL,
 				"Sorry, Requested GPIO<0x%X> is not correspond to NVM LED bit map<0x%X>!!!",
@@ -627,7 +721,7 @@ 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 *)ucResetValue = pgpio_multi_info[WIMAX_IDX].uiGPIOMask &
 			pgpio_multi_info[WIMAX_IDX].uiGPIOCommand &
 			pgpio_multi_info[WIMAX_IDX].uiGPIOValue;
 
@@ -643,7 +737,8 @@ static int bcm_char_ioctl_gpio_multi_request(void __user *argp,
 		}
 
 		/* Clear to 0's in GPIO OUTPUT REGISTER */
-		*(UINT *)ucResetValue = (pgpio_multi_info[WIMAX_IDX].uiGPIOMask &
+		*(UINT *)ucResetValue =
+			(pgpio_multi_info[WIMAX_IDX].uiGPIOMask &
 			pgpio_multi_info[WIMAX_IDX].uiGPIOCommand &
 			(~(pgpio_multi_info[WIMAX_IDX].uiGPIOValue)));
 
@@ -661,7 +756,7 @@ static int bcm_char_ioctl_gpio_multi_request(void __user *argp,
 
 	if (pgpio_multi_info[WIMAX_IDX].uiGPIOMask) {
 		bytes = rdmaltWithLock(Adapter, (UINT)GPIO_PIN_STATE_REGISTER,
-			(PUINT)ucResetValue, sizeof(UINT));
+				       (PUINT)ucResetValue, sizeof(UINT));
 
 		if (bytes < 0) {
 			Status = bytes;
@@ -672,7 +767,8 @@ static int bcm_char_ioctl_gpio_multi_request(void __user *argp,
 			Status = STATUS_SUCCESS;
 		}
 
-		pgpio_multi_info[WIMAX_IDX].uiGPIOValue = (*(UINT *)ucResetValue &
+		pgpio_multi_info[WIMAX_IDX].uiGPIOValue =
+			(*(UINT *)ucResetValue &
 			pgpio_multi_info[WIMAX_IDX].uiGPIOMask);
 	}
 
@@ -688,7 +784,7 @@ static int bcm_char_ioctl_gpio_multi_request(void __user *argp,
 }
 
 static int bcm_char_ioctl_gpio_mode_request(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+		struct bcm_mini_adapter *Adapter)
 {
 	struct bcm_gpio_multi_mode gpio_multi_mode[MAX_IDX];
 	struct bcm_gpio_multi_mode *pgpio_multi_mode =
@@ -728,8 +824,8 @@ static int bcm_char_ioctl_gpio_mode_request(void __user *argp,
 	}
 
 	/* Validating the request */
-	if (IsReqGpioIsLedInNVM(Adapter,
-		pgpio_multi_mode[WIMAX_IDX].uiGPIOMask) == false) {
+	if (IsReqGpioIsLedInNVM(Adapter, pgpio_multi_mode[WIMAX_IDX].uiGPIOMask)
+			== false) {
 		BCM_DEBUG_PRINT(Adapter, 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,
@@ -739,11 +835,13 @@ static int bcm_char_ioctl_gpio_mode_request(void __user *argp,
 
 	if (pgpio_multi_mode[WIMAX_IDX].uiGPIOMask) {
 		/* write all OUT's (1's) */
-		*(UINT *) ucResetValue |= (pgpio_multi_mode[WIMAX_IDX].uiGPIOMode &
+		*(UINT *) ucResetValue |=
+			(pgpio_multi_mode[WIMAX_IDX].uiGPIOMode &
 					pgpio_multi_mode[WIMAX_IDX].uiGPIOMask);
 
 		/* write all IN's (0's) */
-		*(UINT *) ucResetValue &= ~((~pgpio_multi_mode[WIMAX_IDX].uiGPIOMode) &
+		*(UINT *) ucResetValue &=
+			~((~pgpio_multi_mode[WIMAX_IDX].uiGPIOMode) &
 					pgpio_multi_mode[WIMAX_IDX].uiGPIOMask);
 
 		/* Currently implemented return the modes of all GPIO's
@@ -779,7 +877,7 @@ static int bcm_char_ioctl_gpio_mode_request(void __user *argp,
 }
 
 static int bcm_char_ioctl_misc_request(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+		struct bcm_mini_adapter *Adapter)
 {
 	struct bcm_ioctl_buffer IoBuffer;
 	PVOID pvBuffer = NULL;
@@ -801,9 +899,11 @@ static int bcm_char_ioctl_misc_request(void __user *argp,
 		return PTR_ERR(pvBuffer);
 
 	down(&Adapter->LowPowerModeSync);
-	Status = wait_event_interruptible_timeout(Adapter->lowpower_mode_wait_queue,
-		!Adapter->bPreparingForLowPowerMode,
-		(1 * HZ));
+	Status = wait_event_interruptible_timeout(
+			Adapter->lowpower_mode_wait_queue,
+			!Adapter->bPreparingForLowPowerMode,
+			(1 * HZ));
+
 	if (Status == -ERESTARTSYS)
 		goto cntrlEnd;
 
@@ -822,7 +922,7 @@ cntrlEnd:
 }
 
 static int bcm_char_ioctl_buffer_download_start(
-	struct bcm_mini_adapter *Adapter)
+		struct bcm_mini_adapter *Adapter)
 {
 	INT Status;
 
@@ -833,7 +933,8 @@ static int bcm_char_ioctl_buffer_download_start(
 	}
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-		"Starting the firmware download PID =0x%x!!!!\n", current->pid);
+			"Starting the firmware download PID =0x%x!!!!\n",
+			current->pid);
 
 	if (down_trylock(&Adapter->fw_download_sema))
 		return -EBUSY;
@@ -858,7 +959,7 @@ static int bcm_char_ioctl_buffer_download_start(
 }
 
 static int bcm_char_ioctl_buffer_download(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+		struct bcm_mini_adapter *Adapter)
 {
 	struct bcm_firmware_info *psFwInfo = NULL;
 	struct bcm_ioctl_buffer IoBuffer;
@@ -943,7 +1044,7 @@ static int bcm_char_ioctl_buffer_download(void __user *argp,
 }
 
 static int bcm_char_ioctl_buffer_download_stop(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+		struct bcm_mini_adapter *Adapter)
 {
 	INT Status;
 	int timeout = 0;
@@ -1036,7 +1137,7 @@ static int bcm_char_ioctl_chip_reset(struct bcm_mini_adapter *Adapter)
 }
 
 static int bcm_char_ioctl_qos_threshold(ULONG arg,
-	struct bcm_mini_adapter *Adapter)
+					struct bcm_mini_adapter *Adapter)
 {
 	USHORT uiLoopIndex;
 
@@ -1388,7 +1489,6 @@ static int bcm_char_ioctl_nvm_rw(void __user *argp,
 	struct timeval tv0, tv1;
 	struct bcm_ioctl_buffer IoBuffer;
 	PUCHAR pReadData = NULL;
-	ULONG ulDSDMagicNumInUsrBuff = 0;
 	INT Status = STATUS_FAILURE;
 
 	memset(&tv0, 0, sizeof(struct timeval));
@@ -1438,34 +1538,10 @@ static int bcm_char_ioctl_nvm_rw(void __user *argp,
 
 	do_gettimeofday(&tv0);
 	if (IOCTL_BCM_NVM_READ == cmd) {
-		down(&Adapter->NVMRdmWrmLock);
-
-		if ((Adapter->IdleMode == TRUE) ||
-			(Adapter->bShutStatus == TRUE) ||
-			(Adapter->bPreparingForLowPowerMode == TRUE)) {
-
-			BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"Device is in Idle/Shutdown Mode\n");
-			up(&Adapter->NVMRdmWrmLock);
-			kfree(pReadData);
-			return -EACCES;
-		}
-
-		Status = BeceemNVMRead(Adapter, (PUINT)pReadData,
-			stNVMReadWrite.uiOffset, stNVMReadWrite.uiNumBytes);
-		up(&Adapter->NVMRdmWrmLock);
-
-		if (Status != STATUS_SUCCESS) {
-			kfree(pReadData);
-			return Status;
-		}
-
-		if (copy_to_user(stNVMReadWrite.pBuffer, pReadData,
-			stNVMReadWrite.uiNumBytes)) {
-			kfree(pReadData);
-			return -EFAULT;
-		}
+		int ret = bcm_handle_nvm_read_cmd(Adapter, pReadData,
+				&stNVMReadWrite);
+		if (ret != STATUS_SUCCESS)
+			return ret;
 	} else {
 		down(&Adapter->NVMRdmWrmLock);
 
@@ -1483,48 +1559,11 @@ static int bcm_char_ioctl_nvm_rw(void __user *argp,
 
 		Adapter->bHeaderChangeAllowed = TRUE;
 		if (IsFlash2x(Adapter)) {
-			/*
-			 * New Requirement:-
-			 * DSD section updation will be allowed in two case:-
-			 * 1.  if DSD sig is present in DSD header means dongle
-			 * is ok and updation is fruitfull
-			 * 2.  if point 1 failes then user buff should have
-			 * DSD sig. this point ensures that if dongle is
-			 * corrupted then user space program first modify
-			 * the DSD header with valid DSD sig so that this
-			 * as well as further write may be worthwhile.
-			 *
-			 * This restriction has been put assuming that
-			 * if DSD sig is corrupted, DSD data won't be
-			 * considered valid.
-			 */
-
-			Status = BcmFlash2xCorruptSig(Adapter,
-				Adapter->eActiveDSD);
-			if (Status != STATUS_SUCCESS) {
-				if (((stNVMReadWrite.uiOffset + stNVMReadWrite.uiNumBytes) !=
-					Adapter->uiNVMDSDSize) ||
-					(stNVMReadWrite.uiNumBytes < SIGNATURE_SIZE)) {
-
-					BCM_DEBUG_PRINT(Adapter, 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;
-				}
-
-				ulDSDMagicNumInUsrBuff = ntohl(*(PUINT)(pReadData + stNVMReadWrite.uiNumBytes - SIGNATURE_SIZE));
-				if (ulDSDMagicNumInUsrBuff !=
-					DSD_IMAGE_MAGIC_NUMBER) {
-					BCM_DEBUG_PRINT(Adapter,
-					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;
-				}
-			}
+			int ret = handle_flash2x_adapter(Adapter,
+							pReadData,
+							&stNVMReadWrite);
+			if (ret != STATUS_SUCCESS)
+				return ret;
 		}
 
 		Status = BeceemNVMWrite(Adapter, (PUINT)pReadData,
@@ -1546,7 +1585,8 @@ static int bcm_char_ioctl_nvm_rw(void __user *argp,
 	do_gettimeofday(&tv1);
 	BCM_DEBUG_PRINT(Adapter, 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);
+		(tv1.tv_sec - tv0.tv_sec)*1000 +
+		(tv1.tv_usec - tv0.tv_usec)/1000);
 
 	kfree(pReadData);
 	return STATUS_SUCCESS;
@@ -1582,13 +1622,17 @@ static int bcm_char_ioctl_flash2x_section_read(void __user *argp,
 		return -EFAULT;
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"\nsFlash2xRead.Section :%x", sFlash2xRead.Section);
+			"\nsFlash2xRead.Section :%x",
+			sFlash2xRead.Section);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"\nsFlash2xRead.offset :%x", sFlash2xRead.offset);
+			"\nsFlash2xRead.offset :%x",
+			sFlash2xRead.offset);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"\nsFlash2xRead.numOfBytes :%x", sFlash2xRead.numOfBytes);
+			"\nsFlash2xRead.numOfBytes :%x",
+			sFlash2xRead.numOfBytes);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"\nsFlash2xRead.bVerify :%x\n", sFlash2xRead.bVerify);
+			"\nsFlash2xRead.bVerify :%x\n",
+			sFlash2xRead.bVerify);
 
 	/* This was internal to driver for raw read.
 	 * now it has ben exposed to user space app.
@@ -1608,7 +1652,7 @@ static int bcm_char_ioctl_flash2x_section_read(void __user *argp,
 
 	if (pReadBuff == NULL) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Memory allocation failed for Flash 2.x Read Structure");
+				"Memory allocation failed for Flash 2.x Read Structure");
 		return -ENOMEM;
 	}
 	down(&Adapter->NVMRdmWrmLock);
@@ -1618,7 +1662,8 @@ static int bcm_char_ioctl_flash2x_section_read(void __user *argp,
 		(Adapter->bPreparingForLowPowerMode == TRUE)) {
 
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
-			DBG_LVL_ALL, "Device is in Idle/Shutdown Mode\n");
+				DBG_LVL_ALL,
+				"Device is in Idle/Shutdown Mode\n");
 		up(&Adapter->NVMRdmWrmLock);
 		kfree(pReadBuff);
 		return -EACCES;
@@ -1735,8 +1780,10 @@ static int bcm_char_ioctl_flash2x_section_write(void __user *argp,
 
 	/* extracting the remainder of the given offset. */
 	WriteBytes = Adapter->uiSectorSize;
-	if (WriteOffset % Adapter->uiSectorSize)
-		WriteBytes = Adapter->uiSectorSize - (WriteOffset % Adapter->uiSectorSize);
+	if (WriteOffset % Adapter->uiSectorSize) {
+		WriteBytes = Adapter->uiSectorSize -
+			(WriteOffset % Adapter->uiSectorSize);
+	}
 
 	if (NOB < WriteBytes)
 		WriteBytes = NOB;
@@ -1769,8 +1816,10 @@ static int bcm_char_ioctl_flash2x_section_write(void __user *argp,
 
 		/* Writing the data from Flash 2.x */
 		Status = BcmFlash2xBulkWrite(Adapter, (PUINT)pWriteBuff,
-			sFlash2xWrite.Section, WriteOffset, WriteBytes,
-			sFlash2xWrite.bVerify);
+					     sFlash2xWrite.Section,
+					     WriteOffset,
+					     WriteBytes,
+					     sFlash2xWrite.bVerify);
 
 		if (Status) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
@@ -1800,7 +1849,6 @@ static int bcm_char_ioctl_flash2x_section_bitmap(void __user *argp,
 {
 	struct bcm_flash2x_bitmap *psFlash2xBitMap;
 	struct bcm_ioctl_buffer IoBuffer;
-	INT Status = STATUS_FAILURE;
 
 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 	"IOCTL_BCM_GET_FLASH2X_SECTION_BITMAP Called");
@@ -1811,7 +1859,9 @@ BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 	if (IoBuffer.OutputLength != sizeof(struct bcm_flash2x_bitmap))
 		return -EINVAL;
 
-	psFlash2xBitMap = kzalloc(sizeof(struct bcm_flash2x_bitmap), GFP_KERNEL);
+	psFlash2xBitMap = kzalloc(sizeof(struct bcm_flash2x_bitmap),
+			GFP_KERNEL);
+
 	if (psFlash2xBitMap == NULL) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
 			"Memory is not available");
@@ -1841,7 +1891,7 @@ BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 	}
 
 	kfree(psFlash2xBitMap);
-	return Status;
+	return STATUS_FAILURE;
 }
 
 static int bcm_char_ioctl_set_active_section(void __user *argp,
@@ -1852,23 +1902,24 @@ static int bcm_char_ioctl_set_active_section(void __user *argp,
 	struct bcm_ioctl_buffer IoBuffer;
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"IOCTL_BCM_SET_ACTIVE_SECTION Called");
+			"IOCTL_BCM_SET_ACTIVE_SECTION Called");
 
 	if (IsFlash2x(Adapter) != TRUE) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Flash Does not have 2.x map");
+				"Flash Does not have 2.x map");
 		return -EINVAL;
 	}
 
-	Status = copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer));
+	Status = copy_from_user(&IoBuffer, argp,
+				sizeof(struct bcm_ioctl_buffer));
 	if (Status) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Copy of IOCTL BUFFER failed");
+				"Copy of IOCTL BUFFER failed");
 		return -EFAULT;
 	}
 
 	Status = copy_from_user(&eFlash2xSectionVal,
-		IoBuffer.InputBuffer, sizeof(INT));
+				IoBuffer.InputBuffer, sizeof(INT));
 	if (Status) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
 			"Copy of flash section val failed");
@@ -1882,7 +1933,7 @@ static int bcm_char_ioctl_set_active_section(void __user *argp,
 		(Adapter->bPreparingForLowPowerMode == TRUE)) {
 
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"Device is in Idle/Shutdown Mode\n");
+				"Device is in Idle/Shutdown Mode\n");
 		up(&Adapter->NVMRdmWrmLock);
 		return -EACCES;
 	}
@@ -1890,8 +1941,8 @@ static int bcm_char_ioctl_set_active_section(void __user *argp,
 	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);
+				"Failed to make it's priority Highest. Status %d",
+				Status);
 
 	up(&Adapter->NVMRdmWrmLock);
 
@@ -1906,57 +1957,59 @@ static int bcm_char_ioctl_copy_section(void __user *argp,
 	INT Status = STATUS_SUCCESS;
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"IOCTL_BCM_COPY_SECTION  Called");
+			"IOCTL_BCM_COPY_SECTION  Called");
 
 	Adapter->bAllDSDWriteAllow = false;
 	if (IsFlash2x(Adapter) != TRUE) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Flash Does not have 2.x map");
+				"Flash Does not have 2.x map");
 		return -EINVAL;
 	}
 
-	Status = copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer));
+	Status = copy_from_user(&IoBuffer, 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);
+				"Copy of IOCTL BUFFER failed Status :%d",
+				Status);
 		return -EFAULT;
 	}
 
 	Status = copy_from_user(&sCopySectStrut, IoBuffer.InputBuffer,
-			sizeof(struct bcm_flash2x_copy_section));
+				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);
+				"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);
+			"Source SEction :%x", sCopySectStrut.SrcSection);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"Destination SEction :%x", sCopySectStrut.DstSection);
+			"Destination SEction :%x", sCopySectStrut.DstSection);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"offset :%x", sCopySectStrut.offset);
+			"offset :%x", sCopySectStrut.offset);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"NOB :%x", sCopySectStrut.numOfBytes);
+			"NOB :%x", sCopySectStrut.numOfBytes);
 
 	if (IsSectionExistInFlash(Adapter, sCopySectStrut.SrcSection) == false) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Source Section<%x> does not exist in Flash ",
-			sCopySectStrut.SrcSection);
+				"Source Section<%x> does not exist in Flash ",
+				sCopySectStrut.SrcSection);
 		return -EINVAL;
 	}
 
 	if (IsSectionExistInFlash(Adapter, sCopySectStrut.DstSection) == false) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Destinatio Section<%x> does not exist in Flash ",
-			sCopySectStrut.DstSection);
+				"Destinatio Section<%x> does not exist in Flash ",
+				sCopySectStrut.DstSection);
 		return -EINVAL;
 	}
 
 	if (sCopySectStrut.SrcSection == sCopySectStrut.DstSection) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"Source and Destination section should be different");
+				"Source and Destination section should be different");
 		return -EINVAL;
 	}
 
@@ -1967,7 +2020,7 @@ static int bcm_char_ioctl_copy_section(void __user *argp,
 		(Adapter->bPreparingForLowPowerMode == TRUE)) {
 
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"Device is in Idle/Shutdown Mode\n");
+				"Device is in Idle/Shutdown Mode\n");
 		up(&Adapter->NVMRdmWrmLock);
 		return -EACCES;
 	}
@@ -1976,13 +2029,13 @@ static int bcm_char_ioctl_copy_section(void __user *argp,
 		sCopySectStrut.SrcSection == ISO_IMAGE2) {
 		if (IsNonCDLessDevice(Adapter)) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-				"Device is Non-CDLess hence won't have ISO !!");
+					"Device is Non-CDLess hence won't have ISO !!");
 			Status = -EINVAL;
 		} else if (sCopySectStrut.numOfBytes == 0) {
 			Status = BcmCopyISO(Adapter, sCopySectStrut);
 		} else {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-				"Partial Copy of ISO section is not Allowed..");
+					"Partial Copy of ISO section is not Allowed..");
 			Status = STATUS_FAILURE;
 		}
 		up(&Adapter->NVMRdmWrmLock);
@@ -2004,18 +2057,19 @@ static int bcm_char_ioctl_get_flash_cs_info(void __user *argp,
 	INT Status = STATUS_SUCCESS;
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		" IOCTL_BCM_GET_FLASH_CS_INFO Called");
+			" IOCTL_BCM_GET_FLASH_CS_INFO Called");
 
-	Status = copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer));
+	Status = copy_from_user(&IoBuffer, argp,
+			sizeof(struct bcm_ioctl_buffer));
 	if (Status) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Copy of IOCTL BUFFER failed");
+				"Copy of IOCTL BUFFER failed");
 		return -EFAULT;
 	}
 
 	if (Adapter->eNVMType != NVM_FLASH) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Connected device does not have flash");
+				"Connected device does not have flash");
 		return -EINVAL;
 	}
 
@@ -2024,15 +2078,15 @@ static int bcm_char_ioctl_get_flash_cs_info(void __user *argp,
 			return -EINVAL;
 
 		if (copy_to_user(IoBuffer.OutputBuffer,
-			Adapter->psFlash2xCSInfo,
-			sizeof(struct bcm_flash2x_cs_info)))
+				 Adapter->psFlash2xCSInfo,
+				 sizeof(struct bcm_flash2x_cs_info)))
 			return -EFAULT;
 	} else {
 		if (IoBuffer.OutputLength < sizeof(struct bcm_flash_cs_info))
 			return -EINVAL;
 
 		if (copy_to_user(IoBuffer.OutputBuffer, Adapter->psFlashCSInfo,
-			sizeof(struct bcm_flash_cs_info)))
+				 sizeof(struct bcm_flash_cs_info)))
 			return -EFAULT;
 	}
 	return Status;
@@ -2048,45 +2102,46 @@ static int bcm_char_ioctl_select_dsd(void __user *argp,
 
 	eFlash2xSectionVal = NO_SECTION_VAL;
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"IOCTL_BCM_SELECT_DSD Called");
+			"IOCTL_BCM_SELECT_DSD Called");
 
 	if (IsFlash2x(Adapter) != TRUE) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Flash Does not have 2.x map");
+				"Flash Does not have 2.x map");
 		return -EINVAL;
 	}
 
-	Status = copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer));
+	Status = copy_from_user(&IoBuffer, argp,
+				sizeof(struct bcm_ioctl_buffer));
 	if (Status) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Copy of IOCTL BUFFER failed");
+				"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,
-			"Copy of flash section val failed");
+				"Copy of flash section val failed");
 		return -EFAULT;
 	}
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"Read Section :%d", eFlash2xSectionVal);
+			"Read Section :%d", eFlash2xSectionVal);
 	if ((eFlash2xSectionVal != DSD0) &&
 		(eFlash2xSectionVal != DSD1) &&
 		(eFlash2xSectionVal != DSD2)) {
 
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Passed section<%x> is not DSD section",
-			eFlash2xSectionVal);
+				"Passed section<%x> is not DSD section",
+				eFlash2xSectionVal);
 		return STATUS_FAILURE;
 	}
 
 	SectOfset = BcmGetSectionValStartOffset(Adapter, eFlash2xSectionVal);
 	if (SectOfset == INVALID_OFFSET) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Provided Section val <%d> does not exist in Flash 2.x",
-			eFlash2xSectionVal);
+				"Provided Section val <%d> does not exist in Flash 2.x",
+				eFlash2xSectionVal);
 		return -EINVAL;
 	}
 
@@ -2112,14 +2167,14 @@ static int bcm_char_ioctl_nvm_raw_read(void __user *argp,
 
 	if (Adapter->eNVMType != NVM_FLASH) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"NVM TYPE is not Flash");
+				"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,
-			"copy_from_user 1 failed\n");
+				"copy_from_user 1 failed\n");
 		return -EFAULT;
 	}
 
@@ -2141,7 +2196,7 @@ static int bcm_char_ioctl_nvm_raw_read(void __user *argp,
 	pReadBuff = kzalloc(BuffSize , GFP_KERNEL);
 	if (pReadBuff == NULL) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Memory allocation failed for Flash 2.x Read Structure");
+				"Memory allocation failed for Flash 2.x Read Structure");
 		return -ENOMEM;
 	}
 	down(&Adapter->NVMRdmWrmLock);
@@ -2150,8 +2205,8 @@ static int bcm_char_ioctl_nvm_raw_read(void __user *argp,
 		(Adapter->bShutStatus == TRUE) ||
 		(Adapter->bPreparingForLowPowerMode == TRUE)) {
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
-			DBG_LVL_ALL, "Device is in Idle/Shutdown Mode\n");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+				"Device is in Idle/Shutdown Mode\n");
 		kfree(pReadBuff);
 		up(&Adapter->NVMRdmWrmLock);
 		return -EACCES;
@@ -2170,17 +2225,19 @@ static int bcm_char_ioctl_nvm_raw_read(void __user *argp,
 			ReadOffset, ReadBytes);
 		if (Status) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-				"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);
+				       DBG_LVL_ALL, pReadBuff, ReadBytes);
 
 		Status = copy_to_user(OutPutBuff, pReadBuff, ReadBytes);
 		if (Status) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-				"Copy to use failed with status :%d", Status);
+					"Copy to use failed with status :%d",
+					Status);
 			up(&Adapter->NVMRdmWrmLock);
 			kfree(pReadBuff);
 			return -EFAULT;
@@ -2205,26 +2262,27 @@ static int bcm_char_ioctl_cntrlmsg_mask(void __user *argp,
 	ULONG RxCntrlMsgBitMask = 0;
 
 	/* Copy Ioctl Buffer structure */
-	Status = copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer));
+	Status = copy_from_user(&IoBuffer, argp,
+			sizeof(struct bcm_ioctl_buffer));
 	if (Status) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"copy of Ioctl buffer is failed from user space");
+				"copy of Ioctl buffer is failed from user space");
 		return -EFAULT;
 	}
 
 	if (IoBuffer.InputLength != sizeof(unsigned long))
 		return -EINVAL;
 
-	Status = copy_from_user(&RxCntrlMsgBitMask,
-		IoBuffer.InputBuffer, IoBuffer.InputLength);
+	Status = copy_from_user(&RxCntrlMsgBitMask, IoBuffer.InputBuffer,
+				IoBuffer.InputLength);
 	if (Status) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"copy of control bit mask failed from user space");
+				"copy of control bit mask failed from user space");
 		return -EFAULT;
 	}
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"\n Got user defined cntrl msg bit mask :%lx",
-		RxCntrlMsgBitMask);
+			"\n Got user defined cntrl msg bit mask :%lx",
+			RxCntrlMsgBitMask);
 	pTarang->RxCntrlMsgBitMask = RxCntrlMsgBitMask;
 
 	return Status;
@@ -2236,8 +2294,8 @@ static int bcm_char_ioctl_get_device_driver_info(void __user *argp,
 	struct bcm_driver_info DevInfo;
 	struct bcm_ioctl_buffer IoBuffer;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
-		DBG_LVL_ALL, "Called IOCTL_BCM_GET_DEVICE_DRIVER_INFO\n");
+	BCM_DEBUG_PRINT(Adapter, 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;
@@ -2265,7 +2323,7 @@ static int bcm_char_ioctl_time_since_net_entry(void __user *argp,
 	struct bcm_ioctl_buffer IoBuffer;
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"IOCTL_BCM_TIME_SINCE_NET_ENTRY called");
+			"IOCTL_BCM_TIME_SINCE_NET_ENTRY called");
 
 	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
@@ -2277,7 +2335,7 @@ static int bcm_char_ioctl_time_since_net_entry(void __user *argp,
 		get_seconds() - Adapter->liTimeSinceLastNetEntry;
 
 	if (copy_to_user(IoBuffer.OutputBuffer, &stTimeElapsedSinceNetEntry,
-		sizeof(struct bcm_time_elapsed)))
+			 sizeof(struct bcm_time_elapsed)))
 		return -EFAULT;
 
 	return STATUS_SUCCESS;
@@ -2355,7 +2413,8 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 		return Status;
 
 	case BCM_LED_THREAD_STATE_CHANGE_REQ:
-		Status = bcm_char_ioctl_led_thread_state_change_req(argp, Adapter);
+		Status = bcm_char_ioctl_led_thread_state_change_req(argp,
+								    Adapter);
 		return Status;
 
 	case IOCTL_BCM_GPIO_STATUS_REQUEST:
@@ -2394,13 +2453,15 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 
 	case IOCTL_BE_BUCKET_SIZE:
 		Status = 0;
-		if (get_user(Adapter->BEBucketSize, (unsigned long __user *)arg))
+		if (get_user(Adapter->BEBucketSize,
+			     (unsigned long __user *)arg))
 			Status = -EFAULT;
 		break;
 
 	case IOCTL_RTPS_BUCKET_SIZE:
 		Status = 0;
-		if (get_user(Adapter->rtPSBucketSize, (unsigned long __user *)arg))
+		if (get_user(Adapter->rtPSBucketSize,
+			     (unsigned long __user *)arg))
 			Status = -EFAULT;
 		break;
 
@@ -2420,7 +2481,7 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 
 	case IOCTL_GET_PACK_INFO:
 		if (copy_to_user(argp, &Adapter->PackInfo,
-			sizeof(struct bcm_packet_info)*NO_OF_QUEUES))
+				 sizeof(struct bcm_packet_info)*NO_OF_QUEUES))
 			return -EFAULT;
 		Status = STATUS_SUCCESS;
 		break;
@@ -2451,7 +2512,7 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 
 	case IOCTL_BCM_WAKE_UP_DEVICE_FROM_IDLE:
 		if ((false == Adapter->bTriedToWakeUpFromlowPowerMode) &&
-			(TRUE == Adapter->IdleMode)) {
+				(TRUE == Adapter->IdleMode)) {
 			Adapter->usIdleModePattern = ABORT_IDLE_MODE;
 			Adapter->bWakeUpDevice = TRUE;
 			wake_up(&Adapter->process_rx_cntrlpkt);
@@ -2501,7 +2562,7 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 		/* Right Now we are taking care of only DSD */
 		Adapter->bAllDSDWriteAllow = false;
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"IOCTL_BCM_IDENTIFY_ACTIVE_SECTION called");
+				"IOCTL_BCM_IDENTIFY_ACTIVE_SECTION called");
 		Status = STATUS_SUCCESS;
 		break;
 
@@ -2534,8 +2595,8 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 		return Status;
 
 	case IOCTL_CLOSE_NOTIFICATION:
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
-			DBG_LVL_ALL, "IOCTL_CLOSE_NOTIFICATION");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+				"IOCTL_CLOSE_NOTIFICATION");
 		break;
 
 	default:
@@ -2569,8 +2630,8 @@ int register_control_device_interface(struct bcm_mini_adapter *Adapter)
 	}
 
 	Adapter->pstCreatedClassDevice = device_create(bcm_class, NULL,
-						MKDEV(Adapter->major, 0),
-						Adapter, DEV_NAME);
+						       MKDEV(Adapter->major, 0),
+						       Adapter, DEV_NAME);
 
 	if (IS_ERR(Adapter->pstCreatedClassDevice)) {
 		pr_err(DRV_NAME ": class device create failed\n");
diff --git a/drivers/staging/bcm/CmHost.c b/drivers/staging/bcm/CmHost.c
index 632f81a7c63..fb1d932c5d7 100644
--- a/drivers/staging/bcm/CmHost.c
+++ b/drivers/staging/bcm/CmHost.c
@@ -13,7 +13,19 @@ enum E_CLASSIFIER_ACTION {
 	eDeleteClassifier
 };
 
-static ULONG GetNextTargetBufferLocation(struct bcm_mini_adapter *Adapter, B_UINT16 tid);
+static ULONG GetNextTargetBufferLocation(struct bcm_mini_adapter *Adapter,
+		B_UINT16 tid);
+static void restore_endianess_of_pstClassifierEntry(
+		struct bcm_classifier_rule *pstClassifierEntry,
+		enum bcm_ipaddr_context eIpAddrContext);
+
+static void apply_phs_rule_to_all_classifiers(
+		register struct bcm_mini_adapter *Adapter,
+		register UINT uiSearchRuleIndex,
+		USHORT uVCID,
+		struct bcm_phs_rule *sPhsRule,
+		struct bcm_phs_rules *cPhsRule,
+		struct bcm_add_indication_alt *pstAddIndication);
 
 /************************************************************
  * Function - SearchSfid
@@ -67,13 +79,16 @@ static int SearchFreeSfid(struct bcm_mini_adapter *Adapter)
  *  B_UINT16  uiClassifierID - The classifier ID to be searched
  * Return: int :Classifier table index of matching entry
  */
-static int SearchClsid(struct bcm_mini_adapter *Adapter, ULONG ulSFID, B_UINT16  uiClassifierID)
+static int SearchClsid(struct bcm_mini_adapter *Adapter,
+		ULONG ulSFID,
+		B_UINT16 uiClassifierID)
 {
 	int i;
 
 	for (i = 0; i < MAX_CLASSIFIERS; i++) {
 		if ((Adapter->astClassifierTable[i].bUsed) &&
-			(Adapter->astClassifierTable[i].uiClassifierRuleIndex == uiClassifierID) &&
+			(Adapter->astClassifierTable[i].uiClassifierRuleIndex
+				== uiClassifierID) &&
 			(Adapter->astClassifierTable[i].ulSFID == ulSFID))
 			return i;
 	}
@@ -98,7 +113,8 @@ static int SearchFreeClsid(struct bcm_mini_adapter *Adapter /**Adapter Context*/
 	return MAX_CLASSIFIERS+1;
 }
 
-static VOID deleteSFBySfid(struct bcm_mini_adapter *Adapter, UINT uiSearchRuleIndex)
+static VOID deleteSFBySfid(struct bcm_mini_adapter *Adapter,
+		UINT uiSearchRuleIndex)
 {
 	/* deleting all the packet held in the SF */
 	flush_queue(Adapter, uiSearchRuleIndex);
@@ -107,7 +123,8 @@ static VOID deleteSFBySfid(struct bcm_mini_adapter *Adapter, UINT uiSearchRuleIn
 	DeleteAllClassifiersForSF(Adapter, uiSearchRuleIndex);
 
 	/* Resetting only MIBS related entries in the SF */
-	memset((PVOID)&Adapter->PackInfo[uiSearchRuleIndex], 0, sizeof(struct bcm_mibs_table));
+	memset((PVOID)&Adapter->PackInfo[uiSearchRuleIndex], 0,
+			sizeof(struct bcm_mibs_table));
 }
 
 static inline VOID
@@ -125,70 +142,109 @@ CopyIpAddrToClassifier(struct bcm_classifier_rule *pstClassifierEntry,
 		nSizeOfIPAddressInBytes = IPV6_ADDRESS_SIZEINBYTES;
 
 	/* Destination Ip Address */
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Ip Address Range Length:0x%X ", u8IpAddressLen);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Ip Address Range Length:0x%X ", u8IpAddressLen);
 	if ((bIpVersion6 ? (IPV6_ADDRESS_SIZEINBYTES * MAX_IP_RANGE_LENGTH * 2) :
 			(TOTAL_MASKED_ADDRESS_IN_BYTES)) >= u8IpAddressLen) {
+
+		union u_ip_address *st_dest_ip =
+			&pstClassifierEntry->stDestIpAddress;
+
+		union u_ip_address *st_src_ip =
+			&pstClassifierEntry->stSrcIpAddress;
+
 		/*
 		 * checking both the mask and address togethor in Classification.
 		 * So length will be : TotalLengthInBytes/nSizeOfIPAddressInBytes * 2
 		 * (nSizeOfIPAddressInBytes for address and nSizeOfIPAddressInBytes for mask)
 		 */
 		if (eIpAddrContext == eDestIpAddress) {
-			pstClassifierEntry->ucIPDestinationAddressLength = u8IpAddressLen/(nSizeOfIPAddressInBytes * 2);
+			pstClassifierEntry->ucIPDestinationAddressLength =
+				u8IpAddressLen/(nSizeOfIPAddressInBytes * 2);
 			if (bIpVersion6) {
-				ptrClassifierIpAddress = pstClassifierEntry->stDestIpAddress.ucIpv6Address;
-				ptrClassifierIpMask = pstClassifierEntry->stDestIpAddress.ucIpv6Mask;
+				ptrClassifierIpAddress =
+					st_dest_ip->ucIpv6Address;
+				ptrClassifierIpMask =
+					st_dest_ip->ucIpv6Mask;
 			} else {
-				ptrClassifierIpAddress = pstClassifierEntry->stDestIpAddress.ucIpv4Address;
-				ptrClassifierIpMask = pstClassifierEntry->stDestIpAddress.ucIpv4Mask;
+				ptrClassifierIpAddress =
+					st_dest_ip->ucIpv4Address;
+				ptrClassifierIpMask =
+					st_dest_ip->ucIpv4Mask;
 			}
 		} else if (eIpAddrContext == eSrcIpAddress) {
-			pstClassifierEntry->ucIPSourceAddressLength = u8IpAddressLen/(nSizeOfIPAddressInBytes * 2);
+			pstClassifierEntry->ucIPSourceAddressLength =
+				u8IpAddressLen/(nSizeOfIPAddressInBytes * 2);
 			if (bIpVersion6) {
-				ptrClassifierIpAddress = pstClassifierEntry->stSrcIpAddress.ucIpv6Address;
-				ptrClassifierIpMask = pstClassifierEntry->stSrcIpAddress.ucIpv6Mask;
+				ptrClassifierIpAddress =
+					st_src_ip->ucIpv6Address;
+				ptrClassifierIpMask = st_src_ip->ucIpv6Mask;
 			} else {
-				ptrClassifierIpAddress = pstClassifierEntry->stSrcIpAddress.ucIpv4Address;
-				ptrClassifierIpMask = pstClassifierEntry->stSrcIpAddress.ucIpv4Mask;
+				ptrClassifierIpAddress =
+					st_src_ip->ucIpv4Address;
+				ptrClassifierIpMask = st_src_ip->ucIpv4Mask;
 			}
 		}
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Address Length:0x%X\n", pstClassifierEntry->ucIPDestinationAddressLength);
-		while ((u8IpAddressLen >= nSizeOfIPAddressInBytes) && (i < MAX_IP_RANGE_LENGTH)) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"Address Length:0x%X\n",
+				pstClassifierEntry->ucIPDestinationAddressLength);
+		while ((u8IpAddressLen >= nSizeOfIPAddressInBytes)
+				&& (i < MAX_IP_RANGE_LENGTH)) {
 			memcpy(ptrClassifierIpAddress +
 				(i * nSizeOfIPAddressInBytes),
-				(pu8IpAddressMaskSrc+(i*nSizeOfIPAddressInBytes*2)),
+				(pu8IpAddressMaskSrc
+					+ (i * nSizeOfIPAddressInBytes * 2)),
 				nSizeOfIPAddressInBytes);
 
 			if (!bIpVersion6) {
 				if (eIpAddrContext == eSrcIpAddress) {
-					pstClassifierEntry->stSrcIpAddress.ulIpv4Addr[i] = ntohl(pstClassifierEntry->stSrcIpAddress.ulIpv4Addr[i]);
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Src Ip Address:0x%luX ",
-							pstClassifierEntry->stSrcIpAddress.ulIpv4Addr[i]);
+					st_src_ip->ulIpv4Addr[i] =
+						ntohl(st_src_ip->ulIpv4Addr[i]);
+					BCM_DEBUG_PRINT(Adapter,
+							DBG_TYPE_OTHERS,
+							CONN_MSG,
+							DBG_LVL_ALL,
+							"Src Ip Address:0x%luX ",
+							st_src_ip->ulIpv4Addr[i]);
 				} else if (eIpAddrContext == eDestIpAddress) {
-					pstClassifierEntry->stDestIpAddress.ulIpv4Addr[i] = ntohl(pstClassifierEntry->stDestIpAddress.ulIpv4Addr[i]);
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Dest Ip Address:0x%luX ",
-							pstClassifierEntry->stDestIpAddress.ulIpv4Addr[i]);
+					st_dest_ip->ulIpv4Addr[i] =
+						ntohl(st_dest_ip->ulIpv4Addr[i]);
+					BCM_DEBUG_PRINT(Adapter,
+							DBG_TYPE_OTHERS,
+							CONN_MSG,
+							DBG_LVL_ALL,
+							"Dest Ip Address:0x%luX ",
+							st_dest_ip->ulIpv4Addr[i]);
 				}
 			}
 			u8IpAddressLen -= nSizeOfIPAddressInBytes;
 			if (u8IpAddressLen >= nSizeOfIPAddressInBytes) {
 				memcpy(ptrClassifierIpMask +
 					(i * nSizeOfIPAddressInBytes),
-					(pu8IpAddressMaskSrc+nSizeOfIPAddressInBytes +
-						(i*nSizeOfIPAddressInBytes*2)),
+					(pu8IpAddressMaskSrc
+						+ nSizeOfIPAddressInBytes
+						+ (i * nSizeOfIPAddressInBytes * 2)),
 					nSizeOfIPAddressInBytes);
 
 				if (!bIpVersion6) {
 					if (eIpAddrContext == eSrcIpAddress) {
-						pstClassifierEntry->stSrcIpAddress.ulIpv4Mask[i] =
-							ntohl(pstClassifierEntry->stSrcIpAddress.ulIpv4Mask[i]);
-						BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Src Ip Mask Address:0x%luX ",
-								pstClassifierEntry->stSrcIpAddress.ulIpv4Mask[i]);
+						st_src_ip->ulIpv4Mask[i] =
+							ntohl(st_src_ip->ulIpv4Mask[i]);
+						BCM_DEBUG_PRINT(Adapter,
+								DBG_TYPE_OTHERS,
+								CONN_MSG,
+								DBG_LVL_ALL,
+								"Src Ip Mask Address:0x%luX ",
+								st_src_ip->ulIpv4Mask[i]);
 					} else if (eIpAddrContext == eDestIpAddress) {
-						pstClassifierEntry->stDestIpAddress.ulIpv4Mask[i] =
-							ntohl(pstClassifierEntry->stDestIpAddress.ulIpv4Mask[i]);
-						BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Dest Ip Mask Address:0x%luX ",
-								pstClassifierEntry->stDestIpAddress.ulIpv4Mask[i]);
+						st_dest_ip->ulIpv4Mask[i] =
+							ntohl(st_dest_ip->ulIpv4Mask[i]);
+						BCM_DEBUG_PRINT(Adapter,
+								DBG_TYPE_OTHERS,
+								CONN_MSG,
+								DBG_LVL_ALL,
+								"Dest Ip Mask Address:0x%luX ",
+								st_dest_ip->ulIpv4Mask[i]);
 					}
 				}
 				u8IpAddressLen -= nSizeOfIPAddressInBytes;
@@ -200,15 +256,10 @@ CopyIpAddrToClassifier(struct bcm_classifier_rule *pstClassifierEntry,
 		}
 		if (bIpVersion6) {
 			/* Restore EndianNess of Struct */
-			for (i = 0; i < MAX_IP_RANGE_LENGTH * 4; i++) {
-				if (eIpAddrContext == eSrcIpAddress) {
-					pstClassifierEntry->stSrcIpAddress.ulIpv6Addr[i] = ntohl(pstClassifierEntry->stSrcIpAddress.ulIpv6Addr[i]);
-					pstClassifierEntry->stSrcIpAddress.ulIpv6Mask[i] = ntohl(pstClassifierEntry->stSrcIpAddress.ulIpv6Mask[i]);
-				} else if (eIpAddrContext == eDestIpAddress) {
-					pstClassifierEntry->stDestIpAddress.ulIpv6Addr[i] = ntohl(pstClassifierEntry->stDestIpAddress.ulIpv6Addr[i]);
-					pstClassifierEntry->stDestIpAddress.ulIpv6Mask[i] = ntohl(pstClassifierEntry->stDestIpAddress.ulIpv6Mask[i]);
-				}
-			}
+			restore_endianess_of_pstClassifierEntry(
+					pstClassifierEntry,
+					eIpAddrContext
+					);
 		}
 	}
 }
@@ -216,16 +267,20 @@ CopyIpAddrToClassifier(struct bcm_classifier_rule *pstClassifierEntry,
 void ClearTargetDSXBuffer(struct bcm_mini_adapter *Adapter, B_UINT16 TID, bool bFreeAll)
 {
 	int i;
+	struct bcm_targetdsx_buffer *curr_buf;
 
 	for (i = 0; i < Adapter->ulTotalTargetBuffersAvailable; i++) {
-		if (Adapter->astTargetDsxBuffer[i].valid)
+		curr_buf = &Adapter->astTargetDsxBuffer[i];
+
+		if (curr_buf->valid)
 			continue;
 
-		if ((bFreeAll) || (Adapter->astTargetDsxBuffer[i].tid == TID)) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "ClearTargetDSXBuffer: found tid %d buffer cleared %lx\n",
-					TID, Adapter->astTargetDsxBuffer[i].ulTargetDsxBuffer);
-			Adapter->astTargetDsxBuffer[i].valid = 1;
-			Adapter->astTargetDsxBuffer[i].tid = 0;
+		if ((bFreeAll) || (curr_buf->tid == TID)) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+					"ClearTargetDSXBuffer: found tid %d buffer cleared %lx\n",
+					TID, curr_buf->ulTargetDsxBuffer);
+			curr_buf->valid = 1;
+			curr_buf->tid = 0;
 			Adapter->ulFreeTargetBufferCnt++;
 		}
 	}
@@ -235,7 +290,10 @@ void ClearTargetDSXBuffer(struct bcm_mini_adapter *Adapter, B_UINT16 TID, bool b
  * @ingroup ctrl_pkt_functions
  * copy classifier rule into the specified SF index
  */
-static inline VOID CopyClassifierRuleToSF(struct bcm_mini_adapter *Adapter, struct bcm_convergence_types *psfCSType, UINT uiSearchRuleIndex, UINT nClassifierIndex)
+static inline VOID CopyClassifierRuleToSF(struct bcm_mini_adapter *Adapter,
+		struct bcm_convergence_types *psfCSType,
+		UINT uiSearchRuleIndex,
+		UINT nClassifierIndex)
 {
 	struct bcm_classifier_rule *pstClassifierEntry = NULL;
 	/* VOID *pvPhsContext = NULL; */
@@ -243,12 +301,16 @@ static inline VOID CopyClassifierRuleToSF(struct bcm_mini_adapter *Adapter, stru
 	/* UCHAR ucProtocolLength=0; */
 	/* ULONG ulPhsStatus; */
 
+	struct bcm_packet_class_rules *pack_class_rule =
+		&psfCSType->cCPacketClassificationRule;
+
 	if (Adapter->PackInfo[uiSearchRuleIndex].usVCID_Value == 0 ||
 		nClassifierIndex > (MAX_CLASSIFIERS-1))
 		return;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Storing Classifier Rule Index : %X",
-			ntohs(psfCSType->cCPacketClassificationRule.u16PacketClassificationRuleIndex));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Storing Classifier Rule Index : %X",
+			ntohs(pack_class_rule->u16PacketClassificationRuleIndex));
 
 	if (nClassifierIndex > MAX_CLASSIFIERS-1)
 		return;
@@ -256,106 +318,152 @@ static inline VOID CopyClassifierRuleToSF(struct bcm_mini_adapter *Adapter, stru
 	pstClassifierEntry = &Adapter->astClassifierTable[nClassifierIndex];
 	if (pstClassifierEntry) {
 		/* Store if Ipv6 */
-		pstClassifierEntry->bIpv6Protocol = (Adapter->PackInfo[uiSearchRuleIndex].ucIpVersion == IPV6) ? TRUE : false;
+		pstClassifierEntry->bIpv6Protocol =
+			(Adapter->PackInfo[uiSearchRuleIndex].ucIpVersion == IPV6) ? TRUE : false;
 
 		/* Destinaiton Port */
-		pstClassifierEntry->ucDestPortRangeLength = psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRangeLength / 4;
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Destination Port Range Length:0x%X ", pstClassifierEntry->ucDestPortRangeLength);
+		pstClassifierEntry->ucDestPortRangeLength =
+			pack_class_rule->u8ProtocolDestPortRangeLength / 4;
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"Destination Port Range Length:0x%X ",
+				pstClassifierEntry->ucDestPortRangeLength);
 
-		if (psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRangeLength <= MAX_PORT_RANGE) {
+		if (pack_class_rule->u8ProtocolDestPortRangeLength <= MAX_PORT_RANGE) {
 			for (i = 0; i < (pstClassifierEntry->ucDestPortRangeLength); i++) {
-				pstClassifierEntry->usDestPortRangeLo[i] = *((PUSHORT)(psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRange+i));
+				pstClassifierEntry->usDestPortRangeLo[i] =
+					*((PUSHORT)(pack_class_rule->u8ProtocolDestPortRange+i));
 				pstClassifierEntry->usDestPortRangeHi[i] =
-					*((PUSHORT)(psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRange+2+i));
-				pstClassifierEntry->usDestPortRangeLo[i] = ntohs(pstClassifierEntry->usDestPortRangeLo[i]);
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Destination Port Range Lo:0x%X ",
+					*((PUSHORT)(pack_class_rule->u8ProtocolDestPortRange+2+i));
+				pstClassifierEntry->usDestPortRangeLo[i] =
+					ntohs(pstClassifierEntry->usDestPortRangeLo[i]);
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
+						CONN_MSG, DBG_LVL_ALL,
+						"Destination Port Range Lo:0x%X ",
 						pstClassifierEntry->usDestPortRangeLo[i]);
-				pstClassifierEntry->usDestPortRangeHi[i] = ntohs(pstClassifierEntry->usDestPortRangeHi[i]);
+				pstClassifierEntry->usDestPortRangeHi[i] =
+					ntohs(pstClassifierEntry->usDestPortRangeHi[i]);
 			}
 		} else {
 			pstClassifierEntry->ucDestPortRangeLength = 0;
 		}
 
 		/* Source Port */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Source Port Range Length:0x%X ",
-				psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRangeLength);
-		if (psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRangeLength <= MAX_PORT_RANGE) {
-			pstClassifierEntry->ucSrcPortRangeLength = psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRangeLength/4;
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"Source Port Range Length:0x%X ",
+				pack_class_rule->u8ProtocolSourcePortRangeLength);
+		if (pack_class_rule->u8ProtocolSourcePortRangeLength <= MAX_PORT_RANGE) {
+			pstClassifierEntry->ucSrcPortRangeLength =
+				pack_class_rule->u8ProtocolSourcePortRangeLength/4;
 			for (i = 0; i < (pstClassifierEntry->ucSrcPortRangeLength); i++) {
 				pstClassifierEntry->usSrcPortRangeLo[i] =
-					*((PUSHORT)(psfCSType->cCPacketClassificationRule.
+					*((PUSHORT)(pack_class_rule->
 							u8ProtocolSourcePortRange+i));
 				pstClassifierEntry->usSrcPortRangeHi[i] =
-					*((PUSHORT)(psfCSType->cCPacketClassificationRule.
+					*((PUSHORT)(pack_class_rule->
 							u8ProtocolSourcePortRange+2+i));
 				pstClassifierEntry->usSrcPortRangeLo[i] =
 					ntohs(pstClassifierEntry->usSrcPortRangeLo[i]);
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Source Port Range Lo:0x%X ",
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
+						CONN_MSG, DBG_LVL_ALL,
+						"Source Port Range Lo:0x%X ",
 						pstClassifierEntry->usSrcPortRangeLo[i]);
-				pstClassifierEntry->usSrcPortRangeHi[i] = ntohs(pstClassifierEntry->usSrcPortRangeHi[i]);
+				pstClassifierEntry->usSrcPortRangeHi[i] =
+					ntohs(pstClassifierEntry->usSrcPortRangeHi[i]);
 			}
 		}
 		/* Destination Ip Address and Mask */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Ip Destination Parameters : ");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"Ip Destination Parameters : ");
 		CopyIpAddrToClassifier(pstClassifierEntry,
-				psfCSType->cCPacketClassificationRule.u8IPDestinationAddressLength,
-				psfCSType->cCPacketClassificationRule.u8IPDestinationAddress,
+				pack_class_rule->u8IPDestinationAddressLength,
+				pack_class_rule->u8IPDestinationAddress,
 				(Adapter->PackInfo[uiSearchRuleIndex].ucIpVersion == IPV6) ?
 			TRUE : false, eDestIpAddress);
 
 		/* Source Ip Address and Mask */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Ip Source Parameters : ");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"Ip Source Parameters : ");
 
 		CopyIpAddrToClassifier(pstClassifierEntry,
-				psfCSType->cCPacketClassificationRule.u8IPMaskedSourceAddressLength,
-				psfCSType->cCPacketClassificationRule.u8IPMaskedSourceAddress,
+				pack_class_rule->u8IPMaskedSourceAddressLength,
+				pack_class_rule->u8IPMaskedSourceAddress,
 				(Adapter->PackInfo[uiSearchRuleIndex].ucIpVersion == IPV6) ? TRUE : false,
 				eSrcIpAddress);
 
 		/* TOS */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "TOS Length:0x%X ", psfCSType->cCPacketClassificationRule.u8IPTypeOfServiceLength);
-		if (psfCSType->cCPacketClassificationRule.u8IPTypeOfServiceLength == 3) {
-			pstClassifierEntry->ucIPTypeOfServiceLength = psfCSType->cCPacketClassificationRule.u8IPTypeOfServiceLength;
-			pstClassifierEntry->ucTosLow = psfCSType->cCPacketClassificationRule.u8IPTypeOfService[0];
-			pstClassifierEntry->ucTosHigh = psfCSType->cCPacketClassificationRule.u8IPTypeOfService[1];
-			pstClassifierEntry->ucTosMask = psfCSType->cCPacketClassificationRule.u8IPTypeOfService[2];
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"TOS Length:0x%X ",
+				pack_class_rule->u8IPTypeOfServiceLength);
+		if (pack_class_rule->u8IPTypeOfServiceLength == 3) {
+			pstClassifierEntry->ucIPTypeOfServiceLength =
+				pack_class_rule->u8IPTypeOfServiceLength;
+			pstClassifierEntry->ucTosLow =
+				pack_class_rule->u8IPTypeOfService[0];
+			pstClassifierEntry->ucTosHigh =
+				pack_class_rule->u8IPTypeOfService[1];
+			pstClassifierEntry->ucTosMask =
+				pack_class_rule->u8IPTypeOfService[2];
 			pstClassifierEntry->bTOSValid = TRUE;
 		}
-		if (psfCSType->cCPacketClassificationRule.u8Protocol == 0) {
+		if (pack_class_rule->u8Protocol == 0) {
 			/* we didn't get protocol field filled in by the BS */
 			pstClassifierEntry->ucProtocolLength = 0;
 		} else {
 			pstClassifierEntry->ucProtocolLength = 1; /* 1 valid protocol */
 		}
 
-		pstClassifierEntry->ucProtocol[0] = psfCSType->cCPacketClassificationRule.u8Protocol;
-		pstClassifierEntry->u8ClassifierRulePriority = psfCSType->cCPacketClassificationRule.u8ClassifierRulePriority;
+		pstClassifierEntry->ucProtocol[0] = pack_class_rule->u8Protocol;
+		pstClassifierEntry->u8ClassifierRulePriority =
+			pack_class_rule->u8ClassifierRulePriority;
 
 		/* store the classifier rule ID and set this classifier entry as valid */
-		pstClassifierEntry->ucDirection = Adapter->PackInfo[uiSearchRuleIndex].ucDirection;
-		pstClassifierEntry->uiClassifierRuleIndex = ntohs(psfCSType->cCPacketClassificationRule.u16PacketClassificationRuleIndex);
-		pstClassifierEntry->usVCID_Value = Adapter->PackInfo[uiSearchRuleIndex].usVCID_Value;
-		pstClassifierEntry->ulSFID = Adapter->PackInfo[uiSearchRuleIndex].ulSFID;
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Search Index %d Dir: %d, Index: %d, Vcid: %d\n",
-				uiSearchRuleIndex, pstClassifierEntry->ucDirection,
+		pstClassifierEntry->ucDirection =
+			Adapter->PackInfo[uiSearchRuleIndex].ucDirection;
+		pstClassifierEntry->uiClassifierRuleIndex =
+			ntohs(pack_class_rule->u16PacketClassificationRuleIndex);
+		pstClassifierEntry->usVCID_Value =
+			Adapter->PackInfo[uiSearchRuleIndex].usVCID_Value;
+		pstClassifierEntry->ulSFID =
+			Adapter->PackInfo[uiSearchRuleIndex].ulSFID;
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"Search Index %d Dir: %d, Index: %d, Vcid: %d\n",
+				uiSearchRuleIndex,
+				pstClassifierEntry->ucDirection,
 				pstClassifierEntry->uiClassifierRuleIndex,
 				pstClassifierEntry->usVCID_Value);
 
-		if (psfCSType->cCPacketClassificationRule.u8AssociatedPHSI)
-			pstClassifierEntry->u8AssociatedPHSI = psfCSType->cCPacketClassificationRule.u8AssociatedPHSI;
+		if (pack_class_rule->u8AssociatedPHSI)
+			pstClassifierEntry->u8AssociatedPHSI =
+				pack_class_rule->u8AssociatedPHSI;
 
 		/* Copy ETH CS Parameters */
-		pstClassifierEntry->ucEthCSSrcMACLen = (psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddressLength);
-		memcpy(pstClassifierEntry->au8EThCSSrcMAC, psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress, MAC_ADDRESS_SIZE);
-		memcpy(pstClassifierEntry->au8EThCSSrcMACMask, psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress + MAC_ADDRESS_SIZE, MAC_ADDRESS_SIZE);
-		pstClassifierEntry->ucEthCSDestMACLen = (psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddressLength);
-		memcpy(pstClassifierEntry->au8EThCSDestMAC, psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress, MAC_ADDRESS_SIZE);
-		memcpy(pstClassifierEntry->au8EThCSDestMACMask, psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress + MAC_ADDRESS_SIZE, MAC_ADDRESS_SIZE);
-		pstClassifierEntry->ucEtherTypeLen = (psfCSType->cCPacketClassificationRule.u8EthertypeLength);
-		memcpy(pstClassifierEntry->au8EthCSEtherType, psfCSType->cCPacketClassificationRule.u8Ethertype, NUM_ETHERTYPE_BYTES);
-		memcpy(pstClassifierEntry->usUserPriority, &psfCSType->cCPacketClassificationRule.u16UserPriority, 2);
-		pstClassifierEntry->usVLANID = ntohs(psfCSType->cCPacketClassificationRule.u16VLANID);
-		pstClassifierEntry->usValidityBitMap = ntohs(psfCSType->cCPacketClassificationRule.u16ValidityBitMap);
+		pstClassifierEntry->ucEthCSSrcMACLen =
+			(pack_class_rule->u8EthernetSourceMACAddressLength);
+		memcpy(pstClassifierEntry->au8EThCSSrcMAC,
+				pack_class_rule->u8EthernetSourceMACAddress,
+				MAC_ADDRESS_SIZE);
+		memcpy(pstClassifierEntry->au8EThCSSrcMACMask,
+				pack_class_rule->u8EthernetSourceMACAddress
+				+ MAC_ADDRESS_SIZE, MAC_ADDRESS_SIZE);
+		pstClassifierEntry->ucEthCSDestMACLen =
+			(pack_class_rule->u8EthernetDestMacAddressLength);
+		memcpy(pstClassifierEntry->au8EThCSDestMAC,
+				pack_class_rule->u8EthernetDestMacAddress,
+				MAC_ADDRESS_SIZE);
+		memcpy(pstClassifierEntry->au8EThCSDestMACMask,
+				pack_class_rule->u8EthernetDestMacAddress
+				+ MAC_ADDRESS_SIZE, MAC_ADDRESS_SIZE);
+		pstClassifierEntry->ucEtherTypeLen =
+			(pack_class_rule->u8EthertypeLength);
+		memcpy(pstClassifierEntry->au8EthCSEtherType,
+				pack_class_rule->u8Ethertype,
+				NUM_ETHERTYPE_BYTES);
+		memcpy(pstClassifierEntry->usUserPriority,
+				&pack_class_rule->u16UserPriority, 2);
+		pstClassifierEntry->usVLANID =
+			ntohs(pack_class_rule->u16VLANID);
+		pstClassifierEntry->usValidityBitMap =
+			ntohs(pack_class_rule->u16ValidityBitMap);
 
 		pstClassifierEntry->bUsed = TRUE;
 	}
@@ -364,7 +472,8 @@ static inline VOID CopyClassifierRuleToSF(struct bcm_mini_adapter *Adapter, stru
 /*
  * @ingroup ctrl_pkt_functions
  */
-static inline VOID DeleteClassifierRuleFromSF(struct bcm_mini_adapter *Adapter, UINT uiSearchRuleIndex, UINT nClassifierIndex)
+static inline VOID DeleteClassifierRuleFromSF(struct bcm_mini_adapter *Adapter,
+		UINT uiSearchRuleIndex, UINT nClassifierIndex)
 {
 	struct bcm_classifier_rule *pstClassifierEntry = NULL;
 	B_UINT16 u16PacketClassificationRuleIndex;
@@ -380,22 +489,26 @@ static inline VOID DeleteClassifierRuleFromSF(struct bcm_mini_adapter *Adapter,
 	if (usVCID == 0)
 		return;
 
-	u16PacketClassificationRuleIndex = Adapter->astClassifierTable[nClassifierIndex].uiClassifierRuleIndex;
+	u16PacketClassificationRuleIndex =
+		Adapter->astClassifierTable[nClassifierIndex].uiClassifierRuleIndex;
 	pstClassifierEntry = &Adapter->astClassifierTable[nClassifierIndex];
 	if (pstClassifierEntry) {
 		pstClassifierEntry->bUsed = false;
 		pstClassifierEntry->uiClassifierRuleIndex = 0;
-		memset(pstClassifierEntry, 0, sizeof(struct bcm_classifier_rule));
+		memset(pstClassifierEntry, 0,
+				sizeof(struct bcm_classifier_rule));
 
 		/* Delete the PHS Rule for this classifier */
-		PhsDeleteClassifierRule(&Adapter->stBCMPhsContext, usVCID, u16PacketClassificationRuleIndex);
+		PhsDeleteClassifierRule(&Adapter->stBCMPhsContext, usVCID,
+				u16PacketClassificationRuleIndex);
 	}
 }
 
 /*
  * @ingroup ctrl_pkt_functions
  */
-VOID DeleteAllClassifiersForSF(struct bcm_mini_adapter *Adapter, UINT uiSearchRuleIndex)
+VOID DeleteAllClassifiersForSF(struct bcm_mini_adapter *Adapter,
+		UINT uiSearchRuleIndex)
 {
 	struct bcm_classifier_rule *pstClassifierEntry = NULL;
 	int i;
@@ -414,7 +527,8 @@ VOID DeleteAllClassifiersForSF(struct bcm_mini_adapter *Adapter, UINT uiSearchRu
 			pstClassifierEntry = &Adapter->astClassifierTable[i];
 
 			if (pstClassifierEntry->bUsed)
-				DeleteClassifierRuleFromSF(Adapter, uiSearchRuleIndex, i);
+				DeleteClassifierRuleFromSF(Adapter,
+						uiSearchRuleIndex, i);
 		}
 	}
 
@@ -441,324 +555,262 @@ static VOID CopyToAdapter(register struct bcm_mini_adapter *Adapter, /* <Pointer
 	int i;
 	struct bcm_convergence_types *psfCSType = NULL;
 	struct bcm_phs_rule sPhsRule;
-	USHORT uVCID = Adapter->PackInfo[uiSearchRuleIndex].usVCID_Value;
+	struct bcm_packet_info *curr_packinfo =
+		&Adapter->PackInfo[uiSearchRuleIndex];
+	USHORT uVCID = curr_packinfo->usVCID_Value;
 	UINT UGIValue = 0;
 
-	Adapter->PackInfo[uiSearchRuleIndex].bValid = TRUE;
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Search Rule Index = %d\n", uiSearchRuleIndex);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "%s: SFID= %x ", __func__, ntohl(psfLocalSet->u32SFID));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Updating Queue %d", uiSearchRuleIndex);
+	curr_packinfo->bValid = TRUE;
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Search Rule Index = %d\n", uiSearchRuleIndex);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"%s: SFID= %x ", __func__, ntohl(psfLocalSet->u32SFID));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Updating Queue %d", uiSearchRuleIndex);
 
 	ulSFID = ntohl(psfLocalSet->u32SFID);
 	/* Store IP Version used */
 	/* Get The Version Of IP used (IPv6 or IPv4) from CSSpecification field of SF */
 
-	Adapter->PackInfo[uiSearchRuleIndex].bIPCSSupport = 0;
-	Adapter->PackInfo[uiSearchRuleIndex].bEthCSSupport = 0;
+	curr_packinfo->bIPCSSupport = 0;
+	curr_packinfo->bEthCSSupport = 0;
 
 	/* Enable IP/ETh CS Support As Required */
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "CopyToAdapter : u8CSSpecification : %X\n", psfLocalSet->u8CSSpecification);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"CopyToAdapter : u8CSSpecification : %X\n",
+			psfLocalSet->u8CSSpecification);
 	switch (psfLocalSet->u8CSSpecification) {
 	case eCSPacketIPV4:
-	{
-		Adapter->PackInfo[uiSearchRuleIndex].bIPCSSupport = IPV4_CS;
+		curr_packinfo->bIPCSSupport = IPV4_CS;
 		break;
-	}
 	case eCSPacketIPV6:
-	{
-		Adapter->PackInfo[uiSearchRuleIndex].bIPCSSupport = IPV6_CS;
+		curr_packinfo->bIPCSSupport = IPV6_CS;
 		break;
-	}
 	case eCS802_3PacketEthernet:
 	case eCS802_1QPacketVLAN:
-	{
-		Adapter->PackInfo[uiSearchRuleIndex].bEthCSSupport = ETH_CS_802_3;
+		curr_packinfo->bEthCSSupport = ETH_CS_802_3;
 		break;
-	}
 	case eCSPacketIPV4Over802_1QVLAN:
 	case eCSPacketIPV4Over802_3Ethernet:
-	{
-		Adapter->PackInfo[uiSearchRuleIndex].bIPCSSupport = IPV4_CS;
-		Adapter->PackInfo[uiSearchRuleIndex].bEthCSSupport = ETH_CS_802_3;
+		curr_packinfo->bIPCSSupport = IPV4_CS;
+		curr_packinfo->bEthCSSupport = ETH_CS_802_3;
 		break;
-	}
 	case eCSPacketIPV6Over802_1QVLAN:
 	case eCSPacketIPV6Over802_3Ethernet:
-	{
-		Adapter->PackInfo[uiSearchRuleIndex].bIPCSSupport = IPV6_CS;
-		Adapter->PackInfo[uiSearchRuleIndex].bEthCSSupport = ETH_CS_802_3;
+		curr_packinfo->bIPCSSupport = IPV6_CS;
+		curr_packinfo->bEthCSSupport = ETH_CS_802_3;
 		break;
-	}
 	default:
-	{
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Error in value of CS Classification.. setting default to IP CS\n");
-		Adapter->PackInfo[uiSearchRuleIndex].bIPCSSupport = IPV4_CS;
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"Error in value of CS Classification.. setting default to IP CS\n");
+		curr_packinfo->bIPCSSupport = IPV4_CS;
 		break;
 	}
-	}
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "CopyToAdapter : Queue No : %X ETH CS Support :  %X  , IP CS Support : %X\n",
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"CopyToAdapter : Queue No : %X ETH CS Support :  %X  , IP CS Support : %X\n",
 			uiSearchRuleIndex,
-			Adapter->PackInfo[uiSearchRuleIndex].bEthCSSupport,
-			Adapter->PackInfo[uiSearchRuleIndex].bIPCSSupport);
+			curr_packinfo->bEthCSSupport,
+			curr_packinfo->bIPCSSupport);
 
 	/* Store IP Version used */
 	/* Get The Version Of IP used (IPv6 or IPv4) from CSSpecification field of SF */
-	if (Adapter->PackInfo[uiSearchRuleIndex].bIPCSSupport == IPV6_CS)
-		Adapter->PackInfo[uiSearchRuleIndex].ucIpVersion = IPV6;
+	if (curr_packinfo->bIPCSSupport == IPV6_CS)
+		curr_packinfo->ucIpVersion = IPV6;
 	else
-		Adapter->PackInfo[uiSearchRuleIndex].ucIpVersion = IPV4;
+		curr_packinfo->ucIpVersion = IPV4;
 
 	/* To ensure that the ETH CS code doesn't gets executed if the BS doesn't supports ETH CS */
 	if (!Adapter->bETHCSEnabled)
-		Adapter->PackInfo[uiSearchRuleIndex].bEthCSSupport = 0;
+		curr_packinfo->bEthCSSupport = 0;
 
 	if (psfLocalSet->u8ServiceClassNameLength > 0 && psfLocalSet->u8ServiceClassNameLength < 32)
-		memcpy(Adapter->PackInfo[uiSearchRuleIndex].ucServiceClassName,	psfLocalSet->u8ServiceClassName, psfLocalSet->u8ServiceClassNameLength);
+		memcpy(curr_packinfo->ucServiceClassName,
+				psfLocalSet->u8ServiceClassName,
+				psfLocalSet->u8ServiceClassNameLength);
 
-	Adapter->PackInfo[uiSearchRuleIndex].u8QueueType = psfLocalSet->u8ServiceFlowSchedulingType;
+	curr_packinfo->u8QueueType = psfLocalSet->u8ServiceFlowSchedulingType;
 
-	if (Adapter->PackInfo[uiSearchRuleIndex].u8QueueType == BE && Adapter->PackInfo[uiSearchRuleIndex].ucDirection)
+	if (curr_packinfo->u8QueueType == BE && curr_packinfo->ucDirection)
 		Adapter->usBestEffortQueueIndex = uiSearchRuleIndex;
 
-	Adapter->PackInfo[uiSearchRuleIndex].ulSFID = ntohl(psfLocalSet->u32SFID);
+	curr_packinfo->ulSFID = ntohl(psfLocalSet->u32SFID);
 
-	Adapter->PackInfo[uiSearchRuleIndex].u8TrafficPriority = psfLocalSet->u8TrafficPriority;
+	curr_packinfo->u8TrafficPriority = psfLocalSet->u8TrafficPriority;
 
 	/* copy all the classifier in the Service Flow param  structure */
 	for (i = 0; i < psfLocalSet->u8TotalClassifiers; i++) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Classifier index =%d", i);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"Classifier index =%d", i);
 		psfCSType = &psfLocalSet->cConvergenceSLTypes[i];
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Classifier index =%d", i);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"Classifier index =%d", i);
 
 		if (psfCSType->cCPacketClassificationRule.u8ClassifierRulePriority)
-			Adapter->PackInfo[uiSearchRuleIndex].bClassifierPriority = TRUE;
+			curr_packinfo->bClassifierPriority = TRUE;
 
 		if (psfCSType->cCPacketClassificationRule.u8ClassifierRulePriority)
-			Adapter->PackInfo[uiSearchRuleIndex].bClassifierPriority = TRUE;
+			curr_packinfo->bClassifierPriority = TRUE;
 
 		if (ucDsxType == DSA_ACK) {
 			eClassifierAction = eAddClassifier;
 		} else if (ucDsxType == DSC_ACK) {
 			switch (psfCSType->u8ClassfierDSCAction) {
 			case 0: /* DSC Add Classifier */
-			{
 				eClassifierAction = eAddClassifier;
-			}
-			break;
+				break;
 			case 1: /* DSC Replace Classifier */
-			{
 				eClassifierAction = eReplaceClassifier;
-			}
-			break;
+				break;
 			case 2: /* DSC Delete Classifier */
-			{
 				eClassifierAction = eDeleteClassifier;
-			}
-			break;
+				break;
 			default:
-			{
 				eClassifierAction = eInvalidClassifierAction;
 			}
-			}
 		}
 
 		u16PacketClassificationRuleIndex = ntohs(psfCSType->cCPacketClassificationRule.u16PacketClassificationRuleIndex);
 
 		switch (eClassifierAction) {
 		case eAddClassifier:
-		{
 			/* Get a Free Classifier Index From Classifier table for this SF to add the Classifier */
 			/* Contained in this message */
-			nClassifierIndex = SearchClsid(Adapter, ulSFID, u16PacketClassificationRuleIndex);
+			nClassifierIndex = SearchClsid(Adapter,
+					ulSFID,
+					u16PacketClassificationRuleIndex);
 
 			if (nClassifierIndex > MAX_CLASSIFIERS) {
 				nClassifierIndex = SearchFreeClsid(Adapter);
 				if (nClassifierIndex > MAX_CLASSIFIERS) {
 					/* Failed To get a free Entry */
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Error Failed To get a free Classifier Entry");
+					BCM_DEBUG_PRINT(Adapter,
+							DBG_TYPE_OTHERS,
+							CONN_MSG,
+							DBG_LVL_ALL,
+							"Error Failed To get a free Classifier Entry");
 					break;
 				}
 				/* Copy the Classifier Rule for this service flow into our Classifier table maintained per SF. */
-				CopyClassifierRuleToSF(Adapter, psfCSType, uiSearchRuleIndex, nClassifierIndex);
+				CopyClassifierRuleToSF(Adapter, psfCSType,
+						uiSearchRuleIndex,
+						nClassifierIndex);
 			} else {
 				/* This Classifier Already Exists and it is invalid to Add Classifier with existing PCRI */
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
+						CONN_MSG,
+						DBG_LVL_ALL,
 						"CopyToAdapter: Error The Specified Classifier Already Exists and attempted To Add Classifier with Same PCRI : 0x%x\n",
 						u16PacketClassificationRuleIndex);
 			}
-		}
-		break;
+			break;
 		case eReplaceClassifier:
-		{
 			/* Get the Classifier Index From Classifier table for this SF and replace existing  Classifier */
 			/* with the new classifier Contained in this message */
-			nClassifierIndex = SearchClsid(Adapter, ulSFID, u16PacketClassificationRuleIndex);
+			nClassifierIndex = SearchClsid(Adapter, ulSFID,
+					u16PacketClassificationRuleIndex);
 			if (nClassifierIndex > MAX_CLASSIFIERS) {
 				/* Failed To search the classifier */
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Error Search for Classifier To be replaced failed");
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
+						CONN_MSG, DBG_LVL_ALL,
+						"Error Search for Classifier To be replaced failed");
 				break;
 			}
 			/* Copy the Classifier Rule for this service flow into our Classifier table maintained per SF. */
-			CopyClassifierRuleToSF(Adapter, psfCSType, uiSearchRuleIndex, nClassifierIndex);
-		}
-		break;
+			CopyClassifierRuleToSF(Adapter, psfCSType,
+					uiSearchRuleIndex, nClassifierIndex);
+			break;
 		case eDeleteClassifier:
-		{
 			/* Get the Classifier Index From Classifier table for this SF and replace existing  Classifier */
 			/* with the new classifier Contained in this message */
-			nClassifierIndex = SearchClsid(Adapter, ulSFID, u16PacketClassificationRuleIndex);
+			nClassifierIndex = SearchClsid(Adapter, ulSFID,
+					u16PacketClassificationRuleIndex);
 			if (nClassifierIndex > MAX_CLASSIFIERS)	{
 				/* Failed To search the classifier */
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Error Search for Classifier To be deleted failed");
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
+						CONN_MSG, DBG_LVL_ALL,
+						"Error Search for Classifier To be deleted failed");
 				break;
 			}
 
 			/* Delete This classifier */
-			DeleteClassifierRuleFromSF(Adapter, uiSearchRuleIndex, nClassifierIndex);
-		}
-		break;
+			DeleteClassifierRuleFromSF(Adapter, uiSearchRuleIndex,
+					nClassifierIndex);
+			break;
 		default:
-		{
 			/* Invalid Action for classifier */
 			break;
 		}
-		}
 	}
 
 	/* Repeat parsing Classification Entries to process PHS Rules */
 	for (i = 0; i < psfLocalSet->u8TotalClassifiers; i++) {
 		psfCSType = &psfLocalSet->cConvergenceSLTypes[i];
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "psfCSType->u8PhsDSCAction : 0x%x\n", psfCSType->u8PhsDSCAction);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"psfCSType->u8PhsDSCAction : 0x%x\n",
+				psfCSType->u8PhsDSCAction);
 
 		switch (psfCSType->u8PhsDSCAction) {
 		case eDeleteAllPHSRules:
-		{
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Deleting All PHS Rules For VCID: 0x%X\n", uVCID);
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG,
+					DBG_LVL_ALL,
+					"Deleting All PHS Rules For VCID: 0x%X\n",
+					uVCID);
 
 			/* Delete All the PHS rules for this Service flow */
 			PhsDeleteSFRules(&Adapter->stBCMPhsContext, uVCID);
 			break;
-		}
 		case eDeletePHSRule:
-		{
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "PHS DSC Action = Delete PHS Rule\n");
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG,
+					DBG_LVL_ALL,
+					"PHS DSC Action = Delete PHS Rule\n");
 
 			if (psfCSType->cPhsRule.u8PHSI)
-				PhsDeletePHSRule(&Adapter->stBCMPhsContext, uVCID, psfCSType->cCPacketClassificationRule.u8AssociatedPHSI);
+				PhsDeletePHSRule(&Adapter->stBCMPhsContext,
+						uVCID,
+						psfCSType->cCPacketClassificationRule.u8AssociatedPHSI);
 
 			break;
-		}
 		default:
-		{
 			if (ucDsxType == DSC_ACK) {
 				/* BCM_DEBUG_PRINT(CONN_MSG,("Invalid PHS DSC Action For DSC\n",psfCSType->cPhsRule.u8PHSI)); */
 				break; /* FOr DSC ACK Case PHS DSC Action must be in valid set */
 			}
-		}
 		/* Proceed To Add PHS rule for DSA_ACK case even if PHS DSC action is unspecified */
 		/* No Break Here . Intentionally! */
 
 		case eAddPHSRule:
 		case eSetPHSRule:
-		{
 			if (psfCSType->cPhsRule.u8PHSI)	{
 				/* Apply This PHS Rule to all classifiers whose Associated PHSI Match */
-				unsigned int uiClassifierIndex = 0;
-				if (pstAddIndication->u8Direction == UPLINK_DIR) {
-					for (uiClassifierIndex = 0; uiClassifierIndex < MAX_CLASSIFIERS; uiClassifierIndex++) {
-						if ((Adapter->astClassifierTable[uiClassifierIndex].bUsed) &&
-							(Adapter->astClassifierTable[uiClassifierIndex].ulSFID == Adapter->PackInfo[uiSearchRuleIndex].ulSFID) &&
-							(Adapter->astClassifierTable[uiClassifierIndex].u8AssociatedPHSI == psfCSType->cPhsRule.u8PHSI)) {
-							BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-									"Adding PHS Rule For Classifier: 0x%x cPhsRule.u8PHSI: 0x%x\n",
-									Adapter->astClassifierTable[uiClassifierIndex].uiClassifierRuleIndex,
-									psfCSType->cPhsRule.u8PHSI);
-							/* Update The PHS Rule for this classifier as Associated PHSI id defined */
-
-							/* Copy the PHS Rule */
-							sPhsRule.u8PHSI = psfCSType->cPhsRule.u8PHSI;
-							sPhsRule.u8PHSFLength = psfCSType->cPhsRule.u8PHSFLength;
-							sPhsRule.u8PHSMLength = psfCSType->cPhsRule.u8PHSMLength;
-							sPhsRule.u8PHSS = psfCSType->cPhsRule.u8PHSS;
-							sPhsRule.u8PHSV = psfCSType->cPhsRule.u8PHSV;
-							memcpy(sPhsRule.u8PHSF, psfCSType->cPhsRule.u8PHSF, MAX_PHS_LENGTHS);
-							memcpy(sPhsRule.u8PHSM, psfCSType->cPhsRule.u8PHSM, MAX_PHS_LENGTHS);
-							sPhsRule.u8RefCnt = 0;
-							sPhsRule.bUnclassifiedPHSRule = false;
-							sPhsRule.PHSModifiedBytes = 0;
-							sPhsRule.PHSModifiedNumPackets = 0;
-							sPhsRule.PHSErrorNumPackets = 0;
-
-							/* bPHSRuleAssociated = TRUE; */
-							/* Store The PHS Rule for this classifier */
-
-							PhsUpdateClassifierRule(
-								&Adapter->stBCMPhsContext,
-								uVCID,
-								Adapter->astClassifierTable[uiClassifierIndex].uiClassifierRuleIndex,
-								&sPhsRule,
-								Adapter->astClassifierTable[uiClassifierIndex].u8AssociatedPHSI);
-
-							/* Update PHS Rule For the Classifier */
-							if (sPhsRule.u8PHSI) {
-								Adapter->astClassifierTable[uiClassifierIndex].u32PHSRuleID = sPhsRule.u8PHSI;
-								memcpy(&Adapter->astClassifierTable[uiClassifierIndex].sPhsRule, &sPhsRule, sizeof(struct bcm_phs_rule));
-							}
-						}
-					}
-				} else {
-					/* Error PHS Rule specified in signaling could not be applied to any classifier */
-
-					/* Copy the PHS Rule */
-					sPhsRule.u8PHSI = psfCSType->cPhsRule.u8PHSI;
-					sPhsRule.u8PHSFLength = psfCSType->cPhsRule.u8PHSFLength;
-					sPhsRule.u8PHSMLength = psfCSType->cPhsRule.u8PHSMLength;
-					sPhsRule.u8PHSS = psfCSType->cPhsRule.u8PHSS;
-					sPhsRule.u8PHSV = psfCSType->cPhsRule.u8PHSV;
-					memcpy(sPhsRule.u8PHSF, psfCSType->cPhsRule.u8PHSF, MAX_PHS_LENGTHS);
-					memcpy(sPhsRule.u8PHSM, psfCSType->cPhsRule.u8PHSM, MAX_PHS_LENGTHS);
-					sPhsRule.u8RefCnt = 0;
-					sPhsRule.bUnclassifiedPHSRule = TRUE;
-					sPhsRule.PHSModifiedBytes = 0;
-					sPhsRule.PHSModifiedNumPackets = 0;
-					sPhsRule.PHSErrorNumPackets = 0;
-					/* Store The PHS Rule for this classifier */
-
-					/*
-					 * Passing the argument u8PHSI instead of clsid. Because for DL with no classifier rule,
-					 * clsid will be zero hence we can't have multiple PHS rules for the same SF.
-					 * To support multiple PHS rule, passing u8PHSI.
-					 */
-					PhsUpdateClassifierRule(
-						&Adapter->stBCMPhsContext,
+				apply_phs_rule_to_all_classifiers(Adapter,
+						uiSearchRuleIndex,
 						uVCID,
-						sPhsRule.u8PHSI,
 						&sPhsRule,
-						sPhsRule.u8PHSI);
-				}
+						&psfCSType->cPhsRule,
+						pstAddIndication);
 			}
-		}
-		break;
+			break;
 		}
 	}
 
 	if (psfLocalSet->u32MaxSustainedTrafficRate == 0) {
 		/* No Rate Limit . Set Max Sustained Traffic Rate to Maximum */
-		Adapter->PackInfo[uiSearchRuleIndex].uiMaxAllowedRate = WIMAX_MAX_ALLOWED_RATE;
+		curr_packinfo->uiMaxAllowedRate = WIMAX_MAX_ALLOWED_RATE;
 	} else if (ntohl(psfLocalSet->u32MaxSustainedTrafficRate) > WIMAX_MAX_ALLOWED_RATE) {
 		/* Too large Allowed Rate specified. Limiting to Wi Max  Allowed rate */
-		Adapter->PackInfo[uiSearchRuleIndex].uiMaxAllowedRate = WIMAX_MAX_ALLOWED_RATE;
+		curr_packinfo->uiMaxAllowedRate = WIMAX_MAX_ALLOWED_RATE;
 	} else {
-		Adapter->PackInfo[uiSearchRuleIndex].uiMaxAllowedRate =  ntohl(psfLocalSet->u32MaxSustainedTrafficRate);
+		curr_packinfo->uiMaxAllowedRate =
+			ntohl(psfLocalSet->u32MaxSustainedTrafficRate);
 	}
 
-	Adapter->PackInfo[uiSearchRuleIndex].uiMaxLatency = ntohl(psfLocalSet->u32MaximumLatency);
-	if (Adapter->PackInfo[uiSearchRuleIndex].uiMaxLatency == 0) /* 0 should be treated as infinite */
-		Adapter->PackInfo[uiSearchRuleIndex].uiMaxLatency = MAX_LATENCY_ALLOWED;
+	curr_packinfo->uiMaxLatency = ntohl(psfLocalSet->u32MaximumLatency);
+	if (curr_packinfo->uiMaxLatency == 0) /* 0 should be treated as infinite */
+		curr_packinfo->uiMaxLatency = MAX_LATENCY_ALLOWED;
 
-	if ((Adapter->PackInfo[uiSearchRuleIndex].u8QueueType == ERTPS ||
-			Adapter->PackInfo[uiSearchRuleIndex].u8QueueType == UGS))
+	if ((curr_packinfo->u8QueueType == ERTPS ||
+			curr_packinfo->u8QueueType == UGS))
 		UGIValue = ntohs(psfLocalSet->u16UnsolicitedGrantInterval);
 
 	if (UGIValue == 0)
@@ -770,42 +822,45 @@ static VOID CopyToAdapter(register struct bcm_mini_adapter *Adapter, /* <Pointer
 	 * The extra amount of token is to ensure that a large amount of jitter won't have loss in throughput...
 	 * In case of non-UGI based connection, 200 frames worth of data is the max token count at host...
 	 */
-	Adapter->PackInfo[uiSearchRuleIndex].uiMaxBucketSize =
-		(DEFAULT_UGI_FACTOR*Adapter->PackInfo[uiSearchRuleIndex].uiMaxAllowedRate*UGIValue)/1000;
+	curr_packinfo->uiMaxBucketSize =
+		(DEFAULT_UGI_FACTOR*curr_packinfo->uiMaxAllowedRate*UGIValue)/1000;
 
-	if (Adapter->PackInfo[uiSearchRuleIndex].uiMaxBucketSize < WIMAX_MAX_MTU*8) {
+	if (curr_packinfo->uiMaxBucketSize < WIMAX_MAX_MTU*8) {
 		UINT UGIFactor = 0;
 		/* Special Handling to ensure the biggest size of packet can go out from host to FW as follows:
 		 * 1. Any packet from Host to FW can go out in different packet size.
 		 * 2. So in case the Bucket count is smaller than MTU, the packets of size (Size > TokenCount), will get dropped.
 		 * 3. We can allow packets of MaxSize from Host->FW that can go out from FW in multiple SDUs by fragmentation at Wimax Layer
 		 */
-		UGIFactor = (Adapter->PackInfo[uiSearchRuleIndex].uiMaxLatency/UGIValue + 1);
+		UGIFactor = (curr_packinfo->uiMaxLatency/UGIValue + 1);
 
 		if (UGIFactor > DEFAULT_UGI_FACTOR)
-			Adapter->PackInfo[uiSearchRuleIndex].uiMaxBucketSize =
-				(UGIFactor*Adapter->PackInfo[uiSearchRuleIndex].uiMaxAllowedRate*UGIValue)/1000;
+			curr_packinfo->uiMaxBucketSize =
+				(UGIFactor*curr_packinfo->uiMaxAllowedRate*UGIValue)/1000;
 
-		if (Adapter->PackInfo[uiSearchRuleIndex].uiMaxBucketSize > WIMAX_MAX_MTU*8)
-			Adapter->PackInfo[uiSearchRuleIndex].uiMaxBucketSize = WIMAX_MAX_MTU*8;
+		if (curr_packinfo->uiMaxBucketSize > WIMAX_MAX_MTU*8)
+			curr_packinfo->uiMaxBucketSize = WIMAX_MAX_MTU*8;
 	}
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "LAT: %d, UGI: %d\n", Adapter->PackInfo[uiSearchRuleIndex].uiMaxLatency, UGIValue);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "uiMaxAllowedRate: 0x%x, u32MaxSustainedTrafficRate: 0x%x ,uiMaxBucketSize: 0x%x",
-			Adapter->PackInfo[uiSearchRuleIndex].uiMaxAllowedRate,
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"LAT: %d, UGI: %d\n", curr_packinfo->uiMaxLatency,
+			UGIValue);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"uiMaxAllowedRate: 0x%x, u32MaxSustainedTrafficRate: 0x%x ,uiMaxBucketSize: 0x%x",
+			curr_packinfo->uiMaxAllowedRate,
 			ntohl(psfLocalSet->u32MaxSustainedTrafficRate),
-			Adapter->PackInfo[uiSearchRuleIndex].uiMaxBucketSize);
+			curr_packinfo->uiMaxBucketSize);
 
 	/* copy the extended SF Parameters to Support MIBS */
 	CopyMIBSExtendedSFParameters(Adapter, psfLocalSet, uiSearchRuleIndex);
 
 	/* store header suppression enabled flag per SF */
-	Adapter->PackInfo[uiSearchRuleIndex].bHeaderSuppressionEnabled =
+	curr_packinfo->bHeaderSuppressionEnabled =
 		!(psfLocalSet->u8RequesttransmissionPolicy &
 			MASK_DISABLE_HEADER_SUPPRESSION);
 
-	kfree(Adapter->PackInfo[uiSearchRuleIndex].pstSFIndication);
-	Adapter->PackInfo[uiSearchRuleIndex].pstSFIndication = pstAddIndication;
+	kfree(curr_packinfo->pstSFIndication);
+	curr_packinfo->pstSFIndication = pstAddIndication;
 
 	/* Re Sort the SF list in PackInfo according to Traffic Priority */
 	SortPackInfo(Adapter);
@@ -815,7 +870,8 @@ static VOID CopyToAdapter(register struct bcm_mini_adapter *Adapter, /* <Pointer
 	 */
 	SortClassifiers(Adapter);
 	DumpPhsRules(&Adapter->stBCMPhsContext);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "%s <=====", __func__);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"%s <=====", __func__);
 }
 
 /***********************************************************************
@@ -1197,123 +1253,179 @@ static VOID DumpCmControlPacket(PVOID pvBuffer)
 
 	for (nIndex = 0; nIndex < nCurClassifierCnt; nIndex++)	{
 		struct bcm_convergence_types *psfCSType = NULL;
+		struct bcm_packet_class_rules *clsRule = NULL;
 
-		psfCSType =  &pstAddIndication->sfActiveSet.cConvergenceSLTypes[nIndex];
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " CCPacketClassificationRuleSI====>");
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8ClassifierRulePriority: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8ClassifierRulePriority);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8IPTypeOfServiceLength: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8IPTypeOfServiceLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8IPTypeOfService[3]: 0x%X ,0x%X ,0x%X ",
-				psfCSType->cCPacketClassificationRule.u8IPTypeOfService[0],
-				psfCSType->cCPacketClassificationRule.u8IPTypeOfService[1],
-				psfCSType->cCPacketClassificationRule.u8IPTypeOfService[2]);
+		psfCSType = &pstAddIndication->sfActiveSet.cConvergenceSLTypes[nIndex];
+		clsRule	= &psfCSType->cCPacketClassificationRule;
+
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, " CCPacketClassificationRuleSI====>");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, " u8ClassifierRulePriority: 0x%X ",
+				clsRule->u8ClassifierRulePriority);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, " u8IPTypeOfServiceLength: 0x%X ",
+				clsRule->u8IPTypeOfServiceLength);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8IPTypeOfService[3]: 0x%X ,0x%X ,0x%X ",
+				clsRule->u8IPTypeOfService[0],
+				clsRule->u8IPTypeOfService[1],
+				clsRule->u8IPTypeOfService[2]);
 
 		for (uiLoopIndex = 0; uiLoopIndex < 1; uiLoopIndex++)
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8Protocol: 0x%X ", psfCSType->cCPacketClassificationRule.u8Protocol);
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+					DBG_LVL_ALL,
+					" u8Protocol: 0x%X ",
+					clsRule->u8Protocol);
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPMaskedSourceAddressLength: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8IPMaskedSourceAddressLength);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				"u8IPMaskedSourceAddressLength: 0x%X ",
+				clsRule->u8IPMaskedSourceAddressLength);
 
 		for (uiLoopIndex = 0; uiLoopIndex < 32; uiLoopIndex++)
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPMaskedSourceAddress[32]: 0x%X ",
-					psfCSType->cCPacketClassificationRule.u8IPMaskedSourceAddress[uiLoopIndex]);
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+					DBG_LVL_ALL,
+					"u8IPMaskedSourceAddress[32]: 0x%X ",
+					clsRule->u8IPMaskedSourceAddress[uiLoopIndex]);
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPDestinationAddressLength: 0x%02X ",
-				psfCSType->cCPacketClassificationRule.u8IPDestinationAddressLength);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				"u8IPDestinationAddressLength: 0x%02X ",
+				clsRule->u8IPDestinationAddressLength);
 
 		for (uiLoopIndex = 0; uiLoopIndex < 32; uiLoopIndex++)
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8IPDestinationAddress[32]:0x%X ",
-					psfCSType->cCPacketClassificationRule.u8IPDestinationAddress[uiLoopIndex]);
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+					DBG_LVL_ALL,
+					" u8IPDestinationAddress[32]:0x%X ",
+					clsRule->u8IPDestinationAddress[uiLoopIndex]);
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8ProtocolSourcePortRangeLength: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRangeLength);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8ProtocolSourcePortRangeLength: 0x%X ",
+				clsRule->u8ProtocolSourcePortRangeLength);
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8ProtocolSourcePortRange[4]: 0x%X ,0x%X ,0x%X ,0x%X ",
-				psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRange[0],
-				psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRange[1],
-				psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRange[2],
-				psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRange[3]);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8ProtocolSourcePortRange[4]: 0x%X ,0x%X ,0x%X ,0x%X ",
+				clsRule->u8ProtocolSourcePortRange[0],
+				clsRule->u8ProtocolSourcePortRange[1],
+				clsRule->u8ProtocolSourcePortRange[2],
+				clsRule->u8ProtocolSourcePortRange[3]);
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8ProtocolDestPortRangeLength: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRangeLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8ProtocolDestPortRange[4]: 0x%X ,0x%X ,0x%X ,0x%X ",
-				psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRange[0],
-				psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRange[1],
-				psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRange[2],
-				psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRange[3]);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8ProtocolDestPortRangeLength: 0x%X ",
+				clsRule->u8ProtocolDestPortRangeLength);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8ProtocolDestPortRange[4]: 0x%X ,0x%X ,0x%X ,0x%X ",
+				clsRule->u8ProtocolDestPortRange[0],
+				clsRule->u8ProtocolDestPortRange[1],
+				clsRule->u8ProtocolDestPortRange[2],
+				clsRule->u8ProtocolDestPortRange[3]);
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8EthernetDestMacAddressLength: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddressLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8EthernetDestMacAddress[6]: 0x%X ,0x%X ,0x%X ,0x%X ,0x%X ,0x%X",
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[0],
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[1],
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[2],
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[3],
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[4],
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[5]);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8EthernetSourceMACAddressLength: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddressLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8EthernetSourceMACAddress[6]: 0x%X ,0x%X ,0x%X ,0x%X ,0x%X ,0x%X",
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[0],
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[1],
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[2],
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[3],
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[4],
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[5]);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8EthertypeLength: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8EthertypeLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8Ethertype[3]: 0x%X ,0x%X ,0x%X ",
-				psfCSType->cCPacketClassificationRule.u8Ethertype[0],
-				psfCSType->cCPacketClassificationRule.u8Ethertype[1],
-				psfCSType->cCPacketClassificationRule.u8Ethertype[2]);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u16UserPriority: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u16UserPriority);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u16VLANID: 0x%X ", psfCSType->cCPacketClassificationRule.u16VLANID);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8AssociatedPHSI: 0x%X ", psfCSType->cCPacketClassificationRule.u8AssociatedPHSI);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u16PacketClassificationRuleIndex:0x%X ",
-				psfCSType->cCPacketClassificationRule.u16PacketClassificationRuleIndex);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8EthernetDestMacAddressLength: 0x%X ",
+				clsRule->u8EthernetDestMacAddressLength);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8EthernetDestMacAddress[6]: 0x%X ,0x%X ,0x%X ,0x%X ,0x%X ,0x%X",
+				clsRule->u8EthernetDestMacAddress[0],
+				clsRule->u8EthernetDestMacAddress[1],
+				clsRule->u8EthernetDestMacAddress[2],
+				clsRule->u8EthernetDestMacAddress[3],
+				clsRule->u8EthernetDestMacAddress[4],
+				clsRule->u8EthernetDestMacAddress[5]);
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8VendorSpecificClassifierParamLength:0x%X ",
-				psfCSType->cCPacketClassificationRule.u8VendorSpecificClassifierParamLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8VendorSpecificClassifierParam[1]:0x%X ",
-				psfCSType->cCPacketClassificationRule.u8VendorSpecificClassifierParam[0]);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8EthernetSourceMACAddressLength: 0x%X ",
+				clsRule->u8EthernetDestMacAddressLength);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				"u8EthernetSourceMACAddress[6]: 0x%X ,0x%X ,0x%X ,0x%X ,0x%X ,0x%X",
+				clsRule->u8EthernetSourceMACAddress[0],
+				clsRule->u8EthernetSourceMACAddress[1],
+				clsRule->u8EthernetSourceMACAddress[2],
+				clsRule->u8EthernetSourceMACAddress[3],
+				clsRule->u8EthernetSourceMACAddress[4],
+				clsRule->u8EthernetSourceMACAddress[5]);
+
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, " u8EthertypeLength: 0x%X ",
+				clsRule->u8EthertypeLength);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8Ethertype[3]: 0x%X ,0x%X ,0x%X ",
+				clsRule->u8Ethertype[0],
+				clsRule->u8Ethertype[1],
+				clsRule->u8Ethertype[2]);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, " u16UserPriority: 0x%X ",
+				clsRule->u16UserPriority);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, " u16VLANID: 0x%X ",
+				clsRule->u16VLANID);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, " u8AssociatedPHSI: 0x%X ",
+				clsRule->u8AssociatedPHSI);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u16PacketClassificationRuleIndex:0x%X ",
+				clsRule->u16PacketClassificationRuleIndex);
+
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8VendorSpecificClassifierParamLength:0x%X ",
+				clsRule->u8VendorSpecificClassifierParamLength);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8VendorSpecificClassifierParam[1]:0x%X ",
+				clsRule->u8VendorSpecificClassifierParam[0]);
 #ifdef VERSION_D5
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8IPv6FlowLableLength: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLableLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8IPv6FlowLable[6]: 0x%X ,0x%X ,0x%X ,0x%X ,0x%X ,0x%X ",
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[0],
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[1],
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[2],
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[3],
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[4],
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[5]);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, " u8IPv6FlowLableLength: 0x%X ",
+				clsRule->u8IPv6FlowLableLength);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8IPv6FlowLable[6]: 0x%X ,0x%X ,0x%X ,0x%X ,0x%X ,0x%X ",
+				clsRule->u8IPv6FlowLable[0],
+				clsRule->u8IPv6FlowLable[1],
+				clsRule->u8IPv6FlowLable[2],
+				clsRule->u8IPv6FlowLable[3],
+				clsRule->u8IPv6FlowLable[4],
+				clsRule->u8IPv6FlowLable[5]);
 #endif
 	}
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " bValid: 0x%X", pstAddIndication->sfActiveSet.bValid);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL,
+			" bValid: 0x%X", pstAddIndication->sfActiveSet.bValid);
 }
 
-static inline ULONG RestoreSFParam(struct bcm_mini_adapter *Adapter, ULONG ulAddrSFParamSet, PUCHAR pucDestBuffer)
+static inline ULONG RestoreSFParam(struct bcm_mini_adapter *Adapter,
+		ULONG ulAddrSFParamSet, PUCHAR pucDestBuffer)
 {
 	UINT  nBytesToRead = sizeof(struct bcm_connect_mgr_params);
 
 	if (ulAddrSFParamSet == 0 || NULL == pucDestBuffer) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Got Param address as 0!!");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"Got Param address as 0!!");
 		return 0;
 	}
 	ulAddrSFParamSet = ntohl(ulAddrSFParamSet);
 
 	/* Read out the SF Param Set At the indicated Location */
-	if (rdm(Adapter, ulAddrSFParamSet, (PUCHAR)pucDestBuffer, nBytesToRead) < 0)
+	if (rdm(Adapter, ulAddrSFParamSet,(PUCHAR)pucDestBuffer, nBytesToRead) < 0)
 		return STATUS_FAILURE;
 
 	return 1;
 }
 
-static ULONG StoreSFParam(struct bcm_mini_adapter *Adapter, PUCHAR pucSrcBuffer, ULONG ulAddrSFParamSet)
+static ULONG StoreSFParam(struct bcm_mini_adapter *Adapter, PUCHAR pucSrcBuffer,
+		ULONG ulAddrSFParamSet)
 {
 	UINT nBytesToWrite = sizeof(struct bcm_connect_mgr_params);
 	int ret = 0;
@@ -1323,13 +1435,15 @@ static ULONG StoreSFParam(struct bcm_mini_adapter *Adapter, PUCHAR pucSrcBuffer,
 
 	ret = wrm(Adapter, ulAddrSFParamSet, (u8 *)pucSrcBuffer, nBytesToWrite);
 	if (ret < 0) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "%s:%d WRM failed", __func__, __LINE__);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"%s:%d WRM failed", __func__, __LINE__);
 		return ret;
 	}
 	return 1;
 }
 
-ULONG StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter, PVOID pvBuffer, UINT *puBufferLength)
+ULONG StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter,
+		PVOID pvBuffer, UINT *puBufferLength)
 {
 	struct bcm_add_indication_alt *pstAddIndicationAlt = NULL;
 	struct bcm_add_indication *pstAddIndication = NULL;
@@ -1363,13 +1477,15 @@ ULONG StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter, PVOID pvBu
 	}
 	/* For DSA_REQ, only up to "psfAuthorizedSet" parameter should be accessed by driver! */
 
-	pstAddIndication = kmalloc(sizeof(struct bcm_add_indication), GFP_KERNEL);
+	pstAddIndication = kmalloc(sizeof(struct bcm_add_indication),
+			GFP_KERNEL);
 	if (pstAddIndication == NULL)
 		return 0;
 
 	/* AUTHORIZED SET */
 	pstAddIndication->psfAuthorizedSet = (struct bcm_connect_mgr_params *)
-			GetNextTargetBufferLocation(Adapter, pstAddIndicationAlt->u16TID);
+			GetNextTargetBufferLocation(Adapter,
+					pstAddIndicationAlt->u16TID);
 	if (!pstAddIndication->psfAuthorizedSet) {
 		kfree(pstAddIndication);
 		return 0;
@@ -1382,7 +1498,9 @@ ULONG StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter, PVOID pvBu
 	}
 
 	/* this can't possibly be right */
-	pstAddIndication->psfAuthorizedSet = (struct bcm_connect_mgr_params *)ntohl((ULONG)pstAddIndication->psfAuthorizedSet);
+	pstAddIndication->psfAuthorizedSet =
+		(struct bcm_connect_mgr_params *) ntohl(
+				(ULONG)pstAddIndication->psfAuthorizedSet);
 
 	if (pstAddIndicationAlt->u8Type == DSA_REQ) {
 		struct bcm_add_request AddRequest;
@@ -1411,31 +1529,39 @@ ULONG StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter, PVOID pvBu
 
 	/* ADMITTED SET */
 	pstAddIndication->psfAdmittedSet = (struct bcm_connect_mgr_params *)
-		GetNextTargetBufferLocation(Adapter, pstAddIndicationAlt->u16TID);
+		GetNextTargetBufferLocation(Adapter,
+				pstAddIndicationAlt->u16TID);
 	if (!pstAddIndication->psfAdmittedSet) {
 		kfree(pstAddIndication);
 		return 0;
 	}
-	if (StoreSFParam(Adapter, (PUCHAR)&pstAddIndicationAlt->sfAdmittedSet, (ULONG)pstAddIndication->psfAdmittedSet) != 1) {
+	if (StoreSFParam(Adapter, (PUCHAR)&pstAddIndicationAlt->sfAdmittedSet,
+				(ULONG)pstAddIndication->psfAdmittedSet) != 1) {
 		kfree(pstAddIndication);
 		return 0;
 	}
 
-	pstAddIndication->psfAdmittedSet = (struct bcm_connect_mgr_params *)ntohl((ULONG)pstAddIndication->psfAdmittedSet);
+	pstAddIndication->psfAdmittedSet =
+		(struct bcm_connect_mgr_params *) ntohl(
+				(ULONG) pstAddIndication->psfAdmittedSet);
 
 	/* ACTIVE SET */
 	pstAddIndication->psfActiveSet = (struct bcm_connect_mgr_params *)
-		GetNextTargetBufferLocation(Adapter, pstAddIndicationAlt->u16TID);
+		GetNextTargetBufferLocation(Adapter,
+				pstAddIndicationAlt->u16TID);
 	if (!pstAddIndication->psfActiveSet) {
 		kfree(pstAddIndication);
 		return 0;
 	}
-	if (StoreSFParam(Adapter, (PUCHAR)&pstAddIndicationAlt->sfActiveSet, (ULONG)pstAddIndication->psfActiveSet) != 1) {
+	if (StoreSFParam(Adapter, (PUCHAR)&pstAddIndicationAlt->sfActiveSet,
+				(ULONG)pstAddIndication->psfActiveSet) != 1) {
 		kfree(pstAddIndication);
 		return 0;
 	}
 
-	pstAddIndication->psfActiveSet = (struct bcm_connect_mgr_params *)ntohl((ULONG)pstAddIndication->psfActiveSet);
+	pstAddIndication->psfActiveSet =
+		(struct bcm_connect_mgr_params *) ntohl(
+				(ULONG)pstAddIndication->psfActiveSet);
 
 	(*puBufferLength) = sizeof(struct bcm_add_indication);
 	*(struct bcm_add_indication *)pvBuffer = *pstAddIndication;
@@ -1444,40 +1570,63 @@ ULONG StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter, PVOID pvBu
 }
 
 static inline struct bcm_add_indication_alt
-*RestoreCmControlResponseMessage(register struct bcm_mini_adapter *Adapter, register PVOID pvBuffer)
+*RestoreCmControlResponseMessage(register struct bcm_mini_adapter *Adapter,
+		register PVOID pvBuffer)
 {
 	ULONG ulStatus = 0;
 	struct bcm_add_indication *pstAddIndication = NULL;
 	struct bcm_add_indication_alt *pstAddIndicationDest = NULL;
 
 	pstAddIndication = pvBuffer;
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "=====>");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"=====>");
 	if ((pstAddIndication->u8Type == DSD_REQ) ||
 		(pstAddIndication->u8Type == DSD_RSP) ||
 		(pstAddIndication->u8Type == DSD_ACK))
 		return pvBuffer;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Inside RestoreCmControlResponseMessage ");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Inside RestoreCmControlResponseMessage ");
 	/*
 	 * Need to Allocate memory to contain the SUPER Large structures
 	 * Our driver can't create these structures on Stack :(
 	 */
-	pstAddIndicationDest = kmalloc(sizeof(struct bcm_add_indication_alt), GFP_KERNEL);
+	pstAddIndicationDest = kmalloc(sizeof(struct bcm_add_indication_alt),
+			GFP_KERNEL);
 
 	if (pstAddIndicationDest) {
-		memset(pstAddIndicationDest, 0, sizeof(struct bcm_add_indication_alt));
+		memset(pstAddIndicationDest, 0,
+				sizeof(struct bcm_add_indication_alt));
 	} else {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Failed to allocate memory for SF Add Indication Structure ");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG,
+				DBG_LVL_ALL,
+				"Failed to allocate memory for SF Add Indication Structure ");
 		return NULL;
 	}
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "AddIndication-u8Type : 0x%X", pstAddIndication->u8Type);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "AddIndication-u8Direction : 0x%X", pstAddIndication->eConnectionDir);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "AddIndication-u8TID : 0x%X", ntohs(pstAddIndication->u16TID));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "AddIndication-u8CID : 0x%X", ntohs(pstAddIndication->u16CID));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "AddIndication-u16VCID : 0x%X", ntohs(pstAddIndication->u16VCID));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "AddIndication-autorized set loc : %p", pstAddIndication->psfAuthorizedSet);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "AddIndication-admitted set loc : %p", pstAddIndication->psfAdmittedSet);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "AddIndication-Active set loc : %p", pstAddIndication->psfActiveSet);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"AddIndication-u8Type : 0x%X",
+			pstAddIndication->u8Type);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"AddIndication-u8Direction : 0x%X",
+			pstAddIndication->eConnectionDir);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"AddIndication-u8TID : 0x%X",
+			ntohs(pstAddIndication->u16TID));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"AddIndication-u8CID : 0x%X",
+			ntohs(pstAddIndication->u16CID));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"AddIndication-u16VCID : 0x%X",
+			ntohs(pstAddIndication->u16VCID));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"AddIndication-autorized set loc : %p",
+			pstAddIndication->psfAuthorizedSet);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"AddIndication-admitted set loc : %p",
+			pstAddIndication->psfAdmittedSet);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"AddIndication-Active set loc : %p",
+			pstAddIndication->psfActiveSet);
 
 	pstAddIndicationDest->u8Type = pstAddIndication->u8Type;
 	pstAddIndicationDest->u8Direction = pstAddIndication->eConnectionDir;
@@ -1486,39 +1635,60 @@ static inline struct bcm_add_indication_alt
 	pstAddIndicationDest->u16VCID = pstAddIndication->u16VCID;
 	pstAddIndicationDest->u8CC = pstAddIndication->u8CC;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,  "Restoring Active Set ");
-	ulStatus = RestoreSFParam(Adapter, (ULONG)pstAddIndication->psfActiveSet, (PUCHAR)&pstAddIndicationDest->sfActiveSet);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Restoring Active Set ");
+	ulStatus = RestoreSFParam(Adapter,
+			(ULONG)pstAddIndication->psfActiveSet,
+			(PUCHAR)&pstAddIndicationDest->sfActiveSet);
 	if (ulStatus != 1)
 		goto failed_restore_sf_param;
 
 	if (pstAddIndicationDest->sfActiveSet.u8TotalClassifiers > MAX_CLASSIFIERS_IN_SF)
-		pstAddIndicationDest->sfActiveSet.u8TotalClassifiers = MAX_CLASSIFIERS_IN_SF;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,  "Restoring Admitted Set ");
-	ulStatus = RestoreSFParam(Adapter, (ULONG)pstAddIndication->psfAdmittedSet, (PUCHAR)&pstAddIndicationDest->sfAdmittedSet);
+		pstAddIndicationDest->sfActiveSet.u8TotalClassifiers =
+			MAX_CLASSIFIERS_IN_SF;
+
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Restoring Admitted Set ");
+	ulStatus = RestoreSFParam(Adapter,
+			(ULONG)pstAddIndication->psfAdmittedSet,
+			(PUCHAR)&pstAddIndicationDest->sfAdmittedSet);
 	if (ulStatus != 1)
 		goto failed_restore_sf_param;
 
 	if (pstAddIndicationDest->sfAdmittedSet.u8TotalClassifiers > MAX_CLASSIFIERS_IN_SF)
-		pstAddIndicationDest->sfAdmittedSet.u8TotalClassifiers = MAX_CLASSIFIERS_IN_SF;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,  "Restoring Authorized Set ");
-	ulStatus = RestoreSFParam(Adapter, (ULONG)pstAddIndication->psfAuthorizedSet, (PUCHAR)&pstAddIndicationDest->sfAuthorizedSet);
+		pstAddIndicationDest->sfAdmittedSet.u8TotalClassifiers =
+			MAX_CLASSIFIERS_IN_SF;
+
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Restoring Authorized Set ");
+	ulStatus = RestoreSFParam(Adapter,
+			(ULONG)pstAddIndication->psfAuthorizedSet,
+			(PUCHAR)&pstAddIndicationDest->sfAuthorizedSet);
 	if (ulStatus != 1)
 		goto failed_restore_sf_param;
 
 	if (pstAddIndicationDest->sfAuthorizedSet.u8TotalClassifiers > MAX_CLASSIFIERS_IN_SF)
-		pstAddIndicationDest->sfAuthorizedSet.u8TotalClassifiers = MAX_CLASSIFIERS_IN_SF;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Dumping the whole raw packet");
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "============================================================");
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, " pstAddIndicationDest->sfActiveSet size  %zx %p", sizeof(*pstAddIndicationDest), pstAddIndicationDest);
-	/* BCM_DEBUG_PRINT_BUFFER(Adapter,DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, (unsigned char *)pstAddIndicationDest, sizeof(*pstAddIndicationDest)); */
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "============================================================");
+		pstAddIndicationDest->sfAuthorizedSet.u8TotalClassifiers =
+			MAX_CLASSIFIERS_IN_SF;
+
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Dumping the whole raw packet");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+		"============================================================");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			" pstAddIndicationDest->sfActiveSet size  %zx %p",
+			sizeof(*pstAddIndicationDest), pstAddIndicationDest);
+	/* BCM_DEBUG_PRINT_BUFFER(Adapter,DBG_TYPE_OTHERS, CONN_MSG,
+	 *		DBG_LVL_ALL, (unsigned char *)pstAddIndicationDest,
+	 *		sizeof(*pstAddIndicationDest));
+	 */
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"============================================================");
 	return pstAddIndicationDest;
 failed_restore_sf_param:
 	kfree(pstAddIndicationDest);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "<=====");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"<=====");
 	return NULL;
 }
 
@@ -1530,31 +1700,44 @@ ULONG SetUpTargetDsxBuffers(struct bcm_mini_adapter *Adapter)
 	int Status;
 
 	if (!Adapter) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Adapter was NULL!!!");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"Adapter was NULL!!!");
 		return 0;
 	}
 
 	if (Adapter->astTargetDsxBuffer[0].ulTargetDsxBuffer)
 		return 1;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Size of Each DSX Buffer(Also size of connection manager parameters): %zx ", sizeof(struct bcm_connect_mgr_params));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Reading DSX buffer From Target location %x ", DSX_MESSAGE_EXCHANGE_BUFFER);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Size of Each DSX Buffer(Also size of connection manager parameters): %zx ",
+			sizeof(struct bcm_connect_mgr_params));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Reading DSX buffer From Target location %x ",
+			DSX_MESSAGE_EXCHANGE_BUFFER);
 
-	Status = rdmalt(Adapter, DSX_MESSAGE_EXCHANGE_BUFFER, (PUINT)&ulTargetDsxBuffersBase, sizeof(UINT));
+	Status = rdmalt(Adapter, DSX_MESSAGE_EXCHANGE_BUFFER,
+			(PUINT)&ulTargetDsxBuffersBase, sizeof(UINT));
 	if (Status < 0) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "RDM failed!!");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"RDM failed!!");
 		return 0;
 	}
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Base Address Of DSX  Target Buffer : 0x%lx", ulTargetDsxBuffersBase);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,  "Tgt Buffer is Now %lx :", ulTargetDsxBuffersBase);
-	ulCntTargetBuffers = DSX_MESSAGE_EXCHANGE_BUFFER_SIZE / sizeof(struct bcm_connect_mgr_params);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Base Address Of DSX  Target Buffer : 0x%lx",
+			ulTargetDsxBuffersBase);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Tgt Buffer is Now %lx :", ulTargetDsxBuffersBase);
+	ulCntTargetBuffers = DSX_MESSAGE_EXCHANGE_BUFFER_SIZE /
+		sizeof(struct bcm_connect_mgr_params);
 
 	Adapter->ulTotalTargetBuffersAvailable =
 		ulCntTargetBuffers > MAX_TARGET_DSX_BUFFERS ?
 		MAX_TARGET_DSX_BUFFERS : ulCntTargetBuffers;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, " Total Target DSX Buffer setup %lx ", Adapter->ulTotalTargetBuffersAvailable);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			" Total Target DSX Buffer setup %lx ",
+			Adapter->ulTotalTargetBuffersAvailable);
 
 	for (i = 0; i < Adapter->ulTotalTargetBuffersAvailable; i++) {
 		Adapter->astTargetDsxBuffer[i].ulTargetDsxBuffer = ulTargetDsxBuffersBase;
@@ -1569,12 +1752,14 @@ ULONG SetUpTargetDsxBuffers(struct bcm_mini_adapter *Adapter)
 	return 1;
 }
 
-static ULONG GetNextTargetBufferLocation(struct bcm_mini_adapter *Adapter, B_UINT16 tid)
+static ULONG GetNextTargetBufferLocation(struct bcm_mini_adapter *Adapter,
+		B_UINT16 tid)
 {
 	ULONG dsx_buf;
 	ULONG idx, max_try;
 
-	if ((Adapter->ulTotalTargetBuffersAvailable == 0) || (Adapter->ulFreeTargetBufferCnt == 0)) {
+	if ((Adapter->ulTotalTargetBuffersAvailable == 0)
+			|| (Adapter->ulFreeTargetBufferCnt == 0)) {
 		ClearTargetDSXBuffer(Adapter, tid, false);
 		return 0;
 	}
@@ -1587,7 +1772,9 @@ static ULONG GetNextTargetBufferLocation(struct bcm_mini_adapter *Adapter, B_UIN
 	}
 
 	if (max_try == 0) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "\n GetNextTargetBufferLocation : Error No Free Target DSX Buffers FreeCnt : %lx ", Adapter->ulFreeTargetBufferCnt);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+				"\n GetNextTargetBufferLocation : Error No Free Target DSX Buffers FreeCnt : %lx ",
+				Adapter->ulFreeTargetBufferCnt);
 		ClearTargetDSXBuffer(Adapter, tid, false);
 		return 0;
 	}
@@ -1598,7 +1785,9 @@ static ULONG GetNextTargetBufferLocation(struct bcm_mini_adapter *Adapter, B_UIN
 	Adapter->ulFreeTargetBufferCnt--;
 	idx = (idx+1)%Adapter->ulTotalTargetBuffersAvailable;
 	Adapter->ulCurrentTargetBuffer = idx;
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "GetNextTargetBufferLocation :Returning address %lx tid %d\n", dsx_buf, tid);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+			"GetNextTargetBufferLocation :Returning address %lx tid %d\n",
+			dsx_buf, tid);
 
 	return dsx_buf;
 }
@@ -1609,7 +1798,8 @@ int AllocAdapterDsxBuffer(struct bcm_mini_adapter *Adapter)
 	 * Need to Allocate memory to contain the SUPER Large structures
 	 * Our driver can't create these structures on Stack
 	 */
-	Adapter->caDsxReqResp = kmalloc(sizeof(struct bcm_add_indication_alt)+LEADER_SIZE, GFP_KERNEL);
+	Adapter->caDsxReqResp = kmalloc(sizeof(struct bcm_add_indication_alt)
+			+ LEADER_SIZE, GFP_KERNEL);
 	if (!Adapter->caDsxReqResp)
 		return -ENOMEM;
 
@@ -1867,29 +2057,37 @@ bool CmControlResponseMessage(struct bcm_mini_adapter *Adapter,  /* <Pointer to
 	return TRUE;
 }
 
-int get_dsx_sf_data_to_application(struct bcm_mini_adapter *Adapter, UINT uiSFId, void __user *user_buffer)
+int get_dsx_sf_data_to_application(struct bcm_mini_adapter *Adapter,
+		UINT uiSFId, void __user *user_buffer)
 {
 	int status = 0;
 	struct bcm_packet_info *psSfInfo = NULL;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "status =%d", status);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"status =%d", status);
 	status = SearchSfid(Adapter, uiSFId);
 	if (status >= NO_OF_QUEUES) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "SFID %d not present in queue !!!", uiSFId);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"SFID %d not present in queue !!!", uiSFId);
 		return -EINVAL;
 	}
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "status =%d", status);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"status =%d", status);
 	psSfInfo = &Adapter->PackInfo[status];
-	if (psSfInfo->pstSFIndication && copy_to_user(user_buffer,
-							psSfInfo->pstSFIndication, sizeof(struct bcm_add_indication_alt))) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "copy to user failed SFID %d, present in queue !!!", uiSFId);
+	if (psSfInfo->pstSFIndication
+			&& copy_to_user(user_buffer, psSfInfo->pstSFIndication,
+				sizeof(struct bcm_add_indication_alt))) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+				"copy to user failed SFID %d, present in queue !!!",
+				uiSFId);
 		status = -EFAULT;
 		return status;
 	}
 	return STATUS_SUCCESS;
 }
 
-VOID OverrideServiceFlowParams(struct bcm_mini_adapter *Adapter, PUINT puiBuffer)
+VOID OverrideServiceFlowParams(struct bcm_mini_adapter *Adapter,
+		PUINT puiBuffer)
 {
 	B_UINT32 u32NumofSFsinMsg = ntohl(*(puiBuffer + 1));
 	struct bcm_stim_sfhostnotify *pHostInfo = NULL;
@@ -1897,7 +2095,8 @@ VOID OverrideServiceFlowParams(struct bcm_mini_adapter *Adapter, PUINT puiBuffer
 	ULONG ulSFID = 0;
 
 	puiBuffer += 2;
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "u32NumofSFsinMsg: 0x%x\n", u32NumofSFsinMsg);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"u32NumofSFsinMsg: 0x%x\n", u32NumofSFsinMsg);
 
 	while (u32NumofSFsinMsg != 0 && u32NumofSFsinMsg < NO_OF_QUEUES) {
 		u32NumofSFsinMsg--;
@@ -1906,31 +2105,149 @@ VOID OverrideServiceFlowParams(struct bcm_mini_adapter *Adapter, PUINT puiBuffer
 
 		ulSFID = ntohl(pHostInfo->SFID);
 		uiSearchRuleIndex = SearchSfid(Adapter, ulSFID);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "SFID: 0x%lx\n", ulSFID);
-
-		if (uiSearchRuleIndex >= NO_OF_QUEUES || uiSearchRuleIndex == HiPriority) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "The SFID <%lx> doesn't exist in host entry or is Invalid\n", ulSFID);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"SFID: 0x%lx\n", ulSFID);
+
+		if (uiSearchRuleIndex >= NO_OF_QUEUES
+				|| uiSearchRuleIndex == HiPriority) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG,
+					DBG_LVL_ALL,
+					"The SFID <%lx> doesn't exist in host entry or is Invalid\n",
+					ulSFID);
 			continue;
 		}
 
 		if (pHostInfo->RetainSF == false) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Going to Delete SF");
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG,
+					DBG_LVL_ALL, "Going to Delete SF");
 			deleteSFBySfid(Adapter, uiSearchRuleIndex);
 		} else {
-			Adapter->PackInfo[uiSearchRuleIndex].usVCID_Value = ntohs(pHostInfo->VCID);
-			Adapter->PackInfo[uiSearchRuleIndex].usCID = ntohs(pHostInfo->newCID);
-			Adapter->PackInfo[uiSearchRuleIndex].bActive = false;
+			struct bcm_packet_info *packinfo =
+				&Adapter->PackInfo[uiSearchRuleIndex];
 
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "pHostInfo->QoSParamSet: 0x%x\n", pHostInfo->QoSParamSet);
+			packinfo->usVCID_Value = ntohs(pHostInfo->VCID);
+			packinfo->usCID = ntohs(pHostInfo->newCID);
+			packinfo->bActive = false;
+
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG,
+					DBG_LVL_ALL,
+					"pHostInfo->QoSParamSet: 0x%x\n",
+					pHostInfo->QoSParamSet);
 
 			if (pHostInfo->QoSParamSet & 0x1)
-				Adapter->PackInfo[uiSearchRuleIndex].bAuthorizedSet = TRUE;
+				packinfo->bAuthorizedSet = TRUE;
 			if (pHostInfo->QoSParamSet & 0x2)
-				Adapter->PackInfo[uiSearchRuleIndex].bAdmittedSet = TRUE;
+				packinfo->bAdmittedSet = TRUE;
 			if (pHostInfo->QoSParamSet & 0x4) {
-				Adapter->PackInfo[uiSearchRuleIndex].bActiveSet = TRUE;
-				Adapter->PackInfo[uiSearchRuleIndex].bActive = TRUE;
+				packinfo->bActiveSet = TRUE;
+				packinfo->bActive = TRUE;
+			}
+		}
+	}
+}
+
+static void restore_endianess_of_pstClassifierEntry(
+		struct bcm_classifier_rule *pstClassifierEntry,
+		enum bcm_ipaddr_context eIpAddrContext)
+{
+	int i;
+	union u_ip_address *stSrc  = &pstClassifierEntry->stSrcIpAddress;
+	union u_ip_address *stDest = &pstClassifierEntry->stDestIpAddress;
+
+	for (i = 0; i < MAX_IP_RANGE_LENGTH * 4; i++) {
+		if (eIpAddrContext == eSrcIpAddress) {
+			stSrc->ulIpv6Addr[i] = ntohl(stSrc->ulIpv6Addr[i]);
+			stSrc->ulIpv6Mask[i] = ntohl(stSrc->ulIpv6Mask[i]);
+		} else if (eIpAddrContext == eDestIpAddress) {
+			stDest->ulIpv6Addr[i] = ntohl(stDest->ulIpv6Addr[i]);
+			stDest->ulIpv6Mask[i] = ntohl(stDest->ulIpv6Mask[i]);
+		}
+	}
+}
+
+static void apply_phs_rule_to_all_classifiers(
+		register struct bcm_mini_adapter *Adapter,		/* <Pointer to the Adapter structure */
+		register UINT uiSearchRuleIndex,			/* <Index of Queue, to which this data belongs */
+		USHORT uVCID,
+		struct bcm_phs_rule *sPhsRule,
+		struct bcm_phs_rules *cPhsRule,
+		struct bcm_add_indication_alt *pstAddIndication)
+{
+	unsigned int uiClassifierIndex = 0;
+	struct bcm_classifier_rule *curr_classifier = NULL;
+
+	if (pstAddIndication->u8Direction == UPLINK_DIR) {
+		for (uiClassifierIndex = 0; uiClassifierIndex < MAX_CLASSIFIERS; uiClassifierIndex++) {
+			curr_classifier =
+				&Adapter->astClassifierTable[uiClassifierIndex];
+			if ((curr_classifier->bUsed) &&
+				(curr_classifier->ulSFID == Adapter->PackInfo[uiSearchRuleIndex].ulSFID) &&
+				(curr_classifier->u8AssociatedPHSI == cPhsRule->u8PHSI)) {
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+						"Adding PHS Rule For Classifier: 0x%x cPhsRule.u8PHSI: 0x%x\n",
+						curr_classifier->uiClassifierRuleIndex,
+						cPhsRule->u8PHSI);
+				/* Update The PHS Rule for this classifier as Associated PHSI id defined */
+
+				/* Copy the PHS Rule */
+				sPhsRule->u8PHSI = cPhsRule->u8PHSI;
+				sPhsRule->u8PHSFLength = cPhsRule->u8PHSFLength;
+				sPhsRule->u8PHSMLength = cPhsRule->u8PHSMLength;
+				sPhsRule->u8PHSS = cPhsRule->u8PHSS;
+				sPhsRule->u8PHSV = cPhsRule->u8PHSV;
+				memcpy(sPhsRule->u8PHSF, cPhsRule->u8PHSF, MAX_PHS_LENGTHS);
+				memcpy(sPhsRule->u8PHSM, cPhsRule->u8PHSM, MAX_PHS_LENGTHS);
+				sPhsRule->u8RefCnt = 0;
+				sPhsRule->bUnclassifiedPHSRule = false;
+				sPhsRule->PHSModifiedBytes = 0;
+				sPhsRule->PHSModifiedNumPackets = 0;
+				sPhsRule->PHSErrorNumPackets = 0;
+
+				/* bPHSRuleAssociated = TRUE; */
+				/* Store The PHS Rule for this classifier */
+
+				PhsUpdateClassifierRule(
+					&Adapter->stBCMPhsContext,
+					uVCID,
+					curr_classifier->uiClassifierRuleIndex,
+					sPhsRule,
+					curr_classifier->u8AssociatedPHSI);
+
+				/* Update PHS Rule For the Classifier */
+				if (sPhsRule->u8PHSI) {
+					curr_classifier->u32PHSRuleID = sPhsRule->u8PHSI;
+					memcpy(&curr_classifier->sPhsRule, sPhsRule, sizeof(struct bcm_phs_rule));
+				}
 			}
 		}
+	} else {
+		/* Error PHS Rule specified in signaling could not be applied to any classifier */
+
+		/* Copy the PHS Rule */
+		sPhsRule->u8PHSI = cPhsRule->u8PHSI;
+		sPhsRule->u8PHSFLength = cPhsRule->u8PHSFLength;
+		sPhsRule->u8PHSMLength = cPhsRule->u8PHSMLength;
+		sPhsRule->u8PHSS = cPhsRule->u8PHSS;
+		sPhsRule->u8PHSV = cPhsRule->u8PHSV;
+		memcpy(sPhsRule->u8PHSF, cPhsRule->u8PHSF, MAX_PHS_LENGTHS);
+		memcpy(sPhsRule->u8PHSM, cPhsRule->u8PHSM, MAX_PHS_LENGTHS);
+		sPhsRule->u8RefCnt = 0;
+		sPhsRule->bUnclassifiedPHSRule = TRUE;
+		sPhsRule->PHSModifiedBytes = 0;
+		sPhsRule->PHSModifiedNumPackets = 0;
+		sPhsRule->PHSErrorNumPackets = 0;
+		/* Store The PHS Rule for this classifier */
+
+		/*
+		 * Passing the argument u8PHSI instead of clsid. Because for DL with no classifier rule,
+		 * clsid will be zero hence we can't have multiple PHS rules for the same SF.
+		 * To support multiple PHS rule, passing u8PHSI.
+		 */
+		PhsUpdateClassifierRule(
+			&Adapter->stBCMPhsContext,
+			uVCID,
+			sPhsRule->u8PHSI,
+			sPhsRule,
+			sPhsRule->u8PHSI);
 	}
 }
diff --git a/drivers/staging/bcm/InterfaceDld.c b/drivers/staging/bcm/InterfaceDld.c
index 005e4607b26..e1925bdc127 100644
--- a/drivers/staging/bcm/InterfaceDld.c
+++ b/drivers/staging/bcm/InterfaceDld.c
@@ -53,8 +53,8 @@ int InterfaceFileReadbackFromChip(PVOID arg, struct file *flp,
 	struct bcm_interface_adapter *psIntfAdapter = arg;
 	int bytes;
 
-	buff = kmalloc(MAX_TRANSFER_CTRL_BYTE_USB, GFP_DMA);
-	buff_readback = kmalloc(MAX_TRANSFER_CTRL_BYTE_USB , GFP_DMA);
+	buff = kzalloc(MAX_TRANSFER_CTRL_BYTE_USB, GFP_DMA);
+	buff_readback = kzalloc(MAX_TRANSFER_CTRL_BYTE_USB , GFP_DMA);
 	if (!buff || !buff_readback) {
 		kfree(buff);
 		kfree(buff_readback);
@@ -64,8 +64,6 @@ int InterfaceFileReadbackFromChip(PVOID arg, struct file *flp,
 
 	is_config_file = (on_chip_loc == CONFIG_BEGIN_ADDR) ? 1 : 0;
 
-	memset(buff_readback, 0, MAX_TRANSFER_CTRL_BYTE_USB);
-	memset(buff, 0, MAX_TRANSFER_CTRL_BYTE_USB);
 	while (1) {
 		oldfs = get_fs();
 		set_fs(get_ds());
diff --git a/drivers/staging/bcm/InterfaceIdleMode.c b/drivers/staging/bcm/InterfaceIdleMode.c
index fecf81ffe06..c84ee494f55 100644
--- a/drivers/staging/bcm/InterfaceIdleMode.c
+++ b/drivers/staging/bcm/InterfaceIdleMode.c
@@ -223,7 +223,6 @@ static int InterfaceAbortIdlemode(struct bcm_mini_adapter *Adapter,
 }
 int InterfaceIdleModeWakeup(struct bcm_mini_adapter *Adapter)
 {
-	ULONG	Status = 0;
 	if (Adapter->bTriedToWakeUpFromlowPowerMode) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
 		IDLE_MODE, DBG_LVL_ALL,
@@ -233,7 +232,7 @@ int InterfaceIdleModeWakeup(struct bcm_mini_adapter *Adapter)
 		InterfaceAbortIdlemode(Adapter, Adapter->usIdleModePattern);
 
 	}
-	return Status;
+	return 0;
 }
 
 void InterfaceHandleShutdownModeWakeup(struct bcm_mini_adapter *Adapter)
diff --git a/drivers/staging/bcm/InterfaceMisc.h b/drivers/staging/bcm/InterfaceMisc.h
index bce6869a747..efb68604615 100644
--- a/drivers/staging/bcm/InterfaceMisc.h
+++ b/drivers/staging/bcm/InterfaceMisc.h
@@ -14,13 +14,13 @@ InterfaceWRM(struct bcm_interface_adapter *psIntfAdapter,
 			INT len);
 
 
-int InterfaceFileDownload( PVOID psIntfAdapter,
-                        struct file *flp,
-                        unsigned int on_chip_loc);
+int InterfaceFileDownload(PVOID psIntfAdapter,
+			struct file *flp,
+			unsigned int on_chip_loc);
 
-int InterfaceFileReadbackFromChip( PVOID psIntfAdapter,
-                        struct file *flp,
-                        unsigned int on_chip_loc);
+int InterfaceFileReadbackFromChip(PVOID psIntfAdapter,
+			struct file *flp,
+			unsigned int on_chip_loc);
 
 
 int BcmRDM(PVOID arg,
diff --git a/drivers/staging/bcm/PHSModule.c b/drivers/staging/bcm/PHSModule.c
index afc7bcc3e54..07c5a0bae1e 100644
--- a/drivers/staging/bcm/PHSModule.c
+++ b/drivers/staging/bcm/PHSModule.c
@@ -409,7 +409,6 @@ ULONG PhsUpdateClassifierRule(IN void *pvContext,
  */
 ULONG PhsDeletePHSRule(IN void *pvContext, IN B_UINT16 uiVcid, IN B_UINT8 u8PHSI)
 {
-	ULONG lStatus = 0;
 	UINT nSFIndex = 0, nClsidIndex = 0;
 	struct bcm_phs_entry *pstServiceFlowEntry = NULL;
 	struct bcm_phs_classifier_table *pstClassifierRulesTable = NULL;
@@ -446,7 +445,7 @@ ULONG PhsDeletePHSRule(IN void *pvContext, IN B_UINT16 uiVcid, IN B_UINT8 u8PHSI
 			}
 		}
 	}
-	return lStatus;
+	return 0;
 }
 
 /*
@@ -467,7 +466,6 @@ ULONG PhsDeletePHSRule(IN void *pvContext, IN B_UINT16 uiVcid, IN B_UINT8 u8PHSI
  */
 ULONG PhsDeleteClassifierRule(IN void *pvContext, IN B_UINT16 uiVcid, IN B_UINT16 uiClsId)
 {
-	ULONG lStatus = 0;
 	UINT nSFIndex = 0, nClsidIndex = 0;
 	struct bcm_phs_entry *pstServiceFlowEntry = NULL;
 	struct bcm_phs_classifier_entry *pstClassifierEntry = NULL;
@@ -504,7 +502,7 @@ ULONG PhsDeleteClassifierRule(IN void *pvContext, IN B_UINT16 uiVcid, IN B_UINT1
 			memset(pstClassifierEntry, 0, sizeof(struct bcm_phs_classifier_entry));
 		}
 	}
-	return lStatus;
+	return 0;
 }
 
 /*
@@ -524,7 +522,6 @@ ULONG PhsDeleteClassifierRule(IN void *pvContext, IN B_UINT16 uiVcid, IN B_UINT1
  */
 ULONG PhsDeleteSFRules(IN void *pvContext, IN B_UINT16 uiVcid)
 {
-	ULONG lStatus = 0;
 	UINT nSFIndex = 0, nClsidIndex = 0;
 	struct bcm_phs_entry *pstServiceFlowEntry = NULL;
 	struct bcm_phs_classifier_table *pstClassifierRulesTable = NULL;
@@ -573,7 +570,7 @@ ULONG PhsDeleteSFRules(IN void *pvContext, IN B_UINT16 uiVcid)
 		pstServiceFlowEntry->uiVcid = 0;
 	}
 
-	return lStatus;
+	return 0;
 }
 
 /*
diff --git a/drivers/staging/bcm/Qos.c b/drivers/staging/bcm/Qos.c
index 4f315835ddf..0c742da8c6b 100644
--- a/drivers/staging/bcm/Qos.c
+++ b/drivers/staging/bcm/Qos.c
@@ -4,11 +4,18 @@ This file contains the routines related to Quality of Service.
 */
 #include "headers.h"
 
-static void EThCSGetPktInfo(struct bcm_mini_adapter *Adapter, PVOID pvEthPayload, struct bcm_eth_packet_info *pstEthCsPktInfo);
-static bool EThCSClassifyPkt(struct bcm_mini_adapter *Adapter, struct sk_buff* skb, struct bcm_eth_packet_info *pstEthCsPktInfo, struct bcm_classifier_rule *pstClassifierRule, B_UINT8 EthCSCupport);
+static void EThCSGetPktInfo(struct bcm_mini_adapter *Adapter,
+			    PVOID pvEthPayload,
+			    struct bcm_eth_packet_info *pstEthCsPktInfo);
 
-static USHORT	IpVersion4(struct bcm_mini_adapter *Adapter, struct iphdr *iphd,
-			   struct bcm_classifier_rule *pstClassifierRule);
+static bool EThCSClassifyPkt(struct bcm_mini_adapter *Adapter,
+			     struct sk_buff *skb,
+			     struct bcm_eth_packet_info *pstEthCsPktInfo,
+			     struct bcm_classifier_rule *pstClassifierRule,
+			     B_UINT8 EthCSCupport);
+
+static USHORT IpVersion4(struct bcm_mini_adapter *Adapter, struct iphdr *iphd,
+			 struct bcm_classifier_rule *pstClassifierRule);
 
 static VOID PruneQueue(struct bcm_mini_adapter *Adapter, INT iIndex);
 
@@ -33,14 +40,11 @@ static bool MatchSrcIpAddress(struct bcm_classifier_rule *pstClassifierRule, ULO
 	ulSrcIP = ntohl(ulSrcIP);
 	if (0 == pstClassifierRule->ucIPSourceAddressLength)
 		return TRUE;
-	for (ucLoopIndex = 0; ucLoopIndex < (pstClassifierRule->ucIPSourceAddressLength); ucLoopIndex++)
-	{
+	for (ucLoopIndex = 0; ucLoopIndex < (pstClassifierRule->ucIPSourceAddressLength); ucLoopIndex++) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Src Ip Address Mask:0x%x PacketIp:0x%x and Classification:0x%x", (UINT)pstClassifierRule->stSrcIpAddress.ulIpv4Mask[ucLoopIndex], (UINT)ulSrcIP, (UINT)pstClassifierRule->stSrcIpAddress.ulIpv6Addr[ucLoopIndex]);
 		if ((pstClassifierRule->stSrcIpAddress.ulIpv4Mask[ucLoopIndex] & ulSrcIP) ==
 				(pstClassifierRule->stSrcIpAddress.ulIpv4Addr[ucLoopIndex] & pstClassifierRule->stSrcIpAddress.ulIpv4Mask[ucLoopIndex]))
-		{
 			return TRUE;
-		}
 	}
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Src Ip Address Not Matched");
 	return false;
@@ -68,13 +72,10 @@ static bool MatchDestIpAddress(struct bcm_classifier_rule *pstClassifierRule, UL
 		return TRUE;
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Destination Ip Address 0x%x 0x%x 0x%x  ", (UINT)ulDestIP, (UINT)pstClassifierRule->stDestIpAddress.ulIpv4Mask[ucLoopIndex], (UINT)pstClassifierRule->stDestIpAddress.ulIpv4Addr[ucLoopIndex]);
 
-	for (ucLoopIndex = 0; ucLoopIndex < (pstClassifierRule->ucIPDestinationAddressLength); ucLoopIndex++)
-	{
+	for (ucLoopIndex = 0; ucLoopIndex < (pstClassifierRule->ucIPDestinationAddressLength); ucLoopIndex++) {
 		if ((pstClassifierRule->stDestIpAddress.ulIpv4Mask[ucLoopIndex] & ulDestIP) ==
 				(pstClassifierRule->stDestIpAddress.ulIpv4Addr[ucLoopIndex] & pstClassifierRule->stDestIpAddress.ulIpv4Mask[ucLoopIndex]))
-		{
 			return TRUE;
-		}
 	}
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Destination Ip Address Not Matched");
 	return false;
@@ -99,9 +100,8 @@ static bool MatchTos(struct bcm_classifier_rule *pstClassifierRule, UCHAR ucType
 		return TRUE;
 
 	if (((pstClassifierRule->ucTosMask & ucTypeOfService) <= pstClassifierRule->ucTosHigh) && ((pstClassifierRule->ucTosMask & ucTypeOfService) >= pstClassifierRule->ucTosLow))
-	{
 		return TRUE;
-	}
+
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Type Of Service Not Matched");
 	return false;
 }
@@ -123,13 +123,10 @@ bool MatchProtocol(struct bcm_classifier_rule *pstClassifierRule, UCHAR ucProtoc
 	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	if (0 == pstClassifierRule->ucProtocolLength)
 		return TRUE;
-	for (ucLoopIndex = 0; ucLoopIndex < pstClassifierRule->ucProtocolLength; ucLoopIndex++)
-	{
+	for (ucLoopIndex = 0; ucLoopIndex < pstClassifierRule->ucProtocolLength; ucLoopIndex++) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Protocol:0x%X Classification Protocol:0x%X", ucProtocol, pstClassifierRule->ucProtocol[ucLoopIndex]);
 		if (pstClassifierRule->ucProtocol[ucLoopIndex] == ucProtocol)
-		{
 			return TRUE;
-		}
 	}
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Protocol Not Matched");
 	return false;
@@ -155,13 +152,10 @@ bool MatchSrcPort(struct bcm_classifier_rule *pstClassifierRule, USHORT ushSrcPo
 
 	if (0 == pstClassifierRule->ucSrcPortRangeLength)
 		return TRUE;
-	for (ucLoopIndex = 0; ucLoopIndex < pstClassifierRule->ucSrcPortRangeLength; ucLoopIndex++)
-	{
+	for (ucLoopIndex = 0; ucLoopIndex < pstClassifierRule->ucSrcPortRangeLength; ucLoopIndex++) {
 		if (ushSrcPort <= pstClassifierRule->usSrcPortRangeHi[ucLoopIndex] &&
 			ushSrcPort >= pstClassifierRule->usSrcPortRangeLo[ucLoopIndex])
-		{
 			return TRUE;
-		}
 	}
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Src Port: %x Not Matched ", ushSrcPort);
 	return false;
@@ -186,15 +180,12 @@ bool MatchDestPort(struct bcm_classifier_rule *pstClassifierRule, USHORT ushDest
 	if (0 == pstClassifierRule->ucDestPortRangeLength)
 		return TRUE;
 
-	for (ucLoopIndex = 0; ucLoopIndex < pstClassifierRule->ucDestPortRangeLength; ucLoopIndex++)
-	{
+	for (ucLoopIndex = 0; ucLoopIndex < pstClassifierRule->ucDestPortRangeLength; ucLoopIndex++) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Matching Port:0x%X   0x%X  0x%X", ushDestPort, pstClassifierRule->usDestPortRangeLo[ucLoopIndex], pstClassifierRule->usDestPortRangeHi[ucLoopIndex]);
 
 		if (ushDestPort <= pstClassifierRule->usDestPortRangeHi[ucLoopIndex] &&
 			ushDestPort >= pstClassifierRule->usDestPortRangeLo[ucLoopIndex])
-		{
 			return TRUE;
-		}
 	}
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Dest Port: %x Not Matched", ushDestPort);
 	return false;
@@ -273,21 +264,13 @@ static USHORT	IpVersion4(struct bcm_mini_adapter *Adapter,
 		bClassificationSucceed = TRUE;
 	} while (0);
 
-	if (TRUE == bClassificationSucceed)
-	{
+	if (TRUE == bClassificationSucceed) {
 		INT iMatchedSFQueueIndex = 0;
 		iMatchedSFQueueIndex = SearchSfid(Adapter, pstClassifierRule->ulSFID);
 		if (iMatchedSFQueueIndex >= NO_OF_QUEUES)
-		{
 			bClassificationSucceed = false;
-		}
-		else
-		{
-			if (false == Adapter->PackInfo[iMatchedSFQueueIndex].bActive)
-			{
-				bClassificationSucceed = false;
-			}
-		}
+		else if (false == Adapter->PackInfo[iMatchedSFQueueIndex].bActive)
+			bClassificationSucceed = false;
 	}
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "IpVersion4 <==========");
@@ -299,8 +282,7 @@ VOID PruneQueueAllSF(struct bcm_mini_adapter *Adapter)
 {
 	UINT iIndex = 0;
 
-	for (iIndex = 0; iIndex < HiPriority; iIndex++)
-	{
+	for (iIndex = 0; iIndex < HiPriority; iIndex++) {
 		if (!Adapter->PackInfo[iIndex].bValid)
 			continue;
 
@@ -334,10 +316,10 @@ static VOID PruneQueue(struct bcm_mini_adapter *Adapter, INT iIndex)
 
 	spin_lock_bh(&Adapter->PackInfo[iIndex].SFQueueLock);
 
-	while (1)
+	while (1) {
 //	while((UINT)Adapter->PackInfo[iIndex].uiCurrentPacketsOnHost >
-//		SF_MAX_ALLOWED_PACKETS_TO_BACKUP)
-	{
+//		SF_MAX_ALLOWED_PACKETS_TO_BACKUP) {
+
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, PRUNE_QUEUE, DBG_LVL_ALL, "uiCurrentBytesOnHost:%x uiMaxBucketSize :%x",
 		Adapter->PackInfo[iIndex].uiCurrentBytesOnHost,
 		Adapter->PackInfo[iIndex].uiMaxBucketSize);
@@ -350,8 +332,7 @@ static VOID PruneQueue(struct bcm_mini_adapter *Adapter, INT iIndex)
 			((1000*(jiffies - *((B_UINT32 *)(PacketToDrop->cb)+SKB_CB_LATENCY_OFFSET))/HZ) <= Adapter->PackInfo[iIndex].uiMaxLatency))
 			break;
 
-		if (PacketToDrop)
-		{
+		if (PacketToDrop) {
 			if (netif_msg_tx_err(Adapter))
 				pr_info(PFX "%s: tx queue %d overlimit\n",
 					Adapter->dev->name, iIndex);
@@ -394,20 +375,16 @@ VOID flush_all_queues(struct bcm_mini_adapter *Adapter)
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "=====>");
 
 //	down(&Adapter->data_packet_queue_lock);
-	for (iQIndex = LowPriority; iQIndex < HiPriority; iQIndex++)
-	{
+	for (iQIndex = LowPriority; iQIndex < HiPriority; iQIndex++) {
 		struct net_device_stats *netstats = &Adapter->dev->stats;
 
 		spin_lock_bh(&Adapter->PackInfo[iQIndex].SFQueueLock);
-		while (Adapter->PackInfo[iQIndex].FirstTxQueue)
-		{
+		while (Adapter->PackInfo[iQIndex].FirstTxQueue) {
 			PacketToDrop = Adapter->PackInfo[iQIndex].FirstTxQueue;
-			if (PacketToDrop)
-			{
+			if (PacketToDrop) {
 				uiTotalPacketLength = PacketToDrop->len;
 				netstats->tx_dropped++;
-			}
-			else
+			} else
 				uiTotalPacketLength = 0;
 
 			DEQUEUEPACKET(Adapter->PackInfo[iQIndex].FirstTxQueue,
@@ -455,58 +432,42 @@ USHORT ClassifyPacket(struct bcm_mini_adapter *Adapter, struct sk_buff* skb)
 	*((UINT32*) (skb->cb) +SKB_CB_TCPACK_OFFSET) = 0;
 	EThCSGetPktInfo(Adapter, pvEThPayload, &stEthCsPktInfo);
 
-	switch (stEthCsPktInfo.eNwpktEthFrameType)
-	{
+	switch (stEthCsPktInfo.eNwpktEthFrameType) {
 		case eEth802LLCFrame:
-		{
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "ClassifyPacket : 802LLCFrame\n");
 			pIpHeader = pvEThPayload + sizeof(struct bcm_eth_llc_frame);
 			break;
-		}
-
 		case eEth802LLCSNAPFrame:
-		{
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "ClassifyPacket : 802LLC SNAP Frame\n");
 			pIpHeader = pvEThPayload + sizeof(struct bcm_eth_llc_snap_frame);
 			break;
-		}
 		case eEth802QVLANFrame:
-		{
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "ClassifyPacket : 802.1Q VLANFrame\n");
 			pIpHeader = pvEThPayload + sizeof(struct bcm_eth_q_frame);
 			break;
-		}
 		case eEthOtherFrame:
-		{
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "ClassifyPacket : ETH Other Frame\n");
 			pIpHeader = pvEThPayload + sizeof(struct bcm_ethernet2_frame);
 			break;
-		}
 		default:
-		{
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "ClassifyPacket : Unrecognized ETH Frame\n");
 			pIpHeader = pvEThPayload + sizeof(struct bcm_ethernet2_frame);
 			break;
-		}
 	}
 
-	if (stEthCsPktInfo.eNwpktIPFrameType == eIPv4Packet)
-	{
+	if (stEthCsPktInfo.eNwpktIPFrameType == eIPv4Packet) {
 		usCurrFragment = (ntohs(pIpHeader->frag_off) & IP_OFFSET);
 		if ((ntohs(pIpHeader->frag_off) & IP_MF) || usCurrFragment)
 			bFragmentedPkt = TRUE;
 
-		if (bFragmentedPkt)
-		{
+		if (bFragmentedPkt) {
 				//Fragmented  Packet. Get Frag Classifier Entry.
 			pstClassifierRule = GetFragIPClsEntry(Adapter, pIpHeader->id, pIpHeader->saddr);
-			if (pstClassifierRule)
-			{
+			if (pstClassifierRule) {
 					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "It is next Fragmented pkt");
 					bClassificationSucceed = TRUE;
 			}
-			if (!(ntohs(pIpHeader->frag_off) & IP_MF))
-			{
+			if (!(ntohs(pIpHeader->frag_off) & IP_MF)) {
 				//Fragmented Last packet . Remove Frag Classifier Entry
 				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "This is the last fragmented Pkt");
 				DelFragIPClsEntry(Adapter, pIpHeader->id, pIpHeader->saddr);
@@ -514,23 +475,19 @@ USHORT ClassifyPacket(struct bcm_mini_adapter *Adapter, struct sk_buff* skb)
 		}
 	}
 
-	for (uiLoopIndex = MAX_CLASSIFIERS - 1; uiLoopIndex >= 0; uiLoopIndex--)
-	{
+	for (uiLoopIndex = MAX_CLASSIFIERS - 1; uiLoopIndex >= 0; uiLoopIndex--) {
 		if (bClassificationSucceed)
 			break;
 		//Iterate through all classifiers which are already in order of priority
 		//to classify the packet until match found
-		do
-		{
-			if (false == Adapter->astClassifierTable[uiLoopIndex].bUsed)
-			{
+		do {
+			if (false == Adapter->astClassifierTable[uiLoopIndex].bUsed) {
 				bClassificationSucceed = false;
 				break;
 			}
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "Adapter->PackInfo[%d].bvalid=True\n", uiLoopIndex);
 
-			if (0 == Adapter->astClassifierTable[uiLoopIndex].ucDirection)
-			{
+			if (0 == Adapter->astClassifierTable[uiLoopIndex].ucDirection) {
 				bClassificationSucceed = false;//cannot be processed for classification.
 				break;						// it is a down link connection
 			}
@@ -543,11 +500,9 @@ USHORT ClassifyPacket(struct bcm_mini_adapter *Adapter, struct sk_buff* skb)
 				break;
 			}
 
-			if (Adapter->PackInfo[uiSfIndex].bEthCSSupport)
-			{
+			if (Adapter->PackInfo[uiSfIndex].bEthCSSupport) {
 
-				if (eEthUnsupportedFrame == stEthCsPktInfo.eNwpktEthFrameType)
-				{
+				if (eEthUnsupportedFrame == stEthCsPktInfo.eNwpktEthFrameType) {
 					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, " ClassifyPacket : Packet Not a Valid Supported Ethernet Frame\n");
 					bClassificationSucceed = false;
 					break;
@@ -558,17 +513,12 @@ USHORT ClassifyPacket(struct bcm_mini_adapter *Adapter, struct sk_buff* skb)
 				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "Performing ETH CS Classification on Classifier Rule ID : %x Service Flow ID : %lx\n", pstClassifierRule->uiClassifierRuleIndex, Adapter->PackInfo[uiSfIndex].ulSFID);
 				bClassificationSucceed = EThCSClassifyPkt(Adapter, skb, &stEthCsPktInfo, pstClassifierRule, Adapter->PackInfo[uiSfIndex].bEthCSSupport);
 
-				if (!bClassificationSucceed)
-				{
+				if (!bClassificationSucceed) {
 					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "ClassifyPacket : Ethernet CS Classification Failed\n");
 					break;
 				}
-			}
-
-			else // No ETH Supported on this SF
-			{
-				if (eEthOtherFrame != stEthCsPktInfo.eNwpktEthFrameType)
-				{
+			} else {	// No ETH Supported on this SF
+				if (eEthOtherFrame != stEthCsPktInfo.eNwpktEthFrameType) {
 					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, " ClassifyPacket : Packet Not a 802.3 Ethernet Frame... hence not allowed over non-ETH CS SF\n");
 					bClassificationSucceed = false;
 					break;
@@ -577,11 +527,9 @@ USHORT ClassifyPacket(struct bcm_mini_adapter *Adapter, struct sk_buff* skb)
 
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "Proceeding to IP CS Clasification");
 
-			if (Adapter->PackInfo[uiSfIndex].bIPCSSupport)
-			{
+			if (Adapter->PackInfo[uiSfIndex].bIPCSSupport) {
 
-				if (stEthCsPktInfo.eNwpktIPFrameType == eNonIPPacket)
-				{
+				if (stEthCsPktInfo.eNwpktIPFrameType == eNonIPPacket) {
 					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, " ClassifyPacket : Packet is Not an IP Packet\n");
 					bClassificationSucceed = false;
 					break;
@@ -598,31 +546,26 @@ USHORT ClassifyPacket(struct bcm_mini_adapter *Adapter, struct sk_buff* skb)
 		} while (0);
 	}
 
-	if (bClassificationSucceed == TRUE)
-	{
+	if (bClassificationSucceed == TRUE) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "CF id : %d, SF ID is =%lu", pstClassifierRule->uiClassifierRuleIndex, pstClassifierRule->ulSFID);
 
 		//Store The matched Classifier in SKB
 		*((UINT32*)(skb->cb)+SKB_CB_CLASSIFICATION_OFFSET) = pstClassifierRule->uiClassifierRuleIndex;
-		if ((TCP == pIpHeader->protocol) && !bFragmentedPkt && (ETH_AND_IP_HEADER_LEN + TCP_HEADER_LEN <= skb->len))
-		{
+		if ((TCP == pIpHeader->protocol) && !bFragmentedPkt && (ETH_AND_IP_HEADER_LEN + TCP_HEADER_LEN <= skb->len)) {
 			 IpHeaderLength   = pIpHeader->ihl;
 			 pTcpHeader = (struct bcm_tcp_header *)(((PUCHAR)pIpHeader)+(IpHeaderLength*4));
 			 TcpHeaderLength  = GET_TCP_HEADER_LEN(pTcpHeader->HeaderLength);
 
 			if ((pTcpHeader->ucFlags & TCP_ACK) &&
 			   (ntohs(pIpHeader->tot_len) == (IpHeaderLength*4)+(TcpHeaderLength*4)))
-			{
 				*((UINT32*) (skb->cb) + SKB_CB_TCPACK_OFFSET) = TCP_ACK;
-			}
 		}
 
 		usIndex = SearchSfid(Adapter, pstClassifierRule->ulSFID);
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "index is	=%d", usIndex);
 
 		//If this is the first fragment of a Fragmented pkt, add this CF. Only This CF should be used for all other fragment of this Pkt.
-		if (bFragmentedPkt && (usCurrFragment == 0))
-		{
+		if (bFragmentedPkt && (usCurrFragment == 0)) {
 			//First Fragment of Fragmented Packet. Create Frag CLS Entry
 			struct bcm_fragmented_packet_info stFragPktInfo;
 			stFragPktInfo.bUsed = TRUE;
@@ -648,9 +591,8 @@ static bool EthCSMatchSrcMACAddress(struct bcm_classifier_rule *pstClassifierRul
 	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	if (pstClassifierRule->ucEthCSSrcMACLen == 0)
 		return TRUE;
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s\n", __FUNCTION__);
-	for (i = 0; i < MAC_ADDRESS_SIZE; i++)
-	{
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s\n", __func__);
+	for (i = 0; i < MAC_ADDRESS_SIZE; i++) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "SRC MAC[%x] = %x ClassifierRuleSrcMAC = %x Mask : %x\n", i, Mac[i], pstClassifierRule->au8EThCSSrcMAC[i], pstClassifierRule->au8EThCSSrcMACMask[i]);
 		if ((pstClassifierRule->au8EThCSSrcMAC[i] & pstClassifierRule->au8EThCSSrcMACMask[i]) !=
 			(Mac[i] & pstClassifierRule->au8EThCSSrcMACMask[i]))
@@ -665,9 +607,8 @@ static bool EthCSMatchDestMACAddress(struct bcm_classifier_rule *pstClassifierRu
 	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	if (pstClassifierRule->ucEthCSDestMACLen == 0)
 		return TRUE;
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "%s\n", __FUNCTION__);
-	for (i = 0; i < MAC_ADDRESS_SIZE; i++)
-	{
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "%s\n", __func__);
+	for (i = 0; i < MAC_ADDRESS_SIZE; i++) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "SRC MAC[%x] = %x ClassifierRuleSrcMAC = %x Mask : %x\n", i, Mac[i], pstClassifierRule->au8EThCSDestMAC[i], pstClassifierRule->au8EThCSDestMACMask[i]);
 		if ((pstClassifierRule->au8EThCSDestMAC[i] & pstClassifierRule->au8EThCSDestMACMask[i]) !=
 			(Mac[i] & pstClassifierRule->au8EThCSDestMACMask[i]))
@@ -683,10 +624,9 @@ static bool EthCSMatchEThTypeSAP(struct bcm_classifier_rule *pstClassifierRule,
 		(pstClassifierRule->au8EthCSEtherType[0] == 0))
 		return TRUE;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s SrcEtherType:%x CLS EtherType[0]:%x\n", __FUNCTION__, pstEthCsPktInfo->usEtherType, pstClassifierRule->au8EthCSEtherType[0]);
-	if (pstClassifierRule->au8EthCSEtherType[0] == 1)
-	{
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s  CLS EtherType[1]:%x EtherType[2]:%x\n", __FUNCTION__, pstClassifierRule->au8EthCSEtherType[1], pstClassifierRule->au8EthCSEtherType[2]);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s SrcEtherType:%x CLS EtherType[0]:%x\n", __func__, pstEthCsPktInfo->usEtherType, pstClassifierRule->au8EthCSEtherType[0]);
+	if (pstClassifierRule->au8EthCSEtherType[0] == 1) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s  CLS EtherType[1]:%x EtherType[2]:%x\n", __func__, pstClassifierRule->au8EthCSEtherType[1], pstClassifierRule->au8EthCSEtherType[2]);
 
 		if (memcmp(&pstEthCsPktInfo->usEtherType, &pstClassifierRule->au8EthCSEtherType[1], 2) == 0)
 			return TRUE;
@@ -694,12 +634,11 @@ static bool EthCSMatchEThTypeSAP(struct bcm_classifier_rule *pstClassifierRule,
 			return false;
 	}
 
-	if (pstClassifierRule->au8EthCSEtherType[0] == 2)
-	{
+	if (pstClassifierRule->au8EthCSEtherType[0] == 2) {
 		if (eEth802LLCFrame != pstEthCsPktInfo->eNwpktEthFrameType)
 			return false;
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s  EthCS DSAP:%x EtherType[2]:%x\n", __FUNCTION__, pstEthCsPktInfo->ucDSAP, pstClassifierRule->au8EthCSEtherType[2]);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s  EthCS DSAP:%x EtherType[2]:%x\n", __func__, pstEthCsPktInfo->ucDSAP, pstClassifierRule->au8EthCSEtherType[2]);
 		if (pstEthCsPktInfo->ucDSAP == pstClassifierRule->au8EthCSEtherType[2])
 			return TRUE;
 		else
@@ -718,11 +657,10 @@ static bool EthCSMatchVLANRules(struct bcm_classifier_rule *pstClassifierRule, s
 	B_UINT8 uPriority = 0;
 	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s  CLS UserPrio:%x CLS VLANID:%x\n", __FUNCTION__, ntohs(*((USHORT *)pstClassifierRule->usUserPriority)), pstClassifierRule->usVLANID);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s  CLS UserPrio:%x CLS VLANID:%x\n", __func__, ntohs(*((USHORT *)pstClassifierRule->usUserPriority)), pstClassifierRule->usVLANID);
 
 	/* In case FW didn't receive the TLV, the priority field should be ignored */
-	if (pstClassifierRule->usValidityBitMap & (1<<PKT_CLASSIFICATION_USER_PRIORITY_VALID))
-	{
+	if (pstClassifierRule->usValidityBitMap & (1<<PKT_CLASSIFICATION_USER_PRIORITY_VALID)) {
 		if (pstEthCsPktInfo->eNwpktEthFrameType != eEth802QVLANFrame)
 				return false;
 
@@ -739,14 +677,13 @@ static bool EthCSMatchVLANRules(struct bcm_classifier_rule *pstClassifierRule, s
 
 	bClassificationSucceed = false;
 
-	if (pstClassifierRule->usValidityBitMap & (1<<PKT_CLASSIFICATION_VLANID_VALID))
-	{
+	if (pstClassifierRule->usValidityBitMap & (1<<PKT_CLASSIFICATION_VLANID_VALID)) {
 		if (pstEthCsPktInfo->eNwpktEthFrameType != eEth802QVLANFrame)
 				return false;
 
 		usVLANID = ntohs(*(USHORT *)(skb->data + sizeof(struct bcm_eth_header))) & 0xFFF;
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s  Pkt VLANID %x Priority: %d\n", __FUNCTION__, usVLANID, uPriority);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s  Pkt VLANID %x Priority: %d\n", __func__, usVLANID, uPriority);
 
 		if (usVLANID == ((pstClassifierRule->usVLANID & 0xFFF0) >> 4))
 			bClassificationSucceed = TRUE;
@@ -800,32 +737,24 @@ static void EThCSGetPktInfo(struct bcm_mini_adapter *Adapter, PVOID pvEthPayload
 	USHORT u16Etype = ntohs(((struct bcm_eth_header *)pvEthPayload)->u16Etype);
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "EthCSGetPktInfo : Eth Hdr Type : %X\n", u16Etype);
-	if (u16Etype > 0x5dc)
-	{
+	if (u16Etype > 0x5dc) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "EthCSGetPktInfo : ETH2 Frame\n");
 		//ETH2 Frame
-		if (u16Etype == ETHERNET_FRAMETYPE_802QVLAN)
-		{
+		if (u16Etype == ETHERNET_FRAMETYPE_802QVLAN) {
 			//802.1Q VLAN Header
 			pstEthCsPktInfo->eNwpktEthFrameType = eEth802QVLANFrame;
 			u16Etype = ((struct bcm_eth_q_frame *)pvEthPayload)->EthType;
 			//((ETH_CS_802_Q_FRAME*)pvEthPayload)->UserPriority
-		}
-		else
-		{
+		} else {
 			pstEthCsPktInfo->eNwpktEthFrameType = eEthOtherFrame;
 			u16Etype = ntohs(u16Etype);
 		}
-
-	}
-	else
-	{
+	} else {
 		//802.2 LLC
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "802.2 LLC Frame\n");
 		pstEthCsPktInfo->eNwpktEthFrameType = eEth802LLCFrame;
 		pstEthCsPktInfo->ucDSAP = ((struct bcm_eth_llc_frame *)pvEthPayload)->DSAP;
-		if (pstEthCsPktInfo->ucDSAP == 0xAA && ((struct bcm_eth_llc_frame *)pvEthPayload)->SSAP == 0xAA)
-		{
+		if (pstEthCsPktInfo->ucDSAP == 0xAA && ((struct bcm_eth_llc_frame *)pvEthPayload)->SSAP == 0xAA) {
 			//SNAP Frame
 			pstEthCsPktInfo->eNwpktEthFrameType = eEth802LLCSNAPFrame;
 			u16Etype = ((struct bcm_eth_llc_snap_frame *)pvEthPayload)->usEtherType;
diff --git a/drivers/staging/bcm/Transmit.c b/drivers/staging/bcm/Transmit.c
index 2ed4836b965..49767468ac2 100644
--- a/drivers/staging/bcm/Transmit.c
+++ b/drivers/staging/bcm/Transmit.c
@@ -46,12 +46,14 @@ int SendControlPacket(struct bcm_mini_adapter *Adapter, char *pControlPacket)
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Tx");
 	if (!pControlPacket || !Adapter) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Got NULL Control Packet or Adapter");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL,
+				"Got NULL Control Packet or Adapter");
 		return STATUS_FAILURE;
 	}
 	if ((atomic_read(&Adapter->CurrNumFreeTxDesc) <
 			((PLeader->PLength-1)/MAX_DEVICE_DESC_SIZE)+1))	{
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "NO FREE DESCRIPTORS TO SEND CONTROL PACKET");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL,
+				"NO FREE DESCRIPTORS TO SEND CONTROL PACKET");
 		return STATUS_FAILURE;
 	}
 
@@ -109,7 +111,8 @@ int SetupNextSend(struct bcm_mini_adapter *Adapter,  struct sk_buff *Packet, USH
 			(UINT *)&Packet->len, Adapter->PackInfo[QueueIndex].bEthCSSupport);
 
 	if (status != STATUS_SUCCESS) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, NEXT_SEND, DBG_LVL_ALL, "PHS Transmit failed..\n");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, NEXT_SEND, DBG_LVL_ALL,
+				"PHS Transmit failed..\n");
 		goto errExit;
 	}
 
@@ -217,12 +220,15 @@ int tx_pkt_handler(struct bcm_mini_adapter *Adapter /**< pointer to adapter obje
 			Adapter->LinkStatus == SYNC_UP_REQUEST &&
 			!Adapter->bSyncUpRequestSent) {
 
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL, "Calling LinkMessage");
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS,
+					DBG_LVL_ALL, "Calling LinkMessage");
 			LinkMessage(Adapter);
 		}
 
 		if ((Adapter->IdleMode || Adapter->bShutStatus) && atomic_read(&Adapter->TotalPacketCount)) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL, "Device in Low Power mode...waking up");
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX,
+					TX_PACKETS, DBG_LVL_ALL,
+					"Device in Low Power mode...waking up");
 			Adapter->usIdleModePattern = ABORT_IDLE_MODE;
 			Adapter->bWakeUpDevice = TRUE;
 			wake_up(&Adapter->process_rx_cntrlpkt);
@@ -232,7 +238,8 @@ int tx_pkt_handler(struct bcm_mini_adapter *Adapter /**< pointer to adapter obje
 		atomic_set(&Adapter->TxPktAvail, 0);
 	}
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL, "Exiting the tx thread..\n");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL,
+			"Exiting the tx thread..\n");
 	Adapter->transmit_packet_thread = NULL;
 	return 0;
 }
diff --git a/drivers/staging/bcm/hostmibs.c b/drivers/staging/bcm/hostmibs.c
index 39ace5510c4..42d9004e357 100644
--- a/drivers/staging/bcm/hostmibs.c
+++ b/drivers/staging/bcm/hostmibs.c
@@ -9,37 +9,40 @@
 
 #include "headers.h"
 
-INT ProcessGetHostMibs(struct bcm_mini_adapter *Adapter, struct bcm_host_stats_mibs *pstHostMibs)
+INT ProcessGetHostMibs(struct bcm_mini_adapter *Adapter,
+		       struct bcm_host_stats_mibs *pstHostMibs)
 {
 	struct bcm_phs_entry *pstServiceFlowEntry = NULL;
 	struct bcm_phs_rule *pstPhsRule = NULL;
 	struct bcm_phs_classifier_table *pstClassifierTable = NULL;
 	struct bcm_phs_classifier_entry *pstClassifierRule = NULL;
-	struct bcm_phs_extension *pDeviceExtension = (struct bcm_phs_extension *) &Adapter->stBCMPhsContext;
-
-	UINT nClassifierIndex = 0, nPhsTableIndex = 0, nSfIndex = 0, uiIndex = 0;
+	struct bcm_phs_extension *pDeviceExtension = &Adapter->stBCMPhsContext;
+	UINT nClassifierIndex = 0;
+	UINT nPhsTableIndex = 0;
+	UINT nSfIndex = 0;
+	UINT uiIndex = 0;
 
 	if (pDeviceExtension == NULL) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, HOST_MIBS, DBG_LVL_ALL, "Invalid Device Extension\n");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, HOST_MIBS,
+				DBG_LVL_ALL, "Invalid Device Extension\n");
 		return STATUS_FAILURE;
 	}
 
 	/* Copy the classifier Table */
-	for (nClassifierIndex = 0; nClassifierIndex < MAX_CLASSIFIERS; nClassifierIndex++) {
+	for (nClassifierIndex = 0; nClassifierIndex < MAX_CLASSIFIERS;
+							nClassifierIndex++) {
 		if (Adapter->astClassifierTable[nClassifierIndex].bUsed == TRUE)
-			memcpy((PVOID) &pstHostMibs->
-			       astClassifierTable[nClassifierIndex],
-			       (PVOID) &Adapter->
-			       astClassifierTable[nClassifierIndex],
+			memcpy(&pstHostMibs->astClassifierTable[nClassifierIndex],
+			       &Adapter->astClassifierTable[nClassifierIndex],
 			       sizeof(struct bcm_mibs_classifier_rule));
 	}
 
 	/* Copy the SF Table */
 	for (nSfIndex = 0; nSfIndex < NO_OF_QUEUES; nSfIndex++) {
 		if (Adapter->PackInfo[nSfIndex].bValid) {
-			memcpy((PVOID) &pstHostMibs->astSFtable[nSfIndex],
-			       (PVOID) &Adapter->PackInfo[nSfIndex],
-				sizeof(struct bcm_mibs_table));
+			memcpy(&pstHostMibs->astSFtable[nSfIndex],
+			       &Adapter->PackInfo[nSfIndex],
+			       sizeof(struct bcm_mibs_table));
 		} else {
 			/* If index in not valid,
 			 * don't process this for the PHS table.
@@ -68,9 +71,9 @@ INT ProcessGetHostMibs(struct bcm_mini_adapter *Adapter, struct bcm_host_stats_m
 				pstHostMibs->astPhsRulesTable[nPhsTableIndex].
 				    ulSFID = Adapter->PackInfo[nSfIndex].ulSFID;
 
-				memcpy(&pstHostMibs->
-				       astPhsRulesTable[nPhsTableIndex].u8PHSI,
-				       &pstPhsRule->u8PHSI, sizeof(struct bcm_phs_rule));
+				memcpy(&pstHostMibs->astPhsRulesTable[nPhsTableIndex].u8PHSI,
+				       &pstPhsRule->u8PHSI,
+				       sizeof(struct bcm_phs_rule));
 				nPhsTableIndex++;
 
 			}
@@ -82,26 +85,32 @@ INT ProcessGetHostMibs(struct bcm_mini_adapter *Adapter, struct bcm_host_stats_m
 	/* Copy other Host Statistics parameters */
 	pstHostMibs->stHostInfo.GoodTransmits = Adapter->dev->stats.tx_packets;
 	pstHostMibs->stHostInfo.GoodReceives = Adapter->dev->stats.rx_packets;
-	pstHostMibs->stHostInfo.CurrNumFreeDesc = atomic_read(&Adapter->CurrNumFreeTxDesc);
+	pstHostMibs->stHostInfo.CurrNumFreeDesc =
+				atomic_read(&Adapter->CurrNumFreeTxDesc);
 	pstHostMibs->stHostInfo.BEBucketSize = Adapter->BEBucketSize;
 	pstHostMibs->stHostInfo.rtPSBucketSize = Adapter->rtPSBucketSize;
 	pstHostMibs->stHostInfo.TimerActive = Adapter->TimerActive;
 	pstHostMibs->stHostInfo.u32TotalDSD = Adapter->u32TotalDSD;
 
-	memcpy(pstHostMibs->stHostInfo.aTxPktSizeHist, Adapter->aTxPktSizeHist, sizeof(UINT32) * MIBS_MAX_HIST_ENTRIES);
-	memcpy(pstHostMibs->stHostInfo.aRxPktSizeHist, Adapter->aRxPktSizeHist, sizeof(UINT32) * MIBS_MAX_HIST_ENTRIES);
+	memcpy(pstHostMibs->stHostInfo.aTxPktSizeHist, Adapter->aTxPktSizeHist,
+	       sizeof(UINT32) * MIBS_MAX_HIST_ENTRIES);
+	memcpy(pstHostMibs->stHostInfo.aRxPktSizeHist, Adapter->aRxPktSizeHist,
+	       sizeof(UINT32) * MIBS_MAX_HIST_ENTRIES);
 
 	return STATUS_SUCCESS;
 }
 
-VOID GetDroppedAppCntrlPktMibs(struct bcm_host_stats_mibs *pstHostMibs, struct bcm_tarang_data *pTarang)
+VOID GetDroppedAppCntrlPktMibs(struct bcm_host_stats_mibs *pstHostMibs,
+			       struct bcm_tarang_data *pTarang)
 {
 	memcpy(&(pstHostMibs->stDroppedAppCntrlMsgs),
 	       &(pTarang->stDroppedAppCntrlMsgs),
 	       sizeof(struct bcm_mibs_dropped_cntrl_msg));
 }
 
-VOID CopyMIBSExtendedSFParameters(struct bcm_mini_adapter *Adapter, struct bcm_connect_mgr_params *psfLocalSet, UINT uiSearchRuleIndex)
+VOID CopyMIBSExtendedSFParameters(struct bcm_mini_adapter *Adapter,
+				  struct bcm_connect_mgr_params *psfLocalSet,
+				  UINT uiSearchRuleIndex)
 {
 	struct bcm_mibs_parameters *t = &Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable;
 
diff --git a/drivers/staging/comedi/Kconfig b/drivers/staging/comedi/Kconfig
index 703c5d40ae5..5d56428d508 100644
--- a/drivers/staging/comedi/Kconfig
+++ b/drivers/staging/comedi/Kconfig
@@ -160,16 +160,18 @@ config COMEDI_PCL730
 	  Enable support for various simple ISA or PC/104 Digital I/O boards.
 	  These boards all use 8-bit I/O ports.
 
-	  Advantech PCL-730   isolated - 16 in/16 out  ttl - 16 in/16 out
-	  ICP ISO-730         isolated - 16 in/16 out  ttl - 16 in/16 out
-	  ADlink ACL-7130     isolated - 16 in/16 out  ttl - 16 in/16 out
-	  Advantech PCM-3730  isolated - 8 in/8 out    ttl - 16 in/16 out
-	  Advantech PCL-725   isolated - 8 in/8 out
-	  ICP P8R8-DIO        isolated - 8 in/8 out
-	  ADlink ACL-7225b    isolated - 16 in/16 out
-	  ICP P16R16-DIO      isolated - 16 in/16 out
-	  Advantech PCL-733   isolated - 32 in
-	  Advantech PCL-734   isolated - 32 out
+	  Advantech PCL-730             iso - 16 in/16 out  ttl - 16 in/16 out
+	  ICP ISO-730                   iso - 16 in/16 out  ttl - 16 in/16 out
+	  ADlink ACL-7130               iso - 16 in/16 out  ttl - 16 in/16 out
+	  Advantech PCM-3730            iso - 8 in/8 out    ttl - 16 in/16 out
+	  Advantech PCL-725             iso - 8 in/8 out
+	  ICP P8R8-DIO                  iso - 8 in/8 out
+	  ADlink ACL-7225b              iso - 16 in/16 out
+	  ICP P16R16-DIO                iso - 16 in/16 out
+	  Advantech PCL-733             iso - 32 in
+	  Advantech PCL-734             iso - 32 out
+	  Diamond Systems OPMM-1616-XT  iso - 16 in/16 out
+	  Diamond Systems PEARL-MM-P    iso - 16 out
 
 	  To compile this driver as a module, choose M here: the module will be
 	  called pcl730.
diff --git a/drivers/staging/comedi/TODO b/drivers/staging/comedi/TODO
index fa8da9aada3..b68fbdb5eeb 100644
--- a/drivers/staging/comedi/TODO
+++ b/drivers/staging/comedi/TODO
@@ -3,7 +3,6 @@ TODO:
 	- Lindent
 	- remove all wrappers
 	- audit userspace interface
-	- reserve major number
 	- cleanup the individual comedi drivers as well
 
 Please send patches to Greg Kroah-Hartman <greg@kroah.com> and
diff --git a/drivers/staging/comedi/comedi_buf.c b/drivers/staging/comedi/comedi_buf.c
index 25759501616..df4a9c4bca3 100644
--- a/drivers/staging/comedi/comedi_buf.c
+++ b/drivers/staging/comedi/comedi_buf.c
@@ -183,9 +183,9 @@ comedi_buf_map_from_subdev_get(struct comedi_subdevice *s)
 	return bm;
 }
 
-bool comedi_buf_is_mmapped(struct comedi_async *async)
+bool comedi_buf_is_mmapped(struct comedi_subdevice *s)
 {
-	struct comedi_buf_map *bm = async->buf_map;
+	struct comedi_buf_map *bm = s->async->buf_map;
 
 	return bm && (atomic_read(&bm->refcount.refcount) > 1);
 }
@@ -222,8 +222,10 @@ int comedi_buf_alloc(struct comedi_device *dev, struct comedi_subdevice *s,
 	return 0;
 }
 
-void comedi_buf_reset(struct comedi_async *async)
+void comedi_buf_reset(struct comedi_subdevice *s)
 {
+	struct comedi_async *async = s->async;
+
 	async->buf_write_alloc_count = 0;
 	async->buf_write_count = 0;
 	async->buf_read_alloc_count = 0;
@@ -241,18 +243,20 @@ void comedi_buf_reset(struct comedi_async *async)
 	async->events = 0;
 }
 
-static unsigned int comedi_buf_write_n_available(struct comedi_async *async)
+static unsigned int comedi_buf_write_n_available(struct comedi_subdevice *s)
 {
+	struct comedi_async *async = s->async;
 	unsigned int free_end = async->buf_read_count + async->prealloc_bufsz;
 
 	return free_end - async->buf_write_alloc_count;
 }
 
-static unsigned int __comedi_buf_write_alloc(struct comedi_async *async,
+static unsigned int __comedi_buf_write_alloc(struct comedi_subdevice *s,
 					     unsigned int nbytes,
 					     int strict)
 {
-	unsigned int available = comedi_buf_write_n_available(async);
+	struct comedi_async *async = s->async;
+	unsigned int available = comedi_buf_write_n_available(s);
 
 	if (nbytes > available)
 		nbytes = strict ? 0 : available;
@@ -269,10 +273,10 @@ static unsigned int __comedi_buf_write_alloc(struct comedi_async *async,
 }
 
 /* allocates chunk for the writer from free buffer space */
-unsigned int comedi_buf_write_alloc(struct comedi_async *async,
+unsigned int comedi_buf_write_alloc(struct comedi_subdevice *s,
 				    unsigned int nbytes)
 {
-	return __comedi_buf_write_alloc(async, nbytes, 0);
+	return __comedi_buf_write_alloc(s, nbytes, 0);
 }
 EXPORT_SYMBOL_GPL(comedi_buf_write_alloc);
 
@@ -280,10 +284,10 @@ EXPORT_SYMBOL_GPL(comedi_buf_write_alloc);
  * munging is applied to data by core as it passes between user
  * and kernel space
  */
-static unsigned int comedi_buf_munge(struct comedi_async *async,
+static unsigned int comedi_buf_munge(struct comedi_subdevice *s,
 				     unsigned int num_bytes)
 {
-	struct comedi_subdevice *s = async->subdevice;
+	struct comedi_async *async = s->async;
 	unsigned int count = 0;
 	const unsigned num_sample_bytes = bytes_per_sample(s);
 
@@ -323,23 +327,26 @@ static unsigned int comedi_buf_munge(struct comedi_async *async,
 	return count;
 }
 
-unsigned int comedi_buf_write_n_allocated(struct comedi_async *async)
+unsigned int comedi_buf_write_n_allocated(struct comedi_subdevice *s)
 {
+	struct comedi_async *async = s->async;
+
 	return async->buf_write_alloc_count - async->buf_write_count;
 }
 
 /* transfers a chunk from writer to filled buffer space */
-unsigned int comedi_buf_write_free(struct comedi_async *async,
+unsigned int comedi_buf_write_free(struct comedi_subdevice *s,
 				   unsigned int nbytes)
 {
-	unsigned int allocated = comedi_buf_write_n_allocated(async);
+	struct comedi_async *async = s->async;
+	unsigned int allocated = comedi_buf_write_n_allocated(s);
 
 	if (nbytes > allocated)
 		nbytes = allocated;
 
 	async->buf_write_count += nbytes;
 	async->buf_write_ptr += nbytes;
-	comedi_buf_munge(async, async->buf_write_count - async->munge_count);
+	comedi_buf_munge(s, async->buf_write_count - async->munge_count);
 	if (async->buf_write_ptr >= async->prealloc_bufsz)
 		async->buf_write_ptr %= async->prealloc_bufsz;
 
@@ -347,8 +354,9 @@ unsigned int comedi_buf_write_free(struct comedi_async *async,
 }
 EXPORT_SYMBOL_GPL(comedi_buf_write_free);
 
-unsigned int comedi_buf_read_n_available(struct comedi_async *async)
+unsigned int comedi_buf_read_n_available(struct comedi_subdevice *s)
 {
+	struct comedi_async *async = s->async;
 	unsigned num_bytes;
 
 	if (!async)
@@ -367,9 +375,10 @@ unsigned int comedi_buf_read_n_available(struct comedi_async *async)
 EXPORT_SYMBOL_GPL(comedi_buf_read_n_available);
 
 /* allocates a chunk for the reader from filled (and munged) buffer space */
-unsigned int comedi_buf_read_alloc(struct comedi_async *async,
+unsigned int comedi_buf_read_alloc(struct comedi_subdevice *s,
 				   unsigned int nbytes)
 {
+	struct comedi_async *async = s->async;
 	unsigned int available;
 
 	available = async->munge_count - async->buf_read_alloc_count;
@@ -394,9 +403,10 @@ static unsigned int comedi_buf_read_n_allocated(struct comedi_async *async)
 }
 
 /* transfers control of a chunk from reader to free buffer space */
-unsigned int comedi_buf_read_free(struct comedi_async *async,
+unsigned int comedi_buf_read_free(struct comedi_subdevice *s,
 				  unsigned int nbytes)
 {
+	struct comedi_async *async = s->async;
 	unsigned int allocated;
 
 	/*
@@ -416,36 +426,39 @@ unsigned int comedi_buf_read_free(struct comedi_async *async,
 }
 EXPORT_SYMBOL_GPL(comedi_buf_read_free);
 
-int comedi_buf_put(struct comedi_async *async, unsigned short x)
+int comedi_buf_put(struct comedi_subdevice *s, unsigned short x)
 {
-	unsigned int n = __comedi_buf_write_alloc(async, sizeof(short), 1);
+	struct comedi_async *async = s->async;
+	unsigned int n = __comedi_buf_write_alloc(s, sizeof(short), 1);
 
 	if (n < sizeof(short)) {
 		async->events |= COMEDI_CB_ERROR;
 		return 0;
 	}
 	*(unsigned short *)(async->prealloc_buf + async->buf_write_ptr) = x;
-	comedi_buf_write_free(async, sizeof(short));
+	comedi_buf_write_free(s, sizeof(short));
 	return 1;
 }
 EXPORT_SYMBOL_GPL(comedi_buf_put);
 
-int comedi_buf_get(struct comedi_async *async, unsigned short *x)
+int comedi_buf_get(struct comedi_subdevice *s, unsigned short *x)
 {
-	unsigned int n = comedi_buf_read_n_available(async);
+	struct comedi_async *async = s->async;
+	unsigned int n = comedi_buf_read_n_available(s);
 
 	if (n < sizeof(short))
 		return 0;
-	comedi_buf_read_alloc(async, sizeof(short));
+	comedi_buf_read_alloc(s, sizeof(short));
 	*x = *(unsigned short *)(async->prealloc_buf + async->buf_read_ptr);
-	comedi_buf_read_free(async, sizeof(short));
+	comedi_buf_read_free(s, sizeof(short));
 	return 1;
 }
 EXPORT_SYMBOL_GPL(comedi_buf_get);
 
-void comedi_buf_memcpy_to(struct comedi_async *async, unsigned int offset,
+void comedi_buf_memcpy_to(struct comedi_subdevice *s, unsigned int offset,
 			  const void *data, unsigned int num_bytes)
 {
+	struct comedi_async *async = s->async;
 	unsigned int write_ptr = async->buf_write_ptr + offset;
 
 	if (write_ptr >= async->prealloc_bufsz)
@@ -469,10 +482,11 @@ void comedi_buf_memcpy_to(struct comedi_async *async, unsigned int offset,
 }
 EXPORT_SYMBOL_GPL(comedi_buf_memcpy_to);
 
-void comedi_buf_memcpy_from(struct comedi_async *async, unsigned int offset,
+void comedi_buf_memcpy_from(struct comedi_subdevice *s, unsigned int offset,
 			    void *dest, unsigned int nbytes)
 {
 	void *src;
+	struct comedi_async *async = s->async;
 	unsigned int read_ptr = async->buf_read_ptr + offset;
 
 	if (read_ptr >= async->prealloc_bufsz)
diff --git a/drivers/staging/comedi/comedi_fops.c b/drivers/staging/comedi/comedi_fops.c
index acc80197e35..9d99fb3c18a 100644
--- a/drivers/staging/comedi/comedi_fops.c
+++ b/drivers/staging/comedi/comedi_fops.c
@@ -238,9 +238,9 @@ comedi_write_subdevice(const struct comedi_device *dev, unsigned int minor)
 }
 
 static int resize_async_buffer(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_async *async, unsigned new_size)
+			       struct comedi_subdevice *s, unsigned new_size)
 {
+	struct comedi_async *async = s->async;
 	int retval;
 
 	if (new_size > async->max_bufsize)
@@ -251,7 +251,7 @@ static int resize_async_buffer(struct comedi_device *dev,
 			"subdevice is busy, cannot resize buffer\n");
 		return -EBUSY;
 	}
-	if (comedi_buf_is_mmapped(async)) {
+	if (comedi_buf_is_mmapped(s)) {
 		dev_dbg(dev->class_dev,
 			"subdevice is mmapped, cannot resize buffer\n");
 		return -EBUSY;
@@ -380,7 +380,7 @@ static ssize_t read_buffer_kb_store(struct device *csdev,
 	mutex_lock(&dev->mutex);
 	s = comedi_read_subdevice(dev, minor);
 	if (s && (s->subdev_flags & SDF_CMD_READ) && s->async)
-		err = resize_async_buffer(dev, s, s->async, size);
+		err = resize_async_buffer(dev, s, size);
 	else
 		err = -EINVAL;
 	mutex_unlock(&dev->mutex);
@@ -493,7 +493,7 @@ static ssize_t write_buffer_kb_store(struct device *csdev,
 	mutex_lock(&dev->mutex);
 	s = comedi_write_subdevice(dev, minor);
 	if (s && (s->subdev_flags & SDF_CMD_WRITE) && s->async)
-		err = resize_async_buffer(dev, s, s->async, size);
+		err = resize_async_buffer(dev, s, size);
 	else
 		err = -EINVAL;
 	mutex_unlock(&dev->mutex);
@@ -583,7 +583,7 @@ static void do_become_nonbusy(struct comedi_device *dev,
 
 	comedi_set_subdevice_runflags(s, SRF_RUNNING, 0);
 	if (async) {
-		comedi_buf_reset(async);
+		comedi_buf_reset(s);
 		async->inttrig = NULL;
 		kfree(async->cmd.chanlist);
 		async->cmd.chanlist = NULL;
@@ -635,7 +635,7 @@ static int is_device_busy(struct comedi_device *dev)
 		s = &dev->subdevices[i];
 		if (s->busy)
 			return 1;
-		if (s->async && comedi_buf_is_mmapped(s->async))
+		if (s->async && comedi_buf_is_mmapped(s))
 			return 1;
 	}
 
@@ -668,6 +668,7 @@ static int do_devconfig_ioctl(struct comedi_device *dev,
 			return -EBUSY;
 		if (dev->attached) {
 			struct module *driver_module = dev->driver->module;
+
 			comedi_device_detach(dev);
 			module_put(driver_module);
 		}
@@ -740,7 +741,7 @@ static int do_bufconfig_ioctl(struct comedi_device *dev,
 	}
 
 	if (bc.size) {
-		retval = resize_async_buffer(dev, s, async, bc.size);
+		retval = resize_async_buffer(dev, s, bc.size);
 		if (retval < 0)
 			return retval;
 	}
@@ -992,8 +993,8 @@ static int do_bufinfo_ioctl(struct comedi_device *dev,
 		return -EACCES;
 
 	if (bi.bytes_read && (s->subdev_flags & SDF_CMD_READ)) {
-		bi.bytes_read = comedi_buf_read_alloc(async, bi.bytes_read);
-		comedi_buf_read_free(async, bi.bytes_read);
+		bi.bytes_read = comedi_buf_read_alloc(s, bi.bytes_read);
+		comedi_buf_read_free(s, bi.bytes_read);
 
 		if (comedi_is_subdevice_idle(s) &&
 		    async->buf_write_count == async->buf_read_count) {
@@ -1003,8 +1004,8 @@ static int do_bufinfo_ioctl(struct comedi_device *dev,
 
 	if (bi.bytes_written && (s->subdev_flags & SDF_CMD_WRITE)) {
 		bi.bytes_written =
-		    comedi_buf_write_alloc(async, bi.bytes_written);
-		comedi_buf_write_free(async, bi.bytes_written);
+		    comedi_buf_write_alloc(s, bi.bytes_written);
+		comedi_buf_write_free(s, bi.bytes_written);
 	}
 
 copyback_position:
@@ -1435,13 +1436,15 @@ static int __comedi_get_user_cmd(struct comedi_device *dev,
 	s = &dev->subdevices[cmd->subdev];
 
 	if (s->type == COMEDI_SUBD_UNUSED) {
-		dev_dbg(dev->class_dev, "%d not valid subdevice\n", cmd->subdev);
+		dev_dbg(dev->class_dev, "%d not valid subdevice\n",
+			cmd->subdev);
 		return -EIO;
 	}
 
 	if (!s->do_cmd || !s->do_cmdtest || !s->async) {
 		dev_dbg(dev->class_dev,
-			"subdevice %d does not support commands\n", cmd->subdev);
+			"subdevice %d does not support commands\n",
+			cmd->subdev);
 		return -EIO;
 	}
 
@@ -1554,7 +1557,7 @@ static int do_cmd_ioctl(struct comedi_device *dev,
 		goto cleanup;
 	}
 
-	comedi_buf_reset(async);
+	comedi_buf_reset(s);
 
 	async->cb_mask =
 	    COMEDI_CB_EOA | COMEDI_CB_BLOCK | COMEDI_CB_ERROR |
@@ -1597,7 +1600,6 @@ static int do_cmdtest_ioctl(struct comedi_device *dev,
 {
 	struct comedi_cmd cmd;
 	struct comedi_subdevice *s;
-	unsigned int *chanlist = NULL;
 	unsigned int __user *user_chanlist;
 	int ret;
 
@@ -1626,8 +1628,6 @@ static int do_cmdtest_ioctl(struct comedi_device *dev,
 		ret = -EFAULT;
 	}
 
-	kfree(chanlist);
-
 	return ret;
 }
 
@@ -2029,18 +2029,18 @@ static unsigned int comedi_poll(struct file *file, poll_table *wait)
 	if (s && s->async) {
 		poll_wait(file, &s->async->wait_head, wait);
 		if (!s->busy || !comedi_is_subdevice_running(s) ||
-		    comedi_buf_read_n_available(s->async) > 0)
+		    comedi_buf_read_n_available(s) > 0)
 			mask |= POLLIN | POLLRDNORM;
 	}
 
 	s = comedi_write_subdevice(dev, minor);
 	if (s && s->async) {
-		unsigned int bps = bytes_per_sample(s->async->subdevice);
+		unsigned int bps = bytes_per_sample(s);
 
 		poll_wait(file, &s->async->wait_head, wait);
-		comedi_buf_write_alloc(s->async, s->async->prealloc_bufsz);
+		comedi_buf_write_alloc(s, s->async->prealloc_bufsz);
 		if (!s->busy || !comedi_is_subdevice_running(s) ||
-		    comedi_buf_write_n_allocated(s->async) >= bps)
+		    comedi_buf_write_n_allocated(s) >= bps)
 			mask |= POLLOUT | POLLWRNORM;
 	}
 
@@ -2136,9 +2136,9 @@ static ssize_t comedi_write(struct file *file, const char __user *buf,
 		m = n;
 		if (async->buf_write_ptr + m > async->prealloc_bufsz)
 			m = async->prealloc_bufsz - async->buf_write_ptr;
-		comedi_buf_write_alloc(async, async->prealloc_bufsz);
-		if (m > comedi_buf_write_n_allocated(async))
-			m = comedi_buf_write_n_allocated(async);
+		comedi_buf_write_alloc(s, async->prealloc_bufsz);
+		if (m > comedi_buf_write_n_allocated(s))
+			m = comedi_buf_write_n_allocated(s);
 		if (m < n)
 			n = m;
 
@@ -2167,7 +2167,7 @@ static ssize_t comedi_write(struct file *file, const char __user *buf,
 			n -= m;
 			retval = -EFAULT;
 		}
-		comedi_buf_write_free(async, n);
+		comedi_buf_write_free(s, n);
 
 		count += n;
 		nbytes -= n;
@@ -2229,7 +2229,7 @@ static ssize_t comedi_read(struct file *file, char __user *buf, size_t nbytes,
 
 		n = nbytes;
 
-		m = comedi_buf_read_n_available(async);
+		m = comedi_buf_read_n_available(s);
 		/* printk("%d available\n",m); */
 		if (async->buf_read_ptr + m > async->prealloc_bufsz)
 			m = async->prealloc_bufsz - async->buf_read_ptr;
@@ -2272,8 +2272,8 @@ static ssize_t comedi_read(struct file *file, char __user *buf, size_t nbytes,
 			retval = -EFAULT;
 		}
 
-		comedi_buf_read_alloc(async, n);
-		comedi_buf_read_free(async, n);
+		comedi_buf_read_alloc(s, n);
+		comedi_buf_read_free(s, n);
 
 		count += n;
 		nbytes -= n;
@@ -2327,46 +2327,12 @@ static int comedi_open(struct inode *inode, struct file *file)
 		return -ENODEV;
 	}
 
-	/* This is slightly hacky, but we want module autoloading
-	 * to work for root.
-	 * case: user opens device, attached -> ok
-	 * case: user opens device, unattached, !in_request_module -> autoload
-	 * case: user opens device, unattached, in_request_module -> fail
-	 * case: root opens device, attached -> ok
-	 * case: root opens device, unattached, in_request_module -> ok
-	 *   (typically called from modprobe)
-	 * case: root opens device, unattached, !in_request_module -> autoload
-	 *
-	 * The last could be changed to "-> ok", which would deny root
-	 * autoloading.
-	 */
-	mutex_lock(&dev->mutex);
-	if (dev->attached)
-		goto ok;
-	if (!capable(CAP_NET_ADMIN) && dev->in_request_module) {
-		dev_dbg(dev->class_dev, "in request module\n");
-		rc = -ENODEV;
-		goto out;
-	}
-	if (capable(CAP_NET_ADMIN) && dev->in_request_module)
-		goto ok;
-
-	dev->in_request_module = true;
-
-#ifdef CONFIG_KMOD
-	mutex_unlock(&dev->mutex);
-	request_module("char-major-%i-%i", COMEDI_MAJOR, dev->minor);
 	mutex_lock(&dev->mutex);
-#endif
-
-	dev->in_request_module = false;
-
 	if (!dev->attached && !capable(CAP_NET_ADMIN)) {
 		dev_dbg(dev->class_dev, "not attached and not CAP_NET_ADMIN\n");
 		rc = -ENODEV;
 		goto out;
 	}
-ok:
 	if (dev->attached && dev->use_count == 0) {
 		if (!try_module_get(dev->driver->module)) {
 			rc = -ENOSYS;
@@ -2654,6 +2620,7 @@ static int __init comedi_init(void)
 	/* create devices files for legacy/manual use */
 	for (i = 0; i < comedi_num_legacy_minors; i++) {
 		struct comedi_device *dev;
+
 		dev = comedi_alloc_board_minor(NULL);
 		if (IS_ERR(dev)) {
 			comedi_cleanup_board_minors();
diff --git a/drivers/staging/comedi/comedi_internal.h b/drivers/staging/comedi/comedi_internal.h
index a492f2d2436..e978c223f5b 100644
--- a/drivers/staging/comedi/comedi_internal.h
+++ b/drivers/staging/comedi/comedi_internal.h
@@ -15,13 +15,13 @@ void comedi_free_subdevice_minor(struct comedi_subdevice *s);
 
 int comedi_buf_alloc(struct comedi_device *dev, struct comedi_subdevice *s,
 		     unsigned long new_size);
-void comedi_buf_reset(struct comedi_async *async);
-bool comedi_buf_is_mmapped(struct comedi_async *async);
+void comedi_buf_reset(struct comedi_subdevice *s);
+bool comedi_buf_is_mmapped(struct comedi_subdevice *s);
 void comedi_buf_map_get(struct comedi_buf_map *bm);
 int comedi_buf_map_put(struct comedi_buf_map *bm);
 struct comedi_buf_map *comedi_buf_map_from_subdev_get(
 		struct comedi_subdevice *s);
-unsigned int comedi_buf_write_n_allocated(struct comedi_async *async);
+unsigned int comedi_buf_write_n_allocated(struct comedi_subdevice *s);
 void comedi_device_cancel_all(struct comedi_device *dev);
 
 extern unsigned int comedi_default_buf_size_kb;
diff --git a/drivers/staging/comedi/comedidev.h b/drivers/staging/comedi/comedidev.h
index d46123a9758..8f4e44bfbe0 100644
--- a/drivers/staging/comedi/comedidev.h
+++ b/drivers/staging/comedi/comedidev.h
@@ -108,8 +108,6 @@ struct comedi_buf_map {
 };
 
 struct comedi_async {
-	struct comedi_subdevice *subdevice;
-
 	void *prealloc_buf;	/* pre-allocated buffer */
 	unsigned int prealloc_bufsz;	/* buffer size, in bytes */
 	struct comedi_buf_map *buf_map;	/* map of buffer pages */
@@ -182,7 +180,6 @@ struct comedi_device {
 	const char *board_name;
 	const void *board_ptr;
 	bool attached:1;
-	bool in_request_module:1;
 	bool ioenabled:1;
 	spinlock_t spinlock;
 	struct mutex mutex;
@@ -336,19 +333,19 @@ static inline unsigned int bytes_per_sample(const struct comedi_subdevice *subd)
  */
 int comedi_set_hw_dev(struct comedi_device *dev, struct device *hw_dev);
 
-unsigned int comedi_buf_write_alloc(struct comedi_async *, unsigned int);
-unsigned int comedi_buf_write_free(struct comedi_async *, unsigned int);
+unsigned int comedi_buf_write_alloc(struct comedi_subdevice *s, unsigned int n);
+unsigned int comedi_buf_write_free(struct comedi_subdevice *s, unsigned int n);
 
-unsigned int comedi_buf_read_n_available(struct comedi_async *);
-unsigned int comedi_buf_read_alloc(struct comedi_async *, unsigned int);
-unsigned int comedi_buf_read_free(struct comedi_async *, unsigned int);
+unsigned int comedi_buf_read_n_available(struct comedi_subdevice *s);
+unsigned int comedi_buf_read_alloc(struct comedi_subdevice *s, unsigned int n);
+unsigned int comedi_buf_read_free(struct comedi_subdevice *s, unsigned int n);
 
-int comedi_buf_put(struct comedi_async *, unsigned short);
-int comedi_buf_get(struct comedi_async *, unsigned short *);
+int comedi_buf_put(struct comedi_subdevice *s, unsigned short x);
+int comedi_buf_get(struct comedi_subdevice *s, unsigned short *x);
 
-void comedi_buf_memcpy_to(struct comedi_async *async, unsigned int offset,
+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_async *async, unsigned int offset,
+void comedi_buf_memcpy_from(struct comedi_subdevice *s, unsigned int offset,
 			    void *destination, unsigned int num_bytes);
 
 /* drivers.c - general comedi driver functions */
diff --git a/drivers/staging/comedi/drivers.c b/drivers/staging/comedi/drivers.c
index ab0e8ed4729..299726f39e2 100644
--- a/drivers/staging/comedi/drivers.c
+++ b/drivers/staging/comedi/drivers.c
@@ -258,6 +258,7 @@ static int insn_rw_emulate_bits(struct comedi_device *dev,
 	const unsigned base_bitfield_channel =
 	    (chan < channels_per_bitfield) ? 0 : chan;
 	unsigned int new_data[2];
+
 	memset(new_data, 0, sizeof(new_data));
 	memset(&new_insn, 0, sizeof(new_insn));
 	new_insn.insn = INSN_BITS;
@@ -306,7 +307,6 @@ static int __comedi_device_postconfig_async(struct comedi_device *dev,
 		return -ENOMEM;
 
 	init_waitqueue_head(&async->wait_head);
-	async->subdevice = s;
 	s->async = async;
 
 	async->max_bufsize = comedi_default_buf_maxsize_kb * 1024;
diff --git a/drivers/staging/comedi/drivers/8253.h b/drivers/staging/comedi/drivers/8253.h
index e3d737cf730..5829b46b757 100644
--- a/drivers/staging/comedi/drivers/8253.h
+++ b/drivers/staging/comedi/drivers/8253.h
@@ -30,106 +30,11 @@
 #define I8254_OSC_BASE_2MHZ		500
 #define I8254_OSC_BASE_1MHZ		1000
 
-#define i8253_cascade_ns_to_timer i8253_cascade_ns_to_timer_2div
-
-static inline void i8253_cascade_ns_to_timer_2div_old(int i8253_osc_base,
-						      unsigned int *d1,
-						      unsigned int *d2,
-						      unsigned int *nanosec,
-						      int round_mode)
-{
-	int divider;
-	int div1, div2;
-	int div1_glb, div2_glb, ns_glb;
-	int div1_lub, div2_lub, ns_lub;
-	int ns;
-
-	divider = (*nanosec + i8253_osc_base / 2) / i8253_osc_base;
-
-	/* find 2 integers 1<={x,y}<=65536 such that x*y is
-	   close to divider */
-
-	div1_lub = div2_lub = 0;
-	div1_glb = div2_glb = 0;
-
-	ns_glb = 0;
-	ns_lub = 0xffffffff;
-
-	div2 = 0x10000;
-	for (div1 = divider / 65536 + 1; div1 < div2; div1++) {
-		div2 = divider / div1;
-
-		ns = i8253_osc_base * div1 * div2;
-		if (ns <= *nanosec && ns > ns_glb) {
-			ns_glb = ns;
-			div1_glb = div1;
-			div2_glb = div2;
-		}
-
-		div2++;
-		if (div2 <= 65536) {
-			ns = i8253_osc_base * div1 * div2;
-			if (ns > *nanosec && ns < ns_lub) {
-				ns_lub = ns;
-				div1_lub = div1;
-				div2_lub = div2;
-			}
-		}
-	}
-
-	*nanosec = div1_lub * div2_lub * i8253_osc_base;
-	*d1 = div1_lub & 0xffff;
-	*d2 = div2_lub & 0xffff;
-	return;
-}
-
-static inline void i8253_cascade_ns_to_timer_power(int i8253_osc_base,
-						   unsigned int *d1,
-						   unsigned int *d2,
-						   unsigned int *nanosec,
-						   int round_mode)
-{
-	int div1, div2;
-	int base;
-
-	for (div1 = 2; div1 <= (1 << 16); div1 <<= 1) {
-		base = i8253_osc_base * div1;
-		round_mode &= TRIG_ROUND_MASK;
-		switch (round_mode) {
-		case TRIG_ROUND_NEAREST:
-		default:
-			div2 = (*nanosec + base / 2) / base;
-			break;
-		case TRIG_ROUND_DOWN:
-			div2 = (*nanosec) / base;
-			break;
-		case TRIG_ROUND_UP:
-			div2 = (*nanosec + base - 1) / base;
-			break;
-		}
-		if (div2 < 2)
-			div2 = 2;
-		if (div2 <= 65536) {
-			*nanosec = div2 * base;
-			*d1 = div1 & 0xffff;
-			*d2 = div2 & 0xffff;
-			return;
-		}
-	}
-
-	/* shouldn't get here */
-	div1 = 0x10000;
-	div2 = 0x10000;
-	*nanosec = div1 * div2 * i8253_osc_base;
-	*d1 = div1 & 0xffff;
-	*d2 = div2 & 0xffff;
-}
-
-static inline void i8253_cascade_ns_to_timer_2div(int i8253_osc_base,
-						  unsigned int *d1,
-						  unsigned int *d2,
-						  unsigned int *nanosec,
-						  int round_mode)
+static inline void i8253_cascade_ns_to_timer(int i8253_osc_base,
+					     unsigned int *d1,
+					     unsigned int *d2,
+					     unsigned int *nanosec,
+					     int round_mode)
 {
 	unsigned int divider;
 	unsigned int div1, div2;
@@ -386,7 +291,7 @@ static inline int i8254_set_mode(unsigned long base_address,
 
 	if (counter_number > 2)
 		return -1;
-	if (mode > (I8254_MODE5 | I8254_BINARY))
+	if (mode > (I8254_MODE5 | I8254_BCD))
 		return -1;
 
 	byte = counter_number << 6;
@@ -406,7 +311,7 @@ static inline int i8254_mm_set_mode(void __iomem *base_address,
 
 	if (counter_number > 2)
 		return -1;
-	if (mode > (I8254_MODE5 | I8254_BINARY))
+	if (mode > (I8254_MODE5 | I8254_BCD))
 		return -1;
 
 	byte = counter_number << 6;
diff --git a/drivers/staging/comedi/drivers/8255.c b/drivers/staging/comedi/drivers/8255.c
index 48817f087d9..46113a37413 100644
--- a/drivers/staging/comedi/drivers/8255.c
+++ b/drivers/staging/comedi/drivers/8255.c
@@ -117,7 +117,7 @@ void subdev_8255_interrupt(struct comedi_device *dev,
 	d = spriv->io(0, _8255_DATA, 0, iobase);
 	d |= (spriv->io(0, _8255_DATA + 1, 0, iobase) << 8);
 
-	comedi_buf_put(s->async, d);
+	comedi_buf_put(s, d);
 	s->async->events |= COMEDI_CB_EOS;
 
 	comedi_event(dev, s);
@@ -231,7 +231,7 @@ static int subdev_8255_cmdtest(struct comedi_device *dev,
 	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, 1);
+	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)
@@ -298,6 +298,7 @@ int subdev_8255_init_irq(struct comedi_device *dev, struct comedi_subdevice *s,
 	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;
diff --git a/drivers/staging/comedi/drivers/8255.h b/drivers/staging/comedi/drivers/8255.h
index 4f16ea78f86..795d232a6c0 100644
--- a/drivers/staging/comedi/drivers/8255.h
+++ b/drivers/staging/comedi/drivers/8255.h
@@ -22,10 +22,10 @@
 #include "../comedidev.h"
 
 int subdev_8255_init(struct comedi_device *dev, struct comedi_subdevice *s,
-		     int (*io) (int, int, int, unsigned long),
+		     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),
+			 int (*io)(int, int, int, unsigned long),
 			 unsigned long iobase);
 void subdev_8255_interrupt(struct comedi_device *dev,
 			   struct comedi_subdevice *s);
diff --git a/drivers/staging/comedi/drivers/addi-data/addi_common.h b/drivers/staging/comedi/drivers/addi-data/addi_common.h
index 2ed2da3499f..5c6a11c35de 100644
--- a/drivers/staging/comedi/drivers/addi-data/addi_common.h
+++ b/drivers/staging/comedi/drivers/addi-data/addi_common.h
@@ -118,18 +118,10 @@ struct addi_private {
 	int i_IobaseAmcc;	/*  base+size for AMCC chip */
 	int i_IobaseAddon;	/* addon base address */
 	int i_IobaseReserved;
-	unsigned char b_AiContinuous;	/*  we do unlimited AI */
 	unsigned int ui_AiActualScan;	/* how many scans we finished */
 	unsigned int ui_AiNbrofChannels;	/*  how many channels is measured */
-	unsigned int ui_AiScanLength;	/*  Length of actual scanlist */
-	unsigned int *pui_AiChannelList;	/*  actual chanlist */
 	unsigned int ui_AiChannelList[32];	/*  actual chanlist */
 	unsigned int ui_AiReadData[32];
-	unsigned int ui_AiTimer0;	/* Timer Constant for Timer0 */
-	unsigned int ui_AiTimer1;	/* Timer constant for Timer1 */
-	unsigned int ui_AiFlags;
-	unsigned int ui_AiDataLength;
-	unsigned int ui_AiNbrofScans;	/*  number of scans to do */
 	unsigned short us_UseDma;	/*  To use Dma or not */
 	unsigned char b_DmaDoubleBuffer;	/*  we can use double buffering */
 	unsigned int ui_DmaActualBuffer;	/*  which buffer is used now */
@@ -145,7 +137,7 @@ struct addi_private {
 	unsigned short us_OutputRegister;	/*  Contain data written at iobase + 0 */
 	unsigned char b_Timer2Mode;	/*  Specify the timer 2 mode */
 	unsigned char b_Timer2Interrupt;	/* Timer2  interrupt enable or disable */
-	unsigned char b_AiCyclicAcquisition;	/*  indicate cyclic acquisition */
+	unsigned int ai_running:1;
 	unsigned char b_InterruptMode;	/*  eoc eos or dma */
 	unsigned char b_EocEosInterrupt;	/*  Enable disable eoc eos interrupt */
 	unsigned int ui_EocEosConversionTime;
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1564.c b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1564.c
index 9c86b02eb6d..0ba5385226a 100644
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1564.c
+++ b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1564.c
@@ -21,6 +21,8 @@
  *
  */
 
+#include "../addi_watchdog.h"
+
 #define APCI1564_ADDRESS_RANGE				128
 
 /* Digital Input IRQ Function Selection */
@@ -76,7 +78,7 @@
 #define APCI1564_TIMER_WARN_TIMEBASE_REG		0x64
 
 /*
- * devpriv->iobase Register Map
+ * dev>iobase Register Map
  */
 #define APCI1564_TCW_REG(x)				(0x00 + ((x) * 0x20))
 #define APCI1564_TCW_RELOAD_REG(x)			(0x04 + ((x) * 0x20))
@@ -127,18 +129,6 @@ static int apci1564_di_config(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int apci1564_di_insn_bits(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn,
-				 unsigned int *data)
-{
-	struct addi_private *devpriv = dev->private;
-
-	data[1] = inl(devpriv->i_IobaseAmcc + APCI1564_DI_REG);
-
-	return insn->n;
-}
-
 /*
  * Configures The Digital Output Subdevice.
  *
@@ -180,23 +170,6 @@ static int apci1564_do_config(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int apci1564_do_insn_bits(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn,
-				 unsigned int *data)
-{
-	struct addi_private *devpriv = dev->private;
-
-	s->state = inl(devpriv->i_IobaseAmcc + APCI1564_DO_REG);
-
-	if (comedi_dio_update_state(s, data))
-		outl(s->state, devpriv->i_IobaseAmcc + APCI1564_DO_REG);
-
-	data[1] = s->state;
-
-	return insn->n;
-}
-
 /*
  * Configures The Timer, Counter or Watchdog
  *
@@ -239,13 +212,13 @@ static int apci1564_timer_config(struct comedi_device *dev,
 			outl(0x0, devpriv->i_IobaseAmcc + APCI1564_DO_IRQ_REG);
 			outl(0x0, devpriv->i_IobaseAmcc + APCI1564_WDOG_IRQ_REG);
 			outl(0x0,
-				devpriv->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER1));
+			     dev->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER1));
 			outl(0x0,
-				devpriv->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER2));
+			     dev->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER2));
 			outl(0x0,
-				devpriv->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER3));
+			     dev->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER3));
 			outl(0x0,
-				devpriv->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER4));
+			     dev->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER4));
 		} else {
 			/* disable Timer interrupt */
 			outl(0x0, devpriv->i_IobaseAmcc + APCI1564_TIMER_CTRL_REG);
@@ -266,13 +239,13 @@ static int apci1564_timer_config(struct comedi_device *dev,
 		devpriv->b_ModeSelectRegister = data[5];
 
 		/* First Stop The Counter */
-		ul_Command1 = inl(devpriv->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
+		ul_Command1 = inl(dev->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
 		ul_Command1 = ul_Command1 & 0xFFFFF9FEUL;
 		/* Stop The Timer */
-		outl(ul_Command1, devpriv->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
+		outl(ul_Command1, dev->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
 
 		/* Set the reload value */
-		outl(data[3], devpriv->iobase + APCI1564_TCW_RELOAD_REG(data[5] - 1));
+		outl(data[3], dev->iobase + APCI1564_TCW_RELOAD_REG(data[5] - 1));
 
 		/* Set the mode :             */
 		/* - Disable the hardware     */
@@ -285,15 +258,15 @@ 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, devpriv->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
+		outl(ul_Command1, dev->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
 
 		/*  Enable or Disable Interrupt */
 		ul_Command1 = (ul_Command1 & 0xFFFFF9FD) | (data[1] << 1);
-		outl(ul_Command1, devpriv->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
+		outl(ul_Command1, dev->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
 
 		/* Set the Up/Down selection */
 		ul_Command1 = (ul_Command1 & 0xFFFBF9FFUL) | (data[6] << 18);
-		outl(ul_Command1, devpriv->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
+		outl(ul_Command1, dev->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
 	} else {
 		dev_err(dev->class_dev, "Invalid subdevice.\n");
 	}
@@ -349,8 +322,8 @@ static int apci1564_timer_write(struct comedi_device *dev,
 	}
 	if (devpriv->b_TimerSelectMode == ADDIDATA_COUNTER) {
 		ul_Command1 =
-			inl(devpriv->iobase +
-				APCI1564_TCW_CTRL_REG(devpriv->b_ModeSelectRegister - 1));
+			inl(dev->iobase +
+			    APCI1564_TCW_CTRL_REG(devpriv->b_ModeSelectRegister - 1));
 		if (data[1] == 1) {
 			/* Start the Counter subdevice */
 			ul_Command1 = (ul_Command1 & 0xFFFFF9FFUL) | 0x1UL;
@@ -362,9 +335,8 @@ static int apci1564_timer_write(struct comedi_device *dev,
 			/*  Clears the Counter subdevice */
 			ul_Command1 = (ul_Command1 & 0xFFFFF9FFUL) | 0x400;
 		}
-		outl(ul_Command1,
-			devpriv->iobase +
-			APCI1564_TCW_CTRL_REG(devpriv->b_ModeSelectRegister - 1));
+		outl(ul_Command1, dev->iobase +
+		     APCI1564_TCW_CTRL_REG(devpriv->b_ModeSelectRegister - 1));
 	}
 	return insn->n;
 }
@@ -393,11 +365,11 @@ static int apci1564_timer_read(struct comedi_device *dev,
 	} else if (devpriv->b_TimerSelectMode == ADDIDATA_COUNTER) {
 		/*  Read the Counter Actual Value. */
 		data[0] =
-			inl(devpriv->iobase +
-				APCI1564_TCW_REG(devpriv->b_ModeSelectRegister - 1));
+			inl(dev->iobase +
+			    APCI1564_TCW_REG(devpriv->b_ModeSelectRegister - 1));
 		ul_Command1 =
-			inl(devpriv->iobase +
-				APCI1564_TCW_STATUS_REG(devpriv->b_ModeSelectRegister - 1));
+			inl(dev->iobase +
+			    APCI1564_TCW_STATUS_REG(devpriv->b_ModeSelectRegister - 1));
 
 		/* Get the software trigger status */
 		data[1] = (unsigned char) ((ul_Command1 >> 1) & 1);
@@ -446,13 +418,13 @@ static void apci1564_interrupt(int irq, void *d)
 	ui_DO = inl(devpriv->i_IobaseAmcc + APCI1564_DO_IRQ_REG) & 0x01;
 	ui_Timer = inl(devpriv->i_IobaseAmcc + APCI1564_TIMER_IRQ_REG) & 0x01;
 	ui_C1 =
-		inl(devpriv->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER1)) & 0x1;
+		inl(dev->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER1)) & 0x1;
 	ui_C2 =
-		inl(devpriv->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER2)) & 0x1;
+		inl(dev->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER2)) & 0x1;
 	ui_C3 =
-		inl(devpriv->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER3)) & 0x1;
+		inl(dev->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER3)) & 0x1;
 	ui_C4 =
-		inl(devpriv->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER4)) & 0x1;
+		inl(dev->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER4)) & 0x1;
 	if (ui_DI == 0 && ui_DO == 0 && ui_Timer == 0 && ui_C1 == 0
 		&& ui_C2 == 0 && ui_C3 == 0 && ui_C4 == 0) {
 		dev_err(dev->class_dev, "Interrupt from unknown source.\n");
@@ -506,16 +478,16 @@ static void apci1564_interrupt(int irq, void *d)
 
 			/*  Disable Counter Interrupt */
 			ul_Command2 =
-				inl(devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER1));
+				inl(dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER1));
 			outl(0x0,
-			     devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER1));
+			     dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER1));
 
 			/* Send a signal to from kernel to user space */
 			send_sig(SIGIO, devpriv->tsk_Current, 0);
 
 			/*  Enable Counter Interrupt */
 			outl(ul_Command2,
-			     devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER1));
+			     dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER1));
 		}
 	}
 
@@ -525,16 +497,16 @@ static void apci1564_interrupt(int irq, void *d)
 
 			/*  Disable Counter Interrupt */
 			ul_Command2 =
-				inl(devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER2));
+				inl(dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER2));
 			outl(0x0,
-			     devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER2));
+			     dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER2));
 
 			/* Send a signal to from kernel to user space */
 			send_sig(SIGIO, devpriv->tsk_Current, 0);
 
 			/*  Enable Counter Interrupt */
 			outl(ul_Command2,
-			     devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER2));
+			     dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER2));
 		}
 	}
 
@@ -544,16 +516,16 @@ static void apci1564_interrupt(int irq, void *d)
 
 			/*  Disable Counter Interrupt */
 			ul_Command2 =
-				inl(devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER3));
+				inl(dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER3));
 			outl(0x0,
-			     devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER3));
+			     dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER3));
 
 			/* Send a signal to from kernel to user space */
 			send_sig(SIGIO, devpriv->tsk_Current, 0);
 
 			/*  Enable Counter Interrupt */
 			outl(ul_Command2,
-			     devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER3));
+			     dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER3));
 		}
 	}
 
@@ -563,45 +535,17 @@ static void apci1564_interrupt(int irq, void *d)
 
 			/*  Disable Counter Interrupt */
 			ul_Command2 =
-				inl(devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER4));
+				inl(dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER4));
 			outl(0x0,
-			     devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER4));
+			     dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER4));
 
 			/* Send a signal to from kernel to user space */
 			send_sig(SIGIO, devpriv->tsk_Current, 0);
 
 			/*  Enable Counter Interrupt */
 			outl(ul_Command2,
-			     devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER4));
+			     dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER4));
 		}
 	}
 	return;
 }
-
-static int apci1564_reset(struct comedi_device *dev)
-{
-	struct addi_private *devpriv = dev->private;
-
-	/* disable the interrupts */
-	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_DI_IRQ_REG);
-	/* Reset the interrupt status register */
-	inl(devpriv->i_IobaseAmcc + APCI1564_DI_INT_STATUS_REG);
-	/* Disable the and/or interrupt */
-	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_DI_INT_MODE1_REG);
-	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_DI_INT_MODE2_REG);
-	devpriv->b_DigitalOutputRegister = 0;
-	ui_Type = 0;
-	/* Resets the output channels */
-	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_DO_REG);
-	/* Disables the interrupt. */
-	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_DO_INT_CTRL_REG);
-	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_WDOG_RELOAD_REG);
-	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_TIMER_REG);
-	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_TIMER_CTRL_REG);
-
-	outl(0x0, devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER1));
-	outl(0x0, devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER2));
-	outl(0x0, devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER3));
-	outl(0x0, devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER4));
-	return 0;
-}
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3120.c b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3120.c
index 70e8f426285..764c8f17f8f 100644
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3120.c
+++ b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3120.c
@@ -609,11 +609,10 @@ static int apci3120_reset(struct comedi_device *dev)
 	unsigned int i;
 	unsigned short us_TmpValue;
 
-	devpriv->b_AiCyclicAcquisition = APCI3120_DISABLE;
+	devpriv->ai_running = 0;
 	devpriv->b_EocEosInterrupt = APCI3120_DISABLE;
 	devpriv->b_InterruptMode = APCI3120_EOC_MODE;
 	devpriv->ui_EocEosConversionTime = 0;	/*  set eoc eos conv time to 0 */
-	devpriv->b_OutputMemoryStatus = 0;
 
 	/*  variables used in timer subdevice */
 	devpriv->b_Timer2Mode = 0;
@@ -720,10 +719,9 @@ static int apci3120_cancel(struct comedi_device *dev,
 	inw(dev->iobase + APCI3120_RD_STATUS);
 	devpriv->ui_AiActualScan = 0;
 	s->async->cur_chan = 0;
-	devpriv->b_AiContinuous = 0;
 	devpriv->ui_DmaActualBuffer = 0;
 
-	devpriv->b_AiCyclicAcquisition = APCI3120_DISABLE;
+	devpriv->ai_running = 0;
 	devpriv->b_InterruptMode = APCI3120_EOC_MODE;
 	devpriv->b_EocEosInterrupt = APCI3120_DISABLE;
 	apci3120_reset(dev);
@@ -734,7 +732,6 @@ static int apci3120_ai_cmdtest(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_cmd *cmd)
 {
-	const struct addi_board *this_board = comedi_board(dev);
 	int err = 0;
 
 	/* Step 1 : check if triggers are trivially valid */
@@ -767,20 +764,16 @@ static int apci3120_ai_cmdtest(struct comedi_device *dev,
 	if (cmd->scan_begin_src == TRIG_TIMER)	/* Test Delay timing */
 		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg, 100000);
 
-	if (cmd->convert_src == TRIG_TIMER) {	/*  Test Acquisition timing */
-		if (cmd->scan_begin_src == TRIG_TIMER) {
-			if (cmd->convert_arg)
-				err |= cfc_check_trigger_arg_min(
-						&cmd->convert_arg, 10000);
-		} else {
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		if (cmd->convert_arg)
 			err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
-							10000);
-		}
+							 10000);
+	} else {
+		err |= cfc_check_trigger_arg_min(&cmd->convert_arg, 10000);
 	}
 
 	err |= cfc_check_trigger_arg_min(&cmd->chanlist_len, 1);
-	err |= cfc_check_trigger_arg_max(&cmd->chanlist_len,
-					 this_board->i_AiChannelList);
+	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
 		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
@@ -792,15 +785,10 @@ static int apci3120_ai_cmdtest(struct comedi_device *dev,
 
 	/*  step 4: fix up any arguments */
 
-	if (cmd->convert_src == TRIG_TIMER) {
-
-		if (cmd->scan_begin_src == TRIG_TIMER &&
-			cmd->scan_begin_arg <
-			cmd->convert_arg * cmd->scan_end_arg) {
-			cmd->scan_begin_arg =
-				cmd->convert_arg * cmd->scan_end_arg;
-			err++;
-		}
+	if (cmd->scan_begin_src == TRIG_TIMER &&
+	    cmd->scan_begin_arg < cmd->convert_arg * cmd->scan_end_arg) {
+		cmd->scan_begin_arg = cmd->convert_arg * cmd->scan_end_arg;
+		err |= -EINVAL;
 	}
 
 	if (err)
@@ -821,16 +809,13 @@ static int apci3120_cyclic_ai(int mode,
 {
 	const struct addi_board *this_board = comedi_board(dev);
 	struct addi_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned char b_Tmp;
 	unsigned int ui_Tmp, ui_DelayTiming = 0, ui_TimerValue1 = 0, dmalen0 =
 		0, dmalen1 = 0, ui_TimerValue2 =
 		0, ui_TimerValue0, ui_ConvertTiming;
 	unsigned short us_TmpValue;
 
-	/* BEGIN JK 07.05.04: Comparison between WIN32 and Linux driver */
-	/* devpriv->b_AiCyclicAcquisition=APCI3120_ENABLE; */
-	/* END JK 07.05.04: Comparison between WIN32 and Linux driver */
-
 	/*******************/
 	/* Resets the FIFO */
 	/*******************/
@@ -840,12 +825,7 @@ static int apci3120_cyclic_ai(int mode,
 	/* inw(dev->iobase+APCI3120_RD_STATUS); */
 	/* END JK 07.05.04: Comparison between WIN32 and Linux driver */
 
-	/***************************/
-	/* Acquisition initialized */
-	/***************************/
-	/* BEGIN JK 07.05.04: Comparison between WIN32 and Linux driver */
-	devpriv->b_AiCyclicAcquisition = APCI3120_ENABLE;
-	/* END JK 07.05.04: Comparison between WIN32 and Linux driver */
+	devpriv->ai_running = 1;
 
 	/*  clear software  registers */
 	devpriv->b_TimerSelectMode = 0;
@@ -892,17 +872,17 @@ static int apci3120_cyclic_ai(int mode,
 	devpriv->ui_DmaActualBuffer = 0;
 
 	/*  value for timer2  minus -2 has to be done .....dunno y?? */
-	ui_TimerValue2 = devpriv->ui_AiNbrofScans - 2;
-	ui_ConvertTiming = devpriv->ui_AiTimer0;
+	ui_TimerValue2 = cmd->stop_arg - 2;
+	ui_ConvertTiming = cmd->convert_arg;
 
 	if (mode == 2)
-		ui_DelayTiming = devpriv->ui_AiTimer1;
+		ui_DelayTiming = cmd->scan_begin_arg;
 
    /**********************************/
 	/* Initializes the sequence array */
    /**********************************/
 	if (!apci3120_setup_chan_list(dev, s, devpriv->ui_AiNbrofChannels,
-			devpriv->pui_AiChannelList, 0))
+			cmd->chanlist, 0))
 		return -EINVAL;
 
 	us_TmpValue = (unsigned short) inw(dev->iobase + APCI3120_RD_STATUS);
@@ -1039,7 +1019,7 @@ static int apci3120_cyclic_ai(int mode,
 		outb(devpriv->b_ModeSelectRegister,
 			dev->iobase + APCI3120_WRITE_MODE_SELECT);
 
-		if (!devpriv->b_AiContinuous) {
+		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)
@@ -1107,6 +1087,7 @@ static int apci3120_cyclic_ai(int mode,
 		}
 	} else {
 		/* 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 */
@@ -1125,37 +1106,38 @@ static int apci3120_cyclic_ai(int mode,
 		dmalen0 = devpriv->ui_DmaBufferSize[0];
 		dmalen1 = devpriv->ui_DmaBufferSize[1];
 
-		if (!devpriv->b_AiContinuous) {
-
-			if (dmalen0 > (devpriv->ui_AiNbrofScans * devpriv->ui_AiScanLength * 2)) {	/*  must we fill full first buffer? */
-				dmalen0 =
-					devpriv->ui_AiNbrofScans *
-					devpriv->ui_AiScanLength * 2;
-			} else if (dmalen1 > (devpriv->ui_AiNbrofScans * devpriv->ui_AiScanLength * 2 - dmalen0))	/*  and must we fill full second buffer when first is once filled? */
-				dmalen1 =
-					devpriv->ui_AiNbrofScans *
-					devpriv->ui_AiScanLength * 2 - dmalen0;
+		if (cmd->stop_src == TRIG_COUNT) {
+			/*
+			 * Must we fill full first buffer? And must we fill
+			 * full second buffer when first is once filled?
+			 */
+			if (dmalen0 > (cmd->stop_arg * scan_bytes)) {
+				dmalen0 = cmd->stop_arg * scan_bytes;
+			} else if (dmalen1 > (cmd->stop_arg * scan_bytes -
+					      dmalen0))
+				dmalen1 = cmd->stop_arg * scan_bytes -
+					  dmalen0;
 		}
 
-		if (devpriv->ui_AiFlags & TRIG_WAKE_EOS) {
+		if (cmd->flags & TRIG_WAKE_EOS) {
 			/*  don't we want wake up every scan? */
-			if (dmalen0 > (devpriv->ui_AiScanLength * 2)) {
-				dmalen0 = devpriv->ui_AiScanLength * 2;
-				if (devpriv->ui_AiScanLength & 1)
+			if (dmalen0 > scan_bytes) {
+				dmalen0 = scan_bytes;
+				if (cmd->scan_end_arg & 1)
 					dmalen0 += 2;
 			}
-			if (dmalen1 > (devpriv->ui_AiScanLength * 2)) {
-				dmalen1 = devpriv->ui_AiScanLength * 2;
-				if (devpriv->ui_AiScanLength & 1)
+			if (dmalen1 > scan_bytes) {
+				dmalen1 = scan_bytes;
+				if (cmd->scan_end_arg & 1)
 					dmalen1 -= 2;
 				if (dmalen1 < 4)
 					dmalen1 = 4;
 			}
 		} else {	/*  isn't output buff smaller that our DMA buff? */
-			if (dmalen0 > (devpriv->ui_AiDataLength))
-				dmalen0 = devpriv->ui_AiDataLength;
-			if (dmalen1 > (devpriv->ui_AiDataLength))
-				dmalen1 = devpriv->ui_AiDataLength;
+			if (dmalen0 > s->async->prealloc_bufsz)
+				dmalen0 = s->async->prealloc_bufsz;
+			if (dmalen1 > s->async->prealloc_bufsz)
+				dmalen1 = s->async->prealloc_bufsz;
 		}
 		devpriv->ui_DmaBufferUsesize[0] = dmalen0;
 		devpriv->ui_DmaBufferUsesize[1] = dmalen1;
@@ -1293,8 +1275,8 @@ static int apci3120_cyclic_ai(int mode,
 		/* END JK 07.05.04: Comparison between WIN32 and Linux driver */
 	}
 
-	if ((devpriv->us_UseDma == APCI3120_DISABLE)
-		&& !devpriv->b_AiContinuous) {
+	if (devpriv->us_UseDma == APCI3120_DISABLE &&
+	    cmd->stop_src == TRIG_COUNT) {
 		/*  set gate 2   to start conversion */
 		devpriv->us_OutputRegister =
 			devpriv->us_OutputRegister | APCI3120_ENABLE_TIMER2;
@@ -1337,50 +1319,17 @@ static int apci3120_ai_cmd(struct comedi_device *dev,
 	struct comedi_cmd *cmd = &s->async->cmd;
 
 	/* loading private structure with cmd structure inputs */
-	devpriv->ui_AiFlags = cmd->flags;
 	devpriv->ui_AiNbrofChannels = cmd->chanlist_len;
-	devpriv->ui_AiScanLength = cmd->scan_end_arg;
-	devpriv->pui_AiChannelList = cmd->chanlist;
-
-	/* UPDATE-0.7.57->0.7.68devpriv->ui_AiDataLength=s->async->data_len; */
-	devpriv->ui_AiDataLength = s->async->prealloc_bufsz;
-
-	if (cmd->stop_src == TRIG_COUNT)
-		devpriv->ui_AiNbrofScans = cmd->stop_arg;
-	else
-		devpriv->ui_AiNbrofScans = 0;
-
-	devpriv->ui_AiTimer0 = 0;	/*  variables changed to timer0,timer1 */
-	devpriv->ui_AiTimer1 = 0;
-	if ((devpriv->ui_AiNbrofScans == 0) || (devpriv->ui_AiNbrofScans == -1))
-		devpriv->b_AiContinuous = 1;	/*  user want neverending analog acquisition */
-	/*  stopped using cancel */
 
 	if (cmd->start_src == TRIG_EXT)
 		devpriv->b_ExttrigEnable = APCI3120_ENABLE;
 	else
 		devpriv->b_ExttrigEnable = APCI3120_DISABLE;
 
-	if (cmd->scan_begin_src == TRIG_FOLLOW) {
-		/*  mode 1 or 3 */
-		if (cmd->convert_src == TRIG_TIMER) {
-			/*  mode 1 */
-
-			devpriv->ui_AiTimer0 = cmd->convert_arg;	/*  timer constant in nano seconds */
-			/* return this_board->ai_cmd(1,dev,s); */
-			return apci3120_cyclic_ai(1, dev, s);
-		}
-
-	}
-	if ((cmd->scan_begin_src == TRIG_TIMER)
-		&& (cmd->convert_src == TRIG_TIMER)) {
-		/*  mode 2 */
-		devpriv->ui_AiTimer1 = cmd->scan_begin_arg;
-		devpriv->ui_AiTimer0 = cmd->convert_arg;	/*  variable changed timer2 to timer0 */
-		/* return this_board->ai_cmd(2,dev,s); */
+	if (cmd->scan_begin_src == TRIG_FOLLOW)
+		return apci3120_cyclic_ai(1, dev, s);
+	else	/* TRIG_TIMER */
 		return apci3120_cyclic_ai(2, dev, s);
-	}
-	return -1;
 }
 
 /*
@@ -1392,11 +1341,12 @@ static void v_APCI3120_InterruptDmaMoveBlock16bit(struct comedi_device *dev,
 						  unsigned int num_samples)
 {
 	struct addi_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 
 	devpriv->ui_AiActualScan +=
-		(s->async->cur_chan + num_samples) / devpriv->ui_AiScanLength;
+		(s->async->cur_chan + num_samples) / cmd->scan_end_arg;
 	s->async->cur_chan += num_samples;
-	s->async->cur_chan %= devpriv->ui_AiScanLength;
+	s->async->cur_chan %= cmd->scan_end_arg;
 
 	cfc_write_array_to_buffer(s, dma_buffer, num_samples * sizeof(short));
 }
@@ -1412,6 +1362,7 @@ static void apci3120_interrupt_dma(int irq, void *d)
 	struct comedi_device *dev = d;
 	struct addi_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int next_dma_buf, samplesinbuf;
 	unsigned long low_word, high_word, var;
 	unsigned int ui_Tmp;
@@ -1427,8 +1378,6 @@ static void apci3120_interrupt_dma(int irq, void *d)
 	if (samplesinbuf & 1) {
 		comedi_error(dev, "Odd count of bytes in DMA ring!");
 		apci3120_cancel(dev, s);
-		devpriv->b_AiCyclicAcquisition = APCI3120_DISABLE;
-
 		return;
 	}
 	samplesinbuf = samplesinbuf >> 1;	/*  number of received samples */
@@ -1489,16 +1438,15 @@ static void apci3120_interrupt_dma(int irq, void *d)
 			devpriv->ul_DmaBufferVirtual[devpriv->
 				ui_DmaActualBuffer], samplesinbuf);
 
-		if (!(devpriv->ui_AiFlags & TRIG_WAKE_EOS)) {
+		if (!(cmd->flags & TRIG_WAKE_EOS)) {
 			s->async->events |= COMEDI_CB_EOS;
 			comedi_event(dev, s);
 		}
 	}
-	if (!devpriv->b_AiContinuous)
-		if (devpriv->ui_AiActualScan >= devpriv->ui_AiNbrofScans) {
+	if (cmd->stop_src == TRIG_COUNT)
+		if (devpriv->ui_AiActualScan >= cmd->stop_arg) {
 			/*  all data sampled */
 			apci3120_cancel(dev, s);
-			devpriv->b_AiCyclicAcquisition = APCI3120_DISABLE;
 			s->async->events |= COMEDI_CB_EOA;
 			comedi_event(dev, s);
 			return;
@@ -1572,7 +1520,7 @@ static int apci3120_interrupt_handle_eos(struct comedi_device *dev)
 	n_chan = devpriv->ui_AiNbrofChannels;
 
 	for (i = 0; i < n_chan; i++)
-		err &= comedi_buf_put(s->async, inw(dev->iobase + 0));
+		err &= comedi_buf_put(s, inw(dev->iobase + 0));
 
 	s->async->events |= COMEDI_CB_EOS;
 
@@ -1648,7 +1596,7 @@ static void apci3120_interrupt(int irq, void *d)
 
 		if (devpriv->b_EocEosInterrupt == APCI3120_ENABLE) {	/*  enable this in without DMA ??? */
 
-			if (devpriv->b_AiCyclicAcquisition == APCI3120_ENABLE) {
+			if (devpriv->ai_running) {
 				ui_Check = 0;
 				apci3120_interrupt_handle_eos(dev);
 				devpriv->ui_AiActualScan++;
@@ -1690,8 +1638,6 @@ static void apci3120_interrupt(int irq, void *d)
 
 		switch (devpriv->b_Timer2Mode) {
 		case APCI3120_COUNTER:
-
-			devpriv->b_AiCyclicAcquisition = APCI3120_DISABLE;
 			devpriv->b_ModeSelectRegister =
 				devpriv->
 				b_ModeSelectRegister & APCI3120_DISABLE_EOS_INT;
@@ -1707,7 +1653,6 @@ static void apci3120_interrupt(int irq, void *d)
 
 			/* stop timer 0 and timer 1 */
 			apci3120_cancel(dev, s);
-			devpriv->b_AiCyclicAcquisition = APCI3120_DISABLE;
 
 			/* UPDATE-0.7.57->0.7.68comedi_done(dev,s); */
 			s->async->events |= COMEDI_CB_EOA;
@@ -1746,7 +1691,7 @@ static void apci3120_interrupt(int irq, void *d)
 	}
 
 	if ((int_daq & 0x4) && (devpriv->b_InterruptMode == APCI3120_DMA_MODE)) {
-		if (devpriv->b_AiCyclicAcquisition == APCI3120_ENABLE) {
+		if (devpriv->ai_running) {
 
 			/****************************/
 			/* Clear Timer Write TC int */
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3200.c b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3200.c
index 0536d837386..f540394d17b 100644
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3200.c
+++ b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3200.c
@@ -2221,12 +2221,11 @@ static int apci3200_ai_cmdtest(struct comedi_device *dev,
 	unsigned int ui_ConvertTimeBase = 0;
 	unsigned int ui_DelayTime = 0;
 	unsigned int ui_DelayTimeBase = 0;
-	int i_Triggermode = 0;
-	int i_TriggerEdge = 0;
 	int i_NbrOfChannel = 0;
 	int i_Cpt = 0;
 	double d_ConversionTimeForAllChannels = 0.0;
 	double d_SCANTimeNewUnit = 0.0;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -2253,23 +2252,37 @@ static int apci3200_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 2b : and mutually compatible */
 
-	if (cmd->start_src == TRIG_EXT) {
-		i_TriggerEdge = cmd->start_arg & 0xFFFF;
-		i_Triggermode = cmd->start_arg >> 16;
-		if (i_TriggerEdge < 1 || i_TriggerEdge > 3) {
-			err++;
-			printk("\nThe trigger edge selection is in error\n");
-		}
-		if (i_Triggermode != 2) {
-			err++;
-			printk("\nThe trigger mode selection is in error\n");
-		}
-	}
-
 	if (err) {
 		apci3200_reset(dev);
 		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:
+		/* validate the trigger edge selection */
+		arg = cmd->start_arg & 0xffff;
+		if (arg < 1 || arg > 3) {
+			cmd->start_arg &= ~0xffff;
+			cmd->start_arg |= 1;
+			err |= -EINVAL;
+		}
+		/* validate the trigger mode selection */
+		arg = cmd->start_arg >> 16;
+		if (arg != 2) {
+			cmd->start_arg &= ~(0xffff << 16);
+			cmd->start_arg |= (2 << 16);
+			err |= -EINVAL;
+		}
+		break;
+	}
+
+	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+
 	/* i_FirstChannel=cmd->chanlist[0]; */
 	s_BoardInfos[dev->minor].i_FirstChannel = cmd->chanlist[0];
 	/* i_LastChannel=cmd->chanlist[1]; */
@@ -2689,8 +2702,7 @@ static int i_APCI3200_InterruptHandleEos(struct comedi_device *dev)
 			s->async->events |= COMEDI_CB_EOS;
 
 			/*  Test if enougth memory is available and allocate it for 7 values */
-			/* n = comedi_buf_write_alloc(s->async, 7*sizeof(unsigned int)); */
-			n = comedi_buf_write_alloc(s->async,
+			n = comedi_buf_write_alloc(s,
 				(7 + 12) * sizeof(unsigned int));
 
 			/*  If not enough memory available, event is set to Comedi Buffer Error */
@@ -2699,12 +2711,12 @@ static int i_APCI3200_InterruptHandleEos(struct comedi_device *dev)
 				s->async->events |= COMEDI_CB_ERROR;
 			}
 			/*  Write all 7 scan values in the comedi buffer */
-			comedi_buf_memcpy_to(s->async, 0,
+			comedi_buf_memcpy_to(s, 0,
 				(unsigned int *) s_BoardInfos[dev->minor].
 				ui_ScanValueArray, (7 + 12) * sizeof(unsigned int));
 
 			/*  Update comedi buffer pinters indexes */
-			comedi_buf_write_free(s->async,
+			comedi_buf_write_free(s,
 				(7 + 12) * sizeof(unsigned int));
 
 			/*  Send events */
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3501.c b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3501.c
index 20e89b0bdc4..e82c3fcd048 100644
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3501.c
+++ b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3501.c
@@ -102,11 +102,7 @@ static int apci3501_write_insn_timer(struct comedi_device *dev,
 			ul_Command1 = (ul_Command1 & 0xFFFFF9FFUL) | 0x1UL;
 			/* Enable the Watchdog */
 			outl(ul_Command1, dev->iobase + APCI3501_TIMER_CTRL_REG);
-		}
-
-		else if (data[1] == 0)	/* Stop The Watchdog */
-		{
-			/* Stop The Watchdog */
+		} else if (data[1] == 0) { /* Stop The Watchdog */
 			ul_Command1 = inl(dev->iobase + APCI3501_TIMER_CTRL_REG);
 			ul_Command1 = ul_Command1 & 0xFFFFF9FEUL;
 			outl(0x0, dev->iobase + APCI3501_TIMER_CTRL_REG);
diff --git a/drivers/staging/comedi/drivers/addi_apci_1032.c b/drivers/staging/comedi/drivers/addi_apci_1032.c
index 0daa0ea63b5..1b2e7c040c9 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1032.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1032.c
@@ -198,7 +198,7 @@ static int apci1032_cos_cmdtest(struct comedi_device *dev,
 	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, 1);
+	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)
@@ -262,7 +262,7 @@ static irqreturn_t apci1032_interrupt(int irq, void *d)
 	outl(ctrl & ~APCI1032_CTRL_INT_ENA, dev->iobase + APCI1032_CTRL_REG);
 
 	s->state = inl(dev->iobase + APCI1032_STATUS_REG) & 0xffff;
-	comedi_buf_put(s->async, s->state);
+	comedi_buf_put(s, s->state);
 	s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
 	comedi_event(dev, s);
 
@@ -332,6 +332,7 @@ static int apci1032_auto_attach(struct comedi_device *dev,
 		s->range_table	= &range_digital;
 		s->insn_config	= apci1032_cos_insn_config;
 		s->insn_bits	= apci1032_cos_insn_bits;
+		s->len_chanlist	= 1;
 		s->do_cmdtest	= apci1032_cos_cmdtest;
 		s->do_cmd	= apci1032_cos_cmd;
 		s->cancel	= apci1032_cos_cancel;
@@ -379,5 +380,5 @@ static struct pci_driver apci1032_pci_driver = {
 module_comedi_pci_driver(apci1032_driver, apci1032_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("ADDI-DATA APCI-1032, 32 channel DI boards");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/addi_apci_1500.c b/drivers/staging/comedi/drivers/addi_apci_1500.c
index bd8e08ca14c..eab75eb2647 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1500.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1500.c
@@ -72,5 +72,5 @@ static struct pci_driver apci1500_pci_driver = {
 module_comedi_pci_driver(apci1500_driver, apci1500_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("ADDI-DATA APCI-1500, 16 channel DI / 16 channel DO boards");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/addi_apci_1564.c b/drivers/staging/comedi/drivers/addi_apci_1564.c
index 27aa9ae1bdd..13d9962b47e 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1564.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1564.c
@@ -3,50 +3,168 @@
 
 #include "../comedidev.h"
 #include "comedi_fc.h"
-#include "amcc_s5933.h"
 
 #include "addi-data/addi_common.h"
 
-#include "addi-data/addi_eeprom.c"
 #include "addi-data/hwdrv_apci1564.c"
-#include "addi-data/addi_common.c"
-
-static const struct addi_board apci1564_boardtypes[] = {
-	{
-		.pc_DriverName		= "apci1564",
-		.i_IorangeBase1		= APCI1564_ADDRESS_RANGE,
-		.i_PCIEeprom		= ADDIDATA_EEPROM,
-		.pc_EepromChip		= ADDIDATA_93C76,
-		.i_NbrDiChannel		= 32,
-		.i_NbrDoChannel		= 32,
-		.i_DoMaxdata		= 0xffffffff,
-		.i_Timer		= 1,
-		.interrupt		= apci1564_interrupt,
-		.reset			= apci1564_reset,
-		.di_config		= apci1564_di_config,
-		.di_bits		= apci1564_di_insn_bits,
-		.do_config		= apci1564_do_config,
-		.do_bits		= apci1564_do_insn_bits,
-		.do_read		= apci1564_do_read,
-		.timer_config		= apci1564_timer_config,
-		.timer_write		= apci1564_timer_write,
-		.timer_read		= apci1564_timer_read,
-	},
-};
+
+static irqreturn_t v_ADDI_Interrupt(int irq, void *d)
+{
+	apci1564_interrupt(irq, d);
+	return IRQ_RETVAL(1);
+}
+
+static int apci1564_di_insn_bits(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned int *data)
+{
+	struct addi_private *devpriv = dev->private;
+
+	data[1] = inl(devpriv->i_IobaseAmcc + APCI1564_DI_REG);
+
+	return insn->n;
+}
+
+static int apci1564_do_insn_bits(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned int *data)
+{
+	struct addi_private *devpriv = dev->private;
+
+	s->state = inl(devpriv->i_IobaseAmcc + APCI1564_DO_REG);
+
+	if (comedi_dio_update_state(s, data))
+		outl(s->state, devpriv->i_IobaseAmcc + APCI1564_DO_REG);
+
+	data[1] = s->state;
+
+	return insn->n;
+}
+
+static int apci1564_reset(struct comedi_device *dev)
+{
+	struct addi_private *devpriv = dev->private;
+
+	ui_Type = 0;
+
+	/* Disable the input interrupts and reset status register */
+	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_DI_IRQ_REG);
+	inl(devpriv->i_IobaseAmcc + APCI1564_DI_INT_STATUS_REG);
+	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_DI_INT_MODE1_REG);
+	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_DI_INT_MODE2_REG);
+
+	/* Reset the output channels and disable interrupts */
+	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_DO_REG);
+	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_DO_INT_CTRL_REG);
+
+	/* Reset the watchdog registers */
+	addi_watchdog_reset(devpriv->i_IobaseAmcc + APCI1564_WDOG_REG);
+
+	/* Reset the timer registers */
+	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_TIMER_CTRL_REG);
+	outl(0x0, devpriv->i_IobaseAmcc + 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));
+
+	return 0;
+}
 
 static int apci1564_auto_attach(struct comedi_device *dev,
-				unsigned long context)
+				      unsigned long context_unused)
+{
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	struct addi_private *devpriv;
+	struct comedi_subdevice *s;
+	int ret;
+
+	dev->board_name = dev->driver->driver_name;
+
+	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
+	if (!devpriv)
+		return -ENOMEM;
+
+	ret = comedi_pci_enable(dev);
+	if (ret)
+		return ret;
+
+	dev->iobase = pci_resource_start(pcidev, 1);
+	devpriv->i_IobaseAmcc = pci_resource_start(pcidev, 0);
+
+	apci1564_reset(dev);
+
+	if (pcidev->irq > 0) {
+		ret = request_irq(pcidev->irq, v_ADDI_Interrupt, IRQF_SHARED,
+				  dev->board_name, dev);
+		if (ret == 0)
+			dev->irq = pcidev->irq;
+	}
+
+	ret = comedi_alloc_subdevices(dev, 3);
+	if (ret)
+		return ret;
+
+	/*  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->len_chanlist = 32;
+	s->range_table = &range_digital;
+	s->insn_config = apci1564_di_config;
+	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 = 0xffffffff;
+	s->len_chanlist = 32;
+	s->range_table = &range_digital;
+	s->insn_config = apci1564_do_config;
+	s->insn_bits = apci1564_do_insn_bits;
+	s->insn_read = apci1564_do_read;
+
+	/*  Allocate and Initialise Timer Subdevice Structures */
+	s = &dev->subdevices[2];
+	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;
+
+	return 0;
+}
+
+static void apci1564_detach(struct comedi_device *dev)
 {
-	dev->board_ptr = &apci1564_boardtypes[0];
+	struct addi_private *devpriv = dev->private;
 
-	return addi_auto_attach(dev, context);
+	if (devpriv) {
+		if (dev->iobase)
+			apci1564_reset(dev);
+		if (dev->irq)
+			free_irq(dev->irq, dev);
+	}
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver apci1564_driver = {
 	.driver_name	= "addi_apci_1564",
 	.module		= THIS_MODULE,
 	.auto_attach	= apci1564_auto_attach,
-	.detach		= i_ADDI_Detach,
+	.detach		= apci1564_detach,
 };
 
 static int apci1564_pci_probe(struct pci_dev *dev,
@@ -70,5 +188,5 @@ static struct pci_driver apci1564_pci_driver = {
 module_comedi_pci_driver(apci1564_driver, apci1564_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("ADDI-DATA APCI-1564, 32 channel DI / 32 channel DO boards");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/addi_apci_2032.c b/drivers/staging/comedi/drivers/addi_apci_2032.c
index c9b933cb598..be0a8a7bd3b 100644
--- a/drivers/staging/comedi/drivers/addi_apci_2032.c
+++ b/drivers/staging/comedi/drivers/addi_apci_2032.c
@@ -197,6 +197,7 @@ static irqreturn_t apci2032_interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = d;
 	struct comedi_subdevice *s = dev->read_subdev;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	struct apci2032_int_private *subpriv;
 	unsigned int val;
 	bool do_event = false;
@@ -222,21 +223,20 @@ static irqreturn_t apci2032_interrupt(int irq, void *d)
 	 */
 
 	if (subpriv->active && (val & subpriv->enabled_isns) != 0) {
-		unsigned short bits;
-		unsigned int n, len;
-		unsigned int *chanlist;
+		unsigned short bits = 0;
+		int i;
 
 		/* Bits in scan data correspond to indices in channel list. */
-		bits = 0;
-		len = s->async->cmd.chanlist_len;
-		chanlist = &s->async->cmd.chanlist[0];
-		for (n = 0; n < len; n++)
-			if ((val & (1U << CR_CHAN(chanlist[n]))) != 0)
-				bits |= 1U << n;
-
-		if (comedi_buf_put(s->async, bits)) {
+		for (i = 0; i < cmd->chanlist_len; i++) {
+			unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+
+			if (val & (1 << chan))
+				bits |= (1 << i);
+		}
+
+		if (comedi_buf_put(s, bits)) {
 			s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
-			if (s->async->cmd.stop_src == TRIG_COUNT &&
+			if (cmd->stop_src == TRIG_COUNT &&
 			    subpriv->stop_count > 0) {
 				subpriv->stop_count--;
 				if (subpriv->stop_count == 0) {
@@ -374,5 +374,5 @@ static struct pci_driver apci2032_pci_driver = {
 module_comedi_pci_driver(apci2032_driver, apci2032_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("ADDI-DATA APCI-2032, 32 channel DO boards");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/addi_apci_3120.c b/drivers/staging/comedi/drivers/addi_apci_3120.c
index 57ee6e5c763..0cfb12fa1cb 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3120.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3120.c
@@ -246,5 +246,5 @@ static struct pci_driver apci3120_pci_driver = {
 module_comedi_pci_driver(apci3120_driver, apci3120_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("ADDI-DATA APCI-3120, Analog input board");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/addi_apci_3xxx.c b/drivers/staging/comedi/drivers/addi_apci_3xxx.c
index 6dc11c407f5..0532b6cc40e 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3xxx.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3xxx.c
@@ -373,7 +373,7 @@ static irqreturn_t apci3xxx_irq_handler(int irq, void *d)
 		writel(status, devpriv->mmio + 16);
 
 		val = readl(devpriv->mmio + 28);
-		comedi_buf_put(s->async, val);
+		comedi_buf_put(s, val);
 
 		s->async->events |= COMEDI_CB_EOA;
 		comedi_event(dev, s);
@@ -533,7 +533,7 @@ static int apci3xxx_ai_cmdtest(struct comedi_device *dev,
 {
 	const struct apci3xxx_boardinfo *board = comedi_board(dev);
 	int err = 0;
-	unsigned int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -573,31 +573,9 @@ static int apci3xxx_ai_cmdtest(struct comedi_device *dev,
 
 	/* step 4: fix up any arguments */
 
-	/*
-	 * FIXME: The hardware supports multiple scan modes but the original
-	 * addi-data driver only supported reading a single channel with
-	 * interrupts. Need a proper datasheet to fix this.
-	 *
-	 * The following scan modes are supported by the hardware:
-	 * 1) Single software scan
-	 * 2) Single hardware triggered scan
-	 * 3) Continuous software scan
-	 * 4) Continuous software scan with timer delay
-	 * 5) Continuous hardware triggered scan
-	 * 6) Continuous hardware triggered scan with timer delay
-	 *
-	 * For now, limit the chanlist to a single channel.
-	 */
-	if (cmd->chanlist_len > 1) {
-		cmd->chanlist_len = 1;
-		err |= -EINVAL;
-	}
-
-	tmp = cmd->convert_arg;
-	err |= apci3xxx_ai_ns_to_timer(dev, &cmd->convert_arg,
-				       cmd->flags & TRIG_ROUND_MASK);
-	if (tmp != cmd->convert_arg)
-		err |= -EINVAL;
+	arg = cmd->convert_arg;
+	err |= apci3xxx_ai_ns_to_timer(dev, &arg, cmd->flags & TRIG_ROUND_MASK);
+	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 
 	if (err)
 		return 4;
@@ -719,7 +697,8 @@ static int apci3xxx_dio_insn_config(struct comedi_device *dev,
 		if (chan < 16)
 			return -EINVAL;
 		else
-			/* changing any channel in port 2 changes the entire port */
+			/* changing any channel in port 2 */
+			/* changes the entire port        */
 			mask = 0xff0000;
 	}
 
@@ -842,12 +821,30 @@ static int apci3xxx_auto_attach(struct comedi_device *dev,
 		s->subdev_flags	= SDF_READABLE | board->ai_subdev_flags;
 		s->n_chan	= board->ai_n_chan;
 		s->maxdata	= board->ai_maxdata;
-		s->len_chanlist	= s->n_chan;
 		s->range_table	= &apci3xxx_ai_range;
 		s->insn_read	= apci3xxx_ai_insn_read;
 		if (dev->irq) {
+			/*
+			 * FIXME: The hardware supports multiple scan modes
+			 * but the original addi-data driver only supported
+			 * reading a single channel with interrupts. Need a
+			 * proper datasheet to fix this.
+			 *
+			 * The following scan modes are supported by the
+			 * hardware:
+			 *   1) Single software scan
+			 *   2) Single hardware triggered scan
+			 *   3) Continuous software scan
+			 *   4) Continuous software scan with timer delay
+			 *   5) Continuous hardware triggered scan
+			 *   6) Continuous hardware triggered scan with timer
+			 *      delay
+			 *
+			 * For now, limit the chanlist to a single channel.
+			 */
 			dev->read_subdev = s;
 			s->subdev_flags	|= SDF_CMD_READ;
+			s->len_chanlist	= 1;
 			s->do_cmdtest	= apci3xxx_ai_cmdtest;
 			s->do_cmd	= apci3xxx_ai_cmd;
 			s->cancel	= apci3xxx_ai_cancel;
diff --git a/drivers/staging/comedi/drivers/adl_pci9111.c b/drivers/staging/comedi/drivers/adl_pci9111.c
index a29ceacb966..584fd57ecb7 100644
--- a/drivers/staging/comedi/drivers/adl_pci9111.c
+++ b/drivers/staging/comedi/drivers/adl_pci9111.c
@@ -137,10 +137,8 @@ struct pci9111_private_data {
 	unsigned long lcr_io_base;
 
 	int stop_counter;
-	int stop_is_none;
 
 	unsigned int scan_delay;
-	unsigned int chanlist_len;
 	unsigned int chunk_counter;
 	unsigned int chunk_num_samples;
 
@@ -314,148 +312,137 @@ static int pci9111_ai_cancel(struct comedi_device *dev,
 	return 0;
 }
 
+static int pci9111_ai_check_chanlist(struct comedi_device *dev,
+				     struct comedi_subdevice *s,
+				     struct comedi_cmd *cmd)
+{
+	unsigned int range0 = CR_RANGE(cmd->chanlist[0]);
+	unsigned int aref0 = CR_AREF(cmd->chanlist[0]);
+	int i;
+
+	for (i = 1; 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]);
+
+		if (chan != i) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must be consecutive channels,counting upwards from 0\n");
+			return -EINVAL;
+		}
+
+		if (range != range0) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must all have the same gain\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 pci9111_ai_do_cmd_test(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
 				  struct comedi_cmd *cmd)
 {
 	struct pci9111_private_data *dev_private = dev->private;
-	int tmp;
-	int error = 0;
-	int range, reference;
-	int i;
+	int err = 0;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	error |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	error |= cfc_check_trigger_src(&cmd->scan_begin_src,
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
 					TRIG_TIMER | TRIG_FOLLOW | TRIG_EXT);
-	error |= cfc_check_trigger_src(&cmd->convert_src,
+	err |= cfc_check_trigger_src(&cmd->convert_src,
 					TRIG_TIMER | TRIG_EXT);
-	error |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	error |= cfc_check_trigger_src(&cmd->stop_src,
+	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= cfc_check_trigger_src(&cmd->stop_src,
 					TRIG_COUNT | TRIG_NONE);
 
-	if (error)
+	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	error |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	error |= cfc_check_trigger_is_unique(cmd->convert_src);
-	error |= cfc_check_trigger_is_unique(cmd->stop_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->convert_src == TRIG_TIMER) &&
-	    !((cmd->scan_begin_src == TRIG_TIMER) ||
-	      (cmd->scan_begin_src == TRIG_FOLLOW)))
-		error |= -EINVAL;
-	if ((cmd->convert_src == TRIG_EXT) &&
-	    !((cmd->scan_begin_src == TRIG_EXT) ||
-	      (cmd->scan_begin_src == TRIG_FOLLOW)))
-		error |= -EINVAL;
+	if (cmd->scan_begin_src != TRIG_FOLLOW) {
+		if (cmd->scan_begin_src != cmd->convert_src)
+			err |= -EINVAL;
+	}
 
-	if (error)
+	if (err)
 		return 2;
 
 	/* Step 3: check if arguments are trivially valid */
 
-	error |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
 
 	if (cmd->convert_src == TRIG_TIMER)
-		error |= cfc_check_trigger_arg_min(&cmd->convert_arg,
+		err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
 					PCI9111_AI_ACQUISITION_PERIOD_MIN_NS);
 	else	/* TRIG_EXT */
-		error |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
 
 	if (cmd->scan_begin_src == TRIG_TIMER)
-		error |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
+		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
 					PCI9111_AI_ACQUISITION_PERIOD_MIN_NS);
 	else	/* TRIG_FOLLOW || TRIG_EXT */
-		error |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
 
-	error |= cfc_check_trigger_arg_is(&cmd->scan_end_arg,
-					  cmd->chanlist_len);
+	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		error |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		error |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
 
-	if (error)
+	if (err)
 		return 3;
 
-	/*  Step 4 : fix up any arguments */
+	/* Step 4: fix up any arguments */
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
+		arg = cmd->convert_arg;
 		i8253_cascade_ns_to_timer(I8254_OSC_BASE_2MHZ,
 					  &dev_private->div1,
 					  &dev_private->div2,
-					  &cmd->convert_arg, cmd->flags);
-		if (tmp != cmd->convert_arg)
-			error++;
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
-	/*  There's only one timer on this card, so the scan_begin timer must */
-	/*  be a multiple of chanlist_len*convert_arg */
 
+	/*
+	 * There's only one timer on this card, so the scan_begin timer
+	 * must be a multiple of chanlist_len*convert_arg
+	 */
 	if (cmd->scan_begin_src == TRIG_TIMER) {
+		arg = cmd->chanlist_len * cmd->convert_arg;
 
-		unsigned int scan_begin_min;
-		unsigned int scan_begin_arg;
-		unsigned int scan_factor;
-
-		scan_begin_min = cmd->chanlist_len * cmd->convert_arg;
-
-		if (cmd->scan_begin_arg != scan_begin_min) {
-			if (scan_begin_min < cmd->scan_begin_arg) {
-				scan_factor =
-				    cmd->scan_begin_arg / scan_begin_min;
-				scan_begin_arg = scan_factor * scan_begin_min;
-				if (cmd->scan_begin_arg != scan_begin_arg) {
-					cmd->scan_begin_arg = scan_begin_arg;
-					error++;
-				}
-			} else {
-				cmd->scan_begin_arg = scan_begin_min;
-				error++;
-			}
-		}
+		if (arg < cmd->scan_begin_arg)
+			arg *= (cmd->scan_begin_arg / arg);
+
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
-	if (error)
+	if (err)
 		return 4;
 
-	/*  Step 5 : check channel list */
-
-	if (cmd->chanlist) {
-
-		range = CR_RANGE(cmd->chanlist[0]);
-		reference = CR_AREF(cmd->chanlist[0]);
-
-		if (cmd->chanlist_len > 1) {
-			for (i = 0; i < cmd->chanlist_len; i++) {
-				if (CR_CHAN(cmd->chanlist[i]) != i) {
-					comedi_error(dev,
-						     "entries in chanlist must be consecutive "
-						     "channels,counting upwards from 0\n");
-					error++;
-				}
-				if (CR_RANGE(cmd->chanlist[i]) != range) {
-					comedi_error(dev,
-						     "entries in chanlist must all have the same gain\n");
-					error++;
-				}
-				if (CR_AREF(cmd->chanlist[i]) != reference) {
-					comedi_error(dev,
-						     "entries in chanlist must all have the same reference\n");
-					error++;
-				}
-			}
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= pci9111_ai_check_chanlist(dev, s, cmd);
 
-	if (error)
+	if (err)
 		return 5;
 
 	return 0;
@@ -466,18 +453,18 @@ static int pci9111_ai_do_cmd(struct comedi_device *dev,
 			     struct comedi_subdevice *s)
 {
 	struct pci9111_private_data *dev_private = dev->private;
-	struct comedi_cmd *async_cmd = &s->async->cmd;
+	struct comedi_cmd *cmd = &s->async->cmd;
 
 	/*  Set channel scan limit */
 	/*  PCI9111 allows only scanning from channel 0 to channel n */
 	/*  TODO: handle the case of an external multiplexer */
 
-	if (async_cmd->chanlist_len > 1) {
-		outb(async_cmd->chanlist_len - 1,
+	if (cmd->chanlist_len > 1) {
+		outb(cmd->chanlist_len - 1,
 			dev->iobase + PCI9111_AI_CHANNEL_REG);
 		pci9111_autoscan_set(dev, true);
 	} else {
-		outb(CR_CHAN(async_cmd->chanlist[0]),
+		outb(CR_CHAN(cmd->chanlist[0]),
 			dev->iobase + PCI9111_AI_CHANNEL_REG);
 		pci9111_autoscan_set(dev, false);
 	}
@@ -485,22 +472,18 @@ static int pci9111_ai_do_cmd(struct comedi_device *dev,
 	/*  Set gain */
 	/*  This is the same gain on every channel */
 
-	outb(CR_RANGE(async_cmd->chanlist[0]) & PCI9111_AI_RANGE_MASK,
+	outb(CR_RANGE(cmd->chanlist[0]) & PCI9111_AI_RANGE_MASK,
 		dev->iobase + PCI9111_AI_RANGE_STAT_REG);
 
 	/* Set counter */
-	if (async_cmd->stop_src == TRIG_COUNT) {
-		dev_private->stop_counter =
-		    async_cmd->stop_arg * async_cmd->chanlist_len;
-		dev_private->stop_is_none = 0;
-	} else {	/* TRIG_NONE */
+	if (cmd->stop_src == TRIG_COUNT)
+		dev_private->stop_counter = cmd->stop_arg * cmd->chanlist_len;
+	else	/* TRIG_NONE */
 		dev_private->stop_counter = 0;
-		dev_private->stop_is_none = 1;
-	}
 
 	/*  Set timer pacer */
 	dev_private->scan_delay = 0;
-	if (async_cmd->convert_src == TRIG_TIMER) {
+	if (cmd->convert_src == TRIG_TIMER) {
 		pci9111_trigger_source_set(dev, software);
 		pci9111_timer_set(dev);
 		pci9111_fifo_reset(dev);
@@ -510,11 +493,9 @@ static int pci9111_ai_do_cmd(struct comedi_device *dev,
 		plx9050_interrupt_control(dev_private->lcr_io_base, true, true,
 					  false, true, true);
 
-		if (async_cmd->scan_begin_src == TRIG_TIMER) {
-			dev_private->scan_delay =
-				(async_cmd->scan_begin_arg /
-				 (async_cmd->convert_arg *
-				  async_cmd->chanlist_len)) - 1;
+		if (cmd->scan_begin_src == TRIG_TIMER) {
+			dev_private->scan_delay = (cmd->scan_begin_arg /
+				(cmd->convert_arg * cmd->chanlist_len)) - 1;
 		}
 	} else {	/* TRIG_EXT */
 		pci9111_trigger_source_set(dev, external);
@@ -527,10 +508,9 @@ static int pci9111_ai_do_cmd(struct comedi_device *dev,
 	}
 
 	dev_private->stop_counter *= (1 + dev_private->scan_delay);
-	dev_private->chanlist_len = async_cmd->chanlist_len;
 	dev_private->chunk_counter = 0;
-	dev_private->chunk_num_samples =
-	    dev_private->chanlist_len * (1 + dev_private->scan_delay);
+	dev_private->chunk_num_samples = cmd->chanlist_len *
+					 (1 + dev_private->scan_delay);
 
 	return 0;
 }
@@ -551,12 +531,71 @@ static void pci9111_ai_munge(struct comedi_device *dev,
 		array[i] = ((array[i] >> shift) & maxdata) ^ invert;
 }
 
+static void pci9111_handle_fifo_half_full(struct comedi_device *dev,
+					  struct comedi_subdevice *s)
+{
+	struct pci9111_private_data *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
+	unsigned int total = 0;
+	unsigned int samples;
+
+	if (cmd->stop_src == TRIG_COUNT &&
+	    PCI9111_FIFO_HALF_SIZE > devpriv->stop_counter)
+		samples = devpriv->stop_counter;
+	else
+		samples = PCI9111_FIFO_HALF_SIZE;
+
+	insw(dev->iobase + PCI9111_AI_FIFO_REG,
+	     devpriv->ai_bounce_buffer, samples);
+
+	if (devpriv->scan_delay < 1) {
+		total = cfc_write_array_to_buffer(s,
+						  devpriv->ai_bounce_buffer,
+						  samples * sizeof(short));
+	} else {
+		unsigned int pos = 0;
+		unsigned int to_read;
+
+		while (pos < samples) {
+			if (devpriv->chunk_counter < cmd->chanlist_len) {
+				to_read = cmd->chanlist_len -
+					  devpriv->chunk_counter;
+
+				if (to_read > samples - pos)
+					to_read = samples - pos;
+
+				total += cfc_write_array_to_buffer(s,
+						devpriv->ai_bounce_buffer + pos,
+						to_read * sizeof(short));
+			} else {
+				to_read = devpriv->chunk_num_samples -
+					  devpriv->chunk_counter;
+
+				if (to_read > samples - pos)
+					to_read = samples - pos;
+
+				total += to_read * sizeof(short);
+			}
+
+			pos += to_read;
+			devpriv->chunk_counter += to_read;
+
+			if (devpriv->chunk_counter >=
+			    devpriv->chunk_num_samples)
+				devpriv->chunk_counter = 0;
+		}
+	}
+
+	devpriv->stop_counter -= total / sizeof(short);
+}
+
 static irqreturn_t pci9111_interrupt(int irq, void *p_device)
 {
 	struct comedi_device *dev = p_device;
 	struct pci9111_private_data *dev_private = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
 	struct comedi_async *async;
+	struct comedi_cmd *cmd;
 	unsigned int status;
 	unsigned long irq_flags;
 	unsigned char intcsr;
@@ -568,6 +607,7 @@ static irqreturn_t pci9111_interrupt(int irq, void *p_device)
 	}
 
 	async = s->async;
+	cmd = &async->cmd;
 
 	spin_lock_irqsave(&dev->spinlock, irq_flags);
 
@@ -599,79 +639,11 @@ static irqreturn_t pci9111_interrupt(int irq, void *p_device)
 		}
 
 		/* '0' means FIFO is half-full */
-		if (!(status & PCI9111_AI_STAT_FF_HF)) {
-			unsigned int num_samples;
-			unsigned int bytes_written = 0;
-
-			num_samples =
-			    PCI9111_FIFO_HALF_SIZE >
-			    dev_private->stop_counter
-			    && !dev_private->
-			    stop_is_none ? dev_private->stop_counter :
-			    PCI9111_FIFO_HALF_SIZE;
-			insw(dev->iobase + PCI9111_AI_FIFO_REG,
-			     dev_private->ai_bounce_buffer, num_samples);
-
-			if (dev_private->scan_delay < 1) {
-				bytes_written =
-				    cfc_write_array_to_buffer(s,
-							      dev_private->
-							      ai_bounce_buffer,
-							      num_samples *
-							      sizeof(short));
-			} else {
-				int position = 0;
-				int to_read;
-
-				while (position < num_samples) {
-					if (dev_private->chunk_counter <
-					    dev_private->chanlist_len) {
-						to_read =
-						    dev_private->chanlist_len -
-						    dev_private->chunk_counter;
-
-						if (to_read >
-						    num_samples - position)
-							to_read =
-							    num_samples -
-							    position;
-
-						bytes_written +=
-						    cfc_write_array_to_buffer
-						    (s,
-						     dev_private->ai_bounce_buffer
-						     + position,
-						     to_read * sizeof(short));
-					} else {
-						to_read =
-						    dev_private->chunk_num_samples
-						    -
-						    dev_private->chunk_counter;
-						if (to_read >
-						    num_samples - position)
-							to_read =
-							    num_samples -
-							    position;
-
-						bytes_written +=
-						    sizeof(short) * to_read;
-					}
-
-					position += to_read;
-					dev_private->chunk_counter += to_read;
-
-					if (dev_private->chunk_counter >=
-					    dev_private->chunk_num_samples)
-						dev_private->chunk_counter = 0;
-				}
-			}
-
-			dev_private->stop_counter -=
-			    bytes_written / sizeof(short);
-		}
+		if (!(status & PCI9111_AI_STAT_FF_HF))
+			pci9111_handle_fifo_half_full(dev, s);
 	}
 
-	if (dev_private->stop_counter == 0 && !dev_private->stop_is_none)
+	if (cmd->stop_src == TRIG_COUNT && dev_private->stop_counter == 0)
 		async->events |= COMEDI_CB_EOA;
 
 	outb(0, dev->iobase + PCI9111_INT_CLR_REG);
diff --git a/drivers/staging/comedi/drivers/adl_pci9118.c b/drivers/staging/comedi/drivers/adl_pci9118.c
index 3cfa1756fa6..59a65cbc6db 100644
--- a/drivers/staging/comedi/drivers/adl_pci9118.c
+++ b/drivers/staging/comedi/drivers/adl_pci9118.c
@@ -96,7 +96,7 @@ Configuration options:
 				 * correct channel number on every 12 bit sample
 				 */
 
-#define IORANGE_9118 	64	/* I hope */
+#define IORANGE_9118	64	/* I hope */
 #define PCI9118_CHANLEN	255	/*
 				 * len of chanlist, some source say 256,
 				 * but reality looks like 255 :-(
@@ -320,14 +320,9 @@ struct pci9118_private {
 	unsigned char AdControlReg;	/* A/D control register */
 	unsigned char IntControlReg;	/* Interrupt control register */
 	unsigned char AdFunctionReg;	/* A/D function register */
-	char valid;			/* driver is ok */
 	char ai_neverending;		/* we do unlimited AI */
-	unsigned int i8254_osc_base;	/* frequence of onboard oscilator */
 	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_buf_ptr;	/* data buffer ptr in samples */
-	unsigned int ai_n_chan;		/* how many channels is measured */
-	unsigned int ai_n_scanlen;	/* len of actual scanlist */
 	unsigned int ai_n_realscanlen;	/*
 					 * what we must transfer for one
 					 * outgoing scan include front/back adds
@@ -341,9 +336,6 @@ struct pci9118_private {
 					 * how many channels we must add
 					 * before scan to satisfy DMA?
 					 */
-	unsigned int *ai_chanlist;	/* actual chanlist */
-	unsigned int ai_timer1;
-	unsigned int ai_timer2;
 	unsigned int ai_flags;
 	char ai12_startstop;		/*
 					 * measure can start/stop
@@ -353,9 +345,7 @@ struct pci9118_private {
 						 * divisors for start of measure
 						 * on external start
 						 */
-	unsigned int ai_data_len;
 	unsigned short ao_data[2];		/* data output buffer */
-	unsigned int ai_scans;			/* number of scans to do */
 	char dma_doublebuf;			/* we can use double buffering */
 	unsigned int dma_actbuf;		/* which buffer is used now */
 	unsigned short *dmabuf_virt[2];		/*
@@ -372,31 +362,12 @@ struct pci9118_private {
 						 */
 	unsigned int dmabuf_used_size[2];	/* which size was truly used */
 	unsigned int dmabuf_panic_size[2];
-	unsigned int dmabuf_samples[2];		/* size in samples */
 	int dmabuf_pages[2];			/* number of pages in buffer */
-	unsigned char cnt0_users;		/*
-						 * bit field of 8254 CNT0 users
-						 * (0-unused, 1-AO, 2-DI, 3-DO)
-						 */
 	unsigned char exttrg_users;		/*
 						 * bit field of external trigger
 						 * users(0-AI, 1-AO, 2-DI, 3-DO)
 						 */
-	unsigned int cnt0_divisor;		/* actual CNT0 divisor */
-	void (*int_ai_func) (struct comedi_device *, struct comedi_subdevice *,
-		unsigned short,
-		unsigned int,
-		unsigned short);	/*
-					 * ptr to actual interrupt
-					 * AI function
-					 */
-	unsigned char ai16bits;		/* =1 16 bit card */
 	unsigned char usedma;		/* =1 use DMA transfer and not INT */
-	unsigned char useeoshandle;	/*
-					 * =1 change WAKE_EOS DMA transfer
-					 * to fit on every second
-					 */
-	unsigned char usessh;		/* =1 turn on S&H support */
 	int softsshdelay;		/*
 					 * >0 use software S&H,
 					 * numer is requested delay in ns
@@ -411,7 +382,6 @@ struct pci9118_private {
 					 */
 	unsigned int ai_maskerr;	/* which warning was printed */
 	unsigned int ai_maskharderr;	/* on which error bits stops */
-	unsigned int ai_inttrig_start;	/* TRIG_INT for start */
 };
 
 static int check_channel_list(struct comedi_device *dev,
@@ -468,7 +438,7 @@ static int check_channel_list(struct comedi_device *dev,
 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, char useeos)
+			      int backadd, int usedma)
 {
 	struct pci9118_private *devpriv = dev->private;
 	unsigned int i, differencial = 0, bipolar = 0;
@@ -552,18 +522,6 @@ static int setup_channel_list(struct comedi_device *dev,
 #ifdef PCI9118_PARANOIDCHECK
 	devpriv->chanlist[n_chan ^ usedma] = devpriv->chanlist[0 ^ usedma];
 						/* for 32bit operations */
-	if (useeos) {
-		for (i = 1; i < n_chan; i++) {	/* store range list to card */
-			devpriv->chanlist[(n_chan + i) ^ usedma] =
-			    (CR_CHAN(chanlist[i]) & 0xf) << rot;
-		}
-		devpriv->chanlist[(2 * n_chan) ^ usedma] =
-						devpriv->chanlist[0 ^ usedma];
-						/* for 32bit operations */
-		useeos = 2;
-	} else {
-		useeos = 1;
-	}
 #endif
 	outl(0, dev->iobase + PCI9118_SCANMOD);	/* close scan queue */
 	/* udelay(100); important delay, or first sample will be crippled */
@@ -603,7 +561,7 @@ static int pci9118_insn_read_ai(struct comedi_device *dev,
 						 * trigger stop
 						 */
 
-	if (!setup_channel_list(dev, s, 1, &insn->chanspec, 0, 0, 0, 0, 0))
+	if (!setup_channel_list(dev, s, 1, &insn->chanspec, 0, 0, 0, 0))
 		return -EINVAL;
 
 	outl(0, dev->iobase + PCI9118_DELFIFO);	/* flush FIFO */
@@ -618,7 +576,7 @@ static int pci9118_insn_read_ai(struct comedi_device *dev,
 			return ret;
 		}
 
-		if (devpriv->ai16bits) {
+		if (s->maxdata == 0xffff) {
 			data[n] =
 			    (inl(dev->iobase +
 				 PCI9118_AD_DATA) & 0xffff) ^ 0x8000;
@@ -713,10 +671,11 @@ static unsigned int defragment_dma_buffer(struct comedi_device *dev,
 					  unsigned int num_samples)
 {
 	struct pci9118_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int i = 0, j = 0;
 	unsigned int start_pos = devpriv->ai_add_front,
-	    stop_pos = devpriv->ai_add_front + devpriv->ai_n_chan;
-	unsigned int raw_scanlen = devpriv->ai_add_front + devpriv->ai_n_chan +
+	    stop_pos = devpriv->ai_add_front + cmd->chanlist_len;
+	unsigned int raw_scanlen = devpriv->ai_add_front + cmd->chanlist_len +
 	    devpriv->ai_add_back;
 
 	for (i = 0; i < num_samples; i++) {
@@ -737,13 +696,14 @@ static int move_block_from_dma(struct comedi_device *dev,
 					unsigned int num_samples)
 {
 	struct pci9118_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int num_bytes;
 
 	num_samples = defragment_dma_buffer(dev, s, dma_buffer, num_samples);
 	devpriv->ai_act_scan +=
-	    (s->async->cur_chan + num_samples) / devpriv->ai_n_scanlen;
+	    (s->async->cur_chan + num_samples) / cmd->scan_end_arg;
 	s->async->cur_chan += num_samples;
-	s->async->cur_chan %= devpriv->ai_n_scanlen;
+	s->async->cur_chan %= cmd->scan_end_arg;
 	num_bytes =
 	    cfc_write_array_to_buffer(s, dma_buffer,
 				      num_samples * sizeof(short));
@@ -791,47 +751,51 @@ static void pci9118_calc_divisors(char mode, struct comedi_device *dev,
 				  unsigned int *tim1, unsigned int *tim2,
 				  unsigned int flags, int chans,
 				  unsigned int *div1, unsigned int *div2,
-				  char usessh, unsigned int chnsshfront)
+				  unsigned int chnsshfront)
 {
 	const struct boardtype *this_board = comedi_board(dev);
-	struct pci9118_private *devpriv = dev->private;
+	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(devpriv->i8254_osc_base,
+		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 / devpriv->i8254_osc_base;
+		*div1 = *tim2 / I8254_OSC_BASE_4MHZ;
 						/* convert timer (burst) */
 		if (*div1 < this_board->ai_pacer_min)
 			*div1 = this_board->ai_pacer_min;
-		*div2 = *tim1 / devpriv->i8254_osc_base;	/* scan timer */
+		*div2 = *tim1 / I8254_OSC_BASE_4MHZ;	/* scan timer */
 		*div2 = *div2 / *div1;		/* major timer is c1*c2 */
 		if (*div2 < chans)
 			*div2 = chans;
 
-		*tim2 = *div1 * devpriv->i8254_osc_base;
-							/* real convert timer */
+		*tim2 = *div1 * I8254_OSC_BASE_4MHZ;	/* real convert timer */
 
-		if (usessh && (chnsshfront == 0))	/* use BSSH signal */
+		if (cmd->convert_src == TRIG_NOW && !chnsshfront) {
+			/* use BSSH signal */
 			if (*div2 < (chans + 2))
 				*div2 = chans + 2;
+		}
 
-		*tim1 = *div1 * *div2 * devpriv->i8254_osc_base;
+		*tim1 = *div1 * *div2 * I8254_OSC_BASE_4MHZ;
 		break;
 	}
 }
 
-static void start_pacer(struct comedi_device *dev, int mode,
-			unsigned int divisor1, unsigned int divisor2)
+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); */
@@ -855,7 +819,7 @@ static int pci9118_ai_cancel(struct comedi_device *dev,
 			(~EN_A2P_TRANSFERS),
 			devpriv->iobase_a + AMCC_OP_REG_MCSR);	/* stop DMA */
 	pci9118_exttrg_del(dev, EXTTRG_AI);
-	start_pacer(dev, 0, 0, 0);	/* stop 8254 counters */
+	pci9118_start_pacer(dev, 0);	/* stop 8254 counters */
 	devpriv->AdFunctionReg = AdFunction_PDTrg | AdFunction_PETrg;
 	outl(devpriv->AdFunctionReg, dev->iobase + PCI9118_ADFUNC);
 					/*
@@ -882,7 +846,6 @@ static int pci9118_ai_cancel(struct comedi_device *dev,
 	devpriv->ai_act_dmapos = 0;
 	s->async->cur_chan = 0;
 	s->async->inttrig = NULL;
-	devpriv->ai_buf_ptr = 0;
 	devpriv->ai_neverending = 0;
 	devpriv->dma_actbuf = 0;
 
@@ -938,7 +901,7 @@ static void pci9118_ai_munge(struct comedi_device *dev,
 	for (i = 0; i < num_samples; i++) {
 		if (devpriv->usedma)
 			array[i] = be16_to_cpu(array[i]);
-		if (devpriv->ai16bits)
+		if (s->maxdata == 0xffff)
 			array[i] ^= 0x8000;
 		else
 			array[i] = (array[i] >> 4) & 0x0fff;
@@ -953,6 +916,7 @@ static void interrupt_pci9118_ai_onesample(struct comedi_device *dev,
 					   unsigned short int_daq)
 {
 	struct pci9118_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned short sampl;
 
 	if (int_adstat & devpriv->ai_maskerr)
@@ -962,7 +926,7 @@ static void interrupt_pci9118_ai_onesample(struct comedi_device *dev,
 	sampl = inw(dev->iobase + PCI9118_AD_DATA);
 
 #ifdef PCI9118_PARANOIDCHECK
-	if (devpriv->ai16bits == 0) {
+	if (s->maxdata != 0xffff) {
 		if ((sampl & 0x000f) != devpriv->chanlist[s->async->cur_chan]) {
 							/* data dropout! */
 			dev_info(dev->class_dev,
@@ -977,13 +941,13 @@ static void interrupt_pci9118_ai_onesample(struct comedi_device *dev,
 #endif
 	cfc_write_to_buffer(s, sampl);
 	s->async->cur_chan++;
-	if (s->async->cur_chan >= devpriv->ai_n_scanlen) {
+	if (s->async->cur_chan >= cmd->scan_end_arg) {
 							/* one scan done */
-		s->async->cur_chan %= devpriv->ai_n_scanlen;
+		s->async->cur_chan %= cmd->scan_end_arg;
 		devpriv->ai_act_scan++;
 		if (!devpriv->ai_neverending) {
 			/* all data sampled? */
-			if (devpriv->ai_act_scan >= devpriv->ai_scans)
+			if (devpriv->ai_act_scan >= cmd->stop_arg)
 				s->async->events |= COMEDI_CB_EOA;
 		}
 	}
@@ -998,6 +962,7 @@ static void interrupt_pci9118_ai_dma(struct comedi_device *dev,
 				     unsigned short int_daq)
 {
 	struct pci9118_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int next_dma_buf, samplesinbuf, sampls, m;
 
 	if (int_amcc & MASTER_ABORT_INT) {
@@ -1037,20 +1002,18 @@ static void interrupt_pci9118_ai_dma(struct comedi_device *dev,
 	}
 
 	if (samplesinbuf) {
-		m = devpriv->ai_data_len >> 1;	/*
-						 * how many samples is to
-						 * end of buffer
-						 */
+		/* 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);
-		m = m - sampls;		/* m= how many samples was transferred */
+		m = m - sampls;		/* m=how many samples was transferred */
 	}
 
 	if (!devpriv->ai_neverending) {
 		/* all data sampled? */
-		if (devpriv->ai_act_scan >= devpriv->ai_scans)
+		if (devpriv->ai_act_scan >= cmd->stop_arg)
 			s->async->events |= COMEDI_CB_EOA;
 	}
 
@@ -1068,78 +1031,71 @@ static void interrupt_pci9118_ai_dma(struct comedi_device *dev,
 	cfc_handle_events(dev, s);
 }
 
-static irqreturn_t interrupt_pci9118(int irq, void *d)
+static irqreturn_t pci9118_interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = d;
+	struct comedi_subdevice *s = dev->read_subdev;
 	struct pci9118_private *devpriv = dev->private;
-	unsigned int int_daq = 0, int_amcc, int_adstat;
+	unsigned int intsrc;	/* IRQ reasons from card */
+	unsigned int intcsr;	/* INT register from AMCC chip */
+	unsigned int adstat;	/* STATUS register */
 
 	if (!dev->attached)
-		return IRQ_NONE;	/* not fully initialized */
-
-	int_daq = inl(dev->iobase + PCI9118_INTSRC) & 0xf;
-					/* get IRQ reasons from card */
-	int_amcc = inl(devpriv->iobase_a + AMCC_OP_REG_INTCSR);
-					/* get INT register from AMCC chip */
-
-	if ((!int_daq) && (!(int_amcc & ANY_S593X_INT)))
-		return IRQ_NONE;	/* interrupt from other source */
-
-	outl(int_amcc | 0x00ff0000, devpriv->iobase_a + AMCC_OP_REG_INTCSR);
-					/* shutdown IRQ reasons in AMCC */
-
-	int_adstat = inw(dev->iobase + PCI9118_ADSTAT) & 0x1ff;
-					/* get STATUS register */
-
-	if (devpriv->ai_do) {
-		if (devpriv->ai12_startstop)
-			if ((int_adstat & AdStatus_DTH) &&
-							(int_daq & Int_DTrg)) {
-						/* start stop of measure */
-				if (devpriv->ai12_startstop & START_AI_EXT) {
-					devpriv->ai12_startstop &=
-					    ~START_AI_EXT;
-					if (!(devpriv->ai12_startstop &
-							STOP_AI_EXT))
-							pci9118_exttrg_del
-							(dev, EXTTRG_AI);
-						/* deactivate EXT trigger */
-					start_pacer(dev, devpriv->ai_do,
-						devpriv->ai_divisor1,
-						devpriv->ai_divisor2);
-						/* start pacer */
-					outl(devpriv->AdControlReg,
-						dev->iobase + PCI9118_ADCNTRL);
-				} else {
-					if (devpriv->ai12_startstop &
-						STOP_AI_EXT) {
-						devpriv->ai12_startstop &=
-							~STOP_AI_EXT;
-						pci9118_exttrg_del
-							(dev, EXTTRG_AI);
-						/* deactivate EXT trigger */
-						devpriv->ai_neverending = 0;
-						/*
-						 * well, on next interrupt from
-						 * DMA/EOC measure will stop
-						 */
-					}
-				}
-			}
+		return IRQ_NONE;
+
+	intsrc = inl(dev->iobase + PCI9118_INTSRC) & 0xf;
+	intcsr = inl(devpriv->iobase_a + AMCC_OP_REG_INTCSR);
+
+	if (!intsrc && !(intcsr & ANY_S593X_INT))
+		return IRQ_NONE;
+
+	outl(intcsr | 0x00ff0000, devpriv->iobase_a + AMCC_OP_REG_INTCSR);
+
+	adstat = inw(dev->iobase + PCI9118_ADSTAT) & 0x1ff;
 
-		(devpriv->int_ai_func) (dev, dev->read_subdev, int_adstat,
-					int_amcc, int_daq);
+	if (!devpriv->ai_do)
+		return IRQ_HANDLED;
 
+	if (devpriv->ai12_startstop) {
+		if ((adstat & AdStatus_DTH) && (intsrc & Int_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);
+
+				/* start pacer */
+				pci9118_start_pacer(dev, devpriv->ai_do);
+				outl(devpriv->AdControlReg,
+				     dev->iobase + PCI9118_ADCNTRL);
+			} else if (devpriv->ai12_startstop & STOP_AI_EXT) {
+				/* deactivate EXT trigger */
+				devpriv->ai12_startstop &= ~STOP_AI_EXT;
+				pci9118_exttrg_del(dev, EXTTRG_AI);
+
+				/* on next interrupt measure will stop */
+				devpriv->ai_neverending = 0;
+			}
+		}
 	}
+
+	if (devpriv->usedma)
+		interrupt_pci9118_ai_dma(dev, s, adstat, intcsr, intsrc);
+	else
+		interrupt_pci9118_ai_onesample(dev, s, adstat, intcsr, intsrc);
+
 	return IRQ_HANDLED;
 }
 
 static int pci9118_ai_inttrig(struct comedi_device *dev,
-			      struct comedi_subdevice *s, unsigned int trignum)
+			      struct comedi_subdevice *s,
+			      unsigned int trig_num)
 {
 	struct pci9118_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 
-	if (trignum != devpriv->ai_inttrig_start)
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	devpriv->ai12_startstop &= ~START_AI_INT;
@@ -1148,8 +1104,7 @@ static int pci9118_ai_inttrig(struct comedi_device *dev,
 	outl(devpriv->IntControlReg, dev->iobase + PCI9118_INTCTRL);
 	outl(devpriv->AdFunctionReg, dev->iobase + PCI9118_ADFUNC);
 	if (devpriv->ai_do != 3) {
-		start_pacer(dev, devpriv->ai_do, devpriv->ai_divisor1,
-			    devpriv->ai_divisor2);
+		pci9118_start_pacer(dev, devpriv->ai_do);
 		devpriv->AdControlReg |= AdControl_SoftG;
 	}
 	outl(devpriv->AdControlReg, dev->iobase + PCI9118_ADCNTRL);
@@ -1165,7 +1120,7 @@ static int pci9118_ai_cmdtest(struct comedi_device *dev,
 	struct pci9118_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int flags;
-	int tmp;
+	unsigned int arg;
 	unsigned int divisor1 = 0, divisor2 = 0;
 
 	/* Step 1 : check if triggers are trivially valid */
@@ -1221,8 +1176,15 @@ static int pci9118_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	if (cmd->start_src & (TRIG_NOW | TRIG_EXT))
+	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);
@@ -1259,8 +1221,6 @@ static int pci9118_ai_cmdtest(struct comedi_device *dev,
 		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_max(&cmd->chanlist_len,
-					 this_board->n_aichanlist);
 
 	err |= cfc_check_trigger_arg_min(&cmd->scan_end_arg,
 					 cmd->chanlist_len);
@@ -1277,45 +1237,30 @@ static int pci9118_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
-		i8253_cascade_ns_to_timer(devpriv->i8254_osc_base,
+		arg = cmd->scan_begin_arg;
+		i8253_cascade_ns_to_timer(I8254_OSC_BASE_4MHZ,
 					  &divisor1, &divisor2,
-					  &cmd->scan_begin_arg, cmd->flags);
-		if (cmd->scan_begin_arg < this_board->ai_ns_min)
-			cmd->scan_begin_arg = this_board->ai_ns_min;
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (cmd->convert_src & (TRIG_TIMER | TRIG_NOW)) {
-		tmp = cmd->convert_arg;
-		i8253_cascade_ns_to_timer(devpriv->i8254_osc_base,
+		arg = cmd->convert_arg;
+		i8253_cascade_ns_to_timer(I8254_OSC_BASE_4MHZ,
 					  &divisor1, &divisor2,
-					  &cmd->convert_arg, cmd->flags);
-		if (cmd->convert_arg < this_board->ai_ns_min)
-			cmd->convert_arg = this_board->ai_ns_min;
-		if (tmp != cmd->convert_arg)
-			err++;
-		if (cmd->scan_begin_src == TRIG_TIMER
-		    && cmd->convert_src == TRIG_NOW) {
+					  &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) {
-				if (cmd->scan_begin_arg <
-				    this_board->ai_ns_min *
-				    (cmd->scan_end_arg + 2)) {
-					cmd->scan_begin_arg =
-					    this_board->ai_ns_min *
-					    (cmd->scan_end_arg + 2);
-					err++;
-				}
+				arg = this_board->ai_ns_min *
+				      (cmd->scan_end_arg + 2);
 			} else {
-				if (cmd->scan_begin_arg <
-				    cmd->convert_arg * cmd->chanlist_len) {
-					cmd->scan_begin_arg =
-					    cmd->convert_arg *
-					    cmd->chanlist_len;
-					err++;
-				}
+				arg = cmd->convert_arg * cmd->chanlist_len;
 			}
+			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
+							 arg);
 		}
 	}
 
@@ -1330,23 +1275,23 @@ static int pci9118_ai_cmdtest(struct comedi_device *dev,
 	return 0;
 }
 
-static int Compute_and_setup_dma(struct comedi_device *dev)
+static int Compute_and_setup_dma(struct comedi_device *dev,
+				 struct comedi_subdevice *s)
 {
 	struct pci9118_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int dmalen0, dmalen1, i;
 
 	dmalen0 = devpriv->dmabuf_size[0];
 	dmalen1 = devpriv->dmabuf_size[1];
 	/* isn't output buff smaller that our DMA buff? */
-	if (dmalen0 > (devpriv->ai_data_len)) {
-		dmalen0 = devpriv->ai_data_len & ~3L;	/*
-							 * align to 32bit down
-							 */
+	if (dmalen0 > s->async->prealloc_bufsz) {
+		/* align to 32bit down */
+		dmalen0 = s->async->prealloc_bufsz & ~3L;
 	}
-	if (dmalen1 > (devpriv->ai_data_len)) {
-		dmalen1 = devpriv->ai_data_len & ~3L;	/*
-							 * align to 32bit down
-							 */
+	if (dmalen1 > s->async->prealloc_bufsz) {
+		/* align to 32bit down */
+		dmalen1 = s->async->prealloc_bufsz & ~3L;
 	}
 
 	/* we want wake up every scan? */
@@ -1360,8 +1305,6 @@ static int Compute_and_setup_dma(struct comedi_device *dev)
 		} else {
 			/* short first DMA buffer to one scan */
 			dmalen0 = devpriv->ai_n_realscanlen << 1;
-			if (devpriv->useeoshandle)
-				dmalen0 += 2;
 			if (dmalen0 < 4) {
 				dev_info(dev->class_dev,
 					 "ERR: DMA0 buf len bug? (%d<4)\n",
@@ -1380,8 +1323,6 @@ static int Compute_and_setup_dma(struct comedi_device *dev)
 		} else {
 			/* short second DMA buffer to one scan */
 			dmalen1 = devpriv->ai_n_realscanlen << 1;
-			if (devpriv->useeoshandle)
-				dmalen1 -= 2;
 			if (dmalen1 < 4) {
 				dev_info(dev->class_dev,
 					 "ERR: DMA1 buf len bug? (%d<4)\n",
@@ -1416,10 +1357,10 @@ static int Compute_and_setup_dma(struct comedi_device *dev)
 			/* fits whole measure into one DMA buffer? */
 			if (dmalen0 >
 			    ((devpriv->ai_n_realscanlen << 1) *
-			     devpriv->ai_scans)) {
+			     cmd->stop_arg)) {
 				dmalen0 =
 				    (devpriv->ai_n_realscanlen << 1) *
-				    devpriv->ai_scans;
+				    cmd->stop_arg;
 				dmalen0 &= ~3L;
 			} else {	/*
 					 * fits whole measure into
@@ -1427,10 +1368,10 @@ static int Compute_and_setup_dma(struct comedi_device *dev)
 					 */
 				if (dmalen1 >
 				    ((devpriv->ai_n_realscanlen << 1) *
-				     devpriv->ai_scans - dmalen0))
+				     cmd->stop_arg - dmalen0))
 					dmalen1 =
 					    (devpriv->ai_n_realscanlen << 1) *
-					    devpriv->ai_scans - dmalen0;
+					    cmd->stop_arg - dmalen0;
 				dmalen1 &= ~3L;
 			}
 		}
@@ -1442,18 +1383,18 @@ static int Compute_and_setup_dma(struct comedi_device *dev)
 	devpriv->dmabuf_use_size[1] = dmalen1;
 
 #if 0
-	if (devpriv->ai_n_scanlen < this_board->half_fifo_size) {
+	if (cmd->scan_end_arg < this_board->half_fifo_size) {
 		devpriv->dmabuf_panic_size[0] =
-		    (this_board->half_fifo_size / devpriv->ai_n_scanlen +
-		     1) * devpriv->ai_n_scanlen * sizeof(short);
+		    (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 / devpriv->ai_n_scanlen +
-		     1) * devpriv->ai_n_scanlen * sizeof(short);
+		    (this_board->half_fifo_size / cmd->scan_end_arg +
+		     1) * cmd->scan_end_arg * sizeof(short);
 	} else {
 		devpriv->dmabuf_panic_size[0] =
-		    (devpriv->ai_n_scanlen << 1) % devpriv->dmabuf_size[0];
+		    (cmd->scan_end_arg << 1) % devpriv->dmabuf_size[0];
 		devpriv->dmabuf_panic_size[1] =
-		    (devpriv->ai_n_scanlen << 1) % devpriv->dmabuf_size[1];
+		    (cmd->scan_end_arg << 1) % devpriv->dmabuf_size[1];
 	}
 #endif
 
@@ -1500,9 +1441,6 @@ static int pci9118_ai_docmd_sampl(struct comedi_device *dev,
 		return -EIO;
 	}
 
-	devpriv->int_ai_func = interrupt_pci9118_ai_onesample;
-						/* transfer function */
-
 	if (devpriv->ai12_startstop)
 		pci9118_exttrg_add(dev, EXTTRG_AI);
 						/* activate EXT trigger */
@@ -1520,8 +1458,7 @@ static int pci9118_ai_docmd_sampl(struct comedi_device *dev,
 		outl(devpriv->IntControlReg, dev->iobase + PCI9118_INTCTRL);
 		outl(devpriv->AdFunctionReg, dev->iobase + PCI9118_ADFUNC);
 		if (devpriv->ai_do != 3) {
-			start_pacer(dev, devpriv->ai_do, devpriv->ai_divisor1,
-				    devpriv->ai_divisor2);
+			pci9118_start_pacer(dev, devpriv->ai_do);
 			devpriv->AdControlReg |= AdControl_SoftG;
 		}
 		outl(devpriv->IntControlReg, dev->iobase + PCI9118_INTCTRL);
@@ -1534,8 +1471,9 @@ 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);
+	Compute_and_setup_dma(dev, s);
 
 	switch (devpriv->ai_do) {
 	case 1:
@@ -1548,7 +1486,7 @@ static int pci9118_ai_docmd_dma(struct comedi_device *dev,
 		devpriv->AdFunctionReg =
 		    AdFunction_PDTrg | AdFunction_PETrg | AdFunction_BM |
 		    AdFunction_BS;
-		if (devpriv->usessh && (!devpriv->softsshdelay))
+		if (cmd->convert_src == TRIG_NOW && !devpriv->softsshdelay)
 			devpriv->AdFunctionReg |= AdFunction_BSSH;
 		outl(devpriv->ai_n_realscanlen, dev->iobase + PCI9118_BURST);
 		break;
@@ -1580,9 +1518,6 @@ static int pci9118_ai_docmd_dma(struct comedi_device *dev,
 						/* activate EXT trigger */
 	}
 
-	devpriv->int_ai_func = interrupt_pci9118_ai_dma;
-						/* transfer function */
-
 	outl(0x02000000 | AINT_WRITE_COMPL,
 	     devpriv->iobase_a + AMCC_OP_REG_INTCSR);
 
@@ -1590,8 +1525,7 @@ static int pci9118_ai_docmd_dma(struct comedi_device *dev,
 		outl(devpriv->AdFunctionReg, dev->iobase + PCI9118_ADFUNC);
 		outl(devpriv->IntControlReg, dev->iobase + PCI9118_INTCTRL);
 		if (devpriv->ai_do != 3) {
-			start_pacer(dev, devpriv->ai_do, devpriv->ai_divisor1,
-				    devpriv->ai_divisor2);
+			pci9118_start_pacer(dev, devpriv->ai_do);
 			devpriv->AdControlReg |= AdControl_SoftG;
 		}
 		outl(devpriv->AdControlReg, dev->iobase + PCI9118_ADCNTRL);
@@ -1610,12 +1544,6 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 	devpriv->ai12_startstop = 0;
 	devpriv->ai_flags = cmd->flags;
-	devpriv->ai_n_chan = cmd->chanlist_len;
-	devpriv->ai_n_scanlen = cmd->scan_end_arg;
-	devpriv->ai_chanlist = cmd->chanlist;
-	devpriv->ai_data_len = s->async->prealloc_bufsz;
-	devpriv->ai_timer1 = 0;
-	devpriv->ai_timer2 = 0;
 	devpriv->ai_add_front = 0;
 	devpriv->ai_add_back = 0;
 	devpriv->ai_maskerr = 0x10e;
@@ -1629,31 +1557,12 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	}
 	if (cmd->start_src == TRIG_INT) {
 		devpriv->ai12_startstop |= START_AI_INT;
-		devpriv->ai_inttrig_start = cmd->start_arg;
 		s->async->inttrig = pci9118_ai_inttrig;
 	}
-#if 0
-	if (cmd->stop_src == TRIG_INT) {
-		devpriv->ai_neverending = 1;
-		devpriv->ai12_startstop |= STOP_AI_INT;
-	}
-#endif
 	if (cmd->stop_src == TRIG_NONE)
 		devpriv->ai_neverending = 1;
-	if (cmd->stop_src == TRIG_COUNT) {
-		devpriv->ai_scans = cmd->stop_arg;
+	if (cmd->stop_src == TRIG_COUNT)
 		devpriv->ai_neverending = 0;
-	} else {
-		devpriv->ai_scans = 0;
-	}
-
-	/* use sample&hold signal? */
-	if (cmd->convert_src == TRIG_NOW)
-		devpriv->usessh = 1;
-	/* yes */
-	else
-		devpriv->usessh = 0;
-				/*  no */
 
 	/*
 	 * use additional sample at end of every scan
@@ -1661,11 +1570,10 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	 */
 	devpriv->ai_add_front = 0;
 	devpriv->ai_add_back = 0;
-	devpriv->useeoshandle = 0;
 	if (devpriv->master) {
 		devpriv->usedma = 1;
 		if ((cmd->flags & TRIG_WAKE_EOS) &&
-		    (devpriv->ai_n_scanlen == 1)) {
+		    (cmd->scan_end_arg == 1)) {
 			if (cmd->convert_src == TRIG_NOW)
 				devpriv->ai_add_back = 1;
 			if (cmd->convert_src == TRIG_TIMER) {
@@ -1677,13 +1585,9 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 			}
 		}
 		if ((cmd->flags & TRIG_WAKE_EOS) &&
-		    (devpriv->ai_n_scanlen & 1) &&
-		    (devpriv->ai_n_scanlen > 1)) {
+		    (cmd->scan_end_arg & 1) &&
+		    (cmd->scan_end_arg > 1)) {
 			if (cmd->scan_begin_src == TRIG_FOLLOW) {
-				/*
-				 * vpriv->useeoshandle=1; // change DMA transfer
-				 * block to fit EOS on every second call
-				 */
 				devpriv->usedma = 0;
 				/*
 				 * XXX maybe can be corrected to use 16 bit DMA
@@ -1703,7 +1607,7 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	 * we need software S&H signal?
 	 * It adds two samples before every scan as minimum
 	 */
-	if (devpriv->usessh && devpriv->softsshdelay) {
+	if (cmd->convert_src == TRIG_NOW && devpriv->softsshdelay) {
 		devpriv->ai_add_front = 2;
 		if ((devpriv->usedma == 1) && (devpriv->ai_add_back == 1)) {
 							/* move it to front */
@@ -1720,7 +1624,7 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 			devpriv->ai_add_front = addchans + 1;
 			if (devpriv->usedma == 1)
 				if ((devpriv->ai_add_front +
-				     devpriv->ai_n_chan +
+				     cmd->chanlist_len +
 				     devpriv->ai_add_back) & 1)
 					devpriv->ai_add_front++;
 							/* round up to 32 bit */
@@ -1729,21 +1633,20 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	/* well, we now know what must be all added */
 	devpriv->ai_n_realscanlen =	/*
 					 * what we must take from card in real
-					 * to have ai_n_scanlen on output?
+					 * to have cmd->scan_end_arg on output?
 					 */
-	    (devpriv->ai_add_front + devpriv->ai_n_chan +
-	     devpriv->ai_add_back) * (devpriv->ai_n_scanlen /
-				      devpriv->ai_n_chan);
+	    (devpriv->ai_add_front + cmd->chanlist_len +
+	     devpriv->ai_add_back) * (cmd->scan_end_arg /
+				      cmd->chanlist_len);
 
 	/* check and setup channel list */
-	if (!check_channel_list(dev, s, devpriv->ai_n_chan,
-				devpriv->ai_chanlist, devpriv->ai_add_front,
+	if (!check_channel_list(dev, s, cmd->chanlist_len,
+				cmd->chanlist, devpriv->ai_add_front,
 				devpriv->ai_add_back))
 		return -EINVAL;
-	if (!setup_channel_list(dev, s, devpriv->ai_n_chan,
-				devpriv->ai_chanlist, 0, devpriv->ai_add_front,
-				devpriv->ai_add_back, devpriv->usedma,
-				devpriv->useeoshandle))
+	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 */
@@ -1765,9 +1668,8 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 				      devpriv->ai_flags,
 				      devpriv->ai_n_realscanlen,
 				      &devpriv->ai_divisor1,
-				      &devpriv->ai_divisor2, devpriv->usessh,
+				      &devpriv->ai_divisor2,
 				      devpriv->ai_add_front);
-		devpriv->ai_timer2 = cmd->convert_arg;
 	}
 
 	if ((cmd->scan_begin_src == TRIG_TIMER) &&
@@ -1787,10 +1689,8 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 				      devpriv->ai_flags,
 				      devpriv->ai_n_realscanlen,
 				      &devpriv->ai_divisor1,
-				      &devpriv->ai_divisor2, devpriv->usessh,
+				      &devpriv->ai_divisor2,
 				      devpriv->ai_add_front);
-		devpriv->ai_timer1 = cmd->scan_begin_arg;
-		devpriv->ai_timer2 = cmd->convert_arg;
 	}
 
 	if ((cmd->scan_begin_src == TRIG_FOLLOW)
@@ -1798,7 +1698,7 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		devpriv->ai_do = 3;
 	}
 
-	start_pacer(dev, -1, 0, 0);	/* stop pacer */
+	pci9118_start_pacer(dev, -1);	/* stop pacer */
 
 	devpriv->AdControlReg = 0;	/*
 					 * bipolar, S.E., use 8254, stop 8354,
@@ -1824,7 +1724,6 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	devpriv->ai_act_scan = 0;
 	devpriv->ai_act_dmapos = 0;
 	s->async->cur_chan = 0;
-	devpriv->ai_buf_ptr = 0;
 
 	if (devpriv->usedma)
 		ret = pci9118_ai_docmd_dma(dev, s);
@@ -1845,7 +1744,7 @@ static int pci9118_reset(struct comedi_device *dev)
 						/* disable interrupts source */
 	outl(0x30, dev->iobase + PCI9118_CNTCTRL);
 /* outl(0xb4, dev->iobase + PCI9118_CNTCTRL); */
-	start_pacer(dev, 0, 0, 0);		/* stop 8254 counters */
+	pci9118_start_pacer(dev, 0);		/* stop 8254 counters */
 	devpriv->AdControlReg = 0;
 	outl(devpriv->AdControlReg, dev->iobase + PCI9118_ADCNTRL);
 						/*
@@ -1888,7 +1787,6 @@ static int pci9118_reset(struct comedi_device *dev)
 						 * disable INT and DMA
 						 */
 
-	devpriv->cnt0_users = 0;
 	devpriv->exttrg_users = 0;
 
 	return 0;
@@ -1971,8 +1869,6 @@ static int pci9118_common_attach(struct comedi_device *dev, int disable_irq,
 			if (devpriv->dmabuf_virt[i]) {
 				devpriv->dmabuf_pages[i] = pages;
 				devpriv->dmabuf_size[i] = PAGE_SIZE * pages;
-				devpriv->dmabuf_samples[i] =
-				    devpriv->dmabuf_size[i] >> 1;
 				devpriv->dmabuf_hw[i] =
 				    virt_to_bus((void *)
 						devpriv->dmabuf_virt[i]);
@@ -2015,7 +1911,7 @@ static int pci9118_common_attach(struct comedi_device *dev, int disable_irq,
 				/* Enable parity check for parity error */
 
 	if (!disable_irq && pcidev->irq) {
-		ret = request_irq(pcidev->irq, interrupt_pci9118, IRQF_SHARED,
+		ret = request_irq(pcidev->irq, pci9118_interrupt, IRQF_SHARED,
 				  dev->board_name, dev);
 		if (ret == 0)
 			dev->irq = pcidev->irq;
@@ -2074,22 +1970,11 @@ static int pci9118_common_attach(struct comedi_device *dev, int disable_irq,
 	s->range_table = &range_digital;
 	s->insn_bits = pci9118_insn_bits_do;
 
-	devpriv->valid = 1;
-	devpriv->i8254_osc_base = I8254_OSC_BASE_4MHZ;
 	devpriv->ai_maskharderr = 0x10a;
 					/* default measure crash condition */
 	if (hw_err_mask)		/* disable some requested */
 		devpriv->ai_maskharderr &= ~hw_err_mask;
 
-	switch (this_board->ai_maxdata) {
-	case 0xffff:
-		devpriv->ai16bits = 1;
-		break;
-	default:
-		devpriv->ai16bits = 0;
-		break;
-	}
-
 	return 0;
 }
 
@@ -2152,7 +2037,7 @@ static void pci9118_detach(struct comedi_device *dev)
 	struct pci9118_private *devpriv = dev->private;
 
 	if (devpriv) {
-		if (devpriv->valid)
+		if (dev->iobase)
 			pci9118_reset(dev);
 		if (dev->irq)
 			free_irq(dev->irq, dev);
diff --git a/drivers/staging/comedi/drivers/adv_pci1710.c b/drivers/staging/comedi/drivers/adv_pci1710.c
index 28ec4854831..602b7a1e40e 100644
--- a/drivers/staging/comedi/drivers/adv_pci1710.c
+++ b/drivers/staging/comedi/drivers/adv_pci1710.c
@@ -51,10 +51,6 @@ Configuration options:
 #include "8253.h"
 #include "amcc_s5933.h"
 
-#define PCI171x_PARANOIDCHECK	/* if defined, then is used code which control
-				 * correct channel number on every 12 bit
-				 * sample */
-
 /* hardware types of the cards */
 #define TYPE_PCI171X	0
 #define TYPE_PCI1713	2
@@ -73,6 +69,9 @@ Configuration options:
 #define PCI171x_DAREF	14	/* W:   D/A reference control */
 #define PCI171x_DI	16	/* R:   digi inputs */
 #define PCI171x_DO	16	/* R:   digi inputs */
+
+#define PCI171X_TIMER_BASE	0x18
+
 #define PCI171x_CNT0	24	/* R/W: 8254 counter 0 */
 #define PCI171x_CNT1	26	/* R/W: 8254 counter 1 */
 #define PCI171x_CNT2	28	/* R/W: 8254 counter 2 */
@@ -298,29 +297,18 @@ static const struct boardtype boardtypes[] = {
 };
 
 struct pci1710_private {
-	char neverending_ai;	/*  we do unlimited AI */
 	unsigned int CntrlReg;	/*  Control register */
-	unsigned int i8254_osc_base;	/*  frequence of onboard oscilator */
-	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_act_chan;	/*  actual position in actual scan */
-	unsigned int ai_buf_ptr;	/*  data buffer ptr in samples */
-	unsigned char ai_eos;	/*  1=EOS wake up */
 	unsigned char ai_et;
 	unsigned int ai_et_CntrlReg;
 	unsigned int ai_et_MuxVal;
-	unsigned int ai_et_div1, ai_et_div2;
+	unsigned int next_divisor1;
+	unsigned int next_divisor2;
+	unsigned int divisor1;
+	unsigned int divisor2;
 	unsigned int act_chanlist[32];	/*  list of scanned channel */
-	unsigned char act_chanlist_len;	/*  len of scanlist */
-	unsigned char act_chanlist_pos;	/*  actual position in MUX list */
+	unsigned char saved_seglen;	/* len of the non-repeating chanlist */
 	unsigned char da_ranges;	/*  copy of D/A outpit range register */
-	unsigned int ai_scans;	/*  len of scanlist */
-	unsigned int ai_n_chan;	/*  how many channels is measured */
-	unsigned int *ai_chanlist;	/*  actaul chanlist */
-	unsigned int ai_flags;	/*  flaglist */
-	unsigned int ai_data_len;	/*  len of data buffer */
-	unsigned int ai_timer1;	/*  timers */
-	unsigned int ai_timer2;
 	unsigned short ao_data[4];	/*  data output buffer */
 	unsigned int cnt0_write_wait;	/* after a write, wait for update of the
 					 * internal state */
@@ -334,62 +322,90 @@ static const unsigned int muxonechan[] = {
 	0x1818, 0x1919, 0x1a1a, 0x1b1b, 0x1c1c, 0x1d1d, 0x1e1e, 0x1f1f
 };
 
-/*
-==============================================================================
- Check if channel list from user is built correctly
- If it's ok, then program scan/gain logic.
- This works for all cards.
-*/
-static int check_channel_list(struct comedi_device *dev,
+static int pci171x_ai_dropout(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
-			      unsigned int *chanlist, unsigned int n_chan)
+			      unsigned int chan,
+			      unsigned int val)
+{
+	const struct boardtype *board = comedi_board(dev);
+	struct pci1710_private *devpriv = dev->private;
+
+	if (board->cardtype != TYPE_PCI1713) {
+		if ((val & 0xf000) != devpriv->act_chanlist[chan]) {
+			dev_err(dev->class_dev,
+				"A/D data droput: received from channel %d, expected %d\n",
+				(val >> 12) & 0xf,
+				(devpriv->act_chanlist[chan] >> 12) & 0xf);
+			return -ENODATA;
+		}
+	}
+	return 0;
+}
+
+static int pci171x_ai_check_chanlist(struct comedi_device *dev,
+				     struct comedi_subdevice *s,
+				     struct comedi_cmd *cmd)
 {
+	struct pci1710_private *devpriv = dev->private;
+	unsigned int chan0 = CR_CHAN(cmd->chanlist[0]);
+	unsigned int last_aref = CR_AREF(cmd->chanlist[0]);
+	unsigned int next_chan = (chan0 + 1) % s->n_chan;
 	unsigned int chansegment[32];
-	unsigned int i, nowmustbechan, seglen, segpos;
+	unsigned int seglen;
+	int i;
 
-	/* correct channel and range number check itself comedi/range.c */
-	if (n_chan < 1) {
-		comedi_error(dev, "range/channel list is empty!");
+	if (cmd->chanlist_len == 1) {
+		devpriv->saved_seglen = cmd->chanlist_len;
 		return 0;
 	}
 
-	if (n_chan == 1)
-		return 1; /* seglen=1 */
+	/* first channel is always ok */
+	chansegment[0] = cmd->chanlist[0];
 
-	chansegment[0] = chanlist[0]; /*  first channel is every time ok */
-	for (i = 1, seglen = 1; i < n_chan; i++, seglen++) {
-		if (chanlist[0] == chanlist[i])
+	for (i = 1; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+		unsigned int aref = CR_AREF(cmd->chanlist[i]);
+
+		if (cmd->chanlist[0] == cmd->chanlist[i])
 			break;	/*  we detected a loop, stop */
-		if ((CR_CHAN(chanlist[i]) & 1) &&
-		    (CR_AREF(chanlist[i]) == AREF_DIFF)) {
-			comedi_error(dev, "Odd channel cannot be differential input!\n");
-			return 0;
+
+		if (aref == AREF_DIFF && (chan & 1)) {
+			dev_err(dev->class_dev,
+				"Odd channel cannot be differential input!\n");
+			return -EINVAL;
 		}
-		nowmustbechan = (CR_CHAN(chansegment[i - 1]) + 1) % s->n_chan;
-		if (CR_AREF(chansegment[i - 1]) == AREF_DIFF)
-			nowmustbechan = (nowmustbechan + 1) % s->n_chan;
-		if (nowmustbechan != CR_CHAN(chanlist[i])) {
-			printk("channel list must be continuous! chanlist[%i]=%d but must be %d or %d!\n",
-			       i, CR_CHAN(chanlist[i]), nowmustbechan,
-			       CR_CHAN(chanlist[0]));
-			return 0;
+
+		if (last_aref == AREF_DIFF)
+			next_chan = (next_chan + 1) % s->n_chan;
+		if (chan != next_chan) {
+			dev_err(dev->class_dev,
+				"channel list must be continuous! chanlist[%i]=%d but must be %d or %d!\n",
+				i, chan, next_chan, chan0);
+			return -EINVAL;
 		}
-		chansegment[i] = chanlist[i]; /* next correct channel in list */
-	}
 
-	for (i = 0, segpos = 0; i < n_chan; i++) {
-		if (chanlist[i] != chansegment[i % seglen]) {
-			printk("bad channel, reference or range number! chanlist[%i]=%d,%d,%d and not %d,%d,%d!\n",
-			       i, CR_CHAN(chansegment[i]),
-			       CR_RANGE(chansegment[i]),
-			       CR_AREF(chansegment[i]),
-			       CR_CHAN(chanlist[i % seglen]),
-			       CR_RANGE(chanlist[i % seglen]),
-			       CR_AREF(chansegment[i % seglen]));
-			return 0;
+		/* next correct channel in list */
+		chansegment[i] = cmd->chanlist[i];
+		last_aref = aref;
+	}
+	seglen = i;
+
+	for (i = 0; i < cmd->chanlist_len; i++) {
+		if (cmd->chanlist[i] != chansegment[i % seglen]) {
+			dev_err(dev->class_dev,
+				"bad channel, reference or range number! chanlist[%i]=%d,%d,%d and not %d,%d,%d!\n",
+				i, CR_CHAN(chansegment[i]),
+				CR_RANGE(chansegment[i]),
+				CR_AREF(chansegment[i]),
+				CR_CHAN(cmd->chanlist[i % seglen]),
+				CR_RANGE(cmd->chanlist[i % seglen]),
+				CR_AREF(chansegment[i % seglen]));
+			return -EINVAL;
 		}
 	}
-	return seglen;
+	devpriv->saved_seglen = seglen;
+
+	return 0;
 }
 
 static void setup_channel_list(struct comedi_device *dev,
@@ -401,9 +417,6 @@ static void setup_channel_list(struct comedi_device *dev,
 	struct pci1710_private *devpriv = dev->private;
 	unsigned int i, range, chanprog;
 
-	devpriv->act_chanlist_len = seglen;
-	devpriv->act_chanlist_pos = 0;
-
 	for (i = 0; i < seglen; i++) {	/*  store range list to card */
 		chanprog = muxonechan[CR_CHAN(chanlist[i])];
 		outw(chanprog, dev->iobase + PCI171x_MUX); /* select channel */
@@ -411,17 +424,13 @@ static void setup_channel_list(struct comedi_device *dev,
 		if (CR_AREF(chanlist[i]) == AREF_DIFF)
 			range |= 0x0020;
 		outw(range, dev->iobase + PCI171x_RANGE); /* select gain */
-#ifdef PCI171x_PARANOIDCHECK
 		devpriv->act_chanlist[i] =
 			(CR_CHAN(chanlist[i]) << 12) & 0xf000;
-#endif
 	}
-#ifdef PCI171x_PARANOIDCHECK
 	for ( ; i < n_chan; i++) { /* store remainder of channel list */
 		devpriv->act_chanlist[i] =
 			(CR_CHAN(chanlist[i]) << 12) & 0xf000;
 	}
-#endif
 
 	devpriv->ai_et_MuxVal =
 		CR_CHAN(chanlist[0]) | (CR_CHAN(chanlist[seglen - 1]) << 8);
@@ -447,12 +456,9 @@ static int pci171x_insn_read_ai(struct comedi_device *dev,
 				struct comedi_insn *insn, unsigned int *data)
 {
 	struct pci1710_private *devpriv = dev->private;
-	int ret;
-	int n;
-#ifdef PCI171x_PARANOIDCHECK
-	const struct boardtype *this_board = comedi_board(dev);
-	unsigned int idata;
-#endif
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	int ret = 0;
+	int i;
 
 	devpriv->CntrlReg &= Control_CNT0;
 	devpriv->CntrlReg |= Control_SW;	/*  set software trigger */
@@ -462,34 +468,27 @@ static int pci171x_insn_read_ai(struct comedi_device *dev,
 
 	setup_channel_list(dev, s, &insn->chanspec, 1, 1);
 
-	for (n = 0; n < insn->n; n++) {
+	for (i = 0; i < insn->n; i++) {
+		unsigned int val;
+
 		outw(0, dev->iobase + PCI171x_SOFTTRG);	/* start conversion */
 
 		ret = comedi_timeout(dev, s, insn, pci171x_ai_eoc, 0);
-		if (ret) {
-			outb(0, dev->iobase + PCI171x_CLRFIFO);
-			outb(0, dev->iobase + PCI171x_CLRINT);
-			return ret;
-		}
+		if (ret)
+			break;
 
-#ifdef PCI171x_PARANOIDCHECK
-		idata = inw(dev->iobase + PCI171x_AD_DATA);
-		if (this_board->cardtype != TYPE_PCI1713)
-			if ((idata & 0xf000) != devpriv->act_chanlist[0]) {
-				comedi_error(dev, "A/D insn data droput!");
-				return -ETIME;
-			}
-		data[n] = idata & 0x0fff;
-#else
-		data[n] = inw(dev->iobase + PCI171x_AD_DATA) & 0x0fff;
-#endif
+		val = inw(dev->iobase + PCI171x_AD_DATA);
+		ret = pci171x_ai_dropout(dev, s, chan, val);
+		if (ret)
+			break;
 
+		data[i] = val & s->maxdata;
 	}
 
 	outb(0, dev->iobase + PCI171x_CLRFIFO);
 	outb(0, dev->iobase + PCI171x_CLRINT);
 
-	return n;
+	return ret ? ret : insn->n;
 }
 
 /*
@@ -571,20 +570,18 @@ static int pci171x_insn_bits_do(struct comedi_device *dev,
 	return insn->n;
 }
 
-/*
-==============================================================================
-*/
-static void start_pacer(struct comedi_device *dev, int mode,
-			unsigned int divisor1, unsigned int divisor2)
+static void pci171x_start_pacer(struct comedi_device *dev,
+				bool load_counters)
 {
-	outw(0xb4, dev->iobase + PCI171x_CNTCTRL);
-	outw(0x74, dev->iobase + PCI171x_CNTCTRL);
-
-	if (mode == 1) {
-		outw(divisor2 & 0xff, dev->iobase + PCI171x_CNT2);
-		outw((divisor2 >> 8) & 0xff, dev->iobase + PCI171x_CNT2);
-		outw(divisor1 & 0xff, dev->iobase + PCI171x_CNT1);
-		outw((divisor1 >> 8) & 0xff, dev->iobase + PCI171x_CNT1);
+	struct pci1710_private *devpriv = dev->private;
+	unsigned long timer_base = dev->iobase + PCI171X_TIMER_BASE;
+
+	i8254_set_mode(timer_base, 1, 2, I8254_MODE2 | I8254_BINARY);
+	i8254_set_mode(timer_base, 1, 1, I8254_MODE2 | I8254_BINARY);
+
+	if (load_counters) {
+		i8254_write(timer_base, 1, 2, devpriv->divisor2);
+		i8254_write(timer_base, 1, 1, devpriv->divisor1);
 	}
 }
 
@@ -727,45 +724,37 @@ static int pci171x_ai_cancel(struct comedi_device *dev,
 		devpriv->CntrlReg |= Control_SW;
 
 		outw(devpriv->CntrlReg, dev->iobase + PCI171x_CONTROL);	/*  reset any operations */
-		start_pacer(dev, -1, 0, 0);
+		pci171x_start_pacer(dev, false);
 		outb(0, dev->iobase + PCI171x_CLRFIFO);
 		outb(0, dev->iobase + PCI171x_CLRINT);
 		break;
 	}
 
-	devpriv->ai_do = 0;
 	devpriv->ai_act_scan = 0;
 	s->async->cur_chan = 0;
-	devpriv->ai_buf_ptr = 0;
-	devpriv->neverending_ai = 0;
 
 	return 0;
 }
 
-/*
-==============================================================================
-*/
-static void interrupt_pci1710_every_sample(void *d)
+static void pci1710_handle_every_sample(struct comedi_device *dev,
+					struct comedi_subdevice *s)
 {
-	struct comedi_device *dev = d;
 	struct pci1710_private *devpriv = dev->private;
-	struct comedi_subdevice *s = dev->read_subdev;
-	int m;
-#ifdef PCI171x_PARANOIDCHECK
-	const struct boardtype *this_board = comedi_board(dev);
-	unsigned short sampl;
-#endif
+	struct comedi_cmd *cmd = &s->async->cmd;
+	unsigned int status;
+	unsigned int val;
+	int ret;
 
-	m = inw(dev->iobase + PCI171x_STATUS);
-	if (m & Status_FE) {
-		dev_dbg(dev->class_dev, "A/D FIFO empty (%4x)\n", m);
+	status = inw(dev->iobase + PCI171x_STATUS);
+	if (status & Status_FE) {
+		dev_dbg(dev->class_dev, "A/D FIFO empty (%4x)\n", status);
 		s->async->events |= COMEDI_CB_EOA | COMEDI_CB_ERROR;
 		cfc_handle_events(dev, s);
 		return;
 	}
-	if (m & Status_FF) {
+	if (status & Status_FF) {
 		dev_dbg(dev->class_dev,
-			"A/D FIFO Full status (Fatal Error!) (%4x)\n", m);
+			"A/D FIFO Full status (Fatal Error!) (%4x)\n", status);
 		s->async->events |= COMEDI_CB_EOA | COMEDI_CB_ERROR;
 		cfc_handle_events(dev, s);
 		return;
@@ -774,42 +763,27 @@ static void interrupt_pci1710_every_sample(void *d)
 	outb(0, dev->iobase + PCI171x_CLRINT);	/*  clear our INT request */
 
 	for (; !(inw(dev->iobase + PCI171x_STATUS) & Status_FE);) {
-#ifdef PCI171x_PARANOIDCHECK
-		sampl = inw(dev->iobase + PCI171x_AD_DATA);
-		if (this_board->cardtype != TYPE_PCI1713)
-			if ((sampl & 0xf000) !=
-			    devpriv->act_chanlist[s->async->cur_chan]) {
-				printk
-				    ("comedi: A/D data dropout: received data from channel %d, expected %d!\n",
-				     (sampl & 0xf000) >> 12,
-				     (devpriv->
-				      act_chanlist[s->
-						   async->cur_chan] & 0xf000) >>
-				     12);
-				s->async->events |=
-				    COMEDI_CB_EOA | COMEDI_CB_ERROR;
-				cfc_handle_events(dev, s);
-				return;
-			}
-		comedi_buf_put(s->async, sampl & 0x0fff);
-#else
-		comedi_buf_put(s->async,
-			       inw(dev->iobase + PCI171x_AD_DATA) & 0x0fff);
-#endif
-		++s->async->cur_chan;
+		val = inw(dev->iobase + PCI171x_AD_DATA);
+		ret = pci171x_ai_dropout(dev, s, s->async->cur_chan, val);
+		if (ret) {
+			s->async->events |= COMEDI_CB_EOA | COMEDI_CB_ERROR;
+			break;
+		}
 
-		if (s->async->cur_chan >= devpriv->ai_n_chan)
+		comedi_buf_put(s, val & s->maxdata);
+
+		s->async->cur_chan++;
+		if (s->async->cur_chan >= cmd->chanlist_len)
 			s->async->cur_chan = 0;
 
 
 		if (s->async->cur_chan == 0) {	/*  one scan done */
 			devpriv->ai_act_scan++;
-			if ((!devpriv->neverending_ai) &&
-			    (devpriv->ai_act_scan >= devpriv->ai_scans)) {
+			if (cmd->stop_src == TRIG_COUNT &&
+			    devpriv->ai_act_scan >= cmd->stop_arg) {
 				/*  all data sampled */
 				s->async->events |= COMEDI_CB_EOA;
-				cfc_handle_events(dev, s);
-				return;
+				break;
 			}
 		}
 	}
@@ -826,53 +800,37 @@ static int move_block_from_fifo(struct comedi_device *dev,
 				struct comedi_subdevice *s, int n, int turn)
 {
 	struct pci1710_private *devpriv = dev->private;
-	int i, j;
-#ifdef PCI171x_PARANOIDCHECK
-	const struct boardtype *this_board = comedi_board(dev);
-	unsigned short sampl;
-#endif
+	struct comedi_cmd *cmd = &s->async->cmd;
+	unsigned int val;
+	int ret;
+	int i;
 
-	j = s->async->cur_chan;
 	for (i = 0; i < n; i++) {
-#ifdef PCI171x_PARANOIDCHECK
-		sampl = inw(dev->iobase + PCI171x_AD_DATA);
-		if (this_board->cardtype != TYPE_PCI1713)
-			if ((sampl & 0xf000) != devpriv->act_chanlist[j]) {
-				dev_dbg(dev->class_dev,
-					"A/D FIFO data dropout: received data from channel %d, expected %d! (%d/%d/%d/%d/%d/%4x)\n",
-					(sampl & 0xf000) >> 12,
-					(devpriv->act_chanlist[j] & 0xf000) >> 12,
-					i, j, devpriv->ai_act_scan, n, turn,
-					sampl);
-				s->async->events |=
-				    COMEDI_CB_EOA | COMEDI_CB_ERROR;
-				cfc_handle_events(dev, s);
-				return 1;
-			}
-		comedi_buf_put(s->async, sampl & 0x0fff);
-#else
-		comedi_buf_put(s->async,
-			       inw(dev->iobase + PCI171x_AD_DATA) & 0x0fff);
-#endif
-		j++;
-		if (j >= devpriv->ai_n_chan) {
-			j = 0;
+		val = inw(dev->iobase + PCI171x_AD_DATA);
+
+		ret = pci171x_ai_dropout(dev, s, s->async->cur_chan, val);
+		if (ret) {
+			s->async->events |= COMEDI_CB_EOA | COMEDI_CB_ERROR;
+			return ret;
+		}
+
+		comedi_buf_put(s, val & s->maxdata);
+
+		s->async->cur_chan++;
+		if (s->async->cur_chan >= cmd->chanlist_len) {
+			s->async->cur_chan = 0;
 			devpriv->ai_act_scan++;
 		}
 	}
-	s->async->cur_chan = j;
 	return 0;
 }
 
-/*
-==============================================================================
-*/
-static void interrupt_pci1710_half_fifo(void *d)
+static void pci1710_handle_fifo(struct comedi_device *dev,
+				struct comedi_subdevice *s)
 {
-	struct comedi_device *dev = d;
 	const struct boardtype *this_board = comedi_board(dev);
 	struct pci1710_private *devpriv = dev->private;
-	struct comedi_subdevice *s = dev->read_subdev;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	int m, samplesinbuf;
 
 	m = inw(dev->iobase + PCI171x_STATUS);
@@ -891,8 +849,8 @@ static void interrupt_pci1710_half_fifo(void *d)
 	}
 
 	samplesinbuf = this_board->fifo_half_size;
-	if (samplesinbuf * sizeof(short) >= devpriv->ai_data_len) {
-		m = devpriv->ai_data_len / sizeof(short);
+	if (samplesinbuf * sizeof(short) >= s->async->prealloc_bufsz) {
+		m = s->async->prealloc_bufsz / sizeof(short);
 		if (move_block_from_fifo(dev, s, m, 0))
 			return;
 		samplesinbuf -= m;
@@ -903,13 +861,13 @@ static void interrupt_pci1710_half_fifo(void *d)
 			return;
 	}
 
-	if (!devpriv->neverending_ai)
-		if (devpriv->ai_act_scan >= devpriv->ai_scans) { /* all data
-								    sampled */
-			s->async->events |= COMEDI_CB_EOA;
-			cfc_handle_events(dev, s);
-			return;
-		}
+	if (cmd->stop_src == TRIG_COUNT &&
+	    devpriv->ai_act_scan >= cmd->stop_arg) {
+		/* all data sampled */
+		s->async->events |= COMEDI_CB_EOA;
+		cfc_handle_events(dev, s);
+		return;
+	}
 	outb(0, dev->iobase + PCI171x_CLRINT);	/*  clear our INT request */
 
 	cfc_handle_events(dev, s);
@@ -922,9 +880,15 @@ static irqreturn_t interrupt_service_pci1710(int irq, void *d)
 {
 	struct comedi_device *dev = d;
 	struct pci1710_private *devpriv = dev->private;
+	struct comedi_subdevice *s;
+	struct comedi_cmd *cmd;
 
 	if (!dev->attached)	/*  is device attached? */
 		return IRQ_NONE;	/*  no, exit */
+
+	s = dev->read_subdev;
+	cmd = &s->async->cmd;
+
 	/*  is this interrupt from our board? */
 	if (!(inw(dev->iobase + PCI171x_STATUS) & Status_IRQ))
 		return IRQ_NONE;	/*  no, exit */
@@ -939,95 +903,59 @@ static irqreturn_t interrupt_service_pci1710(int irq, void *d)
 		outb(0, dev->iobase + PCI171x_CLRINT);
 		outw(devpriv->ai_et_MuxVal, dev->iobase + PCI171x_MUX);
 		outw(devpriv->CntrlReg, dev->iobase + PCI171x_CONTROL);
-		/*  start pacer */
-		start_pacer(dev, 1, devpriv->ai_et_div1, devpriv->ai_et_div2);
+		pci171x_start_pacer(dev, true);
 		return IRQ_HANDLED;
 	}
-	if (devpriv->ai_eos) {	/*  We use FIFO half full INT or not? */
-		interrupt_pci1710_every_sample(d);
-	} else {
-		interrupt_pci1710_half_fifo(d);
-	}
+
+	if (cmd->flags & TRIG_WAKE_EOS)
+		pci1710_handle_every_sample(dev, s);
+	else
+		pci1710_handle_fifo(dev, s);
+
 	return IRQ_HANDLED;
 }
 
-/*
-==============================================================================
-*/
-static int pci171x_ai_docmd_and_mode(int mode, struct comedi_device *dev,
-				     struct comedi_subdevice *s)
+static int pci171x_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
-	const struct boardtype *this_board = comedi_board(dev);
 	struct pci1710_private *devpriv = dev->private;
-	unsigned int divisor1 = 0, divisor2 = 0;
-	unsigned int seglen;
+	struct comedi_cmd *cmd = &s->async->cmd;
 
-	start_pacer(dev, -1, 0, 0);	/*  stop pacer */
+	pci171x_start_pacer(dev, false);
 
-	seglen = check_channel_list(dev, s, devpriv->ai_chanlist,
-				    devpriv->ai_n_chan);
-	if (seglen < 1)
-		return -EINVAL;
-	setup_channel_list(dev, s, devpriv->ai_chanlist,
-			   devpriv->ai_n_chan, seglen);
+	setup_channel_list(dev, s, cmd->chanlist, cmd->chanlist_len,
+			   devpriv->saved_seglen);
 
 	outb(0, dev->iobase + PCI171x_CLRFIFO);
 	outb(0, dev->iobase + PCI171x_CLRINT);
 
-	devpriv->ai_do = mode;
-
 	devpriv->ai_act_scan = 0;
 	s->async->cur_chan = 0;
-	devpriv->ai_buf_ptr = 0;
-	devpriv->neverending_ai = 0;
 
 	devpriv->CntrlReg &= Control_CNT0;
-	/*  don't we want wake up every scan?  devpriv->ai_eos=1; */
-	if ((devpriv->ai_flags & TRIG_WAKE_EOS)) {
-		devpriv->ai_eos = 1;
-	} else {
+	if ((cmd->flags & TRIG_WAKE_EOS) == 0)
 		devpriv->CntrlReg |= Control_ONEFH;
-		devpriv->ai_eos = 0;
-	}
 
-	if ((devpriv->ai_scans == 0) || (devpriv->ai_scans == -1))
-		devpriv->neverending_ai = 1;
-	/* well, user want neverending */
-	else
-		devpriv->neverending_ai = 0;
+	devpriv->divisor1 = devpriv->next_divisor1;
+	devpriv->divisor2 = devpriv->next_divisor2;
 
-	switch (mode) {
-	case 1:
-	case 2:
-		if (devpriv->ai_timer1 < this_board->ai_ns_min)
-			devpriv->ai_timer1 = this_board->ai_ns_min;
+	if (cmd->convert_src == TRIG_TIMER) {
 		devpriv->CntrlReg |= Control_PACER | Control_IRQEN;
-		if (mode == 2) {
+		if (cmd->start_src == TRIG_EXT) {
 			devpriv->ai_et_CntrlReg = devpriv->CntrlReg;
 			devpriv->CntrlReg &=
 			    ~(Control_PACER | Control_ONEFH | Control_GATE);
 			devpriv->CntrlReg |= Control_EXT;
 			devpriv->ai_et = 1;
-		} else {
+		} else {	/* TRIG_NOW */
 			devpriv->ai_et = 0;
 		}
-		i8253_cascade_ns_to_timer(devpriv->i8254_osc_base,
-					  &divisor1, &divisor2,
-					  &devpriv->ai_timer1,
-					  devpriv->ai_flags);
 		outw(devpriv->CntrlReg, dev->iobase + PCI171x_CONTROL);
-		if (mode != 2) {
-			/*  start pacer */
-			start_pacer(dev, mode, divisor1, divisor2);
-		} else {
-			devpriv->ai_et_div1 = divisor1;
-			devpriv->ai_et_div2 = divisor2;
-		}
-		break;
-	case 3:
+
+		if (cmd->start_src == TRIG_NOW)
+			pci171x_start_pacer(dev, true);
+	} else {	/* TRIG_EXT */
 		devpriv->CntrlReg |= Control_EXT | Control_IRQEN;
 		outw(devpriv->CntrlReg, dev->iobase + PCI171x_CONTROL);
-		break;
 	}
 
 	return 0;
@@ -1043,8 +971,7 @@ static int pci171x_ai_cmdtest(struct comedi_device *dev,
 	const struct boardtype *this_board = comedi_board(dev);
 	struct pci1710_private *devpriv = dev->private;
 	int err = 0;
-	int tmp;
-	unsigned int divisor1 = 0, divisor2 = 0;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -1092,64 +1019,25 @@ static int pci171x_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
-		i8253_cascade_ns_to_timer(devpriv->i8254_osc_base,
-					  &divisor1, &divisor2,
-					  &cmd->convert_arg, cmd->flags);
-		if (cmd->convert_arg < this_board->ai_ns_min)
-			cmd->convert_arg = this_board->ai_ns_min;
-		if (tmp != cmd->convert_arg)
-			err++;
+		arg = cmd->convert_arg;
+		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
+					  &devpriv->next_divisor1,
+					  &devpriv->next_divisor2,
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
 		return 4;
 
-	/* step 5: complain about special chanlist considerations */
+	/* Step 5: check channel list */
 
-	if (cmd->chanlist) {
-		if (!check_channel_list(dev, s, cmd->chanlist,
-					cmd->chanlist_len))
-			return 5;	/*  incorrect channels list */
-	}
+	err |= pci171x_ai_check_chanlist(dev, s, cmd);
 
-	return 0;
-}
-
-/*
-==============================================================================
-*/
-static int pci171x_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
-{
-	struct pci1710_private *devpriv = dev->private;
-	struct comedi_cmd *cmd = &s->async->cmd;
-
-	devpriv->ai_n_chan = cmd->chanlist_len;
-	devpriv->ai_chanlist = cmd->chanlist;
-	devpriv->ai_flags = cmd->flags;
-	devpriv->ai_data_len = s->async->prealloc_bufsz;
-	devpriv->ai_timer1 = 0;
-	devpriv->ai_timer2 = 0;
-
-	if (cmd->stop_src == TRIG_COUNT)
-		devpriv->ai_scans = cmd->stop_arg;
-	else
-		devpriv->ai_scans = 0;
-
-
-	if (cmd->scan_begin_src == TRIG_FOLLOW) {	/*  mode 1, 2, 3 */
-		if (cmd->convert_src == TRIG_TIMER) {	/*  mode 1 and 2 */
-			devpriv->ai_timer1 = cmd->convert_arg;
-			return pci171x_ai_docmd_and_mode(cmd->start_src ==
-							 TRIG_EXT ? 2 : 1, dev,
-							 s);
-		}
-		if (cmd->convert_src == TRIG_EXT) {	/*  mode 3 */
-			return pci171x_ai_docmd_and_mode(3, dev, s);
-		}
-	}
+	if (err)
+		return 5;
 
-	return -1;
+	return 0;
 }
 
 /*
@@ -1165,7 +1053,7 @@ static int pci171x_reset(struct comedi_device *dev)
 	outw(devpriv->CntrlReg, dev->iobase + PCI171x_CONTROL);	/*  reset any operations */
 	outb(0, dev->iobase + PCI171x_CLRFIFO);	/*  clear FIFO */
 	outb(0, dev->iobase + PCI171x_CLRINT);	/*  clear INT request */
-	start_pacer(dev, -1, 0, 0);	/*  stop 8254 */
+	pci171x_start_pacer(dev, false);
 	devpriv->da_ranges = 0;
 	if (this_board->n_aochan) {
 		outb(devpriv->da_ranges, dev->iobase + PCI171x_DAREF);	/*  set DACs to 0..5V */
@@ -1290,7 +1178,6 @@ static int pci1710_auto_attach(struct comedi_device *dev,
 			s->do_cmd = pci171x_ai_cmd;
 			s->cancel = pci171x_ai_cancel;
 		}
-		devpriv->i8254_osc_base = I8254_OSC_BASE_10MHZ;
 		subdev++;
 	}
 
diff --git a/drivers/staging/comedi/drivers/amplc_dio200_common.c b/drivers/staging/comedi/drivers/amplc_dio200_common.c
index 818a0d7e3d5..3edaa4028da 100644
--- a/drivers/staging/comedi/drivers/amplc_dio200_common.c
+++ b/drivers/staging/comedi/drivers/amplc_dio200_common.c
@@ -130,7 +130,6 @@ struct dio200_subdev_intr {
 	unsigned int enabled_isns;
 	unsigned int stopcount;
 	bool active:1;
-	bool continuous:1;
 };
 
 static inline const struct dio200_layout *
@@ -259,7 +258,7 @@ static int dio200_start_intr(struct comedi_device *dev,
 	struct comedi_cmd *cmd = &s->async->cmd;
 	int retval = 0;
 
-	if (!subpriv->continuous && subpriv->stopcount == 0) {
+	if (cmd->stop_src == TRIG_COUNT && subpriv->stopcount == 0) {
 		/* An empty acquisition! */
 		s->async->events |= COMEDI_CB_EOA;
 		subpriv->active = false;
@@ -281,22 +280,18 @@ static int dio200_start_intr(struct comedi_device *dev,
 	return retval;
 }
 
-/*
- * Internal trigger function to start acquisition for an 'INTERRUPT' subdevice.
- */
-static int
-dio200_inttrig_start_intr(struct comedi_device *dev, struct comedi_subdevice *s,
-			  unsigned int trignum)
+static int dio200_inttrig_start_intr(struct comedi_device *dev,
+				     struct comedi_subdevice *s,
+				     unsigned int trig_num)
 {
-	struct dio200_subdev_intr *subpriv;
+	struct dio200_subdev_intr *subpriv = s->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned long flags;
 	int event = 0;
 
-	if (trignum != 0)
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
-	subpriv = s->private;
-
 	spin_lock_irqsave(&subpriv->spinlock, flags);
 	s->async->inttrig = NULL;
 	if (subpriv->active)
@@ -315,18 +310,18 @@ static void dio200_read_scan_intr(struct comedi_device *dev,
 				  unsigned int triggered)
 {
 	struct dio200_subdev_intr *subpriv = s->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned short val;
-	unsigned int n, ch, len;
+	unsigned int n, ch;
 
 	val = 0;
-	len = s->async->cmd.chanlist_len;
-	for (n = 0; n < len; n++) {
-		ch = CR_CHAN(s->async->cmd.chanlist[n]);
+	for (n = 0; n < cmd->chanlist_len; n++) {
+		ch = CR_CHAN(cmd->chanlist[n]);
 		if (triggered & (1U << ch))
 			val |= (1U << n);
 	}
 	/* Write the scan to the buffer. */
-	if (comedi_buf_put(s->async, val)) {
+	if (comedi_buf_put(s, val)) {
 		s->async->events |= (COMEDI_CB_BLOCK | COMEDI_CB_EOS);
 	} else {
 		/* Error!  Stop acquisition.  */
@@ -336,8 +331,7 @@ static void dio200_read_scan_intr(struct comedi_device *dev,
 	}
 
 	/* Check for end of acquisition. */
-	if (!subpriv->continuous) {
-		/* stop_src == TRIG_COUNT */
+	if (cmd->stop_src == TRIG_COUNT) {
 		if (subpriv->stopcount > 0) {
 			subpriv->stopcount--;
 			if (subpriv->stopcount == 0) {
@@ -516,28 +510,16 @@ static int dio200_subdev_intr_cmd(struct comedi_device *dev,
 	subpriv->active = true;
 
 	/* Set up end of acquisition. */
-	switch (cmd->stop_src) {
-	case TRIG_COUNT:
-		subpriv->continuous = false;
+	if (cmd->stop_src == TRIG_COUNT)
 		subpriv->stopcount = cmd->stop_arg;
-		break;
-	default:
-		/* TRIG_NONE */
-		subpriv->continuous = true;
+	else	/* TRIG_NONE */
 		subpriv->stopcount = 0;
-		break;
-	}
 
-	/* Set up start of acquisition. */
-	switch (cmd->start_src) {
-	case TRIG_INT:
+	if (cmd->start_src == TRIG_INT)
 		s->async->inttrig = dio200_inttrig_start_intr;
-		break;
-	default:
-		/* TRIG_NOW */
+	else	/* TRIG_NOW */
 		event = dio200_start_intr(dev, s);
-		break;
-	}
+
 	spin_unlock_irqrestore(&subpriv->spinlock, flags);
 
 	if (event)
@@ -829,7 +811,7 @@ dio200_subdev_8254_config(struct comedi_device *dev, struct comedi_subdevice *s,
 	spin_lock_irqsave(&subpriv->spinlock, flags);
 	switch (data[0]) {
 	case INSN_CONFIG_SET_COUNTER_MODE:
-		if (data[1] > (I8254_MODE5 | I8254_BINARY))
+		if (data[1] > (I8254_MODE5 | I8254_BCD))
 			ret = -EINVAL;
 		else
 			dio200_subdev_8254_set_mode(dev, s, chan, data[1]);
diff --git a/drivers/staging/comedi/drivers/amplc_pc236.c b/drivers/staging/comedi/drivers/amplc_pc236.c
index b21d7b48f1d..c9a96ad0055 100644
--- a/drivers/staging/comedi/drivers/amplc_pc236.c
+++ b/drivers/staging/comedi/drivers/amplc_pc236.c
@@ -314,7 +314,7 @@ static int pc236_intr_cmdtest(struct comedi_device *dev,
 	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, 1);
+	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)
@@ -361,7 +361,7 @@ static irqreturn_t pc236_interrupt(int irq, void *d)
 
 	handled = pc236_intr_check(dev);
 	if (dev->attached && handled) {
-		comedi_buf_put(s->async, 0);
+		comedi_buf_put(s, 0);
 		s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
 		comedi_event(dev, s);
 	}
@@ -402,6 +402,7 @@ static int pc236_common_attach(struct comedi_device *dev, unsigned long iobase,
 			s->maxdata = 1;
 			s->range_table = &range_digital;
 			s->insn_bits = pc236_intr_insn;
+			s->len_chanlist	= 1;
 			s->do_cmdtest = pc236_intr_cmdtest;
 			s->do_cmd = pc236_intr_cmd;
 			s->cancel = pc236_intr_cancel;
diff --git a/drivers/staging/comedi/drivers/amplc_pci224.c b/drivers/staging/comedi/drivers/amplc_pci224.c
index 29e01e28003..339c47c1eb9 100644
--- a/drivers/staging/comedi/drivers/amplc_pci224.c
+++ b/drivers/staging/comedi/drivers/amplc_pci224.c
@@ -377,7 +377,6 @@ struct pci224_private {
 	unsigned int cached_div1;
 	unsigned int cached_div2;
 	unsigned int ao_stop_count;
-	short ao_stop_continuous;
 	unsigned short ao_enab;	/* max 16 channels so 'short' will do */
 	unsigned char intsce;
 };
@@ -467,16 +466,6 @@ pci224_ao_insn_read(struct comedi_device *dev, struct comedi_subdevice *s,
 }
 
 /*
- * Just a wrapper for the inline function 'i8253_cascade_ns_to_timer'.
- */
-static void
-pci224_cascade_ns_to_timer(int osc_base, unsigned int *d1, unsigned int *d2,
-			   unsigned int *nanosec, int round_mode)
-{
-	i8253_cascade_ns_to_timer(osc_base, d1, d2, nanosec, round_mode);
-}
-
-/*
  * Kills a command running on the AO subdevice.
  */
 static void pci224_ao_stop(struct comedi_device *dev,
@@ -531,7 +520,7 @@ static void pci224_ao_start(struct comedi_device *dev,
 	unsigned long flags;
 
 	set_bit(AO_CMD_STARTED, &devpriv->state);
-	if (!devpriv->ao_stop_continuous && devpriv->ao_stop_count == 0) {
+	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);
@@ -556,21 +545,15 @@ static void pci224_ao_handle_fifo(struct comedi_device *dev,
 {
 	struct pci224_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
+	unsigned int bytes_per_scan = cfc_bytes_per_scan(s);
 	unsigned int num_scans;
 	unsigned int room;
 	unsigned short dacstat;
 	unsigned int i, n;
-	unsigned int bytes_per_scan;
 
-	if (cmd->chanlist_len) {
-		bytes_per_scan = cmd->chanlist_len * sizeof(short);
-	} else {
-		/* Shouldn't get here! */
-		bytes_per_scan = sizeof(short);
-	}
 	/* Determine number of scans available in buffer. */
-	num_scans = comedi_buf_read_n_available(s->async) / bytes_per_scan;
-	if (!devpriv->ao_stop_continuous) {
+	num_scans = comedi_buf_read_n_available(s) / bytes_per_scan;
+	if (cmd->stop_src == TRIG_COUNT) {
 		/* Fixed number of scans. */
 		if (num_scans > devpriv->ao_stop_count)
 			num_scans = devpriv->ao_stop_count;
@@ -582,7 +565,7 @@ 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 (!devpriv->ao_stop_continuous && 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);
@@ -608,8 +591,7 @@ static void pci224_ao_handle_fifo(struct comedi_device *dev,
 		}
 	}
 	/* Determine how many new scans can be put in the FIFO. */
-	if (cmd->chanlist_len)
-		room /= cmd->chanlist_len;
+	room /= cmd->chanlist_len;
 
 	/* Determine how many scans to process. */
 	if (num_scans > room)
@@ -624,7 +606,7 @@ static void pci224_ao_handle_fifo(struct comedi_device *dev,
 			     dev->iobase + PCI224_DACDATA);
 		}
 	}
-	if (!devpriv->ao_stop_continuous) {
+	if (cmd->stop_src == TRIG_COUNT) {
 		devpriv->ao_stop_count -= num_scans;
 		if (devpriv->ao_stop_count == 0) {
 			/*
@@ -671,14 +653,13 @@ static void pci224_ao_handle_fifo(struct comedi_device *dev,
 	cfc_handle_events(dev, s);
 }
 
-/*
- * Internal trigger function to start acquisition on AO subdevice.
- */
-static int
-pci224_ao_inttrig_start(struct comedi_device *dev, struct comedi_subdevice *s,
-			unsigned int trignum)
+static int pci224_ao_inttrig_start(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   unsigned int trig_num)
 {
-	if (trignum != 0)
+	struct comedi_cmd *cmd = &s->async->cmd;
+
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	s->async->inttrig = NULL;
@@ -687,6 +668,37 @@ pci224_ao_inttrig_start(struct comedi_device *dev, struct comedi_subdevice *s,
 	return 1;
 }
 
+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]);
+	unsigned int chan_mask = 0;
+	int i;
+
+	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,
+				"%s: entries in chanlist must contain no duplicate channels\n",
+				__func__);
+			return -EINVAL;
+		}
+		chan_mask |= (1 << chan);
+
+		if (range != range0) {
+			dev_dbg(dev->class_dev,
+				"%s: entries in chanlist must all have the same range index\n",
+				__func__);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 #define MAX_SCAN_PERIOD		0xFFFFFFFFU
 #define MIN_SCAN_PERIOD		2500
 #define CONVERT_PERIOD		625
@@ -700,7 +712,7 @@ pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 {
 	struct pci224_private *devpriv = dev->private;
 	int err = 0;
-	unsigned int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -727,14 +739,14 @@ pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	 * There's only one external trigger signal (which makes these
 	 * tests easier).  Only one thing can use it.
 	 */
-	tmp = 0;
+	arg = 0;
 	if (cmd->start_src & TRIG_EXT)
-		tmp++;
+		arg++;
 	if (cmd->scan_begin_src & TRIG_EXT)
-		tmp++;
+		arg++;
 	if (cmd->stop_src & TRIG_EXT)
-		tmp++;
-	if (tmp > 1)
+		arg++;
+	if (arg > 1)
 		err |= -EINVAL;
 
 	if (err)
@@ -767,10 +779,10 @@ pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		err |= cfc_check_trigger_arg_max(&cmd->scan_begin_arg,
 						 MAX_SCAN_PERIOD);
 
-		tmp = cmd->chanlist_len * CONVERT_PERIOD;
-		if (tmp < MIN_SCAN_PERIOD)
-			tmp = MIN_SCAN_PERIOD;
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg, tmp);
+		arg = cmd->chanlist_len * CONVERT_PERIOD;
+		if (arg < MIN_SCAN_PERIOD)
+			arg = MIN_SCAN_PERIOD;
+		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg, arg);
 		break;
 	case TRIG_EXT:
 		/* Force to external trigger 0. */
@@ -821,98 +833,21 @@ pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	/* Step 4: fix up any arguments. */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		unsigned int div1, div2, round;
-		int round_mode = cmd->flags & TRIG_ROUND_MASK;
-
-		tmp = cmd->scan_begin_arg;
-		/* Check whether to use a single timer. */
-		switch (round_mode) {
-		case TRIG_ROUND_NEAREST:
-		default:
-			round = I8254_OSC_BASE_10MHZ / 2;
-			break;
-		case TRIG_ROUND_DOWN:
-			round = 0;
-			break;
-		case TRIG_ROUND_UP:
-			round = I8254_OSC_BASE_10MHZ - 1;
-			break;
-		}
-		/* Be careful to avoid overflow! */
-		div2 = cmd->scan_begin_arg / I8254_OSC_BASE_10MHZ;
-		div2 += (round + cmd->scan_begin_arg % I8254_OSC_BASE_10MHZ) /
-			I8254_OSC_BASE_10MHZ;
-		if (div2 <= 0x10000) {
-			/* A single timer will suffice. */
-			if (div2 < 2)
-				div2 = 2;
-			cmd->scan_begin_arg = div2 * I8254_OSC_BASE_10MHZ;
-			if (cmd->scan_begin_arg < div2 ||
-			    cmd->scan_begin_arg < I8254_OSC_BASE_10MHZ) {
-				/* Overflow! */
-				cmd->scan_begin_arg = MAX_SCAN_PERIOD;
-			}
-		} else {
-			/* Use two timers. */
-			div1 = devpriv->cached_div1;
-			div2 = devpriv->cached_div2;
-			pci224_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
-						   &div1, &div2,
-						   &cmd->scan_begin_arg,
-						   round_mode);
-			devpriv->cached_div1 = div1;
-			devpriv->cached_div2 = div2;
-		}
-		if (tmp != cmd->scan_begin_arg)
-			err++;
-
+		arg = cmd->scan_begin_arg;
+		/* Use two timers. */
+		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
+					  &devpriv->cached_div1,
+					  &devpriv->cached_div2,
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (err)
 		return 4;
 
-	/* Step 5: check channel list. */
-
-	if (cmd->chanlist && (cmd->chanlist_len > 0)) {
-		unsigned int range;
-		enum { range_err = 1, dupchan_err = 2, };
-		unsigned errors;
-		unsigned int n;
-		unsigned int ch;
-
-		/*
-		 * Check all channels have the same range index.  Don't care
-		 * about analogue reference, as we can't configure it.
-		 *
-		 * Check the list has no duplicate channels.
-		 */
-		range = CR_RANGE(cmd->chanlist[0]);
-		errors = 0;
-		tmp = 0;
-		for (n = 0; n < cmd->chanlist_len; n++) {
-			ch = CR_CHAN(cmd->chanlist[n]);
-			if (tmp & (1U << ch))
-				errors |= dupchan_err;
-
-			tmp |= (1U << ch);
-			if (CR_RANGE(cmd->chanlist[n]) != range)
-				errors |= range_err;
-
-		}
-		if (errors) {
-			if (errors & dupchan_err) {
-				dev_dbg(dev->class_dev,
-					"%s: entries in chanlist must contain no duplicate channels\n",
-					__func__);
-			}
-			if (errors & range_err) {
-				dev_dbg(dev->class_dev,
-					"%s: entries in chanlist must all have the same range index\n",
-					__func__);
-			}
-			err++;
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= pci224_ao_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -920,9 +855,33 @@ pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	return 0;
 }
 
-/*
- * 'do_cmd' function for AO subdevice.
- */
+static void pci224_ao_start_pacer(struct comedi_device *dev,
+				  struct comedi_subdevice *s)
+{
+	struct pci224_private *devpriv = dev->private;
+	unsigned long timer_base = devpriv->iobase1 + PCI224_Z2_CT0;
+
+	/*
+	 * The output of timer Z2-0 will be used as the scan trigger
+	 * source.
+	 */
+	/* Make sure Z2-0 is gated on.  */
+	outb(GAT_CONFIG(0, GAT_VCC), devpriv->iobase1 + PCI224_ZGAT_SCE);
+	/* Cascading with Z2-2. */
+	/* Make sure Z2-2 is gated on.  */
+	outb(GAT_CONFIG(2, GAT_VCC), devpriv->iobase1 + PCI224_ZGAT_SCE);
+	/* Z2-2 needs 10 MHz clock. */
+	outb(CLK_CONFIG(2, CLK_10MHZ), devpriv->iobase1 + PCI224_ZCLK_SCE);
+	/* Load Z2-2 mode (2) and counter (div1). */
+	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
+	i8254_write(timer_base, 0, 2, devpriv->cached_div1);
+	/* Z2-0 is clocked from Z2-2's output. */
+	outb(CLK_CONFIG(0, CLK_OUTNM1), devpriv->iobase1 + PCI224_ZCLK_SCE);
+	/* Load Z2-0 mode (2) and counter (div2). */
+	i8254_set_mode(timer_base, 0, 0, I8254_MODE2 | I8254_BINARY);
+	i8254_write(timer_base, 0, 0, devpriv->cached_div2);
+}
+
 static int pci224_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
 	struct pci224_private *devpriv = dev->private;
@@ -978,106 +937,26 @@ static int pci224_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	outw(devpriv->daccon | PCI224_DACCON_FIFORESET,
 	     dev->iobase + PCI224_DACCON);
 
-	if (cmd->scan_begin_src == TRIG_TIMER) {
-		unsigned int div1, div2, round;
-		unsigned int ns = cmd->scan_begin_arg;
-		int round_mode = cmd->flags & TRIG_ROUND_MASK;
-
-		/* Check whether to use a single timer. */
-		switch (round_mode) {
-		case TRIG_ROUND_NEAREST:
-		default:
-			round = I8254_OSC_BASE_10MHZ / 2;
-			break;
-		case TRIG_ROUND_DOWN:
-			round = 0;
-			break;
-		case TRIG_ROUND_UP:
-			round = I8254_OSC_BASE_10MHZ - 1;
-			break;
-		}
-		/* Be careful to avoid overflow! */
-		div2 = cmd->scan_begin_arg / I8254_OSC_BASE_10MHZ;
-		div2 += (round + cmd->scan_begin_arg % I8254_OSC_BASE_10MHZ) /
-			I8254_OSC_BASE_10MHZ;
-		if (div2 <= 0x10000) {
-			/* A single timer will suffice. */
-			if (div2 < 2)
-				div2 = 2;
-			div2 &= 0xffff;
-			div1 = 1;	/* Flag that single timer to be used. */
-		} else {
-			/* Use two timers. */
-			div1 = devpriv->cached_div1;
-			div2 = devpriv->cached_div2;
-			pci224_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
-						   &div1, &div2,
-						   &ns, round_mode);
-		}
-
-		/*
-		 * The output of timer Z2-0 will be used as the scan trigger
-		 * source.
-		 */
-		/* Make sure Z2-0 is gated on.  */
-		outb(GAT_CONFIG(0, GAT_VCC),
-		     devpriv->iobase1 + PCI224_ZGAT_SCE);
-		if (div1 == 1) {
-			/* Not cascading.  Z2-0 needs 10 MHz clock. */
-			outb(CLK_CONFIG(0, CLK_10MHZ),
-			     devpriv->iobase1 + PCI224_ZCLK_SCE);
-		} else {
-			/* Cascading with Z2-2. */
-			/* Make sure Z2-2 is gated on.  */
-			outb(GAT_CONFIG(2, GAT_VCC),
-			     devpriv->iobase1 + PCI224_ZGAT_SCE);
-			/* Z2-2 needs 10 MHz clock. */
-			outb(CLK_CONFIG(2, CLK_10MHZ),
-			     devpriv->iobase1 + PCI224_ZCLK_SCE);
-			/* Load Z2-2 mode (2) and counter (div1). */
-			i8254_load(devpriv->iobase1 + PCI224_Z2_CT0, 0,
-				   2, div1, 2);
-			/* Z2-0 is clocked from Z2-2's output. */
-			outb(CLK_CONFIG(0, CLK_OUTNM1),
-			     devpriv->iobase1 + PCI224_ZCLK_SCE);
-		}
-		/* Load Z2-0 mode (2) and counter (div2). */
-		i8254_load(devpriv->iobase1 + PCI224_Z2_CT0, 0, 0, div2, 2);
-	}
+	if (cmd->scan_begin_src == TRIG_TIMER)
+		pci224_ao_start_pacer(dev, s);
 
 	/*
 	 * Sort out end of acquisition.
 	 */
-	switch (cmd->stop_src) {
-	case TRIG_COUNT:
-		/* Fixed number of scans.  */
-		devpriv->ao_stop_continuous = 0;
+	if (cmd->stop_src == TRIG_COUNT)
 		devpriv->ao_stop_count = cmd->stop_arg;
-		break;
-	default:
-		/* Continuous scans. */
-		devpriv->ao_stop_continuous = 1;
+	else	/* TRIG_EXT | TRIG_NONE */
 		devpriv->ao_stop_count = 0;
-		break;
-	}
 
-	/*
-	 * Sort out start of acquisition.
-	 */
-	switch (cmd->start_src) {
-	case TRIG_INT:
-		spin_lock_irqsave(&devpriv->ao_spinlock, flags);
-		s->async->inttrig = &pci224_ao_inttrig_start;
-		spin_unlock_irqrestore(&devpriv->ao_spinlock, flags);
-		break;
-	case TRIG_EXT:
+	spin_lock_irqsave(&devpriv->ao_spinlock, flags);
+	if (cmd->start_src == TRIG_INT) {
+		s->async->inttrig = pci224_ao_inttrig_start;
+	} else {	/* TRIG_EXT */
 		/* Enable external interrupt trigger to start acquisition. */
-		spin_lock_irqsave(&devpriv->ao_spinlock, flags);
 		devpriv->intsce |= PCI224_INTR_EXT;
 		outb(devpriv->intsce, devpriv->iobase1 + PCI224_INT_SCE);
-		spin_unlock_irqrestore(&devpriv->ao_spinlock, flags);
-		break;
 	}
+	spin_unlock_irqrestore(&devpriv->ao_spinlock, flags);
 
 	return 0;
 }
@@ -1101,7 +980,7 @@ pci224_ao_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 {
 	const struct pci224_board *thisboard = comedi_board(dev);
 	struct pci224_private *devpriv = dev->private;
-	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned short *array = data;
 	unsigned int length = num_bytes / sizeof(*array);
 	unsigned int offset;
@@ -1111,7 +990,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(async->cmd.chanlist[0])] &
+	if ((devpriv->hwrange[CR_RANGE(cmd->chanlist[0])] &
 	     PCI224_DACCON_POLAR_MASK) == PCI224_DACCON_POLAR_UNI) {
 		/* Unipolar */
 		offset = 0;
diff --git a/drivers/staging/comedi/drivers/amplc_pci230.c b/drivers/staging/comedi/drivers/amplc_pci230.c
index 99e60835dc4..3895bc7cb3e 100644
--- a/drivers/staging/comedi/drivers/amplc_pci230.c
+++ b/drivers/staging/comedi/drivers/amplc_pci230.c
@@ -519,14 +519,6 @@ struct pci230_private {
 					 * level threshold (PCI230+/260+). */
 	unsigned short adcg;	/* ADCG register value. */
 	unsigned char int_en;	/* Interrupt enables bits. */
-	unsigned char ai_continuous;	/* Flag set when cmd->stop_src ==
-					 * TRIG_NONE - user chooses to stop
-					 * continuous conversion by
-					 * cancelation. */
-	unsigned char ao_continuous;	/* Flag set when cmd->stop_src ==
-					 * TRIG_NONE - user chooses to stop
-					 * continuous conversion by
-					 * cancelation. */
 	unsigned char ai_bipolar;	/* Set if bipolar input range so we
 					 * know to mangle it. */
 	unsigned char ao_bipolar;	/* Set if bipolar output range so we
@@ -953,6 +945,38 @@ static int pci230_ao_rinsn(struct comedi_device *dev,
 	return i;
 }
 
+static int pci230_ao_check_chanlist(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    struct comedi_cmd *cmd)
+{
+	unsigned int prev_chan = CR_CHAN(cmd->chanlist[0]);
+	unsigned int range0 = CR_RANGE(cmd->chanlist[0]);
+	int i;
+
+	for (i = 1; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+		unsigned int range = CR_RANGE(cmd->chanlist[i]);
+
+		if (chan < prev_chan) {
+			dev_dbg(dev->class_dev,
+				"%s: channel numbers must increase\n",
+				__func__);
+			return -EINVAL;
+		}
+
+		if (range != range0) {
+			dev_dbg(dev->class_dev,
+				"%s: channels must have the same range\n",
+				__func__);
+			return -EINVAL;
+		}
+
+		prev_chan = chan;
+	}
+
+	return 0;
+}
+
 static int pci230_ao_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
@@ -1065,48 +1089,9 @@ static int pci230_ao_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 4;
 
-	/* Step 5: check channel list if it exists. */
-
-	if (cmd->chanlist && cmd->chanlist_len > 0) {
-		enum {
-			seq_err = (1 << 0),
-			range_err = (1 << 1)
-		};
-		unsigned int errors;
-		unsigned int n;
-		unsigned int chan, prev_chan;
-		unsigned int range, first_range;
-
-		prev_chan = CR_CHAN(cmd->chanlist[0]);
-		first_range = CR_RANGE(cmd->chanlist[0]);
-		errors = 0;
-		for (n = 1; n < cmd->chanlist_len; n++) {
-			chan = CR_CHAN(cmd->chanlist[n]);
-			range = CR_RANGE(cmd->chanlist[n]);
-			/* Channel numbers must strictly increase. */
-			if (chan < prev_chan)
-				errors |= seq_err;
-
-			/* Ranges must be the same. */
-			if (range != first_range)
-				errors |= range_err;
-
-			prev_chan = chan;
-		}
-		if (errors != 0) {
-			err++;
-			if ((errors & seq_err) != 0) {
-				dev_dbg(dev->class_dev,
-					"%s: channel numbers must increase\n",
-					__func__);
-			}
-			if ((errors & range_err) != 0) {
-				dev_dbg(dev->class_dev,
-					"%s: channels must have the same range\n",
-					__func__);
-			}
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= pci230_ao_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -1175,11 +1160,11 @@ static void pci230_handle_ao_nofifo(struct comedi_device *dev,
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
 
-	if (!devpriv->ao_continuous && (devpriv->ao_scan_count == 0))
+	if (cmd->stop_src == TRIG_COUNT && devpriv->ao_scan_count == 0)
 		return;
 	for (i = 0; i < cmd->chanlist_len; i++) {
 		/* Read sample from Comedi's circular buffer. */
-		ret = comedi_buf_get(s->async, &data);
+		ret = comedi_buf_get(s, &data);
 		if (ret == 0) {
 			s->async->events |= COMEDI_CB_OVERFLOW;
 			pci230_ao_stop(dev, s);
@@ -1190,7 +1175,7 @@ static void pci230_handle_ao_nofifo(struct comedi_device *dev,
 		pci230_ao_write_nofifo(dev, data, CR_CHAN(cmd->chanlist[i]));
 	}
 	async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
-	if (!devpriv->ao_continuous) {
+	if (cmd->stop_src == TRIG_COUNT) {
 		devpriv->ao_scan_count--;
 		if (devpriv->ao_scan_count == 0) {
 			/* End of acquisition. */
@@ -1212,16 +1197,14 @@ static int pci230_handle_ao_fifo(struct comedi_device *dev,
 	unsigned int room;
 	unsigned short dacstat;
 	unsigned int i, n;
-	unsigned int bytes_per_scan;
 	unsigned int events = 0;
 	int running;
 
 	/* Get DAC FIFO status. */
 	dacstat = inw(dev->iobase + PCI230_DACCON);
 	/* Determine number of scans available in buffer. */
-	bytes_per_scan = cmd->chanlist_len * sizeof(short);
-	num_scans = comedi_buf_read_n_available(async) / bytes_per_scan;
-	if (!devpriv->ao_continuous) {
+	num_scans = comedi_buf_read_n_available(s) / cfc_bytes_per_scan(s);
+	if (cmd->stop_src == TRIG_COUNT) {
 		/* Fixed number of scans. */
 		if (num_scans > devpriv->ao_scan_count)
 			num_scans = devpriv->ao_scan_count;
@@ -1265,13 +1248,13 @@ static int pci230_handle_ao_fifo(struct comedi_device *dev,
 			for (i = 0; i < cmd->chanlist_len; i++) {
 				unsigned short datum;
 
-				comedi_buf_get(async, &datum);
+				comedi_buf_get(s, &datum);
 				pci230_ao_write_fifo(dev, datum,
 						     CR_CHAN(cmd->chanlist[i]));
 			}
 		}
 		events |= COMEDI_CB_EOS | COMEDI_CB_BLOCK;
-		if (!devpriv->ao_continuous) {
+		if (cmd->stop_src == TRIG_COUNT) {
 			devpriv->ao_scan_count -= num_scans;
 			if (devpriv->ao_scan_count == 0) {
 				/* All data for the command has been written
@@ -1349,7 +1332,7 @@ static void pci230_ao_start(struct comedi_device *dev,
 	unsigned long irqflags;
 
 	set_bit(AO_CMD_STARTED, &devpriv->state);
-	if (!devpriv->ao_continuous && (devpriv->ao_scan_count == 0)) {
+	if (cmd->stop_src == TRIG_COUNT && devpriv->ao_scan_count == 0) {
 		/* An empty acquisition! */
 		async->events |= COMEDI_CB_EOA;
 		pci230_ao_stop(dev, s);
@@ -1434,7 +1417,9 @@ static int pci230_ao_inttrig_start(struct comedi_device *dev,
 				   struct comedi_subdevice *s,
 				   unsigned int trig_num)
 {
-	if (trig_num != 0)
+	struct comedi_cmd *cmd = &s->async->cmd;
+
+	if (trig_num != cmd->start_src)
 		return -EINVAL;
 
 	s->async->inttrig = NULL;
@@ -1460,14 +1445,10 @@ static int pci230_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	}
 
 	/* Get number of scans required. */
-	if (cmd->stop_src == TRIG_COUNT) {
+	if (cmd->stop_src == TRIG_COUNT)
 		devpriv->ao_scan_count = cmd->stop_arg;
-		devpriv->ao_continuous = 0;
-	} else {
-		/* TRIG_NONE, user calls cancel. */
+	else	/* TRIG_NONE, user calls cancel */
 		devpriv->ao_scan_count = 0;
-		devpriv->ao_continuous = 1;
-	}
 
 	/* Set range - see analogue output range table; 0 => unipolar 10V,
 	 * 1 => bipolar +/-10V range scale */
@@ -1552,6 +1533,109 @@ static int pci230_ai_check_scan_period(struct comedi_cmd *cmd)
 	return !err;
 }
 
+static int pci230_ai_check_chanlist(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    struct comedi_cmd *cmd)
+{
+	struct pci230_private *devpriv = dev->private;
+	unsigned int max_diff_chan = (s->n_chan / 2) - 1;
+	unsigned int prev_chan = 0;
+	unsigned int prev_range = 0;
+	unsigned int prev_aref = 0;
+	unsigned int prev_polarity = 0;
+	unsigned int subseq_len = 0;
+	int i;
+
+	for (i = 0; i < cmd->chanlist_len; i++) {
+		unsigned int chanspec = cmd->chanlist[i];
+		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];
+
+		if (aref == AREF_DIFF && chan >= max_diff_chan) {
+			dev_dbg(dev->class_dev,
+				"%s: differential channel number out of range 0 to %u\n",
+				__func__, max_diff_chan);
+			return -EINVAL;
+		}
+
+		if (i > 0) {
+			/*
+			 * Channel numbers must strictly increase or
+			 * subsequence must repeat exactly.
+			 */
+			if (chan <= prev_chan && subseq_len == 0)
+				subseq_len = i;
+
+			if (subseq_len > 0 &&
+			    cmd->chanlist[i % subseq_len] != chanspec) {
+					dev_dbg(dev->class_dev,
+						"%s: channel numbers must increase or sequence must repeat exactly\n",
+						__func__);
+					return -EINVAL;
+			}
+
+			if (aref != prev_aref) {
+				dev_dbg(dev->class_dev,
+					"%s: channel sequence analogue references must be all the same (single-ended or differential)\n",
+					__func__);
+				return -EINVAL;
+			}
+
+			if (polarity != prev_polarity) {
+				dev_dbg(dev->class_dev,
+					"%s: channel sequence ranges must be all bipolar or all unipolar\n",
+					__func__);
+				return -EINVAL;
+			}
+
+			if (aref != AREF_DIFF && range != prev_range &&
+			    ((chan ^ prev_chan) & ~1) == 0) {
+				dev_dbg(dev->class_dev,
+					"%s: single-ended channel pairs must have the same range\n",
+					__func__);
+				return -EINVAL;
+			}
+		}
+		prev_chan = chan;
+		prev_range = range;
+		prev_aref = aref;
+		prev_polarity = polarity;
+	}
+
+	if (subseq_len == 0)
+		subseq_len = cmd->chanlist_len;
+
+	if ((cmd->chanlist_len % subseq_len) != 0) {
+		dev_dbg(dev->class_dev,
+			"%s: sequence must repeat exactly\n", __func__);
+		return -EINVAL;
+	}
+
+	/*
+	 * Buggy PCI230+ or PCI260+ requires channel 0 to be (first) in the
+	 * sequence if the sequence contains more than one channel. Hardware
+	 * versions 1 and 2 have the bug. There is no hardware version 3.
+	 *
+	 * Actually, there are two firmwares that report themselves as
+	 * hardware version 1 (the boards have different ADC chips with
+	 * slightly different timing requirements, which was supposed to
+	 * be invisible to software). The first one doesn't seem to have
+	 * the bug, but the second one does, and we can't tell them apart!
+	 */
+	if (devpriv->hwver > 0 && devpriv->hwver < 4) {
+		if (subseq_len > 1 && CR_CHAN(cmd->chanlist[0]) != 0) {
+			dev_info(dev->class_dev,
+				 "amplc_pci230: ai_cmdtest: Buggy PCI230+/260+ h/w version %u requires first channel of multi-channel sequence to be 0 (corrected in h/w version 4)\n",
+				 devpriv->hwver);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 static int pci230_ai_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
@@ -1740,136 +1824,9 @@ static int pci230_ai_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 4;
 
-	/* Step 5: check channel list if it exists. */
-
-	if (cmd->chanlist && cmd->chanlist_len > 0) {
-		enum {
-			seq_err = 1 << 0,
-			rangepair_err = 1 << 1,
-			polarity_err = 1 << 2,
-			aref_err = 1 << 3,
-			diffchan_err = 1 << 4,
-			buggy_chan0_err = 1 << 5
-		};
-		unsigned int errors;
-		unsigned int chan, prev_chan;
-		unsigned int range, prev_range;
-		unsigned int polarity, prev_polarity;
-		unsigned int aref, prev_aref;
-		unsigned int subseq_len;
-		unsigned int n;
-
-		subseq_len = 0;
-		errors = 0;
-		prev_chan = prev_aref = prev_range = prev_polarity = 0;
-		for (n = 0; n < cmd->chanlist_len; n++) {
-			chan = CR_CHAN(cmd->chanlist[n]);
-			range = CR_RANGE(cmd->chanlist[n]);
-			aref = CR_AREF(cmd->chanlist[n]);
-			polarity = pci230_ai_bipolar[range];
-			/* Only the first half of the channels are available if
-			 * differential.  (These are remapped in software.  In
-			 * hardware, only the even channels are available.) */
-			if ((aref == AREF_DIFF)
-			    && (chan >= (s->n_chan / 2))) {
-				errors |= diffchan_err;
-			}
-			if (n > 0) {
-				/* Channel numbers must strictly increase or
-				 * subsequence must repeat exactly. */
-				if ((chan <= prev_chan)
-				    && (subseq_len == 0)) {
-					subseq_len = n;
-				}
-				if ((subseq_len > 0)
-				    && (cmd->chanlist[n] !=
-					cmd->chanlist[n % subseq_len])) {
-					errors |= seq_err;
-				}
-				/* Channels must have same AREF. */
-				if (aref != prev_aref)
-					errors |= aref_err;
-
-				/* Channel ranges must have same polarity. */
-				if (polarity != prev_polarity)
-					errors |= polarity_err;
-
-				/* Single-ended channel pairs must have same
-				 * range.  */
-				if ((aref != AREF_DIFF)
-				    && (((chan ^ prev_chan) & ~1) == 0)
-				    && (range != prev_range)) {
-					errors |= rangepair_err;
-				}
-			}
-			prev_chan = chan;
-			prev_range = range;
-			prev_aref = aref;
-			prev_polarity = polarity;
-		}
-		if (subseq_len == 0) {
-			/* Subsequence is whole sequence. */
-			subseq_len = n;
-		}
-		/* If channel list is a repeating subsequence, need a whole
-		 * number of repeats. */
-		if ((n % subseq_len) != 0)
-			errors |= seq_err;
-
-		if ((devpriv->hwver > 0) && (devpriv->hwver < 4)) {
-			/*
-			 * Buggy PCI230+ or PCI260+ requires channel 0 to be
-			 * (first) in the sequence if the sequence contains
-			 * more than one channel.  Hardware versions 1 and 2
-			 * have the bug.  There is no hardware version 3.
-			 *
-			 * Actually, there are two firmwares that report
-			 * themselves as hardware version 1 (the boards
-			 * have different ADC chips with slightly different
-			 * timing requirements, which was supposed to be
-			 * invisible to software).  The first one doesn't
-			 * seem to have the bug, but the second one
-			 * does, and we can't tell them apart!
-			 */
-			if ((subseq_len > 1)
-			    && (CR_CHAN(cmd->chanlist[0]) != 0)) {
-				errors |= buggy_chan0_err;
-			}
-		}
-		if (errors != 0) {
-			err++;
-			if ((errors & seq_err) != 0) {
-				dev_dbg(dev->class_dev,
-					"%s: channel numbers must increase or sequence must repeat exactly\n",
-					__func__);
-			}
-			if ((errors & rangepair_err) != 0) {
-				dev_dbg(dev->class_dev,
-					"%s: single-ended channel pairs must have the same range\n",
-					__func__);
-			}
-			if ((errors & polarity_err) != 0) {
-				dev_dbg(dev->class_dev,
-					"%s: channel sequence ranges must be all bipolar or all unipolar\n",
-					__func__);
-			}
-			if ((errors & aref_err) != 0) {
-				dev_dbg(dev->class_dev,
-					"%s: channel sequence analogue references must be all the same (single-ended or differential)\n",
-					__func__);
-			}
-			if ((errors & diffchan_err) != 0) {
-				dev_dbg(dev->class_dev,
-					"%s: differential channel number out of range 0 to %u\n",
-					__func__, (s->n_chan / 2) - 1);
-			}
-			if ((errors & buggy_chan0_err) != 0) {
-				dev_info(dev->class_dev,
-					 "amplc_pci230: ai_cmdtest: Buggy PCI230+/260+ h/w version %u requires first channel of multi-channel sequence to be 0 (corrected in h/w version 4)\n",
-					 devpriv->hwver);
-			}
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= pci230_ai_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -1891,9 +1848,9 @@ static void pci230_ai_update_fifo_trigger_level(struct comedi_device *dev,
 		/* Wake at end of scan. */
 		wake = scanlen - devpriv->ai_scan_pos;
 	} else {
-		if (devpriv->ai_continuous
-		    || (devpriv->ai_scan_count >= PCI230_ADC_FIFOLEVEL_HALFFULL)
-		    || (scanlen >= PCI230_ADC_FIFOLEVEL_HALFFULL)) {
+		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)
@@ -2044,7 +2001,7 @@ static void pci230_ai_start(struct comedi_device *dev,
 	struct comedi_cmd *cmd = &async->cmd;
 
 	set_bit(AI_CMD_STARTED, &devpriv->state);
-	if (!devpriv->ai_continuous && (devpriv->ai_scan_count == 0)) {
+	if (cmd->stop_src == TRIG_COUNT && devpriv->ai_scan_count == 0) {
 		/* An empty acquisition! */
 		async->events |= COMEDI_CB_EOA;
 		pci230_ai_stop(dev, s);
@@ -2177,7 +2134,9 @@ static int pci230_ai_inttrig_start(struct comedi_device *dev,
 				   struct comedi_subdevice *s,
 				   unsigned int trig_num)
 {
-	if (trig_num != 0)
+	struct comedi_cmd *cmd = &s->async->cmd;
+
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	s->async->inttrig = NULL;
@@ -2190,16 +2149,17 @@ static void pci230_handle_ai(struct comedi_device *dev,
 			     struct comedi_subdevice *s)
 {
 	struct pci230_private *devpriv = dev->private;
+	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
+	unsigned int scanlen = cmd->scan_end_arg;
 	unsigned int events = 0;
 	unsigned int status_fifo;
 	unsigned int i;
 	unsigned int todo;
 	unsigned int fifoamount;
-	struct comedi_async *async = s->async;
-	unsigned int scanlen = async->cmd.scan_end_arg;
 
 	/* Determine number of samples to read. */
-	if (devpriv->ai_continuous) {
+	if (cmd->stop_src != TRIG_COUNT) {
 		todo = PCI230_ADC_FIFOLEVEL_HALFFULL;
 	} else if (devpriv->ai_scan_count == 0) {
 		todo = 0;
@@ -2247,7 +2207,7 @@ static void pci230_handle_ai(struct comedi_device *dev,
 			}
 		}
 		/* Read sample and store in Comedi's circular buffer. */
-		if (comedi_buf_put(async, pci230_ai_read(dev)) == 0) {
+		if (comedi_buf_put(s, pci230_ai_read(dev)) == 0) {
 			events |= COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW;
 			comedi_error(dev, "AI buffer overflow");
 			break;
@@ -2261,7 +2221,7 @@ static void pci230_handle_ai(struct comedi_device *dev,
 			async->events |= COMEDI_CB_EOS;
 		}
 	}
-	if (!devpriv->ai_continuous && (devpriv->ai_scan_count == 0)) {
+	if (cmd->stop_src == TRIG_COUNT && devpriv->ai_scan_count == 0) {
 		/* End of acquisition. */
 		events |= COMEDI_CB_EOA;
 	} else {
@@ -2312,14 +2272,10 @@ static int pci230_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 
 	/* Get number of scans required. */
-	if (cmd->stop_src == TRIG_COUNT) {
+	if (cmd->stop_src == TRIG_COUNT)
 		devpriv->ai_scan_count = cmd->stop_arg;
-		devpriv->ai_continuous = 0;
-	} else {
-		/* TRIG_NONE, user calls cancel. */
+	else	/* TRIG_NONE, user calls cancel */
 		devpriv->ai_scan_count = 0;
-		devpriv->ai_continuous = 1;
-	}
 	devpriv->ai_scan_pos = 0;	/* Position within scan. */
 
 	/* Steps;
@@ -2463,12 +2419,10 @@ static int pci230_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		}
 	}
 
-	if (cmd->start_src == TRIG_INT) {
+	if (cmd->start_src == TRIG_INT)
 		s->async->inttrig = pci230_ai_inttrig_start;
-	} else {
-		/* TRIG_NOW */
+	else	/* TRIG_NOW */
 		pci230_ai_start(dev, s);
-	}
 
 	return 0;
 }
diff --git a/drivers/staging/comedi/drivers/cb_das16_cs.c b/drivers/staging/comedi/drivers/cb_das16_cs.c
index 645fcb0cf17..eb1b92d72e8 100644
--- a/drivers/staging/comedi/drivers/cb_das16_cs.c
+++ b/drivers/staging/comedi/drivers/cb_das16_cs.c
@@ -184,6 +184,7 @@ static int das16cs_ao_winsn(struct comedi_device *dev,
 
 		for (bit = 15; bit >= 0; bit--) {
 			int b = (d >> bit) & 0x1;
+
 			b <<= 1;
 			outw(status1 | b | 0x0000, dev->iobase + DAS16CS_MISC1);
 			udelay(1);
diff --git a/drivers/staging/comedi/drivers/cb_pcidas.c b/drivers/staging/comedi/drivers/cb_pcidas.c
index 83a265f3408..7377da1aff7 100644
--- a/drivers/staging/comedi/drivers/cb_pcidas.c
+++ b/drivers/staging/comedi/drivers/cb_pcidas.c
@@ -803,6 +803,33 @@ static int trimpot_read_insn(struct comedi_device *dev,
 	return 1;
 }
 
+static int cb_pcidas_ai_check_chanlist(struct comedi_device *dev,
+				       struct comedi_subdevice *s,
+				       struct comedi_cmd *cmd)
+{
+	unsigned int chan0 = CR_CHAN(cmd->chanlist[0]);
+	unsigned int range0 = CR_RANGE(cmd->chanlist[0]);
+	int i;
+
+	for (i = 1; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+		unsigned int range = CR_RANGE(cmd->chanlist[i]);
+
+		if (chan != (chan0 + i) % s->n_chan) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must be consecutive channels, counting upwards\n");
+			return -EINVAL;
+		}
+
+		if (range != range0) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must all have the same gain\n");
+			return -EINVAL;
+		}
+	}
+	return 0;
+}
+
 static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_cmd *cmd)
@@ -810,8 +837,7 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 	const struct cb_pcidas_board *thisboard = comedi_board(dev);
 	struct cb_pcidas_private *devpriv = dev->private;
 	int err = 0;
-	int tmp;
-	int i, gain, start_chan;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -846,9 +872,12 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 2;
 
-	/* step 3: arguments are trivially compatible */
+	/* 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:
 		/* External trigger, only CR_EDGE and CR_INVERT flags allowed */
 		if ((cmd->start_arg
@@ -862,9 +891,6 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 			err |= -EINVAL;
 		}
 		break;
-	default:
-		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-		break;
 	}
 
 	if (cmd->scan_begin_src == TRIG_TIMER)
@@ -886,45 +912,28 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
+		arg = cmd->scan_begin_arg;
 		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
 					  &devpriv->divisor1,
 					  &devpriv->divisor2,
-					  &cmd->scan_begin_arg, cmd->flags);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
+		arg = cmd->convert_arg;
 		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
 					  &devpriv->divisor1,
 					  &devpriv->divisor2,
-					  &cmd->convert_arg, cmd->flags);
-		if (tmp != cmd->convert_arg)
-			err++;
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
 		return 4;
 
-	/*  check channel/gain list against card's limitations */
-	if (cmd->chanlist) {
-		gain = CR_RANGE(cmd->chanlist[0]);
-		start_chan = CR_CHAN(cmd->chanlist[0]);
-		for (i = 1; i < cmd->chanlist_len; i++) {
-			if (CR_CHAN(cmd->chanlist[i]) !=
-			    (start_chan + i) % s->n_chan) {
-				comedi_error(dev,
-					     "entries in chanlist must be consecutive channels, counting upwards\n");
-				err++;
-			}
-			if (CR_RANGE(cmd->chanlist[i]) != gain) {
-				comedi_error(dev,
-					     "entries in chanlist must all have the same gain\n");
-				err++;
-			}
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= cb_pcidas_ai_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -932,20 +941,16 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 	return 0;
 }
 
-static void cb_pcidas_load_counters(struct comedi_device *dev, unsigned int *ns,
-				    int rounding_flags)
+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;
 
-	i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
-				  &devpriv->divisor1, &devpriv->divisor2,
-				  ns, rounding_flags);
+	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
+	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
 
-	/* Write the values of ctr1 and ctr2 into counters 1 and 2 */
-	i8254_load(devpriv->pacer_counter_dio + ADC8254, 0, 1,
-		   devpriv->divisor1, 2);
-	i8254_load(devpriv->pacer_counter_dio + ADC8254, 0, 2,
-		   devpriv->divisor2, 2);
+	i8254_write(timer_base, 0, 1, devpriv->divisor1);
+	i8254_write(timer_base, 0, 2, devpriv->divisor2);
 }
 
 static int cb_pcidas_ai_cmd(struct comedi_device *dev,
@@ -983,12 +988,8 @@ static int cb_pcidas_ai_cmd(struct comedi_device *dev,
 	outw(bits, devpriv->control_status + ADCMUX_CONT);
 
 	/*  load counters */
-	if (cmd->convert_src == TRIG_TIMER)
-		cb_pcidas_load_counters(dev, &cmd->convert_arg,
-					cmd->flags & TRIG_ROUND_MASK);
-	else if (cmd->scan_begin_src == TRIG_TIMER)
-		cb_pcidas_load_counters(dev, &cmd->scan_begin_arg,
-					cmd->flags & TRIG_ROUND_MASK);
+	if (cmd->scan_begin_src == TRIG_TIMER || cmd->convert_src == TRIG_TIMER)
+		cb_pcidas_ai_load_counters(dev);
 
 	/*  set number of conversions */
 	if (cmd->stop_src == TRIG_COUNT)
@@ -1035,6 +1036,25 @@ static int cb_pcidas_ai_cmd(struct comedi_device *dev,
 	return 0;
 }
 
+static int cb_pcidas_ao_check_chanlist(struct comedi_device *dev,
+				       struct comedi_subdevice *s,
+				       struct comedi_cmd *cmd)
+{
+	unsigned int chan0 = CR_CHAN(cmd->chanlist[0]);
+
+	if (cmd->chanlist_len > 1) {
+		unsigned int chan1 = CR_CHAN(cmd->chanlist[1]);
+
+		if (chan0 != 0 || chan1 != 1) {
+			dev_dbg(dev->class_dev,
+				"channels must be ordered channel 0, channel 1 in chanlist\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 static int cb_pcidas_ao_cmdtest(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_cmd *cmd)
@@ -1042,7 +1062,7 @@ static int cb_pcidas_ao_cmdtest(struct comedi_device *dev,
 	const struct cb_pcidas_board *thisboard = comedi_board(dev);
 	struct cb_pcidas_private *devpriv = dev->private;
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -1085,27 +1105,20 @@ static int cb_pcidas_ao_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
+		arg = cmd->scan_begin_arg;
 		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
 					  &devpriv->ao_divisor1,
 					  &devpriv->ao_divisor2,
-					  &cmd->scan_begin_arg, cmd->flags);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (err)
 		return 4;
 
-	/*  check channel/gain list against card's limitations */
-	if (cmd->chanlist && cmd->chanlist_len > 1) {
-		if (CR_CHAN(cmd->chanlist[0]) != 0 ||
-		    CR_CHAN(cmd->chanlist[1]) != 1) {
-			comedi_error(dev,
-				     "channels must be ordered channel 0, channel 1 in chanlist\n");
-			err++;
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= cb_pcidas_ao_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -1145,7 +1158,7 @@ static int cb_pcidas_ao_inttrig(struct comedi_device *dev,
 	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned long flags;
 
-	if (trig_num != 0)
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	/*  load up fifo */
@@ -1180,6 +1193,18 @@ static int cb_pcidas_ao_inttrig(struct comedi_device *dev,
 	return 0;
 }
 
+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;
+
+	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
+	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
+
+	i8254_write(timer_base, 0, 1, devpriv->ao_divisor1);
+	i8254_write(timer_base, 0, 2, devpriv->ao_divisor2);
+}
+
 static int cb_pcidas_ao_cmd(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
@@ -1209,18 +1234,9 @@ static int cb_pcidas_ao_cmd(struct comedi_device *dev,
 	outw(0, devpriv->ao_registers + DACFIFOCLR);
 
 	/*  load counters */
-	if (cmd->scan_begin_src == TRIG_TIMER) {
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
-					  &devpriv->ao_divisor1,
-					  &devpriv->ao_divisor2,
-					  &cmd->scan_begin_arg, cmd->flags);
+	if (cmd->scan_begin_src == TRIG_TIMER)
+		cb_pcidas_ao_load_counters(dev);
 
-		/* Write the values of ctr1 and ctr2 into counters 1 and 2 */
-		i8254_load(devpriv->pacer_counter_dio + DAC8254, 0, 1,
-			   devpriv->ao_divisor1, 2);
-		i8254_load(devpriv->pacer_counter_dio + DAC8254, 0, 2,
-			   devpriv->ao_divisor2, 2);
-	}
 	/*  set number of conversions */
 	if (cmd->stop_src == TRIG_COUNT)
 		devpriv->ao_count = cmd->chanlist_len * cmd->stop_arg;
@@ -1305,7 +1321,7 @@ static void handle_ao_interrupt(struct comedi_device *dev, unsigned int status)
 					       num_points * sizeof(short));
 		num_points = num_bytes / sizeof(short);
 
-		if (async->cmd.stop_src == TRIG_COUNT)
+		if (cmd->stop_src == TRIG_COUNT)
 			devpriv->ao_count -= num_points;
 		/*  write data to board's fifo */
 		outsw(devpriv->ao_registers + DACDATA, devpriv->ao_buffer,
@@ -1327,6 +1343,7 @@ static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 	struct cb_pcidas_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
 	struct comedi_async *async;
+	struct comedi_cmd *cmd;
 	int status, s5933_status;
 	int half_fifo = thisboard->fifo_size / 2;
 	unsigned int num_samples, i;
@@ -1337,6 +1354,7 @@ static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 		return IRQ_NONE;
 
 	async = s->async;
+	cmd = &async->cmd;
 
 	s5933_status = inl(devpriv->s5933_config + AMCC_OP_REG_INTCSR);
 
@@ -1359,7 +1377,7 @@ static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 	if (status & ADHFI) {
 		/*  read data */
 		num_samples = half_fifo;
-		if (async->cmd.stop_src == TRIG_COUNT &&
+		if (cmd->stop_src == TRIG_COUNT &&
 		    num_samples > devpriv->count) {
 			num_samples = devpriv->count;
 		}
@@ -1368,7 +1386,7 @@ static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 		cfc_write_array_to_buffer(s, devpriv->ai_buffer,
 					  num_samples * sizeof(short));
 		devpriv->count -= num_samples;
-		if (async->cmd.stop_src == TRIG_COUNT && devpriv->count == 0)
+		if (cmd->stop_src == TRIG_COUNT && devpriv->count == 0)
 			async->events |= COMEDI_CB_EOA;
 		/*  clear half-full interrupt latch */
 		spin_lock_irqsave(&dev->spinlock, flags);
@@ -1383,7 +1401,7 @@ static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 					INT_ADCFIFO)) == 0)
 				break;
 			cfc_write_to_buffer(s, inw(devpriv->adc_fifo));
-			if (async->cmd.stop_src == TRIG_COUNT &&
+			if (cmd->stop_src == TRIG_COUNT &&
 			    --devpriv->count == 0) {
 				/* end of acquisition */
 				async->events |= COMEDI_CB_EOA;
diff --git a/drivers/staging/comedi/drivers/cb_pcidas64.c b/drivers/staging/comedi/drivers/cb_pcidas64.c
index f9afcbe1da5..035c3a17600 100644
--- a/drivers/staging/comedi/drivers/cb_pcidas64.c
+++ b/drivers/staging/comedi/drivers/cb_pcidas64.c
@@ -1995,14 +1995,52 @@ static void check_adc_timing(struct comedi_device *dev, struct comedi_cmd *cmd)
 	return;
 }
 
+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);
+	unsigned int aref0 = CR_AREF(cmd->chanlist[0]);
+	int i;
+
+	for (i = 1; i < cmd->chanlist_len; i++) {
+		unsigned int aref = CR_AREF(cmd->chanlist[i]);
+
+		if (aref != aref0) {
+			dev_dbg(dev->class_dev,
+				"all elements in chanlist must use the same analog reference\n");
+			return -EINVAL;
+		}
+	}
+
+	if (board->layout == LAYOUT_4020) {
+		unsigned int chan0 = CR_CHAN(cmd->chanlist[0]);
+
+		for (i = 1; i < cmd->chanlist_len; i++) {
+			unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+
+			if (chan != (chan0 + i)) {
+				dev_dbg(dev->class_dev,
+					"chanlist must use consecutive channels\n");
+				return -EINVAL;
+			}
+		}
+		if (cmd->chanlist_len == 3) {
+			dev_dbg(dev->class_dev,
+				"chanlist cannot be 3 channels long, use 1, 2, or 4 channels\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		      struct comedi_cmd *cmd)
 {
 	const struct pcidas64_board *thisboard = comedi_board(dev);
 	int err = 0;
 	unsigned int tmp_arg, tmp_arg2;
-	int i;
-	int aref;
 	unsigned int triggers;
 
 	/* Step 1 : check if triggers are trivially valid */
@@ -2040,15 +2078,24 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	if (cmd->convert_src == TRIG_EXT && cmd->scan_begin_src == TRIG_TIMER)
 		err |= -EINVAL;
-	if (cmd->stop_src != TRIG_COUNT &&
-	    cmd->stop_src != TRIG_NONE && cmd->stop_src != TRIG_EXT)
-		err |= -EINVAL;
 
 	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 is the CR_CHAN | CR_INVERT of the
+		 * external trigger.
+		 */
+		break;
+	}
+
 	if (cmd->convert_src == TRIG_TIMER) {
 		if (thisboard->layout == LAYOUT_4020) {
 			err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
@@ -2098,36 +2145,9 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	if (err)
 		return 4;
 
-	/*  make sure user is doesn't change analog reference mid chanlist */
-	if (cmd->chanlist) {
-		aref = CR_AREF(cmd->chanlist[0]);
-		for (i = 1; i < cmd->chanlist_len; i++) {
-			if (aref != CR_AREF(cmd->chanlist[i])) {
-				comedi_error(dev,
-					     "all elements in chanlist must use the same analog reference");
-				err++;
-				break;
-			}
-		}
-		/*  check 4020 chanlist */
-		if (thisboard->layout == LAYOUT_4020) {
-			unsigned int first_channel = CR_CHAN(cmd->chanlist[0]);
-			for (i = 1; i < cmd->chanlist_len; i++) {
-				if (CR_CHAN(cmd->chanlist[i]) !=
-				    first_channel + i) {
-					comedi_error(dev,
-						     "chanlist must use consecutive channels");
-					err++;
-					break;
-				}
-			}
-			if (cmd->chanlist_len == 3) {
-				comedi_error(dev,
-					     "chanlist cannot be 3 channels long, use 1, 2, or 4 channels");
-				err++;
-			}
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= cb_pcidas64_ai_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -2703,6 +2723,7 @@ static void drain_dma_buffers(struct comedi_device *dev, unsigned int channel)
 	const struct pcidas64_board *thisboard = comedi_board(dev);
 	struct pcidas64_private *devpriv = dev->private;
 	struct comedi_async *async = dev->read_subdev->async;
+	struct comedi_cmd *cmd = &async->cmd;
 	uint32_t next_transfer_addr;
 	int j;
 	int num_samples = 0;
@@ -2724,7 +2745,7 @@ static void drain_dma_buffers(struct comedi_device *dev, unsigned int channel)
 	      DMA_BUFFER_SIZE) && j < ai_dma_ring_count(thisboard); j++) {
 		/*  transfer data from dma buffer to comedi buffer */
 		num_samples = dma_transfer_size(dev);
-		if (async->cmd.stop_src == TRIG_COUNT) {
+		if (cmd->stop_src == TRIG_COUNT) {
 			if (num_samples > devpriv->ai_count)
 				num_samples = devpriv->ai_count;
 			devpriv->ai_count -= num_samples;
@@ -2873,7 +2894,7 @@ static unsigned int load_ao_dma_buffer(struct comedi_device *dev,
 	buffer_index = devpriv->ao_dma_index;
 	prev_buffer_index = prev_ao_dma_index(dev);
 
-	num_bytes = comedi_buf_read_n_available(dev->write_subdev->async);
+	num_bytes = comedi_buf_read_n_available(dev->write_subdev);
 	if (num_bytes > DMA_BUFFER_SIZE)
 		num_bytes = DMA_BUFFER_SIZE;
 	if (cmd->stop_src == TRIG_COUNT && num_bytes > devpriv->ao_count)
@@ -3184,15 +3205,17 @@ static int prep_ao_dma(struct comedi_device *dev, const struct comedi_cmd *cmd)
 	return 0;
 }
 
-static inline int external_ai_queue_in_use(struct comedi_device *dev)
+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);
 
-	if (dev->read_subdev->busy)
+	if (s->busy)
 		return 0;
 	if (thisboard->layout == LAYOUT_4020)
 		return 0;
-	else if (use_internal_queue_6xxx(&dev->read_subdev->async->cmd))
+	else if (use_internal_queue_6xxx(cmd))
 		return 0;
 	return 1;
 }
@@ -3204,7 +3227,7 @@ static int ao_inttrig(struct comedi_device *dev, struct comedi_subdevice *s,
 	struct comedi_cmd *cmd = &s->async->cmd;
 	int retval;
 
-	if (trig_num != 0)
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	retval = prep_ao_dma(dev, cmd);
@@ -3226,7 +3249,7 @@ static int ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	struct pcidas64_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 
-	if (external_ai_queue_in_use(dev)) {
+	if (external_ai_queue_in_use(dev, s, cmd)) {
 		warn_external_queue(dev);
 		return -EBUSY;
 	}
@@ -3247,13 +3270,32 @@ static int ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	return 0;
 }
 
+static int cb_pcidas64_ao_check_chanlist(struct comedi_device *dev,
+					 struct comedi_subdevice *s,
+					 struct comedi_cmd *cmd)
+{
+	unsigned int chan0 = CR_CHAN(cmd->chanlist[0]);
+	int i;
+
+	for (i = 1; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+
+		if (chan != (chan0 + i)) {
+			dev_dbg(dev->class_dev,
+				"chanlist must use consecutive channels\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 static int ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		      struct comedi_cmd *cmd)
 {
 	const struct pcidas64_board *thisboard = comedi_board(dev);
 	int err = 0;
 	unsigned int tmp_arg;
-	int i;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -3285,6 +3327,8 @@ static int ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	/* Step 3: check if arguments are trivially valid */
 
+	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
 						 thisboard->ao_scan_speed);
@@ -3315,17 +3359,9 @@ static int ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	if (err)
 		return 4;
 
-	if (cmd->chanlist) {
-		unsigned int first_channel = CR_CHAN(cmd->chanlist[0]);
-		for (i = 1; i < cmd->chanlist_len; i++) {
-			if (CR_CHAN(cmd->chanlist[i]) != first_channel + i) {
-				comedi_error(dev,
-					     "chanlist must use consecutive channels");
-				err++;
-				break;
-			}
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= cb_pcidas64_ao_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
diff --git a/drivers/staging/comedi/drivers/cb_pcimdas.c b/drivers/staging/comedi/drivers/cb_pcimdas.c
index d3141c865fe..50e522e6e69 100644
--- a/drivers/staging/comedi/drivers/cb_pcimdas.c
+++ b/drivers/staging/comedi/drivers/cb_pcimdas.c
@@ -31,7 +31,8 @@ Configuration Options:
 
 Developed from cb_pcidas and skel by Richard Bytheway (mocelet@sucs.org).
 Only supports DIO, AO and simple AI in it's present form.
-No interrupts, multi channel or FIFO AI, although the card looks like it could support this.
+No interrupts, multi channel or FIFO AI,
+although the card looks like it could support this.
 See http://www.mccdaq.com/PDFs/Manuals/pcim-das1602-16.pdf for more details.
 */
 
@@ -128,8 +129,12 @@ static int cb_pcimdas_ai_rinsn(struct comedi_device *dev,
 		d = d & 0xfd;
 		outb(d, devpriv->BADR3 + 5);
 	}
-	outb(0x01, devpriv->BADR3 + 6);	/* set bursting off, conversions on */
-	outb(0x00, devpriv->BADR3 + 7);	/* set range to 10V. UP/BP is controlled by a switch on the board */
+
+	/* set bursting off, conversions on */
+	outb(0x01, devpriv->BADR3 + 6);
+
+	/* set range to 10V. UP/BP is controlled by a switch on the board */
+	outb(0x00, devpriv->BADR3 + 7);
 
 	/*
 	 * write channel limits to multiplexer, set Low (bits 0-3) and
diff --git a/drivers/staging/comedi/drivers/comedi_fc.c b/drivers/staging/comedi/drivers/comedi_fc.c
index 9d9b1469e89..c33c3e5680a 100644
--- a/drivers/staging/comedi/drivers/comedi_fc.c
+++ b/drivers/staging/comedi/drivers/comedi_fc.c
@@ -24,7 +24,7 @@
 
 unsigned int cfc_bytes_per_scan(struct comedi_subdevice *s)
 {
-	unsigned int chanlist_len = s->async->cmd.chanlist_len;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int num_samples;
 	unsigned int bits_per_sample;
 
@@ -33,11 +33,11 @@ unsigned int cfc_bytes_per_scan(struct comedi_subdevice *s)
 	case COMEDI_SUBD_DO:
 	case COMEDI_SUBD_DIO:
 		bits_per_sample = 8 * bytes_per_sample(s);
-		num_samples = (chanlist_len + bits_per_sample - 1) /
+		num_samples = (cmd->chanlist_len + bits_per_sample - 1) /
 				bits_per_sample;
 		break;
 	default:
-		num_samples = chanlist_len;
+		num_samples = cmd->chanlist_len;
 		break;
 	}
 	return num_samples * bytes_per_sample(s);
@@ -67,15 +67,15 @@ unsigned int cfc_write_array_to_buffer(struct comedi_subdevice *s,
 	if (num_bytes == 0)
 		return 0;
 
-	retval = comedi_buf_write_alloc(async, num_bytes);
+	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(async, 0, data, num_bytes);
-	comedi_buf_write_free(async, num_bytes);
+	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;
 
@@ -86,16 +86,14 @@ 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)
 {
-	struct comedi_async *async = s->async;
-
 	if (num_bytes == 0)
 		return 0;
 
-	num_bytes = comedi_buf_read_alloc(async, num_bytes);
-	comedi_buf_memcpy_from(async, 0, data, num_bytes);
-	comedi_buf_read_free(async, num_bytes);
+	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);
-	async->events |= COMEDI_CB_BLOCK;
+	s->async->events |= COMEDI_CB_BLOCK;
 
 	return num_bytes;
 }
diff --git a/drivers/staging/comedi/drivers/comedi_parport.c b/drivers/staging/comedi/drivers/comedi_parport.c
index 9de81c7712f..a4274869235 100644
--- a/drivers/staging/comedi/drivers/comedi_parport.c
+++ b/drivers/staging/comedi/drivers/comedi_parport.c
@@ -181,7 +181,7 @@ static int parport_intr_cmdtest(struct comedi_device *dev,
 	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, 1);
+	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)
@@ -229,7 +229,7 @@ static irqreturn_t parport_interrupt(int irq, void *d)
 	if (!(ctrl & PARPORT_CTRL_IRQ_ENA))
 		return IRQ_NONE;
 
-	comedi_buf_put(s->async, 0);
+	comedi_buf_put(s, 0);
 	s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
 
 	comedi_event(dev, s);
@@ -295,6 +295,7 @@ static int parport_attach(struct comedi_device *dev,
 		s->maxdata	= 1;
 		s->range_table	= &range_digital;
 		s->insn_bits	= parport_intr_insn_bits;
+		s->len_chanlist	= 1;
 		s->do_cmdtest	= parport_intr_cmdtest;
 		s->do_cmd	= parport_intr_cmd;
 		s->cancel	= parport_intr_cancel;
diff --git a/drivers/staging/comedi/drivers/comedi_test.c b/drivers/staging/comedi/drivers/comedi_test.c
index cd9562556d2..67a09aa6b72 100644
--- a/drivers/staging/comedi/drivers/comedi_test.c
+++ b/drivers/staging/comedi/drivers/comedi_test.c
@@ -188,6 +188,7 @@ static void waveform_ai_interrupt(unsigned long arg)
 
 	if (cmd->stop_src == TRIG_COUNT) {
 		unsigned int remaining = cmd->stop_arg - devpriv->ai_count;
+
 		if (num_scans >= remaining) {
 			/* about to finish */
 			num_scans = remaining;
@@ -198,6 +199,7 @@ static void waveform_ai_interrupt(unsigned long arg)
 	for (i = 0; i < num_scans; i++) {
 		for (j = 0; j < cmd->chanlist_len; j++) {
 			unsigned short sample;
+
 			sample = fake_waveform(dev, CR_CHAN(cmd->chanlist[j]),
 					       CR_RANGE(cmd->chanlist[j]),
 					       devpriv->usec_current +
@@ -224,7 +226,7 @@ static int waveform_ai_cmdtest(struct comedi_device *dev,
 			       struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -276,22 +278,18 @@ static int waveform_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
+		arg = cmd->scan_begin_arg;
 		/* round to nearest microsec */
-		cmd->scan_begin_arg =
-		    nano_per_micro * ((tmp +
-				       (nano_per_micro / 2)) / nano_per_micro);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+		arg = nano_per_micro *
+		      ((arg + (nano_per_micro / 2)) / nano_per_micro);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
+		arg = cmd->convert_arg;
 		/* round to nearest microsec */
-		cmd->convert_arg =
-		    nano_per_micro * ((tmp +
-				       (nano_per_micro / 2)) / nano_per_micro);
-		if (tmp != cmd->convert_arg)
-			err++;
+		arg = nano_per_micro *
+		      ((arg + (nano_per_micro / 2)) / nano_per_micro);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
@@ -413,11 +411,7 @@ static int waveform_attach(struct comedi_device *dev,
 	s->n_chan = N_CHANS;
 	s->maxdata = 0xffff;
 	s->range_table = &waveform_ai_ranges;
-	s->len_chanlist = s->n_chan * 2;
 	s->insn_write = waveform_ao_insn_write;
-	s->do_cmd = NULL;
-	s->do_cmdtest = NULL;
-	s->cancel = NULL;
 
 	/* Our default loopback value is just a 0V flatline */
 	for (i = 0; i < s->n_chan; i++)
diff --git a/drivers/staging/comedi/drivers/das08.h b/drivers/staging/comedi/drivers/das08.h
index 46a314c5113..18cc170facd 100644
--- a/drivers/staging/comedi/drivers/das08.h
+++ b/drivers/staging/comedi/drivers/das08.h
@@ -40,7 +40,9 @@ struct das08_board_struct {
 };
 
 struct das08_private_struct {
-	unsigned int do_mux_bits;	/*  bits for do/mux register on boards without separate do register */
+	unsigned int do_mux_bits;	/*  bits for do/mux register on boards
+					 *  without separate do register
+					 */
 	const unsigned int *pg_gainlist;
 	unsigned int ao_readback[2];	/* assume 2 AO channels */
 };
diff --git a/drivers/staging/comedi/drivers/das16.c b/drivers/staging/comedi/drivers/das16.c
index 6a7d652ff56..2feecf199f2 100644
--- a/drivers/staging/comedi/drivers/das16.c
+++ b/drivers/staging/comedi/drivers/das16.c
@@ -600,29 +600,56 @@ static void das16_timer_interrupt(unsigned long arg)
 		mod_timer(&devpriv->timer, jiffies + timer_period());
 }
 
+static int das16_ai_check_chanlist(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   struct comedi_cmd *cmd)
+{
+	unsigned int chan0 = CR_CHAN(cmd->chanlist[0]);
+	unsigned int range0 = CR_RANGE(cmd->chanlist[0]);
+	int i;
+
+	for (i = 1; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+		unsigned int range = CR_RANGE(cmd->chanlist[i]);
+
+		if (chan != ((chan0 + i) % s->n_chan)) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must be consecutive channels, counting upwards\n");
+			return -EINVAL;
+		}
+
+		if (range != range0) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must all have the same gain\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 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);
 	struct das16_private_struct *devpriv = dev->private;
-	int err = 0, tmp;
-	int gain, start_chan, i;
-	int mask;
+	int err = 0;
+	unsigned int trig_mask;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
 	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
 
-	mask = TRIG_FOLLOW;
+	trig_mask = TRIG_FOLLOW;
 	if (devpriv->can_burst)
-		mask |= TRIG_TIMER | TRIG_EXT;
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, mask);
+		trig_mask |= TRIG_TIMER | TRIG_EXT;
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src, trig_mask);
 
-	tmp = cmd->convert_src;
-	mask = TRIG_TIMER | TRIG_EXT;
+	trig_mask = TRIG_TIMER | TRIG_EXT;
 	if (devpriv->can_burst)
-		mask |= TRIG_NOW;
-	err |= cfc_check_trigger_src(&cmd->convert_src, mask);
+		trig_mask |= TRIG_NOW;
+	err |= cfc_check_trigger_src(&cmd->convert_src, trig_mask);
 
 	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
 	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
@@ -673,44 +700,28 @@ static int das16_cmd_test(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	/*  step 4: fix up arguments */
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		unsigned int tmp = cmd->scan_begin_arg;
-		/*  set divisors, correct timing arguments */
+		arg = cmd->scan_begin_arg;
 		i8253_cascade_ns_to_timer(devpriv->clockbase,
 					  &devpriv->divisor1,
 					  &devpriv->divisor2,
-					  &cmd->scan_begin_arg, cmd->flags);
-		err += (tmp != cmd->scan_begin_arg);
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 	if (cmd->convert_src == TRIG_TIMER) {
-		unsigned int tmp = cmd->convert_arg;
-		/*  set divisors, correct timing arguments */
+		arg = cmd->convert_arg;
 		i8253_cascade_ns_to_timer(devpriv->clockbase,
 					  &devpriv->divisor1,
 					  &devpriv->divisor2,
-					  &cmd->convert_arg, cmd->flags);
-		err += (tmp != cmd->convert_arg);
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 	if (err)
 		return 4;
 
-	/*  check channel/gain list against card's limitations */
-	if (cmd->chanlist) {
-		gain = CR_RANGE(cmd->chanlist[0]);
-		start_chan = CR_CHAN(cmd->chanlist[0]);
-		for (i = 1; i < cmd->chanlist_len; i++) {
-			if (CR_CHAN(cmd->chanlist[i]) !=
-			    (start_chan + i) % s->n_chan) {
-				dev_err(dev->class_dev,
-					"entries in chanlist must be consecutive channels, counting upwards\n");
-				err++;
-			}
-			if (CR_RANGE(cmd->chanlist[i]) != gain) {
-				dev_err(dev->class_dev,
-					"entries in chanlist must all have the same gain\n");
-				err++;
-			}
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= das16_ai_check_chanlist(dev, s, cmd);
+
 	if (err)
 		return 5;
 
@@ -727,9 +738,10 @@ static unsigned int das16_set_pacer(struct comedi_device *dev, unsigned int ns,
 				  &devpriv->divisor1, &devpriv->divisor2,
 				  &ns, rounding_flags);
 
-	/* Write the values of ctr1 and ctr2 into counters 1 and 2 */
-	i8254_load(timer_base, 0, 1, devpriv->divisor1, 2);
-	i8254_load(timer_base, 0, 2, devpriv->divisor2, 2);
+	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
+	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
+	i8254_write(timer_base, 0, 1, devpriv->divisor1);
+	i8254_write(timer_base, 0, 2, devpriv->divisor2);
 
 	return ns;
 }
@@ -750,8 +762,7 @@ static int das16_cmd_exec(struct comedi_device *dev, struct comedi_subdevice *s)
 		return -1;
 	}
 
-	devpriv->adc_byte_count =
-	    cmd->stop_arg * cmd->chanlist_len * sizeof(uint16_t);
+	devpriv->adc_byte_count = cmd->stop_arg * cfc_bytes_per_scan(s);
 
 	if (devpriv->can_burst)
 		outb(DAS1600_CONV_DISABLE, dev->iobase + DAS1600_CONV_REG);
diff --git a/drivers/staging/comedi/drivers/das16m1.c b/drivers/staging/comedi/drivers/das16m1.c
index 779225831dc..ec039fbff0f 100644
--- a/drivers/staging/comedi/drivers/das16m1.c
+++ b/drivers/staging/comedi/drivers/das16m1.c
@@ -150,11 +150,40 @@ static void munge_sample_array(unsigned short *array, unsigned int num_elements)
 		array[i] = munge_sample(array[i]);
 }
 
+static int das16m1_ai_check_chanlist(struct comedi_device *dev,
+				     struct comedi_subdevice *s,
+				     struct comedi_cmd *cmd)
+{
+	int i;
+
+	if (cmd->chanlist_len == 1)
+		return 0;
+
+	if ((cmd->chanlist_len % 2) != 0) {
+		dev_dbg(dev->class_dev,
+			"chanlist must be of even length or length 1\n");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+
+		if ((i % 2) != (chan % 2)) {
+			dev_dbg(dev->class_dev,
+				 "even/odd channels must go have even/odd chanlist indices\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 static int das16m1_cmd_test(struct comedi_device *dev,
 			    struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
 	struct das16m1_private_struct *devpriv = dev->private;
-	unsigned int err = 0, tmp, i;
+	int err = 0;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -203,35 +232,20 @@ static int das16m1_cmd_test(struct comedi_device *dev,
 	/* step 4: fix up arguments */
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
-		/* calculate counter values that give desired timing */
+		arg = cmd->convert_arg;
 		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
 					  &devpriv->divisor1,
 					  &devpriv->divisor2,
-					  &cmd->convert_arg, cmd->flags);
-		if (tmp != cmd->convert_arg)
-			err++;
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
 		return 4;
 
-	/*  check chanlist against board's peculiarities */
-	if (cmd->chanlist && cmd->chanlist_len > 1) {
-		for (i = 0; i < cmd->chanlist_len; i++) {
-			/*  even/odd channels must go into even/odd queue addresses */
-			if ((i % 2) != (CR_CHAN(cmd->chanlist[i]) % 2)) {
-				comedi_error(dev, "bad chanlist:\n"
-					     " even/odd channels must go have even/odd chanlist indices");
-				err++;
-			}
-		}
-		if ((cmd->chanlist_len % 2) != 0) {
-			comedi_error(dev,
-				     "chanlist must be of even length or length 1");
-			err++;
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= das16m1_ai_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -239,26 +253,16 @@ static int das16m1_cmd_test(struct comedi_device *dev,
 	return 0;
 }
 
-/* This function takes a time in nanoseconds and sets the     *
- * 2 pacer clocks to the closest frequency possible. It also  *
- * returns the actual sampling period.                        */
-static unsigned int das16m1_set_pacer(struct comedi_device *dev,
-				      unsigned int ns, int rounding_flags)
+static void das16m1_set_pacer(struct comedi_device *dev)
 {
 	struct das16m1_private_struct *devpriv = dev->private;
+	unsigned long timer_base = dev->iobase + DAS16M1_8254_SECOND;
 
-	i8253_cascade_ns_to_timer_2div(I8254_OSC_BASE_10MHZ,
-				       &devpriv->divisor1,
-				       &devpriv->divisor2,
-				       &ns, rounding_flags);
-
-	/* Write the values of ctr1 and ctr2 into counters 1 and 2 */
-	i8254_load(dev->iobase + DAS16M1_8254_SECOND, 0, 1, devpriv->divisor1,
-		   2);
-	i8254_load(dev->iobase + DAS16M1_8254_SECOND, 0, 2, devpriv->divisor2,
-		   2);
+	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
+	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
 
-	return ns;
+	i8254_write(timer_base, 0, 1, devpriv->divisor1);
+	i8254_write(timer_base, 0, 2, devpriv->divisor2);
 }
 
 static int das16m1_cmd_exec(struct comedi_device *dev,
@@ -267,6 +271,7 @@ static int das16m1_cmd_exec(struct comedi_device *dev,
 	struct das16m1_private_struct *devpriv = dev->private;
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
+	unsigned long timer_base = dev->iobase + DAS16M1_8254_FIRST;
 	unsigned int byte, i;
 
 	/* disable interrupts and internal pacer */
@@ -278,11 +283,11 @@ static int das16m1_cmd_exec(struct comedi_device *dev,
 	/* Initialize lower half of hardware counter, used to determine how
 	 * many samples are in fifo.  Value doesn't actually load into counter
 	 * until counter's next clock (the next a/d conversion) */
-	i8254_load(dev->iobase + DAS16M1_8254_FIRST, 0, 1, 0, 2);
+	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
+	i8254_write(timer_base, 0, 1, 0);
 	/* remember current reading of counter so we know when counter has
 	 * actually been loaded */
-	devpriv->initial_hw_count =
-	    i8254_read(dev->iobase + DAS16M1_8254_FIRST, 0, 1);
+	devpriv->initial_hw_count = i8254_read(timer_base, 0, 1);
 	/* setup channel/gain queue */
 	for (i = 0; i < cmd->chanlist_len; i++) {
 		outb(i, dev->iobase + DAS16M1_QUEUE_ADDR);
@@ -292,10 +297,14 @@ static int das16m1_cmd_exec(struct comedi_device *dev,
 		outb(byte, dev->iobase + DAS16M1_QUEUE_DATA);
 	}
 
-	/* set counter mode and counts */
-	cmd->convert_arg =
-	    das16m1_set_pacer(dev, cmd->convert_arg,
-			      cmd->flags & TRIG_ROUND_MASK);
+	/* enable interrupts and set internal pacer counter mode and counts */
+	devpriv->control_state &= ~PACER_MASK;
+	if (cmd->convert_src == TRIG_TIMER) {
+		das16m1_set_pacer(dev);
+		devpriv->control_state |= INT_PACER;
+	} else {	/* TRIG_EXT */
+		devpriv->control_state |= EXT_PACER;
+	}
 
 	/*  set control & status register */
 	byte = 0;
@@ -308,13 +317,6 @@ static int das16m1_cmd_exec(struct comedi_device *dev,
 	/* clear interrupt bit */
 	outb(0, dev->iobase + DAS16M1_CLEAR_INTR);
 
-	/* enable interrupts and internal pacer */
-	devpriv->control_state &= ~PACER_MASK;
-	if (cmd->convert_src == TRIG_TIMER)
-		devpriv->control_state |= INT_PACER;
-	else
-		devpriv->control_state |= EXT_PACER;
-
 	devpriv->control_state |= INTE;
 	outb(devpriv->control_state, dev->iobase + DAS16M1_INTR_CONTROL);
 
diff --git a/drivers/staging/comedi/drivers/das1800.c b/drivers/staging/comedi/drivers/das1800.c
index 8e975d6b06d..859519026c4 100644
--- a/drivers/staging/comedi/drivers/das1800.c
+++ b/drivers/staging/comedi/drivers/das1800.c
@@ -547,7 +547,7 @@ static void das1800_flush_dma_channel(struct comedi_device *dev,
 
 	munge_data(dev, buffer, num_samples);
 	cfc_write_array_to_buffer(s, buffer, num_bytes);
-	if (s->async->cmd.stop_src == TRIG_COUNT)
+	if (cmd->stop_src == TRIG_COUNT)
 		devpriv->count -= num_samples;
 
 	return;
@@ -731,7 +731,7 @@ static irqreturn_t das1800_interrupt(int irq, void *d)
 /* converts requested conversion timing to timing compatible with
  * hardware, used only when card is in 'burst mode'
  */
-static unsigned int burst_convert_arg(unsigned int convert_arg, int round_mode)
+static unsigned int burst_convert_arg(unsigned int convert_arg, int flags)
 {
 	unsigned int micro_sec;
 
@@ -740,7 +740,7 @@ static unsigned int burst_convert_arg(unsigned int convert_arg, int round_mode)
 		convert_arg = 64000;
 
 	/*  the conversion time must be an integral number of microseconds */
-	switch (round_mode) {
+	switch (flags & TRIG_ROUND_MASK) {
 	case TRIG_ROUND_NEAREST:
 	default:
 		micro_sec = (convert_arg + 500) / 1000;
@@ -757,6 +757,26 @@ static unsigned int burst_convert_arg(unsigned int convert_arg, int round_mode)
 	return micro_sec * 1000;
 }
 
+static int das1800_ai_check_chanlist(struct comedi_device *dev,
+				     struct comedi_subdevice *s,
+				     struct comedi_cmd *cmd)
+{
+	unsigned int unipolar0 = CR_RANGE(cmd->chanlist[0]) & UNIPOLAR;
+	int i;
+
+	for (i = 1; i < cmd->chanlist_len; i++) {
+		unsigned int unipolar = CR_RANGE(cmd->chanlist[i]) & UNIPOLAR;
+
+		if (unipolar != unipolar0) {
+			dev_dbg(dev->class_dev,
+				"unipolar and bipolar ranges cannot be mixed in the chanlist\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 /* test analog input cmd */
 static int das1800_ai_do_cmdtest(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
@@ -765,9 +785,7 @@ static int das1800_ai_do_cmdtest(struct comedi_device *dev,
 	const struct das1800_board *thisboard = comedi_board(dev);
 	struct das1800_private *devpriv = dev->private;
 	int err = 0;
-	unsigned int tmp_arg;
-	int i;
-	int unipolar;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -825,66 +843,48 @@ static int das1800_ai_do_cmdtest(struct comedi_device *dev,
 
 	/* step 4: fix up any arguments */
 
-	if (cmd->convert_src == TRIG_TIMER) {
-		/*  if we are not in burst mode */
-		if (cmd->scan_begin_src == TRIG_FOLLOW) {
-			tmp_arg = cmd->convert_arg;
-			/* calculate counter values that give desired timing */
+	if (cmd->scan_begin_src == TRIG_FOLLOW &&
+	    cmd->convert_src == TRIG_TIMER) {
+		/* we are not in burst mode */
+		arg = cmd->convert_arg;
+		i8253_cascade_ns_to_timer(I8254_OSC_BASE_5MHZ,
+					  &devpriv->divisor1,
+					  &devpriv->divisor2,
+					  &cmd->convert_arg, cmd->flags);
+		if (arg != cmd->convert_arg)
+			err++;
+	} else if (cmd->convert_src == TRIG_TIMER) {
+		/* we are in burst mode */
+		arg = cmd->convert_arg;
+		cmd->convert_arg = burst_convert_arg(cmd->convert_arg,
+						     cmd->flags);
+		if (arg != cmd->convert_arg)
+			err++;
+
+		if (cmd->scan_begin_src == TRIG_TIMER) {
+			arg = cmd->convert_arg * cmd->chanlist_len;
+			if (arg > cmd->scan_begin_arg) {
+				cmd->scan_begin_arg = arg;
+				err++;
+			}
+
+			arg = cmd->scan_begin_arg;
 			i8253_cascade_ns_to_timer(I8254_OSC_BASE_5MHZ,
 						  &devpriv->divisor1,
 						  &devpriv->divisor2,
-						  &cmd->convert_arg,
+						  &cmd->scan_begin_arg,
 						  cmd->flags);
-			if (tmp_arg != cmd->convert_arg)
-				err++;
-		}
-		/*  if we are in burst mode */
-		else {
-			/*  check that convert_arg is compatible */
-			tmp_arg = cmd->convert_arg;
-			cmd->convert_arg =
-			    burst_convert_arg(cmd->convert_arg,
-					      cmd->flags & TRIG_ROUND_MASK);
-			if (tmp_arg != cmd->convert_arg)
+			if (arg != cmd->scan_begin_arg)
 				err++;
-
-			if (cmd->scan_begin_src == TRIG_TIMER) {
-				/*  if scans are timed faster than conversion rate allows */
-				if (cmd->convert_arg * cmd->chanlist_len >
-				    cmd->scan_begin_arg) {
-					cmd->scan_begin_arg =
-					    cmd->convert_arg *
-					    cmd->chanlist_len;
-					err++;
-				}
-				tmp_arg = cmd->scan_begin_arg;
-				/* calculate counter values that give desired timing */
-				i8253_cascade_ns_to_timer(I8254_OSC_BASE_5MHZ,
-							  &devpriv->divisor1,
-							  &devpriv->divisor2,
-							  &cmd->scan_begin_arg,
-							  cmd->flags);
-				if (tmp_arg != cmd->scan_begin_arg)
-					err++;
-			}
 		}
 	}
 
 	if (err)
 		return 4;
 
-	/*  make sure user is not trying to mix unipolar and bipolar ranges */
-	if (cmd->chanlist) {
-		unipolar = CR_RANGE(cmd->chanlist[0]) & UNIPOLAR;
-		for (i = 1; i < cmd->chanlist_len; i++) {
-			if (unipolar != (CR_RANGE(cmd->chanlist[i]) & UNIPOLAR)) {
-				comedi_error(dev,
-					     "unipolar and bipolar ranges cannot be mixed in the chanlist");
-				err++;
-				break;
-			}
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= das1800_ai_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -962,68 +962,29 @@ static int control_c_bits(const struct comedi_cmd *cmd)
 	return control_c;
 }
 
-/* loads counters with divisor1, divisor2 from private structure */
-static int das1800_set_frequency(struct comedi_device *dev)
+static void das1800_setup_counters(struct comedi_device *dev,
+				   const struct comedi_cmd *cmd)
 {
 	struct das1800_private *devpriv = dev->private;
-	int err = 0;
-
-	/*  counter 1, mode 2 */
-	if (i8254_load(dev->iobase + DAS1800_COUNTER, 0, 1, devpriv->divisor1,
-		       2))
-		err++;
-	/*  counter 2, mode 2 */
-	if (i8254_load(dev->iobase + DAS1800_COUNTER, 0, 2, devpriv->divisor2,
-		       2))
-		err++;
-	if (err)
-		return -1;
+	unsigned long timer_base = dev->iobase + DAS1800_COUNTER;
 
-	return 0;
-}
-
-/* sets up counters */
-static int setup_counters(struct comedi_device *dev,
-			  const struct comedi_cmd *cmd)
-{
-	struct das1800_private *devpriv = dev->private;
-	unsigned int period;
+	/* setup cascaded counters for conversion/scan frequency */
+	if ((cmd->scan_begin_src == TRIG_FOLLOW ||
+	     cmd->scan_begin_src == TRIG_TIMER) &&
+	    cmd->convert_src == TRIG_TIMER) {
+		i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
+		i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
 
-	/*  setup cascaded counters for conversion/scan frequency */
-	switch (cmd->scan_begin_src) {
-	case TRIG_FOLLOW:	/*  not in burst mode */
-		if (cmd->convert_src == TRIG_TIMER) {
-			/* set conversion frequency */
-			period = cmd->convert_arg;
-			i8253_cascade_ns_to_timer(I8254_OSC_BASE_5MHZ,
-						  &devpriv->divisor1,
-						  &devpriv->divisor2,
-						  &period, cmd->flags);
-			if (das1800_set_frequency(dev) < 0)
-				return -1;
-		}
-		break;
-	case TRIG_TIMER:	/*  in burst mode */
-		/* set scan frequency */
-		period = cmd->scan_begin_arg;
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_5MHZ,
-					  &devpriv->divisor1,
-					  &devpriv->divisor2,
-					  &period, cmd->flags);
-		if (das1800_set_frequency(dev) < 0)
-			return -1;
-		break;
-	default:
-		break;
+		i8254_write(timer_base, 0, 1, devpriv->divisor1);
+		i8254_write(timer_base, 0, 2, devpriv->divisor2);
 	}
 
-	/*  setup counter 0 for 'about triggering' */
+	/* setup counter 0 for 'about triggering' */
 	if (cmd->stop_src == TRIG_EXT) {
-		/*  load counter 0 in mode 0 */
-		i8254_load(dev->iobase + DAS1800_COUNTER, 0, 0, 1, 0);
-	}
+		i8254_set_mode(timer_base, 0, 0, I8254_MODE0 | I8254_BINARY);
 
-	return 0;
+		i8254_write(timer_base, 0, 0, 1);
+	}
 }
 
 /* utility function that suggests a dma transfer size based on the conversion period 'ns' */
@@ -1136,7 +1097,6 @@ static int das1800_ai_do_cmd(struct comedi_device *dev,
 			     struct comedi_subdevice *s)
 {
 	struct das1800_private *devpriv = dev->private;
-	int ret;
 	int control_a, control_c;
 	struct comedi_async *async = s->async;
 	const struct comedi_cmd *cmd = &async->cmd;
@@ -1167,11 +1127,7 @@ static int das1800_ai_do_cmd(struct comedi_device *dev,
 
 	/* setup card and start */
 	program_chanlist(dev, cmd);
-	ret = setup_counters(dev, cmd);
-	if (ret < 0) {
-		comedi_error(dev, "Error setting up counters");
-		return ret;
-	}
+	das1800_setup_counters(dev, cmd);
 	setup_dma(dev, cmd);
 	outb(control_c, dev->iobase + DAS1800_CONTROL_C);
 	/*  set conversion rate and length for burst mode */
@@ -1470,7 +1426,7 @@ static int das1800_probe(struct comedi_device *dev)
 static int das1800_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
-	const struct das1800_board *thisboard = comedi_board(dev);
+	const struct das1800_board *thisboard;
 	struct das1800_private *devpriv;
 	struct comedi_subdevice *s;
 	unsigned int irq = it->options[1];
diff --git a/drivers/staging/comedi/drivers/das6402.c b/drivers/staging/comedi/drivers/das6402.c
index e0cfb6cb547..d18eea6c01a 100644
--- a/drivers/staging/comedi/drivers/das6402.c
+++ b/drivers/staging/comedi/drivers/das6402.c
@@ -125,7 +125,7 @@ struct das6402_boardinfo {
 	unsigned int maxdata;
 };
 
-struct das6402_boardinfo das6402_boards[] = {
+static struct das6402_boardinfo das6402_boards[] = {
 	{
 		.name		= "das6402-12",
 		.maxdata	= 0x0fff,
diff --git a/drivers/staging/comedi/drivers/das800.c b/drivers/staging/comedi/drivers/das800.c
index 3e408370dcf..6f7f8d531dd 100644
--- a/drivers/staging/comedi/drivers/das800.c
+++ b/drivers/staging/comedi/drivers/das800.c
@@ -224,7 +224,6 @@ struct das800_private {
 	unsigned int divisor1;	/* counter 1 value for timed conversions */
 	unsigned int divisor2;	/* counter 2 value for timed conversions */
 	unsigned int do_bits;	/* digital output bits */
-	bool forever;		/* flag that we should take data forever */
 };
 
 static void das800_ind_write(struct comedi_device *dev,
@@ -275,31 +274,54 @@ static void das800_disable(struct comedi_device *dev)
 	spin_unlock_irqrestore(&dev->spinlock, irq_flags);
 }
 
-static int das800_set_frequency(struct comedi_device *dev)
+static void das800_set_frequency(struct comedi_device *dev)
 {
 	struct das800_private *devpriv = dev->private;
-	int err = 0;
-
-	if (i8254_load(dev->iobase + DAS800_8254, 0, 1, devpriv->divisor1, 2))
-		err++;
-	if (i8254_load(dev->iobase + DAS800_8254, 0, 2, devpriv->divisor2, 2))
-		err++;
-	if (err)
-		return -1;
+	unsigned long timer_base = dev->iobase + DAS800_8254;
 
-	return 0;
+	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
+	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
+	i8254_write(timer_base, 0, 1, devpriv->divisor1);
+	i8254_write(timer_base, 0, 2, devpriv->divisor2);
 }
 
 static int das800_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 {
 	struct das800_private *devpriv = dev->private;
 
-	devpriv->forever = false;
 	devpriv->count = 0;
 	das800_disable(dev);
 	return 0;
 }
 
+static int das800_ai_check_chanlist(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    struct comedi_cmd *cmd)
+{
+	unsigned int chan0 = CR_CHAN(cmd->chanlist[0]);
+	unsigned int range0 = CR_RANGE(cmd->chanlist[0]);
+	int i;
+
+	for (i = 1; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+		unsigned int range = CR_RANGE(cmd->chanlist[i]);
+
+		if (chan != (chan0 + i) % s->n_chan) {
+			dev_dbg(dev->class_dev,
+				"chanlist must be consecutive, counting upwards\n");
+			return -EINVAL;
+		}
+
+		if (range != range0) {
+			dev_dbg(dev->class_dev,
+				"chanlist must all have the same gain\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 static int das800_ai_do_cmdtest(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_cmd *cmd)
@@ -307,6 +329,7 @@ static int das800_ai_do_cmdtest(struct comedi_device *dev,
 	const struct das800_board *thisboard = comedi_board(dev);
 	struct das800_private *devpriv = dev->private;
 	int err = 0;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -352,41 +375,20 @@ static int das800_ai_do_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		int tmp = cmd->convert_arg;
-
-		/* calculate counter values that give desired timing */
+		arg = cmd->convert_arg;
 		i8253_cascade_ns_to_timer(I8254_OSC_BASE_1MHZ,
 					  &devpriv->divisor1,
 					  &devpriv->divisor2,
-					  &cmd->convert_arg, cmd->flags);
-		if (tmp != cmd->convert_arg)
-			err++;
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
 		return 4;
 
-	/*  check channel/gain list against card's limitations */
-	if (cmd->chanlist) {
-		unsigned int chan = CR_CHAN(cmd->chanlist[0]);
-		unsigned int range = CR_RANGE(cmd->chanlist[0]);
-		unsigned int next;
-		int i;
-
-		for (i = 1; i < cmd->chanlist_len; i++) {
-			next = cmd->chanlist[i];
-			if (CR_CHAN(next) != (chan + i) % N_CHAN_AI) {
-				dev_err(dev->class_dev,
-					"chanlist must be consecutive, counting upwards\n");
-				err++;
-			}
-			if (CR_RANGE(next) != range) {
-				dev_err(dev->class_dev,
-					"chanlist must all have the same gain\n");
-				err++;
-			}
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= das800_ai_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -400,9 +402,10 @@ static int das800_ai_do_cmd(struct comedi_device *dev,
 	const struct das800_board *thisboard = comedi_board(dev);
 	struct das800_private *devpriv = dev->private;
 	struct comedi_async *async = s->async;
-	unsigned int gain = CR_RANGE(async->cmd.chanlist[0]);
-	unsigned int start_chan = CR_CHAN(async->cmd.chanlist[0]);
-	unsigned int end_chan = (start_chan + async->cmd.chanlist_len - 1) % 8;
+	struct comedi_cmd *cmd = &async->cmd;
+	unsigned int gain = CR_RANGE(cmd->chanlist[0]);
+	unsigned int start_chan = CR_CHAN(cmd->chanlist[0]);
+	unsigned int end_chan = (start_chan + cmd->chanlist_len - 1) % 8;
 	unsigned int scan_chans = (end_chan << 3) | start_chan;
 	int conv_bits;
 	unsigned long irq_flags;
@@ -420,28 +423,22 @@ static int das800_ai_do_cmd(struct comedi_device *dev,
 	gain &= 0xf;
 	outb(gain, dev->iobase + DAS800_GAIN);
 
-	if (async->cmd.stop_src == TRIG_COUNT) {
-		devpriv->count = async->cmd.stop_arg * async->cmd.chanlist_len;
-		devpriv->forever = false;
-	} else {	/* TRIG_NONE */
-		devpriv->forever = true;
+	if (cmd->stop_src == TRIG_COUNT)
+		devpriv->count = cmd->stop_arg * cmd->chanlist_len;
+	else	/* TRIG_NONE */
 		devpriv->count = 0;
-	}
 
 	/* enable auto channel scan, send interrupts on end of conversion
 	 * and set clock source to internal or external
 	 */
 	conv_bits = 0;
 	conv_bits |= EACS | IEOC;
-	if (async->cmd.start_src == TRIG_EXT)
+	if (cmd->start_src == TRIG_EXT)
 		conv_bits |= DTEN;
-	if (async->cmd.convert_src == TRIG_TIMER) {
+	if (cmd->convert_src == TRIG_TIMER) {
 		conv_bits |= CASC | ITE;
 		/* set conversion frequency */
-		if (das800_set_frequency(dev) < 0) {
-			comedi_error(dev, "Error setting up counters");
-			return -1;
-		}
+		das800_set_frequency(dev);
 	}
 
 	spin_lock_irqsave(&dev->spinlock, irq_flags);
@@ -465,7 +462,8 @@ static irqreturn_t das800_interrupt(int irq, void *d)
 	struct comedi_device *dev = d;
 	struct das800_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
-	struct comedi_async *async = s ? s->async : NULL;
+	struct comedi_async *async;
+	struct comedi_cmd *cmd;
 	unsigned long irq_flags;
 	unsigned int status;
 	unsigned int val;
@@ -479,6 +477,9 @@ static irqreturn_t das800_interrupt(int irq, void *d)
 	if (!dev->attached)
 		return IRQ_HANDLED;
 
+	async = s->async;
+	cmd = &async->cmd;
+
 	spin_lock_irqsave(&dev->spinlock, irq_flags);
 	status = das800_ind_read(dev, CONTROL1) & STATUS2_HCEN;
 	/*
@@ -510,7 +511,7 @@ static irqreturn_t das800_interrupt(int irq, void *d)
 			val >>= 4;	/* 12-bit sample */
 
 		/* if there are more data points to collect */
-		if (devpriv->count > 0 || devpriv->forever) {
+		if (cmd->stop_src == TRIG_NONE || devpriv->count > 0) {
 			/* write data point to buffer */
 			cfc_write_to_buffer(s, val & s->maxdata);
 			devpriv->count--;
@@ -525,7 +526,7 @@ static irqreturn_t das800_interrupt(int irq, void *d)
 		return IRQ_HANDLED;
 	}
 
-	if (devpriv->count > 0 || devpriv->forever) {
+	if (cmd->stop_src == TRIG_NONE || devpriv->count > 0) {
 		/* Re-enable card's interrupt.
 		 * We already have spinlock, so indirect addressing is safe */
 		das800_ind_write(dev, CONTROL1_INTE | devpriv->do_bits,
@@ -673,7 +674,7 @@ static int das800_probe(struct comedi_device *dev)
 
 static int das800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	const struct das800_board *thisboard = comedi_board(dev);
+	const struct das800_board *thisboard;
 	struct das800_private *devpriv;
 	struct comedi_subdevice *s;
 	unsigned int irq = it->options[1];
diff --git a/drivers/staging/comedi/drivers/dmm32at.c b/drivers/staging/comedi/drivers/dmm32at.c
index c8a36eb5f01..ad7a5d53b97 100644
--- a/drivers/staging/comedi/drivers/dmm32at.c
+++ b/drivers/staging/comedi/drivers/dmm32at.c
@@ -243,13 +243,39 @@ static int dmm32at_ns_to_timer(unsigned int *ns, int round)
 	return *ns;
 }
 
+static int dmm32at_ai_check_chanlist(struct comedi_device *dev,
+				     struct comedi_subdevice *s,
+				     struct comedi_cmd *cmd)
+{
+	unsigned int chan0 = CR_CHAN(cmd->chanlist[0]);
+	unsigned int range0 = CR_RANGE(cmd->chanlist[0]);
+	int i;
+
+	for (i = 1; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+		unsigned int range = CR_RANGE(cmd->chanlist[i]);
+
+		if (chan != (chan0 + i) % s->n_chan) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must be consecutive channels, counting upwards\n");
+			return -EINVAL;
+		}
+		if (range != range0) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must all have the same gain\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 static int dmm32at_ai_cmdtest(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
-	int start_chan, gain, i;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -325,50 +351,28 @@ static int dmm32at_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
-		dmm32at_ns_to_timer(&cmd->scan_begin_arg,
-				    cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+		arg = cmd->scan_begin_arg;
+		dmm32at_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
-		dmm32at_ns_to_timer(&cmd->convert_arg,
-				    cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->convert_arg)
-			err++;
-		if (cmd->scan_begin_src == TRIG_TIMER &&
-		    cmd->scan_begin_arg <
-		    cmd->convert_arg * cmd->scan_end_arg) {
-			cmd->scan_begin_arg =
-			    cmd->convert_arg * cmd->scan_end_arg;
-			err++;
+		arg = cmd->convert_arg;
+		dmm32at_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+		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;
 
-	/* step 5 check the channel list, the channel list for this
-	   board must be consecutive and gains must be the same */
-
-	if (cmd->chanlist) {
-		gain = CR_RANGE(cmd->chanlist[0]);
-		start_chan = CR_CHAN(cmd->chanlist[0]);
-		for (i = 1; i < cmd->chanlist_len; i++) {
-			if (CR_CHAN(cmd->chanlist[i]) !=
-			    (start_chan + i) % s->n_chan) {
-				comedi_error(dev,
-					     "entries in chanlist must be consecutive channels, counting upwards\n");
-				err++;
-			}
-			if (CR_RANGE(cmd->chanlist[i]) != gain) {
-				comedi_error(dev,
-					     "entries in chanlist must all have the same gain\n");
-				err++;
-			}
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= dmm32at_ai_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -464,12 +468,6 @@ static int dmm32at_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		outb(0xff, dev->iobase + DMM32AT_CONV);
 	}
 
-/*	for(i=0;i<cmd->chanlist_len;i++) */
-/*		comedi_buf_put(s->async,i*100); */
-
-/*	s->async->events |= COMEDI_CB_EOA; */
-/*	comedi_event(dev, s); */
-
 	return 0;
 
 }
@@ -510,7 +508,7 @@ static irqreturn_t dmm32at_isr(int irq, void *d)
 
 			/* invert sign bit to make range unsigned */
 			samp = ((msb ^ 0x0080) << 8) + lsb;
-			comedi_buf_put(s->async, samp);
+			comedi_buf_put(s, samp);
 		}
 
 		if (devpriv->ai_scans_left != 0xffffffff) {	/* TRIG_COUNT */
diff --git a/drivers/staging/comedi/drivers/dt2801.c b/drivers/staging/comedi/drivers/dt2801.c
index d4d4e4b497d..4263014426f 100644
--- a/drivers/staging/comedi/drivers/dt2801.c
+++ b/drivers/staging/comedi/drivers/dt2801.c
@@ -545,7 +545,7 @@ static int dt2801_dio_insn_config(struct comedi_device *dev,
 */
 static int dt2801_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	const struct dt2801_board *board = comedi_board(dev);
+	const struct dt2801_board *board;
 	struct dt2801_private *devpriv;
 	struct comedi_subdevice *s;
 	int board_code, type;
diff --git a/drivers/staging/comedi/drivers/dt2814.c b/drivers/staging/comedi/drivers/dt2814.c
index 3794b7e5209..904c9f0e4af 100644
--- a/drivers/staging/comedi/drivers/dt2814.c
+++ b/drivers/staging/comedi/drivers/dt2814.c
@@ -128,7 +128,7 @@ static int dt2814_ai_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -170,10 +170,9 @@ static int dt2814_ai_cmdtest(struct comedi_device *dev,
 
 	/* step 4: fix up any arguments */
 
-	tmp = cmd->scan_begin_arg;
-	dt2814_ns_to_timer(&cmd->scan_begin_arg, cmd->flags & TRIG_ROUND_MASK);
-	if (tmp != cmd->scan_begin_arg)
-		err++;
+	arg = cmd->scan_begin_arg;
+	dt2814_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 
 	if (err)
 		return 4;
diff --git a/drivers/staging/comedi/drivers/dt282x.c b/drivers/staging/comedi/drivers/dt282x.c
index 16cc100531e..c2a66dcf99f 100644
--- a/drivers/staging/comedi/drivers/dt282x.c
+++ b/drivers/staging/comedi/drivers/dt282x.c
@@ -466,7 +466,7 @@ static irqreturn_t dt282x_interrupt(int irq, void *d)
 
 		if (devpriv->ad_2scomp)
 			data ^= 1 << (board->adbits - 1);
-		ret = comedi_buf_put(s->async, data);
+		ret = comedi_buf_put(s, data);
 
 		if (ret == 0)
 			s->async->events |= COMEDI_CB_OVERFLOW;
@@ -578,7 +578,7 @@ static int dt282x_ai_cmdtest(struct comedi_device *dev,
 {
 	const struct dt282x_board *board = comedi_board(dev);
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -633,10 +633,9 @@ static int dt282x_ai_cmdtest(struct comedi_device *dev,
 
 	/* step 4: fix up any arguments */
 
-	tmp = cmd->convert_arg;
-	dt282x_ns_to_timer(&cmd->convert_arg, cmd->flags & TRIG_ROUND_MASK);
-	if (tmp != cmd->convert_arg)
-		err++;
+	arg = cmd->convert_arg;
+	dt282x_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 
 	if (err)
 		return 4;
@@ -825,7 +824,7 @@ static int dt282x_ao_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -852,7 +851,7 @@ static int dt282x_ao_cmdtest(struct comedi_device *dev,
 	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
 	err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg, 5000);
 	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-	err |= cfc_check_trigger_arg_max(&cmd->scan_end_arg, 2);
+	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT) {
 		/* any count is allowed */
@@ -865,10 +864,9 @@ static int dt282x_ao_cmdtest(struct comedi_device *dev,
 
 	/* step 4: fix up any arguments */
 
-	tmp = cmd->scan_begin_arg;
-	dt282x_ns_to_timer(&cmd->scan_begin_arg, cmd->flags & TRIG_ROUND_MASK);
-	if (tmp != cmd->scan_begin_arg)
-		err++;
+	arg = cmd->scan_begin_arg;
+	dt282x_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 
 	if (err)
 		return 4;
@@ -878,12 +876,14 @@ static int dt282x_ao_cmdtest(struct comedi_device *dev,
 }
 
 static int dt282x_ao_inttrig(struct comedi_device *dev,
-			     struct comedi_subdevice *s, unsigned int x)
+			     struct comedi_subdevice *s,
+			     unsigned int trig_num)
 {
 	struct dt282x_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	int size;
 
-	if (x != 0)
+	if (trig_num != cmd->start_src)
 		return -EINVAL;
 
 	size = cfc_read_array_from_buffer(s, devpriv->dma[0].buf,
diff --git a/drivers/staging/comedi/drivers/dt3000.c b/drivers/staging/comedi/drivers/dt3000.c
index 436e451cadf..4ab4de00592 100644
--- a/drivers/staging/comedi/drivers/dt3000.c
+++ b/drivers/staging/comedi/drivers/dt3000.c
@@ -322,7 +322,7 @@ static void dt3k_ai_empty_fifo(struct comedi_device *dev,
 
 	for (i = 0; i < count; i++) {
 		data = readw(devpriv->io_addr + DPR_ADC_buffer + rear);
-		comedi_buf_put(s->async, data);
+		comedi_buf_put(s, data);
 		rear++;
 		if (rear >= AI_FIFO_DEPTH)
 			rear = 0;
@@ -416,7 +416,7 @@ static int dt3k_ai_cmdtest(struct comedi_device *dev,
 {
 	const struct dt3k_boardtype *this_board = comedi_board(dev);
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -466,25 +466,20 @@ static int dt3k_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
-		dt3k_ns_to_timer(100, &cmd->scan_begin_arg,
-				 cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+		arg = cmd->scan_begin_arg;
+		dt3k_ns_to_timer(100, &arg, cmd->flags & TRIG_ROUND_MASK);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
-		dt3k_ns_to_timer(50, &cmd->convert_arg,
-				 cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->convert_arg)
-			err++;
-		if (cmd->scan_begin_src == TRIG_TIMER &&
-		    cmd->scan_begin_arg <
-		    cmd->convert_arg * cmd->scan_end_arg) {
-			cmd->scan_begin_arg =
-			    cmd->convert_arg * cmd->scan_end_arg;
-			err++;
+		arg = cmd->convert_arg;
+		dt3k_ns_to_timer(50, &arg, cmd->flags & TRIG_ROUND_MASK);
+		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);
 		}
 	}
 
diff --git a/drivers/staging/comedi/drivers/gsc_hpdi.c b/drivers/staging/comedi/drivers/gsc_hpdi.c
index 08d7655e24e..22333c1ad88 100644
--- a/drivers/staging/comedi/drivers/gsc_hpdi.c
+++ b/drivers/staging/comedi/drivers/gsc_hpdi.c
@@ -360,12 +360,30 @@ static int gsc_hpdi_cmd(struct comedi_device *dev,
 	return 0;
 }
 
+static int gsc_hpdi_check_chanlist(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   struct comedi_cmd *cmd)
+{
+	int i;
+
+	for (i = 0; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+
+		if (chan != i) {
+			dev_dbg(dev->class_dev,
+				"chanlist must be ch 0 to 31 in order\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 static int gsc_hpdi_cmd_test(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
 			     struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int i;
 
 	if (s->io_bits)
 		return -EINVAL;
@@ -392,6 +410,8 @@ static int gsc_hpdi_cmd_test(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
+	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+
 	if (!cmd->chanlist_len || !cmd->chanlist) {
 		cmd->chanlist_len = 32;
 		err |= -EINVAL;
@@ -411,17 +431,9 @@ static int gsc_hpdi_cmd_test(struct comedi_device *dev,
 	if (err)
 		return 4;
 
-	/* step 5: complain about special chanlist considerations */
-
-	for (i = 0; i < cmd->chanlist_len; i++) {
-		if (CR_CHAN(cmd->chanlist[i]) != i) {
-			/*  XXX could support 8 or 16 channels */
-			dev_err(dev->class_dev,
-				"chanlist must be ch 0 to 31 in order");
-			err |= -EINVAL;
-			break;
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= gsc_hpdi_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
diff --git a/drivers/staging/comedi/drivers/ii_pci20kc.c b/drivers/staging/comedi/drivers/ii_pci20kc.c
index 3558ab3b6e1..2516ce83483 100644
--- a/drivers/staging/comedi/drivers/ii_pci20kc.c
+++ b/drivers/staging/comedi/drivers/ii_pci20kc.c
@@ -461,6 +461,7 @@ static int ii20k_attach(struct comedi_device *dev,
 	id = readb(devpriv->ioaddr + II20K_ID_REG);
 	switch (id & II20K_ID_MASK) {
 	case II20K_ID_PCI20001C_1A:
+		has_dio = false;
 		break;
 	case II20K_ID_PCI20001C_2A:
 		has_dio = true;
diff --git a/drivers/staging/comedi/drivers/me4000.c b/drivers/staging/comedi/drivers/me4000.c
index f02b31b317e..25ce2f78db8 100644
--- a/drivers/staging/comedi/drivers/me4000.c
+++ b/drivers/staging/comedi/drivers/me4000.c
@@ -598,67 +598,35 @@ static int me4000_ai_cancel(struct comedi_device *dev,
 	return 0;
 }
 
-static int ai_check_chanlist(struct comedi_device *dev,
-			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
+static int me4000_ai_check_chanlist(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    struct comedi_cmd *cmd)
 {
-	const struct me4000_board *thisboard = comedi_board(dev);
-	int aref;
+	const struct me4000_board *board = comedi_board(dev);
+	unsigned int max_diff_chan = board->ai_diff_nchan;
+	unsigned int aref0 = CR_AREF(cmd->chanlist[0]);
 	int i;
 
-	/* Check whether a channel list is available */
-	if (!cmd->chanlist_len) {
-		dev_err(dev->class_dev, "No channel list available\n");
-		return -EINVAL;
-	}
-
-	/* Check the channel list size */
-	if (cmd->chanlist_len > ME4000_AI_CHANNEL_LIST_COUNT) {
-		dev_err(dev->class_dev, "Channel list is to large\n");
-		return -EINVAL;
-	}
-
-	/* Check the pointer */
-	if (!cmd->chanlist) {
-		dev_err(dev->class_dev, "NULL pointer to channel list\n");
-		return -EFAULT;
-	}
-
-	/* Check whether aref is equal for all entries */
-	aref = CR_AREF(cmd->chanlist[0]);
 	for (i = 0; i < cmd->chanlist_len; i++) {
-		if (CR_AREF(cmd->chanlist[i]) != aref) {
-			dev_err(dev->class_dev,
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+		unsigned int range = CR_RANGE(cmd->chanlist[i]);
+		unsigned int aref = CR_AREF(cmd->chanlist[i]);
+
+		if (aref != aref0) {
+			dev_dbg(dev->class_dev,
 				"Mode is not equal for all entries\n");
 			return -EINVAL;
 		}
-	}
 
-	/* Check whether channels are available for this ending */
-	if (aref == SDF_DIFF) {
-		for (i = 0; i < cmd->chanlist_len; i++) {
-			if (CR_CHAN(cmd->chanlist[i]) >=
-			    thisboard->ai_diff_nchan) {
-				dev_err(dev->class_dev,
-					"Channel number to high\n");
-				return -EINVAL;
-			}
-		}
-	} else {
-		for (i = 0; i < cmd->chanlist_len; i++) {
-			if (CR_CHAN(cmd->chanlist[i]) >= thisboard->ai_nchan) {
-				dev_err(dev->class_dev,
+		if (aref == SDF_DIFF) {
+			if (chan >= max_diff_chan) {
+				dev_dbg(dev->class_dev,
 					"Channel number to high\n");
 				return -EINVAL;
 			}
-		}
-	}
 
-	/* Check if bipolar is set for all entries when in differential mode */
-	if (aref == SDF_DIFF) {
-		for (i = 0; i < cmd->chanlist_len; i++) {
-			if (CR_RANGE(cmd->chanlist[i]) != 1 &&
-			    CR_RANGE(cmd->chanlist[i]) != 2) {
-				dev_err(dev->class_dev,
+			if (!comedi_range_is_bipolar(s, range)) {
+				dev_dbg(dev->class_dev,
 				       "Bipolar is not selected in differential mode\n");
 				return -EINVAL;
 			}
@@ -934,22 +902,13 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 		err |= -EINVAL;
 	}
 
-	if (cmd->stop_src == TRIG_NONE && cmd->scan_end_src == TRIG_NONE) {
-	} else if (cmd->stop_src == TRIG_COUNT &&
-		   cmd->scan_end_src == TRIG_NONE) {
-	} else if (cmd->stop_src == TRIG_NONE &&
-		   cmd->scan_end_src == TRIG_COUNT) {
-	} else if (cmd->stop_src == TRIG_COUNT &&
-		   cmd->scan_end_src == TRIG_COUNT) {
-	} else {
-		err |= -EINVAL;
-	}
-
 	if (err)
 		return 2;
 
 	/* Step 3: check if arguments are trivially valid */
 
+	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+
 	if (cmd->chanlist_len < 1) {
 		cmd->chanlist_len = 1;
 		err |= -EINVAL;
@@ -1091,10 +1050,11 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 	if (err)
 		return 4;
 
-	/*
-	 * Stage 5. Check the channel list.
-	 */
-	if (ai_check_chanlist(dev, s, cmd))
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= me4000_ai_check_chanlist(dev, s, cmd);
+
+	if (err)
 		return 5;
 
 	return 0;
@@ -1164,7 +1124,7 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 			lval = inl(dev->iobase + ME4000_AI_DATA_REG) & 0xFFFF;
 			lval ^= 0x8000;
 
-			if (!comedi_buf_put(s->async, lval)) {
+			if (!comedi_buf_put(s, lval)) {
 				/*
 				 * Buffer overflow, so stop conversion
 				 * and disable all interrupts
@@ -1209,7 +1169,7 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 			lval = inl(dev->iobase + ME4000_AI_DATA_REG) & 0xFFFF;
 			lval ^= 0x8000;
 
-			if (!comedi_buf_put(s->async, lval)) {
+			if (!comedi_buf_put(s, lval)) {
 				dev_err(dev->class_dev, "Buffer overflow\n");
 				s->async->events |= COMEDI_CB_OVERFLOW;
 				break;
@@ -1392,6 +1352,7 @@ static int me4000_cnt_insn_config(struct comedi_device *dev,
 				  unsigned int *data)
 {
 	struct me4000_info *info = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
 	int err;
 
 	switch (data[0]) {
@@ -1399,16 +1360,17 @@ static int me4000_cnt_insn_config(struct comedi_device *dev,
 		if (insn->n != 1)
 			return -EINVAL;
 
-		err = i8254_load(info->timer_regbase, 0, insn->chanspec, 0,
-				I8254_MODE0 | I8254_BINARY);
+		err = i8254_set_mode(info->timer_regbase, 0, chan,
+				     I8254_MODE0 | I8254_BINARY);
 		if (err)
 			return err;
+		i8254_write(info->timer_regbase, 0, chan, 0);
 		break;
 	case GPCT_SET_OPERATION:
 		if (insn->n != 2)
 			return -EINVAL;
 
-		err = i8254_set_mode(info->timer_regbase, 0, insn->chanspec,
+		err = i8254_set_mode(info->timer_regbase, 0, chan,
 				(data[1] << 1) | I8254_BINARY);
 		if (err)
 			return err;
diff --git a/drivers/staging/comedi/drivers/mite.c b/drivers/staging/comedi/drivers/mite.c
index 1a572c83f99..19c029acbc9 100644
--- a/drivers/staging/comedi/drivers/mite.c
+++ b/drivers/staging/comedi/drivers/mite.c
@@ -307,8 +307,9 @@ EXPORT_SYMBOL_GPL(mite_dma_arm);
 /**************************************/
 
 int mite_buf_change(struct mite_dma_descriptor_ring *ring,
-		    struct comedi_async *async)
+		    struct comedi_subdevice *s)
 {
+	struct comedi_async *async = s->async;
 	unsigned int n_links;
 	int i;
 
@@ -333,7 +334,7 @@ int mite_buf_change(struct mite_dma_descriptor_ring *ring,
 			       n_links * sizeof(struct mite_dma_descriptor),
 			       &ring->descriptors_dma_addr, GFP_KERNEL);
 	if (!ring->descriptors) {
-		dev_err(async->subdevice->device->class_dev,
+		dev_err(s->device->class_dev,
 			"mite: ring buffer allocation failed\n");
 		return -ENOMEM;
 	}
@@ -525,15 +526,15 @@ void mite_dma_disarm(struct mite_channel *mite_chan)
 EXPORT_SYMBOL_GPL(mite_dma_disarm);
 
 int mite_sync_input_dma(struct mite_channel *mite_chan,
-			struct comedi_async *async)
+			struct comedi_subdevice *s)
 {
-	struct comedi_subdevice *s = async->subdevice;
+	struct comedi_async *async = s->async;
 	int count;
 	unsigned int nbytes, old_alloc_count;
 
 	old_alloc_count = async->buf_write_alloc_count;
 	/* write alloc as much as we can */
-	comedi_buf_write_alloc(async, async->prealloc_bufsz);
+	comedi_buf_write_alloc(s, async->prealloc_bufsz);
 
 	nbytes = mite_bytes_written_to_memory_lb(mite_chan);
 	if ((int)(mite_bytes_written_to_memory_ub(mite_chan) -
@@ -550,7 +551,7 @@ int mite_sync_input_dma(struct mite_channel *mite_chan,
 	if (count <= 0)
 		return 0;
 
-	comedi_buf_write_free(async, count);
+	comedi_buf_write_free(s, count);
 	cfc_inc_scan_progress(s, count);
 	async->events |= COMEDI_CB_BLOCK;
 	return 0;
@@ -558,28 +559,25 @@ int mite_sync_input_dma(struct mite_channel *mite_chan,
 EXPORT_SYMBOL_GPL(mite_sync_input_dma);
 
 int mite_sync_output_dma(struct mite_channel *mite_chan,
-			 struct comedi_async *async)
+			 struct comedi_subdevice *s)
 {
-	int count;
+	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
+	u32 stop_count = cmd->stop_arg * cfc_bytes_per_scan(s);
+	unsigned int old_alloc_count = async->buf_read_alloc_count;
 	u32 nbytes_ub, nbytes_lb;
-	unsigned int old_alloc_count;
-	u32 stop_count =
-	    async->cmd.stop_arg * cfc_bytes_per_scan(async->subdevice);
+	int count;
 
-	old_alloc_count = async->buf_read_alloc_count;
 	/*  read alloc as much as we can */
-	comedi_buf_read_alloc(async, async->prealloc_bufsz);
+	comedi_buf_read_alloc(s, async->prealloc_bufsz);
 	nbytes_lb = mite_bytes_read_from_memory_lb(mite_chan);
-	if (async->cmd.stop_src == TRIG_COUNT &&
-	    (int)(nbytes_lb - stop_count) > 0)
+	if (cmd->stop_src == TRIG_COUNT && (int)(nbytes_lb - stop_count) > 0)
 		nbytes_lb = stop_count;
 	nbytes_ub = mite_bytes_read_from_memory_ub(mite_chan);
-	if (async->cmd.stop_src == TRIG_COUNT &&
-	    (int)(nbytes_ub - stop_count) > 0)
+	if (cmd->stop_src == TRIG_COUNT && (int)(nbytes_ub - stop_count) > 0)
 		nbytes_ub = stop_count;
 	if ((int)(nbytes_ub - old_alloc_count) > 0) {
-		dev_warn(async->subdevice->device->class_dev,
-			 "mite: DMA underrun\n");
+		dev_warn(s->device->class_dev, "mite: DMA underrun\n");
 		async->events |= COMEDI_CB_OVERFLOW;
 		return -1;
 	}
@@ -588,7 +586,7 @@ int mite_sync_output_dma(struct mite_channel *mite_chan,
 		return 0;
 
 	if (count) {
-		comedi_buf_read_free(async, count);
+		comedi_buf_read_free(s, count);
 		async->events |= COMEDI_CB_BLOCK;
 	}
 	return 0;
diff --git a/drivers/staging/comedi/drivers/mite.h b/drivers/staging/comedi/drivers/mite.h
index 78f23574799..e6e58e989b7 100644
--- a/drivers/staging/comedi/drivers/mite.h
+++ b/drivers/staging/comedi/drivers/mite.h
@@ -106,9 +106,9 @@ unsigned mite_dma_tcr(struct mite_channel *mite_chan);
 void mite_dma_arm(struct mite_channel *mite_chan);
 void mite_dma_disarm(struct mite_channel *mite_chan);
 int mite_sync_input_dma(struct mite_channel *mite_chan,
-			struct comedi_async *async);
+			struct comedi_subdevice *s);
 int mite_sync_output_dma(struct mite_channel *mite_chan,
-			 struct comedi_async *async);
+			 struct comedi_subdevice *s);
 u32 mite_bytes_written_to_memory_lb(struct mite_channel *mite_chan);
 u32 mite_bytes_written_to_memory_ub(struct mite_channel *mite_chan);
 u32 mite_bytes_read_from_memory_lb(struct mite_channel *mite_chan);
@@ -120,7 +120,7 @@ int mite_done(struct mite_channel *mite_chan);
 void mite_prep_dma(struct mite_channel *mite_chan,
 		   unsigned int num_device_bits, unsigned int num_memory_bits);
 int mite_buf_change(struct mite_dma_descriptor_ring *ring,
-		    struct comedi_async *async);
+		    struct comedi_subdevice *s);
 
 static inline int CHAN_OFFSET(int channel)
 {
diff --git a/drivers/staging/comedi/drivers/ni_6527.c b/drivers/staging/comedi/drivers/ni_6527.c
index 860fc81fb11..c8b1fa793a3 100644
--- a/drivers/staging/comedi/drivers/ni_6527.c
+++ b/drivers/staging/comedi/drivers/ni_6527.c
@@ -215,7 +215,7 @@ static irqreturn_t ni6527_interrupt(int irq, void *d)
 		return IRQ_NONE;
 
 	if (status & NI6527_STATUS_EDGE) {
-		comedi_buf_put(s->async, 0);
+		comedi_buf_put(s, 0);
 		s->async->events |= COMEDI_CB_EOS;
 		comedi_event(dev, s);
 	}
@@ -253,7 +253,7 @@ static int ni6527_intr_cmdtest(struct comedi_device *dev,
 	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, 1);
+	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)
@@ -421,6 +421,7 @@ static int ni6527_auto_attach(struct comedi_device *dev,
 		s->range_table	= &range_digital;
 		s->insn_config	= ni6527_intr_insn_config;
 		s->insn_bits	= ni6527_intr_insn_bits;
+		s->len_chanlist	= 1;
 		s->do_cmdtest	= ni6527_intr_cmdtest;
 		s->do_cmd	= ni6527_intr_cmd;
 		s->cancel	= ni6527_intr_cancel;
diff --git a/drivers/staging/comedi/drivers/ni_65xx.c b/drivers/staging/comedi/drivers/ni_65xx.c
index 6e42001f686..9a139d6b8ef 100644
--- a/drivers/staging/comedi/drivers/ni_65xx.c
+++ b/drivers/staging/comedi/drivers/ni_65xx.c
@@ -439,7 +439,7 @@ static irqreturn_t ni_65xx_interrupt(int irq, void *d)
 	writeb(ClrEdge | ClrOverflow,
 	       devpriv->mite->daq_io_addr + Clear_Register);
 
-	comedi_buf_put(s->async, 0);
+	comedi_buf_put(s, 0);
 	s->async->events |= COMEDI_CB_EOS;
 	comedi_event(dev, s);
 	return IRQ_HANDLED;
@@ -473,7 +473,7 @@ static int ni_65xx_intr_cmdtest(struct comedi_device *dev,
 	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, 1);
+	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)
@@ -491,7 +491,6 @@ static int ni_65xx_intr_cmd(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
 	struct ni_65xx_private *devpriv = dev->private;
-	/* struct comedi_cmd *cmd = &s->async->cmd; */
 
 	writeb(ClrEdge | ClrOverflow,
 	       devpriv->mite->daq_io_addr + Clear_Register);
@@ -671,6 +670,7 @@ static int ni_65xx_auto_attach(struct comedi_device *dev,
 	s->n_chan = 1;
 	s->range_table = &range_unknown;
 	s->maxdata = 1;
+	s->len_chanlist	= 1;
 	s->do_cmdtest = ni_65xx_intr_cmdtest;
 	s->do_cmd = ni_65xx_intr_cmd;
 	s->cancel = ni_65xx_intr_cancel;
diff --git a/drivers/staging/comedi/drivers/ni_660x.c b/drivers/staging/comedi/drivers/ni_660x.c
index 0d4b9019f76..634cde83a02 100644
--- a/drivers/staging/comedi/drivers/ni_660x.c
+++ b/drivers/staging/comedi/drivers/ni_660x.c
@@ -823,9 +823,9 @@ static int ni_660x_input_poll(struct comedi_device *dev,
 
 	/* lock to avoid race with comedi_poll */
 	spin_lock_irqsave(&devpriv->interrupt_lock, flags);
-	mite_sync_input_dma(counter->mite_chan, s->async);
+	mite_sync_input_dma(counter->mite_chan, s);
 	spin_unlock_irqrestore(&devpriv->interrupt_lock, flags);
-	return comedi_buf_read_n_available(s->async);
+	return comedi_buf_read_n_available(s);
 }
 
 static int ni_660x_buf_change(struct comedi_device *dev,
@@ -836,7 +836,7 @@ static int ni_660x_buf_change(struct comedi_device *dev,
 	struct ni_gpct *counter = s->private;
 	int ret;
 
-	ret = mite_buf_change(mite_ring(devpriv, counter), s->async);
+	ret = mite_buf_change(mite_ring(devpriv, counter), s);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/staging/comedi/drivers/ni_at_a2150.c b/drivers/staging/comedi/drivers/ni_at_a2150.c
index 4e39b1f63d8..5bd19494dbf 100644
--- a/drivers/staging/comedi/drivers/ni_at_a2150.c
+++ b/drivers/staging/comedi/drivers/ni_at_a2150.c
@@ -287,14 +287,54 @@ static int a2150_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 	return 0;
 }
 
+static int a2150_ai_check_chanlist(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   struct comedi_cmd *cmd)
+{
+	unsigned int chan0 = CR_CHAN(cmd->chanlist[0]);
+	unsigned int aref0 = CR_AREF(cmd->chanlist[0]);
+	int i;
+
+	if (cmd->chanlist_len == 2 && (chan0 == 1 || chan0 == 3)) {
+		dev_dbg(dev->class_dev,
+			"length 2 chanlist must be channels 0,1 or channels 2,3\n");
+		return -EINVAL;
+	}
+
+	if (cmd->chanlist_len == 3) {
+		dev_dbg(dev->class_dev,
+			"chanlist must have 1,2 or 4 channels\n");
+		return -EINVAL;
+	}
+
+	for (i = 1; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+		unsigned int aref = CR_AREF(cmd->chanlist[i]);
+
+		if (chan != (chan0 + i)) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must be consecutive channels, counting upwards\n");
+			return -EINVAL;
+		}
+
+		if (chan == 2)
+			aref0 = aref;
+		if (aref != aref0) {
+			dev_dbg(dev->class_dev,
+				"channels 0/1 and 2/3 must have the same analog reference\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 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);
 	int err = 0;
-	int tmp;
-	int startChan;
-	int i;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -339,42 +379,17 @@ static int a2150_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
-		a2150_get_timing(dev, &cmd->scan_begin_arg, cmd->flags);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+		arg = cmd->scan_begin_arg;
+		a2150_get_timing(dev, &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (err)
 		return 4;
 
-	/*  check channel/gain list against card's limitations */
-	if (cmd->chanlist) {
-		startChan = CR_CHAN(cmd->chanlist[0]);
-		for (i = 1; i < cmd->chanlist_len; i++) {
-			if (CR_CHAN(cmd->chanlist[i]) != (startChan + i)) {
-				comedi_error(dev,
-					     "entries in chanlist must be consecutive channels, counting upwards\n");
-				err++;
-			}
-		}
-		if (cmd->chanlist_len == 2 && CR_CHAN(cmd->chanlist[0]) == 1) {
-			comedi_error(dev,
-				     "length 2 chanlist must be channels 0,1 or channels 2,3");
-			err++;
-		}
-		if (cmd->chanlist_len == 3) {
-			comedi_error(dev,
-				     "chanlist must have 1,2 or 4 channels");
-			err++;
-		}
-		if (CR_AREF(cmd->chanlist[0]) != CR_AREF(cmd->chanlist[1]) ||
-		    CR_AREF(cmd->chanlist[2]) != CR_AREF(cmd->chanlist[3])) {
-			comedi_error(dev,
-				     "channels 0/1 and 2/3 must have the same analog reference");
-			err++;
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= a2150_ai_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -387,6 +402,7 @@ static int a2150_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	struct a2150_private *devpriv = dev->private;
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
+	unsigned long timer_base = dev->iobase + I8253_BASE_REG;
 	unsigned long lock_flags;
 	unsigned int old_config_bits = devpriv->config_bits;
 	unsigned int trigger_bits;
@@ -454,7 +470,8 @@ static int a2150_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	outw(devpriv->irq_dma_bits, dev->iobase + IRQ_DMA_CNTRL_REG);
 
 	/*  may need to wait 72 sampling periods if timing was changed */
-	i8254_load(dev->iobase + I8253_BASE_REG, 0, 2, 72, 0);
+	i8254_set_mode(timer_base, 0, 2, I8254_MODE0 | I8254_BINARY);
+	i8254_write(timer_base, 0, 2, 72);
 
 	/*  setup start triggering */
 	trigger_bits = 0;
@@ -679,7 +696,7 @@ static int a2150_probe(struct comedi_device *dev)
 
 static int a2150_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	const struct a2150_board *thisboard = comedi_board(dev);
+	const struct a2150_board *thisboard;
 	struct a2150_private *devpriv;
 	struct comedi_subdevice *s;
 	unsigned int irq = it->options[1];
diff --git a/drivers/staging/comedi/drivers/ni_at_ao.c b/drivers/staging/comedi/drivers/ni_at_ao.c
index 10e3e9475ee..c93b47bcca5 100644
--- a/drivers/staging/comedi/drivers/ni_at_ao.c
+++ b/drivers/staging/comedi/drivers/ni_at_ao.c
@@ -310,6 +310,7 @@ static int atao_calib_insn_read(struct comedi_device *dev,
 static void atao_reset(struct comedi_device *dev)
 {
 	struct atao_private *devpriv = dev->private;
+	unsigned long timer_base = dev->iobase + ATAO_82C53_BASE;
 
 	/* This is the reset sequence described in the manual */
 
@@ -317,10 +318,9 @@ static void atao_reset(struct comedi_device *dev)
 	outw(devpriv->cfg1, dev->iobase + ATAO_CFG1_REG);
 
 	/* Put outputs of counter 1 and counter 2 in a high state */
-	i8254_load(dev->iobase + ATAO_82C53_BASE, 0,
-		   0, 0x0003, I8254_MODE4 | I8254_BINARY);
-	i8254_set_mode(dev->iobase + ATAO_82C53_BASE, 0,
-		   1, I8254_MODE4 | I8254_BINARY);
+	i8254_set_mode(timer_base, 0, 0, I8254_MODE4 | I8254_BINARY);
+	i8254_set_mode(timer_base, 0, 1, I8254_MODE4 | I8254_BINARY);
+	i8254_write(timer_base, 0, 0, 0x0003);
 
 	outw(ATAO_CFG2_CALLD_NOP, dev->iobase + ATAO_CFG2_REG);
 
diff --git a/drivers/staging/comedi/drivers/ni_atmio16d.c b/drivers/staging/comedi/drivers/ni_atmio16d.c
index 4262385e4f4..6ad27f50c6e 100644
--- a/drivers/staging/comedi/drivers/ni_atmio16d.c
+++ b/drivers/staging/comedi/drivers/ni_atmio16d.c
@@ -221,7 +221,7 @@ static irqreturn_t atmio16d_interrupt(int irq, void *d)
 	struct comedi_device *dev = d;
 	struct comedi_subdevice *s = dev->read_subdev;
 
-	comedi_buf_put(s->async, inw(dev->iobase + AD_FIFO_REG));
+	comedi_buf_put(s, inw(dev->iobase + AD_FIFO_REG));
 
 	comedi_event(dev, s);
 	return IRQ_HANDLED;
diff --git a/drivers/staging/comedi/drivers/ni_daq_700.c b/drivers/staging/comedi/drivers/ni_daq_700.c
index 171a71d20c8..728bf7f14f7 100644
--- a/drivers/staging/comedi/drivers/ni_daq_700.c
+++ b/drivers/staging/comedi/drivers/ni_daq_700.c
@@ -139,6 +139,8 @@ static int daq700_ai_rinsn(struct comedi_device *dev,
 	/* write channel to multiplexer */
 	/* set mask scan bit high to disable scanning */
 	outb(chan | 0x80, dev->iobase + CMD_R1);
+	/* mux needs 2us to really settle [Fred Brooks]. */
+	udelay(2);
 
 	/* convert n samples */
 	for (n = 0; n < insn->n; n++) {
diff --git a/drivers/staging/comedi/drivers/ni_labpc.c b/drivers/staging/comedi/drivers/ni_labpc.c
index f4216e825f0..3e3f940fa57 100644
--- a/drivers/staging/comedi/drivers/ni_labpc.c
+++ b/drivers/staging/comedi/drivers/ni_labpc.c
@@ -171,32 +171,39 @@ static const struct labpc_boardinfo labpc_boards[] = {
 };
 #endif
 
-static int labpc_counter_load(struct comedi_device *dev,
-			      unsigned long base_address,
-			      unsigned int counter_number,
-			      unsigned int count, unsigned int mode)
+static void labpc_counter_load(struct comedi_device *dev,
+			       unsigned long base_address,
+			       unsigned int counter_number,
+			       unsigned int count,
+			       unsigned int mode)
 {
 	const struct labpc_boardinfo *board = comedi_board(dev);
 
-	if (board->has_mmio)
-		return i8254_mm_load((void __iomem *)base_address, 0,
-				     counter_number, count, mode);
-	else
-		return i8254_load(base_address, 0, counter_number, count, mode);
+	if (board->has_mmio) {
+		void __iomem *mmio_base = (void __iomem *)base_address;
+
+		i8254_mm_set_mode(mmio_base, 0, counter_number, mode);
+		i8254_mm_write(mmio_base, 0, counter_number, count);
+	} else {
+		i8254_set_mode(base_address, 0, counter_number, mode);
+		i8254_write(base_address, 0, counter_number, count);
+	}
 }
 
-static int labpc_counter_set_mode(struct comedi_device *dev,
-				  unsigned long base_address,
-				  unsigned int counter_number,
-				  unsigned int mode)
+static void labpc_counter_set_mode(struct comedi_device *dev,
+				   unsigned long base_address,
+				   unsigned int counter_number,
+				   unsigned int mode)
 {
 	const struct labpc_boardinfo *board = comedi_board(dev);
 
-	if (board->has_mmio)
-		return i8254_mm_set_mode((void __iomem *)base_address, 0,
-					 counter_number, mode);
-	else
-		return i8254_set_mode(base_address, 0, counter_number, mode);
+	if (board->has_mmio) {
+		void __iomem *mmio_base = (void __iomem *)base_address;
+
+		i8254_mm_set_mode(mmio_base, 0, counter_number, mode);
+	} else {
+		i8254_set_mode(base_address, 0, counter_number, mode);
+	}
 }
 
 static int labpc_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
@@ -349,10 +356,8 @@ static int labpc_ai_insn_read(struct comedi_device *dev,
 	devpriv->write_byte(devpriv->cmd4, dev->iobase + CMD4_REG);
 
 	/* initialize pacer counter to prevent any problems */
-	ret = labpc_counter_set_mode(dev, dev->iobase + COUNTER_A_BASE_REG,
-				     0, I8254_MODE2);
-	if (ret)
-		return ret;
+	labpc_counter_set_mode(dev, dev->iobase + COUNTER_A_BASE_REG,
+			       0, I8254_MODE2);
 
 	labpc_clear_adc_fifo(dev);
 
@@ -546,72 +551,60 @@ static enum scan_mode labpc_ai_scan_mode(const struct comedi_cmd *cmd)
 	return 0;
 }
 
-static int labpc_ai_chanlist_invalid(const struct comedi_device *dev,
-				     const struct comedi_cmd *cmd,
-				     enum scan_mode mode)
+static int labpc_ai_check_chanlist(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   struct comedi_cmd *cmd)
 {
-	int channel, range, aref, i;
-
-	if (cmd->chanlist == NULL)
-		return 0;
+	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;
 
-	if (mode == MODE_SINGLE_CHAN_INTERVAL) {
-		if (cmd->chanlist_len > 0xff) {
-			comedi_error(dev,
-				     "ni_labpc: chanlist too long for single channel interval mode\n");
-			return 1;
-		}
-	}
-
-	channel = CR_CHAN(cmd->chanlist[0]);
-	range = CR_RANGE(cmd->chanlist[0]);
-	aref = CR_AREF(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]);
+		unsigned int aref = CR_AREF(cmd->chanlist[i]);
 
 		switch (mode) {
+		case MODE_SINGLE_CHAN:
+			break;
 		case MODE_SINGLE_CHAN_INTERVAL:
-			if (CR_CHAN(cmd->chanlist[i]) != channel) {
-				comedi_error(dev,
-					     "channel scanning order specified in chanlist is not supported by hardware.\n");
-				return 1;
+			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 (CR_CHAN(cmd->chanlist[i]) != i) {
-				comedi_error(dev,
-					     "channel scanning order specified in chanlist is not supported by hardware.\n");
-				return 1;
+			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 (CR_CHAN(cmd->chanlist[i]) !=
-			    cmd->chanlist_len - i - 1) {
-				comedi_error(dev,
-					     "channel scanning order specified in chanlist is not supported by hardware.\n");
-				return 1;
+			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;
-		default:
-			dev_err(dev->class_dev,
-				"ni_labpc: bug! in chanlist check\n");
-			return 1;
-			break;
 		}
 
-		if (CR_RANGE(cmd->chanlist[i]) != range) {
-			comedi_error(dev,
-				     "entries in chanlist must all have the same range\n");
-			return 1;
+		if (range != range0) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must all have the same range\n");
+			return -EINVAL;
 		}
 
-		if (CR_AREF(cmd->chanlist[i]) != aref) {
-			comedi_error(dev,
-				     "entries in chanlist must all have the same reference\n");
-			return 1;
+		if (aref != aref0) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must all have the same reference\n");
+			return -EINVAL;
 		}
 	}
 
@@ -661,8 +654,14 @@ static int labpc_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	if (cmd->start_arg == TRIG_NOW)
+	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;
@@ -711,7 +710,11 @@ static int labpc_ai_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 4;
 
-	if (labpc_ai_chanlist_invalid(dev, cmd, mode))
+	/* 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;
@@ -732,7 +735,6 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	unsigned int aref = CR_AREF(chanspec);
 	enum transfer_type xfer;
 	unsigned long flags;
-	int ret;
 
 	/* make sure board is disabled before setting up acquisition */
 	labpc_cancel(dev, s);
@@ -747,17 +749,12 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		 * load counter a1 with count of 3
 		 * (pc+ manual says this is minimum allowed) using mode 0
 		 */
-		ret = labpc_counter_load(dev, dev->iobase + COUNTER_A_BASE_REG,
-					 1, 3, I8254_MODE0);
+		labpc_counter_load(dev, dev->iobase + COUNTER_A_BASE_REG,
+				   1, 3, I8254_MODE0);
 	} else	{
 		/* just put counter a1 in mode 0 to set its output low */
-		ret = labpc_counter_set_mode(dev,
-					     dev->iobase + COUNTER_A_BASE_REG,
-					     1, I8254_MODE0);
-	}
-	if (ret) {
-		comedi_error(dev, "error loading counter a1");
-		return ret;
+		labpc_counter_set_mode(dev, dev->iobase + COUNTER_A_BASE_REG,
+				       1, I8254_MODE0);
 	}
 
 	/* figure out what method we will use to transfer data */
@@ -802,38 +799,25 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		/*  set up pacing */
 		labpc_adc_timing(dev, cmd, mode);
 		/*  load counter b0 in mode 3 */
-		ret = labpc_counter_load(dev, dev->iobase + COUNTER_B_BASE_REG,
-					 0, devpriv->divisor_b0, I8254_MODE3);
-		if (ret < 0) {
-			comedi_error(dev, "error loading counter b0");
-			return -1;
-		}
+		labpc_counter_load(dev, dev->iobase + 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 */
-		ret = labpc_counter_load(dev, dev->iobase + COUNTER_A_BASE_REG,
-					 0, devpriv->divisor_a0, I8254_MODE2);
+		labpc_counter_load(dev, dev->iobase + COUNTER_A_BASE_REG,
+				   0, devpriv->divisor_a0, I8254_MODE2);
 	} else {
 		/* initialize pacer counter to prevent any problems */
-		ret = labpc_counter_set_mode(dev,
-					     dev->iobase + COUNTER_A_BASE_REG,
-					     0, I8254_MODE2);
-	}
-	if (ret) {
-		comedi_error(dev, "error loading counter a0");
-		return ret;
+		labpc_counter_set_mode(dev, dev->iobase + COUNTER_A_BASE_REG,
+				       0, I8254_MODE2);
 	}
 
 	/*  set up scan pacing */
 	if (labpc_ai_scan_period(cmd, mode)) {
 		/*  load counter b1 in mode 2 */
-		ret = labpc_counter_load(dev, dev->iobase + COUNTER_B_BASE_REG,
-					 1, devpriv->divisor_b1, I8254_MODE2);
-		if (ret < 0) {
-			comedi_error(dev, "error loading counter b1");
-			return -1;
-		}
+		labpc_counter_load(dev, dev->iobase + COUNTER_B_BASE_REG,
+				   1, devpriv->divisor_b1, I8254_MODE2);
 	}
 
 	labpc_clear_adc_fifo(dev);
@@ -890,8 +874,9 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 static int labpc_drain_fifo(struct comedi_device *dev)
 {
 	struct labpc_private *devpriv = dev->private;
-	unsigned short data;
 	struct comedi_async *async = dev->read_subdev->async;
+	struct comedi_cmd *cmd = &async->cmd;
+	unsigned short data;
 	const int timeout = 10000;
 	unsigned int i;
 
@@ -900,7 +885,7 @@ static int labpc_drain_fifo(struct comedi_device *dev)
 	for (i = 0; (devpriv->stat1 & STAT1_DAVAIL) && i < timeout;
 	     i++) {
 		/*  quit if we have all the data we want */
-		if (async->cmd.stop_src == TRIG_COUNT) {
+		if (cmd->stop_src == TRIG_COUNT) {
 			if (devpriv->count == 0)
 				break;
 			devpriv->count--;
diff --git a/drivers/staging/comedi/drivers/ni_labpc_isadma.c b/drivers/staging/comedi/drivers/ni_labpc_isadma.c
index 2149596830a..d9f25fdbb72 100644
--- a/drivers/staging/comedi/drivers/ni_labpc_isadma.c
+++ b/drivers/staging/comedi/drivers/ni_labpc_isadma.c
@@ -87,6 +87,7 @@ void labpc_drain_dma(struct comedi_device *dev)
 	struct labpc_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
 	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
 	int status;
 	unsigned long flags;
 	unsigned int max_points, num_points, residue, leftover;
@@ -108,12 +109,12 @@ void labpc_drain_dma(struct comedi_device *dev)
 	 */
 	residue = get_dma_residue(devpriv->dma_chan) / sample_size;
 	num_points = max_points - residue;
-	if (devpriv->count < num_points && async->cmd.stop_src == TRIG_COUNT)
+	if (cmd->stop_src == TRIG_COUNT && devpriv->count < num_points)
 		num_points = devpriv->count;
 
 	/* figure out how many points will be stored next time */
 	leftover = 0;
-	if (async->cmd.stop_src != TRIG_COUNT) {
+	if (cmd->stop_src != TRIG_COUNT) {
 		leftover = devpriv->dma_transfer_size / sample_size;
 	} else if (devpriv->count > num_points) {
 		leftover = devpriv->count - num_points;
@@ -125,7 +126,7 @@ void labpc_drain_dma(struct comedi_device *dev)
 	for (i = 0; i < num_points; i++)
 		cfc_write_to_buffer(s, devpriv->dma_buffer[i]);
 
-	if (async->cmd.stop_src == TRIG_COUNT)
+	if (cmd->stop_src == TRIG_COUNT)
 		devpriv->count -= num_points;
 
 	/* set address and count for next transfer */
diff --git a/drivers/staging/comedi/drivers/ni_mio_common.c b/drivers/staging/comedi/drivers/ni_mio_common.c
index 8a0e3b7236a..7ffdcc07ef9 100644
--- a/drivers/staging/comedi/drivers/ni_mio_common.c
+++ b/drivers/staging/comedi/drivers/ni_mio_common.c
@@ -864,7 +864,7 @@ static void ni_sync_ai_dma(struct comedi_device *dev)
 
 	spin_lock_irqsave(&devpriv->mite_channel_lock, flags);
 	if (devpriv->ai_mite_chan)
-		mite_sync_input_dma(devpriv->ai_mite_chan, s->async);
+		mite_sync_input_dma(devpriv->ai_mite_chan, s);
 	spin_unlock_irqrestore(&devpriv->mite_channel_lock, flags);
 }
 
@@ -877,7 +877,7 @@ static void mite_handle_b_linkc(struct mite_struct *mite,
 
 	spin_lock_irqsave(&devpriv->mite_channel_lock, flags);
 	if (devpriv->ao_mite_chan)
-		mite_sync_output_dma(devpriv->ao_mite_chan, s->async);
+		mite_sync_output_dma(devpriv->ao_mite_chan, s);
 	spin_unlock_irqrestore(&devpriv->mite_channel_lock, flags);
 }
 
@@ -1149,7 +1149,7 @@ static void ni_ao_fifo_load(struct comedi_device *dev,
 
 	chan = async->cur_chan;
 	for (i = 0; i < n; i++) {
-		err &= comedi_buf_get(async, &d);
+		err &= comedi_buf_get(s, &d);
 		if (err == 0)
 			break;
 
@@ -1159,7 +1159,7 @@ static void ni_ao_fifo_load(struct comedi_device *dev,
 			packed_data = d & 0xffff;
 			/* 6711 only has 16 bit wide ao fifo */
 			if (board->reg_type != ni_reg_6711) {
-				err &= comedi_buf_get(async, &d);
+				err &= comedi_buf_get(s, &d);
 				if (err == 0)
 					break;
 				chan++;
@@ -1200,7 +1200,7 @@ static int ni_ao_fifo_half_empty(struct comedi_device *dev,
 	const struct ni_board_struct *board = comedi_board(dev);
 	int n;
 
-	n = comedi_buf_read_n_available(s->async);
+	n = comedi_buf_read_n_available(s);
 	if (n == 0) {
 		s->async->events |= COMEDI_CB_OVERFLOW;
 		return 0;
@@ -1230,7 +1230,7 @@ static int ni_ao_prep_fifo(struct comedi_device *dev,
 		ni_ao_win_outl(dev, 0x6, AO_FIFO_Offset_Load_611x);
 
 	/* load some data */
-	n = comedi_buf_read_n_available(s->async);
+	n = comedi_buf_read_n_available(s);
 	if (n == 0)
 		return 0;
 
@@ -1468,10 +1468,11 @@ static void ni_ai_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 {
 	struct ni_private *devpriv = dev->private;
 	struct comedi_async *async = s->async;
-	unsigned int i;
+	struct comedi_cmd *cmd = &async->cmd;
 	unsigned int length = num_bytes / bytes_per_sample(s);
 	unsigned short *array = data;
 	unsigned int *larray = data;
+	unsigned int i;
 
 	for (i = 0; i < length; i++) {
 #ifdef PCIDMA
@@ -1485,7 +1486,7 @@ static void ni_ai_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 		else
 			array[i] += devpriv->ai_offset[chan_index];
 		chan_index++;
-		chan_index %= async->cmd.chanlist_len;
+		chan_index %= cmd->chanlist_len;
 	}
 }
 
@@ -1505,7 +1506,7 @@ static int ni_ai_setup_MITE_dma(struct comedi_device *dev)
 /* printk("comedi_debug: using mite channel %i for ai.\n", devpriv->ai_mite_chan->channel); */
 
 	/* write alloc the entire buffer */
-	comedi_buf_write_alloc(s->async, s->async->prealloc_bufsz);
+	comedi_buf_write_alloc(s, s->async->prealloc_bufsz);
 
 	spin_lock_irqsave(&devpriv->mite_channel_lock, flags);
 	if (devpriv->ai_mite_chan == NULL) {
@@ -1545,7 +1546,7 @@ static int ni_ao_setup_MITE_dma(struct comedi_device *dev)
 		return retval;
 
 	/* read alloc the entire buffer */
-	comedi_buf_read_alloc(s->async, s->async->prealloc_bufsz);
+	comedi_buf_read_alloc(s, s->async->prealloc_bufsz);
 
 	spin_lock_irqsave(&devpriv->mite_channel_lock, flags);
 	if (devpriv->ao_mite_chan) {
@@ -2080,7 +2081,7 @@ static int ni_ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	int err = 0;
-	int tmp;
+	unsigned int tmp;
 	unsigned int sources;
 
 	/* Step 1 : check if triggers are trivially valid */
@@ -2119,17 +2120,19 @@ static int ni_ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	if (cmd->start_src == TRIG_EXT) {
-		/* external trigger */
-		unsigned int tmp = CR_CHAN(cmd->start_arg);
+	switch (cmd->start_src) {
+	case TRIG_NOW:
+	case TRIG_INT:
+		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		break;
+	case TRIG_EXT:
+		tmp = CR_CHAN(cmd->start_arg);
 
 		if (tmp > 16)
 			tmp = 16;
 		tmp |= (cmd->start_arg & (CR_INVERT | CR_EDGE));
 		err |= cfc_check_trigger_arg_is(&cmd->start_arg, tmp);
-	} else {
-		/* true for both TRIG_NOW and TRIG_INT */
-		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		break;
 	}
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
@@ -2510,30 +2513,28 @@ static int ni_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	}
 #endif
 
-	switch (cmd->start_src) {
-	case TRIG_NOW:
+	if (cmd->start_src == TRIG_NOW) {
 		/* AI_START1_Pulse */
 		devpriv->stc_writew(dev, AI_START1_Pulse | devpriv->ai_cmd2,
 				    AI_Command_2_Register);
 		s->async->inttrig = NULL;
-		break;
-	case TRIG_EXT:
+	} else if (cmd->start_src == TRIG_EXT) {
 		s->async->inttrig = NULL;
-		break;
-	case TRIG_INT:
-		s->async->inttrig = &ni_ai_inttrig;
-		break;
+	} else {	/* TRIG_INT */
+		s->async->inttrig = ni_ai_inttrig;
 	}
 
 	return 0;
 }
 
-static int ni_ai_inttrig(struct comedi_device *dev, struct comedi_subdevice *s,
-			 unsigned int trignum)
+static int ni_ai_inttrig(struct comedi_device *dev,
+			 struct comedi_subdevice *s,
+			 unsigned int trig_num)
 {
 	struct ni_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 
-	if (trignum != 0)
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	devpriv->stc_writew(dev, AI_START1_Pulse | devpriv->ai_cmd2,
@@ -2710,22 +2711,22 @@ static void ni_ao_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 {
 	const struct ni_board_struct *board = comedi_board(dev);
 	struct comedi_async *async = s->async;
-	unsigned int range;
-	unsigned int i;
-	unsigned int offset;
+	struct comedi_cmd *cmd = &async->cmd;
 	unsigned int length = num_bytes / sizeof(short);
+	unsigned int offset = 1 << (board->aobits - 1);
 	unsigned short *array = data;
+	unsigned int range;
+	unsigned int i;
 
-	offset = 1 << (board->aobits - 1);
 	for (i = 0; i < length; i++) {
-		range = CR_RANGE(async->cmd.chanlist[chan_index]);
+		range = CR_RANGE(cmd->chanlist[chan_index]);
 		if (board->ao_unipolar == 0 || (range & 1) == 0)
 			array[i] -= offset;
 #ifdef PCIDMA
 		array[i] = cpu_to_le16(array[i]);
 #endif
 		chan_index++;
-		chan_index %= async->cmd.chanlist_len;
+		chan_index %= cmd->chanlist_len;
 	}
 }
 
@@ -2946,17 +2947,19 @@ static int ni_ao_insn_config(struct comedi_device *dev,
 	return -EINVAL;
 }
 
-static int ni_ao_inttrig(struct comedi_device *dev, struct comedi_subdevice *s,
-			 unsigned int trignum)
+static int ni_ao_inttrig(struct comedi_device *dev,
+			 struct comedi_subdevice *s,
+			 unsigned int trig_num)
 {
 	const struct ni_board_struct *board __maybe_unused = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	int ret;
 	int interrupt_b_bits;
 	int i;
 	static const int timeout = 1000;
 
-	if (trignum != 0)
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	/* Null trig at beginning prevent ao start trigger from executing more than
@@ -3217,7 +3220,7 @@ static int ni_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 			    AO_BC_TC_Interrupt_Enable, 1);
 	}
 
-	s->async->inttrig = &ni_ao_inttrig;
+	s->async->inttrig = ni_ao_inttrig;
 
 	return 0;
 }
@@ -3228,7 +3231,7 @@ static int ni_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	int err = 0;
-	int tmp;
+	unsigned int tmp;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -3258,17 +3261,18 @@ static int ni_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	if (cmd->start_src == TRIG_EXT) {
-		/* external trigger */
-		unsigned int tmp = CR_CHAN(cmd->start_arg);
+	switch (cmd->start_src) {
+	case TRIG_INT:
+		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		break;
+	case TRIG_EXT:
+		tmp = CR_CHAN(cmd->start_arg);
 
 		if (tmp > 18)
 			tmp = 18;
 		tmp |= (cmd->start_arg & (CR_INVERT | CR_EDGE));
 		err |= cfc_check_trigger_arg_is(&cmd->start_arg, tmp);
-	} else {
-		/* true for both TRIG_NOW and TRIG_INT */
-		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		break;
 	}
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
@@ -3304,11 +3308,6 @@ static int ni_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	if (err)
 		return 4;
 
-	/* step 5: fix up chanlist */
-
-	if (err)
-		return 5;
-
 	return 0;
 }
 
@@ -3439,12 +3438,27 @@ static int ni_m_series_dio_insn_bits(struct comedi_device *dev,
 	return insn->n;
 }
 
+static int ni_cdio_check_chanlist(struct comedi_device *dev,
+				  struct comedi_subdevice *s,
+				  struct comedi_cmd *cmd)
+{
+	int i;
+
+	for (i = 0; i < cmd->chanlist_len; ++i) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+
+		if (chan != i)
+			return -EINVAL;
+	}
+
+	return 0;
+}
+
 static int ni_cdio_cmdtest(struct comedi_device *dev,
 			   struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
 	int err = 0;
 	int tmp;
-	unsigned i;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -3484,12 +3498,9 @@ static int ni_cdio_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 4;
 
-	/* step 5: check chanlist */
-
-	for (i = 0; i < cmd->chanlist_len; ++i) {
-		if (cmd->chanlist[i] != i)
-			err = 1;
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= ni_cdio_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -3530,25 +3541,32 @@ static int ni_cdio_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	retval = ni_request_cdo_mite_channel(dev);
 	if (retval < 0)
 		return retval;
-	s->async->inttrig = &ni_cdo_inttrig;
+
+	s->async->inttrig = ni_cdo_inttrig;
+
 	return 0;
 }
 
-static int ni_cdo_inttrig(struct comedi_device *dev, struct comedi_subdevice *s,
-			  unsigned int trignum)
+static int ni_cdo_inttrig(struct comedi_device *dev,
+			  struct comedi_subdevice *s,
+			  unsigned int trig_num)
 {
 #ifdef PCIDMA
 	struct ni_private *devpriv = dev->private;
 	unsigned long flags;
 #endif
+	struct comedi_cmd *cmd = &s->async->cmd;
 	int retval = 0;
 	unsigned i;
 	const unsigned timeout = 1000;
 
+	if (trig_num != cmd->start_arg)
+		return -EINVAL;
+
 	s->async->inttrig = NULL;
 
 	/* read alloc the entire buffer */
-	comedi_buf_read_alloc(s->async, s->async->prealloc_bufsz);
+	comedi_buf_read_alloc(s, s->async->prealloc_bufsz);
 
 #ifdef PCIDMA
 	spin_lock_irqsave(&devpriv->mite_channel_lock, flags);
@@ -3623,7 +3641,7 @@ static void handle_cdio_interrupt(struct comedi_device *dev)
 			       devpriv->mite->mite_io_addr +
 			       MITE_CHOR(devpriv->cdo_mite_chan->channel));
 		}
-		mite_sync_output_dma(devpriv->cdo_mite_chan, s->async);
+		mite_sync_output_dma(devpriv->cdo_mite_chan, s);
 	}
 	spin_unlock_irqrestore(&devpriv->mite_channel_lock, flags);
 #endif
diff --git a/drivers/staging/comedi/drivers/ni_pcidio.c b/drivers/staging/comedi/drivers/ni_pcidio.c
index 85ac2d964f5..5fc74d6ff6a 100644
--- a/drivers/staging/comedi/drivers/ni_pcidio.c
+++ b/drivers/staging/comedi/drivers/ni_pcidio.c
@@ -359,7 +359,7 @@ static int ni_pcidio_poll(struct comedi_device *dev, struct comedi_subdevice *s)
 	spin_lock_irqsave(&dev->spinlock, irq_flags);
 	spin_lock(&devpriv->mite_channel_lock);
 	if (devpriv->di_mite_chan)
-		mite_sync_input_dma(devpriv->di_mite_chan, s->async);
+		mite_sync_input_dma(devpriv->di_mite_chan, s);
 	spin_unlock(&devpriv->mite_channel_lock);
 	count = s->async->buf_write_count - s->async->buf_read_count;
 	spin_unlock_irqrestore(&dev->spinlock, irq_flags);
@@ -405,7 +405,7 @@ static irqreturn_t nidio_interrupt(int irq, void *d)
 			writel(CHOR_CLRLC,
 			       mite->mite_io_addr +
 			       MITE_CHOR(devpriv->di_mite_chan->channel));
-			mite_sync_input_dma(devpriv->di_mite_chan, s->async);
+			mite_sync_input_dma(devpriv->di_mite_chan, s);
 			/* XXX need to byteswap */
 		}
 		if (m_status & ~(CHSR_INT | CHSR_LINKC | CHSR_DONE | CHSR_DRDY |
@@ -447,8 +447,8 @@ static irqreturn_t nidio_interrupt(int irq, void *d)
 					  Group_1_FIFO);
 				data1 = auxdata & 0xffff;
 				data2 = (auxdata & 0xffff0000) >> 16;
-				comedi_buf_put(async, data1);
-				comedi_buf_put(async, data2);
+				comedi_buf_put(s, data1);
+				comedi_buf_put(s, data2);
 				flags = readb(devpriv->mite->daq_io_addr +
 					      Group_1_Flags);
 			}
@@ -536,7 +536,7 @@ static int ni_pcidio_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -595,11 +595,9 @@ static int ni_pcidio_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
-		ni_pcidio_ns_to_timer(&cmd->scan_begin_arg,
-				      cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+		arg = cmd->scan_begin_arg;
+		ni_pcidio_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (err)
@@ -759,7 +757,7 @@ static int setup_mite_dma(struct comedi_device *dev, struct comedi_subdevice *s)
 		return retval;
 
 	/* write alloc the entire buffer */
-	comedi_buf_write_alloc(s->async, s->async->prealloc_bufsz);
+	comedi_buf_write_alloc(s, s->async->prealloc_bufsz);
 
 	spin_lock_irqsave(&devpriv->mite_channel_lock, flags);
 	if (devpriv->di_mite_chan) {
@@ -773,11 +771,13 @@ static int setup_mite_dma(struct comedi_device *dev, struct comedi_subdevice *s)
 }
 
 static int ni_pcidio_inttrig(struct comedi_device *dev,
-			     struct comedi_subdevice *s, unsigned int trignum)
+			     struct comedi_subdevice *s,
+			     unsigned int trig_num)
 {
 	struct nidio96_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 
-	if (trignum != 0)
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	writeb(devpriv->OpModeBits, devpriv->mite->daq_io_addr + OpMode);
@@ -804,7 +804,7 @@ static int ni_pcidio_change(struct comedi_device *dev,
 	struct nidio96_private *devpriv = dev->private;
 	int ret;
 
-	ret = mite_buf_change(devpriv->di_mite_ring, s->async);
+	ret = mite_buf_change(devpriv->di_mite_ring, s);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/staging/comedi/drivers/ni_pcimio.c b/drivers/staging/comedi/drivers/ni_pcimio.c
index d40df072583..89300dc78e3 100644
--- a/drivers/staging/comedi/drivers/ni_pcimio.c
+++ b/drivers/staging/comedi/drivers/ni_pcimio.c
@@ -1560,7 +1560,7 @@ static int pcimio_ai_change(struct comedi_device *dev,
 	struct ni_private *devpriv = dev->private;
 	int ret;
 
-	ret = mite_buf_change(devpriv->ai_mite_ring, s->async);
+	ret = mite_buf_change(devpriv->ai_mite_ring, s);
 	if (ret < 0)
 		return ret;
 
@@ -1573,7 +1573,7 @@ static int pcimio_ao_change(struct comedi_device *dev,
 	struct ni_private *devpriv = dev->private;
 	int ret;
 
-	ret = mite_buf_change(devpriv->ao_mite_ring, s->async);
+	ret = mite_buf_change(devpriv->ao_mite_ring, s);
 	if (ret < 0)
 		return ret;
 
@@ -1587,7 +1587,7 @@ static int pcimio_gpct0_change(struct comedi_device *dev,
 	struct ni_private *devpriv = dev->private;
 	int ret;
 
-	ret = mite_buf_change(devpriv->gpct_mite_ring[0], s->async);
+	ret = mite_buf_change(devpriv->gpct_mite_ring[0], s);
 	if (ret < 0)
 		return ret;
 
@@ -1601,7 +1601,7 @@ static int pcimio_gpct1_change(struct comedi_device *dev,
 	struct ni_private *devpriv = dev->private;
 	int ret;
 
-	ret = mite_buf_change(devpriv->gpct_mite_ring[1], s->async);
+	ret = mite_buf_change(devpriv->gpct_mite_ring[1], s);
 	if (ret < 0)
 		return ret;
 
@@ -1614,7 +1614,7 @@ static int pcimio_dio_change(struct comedi_device *dev,
 	struct ni_private *devpriv = dev->private;
 	int ret;
 
-	ret = mite_buf_change(devpriv->cdo_mite_ring, s->async);
+	ret = mite_buf_change(devpriv->cdo_mite_ring, s);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/staging/comedi/drivers/ni_tiocmd.c b/drivers/staging/comedi/drivers/ni_tiocmd.c
index 7d64f8892f0..2557ab48cb6 100644
--- a/drivers/staging/comedi/drivers/ni_tiocmd.c
+++ b/drivers/staging/comedi/drivers/ni_tiocmd.c
@@ -89,14 +89,16 @@ static void ni_tio_configure_dma(struct ni_gpct *counter, short enable,
 
 static int ni_tio_input_inttrig(struct comedi_device *dev,
 				struct comedi_subdevice *s,
-				unsigned int trignum)
+				unsigned int trig_num)
 {
+	struct ni_gpct *counter = s->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned long flags;
 	int retval = 0;
-	struct ni_gpct *counter = s->private;
 
 	BUG_ON(counter == NULL);
-	if (trignum != 0)
+
+	if (trig_num != cmd->start_src)
 		return -EINVAL;
 
 	spin_lock_irqsave(&counter->lock, flags);
@@ -113,15 +115,17 @@ static int ni_tio_input_inttrig(struct comedi_device *dev,
 	return retval;
 }
 
-static int ni_tio_input_cmd(struct ni_gpct *counter, struct comedi_async *async)
+static int ni_tio_input_cmd(struct comedi_subdevice *s)
 {
+	struct ni_gpct *counter = s->private;
 	struct ni_gpct_device *counter_dev = counter->counter_dev;
 	unsigned cidx = counter->counter_index;
+	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
 	int retval = 0;
 
 	/* write alloc the entire buffer */
-	comedi_buf_write_alloc(async, async->prealloc_bufsz);
+	comedi_buf_write_alloc(s, async->prealloc_bufsz);
 	counter->mite_chan->dir = COMEDI_INPUT;
 	switch (counter_dev->variant) {
 	case ni_gpct_variant_m_series:
@@ -162,9 +166,10 @@ static int ni_tio_input_cmd(struct ni_gpct *counter, struct comedi_async *async)
 	return retval;
 }
 
-static int ni_tio_output_cmd(struct ni_gpct *counter,
-			     struct comedi_async *async)
+static int ni_tio_output_cmd(struct comedi_subdevice *s)
 {
+	struct ni_gpct *counter = s->private;
+
 	dev_err(counter->counter_dev->dev->class_dev,
 		"output commands not yet implemented.\n");
 	return -ENOTSUPP;
@@ -176,9 +181,10 @@ static int ni_tio_output_cmd(struct ni_gpct *counter,
 	return ni_tio_arm(counter, 1, NI_GPCT_ARM_IMMEDIATE);
 }
 
-static int ni_tio_cmd_setup(struct ni_gpct *counter, struct comedi_async *async)
+static int ni_tio_cmd_setup(struct comedi_subdevice *s)
 {
-	struct comedi_cmd *cmd = &async->cmd;
+	struct comedi_cmd *cmd = &s->async->cmd;
+	struct ni_gpct *counter = s->private;
 	unsigned cidx = counter->counter_index;
 	int set_gate_source = 0;
 	unsigned gate_source;
@@ -217,12 +223,12 @@ int ni_tio_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 			"Interrupt-driven commands not yet implemented.\n");
 		retval = -EIO;
 	} else {
-		retval = ni_tio_cmd_setup(counter, async);
+		retval = ni_tio_cmd_setup(s);
 		if (retval == 0) {
 			if (cmd->flags & CMDF_WRITE)
-				retval = ni_tio_output_cmd(counter, async);
+				retval = ni_tio_output_cmd(s);
 			else
-				retval = ni_tio_input_cmd(counter, async);
+				retval = ni_tio_input_cmd(s);
 		}
 	}
 	spin_unlock_irqrestore(&counter->lock, flags);
@@ -271,8 +277,16 @@ int ni_tio_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	if (cmd->start_src != TRIG_EXT)
+	switch (cmd->start_src) {
+	case TRIG_NOW:
+	case TRIG_INT:
+	case TRIG_OTHER:
 		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		break;
+	case TRIG_EXT:
+		/* start_arg is the start_trigger passed to ni_tio_arm() */
+		break;
+	}
 
 	if (cmd->scan_begin_src != TRIG_EXT)
 		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
@@ -450,7 +464,7 @@ void ni_tio_handle_interrupt(struct ni_gpct *counter,
 		       counter->mite_chan->mite->mite_io_addr +
 		       MITE_CHOR(counter->mite_chan->channel));
 	}
-	mite_sync_input_dma(counter->mite_chan, s->async);
+	mite_sync_input_dma(counter->mite_chan, s);
 	spin_unlock_irqrestore(&counter->lock, flags);
 }
 EXPORT_SYMBOL_GPL(ni_tio_handle_interrupt);
diff --git a/drivers/staging/comedi/drivers/pcl711.c b/drivers/staging/comedi/drivers/pcl711.c
index 7c03a5d17b1..c38d97a9a89 100644
--- a/drivers/staging/comedi/drivers/pcl711.c
+++ b/drivers/staging/comedi/drivers/pcl711.c
@@ -202,6 +202,7 @@ static irqreturn_t pcl711_interrupt(int irq, void *d)
 	struct comedi_device *dev = d;
 	struct pcl711_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int data;
 
 	if (!dev->attached) {
@@ -213,12 +214,11 @@ static irqreturn_t pcl711_interrupt(int irq, void *d)
 
 	outb(PCL711_INT_STAT_CLR, dev->iobase + PCL711_INT_STAT_REG);
 
-	if (comedi_buf_put(s->async, data) == 0) {
+	if (comedi_buf_put(s, data) == 0) {
 		s->async->events |= COMEDI_CB_OVERFLOW | COMEDI_CB_ERROR;
 	} else {
 		s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
-		if (s->async->cmd.stop_src == TRIG_COUNT &&
-		    !(--devpriv->ntrig)) {
+		if (cmd->stop_src == TRIG_COUNT && !(--devpriv->ntrig)) {
 			pcl711_ai_set_mode(dev, PCL711_MODE_SOFTTRIG);
 			s->async->events |= COMEDI_CB_EOA;
 		}
@@ -295,8 +295,8 @@ static int pcl711_ai_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
 	struct pcl711_private *devpriv = dev->private;
-	int tmp;
 	int err = 0;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -344,14 +344,12 @@ static int pcl711_ai_cmdtest(struct comedi_device *dev,
 	/* step 4 */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
+		arg = cmd->scan_begin_arg;
 		i8253_cascade_ns_to_timer(I8254_OSC_BASE_2MHZ,
 					  &devpriv->divisor1,
 					  &devpriv->divisor2,
-					  &cmd->scan_begin_arg,
-					  cmd->flags);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (err)
@@ -360,6 +358,18 @@ static int pcl711_ai_cmdtest(struct comedi_device *dev,
 	return 0;
 }
 
+static void pcl711_ai_load_counters(struct comedi_device *dev)
+{
+	struct pcl711_private *devpriv = dev->private;
+	unsigned long timer_base = dev->iobase + PCL711_TIMER_BASE;
+
+	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
+	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
+
+	i8254_write(timer_base, 0, 1, devpriv->divisor1);
+	i8254_write(timer_base, 0, 2, devpriv->divisor2);
+}
+
 static int pcl711_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
 	struct pcl711_private *devpriv = dev->private;
@@ -378,13 +388,8 @@ static int pcl711_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	}
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		i8254_load(dev->iobase + PCL711_TIMER_BASE, 0,
-			   1, devpriv->divisor1, I8254_MODE2 | I8254_BINARY);
-		i8254_load(dev->iobase + PCL711_TIMER_BASE, 0,
-			   2, devpriv->divisor2, I8254_MODE2 | I8254_BINARY);
-
+		pcl711_ai_load_counters(dev);
 		outb(PCL711_INT_STAT_CLR, dev->iobase + PCL711_INT_STAT_REG);
-
 		pcl711_ai_set_mode(dev, PCL711_MODE_PACER_IRQ);
 	} else {
 		pcl711_ai_set_mode(dev, PCL711_MODE_EXT_IRQ);
diff --git a/drivers/staging/comedi/drivers/pcl726.c b/drivers/staging/comedi/drivers/pcl726.c
index cf9568ee46e..74f6489bd12 100644
--- a/drivers/staging/comedi/drivers/pcl726.c
+++ b/drivers/staging/comedi/drivers/pcl726.c
@@ -197,7 +197,7 @@ static int pcl726_intr_cmdtest(struct comedi_device *dev,
 	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, 1);
+	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)
@@ -240,7 +240,7 @@ static irqreturn_t pcl726_interrupt(int irq, void *d)
 	if (devpriv->cmd_running) {
 		pcl726_intr_cancel(dev, s);
 
-		comedi_buf_put(s->async, 0);
+		comedi_buf_put(s, 0);
 		s->async->events |= (COMEDI_CB_BLOCK | COMEDI_CB_EOS);
 		comedi_event(dev, s);
 	}
@@ -430,6 +430,7 @@ static int pcl726_attach(struct comedi_device *dev,
 		s->maxdata	= 1;
 		s->range_table	= &range_digital;
 		s->insn_bits	= pcl726_intr_insn_bits;
+		s->len_chanlist	= 1;
 		s->do_cmdtest	= pcl726_intr_cmdtest;
 		s->do_cmd	= pcl726_intr_cmd;
 		s->cancel	= pcl726_intr_cancel;
diff --git a/drivers/staging/comedi/drivers/pcl730.c b/drivers/staging/comedi/drivers/pcl730.c
index 2baaf1db6fb..7fb044ce399 100644
--- a/drivers/staging/comedi/drivers/pcl730.c
+++ b/drivers/staging/comedi/drivers/pcl730.c
@@ -17,6 +17,8 @@
  *	    (ICP) P16R16-DIO [p16r16dio]
  *	    (Advantech) PCL-733 [pcl733]
  *	    (Advantech) PCL-734 [pcl734]
+ *	    (Diamond Systems) OPMM-1616-XT [opmm-1616-xt]
+ *	    (Diamond Systems) PEARL-MM-P [prearl-mm-p]
  * Author: José Luis Sánchez (jsanchezv@teleline.es)
  * Status: untested
  *
@@ -70,6 +72,27 @@
  *     BASE+1  Isolated outputs 8-15 (write) or inputs 8-15 (read)
  *     BASE+2  Isolated outputs 16-23 (write) or inputs 16-23 (read)
  *     BASE+3  Isolated outputs 24-31 (write) or inputs 24-31 (read)
+ *
+ * The opmm-1616-xt board has this register mapping:
+ *
+ *     BASE+0  Isolated outputs 0-7 (write) (read back)
+ *     BASE+1  Isolated outputs 8-15 (write) (read back)
+ *     BASE+2  Isolated inputs 0-7 (read)
+ *     BASE+3  Isolated inputs 8-15 (read)
+ *
+ *     These registers are not currently supported:
+ *
+ *     BASE+2  Relay select register (write)
+ *     BASE+3  Board reset control register (write)
+ *     BASE+4  Interrupt control register (write)
+ *     BASE+4  Change detect 7-0 status register (read)
+ *     BASE+5  LED control register (write)
+ *     BASE+5  Change detect 15-8 status register (read)
+ *
+ * The pearl-mm-p board has this register mapping:
+ *
+ *     BASE+0  Isolated outputs 0-7 (write)
+ *     BASE+1  Isolated outputs 8-15 (write)
  */
 
 struct pcl730_board {
@@ -158,6 +181,19 @@ static const struct pcl730_board pcl730_boards[] = {
 		.io_range	= 0x04,
 		.n_subdevs	= 1,
 		.n_iso_out_chan	= 32,
+	}, {
+		.name		= "opmm-1616-xt",
+		.io_range	= 0x10,
+		.is_acl7225b	= 1,
+		.has_readback	= 1,
+		.n_subdevs	= 2,
+		.n_iso_out_chan	= 16,
+		.n_iso_in_chan	= 16,
+	}, {
+		.name		= "pearl-mm-p",
+		.io_range	= 0x02,
+		.n_subdevs	= 1,
+		.n_iso_out_chan	= 16,
 	},
 };
 
diff --git a/drivers/staging/comedi/drivers/pcl812.c b/drivers/staging/comedi/drivers/pcl812.c
index 160eac8083d..4c1b9470647 100644
--- a/drivers/staging/comedi/drivers/pcl812.c
+++ b/drivers/staging/comedi/drivers/pcl812.c
@@ -557,10 +557,8 @@ static void pcl812_ai_setup_dma(struct comedi_device *dev,
 
 	/*  we use EOS, so adapt DMA buffer to one scan */
 	if (devpriv->ai_eos) {
-		devpriv->dmabytestomove[0] =
-			cmd->chanlist_len * sizeof(short);
-		devpriv->dmabytestomove[1] =
-			cmd->chanlist_len * sizeof(short);
+		devpriv->dmabytestomove[0] = cfc_bytes_per_scan(s);
+		devpriv->dmabytestomove[1] = cfc_bytes_per_scan(s);
 		devpriv->dma_runs_to_end = 1;
 	} else {
 		devpriv->dmabytestomove[0] = devpriv->hwdmasize;
@@ -575,8 +573,7 @@ static void pcl812_ai_setup_dma(struct comedi_device *dev,
 			devpriv->dma_runs_to_end = 1;
 		} else {
 			/*  how many samples we must transfer? */
-			bytes = cmd->chanlist_len *
-				cmd->stop_arg * sizeof(short);
+			bytes = cmd->stop_arg * cfc_bytes_per_scan(s);
 
 			/*  how many DMA pages we must fill */
 			devpriv->dma_runs_to_end =
@@ -721,7 +718,7 @@ static int pcl812_ai_cmdtest(struct comedi_device *dev,
 	struct pcl812_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int flags;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -761,7 +758,6 @@ static int pcl812_ai_cmdtest(struct comedi_device *dev,
 		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
 
 	err |= cfc_check_trigger_arg_min(&cmd->chanlist_len, 1);
-	err |= cfc_check_trigger_arg_max(&cmd->chanlist_len, MAX_CHANLIST_LEN);
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
@@ -775,15 +771,12 @@ static int pcl812_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
+		arg = cmd->convert_arg;
 		i8253_cascade_ns_to_timer(I8254_OSC_BASE_2MHZ,
 					  &devpriv->divisor1,
 					  &devpriv->divisor2,
-					  &cmd->convert_arg, cmd->flags);
-		if (cmd->convert_arg < board->ai_ns_min)
-			cmd->convert_arg = board->ai_ns_min;
-		if (tmp != cmd->convert_arg)
-			err++;
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
@@ -811,8 +804,9 @@ static int pcl812_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 				devpriv->ai_dma = 0;
 				break;
 			}
-	} else
+	} else {
 		devpriv->ai_dma = 0;
+	}
 
 	devpriv->ai_act_scan = 0;
 	devpriv->ai_poll_ptr = 0;
@@ -882,7 +876,7 @@ static void pcl812_handle_eoc(struct comedi_device *dev,
 		return;
 	}
 
-	comedi_buf_put(s->async, pcl812_ai_get_sample(dev, s));
+	comedi_buf_put(s, pcl812_ai_get_sample(dev, s));
 
 	/* Set up next channel. Added by abbotti 2010-01-20, but untested. */
 	next_chan = s->async->cur_chan + 1;
@@ -902,7 +896,7 @@ static void transfer_from_dma_buf(struct comedi_device *dev,
 	unsigned int i;
 
 	for (i = len; i; i--) {
-		comedi_buf_put(s->async, ptr[bufptr++]);
+		comedi_buf_put(s, ptr[bufptr++]);
 
 		if (!pcl812_ai_next_chan(dev, s))
 			break;
diff --git a/drivers/staging/comedi/drivers/pcl816.c b/drivers/staging/comedi/drivers/pcl816.c
index 6f276f23fab..d9ca7fe16c9 100644
--- a/drivers/staging/comedi/drivers/pcl816.c
+++ b/drivers/staging/comedi/drivers/pcl816.c
@@ -164,9 +164,7 @@ static void pcl816_ai_setup_dma(struct comedi_device *dev,
 	bytes = devpriv->hwdmasize;
 	if (cmd->stop_src == TRIG_COUNT) {
 		/*  how many */
-		bytes = s->async->cmd.chanlist_len *
-		s->async->cmd.chanlist_len *
-		sizeof(short);
+		bytes = cmd->stop_arg * cfc_bytes_per_scan(s);
 
 		/*  how many DMA pages we must fill */
 		devpriv->dma_runs_to_end = bytes / devpriv->hwdmasize;
@@ -322,7 +320,7 @@ static void transfer_from_dma_buf(struct comedi_device *dev,
 	int i;
 
 	for (i = 0; i < len; i++) {
-		comedi_buf_put(s->async, ptr[bufptr++]);
+		comedi_buf_put(s, ptr[bufptr++]);
 
 		if (!pcl816_ai_next_chan(dev, s))
 			return;
@@ -370,7 +368,7 @@ static int pcl816_ai_cmdtest(struct comedi_device *dev,
 {
 	struct pcl816_private *devpriv = dev->private;
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -417,15 +415,12 @@ static int pcl816_ai_cmdtest(struct comedi_device *dev,
 
 	/* step 4: fix up any arguments */
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
+		arg = cmd->convert_arg;
 		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
 					  &devpriv->divisor1,
 					  &devpriv->divisor2,
-					  &cmd->convert_arg, cmd->flags);
-		if (cmd->convert_arg < 10000)
-			cmd->convert_arg = 10000;
-		if (tmp != cmd->convert_arg)
-			err++;
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
diff --git a/drivers/staging/comedi/drivers/pcl818.c b/drivers/staging/comedi/drivers/pcl818.c
index 6463476ce45..7d00ae639d3 100644
--- a/drivers/staging/comedi/drivers/pcl818.c
+++ b/drivers/staging/comedi/drivers/pcl818.c
@@ -353,7 +353,7 @@ static void pcl818_ai_setup_dma(struct comedi_device *dev,
 	disable_dma(devpriv->dma);	/*  disable dma */
 	bytes = devpriv->hwdmasize;
 	if (cmd->stop_src == TRIG_COUNT) {
-		bytes = cmd->chanlist_len * cmd->stop_arg * sizeof(short);
+		bytes = cmd->stop_arg * cfc_bytes_per_scan(s);
 		devpriv->dma_runs_to_end = bytes / devpriv->hwdmasize;
 		devpriv->last_dma_run = bytes % devpriv->hwdmasize;
 		devpriv->dma_runs_to_end--;
@@ -561,7 +561,7 @@ static void pcl818_handle_eoc(struct comedi_device *dev,
 	if (pcl818_ai_dropout(dev, s, chan))
 		return;
 
-	comedi_buf_put(s->async, val);
+	comedi_buf_put(s, val);
 
 	pcl818_ai_next_chan(dev, s);
 }
@@ -590,7 +590,7 @@ static void pcl818_handle_dma(struct comedi_device *dev,
 		if (pcl818_ai_dropout(dev, s, chan))
 			break;
 
-		comedi_buf_put(s->async, val);
+		comedi_buf_put(s, val);
 
 		if (!pcl818_ai_next_chan(dev, s))
 			break;
@@ -630,7 +630,7 @@ static void pcl818_handle_fifo(struct comedi_device *dev,
 		if (pcl818_ai_dropout(dev, s, chan))
 			break;
 
-		comedi_buf_put(s->async, val);
+		comedi_buf_put(s, val);
 
 		if (!pcl818_ai_next_chan(dev, s))
 			break;
@@ -741,7 +741,7 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	const struct pcl818_board *board = comedi_board(dev);
 	struct pcl818_private *devpriv = dev->private;
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -788,15 +788,12 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	/* step 4: fix up any arguments */
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
+		arg = cmd->convert_arg;
 		i8253_cascade_ns_to_timer(devpriv->i8253_osc_base,
 					  &devpriv->divisor1,
 					  &devpriv->divisor2,
-					  &cmd->convert_arg, cmd->flags);
-		if (cmd->convert_arg < board->ns_min)
-			cmd->convert_arg = board->ns_min;
-		if (tmp != cmd->convert_arg)
-			err++;
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
diff --git a/drivers/staging/comedi/drivers/pcmmio.c b/drivers/staging/comedi/drivers/pcmmio.c
index e89bca84534..fed7e77e030 100644
--- a/drivers/staging/comedi/drivers/pcmmio.c
+++ b/drivers/staging/comedi/drivers/pcmmio.c
@@ -192,7 +192,6 @@ struct pcmmio_private {
 	unsigned int enabled_mask;
 	unsigned int stop_count;
 	unsigned int active:1;
-	unsigned int continuous:1;
 
 	unsigned int ao_readback[8];
 };
@@ -339,8 +338,8 @@ static void pcmmio_handle_dio_intr(struct comedi_device *dev,
 				   unsigned int triggered)
 {
 	struct pcmmio_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int oldevents = s->async->events;
-	unsigned int len = s->async->cmd.chanlist_len;
 	unsigned int val = 0;
 	unsigned long flags;
 	int i;
@@ -353,16 +352,16 @@ static void pcmmio_handle_dio_intr(struct comedi_device *dev,
 	if (!(triggered & devpriv->enabled_mask))
 		goto done;
 
-	for (i = 0; i < len; i++) {
-		unsigned int chan = CR_CHAN(s->async->cmd.chanlist[i]);
+	for (i = 0; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
 
 		if (triggered & (1 << chan))
 			val |= (1 << i);
 	}
 
 	/* Write the scan to the buffer. */
-	if (comedi_buf_put(s->async, val) &&
-	    comedi_buf_put(s->async, val >> 16)) {
+	if (comedi_buf_put(s, val) &&
+	    comedi_buf_put(s, val >> 16)) {
 		s->async->events |= (COMEDI_CB_BLOCK | COMEDI_CB_EOS);
 	} else {
 		/* Overflow! Stop acquisition!! */
@@ -371,15 +370,12 @@ static void pcmmio_handle_dio_intr(struct comedi_device *dev,
 	}
 
 	/* Check for end of acquisition. */
-	if (!devpriv->continuous) {
-		/* stop_src == TRIG_COUNT */
-		if (devpriv->stop_count > 0) {
-			devpriv->stop_count--;
-			if (devpriv->stop_count == 0) {
-				s->async->events |= COMEDI_CB_EOA;
-				/* TODO: STOP_ACQUISITION_CALL_HERE!! */
-				pcmmio_stop_intr(dev, s);
-			}
+	if (cmd->stop_src == TRIG_COUNT && devpriv->stop_count > 0) {
+		devpriv->stop_count--;
+		if (devpriv->stop_count == 0) {
+			s->async->events |= COMEDI_CB_EOA;
+			/* TODO: STOP_ACQUISITION_CALL_HERE!! */
+			pcmmio_stop_intr(dev, s);
 		}
 	}
 
@@ -421,7 +417,7 @@ static int pcmmio_start_intr(struct comedi_device *dev,
 	unsigned int pol_bits = 0;
 	int i;
 
-	if (!devpriv->continuous && devpriv->stop_count == 0) {
+	if (cmd->stop_src == TRIG_COUNT && devpriv->stop_count == 0) {
 		/* An empty acquisition! */
 		s->async->events |= COMEDI_CB_EOA;
 		devpriv->active = 0;
@@ -464,18 +460,16 @@ static int pcmmio_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 	return 0;
 }
 
-/*
- * Internal trigger function to start acquisition for an 'INTERRUPT' subdevice.
- */
-static int
-pcmmio_inttrig_start_intr(struct comedi_device *dev, struct comedi_subdevice *s,
-			  unsigned int trignum)
+static int pcmmio_inttrig_start_intr(struct comedi_device *dev,
+				     struct comedi_subdevice *s,
+				     unsigned int trig_num)
 {
 	struct pcmmio_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned long flags;
 	int event = 0;
 
-	if (trignum != 0)
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	spin_lock_irqsave(&devpriv->spinlock, flags);
@@ -504,28 +498,17 @@ static int pcmmio_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	devpriv->active = 1;
 
 	/* Set up end of acquisition. */
-	switch (cmd->stop_src) {
-	case TRIG_COUNT:
-		devpriv->continuous = 0;
+	if (cmd->stop_src == TRIG_COUNT)
 		devpriv->stop_count = cmd->stop_arg;
-		break;
-	default:
-		/* TRIG_NONE */
-		devpriv->continuous = 1;
+	else	/* TRIG_NONE */
 		devpriv->stop_count = 0;
-		break;
-	}
 
 	/* Set up start of acquisition. */
-	switch (cmd->start_src) {
-	case TRIG_INT:
+	if (cmd->start_src == TRIG_INT)
 		s->async->inttrig = pcmmio_inttrig_start_intr;
-		break;
-	default:
-		/* TRIG_NOW */
+	else	/* TRIG_NOW */
 		event = pcmmio_start_intr(dev, s);
-		break;
-	}
+
 	spin_unlock_irqrestore(&devpriv->spinlock, flags);
 
 	if (event)
diff --git a/drivers/staging/comedi/drivers/pcmuio.c b/drivers/staging/comedi/drivers/pcmuio.c
index a8f390f7a87..62914bb342d 100644
--- a/drivers/staging/comedi/drivers/pcmuio.c
+++ b/drivers/staging/comedi/drivers/pcmuio.c
@@ -317,7 +317,7 @@ static void pcmuio_handle_intr_subdev(struct comedi_device *dev,
 	struct pcmuio_private *devpriv = dev->private;
 	int asic = pcmuio_subdevice_to_asic(s);
 	struct pcmuio_asic *chip = &devpriv->asics[asic];
-	unsigned int len = s->async->cmd.chanlist_len;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned oldevents = s->async->events;
 	unsigned int val = 0;
 	unsigned long flags;
@@ -331,15 +331,15 @@ static void pcmuio_handle_intr_subdev(struct comedi_device *dev,
 	if (!(triggered & chip->enabled_mask))
 		goto done;
 
-	for (i = 0; i < len; i++) {
-		unsigned int chan = CR_CHAN(s->async->cmd.chanlist[i]);
+	for (i = 0; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
 		if (triggered & (1 << chan))
 			val |= (1 << i);
 	}
 
 	/* Write the scan to the buffer. */
-	if (comedi_buf_put(s->async, val) &&
-	    comedi_buf_put(s->async, val >> 16)) {
+	if (comedi_buf_put(s, val) &&
+	    comedi_buf_put(s, val >> 16)) {
 		s->async->events |= (COMEDI_CB_BLOCK | COMEDI_CB_EOS);
 	} else {
 		/* Overflow! Stop acquisition!! */
@@ -460,20 +460,18 @@ static int pcmuio_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 	return 0;
 }
 
-/*
- * Internal trigger function to start acquisition for an 'INTERRUPT' subdevice.
- */
-static int
-pcmuio_inttrig_start_intr(struct comedi_device *dev, struct comedi_subdevice *s,
-			  unsigned int trignum)
+static int pcmuio_inttrig_start_intr(struct comedi_device *dev,
+				     struct comedi_subdevice *s,
+				     unsigned int trig_num)
 {
 	struct pcmuio_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	int asic = pcmuio_subdevice_to_asic(s);
 	struct pcmuio_asic *chip = &devpriv->asics[asic];
 	unsigned long flags;
 	int event = 0;
 
-	if (trignum != 0)
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	spin_lock_irqsave(&chip->spinlock, flags);
@@ -518,15 +516,11 @@ static int pcmuio_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	}
 
 	/* Set up start of acquisition. */
-	switch (cmd->start_src) {
-	case TRIG_INT:
+	if (cmd->start_src == TRIG_INT)
 		s->async->inttrig = pcmuio_inttrig_start_intr;
-		break;
-	default:
-		/* TRIG_NOW */
+	else	/* TRIG_NOW */
 		event = pcmuio_start_intr(dev, s);
-		break;
-	}
+
 	spin_unlock_irqrestore(&chip->spinlock, flags);
 
 	if (event)
diff --git a/drivers/staging/comedi/drivers/quatech_daqp_cs.c b/drivers/staging/comedi/drivers/quatech_daqp_cs.c
index 298dba03f90..b3bbec0a0d2 100644
--- a/drivers/staging/comedi/drivers/quatech_daqp_cs.c
+++ b/drivers/staging/comedi/drivers/quatech_daqp_cs.c
@@ -221,7 +221,7 @@ static enum irqreturn daqp_interrupt(int irq, void *dev_id)
 			data |= inb(dev->iobase + DAQP_FIFO) << 8;
 			data ^= 0x8000;
 
-			comedi_buf_put(s->async, data);
+			comedi_buf_put(s, data);
 
 			/* If there's a limit, decrement it
 			 * and stop conversion if zero
@@ -373,7 +373,7 @@ static int daqp_ai_cmdtest(struct comedi_device *dev,
 			   struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -435,19 +435,15 @@ static int daqp_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
-		daqp_ns_to_timer(&cmd->scan_begin_arg,
-				 cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+		arg = cmd->scan_begin_arg;
+		daqp_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
-		daqp_ns_to_timer(&cmd->convert_arg,
-				 cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->convert_arg)
-			err++;
+		arg = cmd->convert_arg;
+		daqp_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
diff --git a/drivers/staging/comedi/drivers/rtd520.c b/drivers/staging/comedi/drivers/rtd520.c
index cd3fdf973bd..d55c5893203 100644
--- a/drivers/staging/comedi/drivers/rtd520.c
+++ b/drivers/staging/comedi/drivers/rtd520.c
@@ -629,7 +629,7 @@ static int ai_read_n(struct comedi_device *dev, struct comedi_subdevice *s,
 			d = comedi_offset_munge(s, d);
 		d &= s->maxdata;
 
-		if (!comedi_buf_put(s->async, d))
+		if (!comedi_buf_put(s, d))
 			return -1;
 
 		if (devpriv->ai_count > 0)	/* < 0, means read forever */
@@ -658,7 +658,7 @@ static int ai_read_dregs(struct comedi_device *dev, struct comedi_subdevice *s)
 			d = comedi_offset_munge(s, d);
 		d &= s->maxdata;
 
-		if (!comedi_buf_put(s->async, d))
+		if (!comedi_buf_put(s, d))
 			return -1;
 
 		if (devpriv->ai_count > 0)	/* < 0, means read forever */
@@ -779,7 +779,7 @@ static int rtd_ai_cmdtest(struct comedi_device *dev,
 			  struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -878,6 +878,8 @@ static int rtd_ai_cmdtest(struct comedi_device *dev,
 		err |= cfc_check_trigger_arg_max(&cmd->convert_arg, 9);
 	}
 
+	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 {
@@ -891,31 +893,21 @@ static int rtd_ai_cmdtest(struct comedi_device *dev,
 
 	/* step 4: fix up any arguments */
 
-	if (cmd->chanlist_len > RTD_MAX_CHANLIST) {
-		cmd->chanlist_len = RTD_MAX_CHANLIST;
-		err++;
-	}
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
-		rtd_ns_to_timer(&cmd->scan_begin_arg,
-				cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
-
+		arg = cmd->scan_begin_arg;
+		rtd_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
+
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
-		rtd_ns_to_timer(&cmd->convert_arg,
-				cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->convert_arg)
-			err++;
-
-		if (cmd->scan_begin_src == TRIG_TIMER
-		    && (cmd->scan_begin_arg
-			< (cmd->convert_arg * cmd->scan_end_arg))) {
-			cmd->scan_begin_arg =
-			    cmd->convert_arg * cmd->scan_end_arg;
-			err++;
+		arg = cmd->convert_arg;
+		rtd_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+		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);
 		}
 	}
 
diff --git a/drivers/staging/comedi/drivers/s626.c b/drivers/staging/comedi/drivers/s626.c
index 95fadf343f2..0838f8aa695 100644
--- a/drivers/staging/comedi/drivers/s626.c
+++ b/drivers/staging/comedi/drivers/s626.c
@@ -295,10 +295,24 @@ static void s626_debi_replace(struct comedi_device *dev, unsigned int addr,
 
 /* **************  EEPROM ACCESS FUNCTIONS  ************** */
 
-static uint32_t s626_i2c_handshake(struct comedi_device *dev, uint32_t val)
+static int s626_i2c_handshake_eoc(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned long context)
+{
+	bool status;
+
+	status = s626_mc_test(dev, S626_MC2_UPLD_IIC, S626_P_MC2);
+	if (status)
+		return 0;
+	return -EBUSY;
+}
+
+static int s626_i2c_handshake(struct comedi_device *dev, uint32_t val)
 {
 	struct s626_private *devpriv = dev->private;
 	unsigned int ctrl;
+	int ret;
 
 	/* Write I2C command to I2C Transfer Control shadow register */
 	writel(val, devpriv->mmio + S626_P_I2CCTRL);
@@ -308,8 +322,9 @@ static uint32_t s626_i2c_handshake(struct comedi_device *dev, uint32_t val)
 	 * wait for upload confirmation.
 	 */
 	s626_mc_enable(dev, S626_MC2_UPLD_IIC, S626_P_MC2);
-	while (!s626_mc_test(dev, S626_MC2_UPLD_IIC, S626_P_MC2))
-		;
+	ret = comedi_timeout(dev, NULL, NULL, s626_i2c_handshake_eoc, 0);
+	if (ret)
+		return ret;
 
 	/* Wait until I2C bus transfer is finished or an error occurs */
 	do {
@@ -1613,7 +1628,6 @@ static bool s626_handle_eos_interrupt(struct comedi_device *dev)
 		readaddr++;
 
 		/* put data into read buffer */
-		/* comedi_buf_put(async, tempdata); */
 		cfc_write_to_buffer(s, tempdata);
 	}
 
@@ -2029,8 +2043,9 @@ static int s626_ai_insn_read(struct comedi_device *dev,
 	/* Wait for the data to arrive in FB BUFFER 1 register. */
 
 	/* Wait for ADC done */
-	while (!(readl(devpriv->mmio + S626_P_PSR) & S626_PSR_GPIO2))
-		;
+	ret = comedi_timeout(dev, s, insn, s626_ai_eoc, 0);
+	if (ret)
+		return ret;
 
 	/* Fetch ADC data from audio interface's input shift register. */
 
@@ -2060,9 +2075,12 @@ static int s626_ai_load_polllist(uint8_t *ppl, struct comedi_cmd *cmd)
 }
 
 static int s626_ai_inttrig(struct comedi_device *dev,
-			   struct comedi_subdevice *s, unsigned int trignum)
+			   struct comedi_subdevice *s,
+			   unsigned int trig_num)
 {
-	if (trignum != 0)
+	struct comedi_cmd *cmd = &s->async->cmd;
+
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	/* Start executing the RPS program */
@@ -2080,7 +2098,7 @@ static int s626_ai_inttrig(struct comedi_device *dev,
  * Also, it should adjust ns so that it cooresponds to the actual time
  * that the device will use.
  */
-static int s626_ns_to_timer(int *nanosec, int round_mode)
+static int s626_ns_to_timer(unsigned int *nanosec, int round_mode)
 {
 	int divider, base;
 
@@ -2188,7 +2206,7 @@ static int s626_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		 * interval
 		 */
 		k = &s626_enc_chan_info[5];
-		tick = s626_ns_to_timer((int *)&cmd->scan_begin_arg,
+		tick = s626_ns_to_timer(&cmd->scan_begin_arg,
 					cmd->flags & TRIG_ROUND_MASK);
 
 		/* load timer value and enable interrupt */
@@ -2211,7 +2229,7 @@ static int s626_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		 * interval
 		 */
 		k = &s626_enc_chan_info[4];
-		tick = s626_ns_to_timer((int *)&cmd->convert_arg,
+		tick = s626_ns_to_timer(&cmd->convert_arg,
 					cmd->flags & TRIG_ROUND_MASK);
 
 		/* load timer value and enable interrupt */
@@ -2270,7 +2288,7 @@ static int s626_ai_cmdtest(struct comedi_device *dev,
 			   struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -2298,12 +2316,18 @@ static int s626_ai_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 2;
 
-	/* step 3: make sure arguments are trivially compatible */
+	/* Step 3: check if arguments are trivially valid */
 
-	if (cmd->start_src != TRIG_EXT)
+	switch (cmd->start_src) {
+	case TRIG_NOW:
+	case TRIG_INT:
 		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-	if (cmd->start_src == TRIG_EXT)
+		break;
+	case TRIG_EXT:
 		err |= cfc_check_trigger_arg_max(&cmd->start_arg, 39);
+		break;
+	}
+
 	if (cmd->scan_begin_src == TRIG_EXT)
 		err |= cfc_check_trigger_arg_max(&cmd->scan_begin_arg, 39);
 	if (cmd->convert_src == TRIG_EXT)
@@ -2347,24 +2371,20 @@ static int s626_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
-		s626_ns_to_timer((int *)&cmd->scan_begin_arg,
-				 cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+		arg = cmd->scan_begin_arg;
+		s626_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
+
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
-		s626_ns_to_timer((int *)&cmd->convert_arg,
-				 cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->convert_arg)
-			err++;
-		if (cmd->scan_begin_src == TRIG_TIMER &&
-		    cmd->scan_begin_arg < cmd->convert_arg *
-					  cmd->scan_end_arg) {
-			cmd->scan_begin_arg = cmd->convert_arg *
-					      cmd->scan_end_arg;
-			err++;
+		arg = cmd->convert_arg;
+		s626_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+		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);
 		}
 	}
 
@@ -2681,8 +2701,9 @@ static int s626_initialize(struct comedi_device *dev)
 	writel(S626_I2C_CLKSEL | S626_I2C_ABORT,
 	       devpriv->mmio + S626_P_I2CSTAT);
 	s626_mc_enable(dev, S626_MC2_UPLD_IIC, S626_P_MC2);
-	while (!(readl(devpriv->mmio + S626_P_MC2) & S626_MC2_UPLD_IIC))
-		;
+	ret = comedi_timeout(dev, NULL, NULL, s626_i2c_handshake_eoc, 0);
+	if (ret)
+		return ret;
 
 	/*
 	 * Per SAA7146 data sheet, write to STATUS
@@ -2691,8 +2712,9 @@ static int s626_initialize(struct comedi_device *dev)
 	for (i = 0; i < 2; i++) {
 		writel(S626_I2C_CLKSEL, devpriv->mmio + S626_P_I2CSTAT);
 		s626_mc_enable(dev, S626_MC2_UPLD_IIC, S626_P_MC2);
-		while (!s626_mc_test(dev, S626_MC2_UPLD_IIC, S626_P_MC2))
-			;
+		ret = comedi_timeout(dev, NULL, NULL, s626_i2c_handshake_eoc, 0);
+		if (ret)
+			return ret;
 	}
 
 	/*
diff --git a/drivers/staging/comedi/drivers/skel.c b/drivers/staging/comedi/drivers/skel.c
index 39008cf30ec..3bfa221faf4 100644
--- a/drivers/staging/comedi/drivers/skel.c
+++ b/drivers/staging/comedi/drivers/skel.c
@@ -220,7 +220,7 @@ static int skel_ai_cmdtest(struct comedi_device *dev,
 			   struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -286,24 +286,19 @@ static int skel_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
-		skel_ns_to_timer(&cmd->scan_begin_arg,
-				 cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+		arg = cmd->scan_begin_arg;
+		skel_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
-		skel_ns_to_timer(&cmd->convert_arg,
-				 cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->convert_arg)
-			err++;
-		if (cmd->scan_begin_src == TRIG_TIMER &&
-		    cmd->scan_begin_arg <
-		    cmd->convert_arg * cmd->scan_end_arg) {
-			cmd->scan_begin_arg =
-			    cmd->convert_arg * cmd->scan_end_arg;
-			err++;
+		arg = cmd->convert_arg;
+		skel_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+		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);
 		}
 	}
 
diff --git a/drivers/staging/comedi/drivers/usbdux.c b/drivers/staging/comedi/drivers/usbdux.c
index b59af030358..5f65e4213c6 100644
--- a/drivers/staging/comedi/drivers/usbdux.c
+++ b/drivers/staging/comedi/drivers/usbdux.c
@@ -202,14 +202,11 @@ struct usbdux_private {
 	/* input buffer for single insn */
 	uint16_t *insn_buf;
 
-	uint8_t ao_chanlist[USBDUX_NUM_AO_CHAN];
 	unsigned int ao_readback[USBDUX_NUM_AO_CHAN];
 
 	unsigned int high_speed:1;
 	unsigned int ai_cmd_running:1;
-	unsigned int ai_continous:1;
 	unsigned int ao_cmd_running:1;
-	unsigned int ao_continous:1;
 	unsigned int pwm_cmd_running:1;
 
 	/* number of samples to acquire */
@@ -266,7 +263,8 @@ static void usbduxsub_ai_isoc_irq(struct urb *urb)
 	struct comedi_device *dev = urb->context;
 	struct comedi_subdevice *s = dev->read_subdev;
 	struct usbdux_private *devpriv = dev->private;
-	int i, err, n;
+	struct comedi_cmd *cmd = &s->async->cmd;
+	int i, err;
 
 	/* first we test if something unusual has just happened */
 	switch (urb->status) {
@@ -349,7 +347,7 @@ static void usbduxsub_ai_isoc_irq(struct urb *urb)
 	devpriv->ai_counter = devpriv->ai_timer;
 
 	/* test, if we transmit only a fixed number of samples */
-	if (!devpriv->ai_continous) {
+	if (cmd->stop_src == TRIG_COUNT) {
 		/* not continuous, fixed number of samples */
 		devpriv->ai_sample_count--;
 		/* all samples received? */
@@ -363,9 +361,8 @@ static void usbduxsub_ai_isoc_irq(struct urb *urb)
 		}
 	}
 	/* get the data from the USB bus and hand it over to comedi */
-	n = s->async->cmd.chanlist_len;
-	for (i = 0; i < n; i++) {
-		unsigned int range = CR_RANGE(s->async->cmd.chanlist[i]);
+	for (i = 0; i < cmd->chanlist_len; i++) {
+		unsigned int range = CR_RANGE(cmd->chanlist[i]);
 		uint16_t val = le16_to_cpu(devpriv->in_buf[i]);
 
 		/* bipolar data is two's-complement */
@@ -373,7 +370,7 @@ static void usbduxsub_ai_isoc_irq(struct urb *urb)
 			val ^= ((s->maxdata + 1) >> 1);
 
 		/* transfer data */
-		err = comedi_buf_put(s->async, val);
+		err = comedi_buf_put(s, val);
 		if (unlikely(err == 0)) {
 			/* buffer overflow */
 			usbdux_ai_stop(dev, 0);
@@ -414,8 +411,8 @@ static void usbduxsub_ao_isoc_irq(struct urb *urb)
 	struct comedi_device *dev = urb->context;
 	struct comedi_subdevice *s = dev->write_subdev;
 	struct usbdux_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	uint8_t *datap;
-	int len;
 	int ret;
 	int i;
 
@@ -463,7 +460,7 @@ static void usbduxsub_ao_isoc_irq(struct urb *urb)
 		devpriv->ao_counter = devpriv->ao_timer;
 
 		/* handle non continous acquisition */
-		if (!devpriv->ao_continous) {
+		if (cmd->stop_src == TRIG_COUNT) {
 			/* fixed number of samples */
 			devpriv->ao_sample_count--;
 			if (devpriv->ao_sample_count < 0) {
@@ -478,13 +475,12 @@ static void usbduxsub_ao_isoc_irq(struct urb *urb)
 
 		/* transmit data to the USB bus */
 		datap = urb->transfer_buffer;
-		len = s->async->cmd.chanlist_len;
-		*datap++ = len;
-		for (i = 0; i < s->async->cmd.chanlist_len; i++) {
-			unsigned int chan = devpriv->ao_chanlist[i];
+		*datap++ = cmd->chanlist_len;
+		for (i = 0; i < cmd->chanlist_len; i++) {
+			unsigned int chan = CR_CHAN(cmd->chanlist[i]);
 			unsigned short val;
 
-			ret = comedi_buf_get(s->async, &val);
+			ret = comedi_buf_get(s, &val);
 			if (ret < 0) {
 				dev_err(dev->class_dev, "buffer underflow\n");
 				s->async->events |= (COMEDI_CB_EOA |
@@ -692,15 +688,16 @@ static int receive_dux_commands(struct comedi_device *dev, unsigned int command)
 
 static int usbdux_ai_inttrig(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
-			     unsigned int trignum)
+			     unsigned int trig_num)
 {
 	struct usbdux_private *devpriv = dev->private;
-	int ret = -EINVAL;
+	struct comedi_cmd *cmd = &s->async->cmd;
+	int ret;
 
-	down(&devpriv->sem);
+	if (trig_num != cmd->start_arg)
+		return -EINVAL;
 
-	if (trignum != 0)
-		goto ai_trig_exit;
+	down(&devpriv->sem);
 
 	if (!devpriv->ai_cmd_running) {
 		devpriv->ai_cmd_running = 1;
@@ -777,10 +774,8 @@ static int usbdux_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	if (cmd->stop_src == TRIG_COUNT) {
 		/* data arrives as one packet */
 		devpriv->ai_sample_count = cmd->stop_arg;
-		devpriv->ai_continous = 0;
 	} else {
 		/* continous acquisition */
-		devpriv->ai_continous = 1;
 		devpriv->ai_sample_count = 0;
 	}
 
@@ -913,15 +908,16 @@ ao_write_exit:
 
 static int usbdux_ao_inttrig(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
-			     unsigned int trignum)
+			     unsigned int trig_num)
 {
 	struct usbdux_private *devpriv = dev->private;
-	int ret = -EINVAL;
+	struct comedi_cmd *cmd = &s->async->cmd;
+	int ret;
 
-	down(&devpriv->sem);
+	if (trig_num != cmd->start_arg)
+		return -EINVAL;
 
-	if (trignum != 0)
-		goto ao_trig_exit;
+	down(&devpriv->sem);
 
 	if (!devpriv->ao_cmd_running) {
 		devpriv->ao_cmd_running = 1;
@@ -1030,7 +1026,6 @@ static int usbdux_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	struct usbdux_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	int ret = -EBUSY;
-	int i;
 
 	down(&devpriv->sem);
 
@@ -1040,9 +1035,6 @@ static int usbdux_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	/* set current channel of the running acquisition to zero */
 	s->async->cur_chan = 0;
 
-	for (i = 0; i < cmd->chanlist_len; ++i)
-		devpriv->ao_chanlist[i] = CR_CHAN(cmd->chanlist[i]);
-
 	/* we count in steps of 1ms (125us) */
 	/* 125us mode not used yet */
 	if (0) {		/* (devpriv->high_speed) */
@@ -1074,10 +1066,8 @@ static int usbdux_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 			/* data arrives as one packet */
 			devpriv->ao_sample_count = cmd->stop_arg;
 		}
-		devpriv->ao_continous = 0;
 	} else {
 		/* continous acquisition */
-		devpriv->ao_continous = 1;
 		devpriv->ao_sample_count = 0;
 	}
 
diff --git a/drivers/staging/comedi/drivers/usbduxfast.c b/drivers/staging/comedi/drivers/usbduxfast.c
index d6fae11ee4e..85f9dcf5940 100644
--- a/drivers/staging/comedi/drivers/usbduxfast.c
+++ b/drivers/staging/comedi/drivers/usbduxfast.c
@@ -154,7 +154,6 @@ struct usbduxfast_private {
 	uint8_t *duxbuf;
 	int8_t *inbuf;
 	short int ai_cmd_running;	/* asynchronous command is running */
-	short int ai_continous;	/* continous acquisition */
 	long int ai_sample_count;	/* number of samples to acquire */
 	int ignore;		/* counter which ignores the first
 				   buffers */
@@ -239,6 +238,7 @@ static void usbduxfast_ai_interrupt(struct urb *urb)
 	struct comedi_device *dev = urb->context;
 	struct comedi_subdevice *s = dev->read_subdev;
 	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
 	struct usb_device *usb = comedi_to_usb_dev(dev);
 	struct usbduxfast_private *devpriv = dev->private;
 	int n, err;
@@ -285,7 +285,7 @@ static void usbduxfast_ai_interrupt(struct urb *urb)
 	}
 
 	if (!devpriv->ignore) {
-		if (!devpriv->ai_continous) {
+		if (cmd->stop_src == TRIG_COUNT) {
 			/* not continuous, fixed number of samples */
 			n = urb->actual_length / sizeof(uint16_t);
 			if (unlikely(devpriv->ai_sample_count < n)) {
@@ -398,8 +398,7 @@ static int usbduxfast_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	if (cmd->start_src == TRIG_NOW)
-		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
 
 	if (!cmd->chanlist_len)
 		err |= -EINVAL;
@@ -451,21 +450,20 @@ static int usbduxfast_ai_cmdtest(struct comedi_device *dev,
 
 static int usbduxfast_ai_inttrig(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
-				 unsigned int trignum)
+				 unsigned int trig_num)
 {
 	struct usbduxfast_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	int ret;
 
 	if (!devpriv)
 		return -EFAULT;
 
+	if (trig_num != cmd->start_arg)
+		return -EINVAL;
+
 	down(&devpriv->sem);
 
-	if (trignum != 0) {
-		dev_err(dev->class_dev, "invalid trignum\n");
-		up(&devpriv->sem);
-		return -EINVAL;
-	}
 	if (!devpriv->ai_cmd_running) {
 		devpriv->ai_cmd_running = 1;
 		ret = usbduxfast_submit_urb(dev);
@@ -811,20 +809,11 @@ static int usbduxfast_ai_cmd(struct comedi_device *dev,
 		up(&devpriv->sem);
 		return result;
 	}
-	if (cmd->stop_src == TRIG_COUNT) {
+
+	if (cmd->stop_src == TRIG_COUNT)
 		devpriv->ai_sample_count = cmd->stop_arg * cmd->scan_end_arg;
-		if (devpriv->ai_sample_count < 1) {
-			dev_err(dev->class_dev,
-				"(cmd->stop_arg)*(cmd->scan_end_arg)<1, aborting\n");
-			up(&devpriv->sem);
-			return -EFAULT;
-		}
-		devpriv->ai_continous = 0;
-	} else {
-		/* continous acquisition */
-		devpriv->ai_continous = 1;
+	else	/* TRIG_NONE */
 		devpriv->ai_sample_count = 0;
-	}
 
 	if ((cmd->start_src == TRIG_NOW) || (cmd->start_src == TRIG_EXT)) {
 		/* enable this acquisition operation */
@@ -837,12 +826,7 @@ static int usbduxfast_ai_cmd(struct comedi_device *dev,
 			return ret;
 		}
 		s->async->inttrig = NULL;
-	} else {
-		/*
-		 * TRIG_INT
-		 * don't enable the acquision operation
-		 * wait for an internal signal
-		 */
+	} else {	/* TRIG_INT */
 		s->async->inttrig = usbduxfast_ai_inttrig;
 	}
 	up(&devpriv->sem);
diff --git a/drivers/staging/comedi/drivers/usbduxsigma.c b/drivers/staging/comedi/drivers/usbduxsigma.c
index 88c60b6020c..ccc3ef7ba55 100644
--- a/drivers/staging/comedi/drivers/usbduxsigma.c
+++ b/drivers/staging/comedi/drivers/usbduxsigma.c
@@ -161,14 +161,11 @@ struct usbduxsigma_private {
 	/* input buffer for single insn */
 	uint8_t *insn_buf;
 
-	uint8_t ao_chanlist[USBDUXSIGMA_NUM_AO_CHAN];
 	unsigned int ao_readback[USBDUXSIGMA_NUM_AO_CHAN];
 
 	unsigned high_speed:1;
 	unsigned ai_cmd_running:1;
-	unsigned ai_continuous:1;
 	unsigned ao_cmd_running:1;
-	unsigned ao_continuous:1;
 	unsigned pwm_cmd_running:1;
 
 	/* number of samples to acquire */
@@ -223,6 +220,7 @@ static void usbduxsigma_ai_urb_complete(struct urb *urb)
 	struct comedi_device *dev = urb->context;
 	struct usbduxsigma_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int dio_state;
 	uint32_t val;
 	int ret;
@@ -301,7 +299,7 @@ static void usbduxsigma_ai_urb_complete(struct urb *urb)
 	/* timer zero, transfer measurements to comedi */
 	devpriv->ai_counter = devpriv->ai_timer;
 
-	if (!devpriv->ai_continuous) {
+	if (cmd->stop_src == TRIG_COUNT) {
 		/* not continuous, fixed number of samples */
 		devpriv->ai_sample_count--;
 		if (devpriv->ai_sample_count < 0) {
@@ -314,7 +312,7 @@ static void usbduxsigma_ai_urb_complete(struct urb *urb)
 	}
 
 	/* get the data from the USB bus and hand it over to comedi */
-	for (i = 0; i < s->async->cmd.chanlist_len; i++) {
+	for (i = 0; i < cmd->chanlist_len; i++) {
 		/* transfer data, note first byte is the DIO state */
 		val = be32_to_cpu(devpriv->in_buf[i+1]);
 		val &= 0x00ffffff;	/* strip status byte */
@@ -360,8 +358,8 @@ static void usbduxsigma_ao_urb_complete(struct urb *urb)
 	struct comedi_device *dev = urb->context;
 	struct usbduxsigma_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->write_subdev;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	uint8_t *datap;
-	int len;
 	int ret;
 	int i;
 
@@ -403,7 +401,7 @@ static void usbduxsigma_ao_urb_complete(struct urb *urb)
 		/* timer zero, transfer from comedi */
 		devpriv->ao_counter = devpriv->ao_timer;
 
-		if (!devpriv->ao_continuous) {
+		if (cmd->stop_src == TRIG_COUNT) {
 			/* not continuous, fixed number of samples */
 			devpriv->ao_sample_count--;
 			if (devpriv->ao_sample_count < 0) {
@@ -417,13 +415,12 @@ static void usbduxsigma_ao_urb_complete(struct urb *urb)
 
 		/* transmit data to the USB bus */
 		datap = urb->transfer_buffer;
-		len = s->async->cmd.chanlist_len;
-		*datap++ = len;
-		for (i = 0; i < len; i++) {
-			unsigned int chan = devpriv->ao_chanlist[i];
+		*datap++ = cmd->chanlist_len;
+		for (i = 0; i < cmd->chanlist_len; i++) {
+			unsigned int chan = CR_CHAN(cmd->chanlist[i]);
 			unsigned short val;
 
-			ret = comedi_buf_get(s->async, &val);
+			ret = comedi_buf_get(s, &val);
 			if (ret < 0) {
 				dev_err(dev->class_dev, "buffer underflow\n");
 				s->async->events |= (COMEDI_CB_EOA |
@@ -596,10 +593,8 @@ static int usbduxsigma_ai_cmdtest(struct comedi_device *dev,
 	if (cmd->stop_src == TRIG_COUNT) {
 		/* data arrives as one packet */
 		devpriv->ai_sample_count = cmd->stop_arg;
-		devpriv->ai_continuous = 0;
 	} else {
 		/* continuous acquisition */
-		devpriv->ai_continuous = 1;
 		devpriv->ai_sample_count = 0;
 	}
 
@@ -663,12 +658,13 @@ static int usbduxsigma_receive_cmd(struct comedi_device *dev, int command)
 
 static int usbduxsigma_ai_inttrig(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
-				  unsigned int trignum)
+				  unsigned int trig_num)
 {
 	struct usbduxsigma_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	int ret;
 
-	if (trignum != 0)
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	down(&devpriv->sem);
@@ -738,7 +734,6 @@ static int usbduxsigma_ai_cmd(struct comedi_device *dev,
 		}
 		s->async->inttrig = NULL;
 	} else {	/* TRIG_INT */
-		/* wait for an internal signal and submit the urbs later */
 		s->async->inttrig = usbduxsigma_ai_inttrig;
 	}
 
@@ -856,12 +851,13 @@ static int usbduxsigma_ao_insn_write(struct comedi_device *dev,
 
 static int usbduxsigma_ao_inttrig(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
-				  unsigned int trignum)
+				  unsigned int trig_num)
 {
 	struct usbduxsigma_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	int ret;
 
-	if (trignum != 0)
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	down(&devpriv->sem);
@@ -985,10 +981,8 @@ static int usbduxsigma_ao_cmdtest(struct comedi_device *dev,
 			 */
 			devpriv->ao_sample_count = cmd->stop_arg;
 		}
-		devpriv->ao_continuous = 0;
 	} else {
 		/* continuous acquisition */
-		devpriv->ao_continuous = 1;
 		devpriv->ao_sample_count = 0;
 	}
 
@@ -1004,14 +998,11 @@ static int usbduxsigma_ao_cmd(struct comedi_device *dev,
 	struct usbduxsigma_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	int ret;
-	int i;
 
 	down(&devpriv->sem);
 
 	/* set current channel of the running acquisition to zero */
 	s->async->cur_chan = 0;
-	for (i = 0; i < cmd->chanlist_len; ++i)
-		devpriv->ao_chanlist[i] = CR_CHAN(cmd->chanlist[i]);
 
 	devpriv->ao_counter = devpriv->ao_timer;
 
@@ -1027,7 +1018,6 @@ static int usbduxsigma_ao_cmd(struct comedi_device *dev,
 		}
 		s->async->inttrig = NULL;
 	} else {	/* TRIG_INT */
-		/* wait for an internal signal and submit the urbs later */
 		s->async->inttrig = usbduxsigma_ao_inttrig;
 	}
 
diff --git a/drivers/staging/crystalhd/crystalhd_lnx.c b/drivers/staging/crystalhd/crystalhd_lnx.c
index 20be9571990..e6fb331c673 100644
--- a/drivers/staging/crystalhd/crystalhd_lnx.c
+++ b/drivers/staging/crystalhd/crystalhd_lnx.c
@@ -28,7 +28,7 @@ static struct crystalhd_adp *g_adp_info;
 
 static irqreturn_t chd_dec_isr(int irq, void *arg)
 {
-	struct crystalhd_adp *adp = (struct crystalhd_adp *) arg;
+	struct crystalhd_adp *adp = arg;
 	int rc = 0;
 	if (adp)
 		rc = crystalhd_cmd_interrupt(&adp->cmds);
@@ -112,7 +112,7 @@ static void chd_dec_free_iodata(struct crystalhd_adp *adp,
 }
 
 static inline int crystalhd_user_data(void __user *ud, void *dr,
-			 int size, int set)
+				      int size, int set)
 {
 	int rc;
 
@@ -135,7 +135,8 @@ static inline int crystalhd_user_data(void __user *ud, void *dr,
 }
 
 static int chd_dec_fetch_cdata(struct crystalhd_adp *adp,
-	 struct crystalhd_ioctl_data *io, uint32_t m_sz, unsigned long ua)
+			       struct crystalhd_ioctl_data *io, uint32_t m_sz,
+			       unsigned long ua)
 {
 	unsigned long ua_off;
 	int rc = 0;
@@ -154,7 +155,7 @@ static int chd_dec_fetch_cdata(struct crystalhd_adp *adp,
 	io->add_cdata_sz = m_sz;
 	ua_off = ua + sizeof(io->udata);
 	rc = crystalhd_user_data((void __user *)ua_off, io->add_cdata,
-			io->add_cdata_sz, 0);
+				 io->add_cdata_sz, 0);
 	if (rc) {
 		BCMLOG_ERR("failed to pull add_cdata sz:%x ua_off:%x\n",
 			   io->add_cdata_sz, (unsigned int)ua_off);
@@ -167,7 +168,8 @@ static int chd_dec_fetch_cdata(struct crystalhd_adp *adp,
 }
 
 static int chd_dec_release_cdata(struct crystalhd_adp *adp,
-			 struct crystalhd_ioctl_data *io, unsigned long ua)
+				 struct crystalhd_ioctl_data *io,
+				 unsigned long ua)
 {
 	unsigned long ua_off;
 	int rc;
@@ -180,7 +182,7 @@ static int chd_dec_release_cdata(struct crystalhd_adp *adp,
 	if (io->cmd != BCM_IOC_FW_DOWNLOAD) {
 		ua_off = ua + sizeof(io->udata);
 		rc = crystalhd_user_data((void __user *)ua_off, io->add_cdata,
-					io->add_cdata_sz, 1);
+					 io->add_cdata_sz, 1);
 		if (rc) {
 			BCMLOG_ERR(
 				"failed to push add_cdata sz:%x ua_off:%x\n",
@@ -210,7 +212,7 @@ static int chd_dec_proc_user_data(struct crystalhd_adp *adp,
 	}
 
 	rc = crystalhd_user_data((void __user *)ua, &io->udata,
-			sizeof(io->udata), set);
+				 sizeof(io->udata), set);
 	if (rc) {
 		BCMLOG_ERR("failed to %s iodata\n", (set ? "set" : "get"));
 		return rc;
@@ -382,7 +384,7 @@ static int chd_dec_init_chdev(struct crystalhd_adp *adp)
 	}
 
 	dev = device_create(crystalhd_class, NULL,
-			 MKDEV(adp->chd_dec_major, 0), NULL, "crystalhd");
+			    MKDEV(adp->chd_dec_major, 0), NULL, "crystalhd");
 	if (IS_ERR(dev)) {
 		rc = PTR_ERR(dev);
 		BCMLOG_ERR("failed to create device\n");
@@ -397,8 +399,7 @@ static int chd_dec_init_chdev(struct crystalhd_adp *adp)
 
 	/* Allocate general purpose ioctl pool. */
 	for (i = 0; i < CHD_IODATA_POOL_SZ; i++) {
-		temp = kzalloc(sizeof(struct crystalhd_ioctl_data),
-					 GFP_KERNEL);
+		temp = kzalloc(sizeof(*temp), GFP_KERNEL);
 		if (!temp) {
 			BCMLOG_ERR("ioctl data pool kzalloc failed\n");
 			rc = -ENOMEM;
@@ -549,11 +550,11 @@ static int chd_dec_pci_probe(struct pci_dev *pdev,
 	enum BC_STATUS sts = BC_STS_SUCCESS;
 
 	BCMLOG(BCMLOG_DBG,
-		"PCI_INFO: Vendor:0x%04x Device:0x%04x s_vendor:0x%04x s_device: 0x%04x\n",
-		pdev->vendor, pdev->device, pdev->subsystem_vendor,
-		pdev->subsystem_device);
+	       "PCI_INFO: Vendor:0x%04x Device:0x%04x s_vendor:0x%04x s_device: 0x%04x\n",
+	       pdev->vendor, pdev->device, pdev->subsystem_vendor,
+	       pdev->subsystem_device);
 
-	pinfo = kzalloc(sizeof(struct crystalhd_adp), GFP_KERNEL);
+	pinfo = kzalloc(sizeof(*pinfo), GFP_KERNEL);
 	if (!pinfo) {
 		BCMLOG_ERR("Failed to allocate memory\n");
 		return -ENOMEM;
diff --git a/drivers/staging/cxt1e1/functions.c b/drivers/staging/cxt1e1/functions.c
index ee9d39bbd25..65b6fc36edf 100644
--- a/drivers/staging/cxt1e1/functions.c
+++ b/drivers/staging/cxt1e1/functions.c
@@ -24,24 +24,6 @@
 #include "libsbew.h"
 #include "pmcc4.h"
 
-#if defined(CONFIG_SBE_HDLC_V7) || defined(CONFIG_SBE_WAN256T3_HDLC_V7) || \
-defined(CONFIG_SBE_HDLC_V7_MODULE) || \
-defined(CONFIG_SBE_WAN256T3_HDLC_V7_MODULE)
-#define _v7_hdlc_  1
-#else
-#define _v7_hdlc_  0
-#endif
-
-#if _v7_hdlc_
-#define V7(x) (x ## _v7)
-extern int  hdlc_netif_rx_v7(hdlc_device *, struct sk_buff *);
-extern int  register_hdlc_device_v7(hdlc_device *);
-extern int  unregister_hdlc_device_v7(hdlc_device *);
-
-#else
-#define V7(x) x
-#endif
-
 
 #ifndef USE_MAX_INT_DELAY
 static int  dummy = 0;
diff --git a/drivers/staging/cxt1e1/hwprobe.c b/drivers/staging/cxt1e1/hwprobe.c
index 9b4198b1e63..4fa27c8931b 100644
--- a/drivers/staging/cxt1e1/hwprobe.c
+++ b/drivers/staging/cxt1e1/hwprobe.c
@@ -159,8 +159,8 @@ prep_hdw_info(void)
 		hi->ndev = NULL;
 		hi->addr[0] = 0L;
 		hi->addr[1] = 0L;
-		hi->addr_mapped[0] = 0L;
-		hi->addr_mapped[1] = 0L;
+		hi->addr_mapped[0] = NULL;
+		hi->addr_mapped[1] = NULL;
 	}
 }
 
@@ -174,14 +174,14 @@ cleanup_ioremap(void)
 		if (hi->pci_slot == 0xff)
 			break;
 		if (hi->addr_mapped[0]) {
-			iounmap((void *)(hi->addr_mapped[0]));
+			iounmap(hi->addr_mapped[0]);
 			release_mem_region((long) hi->addr[0], hi->len[0]);
-			hi->addr_mapped[0] = 0;
+			hi->addr_mapped[0] = NULL;
 		}
 		if (hi->addr_mapped[1]) {
-			iounmap((void *)(hi->addr_mapped[1]));
+			iounmap(hi->addr_mapped[1]);
 			release_mem_region((long) hi->addr[1], hi->len[1]);
-			hi->addr_mapped[1] = 0;
+			hi->addr_mapped[1] = NULL;
 		}
 	}
 }
@@ -205,7 +205,7 @@ cleanup_devs(void)
 #ifdef CONFIG_SBE_PMCC4_NCOMM
 		free_irq(hi->pdev[1]->irq, hi->ndev);
 #endif
-		OS_kfree(hi->ndev);
+		kfree(hi->ndev);
 	}
 }
 
@@ -329,7 +329,7 @@ c4hw_attach_all(void)
 				return -ENOMEM;
 			}
 
-			hi->addr_mapped[j] = (unsigned long)ioremap(hi->addr[j], hi->len[j]);
+			hi->addr_mapped[j] = ioremap(hi->addr[j], hi->len[j]);
 			if (!hi->addr_mapped[j]) {
 				pr_warning("%s: ioremap fails, addr=0x%lx, len=0x%lx ?\n",
 					   hi->devname, hi->addr[j], hi->len[j]);
diff --git a/drivers/staging/cxt1e1/linux.c b/drivers/staging/cxt1e1/linux.c
index b02f5ade666..09f3d5ca75a 100644
--- a/drivers/staging/cxt1e1/linux.c
+++ b/drivers/staging/cxt1e1/linux.c
@@ -60,7 +60,6 @@ status_t    c4_chan_work_init(mpi_t *, mch_t *);
 void        musycc_wq_chan_restart(void *);
 status_t __init c4_init(ci_t *, u_char *, u_char *);
 status_t __init c4_init2(ci_t *);
-ci_t       *__init c4_new(void *);
 int __init  c4hw_attach_all(void);
 void __init hdw_sn_get(hdw_info_t *, int);
 
@@ -84,23 +83,6 @@ int         musycc_start_xmit(ci_t *, int, void *);
 extern ci_t *CI;
 extern struct s_hdw_info hdw_info[];
 
-#if defined(CONFIG_SBE_HDLC_V7) || defined(CONFIG_SBE_WAN256T3_HDLC_V7) || \
-	defined(CONFIG_SBE_HDLC_V7_MODULE) || defined(CONFIG_SBE_WAN256T3_HDLC_V7_MODULE)
-#define _v7_hdlc_  1
-#else
-#define _v7_hdlc_  0
-#endif
-
-#if _v7_hdlc_
-#define V7(x) (x ## _v7)
-extern int  hdlc_netif_rx_v7(hdlc_device *, struct sk_buff *);
-extern int  register_hdlc_device_v7(hdlc_device *);
-extern int  unregister_hdlc_device_v7(hdlc_device *);
-
-#else
-#define V7(x) x
-#endif
-
 int         error_flag;         /* module load error reporting */
 int         cxt1e1_log_level = LOG_ERROR;
 static int  log_level_default = LOG_ERROR;
@@ -418,7 +400,7 @@ create_chan(struct net_device *ndev, ci_t *ci,
 		struct c4_priv *priv;
 
 		/* allocate then fill in private data structure */
-		priv = OS_kmalloc(sizeof(struct c4_priv));
+		priv = kzalloc(sizeof(struct c4_priv), GFP_KERNEL);
 		if (!priv) {
 			pr_warning("%s: no memory for net_device !\n",
 				   ci->devname);
@@ -428,7 +410,7 @@ create_chan(struct net_device *ndev, ci_t *ci,
 		if (!dev) {
 			pr_warning("%s: no memory for hdlc_device !\n",
 				   ci->devname);
-			OS_kfree(priv);
+			kfree(priv);
 			return NULL;
 		}
 		priv->ci = ci;
@@ -972,8 +954,8 @@ c4_add_dev(hdw_info_t *hi, int brdno, unsigned long f0, unsigned long f1,
 
 	if (register_netdev(ndev) ||
 		(c4_init(ci, (u_char *) f0, (u_char *) f1) != SBE_DRVR_SUCCESS)) {
-		OS_kfree(netdev_priv(ndev));
-		OS_kfree(ndev);
+		kfree(netdev_priv(ndev));
+		kfree(ndev);
 		error_flag = -ENODEV;
 		return NULL;
 	}
@@ -998,8 +980,8 @@ c4_add_dev(hdw_info_t *hi, int brdno, unsigned long f0, unsigned long f1,
 		pr_warning("%s: MUSYCC could not get irq: %d\n",
 			   ndev->name, irq0);
 		unregister_netdev(ndev);
-		OS_kfree(netdev_priv(ndev));
-		OS_kfree(ndev);
+		kfree(netdev_priv(ndev));
+		kfree(ndev);
 		error_flag = -EIO;
 		return NULL;
 	}
@@ -1008,8 +990,8 @@ c4_add_dev(hdw_info_t *hi, int brdno, unsigned long f0, unsigned long f1,
 		pr_warning("%s: EBUS could not get irq: %d\n", hi->devname, irq1);
 		unregister_netdev(ndev);
 		free_irq(irq0, ndev);
-		OS_kfree(netdev_priv(ndev));
-		OS_kfree(ndev);
+		kfree(netdev_priv(ndev));
+		kfree(ndev);
 		error_flag = -EIO;
 		return NULL;
 	}
@@ -1068,8 +1050,8 @@ c4_add_dev(hdw_info_t *hi, int brdno, unsigned long f0, unsigned long f1,
 		unregister_netdev(ndev);
 		free_irq(irq1, ndev);
 		free_irq(irq0, ndev);
-		OS_kfree(netdev_priv(ndev));
-		OS_kfree(ndev);
+		kfree(netdev_priv(ndev));
+		kfree(ndev);
 		/* failure, error_flag is set */
 		return NULL;
 	}
diff --git a/drivers/staging/cxt1e1/musycc.c b/drivers/staging/cxt1e1/musycc.c
index 7b4f6f2108e..0bcbd8a3fc8 100644
--- a/drivers/staging/cxt1e1/musycc.c
+++ b/drivers/staging/cxt1e1/musycc.c
@@ -1,5 +1,5 @@
-static unsigned int max_intcnt = 0;
-static unsigned int max_bh = 0;
+static unsigned int max_intcnt;
+static unsigned int max_bh;
 
 /*-----------------------------------------------------------------------------
  * musycc.c -
@@ -64,132 +64,134 @@ void        musycc_update_timeslots(mpi_t *);
 
 /*******************************************************************/
 
-#if 1
 static int
 musycc_dump_rxbuffer_ring(mch_t *ch, int lockit)
 {
-    struct mdesc *m;
-    unsigned long flags = 0;
+	struct mdesc *m;
+	unsigned long flags = 0;
 
-    u_int32_t status;
-    int         n;
+	u_int32_t status;
+	int         n;
 
-    if (lockit)
-	spin_lock_irqsave(&ch->ch_rxlock, flags);
-    if (ch->rxd_num == 0)
-	pr_info("  ZERO receive buffers allocated for this channel.");
-    else {
-	FLUSH_MEM_READ();
-	m = &ch->mdr[ch->rxix_irq_srv];
-	for (n = ch->rxd_num; n; n--) {
-	    status = le32_to_cpu(m->status);
-	    {
-		pr_info("%c  %08lx[%2d]: sts %08x (%c%c%c%c:%d.) Data [%08x] Next [%08x]\n",
-			(m == &ch->mdr[ch->rxix_irq_srv]) ? 'F' : ' ',
-			(unsigned long) m, n,
-			status,
-			m->data ? (status & HOST_RX_OWNED ? 'H' : 'M') : '-',
-			status & POLL_DISABLED ? 'P' : '-',
-			status & EOBIRQ_ENABLE ? 'b' : '-',
-			status & EOMIRQ_ENABLE ? 'm' : '-',
-			status & LENGTH_MASK,
-			le32_to_cpu(m->data), le32_to_cpu(m->next));
 #ifdef RLD_DUMP_BUFDATA
-		{
-		    u_int32_t  *dp;
-		    int         len = status & LENGTH_MASK;
+	u_int32_t *dp;
+	int len = 0;
+#endif
+	if (lockit)
+		spin_lock_irqsave(&ch->ch_rxlock, flags);
+	if (ch->rxd_num == 0)
+		pr_info("  ZERO receive buffers allocated for this channel.");
+	else {
+		FLUSH_MEM_READ();
+		m = &ch->mdr[ch->rxix_irq_srv];
+		for (n = ch->rxd_num; n; n--) {
+			status = le32_to_cpu(m->status);
+			pr_info("%c  %08lx[%2d]: sts %08x (%c%c%c%c:%d.) Data [%08x] Next [%08x]\n",
+				(m == &ch->mdr[ch->rxix_irq_srv]) ? 'F' : ' ',
+				(unsigned long) m, n,
+				status,
+				m->data ? (status & HOST_RX_OWNED ? 'H' : 'M') : '-',
+				status & POLL_DISABLED ? 'P' : '-',
+				status & EOBIRQ_ENABLE ? 'b' : '-',
+				status & EOMIRQ_ENABLE ? 'm' : '-',
+				status & LENGTH_MASK,
+				le32_to_cpu(m->data), le32_to_cpu(m->next));
+#ifdef RLD_DUMP_BUFDATA
+				len = status & LENGTH_MASK;
 
 #if 1
-		    if (m->data && (status & HOST_RX_OWNED))
+				if (m->data && (status & HOST_RX_OWNED))
 #else
-		    if (m->data)    /* always dump regardless of valid RX
-				     * data */
+				/* always dump regardless of valid RX data */
+				if (m->data)
 #endif
-		    {
-			dp = (u_int32_t *) OS_phystov((void *) (le32_to_cpu(m->data)));
-			if (len >= 0x10)
-			    pr_info("    %x[%x]: %08X %08X %08X %08x\n", (u_int32_t) dp, len,
-				    *dp, *(dp + 1), *(dp + 2), *(dp + 3));
-			else if (len >= 0x08)
-			    pr_info("    %x[%x]: %08X %08X\n", (u_int32_t) dp, len,
-				    *dp, *(dp + 1));
-			else
-			    pr_info("    %x[%x]: %08X\n", (u_int32_t) dp, len, *dp);
-		    }
-		}
+				{
+					dp = (u_int32_t *)OS_phystov((void *)(le32_to_cpu(m->data)));
+					if (len >= 0x10)
+						pr_info("    %x[%x]: %08X %08X %08X %08x\n",
+							(u_int32_t)dp, len,
+							*dp, *(dp + 1),
+							*(dp + 2), *(dp + 3));
+					else if (len >= 0x08)
+						pr_info("    %x[%x]: %08X %08X\n",
+							(u_int32_t)dp, len,
+							*dp, *(dp + 1));
+					else
+						pr_info("    %x[%x]: %08X\n",
+							(u_int32_t)dp,
+							len, *dp);
+				}
 #endif
-	    }
-	    m = m->snext;
+			m = m->snext;
+		}
 	}
-    }                               /* -for- */
-    pr_info("\n");
+	pr_info("\n");
 
-    if (lockit)
-	spin_unlock_irqrestore(&ch->ch_rxlock, flags);
-    return 0;
+	if (lockit)
+		spin_unlock_irqrestore(&ch->ch_rxlock, flags);
+	return 0;
 }
-#endif
 
-#if 1
 static int
 musycc_dump_txbuffer_ring(mch_t *ch, int lockit)
 {
-    struct mdesc *m;
-    unsigned long flags = 0;
-    u_int32_t   status;
-    int         n;
-
-    if (lockit)
-	spin_lock_irqsave(&ch->ch_txlock, flags);
-    if (ch->txd_num == 0)
-	pr_info("  ZERO transmit buffers allocated for this channel.");
-    else {
-	FLUSH_MEM_READ();
-	m = ch->txd_irq_srv;
-	for (n = ch->txd_num; n; n--) {
-	    status = le32_to_cpu(m->status);
-	    {
-		pr_info("%c%c %08lx[%2d]: sts %08x (%c%c%c%c:%d.) Data [%08x] Next [%08x]\n",
-			(m == ch->txd_usr_add) ? 'F' : ' ',
-			(m == ch->txd_irq_srv) ? 'L' : ' ',
-			(unsigned long) m, n,
-			status,
-		     m->data ? (status & MUSYCC_TX_OWNED ? 'M' : 'H') : '-',
-			status & POLL_DISABLED ? 'P' : '-',
-			status & EOBIRQ_ENABLE ? 'b' : '-',
-			status & EOMIRQ_ENABLE ? 'm' : '-',
-			status & LENGTH_MASK,
-			le32_to_cpu(m->data), le32_to_cpu(m->next));
+	struct mdesc *m;
+	unsigned long flags = 0;
+	u_int32_t   status;
+	int         n;
 #ifdef RLD_DUMP_BUFDATA
-		{
-		    u_int32_t  *dp;
-		    int         len = status & LENGTH_MASK;
-
-		    if (m->data) {
-			dp = (u_int32_t *) OS_phystov((void *) (le32_to_cpu(m->data)));
-			if (len >= 0x10)
-			    pr_info("    %x[%x]: %08X %08X %08X %08x\n", (u_int32_t) dp, len,
-				    *dp, *(dp + 1), *(dp + 2), *(dp + 3));
-			else if (len >= 0x08)
-			    pr_info("    %x[%x]: %08X %08X\n", (u_int32_t) dp, len,
-				    *dp, *(dp + 1));
-			else
-			    pr_info("    %x[%x]: %08X\n", (u_int32_t) dp, len, *dp);
-		    }
-		}
+	u_int32_t *dp;
+	int len = 0;
 #endif
-	    }
-	    m = m->snext;
-	}
-    }                               /* -for- */
-    pr_info("\n");
 
-    if (lockit)
-	spin_unlock_irqrestore(&ch->ch_txlock, flags);
-    return 0;
-}
+	if (lockit)
+		spin_lock_irqsave(&ch->ch_txlock, flags);
+	if (ch->txd_num == 0)
+		pr_info("  ZERO transmit buffers allocated for this channel.");
+	else {
+		FLUSH_MEM_READ();
+		m = ch->txd_irq_srv;
+		for (n = ch->txd_num; n; n--) {
+			status = le32_to_cpu(m->status);
+			pr_info("%c%c %08lx[%2d]: sts %08x (%c%c%c%c:%d.) Data [%08x] Next [%08x]\n",
+				(m == ch->txd_usr_add) ? 'F' : ' ',
+				(m == ch->txd_irq_srv) ? 'L' : ' ',
+				(unsigned long) m, n,
+				status,
+				m->data ? (status & MUSYCC_TX_OWNED ? 'M' : 'H') : '-',
+				status & POLL_DISABLED ? 'P' : '-',
+				status & EOBIRQ_ENABLE ? 'b' : '-',
+				status & EOMIRQ_ENABLE ? 'm' : '-',
+				status & LENGTH_MASK,
+				le32_to_cpu(m->data), le32_to_cpu(m->next));
+#ifdef RLD_DUMP_BUFDATA
+			len = status & LENGTH_MASK;
+
+			if (m->data) {
+				dp = (u_int32_t *)OS_phystov((void *)(le32_to_cpu(m->data)));
+				if (len >= 0x10)
+					pr_info("    %x[%x]: %08X %08X %08X %08x\n",
+						(u_int32_t) dp, len,
+						*dp, *(dp + 1),
+						*(dp + 2), *(dp + 3));
+				else if (len >= 0x08)
+					pr_info("    %x[%x]: %08X %08X\n",
+						(u_int32_t)dp, len,
+						*dp, *(dp + 1));
+				else
+					pr_info("    %x[%x]: %08X\n",
+						(u_int32_t)dp, len, *dp);
+			}
 #endif
+			m = m->snext;
+		}
+	}                               /* -for- */
+	pr_info("\n");
 
+	if (lockit)
+		spin_unlock_irqrestore(&ch->ch_txlock, flags);
+	return 0;
+}
 
 /*
  * The following supports a backdoor debug facility which can be used to
@@ -199,12 +201,11 @@ musycc_dump_txbuffer_ring(mch_t *ch, int lockit)
 status_t
 musycc_dump_ring(ci_t *ci, unsigned int chan)
 {
-    mch_t      *ch;
+	mch_t      *ch;
+	int bh;
 
-    if (chan >= MAX_CHANS_USED)
-	return SBE_DRVR_FAIL;       /* E2BIG */
-    {
-	int         bh;
+	if (chan >= MAX_CHANS_USED)
+		return SBE_DRVR_FAIL;       /* E2BIG */
 
 	bh = atomic_read(&ci->bh_pending);
 	pr_info(">> bh_pend %d [%d] ihead %d itail %d [%d] th_cnt %d bh_cnt %d wdcnt %d note %d\n",
@@ -214,40 +215,43 @@ musycc_dump_ring(ci_t *ci, unsigned int chan)
 		ci->wdcount, ci->wd_notify);
 	max_bh = 0;                 /* reset counter */
 	max_intcnt = 0;             /* reset counter */
-    }
-
-    ch = sd_find_chan(dummy, chan);
-    if (!ch) {
-	pr_info(">> musycc_dump_ring: channel %d not up.\n", chan);
-	return ENOENT;
-    }
-    pr_info(">> CI %p CHANNEL %3d @ %p: state %x status/p %x/%x\n", ci, chan, ch, ch->state,
-	    ch->status, ch->p.status);
-    pr_info("--------------------------------\nTX Buffer Ring - Channel %d, txd_num %d. (bd/ch pend %d %d), TXD required %d, txpkt %lu\n",
-	    chan, ch->txd_num,
-	    (u_int32_t) atomic_read(&ci->tx_pending), (u_int32_t) atomic_read(&ch->tx_pending), ch->txd_required, ch->s.tx_packets);
-    pr_info("++ User 0x%p IRQ_SRV 0x%p USR_ADD 0x%p QStopped %x, start_tx %x tx_full %d txd_free %d mode %x\n",
-	    ch->user, ch->txd_irq_srv, ch->txd_usr_add,
-	    sd_queue_stopped(ch->user),
-	    ch->ch_start_tx, ch->tx_full, ch->txd_free, ch->p.chan_mode);
-    musycc_dump_txbuffer_ring(ch, 1);
-    pr_info("RX Buffer Ring - Channel %d, rxd_num %d. IRQ_SRV[%d] 0x%p, start_rx %x rxpkt %lu\n",
-	    chan, ch->rxd_num, ch->rxix_irq_srv,
-	    &ch->mdr[ch->rxix_irq_srv], ch->ch_start_rx, ch->s.rx_packets);
-    musycc_dump_rxbuffer_ring(ch, 1);
-
-    return SBE_DRVR_SUCCESS;
+
+	ch = sd_find_chan(dummy, chan);
+	if (!ch) {
+		pr_info(">> musycc_dump_ring: channel %d not up.\n", chan);
+		return ENOENT;
+	}
+	pr_info(">> CI %p CHANNEL %3d @ %p: state %x status/p %x/%x\n",
+		ci, chan, ch, ch->state,
+		ch->status, ch->p.status);
+	pr_info("--------------------------------\n");
+	pr_info("TX Buffer Ring - Channel %d, txd_num %d. (bd/ch pend %d %d), TXD required %d, txpkt %lu\n",
+		chan, ch->txd_num,
+		(u_int32_t)atomic_read(&ci->tx_pending),
+		(u_int32_t)atomic_read(&ch->tx_pending),
+		ch->txd_required, ch->s.tx_packets);
+	pr_info("++ User 0x%p IRQ_SRV 0x%p USR_ADD 0x%p QStopped %x, start_tx %x tx_full %d txd_free %d mode %x\n",
+		ch->user, ch->txd_irq_srv, ch->txd_usr_add,
+		sd_queue_stopped(ch->user),
+		ch->ch_start_tx, ch->tx_full, ch->txd_free, ch->p.chan_mode);
+	musycc_dump_txbuffer_ring(ch, 1);
+	pr_info("RX Buffer Ring - Channel %d, rxd_num %d. IRQ_SRV[%d] 0x%p, start_rx %x rxpkt %lu\n",
+		chan, ch->rxd_num, ch->rxix_irq_srv,
+		&ch->mdr[ch->rxix_irq_srv], ch->ch_start_rx, ch->s.rx_packets);
+	musycc_dump_rxbuffer_ring(ch, 1);
+
+	return SBE_DRVR_SUCCESS;
 }
 
 
 status_t
 musycc_dump_rings(ci_t *ci, unsigned int start_chan)
 {
-    unsigned int chan;
+	unsigned int chan;
 
-    for (chan = start_chan; chan < (start_chan + 5); chan++)
-	musycc_dump_ring(ci, chan);
-    return SBE_DRVR_SUCCESS;
+	for (chan = start_chan; chan < (start_chan + 5); chan++)
+		musycc_dump_ring(ci, chan);
+	return SBE_DRVR_SUCCESS;
 }
 
 
@@ -259,22 +263,22 @@ musycc_dump_rings(ci_t *ci, unsigned int start_chan)
 void
 musycc_init_mdt(mpi_t *pi)
 {
-    u_int32_t  *addr, cfg;
-    int         i;
-
-    /*
-     * This Idle Code insertion takes effect prior to channel's first
-     * transmitted  message.  After that, each message contains its own Idle
-     * Code information which is to be issued after the message is
-     * transmitted (Ref.MUSYCC 5.2.2.3: MCENBL bit in Group Configuration
-     * Descriptor).
-     */
-
-    addr = (u_int32_t *) ((u_long) pi->reg + MUSYCC_MDT_BASE03_ADDR);
-    cfg = CFG_CH_FLAG_7E << IDLE_CODE;
-
-    for (i = 0; i < 32; addr++, i++)
-	pci_write_32(addr, cfg);
+	u_int32_t  *addr, cfg;
+	int         i;
+
+	/*
+	 * This Idle Code insertion takes effect prior to channel's first
+	 * transmitted  message.  After that, each message contains its own Idle
+	 * Code information which is to be issued after the message is
+	 * transmitted (Ref.MUSYCC 5.2.2.3: MCENBL bit in Group Configuration
+	 * Descriptor).
+	 */
+
+	addr = (u_int32_t *) ((u_long) pi->reg + MUSYCC_MDT_BASE03_ADDR);
+	cfg = CFG_CH_FLAG_7E << IDLE_CODE;
+
+	for (i = 0; i < 32; addr++, i++)
+		pci_write_32(addr, cfg);
 }
 
 
@@ -283,44 +287,45 @@ musycc_init_mdt(mpi_t *pi)
 void
 musycc_update_tx_thp(mch_t *ch)
 {
-    struct mdesc *md;
-    unsigned long flags;
+	struct mdesc *md;
+	unsigned long flags;
 
-    spin_lock_irqsave(&ch->ch_txlock, flags);
-    while (1) {
-	md = ch->txd_irq_srv;
-	FLUSH_MEM_READ();
-	if (!md->data) {
-	    /* No MDs with buffers to process */
-	    spin_unlock_irqrestore(&ch->ch_txlock, flags);
-	    return;
+	spin_lock_irqsave(&ch->ch_txlock, flags);
+	while (1) {
+		md = ch->txd_irq_srv;
+		FLUSH_MEM_READ();
+		if (!md->data) {
+			/* No MDs with buffers to process */
+			spin_unlock_irqrestore(&ch->ch_txlock, flags);
+			return;
+		}
+		if ((le32_to_cpu(md->status)) & MUSYCC_TX_OWNED) {
+			/* this is the MD to restart TX with */
+			break;
+		}
+		/*
+		 * Otherwise, we have a valid, host-owned message descriptor which
+		 * has been successfully transmitted and whose buffer can be freed,
+		 * so... process this MD, it's owned by the host.  (This might give
+		 * as a new, updated txd_irq_srv.)
+		 */
+		musycc_bh_tx_eom(ch->up, ch->gchan);
 	}
-	if ((le32_to_cpu(md->status)) & MUSYCC_TX_OWNED) {
-	    /* this is the MD to restart TX with */
-	    break;
+	md = ch->txd_irq_srv;
+	ch->up->regram->thp[ch->gchan] = cpu_to_le32(OS_vtophys(md));
+	FLUSH_MEM_WRITE();
+
+	if (ch->tx_full) {
+		ch->tx_full = 0;
+		ch->txd_required = 0;
+		sd_enable_xmit(ch->user);  /* re-enable to catch flow controlled
+					    * channel */
 	}
-	/*
-	 * Otherwise, we have a valid, host-owned message descriptor which
-	 * has been successfully transmitted and whose buffer can be freed,
-	 * so... process this MD, it's owned by the host.  (This might give
-	 * as a new, updated txd_irq_srv.)
-	 */
-	musycc_bh_tx_eom(ch->up, ch->gchan);
-    }
-    md = ch->txd_irq_srv;
-    ch->up->regram->thp[ch->gchan] = cpu_to_le32(OS_vtophys(md));
-    FLUSH_MEM_WRITE();
-
-    if (ch->tx_full) {
-	ch->tx_full = 0;
-	ch->txd_required = 0;
-	sd_enable_xmit(ch->user);  /* re-enable to catch flow controlled
-				     * channel */
-    }
-    spin_unlock_irqrestore(&ch->ch_txlock, flags);
+	spin_unlock_irqrestore(&ch->ch_txlock, flags);
 
 #ifdef RLD_TRANS_DEBUG
-    pr_info("++ musycc_update_tx_thp[%d]: setting thp = %p, sts %x\n", ch->channum, md, md->status);
+	pr_info("++ musycc_update_tx_thp[%d]: setting thp = %p, sts %x\n",
+		ch->channum, md, md->status);
 #endif
 }
 
@@ -337,96 +342,88 @@ musycc_update_tx_thp(mch_t *ch)
 void
 musycc_wq_chan_restart(void *arg)      /* channel private structure */
 {
-    mch_t      *ch;
-    mpi_t      *pi;
-    struct mdesc *md;
-#if 0
-    unsigned long flags;
+	mch_t      *ch;
+	mpi_t      *pi;
+	struct mdesc *md;
+
+#if defined(RLD_TRANS_DEBUG) || defined(RLD_RXACT_DEBUG)
+	static int hereb4 = 7;
 #endif
 
-    ch = container_of(arg, struct c4_chan_info, ch_work);
-    pi = ch->up;
+	ch = container_of(arg, struct c4_chan_info, ch_work);
+	pi = ch->up;
 
 #ifdef RLD_TRANS_DEBUG
-    pr_info("wq_chan_restart[%d]: start_RT[%d/%d] status %x\n",
-	    ch->channum, ch->ch_start_rx, ch->ch_start_tx, ch->status);
+	pr_info("wq_chan_restart[%d]: start_RT[%d/%d] status %x\n",
+		ch->channum, ch->ch_start_rx, ch->ch_start_tx, ch->status);
 
 #endif
 
-    /**********************************/
-    /** check for RX restart request **/
-    /**********************************/
+	/**********************************/
+	/** check for RX restart request **/
+	/**********************************/
 
-    if ((ch->ch_start_rx) && (ch->status & RX_ENABLED)) {
+	if ((ch->ch_start_rx) && (ch->status & RX_ENABLED)) {
 
-	ch->ch_start_rx = 0;
+		ch->ch_start_rx = 0;
 #if defined(RLD_TRANS_DEBUG) || defined(RLD_RXACT_DEBUG)
-	{
-	    static int  hereb4 = 7;
-
-	    if (hereb4) {            /* RLD DEBUG */
-		hereb4--;
+		if (hereb4) {            /* RLD DEBUG */
+			hereb4--;
 #ifdef RLD_TRANS_DEBUG
-		md = &ch->mdr[ch->rxix_irq_srv];
-		pr_info("++ musycc_wq_chan_restart[%d] CHAN RX ACTIVATE: rxix_irq_srv %d, md %p sts %x, rxpkt %lu\n",
-		ch->channum, ch->rxix_irq_srv, md, le32_to_cpu(md->status),
-			ch->s.rx_packets);
+			md = &ch->mdr[ch->rxix_irq_srv];
+			pr_info("++ musycc_wq_chan_restart[%d] CHAN RX ACTIVATE: rxix_irq_srv %d, md %p sts %x, rxpkt %lu\n",
+				ch->channum, ch->rxix_irq_srv, md,
+				le32_to_cpu(md->status), ch->s.rx_packets);
 #elif defined(RLD_RXACT_DEBUG)
-		md = &ch->mdr[ch->rxix_irq_srv];
-		pr_info("++ musycc_wq_chan_restart[%d] CHAN RX ACTIVATE: rxix_irq_srv %d, md %p sts %x, rxpkt %lu\n",
-		ch->channum, ch->rxix_irq_srv, md, le32_to_cpu(md->status),
-			ch->s.rx_packets);
-		musycc_dump_rxbuffer_ring(ch, 1);      /* RLD DEBUG */
+			md = &ch->mdr[ch->rxix_irq_srv];
+			pr_info("++ musycc_wq_chan_restart[%d] CHAN RX ACTIVATE: rxix_irq_srv %d, md %p sts %x, rxpkt %lu\n",
+				ch->channum, ch->rxix_irq_srv,
+				md, le32_to_cpu(md->status),
+				ch->s.rx_packets);
+			musycc_dump_rxbuffer_ring(ch, 1);      /* RLD DEBUG */
 #endif
-	    }
-	}
+		}
 #endif
-	musycc_serv_req(pi, SR_CHANNEL_ACTIVATE | SR_RX_DIRECTION | ch->gchan);
-    }
-    /**********************************/
-    /** check for TX restart request **/
-    /**********************************/
+		musycc_serv_req(pi, SR_CHANNEL_ACTIVATE |
+				SR_RX_DIRECTION | ch->gchan);
+	}
+	/**********************************/
+	/** check for TX restart request **/
+	/**********************************/
 
-    if ((ch->ch_start_tx) && (ch->status & TX_ENABLED)) {
-	/* find next unprocessed message, then set TX thp to it */
-	musycc_update_tx_thp(ch);
+	if ((ch->ch_start_tx) && (ch->status & TX_ENABLED)) {
+		/* find next unprocessed message, then set TX thp to it */
+		musycc_update_tx_thp(ch);
 
-#if 0
-	spin_lock_irqsave(&ch->ch_txlock, flags);
-#endif
-	md = ch->txd_irq_srv;
-	if (!md) {
+		md = ch->txd_irq_srv;
+		if (!md) {
 #ifdef RLD_TRANS_DEBUG
-	    pr_info("-- musycc_wq_chan_restart[%d]: WARNING, starting NULL md\n", ch->channum);
-#endif
-#if 0
-	    spin_unlock_irqrestore(&ch->ch_txlock, flags);
-#endif
-	} else if (md->data && ((le32_to_cpu(md->status)) & MUSYCC_TX_OWNED)) {
-	    ch->ch_start_tx = 0;
-#if 0
-	    spin_unlock_irqrestore(&ch->ch_txlock, flags);   /* allow interrupts for service request */
+			pr_info("-- musycc_wq_chan_restart[%d]: WARNING, starting NULL md\n",
+				ch->channum);
 #endif
+		} else if (md->data && ((le32_to_cpu(md->status)) &
+			   MUSYCC_TX_OWNED)) {
+			ch->ch_start_tx = 0;
+
 #ifdef RLD_TRANS_DEBUG
-	    pr_info("++ musycc_wq_chan_restart() CHAN TX ACTIVATE: chan %d txd_irq_srv %p = sts %x, txpkt %lu\n",
-		    ch->channum, ch->txd_irq_srv, ch->txd_irq_srv->status, ch->s.tx_packets);
+			pr_info("++ musycc_wq_chan_restart() CHAN TX ACTIVATE: chan %d txd_irq_srv %p = sts %x, txpkt %lu\n",
+				ch->channum, ch->txd_irq_srv,
+				ch->txd_irq_srv->status, ch->s.tx_packets);
 #endif
-	    musycc_serv_req(pi, SR_CHANNEL_ACTIVATE | SR_TX_DIRECTION | ch->gchan);
-	}
+			musycc_serv_req(pi, SR_CHANNEL_ACTIVATE |
+					SR_TX_DIRECTION | ch->gchan);
+		}
 #ifdef RLD_RESTART_DEBUG
-	else {
-	    /* retain request to start until retried and we have data to xmit */
-	    pr_info("-- musycc_wq_chan_restart[%d]: DELAYED due to md %p sts %x data %x, start_tx %x\n",
-		    ch->channum, md,
-		    le32_to_cpu(md->status),
-		    le32_to_cpu(md->data), ch->ch_start_tx);
-	    musycc_dump_txbuffer_ring(ch, 0);
-#if 0
-	    spin_unlock_irqrestore(&ch->ch_txlock, flags);   /* allow interrupts for service request */
+		else {
+			/* retain request to start until retried and we have data to xmit */
+			pr_info("-- musycc_wq_chan_restart[%d]: DELAYED due to md %p sts %x data %x, start_tx %x\n",
+				ch->channum, md,
+				le32_to_cpu(md->status),
+				le32_to_cpu(md->data), ch->ch_start_tx);
+			musycc_dump_txbuffer_ring(ch, 0);
+		}
 #endif
 	}
-#endif
-    }
 }
 
 
@@ -439,31 +436,32 @@ void
 musycc_chan_restart(mch_t *ch)
 {
 #ifdef RLD_RESTART_DEBUG
-    pr_info("++ musycc_chan_restart[%d]: txd_irq_srv @ %p = sts %x\n",
-	    ch->channum, ch->txd_irq_srv, ch->txd_irq_srv->status);
+	pr_info("++ musycc_chan_restart[%d]: txd_irq_srv @ %p = sts %x\n",
+		ch->channum, ch->txd_irq_srv, ch->txd_irq_srv->status);
 #endif
 
-    /* 2.6 - find next unprocessed message, then set TX thp to it */
+	/* 2.6 - find next unprocessed message, then set TX thp to it */
 #ifdef RLD_RESTART_DEBUG
-    pr_info(">> musycc_chan_restart: scheduling Chan %x workQ @ %p\n", ch->channum, &ch->ch_work);
+	pr_info(">> musycc_chan_restart: scheduling Chan %x workQ @ %p\n",
+		ch->channum, &ch->ch_work);
 #endif
-    c4_wk_chan_restart(ch);        /* work queue mechanism fires off: Ref:
-				     * musycc_wq_chan_restart () */
-
+	c4_wk_chan_restart(ch);        /* work queue mechanism fires off: Ref:
+					* musycc_wq_chan_restart () */
 }
 
 
 void
 rld_put_led(mpi_t *pi, u_int32_t ledval)
 {
-    static u_int32_t led = 0;
+	static u_int32_t led;
 
-    if (ledval == 0)
-	led = 0;
-    else
-	led |= ledval;
+	if (ledval == 0)
+		led = 0;
+	else
+		led |= ledval;
 
-    pci_write_32((u_int32_t *) &pi->up->cpldbase->leds, led);  /* RLD DEBUG TRANHANG */
+	/* RLD DEBUG TRANHANG */
+	pci_write_32((u_int32_t *) &pi->up->cpldbase->leds, led);
 }
 
 
@@ -472,100 +470,110 @@ rld_put_led(mpi_t *pi, u_int32_t ledval)
 void
 musycc_serv_req(mpi_t *pi, u_int32_t req)
 {
-    volatile u_int32_t r;
-    int         rcnt;
+	volatile u_int32_t r;
+	int         rcnt;
 
-    /*
-     * PORT NOTE: Semaphore protect service loop guarantees only a single
-     * operation at a time.  Per MUSYCC Manual - "Issuing service requests to
-     * the same channel group without first receiving ACK from each request
-     * may cause the host to lose track of which service request has been
-     * acknowledged."
-     */
+	/*
+	 * PORT NOTE: Semaphore protect service loop guarantees only a single
+	 * operation at a time.  Per MUSYCC Manual - "Issuing service requests to
+	 * the same channel group without first receiving ACK from each request
+	 * may cause the host to lose track of which service request has been
+	 * acknowledged."
+	 */
 
-    SD_SEM_TAKE(&pi->sr_sem_busy, "serv");     /* only 1 thru here, per
-						 * group */
+	SD_SEM_TAKE(&pi->sr_sem_busy, "serv");     /* only 1 thru here, per
+						    * group */
 
-    if (pi->sr_last == req) {
+	if (pi->sr_last == req) {
 #ifdef RLD_TRANS_DEBUG
-	pr_info(">> same SR, Port %d Req %x\n", pi->portnum, req);
+		pr_info(">> same SR, Port %d Req %x\n", pi->portnum, req);
 #endif
 
-	/*
-	 * The most likely repeated request is the channel activation command
-	 * which follows the occurrence of a Transparent mode TX ONR or a
-	 * BUFF error.  If the previous command was a CHANNEL ACTIVATE,
-	 * precede it with a NOOP command in order maintain coherent control
-	 * of this current (re)ACTIVATE.
-	 */
+		/*
+		 * The most likely repeated request is the channel activation command
+		 * which follows the occurrence of a Transparent mode TX ONR or a
+		 * BUFF error.  If the previous command was a CHANNEL ACTIVATE,
+		 * precede it with a NOOP command in order maintain coherent control
+		 * of this current (re)ACTIVATE.
+		 */
 
-	r = (pi->sr_last & ~SR_GCHANNEL_MASK);
-	if ((r == (SR_CHANNEL_ACTIVATE | SR_TX_DIRECTION)) ||
-	    (r == (SR_CHANNEL_ACTIVATE | SR_RX_DIRECTION))) {
+		r = (pi->sr_last & ~SR_GCHANNEL_MASK);
+		if ((r == (SR_CHANNEL_ACTIVATE | SR_TX_DIRECTION)) ||
+		    (r == (SR_CHANNEL_ACTIVATE | SR_RX_DIRECTION))) {
 #ifdef RLD_TRANS_DEBUG
-	    pr_info(">> same CHAN ACT SR, Port %d Req %x => issue SR_NOOP CMD\n", pi->portnum, req);
+			pr_info(">> same CHAN ACT SR, Port %d Req %x => issue SR_NOOP CMD\n", pi->portnum, req);
 #endif
-	    SD_SEM_GIVE(&pi->sr_sem_busy);     /* allow this next request */
-	    musycc_serv_req(pi, SR_NOOP);
-	    SD_SEM_TAKE(&pi->sr_sem_busy, "serv");     /* relock & continue w/
-							 * original req */
-	} else if (req == SR_NOOP) {
-	    /* no need to issue back-to-back SR_NOOP commands at this time */
+			/* allow this next request */
+			SD_SEM_GIVE(&pi->sr_sem_busy);
+			musycc_serv_req(pi, SR_NOOP);
+			/* relock & continue w/ original req */
+			SD_SEM_TAKE(&pi->sr_sem_busy, "serv");
+		} else if (req == SR_NOOP) {
+			/* no need to issue back-to-back
+			 * SR_NOOP commands at this time
+			 */
 #ifdef RLD_TRANS_DEBUG
-	    pr_info(">> same Port SR_NOOP skipped, Port %d\n", pi->portnum);
+			pr_info(">> same Port SR_NOOP skipped, Port %d\n",
+				pi->portnum);
 #endif
-	    SD_SEM_GIVE(&pi->sr_sem_busy);     /* allow this next request */
-	    return;
+			/* allow this next request */
+			SD_SEM_GIVE(&pi->sr_sem_busy);
+			return;
+		}
 	}
-    }
-    rcnt = 0;
-    pi->sr_last = req;
+	rcnt = 0;
+	pi->sr_last = req;
 rewrite:
-    pci_write_32((u_int32_t *) &pi->reg->srd, req);
-    FLUSH_MEM_WRITE();
-
-    /*
-     * Per MUSYCC Manual, Section 6.1,2 - "When writing an SCR service
-     * request, the host must ensure at least one PCI bus clock cycle has
-     * elapsed before writing another service request.  To meet this minimum
-     * elapsed service request write timing interval, it is recommended that
-     * the host follow any SCR write with another operation which reads from
-     * the same address."
-     */
-    r = pci_read_32((u_int32_t *) &pi->reg->srd);      /* adhere to write
-							 * timing imposition */
-
-
-    if ((r != req) && (req != SR_CHIP_RESET) && (++rcnt <= MUSYCC_SR_RETRY_CNT)) {
-	if (cxt1e1_log_level >= LOG_MONITOR)
-	    pr_info("%s: %d - reissue srv req/last %x/%x (hdw reads %x), Chan %d.\n",
-		    pi->up->devname, rcnt, req, pi->sr_last, r,
-		    (pi->portnum * MUSYCC_NCHANS) + (req & 0x1f));
-	OS_uwait_dummy();          /* this delay helps reduce reissue counts
-				     * (reason not yet researched) */
-	goto rewrite;
-    }
-    if (rcnt > MUSYCC_SR_RETRY_CNT) {
-	pr_warning("%s: failed service request (#%d)= %x, group %d.\n",
-		   pi->up->devname, MUSYCC_SR_RETRY_CNT, req, pi->portnum);
-	SD_SEM_GIVE(&pi->sr_sem_busy); /* allow any next request */
-	return;
-    }
-    if (req == SR_CHIP_RESET) {
+	pci_write_32((u_int32_t *) &pi->reg->srd, req);
+	FLUSH_MEM_WRITE();
+
 	/*
-	 * PORT NOTE: the CHIP_RESET command is NOT ack'd by the MUSYCC, thus
-	 * the upcoming delay is used.  Though the MUSYCC documentation
-	 * suggests a read-after-write would supply the required delay, it's
-	 * unclear what CPU/BUS clock speeds might have been assumed when
-	 * suggesting this 'lack of ACK' workaround.  Thus the use of uwait.
+	 * Per MUSYCC Manual, Section 6.1,2 - "When writing an SCR service
+	 * request, the host must ensure at least one PCI bus clock cycle has
+	 * elapsed before writing another service request.  To meet this minimum
+	 * elapsed service request write timing interval, it is recommended that
+	 * the host follow any SCR write with another operation which reads from
+	 * the same address."
 	 */
-	OS_uwait(100000, "icard"); /* 100ms */
-    } else {
-	FLUSH_MEM_READ();
-	SD_SEM_TAKE(&pi->sr_sem_wait, "sakack");       /* sleep until SACK
-							 * interrupt occurs */
-    }
-    SD_SEM_GIVE(&pi->sr_sem_busy); /* allow any next request */
+
+	/* adhere to write timing imposition */
+	r = pci_read_32((u_int32_t *) &pi->reg->srd);
+
+
+	if ((r != req) && (req != SR_CHIP_RESET) &&
+	    (++rcnt <= MUSYCC_SR_RETRY_CNT)) {
+		if (cxt1e1_log_level >= LOG_MONITOR)
+			pr_info("%s: %d - reissue srv req/last %x/%x (hdw reads %x), Chan %d.\n",
+				pi->up->devname, rcnt, req, pi->sr_last, r,
+				(pi->portnum * MUSYCC_NCHANS) + (req & 0x1f));
+		/* this delay helps reduce reissue counts
+		 * (reason not yet researched)
+		 */
+		OS_uwait_dummy();
+		goto rewrite;
+	}
+	if (rcnt > MUSYCC_SR_RETRY_CNT) {
+		pr_warning("%s: failed service request (#%d)= %x, group %d.\n",
+			   pi->up->devname, MUSYCC_SR_RETRY_CNT,
+			   req, pi->portnum);
+		SD_SEM_GIVE(&pi->sr_sem_busy); /* allow any next request */
+		return;
+	}
+	if (req == SR_CHIP_RESET) {
+		/*
+		 * PORT NOTE: the CHIP_RESET command is NOT ack'd by the MUSYCC, thus
+		 * the upcoming delay is used.  Though the MUSYCC documentation
+		 * suggests a read-after-write would supply the required delay, it's
+		 * unclear what CPU/BUS clock speeds might have been assumed when
+		 * suggesting this 'lack of ACK' workaround.  Thus the use of uwait.
+		 */
+		OS_uwait(100000, "icard"); /* 100ms */
+	} else {
+		FLUSH_MEM_READ();
+		/* sleep until SACK interrupt occurs */
+		SD_SEM_TAKE(&pi->sr_sem_wait, "sakack");
+	}
+	SD_SEM_GIVE(&pi->sr_sem_busy); /* allow any next request */
 }
 
 
@@ -573,94 +581,101 @@ rewrite:
 void
 musycc_update_timeslots(mpi_t *pi)
 {
-    int         i, ch;
-    char        e1mode = IS_FRAME_ANY_E1(pi->p.port_mode);
-
-    for (i = 0; i < 32; i++) {
-	int         usedby = 0, last = 0, ts, j, bits[8];
-
-	u_int8_t lastval = 0;
-
-	if (((i == 0) && e1mode) || /* disable if  E1 mode */
-	    ((i == 16) && ((pi->p.port_mode == CFG_FRAME_E1CRC_CAS) || (pi->p.port_mode == CFG_FRAME_E1CRC_CAS_AMI)))
-	    || ((i > 23) && (!e1mode))) /* disable if T1 mode */
-	    pi->tsm[i] = 0xff;      /* make tslot unavailable for this mode */
-	else
-	    pi->tsm[i] = 0x00;      /* make tslot available for assignment */
-	for (j = 0; j < 8; j++)
-	    bits[j] = -1;
-	for (ch = 0; ch < MUSYCC_NCHANS; ch++) {
-	    if ((pi->chan[ch]->state == UP) && (pi->chan[ch]->p.bitmask[i])) {
-		usedby++;
-		last = ch;
-		lastval = pi->chan[ch]->p.bitmask[i];
+	int         i, ch;
+	char        e1mode = IS_FRAME_ANY_E1(pi->p.port_mode);
+
+	for (i = 0; i < 32; i++) {
+		int         usedby = 0, last = 0, ts, j, bits[8];
+
+		u_int8_t lastval = 0;
+
+		if (((i == 0) && e1mode) || /* disable if  E1 mode */
+		    ((i == 16) && ((pi->p.port_mode == CFG_FRAME_E1CRC_CAS) ||
+		    (pi->p.port_mode == CFG_FRAME_E1CRC_CAS_AMI))) ||
+		    ((i > 23) && (!e1mode))) /* disable if T1 mode */
+			/* make tslot unavailable for this mode */
+			pi->tsm[i] = 0xff;
+		else
+			/* make tslot available for assignment */
+			pi->tsm[i] = 0x00;
 		for (j = 0; j < 8; j++)
-		    if (lastval & (1 << j))
-			bits[j] = ch;
-		pi->tsm[i] |= lastval;
-	    }
-	}
-	if (!usedby)
-	    ts = 0;
-	else if ((usedby == 1) && (lastval == 0xff))
-	    ts = (4 << 5) | last;
-	else if ((usedby == 1) && (lastval == 0x7f))
-	    ts = (5 << 5) | last;
-	else {
-	    int         idx;
-
-	    if (bits[0] < 0)
-		ts = (6 << 5) | (idx = last);
-	    else
-		ts = (7 << 5) | (idx = bits[0]);
-	    for (j = 1; j < 8; j++) {
-		pi->regram->rscm[idx * 8 + j] = (bits[j] < 0) ? 0 : (0x80 | bits[j]);
-		pi->regram->tscm[idx * 8 + j] = (bits[j] < 0) ? 0 : (0x80 | bits[j]);
-	    }
+			bits[j] = -1;
+		for (ch = 0; ch < MUSYCC_NCHANS; ch++) {
+			if ((pi->chan[ch]->state == UP) &&
+			    (pi->chan[ch]->p.bitmask[i])) {
+				usedby++;
+				last = ch;
+				lastval = pi->chan[ch]->p.bitmask[i];
+				for (j = 0; j < 8; j++)
+					if (lastval & (1 << j))
+						bits[j] = ch;
+				pi->tsm[i] |= lastval;
+			}
+		}
+		if (!usedby)
+			ts = 0;
+		else if ((usedby == 1) && (lastval == 0xff))
+			ts = (4 << 5) | last;
+		else if ((usedby == 1) && (lastval == 0x7f))
+			ts = (5 << 5) | last;
+		else {
+			int         idx;
+
+			if (bits[0] < 0)
+				ts = (6 << 5) | (idx = last);
+			else
+				ts = (7 << 5) | (idx = bits[0]);
+			for (j = 1; j < 8; j++) {
+				pi->regram->rscm[idx * 8 + j] =
+					(bits[j] < 0) ? 0 : (0x80 | bits[j]);
+				pi->regram->tscm[idx * 8 + j] =
+					(bits[j] < 0) ? 0 : (0x80 | bits[j]);
+			}
+		}
+		pi->regram->rtsm[i] = ts;
+		pi->regram->ttsm[i] = ts;
 	}
-	pi->regram->rtsm[i] = ts;
-	pi->regram->ttsm[i] = ts;
-    }
-    FLUSH_MEM_WRITE();
-
-    musycc_serv_req(pi, SR_TIMESLOT_MAP | SR_RX_DIRECTION);
-    musycc_serv_req(pi, SR_TIMESLOT_MAP | SR_TX_DIRECTION);
-    musycc_serv_req(pi, SR_SUBCHANNEL_MAP | SR_RX_DIRECTION);
-    musycc_serv_req(pi, SR_SUBCHANNEL_MAP | SR_TX_DIRECTION);
+	FLUSH_MEM_WRITE();
+
+	musycc_serv_req(pi, SR_TIMESLOT_MAP | SR_RX_DIRECTION);
+	musycc_serv_req(pi, SR_TIMESLOT_MAP | SR_TX_DIRECTION);
+	musycc_serv_req(pi, SR_SUBCHANNEL_MAP | SR_RX_DIRECTION);
+	musycc_serv_req(pi, SR_SUBCHANNEL_MAP | SR_TX_DIRECTION);
 }
 #endif
 
 
 #ifdef SBE_WAN256T3_ENABLE
-void
+	void
 musycc_update_timeslots(mpi_t *pi)
 {
-    mch_t      *ch;
+	mch_t      *ch;
 
-    u_int8_t    ts, hmask, tsen;
-    int         gchan;
-    int         i;
+	u_int8_t    ts, hmask, tsen;
+	int         gchan;
+	int         i;
 
 #ifdef SBE_PMCC4_ENABLE
-    hmask = (0x1f << pi->up->p.hypersize) & 0x1f;
+	hmask = (0x1f << pi->up->p.hypersize) & 0x1f;
 #endif
 #ifdef SBE_WAN256T3_ENABLE
-    hmask = (0x1f << hyperdummy) & 0x1f;
+	hmask = (0x1f << hyperdummy) & 0x1f;
 #endif
-    for (i = 0; i < 128; i++) {
-	gchan = ((pi->portnum * MUSYCC_NCHANS) + (i & hmask)) % MUSYCC_NCHANS;
-	ch = pi->chan[gchan];
-	if (ch->p.mode_56k)
-	    tsen = MODE_56KBPS;
-	else
-	    tsen = MODE_64KBPS;     /* also the default */
-	ts = ((pi->portnum % 4) == (i / 32)) ? (tsen << 5) | (i & hmask) : 0;
-	pi->regram->rtsm[i] = ts;
-	pi->regram->ttsm[i] = ts;
-    }
-    FLUSH_MEM_WRITE();
-    musycc_serv_req(pi, SR_TIMESLOT_MAP | SR_RX_DIRECTION);
-    musycc_serv_req(pi, SR_TIMESLOT_MAP | SR_TX_DIRECTION);
+	for (i = 0; i < 128; i++) {
+		gchan = ((pi->portnum * MUSYCC_NCHANS) +
+			(i & hmask)) % MUSYCC_NCHANS;
+		ch = pi->chan[gchan];
+		if (ch->p.mode_56k)
+			tsen = MODE_56KBPS;
+		else
+			tsen = MODE_64KBPS;     /* also the default */
+		ts = ((pi->portnum % 4) == (i / 32)) ? (tsen << 5) | (i & hmask) : 0;
+		pi->regram->rtsm[i] = ts;
+		pi->regram->ttsm[i] = ts;
+	}
+	FLUSH_MEM_WRITE();
+	musycc_serv_req(pi, SR_TIMESLOT_MAP | SR_RX_DIRECTION);
+	musycc_serv_req(pi, SR_TIMESLOT_MAP | SR_TX_DIRECTION);
 }
 #endif
 
@@ -672,59 +687,59 @@ musycc_update_timeslots(mpi_t *pi)
 u_int32_t
 musycc_chan_proto(int proto)
 {
-    int         reg;
-
-    switch (proto) {
-    case CFG_CH_PROTO_TRANS:        /* 0 */
-	reg = MUSYCC_CCD_TRANS;
-	break;
-    case CFG_CH_PROTO_SS7:          /* 1 */
-	reg = MUSYCC_CCD_SS7;
-	break;
-    default:
-    case CFG_CH_PROTO_ISLP_MODE:   /* 4 */
-    case CFG_CH_PROTO_HDLC_FCS16:  /* 2 */
-	reg = MUSYCC_CCD_HDLC_FCS16;
-	break;
-    case CFG_CH_PROTO_HDLC_FCS32:  /* 3 */
-	reg = MUSYCC_CCD_HDLC_FCS32;
-	break;
-    }
-
-    return reg;
+	int         reg;
+
+	switch (proto) {
+	case CFG_CH_PROTO_TRANS:        /* 0 */
+		reg = MUSYCC_CCD_TRANS;
+		break;
+	case CFG_CH_PROTO_SS7:          /* 1 */
+		reg = MUSYCC_CCD_SS7;
+		break;
+	default:
+	case CFG_CH_PROTO_ISLP_MODE:   /* 4 */
+	case CFG_CH_PROTO_HDLC_FCS16:  /* 2 */
+		reg = MUSYCC_CCD_HDLC_FCS16;
+		break;
+	case CFG_CH_PROTO_HDLC_FCS32:  /* 3 */
+		reg = MUSYCC_CCD_HDLC_FCS32;
+		break;
+	}
+
+	return reg;
 }
 
 #ifdef SBE_WAN256T3_ENABLE
 static void __init
 musycc_init_port(mpi_t *pi)
 {
-    pci_write_32((u_int32_t *) &pi->reg->gbp, OS_vtophys(pi->regram));
-
-    pi->regram->grcd =
-	__constant_cpu_to_le32(MUSYCC_GRCD_RX_ENABLE |
-				MUSYCC_GRCD_TX_ENABLE |
-				MUSYCC_GRCD_SF_ALIGN |
-				MUSYCC_GRCD_SUBCHAN_DISABLE |
-				MUSYCC_GRCD_OOFMP_DISABLE |
-				MUSYCC_GRCD_COFAIRQ_DISABLE |
-				MUSYCC_GRCD_MC_ENABLE |
-		       (MUSYCC_GRCD_POLLTH_32 << MUSYCC_GRCD_POLLTH_SHIFT));
-
-    pi->regram->pcd =
-	__constant_cpu_to_le32(MUSYCC_PCD_E1X4_MODE |
-				MUSYCC_PCD_TXDATA_RISING |
-				MUSYCC_PCD_TX_DRIVEN);
-
-    /* Message length descriptor */
-       pi->regram->mld = __constant_cpu_to_le32(cxt1e1_max_mru | (cxt1e1_max_mru << 16));
-    FLUSH_MEM_WRITE();
+	pci_write_32((u_int32_t *) &pi->reg->gbp, OS_vtophys(pi->regram));
+
+	pi->regram->grcd =
+		__constant_cpu_to_le32(MUSYCC_GRCD_RX_ENABLE |
+				       MUSYCC_GRCD_TX_ENABLE |
+				       MUSYCC_GRCD_SF_ALIGN |
+				       MUSYCC_GRCD_SUBCHAN_DISABLE |
+				       MUSYCC_GRCD_OOFMP_DISABLE |
+				       MUSYCC_GRCD_COFAIRQ_DISABLE |
+				       MUSYCC_GRCD_MC_ENABLE |
+				       (MUSYCC_GRCD_POLLTH_32 << MUSYCC_GRCD_POLLTH_SHIFT));
+
+	pi->regram->pcd =
+		__constant_cpu_to_le32(MUSYCC_PCD_E1X4_MODE |
+				       MUSYCC_PCD_TXDATA_RISING |
+				       MUSYCC_PCD_TX_DRIVEN);
+
+	/* Message length descriptor */
+	pi->regram->mld = __constant_cpu_to_le32(cxt1e1_max_mru | (cxt1e1_max_mru << 16));
+	FLUSH_MEM_WRITE();
 
-    musycc_serv_req(pi, SR_GROUP_INIT | SR_RX_DIRECTION);
-    musycc_serv_req(pi, SR_GROUP_INIT | SR_TX_DIRECTION);
+	musycc_serv_req(pi, SR_GROUP_INIT | SR_RX_DIRECTION);
+	musycc_serv_req(pi, SR_GROUP_INIT | SR_TX_DIRECTION);
 
-    musycc_init_mdt(pi);
+	musycc_init_mdt(pi);
 
-    musycc_update_timeslots(pi);
+	musycc_update_timeslots(pi);
 }
 #endif
 
@@ -732,490 +747,456 @@ musycc_init_port(mpi_t *pi)
 status_t    __init
 musycc_init(ci_t *ci)
 {
-    char       *regaddr;        /* temp for address boundary calculations */
-    int         i, gchan;
+	char       *regaddr;        /* temp for address boundary calculations */
+	int         i, gchan;
 
-    OS_sem_init(&ci->sem_wdbusy, SEM_AVAILABLE);       /* watchdog exclusion */
+	OS_sem_init(&ci->sem_wdbusy, SEM_AVAILABLE); /* watchdog exclusion */
 
-    /*
-     * Per MUSYCC manual, Section 6.3.4 - "The host must allocate a dword
-     * aligned memory segment for interrupt queue pointers."
-     */
+	/*
+	 * Per MUSYCC manual, Section 6.3.4 - "The host must allocate a dword
+	 * aligned memory segment for interrupt queue pointers."
+	 */
 
 #define INT_QUEUE_BOUNDARY  4
 
-    regaddr = OS_kmalloc((INT_QUEUE_SIZE + 1) * sizeof(u_int32_t));
-    if (!regaddr)
-	return -ENOMEM;
-    ci->iqd_p_saved = regaddr;      /* save orig value for free's usage */
-    ci->iqd_p = (u_int32_t *) ((unsigned long) (regaddr + INT_QUEUE_BOUNDARY - 1) &
-			       (~(INT_QUEUE_BOUNDARY - 1)));    /* this calculates
-								 * closest boundary */
+	regaddr = kzalloc((INT_QUEUE_SIZE + 1) * sizeof(u_int32_t),
+			  GFP_KERNEL | GFP_DMA);
+	if (!regaddr)
+		return -ENOMEM;
+	ci->iqd_p_saved = regaddr;      /* save orig value for free's usage */
+	/* this calculates closest boundary */
+	ci->iqd_p = (u_int32_t *) ((unsigned long)(regaddr + INT_QUEUE_BOUNDARY - 1) &
+				   (~(INT_QUEUE_BOUNDARY - 1)));
 
-    for (i = 0; i < INT_QUEUE_SIZE; i++)
-	ci->iqd_p[i] = __constant_cpu_to_le32(INT_EMPTY_ENTRY);
+	for (i = 0; i < INT_QUEUE_SIZE; i++)
+		ci->iqd_p[i] = __constant_cpu_to_le32(INT_EMPTY_ENTRY);
 
-    for (i = 0; i < ci->max_port; i++) {
-	mpi_t      *pi = &ci->port[i];
+	for (i = 0; i < ci->max_port; i++) {
+		mpi_t      *pi = &ci->port[i];
 
-	/*
-	 * Per MUSYCC manual, Section 6.3.2 - "The host must allocate a 2KB
-	 * bound memory segment for Channel Group 0."
-	 */
+		/*
+		 * Per MUSYCC manual, Section 6.3.2 - "The host must allocate a 2KB
+		 * bound memory segment for Channel Group 0."
+		 */
 
 #define GROUP_BOUNDARY   0x800
 
-	regaddr = OS_kmalloc(sizeof(struct musycc_groupr) + GROUP_BOUNDARY);
-	if (!regaddr) {
-	    for (gchan = 0; gchan < i; gchan++) {
-		pi = &ci->port[gchan];
-		OS_kfree(pi->reg);
-		pi->reg = NULL;
-	    }
-	    return -ENOMEM;
+		regaddr = kzalloc(sizeof(struct musycc_groupr) + GROUP_BOUNDARY,
+				  GFP_KERNEL | GFP_DMA);
+		if (!regaddr) {
+			for (gchan = 0; gchan < i; gchan++) {
+				pi = &ci->port[gchan];
+				kfree(pi->reg);
+				pi->reg = NULL;
+			}
+			return -ENOMEM;
+		}
+		pi->regram_saved = regaddr; /* save orig value for free's usage */
+		/* this calculates closest boundary */
+		pi->regram = (struct musycc_groupr *) ((unsigned long)(regaddr + GROUP_BOUNDARY - 1) &
+				(~(GROUP_BOUNDARY - 1)));
 	}
-	pi->regram_saved = regaddr; /* save orig value for free's usage */
-	pi->regram = (struct musycc_groupr *) ((unsigned long) (regaddr + GROUP_BOUNDARY - 1) &
-					       (~(GROUP_BOUNDARY - 1)));        /* this calculates
-										 * closest boundary */
-    }
-
-    /* any board centric MUSYCC commands will use group ZERO as its "home" */
-    ci->regram = ci->port[0].regram;
-    musycc_serv_req(&ci->port[0], SR_CHIP_RESET);
-
-    pci_write_32((u_int32_t *) &ci->reg->gbp, OS_vtophys(ci->regram));
-    pci_flush_write(ci);
+
+	/* any board centric MUSYCC commands will use group ZERO as its "home" */
+	ci->regram = ci->port[0].regram;
+	musycc_serv_req(&ci->port[0], SR_CHIP_RESET);
+
+	pci_write_32((u_int32_t *) &ci->reg->gbp, OS_vtophys(ci->regram));
+	pci_flush_write(ci);
 #ifdef CONFIG_SBE_PMCC4_NCOMM
-    ci->regram->__glcd = __constant_cpu_to_le32(GCD_MAGIC);
+	ci->regram->__glcd = __constant_cpu_to_le32(GCD_MAGIC);
 #else
-    /* standard driver POLLS for INTB via CPLD register */
-    ci->regram->__glcd = __constant_cpu_to_le32(GCD_MAGIC | MUSYCC_GCD_INTB_DISABLE);
+	/* standard driver POLLS for INTB via CPLD register */
+	ci->regram->__glcd = __constant_cpu_to_le32(GCD_MAGIC |
+						    MUSYCC_GCD_INTB_DISABLE);
 #endif
 
-    ci->regram->__iqp = cpu_to_le32(OS_vtophys(&ci->iqd_p[0]));
-    ci->regram->__iql = __constant_cpu_to_le32(INT_QUEUE_SIZE - 1);
-    pci_write_32((u_int32_t *) &ci->reg->dacbp, 0);
-    FLUSH_MEM_WRITE();
+	ci->regram->__iqp = cpu_to_le32(OS_vtophys(&ci->iqd_p[0]));
+	ci->regram->__iql = __constant_cpu_to_le32(INT_QUEUE_SIZE - 1);
+	pci_write_32((u_int32_t *) &ci->reg->dacbp, 0);
+	FLUSH_MEM_WRITE();
 
-    ci->state = C_RUNNING;          /* mark as full interrupt processing
-				     * available */
+	ci->state = C_RUNNING;          /* mark as full interrupt processing
+					 * available */
 
-    musycc_serv_req(&ci->port[0], SR_GLOBAL_INIT);     /* FIRST INTERRUPT ! */
+	musycc_serv_req(&ci->port[0], SR_GLOBAL_INIT); /* FIRST INTERRUPT ! */
 
-    /* sanity check settable parameters */
+	/* sanity check settable parameters */
 
-       if (cxt1e1_max_mru > 0xffe) {
-	pr_warning("Maximum allowed MRU exceeded, resetting %d to %d.\n",
-				  cxt1e1_max_mru, 0xffe);
-	       cxt1e1_max_mru = 0xffe;
-    }
-       if (cxt1e1_max_mtu > 0xffe) {
-	pr_warning("Maximum allowed MTU exceeded, resetting %d to %d.\n",
-				  cxt1e1_max_mtu, 0xffe);
-	       cxt1e1_max_mtu = 0xffe;
-    }
+	if (cxt1e1_max_mru > 0xffe) {
+		pr_warning("Maximum allowed MRU exceeded, resetting %d to %d.\n",
+			   cxt1e1_max_mru, 0xffe);
+		cxt1e1_max_mru = 0xffe;
+	}
+	if (cxt1e1_max_mtu > 0xffe) {
+		pr_warning("Maximum allowed MTU exceeded, resetting %d to %d.\n",
+			   cxt1e1_max_mtu, 0xffe);
+		cxt1e1_max_mtu = 0xffe;
+	}
 #ifdef SBE_WAN256T3_ENABLE
-    for (i = 0; i < MUSYCC_NPORTS; i++)
-	musycc_init_port(&ci->port[i]);
+	for (i = 0; i < MUSYCC_NPORTS; i++)
+		musycc_init_port(&ci->port[i]);
 #endif
 
-    return SBE_DRVR_SUCCESS;        /* no error */
+	return SBE_DRVR_SUCCESS;        /* no error */
 }
 
 
 void
 musycc_bh_tx_eom(mpi_t *pi, int gchan)
 {
-    mch_t      *ch;
-    struct mdesc *md;
+	mch_t      *ch;
+	struct mdesc *md;
 
-#if 0
-#ifndef SBE_ISR_INLINE
-    unsigned long flags;
+	volatile u_int32_t status;
 
-#endif
-#endif
-    volatile u_int32_t status;
-
-    ch = pi->chan[gchan];
-    if (!ch || ch->state != UP) {
-	if (cxt1e1_log_level >= LOG_ERROR)
-	    pr_info("%s: intr: xmit EOM on uninitialized channel %d\n",
-		    pi->up->devname, gchan);
-    }
-    if (!ch || !ch->mdt)
-	return;                     /* note: mdt==0 implies a malloc()
-				     * failure w/in chan_up() routine */
-
-#if 0
-#ifdef SBE_ISR_INLINE
-    spin_lock_irq(&ch->ch_txlock);
-#else
-    spin_lock_irqsave(&ch->ch_txlock, flags);
-#endif
-#endif
-    do {
-	FLUSH_MEM_READ();
-	md = ch->txd_irq_srv;
-	status = le32_to_cpu(md->status);
+	ch = pi->chan[gchan];
+	if (!ch || ch->state != UP) {
+		if (cxt1e1_log_level >= LOG_ERROR)
+			pr_info("%s: intr: xmit EOM on uninitialized channel %d\n",
+				pi->up->devname, gchan);
+	}
+	if (!ch || !ch->mdt)
+		return;                     /* note: mdt==0 implies a malloc()
+					     * failure w/in chan_up() routine */
 
-	/*
-	 * Note: Per MUSYCC Ref 6.4.9, the host does not poll a host-owned
-	 * Transmit Buffer Descriptor during Transparent Mode.
-	 */
-	if (status & MUSYCC_TX_OWNED) {
-	    int         readCount, loopCount;
-
-	    /***********************************************************/
-	    /* HW Bug Fix                                              */
-	    /* ----------                                              */
-	    /* Under certain PCI Bus loading conditions, the data      */
-	    /* associated with an update of Shared Memory is delayed   */
-	    /* relative to its PCI Interrupt.  This is caught when     */
-	    /* the host determines it does not yet OWN the descriptor. */
-	    /***********************************************************/
-
-	    readCount = 0;
-	    while (status & MUSYCC_TX_OWNED) {
-		for (loopCount = 0; loopCount < 0x30; loopCount++)
-		    OS_uwait_dummy();  /* use call to avoid optimization
-					 * removal of dummy delay */
+	do {
 		FLUSH_MEM_READ();
+		md = ch->txd_irq_srv;
 		status = le32_to_cpu(md->status);
-		if (readCount++ > 40)
-		    break;          /* don't wait any longer */
-	    }
-	    if (status & MUSYCC_TX_OWNED) {
-		if (cxt1e1_log_level >= LOG_MONITOR) {
-		    pr_info("%s: Port %d Chan %2d - unexpected TX msg ownership intr (md %p sts %x)\n",
-			    pi->up->devname, pi->portnum, ch->channum,
-			    md, status);
-		    pr_info("++ User 0x%p IRQ_SRV 0x%p USR_ADD 0x%p QStopped %x, start_tx %x tx_full %d txd_free %d mode %x\n",
-			    ch->user, ch->txd_irq_srv, ch->txd_usr_add,
-			    sd_queue_stopped(ch->user),
-			    ch->ch_start_tx, ch->tx_full, ch->txd_free, ch->p.chan_mode);
-		    musycc_dump_txbuffer_ring(ch, 0);
+
+		/*
+		 * Note: Per MUSYCC Ref 6.4.9, the host does not poll a host-owned
+		 * Transmit Buffer Descriptor during Transparent Mode.
+		 */
+		if (status & MUSYCC_TX_OWNED) {
+			int         readCount, loopCount;
+
+			/***********************************************************/
+			/* HW Bug Fix                                              */
+			/* ----------                                              */
+			/* Under certain PCI Bus loading conditions, the data      */
+			/* associated with an update of Shared Memory is delayed   */
+			/* relative to its PCI Interrupt.  This is caught when     */
+			/* the host determines it does not yet OWN the descriptor. */
+			/***********************************************************/
+
+			readCount = 0;
+			while (status & MUSYCC_TX_OWNED) {
+				for (loopCount = 0; loopCount < 0x30; loopCount++)
+					/* use call to avoid optimization
+					 * removal of dummy delay */
+					OS_uwait_dummy();
+				FLUSH_MEM_READ();
+				status = le32_to_cpu(md->status);
+				if (readCount++ > 40)
+					break; /* don't wait any longer */
+			}
+			if (status & MUSYCC_TX_OWNED) {
+				if (cxt1e1_log_level >= LOG_MONITOR) {
+					pr_info("%s: Port %d Chan %2d - unexpected TX msg ownership intr (md %p sts %x)\n",
+						pi->up->devname, pi->portnum,
+						ch->channum, md, status);
+					pr_info("++ User 0x%p IRQ_SRV 0x%p USR_ADD 0x%p QStopped %x, start_tx %x tx_full %d txd_free %d mode %x\n",
+						ch->user, ch->txd_irq_srv,
+						ch->txd_usr_add,
+						sd_queue_stopped(ch->user),
+						ch->ch_start_tx, ch->tx_full,
+						ch->txd_free, ch->p.chan_mode);
+					musycc_dump_txbuffer_ring(ch, 0);
+				}
+				break;              /* Not our mdesc, done */
+			} else {
+				if (cxt1e1_log_level >= LOG_MONITOR)
+					pr_info("%s: Port %d Chan %2d - recovered TX msg ownership [%d] (md %p sts %x)\n",
+						pi->up->devname, pi->portnum,
+						ch->channum, readCount,
+						md, status);
+			}
 		}
-		break;              /* Not our mdesc, done */
-	    } else {
-		if (cxt1e1_log_level >= LOG_MONITOR)
-		    pr_info("%s: Port %d Chan %2d - recovered TX msg ownership [%d] (md %p sts %x)\n",
-			    pi->up->devname, pi->portnum, ch->channum, readCount, md, status);
-	    }
-	}
-	ch->txd_irq_srv = md->snext;
-
-	md->data = 0;
-	if (md->mem_token)	{
-	    /* upcount channel */
-	    atomic_sub(OS_mem_token_tlen(md->mem_token), &ch->tx_pending);
-	    /* upcount card */
-	    atomic_sub(OS_mem_token_tlen(md->mem_token), &pi->up->tx_pending);
+		ch->txd_irq_srv = md->snext;
+
+		md->data = 0;
+		if (md->mem_token) {
+			/* upcount channel */
+			atomic_sub(OS_mem_token_tlen(md->mem_token),
+				   &ch->tx_pending);
+			/* upcount card */
+			atomic_sub(OS_mem_token_tlen(md->mem_token),
+				   &pi->up->tx_pending);
 #ifdef SBE_WAN256T3_ENABLE
-	    if (!atomic_read(&pi->up->tx_pending))
-		wan256t3_led(pi->up, LED_TX, 0);
+			if (!atomic_read(&pi->up->tx_pending))
+				wan256t3_led(pi->up, LED_TX, 0);
 #endif
-
-#ifdef CONFIG_SBE_WAN256T3_NCOMM
-	    /* callback that our packet was sent */
-	    {
-		int         hdlcnum = (pi->portnum * 32 + gchan);
-
-		if (hdlcnum >= 228) {
-		    if (nciProcess_TX_complete)
-			(*nciProcess_TX_complete) (hdlcnum,
-						   getuserbychan(gchan));
+			OS_mem_token_free_irq(md->mem_token);
+			md->mem_token = NULL;
 		}
-	    }
-#endif                              /*** CONFIG_SBE_WAN256T3_NCOMM ***/
-
-	    OS_mem_token_free_irq(md->mem_token);
-	    md->mem_token = NULL;
-	}
-	md->status = 0;
+		md->status = 0;
 #ifdef RLD_TXFULL_DEBUG
-	if (cxt1e1_log_level >= LOG_MONITOR2)
-	    pr_info("~~ tx_eom: tx_full %x  txd_free %d -> %d\n",
-		    ch->tx_full, ch->txd_free, ch->txd_free + 1);
+		if (cxt1e1_log_level >= LOG_MONITOR2)
+			pr_info("~~ tx_eom: tx_full %x  txd_free %d -> %d\n",
+				ch->tx_full, ch->txd_free, ch->txd_free + 1);
 #endif
-	++ch->txd_free;
-	FLUSH_MEM_WRITE();
+		++ch->txd_free;
+		FLUSH_MEM_WRITE();
+
+		if ((ch->p.chan_mode != CFG_CH_PROTO_TRANS) &&
+		    (status & EOBIRQ_ENABLE)) {
+			if (cxt1e1_log_level >= LOG_MONITOR)
+				pr_info("%s: Mode (%x) incorrect EOB status (%x)\n",
+					pi->up->devname, ch->p.chan_mode,
+					status);
+			if ((status & EOMIRQ_ENABLE) == 0)
+				break;
+		}
+	} while ((ch->p.chan_mode != CFG_CH_PROTO_TRANS) &&
+		 ((status & EOMIRQ_ENABLE) == 0));
+	/*
+	 * NOTE: (The above 'while' is coupled w/ previous 'do', way above.) Each
+	 * Transparent data buffer has the EOB bit, and NOT the EOM bit, set and
+	 * will furthermore have a separate IQD associated with each messages
+	 * buffer.
+	 */
 
-	if ((ch->p.chan_mode != CFG_CH_PROTO_TRANS) && (status & EOBIRQ_ENABLE)) {
-	    if (cxt1e1_log_level >= LOG_MONITOR)
-		pr_info("%s: Mode (%x) incorrect EOB status (%x)\n",
-			pi->up->devname, ch->p.chan_mode, status);
-	    if ((status & EOMIRQ_ENABLE) == 0)
-		break;
-	}
-    } while ((ch->p.chan_mode != CFG_CH_PROTO_TRANS) && ((status & EOMIRQ_ENABLE) == 0));
-    /*
-     * NOTE: (The above 'while' is coupled w/ previous 'do', way above.) Each
-     * Transparent data buffer has the EOB bit, and NOT the EOM bit, set and
-     * will furthermore have a separate IQD associated with each messages
-     * buffer.
-     */
-
-    FLUSH_MEM_READ();
-    /*
-     * Smooth flow control hysterisis by maintaining task stoppage until half
-     * the available write buffers are available.
-     */
-    if (ch->tx_full && (ch->txd_free >= (ch->txd_num / 2))) {
+	FLUSH_MEM_READ();
 	/*
-	 * Then, only releave task stoppage if we actually have enough
-	 * buffers to service the last requested packet.  It may require MORE
-	 * than half the available!
+	 * Smooth flow control hysterisis by maintaining task stoppage until half
+	 * the available write buffers are available.
 	 */
-	if (ch->txd_free >= ch->txd_required) {
+	if (ch->tx_full && (ch->txd_free >= (ch->txd_num / 2))) {
+		/*
+		 * Then, only releave task stoppage if we actually have enough
+		 * buffers to service the last requested packet.  It may require MORE
+		 * than half the available!
+		 */
+		if (ch->txd_free >= ch->txd_required) {
 
 #ifdef RLD_TXFULL_DEBUG
-	    if (cxt1e1_log_level >= LOG_MONITOR2)
-		pr_info("tx_eom[%d]: enable xmit tx_full no more, txd_free %d txd_num/2 %d\n",
-			ch->channum,
-			ch->txd_free, ch->txd_num / 2);
+			if (cxt1e1_log_level >= LOG_MONITOR2)
+				pr_info("tx_eom[%d]: enable xmit tx_full no more, txd_free %d txd_num/2 %d\n",
+					ch->channum,
+					ch->txd_free, ch->txd_num / 2);
 #endif
-	    ch->tx_full = 0;
-	    ch->txd_required = 0;
-	    sd_enable_xmit(ch->user);  /* re-enable to catch flow controlled
-					 * channel */
+			ch->tx_full = 0;
+			ch->txd_required = 0;
+			/* re-enable to catch flow controlled channel */
+			sd_enable_xmit(ch->user);
+		}
 	}
-    }
 #ifdef RLD_TXFULL_DEBUG
-    else if (ch->tx_full) {
-	if (cxt1e1_log_level >= LOG_MONITOR2)
-	    pr_info("tx_eom[%d]: bypass TX enable though room available? (txd_free %d txd_num/2 %d)\n",
-		    ch->channum,
-		    ch->txd_free, ch->txd_num / 2);
-    }
+	else if (ch->tx_full) {
+		if (cxt1e1_log_level >= LOG_MONITOR2)
+			pr_info("tx_eom[%d]: bypass TX enable though room available? (txd_free %d txd_num/2 %d)\n",
+				ch->channum,
+				ch->txd_free, ch->txd_num / 2);
+	}
 #endif
 
-    FLUSH_MEM_WRITE();
-#if 0
-#ifdef SBE_ISR_INLINE
-    spin_unlock_irq(&ch->ch_txlock);
-#else
-    spin_unlock_irqrestore(&ch->ch_txlock, flags);
-#endif
-#endif
+	FLUSH_MEM_WRITE();
 }
 
 
 static void
 musycc_bh_rx_eom(mpi_t *pi, int gchan)
 {
-    mch_t      *ch;
-    void       *m, *m2;
-    struct mdesc *md;
-    volatile u_int32_t status;
-    u_int32_t   error;
-
-    ch = pi->chan[gchan];
-    if (!ch || ch->state != UP) {
-	if (cxt1e1_log_level > LOG_ERROR)
-	    pr_info("%s: intr: receive EOM on uninitialized channel %d\n",
-		    pi->up->devname, gchan);
-	return;
-    }
-    if (!ch->mdr)
-	return;                     /* can this happen ? */
-
-    for (;;) {
-	FLUSH_MEM_READ();
-	md = &ch->mdr[ch->rxix_irq_srv];
-	status = le32_to_cpu(md->status);
-	if (!(status & HOST_RX_OWNED))
-	    break;                  /* Not our mdesc, done */
-	m = md->mem_token;
-	error = (status >> 16) & 0xf;
-	if (error == 0) {
-#ifdef CONFIG_SBE_WAN256T3_NCOMM
-	    int         hdlcnum = (pi->portnum * 32 + gchan);
-
-	    /*
-	     * if the packet number belongs to NCOMM, then send it to the TMS
-	     * driver
-	     */
-	    if (hdlcnum >= 228) {
-		if (nciProcess_RX_packet)
-		    (*nciProcess_RX_packet) (hdlcnum, status & 0x3fff, m, ch->user);
-	    } else
-#endif                              /*** CONFIG_SBE_WAN256T3_NCOMM ***/
-
-	    {
-		m2 = OS_mem_token_alloc(cxt1e1_max_mru);
-		if (m2) {
-			/* substitute the mbuf+cluster */
-			md->mem_token = m2;
-			md->data = cpu_to_le32(OS_vtophys(
-				OS_mem_token_data(m2)));
-
-			/* pass the received mbuf upward */
-			sd_recv_consume(m, status & LENGTH_MASK, ch->user);
-			ch->s.rx_packets++;
-			ch->s.rx_bytes += status & LENGTH_MASK;
-		} else
-			ch->s.rx_dropped++;
-	    }
-	} else if (error == ERR_FCS)
-	    ch->s.rx_crc_errors++;
-	else if (error == ERR_ALIGN)
-	    ch->s.rx_missed_errors++;
-	else if (error == ERR_ABT)
-	    ch->s.rx_missed_errors++;
-	else if (error == ERR_LNG)
-	    ch->s.rx_length_errors++;
-	else if (error == ERR_SHT)
-	    ch->s.rx_length_errors++;
-	FLUSH_MEM_WRITE();
-	       status = cxt1e1_max_mru;
-	if (ch->p.chan_mode == CFG_CH_PROTO_TRANS)
-	    status |= EOBIRQ_ENABLE;
-	md->status = cpu_to_le32(status);
-
-	/* Check next mdesc in the ring */
-	if (++ch->rxix_irq_srv >= ch->rxd_num)
-	    ch->rxix_irq_srv = 0;
-	FLUSH_MEM_WRITE();
-    }
+	mch_t      *ch;
+	void       *m, *m2;
+	struct mdesc *md;
+	volatile u_int32_t status;
+	u_int32_t   error;
+
+	ch = pi->chan[gchan];
+	if (!ch || ch->state != UP) {
+		if (cxt1e1_log_level > LOG_ERROR)
+			pr_info("%s: intr: receive EOM on uninitialized channel %d\n",
+				pi->up->devname, gchan);
+		return;
+	}
+	if (!ch->mdr)
+		return;                     /* can this happen ? */
+
+	for (;;) {
+		FLUSH_MEM_READ();
+		md = &ch->mdr[ch->rxix_irq_srv];
+		status = le32_to_cpu(md->status);
+		if (!(status & HOST_RX_OWNED))
+			break;                  /* Not our mdesc, done */
+		m = md->mem_token;
+		error = (status >> 16) & 0xf;
+		if (error == 0) {
+			{
+				m2 = OS_mem_token_alloc(cxt1e1_max_mru);
+				if (m2) {
+					/* substitute the mbuf+cluster */
+					md->mem_token = m2;
+					md->data = cpu_to_le32(OS_vtophys(
+							       OS_mem_token_data(m2)));
+
+					/* pass the received mbuf upward */
+					sd_recv_consume(m, status & LENGTH_MASK,
+							ch->user);
+					ch->s.rx_packets++;
+					ch->s.rx_bytes += status & LENGTH_MASK;
+				} else
+					ch->s.rx_dropped++;
+			}
+		} else if (error == ERR_FCS)
+			ch->s.rx_crc_errors++;
+		else if (error == ERR_ALIGN)
+			ch->s.rx_missed_errors++;
+		else if (error == ERR_ABT)
+			ch->s.rx_missed_errors++;
+		else if (error == ERR_LNG)
+			ch->s.rx_length_errors++;
+		else if (error == ERR_SHT)
+			ch->s.rx_length_errors++;
+		FLUSH_MEM_WRITE();
+		status = cxt1e1_max_mru;
+		if (ch->p.chan_mode == CFG_CH_PROTO_TRANS)
+			status |= EOBIRQ_ENABLE;
+		md->status = cpu_to_le32(status);
+
+		/* Check next mdesc in the ring */
+		if (++ch->rxix_irq_srv >= ch->rxd_num)
+			ch->rxix_irq_srv = 0;
+		FLUSH_MEM_WRITE();
+	}
 }
 
 
 irqreturn_t
 musycc_intr_th_handler(void *devp)
 {
-    ci_t       *ci = (ci_t *) devp;
-    volatile u_int32_t status, currInt = 0;
-    u_int32_t   nextInt, intCnt;
-
-    /*
-     * Hardware not available, potential interrupt hang.  But since interrupt
-     * might be shared, just return.
-     */
-    if (ci->state == C_INIT)
-	return IRQ_NONE;
-    /*
-     * Marked as hardware available. Don't service interrupts, just clear the
-     * event.
-     */
-
-    if (ci->state == C_IDLE) {
-	status = pci_read_32((u_int32_t *) &ci->reg->isd);
+	ci_t       *ci = (ci_t *) devp;
+	volatile u_int32_t status, currInt = 0;
+	u_int32_t   nextInt, intCnt;
 
-	/* clear the interrupt but process nothing else */
-	pci_write_32((u_int32_t *) &ci->reg->isd, status);
-	return IRQ_HANDLED;
-    }
-    FLUSH_PCI_READ();
-    FLUSH_MEM_READ();
-
-    status = pci_read_32((u_int32_t *) &ci->reg->isd);
-    nextInt = INTRPTS_NEXTINT(status);
-    intCnt = INTRPTS_INTCNT(status);
-    ci->intlog.drvr_intr_thcount++;
-
-    /*********************************************************/
-    /* HW Bug Fix                                            */
-    /* ----------                                            */
-    /* Under certain PCI Bus loading conditions, the         */
-    /* MUSYCC looses the data associated with an update      */
-    /* of its ISD and erroneously returns the immediately    */
-    /* preceding 'nextInt' value.  However, the 'intCnt'     */
-    /* value appears to be correct.  By not starting service */
-    /* where the 'missing' 'nextInt' SHOULD point causes     */
-    /* the IQD not to be serviced - the 'not serviced'       */
-    /* entries then remain and continue to increase as more  */
-    /* incorrect ISD's are encountered.                      */
-    /*********************************************************/
-
-    if (nextInt != INTRPTS_NEXTINT(ci->intlog.this_status_new)) {
-	if (cxt1e1_log_level >= LOG_MONITOR) {
-	    pr_info("%s: note - updated ISD from %08x to %08x\n",
-		    ci->devname, status,
-	      (status & (~INTRPTS_NEXTINT_M)) | ci->intlog.this_status_new);
-	}
 	/*
-	 * Replace bogus status with software corrected value.
-	 *
-	 * It's not known whether, during this problem occurrence, if the
-	 * INTFULL bit is correctly reported or not.
+	 * Hardware not available, potential interrupt hang.  But since interrupt
+	 * might be shared, just return.
 	 */
-	status = (status & (~INTRPTS_NEXTINT_M)) | (ci->intlog.this_status_new);
-	nextInt = INTRPTS_NEXTINT(status);
-    }
-    /**********************************************/
-    /* Cn847x Bug Fix                             */
-    /* --------------                             */
-    /* Fix for inability to write back same index */
-    /* as read for a full interrupt queue.        */
-    /**********************************************/
-
-    if (intCnt == INT_QUEUE_SIZE)
-	currInt = ((intCnt - 1) + nextInt) & (INT_QUEUE_SIZE - 1);
-    else
-	/************************************************/
-	/* Interrupt Write Location Issues              */
-	/* -------------------------------              */
-	/* When the interrupt status descriptor is      */
-	/* written, the interrupt line is de-asserted   */
-	/* by the Cn847x.  In the case of MIPS          */
-	/* microprocessors, this must occur at the      */
-	/* beginning of the interrupt handler so that   */
-	/* the interrupt handle is not re-entered due   */
-	/* to interrupt dis-assertion latency.          */
-	/* In the case of all other processors, this    */
-	/* action should occur at the end of the        */
-	/* interrupt handler to avoid overwriting the   */
-	/* interrupt queue.                             */
-	/************************************************/
-
-    if (intCnt)
-	currInt = (intCnt + nextInt) & (INT_QUEUE_SIZE - 1);
-    else {
+	if (ci->state == C_INIT)
+		return IRQ_NONE;
 	/*
-	 * NOTE: Servicing an interrupt whose ISD contains a count of ZERO
-	 * can be indicative of a Shared Interrupt chain.  Our driver can be
-	 * called from the system's interrupt handler as a matter of the OS
-	 * walking the chain.  As the chain is walked, the interrupt will
-	 * eventually be serviced by the correct driver/handler.
+	 * Marked as hardware available. Don't service interrupts, just clear the
+	 * event.
 	 */
-#if 0
-	/* chained interrupt = not ours */
-	pr_info(">> %s: intCnt NULL, sts %x, possibly a chained interrupt!\n",
-		ci->devname, status);
-#endif
-	return IRQ_NONE;
-    }
 
-    ci->iqp_tailx = currInt;
+	if (ci->state == C_IDLE) {
+		status = pci_read_32((u_int32_t *) &ci->reg->isd);
+
+		/* clear the interrupt but process nothing else */
+		pci_write_32((u_int32_t *) &ci->reg->isd, status);
+		return IRQ_HANDLED;
+	}
+	FLUSH_PCI_READ();
+	FLUSH_MEM_READ();
+
+	status = pci_read_32((u_int32_t *) &ci->reg->isd);
+	nextInt = INTRPTS_NEXTINT(status);
+	intCnt = INTRPTS_INTCNT(status);
+	ci->intlog.drvr_intr_thcount++;
+
+	/*********************************************************/
+	/* HW Bug Fix                                            */
+	/* ----------                                            */
+	/* Under certain PCI Bus loading conditions, the         */
+	/* MUSYCC looses the data associated with an update      */
+	/* of its ISD and erroneously returns the immediately    */
+	/* preceding 'nextInt' value.  However, the 'intCnt'     */
+	/* value appears to be correct.  By not starting service */
+	/* where the 'missing' 'nextInt' SHOULD point causes     */
+	/* the IQD not to be serviced - the 'not serviced'       */
+	/* entries then remain and continue to increase as more  */
+	/* incorrect ISD's are encountered.                      */
+	/*********************************************************/
+
+	if (nextInt != INTRPTS_NEXTINT(ci->intlog.this_status_new)) {
+		if (cxt1e1_log_level >= LOG_MONITOR) {
+			pr_info("%s: note - updated ISD from %08x to %08x\n",
+				ci->devname, status,
+				(status & (~INTRPTS_NEXTINT_M)) |
+				ci->intlog.this_status_new);
+		}
+		/*
+		 * Replace bogus status with software corrected value.
+		 *
+		 * It's not known whether, during this problem occurrence, if the
+		 * INTFULL bit is correctly reported or not.
+		 */
+		status = (status & (~INTRPTS_NEXTINT_M)) |
+			 (ci->intlog.this_status_new);
+		nextInt = INTRPTS_NEXTINT(status);
+	}
+	/**********************************************/
+	/* Cn847x Bug Fix                             */
+	/* --------------                             */
+	/* Fix for inability to write back same index */
+	/* as read for a full interrupt queue.        */
+	/**********************************************/
+
+	if (intCnt == INT_QUEUE_SIZE)
+		currInt = ((intCnt - 1) + nextInt) & (INT_QUEUE_SIZE - 1);
+	else
+		/************************************************/
+		/* Interrupt Write Location Issues              */
+		/* -------------------------------              */
+		/* When the interrupt status descriptor is      */
+		/* written, the interrupt line is de-asserted   */
+		/* by the Cn847x.  In the case of MIPS          */
+		/* microprocessors, this must occur at the      */
+		/* beginning of the interrupt handler so that   */
+		/* the interrupt handle is not re-entered due   */
+		/* to interrupt dis-assertion latency.          */
+		/* In the case of all other processors, this    */
+		/* action should occur at the end of the        */
+		/* interrupt handler to avoid overwriting the   */
+		/* interrupt queue.                             */
+		/************************************************/
+
+		if (intCnt)
+			currInt = (intCnt + nextInt) & (INT_QUEUE_SIZE - 1);
+		else {
+			/*
+			 * NOTE: Servicing an interrupt whose ISD contains a count of ZERO
+			 * can be indicative of a Shared Interrupt chain.  Our driver can be
+			 * called from the system's interrupt handler as a matter of the OS
+			 * walking the chain.  As the chain is walked, the interrupt will
+			 * eventually be serviced by the correct driver/handler.
+			 */
+			return IRQ_NONE;
+		}
+
+	ci->iqp_tailx = currInt;
 
-    currInt <<= INTRPTS_NEXTINT_S;
-    ci->intlog.last_status_new = ci->intlog.this_status_new;
-    ci->intlog.this_status_new = currInt;
+	currInt <<= INTRPTS_NEXTINT_S;
+	ci->intlog.last_status_new = ci->intlog.this_status_new;
+	ci->intlog.this_status_new = currInt;
 
-    if ((cxt1e1_log_level >= LOG_WARN) && (status & INTRPTS_INTFULL_M))
-	pr_info("%s: Interrupt queue full condition occurred\n", ci->devname);
-    if (cxt1e1_log_level >= LOG_DEBUG)
-	pr_info("%s: interrupts pending, isd @ 0x%p: %x curr %d cnt %d NEXT %d\n",
-		ci->devname, &ci->reg->isd,
-	status, nextInt, intCnt, (intCnt + nextInt) & (INT_QUEUE_SIZE - 1));
+	if ((cxt1e1_log_level >= LOG_WARN) && (status & INTRPTS_INTFULL_M))
+		pr_info("%s: Interrupt queue full condition occurred\n",
+			ci->devname);
+	if (cxt1e1_log_level >= LOG_DEBUG)
+		pr_info("%s: interrupts pending, isd @ 0x%p: %x curr %d cnt %d NEXT %d\n",
+			ci->devname, &ci->reg->isd,
+			status, nextInt, intCnt,
+			(intCnt + nextInt) & (INT_QUEUE_SIZE - 1));
 
-    FLUSH_MEM_WRITE();
+	FLUSH_MEM_WRITE();
 #if defined(SBE_ISR_TASKLET)
-    pci_write_32((u_int32_t *) &ci->reg->isd, currInt);
-    atomic_inc(&ci->bh_pending);
-    tasklet_schedule(&ci->ci_musycc_isr_tasklet);
+	pci_write_32((u_int32_t *) &ci->reg->isd, currInt);
+	atomic_inc(&ci->bh_pending);
+	tasklet_schedule(&ci->ci_musycc_isr_tasklet);
 #elif defined(SBE_ISR_IMMEDIATE)
-    pci_write_32((u_int32_t *) &ci->reg->isd, currInt);
-    atomic_inc(&ci->bh_pending);
-    queue_task(&ci->ci_musycc_isr_tq, &tq_immediate);
-    mark_bh(IMMEDIATE_BH);
+	pci_write_32((u_int32_t *) &ci->reg->isd, currInt);
+	atomic_inc(&ci->bh_pending);
+	queue_task(&ci->ci_musycc_isr_tq, &tq_immediate);
+	mark_bh(IMMEDIATE_BH);
 #elif defined(SBE_ISR_INLINE)
-    (void) musycc_intr_bh_tasklet(ci);
-    pci_write_32((u_int32_t *) &ci->reg->isd, currInt);
+	(void) musycc_intr_bh_tasklet(ci);
+	pci_write_32((u_int32_t *) &ci->reg->isd, currInt);
 #endif
-    return IRQ_HANDLED;
+	return IRQ_HANDLED;
 }
 
 
@@ -1226,574 +1207,513 @@ void
 #endif
 musycc_intr_bh_tasklet(ci_t *ci)
 {
-    mpi_t      *pi;
-    mch_t      *ch;
-    unsigned int intCnt;
-    volatile u_int32_t currInt = 0;
-    volatile unsigned int headx, tailx;
-    int         readCount, loopCount;
-    int         group, gchan, event, err, tx;
-    u_int32_t   badInt = INT_EMPTY_ENTRY;
-    u_int32_t   badInt2 = INT_EMPTY_ENTRY2;
-
-    /*
-     * Hardware not available, potential interrupt hang.  But since interrupt
-     * might be shared, just return.
-     */
-    if ((drvr_state != SBE_DRVR_AVAILABLE) || (ci->state == C_INIT)) {
+	mpi_t      *pi;
+	mch_t      *ch;
+	unsigned int intCnt;
+	volatile u_int32_t currInt = 0;
+	volatile unsigned int headx, tailx;
+	int         readCount, loopCount;
+	int         group, gchan, event, err, tx;
+	u_int32_t   badInt = INT_EMPTY_ENTRY;
+	u_int32_t   badInt2 = INT_EMPTY_ENTRY2;
+
+	/*
+	 * Hardware not available, potential interrupt hang.  But since interrupt
+	 * might be shared, just return.
+	 */
+	if ((drvr_state != SBE_DRVR_AVAILABLE) || (ci->state == C_INIT)) {
 #if defined(SBE_ISR_IMMEDIATE)
-	return 0L;
+		return 0L;
 #else
-	return;
+		return;
 #endif
-    }
+	}
 #if defined(SBE_ISR_TASKLET) || defined(SBE_ISR_IMMEDIATE)
-    if (drvr_state != SBE_DRVR_AVAILABLE) {
+	if (drvr_state != SBE_DRVR_AVAILABLE) {
 #if defined(SBE_ISR_TASKLET)
-	return;
+		return;
 #elif defined(SBE_ISR_IMMEDIATE)
-	return 0L;
+		return 0L;
 #endif
-    }
+	}
 #elif defined(SBE_ISR_INLINE)
-    /* no semaphore taken, no double checks */
+	/* no semaphore taken, no double checks */
 #endif
 
-    ci->intlog.drvr_intr_bhcount++;
-    FLUSH_MEM_READ();
-    {
-	unsigned int bh = atomic_read(&ci->bh_pending);
-
-	max_bh = max(bh, max_bh);
-    }
-    atomic_set(&ci->bh_pending, 0);/* if here, no longer pending */
-    while ((headx = ci->iqp_headx) != (tailx = ci->iqp_tailx)) {
-	intCnt = (tailx >= headx) ? (tailx - headx) : (tailx - headx + INT_QUEUE_SIZE);
-	currInt = le32_to_cpu(ci->iqd_p[headx]);
-
-	max_intcnt = max(intCnt, max_intcnt);  /* RLD DEBUG */
-
-	/**************************************************/
-	/* HW Bug Fix                                     */
-	/* ----------                                     */
-	/* The following code checks for the condition    */
-	/* of interrupt assertion before interrupt        */
-	/* queue update.  This is a problem on several    */
-	/* PCI-Local bridge chips found on some products. */
-	/**************************************************/
-
-	readCount = 0;
-	if ((currInt == badInt) || (currInt == badInt2))
-	    ci->intlog.drvr_int_failure++;
-
-	while ((currInt == badInt) || (currInt == badInt2)) {
-	    for (loopCount = 0; loopCount < 0x30; loopCount++)
-		OS_uwait_dummy();  /* use call to avoid optimization removal
-				     * of dummy delay */
-	    FLUSH_MEM_READ();
-	    currInt = le32_to_cpu(ci->iqd_p[headx]);
-	    if (readCount++ > 20)
-		break;
-	}
+	ci->intlog.drvr_intr_bhcount++;
+	FLUSH_MEM_READ();
+	{
+		unsigned int bh = atomic_read(&ci->bh_pending);
 
-	if ((currInt == badInt) || (currInt == badInt2)) {      /* catch failure of Bug
-								 * Fix checking */
-	    if (cxt1e1_log_level >= LOG_WARN)
-		pr_info("%s: Illegal Interrupt Detected @ 0x%p, mod %d.)\n",
-			ci->devname, &ci->iqd_p[headx], headx);
-
-	    /*
-	     * If the descriptor has not recovered, then leaving the EMPTY
-	     * entry set will not signal to the MUSYCC that this descriptor
-	     * has been serviced. The Interrupt Queue can then start losing
-	     * available descriptors and MUSYCC eventually encounters and
-	     * reports the INTFULL condition.  Per manual, changing any bit
-	     * marks descriptor as available, thus the use of different
-	     * EMPTY_ENTRY values.
-	     */
-
-	    if (currInt == badInt)
-		ci->iqd_p[headx] = __constant_cpu_to_le32(INT_EMPTY_ENTRY2);
-	    else
-		ci->iqd_p[headx] = __constant_cpu_to_le32(INT_EMPTY_ENTRY);
-	    ci->iqp_headx = (headx + 1) & (INT_QUEUE_SIZE - 1); /* insure wrapness */
-	    FLUSH_MEM_WRITE();
-	    FLUSH_MEM_READ();
-	    continue;
+		max_bh = max(bh, max_bh);
 	}
-	group = INTRPT_GRP(currInt);
-	gchan = INTRPT_CH(currInt);
-	event = INTRPT_EVENT(currInt);
-	err = INTRPT_ERROR(currInt);
-	tx = currInt & INTRPT_DIR_M;
-
-	ci->iqd_p[headx] = __constant_cpu_to_le32(INT_EMPTY_ENTRY);
-	FLUSH_MEM_WRITE();
+	atomic_set(&ci->bh_pending, 0);/* if here, no longer pending */
+	while ((headx = ci->iqp_headx) != (tailx = ci->iqp_tailx)) {
+		intCnt = (tailx >= headx) ? (tailx - headx) : (tailx - headx + INT_QUEUE_SIZE);
+		currInt = le32_to_cpu(ci->iqd_p[headx]);
+
+		max_intcnt = max(intCnt, max_intcnt);  /* RLD DEBUG */
+
+		/**************************************************/
+		/* HW Bug Fix                                     */
+		/* ----------                                     */
+		/* The following code checks for the condition    */
+		/* of interrupt assertion before interrupt        */
+		/* queue update.  This is a problem on several    */
+		/* PCI-Local bridge chips found on some products. */
+		/**************************************************/
+
+		readCount = 0;
+		if ((currInt == badInt) || (currInt == badInt2))
+			ci->intlog.drvr_int_failure++;
+
+		while ((currInt == badInt) || (currInt == badInt2)) {
+			for (loopCount = 0; loopCount < 0x30; loopCount++)
+				/* use call to avoid optimization
+				 * removal of dummy delay
+				 */
+				OS_uwait_dummy();
+			FLUSH_MEM_READ();
+			currInt = le32_to_cpu(ci->iqd_p[headx]);
+			if (readCount++ > 20)
+				break;
+		}
 
-	if (cxt1e1_log_level >= LOG_DEBUG) {
-	    if (err != 0)
-		pr_info(" %08x -> err: %2d,", currInt, err);
+		/* catch failure of Bug Fix checking */
+		if ((currInt == badInt) || (currInt == badInt2)) {
+			if (cxt1e1_log_level >= LOG_WARN)
+				pr_info("%s: Illegal Interrupt Detected @ 0x%p, mod %d.)\n",
+					ci->devname, &ci->iqd_p[headx], headx);
+
+			/*
+			 * If the descriptor has not recovered, then leaving the EMPTY
+			 * entry set will not signal to the MUSYCC that this descriptor
+			 * has been serviced. The Interrupt Queue can then start losing
+			 * available descriptors and MUSYCC eventually encounters and
+			 * reports the INTFULL condition.  Per manual, changing any bit
+			 * marks descriptor as available, thus the use of different
+			 * EMPTY_ENTRY values.
+			 */
+
+			if (currInt == badInt)
+				ci->iqd_p[headx] = __constant_cpu_to_le32(INT_EMPTY_ENTRY2);
+			else
+				ci->iqd_p[headx] = __constant_cpu_to_le32(INT_EMPTY_ENTRY);
+			/* insure wrapness */
+			ci->iqp_headx = (headx + 1) & (INT_QUEUE_SIZE - 1);
+			FLUSH_MEM_WRITE();
+			FLUSH_MEM_READ();
+			continue;
+		}
+		group = INTRPT_GRP(currInt);
+		gchan = INTRPT_CH(currInt);
+		event = INTRPT_EVENT(currInt);
+		err = INTRPT_ERROR(currInt);
+		tx = currInt & INTRPT_DIR_M;
 
-	    pr_info("+ interrupt event: %d, grp: %d, chan: %2d, side: %cX\n",
-		    event, group, gchan, tx ? 'T' : 'R');
-	}
-	pi = &ci->port[group];      /* notice that here we assume 1-1 group -
-				     * port mapping */
-	ch = pi->chan[gchan];
-	switch (event) {
-	case EVE_SACK:              /* Service Request Acknowledge */
-	    if (cxt1e1_log_level >= LOG_DEBUG) {
-		volatile u_int32_t r;
-
-		r = pci_read_32((u_int32_t *) &pi->reg->srd);
-		pr_info("- SACK cmd: %08x (hdw= %08x)\n", pi->sr_last, r);
-	    }
-	    SD_SEM_GIVE(&pi->sr_sem_wait);     /* wake up waiting process */
-	    break;
-	case EVE_CHABT:     /* Change To Abort Code (0x7e -> 0xff) */
-	case EVE_CHIC:              /* Change To Idle Code (0xff -> 0x7e) */
-	    break;
-	case EVE_EOM:               /* End Of Message */
-	case EVE_EOB:               /* End Of Buffer (Transparent mode) */
-	    if (tx)
-		musycc_bh_tx_eom(pi, gchan);
-	    else
-		musycc_bh_rx_eom(pi, gchan);
-#if 0
-	    break;
-#else
-	    /*
-	     * MUSYCC Interrupt Descriptor section states that EOB and EOM
-	     * can be combined with the NONE error (as well as others).  So
-	     * drop thru to catch this...
-	     */
-#endif
-	case EVE_NONE:
-	    if (err == ERR_SHT)
-		ch->s.rx_length_errors++;
-	    break;
-	default:
-	    if (cxt1e1_log_level >= LOG_WARN)
-		pr_info("%s: unexpected interrupt event: %d, iqd[%d]: %08x, port: %d\n", ci->devname,
-			event, headx, currInt, group);
-	    break;
-	}                           /* switch on event */
+		ci->iqd_p[headx] = __constant_cpu_to_le32(INT_EMPTY_ENTRY);
+		FLUSH_MEM_WRITE();
 
+		if (cxt1e1_log_level >= LOG_DEBUG) {
+			if (err != 0)
+				pr_info(" %08x -> err: %2d,", currInt, err);
 
-	/*
-	 * Per MUSYCC Manual, Section 6.4.8.3 [Transmit Errors], TX errors
-	 * are service-affecting and require action to resume normal
-	 * bit-level processing.
-	 */
+			pr_info("+ interrupt event: %d, grp: %d, chan: %2d, side: %cX\n",
+				event, group, gchan, tx ? 'T' : 'R');
+		}
+		/* notice that here we assume 1-1 group - port mapping */
+		pi = &ci->port[group];
+		ch = pi->chan[gchan];
+		switch (event) {
+		case EVE_SACK:              /* Service Request Acknowledge */
+			if (cxt1e1_log_level >= LOG_DEBUG) {
+				volatile u_int32_t r;
+
+				r = pci_read_32((u_int32_t *) &pi->reg->srd);
+				pr_info("- SACK cmd: %08x (hdw= %08x)\n",
+					pi->sr_last, r);
+			}
+			/* wake up waiting process */
+			SD_SEM_GIVE(&pi->sr_sem_wait);
+			break;
+		case EVE_CHABT: /* Change To Abort Code (0x7e -> 0xff) */
+		case EVE_CHIC:  /* Change To Idle Code (0xff -> 0x7e) */
+			break;
+		case EVE_EOM:   /* End Of Message */
+		case EVE_EOB:   /* End Of Buffer (Transparent mode) */
+			if (tx)
+				musycc_bh_tx_eom(pi, gchan);
+			else
+				musycc_bh_rx_eom(pi, gchan);
+			/*
+			 * MUSYCC Interrupt Descriptor section states that EOB and EOM
+			 * can be combined with the NONE error (as well as others).  So
+			 * drop thru to catch this...
+			 */
+		case EVE_NONE:
+			if (err == ERR_SHT)
+				ch->s.rx_length_errors++;
+			break;
+		default:
+			if (cxt1e1_log_level >= LOG_WARN)
+				pr_info("%s: unexpected interrupt event: %d, iqd[%d]: %08x, port: %d\n", ci->devname,
+					event, headx, currInt, group);
+			break;
+		}                           /* switch on event */
 
-	switch (err) {
-	case ERR_ONR:
-	    /*
-	     * Per MUSYCC manual, Section  6.4.8.3 [Transmit Errors], this
-	     * error requires Transmit channel reactivation.
-	     *
-	     * Per MUSYCC manual, Section  6.4.8.4 [Receive Errors], this error
-	     * requires Receive channel reactivation.
-	     */
-	    if (tx) {
 
 		/*
-		 * TX ONR Error only occurs when channel is configured for
-		 * Transparent Mode.  However, this code will catch and
-		 * re-activate on ANY TX ONR error.
+		 * Per MUSYCC Manual, Section 6.4.8.3 [Transmit Errors], TX errors
+		 * are service-affecting and require action to resume normal
+		 * bit-level processing.
 		 */
 
-		/*
-		 * Set flag to re-enable on any next transmit attempt.
-		 */
-		ch->ch_start_tx = CH_START_TX_ONR;
+		switch (err) {
+		case ERR_ONR:
+			/*
+			 * Per MUSYCC manual, Section  6.4.8.3 [Transmit Errors], this
+			 * error requires Transmit channel reactivation.
+			 *
+			 * Per MUSYCC manual, Section  6.4.8.4 [Receive Errors], this error
+			 * requires Receive channel reactivation.
+			 */
+			if (tx) {
+
+				/*
+				 * TX ONR Error only occurs when channel is configured for
+				 * Transparent Mode.  However, this code will catch and
+				 * re-activate on ANY TX ONR error.
+				 */
+
+				/*
+				 * Set flag to re-enable on any next transmit attempt.
+				 */
+				ch->ch_start_tx = CH_START_TX_ONR;
 
-		{
 #ifdef RLD_TRANS_DEBUG
-		    if (1 || cxt1e1_log_level >= LOG_MONITOR)
+				if (1 || cxt1e1_log_level >= LOG_MONITOR)
 #else
-		    if (cxt1e1_log_level >= LOG_MONITOR)
+				if (cxt1e1_log_level >= LOG_MONITOR)
 #endif
-		    {
-			pr_info("%s: TX buffer underflow [ONR] on channel %d, mode %x QStopped %x free %d\n",
-				ci->devname, ch->channum, ch->p.chan_mode, sd_queue_stopped(ch->user), ch->txd_free);
+				{
+					pr_info("%s: TX buffer underflow [ONR] on channel %d, mode %x QStopped %x free %d\n",
+						ci->devname, ch->channum,
+						ch->p.chan_mode,
+						sd_queue_stopped(ch->user),
+						ch->txd_free);
 #ifdef RLD_DEBUG
-			if (ch->p.chan_mode == 2) {     /* problem = ONR on HDLC
-							 * mode */
-			    pr_info("++ Failed Last %x Next %x QStopped %x, start_tx %x tx_full %d txd_free %d mode %x\n",
-				    (u_int32_t) ch->txd_irq_srv, (u_int32_t) ch->txd_usr_add,
-				    sd_queue_stopped(ch->user),
-				    ch->ch_start_tx, ch->tx_full, ch->txd_free, ch->p.chan_mode);
-			    musycc_dump_txbuffer_ring(ch, 0);
-			}
+					/* problem = ONR on HDLC mode */
+					if (ch->p.chan_mode == 2) {
+						pr_info("++ Failed Last %x Next %x QStopped %x, start_tx %x tx_full %d txd_free %d mode %x\n",
+							(u_int32_t)ch->txd_irq_srv,
+							(u_int32_t)ch->txd_usr_add,
+							sd_queue_stopped(ch->user),
+							ch->ch_start_tx,
+							ch->tx_full,
+							ch->txd_free,
+							ch->p.chan_mode);
+						musycc_dump_txbuffer_ring(ch, 0);
+					}
 #endif
-		    }
-		}
-	    } else {                 /* RX buffer overrun */
-		/*
-		 * Per MUSYCC manual, Section 6.4.8.4 [Receive Errors],
-		 * channel recovery for this RX ONR error IS required.  It is
-		 * also suggested to increase the number of receive buffers
-		 * for this channel.  Receive channel reactivation IS
-		 * required, and data has been lost.
-		 */
-		ch->s.rx_over_errors++;
-		ch->ch_start_rx = CH_START_RX_ONR;
-
-		if (cxt1e1_log_level >= LOG_WARN) {
-		    pr_info("%s: RX buffer overflow [ONR] on channel %d, mode %x\n",
-			    ci->devname, ch->channum, ch->p.chan_mode);
-		    //musycc_dump_rxbuffer_ring (ch, 0);        /* RLD DEBUG */
-		}
-	    }
-	    musycc_chan_restart(ch);
-	    break;
-	case ERR_BUF:
-	    if (tx) {
-		ch->s.tx_fifo_errors++;
-		ch->ch_start_tx = CH_START_TX_BUF;
-		/*
-		 * Per MUSYCC manual, Section  6.4.8.3 [Transmit Errors],
-		 * this BUFF error requires Transmit channel reactivation.
-		 */
-		if (cxt1e1_log_level >= LOG_MONITOR)
-		    pr_info("%s: TX buffer underrun [BUFF] on channel %d, mode %x\n",
-			    ci->devname, ch->channum, ch->p.chan_mode);
-	    } else {                 /* RX buffer overrun */
-		ch->s.rx_over_errors++;
-		/*
-		 * Per MUSYCC manual, Section 6.4.8.4 [Receive Errors], HDLC
-		 * mode requires NO recovery for this RX BUFF error is
-		 * required.  It is suggested to increase the FIFO buffer
-		 * space for this channel.  Receive channel reactivation is
-		 * not required, but data has been lost.
-		 */
-		if (cxt1e1_log_level >= LOG_WARN)
-		    pr_info("%s: RX buffer overrun [BUFF] on channel %d, mode %x\n",
-			    ci->devname, ch->channum, ch->p.chan_mode);
-		/*
-		 * Per MUSYCC manual, Section 6.4.9.4 [Receive Errors],
-		 * Transparent mode DOES require recovery for the RX BUFF
-		 * error.  It is suggested to increase the FIFO buffer space
-		 * for this channel.  Receive channel reactivation IS
-		 * required and data has been lost.
-		 */
-		if (ch->p.chan_mode == CFG_CH_PROTO_TRANS)
-		    ch->ch_start_rx = CH_START_RX_BUF;
-	    }
-
-	    if (tx || (ch->p.chan_mode == CFG_CH_PROTO_TRANS))
-		musycc_chan_restart(ch);
-	    break;
-	default:
-	    break;
-	}                           /* switch on err */
-
-	/* Check for interrupt lost condition */
-	if ((currInt & INTRPT_ILOST_M) && (cxt1e1_log_level >= LOG_ERROR))
-	    pr_info("%s: Interrupt queue overflow - ILOST asserted\n",
-		    ci->devname);
-	ci->iqp_headx = (headx + 1) & (INT_QUEUE_SIZE - 1);     /* insure wrapness */
-	FLUSH_MEM_WRITE();
-	FLUSH_MEM_READ();
-    }                               /* while */
-    if ((cxt1e1_log_level >= LOG_MONITOR2) && (ci->iqp_headx != ci->iqp_tailx)) {
-	int         bh;
-
-	bh = atomic_read(&CI->bh_pending);
-	pr_info("_bh_: late arrivals, head %d != tail %d, pending %d\n",
-		ci->iqp_headx, ci->iqp_tailx, bh);
-    }
-#if defined(SBE_ISR_IMMEDIATE)
-    return 0L;
+				}
+			} else {                 /* RX buffer overrun */
+				/*
+				 * Per MUSYCC manual, Section 6.4.8.4 [Receive Errors],
+				 * channel recovery for this RX ONR error IS required.  It is
+				 * also suggested to increase the number of receive buffers
+				 * for this channel.  Receive channel reactivation IS
+				 * required, and data has been lost.
+				 */
+				ch->s.rx_over_errors++;
+				ch->ch_start_rx = CH_START_RX_ONR;
+
+				if (cxt1e1_log_level >= LOG_WARN) {
+					pr_info("%s: RX buffer overflow [ONR] on channel %d, mode %x\n",
+						ci->devname, ch->channum,
+						ch->p.chan_mode);
+#ifdef RLD_DEBUG
+					musycc_dump_rxbuffer_ring(ch, 0);
 #endif
-    /* else, nothing returned */
-}
+				}
+			}
+			musycc_chan_restart(ch);
+			break;
+		case ERR_BUF:
+			if (tx) {
+				ch->s.tx_fifo_errors++;
+				ch->ch_start_tx = CH_START_TX_BUF;
+				/*
+				 * Per MUSYCC manual, Section  6.4.8.3 [Transmit Errors],
+				 * this BUFF error requires Transmit channel reactivation.
+				 */
+				if (cxt1e1_log_level >= LOG_MONITOR)
+					pr_info("%s: TX buffer underrun [BUFF] on channel %d, mode %x\n",
+						ci->devname, ch->channum,
+						ch->p.chan_mode);
+			} else {                 /* RX buffer overrun */
+				ch->s.rx_over_errors++;
+				/*
+				 * Per MUSYCC manual, Section 6.4.8.4 [Receive Errors], HDLC
+				 * mode requires NO recovery for this RX BUFF error is
+				 * required.  It is suggested to increase the FIFO buffer
+				 * space for this channel.  Receive channel reactivation is
+				 * not required, but data has been lost.
+				 */
+				if (cxt1e1_log_level >= LOG_WARN)
+					pr_info("%s: RX buffer overrun [BUFF] on channel %d, mode %x\n",
+						ci->devname, ch->channum,
+						ch->p.chan_mode);
+				/*
+				 * Per MUSYCC manual, Section 6.4.9.4 [Receive Errors],
+				 * Transparent mode DOES require recovery for the RX BUFF
+				 * error.  It is suggested to increase the FIFO buffer space
+				 * for this channel.  Receive channel reactivation IS
+				 * required and data has been lost.
+				 */
+				if (ch->p.chan_mode == CFG_CH_PROTO_TRANS)
+					ch->ch_start_rx = CH_START_RX_BUF;
+			}
 
-#if 0
-int         __init
-musycc_new_chan(ci_t *ci, int channum, void *user)
-{
-    mch_t      *ch;
-
-    ch = ci->port[channum / MUSYCC_NCHANS].chan[channum % MUSYCC_NCHANS];
-
-    if (ch->state != UNASSIGNED)
-	return EEXIST;
-    /* NOTE: mch_t already cleared during OS_kmalloc() */
-    ch->state = DOWN;
-    ch->user = user;
-#if 0
-    ch->status = 0;
-    ch->p.status = 0;
-    ch->p.intr_mask = 0;
+			if (tx || (ch->p.chan_mode == CFG_CH_PROTO_TRANS))
+				musycc_chan_restart(ch);
+			break;
+		default:
+			break;
+		}                           /* switch on err */
+
+		/* Check for interrupt lost condition */
+		if ((currInt & INTRPT_ILOST_M) &&
+		    (cxt1e1_log_level >= LOG_ERROR))
+			pr_info("%s: Interrupt queue overflow - ILOST asserted\n",
+				ci->devname);
+		/* insure wrapness */
+		ci->iqp_headx = (headx + 1) & (INT_QUEUE_SIZE - 1);
+		FLUSH_MEM_WRITE();
+		FLUSH_MEM_READ();
+	}                               /* while */
+	if ((cxt1e1_log_level >= LOG_MONITOR2) &&
+	    (ci->iqp_headx != ci->iqp_tailx)) {
+		int         bh;
+
+		bh = atomic_read(&CI->bh_pending);
+		pr_info("_bh_: late arrivals, head %d != tail %d, pending %d\n",
+			ci->iqp_headx, ci->iqp_tailx, bh);
+	}
+#if defined(SBE_ISR_IMMEDIATE)
+	return 0L;
 #endif
-    ch->p.chan_mode = CFG_CH_PROTO_HDLC_FCS16;
-    ch->p.idlecode = CFG_CH_FLAG_7E;
-    ch->p.pad_fill_count = 2;
-    spin_lock_init(&ch->ch_rxlock);
-    spin_lock_init(&ch->ch_txlock);
-
-    return 0;
+	/* else, nothing returned */
 }
-#endif
-
 
 #ifdef SBE_PMCC4_ENABLE
-status_t
+	status_t
 musycc_chan_down(ci_t *dummy, int channum)
 {
-    mpi_t      *pi;
-    mch_t      *ch;
-    int         i, gchan;
-
-    ch = sd_find_chan(dummy, channum);
-    if (!ch)
-	return -EINVAL;
-    pi = ch->up;
-    gchan = ch->gchan;
-
-    /* Deactivate the channel */
-    musycc_serv_req(pi, SR_CHANNEL_DEACTIVATE | SR_RX_DIRECTION | gchan);
-    ch->ch_start_rx = 0;
-    musycc_serv_req(pi, SR_CHANNEL_DEACTIVATE | SR_TX_DIRECTION | gchan);
-    ch->ch_start_tx = 0;
-
-    if (ch->state == DOWN)
-	return 0;
-    ch->state = DOWN;
-
-    pi->regram->thp[gchan] = 0;
-    pi->regram->tmp[gchan] = 0;
-    pi->regram->rhp[gchan] = 0;
-    pi->regram->rmp[gchan] = 0;
-    FLUSH_MEM_WRITE();
-    for (i = 0; i < ch->txd_num; i++)
-	if (ch->mdt[i].mem_token)
-	    OS_mem_token_free(ch->mdt[i].mem_token);
-
-    for (i = 0; i < ch->rxd_num; i++)
-	if (ch->mdr[i].mem_token)
-	    OS_mem_token_free(ch->mdr[i].mem_token);
-
-    OS_kfree(ch->mdr);
-    ch->mdr = NULL;
-    ch->rxd_num = 0;
-    OS_kfree(ch->mdt);
-    ch->mdt = NULL;
-    ch->txd_num = 0;
-
-    musycc_update_timeslots(pi);
-    c4_fifo_free(pi, ch->gchan);
-
-    pi->openchans--;
-    return 0;
-}
-#endif
+	mpi_t      *pi;
+	mch_t      *ch;
+	int         i, gchan;
+
+	ch = sd_find_chan(dummy, channum);
+	if (!ch)
+		return -EINVAL;
+	pi = ch->up;
+	gchan = ch->gchan;
+
+	/* Deactivate the channel */
+	musycc_serv_req(pi, SR_CHANNEL_DEACTIVATE | SR_RX_DIRECTION | gchan);
+	ch->ch_start_rx = 0;
+	musycc_serv_req(pi, SR_CHANNEL_DEACTIVATE | SR_TX_DIRECTION | gchan);
+	ch->ch_start_tx = 0;
 
+	if (ch->state == DOWN)
+		return 0;
+	ch->state = DOWN;
 
-#if 0
-/* TODO: determine if these functions will not be needed and can be removed */
-int
-musycc_del_chan(ci_t *ci, int channum)
-{
-    mch_t      *ch;
-
-    if ((channum < 0) || (channum >= (MUSYCC_NPORTS * MUSYCC_NCHANS)))  /* sanity chk param */
-	return ECHRNG;
-    ch = sd_find_chan(ci, channum);
-    if (!ch)
-	return ENOENT;
-    if (ch->state == UP)
-	musycc_chan_down(ci, channum);
-    ch->state = UNASSIGNED;
-    return 0;
-}
+	pi->regram->thp[gchan] = 0;
+	pi->regram->tmp[gchan] = 0;
+	pi->regram->rhp[gchan] = 0;
+	pi->regram->rmp[gchan] = 0;
+	FLUSH_MEM_WRITE();
+	for (i = 0; i < ch->txd_num; i++)
+		if (ch->mdt[i].mem_token)
+			OS_mem_token_free(ch->mdt[i].mem_token);
 
+	for (i = 0; i < ch->rxd_num; i++)
+		if (ch->mdr[i].mem_token)
+			OS_mem_token_free(ch->mdr[i].mem_token);
 
-int
-musycc_del_chan_stats(ci_t *ci, int channum)
-{
-    mch_t      *ch;
+	kfree(ch->mdr);
+	ch->mdr = NULL;
+	ch->rxd_num = 0;
+	kfree(ch->mdt);
+	ch->mdt = NULL;
+	ch->txd_num = 0;
 
-    if (channum < 0 || channum >= (MUSYCC_NPORTS * MUSYCC_NCHANS))      /* sanity chk param */
-	return ECHRNG;
-    ch = sd_find_chan(ci, channum);
-    if (!ch)
-	return ENOENT;
+	musycc_update_timeslots(pi);
+	c4_fifo_free(pi, ch->gchan);
 
-    memset(&ch->s, 0, sizeof(struct sbecom_chan_stats));
-    return 0;
+	pi->openchans--;
+	return 0;
 }
 #endif
 
-
 int
 musycc_start_xmit(ci_t *ci, int channum, void *mem_token)
 {
-    mch_t      *ch;
-    struct mdesc *md;
-    void       *m2;
-#if 0
-    unsigned long flags;
+	mch_t      *ch;
+	struct mdesc *md;
+	void       *m2;
+	int         txd_need_cnt;
+	u_int32_t   len;
+
+	ch = sd_find_chan(ci, channum);
+	if (!ch)
+		return -ENOENT;
+
+	/* full interrupt processing available */
+	if (ci->state != C_RUNNING)
+		return -EINVAL;
+	if (ch->state != UP)
+		return -EINVAL;
+
+	/* how else to flag unwritable state ? */
+	if (!(ch->status & TX_ENABLED))
+		return -EROFS;
+
+#ifdef RLD_TRANS_DEBUG
+	if (1 || cxt1e1_log_level >= LOG_MONITOR2)
+#else
+	if (cxt1e1_log_level >= LOG_MONITOR2)
 #endif
-    int         txd_need_cnt;
-    u_int32_t   len;
+	{
+		pr_info("++ start_xmt[%d]: state %x start %x full %d free %d required %d stopped %x\n",
+			channum, ch->state, ch->ch_start_tx, ch->tx_full,
+			ch->txd_free, ch->txd_required,
+			sd_queue_stopped(ch->user));
+	}
+	/***********************************************/
+	/** Determine total amount of data to be sent **/
+	/***********************************************/
+	m2 = mem_token;
+	txd_need_cnt = 0;
+	for (len = OS_mem_token_tlen(m2); len > 0;
+	     m2 = (void *) OS_mem_token_next(m2)) {
+		if (!OS_mem_token_len(m2))
+			continue;
+		txd_need_cnt++;
+		len -= OS_mem_token_len(m2);
+	}
 
-    ch = sd_find_chan(ci, channum);
-    if (!ch)
-	return -ENOENT;
+	if (txd_need_cnt == 0) {
+		if (cxt1e1_log_level >= LOG_MONITOR2)
+			pr_info("%s channel %d: no TX data in User buffer\n",
+				ci->devname, channum);
+		OS_mem_token_free(mem_token);
+		return 0;                   /* no data to send */
+	}
+	/*************************************************/
+	/** Are there sufficient descriptors available? **/
+	/*************************************************/
+	if (txd_need_cnt > ch->txd_num) { /* never enough descriptors for this
+					   * large a buffer */
+		if (cxt1e1_log_level >= LOG_DEBUG)
+			pr_info("start_xmit: discarding buffer, insufficient descriptor cnt %d, need %d.\n",
+				ch->txd_num, txd_need_cnt + 1);
+		ch->s.tx_dropped++;
+		OS_mem_token_free(mem_token);
+		return 0;
+	}
 
-    if (ci->state != C_RUNNING)     /* full interrupt processing available */
-	return -EINVAL;
-    if (ch->state != UP)
-	return -EINVAL;
+	/************************************************************/
+	/** flow control the line if not enough descriptors remain **/
+	/************************************************************/
+	if (txd_need_cnt > ch->txd_free) {
+		if (cxt1e1_log_level >= LOG_MONITOR2)
+			pr_info("start_xmit[%d]: EBUSY - need more descriptors, have %d of %d need %d\n",
+				channum, ch->txd_free,
+				ch->txd_num, txd_need_cnt);
+		ch->tx_full = 1;
+		ch->txd_required = txd_need_cnt;
+		sd_disable_xmit(ch->user);
+		return -EBUSY;               /* tell user to try again later */
+	}
+	/**************************************************/
+	/** Put the user data into MUSYCC data buffer(s) **/
+	/**************************************************/
+	m2 = mem_token;
+	md = ch->txd_usr_add;           /* get current available descriptor */
 
-    if (!(ch->status & TX_ENABLED))
-	return -EROFS;               /* how else to flag unwritable state ? */
+	for (len = OS_mem_token_tlen(m2); len > 0; m2 = OS_mem_token_next(m2)) {
+		int         u = OS_mem_token_len(m2);
 
-#ifdef RLD_TRANS_DEBUGx
-    if (1 || cxt1e1_log_level >= LOG_MONITOR2)
-#else
-    if (cxt1e1_log_level >= LOG_MONITOR2)
-#endif
-    {
-	pr_info("++ start_xmt[%d]: state %x start %x full %d free %d required %d stopped %x\n",
-		channum, ch->state, ch->ch_start_tx, ch->tx_full,
-		ch->txd_free, ch->txd_required, sd_queue_stopped(ch->user));
-    }
-    /***********************************************/
-    /** Determine total amount of data to be sent **/
-    /***********************************************/
-    m2 = mem_token;
-    txd_need_cnt = 0;
-    for (len = OS_mem_token_tlen(m2); len > 0;
-	 m2 = (void *) OS_mem_token_next(m2)) {
-	if (!OS_mem_token_len(m2))
-	    continue;
-	txd_need_cnt++;
-	len -= OS_mem_token_len(m2);
-    }
-
-    if (txd_need_cnt == 0) {
-	if (cxt1e1_log_level >= LOG_MONITOR2)
-	    pr_info("%s channel %d: no TX data in User buffer\n", ci->devname, channum);
-	OS_mem_token_free(mem_token);
-	return 0;                   /* no data to send */
-    }
-    /*************************************************/
-    /** Are there sufficient descriptors available? **/
-    /*************************************************/
-    if (txd_need_cnt > ch->txd_num) { /* never enough descriptors for this
-				       * large a buffer */
-	if (cxt1e1_log_level >= LOG_DEBUG)
-	    pr_info("start_xmit: discarding buffer, insufficient descriptor cnt %d, need %d.\n",
-		    ch->txd_num, txd_need_cnt + 1);
-	ch->s.tx_dropped++;
-	OS_mem_token_free(mem_token);
-	return 0;
-    }
-#if 0
-    spin_lock_irqsave(&ch->ch_txlock, flags);
-#endif
-    /************************************************************/
-    /** flow control the line if not enough descriptors remain **/
-    /************************************************************/
-    if (txd_need_cnt > ch->txd_free) {
-	if (cxt1e1_log_level >= LOG_MONITOR2)
-	    pr_info("start_xmit[%d]: EBUSY - need more descriptors, have %d of %d need %d\n",
-		    channum, ch->txd_free, ch->txd_num, txd_need_cnt);
-	ch->tx_full = 1;
-	ch->txd_required = txd_need_cnt;
-	sd_disable_xmit(ch->user);
-#if 0
-	spin_unlock_irqrestore(&ch->ch_txlock, flags);
-#endif
-	return -EBUSY;               /* tell user to try again later */
-    }
-    /**************************************************/
-    /** Put the user data into MUSYCC data buffer(s) **/
-    /**************************************************/
-    m2 = mem_token;
-    md = ch->txd_usr_add;           /* get current available descriptor */
+		if (!u)
+			continue;
+		len -= u;
+
+		/*
+		 * Enable following chunks, yet wait to enable the FIRST chunk until
+		 * after ALL subsequent chunks are setup.
+		 */
+		if (md != ch->txd_usr_add)  /* not first chunk */
+			/* transfer ownership from HOST to MUSYCC */
+			u |= MUSYCC_TX_OWNED;
+
+		if (len)                    /* not last chunk */
+			u |= EOBIRQ_ENABLE;
+		else if (ch->p.chan_mode == CFG_CH_PROTO_TRANS) {
+			/*
+			 * Per MUSYCC Ref 6.4.9 for Transparent Mode, the host must
+			 * always clear EOMIRQ_ENABLE in every Transmit Buffer Descriptor
+			 * (IE. don't set herein).
+			 */
+			u |= EOBIRQ_ENABLE;
+		} else
+			u |= EOMIRQ_ENABLE;     /* EOM, last HDLC chunk */
+
+
+		/* last chunk in hdlc mode */
+		u |= (ch->p.idlecode << IDLE_CODE);
+		if (ch->p.pad_fill_count) {
+			u |= (PADFILL_ENABLE | (ch->p.pad_fill_count << EXTRA_FLAGS));
+		}
+		/* Fill in mds on last segment, others set ZERO
+		 * so that entire token is removed ONLY when ALL
+		 * segments have been transmitted.
+		 */
+		md->mem_token = len ? NULL : mem_token;
 
-    for (len = OS_mem_token_tlen(m2); len > 0; m2 = OS_mem_token_next(m2)) {
-	int         u = OS_mem_token_len(m2);
+		md->data = cpu_to_le32(OS_vtophys(OS_mem_token_data(m2)));
+		FLUSH_MEM_WRITE();
+		md->status = cpu_to_le32(u);
+		--ch->txd_free;
+		md = md->snext;
+	}
+	FLUSH_MEM_WRITE();
 
-	if (!u)
-	    continue;
-	len -= u;
 
 	/*
-	 * Enable following chunks, yet wait to enable the FIRST chunk until
-	 * after ALL subsequent chunks are setup.
+	 * Now transfer ownership of first chunk from HOST to MUSYCC in order to
+	 * fire-off this XMIT.
 	 */
-	if (md != ch->txd_usr_add)  /* not first chunk */
-	    u |= MUSYCC_TX_OWNED;   /* transfer ownership from HOST to MUSYCC */
-
-	if (len)                    /* not last chunk */
-	    u |= EOBIRQ_ENABLE;
-	else if (ch->p.chan_mode == CFG_CH_PROTO_TRANS)	{
-	    /*
-	     * Per MUSYCC Ref 6.4.9 for Transparent Mode, the host must
-	     * always clear EOMIRQ_ENABLE in every Transmit Buffer Descriptor
-	     * (IE. don't set herein).
-	     */
-	    u |= EOBIRQ_ENABLE;
-	} else
-	    u |= EOMIRQ_ENABLE;     /* EOM, last HDLC chunk */
-
-
-	/* last chunk in hdlc mode */
-	u |= (ch->p.idlecode << IDLE_CODE);
-	if (ch->p.pad_fill_count) {
-#if 0
-	    /* NOOP NOTE: u_int8_t cannot be > 0xFF */
-	    /* sanitize pad_fill_count for maximums allowed by hardware */
-	    if (ch->p.pad_fill_count > EXTRA_FLAGS_MASK)
-		ch->p.pad_fill_count = EXTRA_FLAGS_MASK;
-#endif
-	    u |= (PADFILL_ENABLE | (ch->p.pad_fill_count << EXTRA_FLAGS));
-	}
-	md->mem_token = len ? NULL : mem_token;    /* Fill in mds on last
-						 * segment, others set ZERO
-						 * so that entire token is
-						 * removed ONLY when ALL
-						 * segments have been
-						 * transmitted. */
-
-	md->data = cpu_to_le32(OS_vtophys(OS_mem_token_data(m2)));
+	ch->txd_usr_add->status |= __constant_cpu_to_le32(MUSYCC_TX_OWNED);
 	FLUSH_MEM_WRITE();
-	md->status = cpu_to_le32(u);
-	--ch->txd_free;
-	md = md->snext;
-    }
-    FLUSH_MEM_WRITE();
-
-
-    /*
-     * Now transfer ownership of first chunk from HOST to MUSYCC in order to
-     * fire-off this XMIT.
-     */
-    ch->txd_usr_add->status |= __constant_cpu_to_le32(MUSYCC_TX_OWNED);
-    FLUSH_MEM_WRITE();
-    ch->txd_usr_add = md;
-
-    len = OS_mem_token_tlen(mem_token);
-    atomic_add(len, &ch->tx_pending);
-    atomic_add(len, &ci->tx_pending);
-    ch->s.tx_packets++;
-    ch->s.tx_bytes += len;
-    /*
-     * If an ONR was seen, then channel requires poking to restart
-     * transmission.
-     */
-    if (ch->ch_start_tx)
-	musycc_chan_restart(ch);
+	ch->txd_usr_add = md;
+
+	len = OS_mem_token_tlen(mem_token);
+	atomic_add(len, &ch->tx_pending);
+	atomic_add(len, &ci->tx_pending);
+	ch->s.tx_packets++;
+	ch->s.tx_bytes += len;
+	/*
+	 * If an ONR was seen, then channel requires poking to restart
+	 * transmission.
+	 */
+	if (ch->ch_start_tx)
+		musycc_chan_restart(ch);
 #ifdef SBE_WAN256T3_ENABLE
-    wan256t3_led(ci, LED_TX, LEDV_G);
+	wan256t3_led(ci, LED_TX, LEDV_G);
 #endif
-    return 0;
+	return 0;
 }
 
 
diff --git a/drivers/staging/cxt1e1/pmc93x6_eeprom.c b/drivers/staging/cxt1e1/pmc93x6_eeprom.c
index 78cc1709ebd..ba588f1b211 100644
--- a/drivers/staging/cxt1e1/pmc93x6_eeprom.c
+++ b/drivers/staging/cxt1e1/pmc93x6_eeprom.c
@@ -28,6 +28,7 @@
 #include "sbecom_inline_linux.h"
 #include "pmcc4.h"
 #include "sbe_promformat.h"
+#include "pmc93x6_eeprom.h"
 
 #ifndef TRUE
 #define TRUE   1
@@ -43,38 +44,35 @@
  *      using.
  */
 
-#define EE_MFG      (long)0     /* Index to manufacturing record */
-#define EE_FIRST    0x28        /* Index to start testing at */
-#define EE_LIMIT    128         /* Index to end testing at */
-
+#define EE_MFG      (long)0	/* Index to manufacturing record */
+#define EE_FIRST    0x28	/* Index to start testing at */
+#define EE_LIMIT    128		/* Index to end testing at */
 
 /*  Bit Ordering for Instructions
-**
-**  A0, A1, A2, A3, A4, A5, A6, OP0, OP1, SB   (lsb, or 1st bit out)
-**
-*/
-
-#define EPROM_EWEN      0x0019  /* Erase/Write enable (reversed) */
-#define EPROM_EWDS      0x0001  /* Erase/Write disable (reversed) */
-#define EPROM_READ      0x0003  /* Read (reversed) */
-#define EPROM_WRITE     0x0005  /* Write (reversed) */
-#define EPROM_ERASE     0x0007  /* Erase (reversed) */
-#define EPROM_ERAL      0x0009  /* Erase All (reversed) */
-#define EPROM_WRAL      0x0011  /* Write All (reversed) */
-
-#define EPROM_ADR_SZ    7       /* Number of bits in offset address */
-#define EPROM_OP_SZ     3       /* Number of bits in command */
-#define SIZE_ADDR_OP    (EPROM_ADR_SZ + EPROM_OP_SZ)
-#define LC46A_MAX_OPS   10      /* Number of bits in Instruction */
-#define NUM_OF_BITS     8       /* Number of bits in data */
+ *
+ *  A0, A1, A2, A3, A4, A5, A6, OP0, OP1, SB   (lsb, or 1st bit out)
+ *
+ */
 
+#define EPROM_EWEN      0x0019	/* Erase/Write enable (reversed) */
+#define EPROM_EWDS      0x0001	/* Erase/Write disable (reversed) */
+#define EPROM_READ      0x0003	/* Read (reversed) */
+#define EPROM_WRITE     0x0005	/* Write (reversed) */
+#define EPROM_ERASE     0x0007	/* Erase (reversed) */
+#define EPROM_ERAL      0x0009	/* Erase All (reversed) */
+#define EPROM_WRAL      0x0011	/* Write All (reversed) */
 
-/* EEPROM signal bits */
-#define EPROM_ACTIVE_OUT_BIT    0x0001  /* Out data bit */
-#define EPROM_ACTIVE_IN_BIT     0x0002  /* In data bit */
-#define ACTIVE_IN_BIT_SHIFT     0x0001  /* Shift In data bit to LSB */
-#define EPROM_ENCS              0x0004  /* Set EEPROM CS during operation */
+#define EPROM_ADR_SZ    7	/* Number of bits in offset address */
+#define EPROM_OP_SZ     3	/* Number of bits in command */
+#define SIZE_ADDR_OP    (EPROM_ADR_SZ + EPROM_OP_SZ)
+#define LC46A_MAX_OPS   10	/* Number of bits in Instruction */
+#define NUM_OF_BITS     8	/* Number of bits in data */
 
+/* EEPROM signal bits */
+#define EPROM_ACTIVE_OUT_BIT    0x0001	/* Out data bit */
+#define EPROM_ACTIVE_IN_BIT     0x0002	/* In data bit */
+#define ACTIVE_IN_BIT_SHIFT     0x0001	/* Shift In data bit to LSB */
+#define EPROM_ENCS              0x0004	/* Set EEPROM CS during operation */
 
 /*------------------------------------------------------------------------
  *      The ByteReverse table is used to reverses the 8 bits within a byte
@@ -82,29 +80,26 @@
  */
 
 static unsigned char ByteReverse[256];
-static int  ByteReverseBuilt = FALSE;
-
+static int ByteReverseBuilt = FALSE;
 
 /*------------------------------------------------------------------------
  *      mfg_template - initial serial EEPROM data structure
  *------------------------------------------------------------------------
  */
 
-static u8 mfg_template[sizeof(FLD_TYPE2)] =
-{
-    PROM_FORMAT_TYPE2,          /* type; */
-    0x00, 0x1A,                 /* length[2]; */
-    0x00, 0x00, 0x00, 0x00,     /* Crc32[4]; */
-    0x11, 0x76,                 /* Id[2]; */
-    0x07, 0x05,                 /* SubId[2] E1; */
-    0x00, 0xA0, 0xD6, 0x00, 0x00, 0x00, /* Serial[6]; */
-    0x00, 0x00, 0x00, 0x00,     /* CreateTime[4]; */
-    0x00, 0x00, 0x00, 0x00,     /* HeatRunTime[4]; */
-    0x00, 0x00, 0x00, 0x00,     /* HeatRunIterations[4]; */
-    0x00, 0x00, 0x00, 0x00,     /* HeatRunErrors[4]; */
+static u8 mfg_template[sizeof(FLD_TYPE2)] = {
+	PROM_FORMAT_TYPE2,	/* type; */
+	0x00, 0x1A,		/* length[2]; */
+	0x00, 0x00, 0x00, 0x00,	/* Crc32[4]; */
+	0x11, 0x76,		/* Id[2]; */
+	0x07, 0x05,		/* SubId[2] E1; */
+	0x00, 0xA0, 0xD6, 0x00, 0x00, 0x00,	/* Serial[6]; */
+	0x00, 0x00, 0x00, 0x00,	/* CreateTime[4]; */
+	0x00, 0x00, 0x00, 0x00,	/* HeatRunTime[4]; */
+	0x00, 0x00, 0x00, 0x00,	/* HeatRunIterations[4]; */
+	0x00, 0x00, 0x00, 0x00,	/* HeatRunErrors[4]; */
 };
 
-
 /*------------------------------------------------------------------------
  *      BuildByteReverse - build the 8-bit reverse table
  *------------------------------------------------------------------------
@@ -113,39 +108,35 @@ static u8 mfg_template[sizeof(FLD_TYPE2)] =
  *      (the MSB becomes the LSB etc.).
  */
 
-static void
-BuildByteReverse (void)
+static void BuildByteReverse(void)
 {
-    long        half;           /* Used to build by powers to 2 */
-    int         i;
+	/* Used to build by powers to 2 */
+	long half;
+	int i;
 
-    ByteReverse[0] = 0;
+	ByteReverse[0] = 0;
 
-    for (half = 1; half < sizeof (ByteReverse); half <<= 1)
-        for (i = 0; i < half; i++)
-            ByteReverse[half + i] = (char) (ByteReverse[i] | (0x80 / half));
+	for (half = 1; half < sizeof(ByteReverse); half <<= 1)
+		for (i = 0; i < half; i++)
+			ByteReverse[half + i] =
+			    (char)(ByteReverse[i] | (0x80 / half));
 
-    ByteReverseBuilt = TRUE;
+	ByteReverseBuilt = TRUE;
 }
 
-
 /*------------------------------------------------------------------------
  *      eeprom_delay - small delay for EEPROM timing
  *------------------------------------------------------------------------
  */
 
-static void
-eeprom_delay (void)
+static void eeprom_delay(void)
 {
-    int         timeout;
+	int timeout;
 
-    for (timeout = 20; timeout; --timeout)
-    {
-        OS_uwait_dummy ();
-    }
+	for (timeout = 20; timeout; --timeout)
+		OS_uwait_dummy();
 }
 
-
 /*------------------------------------------------------------------------
  *      eeprom_put_byte - Send a byte to the EEPROM serially
  *------------------------------------------------------------------------
@@ -154,23 +145,23 @@ eeprom_delay (void)
  *      the data to the EEPROM.
  */
 
-void
-eeprom_put_byte (long addr, long data, int count)
+static void eeprom_put_byte(long addr, long data, int count)
 {
-    u_int32_t output;
-
-    while (--count >= 0)
-    {
-        output = (data & EPROM_ACTIVE_OUT_BIT) ? 1 : 0; /* Get next data bit */
-        output |= EPROM_ENCS;       /* Add Chip Select */
-        data >>= 1;
-
-        eeprom_delay ();
-        pci_write_32 ((u_int32_t *) addr, output);      /* Output it */
-    }
+	u_int32_t output;
+
+	while (--count >= 0) {
+		/* Get next data bit */
+		output = (data & EPROM_ACTIVE_OUT_BIT) ? 1 : 0;
+		/* Add Chip Select */
+		output |= EPROM_ENCS;
+		data >>= 1;
+
+		eeprom_delay();
+		/* Output it */
+		pci_write_32((u_int32_t *) addr, output);
+	}
 }
 
-
 /*------------------------------------------------------------------------
  *      eeprom_get_byte - Receive a byte from the EEPROM serially
  *------------------------------------------------------------------------
@@ -179,37 +170,35 @@ eeprom_put_byte (long addr, long data, int count)
  *      from the  EEPROM.
  */
 
-u_int32_t
-eeprom_get_byte (long addr)
+static u_int32_t eeprom_get_byte(long addr)
 {
-    u_int32_t   input;
-    u_int32_t   data;
-    int         count;
+	u_int32_t input;
+	u_int32_t data;
+	int count;
 
 /*  Start the Reading of DATA
-**
-**  The first read is a dummy as the data is latched in the
-**  EPLD and read on the next read access to the EEPROM.
-*/
+ *
+ *  The first read is a dummy as the data is latched in the
+ *  EPLD and read on the next read access to the EEPROM.
+ */
 
-    input = pci_read_32 ((u_int32_t *) addr);
+	input = pci_read_32((u_int32_t *) addr);
 
-    data = 0;
-    count = NUM_OF_BITS;
-    while (--count >= 0)
-    {
-        eeprom_delay ();
-        input = pci_read_32 ((u_int32_t *) addr);
+	data = 0;
+	count = NUM_OF_BITS;
+	while (--count >= 0) {
+		eeprom_delay();
+		input = pci_read_32((u_int32_t *) addr);
 
-        data <<= 1;                 /* Shift data over */
-        data |= (input & EPROM_ACTIVE_IN_BIT) ? 1 : 0;
+		/* Shift data over */
+		data <<= 1;
+		data |= (input & EPROM_ACTIVE_IN_BIT) ? 1 : 0;
 
-    }
+	}
 
-    return data;
+	return data;
 }
 
-
 /*------------------------------------------------------------------------
  *      disable_pmc_eeprom - Disable writes to the EEPROM
  *------------------------------------------------------------------------
@@ -217,16 +206,14 @@ eeprom_get_byte (long addr)
  *      Issue the EEPROM command to disable writes.
  */
 
-static void
-disable_pmc_eeprom (long addr)
+static void disable_pmc_eeprom(long addr)
 {
-    eeprom_put_byte (addr, EPROM_EWDS, SIZE_ADDR_OP);
+	eeprom_put_byte(addr, EPROM_EWDS, SIZE_ADDR_OP);
 
-    pci_write_32 ((u_int32_t *) addr, 0);       /* this removes Chip Select
-                                                 * from EEPROM */
+	/* this removes Chip Select from EEPROM */
+	pci_write_32((u_int32_t *) addr, 0);
 }
 
-
 /*------------------------------------------------------------------------
  *      enable_pmc_eeprom - Enable writes to the EEPROM
  *------------------------------------------------------------------------
@@ -234,16 +221,14 @@ disable_pmc_eeprom (long addr)
  *      Issue the EEPROM command to enable writes.
  */
 
-static void
-enable_pmc_eeprom (long addr)
+static void enable_pmc_eeprom(long addr)
 {
-    eeprom_put_byte (addr, EPROM_EWEN, SIZE_ADDR_OP);
+	eeprom_put_byte(addr, EPROM_EWEN, SIZE_ADDR_OP);
 
-    pci_write_32 ((u_int32_t *) addr, 0);       /* this removes Chip Select
-                                                 * from EEPROM */
+	/* this removes Chip Select from EEPROM */
+	pci_write_32((u_int32_t *) addr, 0);
 }
 
-
 /*------------------------------------------------------------------------
  *      pmc_eeprom_read - EEPROM location read
  *------------------------------------------------------------------------
@@ -252,34 +237,40 @@ enable_pmc_eeprom (long addr)
  *      the contents of the specified location to the calling routine.
  */
 
-u_int32_t
-pmc_eeprom_read (long addr, long mem_offset)
+static u_int32_t pmc_eeprom_read(long addr, long mem_offset)
 {
-    u_int32_t   data;           /* Data from chip */
+	/* Data from chip */
+	u_int32_t data;
 
-    if (!ByteReverseBuilt)
-        BuildByteReverse ();
+	if (!ByteReverseBuilt)
+		BuildByteReverse();
 
-    mem_offset = ByteReverse[0x7F & mem_offset];        /* Reverse address */
-    /*
-     * NOTE: The max offset address is 128 or half the reversal table. So the
-     * LSB is always zero and counts as a built in shift of one bit.  So even
-     * though we need to shift 3 bits to make room for the command, we only
-     * need to shift twice more because of the built in shift.
-     */
-    mem_offset <<= 2;               /* Shift for command */
-    mem_offset |= EPROM_READ;       /* Add command */
+	/* Reverse address */
+	mem_offset = ByteReverse[0x7F & mem_offset];
 
-    eeprom_put_byte (addr, mem_offset, SIZE_ADDR_OP);   /* Output chip address */
+	/*
+	 * NOTE: The max offset address is 128 or half the reversal table. So
+	 * the LSB is always zero and counts as a built in shift of one bit.
+	 * So even though we need to shift 3 bits to make room for the command,
+	 * we only need to shift twice more because of the built in shift.
+	 */
 
-    data = eeprom_get_byte (addr);  /* Read chip data */
+	/* Shift for command */
+	mem_offset <<= 2;
+	/* Add command */
+	mem_offset |= EPROM_READ;
 
-    pci_write_32 ((u_int32_t *) addr, 0);       /* Remove Chip Select from
-                                                 * EEPROM */
+	/* Output chip address */
+	eeprom_put_byte(addr, mem_offset, SIZE_ADDR_OP);
 
-    return (data & 0x000000FF);
-}
+	/* Read chip data */
+	data = eeprom_get_byte(addr);
+
+	/* Remove Chip Select from EEPROM */
+	pci_write_32((u_int32_t *) addr, 0);
 
+	return (data & 0x000000FF);
+}
 
 /*------------------------------------------------------------------------
  *      pmc_eeprom_write - EEPROM location write
@@ -292,189 +283,181 @@ pmc_eeprom_read (long addr, long mem_offset)
  *      operation succeeded.
  */
 
-int
-pmc_eeprom_write (long addr, long mem_offset, u_int32_t data)
+static int pmc_eeprom_write(long addr, long mem_offset, u_int32_t data)
 {
-    volatile u_int32_t temp;
-    int         count;
+	u_int32_t temp;
+	int count;
+
+	if (!ByteReverseBuilt)
+		BuildByteReverse();
+
+	/* Reverse address */
+	mem_offset = ByteReverse[0x7F & mem_offset];
 
-    if (!ByteReverseBuilt)
-        BuildByteReverse ();
+	/*
+	 * NOTE: The max offset address is 128 or half the reversal table. So
+	 * the LSB is always zero and counts as a built in shift of one bit.
+	 * So even though we need to shift 3 bits to make room for the command,
+	 * we only need to shift twice more because of the built in shift.
+	 */
 
-    mem_offset = ByteReverse[0x7F & mem_offset];        /* Reverse address */
-    /*
-     * NOTE: The max offset address is 128 or half the reversal table. So the
-     * LSB is always zero and counts as a built in shift of one bit.  So even
-     * though we need to shift 3 bits to make room for the command, we only
-     * need to shift twice more because of the built in shift.
-     */
-    mem_offset <<= 2;               /* Shift for command */
-    mem_offset |= EPROM_WRITE;      /* Add command */
+	/* Shift for command */
+	mem_offset <<= 2;
+	/* Add command */
+	mem_offset |= EPROM_WRITE;
 
-    eeprom_put_byte (addr, mem_offset, SIZE_ADDR_OP);   /* Output chip address */
+	/* Output chip address */
+	eeprom_put_byte(addr, mem_offset, SIZE_ADDR_OP);
 
-    data = ByteReverse[0xFF & data];/* Reverse data */
-    eeprom_put_byte (addr, data, NUM_OF_BITS);  /* Output chip data */
+	/* Reverse data */
+	data = ByteReverse[0xFF & data];
+	/* Output chip data */
+	eeprom_put_byte(addr, data, NUM_OF_BITS);
 
-    pci_write_32 ((u_int32_t *) addr, 0);       /* Remove Chip Select from
-                                                 * EEPROM */
+	/* Remove Chip Select from EEPROM */
+	pci_write_32((u_int32_t *) addr, 0);
 
 /*
-**  Must see Data In at a low state before completing this transaction.
-**
-**  Afterwards, the data bit will return to a high state, ~6 ms, terminating
-**  the operation.
-*/
-    pci_write_32 ((u_int32_t *) addr, EPROM_ENCS);      /* Re-enable Chip Select */
-    temp = pci_read_32 ((u_int32_t *) addr);    /* discard first read */
-    temp = pci_read_32 ((u_int32_t *) addr);
-    if (temp & EPROM_ACTIVE_IN_BIT)
-    {
-        temp = pci_read_32 ((u_int32_t *) addr);
-        if (temp & EPROM_ACTIVE_IN_BIT)
-        {
-            pci_write_32 ((u_int32_t *) addr, 0);       /* Remove Chip Select
-                                                         * from EEPROM */
-            return (1);
-        }
-    }
-    count = 1000;
-    while (count--)
-    {
-        for (temp = 0; temp < 0x10; temp++)
-            OS_uwait_dummy ();
-
-        if (pci_read_32 ((u_int32_t *) addr) & EPROM_ACTIVE_IN_BIT)
-            break;
-    }
-
-    if (count == -1)
-        return (2);
-
-    return (0);
+ *  Must see Data In at a low state before completing this transaction.
+ *
+ *  Afterwards, the data bit will return to a high state, ~6 ms, terminating
+ *  the operation.
+ */
+	/* Re-enable Chip Select */
+	pci_write_32((u_int32_t *) addr, EPROM_ENCS);
+	/* discard first read */
+	temp = pci_read_32((u_int32_t *) addr);
+	temp = pci_read_32((u_int32_t *) addr);
+	if (temp & EPROM_ACTIVE_IN_BIT) {
+		temp = pci_read_32((u_int32_t *) addr);
+		if (temp & EPROM_ACTIVE_IN_BIT) {
+			/* Remove Chip Select from EEPROM */
+			pci_write_32((u_int32_t *) addr, 0);
+			return 1;
+		}
+	}
+	count = 1000;
+	while (count--) {
+		for (temp = 0; temp < 0x10; temp++)
+			OS_uwait_dummy();
+
+		if (pci_read_32((u_int32_t *) addr) & EPROM_ACTIVE_IN_BIT)
+			break;
+	}
+
+	if (count == -1)
+		return 2;
+
+	return 0;
 }
 
-
 /*------------------------------------------------------------------------
  *      pmcGetBuffValue - read the specified value from buffer
  *------------------------------------------------------------------------
  */
 
-long
-pmcGetBuffValue (char *ptr, int size)
+static long pmcGetBuffValue(char *ptr, int size)
 {
-    long        value = 0;
-    int         index;
+	long value = 0;
+	int index;
 
-    for (index = 0; index < size; ++index)
-    {
-        value <<= 8;
-        value |= ptr[index] & 0xFF;
-    }
+	for (index = 0; index < size; ++index) {
+		value <<= 8;
+		value |= ptr[index] & 0xFF;
+	}
 
-    return value;
+	return value;
 }
 
-
 /*------------------------------------------------------------------------
  *      pmcSetBuffValue - save the specified value to buffer
  *------------------------------------------------------------------------
  */
 
-void
-pmcSetBuffValue (char *ptr, long value, int size)
+static void pmcSetBuffValue(char *ptr, long value, int size)
 {
-    int         index = size;
+	int index = size;
 
-    while (--index >= 0)
-    {
-        ptr[index] = (char) (value & 0xFF);
-        value >>= 8;
-    }
+	while (--index >= 0) {
+		ptr[index] = (char)(value & 0xFF);
+		value >>= 8;
+	}
 }
 
-
 /*------------------------------------------------------------------------
  *      pmc_eeprom_read_buffer - read EEPROM data into specified buffer
  *------------------------------------------------------------------------
  */
 
 void
-pmc_eeprom_read_buffer (long addr, long mem_offset, char *dest_ptr, int size)
+pmc_eeprom_read_buffer(long addr, long mem_offset, char *dest_ptr, int size)
 {
-    while (--size >= 0)
-        *dest_ptr++ = (char) pmc_eeprom_read (addr, mem_offset++);
+	while (--size >= 0)
+		*dest_ptr++ = (char)pmc_eeprom_read(addr, mem_offset++);
 }
 
-
 /*------------------------------------------------------------------------
  *      pmc_eeprom_write_buffer - write EEPROM data from specified buffer
  *------------------------------------------------------------------------
  */
 
 void
-pmc_eeprom_write_buffer (long addr, long mem_offset, char *dest_ptr, int size)
+pmc_eeprom_write_buffer(long addr, long mem_offset, char *dest_ptr, int size)
 {
-    enable_pmc_eeprom (addr);
+	enable_pmc_eeprom(addr);
 
-    while (--size >= 0)
-        pmc_eeprom_write (addr, mem_offset++, *dest_ptr++);
+	while (--size >= 0)
+		pmc_eeprom_write(addr, mem_offset++, *dest_ptr++);
 
-    disable_pmc_eeprom (addr);
+	disable_pmc_eeprom(addr);
 }
 
-
 /*------------------------------------------------------------------------
  *      pmcCalcCrc - calculate the CRC for the serial EEPROM structure
  *------------------------------------------------------------------------
  */
 
-u_int32_t
-pmcCalcCrc_T01 (void *bufp)
+static u_int32_t pmcCalcCrc_T01(void *bufp)
 {
-    FLD_TYPE2  *buf = bufp;
-    u_int32_t   crc;            /* CRC of the structure */
+	FLD_TYPE2 *buf = bufp;
+	/* CRC of the structure */
+	u_int32_t crc;
 
-    /* Calc CRC for type and length fields */
-    sbeCrc (
-            (u_int8_t *) &buf->type,
-            (u_int32_t) STRUCT_OFFSET (FLD_TYPE1, Crc32),
-            (u_int32_t) 0,
-            (u_int32_t *) &crc);
+	/* Calc CRC for type and length fields */
+	sbeCrc((u_int8_t *) &buf->type,
+	       (u_int32_t) STRUCT_OFFSET(FLD_TYPE1, Crc32),
+	       (u_int32_t) 0, (u_int32_t *) &crc);
 
 #ifdef EEPROM_TYPE_DEBUG
-    pr_info("sbeCrc: crc 1 calculated as %08x\n", crc); /* RLD DEBUG */
+	/* RLD DEBUG */
+	pr_info("sbeCrc: crc 1 calculated as %08x\n", crc);
 #endif
-    return ~crc;
+	return ~crc;
 }
 
-u_int32_t
-pmcCalcCrc_T02 (void *bufp)
+static u_int32_t pmcCalcCrc_T02(void *bufp)
 {
-    FLD_TYPE2  *buf = bufp;
-    u_int32_t   crc;            /* CRC of the structure */
-
-    /* Calc CRC for type and length fields */
-    sbeCrc (
-            (u_int8_t *) &buf->type,
-            (u_int32_t) STRUCT_OFFSET (FLD_TYPE2, Crc32),
-            (u_int32_t) 0,
-            (u_int32_t *) &crc);
-
-    /* Calc CRC for remaining fields */
-    sbeCrc (
-            (u_int8_t *) &buf->Id[0],
-            (u_int32_t) (sizeof (FLD_TYPE2) - STRUCT_OFFSET (FLD_TYPE2, Id)),
-            (u_int32_t) crc,
-            (u_int32_t *) &crc);
+	FLD_TYPE2 *buf = bufp;
+	/* CRC of the structure */
+	u_int32_t crc;
+
+	/* Calc CRC for type and length fields */
+	sbeCrc((u_int8_t *) &buf->type,
+	       (u_int32_t) STRUCT_OFFSET(FLD_TYPE2, Crc32),
+	       (u_int32_t) 0, (u_int32_t *) &crc);
+
+	/* Calc CRC for remaining fields */
+	sbeCrc((u_int8_t *) &buf->Id[0],
+	       (u_int32_t) (sizeof(FLD_TYPE2) - STRUCT_OFFSET(FLD_TYPE2, Id)),
+	       (u_int32_t) crc, (u_int32_t *) &crc);
 
 #ifdef EEPROM_TYPE_DEBUG
-    pr_info("sbeCrc: crc 2 calculated as %08x\n", crc); /* RLD DEBUG */
+	/* RLD DEBUG */
+	pr_info("sbeCrc: crc 2 calculated as %08x\n", crc);
 #endif
-    return crc;
+	return crc;
 }
 
-
 /*------------------------------------------------------------------------
  *      pmc_init_seeprom - initialize the serial EEPROM structure
  *------------------------------------------------------------------------
@@ -485,64 +468,65 @@ pmcCalcCrc_T02 (void *bufp)
  *      serial number field.
  */
 
-void
-pmc_init_seeprom (u_int32_t addr, u_int32_t serialNum)
+void pmc_init_seeprom(u_int32_t addr, u_int32_t serialNum)
 {
-    PROMFORMAT  buffer;         /* Memory image of structure */
-    u_int32_t   crc;            /* CRC of structure */
-    time_t      createTime;
+	/* Memory image of structure */
+	PROMFORMAT buffer;
+	/* CRC of structure */
+	u_int32_t crc;
+	time_t createTime;
 
-    createTime = get_seconds ();
+	createTime = get_seconds();
 
-    /* use template data */
-    memcpy(&buffer.fldType2, mfg_template, sizeof(buffer.fldType2));
+	/* use template data */
+	memcpy(&buffer.fldType2, mfg_template, sizeof(buffer.fldType2));
 
-    /* Update serial number field in buffer */
-    pmcSetBuffValue (&buffer.fldType2.Serial[3], serialNum, 3);
+	/* Update serial number field in buffer */
+	pmcSetBuffValue(&buffer.fldType2.Serial[3], serialNum, 3);
 
-    /* Update create time field in buffer */
-    pmcSetBuffValue (&buffer.fldType2.CreateTime[0], createTime, 4);
+	/* Update create time field in buffer */
+	pmcSetBuffValue(&buffer.fldType2.CreateTime[0], createTime, 4);
 
-    /* Update CRC field in buffer */
-    crc = pmcCalcCrc_T02 (&buffer);
-    pmcSetBuffValue (&buffer.fldType2.Crc32[0], crc, 4);
+	/* Update CRC field in buffer */
+	crc = pmcCalcCrc_T02(&buffer);
+	pmcSetBuffValue(&buffer.fldType2.Crc32[0], crc, 4);
 
 #ifdef DEBUG
-    for (i = 0; i < sizeof (FLD_TYPE2); ++i)
-        pr_info("[%02X] = %02X\n", i, buffer.bytes[i] & 0xFF);
+	for (i = 0; i < sizeof(FLD_TYPE2); ++i)
+		pr_info("[%02X] = %02X\n", i, buffer.bytes[i] & 0xFF);
 #endif
 
-    /* Write structure to serial EEPROM */
-    pmc_eeprom_write_buffer (addr, EE_MFG, (char *) &buffer, sizeof (FLD_TYPE2));
+	/* Write structure to serial EEPROM */
+	pmc_eeprom_write_buffer(addr, EE_MFG, (char *)&buffer,
+				sizeof(FLD_TYPE2));
 }
 
-
-char
-pmc_verify_cksum (void *bufp)
+char pmc_verify_cksum(void *bufp)
 {
-    FLD_TYPE1  *buf1 = bufp;
-    FLD_TYPE2  *buf2 = bufp;
-    u_int32_t   crc1, crc2;     /* CRC read from EEPROM */
+	FLD_TYPE1 *buf1 = bufp;
+	FLD_TYPE2 *buf2 = bufp;
+	/* CRC read from EEPROM */
+	u_int32_t crc1, crc2;
 
-    /* Retrieve contents of CRC field */
-    crc1 = pmcGetBuffValue (&buf1->Crc32[0], sizeof (buf1->Crc32));
+	/* Retrieve contents of CRC field */
+	crc1 = pmcGetBuffValue(&buf1->Crc32[0], sizeof(buf1->Crc32));
 #ifdef EEPROM_TYPE_DEBUG
-    pr_info("EEPROM: chksum 1 reads   as %08x\n", crc1);        /* RLD DEBUG */
+	/* RLD DEBUG */
+	pr_info("EEPROM: chksum 1 reads   as %08x\n", crc1);
 #endif
-    if ((buf1->type == PROM_FORMAT_TYPE1) &&
-        (pmcCalcCrc_T01 ((void *) buf1) == crc1))
-        return PROM_FORMAT_TYPE1;   /* checksum type 1 verified */
+	if ((buf1->type == PROM_FORMAT_TYPE1) &&
+	    (pmcCalcCrc_T01((void *)buf1) == crc1))
+		return PROM_FORMAT_TYPE1;	/* checksum type 1 verified */
 
-    crc2 = pmcGetBuffValue (&buf2->Crc32[0], sizeof (buf2->Crc32));
+	crc2 = pmcGetBuffValue(&buf2->Crc32[0], sizeof(buf2->Crc32));
 #ifdef EEPROM_TYPE_DEBUG
-    pr_info("EEPROM: chksum 2 reads   as %08x\n", crc2);        /* RLD DEBUG */
+	/* RLD DEBUG */
+	pr_info("EEPROM: chksum 2 reads   as %08x\n", crc2);
 #endif
-    if ((buf2->type == PROM_FORMAT_TYPE2) &&
-        (pmcCalcCrc_T02 ((void *) buf2) == crc2))
-        return PROM_FORMAT_TYPE2;   /* checksum type 2 verified */
+	if ((buf2->type == PROM_FORMAT_TYPE2) &&
+	    (pmcCalcCrc_T02((void *)buf2) == crc2))
+		return PROM_FORMAT_TYPE2;	/* checksum type 2 verified */
 
-    return PROM_FORMAT_Unk;         /* failed to validate */
+	/* failed to validate */
+	return PROM_FORMAT_Unk;
 }
-
-
-/*** End-of-File ***/
diff --git a/drivers/staging/cxt1e1/pmcc4_drv.c b/drivers/staging/cxt1e1/pmcc4_drv.c
index 621a7292647..76bebdd18b3 100644
--- a/drivers/staging/cxt1e1/pmcc4_drv.c
+++ b/drivers/staging/cxt1e1/pmcc4_drv.c
@@ -122,35 +122,6 @@ c4_find_chan (int channum)
     return NULL;
 }
 
-
-ci_t       *__init
-c4_new (void *hi)
-{
-    ci_t       *ci;
-
-#ifdef SBE_MAP_DEBUG
-    pr_warning("c4_new() entered, ci needs %u.\n",
-               (unsigned int) sizeof (ci_t));
-#endif
-
-    ci = (ci_t *) OS_kmalloc (sizeof (ci_t));
-    if (ci)
-    {
-        ci->hdw_info = hi;
-        ci->state = C_INIT;         /* mark as hardware not available */
-        ci->next = c4_list;
-        c4_list = ci;
-        ci->brdno = ci->next ? ci->next->brdno + 1 : 0;
-    } else
-        pr_warning("failed CI malloc, size %u.\n",
-                   (unsigned int) sizeof (ci_t));
-
-    if (!CI)
-        CI = ci;                    /* DEBUG, only board 0 usage */
-    return ci;
-}
-
-
 /***
  * Check port state and set LED states using watchdog or ioctl...
  * also check for in-band SF loopback commands (& cause results if they are there)
@@ -485,12 +456,12 @@ c4_cleanup (void)
             for (j = 0; j < MUSYCC_NCHANS; j++)
             {
                 if (pi->chan[j])
-                    OS_kfree (pi->chan[j]);     /* free mch_t struct */
+                    kfree(pi->chan[j]);     /* free mch_t struct */
             }
-            OS_kfree (pi->regram_saved);
+            kfree(pi->regram_saved);
         }
-        OS_kfree (ci->iqd_p_saved);
-        OS_kfree (ci);
+        kfree(ci->iqd_p_saved);
+        kfree(ci);
         ci = next;                  /* cleanup next board, if any */
     }
 }
@@ -619,7 +590,7 @@ c4_init (ci_t *ci, u_char *func0, u_char *func1)
         /* allocate channel structures for this port */
         for (j = 0; j < MUSYCC_NCHANS; j++)
         {
-            ch = OS_kmalloc (sizeof (mch_t));
+		ch = kzalloc(sizeof(mch_t), GFP_KERNEL | GFP_DMA);
             if (ch)
             {
                 pi->chan[j] = ch;
@@ -1368,8 +1339,8 @@ c4_chan_up (ci_t *ci, int channum)
     ch->txd_num = txnum;
     ch->rxix_irq_srv = 0;
 
-    ch->mdr = OS_kmalloc (sizeof (struct mdesc) * rxnum);
-    ch->mdt = OS_kmalloc (sizeof (struct mdesc) * txnum);
+	ch->mdr = kzalloc(sizeof(struct mdesc) * rxnum, GFP_KERNEL | GFP_DMA);
+	ch->mdt = kzalloc(sizeof(struct mdesc) * txnum, GFP_KERNEL | GFP_DMA);
     if (ch->p.chan_mode == CFG_CH_PROTO_TRANS)
                tmp = __constant_cpu_to_le32 (cxt1e1_max_mru | EOBIRQ_ENABLE);
     else
@@ -1462,10 +1433,10 @@ errfree:
         i--;
         OS_mem_token_free (ch->mdr[i].mem_token);
     }
-    OS_kfree (ch->mdt);
+    kfree(ch->mdt);
     ch->mdt = NULL;
     ch->txd_num = 0;
-    OS_kfree (ch->mdr);
+    kfree(ch->mdr);
     ch->mdr = NULL;
     ch->rxd_num = 0;
     ch->state = DOWN;
diff --git a/drivers/staging/cxt1e1/pmcc4_private.h b/drivers/staging/cxt1e1/pmcc4_private.h
index eb28f095ff8..451f12f5b04 100644
--- a/drivers/staging/cxt1e1/pmcc4_private.h
+++ b/drivers/staging/cxt1e1/pmcc4_private.h
@@ -262,7 +262,7 @@ struct s_hdw_info
     struct pci_dev *pdev[2];
 
     unsigned long addr[2];
-    unsigned long addr_mapped[2];
+    void __iomem *addr_mapped[2];
     unsigned long len[2];
 
     union
diff --git a/drivers/staging/cxt1e1/sbecom_inline_linux.h b/drivers/staging/cxt1e1/sbecom_inline_linux.h
index ba3ff3efe06..f5835c29ef3 100644
--- a/drivers/staging/cxt1e1/sbecom_inline_linux.h
+++ b/drivers/staging/cxt1e1/sbecom_inline_linux.h
@@ -39,27 +39,6 @@ void        pci_write_32 (u_int32_t *p, u_int32_t v);
  * system dependent callbacks
  */
 
-/**********/
-/* malloc */
-/**********/
-
-static inline void *
-OS_kmalloc (size_t size)
-{
-    char       *ptr = kmalloc (size, GFP_KERNEL | GFP_DMA);
-
-    if (ptr)
-        memset (ptr, 0, size);
-    return ptr;
-}
-
-static inline void
-OS_kfree (void *x)
-{
-    kfree (x);
-}
-
-
 /****************/
 /* memory token */
 /****************/
@@ -197,7 +176,7 @@ static inline int
 OS_free_watchdog (struct watchdog *wd)
 {
     OS_stop_watchdog (wd);
-    OS_kfree (wd);
+    kfree(wd);
     return 0;
 }
 
diff --git a/drivers/staging/cxt1e1/sbecrc.c b/drivers/staging/cxt1e1/sbecrc.c
index 81fa8a3a462..a51780f6048 100644
--- a/drivers/staging/cxt1e1/sbecrc.c
+++ b/drivers/staging/cxt1e1/sbecrc.c
@@ -101,7 +101,8 @@ sbeCrc(u_int8_t *buffer,          /* data buffer to crc */
 		tbl = &CRCTable;
 		genCrcTable(tbl);
 #else
-		tbl = (u_int32_t *) OS_kmalloc(CRC_TABLE_ENTRIES * sizeof(u_int32_t));
+		tbl = kzalloc(CRC_TABLE_ENTRIES * sizeof(u_int32_t),
+			      GFP_KERNEL);
 		if (!tbl) {
 			*result = 0;   /* dummy up return value due to malloc
 					* failure */
@@ -125,7 +126,7 @@ sbeCrc(u_int8_t *buffer,          /* data buffer to crc */
 
 #ifndef STATIC_CRC_TABLE
 	crcTableInit = 0;
-	OS_kfree(tbl);
+	kfree(tbl);
 #endif
 }
 
diff --git a/drivers/staging/cxt1e1/sbeproc.c b/drivers/staging/cxt1e1/sbeproc.c
index 840c647fbf4..1c2e52e8b5f 100644
--- a/drivers/staging/cxt1e1/sbeproc.c
+++ b/drivers/staging/cxt1e1/sbeproc.c
@@ -72,7 +72,7 @@ static int sbecom_proc_get_sbe_info(struct seq_file *m, void *v)
 	char       *spd;
 	struct sbe_brd_info *bip;
 
-	bip = OS_kmalloc(sizeof(struct sbe_brd_info));
+	bip = kzalloc(sizeof(struct sbe_brd_info), GFP_KERNEL | GFP_DMA);
 	if (!bip)
 		return -ENOMEM;
 
diff --git a/drivers/staging/dgap/dgap.c b/drivers/staging/dgap/dgap.c
index a5fc3c75ed4..170d6f3e422 100644
--- a/drivers/staging/dgap/dgap.c
+++ b/drivers/staging/dgap/dgap.c
@@ -12,21 +12,6 @@
  * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  * PURPOSE.  See the GNU General Public License for more details.
  *
- * You 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.
- *
- *
- *	NOTE TO LINUX KERNEL HACKERS:  DO NOT REFORMAT THIS CODE!
- *
- *	This is shared code between Digi's CVS archive and the
- *	Linux Kernel sources.
- *	Changing the source just for reformatting needlessly breaks
- *	our CVS diff history.
- *
- *	Send any bug fixes/changes to:  Eng.Linux at digi dot com.
- *	Thank you.
- *
  */
 
 /*
@@ -78,21 +63,11 @@
 
 #include "dgap.h"
 
-#define init_MUTEX(sem)         sema_init(sem, 1)
-#define DECLARE_MUTEX(name)     \
-	struct semaphore name = __SEMAPHORE_INITIALIZER(name, 1)
-
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Digi International, http://www.digi.com");
 MODULE_DESCRIPTION("Driver for the Digi International EPCA PCI based product line");
 MODULE_SUPPORTED_DEVICE("dgap");
 
-/**************************************************************************
- *
- * protos for this file
- *
- */
-
 static int dgap_start(void);
 static void dgap_init_globals(void);
 static int dgap_found_board(struct pci_dev *pdev, int id);
@@ -105,7 +80,6 @@ static int dgap_probe1(struct pci_dev *pdev, int card_type);
 static int dgap_do_remap(struct board_t *brd);
 static irqreturn_t dgap_intr(int irq, void *voidbrd);
 
-/* Our function prototypes */
 static int dgap_tty_open(struct tty_struct *tty, struct file *file);
 static void dgap_tty_close(struct tty_struct *tty, struct file *file);
 static int dgap_block_til_ready(struct tty_struct *tty, struct file *file,
@@ -161,9 +135,9 @@ static void dgap_cmdw_ext(struct channel_t *ch, u16 cmd, u16 word, uint ncmds);
 static int dgap_event(struct board_t *bd);
 
 static void dgap_poll_tasklet(unsigned long data);
-static void dgap_cmdb(struct channel_t *ch, uchar cmd, uchar byte1,
-			uchar byte2, uint ncmds);
-static void dgap_cmdw(struct channel_t *ch, uchar cmd, u16 word, uint ncmds);
+static void dgap_cmdb(struct channel_t *ch, u8 cmd, u8 byte1,
+			u8 byte2, uint ncmds);
+static void dgap_cmdw(struct channel_t *ch, u8 cmd, u16 word, uint ncmds);
 static void dgap_wmove(struct channel_t *ch, char *buf, uint cnt);
 static int dgap_param(struct tty_struct *tty);
 static void dgap_parity_scan(struct channel_t *ch, unsigned char *cbuf,
@@ -176,7 +150,6 @@ static void dgap_firmware_reset_port(struct channel_t *ch);
  */
 static int dgap_gettok(char **in, struct cnode *p);
 static char *dgap_getword(char **in);
-static char *dgap_savestring(char *s);
 static struct cnode *dgap_newnode(int t);
 static int dgap_checknode(struct cnode *p);
 static void dgap_err(char *s);
@@ -202,7 +175,7 @@ static void dgap_remove_tty_sysfs(struct device *c);
 /*
  * Function prototypes from dgap_parse.h
  */
-static int dgap_parsefile(char **in, int Remove);
+static int dgap_parsefile(char **in, int remove);
 static struct cnode *dgap_find_config(int type, int bus, int slot);
 static uint dgap_config_get_num_prts(struct board_t *bd);
 static char *dgap_create_config_string(struct board_t *bd, char *string);
@@ -210,22 +183,21 @@ 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 uchar *ubios, int len);
-static void dgap_do_fep_load(struct board_t *brd, const uchar *ufep, int len);
+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
-static void dgap_do_conc_load(struct board_t *brd, uchar *uaddr, int len);
+static void dgap_do_conc_load(struct board_t *brd, u8 *uaddr, int len);
 #endif
 static int dgap_after_config_loaded(int board);
 static int dgap_finalize_board_init(struct board_t *brd);
 
 static void dgap_get_vpd(struct board_t *brd);
 static void dgap_do_reset_board(struct board_t *brd);
-static int dgap_do_wait_for_bios(struct board_t *brd);
-static int dgap_do_wait_for_fep(struct board_t *brd);
+static int dgap_test_bios(struct board_t *brd);
+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);
 
-/* Driver unload function */
 static void dgap_cleanup_module(void);
 
 module_exit(dgap_cleanup_module);
@@ -233,35 +205,27 @@ module_exit(dgap_cleanup_module);
 /*
  * File operations permitted on Control/Management major.
  */
-static const struct file_operations DgapBoardFops = {
+static const struct file_operations dgap_board_fops = {
 	.owner	= THIS_MODULE,
 };
 
-/*
- * Globals
- */
-static uint dgap_NumBoards;
-static struct board_t *dgap_Board[MAXBOARDS];
+static uint dgap_numboards;
+static struct board_t *dgap_board[MAXBOARDS];
 static ulong dgap_poll_counter;
 static char *dgap_config_buf;
 static int dgap_driver_state = DRIVER_INITIALIZED;
-DEFINE_SPINLOCK(dgap_dl_lock);
 static wait_queue_head_t dgap_dl_wait;
-static int dgap_dl_action;
 static int dgap_poll_tick = 20;	/* Poll interval - 20 ms */
 
-/*
- * Static vars.
- */
 static struct class *dgap_class;
 
-static struct board_t *dgap_BoardsByMajor[256];
+static struct board_t *dgap_boards_by_major[256];
 static uint dgap_count = 500;
 
 /*
  * Poller stuff
  */
-DEFINE_SPINLOCK(dgap_poll_lock);	/* Poll scheduling lock */
+static DEFINE_SPINLOCK(dgap_poll_lock);	/* Poll scheduling lock */
 static ulong dgap_poll_time;		/* Time of next poll */
 static uint dgap_poll_stop;		/* Used to tell poller to stop */
 static struct timer_list dgap_poll_timer;
@@ -310,12 +274,12 @@ MODULE_DEVICE_TABLE(pci, dgap_pci_tbl);
  */
 struct board_id {
 	uint config_type;
-	uchar *name;
+	u8 *name;
 	uint maxports;
 	uint dpatype;
 };
 
-static struct board_id dgap_Ids[] = {
+static struct board_id dgap_ids[] = {
 	{ PPCM,        PCI_DEV_XEM_NAME,     64, (T_PCXM|T_PCLITE|T_PCIBUS) },
 	{ PCX,         PCI_DEV_CX_NAME,     128, (T_CX|T_PCIBUS)            },
 	{ PCX,         PCI_DEV_CX_IBM_NAME, 128, (T_CX|T_PCIBUS)            },
@@ -342,33 +306,33 @@ static struct pci_driver dgap_driver = {
 };
 
 struct firmware_info {
-	uchar *conf_name;       /* dgap.conf */
-	uchar *bios_name;	/* BIOS filename */
-	uchar *fep_name;	/* FEP  filename */
-	uchar *con_name;	/* Concentrator filename  FIXME*/
-	int num;                /* sequence number */
+	u8 *conf_name;  /* dgap.conf */
+	u8 *bios_name;	/* BIOS filename */
+	u8 *fep_name;	/* FEP  filename */
+	u8 *con_name;	/* Concentrator filename  FIXME*/
+	int num;        /* sequence number */
 };
 
 /*
  * Firmware - BIOS, FEP, and CONC filenames
  */
 static struct firmware_info fw_info[] = {
-	{ "dgap/dgap.conf", "dgap/sxbios.bin",  "dgap/sxfep.bin",  0, 0 },
-	{ "dgap/dgap.conf", "dgap/cxpbios.bin", "dgap/cxpfep.bin", 0, 1 },
-	{ "dgap/dgap.conf", "dgap/cxpbios.bin", "dgap/cxpfep.bin", 0, 2 },
-	{ "dgap/dgap.conf", "dgap/pcibios.bin", "dgap/pcifep.bin", 0, 3 },
-	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  0, 4 },
-	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  0, 5 },
-	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  0, 6 },
-	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  0, 7 },
-	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  0, 8 },
-	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  0, 9 },
-	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  0, 10 },
-	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  0, 11 },
-	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  0, 12 },
-	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  0, 13 },
-	{ "dgap/dgap.conf", "dgap/sxbios.bin",  "dgap/sxfep.bin",  0, 14 },
-	{0,}
+	{ "dgap/dgap.conf", "dgap/sxbios.bin",  "dgap/sxfep.bin",  NULL, 0 },
+	{ "dgap/dgap.conf", "dgap/cxpbios.bin", "dgap/cxpfep.bin", NULL, 1 },
+	{ "dgap/dgap.conf", "dgap/cxpbios.bin", "dgap/cxpfep.bin", NULL, 2 },
+	{ "dgap/dgap.conf", "dgap/pcibios.bin", "dgap/pcifep.bin", NULL, 3 },
+	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  NULL, 4 },
+	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  NULL, 5 },
+	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  NULL, 6 },
+	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  NULL, 7 },
+	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  NULL, 8 },
+	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  NULL, 9 },
+	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  NULL, 10 },
+	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  NULL, 11 },
+	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  NULL, 12 },
+	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  NULL, 13 },
+	{ "dgap/dgap.conf", "dgap/sxbios.bin",  "dgap/sxfep.bin",  NULL, 14 },
+	{NULL,}
 };
 
 /*
@@ -394,7 +358,7 @@ static struct digi_t dgap_digi_init = {
  * 1 stop bit.
  */
 
-static struct ktermios DgapDefaultTermios = {
+static struct ktermios dgap_default_termios = {
 	.c_iflag =	(DEFAULT_IFLAGS),	/* iflags */
 	.c_oflag =	(DEFAULT_OFLAGS),	/* oflags */
 	.c_cflag =	(DEFAULT_CFLAGS),	/* cflags */
@@ -434,8 +398,8 @@ static struct cnode dgap_head;
 static char dgap_cword[MAXCWORD];
 
 struct toklist {
-	int	token;
-	char	*string;
+	int token;
+	char *string;
 };
 
 static struct toklist dgap_tlist[] = {
@@ -502,7 +466,7 @@ static struct toklist dgap_tlist[] = {
  */
 static int dgap_init_module(void)
 {
-	int rc = 0;
+	int rc;
 
 	pr_info("%s, Digi International Part Number %s\n", DG_NAME, DG_PART);
 
@@ -535,7 +499,7 @@ module_init(dgap_init_module);
  */
 static int dgap_start(void)
 {
-	int rc = 0;
+	int rc;
 	unsigned long flags;
 	struct device *device;
 
@@ -545,7 +509,7 @@ static int dgap_start(void)
 	 */
 	dgap_init_globals();
 
-	dgap_NumBoards = 0;
+	dgap_numboards = 0;
 
 	pr_info("For the tools package please visit http://www.digi.com\n");
 
@@ -556,7 +520,7 @@ static int dgap_start(void)
 	/*
 	 * Register management/dpa devices
 	 */
-	rc = register_chrdev(DIGI_DGAP_MAJOR, "dgap", &DgapBoardFops);
+	rc = register_chrdev(DIGI_DGAP_MAJOR, "dgap", &dgap_board_fops);
 	if (rc < 0)
 		return rc;
 
@@ -602,24 +566,23 @@ static int dgap_init_pci(void)
 	return pci_register_driver(&dgap_driver);
 }
 
-/* returns count (>= 0), or negative on error */
 static int dgap_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
 	int rc;
 
-	/* wake up and enable device */
+	if (dgap_numboards >= MAXBOARDS)
+		return -EPERM;
+
 	rc = pci_enable_device(pdev);
+	if (rc)
+		return -EIO;
 
-	if (rc < 0) {
-		rc = -EIO;
-	} else {
-		rc = dgap_probe1(pdev, ent->driver_data);
-		if (rc == 0) {
-			dgap_NumBoards++;
-			rc = dgap_firmware_load(pdev, ent->driver_data);
-		}
-	}
-	return rc;
+	rc = dgap_probe1(pdev, ent->driver_data);
+	if (rc)
+		return rc;
+
+	dgap_numboards++;
+	return dgap_firmware_load(pdev, ent->driver_data);
 }
 
 static int dgap_probe1(struct pci_dev *pdev, int card_type)
@@ -655,15 +618,13 @@ static void dgap_cleanup_module(void)
 	class_destroy(dgap_class);
 	unregister_chrdev(DIGI_DGAP_MAJOR, "dgap");
 
-	kfree(dgap_config_buf);
-
-	for (i = 0; i < dgap_NumBoards; ++i) {
-		dgap_remove_ports_sysfiles(dgap_Board[i]);
-		dgap_tty_uninit(dgap_Board[i]);
-		dgap_cleanup_board(dgap_Board[i]);
+	for (i = 0; i < dgap_numboards; ++i) {
+		dgap_remove_ports_sysfiles(dgap_board[i]);
+		dgap_tty_uninit(dgap_board[i]);
+		dgap_cleanup_board(dgap_board[i]);
 	}
 
-	if (dgap_NumBoards)
+	if (dgap_numboards)
 		pci_unregister_driver(&dgap_driver);
 }
 
@@ -674,7 +635,7 @@ static void dgap_cleanup_module(void)
  */
 static void dgap_cleanup_board(struct board_t *brd)
 {
-	int i = 0;
+	int i;
 
 	if (!brd || brd->magic != DGAP_BOARD_MAGIC)
 		return;
@@ -703,7 +664,7 @@ static void dgap_cleanup_board(struct board_t *brd)
 	kfree(brd->flipbuf);
 	kfree(brd->flipflagbuf);
 
-	dgap_Board[brd->boardnum] = NULL;
+	dgap_board[brd->boardnum] = NULL;
 
 	kfree(brd);
 }
@@ -717,28 +678,28 @@ static int dgap_found_board(struct pci_dev *pdev, int id)
 {
 	struct board_t *brd;
 	unsigned int pci_irq;
-	int i = 0;
+	int i;
 
 	/* get the board structure and prep it */
 	brd = kzalloc(sizeof(struct board_t), GFP_KERNEL);
 	if (!brd)
 		return -ENOMEM;
 
-	dgap_Board[dgap_NumBoards] = brd;
+	dgap_board[dgap_numboards] = brd;
 
 	/* store the info for the board we've found */
 	brd->magic = DGAP_BOARD_MAGIC;
-	brd->boardnum = dgap_NumBoards;
+	brd->boardnum = dgap_numboards;
 	brd->firstminor = 0;
 	brd->vendor = dgap_pci_tbl[id].vendor;
 	brd->device = dgap_pci_tbl[id].device;
 	brd->pdev = pdev;
 	brd->pci_bus = pdev->bus->number;
 	brd->pci_slot = PCI_SLOT(pdev->devfn);
-	brd->name = dgap_Ids[id].name;
-	brd->maxports = dgap_Ids[id].maxports;
-	brd->type = dgap_Ids[id].config_type;
-	brd->dpatype = dgap_Ids[id].dpatype;
+	brd->name = dgap_ids[id].name;
+	brd->maxports = dgap_ids[id].maxports;
+	brd->type = dgap_ids[id].config_type;
+	brd->dpatype = dgap_ids[id].dpatype;
 	brd->dpastatus = BD_NOFEP;
 	init_waitqueue_head(&brd->state_wait);
 
@@ -819,9 +780,8 @@ static int dgap_found_board(struct pci_dev *pdev, int id)
 	if (i)
 		brd->state = BOARD_FAILED;
 
-	pr_info("dgap: board %d: %s (rev %d), irq %ld, %s\n",
-		dgap_NumBoards, brd->name, brd->rev, brd->irq,
-		brd->state ? "NOT READY\0" : "READY\0");
+	pr_info("dgap: board %d: %s (rev %d), irq %ld\n",
+		dgap_numboards, brd->name, brd->rev, brd->irq);
 
 	return 0;
 }
@@ -856,14 +816,15 @@ static int dgap_finalize_board_init(struct board_t *brd)
 
 static int dgap_firmware_load(struct pci_dev *pdev, int card_type)
 {
-	struct board_t *brd = dgap_Board[dgap_NumBoards - 1];
+	struct board_t *brd = dgap_board[dgap_numboards - 1];
 	const struct firmware *fw;
+	char *tmp_ptr;
 	int ret;
 
 	dgap_get_vpd(brd);
 	dgap_do_reset_board(brd);
 
-	if (fw_info[card_type].conf_name) {
+	if ((fw_info[card_type].conf_name) && !dgap_config_buf) {
 		ret = request_firmware(&fw, fw_info[card_type].conf_name,
 					 &pdev->dev);
 		if (ret) {
@@ -871,20 +832,28 @@ static int dgap_firmware_load(struct pci_dev *pdev, int card_type)
 				fw_info[card_type].conf_name);
 			return ret;
 		}
+
+		dgap_config_buf = kzalloc(fw->size + 1, GFP_KERNEL);
 		if (!dgap_config_buf) {
-			dgap_config_buf = kmalloc(fw->size + 1, GFP_ATOMIC);
-			if (!dgap_config_buf) {
-				release_firmware(fw);
-				return -ENOMEM;
-			}
+			release_firmware(fw);
+			return -ENOMEM;
 		}
 
 		memcpy(dgap_config_buf, fw->data, fw->size);
 		release_firmware(fw);
-		dgap_config_buf[fw->size + 1] = '\0';
 
-		if (dgap_parsefile(&dgap_config_buf, TRUE) != 0)
+		/*
+		 * preserve dgap_config_buf
+		 * as dgap_parsefile would
+		 * otherwise alter it.
+		 */
+		tmp_ptr = dgap_config_buf;
+
+		if (dgap_parsefile(&tmp_ptr, TRUE) != 0) {
+			kfree(dgap_config_buf);
 			return -EINVAL;
+		}
+		kfree(dgap_config_buf);
 	}
 
 	ret = dgap_after_config_loaded(brd->boardnum);
@@ -911,8 +880,13 @@ static int dgap_firmware_load(struct pci_dev *pdev, int card_type)
 		return -EINVAL;
 	}
 
-	dgap_tty_register(brd);
-	dgap_finalize_board_init(brd);
+	ret = dgap_tty_register(brd);
+	if (ret)
+		return ret;
+
+	ret = dgap_finalize_board_init(brd);
+	if (ret)
+		return ret;
 
 	if (fw_info[card_type].bios_name) {
 		ret = request_firmware(&fw, fw_info[card_type].bios_name,
@@ -926,8 +900,9 @@ static int dgap_firmware_load(struct pci_dev *pdev, int card_type)
 		release_firmware(fw);
 
 		/* Wait for BIOS to test board... */
-		if (!dgap_do_wait_for_bios(brd))
-			return -ENXIO;
+		ret = dgap_test_bios(brd);
+		if (ret)
+			return ret;
 	}
 
 	if (fw_info[card_type].fep_name) {
@@ -942,8 +917,9 @@ static int dgap_firmware_load(struct pci_dev *pdev, int card_type)
 		release_firmware(fw);
 
 		/* Wait for FEP to load on board... */
-		if (!dgap_do_wait_for_fep(brd))
-			return -ENXIO;
+		ret = dgap_test_fep(brd);
+		if (ret)
+			return ret;
 	}
 
 #ifdef DIGI_CONCENTRATORS_SUPPORTED
@@ -999,7 +975,7 @@ static int dgap_firmware_load(struct pci_dev *pdev, int card_type)
 static int dgap_do_remap(struct board_t *brd)
 {
 	if (!brd || brd->magic != DGAP_BOARD_MAGIC)
-		return -ENXIO;
+		return -EIO;
 
 	if (!request_mem_region(brd->membase, 0x200000, "dgap"))
 		return -ENOMEM;
@@ -1077,10 +1053,10 @@ static void dgap_poll_handler(ulong dummy)
 	 * Otherwise, use our new tasklet based poller, which should
 	 * speed things up for multiple boards.
 	 */
-	if ((dgap_NumBoards == 1) || (num_online_cpus() <= 1)) {
-		for (i = 0; i < dgap_NumBoards; i++) {
+	if ((dgap_numboards == 1) || (num_online_cpus() <= 1)) {
+		for (i = 0; i < dgap_numboards; i++) {
 
-			brd = dgap_Board[i];
+			brd = dgap_board[i];
 
 			if (brd->state == BOARD_FAILED)
 				continue;
@@ -1093,8 +1069,8 @@ static void dgap_poll_handler(ulong dummy)
 		 * Go thru each board, kicking off a
 		 * tasklet for each if needed
 		 */
-		for (i = 0; i < dgap_NumBoards; i++) {
-			brd = dgap_Board[i];
+		for (i = 0; i < dgap_numboards; i++) {
+			brd = dgap_board[i];
 
 			/*
 			 * Attempt to grab the board lock.
@@ -1189,15 +1165,14 @@ static irqreturn_t dgap_intr(int irq, void *voidbrd)
  */
 static void dgap_init_globals(void)
 {
-	int i = 0;
+	int i;
 
 	for (i = 0; i < MAXBOARDS; i++)
-		dgap_Board[i] = NULL;
+		dgap_board[i] = NULL;
 
 	init_timer(&dgap_poll_timer);
 
 	init_waitqueue_head(&dgap_dl_wait);
-	dgap_dl_action = 0;
 }
 
 /************************************************************************
@@ -1233,86 +1208,105 @@ static int dgap_ms_sleep(ulong ms)
  */
 static int dgap_tty_register(struct board_t *brd)
 {
-	int rc = 0;
+	int rc;
 
-	brd->SerialDriver = alloc_tty_driver(MAXPORTS);
-
-	snprintf(brd->SerialName, MAXTTYNAMELEN, "tty_dgap_%d_", brd->boardnum);
-	brd->SerialDriver->name = brd->SerialName;
-	brd->SerialDriver->name_base = 0;
-	brd->SerialDriver->major = 0;
-	brd->SerialDriver->minor_start = 0;
-	brd->SerialDriver->type = TTY_DRIVER_TYPE_SERIAL;
-	brd->SerialDriver->subtype = SERIAL_TYPE_NORMAL;
-	brd->SerialDriver->init_termios = DgapDefaultTermios;
-	brd->SerialDriver->driver_name = DRVSTR;
-	brd->SerialDriver->flags = (TTY_DRIVER_REAL_RAW |
+	brd->serial_driver = tty_alloc_driver(MAXPORTS, 0);
+	if (IS_ERR(brd->serial_driver))
+		return PTR_ERR(brd->serial_driver);
+
+	snprintf(brd->serial_name, MAXTTYNAMELEN, "tty_dgap_%d_",
+		 brd->boardnum);
+	brd->serial_driver->name = brd->serial_name;
+	brd->serial_driver->name_base = 0;
+	brd->serial_driver->major = 0;
+	brd->serial_driver->minor_start = 0;
+	brd->serial_driver->type = TTY_DRIVER_TYPE_SERIAL;
+	brd->serial_driver->subtype = SERIAL_TYPE_NORMAL;
+	brd->serial_driver->init_termios = dgap_default_termios;
+	brd->serial_driver->driver_name = DRVSTR;
+	brd->serial_driver->flags = (TTY_DRIVER_REAL_RAW |
 				    TTY_DRIVER_DYNAMIC_DEV |
 				    TTY_DRIVER_HARDWARE_BREAK);
 
 	/* The kernel wants space to store pointers to tty_structs */
-	brd->SerialDriver->ttys =
+	brd->serial_driver->ttys =
 		kzalloc(MAXPORTS * sizeof(struct tty_struct *), GFP_KERNEL);
-	if (!brd->SerialDriver->ttys)
-		return -ENOMEM;
+	if (!brd->serial_driver->ttys) {
+		rc = -ENOMEM;
+		goto free_serial_drv;
+	}
 
 	/*
 	 * Entry points for driver.  Called by the kernel from
 	 * tty_io.c and n_tty.c.
 	 */
-	tty_set_operations(brd->SerialDriver, &dgap_tty_ops);
+	tty_set_operations(brd->serial_driver, &dgap_tty_ops);
 
 	/*
 	 * If we're doing transparent print, we have to do all of the above
 	 * again, separately so we don't get the LD confused about what major
 	 * we are when we get into the dgap_tty_open() routine.
 	 */
-	brd->PrintDriver = alloc_tty_driver(MAXPORTS);
-
-	snprintf(brd->PrintName, MAXTTYNAMELEN, "pr_dgap_%d_", brd->boardnum);
-	brd->PrintDriver->name = brd->PrintName;
-	brd->PrintDriver->name_base = 0;
-	brd->PrintDriver->major = 0;
-	brd->PrintDriver->minor_start = 0;
-	brd->PrintDriver->type = TTY_DRIVER_TYPE_SERIAL;
-	brd->PrintDriver->subtype = SERIAL_TYPE_NORMAL;
-	brd->PrintDriver->init_termios = DgapDefaultTermios;
-	brd->PrintDriver->driver_name = DRVSTR;
-	brd->PrintDriver->flags = (TTY_DRIVER_REAL_RAW |
+	brd->print_driver = tty_alloc_driver(MAXPORTS, 0);
+	if (IS_ERR(brd->print_driver)) {
+		rc = PTR_ERR(brd->print_driver);
+		goto free_serial_drv;
+	}
+
+	snprintf(brd->print_name, MAXTTYNAMELEN, "pr_dgap_%d_",
+		 brd->boardnum);
+	brd->print_driver->name = brd->print_name;
+	brd->print_driver->name_base = 0;
+	brd->print_driver->major = 0;
+	brd->print_driver->minor_start = 0;
+	brd->print_driver->type = TTY_DRIVER_TYPE_SERIAL;
+	brd->print_driver->subtype = SERIAL_TYPE_NORMAL;
+	brd->print_driver->init_termios = dgap_default_termios;
+	brd->print_driver->driver_name = DRVSTR;
+	brd->print_driver->flags = (TTY_DRIVER_REAL_RAW |
 				   TTY_DRIVER_DYNAMIC_DEV |
 				   TTY_DRIVER_HARDWARE_BREAK);
 
 	/* The kernel wants space to store pointers to tty_structs */
-	brd->PrintDriver->ttys =
+	brd->print_driver->ttys =
 		kzalloc(MAXPORTS * sizeof(struct tty_struct *), GFP_KERNEL);
-	if (!brd->PrintDriver->ttys)
-		return -ENOMEM;
+	if (!brd->print_driver->ttys) {
+		rc = -ENOMEM;
+		goto free_print_drv;
+	}
 
 	/*
 	 * Entry points for driver.  Called by the kernel from
 	 * tty_io.c and n_tty.c.
 	 */
-	tty_set_operations(brd->PrintDriver, &dgap_tty_ops);
+	tty_set_operations(brd->print_driver, &dgap_tty_ops);
 
-	if (!brd->dgap_Major_Serial_Registered) {
-		/* Register tty devices */
-		rc = tty_register_driver(brd->SerialDriver);
-		if (rc < 0)
-			return rc;
-		brd->dgap_Major_Serial_Registered = TRUE;
-		dgap_BoardsByMajor[brd->SerialDriver->major] = brd;
-		brd->dgap_Serial_Major = brd->SerialDriver->major;
-	}
+	/* Register tty devices */
+	rc = tty_register_driver(brd->serial_driver);
+	if (rc < 0)
+		goto free_print_drv;
 
-	if (!brd->dgap_Major_TransparentPrint_Registered) {
-		/* Register Transparent Print devices */
-		rc = tty_register_driver(brd->PrintDriver);
-		if (rc < 0)
-			return rc;
-		brd->dgap_Major_TransparentPrint_Registered = TRUE;
-		dgap_BoardsByMajor[brd->PrintDriver->major] = brd;
-		brd->dgap_TransparentPrint_Major = brd->PrintDriver->major;
-	}
+	/* Register Transparent Print devices */
+	rc = tty_register_driver(brd->print_driver);
+	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;
+
+	return 0;
+
+unregister_serial_drv:
+	tty_unregister_driver(brd->serial_driver);
+free_print_drv:
+	put_tty_driver(brd->print_driver);
+free_serial_drv:
+	put_tty_driver(brd->serial_driver);
 
 	return rc;
 }
@@ -1327,15 +1321,15 @@ static int dgap_tty_init(struct board_t *brd)
 {
 	int i;
 	int tlw;
-	uint true_count = 0;
-	uchar *vaddr;
-	uchar modem = 0;
+	uint true_count;
+	u8 __iomem *vaddr;
+	u8 modem;
 	struct channel_t *ch;
-	struct bs_t *bs;
-	struct cm_t *cm;
+	struct bs_t __iomem *bs;
+	struct cm_t __iomem *cm;
 
 	if (!brd)
-		return -ENXIO;
+		return -EIO;
 
 	/*
 	 * Initialize board structure elements.
@@ -1373,7 +1367,7 @@ static int dgap_tty_init(struct board_t *brd)
 		if (!brd->nasync) {
 			brd->state = BOARD_FAILED;
 			brd->dpastatus = BD_NOFEP;
-			return -ENXIO;
+			return -EIO;
 		}
 	}
 
@@ -1384,7 +1378,7 @@ static int dgap_tty_init(struct board_t *brd)
 	for (i = 0; i < brd->nasync; i++) {
 		if (!brd->channels[i]) {
 			brd->channels[i] =
-				kzalloc(sizeof(struct channel_t), GFP_ATOMIC);
+				kzalloc(sizeof(struct channel_t), GFP_KERNEL);
 			if (!brd->channels[i])
 				return -ENOMEM;
 		}
@@ -1393,8 +1387,8 @@ static int dgap_tty_init(struct board_t *brd)
 	ch = brd->channels[0];
 	vaddr = brd->re_map_membase;
 
-	bs = (struct bs_t *) ((ulong) vaddr + CHANBUF);
-	cm = (struct cm_t *) ((ulong) vaddr + CMDBUF);
+	bs = (struct bs_t __iomem *) ((ulong) vaddr + CHANBUF);
+	cm = (struct cm_t __iomem *) ((ulong) vaddr + CMDBUF);
 
 	brd->bd_bs = bs;
 
@@ -1437,8 +1431,8 @@ static int dgap_tty_init(struct board_t *brd)
 			ch->ch_dsr	= DM_DSR;
 		}
 
-		ch->ch_taddr = vaddr + ((ch->ch_bs->tx_seg) << 4);
-		ch->ch_raddr = vaddr + ((ch->ch_bs->rx_seg) << 4);
+		ch->ch_taddr = vaddr + (ioread16(&(ch->ch_bs->tx_seg)) << 4);
+		ch->ch_raddr = vaddr + (ioread16(&(ch->ch_bs->rx_seg)) << 4);
 		ch->ch_tx_win = 0;
 		ch->ch_rx_win = 0;
 		ch->ch_tsize = readw(&(ch->ch_bs->tx_max)) + 1;
@@ -1468,7 +1462,6 @@ static int dgap_tty_init(struct board_t *brd)
 		init_waitqueue_head(&ch->ch_flags_wait);
 		init_waitqueue_head(&ch->ch_tun.un_flags_wait);
 		init_waitqueue_head(&ch->ch_pun.un_flags_wait);
-		init_waitqueue_head(&ch->ch_sniff_wait);
 
 		/* Turn on all modem interrupts for now */
 		modem = (DM_CD | DM_DSR | DM_CTS | DM_RI);
@@ -1498,160 +1491,39 @@ static int dgap_tty_init(struct board_t *brd)
 static void dgap_tty_uninit(struct board_t *brd)
 {
 	struct device *dev;
-	int i = 0;
+	int i;
 
-	if (brd->dgap_Major_Serial_Registered) {
-		dgap_BoardsByMajor[brd->SerialDriver->major] = NULL;
-		brd->dgap_Serial_Major = 0;
+	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->SerialPorts[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->SerialDriver, i);
+			tty_unregister_device(brd->serial_driver, i);
 		}
-		tty_unregister_driver(brd->SerialDriver);
-		kfree(brd->SerialDriver->ttys);
-		brd->SerialDriver->ttys = NULL;
-		put_tty_driver(brd->SerialDriver);
-		kfree(brd->SerialPorts);
-		brd->dgap_Major_Serial_Registered = FALSE;
+		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_TransparentPrint_Registered) {
-		dgap_BoardsByMajor[brd->PrintDriver->major] = NULL;
-		brd->dgap_TransparentPrint_Major = 0;
+	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->PrinterPorts[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->PrintDriver, i);
+			tty_unregister_device(brd->print_driver, i);
 		}
-		tty_unregister_driver(brd->PrintDriver);
-		kfree(brd->PrintDriver->ttys);
-		brd->PrintDriver->ttys = NULL;
-		put_tty_driver(brd->PrintDriver);
-		kfree(brd->PrinterPorts);
-		brd->dgap_Major_TransparentPrint_Registered = FALSE;
+		tty_unregister_driver(brd->print_driver);
+		put_tty_driver(brd->print_driver);
+		kfree(brd->printer_ports);
+		brd->dgap_major_transparent_print_registered = FALSE;
 	}
 }
 
-#define TMPBUFLEN (1024)
-/*
- * dgap_sniff - Dump data out to the "sniff" buffer if the
- * proc sniff file is opened...
- */
-static void dgap_sniff_nowait_nolock(struct channel_t *ch, uchar *text,
-				     uchar *buf, int len)
-{
-	struct timeval tv;
-	int n;
-	int r;
-	int nbuf;
-	int i;
-	int tmpbuflen;
-	char tmpbuf[TMPBUFLEN];
-	char *p = tmpbuf;
-	int too_much_data;
-
-	/* Leave if sniff not open */
-	if (!(ch->ch_sniff_flags & SNIFF_OPEN))
-		return;
-
-	do_gettimeofday(&tv);
-
-	/* Create our header for data dump */
-	p += sprintf(p, "<%ld %ld><%s><", tv.tv_sec, tv.tv_usec, text);
-	tmpbuflen = p - tmpbuf;
-
-	do {
-		too_much_data = 0;
-
-		for (i = 0; i < len && tmpbuflen < (TMPBUFLEN - 4); i++) {
-			p += sprintf(p, "%02x ", *buf);
-			buf++;
-			tmpbuflen = p - tmpbuf;
-		}
-
-		if (tmpbuflen < (TMPBUFLEN - 4)) {
-			if (i > 0)
-				p += sprintf(p - 1, "%s\n", ">");
-			else
-				p += sprintf(p, "%s\n", ">");
-		} else {
-			too_much_data = 1;
-			len -= i;
-		}
-
-		nbuf = strlen(tmpbuf);
-		p = tmpbuf;
-
-		/*
-		 *  Loop while data remains.
-		 */
-		while (nbuf > 0 && ch->ch_sniff_buf) {
-			/*
-			 *  Determine the amount of available space left in the
-			 *  buffer.  If there's none, wait until some appears.
-			 */
-			n = (ch->ch_sniff_out - ch->ch_sniff_in - 1) &
-			     SNIFF_MASK;
-
-			/*
-			 * If there is no space left to write to in our sniff
-			 * buffer, we have no choice but to drop the data.
-			 * We *cannot* sleep here waiting for space, because
-			 * this function was probably called by the
-			 * interrupt/timer routines!
-			 */
-			if (n == 0)
-				return;
-
-			/*
-			 * Copy as much data as will fit.
-			 */
-
-			if (n > nbuf)
-				n = nbuf;
-
-			r = SNIFF_MAX - ch->ch_sniff_in;
-
-			if (r <= n) {
-				memcpy(ch->ch_sniff_buf +
-				       ch->ch_sniff_in, p, r);
-
-				n -= r;
-				ch->ch_sniff_in = 0;
-				p += r;
-				nbuf -= r;
-			}
-
-			memcpy(ch->ch_sniff_buf + ch->ch_sniff_in, p, n);
-
-			ch->ch_sniff_in += n;
-			p += n;
-			nbuf -= n;
-
-			/*
-			 *  Wakeup any thread waiting for data
-			 */
-			if (ch->ch_sniff_flags & SNIFF_WAIT_DATA) {
-				ch->ch_sniff_flags &= ~SNIFF_WAIT_DATA;
-				wake_up_interruptible(&ch->ch_sniff_wait);
-			}
-		}
-
-		/*
-		 * If the user sent us too much data to push into our tmpbuf,
-		 * we need to keep looping around on all the data.
-		 */
-		if (too_much_data) {
-			p = tmpbuf;
-			tmpbuflen = 0;
-		}
-
-	} while (too_much_data);
-}
-
 /*=======================================================================
  *
  *      dgap_input - Process received data.
@@ -1663,21 +1535,21 @@ static void dgap_sniff_nowait_nolock(struct channel_t *ch, uchar *text,
 static void dgap_input(struct channel_t *ch)
 {
 	struct board_t *bd;
-	struct bs_t	*bs;
+	struct bs_t __iomem *bs;
 	struct tty_struct *tp;
 	struct tty_ldisc *ld;
-	uint	rmask;
-	uint	head;
-	uint	tail;
-	int	data_len;
-	ulong	lock_flags;
-	ulong   lock_flags2;
+	uint rmask;
+	uint head;
+	uint tail;
+	int data_len;
+	ulong lock_flags;
+	ulong lock_flags2;
 	int flip_len;
-	int len = 0;
-	int n = 0;
-	uchar *buf;
-	uchar tmpchar;
-	int s = 0;
+	int len;
+	int n;
+	u8 *buf;
+	u8 tmpchar;
+	int s;
 
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
 		return;
@@ -1814,8 +1686,7 @@ static void dgap_input(struct channel_t *ch)
 		if (s <= 0)
 			break;
 
-		memcpy_fromio(buf, (char *) ch->ch_raddr + tail, s);
-		dgap_sniff_nowait_nolock(ch, "USER READ", buf, s);
+		memcpy_fromio(buf, ch->ch_raddr + tail, s);
 
 		tail += s;
 		buf += s;
@@ -1987,29 +1858,27 @@ static void dgap_carrier(struct channel_t *ch)
  */
 static int dgap_tty_open(struct tty_struct *tty, struct file *file)
 {
-	struct board_t	*brd;
+	struct board_t *brd;
 	struct channel_t *ch;
-	struct un_t	*un;
-	struct bs_t	*bs;
-	uint		major = 0;
-	uint		minor = 0;
-	int		rc = 0;
-	ulong		lock_flags;
-	ulong		lock_flags2;
-	u16		head;
-
-	rc = 0;
+	struct un_t *un;
+	struct bs_t __iomem *bs;
+	uint major;
+	uint minor;
+	int rc;
+	ulong lock_flags;
+	ulong lock_flags2;
+	u16 head;
 
 	major = MAJOR(tty_devnum(tty));
 	minor = MINOR(tty_devnum(tty));
 
 	if (major > 255)
-		return -ENXIO;
+		return -EIO;
 
 	/* Get board pointer from our array of majors we have allocated */
-	brd = dgap_BoardsByMajor[major];
+	brd = dgap_boards_by_major[major];
 	if (!brd)
-		return -ENXIO;
+		return -EIO;
 
 	/*
 	 * If board is not yet up to a state of READY, go to
@@ -2026,35 +1895,35 @@ static int dgap_tty_open(struct tty_struct *tty, struct file *file)
 	/* The wait above should guarantee this cannot happen */
 	if (brd->state != BOARD_READY) {
 		spin_unlock_irqrestore(&brd->bd_lock, lock_flags);
-		return -ENXIO;
+		return -EIO;
 	}
 
 	/* If opened device is greater than our number of ports, bail. */
 	if (MINOR(tty_devnum(tty)) > brd->nasync) {
 		spin_unlock_irqrestore(&brd->bd_lock, lock_flags);
-		return -ENXIO;
+		return -EIO;
 	}
 
 	ch = brd->channels[minor];
 	if (!ch) {
 		spin_unlock_irqrestore(&brd->bd_lock, lock_flags);
-		return -ENXIO;
+		return -EIO;
 	}
 
 	/* Grab channel lock */
 	spin_lock_irqsave(&ch->ch_lock, lock_flags2);
 
 	/* Figure out our type */
-	if (major == brd->dgap_Serial_Major) {
+	if (major == brd->dgap_serial_major) {
 		un = &brd->channels[minor]->ch_tun;
 		un->un_type = DGAP_SERIAL;
-	} else if (major == brd->dgap_TransparentPrint_Major) {
+	} else if (major == brd->dgap_transparent_print_major) {
 		un = &brd->channels[minor]->ch_pun;
 		un->un_type = DGAP_PRINT;
 	} else {
 		spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
 		spin_unlock_irqrestore(&brd->bd_lock, lock_flags);
-		return -ENXIO;
+		return -EIO;
 	}
 
 	/* Store our unit into driver_data, so we always have it available. */
@@ -2067,7 +1936,7 @@ static int dgap_tty_open(struct tty_struct *tty, struct file *file)
 	if (!bs) {
 		spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
 		spin_unlock_irqrestore(&brd->bd_lock, lock_flags);
-		return -ENXIO;
+		return -EIO;
 	}
 
 	/*
@@ -2145,18 +2014,18 @@ static int dgap_block_til_ready(struct tty_struct *tty, struct file *file,
 				struct channel_t *ch)
 {
 	int retval = 0;
-	struct un_t *un = NULL;
-	ulong   lock_flags;
-	uint	old_flags = 0;
-	int sleep_on_un_flags = 0;
+	struct un_t *un;
+	ulong lock_flags;
+	uint old_flags;
+	int sleep_on_un_flags;
 
 	if (!tty || tty->magic != TTY_MAGIC || !file || !ch ||
 		ch->magic != DGAP_CHANNEL_MAGIC)
-		return -ENXIO;
+		return -EIO;
 
 	un = tty->driver_data;
 	if (!un || un->magic != DGAP_UNIT_MAGIC)
-		return -ENXIO;
+		return -EIO;
 
 	spin_lock_irqsave(&ch->ch_lock, lock_flags);
 
@@ -2172,7 +2041,7 @@ static int dgap_block_til_ready(struct tty_struct *tty, struct file *file,
 		 * bail with error.
 		 */
 		if (ch->ch_bd->state == BOARD_FAILED) {
-			retval = -ENXIO;
+			retval = -EIO;
 			break;
 		}
 
@@ -2277,9 +2146,9 @@ static int dgap_block_til_ready(struct tty_struct *tty, struct file *file,
  */
 static void dgap_tty_hangup(struct tty_struct *tty)
 {
-	struct board_t	*bd;
+	struct board_t *bd;
 	struct channel_t *ch;
-	struct un_t	*un;
+	struct un_t *un;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -2298,7 +2167,6 @@ static void dgap_tty_hangup(struct tty_struct *tty)
 
 	/* flush the transmit queues */
 	dgap_tty_flush_buffer(tty);
-
 }
 
 /*
@@ -2312,7 +2180,6 @@ static void dgap_tty_close(struct tty_struct *tty, struct file *file)
 	struct channel_t *ch;
 	struct un_t *un;
 	ulong lock_flags;
-	int rc = 0;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -2378,7 +2245,7 @@ static void dgap_tty_close(struct tty_struct *tty, struct file *file)
 		/* wait for output to drain */
 		/* This will also return if we take an interrupt */
 
-		rc = dgap_wait_for_drain(tty);
+		dgap_wait_for_drain(tty);
 
 		dgap_tty_flush_buffer(tty);
 		tty_ldisc_flush(tty);
@@ -2441,17 +2308,17 @@ static void dgap_tty_close(struct tty_struct *tty, struct file *file)
  */
 static int dgap_tty_chars_in_buffer(struct tty_struct *tty)
 {
-	struct board_t *bd = NULL;
-	struct channel_t *ch = NULL;
-	struct un_t *un = NULL;
-	struct bs_t *bs = NULL;
-	uchar tbusy;
-	uint chars = 0;
+	struct board_t *bd;
+	struct channel_t *ch;
+	struct un_t *un;
+	struct bs_t __iomem *bs;
+	u8 tbusy;
+	uint chars;
 	u16 thead, ttail, tmask, chead, ctail;
-	ulong   lock_flags = 0;
-	ulong   lock_flags2 = 0;
+	ulong lock_flags = 0;
+	ulong lock_flags2 = 0;
 
-	if (tty == NULL)
+	if (!tty)
 		return 0;
 
 	un = tty->driver_data;
@@ -2535,27 +2402,25 @@ static int dgap_wait_for_drain(struct tty_struct *tty)
 {
 	struct channel_t *ch;
 	struct un_t *un;
-	struct bs_t *bs;
-	int ret = -EIO;
+	struct bs_t __iomem *bs;
+	int ret = 0;
 	uint count = 1;
-	ulong   lock_flags = 0;
+	ulong lock_flags = 0;
 
 	if (!tty || tty->magic != TTY_MAGIC)
-		return ret;
+		return -EIO;
 
 	un = tty->driver_data;
 	if (!un || un->magic != DGAP_UNIT_MAGIC)
-		return ret;
+		return -EIO;
 
 	ch = un->un_ch;
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
-		return ret;
+		return -EIO;
 
 	bs = ch->ch_bs;
 	if (!bs)
-		return ret;
-
-	ret = 0;
+		return -EIO;
 
 	/* Loop until data is drained */
 	while (count != 0) {
@@ -2596,10 +2461,10 @@ static int dgap_wait_for_drain(struct tty_struct *tty)
  */
 static int dgap_maxcps_room(struct tty_struct *tty, int bytes_available)
 {
-	struct channel_t *ch = NULL;
-	struct un_t *un = NULL;
+	struct channel_t *ch;
+	struct un_t *un;
 
-	if (tty == NULL)
+	if (!tty)
 		return bytes_available;
 
 	un = tty->driver_data;
@@ -2645,8 +2510,8 @@ static int dgap_maxcps_room(struct tty_struct *tty, int bytes_available)
 
 static inline void dgap_set_firmware_event(struct un_t *un, unsigned int event)
 {
-	struct channel_t *ch = NULL;
-	struct bs_t *bs = NULL;
+	struct channel_t *ch;
+	struct bs_t __iomem *bs;
 
 	if (!un || un->magic != DGAP_UNIT_MAGIC)
 		return;
@@ -2678,12 +2543,12 @@ static inline void dgap_set_firmware_event(struct un_t *un, unsigned int event)
  */
 static int dgap_tty_write_room(struct tty_struct *tty)
 {
-	struct channel_t *ch = NULL;
-	struct un_t *un = NULL;
-	struct bs_t *bs = NULL;
+	struct channel_t *ch;
+	struct un_t *un;
+	struct bs_t __iomem *bs;
 	u16 head, tail, tmask;
-	int ret = 0;
-	ulong   lock_flags = 0;
+	int ret;
+	ulong lock_flags = 0;
 
 	if (!tty)
 		return 0;
@@ -2767,13 +2632,13 @@ static int dgap_tty_put_char(struct tty_struct *tty, unsigned char c)
 static int dgap_tty_write(struct tty_struct *tty, const unsigned char *buf,
 				int count)
 {
-	struct channel_t *ch = NULL;
-	struct un_t *un = NULL;
-	struct bs_t *bs = NULL;
-	char *vaddr = NULL;
+	struct channel_t *ch;
+	struct un_t *un;
+	struct bs_t __iomem *bs;
+	char __iomem *vaddr;
 	u16 head, tail, tmask, remain;
-	int bufcount = 0, n = 0;
-	int orig_count = 0;
+	int bufcount, n;
+	int orig_count;
 	ulong lock_flags;
 
 	if (!tty)
@@ -2855,16 +2720,6 @@ static int dgap_tty_write(struct tty_struct *tty, const unsigned char *buf,
 		ch->ch_flags &= ~CH_PRON;
 	}
 
-	/*
-	 * If there is nothing left to copy, or
-	 * I can't handle any more data, leave.
-	 */
-	if (count <= 0) {
-		dgap_set_firmware_event(un, UN_LOW | UN_EMPTY);
-		spin_unlock_irqrestore(&ch->ch_lock, lock_flags);
-		return 0;
-	}
-
 	n = count;
 
 	/*
@@ -2878,9 +2733,7 @@ static int dgap_tty_write(struct tty_struct *tty, const unsigned char *buf,
 		n -= remain;
 		vaddr = ch->ch_taddr + head;
 
-		memcpy_toio(vaddr, (uchar *) buf, remain);
-		dgap_sniff_nowait_nolock(ch, "USER WRITE", (uchar *) buf,
-					remain);
+		memcpy_toio(vaddr, (u8 *) buf, remain);
 
 		head = ch->ch_tstart;
 		buf += remain;
@@ -2894,10 +2747,7 @@ static int dgap_tty_write(struct tty_struct *tty, const unsigned char *buf,
 		vaddr = ch->ch_taddr + head;
 		remain = n;
 
-		memcpy_toio(vaddr, (uchar *) buf, remain);
-		dgap_sniff_nowait_nolock(ch, "USER WRITE", (uchar *)buf,
-					remain);
-
+		memcpy_toio(vaddr, (u8 *) buf, remain);
 		head += remain;
 
 	}
@@ -2949,20 +2799,20 @@ static int dgap_tty_tiocmget(struct tty_struct *tty)
 {
 	struct channel_t *ch;
 	struct un_t *un;
-	int result = -EIO;
-	uchar mstat = 0;
+	int result;
+	u8 mstat;
 	ulong lock_flags;
 
 	if (!tty || tty->magic != TTY_MAGIC)
-		return result;
+		return -EIO;
 
 	un = tty->driver_data;
 	if (!un || un->magic != DGAP_UNIT_MAGIC)
-		return result;
+		return -EIO;
 
 	ch = un->un_ch;
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
-		return result;
+		return -EIO;
 
 	spin_lock_irqsave(&ch->ch_lock, lock_flags);
 
@@ -3001,24 +2851,23 @@ static int dgap_tty_tiocmset(struct tty_struct *tty,
 	struct board_t *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	int ret = -EIO;
 	ulong lock_flags;
 	ulong lock_flags2;
 
 	if (!tty || tty->magic != TTY_MAGIC)
-		return ret;
+		return -EIO;
 
 	un = tty->driver_data;
 	if (!un || un->magic != DGAP_UNIT_MAGIC)
-		return ret;
+		return -EIO;
 
 	ch = un->un_ch;
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
-		return ret;
+		return -EIO;
 
 	bd = ch->ch_bd;
 	if (!bd || bd->magic != DGAP_BOARD_MAGIC)
-		return ret;
+		return -EIO;
 
 	spin_lock_irqsave(&bd->bd_lock, lock_flags);
 	spin_lock_irqsave(&ch->ch_lock, lock_flags2);
@@ -3061,24 +2910,23 @@ static int dgap_tty_send_break(struct tty_struct *tty, int msec)
 	struct board_t *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	int ret = -EIO;
 	ulong lock_flags;
 	ulong lock_flags2;
 
 	if (!tty || tty->magic != TTY_MAGIC)
-		return ret;
+		return -EIO;
 
 	un = tty->driver_data;
 	if (!un || un->magic != DGAP_UNIT_MAGIC)
-		return ret;
+		return -EIO;
 
 	ch = un->un_ch;
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
-		return ret;
+		return -EIO;
 
 	bd = ch->ch_bd;
 	if (!bd || bd->magic != DGAP_BOARD_MAGIC)
-		return ret;
+		return -EIO;
 
 	switch (msec) {
 	case -1:
@@ -3175,13 +3023,13 @@ static void dgap_tty_send_xchar(struct tty_struct *tty, char c)
  */
 static int dgap_get_modem_info(struct channel_t *ch, unsigned int __user *value)
 {
-	int result = 0;
-	uchar mstat = 0;
+	int result;
+	u8 mstat;
 	ulong lock_flags;
-	int rc = 0;
+	int rc;
 
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
-		return -ENXIO;
+		return -EIO;
 
 	spin_lock_irqsave(&ch->ch_lock, lock_flags);
 
@@ -3222,25 +3070,25 @@ static int dgap_set_modem_info(struct tty_struct *tty, unsigned int command,
 	struct board_t *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	int ret = -ENXIO;
-	unsigned int arg = 0;
+	int ret;
+	unsigned int arg;
 	ulong lock_flags;
 	ulong lock_flags2;
 
 	if (!tty || tty->magic != TTY_MAGIC)
-		return ret;
+		return -EIO;
 
 	un = tty->driver_data;
 	if (!un || un->magic != DGAP_UNIT_MAGIC)
-		return ret;
+		return -EIO;
 
 	ch = un->un_ch;
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
-		return ret;
+		return -EIO;
 
 	bd = ch->ch_bd;
 	if (!bd || bd->magic != DGAP_BOARD_MAGIC)
-		return ret;
+		return -EIO;
 
 	ret = get_user(arg, value);
 	if (ret)
@@ -3360,7 +3208,7 @@ static int dgap_tty_digiseta(struct tty_struct *tty,
 	struct channel_t *ch;
 	struct un_t *un;
 	struct digi_t new_digi;
-	ulong   lock_flags = 0;
+	ulong lock_flags = 0;
 	unsigned long lock_flags2;
 
 	if (!tty || tty->magic != TTY_MAGIC)
@@ -3640,8 +3488,8 @@ static void dgap_tty_throttle(struct tty_struct *tty)
 	struct board_t *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	ulong   lock_flags;
-	ulong   lock_flags2;
+	ulong lock_flags;
+	ulong lock_flags2;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -3676,8 +3524,8 @@ static void dgap_tty_unthrottle(struct tty_struct *tty)
 	struct board_t *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	ulong   lock_flags;
-	ulong   lock_flags2;
+	ulong lock_flags;
+	ulong lock_flags2;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -3712,8 +3560,8 @@ static void dgap_tty_start(struct tty_struct *tty)
 	struct board_t *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	ulong   lock_flags;
-	ulong   lock_flags2;
+	ulong lock_flags;
+	ulong lock_flags2;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -3737,7 +3585,6 @@ static void dgap_tty_start(struct tty_struct *tty)
 
 	spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
 	spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
-
 }
 
 static void dgap_tty_stop(struct tty_struct *tty)
@@ -3745,8 +3592,8 @@ static void dgap_tty_stop(struct tty_struct *tty)
 	struct board_t *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	ulong   lock_flags;
-	ulong   lock_flags2;
+	ulong lock_flags;
+	ulong lock_flags2;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -3770,7 +3617,6 @@ static void dgap_tty_stop(struct tty_struct *tty)
 
 	spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
 	spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
-
 }
 
 /*
@@ -3791,8 +3637,8 @@ static void dgap_tty_flush_chars(struct tty_struct *tty)
 	struct board_t *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	ulong   lock_flags;
-	ulong   lock_flags2;
+	ulong lock_flags;
+	ulong lock_flags2;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -3828,9 +3674,9 @@ static void dgap_tty_flush_buffer(struct tty_struct *tty)
 	struct board_t *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	ulong   lock_flags;
-	ulong   lock_flags2;
-	u16	head = 0;
+	ulong lock_flags;
+	ulong lock_flags2;
+	u16 head;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -3888,9 +3734,9 @@ static int dgap_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 	struct channel_t *ch;
 	struct un_t *un;
 	int rc;
-	u16	head = 0;
-	ulong   lock_flags = 0;
-	ulong   lock_flags2 = 0;
+	u16 head;
+	ulong lock_flags = 0;
+	ulong lock_flags2 = 0;
 	void __user *uarg = (void __user *) arg;
 
 	if (!tty || tty->magic != TTY_MAGIC)
@@ -4266,18 +4112,18 @@ static int dgap_after_config_loaded(int board)
 	/*
 	 * Initialize KME waitqueues...
 	 */
-	init_waitqueue_head(&(dgap_Board[board]->kme_wait));
+	init_waitqueue_head(&(dgap_board[board]->kme_wait));
 
 	/*
 	 * allocate flip buffer for board.
 	 */
-	dgap_Board[board]->flipbuf = kmalloc(MYFLIPLEN, GFP_ATOMIC);
-	if (!dgap_Board[board]->flipbuf)
+	dgap_board[board]->flipbuf = kmalloc(MYFLIPLEN, GFP_KERNEL);
+	if (!dgap_board[board]->flipbuf)
 		return -ENOMEM;
 
-	dgap_Board[board]->flipflagbuf = kmalloc(MYFLIPLEN, GFP_ATOMIC);
-	if (!dgap_Board[board]->flipflagbuf) {
-		kfree(dgap_Board[board]->flipbuf);
+	dgap_board[board]->flipflagbuf = kmalloc(MYFLIPLEN, GFP_KERNEL);
+	if (!dgap_board[board]->flipflagbuf) {
+		kfree(dgap_board[board]->flipbuf);
 		return -ENOMEM;
 	}
 
@@ -4292,36 +4138,37 @@ static int dgap_tty_register_ports(struct board_t *brd)
 	struct channel_t *ch;
 	int i;
 
-	brd->SerialPorts = kcalloc(brd->nasync, sizeof(*brd->SerialPorts),
+	brd->serial_ports = kcalloc(brd->nasync, sizeof(*brd->serial_ports),
 					GFP_KERNEL);
-	if (brd->SerialPorts == NULL)
+	if (!brd->serial_ports)
 		return -ENOMEM;
-	for (i = 0; i < brd->nasync; i++)
-		tty_port_init(&brd->SerialPorts[i]);
 
-	brd->PrinterPorts = kcalloc(brd->nasync, sizeof(*brd->PrinterPorts),
+	brd->printer_ports = kcalloc(brd->nasync, sizeof(*brd->printer_ports),
 					GFP_KERNEL);
-	if (brd->PrinterPorts == NULL) {
-		kfree(brd->SerialPorts);
+	if (!brd->printer_ports) {
+		kfree(brd->serial_ports);
 		return -ENOMEM;
 	}
-	for (i = 0; i < brd->nasync; i++)
-		tty_port_init(&brd->PrinterPorts[i]);
+
+	for (i = 0; i < brd->nasync; i++) {
+		tty_port_init(&brd->serial_ports[i]);
+		tty_port_init(&brd->printer_ports[i]);
+	}
 
 	ch = brd->channels[0];
 	for (i = 0; i < brd->nasync; i++, ch = brd->channels[i]) {
 
 		struct device *classp;
 
-		classp = tty_port_register_device(&brd->SerialPorts[i],
-					brd->SerialDriver,
+		classp = tty_port_register_device(&brd->serial_ports[i],
+					brd->serial_driver,
 					brd->firstminor + i, NULL);
 
 		dgap_create_tty_sysfs(&ch->ch_tun, classp);
 		ch->ch_tun.un_sysfs = classp;
 
-		classp = tty_port_register_device(&brd->PrinterPorts[i],
-					brd->PrintDriver,
+		classp = tty_port_register_device(&brd->printer_ports[i],
+					brd->print_driver,
 					brd->firstminor + i, NULL);
 
 		dgap_create_tty_sysfs(&ch->ch_pun, classp);
@@ -4336,9 +4183,9 @@ static int dgap_tty_register_ports(struct board_t *brd)
  * Copies the BIOS code from the user to the board,
  * and starts the BIOS running.
  */
-static void dgap_do_bios_load(struct board_t *brd, const uchar *ubios, int len)
+static void dgap_do_bios_load(struct board_t *brd, const u8 *ubios, int len)
 {
-	uchar *addr;
+	u8 __iomem *addr;
 	uint offset;
 	int i;
 
@@ -4369,16 +4216,15 @@ static void dgap_do_bios_load(struct board_t *brd, const uchar *ubios, int len)
 /*
  * Checks to see if the BIOS completed running on the card.
  */
-static int dgap_do_wait_for_bios(struct board_t *brd)
+static int dgap_test_bios(struct board_t *brd)
 {
-	uchar *addr;
+	u8 __iomem *addr;
 	u16 word;
 	u16 err1;
 	u16 err2;
-	int ret = 0;
 
 	if (!brd || (brd->magic != DGAP_BOARD_MAGIC) || !brd->re_map_membase)
-		return ret;
+		return -EINVAL;
 
 	addr = brd->re_map_membase;
 	word = readw(addr + POSTAREA);
@@ -4392,7 +4238,7 @@ static int dgap_do_wait_for_bios(struct board_t *brd)
 	while (brd->wait_for_bios < 1000) {
 		/* Check to see if BIOS thinks board is good. (GD). */
 		if (word == *(u16 *) "GD")
-			return 1;
+			return 0;
 		msleep_interruptible(10);
 		brd->wait_for_bios++;
 		word = readw(addr + POSTAREA);
@@ -4406,16 +4252,16 @@ static int dgap_do_wait_for_bios(struct board_t *brd)
 	brd->state = BOARD_FAILED;
 	brd->dpastatus = BD_NOBIOS;
 
-	return ret;
+	return -EIO;
 }
 
 /*
  * Copies the FEP code from the user to the board,
  * and starts the FEP running.
  */
-static void dgap_do_fep_load(struct board_t *brd, const uchar *ufep, int len)
+static void dgap_do_fep_load(struct board_t *brd, const u8 *ufep, int len)
 {
-	uchar *addr;
+	u8 __iomem *addr;
 	uint offset;
 
 	if (!brd || (brd->magic != DGAP_BOARD_MAGIC) || !brd->re_map_membase)
@@ -4434,8 +4280,9 @@ static void dgap_do_fep_load(struct board_t *brd, const uchar *ufep, int len)
 	 * it its config string describing how the concentrators look.
 	 */
 	if ((brd->type == PCX) || (brd->type == PEPC)) {
-		uchar string[100];
-		uchar *config, *xconfig;
+		u8 string[100];
+		u8 __iomem *config;
+		u8 *xconfig;
 		int i = 0;
 
 		xconfig = dgap_create_config_string(brd, string);
@@ -4457,16 +4304,15 @@ static void dgap_do_fep_load(struct board_t *brd, const uchar *ufep, int len)
 /*
  * Waits for the FEP to report thats its ready for us to use.
  */
-static int dgap_do_wait_for_fep(struct board_t *brd)
+static int dgap_test_fep(struct board_t *brd)
 {
-	uchar *addr;
+	u8 __iomem *addr;
 	u16 word;
 	u16 err1;
 	u16 err2;
-	int ret = 0;
 
 	if (!brd || (brd->magic != DGAP_BOARD_MAGIC) || !brd->re_map_membase)
-		return ret;
+		return -EINVAL;
 
 	addr = brd->re_map_membase;
 	word = readw(addr + FEPSTAT);
@@ -4486,7 +4332,7 @@ static int dgap_do_wait_for_fep(struct board_t *brd)
 			if (word == *(u16 *) "5A")
 				brd->bd_flags |= BD_FEP5PLUS;
 
-			return 1;
+			return 0;
 		}
 		msleep_interruptible(10);
 		brd->wait_for_fep++;
@@ -4501,7 +4347,7 @@ static int dgap_do_wait_for_fep(struct board_t *brd)
 	brd->state = BOARD_FAILED;
 	brd->dpastatus = BD_NOFEP;
 
-	return ret;
+	return -EIO;
 }
 
 /*
@@ -4509,10 +4355,10 @@ static int dgap_do_wait_for_fep(struct board_t *brd)
  */
 static void dgap_do_reset_board(struct board_t *brd)
 {
-	uchar check;
+	u8 check;
 	u32 check1;
 	u32 check2;
-	int i = 0;
+	int i;
 
 	if (!brd || (brd->magic != DGAP_BOARD_MAGIC) ||
 	    !brd->re_map_membase || !brd->re_map_port)
@@ -4550,17 +4396,16 @@ static void dgap_do_reset_board(struct board_t *brd)
 		brd->dpastatus = BD_NOFEP;
 		return;
 	}
-
 }
 
 #ifdef DIGI_CONCENTRATORS_SUPPORTED
 /*
  * Sends a concentrator image into the FEP5 board.
  */
-static void dgap_do_conc_load(struct board_t *brd, uchar *uaddr, int len)
+static void dgap_do_conc_load(struct board_t *brd, u8 *uaddr, int len)
 {
-	char *vaddr;
-	u16 offset = 0;
+	char __iomem *vaddr;
+	u16 offset;
 	struct downld_t *to_dp;
 
 	if (!brd || (brd->magic != DGAP_BOARD_MAGIC) || !brd->re_map_membase)
@@ -4590,8 +4435,8 @@ static void dgap_get_vpd(struct board_t *brd)
 	u16 vpd_offset;
 	u16 image_length;
 	u16 i;
-	uchar byte1;
-	uchar byte2;
+	u8 byte1;
+	u8 byte2;
 
 	/*
 	 * Poke the magic number at the PCI Rom Address location.
@@ -4696,8 +4541,8 @@ static void dgap_get_vpd(struct board_t *brd)
 static void dgap_poll_tasklet(unsigned long data)
 {
 	struct board_t *bd = (struct board_t *) data;
-	ulong  lock_flags;
-	char *vaddr;
+	ulong lock_flags;
+	char __iomem *vaddr;
 	u16 head, tail;
 
 	if (!bd || (bd->magic != DGAP_BOARD_MAGIC))
@@ -4715,7 +4560,7 @@ static void dgap_poll_tasklet(unsigned long data)
 	 */
 	if (bd->state == BOARD_READY) {
 
-		struct ev_t *eaddr = NULL;
+		struct ev_t __iomem *eaddr;
 
 		if (!bd->re_map_membase) {
 			spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
@@ -4729,7 +4574,7 @@ static void dgap_poll_tasklet(unsigned long data)
 		if (!bd->nasync)
 			goto out;
 
-		eaddr = (struct ev_t *) (vaddr + EVBUF);
+		eaddr = (struct ev_t __iomem *) (vaddr + EVBUF);
 
 		/* Get our head and tail */
 		head = readw(&(eaddr->ev_head));
@@ -4770,15 +4615,15 @@ out:
  *                        in the cmd buffer before returning.
  *
  *=======================================================================*/
-static void dgap_cmdb(struct channel_t *ch, uchar cmd, uchar byte1,
-			uchar byte2, uint ncmds)
+static void dgap_cmdb(struct channel_t *ch, u8 cmd, u8 byte1,
+			u8 byte2, uint ncmds)
 {
-	char		*vaddr = NULL;
-	struct cm_t	*cm_addr = NULL;
-	uint		count;
-	uint		n;
-	u16		head;
-	u16		tail;
+	char __iomem *vaddr;
+	struct __iomem cm_t *cm_addr;
+	uint count;
+	uint n;
+	u16 head;
+	u16 tail;
 
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
 		return;
@@ -4798,7 +4643,7 @@ static void dgap_cmdb(struct channel_t *ch, uchar cmd, uchar byte1,
 	if (!vaddr)
 		return;
 
-	cm_addr = (struct cm_t *) (vaddr + CMDBUF);
+	cm_addr = (struct cm_t __iomem *) (vaddr + CMDBUF);
 	head = readw(&(cm_addr->cm_head));
 
 	/*
@@ -4812,10 +4657,10 @@ static void dgap_cmdb(struct channel_t *ch, uchar cmd, uchar byte1,
 	/*
 	 * Put the data in the circular command buffer.
 	 */
-	writeb(cmd, (char *) (vaddr + head + CMDSTART + 0));
-	writeb((uchar) ch->ch_portnum, (char *) (vaddr + head + CMDSTART + 1));
-	writeb(byte1, (char *) (vaddr + head + CMDSTART + 2));
-	writeb(byte2, (char *) (vaddr + head + CMDSTART + 3));
+	writeb(cmd, (vaddr + head + CMDSTART + 0));
+	writeb((u8) ch->ch_portnum, (vaddr + head + CMDSTART + 1));
+	writeb(byte1, (vaddr + head + CMDSTART + 2));
+	writeb(byte2, (vaddr + head + CMDSTART + 3));
 
 	head = (head + 4) & (CMDMAX - CMDSTART - 4);
 
@@ -4856,14 +4701,14 @@ static void dgap_cmdb(struct channel_t *ch, uchar cmd, uchar byte1,
  *                        in the cmd buffer before returning.
  *
  *=======================================================================*/
-static void dgap_cmdw(struct channel_t *ch, uchar cmd, u16 word, uint ncmds)
+static void dgap_cmdw(struct channel_t *ch, u8 cmd, u16 word, uint ncmds)
 {
-	char		*vaddr = NULL;
-	struct cm_t	*cm_addr = NULL;
-	uint		count;
-	uint		n;
-	u16		head;
-	u16		tail;
+	char __iomem *vaddr;
+	struct __iomem cm_t *cm_addr;
+	uint count;
+	uint n;
+	u16 head;
+	u16 tail;
 
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
 		return;
@@ -4882,7 +4727,7 @@ static void dgap_cmdw(struct channel_t *ch, uchar cmd, u16 word, uint ncmds)
 	if (!vaddr)
 		return;
 
-	cm_addr = (struct cm_t *) (vaddr + CMDBUF);
+	cm_addr = (struct cm_t __iomem *) (vaddr + CMDBUF);
 	head = readw(&(cm_addr->cm_head));
 
 	/*
@@ -4896,9 +4741,9 @@ static void dgap_cmdw(struct channel_t *ch, uchar cmd, u16 word, uint ncmds)
 	/*
 	 * Put the data in the circular command buffer.
 	 */
-	writeb(cmd, (char *) (vaddr + head + CMDSTART + 0));
-	writeb((uchar) ch->ch_portnum, (char *) (vaddr + head + CMDSTART + 1));
-	writew((u16) word, (char *) (vaddr + head + CMDSTART + 2));
+	writeb(cmd, (vaddr + head + CMDSTART + 0));
+	writeb((u8) ch->ch_portnum, (vaddr + head + CMDSTART + 1));
+	writew((u16) word, (vaddr + head + CMDSTART + 2));
 
 	head = (head + 4) & (CMDMAX - CMDSTART - 4);
 
@@ -4941,12 +4786,12 @@ static void dgap_cmdw(struct channel_t *ch, uchar cmd, u16 word, uint ncmds)
  *=======================================================================*/
 static void dgap_cmdw_ext(struct channel_t *ch, u16 cmd, u16 word, uint ncmds)
 {
-	char		*vaddr = NULL;
-	struct cm_t	*cm_addr = NULL;
-	uint		count;
-	uint		n;
-	u16		head;
-	u16		tail;
+	char __iomem *vaddr;
+	struct __iomem cm_t *cm_addr;
+	uint count;
+	uint n;
+	u16 head;
+	u16 tail;
 
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
 		return;
@@ -4965,7 +4810,7 @@ static void dgap_cmdw_ext(struct channel_t *ch, u16 cmd, u16 word, uint ncmds)
 	if (!vaddr)
 		return;
 
-	cm_addr = (struct cm_t *) (vaddr + CMDBUF);
+	cm_addr = (struct cm_t __iomem *) (vaddr + CMDBUF);
 	head = readw(&(cm_addr->cm_head));
 
 	/*
@@ -4981,19 +4826,19 @@ static void dgap_cmdw_ext(struct channel_t *ch, u16 cmd, u16 word, uint ncmds)
 	 */
 
 	/* Write an FF to tell the FEP that we want an extended command */
-	writeb((uchar) 0xff, (char *) (vaddr + head + CMDSTART + 0));
+	writeb((u8) 0xff, (vaddr + head + CMDSTART + 0));
 
-	writeb((uchar) ch->ch_portnum, (uchar *) (vaddr + head + CMDSTART + 1));
-	writew((u16) cmd, (char *) (vaddr + head + CMDSTART + 2));
+	writeb((u8) ch->ch_portnum, (vaddr + head + CMDSTART + 1));
+	writew((u16) cmd, (vaddr + head + CMDSTART + 2));
 
 	/*
 	 * If the second part of the command won't fit,
 	 * put it at the beginning of the circular buffer.
 	 */
 	if (((head + 4) >= ((CMDMAX - CMDSTART)) || (head & 03)))
-		writew((u16) word, (char *) (vaddr + CMDSTART));
+		writew((u16) word, (vaddr + CMDSTART));
 	else
-		writew((u16) word, (char *) (vaddr + head + CMDSTART + 4));
+		writew((u16) word, (vaddr + head + CMDSTART + 4));
 
 	head = (head + 8) & (CMDMAX - CMDSTART - 4);
 
@@ -5034,10 +4879,10 @@ static void dgap_cmdw_ext(struct channel_t *ch, u16 cmd, u16 word, uint ncmds)
  *=======================================================================*/
 static void dgap_wmove(struct channel_t *ch, char *buf, uint cnt)
 {
-	int    n;
-	char   *taddr;
-	struct bs_t    *bs;
-	u16    head;
+	int n;
+	char __iomem *taddr;
+	struct bs_t __iomem *bs;
+	u16 head;
 
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
 		return;
@@ -5088,9 +4933,9 @@ static void dgap_wmove(struct channel_t *ch, char *buf, uint cnt)
  */
 static uint dgap_get_custom_baud(struct channel_t *ch)
 {
-	uchar *vaddr;
-	ulong offset = 0;
-	uint value = 0;
+	u8 __iomem *vaddr;
+	ulong offset;
+	uint value;
 
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
 		return 0;
@@ -5110,8 +4955,8 @@ static uint dgap_get_custom_baud(struct channel_t *ch)
 	 * Go get from fep mem, what the fep
 	 * believes the custom baud rate is.
 	 */
-	offset = ((((*(unsigned short *)(vaddr + ECS_SEG)) << 4) +
-		(ch->ch_portnum * 0x28) + LINE_SPEED));
+	offset = (ioread16(vaddr + ECS_SEG) << 4) + (ch->ch_portnum * 0x28)
+	       + LINE_SPEED;
 
 	value = readw(vaddr + offset);
 	return value;
@@ -5155,32 +5000,32 @@ static int dgap_param(struct tty_struct *tty)
 	struct ktermios *ts;
 	struct board_t *bd;
 	struct channel_t *ch;
-	struct bs_t   *bs;
-	struct un_t   *un;
-	u16	head;
-	u16	cflag;
-	u16	iflag;
-	uchar	mval;
-	uchar	hflow;
+	struct bs_t __iomem *bs;
+	struct un_t *un;
+	u16 head;
+	u16 cflag;
+	u16 iflag;
+	u8 mval;
+	u8 hflow;
 
 	if (!tty || tty->magic != TTY_MAGIC)
-		return -ENXIO;
+		return -EIO;
 
 	un = (struct un_t *) tty->driver_data;
 	if (!un || un->magic != DGAP_UNIT_MAGIC)
-		return -ENXIO;
+		return -EIO;
 
 	ch = un->un_ch;
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
-		return -ENXIO;
+		return -EIO;
 
 	bd = ch->ch_bd;
 	if (!bd || bd->magic != DGAP_BOARD_MAGIC)
-		return -ENXIO;
+		return -EIO;
 
 	bs = ch->ch_bs;
 	if (!bs)
-		return -ENXIO;
+		return -EIO;
 
 	ts = &tty->termios;
 
@@ -5427,10 +5272,9 @@ static int dgap_param(struct tty_struct *tty)
 		ch->ch_hflow = hflow;
 
 		/* Okay to have channel and board locks held calling this */
-		dgap_cmdb(ch, SHFLOW, (uchar) hflow, 0xff, 0);
+		dgap_cmdb(ch, SHFLOW, (u8) hflow, 0xff, 0);
 	}
 
-
 	/*
 	 * Set RTS and/or DTR Toggle if needed,
 	 * but only if product is FEP5+ based.
@@ -5455,7 +5299,7 @@ static int dgap_param(struct tty_struct *tty)
 		ch->ch_mostat = mval;
 
 		/* Okay to have channel and board locks held calling this */
-		dgap_cmdb(ch, SMODEM, (uchar) mval, D_RTS(ch)|D_DTR(ch), 0);
+		dgap_cmdb(ch, SMODEM, (u8) mval, D_RTS(ch)|D_DTR(ch), 0);
 	}
 
 	/*
@@ -5606,21 +5450,21 @@ static void dgap_write_wakeup(struct board_t *bd, struct channel_t *ch,
 static int dgap_event(struct board_t *bd)
 {
 	struct channel_t *ch;
-	ulong		lock_flags;
-	ulong		lock_flags2;
-	struct bs_t	*bs;
-	uchar		*event;
-	uchar		*vaddr = NULL;
-	struct ev_t	*eaddr = NULL;
-	uint		head;
-	uint		tail;
-	int		port;
-	int		reason;
-	int		modem;
-	int		b1;
+	ulong lock_flags;
+	ulong lock_flags2;
+	struct bs_t __iomem *bs;
+	u8 __iomem *event;
+	u8 __iomem *vaddr;
+	struct ev_t __iomem *eaddr;
+	uint head;
+	uint tail;
+	int port;
+	int reason;
+	int modem;
+	int b1;
 
 	if (!bd || bd->magic != DGAP_BOARD_MAGIC)
-		return -ENXIO;
+		return -EIO;
 
 	spin_lock_irqsave(&bd->bd_lock, lock_flags);
 
@@ -5628,10 +5472,10 @@ static int dgap_event(struct board_t *bd)
 
 	if (!vaddr) {
 		spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
-		return -ENXIO;
+		return -EIO;
 	}
 
-	eaddr = (struct ev_t *) (vaddr + EVBUF);
+	eaddr = (struct ev_t __iomem *) (vaddr + EVBUF);
 
 	/* Get our head and tail */
 	head = readw(&(eaddr->ev_head));
@@ -5645,7 +5489,7 @@ static int dgap_event(struct board_t *bd)
 	    (head | tail) & 03) {
 		/* Let go of board lock */
 		spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
-		return -ENXIO;
+		return -EIO;
 	}
 
 	/*
@@ -5659,10 +5503,10 @@ static int dgap_event(struct board_t *bd)
 
 		event = bd->re_map_membase + tail + EVSTART;
 
-		port   = event[0];
-		reason = event[1];
-		modem  = event[2];
-		b1     = event[3];
+		port   = ioread8(event);
+		reason = ioread8(event + 1);
+		modem  = ioread8(event + 2);
+		b1     = ioread8(event + 3);
 
 		/*
 		 * Make sure the interrupt is valid.
@@ -5795,7 +5639,7 @@ static DRIVER_ATTR(version, S_IRUSR, dgap_driver_version_show, NULL);
 
 static ssize_t dgap_driver_boards_show(struct device_driver *ddp, char *buf)
 {
-	return snprintf(buf, PAGE_SIZE, "%d\n", dgap_NumBoards);
+	return snprintf(buf, PAGE_SIZE, "%d\n", dgap_numboards);
 }
 static DRIVER_ATTR(boards, S_IRUSR, dgap_driver_boards_show, NULL);
 
@@ -5873,7 +5717,7 @@ static ssize_t dgap_ports_state_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i = 0;
+	int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -5894,7 +5738,7 @@ static ssize_t dgap_ports_baud_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i = 0;
+	int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -5915,7 +5759,7 @@ static ssize_t dgap_ports_msignals_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i = 0;
+	int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -5952,7 +5796,7 @@ static ssize_t dgap_ports_iflag_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i = 0;
+	int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -5972,7 +5816,7 @@ static ssize_t dgap_ports_cflag_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i = 0;
+	int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -5992,7 +5836,7 @@ static ssize_t dgap_ports_oflag_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i = 0;
+	int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -6012,7 +5856,7 @@ static ssize_t dgap_ports_lflag_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i = 0;
+	int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -6032,7 +5876,7 @@ static ssize_t dgap_ports_digi_flag_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i = 0;
+	int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -6052,7 +5896,7 @@ static ssize_t dgap_ports_rxcount_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i = 0;
+	int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -6072,7 +5916,7 @@ static ssize_t dgap_ports_txcount_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i = 0;
+	int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -6396,13 +6240,13 @@ static ssize_t dgap_tty_name_show(struct device *d,
 	struct board_t *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	int	cn;
-	int	bn;
-	struct cnode *cptr = NULL;
+	int cn;
+	int bn;
+	struct cnode *cptr;
 	int found = FALSE;
 	int ncount = 0;
 	int starto = 0;
-	int i = 0;
+	int i;
 
 	if (!d)
 		return 0;
@@ -6494,7 +6338,6 @@ static ssize_t dgap_tty_name_show(struct device *d,
 
 	return snprintf(buf, PAGE_SIZE, "%s_dgap_%d_%d\n",
 		(un->un_type == DGAP_PRINT) ? "pr" : "tty", bn, cn);
-
 }
 static DEVICE_ATTR(custom_name, S_IRUSR, dgap_tty_name_show, NULL);
 
@@ -6538,18 +6381,18 @@ static void dgap_remove_tty_sysfs(struct device *c)
 /*
  * Parse a configuration file read into memory as a string.
  */
-static int	dgap_parsefile(char **in, int Remove)
+static int dgap_parsefile(char **in, int remove)
 {
 	struct cnode *p, *brd, *line, *conc;
-	int	rc;
-	char	*s = NULL;
-	int	linecnt = 0;
+	int rc;
+	char *s;
+	int linecnt = 0;
 
 	p = &dgap_head;
 	brd = line = conc = NULL;
 
 	/* perhaps we are adding to an existing list? */
-	while (p->next != NULL)
+	while (p->next)
 		p = p->next;
 
 	/* file must start with a BEGIN */
@@ -6589,7 +6432,7 @@ static int	dgap_parsefile(char **in, int Remove)
 			}
 			p = p->next;
 
-			p->u.board.status = dgap_savestring("No");
+			p->u.board.status = kstrdup("No", GFP_KERNEL);
 			line = conc = NULL;
 			brd = p;
 			linecnt = -1;
@@ -6683,11 +6526,11 @@ static int	dgap_parsefile(char **in, int Remove)
 				return -1;
 			}
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
-			p->u.board.portstr = dgap_savestring(s);
+			p->u.board.portstr = kstrdup(s, GFP_KERNEL);
 			if (kstrtol(s, 0, &p->u.board.port)) {
 				dgap_err("bad number for IO port");
 				return -1;
@@ -6701,11 +6544,11 @@ static int	dgap_parsefile(char **in, int Remove)
 				return -1;
 			}
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
-			p->u.board.addrstr = dgap_savestring(s);
+			p->u.board.addrstr = kstrdup(s, GFP_KERNEL);
 			if (kstrtoul(s, 0, &p->u.board.addr)) {
 				dgap_err("bad number for memory address");
 				return -1;
@@ -6719,22 +6562,22 @@ static int	dgap_parsefile(char **in, int Remove)
 				return -1;
 			}
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
-			p->u.board.pcibusstr = dgap_savestring(s);
+			p->u.board.pcibusstr = kstrdup(s, GFP_KERNEL);
 			if (kstrtoul(s, 0, &p->u.board.pcibus)) {
 				dgap_err("bad number for pci bus");
 				return -1;
 			}
 			p->u.board.v_pcibus = 1;
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
-			p->u.board.pcislotstr = dgap_savestring(s);
+			p->u.board.pcislotstr = kstrdup(s, GFP_KERNEL);
 			if (kstrtoul(s, 0, &p->u.board.pcislot)) {
 				dgap_err("bad number for pci slot");
 				return -1;
@@ -6748,11 +6591,11 @@ static int	dgap_parsefile(char **in, int Remove)
 				return -1;
 			}
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
-			p->u.board.method = dgap_savestring(s);
+			p->u.board.method = kstrdup(s, GFP_KERNEL);
 			p->u.board.v_method = 1;
 			break;
 
@@ -6762,17 +6605,17 @@ static int	dgap_parsefile(char **in, int Remove)
 				return -1;
 			}
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
-			p->u.board.status = dgap_savestring(s);
+			p->u.board.status = kstrdup(s, GFP_KERNEL);
 			break;
 
 		case NPORTS:	/* number of ports */
 			if (p->type == BNODE) {
 				s = dgap_getword(in);
-				if (s == NULL) {
+				if (!s) {
 					dgap_err("unexpected end of file");
 					return -1;
 				}
@@ -6783,7 +6626,7 @@ static int	dgap_parsefile(char **in, int Remove)
 				p->u.board.v_nport = 1;
 			} else if (p->type == CNODE) {
 				s = dgap_getword(in);
-				if (s == NULL) {
+				if (!s) {
 					dgap_err("unexpected end of file");
 					return -1;
 				}
@@ -6794,7 +6637,7 @@ static int	dgap_parsefile(char **in, int Remove)
 				p->u.conc.v_nport = 1;
 			} else if (p->type == MNODE) {
 				s = dgap_getword(in);
-				if (s == NULL) {
+				if (!s) {
 					dgap_err("unexpected end of file");
 					return -1;
 				}
@@ -6811,18 +6654,18 @@ static int	dgap_parsefile(char **in, int Remove)
 
 		case ID:	/* letter ID used in tty name */
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
 
-			p->u.board.status = dgap_savestring(s);
+			p->u.board.status = kstrdup(s, GFP_KERNEL);
 
 			if (p->type == CNODE) {
-				p->u.conc.id = dgap_savestring(s);
+				p->u.conc.id = kstrdup(s, GFP_KERNEL);
 				p->u.conc.v_id = 1;
 			} else if (p->type == MNODE) {
-				p->u.module.id = dgap_savestring(s);
+				p->u.module.id = kstrdup(s, GFP_KERNEL);
 				p->u.module.v_id = 1;
 			} else {
 				dgap_err("id only valid for concentrators or modules");
@@ -6833,7 +6676,7 @@ static int	dgap_parsefile(char **in, int Remove)
 		case STARTO:	/* start offset of ID */
 			if (p->type == BNODE) {
 				s = dgap_getword(in);
-				if (s == NULL) {
+				if (!s) {
 					dgap_err("unexpected end of file");
 					return -1;
 				}
@@ -6844,7 +6687,7 @@ static int	dgap_parsefile(char **in, int Remove)
 				p->u.board.v_start = 1;
 			} else if (p->type == CNODE) {
 				s = dgap_getword(in);
-				if (s == NULL) {
+				if (!s) {
 					dgap_err("unexpected end of file");
 					return -1;
 				}
@@ -6855,7 +6698,7 @@ static int	dgap_parsefile(char **in, int Remove)
 				p->u.conc.v_start = 1;
 			} else if (p->type == MNODE) {
 				s = dgap_getword(in);
-				if (s == NULL) {
+				if (!s) {
 					dgap_err("unexpected end of file");
 					return -1;
 				}
@@ -6884,7 +6727,7 @@ static int	dgap_parsefile(char **in, int Remove)
 				dgap_err("unexpeced end of file");
 				return -1;
 			}
-			p->u.ttyname = dgap_savestring(s);
+			p->u.ttyname = kstrdup(s, GFP_KERNEL);
 			if (!p->u.ttyname) {
 				dgap_err("out of memory");
 				return -1;
@@ -6905,7 +6748,7 @@ static int	dgap_parsefile(char **in, int Remove)
 				dgap_err("unexpeced end of file");
 				return -1;
 			}
-			p->u.cuname = dgap_savestring(s);
+			p->u.cuname = kstrdup(s, GFP_KERNEL);
 			if (!p->u.cuname) {
 				dgap_err("out of memory");
 				return -1;
@@ -6915,7 +6758,7 @@ static int	dgap_parsefile(char **in, int Remove)
 		case LINE:	/* line information */
 			if (dgap_checknode(p))
 				return -1;
-			if (brd == NULL) {
+			if (!brd) {
 				dgap_err("must specify board before line info");
 				return -1;
 			}
@@ -6938,7 +6781,7 @@ static int	dgap_parsefile(char **in, int Remove)
 		case CONC:	/* concentrator information */
 			if (dgap_checknode(p))
 				return -1;
-			if (line == NULL) {
+			if (!line) {
 				dgap_err("must specify line info before concentrator");
 				return -1;
 			}
@@ -6977,7 +6820,7 @@ static int	dgap_parsefile(char **in, int Remove)
 		case MOD:	/* EBI module */
 			if (dgap_checknode(p))
 				return -1;
-			if (brd == NULL) {
+			if (!brd) {
 				dgap_err("must specify board info before EBI modules");
 				return -1;
 			}
@@ -6986,7 +6829,7 @@ static int	dgap_parsefile(char **in, int Remove)
 				linecnt = 0;
 				break;
 			default:
-				if (conc == NULL) {
+				if (!conc) {
 					dgap_err("must specify concentrator info before EBI module");
 					return -1;
 				}
@@ -7029,7 +6872,7 @@ static int	dgap_parsefile(char **in, int Remove)
 					dgap_err("unexpected end of file");
 					return -1;
 				}
-				p->u.line.cable = dgap_savestring(s);
+				p->u.line.cable = kstrdup(s, GFP_KERNEL);
 				p->u.line.v_cable = 1;
 			}
 			break;
@@ -7037,7 +6880,7 @@ static int	dgap_parsefile(char **in, int Remove)
 		case SPEED:	/* sync line speed indication */
 			if (p->type == LNODE) {
 				s = dgap_getword(in);
-				if (s == NULL) {
+				if (!s) {
 					dgap_err("unexpected end of file");
 					return -1;
 				}
@@ -7048,7 +6891,7 @@ static int	dgap_parsefile(char **in, int Remove)
 				p->u.line.v_speed = 1;
 			} else if (p->type == CNODE) {
 				s = dgap_getword(in);
-				if (s == NULL) {
+				if (!s) {
 					dgap_err("unexpected end of file");
 					return -1;
 				}
@@ -7070,7 +6913,7 @@ static int	dgap_parsefile(char **in, int Remove)
 					dgap_err("unexpected end of file");
 					return -1;
 				}
-				p->u.conc.connect = dgap_savestring(s);
+				p->u.conc.connect = kstrdup(s, GFP_KERNEL);
 				p->u.conc.v_connect = 1;
 			}
 			break;
@@ -7088,7 +6931,7 @@ static int	dgap_parsefile(char **in, int Remove)
 				dgap_err("unexpeced end of file");
 				return -1;
 			}
-			p->u.printname = dgap_savestring(s);
+			p->u.printname = kstrdup(s, GFP_KERNEL);
 			if (!p->u.printname) {
 				dgap_err("out of memory");
 				return -1;
@@ -7105,7 +6948,7 @@ static int	dgap_parsefile(char **in, int Remove)
 			}
 			p = p->next;
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
@@ -7125,7 +6968,7 @@ static int	dgap_parsefile(char **in, int Remove)
 			}
 			p = p->next;
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
@@ -7145,7 +6988,7 @@ static int	dgap_parsefile(char **in, int Remove)
 			}
 			p = p->next;
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
@@ -7165,7 +7008,7 @@ static int	dgap_parsefile(char **in, int Remove)
 			}
 			p = p->next;
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
@@ -7185,7 +7028,7 @@ static int	dgap_parsefile(char **in, int Remove)
 			}
 			p = p->next;
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
@@ -7205,7 +7048,7 @@ static int	dgap_parsefile(char **in, int Remove)
 			}
 			p = p->next;
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
@@ -7225,7 +7068,7 @@ static int	dgap_parsefile(char **in, int Remove)
 			}
 			p = p->next;
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
@@ -7245,7 +7088,7 @@ static int	dgap_parsefile(char **in, int Remove)
 			}
 			p = p->next;
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
@@ -7265,7 +7108,7 @@ static int	dgap_parsefile(char **in, int Remove)
 			}
 			p = p->next;
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
@@ -7285,7 +7128,7 @@ static int	dgap_parsefile(char **in, int Remove)
  */
 static char *dgap_sindex(char *string, char *group)
 {
-	char    *ptr;
+	char *ptr;
 
 	if (!string || !group)
 		return (char *) NULL;
@@ -7317,7 +7160,7 @@ static char *dgap_sindex(char *string, char *group)
  */
 static int dgap_gettok(char **in, struct cnode *p)
 {
-	char	*w;
+	char *w;
 	struct toklist *t;
 
 	if (strstr(dgap_cword, "boar")) {
@@ -7386,8 +7229,8 @@ static struct cnode *dgap_newnode(int t)
 {
 	struct cnode *n;
 
-	n = kmalloc(sizeof(struct cnode), GFP_ATOMIC);
-	if (n != NULL) {
+	n = kmalloc(sizeof(struct cnode), GFP_KERNEL);
+	if (n) {
 		memset((char *)n, 0, sizeof(struct cnode));
 		n->type = t;
 	}
@@ -7454,37 +7297,21 @@ static int dgap_checknode(struct cnode *p)
 }
 
 /*
- * save a string somewhere
- */
-static char	*dgap_savestring(char *s)
-{
-	char	*p;
-
-	p = kmalloc(strlen(s) + 1, GFP_ATOMIC);
-	if (p)
-		strcpy(p, s);
-	return p;
-}
-
-/*
  * Given a board pointer, returns whether we should use interrupts or not.
  */
 static uint dgap_config_get_useintr(struct board_t *bd)
 {
-	struct cnode *p = NULL;
+	struct cnode *p;
 
 	if (!bd)
 		return 0;
 
 	for (p = bd->bd_config; p; p = p->next) {
-		switch (p->type) {
-		case INTRNODE:
+		if (p->type == INTRNODE) {
 			/*
 			 * check for pcxr types.
 			 */
 			return p->u.useintr;
-		default:
-			break;
 		}
 	}
 
@@ -7497,20 +7324,17 @@ static uint dgap_config_get_useintr(struct board_t *bd)
  */
 static uint dgap_config_get_altpin(struct board_t *bd)
 {
-	struct cnode *p = NULL;
+	struct cnode *p;
 
 	if (!bd)
 		return 0;
 
 	for (p = bd->bd_config; p; p = p->next) {
-		switch (p->type) {
-		case ANODE:
+		if (p->type == ANODE) {
 			/*
 			 * check for pcxr types.
 			 */
 			return p->u.altpin;
-		default:
-			break;
 		}
 	}
 
@@ -7524,59 +7348,61 @@ static uint dgap_config_get_altpin(struct board_t *bd)
  */
 static struct cnode *dgap_find_config(int type, int bus, int slot)
 {
-	struct cnode *p, *prev = NULL, *prev2 = NULL, *found = NULL;
+	struct cnode *p, *prev, *prev2, *found;
 
 	p = &dgap_head;
 
-	while (p->next != NULL) {
+	while (p->next) {
 		prev = p;
 		p = p->next;
 
-		if (p->type == BNODE) {
+		if (p->type != BNODE)
+			continue;
 
-			if (p->u.board.type == type) {
+		if (p->u.board.type != type)
+			continue;
 
-				if (p->u.board.v_pcibus &&
-				    p->u.board.pcibus != bus)
-					continue;
-				if (p->u.board.v_pcislot &&
-				    p->u.board.pcislot != slot)
-					continue;
+		if (p->u.board.v_pcibus &&
+		    p->u.board.pcibus != bus)
+			continue;
 
-				found = p;
-				/*
-				 * Keep walking thru the list till we
-				 * find the next board.
-				 */
-				while (p->next != NULL) {
-					prev2 = p;
-					p = p->next;
-					if (p->type == BNODE) {
-
-						/*
-						 * Mark the end of our 1 board
-						 * chain of configs.
-						 */
-						prev2->next = NULL;
-
-						/*
-						 * Link the "next" board to the
-						 * previous board, effectively
-						 * "unlinking" our board from
-						 * the main config.
-						 */
-						prev->next = p;
-
-						return found;
-					}
-				}
-				/*
-				 * It must be the last board in the list.
-				 */
-				prev->next = NULL;
-				return found;
-			}
+		if (p->u.board.v_pcislot &&
+		    p->u.board.pcislot != slot)
+			continue;
+
+		found = p;
+		/*
+		 * Keep walking thru the list till we
+		 * find the next board.
+		 */
+		while (p->next) {
+			prev2 = p;
+			p = p->next;
+
+			if (p->type != BNODE)
+				continue;
+
+			/*
+			 * Mark the end of our 1 board
+			 * chain of configs.
+			 */
+			prev2->next = NULL;
+
+			/*
+			 * Link the "next" board to the
+			 * previous board, effectively
+			 * "unlinking" our board from
+			 * the main config.
+			 */
+			prev->next = p;
+
+			return found;
 		}
+		/*
+		 * It must be the last board in the list.
+		 */
+		prev->next = NULL;
+		return found;
 	}
 	return NULL;
 }
@@ -7589,7 +7415,7 @@ static struct cnode *dgap_find_config(int type, int bus, int slot)
 static uint dgap_config_get_num_prts(struct board_t *bd)
 {
 	int count = 0;
-	struct cnode *p = NULL;
+	struct cnode *p;
 
 	if (!bd)
 		return 0;
@@ -7618,8 +7444,8 @@ static uint dgap_config_get_num_prts(struct board_t *bd)
 static char *dgap_create_config_string(struct board_t *bd, char *string)
 {
 	char *ptr = string;
-	struct cnode *p = NULL;
-	struct cnode *q = NULL;
+	struct cnode *p;
+	struct cnode *q;
 	int speed;
 
 	if (!bd) {
@@ -7645,13 +7471,11 @@ static char *dgap_create_config_string(struct board_t *bd, char *string)
 			 */
 			speed = p->u.conc.speed;
 			q = p->next;
-			if ((q != NULL) && (q->type == MNODE)) {
+			if (q && (q->type == MNODE)) {
 				*ptr = (p->u.conc.nport + 0x80);
 				ptr++;
 				p = q;
-				while ((q->next != NULL) &&
-				       (q->next->type) == MNODE) {
-
+				while (q->next && (q->next->type) == MNODE) {
 					*ptr = (q->u.module.nport + 0x80);
 					ptr++;
 					p = q;
diff --git a/drivers/staging/dgap/dgap.h b/drivers/staging/dgap/dgap.h
index 6b8f5f85832..03c020e35f8 100644
--- a/drivers/staging/dgap/dgap.h
+++ b/drivers/staging/dgap/dgap.h
@@ -39,9 +39,6 @@
 # define FALSE 0
 #endif
 
-/* Required for our shared headers! */
-typedef unsigned char		uchar;
-
 #if !defined(TTY_FLIPBUF_SIZE)
 # define TTY_FLIPBUF_SIZE 512
 #endif
@@ -61,7 +58,7 @@ typedef unsigned char		uchar;
 
 /*
  * defines from dgap_pci.h
- */ 
+ */
 #define PCIMAX 32			/* maximum number of PCI boards */
 
 #define DIGI_VID		0x114F
@@ -186,7 +183,7 @@ typedef unsigned char		uchar;
 			ECHOCTL | ECHOKE | IEXTEN)
 
 #ifndef _POSIX_VDISABLE
-#define   _POSIX_VDISABLE '\0'
+#define _POSIX_VDISABLE ('\0')
 #endif
 
 #define SNIFF_MAX	65536		/* Sniff buffer size (2^n) */
@@ -207,9 +204,11 @@ typedef unsigned char		uchar;
 #define EVSTART         0x0800L         /* Start of event buffer        */
 #define EVMAX           0x0c00L         /* End of event buffer          */
 #define FEP5_PLUS       0x0E40          /* ASCII '5' and ASCII 'A' is here  */
-#define ECS_SEG         0x0E44          /* Segment of the extended channel structure */
-#define LINE_SPEED      0x10            /* Offset into ECS_SEG for line speed   */
-                                        /* if the fep has extended capabilities */
+#define ECS_SEG         0x0E44          /* Segment of the extended      */
+					/* channel structure            */
+#define LINE_SPEED      0x10            /* Offset into ECS_SEG for line */
+					/* speed if the fep has extended */
+					/* capabilities                 */
 
 /* BIOS MAGIC SPOTS */
 #define ERROR           0x0C14L		/* BIOS error code              */
@@ -247,12 +246,12 @@ typedef unsigned char		uchar;
 
 #define FEPTIMEOUT 200000
 
-#define ENABLE_INTR		0x0e04		/* Enable interrupts flag */
-#define FEPPOLL_MIN		1		/* minimum of 1 millisecond */
-#define FEPPOLL_MAX		20		/* maximum of 20 milliseconds */
-#define FEPPOLL			0x0c26		/* Fep event poll interval */
+#define ENABLE_INTR	0x0e04		/* Enable interrupts flag */
+#define FEPPOLL_MIN	1		/* minimum of 1 millisecond */
+#define FEPPOLL_MAX	20		/* maximum of 20 milliseconds */
+#define FEPPOLL		0x0c26		/* Fep event poll interval */
 
-#define	IALTPIN			0x0080		/* Input flag to swap DSR <-> DCD */
+#define	IALTPIN		0x0080		/* Input flag to swap DSR <-> DCD */
 
 /************************************************************************
  * FEP supported functions
@@ -280,14 +279,12 @@ typedef unsigned char		uchar;
 #define SPINTFC		0xfc		/* Reserved			*/
 #define SCOMMODE	0xfd		/* Set RS232/422 mode		*/
 
-
 /************************************************************************
  *	Modes for SCOMMODE
  ************************************************************************/
 #define MODE_232	0x00
 #define MODE_422	0x01
 
-
 /************************************************************************
  *      Event flags.
  ************************************************************************/
@@ -362,13 +359,13 @@ typedef unsigned char		uchar;
 #define MC8E8K  36
 
 #define AVANFS	42	/* start of Avanstar family definitions */
-#define A8P 	42
+#define A8P	42
 #define A16P	43
 #define AVANFE	43	/* end of Avanstar family definitions */
 
-#define DA2000FS	44	/* start of AccelePort 2000 family definitions */
-#define DA22 		44 /* AccelePort 2002 */
-#define DA24 		45 /* AccelePort 2004 */
+#define DA2000FS	44 /* start of AccelePort 2000 family definitions */
+#define DA22		44 /* AccelePort 2002 */
+#define DA24		45 /* AccelePort 2004 */
 #define DA28		46 /* AccelePort 2008 */
 #define DA216		47 /* AccelePort 2016 */
 #define DAR4		48 /* AccelePort RAS 4 port */
@@ -492,8 +489,6 @@ enum {
 	REQUESTED_CONCENTRATOR
 };
 
-
-
 /*
  * Modem line constants are defined as macros because DSR and
  * DCD are swapable using the ditty altpin option.
@@ -505,14 +500,12 @@ enum {
 #define D_RI(ch)        DM_RI           /* Ring indicator       */
 #define D_DTR(ch)       DM_DTR          /* Data terminal ready  */
 
-
 /*************************************************************************
  *
  * Structures and closely related defines.
  *
  *************************************************************************/
 
-
 /*
  * A structure to hold a statistics counter.  We also
  * compute moving averages for this counter.
@@ -524,7 +517,6 @@ struct macounter {
 	ulong		ema;	/* Exponential moving average */
 };
 
-
 /************************************************************************
  * Device flag definitions for bd_flags.
  ************************************************************************/
@@ -546,11 +538,11 @@ struct board_t {
 	u16		device;		/* PCI device ID */
 	u16		subvendor;	/* PCI subsystem vendor ID */
 	u16		subdevice;	/* PCI subsystem device ID */
-	uchar		rev;		/* PCI revision ID */
+	u8		rev;		/* PCI revision ID */
 	uint		pci_bus;	/* PCI bus value */
 	uint		pci_slot;	/* PCI slot value */
 	u16		maxports;	/* MAX ports this board can handle */
-	uchar		vpd[VPDSIZE];	/* VPD of board, if found */
+	u8		vpd[VPDSIZE];	/* VPD of board, if found */
 	u32		bd_flags;	/* Board flags */
 
 	spinlock_t	bd_lock;	/* Used to protect board */
@@ -571,48 +563,53 @@ struct board_t {
 	ulong		irq;		/* Interrupt request number */
 	ulong		intr_count;	/* Count of interrupts */
 	u32		intr_used;	/* Non-zero if using interrupts */
-	u32		intr_running;	/* Non-zero if FEP knows its doing interrupts */
+	u32		intr_running;	/* Non-zero if FEP knows its doing */
+					/* interrupts */
 
 	ulong		port;		/* Start of base io port of the card */
 	ulong		port_end;	/* End of base io port of the card */
 	ulong		membase;	/* Start of base memory of the card */
 	ulong		membase_end;	/* End of base memory of the card */
 
-	uchar 		*re_map_port;	/* Remapped io port of the card */
-	uchar		*re_map_membase;/* Remapped memory of the card */
+	u8 __iomem	*re_map_port;	/* Remapped io port of the card */
+	u8 __iomem	*re_map_membase;/* Remapped memory of the card */
 
-	uchar		runwait;	/* # Processes waiting for FEP  */
-	uchar		inhibit_poller; /* Tells  the poller to leave us alone */
+	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 channels. */
+	struct channel_t *channels[MAXPORTS]; /* array of pointers to our */
+					      /* channels.                */
 
-	struct tty_driver	*SerialDriver;
-	struct tty_port *SerialPorts;
-	char		SerialName[200];
-	struct tty_driver	*PrintDriver;
-	struct tty_port *PrinterPorts;
-	char		PrintName[200];
+	struct tty_driver	*serial_driver;
+	struct tty_port *serial_ports;
+	char		serial_name[200];
+	struct tty_driver	*print_driver;
+	struct tty_port *printer_ports;
+	char		print_name[200];
 
-	u32		dgap_Major_Serial_Registered;
-	u32		dgap_Major_TransparentPrint_Registered;
+	u32		dgap_major_serial_registered;
+	u32		dgap_major_transparent_print_registered;
 
-	u32		dgap_Serial_Major;
-	u32		dgap_TransparentPrint_Major;
+	u32		dgap_serial_major;
+	u32		dgap_transparent_print_major;
 
-	struct bs_t	*bd_bs;			/* Base structure pointer       */
+	struct bs_t __iomem *bd_bs;	/* Base structure pointer         */
 
-	char	*flipbuf;		/* Our flip buffer, alloced if board is found */
-	char	*flipflagbuf;		/* Our flip flag buffer, alloced if board is found */
+	char	*flipbuf;		/* Our flip buffer, alloced if    */
+					/* board is found                 */
+	char	*flipflagbuf;		/* Our flip flag buffer, alloced  */
+					/* if board is found              */
 
-	u16		dpatype;	/* The board "type", as defined by DPA */
-	u16		dpastatus;	/* The board "status", as defined by DPA */
-	wait_queue_head_t kme_wait;	/* Needed for DPA support */
+	u16		dpatype;	/* The board "type", as defined   */
+					/* by DPA                         */
+	u16		dpastatus;	/* The board "status", as defined */
+					/* by DPA                         */
+	wait_queue_head_t kme_wait;	/* Needed for DPA support         */
 
-	u32		conc_dl_status;	/* Status of any pending conc download */
+	u32		conc_dl_status;	/* Status of any pending conc     */
+					/* download                       */
 };
 
-
-
 /************************************************************************
  * Unit flag definitions for un_flags.
  ************************************************************************/
@@ -640,7 +637,7 @@ struct un_t {
 	struct	channel_t *un_ch;
 	u32	un_time;
 	u32	un_type;
-	u32	un_open_count;	/* Counter of opens to port		*/
+	int	un_open_count;	/* Counter of opens to port		*/
 	struct tty_struct *un_tty;/* Pointer to unit tty structure	*/
 	u32	un_flags;	/* Unit flags				*/
 	wait_queue_head_t un_flags_wait; /* Place to sleep to wait on unit */
@@ -650,7 +647,6 @@ struct un_t {
 	struct device *un_sysfs;
 };
 
-
 /************************************************************************
  * Device flag definitions for ch_flags.
  ************************************************************************/
@@ -677,7 +673,6 @@ struct un_t {
 #define SNIFF_WAIT_DATA	0x2
 #define SNIFF_WAIT_SPACE 0x4
 
-
 /************************************************************************
  ***	Definitions for Digi ditty(1) command.
  ************************************************************************/
@@ -689,8 +684,8 @@ struct un_t {
 
 #if !defined(TIOCMODG)
 
-#define	TIOCMODG	(('d'<<8) | 250)		/* get modem ctrl state	*/
-#define	TIOCMODS	(('d'<<8) | 251)		/* set modem ctrl state	*/
+#define	TIOCMODG	(('d'<<8) | 250)	/* get modem ctrl state	*/
+#define	TIOCMODS	(('d'<<8) | 251)	/* set modem ctrl state	*/
 
 #ifndef TIOCM_LE
 #define		TIOCM_LE	0x01		/* line enable		*/
@@ -709,16 +704,15 @@ struct un_t {
 #endif
 
 #if !defined(TIOCMSET)
-#define	TIOCMSET	(('d'<<8) | 252)		/* set modem ctrl state	*/
-#define	TIOCMGET	(('d'<<8) | 253)		/* set modem ctrl state	*/
+#define	TIOCMSET	(('d'<<8) | 252)	/* set modem ctrl state	*/
+#define	TIOCMGET	(('d'<<8) | 253)	/* set modem ctrl state	*/
 #endif
 
 #if !defined(TIOCMBIC)
-#define	TIOCMBIC	(('d'<<8) | 254)		/* set modem ctrl state */
-#define	TIOCMBIS	(('d'<<8) | 255)		/* set modem ctrl state */
+#define	TIOCMBIC	(('d'<<8) | 254)	/* set modem ctrl state */
+#define	TIOCMBIS	(('d'<<8) | 255)	/* set modem ctrl state */
 #endif
 
-
 #if !defined(TIOCSDTR)
 #define	TIOCSDTR	(('e'<<8) | 0)		/* set DTR		*/
 #define	TIOCCDTR	(('e'<<8) | 1)		/* clear DTR		*/
@@ -737,26 +731,25 @@ struct un_t {
 						/* Adapter Memory	*/
 
 #define	DIGI_GETFLOW	(('e'<<8) | 99)		/* Get startc/stopc flow */
-						/* control characters 	 */
-#define	DIGI_SETFLOW	(('e'<<8) | 100)		/* Set startc/stopc flow */
+						/* 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 	 */
-#define	DIGI_SETAFLOW	(('e'<<8) | 102)		/* Set Aux. startc/stopc */
+#define	DIGI_GETAFLOW	(('e'<<8) | 101)	/* Get Aux. startc/stopc */
+						/* flow control chars    */
+#define	DIGI_SETAFLOW	(('e'<<8) | 102)	/* Set Aux. startc/stopc */
 						/* flow control chars	 */
 
-#define DIGI_GEDELAY	(('d'<<8) | 246)		/* Get edelay */
-#define DIGI_SEDELAY	(('d'<<8) | 247)		/* Set edelay */
+#define DIGI_GEDELAY	(('d'<<8) | 246)	/* Get edelay */
+#define DIGI_SEDELAY	(('d'<<8) | 247)	/* Set edelay */
 
 struct	digiflow_t {
-	unsigned char	startc;				/* flow cntl start char	*/
-	unsigned char	stopc;				/* flow cntl stop char	*/
+	unsigned char	startc;			/* flow cntl start char	*/
+	unsigned char	stopc;			/* flow cntl stop char	*/
 };
 
-
 #ifdef	FLOW_2200
-#define	F2200_GETA	(('e'<<8) | 104)		/* Get 2x36 flow cntl flags */
-#define	F2200_SETAW	(('e'<<8) | 105)		/* Set 2x36 flow cntl flags */
+#define	F2200_GETA	(('e'<<8) | 104)	/* Get 2x36 flow cntl flags */
+#define	F2200_SETAW	(('e'<<8) | 105)	/* Set 2x36 flow cntl flags */
 #define		F2200_MASK	0x03		/* 2200 flow cntl bit mask  */
 #define		FCNTL_2200	0x01		/* 2x36 terminal flow cntl  */
 #define		PCNTL_2200	0x02		/* 2x36 printer flow cntl   */
@@ -830,59 +823,6 @@ struct rw_t {
 	unsigned char	rw_data[128];	/* Data to read/write		*/
 };
 
-/***********************************************************************
- * Shrink Buffer and Board Information definitions and structures.
-
- ************************************************************************/
-			/* Board type return codes */
-#define	PCXI_TYPE 1     /* Board type at the designated port is a PC/Xi */
-#define PCXM_TYPE 2     /* Board type at the designated port is a PC/Xm */
-#define	PCXE_TYPE 3     /* Board type at the designated port is a PC/Xe */
-#define	MCXI_TYPE 4     /* Board type at the designated port is a MC/Xi */
-#define COMXI_TYPE 5     /* Board type at the designated port is a COM/Xi */
-
-			 /* Non-Zero Result codes. */
-#define RESULT_NOBDFND 1 /* A Digi product at that port is not config installed */
-#define RESULT_NODESCT 2 /* A memory descriptor was not obtainable */
-#define RESULT_NOOSSIG 3 /* FEP/OS signature was not detected on the board */
-#define RESULT_TOOSML  4 /* Too small an area to shrink.  */
-#define RESULT_NOCHAN  5 /* Channel structure for the board was not found */
-
-struct shrink_buf_struct {
-	unsigned long	shrink_buf_vaddr;	/* Virtual address of board */
-	unsigned long	shrink_buf_phys;	/* Physical address of board */
-	unsigned long	shrink_buf_bseg;	/* Amount of board memory */
-	unsigned long	shrink_buf_hseg;	/* '186 Beginning of Dual-Port */
-
-	unsigned long	shrink_buf_lseg;	/* '186 Beginning of freed memory						*/
-	unsigned long	shrink_buf_mseg;	/* Linear address from start of
-						   dual-port were freed memory
-						   begins, host viewpoint. */
-
-	unsigned long	shrink_buf_bdparam;	/* Parameter for xxmemon and
-						   xxmemoff */
-
-	unsigned long	shrink_buf_reserva;	/* Reserved */
-	unsigned long	shrink_buf_reservb;	/* Reserved */
-	unsigned long	shrink_buf_reservc;	/* Reserved */
-	unsigned long	shrink_buf_reservd;	/* Reserved */
-
-	unsigned char	shrink_buf_result;	/* Reason for call failing
-						   Zero is Good return */
-	unsigned char	shrink_buf_init;	/* Non-Zero if it caused an
-						   xxinit call. */
-
-	unsigned char	shrink_buf_anports;	/* Number of async 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
-							      4 = MC/Xi
-							      5 = COMX/i */
-	unsigned char	shrink_buf_card;	/* Card number */
-
-};
-
 /************************************************************************
  * Structure to get driver status information
  ************************************************************************/
@@ -892,7 +832,7 @@ struct digi_dinfo {
 	char		dinfo_version[16];	/* driver version       */
 };
 
-#define	DIGI_GETDD	(('d'<<8) | 248)		/* get driver info      */
+#define	DIGI_GETDD	(('d'<<8) | 248)	/* get driver info      */
 
 /************************************************************************
  * Structure used with ioctl commands for per-board information
@@ -912,7 +852,7 @@ struct digi_info {
 	char		info_reserved[7];	/* for future expansion    */
 };
 
-#define	DIGI_GETBD	(('d'<<8) | 249)		/* get board info          */
+#define	DIGI_GETBD	(('d'<<8) | 249)	/* get board info          */
 
 struct digi_stat {
 	unsigned int	info_chan;		/* Channel number (0 based)  */
@@ -927,7 +867,7 @@ struct digi_stat {
 	unsigned long	info_reserved[8];	/* for future expansion    */
 };
 
-#define	DIGI_GETSTAT	(('d'<<8) | 244)		/* get board info          */
+#define	DIGI_GETSTAT	(('d'<<8) | 244)	/* get board info          */
 /************************************************************************
  *
  * Structure used with ioctl commands for per-channel information
@@ -936,9 +876,9 @@ struct digi_stat {
 struct digi_ch {
 	unsigned long	info_bdnum;		/* Board number (0 based)  */
 	unsigned long	info_channel;		/* Channel index number    */
-	unsigned long	info_ch_cflag;		/* Channel cflag   	   */
-	unsigned long	info_ch_iflag;		/* Channel iflag   	   */
-	unsigned long	info_ch_oflag;		/* Channel oflag   	   */
+	unsigned long	info_ch_cflag;		/* Channel cflag           */
+	unsigned long	info_ch_iflag;		/* Channel iflag           */
+	unsigned long	info_ch_oflag;		/* Channel oflag           */
 	unsigned long	info_chsize;		/* Channel structure size  */
 	unsigned long	info_sleep_stat;	/* sleep status		   */
 	dev_t		info_dev;		/* device number	   */
@@ -970,7 +910,7 @@ struct digi_cmd {
 #define INFO_CH_WLOW	0x0020
 #define INFO_XXBUF_BUSY 0x0040
 
-#define	DIGI_GETCH	(('d'<<8) | 245)		/* get board info          */
+#define	DIGI_GETCH	(('d'<<8) | 245)	/* get board info          */
 
 /* Board type definitions */
 
@@ -1015,24 +955,25 @@ struct digi_cmd {
 #define BD_TRIBOOT	0x8
 #define	BD_BADKME	0x80
 
-#define DIGI_LOOPBACK	      (('d'<<8) | 252)		/* Enable/disable UART internal loopback */
-#define DIGI_SPOLL            (('d'<<8) | 254)		/* change poller rate   */
+#define DIGI_LOOPBACK	(('d'<<8) | 252)	/* Enable/disable UART  */
+						/* internal loopback    */
+#define DIGI_SPOLL	(('d'<<8) | 254)	/* change poller rate   */
 
-#define DIGI_SETCUSTOMBAUD	_IOW('e', 106, int)	/* Set integer baud rate */
-#define DIGI_GETCUSTOMBAUD	_IOR('e', 107, int)	/* Get integer baud rate */
-#define DIGI_RESET_PORT		(('e'<<8) | 93)		/* Reset port		*/
+#define DIGI_SETCUSTOMBAUD _IOW('e', 106, int)	/* Set integer baud rate */
+#define DIGI_GETCUSTOMBAUD _IOR('e', 107, int)	/* Get integer baud rate */
+#define DIGI_RESET_PORT	   (('e'<<8) | 93)	/* Reset port		 */
 
 /************************************************************************
  * Channel information structure.
  ************************************************************************/
 struct channel_t {
 	int magic;			/* Channel Magic Number		*/
-	struct bs_t	*ch_bs;		/* Base structure pointer       */
-	struct cm_t	*ch_cm;		/* Command queue pointer        */
+	struct bs_t __iomem *ch_bs;	/* Base structure pointer       */
+	struct cm_t __iomem *ch_cm;	/* Command queue pointer        */
 	struct board_t *ch_bd;		/* Board structure pointer      */
-	unsigned char *ch_vaddr;	/* FEP memory origin            */
-	unsigned char *ch_taddr;	/* Write buffer origin          */
-	unsigned char *ch_raddr;	/* Read buffer origin           */
+	u8 __iomem *ch_vaddr;		/* FEP memory origin            */
+	u8 __iomem *ch_taddr;		/* Write buffer origin          */
+	u8 __iomem *ch_raddr;		/* Read buffer origin           */
 	struct digi_t  ch_digi;		/* Transparent Print structure  */
 	struct un_t ch_tun;		/* Terminal unit info           */
 	struct un_t ch_pun;		/* Printer unit info            */
@@ -1041,14 +982,14 @@ struct channel_t {
 	wait_queue_head_t ch_flags_wait;
 
 	u32	pscan_state;
-	uchar	pscan_savechar;
+	u8	pscan_savechar;
 
 	u32 ch_portnum;			/* Port number, 0 offset.	*/
 	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_close_delay;		/* How long we should drop      */
+					/* RTS/DTR for                  */
 
 	u32	ch_cpstime;		/* Time for CPS calculations    */
 
@@ -1057,7 +998,7 @@ struct channel_t {
 	tcflag_t ch_c_oflag;		/* channel oflags               */
 	tcflag_t ch_c_lflag;		/* channel lflags               */
 
-	u16  ch_fepiflag;            /* FEP tty iflags               */
+	u16  ch_fepiflag;		/* FEP tty iflags               */
 	u16  ch_fepcflag;		/* FEP tty cflags               */
 	u16  ch_fepoflag;		/* FEP tty oflags               */
 	u16  ch_wopen;			/* Waiting for open process cnt */
@@ -1071,75 +1012,69 @@ struct channel_t {
 
 	u16  ch_cook;			/* Output character mask        */
 
-	uchar   ch_card;		/* Card channel is on           */
-	uchar   ch_stopc;		/* Stop character               */
-	uchar   ch_startc;		/* Start character              */
-
-	uchar   ch_mostat;		/* FEP output modem status      */
-	uchar   ch_mistat;		/* FEP input modem status       */
-	uchar   ch_mforce;		/* Modem values to be forced    */
-	uchar   ch_mval;		/* Force values                 */
-	uchar   ch_fepstopc;		/* FEP stop character           */
-	uchar   ch_fepstartc;		/* FEP start character          */
-
-	uchar   ch_astopc;		/* Auxiliary Stop character     */
-	uchar   ch_astartc;		/* Auxiliary Start character    */
-	uchar   ch_fepastopc;		/* Auxiliary FEP stop char      */
-	uchar   ch_fepastartc;		/* Auxiliary FEP start char     */
-
-	uchar   ch_hflow;		/* FEP hardware handshake       */
-	uchar   ch_dsr;			/* stores real dsr value        */
-	uchar   ch_cd;			/* stores real cd value         */
-	uchar   ch_tx_win;		/* channel tx buffer window     */
-	uchar   ch_rx_win;		/* channel rx buffer window     */
+	u8   ch_card;			/* Card channel is on           */
+	u8   ch_stopc;			/* Stop character               */
+	u8   ch_startc;			/* Start character              */
+
+	u8   ch_mostat;			/* FEP output modem status      */
+	u8   ch_mistat;			/* FEP input modem status       */
+	u8   ch_mforce;			/* Modem values to be forced    */
+	u8   ch_mval;			/* Force values                 */
+	u8   ch_fepstopc;		/* FEP stop character           */
+	u8   ch_fepstartc;		/* FEP start character          */
+
+	u8   ch_astopc;			/* Auxiliary Stop character     */
+	u8   ch_astartc;		/* Auxiliary Start character    */
+	u8   ch_fepastopc;		/* Auxiliary FEP stop char      */
+	u8   ch_fepastartc;		/* Auxiliary FEP start char     */
+
+	u8   ch_hflow;			/* FEP hardware handshake       */
+	u8   ch_dsr;			/* stores real dsr value        */
+	u8   ch_cd;			/* stores real cd value         */
+	u8   ch_tx_win;			/* channel tx buffer window     */
+	u8   ch_rx_win;			/* channel rx buffer window     */
 	uint	ch_custom_speed;	/* Custom baud, if set		*/
-	uint	ch_baud_info;		/* Current baud info for /proc output	*/
-	ulong	ch_rxcount;		/* total of data received so far	*/
-	ulong	ch_txcount;		/* total of data transmitted so far	*/
-	ulong	ch_err_parity;		/* Count of parity errors on channel	*/
-	ulong	ch_err_frame;		/* Count of framing errors on channel	*/
+	uint	ch_baud_info;		/* Current baud info for /proc output */
+	ulong	ch_rxcount;		/* total of data received so far      */
+	ulong	ch_txcount;		/* total of data transmitted so far   */
+	ulong	ch_err_parity;		/* Count of parity errors on channel  */
+	ulong	ch_err_frame;		/* Count of framing errors on channel */
 	ulong	ch_err_break;		/* Count of breaks on channel	*/
 	ulong	ch_err_overrun;		/* Count of overruns on channel	*/
-
-	uint ch_sniff_in;
-	uint ch_sniff_out;
-	char *ch_sniff_buf;		/* Sniff buffer for proc */
-	ulong ch_sniff_flags;		/* Channel flags                */
-	wait_queue_head_t ch_sniff_wait;
 };
 
 /************************************************************************
  * Command structure definition.
  ************************************************************************/
 struct cm_t {
-	volatile unsigned short cm_head;	/* Command buffer head offset	*/
-	volatile unsigned short cm_tail;	/* Command buffer tail offset	*/
-	volatile unsigned short cm_start;	/* start offset of buffer	*/
-	volatile unsigned short cm_max;		/* last offset of buffer	*/
+	unsigned short cm_head;		/* Command buffer head offset */
+	unsigned short cm_tail;		/* Command buffer tail offset */
+	unsigned short cm_start;	/* start offset of buffer     */
+	unsigned short cm_max;		/* last offset of buffer      */
 };
 
 /************************************************************************
  * Event structure definition.
  ************************************************************************/
 struct ev_t {
-	volatile unsigned short ev_head;	/* Command buffer head offset	*/
-	volatile unsigned short ev_tail;	/* Command buffer tail offset	*/
-	volatile unsigned short ev_start;	/* start offset of buffer	*/
-	volatile unsigned short ev_max;		/* last offset of buffer	*/
+	unsigned short ev_head;		/* Command buffer head offset */
+	unsigned short ev_tail;		/* Command buffer tail offset */
+	unsigned short ev_start;	/* start offset of buffer     */
+	unsigned short ev_max;		/* last offset of buffer      */
 };
 
 /************************************************************************
  * Download buffer structure.
  ************************************************************************/
 struct downld_t {
-	uchar	dl_type;		/* Header                       */
-	uchar	dl_seq;			/* Download sequence            */
+	u8	dl_type;		/* Header                       */
+	u8	dl_seq;			/* Download sequence            */
 	ushort	dl_srev;		/* Software revision number     */
 	ushort	dl_lrev;		/* Low revision number          */
 	ushort	dl_hrev;		/* High revision number         */
 	ushort	dl_seg;			/* Start segment address        */
 	ushort	dl_size;		/* Number of bytes to download  */
-	uchar	dl_data[1024];		/* Download data                */
+	u8	dl_data[1024];		/* Download data                */
 };
 
 /************************************************************************
@@ -1152,70 +1087,74 @@ struct downld_t {
  *        U = unknown (may be changed w/o notice)                       *
  ************************************************************************/
 struct bs_t {
-	volatile unsigned short  tp_jmp;	/* Transmit poll jump		 */
-	volatile unsigned short  tc_jmp;	/* Cooked procedure jump	 */
-	volatile unsigned short  ri_jmp;	/* Not currently used		 */
-	volatile unsigned short  rp_jmp;	/* Receive poll jump		 */
-
-	volatile unsigned short  tx_seg;	/* W  Tx segment	 */
-	volatile unsigned short  tx_head;	/* W  Tx buffer head offset	*/
-	volatile unsigned short  tx_tail;	/* R  Tx buffer tail offset	*/
-	volatile unsigned short  tx_max;	/* W  Tx buffer size - 1	 */
-
-	volatile unsigned short  rx_seg;	/* W  Rx segment		*/
-	volatile unsigned short  rx_head;	/* W  Rx buffer head offset	*/
-	volatile unsigned short  rx_tail;	/* R  Rx buffer tail offset	*/
-	volatile unsigned short  rx_max;	/* W  Rx buffer size - 1	 */
-
-	volatile unsigned short  tx_lw;		/* W  Tx buffer low water mark  */
-	volatile unsigned short  rx_lw;		/* W  Rx buffer low water mark  */
-	volatile unsigned short  rx_hw;		/* W  Rx buffer high water mark */
-	volatile unsigned short  incr;		/* W  Increment to next channel */
-
-	volatile unsigned short  fepdev;	/* U  SCC device base address    */
-	volatile unsigned short  edelay;	/* W  Exception delay            */
-	volatile unsigned short  blen;		/* W  Break length              */
-	volatile unsigned short  btime;		/* U  Break complete time       */
-
-	volatile unsigned short  iflag;		/* C  UNIX input flags          */
-	volatile unsigned short  oflag;		/* C  UNIX output flags         */
-	volatile unsigned short  cflag;		/* C  UNIX control flags        */
-	volatile unsigned short  wfill[13];	/* U  Reserved for expansion    */
-
-	volatile unsigned char   num;		/* U  Channel number            */
-	volatile unsigned char   ract;		/* U  Receiver active counter   */
-	volatile unsigned char   bstat;		/* U  Break status bits         */
-	volatile unsigned char   tbusy;		/* W  Transmit busy             */
-	volatile unsigned char   iempty;	/* W  Transmit empty event enable */
-	volatile unsigned char   ilow;		/* W  Transmit low-water event enable */
-	volatile unsigned char   idata;		/* W  Receive data interrupt enable */
-	volatile unsigned char   eflag;		/* U  Host event flags          */
-
-	volatile unsigned char   tflag;		/* U  Transmit flags            */
-	volatile unsigned char   rflag;		/* U  Receive flags             */
-	volatile unsigned char   xmask;		/* U  Transmit ready flags      */
-	volatile unsigned char   xval;		/* U  Transmit ready value      */
-	volatile unsigned char   m_stat;	/* RC Modem status bits          */
-	volatile unsigned char   m_change;	/* U  Modem bits which changed  */
-	volatile unsigned char   m_int;		/* W  Modem interrupt enable bits */
-	volatile unsigned char   m_last;	/* U  Last modem status         */
-
-	volatile unsigned char   mtran;		/* C   Unreported modem trans   */
-	volatile unsigned char   orun;		/* C   Buffer overrun occurred  */
-	volatile unsigned char   astartc;	/* W   Auxiliary Xon char       */
-	volatile unsigned char   astopc;	/* W   Auxiliary Xoff char      */
-	volatile unsigned char   startc;	/* W   Xon character             */
-	volatile unsigned char   stopc;		/* W   Xoff character           */
-	volatile unsigned char   vnextc;	/* W   Vnext character           */
-	volatile unsigned char   hflow;		/* C   Software flow control    */
-	
-	volatile unsigned char   fillc;		/* U   Delay Fill character     */
-	volatile unsigned char   ochar;		/* U   Saved output character   */
-	volatile unsigned char   omask;		/* U   Output character mask    */
-
-	volatile unsigned char   bfill[13];	/* U   Reserved for expansion   */
-
-	volatile unsigned char   scc[16];	/* U   SCC registers            */
+	unsigned short  tp_jmp;		/* Transmit poll jump	 */
+	unsigned short  tc_jmp;		/* Cooked procedure jump */
+	unsigned short  ri_jmp;		/* Not currently used	 */
+	unsigned short  rp_jmp;		/* Receive poll jump	 */
+
+	unsigned short  tx_seg;		/* W Tx segment	 */
+	unsigned short  tx_head;	/* W Tx buffer head offset */
+	unsigned short  tx_tail;	/* R Tx buffer tail offset */
+	unsigned short  tx_max;		/* W Tx buffer size - 1    */
+
+	unsigned short  rx_seg;		/* W Rx segment	    */
+	unsigned short  rx_head;	/* W Rx buffer head offset */
+	unsigned short  rx_tail;	/* R Rx buffer tail offset */
+	unsigned short  rx_max;		/* W Rx buffer size - 1    */
+
+	unsigned short  tx_lw;		/* W Tx buffer low water mark */
+	unsigned short  rx_lw;		/* W Rx buffer low water mark */
+	unsigned short  rx_hw;		/* W Rx buffer high water mark*/
+	unsigned short  incr;		/* W Increment to next channel*/
+
+	unsigned short  fepdev;		/* U SCC device base address  */
+	unsigned short  edelay;		/* W Exception delay          */
+	unsigned short  blen;		/* W Break length             */
+	unsigned short  btime;		/* U Break complete time      */
+
+	unsigned short  iflag;		/* C UNIX input flags         */
+	unsigned short  oflag;		/* C UNIX output flags        */
+	unsigned short  cflag;		/* C UNIX control flags       */
+	unsigned short  wfill[13];	/* U Reserved for expansion   */
+
+	unsigned char   num;		/* U Channel number           */
+	unsigned char   ract;		/* U Receiver active counter  */
+	unsigned char   bstat;		/* U Break status bits        */
+	unsigned char   tbusy;		/* W Transmit busy            */
+	unsigned char   iempty;		/* W Transmit empty event     */
+					/* enable                     */
+	unsigned char   ilow;		/* W Transmit low-water event */
+					/* enable                     */
+	unsigned char   idata;		/* W Receive data interrupt   */
+					/* enable                     */
+	unsigned char   eflag;		/* U Host event flags         */
+
+	unsigned char   tflag;		/* U Transmit flags           */
+	unsigned char   rflag;		/* U Receive flags            */
+	unsigned char   xmask;		/* U Transmit ready flags     */
+	unsigned char   xval;		/* U Transmit ready value     */
+	unsigned char   m_stat;		/* RC Modem status bits       */
+	unsigned char   m_change;	/* U Modem bits which changed */
+	unsigned char   m_int;		/* W Modem interrupt enable   */
+					/* bits                       */
+	unsigned char   m_last;		/* U Last modem status        */
+
+	unsigned char   mtran;		/* C Unreported modem trans   */
+	unsigned char   orun;		/* C Buffer overrun occurred  */
+	unsigned char   astartc;	/* W Auxiliary Xon char       */
+	unsigned char   astopc;		/* W Auxiliary Xoff char      */
+	unsigned char   startc;		/* W Xon character            */
+	unsigned char   stopc;		/* W Xoff character           */
+	unsigned char   vnextc;		/* W Vnext character          */
+	unsigned char   hflow;		/* C Software flow control    */
+
+	unsigned char   fillc;		/* U Delay Fill character     */
+	unsigned char   ochar;		/* U Saved output character   */
+	unsigned char   omask;		/* U Output character mask    */
+
+	unsigned char   bfill[13];	/* U Reserved for expansion   */
+
+	unsigned char   scc[16];	/* U SCC registers            */
 };
 
 struct cnode {
@@ -1225,7 +1164,7 @@ struct cnode {
 
 	union {
 		struct {
-			char  type;	/* Board Type 		*/
+			char  type;	/* Board Type           */
 			long  port;	/* I/O Address		*/
 			char  *portstr; /* I/O Address in string */
 			long  addr;	/* Memory Address	*/
@@ -1294,29 +1233,17 @@ struct cnode {
 		} module;
 
 		char *ttyname;
-
 		char *cuname;
-
 		char *printname;
-
 		long majornumber;
-
 		long altpin;
-
 		long ttysize;
-
 		long chsize;
-
 		long bssize;
-
 		long unsize;
-
 		long f2size;
-
 		long vpixsize;
-
 		long useintr;
 	} u;
 };
-
 #endif
diff --git a/drivers/staging/dgnc/dgnc_cls.c b/drivers/staging/dgnc/dgnc_cls.c
index 60d9b62c6f1..8e265c20db5 100644
--- a/drivers/staging/dgnc/dgnc_cls.c
+++ b/drivers/staging/dgnc/dgnc_cls.c
@@ -453,10 +453,6 @@ static inline void cls_parse_isr(struct dgnc_board *brd, uint port)
 			cls_copy_data_from_queue_to_uart(ch);
 		}
 
-		/* Received Xoff signal/Special character */
-		if (isr & UART_IIR_XOFF)
-			/* Empty */
-
 		/* CTS/RTS change of state */
 		if (isr & UART_IIR_CTSRTS) {
 			brd->intr_modem++;
diff --git a/drivers/staging/dgnc/dgnc_cls.h b/drivers/staging/dgnc/dgnc_cls.h
index ffe8535a84a..465d79a6f75 100644
--- a/drivers/staging/dgnc/dgnc_cls.h
+++ b/drivers/staging/dgnc/dgnc_cls.h
@@ -61,7 +61,6 @@ struct cls_uart_struct {
 #define UART_16654_FCR_RXTRIGGER_56	0x80
 #define UART_16654_FCR_RXTRIGGER_60     0xC0
 
-#define UART_IIR_XOFF			0x10	/* Received Xoff signal/Special character */
 #define UART_IIR_CTSRTS			0x20	/* Received CTS/RTS change of state */
 #define UART_IIR_RDI_TIMEOUT		0x0C    /* Receiver data TIMEOUT */
 
diff --git a/drivers/staging/dgnc/dgnc_driver.c b/drivers/staging/dgnc/dgnc_driver.c
index b1a39b25b41..5af8300dfb0 100644
--- a/drivers/staging/dgnc/dgnc_driver.c
+++ b/drivers/staging/dgnc/dgnc_driver.c
@@ -153,8 +153,7 @@ struct board_id {
 	unsigned int is_pci_express;
 };
 
-static struct board_id dgnc_Ids[] =
-{
+static struct board_id dgnc_Ids[] = {
 	{	PCI_DEVICE_CLASSIC_4_PCI_NAME,		4,	0	},
 	{	PCI_DEVICE_CLASSIC_4_422_PCI_NAME,	4,	0	},
 	{	PCI_DEVICE_CLASSIC_8_PCI_NAME,		8,	0	},
@@ -219,9 +218,8 @@ int dgnc_init_module(void)
 	 */
 	rc = dgnc_start();
 
-	if (rc < 0) {
+	if (rc < 0)
 		return rc;
-	}
 
 	/*
 	 * Find and configure all the cards
@@ -239,8 +237,7 @@ int dgnc_init_module(void)
 			pr_warn("WARNING: dgnc driver load failed.  No Digi Neo or Classic boards found.\n");
 
 		dgnc_cleanup_module();
-	}
-	else {
+	} else {
 		dgnc_create_driver_sysfiles(&dgnc_driver);
 	}
 
@@ -451,20 +448,15 @@ static void dgnc_cleanup_board(struct dgnc_board *brd)
 	/* Free all allocated channels structs */
 	for (i = 0; i < MAXPORTS ; i++) {
 		if (brd->channels[i]) {
-			if (brd->channels[i]->ch_rqueue)
-				kfree(brd->channels[i]->ch_rqueue);
-			if (brd->channels[i]->ch_equeue)
-				kfree(brd->channels[i]->ch_equeue);
-			if (brd->channels[i]->ch_wqueue)
-				kfree(brd->channels[i]->ch_wqueue);
-
+			kfree(brd->channels[i]->ch_rqueue);
+			kfree(brd->channels[i]->ch_equeue);
+			kfree(brd->channels[i]->ch_wqueue);
 			kfree(brd->channels[i]);
 			brd->channels[i] = NULL;
 		}
 	}
 
-	if (brd->flipbuf)
-		kfree(brd->flipbuf);
+	kfree(brd->flipbuf);
 
 	dgnc_Board[brd->boardnum] = NULL;
 
@@ -519,9 +511,8 @@ static int dgnc_found_board(struct pci_dev *pdev, int id)
 
 	brd->state		= BOARD_FOUND;
 
-	for (i = 0; i < MAXPORTS; i++) {
+	for (i = 0; i < MAXPORTS; i++)
 		brd->channels[i] = NULL;
-	}
 
 	/* store which card & revision we have */
 	pci_read_config_word(pdev, PCI_SUBSYSTEM_VENDOR_ID, &brd->subvendor);
@@ -730,15 +721,18 @@ static int dgnc_finalize_board_init(struct dgnc_board *brd) {
 	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);
+		rc = request_irq(brd->irq, brd->bd_ops->intr,
+				 IRQF_SHARED, "DGNC", brd);
 
 		if (rc) {
-			printk("Failed to hook IRQ %d\n",brd->irq);
+			dev_err(&brd->pdev->dev,
+				"Failed to hook IRQ %d\n", brd->irq);
 			brd->state = BOARD_FAILED;
 			brd->dpastatus = BD_NOFEP;
 			rc = -ENODEV;
 		} else {
-			DPR_INIT(("Requested and received usage of IRQ %d\n", brd->irq));
+			DPR_INIT(("Requested and received usage of IRQ %d\n",
+				  brd->irq));
 		}
 	}
 	return rc;
@@ -799,9 +793,8 @@ static void dgnc_poll_handler(ulong dummy)
 	 * driver tells us its up and running, and has
 	 * everything it needs.
 	 */
-	if (dgnc_driver_state != DRIVER_READY) {
+	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++) {
@@ -831,9 +824,8 @@ schedule_poller:
 
 	new_time = dgnc_poll_time - jiffies;
 
-	if ((ulong) new_time >= 2 * dgnc_poll_tick) {
+	if ((ulong) new_time >= 2 * dgnc_poll_tick)
 		dgnc_poll_time = jiffies +  dgnc_jiffies_from_ms(dgnc_poll_tick);
-	}
 
 	init_timer(&dgnc_poll_timer);
 	dgnc_poll_timer.function = dgnc_poll_handler;
@@ -860,9 +852,8 @@ static void dgnc_init_globals(void)
 	dgnc_trcbuf_size	= trcbuf_size;
 	dgnc_debug		= debug;
 
-	for (i = 0; i < MAXBOARDS; i++) {
+	for (i = 0; i < MAXBOARDS; i++)
 		dgnc_Board[i] = NULL;
-	}
 
 	init_timer(&dgnc_poll_timer);
 }
diff --git a/drivers/staging/dgnc/dgnc_neo.c b/drivers/staging/dgnc/dgnc_neo.c
index cf22c7b725f..9de988cc892 100644
--- a/drivers/staging/dgnc/dgnc_neo.c
+++ b/drivers/staging/dgnc/dgnc_neo.c
@@ -395,7 +395,8 @@ static inline void neo_clear_break(struct channel_t *ch, int force)
 
 	/* Turn break off, and unset some variables */
 	if (ch->ch_flags & CH_BREAK_SENDING) {
-		if ((jiffies >= ch->ch_stop_sending_break) || force) {
+		if (time_after_eq(jiffies, ch->ch_stop_sending_break)
+		    || force) {
 			uchar temp = readb(&ch->ch_neo_uart->lcr);
 			writeb((temp & ~UART_LCR_SBC), &ch->ch_neo_uart->lcr);
 			neo_pci_posting_flush(ch->ch_bd);
@@ -434,9 +435,8 @@ static inline void neo_parse_isr(struct dgnc_board *brd, uint port)
 		isr = readb(&ch->ch_neo_uart->isr_fcr);
 
 		/* Bail if no pending interrupt */
-		if (isr & UART_IIR_NO_INT)  {
+		if (isr & UART_IIR_NO_INT)
 			break;
-		}
 
 		/*
 		 * Yank off the upper 2 bits, which just show that the FIFO's are enabled.
@@ -485,8 +485,7 @@ static inline void neo_parse_isr(struct dgnc_board *brd, uint port)
 					DGNC_UNLOCK(ch->ch_lock, lock_flags);
 				}
 				DPR_INTR(("Port %d. XON detected in incoming data\n", port));
-			}
-			else if (cause == UART_17158_XOFF_DETECT) {
+			} else if (cause == UART_17158_XOFF_DETECT) {
 				if (!(brd->channels[port]->ch_flags & CH_STOP)) {
 					DGNC_LOCK(ch->ch_lock, lock_flags);
 					ch->ch_flags |= CH_STOP;
@@ -511,8 +510,7 @@ static inline void neo_parse_isr(struct dgnc_board *brd, uint port)
 					DGNC_LOCK(ch->ch_lock, lock_flags);
 					ch->ch_mostat |= UART_MCR_RTS;
 					DGNC_UNLOCK(ch->ch_lock, lock_flags);
-				}
-				else {
+				} else {
 					DGNC_LOCK(ch->ch_lock, lock_flags);
 					ch->ch_mostat &= ~(UART_MCR_RTS);
 					DGNC_UNLOCK(ch->ch_lock, lock_flags);
@@ -522,8 +520,7 @@ static inline void neo_parse_isr(struct dgnc_board *brd, uint port)
 					DGNC_LOCK(ch->ch_lock, lock_flags);
 					ch->ch_mostat |= UART_MCR_DTR;
 					DGNC_UNLOCK(ch->ch_lock, lock_flags);
-				}
-				else {
+				} else {
 					DGNC_LOCK(ch->ch_lock, lock_flags);
 					ch->ch_mostat &= ~(UART_MCR_DTR);
 					DGNC_UNLOCK(ch->ch_lock, lock_flags);
@@ -624,8 +621,7 @@ static inline void neo_parse_lsr(struct dgnc_board *brd, uint port)
 
 		/* 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) {
+	} else if (linestatus & UART_17158_TX_AND_FIFO_CLR) {
 		brd->intr_tx++;
 		ch->ch_intr_tx++;
 		DGNC_LOCK(ch->ch_lock, lock_flags);
@@ -654,24 +650,20 @@ static void neo_param(struct tty_struct *tty)
 	struct channel_t *ch;
 	struct un_t   *un;
 
-	if (!tty || tty->magic != TTY_MAGIC) {
+	if (!tty || tty->magic != TTY_MAGIC)
 		return;
-	}
 
 	un = (struct un_t *) tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC) {
+	if (!un || un->magic != DGNC_UNIT_MAGIC)
 		return;
-	}
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC) {
+	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return;
-	}
 
 	bd = ch->ch_bd;
-	if (!bd || bd->magic != DGNC_BOARD_MAGIC) {
+	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));
@@ -777,13 +769,11 @@ static void neo_param(struct tty_struct *tty)
 		}
 	}
 
-	if (ch->ch_c_cflag & PARENB) {
+	if (ch->ch_c_cflag & PARENB)
 		lcr |= UART_LCR_PARITY;
-	}
 
-	if (!(ch->ch_c_cflag & PARODD)) {
+	if (!(ch->ch_c_cflag & PARODD))
 		lcr |= UART_LCR_EPAR;
-	}
 
 	/*
 	 * Not all platforms support mark/space parity,
@@ -832,26 +822,23 @@ static void neo_param(struct tty_struct *tty)
 	if (uart_lcr != lcr)
 		writeb(lcr, &ch->ch_neo_uart->lcr);
 
-	if (ch->ch_c_cflag & CREAD) {
+	if (ch->ch_c_cflag & CREAD)
 		ier |= (UART_IER_RDI | UART_IER_RLSI);
-	}
-	else {
+	else
 		ier &= ~(UART_IER_RDI | UART_IER_RLSI);
-	}
 
 	/*
 	 * Have the UART interrupt on modem signal changes ONLY when
 	 * we are in hardware flow control mode, or CLOCAL/FORCEDCD is not set.
 	 */
-	if ((ch->ch_digi.digi_flags & CTSPACE) || (ch->ch_digi.digi_flags & RTSPACE) ||
-		(ch->ch_c_cflag & CRTSCTS) || !(ch->ch_digi.digi_flags & DIGI_FORCEDCD) ||
-		!(ch->ch_c_cflag & CLOCAL))
-	{
+	if ((ch->ch_digi.digi_flags & CTSPACE) ||
+	    (ch->ch_digi.digi_flags & RTSPACE) ||
+	    (ch->ch_c_cflag & CRTSCTS) ||
+	    !(ch->ch_digi.digi_flags & DIGI_FORCEDCD) ||
+	    !(ch->ch_c_cflag & CLOCAL))
 		ier |= UART_IER_MSI;
-	}
-	else {
+	else
 		ier &= ~UART_IER_MSI;
-	}
 
 	ier |= UART_IER_THRI;
 
@@ -863,29 +850,25 @@ static void neo_param(struct tty_struct *tty)
 
 	if (ch->ch_digi.digi_flags & CTSPACE || ch->ch_c_cflag & CRTSCTS) {
 		neo_set_cts_flow_control(ch);
-	}
-	else if (ch->ch_c_iflag & IXON) {
+	} else if (ch->ch_c_iflag & IXON) {
 		/* If start/stop is set to disable, then we should disable flow control */
 		if ((ch->ch_startc == _POSIX_VDISABLE) || (ch->ch_stopc == _POSIX_VDISABLE))
 			neo_set_no_output_flow_control(ch);
 		else
 			neo_set_ixon_flow_control(ch);
-	}
-	else {
+	} else {
 		neo_set_no_output_flow_control(ch);
 	}
 
 	if (ch->ch_digi.digi_flags & RTSPACE || ch->ch_c_cflag & CRTSCTS) {
 		neo_set_rts_flow_control(ch);
-	}
-	else if (ch->ch_c_iflag & IXOFF) {
+	} else if (ch->ch_c_iflag & IXOFF) {
 		/* If start/stop is set to disable, then we should disable flow control */
 		if ((ch->ch_startc == _POSIX_VDISABLE) || (ch->ch_stopc == _POSIX_VDISABLE))
 			neo_set_no_input_flow_control(ch);
 		else
 			neo_set_ixoff_flow_control(ch);
-	}
-	else {
+	} else {
 		neo_set_no_input_flow_control(ch);
 	}
 
@@ -1225,12 +1208,10 @@ static void neo_copy_data_from_uart_to_queue(struct channel_t *ch)
 		 * The count can be any where from 0-3 bytes "off".
 		 * Bizarre, but true.
 		 */
-		if ((ch->ch_bd->dvid & 0xf0) >= UART_XR17E158_DVID) {
+		if ((ch->ch_bd->dvid & 0xf0) >= UART_XR17E158_DVID)
 			total -= 1;
-		}
-		else {
+		else
 			total -= 3;
-		}
 	}
 
 
@@ -1274,9 +1255,8 @@ static void neo_copy_data_from_uart_to_queue(struct channel_t *ch)
 		 * will reset some bits after our read, we need to ensure
 		 * we don't miss our TX FIFO emptys.
 		 */
-		if (linestatus & (UART_LSR_THRE | UART_17158_TX_AND_FIFO_CLR)) {
+		if (linestatus & (UART_LSR_THRE | UART_17158_TX_AND_FIFO_CLR))
 			ch->ch_flags |= (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
-		}
 
 		linestatus = 0;
 
@@ -1404,19 +1384,16 @@ static int neo_drain(struct tty_struct *tty, uint seconds)
 	struct un_t *un;
 	int rc = 0;
 
-	if (!tty || tty->magic != TTY_MAGIC) {
+	if (!tty || tty->magic != TTY_MAGIC)
 		return -ENXIO;
-	}
 
 	un = (struct un_t *) tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC) {
+	if (!un || un->magic != DGNC_UNIT_MAGIC)
 		return -ENXIO;
-	}
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC) {
+	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return -ENXIO;
-	}
 
 	DPR_IOCTL(("%d Drain wait started.\n", __LINE__));
 
@@ -1433,12 +1410,10 @@ 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) {
+	if (rc)
 		DPR_IOCTL(("%d Drain - User ctrl c'ed\n", __LINE__));
-	}
-	else {
+	else
 		DPR_IOCTL(("%d Drain wait finished.\n", __LINE__));
-	}
 
 	return rc;
 }
@@ -1454,9 +1429,8 @@ static void neo_flush_uart_write(struct channel_t *ch)
 	uchar tmp = 0;
 	int i = 0;
 
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC) {
+	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return;
-	}
 
 	writeb((UART_FCR_ENABLE_FIFO | UART_FCR_CLEAR_XMIT), &ch->ch_neo_uart->isr_fcr);
 	neo_pci_posting_flush(ch->ch_bd);
@@ -1468,8 +1442,7 @@ static void neo_flush_uart_write(struct channel_t *ch)
 		if (tmp & 4) {
 			DPR_IOCTL(("Still flushing TX UART... i: %d\n", i));
 			udelay(10);
-		}
-		else
+		} else
 			break;
 	}
 
@@ -1487,9 +1460,8 @@ static void neo_flush_uart_read(struct channel_t *ch)
 	uchar tmp = 0;
 	int i = 0;
 
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC) {
+	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return;
-	}
 
 	writeb((UART_FCR_ENABLE_FIFO | UART_FCR_CLEAR_RCVR), &ch->ch_neo_uart->isr_fcr);
 	neo_pci_posting_flush(ch->ch_bd);
@@ -1501,8 +1473,7 @@ static void neo_flush_uart_read(struct channel_t *ch)
 		if (tmp & 2) {
 			DPR_IOCTL(("Still flushing RX UART... i: %d\n", i));
 			udelay(10);
-		}
-		else
+		} else
 			break;
 	}
 }
@@ -1598,8 +1569,7 @@ static void neo_copy_data_from_queue_to_uart(struct channel_t *ch)
 		}
 
 		n = UART_17158_TX_FIFOSIZE - ch->ch_t_tlevel;
-	}
-	else {
+	} else {
 		n = UART_17158_TX_FIFOSIZE - readb(&ch->ch_neo_uart->tfifo);
 	}
 
@@ -1963,13 +1933,10 @@ static void neo_vpd(struct dgnc_board *brd)
 		||  (brd->vpd[0x7F] != 0x78))   /* small resource end tag */
 	{
 		memset(brd->vpd, '\0', NEO_VPD_IMAGESIZE);
-	}
-	else {
+	} else {
 		/* Search for the serial number */
-		for (i = 0; i < NEO_VPD_IMAGEBYTES - 3; i++) {
-			if (brd->vpd[i] == 'S' && brd->vpd[i + 1] == 'N') {
+		for (i = 0; i < NEO_VPD_IMAGEBYTES - 3; i++)
+			if (brd->vpd[i] == 'S' && brd->vpd[i + 1] == 'N')
 				strncpy(brd->serial_num, &(brd->vpd[i + 3]), 9);
-			}
-		}
 	}
 }
diff --git a/drivers/staging/dgnc/dgnc_sysfs.c b/drivers/staging/dgnc/dgnc_sysfs.c
index 946230c2348..0f0e8fcb663 100644
--- a/drivers/staging/dgnc/dgnc_sysfs.c
+++ b/drivers/staging/dgnc/dgnc_sysfs.c
@@ -739,7 +739,7 @@ void dgnc_create_tty_sysfs(struct un_t *un, struct device *c)
 
 	ret = sysfs_create_group(&c->kobj, &dgnc_tty_attribute_group);
 	if (ret) {
-		printk(KERN_ERR "dgnc: failed to create sysfs tty device attributes.\n");
+		dev_err(c, "dgnc: failed to create sysfs tty device attributes.\n");
 		sysfs_remove_group(&c->kobj, &dgnc_tty_attribute_group);
 		return;
 	}
diff --git a/drivers/staging/dgnc/dgnc_tty.c b/drivers/staging/dgnc/dgnc_tty.c
index f0b17c36edd..4135cb0ed9f 100644
--- a/drivers/staging/dgnc/dgnc_tty.c
+++ b/drivers/staging/dgnc/dgnc_tty.c
@@ -533,9 +533,8 @@ void dgnc_sniff_nowait_nolock(struct channel_t *ch, uchar *text, uchar *buf, int
 			 * We *cannot* sleep here waiting for space, because this
 			 * function was probably called by the interrupt/timer routines!
 			 */
-			if (n == 0) {
+			if (n == 0)
 				return;
-			}
 
 			/*
 			 * Copy as much data as will fit.
@@ -661,7 +660,7 @@ void dgnc_input(struct channel_t *ch)
 	tp = ch->ch_tun.un_tty;
 
 	bd = ch->ch_bd;
-	if(!bd || bd->magic != DGNC_BOARD_MAGIC)
+	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
 		return;
 
 	DGNC_LOCK(ch->ch_lock, lock_flags);
@@ -803,8 +802,7 @@ void dgnc_input(struct channel_t *ch)
 				else
 					tty_insert_flip_char(tp->port, *(ch->ch_rqueue + tail + i), TTY_NORMAL);
 			}
-		}
-		else {
+		} else {
 			tty_insert_flip_string(tp->port, ch->ch_rqueue + tail, s);
 		}
 
@@ -857,14 +855,11 @@ void dgnc_carrier(struct channel_t *ch)
 		phys_carrier = 1;
 	}
 
-	if (ch->ch_digi.digi_flags & DIGI_FORCEDCD) {
+	if (ch->ch_digi.digi_flags & DIGI_FORCEDCD)
 		virt_carrier = 1;
-	}
 
-	if (ch->ch_c_cflag & CLOCAL) {
+	if (ch->ch_c_cflag & CLOCAL)
 		virt_carrier = 1;
-	}
-
 
 	DPR_CARR(("DCD: physical: %d virt: %d\n", phys_carrier, virt_carrier));
 
@@ -1005,11 +1000,10 @@ static void dgnc_set_custom_speed(struct channel_t *ch, uint newrate)
 			deltahigh = testrate_high - newrate;
 			deltalow = newrate - testrate_low;
 
-			if (deltahigh < deltalow) {
+			if (deltahigh < deltalow)
 				newrate = testrate_high;
-			} else {
+			else
 				newrate = testrate_low;
-			}
 		}
 	}
 
@@ -1044,7 +1038,7 @@ void dgnc_check_queue_flow_control(struct channel_t *ch)
 	if (qleft < 256) {
 		/* HWFLOW */
 		if (ch->ch_digi.digi_flags & CTSPACE || ch->ch_c_cflag & CRTSCTS) {
-			if(!(ch->ch_flags & CH_RECEIVER_OFF)) {
+			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",
@@ -1222,15 +1216,13 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 	major = MAJOR(tty_devnum(tty));
 	minor = MINOR(tty_devnum(tty));
 
-	if (major > 255) {
+	if (major > 255)
 		return -ENXIO;
-	}
 
 	/* Get board pointer from our array of majors we have allocated */
 	brd = dgnc_BoardsByMajor[major];
-	if (!brd) {
+	if (!brd)
 		return -ENXIO;
-	}
 
 	/*
 	 * If board is not yet up to a state of READY, go to
@@ -1239,9 +1231,8 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 	rc = wait_event_interruptible(brd->state_wait,
 		(brd->state & BOARD_READY));
 
-	if (rc) {
+	if (rc)
 		return rc;
-	}
 
 	DGNC_LOCK(brd->bd_lock, lock_flags);
 
@@ -1267,12 +1258,10 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 	if (!IS_PRINT(minor)) {
 		un = &brd->channels[PORT_NUM(minor)]->ch_tun;
 		un->un_type = DGNC_SERIAL;
-	}
-	else if (IS_PRINT(minor)) {
+	} else if (IS_PRINT(minor)) {
 		un = &brd->channels[PORT_NUM(minor)]->ch_pun;
 		un->un_type = DGNC_PRINT;
-	}
-	else {
+	} else {
 		DGNC_UNLOCK(ch->ch_lock, lock_flags);
 		DPR_OPEN(("%d Unknown TYPE!\n", __LINE__));
 		return -ENXIO;
@@ -1409,10 +1398,9 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 
 	rc = dgnc_block_til_ready(tty, file, ch);
 
-	if (rc) {
+	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);
@@ -1444,9 +1432,8 @@ static int dgnc_block_til_ready(struct tty_struct *tty, struct file *file, struc
 	}
 
 	un = tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC) {
+	if (!un || un->magic != DGNC_UNIT_MAGIC)
 		return -ENXIO;
-	}
 
 	DPR_OPEN(("dgnc_block_til_ready - before block.\n"));
 
@@ -1489,9 +1476,8 @@ static int dgnc_block_til_ready(struct tty_struct *tty, struct file *file, struc
 			 * 3) DCD (fake or real) is active.
 			 */
 
-			if (file->f_flags & O_NONBLOCK) {
+			if (file->f_flags & O_NONBLOCK)
 				break;
-			}
 
 			if (tty->flags & (1 << TTY_IO_ERROR)) {
 				retval = -EIO;
@@ -1507,8 +1493,7 @@ static int dgnc_block_til_ready(struct tty_struct *tty, struct file *file, struc
 				DPR_OPEN(("%d: ch_flags: %x\n", __LINE__, ch->ch_flags));
 				break;
 			}
-		}
-		else {
+		} else {
 			sleep_on_un_flags = 1;
 		}
 
@@ -1547,14 +1532,12 @@ static int dgnc_block_til_ready(struct tty_struct *tty, struct file *file, struc
 		/*
 		 * Wait for something in the flags to change from the current value.
 		 */
-		if (sleep_on_un_flags) {
+		if (sleep_on_un_flags)
 			retval = wait_event_interruptible(un->un_flags_wait,
 				(old_flags != (ch->ch_tun.un_flags | ch->ch_pun.un_flags)));
-		}
-		else {
+		else
 			retval = wait_event_interruptible(ch->ch_flags_wait,
 				(old_flags != ch->ch_flags));
-		}
 
 		DPR_OPEN(("After sleep... retval: %x\n", retval));
 
@@ -1693,7 +1676,7 @@ static void dgnc_tty_close(struct tty_struct *tty, struct file *file)
 		/*
 		 * turn off print device when closing print device.
 		 */
-		if ((un->un_type == DGNC_PRINT) && (ch->ch_flags & CH_PRON) ) {
+		if ((un->un_type == DGNC_PRINT) && (ch->ch_flags & CH_PRON)) {
 			dgnc_wmove(ch, ch->ch_digi.digi_offstr,
 				(int) ch->ch_digi.digi_offlen);
 			ch->ch_flags &= ~CH_PRON;
@@ -1708,9 +1691,8 @@ static void dgnc_tty_close(struct tty_struct *tty, struct file *file)
 
 		DPR_CLOSE(("After calling wait_for_drain\n"));
 
-		if (rc) {
+		if (rc)
 			DPR_BASIC(("dgnc_tty_close - bad return: %d ", rc));
-		}
 
 		dgnc_tty_flush_buffer(tty);
 		tty_ldisc_flush(tty);
@@ -1748,12 +1730,11 @@ static void dgnc_tty_close(struct tty_struct *tty, struct file *file)
 
 		/* Turn off UART interrupts for this port */
 		ch->ch_bd->bd_ops->uart_off(ch);
-	}
-	else {
+	} else {
 		/*
 		 * turn off print device when closing print device.
 		 */
-		if ((un->un_type == DGNC_PRINT) && (ch->ch_flags & CH_PRON) ) {
+		if ((un->un_type == DGNC_PRINT) && (ch->ch_flags & CH_PRON)) {
 			dgnc_wmove(ch, ch->ch_digi.digi_offstr,
 				(int) ch->ch_digi.digi_offlen);
 			ch->ch_flags &= ~CH_PRON;
@@ -1857,7 +1838,7 @@ static int dgnc_maxcps_room(struct tty_struct *tty, int bytes_available)
 	if (un->un_type != DGNC_PRINT)
 		return bytes_available;
 
-	if (ch->ch_digi.digi_maxcps > 0 && ch->ch_digi.digi_bufsize > 0 ) {
+	if (ch->ch_digi.digi_maxcps > 0 && ch->ch_digi.digi_bufsize > 0) {
 		int cps_limit = 0;
 		unsigned long current_time = jiffies;
 		unsigned long buffer_time = current_time +
@@ -1867,12 +1848,10 @@ static int dgnc_maxcps_room(struct tty_struct *tty, int bytes_available)
 			/* buffer is empty */
 			ch->ch_cpstime = current_time;	    /* reset ch_cpstime */
 			cps_limit = ch->ch_digi.digi_bufsize;
-		}
-		else if (ch->ch_cpstime < buffer_time) {
+		} else if (ch->ch_cpstime < buffer_time) {
 			/* still room in the buffer */
 			cps_limit = ((buffer_time - ch->ch_cpstime) * ch->ch_digi.digi_maxcps) / HZ;
-		}
-		else {
+		} else {
 			/* no room in the buffer */
 			cps_limit = 0;
 		}
@@ -1931,8 +1910,7 @@ static int dgnc_tty_write_room(struct tty_struct *tty)
 		if (!(ch->ch_flags & CH_PRON))
 			ret -= ch->ch_digi.digi_onlen;
 		ret -= ch->ch_digi.digi_offlen;
-	}
-	else {
+	} else {
 		if (ch->ch_flags & CH_PRON)
 			ret -= ch->ch_digi.digi_offlen;
 	}
@@ -2086,9 +2064,8 @@ static int dgnc_tty_write(struct tty_struct *tty,
 		 * the board.
 		 */
 		/* we're allowed to block if it's from_user */
-		if (down_interruptible(&dgnc_TmpWriteSem)) {
+		if (down_interruptible(&dgnc_TmpWriteSem))
 			return -EINTR;
-		}
 
 		/*
 		 * copy_from_user() returns the number
@@ -2271,21 +2248,17 @@ static int dgnc_tty_tiocmset(struct tty_struct *tty,
 
 	DGNC_LOCK(ch->ch_lock, lock_flags);
 
-	if (set & TIOCM_RTS) {
+	if (set & TIOCM_RTS)
 		ch->ch_mostat |= UART_MCR_RTS;
-	}
 
-	if (set & TIOCM_DTR) {
+	if (set & TIOCM_DTR)
 		ch->ch_mostat |= UART_MCR_DTR;
-	}
 
-	if (clear & TIOCM_RTS) {
+	if (clear & TIOCM_RTS)
 		ch->ch_mostat &= ~(UART_MCR_RTS);
-	}
 
-	if (clear & TIOCM_DTR) {
+	if (clear & TIOCM_DTR)
 		ch->ch_mostat &= ~(UART_MCR_DTR);
-	}
 
 	ch->ch_bd->bd_ops->assert_modem_signals(ch);
 
@@ -2535,42 +2508,34 @@ static int dgnc_set_modem_info(struct tty_struct *tty, unsigned int command, uns
 
 	switch (command) {
 	case TIOCMBIS:
-		if (arg & TIOCM_RTS) {
+		if (arg & TIOCM_RTS)
 			ch->ch_mostat |= UART_MCR_RTS;
-		}
 
-		if (arg & TIOCM_DTR) {
+		if (arg & TIOCM_DTR)
 			ch->ch_mostat |= UART_MCR_DTR;
-		}
 
 		break;
 
 	case TIOCMBIC:
-		if (arg & TIOCM_RTS) {
+		if (arg & TIOCM_RTS)
 			ch->ch_mostat &= ~(UART_MCR_RTS);
-		}
 
-		if (arg & TIOCM_DTR) {
+		if (arg & TIOCM_DTR)
 			ch->ch_mostat &= ~(UART_MCR_DTR);
-		}
 
 		break;
 
 	case TIOCMSET:
 
-		if (arg & TIOCM_RTS) {
+		if (arg & TIOCM_RTS)
 			ch->ch_mostat |= UART_MCR_RTS;
-		}
-		else {
+		else
 			ch->ch_mostat &= ~(UART_MCR_RTS);
-		}
 
-		if (arg & TIOCM_DTR) {
+		if (arg & TIOCM_DTR)
 			ch->ch_mostat |= UART_MCR_DTR;
-		}
-		else {
+		else
 			ch->ch_mostat &= ~(UART_MCR_DTR);
-		}
 
 		break;
 
@@ -3048,9 +3013,8 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		 */
 		rc = tty_check_change(tty);
 		DGNC_UNLOCK(ch->ch_lock, lock_flags);
-		if (rc) {
+		if (rc)
 			return rc;
-		}
 
 		rc = ch->ch_bd->bd_ops->drain(tty, 0);
 
@@ -3061,7 +3025,7 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 
 		DGNC_LOCK(ch->ch_lock, lock_flags);
 
-		if(((cmd == TCSBRK) && (!arg)) || (cmd == TCSBRKP)) {
+		if (((cmd == TCSBRK) && (!arg)) || (cmd == TCSBRKP)) {
 			ch->ch_bd->bd_ops->send_break(ch, 250);
 		}
 
@@ -3081,9 +3045,8 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		 */
 		rc = tty_check_change(tty);
 		DGNC_UNLOCK(ch->ch_lock, lock_flags);
-		if (rc) {
+		if (rc)
 			return rc;
-		}
 
 		rc = ch->ch_bd->bd_ops->drain(tty, 0);
 		if (rc) {
@@ -3105,9 +3068,8 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 	case TIOCSBRK:
 		rc = tty_check_change(tty);
 		DGNC_UNLOCK(ch->ch_lock, lock_flags);
-		if (rc) {
+		if (rc)
 			return rc;
-		}
 
 		rc = ch->ch_bd->bd_ops->drain(tty, 0);
 		if (rc) {
@@ -3279,8 +3241,7 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 				return -EINTR;
 			}
 			DGNC_LOCK(ch->ch_lock, lock_flags);
-		}
-		else {
+		} else {
 			tty_ldisc_flush(tty);
 		}
 		/* fall thru */
@@ -3370,9 +3331,9 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 
 		DGNC_UNLOCK(ch->ch_lock, lock_flags);
 
-		if (copy_to_user(uarg, &buf, sizeof(buf))) {
+		if (copy_to_user(uarg, &buf, sizeof(buf)))
 			return -EFAULT;
-		}
+
 		return 0;
 	}
 
@@ -3389,9 +3350,9 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		/* NOTE: MORE EVENTS NEEDS TO BE ADDED HERE */
 		if (ch->ch_flags & CH_BREAK_SENDING)
 			events |= EV_TXB;
-		if ((ch->ch_flags & CH_STOP) || (ch->ch_flags & CH_FORCED_STOP)) {
+		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)) {
 			events |= (EV_IPU | EV_IPS);
 		}
@@ -3418,9 +3379,8 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		/*
 		 * Get data from user first.
 		 */
-		if (copy_from_user(&buf, uarg, sizeof(buf))) {
+		if (copy_from_user(&buf, uarg, sizeof(buf)))
 			return -EFAULT;
-		}
 
 		DGNC_LOCK(ch->ch_lock, lock_flags);
 
@@ -3449,24 +3409,22 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		 * insert more characters into our queue for OPOST processing
 		 * that the RealPort Server doesn't know about.
 		 */
-		if (buf.txbuf > tdist) {
+		if (buf.txbuf > tdist)
 			buf.txbuf = tdist;
-		}
 
 		/*
 		 * Report whether our queue and UART TX are completely empty.
 		 */
-		if (count) {
+		if (count)
 			buf.txdone = 0;
-		} else {
+		else
 			buf.txdone = 1;
-		}
 
 		DGNC_UNLOCK(ch->ch_lock, lock_flags);
 
-		if (copy_to_user(uarg, &buf, sizeof(buf))) {
+		if (copy_to_user(uarg, &buf, sizeof(buf)))
 			return -EFAULT;
-		}
+
 		return 0;
 	}
 	default:
diff --git a/drivers/staging/et131x/Module.symvers b/drivers/staging/et131x/Module.symvers
deleted file mode 100644
index e69de29bb2d..00000000000
--- a/drivers/staging/et131x/Module.symvers
+++ /dev/null
diff --git a/drivers/staging/et131x/et131x.c b/drivers/staging/et131x/et131x.c
index d329cf31436..0901ef5d6e8 100644
--- a/drivers/staging/et131x/et131x.c
+++ b/drivers/staging/et131x/et131x.c
@@ -762,6 +762,7 @@ static int et131x_init_eeprom(struct et131x_adapter *adapter)
 	 */
 	if (eestatus & 0x4C) {
 		int write_failed = 0;
+
 		if (pdev->revision == 0x01) {
 			int	i;
 			static const u8 eedata[4] = { 0xFE, 0x13, 0x10, 0xFF };
@@ -2091,6 +2092,7 @@ static void et1310_disable_phy_coma(struct et131x_adapter *adapter)
 static inline u32 bump_free_buff_ring(u32 *free_buff_ring, u32 limit)
 {
 	u32 tmp_free_buff_ring = *free_buff_ring;
+
 	tmp_free_buff_ring++;
 	/* This works for all cases where limit < 1024. The 1023 case
 	 * works because 1023++ is 1024 which means the if condition is not
@@ -4237,7 +4239,6 @@ static int et131x_ioctl(struct net_device *netdev, struct ifreq *reqbuf,
 static int et131x_set_packet_filter(struct et131x_adapter *adapter)
 {
 	int filter = adapter->packet_filter;
-	int status = 0;
 	u32 ctrl;
 	u32 pf_ctrl;
 
@@ -4288,7 +4289,7 @@ static int et131x_set_packet_filter(struct et131x_adapter *adapter)
 		writel(pf_ctrl, &adapter->regs->rxmac.pf_ctrl);
 		writel(ctrl, &adapter->regs->rxmac.ctrl);
 	}
-	return status;
+	return 0;
 }
 
 /* et131x_multicast - The handler to configure multicasting on the interface */
diff --git a/drivers/staging/frontier/tranzport.c b/drivers/staging/frontier/tranzport.c
index 0571988c58f..2f86163d701 100644
--- a/drivers/staging/frontier/tranzport.c
+++ b/drivers/staging/frontier/tranzport.c
@@ -271,8 +271,8 @@ static void usb_tranzport_interrupt_in_callback(struct urb *urb)
 			 dev->interrupt_in_buffer[6],
 			 dev->interrupt_in_buffer[7]);
 #if SUPPRESS_EXTRA_OFFLINE_EVENTS
-	if (dev->offline == 2 && dev->interrupt_in_buffer[1] == 0xff)
-		goto resubmit;
+		if (dev->offline == 2 && dev->interrupt_in_buffer[1] == 0xff)
+			goto resubmit;
 		if (dev->offline == 1 && dev->interrupt_in_buffer[1] == 0xff) {
 			dev->offline = 2;
 			goto resubmit;
@@ -285,8 +285,8 @@ static void usb_tranzport_interrupt_in_callback(struct urb *urb)
 			dev->offline = 1;
 
 #endif	/* SUPPRESS_EXTRA_OFFLINE_EVENTS */
-	   dbg_info(&dev->intf->dev, "%s: head, tail are %x, %x\n",
-		__func__, dev->ring_head, dev->ring_tail);
+		dbg_info(&dev->intf->dev, "%s: head, tail are %x, %x\n",
+			 __func__, dev->ring_head, dev->ring_tail);
 
 		next_ring_head = (dev->ring_head + 1) % ring_buffer_size;
 
@@ -475,6 +475,7 @@ static unsigned int usb_tranzport_poll(struct file *file, poll_table *wait)
 {
 	struct usb_tranzport *dev;
 	unsigned int mask = 0;
+
 	dev = file->private_data;
 	poll_wait(file, &dev->read_wait, wait);
 	poll_wait(file, &dev->write_wait, wait);
@@ -937,6 +938,7 @@ static void usb_tranzport_disconnect(struct usb_interface *intf)
 {
 	struct usb_tranzport *dev;
 	int minor;
+
 	mutex_lock(&disconnect_mutex);
 	dev = usb_get_intfdata(intf);
 	usb_set_intfdata(intf, NULL);
diff --git a/drivers/staging/ft1000/ft1000-pcmcia/ft1000.h b/drivers/staging/ft1000/ft1000-pcmcia/ft1000.h
index 65f7ab6cb46..0c21ac68003 100644
--- a/drivers/staging/ft1000/ft1000-pcmcia/ft1000.h
+++ b/drivers/staging/ft1000/ft1000-pcmcia/ft1000.h
@@ -25,7 +25,10 @@
 
 #define FT1000_DPRAM_BASE	0x0000	/* Dual Port RAM starting offset */
 
-/* Maximum number of occurrence of pseudo header errors before resetting PC Card. */
+/*
+ * Maximum number of occurrence of pseudo header errors before resetting PC
+ * Card.
+ */
 #define MAX_PH_ERR	300
 
 #define SUCCESS	0x00
@@ -40,16 +43,17 @@ struct ft1000_pcmcia {
 struct pcmcia_device;
 struct net_device;
 extern struct net_device *init_ft1000_card(struct pcmcia_device *link,
-						void *ft1000_reset);
+					   void *ft1000_reset);
 extern void stop_ft1000_card(struct net_device *dev);
 extern int card_download(struct net_device *dev, const u8 *pFileStart,
-			size_t FileLength);
+			 size_t FileLength);
 extern void ft1000InitProc(struct net_device *dev);
 extern void ft1000CleanupProc(struct net_device *dev);
 
 extern u16 ft1000_read_dpram(struct net_device *dev, int offset);
 extern void card_bootload(struct net_device *dev);
-extern u16 ft1000_read_dpram_mag_16(struct net_device *dev, int offset, int Index);
+extern u16 ft1000_read_dpram_mag_16(struct net_device *dev, int offset,
+				    int Index);
 extern u32 ft1000_read_dpram_mag_32(struct net_device *dev, int offset);
 void ft1000_write_dpram_mag_32(struct net_device *dev, int offset, u32 value);
 
@@ -60,7 +64,8 @@ static inline u16 ft1000_read_reg(struct net_device *dev, u16 offset)
 }
 
 /* Set the value of a given ASIC register. */
-static inline void ft1000_write_reg(struct net_device *dev, u16 offset, u16 value)
+static inline void ft1000_write_reg(struct net_device *dev, u16 offset,
+				    u16 value)
 {
 	outw(value, dev->base_addr + offset);
 }
diff --git a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_cs.c b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_cs.c
index f376ca43720..1f8b3ca35c6 100644
--- a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_cs.c
+++ b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_cs.c
@@ -95,20 +95,20 @@ static int ft1000_config(struct pcmcia_device *link)
 	/* setup IO window */
 	ret = pcmcia_loop_config(link, ft1000_confcheck, NULL);
 	if (ret) {
-		printk(KERN_INFO "ft1000: Could not configure pcmcia\n");
+		dev_err(&link->dev, "Could not configure pcmcia\n");
 		return -ENODEV;
 	}
 
 	/* configure device */
 	ret = pcmcia_enable_device(link);
 	if (ret) {
-		printk(KERN_INFO "ft1000: could not enable pcmcia\n");
+		dev_err(&link->dev, "Could not enable pcmcia\n");
 		goto failed;
 	}
 
 	link->priv = init_ft1000_card(link, &ft1000_reset);
 	if (!link->priv) {
-		printk(KERN_INFO "ft1000: Could not register as network device\n");
+		dev_err(&link->dev, "Could not register as network device\n");
 		goto failed;
 	}
 
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c b/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
index ffdc7f597a9..a8945b78596 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
@@ -417,8 +417,8 @@ static long ft1000_ioctl(struct file *file, unsigned int command,
     u16 tempword;
     unsigned long flags;
     struct timeval tv;
-    IOCTL_GET_VER get_ver_data;
-    IOCTL_GET_DSP_STAT get_stat_data;
+	struct IOCTL_GET_VER get_ver_data;
+	struct IOCTL_GET_DSP_STAT get_stat_data;
     u8 ConnectionMsg[] = {0x00,0x44,0x10,0x20,0x80,0x00,0x00,0x00,0x00,0x00,0x03,0x00,0x00,0x00,0x93,0x64,
                           0x00,0x00,0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x05,0x00,0x00,0x00,0x0a,
                           0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
@@ -527,8 +527,8 @@ static long ft1000_ioctl(struct file *file, unsigned int command,
         break;
     case IOCTL_SET_DPRAM_CMD:
         {
-            IOCTL_DPRAM_BLK *dpram_data = NULL;
-            /* IOCTL_DPRAM_COMMAND dpram_command; */
+		struct IOCTL_DPRAM_BLK *dpram_data = NULL;
+		/* struct IOCTL_DPRAM_COMMAND dpram_command; */
             u16 qtype;
             u16 msgsz;
 		struct pseudo_hdr *ppseudo_hdr;
@@ -672,7 +672,7 @@ static long ft1000_ioctl(struct file *file, unsigned int command,
     case IOCTL_GET_DPRAM_CMD:
         {
 		struct dpram_blk *pdpram_blk;
-            IOCTL_DPRAM_BLK __user *pioctl_dpram;
+		struct IOCTL_DPRAM_BLK __user *pioctl_dpram;
             int msglen;
 
             /* DEBUG("FT1000:ft1000_ioctl: IOCTL_FT1000_GET_DPRAM called\n"); */
@@ -785,7 +785,7 @@ static int ft1000_release(struct inode *inode, struct file *file)
 
     /* initialize application information */
     ft1000dev->appcnt--;
-    DEBUG("ft1000_chdev:%s:appcnt = %d\n", __FUNCTION__, ft1000dev->appcnt);
+    DEBUG("ft1000_chdev:%s:appcnt = %d\n", __func__, ft1000dev->appcnt);
     ft1000dev->app_info[i].fileobject = NULL;
 
     return 0;
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_download.c b/drivers/staging/ft1000/ft1000-usb/ft1000_download.c
index cab9cdf6273..65f98016713 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_download.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_download.c
@@ -522,7 +522,6 @@ static void usb_dnld_complete(struct urb *urb)
 static int write_blk_fifo(struct ft1000_usb *ft1000dev, u16 **pUsFile,
 			  u8 **pUcFile, long word_length)
 {
-	int Status = 0;
 	int byte_length;
 
 	byte_length = word_length * 4;
@@ -547,7 +546,7 @@ static int write_blk_fifo(struct ft1000_usb *ft1000dev, u16 **pUsFile,
 	*pUsFile = *pUsFile + (word_length << 1);
 	*pUcFile = *pUcFile + (word_length << 2);
 
-	return Status;
+	return 0;
 }
 
 static int scram_start_dwnld(struct ft1000_usb *ft1000dev, u16 *hshake,
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_ioctl.h b/drivers/staging/ft1000/ft1000-usb/ft1000_ioctl.h
index 419e534874f..cb644a58d9f 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_ioctl.h
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_ioctl.h
@@ -23,83 +23,101 @@
 * History:
 * 11/5/02    Whc                Created.
 *
-*---------------------------------------------------------------------------//---------------------------------------------------------------------------
+*---------------------------------------------------------------------------
 */
 #ifndef _FT1000IOCTLH_
 #define _FT1000IOCTLH_
 
-typedef struct _IOCTL_GET_VER {
-    unsigned long drv_ver;
-} __packed IOCTL_GET_VER, *PIOCTL_GET_VER;
+struct IOCTL_GET_VER {
+	unsigned long drv_ver;
+} __packed;
 
 /* Data structure for Dsp statistics */
-typedef struct _IOCTL_GET_DSP_STAT {
-    unsigned char DspVer[DSPVERSZ];        /* DSP version number */
-    unsigned char HwSerNum[HWSERNUMSZ];    /* Hardware Serial Number */
-    unsigned char Sku[SKUSZ];              /* SKU */
-    unsigned char eui64[EUISZ];            /* EUI64 */
-    unsigned short ConStat;                /* Connection Status */
-                                /*    Bits 0-3 = Connection Status Field */
-                                /*               0000=Idle (Disconnect) */
-                                /*               0001=Searching */
-                                /*               0010=Active (Connected) */
-                                /*               0011=Waiting for L2 down */
-                                /*               0100=Sleep */
-    unsigned short LedStat;                /* Led Status */
-                                /*    Bits 0-3   = Signal Strength Field */
-                                /*                 0000 = -105dBm to -92dBm */
-                                /*                 0001 = -92dBm to -85dBm */
-                                /*                 0011 = -85dBm to -75dBm */
-                                /*                 0111 = -75dBm to -50dBm */
-                                /*                 1111 = -50dBm to 0dBm */
-                                /*    Bits 4-7   = Reserved */
-                                /*    Bits 8-11  = SNR Field */
-                                /*                 0000 = <2dB */
-                                /*                 0001 = 2dB to 8dB */
-                                /*                 0011 = 8dB to 15dB */
-                                /*                 0111 = 15dB to 22dB */
-                                /*                 1111 = >22dB */
-                                /*    Bits 12-15 = Reserved */
-    unsigned long nTxPkts;                /* Number of packets transmitted from host to dsp */
-    unsigned long nRxPkts;                /* Number of packets received from dsp to host */
-    unsigned long nTxBytes;               /* Number of bytes transmitted from host to dsp */
-    unsigned long nRxBytes;               /* Number of bytes received from dsp to host */
-    unsigned long ConTm;                  /* Current session connection time in seconds */
-    unsigned char CalVer[CALVERSZ];       /* Proprietary Calibration Version */
-    unsigned char CalDate[CALDATESZ];     /* Proprietary Calibration Date */
-} __packed IOCTL_GET_DSP_STAT, *PIOCTL_GET_DSP_STAT;
+struct IOCTL_GET_DSP_STAT {
+	unsigned char DspVer[DSPVERSZ];        /* DSP version number */
+	unsigned char HwSerNum[HWSERNUMSZ];    /* Hardware Serial Number */
+	unsigned char Sku[SKUSZ];              /* SKU */
+	unsigned char eui64[EUISZ];            /* EUI64 */
+	unsigned short ConStat;                /* Connection Status */
+	/*    Bits 0-3 = Connection Status Field */
+	/*               0000=Idle (Disconnect) */
+	/*               0001=Searching */
+	/*               0010=Active (Connected) */
+	/*               0011=Waiting for L2 down */
+	/*               0100=Sleep */
+	unsigned short LedStat;                /* Led Status */
+	/*    Bits 0-3   = Signal Strength Field */
+	/*                 0000 = -105dBm to -92dBm */
+	/*                 0001 = -92dBm to -85dBm */
+	/*                 0011 = -85dBm to -75dBm */
+	/*                 0111 = -75dBm to -50dBm */
+	/*                 1111 = -50dBm to 0dBm */
+	/*    Bits 4-7   = Reserved */
+	/*    Bits 8-11  = SNR Field */
+	/*                 0000 = <2dB */
+	/*                 0001 = 2dB to 8dB */
+	/*                 0011 = 8dB to 15dB */
+	/*                 0111 = 15dB to 22dB */
+	/*                 1111 = >22dB */
+	/*    Bits 12-15 = Reserved */
+	unsigned long nTxPkts;                /* Number of packets transmitted
+					       * from host to dsp
+					       */
+	unsigned long nRxPkts;                /* Number of packets received from
+					       * dsp to host
+					       */
+	unsigned long nTxBytes;               /* Number of bytes transmitted
+					       * from host to dsp
+					       */
+	unsigned long nRxBytes;               /* Number of bytes received from
+					       * dsp to host
+					       */
+	unsigned long ConTm;                  /* Current session connection time
+					       * in seconds
+					       */
+	unsigned char CalVer[CALVERSZ];       /* Proprietary Calibration
+					       * Version
+					       */
+	unsigned char CalDate[CALDATESZ];     /* Proprietary Calibration Date */
+} __packed;
 
 /* Data structure for Dual Ported RAM messaging between Host and Dsp */
-typedef struct _IOCTL_DPRAM_BLK {
-    unsigned short total_len;
+struct IOCTL_DPRAM_BLK {
+	unsigned short total_len;
 	struct pseudo_hdr pseudohdr;
-    unsigned char buffer[1780];
-} __packed IOCTL_DPRAM_BLK, *PIOCTL_DPRAM_BLK;
+	unsigned char buffer[1780];
+} __packed;
 
-typedef struct _IOCTL_DPRAM_COMMAND {
-    unsigned short extra;
-    IOCTL_DPRAM_BLK dpram_blk;
-} __packed IOCTL_DPRAM_COMMAND, *PIOCTL_DPRAM_COMMAND;
+struct IOCTL_DPRAM_COMMAND {
+	unsigned short extra;
+	struct IOCTL_DPRAM_BLK dpram_blk;
+} __packed;
 
 /*
 * Custom IOCTL command codes
 */
 #define FT1000_MAGIC_CODE      'F'
 
-#define IOCTL_REGISTER_CMD					0
-#define IOCTL_SET_DPRAM_CMD					3
-#define IOCTL_GET_DPRAM_CMD					4
-#define IOCTL_GET_DSP_STAT_CMD      6
-#define IOCTL_GET_VER_CMD           7
-#define IOCTL_CONNECT               10
-#define IOCTL_DISCONNECT            11
+#define IOCTL_REGISTER_CMD	0
+#define IOCTL_SET_DPRAM_CMD	3
+#define IOCTL_GET_DPRAM_CMD	4
+#define IOCTL_GET_DSP_STAT_CMD	6
+#define IOCTL_GET_VER_CMD	7
+#define IOCTL_CONNECT		10
+#define IOCTL_DISCONNECT	11
 
-#define IOCTL_FT1000_GET_DSP_STAT _IOR(FT1000_MAGIC_CODE, IOCTL_GET_DSP_STAT_CMD, sizeof(IOCTL_GET_DSP_STAT)
-#define IOCTL_FT1000_GET_VER _IOR(FT1000_MAGIC_CODE, IOCTL_GET_VER_CMD, sizeof(IOCTL_GET_VER)
-#define IOCTL_FT1000_CONNECT _IOW(FT1000_MAGIC_CODE, IOCTL_CONNECT, 0
-#define IOCTL_FT1000_DISCONNECT _IOW(FT1000_MAGIC_CODE, IOCTL_DISCONNECT, 0
-#define IOCTL_FT1000_SET_DPRAM _IOW(FT1000_MAGIC_CODE, IOCTL_SET_DPRAM_CMD, sizeof(IOCTL_DPRAM_BLK)
-#define IOCTL_FT1000_GET_DPRAM _IOR(FT1000_MAGIC_CODE, IOCTL_GET_DPRAM_CMD, sizeof(IOCTL_DPRAM_BLK)
-#define IOCTL_FT1000_REGISTER  _IOW(FT1000_MAGIC_CODE, IOCTL_REGISTER_CMD, sizeof(unsigned short *)
-#endif /* _FT1000IOCTLH_ */
+#define IOCTL_FT1000_GET_DSP_STAT _IOR(FT1000_MAGIC_CODE,      \
+				       IOCTL_GET_DSP_STAT_CMD, \
+				       struct IOCTL_GET_DSP_STAT)
+#define IOCTL_FT1000_GET_VER _IOR(FT1000_MAGIC_CODE, IOCTL_GET_VER_CMD, \
+				  struct IOCTL_GET_VER)
+#define IOCTL_FT1000_CONNECT _IO(FT1000_MAGIC_CODE, IOCTL_CONNECT)
+#define IOCTL_FT1000_DISCONNECT _IO(FT1000_MAGIC_CODE, IOCTL_DISCONNECT)
+#define IOCTL_FT1000_SET_DPRAM _IOW(FT1000_MAGIC_CODE, IOCTL_SET_DPRAM_CMD, \
+				    struct IOCTL_DPRAM_BLK)
+#define IOCTL_FT1000_GET_DPRAM _IOR(FT1000_MAGIC_CODE, IOCTL_GET_DPRAM_CMD, \
+				    struct IOCTL_DPRAM_BLK)
+#define IOCTL_FT1000_REGISTER  _IOW(FT1000_MAGIC_CODE, IOCTL_REGISTER_CMD, \
+				    unsigned short *)
 
+#endif /* _FT1000IOCTLH_ */
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_proc.c b/drivers/staging/ft1000/ft1000-usb/ft1000_proc.c
index 2575d0d6bff..e89b5d2c8e6 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_proc.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_proc.c
@@ -32,17 +32,20 @@
 
 
 #define seq_putx(m, message, size, var) \
-	seq_printf(m, message);	\
-	for (i = 0; i < (size - 1); i++) \
-		seq_printf(m, "%02x:", var[i]); \
-	seq_printf(m, "%02x\n", var[i])
+	do { \
+		seq_printf(m, message);	\
+		for (i = 0; i < (size - 1); i++) \
+			seq_printf(m, "%02x:", var[i]); \
+		seq_printf(m, "%02x\n", var[i]);	\
+	} while (0)
 
 #define seq_putd(m, message, size, var) \
-	seq_printf(m, message); \
-	for (i = 0; i < (size - 1); i++) \
-		seq_printf(m, "%d.", var[i]); \
-	seq_printf(m, "%d\n", var[i])
-
+	do { \
+		seq_printf(m, message); \
+		for (i = 0; i < (size - 1); i++) \
+			seq_printf(m, "%d.", var[i]); \
+		seq_printf(m, "%d\n", var[i]);	      \
+	} while (0)
 
 #define FTNET_PROC init_net.proc_net
 
@@ -200,7 +203,7 @@ int ft1000_init_proc(struct net_device *dev)
 
 	info->ft1000_proc_dir = proc_mkdir(FT1000_PROC_DIR, FTNET_PROC);
 	if (info->ft1000_proc_dir == NULL) {
-		printk(KERN_WARNING "Unable to create %s dir.\n",
+		netdev_warn(dev, "Unable to create %s dir.\n",
 			FT1000_PROC_DIR);
 		goto fail;
 	}
@@ -210,7 +213,7 @@ int ft1000_init_proc(struct net_device *dev)
 				 &ft1000_proc_fops, dev);
 
 	if (!ft1000_proc_file) {
-		printk(KERN_WARNING "Unable to create /proc entry.\n");
+		netdev_warn(dev, "Unable to create /proc entry.\n");
 		goto fail_entry;
 	}
 
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c
index e40763e60db..0a2544c7829 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c
@@ -35,7 +35,7 @@ static struct usb_device_id id_table[] = {
 
 MODULE_DEVICE_TABLE(usb, id_table);
 
-static bool gPollingfailed = false;
+static bool gPollingfailed;
 static int ft1000_poll_thread(void *arg)
 {
 	int ret;
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.h b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.h
index a6fdd524ee6..2d4b02e2382 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.h
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.h
@@ -14,17 +14,21 @@
 struct app_info_block {
 	u32 nTxMsg;                    /* DPRAM msg sent to DSP with app_id */
 	u32 nRxMsg;                    /* DPRAM msg rcv from dsp with app_id */
-	u32 nTxMsgReject;              /* DPRAM msg rejected due to DSP doorbell set */
+	u32 nTxMsgReject;              /* DPRAM msg rejected due to DSP doorbell
+					* set
+					*/
 	u32 nRxMsgMiss;                /* DPRAM msg dropped due to overflow */
 	struct fown_struct *fileobject;/* Application's file object */
 	u16 app_id;                    /* Application id */
 	int DspBCMsgFlag;
 	int NumOfMsg;                   /* number of messages queued up */
 	wait_queue_head_t wait_dpram_msg;
-	struct list_head app_sqlist;   /* link list of msgs for applicaton on slow queue */
+	struct list_head app_sqlist;   /* link list of msgs for applicaton on
+					* slow queue
+					*/
 } __packed;
 
-#define DEBUG(args...) printk(KERN_INFO args)
+#define DEBUG(args...) pr_info(args)
 
 #define FALSE           0
 #define TRUE            1
@@ -41,7 +45,9 @@ struct app_info_block {
 /* MEMORY MAP FOR MAGNEMITE */
 /* the indexes are swapped comparing to PCMCIA - is it OK or a bug? */
 #undef FT1000_MAG_DSP_LED_INDX
-#define FT1000_MAG_DSP_LED_INDX		0x1	/* dsp led status for PAD device */
+#define FT1000_MAG_DSP_LED_INDX		0x1	/* dsp led status for PAD
+						 * device
+						 */
 #undef FT1000_MAG_DSP_CON_STATE_INDX
 #define FT1000_MAG_DSP_CON_STATE_INDX	0x0	/* DSP Connection Status Info */
 
@@ -99,21 +105,21 @@ struct dpram_blk {
 } __packed;
 
 int ft1000_read_register(struct ft1000_usb *ft1000dev,
-			u16 *Data, u16 nRegIndx);
+			 u16 *Data, u16 nRegIndx);
 int ft1000_write_register(struct ft1000_usb *ft1000dev,
-			u16 value, u16 nRegIndx);
+			  u16 value, u16 nRegIndx);
 int ft1000_read_dpram32(struct ft1000_usb *ft1000dev,
 			u16 indx, u8 *buffer, u16 cnt);
 int ft1000_write_dpram32(struct ft1000_usb *ft1000dev,
-			u16 indx, u8 *buffer, u16 cnt);
+			 u16 indx, u8 *buffer, u16 cnt);
 int ft1000_read_dpram16(struct ft1000_usb *ft1000dev,
 			u16 indx, u8 *buffer, u8 highlow);
 int ft1000_write_dpram16(struct ft1000_usb *ft1000dev,
-			u16 indx, u16 value, u8 highlow);
+			 u16 indx, u16 value, u8 highlow);
 int fix_ft1000_read_dpram32(struct ft1000_usb *ft1000dev,
-			u16 indx, u8 *buffer);
+			    u16 indx, u8 *buffer);
 int fix_ft1000_write_dpram32(struct ft1000_usb *ft1000dev,
-			u16 indx, u8 *buffer);
+			     u16 indx, u8 *buffer);
 
 extern void *pFileStart;
 extern size_t FileLength;
@@ -125,12 +131,13 @@ int scram_dnldr(struct ft1000_usb *ft1000dev, void *pFileStart,
 
 extern struct list_head freercvpool;
 
-extern spinlock_t free_buff_lock;   /* lock to arbitrate free buffer list for receive command data */
+/* lock to arbitrate free buffer list for receive command data */
+extern spinlock_t free_buff_lock;
 
 int ft1000_create_dev(struct ft1000_usb *dev);
 void ft1000_destroy_dev(struct net_device *dev);
 extern void card_send_command(struct ft1000_usb *ft1000dev,
-				void *ptempbuffer, int size);
+			      void *ptempbuffer, int size);
 
 struct dpram_blk *ft1000_get_buffer(struct list_head *bufflist);
 void ft1000_free_buffer(struct dpram_blk *pdpram_blk, struct list_head *plist);
@@ -139,12 +146,10 @@ int dsp_reload(struct ft1000_usb *ft1000dev);
 int init_ft1000_netdev(struct ft1000_usb *ft1000dev);
 struct usb_interface;
 int reg_ft1000_netdev(struct ft1000_usb *ft1000dev,
-			struct usb_interface *intf);
+		      struct usb_interface *intf);
 int ft1000_poll(void *dev_id);
 
 int ft1000_init_proc(struct net_device *dev);
 void ft1000_cleanup_proc(struct ft1000_info *info);
 
-
-
-#endif
+#endif  /* _FT1000_USB_H_ */
diff --git a/drivers/staging/ft1000/ft1000.h b/drivers/staging/ft1000/ft1000.h
index ccb821a1f37..db57430f3b9 100644
--- a/drivers/staging/ft1000/ft1000.h
+++ b/drivers/staging/ft1000/ft1000.h
@@ -1,5 +1,6 @@
 /*
- * Common structures and definitions for FT1000 Flarion Flash OFDM PCMCIA and USB devices
+ * Common structures and definitions for FT1000 Flarion Flash OFDM PCMCIA and
+ * USB devices.
  *
  * Originally copyright (c) 2002 Flarion Technologies
  *
@@ -17,7 +18,9 @@
 #define MAGNEMITE_ID	0x1a01	/* ASIC ID for Magnemite */
 
 /* MEMORY MAP common to both ELECTRABUZZ and MAGNEMITE */
-#define	FT1000_REG_DPRAM_ADDR	0x000E	/* DPADR - Dual Port Ram Indirect Address Register */
+#define	FT1000_REG_DPRAM_ADDR	0x000E	/* DPADR - Dual Port Ram Indirect
+					 * Address Register
+					 */
 #define	FT1000_REG_SUP_CTRL	0x0020	/* HCTR - Host Control Register */
 #define	FT1000_REG_SUP_STAT	0x0022	/* HSTAT - Host Status Register */
 #define	FT1000_REG_RESET	0x0024	/* HCTR - Host Control Register */
diff --git a/drivers/staging/fwserial/dma_fifo.c b/drivers/staging/fwserial/dma_fifo.c
index 5e846344550..02790624959 100644
--- a/drivers/staging/fwserial/dma_fifo.c
+++ b/drivers/staging/fwserial/dma_fifo.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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  */
 
 #include <linux/kernel.h>
@@ -169,9 +165,9 @@ int dma_fifo_in(struct dma_fifo *fifo, const void *src, int n)
 	memcpy(fifo->data, src + l, n - l);
 
 	if (FAIL(fifo, addr_check(fifo->done, fifo->in, fifo->in + n) ||
-			fifo->avail < n,
-			"fifo corrupt: in:%u out:%u done:%u n:%d avail:%d",
-			fifo->in, fifo->out, fifo->done, n, fifo->avail))
+		 fifo->avail < n,
+		 "fifo corrupt: in:%u out:%u done:%u n:%d avail:%d",
+		 fifo->in, fifo->out, fifo->done, n, fifo->avail))
 		return -ENXIO;
 
 	fifo->in += n;
@@ -236,12 +232,12 @@ int dma_fifo_out_pend(struct dma_fifo *fifo, struct dma_pending *pended)
 	++fifo->open;
 
 	if (FAIL(fifo, fifo->open > fifo->open_limit,
-			"past open limit:%d (limit:%d)",
-			fifo->open, fifo->open_limit))
+		 "past open limit:%d (limit:%d)",
+		 fifo->open, fifo->open_limit))
 		return -ENXIO;
 	if (FAIL(fifo, fifo->out & (fifo->align - 1),
-			"fifo out unaligned:%u (align:%u)",
-			fifo->out, fifo->align))
+		 "fifo out unaligned:%u (align:%u)",
+		 fifo->out, fifo->align))
 		return -ENXIO;
 
 	return len - n;
@@ -264,8 +260,8 @@ int dma_fifo_out_complete(struct dma_fifo *fifo, struct dma_pending *complete)
 		return -EINVAL;
 
 	if (FAIL(fifo, list_empty(&fifo->pending) != (fifo->open == 0),
-			"pending list disagrees with open count:%d",
-			fifo->open))
+		 "pending list disagrees with open count:%d",
+		 fifo->open))
 		return -ENXIO;
 
 	tmp = complete->data;
@@ -282,10 +278,10 @@ int dma_fifo_out_complete(struct dma_fifo *fifo, struct dma_pending *complete)
 		}
 
 		if (FAIL(fifo, pending->out != fifo->done ||
-				addr_check(fifo->in, fifo->done, pending->next),
-				"in:%u out:%u done:%u saved:%u next:%u",
-				fifo->in, fifo->out, fifo->done, pending->out,
-				pending->next))
+			 addr_check(fifo->in, fifo->done, pending->next),
+			 "in:%u out:%u done:%u saved:%u next:%u",
+			 fifo->in, fifo->out, fifo->done, pending->out,
+			 pending->next))
 			return -ENXIO;
 
 		list_del_init(&pending->link);
@@ -300,7 +296,7 @@ int dma_fifo_out_complete(struct dma_fifo *fifo, struct dma_pending *complete)
 	if (FAIL(fifo, fifo->open < 0, "open dma:%d < 0", fifo->open))
 		return -ENXIO;
 	if (FAIL(fifo, fifo->avail > fifo->size, "fifo avail:%d > size:%d",
-			fifo->avail, fifo->size))
+		 fifo->avail, fifo->size))
 		return -ENXIO;
 
 	return 0;
diff --git a/drivers/staging/fwserial/dma_fifo.h b/drivers/staging/fwserial/dma_fifo.h
index a113fe1e6f1..410988224f8 100644
--- a/drivers/staging/fwserial/dma_fifo.h
+++ b/drivers/staging/fwserial/dma_fifo.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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  */
 
 #ifndef _DMA_FIFO_H_
@@ -85,15 +81,15 @@ static inline bool dp_is_completed(struct dma_pending *dp)
 	return (unsigned long)dp->data & 1UL;
 }
 
-extern void dma_fifo_init(struct dma_fifo *fifo);
-extern int dma_fifo_alloc(struct dma_fifo *fifo, int size, unsigned align,
-			  int tx_limit, int open_limit, gfp_t gfp_mask);
-extern void dma_fifo_free(struct dma_fifo *fifo);
-extern void dma_fifo_reset(struct dma_fifo *fifo);
-extern int dma_fifo_in(struct dma_fifo *fifo, const void *src, int n);
-extern int dma_fifo_out_pend(struct dma_fifo *fifo, struct dma_pending *pended);
-extern int dma_fifo_out_complete(struct dma_fifo *fifo,
-				 struct dma_pending *complete);
+void dma_fifo_init(struct dma_fifo *fifo);
+int dma_fifo_alloc(struct dma_fifo *fifo, int size, unsigned align,
+		   int tx_limit, int open_limit, gfp_t gfp_mask);
+void dma_fifo_free(struct dma_fifo *fifo);
+void dma_fifo_reset(struct dma_fifo *fifo);
+int dma_fifo_in(struct dma_fifo *fifo, const void *src, int n);
+int dma_fifo_out_pend(struct dma_fifo *fifo, struct dma_pending *pended);
+int dma_fifo_out_complete(struct dma_fifo *fifo,
+			  struct dma_pending *complete);
 
 /* returns the # of used bytes in the fifo */
 static inline int dma_fifo_level(struct dma_fifo *fifo)
diff --git a/drivers/staging/fwserial/fwserial.c b/drivers/staging/fwserial/fwserial.c
index b22142ee526..384758b11e3 100644
--- a/drivers/staging/fwserial/fwserial.c
+++ b/drivers/staging/fwserial/fwserial.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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  */
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
@@ -530,7 +526,7 @@ static void fwtty_emit_breaks(struct work_struct *work)
 	while (n) {
 		t = min(n, 16);
 		c = tty_insert_flip_string_fixed_flag(&port->port, buf,
-				TTY_BREAK, t);
+						      TTY_BREAK, t);
 		n -= c;
 		brk += c;
 		if (c < t)
@@ -638,9 +634,9 @@ static void fwtty_port_handler(struct fw_card *card,
 
 	switch (tcode) {
 	case TCODE_WRITE_QUADLET_REQUEST:
-		if (addr != port->rx_handler.offset || len != 4)
+		if (addr != port->rx_handler.offset || len != 4) {
 			rcode = RCODE_ADDRESS_ERROR;
-		else {
+		} else {
 			fwtty_update_port_status(port, *(unsigned *)data);
 			rcode = RCODE_COMPLETE;
 		}
@@ -741,7 +737,7 @@ static int fwtty_tx(struct fwtty_port *port, bool drain)
 	/* try to write as many dma transactions out as possible */
 	n = -EAGAIN;
 	while (!tty->stopped && !tty->hw_stopped &&
-			!test_bit(STOP_TX, &port->flags)) {
+	       !test_bit(STOP_TX, &port->flags)) {
 		txn = kmem_cache_alloc(fwtty_txn_cache, GFP_ATOMIC);
 		if (!txn) {
 			n = -ENOMEM;
@@ -756,11 +752,11 @@ static int fwtty_tx(struct fwtty_port *port, bool drain)
 
 		if (n < 0) {
 			kmem_cache_free(fwtty_txn_cache, txn);
-			if (n == -EAGAIN)
+			if (n == -EAGAIN) {
 				++port->stats.tx_stall;
-			else if (n == -ENODATA)
+			} else if (n == -ENODATA) {
 				fwtty_profile_data(port->stats.txns, 0);
-			else {
+			} else {
 				++port->stats.fifo_errs;
 				fwtty_err_ratelimited(port, "fifo err: %d\n",
 						      n);
@@ -884,7 +880,7 @@ static void fwserial_destroy(struct kref *kref)
 	for (j = 0; j < num_ports; ++i, ++j) {
 		port_table_corrupt |= port_table[i] != ports[j];
 		WARN_ONCE(port_table_corrupt, "port_table[%d]: %p != ports[%d]: %p",
-		     i, port_table[i], j, ports[j]);
+			  i, port_table[i], j, ports[j]);
 
 		port_table[i] = NULL;
 	}
@@ -1257,15 +1253,16 @@ static int set_serial_info(struct fwtty_port *port,
 		return -EFAULT;
 
 	if (tmp.irq != 0 || tmp.port != 0 || tmp.custom_divisor != 0 ||
-			tmp.baud_base != 400000000)
+	    tmp.baud_base != 400000000)
 		return -EPERM;
 
 	if (!capable(CAP_SYS_ADMIN)) {
 		if (((tmp.flags & ~ASYNC_USR_MASK) !=
 		     (port->port.flags & ~ASYNC_USR_MASK)))
 			return -EPERM;
-	} else
+	} else {
 		port->port.close_delay = tmp.close_delay * HZ / 100;
+	}
 
 	return 0;
 }
@@ -1308,9 +1305,9 @@ static void fwtty_set_termios(struct tty_struct *tty, struct ktermios *old)
 	spin_lock_bh(&port->lock);
 	baud = set_termios(port, tty);
 
-	if ((baud == 0) && (old->c_cflag & CBAUD))
+	if ((baud == 0) && (old->c_cflag & CBAUD)) {
 		port->mctrl &= ~(TIOCM_DTR | TIOCM_RTS);
-	else if ((baud != 0) && !(old->c_cflag & CBAUD)) {
+	} else if ((baud != 0) && !(old->c_cflag & CBAUD)) {
 		if (C_CRTSCTS(tty) || !test_bit(TTY_THROTTLED, &tty->flags))
 			port->mctrl |= TIOCM_DTR | TIOCM_RTS;
 		else
@@ -1733,8 +1730,9 @@ static inline int fwserial_send_mgmt_sync(struct fwtty_peer *peer,
 		    rcode == RCODE_GENERATION) {
 			fwtty_dbg(&peer->unit, "mgmt write error: %d\n", rcode);
 			continue;
-		} else
+		} else {
 			break;
+		}
 	} while (--tries > 0);
 	return rcode;
 }
@@ -1809,7 +1807,7 @@ static void fwserial_release_port(struct fwtty_port *port, bool reset)
 	port->max_payload = link_speed_to_max_payload(SCODE_100);
 	dma_fifo_change_tx_limit(&port->tx_fifo, port->max_payload);
 
-	rcu_assign_pointer(port->peer, NULL);
+	RCU_INIT_POINTER(port->peer, NULL);
 	spin_unlock_bh(&port->lock);
 
 	if (port->port.console && port->fwcon_ops->notify != NULL)
@@ -1818,7 +1816,7 @@ static void fwserial_release_port(struct fwtty_port *port, bool reset)
 
 static void fwserial_plug_timeout(unsigned long data)
 {
-	struct fwtty_peer *peer = (struct fwtty_peer *) data;
+	struct fwtty_peer *peer = (struct fwtty_peer *)data;
 	struct fwtty_port *port;
 
 	spin_lock_bh(&peer->lock);
@@ -2242,7 +2240,7 @@ static int fwserial_create(struct fw_unit *unit)
 		port->max_payload = link_speed_to_max_payload(SCODE_100);
 		dma_fifo_init(&port->tx_fifo);
 
-		rcu_assign_pointer(port->peer, NULL);
+		RCU_INIT_POINTER(port->peer, NULL);
 		serial->ports[i] = port;
 
 		/* get unique bus addr region for port's status & recv fifo */
@@ -2744,9 +2742,9 @@ static int fwserial_parse_mgmt_write(struct fwtty_peer *peer,
 		break;
 
 	case FWSC_VIRT_CABLE_UNPLUG_RSP:
-		if (peer->state != FWPS_UNPLUG_PENDING)
+		if (peer->state != FWPS_UNPLUG_PENDING) {
 			rcode = RCODE_CONFLICT_ERROR;
-		else {
+		} else {
 			if (be16_to_cpu(pkt->hdr.code) & FWSC_RSP_NACK)
 				fwtty_notice(&peer->unit, "NACK unplug?\n");
 			port = peer_revert_state(peer);
diff --git a/drivers/staging/gdm72xx/TODO b/drivers/staging/gdm72xx/TODO
index 5ab27fb2959..62d0cd6225c 100644
--- a/drivers/staging/gdm72xx/TODO
+++ b/drivers/staging/gdm72xx/TODO
@@ -1,3 +1,2 @@
 TODO:
-- Replace kernel_thread with kthread in gdm_usb.c
 - Clean up coding style to meet kernel standard.
diff --git a/drivers/staging/gdm72xx/gdm_qos.c b/drivers/staging/gdm72xx/gdm_qos.c
index 50d43ada093..df6f000534d 100644
--- a/drivers/staging/gdm72xx/gdm_qos.c
+++ b/drivers/staging/gdm72xx/gdm_qos.c
@@ -24,7 +24,7 @@
 #include "hci.h"
 #include "gdm_qos.h"
 
-#define B2H(x)		__be16_to_cpu(x)
+#define B2H(x)	__be16_to_cpu(x)
 
 #define MAX_FREE_LIST_CNT		32
 static struct {
@@ -48,7 +48,7 @@ static void *alloc_qos_entry(void)
 	spin_lock_irqsave(&qos_free_list.lock, flags);
 	if (qos_free_list.cnt) {
 		entry = list_entry(qos_free_list.head.prev, struct qos_entry_s,
-					list);
+				   list);
 		list_del(&entry->list);
 		qos_free_list.cnt--;
 		spin_unlock_irqrestore(&qos_free_list.lock, flags);
@@ -56,13 +56,13 @@ static void *alloc_qos_entry(void)
 	}
 	spin_unlock_irqrestore(&qos_free_list.lock, flags);
 
-	entry = kmalloc(sizeof(struct qos_entry_s), GFP_ATOMIC);
+	entry = kmalloc(sizeof(*entry), GFP_ATOMIC);
 	return entry;
 }
 
 static void free_qos_entry(void *entry)
 {
-	struct qos_entry_s *qentry = (struct qos_entry_s *) entry;
+	struct qos_entry_s *qentry = (struct qos_entry_s *)entry;
 	unsigned long flags;
 
 	spin_lock_irqsave(&qos_free_list.lock, flags);
@@ -142,24 +142,24 @@ void gdm_qos_release_list(void *nic_ptr)
 	free_qos_entry_list(&free_list);
 }
 
-static u32 chk_ipv4_rule(struct gdm_wimax_csr_s *csr, u8 *Stream, u8 *port)
+static u32 chk_ipv4_rule(struct gdm_wimax_csr_s *csr, u8 *stream, u8 *port)
 {
 	int i;
 
 	if (csr->classifier_rule_en&IPTYPEOFSERVICE) {
-		if (((Stream[1] & csr->ip2s_mask) < csr->ip2s_lo) ||
-		((Stream[1] & csr->ip2s_mask) > csr->ip2s_hi))
+		if (((stream[1] & csr->ip2s_mask) < csr->ip2s_lo) ||
+		    ((stream[1] & csr->ip2s_mask) > csr->ip2s_hi))
 			return 1;
 	}
 
 	if (csr->classifier_rule_en&PROTOCOL) {
-		if (Stream[9] != csr->protocol)
+		if (stream[9] != csr->protocol)
 			return 1;
 	}
 
 	if (csr->classifier_rule_en&IPMASKEDSRCADDRESS) {
 		for (i = 0; i < 4; i++) {
-			if ((Stream[12 + i] & csr->ipsrc_addrmask[i]) !=
+			if ((stream[12 + i] & csr->ipsrc_addrmask[i]) !=
 			(csr->ipsrc_addr[i] & csr->ipsrc_addrmask[i]))
 				return 1;
 		}
@@ -167,7 +167,7 @@ static u32 chk_ipv4_rule(struct gdm_wimax_csr_s *csr, u8 *Stream, u8 *port)
 
 	if (csr->classifier_rule_en&IPMASKEDDSTADDRESS) {
 		for (i = 0; i < 4; i++) {
-			if ((Stream[16 + i] & csr->ipdst_addrmask[i]) !=
+			if ((stream[16 + i] & csr->ipdst_addrmask[i]) !=
 			(csr->ipdst_addr[i] & csr->ipdst_addrmask[i]))
 				return 1;
 		}
@@ -190,25 +190,24 @@ static u32 chk_ipv4_rule(struct gdm_wimax_csr_s *csr, u8 *Stream, u8 *port)
 
 static u32 get_qos_index(struct nic *nic, u8 *iph, u8 *tcpudph)
 {
-	u32	IP_Ver, Header_Len, i;
+	u32 IP_ver, i;
 	struct qos_cb_s *qcb = &nic->qos;
 
 	if (iph == NULL || tcpudph == NULL)
 		return -1;
 
-	IP_Ver = (iph[0]>>4)&0xf;
-	Header_Len = iph[0]&0xf;
+	IP_ver = (iph[0]>>4)&0xf;
 
-	if (IP_Ver == 4) {
-		for (i = 0; i < QOS_MAX; i++) {
-			if (qcb->csr[i].enabled) {
-				if (qcb->csr[i].classifier_rule_en) {
-					if (chk_ipv4_rule(&qcb->csr[i], iph,
-					tcpudph) == 0)
-						return i;
-				}
-			}
-		}
+	if (IP_ver != 4)
+		return -1;
+
+	for (i = 0; i < QOS_MAX; i++) {
+		if (!qcb->csr[i].enabled)
+			continue;
+		if (!qcb->csr[i].classifier_rule_en)
+			continue;
+		if (chk_ipv4_rule(&qcb->csr[i], iph, tcpudph) == 0)
+			return i;
 	}
 
 	return -1;
@@ -223,22 +222,21 @@ static u32 extract_qos_list(struct nic *nic, struct list_head *head)
 	INIT_LIST_HEAD(head);
 
 	for (i = 0; i < QOS_MAX; i++) {
-		if (qcb->csr[i].enabled) {
-			if (qcb->csr[i].qos_buf_count < qcb->qos_limit_size) {
-				if (!list_empty(&qcb->qos_list[i])) {
-					entry = list_entry(
-					qcb->qos_list[i].prev,
-					struct qos_entry_s, list);
-					list_move_tail(&entry->list, head);
-					qcb->csr[i].qos_buf_count++;
-
-					if (!list_empty(&qcb->qos_list[i]))
-						netdev_warn(nic->netdev,
-							    "Index(%d) is piled!!\n",
-							    i);
-				}
-			}
-		}
+		if (!qcb->csr[i].enabled)
+			continue;
+		if (qcb->csr[i].qos_buf_count >= qcb->qos_limit_size)
+			continue;
+		if (list_empty(&qcb->qos_list[i]))
+			continue;
+
+		entry = list_entry(qcb->qos_list[i].prev, struct qos_entry_s,
+				   list);
+
+		list_move_tail(&entry->list, head);
+		qcb->csr[i].qos_buf_count++;
+
+		if (!list_empty(&qcb->qos_list[i]))
+			netdev_warn(nic->netdev, "Index(%d) is piled!!\n", i);
 	}
 
 	return 0;
@@ -261,14 +259,14 @@ int gdm_qos_send_hci_pkt(struct sk_buff *skb, struct net_device *dev)
 	int index;
 	struct qos_cb_s *qcb = &nic->qos;
 	unsigned long flags;
-	struct ethhdr *ethh = (struct ethhdr *) (skb->data + HCI_HEADER_SIZE);
-	struct iphdr *iph = (struct iphdr *) ((char *) ethh + ETH_HLEN);
+	struct ethhdr *ethh = (struct ethhdr *)(skb->data + HCI_HEADER_SIZE);
+	struct iphdr *iph = (struct iphdr *)((char *)ethh + ETH_HLEN);
 	struct tcphdr *tcph;
 	struct qos_entry_s *entry = NULL;
 	struct list_head send_list;
 	int ret = 0;
 
-	tcph = (struct tcphdr *) iph + iph->ihl*4;
+	tcph = (struct tcphdr *)iph + iph->ihl*4;
 
 	if (B2H(ethh->h_proto) == ETH_P_IP) {
 		if (qcb->qos_list_cnt && !qos_free_list.cnt) {
@@ -281,7 +279,7 @@ int gdm_qos_send_hci_pkt(struct sk_buff *skb, struct net_device *dev)
 
 		spin_lock_irqsave(&qcb->qos_lock, flags);
 		if (qcb->qos_list_cnt) {
-			index = get_qos_index(nic, (u8 *)iph, (u8 *) tcph);
+			index = get_qos_index(nic, (u8 *)iph, (u8 *)tcph);
 			if (index == -1)
 				index = qcb->qos_null_idx;
 
@@ -336,16 +334,16 @@ void gdm_recv_qos_hci_packet(void *nic_ptr, u8 *buf, int size)
 {
 	struct nic *nic = nic_ptr;
 	u32 i, SFID, index, pos;
-	u8 subCmdEvt;
+	u8 sub_cmd_evt;
 	struct qos_cb_s *qcb = &nic->qos;
 	struct qos_entry_s *entry, *n;
 	struct list_head send_list;
 	struct list_head free_list;
 	unsigned long flags;
 
-	subCmdEvt = (u8)buf[4];
+	sub_cmd_evt = (u8)buf[4];
 
-	if (subCmdEvt == QOS_REPORT) {
+	if (sub_cmd_evt == QOS_REPORT) {
 		spin_lock_irqsave(&qcb->qos_lock, flags);
 		for (i = 0; i < qcb->qos_list_cnt; i++) {
 			SFID = ((buf[(i*5)+6]<<24)&0xff000000);
@@ -367,7 +365,7 @@ void gdm_recv_qos_hci_packet(void *nic_ptr, u8 *buf, int size)
 		return;
 	}
 
-	/* subCmdEvt == QOS_ADD || subCmdEvt == QOS_CHANG_DEL */
+	/* sub_cmd_evt == QOS_ADD || sub_cmd_evt == QOS_CHANG_DEL */
 	pos = 6;
 	SFID = ((buf[pos++]<<24)&0xff000000);
 	SFID += ((buf[pos++]<<16)&0xff0000);
@@ -377,12 +375,12 @@ void gdm_recv_qos_hci_packet(void *nic_ptr, u8 *buf, int size)
 	index = get_csr(qcb, SFID, 1);
 	if (index == -1) {
 		netdev_err(nic->netdev,
-			   "QoS ERROR: csr Update Error / Wrong index (%d) \n",
+			   "QoS ERROR: csr Update Error / Wrong index (%d)\n",
 			   index);
 		return;
 	}
 
-	if (subCmdEvt == QOS_ADD) {
+	if (sub_cmd_evt == QOS_ADD) {
 		netdev_dbg(nic->netdev, "QOS_ADD SFID = 0x%x, index=%d\n",
 			   SFID, index);
 
@@ -423,7 +421,7 @@ void gdm_recv_qos_hci_packet(void *nic_ptr, u8 *buf, int size)
 
 		qcb->qos_limit_size = 254/qcb->qos_list_cnt;
 		spin_unlock_irqrestore(&qcb->qos_lock, flags);
-	} else if (subCmdEvt == QOS_CHANGE_DEL) {
+	} else if (sub_cmd_evt == QOS_CHANGE_DEL) {
 		netdev_dbg(nic->netdev, "QOS_CHANGE_DEL SFID = 0x%x, index=%d\n",
 			   SFID, index);
 
@@ -435,7 +433,7 @@ void gdm_recv_qos_hci_packet(void *nic_ptr, u8 *buf, int size)
 		qcb->qos_limit_size = 254/qcb->qos_list_cnt;
 
 		list_for_each_entry_safe(entry, n, &qcb->qos_list[index],
-					list) {
+					 list) {
 			list_move_tail(&entry->list, &free_list);
 		}
 		spin_unlock_irqrestore(&qcb->qos_lock, flags);
diff --git a/drivers/staging/gdm72xx/gdm_qos.h b/drivers/staging/gdm72xx/gdm_qos.h
index 8f18119d22a..6543cff2a87 100644
--- a/drivers/staging/gdm72xx/gdm_qos.h
+++ b/drivers/staging/gdm72xx/gdm_qos.h
@@ -34,23 +34,6 @@
 #define	IEEE802_1QVLANID		0x10
 
 struct gdm_wimax_csr_s {
-	/*	union{
-		U16 all;
-		struct _CS_CLASSIFIER_RULE_ENABLE{
-			IPTypeOfService:1,
-			Protocol:1,
-			IPMaskedSrcAddress:1,
-			IPMaskedDstAddress:1,
-			ProtocolSrcPortRange:1,
-			ProtocolDstPortRange:1,
-			DstMacAddr:1,
-			SrcMacAddr:1,
-			Ethertype:1,
-			IEEE802_1DUserPriority:1,
-			IEEE802_1QVLANID:1,
-			Reserved:5;
-		} fields;
-	} */
 	BOOLEAN		enabled;
 	u32		SFID;
 	u8		qos_buf_count;
diff --git a/drivers/staging/gdm72xx/gdm_sdio.c b/drivers/staging/gdm72xx/gdm_sdio.c
index c24653739e1..7398d451ccc 100644
--- a/drivers/staging/gdm72xx/gdm_sdio.c
+++ b/drivers/staging/gdm72xx/gdm_sdio.c
@@ -31,11 +31,11 @@
 #define MAX_NR_RX_BUF	4
 
 #define SDU_TX_BUF_SIZE	2048
-#define TX_BUF_SIZE		2048
+#define TX_BUF_SIZE	2048
 #define TX_CHUNK_SIZE	(2048 - TYPE_A_HEADER_SIZE)
-#define RX_BUF_SIZE		(25*1024)
+#define RX_BUF_SIZE	(25*1024)
 
-#define TX_HZ	2000
+#define TX_HZ		2000
 #define TX_INTERVAL	(1000000/TX_HZ)
 
 static int init_sdio(struct sdiowm_dev *sdev);
@@ -127,10 +127,10 @@ static void put_rx_struct(struct rx_cxt *rx, struct sdio_rx *r)
 static int init_sdio(struct sdiowm_dev *sdev)
 {
 	int ret = 0, i;
-	struct tx_cxt	*tx = &sdev->tx;
-	struct rx_cxt	*rx = &sdev->rx;
-	struct sdio_tx	*t;
-	struct sdio_rx	*r;
+	struct tx_cxt *tx = &sdev->tx;
+	struct rx_cxt *rx = &sdev->rx;
+	struct sdio_tx *t;
+	struct sdio_rx *r;
 
 	INIT_LIST_HEAD(&tx->free_list);
 	INIT_LIST_HEAD(&tx->sdu_list);
@@ -313,7 +313,7 @@ static void send_sdu(struct sdio_func *func, struct tx_cxt *tx)
 }
 
 static void send_hci(struct sdio_func *func, struct tx_cxt *tx,
-			struct sdio_tx *t)
+		     struct sdio_tx *t)
 {
 	unsigned long flags;
 
@@ -380,7 +380,7 @@ static void do_tx(struct work_struct *work)
 }
 
 static int gdm_sdio_send(void *priv_dev, void *data, int len,
-			void (*cb)(void *data), void *cb_data)
+			 void (*cb)(void *data), void *cb_data)
 {
 	struct sdiowm_dev *sdev = priv_dev;
 	struct tx_cxt *tx = &sdev->tx;
@@ -510,6 +510,7 @@ static void gdm_sdio_irq(struct sdio_func *func)
 
 	if (hdr[3] == 1) {	/* Ack */
 		u32 *ack_seq = (u32 *)&hdr[4];
+
 		spin_lock_irqsave(&tx->lock, flags);
 		tx->can_send = 1;
 
@@ -521,7 +522,7 @@ static void gdm_sdio_irq(struct sdio_func *func)
 	}
 
 	memcpy(rx->rx_buf, hdr + TYPE_A_HEADER_SIZE,
-			TYPE_A_LOOKAHEAD_SIZE - TYPE_A_HEADER_SIZE);
+	       TYPE_A_LOOKAHEAD_SIZE - TYPE_A_HEADER_SIZE);
 
 	buf = rx->rx_buf + TYPE_A_LOOKAHEAD_SIZE - TYPE_A_HEADER_SIZE;
 	remain = len - TYPE_A_LOOKAHEAD_SIZE + TYPE_A_HEADER_SIZE;
@@ -577,8 +578,8 @@ done:
 }
 
 static int gdm_sdio_receive(void *priv_dev,
-				void (*cb)(void *cb_data, void *data, int len),
-				void *cb_data)
+			    void (*cb)(void *cb_data, void *data, int len),
+			    void *cb_data)
 {
 	struct sdiowm_dev *sdev = priv_dev;
 	struct rx_cxt *rx = &sdev->rx;
@@ -602,7 +603,7 @@ static int gdm_sdio_receive(void *priv_dev,
 }
 
 static int sdio_wimax_probe(struct sdio_func *func,
-				const struct sdio_device_id *id)
+			    const struct sdio_device_id *id)
 {
 	int ret;
 	struct phy_dev *phy_dev = NULL;
diff --git a/drivers/staging/gdm72xx/gdm_sdio.h b/drivers/staging/gdm72xx/gdm_sdio.h
index 216e98f31ba..0c0e2cbb727 100644
--- a/drivers/staging/gdm72xx/gdm_sdio.h
+++ b/drivers/staging/gdm72xx/gdm_sdio.h
@@ -22,10 +22,8 @@
 struct sdio_tx {
 	struct list_head	list;
 	struct tx_cxt		*tx_cxt;
-
-	u8	*buf;
-	int	len;
-
+	u8			*buf;
+	int			len;
 	void (*callback)(void *cb_data);
 	void *cb_data;
 };
@@ -35,18 +33,15 @@ struct tx_cxt {
 	struct list_head	sdu_list;
 	struct list_head	hci_list;
 	struct timeval		sdu_stamp;
-
-	u8	*sdu_buf;
-
-	spinlock_t			lock;
-	int	can_send;
-	int stop_sdu_tx;
+	u8			*sdu_buf;
+	spinlock_t		lock;
+	int			can_send;
+	int			stop_sdu_tx;
 };
 
 struct sdio_rx {
 	struct list_head	list;
 	struct rx_cxt		*rx_cxt;
-
 	void (*callback)(void *cb_data, void *data, int len);
 	void *cb_data;
 };
@@ -54,18 +49,14 @@ struct sdio_rx {
 struct rx_cxt {
 	struct list_head	free_list;
 	struct list_head	req_list;
-
-	u8		*rx_buf;
-
-	spinlock_t			lock;
+	u8			*rx_buf;
+	spinlock_t		lock;
 };
 
 struct sdiowm_dev {
 	struct sdio_func	*func;
-
-	struct tx_cxt	tx;
-	struct rx_cxt	rx;
-
+	struct tx_cxt		tx;
+	struct rx_cxt		rx;
 	struct work_struct	ws;
 };
 
diff --git a/drivers/staging/gdm72xx/gdm_usb.c b/drivers/staging/gdm72xx/gdm_usb.c
index 20539d80939..78d6667fa0d 100644
--- a/drivers/staging/gdm72xx/gdm_usb.c
+++ b/drivers/staging/gdm72xx/gdm_usb.c
@@ -27,6 +27,7 @@
 MODULE_DEVICE_TABLE(usb, id_table);
 
 #define TX_BUF_SIZE		2048
+
 #if defined(CONFIG_WIMAX_GDM72XX_WIMAX2)
 #define RX_BUF_SIZE		(128*1024)	/* For packet aggregation */
 #else
@@ -166,10 +167,10 @@ static void put_rx_struct(struct rx_cxt *rx, struct usb_rx *r)
 static int init_usb(struct usbwm_dev *udev)
 {
 	int ret = 0, i;
-	struct tx_cxt	*tx = &udev->tx;
-	struct rx_cxt	*rx = &udev->rx;
-	struct usb_tx	*t;
-	struct usb_rx	*r;
+	struct tx_cxt *tx = &udev->tx;
+	struct rx_cxt *rx = &udev->rx;
+	struct usb_tx *t;
+	struct usb_rx *r;
 	unsigned long flags;
 
 	INIT_LIST_HEAD(&tx->free_list);
@@ -215,10 +216,10 @@ fail:
 
 static void release_usb(struct usbwm_dev *udev)
 {
-	struct tx_cxt	*tx = &udev->tx;
-	struct rx_cxt	*rx = &udev->rx;
-	struct usb_tx	*t, *t_next;
-	struct usb_rx	*r, *r_next;
+	struct tx_cxt *tx = &udev->tx;
+	struct rx_cxt *rx = &udev->rx;
+	struct usb_tx *t, *t_next;
+	struct usb_rx *r, *r_next;
 	unsigned long flags;
 
 	spin_lock_irqsave(&tx->lock, flags);
@@ -344,16 +345,11 @@ static int gdm_usb_send(void *priv_dev, void *data, int len,
 	if ((len % 512) == 0)
 		len++;
 
-	usb_fill_bulk_urb(t->urb,
-			usbdev,
-			usb_sndbulkpipe(usbdev, 1),
-			t->buf,
-			len + padding,
-			gdm_usb_send_complete,
-			t);
+	usb_fill_bulk_urb(t->urb, usbdev, usb_sndbulkpipe(usbdev, 1), t->buf,
+			  len + padding, gdm_usb_send_complete, t);
 
-	print_hex_dump_debug("usb_send: ", DUMP_PREFIX_NONE, 16, 1,
-			     t->buf, len + padding, false);
+	print_hex_dump_debug("usb_send: ", DUMP_PREFIX_NONE, 16, 1, t->buf,
+			     len + padding, false);
 #ifdef CONFIG_WIMAX_GDM72XX_USB_PM
 	if (usbdev->state & USB_STATE_SUSPENDED) {
 		list_add_tail(&t->p_list, &tx->pending_list);
@@ -470,8 +466,8 @@ static void gdm_usb_rcv_complete(struct urb *urb)
 }
 
 static int gdm_usb_receive(void *priv_dev,
-			void (*cb)(void *cb_data, void *data, int len),
-			void *cb_data)
+			   void (*cb)(void *cb_data, void *data, int len),
+			   void *cb_data)
 {
 	struct usbwm_dev *udev = priv_dev;
 	struct usb_device *usbdev = udev->usbdev;
@@ -494,13 +490,8 @@ static int gdm_usb_receive(void *priv_dev,
 	r->callback = cb;
 	r->cb_data = cb_data;
 
-	usb_fill_bulk_urb(r->urb,
-			usbdev,
-			usb_rcvbulkpipe(usbdev, 0x82),
-			r->buf,
-			RX_BUF_SIZE,
-			gdm_usb_rcv_complete,
-			r);
+	usb_fill_bulk_urb(r->urb, usbdev, usb_rcvbulkpipe(usbdev, 0x82), r->buf,
+			  RX_BUF_SIZE, gdm_usb_rcv_complete, r);
 
 	return usb_submit_urb(r->urb, GFP_ATOMIC);
 }
@@ -518,8 +509,8 @@ static void do_pm_control(struct work_struct *work)
 		usb_autopm_put_interface(udev->intf);
 
 	spin_lock_irqsave(&tx->lock, flags);
-	if (!(udev->usbdev->state & USB_STATE_SUSPENDED)
-		&& (!list_empty(&tx->hci_list) || !list_empty(&tx->sdu_list))) {
+	if (!(udev->usbdev->state & USB_STATE_SUSPENDED) &&
+	    (!list_empty(&tx->hci_list) || !list_empty(&tx->sdu_list))) {
 		struct usb_tx *t, *temp;
 
 		list_for_each_entry_safe(t, temp, &tx->pending_list, p_list) {
@@ -537,7 +528,7 @@ static void do_pm_control(struct work_struct *work)
 #endif /* CONFIG_WIMAX_GDM72XX_USB_PM */
 
 static int gdm_usb_probe(struct usb_interface *intf,
-				const struct usb_device_id *id)
+			 const struct usb_device_id *id)
 {
 	int ret = 0;
 	u8 bConfigurationValue;
@@ -567,7 +558,7 @@ static int gdm_usb_probe(struct usb_interface *intf,
 
 	/* Support for EEPROM bootloader */
 	if (bConfigurationValue == DOWNLOAD_CONF_VALUE ||
-		idProduct & B_DOWNLOAD) {
+	    idProduct & B_DOWNLOAD) {
 		ret = usb_boot(usbdev, bcdDevice);
 		goto out;
 	}
@@ -639,8 +630,9 @@ static void gdm_usb_disconnect(struct usb_interface *intf)
 	idProduct = L2H(usbdev->descriptor.idProduct);
 
 	if (idProduct != EMERGENCY_PID &&
-			bConfigurationValue != DOWNLOAD_CONF_VALUE &&
-			(idProduct & B_DOWNLOAD) == 0) {
+	    bConfigurationValue != DOWNLOAD_CONF_VALUE &&
+	    (idProduct & B_DOWNLOAD) == 0) {
+
 		udev = phy_dev->priv_dev;
 		udev->usbdev = NULL;
 
@@ -730,7 +722,7 @@ static int k_mode_thread(void *arg)
 			spin_unlock_irqrestore(&k_lock, flags2);
 
 			expire = jiffies + K_WAIT_TIME;
-			while (jiffies < expire)
+			while (time_before(jiffies, expire))
 				schedule_timeout(K_WAIT_TIME);
 
 			spin_lock_irqsave(&rx->lock, flags);
@@ -743,7 +735,7 @@ static int k_mode_thread(void *arg)
 			spin_lock_irqsave(&tx->lock, flags);
 
 			list_for_each_entry_safe(t, temp, &tx->pending_list,
-						p_list) {
+						 p_list) {
 				list_del(&t->p_list);
 				ret = usb_submit_urb(t->urb, GFP_ATOMIC);
 
@@ -759,8 +751,8 @@ static int k_mode_thread(void *arg)
 			spin_lock_irqsave(&k_lock, flags2);
 		}
 		wait_event_interruptible_lock_irq(k_wait,
-						  !list_empty(&k_list) || k_mode_stop,
-						  k_lock);
+						  !list_empty(&k_list) ||
+						  k_mode_stop, k_lock);
 		spin_unlock_irqrestore(&k_lock, flags2);
 	}
 	return 0;
diff --git a/drivers/staging/gdm72xx/gdm_usb.h b/drivers/staging/gdm72xx/gdm_usb.h
index f2c54511bb9..30506529a8c 100644
--- a/drivers/staging/gdm72xx/gdm_usb.h
+++ b/drivers/staging/gdm72xx/gdm_usb.h
@@ -28,12 +28,10 @@ struct usb_tx {
 	struct list_head	p_list;
 #endif
 	struct tx_cxt		*tx_cxt;
-
 	struct urb		*urb;
 	u8			*buf;
-
 	void (*callback)(void *cb_data);
-	void *cb_data;
+	void			*cb_data;
 };
 
 struct tx_cxt {
@@ -43,17 +41,14 @@ struct tx_cxt {
 #if defined(CONFIG_WIMAX_GDM72XX_USB_PM) || defined(CONFIG_WIMAX_GDM72XX_K_MODE)
 	struct list_head	pending_list;
 #endif
-
 	spinlock_t		lock;
 };
 
 struct usb_rx {
 	struct list_head	list;
 	struct rx_cxt		*rx_cxt;
-
 	struct urb		*urb;
 	u8			*buf;
-
 	void (*callback)(void *cb_data, void *data, int len);
 	void *cb_data;
 };
@@ -75,11 +70,9 @@ struct usbwm_dev {
 	int bw_switch;
 	struct list_head	list;
 #endif
-
 	struct tx_cxt		tx;
 	struct rx_cxt		rx;
-
-	int padding;
+	int			padding;
 };
 
 #endif /* __GDM_USB_H__ */
diff --git a/drivers/staging/gdm72xx/gdm_wimax.c b/drivers/staging/gdm72xx/gdm_wimax.c
index 05ce2a22c22..e5e51158512 100644
--- a/drivers/staging/gdm72xx/gdm_wimax.c
+++ b/drivers/staging/gdm72xx/gdm_wimax.c
@@ -35,10 +35,10 @@
 #define EVT_MAX_SIZE	2048
 
 struct evt_entry {
-	struct list_head list;
-	struct net_device *dev;
-	char evt_data[EVT_MAX_SIZE];
-	int	 size;
+	struct	list_head list;
+	struct	net_device *dev;
+	char	evt_data[EVT_MAX_SIZE];
+	int	size;
 };
 
 static void __gdm_wimax_event_send(struct work_struct *work);
@@ -52,7 +52,6 @@ static struct {
 	struct sock *sock;
 	struct list_head evtq;
 	spinlock_t evt_lock;
-
 	struct list_head freeq;
 	struct work_struct ws;
 } wm_event;
@@ -135,24 +134,24 @@ static void dump_eth_packet(struct net_device *dev, const char *title,
 	u16 port = 0;
 
 	protocol = (data[12]<<8) | data[13];
-	ih = (struct iphdr *) (data+ETH_HLEN);
+	ih = (struct iphdr *)(data+ETH_HLEN);
 
 	if (protocol == ETH_P_IP) {
-		uh = (struct udphdr *) ((char *)ih + sizeof(struct iphdr));
+		uh = (struct udphdr *)((char *)ih + sizeof(struct iphdr));
 		ip_protocol = ih->protocol;
 		port = ntohs(uh->dest);
 	} else if (protocol == ETH_P_IPV6) {
-		struct ipv6hdr *i6h = (struct ipv6hdr *) data;
-		uh = (struct udphdr *) ((char *)i6h + sizeof(struct ipv6hdr));
+		struct ipv6hdr *i6h = (struct ipv6hdr *)data;
+
+		uh = (struct udphdr *)((char *)i6h + sizeof(struct ipv6hdr));
 		ip_protocol = i6h->nexthdr;
 		port = ntohs(uh->dest);
 	}
 
-	netdev_dbg(dev, "[%s] len=%d, %s, %s, %s\n",
-		title, len,
-		get_protocol_name(protocol),
-		get_ip_protocol_name(ip_protocol),
-		get_port_name(port));
+	netdev_dbg(dev, "[%s] len=%d, %s, %s, %s\n", title, len,
+		   get_protocol_name(protocol),
+		   get_ip_protocol_name(ip_protocol),
+		   get_port_name(port));
 
 	if (!(data[0] == 0xff && data[1] == 0xff)) {
 		if (protocol == ETH_P_IP)
@@ -168,7 +167,6 @@ static inline int gdm_wimax_header(struct sk_buff **pskb)
 {
 	u16 buf[HCI_HEADER_SIZE / sizeof(u16)];
 	struct sk_buff *skb = *pskb;
-	int ret = 0;
 
 	if (unlikely(skb_headroom(skb) < HCI_HEADER_SIZE)) {
 		struct sk_buff *skb2;
@@ -188,7 +186,7 @@ static inline int gdm_wimax_header(struct sk_buff **pskb)
 	memcpy(skb->data, buf, HCI_HEADER_SIZE);
 
 	*pskb = skb;
-	return ret;
+	return 0;
 }
 
 static void gdm_wimax_event_rcv(struct net_device *dev, u16 type, void *msg,
@@ -196,9 +194,10 @@ static void gdm_wimax_event_rcv(struct net_device *dev, u16 type, void *msg,
 {
 	struct nic *nic = netdev_priv(dev);
 
-	u8 *buf = (u8 *) msg;
+	u8 *buf = (u8 *)msg;
 	u16 hci_cmd =  (buf[0]<<8) | buf[1];
 	u16 hci_len = (buf[2]<<8) | buf[3];
+
 	netdev_dbg(dev, "H=>D: 0x%04x(%d)\n", hci_cmd, hci_len);
 
 	gdm_wimax_send(nic, msg, len);
@@ -263,9 +262,9 @@ static struct evt_entry *get_event_entry(void)
 {
 	struct evt_entry *e;
 
-	if (list_empty(&wm_event.freeq))
+	if (list_empty(&wm_event.freeq)) {
 		e = alloc_event_entry();
-	else {
+	} else {
 		e = list_entry(wm_event.freeq.next, struct evt_entry, list);
 		list_del(&e->list);
 	}
@@ -310,6 +309,7 @@ static int gdm_wimax_event_send(struct net_device *dev, char *buf, int size)
 
 	u16 hci_cmd =  ((u8)buf[0]<<8) | (u8)buf[1];
 	u16 hci_len = ((u8)buf[2]<<8) | (u8)buf[3];
+
 	netdev_dbg(dev, "D=>H: 0x%04x(%d)\n", hci_cmd, hci_len);
 
 	spin_lock_irqsave(&wm_event.evt_lock, flags);
@@ -347,7 +347,7 @@ int gdm_wimax_send_tx(struct sk_buff *skb, struct net_device *dev)
 	struct nic *nic = netdev_priv(dev);
 
 	ret = gdm_wimax_send_with_cb(nic, skb->data, skb->len, tx_complete,
-					nic);
+				     nic);
 	if (ret == -ENOSPC) {
 		netif_stop_queue(dev);
 		ret = 0;
@@ -368,7 +368,7 @@ static int gdm_wimax_tx(struct sk_buff *skb, struct net_device *dev)
 {
 	int ret = 0;
 	struct nic *nic = netdev_priv(dev);
-	struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
+	struct fsm_s *fsm = (struct fsm_s *)nic->sdk_data[SIOC_DATA_FSM].buf;
 
 	dump_eth_packet(dev, "TX", skb->data, skb->len);
 
@@ -379,9 +379,9 @@ static int gdm_wimax_tx(struct sk_buff *skb, struct net_device *dev)
 	}
 
 	#if !defined(LOOPBACK_TEST)
-	if (!fsm)
+	if (!fsm) {
 		netdev_err(dev, "ASSERTION ERROR: fsm is NULL!!\n");
-	else if (fsm->m_status != M_CONNECTED) {
+	} else if (fsm->m_status != M_CONNECTED) {
 		netdev_emerg(dev, "ASSERTION ERROR: Device is NOT ready. status=%d\n",
 			     fsm->m_status);
 		kfree_skb(skb);
@@ -408,7 +408,7 @@ static int gdm_wimax_set_config(struct net_device *dev, struct ifmap *map)
 static void __gdm_wimax_set_mac_addr(struct net_device *dev, char *mac_addr)
 {
 	u16 hci_pkt_buf[32 / sizeof(u16)];
-	u8 *pkt = (u8 *) &hci_pkt_buf[0];
+	u8 *pkt = (u8 *)&hci_pkt_buf[0];
 	struct nic *nic = netdev_priv(dev);
 
 	/* Since dev is registered as a ethernet device,
@@ -454,7 +454,7 @@ static struct net_device_stats *gdm_wimax_stats(struct net_device *dev)
 static int gdm_wimax_open(struct net_device *dev)
 {
 	struct nic *nic = netdev_priv(dev);
-	struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
+	struct fsm_s *fsm = (struct fsm_s *)nic->sdk_data[SIOC_DATA_FSM].buf;
 
 	netif_start_queue(dev);
 
@@ -466,7 +466,7 @@ static int gdm_wimax_open(struct net_device *dev)
 static int gdm_wimax_close(struct net_device *dev)
 {
 	struct nic *nic = netdev_priv(dev);
-	struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
+	struct fsm_s *fsm = (struct fsm_s *)nic->sdk_data[SIOC_DATA_FSM].buf;
 
 	netif_stop_queue(dev);
 
@@ -536,17 +536,17 @@ static void gdm_wimax_cleanup_ioctl(struct net_device *dev)
 static void gdm_update_fsm(struct net_device *dev, struct fsm_s *new_fsm)
 {
 	struct nic *nic = netdev_priv(dev);
-	struct fsm_s *cur_fsm =
-		(struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
+	struct fsm_s *cur_fsm = (struct fsm_s *)
+					nic->sdk_data[SIOC_DATA_FSM].buf;
 
 	if (!cur_fsm)
 		return;
 
 	if (cur_fsm->m_status != new_fsm->m_status ||
-		cur_fsm->c_status != new_fsm->c_status) {
-		if (new_fsm->m_status == M_CONNECTED)
+	    cur_fsm->c_status != new_fsm->c_status) {
+		if (new_fsm->m_status == M_CONNECTED) {
 			netif_carrier_on(dev);
-		else if (cur_fsm->m_status == M_CONNECTED) {
+		} else if (cur_fsm->m_status == M_CONNECTED) {
 			netif_carrier_off(dev);
 			#if defined(CONFIG_WIMAX_GDM72XX_QOS)
 			gdm_qos_release_list(nic);
@@ -558,7 +558,7 @@ static void gdm_update_fsm(struct net_device *dev, struct fsm_s *new_fsm)
 
 static int gdm_wimax_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 {
-	struct wm_req_s *req = (struct wm_req_s *) ifr;
+	struct wm_req_s *req = (struct wm_req_s *)ifr;
 	struct nic *nic = netdev_priv(dev);
 	int ret;
 
@@ -574,8 +574,8 @@ static int gdm_wimax_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 			return -EOPNOTSUPP;
 		}
 		if (req->cmd == SIOCG_DATA) {
-			ret = gdm_wimax_ioctl_get_data(&req->data,
-						&nic->sdk_data[req->data_id]);
+			ret = gdm_wimax_ioctl_get_data(
+				&req->data, &nic->sdk_data[req->data_id]);
 			if (ret < 0)
 				return ret;
 		} else if (req->cmd == SIOCS_DATA) {
@@ -583,7 +583,7 @@ static int gdm_wimax_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 				/*NOTE: gdm_update_fsm should be called
 				before gdm_wimax_ioctl_set_data is called*/
 				gdm_update_fsm(dev,
-						(struct fsm_s *) req->data.buf);
+					       (struct fsm_s *)req->data.buf);
 			}
 			ret = gdm_wimax_ioctl_set_data(
 				&nic->sdk_data[req->data_id], &req->data);
@@ -603,7 +603,7 @@ static void gdm_wimax_prepare_device(struct net_device *dev)
 {
 	struct nic *nic = netdev_priv(dev);
 	u16 buf[32 / sizeof(u16)];
-	struct hci_s *hci = (struct hci_s *) buf;
+	struct hci_s *hci = (struct hci_s *)buf;
 	u16 len = 0;
 	u32 val = 0;
 
@@ -661,7 +661,7 @@ static int gdm_wimax_hci_get_tlv(u8 *buf, u8 *T, u16 *L, u8 **V)
 }
 
 static int gdm_wimax_get_prepared_info(struct net_device *dev, char *buf,
-					int len)
+				       int len)
 {
 	u8 T, *V;
 	u16 L;
@@ -740,7 +740,7 @@ static void gdm_wimax_transmit_aggr_pkt(struct net_device *dev, char *buf,
 	int length;
 
 	while (len > 0) {
-		hci = (struct hci_s *) buf;
+		hci = (struct hci_s *)buf;
 
 		if (B2H(hci->cmd_evt) != WIMAX_RX_SDU) {
 			netdev_err(dev, "Wrong cmd_evt(0x%04X)\n",
@@ -786,7 +786,7 @@ static void gdm_wimax_transmit_pkt(struct net_device *dev, char *buf, int len)
 	switch (cmd_evt) {
 	case WIMAX_RX_SDU_AGGR:
 		gdm_wimax_transmit_aggr_pkt(dev, &buf[HCI_HEADER_SIZE],
-						cmd_len);
+					    cmd_len);
 		break;
 	case WIMAX_RX_SDU:
 		gdm_wimax_netif_rx(dev, &buf[HCI_HEADER_SIZE], cmd_len);
@@ -822,13 +822,13 @@ static void gdm_wimax_ind_fsm_update(struct net_device *dev, struct fsm_s *fsm)
 	memcpy(&hci_pkt_buf[HCI_HEADER_SIZE], fsm, sizeof(struct fsm_s));
 
 	gdm_wimax_event_send(dev, hci_pkt_buf,
-				HCI_HEADER_SIZE + sizeof(struct fsm_s));
+			     HCI_HEADER_SIZE + sizeof(struct fsm_s));
 }
 
 static void gdm_wimax_ind_if_updown(struct net_device *dev, int if_up)
 {
 	u16 buf[32 / sizeof(u16)];
-	struct hci_s *hci = (struct hci_s *) buf;
+	struct hci_s *hci = (struct hci_s *)buf;
 	unsigned char up_down;
 
 	up_down = if_up ? WIMAX_IF_UP : WIMAX_IF_DOWN;
@@ -855,17 +855,13 @@ static void prepare_rx_complete(void *arg, void *data, int len)
 	int ret;
 
 	ret = gdm_wimax_get_prepared_info(nic->netdev, data, len);
-	if (ret == 1)
+	if (ret == 1) {
 		gdm_wimax_rcv_with_cb(nic, rx_complete, nic);
-	else {
+	} else {
 		if (ret < 0)
 			netdev_err(nic->netdev,
 				   "get_prepared_info failed(%d)\n", ret);
 		gdm_wimax_rcv_with_cb(nic, prepare_rx_complete, nic);
-		#if 0
-		/* Re-prepare WiMax device */
-		gdm_wimax_prepare_device(nic->netdev);
-		#endif
 	}
 }
 
@@ -875,13 +871,13 @@ static void start_rx_proc(struct nic *nic)
 }
 
 static struct net_device_ops gdm_netdev_ops = {
-	.ndo_open				= gdm_wimax_open,
-	.ndo_stop				= gdm_wimax_close,
-	.ndo_set_config			= gdm_wimax_set_config,
-	.ndo_start_xmit			= gdm_wimax_tx,
-	.ndo_get_stats			= gdm_wimax_stats,
+	.ndo_open		= gdm_wimax_open,
+	.ndo_stop		= gdm_wimax_close,
+	.ndo_set_config		= gdm_wimax_set_config,
+	.ndo_start_xmit		= gdm_wimax_tx,
+	.ndo_get_stats		= gdm_wimax_stats,
 	.ndo_set_mac_address	= gdm_wimax_set_mac_addr,
-	.ndo_do_ioctl			= gdm_wimax_ioctl,
+	.ndo_do_ioctl		= gdm_wimax_ioctl,
 };
 
 int register_wimax_device(struct phy_dev *phy_dev, struct device *pdev)
@@ -948,7 +944,7 @@ cleanup:
 void unregister_wimax_device(struct phy_dev *phy_dev)
 {
 	struct nic *nic = netdev_priv(phy_dev->netdev);
-	struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
+	struct fsm_s *fsm = (struct fsm_s *)nic->sdk_data[SIOC_DATA_FSM].buf;
 
 	if (fsm)
 		fsm->m_status = M_INIT;
diff --git a/drivers/staging/gdm72xx/gdm_wimax.h b/drivers/staging/gdm72xx/gdm_wimax.h
index 1fcfc855541..7e2c88877ed 100644
--- a/drivers/staging/gdm72xx/gdm_wimax.h
+++ b/drivers/staging/gdm72xx/gdm_wimax.h
@@ -23,10 +23,6 @@
 
 #define DRIVER_VERSION		"3.2.3"
 
-/*#define ETH_P_IP	0x0800 */
-/*#define ETH_P_ARP	0x0806 */
-/*#define ETH_P_IPV6	0x86DD */
-
 #define H2L(x)		__cpu_to_le16(x)
 #define L2H(x)		__le16_to_cpu(x)
 #define DH2L(x)		__cpu_to_le32(x)
@@ -38,12 +34,11 @@
 #define DB2H(x)		__be32_to_cpu(x)
 
 struct phy_dev {
-	void	*priv_dev;
+	void			*priv_dev;
 	struct net_device	*netdev;
-
-	int	(*send_func)(void *priv_dev, void *data, int len,
-			void (*cb)(void *cb_data), void *cb_data);
-	int	(*rcv_func)(void *priv_dev,
+	int (*send_func)(void *priv_dev, void *data, int len,
+			 void (*cb)(void *cb_data), void *cb_data);
+	int (*rcv_func)(void *priv_dev,
 			void (*cb)(void *cb_data, void *data, int len),
 			void *cb_data);
 };
@@ -51,21 +46,15 @@ struct phy_dev {
 struct nic {
 	struct net_device	*netdev;
 	struct phy_dev		*phy_dev;
-
 	struct net_device_stats	stats;
-
-	struct data_s	sdk_data[SIOC_DATA_MAX];
-
+	struct data_s		sdk_data[SIOC_DATA_MAX];
 #if defined(CONFIG_WIMAX_GDM72XX_QOS)
-	struct qos_cb_s	qos;
+	struct qos_cb_s		qos;
 #endif
-
 };
 
-/*#define LOOPBACK_TEST */
-
-extern int register_wimax_device(struct phy_dev *phy_dev, struct device *pdev);
-extern int gdm_wimax_send_tx(struct sk_buff *skb, struct net_device *dev);
-extern void unregister_wimax_device(struct phy_dev *phy_dev);
+int register_wimax_device(struct phy_dev *phy_dev, struct device *pdev);
+int gdm_wimax_send_tx(struct sk_buff *skb, struct net_device *dev);
+void unregister_wimax_device(struct phy_dev *phy_dev);
 
 #endif
diff --git a/drivers/staging/gdm72xx/hci.h b/drivers/staging/gdm72xx/hci.h
index 0e0676622f1..2485a379912 100644
--- a/drivers/staging/gdm72xx/hci.h
+++ b/drivers/staging/gdm72xx/hci.h
@@ -18,18 +18,18 @@
 #define HCI_VALUE_OFFS		(HCI_HEADER_SIZE)
 #define HCI_MAX_PACKET		2048
 #define HCI_MAX_PARAM		(HCI_MAX_PACKET-HCI_HEADER_SIZE)
-#define HCI_MAX_TLV			32
+#define HCI_MAX_TLV		32
 
 /* CMD-EVT */
 
 /* Category 0 */
-#define WIMAX_RESET				0x0000
-#define WIMAX_SET_INFO			0x0001
-#define WIMAX_GET_INFO			0x0002
+#define WIMAX_RESET		0x0000
+#define WIMAX_SET_INFO		0x0001
+#define WIMAX_GET_INFO		0x0002
 #define WIMAX_GET_INFO_RESULT	0x8003
-#define WIMAX_RADIO_OFF			0x0004
-#define WIMAX_RADIO_ON			0x0006
-#define WIMAX_WIMAX_RESET		0x0007	/* Is this still here */
+#define WIMAX_RADIO_OFF		0x0004
+#define WIMAX_RADIO_ON		0x0006
+#define WIMAX_WIMAX_RESET	0x0007	/* Is this still here */
 
 /* Category 1 */
 #define WIMAX_NET_ENTRY			0x0100
@@ -40,26 +40,22 @@
 #define WIMAX_EXIT_IDLE			0x0106
 #define WIMAX_MODE_CHANGE		0x8108
 #define WIMAX_HANDOVER			0x8109	/* obsolete */
-
-#define WIMAX_SCAN				0x010d
+#define WIMAX_SCAN			0x010d
 #define WIMAX_SCAN_COMPLETE		0x810e
 #define WIMAX_SCAN_RESULT		0x810f
-
 #define WIMAX_CONNECT			0x0110
 #define WIMAX_CONNECT_START		0x8111
-#define WIMAX_CONNECT_COMPLETE	0x8112
+#define WIMAX_CONNECT_COMPLETE		0x8112
 #define WIMAX_ASSOC_START		0x8113
-#define WIMAX_ASSOC_COMPLETE	0x8114
+#define WIMAX_ASSOC_COMPLETE		0x8114
 #define WIMAX_DISCONN_IND		0x8115
 #define WIMAX_ENTRY_IND			0x8116
 #define WIMAX_HO_START			0x8117
 #define WIMAX_HO_COMPLETE		0x8118
-#define WIMAX_RADIO_STATE_IND	0x8119
+#define WIMAX_RADIO_STATE_IND		0x8119
 #define WIMAX_IP_RENEW_IND		0x811a
-
-#define WIMAX_DISCOVER_NSP			0x011d
+#define WIMAX_DISCOVER_NSP		0x011d
 #define WIMAX_DISCOVER_NSP_RESULT	0x811e
-
 #define WIMAX_SDU_TX_FLOW		0x8125
 
 /* Category 2 */
@@ -71,34 +67,33 @@
 #define WIMAX_TX_SDU_AGGR	0x0205
 
 /* Category 3 */
-#define WIMAX_DM_CMD				0x030a
-#define WIMAX_DM_RSP				0x830b
-
-#define WIMAX_CLI_CMD				0x030c
-#define WIMAX_CLI_RSP				0x830d
+#define WIMAX_DM_CMD		0x030a
+#define WIMAX_DM_RSP		0x830b
 
-#define WIMAX_DL_IMAGE				0x0310
-#define WIMAX_DL_IMAGE_STATUS		0x8311
-#define WIMAX_UL_IMAGE				0x0312
-#define WIMAX_UL_IMAGE_RESULT		0x8313
-#define WIMAX_UL_IMAGE_STATUS		0x0314
+#define WIMAX_CLI_CMD		0x030c
+#define WIMAX_CLI_RSP		0x830d
 
-#define WIMAX_EVT_MODEM_REPORT		0x8325
+#define WIMAX_DL_IMAGE		0x0310
+#define WIMAX_DL_IMAGE_STATUS	0x8311
+#define WIMAX_UL_IMAGE		0x0312
+#define WIMAX_UL_IMAGE_RESULT	0x8313
+#define WIMAX_UL_IMAGE_STATUS	0x0314
+#define WIMAX_EVT_MODEM_REPORT	0x8325
 
 /* Category 0xF */
-#define WIMAX_FSM_UPDATE			0x8F01
-#define WIMAX_IF_UPDOWN				0x8F02
-	#define WIMAX_IF_UP				1
-	#define WIMAX_IF_DOWN			2
+#define WIMAX_FSM_UPDATE	0x8F01
+#define WIMAX_IF_UPDOWN		0x8F02
+#define WIMAX_IF_UP		1
+#define WIMAX_IF_DOWN		2
 
 /* WIMAX mode */
-#define W_NULL				0
-#define W_STANDBY			1
-#define W_OOZ				2
-#define W_AWAKE				3
-#define W_IDLE				4
-#define W_SLEEP				5
-#define W_WAIT				6
+#define W_NULL		0
+#define W_STANDBY	1
+#define W_OOZ		2
+#define W_AWAKE		3
+#define W_IDLE		4
+#define W_SLEEP		5
+#define W_WAIT		6
 
 #define W_NET_ENTRY_RNG		0x80
 #define W_NET_ENTRY_SBC		0x81
@@ -113,8 +108,8 @@
 #define W_NET_ENTRY_DSX_FAIL	0x1104000
 
 /* Scan Type */
-#define W_SCAN_ALL_CHANNEL				0
-#define W_SCAN_ALL_SUBSCRIPTION			1
+#define W_SCAN_ALL_CHANNEL		0
+#define W_SCAN_ALL_SUBSCRIPTION		1
 #define W_SCAN_SPECIFIED_SUBSCRIPTION	2
 
 /*
@@ -126,7 +121,7 @@
  *
  */
 #define TLV_L(x)		(((x) >> 16) & 0xff)
-#define TLV_T(x)			((x) & 0xff)
+#define TLV_T(x)		((x) & 0xff)
 #define TLV_COMPOSITE(x)	((x) >> 31)
 
 /* GENERAL */
@@ -141,7 +136,6 @@
 #define T_OOZ_SCAN_INTERVAL		(0x08	| (4 << 16))
 #define T_IMEI				(0x09	| (8 << 16))
 #define T_PID				(0x0a	| (12 << 16))
-
 #define T_CAPABILITY			(0x1a	| (4 << 16))
 #define T_RELEASE_NUMBER		(0x1b	| (4 << 16))
 #define T_DRIVER_REVISION		(0x1c	| (4 << 16))
@@ -150,19 +144,16 @@
 #define T_PHY_HW_REVISION		(0x1f	| (4 << 16))
 
 /* HANDOVER */
-#define T_SCAN_INTERVAL		(0x20	| (1 << 16))
-
+#define T_SCAN_INTERVAL			(0x20	| (1 << 16))
 #define T_RSC_RETAIN_TIME		(0x2f	| (2 << 16))
 
 /* SLEEP */
 #define T_TYPE1_ISW			(0x40	| (1 << 16))
-
 #define T_SLP_START_TO			(0x4a	| (2 << 16))
 
 /* IDLE */
 #define T_IDLE_MODE_TO			(0x50	| (2 << 16))
-
-#define T_IDLE_START_TO		(0x54	| (2 << 16))
+#define T_IDLE_START_TO			(0x54	| (2 << 16))
 
 /* MONITOR */
 #define T_RSSI				(0x60	| (1 << 16))
@@ -180,7 +171,7 @@
 #define T_CS_TYPE			(0xa6	| (2 << 16))
 #define T_VENDOR_NAME			(0xa7	| (0 << 16))
 #define T_MOD_NAME			(0xa8	| (0 << 16))
-#define T_PACKET_FILTER		(0xa9	| (1 << 16))
+#define T_PACKET_FILTER			(0xa9	| (1 << 16))
 #define T_NSP_CHANGE_COUNT		(0xaa	| (4 << 16))
 #define T_RADIO_STATE			(0xab	| (1 << 16))
 #define T_URI_CONTACT_TYPE		(0xac	| (1 << 16))
diff --git a/drivers/staging/gdm72xx/netlink_k.c b/drivers/staging/gdm72xx/netlink_k.c
index af7f1c1d0b5..9bf00e683f1 100644
--- a/drivers/staging/gdm72xx/netlink_k.c
+++ b/drivers/staging/gdm72xx/netlink_k.c
@@ -56,7 +56,7 @@ static void netlink_rcv_cb(struct sk_buff *skb)
 		nlh = (struct nlmsghdr *)skb->data;
 
 		if (skb->len < nlh->nlmsg_len ||
-		nlh->nlmsg_len > ND_MAX_MSG_LEN) {
+		    nlh->nlmsg_len > ND_MAX_MSG_LEN) {
 			netdev_err(skb->dev, "Invalid length (%d,%d)\n",
 				   skb->len, nlh->nlmsg_len);
 			return;
@@ -75,8 +75,9 @@ static void netlink_rcv_cb(struct sk_buff *skb)
 				netdev_err(skb->dev,
 					   "dev_get_by_index(%d) is not found.\n",
 					   ifindex);
-		} else
+		} else {
 			netdev_err(skb->dev, "Unregistered Callback\n");
+		}
 	}
 }
 
@@ -88,7 +89,7 @@ static void netlink_rcv(struct sk_buff *skb)
 }
 
 struct sock *netlink_init(int unit, void (*cb)(struct net_device *dev, u16 type,
-						void *msg, int len))
+					       void *msg, int len))
 {
 	struct sock *sock;
 	struct netlink_kernel_cfg cfg = {
@@ -144,9 +145,9 @@ 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 {
+	} else {
 		if (ret != -ESRCH) {
 			pr_err("netlink_broadcast g=%d, t=%d, l=%d, r=%d\n",
 			       group, type, len, ret);
diff --git a/drivers/staging/gdm72xx/netlink_k.h b/drivers/staging/gdm72xx/netlink_k.h
index 1dffaa6156e..b6caac16b3d 100644
--- a/drivers/staging/gdm72xx/netlink_k.h
+++ b/drivers/staging/gdm72xx/netlink_k.h
@@ -16,8 +16,8 @@
 #include <linux/netdevice.h>
 #include <net/sock.h>
 
-struct sock *netlink_init(int unit,
-	void (*cb)(struct net_device *dev, u16 type, void *msg, int len));
+struct sock *netlink_init(int unit, void (*cb)(struct net_device *dev, u16 type,
+					       void *msg, int len));
 void netlink_exit(struct sock *sock);
 int netlink_send(struct sock *sock, int group, u16 type, void *msg, int len);
 
diff --git a/drivers/staging/gdm72xx/sdio_boot.c b/drivers/staging/gdm72xx/sdio_boot.c
index cbe5dcfc2ac..2c02842ac5d 100644
--- a/drivers/staging/gdm72xx/sdio_boot.c
+++ b/drivers/staging/gdm72xx/sdio_boot.c
@@ -41,11 +41,11 @@ static u8 *tx_buf;
 
 static int ack_ready(struct sdio_func *func)
 {
-	unsigned long start = jiffies;
+	unsigned long wait = jiffies + HZ;
 	u8 val;
 	int ret;
 
-	while ((jiffies - start) < HZ) {
+	while (time_before(jiffies, wait)) {
 		val = sdio_readb(func, 0x13, &ret);
 		if (val & 0x01)
 			return 1;
diff --git a/drivers/staging/gdm72xx/sdio_boot.h b/drivers/staging/gdm72xx/sdio_boot.h
index 373ac28063c..045c1f45053 100644
--- a/drivers/staging/gdm72xx/sdio_boot.h
+++ b/drivers/staging/gdm72xx/sdio_boot.h
@@ -16,6 +16,6 @@
 
 struct sdio_func;
 
-extern int sdio_boot(struct sdio_func *func);
+int sdio_boot(struct sdio_func *func);
 
 #endif /* __SDIO_BOOT_H__ */
diff --git a/drivers/staging/gdm72xx/usb_boot.c b/drivers/staging/gdm72xx/usb_boot.c
index 0d45eb680be..d59bac872ff 100644
--- a/drivers/staging/gdm72xx/usb_boot.c
+++ b/drivers/staging/gdm72xx/usb_boot.c
@@ -67,6 +67,7 @@ struct fw_info {
 static void array_le32_to_cpu(u32 *arr, int num)
 {
 	int i;
+
 	for (i = 0; i < num; i++, arr++)
 		*arr = __le32_to_cpu(*arr);
 }
@@ -79,7 +80,7 @@ static int gdm_wibro_send(struct usb_device *usbdev, void *data, int len)
 	int actual;
 
 	ret = usb_bulk_msg(usbdev, usb_sndbulkpipe(usbdev, 1), data, len,
-			&actual, 1000);
+			   &actual, 1000);
 
 	if (ret < 0) {
 		dev_err(&usbdev->dev, "Error : usb_bulk_msg ( result = %d )\n",
@@ -95,7 +96,7 @@ static int gdm_wibro_recv(struct usb_device *usbdev, void *data, int len)
 	int actual;
 
 	ret = usb_bulk_msg(usbdev, usb_rcvbulkpipe(usbdev, 2), data, len,
-			&actual, 5000);
+			   &actual, 5000);
 
 	if (ret < 0) {
 		dev_err(&usbdev->dev,
@@ -106,8 +107,8 @@ static int gdm_wibro_recv(struct usb_device *usbdev, void *data, int len)
 }
 
 static int download_image(struct usb_device *usbdev,
-				const struct firmware *firm,
-				loff_t pos, u32 img_len, u32 magic_num)
+			  const struct firmware *firm,
+			  loff_t pos, u32 img_len, u32 magic_num)
 {
 	struct dn_header h;
 	int ret = 0;
@@ -169,14 +170,7 @@ int usb_boot(struct usb_device *usbdev, u16 pid)
 	memcpy(&hdr, firm->data, sizeof(hdr));
 
 	array_le32_to_cpu((u32 *)&hdr, 19);
-#if 0
-	if (hdr.magic_code != 0x10767fff) {
-		dev_err(&usbdev->dev, "Invalid magic code 0x%08x\n",
-			hdr.magic_code);
-		ret = -EINVAL;
-		goto out;
-	}
-#endif
+
 	if (hdr.count > MAX_IMG_CNT) {
 		dev_err(&usbdev->dev, "Too many images. %d\n", hdr.count);
 		ret = -EINVAL;
@@ -201,14 +195,6 @@ int usb_boot(struct usb_device *usbdev, u16 pid)
 		memcpy(&fw_info, firm->data + pos, sizeof(fw_info));
 
 		array_le32_to_cpu((u32 *)&fw_info, 8);
-#if 0
-		if ((fw_info.id & 0xfffff000) != 0x10767000) {
-			dev_err(&usbdev->dev, "Invalid FW id. 0x%08x\n",
-				fw_info.id);
-			ret = -EIO;
-			goto out;
-		}
-#endif
 
 		if ((fw_info.id & 0xffff) != pid)
 			continue;
@@ -219,8 +205,8 @@ int usb_boot(struct usb_device *usbdev, u16 pid)
 			goto out;
 		}
 
-		ret = download_image(usbdev, firm, pos,
-				fw_info.kernel_len, DN_KERNEL_MAGIC_NUMBER);
+		ret = download_image(usbdev, firm, pos, fw_info.kernel_len,
+				     DN_KERNEL_MAGIC_NUMBER);
 		if (ret < 0)
 			goto out;
 		dev_info(&usbdev->dev, "GCT: Kernel download success.\n");
@@ -231,7 +217,7 @@ int usb_boot(struct usb_device *usbdev, u16 pid)
 			goto out;
 		}
 		ret = download_image(usbdev, firm, pos, fw_info.rootfs_len,
-				DN_ROOTFS_MAGIC_NUMBER);
+				     DN_ROOTFS_MAGIC_NUMBER);
 		if (ret < 0)
 			goto out;
 		dev_info(&usbdev->dev, "GCT: Filesystem download success.\n");
@@ -276,7 +262,7 @@ out:
 }
 
 static int em_download_image(struct usb_device *usbdev, const char *img_name,
-				char *type_string)
+			     char *type_string)
 {
 	char *buf = NULL;
 	loff_t pos = 0;
@@ -347,11 +333,8 @@ out:
 
 static int em_fw_reset(struct usb_device *usbdev)
 {
-	int ret;
-
 	/*Send ZLP*/
-	ret = gdm_wibro_send(usbdev, NULL, 0);
-	return ret;
+	return gdm_wibro_send(usbdev, NULL, 0);
 }
 
 int usb_emergency(struct usb_device *usbdev)
diff --git a/drivers/staging/gdm72xx/usb_boot.h b/drivers/staging/gdm72xx/usb_boot.h
index c715cd3cd30..05308e25305 100644
--- a/drivers/staging/gdm72xx/usb_boot.h
+++ b/drivers/staging/gdm72xx/usb_boot.h
@@ -16,7 +16,7 @@
 
 struct usb_device;
 
-extern int usb_boot(struct usb_device *usbdev, u16 pid);
-extern int usb_emergency(struct usb_device *usbdev);
+int usb_boot(struct usb_device *usbdev, u16 pid);
+int usb_emergency(struct usb_device *usbdev);
 
 #endif /* __USB_BOOT_H__ */
diff --git a/drivers/staging/gdm72xx/usb_ids.h b/drivers/staging/gdm72xx/usb_ids.h
index b34616b7203..1a61b359976 100644
--- a/drivers/staging/gdm72xx/usb_ids.h
+++ b/drivers/staging/gdm72xx/usb_ids.h
@@ -29,7 +29,7 @@
 	.idVendor = (vend), .idProduct = (prod), .bInterfaceClass = (intf)
 
 #define EMERGENCY_PID		0x720f
-#define BL_PID_MASK			0xffc0
+#define BL_PID_MASK		0xffc0
 
 #define USB_DEVICE_BOOTLOADER(vid, pid)	\
 	{USB_DEVICE((vid), ((pid)&BL_PID_MASK)|B_DOWNLOAD)},	\
diff --git a/drivers/staging/gdm72xx/wm_ioctl.h b/drivers/staging/gdm72xx/wm_ioctl.h
index 9f46e06f230..d022c6ca0e2 100644
--- a/drivers/staging/gdm72xx/wm_ioctl.h
+++ b/drivers/staging/gdm72xx/wm_ioctl.h
@@ -19,10 +19,10 @@
 
 #define NETLINK_WIMAX	31
 
-#define SIOCWMIOCTL			SIOCDEVPRIVATE
+#define SIOCWMIOCTL	SIOCDEVPRIVATE
 
-#define SIOCG_DATA			0x8D10
-#define SIOCS_DATA			0x8D11
+#define SIOCG_DATA	0x8D10
+#define SIOCS_DATA	0x8D11
 
 enum {
 	SIOC_DATA_FSM,
@@ -34,7 +34,7 @@ enum {
 	SIOC_DATA_END
 };
 
-#define SIOC_DATA_MAX			16
+#define SIOC_DATA_MAX	16
 
 /* FSM */
 enum {
@@ -67,23 +67,21 @@ enum {
 };
 
 struct fsm_s {
-	int		m_status;	/*main status*/
-	int		c_status;	/*connection status*/
-	int		d_status;	/*oma-dm status*/
+	int	m_status;	/*main status*/
+	int	c_status;	/*connection status*/
+	int	d_status;	/*oma-dm status*/
 };
 
 struct data_s {
-	int		size;
+	int	size;
 	void	*buf;
 };
 
 struct wm_req_s {
 	union {
-		char	ifrn_name[IFNAMSIZ];
+		char ifrn_name[IFNAMSIZ];
 	} ifr_ifrn;
-
 	unsigned short	cmd;
-
 	unsigned short	data_id;
 	struct data_s	data;
 
@@ -91,7 +89,7 @@ struct wm_req_s {
 };
 
 #ifndef ifr_name
-#define ifr_name	ifr_ifrn.ifrn_name
+#define ifr_name ifr_ifrn.ifrn_name
 #endif
 
 #endif
diff --git a/drivers/staging/goldfish/README b/drivers/staging/goldfish/README
index 93d65b0f0f8..183af005323 100644
--- a/drivers/staging/goldfish/README
+++ b/drivers/staging/goldfish/README
@@ -5,7 +5,6 @@ Audio
 
 NAND
 ----
-- Switch from spinlock to mutex
 - Remove excess checking of parameters in calls
 - Use dma coherent memory not kmalloc/__pa for the memory (this is just
   a cleanliness issue not a correctness one)
diff --git a/drivers/staging/goldfish/goldfish_audio.c b/drivers/staging/goldfish/goldfish_audio.c
index 7ac2602242f..cbd456770af 100644
--- a/drivers/staging/goldfish/goldfish_audio.c
+++ b/drivers/staging/goldfish/goldfish_audio.c
@@ -26,6 +26,7 @@
 #include <linux/sched.h>
 #include <linux/dma-mapping.h>
 #include <linux/uaccess.h>
+#include <linux/goldfish.h>
 
 MODULE_AUTHOR("Google, Inc.");
 MODULE_DESCRIPTION("Android QEMU Audio Driver");
@@ -60,6 +61,8 @@ struct goldfish_audio {
 
 #define AUDIO_READ(data, addr)		(readl(data->reg_base + addr))
 #define AUDIO_WRITE(data, addr, x)	(writel(x, data->reg_base + addr))
+#define AUDIO_WRITE64(data, addr, addr2, x)	\
+	(gf_write64((u64)(x), data->reg_base + addr, data->reg_base+addr2))
 
 /*
  *  temporary variable used between goldfish_audio_probe() and
@@ -78,11 +81,14 @@ enum {
 	/* set number of bytes in buffer to write */
 	AUDIO_WRITE_BUFFER_1  = 0x10,
 	AUDIO_WRITE_BUFFER_2  = 0x14,
+	AUDIO_SET_WRITE_BUFFER_1_HIGH = 0x28,
+	AUDIO_SET_WRITE_BUFFER_2_HIGH = 0x30,
 
 	/* true if audio input is supported */
 	AUDIO_READ_SUPPORTED = 0x18,
 	/* buffer to use for audio input */
 	AUDIO_SET_READ_BUFFER = 0x1C,
+	AUDIO_SET_READ_BUFFER_HIGH = 0x34,
 
 	/* driver writes number of bytes to read */
 	AUDIO_START_READ  = 0x20,
@@ -147,6 +153,7 @@ static ssize_t goldfish_audio_write(struct file *fp, const char __user *buf,
 
 	while (count > 0) {
 		ssize_t copy = count;
+
 		if (copy > WRITE_BUFFER_SIZE)
 			copy = WRITE_BUFFER_SIZE;
 		wait_event_interruptible(data->wait, (data->buffer_status &
@@ -321,14 +328,19 @@ static int goldfish_audio_probe(struct platform_device *pdev)
 		goto err_misc_register_failed;
 	}
 
-	AUDIO_WRITE(data, AUDIO_SET_WRITE_BUFFER_1, buf_addr);
-	AUDIO_WRITE(data, AUDIO_SET_WRITE_BUFFER_2,
-						buf_addr + WRITE_BUFFER_SIZE);
+	AUDIO_WRITE64(data, AUDIO_SET_WRITE_BUFFER_1,
+				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);
+
+	buf_addr += WRITE_BUFFER_SIZE;
 
 	data->read_supported = AUDIO_READ(data, AUDIO_READ_SUPPORTED);
 	if (data->read_supported)
-		AUDIO_WRITE(data, AUDIO_SET_READ_BUFFER,
-					buf_addr + 2 * WRITE_BUFFER_SIZE);
+		AUDIO_WRITE64(data, AUDIO_SET_READ_BUFFER,
+				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 eca0873098c..092604c698b 100644
--- a/drivers/staging/goldfish/goldfish_nand.c
+++ b/drivers/staging/goldfish/goldfish_nand.c
@@ -24,13 +24,14 @@
 #include <linux/vmalloc.h>
 #include <linux/mtd/mtd.h>
 #include <linux/platform_device.h>
-
+#include <linux/mutex.h>
+#include <linux/goldfish.h>
 #include <asm/div64.h>
 
 #include "goldfish_nand_reg.h"
 
 struct goldfish_nand {
-	spinlock_t              lock;
+	struct mutex            lock;
 	unsigned char __iomem  *base;
 	struct cmd_params       *cmd_params;
 	size_t                  mtd_count;
@@ -66,7 +67,7 @@ static u32 goldfish_nand_cmd_with_params(struct mtd_info *mtd,
 	cps->addr_high = (u32)(addr >> 32);
 	cps->addr_low = (u32)addr;
 	cps->transfer_size = len;
-	cps->data = (u32)ptr;
+	cps->data = (unsigned long)ptr;
 	writel(cmdp, base + NAND_COMMAND);
 	*rv = cps->result;
 	return 0;
@@ -77,20 +78,19 @@ static u32 goldfish_nand_cmd(struct mtd_info *mtd, enum nand_cmd cmd,
 {
 	struct goldfish_nand *nand = mtd->priv;
 	u32 rv;
-	unsigned long irq_flags;
 	unsigned char __iomem  *base = nand->base;
 
-	spin_lock_irqsave(&nand->lock, irq_flags);
+	mutex_lock(&nand->lock);
 	if (goldfish_nand_cmd_with_params(mtd, cmd, addr, len, ptr, &rv)) {
 		writel(mtd - nand->mtd, base + NAND_DEV);
 		writel((u32)(addr >> 32), base + NAND_ADDR_HIGH);
 		writel((u32)addr, base + NAND_ADDR_LOW);
 		writel(len, base + NAND_TRANSFER_SIZE);
-		writel((u32)ptr, base + NAND_DATA);
+		gf_write64((u64)ptr, base + NAND_DATA, base + NAND_DATA_HIGH);
 		writel(cmd, base + NAND_COMMAND);
 		rv = readl(base + NAND_RESULT);
 	}
-	spin_unlock_irqrestore(&nand->lock, irq_flags);
+	mutex_unlock(&nand->lock);
 	return rv;
 }
 
@@ -199,8 +199,6 @@ static int goldfish_nand_read(struct mtd_info *mtd, loff_t from, size_t len,
 
 	if (from + len > mtd->size)
 		goto invalid_arg;
-	if (len != mtd->writesize)
-		goto invalid_arg;
 
 	rem = do_div(from, mtd->writesize);
 	if (rem)
@@ -223,8 +221,6 @@ static int goldfish_nand_write(struct mtd_info *mtd, loff_t to, size_t len,
 
 	if (to + len > mtd->size)
 		goto invalid_arg;
-	if (len != mtd->writesize)
-		goto invalid_arg;
 
 	rem = do_div(to, mtd->writesize);
 	if (rem)
@@ -307,12 +303,11 @@ static int goldfish_nand_init_device(struct platform_device *pdev,
 	u32 name_len;
 	u32 result;
 	u32 flags;
-	unsigned long irq_flags;
 	unsigned char __iomem  *base = nand->base;
 	struct mtd_info *mtd = &nand->mtd[id];
 	char *name;
 
-	spin_lock_irqsave(&nand->lock, irq_flags);
+	mutex_lock(&nand->lock);
 	writel(id, base + NAND_DEV);
 	flags = readl(base + NAND_DEV_FLAGS);
 	name_len = readl(base + NAND_DEV_NAME_LEN);
@@ -329,7 +324,7 @@ static int goldfish_nand_init_device(struct platform_device *pdev,
 		"goldfish nand dev%d: size %llx, page %d, extra %d, erase %d\n",
 		       id, mtd->size, mtd->writesize,
 		       mtd->oobsize, mtd->erasesize);
-	spin_unlock_irqrestore(&nand->lock, irq_flags);
+	mutex_unlock(&nand->lock);
 
 	mtd->priv = nand;
 
@@ -405,7 +400,7 @@ static int goldfish_nand_probe(struct platform_device *pdev)
 	if (nand == NULL)
 		return -ENOMEM;
 
-	spin_lock_init(&nand->lock);
+	mutex_init(&nand->lock);
 	nand->base = base;
 	nand->mtd_count = num_dev;
 	platform_set_drvdata(pdev, nand);
@@ -425,6 +420,7 @@ static int goldfish_nand_remove(struct platform_device *pdev)
 {
 	struct goldfish_nand *nand = platform_get_drvdata(pdev);
 	int i;
+
 	for (i = 0; i < nand->mtd_count; i++) {
 		if (nand->mtd[i].name)
 			mtd_device_unregister(&nand->mtd[i]);
diff --git a/drivers/staging/goldfish/goldfish_nand_reg.h b/drivers/staging/goldfish/goldfish_nand_reg.h
index ddfda71ab27..fe7f47c7a5c 100644
--- a/drivers/staging/goldfish/goldfish_nand_reg.h
+++ b/drivers/staging/goldfish/goldfish_nand_reg.h
@@ -57,6 +57,7 @@ enum nand_reg {
 	NAND_RESULT         = 0x040,
 	NAND_COMMAND        = 0x044,
 	NAND_DATA           = 0x048,
+	NAND_DATA_HIGH	    = 0x100,
 	NAND_TRANSFER_SIZE  = 0x04c,
 	NAND_ADDR_LOW       = 0x050,
 	NAND_ADDR_HIGH      = 0x054,
@@ -69,7 +70,7 @@ struct cmd_params {
 	uint32_t addr_low;
 	uint32_t addr_high;
 	uint32_t transfer_size;
-	uint32_t data;
+	unsigned long data;
 	uint32_t result;
 };
 #endif
diff --git a/drivers/staging/gs_fpgaboot/gs_fpgaboot.c b/drivers/staging/gs_fpgaboot/gs_fpgaboot.c
index 7506900c9b8..6aa9d7c3013 100644
--- a/drivers/staging/gs_fpgaboot/gs_fpgaboot.c
+++ b/drivers/staging/gs_fpgaboot/gs_fpgaboot.c
@@ -214,8 +214,8 @@ static int gs_download_image(struct fpgaimage *fimage, enum wbus bus_bytes)
 {
 	char *bitdata;
 	int size, i, cnt;
-	cnt = 0;
 
+	cnt = 0;
 	bitdata = (char *)fimage->fpgadata;
 	size = fimage->lendata;
 
diff --git a/drivers/staging/gs_fpgaboot/io.c b/drivers/staging/gs_fpgaboot/io.c
index b7be8e37b8d..23c12f47c8f 100644
--- a/drivers/staging/gs_fpgaboot/io.c
+++ b/drivers/staging/gs_fpgaboot/io.c
@@ -44,6 +44,7 @@ static inline void xl_cclk_b(int32_t i);
 void xl_shift_cclk(int count)
 {
 	int i;
+
 	for (i = 0; i < count; i++) {
 		xl_cclk_b(1);
 		xl_cclk_b(0);
@@ -85,6 +86,7 @@ void xl_shift_bytes_out(enum wbus bus_byte, unsigned char *pdata)
 static inline unsigned char bitswap(unsigned char s)
 {
 	unsigned char d;
+
 	d = (((s&0x80)>>7) | ((s&0x40)>>5) | ((s&0x20)>>3) | ((s&0x10)>>1) |
 		((s&0x08)<<1) | ((s&0x04)<<3) | ((s&0x02)<<5) | ((s&0x01)<<7));
 	return d;
@@ -135,6 +137,7 @@ static inline void mpc85xx_gpio_set_high(int32_t port, uint32_t gpios)
 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);
@@ -145,6 +148,7 @@ static inline void gpio_set_value(int32_t port, uint32_t gpio, uint32_t value)
 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);
 }
@@ -184,6 +188,7 @@ int xl_get_done_b(void)
 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;
@@ -195,6 +200,7 @@ static inline uint32_t bit_remap_byte0(uint32_t s)
 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);
diff --git a/drivers/staging/iio/Documentation/iio_utils.h b/drivers/staging/iio/Documentation/iio_utils.h
index 2064839ef2c..a9cfc06edb0 100644
--- a/drivers/staging/iio/Documentation/iio_utils.h
+++ b/drivers/staging/iio/Documentation/iio_utils.h
@@ -318,7 +318,7 @@ inline int build_channel_array(const char *device_dir,
 				free(filename);
 				goto error_close_dir;
 			}
-			fscanf(sysfsfp, "%u", &ret);
+			fscanf(sysfsfp, "%i", &ret);
 			if (ret == 1)
 				(*counter)++;
 			fclose(sysfsfp);
@@ -350,7 +350,7 @@ inline int build_channel_array(const char *device_dir,
 				ret = -errno;
 				goto error_cleanup_array;
 			}
-			fscanf(sysfsfp, "%u", &current_enabled);
+			fscanf(sysfsfp, "%i", &current_enabled);
 			fclose(sysfsfp);
 
 			if (!current_enabled) {
diff --git a/drivers/staging/iio/adc/Kconfig b/drivers/staging/iio/adc/Kconfig
index 363329808a4..b87e382ad76 100644
--- a/drivers/staging/iio/adc/Kconfig
+++ b/drivers/staging/iio/adc/Kconfig
@@ -37,26 +37,6 @@ config AD7606_IFACE_SPI
 	  Say yes here to include parallel interface support on the AD7606
 	  ADC driver.
 
-config AD799X
-	tristate "Analog Devices AD799x ADC driver"
-	depends on I2C
-	select IIO_TRIGGER if IIO_BUFFER
-	select AD799X_RING_BUFFER
-	help
-	  Say yes here to build support for Analog Devices:
-	  ad7991, ad7995, ad7999, ad7992, ad7993, ad7994, ad7997, ad7998
-	  i2c analog to digital converters (ADC). Provides direct access
-	  via sysfs.
-
-config AD799X_RING_BUFFER
-	bool "Analog Devices AD799x: use ring buffer"
-	depends on AD799X
-	select IIO_BUFFER
-	select IIO_TRIGGERED_BUFFER
-	help
-	  Say yes here to include ring buffer support in the AD799X
-	  ADC driver.
-
 config AD7780
 	tristate "Analog Devices AD7780 and similar ADCs driver"
 	depends on SPI
diff --git a/drivers/staging/iio/adc/Makefile b/drivers/staging/iio/adc/Makefile
index 3e9fb143d25..afdcd1ff08f 100644
--- a/drivers/staging/iio/adc/Makefile
+++ b/drivers/staging/iio/adc/Makefile
@@ -8,10 +8,6 @@ ad7606-$(CONFIG_AD7606_IFACE_PARALLEL) += ad7606_par.o
 ad7606-$(CONFIG_AD7606_IFACE_SPI) += ad7606_spi.o
 obj-$(CONFIG_AD7606) += ad7606.o
 
-ad799x-y := ad799x_core.o
-ad799x-$(CONFIG_AD799X_RING_BUFFER) += ad799x_ring.o
-obj-$(CONFIG_AD799X) += ad799x.o
-
 obj-$(CONFIG_AD7291) += ad7291.o
 obj-$(CONFIG_AD7780) += ad7780.o
 obj-$(CONFIG_AD7816) += ad7816.o
diff --git a/drivers/staging/iio/adc/ad7280a.c b/drivers/staging/iio/adc/ad7280a.c
index 1ac11f64827..d215edf66af 100644
--- a/drivers/staging/iio/adc/ad7280a.c
+++ b/drivers/staging/iio/adc/ad7280a.c
@@ -443,7 +443,7 @@ static ssize_t ad7280_show_balance_timer(struct device *dev,
 
 	msecs = (ret >> 3) * 71500;
 
-	return sprintf(buf, "%d\n", msecs);
+	return sprintf(buf, "%u\n", msecs);
 }
 
 static ssize_t ad7280_store_balance_timer(struct device *dev,
@@ -619,7 +619,7 @@ static ssize_t ad7280_read_channel_config(struct device *dev,
 		return -EINVAL;
 	}
 
-	return sprintf(buf, "%d\n", val);
+	return sprintf(buf, "%u\n", val);
 }
 
 static ssize_t ad7280_write_channel_config(struct device *dev,
diff --git a/drivers/staging/iio/adc/ad7606.h b/drivers/staging/iio/adc/ad7606.h
index 93c7299e835..ec89d055cf5 100644
--- a/drivers/staging/iio/adc/ad7606.h
+++ b/drivers/staging/iio/adc/ad7606.h
@@ -14,7 +14,7 @@
  */
 
 /**
- * struct ad7606_platform_data - platform/board specifc information
+ * struct ad7606_platform_data - platform/board specific information
  * @default_os:		default oversampling value {0, 2, 4, 8, 16, 32, 64}
  * @default_range:	default range +/-{5000, 10000} mVolt
  * @gpio_convst:	number of gpio connected to the CONVST pin
@@ -41,7 +41,7 @@ struct ad7606_platform_data {
 };
 
 /**
- * struct ad7606_chip_info - chip specifc information
+ * struct ad7606_chip_info - chip specific information
  * @name:		identification string for chip
  * @int_vref_mv:	the internal reference voltage
  * @channels:		channel specification
diff --git a/drivers/staging/iio/adc/ad7816.c b/drivers/staging/iio/adc/ad7816.c
index 2369cf28412..158d770f961 100644
--- a/drivers/staging/iio/adc/ad7816.c
+++ b/drivers/staging/iio/adc/ad7816.c
@@ -40,7 +40,7 @@
 
 
 /*
- * struct ad7816_chip_info - chip specifc information
+ * struct ad7816_chip_info - chip specific information
  */
 
 struct ad7816_chip_info {
diff --git a/drivers/staging/iio/adc/ad799x.h b/drivers/staging/iio/adc/ad799x.h
deleted file mode 100644
index fc8c85298fe..00000000000
--- a/drivers/staging/iio/adc/ad799x.h
+++ /dev/null
@@ -1,121 +0,0 @@
-/*
- * Copyright (C) 2010-2011 Michael Hennerich, Analog Devices Inc.
- * Copyright (C) 2008-2010 Jonathan Cameron
- *
- * 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.
- *
- * ad799x.h
- */
-
-#ifndef _AD799X_H_
-#define  _AD799X_H_
-
-#define AD799X_CHANNEL_SHIFT			4
-#define AD799X_STORAGEBITS			16
-/*
- * AD7991, AD7995 and AD7999 defines
- */
-
-#define AD7991_REF_SEL				0x08
-#define AD7991_FLTR				0x04
-#define AD7991_BIT_TRIAL_DELAY			0x02
-#define AD7991_SAMPLE_DELAY			0x01
-
-/*
- * AD7992, AD7993, AD7994, AD7997 and AD7998 defines
- */
-
-#define AD7998_FLTR				0x08
-#define AD7998_ALERT_EN				0x04
-#define AD7998_BUSY_ALERT			0x02
-#define AD7998_BUSY_ALERT_POL			0x01
-
-#define AD7998_CONV_RES_REG			0x0
-#define AD7998_ALERT_STAT_REG			0x1
-#define AD7998_CONF_REG				0x2
-#define AD7998_CYCLE_TMR_REG			0x3
-
-#define AD7998_DATALOW_REG(x)			((x) * 3 + 0x4)
-#define AD7998_DATAHIGH_REG(x)			((x) * 3 + 0x5)
-#define AD7998_HYST_REG(x)			((x) * 3 + 0x6)
-
-#define AD7998_CYC_MASK				0x7
-#define AD7998_CYC_DIS				0x0
-#define AD7998_CYC_TCONF_32			0x1
-#define AD7998_CYC_TCONF_64			0x2
-#define AD7998_CYC_TCONF_128			0x3
-#define AD7998_CYC_TCONF_256			0x4
-#define AD7998_CYC_TCONF_512			0x5
-#define AD7998_CYC_TCONF_1024			0x6
-#define AD7998_CYC_TCONF_2048			0x7
-
-#define AD7998_ALERT_STAT_CLEAR			0xFF
-
-/*
- * AD7997 and AD7997 defines
- */
-
-#define AD7997_8_READ_SINGLE			0x80
-#define AD7997_8_READ_SEQUENCE			0x70
-/* TODO: move this into a common header */
-#define RES_MASK(bits)	((1 << (bits)) - 1)
-
-enum {
-	ad7991,
-	ad7995,
-	ad7999,
-	ad7992,
-	ad7993,
-	ad7994,
-	ad7997,
-	ad7998
-};
-
-struct ad799x_state;
-
-/**
- * struct ad799x_chip_info - chip specifc information
- * @channel:		channel specification
- * @num_channels:	number of channels
- * @monitor_mode:	whether the chip supports monitor interrupts
- * @default_config:	device default configuration
- * @event_attrs:	pointer to the monitor event attribute group
- */
-
-struct ad799x_chip_info {
-	struct iio_chan_spec		channel[9];
-	int				num_channels;
-	u16				default_config;
-	const struct iio_info		*info;
-};
-
-struct ad799x_state {
-	struct i2c_client		*client;
-	const struct ad799x_chip_info	*chip_info;
-	struct regulator		*reg;
-	struct regulator		*vref;
-	unsigned			id;
-	u16				config;
-
-	u8				*rx_buf;
-	unsigned int			transfer_size;
-};
-
-#ifdef CONFIG_AD799X_RING_BUFFER
-int ad799x_register_ring_funcs_and_init(struct iio_dev *indio_dev);
-void ad799x_ring_cleanup(struct iio_dev *indio_dev);
-#else /* CONFIG_AD799X_RING_BUFFER */
-
-static inline int
-ad799x_register_ring_funcs_and_init(struct iio_dev *indio_dev)
-{
-	return 0;
-}
-
-static inline void ad799x_ring_cleanup(struct iio_dev *indio_dev)
-{
-}
-#endif /* CONFIG_AD799X_RING_BUFFER */
-#endif /* _AD799X_H_ */
diff --git a/drivers/staging/iio/adc/ad799x_ring.c b/drivers/staging/iio/adc/ad799x_ring.c
deleted file mode 100644
index 0ff6c03a483..00000000000
--- a/drivers/staging/iio/adc/ad799x_ring.c
+++ /dev/null
@@ -1,84 +0,0 @@
-/*
- * Copyright (C) 2010-2012 Michael Hennerich, Analog Devices Inc.
- * Copyright (C) 2008-2010 Jonathan Cameron
- *
- * 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.
- *
- * ad799x_ring.c
- */
-
-#include <linux/interrupt.h>
-#include <linux/slab.h>
-#include <linux/kernel.h>
-#include <linux/list.h>
-#include <linux/i2c.h>
-#include <linux/bitops.h>
-
-#include <linux/iio/iio.h>
-#include <linux/iio/buffer.h>
-#include <linux/iio/trigger_consumer.h>
-#include <linux/iio/triggered_buffer.h>
-
-#include "ad799x.h"
-
-/**
- * ad799x_trigger_handler() bh of trigger launched polling to ring buffer
- *
- * Currently there is no option in this driver to disable the saving of
- * timestamps within the ring.
- **/
-
-static irqreturn_t ad799x_trigger_handler(int irq, void *p)
-{
-	struct iio_poll_func *pf = p;
-	struct iio_dev *indio_dev = pf->indio_dev;
-	struct ad799x_state *st = iio_priv(indio_dev);
-	int b_sent;
-	u8 cmd;
-
-	switch (st->id) {
-	case ad7991:
-	case ad7995:
-	case ad7999:
-		cmd = st->config |
-			(*indio_dev->active_scan_mask << AD799X_CHANNEL_SHIFT);
-		break;
-	case ad7992:
-	case ad7993:
-	case ad7994:
-		cmd = (*indio_dev->active_scan_mask << AD799X_CHANNEL_SHIFT) |
-			AD7998_CONV_RES_REG;
-		break;
-	case ad7997:
-	case ad7998:
-		cmd = AD7997_8_READ_SEQUENCE | AD7998_CONV_RES_REG;
-		break;
-	default:
-		cmd = 0;
-	}
-
-	b_sent = i2c_smbus_read_i2c_block_data(st->client,
-			cmd, st->transfer_size, st->rx_buf);
-	if (b_sent < 0)
-		goto out;
-
-	iio_push_to_buffers_with_timestamp(indio_dev, st->rx_buf,
-			iio_get_time_ns());
-out:
-	iio_trigger_notify_done(indio_dev->trig);
-
-	return IRQ_HANDLED;
-}
-
-int ad799x_register_ring_funcs_and_init(struct iio_dev *indio_dev)
-{
-	return iio_triggered_buffer_setup(indio_dev, NULL,
-		&ad799x_trigger_handler, NULL);
-}
-
-void ad799x_ring_cleanup(struct iio_dev *indio_dev)
-{
-	iio_triggered_buffer_cleanup(indio_dev);
-}
diff --git a/drivers/staging/iio/adc/spear_adc.c b/drivers/staging/iio/adc/spear_adc.c
index 970d9edc73b..c5492ba5075 100644
--- a/drivers/staging/iio/adc/spear_adc.c
+++ b/drivers/staging/iio/adc/spear_adc.c
@@ -22,39 +22,36 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 
-/*
- * SPEAR registers definitions
- */
-
-#define SCAN_RATE_LO(x)		((x) & 0xFFFF)
-#define SCAN_RATE_HI(x)		(((x) >> 0x10) & 0xFFFF)
-#define CLK_LOW(x)		(((x) & 0xf) << 0)
-#define CLK_HIGH(x)		(((x) & 0xf) << 4)
+/* SPEAR registers definitions */
+#define SPEAR600_ADC_SCAN_RATE_LO(x)	((x) & 0xFFFF)
+#define SPEAR600_ADC_SCAN_RATE_HI(x)	(((x) >> 0x10) & 0xFFFF)
+#define SPEAR_ADC_CLK_LOW(x)		(((x) & 0xf) << 0)
+#define SPEAR_ADC_CLK_HIGH(x)		(((x) & 0xf) << 4)
 
 /* Bit definitions for SPEAR_ADC_STATUS */
-#define START_CONVERSION	(1 << 0)
-#define CHANNEL_NUM(x)		((x) << 1)
-#define ADC_ENABLE		(1 << 4)
-#define AVG_SAMPLE(x)		((x) << 5)
-#define VREF_INTERNAL		(1 << 9)
+#define SPEAR_ADC_STATUS_START_CONVERSION	(1 << 0)
+#define SPEAR_ADC_STATUS_CHANNEL_NUM(x)		((x) << 1)
+#define SPEAR_ADC_STATUS_ADC_ENABLE		(1 << 4)
+#define SPEAR_ADC_STATUS_AVG_SAMPLE(x)		((x) << 5)
+#define SPEAR_ADC_STATUS_VREF_INTERNAL		(1 << 9)
 
-#define DATA_MASK		0x03ff
-#define DATA_BITS		10
+#define SPEAR_ADC_DATA_MASK		0x03ff
+#define SPEAR_ADC_DATA_BITS		10
 
-#define MOD_NAME "spear-adc"
+#define SPEAR_ADC_MOD_NAME "spear-adc"
 
-#define ADC_CHANNEL_NUM		8
+#define SPEAR_ADC_CHANNEL_NUM		8
 
-#define CLK_MIN			2500000
-#define CLK_MAX			20000000
+#define SPEAR_ADC_CLK_MIN			2500000
+#define SPEAR_ADC_CLK_MAX			20000000
 
 struct adc_regs_spear3xx {
 	u32 status;
 	u32 average;
 	u32 scan_rate;
 	u32 clk;	/* Not avail for 1340 & 1310 */
-	u32 ch_ctrl[ADC_CHANNEL_NUM];
-	u32 ch_data[ADC_CHANNEL_NUM];
+	u32 ch_ctrl[SPEAR_ADC_CHANNEL_NUM];
+	u32 ch_data[SPEAR_ADC_CHANNEL_NUM];
 };
 
 struct chan_data {
@@ -66,14 +63,14 @@ struct adc_regs_spear6xx {
 	u32 status;
 	u32 pad[2];
 	u32 clk;
-	u32 ch_ctrl[ADC_CHANNEL_NUM];
-	struct chan_data ch_data[ADC_CHANNEL_NUM];
+	u32 ch_ctrl[SPEAR_ADC_CHANNEL_NUM];
+	struct chan_data ch_data[SPEAR_ADC_CHANNEL_NUM];
 	u32 scan_rate_lo;
 	u32 scan_rate_hi;
 	struct chan_data average;
 };
 
-struct spear_adc_info {
+struct spear_adc_state {
 	struct device_node *np;
 	struct adc_regs_spear3xx __iomem *adc_base_spear3xx;
 	struct adc_regs_spear6xx __iomem *adc_base_spear6xx;
@@ -91,100 +88,129 @@ struct spear_adc_info {
  * static inline functions, because of different register offsets
  * on different SoC variants (SPEAr300 vs SPEAr600 etc).
  */
-static void spear_adc_set_status(struct spear_adc_info *info, u32 val)
+static void spear_adc_set_status(struct spear_adc_state *st, u32 val)
 {
-	__raw_writel(val, &info->adc_base_spear6xx->status);
+	__raw_writel(val, &st->adc_base_spear6xx->status);
 }
 
-static void spear_adc_set_clk(struct spear_adc_info *info, u32 val)
+static void spear_adc_set_clk(struct spear_adc_state *st, u32 val)
 {
 	u32 clk_high, clk_low, count;
-	u32 apb_clk = clk_get_rate(info->clk);
+	u32 apb_clk = clk_get_rate(st->clk);
 
 	count = (apb_clk + val - 1) / val;
 	clk_low = count / 2;
 	clk_high = count - clk_low;
-	info->current_clk = apb_clk / count;
+	st->current_clk = apb_clk / count;
 
-	__raw_writel(CLK_LOW(clk_low) | CLK_HIGH(clk_high),
-		     &info->adc_base_spear6xx->clk);
+	__raw_writel(SPEAR_ADC_CLK_LOW(clk_low) | SPEAR_ADC_CLK_HIGH(clk_high),
+		     &st->adc_base_spear6xx->clk);
 }
 
-static void spear_adc_set_ctrl(struct spear_adc_info *info, int n,
+static void spear_adc_set_ctrl(struct spear_adc_state *st, int n,
 			       u32 val)
 {
-	__raw_writel(val, &info->adc_base_spear6xx->ch_ctrl[n]);
+	__raw_writel(val, &st->adc_base_spear6xx->ch_ctrl[n]);
 }
 
-static u32 spear_adc_get_average(struct spear_adc_info *info)
+static u32 spear_adc_get_average(struct spear_adc_state *st)
 {
-	if (of_device_is_compatible(info->np, "st,spear600-adc")) {
-		return __raw_readl(&info->adc_base_spear6xx->average.msb) &
-			DATA_MASK;
+	if (of_device_is_compatible(st->np, "st,spear600-adc")) {
+		return __raw_readl(&st->adc_base_spear6xx->average.msb) &
+			SPEAR_ADC_DATA_MASK;
 	} else {
-		return __raw_readl(&info->adc_base_spear3xx->average) &
-			DATA_MASK;
+		return __raw_readl(&st->adc_base_spear3xx->average) &
+			SPEAR_ADC_DATA_MASK;
 	}
 }
 
-static void spear_adc_set_scanrate(struct spear_adc_info *info, u32 rate)
+static void spear_adc_set_scanrate(struct spear_adc_state *st, u32 rate)
 {
-	if (of_device_is_compatible(info->np, "st,spear600-adc")) {
-		__raw_writel(SCAN_RATE_LO(rate),
-			     &info->adc_base_spear6xx->scan_rate_lo);
-		__raw_writel(SCAN_RATE_HI(rate),
-			     &info->adc_base_spear6xx->scan_rate_hi);
+	if (of_device_is_compatible(st->np, "st,spear600-adc")) {
+		__raw_writel(SPEAR600_ADC_SCAN_RATE_LO(rate),
+			     &st->adc_base_spear6xx->scan_rate_lo);
+		__raw_writel(SPEAR600_ADC_SCAN_RATE_HI(rate),
+			     &st->adc_base_spear6xx->scan_rate_hi);
 	} else {
-		__raw_writel(rate, &info->adc_base_spear3xx->scan_rate);
+		__raw_writel(rate, &st->adc_base_spear3xx->scan_rate);
 	}
 }
 
-static int spear_read_raw(struct iio_dev *indio_dev,
-			  struct iio_chan_spec const *chan,
-			  int *val,
-			  int *val2,
-			  long mask)
+static int spear_adc_read_raw(struct iio_dev *indio_dev,
+			      struct iio_chan_spec const *chan,
+			      int *val,
+			      int *val2,
+			      long mask)
 {
-	struct spear_adc_info *info = iio_priv(indio_dev);
+	struct spear_adc_state *st = iio_priv(indio_dev);
 	u32 status;
 
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW:
 		mutex_lock(&indio_dev->mlock);
 
-		status = CHANNEL_NUM(chan->channel) |
-			AVG_SAMPLE(info->avg_samples) |
-			START_CONVERSION | ADC_ENABLE;
-		if (info->vref_external == 0)
-			status |= VREF_INTERNAL;
+		status = SPEAR_ADC_STATUS_CHANNEL_NUM(chan->channel) |
+			SPEAR_ADC_STATUS_AVG_SAMPLE(st->avg_samples) |
+			SPEAR_ADC_STATUS_START_CONVERSION |
+			SPEAR_ADC_STATUS_ADC_ENABLE;
+		if (st->vref_external == 0)
+			status |= SPEAR_ADC_STATUS_VREF_INTERNAL;
 
-		spear_adc_set_status(info, status);
-		wait_for_completion(&info->completion); /* set by ISR */
-		*val = info->value;
+		spear_adc_set_status(st, status);
+		wait_for_completion(&st->completion); /* set by ISR */
+		*val = st->value;
 
 		mutex_unlock(&indio_dev->mlock);
 
 		return IIO_VAL_INT;
 
 	case IIO_CHAN_INFO_SCALE:
-		*val = info->vref_external;
-		*val2 = DATA_BITS;
+		*val = st->vref_external;
+		*val2 = SPEAR_ADC_DATA_BITS;
 		return IIO_VAL_FRACTIONAL_LOG2;
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		*val = st->current_clk;
+		return IIO_VAL_INT;
 	}
 
 	return -EINVAL;
 }
 
+static int spear_adc_write_raw(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       int val,
+			       int val2,
+			       long mask)
+{
+	struct spear_adc_state *st = iio_priv(indio_dev);
+	int ret = 0;
+
+	if (mask != IIO_CHAN_INFO_SAMP_FREQ)
+		return -EINVAL;
+
+	mutex_lock(&indio_dev->mlock);
+
+	if ((val < SPEAR_ADC_CLK_MIN) ||
+		(val > SPEAR_ADC_CLK_MAX) ||
+		(val2 != 0)) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	spear_adc_set_clk(st, val);
+
+out:
+	mutex_unlock(&indio_dev->mlock);
+	return ret;
+}
+
 #define SPEAR_ADC_CHAN(idx) {				\
 	.type = IIO_VOLTAGE,				\
 	.indexed = 1,					\
 	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),	\
 	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),	\
+	.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ),\
 	.channel = idx,					\
-	.scan_type = {					\
-		.sign = 'u',				\
-		.storagebits = 16,			\
-	},						\
 }
 
 static const struct iio_chan_spec spear_adc_iio_channels[] = {
@@ -200,92 +226,34 @@ static const struct iio_chan_spec spear_adc_iio_channels[] = {
 
 static irqreturn_t spear_adc_isr(int irq, void *dev_id)
 {
-	struct spear_adc_info *info = (struct spear_adc_info *)dev_id;
+	struct spear_adc_state *st = (struct spear_adc_state *)dev_id;
 
 	/* Read value to clear IRQ */
-	info->value = spear_adc_get_average(info);
-	complete(&info->completion);
+	st->value = spear_adc_get_average(st);
+	complete(&st->completion);
 
 	return IRQ_HANDLED;
 }
 
-static int spear_adc_configure(struct spear_adc_info *info)
+static int spear_adc_configure(struct spear_adc_state *st)
 {
 	int i;
 
 	/* Reset ADC core */
-	spear_adc_set_status(info, 0);
-	__raw_writel(0, &info->adc_base_spear6xx->clk);
+	spear_adc_set_status(st, 0);
+	__raw_writel(0, &st->adc_base_spear6xx->clk);
 	for (i = 0; i < 8; i++)
-		spear_adc_set_ctrl(info, i, 0);
-	spear_adc_set_scanrate(info, 0);
+		spear_adc_set_ctrl(st, i, 0);
+	spear_adc_set_scanrate(st, 0);
 
-	spear_adc_set_clk(info, info->sampling_freq);
+	spear_adc_set_clk(st, st->sampling_freq);
 
 	return 0;
 }
 
-static ssize_t spear_adc_read_frequency(struct device *dev,
-					struct device_attribute *attr,
-					char *buf)
-{
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
-	struct spear_adc_info *info = iio_priv(indio_dev);
-
-	return sprintf(buf, "%d\n", info->current_clk);
-}
-
-static ssize_t spear_adc_write_frequency(struct device *dev,
-					 struct device_attribute *attr,
-					 const char *buf,
-					 size_t len)
-{
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
-	struct spear_adc_info *info = iio_priv(indio_dev);
-	u32 clk_high, clk_low, count;
-	u32 apb_clk = clk_get_rate(info->clk);
-	unsigned long lval;
-	int ret;
-
-	ret = kstrtoul(buf, 10, &lval);
-	if (ret)
-		return ret;
-
-	mutex_lock(&indio_dev->mlock);
-
-	if ((lval < CLK_MIN) || (lval > CLK_MAX)) {
-		ret = -EINVAL;
-		goto out;
-	}
-
-	count = (apb_clk + lval - 1) / lval;
-	clk_low = count / 2;
-	clk_high = count - clk_low;
-	info->current_clk = apb_clk / count;
-	spear_adc_set_clk(info, lval);
-
-out:
-	mutex_unlock(&indio_dev->mlock);
-
-	return ret ? ret : len;
-}
-
-static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO,
-			      spear_adc_read_frequency,
-			      spear_adc_write_frequency);
-
-static struct attribute *spear_attributes[] = {
-	&iio_dev_attr_sampling_frequency.dev_attr.attr,
-	NULL
-};
-
-static const struct attribute_group spear_attribute_group = {
-	.attrs = spear_attributes,
-};
-
-static const struct iio_info spear_adc_iio_info = {
-	.read_raw = &spear_read_raw,
-	.attrs = &spear_attribute_group,
+static const struct iio_info spear_adc_info = {
+	.read_raw = &spear_adc_read_raw,
+	.write_raw = &spear_adc_write_raw,
 	.driver_module = THIS_MODULE,
 };
 
@@ -293,40 +261,40 @@ static int spear_adc_probe(struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
 	struct device *dev = &pdev->dev;
-	struct spear_adc_info *info;
-	struct iio_dev *iodev = NULL;
+	struct spear_adc_state *st;
+	struct iio_dev *indio_dev = NULL;
 	int ret = -ENODEV;
 	int irq;
 
-	iodev = devm_iio_device_alloc(dev, sizeof(struct spear_adc_info));
-	if (!iodev) {
+	indio_dev = devm_iio_device_alloc(dev, sizeof(struct spear_adc_state));
+	if (!indio_dev) {
 		dev_err(dev, "failed allocating iio device\n");
 		return -ENOMEM;
 	}
 
-	info = iio_priv(iodev);
-	info->np = np;
+	st = iio_priv(indio_dev);
+	st->np = np;
 
 	/*
 	 * SPEAr600 has a different register layout than other SPEAr SoC's
 	 * (e.g. SPEAr3xx). Let's provide two register base addresses
 	 * to support multi-arch kernels.
 	 */
-	info->adc_base_spear6xx = of_iomap(np, 0);
-	if (!info->adc_base_spear6xx) {
+	st->adc_base_spear6xx = of_iomap(np, 0);
+	if (!st->adc_base_spear6xx) {
 		dev_err(dev, "failed mapping memory\n");
 		return -ENOMEM;
 	}
-	info->adc_base_spear3xx =
-		(struct adc_regs_spear3xx __iomem *)info->adc_base_spear6xx;
+	st->adc_base_spear3xx =
+		(struct adc_regs_spear3xx __iomem *)st->adc_base_spear6xx;
 
-	info->clk = clk_get(dev, NULL);
-	if (IS_ERR(info->clk)) {
+	st->clk = clk_get(dev, NULL);
+	if (IS_ERR(st->clk)) {
 		dev_err(dev, "failed getting clock\n");
 		goto errout1;
 	}
 
-	ret = clk_prepare_enable(info->clk);
+	ret = clk_prepare_enable(st->clk);
 	if (ret) {
 		dev_err(dev, "failed enabling clock\n");
 		goto errout2;
@@ -339,14 +307,15 @@ static int spear_adc_probe(struct platform_device *pdev)
 		goto errout3;
 	}
 
-	ret = devm_request_irq(dev, irq, spear_adc_isr, 0, MOD_NAME, info);
+	ret = devm_request_irq(dev, irq, spear_adc_isr, 0, SPEAR_ADC_MOD_NAME,
+			       st);
 	if (ret < 0) {
 		dev_err(dev, "failed requesting interrupt\n");
 		goto errout3;
 	}
 
 	if (of_property_read_u32(np, "sampling-frequency",
-				 &info->sampling_freq)) {
+				 &st->sampling_freq)) {
 		dev_err(dev, "sampling-frequency missing in DT\n");
 		ret = -EINVAL;
 		goto errout3;
@@ -356,28 +325,28 @@ static int spear_adc_probe(struct platform_device *pdev)
 	 * Optional avg_samples defaults to 0, resulting in single data
 	 * conversion
 	 */
-	of_property_read_u32(np, "average-samples", &info->avg_samples);
+	of_property_read_u32(np, "average-samples", &st->avg_samples);
 
 	/*
 	 * Optional vref_external defaults to 0, resulting in internal vref
 	 * selection
 	 */
-	of_property_read_u32(np, "vref-external", &info->vref_external);
+	of_property_read_u32(np, "vref-external", &st->vref_external);
 
-	spear_adc_configure(info);
+	spear_adc_configure(st);
 
-	platform_set_drvdata(pdev, iodev);
+	platform_set_drvdata(pdev, indio_dev);
 
-	init_completion(&info->completion);
+	init_completion(&st->completion);
 
-	iodev->name = MOD_NAME;
-	iodev->dev.parent = dev;
-	iodev->info = &spear_adc_iio_info;
-	iodev->modes = INDIO_DIRECT_MODE;
-	iodev->channels = spear_adc_iio_channels;
-	iodev->num_channels = ARRAY_SIZE(spear_adc_iio_channels);
+	indio_dev->name = SPEAR_ADC_MOD_NAME;
+	indio_dev->dev.parent = dev;
+	indio_dev->info = &spear_adc_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = spear_adc_iio_channels;
+	indio_dev->num_channels = ARRAY_SIZE(spear_adc_iio_channels);
 
-	ret = iio_device_register(iodev);
+	ret = iio_device_register(indio_dev);
 	if (ret)
 		goto errout3;
 
@@ -386,23 +355,23 @@ static int spear_adc_probe(struct platform_device *pdev)
 	return 0;
 
 errout3:
-	clk_disable_unprepare(info->clk);
+	clk_disable_unprepare(st->clk);
 errout2:
-	clk_put(info->clk);
+	clk_put(st->clk);
 errout1:
-	iounmap(info->adc_base_spear6xx);
+	iounmap(st->adc_base_spear6xx);
 	return ret;
 }
 
 static int spear_adc_remove(struct platform_device *pdev)
 {
-	struct iio_dev *iodev = platform_get_drvdata(pdev);
-	struct spear_adc_info *info = iio_priv(iodev);
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct spear_adc_state *st = iio_priv(indio_dev);
 
-	iio_device_unregister(iodev);
-	clk_disable_unprepare(info->clk);
-	clk_put(info->clk);
-	iounmap(info->adc_base_spear6xx);
+	iio_device_unregister(indio_dev);
+	clk_disable_unprepare(st->clk);
+	clk_put(st->clk);
+	iounmap(st->adc_base_spear6xx);
 
 	return 0;
 }
@@ -419,7 +388,7 @@ static struct platform_driver spear_adc_driver = {
 	.probe		= spear_adc_probe,
 	.remove		= spear_adc_remove,
 	.driver		= {
-		.name	= MOD_NAME,
+		.name	= SPEAR_ADC_MOD_NAME,
 		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(spear_adc_dt_ids),
 	},
diff --git a/drivers/staging/iio/addac/adt7316.c b/drivers/staging/iio/addac/adt7316.c
index 9f0ebb32900..5f1770e6f6c 100644
--- a/drivers/staging/iio/addac/adt7316.c
+++ b/drivers/staging/iio/addac/adt7316.c
@@ -172,7 +172,7 @@
 #define ID_ADT75XX		0x10
 
 /*
- * struct adt7316_chip_info - chip specifc information
+ * struct adt7316_chip_info - chip specific information
  */
 
 struct adt7316_chip_info {
@@ -208,7 +208,7 @@ struct adt7316_chip_info {
 	(ADT7316_TEMP_INT_MASK)
 
 /*
- * struct adt7316_chip_info - chip specifc information
+ * struct adt7316_chip_info - chip specific information
  */
 
 struct adt7316_limit_regs {
diff --git a/drivers/staging/iio/cdc/ad7152.c b/drivers/staging/iio/cdc/ad7152.c
index f2c309d1eb5..87110d940e9 100644
--- a/drivers/staging/iio/cdc/ad7152.c
+++ b/drivers/staging/iio/cdc/ad7152.c
@@ -78,7 +78,7 @@ enum {
 };
 
 /*
- * struct ad7152_chip_info - chip specifc information
+ * struct ad7152_chip_info - chip specific information
  */
 
 struct ad7152_chip_info {
diff --git a/drivers/staging/iio/cdc/ad7746.c b/drivers/staging/iio/cdc/ad7746.c
index cbb1588d591..e6e9eaa9eab 100644
--- a/drivers/staging/iio/cdc/ad7746.c
+++ b/drivers/staging/iio/cdc/ad7746.c
@@ -91,7 +91,7 @@
 #define AD7746_CAPDAC_DACP(x)		((x) & 0x7F)
 
 /*
- * struct ad7746_chip_info - chip specifc information
+ * struct ad7746_chip_info - chip specific information
  */
 
 struct ad7746_chip_info {
diff --git a/drivers/staging/iio/light/tsl2583.c b/drivers/staging/iio/light/tsl2583.c
index 0a60def9273..fa964987966 100644
--- a/drivers/staging/iio/light/tsl2583.c
+++ b/drivers/staging/iio/light/tsl2583.c
@@ -165,8 +165,9 @@ taos_i2c_read(struct i2c_client *client, u8 reg, u8 *val, unsigned int len)
 		/* select register to write */
 		ret = i2c_smbus_write_byte(client, (TSL258X_CMD_REG | reg));
 		if (ret < 0) {
-			dev_err(&client->dev, "taos_i2c_read failed to write"
-				" register %x\n", reg);
+			dev_err(&client->dev,
+				"taos_i2c_read failed to write register %x\n",
+				reg);
 			return ret;
 		}
 		/* read the data */
@@ -211,7 +212,7 @@ static int taos_get_lux(struct iio_dev *indio_dev)
 	if (chip->taos_chip_status != TSL258X_CHIP_WORKING) {
 		/* device is not enabled */
 		dev_err(&chip->client->dev, "taos_get_lux device is not enabled\n");
-		ret = -EBUSY ;
+		ret = -EBUSY;
 		goto out_unlock;
 	}
 
@@ -231,8 +232,9 @@ static int taos_get_lux(struct iio_dev *indio_dev)
 		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, "taos_get_lux failed to read"
-				" register %x\n", reg);
+			dev_err(&chip->client->dev,
+				"taos_get_lux failed to read register %x\n",
+				reg);
 			goto out_unlock;
 		}
 	}
@@ -449,7 +451,7 @@ static int taos_chip_on(struct iio_dev *indio_dev)
 		}
 	}
 
-	msleep(3);
+	usleep_range(3000, 3500);
 	/* NOW enable the ADC
 	 * initialize the desired mode of operation */
 	utmp = TSL258X_CNTL_PWR_ON | TSL258X_CNTL_ADC_ENBL;
@@ -809,9 +811,7 @@ static int taos_probe(struct i2c_client *clientp,
 
 	if (!i2c_check_functionality(clientp->adapter,
 		I2C_FUNC_SMBUS_BYTE_DATA)) {
-		dev_err(&clientp->dev,
-			"taos_probe() - i2c smbus byte data "
-			"functions unsupported\n");
+		dev_err(&clientp->dev, "taos_probe() - i2c smbus byte data func unsupported\n");
 		return -EOPNOTSUPP;
 	}
 
@@ -830,30 +830,32 @@ static int taos_probe(struct i2c_client *clientp,
 		ret = i2c_smbus_write_byte(clientp,
 				(TSL258X_CMD_REG | (TSL258X_CNTRL + i)));
 		if (ret < 0) {
-			dev_err(&clientp->dev, "i2c_smbus_write_bytes() to cmd "
-				"reg failed in taos_probe(), err = %d\n", ret);
+			dev_err(&clientp->dev,
+				"i2c_smbus_write_byte to cmd reg failed in taos_probe(), err = %d\n",
+				ret);
 			return ret;
 		}
 		ret = i2c_smbus_read_byte(clientp);
 		if (ret < 0) {
-			dev_err(&clientp->dev, "i2c_smbus_read_byte from "
-				"reg failed in taos_probe(), err = %d\n", ret);
-
+			dev_err(&clientp->dev,
+				"i2c_smbus_read_byte from reg failed in taos_probe(), err = %d\n",
+				ret);
 			return ret;
 		}
 		buf[i] = ret;
 	}
 
 	if (!taos_tsl258x_device(buf)) {
-		dev_info(&clientp->dev, "i2c device found but does not match "
-			"expected id in taos_probe()\n");
+		dev_info(&clientp->dev,
+			"i2c device found but does not match expected id in taos_probe()\n");
 		return -EINVAL;
 	}
 
 	ret = i2c_smbus_write_byte(clientp, (TSL258X_CMD_REG | TSL258X_CNTRL));
 	if (ret < 0) {
-		dev_err(&clientp->dev, "i2c_smbus_write_byte() to cmd reg "
-			"failed in taos_probe(), err = %d\n", ret);
+		dev_err(&clientp->dev,
+			"i2c_smbus_write_byte() to cmd reg failed in taos_probe(), err = %d\n",
+			ret);
 		return ret;
 	}
 
diff --git a/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c b/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
index 48a6afa8408..38ecb4bb6e4 100644
--- a/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
+++ b/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
@@ -33,7 +33,8 @@ 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);
 	if (trig_info->frequency == 0)
 		return -EINVAL;
-	printk(KERN_INFO "trigger frequency is %d\n", trig_info->frequency);
+	dev_info(&trig_info->rtc->dev, "trigger frequency is %d\n",
+			trig_info->frequency);
 	return rtc_irq_set_state(trig_info->rtc, &trig_info->task, state);
 }
 
diff --git a/drivers/staging/imx-drm/imx-drm-core.c b/drivers/staging/imx-drm/imx-drm-core.c
index 4144a75e5f7..c270c9ae6d2 100644
--- a/drivers/staging/imx-drm/imx-drm-core.c
+++ b/drivers/staging/imx-drm/imx-drm-core.c
@@ -517,7 +517,7 @@ int imx_drm_encoder_get_mux_id(struct device_node *node,
 		of_node_put(port);
 		if (port == imx_crtc->port) {
 			ret = of_graph_parse_endpoint(ep, &endpoint);
-			return ret ? ret : endpoint.id;
+			return ret ? ret : endpoint.port;
 		}
 	} while (ep);
 
@@ -675,6 +675,11 @@ static int imx_drm_platform_probe(struct platform_device *pdev)
 			if (!remote || !of_device_is_available(remote)) {
 				of_node_put(remote);
 				continue;
+			} else if (!of_device_is_available(remote->parent)) {
+				dev_warn(&pdev->dev, "parent device of %s is not available\n",
+					 remote->full_name);
+				of_node_put(remote);
+				continue;
 			}
 
 			ret = imx_drm_add_component(&pdev->dev, remote);
diff --git a/drivers/staging/imx-drm/imx-hdmi.c b/drivers/staging/imx-drm/imx-hdmi.c
index d47dedd2cdb..1b440483f28 100644
--- a/drivers/staging/imx-drm/imx-hdmi.c
+++ b/drivers/staging/imx-drm/imx-hdmi.c
@@ -120,8 +120,6 @@ struct imx_hdmi {
 	struct clk *isfr_clk;
 	struct clk *iahb_clk;
 
-	enum drm_connector_status connector_status;
-
 	struct hdmi_data_info hdmi_data;
 	int vic;
 
@@ -159,6 +157,7 @@ static inline u8 hdmi_readb(struct imx_hdmi *hdmi, int offset)
 static void hdmi_modb(struct imx_hdmi *hdmi, u8 data, u8 mask, unsigned reg)
 {
 	u8 val = hdmi_readb(hdmi, reg) & ~mask;
+
 	val |= data & mask;
 	hdmi_writeb(hdmi, val, reg);
 }
@@ -659,13 +658,10 @@ static inline void hdmi_phy_test_dout(struct imx_hdmi *hdmi,
 
 static bool hdmi_phy_wait_i2c_done(struct imx_hdmi *hdmi, int msec)
 {
-	unsigned char val = 0;
-	val = hdmi_readb(hdmi, HDMI_IH_I2CMPHY_STAT0) & 0x3;
-	while (!val) {
-		udelay(1000);
+	while ((hdmi_readb(hdmi, HDMI_IH_I2CMPHY_STAT0) & 0x3) == 0) {
 		if (msec-- == 0)
 			return false;
-		val = hdmi_readb(hdmi, HDMI_IH_I2CMPHY_STAT0) & 0x3;
+		udelay(1000);
 	}
 	return true;
 }
@@ -1382,7 +1378,9 @@ static enum drm_connector_status imx_hdmi_connector_detect(struct drm_connector
 {
 	struct imx_hdmi *hdmi = container_of(connector, struct imx_hdmi,
 					     connector);
-	return hdmi->connector_status;
+
+	return hdmi_readb(hdmi, HDMI_PHY_STAT0) & HDMI_PHY_HPD ?
+		connector_status_connected : connector_status_disconnected;
 }
 
 static int imx_hdmi_connector_get_modes(struct drm_connector *connector)
@@ -1524,7 +1522,6 @@ static irqreturn_t imx_hdmi_irq(int irq, void *dev_id)
 
 			hdmi_modb(hdmi, 0, HDMI_PHY_HPD, HDMI_PHY_POL0);
 
-			hdmi->connector_status = connector_status_connected;
 			imx_hdmi_poweron(hdmi);
 		} else {
 			dev_dbg(hdmi->dev, "EVENT=plugout\n");
@@ -1532,7 +1529,6 @@ static irqreturn_t imx_hdmi_irq(int irq, void *dev_id)
 			hdmi_modb(hdmi, HDMI_PHY_HPD, HDMI_PHY_HPD,
 				HDMI_PHY_POL0);
 
-			hdmi->connector_status = connector_status_disconnected;
 			imx_hdmi_poweroff(hdmi);
 		}
 		drm_helper_hpd_irq_event(hdmi->connector.dev);
@@ -1606,12 +1602,12 @@ static int imx_hdmi_bind(struct device *dev, struct device *master, void *data)
 		return -ENOMEM;
 
 	hdmi->dev = dev;
-	hdmi->connector_status = connector_status_disconnected;
 	hdmi->sample_rate = 48000;
 	hdmi->ratio = 100;
 
 	if (of_id) {
 		const struct platform_device_id *device_id = of_id->data;
+
 		hdmi->dev_type = device_id->driver_data;
 	}
 
@@ -1628,7 +1624,7 @@ static int imx_hdmi_bind(struct device *dev, struct device *master, void *data)
 
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0)
-		return -EINVAL;
+		return irq;
 
 	ret = devm_request_threaded_irq(dev, irq, imx_hdmi_hardirq,
 					imx_hdmi_irq, IRQF_SHARED,
diff --git a/drivers/staging/imx-drm/imx-tve.c b/drivers/staging/imx-drm/imx-tve.c
index 575533f4fd6..a23f4f77314 100644
--- a/drivers/staging/imx-drm/imx-tve.c
+++ b/drivers/staging/imx-drm/imx-tve.c
@@ -582,7 +582,7 @@ static int imx_tve_bind(struct device *dev, struct device *master, void *data)
 	tve->dev = dev;
 	spin_lock_init(&tve->lock);
 
-	ddc_node = of_parse_phandle(np, "i2c-ddc-bus", 0);
+	ddc_node = of_parse_phandle(np, "ddc-i2c-bus", 0);
 	if (ddc_node) {
 		tve->ddc = of_find_i2c_adapter_by_node(ddc_node);
 		of_node_put(ddc_node);
diff --git a/drivers/staging/imx-drm/ipu-v3/imx-ipu-v3.h b/drivers/staging/imx-drm/ipu-v3/imx-ipu-v3.h
index c4d14ead583..c2c6fab05ea 100644
--- a/drivers/staging/imx-drm/ipu-v3/imx-ipu-v3.h
+++ b/drivers/staging/imx-drm/ipu-v3/imx-ipu-v3.h
@@ -76,6 +76,7 @@ enum ipu_channel_irq {
 	IPU_IRQ_EOS = 192,
 };
 
+int ipu_map_irq(struct ipu_soc *ipu, int irq);
 int ipu_idmac_channel_irq(struct ipu_soc *ipu, struct ipuv3_channel *channel,
 		enum ipu_channel_irq irq);
 
@@ -114,8 +115,10 @@ struct ipu_dc *ipu_dc_get(struct ipu_soc *ipu, int channel);
 void ipu_dc_put(struct ipu_dc *dc);
 int ipu_dc_init_sync(struct ipu_dc *dc, struct ipu_di *di, bool interlaced,
 		u32 pixel_fmt, u32 width);
+void ipu_dc_enable(struct ipu_soc *ipu);
 void ipu_dc_enable_channel(struct ipu_dc *dc);
 void ipu_dc_disable_channel(struct ipu_dc *dc);
+void ipu_dc_disable(struct ipu_soc *ipu);
 
 /*
  * IPU Display Interface (di) functions
@@ -152,8 +155,10 @@ void ipu_dmfc_put(struct dmfc_channel *dmfc);
 
 struct ipu_dp *ipu_dp_get(struct ipu_soc *ipu, unsigned int flow);
 void ipu_dp_put(struct ipu_dp *);
+int ipu_dp_enable(struct ipu_soc *ipu);
 int ipu_dp_enable_channel(struct ipu_dp *dp);
 void ipu_dp_disable_channel(struct ipu_dp *dp);
+void ipu_dp_disable(struct ipu_soc *ipu);
 int ipu_dp_setup_channel(struct ipu_dp *dp,
 		enum ipu_color_space in, enum ipu_color_space out);
 int ipu_dp_set_window_pos(struct ipu_dp *, u16 x_pos, u16 y_pos);
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-common.c b/drivers/staging/imx-drm/ipu-v3/ipu-common.c
index ca85d3d70ae..a1f7b2001c8 100644
--- a/drivers/staging/imx-drm/ipu-v3/ipu-common.c
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-common.c
@@ -697,6 +697,12 @@ int ipu_idmac_enable_channel(struct ipuv3_channel *channel)
 }
 EXPORT_SYMBOL_GPL(ipu_idmac_enable_channel);
 
+bool ipu_idmac_channel_busy(struct ipu_soc *ipu, unsigned int chno)
+{
+	return (ipu_idmac_read(ipu, IDMAC_CHA_BUSY(chno)) & idma_mask(chno));
+}
+EXPORT_SYMBOL_GPL(ipu_idmac_channel_busy);
+
 int ipu_idmac_wait_busy(struct ipuv3_channel *channel, int ms)
 {
 	struct ipu_soc *ipu = channel->ipu;
@@ -714,6 +720,22 @@ int ipu_idmac_wait_busy(struct ipuv3_channel *channel, int ms)
 }
 EXPORT_SYMBOL_GPL(ipu_idmac_wait_busy);
 
+int ipu_wait_interrupt(struct ipu_soc *ipu, int irq, int ms)
+{
+	unsigned long timeout;
+
+	timeout = jiffies + msecs_to_jiffies(ms);
+	ipu_cm_write(ipu, BIT(irq % 32), IPU_INT_STAT(irq / 32));
+	while (!(ipu_cm_read(ipu, IPU_INT_STAT(irq / 32) & BIT(irq % 32)))) {
+		if (time_after(jiffies, timeout))
+			return -ETIMEDOUT;
+		cpu_relax();
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_wait_interrupt);
+
 int ipu_idmac_disable_channel(struct ipuv3_channel *channel)
 {
 	struct ipu_soc *ipu = channel->ipu;
@@ -900,7 +922,8 @@ static void ipu_irq_handle(struct ipu_soc *ipu, const int *regs, int num_regs)
 		status &= ipu_cm_read(ipu, IPU_INT_CTRL(regs[i]));
 
 		for_each_set_bit(bit, &status, 32) {
-			irq = irq_linear_revmap(ipu->domain, regs[i] * 32 + bit);
+			irq = irq_linear_revmap(ipu->domain,
+						regs[i] * 32 + bit);
 			if (irq)
 				generic_handle_irq(irq);
 		}
@@ -933,15 +956,22 @@ static void ipu_err_irq_handler(unsigned int irq, struct irq_desc *desc)
 	chained_irq_exit(chip, desc);
 }
 
-int ipu_idmac_channel_irq(struct ipu_soc *ipu, struct ipuv3_channel *channel,
-		enum ipu_channel_irq irq_type)
+int ipu_map_irq(struct ipu_soc *ipu, int irq)
 {
-	int irq = irq_linear_revmap(ipu->domain, irq_type + channel->num);
+	int virq;
 
-	if (!irq)
-		irq = irq_create_mapping(ipu->domain, irq_type + channel->num);
+	virq = irq_linear_revmap(ipu->domain, irq);
+	if (!virq)
+		virq = irq_create_mapping(ipu->domain, irq);
 
-	return irq;
+	return virq;
+}
+EXPORT_SYMBOL_GPL(ipu_map_irq);
+
+int ipu_idmac_channel_irq(struct ipu_soc *ipu, struct ipuv3_channel *channel,
+		enum ipu_channel_irq irq_type)
+{
+	return ipu_map_irq(ipu, irq_type + channel->num);
 }
 EXPORT_SYMBOL_GPL(ipu_idmac_channel_irq);
 
@@ -1053,7 +1083,8 @@ static int ipu_irq_init(struct ipu_soc *ipu)
 	}
 
 	ret = irq_alloc_domain_generic_chips(ipu->domain, 32, 1, "IPU",
-					     handle_level_irq, 0, IRQF_VALID, 0);
+					     handle_level_irq, 0,
+					     IRQF_VALID, 0);
 	if (ret < 0) {
 		dev_err(ipu->dev, "failed to alloc generic irq chips\n");
 		irq_domain_remove(ipu->domain);
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-dc.c b/drivers/staging/imx-drm/ipu-v3/ipu-dc.c
index d5de8bb5c80..784a4a13eac 100644
--- a/drivers/staging/imx-drm/ipu-v3/ipu-dc.c
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-dc.c
@@ -18,6 +18,7 @@
 #include <linux/types.h>
 #include <linux/errno.h>
 #include <linux/delay.h>
+#include <linux/interrupt.h>
 #include <linux/io.h>
 
 #include "../imx-drm.h"
@@ -91,6 +92,7 @@ enum ipu_dc_map {
 	IPU_DC_MAP_RGB565,
 	IPU_DC_MAP_GBR24, /* TVEv2 */
 	IPU_DC_MAP_BGR666,
+	IPU_DC_MAP_LVDS666,
 	IPU_DC_MAP_BGR24,
 };
 
@@ -110,6 +112,9 @@ struct ipu_dc_priv {
 	struct device		*dev;
 	struct ipu_dc		channels[IPU_DC_NUM_CHANNELS];
 	struct mutex		mutex;
+	struct completion	comp;
+	int			dc_irq;
+	int			dp_irq;
 };
 
 static void dc_link_event(struct ipu_dc *dc, int event, int addr, int priority)
@@ -153,6 +158,8 @@ static int ipu_pixfmt_to_map(u32 fmt)
 		return IPU_DC_MAP_GBR24;
 	case V4L2_PIX_FMT_BGR666:
 		return IPU_DC_MAP_BGR666;
+	case v4l2_fourcc('L', 'V', 'D', '6'):
+		return IPU_DC_MAP_LVDS666;
 	case V4L2_PIX_FMT_BGR24:
 		return IPU_DC_MAP_BGR24;
 	default:
@@ -220,12 +227,16 @@ int ipu_dc_init_sync(struct ipu_dc *dc, struct ipu_di *di, bool interlaced,
 	writel(0x0, dc->base + DC_WR_CH_ADDR);
 	writel(width, priv->dc_reg + DC_DISP_CONF2(dc->di));
 
-	ipu_module_enable(priv->ipu, IPU_CONF_DC_EN);
-
 	return 0;
 }
 EXPORT_SYMBOL_GPL(ipu_dc_init_sync);
 
+void ipu_dc_enable(struct ipu_soc *ipu)
+{
+	ipu_module_enable(ipu, IPU_CONF_DC_EN);
+}
+EXPORT_SYMBOL_GPL(ipu_dc_enable);
+
 void ipu_dc_enable_channel(struct ipu_dc *dc)
 {
 	int di;
@@ -239,41 +250,55 @@ void ipu_dc_enable_channel(struct ipu_dc *dc)
 }
 EXPORT_SYMBOL_GPL(ipu_dc_enable_channel);
 
+static irqreturn_t dc_irq_handler(int irq, void *dev_id)
+{
+	struct ipu_dc *dc = dev_id;
+	u32 reg;
+
+	reg = readl(dc->base + DC_WR_CH_CONF);
+	reg &= ~DC_WR_CH_CONF_PROG_TYPE_MASK;
+	writel(reg, dc->base + DC_WR_CH_CONF);
+
+	/* The Freescale BSP kernel clears DIx_COUNTER_RELEASE here */
+
+	complete(&dc->priv->comp);
+	return IRQ_HANDLED;
+}
+
 void ipu_dc_disable_channel(struct ipu_dc *dc)
 {
 	struct ipu_dc_priv *priv = dc->priv;
+	int irq, ret;
 	u32 val;
-	int irq = 0, timeout = 50;
 
+	/* TODO: Handle MEM_FG_SYNC differently from MEM_BG_SYNC */
 	if (dc->chno == 1)
-		irq = IPU_IRQ_DC_FC_1;
+		irq = priv->dc_irq;
 	else if (dc->chno == 5)
-		irq = IPU_IRQ_DP_SF_END;
+		irq = priv->dp_irq;
 	else
 		return;
 
-	/* should wait for the interrupt here */
-	mdelay(50);
+	init_completion(&priv->comp);
+	enable_irq(irq);
+	ret = wait_for_completion_timeout(&priv->comp, msecs_to_jiffies(50));
+	disable_irq(irq);
+	if (ret <= 0) {
+		dev_warn(priv->dev, "DC stop timeout after 50 ms\n");
 
-	if (dc->di == 0)
-		val = 0x00000002;
-	else
-		val = 0x00000020;
-
-	/* Wait for DC triple buffer to empty */
-	while ((readl(priv->dc_reg + DC_STAT) & val) != val) {
-		usleep_range(2000, 20000);
-		timeout -= 2;
-		if (timeout <= 0)
-			break;
+		val = readl(dc->base + DC_WR_CH_CONF);
+		val &= ~DC_WR_CH_CONF_PROG_TYPE_MASK;
+		writel(val, dc->base + DC_WR_CH_CONF);
 	}
-
-	val = readl(dc->base + DC_WR_CH_CONF);
-	val &= ~DC_WR_CH_CONF_PROG_TYPE_MASK;
-	writel(val, dc->base + DC_WR_CH_CONF);
 }
 EXPORT_SYMBOL_GPL(ipu_dc_disable_channel);
 
+void ipu_dc_disable(struct ipu_soc *ipu)
+{
+	ipu_module_disable(ipu, IPU_CONF_DC_EN);
+}
+EXPORT_SYMBOL_GPL(ipu_dc_disable);
+
 static void ipu_dc_map_config(struct ipu_dc_priv *priv, enum ipu_dc_map map,
 		int byte_num, int offset, int mask)
 {
@@ -340,7 +365,7 @@ int ipu_dc_init(struct ipu_soc *ipu, struct device *dev,
 	struct ipu_dc_priv *priv;
 	static int channel_offsets[] = { 0, 0x1c, 0x38, 0x54, 0x58, 0x5c,
 		0x78, 0, 0x94, 0xb4};
-	int i;
+	int i, ret;
 
 	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
 	if (!priv)
@@ -361,13 +386,31 @@ int ipu_dc_init(struct ipu_soc *ipu, struct device *dev,
 		priv->channels[i].base = priv->dc_reg + channel_offsets[i];
 	}
 
+	priv->dc_irq = ipu_map_irq(ipu, IPU_IRQ_DC_FC_1);
+	if (!priv->dc_irq)
+		return -EINVAL;
+	ret = devm_request_irq(dev, priv->dc_irq, dc_irq_handler, 0, NULL,
+			       &priv->channels[1]);
+	if (ret < 0)
+		return ret;
+	disable_irq(priv->dc_irq);
+	priv->dp_irq = ipu_map_irq(ipu, IPU_IRQ_DP_SF_END);
+	if (!priv->dp_irq)
+		return -EINVAL;
+	ret = devm_request_irq(dev, priv->dp_irq, dc_irq_handler, 0, NULL,
+			       &priv->channels[5]);
+	if (ret < 0)
+		return ret;
+	disable_irq(priv->dp_irq);
+
 	writel(DC_WR_CH_CONF_WORD_SIZE_24 | DC_WR_CH_CONF_DISP_ID_PARALLEL(1) |
 			DC_WR_CH_CONF_PROG_DI_ID,
 			priv->channels[1].base + DC_WR_CH_CONF);
 	writel(DC_WR_CH_CONF_WORD_SIZE_24 | DC_WR_CH_CONF_DISP_ID_PARALLEL(0),
 			priv->channels[5].base + DC_WR_CH_CONF);
 
-	writel(DC_GEN_SYNC_1_6_SYNC | DC_GEN_SYNC_PRIORITY_1, priv->dc_reg + DC_GEN);
+	writel(DC_GEN_SYNC_1_6_SYNC | DC_GEN_SYNC_PRIORITY_1,
+		priv->dc_reg + DC_GEN);
 
 	ipu->dc_priv = priv;
 
@@ -398,6 +441,12 @@ int ipu_dc_init(struct ipu_soc *ipu, struct device *dev,
 	ipu_dc_map_config(priv, IPU_DC_MAP_BGR666, 1, 11, 0xfc); /* green */
 	ipu_dc_map_config(priv, IPU_DC_MAP_BGR666, 2, 17, 0xfc); /* red */
 
+	/* lvds666 */
+	ipu_dc_map_clear(priv, IPU_DC_MAP_LVDS666);
+	ipu_dc_map_config(priv, IPU_DC_MAP_LVDS666, 0, 5, 0xfc); /* blue */
+	ipu_dc_map_config(priv, IPU_DC_MAP_LVDS666, 1, 13, 0xfc); /* green */
+	ipu_dc_map_config(priv, IPU_DC_MAP_LVDS666, 2, 21, 0xfc); /* red */
+
 	/* bgr24 */
 	ipu_dc_map_clear(priv, IPU_DC_MAP_BGR24);
 	ipu_dc_map_config(priv, IPU_DC_MAP_BGR24, 2, 7, 0xff); /* red */
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-di.c b/drivers/staging/imx-drm/ipu-v3/ipu-di.c
index 82a9ebad697..849b3e120ef 100644
--- a/drivers/staging/imx-drm/ipu-v3/ipu-di.c
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-di.c
@@ -595,7 +595,7 @@ int ipu_di_init_sync_panel(struct ipu_di *di, struct ipu_di_signal_cfg *sig)
 		}
 	}
 
-	if (!sig->clk_pol)
+	if (sig->clk_pol)
 		di_gen |= DI_GEN_POLARITY_DISP_CLK;
 
 	ipu_di_write(di, di_gen, DI_GENERAL);
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-dmfc.c b/drivers/staging/imx-drm/ipu-v3/ipu-dmfc.c
index 45213017fa4..59f182b28fc 100644
--- a/drivers/staging/imx-drm/ipu-v3/ipu-dmfc.c
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-dmfc.c
@@ -28,7 +28,12 @@
 #define DMFC_GENERAL1		0x0014
 #define DMFC_GENERAL2		0x0018
 #define DMFC_IC_CTRL		0x001c
-#define DMFC_STAT		0x0020
+#define DMFC_WR_CHAN_ALT	0x0020
+#define DMFC_WR_CHAN_DEF_ALT	0x0024
+#define DMFC_DP_CHAN_ALT	0x0028
+#define DMFC_DP_CHAN_DEF_ALT	0x002c
+#define DMFC_GENERAL1_ALT	0x0030
+#define DMFC_STAT		0x0034
 
 #define DMFC_WR_CHAN_1_28		0
 #define DMFC_WR_CHAN_2_41		8
@@ -133,6 +138,20 @@ int ipu_dmfc_enable_channel(struct dmfc_channel *dmfc)
 }
 EXPORT_SYMBOL_GPL(ipu_dmfc_enable_channel);
 
+static void ipu_dmfc_wait_fifos(struct ipu_dmfc_priv *priv)
+{
+	unsigned long timeout = jiffies + msecs_to_jiffies(1000);
+
+	while ((readl(priv->base + DMFC_STAT) & 0x02fff000) != 0x02fff000) {
+		if (time_after(jiffies, timeout)) {
+			dev_warn(priv->dev,
+				 "Timeout waiting for DMFC FIFOs to clear\n");
+			break;
+		}
+		cpu_relax();
+	}
+}
+
 void ipu_dmfc_disable_channel(struct dmfc_channel *dmfc)
 {
 	struct ipu_dmfc_priv *priv = dmfc->priv;
@@ -141,8 +160,10 @@ void ipu_dmfc_disable_channel(struct dmfc_channel *dmfc)
 
 	priv->use_count--;
 
-	if (!priv->use_count)
+	if (!priv->use_count) {
+		ipu_dmfc_wait_fifos(priv);
 		ipu_module_disable(priv->ipu, IPU_CONF_DMFC_EN);
+	}
 
 	if (priv->use_count < 0)
 		priv->use_count = 0;
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-dp.c b/drivers/staging/imx-drm/ipu-v3/ipu-dp.c
index 58f87c8d7c0..d90f82a87d1 100644
--- a/drivers/staging/imx-drm/ipu-v3/ipu-dp.c
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-dp.c
@@ -215,10 +215,9 @@ int ipu_dp_setup_channel(struct ipu_dp *dp,
 }
 EXPORT_SYMBOL_GPL(ipu_dp_setup_channel);
 
-int ipu_dp_enable_channel(struct ipu_dp *dp)
+int ipu_dp_enable(struct ipu_soc *ipu)
 {
-	struct ipu_flow *flow = to_flow(dp);
-	struct ipu_dp_priv *priv = flow->priv;
+	struct ipu_dp_priv *priv = ipu->dp_priv;
 
 	mutex_lock(&priv->mutex);
 
@@ -227,15 +226,28 @@ int ipu_dp_enable_channel(struct ipu_dp *dp)
 
 	priv->use_count++;
 
-	if (dp->foreground) {
-		u32 reg;
+	mutex_unlock(&priv->mutex);
 
-		reg = readl(flow->base + DP_COM_CONF);
-		reg |= DP_COM_CONF_FG_EN;
-		writel(reg, flow->base + DP_COM_CONF);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_dp_enable);
 
-		ipu_srm_dp_sync_update(priv->ipu);
-	}
+int ipu_dp_enable_channel(struct ipu_dp *dp)
+{
+	struct ipu_flow *flow = to_flow(dp);
+	struct ipu_dp_priv *priv = flow->priv;
+	u32 reg;
+
+	if (!dp->foreground)
+		return 0;
+
+	mutex_lock(&priv->mutex);
+
+	reg = readl(flow->base + DP_COM_CONF);
+	reg |= DP_COM_CONF_FG_EN;
+	writel(reg, flow->base + DP_COM_CONF);
+
+	ipu_srm_dp_sync_update(priv->ipu);
 
 	mutex_unlock(&priv->mutex);
 
@@ -247,25 +259,38 @@ void ipu_dp_disable_channel(struct ipu_dp *dp)
 {
 	struct ipu_flow *flow = to_flow(dp);
 	struct ipu_dp_priv *priv = flow->priv;
+	u32 reg, csc;
+
+	if (!dp->foreground)
+		return;
 
 	mutex_lock(&priv->mutex);
 
-	priv->use_count--;
+	reg = readl(flow->base + DP_COM_CONF);
+	csc = reg & DP_COM_CONF_CSC_DEF_MASK;
+	if (csc == DP_COM_CONF_CSC_DEF_FG)
+		reg &= ~DP_COM_CONF_CSC_DEF_MASK;
 
-	if (dp->foreground) {
-		u32 reg, csc;
+	reg &= ~DP_COM_CONF_FG_EN;
+	writel(reg, flow->base + DP_COM_CONF);
 
-		reg = readl(flow->base + DP_COM_CONF);
-		csc = reg & DP_COM_CONF_CSC_DEF_MASK;
-		if (csc == DP_COM_CONF_CSC_DEF_FG)
-			reg &= ~DP_COM_CONF_CSC_DEF_MASK;
+	writel(0, flow->base + DP_FG_POS);
+	ipu_srm_dp_sync_update(priv->ipu);
 
-		reg &= ~DP_COM_CONF_FG_EN;
-		writel(reg, flow->base + DP_COM_CONF);
+	if (ipu_idmac_channel_busy(priv->ipu, IPUV3_CHANNEL_MEM_BG_SYNC))
+		ipu_wait_interrupt(priv->ipu, IPU_IRQ_DP_SF_END, 50);
 
-		writel(0, flow->base + DP_FG_POS);
-		ipu_srm_dp_sync_update(priv->ipu);
-	}
+	mutex_unlock(&priv->mutex);
+}
+EXPORT_SYMBOL_GPL(ipu_dp_disable_channel);
+
+void ipu_dp_disable(struct ipu_soc *ipu)
+{
+	struct ipu_dp_priv *priv = ipu->dp_priv;
+
+	mutex_lock(&priv->mutex);
+
+	priv->use_count--;
 
 	if (!priv->use_count)
 		ipu_module_disable(priv->ipu, IPU_CONF_DP_EN);
@@ -275,7 +300,7 @@ void ipu_dp_disable_channel(struct ipu_dp *dp)
 
 	mutex_unlock(&priv->mutex);
 }
-EXPORT_SYMBOL_GPL(ipu_dp_disable_channel);
+EXPORT_SYMBOL_GPL(ipu_dp_disable);
 
 struct ipu_dp *ipu_dp_get(struct ipu_soc *ipu, unsigned int flow)
 {
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-prv.h b/drivers/staging/imx-drm/ipu-v3/ipu-prv.h
index 4df00501adc..bfc1b336648 100644
--- a/drivers/staging/imx-drm/ipu-v3/ipu-prv.h
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-prv.h
@@ -185,6 +185,9 @@ void ipu_srm_dp_sync_update(struct ipu_soc *ipu);
 int ipu_module_enable(struct ipu_soc *ipu, u32 mask);
 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_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/staging/imx-drm/ipuv3-crtc.c b/drivers/staging/imx-drm/ipuv3-crtc.c
index c48f640db00..47bec5e1735 100644
--- a/drivers/staging/imx-drm/ipuv3-crtc.c
+++ b/drivers/staging/imx-drm/ipuv3-crtc.c
@@ -60,24 +60,32 @@ struct ipu_crtc {
 
 static void ipu_fb_enable(struct ipu_crtc *ipu_crtc)
 {
+	struct ipu_soc *ipu = dev_get_drvdata(ipu_crtc->dev->parent);
+
 	if (ipu_crtc->enabled)
 		return;
 
-	ipu_di_enable(ipu_crtc->di);
-	ipu_dc_enable_channel(ipu_crtc->dc);
+	ipu_dc_enable(ipu);
 	ipu_plane_enable(ipu_crtc->plane[0]);
+	/* Start DC channel and DI after IDMAC */
+	ipu_dc_enable_channel(ipu_crtc->dc);
+	ipu_di_enable(ipu_crtc->di);
 
 	ipu_crtc->enabled = 1;
 }
 
 static void ipu_fb_disable(struct ipu_crtc *ipu_crtc)
 {
+	struct ipu_soc *ipu = dev_get_drvdata(ipu_crtc->dev->parent);
+
 	if (!ipu_crtc->enabled)
 		return;
 
-	ipu_plane_disable(ipu_crtc->plane[0]);
+	/* Stop DC channel and DI before IDMAC */
 	ipu_dc_disable_channel(ipu_crtc->dc);
 	ipu_di_disable(ipu_crtc->di);
+	ipu_plane_disable(ipu_crtc->plane[0]);
+	ipu_dc_disable(ipu);
 
 	ipu_crtc->enabled = 0;
 }
@@ -158,7 +166,7 @@ static int ipu_crtc_mode_set(struct drm_crtc *crtc,
 		sig_cfg.Vsync_pol = 1;
 
 	sig_cfg.enable_pol = 1;
-	sig_cfg.clk_pol = 1;
+	sig_cfg.clk_pol = 0;
 	sig_cfg.width = mode->hdisplay;
 	sig_cfg.height = mode->vdisplay;
 	sig_cfg.pixel_fmt = out_pixel_fmt;
diff --git a/drivers/staging/imx-drm/ipuv3-plane.c b/drivers/staging/imx-drm/ipuv3-plane.c
index 27a8d735dae..5697e59ddf1 100644
--- a/drivers/staging/imx-drm/ipuv3-plane.c
+++ b/drivers/staging/imx-drm/ipuv3-plane.c
@@ -239,6 +239,8 @@ err_out:
 
 void ipu_plane_enable(struct ipu_plane *ipu_plane)
 {
+	if (ipu_plane->dp)
+		ipu_dp_enable(ipu_plane->ipu);
 	ipu_dmfc_enable_channel(ipu_plane->dmfc);
 	ipu_idmac_enable_channel(ipu_plane->ipu_ch);
 	if (ipu_plane->dp)
@@ -257,6 +259,8 @@ void ipu_plane_disable(struct ipu_plane *ipu_plane)
 		ipu_dp_disable_channel(ipu_plane->dp);
 	ipu_idmac_disable_channel(ipu_plane->ipu_ch);
 	ipu_dmfc_disable_channel(ipu_plane->dmfc);
+	if (ipu_plane->dp)
+		ipu_dp_disable(ipu_plane->ipu);
 }
 
 static void ipu_plane_dpms(struct ipu_plane *ipu_plane, int mode)
diff --git a/drivers/staging/imx-drm/parallel-display.c b/drivers/staging/imx-drm/parallel-display.c
index c60b6c645f4..eaf4dda1a0c 100644
--- a/drivers/staging/imx-drm/parallel-display.c
+++ b/drivers/staging/imx-drm/parallel-display.c
@@ -219,6 +219,8 @@ static int imx_pd_bind(struct device *dev, struct device *master, void *data)
 			imxpd->interface_pix_fmt = V4L2_PIX_FMT_RGB565;
 		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');
 	}
 
 	panel_node = of_parse_phandle(np, "fsl,panel", 0);
diff --git a/drivers/staging/keucr/init.c b/drivers/staging/keucr/init.c
index e6118390654..1e7449d6d12 100644
--- a/drivers/staging/keucr/init.c
+++ b/drivers/staging/keucr/init.c
@@ -9,6 +9,7 @@
 #include "usb.h"
 #include "scsiglue.h"
 #include "transport.h"
+#include "smil.h"
 #include "init.h"
 
 /*
diff --git a/drivers/staging/keucr/init.h b/drivers/staging/keucr/init.h
index 98d2e3ba854..d1367e726ff 100644
--- a/drivers/staging/keucr/init.h
+++ b/drivers/staging/keucr/init.h
@@ -1,10 +1,5 @@
 #include "common.h"
 
-extern u32 MediaChange;
-extern int Check_D_MediaFmt(struct us_data *);
-
-
-
 static u8 SM_Init[] = {
 0x7B, 0x09, 0x7C, 0xF0, 0x7D, 0x10, 0x7E, 0xE9,
 0x7F, 0xCC, 0x12, 0x2F, 0x71, 0x90, 0xE9, 0xCC,
diff --git a/drivers/staging/keucr/scsiglue.c b/drivers/staging/keucr/scsiglue.c
index ac3d34dcc43..7d8d444910c 100644
--- a/drivers/staging/keucr/scsiglue.c
+++ b/drivers/staging/keucr/scsiglue.c
@@ -106,7 +106,7 @@ static int queuecommand_lck(struct scsi_cmnd *srb,
 	/* check for state-transition errors */
 	if (us->srb != NULL) {
 		/* pr_info("Error in %s: us->srb = %p\n"
-				 __FUNCTION__, us->srb); */
+				 __func__, us->srb); */
 		return SCSI_MLQUEUE_HOST_BUSY;
 	}
 
diff --git a/drivers/staging/keucr/smil.h b/drivers/staging/keucr/smil.h
index 39951738d23..f938759337e 100644
--- a/drivers/staging/keucr/smil.h
+++ b/drivers/staging/keucr/smil.h
@@ -204,6 +204,7 @@ extern struct keucr_media_area    CisArea;
 int         Init_D_SmartMedia(void);
 int         Pwoff_D_SmartMedia(void);
 int         Check_D_SmartMedia(void);
+int         Check_D_MediaFmt(struct us_data *);
 int         Check_D_Parameter(struct us_data *, u16 *, u8 *, u8 *);
 int         Media_D_ReadSector(struct us_data *, u32, u16, u8 *);
 int         Media_D_WriteSector(struct us_data *, u32, u16, u8 *);
@@ -284,6 +285,4 @@ u8 correct_data(u8 *, u8 *, u8,   u8,   u8);
 int  _Correct_D_SwECC(u8 *, u8 *, u8 *);
 void _Calculate_D_SwECC(u8 *, u8 *);
 
-void SM_Init(void);
-
 #endif /* already included */
diff --git a/drivers/staging/keucr/smilmain.c b/drivers/staging/keucr/smilmain.c
index fc7cbc6f8a4..42ec8a669ad 100644
--- a/drivers/staging/keucr/smilmain.c
+++ b/drivers/staging/keucr/smilmain.c
@@ -141,23 +141,6 @@ int Media_D_CopySector(struct us_data *us, u32 start, u16 count, u8 *buf)
 	return NO_ERROR;
 }
 
-/* ----- Release_D_CopySector() ------------------------------------------ */
-static int Release_D_CopySector(struct us_data *us)
-{
-	Log2Phy[Media.Zone][Media.LogBlock] = WriteBlock;
-	Media.PhyBlock = ReadBlock;
-
-	if (Media.PhyBlock == NO_ASSIGN) {
-		Media.PhyBlock = WriteBlock;
-		return SMSUCCESS;
-	}
-
-	Clr_D_Bit(Assign[Media.Zone], Media.PhyBlock);
-	Media.PhyBlock = WriteBlock;
-
-	return SMSUCCESS;
-}
-
 /* SmartMedia Physical Format Test Subroutine */
 /* ----- Check_D_MediaFmt() --------------------------------------------- */
 int Check_D_MediaFmt(struct us_data *us)
@@ -184,6 +167,23 @@ int Check_D_MediaFmt(struct us_data *us)
 	return SMSUCCESS;
 }
 
+/* ----- Release_D_CopySector() ------------------------------------------ */
+static int Release_D_CopySector(struct us_data *us)
+{
+	Log2Phy[Media.Zone][Media.LogBlock] = WriteBlock;
+	Media.PhyBlock = ReadBlock;
+
+	if (Media.PhyBlock == NO_ASSIGN) {
+		Media.PhyBlock = WriteBlock;
+		return SMSUCCESS;
+	}
+
+	Clr_D_Bit(Assign[Media.Zone], Media.PhyBlock);
+	Media.PhyBlock = WriteBlock;
+
+	return SMSUCCESS;
+}
+
 /* SmartMedia Physical Address Control Subroutine */
 /* ----- Conv_D_MediaAddr() --------------------------------------------- */
 static int Conv_D_MediaAddr(struct us_data *us, u32 addr)
diff --git a/drivers/staging/keucr/smilsub.c b/drivers/staging/keucr/smilsub.c
index 44ced826503..e981f14f3bf 100644
--- a/drivers/staging/keucr/smilsub.c
+++ b/drivers/staging/keucr/smilsub.c
@@ -133,6 +133,7 @@ void Set_D_LogBlockAddr(u8 *redundant)
 void Set_D_FailBlock(u8 *redundant)
 {
 	char i;
+
 	for (i = 0; i < REDTSIZE; i++)
 		*redundant++ = (u8)((i == REDT_BLOCK) ? 0xF0 : 0xFF);
 }
diff --git a/drivers/staging/keucr/transport.c b/drivers/staging/keucr/transport.c
index ae9414755d2..5e59525271f 100644
--- a/drivers/staging/keucr/transport.c
+++ b/drivers/staging/keucr/transport.c
@@ -669,6 +669,7 @@ int usb_stor_Bulk_transport(struct scsi_cmnd *srb, struct us_data *us)
 	/*  R/W data */
 	if (transfer_length) {
 		unsigned int pipe;
+
 		if (srb->sc_data_direction == DMA_FROM_DEVICE)
 			pipe = us->recv_bulk_pipe;
 		else
diff --git a/drivers/staging/line6/capture.c b/drivers/staging/line6/capture.c
index 0eda51d2e9a..e6ca631e3f7 100644
--- a/drivers/staging/line6/capture.c
+++ b/drivers/staging/line6/capture.c
@@ -97,6 +97,7 @@ void line6_unlink_audio_in_urbs(struct snd_line6_pcm *line6pcm)
 		if (test_bit(i, &line6pcm->active_urb_in)) {
 			if (!test_and_set_bit(i, &line6pcm->unlink_urb_in)) {
 				struct urb *u = line6pcm->urb_audio_in[i];
+
 				usb_unlink_urb(u);
 			}
 		}
@@ -334,6 +335,7 @@ static int snd_line6_capture_hw_params(struct snd_pcm_substream *substream,
 static int snd_line6_capture_hw_free(struct snd_pcm_substream *substream)
 {
 	struct snd_line6_pcm *line6pcm = snd_pcm_substream_chip(substream);
+
 	line6_pcm_release(line6pcm, LINE6_BIT_PCM_ALSA_CAPTURE_BUFFER);
 	return snd_pcm_lib_free_pages(substream);
 }
@@ -380,6 +382,7 @@ static snd_pcm_uframes_t
 snd_line6_capture_pointer(struct snd_pcm_substream *substream)
 {
 	struct snd_line6_pcm *line6pcm = snd_pcm_substream_chip(substream);
+
 	return line6pcm->pos_in_done;
 }
 
diff --git a/drivers/staging/line6/driver.c b/drivers/staging/line6/driver.c
index 77f1b421e95..ef511c76a6e 100644
--- a/drivers/staging/line6/driver.c
+++ b/drivers/staging/line6/driver.c
@@ -124,6 +124,7 @@ static int line6_send_raw_message_async_part(struct message *msg,
 static int line6_start_listen(struct usb_line6 *line6)
 {
 	int err;
+
 	usb_fill_int_urb(line6->urb_listen, line6->usbdev,
 			 usb_rcvintpipe(line6->usbdev, line6->ep_control_read),
 			 line6->buffer_listen, LINE6_BUFSIZE_LISTEN,
diff --git a/drivers/staging/line6/midi.c b/drivers/staging/line6/midi.c
index 02345fb06e3..1ac343b649c 100644
--- a/drivers/staging/line6/midi.c
+++ b/drivers/staging/line6/midi.c
@@ -183,6 +183,7 @@ static void line6_midi_output_drain(struct snd_rawmidi_substream *substream)
 	struct usb_line6 *line6 =
 	    line6_rawmidi_substream_midi(substream)->line6;
 	struct snd_line6_midi *midi = line6->line6midi;
+
 	wait_event_interruptible(midi->send_wait,
 				 midi->num_active_send_urbs == 0);
 }
@@ -260,6 +261,7 @@ static int snd_line6_new_midi(struct snd_line6_midi *line6midi)
 static int snd_line6_midi_free(struct snd_device *device)
 {
 	struct snd_line6_midi *line6midi = device->device_data;
+
 	line6_midibuf_destroy(&line6midi->midibuf_in);
 	line6_midibuf_destroy(&line6midi->midibuf_out);
 	return 0;
diff --git a/drivers/staging/line6/pcm.c b/drivers/staging/line6/pcm.c
index 661080b3c39..a3136b189ee 100644
--- a/drivers/staging/line6/pcm.c
+++ b/drivers/staging/line6/pcm.c
@@ -475,7 +475,7 @@ int line6_init_pcm(struct usb_line6 *line6,
 		MISSING_CASE;
 	}
 
-	line6pcm = kzalloc(sizeof(struct snd_line6_pcm), GFP_KERNEL);
+	line6pcm = kzalloc(sizeof(*line6pcm), GFP_KERNEL);
 
 	if (line6pcm == NULL)
 		return -ENOMEM;
diff --git a/drivers/staging/line6/playback.c b/drivers/staging/line6/playback.c
index 41869caf19a..2ca8900e68c 100644
--- a/drivers/staging/line6/playback.c
+++ b/drivers/staging/line6/playback.c
@@ -34,6 +34,7 @@ static void change_volume(struct urb *urb_out, int volume[],
 
 	if (bytes_per_frame == 4) {
 		short *p, *buf_end;
+
 		p = (short *)urb_out->transfer_buffer;
 		buf_end = p + urb_out->transfer_buffer_length / sizeof(*p);
 
@@ -43,11 +44,13 @@ static void change_volume(struct urb *urb_out, int volume[],
 		}
 	} else if (bytes_per_frame == 6) {
 		unsigned char *p, *buf_end;
+
 		p = (unsigned char *)urb_out->transfer_buffer;
 		buf_end = p + urb_out->transfer_buffer_length;
 
 		for (; p < buf_end; p += 3) {
 			int val;
+
 			val = p[0] + (p[1] << 8) + ((signed char)p[2] << 16);
 			val = (val * volume[chn & 1]) >> 8;
 			p[0] = val;
@@ -116,6 +119,7 @@ static void add_monitor_signal(struct urb *urb_out, unsigned char *signal,
 
 	if (bytes_per_frame == 4) {
 		short *pi, *po, *buf_end;
+
 		pi = (short *)signal;
 		po = (short *)urb_out->transfer_buffer;
 		buf_end = po + urb_out->transfer_buffer_length / sizeof(*po);
@@ -171,6 +175,7 @@ static int submit_audio_out_urb(struct snd_line6_pcm *line6pcm)
 
 		if (fsize == 0) {
 			int n;
+
 			line6pcm->count_out += frame_increment;
 			n = line6pcm->count_out / frame_factor;
 			line6pcm->count_out -= n * frame_factor;
@@ -207,6 +212,7 @@ static int submit_audio_out_urb(struct snd_line6_pcm *line6pcm)
 			   copy the data to the temp buffer.
 			 */
 			int len;
+
 			len = runtime->buffer_size - line6pcm->pos_out;
 
 			if (len > 0) {
@@ -305,6 +311,7 @@ void line6_unlink_audio_out_urbs(struct snd_line6_pcm *line6pcm)
 		if (test_bit(i, &line6pcm->active_urb_out)) {
 			if (!test_and_set_bit(i, &line6pcm->unlink_urb_out)) {
 				struct urb *u = line6pcm->urb_audio_out[i];
+
 				usb_unlink_urb(u);
 			}
 		}
@@ -358,7 +365,6 @@ static void audio_out_callback(struct urb *urb)
 {
 	int i, index, length = 0, shutdown = 0;
 	unsigned long flags;
-
 	struct snd_line6_pcm *line6pcm = (struct snd_line6_pcm *)urb->context;
 	struct snd_pcm_substream *substream =
 	    get_substream(line6pcm, SNDRV_PCM_STREAM_PLAYBACK);
@@ -384,6 +390,7 @@ static void audio_out_callback(struct urb *urb)
 
 	if (test_bit(LINE6_INDEX_PCM_ALSA_PLAYBACK_STREAM, &line6pcm->flags)) {
 		struct snd_pcm_runtime *runtime = substream->runtime;
+
 		line6pcm->pos_out_done +=
 		    length / line6pcm->properties->bytes_per_frame;
 
@@ -480,6 +487,7 @@ static int snd_line6_playback_hw_params(struct snd_pcm_substream *substream,
 static int snd_line6_playback_hw_free(struct snd_pcm_substream *substream)
 {
 	struct snd_line6_pcm *line6pcm = snd_pcm_substream_chip(substream);
+
 	line6_pcm_release(line6pcm, LINE6_BIT_PCM_ALSA_PLAYBACK_BUFFER);
 	return snd_pcm_lib_free_pages(substream);
 }
@@ -534,6 +542,7 @@ static snd_pcm_uframes_t
 snd_line6_playback_pointer(struct snd_pcm_substream *substream)
 {
 	struct snd_line6_pcm *line6pcm = snd_pcm_substream_chip(substream);
+
 	return line6pcm->pos_out_done;
 }
 
diff --git a/drivers/staging/line6/pod.c b/drivers/staging/line6/pod.c
index f4e95a614e3..44f4b2f9857 100644
--- a/drivers/staging/line6/pod.c
+++ b/drivers/staging/line6/pod.c
@@ -197,6 +197,7 @@ static ssize_t serial_number_show(struct device *dev,
 {
 	struct usb_interface *interface = to_usb_interface(dev);
 	struct usb_line6_pod *pod = usb_get_intfdata(interface);
+
 	return sprintf(buf, "%d\n", pod->serial_number);
 }
 
@@ -208,6 +209,7 @@ static ssize_t firmware_version_show(struct device *dev,
 {
 	struct usb_interface *interface = to_usb_interface(dev);
 	struct usb_line6_pod *pod = usb_get_intfdata(interface);
+
 	return sprintf(buf, "%d.%02d\n", pod->firmware_version / 100,
 		       pod->firmware_version % 100);
 }
@@ -220,6 +222,7 @@ static ssize_t device_id_show(struct device *dev,
 {
 	struct usb_interface *interface = to_usb_interface(dev);
 	struct usb_line6_pod *pod = usb_get_intfdata(interface);
+
 	return sprintf(buf, "%d\n", pod->device_id);
 }
 
@@ -243,6 +246,7 @@ static void pod_startup2(unsigned long data)
 {
 	struct usb_line6_pod *pod = (struct usb_line6_pod *)data;
 	struct usb_line6 *line6 = &pod->line6;
+
 	CHECK_STARTUP_PROGRESS(pod->startup_progress, POD_STARTUP_VERSIONREQ);
 
 	/* request firmware version: */
@@ -294,6 +298,7 @@ static int snd_pod_control_monitor_get(struct snd_kcontrol *kcontrol,
 {
 	struct snd_line6_pcm *line6pcm = snd_kcontrol_chip(kcontrol);
 	struct usb_line6_pod *pod = (struct usb_line6_pod *)line6pcm->line6;
+
 	ucontrol->value.integer.value[0] = pod->monitor_level;
 	return 0;
 }
diff --git a/drivers/staging/line6/toneport.c b/drivers/staging/line6/toneport.c
index af2e7e50c13..69437158d38 100644
--- a/drivers/staging/line6/toneport.c
+++ b/drivers/staging/line6/toneport.c
@@ -185,6 +185,7 @@ static int snd_toneport_monitor_get(struct snd_kcontrol *kcontrol,
 				    struct snd_ctl_elem_value *ucontrol)
 {
 	struct snd_line6_pcm *line6pcm = snd_kcontrol_chip(kcontrol);
+
 	ucontrol->value.integer.value[0] = line6pcm->volume_monitor;
 	return 0;
 }
@@ -213,6 +214,7 @@ static int snd_toneport_source_info(struct snd_kcontrol *kcontrol,
 				    struct snd_ctl_elem_info *uinfo)
 {
 	const int size = ARRAY_SIZE(toneport_source_info);
+
 	uinfo->type = SNDRV_CTL_ELEM_TYPE_ENUMERATED;
 	uinfo->count = 1;
 	uinfo->value.enumerated.items = size;
@@ -262,6 +264,7 @@ static void toneport_start_pcm(unsigned long arg)
 {
 	struct usb_line6_toneport *toneport = (struct usb_line6_toneport *)arg;
 	struct usb_line6 *line6 = &toneport->line6;
+
 	line6_pcm_acquire(line6->line6pcm, LINE6_BITS_PCM_MONITOR);
 }
 
diff --git a/drivers/staging/line6/variax.c b/drivers/staging/line6/variax.c
index bd0f694fa8d..ae2be99f9a9 100644
--- a/drivers/staging/line6/variax.c
+++ b/drivers/staging/line6/variax.c
@@ -95,6 +95,7 @@ static void variax_startup3(struct usb_line6_variax *variax)
 static void variax_startup4(unsigned long data)
 {
 	struct usb_line6_variax *variax = (struct usb_line6_variax *)data;
+
 	CHECK_STARTUP_PROGRESS(variax->startup_progress,
 			       VARIAX_STARTUP_ACTIVATE);
 
@@ -107,6 +108,7 @@ static void variax_startup4(unsigned long data)
 static void variax_startup5(unsigned long data)
 {
 	struct usb_line6_variax *variax = (struct usb_line6_variax *)data;
+
 	CHECK_STARTUP_PROGRESS(variax->startup_progress,
 			       VARIAX_STARTUP_WORKQUEUE);
 
diff --git a/drivers/staging/lustre/TODO b/drivers/staging/lustre/TODO
index 0a2b6cb3775..e325e1e9832 100644
--- a/drivers/staging/lustre/TODO
+++ b/drivers/staging/lustre/TODO
@@ -9,6 +9,5 @@
 * Other minor misc cleanups...
 
 Please send any patches to Greg Kroah-Hartman <greg@kroah.com>, Andreas Dilger
-<andreas.dilger@intel.com>, Oleg Drokin <oleg.drokin@intel.com> and
-Peng Tao <tao.peng@emc.com>. CCing hpdd-discuss <hpdd-discuss@lists.01.org>
-would be great too.
+<andreas.dilger@intel.com>, Oleg Drokin <oleg.drokin@intel.com>. CCing
+hpdd-discuss <hpdd-discuss@lists.01.org> would be great too.
diff --git a/drivers/staging/lustre/include/linux/libcfs/curproc.h b/drivers/staging/lustre/include/linux/libcfs/curproc.h
index 8fd47c98fd3..b314f34d2e6 100644
--- a/drivers/staging/lustre/include/linux/libcfs/curproc.h
+++ b/drivers/staging/lustre/include/linux/libcfs/curproc.h
@@ -56,7 +56,6 @@
 /* check if task is running in compat mode.*/
 #define current_pid()		(current->pid)
 #define current_comm()		(current->comm)
-int cfs_get_environ(const char *key, char *value, int *val_len);
 
 typedef __u32 cfs_cap_t;
 
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs.h b/drivers/staging/lustre/include/linux/libcfs/libcfs.h
index 4a6c7da7217..26b53f6420e 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs.h
@@ -42,6 +42,7 @@
 #endif
 
 #include <linux/libcfs/linux/libcfs.h>
+#include <linux/gfp.h>
 
 #include "curproc.h"
 
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_cpu.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_cpu.h
index c87efb49ebc..a140e5d9186 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_cpu.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_cpu.h
@@ -128,7 +128,7 @@ struct cfs_cpt_table *cfs_cpt_table_alloc(unsigned int ncpt);
 int
 cfs_cpt_number(struct cfs_cpt_table *cptab);
 /**
- * return number of HW cores or hypter-threadings in a CPU partition \a cpt
+ * return number of HW cores or hyper-threadings in a CPU partition \a cpt
  */
 int cfs_cpt_weight(struct cfs_cpt_table *cptab, int cpt);
 /**
@@ -152,7 +152,7 @@ int cfs_cpt_of_cpu(struct cfs_cpt_table *cptab, int cpu);
  */
 int cfs_cpt_bind(struct cfs_cpt_table *cptab, int cpt);
 /**
- * add \a cpu to CPU partion @cpt of \a cptab, return 1 for success,
+ * add \a cpu to CPU partition @cpt of \a cptab, return 1 for success,
  * otherwise 0 is returned
  */
 int cfs_cpt_set_cpu(struct cfs_cpt_table *cptab, int cpt, int cpu);
@@ -203,6 +203,11 @@ void cfs_cpt_clear(struct cfs_cpt_table *cptab, int cpt);
 int cfs_cpt_spread_node(struct cfs_cpt_table *cptab, int cpt);
 
 /**
+ * return number of HTs in the same core of \a cpu
+ */
+int cfs_cpu_ht_nsiblings(int cpu);
+
+/**
  * iterate over all CPU partitions in \a cptab
  */
 #define cfs_cpt_for_each(i, cptab)	\
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_crypto.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_crypto.h
index 776e9c0e48c..9e610a9ce51 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_crypto.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_crypto.h
@@ -31,7 +31,7 @@
 struct cfs_crypto_hash_type {
 	char		*cht_name;      /**< hash algorithm name, equal to
 					 * format name for crypto api */
-	unsigned int    cht_key;	/**< init key by default (vaild for
+	unsigned int    cht_key;	/**< init key by default (valid for
 					 * 4 bytes context like crc32, adler */
 	unsigned int    cht_size;       /**< hash digest size */
 };
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_debug.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_debug.h
index 2bd4885ce06..b270d84def9 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_debug.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_debug.h
@@ -183,7 +183,7 @@ struct libcfs_debug_msg_data {
 do {								\
 	(data)->msg_subsys = DEBUG_SUBSYSTEM;			\
 	(data)->msg_file   = __FILE__;				\
-	(data)->msg_fn     = __FUNCTION__;			\
+	(data)->msg_fn     = __func__;				\
 	(data)->msg_line   = __LINE__;				\
 	(data)->msg_cdls   = (cdls);				\
 	(data)->msg_mask   = (mask);				\
@@ -193,7 +193,7 @@ do {								\
 	static struct libcfs_debug_msg_data dataname = {	\
 	       .msg_subsys = DEBUG_SUBSYSTEM,			\
 	       .msg_file   = __FILE__,				\
-	       .msg_fn     = __FUNCTION__,			\
+	       .msg_fn     = __func__,				\
 	       .msg_line   = __LINE__,				\
 	       .msg_cdls   = (cdls)	 };			\
 	dataname.msg_mask   = (mask);
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_hash.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_hash.h
index e5d5db25562..954164361ca 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_hash.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_hash.h
@@ -59,8 +59,8 @@
 /*
  * Ideally we would use HAVE_HASH_LONG for this, but on linux we configure
  * the linux kernel and user space at the same time, so we need to differentiate
- * between them explicitely. If this is not needed on other architectures, then
- * we'll need to move the functions to archi specific headers.
+ * between them explicitly. If this is not needed on other architectures, then
+ * we'll need to move the functions to architecture specific headers.
  */
 
 #include <linux/hash.h>
@@ -86,7 +86,7 @@ union cfs_hash_lock {
 
 /**
  * cfs_hash_bucket is a container of:
- * - lock, couter ...
+ * - lock, counter ...
  * - array of hash-head starting from hsb_head[0], hash-head can be one of
  *   . cfs_hash_head_t
  *   . cfs_hash_head_dep_t
@@ -136,7 +136,7 @@ enum cfs_hash_tag {
 	CFS_HASH_NO_BKTLOCK     = 1 << 1,
 	/** rwlock to protect bucket */
 	CFS_HASH_RW_BKTLOCK     = 1 << 2,
-	/** spinlcok to protect bucket */
+	/** spinlock to protect bucket */
 	CFS_HASH_SPIN_BKTLOCK   = 1 << 3,
 	/** always add new item to tail */
 	CFS_HASH_ADD_TAIL       = 1 << 4,
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h
index dddccca120c..740bfcd2f09 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h
@@ -153,7 +153,7 @@ do {									    \
  * default allocator
  */
 #define LIBCFS_ALLOC(ptr, size) \
-	LIBCFS_ALLOC_GFP(ptr, size, __GFP_IO)
+	LIBCFS_ALLOC_GFP(ptr, size, GFP_NOFS)
 
 /**
  * non-sleeping allocator
@@ -177,7 +177,7 @@ do {									    \
 
 /** default numa allocator */
 #define LIBCFS_CPT_ALLOC(ptr, cptab, cpt, size)				    \
-	LIBCFS_CPT_ALLOC_GFP(ptr, cptab, cpt, size, __GFP_IO)
+	LIBCFS_CPT_ALLOC_GFP(ptr, cptab, cpt, size, GFP_NOFS)
 
 #define LIBCFS_FREE(ptr, size)					  \
 do {								    \
diff --git a/drivers/staging/lustre/include/linux/libcfs/linux/linux-lock.h b/drivers/staging/lustre/include/linux/libcfs/linux/linux-lock.h
index d6e00f92e4a..b75e401d9a9 100644
--- a/drivers/staging/lustre/include/linux/libcfs/linux/linux-lock.h
+++ b/drivers/staging/lustre/include/linux/libcfs/linux/linux-lock.h
@@ -66,7 +66,7 @@
  * - spin_unlock(x)
  * - spin_unlock_bh(x)
  * - spin_trylock(x)
- * - spin_is_locked(x)
+ * - assert_spin_locked(x)
  *
  * - spin_lock_irq(x)
  * - spin_lock_irqsave(x, f)
diff --git a/drivers/staging/lustre/include/linux/libcfs/linux/linux-mem.h b/drivers/staging/lustre/include/linux/libcfs/linux/linux-mem.h
index 2af15d41e77..ccee5c3e414 100644
--- a/drivers/staging/lustre/include/linux/libcfs/linux/linux-mem.h
+++ b/drivers/staging/lustre/include/linux/libcfs/linux/linux-mem.h
@@ -53,6 +53,11 @@
 #include <linux/memcontrol.h>
 #include <linux/mm_inline.h>
 
+#ifndef HAVE_LIBCFS_CPT
+/* Need this for cfs_cpt_table */
+#include <linux/libcfs/libcfs_cpu.h>
+#endif
+
 #define CFS_PAGE_MASK		   (~((__u64)PAGE_CACHE_SIZE-1))
 #define page_index(p)       ((p)->index)
 
diff --git a/drivers/staging/lustre/include/linux/lnet/lib-lnet.h b/drivers/staging/lustre/include/linux/lnet/lib-lnet.h
index 856fcfaafaf..06ff463e4af 100644
--- a/drivers/staging/lustre/include/linux/lnet/lib-lnet.h
+++ b/drivers/staging/lustre/include/linux/lnet/lib-lnet.h
@@ -181,7 +181,7 @@ lnet_net_lock_current(void)
 #define MAX_PORTALS     64
 
 /* these are only used by code with LNET_USE_LIB_FREELIST, but we still
- * exported them to !LNET_USE_LIB_FREELIST for easy implemetation */
+ * exported them to !LNET_USE_LIB_FREELIST for easy implementation */
 #define LNET_FL_MAX_MES		2048
 #define LNET_FL_MAX_MDS		2048
 #define LNET_FL_MAX_EQS		512
diff --git a/drivers/staging/lustre/include/linux/lnet/lib-types.h b/drivers/staging/lustre/include/linux/lnet/lib-types.h
index 1c13ef7df80..a63654b660d 100644
--- a/drivers/staging/lustre/include/linux/lnet/lib-types.h
+++ b/drivers/staging/lustre/include/linux/lnet/lib-types.h
@@ -204,7 +204,7 @@ typedef struct lnet_msg {
 	unsigned int	  msg_receiving:1;    /* being received */
 	unsigned int	  msg_txcredit:1;     /* taken an NI send credit */
 	unsigned int	  msg_peertxcredit:1; /* taken a peer send credit */
-	unsigned int	  msg_rtrcredit:1;    /* taken a globel router credit */
+	unsigned int	  msg_rtrcredit:1;    /* taken a global router credit */
 	unsigned int	  msg_peerrtrcredit:1; /* taken a peer router credit */
 	unsigned int	  msg_onactivelist:1; /* on the activelist */
 
@@ -342,7 +342,7 @@ typedef struct lnet_lnd {
 
 	/* Start receiving 'mlen' bytes of payload data, skipping the following
 	 * 'rlen' - 'mlen' bytes. 'private' is the 'private' passed to
-	 * lnet_parse().  Return non-zero for immedaite failure, otherwise
+	 * lnet_parse().  Return non-zero for immediate failure, otherwise
 	 * complete later with lnet_finalize().  This also gives back a receive
 	 * credit if the LND does flow control. */
 	int (*lnd_recv)(struct lnet_ni *ni, void *private, lnet_msg_t *msg,
@@ -594,7 +594,7 @@ struct lnet_match_table {
 	unsigned int		mt_cpt;
 	unsigned int		mt_portal;      /* portal index */
 	/* match table is set as "enabled" if there's non-exhausted MD
-	 * attached on mt_mhash, it's only valide for wildcard portal */
+	 * attached on mt_mhash, it's only valid for wildcard portal */
 	unsigned int		mt_enabled;
 	/* bitmap to flag whether MEs on mt_hash are exhausted or not */
 	__u64			mt_exhausted[LNET_MT_EXHAUSTED_BMAP];
diff --git a/drivers/staging/lustre/include/linux/lnet/lnetst.h b/drivers/staging/lustre/include/linux/lnet/lnetst.h
index e060599314d..87fe9ac76a6 100644
--- a/drivers/staging/lustre/include/linux/lnet/lnetst.h
+++ b/drivers/staging/lustre/include/linux/lnet/lnetst.h
@@ -402,7 +402,7 @@ typedef struct {
 /* add stat in session */
 typedef struct {
 	int		     lstio_sta_key;	  /* IN: session key */
-	int		     lstio_sta_timeout;      /* IN: timeout for stat requst */
+	int		     lstio_sta_timeout;      /* IN: timeout for stat request */
 	int		     lstio_sta_nmlen;	/* IN: group name length */
 	char		   *lstio_sta_namep;	/* IN: group name */
 	int		     lstio_sta_count;	/* IN: # of pid */
diff --git a/drivers/staging/lustre/include/linux/lnet/ptllnd.h b/drivers/staging/lustre/include/linux/lnet/ptllnd.h
index 564f5d3a9b4..313442a7ed3 100644
--- a/drivers/staging/lustre/include/linux/lnet/ptllnd.h
+++ b/drivers/staging/lustre/include/linux/lnet/ptllnd.h
@@ -66,7 +66,7 @@
 /*#define PTL_MD_LUSTRE_COMPLETION_SEMANTICS */
 
 /* Can compare handles directly on Cray Portals */
-#define PtlHandleIsEqual(a,b) ((a) == (b))
+#define PtlHandleIsEqual(a, b) ((a) == (b))
 
 /* Different error types on Cray Portals*/
 #define ptl_err_t ptl_ni_fail_t
diff --git a/drivers/staging/lustre/include/linux/lnet/ptllnd_wire.h b/drivers/staging/lustre/include/linux/lnet/ptllnd_wire.h
index 7d12b3a23a9..0d3ec5be0a0 100644
--- a/drivers/staging/lustre/include/linux/lnet/ptllnd_wire.h
+++ b/drivers/staging/lustre/include/linux/lnet/ptllnd_wire.h
@@ -64,26 +64,22 @@
 					 * above is for bulk data transfer */
 #define LNET_MSG_MATCHBITS       0      /* the value for the message channel */
 
-typedef struct
-{
+typedef struct {
 	lnet_hdr_t	kptlim_hdr;	     /* portals header */
 	char	      kptlim_payload[0];      /* piggy-backed payload */
 } WIRE_ATTR kptl_immediate_msg_t;
 
-typedef struct
-{
+typedef struct {
 	lnet_hdr_t	kptlrm_hdr;	     /* portals header */
 	__u64	     kptlrm_matchbits;       /* matchbits */
 } WIRE_ATTR kptl_rdma_msg_t;
 
-typedef struct
-{
+typedef struct {
 	__u64	     kptlhm_matchbits;       /* matchbits */
 	__u32	     kptlhm_max_msg_size;    /* max message size */
 } WIRE_ATTR kptl_hello_msg_t;
 
-typedef struct
-{
+typedef struct {
 	/* First 2 fields fixed FOR ALL TIME */
 	__u32	   ptlm_magic;     /* I'm a Portals LND message */
 	__u16	   ptlm_version;   /* this is my version number */
@@ -107,7 +103,7 @@ typedef struct
 } kptl_msg_t;
 
 /* kptl_msg_t::ptlm_credits is only a __u8 */
-#define PTLLND_MSG_MAX_CREDITS ((typeof(((kptl_msg_t*) 0)->ptlm_credits)) -1)
+#define PTLLND_MSG_MAX_CREDITS ((typeof(((kptl_msg_t*) 0)->ptlm_credits)) - 1)
 
 #define PTLLND_MSG_MAGIC		LNET_PROTO_PTL_MAGIC
 #define PTLLND_MSG_VERSION	      0x04
diff --git a/drivers/staging/lustre/include/linux/lnet/types.h b/drivers/staging/lustre/include/linux/lnet/types.h
index c833ce8544d..2add7976b3b 100644
--- a/drivers/staging/lustre/include/linux/lnet/types.h
+++ b/drivers/staging/lustre/include/linux/lnet/types.h
@@ -112,7 +112,7 @@ static inline void LNetInvalidateHandle(lnet_handle_any_t *h)
  */
 static inline int LNetHandleIsEqual (lnet_handle_any_t h1, lnet_handle_any_t h2)
 {
-	return (h1.cookie == h2.cookie);
+	return h1.cookie == h2.cookie;
 }
 
 /**
@@ -122,7 +122,7 @@ static inline int LNetHandleIsEqual (lnet_handle_any_t h1, lnet_handle_any_t h2)
  */
 static inline int LNetHandleIsInvalid(lnet_handle_any_t h)
 {
-	return (LNET_WIRE_HANDLE_COOKIE_NONE == h.cookie);
+	return LNET_WIRE_HANDLE_COOKIE_NONE == h.cookie;
 }
 
 /**
@@ -181,7 +181,7 @@ typedef struct {
 	 * address of an array of lnet_kiov_t and the length field specifies
 	 * the number of entries in the array. The length can't be bigger
 	 * than LNET_MAX_IOV. The lnet_kiov_t is used to describe page-based
-	 * fragments that are not necessarily mapped in virtal memory.
+	 * fragments that are not necessarily mapped in virtual memory.
 	 * - LNET_MD_IOVEC bit set: The start field points to the starting
 	 * address of an array of struct iovec and the length field specifies
 	 * the number of entries in the array. The length can't be bigger
@@ -381,7 +381,7 @@ typedef enum {
 
 #define LNET_SEQ_BASETYPE       long
 typedef unsigned LNET_SEQ_BASETYPE lnet_seq_t;
-#define LNET_SEQ_GT(a,b)	(((signed LNET_SEQ_BASETYPE)((a) - (b))) > 0)
+#define LNET_SEQ_GT(a, b)	(((signed LNET_SEQ_BASETYPE)((a) - (b))) > 0)
 
 /**
  * Information about an event on a MD.
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
index 0061c8afa20..892c41991f8 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
@@ -1141,7 +1141,7 @@ kiblnd_alloc_pages(kib_pages_t **pp, int cpt, int npages)
 	for (i = 0; i < npages; i++) {
 		p->ibp_pages[i] = alloc_pages_node(
 				    cfs_cpt_spread_node(lnet_cpt_table(), cpt),
-				    __GFP_IO, 0);
+				    GFP_NOFS, 0);
 		if (p->ibp_pages[i] == NULL) {
 			CERROR("Can't allocate page %d of %d\n", i, npages);
 			kiblnd_free_pages(p);
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
index 6173e74d749..dfd16e7ca8a 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
@@ -40,7 +40,7 @@
 
 #include "o2iblnd.h"
 
-void
+static void
 kiblnd_tx_done (lnet_ni_t *ni, kib_tx_t *tx)
 {
 	lnet_msg_t *lntmsg[2];
@@ -99,7 +99,7 @@ kiblnd_txlist_done (lnet_ni_t *ni, struct list_head *txlist, int status)
 	}
 }
 
-kib_tx_t *
+static kib_tx_t *
 kiblnd_get_idle_tx(lnet_ni_t *ni, lnet_nid_t target)
 {
 	kib_net_t		*net = (kib_net_t *)ni->ni_data;
@@ -127,7 +127,7 @@ kiblnd_get_idle_tx(lnet_ni_t *ni, lnet_nid_t target)
 	return tx;
 }
 
-void
+static void
 kiblnd_drop_rx(kib_rx_t *rx)
 {
 	kib_conn_t		*conn	= rx->rx_conn;
@@ -209,7 +209,7 @@ kiblnd_post_rx (kib_rx_t *rx, int credit)
 	return 0;
 }
 
-kib_tx_t *
+static kib_tx_t *
 kiblnd_find_waiting_tx_locked(kib_conn_t *conn, int txtype, __u64 cookie)
 {
 	struct list_head   *tmp;
@@ -234,7 +234,7 @@ kiblnd_find_waiting_tx_locked(kib_conn_t *conn, int txtype, __u64 cookie)
 	return NULL;
 }
 
-void
+static void
 kiblnd_handle_completion(kib_conn_t *conn, int txtype, int status, __u64 cookie)
 {
 	kib_tx_t    *tx;
@@ -273,7 +273,7 @@ kiblnd_handle_completion(kib_conn_t *conn, int txtype, int status, __u64 cookie)
 		kiblnd_tx_done(ni, tx);
 }
 
-void
+static void
 kiblnd_send_completion(kib_conn_t *conn, int type, int status, __u64 cookie)
 {
 	lnet_ni_t   *ni = conn->ibc_peer->ibp_ni;
@@ -292,7 +292,7 @@ kiblnd_send_completion(kib_conn_t *conn, int type, int status, __u64 cookie)
 	kiblnd_queue_tx(tx, conn);
 }
 
-void
+static void
 kiblnd_handle_rx (kib_rx_t *rx)
 {
 	kib_msg_t    *msg = rx->rx_msg;
@@ -453,7 +453,7 @@ kiblnd_handle_rx (kib_rx_t *rx)
 		kiblnd_post_rx(rx, post_credit);
 }
 
-void
+static void
 kiblnd_rx_complete (kib_rx_t *rx, int status, int nob)
 {
 	kib_msg_t    *msg = rx->rx_msg;
@@ -524,7 +524,7 @@ kiblnd_rx_complete (kib_rx_t *rx, int status, int nob)
 	kiblnd_drop_rx(rx);		     /* Don't re-post rx. */
 }
 
-struct page *
+static struct page *
 kiblnd_kvaddr_to_page (unsigned long vaddr)
 {
 	struct page *page;
@@ -695,7 +695,7 @@ kiblnd_map_tx(lnet_ni_t *ni, kib_tx_t *tx,
 }
 
 
-int
+static int
 kiblnd_setup_rd_iov(lnet_ni_t *ni, kib_tx_t *tx, kib_rdma_desc_t *rd,
 		    unsigned int niov, struct iovec *iov, int offset, int nob)
 {
@@ -748,7 +748,7 @@ kiblnd_setup_rd_iov(lnet_ni_t *ni, kib_tx_t *tx, kib_rdma_desc_t *rd,
 	return kiblnd_map_tx(ni, tx, rd, sg - tx->tx_frags);
 }
 
-int
+static int
 kiblnd_setup_rd_kiov (lnet_ni_t *ni, kib_tx_t *tx, kib_rdma_desc_t *rd,
 		      int nkiov, lnet_kiov_t *kiov, int offset, int nob)
 {
@@ -788,8 +788,10 @@ kiblnd_setup_rd_kiov (lnet_ni_t *ni, kib_tx_t *tx, kib_rdma_desc_t *rd,
 	return kiblnd_map_tx(ni, tx, rd, sg - tx->tx_frags);
 }
 
-int
+static int
 kiblnd_post_tx_locked (kib_conn_t *conn, kib_tx_t *tx, int credit)
+	__releases(conn->ibc_lock)
+	__acquires(conn->ibc_lock)
 {
 	kib_msg_t	 *msg = tx->tx_msg;
 	kib_peer_t	*peer = conn->ibc_peer;
@@ -992,7 +994,7 @@ kiblnd_check_sends (kib_conn_t *conn)
 	kiblnd_conn_decref(conn); /* ...until here */
 }
 
-void
+static void
 kiblnd_tx_complete (kib_tx_t *tx, int status)
 {
 	int	   failed = (status != IB_WC_SUCCESS);
@@ -1266,7 +1268,7 @@ static int kiblnd_resolve_addr(struct rdma_cm_id *cmid,
 	return rc;
 }
 
-void
+static void
 kiblnd_connect_peer (kib_peer_t *peer)
 {
 	struct rdma_cm_id *cmid;
@@ -1623,7 +1625,7 @@ kiblnd_send (lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg)
 	return 0;
 }
 
-void
+static void
 kiblnd_reply (lnet_ni_t *ni, kib_rx_t *rx, lnet_msg_t *lntmsg)
 {
 	lnet_process_id_t target = lntmsg->msg_target;
@@ -1810,7 +1812,7 @@ kiblnd_thread_start(int (*fn)(void *arg), void *arg, char *name)
 	return 0;
 }
 
-void
+static void
 kiblnd_thread_fini (void)
 {
 	atomic_dec (&kiblnd_data.kib_nthreads);
@@ -1824,7 +1826,7 @@ kiblnd_peer_alive (kib_peer_t *peer)
 	mb();
 }
 
-void
+static void
 kiblnd_peer_notify (kib_peer_t *peer)
 {
 	int	   error = 0;
@@ -1930,7 +1932,7 @@ kiblnd_close_conn(kib_conn_t *conn, int error)
 	write_unlock_irqrestore(&kiblnd_data.kib_global_lock, flags);
 }
 
-void
+static void
 kiblnd_handle_early_rxs(kib_conn_t *conn)
 {
 	unsigned long    flags;
@@ -1953,7 +1955,7 @@ kiblnd_handle_early_rxs(kib_conn_t *conn)
 	write_unlock_irqrestore(&kiblnd_data.kib_global_lock, flags);
 }
 
-void
+static void
 kiblnd_abort_txs(kib_conn_t *conn, struct list_head *txs)
 {
 	LIST_HEAD       (zombies);
@@ -1989,7 +1991,7 @@ kiblnd_abort_txs(kib_conn_t *conn, struct list_head *txs)
 	kiblnd_txlist_done(conn->ibc_peer->ibp_ni, &zombies, -ECONNABORTED);
 }
 
-void
+static void
 kiblnd_finalise_conn (kib_conn_t *conn)
 {
 	LASSERT (!in_interrupt());
@@ -2163,7 +2165,7 @@ kiblnd_connreq_done(kib_conn_t *conn, int status)
 	kiblnd_handle_early_rxs(conn);
 }
 
-void
+static void
 kiblnd_reject(struct rdma_cm_id *cmid, kib_rej_t *rej)
 {
 	int	  rc;
@@ -2174,7 +2176,7 @@ kiblnd_reject(struct rdma_cm_id *cmid, kib_rej_t *rej)
 		CWARN("Error %d sending reject\n", rc);
 }
 
-int
+static int
 kiblnd_passive_connect (struct rdma_cm_id *cmid, void *priv, int priv_nob)
 {
 	rwlock_t		*g_lock = &kiblnd_data.kib_global_lock;
@@ -2448,7 +2450,7 @@ kiblnd_passive_connect (struct rdma_cm_id *cmid, void *priv, int priv_nob)
 	return -ECONNREFUSED;
 }
 
-void
+static void
 kiblnd_reconnect (kib_conn_t *conn, int version,
 		  __u64 incarnation, int why, kib_connparams_t *cp)
 {
@@ -2512,7 +2514,7 @@ kiblnd_reconnect (kib_conn_t *conn, int version,
 	kiblnd_connect_peer(peer);
 }
 
-void
+static void
 kiblnd_rejected (kib_conn_t *conn, int reason, void *priv, int priv_nob)
 {
 	kib_peer_t    *peer = conn->ibc_peer;
@@ -2609,13 +2611,17 @@ kiblnd_rejected (kib_conn_t *conn, int reason, void *priv, int priv_nob)
 
 			case IBLND_REJECT_MSG_QUEUE_SIZE:
 				CERROR("%s rejected: incompatible message queue depth %d, %d\n",
-				       libcfs_nid2str(peer->ibp_nid), cp->ibcp_queue_depth,
+				       libcfs_nid2str(peer->ibp_nid),
+				       cp != NULL ? cp->ibcp_queue_depth :
+				       IBLND_MSG_QUEUE_SIZE(rej->ibr_version),
 				       IBLND_MSG_QUEUE_SIZE(conn->ibc_version));
 				break;
 
 			case IBLND_REJECT_RDMA_FRAGS:
 				CERROR("%s rejected: incompatible # of RDMA fragments %d, %d\n",
-				       libcfs_nid2str(peer->ibp_nid), cp->ibcp_max_frags,
+				       libcfs_nid2str(peer->ibp_nid),
+				       cp != NULL ? cp->ibcp_max_frags :
+				       IBLND_RDMA_FRAGS(rej->ibr_version),
 				       IBLND_RDMA_FRAGS(conn->ibc_version));
 				break;
 
@@ -2647,7 +2653,7 @@ kiblnd_rejected (kib_conn_t *conn, int reason, void *priv, int priv_nob)
 	kiblnd_connreq_done(conn, -ECONNREFUSED);
 }
 
-void
+static void
 kiblnd_check_connreply (kib_conn_t *conn, void *priv, int priv_nob)
 {
 	kib_peer_t    *peer = conn->ibc_peer;
@@ -2746,7 +2752,7 @@ kiblnd_check_connreply (kib_conn_t *conn, void *priv, int priv_nob)
 	kiblnd_connreq_done(conn, 0);
 }
 
-int
+static int
 kiblnd_active_connect (struct rdma_cm_id *cmid)
 {
 	kib_peer_t	      *peer = (kib_peer_t *)cmid->context;
@@ -3017,7 +3023,7 @@ kiblnd_conn_timed_out_locked(kib_conn_t *conn)
 		kiblnd_check_txs_locked(conn, &conn->ibc_active_txs);
 }
 
-void
+static void
 kiblnd_check_conns (int idx)
 {
 	LIST_HEAD (closes);
@@ -3100,7 +3106,7 @@ kiblnd_check_conns (int idx)
 	}
 }
 
-void
+static void
 kiblnd_disconnect_conn (kib_conn_t *conn)
 {
 	LASSERT (!in_interrupt());
@@ -3239,7 +3245,7 @@ kiblnd_qp_event(struct ib_event *event, void *arg)
 	}
 }
 
-void
+static void
 kiblnd_complete (struct ib_wc *wc)
 {
 	switch (kiblnd_wreqid2type(wc->wr_id)) {
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c
index 21d36ee5378..775dcd29c20 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c
@@ -58,10 +58,10 @@ ksocknal_ip2iface(lnet_ni_t *ni, __u32 ip)
 		iface = &net->ksnn_interfaces[i];
 
 		if (iface->ksni_ipaddr == ip)
-			return (iface);
+			return iface;
 	}
 
-	return (NULL);
+	return NULL;
 }
 
 ksock_route_t *
@@ -71,7 +71,7 @@ ksocknal_create_route (__u32 ipaddr, int port)
 
 	LIBCFS_ALLOC (route, sizeof (*route));
 	if (route == NULL)
-		return (NULL);
+		return NULL;
 
 	atomic_set (&route->ksnr_refcount, 1);
 	route->ksnr_peer = NULL;
@@ -85,7 +85,7 @@ ksocknal_create_route (__u32 ipaddr, int port)
 	route->ksnr_conn_count = 0;
 	route->ksnr_share_count = 0;
 
-	return (route);
+	return route;
 }
 
 void
@@ -197,9 +197,9 @@ ksocknal_find_peer_locked (lnet_ni_t *ni, lnet_process_id_t id)
 		CDEBUG(D_NET, "got peer [%p] -> %s (%d)\n",
 		       peer, libcfs_id2str(id),
 		       atomic_read(&peer->ksnp_refcount));
-		return (peer);
+		return peer;
 	}
-	return (NULL);
+	return NULL;
 }
 
 ksock_peer_t *
@@ -213,7 +213,7 @@ ksocknal_find_peer (lnet_ni_t *ni, lnet_process_id_t id)
 		ksocknal_peer_addref(peer);
 	read_unlock(&ksocknal_data.ksnd_global_lock);
 
-	return (peer);
+	return peer;
 }
 
 void
@@ -318,7 +318,7 @@ ksocknal_get_peer_info (lnet_ni_t *ni, int index,
 	}
  out:
 	read_unlock(&ksocknal_data.ksnd_global_lock);
-	return (rc);
+	return rc;
 }
 
 void
@@ -459,7 +459,7 @@ ksocknal_add_peer (lnet_ni_t *ni, lnet_process_id_t id, __u32 ipaddr, int port)
 
 	if (id.nid == LNET_NID_ANY ||
 	    id.pid == LNET_PID_ANY)
-		return (-EINVAL);
+		return -EINVAL;
 
 	/* Have a brand new peer ready... */
 	rc = ksocknal_create_peer(&peer, ni, id);
@@ -469,7 +469,7 @@ ksocknal_add_peer (lnet_ni_t *ni, lnet_process_id_t id, __u32 ipaddr, int port)
 	route = ksocknal_create_route (ipaddr, port);
 	if (route == NULL) {
 		ksocknal_peer_decref(peer);
-		return (-ENOMEM);
+		return -ENOMEM;
 	}
 
 	write_lock_bh(&ksocknal_data.ksnd_global_lock);
@@ -506,7 +506,7 @@ ksocknal_add_peer (lnet_ni_t *ni, lnet_process_id_t id, __u32 ipaddr, int port)
 
 	write_unlock_bh(&ksocknal_data.ksnd_global_lock);
 
-	return (0);
+	return 0;
 }
 
 void
@@ -620,7 +620,7 @@ ksocknal_del_peer (lnet_ni_t *ni, lnet_process_id_t id, __u32 ip)
 
 	ksocknal_txlist_done(ni, &zombies, 1);
 
-	return (rc);
+	return rc;
 }
 
 ksock_conn_t *
@@ -650,15 +650,14 @@ ksocknal_get_conn_by_idx (lnet_ni_t *ni, int index)
 				conn = list_entry (ctmp, ksock_conn_t,
 						       ksnc_list);
 				ksocknal_conn_addref(conn);
-				read_unlock(&ksocknal_data. \
-						 ksnd_global_lock);
-				return (conn);
+				read_unlock(&ksocknal_data.ksnd_global_lock);
+				return conn;
 			}
 		}
 	}
 
 	read_unlock(&ksocknal_data.ksnd_global_lock);
-	return (NULL);
+	return NULL;
 }
 
 ksock_sched_t *
@@ -709,7 +708,7 @@ ksocknal_local_ipvec (lnet_ni_t *ni, __u32 *ipaddrs)
 	}
 
 	read_unlock(&ksocknal_data.ksnd_global_lock);
-	return (nip);
+	return nip;
 }
 
 int
@@ -741,7 +740,7 @@ ksocknal_match_peerip (ksock_interface_t *iface, __u32 *ips, int nips)
 	}
 
 	LASSERT (best >= 0);
-	return (best);
+	return best;
 }
 
 int
@@ -793,8 +792,6 @@ ksocknal_select_ips(ksock_peer_t *peer, __u32 *peerips, int n_peerips)
 			ip = peer->ksnp_passive_ips[i];
 			best_iface = ksocknal_ip2iface(peer->ksnp_ni, ip);
 
-			/* peer passive ips are kept up to date */
-			LASSERT(best_iface != NULL);
 		} else {
 			/* choose a new interface */
 			LASSERT (i == peer->ksnp_n_passive_ips);
@@ -835,8 +832,6 @@ ksocknal_select_ips(ksock_peer_t *peer, __u32 *peerips, int n_peerips)
 			peer->ksnp_n_passive_ips = i+1;
 		}
 
-		LASSERT (best_iface != NULL);
-
 		/* mark the best matching peer IP used */
 		j = ksocknal_match_peerip(best_iface, peerips, n_peerips);
 		peerips[j] = 0;
@@ -847,7 +842,7 @@ ksocknal_select_ips(ksock_peer_t *peer, __u32 *peerips, int n_peerips)
 
 	write_unlock_bh(global_lock);
 
-	return (n_ips);
+	return n_ips;
 }
 
 void
@@ -1571,7 +1566,7 @@ ksocknal_terminate_conn (ksock_conn_t *conn)
 	conn->ksnc_tx_ready = 1;
 
 	if (!conn->ksnc_tx_scheduled &&
-	    !list_empty(&conn->ksnc_tx_queue)){
+	    !list_empty(&conn->ksnc_tx_queue)) {
 		list_add_tail (&conn->ksnc_tx_list,
 			       &sched->kss_tx_conns);
 		conn->ksnc_tx_scheduled = 1;
@@ -1709,7 +1704,7 @@ ksocknal_close_peer_conns_locked (ksock_peer_t *peer, __u32 ipaddr, int why)
 		}
 	}
 
-	return (count);
+	return count;
 }
 
 int
@@ -1725,7 +1720,7 @@ ksocknal_close_conn_and_siblings (ksock_conn_t *conn, int why)
 
 	write_unlock_bh(&ksocknal_data.ksnd_global_lock);
 
-	return (count);
+	return count;
 }
 
 int
@@ -1766,7 +1761,7 @@ ksocknal_close_matching_conns (lnet_process_id_t id, __u32 ipaddr)
 
 	/* wildcards always succeed */
 	if (id.nid == LNET_NID_ANY || id.pid == LNET_PID_ANY || ipaddr == 0)
-		return (0);
+		return 0;
 
 	if (count == 0)
 		return -ENOENT;
@@ -1937,7 +1932,7 @@ ksocknal_push (lnet_ni_t *ni, lnet_process_id_t id)
 
 	}
 
-	return (rc);
+	return rc;
 }
 
 int
@@ -1955,7 +1950,7 @@ ksocknal_add_interface(lnet_ni_t *ni, __u32 ipaddress, __u32 netmask)
 
 	if (ipaddress == 0 ||
 	    netmask == 0)
-		return (-EINVAL);
+		return -EINVAL;
 
 	write_lock_bh(&ksocknal_data.ksnd_global_lock);
 
@@ -1999,7 +1994,7 @@ ksocknal_add_interface(lnet_ni_t *ni, __u32 ipaddress, __u32 netmask)
 
 	write_unlock_bh(&ksocknal_data.ksnd_global_lock);
 
-	return (rc);
+	return rc;
 }
 
 void
@@ -2088,7 +2083,7 @@ ksocknal_del_interface(lnet_ni_t *ni, __u32 ipaddress)
 
 	write_unlock_bh(&ksocknal_data.ksnd_global_lock);
 
-	return (rc);
+	return rc;
 }
 
 int
@@ -2098,7 +2093,7 @@ ksocknal_ctl(lnet_ni_t *ni, unsigned int cmd, void *arg)
 	struct libcfs_ioctl_data *data = arg;
 	int rc;
 
-	switch(cmd) {
+	switch (cmd) {
 	case IOC_LIBCFS_GET_INTERFACE: {
 		ksock_net_t       *net = ni->ni_data;
 		ksock_interface_t *iface;
@@ -2299,12 +2294,12 @@ ksocknal_base_shutdown(void)
 				for (j = 0; j < info->ksi_nthreads_max; j++) {
 
 					sched = &info->ksi_scheds[j];
-					LASSERT(list_empty(&sched->\
-							       kss_tx_conns));
-					LASSERT(list_empty(&sched->\
-							       kss_rx_conns));
-					LASSERT(list_empty(&sched-> \
-						  kss_zombie_noop_txs));
+					LASSERT(list_empty(
+						&sched->kss_tx_conns));
+					LASSERT(list_empty(
+						&sched->kss_rx_conns));
+					LASSERT(list_empty(
+						&sched->kss_zombie_noop_txs));
 					LASSERT(sched->kss_nconns == 0);
 				}
 			}
@@ -2514,7 +2509,8 @@ ksocknal_debug_peerhash (lnet_ni_t *ni)
 		list_for_each (tmp, &ksocknal_data.ksnd_peers[i]) {
 			peer = list_entry (tmp, ksock_peer_t, ksnp_list);
 
-			if (peer->ksnp_ni == ni) break;
+			if (peer->ksnp_ni == ni)
+				break;
 
 			peer = NULL;
 		}
@@ -2683,8 +2679,8 @@ ksocknal_search_new_ipif(ksock_net_t *net)
 		list_for_each_entry(tmp, &ksocknal_data.ksnd_nets,
 					ksnn_list) {
 			for (j = 0; !found && j < tmp->ksnn_ninterfaces; j++) {
-				char *ifnam2 = &tmp->ksnn_interfaces[j].\
-					     ksni_name[0];
+				char *ifnam2 =
+					&tmp->ksnn_interfaces[j].ksni_name[0];
 				char *colon2 = strchr(ifnam2, ':');
 
 				if (colon2 != NULL)
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h
index df2be7a7f46..109a2393247 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h
@@ -418,7 +418,7 @@ ksocknal_nid2peerlist (lnet_nid_t nid)
 {
 	unsigned int hash = ((unsigned int)nid) % ksocknal_data.ksnd_peer_hash_size;
 
-	return (&ksocknal_data.ksnd_peers [hash]);
+	return &ksocknal_data.ksnd_peers[hash];
 }
 
 static inline void
@@ -452,7 +452,7 @@ ksocknal_connsock_addref (ksock_conn_t *conn)
 	}
 	read_unlock(&ksocknal_data.ksnd_global_lock);
 
-	return (rc);
+	return rc;
 }
 
 static inline void
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
index bdf95ea8a54..75bd6583493 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
@@ -120,7 +120,7 @@ ksocknal_send_iov (ksock_conn_t *conn, ksock_tx_t *tx)
 	rc = ksocknal_lib_send_iov(conn, tx);
 
 	if (rc <= 0)			    /* sent nothing? */
-		return (rc);
+		return rc;
 
 	nob = rc;
 	LASSERT (nob <= tx->tx_resid);
@@ -133,7 +133,7 @@ ksocknal_send_iov (ksock_conn_t *conn, ksock_tx_t *tx)
 		if (nob < (int) iov->iov_len) {
 			iov->iov_base = (void *)((char *)iov->iov_base + nob);
 			iov->iov_len -= nob;
-			return (rc);
+			return rc;
 		}
 
 		nob -= iov->iov_len;
@@ -141,7 +141,7 @@ ksocknal_send_iov (ksock_conn_t *conn, ksock_tx_t *tx)
 		tx->tx_niov--;
 	} while (nob != 0);
 
-	return (rc);
+	return rc;
 }
 
 int
@@ -158,7 +158,7 @@ ksocknal_send_kiov (ksock_conn_t *conn, ksock_tx_t *tx)
 	rc = ksocknal_lib_send_kiov(conn, tx);
 
 	if (rc <= 0)			    /* sent nothing? */
-		return (rc);
+		return rc;
 
 	nob = rc;
 	LASSERT (nob <= tx->tx_resid);
@@ -179,7 +179,7 @@ ksocknal_send_kiov (ksock_conn_t *conn, ksock_tx_t *tx)
 		tx->tx_nkiov--;
 	} while (nob != 0);
 
-	return (rc);
+	return rc;
 }
 
 int
@@ -198,7 +198,7 @@ ksocknal_transmit (ksock_conn_t *conn, ksock_tx_t *tx)
 	rc = ksocknal_connsock_addref(conn);
 	if (rc != 0) {
 		LASSERT (conn->ksnc_closing);
-		return (-ESHUTDOWN);
+		return -ESHUTDOWN;
 	}
 
 	do {
@@ -245,7 +245,7 @@ ksocknal_transmit (ksock_conn_t *conn, ksock_tx_t *tx)
 	} while (tx->tx_resid != 0);
 
 	ksocknal_connsock_decref(conn);
-	return (rc);
+	return rc;
 }
 
 int
@@ -262,7 +262,7 @@ ksocknal_recv_iov (ksock_conn_t *conn)
 	rc = ksocknal_lib_recv_iov(conn);
 
 	if (rc <= 0)
-		return (rc);
+		return rc;
 
 	/* received something... */
 	nob = rc;
@@ -282,7 +282,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);
-			return (-EAGAIN);
+			return -EAGAIN;
 		}
 
 		nob -= iov->iov_len;
@@ -290,7 +290,7 @@ ksocknal_recv_iov (ksock_conn_t *conn)
 		conn->ksnc_rx_niov--;
 	} while (nob != 0);
 
-	return (rc);
+	return rc;
 }
 
 int
@@ -306,7 +306,7 @@ ksocknal_recv_kiov (ksock_conn_t *conn)
 	rc = ksocknal_lib_recv_kiov(conn);
 
 	if (rc <= 0)
-		return (rc);
+		return rc;
 
 	/* received something... */
 	nob = rc;
@@ -353,7 +353,7 @@ ksocknal_receive (ksock_conn_t *conn)
 	rc = ksocknal_connsock_addref(conn);
 	if (rc != 0) {
 		LASSERT (conn->ksnc_closing);
-		return (-ESHUTDOWN);
+		return -ESHUTDOWN;
 	}
 
 	for (;;) {
@@ -515,11 +515,11 @@ ksocknal_process_transmit (ksock_conn_t *conn, ksock_tx_t *tx)
 		/* Sent everything OK */
 		LASSERT (rc == 0);
 
-		return (0);
+		return 0;
 	}
 
 	if (rc == -EAGAIN)
-		return (rc);
+		return rc;
 
 	if (rc == -ENOMEM) {
 		static int counter;
@@ -542,7 +542,7 @@ ksocknal_process_transmit (ksock_conn_t *conn, ksock_tx_t *tx)
 			wake_up (&ksocknal_data.ksnd_reaper_waitq);
 
 		spin_unlock_bh(&ksocknal_data.ksnd_reaper_lock);
-		return (rc);
+		return rc;
 	}
 
 	/* Actual error */
@@ -576,7 +576,7 @@ ksocknal_process_transmit (ksock_conn_t *conn, ksock_tx_t *tx)
 	ksocknal_close_conn_and_siblings (conn,
 					  (conn->ksnc_closing) ? 0 : rc);
 
-	return (rc);
+	return rc;
 }
 
 void
@@ -808,10 +808,10 @@ ksocknal_find_connectable_route_locked (ksock_peer_t *peer)
 			continue;
 		}
 
-		return (route);
+		return route;
 	}
 
-	return (NULL);
+	return NULL;
 }
 
 ksock_route_t *
@@ -826,10 +826,10 @@ ksocknal_find_connecting_route_locked (ksock_peer_t *peer)
 		LASSERT (!route->ksnr_connecting || route->ksnr_scheduled);
 
 		if (route->ksnr_scheduled)
-			return (route);
+			return route;
 	}
 
-	return (NULL);
+	return NULL;
 }
 
 int
@@ -857,7 +857,7 @@ ksocknal_launch_packet (lnet_ni_t *ni, ksock_tx_t *tx, lnet_process_id_t id)
 					 * connection... */
 					ksocknal_queue_tx_locked (tx, conn);
 					read_unlock(g_lock);
-					return (0);
+					return 0;
 				}
 			}
 		}
@@ -901,7 +901,7 @@ ksocknal_launch_packet (lnet_ni_t *ni, ksock_tx_t *tx, lnet_process_id_t id)
 		/* Connection exists; queue message on it */
 		ksocknal_queue_tx_locked (tx, conn);
 		write_unlock_bh(g_lock);
-		return (0);
+		return 0;
 	}
 
 	if (peer->ksnp_accepting > 0 ||
@@ -920,7 +920,7 @@ ksocknal_launch_packet (lnet_ni_t *ni, ksock_tx_t *tx, lnet_process_id_t id)
 
 	/* NB Routes may be ignored if connections to them failed recently */
 	CNETERR("No usable routes to %s\n", libcfs_id2str(id));
-	return (-EHOSTUNREACH);
+	return -EHOSTUNREACH;
 }
 
 int
@@ -965,7 +965,7 @@ ksocknal_send(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg)
 		       type, desc_size);
 		if (lntmsg->msg_vmflush)
 			cfs_memory_pressure_restore(mpflag);
-		return (-ENOMEM);
+		return -ENOMEM;
 	}
 
 	tx->tx_conn = NULL;		     /* set when assigned a conn */
@@ -999,10 +999,10 @@ ksocknal_send(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg)
 		cfs_memory_pressure_restore(mpflag);
 
 	if (rc == 0)
-		return (0);
+		return 0;
 
 	ksocknal_free_tx(tx);
-	return (-EIO);
+	return -EIO;
 }
 
 int
@@ -1078,7 +1078,7 @@ ksocknal_new_packet (ksock_conn_t *conn, int nob_to_skip)
 		conn->ksnc_rx_kiov = NULL;
 		conn->ksnc_rx_nkiov = 0;
 		conn->ksnc_rx_csum = ~0;
-		return (1);
+		return 1;
 	}
 
 	/* Set up to skip as much as possible now.  If there's more left
@@ -1106,7 +1106,7 @@ ksocknal_new_packet (ksock_conn_t *conn, int nob_to_skip)
 	conn->ksnc_rx_kiov = NULL;
 	conn->ksnc_rx_nkiov = 0;
 	conn->ksnc_rx_nob_wanted = skipped;
-	return (0);
+	return 0;
 }
 
 int
@@ -1153,7 +1153,7 @@ ksocknal_process_receive (ksock_conn_t *conn)
 
 		if (conn->ksnc_rx_nob_wanted != 0) {
 			/* short read */
-			return (-EAGAIN);
+			return -EAGAIN;
 		}
 	}
 	switch (conn->ksnc_rx_state) {
@@ -1172,7 +1172,7 @@ ksocknal_process_receive (ksock_conn_t *conn)
 			       conn->ksnc_msg.ksm_type);
 			ksocknal_new_packet(conn, 0);
 			ksocknal_close_conn_and_siblings(conn, -EPROTO);
-			return (-EPROTO);
+			return -EPROTO;
 		}
 
 		if (conn->ksnc_msg.ksm_type == KSOCK_MSG_NOOP &&
@@ -1184,7 +1184,7 @@ ksocknal_process_receive (ksock_conn_t *conn)
 			       conn->ksnc_msg.ksm_csum, conn->ksnc_rx_csum);
 			ksocknal_new_packet(conn, 0);
 			ksocknal_close_conn_and_siblings(conn, -EPROTO);
-			return (-EIO);
+			return -EIO;
 		}
 
 		if (conn->ksnc_msg.ksm_zc_cookies[1] != 0) {
@@ -1204,7 +1204,7 @@ ksocknal_process_receive (ksock_conn_t *conn)
 				       cookie, conn->ksnc_msg.ksm_zc_cookies[1]);
 				ksocknal_new_packet(conn, 0);
 				ksocknal_close_conn_and_siblings(conn, -EPROTO);
-				return (rc);
+				return rc;
 			}
 		}
 
@@ -1252,7 +1252,7 @@ ksocknal_process_receive (ksock_conn_t *conn)
 			ksocknal_new_packet(conn, 0);
 			ksocknal_close_conn_and_siblings (conn, rc);
 			ksocknal_conn_decref(conn);
-			return (-EPROTO);
+			return -EPROTO;
 		}
 
 		/* I'm racing with ksocknal_recv() */
@@ -1295,7 +1295,7 @@ ksocknal_process_receive (ksock_conn_t *conn)
 		if (rc != 0) {
 			ksocknal_new_packet(conn, 0);
 			ksocknal_close_conn_and_siblings (conn, rc);
-			return (-EPROTO);
+			return -EPROTO;
 		}
 		/* Fall through */
 
@@ -1311,7 +1311,7 @@ ksocknal_process_receive (ksock_conn_t *conn)
 
 	/* Not Reached */
 	LBUG ();
-	return (-EINVAL);		       /* keep gcc happy */
+	return -EINVAL;		       /* keep gcc happy */
 }
 
 int
@@ -1679,13 +1679,13 @@ ksocknal_invert_type(int type)
 	{
 	case SOCKLND_CONN_ANY:
 	case SOCKLND_CONN_CONTROL:
-		return (type);
+		return type;
 	case SOCKLND_CONN_BULK_IN:
 		return SOCKLND_CONN_BULK_OUT;
 	case SOCKLND_CONN_BULK_OUT:
 		return SOCKLND_CONN_BULK_IN;
 	default:
-		return (SOCKLND_CONN_NONE);
+		return SOCKLND_CONN_NONE;
 	}
 }
 
@@ -2291,7 +2291,7 @@ ksocknal_find_timed_out_conn (ksock_peer_t *peer)
 				break;
 			}
 
-			return (conn);
+			return conn;
 		}
 
 		if (conn->ksnc_rx_started &&
@@ -2307,7 +2307,7 @@ ksocknal_find_timed_out_conn (ksock_peer_t *peer)
 				conn->ksnc_rx_state,
 				conn->ksnc_rx_nob_wanted,
 				conn->ksnc_rx_nob_left);
-			return (conn);
+			return conn;
 		}
 
 		if ((!list_empty(&conn->ksnc_tx_queue) ||
@@ -2322,11 +2322,11 @@ ksocknal_find_timed_out_conn (ksock_peer_t *peer)
 				libcfs_id2str(peer->ksnp_id),
 				&conn->ksnc_ipaddr,
 				conn->ksnc_port);
-			return (conn);
+			return conn;
 		}
 	}
 
-	return (NULL);
+	return NULL;
 }
 
 static inline void
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.c
index 37758d1c8a6..d18bab19cd9 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.c
@@ -188,7 +188,6 @@ void
 ksocknal_lib_eager_ack (ksock_conn_t *conn)
 {
 	int	    opt = 1;
-	mm_segment_t   oldmm = get_fs();
 	struct socket *sock = conn->ksnc_sock;
 
 	/* Remind the socket to ACK eagerly.  If I don't, the socket might
@@ -196,10 +195,8 @@ ksocknal_lib_eager_ack (ksock_conn_t *conn)
 	 * on, introducing delay in completing zero-copy sends in my
 	 * peer. */
 
-	set_fs(KERNEL_DS);
-	sock->ops->setsockopt (sock, SOL_TCP, TCP_QUICKACK,
+	kernel_setsockopt(sock, SOL_TCP, TCP_QUICKACK,
 			       (char *)&opt, sizeof (opt));
-	set_fs(oldmm);
 }
 
 int
@@ -383,7 +380,7 @@ ksocknal_lib_recv_kiov (ksock_conn_t *conn)
 			kunmap(kiov[i].kiov_page);
 	}
 
-	return (rc);
+	return rc;
 }
 
 void
@@ -428,7 +425,6 @@ ksocknal_lib_csum_tx(ksock_tx_t *tx)
 int
 ksocknal_lib_get_conn_tunables (ksock_conn_t *conn, int *txmem, int *rxmem, int *nagle)
 {
-	mm_segment_t   oldmm = get_fs ();
 	struct socket *sock = conn->ksnc_sock;
 	int	    len;
 	int	    rc;
@@ -437,16 +433,14 @@ ksocknal_lib_get_conn_tunables (ksock_conn_t *conn, int *txmem, int *rxmem, int
 	if (rc != 0) {
 		LASSERT (conn->ksnc_closing);
 		*txmem = *rxmem = *nagle = 0;
-		return (-ESHUTDOWN);
+		return -ESHUTDOWN;
 	}
 
 	rc = libcfs_sock_getbuf(sock, txmem, rxmem);
 	if (rc == 0) {
 		len = sizeof(*nagle);
-		set_fs(KERNEL_DS);
-		rc = sock->ops->getsockopt(sock, SOL_TCP, TCP_NODELAY,
+		rc = kernel_getsockopt(sock, SOL_TCP, TCP_NODELAY,
 					   (char *)nagle, &len);
-		set_fs(oldmm);
 	}
 
 	ksocknal_connsock_decref(conn);
@@ -456,13 +450,12 @@ ksocknal_lib_get_conn_tunables (ksock_conn_t *conn, int *txmem, int *rxmem, int
 	else
 		*txmem = *rxmem = *nagle = 0;
 
-	return (rc);
+	return rc;
 }
 
 int
 ksocknal_lib_setup_sock (struct socket *sock)
 {
-	mm_segment_t    oldmm = get_fs ();
 	int	     rc;
 	int	     option;
 	int	     keep_idle;
@@ -479,35 +472,29 @@ ksocknal_lib_setup_sock (struct socket *sock)
 	linger.l_onoff = 0;
 	linger.l_linger = 0;
 
-	set_fs (KERNEL_DS);
-	rc = sock_setsockopt (sock, SOL_SOCKET, SO_LINGER,
+	rc = kernel_setsockopt(sock, SOL_SOCKET, SO_LINGER,
 			      (char *)&linger, sizeof (linger));
-	set_fs (oldmm);
 	if (rc != 0) {
 		CERROR ("Can't set SO_LINGER: %d\n", rc);
-		return (rc);
+		return rc;
 	}
 
 	option = -1;
-	set_fs (KERNEL_DS);
-	rc = sock->ops->setsockopt (sock, SOL_TCP, TCP_LINGER2,
+	rc = kernel_setsockopt(sock, SOL_TCP, TCP_LINGER2,
 				    (char *)&option, sizeof (option));
-	set_fs (oldmm);
 	if (rc != 0) {
 		CERROR ("Can't set SO_LINGER2: %d\n", rc);
-		return (rc);
+		return rc;
 	}
 
 	if (!*ksocknal_tunables.ksnd_nagle) {
 		option = 1;
 
-		set_fs (KERNEL_DS);
-		rc = sock->ops->setsockopt (sock, SOL_TCP, TCP_NODELAY,
+		rc = kernel_setsockopt(sock, SOL_TCP, TCP_NODELAY,
 					    (char *)&option, sizeof (option));
-		set_fs (oldmm);
 		if (rc != 0) {
 			CERROR ("Can't disable nagle: %d\n", rc);
-			return (rc);
+			return rc;
 		}
 	}
 
@@ -518,7 +505,7 @@ ksocknal_lib_setup_sock (struct socket *sock)
 		CERROR ("Can't set buffer tx %d, rx %d buffers: %d\n",
 			*ksocknal_tunables.ksnd_tx_buffer_size,
 			*ksocknal_tunables.ksnd_rx_buffer_size, rc);
-		return (rc);
+		return rc;
 	}
 
 /* TCP_BACKOFF_* sockopt tunables unsupported in stock kernels */
@@ -531,46 +518,38 @@ ksocknal_lib_setup_sock (struct socket *sock)
 	do_keepalive = (keep_idle > 0 && keep_count > 0 && keep_intvl > 0);
 
 	option = (do_keepalive ? 1 : 0);
-	set_fs (KERNEL_DS);
-	rc = sock_setsockopt (sock, SOL_SOCKET, SO_KEEPALIVE,
+	rc = kernel_setsockopt(sock, SOL_SOCKET, SO_KEEPALIVE,
 			      (char *)&option, sizeof (option));
-	set_fs (oldmm);
 	if (rc != 0) {
 		CERROR ("Can't set SO_KEEPALIVE: %d\n", rc);
-		return (rc);
+		return rc;
 	}
 
 	if (!do_keepalive)
-		return (0);
+		return 0;
 
-	set_fs (KERNEL_DS);
-	rc = sock->ops->setsockopt (sock, SOL_TCP, TCP_KEEPIDLE,
+	rc = kernel_setsockopt(sock, SOL_TCP, TCP_KEEPIDLE,
 				    (char *)&keep_idle, sizeof (keep_idle));
-	set_fs (oldmm);
 	if (rc != 0) {
 		CERROR ("Can't set TCP_KEEPIDLE: %d\n", rc);
-		return (rc);
+		return rc;
 	}
 
-	set_fs (KERNEL_DS);
-	rc = sock->ops->setsockopt (sock, SOL_TCP, TCP_KEEPINTVL,
+	rc = kernel_setsockopt(sock, SOL_TCP, TCP_KEEPINTVL,
 				    (char *)&keep_intvl, sizeof (keep_intvl));
-	set_fs (oldmm);
 	if (rc != 0) {
 		CERROR ("Can't set TCP_KEEPINTVL: %d\n", rc);
-		return (rc);
+		return rc;
 	}
 
-	set_fs (KERNEL_DS);
-	rc = sock->ops->setsockopt (sock, SOL_TCP, TCP_KEEPCNT,
+	rc = kernel_setsockopt(sock, SOL_TCP, TCP_KEEPCNT,
 				    (char *)&keep_count, sizeof (keep_count));
-	set_fs (oldmm);
 	if (rc != 0) {
 		CERROR ("Can't set TCP_KEEPCNT: %d\n", rc);
-		return (rc);
+		return rc;
 	}
 
-	return (0);
+	return 0;
 }
 
 void
@@ -581,7 +560,6 @@ ksocknal_lib_push_conn (ksock_conn_t *conn)
 	int	     nonagle;
 	int	     val = 1;
 	int	     rc;
-	mm_segment_t    oldmm;
 
 	rc = ksocknal_connsock_addref(conn);
 	if (rc != 0)			    /* being shut down */
@@ -595,15 +573,10 @@ ksocknal_lib_push_conn (ksock_conn_t *conn)
 	tp->nonagle = 1;
 	release_sock (sk);
 
-	oldmm = get_fs ();
-	set_fs (KERNEL_DS);
-
-	rc = sk->sk_prot->setsockopt (sk, SOL_TCP, TCP_NODELAY,
+	rc = kernel_setsockopt(conn->ksnc_sock, SOL_TCP, TCP_NODELAY,
 				      (char *)&val, sizeof (val));
 	LASSERT (rc == 0);
 
-	set_fs (oldmm);
-
 	lock_sock (sk);
 	tp->nonagle = nonagle;
 	release_sock (sk);
diff --git a/drivers/staging/lustre/lnet/lnet/api-ni.c b/drivers/staging/lustre/lnet/lnet/api-ni.c
index 45c23194081..3f878dee2e4 100644
--- a/drivers/staging/lustre/lnet/lnet/api-ni.c
+++ b/drivers/staging/lustre/lnet/lnet/api-ni.c
@@ -127,8 +127,7 @@ lnet_create_remote_nets_table(void)
 static void
 lnet_destroy_remote_nets_table(void)
 {
-	int		i;
-	struct list_head	*hash;
+	int i;
 
 	if (the_lnet.ln_remote_nets_hash == NULL)
 		return;
@@ -137,7 +136,8 @@ lnet_destroy_remote_nets_table(void)
 		LASSERT(list_empty(&the_lnet.ln_remote_nets_hash[i]));
 
 	LIBCFS_FREE(the_lnet.ln_remote_nets_hash,
-		    LNET_REMOTE_NETS_HASH_SIZE * sizeof(*hash));
+		    LNET_REMOTE_NETS_HASH_SIZE *
+		    sizeof(the_lnet.ln_remote_nets_hash[0]));
 	the_lnet.ln_remote_nets_hash = NULL;
 }
 
@@ -338,7 +338,7 @@ lnet_counters_get(lnet_counters_t *counters)
 		counters->send_count   += ctr->send_count;
 		counters->recv_count   += ctr->recv_count;
 		counters->route_count  += ctr->route_count;
-		counters->drop_length  += ctr->drop_length;
+		counters->drop_count   += ctr->drop_count;
 		counters->send_length  += ctr->send_length;
 		counters->recv_length  += ctr->recv_length;
 		counters->route_length += ctr->route_length;
@@ -986,12 +986,11 @@ lnet_shutdown_lndnis (void)
 			break;
 		}
 
-		while (!list_empty(&ni->ni_list)) {
+		if (!list_empty(&ni->ni_list)) {
 			lnet_net_unlock(LNET_LOCK_EX);
 			++i;
 			if ((i & (-i)) == i) {
-				CDEBUG(D_WARNING,
-				       "Waiting for zombie LNI %s\n",
+				CDEBUG(D_WARNING, "Waiting for zombie LNI %s\n",
 				       libcfs_nid2str(ni->ni_nid));
 			}
 			set_current_state(TASK_UNINTERRUPTIBLE);
@@ -1016,6 +1015,8 @@ lnet_shutdown_lndnis (void)
 			       libcfs_nid2str(ni->ni_nid));
 
 		lnet_ni_free(ni);
+		i = 2;
+
 		lnet_net_lock(LNET_LOCK_EX);
 	}
 
@@ -1926,6 +1927,7 @@ lnet_ping (lnet_process_id_t id, int timeout_ms, lnet_process_id_t *ids, int n_i
 
 	rc = -EFAULT;			   /* If I SEGV... */
 
+	memset(&tmpid, 0, sizeof(tmpid));
 	for (i = 0; i < n_ids; i++) {
 		tmpid.pid = info->pi_pid;
 		tmpid.nid = info->pi_ni[i].ns_nid;
diff --git a/drivers/staging/lustre/lnet/lnet/lib-eq.c b/drivers/staging/lustre/lnet/lnet/lib-eq.c
index 7ce07f61b2e..d25dcd8ba56 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-eq.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-eq.c
@@ -238,7 +238,7 @@ lnet_eq_enqueue_event(lnet_eq_t *eq, lnet_event_t *ev)
 	lnet_eq_wait_unlock();
 }
 
-int
+static int
 lnet_eq_dequeue_event(lnet_eq_t *eq, lnet_event_t *ev)
 {
 	int		new_index = eq->eq_deq_seq & (eq->eq_size - 1);
@@ -325,6 +325,7 @@ EXPORT_SYMBOL(LNetEQWait);
 
 static int
 lnet_eq_wait_locked(int *timeout_ms)
+__must_hold(&the_lnet.ln_eq_wait_lock)
 {
 	int		tms = *timeout_ms;
 	int		wait;
diff --git a/drivers/staging/lustre/lnet/lnet/router.c b/drivers/staging/lustre/lnet/lnet/router.c
index 995f50976c4..926923a104c 100644
--- a/drivers/staging/lustre/lnet/lnet/router.c
+++ b/drivers/staging/lustre/lnet/lnet/router.c
@@ -145,7 +145,7 @@ lnet_ni_notify_locked(lnet_ni_t *ni, lnet_peer_t *lp)
 	 * NB individual events can be missed; the only guarantee is that you
 	 * always get the most recent news */
 
-	if (lp->lp_notifying)
+	if (lp->lp_notifying || ni == NULL)
 		return;
 
 	lp->lp_notifying = 1;
diff --git a/drivers/staging/lustre/lustre/fid/lproc_fid.c b/drivers/staging/lustre/lustre/fid/lproc_fid.c
index ddd813cab50..6f5674d1aa7 100644
--- a/drivers/staging/lustre/lustre/fid/lproc_fid.c
+++ b/drivers/staging/lustre/lustre/fid/lproc_fid.c
@@ -54,32 +54,49 @@
 #include <lustre_fid.h>
 #include "fid_internal.h"
 
+/* Format: [0x64BIT_INT - 0x64BIT_INT] + 32 bytes just in case */
+#define MAX_FID_RANGE_STRLEN (32 + 2 * 2 * sizeof(__u64))
 /*
  * Note: this function is only used for testing, it is no safe for production
  * use.
  */
-static int
-lprocfs_fid_write_common(const char *buffer, unsigned long count,
-			 struct lu_seq_range *range)
+static int lprocfs_fid_write_common(const char __user *buffer, size_t count,
+				    struct lu_seq_range *range)
 {
 	struct lu_seq_range tmp;
 	int rc;
+	char kernbuf[MAX_FID_RANGE_STRLEN];
 
 	LASSERT(range != NULL);
 
-	rc = sscanf(buffer, "[%llx - %llx]\n",
+	if (count >= sizeof(kernbuf))
+		return -EINVAL;
+
+	if (copy_from_user(kernbuf, buffer, count))
+		return -EFAULT;
+
+	kernbuf[count] = 0;
+
+	if (count == 5 && strcmp(kernbuf, "clear") == 0) {
+		memset(range, 0, sizeof(*range));
+		return count;
+	}
+
+	/* of the form "[0x0000000240000400 - 0x000000028000400]" */
+	rc = sscanf(kernbuf, "[%llx - %llx]\n",
 		    (long long unsigned *)&tmp.lsr_start,
 		    (long long unsigned *)&tmp.lsr_end);
-	if (rc != 2 || !range_is_sane(&tmp) || range_is_zero(&tmp))
+	if (!range_is_sane(&tmp) || range_is_zero(&tmp) ||
+	    tmp.lsr_start < range->lsr_start || tmp.lsr_end > range->lsr_end)
 		return -EINVAL;
 	*range = tmp;
-	return 0;
+	return count;
 }
 
 /* Client side procfs stuff */
-static ssize_t
-lprocfs_fid_space_seq_write(struct file *file, const char *buffer,
-			    size_t count, loff_t *off)
+static ssize_t lprocfs_fid_space_seq_write(struct file *file,
+					   const char __user *buffer,
+					   size_t count, loff_t *off)
 {
 	struct lu_client_seq *seq = ((struct seq_file *)file->private_data)->private;
 	int rc;
@@ -114,9 +131,9 @@ lprocfs_fid_space_seq_show(struct seq_file *m, void *unused)
 	return rc;
 }
 
-static ssize_t
-lprocfs_fid_width_seq_write(struct file *file, const char *buffer,
-			    size_t count, loff_t *off)
+static ssize_t lprocfs_fid_width_seq_write(struct file *file,
+					   const char __user *buffer,
+					   size_t count, loff_t *off)
 {
 	struct lu_client_seq *seq = ((struct seq_file *)file->private_data)->private;
 	__u64  max;
diff --git a/drivers/staging/lustre/lustre/include/dt_object.h b/drivers/staging/lustre/lustre/include/dt_object.h
index 9304c269afa..9b7921d1dea 100644
--- a/drivers/staging/lustre/lustre/include/dt_object.h
+++ b/drivers/staging/lustre/lustre/include/dt_object.h
@@ -441,7 +441,8 @@ struct dt_object_operations {
 					struct dt_object *dt,
 					struct lustre_capa *old,
 					__u64 opc);
-	int (*do_object_sync)(const struct lu_env *, struct dt_object *);
+	int (*do_object_sync)(const struct lu_env *env, struct dt_object *obj,
+			      __u64 start, __u64 end);
 	/**
 	 * Get object info of next level. Currently, only get inode from osd.
 	 * This is only used by quota b=16542
@@ -900,13 +901,13 @@ static inline int dt_object_lock(const struct lu_env *env,
 int dt_lookup_dir(const struct lu_env *env, struct dt_object *dir,
 		  const char *name, struct lu_fid *fid);
 
-static inline int dt_object_sync(const struct lu_env *env,
-				 struct dt_object *o)
+static inline int dt_object_sync(const struct lu_env *env, struct dt_object *o,
+				 __u64 start, __u64 end)
 {
 	LASSERT(o);
 	LASSERT(o->do_ops);
 	LASSERT(o->do_ops->do_object_sync);
-	return o->do_ops->do_object_sync(env, o);
+	return o->do_ops->do_object_sync(env, o, start, end);
 }
 
 int dt_declare_version_set(const struct lu_env *env, struct dt_object *o,
diff --git a/drivers/staging/lustre/lustre/include/ioctl.h b/drivers/staging/lustre/lustre/include/ioctl.h
deleted file mode 100644
index b986920926b..00000000000
--- a/drivers/staging/lustre/lustre/include/ioctl.h
+++ /dev/null
@@ -1,106 +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 _IOWR
-
-/* On i386 and x86_64, _ASM_I386_IOCTL_H is defined by the kernel's ioctl.h,
- * and on newer kernels this header is shared as _ASM_GENERIC_IOCTL_H.
- *
- * We can avoid any problems with the kernel header being included again by
- * defining _ASM_I386_IOCTL_H here so that a later occurrence of <asm/ioctl.h>
- * does not include the kernel's ioctl.h after this one. b=14746 */
-#define _ASM_I386_IOCTL_H
-#define _ASM_GENERIC_IOCTL_H
-
-/* ioctl command encoding: 32 bits total, command in lower 16 bits,
- * size of the parameter structure in the lower 14 bits of the
- * upper 16 bits.
- * Encoding the size of the parameter structure in the ioctl request
- * The highest 2 bits are reserved for indicating the ``access mode''.
- * NOTE: This limits the max parameter size to 16kB -1 !
- */
-
-/*
- * The following is for compatibility across the various Linux
- * platforms.  The i386 ioctl numbering scheme doesn't really enforce
- * a type field.  De facto, however, the top 8 bits of the lower 16
- * bits are indeed used as a type field, so we might just as well make
- * this explicit here.  Please be sure to use the decoding macros
- * below from now on.
- */
-#define _IOC_NRBITS     8
-#define _IOC_TYPEBITS   8
-#define _IOC_SIZEBITS   14
-#define _IOC_DIRBITS    2
-
-#define _IOC_NRMASK     ((1 << _IOC_NRBITS)-1)
-#define _IOC_TYPEMASK   ((1 << _IOC_TYPEBITS)-1)
-#define _IOC_SIZEMASK   ((1 << _IOC_SIZEBITS)-1)
-#define _IOC_DIRMASK    ((1 << _IOC_DIRBITS)-1)
-
-#define _IOC_NRSHIFT    0
-#define _IOC_TYPESHIFT  (_IOC_NRSHIFT+_IOC_NRBITS)
-#define _IOC_SIZESHIFT  (_IOC_TYPESHIFT+_IOC_TYPEBITS)
-#define _IOC_DIRSHIFT   (_IOC_SIZESHIFT+_IOC_SIZEBITS)
-
-/*
- * Direction bits.
- */
-#define _IOC_NONE       0U
-#define _IOC_WRITE      1U
-#define _IOC_READ       2U
-
-#define _IOC(dir,type,nr,size) (((dir)  << _IOC_DIRSHIFT) | ((type) << _IOC_TYPESHIFT) | ((nr)   << _IOC_NRSHIFT) | ((size) << _IOC_SIZESHIFT))
-
-/* used to create numbers */
-#define _IO(type,nr)	    _IOC(_IOC_NONE,(type),(nr),0)
-#define _IOR(type,nr,size)      _IOC(_IOC_READ,(type),(nr),sizeof(size))
-#define _IOW(type,nr,size)      _IOC(_IOC_WRITE,(type),(nr),sizeof(size))
-#define _IOWR(type,nr,size)     _IOC(_IOC_READ|_IOC_WRITE,(type),(nr),sizeof(size))
-
-/* used to decode ioctl numbers.. */
-#define _IOC_DIR(nr)	    (((nr) >> _IOC_DIRSHIFT) & _IOC_DIRMASK)
-#define _IOC_TYPE(nr)	   (((nr) >> _IOC_TYPESHIFT) & _IOC_TYPEMASK)
-#define _IOC_NR(nr)	     (((nr) >> _IOC_NRSHIFT) & _IOC_NRMASK)
-#define _IOC_SIZE(nr)	   (((nr) >> _IOC_SIZESHIFT) & _IOC_SIZEMASK)
-
-/* ...and for the drivers/sound files... */
-
-#define IOC_IN	  (_IOC_WRITE << _IOC_DIRSHIFT)
-#define IOC_OUT	 (_IOC_READ << _IOC_DIRSHIFT)
-#define IOC_INOUT       ((_IOC_WRITE|_IOC_READ) << _IOC_DIRSHIFT)
-#define IOCSIZE_MASK    (_IOC_SIZEMASK << _IOC_SIZESHIFT)
-#define IOCSIZE_SHIFT   (_IOC_SIZESHIFT)
-
-#endif /* _IOWR */
diff --git a/drivers/staging/lustre/lustre/include/linux/obd.h b/drivers/staging/lustre/lustre/include/linux/obd.h
index dc36f75eb63..f96f65d5168 100644
--- a/drivers/staging/lustre/lustre/include/linux/obd.h
+++ b/drivers/staging/lustre/lustre/include/linux/obd.h
@@ -80,8 +80,8 @@ static inline void __client_obd_list_lock(client_obd_lock_t *lock,
 			break;
 		}
 
-		if ((jiffies - cur > 5 * HZ) &&
-		    (jiffies - lock->time > 5 * HZ)) {
+		if (time_before(cur + 5 * HZ, jiffies) &&
+		    time_before(lock->time + 5 * HZ, jiffies)) {
 			struct task_struct *task = lock->task;
 
 			if (task == NULL)
@@ -104,7 +104,7 @@ static inline void __client_obd_list_lock(client_obd_lock_t *lock,
 }
 
 #define client_obd_list_lock(lock) \
-	__client_obd_list_lock(lock, __FUNCTION__, __LINE__)
+	__client_obd_list_lock(lock, __func__, __LINE__)
 
 static inline void client_obd_list_unlock(client_obd_lock_t *lock)
 {
diff --git a/drivers/staging/lustre/lustre/include/lprocfs_status.h b/drivers/staging/lustre/lustre/include/lprocfs_status.h
index 428e3e4ce05..1b7f6a9bf62 100644
--- a/drivers/staging/lustre/lustre/include/lprocfs_status.h
+++ b/drivers/staging/lustre/lustre/include/lprocfs_status.h
@@ -369,6 +369,7 @@ static inline void s2dhms(struct dhms *ts, time_t secs)
 #define JOBSTATS_JOBID_VAR_MAX_LEN	20
 #define JOBSTATS_DISABLE		"disable"
 #define JOBSTATS_PROCNAME_UID		"procname_uid"
+#define JOBSTATS_NODELOCAL		"nodelocal"
 
 extern int lprocfs_write_frac_helper(const char *buffer, unsigned long count,
 				     int *val, int mult);
@@ -687,7 +688,7 @@ static int name##_single_open(cfs_inode_t *inode, struct file *file)	\
 {									\
 	return single_open(file, name##_seq_show, PDE_DATA(inode));	\
 }									\
-struct file_operations name##_fops = {				     \
+static struct file_operations name##_fops = {				\
 	.owner   = THIS_MODULE,					    \
 	.open    = name##_single_open,				     \
 	.read    = seq_read,					       \
@@ -730,7 +731,7 @@ struct file_operations name##_fops = {				     \
 	{								\
 		return single_open(file, NULL, PDE_DATA(inode));	\
 	}								\
-	struct file_operations name##_##type##_fops = {			\
+	static struct file_operations name##_##type##_fops = {	\
 		.open	= name##_##type##_open,				\
 		.write	= name##_##type##_write,			\
 		.release = lprocfs_single_release,			\
diff --git a/drivers/staging/lustre/lustre/include/lu_object.h b/drivers/staging/lustre/lustre/include/lu_object.h
index 6773bca1e0d..98149f5da15 100644
--- a/drivers/staging/lustre/lustre/include/lu_object.h
+++ b/drivers/staging/lustre/lustre/include/lu_object.h
@@ -516,6 +516,10 @@ enum lu_object_header_attr {
  */
 struct lu_object_header {
 	/**
+	 * Fid, uniquely identifying this object.
+	 */
+	struct lu_fid		loh_fid;
+	/**
 	 * Object flags from enum lu_object_header_flags. Set and checked
 	 * atomically.
 	 */
@@ -525,10 +529,6 @@ struct lu_object_header {
 	 */
 	atomic_t	   loh_ref;
 	/**
-	 * Fid, uniquely identifying this object.
-	 */
-	struct lu_fid	  loh_fid;
-	/**
 	 * Common object attributes, cached for efficiency. From enum
 	 * lu_object_header_attr.
 	 */
diff --git a/drivers/staging/lustre/lustre/include/lu_ref.h b/drivers/staging/lustre/lustre/include/lu_ref.h
index 50a2a7f786d..b451a888ca3 100644
--- a/drivers/staging/lustre/lustre/include/lu_ref.h
+++ b/drivers/staging/lustre/lustre/include/lu_ref.h
@@ -69,12 +69,12 @@
  *
  *	// current thread acquired a temporary reference to foo.
  *	foo_get(foo);
- *	lu_ref_add(&foo->reference, __FUNCTION__, current);
+ *	lu_ref_add(&foo->reference, __func__, current);
  *
  *	...
  *
  *	// temporary reference is released.
- *	lu_ref_del(&foo->reference, __FUNCTION__, current);
+ *	lu_ref_del(&foo->reference, __func__, current);
  *	foo_put(foo);
  * \endcode
  *
diff --git a/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h b/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h
index 87905bbc68e..83014c9fea6 100644
--- a/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h
+++ b/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h
@@ -265,7 +265,7 @@ static inline __u64 range_space(const struct lu_seq_range *range)
 
 static inline void range_init(struct lu_seq_range *range)
 {
-	range->lsr_start = range->lsr_end = range->lsr_index = 0;
+	memset(range, 0, sizeof(*range));
 }
 
 /**
@@ -1682,6 +1682,30 @@ static inline __u32 lov_mds_md_size(__u16 stripes, __u32 lmm_magic)
 				stripes * sizeof(struct lov_ost_data_v1);
 }
 
+static inline __u32
+lov_mds_md_max_stripe_count(size_t buf_size, __u32 lmm_magic)
+{
+	switch (lmm_magic) {
+	case LOV_MAGIC_V1: {
+		struct lov_mds_md_v1 lmm;
+
+		if (buf_size < sizeof(lmm))
+			return 0;
+
+		return (buf_size - sizeof(lmm)) / sizeof(lmm.lmm_objects[0]);
+	}
+	case LOV_MAGIC_V3: {
+		struct lov_mds_md_v3 lmm;
+
+		if (buf_size < sizeof(lmm))
+			return 0;
+
+		return (buf_size - sizeof(lmm)) / sizeof(lmm.lmm_objects[0]);
+	}
+	default:
+		return 0;
+	}
+}
 
 #define OBD_MD_FLID	(0x00000001ULL) /* object ID */
 #define OBD_MD_FLATIME     (0x00000002ULL) /* access time */
@@ -2681,6 +2705,8 @@ enum seq_op {
  * protocol, this will limit the max number of OSTs per LOV */
 
 #define LOV_DESC_MAGIC 0xB0CCDE5C
+#define LOV_DESC_QOS_MAXAGE_DEFAULT 5  /* Seconds */
+#define LOV_DESC_STRIPE_SIZE_DEFAULT (1 << LNET_MTU_BITS)
 
 /* LOV settings descriptor (should only contain static info) */
 struct lov_desc {
diff --git a/drivers/staging/lustre/lustre/include/lustre/lustre_user.h b/drivers/staging/lustre/lustre/include/lustre/lustre_user.h
index f5f369e603d..95c754f2075 100644
--- a/drivers/staging/lustre/lustre/include/lustre/lustre_user.h
+++ b/drivers/staging/lustre/lustre/include/lustre/lustre_user.h
@@ -1106,7 +1106,8 @@ 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-> \
-								hal_fsname)));
+								hal_fsname)
+							 + 1));
 }
 /* Return pointer to next hai */
 static inline struct hsm_action_item * hai_next(struct hsm_action_item *hai)
@@ -1121,7 +1122,7 @@ static inline int hal_size(struct hsm_action_list *hal)
 	int i, sz;
 	struct hsm_action_item *hai;
 
-	sz = sizeof(*hal) + cfs_size_round(strlen(hal->hal_fsname));
+	sz = sizeof(*hal) + cfs_size_round(strlen(hal->hal_fsname) + 1);
 	hai = hai_zero(hal);
 	for (i = 0; i < hal->hal_count; i++, hai = hai_next(hai))
 		sz += cfs_size_round(hai->hai_len);
diff --git a/drivers/staging/lustre/lustre/include/lustre_debug.h b/drivers/staging/lustre/lustre/include/lustre_debug.h
index 7ec91edd680..6146ccb8cce 100644
--- a/drivers/staging/lustre/lustre/include/lustre_debug.h
+++ b/drivers/staging/lustre/lustre/include/lustre_debug.h
@@ -48,7 +48,6 @@
 /* lib/debug.c */
 void dump_lniobuf(struct niobuf_local *lnb);
 int dump_req(struct ptlrpc_request *req);
-void dump_lsm(int level, struct lov_stripe_md *lsm);
 int block_debug_setup(void *addr, int len, __u64 off, __u64 id);
 int block_debug_check(char *who, void *addr, int len, __u64 off, __u64 id);
 
diff --git a/drivers/staging/lustre/lustre/include/lustre_dlm.h b/drivers/staging/lustre/lustre/include/lustre_dlm.h
index 3e25f001c07..0c6b7841e56 100644
--- a/drivers/staging/lustre/lustre/include/lustre_dlm.h
+++ b/drivers/staging/lustre/lustre/include/lustre_dlm.h
@@ -212,7 +212,7 @@ struct ldlm_pool_ops {
 	int (*po_recalc)(struct ldlm_pool *pl);
 	/** Cancel at least \a nr locks from pool \a pl */
 	int (*po_shrink)(struct ldlm_pool *pl, int nr,
-			 unsigned int gfp_mask);
+			 gfp_t gfp_mask);
 	int (*po_setup)(struct ldlm_pool *pl, int limit);
 };
 
@@ -260,7 +260,7 @@ struct ldlm_pool {
 	/** Recalculation period for pool. */
 	time_t			pl_recalc_period;
 	/** Recalculation and shrink operations. */
-	struct ldlm_pool_ops	*pl_ops;
+	const struct ldlm_pool_ops	*pl_ops;
 	/** Number of planned locks for next period. */
 	int			pl_grant_plan;
 	/** Pool statistics. */
@@ -1312,11 +1312,11 @@ int ldlm_lock_change_resource(struct ldlm_namespace *, struct ldlm_lock *,
 			      const struct ldlm_res_id *);
 
 #define LDLM_RESOURCE_ADDREF(res) do {				  \
-	lu_ref_add_atomic(&(res)->lr_reference, __FUNCTION__, current);  \
+	lu_ref_add_atomic(&(res)->lr_reference, __func__, current);  \
 } while (0)
 
 #define LDLM_RESOURCE_DELREF(res) do {				  \
-	lu_ref_del(&(res)->lr_reference, __FUNCTION__, current);  \
+	lu_ref_del(&(res)->lr_reference, __func__, current);	  \
 } while (0)
 
 /* ldlm_request.c */
@@ -1445,7 +1445,7 @@ static inline void unlock_res(struct ldlm_resource *res)
 /** Check if resource is already locked, assert if not. */
 static inline void check_res_locked(struct ldlm_resource *res)
 {
-	LASSERT(spin_is_locked(&res->lr_lock));
+	assert_spin_locked(&res->lr_lock);
 }
 
 struct ldlm_resource * lock_res_and_lock(struct ldlm_lock *lock);
@@ -1463,7 +1463,7 @@ void ldlm_pools_fini(void);
 int ldlm_pool_init(struct ldlm_pool *pl, struct ldlm_namespace *ns,
 		   int idx, ldlm_side_t client);
 int ldlm_pool_shrink(struct ldlm_pool *pl, int nr,
-		     unsigned int gfp_mask);
+		     gfp_t gfp_mask);
 void ldlm_pool_fini(struct ldlm_pool *pl);
 int ldlm_pool_setup(struct ldlm_pool *pl, int limit);
 int ldlm_pool_recalc(struct ldlm_pool *pl);
diff --git a/drivers/staging/lustre/lustre/include/lustre_lib.h b/drivers/staging/lustre/lustre/include/lustre_lib.h
index 0368ca6ffcc..3c26bbdc44b 100644
--- a/drivers/staging/lustre/lustre/include/lustre_lib.h
+++ b/drivers/staging/lustre/lustre/include/lustre_lib.h
@@ -94,19 +94,6 @@ struct obd_client_handle {
 void statfs_pack(struct obd_statfs *osfs, struct kstatfs *sfs);
 void statfs_unpack(struct kstatfs *sfs, struct obd_statfs *osfs);
 
-/* l_lock.c */
-struct lustre_lock {
-	int			l_depth;
-	struct task_struct	*l_owner;
-	struct semaphore	l_sem;
-	spinlock_t		l_spin;
-};
-
-void l_lock_init(struct lustre_lock *);
-void l_lock(struct lustre_lock *);
-void l_unlock(struct lustre_lock *);
-int l_has_lock(struct lustre_lock *);
-
 /*
  * For md echo client
  */
@@ -192,24 +179,25 @@ static inline int obd_ioctl_packlen(struct obd_ioctl_data *data)
 
 static inline int obd_ioctl_is_invalid(struct obd_ioctl_data *data)
 {
-	if (data->ioc_len > (1<<30)) {
-		CERROR("OBD ioctl: ioc_len larger than 1<<30\n");
+	if (data->ioc_len > OBD_MAX_IOCTL_BUFFER) {
+		CERROR("OBD ioctl: ioc_len larger than %d\n",
+		       OBD_MAX_IOCTL_BUFFER);
 		return 1;
 	}
-	if (data->ioc_inllen1 > (1<<30)) {
-		CERROR("OBD ioctl: ioc_inllen1 larger than 1<<30\n");
+	if (data->ioc_inllen1 > OBD_MAX_IOCTL_BUFFER) {
+		CERROR("OBD ioctl: ioc_inllen1 larger than ioc_len\n");
 		return 1;
 	}
-	if (data->ioc_inllen2 > (1<<30)) {
-		CERROR("OBD ioctl: ioc_inllen2 larger than 1<<30\n");
+	if (data->ioc_inllen2 > OBD_MAX_IOCTL_BUFFER) {
+		CERROR("OBD ioctl: ioc_inllen2 larger than ioc_len\n");
 		return 1;
 	}
-	if (data->ioc_inllen3 > (1<<30)) {
-		CERROR("OBD ioctl: ioc_inllen3 larger than 1<<30\n");
+	if (data->ioc_inllen3 > OBD_MAX_IOCTL_BUFFER) {
+		CERROR("OBD ioctl: ioc_inllen3 larger than ioc_len\n");
 		return 1;
 	}
-	if (data->ioc_inllen4 > (1<<30)) {
-		CERROR("OBD ioctl: ioc_inllen4 larger than 1<<30\n");
+	if (data->ioc_inllen4 > OBD_MAX_IOCTL_BUFFER) {
+		CERROR("OBD ioctl: ioc_inllen4 larger than ioc_len\n");
 		return 1;
 	}
 	if (data->ioc_inlbuf1 && !data->ioc_inllen1) {
diff --git a/drivers/staging/lustre/lustre/include/lustre_log.h b/drivers/staging/lustre/lustre/include/lustre_log.h
index 896c7576aa0..1a9a9228b79 100644
--- a/drivers/staging/lustre/lustre/include/lustre_log.h
+++ b/drivers/staging/lustre/lustre/include/lustre_log.h
@@ -206,11 +206,7 @@ int llog_setup(const struct lu_env *env, struct obd_device *obd,
 int __llog_ctxt_put(const struct lu_env *env, struct llog_ctxt *ctxt);
 int llog_cleanup(const struct lu_env *env, struct llog_ctxt *);
 int llog_sync(struct llog_ctxt *ctxt, struct obd_export *exp, int flags);
-int llog_obd_add(const struct lu_env *env, struct llog_ctxt *ctxt,
-		 struct llog_rec_hdr *rec, struct lov_stripe_md *lsm,
-		 struct llog_cookie *logcookies, int numcookies);
 int llog_cancel(const struct lu_env *env, struct llog_ctxt *ctxt,
-		struct lov_stripe_md *lsm, int count,
 		struct llog_cookie *cookies, int flags);
 
 int obd_llog_init(struct obd_device *obd, struct obd_llog_group *olg,
@@ -242,7 +238,6 @@ struct llog_operations {
 			int flags);
 	int (*lop_cleanup)(const struct lu_env *env, struct llog_ctxt *ctxt);
 	int (*lop_cancel)(const struct lu_env *env, struct llog_ctxt *ctxt,
-			  struct lov_stripe_md *lsm, int count,
 			  struct llog_cookie *cookies, int flags);
 	int (*lop_connect)(struct llog_ctxt *ctxt, struct llog_logid *logid,
 			   struct llog_gen *gen, struct obd_uuid *uuid);
@@ -296,11 +291,6 @@ struct llog_operations {
 	int (*lop_add)(const struct lu_env *env, struct llog_handle *lgh,
 		       struct llog_rec_hdr *rec, struct llog_cookie *cookie,
 		       void *buf, struct thandle *th);
-	/* Old llog_add version, used in MDS-LOV-OSC now and will gone with
-	 * LOD/OSP replacement */
-	int (*lop_obd_add)(const struct lu_env *env, struct llog_ctxt *ctxt,
-			   struct llog_rec_hdr *rec, struct lov_stripe_md *lsm,
-			   struct llog_cookie *logcookies, int numcookies);
 };
 
 /* In-memory descriptor for a log object or log catalog */
diff --git a/drivers/staging/lustre/lustre/include/lustre_mdc.h b/drivers/staging/lustre/lustre/include/lustre_mdc.h
index 468f36344a3..66765d4d201 100644
--- a/drivers/staging/lustre/lustre/include/lustre_mdc.h
+++ b/drivers/staging/lustre/lustre/include/lustre_mdc.h
@@ -140,17 +140,26 @@ static inline void mdc_put_rpc_lock(struct mdc_rpc_lock *lck,
 	mutex_unlock(&lck->rpcl_mutex);
 }
 
+/* Update the maximum observed easize and cookiesize.  The default easize
+ * and cookiesize is initialized to the minimum value but allowed to grow
+ * up to a single page in size if required to handle the common case.
+ */
 static inline void mdc_update_max_ea_from_body(struct obd_export *exp,
 					       struct mdt_body *body)
 {
 	if (body->valid & OBD_MD_FLMODEASIZE) {
-		if (exp->exp_obd->u.cli.cl_max_mds_easize < body->max_mdsize)
-			exp->exp_obd->u.cli.cl_max_mds_easize =
-						body->max_mdsize;
-		if (exp->exp_obd->u.cli.cl_max_mds_cookiesize <
-						body->max_cookiesize)
-			exp->exp_obd->u.cli.cl_max_mds_cookiesize =
-						body->max_cookiesize;
+		struct client_obd *cli = &exp->exp_obd->u.cli;
+
+		if (cli->cl_max_mds_easize < body->max_mdsize) {
+			cli->cl_max_mds_easize = body->max_mdsize;
+			cli->cl_default_mds_easize =
+			    min_t(__u32, body->max_mdsize, PAGE_CACHE_SIZE);
+		}
+		if (cli->cl_max_mds_cookiesize < body->max_cookiesize) {
+			cli->cl_max_mds_cookiesize = body->max_cookiesize;
+			cli->cl_default_mds_cookiesize =
+			    min_t(__u32, body->max_cookiesize, PAGE_CACHE_SIZE);
+		}
 	}
 }
 
diff --git a/drivers/staging/lustre/lustre/include/lustre_net.h b/drivers/staging/lustre/lustre/include/lustre_net.h
index 745adbb74cf..f6b7d10cb78 100644
--- a/drivers/staging/lustre/lustre/include/lustre_net.h
+++ b/drivers/staging/lustre/lustre/include/lustre_net.h
@@ -264,6 +264,8 @@
 #define LDLM_MAXREQSIZE   (5 * 1024)
 #define LDLM_MAXREPSIZE   (1024)
 
+#define MDS_MAXREQSIZE		(5 * 1024)	/* >= 4736 */
+
 #define OST_MAXREQSIZE		(5 * 1024)
 
 /* Macro to hide a typecast. */
@@ -717,7 +719,7 @@ struct ptlrpc_nrs_pol_ops {
 	 *			 \a nrq
 	 * \param[in,out] nrq	 The request
 	 *
-	 * \pre spin_is_locked(&svcpt->scp_req_lock)
+	 * \pre assert_spin_locked(&svcpt->scp_req_lock)
 	 *
 	 * \see ptlrpc_nrs_req_stop_nolock()
 	 */
diff --git a/drivers/staging/lustre/lustre/include/obd.h b/drivers/staging/lustre/lustre/include/obd.h
index 72cf3fe4b0c..d5c4613f182 100644
--- a/drivers/staging/lustre/lustre/include/obd.h
+++ b/drivers/staging/lustre/lustre/include/obd.h
@@ -132,6 +132,13 @@ static inline bool lsm_has_objects(struct lov_stripe_md *lsm)
 	return true;
 }
 
+static inline int lov_stripe_md_size(unsigned int stripe_count)
+{
+	struct lov_stripe_md lsm;
+
+	return sizeof(lsm) + stripe_count * sizeof(lsm.lsm_oinfo[0]);
+}
+
 struct obd_info;
 
 typedef int (*obd_enqueue_update_f)(void *cookie, int rc);
@@ -309,9 +316,10 @@ struct client_obd {
 	int		      cl_conn_count;
 	/* max_mds_easize is purely a performance thing so we don't have to
 	 * call obd_size_diskmd() all the time. */
-	int		      cl_default_mds_easize;
-	int		      cl_max_mds_easize;
-	int		      cl_max_mds_cookiesize;
+	int			 cl_default_mds_easize;
+	int			 cl_max_mds_easize;
+	int			 cl_default_mds_cookiesize;
+	int			 cl_max_mds_cookiesize;
 
 	enum lustre_sec_part     cl_sp_me;
 	enum lustre_sec_part     cl_sp_to;
@@ -398,7 +406,7 @@ struct client_obd {
 	struct mdc_rpc_lock     *cl_close_lock;
 
 	/* mgc datastruct */
-	struct semaphore	 cl_mgc_sem;
+	struct mutex		 cl_mgc_mutex;
 	struct local_oid_storage *cl_mgc_los;
 	struct dt_object	*cl_mgc_configs_dir;
 	atomic_t	     cl_mgc_refcount;
@@ -598,6 +606,7 @@ struct lmv_obd {
 	int			max_easize;
 	int			max_def_easize;
 	int			max_cookiesize;
+	int			max_def_cookiesize;
 	int			server_timeout;
 
 	int			tgts_size; /* size of tgts array */
@@ -989,7 +998,10 @@ enum obd_cleanup_stage {
 #define KEY_LOCK_TO_STRIPE      "lock_to_stripe"
 #define KEY_LOVDESC	     "lovdesc"
 #define KEY_LOV_IDX	     "lov_idx"
-#define KEY_MAX_EASIZE	  "max_easize"
+#define KEY_MAX_EASIZE		"max_easize"
+#define KEY_DEFAULT_EASIZE	"default_easize"
+#define KEY_MAX_COOKIESIZE	"max_cookiesize"
+#define KEY_DEFAULT_COOKIESIZE	"default_cookiesize"
 #define KEY_MDS_CONN	    "mds_conn"
 #define KEY_MGSSEC	      "mgssec"
 #define KEY_NEXT_ID	     "next_id"
@@ -1383,7 +1395,7 @@ struct md_ops {
 			  const char *, int, int, int,
 			  struct ptlrpc_request **);
 
-	int (*m_init_ea_size)(struct obd_export *, int, int, int);
+	int (*m_init_ea_size)(struct obd_export *, int, int, int, int);
 
 	int (*m_get_lustre_md)(struct obd_export *, struct ptlrpc_request *,
 			       struct obd_export *, struct obd_export *,
diff --git a/drivers/staging/lustre/lustre/include/obd_class.h b/drivers/staging/lustre/lustre/include/obd_class.h
index 9d1f266e55e..e265820c009 100644
--- a/drivers/staging/lustre/lustre/include/obd_class.h
+++ b/drivers/staging/lustre/lustre/include/obd_class.h
@@ -711,15 +711,6 @@ static inline int obd_size_diskmd(struct obd_export *exp,
 	return obd_packmd(exp, NULL, mem_src);
 }
 
-/* helper functions */
-static inline int obd_alloc_diskmd(struct obd_export *exp,
-				   struct lov_mds_md **disk_tgt)
-{
-	LASSERT(disk_tgt);
-	LASSERT(*disk_tgt == NULL);
-	return obd_packmd(exp, disk_tgt, NULL);
-}
-
 static inline int obd_free_diskmd(struct obd_export *exp,
 				  struct lov_mds_md **disk_tgt)
 {
@@ -2055,12 +2046,13 @@ static inline ldlm_mode_t md_lock_match(struct obd_export *exp, __u64 flags,
 }
 
 static inline int md_init_ea_size(struct obd_export *exp, int easize,
-				  int def_asize, int cookiesize)
+				  int def_asize, int cookiesize,
+				  int def_cookiesize)
 {
 	EXP_CHECK_MD_OP(exp, init_ea_size);
 	EXP_MD_COUNTER_INCREMENT(exp, init_ea_size);
 	return MDP(exp->exp_obd, init_ea_size)(exp, easize, def_asize,
-					       cookiesize);
+					       cookiesize, def_cookiesize);
 }
 
 static inline int md_get_remote_perm(struct obd_export *exp,
@@ -2133,7 +2125,7 @@ extern struct kmem_cache *obdo_cachep;
 
 #define OBDO_ALLOC(ptr)						       \
 do {									  \
-	OBD_SLAB_ALLOC_PTR_GFP((ptr), obdo_cachep, __GFP_IO);	     \
+	OBD_SLAB_ALLOC_PTR_GFP((ptr), obdo_cachep, GFP_NOFS);             \
 } while(0)
 
 #define OBDO_FREE(ptr)							\
@@ -2190,6 +2182,9 @@ void class_exit_uuidlist(void);
 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[];
+
 /* prng.c */
 #define ll_generate_random_uuid(uuid_out) cfs_get_random_bytes(uuid_out, sizeof(class_uuid_t))
 
diff --git a/drivers/staging/lustre/lustre/include/obd_lov.h b/drivers/staging/lustre/lustre/include/obd_lov.h
deleted file mode 100644
index 235718b366d..00000000000
--- a/drivers/staging/lustre/lustre/include/obd_lov.h
+++ /dev/null
@@ -1,116 +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.
- */
-
-#ifndef _OBD_LOV_H__
-#define _OBD_LOV_H__
-
-#define LOV_DEFAULT_STRIPE_SIZE (1 << LNET_MTU_BITS)
-
-static inline int lov_stripe_md_size(__u16 stripes)
-{
-	return sizeof(struct lov_stripe_md) + stripes*sizeof(struct lov_oinfo*);
-}
-
-struct lov_version_size {
-	__u32   lvs_magic;
-	size_t  lvs_lmm_size;
-	size_t  lvs_lod_size;
-};
-
-static inline __u32 lov_mds_md_stripecnt(int ea_size, __u32 lmm_magic)
-{
-	static const struct lov_version_size lmm_ver_size[] = {
-			{ .lvs_magic = LOV_MAGIC_V3,
-			  .lvs_lmm_size = sizeof(struct lov_mds_md_v3),
-			  .lvs_lod_size = sizeof(struct lov_ost_data_v1) },
-			{ .lvs_magic = LOV_MAGIC_V1,
-			  .lvs_lmm_size = sizeof(struct lov_mds_md_v1),
-			  .lvs_lod_size = sizeof(struct lov_ost_data_v1)} };
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(lmm_ver_size); i++) {
-		if (lmm_magic == lmm_ver_size[i].lvs_magic) {
-			if (ea_size <= lmm_ver_size[i].lvs_lmm_size)
-				return 0;
-			return (ea_size - lmm_ver_size[i].lvs_lmm_size) /
-				lmm_ver_size[i].lvs_lod_size;
-		}
-	}
-
-	/* Invalid LOV magic, so no stripes could fit */
-	return 0;
-}
-
-/* lov_do_div64(a, b) returns a % b, and a = a / b.
- * The 32-bit code is LOV-specific due to knowing about stripe limits in
- * order to reduce the divisor to a 32-bit number.  If the divisor is
- * already a 32-bit value the compiler handles this directly. */
-#if BITS_PER_LONG > 32
-# define lov_do_div64(n,base) ({					\
-	uint64_t __base = (base);					\
-	uint64_t __rem;							\
-	__rem = ((uint64_t)(n)) % __base;				\
-	(n) = ((uint64_t)(n)) / __base;					\
-	__rem;								\
-  })
-#else
-# define lov_do_div64(n,base) ({					\
-	uint64_t __rem;							\
-	if ((sizeof(base) > 4) && (((base) & 0xffffffff00000000ULL) != 0)) {  \
-		int __remainder;					      \
-		LASSERTF(!((base) & (LOV_MIN_STRIPE_SIZE - 1)), "64 bit lov " \
-			 "division %llu / %llu\n", (n), (uint64_t)(base));    \
-		__remainder = (n) & (LOV_MIN_STRIPE_SIZE - 1);		\
-		(n) >>= LOV_MIN_STRIPE_BITS;				\
-		__rem = do_div(n, (base) >> LOV_MIN_STRIPE_BITS);	\
-		__rem <<= LOV_MIN_STRIPE_BITS;				\
-		__rem += __remainder;					\
-	} else {							\
-		__rem = do_div(n, base);				\
-	}								\
-	__rem;								\
-  })
-#endif
-
-#define IOC_LOV_TYPE		   'g'
-#define IOC_LOV_MIN_NR		 50
-#define IOC_LOV_SET_OSC_ACTIVE	 _IOWR('g', 50, long)
-#define IOC_LOV_MAX_NR		 50
-
-#define QOS_DEFAULT_THRESHOLD	   10 /* MB */
-#define QOS_DEFAULT_MAXAGE	      5  /* Seconds */
-
-#endif
diff --git a/drivers/staging/lustre/lustre/include/obd_support.h b/drivers/staging/lustre/lustre/include/obd_support.h
index 5ec336968fc..cc5af509b26 100644
--- a/drivers/staging/lustre/lustre/include/obd_support.h
+++ b/drivers/staging/lustre/lustre/include/obd_support.h
@@ -641,8 +641,8 @@ do {									      \
 #define OBD_ALLOC_GFP(ptr, size, gfp_mask)				      \
 	__OBD_MALLOC_VERBOSE(ptr, NULL, 0, size, gfp_mask)
 
-#define OBD_ALLOC(ptr, size) OBD_ALLOC_GFP(ptr, size, __GFP_IO)
-#define OBD_ALLOC_WAIT(ptr, size) OBD_ALLOC_GFP(ptr, size, GFP_IOFS)
+#define OBD_ALLOC(ptr, size) OBD_ALLOC_GFP(ptr, size, GFP_NOFS)
+#define OBD_ALLOC_WAIT(ptr, size) OBD_ALLOC_GFP(ptr, size, GFP_KERNEL)
 #define OBD_ALLOC_PTR(ptr) OBD_ALLOC(ptr, sizeof(*(ptr)))
 #define OBD_ALLOC_PTR_WAIT(ptr) OBD_ALLOC_WAIT(ptr, sizeof(*(ptr)))
 
@@ -650,7 +650,7 @@ do {									      \
 	__OBD_MALLOC_VERBOSE(ptr, cptab, cpt, size, gfp_mask)
 
 #define OBD_CPT_ALLOC(ptr, cptab, cpt, size)				      \
-	OBD_CPT_ALLOC_GFP(ptr, cptab, cpt, size, __GFP_IO)
+	OBD_CPT_ALLOC_GFP(ptr, cptab, cpt, size, GFP_NOFS)
 
 #define OBD_CPT_ALLOC_PTR(ptr, cptab, cpt)				      \
 	OBD_CPT_ALLOC(ptr, cptab, cpt, sizeof(*(ptr)))
@@ -793,10 +793,10 @@ do {									  \
 } while(0)
 
 #define OBD_SLAB_ALLOC(ptr, slab, size)					      \
-	OBD_SLAB_ALLOC_GFP(ptr, slab, size, __GFP_IO)
+	OBD_SLAB_ALLOC_GFP(ptr, slab, size, GFP_NOFS)
 
 #define OBD_SLAB_CPT_ALLOC(ptr, slab, cptab, cpt, size)			      \
-	OBD_SLAB_CPT_ALLOC_GFP(ptr, slab, cptab, cpt, size, __GFP_IO)
+	OBD_SLAB_CPT_ALLOC_GFP(ptr, slab, cptab, cpt, size, GFP_NOFS)
 
 #define OBD_SLAB_ALLOC_PTR(ptr, slab)					      \
 	OBD_SLAB_ALLOC(ptr, slab, sizeof(*(ptr)))
diff --git a/drivers/staging/lustre/lustre/lclient/lcommon_cl.c b/drivers/staging/lustre/lustre/lclient/lcommon_cl.c
index 6907a16dbbd..dc24cfa5803 100644
--- a/drivers/staging/lustre/lustre/lclient/lcommon_cl.c
+++ b/drivers/staging/lustre/lustre/lclient/lcommon_cl.c
@@ -63,7 +63,7 @@
 
 #include "../llite/llite_internal.h"
 
-const struct cl_req_operations ccc_req_ops;
+static const struct cl_req_operations ccc_req_ops;
 
 /*
  * ccc_ prefix stands for "Common Client Code".
@@ -112,12 +112,11 @@ static struct lu_kmem_descr ccc_caches[] = {
  *
  */
 
-void *ccc_key_init(const struct lu_context *ctx,
-			  struct lu_context_key *key)
+void *ccc_key_init(const struct lu_context *ctx, struct lu_context_key *key)
 {
 	struct ccc_thread_info *info;
 
-	OBD_SLAB_ALLOC_PTR_GFP(info, ccc_thread_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(info, ccc_thread_kmem, GFP_NOFS);
 	if (info == NULL)
 		info = ERR_PTR(-ENOMEM);
 	return info;
@@ -135,7 +134,7 @@ void *ccc_session_key_init(const struct lu_context *ctx,
 {
 	struct ccc_session *session;
 
-	OBD_SLAB_ALLOC_PTR_GFP(session, ccc_session_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(session, ccc_session_kmem, GFP_NOFS);
 	if (session == NULL)
 		session = ERR_PTR(-ENOMEM);
 	return session;
@@ -251,7 +250,7 @@ int ccc_req_init(const struct lu_env *env, struct cl_device *dev,
 	struct ccc_req *vrq;
 	int result;
 
-	OBD_SLAB_ALLOC_PTR_GFP(vrq, ccc_req_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(vrq, ccc_req_kmem, GFP_NOFS);
 	if (vrq != NULL) {
 		cl_req_slice_add(req, &vrq->crq_cl, dev, &ccc_req_ops);
 		result = 0;
@@ -327,7 +326,7 @@ struct lu_object *ccc_object_alloc(const struct lu_env *env,
 	struct ccc_object *vob;
 	struct lu_object  *obj;
 
-	OBD_SLAB_ALLOC_PTR_GFP(vob, ccc_object_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(vob, ccc_object_kmem, GFP_NOFS);
 	if (vob != NULL) {
 		struct cl_object_header *hdr;
 
@@ -396,7 +395,7 @@ int ccc_lock_init(const struct lu_env *env,
 
 	CLOBINVRNT(env, obj, ccc_object_invariant(obj));
 
-	OBD_SLAB_ALLOC_PTR_GFP(clk, ccc_lock_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(clk, ccc_lock_kmem, GFP_NOFS);
 	if (clk != NULL) {
 		cl_lock_slice_add(lock, &clk->clk_cl, obj, lkops);
 		result = 0;
@@ -963,7 +962,7 @@ void ccc_req_attr_set(const struct lu_env *env,
 	       JOBSTATS_JOBID_SIZE);
 }
 
-const struct cl_req_operations ccc_req_ops = {
+static const struct cl_req_operations ccc_req_ops = {
 	.cro_attr_set   = ccc_req_attr_set,
 	.cro_completion = ccc_req_completion
 };
diff --git a/drivers/staging/lustre/lustre/lclient/lcommon_misc.c b/drivers/staging/lustre/lustre/lclient/lcommon_misc.c
index e04c2d37c24..21de1cd2afb 100644
--- a/drivers/staging/lustre/lustre/lclient/lcommon_misc.c
+++ b/drivers/staging/lustre/lustre/lclient/lcommon_misc.c
@@ -56,7 +56,7 @@ int cl_init_ea_size(struct obd_export *md_exp, struct obd_export *dt_exp)
 	__u32 valsize = sizeof(struct lov_desc);
 	int rc, easize, def_easize, cookiesize;
 	struct lov_desc desc;
-	__u16 stripes;
+	__u16 stripes, def_stripes;
 
 	rc = obd_get_info(NULL, dt_exp, sizeof(KEY_LOVDESC), KEY_LOVDESC,
 			  &valsize, &desc, NULL);
@@ -67,15 +67,20 @@ int cl_init_ea_size(struct obd_export *md_exp, struct obd_export *dt_exp)
 	lsm.lsm_stripe_count = stripes;
 	easize = obd_size_diskmd(dt_exp, &lsm);
 
-	lsm.lsm_stripe_count = desc.ld_default_stripe_count;
+	def_stripes = min_t(__u32, desc.ld_default_stripe_count,
+			    LOV_MAX_STRIPE_COUNT);
+	lsm.lsm_stripe_count = def_stripes;
 	def_easize = obd_size_diskmd(dt_exp, &lsm);
 
 	cookiesize = stripes * sizeof(struct llog_cookie);
 
-	CDEBUG(D_HA, "updating max_mdsize/max_cookiesize: %d/%d\n",
-	       easize, cookiesize);
+	/* default cookiesize is 0 because from 2.4 server doesn't send
+	 * llog cookies to client. */
+	CDEBUG(D_HA,
+	       "updating def/max_easize: %d/%d def/max_cookiesize: 0/%d\n",
+	       def_easize, easize, cookiesize);
 
-	rc = md_init_ea_size(md_exp, easize, def_easize, cookiesize);
+	rc = md_init_ea_size(md_exp, easize, def_easize, cookiesize, 0);
 	return rc;
 }
 
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_extent.c b/drivers/staging/lustre/lustre/ldlm/ldlm_extent.c
index ac5d66aa7f0..bde3a82ce0d 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_extent.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_extent.c
@@ -104,7 +104,7 @@ struct ldlm_interval *ldlm_interval_alloc(struct ldlm_lock *lock)
 	struct ldlm_interval *node;
 
 	LASSERT(lock->l_resource->lr_type == LDLM_EXTENT);
-	OBD_SLAB_ALLOC_PTR_GFP(node, ldlm_interval_slab, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(node, ldlm_interval_slab, GFP_NOFS);
 	if (node == NULL)
 		return NULL;
 
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h b/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
index 8cd79633dea..f997566ef11 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
@@ -247,7 +247,7 @@ typedef enum ldlm_policy_res ldlm_policy_res_t;
 	struct __##var##__dummy_read {;} /* semicolon catcher */
 
 #define LDLM_POOL_PROC_WRITER(var, type)				    \
-	int lprocfs_wr_##var(struct file *file, const char *buffer,	    \
+	static int lprocfs_wr_##var(struct file *file, const char *buffer,  \
 			     unsigned long count, void *data)		    \
 	{								    \
 		struct ldlm_pool *pl = data;				    \
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c b/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c
index 1a8c0d7005c..8bb59155968 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c
@@ -325,7 +325,7 @@ int client_obd_setup(struct obd_device *obddev, struct lustre_cfg *lcfg)
 	}
 
 	init_rwsem(&cli->cl_sem);
-	sema_init(&cli->cl_mgc_sem, 1);
+	mutex_init(&cli->cl_mgc_mutex);
 	cli->cl_conn_count = 0;
 	memcpy(server_uuid.uuid, lustre_cfg_buf(lcfg, 2),
 	       min_t(unsigned int, LUSTRE_CFG_BUFLEN(lcfg, 2),
@@ -514,7 +514,7 @@ int client_connect_import(const struct lu_env *env,
 		LASSERT (imp->imp_state == LUSTRE_IMP_DISCON);
 		GOTO(out_ldlm, rc);
 	}
-	LASSERT((*exp)->exp_connection);
+	LASSERT(*exp != NULL && (*exp)->exp_connection);
 
 	if (data) {
 		LASSERTF((ocd->ocd_connect_flags & data->ocd_connect_flags) ==
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c b/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
index 0548aca45e8..1b3f5c1eb80 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
@@ -232,8 +232,6 @@ int ldlm_lock_remove_from_lru_nolock(struct ldlm_lock *lock)
 
 		LASSERT(lock->l_resource->lr_type != LDLM_FLOCK);
 		list_del_init(&lock->l_lru);
-		if (lock->l_flags & LDLM_FL_SKIPPED)
-			lock->l_flags &= ~LDLM_FL_SKIPPED;
 		LASSERT(ns->ns_nr_unused > 0);
 		ns->ns_nr_unused--;
 		rc = 1;
@@ -271,6 +269,8 @@ void ldlm_lock_add_to_lru_nolock(struct ldlm_lock *lock)
 	LASSERT(list_empty(&lock->l_lru));
 	LASSERT(lock->l_resource->lr_type != LDLM_FLOCK);
 	list_add_tail(&lock->l_lru, &ns->ns_unused_list);
+	if (lock->l_flags & LDLM_FL_SKIPPED)
+		lock->l_flags &= ~LDLM_FL_SKIPPED;
 	LASSERT(ns->ns_nr_unused >= 0);
 	ns->ns_nr_unused++;
 }
@@ -437,7 +437,7 @@ static struct ldlm_lock *ldlm_lock_new(struct ldlm_resource *resource)
 	if (resource == NULL)
 		LBUG();
 
-	OBD_SLAB_ALLOC_PTR_GFP(lock, ldlm_lock_slab, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(lock, ldlm_lock_slab, GFP_NOFS);
 	if (lock == NULL)
 		return NULL;
 
@@ -1624,7 +1624,7 @@ ldlm_error_t ldlm_lock_enqueue(struct ldlm_namespace *ns,
 	 * have to allocate the interval node early otherwise we can't regrant
 	 * this lock in the future. - jay */
 	if (!local && (*flags & LDLM_FL_REPLAY) && res->lr_type == LDLM_EXTENT)
-		OBD_SLAB_ALLOC_PTR_GFP(node, ldlm_interval_slab, __GFP_IO);
+		OBD_SLAB_ALLOC_PTR_GFP(node, ldlm_interval_slab, GFP_NOFS);
 
 	lock_res_and_lock(lock);
 	if (local && lock->l_req_mode == lock->l_granted_mode) {
@@ -2146,7 +2146,7 @@ struct ldlm_resource *ldlm_lock_convert(struct ldlm_lock *lock, int new_mode,
 
 	/* I can't check the type of lock here because the bitlock of lock
 	 * is not held here, so do the allocation blindly. -jay */
-	OBD_SLAB_ALLOC_PTR_GFP(node, ldlm_interval_slab, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(node, ldlm_interval_slab, GFP_NOFS);
 	if (node == NULL)
 		/* Actually, this causes EDEADLOCK to be returned */
 		return NULL;
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c b/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
index 6758646f575..2cc698179b6 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
@@ -378,7 +378,7 @@ static int ldlm_srv_pool_recalc(struct ldlm_pool *pl)
  * locks smaller in next 10h.
  */
 static int ldlm_srv_pool_shrink(struct ldlm_pool *pl,
-				int nr, unsigned int gfp_mask)
+				int nr, gfp_t gfp_mask)
 {
 	__u32 limit;
 
@@ -518,7 +518,7 @@ static int ldlm_cli_pool_recalc(struct ldlm_pool *pl)
  * passed \a pl according to \a nr and \a gfp_mask.
  */
 static int ldlm_cli_pool_shrink(struct ldlm_pool *pl,
-				int nr, unsigned int gfp_mask)
+				int nr, gfp_t gfp_mask)
 {
 	struct ldlm_namespace *ns;
 	int unused;
@@ -546,13 +546,13 @@ static int ldlm_cli_pool_shrink(struct ldlm_pool *pl,
 		return ldlm_cancel_lru(ns, nr, LCF_ASYNC, LDLM_CANCEL_SHRINK);
 }
 
-struct ldlm_pool_ops ldlm_srv_pool_ops = {
+static const struct ldlm_pool_ops ldlm_srv_pool_ops = {
 	.po_recalc = ldlm_srv_pool_recalc,
 	.po_shrink = ldlm_srv_pool_shrink,
 	.po_setup  = ldlm_srv_pool_setup
 };
 
-struct ldlm_pool_ops ldlm_cli_pool_ops = {
+static const struct ldlm_pool_ops ldlm_cli_pool_ops = {
 	.po_recalc = ldlm_cli_pool_recalc,
 	.po_shrink = ldlm_cli_pool_shrink
 };
@@ -603,7 +603,7 @@ int ldlm_pool_recalc(struct ldlm_pool *pl)
  * freeable locks. Otherwise, return the number of canceled locks.
  */
 int ldlm_pool_shrink(struct ldlm_pool *pl, int nr,
-		     unsigned int gfp_mask)
+		     gfp_t gfp_mask)
 {
 	int cancel = 0;
 
@@ -718,7 +718,7 @@ LPROC_SEQ_FOPS_RO(lprocfs_grant_speed);
 		snprintf(var_name, MAX_STRING_SIZE, #name);	\
 		pool_vars[0].data = var;			\
 		pool_vars[0].fops = ops;			\
-		lprocfs_add_vars(pl->pl_proc_dir, pool_vars, 0);\
+		lprocfs_add_vars(pl->pl_proc_dir, pool_vars, NULL);\
 	} while (0)
 
 static int ldlm_pool_proc_init(struct ldlm_pool *pl)
@@ -1029,7 +1029,7 @@ static struct completion ldlm_pools_comp;
  * count locks from all namespaces (if possible). Returns number of
  * cached locks.
  */
-static unsigned long ldlm_pools_count(ldlm_side_t client, unsigned int gfp_mask)
+static unsigned long ldlm_pools_count(ldlm_side_t client, gfp_t gfp_mask)
 {
 	int total = 0, nr_ns;
 	struct ldlm_namespace *ns;
@@ -1082,7 +1082,7 @@ static unsigned long ldlm_pools_count(ldlm_side_t client, unsigned int gfp_mask)
 	return total;
 }
 
-static unsigned long ldlm_pools_scan(ldlm_side_t client, int nr, unsigned int gfp_mask)
+static unsigned long ldlm_pools_scan(ldlm_side_t client, int nr, gfp_t gfp_mask)
 {
 	unsigned long freed = 0;
 	int tmp, nr_ns;
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c b/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
index 2824d4a9a85..c55d72f79b2 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
@@ -89,7 +89,7 @@ int ldlm_proc_setup(void)
 {
 	int rc;
 	struct lprocfs_vars list[] = {
-		{ "dump_namespaces", &ldlm_dump_ns_fops, 0, 0222 },
+		{ "dump_namespaces", &ldlm_dump_ns_fops, NULL, 0222 },
 		{ "dump_granted_max", &ldlm_rw_uint_fops,
 		  &ldlm_dump_granted_max },
 		{ "cancel_unused_locks_before_replay", &ldlm_rw_uint_fops,
@@ -322,7 +322,7 @@ void ldlm_namespace_proc_unregister(struct ldlm_namespace *ns)
 		snprintf(lock_name, MAX_STRING_SIZE, name);	\
 		lock_vars[0].data = var;			\
 		lock_vars[0].fops = ops;			\
-		lprocfs_add_vars(ns_pde, lock_vars, 0);		\
+		lprocfs_add_vars(ns_pde, lock_vars, NULL);	\
 	} while (0)
 
 int ldlm_namespace_proc_register(struct ldlm_namespace *ns)
@@ -1014,7 +1014,7 @@ static struct ldlm_resource *ldlm_resource_new(void)
 	struct ldlm_resource *res;
 	int idx;
 
-	OBD_SLAB_ALLOC_PTR_GFP(res, ldlm_resource_slab, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(res, ldlm_resource_slab, GFP_NOFS);
 	if (res == NULL)
 		return NULL;
 
diff --git a/drivers/staging/lustre/lustre/libcfs/libcfs_cpu.c b/drivers/staging/lustre/lustre/libcfs/libcfs_cpu.c
index 1fb37008cda..a1a7bf44ccc 100644
--- a/drivers/staging/lustre/lustre/libcfs/libcfs_cpu.c
+++ b/drivers/staging/lustre/lustre/libcfs/libcfs_cpu.c
@@ -74,6 +74,22 @@ cfs_cpt_table_free(struct cfs_cpt_table *cptab)
 }
 EXPORT_SYMBOL(cfs_cpt_table_free);
 
+#ifdef CONFIG_SMP
+int
+cfs_cpt_table_print(struct cfs_cpt_table *cptab, char *buf, int len)
+{
+	int	rc = 0;
+
+	rc = snprintf(buf, len, "%d\t: %d\n", 0, 0);
+	len -= rc;
+	if (len <= 0)
+		return -EFBIG;
+
+	return rc;
+}
+EXPORT_SYMBOL(cfs_cpt_table_print);
+#endif /* CONFIG_SMP */
+
 int
 cfs_cpt_number(struct cfs_cpt_table *cptab)
 {
@@ -161,6 +177,13 @@ cfs_cpt_spread_node(struct cfs_cpt_table *cptab, int cpt)
 EXPORT_SYMBOL(cfs_cpt_spread_node);
 
 int
+cfs_cpu_ht_nsiblings(int cpu)
+{
+	return 1;
+}
+EXPORT_SYMBOL(cfs_cpu_ht_nsiblings);
+
+int
 cfs_cpt_current(struct cfs_cpt_table *cptab, int remap)
 {
 	return 0;
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c
index 77b1ef64ecc..fc21210d77e 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c
@@ -71,7 +71,7 @@ struct cfs_cpt_data {
 	/* reserved for hotplug */
 	unsigned long		cpt_version;
 	/* mutex to protect cpt_cpumask */
-	struct semaphore	cpt_mutex;
+	struct mutex		cpt_mutex;
 	/* scratch buffer for set/unset_node */
 	cpumask_t		*cpt_cpumask;
 };
@@ -420,14 +420,14 @@ cfs_cpt_set_node(struct cfs_cpt_table *cptab, int cpt, int node)
 		return 0;
 	}
 
-	down(&cpt_data.cpt_mutex);
+	mutex_lock(&cpt_data.cpt_mutex);
 
 	mask = cpt_data.cpt_cpumask;
 	cfs_node_to_cpumask(node, mask);
 
 	rc = cfs_cpt_set_cpumask(cptab, cpt, mask);
 
-	up(&cpt_data.cpt_mutex);
+	mutex_unlock(&cpt_data.cpt_mutex);
 
 	return rc;
 }
@@ -444,14 +444,14 @@ cfs_cpt_unset_node(struct cfs_cpt_table *cptab, int cpt, int node)
 		return;
 	}
 
-	down(&cpt_data.cpt_mutex);
+	mutex_lock(&cpt_data.cpt_mutex);
 
 	mask = cpt_data.cpt_cpumask;
 	cfs_node_to_cpumask(node, mask);
 
 	cfs_cpt_unset_cpumask(cptab, cpt, mask);
 
-	up(&cpt_data.cpt_mutex);
+	mutex_unlock(&cpt_data.cpt_mutex);
 }
 EXPORT_SYMBOL(cfs_cpt_unset_node);
 
@@ -881,7 +881,7 @@ cfs_cpt_table_create_pattern(char *pattern)
 			break;
 		}
 
-		if (sscanf(str, "%u%n", &cpt, &n) < 1) {
+		if (sscanf(str, "%d%n", &cpt, &n) < 1) {
 			CERROR("Invalid cpu pattern %s\n", str);
 			goto failed;
 		}
@@ -969,11 +969,11 @@ cfs_cpu_notify(struct notifier_block *self, unsigned long action, void *hcpu)
 			break;
 		}
 
-		down(&cpt_data.cpt_mutex);
+		mutex_lock(&cpt_data.cpt_mutex);
 		/* if all HTs in a core are offline, it may break affinity */
 		cfs_cpu_ht_siblings(cpu, cpt_data.cpt_cpumask);
 		warn = any_online_cpu(*cpt_data.cpt_cpumask) >= nr_cpu_ids;
-		up(&cpt_data.cpt_mutex);
+		mutex_unlock(&cpt_data.cpt_mutex);
 		CDEBUG(warn ? D_WARNING : D_INFO,
 		       "Lustre: can't support CPU plug-out well now, "
 		       "performance and stability could be impacted "
@@ -1017,7 +1017,7 @@ cfs_cpu_init(void)
 	}
 
 	spin_lock_init(&cpt_data.cpt_lock);
-	sema_init(&cpt_data.cpt_mutex, 1);
+	mutex_init(&cpt_data.cpt_mutex);
 
 #ifdef CONFIG_HOTPLUG_CPU
 	register_hotcpu_notifier(&cfs_cpu_notifier);
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-curproc.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-curproc.c
index e74c3e28a97..bd301ce0225 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-curproc.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-curproc.c
@@ -100,158 +100,6 @@ cfs_cap_t cfs_curproc_cap_pack(void)
 	return cap;
 }
 
-static int cfs_access_process_vm(struct task_struct *tsk, unsigned long addr,
-				 void *buf, int len, int write)
-{
-	/* Just copied from kernel for the kernels which doesn't
-	 * have access_process_vm() exported */
-	struct mm_struct *mm;
-	struct vm_area_struct *vma;
-	struct page *page;
-	void *old_buf = buf;
-
-	mm = get_task_mm(tsk);
-	if (!mm)
-		return 0;
-
-	down_read(&mm->mmap_sem);
-	/* ignore errors, just check how much was successfully transferred */
-	while (len) {
-		int bytes, rc, offset;
-		void *maddr;
-
-		rc = get_user_pages(tsk, mm, addr, 1,
-				     write, 1, &page, &vma);
-		if (rc <= 0)
-			break;
-
-		bytes = len;
-		offset = addr & (PAGE_SIZE-1);
-		if (bytes > PAGE_SIZE-offset)
-			bytes = PAGE_SIZE-offset;
-
-		maddr = kmap(page);
-		if (write) {
-			copy_to_user_page(vma, page, addr,
-					  maddr + offset, buf, bytes);
-			set_page_dirty_lock(page);
-		} else {
-			copy_from_user_page(vma, page, addr,
-					    buf, maddr + offset, bytes);
-		}
-		kunmap(page);
-		page_cache_release(page);
-		len -= bytes;
-		buf += bytes;
-		addr += bytes;
-	}
-	up_read(&mm->mmap_sem);
-	mmput(mm);
-
-	return buf - old_buf;
-}
-
-/* Read the environment variable of current process specified by @key. */
-int cfs_get_environ(const char *key, char *value, int *val_len)
-{
-	struct mm_struct *mm;
-	char *buffer, *tmp_buf = NULL;
-	int buf_len = PAGE_CACHE_SIZE;
-	int key_len = strlen(key);
-	unsigned long addr;
-	int rc;
-
-	buffer = kmalloc(buf_len, GFP_USER);
-	if (!buffer)
-		return -ENOMEM;
-
-	mm = get_task_mm(current);
-	if (!mm) {
-		kfree(buffer);
-		return -EINVAL;
-	}
-
-	/* Avoid deadlocks on mmap_sem if called from sys_mmap_pgoff(),
-	 * which is already holding mmap_sem for writes.  If some other
-	 * thread gets the write lock in the meantime, this thread will
-	 * block, but at least it won't deadlock on itself.  LU-1735 */
-	if (down_read_trylock(&mm->mmap_sem) == 0) {
-		kfree(buffer);
-		return -EDEADLK;
-	}
-	up_read(&mm->mmap_sem);
-
-	addr = mm->env_start;
-	while (addr < mm->env_end) {
-		int this_len, retval, scan_len;
-		char *env_start, *env_end;
-
-		memset(buffer, 0, buf_len);
-
-		this_len = min_t(int, mm->env_end - addr, buf_len);
-		retval = cfs_access_process_vm(current, addr, buffer,
-					       this_len, 0);
-		if (retval != this_len)
-			break;
-
-		addr += retval;
-
-		/* Parse the buffer to find out the specified key/value pair.
-		 * The "key=value" entries are separated by '\0'. */
-		env_start = buffer;
-		scan_len = this_len;
-		while (scan_len) {
-			char *entry;
-			int entry_len;
-
-			env_end = memscan(env_start, '\0', scan_len);
-			LASSERT(env_end >= env_start &&
-				env_end <= env_start + scan_len);
-
-			/* The last entry of this buffer cross the buffer
-			 * boundary, reread it in next cycle. */
-			if (unlikely(env_end - env_start == scan_len)) {
-				/* This entry is too large to fit in buffer */
-				if (unlikely(scan_len == this_len)) {
-					CERROR("Too long env variable.\n");
-					GOTO(out, rc = -EINVAL);
-				}
-				addr -= scan_len;
-				break;
-			}
-
-			entry = env_start;
-			entry_len = env_end - env_start;
-
-			/* Key length + length of '=' */
-			if (entry_len > key_len + 1 &&
-			    !memcmp(entry, key, key_len)) {
-				entry += key_len + 1;
-				entry_len -= key_len + 1;
-				/* The 'value' buffer passed in is too small.*/
-				if (entry_len >= *val_len)
-					GOTO(out, rc = -EOVERFLOW);
-
-				memcpy(value, entry, entry_len);
-				*val_len = entry_len;
-				GOTO(out, rc = 0);
-			}
-
-			scan_len -= (env_end - env_start + 1);
-			env_start = env_end + 1;
-		}
-	}
-	GOTO(out, rc = -ENOENT);
-
-out:
-	mmput(mm);
-	kfree((void *)buffer);
-	if (tmp_buf)
-		kfree((void *)tmp_buf);
-	return rc;
-}
-EXPORT_SYMBOL(cfs_get_environ);
-
 EXPORT_SYMBOL(cfs_cap_raise);
 EXPORT_SYMBOL(cfs_cap_lower);
 EXPORT_SYMBOL(cfs_cap_raised);
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-module.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-module.c
index e6eae0666f0..581b4728c6c 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-module.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-module.c
@@ -44,14 +44,13 @@ int libcfs_ioctl_getdata(char *buf, char *end, void *arg)
 {
 	struct libcfs_ioctl_hdr   *hdr;
 	struct libcfs_ioctl_data  *data;
-	int err;
+	int orig_len;
 
 	hdr = (struct libcfs_ioctl_hdr *)buf;
 	data = (struct libcfs_ioctl_data *)buf;
 
-	err = copy_from_user(buf, (void *)arg, sizeof(*hdr));
-	if (err)
-		return err;
+	if (copy_from_user(buf, (void *)arg, sizeof(*hdr)))
+		return -EFAULT;
 
 	if (hdr->ioc_version != LIBCFS_IOCTL_VERSION) {
 		CERROR("PORTALS: version mismatch kernel vs application\n");
@@ -69,9 +68,11 @@ int libcfs_ioctl_getdata(char *buf, char *end, void *arg)
 		return -EINVAL;
 	}
 
-	err = copy_from_user(buf, (void *)arg, hdr->ioc_len);
-	if (err)
-		return err;
+	orig_len = hdr->ioc_len;
+	if (copy_from_user(buf, (void *)arg, hdr->ioc_len))
+		return -EFAULT;
+	if (orig_len != data->ioc_len)
+		return -EINVAL;
 
 	if (libcfs_ioctl_is_invalid(data)) {
 		CERROR("PORTALS: ioctl not correctly formatted\n");
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-tcpip.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-tcpip.c
index 7539fe16d76..ac3a444a918 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-tcpip.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-tcpip.c
@@ -46,31 +46,16 @@
 int
 libcfs_sock_ioctl(int cmd, unsigned long arg)
 {
-	mm_segment_t    oldmm = get_fs();
 	struct socket  *sock;
 	int	     rc;
-	struct file    *sock_filp;
 
 	rc = sock_create (PF_INET, SOCK_STREAM, 0, &sock);
 	if (rc != 0) {
 		CERROR ("Can't create socket: %d\n", rc);
 		return rc;
 	}
-
-	sock_filp = sock_alloc_file(sock, 0, NULL);
-	if (IS_ERR(sock_filp)) {
-		sock_release(sock);
-		rc = PTR_ERR(sock_filp);
-		goto out;
-	}
-
-	set_fs(KERNEL_DS);
-	if (sock_filp->f_op->unlocked_ioctl)
-		rc = sock_filp->f_op->unlocked_ioctl(sock_filp, cmd, arg);
-	set_fs(oldmm);
-
-	fput(sock_filp);
-out:
+	rc = kernel_sock_ioctl(sock, cmd, arg);
+	sock_release(sock);
 	return rc;
 }
 
@@ -255,7 +240,6 @@ int
 libcfs_sock_write (struct socket *sock, void *buffer, int nob, int timeout)
 {
 	int	    rc;
-	mm_segment_t   oldmm = get_fs();
 	long	   ticks = timeout * HZ;
 	unsigned long  then;
 	struct timeval tv;
@@ -279,10 +263,8 @@ libcfs_sock_write (struct socket *sock, void *buffer, int nob, int timeout)
 				.tv_sec = ticks / HZ,
 				.tv_usec = ((ticks % HZ) * 1000000) / HZ
 			};
-			set_fs(KERNEL_DS);
-			rc = sock_setsockopt(sock, SOL_SOCKET, SO_SNDTIMEO,
+			rc = kernel_setsockopt(sock, SOL_SOCKET, SO_SNDTIMEO,
 					     (char *)&tv, sizeof(tv));
-			set_fs(oldmm);
 			if (rc != 0) {
 				CERROR("Can't set socket send timeout "
 				       "%ld.%06d: %d\n",
@@ -321,7 +303,6 @@ int
 libcfs_sock_read (struct socket *sock, void *buffer, int nob, int timeout)
 {
 	int	    rc;
-	mm_segment_t   oldmm = get_fs();
 	long	   ticks = timeout * HZ;
 	unsigned long  then;
 	struct timeval tv;
@@ -343,10 +324,8 @@ libcfs_sock_read (struct socket *sock, void *buffer, int nob, int timeout)
 			.tv_sec = ticks / HZ,
 			.tv_usec = ((ticks % HZ) * 1000000) / HZ
 		};
-		set_fs(KERNEL_DS);
-		rc = sock_setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO,
+		rc = kernel_setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO,
 				     (char *)&tv, sizeof(tv));
-		set_fs(oldmm);
 		if (rc != 0) {
 			CERROR("Can't set socket recv timeout %ld.%06d: %d\n",
 			       (long)tv.tv_sec, (int)tv.tv_usec, rc);
@@ -384,7 +363,6 @@ libcfs_sock_create (struct socket **sockp, int *fatal,
 	struct socket      *sock;
 	int		 rc;
 	int		 option;
-	mm_segment_t	oldmm = get_fs();
 
 	/* All errors are fatal except bind failure if the port is in use */
 	*fatal = 1;
@@ -396,11 +374,9 @@ libcfs_sock_create (struct socket **sockp, int *fatal,
 		return (rc);
 	}
 
-	set_fs (KERNEL_DS);
 	option = 1;
-	rc = sock_setsockopt(sock, SOL_SOCKET, SO_REUSEADDR,
+	rc = kernel_setsockopt(sock, SOL_SOCKET, SO_REUSEADDR,
 			     (char *)&option, sizeof (option));
-	set_fs (oldmm);
 	if (rc != 0) {
 		CERROR("Can't set SO_REUSEADDR for socket: %d\n", rc);
 		goto failed;
@@ -437,16 +413,13 @@ libcfs_sock_create (struct socket **sockp, int *fatal,
 int
 libcfs_sock_setbuf (struct socket *sock, int txbufsize, int rxbufsize)
 {
-	mm_segment_t	oldmm = get_fs();
 	int		 option;
 	int		 rc;
 
 	if (txbufsize != 0) {
 		option = txbufsize;
-		set_fs (KERNEL_DS);
-		rc = sock_setsockopt(sock, SOL_SOCKET, SO_SNDBUF,
+		rc = kernel_setsockopt(sock, SOL_SOCKET, SO_SNDBUF,
 				     (char *)&option, sizeof (option));
-		set_fs (oldmm);
 		if (rc != 0) {
 			CERROR ("Can't set send buffer %d: %d\n",
 				option, rc);
@@ -456,10 +429,8 @@ libcfs_sock_setbuf (struct socket *sock, int txbufsize, int rxbufsize)
 
 	if (rxbufsize != 0) {
 		option = rxbufsize;
-		set_fs (KERNEL_DS);
-		rc = sock_setsockopt (sock, SOL_SOCKET, SO_RCVBUF,
+		rc = kernel_setsockopt(sock, SOL_SOCKET, SO_RCVBUF,
 				      (char *)&option, sizeof (option));
-		set_fs (oldmm);
 		if (rc != 0) {
 			CERROR ("Can't set receive buffer %d: %d\n",
 				option, rc);
diff --git a/drivers/staging/lustre/lustre/libcfs/module.c b/drivers/staging/lustre/lustre/libcfs/module.c
index 24ae26d5def..b16ee08fe74 100644
--- a/drivers/staging/lustre/lustre/libcfs/module.c
+++ b/drivers/staging/lustre/lustre/libcfs/module.c
@@ -87,7 +87,8 @@ kportal_memhog_free (struct libcfs_device_userstate *ldu)
 }
 
 int
-kportal_memhog_alloc (struct libcfs_device_userstate *ldu, int npages, int flags)
+kportal_memhog_alloc(struct libcfs_device_userstate *ldu, int npages,
+		     gfp_t flags)
 {
 	struct page **level0p;
 	struct page **level1p;
diff --git a/drivers/staging/lustre/lustre/libcfs/nidstrings.c b/drivers/staging/lustre/lustre/libcfs/nidstrings.c
index cfb274fc9cb..87705ae4480 100644
--- a/drivers/staging/lustre/lustre/libcfs/nidstrings.c
+++ b/drivers/staging/lustre/lustre/libcfs/nidstrings.c
@@ -227,11 +227,11 @@ libcfs_ip_addr2str(__u32 addr, char *str)
 int
 libcfs_ip_str2addr(const char *str, int nob, __u32 *addr)
 {
-	int   a;
-	int   b;
-	int   c;
-	int   d;
-	int   n = nob;			  /* XscanfX */
+	unsigned int	a;
+	unsigned int	b;
+	unsigned int	c;
+	unsigned int	d;
+	int		n = nob; /* XscanfX */
 
 	/* numeric IP? */
 	if (sscanf(str, "%u.%u.%u.%u%n", &a, &b, &c, &d, &n) >= 4 &&
@@ -419,7 +419,7 @@ libcfs_str2net_internal(const char *str, __u32 *net)
 {
 	struct netstrfns *uninitialized_var(nf);
 	int	       nob;
-	int	       netnum;
+	unsigned int   netnum;
 	int	       i;
 
 	for (i = 0; i < libcfs_nnetstrfns; i++) {
diff --git a/drivers/staging/lustre/lustre/libcfs/tracefile.c b/drivers/staging/lustre/lustre/libcfs/tracefile.c
index c8599eefeb7..07845e84424 100644
--- a/drivers/staging/lustre/lustre/libcfs/tracefile.c
+++ b/drivers/staging/lustre/lustre/libcfs/tracefile.c
@@ -66,7 +66,7 @@ cfs_tage_from_list(struct list_head *list)
 	return list_entry(list, struct cfs_trace_page, linkage);
 }
 
-static struct cfs_trace_page *cfs_tage_alloc(int gfp)
+static struct cfs_trace_page *cfs_tage_alloc(gfp_t gfp)
 {
 	struct page	    *page;
 	struct cfs_trace_page *tage;
@@ -114,7 +114,7 @@ static void cfs_tage_to_tail(struct cfs_trace_page *tage,
 	list_move_tail(&tage->linkage, queue);
 }
 
-int cfs_trace_refill_stock(struct cfs_trace_cpu_data *tcd, int gfp,
+int cfs_trace_refill_stock(struct cfs_trace_cpu_data *tcd, gfp_t gfp,
 			   struct list_head *stock)
 {
 	int i;
@@ -416,13 +416,13 @@ console:
 			cdls->cdls_delay /= libcfs_console_backoff * 4;
 		} else {
 			cdls->cdls_delay *= libcfs_console_backoff;
-
-			if (cdls->cdls_delay < libcfs_console_min_delay)
-				cdls->cdls_delay = libcfs_console_min_delay;
-			else if (cdls->cdls_delay > libcfs_console_max_delay)
-				cdls->cdls_delay = libcfs_console_max_delay;
 		}
 
+		if (cdls->cdls_delay < libcfs_console_min_delay)
+			cdls->cdls_delay = libcfs_console_min_delay;
+		else if (cdls->cdls_delay > libcfs_console_max_delay)
+			cdls->cdls_delay = libcfs_console_max_delay;
+
 		/* ensure cdls_next is never zero after it's been seen */
 		cdls->cdls_next = (cfs_time_current() + cdls->cdls_delay) | 1;
 	}
diff --git a/drivers/staging/lustre/lustre/libcfs/tracefile.h b/drivers/staging/lustre/lustre/libcfs/tracefile.h
index 7e8d17c12b5..55ecfc9f201 100644
--- a/drivers/staging/lustre/lustre/libcfs/tracefile.h
+++ b/drivers/staging/lustre/lustre/libcfs/tracefile.h
@@ -307,7 +307,7 @@ cfs_trace_put_tcd (struct cfs_trace_cpu_data *tcd)
 	put_cpu();
 }
 
-int cfs_trace_refill_stock(struct cfs_trace_cpu_data *tcd, int gfp,
+int cfs_trace_refill_stock(struct cfs_trace_cpu_data *tcd, gfp_t gfp,
 			   struct list_head *stock);
 
 
diff --git a/drivers/staging/lustre/lustre/libcfs/workitem.c b/drivers/staging/lustre/lustre/libcfs/workitem.c
index ba16fd5db70..0a03bf7ba3e 100644
--- a/drivers/staging/lustre/lustre/libcfs/workitem.c
+++ b/drivers/staging/lustre/lustre/libcfs/workitem.c
@@ -75,7 +75,7 @@ typedef struct cfs_wi_sched {
 	char			ws_name[CFS_WS_NAME_LEN];
 } cfs_wi_sched_t;
 
-struct cfs_workitem_data {
+static struct cfs_workitem_data {
 	/** serialize */
 	spinlock_t		wi_glock;
 	/** list of all schedulers */
diff --git a/drivers/staging/lustre/lustre/llite/dcache.c b/drivers/staging/lustre/lustre/llite/dcache.c
index 8b550808617..7d520d8f4a6 100644
--- a/drivers/staging/lustre/lustre/llite/dcache.c
+++ b/drivers/staging/lustre/lustre/llite/dcache.c
@@ -69,8 +69,7 @@ static void ll_release(struct dentry *de)
 		ll_intent_release(lld->lld_it);
 		OBD_FREE(lld->lld_it, sizeof(*lld->lld_it));
 	}
-	LASSERT(lld->lld_cwd_count == 0);
-	LASSERT(lld->lld_mnt_count == 0);
+
 	de->d_fsdata = NULL;
 	call_rcu(&lld->lld_rcu_head, free_dentry_data);
 }
@@ -82,8 +81,9 @@ static void ll_release(struct dentry *de)
  * an AST before calling d_revalidate_it().  The dentry still exists (marked
  * INVALID) so d_lookup() matches it, but we have no lock on it (so
  * lock_match() fails) and we spin around real_lookup(). */
-int ll_dcompare(const struct dentry *parent, const struct dentry *dentry,
-		unsigned int len, const char *str, const struct qstr *name)
+static int ll_dcompare(const struct dentry *parent, const struct dentry *dentry,
+		       unsigned int len, const char *str,
+		       const struct qstr *name)
 {
 	if (len != name->len)
 		return 1;
@@ -238,7 +238,8 @@ void ll_intent_release(struct lookup_intent *it)
 	ll_intent_drop_lock(it);
 	/* We are still holding extra reference on a request, need to free it */
 	if (it_disposition(it, DISP_ENQ_OPEN_REF))
-		 ptlrpc_req_finished(it->d.lustre.it_data); /* ll_file_open */
+		ptlrpc_req_finished(it->d.lustre.it_data); /* ll_file_open */
+
 	if (it_disposition(it, DISP_ENQ_CREATE_REF)) /* create rec */
 		ptlrpc_req_finished(it->d.lustre.it_data);
 
@@ -316,15 +317,6 @@ void ll_lookup_finish_locks(struct lookup_intent *it, struct dentry *dentry)
 	}
 }
 
-void ll_frob_intent(struct lookup_intent **itp, struct lookup_intent *deft)
-{
-	struct lookup_intent *it = *itp;
-
-	if (!it || it->it_op == IT_GETXATTR)
-		it = *itp = deft;
-
-}
-
 static int ll_revalidate_dentry(struct dentry *dentry,
 				unsigned int lookup_flags)
 {
@@ -356,7 +348,7 @@ static int ll_revalidate_dentry(struct dentry *dentry,
 /*
  * Always trust cached dentries. Update statahead window if necessary.
  */
-int ll_revalidate_nd(struct dentry *dentry, unsigned int flags)
+static int ll_revalidate_nd(struct dentry *dentry, unsigned int flags)
 {
 	int rc;
 
@@ -368,7 +360,7 @@ int ll_revalidate_nd(struct dentry *dentry, unsigned int flags)
 }
 
 
-void ll_d_iput(struct dentry *de, struct inode *inode)
+static void ll_d_iput(struct dentry *de, struct inode *inode)
 {
 	LASSERT(inode);
 	if (!find_cbdata(inode))
@@ -376,7 +368,7 @@ void ll_d_iput(struct dentry *de, struct inode *inode)
 	iput(inode);
 }
 
-struct dentry_operations ll_d_ops = {
+const struct dentry_operations ll_d_ops = {
 	.d_revalidate = ll_revalidate_nd,
 	.d_release = ll_release,
 	.d_delete  = ll_ddelete,
diff --git a/drivers/staging/lustre/lustre/llite/dir.c b/drivers/staging/lustre/lustre/llite/dir.c
index 7fbc18e3e65..ae6f61aa4da 100644
--- a/drivers/staging/lustre/lustre/llite/dir.c
+++ b/drivers/staging/lustre/lustre/llite/dir.c
@@ -632,7 +632,7 @@ out:
 	return rc;
 }
 
-int ll_send_mgc_param(struct obd_export *mgc, char *string)
+static int ll_send_mgc_param(struct obd_export *mgc, char *string)
 {
 	struct mgs_send_param *msp;
 	int rc = 0;
@@ -795,7 +795,7 @@ int ll_dir_getstripe(struct inode *inode, struct lov_mds_md **lmmp,
 	int rc, lmmsize;
 	struct md_op_data *op_data;
 
-	rc = ll_get_max_mdsize(sbi, &lmmsize);
+	rc = ll_get_default_mdsize(sbi, &lmmsize);
 	if (rc)
 		return rc;
 
@@ -1804,6 +1804,11 @@ out_rmdir:
 		/* Compute the whole struct size */
 		totalsize = hur_len(hur);
 		OBD_FREE_PTR(hur);
+
+		/* Final size will be more than double totalsize */
+		if (totalsize >= MDS_MAXREQSIZE / 3)
+			return -E2BIG;
+
 		OBD_ALLOC_LARGE(hur, totalsize);
 		if (hur == NULL)
 			return -ENOMEM;
@@ -1959,17 +1964,17 @@ out:
 	return ret;
 }
 
-int ll_dir_open(struct inode *inode, struct file *file)
+static int ll_dir_open(struct inode *inode, struct file *file)
 {
 	return ll_file_open(inode, file);
 }
 
-int ll_dir_release(struct inode *inode, struct file *file)
+static int ll_dir_release(struct inode *inode, struct file *file)
 {
 	return ll_file_release(inode, file);
 }
 
-struct file_operations ll_dir_operations = {
+const struct file_operations ll_dir_operations = {
 	.llseek   = ll_dir_seek,
 	.open     = ll_dir_open,
 	.release  = ll_dir_release,
diff --git a/drivers/staging/lustre/lustre/llite/file.c b/drivers/staging/lustre/lustre/llite/file.c
index 8e844a6371e..c4ddec2b358 100644
--- a/drivers/staging/lustre/lustre/llite/file.c
+++ b/drivers/staging/lustre/lustre/llite/file.c
@@ -50,11 +50,21 @@
 
 #include "cl_object.h"
 
-struct ll_file_data *ll_file_data_get(void)
+static int
+ll_put_grouplock(struct inode *inode, struct file *file, unsigned long arg);
+
+static int ll_lease_close(struct obd_client_handle *och, struct inode *inode,
+			  bool *lease_broken);
+
+static enum llioc_iter
+ll_iocontrol_call(struct inode *inode, struct file *file,
+		  unsigned int cmd, unsigned long arg, int *rcp);
+
+static struct ll_file_data *ll_file_data_get(void)
 {
 	struct ll_file_data *fd;
 
-	OBD_SLAB_ALLOC_PTR_GFP(fd, ll_file_data_slab, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(fd, ll_file_data_slab, GFP_NOFS);
 	if (fd == NULL)
 		return NULL;
 	fd->fd_write_failed = false;
@@ -247,8 +257,8 @@ int ll_md_real_close(struct inode *inode, fmode_t fmode)
 	return rc;
 }
 
-int ll_md_close(struct obd_export *md_exp, struct inode *inode,
-		struct file *file)
+static int ll_md_close(struct obd_export *md_exp, struct inode *inode,
+		       struct file *file)
 {
 	struct ll_file_data *fd = LUSTRE_FPRIVATE(file);
 	struct ll_inode_info *lli = ll_i2info(inode);
@@ -482,8 +492,8 @@ static int ll_och_fill(struct obd_export *md_exp, struct lookup_intent *it,
 	return md_set_open_replay_data(md_exp, och, it);
 }
 
-int ll_local_open(struct file *file, struct lookup_intent *it,
-		  struct ll_file_data *fd, struct obd_client_handle *och)
+static int ll_local_open(struct file *file, struct lookup_intent *it,
+			 struct ll_file_data *fd, struct obd_client_handle *och)
 {
 	struct inode *inode = file->f_dentry->d_inode;
 	struct ll_inode_info *lli = ll_i2info(inode);
@@ -733,8 +743,9 @@ static int ll_md_blocking_lease_ast(struct ldlm_lock *lock,
 /**
  * Acquire a lease and open the file.
  */
-struct obd_client_handle *ll_lease_open(struct inode *inode, struct file *file,
-					fmode_t fmode, __u64 open_flags)
+static struct obd_client_handle *
+ll_lease_open(struct inode *inode, struct file *file, fmode_t fmode,
+	      __u64 open_flags)
 {
 	struct lookup_intent it = { .it_op = IT_OPEN };
 	struct ll_sb_info *sbi = ll_i2sbi(inode);
@@ -862,14 +873,13 @@ out:
 	OBD_FREE_PTR(och);
 	return ERR_PTR(rc);
 }
-EXPORT_SYMBOL(ll_lease_open);
 
 /**
  * Release lease and close the file.
  * It will check if the lease has ever broken.
  */
-int ll_lease_close(struct obd_client_handle *och, struct inode *inode,
-			bool *lease_broken)
+static int ll_lease_close(struct obd_client_handle *och, struct inode *inode,
+			  bool *lease_broken)
 {
 	struct ldlm_lock *lock;
 	bool cancelled = true;
@@ -895,7 +905,6 @@ int ll_lease_close(struct obd_client_handle *och, struct inode *inode,
 				       NULL);
 	return rc;
 }
-EXPORT_SYMBOL(ll_lease_close);
 
 /* Fills the obdo with the attributes for the lsm */
 static int ll_lsm_getattr(struct lov_stripe_md *lsm, struct obd_export *exp,
@@ -1440,7 +1449,7 @@ int ll_lov_getstripe_ea_info(struct inode *inode, const char *filename,
 	struct md_op_data *op_data;
 	int rc, lmmsize;
 
-	rc = ll_get_max_mdsize(sbi, &lmmsize);
+	rc = ll_get_default_mdsize(sbi, &lmmsize);
 	if (rc)
 		return rc;
 
@@ -1590,7 +1599,8 @@ static int ll_lov_getstripe(struct inode *inode, unsigned long arg)
 	return rc;
 }
 
-int ll_get_grouplock(struct inode *inode, struct file *file, unsigned long arg)
+static int
+ll_get_grouplock(struct inode *inode, struct file *file, unsigned long arg)
 {
 	struct ll_inode_info   *lli = ll_i2info(inode);
 	struct ll_file_data    *fd = LUSTRE_FPRIVATE(file);
@@ -1710,13 +1720,13 @@ out:
  * Get size for inode for which FIEMAP mapping is requested.
  * Make the FIEMAP get_info call and returns the result.
  */
-int ll_do_fiemap(struct inode *inode, struct ll_user_fiemap *fiemap,
-	      int num_bytes)
+static int ll_do_fiemap(struct inode *inode, struct ll_user_fiemap *fiemap,
+			size_t num_bytes)
 {
 	struct obd_export *exp = ll_i2dtexp(inode);
 	struct lov_stripe_md *lsm = NULL;
 	struct ll_fiemap_info_key fm_key = { .name = KEY_FIEMAP, };
-	int vallen = num_bytes;
+	__u32 vallen = num_bytes;
 	int rc;
 
 	/* Checks for fiemap flags */
@@ -1819,6 +1829,10 @@ static int ll_ioctl_fiemap(struct inode *inode, unsigned long arg)
 	if (get_user(extent_count,
 	    &((struct ll_user_fiemap __user *)arg)->fm_extent_count))
 		return -EFAULT;
+
+	if (extent_count >=
+	    (SIZE_MAX - sizeof(*fiemap_s)) / sizeof(struct ll_fiemap_extent))
+		return -EINVAL;
 	num_bytes = sizeof(*fiemap_s) + (extent_count *
 					 sizeof(struct ll_fiemap_extent));
 
@@ -2190,7 +2204,8 @@ out:
 	return rc;
 }
 
-long ll_file_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+static long
+ll_file_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 {
 	struct inode		*inode = file->f_dentry->d_inode;
 	struct ll_file_data	*fd = LUSTRE_FPRIVATE(file);
@@ -2509,7 +2524,7 @@ long ll_file_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 }
 
 
-loff_t ll_file_seek(struct file *file, loff_t offset, int origin)
+static loff_t ll_file_seek(struct file *file, loff_t offset, int origin)
 {
 	struct inode *inode = file->f_dentry->d_inode;
 	loff_t retval, eof = 0;
@@ -2533,7 +2548,7 @@ loff_t ll_file_seek(struct file *file, loff_t offset, int origin)
 	return retval;
 }
 
-int ll_flush(struct file *file, fl_owner_t id)
+static int ll_flush(struct file *file, fl_owner_t id)
 {
 	struct inode *inode = file->f_dentry->d_inode;
 	struct ll_inode_info *lli = ll_i2info(inode);
@@ -2559,7 +2574,7 @@ int ll_flush(struct file *file, fl_owner_t id)
 
 /**
  * Called to make sure a portion of file has been written out.
- * if @local_only is not true, it will send OST_SYNC RPCs to ost.
+ * if @mode is not CL_FSYNC_LOCAL, it will send OST_SYNC RPCs to OST.
  *
  * Return how many pages have been written.
  */
@@ -2653,11 +2668,10 @@ int ll_fsync(struct file *file, loff_t start, loff_t end, int datasync)
 	if (!err)
 		ptlrpc_req_finished(req);
 
-	if (datasync && S_ISREG(inode->i_mode)) {
+	if (S_ISREG(inode->i_mode)) {
 		struct ll_file_data *fd = LUSTRE_FPRIVATE(file);
 
-		err = cl_sync_file_range(inode, 0, OBD_OBJECT_EOF,
-				CL_FSYNC_ALL, 0);
+		err = cl_sync_file_range(inode, start, end, CL_FSYNC_ALL, 0);
 		if (rc == 0 && err < 0)
 			rc = err;
 		if (rc < 0)
@@ -2670,7 +2684,8 @@ int ll_fsync(struct file *file, loff_t start, loff_t end, int datasync)
 	return rc;
 }
 
-int ll_file_flock(struct file *file, int cmd, struct file_lock *file_lock)
+static int
+ll_file_flock(struct file *file, int cmd, struct file_lock *file_lock)
 {
 	struct inode *inode = file->f_dentry->d_inode;
 	struct ll_sb_info *sbi = ll_i2sbi(inode);
@@ -2691,20 +2706,15 @@ int ll_file_flock(struct file *file, int cmd, struct file_lock *file_lock)
 
 	ll_stats_ops_tally(ll_i2sbi(inode), LPROC_LL_FLOCK, 1);
 
-	if (file_lock->fl_flags & FL_FLOCK) {
+	if (file_lock->fl_flags & FL_FLOCK)
 		LASSERT((cmd == F_SETLKW) || (cmd == F_SETLK));
-		/* flocks are whole-file locks */
-		flock.l_flock.end = OFFSET_MAX;
-		/* For flocks owner is determined by the local file descriptor*/
-		flock.l_flock.owner = (unsigned long)file_lock->fl_file;
-	} else if (file_lock->fl_flags & FL_POSIX) {
-		flock.l_flock.owner = (unsigned long)file_lock->fl_owner;
-		flock.l_flock.start = file_lock->fl_start;
-		flock.l_flock.end = file_lock->fl_end;
-	} else {
+	else if (!(file_lock->fl_flags & FL_POSIX))
 		return -EINVAL;
-	}
+
+	flock.l_flock.owner = (unsigned long)file_lock->fl_owner;
 	flock.l_flock.pid = file_lock->fl_pid;
+	flock.l_flock.start = file_lock->fl_start;
+	flock.l_flock.end = file_lock->fl_end;
 
 	/* Somewhat ugly workaround for svc lockd.
 	 * lockd installs custom fl_lmops->lm_compare_owner that checks
@@ -2799,7 +2809,8 @@ int ll_file_flock(struct file *file, int cmd, struct file_lock *file_lock)
 	return rc;
 }
 
-int ll_file_noflock(struct file *file, int cmd, struct file_lock *file_lock)
+static int
+ll_file_noflock(struct file *file, int cmd, struct file_lock *file_lock)
 {
 	return -ENOSYS;
 }
@@ -2882,16 +2893,16 @@ static int ll_inode_revalidate_fini(struct inode *inode, int rc)
 		if (!S_ISREG(inode->i_mode) && !S_ISDIR(inode->i_mode))
 			return 0;
 	} else if (rc != 0) {
-		CERROR("%s: revalidate FID "DFID" error: rc = %d\n",
-		       ll_get_fsname(inode->i_sb, NULL, 0),
-		       PFID(ll_inode2fid(inode)), rc);
+		CDEBUG_LIMIT((rc == -EACCES || rc == -EIDRM) ? D_INFO : D_ERROR,
+			     "%s: revalidate FID "DFID" error: rc = %d\n",
+			     ll_get_fsname(inode->i_sb, NULL, 0),
+			     PFID(ll_inode2fid(inode)), rc);
 	}
 
 	return rc;
 }
 
-int __ll_inode_revalidate_it(struct dentry *dentry, struct lookup_intent *it,
-			     __u64 ibits)
+static int __ll_inode_revalidate(struct dentry *dentry, __u64 ibits)
 {
 	struct inode *inode = dentry->d_inode;
 	struct ptlrpc_request *req = NULL;
@@ -2956,7 +2967,7 @@ int __ll_inode_revalidate_it(struct dentry *dentry, struct lookup_intent *it,
 		int ealen = 0;
 
 		if (S_ISREG(inode->i_mode)) {
-			rc = ll_get_max_mdsize(sbi, &ealen);
+			rc = ll_get_default_mdsize(sbi, &ealen);
 			if (rc)
 				return rc;
 			valid |= OBD_MD_FLEASIZE | OBD_MD_FLMODEASIZE;
@@ -2986,13 +2997,12 @@ out:
 	return rc;
 }
 
-int ll_inode_revalidate_it(struct dentry *dentry, struct lookup_intent *it,
-			   __u64 ibits)
+static int ll_inode_revalidate(struct dentry *dentry, __u64 ibits)
 {
 	struct inode *inode = dentry->d_inode;
 	int rc;
 
-	rc = __ll_inode_revalidate_it(dentry, it, ibits);
+	rc = __ll_inode_revalidate(dentry, ibits);
 	if (rc != 0)
 		return rc;
 
@@ -3015,16 +3025,15 @@ int ll_inode_revalidate_it(struct dentry *dentry, struct lookup_intent *it,
 	return rc;
 }
 
-int ll_getattr_it(struct vfsmount *mnt, struct dentry *de,
-		  struct lookup_intent *it, struct kstat *stat)
+int ll_getattr(struct vfsmount *mnt, struct dentry *de, struct kstat *stat)
 {
 	struct inode *inode = de->d_inode;
 	struct ll_sb_info *sbi = ll_i2sbi(inode);
 	struct ll_inode_info *lli = ll_i2info(inode);
 	int res = 0;
 
-	res = ll_inode_revalidate_it(de, it, MDS_INODELOCK_UPDATE |
-					     MDS_INODELOCK_LOOKUP);
+	res = ll_inode_revalidate(de, MDS_INODELOCK_UPDATE |
+				      MDS_INODELOCK_LOOKUP);
 	ll_stats_ops_tally(sbi, LPROC_LL_GETATTR, 1);
 
 	if (res)
@@ -3050,15 +3059,9 @@ int ll_getattr_it(struct vfsmount *mnt, struct dentry *de,
 
 	return 0;
 }
-int ll_getattr(struct vfsmount *mnt, struct dentry *de, struct kstat *stat)
-{
-	struct lookup_intent it = { .it_op = IT_GETATTR };
-
-	return ll_getattr_it(mnt, de, &it, stat);
-}
 
-int ll_fiemap(struct inode *inode, struct fiemap_extent_info *fieinfo,
-		__u64 start, __u64 len)
+static int ll_fiemap(struct inode *inode, struct fiemap_extent_info *fieinfo,
+		     __u64 start, __u64 len)
 {
 	int rc;
 	size_t num_bytes;
@@ -3076,21 +3079,24 @@ int ll_fiemap(struct inode *inode, struct fiemap_extent_info *fieinfo,
 	fiemap->fm_extent_count = fieinfo->fi_extents_max;
 	fiemap->fm_start = start;
 	fiemap->fm_length = len;
-	memcpy(&fiemap->fm_extents[0], fieinfo->fi_extents_start,
-	       sizeof(struct ll_fiemap_extent));
+	if (extent_count > 0)
+		memcpy(&fiemap->fm_extents[0], fieinfo->fi_extents_start,
+		       sizeof(struct ll_fiemap_extent));
 
 	rc = ll_do_fiemap(inode, fiemap, num_bytes);
 
 	fieinfo->fi_flags = fiemap->fm_flags;
 	fieinfo->fi_extents_mapped = fiemap->fm_mapped_extents;
-	memcpy(fieinfo->fi_extents_start, &fiemap->fm_extents[0],
-	       fiemap->fm_mapped_extents * sizeof(struct ll_fiemap_extent));
+	if (extent_count > 0)
+		memcpy(fieinfo->fi_extents_start, &fiemap->fm_extents[0],
+		       fiemap->fm_mapped_extents *
+		       sizeof(struct ll_fiemap_extent));
 
 	OBD_FREE_LARGE(fiemap, num_bytes);
 	return rc;
 }
 
-struct posix_acl * ll_get_acl(struct inode *inode, int type)
+struct posix_acl *ll_get_acl(struct inode *inode, int type)
 {
 	struct ll_inode_info *lli = ll_i2info(inode);
 	struct posix_acl *acl = NULL;
@@ -3117,10 +3123,8 @@ int ll_inode_permission(struct inode *inode, int mask)
 	* need to do it before permission check. */
 
 	if (inode == inode->i_sb->s_root->d_inode) {
-		struct lookup_intent it = { .it_op = IT_LOOKUP };
-
-		rc = __ll_inode_revalidate_it(inode->i_sb->s_root, &it,
-					      MDS_INODELOCK_LOOKUP);
+		rc = __ll_inode_revalidate(inode->i_sb->s_root,
+					   MDS_INODELOCK_LOOKUP);
 		if (rc)
 			return rc;
 	}
@@ -3272,8 +3276,9 @@ void ll_iocontrol_unregister(void *magic)
 EXPORT_SYMBOL(ll_iocontrol_register);
 EXPORT_SYMBOL(ll_iocontrol_unregister);
 
-enum llioc_iter ll_iocontrol_call(struct inode *inode, struct file *file,
-			unsigned int cmd, unsigned long arg, int *rcp)
+static enum llioc_iter
+ll_iocontrol_call(struct inode *inode, struct file *file,
+		  unsigned int cmd, unsigned long arg, int *rcp)
 {
 	enum llioc_iter ret = LLIOC_CONT;
 	struct llioc_data *data;
@@ -3358,7 +3363,7 @@ static int ll_layout_fetch(struct inode *inode, struct ldlm_lock *lock)
 	 * layout here. Please note that we can't use the LVB buffer in
 	 * completion AST because it doesn't have a large enough buffer */
 	oc = ll_mdscapa_get(inode);
-	rc = ll_get_max_mdsize(sbi, &lmmsize);
+	rc = ll_get_default_mdsize(sbi, &lmmsize);
 	if (rc == 0)
 		rc = md_getxattr(sbi->ll_md_exp, ll_inode2fid(inode), oc,
 				OBD_MD_FLXATTR, XATTR_NAME_LOV, NULL, 0,
@@ -3368,7 +3373,7 @@ 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 || body->eadatasize > lmmsize)
+	if (body == NULL)
 		GOTO(out, rc = -EPROTO);
 
 	lmmsize = body->eadatasize;
@@ -3435,7 +3440,7 @@ static int ll_layout_lock_set(struct lustre_handle *lockh, ldlm_mode_t mode,
 		if (lvb_ready) {
 			/* layout_gen must be valid if layout lock is not
 			 * cancelled and stripe has already set */
-			*gen = lli->lli_layout_gen;
+			*gen = ll_layout_version_get(lli);
 			rc = 0;
 		}
 		GOTO(out, rc);
@@ -3533,32 +3538,20 @@ int ll_layout_refresh(struct inode *inode, __u32 *gen)
 	};
 	int rc;
 
-	*gen = lli->lli_layout_gen;
-	if (!(sbi->ll_flags & LL_SBI_LAYOUT_LOCK))
+	*gen = ll_layout_version_get(lli);
+	if (!(sbi->ll_flags & LL_SBI_LAYOUT_LOCK) || *gen != LL_LAYOUT_GEN_NONE)
 		return 0;
 
 	/* sanity checks */
 	LASSERT(fid_is_sane(ll_inode2fid(inode)));
 	LASSERT(S_ISREG(inode->i_mode));
 
-	/* mostly layout lock is caching on the local side, so try to match
-	 * it before grabbing layout lock mutex. */
-	mode = ll_take_md_lock(inode, MDS_INODELOCK_LAYOUT, &lockh, 0,
-			       LCK_CR | LCK_CW | LCK_PR | LCK_PW);
-	if (mode != 0) { /* hit cached lock */
-		rc = ll_layout_lock_set(&lockh, mode, inode, gen, false);
-		if (rc == 0)
-			return 0;
-
-		/* better hold lli_layout_mutex to try again otherwise
-		 * it will have starvation problem. */
-	}
-
 	/* take layout lock mutex to enqueue layout lock exclusively. */
 	mutex_lock(&lli->lli_layout_mutex);
 
 again:
-	/* try again. Maybe somebody else has done this. */
+	/* mostly layout lock is caching on the local side, so try to match
+	 * it before grabbing layout lock mutex. */
 	mode = ll_take_md_lock(inode, MDS_INODELOCK_LAYOUT, &lockh, 0,
 			       LCK_CR | LCK_CW | LCK_PR | LCK_PW);
 	if (mode != 0) { /* hit cached lock */
diff --git a/drivers/staging/lustre/lustre/llite/llite_capa.c b/drivers/staging/lustre/lustre/llite/llite_capa.c
index edd512b20ee..d06d0b1ab08 100644
--- a/drivers/staging/lustre/lustre/llite/llite_capa.c
+++ b/drivers/staging/lustre/lustre/llite/llite_capa.c
@@ -68,6 +68,8 @@ static unsigned long long ll_capa_renewal_noent = 0;
 static unsigned long long ll_capa_renewal_failed = 0;
 static unsigned long long ll_capa_renewal_retries = 0;
 
+static int ll_update_capa(struct obd_capa *ocapa, struct lustre_capa *capa);
+
 static inline void update_capa_timer(struct obd_capa *ocapa, cfs_time_t expiry)
 {
 	if (cfs_time_before(expiry, ll_capa_timer.expires) ||
@@ -515,7 +517,7 @@ static inline void delay_capa_renew(struct obd_capa *oc, cfs_time_t delay)
 	oc->c_expiry = cfs_time_add(oc->c_expiry, cfs_time_seconds(delay));
 }
 
-int ll_update_capa(struct obd_capa *ocapa, struct lustre_capa *capa)
+static int ll_update_capa(struct obd_capa *ocapa, struct lustre_capa *capa)
 {
 	struct inode *inode = ocapa->u.cli.inode;
 	int rc = 0;
diff --git a/drivers/staging/lustre/lustre/llite/llite_internal.h b/drivers/staging/lustre/lustre/llite/llite_internal.h
index 69aba0afca4..dde7632ba01 100644
--- a/drivers/staging/lustre/lustre/llite/llite_internal.h
+++ b/drivers/staging/lustre/lustre/llite/llite_internal.h
@@ -71,10 +71,6 @@
 #define LUSTRE_FPRIVATE(file) ((file)->private_data)
 
 struct ll_dentry_data {
-	int				lld_cwd_count;
-	int				lld_mnt_count;
-	struct obd_client_handle	lld_cwd_och;
-	struct obd_client_handle	lld_mnt_och;
 	struct lookup_intent		*lld_it;
 	unsigned int			lld_sa_generation;
 	unsigned int			lld_invalid:1;
@@ -83,8 +79,6 @@ struct ll_dentry_data {
 
 #define ll_d2d(de) ((struct ll_dentry_data*)((de)->d_fsdata))
 
-extern struct file_operations ll_pgcache_seq_fops;
-
 #define LLI_INODE_MAGIC		 0x111d0de5
 #define LLI_INODE_DEAD		  0xdeadd00d
 
@@ -120,16 +114,12 @@ enum lli_flags {
 	/* Sizeon-on-MDS attributes are changed. An attribute update needs to
 	 * be sent to MDS. */
 	LLIF_SOM_DIRTY	  = (1 << 3),
-	/* File is contented */
-	LLIF_CONTENDED	  = (1 << 4),
-	/* Truncate uses server lock for this file */
-	LLIF_SRVLOCK	    = (1 << 5),
 	/* File data is modified. */
-	LLIF_DATA_MODIFIED      = (1 << 6),
+	LLIF_DATA_MODIFIED      = (1 << 4),
 	/* File is being restored */
-	LLIF_FILE_RESTORING	= (1 << 7),
+	LLIF_FILE_RESTORING	= (1 << 5),
 	/* Xattr cache is attached to the file */
-	LLIF_XATTR_CACHE	= (1 << 8),
+	LLIF_XATTR_CACHE	= (1 << 6),
 };
 
 struct ll_inode_info {
@@ -194,7 +184,6 @@ struct ll_inode_info {
 			 * cleanup the dir readahead. */
 			void			   *d_opendir_key;
 			struct ll_statahead_info       *d_sai;
-			struct posix_acl	       *d_def_acl;
 			/* protect statahead stuff. */
 			spinlock_t			d_sa_lock;
 			/* "opendir_pid" is the token when lookup/revalid
@@ -205,14 +194,12 @@ struct ll_inode_info {
 #define lli_readdir_mutex       u.d.d_readdir_mutex
 #define lli_opendir_key	 u.d.d_opendir_key
 #define lli_sai		 u.d.d_sai
-#define lli_def_acl	     u.d.d_def_acl
 #define lli_sa_lock	     u.d.d_sa_lock
 #define lli_opendir_pid	 u.d.d_opendir_pid
 
 		/* for non-directory */
 		struct {
-			struct semaphore		f_size_sem;
-			void				*f_size_sem_owner;
+			struct mutex			f_size_mutex;
 			char				*f_symlink_name;
 			__u64				f_maxbytes;
 			/*
@@ -233,11 +220,6 @@ struct ll_inode_info {
 			/* for writepage() only to communicate to fsync */
 			int				f_async_rc;
 
-			/* volatile file criteria is based on file name, this
-			 * flag is used to keep the test result, so the strcmp
-			 * is done only once
-			 */
-			bool				f_volatile;
 			/*
 			 * whenever a process try to read/write the file, the
 			 * jobid of the process will be saved here, and it'll
@@ -249,8 +231,7 @@ struct ll_inode_info {
 			char		     f_jobid[JOBSTATS_JOBID_SIZE];
 		} f;
 
-#define lli_size_sem	    u.f.f_size_sem
-#define lli_size_sem_owner      u.f.f_size_sem_owner
+#define lli_size_mutex          u.f.f_size_mutex
 #define lli_symlink_name	u.f.f_symlink_name
 #define lli_maxbytes	    u.f.f_maxbytes
 #define lli_trunc_sem	   u.f.f_trunc_sem
@@ -261,7 +242,6 @@ struct ll_inode_info {
 #define lli_agl_index		u.f.f_agl_index
 #define lli_async_rc		u.f.f_async_rc
 #define lli_jobid		u.f.f_jobid
-#define lli_volatile		u.f.f_volatile
 
 	} u;
 
@@ -280,14 +260,33 @@ struct ll_inode_info {
 
 	/* mutex to request for layout lock exclusively. */
 	struct mutex			lli_layout_mutex;
-	/* valid only inside LAYOUT ibits lock, protected by lli_layout_mutex */
+	/* Layout version, protected by lli_layout_lock */
 	__u32				lli_layout_gen;
+	spinlock_t			lli_layout_lock;
 
 	struct rw_semaphore		lli_xattrs_list_rwsem;
 	struct mutex			lli_xattrs_enq_lock;
 	struct list_head		lli_xattrs;/* ll_xattr_entry->xe_list */
 };
 
+static inline __u32 ll_layout_version_get(struct ll_inode_info *lli)
+{
+	__u32 gen;
+
+	spin_lock(&lli->lli_layout_lock);
+	gen = lli->lli_layout_gen;
+	spin_unlock(&lli->lli_layout_lock);
+
+	return gen;
+}
+
+static inline void ll_layout_version_set(struct ll_inode_info *lli, __u32 gen)
+{
+	spin_lock(&lli->lli_layout_lock);
+	lli->lli_layout_gen = gen;
+	spin_unlock(&lli->lli_layout_lock);
+}
+
 int ll_xattr_cache_destroy(struct inode *inode);
 
 int ll_xattr_cache_get(struct inode *inode,
@@ -300,7 +299,7 @@ int ll_xattr_cache_get(struct inode *inode,
  * Locking to guarantee consistency of non-atomic updates to long long i_size,
  * consistency between file size and KMS.
  *
- * Implemented by ->lli_size_sem and ->lsm_lock, nested in that order.
+ * Implemented by ->lli_size_mutex and ->lsm_lock, nested in that order.
  */
 
 void ll_inode_size_lock(struct inode *inode);
@@ -442,10 +441,6 @@ enum stats_track_type {
 	"xattr",	\
 }
 
-/* default value for ll_sb_info->contention_time */
-#define SBI_DEFAULT_CONTENTION_SECONDS     60
-/* default value for lockless_truncate_enable */
-#define SBI_DEFAULT_LOCKLESS_TRUNCATE_ENABLE 1
 #define RCE_HASHES      32
 
 struct rmtacl_ctl_entry {
@@ -656,12 +651,6 @@ static inline struct inode *ll_info2i(struct ll_inode_info *lli)
 	return &lli->lli_vfs_inode;
 }
 
-struct it_cb_data {
-	struct inode  *icbd_parent;
-	struct dentry **icbd_childp;
-	obd_id	hash;
-};
-
 __u32 ll_i2suppgid(struct inode *i);
 void ll_i2gids(__u32 *suppgids, struct inode *i1,struct inode *i2);
 
@@ -669,21 +658,13 @@ static inline int ll_need_32bit_api(struct ll_sb_info *sbi)
 {
 #if BITS_PER_LONG == 32
 	return 1;
+#elif defined(CONFIG_COMPAT)
+	return unlikely(is_compat_task() || (sbi->ll_flags & LL_SBI_32BIT_API));
 #else
-	return unlikely(
-#ifdef CONFIG_COMPAT
-		is_compat_task() ||
-#endif
-		(sbi->ll_flags & LL_SBI_32BIT_API)
-	);
+	return unlikely(sbi->ll_flags & LL_SBI_32BIT_API);
 #endif
 }
 
-#define LLAP_MAGIC 98764321
-
-extern struct kmem_cache *ll_async_page_slab;
-extern size_t ll_async_page_slab_size;
-
 void ll_ra_read_in(struct file *f, struct ll_ra_read *rar);
 void ll_ra_read_ex(struct file *f, struct ll_ra_read *rar);
 struct ll_ra_read *ll_ra_read_get(struct file *f);
@@ -716,14 +697,16 @@ static inline void ll_rw_stats_tally(struct ll_sb_info *sbi, pid_t pid,
 
 /* llite/dir.c */
 void ll_release_page(struct page *page, int remove);
-extern struct file_operations ll_dir_operations;
-extern struct inode_operations ll_dir_inode_operations;
+extern const struct file_operations ll_dir_operations;
+extern const struct inode_operations ll_dir_inode_operations;
 struct page *ll_get_dir_page(struct inode *dir, __u64 hash,
 			     struct ll_dir_chain *chain);
 int ll_dir_read(struct inode *inode, struct dir_context *ctx);
 
 int ll_get_mdt_idx(struct inode *inode);
 /* llite/namei.c */
+extern const struct inode_operations ll_special_inode_operations;
+
 int ll_objects_destroy(struct ptlrpc_request *request,
 		       struct inode *dir);
 struct inode *ll_iget(struct super_block *sb, ino_t hash,
@@ -738,43 +721,34 @@ int ll_prepare_write(struct file *, struct page *, unsigned from, unsigned to);
 int ll_commit_write(struct file *, struct page *, unsigned from, unsigned to);
 int ll_writepage(struct page *page, struct writeback_control *wbc);
 int ll_writepages(struct address_space *, struct writeback_control *wbc);
-void ll_removepage(struct page *page);
 int ll_readpage(struct file *file, struct page *page);
 void ll_readahead_init(struct inode *inode, struct ll_readahead_state *ras);
-int ll_file_punch(struct inode *, loff_t, int);
-ssize_t ll_file_lockless_io(struct file *, char *, size_t, loff_t *, int);
-void ll_clear_file_contended(struct inode*);
-int ll_sync_page_range(struct inode *, struct address_space *, loff_t, size_t);
 int ll_readahead(const struct lu_env *env, struct cl_io *io,
 		 struct ll_readahead_state *ras, struct address_space *mapping,
 		 struct cl_page_list *queue, int flags);
 
+#ifndef MS_HAS_NEW_AOPS
+extern const struct address_space_operations ll_aops;
+#else
+extern const struct address_space_operations_ext ll_aops;
+#endif
+
 /* llite/file.c */
 extern struct file_operations ll_file_operations;
 extern struct file_operations ll_file_operations_flock;
 extern struct file_operations ll_file_operations_noflock;
 extern struct inode_operations ll_file_inode_operations;
-extern int ll_inode_revalidate_it(struct dentry *, struct lookup_intent *,
-				  __u64);
 extern int ll_have_md_lock(struct inode *inode, __u64 *bits,
 			   ldlm_mode_t l_req_mode);
 extern ldlm_mode_t ll_take_md_lock(struct inode *inode, __u64 bits,
 				   struct lustre_handle *lockh, __u64 flags,
 				   ldlm_mode_t mode);
-int __ll_inode_revalidate_it(struct dentry *, struct lookup_intent *,
-			     __u64 bits);
-int ll_revalidate_nd(struct dentry *dentry, unsigned int flags);
 int ll_file_open(struct inode *inode, struct file *file);
 int ll_file_release(struct inode *inode, struct file *file);
 int ll_glimpse_ioctl(struct ll_sb_info *sbi,
 		     struct lov_stripe_md *lsm, lstat_t *st);
 void ll_ioepoch_open(struct ll_inode_info *lli, __u64 ioepoch);
-int ll_local_open(struct file *file,
-		  struct lookup_intent *it, struct ll_file_data *fd,
-		  struct obd_client_handle *och);
 int ll_release_openhandle(struct dentry *, struct lookup_intent *);
-int ll_md_close(struct obd_export *md_exp, struct inode *inode,
-		struct file *file);
 int ll_md_real_close(struct inode *inode, fmode_t fmode);
 void ll_ioepoch_close(struct inode *inode, struct md_op_data *op_data,
 		      struct obd_client_handle **och, unsigned long flags);
@@ -782,15 +756,10 @@ void ll_done_writing_attr(struct inode *inode, struct md_op_data *op_data);
 int ll_som_update(struct inode *inode, struct md_op_data *op_data);
 int ll_inode_getattr(struct inode *inode, struct obdo *obdo,
 		     __u64 ioepoch, int sync);
-int ll_md_setattr(struct dentry *dentry, struct md_op_data *op_data,
-		  struct md_open_data **mod);
 void ll_pack_inode2opdata(struct inode *inode, struct md_op_data *op_data,
 			  struct lustre_handle *fh);
-int ll_getattr_it(struct vfsmount *mnt, struct dentry *de,
-	       struct lookup_intent *it, struct kstat *stat);
 int ll_getattr(struct vfsmount *mnt, struct dentry *de, struct kstat *stat);
-struct ll_file_data *ll_file_data_get(void);
-struct posix_acl * ll_get_acl(struct inode *inode, int type);
+struct posix_acl *ll_get_acl(struct inode *inode, int type);
 
 int ll_inode_permission(struct inode *inode, int mask);
 
@@ -805,44 +774,30 @@ int ll_dir_setstripe(struct inode *inode, struct lov_user_md *lump,
 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_do_fiemap(struct inode *inode, struct ll_user_fiemap *fiemap,
-	      int num_bytes);
 int ll_merge_lvb(const struct lu_env *env, struct inode *inode);
-int ll_get_grouplock(struct inode *inode, struct file *file, unsigned long arg);
-int ll_put_grouplock(struct inode *inode, struct file *file, unsigned long arg);
 int ll_fid2path(struct inode *inode, void *arg);
 int ll_data_version(struct inode *inode, __u64 *data_version, int extent_lock);
 int ll_hsm_release(struct inode *inode);
 
-struct obd_client_handle *ll_lease_open(struct inode *inode, struct file *file,
-					fmode_t mode, __u64 flags);
-int ll_lease_close(struct obd_client_handle *och, struct inode *inode,
-		   bool *lease_broken);
-
 /* llite/dcache.c */
 
 int ll_d_init(struct dentry *de);
-extern struct dentry_operations ll_d_ops;
+extern const struct dentry_operations ll_d_ops;
 void ll_intent_drop_lock(struct lookup_intent *);
 void ll_intent_release(struct lookup_intent *);
 void ll_invalidate_aliases(struct inode *);
-void ll_frob_intent(struct lookup_intent **itp, struct lookup_intent *deft);
 void ll_lookup_finish_locks(struct lookup_intent *it, struct dentry *dentry);
-int ll_dcompare(const struct dentry *parent, const struct dentry *dentry,
-		unsigned int len, const char *str, const struct qstr *d_name);
 int ll_revalidate_it_finish(struct ptlrpc_request *request,
 			    struct lookup_intent *it, struct dentry *de);
 
 /* llite/llite_lib.c */
 extern struct super_operations lustre_super_operations;
 
-char *ll_read_opt(const char *opt, char *data);
 void ll_lli_init(struct ll_inode_info *lli);
 int ll_fill_super(struct super_block *sb, struct vfsmount *mnt);
 void ll_put_super(struct super_block *sb);
 void ll_kill_super(struct super_block *sb);
 struct inode *ll_inode_from_resource_lock(struct ldlm_lock *lock);
-struct inode *ll_inode_from_lock(struct ldlm_lock *lock);
 void ll_clear_inode(struct inode *inode);
 int ll_setattr_raw(struct dentry *dentry, struct iattr *attr, bool hsm_import);
 int ll_setattr(struct dentry *de, struct iattr *attr);
@@ -862,9 +817,11 @@ void ll_dirty_page_discard_warn(struct page *page, int ioret);
 int ll_prep_inode(struct inode **inode, struct ptlrpc_request *req,
 		  struct super_block *, struct lookup_intent *);
 void lustre_dump_dentry(struct dentry *, int recur);
-void lustre_dump_inode(struct inode *);
 int ll_obd_statfs(struct inode *inode, void *arg);
 int ll_get_max_mdsize(struct ll_sb_info *sbi, int *max_mdsize);
+int ll_get_default_mdsize(struct ll_sb_info *sbi, int *default_mdsize);
+int ll_get_max_cookiesize(struct ll_sb_info *sbi, int *max_cookiesize);
+int ll_get_default_cookiesize(struct ll_sb_info *sbi, int *default_cookiesize);
 int ll_process_config(struct lustre_cfg *lcfg);
 struct md_op_data *ll_prep_md_op_data(struct md_op_data *op_data,
 				      struct inode *i1, struct inode *i2,
@@ -881,15 +838,6 @@ void get_uuid2fsid(const char *name, int len, __kernel_fsid_t *fsid);
 struct inode *search_inode_for_lustre(struct super_block *sb,
 				      const struct lu_fid *fid);
 
-/* llite/special.c */
-extern struct inode_operations ll_special_inode_operations;
-extern struct file_operations ll_special_chr_inode_fops;
-extern struct file_operations ll_special_chr_file_fops;
-extern struct file_operations ll_special_blk_inode_fops;
-extern struct file_operations ll_special_fifo_inode_fops;
-extern struct file_operations ll_special_fifo_file_fops;
-extern struct file_operations ll_special_sock_inode_fops;
-
 /* llite/symlink.c */
 extern struct inode_operations ll_fast_symlink_inode_operations;
 
@@ -957,11 +905,6 @@ struct vvp_io {
 	 * Set when cui_bead has been initialized.
 	 */
 	int		  cui_ra_window_set;
-	/**
-	 * Partially truncated page, that vvp_io_trunc_start() keeps locked
-	 * across truncate.
-	 */
-	struct cl_page      *cui_partpage;
 };
 
 /**
@@ -990,12 +933,9 @@ struct ll_cl_context {
 	struct cl_page *lcc_page;
 	struct lu_env  *lcc_env;
 	int	     lcc_refcheck;
-	int	     lcc_created;
 };
 
 struct vvp_thread_info {
-	struct ost_lvb       vti_lvb;
-	struct cl_2queue     vti_queue;
 	struct iovec	 vti_local_iov;
 	struct vvp_io_args   vti_args;
 	struct ra_io_arg     vti_ria;
@@ -1042,25 +982,17 @@ static inline struct vvp_io *vvp_env_io(const struct lu_env *env)
 	return &vvp_env_session(env)->vs_ios;
 }
 
+int vvp_global_init(void);
+void vvp_global_fini(void);
+
 void ll_queue_done_writing(struct inode *inode, unsigned long flags);
 void ll_close_thread_shutdown(struct ll_close_queue *lcq);
 int ll_close_thread_start(struct ll_close_queue **lcq_ret);
 
 /* llite/llite_mmap.c */
-typedef struct rb_root  rb_root_t;
-typedef struct rb_node  rb_node_t;
-
-struct ll_lock_tree_node;
-struct ll_lock_tree {
-	rb_root_t		       lt_root;
-	struct list_head		      lt_locked_list;
-	struct ll_file_data	    *lt_fd;
-};
 
 int ll_teardown_mmaps(struct address_space *mapping, __u64 first, __u64 last);
 int ll_file_mmap(struct file * file, struct vm_area_struct * vma);
-struct ll_lock_tree_node * ll_node_from_inode(struct inode *inode, __u64 start,
-					      __u64 end, ldlm_mode_t mode);
 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,
@@ -1127,11 +1059,6 @@ static inline struct lu_fid *ll_inode2fid(struct inode *inode)
 	return fid;
 }
 
-static inline int ll_mds_max_easize(struct super_block *sb)
-{
-	return sbi2mdc(ll_s2sbi(sb))->cl_max_mds_easize;
-}
-
 static inline __u64 ll_file_maxbytes(struct inode *inode)
 {
 	return ll_i2info(inode)->lli_maxbytes;
@@ -1149,7 +1076,6 @@ int ll_removexattr(struct dentry *dentry, const char *name);
 extern struct kmem_cache *ll_remote_perm_cachep;
 extern struct kmem_cache *ll_rmtperm_hash_cachep;
 
-struct hlist_head *alloc_rmtperm_hash(void);
 void free_rmtperm_hash(struct hlist_head *hash);
 int ll_update_remote_perm(struct inode *inode, struct mdt_remote_perm *perm);
 int lustre_check_remote_perm(struct inode *inode, int mask);
@@ -1162,7 +1088,6 @@ void ll_capa_thread_stop(void);
 void ll_capa_timer_callback(unsigned long unused);
 
 struct obd_capa *ll_add_capa(struct inode *inode, struct obd_capa *ocapa);
-int ll_update_capa(struct obd_capa *ocapa, struct lustre_capa *capa);
 
 void ll_capa_open(struct inode *inode);
 void ll_capa_close(struct inode *inode);
@@ -1182,14 +1107,12 @@ extern struct lu_device_type vvp_device_type;
  */
 int cl_sb_init(struct super_block *sb);
 int cl_sb_fini(struct super_block *sb);
-enum cl_lock_mode  vvp_mode_from_vma(struct vm_area_struct *vma);
 void ll_io_init(struct cl_io *io, const struct file *file, int write);
 
 void ras_update(struct ll_sb_info *sbi, struct inode *inode,
 		struct ll_readahead_state *ras, unsigned long index,
 		unsigned hit);
 void ll_ra_count_put(struct ll_sb_info *sbi, unsigned long len);
-int ll_is_file_contended(struct file *file);
 void ll_ra_stats_inc(struct address_space *mapping, enum ra_stat which);
 
 /* llite/llite_rmtacl.c */
@@ -1262,7 +1185,6 @@ struct ll_statahead_info {
 	unsigned int	    sai_skip_hidden;/* skipped hidden dentry count */
 	unsigned int	    sai_ls_all:1,   /* "ls -al", do stat-ahead for
 						 * hidden entries */
-				sai_in_readpage:1,/* statahead is in readdir()*/
 				sai_agl_valid:1;/* AGL is valid for the dir */
 	wait_queue_head_t	     sai_waitq;      /* stat-ahead wait queue */
 	struct ptlrpc_thread    sai_thread;     /* stat-ahead thread */
@@ -1387,9 +1309,6 @@ typedef enum llioc_iter (*llioc_callback_t)(struct inode *inode,
 		struct file *file, unsigned int cmd, unsigned long arg,
 		void *magic, int *rcp);
 
-enum llioc_iter ll_iocontrol_call(struct inode *inode, struct file *file,
-		unsigned int cmd, unsigned long arg, int *rcp);
-
 /* export functions */
 /* Register ioctl block dynamatically for a regular file.
  *
@@ -1431,7 +1350,7 @@ static inline void cl_isize_unlock(struct inode *inode)
 
 static inline void cl_isize_write_nolock(struct inode *inode, loff_t kms)
 {
-	LASSERT(down_trylock(&ll_i2info(inode)->lli_size_sem) != 0);
+	LASSERT(mutex_is_locked(&ll_i2info(inode)->lli_size_mutex));
 	i_size_write(inode, kms);
 }
 
diff --git a/drivers/staging/lustre/lustre/llite/llite_lib.c b/drivers/staging/lustre/lustre/llite/llite_lib.c
index 7c4fd97a7fa..deca27ea33f 100644
--- a/drivers/staging/lustre/lustre/llite/llite_lib.c
+++ b/drivers/staging/lustre/lustre/llite/llite_lib.c
@@ -58,14 +58,8 @@
 struct kmem_cache *ll_file_data_slab;
 struct proc_dir_entry *proc_lustre_fs_root;
 
-LIST_HEAD(ll_super_blocks);
-DEFINE_SPINLOCK(ll_sb_lock);
-
-#ifndef MS_HAS_NEW_AOPS
-extern struct address_space_operations ll_aops;
-#else
-extern struct address_space_operations_ext ll_aops;
-#endif
+static LIST_HEAD(ll_super_blocks);
+static DEFINE_SPINLOCK(ll_sb_lock);
 
 #ifndef log2
 #define log2(n) ffz(~(n))
@@ -143,7 +137,7 @@ static struct ll_sb_info *ll_init_sbi(void)
 	return sbi;
 }
 
-void ll_free_sbi(struct super_block *sb)
+static void ll_free_sbi(struct super_block *sb)
 {
 	struct ll_sb_info *sbi = ll_s2sbi(sb);
 
@@ -597,8 +591,7 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 
 	return err;
 out_root:
-	if (root)
-		iput(root);
+	iput(root);
 out_lock_cn_cb:
 	obd_fid_fini(sbi->ll_dt_exp->exp_obd);
 out_dt:
@@ -634,7 +627,46 @@ int ll_get_max_mdsize(struct ll_sb_info *sbi, int *lmmsize)
 	return rc;
 }
 
-void ll_dump_inode(struct inode *inode)
+int ll_get_default_mdsize(struct ll_sb_info *sbi, int *lmmsize)
+{
+	int size, rc;
+
+	size = sizeof(int);
+	rc = obd_get_info(NULL, sbi->ll_md_exp, sizeof(KEY_DEFAULT_EASIZE),
+			 KEY_DEFAULT_EASIZE, &size, lmmsize, NULL);
+	if (rc)
+		CERROR("Get default mdsize error rc %d\n", rc);
+
+	return rc;
+}
+
+int ll_get_max_cookiesize(struct ll_sb_info *sbi, int *lmmsize)
+{
+	int size, rc;
+
+	size = sizeof(int);
+	rc = obd_get_info(NULL, sbi->ll_md_exp, sizeof(KEY_MAX_COOKIESIZE),
+			  KEY_MAX_COOKIESIZE, &size, lmmsize, NULL);
+	if (rc)
+		CERROR("Get max cookiesize error rc %d\n", rc);
+
+	return rc;
+}
+
+int ll_get_default_cookiesize(struct ll_sb_info *sbi, int *lmmsize)
+{
+	int size, rc;
+
+	size = sizeof(int);
+	rc = obd_get_info(NULL, sbi->ll_md_exp, sizeof(KEY_DEFAULT_COOKIESIZE),
+			  KEY_DEFAULT_COOKIESIZE, &size, lmmsize, NULL);
+	if (rc)
+		CERROR("Get default cookiesize error rc %d\n", rc);
+
+	return rc;
+}
+
+static void ll_dump_inode(struct inode *inode)
 {
 	struct ll_d_hlist_node *tmp;
 	int dentry_count = 0;
@@ -677,7 +709,7 @@ void lustre_dump_dentry(struct dentry *dentry, int recur)
 	}
 }
 
-void client_common_put_super(struct super_block *sb)
+static void client_common_put_super(struct super_block *sb)
 {
 	struct ll_sb_info *sbi = ll_s2sbi(sb);
 
@@ -726,30 +758,6 @@ void ll_kill_super(struct super_block *sb)
 	}
 }
 
-char *ll_read_opt(const char *opt, char *data)
-{
-	char *value;
-	char *retval;
-
-	CDEBUG(D_SUPER, "option: %s, data %s\n", opt, data);
-	if (strncmp(opt, data, strlen(opt)))
-		return NULL;
-	value = strchr(data, '=');
-	if (value == NULL)
-		return NULL;
-
-	value++;
-	OBD_ALLOC(retval, strlen(value) + 1);
-	if (!retval) {
-		CERROR("out of memory!\n");
-		return NULL;
-	}
-
-	memcpy(retval, value, strlen(value)+1);
-	CDEBUG(D_SUPER, "Assigned option: %s, value %s\n", opt, retval);
-	return retval;
-}
-
 static inline int ll_set_opt(const char *opt, char *data, int fl)
 {
 	if (strncmp(opt, data, strlen(opt)) != 0)
@@ -927,7 +935,8 @@ void ll_lli_init(struct ll_inode_info *lli)
 	mutex_init(&lli->lli_och_mutex);
 	spin_lock_init(&lli->lli_agl_lock);
 	lli->lli_has_smd = false;
-	lli->lli_layout_gen = LL_LAYOUT_GEN_NONE;
+	spin_lock_init(&lli->lli_layout_lock);
+	ll_layout_version_set(lli, LL_LAYOUT_GEN_NONE);
 	lli->lli_clob = NULL;
 
 	init_rwsem(&lli->lli_xattrs_list_rwsem);
@@ -938,12 +947,10 @@ void ll_lli_init(struct ll_inode_info *lli)
 		mutex_init(&lli->lli_readdir_mutex);
 		lli->lli_opendir_key = NULL;
 		lli->lli_sai = NULL;
-		lli->lli_def_acl = NULL;
 		spin_lock_init(&lli->lli_sa_lock);
 		lli->lli_opendir_pid = 0;
 	} else {
-		sema_init(&lli->lli_size_sem, 1);
-		lli->lli_size_sem_owner = NULL;
+		mutex_init(&lli->lli_size_mutex);
 		lli->lli_symlink_name = NULL;
 		init_rwsem(&lli->lli_trunc_sem);
 		mutex_init(&lli->lli_write_mutex);
@@ -952,7 +959,6 @@ void ll_lli_init(struct ll_inode_info *lli)
 		INIT_LIST_HEAD(&lli->lli_agl_list);
 		lli->lli_agl_index = 0;
 		lli->lli_async_rc = 0;
-		lli->lli_volatile = false;
 	}
 	mutex_init(&lli->lli_layout_mutex);
 }
@@ -1151,28 +1157,6 @@ struct inode *ll_inode_from_resource_lock(struct ldlm_lock *lock)
 	return inode;
 }
 
-struct inode *ll_inode_from_lock(struct ldlm_lock *lock)
-{
-	struct inode *inode = NULL;
-	/* NOTE: we depend on atomic igrab() -bzzz */
-	lock_res_and_lock(lock);
-	if (lock->l_ast_data) {
-		struct ll_inode_info *lli = ll_i2info(lock->l_ast_data);
-		if (lli->lli_inode_magic == LLI_INODE_MAGIC) {
-			inode = igrab(lock->l_ast_data);
-		} else {
-			inode = lock->l_ast_data;
-			LDLM_DEBUG_LIMIT(inode->i_state & I_FREEING ?  D_INFO :
-					 D_WARNING, lock, "l_ast_data %p is "
-					 "bogus: magic %08x", lock->l_ast_data,
-					 lli->lli_inode_magic);
-			inode = NULL;
-		}
-	}
-	unlock_res_and_lock(lock);
-	return inode;
-}
-
 void ll_clear_inode(struct inode *inode)
 {
 	struct ll_inode_info *lli = ll_i2info(inode);
@@ -1449,7 +1433,6 @@ int ll_setattr_raw(struct dentry *dentry, struct iattr *attr, bool hsm_import)
 		if (attr->ia_valid & ATTR_SIZE)
 			inode_dio_write_done(inode);
 		mutex_unlock(&inode->i_mutex);
-		down_write(&lli->lli_trunc_sem);
 	}
 
 	memcpy(&op_data->op_attr, attr, sizeof(*attr));
@@ -1513,7 +1496,11 @@ int ll_setattr_raw(struct dentry *dentry, struct iattr *attr, bool hsm_import)
 		 * excessive to send mtime/atime updates to OSTs when not
 		 * setting times to past, but it is necessary due to possible
 		 * time de-synchronization between MDT inode and OST objects */
+		if (attr->ia_valid & ATTR_SIZE)
+			down_write(&lli->lli_trunc_sem);
 		rc = ll_setattr_ost(inode, attr);
+		if (attr->ia_valid & ATTR_SIZE)
+			up_write(&lli->lli_trunc_sem);
 out:
 	if (op_data) {
 		if (op_data->op_ioepoch) {
@@ -1524,7 +1511,6 @@ out:
 		ll_finish_md_op_data(op_data);
 	}
 	if (!S_ISDIR(inode->i_mode)) {
-		up_write(&lli->lli_trunc_sem);
 		mutex_lock(&inode->i_mutex);
 		if ((attr->ia_valid & ATTR_SIZE) && !hsm_import)
 			inode_dio_wait(inode);
@@ -1658,10 +1644,7 @@ void ll_inode_size_lock(struct inode *inode)
 	LASSERT(!S_ISDIR(inode->i_mode));
 
 	lli = ll_i2info(inode);
-	LASSERT(lli->lli_size_sem_owner != current);
-	down(&lli->lli_size_sem);
-	LASSERT(lli->lli_size_sem_owner == NULL);
-	lli->lli_size_sem_owner = current;
+	mutex_lock(&lli->lli_size_mutex);
 }
 
 void ll_inode_size_unlock(struct inode *inode)
@@ -1669,9 +1652,7 @@ void ll_inode_size_unlock(struct inode *inode)
 	struct ll_inode_info *lli;
 
 	lli = ll_i2info(inode);
-	LASSERT(lli->lli_size_sem_owner == current);
-	lli->lli_size_sem_owner = NULL;
-	up(&lli->lli_size_sem);
+	mutex_unlock(&lli->lli_size_mutex);
 }
 
 void ll_update_inode(struct inode *inode, struct lustre_md *md)
@@ -2420,11 +2401,12 @@ void ll_dirty_page_discard_warn(struct page *page, int ioret)
 			path = ll_d_path(dentry, buf, PAGE_SIZE);
 	}
 
-	CWARN("%s: dirty page discard: %s/fid: "DFID"/%s may get corrupted "
-	      "(rc %d)\n", ll_get_fsname(page->mapping->host->i_sb, NULL, 0),
-	      s2lsi(page->mapping->host->i_sb)->lsi_lmd->lmd_dev,
-	      PFID(&obj->cob_header.coh_lu.loh_fid),
-	      (path && !IS_ERR(path)) ? path : "", ioret);
+	CDEBUG(D_WARNING,
+	       "%s: dirty page discard: %s/fid: "DFID"/%s may get corrupted "
+	       "(rc %d)\n", ll_get_fsname(page->mapping->host->i_sb, NULL, 0),
+	       s2lsi(page->mapping->host->i_sb)->lsi_lmd->lmd_dev,
+	       PFID(&obj->cob_header.coh_lu.loh_fid),
+	       (path && !IS_ERR(path)) ? path : "", ioret);
 
 	if (dentry != NULL)
 		dput(dentry);
diff --git a/drivers/staging/lustre/lustre/llite/llite_mmap.c b/drivers/staging/lustre/lustre/llite/llite_mmap.c
index 90b2c0d275f..426c7396166 100644
--- a/drivers/staging/lustre/lustre/llite/llite_mmap.c
+++ b/drivers/staging/lustre/lustre/llite/llite_mmap.c
@@ -51,10 +51,7 @@
 #include "llite_internal.h"
 #include <linux/lustre_compat25.h>
 
-struct page *ll_nopage(struct vm_area_struct *vma, unsigned long address,
-		       int *type);
-
-static struct vm_operations_struct ll_file_vm_ops;
+static const struct vm_operations_struct ll_file_vm_ops;
 
 void policy_from_vma(ldlm_policy_data_t *policy,
 			    struct vm_area_struct *vma, unsigned long addr,
@@ -97,10 +94,10 @@ struct vm_area_struct *our_vma(struct mm_struct *mm, unsigned long addr,
  * \retval EINVAL if env can't allocated
  * \return other error codes from cl_io_init.
  */
-struct cl_io *ll_fault_io_init(struct vm_area_struct *vma,
-			       struct lu_env **env_ret,
-			       struct cl_env_nest *nest,
-			       pgoff_t index, unsigned long *ra_flags)
+static struct cl_io *
+ll_fault_io_init(struct vm_area_struct *vma, struct lu_env **env_ret,
+		 struct cl_env_nest *nest, pgoff_t index,
+		 unsigned long *ra_flags)
 {
 	struct file	       *file = vma->vm_file;
 	struct inode	       *inode = file->f_dentry->d_inode;
@@ -446,14 +443,6 @@ static void ll_vm_close(struct vm_area_struct *vma)
 	LASSERT(atomic_read(&vob->cob_mmap_cnt) >= 0);
 }
 
-
-/* return the user space pointer that maps to a file offset via a vma */
-static inline unsigned long file_to_user(struct vm_area_struct *vma, __u64 byte)
-{
-	return vma->vm_start + (byte - ((__u64)vma->vm_pgoff << PAGE_CACHE_SHIFT));
-
-}
-
 /* XXX put nice comment here.  talk about __free_pte -> dirty pages and
  * nopage's reference passing to the pte */
 int ll_teardown_mmaps(struct address_space *mapping, __u64 first, __u64 last)
@@ -470,7 +459,7 @@ int ll_teardown_mmaps(struct address_space *mapping, __u64 first, __u64 last)
 	return rc;
 }
 
-static struct vm_operations_struct ll_file_vm_ops = {
+static const struct vm_operations_struct ll_file_vm_ops = {
 	.fault			= ll_fault,
 	.page_mkwrite		= ll_page_mkwrite,
 	.open			= ll_vm_open,
diff --git a/drivers/staging/lustre/lustre/llite/llite_nfs.c b/drivers/staging/lustre/lustre/llite/llite_nfs.c
index 3580069789f..a614b913ddc 100644
--- a/drivers/staging/lustre/lustre/llite/llite_nfs.c
+++ b/drivers/staging/lustre/lustre/llite/llite_nfs.c
@@ -51,7 +51,9 @@ __u32 get_uuid2int(const char *name, int len)
 	__u32 key0 = 0x12a3fe2d, key1 = 0x37abe8f9;
 	while (len--) {
 		__u32 key = key1 + (key0 ^ (*name++ * 7152373));
-		if (key & 0x80000000) key -= 0x7fffffff;
+
+		if (key & 0x80000000)
+			key -= 0x7fffffff;
 		key1 = key0;
 		key0 = key;
 	}
@@ -98,7 +100,7 @@ struct inode *search_inode_for_lustre(struct super_block *sb,
 	if (inode)
 		return inode;
 
-	rc = ll_get_max_mdsize(sbi, &eadatalen);
+	rc = ll_get_default_mdsize(sbi, &eadatalen);
 	if (rc)
 		return ERR_PTR(rc);
 
@@ -290,7 +292,7 @@ static struct dentry *ll_get_parent(struct dentry *dchild)
 	CDEBUG(D_INFO, "getting parent for (%lu,"DFID")\n",
 			dir->i_ino, PFID(ll_inode2fid(dir)));
 
-	rc = ll_get_max_mdsize(sbi, &lmmsize);
+	rc = ll_get_default_mdsize(sbi, &lmmsize);
 	if (rc != 0)
 		return ERR_PTR(rc);
 
diff --git a/drivers/staging/lustre/lustre/llite/lloop.c b/drivers/staging/lustre/lustre/llite/lloop.c
index f78eda235c7..0ff8c3362a8 100644
--- a/drivers/staging/lustre/lustre/llite/lloop.c
+++ b/drivers/staging/lustre/lustre/llite/lloop.c
@@ -122,9 +122,6 @@ struct lloop_device {
 	loff_t	       lo_offset;
 	loff_t	       lo_sizelimit;
 	int		  lo_flags;
-	int		(*ioctl)(struct lloop_device *, int cmd,
-				    unsigned long arg);
-
 	struct file	 *lo_backing_file;
 	struct block_device *lo_device;
 	unsigned	     lo_blocksize;
@@ -407,7 +404,7 @@ static int loop_thread(void *data)
 	int refcheck;
 	int ret = 0;
 
-	set_user_nice(current, -20);
+	set_user_nice(current, MIN_NICE);
 
 	lo->lo_state = LLOOP_BOUND;
 
@@ -509,7 +506,6 @@ static int loop_set_fd(struct lloop_device *lo, struct file *unused,
 	lo->lo_device = bdev;
 	lo->lo_flags = lo_flags;
 	lo->lo_backing_file = file;
-	lo->ioctl = NULL;
 	lo->lo_sizelimit = 0;
 	lo->old_gfp_mask = mapping_gfp_mask(mapping);
 	mapping_set_gfp_mask(mapping, lo->old_gfp_mask & ~(__GFP_IO|__GFP_FS));
@@ -568,7 +564,6 @@ static int loop_clr_fd(struct lloop_device *lo, struct block_device *bdev,
 
 	down(&lo->lo_sem);
 	lo->lo_backing_file = NULL;
-	lo->ioctl = NULL;
 	lo->lo_device = NULL;
 	lo->lo_offset = 0;
 	lo->lo_sizelimit = 0;
@@ -624,7 +619,10 @@ static int lo_ioctl(struct block_device *bdev, fmode_t mode,
 	case LL_IOC_LLOOP_INFO: {
 		struct lu_fid fid;
 
-		LASSERT(lo->lo_backing_file != NULL);
+		if (lo->lo_backing_file == NULL) {
+			err = -ENOENT;
+			break;
+		}
 		if (inode == NULL)
 			inode = lo->lo_backing_file->f_dentry->d_inode;
 		if (lo->lo_state == LLOOP_BOUND)
diff --git a/drivers/staging/lustre/lustre/llite/lproc_llite.c b/drivers/staging/lustre/lustre/llite/lproc_llite.c
index a9a104a6a4e..77ee9e58cf8 100644
--- a/drivers/staging/lustre/lustre/llite/lproc_llite.c
+++ b/drivers/staging/lustre/lustre/llite/lproc_llite.c
@@ -41,12 +41,12 @@
 #include <obd_support.h>
 
 #include "llite_internal.h"
+#include "vvp_internal.h"
 
 /* /proc/lustre/llite mount point registration */
-extern struct file_operations vvp_dump_pgcache_file_ops;
-struct file_operations ll_rw_extents_stats_fops;
-struct file_operations ll_rw_extents_stats_pp_fops;
-struct file_operations ll_rw_offset_stats_fops;
+static struct file_operations ll_rw_extents_stats_fops;
+static struct file_operations ll_rw_extents_stats_pp_fops;
+static struct file_operations ll_rw_offset_stats_fops;
 
 static int ll_blksize_seq_show(struct seq_file *m, void *v)
 {
@@ -367,8 +367,9 @@ static int ll_max_cached_mb_seq_show(struct seq_file *m, void *v)
 			cache->ccc_lru_shrinkers);
 }
 
-static ssize_t ll_max_cached_mb_seq_write(struct file *file, const char *buffer,
-				      size_t count, loff_t *off)
+static ssize_t ll_max_cached_mb_seq_write(struct file *file,
+					  const char __user *buffer,
+					  size_t count, loff_t *off)
 {
 	struct super_block *sb = ((struct seq_file *)file->private_data)->private;
 	struct ll_sb_info *sbi = ll_s2sbi(sb);
@@ -376,9 +377,18 @@ static ssize_t ll_max_cached_mb_seq_write(struct file *file, const char *buffer,
 	int mult, rc, pages_number;
 	int diff = 0;
 	int nrpages = 0;
+	char kernbuf[128];
+
+	if (count >= sizeof(kernbuf))
+		return -EINVAL;
+
+	if (copy_from_user(kernbuf, buffer, count))
+		return -EFAULT;
+	kernbuf[count] = 0;
 
 	mult = 1 << (20 - PAGE_CACHE_SHIFT);
-	buffer = lprocfs_find_named_value(buffer, "max_cached_mb:", &count);
+	buffer += lprocfs_find_named_value(kernbuf, "max_cached_mb:", &count) -
+		  kernbuf;
 	rc = lprocfs_write_frac_helper(buffer, count, &pages_number, mult);
 	if (rc)
 		return rc;
@@ -681,7 +691,7 @@ static ssize_t ll_lazystatfs_seq_write(struct file *file, const char *buffer,
 }
 LPROC_SEQ_FOPS(ll_lazystatfs);
 
-static int ll_maxea_size_seq_show(struct seq_file *m, void *v)
+static int ll_max_easize_seq_show(struct seq_file *m, void *v)
 {
 	struct super_block *sb = m->private;
 	struct ll_sb_info *sbi = ll_s2sbi(sb);
@@ -694,7 +704,52 @@ static int ll_maxea_size_seq_show(struct seq_file *m, void *v)
 
 	return seq_printf(m, "%u\n", ealen);
 }
-LPROC_SEQ_FOPS_RO(ll_maxea_size);
+LPROC_SEQ_FOPS_RO(ll_max_easize);
+
+static int ll_defult_easize_seq_show(struct seq_file *m, void *v)
+{
+	struct super_block *sb = m->private;
+	struct ll_sb_info *sbi = ll_s2sbi(sb);
+	unsigned int ealen;
+	int rc;
+
+	rc = ll_get_default_mdsize(sbi, &ealen);
+	if (rc)
+		return rc;
+
+	return seq_printf(m, "%u\n", ealen);
+}
+LPROC_SEQ_FOPS_RO(ll_defult_easize);
+
+static int ll_max_cookiesize_seq_show(struct seq_file *m, void *v)
+{
+	struct super_block *sb = m->private;
+	struct ll_sb_info *sbi = ll_s2sbi(sb);
+	unsigned int cookielen;
+	int rc;
+
+	rc = ll_get_max_cookiesize(sbi, &cookielen);
+	if (rc)
+		return rc;
+
+	return seq_printf(m, "%u\n", cookielen);
+}
+LPROC_SEQ_FOPS_RO(ll_max_cookiesize);
+
+static int ll_defult_cookiesize_seq_show(struct seq_file *m, void *v)
+{
+	struct super_block *sb = m->private;
+	struct ll_sb_info *sbi = ll_s2sbi(sb);
+	unsigned int cookielen;
+	int rc;
+
+	rc = ll_get_default_cookiesize(sbi, &cookielen);
+	if (rc)
+		return rc;
+
+	return seq_printf(m, "%u\n", cookielen);
+}
+LPROC_SEQ_FOPS_RO(ll_defult_cookiesize);
 
 static int ll_sbi_flags_seq_show(struct seq_file *m, void *v)
 {
@@ -781,7 +836,10 @@ static struct lprocfs_vars lprocfs_llite_obd_vars[] = {
 	{ "statahead_agl",    &ll_statahead_agl_fops, 0 },
 	{ "statahead_stats",  &ll_statahead_stats_fops, 0, 0 },
 	{ "lazystatfs",       &ll_lazystatfs_fops, 0 },
-	{ "max_easize",       &ll_maxea_size_fops, 0, 0 },
+	{ "max_easize",       &ll_max_easize_fops, 0, 0 },
+	{ "default_easize",   &ll_defult_easize_fops, 0, 0 },
+	{ "max_cookiesize",   &ll_max_cookiesize_fops, 0, 0 },
+	{ "default_cookiesize", &ll_defult_cookiesize_fops, 0, 0 },
 	{ "sbi_flags",	      &ll_sbi_flags_fops, 0, 0 },
 	{ "xattr_cache",      &ll_xattr_cache_fops, 0, 0 },
 	{ 0 }
@@ -789,7 +847,7 @@ static struct lprocfs_vars lprocfs_llite_obd_vars[] = {
 
 #define MAX_STRING_SIZE 128
 
-struct llite_file_opcode {
+static const struct llite_file_opcode {
 	__u32       opcode;
 	__u32       type;
 	const char *opname;
@@ -1115,7 +1173,8 @@ static int ll_rw_extents_stats_pp_seq_show(struct seq_file *seq, void *v)
 }
 
 static ssize_t ll_rw_extents_stats_pp_seq_write(struct file *file,
-						const char *buf, size_t len,
+						const char __user *buf,
+						size_t len,
 						loff_t *off)
 {
 	struct seq_file *seq = file->private_data;
@@ -1124,10 +1183,24 @@ static ssize_t ll_rw_extents_stats_pp_seq_write(struct file *file,
 	int i;
 	int value = 1, rc = 0;
 
+	if (len == 0)
+		return -EINVAL;
+
 	rc = lprocfs_write_helper(buf, len, &value);
-	if (rc < 0 && (strcmp(buf, "disabled") == 0 ||
-		       strcmp(buf, "Disabled") == 0))
-		value = 0;
+	if (rc < 0 && len < 16) {
+		char kernbuf[16];
+
+		if (copy_from_user(kernbuf, buf, len))
+			return -EFAULT;
+		kernbuf[len] = 0;
+
+		if (kernbuf[len - 1] == '\n')
+			kernbuf[len - 1] = 0;
+
+		if (strcmp(kernbuf, "disabled") == 0 ||
+		    strcmp(kernbuf, "Disabled") == 0)
+			value = 0;
+	}
 
 	if (value == 0)
 		sbi->ll_rw_stats_on = 0;
@@ -1174,8 +1247,9 @@ static int ll_rw_extents_stats_seq_show(struct seq_file *seq, void *v)
 	return 0;
 }
 
-static ssize_t ll_rw_extents_stats_seq_write(struct file *file, const char *buf,
-					size_t len, loff_t *off)
+static ssize_t ll_rw_extents_stats_seq_write(struct file *file,
+					     const char __user *buf,
+					     size_t len, loff_t *off)
 {
 	struct seq_file *seq = file->private_data;
 	struct ll_sb_info *sbi = seq->private;
@@ -1183,15 +1257,30 @@ static ssize_t ll_rw_extents_stats_seq_write(struct file *file, const char *buf,
 	int i;
 	int value = 1, rc = 0;
 
+	if (len == 0)
+		return -EINVAL;
+
 	rc = lprocfs_write_helper(buf, len, &value);
-	if (rc < 0 && (strcmp(buf, "disabled") == 0 ||
-		       strcmp(buf, "Disabled") == 0))
-		value = 0;
+	if (rc < 0 && len < 16) {
+		char kernbuf[16];
+
+		if (copy_from_user(kernbuf, buf, len))
+			return -EFAULT;
+		kernbuf[len] = 0;
+
+		if (kernbuf[len - 1] == '\n')
+			kernbuf[len - 1] = 0;
+
+		if (strcmp(kernbuf, "disabled") == 0 ||
+		    strcmp(kernbuf, "Disabled") == 0)
+			value = 0;
+	}
 
 	if (value == 0)
 		sbi->ll_rw_stats_on = 0;
 	else
 		sbi->ll_rw_stats_on = 1;
+
 	spin_lock(&sbi->ll_pp_extent_lock);
 	for (i = 0; i <= LL_PROCESS_HIST_MAX; i++) {
 		io_extents->pp_extents[i].pid = 0;
@@ -1202,7 +1291,6 @@ static ssize_t ll_rw_extents_stats_seq_write(struct file *file, const char *buf,
 
 	return len;
 }
-
 LPROC_SEQ_FOPS(ll_rw_extents_stats);
 
 void ll_rw_stats_tally(struct ll_sb_info *sbi, pid_t pid,
@@ -1362,8 +1450,9 @@ static int ll_rw_offset_stats_seq_show(struct seq_file *seq, void *v)
 	return 0;
 }
 
-static ssize_t ll_rw_offset_stats_seq_write(struct file *file, const char *buf,
-				       size_t len, loff_t *off)
+static ssize_t ll_rw_offset_stats_seq_write(struct file *file,
+					    const char __user *buf,
+					    size_t len, loff_t *off)
 {
 	struct seq_file *seq = file->private_data;
 	struct ll_sb_info *sbi = seq->private;
@@ -1371,11 +1460,25 @@ static ssize_t ll_rw_offset_stats_seq_write(struct file *file, const char *buf,
 	struct ll_rw_process_info *offset_info = sbi->ll_rw_offset_info;
 	int value = 1, rc = 0;
 
+	if (len == 0)
+		return -EINVAL;
+
 	rc = lprocfs_write_helper(buf, len, &value);
 
-	if (rc < 0 && (strcmp(buf, "disabled") == 0 ||
-			   strcmp(buf, "Disabled") == 0))
-		value = 0;
+	if (rc < 0 && len < 16) {
+		char kernbuf[16];
+
+		if (copy_from_user(kernbuf, buf, len))
+			return -EFAULT;
+		kernbuf[len] = 0;
+
+		if (kernbuf[len - 1] == '\n')
+			kernbuf[len - 1] = 0;
+
+		if (strcmp(kernbuf, "disabled") == 0 ||
+		    strcmp(kernbuf, "Disabled") == 0)
+			value = 0;
+	}
 
 	if (value == 0)
 		sbi->ll_rw_stats_on = 0;
diff --git a/drivers/staging/lustre/lustre/llite/namei.c b/drivers/staging/lustre/lustre/llite/namei.c
index 25a6ea580f0..dfa1e745dfd 100644
--- a/drivers/staging/lustre/lustre/llite/namei.c
+++ b/drivers/staging/lustre/lustre/llite/namei.c
@@ -75,14 +75,6 @@ static int ll_d_mountpoint(struct dentry *dparent, struct dentry *dchild,
 	return mounted;
 }
 
-int ll_unlock(__u32 mode, struct lustre_handle *lockh)
-{
-	ldlm_lock_decref(lockh, mode);
-
-	return 0;
-}
-
-
 /* called from iget5_locked->find_inode() under inode_hash_lock spinlock */
 static int ll_test_inode(struct inode *inode, void *opaque)
 {
@@ -433,12 +425,10 @@ struct dentry *ll_splice_alias(struct inode *inode, struct dentry *de)
 	return de;
 }
 
-int ll_lookup_it_finish(struct ptlrpc_request *request,
-			struct lookup_intent *it, void *data)
+static int ll_lookup_it_finish(struct ptlrpc_request *request,
+			       struct lookup_intent *it,
+			       struct inode *parent, struct dentry **de)
 {
-	struct it_cb_data *icbd = data;
-	struct dentry **de = icbd->icbd_childp;
-	struct inode *parent = icbd->icbd_parent;
 	struct inode *inode = NULL;
 	__u64 bits = 0;
 	int rc;
@@ -513,7 +503,6 @@ static struct dentry *ll_lookup_it(struct inode *parent, struct dentry *dentry,
 	struct dentry *save = dentry, *retval;
 	struct ptlrpc_request *req = NULL;
 	struct md_op_data *op_data;
-	struct it_cb_data icbd;
 	__u32 opc;
 	int rc;
 
@@ -527,7 +516,8 @@ static struct dentry *ll_lookup_it(struct inode *parent, struct dentry *dentry,
 	if (d_mountpoint(dentry))
 		CERROR("Tell Peter, lookup on mtpt, it %s\n", LL_IT2STR(it));
 
-	ll_frob_intent(&it, &lookup_it);
+	if (it == NULL || it->it_op == IT_GETXATTR)
+		it = &lookup_it;
 
 	if (it->it_op == IT_GETATTR) {
 		rc = ll_statahead_enter(parent, &dentry, 0);
@@ -538,9 +528,6 @@ static struct dentry *ll_lookup_it(struct inode *parent, struct dentry *dentry,
 		}
 	}
 
-	icbd.icbd_childp = &dentry;
-	icbd.icbd_parent = parent;
-
 	if (it->it_op & IT_CREAT)
 		opc = LUSTRE_OPC_CREATE;
 	else
@@ -562,7 +549,7 @@ static struct dentry *ll_lookup_it(struct inode *parent, struct dentry *dentry,
 	if (rc < 0)
 		GOTO(out, retval = ERR_PTR(rc));
 
-	rc = ll_lookup_it_finish(req, it, &icbd);
+	rc = ll_lookup_it_finish(req, it, parent, &dentry);
 	if (rc != 0) {
 		ll_intent_release(it);
 		GOTO(out, retval = ERR_PTR(rc));
@@ -697,10 +684,7 @@ out_release:
 
 
 /* We depend on "mode" being set with the proper file type/umask by now */
-static struct inode *ll_create_node(struct inode *dir, const char *name,
-				    int namelen, const void *data, int datalen,
-				    int mode, __u64 extra,
-				    struct lookup_intent *it)
+static struct inode *ll_create_node(struct inode *dir, struct lookup_intent *it)
 {
 	struct inode *inode = NULL;
 	struct ptlrpc_request *request = NULL;
@@ -757,14 +741,10 @@ static int ll_create_it(struct inode *dir, struct dentry *dentry, int mode,
 	if (rc)
 		return rc;
 
-	inode = ll_create_node(dir, dentry->d_name.name, dentry->d_name.len,
-			       NULL, 0, mode, 0, it);
+	inode = ll_create_node(dir, it);
 	if (IS_ERR(inode))
 		return PTR_ERR(inode);
 
-	if (filename_is_volatile(dentry->d_name.name, dentry->d_name.len, NULL))
-		ll_i2info(inode)->lli_volatile = true;
-
 	d_instantiate(dentry, inode);
 	return 0;
 }
@@ -1238,7 +1218,7 @@ static int ll_rename(struct inode *old_dir, struct dentry *old_dentry,
 	return err;
 }
 
-struct inode_operations ll_dir_inode_operations = {
+const struct inode_operations ll_dir_inode_operations = {
 	.mknod	      = ll_mknod,
 	.atomic_open	    = ll_atomic_open,
 	.lookup	     = ll_lookup_nd,
@@ -1260,7 +1240,7 @@ struct inode_operations ll_dir_inode_operations = {
 	.get_acl	    = ll_get_acl,
 };
 
-struct inode_operations ll_special_inode_operations = {
+const struct inode_operations ll_special_inode_operations = {
 	.setattr	= ll_setattr,
 	.getattr	= ll_getattr,
 	.permission     = ll_inode_permission,
diff --git a/drivers/staging/lustre/lustre/llite/remote_perm.c b/drivers/staging/lustre/lustre/llite/remote_perm.c
index dedd56ae1e6..a8b1117b8f6 100644
--- a/drivers/staging/lustre/lustre/llite/remote_perm.c
+++ b/drivers/staging/lustre/lustre/llite/remote_perm.c
@@ -77,7 +77,7 @@ static inline void free_ll_remote_perm(struct ll_remote_perm *lrp)
 	OBD_SLAB_FREE(lrp, ll_remote_perm_cachep, sizeof(*lrp));
 }
 
-struct hlist_head *alloc_rmtperm_hash(void)
+static struct hlist_head *alloc_rmtperm_hash(void)
 {
 	struct hlist_head *hash;
 	int i;
diff --git a/drivers/staging/lustre/lustre/llite/rw.c b/drivers/staging/lustre/lustre/llite/rw.c
index 416f7a094a6..f0122c568a0 100644
--- a/drivers/staging/lustre/lustre/llite/rw.c
+++ b/drivers/staging/lustre/lustre/llite/rw.c
@@ -77,12 +77,6 @@ static void ll_cl_fini(struct ll_cl_context *lcc)
 		cl_page_put(env, page);
 	}
 
-	if (io && lcc->lcc_created) {
-		cl_io_end(env, io);
-		cl_io_unlock(env, io);
-		cl_io_iter_fini(env, io);
-		cl_io_fini(env, io);
-	}
 	cl_env_put(env, &lcc->lcc_refcheck);
 }
 
@@ -167,7 +161,6 @@ static struct ll_cl_context *ll_cl_init(struct file *file,
 			}
 		} else
 			result = io->ci_result;
-		lcc->lcc_created = 1;
 	}
 
 	lcc->lcc_io = io;
diff --git a/drivers/staging/lustre/lustre/llite/rw26.c b/drivers/staging/lustre/lustre/llite/rw26.c
index 7e3e0967993..55ca8d3c3e4 100644
--- a/drivers/staging/lustre/lustre/llite/rw26.c
+++ b/drivers/staging/lustre/lustre/llite/rw26.c
@@ -385,8 +385,8 @@ static ssize_t ll_direct_IO_26(int rw, struct kiocb *iocb,
 	if ((file_offset & ~CFS_PAGE_MASK) || (count & ~CFS_PAGE_MASK))
 		return -EINVAL;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:inode=%lu/%u(%p), size=%lu (max %lu), "
-	       "offset=%lld=%llx, pages %lu (max %lu)\n",
+	CDEBUG(D_VFSTRACE,
+	       "VFS Op:inode=%lu/%u(%p), size=%zd (max %lu), offset=%lld=%llx, pages %zd (max %lu)\n",
 	       inode->i_ino, inode->i_generation, inode, count, MAX_DIO_SIZE,
 	       file_offset, file_offset, count >> PAGE_CACHE_SHIFT,
 	       MAX_DIO_SIZE >> PAGE_CACHE_SHIFT);
@@ -529,9 +529,9 @@ static int ll_write_end(struct file *file, struct address_space *mapping,
 }
 
 #ifdef CONFIG_MIGRATION
-int ll_migratepage(struct address_space *mapping,
-		struct page *newpage, struct page *page
-		, enum migrate_mode mode
+static int ll_migratepage(struct address_space *mapping,
+			 struct page *newpage, struct page *page,
+			 enum migrate_mode mode
 		)
 {
 	/* Always fail page migration until we have a proper implementation */
@@ -540,9 +540,8 @@ int ll_migratepage(struct address_space *mapping,
 #endif
 
 #ifndef MS_HAS_NEW_AOPS
-struct address_space_operations ll_aops = {
-	.readpage       = ll_readpage,
-//	.readpages      = ll_readpages,
+const struct address_space_operations ll_aops = {
+	.readpage	= ll_readpage,
 	.direct_IO      = ll_direct_IO_26,
 	.writepage      = ll_writepage,
 	.writepages     = ll_writepages,
@@ -554,10 +553,9 @@ struct address_space_operations ll_aops = {
 #ifdef CONFIG_MIGRATION
 	.migratepage    = ll_migratepage,
 #endif
-	.bmap	   = NULL
 };
 #else
-struct address_space_operations_ext ll_aops = {
+const struct address_space_operations_ext ll_aops = {
 	.orig_aops.readpage       = ll_readpage,
 //	.orig_aops.readpages      = ll_readpages,
 	.orig_aops.direct_IO      = ll_direct_IO_26,
@@ -571,7 +569,6 @@ struct address_space_operations_ext ll_aops = {
 #ifdef CONFIG_MIGRATION
 	.orig_aops.migratepage    = ll_migratepage,
 #endif
-	.orig_aops.bmap	   = NULL,
 	.write_begin    = ll_write_begin,
 	.write_end      = ll_write_end
 };
diff --git a/drivers/staging/lustre/lustre/llite/statahead.c b/drivers/staging/lustre/lustre/llite/statahead.c
index c8624b5a987..1b47774d744 100644
--- a/drivers/staging/lustre/lustre/llite/statahead.c
+++ b/drivers/staging/lustre/lustre/llite/statahead.c
@@ -958,13 +958,18 @@ static int ll_agl_thread(void *arg)
 	struct ptlrpc_thread     *thread = &sai->sai_agl_thread;
 	struct l_wait_info	lwi    = { 0 };
 
-	CDEBUG(D_READA, "agl thread started: [pid %d] [parent %.*s]\n",
-	       current_pid(), parent->d_name.len, parent->d_name.name);
+	thread->t_pid = current_pid();
+	CDEBUG(D_READA, "agl thread started: sai %p, parent %.*s\n",
+	       sai, parent->d_name.len, parent->d_name.name);
 
 	atomic_inc(&sbi->ll_agl_total);
 	spin_lock(&plli->lli_agl_lock);
 	sai->sai_agl_valid = 1;
-	thread_set_flags(thread, SVC_RUNNING);
+	if (thread_is_init(thread))
+		/* If someone else has changed the thread state
+		 * (e.g. already changed to SVC_STOPPING), we can't just
+		 * blindly overwrite that setting. */
+		thread_set_flags(thread, SVC_RUNNING);
 	spin_unlock(&plli->lli_agl_lock);
 	wake_up(&thread->t_ctl_waitq);
 
@@ -1004,8 +1009,8 @@ static int ll_agl_thread(void *arg)
 	spin_unlock(&plli->lli_agl_lock);
 	wake_up(&thread->t_ctl_waitq);
 	ll_sai_put(sai);
-	CDEBUG(D_READA, "agl thread stopped: [pid %d] [parent %.*s]\n",
-	       current_pid(), parent->d_name.len, parent->d_name.name);
+	CDEBUG(D_READA, "agl thread stopped: sai %p, parent %.*s\n",
+	       sai, parent->d_name.len, parent->d_name.name);
 	return 0;
 }
 
@@ -1016,8 +1021,8 @@ static void ll_start_agl(struct dentry *parent, struct ll_statahead_info *sai)
 	struct ll_inode_info  *plli;
 	struct task_struct *task;
 
-	CDEBUG(D_READA, "start agl thread: [pid %d] [parent %.*s]\n",
-	       current_pid(), parent->d_name.len, parent->d_name.name);
+	CDEBUG(D_READA, "start agl thread: sai %p, parent %.*s\n",
+	       sai, parent->d_name.len, parent->d_name.name);
 
 	plli = ll_i2info(parent->d_inode);
 	task = kthread_run(ll_agl_thread, parent,
@@ -1050,15 +1055,20 @@ static int ll_statahead_thread(void *arg)
 	struct ll_dir_chain       chain;
 	struct l_wait_info	lwi    = { 0 };
 
-	CDEBUG(D_READA, "statahead thread started: [pid %d] [parent %.*s]\n",
-	       current_pid(), parent->d_name.len, parent->d_name.name);
+	thread->t_pid = current_pid();
+	CDEBUG(D_READA, "statahead thread starting: sai %p, parent %.*s\n",
+	       sai, parent->d_name.len, parent->d_name.name);
 
 	if (sbi->ll_flags & LL_SBI_AGL_ENABLED)
 		ll_start_agl(parent, sai);
 
 	atomic_inc(&sbi->ll_sa_total);
 	spin_lock(&plli->lli_sa_lock);
-	thread_set_flags(thread, SVC_RUNNING);
+	if (thread_is_init(thread))
+		/* If someone else has changed the thread state
+		 * (e.g. already changed to SVC_STOPPING), we can't just
+		 * blindly overwrite that setting. */
+		thread_set_flags(thread, SVC_RUNNING);
 	spin_unlock(&plli->lli_sa_lock);
 	wake_up(&thread->t_ctl_waitq);
 
@@ -1220,9 +1230,7 @@ do_it:
 			 */
 			ll_release_page(page, le32_to_cpu(dp->ldp_flags) &
 					      LDF_COLLIDE);
-			sai->sai_in_readpage = 1;
 			page = ll_get_dir_page(dir, pos, &chain);
-			sai->sai_in_readpage = 0;
 		} else {
 			LASSERT(le32_to_cpu(dp->ldp_flags) & LDF_COLLIDE);
 			ll_release_page(page, 1);
@@ -1239,8 +1247,8 @@ out:
 		spin_unlock(&plli->lli_agl_lock);
 		wake_up(&agl_thread->t_ctl_waitq);
 
-		CDEBUG(D_READA, "stop agl thread: [pid %d]\n",
-		       current_pid());
+		CDEBUG(D_READA, "stop agl thread: sai %p pid %u\n",
+		       sai, (unsigned int)agl_thread->t_pid);
 		l_wait_event(agl_thread->t_ctl_waitq,
 			     thread_is_stopped(agl_thread),
 			     &lwi);
@@ -1266,8 +1274,8 @@ out:
 	wake_up(&thread->t_ctl_waitq);
 	ll_sai_put(sai);
 	dput(parent);
-	CDEBUG(D_READA, "statahead thread stopped: [pid %d] [parent %.*s]\n",
-	       current_pid(), parent->d_name.len, parent->d_name.name);
+	CDEBUG(D_READA, "statahead thread stopped: sai %p, parent %.*s\n",
+	       sai, parent->d_name.len, parent->d_name.name);
 	return rc;
 }
 
@@ -1298,8 +1306,8 @@ void ll_stop_statahead(struct inode *dir, void *key)
 			spin_unlock(&lli->lli_sa_lock);
 			wake_up(&thread->t_ctl_waitq);
 
-			CDEBUG(D_READA, "stop statahead thread: [pid %d]\n",
-			       current_pid());
+			CDEBUG(D_READA, "stop statahead thread: sai %p pid %u\n",
+			       lli->lli_sai, (unsigned int)thread->t_pid);
 			l_wait_event(thread->t_ctl_waitq,
 				     thread_is_stopped(thread),
 				     &lwi);
@@ -1473,10 +1481,10 @@ ll_sai_unplug(struct ll_statahead_info *sai, struct ll_sa_entry *entry)
 			CDEBUG(D_READA, "Statahead for dir "DFID" hit "
 			       "ratio too low: hit/miss "LPU64"/"LPU64
 			       ", sent/replied "LPU64"/"LPU64", stopping "
-			       "statahead thread: pid %d\n",
+			       "statahead thread\n",
 			       PFID(&lli->lli_fid), sai->sai_hit,
 			       sai->sai_miss, sai->sai_sent,
-			       sai->sai_replied, current_pid());
+			       sai->sai_replied);
 			spin_lock(&lli->lli_sa_lock);
 			if (!thread_is_stopped(thread))
 				thread_set_flags(thread, SVC_STOPPING);
@@ -1553,12 +1561,6 @@ int do_statahead_enter(struct inode *dir, struct dentry **dentryp,
 			return entry ? 1 : -EAGAIN;
 		}
 
-		/* if statahead is busy in readdir, help it do post-work */
-		while (!ll_sa_entry_stated(entry) &&
-		       sai->sai_in_readpage &&
-		       !sa_received_empty(sai))
-			ll_post_statahead(sai);
-
 		if (!ll_sa_entry_stated(entry)) {
 			sai->sai_index_wait = entry->se_index;
 			lwi = LWI_TIMEOUT_INTR(cfs_time_seconds(30), NULL,
@@ -1655,9 +1657,15 @@ int do_statahead_enter(struct inode *dir, struct dentry **dentryp,
 		GOTO(out, rc = -EAGAIN);
 	}
 
-	CDEBUG(D_READA, "start statahead thread: [pid %d] [parent %.*s]\n",
-	       current_pid(), parent->d_name.len, parent->d_name.name);
+	CDEBUG(D_READA, "start statahead thread: sai %p, parent %.*s\n",
+	       sai, parent->d_name.len, parent->d_name.name);
 
+	/* The sai buffer already has one reference taken at allocation time,
+	 * but as soon as we expose the sai by attaching it to the lli that
+	 * default reference can be dropped by another thread calling
+	 * ll_stop_statahead. We need to take a local reference to protect
+	 * the sai buffer while we intend to access it. */
+	ll_sai_get(sai);
 	lli->lli_sai = sai;
 
 	plli = ll_i2info(parent->d_inode);
@@ -1670,6 +1678,9 @@ int do_statahead_enter(struct inode *dir, struct dentry **dentryp,
 		lli->lli_opendir_key = NULL;
 		thread_set_flags(thread, SVC_STOPPED);
 		thread_set_flags(&sai->sai_agl_thread, SVC_STOPPED);
+		/* Drop both our own local reference and the default
+		 * reference from allocation time. */
+		ll_sai_put(sai);
 		ll_sai_put(sai);
 		LASSERT(lli->lli_sai == NULL);
 		return -EAGAIN;
@@ -1678,6 +1689,7 @@ int do_statahead_enter(struct inode *dir, struct dentry **dentryp,
 	l_wait_event(thread->t_ctl_waitq,
 		     thread_is_running(thread) || thread_is_stopped(thread),
 		     &lwi);
+	ll_sai_put(sai);
 
 	/*
 	 * We don't stat-ahead for the first dirent since we are already in
diff --git a/drivers/staging/lustre/lustre/llite/super25.c b/drivers/staging/lustre/lustre/llite/super25.c
index e21e1c760a8..951fdb1265b 100644
--- a/drivers/staging/lustre/lustre/llite/super25.c
+++ b/drivers/staging/lustre/lustre/llite/super25.c
@@ -52,7 +52,7 @@ static struct inode *ll_alloc_inode(struct super_block *sb)
 {
 	struct ll_inode_info *lli;
 	ll_stats_ops_tally(ll_s2sbi(sb), LPROC_LL_ALLOC_INODE, 1);
-	OBD_SLAB_ALLOC_PTR_GFP(lli, ll_inode_cachep, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(lli, ll_inode_cachep, GFP_NOFS);
 	if (lli == NULL)
 		return NULL;
 
@@ -72,7 +72,7 @@ static void ll_destroy_inode(struct inode *inode)
 	call_rcu(&inode->i_rcu, ll_inode_destroy_callback);
 }
 
-int ll_init_inodecache(void)
+static int ll_init_inodecache(void)
 {
 	ll_inode_cachep = kmem_cache_create("lustre_inode_cache",
 					       sizeof(struct ll_inode_info),
@@ -82,7 +82,7 @@ int ll_init_inodecache(void)
 	return 0;
 }
 
-void ll_destroy_inodecache(void)
+static void ll_destroy_inodecache(void)
 {
 	kmem_cache_destroy(ll_inode_cachep);
 }
@@ -103,9 +103,6 @@ MODULE_ALIAS_FS("lustre");
 
 void lustre_register_client_process_config(int (*cpc)(struct lustre_cfg *lcfg));
 
-int vvp_global_init(void);
-void vvp_global_fini(void);
-
 static int __init init_lustre_lite(void)
 {
 	int i, rc, seed[2];
diff --git a/drivers/staging/lustre/lustre/llite/symlink.c b/drivers/staging/lustre/lustre/llite/symlink.c
index 80d48b5ae24..129d3023dce 100644
--- a/drivers/staging/lustre/lustre/llite/symlink.c
+++ b/drivers/staging/lustre/lustre/llite/symlink.c
@@ -120,7 +120,7 @@ static void *ll_follow_link(struct dentry *dentry, struct nameidata *nd)
 	struct inode *inode = dentry->d_inode;
 	struct ptlrpc_request *request = NULL;
 	int rc;
-	char *symname;
+	char *symname = NULL;
 
 	CDEBUG(D_VFSTRACE, "VFS Op\n");
 	/* Limit the recursive symlink depth to 5 instead of default
diff --git a/drivers/staging/lustre/lustre/llite/vvp_dev.c b/drivers/staging/lustre/lustre/llite/vvp_dev.c
index c4d1580b7be..0f68c16abe3 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_dev.c
+++ b/drivers/staging/lustre/lustre/llite/vvp_dev.c
@@ -43,7 +43,7 @@
 
 #include <obd.h>
 #include <lustre_lite.h>
-
+#include "llite_internal.h"
 #include "vvp_internal.h"
 
 /*****************************************************************************
@@ -57,7 +57,7 @@
  * "llite_" (var. "ll_") prefix.
  */
 
-struct kmem_cache *vvp_thread_kmem;
+static struct kmem_cache *vvp_thread_kmem;
 static struct kmem_cache *vvp_session_kmem;
 static struct lu_kmem_descr vvp_caches[] = {
 	{
@@ -80,7 +80,7 @@ static void *vvp_key_init(const struct lu_context *ctx,
 {
 	struct vvp_thread_info *info;
 
-	OBD_SLAB_ALLOC_PTR_GFP(info, vvp_thread_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(info, vvp_thread_kmem, GFP_NOFS);
 	if (info == NULL)
 		info = ERR_PTR(-ENOMEM);
 	return info;
@@ -98,7 +98,7 @@ static void *vvp_session_key_init(const struct lu_context *ctx,
 {
 	struct vvp_session *session;
 
-	OBD_SLAB_ALLOC_PTR_GFP(session, vvp_session_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(session, vvp_session_kmem, GFP_NOFS);
 	if (session == NULL)
 		session = ERR_PTR(-ENOMEM);
 	return session;
@@ -536,7 +536,7 @@ static int vvp_dump_pgcache_seq_open(struct inode *inode, struct file *filp)
 	return result;
 }
 
-struct file_operations vvp_dump_pgcache_file_ops = {
+const struct file_operations vvp_dump_pgcache_file_ops = {
 	.owner   = THIS_MODULE,
 	.open    = vvp_dump_pgcache_seq_open,
 	.read    = seq_read,
diff --git a/drivers/staging/lustre/lustre/llite/vvp_internal.h b/drivers/staging/lustre/lustre/llite/vvp_internal.h
index c82bf17f55a..3c9a03d9955 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_internal.h
+++ b/drivers/staging/lustre/lustre/llite/vvp_internal.h
@@ -57,6 +57,6 @@ struct lu_object *vvp_object_alloc(const struct lu_env *env,
 
 struct ccc_object *cl_inode2ccc(struct inode *inode);
 
-extern struct kmem_cache *vvp_thread_kmem;
+extern const struct file_operations vvp_dump_pgcache_file_ops;
 
 #endif /* VVP_INTERNAL_H */
diff --git a/drivers/staging/lustre/lustre/llite/vvp_io.c b/drivers/staging/lustre/lustre/llite/vvp_io.c
index c7d70091246..7dd2b4723c5 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_io.c
+++ b/drivers/staging/lustre/lustre/llite/vvp_io.c
@@ -80,7 +80,7 @@ static bool can_populate_pages(const struct lu_env *env, struct cl_io *io,
 	case CIT_WRITE:
 		/* don't need lock here to check lli_layout_gen as we have held
 		 * extent lock and GROUP lock has to hold to swap layout */
-		if (lli->lli_layout_gen != cio->cui_layout_gen) {
+		if (ll_layout_version_get(lli) != cio->cui_layout_gen) {
 			io->ci_need_restart = 1;
 			/* this will return application a short read/write */
 			io->ci_continue = 0;
@@ -190,7 +190,7 @@ static void vvp_io_fault_fini(const struct lu_env *env,
 	vvp_io_fini(env, ios);
 }
 
-enum cl_lock_mode vvp_mode_from_vma(struct vm_area_struct *vma)
+static enum cl_lock_mode vvp_mode_from_vma(struct vm_area_struct *vma)
 {
 	/*
 	 * we only want to hold PW locks if the mmap() can generate
diff --git a/drivers/staging/lustre/lustre/llite/vvp_object.c b/drivers/staging/lustre/lustre/llite/vvp_object.c
index 25973dedd9a..65b6db1b71b 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_object.c
+++ b/drivers/staging/lustre/lustre/llite/vvp_object.c
@@ -120,13 +120,28 @@ static int vvp_attr_set(const struct lu_env *env, struct cl_object *obj,
 	return 0;
 }
 
-int vvp_conf_set(const struct lu_env *env, struct cl_object *obj,
-		const struct cl_object_conf *conf)
+static int vvp_conf_set(const struct lu_env *env, struct cl_object *obj,
+			const struct cl_object_conf *conf)
 {
 	struct ll_inode_info *lli = ll_i2info(conf->coc_inode);
 
 	if (conf->coc_opc == OBJECT_CONF_INVALIDATE) {
-		lli->lli_layout_gen = LL_LAYOUT_GEN_NONE;
+		CDEBUG(D_VFSTRACE, DFID ": losing layout lock\n",
+		       PFID(&lli->lli_fid));
+
+		ll_layout_version_set(lli, LL_LAYOUT_GEN_NONE);
+
+		/* Clean up page mmap for this inode.
+		 * The reason for us to do this is that if the page has
+		 * already been installed into memory space, the process
+		 * can access it without interacting with lustre, so this
+		 * page may be stale due to layout change, and the process
+		 * will never be notified.
+		 * This operation is expensive but mmap processes have to pay
+		 * a price themselves. */
+		unmap_mapping_range(conf->coc_inode->i_mapping,
+				    0, OBD_OBJECT_EOF, 0);
+
 		return 0;
 	}
 
@@ -134,18 +149,18 @@ int vvp_conf_set(const struct lu_env *env, struct cl_object *obj,
 		return 0;
 
 	if (conf->u.coc_md != NULL && conf->u.coc_md->lsm != NULL) {
-		CDEBUG(D_VFSTRACE, "layout lock change: %u -> %u\n",
-			lli->lli_layout_gen,
-			conf->u.coc_md->lsm->lsm_layout_gen);
+		CDEBUG(D_VFSTRACE, DFID ": layout version change: %u -> %u\n",
+		       PFID(&lli->lli_fid), lli->lli_layout_gen,
+		       conf->u.coc_md->lsm->lsm_layout_gen);
 
 		lli->lli_has_smd = lsm_has_objects(conf->u.coc_md->lsm);
-		lli->lli_layout_gen = conf->u.coc_md->lsm->lsm_layout_gen;
+		ll_layout_version_set(lli, conf->u.coc_md->lsm->lsm_layout_gen);
 	} else {
-		CDEBUG(D_VFSTRACE, "layout lock destroyed: %u.\n",
-			lli->lli_layout_gen);
+		CDEBUG(D_VFSTRACE, DFID ": layout nuked: %u.\n",
+		       PFID(&lli->lli_fid), lli->lli_layout_gen);
 
 		lli->lli_has_smd = false;
-		lli->lli_layout_gen = LL_LAYOUT_GEN_EMPTY;
+		ll_layout_version_set(lli, LL_LAYOUT_GEN_EMPTY);
 	}
 	return 0;
 }
diff --git a/drivers/staging/lustre/lustre/llite/xattr.c b/drivers/staging/lustre/lustre/llite/xattr.c
index b1ed4d9ea6b..c6c27bbb43b 100644
--- a/drivers/staging/lustre/lustre/llite/xattr.c
+++ b/drivers/staging/lustre/lustre/llite/xattr.c
@@ -112,8 +112,8 @@ int ll_setxattr_common(struct inode *inode, const char *name,
 	struct ptlrpc_request *req = NULL;
 	int xattr_type, rc;
 	struct obd_capa *oc;
-	struct rmtacl_ctl_entry *rce = NULL;
 #ifdef CONFIG_FS_POSIX_ACL
+	struct rmtacl_ctl_entry *rce = NULL;
 	posix_acl_xattr_header *new_value = NULL;
 	ext_acl_xattr_header *acl = NULL;
 #endif
@@ -124,6 +124,11 @@ int ll_setxattr_common(struct inode *inode, const char *name,
 	if (rc)
 		return rc;
 
+	if ((xattr_type == XATTR_ACL_ACCESS_T ||
+	     xattr_type == XATTR_ACL_DEFAULT_T) &&
+	    !inode_owner_or_capable(inode))
+		return -EPERM;
+
 	/* b10667: ignore lustre special xattr for now */
 	if ((xattr_type == XATTR_TRUSTED_T && strcmp(name, "trusted.lov") == 0) ||
 	    (xattr_type == XATTR_LUSTRE_T && strcmp(name, "lustre.lov") == 0))
diff --git a/drivers/staging/lustre/lustre/llite/xattr_cache.c b/drivers/staging/lustre/lustre/llite/xattr_cache.c
index 4defa2fd83b..4dd83fc0368 100644
--- a/drivers/staging/lustre/lustre/llite/xattr_cache.c
+++ b/drivers/staging/lustre/lustre/llite/xattr_cache.c
@@ -120,7 +120,7 @@ static int ll_xattr_cache_add(struct list_head *cache,
 		return -EPROTO;
 	}
 
-	OBD_SLAB_ALLOC_PTR_GFP(xattr, xattr_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(xattr, xattr_kmem, GFP_NOFS);
 	if (xattr == NULL) {
 		CDEBUG(D_CACHE, "failed to allocate xattr\n");
 		return -ENOMEM;
@@ -232,7 +232,7 @@ static int ll_xattr_cache_list(struct list_head *cache,
  * \retval 0 @cache is not initialized
  * \retval 1 @cache is initialized
  */
-int ll_xattr_cache_valid(struct ll_inode_info *lli)
+static int ll_xattr_cache_valid(struct ll_inode_info *lli)
 {
 	return !!(lli->lli_flags & LLIF_XATTR_CACHE);
 }
diff --git a/drivers/staging/lustre/lustre/lmv/lmv_obd.c b/drivers/staging/lustre/lustre/lmv/lmv_obd.c
index 3ba0a0a1d94..4edf8a31221 100644
--- a/drivers/staging/lustre/lustre/lmv/lmv_obd.c
+++ b/drivers/staging/lustre/lustre/lmv/lmv_obd.c
@@ -280,7 +280,7 @@ static void lmv_set_timeouts(struct obd_device *obd)
 }
 
 static int lmv_init_ea_size(struct obd_export *exp, int easize,
-			    int def_easize, int cookiesize)
+			    int def_easize, int cookiesize, int def_cookiesize)
 {
 	struct obd_device   *obd = exp->exp_obd;
 	struct lmv_obd      *lmv = &obd->u.lmv;
@@ -300,6 +300,10 @@ static int lmv_init_ea_size(struct obd_export *exp, int easize,
 		lmv->max_cookiesize = cookiesize;
 		change = 1;
 	}
+	if (lmv->max_def_cookiesize < def_cookiesize) {
+		lmv->max_def_cookiesize = def_cookiesize;
+		change = 1;
+	}
 	if (change == 0)
 		return 0;
 
@@ -315,7 +319,7 @@ static int lmv_init_ea_size(struct obd_export *exp, int easize,
 		}
 
 		rc = md_init_ea_size(lmv->tgts[i]->ltd_exp, easize, def_easize,
-				     cookiesize);
+				     cookiesize, def_cookiesize);
 		if (rc) {
 			CERROR("%s: obd_init_ea_size() failed on MDT target %d:"
 			       " rc = %d.\n", obd->obd_name, i, rc);
@@ -400,8 +404,8 @@ int lmv_connect_mdc(struct obd_device *obd, struct lmv_tgt_desc *tgt)
 	tgt->ltd_exp = mdc_exp;
 	lmv->desc.ld_active_tgt_count++;
 
-	md_init_ea_size(tgt->ltd_exp, lmv->max_easize,
-			lmv->max_def_easize, lmv->max_cookiesize);
+	md_init_ea_size(tgt->ltd_exp, lmv->max_easize, lmv->max_def_easize,
+			lmv->max_cookiesize, lmv->max_def_cookiesize);
 
 	CDEBUG(D_CONFIG, "Connected to %s(%s) successfully (%d)\n",
 		mdc_obd->obd_name, mdc_obd->obd_uuid.uuid,
@@ -527,9 +531,8 @@ static int lmv_add_target(struct obd_device *obd, struct obd_uuid *uuidp,
 			spin_unlock(&lmv->lmv_lock);
 		} else {
 			int easize = sizeof(struct lmv_stripe_md) +
-				     lmv->desc.ld_tgt_count *
-				     sizeof(struct lu_fid);
-			lmv_init_ea_size(obd->obd_self_export, easize, 0, 0);
+				lmv->desc.ld_tgt_count * sizeof(struct lu_fid);
+			lmv_init_ea_size(obd->obd_self_export, easize, 0, 0, 0);
 		}
 	}
 
@@ -578,7 +581,7 @@ int lmv_check_connect(struct obd_device *obd)
 	class_export_put(lmv->exp);
 	lmv->connected = 1;
 	easize = lmv_get_easize(lmv);
-	lmv_init_ea_size(obd->obd_self_export, easize, 0, 0);
+	lmv_init_ea_size(obd->obd_self_export, easize, 0, 0, 0);
 	lmv_init_unlock(lmv);
 	return 0;
 
@@ -2340,7 +2343,11 @@ static int lmv_get_info(const struct lu_env *env, struct obd_export *exp,
 				return 0;
 		}
 		return -EINVAL;
-	} else if (KEY_IS(KEY_MAX_EASIZE) || KEY_IS(KEY_CONN_DATA)) {
+	} else if (KEY_IS(KEY_MAX_EASIZE) ||
+		   KEY_IS(KEY_DEFAULT_EASIZE) ||
+		   KEY_IS(KEY_MAX_COOKIESIZE) ||
+		   KEY_IS(KEY_DEFAULT_COOKIESIZE) ||
+		   KEY_IS(KEY_CONN_DATA)) {
 		rc = lmv_check_connect(obd);
 		if (rc)
 			return rc;
diff --git a/drivers/staging/lustre/lustre/lmv/lproc_lmv.c b/drivers/staging/lustre/lustre/lmv/lproc_lmv.c
index 5d5c3081c46..ae73c82ce49 100644
--- a/drivers/staging/lustre/lustre/lmv/lproc_lmv.c
+++ b/drivers/staging/lustre/lustre/lmv/lproc_lmv.c
@@ -200,19 +200,19 @@ static int lmv_target_seq_open(struct inode *inode, struct file *file)
 LPROC_SEQ_FOPS_RO_TYPE(lmv, uuid);
 
 struct lprocfs_vars lprocfs_lmv_obd_vars[] = {
-	{ "numobd",	  &lmv_numobd_fops,	  0, 0 },
-	{ "placement",	  &lmv_placement_fops,    0, 0 },
-	{ "activeobd",	  &lmv_activeobd_fops,    0, 0 },
-	{ "uuid",	  &lmv_uuid_fops,	  0, 0 },
-	{ "desc_uuid",	  &lmv_desc_uuid_fops,    0, 0 },
-	{ 0 }
+	{ "numobd",	  &lmv_numobd_fops,	  NULL, 0 },
+	{ "placement",	  &lmv_placement_fops,    NULL, 0 },
+	{ "activeobd",	  &lmv_activeobd_fops,    NULL, 0 },
+	{ "uuid",	  &lmv_uuid_fops,	  NULL, 0 },
+	{ "desc_uuid",	  &lmv_desc_uuid_fops,    NULL, 0 },
+	{ NULL }
 };
 
 LPROC_SEQ_FOPS_RO_TYPE(lmv, numrefs);
 
 static struct lprocfs_vars lprocfs_lmv_module_vars[] = {
-	{ "num_refs",	   &lmv_numrefs_fops, 0, 0 },
-	{ 0 }
+	{ "num_refs",	   &lmv_numrefs_fops, NULL, 0 },
+	{ NULL }
 };
 
 struct file_operations lmv_proc_target_fops = {
diff --git a/drivers/staging/lustre/lustre/lov/Makefile b/drivers/staging/lustre/lustre/lov/Makefile
index 9a5f26d5558..a908edb533d 100644
--- a/drivers/staging/lustre/lustre/lov/Makefile
+++ b/drivers/staging/lustre/lustre/lov/Makefile
@@ -1,5 +1,5 @@
 obj-$(CONFIG_LUSTRE_FS) += lov.o
-lov-y := lov_log.o lov_obd.o lov_pack.o lov_offset.o lov_merge.o \
+lov-y := lov_obd.o lov_pack.o lov_offset.o lov_merge.o \
 	 lov_request.o lov_ea.o lov_dev.o lov_object.o lov_page.o  \
 	 lov_lock.o lov_io.o lovsub_dev.o lovsub_object.o lovsub_page.o      \
 	 lovsub_lock.o lovsub_io.o lov_pool.o
diff --git a/drivers/staging/lustre/lustre/lov/lov_dev.c b/drivers/staging/lustre/lustre/lov/lov_dev.c
index 1f33b04b0c3..53e5781ba1d 100644
--- a/drivers/staging/lustre/lustre/lov/lov_dev.c
+++ b/drivers/staging/lustre/lustre/lov/lov_dev.c
@@ -143,7 +143,7 @@ static void *lov_key_init(const struct lu_context *ctx,
 {
 	struct lov_thread_info *info;
 
-	OBD_SLAB_ALLOC_PTR_GFP(info, lov_thread_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(info, lov_thread_kmem, GFP_NOFS);
 	if (info != NULL)
 		INIT_LIST_HEAD(&info->lti_closure.clc_list);
 	else
@@ -170,7 +170,7 @@ static void *lov_session_key_init(const struct lu_context *ctx,
 {
 	struct lov_session *info;
 
-	OBD_SLAB_ALLOC_PTR_GFP(info, lov_session_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(info, lov_session_kmem, GFP_NOFS);
 	if (info == NULL)
 		info = ERR_PTR(-ENOMEM);
 	return info;
@@ -260,7 +260,7 @@ static int lov_req_init(const struct lu_env *env, struct cl_device *dev,
 	struct lov_req *lr;
 	int result;
 
-	OBD_SLAB_ALLOC_PTR_GFP(lr, lov_req_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(lr, lov_req_kmem, GFP_NOFS);
 	if (lr != NULL) {
 		cl_req_slice_add(req, &lr->lr_cl, dev, &lov_req_ops);
 		result = 0;
diff --git a/drivers/staging/lustre/lustre/lov/lov_ea.c b/drivers/staging/lustre/lustre/lov/lov_ea.c
index 6f356e02554..a0c148e31f6 100644
--- a/drivers/staging/lustre/lustre/lov/lov_ea.c
+++ b/drivers/staging/lustre/lustre/lov/lov_ea.c
@@ -44,7 +44,6 @@
 #include <linux/libcfs/libcfs.h>
 
 #include <obd_class.h>
-#include <obd_lov.h>
 #include <lustre/lustre_idl.h>
 
 #include "lov_internal.h"
@@ -101,7 +100,7 @@ struct lov_stripe_md *lsm_alloc_plain(__u16 stripe_count, int *size)
 		return NULL;
 
 	for (i = 0; i < stripe_count; i++) {
-		OBD_SLAB_ALLOC_PTR_GFP(loi, lov_oinfo_slab, __GFP_IO);
+		OBD_SLAB_ALLOC_PTR_GFP(loi, lov_oinfo_slab, GFP_NOFS);
 		if (loi == NULL)
 			goto err;
 		lsm->lsm_oinfo[i] = loi;
@@ -346,3 +345,14 @@ const struct lsm_operations lsm_v3_ops = {
 	.lsm_lmm_verify	 = lsm_lmm_verify_v3,
 	.lsm_unpackmd	   = lsm_unpackmd_v3,
 };
+
+void dump_lsm(unsigned int level, const struct lov_stripe_md *lsm)
+{
+	CDEBUG(level, "lsm %p, objid "DOSTID", maxbytes "LPX64", magic 0x%08X,"
+	       " stripe_size %u, stripe_count %u, refc: %d,"
+	       " layout_gen %u, pool ["LOV_POOLNAMEF"]\n", lsm,
+	       POSTID(&lsm->lsm_oi), lsm->lsm_maxbytes, lsm->lsm_magic,
+	       lsm->lsm_stripe_size, lsm->lsm_stripe_count,
+	       atomic_read(&lsm->lsm_refc), lsm->lsm_layout_gen,
+	       lsm->lsm_pool_name);
+}
diff --git a/drivers/staging/lustre/lustre/lov/lov_internal.h b/drivers/staging/lustre/lustre/lov/lov_internal.h
index 2b22a03c038..38508a5c827 100644
--- a/drivers/staging/lustre/lustre/lov/lov_internal.h
+++ b/drivers/staging/lustre/lustre/lov/lov_internal.h
@@ -38,9 +38,39 @@
 #define LOV_INTERNAL_H
 
 #include <obd_class.h>
-#include <obd_lov.h>
 #include <lustre/lustre_user.h>
 
+/* lov_do_div64(a, b) returns a % b, and a = a / b.
+ * The 32-bit code is LOV-specific due to knowing about stripe limits in
+ * order to reduce the divisor to a 32-bit number.  If the divisor is
+ * already a 32-bit value the compiler handles this directly. */
+#if BITS_PER_LONG == 64
+# define lov_do_div64(n, base) ({					\
+	uint64_t __base = (base);					\
+	uint64_t __rem;							\
+	__rem = ((uint64_t)(n)) % __base;				\
+	(n) = ((uint64_t)(n)) / __base;					\
+	__rem;								\
+})
+#elif BITS_PER_LONG == 32
+# define lov_do_div64(n, base) ({					\
+	uint64_t __rem;							\
+	if ((sizeof(base) > 4) && (((base) & 0xffffffff00000000ULL) != 0)) {  \
+		int __remainder;					      \
+		LASSERTF(!((base) & (LOV_MIN_STRIPE_SIZE - 1)), "64 bit lov " \
+			 "division %llu / %llu\n", (n), (uint64_t)(base));    \
+		__remainder = (n) & (LOV_MIN_STRIPE_SIZE - 1);		\
+		(n) >>= LOV_MIN_STRIPE_BITS;				\
+		__rem = do_div(n, (base) >> LOV_MIN_STRIPE_BITS);	\
+		__rem <<= LOV_MIN_STRIPE_BITS;				\
+		__rem += __remainder;					\
+	} else {							\
+		__rem = do_div(n, base);				\
+	}								\
+	__rem;								\
+})
+#endif
+
 struct lov_lock_handles {
 	struct portals_handle   llh_handle;
 	atomic_t	    llh_refcount;
@@ -251,10 +281,6 @@ int lov_process_config_base(struct obd_device *obd, struct lustre_cfg *lcfg,
 			    __u32 *indexp, int *genp);
 int lov_del_target(struct obd_device *obd, __u32 index,
 		   struct obd_uuid *uuidp, int gen);
-/* lov_log.c */
-int lov_llog_init(struct obd_device *obd, struct obd_llog_group *olg,
-		  struct obd_device *tgt, int *idx);
-int lov_llog_finish(struct obd_device *obd, int count);
 
 /* lov_pack.c */
 int lov_packmd(struct obd_export *exp, struct lov_mds_md **lmm,
@@ -279,6 +305,7 @@ void lov_dump_lmm(int level, void *lmm);
 /* lov_ea.c */
 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);
diff --git a/drivers/staging/lustre/lustre/lov/lov_lock.c b/drivers/staging/lustre/lustre/lov/lov_lock.c
index ed2726e523e..08ac3745f0d 100644
--- a/drivers/staging/lustre/lustre/lov/lov_lock.c
+++ b/drivers/staging/lustre/lustre/lov/lov_lock.c
@@ -144,7 +144,7 @@ static struct cl_lock *lov_sublock_alloc(const struct lu_env *env,
 
 	LASSERT(idx < lck->lls_nr);
 
-	OBD_SLAB_ALLOC_PTR_GFP(link, lov_lock_link_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(link, lov_lock_link_kmem, GFP_NOFS);
 	if (link != NULL) {
 		struct lov_sublock_env *subenv;
 		struct lov_lock_sub  *lls;
@@ -346,41 +346,7 @@ static int lov_lock_sub_init(const struct lu_env *env,
 		}
 	}
 	LASSERT(nr == lck->lls_nr);
-	/*
-	 * Then, create sub-locks. Once at least one sub-lock was created,
-	 * top-lock can be reached by other threads.
-	 */
-	for (i = 0; i < lck->lls_nr; ++i) {
-		struct cl_lock       *sublock;
-		struct lov_lock_link *link;
 
-		if (lck->lls_sub[i].sub_lock == NULL) {
-			sublock = lov_sublock_alloc(env, io, lck, i, &link);
-			if (IS_ERR(sublock)) {
-				result = PTR_ERR(sublock);
-				break;
-			}
-			cl_lock_get_trust(sublock);
-			cl_lock_mutex_get(env, sublock);
-			cl_lock_mutex_get(env, parent);
-			/*
-			 * recheck under mutex that sub-lock wasn't created
-			 * concurrently, and that top-lock is still alive.
-			 */
-			if (lck->lls_sub[i].sub_lock == NULL &&
-			    parent->cll_state < CLS_FREEING) {
-				lov_sublock_adopt(env, lck, sublock, i, link);
-				cl_lock_mutex_put(env, parent);
-			} else {
-				OBD_SLAB_FREE_PTR(link, lov_lock_link_kmem);
-				cl_lock_mutex_put(env, parent);
-				cl_lock_unhold(env, sublock,
-					       "lov-parent", parent);
-			}
-			cl_lock_mutex_put(env, sublock);
-			cl_lock_put(env, sublock);
-		}
-	}
 	/*
 	 * Some sub-locks can be missing at this point. This is not a problem,
 	 * because enqueue will create them anyway. Main duty of this function
@@ -533,7 +499,7 @@ static int lov_lock_enqueue_one(const struct lu_env *env, struct lov_lock *lck,
 static int lov_sublock_fill(const struct lu_env *env, struct cl_lock *parent,
 			    struct cl_io *io, struct lov_lock *lck, int idx)
 {
-	struct lov_lock_link *link;
+	struct lov_lock_link *link = NULL;
 	struct cl_lock       *sublock;
 	int		   result;
 
@@ -1159,7 +1125,7 @@ int lov_lock_init_raid0(const struct lu_env *env, struct cl_object *obj,
 	struct lov_lock *lck;
 	int result;
 
-	OBD_SLAB_ALLOC_PTR_GFP(lck, lov_lock_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(lck, lov_lock_kmem, GFP_NOFS);
 	if (lck != NULL) {
 		cl_lock_slice_add(lock, &lck->lls_cl, obj, &lov_lock_ops);
 		result = lov_lock_sub_init(env, lck, io);
@@ -1194,7 +1160,7 @@ int lov_lock_init_empty(const struct lu_env *env, struct cl_object *obj,
 	struct lov_lock *lck;
 	int result = -ENOMEM;
 
-	OBD_SLAB_ALLOC_PTR_GFP(lck, lov_lock_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(lck, lov_lock_kmem, GFP_NOFS);
 	if (lck != NULL) {
 		cl_lock_slice_add(lock, &lck->lls_cl, obj, &lov_empty_lock_ops);
 		lck->lls_orig = lock->cll_descr;
diff --git a/drivers/staging/lustre/lustre/lov/lov_log.c b/drivers/staging/lustre/lustre/lov/lov_log.c
deleted file mode 100644
index 3eedd935d1b..00000000000
--- a/drivers/staging/lustre/lustre/lov/lov_log.c
+++ /dev/null
@@ -1,272 +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/lov/lov_log.c
- *
- * Author: Phil Schwan <phil@clusterfs.com>
- * Author: Peter Braam <braam@clusterfs.com>
- * Author: Mike Shaver <shaver@clusterfs.com>
- */
-
-#define DEBUG_SUBSYSTEM S_LOV
-#include <linux/libcfs/libcfs.h>
-
-#include <obd_support.h>
-#include <lustre_lib.h>
-#include <lustre_net.h>
-#include <lustre/lustre_idl.h>
-#include <lustre_dlm.h>
-#include <lustre_mds.h>
-#include <obd_class.h>
-#include <obd_lov.h>
-#include <obd_ost.h>
-#include <lprocfs_status.h>
-#include <lustre_log.h>
-
-#include "lov_internal.h"
-
-/* Add log records for each OSC that this object is striped over, and return
- * cookies for each one.  We _would_ have nice abstraction here, except that
- * we need to keep cookies in stripe order, even if some are NULL, so that
- * the right cookies are passed back to the right OSTs at the client side.
- * Unset cookies should be all-zero (which will never occur naturally). */
-static int lov_llog_origin_add(const struct lu_env *env,
-			       struct llog_ctxt *ctxt,
-			       struct llog_rec_hdr *rec,
-			       struct lov_stripe_md *lsm,
-			       struct llog_cookie *logcookies, int numcookies)
-{
-	struct obd_device *obd = ctxt->loc_obd;
-	struct lov_obd *lov = &obd->u.lov;
-	int i, rc = 0, cookies = 0;
-
-	LASSERTF(logcookies && numcookies >= lsm->lsm_stripe_count,
-		 "logcookies %p, numcookies %d lsm->lsm_stripe_count %d \n",
-		 logcookies, numcookies, lsm->lsm_stripe_count);
-
-	for (i = 0; i < lsm->lsm_stripe_count; i++) {
-		struct lov_oinfo *loi = lsm->lsm_oinfo[i];
-		struct obd_device *child =
-			lov->lov_tgts[loi->loi_ost_idx]->ltd_exp->exp_obd;
-		struct llog_ctxt *cctxt = llog_get_context(child, ctxt->loc_idx);
-
-		/* fill mds unlink/setattr log record */
-		switch (rec->lrh_type) {
-		case MDS_UNLINK_REC: {
-			struct llog_unlink_rec *lur = (struct llog_unlink_rec *)rec;
-			lur->lur_oid = ostid_id(&loi->loi_oi);
-			lur->lur_oseq = (__u32)ostid_seq(&loi->loi_oi);
-			break;
-		}
-		case MDS_SETATTR64_REC: {
-			struct llog_setattr64_rec *lsr = (struct llog_setattr64_rec *)rec;
-			lsr->lsr_oi = loi->loi_oi;
-			break;
-		}
-		default:
-			break;
-		}
-
-		/* inject error in llog_obd_add() below */
-		if (OBD_FAIL_CHECK(OBD_FAIL_MDS_FAIL_LOV_LOG_ADD)) {
-			llog_ctxt_put(cctxt);
-			cctxt = NULL;
-		}
-		rc = llog_obd_add(env, cctxt, rec, NULL, logcookies + cookies,
-				  numcookies - cookies);
-		llog_ctxt_put(cctxt);
-		if (rc < 0) {
-			CERROR("Can't add llog (rc = %d) for stripe %d\n",
-			       rc, cookies);
-			memset(logcookies + cookies, 0,
-			       sizeof(struct llog_cookie));
-			rc = 1; /* skip this cookie */
-		}
-		/* Note that rc is always 1 if llog_obd_add was successful */
-		cookies += rc;
-	}
-	return cookies;
-}
-
-static int lov_llog_origin_connect(struct llog_ctxt *ctxt,
-				   struct llog_logid *logid,
-				   struct llog_gen *gen,
-				   struct obd_uuid *uuid)
-{
-	struct obd_device *obd = ctxt->loc_obd;
-	struct lov_obd *lov = &obd->u.lov;
-	int i, rc = 0, err = 0;
-
-	obd_getref(obd);
-	for (i = 0; i < lov->desc.ld_tgt_count; i++) {
-		struct obd_device *child;
-		struct llog_ctxt *cctxt;
-
-		if (!lov->lov_tgts[i] || !lov->lov_tgts[i]->ltd_active)
-			continue;
-		if (uuid && !obd_uuid_equals(uuid, &lov->lov_tgts[i]->ltd_uuid))
-			continue;
-		CDEBUG(D_CONFIG, "connect %d/%d\n", i, lov->desc.ld_tgt_count);
-		child = lov->lov_tgts[i]->ltd_exp->exp_obd;
-		cctxt = llog_get_context(child, ctxt->loc_idx);
-		rc = llog_connect(cctxt, logid, gen, uuid);
-		llog_ctxt_put(cctxt);
-
-		if (rc) {
-			CERROR("error osc_llog_connect tgt %d (%d)\n", i, rc);
-			if (!err)
-				err = rc;
-		}
-	}
-	obd_putref(obd);
-
-	return err;
-}
-
-/* the replicators commit callback */
-static int lov_llog_repl_cancel(const struct lu_env *env,
-				struct llog_ctxt *ctxt,
-				struct lov_stripe_md *lsm,
-				int count, struct llog_cookie *cookies,
-				int flags)
-{
-	struct lov_obd *lov;
-	struct obd_device *obd = ctxt->loc_obd;
-	int rc = 0, i;
-
-	LASSERT(lsm != NULL);
-	LASSERT(count == lsm->lsm_stripe_count);
-
-	lov = &obd->u.lov;
-	obd_getref(obd);
-	for (i = 0; i < count; i++, cookies++) {
-		struct lov_oinfo *loi = lsm->lsm_oinfo[i];
-		struct obd_device *child =
-			lov->lov_tgts[loi->loi_ost_idx]->ltd_exp->exp_obd;
-		struct llog_ctxt *cctxt =
-			llog_get_context(child, ctxt->loc_idx);
-		int err;
-
-		err = llog_cancel(env, cctxt, NULL, 1, cookies, flags);
-		llog_ctxt_put(cctxt);
-		if (err && lov->lov_tgts[loi->loi_ost_idx]->ltd_active) {
-			CERROR("%s: objid "DOSTID" subobj "DOSTID
-			       " on OST idx %d: rc = %d\n",
-			       obd->obd_name, POSTID(&lsm->lsm_oi),
-			       POSTID(&loi->loi_oi), loi->loi_ost_idx, err);
-			if (!rc)
-				rc = err;
-		}
-	}
-	obd_putref(obd);
-	return rc;
-}
-
-static struct llog_operations lov_mds_ost_orig_logops = {
-	.lop_obd_add	= lov_llog_origin_add,
-	.lop_connect	= lov_llog_origin_connect,
-};
-
-static struct llog_operations lov_size_repl_logops = {
-	.lop_cancel	= lov_llog_repl_cancel,
-};
-
-int lov_llog_init(struct obd_device *obd, struct obd_llog_group *olg,
-		  struct obd_device *disk_obd, int *index)
-{
-	struct lov_obd *lov = &obd->u.lov;
-	struct obd_device *child;
-	int i, rc = 0;
-
-	LASSERT(olg == &obd->obd_olg);
-	rc = llog_setup(NULL, obd, olg, LLOG_MDS_OST_ORIG_CTXT, disk_obd,
-			&lov_mds_ost_orig_logops);
-	if (rc)
-		return rc;
-
-	rc = llog_setup(NULL, obd, olg, LLOG_SIZE_REPL_CTXT, disk_obd,
-			&lov_size_repl_logops);
-	if (rc)
-		GOTO(err_cleanup, rc);
-
-	obd_getref(obd);
-	/* count may not match lov->desc.ld_tgt_count during dynamic ost add */
-	for (i = 0; i < lov->desc.ld_tgt_count; i++) {
-		if (!lov->lov_tgts[i])
-			continue;
-
-		if (index && i != *index)
-			continue;
-
-		child = lov->lov_tgts[i]->ltd_obd;
-		rc = obd_llog_init(child, &child->obd_olg, disk_obd, &i);
-		if (rc)
-			CERROR("error osc_llog_init idx %d osc '%s' tgt '%s' "
-			       "(rc=%d)\n", i, child->obd_name,
-			       disk_obd->obd_name, rc);
-		rc = 0;
-	}
-	obd_putref(obd);
-	GOTO(err_cleanup, rc);
-err_cleanup:
-	if (rc) {
-		struct llog_ctxt *ctxt =
-			llog_get_context(obd, LLOG_SIZE_REPL_CTXT);
-		if (ctxt)
-			llog_cleanup(NULL, ctxt);
-		ctxt = llog_get_context(obd, LLOG_MDS_OST_ORIG_CTXT);
-		if (ctxt)
-			llog_cleanup(NULL, ctxt);
-	}
-	return rc;
-}
-
-int lov_llog_finish(struct obd_device *obd, int count)
-{
-	struct llog_ctxt *ctxt;
-
-	/* cleanup our llogs only if the ctxts have been setup
-	 * (client lov doesn't setup, mds lov does). */
-	ctxt = llog_get_context(obd, LLOG_MDS_OST_ORIG_CTXT);
-	if (ctxt)
-		llog_cleanup(NULL, ctxt);
-
-	ctxt = llog_get_context(obd, LLOG_SIZE_REPL_CTXT);
-	if (ctxt)
-		llog_cleanup(NULL, ctxt);
-
-	/* lov->tgt llogs are cleaned during osc_cleanup. */
-	return 0;
-}
diff --git a/drivers/staging/lustre/lustre/lov/lov_merge.c b/drivers/staging/lustre/lustre/lov/lov_merge.c
index 9defa55d991..da959e90137 100644
--- a/drivers/staging/lustre/lustre/lov/lov_merge.c
+++ b/drivers/staging/lustre/lustre/lov/lov_merge.c
@@ -39,8 +39,6 @@
 #include <linux/libcfs/libcfs.h>
 
 #include <obd_class.h>
-#include <obd_lov.h>
-
 #include "lov_internal.h"
 
 /** Merge the lock value block(&lvb) attributes and KMS from each of the
@@ -60,7 +58,7 @@ int lov_merge_lvb_kms(struct lov_stripe_md *lsm,
 	int i;
 	int rc = 0;
 
-	LASSERT(spin_is_locked(&lsm->lsm_lock));
+	assert_spin_locked(&lsm->lsm_lock);
 	LASSERT(lsm->lsm_lock_owner == current_pid());
 
 	CDEBUG(D_INODE, "MDT ID "DOSTID" initial value: s="LPU64" m="LPU64
@@ -147,7 +145,7 @@ int lov_adjust_kms(struct obd_export *exp, struct lov_stripe_md *lsm,
 	int stripe = 0;
 	__u64 kms;
 
-	LASSERT(spin_is_locked(&lsm->lsm_lock));
+	assert_spin_locked(&lsm->lsm_lock);
 	LASSERT(lsm->lsm_lock_owner == current_pid());
 
 	if (shrink) {
diff --git a/drivers/staging/lustre/lustre/lov/lov_obd.c b/drivers/staging/lustre/lustre/lov/lov_obd.c
index 02509d0cb10..2d843b1c1de 100644
--- a/drivers/staging/lustre/lustre/lov/lov_obd.c
+++ b/drivers/staging/lustre/lustre/lov/lov_obd.c
@@ -50,16 +50,13 @@
 #include <lustre/lustre_idl.h>
 #include <lustre_dlm.h>
 #include <lustre_mds.h>
-#include <lustre_debug.h>
 #include <obd_class.h>
-#include <obd_lov.h>
 #include <obd_ost.h>
 #include <lprocfs_status.h>
 #include <lustre_param.h>
 #include <cl_object.h>
 #include <lclient.h> /* for cl_client_lru */
 #include <lustre/ll_fiemap.h>
-#include <lustre_log.h>
 #include <lustre_fid.h>
 
 #include "lov_internal.h"
@@ -280,7 +277,7 @@ static int lov_disconnect_obd(struct obd_device *obd, struct lov_tgt_desc *tgt)
 
 	osc_obd = class_exp2obd(tgt->ltd_exp);
 	CDEBUG(D_CONFIG, "%s: disconnecting target %s\n",
-	       obd->obd_name, osc_obd->obd_name);
+		obd->obd_name, osc_obd ? osc_obd->obd_name : "NULL");
 
 	if (tgt->ltd_active) {
 		tgt->ltd_active = 0;
@@ -288,11 +285,11 @@ static int lov_disconnect_obd(struct obd_device *obd, struct lov_tgt_desc *tgt)
 		tgt->ltd_exp->exp_obd->obd_inactive = 1;
 	}
 
-	lov_proc_dir = obd->obd_proc_private;
-	if (lov_proc_dir)
-		lprocfs_remove_proc_entry(osc_obd->obd_name, lov_proc_dir);
-
 	if (osc_obd) {
+		lov_proc_dir = obd->obd_proc_private;
+		if (lov_proc_dir) {
+			lprocfs_remove_proc_entry(osc_obd->obd_name, lov_proc_dir);
+		}
 		/* Pass it on to our clients.
 		 * XXX This should be an argument to disconnect,
 		 * XXX not a back-door flag on the OBD.  Ah well.
@@ -726,8 +723,8 @@ void lov_fix_desc_stripe_size(__u64 *val)
 		if (*val != 0)
 			LCONSOLE_INFO("Increasing default stripe size to "
 				      "minimum %u\n",
-				      LOV_DEFAULT_STRIPE_SIZE);
-		*val = LOV_DEFAULT_STRIPE_SIZE;
+				      LOV_DESC_STRIPE_SIZE_DEFAULT);
+		*val = LOV_DESC_STRIPE_SIZE_DEFAULT;
 	} else if (*val & (LOV_MIN_STRIPE_SIZE - 1)) {
 		*val &= ~(LOV_MIN_STRIPE_SIZE - 1);
 		LCONSOLE_WARN("Changing default stripe size to "LPU64" (a "
@@ -753,9 +750,8 @@ void lov_fix_desc_pattern(__u32 *val)
 
 void lov_fix_desc_qos_maxage(__u32 *val)
 {
-	/* fix qos_maxage */
 	if (*val == 0)
-		*val = QOS_DEFAULT_MAXAGE;
+		*val = LOV_DESC_QOS_MAXAGE_DEFAULT;
 }
 
 void lov_fix_desc(struct lov_desc *desc)
@@ -861,12 +857,10 @@ static int lov_precleanup(struct obd_device *obd, enum obd_cleanup_stage stage)
 		}
 		break;
 	}
-	case OBD_CLEANUP_EXPORTS:
-		rc = obd_llog_finish(obd, 0);
-		if (rc != 0)
-			CERROR("failed to cleanup llogging subsystems\n");
+	default:
 		break;
 	}
+
 	return rc;
 }
 
@@ -2254,11 +2248,12 @@ static int lov_fiemap(struct lov_obd *lov, __u32 keylen, void *key,
 	if (fm_end_offset == -EINVAL)
 		GOTO(out, rc = -EINVAL);
 
+	if (fiemap_count_to_size(fiemap->fm_extent_count) > *vallen)
+		fiemap->fm_extent_count = fiemap_size_to_count(*vallen);
 	if (fiemap->fm_extent_count == 0) {
 		get_num_extents = 1;
 		count_local = 0;
 	}
-
 	/* Check each stripe */
 	for (cur_stripe = start_stripe, i = 0; i < stripe_count;
 	     i++, cur_stripe = (cur_stripe + 1) % lsm->lsm_stripe_count) {
@@ -2808,8 +2803,6 @@ struct obd_ops lov_obd_ops = {
 	.o_get_info	    = lov_get_info,
 	.o_set_info_async      = lov_set_info_async,
 	.o_extent_calc	 = lov_extent_calc,
-	.o_llog_init	   = lov_llog_init,
-	.o_llog_finish	 = lov_llog_finish,
 	.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 d6b2cb45b93..992c80ab3d5 100644
--- a/drivers/staging/lustre/lustre/lov/lov_object.c
+++ b/drivers/staging/lustre/lustre/lov/lov_object.c
@@ -42,7 +42,6 @@
 #define DEBUG_SUBSYSTEM S_LOV
 
 #include "lov_cl_internal.h"
-#include <lustre_debug.h>
 
 /** \addtogroup lov
  *  @{
@@ -885,7 +884,7 @@ struct lu_object *lov_object_alloc(const struct lu_env *env,
 	struct lov_object *lov;
 	struct lu_object  *obj;
 
-	OBD_SLAB_ALLOC_PTR_GFP(lov, lov_object_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(lov, lov_object_kmem, GFP_NOFS);
 	if (lov != NULL) {
 		obj = lov2lu(lov);
 		lu_object_init(obj, NULL, dev);
diff --git a/drivers/staging/lustre/lustre/lov/lov_offset.c b/drivers/staging/lustre/lustre/lov/lov_offset.c
index 04863a7c5e0..379568f8245 100644
--- a/drivers/staging/lustre/lustre/lov/lov_offset.c
+++ b/drivers/staging/lustre/lustre/lov/lov_offset.c
@@ -39,7 +39,6 @@
 #include <linux/libcfs/libcfs.h>
 
 #include <obd_class.h>
-#include <obd_lov.h>
 
 #include "lov_internal.h"
 
diff --git a/drivers/staging/lustre/lustre/lov/lov_pack.c b/drivers/staging/lustre/lustre/lov/lov_pack.c
index 74200cf1b33..59ab7c30ffb 100644
--- a/drivers/staging/lustre/lustre/lov/lov_pack.c
+++ b/drivers/staging/lustre/lustre/lov/lov_pack.c
@@ -44,7 +44,6 @@
 
 #include <lustre_net.h>
 #include <obd.h>
-#include <obd_lov.h>
 #include <obd_class.h>
 #include <obd_support.h>
 #include <lustre/lustre_user.h>
@@ -177,8 +176,9 @@ int lov_packmd(struct obd_export *exp, struct lov_mds_md **lmmp,
 		 * Anyway, this is pretty inaccurate since ld_tgt_count now
 		 * represents max index and we should rely on the actual number
 		 * of OSTs instead */
-		stripe_count = lov_mds_md_stripecnt(lov->lov_ocd.ocd_max_easize,
-						    lmm_magic);
+		stripe_count = lov_mds_md_max_stripe_count(
+			lov->lov_ocd.ocd_max_easize, lmm_magic);
+
 		if (stripe_count > lov->desc.ld_tgt_count)
 			stripe_count = lov->desc.ld_tgt_count;
 	}
@@ -264,8 +264,8 @@ __u16 lov_get_stripecnt(struct lov_obd *lov, __u32 magic, __u16 stripe_count)
 	 * larger EA sizes */
 	if (lov->lov_ocd.ocd_connect_flags & OBD_CONNECT_MAX_EASIZE &&
 	    lov->lov_ocd.ocd_max_easize)
-		max_stripes = lov_mds_md_stripecnt(lov->lov_ocd.ocd_max_easize,
-						   magic);
+		max_stripes = lov_mds_md_max_stripe_count(
+			lov->lov_ocd.ocd_max_easize, magic);
 
 	if (stripe_count > max_stripes)
 		stripe_count = max_stripes;
diff --git a/drivers/staging/lustre/lustre/lov/lov_request.c b/drivers/staging/lustre/lustre/lov/lov_request.c
index a5481d7eb5d..bd6490d0129 100644
--- a/drivers/staging/lustre/lustre/lov/lov_request.c
+++ b/drivers/staging/lustre/lustre/lov/lov_request.c
@@ -39,7 +39,6 @@
 #include <linux/libcfs/libcfs.h>
 
 #include <obd_class.h>
-#include <obd_lov.h>
 #include <lustre/lustre_idl.h>
 
 #include "lov_internal.h"
diff --git a/drivers/staging/lustre/lustre/lov/lovsub_dev.c b/drivers/staging/lustre/lustre/lov/lovsub_dev.c
index 926c35a25ce..52fb6c162ad 100644
--- a/drivers/staging/lustre/lustre/lov/lovsub_dev.c
+++ b/drivers/staging/lustre/lustre/lov/lovsub_dev.c
@@ -146,7 +146,7 @@ static int lovsub_req_init(const struct lu_env *env, struct cl_device *dev,
 	struct lovsub_req *lsr;
 	int result;
 
-	OBD_SLAB_ALLOC_PTR_GFP(lsr, lovsub_req_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(lsr, lovsub_req_kmem, GFP_NOFS);
 	if (lsr != NULL) {
 		cl_req_slice_add(req, &lsr->lsrq_cl, dev, &lovsub_req_ops);
 		result = 0;
diff --git a/drivers/staging/lustre/lustre/lov/lovsub_lock.c b/drivers/staging/lustre/lustre/lov/lovsub_lock.c
index 80305aa6171..62b696d25d8 100644
--- a/drivers/staging/lustre/lustre/lov/lovsub_lock.c
+++ b/drivers/staging/lustre/lustre/lov/lovsub_lock.c
@@ -453,7 +453,7 @@ int lovsub_lock_init(const struct lu_env *env, struct cl_object *obj,
 	struct lovsub_lock *lsk;
 	int result;
 
-	OBD_SLAB_ALLOC_PTR_GFP(lsk, lovsub_lock_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(lsk, lovsub_lock_kmem, GFP_NOFS);
 	if (lsk != NULL) {
 		INIT_LIST_HEAD(&lsk->lss_parents);
 		cl_lock_slice_add(lock, &lsk->lss_cl, obj, &lovsub_lock_ops);
diff --git a/drivers/staging/lustre/lustre/lov/lovsub_object.c b/drivers/staging/lustre/lustre/lov/lovsub_object.c
index 89760b3bf3f..57e3629fcce 100644
--- a/drivers/staging/lustre/lustre/lov/lovsub_object.c
+++ b/drivers/staging/lustre/lustre/lov/lovsub_object.c
@@ -145,7 +145,7 @@ struct lu_object *lovsub_object_alloc(const struct lu_env *env,
 	struct lovsub_object *los;
 	struct lu_object     *obj;
 
-	OBD_SLAB_ALLOC_PTR_GFP(los, lovsub_object_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(los, lovsub_object_kmem, GFP_NOFS);
 	if (los != NULL) {
 		struct cl_object_header *hdr;
 
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_locks.c b/drivers/staging/lustre/lustre/mdc/mdc_locks.c
index 53022ec390f..1a8cd98ad6d 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_locks.c
+++ b/drivers/staging/lustre/lustre/mdc/mdc_locks.c
@@ -442,9 +442,9 @@ static struct ptlrpc_request *mdc_intent_unlink_pack(struct obd_export *exp,
 	mdc_unlink_pack(req, op_data);
 
 	req_capsule_set_size(&req->rq_pill, &RMF_MDT_MD, RCL_SERVER,
-			     obddev->u.cli.cl_max_mds_easize);
+			     obddev->u.cli.cl_default_mds_easize);
 	req_capsule_set_size(&req->rq_pill, &RMF_ACL, RCL_SERVER,
-			     obddev->u.cli.cl_max_mds_cookiesize);
+			     obddev->u.cli.cl_default_mds_cookiesize);
 	ptlrpc_request_set_replen(req);
 	return req;
 }
@@ -462,6 +462,7 @@ static struct ptlrpc_request *mdc_intent_getattr_pack(struct obd_export *exp,
 					       OBD_MD_FLRMTPERM : OBD_MD_FLACL);
 	struct ldlm_intent    *lit;
 	int		    rc;
+	int		    easize;
 
 	req = ptlrpc_request_alloc(class_exp2cliimp(exp),
 				   &RQF_LDLM_INTENT_GETATTR);
@@ -482,12 +483,15 @@ static struct ptlrpc_request *mdc_intent_getattr_pack(struct obd_export *exp,
 	lit = req_capsule_client_get(&req->rq_pill, &RMF_LDLM_INTENT);
 	lit->opc = (__u64)it->it_op;
 
+	if (obddev->u.cli.cl_default_mds_easize > 0)
+		easize = obddev->u.cli.cl_default_mds_easize;
+	else
+		easize = obddev->u.cli.cl_max_mds_easize;
+
 	/* pack the intended request */
-	mdc_getattr_pack(req, valid, it->it_flags, op_data,
-			 obddev->u.cli.cl_max_mds_easize);
+	mdc_getattr_pack(req, valid, it->it_flags, op_data, easize);
 
-	req_capsule_set_size(&req->rq_pill, &RMF_MDT_MD, RCL_SERVER,
-			     obddev->u.cli.cl_max_mds_easize);
+	req_capsule_set_size(&req->rq_pill, &RMF_MDT_MD, RCL_SERVER, easize);
 	if (client_is_remote(exp))
 		req_capsule_set_size(&req->rq_pill, &RMF_ACL, RCL_SERVER,
 				     sizeof(struct mdt_remote_perm));
@@ -528,7 +532,7 @@ static struct ptlrpc_request *mdc_intent_layout_pack(struct obd_export *exp,
 	layout->li_opc = LAYOUT_INTENT_ACCESS;
 
 	req_capsule_set_size(&req->rq_pill, &RMF_DLM_LVB, RCL_SERVER,
-			obd->u.cli.cl_max_mds_easize);
+			     obd->u.cli.cl_default_mds_easize);
 	ptlrpc_request_set_replen(req);
 	return req;
 }
@@ -893,7 +897,10 @@ resend:
 	mdc_put_rpc_lock(obddev->u.cli.cl_rpc_lock, it);
 
 	if (rc < 0) {
-		CERROR("ldlm_cli_enqueue: %d\n", rc);
+		CDEBUG_LIMIT((rc == -EACCES || rc == -EIDRM) ? D_INFO : D_ERROR,
+			     "%s: ldlm_cli_enqueue failed: rc = %d\n",
+			     obddev->obd_name, rc);
+
 		mdc_clear_replay_flag(req, rc);
 		ptlrpc_req_finished(req);
 		return rc;
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_reint.c b/drivers/staging/lustre/lustre/mdc/mdc_reint.c
index d79aa1641fe..08e80940ee4 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_reint.c
+++ b/drivers/staging/lustre/lustre/mdc/mdc_reint.c
@@ -199,7 +199,8 @@ int mdc_setattr(struct obd_export *exp, struct md_op_data *op_data,
 	*request = req;
 	if (rc && req->rq_commit_cb) {
 		/* Put an extra reference on \var mod on error case. */
-		obd_mod_put(*mod);
+		if (mod != NULL && *mod != NULL)
+			obd_mod_put(*mod);
 		req->rq_commit_cb(req);
 	}
 	return rc;
@@ -357,9 +358,9 @@ int mdc_unlink(struct obd_export *exp, struct md_op_data *op_data,
 	mdc_unlink_pack(req, op_data);
 
 	req_capsule_set_size(&req->rq_pill, &RMF_MDT_MD, RCL_SERVER,
-			     obd->u.cli.cl_max_mds_easize);
+			     obd->u.cli.cl_default_mds_easize);
 	req_capsule_set_size(&req->rq_pill, &RMF_LOGCOOKIES, RCL_SERVER,
-			     obd->u.cli.cl_max_mds_cookiesize);
+			     obd->u.cli.cl_default_mds_cookiesize);
 	ptlrpc_request_set_replen(req);
 
 	*request = req;
@@ -470,9 +471,9 @@ int mdc_rename(struct obd_export *exp, struct md_op_data *op_data,
 	mdc_rename_pack(req, op_data, old, oldlen, new, newlen);
 
 	req_capsule_set_size(&req->rq_pill, &RMF_MDT_MD, RCL_SERVER,
-			     obd->u.cli.cl_max_mds_easize);
+			     obd->u.cli.cl_default_mds_easize);
 	req_capsule_set_size(&req->rq_pill, &RMF_LOGCOOKIES, RCL_SERVER,
-			     obd->u.cli.cl_max_mds_cookiesize);
+			     obd->u.cli.cl_default_mds_cookiesize);
 	ptlrpc_request_set_replen(req);
 
 	rc = mdc_reint(req, obd->u.cli.cl_rpc_lock, LUSTRE_IMP_FULL);
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_request.c b/drivers/staging/lustre/lustre/mdc/mdc_request.c
index bde9f93c149..fca43cf1d67 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_request.c
+++ b/drivers/staging/lustre/lustre/mdc/mdc_request.c
@@ -903,9 +903,9 @@ int mdc_close(struct obd_export *exp, struct md_op_data *op_data,
 	mdc_close_pack(req, op_data);
 
 	req_capsule_set_size(&req->rq_pill, &RMF_MDT_MD, RCL_SERVER,
-			     obd->u.cli.cl_max_mds_easize);
+			     obd->u.cli.cl_default_mds_easize);
 	req_capsule_set_size(&req->rq_pill, &RMF_LOGCOOKIES, RCL_SERVER,
-			     obd->u.cli.cl_max_mds_cookiesize);
+			     obd->u.cli.cl_default_mds_cookiesize);
 
 	ptlrpc_request_set_replen(req);
 
@@ -2153,12 +2153,40 @@ int mdc_get_info(const struct lu_env *env, struct obd_export *exp,
 
 		if (*vallen != sizeof(int))
 			return -EINVAL;
-		mdsize = *(int*)val;
+		mdsize = *(int *)val;
 		if (mdsize > exp->exp_obd->u.cli.cl_max_mds_easize)
 			exp->exp_obd->u.cli.cl_max_mds_easize = mdsize;
 		max_easize = val;
 		*max_easize = exp->exp_obd->u.cli.cl_max_mds_easize;
 		return 0;
+	} else if (KEY_IS(KEY_DEFAULT_EASIZE)) {
+		int *default_easize;
+
+		if (*vallen != sizeof(int))
+			return -EINVAL;
+		default_easize = val;
+		*default_easize = exp->exp_obd->u.cli.cl_default_mds_easize;
+		return 0;
+	} else if (KEY_IS(KEY_MAX_COOKIESIZE)) {
+		int mdsize, *max_cookiesize;
+
+		if (*vallen != sizeof(int))
+			return -EINVAL;
+		mdsize = *(int *)val;
+		if (mdsize > exp->exp_obd->u.cli.cl_max_mds_cookiesize)
+			exp->exp_obd->u.cli.cl_max_mds_cookiesize = mdsize;
+		max_cookiesize = val;
+		*max_cookiesize = exp->exp_obd->u.cli.cl_max_mds_cookiesize;
+		return 0;
+	} else if (KEY_IS(KEY_DEFAULT_COOKIESIZE)) {
+		int *default_cookiesize;
+
+		if (*vallen != sizeof(int))
+			return -EINVAL;
+		default_cookiesize = val;
+		*default_cookiesize =
+			exp->exp_obd->u.cli.cl_default_mds_cookiesize;
+		return 0;
 	} else if (KEY_IS(KEY_CONN_DATA)) {
 		struct obd_import *imp = class_exp2cliimp(exp);
 		struct obd_connect_data *data = val;
@@ -2439,11 +2467,15 @@ err_rpc_lock:
 }
 
 /* Initialize the default and maximum LOV EA and cookie sizes.  This allows
- * us to make MDS RPCs with large enough reply buffers to hold the
- * maximum-sized (= maximum striped) EA and cookie without having to
- * calculate this (via a call into the LOV + OSCs) each time we make an RPC. */
+ * us to make MDS RPCs with large enough reply buffers to hold a default
+ * sized EA and cookie without having to calculate this (via a call into the
+ * LOV + OSCs) each time we make an RPC.  The maximum size is also tracked
+ * but not used to avoid wastefully vmalloc()'ing large reply buffers when
+ * a large number of stripes is possible.  If a larger reply buffer is
+ * required it will be reallocated in the ptlrpc layer due to overflow.
+ */
 static int mdc_init_ea_size(struct obd_export *exp, int easize,
-		     int def_easize, int cookiesize)
+			    int def_easize, int cookiesize, int def_cookiesize)
 {
 	struct obd_device *obd = exp->exp_obd;
 	struct client_obd *cli = &obd->u.cli;
@@ -2457,6 +2489,9 @@ static int mdc_init_ea_size(struct obd_export *exp, int easize,
 	if (cli->cl_max_mds_cookiesize < cookiesize)
 		cli->cl_max_mds_cookiesize = cookiesize;
 
+	if (cli->cl_default_mds_cookiesize < def_cookiesize)
+		cli->cl_default_mds_cookiesize = def_cookiesize;
+
 	return 0;
 }
 
diff --git a/drivers/staging/lustre/lustre/mgc/lproc_mgc.c b/drivers/staging/lustre/lustre/mgc/lproc_mgc.c
index 1506af13f1b..6c877c5a6a7 100644
--- a/drivers/staging/lustre/lustre/mgc/lproc_mgc.c
+++ b/drivers/staging/lustre/lustre/mgc/lproc_mgc.c
@@ -56,21 +56,21 @@ static int mgc_ir_state_seq_show(struct seq_file *m, void *v)
 LPROC_SEQ_FOPS_RO(mgc_ir_state);
 
 static struct lprocfs_vars lprocfs_mgc_obd_vars[] = {
-	{ "uuid",	     &mgc_uuid_fops,	  0, 0 },
-	{ "ping",	     &mgc_ping_fops,      0, 0222 },
-	{ "connect_flags",   &mgc_connect_flags_fops, 0, 0 },
-	{ "mgs_server_uuid", &mgc_server_uuid_fops,   0, 0 },
-	{ "mgs_conn_uuid",   &mgc_conn_uuid_fops,     0, 0 },
-	{ "import",	     &mgc_import_fops,	0, 0 },
-	{ "state",	     &mgc_state_fops,	 0, 0 },
-	{ "ir_state",	     &mgc_ir_state_fops,  0, 0 },
-	{ 0 }
+	{ "uuid",	     &mgc_uuid_fops,	  NULL, 0 },
+	{ "ping",	     &mgc_ping_fops,      NULL, 0222 },
+	{ "connect_flags",   &mgc_connect_flags_fops, NULL, 0 },
+	{ "mgs_server_uuid", &mgc_server_uuid_fops,   NULL, 0 },
+	{ "mgs_conn_uuid",   &mgc_conn_uuid_fops,     NULL, 0 },
+	{ "import",	     &mgc_import_fops,	NULL, 0 },
+	{ "state",	     &mgc_state_fops,	 NULL, 0 },
+	{ "ir_state",	     &mgc_ir_state_fops,  NULL, 0 },
+	{ NULL }
 };
 
 LPROC_SEQ_FOPS_RO_TYPE(mgc, numrefs);
 static struct lprocfs_vars lprocfs_mgc_module_vars[] = {
-	{ "num_refs",	&mgc_numrefs_fops,       0, 0 },
-	{ 0 }
+	{ "num_refs",	&mgc_numrefs_fops,       NULL, 0 },
+	{ NULL }
 };
 
 void lprocfs_mgc_init_vars(struct lprocfs_static_vars *lvars)
diff --git a/drivers/staging/lustre/lustre/mgc/mgc_request.c b/drivers/staging/lustre/lustre/mgc/mgc_request.c
index de9fb1433ed..a806aeffe02 100644
--- a/drivers/staging/lustre/lustre/mgc/mgc_request.c
+++ b/drivers/staging/lustre/lustre/mgc/mgc_request.c
@@ -672,8 +672,8 @@ static int mgc_fs_setup(struct obd_device *obd, struct super_block *sb)
 	if (env == NULL)
 		return -ENOMEM;
 
-	/* The mgc fs exclusion sem. Only one fs can be setup at a time. */
-	down(&cli->cl_mgc_sem);
+	/* The mgc fs exclusion mutex. Only one fs can be setup at a time. */
+	mutex_lock(&cli->cl_mgc_mutex);
 
 	cfs_cleanup_group_info();
 
@@ -727,7 +727,7 @@ out_los:
 	if (rc < 0) {
 		local_oid_storage_fini(env, cli->cl_mgc_los);
 		cli->cl_mgc_los = NULL;
-		up(&cli->cl_mgc_sem);
+		mutex_unlock(&cli->cl_mgc_mutex);
 	}
 out_env:
 	lu_env_fini(env);
@@ -759,7 +759,7 @@ static int mgc_fs_cleanup(struct obd_device *obd)
 
 unlock:
 	class_decref(obd, "mgc_fs", obd);
-	up(&cli->cl_mgc_sem);
+	mutex_unlock(&cli->cl_mgc_mutex);
 
 	return 0;
 }
diff --git a/drivers/staging/lustre/lustre/obdclass/cl_lock.c b/drivers/staging/lustre/lustre/obdclass/cl_lock.c
index d795cef3f16..df77c4fc0ea 100644
--- a/drivers/staging/lustre/lustre/obdclass/cl_lock.c
+++ b/drivers/staging/lustre/lustre/obdclass/cl_lock.c
@@ -139,7 +139,7 @@ static void cl_lock_trace0(int level, const struct lu_env *env,
 	       func, line);
 }
 #define cl_lock_trace(level, env, prefix, lock)			 \
-	cl_lock_trace0(level, env, prefix, lock, __FUNCTION__, __LINE__)
+	cl_lock_trace0(level, env, prefix, lock, __func__, __LINE__)
 
 #define RETIP ((unsigned long)__builtin_return_address(0))
 
@@ -360,7 +360,7 @@ static struct cl_lock *cl_lock_alloc(const struct lu_env *env,
 	struct cl_lock	  *lock;
 	struct lu_object_header *head;
 
-	OBD_SLAB_ALLOC_PTR_GFP(lock, cl_lock_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(lock, cl_lock_kmem, GFP_NOFS);
 	if (lock != NULL) {
 		atomic_set(&lock->cll_ref, 1);
 		lock->cll_descr = *descr;
@@ -478,7 +478,7 @@ static struct cl_lock *cl_lock_lookup(const struct lu_env *env,
 	struct cl_object_header *head;
 
 	head = cl_object_header(obj);
-	LINVRNT(spin_is_locked(&head->coh_lock_guard));
+	assert_spin_locked(&head->coh_lock_guard);
 	CS_LOCK_INC(obj, lookup);
 	list_for_each_entry(lock, &head->coh_locks, cll_linkage) {
 		int matched;
@@ -533,6 +533,7 @@ static struct cl_lock *cl_lock_find(const struct lu_env *env,
 			spin_lock(&head->coh_lock_guard);
 			ghost = cl_lock_lookup(env, obj, io, need);
 			if (ghost == NULL) {
+				cl_lock_get_trust(lock);
 				list_add_tail(&lock->cll_linkage,
 						  &head->coh_locks);
 				spin_unlock(&head->coh_lock_guard);
@@ -791,15 +792,22 @@ static void cl_lock_delete0(const struct lu_env *env, struct cl_lock *lock)
 	LINVRNT(cl_lock_invariant(env, lock));
 
 	if (lock->cll_state < CLS_FREEING) {
+		bool in_cache;
+
 		LASSERT(lock->cll_state != CLS_INTRANSIT);
 		cl_lock_state_set(env, lock, CLS_FREEING);
 
 		head = cl_object_header(lock->cll_descr.cld_obj);
 
 		spin_lock(&head->coh_lock_guard);
-		list_del_init(&lock->cll_linkage);
+		in_cache = !list_empty(&lock->cll_linkage);
+		if (in_cache)
+			list_del_init(&lock->cll_linkage);
 		spin_unlock(&head->coh_lock_guard);
 
+		if (in_cache) /* coh_locks cache holds a refcount. */
+			cl_lock_put(env, lock);
+
 		/*
 		 * From now on, no new references to this lock can be acquired
 		 * by cl_lock_lookup().
diff --git a/drivers/staging/lustre/lustre/obdclass/cl_object.c b/drivers/staging/lustre/lustre/obdclass/cl_object.c
index 0fc256f59e9..41cbc95b916 100644
--- a/drivers/staging/lustre/lustre/obdclass/cl_object.c
+++ b/drivers/staging/lustre/lustre/obdclass/cl_object.c
@@ -220,7 +220,7 @@ int cl_object_attr_get(const struct lu_env *env, struct cl_object *obj,
 	struct lu_object_header *top;
 	int result;
 
-	LASSERT(spin_is_locked(cl_object_attr_guard(obj)));
+	assert_spin_locked(cl_object_attr_guard(obj));
 
 	top = obj->co_lu.lo_header;
 	result = 0;
@@ -251,7 +251,7 @@ int cl_object_attr_set(const struct lu_env *env, struct cl_object *obj,
 	struct lu_object_header *top;
 	int result;
 
-	LASSERT(spin_is_locked(cl_object_attr_guard(obj)));
+	assert_spin_locked(cl_object_attr_guard(obj));
 
 	top = obj->co_lu.lo_header;
 	result = 0;
@@ -684,7 +684,7 @@ static struct lu_env *cl_env_new(__u32 ctx_tags, __u32 ses_tags, void *debug)
 	struct lu_env *env;
 	struct cl_env *cle;
 
-	OBD_SLAB_ALLOC_PTR_GFP(cle, cl_env_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(cle, cl_env_kmem, GFP_NOFS);
 	if (cle != NULL) {
 		int rc;
 
diff --git a/drivers/staging/lustre/lustre/obdclass/cl_page.c b/drivers/staging/lustre/lustre/obdclass/cl_page.c
index 2a5ce376e57..1b616e4fe14 100644
--- a/drivers/staging/lustre/lustre/obdclass/cl_page.c
+++ b/drivers/staging/lustre/lustre/obdclass/cl_page.c
@@ -130,7 +130,7 @@ struct cl_page *cl_page_lookup(struct cl_object_header *hdr, pgoff_t index)
 {
 	struct cl_page *page;
 
-	LASSERT(spin_is_locked(&hdr->coh_page_guard));
+	assert_spin_locked(&hdr->coh_page_guard);
 
 	page = radix_tree_lookup(&hdr->coh_tree, index);
 	if (page != NULL)
@@ -292,7 +292,7 @@ static struct cl_page *cl_page_alloc(const struct lu_env *env,
 	struct lu_object_header *head;
 
 	OBD_ALLOC_GFP(page, cl_object_header(o)->coh_page_bufsize,
-			__GFP_IO);
+			GFP_NOFS);
 	if (page != NULL) {
 		int result = 0;
 		atomic_set(&page->cp_ref, 1);
diff --git a/drivers/staging/lustre/lustre/obdclass/class_obd.c b/drivers/staging/lustre/lustre/obdclass/class_obd.c
index c93131e0d2d..dde04b767a6 100644
--- a/drivers/staging/lustre/lustre/obdclass/class_obd.c
+++ b/drivers/staging/lustre/lustre/obdclass/class_obd.c
@@ -102,23 +102,17 @@ EXPORT_SYMBOL(obd_dirty_transit_pages);
 char obd_jobid_var[JOBSTATS_JOBID_VAR_MAX_LEN + 1] = JOBSTATS_DISABLE;
 EXPORT_SYMBOL(obd_jobid_var);
 
-/* Get jobid of current process by reading the environment variable
- * stored in between the "env_start" & "env_end" of task struct.
- *
- * TODO:
- * It's better to cache the jobid for later use if there is any
- * efficient way, the cl_env code probably could be reused for this
- * purpose.
+char obd_jobid_node[JOBSTATS_JOBID_SIZE + 1];
+
+/* Get jobid of current process from stored variable or calculate
+ * it from pid and user_id.
  *
- * If some job scheduler doesn't store jobid in the "env_start/end",
- * then an upcall could be issued here to get the jobid by utilizing
- * the userspace tools/api. Then, the jobid must be cached.
+ * Historically this was also done by reading the environment variable
+ * stored in between the "env_start" & "env_end" of task struct.
+ * This is now deprecated.
  */
 int lustre_get_jobid(char *jobid)
 {
-	int jobid_len = JOBSTATS_JOBID_SIZE;
-	int rc = 0;
-
 	memset(jobid, 0, JOBSTATS_JOBID_SIZE);
 	/* Jobstats isn't enabled */
 	if (strcmp(obd_jobid_var, JOBSTATS_DISABLE) == 0)
@@ -132,31 +126,13 @@ int lustre_get_jobid(char *jobid)
 		return 0;
 	}
 
-	rc = cfs_get_environ(obd_jobid_var, jobid, &jobid_len);
-	if (rc) {
-		if (rc == -EOVERFLOW) {
-			/* For the PBS_JOBID and LOADL_STEP_ID keys (which are
-			 * variable length strings instead of just numbers), it
-			 * might make sense to keep the unique parts for JobID,
-			 * instead of just returning an error.  That means a
-			 * larger temp buffer for cfs_get_environ(), then
-			 * truncating the string at some separator to fit into
-			 * the specified jobid_len.  Fix later if needed. */
-			static bool printed;
-			if (unlikely(!printed)) {
-				LCONSOLE_ERROR_MSG(0x16b, "%s value too large "
-						   "for JobID buffer (%d)\n",
-						   obd_jobid_var, jobid_len);
-				printed = true;
-			}
-		} else {
-			CDEBUG((rc == -ENOENT || rc == -EINVAL ||
-				rc == -EDEADLK) ? D_INFO : D_ERROR,
-			       "Get jobid for (%s) failed: rc = %d\n",
-			       obd_jobid_var, rc);
-		}
+	/* Whole node dedicated to single job */
+	if (strcmp(obd_jobid_var, JOBSTATS_NODELOCAL) == 0) {
+		strcpy(jobid, obd_jobid_node);
+		return 0;
 	}
-	return rc;
+
+	return -ENOENT;
 }
 EXPORT_SYMBOL(lustre_get_jobid);
 
diff --git a/drivers/staging/lustre/lustre/obdclass/debug.c b/drivers/staging/lustre/lustre/obdclass/debug.c
index 15f71bbb727..a4e7e754177 100644
--- a/drivers/staging/lustre/lustre/obdclass/debug.c
+++ b/drivers/staging/lustre/lustre/obdclass/debug.c
@@ -56,18 +56,6 @@ void dump_lniobuf(struct niobuf_local *nb)
 }
 EXPORT_SYMBOL(dump_lniobuf);
 
-void dump_lsm(int level, struct lov_stripe_md *lsm)
-{
-	CDEBUG(level, "lsm %p, objid "DOSTID", maxbytes "LPX64", magic 0x%08X,"
-	       " stripe_size %u, stripe_count %u, refc: %d,"
-	       " layout_gen %u, pool ["LOV_POOLNAMEF"]\n", lsm,
-	       POSTID(&lsm->lsm_oi), lsm->lsm_maxbytes, lsm->lsm_magic,
-	       lsm->lsm_stripe_size, lsm->lsm_stripe_count,
-	       atomic_read(&lsm->lsm_refc), lsm->lsm_layout_gen,
-	       lsm->lsm_pool_name);
-}
-EXPORT_SYMBOL(dump_lsm);
-
 #define LPDS sizeof(__u64)
 int block_debug_setup(void *addr, int len, __u64 off, __u64 id)
 {
diff --git a/drivers/staging/lustre/lustre/obdclass/genops.c b/drivers/staging/lustre/lustre/obdclass/genops.c
index f2bdea33041..3210ad8184b 100644
--- a/drivers/staging/lustre/lustre/obdclass/genops.c
+++ b/drivers/staging/lustre/lustre/obdclass/genops.c
@@ -72,7 +72,7 @@ static struct obd_device *obd_device_alloc(void)
 {
 	struct obd_device *obd;
 
-	OBD_SLAB_ALLOC_PTR_GFP(obd, obd_device_cachep, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(obd, obd_device_cachep, GFP_NOFS);
 	if (obd != NULL) {
 		obd->obd_magic = OBD_DEVICE_MAGIC;
 	}
@@ -615,13 +615,13 @@ int class_notify_sptlrpc_conf(const char *fsname, int namelen)
 		if (strncmp(obd->obd_name, fsname, namelen))
 			continue;
 
-		class_incref(obd, __FUNCTION__, obd);
+		class_incref(obd, __func__, obd);
 		read_unlock(&obd_dev_lock);
 		rc2 = obd_set_info_async(NULL, obd->obd_self_export,
 					 sizeof(KEY_SPTLRPC_CONF),
 					 KEY_SPTLRPC_CONF, 0, NULL, NULL);
 		rc = rc ? rc : rc2;
-		class_decref(obd, __FUNCTION__, obd);
+		class_decref(obd, __func__, obd);
 		read_lock(&obd_dev_lock);
 	}
 	read_unlock(&obd_dev_lock);
diff --git a/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c b/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c
index ba20776ebfa..bdf2eed2952 100644
--- a/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c
+++ b/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c
@@ -122,6 +122,8 @@ int obd_ioctl_getdata(char **buf, int *len, void *arg)
 		OBD_FREE_LARGE(*buf, hdr.ioc_len);
 		return err;
 	}
+	if (hdr.ioc_len != data->ioc_len)
+		return -EINVAL;
 
 	if (obd_ioctl_is_invalid(data)) {
 		CERROR("ioctl not correctly formatted\n");
@@ -244,7 +246,7 @@ static int obd_proc_health_seq_show(struct seq_file *m, void *v)
 		if (obd->obd_stopping)
 			continue;
 
-		class_incref(obd, __FUNCTION__, current);
+		class_incref(obd, __func__, current);
 		read_unlock(&obd_dev_lock);
 
 		if (obd_health_check(NULL, obd)) {
@@ -252,7 +254,7 @@ static int obd_proc_health_seq_show(struct seq_file *m, void *v)
 				      obd->obd_name);
 			rc++;
 		}
-		class_decref(obd, __FUNCTION__, current);
+		class_decref(obd, __func__, current);
 		read_lock(&obd_dev_lock);
 	}
 	read_unlock(&obd_dev_lock);
@@ -277,12 +279,44 @@ static ssize_t obd_proc_jobid_var_seq_write(struct file *file, const char *buffe
 		return -EINVAL;
 
 	memset(obd_jobid_var, 0, JOBSTATS_JOBID_VAR_MAX_LEN + 1);
+
+	/* This might leave the var invalid on error, which is probably fine.*/
+	if (copy_from_user(obd_jobid_var, buffer, count))
+		return -EFAULT;
+
 	/* Trim the trailing '\n' if any */
-	memcpy(obd_jobid_var, buffer, count - (buffer[count - 1] == '\n'));
+	if (obd_jobid_var[count - 1] == '\n')
+		obd_jobid_var[count - 1] = 0;
+
 	return count;
 }
 LPROC_SEQ_FOPS(obd_proc_jobid_var);
 
+static int obd_proc_jobid_name_seq_show(struct seq_file *m, void *v)
+{
+	return seq_printf(m, "%s\n", obd_jobid_var);
+}
+
+static ssize_t obd_proc_jobid_name_seq_write(struct file *file,
+					     const char __user *buffer,
+					     size_t count, loff_t *off)
+{
+	if (!count || count > JOBSTATS_JOBID_SIZE)
+		return -EINVAL;
+
+	if (copy_from_user(obd_jobid_node, buffer, count))
+		return -EFAULT;
+
+	obd_jobid_node[count] = 0;
+
+	/* Trim the trailing '\n' if any */
+	if (obd_jobid_node[count - 1] == '\n')
+		obd_jobid_node[count - 1] = 0;
+
+	return count;
+}
+LPROC_SEQ_FOPS(obd_proc_jobid_name);
+
 /* Root for /proc/fs/lustre */
 struct proc_dir_entry *proc_lustre_root = NULL;
 EXPORT_SYMBOL(proc_lustre_root);
@@ -292,6 +326,8 @@ struct lprocfs_vars lprocfs_base[] = {
 	{ "pinger", &obd_proc_pinger_fops },
 	{ "health_check", &obd_proc_health_fops },
 	{ "jobid_var", &obd_proc_jobid_var_fops },
+	{ .name =	"jobid_name",
+	  .fops =	&obd_proc_jobid_name_fops},
 	{ 0 }
 };
 
diff --git a/drivers/staging/lustre/lustre/obdclass/llog_ioctl.c b/drivers/staging/lustre/lustre/obdclass/llog_ioctl.c
index da558a5dc92..e192aab193b 100644
--- a/drivers/staging/lustre/lustre/obdclass/llog_ioctl.c
+++ b/drivers/staging/lustre/lustre/obdclass/llog_ioctl.c
@@ -385,7 +385,7 @@ int llog_ioctl(const struct lu_env *env, struct llog_ctxt *ctxt, int cmd,
 			GOTO(out_close, rc = -EINVAL);
 		}
 
-		if (data->ioc_inlbuf2 > 0) {
+		if (data->ioc_inllen2 > 0) {
 			/* remove indicate log from the catalog */
 			rc = str2logid(&plain, data->ioc_inlbuf2,
 				       data->ioc_inllen2);
diff --git a/drivers/staging/lustre/lustre/obdclass/llog_obd.c b/drivers/staging/lustre/lustre/obdclass/llog_obd.c
index 71817af7539..2c6d81eb5c6 100644
--- a/drivers/staging/lustre/lustre/obdclass/llog_obd.c
+++ b/drivers/staging/lustre/lustre/obdclass/llog_obd.c
@@ -226,34 +226,7 @@ int llog_sync(struct llog_ctxt *ctxt, struct obd_export *exp, int flags)
 }
 EXPORT_SYMBOL(llog_sync);
 
-int llog_obd_add(const struct lu_env *env, struct llog_ctxt *ctxt,
-		 struct llog_rec_hdr *rec, struct lov_stripe_md *lsm,
-		 struct llog_cookie *logcookies, int numcookies)
-{
-	int raised, rc;
-
-	if (!ctxt) {
-		CERROR("No ctxt\n");
-		return -ENODEV;
-	}
-
-	if (ctxt->loc_flags & LLOG_CTXT_FLAG_UNINITIALIZED)
-		return -ENXIO;
-
-	CTXT_CHECK_OP(ctxt, obd_add, -EOPNOTSUPP);
-	raised = cfs_cap_raised(CFS_CAP_SYS_RESOURCE);
-	if (!raised)
-		cfs_cap_raise(CFS_CAP_SYS_RESOURCE);
-	rc = CTXTP(ctxt, obd_add)(env, ctxt, rec, lsm, logcookies,
-				  numcookies);
-	if (!raised)
-		cfs_cap_lower(CFS_CAP_SYS_RESOURCE);
-	return rc;
-}
-EXPORT_SYMBOL(llog_obd_add);
-
 int llog_cancel(const struct lu_env *env, struct llog_ctxt *ctxt,
-		struct lov_stripe_md *lsm, int count,
 		struct llog_cookie *cookies, int flags)
 {
 	int rc;
@@ -264,7 +237,7 @@ int llog_cancel(const struct lu_env *env, struct llog_ctxt *ctxt,
 	}
 
 	CTXT_CHECK_OP(ctxt, cancel, -EOPNOTSUPP);
-	rc = CTXTP(ctxt, cancel)(env, ctxt, lsm, count, cookies, flags);
+	rc = CTXTP(ctxt, cancel)(env, ctxt, cookies, flags);
 	return rc;
 }
 EXPORT_SYMBOL(llog_cancel);
diff --git a/drivers/staging/lustre/lustre/obdclass/obd_mount.c b/drivers/staging/lustre/lustre/obdclass/obd_mount.c
index 6f8ba54e966..a034aee37fc 100644
--- a/drivers/staging/lustre/lustre/obdclass/obd_mount.c
+++ b/drivers/staging/lustre/lustre/obdclass/obd_mount.c
@@ -187,7 +187,7 @@ int lustre_start_simple(char *obdname, char *type, char *uuid,
 	int rc;
 	CDEBUG(D_MOUNT, "Starting obd %s (typ=%s)\n", obdname, type);
 
-	rc = do_lcfg(obdname, 0, LCFG_ATTACH, type, uuid, 0, 0);
+	rc = do_lcfg(obdname, 0, LCFG_ATTACH, type, uuid, NULL, NULL);
 	if (rc) {
 		CERROR("%s attach error %d\n", obdname, rc);
 		return rc;
@@ -195,7 +195,7 @@ int lustre_start_simple(char *obdname, char *type, char *uuid,
 	rc = do_lcfg(obdname, 0, LCFG_SETUP, s1, s2, s3, s4);
 	if (rc) {
 		CERROR("%s setup error %d\n", obdname, rc);
-		do_lcfg(obdname, 0, LCFG_DETACH, 0, 0, 0, 0);
+		do_lcfg(obdname, 0, LCFG_DETACH, NULL, NULL, NULL, NULL);
 	}
 	return rc;
 }
@@ -337,7 +337,8 @@ int lustre_start_mgc(struct super_block *sb)
 			lnet_process_id_t id;
 			while ((rc = LNetGetId(i++, &id)) != -ENOENT) {
 				rc = do_lcfg(mgcname, id.nid,
-					     LCFG_ADD_UUID, niduuid, 0,0,0);
+					     LCFG_ADD_UUID, niduuid,
+					     NULL, NULL, NULL);
 			}
 		} else {
 			/* Use mgsnode= nids */
@@ -351,7 +352,8 @@ int lustre_start_mgc(struct super_block *sb)
 			}
 			while (class_parse_nid(ptr, &nid, &ptr) == 0) {
 				rc = do_lcfg(mgcname, nid,
-					     LCFG_ADD_UUID, niduuid, 0,0,0);
+					     LCFG_ADD_UUID, niduuid,
+					     NULL, NULL, NULL);
 				i++;
 			}
 		}
@@ -360,7 +362,7 @@ int lustre_start_mgc(struct super_block *sb)
 		ptr = lsi->lsi_lmd->lmd_dev;
 		while (class_parse_nid(ptr, &nid, &ptr) == 0) {
 			rc = do_lcfg(mgcname, nid,
-				     LCFG_ADD_UUID, niduuid, 0,0,0);
+				     LCFG_ADD_UUID, niduuid, NULL, NULL, NULL);
 			i++;
 			/* Stop at the first failover nid */
 			if (*ptr == ':')
@@ -381,7 +383,7 @@ int lustre_start_mgc(struct super_block *sb)
 	/* Start the MGC */
 	rc = lustre_start_simple(mgcname, LUSTRE_MGC_NAME,
 				 (char *)uuid->uuid, LUSTRE_MGS_OBDNAME,
-				 niduuid, 0, 0);
+				 niduuid, NULL, NULL);
 	OBD_FREE_PTR(uuid);
 	if (rc)
 		GOTO(out_free, rc);
@@ -396,13 +398,13 @@ int lustre_start_mgc(struct super_block *sb)
 		while (class_parse_nid_quiet(ptr, &nid, &ptr) == 0) {
 			j++;
 			rc = do_lcfg(mgcname, nid,
-				     LCFG_ADD_UUID, niduuid, 0,0,0);
+				     LCFG_ADD_UUID, niduuid, NULL, NULL, NULL);
 			if (*ptr == ':')
 				break;
 		}
 		if (j > 0) {
 			rc = do_lcfg(mgcname, 0, LCFG_ADD_CONN,
-				     niduuid, 0, 0, 0);
+				     niduuid, NULL, NULL, NULL);
 			i++;
 		} else {
 			/* at ":/fsname" */
@@ -480,7 +482,7 @@ static int lustre_stop_mgc(struct super_block *sb)
 {
 	struct lustre_sb_info *lsi = s2lsi(sb);
 	struct obd_device *obd;
-	char *niduuid = 0, *ptr = 0;
+	char *niduuid = NULL, *ptr = NULL;
 	int i, rc = 0, len = 0;
 
 	if (!lsi)
@@ -532,7 +534,7 @@ static int lustre_stop_mgc(struct super_block *sb)
 	for (i = 0; i < lsi->lsi_lmd->lmd_mgs_failnodes; i++) {
 		sprintf(ptr, "_%x", i);
 		rc = do_lcfg(LUSTRE_MGC_OBDNAME, 0, LCFG_DEL_UUID,
-			     niduuid, 0, 0, 0);
+			     niduuid, NULL, NULL, NULL);
 		if (rc)
 			CERROR("del MDC UUID %s failed: rc = %d\n",
 			       niduuid, rc);
diff --git a/drivers/staging/lustre/lustre/obdecho/echo_client.c b/drivers/staging/lustre/lustre/obdecho/echo_client.c
index 754aa8e5566..cdc46719bbd 100644
--- a/drivers/staging/lustre/lustre/obdecho/echo_client.c
+++ b/drivers/staging/lustre/lustre/obdecho/echo_client.c
@@ -47,7 +47,6 @@
 #include <lustre_fid.h>
 #include <lustre_acl.h>
 #include <lustre_net.h>
-#include <obd_lov.h>
 
 #include "echo_internal.h"
 
@@ -428,7 +427,7 @@ static int echo_lock_init(const struct lu_env *env,
 {
 	struct echo_lock *el;
 
-	OBD_SLAB_ALLOC_PTR_GFP(el, echo_lock_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(el, echo_lock_kmem, GFP_NOFS);
 	if (el != NULL) {
 		cl_lock_slice_add(lock, &el->el_cl, obj, &echo_lock_ops);
 		el->el_object = cl2echo_obj(obj);
@@ -599,7 +598,7 @@ static struct lu_object *echo_object_alloc(const struct lu_env *env,
 
 	/* we're the top dev. */
 	LASSERT(hdr == NULL);
-	OBD_SLAB_ALLOC_PTR_GFP(eco, echo_object_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(eco, echo_object_kmem, GFP_NOFS);
 	if (eco != NULL) {
 		struct cl_object_header *hdr = &eco->eo_hdr;
 
@@ -663,7 +662,7 @@ static void *echo_thread_key_init(const struct lu_context *ctx,
 {
 	struct echo_thread_info *info;
 
-	OBD_SLAB_ALLOC_PTR_GFP(info, echo_thread_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(info, echo_thread_kmem, GFP_NOFS);
 	if (info == NULL)
 		info = ERR_PTR(-ENOMEM);
 	return info;
@@ -693,7 +692,7 @@ static void *echo_session_key_init(const struct lu_context *ctx,
 {
 	struct echo_session_info *session;
 
-	OBD_SLAB_ALLOC_PTR_GFP(session, echo_session_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(session, echo_session_kmem, GFP_NOFS);
 	if (session == NULL)
 		session = ERR_PTR(-ENOMEM);
 	return session;
@@ -2432,7 +2431,7 @@ static int echo_client_kbrw(struct echo_device *ed, int rw, struct obdo *oa,
 	int		     i;
 	int		     rc;
 	int		     verify;
-	int		     gfp_mask;
+	gfp_t		     gfp_mask;
 	int		     brw_flags = 0;
 
 	verify = (ostid_id(&oa->o_oi) != ECHO_PERSISTENT_OBJID &&
diff --git a/drivers/staging/lustre/lustre/obdecho/lproc_echo.c b/drivers/staging/lustre/lustre/obdecho/lproc_echo.c
index b9abac1c4dc..8fe9245a8aa 100644
--- a/drivers/staging/lustre/lustre/obdecho/lproc_echo.c
+++ b/drivers/staging/lustre/lustre/obdecho/lproc_echo.c
@@ -39,13 +39,13 @@
 #ifdef LPROCFS
 LPROC_SEQ_FOPS_RO_TYPE(echo, uuid);
 static struct lprocfs_vars lprocfs_echo_obd_vars[] = {
-	{ "uuid",	 &echo_uuid_fops,	0, 0 },
+	{ "uuid",	 &echo_uuid_fops,	NULL, 0 },
 	{ 0 }
 };
 
 LPROC_SEQ_FOPS_RO_TYPE(echo, numrefs);
 static struct lprocfs_vars lprocfs_echo_module_vars[] = {
-	{ "num_refs",     &echo_numrefs_fops,     0, 0 },
+	{ "num_refs",     &echo_numrefs_fops,     NULL, 0 },
 	{ 0 }
 };
 
diff --git a/drivers/staging/lustre/lustre/osc/lproc_osc.c b/drivers/staging/lustre/lustre/osc/lproc_osc.c
index 0b59fc16c50..0cadfcd9226 100644
--- a/drivers/staging/lustre/lustre/osc/lproc_osc.c
+++ b/drivers/staging/lustre/lustre/osc/lproc_osc.c
@@ -174,15 +174,25 @@ static int osc_cached_mb_seq_show(struct seq_file *m, void *v)
 }
 
 /* shrink the number of caching pages to a specific number */
-static ssize_t osc_cached_mb_seq_write(struct file *file, const char *buffer,
-				   size_t count, loff_t *off)
+static ssize_t osc_cached_mb_seq_write(struct file *file,
+				       const char __user *buffer,
+				       size_t count, loff_t *off)
 {
 	struct obd_device *dev = ((struct seq_file *)file->private_data)->private;
 	struct client_obd *cli = &dev->u.cli;
 	int pages_number, mult, rc;
+	char kernbuf[128];
+
+	if (count >= sizeof(kernbuf))
+		return -EINVAL;
+
+	if (copy_from_user(kernbuf, buffer, count))
+		return -EFAULT;
+	kernbuf[count] = 0;
 
 	mult = 1 << (20 - PAGE_CACHE_SHIFT);
-	buffer = lprocfs_find_named_value(buffer, "used_mb:", &count);
+	buffer += lprocfs_find_named_value(kernbuf, "used_mb:", &count) -
+		  kernbuf;
 	rc = lprocfs_write_frac_helper(buffer, count, &pages_number, mult);
 	if (rc)
 		return rc;
@@ -515,44 +525,44 @@ LPROC_SEQ_FOPS_RW_TYPE(osc, import);
 LPROC_SEQ_FOPS_RW_TYPE(osc, pinger_recov);
 
 static struct lprocfs_vars lprocfs_osc_obd_vars[] = {
-	{ "uuid",	     &osc_uuid_fops,	0, 0 },
-	{ "ping",	     &osc_ping_fops,    0, 0222 },
-	{ "connect_flags",   &osc_connect_flags_fops, 0, 0 },
-	{ "blocksize",       &osc_blksize_fops,     0, 0 },
-	{ "kbytestotal",     &osc_kbytestotal_fops, 0, 0 },
-	{ "kbytesfree",      &osc_kbytesfree_fops,  0, 0 },
-	{ "kbytesavail",     &osc_kbytesavail_fops, 0, 0 },
-	{ "filestotal",      &osc_filestotal_fops,  0, 0 },
-	{ "filesfree",       &osc_filesfree_fops,   0, 0 },
-	//{ "filegroups",      lprocfs_rd_filegroups,  0, 0 },
-	{ "ost_server_uuid", &osc_server_uuid_fops, 0, 0 },
-	{ "ost_conn_uuid",   &osc_conn_uuid_fops, 0, 0 },
-	{ "active",	     &osc_active_fops, 0 },
-	{ "max_pages_per_rpc", &osc_obd_max_pages_per_rpc_fops, 0 },
-	{ "max_rpcs_in_flight", &osc_max_rpcs_in_flight_fops, 0 },
-	{ "destroys_in_flight", &osc_destroys_in_flight_fops, 0, 0 },
-	{ "max_dirty_mb",    &osc_max_dirty_mb_fops, 0 },
-	{ "osc_cached_mb",   &osc_cached_mb_fops, 0 },
-	{ "cur_dirty_bytes", &osc_cur_dirty_bytes_fops, 0, 0 },
-	{ "cur_grant_bytes", &osc_cur_grant_bytes_fops, 0 },
-	{ "cur_lost_grant_bytes", &osc_cur_lost_grant_bytes_fops, 0, 0},
-	{ "grant_shrink_interval", &osc_grant_shrink_interval_fops, 0 },
-	{ "checksums",       &osc_checksum_fops, 0 },
-	{ "checksum_type",   &osc_checksum_type_fops, 0 },
-	{ "resend_count",    &osc_resend_count_fops, 0},
-	{ "timeouts",	     &osc_timeouts_fops, 0, 0 },
-	{ "contention_seconds", &osc_contention_seconds_fops, 0 },
-	{ "lockless_truncate",  &osc_lockless_truncate_fops, 0 },
-	{ "import",		&osc_import_fops, 0 },
-	{ "state",		&osc_state_fops, 0, 0 },
-	{ "pinger_recov",	&osc_pinger_recov_fops, 0 },
-	{ 0 }
+	{ "uuid",	     &osc_uuid_fops,	NULL, 0 },
+	{ "ping",	     &osc_ping_fops,    NULL, 0222 },
+	{ "connect_flags",   &osc_connect_flags_fops, NULL, 0 },
+	{ "blocksize",       &osc_blksize_fops,     NULL, 0 },
+	{ "kbytestotal",     &osc_kbytestotal_fops, NULL, 0 },
+	{ "kbytesfree",      &osc_kbytesfree_fops,  NULL, 0 },
+	{ "kbytesavail",     &osc_kbytesavail_fops, NULL, 0 },
+	{ "filestotal",      &osc_filestotal_fops,  NULL, 0 },
+	{ "filesfree",       &osc_filesfree_fops,   NULL, 0 },
+	/*{ "filegroups",      lprocfs_rd_filegroups,  NULL, 0 },*/
+	{ "ost_server_uuid", &osc_server_uuid_fops, NULL, 0 },
+	{ "ost_conn_uuid",   &osc_conn_uuid_fops, NULL, 0 },
+	{ "active",	     &osc_active_fops, NULL },
+	{ "max_pages_per_rpc", &osc_obd_max_pages_per_rpc_fops, NULL },
+	{ "max_rpcs_in_flight", &osc_max_rpcs_in_flight_fops, NULL },
+	{ "destroys_in_flight", &osc_destroys_in_flight_fops, NULL, 0 },
+	{ "max_dirty_mb",    &osc_max_dirty_mb_fops, NULL },
+	{ "osc_cached_mb",   &osc_cached_mb_fops, NULL },
+	{ "cur_dirty_bytes", &osc_cur_dirty_bytes_fops, NULL, 0 },
+	{ "cur_grant_bytes", &osc_cur_grant_bytes_fops, NULL },
+	{ "cur_lost_grant_bytes", &osc_cur_lost_grant_bytes_fops, NULL, 0},
+	{ "grant_shrink_interval", &osc_grant_shrink_interval_fops, NULL },
+	{ "checksums",       &osc_checksum_fops, NULL },
+	{ "checksum_type",   &osc_checksum_type_fops, NULL },
+	{ "resend_count",    &osc_resend_count_fops, NULL},
+	{ "timeouts",	     &osc_timeouts_fops, NULL, 0 },
+	{ "contention_seconds", &osc_contention_seconds_fops, NULL },
+	{ "lockless_truncate",  &osc_lockless_truncate_fops, NULL },
+	{ "import",		&osc_import_fops, NULL },
+	{ "state",		&osc_state_fops, NULL, 0 },
+	{ "pinger_recov",	&osc_pinger_recov_fops, NULL },
+	{ NULL }
 };
 
 LPROC_SEQ_FOPS_RO_TYPE(osc, numrefs);
 static struct lprocfs_vars lprocfs_osc_module_vars[] = {
-	{ "num_refs",	&osc_numrefs_fops,     0, 0 },
-	{ 0 }
+	{ "num_refs",	&osc_numrefs_fops,     NULL, 0 },
+	{ NULL }
 };
 
 #define pct(a,b) (b ? a * 100 / b : 0)
diff --git a/drivers/staging/lustre/lustre/osc/osc_cache.c b/drivers/staging/lustre/lustre/osc/osc_cache.c
index fe9989a49f8..00f38eeb578 100644
--- a/drivers/staging/lustre/lustre/osc/osc_cache.c
+++ b/drivers/staging/lustre/lustre/osc/osc_cache.c
@@ -1311,7 +1311,7 @@ static int osc_completion(const struct lu_env *env, struct osc_async_page *oap,
 static void osc_consume_write_grant(struct client_obd *cli,
 				    struct brw_page *pga)
 {
-	LASSERT(spin_is_locked(&cli->cl_loi_list_lock.lock));
+	assert_spin_locked(&cli->cl_loi_list_lock.lock);
 	LASSERT(!(pga->flag & OBD_BRW_FROM_GRANT));
 	atomic_inc(&obd_dirty_pages);
 	cli->cl_dirty += PAGE_CACHE_SIZE;
@@ -1326,7 +1326,7 @@ static void osc_consume_write_grant(struct client_obd *cli,
 static void osc_release_write_grant(struct client_obd *cli,
 				    struct brw_page *pga)
 {
-	LASSERT(spin_is_locked(&cli->cl_loi_list_lock.lock));
+	assert_spin_locked(&cli->cl_loi_list_lock.lock);
 	if (!(pga->flag & OBD_BRW_FROM_GRANT)) {
 		return;
 	}
diff --git a/drivers/staging/lustre/lustre/osc/osc_cl_internal.h b/drivers/staging/lustre/lustre/osc/osc_cl_internal.h
index 9e7899fa4cc..e74b7bb9776 100644
--- a/drivers/staging/lustre/lustre/osc/osc_cl_internal.h
+++ b/drivers/staging/lustre/lustre/osc/osc_cl_internal.h
@@ -176,7 +176,16 @@ static inline void osc_object_unlock(struct osc_object *obj)
 
 static inline int osc_object_is_locked(struct osc_object *obj)
 {
+#if defined(CONFIG_SMP) || defined(CONFIG_DEBUG_SPINLOCK)
 	return spin_is_locked(&obj->oo_lock);
+#else
+	/*
+	 * It is not perfect to return true all the time.
+	 * But since this function is only used for assertion
+	 * and checking, it seems OK.
+	 */
+	return 1;
+#endif
 }
 
 /*
diff --git a/drivers/staging/lustre/lustre/osc/osc_dev.c b/drivers/staging/lustre/lustre/osc/osc_dev.c
index 35f25786763..a7c1ec0d56f 100644
--- a/drivers/staging/lustre/lustre/osc/osc_dev.c
+++ b/drivers/staging/lustre/lustre/osc/osc_dev.c
@@ -122,7 +122,7 @@ static void *osc_key_init(const struct lu_context *ctx,
 {
 	struct osc_thread_info *info;
 
-	OBD_SLAB_ALLOC_PTR_GFP(info, osc_thread_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(info, osc_thread_kmem, GFP_NOFS);
 	if (info == NULL)
 		info = ERR_PTR(-ENOMEM);
 	return info;
@@ -146,7 +146,7 @@ static void *osc_session_init(const struct lu_context *ctx,
 {
 	struct osc_session *info;
 
-	OBD_SLAB_ALLOC_PTR_GFP(info, osc_session_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(info, osc_session_kmem, GFP_NOFS);
 	if (info == NULL)
 		info = ERR_PTR(-ENOMEM);
 	return info;
@@ -193,7 +193,7 @@ static int osc_device_init(const struct lu_env *env, struct lu_device *d,
 static struct lu_device *osc_device_fini(const struct lu_env *env,
 					 struct lu_device *d)
 {
-	return 0;
+	return NULL;
 }
 
 static struct lu_device *osc_device_free(const struct lu_env *env,
diff --git a/drivers/staging/lustre/lustre/osc/osc_io.c b/drivers/staging/lustre/lustre/osc/osc_io.c
index 5f3c545418d..09e06eb0853 100644
--- a/drivers/staging/lustre/lustre/osc/osc_io.c
+++ b/drivers/staging/lustre/lustre/osc/osc_io.c
@@ -509,12 +509,11 @@ static void osc_io_setattr_end(const struct lu_env *env,
 static int osc_io_read_start(const struct lu_env *env,
 			     const struct cl_io_slice *slice)
 {
-	struct osc_io    *oio   = cl2osc_io(env, slice);
 	struct cl_object *obj   = slice->cis_obj;
 	struct cl_attr   *attr  = &osc_env_info(env)->oti_attr;
 	int rc = 0;
 
-	if (oio->oi_lockless == 0 && !slice->cis_io->ci_noatime) {
+	if (!slice->cis_io->ci_noatime) {
 		cl_object_attr_lock(obj);
 		attr->cat_atime = LTIME_S(CURRENT_TIME);
 		rc = cl_object_attr_set(env, obj, attr, CAT_ATIME);
@@ -526,24 +525,17 @@ static int osc_io_read_start(const struct lu_env *env,
 static int osc_io_write_start(const struct lu_env *env,
 			      const struct cl_io_slice *slice)
 {
-	struct osc_io    *oio   = cl2osc_io(env, slice);
 	struct cl_object *obj   = slice->cis_obj;
 	struct cl_attr   *attr  = &osc_env_info(env)->oti_attr;
-	int	      result = 0;
+	int rc = 0;
 
-	if (oio->oi_lockless == 0) {
-		OBD_FAIL_TIMEOUT(OBD_FAIL_OSC_DELAY_SETTIME, 1);
-		cl_object_attr_lock(obj);
-		result = cl_object_attr_get(env, obj, attr);
-		if (result == 0) {
-			attr->cat_mtime = attr->cat_ctime =
-				LTIME_S(CURRENT_TIME);
-			result = cl_object_attr_set(env, obj, attr,
-						    CAT_MTIME | CAT_CTIME);
-		}
-		cl_object_attr_unlock(obj);
-	}
-	return result;
+	OBD_FAIL_TIMEOUT(OBD_FAIL_OSC_DELAY_SETTIME, 1);
+	cl_object_attr_lock(obj);
+	attr->cat_mtime = attr->cat_ctime = LTIME_S(CURRENT_TIME);
+	rc = cl_object_attr_set(env, obj, attr, CAT_MTIME | CAT_CTIME);
+	cl_object_attr_unlock(obj);
+
+	return rc;
 }
 
 static int osc_fsync_ost(const struct lu_env *env, struct osc_object *obj,
@@ -812,7 +804,7 @@ int osc_req_init(const struct lu_env *env, struct cl_device *dev,
 	struct osc_req *or;
 	int result;
 
-	OBD_SLAB_ALLOC_PTR_GFP(or, osc_req_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(or, osc_req_kmem, GFP_NOFS);
 	if (or != NULL) {
 		cl_req_slice_add(req, &or->or_cl, dev, &osc_req_ops);
 		result = 0;
diff --git a/drivers/staging/lustre/lustre/osc/osc_lock.c b/drivers/staging/lustre/lustre/osc/osc_lock.c
index ef7b9c2b208..a46129bec17 100644
--- a/drivers/staging/lustre/lustre/osc/osc_lock.c
+++ b/drivers/staging/lustre/lustre/osc/osc_lock.c
@@ -1192,6 +1192,7 @@ static int osc_lock_wait(const struct lu_env *env,
 
 		LASSERT(olck->ols_agl);
 		olck->ols_agl = 0;
+		olck->ols_flags &= ~LDLM_FL_BLOCK_NOWAIT;
 		rc = osc_lock_enqueue(env, slice, NULL, CEF_ASYNC | CEF_MUST);
 		if (rc != 0)
 			return rc;
@@ -1558,7 +1559,7 @@ int osc_lock_init(const struct lu_env *env,
 	struct osc_lock *clk;
 	int result;
 
-	OBD_SLAB_ALLOC_PTR_GFP(clk, osc_lock_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(clk, osc_lock_kmem, GFP_NOFS);
 	if (clk != NULL) {
 		__u32 enqflags = lock->cll_descr.cld_enq_flags;
 
diff --git a/drivers/staging/lustre/lustre/osc/osc_object.c b/drivers/staging/lustre/lustre/osc/osc_object.c
index 9d34de873fa..f9bfdc82012 100644
--- a/drivers/staging/lustre/lustre/osc/osc_object.c
+++ b/drivers/staging/lustre/lustre/osc/osc_object.c
@@ -260,7 +260,7 @@ struct lu_object *osc_object_alloc(const struct lu_env *env,
 	struct osc_object *osc;
 	struct lu_object  *obj;
 
-	OBD_SLAB_ALLOC_PTR_GFP(osc, osc_object_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(osc, osc_object_kmem, GFP_NOFS);
 	if (osc != NULL) {
 		obj = osc2lu(osc);
 		lu_object_init(obj, NULL, dev);
diff --git a/drivers/staging/lustre/lustre/osc/osc_request.c b/drivers/staging/lustre/lustre/osc/osc_request.c
index ee6953ac735..294db843012 100644
--- a/drivers/staging/lustre/lustre/osc/osc_request.c
+++ b/drivers/staging/lustre/lustre/osc/osc_request.c
@@ -44,7 +44,6 @@
 #include <lustre/lustre_user.h>
 #include <obd_cksum.h>
 #include <obd_ost.h>
-#include <obd_lov.h>
 
 #include <lustre_ha.h>
 #include <lprocfs_status.h>
diff --git a/drivers/staging/lustre/lustre/ptlrpc/client.c b/drivers/staging/lustre/lustre/ptlrpc/client.c
index 4c9e0069508..7246e8ce9c1 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/client.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/client.c
@@ -379,6 +379,34 @@ static int ptlrpc_at_recv_early_reply(struct ptlrpc_request *req)
 	return rc;
 }
 
+struct kmem_cache *request_cache;
+
+int ptlrpc_request_cache_init(void)
+{
+	request_cache = kmem_cache_create("ptlrpc_cache",
+					  sizeof(struct ptlrpc_request),
+					  0, SLAB_HWCACHE_ALIGN, NULL);
+	return request_cache == NULL ? -ENOMEM : 0;
+}
+
+void ptlrpc_request_cache_fini(void)
+{
+	kmem_cache_destroy(request_cache);
+}
+
+struct ptlrpc_request *ptlrpc_request_cache_alloc(int flags)
+{
+	struct ptlrpc_request *req;
+
+	OBD_SLAB_ALLOC_PTR_GFP(req, request_cache, flags);
+	return req;
+}
+
+void ptlrpc_request_cache_free(struct ptlrpc_request *req)
+{
+	OBD_SLAB_FREE_PTR(req, request_cache);
+}
+
 /**
  * Wind down request pool \a pool.
  * Frees all requests from the pool too
@@ -397,7 +425,7 @@ void ptlrpc_free_rq_pool(struct ptlrpc_request_pool *pool)
 		LASSERT(req->rq_reqbuf);
 		LASSERT(req->rq_reqbuf_len == pool->prp_rq_size);
 		OBD_FREE_LARGE(req->rq_reqbuf, pool->prp_rq_size);
-		OBD_FREE(req, sizeof(*req));
+		ptlrpc_request_cache_free(req);
 	}
 	spin_unlock(&pool->prp_lock);
 	OBD_FREE(pool, sizeof(*pool));
@@ -427,12 +455,12 @@ void ptlrpc_add_rqs_to_pool(struct ptlrpc_request_pool *pool, int num_rq)
 		struct lustre_msg *msg;
 
 		spin_unlock(&pool->prp_lock);
-		OBD_ALLOC(req, sizeof(struct ptlrpc_request));
+		req = ptlrpc_request_cache_alloc(GFP_NOFS);
 		if (!req)
 			return;
 		OBD_ALLOC_LARGE(msg, size);
 		if (!msg) {
-			OBD_FREE(req, sizeof(struct ptlrpc_request));
+			ptlrpc_request_cache_free(req);
 			return;
 		}
 		req->rq_reqbuf = msg;
@@ -668,7 +696,7 @@ struct ptlrpc_request *__ptlrpc_request_alloc(struct obd_import *imp,
 		request = ptlrpc_prep_req_from_pool(pool);
 
 	if (!request)
-		OBD_ALLOC_PTR(request);
+		request = ptlrpc_request_cache_alloc(GFP_NOFS);
 
 	if (request) {
 		LASSERTF((unsigned long)imp > 0x1000, "%p", imp);
@@ -739,7 +767,7 @@ void ptlrpc_request_free(struct ptlrpc_request *request)
 	if (request->rq_pool)
 		__ptlrpc_free_req_to_pool(request);
 	else
-		OBD_FREE_PTR(request);
+		ptlrpc_request_cache_free(request);
 }
 EXPORT_SYMBOL(ptlrpc_request_free);
 
@@ -2233,7 +2261,7 @@ static void __ptlrpc_free_req(struct ptlrpc_request *request, int locked)
 	if (request->rq_pool)
 		__ptlrpc_free_req_to_pool(request);
 	else
-		OBD_FREE(request, sizeof(*request));
+		ptlrpc_request_cache_free(request);
 }
 
 static int __ptlrpc_req_finished(struct ptlrpc_request *request, int locked);
@@ -2243,7 +2271,7 @@ static int __ptlrpc_req_finished(struct ptlrpc_request *request, int locked);
  */
 void ptlrpc_req_finished_with_imp_lock(struct ptlrpc_request *request)
 {
-	LASSERT(spin_is_locked(&request->rq_import->imp_lock));
+	assert_spin_locked(&request->rq_import->imp_lock);
 	(void)__ptlrpc_req_finished(request, 1);
 }
 EXPORT_SYMBOL(ptlrpc_req_finished_with_imp_lock);
@@ -2424,9 +2452,7 @@ void ptlrpc_free_committed(struct obd_import *imp)
 	bool		       skip_committed_list = true;
 
 	LASSERT(imp != NULL);
-
-	LASSERT(spin_is_locked(&imp->imp_lock));
-
+	assert_spin_locked(&imp->imp_lock);
 
 	if (imp->imp_peer_committed_transno == imp->imp_last_transno_checked &&
 	    imp->imp_generation == imp->imp_last_generation_checked) {
@@ -2557,7 +2583,7 @@ void ptlrpc_retain_replayable_request(struct ptlrpc_request *req,
 {
 	struct list_head *tmp;
 
-	LASSERT(spin_is_locked(&imp->imp_lock));
+	assert_spin_locked(&imp->imp_lock);
 
 	if (req->rq_transno == 0) {
 		DEBUG_REQ(D_EMERG, req, "saving request with zero transno");
@@ -3023,7 +3049,7 @@ void *ptlrpcd_alloc_work(struct obd_import *imp,
 		return ERR_PTR(-EINVAL);
 
 	/* copy some code from deprecated fakereq. */
-	OBD_ALLOC_PTR(req);
+	req = ptlrpc_request_cache_alloc(GFP_NOFS);
 	if (req == NULL) {
 		CERROR("ptlrpc: run out of memory!\n");
 		return ERR_PTR(-ENOMEM);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/events.c b/drivers/staging/lustre/lustre/ptlrpc/events.c
index 6ea0a491cfb..aa85239f6cd 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/events.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/events.c
@@ -307,7 +307,7 @@ void request_in_callback(lnet_event_t *ev)
 			/* We moaned above already... */
 			return;
 		}
-		OBD_ALLOC_GFP(req, sizeof(*req), ALLOC_ATOMIC_TRY);
+		req = ptlrpc_request_cache_alloc(ALLOC_ATOMIC_TRY);
 		if (req == NULL) {
 			CERROR("Can't allocate incoming request descriptor: "
 			       "Dropping %s RPC from %s\n",
diff --git a/drivers/staging/lustre/lustre/ptlrpc/gss/gss_bulk.c b/drivers/staging/lustre/lustre/ptlrpc/gss/gss_bulk.c
index 7852bf30a3a..93794bd928c 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/gss/gss_bulk.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/gss/gss_bulk.c
@@ -176,31 +176,31 @@ int gss_cli_ctx_unwrap_bulk(struct ptlrpc_cli_ctx *ctx,
 	switch (SPTLRPC_FLVR_SVC(req->rq_flvr.sf_rpc)) {
 	case SPTLRPC_SVC_NULL:
 		vmsg = req->rq_repdata;
+		LASSERT(vmsg != NULL && vmsg->lm_bufcount >= 3);
 		voff = vmsg->lm_bufcount - 1;
-		LASSERT(vmsg && vmsg->lm_bufcount >= 3);
 
 		rmsg = req->rq_reqbuf;
+		LASSERT(rmsg != NULL && rmsg->lm_bufcount >= 3);
 		roff = rmsg->lm_bufcount - 1; /* last segment */
-		LASSERT(rmsg && rmsg->lm_bufcount >= 3);
 		break;
 	case SPTLRPC_SVC_AUTH:
 	case SPTLRPC_SVC_INTG:
 		vmsg = req->rq_repdata;
+		LASSERT(vmsg != NULL && vmsg->lm_bufcount >= 4);
 		voff = vmsg->lm_bufcount - 2;
-		LASSERT(vmsg && vmsg->lm_bufcount >= 4);
 
 		rmsg = req->rq_reqbuf;
+		LASSERT(rmsg != NULL && rmsg->lm_bufcount >= 4);
 		roff = rmsg->lm_bufcount - 2; /* second last segment */
-		LASSERT(rmsg && rmsg->lm_bufcount >= 4);
 		break;
 	case SPTLRPC_SVC_PRIV:
 		vmsg = req->rq_repdata;
+		LASSERT(vmsg != NULL && vmsg->lm_bufcount >= 2);
 		voff = vmsg->lm_bufcount - 1;
-		LASSERT(vmsg && vmsg->lm_bufcount >= 2);
 
 		rmsg = req->rq_clrbuf;
+		LASSERT(rmsg != NULL && rmsg->lm_bufcount >= 2);
 		roff = rmsg->lm_bufcount - 1; /* last segment */
-		LASSERT(rmsg && rmsg->lm_bufcount >= 2);
 		break;
 	default:
 		LBUG();
diff --git a/drivers/staging/lustre/lustre/ptlrpc/gss/gss_pipefs.c b/drivers/staging/lustre/lustre/ptlrpc/gss/gss_pipefs.c
index 7a1ff4fecbe..3be5bc14c4e 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/gss/gss_pipefs.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/gss/gss_pipefs.c
@@ -137,7 +137,7 @@ void ctx_enhash_pf(struct ptlrpc_cli_ctx *ctx, struct hlist_head *hash)
 static
 void ctx_unhash_pf(struct ptlrpc_cli_ctx *ctx, struct hlist_head *freelist)
 {
-	LASSERT(spin_is_locked(&ctx->cc_sec->ps_lock));
+	assert_spin_locked(&ctx->cc_sec->ps_lock);
 	LASSERT(atomic_read(&ctx->cc_refcount) > 0);
 	LASSERT(test_bit(PTLRPC_CTX_CACHED_BIT, &ctx->cc_flags));
 	LASSERT(!hlist_unhashed(&ctx->cc_cache));
@@ -719,7 +719,7 @@ void gss_unhash_msg_nolock(struct gss_upcall_msg *gmsg)
 	__u32 idx = gmsg->gum_mechidx;
 
 	LASSERT(idx < MECH_MAX);
-	LASSERT(spin_is_locked(&upcall_locks[idx]));
+	assert_spin_locked(&upcall_locks[idx]);
 
 	if (list_empty(&gmsg->gum_list))
 		return;
diff --git a/drivers/staging/lustre/lustre/ptlrpc/import.c b/drivers/staging/lustre/lustre/ptlrpc/import.c
index 537aa6204a5..8573f328bd2 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/import.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/import.c
@@ -194,7 +194,7 @@ int ptlrpc_set_import_discon(struct obd_import *imp, __u32 conn_cnt)
 /* Must be called with imp_lock held! */
 static void ptlrpc_deactivate_and_unlock_import(struct obd_import *imp)
 {
-	LASSERT(spin_is_locked(&imp->imp_lock));
+	assert_spin_locked(&imp->imp_lock);
 
 	CDEBUG(D_HA, "setting import %s INVALID\n", obd2cli_tgt(imp->imp_obd));
 	imp->imp_invalid = 1;
@@ -381,6 +381,11 @@ void ptlrpc_activate_import(struct obd_import *imp)
 	struct obd_device *obd = imp->imp_obd;
 
 	spin_lock(&imp->imp_lock);
+	if (imp->imp_deactive != 0) {
+		spin_unlock(&imp->imp_lock);
+		return;
+	}
+
 	imp->imp_invalid = 0;
 	spin_unlock(&imp->imp_lock);
 	obd_import_event(obd, imp, IMP_EVENT_ACTIVE);
@@ -640,7 +645,7 @@ int ptlrpc_connect_import(struct obd_import *imp)
 	if (rc)
 		GOTO(out, rc);
 
-	rc = sptlrpc_import_sec_adapt(imp, NULL, 0);
+	rc = sptlrpc_import_sec_adapt(imp, NULL, NULL);
 	if (rc)
 		GOTO(out, rc);
 
@@ -1399,26 +1404,33 @@ int ptlrpc_disconnect_import(struct obd_import *imp, int noclose)
 {
 	struct ptlrpc_request *req;
 	int rq_opc, rc = 0;
-	int nowait = imp->imp_obd->obd_force;
 
-	if (nowait)
+	if (imp->imp_obd->obd_force)
 		GOTO(set_state, rc);
 
 	switch (imp->imp_connect_op) {
-	case OST_CONNECT: rq_opc = OST_DISCONNECT; break;
-	case MDS_CONNECT: rq_opc = MDS_DISCONNECT; break;
-	case MGS_CONNECT: rq_opc = MGS_DISCONNECT; break;
+	case OST_CONNECT:
+		rq_opc = OST_DISCONNECT;
+		break;
+	case MDS_CONNECT:
+		rq_opc = MDS_DISCONNECT;
+		break;
+	case MGS_CONNECT:
+		rq_opc = MGS_DISCONNECT;
+		break;
 	default:
-		CERROR("don't know how to disconnect from %s (connect_op %d)\n",
-		       obd2cli_tgt(imp->imp_obd), imp->imp_connect_op);
-		return -EINVAL;
+		rc = -EINVAL;
+		CERROR("%s: don't know how to disconnect from %s "
+		       "(connect_op %d): rc = %d\n",
+		       imp->imp_obd->obd_name, obd2cli_tgt(imp->imp_obd),
+		       imp->imp_connect_op, rc);
+		return rc;
 	}
 
 	if (ptlrpc_import_in_recovery(imp)) {
 		struct l_wait_info lwi;
 		cfs_duration_t timeout;
 
-
 		if (AT_OFF) {
 			if (imp->imp_server_timeout)
 				timeout = cfs_time_seconds(obd_timeout / 2);
@@ -1441,7 +1453,6 @@ 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);
-
 	spin_unlock(&imp->imp_lock);
 
 	req = ptlrpc_request_alloc_pack(imp, &RQF_MDS_DISCONNECT,
@@ -1474,6 +1485,9 @@ out:
 	memset(&imp->imp_remote_handle, 0, sizeof(imp->imp_remote_handle));
 	spin_unlock(&imp->imp_lock);
 
+	if (rc == -ETIMEDOUT || rc == -ENOTCONN || rc == -ESHUTDOWN)
+		rc = 0;
+
 	return rc;
 }
 EXPORT_SYMBOL(ptlrpc_disconnect_import);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c b/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c
index 58f1c8bf25b..6b9c6db1f2d 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c
@@ -449,7 +449,7 @@ void nrs_policy_get_info_locked(struct ptlrpc_nrs_policy *policy,
 {
 	LASSERT(policy != NULL);
 	LASSERT(info != NULL);
-	LASSERT(spin_is_locked(&policy->pol_nrs->nrs_lock));
+	assert_spin_locked(&policy->pol_nrs->nrs_lock);
 
 	memcpy(info->pi_name, policy->pol_desc->pd_name, NRS_POL_NAME_MAX);
 
@@ -1190,7 +1190,7 @@ int lprocfs_wr_evict_client(struct file *file, const char *buffer,
 	 * the proc entries under the being destroyed export{}, so I have
 	 * to drop the lock at first here.
 	 * - jay, jxiong@clusterfs.com */
-	class_incref(obd, __FUNCTION__, current);
+	class_incref(obd, __func__, current);
 
 	if (strncmp(tmpbuf, "nid:", 4) == 0)
 		obd_export_evict_by_nid(obd, tmpbuf + 4);
@@ -1199,7 +1199,7 @@ int lprocfs_wr_evict_client(struct file *file, const char *buffer,
 	else
 		obd_export_evict_by_uuid(obd, tmpbuf);
 
-	class_decref(obd, __FUNCTION__, current);
+	class_decref(obd, __func__, current);
 
 out:
 	OBD_FREE(kbuf, BUFLEN);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/nrs.c b/drivers/staging/lustre/lustre/ptlrpc/nrs.c
index bcba1c8e869..12151aa2a1e 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/nrs.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/nrs.c
@@ -746,7 +746,7 @@ static int nrs_policy_register(struct ptlrpc_nrs *nrs,
 	LASSERT(desc->pd_compat != NULL);
 
 	OBD_CPT_ALLOC_GFP(policy, svcpt->scp_service->srv_cptable,
-			  svcpt->scp_cpt, sizeof(*policy), __GFP_IO);
+			  svcpt->scp_cpt, sizeof(*policy), GFP_NOFS);
 	if (policy == NULL)
 		return -ENOMEM;
 
diff --git a/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c b/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c
index 45c0b84621e..cddeeb6bb23 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c
@@ -300,6 +300,7 @@ lustre_get_emerg_rs(struct ptlrpc_service_part *svcpt)
 	spin_unlock(&svcpt->scp_rep_lock);
 
 	memset(rs, 0, svcpt->scp_service->srv_max_reply_size);
+	rs->rs_size = svcpt->scp_service->srv_max_reply_size;
 	rs->rs_svcpt = svcpt;
 	rs->rs_prealloc = 1;
 out:
diff --git a/drivers/staging/lustre/lustre/ptlrpc/pinger.c b/drivers/staging/lustre/lustre/ptlrpc/pinger.c
index 6dff502ce23..38099d9dfda 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/pinger.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/pinger.c
@@ -368,7 +368,7 @@ EXPORT_SYMBOL(ptlrpc_pinger_sending_on_import);
 void ptlrpc_pinger_commit_expected(struct obd_import *imp)
 {
 	ptlrpc_update_next_ping(imp, 1);
-	LASSERT(spin_is_locked(&imp->imp_lock));
+	assert_spin_locked(&imp->imp_lock);
 	/*
 	 * Avoid reading stale imp_connect_data.  When not sure if pings are
 	 * expected or not on next connection, we assume they are not and force
diff --git a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
index e3b5a920bca..7c940553059 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
+++ b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
@@ -55,6 +55,10 @@ int ptlrpcd_start(int index, int max, const char *name, struct ptlrpcd_ctl *pc);
 /* client.c */
 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);
+void ptlrpc_request_cache_free(struct ptlrpc_request *req);
 void ptlrpc_init_xid(void);
 
 /* events.c */
diff --git a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_module.c b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_module.c
index 0efd35887a1..251ae75c2dd 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_module.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_module.c
@@ -73,29 +73,34 @@ __init int ptlrpc_init(void)
 		return rc;
 
 	cleanup_phase = 1;
+	rc = ptlrpc_request_cache_init();
+	if (rc)
+		GOTO(cleanup, rc);
 
+	cleanup_phase = 2;
 	rc = ptlrpc_init_portals();
 	if (rc)
 		GOTO(cleanup, rc);
-	cleanup_phase = 2;
+
+	cleanup_phase = 3;
 
 	rc = ptlrpc_connection_init();
 	if (rc)
 		GOTO(cleanup, rc);
-	cleanup_phase = 3;
 
+	cleanup_phase = 4;
 	ptlrpc_put_connection_superhack = ptlrpc_connection_put;
 
 	rc = ptlrpc_start_pinger();
 	if (rc)
 		GOTO(cleanup, rc);
-	cleanup_phase = 4;
 
+	cleanup_phase = 5;
 	rc = ldlm_init();
 	if (rc)
 		GOTO(cleanup, rc);
-	cleanup_phase = 5;
 
+	cleanup_phase = 6;
 	rc = sptlrpc_init();
 	if (rc)
 		GOTO(cleanup, rc);
@@ -115,19 +120,29 @@ cleanup:
 	switch (cleanup_phase) {
 	case 8:
 		ptlrpc_nrs_fini();
+		/* Fall through */
 	case 7:
 		sptlrpc_fini();
-	case 5:
+		/* Fall through */
+	case 6:
 		ldlm_exit();
-	case 4:
+		/* Fall through */
+	case 5:
 		ptlrpc_stop_pinger();
-	case 3:
+		/* Fall through */
+	case 4:
 		ptlrpc_connection_fini();
-	case 2:
+		/* Fall through */
+	case 3:
 		ptlrpc_exit_portals();
+		/* Fall through */
+	case 2:
+		ptlrpc_request_cache_fini();
+		/* Fall through */
 	case 1:
 		ptlrpc_hr_fini();
 		req_layout_fini();
+		/* Fall through */
 	default: ;
 	}
 
@@ -142,6 +157,7 @@ static void __exit ptlrpc_exit(void)
 	ldlm_exit();
 	ptlrpc_stop_pinger();
 	ptlrpc_exit_portals();
+	ptlrpc_request_cache_fini();
 	ptlrpc_hr_fini();
 	ptlrpc_connection_fini();
 }
diff --git a/drivers/staging/lustre/lustre/ptlrpc/recover.c b/drivers/staging/lustre/lustre/ptlrpc/recover.c
index 48ae328ce24..9cec8a649dc 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/recover.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/recover.c
@@ -49,7 +49,6 @@
 #include <obd.h>
 #include <obd_ost.h>
 #include <obd_class.h>
-#include <obd_lov.h> /* for IOC_LOV_SET_OSC_ACTIVE */
 #include <linux/list.h>
 
 #include "ptlrpc_internal.h"
@@ -369,11 +368,14 @@ EXPORT_SYMBOL(ptlrpc_recover_import);
 int ptlrpc_import_in_recovery(struct obd_import *imp)
 {
 	int in_recovery = 1;
+
 	spin_lock(&imp->imp_lock);
 	if (imp->imp_state == LUSTRE_IMP_FULL ||
 	    imp->imp_state == LUSTRE_IMP_CLOSED ||
-	    imp->imp_state == LUSTRE_IMP_DISCON)
+	    imp->imp_state == LUSTRE_IMP_DISCON ||
+	    imp->imp_obd->obd_no_recov)
 		in_recovery = 0;
 	spin_unlock(&imp->imp_lock);
+
 	return in_recovery;
 }
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec.c b/drivers/staging/lustre/lustre/ptlrpc/sec.c
index d80418057f2..28ac824a73f 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec.c
@@ -354,7 +354,7 @@ static int import_sec_check_expire(struct obd_import *imp)
 		return 0;
 
 	CDEBUG(D_SEC, "found delayed sec adapt expired, do it now\n");
-	return sptlrpc_import_sec_adapt(imp, NULL, 0);
+	return sptlrpc_import_sec_adapt(imp, NULL, NULL);
 }
 
 static int import_sec_validate_get(struct obd_import *imp,
@@ -904,7 +904,7 @@ int sptlrpc_import_check_ctx(struct obd_import *imp)
 		return -EACCES;
 	}
 
-	OBD_ALLOC_PTR(req);
+	req = ptlrpc_request_cache_alloc(GFP_NOFS);
 	if (!req)
 		return -ENOMEM;
 
@@ -920,7 +920,7 @@ int sptlrpc_import_check_ctx(struct obd_import *imp)
 	rc = sptlrpc_req_refresh_ctx(req, 0);
 	LASSERT(list_empty(&req->rq_ctx_chain));
 	sptlrpc_cli_ctx_put(req->rq_cli_ctx, 1);
-	OBD_FREE_PTR(req);
+	ptlrpc_request_cache_free(req);
 
 	return rc;
 }
@@ -1088,7 +1088,7 @@ int sptlrpc_cli_unwrap_early_reply(struct ptlrpc_request *req,
 	int		     early_bufsz, early_size;
 	int		     rc;
 
-	OBD_ALLOC_PTR(early_req);
+	early_req = ptlrpc_request_cache_alloc(GFP_NOFS);
 	if (early_req == NULL)
 		return -ENOMEM;
 
@@ -1160,7 +1160,7 @@ err_ctx:
 err_buf:
 	OBD_FREE_LARGE(early_buf, early_bufsz);
 err_req:
-	OBD_FREE_PTR(early_req);
+	ptlrpc_request_cache_free(early_req);
 	return rc;
 }
 
@@ -1177,7 +1177,7 @@ void sptlrpc_cli_finish_early_reply(struct ptlrpc_request *early_req)
 
 	sptlrpc_cli_ctx_put(early_req->rq_cli_ctx, 1);
 	OBD_FREE_LARGE(early_req->rq_repbuf, early_req->rq_repbuf_len);
-	OBD_FREE_PTR(early_req);
+	ptlrpc_request_cache_free(early_req);
 }
 
 /**************************************************
@@ -2086,8 +2086,18 @@ int sptlrpc_svc_alloc_rs(struct ptlrpc_request *req, int msglen)
 
 	rc = policy->sp_sops->alloc_rs(req, msglen);
 	if (unlikely(rc == -ENOMEM)) {
+		struct ptlrpc_service_part *svcpt = req->rq_rqbd->rqbd_svcpt;
+		if (svcpt->scp_service->srv_max_reply_size <
+		   msglen + sizeof(struct ptlrpc_reply_state)) {
+			/* Just return failure if the size is too big */
+			CERROR("size of message is too big (%zd), %d allowed",
+				msglen + sizeof(struct ptlrpc_reply_state),
+				svcpt->scp_service->srv_max_reply_size);
+			return -ENOMEM;
+		}
+
 		/* failed alloc, try emergency pool */
-		rs = lustre_get_emerg_rs(req->rq_rqbd->rqbd_svcpt);
+		rs = lustre_get_emerg_rs(svcpt);
 		if (rs == NULL)
 			return -ENOMEM;
 
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c b/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
index 96566813274..9d51badea73 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
@@ -421,7 +421,7 @@ static int enc_pools_add_pages(int npages)
 			goto out_pools;
 
 		for (j = 0; j < PAGES_PER_POOL && alloced < npages; j++) {
-			pools[i][j] = alloc_page(__GFP_IO |
+			pools[i][j] = alloc_page(GFP_NOFS |
 						     __GFP_HIGHMEM);
 			if (pools[i][j] == NULL)
 				goto out_pools;
@@ -450,7 +450,7 @@ out:
 
 static inline void enc_pools_wakeup(void)
 {
-	LASSERT(spin_is_locked(&page_pools.epp_lock));
+	assert_spin_locked(&page_pools.epp_lock);
 	LASSERT(page_pools.epp_waitqlen >= 0);
 
 	if (unlikely(page_pools.epp_waitqlen)) {
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec_config.c b/drivers/staging/lustre/lustre/ptlrpc/sec_config.c
index bf56120abfa..231656ed766 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec_config.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec_config.c
@@ -745,11 +745,13 @@ void sptlrpc_conf_log_update_begin(const char *logname)
 	mutex_lock(&sptlrpc_conf_lock);
 
 	conf = sptlrpc_conf_get(fsname, 0);
-	if (conf && conf->sc_local) {
-		LASSERT(conf->sc_updated == 0);
-		sptlrpc_conf_free_rsets(conf);
+	if (conf) {
+		if(conf->sc_local) {
+			LASSERT(conf->sc_updated == 0);
+			sptlrpc_conf_free_rsets(conf);
+		}
+		conf->sc_modified = 0;
 	}
-	conf->sc_modified = 0;
 
 	mutex_unlock(&sptlrpc_conf_lock);
 }
diff --git a/drivers/staging/lustre/lustre/ptlrpc/service.c b/drivers/staging/lustre/lustre/ptlrpc/service.c
index 192adec5382..d278f2e2180 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/service.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/service.c
@@ -384,8 +384,8 @@ void ptlrpc_dispatch_difficult_reply(struct ptlrpc_reply_state *rs)
 void
 ptlrpc_schedule_difficult_reply(struct ptlrpc_reply_state *rs)
 {
-	LASSERT(spin_is_locked(&rs->rs_svcpt->scp_rep_lock));
-	LASSERT(spin_is_locked(&rs->rs_lock));
+	assert_spin_locked(&rs->rs_svcpt->scp_rep_lock);
+	assert_spin_locked(&rs->rs_lock);
 	LASSERT(rs->rs_difficult);
 	rs->rs_scheduled_ever = 1;  /* flag any notification attempt */
 
@@ -842,7 +842,7 @@ static void ptlrpc_server_free_request(struct ptlrpc_request *req)
 		/* NB request buffers use an embedded
 		 * req if the incoming req unlinked the
 		 * MD; this isn't one of them! */
-		OBD_FREE(req, sizeof(*req));
+		ptlrpc_request_cache_free(req);
 	}
 }
 
@@ -1042,9 +1042,6 @@ static void ptlrpc_update_export_timer(struct obd_export *exp, long extra_delay)
 		return;
 
 	exp->exp_last_request_time = new_time;
-	CDEBUG(D_HA, "updating export %s at "CFS_TIME_T" exp %p\n",
-	       exp->exp_client_uuid.uuid,
-	       exp->exp_last_request_time, exp);
 
 	/* exports may get disconnected from the chain even though the
 	   export has references, so we must keep the spin lock while
@@ -1308,14 +1305,12 @@ static int ptlrpc_at_send_early_reply(struct ptlrpc_request *req)
 	}
 	newdl = cfs_time_current_sec() + at_get(&svcpt->scp_at_estimate);
 
-	OBD_ALLOC(reqcopy, sizeof(*reqcopy));
+	reqcopy = ptlrpc_request_cache_alloc(GFP_NOFS);
 	if (reqcopy == NULL)
 		return -ENOMEM;
 	OBD_ALLOC_LARGE(reqmsg, req->rq_reqlen);
-	if (!reqmsg) {
-		OBD_FREE(reqcopy, sizeof(*reqcopy));
-		return -ENOMEM;
-	}
+	if (!reqmsg)
+		GOTO(out_free, rc = -ENOMEM);
 
 	*reqcopy = *req;
 	reqcopy->rq_reply_state = NULL;
@@ -1372,7 +1367,8 @@ out_put:
 out:
 	sptlrpc_svc_ctx_decref(reqcopy);
 	OBD_FREE_LARGE(reqmsg, req->rq_reqlen);
-	OBD_FREE(reqcopy, sizeof(*reqcopy));
+out_free:
+	ptlrpc_request_cache_free(reqcopy);
 	return rc;
 }
 
diff --git a/drivers/staging/media/as102/as102_usb_drv.c b/drivers/staging/media/as102/as102_usb_drv.c
index e4a69454ebe..e6f6278e97d 100644
--- a/drivers/staging/media/as102/as102_usb_drv.c
+++ b/drivers/staging/media/as102/as102_usb_drv.c
@@ -249,7 +249,7 @@ static void as102_free_usb_stream_buffer(struct as102_dev_t *dev)
 
 static int as102_alloc_usb_stream_buffer(struct as102_dev_t *dev)
 {
-	int i, ret = 0;
+	int i;
 
 	dev->stream = usb_alloc_coherent(dev->bus_adap.usb_dev,
 				       MAX_STREAM_URB * AS102_USB_BUF_SIZE,
@@ -280,7 +280,7 @@ static int as102_alloc_usb_stream_buffer(struct as102_dev_t *dev)
 
 		dev->stream_urb[i] = urb;
 	}
-	return ret;
+	return 0;
 }
 
 static void as102_usb_stop_stream(struct as102_dev_t *dev)
@@ -458,7 +458,6 @@ exit:
 
 static int as102_release(struct inode *inode, struct file *file)
 {
-	int ret = 0;
 	struct as102_dev_t *dev = NULL;
 
 	dev = file->private_data;
@@ -467,7 +466,7 @@ static int as102_release(struct inode *inode, struct file *file)
 		kref_put(&dev->kref, as102_usb_release);
 	}
 
-	return ret;
+	return 0;
 }
 
 MODULE_DEVICE_TABLE(usb, as102_usb_id_table);
diff --git a/drivers/staging/media/bcm2048/radio-bcm2048.c b/drivers/staging/media/bcm2048/radio-bcm2048.c
index b2cd3a85166..bbf236e842a 100644
--- a/drivers/staging/media/bcm2048/radio-bcm2048.c
+++ b/drivers/staging/media/bcm2048/radio-bcm2048.c
@@ -737,7 +737,7 @@ static int bcm2048_set_region(struct bcm2048_device *bdev, u8 region)
 	int err;
 	u32 new_frequency = 0;
 
-	if (region > ARRAY_SIZE(region_configs))
+	if (region >= ARRAY_SIZE(region_configs))
 		return -EINVAL;
 
 	mutex_lock(&bdev->mutex);
diff --git a/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.h b/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.h
index 68f6fe43a5b..2632a806c4a 100644
--- a/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.h
+++ b/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.h
@@ -87,8 +87,6 @@ struct vpfe_fh {
 	struct vpfe_video_device *video;
 	/* Indicates whether this file handle is doing IO */
 	u8 io_allowed;
-	/* Used to keep track priority of this instance */
-	enum v4l2_priority prio;
 };
 
 void mbus_to_pix(const struct v4l2_mbus_framefmt *mbus,
diff --git a/drivers/staging/media/davinci_vpfe/vpfe_video.c b/drivers/staging/media/davinci_vpfe/vpfe_video.c
index 8c101cbbee9..d95c427043d 100644
--- a/drivers/staging/media/davinci_vpfe/vpfe_video.c
+++ b/drivers/staging/media/davinci_vpfe/vpfe_video.c
@@ -415,7 +415,6 @@ static int vpfe_open(struct file *file)
 	video->usrs++;
 	/* Set io_allowed member to false */
 	handle->io_allowed = 0;
-	v4l2_prio_open(&video->prio, &handle->prio);
 	handle->video = video;
 	file->private_data = &handle->vfh;
 	mutex_unlock(&video->lock);
@@ -532,8 +531,8 @@ static int vpfe_release(struct file *file)
 	}
 	/* Decrement device users counter */
 	video->usrs--;
-	/* Close the priority */
-	v4l2_prio_close(&video->prio, fh->prio);
+	v4l2_fh_del(&fh->vfh);
+	v4l2_fh_exit(&fh->vfh);
 	/* If this is the last file handle */
 	if (!video->usrs)
 		video->initialized = 0;
@@ -945,7 +944,7 @@ static int vpfe_s_std(struct file *file, void *priv, v4l2_std_id std_id)
 		goto unlock_out;
 	}
 	ret = v4l2_device_call_until_err(&vpfe_dev->v4l2_dev, sdinfo->grp_id,
-					 core, s_std, std_id);
+					 video, s_std, std_id);
 	if (ret < 0) {
 		v4l2_err(&vpfe_dev->v4l2_dev, "Failed to set standard\n");
 		video->stdid = V4L2_STD_UNKNOWN;
@@ -987,8 +986,10 @@ vpfe_enum_dv_timings(struct file *file, void *fh,
 	struct vpfe_device *vpfe_dev = video->vpfe_dev;
 	struct v4l2_subdev *subdev = video->current_ext_subdev->subdev;
 
+	timings->pad = 0;
+
 	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_enum_dv_timings\n");
-	return v4l2_subdev_call(subdev, video, enum_dv_timings, timings);
+	return v4l2_subdev_call(subdev, pad, enum_dv_timings, timings);
 }
 
 /*
@@ -1219,8 +1220,16 @@ static int vpfe_start_streaming(struct vb2_queue *vq, unsigned int count)
 	video->state = VPFE_VIDEO_BUFFER_QUEUED;
 
 	ret = vpfe_start_capture(video);
-	if (ret)
+	if (ret) {
+		struct vpfe_cap_buffer *buf, *tmp;
+
+		vb2_buffer_done(&video->cur_frm->vb, VB2_BUF_STATE_QUEUED);
+		list_for_each_entry_safe(buf, tmp, &video->dma_queue, list) {
+			list_del(&buf->list);
+			vb2_buffer_done(&buf->vb, VB2_BUF_STATE_QUEUED);
+		}
 		goto unlock_out;
+	}
 
 	mutex_unlock(&video->lock);
 
@@ -1242,21 +1251,29 @@ static int vpfe_buffer_init(struct vb2_buffer *vb)
 }
 
 /* abort streaming and wait for last buffer */
-static int vpfe_stop_streaming(struct vb2_queue *vq)
+static void vpfe_stop_streaming(struct vb2_queue *vq)
 {
 	struct vpfe_fh *fh = vb2_get_drv_priv(vq);
 	struct vpfe_video_device *video = fh->video;
 
-	if (!vb2_is_streaming(vq))
-		return 0;
 	/* release all active buffers */
+	if (video->cur_frm == video->next_frm) {
+		vb2_buffer_done(&video->cur_frm->vb, VB2_BUF_STATE_ERROR);
+	} else {
+		if (video->cur_frm != NULL)
+			vb2_buffer_done(&video->cur_frm->vb,
+					VB2_BUF_STATE_ERROR);
+		if (video->next_frm != NULL)
+			vb2_buffer_done(&video->next_frm->vb,
+					VB2_BUF_STATE_ERROR);
+	}
+
 	while (!list_empty(&video->dma_queue)) {
 		video->next_frm = list_entry(video->dma_queue.next,
 						struct vpfe_cap_buffer, list);
 		list_del(&video->next_frm->list);
 		vb2_buffer_done(&video->next_frm->vb, VB2_BUF_STATE_ERROR);
 	}
-	return 0;
 }
 
 static void vpfe_buf_cleanup(struct vb2_buffer *vb)
@@ -1581,8 +1598,6 @@ int vpfe_video_init(struct vpfe_video_device *video, const char *name)
 	snprintf(video->video_dev.name, sizeof(video->video_dev.name),
 		 "DAVINCI VIDEO %s %s", name, direction);
 
-	/* Initialize prio member of device object */
-	v4l2_prio_init(&video->prio);
 	spin_lock_init(&video->irqlock);
 	spin_lock_init(&video->dma_queue_lock);
 	mutex_init(&video->lock);
@@ -1591,6 +1606,7 @@ int vpfe_video_init(struct vpfe_video_device *video, const char *name)
 	if (ret < 0)
 		return ret;
 
+	set_bit(V4L2_FL_USE_FH_PRIO, &video->video_dev.flags);
 	video_set_drvdata(&video->video_dev, video);
 
 	return 0;
diff --git a/drivers/staging/media/davinci_vpfe/vpfe_video.h b/drivers/staging/media/davinci_vpfe/vpfe_video.h
index ca9a7024523..1b1b6c4a56b 100644
--- a/drivers/staging/media/davinci_vpfe/vpfe_video.h
+++ b/drivers/staging/media/davinci_vpfe/vpfe_video.h
@@ -102,8 +102,6 @@ struct vpfe_video_device {
 	 * user has selected
 	 */
 	enum v4l2_memory			memory;
-	/* Used to keep track of state of the priority */
-	struct v4l2_prio_state			prio;
 	/* number of open instances of the channel */
 	u32					usrs;
 	/* flag to indicate whether decoder is initialized */
diff --git a/drivers/staging/media/dt3155v4l/dt3155v4l.c b/drivers/staging/media/dt3155v4l/dt3155v4l.c
index afbc2e51960..40580228a6c 100644
--- a/drivers/staging/media/dt3155v4l/dt3155v4l.c
+++ b/drivers/staging/media/dt3155v4l/dt3155v4l.c
@@ -262,7 +262,7 @@ dt3155_buf_prepare(struct vb2_buffer *vb)
 	return 0;
 }
 
-static int
+static void
 dt3155_stop_streaming(struct vb2_queue *q)
 {
 	struct dt3155_priv *pd = vb2_get_drv_priv(q);
@@ -276,7 +276,6 @@ dt3155_stop_streaming(struct vb2_queue *q)
 	}
 	spin_unlock_irq(&pd->lock);
 	msleep(45); /* irq hendler will stop the hardware */
-	return 0;
 }
 
 static void
@@ -907,8 +906,10 @@ dt3155_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if (!pd)
 		return -ENOMEM;
 	pd->vdev = video_device_alloc();
-	if (!pd->vdev)
+	if (!pd->vdev) {
+		err = -ENOMEM;
 		goto err_video_device_alloc;
+	}
 	*pd->vdev = dt3155_vdev;
 	pci_set_drvdata(pdev, pd);    /* for use in dt3155_remove() */
 	video_set_drvdata(pd->vdev, pd);  /* for use in video_fops */
diff --git a/drivers/staging/media/go7007/go7007-v4l2.c b/drivers/staging/media/go7007/go7007-v4l2.c
index b397aa3c0f4..da7b5493e13 100644
--- a/drivers/staging/media/go7007/go7007-v4l2.c
+++ b/drivers/staging/media/go7007/go7007-v4l2.c
@@ -516,7 +516,7 @@ static int go7007_start_streaming(struct vb2_queue *q, unsigned int count)
 	return ret;
 }
 
-static int go7007_stop_streaming(struct vb2_queue *q)
+static void go7007_stop_streaming(struct vb2_queue *q)
 {
 	struct go7007 *go = vb2_get_drv_priv(q);
 	unsigned long flags;
@@ -538,7 +538,6 @@ static int go7007_stop_streaming(struct vb2_queue *q)
 	/* Turn on Capture LED */
 	if (go->board_id == GO7007_BOARDID_ADS_USBAV_709)
 		go7007_write_addr(go, 0x3c82, 0x000d);
-	return 0;
 }
 
 static struct vb2_ops go7007_video_qops = {
@@ -666,7 +665,7 @@ static int go7007_s_std(struct go7007 *go)
 		go->sensor_framerate = 30000;
 	}
 
-	call_all(&go->v4l2_dev, core, s_std, go->std);
+	call_all(&go->v4l2_dev, video, s_std, go->std);
 	set_capture_size(go, NULL, 0);
 	return 0;
 }
diff --git a/drivers/staging/media/go7007/go7007.txt b/drivers/staging/media/go7007/go7007.txt
index dc0026cff9f..c8e5eb09d38 100644
--- a/drivers/staging/media/go7007/go7007.txt
+++ b/drivers/staging/media/go7007/go7007.txt
@@ -79,7 +79,6 @@ for custom-built kernels, the following options need to be enabled in the
 kernel as built-in or modules:
 
 	CONFIG_MODULES           - Enable loadable module support
-	CONFIG_KMOD              - Automatic kernel module loading
 	CONFIG_FW_LOADER         - Hotplug firmware loading support
 	CONFIG_I2C               - I2C support
 	CONFIG_VIDEO_DEV         - Video For Linux
diff --git a/drivers/staging/media/go7007/s2250-board.c b/drivers/staging/media/go7007/s2250-board.c
index 696a8075669..eaa2b0990a1 100644
--- a/drivers/staging/media/go7007/s2250-board.c
+++ b/drivers/staging/media/go7007/s2250-board.c
@@ -474,7 +474,6 @@ static const struct v4l2_ctrl_ops s2250_ctrl_ops = {
 
 static const struct v4l2_subdev_core_ops s2250_core_ops = {
 	.log_status = s2250_log_status,
-	.s_std = s2250_s_std,
 };
 
 static const struct v4l2_subdev_audio_ops s2250_audio_ops = {
@@ -482,6 +481,7 @@ static const struct v4l2_subdev_audio_ops s2250_audio_ops = {
 };
 
 static const struct v4l2_subdev_video_ops s2250_video_ops = {
+	.s_std = s2250_s_std,
 	.s_routing = s2250_s_video_routing,
 	.s_mbus_fmt = s2250_s_mbus_fmt,
 };
diff --git a/drivers/staging/media/go7007/saa7134-go7007.c b/drivers/staging/media/go7007/saa7134-go7007.c
index 6e2ca338cdd..e40f7fbfc0a 100644
--- a/drivers/staging/media/go7007/saa7134-go7007.c
+++ b/drivers/staging/media/go7007/saa7134-go7007.c
@@ -434,11 +434,15 @@ static const struct v4l2_subdev_core_ops saa7134_go7007_core_ops = {
 	.g_ctrl = saa7134_go7007_g_ctrl,
 	.s_ctrl = saa7134_go7007_s_ctrl,
 	.queryctrl = saa7134_go7007_queryctrl,
+};
+
+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 = {
 	.core = &saa7134_go7007_core_ops,
+	.video = &saa7134_go7007_video_ops,
 };
 
 /* --------------------------------------------------------------------------*/
diff --git a/drivers/staging/media/lirc/lirc_bt829.c b/drivers/staging/media/lirc/lirc_bt829.c
index 30edc740ac2..fe010542ab4 100644
--- a/drivers/staging/media/lirc/lirc_bt829.c
+++ b/drivers/staging/media/lirc/lirc_bt829.c
@@ -64,7 +64,7 @@ static bool debug;
 
 static int atir_minor;
 static phys_addr_t pci_addr_phys;
-static unsigned char *pci_addr_lin;
+static unsigned char __iomem *pci_addr_lin;
 
 static struct lirc_driver atir_driver;
 
@@ -382,7 +382,7 @@ static unsigned char do_get_bits(void)
 
 static unsigned int read_index(unsigned char index)
 {
-	unsigned char *addr;
+	unsigned char __iomem *addr;
 	unsigned int value;
 	/*  addr = pci_addr_lin + DATA_PCI_OFF + ((index & 0xFF) << 2); */
 	addr = pci_addr_lin + ((index & 0xFF) << 2);
@@ -392,7 +392,7 @@ static unsigned int read_index(unsigned char index)
 
 static void write_index(unsigned char index, unsigned int reg_val)
 {
-	unsigned char *addr;
+	unsigned char __iomem *addr;
 	addr = pci_addr_lin + ((index & 0xFF) << 2);
 	writel(reg_val, addr);
 }
diff --git a/drivers/staging/media/lirc/lirc_igorplugusb.c b/drivers/staging/media/lirc/lirc_igorplugusb.c
index f508a1372ad..44b0d076608 100644
--- a/drivers/staging/media/lirc/lirc_igorplugusb.c
+++ b/drivers/staging/media/lirc/lirc_igorplugusb.c
@@ -60,19 +60,6 @@
 #define DRIVER_DESC		"Igorplug USB remote driver for LIRC"
 #define DRIVER_NAME		"lirc_igorplugusb"
 
-/* debugging support */
-#ifdef CONFIG_USB_DEBUG
-static bool debug = true;
-#else
-static bool debug;
-#endif
-
-#define dprintk(fmt, args...)					\
-	do {							\
-		if (debug)					\
-			printk(KERN_DEBUG fmt, ## args);	\
-	} while (0)
-
 /* One mode2 pulse/space has 4 bytes. */
 #define CODE_LENGTH	     sizeof(int)
 
@@ -237,7 +224,7 @@ static int unregister_from_lirc(struct igorplug *ir)
 		return -EINVAL;
 	}
 
-	dprintk(DRIVER_NAME "[%d]: calling lirc_unregister_driver\n", devnum);
+	dev_dbg(&ir->usbdev->dev, "calling lirc_unregister_driver\n");
 	lirc_unregister_driver(d->minor);
 
 	return devnum;
@@ -252,7 +239,7 @@ static int set_use_inc(void *data)
 		return -EIO;
 	}
 
-	dprintk(DRIVER_NAME "[%d]: set use inc\n", ir->devnum);
+	dev_dbg(&ir->usbdev->dev, "set use inc\n");
 
 	if (!ir->usbdev)
 		return -ENODEV;
@@ -269,7 +256,7 @@ static void set_use_dec(void *data)
 		return;
 	}
 
-	dprintk(DRIVER_NAME "[%d]: set use dec\n", ir->devnum);
+	dev_dbg(&ir->usbdev->dev, "set use dec\n");
 }
 
 static void send_fragment(struct igorplug *ir, struct lirc_buffer *buf,
@@ -321,7 +308,7 @@ static int igorplugusb_remote_poll(void *data, struct lirc_buffer *buf)
 		if (ret < DEVICE_HEADERLEN)
 			return -ENODATA;
 
-		dprintk(DRIVER_NAME ": Got %d bytes. Header: %*ph\n",
+		dev_dbg(&ir->usbdev->dev, "Got %d bytes. Header: %*ph\n",
 			ret, 3, ir->buf_in);
 
 		do_gettimeofday(&now);
@@ -385,7 +372,7 @@ static int igorplugusb_remote_probe(struct usb_interface *intf,
 	char buf[63], name[128] = "";
 	int ret;
 
-	dprintk(DRIVER_NAME ": usb probe called.\n");
+	dev_dbg(&intf->dev, "%s: usb probe called.\n", __func__);
 
 	dev = interface_to_usbdev(intf);
 
@@ -405,8 +392,8 @@ static int igorplugusb_remote_probe(struct usb_interface *intf,
 	devnum = dev->devnum;
 	maxp = usb_maxpacket(dev, pipe, usb_pipeout(pipe));
 
-	dprintk(DRIVER_NAME "[%d]: bytes_in_key=%zu maxp=%d\n",
-		devnum, CODE_LENGTH, maxp);
+	dev_dbg(&intf->dev, "%s: bytes_in_key=%zu maxp=%d\n",
+		__func__, CODE_LENGTH, maxp);
 
 	ir = devm_kzalloc(&intf->dev, sizeof(*ir), GFP_KERNEL);
 	if (!ir)
@@ -525,6 +512,3 @@ MODULE_DEVICE_TABLE(usb, igorplugusb_remote_id_table);
 
 module_param(sample_rate, int, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(sample_rate, "Sampling rate in Hz (default: 100)");
-
-module_param(debug, bool, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(debug, "Debug enabled or not");
diff --git a/drivers/staging/media/lirc/lirc_parallel.c b/drivers/staging/media/lirc/lirc_parallel.c
index 62f5137b947..1394f020e46 100644
--- a/drivers/staging/media/lirc/lirc_parallel.c
+++ b/drivers/staging/media/lirc/lirc_parallel.c
@@ -324,7 +324,8 @@ static loff_t lirc_lseek(struct file *filep, loff_t offset, int orig)
 	return -ESPIPE;
 }
 
-static ssize_t lirc_read(struct file *filep, char *buf, size_t n, loff_t *ppos)
+static ssize_t lirc_read(struct file *filep, char __user *buf, size_t n,
+			 loff_t *ppos)
 {
 	int result = 0;
 	int count = 0;
@@ -362,7 +363,7 @@ static ssize_t lirc_read(struct file *filep, char *buf, size_t n, loff_t *ppos)
 	return count ? count : result;
 }
 
-static ssize_t lirc_write(struct file *filep, const char *buf, size_t n,
+static ssize_t lirc_write(struct file *filep, const char __user *buf, size_t n,
 			  loff_t *ppos)
 {
 	int count;
@@ -463,43 +464,44 @@ static unsigned int lirc_poll(struct file *file, poll_table *wait)
 static long lirc_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 {
 	int result;
-	__u32 features = LIRC_CAN_SET_TRANSMITTER_MASK |
-			 LIRC_CAN_SEND_PULSE | LIRC_CAN_REC_MODE2;
-	__u32 mode;
-	__u32 value;
+	u32 __user *uptr = (u32 __user *)arg;
+	u32 features = LIRC_CAN_SET_TRANSMITTER_MASK |
+		       LIRC_CAN_SEND_PULSE | LIRC_CAN_REC_MODE2;
+	u32 mode;
+	u32 value;
 
 	switch (cmd) {
 	case LIRC_GET_FEATURES:
-		result = put_user(features, (__u32 *) arg);
+		result = put_user(features, uptr);
 		if (result)
 			return result;
 		break;
 	case LIRC_GET_SEND_MODE:
-		result = put_user(LIRC_MODE_PULSE, (__u32 *) arg);
+		result = put_user(LIRC_MODE_PULSE, uptr);
 		if (result)
 			return result;
 		break;
 	case LIRC_GET_REC_MODE:
-		result = put_user(LIRC_MODE_MODE2, (__u32 *) arg);
+		result = put_user(LIRC_MODE_MODE2, uptr);
 		if (result)
 			return result;
 		break;
 	case LIRC_SET_SEND_MODE:
-		result = get_user(mode, (__u32 *) arg);
+		result = get_user(mode, uptr);
 		if (result)
 			return result;
 		if (mode != LIRC_MODE_PULSE)
 			return -EINVAL;
 		break;
 	case LIRC_SET_REC_MODE:
-		result = get_user(mode, (__u32 *) arg);
+		result = get_user(mode, uptr);
 		if (result)
 			return result;
 		if (mode != LIRC_MODE_MODE2)
 			return -ENOSYS;
 		break;
 	case LIRC_SET_TRANSMITTER_MASK:
-		result = get_user(value, (__u32 *) arg);
+		result = get_user(value, uptr);
 		if (result)
 			return result;
 		if ((value & LIRC_PARALLEL_TRANSMITTER_MASK) != value)
diff --git a/drivers/staging/media/lirc/lirc_serial.c b/drivers/staging/media/lirc/lirc_serial.c
index 10c685d5de7..efe561cd093 100644
--- a/drivers/staging/media/lirc/lirc_serial.c
+++ b/drivers/staging/media/lirc/lirc_serial.c
@@ -73,9 +73,6 @@
 #include <linux/fcntl.h>
 #include <linux/spinlock.h>
 
-#ifdef CONFIG_LIRC_SERIAL_NSLU2
-#include <asm/hardware.h>
-#endif
 /* From Intel IXP42X Developer's Manual (#252480-005): */
 /* ftp://download.intel.com/design/network/manuals/25248005.pdf */
 #define UART_IE_IXP42X_UUE   0x40 /* IXP42X UART Unit enable */
@@ -198,33 +195,6 @@ static struct lirc_serial hardware[] = {
 		.features    = LIRC_CAN_REC_MODE2
 #endif
 	},
-
-#ifdef CONFIG_LIRC_SERIAL_NSLU2
-	/*
-	 * Modified Linksys Network Storage Link USB 2.0 (NSLU2):
-	 * We receive on CTS of the 2nd serial port (R142,LHS), we
-	 * transmit with a IR diode between GPIO[1] (green status LED),
-	 * and ground (Matthias Goebl <matthias.goebl@goebl.net>).
-	 * See also http://www.nslu2-linux.org for this device
-	 */
-	[LIRC_NSLU2] = {
-		.lock = __SPIN_LOCK_UNLOCKED(hardware[LIRC_NSLU2].lock),
-		.signal_pin        = UART_MSR_CTS,
-		.signal_pin_change = UART_MSR_DCTS,
-		.on  = (UART_MCR_RTS | UART_MCR_OUT2 | UART_MCR_DTR),
-		.off = (UART_MCR_RTS | UART_MCR_OUT2),
-		.send_pulse = send_pulse_homebrew,
-		.send_space = send_space_homebrew,
-#ifdef CONFIG_LIRC_SERIAL_TRANSMITTER
-		.features    = (LIRC_CAN_SET_SEND_DUTY_CYCLE |
-				LIRC_CAN_SET_SEND_CARRIER |
-				LIRC_CAN_SEND_PULSE | LIRC_CAN_REC_MODE2)
-#else
-		.features    = LIRC_CAN_REC_MODE2
-#endif
-	},
-#endif
-
 };
 
 #define RS_ISR_PASS_LIMIT 256
@@ -315,16 +285,6 @@ static void soutp(int offset, u8 value)
 
 static void on(void)
 {
-#ifdef CONFIG_LIRC_SERIAL_NSLU2
-	/*
-	 * On NSLU2, we put the transmit diode between the output of the green
-	 * status LED and ground
-	 */
-	if (type == LIRC_NSLU2) {
-		gpio_set_value(NSLU2_LED_GRN, 0);
-		return;
-	}
-#endif
 	if (txsense)
 		soutp(UART_MCR, hardware[type].off);
 	else
@@ -333,12 +293,6 @@ static void on(void)
 
 static void off(void)
 {
-#ifdef CONFIG_LIRC_SERIAL_NSLU2
-	if (type == LIRC_NSLU2) {
-		gpio_set_value(NSLU2_LED_GRN, 1);
-		return;
-	}
-#endif
 	if (txsense)
 		soutp(UART_MCR, hardware[type].on);
 	else
@@ -793,20 +747,6 @@ static int hardware_init_port(void)
 	sinp(UART_IIR);
 	sinp(UART_MSR);
 
-#ifdef CONFIG_LIRC_SERIAL_NSLU2
-	if (type == LIRC_NSLU2) {
-		/* Setup NSLU2 UART */
-
-		/* Enable UART */
-		soutp(UART_IER, sinp(UART_IER) | UART_IE_IXP42X_UUE);
-		/* Disable Receiver data Time out interrupt */
-		soutp(UART_IER, sinp(UART_IER) & ~UART_IE_IXP42X_RTOIE);
-		/* set out2 = interrupt unmask; off() doesn't set MCR
-		   on NSLU2 */
-		soutp(UART_MCR, UART_MCR_RTS|UART_MCR_OUT2);
-	}
-#endif
-
 	/* Set line for power source */
 	off();
 
@@ -842,16 +782,6 @@ static int lirc_serial_probe(struct platform_device *dev)
 {
 	int i, nlow, nhigh, result;
 
-#ifdef CONFIG_LIRC_SERIAL_NSLU2
-	/* This GPIO is used for a LED on the NSLU2 */
-	result = devm_gpio_request(dev, NSLU2_LED_GRN, "lirc-serial");
-	if (result)
-		return result;
-	result = gpio_direction_output(NSLU2_LED_GRN, 0);
-	if (result)
-		return result;
-#endif
-
 	result = request_irq(irq, lirc_irq_handler,
 			     (share_irq ? IRQF_SHARED : 0),
 			     LIRC_DRIVER_NAME, (void *)&hardware);
@@ -1011,7 +941,8 @@ static ssize_t lirc_write(struct file *file, const char __user *buf,
 static long lirc_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 {
 	int result;
-	__u32 value;
+	u32 __user *uptr = (u32 __user *)arg;
+	u32 value;
 
 	switch (cmd) {
 	case LIRC_GET_SEND_MODE:
@@ -1020,7 +951,7 @@ static long lirc_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 
 		result = put_user(LIRC_SEND2MODE
 				  (hardware[type].features&LIRC_CAN_SEND_MASK),
-				  (__u32 *) arg);
+				  uptr);
 		if (result)
 			return result;
 		break;
@@ -1029,7 +960,7 @@ static long lirc_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 		if (!(hardware[type].features&LIRC_CAN_SEND_MASK))
 			return -ENOIOCTLCMD;
 
-		result = get_user(value, (__u32 *) arg);
+		result = get_user(value, uptr);
 		if (result)
 			return result;
 		/* only LIRC_MODE_PULSE supported */
@@ -1046,7 +977,7 @@ static long lirc_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 		if (!(hardware[type].features&LIRC_CAN_SET_SEND_DUTY_CYCLE))
 			return -ENOIOCTLCMD;
 
-		result = get_user(value, (__u32 *) arg);
+		result = get_user(value, uptr);
 		if (result)
 			return result;
 		if (value <= 0 || value > 100)
@@ -1059,7 +990,7 @@ static long lirc_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 		if (!(hardware[type].features&LIRC_CAN_SET_SEND_CARRIER))
 			return -ENOIOCTLCMD;
 
-		result = get_user(value, (__u32 *) arg);
+		result = get_user(value, uptr);
 		if (result)
 			return result;
 		if (value > 500000 || value < 20000)
@@ -1216,14 +1147,6 @@ static int __init lirc_serial_init_module(void)
 		io = io ? io : 0x3f8;
 		irq = irq ? irq : 4;
 		break;
-#ifdef CONFIG_LIRC_SERIAL_NSLU2
-	case LIRC_NSLU2:
-		io = io ? io : IRQ_IXP4XX_UART2;
-		irq = irq ? irq : (IXP4XX_UART2_BASE_VIRT + REG_OFFSET);
-		iommap = iommap ? iommap : IXP4XX_UART2_BASE_PHYS;
-		ioshift = ioshift ? ioshift : 2;
-		break;
-#endif
 	default:
 		return -EINVAL;
 	}
@@ -1231,9 +1154,6 @@ static int __init lirc_serial_init_module(void)
 		switch (type) {
 		case LIRC_HOMEBREW:
 		case LIRC_IGOR:
-#ifdef CONFIG_LIRC_SERIAL_NSLU2
-		case LIRC_NSLU2:
-#endif
 			hardware[type].features &=
 				~(LIRC_CAN_SET_SEND_DUTY_CYCLE|
 				  LIRC_CAN_SET_SEND_CARRIER);
diff --git a/drivers/staging/media/lirc/lirc_sir.c b/drivers/staging/media/lirc/lirc_sir.c
index f781c532b3c..e31cbb81f05 100644
--- a/drivers/staging/media/lirc/lirc_sir.c
+++ b/drivers/staging/media/lirc/lirc_sir.c
@@ -187,10 +187,10 @@ static bool debug;
 
 /* Communication with user-space */
 static unsigned int lirc_poll(struct file *file, poll_table *wait);
-static ssize_t lirc_read(struct file *file, char *buf, size_t count,
-		loff_t *ppos);
-static ssize_t lirc_write(struct file *file, const char *buf, size_t n,
-		loff_t *pos);
+static ssize_t lirc_read(struct file *file, char __user *buf, size_t count,
+			 loff_t *ppos);
+static ssize_t lirc_write(struct file *file, const char __user *buf, size_t n,
+			  loff_t *pos);
 static long lirc_ioctl(struct file *filep, unsigned int cmd, unsigned long arg);
 static void add_read_queue(int flag, unsigned long val);
 static int init_chrdev(void);
@@ -252,8 +252,8 @@ static unsigned int lirc_poll(struct file *file, poll_table *wait)
 	return 0;
 }
 
-static ssize_t lirc_read(struct file *file, char *buf, size_t count,
-		loff_t *ppos)
+static ssize_t lirc_read(struct file *file, char __user *buf, size_t count,
+			 loff_t *ppos)
 {
 	int n = 0;
 	int retval = 0;
@@ -266,9 +266,9 @@ static ssize_t lirc_read(struct file *file, char *buf, size_t count,
 	set_current_state(TASK_INTERRUPTIBLE);
 	while (n < count) {
 		if (rx_head != rx_tail) {
-			if (copy_to_user((void *) buf + n,
-					(void *) (rx_buf + rx_head),
-					sizeof(int))) {
+			if (copy_to_user(buf + n,
+					 rx_buf + rx_head,
+					 sizeof(int))) {
 				retval = -EFAULT;
 				break;
 			}
@@ -291,8 +291,8 @@ static ssize_t lirc_read(struct file *file, char *buf, size_t count,
 	set_current_state(TASK_RUNNING);
 	return n ? n : retval;
 }
-static ssize_t lirc_write(struct file *file, const char *buf, size_t n,
-				loff_t *pos)
+static ssize_t lirc_write(struct file *file, const char __user *buf, size_t n,
+			  loff_t *pos)
 {
 	unsigned long flags;
 	int i, count;
@@ -338,8 +338,9 @@ static ssize_t lirc_write(struct file *file, const char *buf, size_t n,
 
 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;
+	u32 value = 0;
 #ifdef LIRC_ON_SA1100
 
 	if (cmd == LIRC_GET_FEATURES)
@@ -364,16 +365,16 @@ static long lirc_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 	case LIRC_GET_FEATURES:
 	case LIRC_GET_SEND_MODE:
 	case LIRC_GET_REC_MODE:
-		retval = put_user(value, (__u32 *) arg);
+		retval = put_user(value, uptr);
 		break;
 
 	case LIRC_SET_SEND_MODE:
 	case LIRC_SET_REC_MODE:
-		retval = get_user(value, (__u32 *) arg);
+		retval = get_user(value, uptr);
 		break;
 #ifdef LIRC_ON_SA1100
 	case LIRC_SET_SEND_DUTY_CYCLE:
-		retval = get_user(value, (__u32 *) arg);
+		retval = get_user(value, uptr);
 		if (retval)
 			return retval;
 		if (value <= 0 || value > 100)
@@ -388,7 +389,7 @@ static long lirc_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 			space_width -= LIRC_ON_SA1100_TRANSMITTER_LATENCY;
 		break;
 	case LIRC_SET_SEND_CARRIER:
-		retval = get_user(value, (__u32 *) arg);
+		retval = get_user(value, uptr);
 		if (retval)
 			return retval;
 		if (value > 500000 || value < 20000)
diff --git a/drivers/staging/media/lirc/lirc_zilog.c b/drivers/staging/media/lirc/lirc_zilog.c
index e1feb616459..3259aacfd35 100644
--- a/drivers/staging/media/lirc/lirc_zilog.c
+++ b/drivers/staging/media/lirc/lirc_zilog.c
@@ -892,7 +892,8 @@ out:
 }
 
 /* copied from lirc_dev */
-static ssize_t read(struct file *filep, char *outbuf, size_t n, loff_t *ppos)
+static ssize_t read(struct file *filep, char __user *outbuf, size_t n,
+		    loff_t *ppos)
 {
 	struct IR *ir = filep->private_data;
 	struct IR_rx *rx;
@@ -954,7 +955,7 @@ static ssize_t read(struct file *filep, char *outbuf, size_t n, loff_t *ppos)
 			}
 			m = lirc_buffer_read(rbuf, buf);
 			if (m == rbuf->chunk_size) {
-				ret = copy_to_user((void *)outbuf+written, buf,
+				ret = copy_to_user(outbuf + written, buf,
 						   rbuf->chunk_size);
 				written += rbuf->chunk_size;
 			} else {
@@ -1094,8 +1095,8 @@ static int send_code(struct IR_tx *tx, unsigned int code, unsigned int key)
  * sent to the device.  We have a spin lock as per i2c documentation to prevent
  * multiple concurrent sends which would probably cause the device to explode.
  */
-static ssize_t write(struct file *filep, const char *buf, size_t n,
-			  loff_t *ppos)
+static ssize_t write(struct file *filep, const char __user *buf, size_t n,
+		     loff_t *ppos)
 {
 	struct IR *ir = filep->private_data;
 	struct IR_tx *tx;
@@ -1237,6 +1238,7 @@ static unsigned int poll(struct file *filep, poll_table *wait)
 static long ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 {
 	struct IR *ir = filep->private_data;
+	unsigned long __user *uptr = (unsigned long __user *)arg;
 	int result;
 	unsigned long mode, features;
 
@@ -1244,11 +1246,10 @@ static long ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 
 	switch (cmd) {
 	case LIRC_GET_LENGTH:
-		result = put_user((unsigned long)13,
-				  (unsigned long *)arg);
+		result = put_user(13UL, uptr);
 		break;
 	case LIRC_GET_FEATURES:
-		result = put_user(features, (unsigned long *) arg);
+		result = put_user(features, uptr);
 		break;
 	case LIRC_GET_REC_MODE:
 		if (!(features&LIRC_CAN_REC_MASK))
@@ -1256,13 +1257,13 @@ static long ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 
 		result = put_user(LIRC_REC2MODE
 				  (features&LIRC_CAN_REC_MASK),
-				  (unsigned long *)arg);
+				  uptr);
 		break;
 	case LIRC_SET_REC_MODE:
 		if (!(features&LIRC_CAN_REC_MASK))
 			return -ENOSYS;
 
-		result = get_user(mode, (unsigned long *)arg);
+		result = get_user(mode, uptr);
 		if (!result && !(LIRC_MODE2REC(mode) & features))
 			result = -EINVAL;
 		break;
@@ -1270,13 +1271,13 @@ static long ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 		if (!(features&LIRC_CAN_SEND_MASK))
 			return -ENOSYS;
 
-		result = put_user(LIRC_MODE_PULSE, (unsigned long *) arg);
+		result = put_user(LIRC_MODE_PULSE, uptr);
 		break;
 	case LIRC_SET_SEND_MODE:
 		if (!(features&LIRC_CAN_SEND_MASK))
 			return -ENOSYS;
 
-		result = get_user(mode, (unsigned long *) arg);
+		result = get_user(mode, uptr);
 		if (!result && mode != LIRC_MODE_PULSE)
 			return -EINVAL;
 		break;
diff --git a/drivers/staging/media/msi3101/sdr-msi3101.c b/drivers/staging/media/msi3101/sdr-msi3101.c
index 65d351f99da..08d0d096b88 100644
--- a/drivers/staging/media/msi3101/sdr-msi3101.c
+++ b/drivers/staging/media/msi3101/sdr-msi3101.c
@@ -1074,14 +1074,13 @@ static int msi3101_start_streaming(struct vb2_queue *vq, unsigned int count)
 	return ret;
 }
 
-static int msi3101_stop_streaming(struct vb2_queue *vq)
+static void msi3101_stop_streaming(struct vb2_queue *vq)
 {
 	struct msi3101_state *s = vb2_get_drv_priv(vq);
-	int ret;
+
 	dev_dbg(&s->udev->dev, "%s:\n", __func__);
 
-	if (mutex_lock_interruptible(&s->v4l2_lock))
-		return -ERESTARTSYS;
+	mutex_lock(&s->v4l2_lock);
 
 	if (s->udev)
 		msi3101_isoc_cleanup(s);
@@ -1090,22 +1089,15 @@ static int msi3101_stop_streaming(struct vb2_queue *vq)
 
 	/* according to tests, at least 700us delay is required  */
 	msleep(20);
-	ret = msi3101_ctrl_msg(s, CMD_STOP_STREAMING, 0);
-	if (ret)
-		goto err_sleep_tuner;
-
-	/* sleep USB IF / ADC */
-	ret = msi3101_ctrl_msg(s, CMD_WREG, 0x01000003);
-	if (ret)
-		goto err_sleep_tuner;
+	if (!msi3101_ctrl_msg(s, CMD_STOP_STREAMING, 0)) {
+		/* sleep USB IF / ADC */
+		msi3101_ctrl_msg(s, CMD_WREG, 0x01000003);
+	}
 
-err_sleep_tuner:
 	/* sleep tuner */
-	ret = v4l2_subdev_call(s->v4l2_subdev, core, s_power, 0);
+	v4l2_subdev_call(s->v4l2_subdev, core, s_power, 0);
 
 	mutex_unlock(&s->v4l2_lock);
-
-	return ret;
 }
 
 static struct vb2_ops msi3101_vb2_ops = {
diff --git a/drivers/staging/media/omap24xx/tcm825x.c b/drivers/staging/media/omap24xx/tcm825x.c
index f4dd32df235..3367ccd1d1e 100644
--- a/drivers/staging/media/omap24xx/tcm825x.c
+++ b/drivers/staging/media/omap24xx/tcm825x.c
@@ -89,10 +89,10 @@ static const struct tcm825x_reg rgb565	=	{ 0x02, TCM825X_PICFMT };
 
 /* Our own specific controls */
 #define V4L2_CID_ALC				V4L2_CID_PRIVATE_BASE
-#define V4L2_CID_H_EDGE_EN			V4L2_CID_PRIVATE_BASE + 1
-#define V4L2_CID_V_EDGE_EN			V4L2_CID_PRIVATE_BASE + 2
-#define V4L2_CID_LENS				V4L2_CID_PRIVATE_BASE + 3
-#define V4L2_CID_MAX_EXPOSURE_TIME		V4L2_CID_PRIVATE_BASE + 4
+#define V4L2_CID_H_EDGE_EN			(V4L2_CID_PRIVATE_BASE + 1)
+#define V4L2_CID_V_EDGE_EN			(V4L2_CID_PRIVATE_BASE + 2)
+#define V4L2_CID_LENS				(V4L2_CID_PRIVATE_BASE + 3)
+#define V4L2_CID_MAX_EXPOSURE_TIME		(V4L2_CID_PRIVATE_BASE + 4)
 #define V4L2_CID_LAST_PRIV			V4L2_CID_MAX_EXPOSURE_TIME
 
 /*  Video controls  */
@@ -914,8 +914,8 @@ static int __init tcm825x_init(void)
 
 	rval = i2c_add_driver(&tcm825x_i2c_driver);
 	if (rval)
-		printk(KERN_INFO "%s: failed registering " TCM825X_NAME "\n",
-		       __func__);
+		pr_info("%s: failed registering " TCM825X_NAME "\n",
+			__func__);
 
 	return rval;
 }
diff --git a/drivers/staging/media/omap24xx/tcm825x.h b/drivers/staging/media/omap24xx/tcm825x.h
index 9970fb1c596..8a29636d1ad 100644
--- a/drivers/staging/media/omap24xx/tcm825x.h
+++ b/drivers/staging/media/omap24xx/tcm825x.h
@@ -21,8 +21,8 @@
 
 #define TCM825X_NAME "tcm825x"
 
-#define TCM825X_MASK(x)  x & 0x00ff
-#define TCM825X_ADDR(x) (x & 0xff00) >> 8
+#define TCM825X_MASK(x) (x & 0x00ff)
+#define TCM825X_ADDR(x) ((x & 0xff00) >> 8)
 
 /* The TCM825X I2C sensor chip has a fixed slave address of 0x3d. */
 #define TCM825X_I2C_ADDR	0x3d
diff --git a/drivers/staging/media/omap4iss/Kconfig b/drivers/staging/media/omap4iss/Kconfig
index b9fe753969b..78b0fba7047 100644
--- a/drivers/staging/media/omap4iss/Kconfig
+++ b/drivers/staging/media/omap4iss/Kconfig
@@ -4,9 +4,3 @@ config VIDEO_OMAP4
 	select VIDEOBUF2_DMA_CONTIG
 	---help---
 	  Driver for an OMAP 4 ISS controller.
-
-config VIDEO_OMAP4_DEBUG
-	bool "OMAP 4 Camera debug messages"
-	depends on VIDEO_OMAP4
-	---help---
-	  Enable debug messages on OMAP 4 ISS controller driver.
diff --git a/drivers/staging/media/omap4iss/iss.c b/drivers/staging/media/omap4iss/iss.c
index 61fbfcd1358..2e422dde074 100644
--- a/drivers/staging/media/omap4iss/iss.c
+++ b/drivers/staging/media/omap4iss/iss.c
@@ -204,7 +204,7 @@ void omap4iss_configure_bridge(struct iss_device *iss,
 	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_CTRL, isp5ctrl_val);
 }
 
-#if defined(DEBUG) && defined(ISS_ISR_DEBUG)
+#ifdef ISS_ISR_DEBUG
 static void iss_isr_dbg(struct iss_device *iss, u32 irqstatus)
 {
 	static const char * const name[] = {
@@ -347,14 +347,14 @@ static irqreturn_t iss_isr(int irq, void *_iss)
 			omap4iss_resizer_isr(&iss->resizer,
 					     isp_irqstatus & resizer_events);
 
-#if defined(DEBUG) && defined(ISS_ISR_DEBUG)
+#ifdef ISS_ISR_DEBUG
 		iss_isp_isr_dbg(iss, isp_irqstatus);
 #endif
 	}
 
 	omap4iss_flush(iss);
 
-#if defined(DEBUG) && defined(ISS_ISR_DEBUG)
+#ifdef ISS_ISR_DEBUG
 	iss_isr_dbg(iss, irqstatus);
 #endif
 
@@ -734,18 +734,17 @@ static int iss_pipeline_is_last(struct media_entity *me)
 
 static int iss_reset(struct iss_device *iss)
 {
-	unsigned long timeout = 0;
+	unsigned int timeout;
 
 	iss_reg_set(iss, OMAP4_ISS_MEM_TOP, ISS_HL_SYSCONFIG,
 		    ISS_HL_SYSCONFIG_SOFTRESET);
 
-	while (iss_reg_read(iss, OMAP4_ISS_MEM_TOP, ISS_HL_SYSCONFIG) &
-	       ISS_HL_SYSCONFIG_SOFTRESET) {
-		if (timeout++ > 100) {
-			dev_alert(iss->dev, "cannot reset ISS\n");
-			return -ETIMEDOUT;
-		}
-		usleep_range(10, 10);
+	timeout = iss_poll_condition_timeout(
+		!(iss_reg_read(iss, OMAP4_ISS_MEM_TOP, ISS_HL_SYSCONFIG) &
+		ISS_HL_SYSCONFIG_SOFTRESET), 1000, 10, 100);
+	if (timeout) {
+		dev_err(iss->dev, "ISS reset timeout\n");
+		return -ETIMEDOUT;
 	}
 
 	iss->crashed = 0;
@@ -754,7 +753,7 @@ static int iss_reset(struct iss_device *iss)
 
 static int iss_isp_reset(struct iss_device *iss)
 {
-	unsigned long timeout = 0;
+	unsigned int timeout;
 
 	/* Fist, ensure that the ISP is IDLE (no transactions happening) */
 	iss_reg_update(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_SYSCONFIG,
@@ -763,29 +762,24 @@ static int iss_isp_reset(struct iss_device *iss)
 
 	iss_reg_set(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_CTRL, ISP5_CTRL_MSTANDBY);
 
-	for (;;) {
-		if (iss_reg_read(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_CTRL) &
-		    ISP5_CTRL_MSTANDBY_WAIT)
-			break;
-		if (timeout++ > 1000) {
-			dev_alert(iss->dev, "cannot set ISP5 to standby\n");
-			return -ETIMEDOUT;
-		}
-		usleep_range(1000, 1500);
+	timeout = iss_poll_condition_timeout(
+		iss_reg_read(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_CTRL) &
+		ISP5_CTRL_MSTANDBY_WAIT, 1000000, 1000, 1500);
+	if (timeout) {
+		dev_err(iss->dev, "ISP5 standby timeout\n");
+		return -ETIMEDOUT;
 	}
 
 	/* Now finally, do the reset */
 	iss_reg_set(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_SYSCONFIG,
 		    ISP5_SYSCONFIG_SOFTRESET);
 
-	timeout = 0;
-	while (iss_reg_read(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_SYSCONFIG) &
-	       ISP5_SYSCONFIG_SOFTRESET) {
-		if (timeout++ > 1000) {
-			dev_alert(iss->dev, "cannot reset ISP5\n");
-			return -ETIMEDOUT;
-		}
-		usleep_range(1000, 1500);
+	timeout = iss_poll_condition_timeout(
+		!(iss_reg_read(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_SYSCONFIG) &
+		ISP5_SYSCONFIG_SOFTRESET), 1000000, 1000, 1500);
+	if (timeout) {
+		dev_err(iss->dev, "ISP5 reset timeout\n");
+		return -ETIMEDOUT;
 	}
 
 	return 0;
diff --git a/drivers/staging/media/omap4iss/iss.h b/drivers/staging/media/omap4iss/iss.h
index 346db923317..05cd9bf3b41 100644
--- a/drivers/staging/media/omap4iss/iss.h
+++ b/drivers/staging/media/omap4iss/iss.h
@@ -233,4 +233,18 @@ void iss_reg_update(struct iss_device *iss, enum iss_mem_resources res,
 	iss_reg_write(iss, res, offset, (v & ~clr) | set);
 }
 
+#define iss_poll_condition_timeout(cond, timeout, min_ival, max_ival)	\
+({									\
+	unsigned long __timeout = jiffies + usecs_to_jiffies(timeout);	\
+	unsigned int __min_ival = (min_ival);				\
+	unsigned int __max_ival = (max_ival);				\
+	bool __cond;							\
+	while (!(__cond = (cond))) {					\
+		if (time_after(jiffies, __timeout))			\
+			break;						\
+		usleep_range(__min_ival, __max_ival);			\
+	}								\
+	!__cond;							\
+})
+
 #endif /* _OMAP4_ISS_H_ */
diff --git a/drivers/staging/media/omap4iss/iss_csi2.c b/drivers/staging/media/omap4iss/iss_csi2.c
index 61fc350eb25..bf8a6572610 100644
--- a/drivers/staging/media/omap4iss/iss_csi2.c
+++ b/drivers/staging/media/omap4iss/iss_csi2.c
@@ -487,9 +487,7 @@ static void csi2_irq_status_set(struct iss_csi2_device *csi2, int enable)
  */
 int omap4iss_csi2_reset(struct iss_csi2_device *csi2)
 {
-	u8 soft_reset_retries = 0;
-	u32 reg;
-	int i;
+	unsigned int timeout;
 
 	if (!csi2->available)
 		return -ENODEV;
@@ -500,37 +498,22 @@ int omap4iss_csi2_reset(struct iss_csi2_device *csi2)
 	iss_reg_set(csi2->iss, csi2->regs1, CSI2_SYSCONFIG,
 		    CSI2_SYSCONFIG_SOFT_RESET);
 
-	do {
-		reg = iss_reg_read(csi2->iss, csi2->regs1, CSI2_SYSSTATUS)
-		    & CSI2_SYSSTATUS_RESET_DONE;
-		if (reg == CSI2_SYSSTATUS_RESET_DONE)
-			break;
-		soft_reset_retries++;
-		if (soft_reset_retries < 5)
-			usleep_range(100, 100);
-	} while (soft_reset_retries < 5);
-
-	if (soft_reset_retries == 5) {
-		dev_err(csi2->iss->dev,
-			"CSI2: Soft reset try count exceeded!\n");
+	timeout = iss_poll_condition_timeout(
+		iss_reg_read(csi2->iss, csi2->regs1, CSI2_SYSSTATUS) &
+		CSI2_SYSSTATUS_RESET_DONE, 500, 100, 200);
+	if (timeout) {
+		dev_err(csi2->iss->dev, "CSI2: Soft reset timeout!\n");
 		return -EBUSY;
 	}
 
 	iss_reg_set(csi2->iss, csi2->regs1, CSI2_COMPLEXIO_CFG,
 		    CSI2_COMPLEXIO_CFG_RESET_CTRL);
 
-	i = 100;
-	do {
-		reg = iss_reg_read(csi2->iss, csi2->phy->phy_regs, REGISTER1)
-		    & REGISTER1_RESET_DONE_CTRLCLK;
-		if (reg == REGISTER1_RESET_DONE_CTRLCLK)
-			break;
-		usleep_range(100, 100);
-	} while (--i > 0);
-
-	if (i == 0) {
-		dev_err(csi2->iss->dev,
-			"CSI2: Reset for CSI2_96M_FCLK domain Failed!\n");
+	timeout = iss_poll_condition_timeout(
+		iss_reg_read(csi2->iss, csi2->phy->phy_regs, REGISTER1) &
+		REGISTER1_RESET_DONE_CTRLCLK, 10000, 100, 500);
+	if (timeout) {
+		dev_err(csi2->iss->dev, "CSI2: CSI2_96M_FCLK reset timeout!\n");
 		return -EBUSY;
 	}
 
diff --git a/drivers/staging/media/omap4iss/iss_video.h b/drivers/staging/media/omap4iss/iss_video.h
index 878e4a3082e..9dccdb154e1 100644
--- a/drivers/staging/media/omap4iss/iss_video.h
+++ b/drivers/staging/media/omap4iss/iss_video.h
@@ -140,7 +140,7 @@ enum iss_video_dmaqueue_flags {
  *		if there was no buffer previously queued.
  */
 struct iss_video_operations {
-	int(*queue)(struct iss_video *video, struct iss_buffer *buffer);
+	int (*queue)(struct iss_video *video, struct iss_buffer *buffer);
 };
 
 struct iss_video {
diff --git a/drivers/staging/media/rtl2832u_sdr/rtl2832_sdr.c b/drivers/staging/media/rtl2832u_sdr/rtl2832_sdr.c
index 104ee8af79a..093df6b6ae3 100644
--- a/drivers/staging/media/rtl2832u_sdr/rtl2832_sdr.c
+++ b/drivers/staging/media/rtl2832u_sdr/rtl2832_sdr.c
@@ -1032,13 +1032,12 @@ err:
 	return ret;
 }
 
-static int rtl2832_sdr_stop_streaming(struct vb2_queue *vq)
+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__);
 
-	if (mutex_lock_interruptible(&s->v4l2_lock))
-		return -ERESTARTSYS;
+	mutex_lock(&s->v4l2_lock);
 
 	rtl2832_sdr_kill_urbs(s);
 	rtl2832_sdr_free_urbs(s);
@@ -1053,8 +1052,6 @@ static int rtl2832_sdr_stop_streaming(struct vb2_queue *vq)
 		s->d->props->power_ctrl(s->d, 0);
 
 	mutex_unlock(&s->v4l2_lock);
-
-	return 0;
 }
 
 static struct vb2_ops rtl2832_sdr_vb2_ops = {
diff --git a/drivers/staging/media/sn9c102/sn9c102.h b/drivers/staging/media/sn9c102/sn9c102.h
index 8a917f06050..37ca7225fcf 100644
--- a/drivers/staging/media/sn9c102/sn9c102.h
+++ b/drivers/staging/media/sn9c102/sn9c102.h
@@ -53,7 +53,7 @@ enum sn9c102_frame_state {
 };
 
 struct sn9c102_frame_t {
-	void* bufmem;
+	void *bufmem;
 	struct v4l2_buffer buf;
 	enum sn9c102_frame_state state;
 	struct list_head frame;
@@ -99,17 +99,17 @@ static DEFINE_MUTEX(sn9c102_sysfs_lock);
 static DECLARE_RWSEM(sn9c102_dev_lock);
 
 struct sn9c102_device {
-	struct video_device* v4ldev;
+	struct video_device *v4ldev;
 
 	struct v4l2_device v4l2_dev;
 
 	enum sn9c102_bridge bridge;
 	struct sn9c102_sensor sensor;
 
-	struct usb_device* usbdev;
-	struct urb* urb[SN9C102_URBS];
-	void* transfer_buffer[SN9C102_URBS];
-	u8* control_buffer;
+	struct usb_device *usbdev;
+	struct urb *urb[SN9C102_URBS];
+	void *transfer_buffer[SN9C102_URBS];
+	u8 *control_buffer;
 
 	struct sn9c102_frame_t *frame_current, frame[SN9C102_MAX_FRAMES];
 	struct list_head inqueue, outqueue;
@@ -139,28 +139,28 @@ struct sn9c102_device {
 /*****************************************************************************/
 
 struct sn9c102_device*
-sn9c102_match_id(struct sn9c102_device* cam, const struct usb_device_id *id)
+sn9c102_match_id(struct sn9c102_device *cam, const struct usb_device_id *id)
 {
 	return usb_match_id(usb_ifnum_to_if(cam->usbdev, 0), id) ? cam : NULL;
 }
 
 
 void
-sn9c102_attach_sensor(struct sn9c102_device* cam,
-		      const struct sn9c102_sensor* sensor)
+sn9c102_attach_sensor(struct sn9c102_device *cam,
+		      const struct sn9c102_sensor *sensor)
 {
 	memcpy(&cam->sensor, sensor, sizeof(struct sn9c102_sensor));
 }
 
 
 enum sn9c102_bridge
-sn9c102_get_bridge(struct sn9c102_device* cam)
+sn9c102_get_bridge(struct sn9c102_device *cam)
 {
 	return cam->bridge;
 }
 
 
-struct sn9c102_sensor* sn9c102_get_sensor(struct sn9c102_device* cam)
+struct sn9c102_sensor *sn9c102_get_sensor(struct sn9c102_device *cam)
 {
 	return &cam->sensor;
 }
@@ -198,9 +198,9 @@ do {                                                                          \
 	}                                                                     \
 } while (0)
 #else
-#	define DBG(level, fmt, args...) do {;} while(0)
-#	define V4LDBG(level, name, cmd) do {;} while(0)
-#	define KDBG(level, fmt, args...) do {;} while(0)
+#	define DBG(level, fmt, args...) do { ; } while (0)
+#	define V4LDBG(level, name, cmd) do { ; } while (0)
+#	define KDBG(level, fmt, args...) do { ; } while (0)
 #endif
 
 #undef PDBG
@@ -209,6 +209,6 @@ dev_info(&cam->usbdev->dev, "[%s:%s:%d] " fmt "\n", __FILE__, __func__,   \
 	 __LINE__ , ## args)
 
 #undef PDBGG
-#define PDBGG(fmt, args...) do {;} while(0) /* placeholder */
+#define PDBGG(fmt, args...) do { ; } while (0) /* placeholder */
 
 #endif /* _SN9C102_H_ */
diff --git a/drivers/staging/media/sn9c102/sn9c102_core.c b/drivers/staging/media/sn9c102/sn9c102_core.c
index 71f594f5aa7..98b30579b0a 100644
--- a/drivers/staging/media/sn9c102/sn9c102_core.c
+++ b/drivers/staging/media/sn9c102/sn9c102_core.c
@@ -139,15 +139,15 @@ static int (*sn9c102_sensor_table[])(struct sn9c102_device *) = {
 /*****************************************************************************/
 
 static u32
-sn9c102_request_buffers(struct sn9c102_device* cam, u32 count,
+sn9c102_request_buffers(struct sn9c102_device *cam, u32 count,
 			enum sn9c102_io_method io)
 {
-	struct v4l2_pix_format* p = &(cam->sensor.pix_format);
-	struct v4l2_rect* r = &(cam->sensor.cropcap.bounds);
+	struct v4l2_pix_format *p = &(cam->sensor.pix_format);
+	struct v4l2_rect *r = &(cam->sensor.cropcap.bounds);
 	size_t imagesize = cam->module_param.force_munmap || io == IO_READ ?
 			   (p->width * p->height * p->priv) / 8 :
 			   (r->width * r->height * p->priv) / 8;
-	void* buff = NULL;
+	void *buff = NULL;
 	u32 i;
 
 	if (count > SN9C102_MAX_FRAMES)
@@ -180,7 +180,7 @@ sn9c102_request_buffers(struct sn9c102_device* cam, u32 count,
 }
 
 
-static void sn9c102_release_buffers(struct sn9c102_device* cam)
+static void sn9c102_release_buffers(struct sn9c102_device *cam)
 {
 	if (cam->nbuffers) {
 		vfree(cam->frame[0].bufmem);
@@ -190,7 +190,7 @@ static void sn9c102_release_buffers(struct sn9c102_device* cam)
 }
 
 
-static void sn9c102_empty_framequeues(struct sn9c102_device* cam)
+static void sn9c102_empty_framequeues(struct sn9c102_device *cam)
 {
 	u32 i;
 
@@ -204,7 +204,7 @@ static void sn9c102_empty_framequeues(struct sn9c102_device* cam)
 }
 
 
-static void sn9c102_requeue_outqueue(struct sn9c102_device* cam)
+static void sn9c102_requeue_outqueue(struct sn9c102_device *cam)
 {
 	struct sn9c102_frame_t *i;
 
@@ -217,7 +217,7 @@ static void sn9c102_requeue_outqueue(struct sn9c102_device* cam)
 }
 
 
-static void sn9c102_queue_unusedframes(struct sn9c102_device* cam)
+static void sn9c102_queue_unusedframes(struct sn9c102_device *cam)
 {
 	unsigned long lock_flags;
 	u32 i;
@@ -237,11 +237,11 @@ static void sn9c102_queue_unusedframes(struct sn9c102_device* cam)
    Write a sequence of count value/register pairs. Returns -1 after the first
    failed write, or 0 for no errors.
 */
-int sn9c102_write_regs(struct sn9c102_device* cam, const u8 valreg[][2],
+int sn9c102_write_regs(struct sn9c102_device *cam, const u8 valreg[][2],
 		       int count)
 {
-	struct usb_device* udev = cam->usbdev;
-	u8* buff = cam->control_buffer;
+	struct usb_device *udev = cam->usbdev;
+	u8 *buff = cam->control_buffer;
 	int i, res;
 
 	for (i = 0; i < count; i++) {
@@ -273,10 +273,10 @@ int sn9c102_write_regs(struct sn9c102_device* cam, const u8 valreg[][2],
 }
 
 
-int sn9c102_write_reg(struct sn9c102_device* cam, u8 value, u16 index)
+int sn9c102_write_reg(struct sn9c102_device *cam, u8 value, u16 index)
 {
-	struct usb_device* udev = cam->usbdev;
-	u8* buff = cam->control_buffer;
+	struct usb_device *udev = cam->usbdev;
+	u8 *buff = cam->control_buffer;
 	int res;
 
 	if (index >= ARRAY_SIZE(cam->reg))
@@ -299,10 +299,10 @@ int sn9c102_write_reg(struct sn9c102_device* cam, u8 value, u16 index)
 
 
 /* NOTE: with the SN9C10[123] reading some registers always returns 0 */
-int sn9c102_read_reg(struct sn9c102_device* cam, u16 index)
+int sn9c102_read_reg(struct sn9c102_device *cam, u16 index)
 {
-	struct usb_device* udev = cam->usbdev;
-	u8* buff = cam->control_buffer;
+	struct usb_device *udev = cam->usbdev;
+	u8 *buff = cam->control_buffer;
 	int res;
 
 	res = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0), 0x00, 0xc1,
@@ -315,7 +315,7 @@ int sn9c102_read_reg(struct sn9c102_device* cam, u16 index)
 }
 
 
-int sn9c102_pread_reg(struct sn9c102_device* cam, u16 index)
+int sn9c102_pread_reg(struct sn9c102_device *cam, u16 index)
 {
 	if (index >= ARRAY_SIZE(cam->reg))
 		return -1;
@@ -325,8 +325,8 @@ int sn9c102_pread_reg(struct sn9c102_device* cam, u16 index)
 
 
 static int
-sn9c102_i2c_wait(struct sn9c102_device* cam,
-		 const struct sn9c102_sensor* sensor)
+sn9c102_i2c_wait(struct sn9c102_device *cam,
+		 const struct sn9c102_sensor *sensor)
 {
 	int i, r;
 
@@ -346,8 +346,8 @@ sn9c102_i2c_wait(struct sn9c102_device* cam,
 
 
 static int
-sn9c102_i2c_detect_read_error(struct sn9c102_device* cam,
-			      const struct sn9c102_sensor* sensor)
+sn9c102_i2c_detect_read_error(struct sn9c102_device *cam,
+			      const struct sn9c102_sensor *sensor)
 {
 	int r , err = 0;
 
@@ -368,22 +368,23 @@ sn9c102_i2c_detect_read_error(struct sn9c102_device* cam,
 
 
 static int
-sn9c102_i2c_detect_write_error(struct sn9c102_device* cam,
-			       const struct sn9c102_sensor* sensor)
+sn9c102_i2c_detect_write_error(struct sn9c102_device *cam,
+			       const struct sn9c102_sensor *sensor)
 {
 	int r;
+
 	r = sn9c102_read_reg(cam, 0x08);
 	return (r < 0 || (r >= 0 && (r & 0x08))) ? -EIO : 0;
 }
 
 
 int
-sn9c102_i2c_try_raw_read(struct sn9c102_device* cam,
-			 const struct sn9c102_sensor* sensor, u8 data0,
+sn9c102_i2c_try_raw_read(struct sn9c102_device *cam,
+			 const struct sn9c102_sensor *sensor, u8 data0,
 			 u8 data1, u8 n, u8 buffer[])
 {
-	struct usb_device* udev = cam->usbdev;
-	u8* data = cam->control_buffer;
+	struct usb_device *udev = cam->usbdev;
+	u8 *data = cam->control_buffer;
 	int i = 0, err = 0, res;
 
 	/* Write cycle */
@@ -437,12 +438,12 @@ sn9c102_i2c_try_raw_read(struct sn9c102_device* cam,
 
 
 int
-sn9c102_i2c_try_raw_write(struct sn9c102_device* cam,
-			  const struct sn9c102_sensor* sensor, u8 n, u8 data0,
+sn9c102_i2c_try_raw_write(struct sn9c102_device *cam,
+			  const struct sn9c102_sensor *sensor, u8 n, u8 data0,
 			  u8 data1, u8 data2, u8 data3, u8 data4, u8 data5)
 {
-	struct usb_device* udev = cam->usbdev;
-	u8* data = cam->control_buffer;
+	struct usb_device *udev = cam->usbdev;
+	u8 *data = cam->control_buffer;
 	int err = 0, res;
 
 	/* Write cycle. It usually is address + value */
@@ -476,16 +477,16 @@ sn9c102_i2c_try_raw_write(struct sn9c102_device* cam,
 
 
 int
-sn9c102_i2c_try_read(struct sn9c102_device* cam,
-		     const struct sn9c102_sensor* sensor, u8 address)
+sn9c102_i2c_try_read(struct sn9c102_device *cam,
+		     const struct sn9c102_sensor *sensor, u8 address)
 {
 	return sn9c102_i2c_try_raw_read(cam, sensor, sensor->i2c_slave_id,
 					address, 1, NULL);
 }
 
 
-static int sn9c102_i2c_try_write(struct sn9c102_device* cam,
-				 const struct sn9c102_sensor* sensor,
+static int sn9c102_i2c_try_write(struct sn9c102_device *cam,
+				 const struct sn9c102_sensor *sensor,
 				 u8 address, u8 value)
 {
 	return sn9c102_i2c_try_raw_write(cam, sensor, 3,
@@ -494,20 +495,20 @@ static int sn9c102_i2c_try_write(struct sn9c102_device* cam,
 }
 
 
-int sn9c102_i2c_read(struct sn9c102_device* cam, u8 address)
+int sn9c102_i2c_read(struct sn9c102_device *cam, u8 address)
 {
 	return sn9c102_i2c_try_read(cam, &cam->sensor, address);
 }
 
 
-int sn9c102_i2c_write(struct sn9c102_device* cam, u8 address, u8 value)
+int sn9c102_i2c_write(struct sn9c102_device *cam, u8 address, u8 value)
 {
 	return sn9c102_i2c_try_write(cam, &cam->sensor, address, value);
 }
 
 /*****************************************************************************/
 
-static size_t sn9c102_sof_length(struct sn9c102_device* cam)
+static size_t sn9c102_sof_length(struct sn9c102_device *cam)
 {
 	switch (cam->bridge) {
 	case BRIDGE_SN9C101:
@@ -525,7 +526,7 @@ static size_t sn9c102_sof_length(struct sn9c102_device* cam)
 
 
 static void*
-sn9c102_find_sof_header(struct sn9c102_device* cam, void* mem, size_t len)
+sn9c102_find_sof_header(struct sn9c102_device *cam, void *mem, size_t len)
 {
 	static const char marker[6] = {0xff, 0xff, 0x00, 0xc4, 0xc4, 0x96};
 	const char *m = mem;
@@ -547,7 +548,7 @@ sn9c102_find_sof_header(struct sn9c102_device* cam, void* mem, size_t len)
 		}
 
 		/* Search for the SOF marker (fixed part) in the header */
-		for (j = 0, b=cam->sof.bytesread; j+b < sizeof(marker); j++) {
+		for (j = 0, b = cam->sof.bytesread; j+b < sizeof(marker); j++) {
 			if (unlikely(i+j == len))
 				return NULL;
 			if (*(m+i+j) == marker[cam->sof.bytesread]) {
@@ -570,7 +571,7 @@ sn9c102_find_sof_header(struct sn9c102_device* cam, void* mem, size_t len)
 
 
 static void*
-sn9c102_find_eof_header(struct sn9c102_device* cam, void* mem, size_t len)
+sn9c102_find_eof_header(struct sn9c102_device *cam, void *mem, size_t len)
 {
 	static const u8 eof_header[4][4] = {
 		{0x00, 0x00, 0x00, 0x00},
@@ -600,7 +601,7 @@ sn9c102_find_eof_header(struct sn9c102_device* cam, void* mem, size_t len)
 
 
 static void
-sn9c102_write_jpegheader(struct sn9c102_device* cam, struct sn9c102_frame_t* f)
+sn9c102_write_jpegheader(struct sn9c102_device *cam, struct sn9c102_frame_t *f)
 {
 	static const u8 jpeg_header[589] = {
 		0xff, 0xd8, 0xff, 0xdb, 0x00, 0x84, 0x00, 0x06, 0x04, 0x05,
@@ -687,8 +688,8 @@ sn9c102_write_jpegheader(struct sn9c102_device* cam, struct sn9c102_frame_t* f)
 
 static void sn9c102_urb_complete(struct urb *urb)
 {
-	struct sn9c102_device* cam = urb->context;
-	struct sn9c102_frame_t** f;
+	struct sn9c102_device *cam = urb->context;
+	struct sn9c102_frame_t **f;
 	size_t imagesize, soflen;
 	u8 i;
 	int err = 0;
@@ -787,7 +788,7 @@ end_of_frame:
 
 					b = (*f)->buf.bytesused;
 					(*f)->state = F_DONE;
-					(*f)->buf.sequence= ++cam->frame_count;
+					(*f)->buf.sequence = ++cam->frame_count;
 
 					spin_lock(&cam->queue_lock);
 					list_move_tail(&(*f)->frame,
@@ -796,7 +797,7 @@ end_of_frame:
 						(*f) = list_entry(
 							cam->inqueue.next,
 							struct sn9c102_frame_t,
-							frame );
+							frame);
 					else
 						(*f) = NULL;
 					spin_unlock(&cam->queue_lock);
@@ -883,11 +884,11 @@ resubmit_urb:
 }
 
 
-static int sn9c102_start_transfer(struct sn9c102_device* cam)
+static int sn9c102_start_transfer(struct sn9c102_device *cam)
 {
 	struct usb_device *udev = cam->usbdev;
-	struct urb* urb;
-	struct usb_host_interface* altsetting = usb_altnum_to_altsetting(
+	struct urb *urb;
+	struct usb_host_interface *altsetting = usb_altnum_to_altsetting(
 						    usb_ifnum_to_if(udev, 0),
 						    SN9C102_ALTERNATE_SETTING);
 	const unsigned int psz = le16_to_cpu(altsetting->
@@ -971,7 +972,7 @@ free_buffers:
 }
 
 
-static int sn9c102_stop_transfer(struct sn9c102_device* cam)
+static int sn9c102_stop_transfer(struct sn9c102_device *cam)
 {
 	struct usb_device *udev = cam->usbdev;
 	s8 i;
@@ -994,7 +995,7 @@ static int sn9c102_stop_transfer(struct sn9c102_device* cam)
 }
 
 
-static int sn9c102_stream_interrupt(struct sn9c102_device* cam)
+static int sn9c102_stream_interrupt(struct sn9c102_device *cam)
 {
 	cam->stream = STREAM_INTERRUPT;
 	wait_event_timeout(cam->wait_stream,
@@ -1017,10 +1018,10 @@ static int sn9c102_stream_interrupt(struct sn9c102_device* cam)
 /*****************************************************************************/
 
 #ifdef CONFIG_VIDEO_ADV_DEBUG
-static u16 sn9c102_strtou16(const char* buff, size_t len, ssize_t* count)
+static u16 sn9c102_strtou16(const char *buff, size_t len, ssize_t *count)
 {
 	char str[7];
-	char* endp;
+	char *endp;
 	unsigned long val;
 
 	if (len < 6) {
@@ -1048,10 +1049,10 @@ static u16 sn9c102_strtou16(const char* buff, size_t len, ssize_t* count)
    NOTE 2: buffers are PAGE_SIZE long
 */
 
-static ssize_t sn9c102_show_reg(struct device* cd,
-				struct device_attribute *attr, char* buf)
+static ssize_t sn9c102_show_reg(struct device *cd,
+				struct device_attribute *attr, char *buf)
 {
-	struct sn9c102_device* cam;
+	struct sn9c102_device *cam;
 	ssize_t count;
 
 	if (mutex_lock_interruptible(&sn9c102_sysfs_lock))
@@ -1072,10 +1073,10 @@ static ssize_t sn9c102_show_reg(struct device* cd,
 
 
 static ssize_t
-sn9c102_store_reg(struct device* cd, struct device_attribute *attr,
-		  const char* buf, size_t len)
+sn9c102_store_reg(struct device *cd, struct device_attribute *attr,
+		  const char *buf, size_t len)
 {
-	struct sn9c102_device* cam;
+	struct sn9c102_device *cam;
 	u16 index;
 	ssize_t count;
 
@@ -1105,10 +1106,10 @@ sn9c102_store_reg(struct device* cd, struct device_attribute *attr,
 }
 
 
-static ssize_t sn9c102_show_val(struct device* cd,
-				struct device_attribute *attr, char* buf)
+static ssize_t sn9c102_show_val(struct device *cd,
+				struct device_attribute *attr, char *buf)
 {
-	struct sn9c102_device* cam;
+	struct sn9c102_device *cam;
 	ssize_t count;
 	int val;
 
@@ -1138,10 +1139,10 @@ static ssize_t sn9c102_show_val(struct device* cd,
 
 
 static ssize_t
-sn9c102_store_val(struct device* cd, struct device_attribute *attr,
-		  const char* buf, size_t len)
+sn9c102_store_val(struct device *cd, struct device_attribute *attr,
+		  const char *buf, size_t len)
 {
-	struct sn9c102_device* cam;
+	struct sn9c102_device *cam;
 	u16 value;
 	ssize_t count;
 	int err;
@@ -1177,10 +1178,10 @@ sn9c102_store_val(struct device* cd, struct device_attribute *attr,
 }
 
 
-static ssize_t sn9c102_show_i2c_reg(struct device* cd,
-				    struct device_attribute *attr, char* buf)
+static ssize_t sn9c102_show_i2c_reg(struct device *cd,
+				    struct device_attribute *attr, char *buf)
 {
-	struct sn9c102_device* cam;
+	struct sn9c102_device *cam;
 	ssize_t count;
 
 	if (mutex_lock_interruptible(&sn9c102_sysfs_lock))
@@ -1203,10 +1204,10 @@ static ssize_t sn9c102_show_i2c_reg(struct device* cd,
 
 
 static ssize_t
-sn9c102_store_i2c_reg(struct device* cd, struct device_attribute *attr,
-		      const char* buf, size_t len)
+sn9c102_store_i2c_reg(struct device *cd, struct device_attribute *attr,
+		      const char *buf, size_t len)
 {
-	struct sn9c102_device* cam;
+	struct sn9c102_device *cam;
 	u16 index;
 	ssize_t count;
 
@@ -1236,10 +1237,10 @@ sn9c102_store_i2c_reg(struct device* cd, struct device_attribute *attr,
 }
 
 
-static ssize_t sn9c102_show_i2c_val(struct device* cd,
-				    struct device_attribute *attr, char* buf)
+static ssize_t sn9c102_show_i2c_val(struct device *cd,
+				    struct device_attribute *attr, char *buf)
 {
-	struct sn9c102_device* cam;
+	struct sn9c102_device *cam;
 	ssize_t count;
 	int val;
 
@@ -1274,10 +1275,10 @@ static ssize_t sn9c102_show_i2c_val(struct device* cd,
 
 
 static ssize_t
-sn9c102_store_i2c_val(struct device* cd, struct device_attribute *attr,
-		      const char* buf, size_t len)
+sn9c102_store_i2c_val(struct device *cd, struct device_attribute *attr,
+		      const char *buf, size_t len)
 {
-	struct sn9c102_device* cam;
+	struct sn9c102_device *cam;
 	u16 value;
 	ssize_t count;
 	int err;
@@ -1319,10 +1320,10 @@ sn9c102_store_i2c_val(struct device* cd, struct device_attribute *attr,
 
 
 static ssize_t
-sn9c102_store_green(struct device* cd, struct device_attribute *attr,
-		    const char* buf, size_t len)
+sn9c102_store_green(struct device *cd, struct device_attribute *attr,
+		    const char *buf, size_t len)
 {
-	struct sn9c102_device* cam;
+	struct sn9c102_device *cam;
 	enum sn9c102_bridge bridge;
 	ssize_t res = 0;
 	u16 value;
@@ -1350,7 +1351,8 @@ sn9c102_store_green(struct device* cd, struct device_attribute *attr,
 	case BRIDGE_SN9C102:
 		if (value > 0x0f)
 			return -EINVAL;
-		if ((res = sn9c102_store_reg(cd, attr, "0x11", 4)) >= 0)
+		res = sn9c102_store_reg(cd, attr, "0x11", 4);
+		if (res >= 0)
 			res = sn9c102_store_val(cd, attr, buf, len);
 		break;
 	case BRIDGE_SN9C103:
@@ -1358,7 +1360,8 @@ sn9c102_store_green(struct device* cd, struct device_attribute *attr,
 	case BRIDGE_SN9C120:
 		if (value > 0x7f)
 			return -EINVAL;
-		if ((res = sn9c102_store_reg(cd, attr, "0x07", 4)) >= 0)
+		res = sn9c102_store_reg(cd, attr, "0x07", 4);
+		if (res >= 0)
 			res = sn9c102_store_val(cd, attr, buf, len);
 		break;
 	}
@@ -1368,8 +1371,8 @@ sn9c102_store_green(struct device* cd, struct device_attribute *attr,
 
 
 static ssize_t
-sn9c102_store_blue(struct device* cd, struct device_attribute *attr,
-		   const char* buf, size_t len)
+sn9c102_store_blue(struct device *cd, struct device_attribute *attr,
+		   const char *buf, size_t len)
 {
 	ssize_t res = 0;
 	u16 value;
@@ -1379,7 +1382,8 @@ sn9c102_store_blue(struct device* cd, struct device_attribute *attr,
 	if (!count || value > 0x7f)
 		return -EINVAL;
 
-	if ((res = sn9c102_store_reg(cd, attr, "0x06", 4)) >= 0)
+	res = sn9c102_store_reg(cd, attr, "0x06", 4);
+	if (res >= 0)
 		res = sn9c102_store_val(cd, attr, buf, len);
 
 	return res;
@@ -1387,8 +1391,8 @@ sn9c102_store_blue(struct device* cd, struct device_attribute *attr,
 
 
 static ssize_t
-sn9c102_store_red(struct device* cd, struct device_attribute *attr,
-		  const char* buf, size_t len)
+sn9c102_store_red(struct device *cd, struct device_attribute *attr,
+		  const char *buf, size_t len)
 {
 	ssize_t res = 0;
 	u16 value;
@@ -1397,19 +1401,19 @@ sn9c102_store_red(struct device* cd, struct device_attribute *attr,
 	value = sn9c102_strtou16(buf, len, &count);
 	if (!count || value > 0x7f)
 		return -EINVAL;
-
-	if ((res = sn9c102_store_reg(cd, attr, "0x05", 4)) >= 0)
+	res = sn9c102_store_reg(cd, attr, "0x05", 4);
+	if (res >= 0)
 		res = sn9c102_store_val(cd, attr, buf, len);
 
 	return res;
 }
 
 
-static ssize_t sn9c102_show_frame_header(struct device* cd,
+static ssize_t sn9c102_show_frame_header(struct device *cd,
 					 struct device_attribute *attr,
-					 char* buf)
+					 char *buf)
 {
-	struct sn9c102_device* cam;
+	struct sn9c102_device *cam;
 	ssize_t count;
 
 	cam = video_get_drvdata(container_of(cd, struct video_device, dev));
@@ -1437,7 +1441,7 @@ static DEVICE_ATTR(red, S_IWUSR, NULL, sn9c102_store_red);
 static DEVICE_ATTR(frame_header, S_IRUGO, sn9c102_show_frame_header, NULL);
 
 
-static int sn9c102_create_sysfs(struct sn9c102_device* cam)
+static int sn9c102_create_sysfs(struct sn9c102_device *cam)
 {
 	struct device *dev = &(cam->v4ldev->dev);
 	int err = 0;
@@ -1498,7 +1502,7 @@ err_out:
 /*****************************************************************************/
 
 static int
-sn9c102_set_pix_format(struct sn9c102_device* cam, struct v4l2_pix_format* pix)
+sn9c102_set_pix_format(struct sn9c102_device *cam, struct v4l2_pix_format *pix)
 {
 	int err = 0;
 
@@ -1538,8 +1542,8 @@ sn9c102_set_pix_format(struct sn9c102_device* cam, struct v4l2_pix_format* pix)
 
 
 static int
-sn9c102_set_compression(struct sn9c102_device* cam,
-			struct v4l2_jpegcompression* compression)
+sn9c102_set_compression(struct sn9c102_device *cam,
+			struct v4l2_jpegcompression *compression)
 {
 	int i, err = 0;
 
@@ -1586,7 +1590,7 @@ sn9c102_set_compression(struct sn9c102_device* cam,
 }
 
 
-static int sn9c102_set_scale(struct sn9c102_device* cam, u8 scale)
+static int sn9c102_set_scale(struct sn9c102_device *cam, u8 scale)
 {
 	u8 r = 0;
 	int err = 0;
@@ -1609,9 +1613,9 @@ static int sn9c102_set_scale(struct sn9c102_device* cam, u8 scale)
 }
 
 
-static int sn9c102_set_crop(struct sn9c102_device* cam, struct v4l2_rect* rect)
+static int sn9c102_set_crop(struct sn9c102_device *cam, struct v4l2_rect *rect)
 {
-	struct sn9c102_sensor* s = &cam->sensor;
+	struct sn9c102_sensor *s = &cam->sensor;
 	u8 h_start = (u8)(rect->left - s->cropcap.bounds.left),
 	   v_start = (u8)(rect->top - s->cropcap.bounds.top),
 	   h_size = (u8)(rect->width / 16),
@@ -1632,12 +1636,12 @@ static int sn9c102_set_crop(struct sn9c102_device* cam, struct v4l2_rect* rect)
 }
 
 
-static int sn9c102_init(struct sn9c102_device* cam)
+static int sn9c102_init(struct sn9c102_device *cam)
 {
-	struct sn9c102_sensor* s = &cam->sensor;
+	struct sn9c102_sensor *s = &cam->sensor;
 	struct v4l2_control ctrl;
 	struct v4l2_queryctrl *qctrl;
-	struct v4l2_rect* rect;
+	struct v4l2_rect *rect;
 	u8 i = 0;
 	int err = 0;
 
@@ -1669,7 +1673,7 @@ static int sn9c102_init(struct sn9c102_device* cam)
 		    cam->bridge == BRIDGE_SN9C102 ||
 		    cam->bridge == BRIDGE_SN9C103) {
 			if (s->pix_format.pixelformat == V4L2_PIX_FMT_JPEG)
-				s->pix_format.pixelformat= V4L2_PIX_FMT_SBGGR8;
+				s->pix_format.pixelformat = V4L2_PIX_FMT_SBGGR8;
 			cam->compression.quality =  cam->reg[0x17] & 0x01 ?
 						    0 : 1;
 		} else {
@@ -1761,7 +1765,7 @@ static void sn9c102_release_resources(struct kref *kref)
 
 static int sn9c102_open(struct file *filp)
 {
-	struct sn9c102_device* cam;
+	struct sn9c102_device *cam;
 	int err = 0;
 
 	/*
@@ -1873,7 +1877,7 @@ out:
 
 static int sn9c102_release(struct file *filp)
 {
-	struct sn9c102_device* cam;
+	struct sn9c102_device *cam;
 
 	down_write(&sn9c102_dev_lock);
 
@@ -1895,10 +1899,10 @@ static int sn9c102_release(struct file *filp)
 
 
 static ssize_t
-sn9c102_read(struct file* filp, char __user * buf, size_t count, loff_t* f_pos)
+sn9c102_read(struct file *filp, char __user *buf, size_t count, loff_t *f_pos)
 {
 	struct sn9c102_device *cam = video_drvdata(filp);
-	struct sn9c102_frame_t* f, * i;
+	struct sn9c102_frame_t *f, *i;
 	unsigned long lock_flags;
 	long timeout;
 	int err = 0;
@@ -1927,7 +1931,7 @@ sn9c102_read(struct file* filp, char __user * buf, size_t count, loff_t* f_pos)
 	}
 
 	if (cam->io == IO_NONE) {
-		if (!sn9c102_request_buffers(cam,cam->nreadbuffers, IO_READ)) {
+		if (!sn9c102_request_buffers(cam, cam->nreadbuffers, IO_READ)) {
 			DBG(1, "read() failed, not enough memory");
 			mutex_unlock(&cam->fileop_mutex);
 			return -ENOMEM;
@@ -1954,17 +1958,17 @@ sn9c102_read(struct file* filp, char __user * buf, size_t count, loff_t* f_pos)
 		}
 		if (!cam->module_param.frame_timeout) {
 			err = wait_event_interruptible
-			      ( cam->wait_frame,
+			      (cam->wait_frame,
 				(!list_empty(&cam->outqueue)) ||
 				(cam->state & DEV_DISCONNECTED) ||
-				(cam->state & DEV_MISCONFIGURED) );
+				(cam->state & DEV_MISCONFIGURED));
 			if (err) {
 				mutex_unlock(&cam->fileop_mutex);
 				return err;
 			}
 		} else {
 			timeout = wait_event_interruptible_timeout
-				  ( cam->wait_frame,
+				  (cam->wait_frame,
 				    (!list_empty(&cam->outqueue)) ||
 				    (cam->state & DEV_DISCONNECTED) ||
 				    (cam->state & DEV_MISCONFIGURED),
@@ -2024,7 +2028,7 @@ exit:
 static unsigned int sn9c102_poll(struct file *filp, poll_table *wait)
 {
 	struct sn9c102_device *cam = video_drvdata(filp);
-	struct sn9c102_frame_t* f;
+	struct sn9c102_frame_t *f;
 	unsigned long lock_flags;
 	unsigned int mask = 0;
 
@@ -2076,17 +2080,17 @@ error:
 }
 
 
-static void sn9c102_vm_open(struct vm_area_struct* vma)
+static void sn9c102_vm_open(struct vm_area_struct *vma)
 {
-	struct sn9c102_frame_t* f = vma->vm_private_data;
+	struct sn9c102_frame_t *f = vma->vm_private_data;
 	f->vma_use_count++;
 }
 
 
-static void sn9c102_vm_close(struct vm_area_struct* vma)
+static void sn9c102_vm_close(struct vm_area_struct *vma)
 {
 	/* NOTE: buffers are not freed here */
-	struct sn9c102_frame_t* f = vma->vm_private_data;
+	struct sn9c102_frame_t *f = vma->vm_private_data;
 	f->vma_use_count--;
 }
 
@@ -2097,7 +2101,7 @@ static const struct vm_operations_struct sn9c102_vm_ops = {
 };
 
 
-static int sn9c102_mmap(struct file* filp, struct vm_area_struct *vma)
+static int sn9c102_mmap(struct file *filp, struct vm_area_struct *vma)
 {
 	struct sn9c102_device *cam = video_drvdata(filp);
 	unsigned long size = vma->vm_end - vma->vm_start,
@@ -2166,7 +2170,7 @@ static int sn9c102_mmap(struct file* filp, struct vm_area_struct *vma)
 /*****************************************************************************/
 
 static int
-sn9c102_vidioc_querycap(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_querycap(struct sn9c102_device *cam, void __user *arg)
 {
 	struct v4l2_capability cap = {
 		.driver = "sn9c102",
@@ -2188,7 +2192,7 @@ sn9c102_vidioc_querycap(struct sn9c102_device* cam, void __user * arg)
 
 
 static int
-sn9c102_vidioc_enuminput(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_enuminput(struct sn9c102_device *cam, void __user *arg)
 {
 	struct v4l2_input i;
 
@@ -2211,7 +2215,7 @@ sn9c102_vidioc_enuminput(struct sn9c102_device* cam, void __user * arg)
 
 
 static int
-sn9c102_vidioc_g_input(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_g_input(struct sn9c102_device *cam, void __user *arg)
 {
 	int index = 0;
 
@@ -2223,7 +2227,7 @@ sn9c102_vidioc_g_input(struct sn9c102_device* cam, void __user * arg)
 
 
 static int
-sn9c102_vidioc_s_input(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_s_input(struct sn9c102_device *cam, void __user *arg)
 {
 	int index;
 
@@ -2238,9 +2242,9 @@ sn9c102_vidioc_s_input(struct sn9c102_device* cam, void __user * arg)
 
 
 static int
-sn9c102_vidioc_query_ctrl(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_query_ctrl(struct sn9c102_device *cam, void __user *arg)
 {
-	struct sn9c102_sensor* s = &cam->sensor;
+	struct sn9c102_sensor *s = &cam->sensor;
 	struct v4l2_queryctrl qc;
 	u8 i;
 
@@ -2260,9 +2264,9 @@ sn9c102_vidioc_query_ctrl(struct sn9c102_device* cam, void __user * arg)
 
 
 static int
-sn9c102_vidioc_g_ctrl(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_g_ctrl(struct sn9c102_device *cam, void __user *arg)
 {
-	struct sn9c102_sensor* s = &cam->sensor;
+	struct sn9c102_sensor *s = &cam->sensor;
 	struct v4l2_control ctrl;
 	int err = 0;
 	u8 i;
@@ -2295,9 +2299,9 @@ exit:
 
 
 static int
-sn9c102_vidioc_s_ctrl(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_s_ctrl(struct sn9c102_device *cam, void __user *arg)
 {
-	struct sn9c102_sensor* s = &cam->sensor;
+	struct sn9c102_sensor *s = &cam->sensor;
 	struct v4l2_control ctrl;
 	u8 i;
 	int err = 0;
@@ -2335,9 +2339,9 @@ sn9c102_vidioc_s_ctrl(struct sn9c102_device* cam, void __user * arg)
 
 
 static int
-sn9c102_vidioc_cropcap(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_cropcap(struct sn9c102_device *cam, void __user *arg)
 {
-	struct v4l2_cropcap* cc = &(cam->sensor.cropcap);
+	struct v4l2_cropcap *cc = &(cam->sensor.cropcap);
 
 	cc->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 	cc->pixelaspect.numerator = 1;
@@ -2351,9 +2355,9 @@ sn9c102_vidioc_cropcap(struct sn9c102_device* cam, void __user * arg)
 
 
 static int
-sn9c102_vidioc_g_crop(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_g_crop(struct sn9c102_device *cam, void __user *arg)
 {
-	struct sn9c102_sensor* s = &cam->sensor;
+	struct sn9c102_sensor *s = &cam->sensor;
 	struct v4l2_crop crop = {
 		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
 	};
@@ -2368,13 +2372,13 @@ sn9c102_vidioc_g_crop(struct sn9c102_device* cam, void __user * arg)
 
 
 static int
-sn9c102_vidioc_s_crop(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_s_crop(struct sn9c102_device *cam, void __user *arg)
 {
-	struct sn9c102_sensor* s = &cam->sensor;
+	struct sn9c102_sensor *s = &cam->sensor;
 	struct v4l2_crop crop;
-	struct v4l2_rect* rect;
-	struct v4l2_rect* bounds = &(s->cropcap.bounds);
-	struct v4l2_pix_format* pix_format = &(s->pix_format);
+	struct v4l2_rect *rect;
+	struct v4l2_rect *bounds = &(s->cropcap.bounds);
+	struct v4l2_pix_format *pix_format = &(s->pix_format);
 	u8 scale;
 	const enum sn9c102_stream_state stream = cam->stream;
 	const u32 nbuffers = cam->nbuffers;
@@ -2482,7 +2486,7 @@ sn9c102_vidioc_s_crop(struct sn9c102_device* cam, void __user * arg)
 
 
 static int
-sn9c102_vidioc_enum_framesizes(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_enum_framesizes(struct sn9c102_device *cam, void __user *arg)
 {
 	struct v4l2_frmsizeenum frmsize;
 
@@ -2523,7 +2527,7 @@ sn9c102_vidioc_enum_framesizes(struct sn9c102_device* cam, void __user * arg)
 
 
 static int
-sn9c102_vidioc_enum_fmt(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_enum_fmt(struct sn9c102_device *cam, void __user *arg)
 {
 	struct v4l2_fmtdesc fmtd;
 
@@ -2565,10 +2569,10 @@ sn9c102_vidioc_enum_fmt(struct sn9c102_device* cam, void __user * arg)
 
 
 static int
-sn9c102_vidioc_g_fmt(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_g_fmt(struct sn9c102_device *cam, void __user *arg)
 {
 	struct v4l2_format format;
-	struct v4l2_pix_format* pfmt = &(cam->sensor.pix_format);
+	struct v4l2_pix_format *pfmt = &(cam->sensor.pix_format);
 
 	if (copy_from_user(&format, arg, sizeof(format)))
 		return -EFAULT;
@@ -2593,14 +2597,14 @@ sn9c102_vidioc_g_fmt(struct sn9c102_device* cam, void __user * arg)
 
 
 static int
-sn9c102_vidioc_try_s_fmt(struct sn9c102_device* cam, unsigned int cmd,
-			 void __user * arg)
+sn9c102_vidioc_try_s_fmt(struct sn9c102_device *cam, unsigned int cmd,
+			 void __user *arg)
 {
-	struct sn9c102_sensor* s = &cam->sensor;
+	struct sn9c102_sensor *s = &cam->sensor;
 	struct v4l2_format format;
-	struct v4l2_pix_format* pix;
-	struct v4l2_pix_format* pfmt = &(s->pix_format);
-	struct v4l2_rect* bounds = &(s->cropcap.bounds);
+	struct v4l2_pix_format *pix;
+	struct v4l2_pix_format *pfmt = &(s->pix_format);
+	struct v4l2_rect *bounds = &(s->cropcap.bounds);
 	struct v4l2_rect rect;
 	u8 scale;
 	const enum sn9c102_stream_state stream = cam->stream;
@@ -2742,7 +2746,7 @@ sn9c102_vidioc_try_s_fmt(struct sn9c102_device* cam, unsigned int cmd,
 
 
 static int
-sn9c102_vidioc_g_jpegcomp(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_g_jpegcomp(struct sn9c102_device *cam, void __user *arg)
 {
 	if (copy_to_user(arg, &cam->compression, sizeof(cam->compression)))
 		return -EFAULT;
@@ -2752,7 +2756,7 @@ sn9c102_vidioc_g_jpegcomp(struct sn9c102_device* cam, void __user * arg)
 
 
 static int
-sn9c102_vidioc_s_jpegcomp(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_s_jpegcomp(struct sn9c102_device *cam, void __user *arg)
 {
 	struct v4l2_jpegcompression jc;
 	const enum sn9c102_stream_state stream = cam->stream;
@@ -2788,7 +2792,7 @@ sn9c102_vidioc_s_jpegcomp(struct sn9c102_device* cam, void __user * arg)
 
 
 static int
-sn9c102_vidioc_reqbufs(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_reqbufs(struct sn9c102_device *cam, void __user *arg)
 {
 	struct v4l2_requestbuffers rb;
 	u32 i;
@@ -2839,7 +2843,7 @@ sn9c102_vidioc_reqbufs(struct sn9c102_device* cam, void __user * arg)
 
 
 static int
-sn9c102_vidioc_querybuf(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_querybuf(struct sn9c102_device *cam, void __user *arg)
 {
 	struct v4l2_buffer b;
 
@@ -2868,7 +2872,7 @@ sn9c102_vidioc_querybuf(struct sn9c102_device* cam, void __user * arg)
 
 
 static int
-sn9c102_vidioc_qbuf(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_qbuf(struct sn9c102_device *cam, void __user *arg)
 {
 	struct v4l2_buffer b;
 	unsigned long lock_flags;
@@ -2896,8 +2900,8 @@ sn9c102_vidioc_qbuf(struct sn9c102_device* cam, void __user * arg)
 
 
 static int
-sn9c102_vidioc_dqbuf(struct sn9c102_device* cam, struct file* filp,
-		     void __user * arg)
+sn9c102_vidioc_dqbuf(struct sn9c102_device *cam, struct file *filp,
+		     void __user *arg)
 {
 	struct v4l2_buffer b;
 	struct sn9c102_frame_t *f;
@@ -2918,20 +2922,20 @@ sn9c102_vidioc_dqbuf(struct sn9c102_device* cam, struct file* filp,
 			return -EAGAIN;
 		if (!cam->module_param.frame_timeout) {
 			err = wait_event_interruptible
-			      ( cam->wait_frame,
+			      (cam->wait_frame,
 				(!list_empty(&cam->outqueue)) ||
 				(cam->state & DEV_DISCONNECTED) ||
-				(cam->state & DEV_MISCONFIGURED) );
+				(cam->state & DEV_MISCONFIGURED));
 			if (err)
 				return err;
 		} else {
 			timeout = wait_event_interruptible_timeout
-				  ( cam->wait_frame,
+				  (cam->wait_frame,
 				    (!list_empty(&cam->outqueue)) ||
 				    (cam->state & DEV_DISCONNECTED) ||
 				    (cam->state & DEV_MISCONFIGURED),
 				    cam->module_param.frame_timeout *
-				    1000 * msecs_to_jiffies(1) );
+				    1000 * msecs_to_jiffies(1));
 			if (timeout < 0)
 				return timeout;
 			else if (timeout == 0 &&
@@ -2967,7 +2971,7 @@ sn9c102_vidioc_dqbuf(struct sn9c102_device* cam, struct file* filp,
 
 
 static int
-sn9c102_vidioc_streamon(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_streamon(struct sn9c102_device *cam, void __user *arg)
 {
 	int type;
 
@@ -2986,7 +2990,7 @@ sn9c102_vidioc_streamon(struct sn9c102_device* cam, void __user * arg)
 
 
 static int
-sn9c102_vidioc_streamoff(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_streamoff(struct sn9c102_device *cam, void __user *arg)
 {
 	int type, err;
 
@@ -3011,7 +3015,7 @@ sn9c102_vidioc_streamoff(struct sn9c102_device* cam, void __user * arg)
 
 
 static int
-sn9c102_vidioc_g_parm(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_g_parm(struct sn9c102_device *cam, void __user *arg)
 {
 	struct v4l2_streamparm sp;
 
@@ -3032,7 +3036,7 @@ sn9c102_vidioc_g_parm(struct sn9c102_device* cam, void __user * arg)
 
 
 static int
-sn9c102_vidioc_s_parm(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_s_parm(struct sn9c102_device *cam, void __user *arg)
 {
 	struct v4l2_streamparm sp;
 
@@ -3060,7 +3064,7 @@ sn9c102_vidioc_s_parm(struct sn9c102_device* cam, void __user * arg)
 
 
 static int
-sn9c102_vidioc_enumaudio(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_enumaudio(struct sn9c102_device *cam, void __user *arg)
 {
 	struct v4l2_audio audio;
 
@@ -3085,7 +3089,7 @@ sn9c102_vidioc_enumaudio(struct sn9c102_device* cam, void __user * arg)
 
 
 static int
-sn9c102_vidioc_g_audio(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_g_audio(struct sn9c102_device *cam, void __user *arg)
 {
 	struct v4l2_audio audio;
 
@@ -3106,7 +3110,7 @@ sn9c102_vidioc_g_audio(struct sn9c102_device* cam, void __user * arg)
 
 
 static int
-sn9c102_vidioc_s_audio(struct sn9c102_device* cam, void __user * arg)
+sn9c102_vidioc_s_audio(struct sn9c102_device *cam, void __user *arg)
 {
 	struct v4l2_audio audio;
 
@@ -3266,10 +3270,10 @@ static const struct v4l2_file_operations sn9c102_fops = {
 
 /* It exists a single interface only. We do not need to validate anything. */
 static int
-sn9c102_usb_probe(struct usb_interface* intf, const struct usb_device_id* id)
+sn9c102_usb_probe(struct usb_interface *intf, const struct usb_device_id *id)
 {
 	struct usb_device *udev = interface_to_usbdev(intf);
-	struct sn9c102_device* cam;
+	struct sn9c102_device *cam;
 	static unsigned int dev_nr;
 	unsigned int i;
 	int err = 0, r;
@@ -3419,9 +3423,9 @@ fail:
 }
 
 
-static void sn9c102_usb_disconnect(struct usb_interface* intf)
+static void sn9c102_usb_disconnect(struct usb_interface *intf)
 {
-	struct sn9c102_device* cam;
+	struct sn9c102_device *cam;
 
 	down_write(&sn9c102_dev_lock);
 
diff --git a/drivers/staging/media/sn9c102/sn9c102_devtable.h b/drivers/staging/media/sn9c102/sn9c102_devtable.h
index b3d2cc72965..b187a8a304e 100644
--- a/drivers/staging/media/sn9c102/sn9c102_devtable.h
+++ b/drivers/staging/media/sn9c102/sn9c102_devtable.h
@@ -48,10 +48,8 @@ static const struct usb_device_id sn9c102_id_table[] = {
 	{ SN9C102_USB_DEVICE(0x0c45, 0x600d, BRIDGE_SN9C102), },
 /*	{ SN9C102_USB_DEVICE(0x0c45, 0x6011, BRIDGE_SN9C102), }, OV6650 */
 	{ SN9C102_USB_DEVICE(0x0c45, 0x6019, BRIDGE_SN9C102), },
-#endif
 	{ SN9C102_USB_DEVICE(0x0c45, 0x6024, BRIDGE_SN9C102), },
 	{ SN9C102_USB_DEVICE(0x0c45, 0x6025, BRIDGE_SN9C102), },
-#if !defined CONFIG_USB_GSPCA_SONIXB && !defined CONFIG_USB_GSPCA_SONIXB_MODULE
 	{ SN9C102_USB_DEVICE(0x0c45, 0x6028, BRIDGE_SN9C102), },
 	{ SN9C102_USB_DEVICE(0x0c45, 0x6029, BRIDGE_SN9C102), },
 	{ SN9C102_USB_DEVICE(0x0c45, 0x602a, BRIDGE_SN9C102), },
@@ -131,17 +129,17 @@ static const struct usb_device_id sn9c102_id_table[] = {
    initialization of the SN9C1XX chip.
    Functions must return 0 on success, the appropriate error otherwise.
 */
-extern int sn9c102_probe_hv7131d(struct sn9c102_device* cam);
-extern int sn9c102_probe_hv7131r(struct sn9c102_device* cam);
-extern int sn9c102_probe_mi0343(struct sn9c102_device* cam);
-extern int sn9c102_probe_mi0360(struct sn9c102_device* cam);
+extern int sn9c102_probe_hv7131d(struct sn9c102_device *cam);
+extern int sn9c102_probe_hv7131r(struct sn9c102_device *cam);
+extern int sn9c102_probe_mi0343(struct sn9c102_device *cam);
+extern int sn9c102_probe_mi0360(struct sn9c102_device *cam);
 extern int sn9c102_probe_mt9v111(struct sn9c102_device *cam);
-extern int sn9c102_probe_ov7630(struct sn9c102_device* cam);
-extern int sn9c102_probe_ov7660(struct sn9c102_device* cam);
-extern int sn9c102_probe_pas106b(struct sn9c102_device* cam);
-extern int sn9c102_probe_pas202bcb(struct sn9c102_device* cam);
-extern int sn9c102_probe_tas5110c1b(struct sn9c102_device* cam);
-extern int sn9c102_probe_tas5110d(struct sn9c102_device* cam);
-extern int sn9c102_probe_tas5130d1b(struct sn9c102_device* cam);
+extern int sn9c102_probe_ov7630(struct sn9c102_device *cam);
+extern int sn9c102_probe_ov7660(struct sn9c102_device *cam);
+extern int sn9c102_probe_pas106b(struct sn9c102_device *cam);
+extern int sn9c102_probe_pas202bcb(struct sn9c102_device *cam);
+extern int sn9c102_probe_tas5110c1b(struct sn9c102_device *cam);
+extern int sn9c102_probe_tas5110d(struct sn9c102_device *cam);
+extern int sn9c102_probe_tas5130d1b(struct sn9c102_device *cam);
 
 #endif /* _SN9C102_DEVTABLE_H_ */
diff --git a/drivers/staging/media/sn9c102/sn9c102_hv7131d.c b/drivers/staging/media/sn9c102/sn9c102_hv7131d.c
index 46807217652..f1d94f0190c 100644
--- a/drivers/staging/media/sn9c102/sn9c102_hv7131d.c
+++ b/drivers/staging/media/sn9c102/sn9c102_hv7131d.c
@@ -23,7 +23,7 @@
 #include "sn9c102_devtable.h"
 
 
-static int hv7131d_init(struct sn9c102_device* cam)
+static int hv7131d_init(struct sn9c102_device *cam)
 {
 	int err;
 
@@ -39,8 +39,8 @@ static int hv7131d_init(struct sn9c102_device* cam)
 }
 
 
-static int hv7131d_get_ctrl(struct sn9c102_device* cam,
-			    struct v4l2_control* ctrl)
+static int hv7131d_get_ctrl(struct sn9c102_device *cam,
+			    struct v4l2_control *ctrl)
 {
 	switch (ctrl->id) {
 	case V4L2_CID_EXPOSURE:
@@ -88,8 +88,8 @@ static int hv7131d_get_ctrl(struct sn9c102_device* cam,
 }
 
 
-static int hv7131d_set_ctrl(struct sn9c102_device* cam,
-			    const struct v4l2_control* ctrl)
+static int hv7131d_set_ctrl(struct sn9c102_device *cam,
+			    const struct v4l2_control *ctrl)
 {
 	int err = 0;
 
@@ -121,10 +121,10 @@ static int hv7131d_set_ctrl(struct sn9c102_device* cam,
 }
 
 
-static int hv7131d_set_crop(struct sn9c102_device* cam,
-			    const struct v4l2_rect* rect)
+static int hv7131d_set_crop(struct sn9c102_device *cam,
+			    const struct v4l2_rect *rect)
 {
-	struct sn9c102_sensor* s = sn9c102_get_sensor(cam);
+	struct sn9c102_sensor *s = sn9c102_get_sensor(cam);
 	int err = 0;
 	u8 h_start = (u8)(rect->left - s->cropcap.bounds.left) + 2,
 	   v_start = (u8)(rect->top - s->cropcap.bounds.top) + 2;
@@ -136,8 +136,8 @@ static int hv7131d_set_crop(struct sn9c102_device* cam,
 }
 
 
-static int hv7131d_set_pix_format(struct sn9c102_device* cam,
-				  const struct v4l2_pix_format* pix)
+static int hv7131d_set_pix_format(struct sn9c102_device *cam,
+				  const struct v4l2_pix_format *pix)
 {
 	int err = 0;
 
@@ -248,7 +248,7 @@ static const struct sn9c102_sensor hv7131d = {
 };
 
 
-int sn9c102_probe_hv7131d(struct sn9c102_device* cam)
+int sn9c102_probe_hv7131d(struct sn9c102_device *cam)
 {
 	int r0 = 0, r1 = 0, err;
 
diff --git a/drivers/staging/media/sn9c102/sn9c102_hv7131r.c b/drivers/staging/media/sn9c102/sn9c102_hv7131r.c
index 26a91115b83..51b24e000e8 100644
--- a/drivers/staging/media/sn9c102/sn9c102_hv7131r.c
+++ b/drivers/staging/media/sn9c102/sn9c102_hv7131r.c
@@ -23,7 +23,7 @@
 #include "sn9c102_devtable.h"
 
 
-static int hv7131r_init(struct sn9c102_device* cam)
+static int hv7131r_init(struct sn9c102_device *cam)
 {
 	int err = 0;
 
@@ -137,8 +137,8 @@ static int hv7131r_init(struct sn9c102_device* cam)
 }
 
 
-static int hv7131r_get_ctrl(struct sn9c102_device* cam,
-			    struct v4l2_control* ctrl)
+static int hv7131r_get_ctrl(struct sn9c102_device *cam,
+			    struct v4l2_control *ctrl)
 {
 	switch (ctrl->id) {
 	case V4L2_CID_GAIN:
@@ -176,8 +176,8 @@ static int hv7131r_get_ctrl(struct sn9c102_device* cam,
 }
 
 
-static int hv7131r_set_ctrl(struct sn9c102_device* cam,
-			    const struct v4l2_control* ctrl)
+static int hv7131r_set_ctrl(struct sn9c102_device *cam,
+			    const struct v4l2_control *ctrl)
 {
 	int err = 0;
 
@@ -197,6 +197,7 @@ static int hv7131r_set_ctrl(struct sn9c102_device* cam,
 	case V4L2_CID_BLACK_LEVEL:
 		{
 			int r = sn9c102_i2c_read(cam, 0x01);
+
 			if (r < 0)
 				return -EIO;
 			err += sn9c102_i2c_write(cam, 0x01,
@@ -211,10 +212,10 @@ static int hv7131r_set_ctrl(struct sn9c102_device* cam,
 }
 
 
-static int hv7131r_set_crop(struct sn9c102_device* cam,
-			    const struct v4l2_rect* rect)
+static int hv7131r_set_crop(struct sn9c102_device *cam,
+			    const struct v4l2_rect *rect)
 {
-	struct sn9c102_sensor* s = sn9c102_get_sensor(cam);
+	struct sn9c102_sensor *s = sn9c102_get_sensor(cam);
 	int err = 0;
 	u8 h_start = (u8)(rect->left - s->cropcap.bounds.left) + 1,
 	   v_start = (u8)(rect->top - s->cropcap.bounds.top) + 1;
@@ -226,8 +227,8 @@ static int hv7131r_set_crop(struct sn9c102_device* cam,
 }
 
 
-static int hv7131r_set_pix_format(struct sn9c102_device* cam,
-				  const struct v4l2_pix_format* pix)
+static int hv7131r_set_pix_format(struct sn9c102_device *cam,
+				  const struct v4l2_pix_format *pix)
 {
 	int err = 0;
 
@@ -347,7 +348,7 @@ static const struct sn9c102_sensor hv7131r = {
 };
 
 
-int sn9c102_probe_hv7131r(struct sn9c102_device* cam)
+int sn9c102_probe_hv7131r(struct sn9c102_device *cam)
 {
 	int devid, err;
 
diff --git a/drivers/staging/media/sn9c102/sn9c102_mi0343.c b/drivers/staging/media/sn9c102/sn9c102_mi0343.c
index 1f5b09bec89..b20fdb6541d 100644
--- a/drivers/staging/media/sn9c102/sn9c102_mi0343.c
+++ b/drivers/staging/media/sn9c102/sn9c102_mi0343.c
@@ -23,9 +23,9 @@
 #include "sn9c102_devtable.h"
 
 
-static int mi0343_init(struct sn9c102_device* cam)
+static int mi0343_init(struct sn9c102_device *cam)
 {
-	struct sn9c102_sensor* s = sn9c102_get_sensor(cam);
+	struct sn9c102_sensor *s = sn9c102_get_sensor(cam);
 	int err = 0;
 
 	err = sn9c102_write_const_regs(cam, {0x00, 0x10}, {0x00, 0x11},
@@ -52,10 +52,10 @@ static int mi0343_init(struct sn9c102_device* cam)
 }
 
 
-static int mi0343_get_ctrl(struct sn9c102_device* cam,
-			   struct v4l2_control* ctrl)
+static int mi0343_get_ctrl(struct sn9c102_device *cam,
+			   struct v4l2_control *ctrl)
 {
-	struct sn9c102_sensor* s = sn9c102_get_sensor(cam);
+	struct sn9c102_sensor *s = sn9c102_get_sensor(cam);
 	u8 data[2];
 
 	switch (ctrl->id) {
@@ -119,10 +119,10 @@ static int mi0343_get_ctrl(struct sn9c102_device* cam,
 }
 
 
-static int mi0343_set_ctrl(struct sn9c102_device* cam,
-			   const struct v4l2_control* ctrl)
+static int mi0343_set_ctrl(struct sn9c102_device *cam,
+			   const struct v4l2_control *ctrl)
 {
-	struct sn9c102_sensor* s = sn9c102_get_sensor(cam);
+	struct sn9c102_sensor *s = sn9c102_get_sensor(cam);
 	u16 reg = 0;
 	int err = 0;
 
@@ -189,10 +189,10 @@ static int mi0343_set_ctrl(struct sn9c102_device* cam,
 }
 
 
-static int mi0343_set_crop(struct sn9c102_device* cam,
-			    const struct v4l2_rect* rect)
+static int mi0343_set_crop(struct sn9c102_device *cam,
+			    const struct v4l2_rect *rect)
 {
-	struct sn9c102_sensor* s = sn9c102_get_sensor(cam);
+	struct sn9c102_sensor *s = sn9c102_get_sensor(cam);
 	int err = 0;
 	u8 h_start = (u8)(rect->left - s->cropcap.bounds.left) + 0,
 	   v_start = (u8)(rect->top - s->cropcap.bounds.top) + 2;
@@ -204,10 +204,10 @@ static int mi0343_set_crop(struct sn9c102_device* cam,
 }
 
 
-static int mi0343_set_pix_format(struct sn9c102_device* cam,
-				 const struct v4l2_pix_format* pix)
+static int mi0343_set_pix_format(struct sn9c102_device *cam,
+				 const struct v4l2_pix_format *pix)
 {
-	struct sn9c102_sensor* s = sn9c102_get_sensor(cam);
+	struct sn9c102_sensor *s = sn9c102_get_sensor(cam);
 	int err = 0;
 
 	if (pix->pixelformat == V4L2_PIX_FMT_SN9C10X) {
@@ -331,7 +331,7 @@ static const struct sn9c102_sensor mi0343 = {
 };
 
 
-int sn9c102_probe_mi0343(struct sn9c102_device* cam)
+int sn9c102_probe_mi0343(struct sn9c102_device *cam)
 {
 	u8 data[2];
 
diff --git a/drivers/staging/media/sn9c102/sn9c102_mi0360.c b/drivers/staging/media/sn9c102/sn9c102_mi0360.c
index d973fc1973d..5f21d1b43e3 100644
--- a/drivers/staging/media/sn9c102/sn9c102_mi0360.c
+++ b/drivers/staging/media/sn9c102/sn9c102_mi0360.c
@@ -23,9 +23,9 @@
 #include "sn9c102_devtable.h"
 
 
-static int mi0360_init(struct sn9c102_device* cam)
+static int mi0360_init(struct sn9c102_device *cam)
 {
-	struct sn9c102_sensor* s = sn9c102_get_sensor(cam);
+	struct sn9c102_sensor *s = sn9c102_get_sensor(cam);
 	int err = 0;
 
 	switch (sn9c102_get_bridge(cam)) {
@@ -147,10 +147,10 @@ static int mi0360_init(struct sn9c102_device* cam)
 }
 
 
-static int mi0360_get_ctrl(struct sn9c102_device* cam,
-			   struct v4l2_control* ctrl)
+static int mi0360_get_ctrl(struct sn9c102_device *cam,
+			   struct v4l2_control *ctrl)
 {
-	struct sn9c102_sensor* s = sn9c102_get_sensor(cam);
+	struct sn9c102_sensor *s = sn9c102_get_sensor(cam);
 	u8 data[2];
 
 	switch (ctrl->id) {
@@ -204,10 +204,10 @@ static int mi0360_get_ctrl(struct sn9c102_device* cam,
 }
 
 
-static int mi0360_set_ctrl(struct sn9c102_device* cam,
-			   const struct v4l2_control* ctrl)
+static int mi0360_set_ctrl(struct sn9c102_device *cam,
+			   const struct v4l2_control *ctrl)
 {
-	struct sn9c102_sensor* s = sn9c102_get_sensor(cam);
+	struct sn9c102_sensor *s = sn9c102_get_sensor(cam);
 	int err = 0;
 
 	switch (ctrl->id) {
@@ -259,10 +259,10 @@ static int mi0360_set_ctrl(struct sn9c102_device* cam,
 }
 
 
-static int mi0360_set_crop(struct sn9c102_device* cam,
-			    const struct v4l2_rect* rect)
+static int mi0360_set_crop(struct sn9c102_device *cam,
+			    const struct v4l2_rect *rect)
 {
-	struct sn9c102_sensor* s = sn9c102_get_sensor(cam);
+	struct sn9c102_sensor *s = sn9c102_get_sensor(cam);
 	int err = 0;
 	u8 h_start = 0, v_start = (u8)(rect->top - s->cropcap.bounds.top) + 1;
 
@@ -285,10 +285,10 @@ static int mi0360_set_crop(struct sn9c102_device* cam,
 }
 
 
-static int mi0360_set_pix_format(struct sn9c102_device* cam,
-				 const struct v4l2_pix_format* pix)
+static int mi0360_set_pix_format(struct sn9c102_device *cam,
+				 const struct v4l2_pix_format *pix)
 {
-	struct sn9c102_sensor* s = sn9c102_get_sensor(cam);
+	struct sn9c102_sensor *s = sn9c102_get_sensor(cam);
 	int err = 0;
 
 	if (pix->pixelformat == V4L2_PIX_FMT_SBGGR8) {
@@ -418,7 +418,7 @@ static const struct sn9c102_sensor mi0360 = {
 };
 
 
-int sn9c102_probe_mi0360(struct sn9c102_device* cam)
+int sn9c102_probe_mi0360(struct sn9c102_device *cam)
 {
 
 	u8 data[2];
diff --git a/drivers/staging/media/sn9c102/sn9c102_ov7630.c b/drivers/staging/media/sn9c102/sn9c102_ov7630.c
index d3a1bd8d564..9ec304dc470 100644
--- a/drivers/staging/media/sn9c102/sn9c102_ov7630.c
+++ b/drivers/staging/media/sn9c102/sn9c102_ov7630.c
@@ -23,7 +23,7 @@
 #include "sn9c102_devtable.h"
 
 
-static int ov7630_init(struct sn9c102_device* cam)
+static int ov7630_init(struct sn9c102_device *cam)
 {
 	int err = 0;
 
@@ -252,8 +252,8 @@ static int ov7630_init(struct sn9c102_device* cam)
 }
 
 
-static int ov7630_get_ctrl(struct sn9c102_device* cam,
-			   struct v4l2_control* ctrl)
+static int ov7630_get_ctrl(struct sn9c102_device *cam,
+			   struct v4l2_control *ctrl)
 {
 	enum sn9c102_bridge bridge = sn9c102_get_bridge(cam);
 	int err = 0;
@@ -330,8 +330,8 @@ static int ov7630_get_ctrl(struct sn9c102_device* cam,
 }
 
 
-static int ov7630_set_ctrl(struct sn9c102_device* cam,
-			   const struct v4l2_control* ctrl)
+static int ov7630_set_ctrl(struct sn9c102_device *cam,
+			   const struct v4l2_control *ctrl)
 {
 	enum sn9c102_bridge bridge = sn9c102_get_bridge(cam);
 	int err = 0;
@@ -385,10 +385,10 @@ static int ov7630_set_ctrl(struct sn9c102_device* cam,
 }
 
 
-static int ov7630_set_crop(struct sn9c102_device* cam,
-			   const struct v4l2_rect* rect)
+static int ov7630_set_crop(struct sn9c102_device *cam,
+			   const struct v4l2_rect *rect)
 {
-	struct sn9c102_sensor* s = sn9c102_get_sensor(cam);
+	struct sn9c102_sensor *s = sn9c102_get_sensor(cam);
 	int err = 0;
 	u8 h_start = 0, v_start = (u8)(rect->top - s->cropcap.bounds.top) + 1;
 
@@ -413,8 +413,8 @@ static int ov7630_set_crop(struct sn9c102_device* cam,
 }
 
 
-static int ov7630_set_pix_format(struct sn9c102_device* cam,
-				 const struct v4l2_pix_format* pix)
+static int ov7630_set_pix_format(struct sn9c102_device *cam,
+				 const struct v4l2_pix_format *pix)
 {
 	int err = 0;
 
@@ -594,7 +594,7 @@ static const struct sn9c102_sensor ov7630 = {
 };
 
 
-int sn9c102_probe_ov7630(struct sn9c102_device* cam)
+int sn9c102_probe_ov7630(struct sn9c102_device *cam)
 {
 	int pid, ver, err = 0;
 
diff --git a/drivers/staging/media/sn9c102/sn9c102_ov7660.c b/drivers/staging/media/sn9c102/sn9c102_ov7660.c
index 530157a234e..ac07805d122 100644
--- a/drivers/staging/media/sn9c102/sn9c102_ov7660.c
+++ b/drivers/staging/media/sn9c102/sn9c102_ov7660.c
@@ -23,7 +23,7 @@
 #include "sn9c102_devtable.h"
 
 
-static int ov7660_init(struct sn9c102_device* cam)
+static int ov7660_init(struct sn9c102_device *cam)
 {
 	int err = 0;
 
@@ -271,8 +271,8 @@ static int ov7660_init(struct sn9c102_device* cam)
 }
 
 
-static int ov7660_get_ctrl(struct sn9c102_device* cam,
-			   struct v4l2_control* ctrl)
+static int ov7660_get_ctrl(struct sn9c102_device *cam,
+			   struct v4l2_control *ctrl)
 {
 	int err = 0;
 
@@ -332,8 +332,8 @@ static int ov7660_get_ctrl(struct sn9c102_device* cam,
 }
 
 
-static int ov7660_set_ctrl(struct sn9c102_device* cam,
-			   const struct v4l2_control* ctrl)
+static int ov7660_set_ctrl(struct sn9c102_device *cam,
+			   const struct v4l2_control *ctrl)
 {
 	int err = 0;
 
@@ -371,10 +371,10 @@ static int ov7660_set_ctrl(struct sn9c102_device* cam,
 }
 
 
-static int ov7660_set_crop(struct sn9c102_device* cam,
-			   const struct v4l2_rect* rect)
+static int ov7660_set_crop(struct sn9c102_device *cam,
+			   const struct v4l2_rect *rect)
 {
-	struct sn9c102_sensor* s = sn9c102_get_sensor(cam);
+	struct sn9c102_sensor *s = sn9c102_get_sensor(cam);
 	int err = 0;
 	u8 h_start = (u8)(rect->left - s->cropcap.bounds.left) + 1,
 	   v_start = (u8)(rect->top - s->cropcap.bounds.top) + 1;
@@ -386,8 +386,8 @@ static int ov7660_set_crop(struct sn9c102_device* cam,
 }
 
 
-static int ov7660_set_pix_format(struct sn9c102_device* cam,
-				 const struct v4l2_pix_format* pix)
+static int ov7660_set_pix_format(struct sn9c102_device *cam,
+				 const struct v4l2_pix_format *pix)
 {
 	int r0, err = 0;
 
@@ -525,7 +525,7 @@ static const struct sn9c102_sensor ov7660 = {
 };
 
 
-int sn9c102_probe_ov7660(struct sn9c102_device* cam)
+int sn9c102_probe_ov7660(struct sn9c102_device *cam)
 {
 	int pid, ver, err;
 
diff --git a/drivers/staging/media/sn9c102/sn9c102_pas106b.c b/drivers/staging/media/sn9c102/sn9c102_pas106b.c
index 47bd82de80f..895931ecac4 100644
--- a/drivers/staging/media/sn9c102/sn9c102_pas106b.c
+++ b/drivers/staging/media/sn9c102/sn9c102_pas106b.c
@@ -24,7 +24,7 @@
 #include "sn9c102_devtable.h"
 
 
-static int pas106b_init(struct sn9c102_device* cam)
+static int pas106b_init(struct sn9c102_device *cam)
 {
 	int err = 0;
 
@@ -48,8 +48,8 @@ static int pas106b_init(struct sn9c102_device* cam)
 }
 
 
-static int pas106b_get_ctrl(struct sn9c102_device* cam,
-			    struct v4l2_control* ctrl)
+static int pas106b_get_ctrl(struct sn9c102_device *cam,
+			    struct v4l2_control *ctrl)
 {
 	switch (ctrl->id) {
 	case V4L2_CID_EXPOSURE:
@@ -103,8 +103,8 @@ static int pas106b_get_ctrl(struct sn9c102_device* cam,
 }
 
 
-static int pas106b_set_ctrl(struct sn9c102_device* cam,
-			    const struct v4l2_control* ctrl)
+static int pas106b_set_ctrl(struct sn9c102_device *cam,
+			    const struct v4l2_control *ctrl)
 {
 	int err = 0;
 
@@ -141,10 +141,10 @@ static int pas106b_set_ctrl(struct sn9c102_device* cam,
 }
 
 
-static int pas106b_set_crop(struct sn9c102_device* cam,
-			    const struct v4l2_rect* rect)
+static int pas106b_set_crop(struct sn9c102_device *cam,
+			    const struct v4l2_rect *rect)
 {
-	struct sn9c102_sensor* s = sn9c102_get_sensor(cam);
+	struct sn9c102_sensor *s = sn9c102_get_sensor(cam);
 	int err = 0;
 	u8 h_start = (u8)(rect->left - s->cropcap.bounds.left) + 4,
 	   v_start = (u8)(rect->top - s->cropcap.bounds.top) + 3;
@@ -156,8 +156,8 @@ static int pas106b_set_crop(struct sn9c102_device* cam,
 }
 
 
-static int pas106b_set_pix_format(struct sn9c102_device* cam,
-				  const struct v4l2_pix_format* pix)
+static int pas106b_set_pix_format(struct sn9c102_device *cam,
+				  const struct v4l2_pix_format *pix)
 {
 	int err = 0;
 
@@ -278,7 +278,7 @@ static const struct sn9c102_sensor pas106b = {
 };
 
 
-int sn9c102_probe_pas106b(struct sn9c102_device* cam)
+int sn9c102_probe_pas106b(struct sn9c102_device *cam)
 {
 	int r0 = 0, r1 = 0;
 	unsigned int pid = 0;
diff --git a/drivers/staging/media/sn9c102/sn9c102_pas202bcb.c b/drivers/staging/media/sn9c102/sn9c102_pas202bcb.c
index cbfacc2dad8..f9e31ae2ad9 100644
--- a/drivers/staging/media/sn9c102/sn9c102_pas202bcb.c
+++ b/drivers/staging/media/sn9c102/sn9c102_pas202bcb.c
@@ -28,7 +28,7 @@
 #include "sn9c102_devtable.h"
 
 
-static int pas202bcb_init(struct sn9c102_device* cam)
+static int pas202bcb_init(struct sn9c102_device *cam)
 {
 	int err = 0;
 
@@ -78,8 +78,8 @@ static int pas202bcb_init(struct sn9c102_device* cam)
 }
 
 
-static int pas202bcb_get_ctrl(struct sn9c102_device* cam,
-			      struct v4l2_control* ctrl)
+static int pas202bcb_get_ctrl(struct sn9c102_device *cam,
+			      struct v4l2_control *ctrl)
 {
 	switch (ctrl->id) {
 	case V4L2_CID_EXPOSURE:
@@ -126,8 +126,8 @@ static int pas202bcb_get_ctrl(struct sn9c102_device* cam,
 }
 
 
-static int pas202bcb_set_pix_format(struct sn9c102_device* cam,
-				    const struct v4l2_pix_format* pix)
+static int pas202bcb_set_pix_format(struct sn9c102_device *cam,
+				    const struct v4l2_pix_format *pix)
 {
 	int err = 0;
 
@@ -140,8 +140,8 @@ static int pas202bcb_set_pix_format(struct sn9c102_device* cam,
 }
 
 
-static int pas202bcb_set_ctrl(struct sn9c102_device* cam,
-			      const struct v4l2_control* ctrl)
+static int pas202bcb_set_ctrl(struct sn9c102_device *cam,
+			      const struct v4l2_control *ctrl)
 {
 	int err = 0;
 
@@ -174,10 +174,10 @@ static int pas202bcb_set_ctrl(struct sn9c102_device* cam,
 }
 
 
-static int pas202bcb_set_crop(struct sn9c102_device* cam,
-			      const struct v4l2_rect* rect)
+static int pas202bcb_set_crop(struct sn9c102_device *cam,
+			      const struct v4l2_rect *rect)
 {
-	struct sn9c102_sensor* s = sn9c102_get_sensor(cam);
+	struct sn9c102_sensor *s = sn9c102_get_sensor(cam);
 	int err = 0;
 	u8 h_start = 0,
 	   v_start = (u8)(rect->top - s->cropcap.bounds.top) + 3;
@@ -299,7 +299,7 @@ static const struct sn9c102_sensor pas202bcb = {
 };
 
 
-int sn9c102_probe_pas202bcb(struct sn9c102_device* cam)
+int sn9c102_probe_pas202bcb(struct sn9c102_device *cam)
 {
 	int r0 = 0, r1 = 0, err = 0;
 	unsigned int pid = 0;
diff --git a/drivers/staging/media/sn9c102/sn9c102_sensor.h b/drivers/staging/media/sn9c102/sn9c102_sensor.h
index 3679970dba2..9f59c815d48 100644
--- a/drivers/staging/media/sn9c102/sn9c102_sensor.h
+++ b/drivers/staging/media/sn9c102/sn9c102_sensor.h
@@ -62,19 +62,19 @@ enum sn9c102_bridge {
 };
 
 /* Return the bridge name */
-enum sn9c102_bridge sn9c102_get_bridge(struct sn9c102_device* cam);
+enum sn9c102_bridge sn9c102_get_bridge(struct sn9c102_device *cam);
 
 /* Return a pointer the sensor struct attached to the camera */
-struct sn9c102_sensor* sn9c102_get_sensor(struct sn9c102_device* cam);
+struct sn9c102_sensor *sn9c102_get_sensor(struct sn9c102_device *cam);
 
 /* Identify a device */
 extern struct sn9c102_device*
-sn9c102_match_id(struct sn9c102_device* cam, const struct usb_device_id *id);
+sn9c102_match_id(struct sn9c102_device *cam, const struct usb_device_id *id);
 
 /* Attach a probed sensor to the camera. */
 extern void
-sn9c102_attach_sensor(struct sn9c102_device* cam,
-		      const struct sn9c102_sensor* sensor);
+sn9c102_attach_sensor(struct sn9c102_device *cam,
+		      const struct sn9c102_sensor *sensor);
 
 /*
    Read/write routines: they always return -1 on error, 0 or the read value
@@ -99,12 +99,12 @@ extern int sn9c102_i2c_try_read(struct sn9c102_device*,
    version returns 0 on success, while the read version returns the first read
    byte.
 */
-extern int sn9c102_i2c_try_raw_write(struct sn9c102_device* cam,
-				     const struct sn9c102_sensor* sensor, u8 n,
+extern int sn9c102_i2c_try_raw_write(struct sn9c102_device *cam,
+				     const struct sn9c102_sensor *sensor, u8 n,
 				     u8 data0, u8 data1, u8 data2, u8 data3,
 				     u8 data4, u8 data5);
-extern int sn9c102_i2c_try_raw_read(struct sn9c102_device* cam,
-				    const struct sn9c102_sensor* sensor,
+extern int sn9c102_i2c_try_raw_read(struct sn9c102_device *cam,
+				    const struct sn9c102_sensor *sensor,
 				    u8 data0, u8 data1, u8 n, u8 buffer[]);
 
 /* To be used after the sensor struct has been attached to the camera struct */
@@ -174,7 +174,7 @@ struct sn9c102_sensor {
 		 they must return 0 on success, the proper error otherwise.
 	*/
 
-	int (*init)(struct sn9c102_device* cam);
+	int (*init)(struct sn9c102_device *cam);
 	/*
 	   This function will be called after the sensor has been attached.
 	   It should be used to initialize the sensor only, but may also
@@ -195,9 +195,9 @@ struct sn9c102_sensor {
 	   V4L2 API. Menu type controls are not handled by this interface.
 	*/
 
-	int (*get_ctrl)(struct sn9c102_device* cam, struct v4l2_control* ctrl);
-	int (*set_ctrl)(struct sn9c102_device* cam,
-			const struct v4l2_control* ctrl);
+	int (*get_ctrl)(struct sn9c102_device *cam, struct v4l2_control *ctrl);
+	int (*set_ctrl)(struct sn9c102_device *cam,
+			const struct v4l2_control *ctrl);
 	/*
 	   You must implement at least the set_ctrl method if you have defined
 	   the list above. The returned value must follow the V4L2
@@ -240,8 +240,8 @@ struct sn9c102_sensor {
 	   will be ignored.
 	*/
 
-	int (*set_crop)(struct sn9c102_device* cam,
-			const struct v4l2_rect* rect);
+	int (*set_crop)(struct sn9c102_device *cam,
+			const struct v4l2_rect *rect);
 	/*
 	   To be called on VIDIOC_C_SETCROP. The core module always calls a
 	   default routine which configures the appropriate SN9C1XX regs (also
@@ -276,8 +276,8 @@ struct sn9c102_sensor {
 		   matches the RGB bayer sequence (i.e. BGBGBG...GRGRGR).
 	*/
 
-	int (*set_pix_format)(struct sn9c102_device* cam,
-			      const struct v4l2_pix_format* pix);
+	int (*set_pix_format)(struct sn9c102_device *cam,
+			      const struct v4l2_pix_format *pix);
 	/*
 	   To be called on VIDIOC_S_FMT, when switching from the SBGGR8 to
 	   SN9C10X pixel format or viceversa. On error return the corresponding
diff --git a/drivers/staging/media/sn9c102/sn9c102_tas5110c1b.c b/drivers/staging/media/sn9c102/sn9c102_tas5110c1b.c
index 04cdfdde856..6a00b626d34 100644
--- a/drivers/staging/media/sn9c102/sn9c102_tas5110c1b.c
+++ b/drivers/staging/media/sn9c102/sn9c102_tas5110c1b.c
@@ -23,7 +23,7 @@
 #include "sn9c102_devtable.h"
 
 
-static int tas5110c1b_init(struct sn9c102_device* cam)
+static int tas5110c1b_init(struct sn9c102_device *cam)
 {
 	int err = 0;
 
@@ -38,8 +38,8 @@ static int tas5110c1b_init(struct sn9c102_device* cam)
 }
 
 
-static int tas5110c1b_set_ctrl(struct sn9c102_device* cam,
-			       const struct v4l2_control* ctrl)
+static int tas5110c1b_set_ctrl(struct sn9c102_device *cam,
+			       const struct v4l2_control *ctrl)
 {
 	int err = 0;
 
@@ -55,10 +55,10 @@ static int tas5110c1b_set_ctrl(struct sn9c102_device* cam,
 }
 
 
-static int tas5110c1b_set_crop(struct sn9c102_device* cam,
-			       const struct v4l2_rect* rect)
+static int tas5110c1b_set_crop(struct sn9c102_device *cam,
+			       const struct v4l2_rect *rect)
 {
-	struct sn9c102_sensor* s = sn9c102_get_sensor(cam);
+	struct sn9c102_sensor *s = sn9c102_get_sensor(cam);
 	int err = 0;
 	u8 h_start = (u8)(rect->left - s->cropcap.bounds.left) + 69,
 	   v_start = (u8)(rect->top - s->cropcap.bounds.top) + 9;
@@ -75,8 +75,8 @@ static int tas5110c1b_set_crop(struct sn9c102_device* cam,
 }
 
 
-static int tas5110c1b_set_pix_format(struct sn9c102_device* cam,
-				     const struct v4l2_pix_format* pix)
+static int tas5110c1b_set_pix_format(struct sn9c102_device *cam,
+				     const struct v4l2_pix_format *pix)
 {
 	int err = 0;
 
@@ -135,7 +135,7 @@ static const struct sn9c102_sensor tas5110c1b = {
 };
 
 
-int sn9c102_probe_tas5110c1b(struct sn9c102_device* cam)
+int sn9c102_probe_tas5110c1b(struct sn9c102_device *cam)
 {
 	const struct usb_device_id tas5110c1b_id_table[] = {
 		{ USB_DEVICE(0x0c45, 0x6001), },
diff --git a/drivers/staging/media/sn9c102/sn9c102_tas5110d.c b/drivers/staging/media/sn9c102/sn9c102_tas5110d.c
index 9372e6f9fcf..eefbf8670c3 100644
--- a/drivers/staging/media/sn9c102/sn9c102_tas5110d.c
+++ b/drivers/staging/media/sn9c102/sn9c102_tas5110d.c
@@ -23,7 +23,7 @@
 #include "sn9c102_devtable.h"
 
 
-static int tas5110d_init(struct sn9c102_device* cam)
+static int tas5110d_init(struct sn9c102_device *cam)
 {
 	int err;
 
@@ -37,10 +37,10 @@ static int tas5110d_init(struct sn9c102_device* cam)
 }
 
 
-static int tas5110d_set_crop(struct sn9c102_device* cam,
-			     const struct v4l2_rect* rect)
+static int tas5110d_set_crop(struct sn9c102_device *cam,
+			     const struct v4l2_rect *rect)
 {
-	struct sn9c102_sensor* s = sn9c102_get_sensor(cam);
+	struct sn9c102_sensor *s = sn9c102_get_sensor(cam);
 	int err = 0;
 	u8 h_start = (u8)(rect->left - s->cropcap.bounds.left) + 69,
 	   v_start = (u8)(rect->top - s->cropcap.bounds.top) + 9;
@@ -55,8 +55,8 @@ static int tas5110d_set_crop(struct sn9c102_device* cam,
 }
 
 
-static int tas5110d_set_pix_format(struct sn9c102_device* cam,
-				     const struct v4l2_pix_format* pix)
+static int tas5110d_set_pix_format(struct sn9c102_device *cam,
+				     const struct v4l2_pix_format *pix)
 {
 	int err = 0;
 
@@ -103,7 +103,7 @@ static const struct sn9c102_sensor tas5110d = {
 };
 
 
-int sn9c102_probe_tas5110d(struct sn9c102_device* cam)
+int sn9c102_probe_tas5110d(struct sn9c102_device *cam)
 {
 	const struct usb_device_id tas5110d_id_table[] = {
 		{ USB_DEVICE(0x0c45, 0x6007), },
diff --git a/drivers/staging/media/sn9c102/sn9c102_tas5130d1b.c b/drivers/staging/media/sn9c102/sn9c102_tas5130d1b.c
index a30bbc4389f..725de857de4 100644
--- a/drivers/staging/media/sn9c102/sn9c102_tas5130d1b.c
+++ b/drivers/staging/media/sn9c102/sn9c102_tas5130d1b.c
@@ -23,7 +23,7 @@
 #include "sn9c102_devtable.h"
 
 
-static int tas5130d1b_init(struct sn9c102_device* cam)
+static int tas5130d1b_init(struct sn9c102_device *cam)
 {
 	int err;
 
@@ -36,8 +36,8 @@ static int tas5130d1b_init(struct sn9c102_device* cam)
 }
 
 
-static int tas5130d1b_set_ctrl(struct sn9c102_device* cam,
-			       const struct v4l2_control* ctrl)
+static int tas5130d1b_set_ctrl(struct sn9c102_device *cam,
+			       const struct v4l2_control *ctrl)
 {
 	int err = 0;
 
@@ -56,10 +56,10 @@ static int tas5130d1b_set_ctrl(struct sn9c102_device* cam,
 }
 
 
-static int tas5130d1b_set_crop(struct sn9c102_device* cam,
-			       const struct v4l2_rect* rect)
+static int tas5130d1b_set_crop(struct sn9c102_device *cam,
+			       const struct v4l2_rect *rect)
 {
-	struct sn9c102_sensor* s = sn9c102_get_sensor(cam);
+	struct sn9c102_sensor *s = sn9c102_get_sensor(cam);
 	u8 h_start = (u8)(rect->left - s->cropcap.bounds.left) + 104,
 	   v_start = (u8)(rect->top - s->cropcap.bounds.top) + 12;
 	int err = 0;
@@ -76,8 +76,8 @@ static int tas5130d1b_set_crop(struct sn9c102_device* cam,
 }
 
 
-static int tas5130d1b_set_pix_format(struct sn9c102_device* cam,
-				     const struct v4l2_pix_format* pix)
+static int tas5130d1b_set_pix_format(struct sn9c102_device *cam,
+				     const struct v4l2_pix_format *pix)
 {
 	int err = 0;
 
@@ -146,7 +146,7 @@ static const struct sn9c102_sensor tas5130d1b = {
 };
 
 
-int sn9c102_probe_tas5130d1b(struct sn9c102_device* cam)
+int sn9c102_probe_tas5130d1b(struct sn9c102_device *cam)
 {
 	const struct usb_device_id tas5130d1b_id_table[] = {
 		{ USB_DEVICE(0x0c45, 0x6024), },
diff --git a/drivers/staging/media/solo6x10/Kconfig b/drivers/staging/media/solo6x10/Kconfig
index 9a4296c2e3a..6a1906fa111 100644
--- a/drivers/staging/media/solo6x10/Kconfig
+++ b/drivers/staging/media/solo6x10/Kconfig
@@ -1,5 +1,5 @@
 config SOLO6X10
-	tristate "Softlogic 6x10 MPEG codec cards"
+	tristate "Bluecherry / Softlogic 6x10 capture cards (MPEG-4/H.264)"
 	depends on PCI && VIDEO_DEV && SND && I2C
 	select FONT_SUPPORT
 	select FONT_8x16
@@ -8,5 +8,11 @@ config SOLO6X10
 	select SND_PCM
 	select FONT_8x16
 	---help---
-	  This driver supports the Softlogic based MPEG-4 and h.264 codec
-	  cards.
+	  This driver supports the Bluecherry H.264 and MPEG-4 hardware
+	  compression capture cards and other Softlogic-based ones.
+
+	  Following cards have been tested:
+	  * Bluecherry BC-H16480A (PCIe, 16 port, H.264)
+	  * Bluecherry BC-H04120A (PCIe, 4 port, H.264)
+	  * Bluecherry BC-H04120A-MPCI (Mini-PCI, 4 port, H.264)
+	  * Bluecherry BC-04120A (PCIe, 4 port, MPEG-4)
diff --git a/drivers/staging/media/solo6x10/solo6x10-enc.c b/drivers/staging/media/solo6x10/solo6x10-enc.c
index 94d5735abf8..2db53b68c62 100644
--- a/drivers/staging/media/solo6x10/solo6x10-enc.c
+++ b/drivers/staging/media/solo6x10/solo6x10-enc.c
@@ -134,51 +134,48 @@ static void solo_capture_config(struct solo_dev *solo_dev)
 	kfree(buf);
 }
 
+#define SOLO_OSD_WRITE_SIZE (16 * OSD_TEXT_MAX)
+
 /* Should be called with enable_lock held */
 int solo_osd_print(struct solo_enc_dev *solo_enc)
 {
 	struct solo_dev *solo_dev = solo_enc->solo_dev;
 	unsigned char *str = solo_enc->osd_text;
 	u8 *buf = solo_enc->osd_buf;
-	u32 reg = solo_reg_read(solo_dev, SOLO_VE_OSD_CH);
+	u32 reg;
 	const struct font_desc *vga = find_font("VGA8x16");
 	const unsigned char *vga_data;
-	int len;
 	int i, j;
 
 	if (WARN_ON_ONCE(!vga))
 		return -ENODEV;
 
-	len = strlen(str);
-
-	if (len == 0) {
+	reg = solo_reg_read(solo_dev, SOLO_VE_OSD_CH);
+	if (!*str) {
 		/* Disable OSD on this channel */
 		reg &= ~(1 << solo_enc->ch);
-		solo_reg_write(solo_dev, SOLO_VE_OSD_CH, reg);
-		return 0;
+		goto out;
 	}
 
-	memset(buf, 0, SOLO_EOSD_EXT_SIZE_MAX);
+	memset(buf, 0, SOLO_OSD_WRITE_SIZE);
 	vga_data = (const unsigned char *)vga->data;
 
-	for (i = 0; i < len; i++) {
-		unsigned char c = str[i];
-
+	for (i = 0; *str; i++, str++) {
 		for (j = 0; j < 16; j++) {
-			buf[(j * 2) + (i % 2) + (i / 2 * 32)] =
-				bitrev8(vga_data[(c * 16) + j]);
+			buf[(j << 1) | (i & 1) | ((i & ~1) << 4)] =
+			    bitrev8(vga_data[(*str << 4) | j]);
 		}
 	}
 
 	solo_p2m_dma(solo_dev, 1, buf,
-		     SOLO_EOSD_EXT_ADDR +
-		     (solo_enc->ch * SOLO_EOSD_EXT_SIZE(solo_dev)),
-		     SOLO_EOSD_EXT_SIZE(solo_dev), 0, 0);
+		     SOLO_EOSD_EXT_ADDR_CHAN(solo_dev, solo_enc->ch),
+		     SOLO_OSD_WRITE_SIZE, 0, 0);
 
 	/* Enable OSD on this channel */
 	reg |= (1 << solo_enc->ch);
-	solo_reg_write(solo_dev, SOLO_VE_OSD_CH, reg);
 
+out:
+	solo_reg_write(solo_dev, SOLO_VE_OSD_CH, reg);
 	return 0;
 }
 
diff --git a/drivers/staging/media/solo6x10/solo6x10-offsets.h b/drivers/staging/media/solo6x10/solo6x10-offsets.h
index f005dca501f..13eeb4470dc 100644
--- a/drivers/staging/media/solo6x10/solo6x10-offsets.h
+++ b/drivers/staging/media/solo6x10/solo6x10-offsets.h
@@ -35,6 +35,8 @@
 #define SOLO_EOSD_EXT_SIZE_MAX			0x20000
 #define SOLO_EOSD_EXT_AREA(__solo) \
 	(SOLO_EOSD_EXT_SIZE(__solo) * 32)
+#define SOLO_EOSD_EXT_ADDR_CHAN(__solo, ch) \
+	(SOLO_EOSD_EXT_ADDR + SOLO_EOSD_EXT_SIZE(__solo) * (ch))
 
 #define SOLO_MOTION_EXT_ADDR(__solo) \
 	(SOLO_EOSD_EXT_ADDR + SOLO_EOSD_EXT_AREA(__solo))
diff --git a/drivers/staging/media/solo6x10/solo6x10-v4l2-enc.c b/drivers/staging/media/solo6x10/solo6x10-v4l2-enc.c
index 2cbe088f169..b8ff113c20f 100644
--- a/drivers/staging/media/solo6x10/solo6x10-v4l2-enc.c
+++ b/drivers/staging/media/solo6x10/solo6x10-v4l2-enc.c
@@ -745,14 +745,13 @@ static int solo_enc_start_streaming(struct vb2_queue *q, unsigned int count)
 	return solo_ring_start(solo_enc->solo_dev);
 }
 
-static int solo_enc_stop_streaming(struct vb2_queue *q)
+static void solo_enc_stop_streaming(struct vb2_queue *q)
 {
 	struct solo_enc_dev *solo_enc = vb2_get_drv_priv(q);
 
 	solo_enc_off(solo_enc);
 	INIT_LIST_HEAD(&solo_enc->vidq_active);
 	solo_ring_stop(solo_enc->solo_dev);
-	return 0;
 }
 
 static struct vb2_ops solo_enc_video_qops = {
diff --git a/drivers/staging/media/solo6x10/solo6x10-v4l2.c b/drivers/staging/media/solo6x10/solo6x10-v4l2.c
index 1815f765d03..5d0100eb38e 100644
--- a/drivers/staging/media/solo6x10/solo6x10-v4l2.c
+++ b/drivers/staging/media/solo6x10/solo6x10-v4l2.c
@@ -336,13 +336,12 @@ static int solo_start_streaming(struct vb2_queue *q, unsigned int count)
 	return solo_start_thread(solo_dev);
 }
 
-static int solo_stop_streaming(struct vb2_queue *q)
+static void solo_stop_streaming(struct vb2_queue *q)
 {
 	struct solo_dev *solo_dev = vb2_get_drv_priv(q);
 
 	solo_stop_thread(solo_dev);
 	INIT_LIST_HEAD(&solo_dev->vidq_active);
-	return 0;
 }
 
 static void solo_buf_queue(struct vb2_buffer *vb)
diff --git a/drivers/staging/mt29f_spinand/mt29f_spinand.c b/drivers/staging/mt29f_spinand/mt29f_spinand.c
index 51dbc13e757..3464e0c521e 100644
--- a/drivers/staging/mt29f_spinand/mt29f_spinand.c
+++ b/drivers/staging/mt29f_spinand/mt29f_spinand.c
@@ -699,6 +699,7 @@ static void spinand_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
 {
 
 	struct spinand_state *state = mtd_to_state(mtd);
+
 	memcpy(state->buf + state->buf_ptr, buf, len);
 	state->buf_ptr += len;
 }
@@ -706,6 +707,7 @@ static void spinand_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
 static void spinand_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 {
 	struct spinand_state *state = mtd_to_state(mtd);
+
 	memcpy(buf, state->buf + state->buf_ptr, len);
 	state->buf_ptr += len;
 }
@@ -924,6 +926,7 @@ static int spinand_remove(struct spi_device *spi)
 
 static const struct of_device_id spinand_dt[] = {
 	{ .compatible = "spinand,mt29f", },
+	{}
 };
 
 /*
diff --git a/drivers/staging/netlogic/xlr_net.c b/drivers/staging/netlogic/xlr_net.c
index c83e3375104..75d7c63cb41 100644
--- a/drivers/staging/netlogic/xlr_net.c
+++ b/drivers/staging/netlogic/xlr_net.c
@@ -268,6 +268,7 @@ static void xlr_make_tx_desc(struct nlm_fmn_msg *msg, unsigned long addr,
 	unsigned long physkb = virt_to_phys(skb);
 	int cpu_core = nlm_core_id();
 	int fr_stn_id = cpu_core * 8 + XLR_FB_STN;	/* FB to 6th bucket */
+
 	msg->msg0 = (((u64)1 << 63)	|	/* End of packet descriptor */
 		((u64)127 << 54)	|	/* No Free back */
 		(u64)skb->len << 40	|	/* Length of data */
diff --git a/drivers/staging/nokia_h4p/nokia_fw-bcm.c b/drivers/staging/nokia_h4p/nokia_fw-bcm.c
index 111ae94032d..b55f5ba1134 100644
--- a/drivers/staging/nokia_h4p/nokia_fw-bcm.c
+++ b/drivers/staging/nokia_h4p/nokia_fw-bcm.c
@@ -73,7 +73,8 @@ void hci_h4p_bcm_parse_fw_event(struct hci_h4p_info *info, struct sk_buff *skb)
 		return;
 	}
 
-	if (fw_skb->data[1] == 0x01 && fw_skb->data[2] == 0xfc && fw_skb->len >= 10) {
+	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) {
diff --git a/drivers/staging/octeon-usb/octeon-hcd.c b/drivers/staging/octeon-usb/octeon-hcd.c
index 8b8ce7293c5..c4c731f6052 100644
--- a/drivers/staging/octeon-usb/octeon-hcd.c
+++ b/drivers/staging/octeon-usb/octeon-hcd.c
@@ -47,6 +47,7 @@
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/pci.h>
+#include <linux/prefetch.h>
 #include <linux/interrupt.h>
 #include <linux/platform_device.h>
 #include <linux/usb.h>
@@ -245,9 +246,6 @@ enum cvmx_usb_pipe_flags {
 	__CVMX_USB_PIPE_FLAGS_NEED_PING	= 1 << 18,
 };
 
-/* Normal prefetch that use the pref instruction. */
-#define CVMX_PREFETCH(address, offset) asm volatile ("pref %[type], %[off](%[rbase])" : : [rbase] "d" (address), [off] "I" (offset), [type] "n" (0))
-
 /* Maximum number of times to retry failed transactions */
 #define MAX_RETRIES		3
 
@@ -462,7 +460,8 @@ struct octeon_hcd {
 	} while (0)
 
 /* Returns the IO address to push/pop stuff data from the FIFOs */
-#define USB_FIFO_ADDRESS(channel, usb_index) (CVMX_USBCX_GOTGCTL(usb_index) + ((channel)+1)*0x1000)
+#define USB_FIFO_ADDRESS(channel, usb_index) \
+	(CVMX_USBCX_GOTGCTL(usb_index) + ((channel)+1)*0x1000)
 
 /**
  * struct octeon_temp_buffer - a bounce buffer for USB transfers
@@ -892,6 +891,7 @@ static int cvmx_usb_initialize(struct cvmx_usb_state *usb,
 	 */
 	{
 		union cvmx_usbcx_gusbcfg usbcx_gusbcfg;
+
 		usbcx_gusbcfg.u32 = __cvmx_usb_read_csr32(usb,
 				CVMX_USBCX_GUSBCFG(usb->index));
 		usbcx_gusbcfg.s.toutcal = 0;
@@ -949,6 +949,7 @@ static int cvmx_usb_initialize(struct cvmx_usb_state *usb,
 		 */
 		{
 			union cvmx_usbcx_hcfg usbcx_hcfg;
+
 			usbcx_hcfg.u32 = __cvmx_usb_read_csr32(usb,
 					CVMX_USBCX_HCFG(usb->index));
 			usbcx_hcfg.s.fslssupp = 0;
@@ -1076,6 +1077,7 @@ static int cvmx_usb_enable(struct cvmx_usb_state *usb)
 	 */
 	{
 		union cvmx_usbcx_gnptxfsiz siz;
+
 		siz.u32 = __cvmx_usb_read_csr32(usb,
 				CVMX_USBCX_GNPTXFSIZ(usb->index));
 		siz.s.nptxfdep = usbcx_ghwcfg3.s.dfifodepth / 2;
@@ -1090,6 +1092,7 @@ static int cvmx_usb_enable(struct cvmx_usb_state *usb)
 	 */
 	{
 		union cvmx_usbcx_hptxfsiz siz;
+
 		siz.u32 = __cvmx_usb_read_csr32(usb,
 				CVMX_USBCX_HPTXFSIZ(usb->index));
 		siz.s.ptxfsize = usbcx_ghwcfg3.s.dfifodepth / 4;
@@ -1219,8 +1222,8 @@ static struct cvmx_usb_port_status cvmx_usb_get_status(
  * Returns: A non-NULL value is a pipe. NULL means an error.
  */
 static struct cvmx_usb_pipe *cvmx_usb_open_pipe(struct cvmx_usb_state *usb,
-						int device_addr, int
-						endpoint_num,
+						int device_addr,
+						int endpoint_num,
 						enum cvmx_usb_speed
 							device_speed,
 						int max_packet,
@@ -1286,7 +1289,8 @@ static struct cvmx_usb_pipe *cvmx_usb_open_pipe(struct cvmx_usb_state *usb,
 	if (__cvmx_usb_pipe_needs_split(usb, pipe)) {
 		pipe->interval = interval*8;
 		/* Force start splits to be schedule on uFrame 0 */
-		pipe->next_tx_frame = ((usb->frame_number+7)&~7) + pipe->interval;
+		pipe->next_tx_frame = ((usb->frame_number+7)&~7) +
+					pipe->interval;
 	} else {
 		pipe->interval = interval;
 		pipe->next_tx_frame = usb->frame_number + pipe->interval;
@@ -1337,9 +1341,13 @@ static void __cvmx_usb_poll_rx_fifo(struct cvmx_usb_state *usb)
 		return;
 
 	/* Get where the DMA engine would have written this data */
-	address = __cvmx_usb_read_csr64(usb, CVMX_USBNX_DMA0_INB_CHN0(usb->index) + channel*8);
+	address = __cvmx_usb_read_csr64(usb,
+			CVMX_USBNX_DMA0_INB_CHN0(usb->index) + channel*8);
+
 	ptr = cvmx_phys_to_ptr(address);
-	__cvmx_usb_write_csr64(usb, CVMX_USBNX_DMA0_INB_CHN0(usb->index) + channel*8, address + bytes);
+	__cvmx_usb_write_csr64(usb,
+			       CVMX_USBNX_DMA0_INB_CHN0(usb->index) + channel*8,
+			       address + bytes);
 
 	/* Loop writing the FIFO data for this packet into memory */
 	while (bytes > 0) {
@@ -1423,6 +1431,7 @@ static void __cvmx_usb_poll_tx_fifo(struct cvmx_usb_state *usb)
 {
 	if (usb->periodic.head != usb->periodic.tail) {
 		union cvmx_usbcx_hptxsts tx_status;
+
 		tx_status.u32 = __cvmx_usb_read_csr32(usb,
 				CVMX_USBCX_HPTXSTS(usb->index));
 		if (__cvmx_usb_fill_tx_hw(usb, &usb->periodic,
@@ -1438,6 +1447,7 @@ static void __cvmx_usb_poll_tx_fifo(struct cvmx_usb_state *usb)
 
 	if (usb->nonperiodic.head != usb->nonperiodic.tail) {
 		union cvmx_usbcx_gnptxsts tx_status;
+
 		tx_status.u32 = __cvmx_usb_read_csr32(usb,
 				CVMX_USBCX_GNPTXSTS(usb->index));
 		if (__cvmx_usb_fill_tx_hw(usb, &usb->nonperiodic,
@@ -1536,25 +1546,33 @@ static void __cvmx_usb_start_channel_control(struct cvmx_usb_state *usb,
 	switch (transaction->stage) {
 	case CVMX_USB_STAGE_NON_CONTROL:
 	case CVMX_USB_STAGE_NON_CONTROL_SPLIT_COMPLETE:
-		cvmx_dprintf("%s: ERROR - Non control stage\n", __FUNCTION__);
+		cvmx_dprintf("%s: ERROR - Non control stage\n", __func__);
 		break;
 	case CVMX_USB_STAGE_SETUP:
 		usbc_hctsiz.s.pid = 3; /* Setup */
 		bytes_to_transfer = sizeof(*header);
 		/* All Control operations start with a setup going OUT */
-		USB_SET_FIELD32(CVMX_USBCX_HCCHARX(channel, usb->index), union cvmx_usbcx_hccharx, epdir, CVMX_USB_DIRECTION_OUT);
+		USB_SET_FIELD32(CVMX_USBCX_HCCHARX(channel, usb->index),
+				union cvmx_usbcx_hccharx, epdir,
+				CVMX_USB_DIRECTION_OUT);
 		/*
 		 * Setup send the control header instead of the buffer data. The
 		 * buffer data will be used in the next stage
 		 */
-		__cvmx_usb_write_csr64(usb, CVMX_USBNX_DMA0_OUTB_CHN0(usb->index) + channel*8, transaction->control_header);
+		__cvmx_usb_write_csr64(usb,
+			CVMX_USBNX_DMA0_OUTB_CHN0(usb->index) + channel*8,
+			transaction->control_header);
 		break;
 	case CVMX_USB_STAGE_SETUP_SPLIT_COMPLETE:
 		usbc_hctsiz.s.pid = 3; /* Setup */
 		bytes_to_transfer = 0;
 		/* All Control operations start with a setup going OUT */
-		USB_SET_FIELD32(CVMX_USBCX_HCCHARX(channel, usb->index), union cvmx_usbcx_hccharx, epdir, CVMX_USB_DIRECTION_OUT);
-		USB_SET_FIELD32(CVMX_USBCX_HCSPLTX(channel, usb->index), union cvmx_usbcx_hcspltx, compsplt, 1);
+		USB_SET_FIELD32(CVMX_USBCX_HCCHARX(channel, usb->index),
+				union cvmx_usbcx_hccharx, epdir,
+				CVMX_USB_DIRECTION_OUT);
+
+		USB_SET_FIELD32(CVMX_USBCX_HCSPLTX(channel, usb->index),
+				union cvmx_usbcx_hcspltx, compsplt, 1);
 		break;
 	case CVMX_USB_STAGE_DATA:
 		usbc_hctsiz.s.pid = __cvmx_usb_get_data_pid(pipe);
@@ -1684,6 +1702,7 @@ static void __cvmx_usb_start_channel(struct cvmx_usb_state *usb,
 		/* Clear all channel status bits */
 		usbc_hcint.u32 = __cvmx_usb_read_csr32(usb,
 				CVMX_USBCX_HCINTX(channel, usb->index));
+
 		__cvmx_usb_write_csr32(usb,
 				       CVMX_USBCX_HCINTX(channel, usb->index),
 				       usbc_hcint.u32);
@@ -1714,18 +1733,31 @@ static void __cvmx_usb_start_channel(struct cvmx_usb_state *usb,
 		__cvmx_usb_write_csr32(usb, CVMX_USBCX_HCINTMSKX(channel, usb->index), usbc_hcintmsk.u32);
 
 		/* Enable the channel interrupt to propagate */
-		usbc_haintmsk.u32 = __cvmx_usb_read_csr32(usb, CVMX_USBCX_HAINTMSK(usb->index));
+		usbc_haintmsk.u32 = __cvmx_usb_read_csr32(usb,
+					CVMX_USBCX_HAINTMSK(usb->index));
 		usbc_haintmsk.s.haintmsk |= 1<<channel;
-		__cvmx_usb_write_csr32(usb, CVMX_USBCX_HAINTMSK(usb->index), usbc_haintmsk.u32);
+		__cvmx_usb_write_csr32(usb,
+					CVMX_USBCX_HAINTMSK(usb->index),
+					usbc_haintmsk.u32);
 	}
 
 	/* Setup the locations the DMA engines use  */
 	{
-		uint64_t dma_address = transaction->buffer + transaction->actual_bytes;
+		uint64_t dma_address = transaction->buffer +
+					transaction->actual_bytes;
+
 		if (transaction->type == CVMX_USB_TRANSFER_ISOCHRONOUS)
-			dma_address = transaction->buffer + transaction->iso_packets[0].offset + transaction->actual_bytes;
-		__cvmx_usb_write_csr64(usb, CVMX_USBNX_DMA0_OUTB_CHN0(usb->index) + channel*8, dma_address);
-		__cvmx_usb_write_csr64(usb, CVMX_USBNX_DMA0_INB_CHN0(usb->index) + channel*8, dma_address);
+			dma_address = transaction->buffer +
+					transaction->iso_packets[0].offset +
+					transaction->actual_bytes;
+
+		__cvmx_usb_write_csr64(usb,
+			CVMX_USBNX_DMA0_OUTB_CHN0(usb->index) + channel*8,
+			dma_address);
+
+		__cvmx_usb_write_csr64(usb,
+			CVMX_USBNX_DMA0_INB_CHN0(usb->index) + channel*8,
+			dma_address);
 	}
 
 	/* Setup both the size of the transfer and the SPLIT characteristics */
@@ -2008,8 +2040,7 @@ static struct cvmx_usb_pipe *__cvmx_usb_find_ready_pipe(
 			 ((((int)current_frame - (int)pipe->split_sc_frame)
 			   & 0x7f) < 0x40)) &&
 			(!usb->active_split || (usb->active_split == t))) {
-			CVMX_PREFETCH(pipe, 128);
-			CVMX_PREFETCH(t, 0);
+			prefetch(t);
 			return pipe;
 		}
 	}
@@ -2036,8 +2067,16 @@ static void __cvmx_usb_schedule(struct cvmx_usb_state *usb, int is_sof)
 		 * Without DMA we need to be careful to not schedule something
 		 * at the end of a frame and cause an overrun.
 		 */
-		union cvmx_usbcx_hfnum hfnum = {.u32 = __cvmx_usb_read_csr32(usb, CVMX_USBCX_HFNUM(usb->index))};
-		union cvmx_usbcx_hfir hfir = {.u32 = __cvmx_usb_read_csr32(usb, CVMX_USBCX_HFIR(usb->index))};
+		union cvmx_usbcx_hfnum hfnum = {
+			.u32 = __cvmx_usb_read_csr32(usb,
+						CVMX_USBCX_HFNUM(usb->index))
+		};
+
+		union cvmx_usbcx_hfir hfir = {
+			.u32 = __cvmx_usb_read_csr32(usb,
+						CVMX_USBCX_HFIR(usb->index))
+		};
+
 		if (hfnum.s.frrem < hfir.s.frint/4)
 			goto done;
 	}
@@ -2522,6 +2561,7 @@ static int cvmx_usb_cancel_all(struct cvmx_usb_state *usb,
 	/* Simply loop through and attempt to cancel each transaction */
 	list_for_each_entry_safe(transaction, next, &pipe->transactions, node) {
 		int result = cvmx_usb_cancel(usb, pipe, transaction);
+
 		if (unlikely(result != 0))
 			return result;
 	}
@@ -2654,19 +2694,20 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 	}
 
 	/* Disable the channel interrupts now that it is done */
-	__cvmx_usb_write_csr32(usb, CVMX_USBCX_HCINTMSKX(channel, usb->index), 0);
+	__cvmx_usb_write_csr32(usb,
+				CVMX_USBCX_HCINTMSKX(channel, usb->index),
+				0);
 	usb->idle_hardware_channels |= (1<<channel);
 
 	/* Make sure this channel is tied to a valid pipe */
 	pipe = usb->pipe_for_channel[channel];
-	CVMX_PREFETCH(pipe, 0);
-	CVMX_PREFETCH(pipe, 128);
+	prefetch(pipe);
 	if (!pipe)
 		return 0;
 	transaction = list_first_entry(&pipe->transactions,
 				       typeof(*transaction),
 				       node);
-	CVMX_PREFETCH(transaction, 0);
+	prefetch(transaction);
 
 	/*
 	 * Disconnect this pipe from the HW channel. Later the schedule
@@ -3021,7 +3062,8 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 					 */
 					if (!buffer_space_left ||
 						(bytes_this_transfer < 188)) {
-						pipe->next_tx_frame += pipe->interval;
+						pipe->next_tx_frame +=
+							pipe->interval;
 						__cvmx_usb_perform_complete(
 							usb,
 							pipe,
@@ -3083,6 +3125,7 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 				pipe->interval;
 	} else {
 		struct cvmx_usb_port_status port;
+
 		port = cvmx_usb_get_status(usb);
 		if (port.port_enabled) {
 			/* We'll retry the exact same transaction again */
@@ -3123,24 +3166,24 @@ static int cvmx_usb_poll(struct cvmx_usb_state *usb)
 	union cvmx_usbcx_hfnum usbc_hfnum;
 	union cvmx_usbcx_gintsts usbc_gintsts;
 
-	CVMX_PREFETCH(usb, 0);
-	CVMX_PREFETCH(usb, 1*128);
-	CVMX_PREFETCH(usb, 2*128);
-	CVMX_PREFETCH(usb, 3*128);
-	CVMX_PREFETCH(usb, 4*128);
+	prefetch_range(usb, sizeof(*usb));
 
 	/* Update the frame counter */
-	usbc_hfnum.u32 = __cvmx_usb_read_csr32(usb, CVMX_USBCX_HFNUM(usb->index));
+	usbc_hfnum.u32 = __cvmx_usb_read_csr32(usb,
+						CVMX_USBCX_HFNUM(usb->index));
 	if ((usb->frame_number&0x3fff) > usbc_hfnum.s.frnum)
 		usb->frame_number += 0x4000;
 	usb->frame_number &= ~0x3fffull;
 	usb->frame_number |= usbc_hfnum.s.frnum;
 
 	/* Read the pending interrupts */
-	usbc_gintsts.u32 = __cvmx_usb_read_csr32(usb, CVMX_USBCX_GINTSTS(usb->index));
+	usbc_gintsts.u32 = __cvmx_usb_read_csr32(usb,
+						CVMX_USBCX_GINTSTS(usb->index));
 
 	/* Clear the interrupts now that we know about them */
-	__cvmx_usb_write_csr32(usb, CVMX_USBCX_GINTSTS(usb->index), usbc_gintsts.u32);
+	__cvmx_usb_write_csr32(usb,
+				CVMX_USBCX_GINTSTS(usb->index),
+				usbc_gintsts.u32);
 
 	if (usbc_gintsts.s.rxflvl) {
 		/*
@@ -3196,7 +3239,9 @@ static int cvmx_usb_poll(struct cvmx_usb_state *usb)
 		 * to clear this bit.
 		 */
 		union cvmx_usbcx_haint usbc_haint;
-		usbc_haint.u32 = __cvmx_usb_read_csr32(usb, CVMX_USBCX_HAINT(usb->index));
+
+		usbc_haint.u32 = __cvmx_usb_read_csr32(usb,
+					CVMX_USBCX_HAINT(usb->index));
 		while (usbc_haint.u32) {
 			int channel;
 
@@ -3267,6 +3312,7 @@ static int octeon_usb_urb_enqueue(struct usb_hcd *hcd,
 		enum cvmx_usb_speed speed;
 		int split_device = 0;
 		int split_port = 0;
+
 		switch (usb_pipetype(urb->pipe)) {
 		case PIPE_ISOCHRONOUS:
 			transfer_type = CVMX_USB_TRANSFER_ISOCHRONOUS;
@@ -3303,6 +3349,7 @@ static int octeon_usb_urb_enqueue(struct usb_hcd *hcd,
 			 * tree.
 			 */
 			struct usb_device *dev = urb->dev;
+
 			while (dev->parent) {
 				/*
 				 * If our parent is high speed then he'll
@@ -3462,6 +3509,7 @@ static void octeon_usb_endpoint_disable(struct usb_hcd *hcd,
 		struct octeon_hcd *priv = hcd_to_octeon(hcd);
 		struct cvmx_usb_pipe *pipe = ep->hcpriv;
 		unsigned long flags;
+
 		spin_lock_irqsave(&priv->lock, flags);
 		cvmx_usb_cancel_all(&priv->usb, pipe);
 		if (cvmx_usb_close_pipe(&priv->usb, pipe))
@@ -3483,10 +3531,11 @@ static int octeon_usb_hub_status_data(struct usb_hcd *hcd, char *buf)
 	buf[0] = 0;
 	buf[0] = port_status.connect_change << 1;
 
-	return (buf[0] != 0);
+	return buf[0] != 0;
 }
 
-static int octeon_usb_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue, u16 wIndex, char *buf, u16 wLength)
+static int octeon_usb_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
+				u16 wIndex, char *buf, u16 wLength)
 {
 	struct octeon_hcd *priv = hcd_to_octeon(hcd);
 	struct device *dev = hcd->self.controller;
@@ -3778,6 +3827,7 @@ static int octeon_usb_probe(struct platform_device *pdev)
 	if (irq < 0) {
 		/* Defective device tree, but we know how to fix it. */
 		irq_hw_number_t hwirq = usb_num ? (1 << 6) + 17 : 56;
+
 		irq = irq_create_mapping(NULL, hwirq);
 	}
 
diff --git a/drivers/staging/olpc_dcon/olpc_dcon.c b/drivers/staging/olpc_dcon/olpc_dcon.c
index 26b4ec56fd3..eb83b28b8cd 100644
--- a/drivers/staging/olpc_dcon/olpc_dcon.c
+++ b/drivers/staging/olpc_dcon/olpc_dcon.c
@@ -210,6 +210,7 @@ static void dcon_sleep(struct dcon_priv *dcon, bool sleep)
 
 	if (sleep) {
 		u8 pm = 0;
+
 		x = olpc_ec_cmd(EC_DCON_POWER_MODE, &pm, 1, NULL, 0);
 		if (x)
 			pr_warn("unable to force dcon to power down: %d!\n", x);
@@ -240,6 +241,7 @@ static void dcon_sleep(struct dcon_priv *dcon, bool sleep)
 static void dcon_load_holdoff(struct dcon_priv *dcon)
 {
 	struct timespec delta_t, now;
+
 	while (1) {
 		getnstimeofday(&now);
 		delta_t = timespec_sub(now, dcon->load_time);
@@ -399,14 +401,15 @@ static ssize_t dcon_mode_show(struct device *dev,
 	struct device_attribute *attr, char *buf)
 {
 	struct dcon_priv *dcon = dev_get_drvdata(dev);
+
 	return sprintf(buf, "%4.4X\n", dcon->disp_mode);
 }
 
 static ssize_t dcon_sleep_show(struct device *dev,
 	struct device_attribute *attr, char *buf)
 {
-
 	struct dcon_priv *dcon = dev_get_drvdata(dev);
+
 	return sprintf(buf, "%d\n", dcon->asleep);
 }
 
@@ -414,6 +417,7 @@ static ssize_t dcon_freeze_show(struct device *dev,
 	struct device_attribute *attr, char *buf)
 {
 	struct dcon_priv *dcon = dev_get_drvdata(dev);
+
 	return sprintf(buf, "%d\n", dcon->curr_src == DCON_SOURCE_DCON ? 1 : 0);
 }
 
@@ -421,6 +425,7 @@ static ssize_t dcon_mono_show(struct device *dev,
 	struct device_attribute *attr, char *buf)
 {
 	struct dcon_priv *dcon = dev_get_drvdata(dev);
+
 	return sprintf(buf, "%d\n", dcon->mono);
 }
 
@@ -534,6 +539,7 @@ static int dcon_bl_update(struct backlight_device *dev)
 static int dcon_bl_get(struct backlight_device *dev)
 {
 	struct dcon_priv *dcon = bl_get_data(dev);
+
 	return dcon->bl_val;
 }
 
diff --git a/drivers/staging/ozwpan/TODO b/drivers/staging/ozwpan/TODO
index b4febd79a68..f32c1c0bc87 100644
--- a/drivers/staging/ozwpan/TODO
+++ b/drivers/staging/ozwpan/TODO
@@ -10,5 +10,5 @@ TODO:
 	- testing with as many devices as possible.
 
 Please send any patches for this driver to
-Rupesh Gujare <rupesh.gujare@atmel.com>
+Shigekatsu Tateno <shigekatsu.tateno@atmel.com>
 and Greg Kroah-Hartman <gregkh@linuxfoundation.org>.
diff --git a/drivers/staging/ozwpan/ozhcd.c b/drivers/staging/ozwpan/ozhcd.c
index b3d401ae41b..30bd9286d47 100644
--- a/drivers/staging/ozwpan/ozhcd.c
+++ b/drivers/staging/ozwpan/ozhcd.c
@@ -294,6 +294,7 @@ 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;
@@ -424,6 +425,7 @@ static void oz_ep_free(struct oz_port *port, struct oz_endpoint *ep)
 	if (port) {
 		struct 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);
@@ -568,6 +570,7 @@ static int oz_dequeue_ep_urb(struct oz_port *port, u8 ep_addr, int in_dir,
 		ep = port->out_ep[ep_addr];
 	if (ep) {
 		struct list_head *e;
+
 		list_for_each(e, &ep->urb_list) {
 			urbl = container_of(e, struct oz_urb_link, link);
 			if (urbl->urb == urb) {
@@ -599,6 +602,7 @@ static struct urb *oz_find_urb_by_id(struct oz_port *port, int ep_ix,
 	ep = port->out_ep[ep_ix];
 	if (ep) {
 		struct list_head *e;
+
 		list_for_each(e, &ep->urb_list) {
 			urbl = container_of(e, struct oz_urb_link, link);
 			if (urbl->req_id == req_id) {
@@ -815,6 +819,7 @@ void oz_hcd_get_desc_cnf(void *hport, u8 req_id, int status, const u8 *desc,
 	if (status == 0) {
 		int copy_len;
 		int required_size = urb->transfer_buffer_length;
+
 		if (required_size > total_size)
 			required_size = total_size;
 		copy_len = required_size-offset;
@@ -826,6 +831,7 @@ void oz_hcd_get_desc_cnf(void *hport, u8 req_id, int status, const u8 *desc,
 			struct usb_ctrlrequest *setup =
 				(struct usb_ctrlrequest *)urb->setup_packet;
 			unsigned wvalue = le16_to_cpu(setup->wValue);
+
 			if (oz_enqueue_ep_urb(port, 0, 0, urb, req_id))
 				err = -ENOMEM;
 			else if (oz_usb_get_desc_req(port->hpd, req_id,
@@ -919,6 +925,7 @@ static void oz_hcd_complete_set_interface(struct oz_port *port, struct urb *urb,
 	if ((rcode == 0) && (port->config_num > 0)) {
 		struct usb_host_config *config;
 		struct usb_host_interface *intf;
+
 		oz_dbg(ON, "Set interface %d alt %d\n", if_num, alt);
 		oz_clean_endpoints_for_interface(hcd, port, if_num);
 		config = &urb->dev->config[port->config_num-1];
@@ -974,6 +981,7 @@ void oz_hcd_control_cnf(void *hport, u8 req_id, u8 rcode, const u8 *data,
 
 	} else {
 		int copy_len;
+
 		oz_dbg(ON, "VENDOR-CLASS - cnf\n");
 		if (data_len) {
 			if (data_len <= urb->transfer_buffer_length)
@@ -1047,6 +1055,7 @@ void oz_hcd_data_ind(void *hport, u8 endpoint, const u8 *data, int data_len)
 					struct oz_urb_link, link);
 			struct urb *urb;
 			int copy_len;
+
 			list_del_init(&urbl->link);
 			spin_unlock_bh(&ozhcd->hcd_lock);
 			urb = urbl->urb;
@@ -1140,6 +1149,7 @@ int oz_hcd_heartbeat(void *hport)
 	spin_lock_bh(&ozhcd->hcd_lock);
 	list_for_each(e, &port->isoc_in_ep) {
 		struct oz_endpoint *ep = ep_from_link(e);
+
 		if (ep->flags & OZ_F_EP_BUFFERING) {
 			if (ep->buffered_units >= OZ_IN_BUFFERING_UNITS) {
 				ep->flags &= ~OZ_F_EP_BUFFERING;
@@ -1160,6 +1170,7 @@ int oz_hcd_heartbeat(void *hport)
 			int len = 0;
 			int copy_len;
 			int i;
+
 			if (ep->credit  < urb->number_of_packets)
 				break;
 			if (ep->buffered_units < urb->number_of_packets)
@@ -1215,6 +1226,7 @@ int oz_hcd_heartbeat(void *hport)
 	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);
@@ -1557,6 +1569,7 @@ static void oz_process_ep0_urb(struct oz_hcd *ozhcd, struct urb *urb,
 	}
 	if (!rc && !complete) {
 		int data_len = 0;
+
 		if ((setup->bRequestType & USB_DIR_IN) == 0)
 			data_len = wlength;
 		urb->actual_length = data_len;
@@ -1761,6 +1774,7 @@ static void oz_hcd_clear_orphanage(struct oz_hcd *ozhcd, int status)
 {
 	if (ozhcd) {
 		struct oz_urb_link *urbl;
+
 		while (!list_empty(&ozhcd->orphanage)) {
 			urbl = list_first_entry(&ozhcd->orphanage,
 				struct oz_urb_link, link);
@@ -1997,7 +2011,6 @@ static void oz_get_hub_descriptor(struct usb_hcd *hcd,
 static int oz_set_port_feature(struct usb_hcd *hcd, u16 wvalue, u16 windex)
 {
 	struct oz_port *port;
-	int err = 0;
 	u8 port_id = (u8)windex;
 	struct oz_hcd *ozhcd = oz_hcd_private(hcd);
 	unsigned set_bits = 0;
@@ -2064,7 +2077,7 @@ static int oz_set_port_feature(struct usb_hcd *hcd, u16 wvalue, u16 windex)
 		spin_unlock_bh(&port->port_lock);
 	}
 	oz_dbg(HUB, "Port[%d] status = 0x%x\n", port_id, port->status);
-	return err;
+	return 0;
 }
 
 /*
@@ -2073,7 +2086,6 @@ static int oz_set_port_feature(struct usb_hcd *hcd, u16 wvalue, u16 windex)
 static int oz_clear_port_feature(struct usb_hcd *hcd, u16 wvalue, u16 windex)
 {
 	struct oz_port *port;
-	int err = 0;
 	u8 port_id = (u8)windex;
 	struct oz_hcd *ozhcd = oz_hcd_private(hcd);
 	unsigned clear_bits = 0;
@@ -2140,7 +2152,7 @@ static int oz_clear_port_feature(struct usb_hcd *hcd, u16 wvalue, u16 windex)
 	}
 	oz_dbg(HUB, "Port[%d] status = 0x%x\n",
 	       port_id, ozhcd->ports[port_id-1].status);
-	return err;
+	return 0;
 }
 
 /*
@@ -2258,6 +2270,7 @@ static int oz_plat_probe(struct platform_device *dev)
 	spin_lock_init(&ozhcd->hcd_lock);
 	for (i = 0; i < OZ_NB_PORTS; i++) {
 		struct oz_port *port = &ozhcd->ports[i];
+
 		port->ozhcd = ozhcd;
 		port->flags = 0;
 		port->status = 0;
diff --git a/drivers/staging/ozwpan/ozproto.c b/drivers/staging/ozwpan/ozproto.c
index f09acd0bb01..11020559919 100644
--- a/drivers/staging/ozwpan/ozproto.c
+++ b/drivers/staging/ozwpan/ozproto.c
@@ -765,7 +765,7 @@ static char *oz_get_next_device_name(char *s, char *dname, int max_size)
 int oz_protocol_init(char *devs)
 {
 	skb_queue_head_init(&g_rx_queue);
-	if (devs && (devs[0] == '*')) {
+	if (devs[0] == '*') {
 		oz_binding_add(NULL);
 	} else {
 		char d[32];
@@ -792,7 +792,7 @@ int oz_get_pd_list(struct oz_mac_addr *addr, int max_count)
 		if (count >= max_count)
 			break;
 		pd = container_of(e, struct oz_pd, link);
-		memcpy(&addr[count++], pd->mac_addr, ETH_ALEN);
+		ether_addr_copy((u8 *)&addr[count++], pd->mac_addr);
 	}
 	spin_unlock_bh(&g_polling_lock);
 	return count;
diff --git a/drivers/staging/panel/panel.c b/drivers/staging/panel/panel.c
index 08f9a489611..4e9229363c3 100644
--- a/drivers/staging/panel/panel.c
+++ b/drivers/staging/panel/panel.c
@@ -462,7 +462,7 @@ MODULE_PARM_DESC(lcd_type,
 static int lcd_proto = -1;
 module_param(lcd_proto, int, 0000);
 MODULE_PARM_DESC(lcd_proto,
-		"LCD communication: 0=parallel (//), 1=serial, 2=TI LCD Interface");
+		 "LCD communication: 0=parallel (//), 1=serial, 2=TI LCD Interface");
 
 static int lcd_charset = -1;
 module_param(lcd_charset, int, 0000);
@@ -664,8 +664,12 @@ static void pin_to_bits(int pin, unsigned char *d_val, unsigned char *c_val)
 {
 	int d_bit, c_bit, inv;
 
-	d_val[0] = c_val[0] = d_val[1] = c_val[1] = 0;
-	d_val[2] = c_val[2] = 0xFF;
+	d_val[0] = 0;
+	c_val[0] = 0;
+	d_val[1] = 0;
+	c_val[1] = 0;
+	d_val[2] = 0xFF;
+	c_val[2] = 0xFF;
 
 	if (pin == 0)
 		return;
@@ -674,7 +678,8 @@ static void pin_to_bits(int pin, unsigned char *d_val, unsigned char *c_val)
 	if (inv)
 		pin = -pin;
 
-	d_bit = c_bit = 0;
+	d_bit = 0;
+	c_bit = 0;
 
 	switch (pin) {
 	case PIN_STROBE:	/* strobe, inverted */
@@ -711,10 +716,9 @@ static void pin_to_bits(int pin, unsigned char *d_val, unsigned char *c_val)
 /* sleeps that many milliseconds with a reschedule */
 static void long_sleep(int ms)
 {
-
-	if (in_interrupt())
+	if (in_interrupt()) {
 		mdelay(ms);
-	else {
+	} else {
 		current->state = TASK_INTERRUPTIBLE;
 		schedule_timeout((ms * HZ + 999) / 1000);
 	}
@@ -867,7 +871,9 @@ static void lcd_print(char c)
 static void lcd_clear_fast_s(void)
 {
 	int pos;
-	lcd_addr_x = lcd_addr_y = 0;
+
+	lcd_addr_x = 0;
+	lcd_addr_y = 0;
 	lcd_gotoxy();
 
 	spin_lock_irq(&pprt_lock);
@@ -879,7 +885,8 @@ static void lcd_clear_fast_s(void)
 	}
 	spin_unlock_irq(&pprt_lock);
 
-	lcd_addr_x = lcd_addr_y = 0;
+	lcd_addr_x = 0;
+	lcd_addr_y = 0;
 	lcd_gotoxy();
 }
 
@@ -887,7 +894,9 @@ static void lcd_clear_fast_s(void)
 static void lcd_clear_fast_p8(void)
 {
 	int pos;
-	lcd_addr_x = lcd_addr_y = 0;
+
+	lcd_addr_x = 0;
+	lcd_addr_y = 0;
 	lcd_gotoxy();
 
 	spin_lock_irq(&pprt_lock);
@@ -914,7 +923,8 @@ static void lcd_clear_fast_p8(void)
 	}
 	spin_unlock_irq(&pprt_lock);
 
-	lcd_addr_x = lcd_addr_y = 0;
+	lcd_addr_x = 0;
+	lcd_addr_y = 0;
 	lcd_gotoxy();
 }
 
@@ -922,7 +932,9 @@ static void lcd_clear_fast_p8(void)
 static void lcd_clear_fast_tilcd(void)
 {
 	int pos;
-	lcd_addr_x = lcd_addr_y = 0;
+
+	lcd_addr_x = 0;
+	lcd_addr_y = 0;
 	lcd_gotoxy();
 
 	spin_lock_irq(&pprt_lock);
@@ -934,7 +946,8 @@ static void lcd_clear_fast_tilcd(void)
 
 	spin_unlock_irq(&pprt_lock);
 
-	lcd_addr_x = lcd_addr_y = 0;
+	lcd_addr_x = 0;
+	lcd_addr_y = 0;
 	lcd_gotoxy();
 }
 
@@ -942,14 +955,14 @@ static void lcd_clear_fast_tilcd(void)
 static void lcd_clear_display(void)
 {
 	lcd_write_cmd(0x01);	/* clear display */
-	lcd_addr_x = lcd_addr_y = 0;
+	lcd_addr_x = 0;
+	lcd_addr_y = 0;
 	/* we must wait a few milliseconds (15) */
 	long_sleep(15);
 }
 
 static void lcd_init_display(void)
 {
-
 	lcd_flags = ((lcd_height > 1) ? LCD_FLAG_N : 0)
 	    | LCD_FLAG_D | LCD_FLAG_C | LCD_FLAG_B;
 
@@ -1092,6 +1105,7 @@ static inline int handle_lcd_special_code(void)
 		break;
 	case 'k': {	/* kill end of line */
 		int x;
+
 		for (x = lcd_addr_x; x < lcd_bwidth; x++)
 			lcd_write_data(' ');
 
@@ -1137,13 +1151,13 @@ static inline int handle_lcd_special_code(void)
 		value = 0;
 		while (*esc && cgoffset < 8) {
 			shift ^= 4;
-			if (*esc >= '0' && *esc <= '9')
+			if (*esc >= '0' && *esc <= '9') {
 				value |= (*esc - '0') << shift;
-			else if (*esc >= 'A' && *esc <= 'Z')
+			} else if (*esc >= 'A' && *esc <= 'Z') {
 				value |= (*esc - 'A' + 10) << shift;
-			else if (*esc >= 'a' && *esc <= 'z')
+			} else if (*esc >= 'a' && *esc <= 'z') {
 				value |= (*esc - 'a' + 10) << shift;
-			else {
+			} else {
 				esc++;
 				continue;
 			}
@@ -1179,8 +1193,9 @@ static inline int handle_lcd_special_code(void)
 				esc++;
 				if (kstrtoul(esc, 10, &lcd_addr_y) < 0)
 					break;
-			} else
+			} else {
 				break;
+			}
 		}
 
 		lcd_gotoxy();
@@ -1217,111 +1232,114 @@ static inline int handle_lcd_special_code(void)
 	return processed;
 }
 
+static void lcd_write_char(char c)
+{
+	/* first, we'll test if we're in escape mode */
+	if ((c != '\n') && lcd_escape_len >= 0) {
+		/* yes, let's add this char to the buffer */
+		lcd_escape[lcd_escape_len++] = c;
+		lcd_escape[lcd_escape_len] = 0;
+	} else {
+		/* aborts any previous escape sequence */
+		lcd_escape_len = -1;
+
+		switch (c) {
+		case LCD_ESCAPE_CHAR:
+			/* start of an escape sequence */
+			lcd_escape_len = 0;
+			lcd_escape[lcd_escape_len] = 0;
+			break;
+		case '\b':
+			/* go back one char and clear it */
+			if (lcd_addr_x > 0) {
+				/* check if we're not at the
+				   end of the line */
+				if (lcd_addr_x < lcd_bwidth)
+					/* back one char */
+					lcd_write_cmd(0x10);
+				lcd_addr_x--;
+			}
+			/* replace with a space */
+			lcd_write_data(' ');
+			/* back one char again */
+			lcd_write_cmd(0x10);
+			break;
+		case '\014':
+			/* quickly clear the display */
+			lcd_clear_fast();
+			break;
+		case '\n':
+			/* flush the remainder of the current line and
+			   go to the beginning of the next line */
+			for (; lcd_addr_x < lcd_bwidth; lcd_addr_x++)
+				lcd_write_data(' ');
+			lcd_addr_x = 0;
+			lcd_addr_y = (lcd_addr_y + 1) % lcd_height;
+			lcd_gotoxy();
+			break;
+		case '\r':
+			/* go to the beginning of the same line */
+			lcd_addr_x = 0;
+			lcd_gotoxy();
+			break;
+		case '\t':
+			/* print a space instead of the tab */
+			lcd_print(' ');
+			break;
+		default:
+			/* simply print this char */
+			lcd_print(c);
+			break;
+		}
+	}
+
+	/* now we'll see if we're in an escape mode and if the current
+	   escape sequence can be understood. */
+	if (lcd_escape_len >= 2) {
+		int processed = 0;
+
+		if (!strcmp(lcd_escape, "[2J")) {
+			/* clear the display */
+			lcd_clear_fast();
+			processed = 1;
+		} else if (!strcmp(lcd_escape, "[H")) {
+			/* cursor to home */
+			lcd_addr_x = 0;
+			lcd_addr_y = 0;
+			lcd_gotoxy();
+			processed = 1;
+		}
+		/* codes starting with ^[[L */
+		else if ((lcd_escape_len >= 3) &&
+			 (lcd_escape[0] == '[') &&
+			 (lcd_escape[1] == 'L')) {
+			processed = handle_lcd_special_code();
+		}
+
+		/* LCD special escape codes */
+		/* flush the escape sequence if it's been processed
+		   or if it is getting too long. */
+		if (processed || (lcd_escape_len >= LCD_ESCAPE_LEN))
+			lcd_escape_len = -1;
+	} /* escape codes */
+}
+
 static ssize_t lcd_write(struct file *file,
-			 const char *buf, size_t count, loff_t *ppos)
+			 const char __user *buf, size_t count, loff_t *ppos)
 {
-	const char *tmp = buf;
+	const char __user *tmp = buf;
 	char c;
 
-	for (; count-- > 0; (ppos ? (*ppos)++ : 0), ++tmp) {
+	for (; count-- > 0; (*ppos)++, tmp++) {
 		if (!in_interrupt() && (((count + 1) & 0x1f) == 0))
 			/* let's be a little nice with other processes
 			   that need some CPU */
 			schedule();
 
-		if (ppos == NULL && file == NULL)
-			/* let's not use get_user() from the kernel ! */
-			c = *tmp;
-		else if (get_user(c, tmp))
+		if (get_user(c, tmp))
 			return -EFAULT;
 
-		/* first, we'll test if we're in escape mode */
-		if ((c != '\n') && lcd_escape_len >= 0) {
-			/* yes, let's add this char to the buffer */
-			lcd_escape[lcd_escape_len++] = c;
-			lcd_escape[lcd_escape_len] = 0;
-		} else {
-			/* aborts any previous escape sequence */
-			lcd_escape_len = -1;
-
-			switch (c) {
-			case LCD_ESCAPE_CHAR:
-				/* start of an escape sequence */
-				lcd_escape_len = 0;
-				lcd_escape[lcd_escape_len] = 0;
-				break;
-			case '\b':
-				/* go back one char and clear it */
-				if (lcd_addr_x > 0) {
-					/* check if we're not at the
-					   end of the line */
-					if (lcd_addr_x < lcd_bwidth)
-						/* back one char */
-						lcd_write_cmd(0x10);
-					lcd_addr_x--;
-				}
-				/* replace with a space */
-				lcd_write_data(' ');
-				/* back one char again */
-				lcd_write_cmd(0x10);
-				break;
-			case '\014':
-				/* quickly clear the display */
-				lcd_clear_fast();
-				break;
-			case '\n':
-				/* flush the remainder of the current line and
-				   go to the beginning of the next line */
-				for (; lcd_addr_x < lcd_bwidth; lcd_addr_x++)
-					lcd_write_data(' ');
-				lcd_addr_x = 0;
-				lcd_addr_y = (lcd_addr_y + 1) % lcd_height;
-				lcd_gotoxy();
-				break;
-			case '\r':
-				/* go to the beginning of the same line */
-				lcd_addr_x = 0;
-				lcd_gotoxy();
-				break;
-			case '\t':
-				/* print a space instead of the tab */
-				lcd_print(' ');
-				break;
-			default:
-				/* simply print this char */
-				lcd_print(c);
-				break;
-			}
-		}
-
-		/* now we'll see if we're in an escape mode and if the current
-		   escape sequence can be understood. */
-		if (lcd_escape_len >= 2) {
-			int processed = 0;
-
-			if (!strcmp(lcd_escape, "[2J")) {
-				/* clear the display */
-				lcd_clear_fast();
-				processed = 1;
-			} else if (!strcmp(lcd_escape, "[H")) {
-				/* cursor to home */
-				lcd_addr_x = lcd_addr_y = 0;
-				lcd_gotoxy();
-				processed = 1;
-			}
-			/* codes starting with ^[[L */
-			else if ((lcd_escape_len >= 3) &&
-				 (lcd_escape[0] == '[') &&
-				 (lcd_escape[1] == 'L')) {
-				processed = handle_lcd_special_code();
-			}
-
-			/* LCD special escape codes */
-			/* flush the escape sequence if it's been processed
-			   or if it is getting too long. */
-			if (processed || (lcd_escape_len >= LCD_ESCAPE_LEN))
-				lcd_escape_len = -1;
-		} /* escape codes */
+		lcd_write_char(c);
 	}
 
 	return tmp - buf;
@@ -1365,8 +1383,19 @@ static struct miscdevice lcd_dev = {
 /* public function usable from the kernel for any purpose */
 static void panel_lcd_print(const char *s)
 {
-	if (lcd_enabled && lcd_initialized)
-		lcd_write(NULL, s, strlen(s), NULL);
+	const char *tmp = s;
+	int count = strlen(s);
+
+	if (lcd_enabled && lcd_initialized) {
+		for (; count-- > 0; tmp++) {
+			if (!in_interrupt() && (((count + 1) & 0x1f) == 0))
+				/* let's be a little nice with other processes
+				   that need some CPU */
+				schedule();
+
+			lcd_write_char(*tmp);
+		}
+	}
 }
 
 /* initialize the LCD driver */
@@ -1560,7 +1589,8 @@ static void lcd_init(void)
 	panel_lcd_print("\x1b[Lc\x1b[Lb\x1b[L*Linux-" UTS_RELEASE "\nPanel-"
 			PANEL_VERSION);
 #endif
-	lcd_addr_x = lcd_addr_y = 0;
+	lcd_addr_x = 0;
+	lcd_addr_y = 0;
 	/* clear the display on the next device opening */
 	lcd_must_clear = 1;
 	lcd_gotoxy();
@@ -1571,11 +1601,10 @@ static void lcd_init(void)
  */
 
 static ssize_t keypad_read(struct file *file,
-			   char *buf, size_t count, loff_t *ppos)
+			   char __user *buf, size_t count, loff_t *ppos)
 {
-
 	unsigned i = *ppos;
-	char *tmp = buf;
+	char __user *tmp = buf;
 
 	if (keypad_buflen == 0) {
 		if (file->f_flags & O_NONBLOCK)
@@ -1598,7 +1627,6 @@ static ssize_t keypad_read(struct file *file,
 
 static int keypad_open(struct inode *inode, struct file *file)
 {
-
 	if (keypad_open_cnt)
 		return -EBUSY;	/* open only once at a time */
 
@@ -1728,8 +1756,8 @@ static inline int input_state_high(struct logical_input *input)
 	 * release function.
 	 * eg: 0 -(press A)-> A -(press B)-> AB : don't match A's release.
 	 */
-	if (((phys_prev & input->mask) == input->value)
-	    && ((phys_curr & input->mask) > input->value)) {
+	if (((phys_prev & input->mask) == input->value) &&
+	    ((phys_curr & input->mask) >  input->value)) {
 		input->state = INPUT_ST_LOW; /* invalidate */
 		return 1;
 	}
@@ -1747,16 +1775,20 @@ static inline int input_state_high(struct logical_input *input)
 
 			if (input->high_timer == 0) {
 				char *press_str = input->u.kbd.press_str;
+
 				if (press_str[0]) {
 					int s = sizeof(input->u.kbd.press_str);
+
 					keypad_send_key(press_str, s);
 				}
 			}
 
 			if (input->u.kbd.repeat_str[0]) {
 				char *repeat_str = input->u.kbd.repeat_str;
+
 				if (input->high_timer >= KEYPAD_REP_START) {
 					int s = sizeof(input->u.kbd.repeat_str);
+
 					input->high_timer -= KEYPAD_REP_DELAY;
 					keypad_send_key(repeat_str, s);
 				}
@@ -1780,8 +1812,8 @@ static inline void input_state_falling(struct logical_input *input)
 {
 #if 0
 	/* FIXME !!! same comment as in input_state_high */
-	if (((phys_prev & input->mask) == input->value)
-	    && ((phys_curr & input->mask) > input->value)) {
+	if (((phys_prev & input->mask) == input->value) &&
+	    ((phys_curr & input->mask) >  input->value)) {
 		input->state = INPUT_ST_LOW;	/* invalidate */
 		return;
 	}
@@ -1794,8 +1826,10 @@ static inline void input_state_falling(struct logical_input *input)
 
 			if (input->u.kbd.repeat_str[0]) {
 				char *repeat_str = input->u.kbd.repeat_str;
+
 				if (input->high_timer >= KEYPAD_REP_START) {
 					int s = sizeof(input->u.kbd.repeat_str);
+
 					input->high_timer -= KEYPAD_REP_DELAY;
 					keypad_send_key(repeat_str, s);
 				}
@@ -1811,12 +1845,15 @@ static inline void input_state_falling(struct logical_input *input)
 		/* call release event */
 		if (input->type == INPUT_TYPE_STD) {
 			void (*release_fct)(int) = input->u.std.release_fct;
+
 			if (release_fct != NULL)
 				release_fct(input->u.std.release_data);
 		} else if (input->type == INPUT_TYPE_KBD) {
 			char *release_str = input->u.kbd.release_str;
+
 			if (release_str[0]) {
 				int s = sizeof(input->u.kbd.release_str);
+
 				keypad_send_key(release_str, s);
 			}
 		}
@@ -1930,12 +1967,17 @@ static int input_name2mask(const char *name, pmask_t *mask, pmask_t *value,
 	char im, om;
 	pmask_t m, v;
 
-	om = im = m = v = 0ULL;
+	om = 0ULL;
+	im = 0ULL;
+	m = 0ULL;
+	v = 0ULL;
 	while (*name) {
 		int in, out, bit, neg;
-		for (in = 0; (in < sizeof(sigtab)) &&
-			     (sigtab[in] != *name); in++)
+
+		for (in = 0; (in < sizeof(sigtab)) && (sigtab[in] != *name);
+		     in++)
 			;
+
 		if (in >= sizeof(sigtab))
 			return 0;	/* input name not found */
 		neg = (in & 1);	/* odd (lower) names are negated */
@@ -1946,10 +1988,11 @@ static int input_name2mask(const char *name, pmask_t *mask, pmask_t *value,
 		if (isdigit(*name)) {
 			out = *name - '0';
 			om |= (1 << out);
-		} else if (*name == '-')
+		} else if (*name == '-') {
 			out = 8;
-		else
+		} else {
 			return 0;	/* unknown bit name */
+		}
 
 		bit = (out * 5) + in;
 
@@ -1977,7 +2020,7 @@ static struct logical_input *panel_bind_key(const char *name, const char *press,
 {
 	struct logical_input *key;
 
-	key = kzalloc(sizeof(struct logical_input), GFP_KERNEL);
+	key = kzalloc(sizeof(*key), GFP_KERNEL);
 	if (!key)
 		return NULL;
 
@@ -2015,7 +2058,7 @@ static struct logical_input *panel_bind_callback(char *name,
 {
 	struct logical_input *callback;
 
-	callback = kmalloc(sizeof(struct logical_input), GFP_KERNEL);
+	callback = kmalloc(sizeof(*callback), GFP_KERNEL);
 	if (!callback)
 		return NULL;
 
@@ -2040,6 +2083,7 @@ static struct logical_input *panel_bind_callback(char *name,
 static void keypad_init(void)
 {
 	int keynum;
+
 	init_waitqueue_head(&keypad_read_wait);
 	keypad_buflen = 0;	/* flushes any eventual noisy keystroke */
 
@@ -2298,7 +2342,7 @@ static void __exit panel_cleanup_module(void)
 	unregister_reboot_notifier(&panel_notifier);
 
 	if (scan_timer.function != NULL)
-		del_timer(&scan_timer);
+		del_timer_sync(&scan_timer);
 
 	if (pprt != NULL) {
 		if (keypad_enabled) {
diff --git a/drivers/staging/rtl8188eu/core/rtw_ap.c b/drivers/staging/rtl8188eu/core/rtw_ap.c
index ff74d0d7efa..85fda6128db 100644
--- a/drivers/staging/rtl8188eu/core/rtw_ap.c
+++ b/drivers/staging/rtl8188eu/core/rtw_ap.c
@@ -1306,6 +1306,10 @@ static void update_bcn_wps_ie(struct adapter *padapter)
 
 	DBG_88E("%s\n", __func__);
 
+	pwps_ie_src = pmlmepriv->wps_beacon_ie;
+	if (pwps_ie_src == NULL)
+		return;
+
 	pwps_ie = rtw_get_wps_ie(ie+_FIXED_IE_LENGTH_, ielen-_FIXED_IE_LENGTH_, NULL, &wps_ielen);
 
 	if (pwps_ie == NULL || wps_ielen == 0)
@@ -1323,10 +1327,6 @@ static void update_bcn_wps_ie(struct adapter *padapter)
 			memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);
 	}
 
-	pwps_ie_src = pmlmepriv->wps_beacon_ie;
-	if (pwps_ie_src == NULL)
-		return;
-
 	wps_ielen = (uint)pwps_ie_src[1];/* to get ie data len */
 	if ((wps_offset+wps_ielen+2+remainder_ielen) <= MAX_IE_SZ) {
 		memcpy(pwps_ie, pwps_ie_src, wps_ielen+2);
diff --git a/drivers/staging/rtl8188eu/core/rtw_br_ext.c b/drivers/staging/rtl8188eu/core/rtw_br_ext.c
index e843c6bd452..f97f05f4165 100644
--- a/drivers/staging/rtl8188eu/core/rtw_br_ext.c
+++ b/drivers/staging/rtl8188eu/core/rtw_br_ext.c
@@ -442,6 +442,7 @@ static void __nat25_db_print(struct adapter *priv)
 void nat25_db_cleanup(struct adapter *priv)
 {
 	int i;
+
 	spin_lock_bh(&priv->br_ext_lock);
 
 	for (i = 0; i < NAT25_HASH_SIZE; i++) {
diff --git a/drivers/staging/rtl8188eu/core/rtw_cmd.c b/drivers/staging/rtl8188eu/core/rtw_cmd.c
index c2fb050337d..1e0b8b49fc1 100644
--- a/drivers/staging/rtl8188eu/core/rtw_cmd.c
+++ b/drivers/staging/rtl8188eu/core/rtw_cmd.c
@@ -1431,41 +1431,6 @@ exit:
 	return res;
 }
 
-u8 rtw_led_blink_cmd(struct adapter *padapter, struct LED_871x *pLed)
-{
-	struct	cmd_obj *pcmdobj;
-	struct	LedBlink_param *ledBlink_param;
-	struct	cmd_priv   *pcmdpriv = &padapter->cmdpriv;
-
-	u8	res = _SUCCESS;
-
-
-	RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_led_blink_cmd\n"));
-
-	pcmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct	cmd_obj));
-	if (pcmdobj == NULL) {
-		res = _FAIL;
-		goto exit;
-	}
-
-	ledBlink_param = (struct	LedBlink_param *)rtw_zmalloc(sizeof(struct	LedBlink_param));
-	if (ledBlink_param == NULL) {
-		kfree(pcmdobj);
-		res = _FAIL;
-		goto exit;
-	}
-
-	ledBlink_param->pLed = pLed;
-
-	init_h2fwcmd_w_parm_no_rsp(pcmdobj, ledBlink_param, GEN_CMD_CODE(_LedBlink));
-	res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
-
-exit:
-
-
-	return res;
-}
-
 u8 rtw_set_csa_cmd(struct adapter *padapter, u8 new_ch_no)
 {
 	struct	cmd_obj *pcmdobj;
diff --git a/drivers/staging/rtl8188eu/core/rtw_led.c b/drivers/staging/rtl8188eu/core/rtw_led.c
index 42b41ab1bce..87d6f063476 100644
--- a/drivers/staging/rtl8188eu/core/rtw_led.c
+++ b/drivers/staging/rtl8188eu/core/rtw_led.c
@@ -40,7 +40,6 @@ void BlinkTimerCallback(void *data)
 /*  */
 /*	Description: */
 /*		Callback function of LED BlinkWorkItem. */
-/*		We dispatch acture LED blink action according to LedStrategy. */
 /*  */
 void BlinkWorkItemCallback(struct work_struct *work)
 {
@@ -71,10 +70,9 @@ void ResetLedStatus(struct LED_871x *pLed)
 
 /*Description: */
 /*		Initialize an LED_871x object. */
-void InitLed871x(struct adapter *padapter, struct LED_871x *pLed, enum LED_PIN_871x LedPin)
+void InitLed871x(struct adapter *padapter, struct LED_871x *pLed)
 {
 	pLed->padapter = padapter;
-	pLed->LedPin = LedPin;
 
 	ResetLedStatus(pLed);
 
@@ -101,87 +99,6 @@ void DeInitLed871x(struct LED_871x *pLed)
 /*		It toggle off LED and schedule corresponding timer if necessary. */
 /*  */
 
-static void SwLedBlink(struct LED_871x *pLed)
-{
-	struct adapter *padapter = pLed->padapter;
-	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
-	u8 bStopBlinking = false;
-
-	/*  Change LED according to BlinkingLedState specified. */
-	if (pLed->BlinkingLedState == RTW_LED_ON) {
-		SwLedOn(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
-	} else {
-		SwLedOff(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
-	}
-
-	/*  Determine if we shall change LED state again. */
-	pLed->BlinkTimes--;
-	switch (pLed->CurrLedState) {
-	case LED_BLINK_NORMAL:
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		break;
-	case LED_BLINK_StartToBlink:
-		if (check_fwstate(pmlmepriv, _FW_LINKED) && check_fwstate(pmlmepriv, WIFI_STATION_STATE))
-			bStopBlinking = true;
-		if (check_fwstate(pmlmepriv, _FW_LINKED) &&
-		    (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
-		    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)))
-			bStopBlinking = true;
-		else if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		break;
-	case LED_BLINK_WPS:
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		break;
-	default:
-		bStopBlinking = true;
-		break;
-	}
-
-	if (bStopBlinking) {
-		/* if (padapter->pwrctrlpriv.cpwm >= PS_STATE_S2) */
-		if (0) {
-			SwLedOff(padapter, pLed);
-		} else if ((check_fwstate(pmlmepriv, _FW_LINKED)) && (!pLed->bLedOn)) {
-			SwLedOn(padapter, pLed);
-		} else if ((check_fwstate(pmlmepriv, _FW_LINKED)) &&  pLed->bLedOn) {
-			SwLedOff(padapter, pLed);
-		}
-		pLed->BlinkTimes = 0;
-		pLed->bLedBlinkInProgress = false;
-	} else {
-		/*  Assign LED state to toggle. */
-		if (pLed->BlinkingLedState == RTW_LED_ON)
-			pLed->BlinkingLedState = RTW_LED_OFF;
-		else
-			pLed->BlinkingLedState = RTW_LED_ON;
-
-		/*  Schedule a timer to toggle LED state. */
-		switch (pLed->CurrLedState) {
-		case LED_BLINK_NORMAL:
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
-			break;
-		case LED_BLINK_SLOWLY:
-		case LED_BLINK_StartToBlink:
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);
-			break;
-		case LED_BLINK_WPS:
-			if (pLed->BlinkingLedState == RTW_LED_ON)
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_LONG_INTERVAL);
-			else
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_LONG_INTERVAL);
-			break;
-		default:
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);
-			break;
-		}
-	}
-}
-
 static void SwLedBlink1(struct LED_871x *pLed)
 {
 	struct adapter *padapter = pLed->padapter;
@@ -319,449 +236,6 @@ static void SwLedBlink1(struct LED_871x *pLed)
 	}
 }
 
-static void SwLedBlink2(struct LED_871x *pLed)
-{
-	struct adapter *padapter = pLed->padapter;
-	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
-	u8 bStopBlinking = false;
-
-	/*  Change LED according to BlinkingLedState specified. */
-	if (pLed->BlinkingLedState == RTW_LED_ON) {
-		SwLedOn(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
-	} else {
-		SwLedOff(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
-	}
-
-	switch (pLed->CurrLedState) {
-	case LED_BLINK_SCAN:
-		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		if (bStopBlinking) {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-				SwLedOff(padapter, pLed);
-			} else if (check_fwstate(pmlmepriv, _FW_LINKED)) {
-				pLed->CurrLedState = RTW_LED_ON;
-				pLed->BlinkingLedState = RTW_LED_ON;
-				SwLedOn(padapter, pLed);
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("stop scan blink CurrLedState %d\n", pLed->CurrLedState));
-
-			} else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
-				pLed->CurrLedState = RTW_LED_OFF;
-				pLed->BlinkingLedState = RTW_LED_OFF;
-				SwLedOff(padapter, pLed);
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("stop scan blink CurrLedState %d\n", pLed->CurrLedState));
-			}
-			pLed->bLedScanBlinkInProgress = false;
-		} else {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-				SwLedOff(padapter, pLed);
-			} else {
-				 if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
-			}
-		}
-		break;
-	case LED_BLINK_TXRX:
-		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		if (bStopBlinking) {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-				SwLedOff(padapter, pLed);
-			} else if (check_fwstate(pmlmepriv, _FW_LINKED)) {
-				pLed->CurrLedState = RTW_LED_ON;
-				pLed->BlinkingLedState = RTW_LED_ON;
-				SwLedOn(padapter, pLed);
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("stop CurrLedState %d\n", pLed->CurrLedState));
-			} else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
-				pLed->CurrLedState = RTW_LED_OFF;
-				pLed->BlinkingLedState = RTW_LED_OFF;
-				SwLedOff(padapter, pLed);
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("stop CurrLedState %d\n", pLed->CurrLedState));
-			}
-			pLed->bLedBlinkInProgress = false;
-		} else {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-				SwLedOff(padapter, pLed);
-			} else {
-				 if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
-			}
-		}
-		break;
-	default:
-		break;
-	}
-}
-
-static void SwLedBlink3(struct LED_871x *pLed)
-{
-	struct adapter *padapter = pLed->padapter;
-	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
-	u8 bStopBlinking = false;
-
-	/*  Change LED according to BlinkingLedState specified. */
-	if (pLed->BlinkingLedState == RTW_LED_ON) {
-		SwLedOn(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
-	} else {
-		if (pLed->CurrLedState != LED_BLINK_WPS_STOP)
-			SwLedOff(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
-	}
-
-	switch (pLed->CurrLedState) {
-	case LED_BLINK_SCAN:
-		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		if (bStopBlinking) {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-				SwLedOff(padapter, pLed);
-			} else if (check_fwstate(pmlmepriv, _FW_LINKED)) {
-				pLed->CurrLedState = RTW_LED_ON;
-				pLed->BlinkingLedState = RTW_LED_ON;
-				if (!pLed->bLedOn)
-					SwLedOn(padapter, pLed);
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-			} else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
-				pLed->CurrLedState = RTW_LED_OFF;
-				pLed->BlinkingLedState = RTW_LED_OFF;
-				if (pLed->bLedOn)
-					SwLedOff(padapter, pLed);
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-			}
-			pLed->bLedScanBlinkInProgress = false;
-		} else {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-				SwLedOff(padapter, pLed);
-			} else {
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
-			}
-		}
-		break;
-	case LED_BLINK_TXRX:
-		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		if (bStopBlinking) {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-				SwLedOff(padapter, pLed);
-			} else if (check_fwstate(pmlmepriv, _FW_LINKED)) {
-				pLed->CurrLedState = RTW_LED_ON;
-				pLed->BlinkingLedState = RTW_LED_ON;
-				if (!pLed->bLedOn)
-					SwLedOn(padapter, pLed);
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-			} else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
-				pLed->CurrLedState = RTW_LED_OFF;
-				pLed->BlinkingLedState = RTW_LED_OFF;
-
-				if (pLed->bLedOn)
-					SwLedOff(padapter, pLed);
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-			}
-			pLed->bLedBlinkInProgress = false;
-		} else {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-				SwLedOff(padapter, pLed);
-			} else {
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
-			}
-		}
-		break;
-	case LED_BLINK_WPS:
-		if (pLed->bLedOn)
-			pLed->BlinkingLedState = RTW_LED_OFF;
-		else
-			pLed->BlinkingLedState = RTW_LED_ON;
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
-		break;
-	case LED_BLINK_WPS_STOP:	/* WPS success */
-		if (pLed->BlinkingLedState == RTW_LED_ON) {
-			pLed->BlinkingLedState = RTW_LED_OFF;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA);
-			bStopBlinking = false;
-		} else {
-			bStopBlinking = true;
-		}
-		if (bStopBlinking) {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-				SwLedOff(padapter, pLed);
-			} else {
-				pLed->CurrLedState = RTW_LED_ON;
-				pLed->BlinkingLedState = RTW_LED_ON;
-				SwLedOn(padapter, pLed);
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-			}
-			pLed->bLedWPSBlinkInProgress = false;
-		}
-		break;
-	default:
-		break;
-	}
-}
-
-static void SwLedBlink4(struct LED_871x *pLed)
-{
-	struct adapter *padapter = pLed->padapter;
-	struct led_priv *ledpriv = &(padapter->ledpriv);
-	struct LED_871x *pLed1 = &(ledpriv->SwLed1);
-	u8 bStopBlinking = false;
-
-	/*  Change LED according to BlinkingLedState specified. */
-	if (pLed->BlinkingLedState == RTW_LED_ON) {
-		SwLedOn(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
-	} else {
-		SwLedOff(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
-	}
-
-	if (!pLed1->bLedWPSBlinkInProgress && pLed1->BlinkingLedState == LED_UNKNOWN) {
-		pLed1->BlinkingLedState = RTW_LED_OFF;
-		pLed1->CurrLedState = RTW_LED_OFF;
-		SwLedOff(padapter, pLed1);
-	}
-
-	switch (pLed->CurrLedState) {
-	case LED_BLINK_SLOWLY:
-		if (pLed->bLedOn)
-			pLed->BlinkingLedState = RTW_LED_OFF;
-		else
-			pLed->BlinkingLedState = RTW_LED_ON;
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
-		break;
-	case LED_BLINK_StartToBlink:
-		if (pLed->bLedOn) {
-			pLed->BlinkingLedState = RTW_LED_OFF;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);
-		} else {
-			pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
-		}
-		break;
-	case LED_BLINK_SCAN:
-		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = false;
-		if (bStopBlinking) {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-				SwLedOff(padapter, pLed);
-			} else {
-				pLed->bLedNoLinkBlinkInProgress = false;
-				pLed->CurrLedState = LED_BLINK_SLOWLY;
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
-			}
-			pLed->bLedScanBlinkInProgress = false;
-		} else {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-				SwLedOff(padapter, pLed);
-			} else {
-				 if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
-			}
-		}
-		break;
-	case LED_BLINK_TXRX:
-		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		if (bStopBlinking) {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-				SwLedOff(padapter, pLed);
-			} else {
-				pLed->bLedNoLinkBlinkInProgress = true;
-				pLed->CurrLedState = LED_BLINK_SLOWLY;
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
-			}
-			pLed->bLedBlinkInProgress = false;
-		} else {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-				SwLedOff(padapter, pLed);
-			} else {
-				 if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
-			}
-		}
-		break;
-	case LED_BLINK_WPS:
-		if (pLed->bLedOn) {
-			pLed->BlinkingLedState = RTW_LED_OFF;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);
-		} else {
-			pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
-		}
-		break;
-	case LED_BLINK_WPS_STOP:	/* WPS authentication fail */
-		if (pLed->bLedOn)
-			pLed->BlinkingLedState = RTW_LED_OFF;
-		else
-			pLed->BlinkingLedState = RTW_LED_ON;
-
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
-		break;
-	case LED_BLINK_WPS_STOP_OVERLAP:	/* WPS session overlap */
-		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0) {
-			if (pLed->bLedOn)
-				pLed->BlinkTimes = 1;
-			else
-				bStopBlinking = true;
-		}
-
-		if (bStopBlinking) {
-			pLed->BlinkTimes = 10;
-			pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_LINK_INTERVAL_ALPHA);
-		} else {
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
-		}
-		break;
-	default:
-		break;
-	}
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("SwLedBlink4 CurrLedState %d\n", pLed->CurrLedState));
-}
-
-static void SwLedBlink5(struct LED_871x *pLed)
-{
-	struct adapter *padapter = pLed->padapter;
-	u8 bStopBlinking = false;
-
-	/*  Change LED according to BlinkingLedState specified. */
-	if (pLed->BlinkingLedState == RTW_LED_ON) {
-		SwLedOn(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
-	} else {
-		SwLedOff(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
-	}
-
-	switch (pLed->CurrLedState) {
-	case LED_BLINK_SCAN:
-		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-
-		if (bStopBlinking) {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-				pLed->CurrLedState = RTW_LED_OFF;
-				pLed->BlinkingLedState = RTW_LED_OFF;
-				if (pLed->bLedOn)
-					SwLedOff(padapter, pLed);
-			} else {
-					pLed->CurrLedState = RTW_LED_ON;
-					pLed->BlinkingLedState = RTW_LED_ON;
-					if (!pLed->bLedOn)
-						_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
-			}
-
-			pLed->bLedScanBlinkInProgress = false;
-		} else {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-				SwLedOff(padapter, pLed);
-			} else {
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
-			}
-		}
-		break;
-	case LED_BLINK_TXRX:
-		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-
-		if (bStopBlinking) {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-				pLed->CurrLedState = RTW_LED_OFF;
-				pLed->BlinkingLedState = RTW_LED_OFF;
-				if (pLed->bLedOn)
-					SwLedOff(padapter, pLed);
-			} else {
-				pLed->CurrLedState = RTW_LED_ON;
-				pLed->BlinkingLedState = RTW_LED_ON;
-				if (!pLed->bLedOn)
-					_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
-			}
-
-			pLed->bLedBlinkInProgress = false;
-		} else {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-				SwLedOff(padapter, pLed);
-			} else {
-				 if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
-			}
-		}
-		break;
-
-	default:
-		break;
-	}
-
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("SwLedBlink5 CurrLedState %d\n", pLed->CurrLedState));
-}
-
-static void SwLedBlink6(struct LED_871x *pLed)
-{
-	struct adapter *padapter = pLed->padapter;
-
-	/*  Change LED according to BlinkingLedState specified. */
-	if (pLed->BlinkingLedState == RTW_LED_ON) {
-		SwLedOn(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
-	} else {
-		SwLedOff(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
-	}
-
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("<==== blink6\n"));
-}
-
  /* ALPHA, added by chiyoko, 20090106 */
 static void SwLedControlMode1(struct adapter *padapter, enum LED_CTL_MODE LedAction)
 {
@@ -969,676 +443,18 @@ static void SwLedControlMode1(struct adapter *padapter, enum LED_CTL_MODE LedAct
 	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Led %d\n", pLed->CurrLedState));
 }
 
- /* Arcadyan/Sitecom , added by chiyoko, 20090216 */
-static void SwLedControlMode2(struct adapter *padapter, enum LED_CTL_MODE LedAction)
-{
-	struct led_priv *ledpriv = &(padapter->ledpriv);
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct LED_871x *pLed = &(ledpriv->SwLed0);
-
-	switch (LedAction) {
-	case LED_CTL_SITE_SURVEY:
-		if (pmlmepriv->LinkDetectInfo.bBusyTraffic) {
-		} else if (!pLed->bLedScanBlinkInProgress) {
-			if (IS_LED_WPS_BLINKING(pLed))
-				return;
-
-			if (pLed->bLedBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedBlinkInProgress = false;
-			}
-			pLed->bLedScanBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_SCAN;
-			pLed->BlinkTimes = 24;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
-		 }
-		break;
-	case LED_CTL_TX:
-	case LED_CTL_RX:
-		if ((!pLed->bLedBlinkInProgress) && (check_fwstate(pmlmepriv, _FW_LINKED))) {
-			if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
-				return;
-			pLed->bLedBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_TXRX;
-			pLed->BlinkTimes = 2;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
-		}
-		break;
-	case LED_CTL_LINK:
-		pLed->CurrLedState = RTW_LED_ON;
-		pLed->BlinkingLedState = RTW_LED_ON;
-		if (pLed->bLedBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedBlinkInProgress = false;
-		}
-		if (pLed->bLedScanBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedScanBlinkInProgress = false;
-		}
-		_set_timer(&(pLed->BlinkTimer), 0);
-		break;
-	case LED_CTL_START_WPS: /* wait until xinpin finish */
-	case LED_CTL_START_WPS_BOTTON:
-		if (!pLed->bLedWPSBlinkInProgress) {
-			if (pLed->bLedBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedBlinkInProgress = false;
-			}
-			if (pLed->bLedScanBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedScanBlinkInProgress = false;
-			}
-			pLed->bLedWPSBlinkInProgress = true;
-			pLed->CurrLedState = RTW_LED_ON;
-			pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), 0);
-		 }
-		break;
-	case LED_CTL_STOP_WPS:
-		pLed->bLedWPSBlinkInProgress = false;
-		if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-			SwLedOff(padapter, pLed);
-		} else {
-			pLed->CurrLedState = RTW_LED_ON;
-			pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), 0);
-			RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-		}
-		break;
-	case LED_CTL_STOP_WPS_FAIL:
-		pLed->bLedWPSBlinkInProgress = false;
-		if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-			SwLedOff(padapter, pLed);
-		} else {
-			pLed->CurrLedState = RTW_LED_OFF;
-			pLed->BlinkingLedState = RTW_LED_OFF;
-			_set_timer(&(pLed->BlinkTimer), 0);
-			RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-		}
-		break;
-	case LED_CTL_START_TO_LINK:
-	case LED_CTL_NO_LINK:
-		if (!IS_LED_BLINKING(pLed)) {
-			pLed->CurrLedState = RTW_LED_OFF;
-			pLed->BlinkingLedState = RTW_LED_OFF;
-			_set_timer(&(pLed->BlinkTimer), 0);
-		}
-		break;
-	case LED_CTL_POWER_OFF:
-		pLed->CurrLedState = RTW_LED_OFF;
-		pLed->BlinkingLedState = RTW_LED_OFF;
-		if (pLed->bLedBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedBlinkInProgress = false;
-		}
-		if (pLed->bLedScanBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedScanBlinkInProgress = false;
-		}
-		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedWPSBlinkInProgress = false;
-		}
-
-		_set_timer(&(pLed->BlinkTimer), 0);
-		break;
-	default:
-		break;
-	}
-
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-}
-
-  /* COREGA, added by chiyoko, 20090316 */
- static void SwLedControlMode3(struct adapter *padapter, enum LED_CTL_MODE LedAction)
-{
-	struct led_priv *ledpriv = &(padapter->ledpriv);
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct LED_871x *pLed = &(ledpriv->SwLed0);
-
-	switch (LedAction) {
-	case LED_CTL_SITE_SURVEY:
-		if (pmlmepriv->LinkDetectInfo.bBusyTraffic) {
-		} else if (!pLed->bLedScanBlinkInProgress) {
-			if (IS_LED_WPS_BLINKING(pLed))
-				return;
-
-			if (pLed->bLedBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedBlinkInProgress = false;
-			}
-			pLed->bLedScanBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_SCAN;
-			pLed->BlinkTimes = 24;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
-		}
-		break;
-	case LED_CTL_TX:
-	case LED_CTL_RX:
-		if ((!pLed->bLedBlinkInProgress) && (check_fwstate(pmlmepriv, _FW_LINKED))) {
-			if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
-				return;
-			pLed->bLedBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_TXRX;
-			pLed->BlinkTimes = 2;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
-		}
-		break;
-	case LED_CTL_LINK:
-		if (IS_LED_WPS_BLINKING(pLed))
-			return;
-		pLed->CurrLedState = RTW_LED_ON;
-		pLed->BlinkingLedState = RTW_LED_ON;
-		if (pLed->bLedBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedBlinkInProgress = false;
-		}
-		if (pLed->bLedScanBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedScanBlinkInProgress = false;
-		}
-
-		_set_timer(&(pLed->BlinkTimer), 0);
-		break;
-	case LED_CTL_START_WPS: /* wait until xinpin finish */
-	case LED_CTL_START_WPS_BOTTON:
-		if (!pLed->bLedWPSBlinkInProgress) {
-			if (pLed->bLedBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedBlinkInProgress = false;
-			}
-			if (pLed->bLedScanBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedScanBlinkInProgress = false;
-			}
-			pLed->bLedWPSBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_WPS;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
-		}
-		break;
-	case LED_CTL_STOP_WPS:
-		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedWPSBlinkInProgress = false;
-		} else {
-			pLed->bLedWPSBlinkInProgress = true;
-		}
-
-		pLed->CurrLedState = LED_BLINK_WPS_STOP;
-		if (pLed->bLedOn) {
-			pLed->BlinkingLedState = RTW_LED_OFF;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA);
-		} else {
-			pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), 0);
-		}
-		break;
-	case LED_CTL_STOP_WPS_FAIL:
-		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedWPSBlinkInProgress = false;
-		}
-		pLed->CurrLedState = RTW_LED_OFF;
-		pLed->BlinkingLedState = RTW_LED_OFF;
-		_set_timer(&(pLed->BlinkTimer), 0);
-		break;
-	case LED_CTL_START_TO_LINK:
-	case LED_CTL_NO_LINK:
-		if (!IS_LED_BLINKING(pLed)) {
-			pLed->CurrLedState = RTW_LED_OFF;
-			pLed->BlinkingLedState = RTW_LED_OFF;
-			_set_timer(&(pLed->BlinkTimer), 0);
-		}
-		break;
-	case LED_CTL_POWER_OFF:
-		pLed->CurrLedState = RTW_LED_OFF;
-		pLed->BlinkingLedState = RTW_LED_OFF;
-		if (pLed->bLedBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedBlinkInProgress = false;
-		}
-		if (pLed->bLedScanBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedScanBlinkInProgress = false;
-		}
-		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedWPSBlinkInProgress = false;
-		}
-
-		_set_timer(&(pLed->BlinkTimer), 0);
-		break;
-	default:
-		break;
-	}
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_,
-		 ("CurrLedState %d\n", pLed->CurrLedState));
-}
-
- /* Edimax-Belkin, added by chiyoko, 20090413 */
-static void SwLedControlMode4(struct adapter *padapter, enum LED_CTL_MODE LedAction)
-{
-	struct led_priv *ledpriv = &(padapter->ledpriv);
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct LED_871x *pLed = &(ledpriv->SwLed0);
-	struct LED_871x *pLed1 = &(ledpriv->SwLed1);
-
-	switch (LedAction) {
-	case LED_CTL_START_TO_LINK:
-		if (pLed1->bLedWPSBlinkInProgress) {
-			pLed1->bLedWPSBlinkInProgress = false;
-			_cancel_timer_ex(&(pLed1->BlinkTimer));
-
-			pLed1->BlinkingLedState = RTW_LED_OFF;
-			pLed1->CurrLedState = RTW_LED_OFF;
-
-			if (pLed1->bLedOn)
-				_set_timer(&(pLed->BlinkTimer), 0);
-		}
-
-		if (!pLed->bLedStartToLinkBlinkInProgress) {
-			if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
-				return;
-			if (pLed->bLedBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedBlinkInProgress = false;
-			}
-			if (pLed->bLedNoLinkBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedNoLinkBlinkInProgress = false;
-			}
-
-			pLed->bLedStartToLinkBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_StartToBlink;
-			if (pLed->bLedOn) {
-				pLed->BlinkingLedState = RTW_LED_OFF;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);
-			} else {
-				pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
-			}
-		}
-		break;
-	case LED_CTL_LINK:
-	case LED_CTL_NO_LINK:
-		/* LED1 settings */
-		if (LedAction == LED_CTL_LINK) {
-			if (pLed1->bLedWPSBlinkInProgress) {
-				pLed1->bLedWPSBlinkInProgress = false;
-				_cancel_timer_ex(&(pLed1->BlinkTimer));
-
-				pLed1->BlinkingLedState = RTW_LED_OFF;
-				pLed1->CurrLedState = RTW_LED_OFF;
-
-				if (pLed1->bLedOn)
-					_set_timer(&(pLed->BlinkTimer), 0);
-			}
-		}
-
-		if (!pLed->bLedNoLinkBlinkInProgress) {
-			if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
-				return;
-			if (pLed->bLedBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedBlinkInProgress = false;
-			}
-
-			pLed->bLedNoLinkBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_SLOWLY;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
-		}
-		break;
-	case LED_CTL_SITE_SURVEY:
-		if ((pmlmepriv->LinkDetectInfo.bBusyTraffic) && (check_fwstate(pmlmepriv, _FW_LINKED))) {
-		} else if (!pLed->bLedScanBlinkInProgress) {
-			if (IS_LED_WPS_BLINKING(pLed))
-				return;
-
-			if (pLed->bLedNoLinkBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedNoLinkBlinkInProgress = false;
-			}
-			if (pLed->bLedBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedBlinkInProgress = false;
-			}
-			pLed->bLedScanBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_SCAN;
-			pLed->BlinkTimes = 24;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
-		}
-		break;
-	case LED_CTL_TX:
-	case LED_CTL_RX:
-		if (!pLed->bLedBlinkInProgress) {
-			if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
-				return;
-			if (pLed->bLedNoLinkBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedNoLinkBlinkInProgress = false;
-			}
-			pLed->bLedBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_TXRX;
-			pLed->BlinkTimes = 2;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
-		}
-		break;
-	case LED_CTL_START_WPS: /* wait until xinpin finish */
-	case LED_CTL_START_WPS_BOTTON:
-		if (pLed1->bLedWPSBlinkInProgress) {
-			pLed1->bLedWPSBlinkInProgress = false;
-			_cancel_timer_ex(&(pLed1->BlinkTimer));
-
-			pLed1->BlinkingLedState = RTW_LED_OFF;
-			pLed1->CurrLedState = RTW_LED_OFF;
-
-			if (pLed1->bLedOn)
-				_set_timer(&(pLed->BlinkTimer), 0);
-		}
-
-		if (!pLed->bLedWPSBlinkInProgress) {
-			if (pLed->bLedNoLinkBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedNoLinkBlinkInProgress = false;
-			}
-			if (pLed->bLedBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedBlinkInProgress = false;
-			}
-			if (pLed->bLedScanBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedScanBlinkInProgress = false;
-			}
-			pLed->bLedWPSBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_WPS;
-			if (pLed->bLedOn) {
-				pLed->BlinkingLedState = RTW_LED_OFF;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);
-			} else {
-				pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
-			}
-		}
-		break;
-	case LED_CTL_STOP_WPS:	/* WPS connect success */
-		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedWPSBlinkInProgress = false;
-		}
-
-		pLed->bLedNoLinkBlinkInProgress = true;
-		pLed->CurrLedState = LED_BLINK_SLOWLY;
-		if (pLed->bLedOn)
-			pLed->BlinkingLedState = RTW_LED_OFF;
-		else
-			pLed->BlinkingLedState = RTW_LED_ON;
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
-
-		break;
-	case LED_CTL_STOP_WPS_FAIL:		/* WPS authentication fail */
-		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedWPSBlinkInProgress = false;
-		}
-		pLed->bLedNoLinkBlinkInProgress = true;
-		pLed->CurrLedState = LED_BLINK_SLOWLY;
-		if (pLed->bLedOn)
-			pLed->BlinkingLedState = RTW_LED_OFF;
-		else
-			pLed->BlinkingLedState = RTW_LED_ON;
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
-
-		/* LED1 settings */
-		if (pLed1->bLedWPSBlinkInProgress)
-			_cancel_timer_ex(&(pLed1->BlinkTimer));
-		else
-			pLed1->bLedWPSBlinkInProgress = true;
-		pLed1->CurrLedState = LED_BLINK_WPS_STOP;
-		if (pLed1->bLedOn)
-			pLed1->BlinkingLedState = RTW_LED_OFF;
-		else
-			pLed1->BlinkingLedState = RTW_LED_ON;
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
-		break;
-	case LED_CTL_STOP_WPS_FAIL_OVERLAP:	/* WPS session overlap */
-		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedWPSBlinkInProgress = false;
-		}
-		pLed->bLedNoLinkBlinkInProgress = true;
-		pLed->CurrLedState = LED_BLINK_SLOWLY;
-		if (pLed->bLedOn)
-			pLed->BlinkingLedState = RTW_LED_OFF;
-		else
-			pLed->BlinkingLedState = RTW_LED_ON;
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
-
-		/* LED1 settings */
-		if (pLed1->bLedWPSBlinkInProgress)
-			_cancel_timer_ex(&(pLed1->BlinkTimer));
-		else
-			pLed1->bLedWPSBlinkInProgress = true;
-		pLed1->CurrLedState = LED_BLINK_WPS_STOP_OVERLAP;
-		pLed1->BlinkTimes = 10;
-		if (pLed1->bLedOn)
-			pLed1->BlinkingLedState = RTW_LED_OFF;
-		else
-			pLed1->BlinkingLedState = RTW_LED_ON;
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
-		break;
-	case LED_CTL_POWER_OFF:
-		pLed->CurrLedState = RTW_LED_OFF;
-		pLed->BlinkingLedState = RTW_LED_OFF;
-
-		if (pLed->bLedNoLinkBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedNoLinkBlinkInProgress = false;
-		}
-		if (pLed->bLedLinkBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedLinkBlinkInProgress = false;
-		}
-		if (pLed->bLedBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedBlinkInProgress = false;
-		}
-		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedWPSBlinkInProgress = false;
-		}
-		if (pLed->bLedScanBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedScanBlinkInProgress = false;
-		}
-		if (pLed->bLedStartToLinkBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedStartToLinkBlinkInProgress = false;
-		}
-		if (pLed1->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed1->BlinkTimer));
-			pLed1->bLedWPSBlinkInProgress = false;
-		}
-		pLed1->BlinkingLedState = LED_UNKNOWN;
-		SwLedOff(padapter, pLed);
-		SwLedOff(padapter, pLed1);
-		break;
-	default:
-		break;
-	}
-
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Led %d\n", pLed->CurrLedState));
-}
-
-
-
- /* Sercomm-Belkin, added by chiyoko, 20090415 */
-static void
-SwLedControlMode5(
-	struct adapter *padapter,
-	enum LED_CTL_MODE LedAction
-)
-{
-	struct led_priv *ledpriv = &(padapter->ledpriv);
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct LED_871x *pLed = &(ledpriv->SwLed0);
-
-	switch (LedAction) {
-	case LED_CTL_POWER_ON:
-	case LED_CTL_NO_LINK:
-	case LED_CTL_LINK:	/* solid blue */
-		pLed->CurrLedState = RTW_LED_ON;
-		pLed->BlinkingLedState = RTW_LED_ON;
-
-		_set_timer(&(pLed->BlinkTimer), 0);
-		break;
-	case LED_CTL_SITE_SURVEY:
-		if ((pmlmepriv->LinkDetectInfo.bBusyTraffic) && (check_fwstate(pmlmepriv, _FW_LINKED))) {
-		} else if (!pLed->bLedScanBlinkInProgress) {
-			if (pLed->bLedBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedBlinkInProgress = false;
-			}
-			pLed->bLedScanBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_SCAN;
-			pLed->BlinkTimes = 24;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
-		}
-		break;
-	case LED_CTL_TX:
-	case LED_CTL_RX:
-		if (!pLed->bLedBlinkInProgress) {
-			if (pLed->CurrLedState == LED_BLINK_SCAN)
-				return;
-			pLed->bLedBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_TXRX;
-			pLed->BlinkTimes = 2;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
-		}
-		break;
-	case LED_CTL_POWER_OFF:
-		pLed->CurrLedState = RTW_LED_OFF;
-		pLed->BlinkingLedState = RTW_LED_OFF;
-
-		if (pLed->bLedBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedBlinkInProgress = false;
-		}
-		SwLedOff(padapter, pLed);
-		break;
-	default:
-		break;
-	}
-
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Led %d\n", pLed->CurrLedState));
-}
-
- /* WNC-Corega, added by chiyoko, 20090902 */
-static void
-SwLedControlMode6(
-	struct adapter *padapter,
-	enum LED_CTL_MODE LedAction
-)
-{
-	struct led_priv *ledpriv = &(padapter->ledpriv);
-	struct LED_871x *pLed0 = &(ledpriv->SwLed0);
-
-	switch (LedAction) {
-	case LED_CTL_POWER_ON:
-	case LED_CTL_LINK:
-	case LED_CTL_NO_LINK:
-		_cancel_timer_ex(&(pLed0->BlinkTimer));
-		pLed0->CurrLedState = RTW_LED_ON;
-		pLed0->BlinkingLedState = RTW_LED_ON;
-		_set_timer(&(pLed0->BlinkTimer), 0);
-		break;
-	case LED_CTL_POWER_OFF:
-		SwLedOff(padapter, pLed0);
-		break;
-	default:
-		break;
-	}
-
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("ledcontrol 6 Led %d\n", pLed0->CurrLedState));
-}
-
 /*  */
 /*	Description: */
 /*		Handler function of LED Blinking. */
-/*		We dispatch acture LED blink action according to LedStrategy. */
 /*  */
 void BlinkHandler(struct LED_871x *pLed)
 {
 	struct adapter *padapter = pLed->padapter;
-	struct led_priv *ledpriv = &(padapter->ledpriv);
 
 	if ((padapter->bSurpriseRemoved) || (padapter->bDriverStopped))
 		return;
 
-	switch (ledpriv->LedStrategy) {
-	case SW_LED_MODE0:
-		SwLedBlink(pLed);
-		break;
-	case SW_LED_MODE1:
-		SwLedBlink1(pLed);
-		break;
-	case SW_LED_MODE2:
-		SwLedBlink2(pLed);
-		break;
-	case SW_LED_MODE3:
-		SwLedBlink3(pLed);
-		break;
-	case SW_LED_MODE4:
-		SwLedBlink4(pLed);
-		break;
-	case SW_LED_MODE5:
-		SwLedBlink5(pLed);
-		break;
-	case SW_LED_MODE6:
-		SwLedBlink6(pLed);
-		break;
-	default:
-		break;
-	}
+	SwLedBlink1(pLed);
 }
 
 void LedControl8188eu(struct adapter *padapter, enum LED_CTL_MODE LedAction)
@@ -1661,32 +477,5 @@ void LedControl8188eu(struct adapter *padapter, enum LED_CTL_MODE LedAction)
 	     LedAction == LED_CTL_POWER_ON))
 		return;
 
-	switch (ledpriv->LedStrategy) {
-	case SW_LED_MODE0:
-		break;
-	case SW_LED_MODE1:
-		SwLedControlMode1(padapter, LedAction);
-		break;
-	case SW_LED_MODE2:
-		SwLedControlMode2(padapter, LedAction);
-		break;
-	case SW_LED_MODE3:
-		SwLedControlMode3(padapter, LedAction);
-		break;
-	case SW_LED_MODE4:
-		SwLedControlMode4(padapter, LedAction);
-		break;
-	case SW_LED_MODE5:
-		SwLedControlMode5(padapter, LedAction);
-		break;
-	case SW_LED_MODE6:
-		SwLedControlMode6(padapter, LedAction);
-		break;
-	default:
-		break;
-	}
-
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_,
-		 ("LedStrategy:%d, LedAction %d\n",
-		 ledpriv->LedStrategy, LedAction));
+	SwLedControlMode1(padapter, LedAction);
 }
diff --git a/drivers/staging/rtl8188eu/core/rtw_mlme.c b/drivers/staging/rtl8188eu/core/rtw_mlme.c
index 769d4ddc675..155282ef78f 100644
--- a/drivers/staging/rtl8188eu/core/rtw_mlme.c
+++ b/drivers/staging/rtl8188eu/core/rtw_mlme.c
@@ -1727,15 +1727,13 @@ int rtw_set_key(struct adapter *adapter, struct security_priv *psecuritypriv, in
 	int	res = _SUCCESS;
 
 	pcmd = (struct	cmd_obj *)rtw_zmalloc(sizeof(struct	cmd_obj));
-	if (pcmd == NULL) {
-		res = _FAIL;  /* try again */
-		goto exit;
-	}
+	if (pcmd == NULL)
+		return _FAIL;  /* try again */
+
 	psetkeyparm = (struct setkey_parm *)rtw_zmalloc(sizeof(struct setkey_parm));
 	if (psetkeyparm == NULL) {
-		kfree(pcmd);
 		res = _FAIL;
-		goto exit;
+		goto err_free_cmd;
 	}
 
 	_rtw_memset(psetkeyparm, 0, sizeof(struct setkey_parm));
@@ -1784,7 +1782,7 @@ int rtw_set_key(struct adapter *adapter, struct security_priv *psecuritypriv, in
 			 ("\n rtw_set_key:psecuritypriv->dot11PrivacyAlgrthm=%x (must be 1 or 2 or 4 or 5)\n",
 			 psecuritypriv->dot11PrivacyAlgrthm));
 		res = _FAIL;
-		goto exit;
+		goto err_free_parm;
 	}
 	pcmd->cmdcode = _SetKey_CMD_;
 	pcmd->parmbuf = (u8 *)psetkeyparm;
@@ -1793,7 +1791,12 @@ int rtw_set_key(struct adapter *adapter, struct security_priv *psecuritypriv, in
 	pcmd->rspsz = 0;
 	_rtw_init_listhead(&pcmd->list);
 	res = rtw_enqueue_cmd(pcmdpriv, pcmd);
-exit:
+	return res;
+
+err_free_parm:
+	kfree(psetkeyparm);
+err_free_cmd:
+	kfree(pcmd);
 	return res;
 }
 
diff --git a/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c b/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
index 3ed5941bedc..f5b49f351a6 100644
--- a/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
+++ b/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
@@ -502,8 +502,6 @@ void mgt_dispatcher(struct adapter *padapter, struct recv_frame *precv_frame)
 		break;
 	default:
 		_mgt_dispatcher(padapter, ptable, precv_frame);
-		if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
-			rtw_hostapd_mlme_rx(padapter, precv_frame);
 		break;
 	}
 #else
@@ -6553,7 +6551,6 @@ u8 collect_bss_info(struct adapter *padapter, struct recv_frame *precv_frame, st
 	struct registry_priv	*pregistrypriv = &padapter->registrypriv;
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
-	__le32 le32_tmp;
 
 	len = packet_len - sizeof(struct rtw_ieee80211_hdr_3addr);
 
@@ -6600,13 +6597,13 @@ u8 collect_bss_info(struct adapter *padapter, struct recv_frame *precv_frame, st
 		return _FAIL;
 	}
 
-	if (*(p + 1)) {
+	if (len) {
 		if (len > NDIS_802_11_LENGTH_SSID) {
 			DBG_88E("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len);
 			return _FAIL;
 		}
-		memcpy(bssid->Ssid.Ssid, (p + 2), *(p + 1));
-		bssid->Ssid.SsidLength = *(p + 1);
+		memcpy(bssid->Ssid.Ssid, (p + 2), len);
+		bssid->Ssid.SsidLength = len;
 	} else {
 		bssid->Ssid.SsidLength = 0;
 	}
@@ -6667,8 +6664,8 @@ u8 collect_bss_info(struct adapter *padapter, struct recv_frame *precv_frame, st
 		return _SUCCESS;
 	}
 
-	memcpy(&le32_tmp, rtw_get_beacon_interval_from_ie(bssid->IEs), 2);
-	bssid->Configuration.BeaconPeriod = le32_to_cpu(le32_tmp);
+	bssid->Configuration.BeaconPeriod =
+		get_unaligned_le16(rtw_get_beacon_interval_from_ie(bssid->IEs));
 
 	val16 = rtw_get_capability((struct wlan_bssid_ex *)bssid);
 
diff --git a/drivers/staging/rtl8188eu/core/rtw_mp.c b/drivers/staging/rtl8188eu/core/rtw_mp.c
index 705f666bca6..17427a68b66 100644
--- a/drivers/staging/rtl8188eu/core/rtw_mp.c
+++ b/drivers/staging/rtl8188eu/core/rtw_mp.c
@@ -21,7 +21,6 @@
 
 #include <drv_types.h>
 
-#include "odm_precomp.h"
 #include "rtl8188e_hal.h"
 #include <linux/vmalloc.h>
 
diff --git a/drivers/staging/rtl8188eu/core/rtw_p2p.c b/drivers/staging/rtl8188eu/core/rtw_p2p.c
index 9425c4991cc..0a15f8cf0d3 100644
--- a/drivers/staging/rtl8188eu/core/rtw_p2p.c
+++ b/drivers/staging/rtl8188eu/core/rtw_p2p.c
@@ -1505,9 +1505,9 @@ static void find_phase_handler(struct adapter *padapter)
 
 	rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH);
 
-        spin_lock_bh(&pmlmepriv->lock);
-        rtw_sitesurvey_cmd(padapter, &ssid, 1, NULL, 0);
-        spin_unlock_bh(&pmlmepriv->lock);
+	spin_lock_bh(&pmlmepriv->lock);
+	rtw_sitesurvey_cmd(padapter, &ssid, 1, NULL, 0);
+	spin_unlock_bh(&pmlmepriv->lock);
 }
 
 void p2p_concurrent_handler(struct adapter *padapter);
diff --git a/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c b/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c
index f6583734aef..739e2504169 100644
--- a/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c
+++ b/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c
@@ -122,7 +122,7 @@ static bool rtw_pwr_unassociated_idle(struct adapter *adapter)
 
 	bool ret = false;
 
-	if (adapter->pwrctrlpriv.ips_deny_time >= jiffies)
+	if (time_after_eq(adapter->pwrctrlpriv.ips_deny_time, jiffies))
 		goto exit;
 
 	if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE|WIFI_SITE_MONITOR) ||
@@ -497,15 +497,6 @@ void rtw_init_pwrctrl_priv(struct adapter *padapter)
 	_init_timer(&(pwrctrlpriv->pwr_state_check_timer), padapter->pnetdev, pwr_state_check_handler, (u8 *)padapter);
 }
 
-u8 rtw_interface_ps_func(struct adapter *padapter, enum hal_intf_ps_func efunc_id, u8 *val)
-{
-	u8 bResult = true;
-	rtw_hal_intf_ps_func(padapter, efunc_id, val);
-
-	return bResult;
-}
-
-
 inline void rtw_set_ips_deny(struct adapter *padapter, u32 ms)
 {
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
@@ -523,9 +514,11 @@ int _rtw_pwr_wakeup(struct adapter *padapter, u32 ips_deffer_ms, const char *cal
 {
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	unsigned long expires;
 	int ret = _SUCCESS;
 
-	if (pwrpriv->ips_deny_time < jiffies + rtw_ms_to_systime(ips_deffer_ms))
+	expires = jiffies + rtw_ms_to_systime(ips_deffer_ms);
+	if (time_before(pwrpriv->ips_deny_time, expires))
 		pwrpriv->ips_deny_time = jiffies + rtw_ms_to_systime(ips_deffer_ms);
 
 {
@@ -580,7 +573,8 @@ int _rtw_pwr_wakeup(struct adapter *padapter, u32 ips_deffer_ms, const char *cal
 	}
 
 exit:
-	if (pwrpriv->ips_deny_time < jiffies + rtw_ms_to_systime(ips_deffer_ms))
+	expires = jiffies + rtw_ms_to_systime(ips_deffer_ms);
+	if (time_before(pwrpriv->ips_deny_time, expires))
 		pwrpriv->ips_deny_time = jiffies + rtw_ms_to_systime(ips_deffer_ms);
 	return ret;
 }
diff --git a/drivers/staging/rtl8188eu/core/rtw_recv.c b/drivers/staging/rtl8188eu/core/rtw_recv.c
index e305d43ebd0..0e73df5975b 100644
--- a/drivers/staging/rtl8188eu/core/rtw_recv.c
+++ b/drivers/staging/rtl8188eu/core/rtw_recv.c
@@ -63,8 +63,6 @@ int _rtw_init_recv_priv(struct recv_priv *precvpriv, struct adapter *padapter)
 
 	int	res = _SUCCESS;
 
-	spin_lock_init(&precvpriv->lock);
-
 	_rtw_init_queue(&precvpriv->free_recv_queue);
 	_rtw_init_queue(&precvpriv->recv_pending_queue);
 	_rtw_init_queue(&precvpriv->uc_swdec_pending_queue);
@@ -73,8 +71,6 @@ int _rtw_init_recv_priv(struct recv_priv *precvpriv, struct adapter *padapter)
 
 	precvpriv->free_recvframe_cnt = NR_RECVFRAME;
 
-	rtw_os_recv_resource_init(precvpriv, padapter);
-
 	precvpriv->pallocated_frame_buf = vzalloc(NR_RECVFRAME * sizeof(struct recv_frame) + RXFRAME_ALIGN_SZ);
 
 	if (precvpriv->pallocated_frame_buf == NULL) {
@@ -101,8 +97,6 @@ int _rtw_init_recv_priv(struct recv_priv *precvpriv, struct adapter *padapter)
 	}
 	precvpriv->rx_pending_cnt = 1;
 
-	sema_init(&precvpriv->allrxreturnevt, 0);
-
 	res = rtw_hal_init_recv_priv(padapter);
 
 	_init_timer(&precvpriv->signal_stat_timer, padapter->pnetdev, RTW_TIMER_HDL_NAME(signal_stat), padapter);
@@ -123,8 +117,6 @@ void _rtw_free_recv_priv (struct recv_priv *precvpriv)
 
 	rtw_free_uc_swdec_pending_queue(padapter);
 
-	rtw_os_recv_resource_free(precvpriv);
-
 	if (precvpriv->pallocated_frame_buf) {
 		vfree(precvpriv->pallocated_frame_buf);
 	}
@@ -289,55 +281,6 @@ u32 rtw_free_uc_swdec_pending_queue(struct adapter *adapter)
 	return cnt;
 }
 
-int rtw_enqueue_recvbuf_to_head(struct recv_buf *precvbuf, struct __queue *queue)
-{
-	spin_lock_bh(&queue->lock);
-
-	rtw_list_delete(&precvbuf->list);
-	rtw_list_insert_head(&precvbuf->list, get_list_head(queue));
-
-	spin_unlock_bh(&queue->lock);
-
-	return _SUCCESS;
-}
-
-int rtw_enqueue_recvbuf(struct recv_buf *precvbuf, struct __queue *queue)
-{
-	unsigned long irqL;
-	spin_lock_irqsave(&queue->lock, irqL);
-
-	rtw_list_delete(&precvbuf->list);
-
-	rtw_list_insert_tail(&precvbuf->list, get_list_head(queue));
-	spin_unlock_irqrestore(&queue->lock, irqL);
-	return _SUCCESS;
-}
-
-struct recv_buf *rtw_dequeue_recvbuf (struct __queue *queue)
-{
-	unsigned long irqL;
-	struct recv_buf *precvbuf;
-	struct list_head *plist, *phead;
-
-	spin_lock_irqsave(&queue->lock, irqL);
-
-	if (_rtw_queue_empty(queue)) {
-		precvbuf = NULL;
-	} else {
-		phead = get_list_head(queue);
-
-		plist = phead->next;
-
-		precvbuf = container_of(plist, struct recv_buf, list);
-
-		rtw_list_delete(&precvbuf->list);
-	}
-
-	spin_unlock_irqrestore(&queue->lock, irqL);
-
-	return precvbuf;
-}
-
 static int recvframe_chkmic(struct adapter *adapter,
 			    struct recv_frame *precvframe)
 {
@@ -554,6 +497,7 @@ static struct recv_frame *portctrl(struct adapter *adapter,
 	u16	ether_type;
 	u16  eapol_type = 0x888e;/* for Funia BD's WPA issue */
 	struct rx_pkt_attrib *pattrib;
+	__be16 be_tmp;
 
 
 	pstapriv = &adapter->stapriv;
@@ -573,8 +517,8 @@ static struct recv_frame *portctrl(struct adapter *adapter,
 	if (auth_alg == 2) {
 		/* get ether_type */
 		ptr = ptr + pfhdr->attrib.hdrlen + LLC_HEADER_SIZE;
-		memcpy(&ether_type, ptr, 2);
-		ether_type = ntohs((unsigned short)ether_type);
+		memcpy(&be_tmp, ptr, 2);
+		ether_type = ntohs(be_tmp);
 
 		if ((psta != NULL) && (psta->ieee8021x_blocked)) {
 			/* blocked */
@@ -647,8 +591,8 @@ static int recv_decache(struct recv_frame *precv_frame, u8 bretry,
 	return _SUCCESS;
 }
 
-void process_pwrbit_data(struct adapter *padapter,
-			 struct recv_frame *precv_frame)
+static void process_pwrbit_data(struct adapter *padapter,
+				struct recv_frame *precv_frame)
 {
 #ifdef CONFIG_88EU_AP_MODE
 	unsigned char pwrbit;
@@ -1822,8 +1766,8 @@ static int check_indicate_seq(struct recv_reorder_ctrl *preorder_ctrl, u16 seq_n
 	return true;
 }
 
-int enqueue_reorder_recvframe(struct recv_reorder_ctrl *preorder_ctrl,
-			      struct recv_frame *prframe)
+static int enqueue_reorder_recvframe(struct recv_reorder_ctrl *preorder_ctrl,
+				     struct recv_frame *prframe)
 {
 	struct rx_pkt_attrib *pattrib = &prframe->attrib;
 	struct __queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
@@ -1871,7 +1815,7 @@ static int recv_indicatepkts_in_order(struct adapter *padapter, struct recv_reor
 			return true;
 
 		prhdr = container_of(plist, struct recv_frame, list);
-	        pattrib = &prhdr->attrib;
+		pattrib = &prhdr->attrib;
 		preorder_ctrl->indicate_seq = pattrib->seq_num;
 	}
 
diff --git a/drivers/staging/rtl8188eu/core/rtw_security.c b/drivers/staging/rtl8188eu/core/rtw_security.c
index c4b16ea6348..05335959b54 100644
--- a/drivers/staging/rtl8188eu/core/rtw_security.c
+++ b/drivers/staging/rtl8188eu/core/rtw_security.c
@@ -1153,7 +1153,7 @@ static int aes_cipher(u8 *key, uint hdrlen, u8 *pframe, uint plen)
 
 	/* Insert MIC into payload */
 	for (j = 0; j < 8; j++)
-	pframe[payload_index+j] = mic[j];	/* message[payload_index+j] = mic[j]; */
+		pframe[payload_index+j] = mic[j];
 
 	payload_index = hdrlen + 8;
 	for (i = 0; i < num_blocks; i++) {
diff --git a/drivers/staging/rtl8188eu/core/rtw_wlan_util.c b/drivers/staging/rtl8188eu/core/rtw_wlan_util.c
index 3dd90599fd4..6fb8caa94ab 100644
--- a/drivers/staging/rtl8188eu/core/rtw_wlan_util.c
+++ b/drivers/staging/rtl8188eu/core/rtw_wlan_util.c
@@ -1599,13 +1599,18 @@ int update_sta_support_rate(struct adapter *padapter, u8 *pvar_ie, uint var_ie_l
 	pIE = (struct ndis_802_11_var_ie *)rtw_get_ie(pvar_ie, _SUPPORTEDRATES_IE_, &ie_len, var_ie_len);
 	if (pIE == NULL)
 		return _FAIL;
+	if (ie_len > NDIS_802_11_LENGTH_RATES_EX)
+		return _FAIL;
 
 	memcpy(pmlmeinfo->FW_sta_info[cam_idx].SupportedRates, pIE->data, ie_len);
 	supportRateNum = ie_len;
 
 	pIE = (struct ndis_802_11_var_ie *)rtw_get_ie(pvar_ie, _EXT_SUPPORTEDRATES_IE_, &ie_len, var_ie_len);
-	if (pIE)
+	if (pIE) {
+		if (supportRateNum + ie_len > NDIS_802_11_LENGTH_RATES_EX)
+			return _FAIL;
 		memcpy((pmlmeinfo->FW_sta_info[cam_idx].SupportedRates + supportRateNum), pIE->data, ie_len);
+	}
 
 	return _SUCCESS;
 }
@@ -1656,26 +1661,3 @@ void beacon_timing_control(struct adapter *padapter)
 {
 	rtw_hal_bcn_related_reg_setting(padapter);
 }
-
-static struct adapter *pbuddy_padapter;
-
-int rtw_handle_dualmac(struct adapter *adapter, bool init)
-{
-	int status = _SUCCESS;
-
-	if (init) {
-		if (pbuddy_padapter == NULL) {
-			pbuddy_padapter = adapter;
-			DBG_88E("%s(): pbuddy_padapter == NULL, Set pbuddy_padapter\n", __func__);
-		} else {
-			adapter->pbuddy_adapter = pbuddy_padapter;
-			pbuddy_padapter->pbuddy_adapter = adapter;
-			/*  clear global value */
-			pbuddy_padapter = NULL;
-			DBG_88E("%s(): pbuddy_padapter exist, Exchange Information\n", __func__);
-		}
-	} else {
-		pbuddy_padapter = NULL;
-	}
-	return status;
-}
diff --git a/drivers/staging/rtl8188eu/core/rtw_xmit.c b/drivers/staging/rtl8188eu/core/rtw_xmit.c
index 8d4265fb486..1413ec8ad3b 100644
--- a/drivers/staging/rtl8188eu/core/rtw_xmit.c
+++ b/drivers/staging/rtl8188eu/core/rtw_xmit.c
@@ -621,8 +621,6 @@ static s32 update_attrib(struct adapter *padapter, struct sk_buff *pkt, struct p
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("update_attrib: bswenc = false\n"));
 	}
 
-	rtw_set_tx_chksum_offload(pkt, pattrib);
-
 	update_attrib_phy_info(pattrib, psta);
 
 exit:
diff --git a/drivers/staging/rtl8188eu/hal/HalPhyRf_8188e.c b/drivers/staging/rtl8188eu/hal/HalPhyRf_8188e.c
index 056052ddd82..7c22658ed0f 100644
--- a/drivers/staging/rtl8188eu/hal/HalPhyRf_8188e.c
+++ b/drivers/staging/rtl8188eu/hal/HalPhyRf_8188e.c
@@ -804,24 +804,12 @@ static void pathb_fill_iqk(struct adapter *adapt, bool iqkok, s32 result[][8], u
 	}
 }
 
-/*  */
-/*  2011/07/26 MH Add an API for testing IQK fail case. */
-/*  */
-/*  MP Already declare in odm.c */
-static bool ODM_CheckPowerStatus(struct adapter *Adapter)
-{
-	return	true;
-}
-
 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;
 
-	if (!ODM_CheckPowerStatus(adapt))
-		return;
-
 	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);
@@ -1294,8 +1282,6 @@ void PHY_IQCalibrate_8188E(struct adapter *adapt, bool recovery)
 	bool is2t;
 
 	is2t = (dm_odm->RFType == ODM_2T2R) ? true : false;
-	if (!ODM_CheckPowerStatus(adapt))
-		return;
 
 	if (!(dm_odm->SupportAbility & ODM_RF_CALIBRATION))
 		return;
diff --git a/drivers/staging/rtl8188eu/hal/hal_intf.c b/drivers/staging/rtl8188eu/hal/hal_intf.c
index d75ca7a27c8..d0ac4a18333 100644
--- a/drivers/staging/rtl8188eu/hal/hal_intf.c
+++ b/drivers/staging/rtl8188eu/hal/hal_intf.c
@@ -60,13 +60,6 @@ void rtw_hal_dm_init(struct adapter *adapt)
 		adapt->HalFunc.dm_init(adapt);
 }
 
-void rtw_hal_dm_deinit(struct adapter *adapt)
-{
-	/*  cancel dm  timer */
-	if (adapt->HalFunc.dm_deinit)
-		adapt->HalFunc.dm_deinit(adapt);
-}
-
 void rtw_hal_sw_led_init(struct adapter *adapt)
 {
 	if (adapt->HalFunc.InitSwLeds)
@@ -207,15 +200,6 @@ u32 rtw_hal_inirp_deinit(struct adapter *adapt)
 	return _FAIL;
 }
 
-u8 rtw_hal_intf_ps_func(struct adapter *adapt,
-			enum hal_intf_ps_func efunc_id, u8 *val)
-{
-	if (adapt->HalFunc.interface_ps_func)
-		return adapt->HalFunc.interface_ps_func(adapt, efunc_id,
-							   val);
-	return _FAIL;
-}
-
 s32 rtw_hal_xmit(struct adapter *adapt, struct xmit_frame *pxmitframe)
 {
 	if (adapt->HalFunc.hal_xmit)
diff --git a/drivers/staging/rtl8188eu/hal/odm.c b/drivers/staging/rtl8188eu/hal/odm.c
index 89a26e3e217..2a0ac4ab23d 100644
--- a/drivers/staging/rtl8188eu/hal/odm.c
+++ b/drivers/staging/rtl8188eu/hal/odm.c
@@ -183,7 +183,6 @@ void ODM_DMInit(struct odm_dm_struct *pDM_Odm)
 	odm_RateAdaptiveMaskInit(pDM_Odm);
 
 	odm_PrimaryCCA_Init(pDM_Odm);    /*  Gary */
-	odm_DynamicBBPowerSavingInit(pDM_Odm);
 	odm_DynamicTxPowerInit(pDM_Odm);
 	odm_TXPowerTrackingInit(pDM_Odm);
 	ODM_EdcaTurboInit(pDM_Odm);
@@ -215,8 +214,6 @@ void ODM_DMWatchdog(struct odm_dm_struct *pDM_Odm)
 
 	odm_RefreshRateAdaptiveMask(pDM_Odm);
 
-	odm_DynamicBBPowerSaving(pDM_Odm);
-	odm_DynamicPrimaryCCA(pDM_Odm);
 	if ((pDM_Odm->AntDivType ==  CG_TRX_HW_ANTDIV)	||
 	    (pDM_Odm->AntDivType == CGCS_RX_HW_ANTDIV)	||
 	    (pDM_Odm->AntDivType == CG_TRX_SMART_ANTDIV))
@@ -514,64 +511,10 @@ void ODM_Write_DIG(struct odm_dm_struct *pDM_Odm, u8 CurrentIGI)
 	struct rtw_dig *pDM_DigTable = &pDM_Odm->DM_DigTable;
 	struct adapter *adapter = pDM_Odm->Adapter;
 
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD,
-		     ("ODM_REG(IGI_A,pDM_Odm)=0x%x, ODM_BIT(IGI,pDM_Odm)=0x%x\n",
-		     ODM_REG(IGI_A, pDM_Odm), ODM_BIT(IGI, pDM_Odm)));
-
 	if (pDM_DigTable->CurIGValue != CurrentIGI) {
-		PHY_SetBBReg(adapter, ODM_REG(IGI_A, pDM_Odm), ODM_BIT(IGI, pDM_Odm), CurrentIGI);
-		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("CurrentIGI(0x%02x).\n", CurrentIGI));
-		/* pDM_DigTable->PreIGValue = pDM_DigTable->CurIGValue; */
+		PHY_SetBBReg(adapter, ODM_REG_IGI_A_11N, ODM_BIT_IGI_11N, CurrentIGI);
 		pDM_DigTable->CurIGValue = CurrentIGI;
 	}
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("ODM_Write_DIG():CurrentIGI=0x%x\n", CurrentIGI));
-
-/*  Add by Neil Chen to enable edcca to MP Platform */
-}
-
-/* Need LPS mode for CE platform --2012--08--24--- */
-/* 8723AS/8189ES */
-void odm_DIGbyRSSI_LPS(struct odm_dm_struct *pDM_Odm)
-{
-	struct adapter *pAdapter = pDM_Odm->Adapter;
-	struct false_alarm_stats *pFalseAlmCnt = &pDM_Odm->FalseAlmCnt;
-
-	u8 RSSI_Lower = DM_DIG_MIN_NIC;   /* 0x1E or 0x1C */
-	u8 bFwCurrentInPSMode = false;
-	u8 CurrentIGI = pDM_Odm->RSSI_Min;
-
-	CurrentIGI = CurrentIGI + RSSI_OFFSET_DIG;
-	bFwCurrentInPSMode = pAdapter->pwrctrlpriv.bFwCurrentInPSMode;
-
-	/*  Using FW PS mode to make IGI */
-	if (bFwCurrentInPSMode) {
-		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("---Neil---odm_DIG is in LPS mode\n"));
-		/* Adjust by  FA in LPS MODE */
-		if (pFalseAlmCnt->Cnt_all > DM_DIG_FA_TH2_LPS)
-			CurrentIGI = CurrentIGI+2;
-		else if (pFalseAlmCnt->Cnt_all > DM_DIG_FA_TH1_LPS)
-			CurrentIGI = CurrentIGI+1;
-		else if (pFalseAlmCnt->Cnt_all < DM_DIG_FA_TH0_LPS)
-			CurrentIGI = CurrentIGI-1;
-	} else {
-		CurrentIGI = RSSI_Lower;
-	}
-
-	/* Lower bound checking */
-
-	/* RSSI Lower bound check */
-	if ((pDM_Odm->RSSI_Min-10) > DM_DIG_MIN_NIC)
-		RSSI_Lower = (pDM_Odm->RSSI_Min-10);
-	else
-		RSSI_Lower = DM_DIG_MIN_NIC;
-
-	/* Upper and Lower Bound checking */
-	 if (CurrentIGI > DM_DIG_MAX_NIC)
-		CurrentIGI = DM_DIG_MAX_NIC;
-	 else if (CurrentIGI < RSSI_Lower)
-		CurrentIGI = RSSI_Lower;
-
-	ODM_Write_DIG(pDM_Odm, CurrentIGI);/* ODM_Write_DIG(pDM_Odm, pDM_DigTable->CurIGValue); */
 }
 
 void odm_DIGInit(struct odm_dm_struct *pDM_Odm)
@@ -579,7 +522,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, pDM_Odm), ODM_BIT(IGI, pDM_Odm));
+	pDM_DigTable->CurIGValue = (u8) PHY_QueryBBReg(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;
@@ -890,64 +833,11 @@ void ODM_Write_CCK_CCA_Thres(struct odm_dm_struct *pDM_Odm, u8 CurCCK_CCAThres)
 	struct adapter *adapt = pDM_Odm->Adapter;
 
 	if (pDM_DigTable->CurCCK_CCAThres != CurCCK_CCAThres)		/* modify by Guo.Mingzhi 2012-01-03 */
-		rtw_write8(adapt, ODM_REG(CCK_CCA, pDM_Odm), CurCCK_CCAThres);
+		rtw_write8(adapt, ODM_REG_CCK_CCA_11N, CurCCK_CCAThres);
 	pDM_DigTable->PreCCK_CCAThres = pDM_DigTable->CurCCK_CCAThres;
 	pDM_DigTable->CurCCK_CCAThres = CurCCK_CCAThres;
 }
 
-/* 3============================================================ */
-/* 3 BB Power Save */
-/* 3============================================================ */
-void odm_DynamicBBPowerSavingInit(struct odm_dm_struct *pDM_Odm)
-{
-	struct rtl_ps *pDM_PSTable = &pDM_Odm->DM_PSTable;
-
-	pDM_PSTable->PreCCAState = CCA_MAX;
-	pDM_PSTable->CurCCAState = CCA_MAX;
-	pDM_PSTable->PreRFState = RF_MAX;
-	pDM_PSTable->CurRFState = RF_MAX;
-	pDM_PSTable->Rssi_val_min = 0;
-	pDM_PSTable->initialize = 0;
-}
-
-void odm_DynamicBBPowerSaving(struct odm_dm_struct *pDM_Odm)
-{
-}
-
-void odm_1R_CCA(struct odm_dm_struct *pDM_Odm)
-{
-	struct adapter *adapter = pDM_Odm->Adapter;
-	struct rtl_ps *pDM_PSTable = &pDM_Odm->DM_PSTable;
-
-	if (pDM_Odm->RSSI_Min != 0xFF) {
-		if (pDM_PSTable->PreCCAState == CCA_2R) {
-			if (pDM_Odm->RSSI_Min >= 35)
-				pDM_PSTable->CurCCAState = CCA_1R;
-			else
-				pDM_PSTable->CurCCAState = CCA_2R;
-		} else {
-			if (pDM_Odm->RSSI_Min <= 30)
-				pDM_PSTable->CurCCAState = CCA_2R;
-			else
-				pDM_PSTable->CurCCAState = CCA_1R;
-		}
-	} else {
-		pDM_PSTable->CurCCAState = CCA_MAX;
-	}
-
-	if (pDM_PSTable->PreCCAState != pDM_PSTable->CurCCAState) {
-		if (pDM_PSTable->CurCCAState == CCA_1R) {
-			if (pDM_Odm->RFType == ODM_2T2R)
-				PHY_SetBBReg(adapter, 0xc04, bMaskByte0, 0x13);
-			else
-				PHY_SetBBReg(adapter, 0xc04, bMaskByte0, 0x23);
-		} else {
-			PHY_SetBBReg(adapter, 0xc04, bMaskByte0, 0x33);
-		}
-		pDM_PSTable->PreCCAState = pDM_PSTable->CurCCAState;
-	}
-}
-
 void ODM_RF_Saving(struct odm_dm_struct *pDM_Odm, u8 bForceInNormal)
 {
 	struct adapter *adapter = pDM_Odm->Adapter;
@@ -1133,10 +1023,6 @@ void odm_RefreshRateAdaptiveMask(struct odm_dm_struct *pDM_Odm)
 	odm_RefreshRateAdaptiveMaskCE(pDM_Odm);
 }
 
-void odm_RefreshRateAdaptiveMaskMP(struct odm_dm_struct *pDM_Odm)
-{
-}
-
 void odm_RefreshRateAdaptiveMaskCE(struct odm_dm_struct *pDM_Odm)
 {
 	u8 i;
@@ -1165,10 +1051,6 @@ void odm_RefreshRateAdaptiveMaskCE(struct odm_dm_struct *pDM_Odm)
 	}
 }
 
-void odm_RefreshRateAdaptiveMaskAPADSL(struct odm_dm_struct *pDM_Odm)
-{
-}
-
 /*  Return Value: bool */
 /*  - true: RATRState is changed. */
 bool ODM_RAStateCheck(struct odm_dm_struct *pDM_Odm, s32 RSSI, bool bForceUpdate, u8 *pRATRState)
@@ -1239,15 +1121,6 @@ void odm_DynamicTxPower(struct odm_dm_struct *pDM_Odm)
 	/*  2012/01/12 MH According to Luke's suggestion, only high power will support the feature. */
 	if (!pDM_Odm->ExtPA)
 		return;
-
-	/*  2011/09/29 MH In HW integration first stage, we provide 4 different handle to operate */
-	/*  at the same time. In the stage2/3, we need to prive universal interface and merge all */
-	/*  HW dynamic mechanism. */
-	odm_DynamicTxPowerNIC(pDM_Odm);
-}
-
-void odm_DynamicTxPowerNIC(struct odm_dm_struct *pDM_Odm)
-{
 }
 
 /* 3============================================================ */
@@ -1386,22 +1259,6 @@ void odm_TXPowerTrackingCheckCE(struct odm_dm_struct *pDM_Odm)
 	}
 }
 
-/* antenna mapping info */
-/*  1: right-side antenna */
-/*  2/0: left-side antenna */
-/* PDM_SWAT_Table->CCK_Ant1_Cnt /OFDM_Ant1_Cnt:  for right-side antenna:   Ant:1    RxDefaultAnt1 */
-/* PDM_SWAT_Table->CCK_Ant2_Cnt /OFDM_Ant2_Cnt:  for left-side antenna:     Ant:0    RxDefaultAnt2 */
-/*  We select left antenna as default antenna in initial process, modify it as needed */
-/*  */
-
-/* 3============================================================ */
-/* 3 SW Antenna Diversity */
-/* 3============================================================ */
-
-void odm_SwAntDivChkAntSwitchCallback(void *FunctionContext)
-{
-}
-
 /* 3============================================================ */
 /* 3 SW Antenna Diversity */
 /* 3============================================================ */
@@ -1527,28 +1384,3 @@ dm_CheckEdcaTurbo_EXIT:
 	pxmitpriv->last_tx_bytes = pxmitpriv->tx_bytes;
 	precvpriv->last_rx_bytes = precvpriv->rx_bytes;
 }
-
-u32 ConvertTo_dB(u32 Value)
-{
-	u8 i;
-	u8 j;
-	u32 dB;
-
-	Value = Value & 0xFFFF;
-	for (i = 0; i < 8; i++) {
-		if (Value <= dB_Invert_Table[i][11])
-			break;
-	}
-
-	if (i >= 8)
-		return 96;	/*  maximum 96 dB */
-
-	for (j = 0; j < 12; j++) {
-		if (Value <= dB_Invert_Table[i][j])
-			break;
-	}
-
-	dB = i*12 + j + 1;
-
-	return dB;
-}
diff --git a/drivers/staging/rtl8188eu/hal/odm_HWConfig.c b/drivers/staging/rtl8188eu/hal/odm_HWConfig.c
index a755df35ec7..f2e1d02b8ae 100644
--- a/drivers/staging/rtl8188eu/hal/odm_HWConfig.c
+++ b/drivers/staging/rtl8188eu/hal/odm_HWConfig.c
@@ -275,10 +275,6 @@ static void odm_RxPhyStatus92CSeries_Parsing(struct odm_dm_struct *dm_odm,
 	dm_odm->DM_FatTable.antsel_rx_keep_2 = pPhyStaRpt->antsel_rx_keep_2;
 }
 
-void odm_Init_RSSIForDM(struct odm_dm_struct *dm_odm)
-{
-}
-
 static void odm_Process_RSSIForDM(struct odm_dm_struct *dm_odm,
 				  struct odm_phy_status_info *pPhyInfo,
 				  struct odm_per_pkt_info *pPktinfo)
@@ -436,14 +432,6 @@ void ODM_PhyStatusQuery(struct odm_dm_struct *dm_odm,
 	ODM_PhyStatusQuery_92CSeries(dm_odm, pPhyInfo, pPhyStatus, pPktinfo);
 }
 
-/*  For future use. */
-void ODM_MacStatusQuery(struct odm_dm_struct *dm_odm, u8 *mac_stat,
-			u8 macid, bool pkt_match_bssid,
-			bool pkttoself, bool pkt_beacon)
-{
-	/*  2011/10/19 Driver team will handle in the future. */
-}
-
 enum HAL_STATUS ODM_ConfigRFWithHeaderFile(struct odm_dm_struct *dm_odm,
 					   enum rf_radio_path content,
 					   enum rf_radio_path rfpath)
diff --git a/drivers/staging/rtl8188eu/hal/odm_RTL8188E.c b/drivers/staging/rtl8188eu/hal/odm_RTL8188E.c
index 323eb93be41..a24d954e273 100644
--- a/drivers/staging/rtl8188eu/hal/odm_RTL8188E.c
+++ b/drivers/staging/rtl8188eu/hal/odm_RTL8188E.c
@@ -20,18 +20,6 @@
 
 #include "odm_precomp.h"
 
-void ODM_DIG_LowerBound_88E(struct odm_dm_struct *dm_odm)
-{
-	struct rtw_dig *pDM_DigTable = &dm_odm->DM_DigTable;
-
-	if (dm_odm->AntDivType == CG_TRX_HW_ANTDIV) {
-		pDM_DigTable->rx_gain_range_min = (u8) pDM_DigTable->AntDiv_RSSI_max;
-		ODM_RT_TRACE(dm_odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD,
-			     ("ODM_DIG_LowerBound_88E(): pDM_DigTable->AntDiv_RSSI_max=%d\n", pDM_DigTable->AntDiv_RSSI_max));
-	}
-	/* If only one Entry connected */
-}
-
 static void odm_RX_HWAntDivInit(struct odm_dm_struct *dm_odm)
 {
 	struct adapter *adapter = dm_odm->Adapter;
@@ -388,15 +376,3 @@ void odm_PrimaryCCA_Init(struct odm_dm_struct *dm_odm)
 	PrimaryCCA->Monitor_flag = 0;
 	PrimaryCCA->PriCCA_flag = 0;
 }
-
-bool ODM_DynamicPrimaryCCA_DupRTS(struct odm_dm_struct *dm_odm)
-{
-	struct dyn_primary_cca *PrimaryCCA = &(dm_odm->DM_PriCCA);
-
-	return	PrimaryCCA->DupRTS_flag;
-}
-
-void odm_DynamicPrimaryCCA(struct odm_dm_struct *dm_odm)
-{
-	return;
-}
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c b/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c
index cf88bf247c8..d5cd30bcb3c 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c
@@ -31,10 +31,6 @@
 
 #include <rtl8188e_hal.h>
 
-static void dm_CheckStatistics(struct adapter *Adapter)
-{
-}
-
 /*  Initialize GPIO setting registers */
 static void dm_InitGPIOSetting(struct adapter *Adapter)
 {
@@ -173,12 +169,6 @@ void rtl8188e_HalDmWatchDog(struct adapter *Adapter)
 	if (Adapter->wdinfo.p2p_ps_mode)
 		fw_ps_awake = false;
 
-	if (hw_init_completed && ((!fw_cur_in_ps) && fw_ps_awake)) {
-		/*  Calculate Tx/Rx statistics. */
-		dm_CheckStatistics(Adapter);
-
-	}
-
 	/* ODM */
 	if (hw_init_completed) {
 		struct mlme_priv *pmlmepriv = &Adapter->mlmepriv;
@@ -213,10 +203,6 @@ void rtl8188e_init_dm_priv(struct adapter *Adapter)
 	ODM_InitDebugSetting(podmpriv);
 }
 
-void rtl8188e_deinit_dm_priv(struct adapter *Adapter)
-{
-}
-
 /*  Add new function to reset the state of antenna diversity before link. */
 /*  Compare RSSI for deciding antenna */
 void AntDivCompare8188E(struct adapter *Adapter, struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src)
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c b/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c
index f9d5558431e..5a22c6df4d0 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c
@@ -1843,7 +1843,6 @@ void rtl8188e_set_hal_ops(struct hal_ops *pHalFunc)
 	pHalFunc->free_hal_data = &rtl8188e_free_hal_data;
 
 	pHalFunc->dm_init = &rtl8188e_init_dm_priv;
-	pHalFunc->dm_deinit = &rtl8188e_deinit_dm_priv;
 
 	pHalFunc->read_chip_version = &rtl8188e_read_chip_version;
 
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_phycfg.c b/drivers/staging/rtl8188eu/hal/rtl8188e_phycfg.c
index 8079fc67861..941ff7467c3 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_phycfg.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188e_phycfg.c
@@ -631,169 +631,6 @@ int PHY_RFConfig8188E(struct adapter *Adapter)
 	return rtStatus;
 }
 
-
-/*-----------------------------------------------------------------------------
- * Function:    PHY_ConfigRFWithParaFile()
- *
- * Overview:    This function read RF parameters from general file format, and do RF 3-wire
- *
- * Input:	struct adapter *Adapter
- *			ps8					pFileName
- *			enum rf_radio_path eRFPath
- *
- * Output:      NONE
- *
- * Return:      RT_STATUS_SUCCESS: configuration file exist
- *
- * Note:		Delay may be required for RF configuration
- *---------------------------------------------------------------------------*/
-int rtl8188e_PHY_ConfigRFWithParaFile(struct adapter *Adapter, u8 *pFileName, enum rf_radio_path eRFPath)
-{
-	return _SUCCESS;
-}
-
-void
-rtl8192c_PHY_GetHWRegOriginalValue(
-		struct adapter *Adapter
-	)
-{
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(Adapter);
-
-	/*  read rx initial gain */
-	pHalData->DefaultInitialGain[0] = (u8)PHY_QueryBBReg(Adapter, rOFDM0_XAAGCCore1, bMaskByte0);
-	pHalData->DefaultInitialGain[1] = (u8)PHY_QueryBBReg(Adapter, rOFDM0_XBAGCCore1, bMaskByte0);
-	pHalData->DefaultInitialGain[2] = (u8)PHY_QueryBBReg(Adapter, rOFDM0_XCAGCCore1, bMaskByte0);
-	pHalData->DefaultInitialGain[3] = (u8)PHY_QueryBBReg(Adapter, rOFDM0_XDAGCCore1, bMaskByte0);
-
-	/*  read framesync */
-	pHalData->framesync = (u8)PHY_QueryBBReg(Adapter, rOFDM0_RxDetector3, bMaskByte0);
-	pHalData->framesyncC34 = PHY_QueryBBReg(Adapter, rOFDM0_RxDetector2, bMaskDWord);
-}
-
-/*  */
-/*	Description: */
-/*		Map dBm into Tx power index according to */
-/*		current HW model, for example, RF and PA, and */
-/*		current wireless mode. */
-/*	By Bruce, 2008-01-29. */
-/*  */
-static	u8 phy_DbmToTxPwrIdx(struct adapter *Adapter, enum wireless_mode WirelessMode, int PowerInDbm)
-{
-	u8 TxPwrIdx = 0;
-	int				Offset = 0;
-
-
-	/*  */
-	/*  Tested by MP, we found that CCK Index 0 equals to 8dbm, OFDM legacy equals to */
-	/*  3dbm, and OFDM HT equals to 0dbm respectively. */
-	/*  Note: */
-	/*	The mapping may be different by different NICs. Do not use this formula for what needs accurate result. */
-	/*  By Bruce, 2008-01-29. */
-	/*  */
-	switch (WirelessMode) {
-	case WIRELESS_MODE_B:
-		Offset = -7;
-		break;
-
-	case WIRELESS_MODE_G:
-	case WIRELESS_MODE_N_24G:
-	default:
-		Offset = -8;
-		break;
-	}
-
-	if ((PowerInDbm - Offset) > 0)
-		TxPwrIdx = (u8)((PowerInDbm - Offset) * 2);
-	else
-		TxPwrIdx = 0;
-
-	/*  Tx Power Index is too large. */
-	if (TxPwrIdx > MAX_TXPWR_IDX_NMODE_92S)
-		TxPwrIdx = MAX_TXPWR_IDX_NMODE_92S;
-
-	return TxPwrIdx;
-}
-
-/*  */
-/*	Description: */
-/*		Map Tx power index into dBm according to */
-/*		current HW model, for example, RF and PA, and */
-/*		current wireless mode. */
-/*	By Bruce, 2008-01-29. */
-/*  */
-static int phy_TxPwrIdxToDbm(struct adapter *Adapter, enum wireless_mode WirelessMode, u8 TxPwrIdx)
-{
-	int				Offset = 0;
-	int				PwrOutDbm = 0;
-
-	/*  */
-	/*  Tested by MP, we found that CCK Index 0 equals to -7dbm, OFDM legacy equals to -8dbm. */
-	/*  Note: */
-	/*	The mapping may be different by different NICs. Do not use this formula for what needs accurate result. */
-	/*  By Bruce, 2008-01-29. */
-	/*  */
-	switch (WirelessMode) {
-	case WIRELESS_MODE_B:
-		Offset = -7;
-		break;
-	case WIRELESS_MODE_G:
-	case WIRELESS_MODE_N_24G:
-	default:
-		Offset = -8;
-		break;
-	}
-
-	PwrOutDbm = TxPwrIdx / 2 + Offset; /*  Discard the decimal part. */
-
-	return PwrOutDbm;
-}
-
-
-/*-----------------------------------------------------------------------------
- * Function:    GetTxPowerLevel8190()
- *
- * Overview:    This function is export to "common" moudule
- *
- * Input:       struct adapter *Adapter
- *			psByte			Power Level
- *
- * Output:      NONE
- *
- * Return:      NONE
- *
- *---------------------------------------------------------------------------*/
-void PHY_GetTxPowerLevel8188E(struct adapter *Adapter, u32 *powerlevel)
-{
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(Adapter);
-	u8 TxPwrLevel = 0;
-	int			TxPwrDbm;
-
-	/*  */
-	/*  Because the Tx power indexes are different, we report the maximum of them to */
-	/*  meet the CCX TPC request. By Bruce, 2008-01-31. */
-	/*  */
-
-	/*  CCK */
-	TxPwrLevel = pHalData->CurrentCckTxPwrIdx;
-	TxPwrDbm = phy_TxPwrIdxToDbm(Adapter, WIRELESS_MODE_B, TxPwrLevel);
-
-	/*  Legacy OFDM */
-	TxPwrLevel = pHalData->CurrentOfdm24GTxPwrIdx + pHalData->LegacyHTTxPowerDiff;
-
-	/*  Compare with Legacy OFDM Tx power. */
-	if (phy_TxPwrIdxToDbm(Adapter, WIRELESS_MODE_G, TxPwrLevel) > TxPwrDbm)
-		TxPwrDbm = phy_TxPwrIdxToDbm(Adapter, WIRELESS_MODE_G, TxPwrLevel);
-
-	/*  HT OFDM */
-	TxPwrLevel = pHalData->CurrentOfdm24GTxPwrIdx;
-
-	/*  Compare with HT OFDM Tx power. */
-	if (phy_TxPwrIdxToDbm(Adapter, WIRELESS_MODE_N_24G, TxPwrLevel) > TxPwrDbm)
-		TxPwrDbm = phy_TxPwrIdxToDbm(Adapter, WIRELESS_MODE_N_24G, TxPwrLevel);
-
-	*powerlevel = TxPwrDbm;
-}
-
 static void getTxPowerIndex88E(struct adapter *Adapter, u8 channel, u8 *cckPowerLevel,
 			       u8 *ofdmPowerLevel, u8 *BW20PowerLevel,
 			       u8 *BW40PowerLevel)
@@ -917,51 +754,6 @@ PHY_SetTxPowerLevel8188E(
 	rtl8188e_PHY_RF6052SetOFDMTxPower(Adapter, &ofdmPowerLevel[0], &BW20PowerLevel[0], &BW40PowerLevel[0], channel);
 }
 
-/*  */
-/*	Description: */
-/*		Update transmit power level of all channel supported. */
-/*  */
-/*	TODO: */
-/*		A mode. */
-/*	By Bruce, 2008-02-04. */
-/*  */
-bool
-PHY_UpdateTxPowerDbm8188E(
-		struct adapter *Adapter,
-		int		powerInDbm
-	)
-{
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(Adapter);
-	u8 idx;
-	u8 rf_path;
-
-	/*  TODO: A mode Tx power. */
-	u8 CckTxPwrIdx = phy_DbmToTxPwrIdx(Adapter, WIRELESS_MODE_B, powerInDbm);
-	u8 OfdmTxPwrIdx = phy_DbmToTxPwrIdx(Adapter, WIRELESS_MODE_N_24G, powerInDbm);
-
-	if (OfdmTxPwrIdx - pHalData->LegacyHTTxPowerDiff > 0)
-		OfdmTxPwrIdx -= pHalData->LegacyHTTxPowerDiff;
-	else
-		OfdmTxPwrIdx = 0;
-
-	for (idx = 0; idx < 14; idx++) {
-		for (rf_path = 0; rf_path < 2; rf_path++) {
-			pHalData->TxPwrLevelCck[rf_path][idx] = CckTxPwrIdx;
-			pHalData->TxPwrLevelHT40_1S[rf_path][idx] =
-			pHalData->TxPwrLevelHT40_2S[rf_path][idx] = OfdmTxPwrIdx;
-		}
-	}
-	return true;
-}
-
-void
-PHY_ScanOperationBackup8188E(
-		struct adapter *Adapter,
-		u8 Operation
-	)
-{
-}
-
 /*-----------------------------------------------------------------------------
  * Function:    PHY_SetBWModeCallback8192C()
  *
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_rf6052.c b/drivers/staging/rtl8188eu/hal/rtl8188e_rf6052.c
index b1cb5c4a6fd..52103da3e26 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_rf6052.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188e_rf6052.c
@@ -46,51 +46,6 @@
 
 #include <rtl8188e_hal.h>
 
-/*---------------------------Define Local Constant---------------------------*/
-/*  Define local structure for debug!!!!! */
-struct rf_shadow {
-	/*  Shadow register value */
-	u32 Value;
-	/*  Compare or not flag */
-	u8 Compare;
-	/*  Record If it had ever modified unpredicted */
-	u8 ErrorOrNot;
-	/*  Recorver Flag */
-	u8 Recorver;
-	/*  */
-	u8 Driver_Write;
-};
-
-/*---------------------------Define Local Constant---------------------------*/
-
-
-/*------------------------Define global variable-----------------------------*/
-
-/*------------------------Define local variable------------------------------*/
-
-/*-----------------------------------------------------------------------------
- * Function:	RF_ChangeTxPath
- *
- * Overview:	For RL6052, we must change some RF settign for 1T or 2T.
- *
- * Input:		u16 DataRate		0x80-8f, 0x90-9f
- *
- * Output:      NONE
- *
- * Return:      NONE
- *
- * Revised History:
- * When			Who		Remark
- * 09/25/2008	MHC		Create Version 0.
- *						Firmwaer support the utility later.
- *
- *---------------------------------------------------------------------------*/
-void rtl8188e_RF_ChangeTxPath(struct adapter *Adapter, u16 DataRate)
-{
-/*  We do not support gain table change inACUT now !!!! Delete later !!! */
-}	/* RF_ChangeTxPath */
-
-
 /*-----------------------------------------------------------------------------
  * Function:    PHY_RF6052SetBandwidth()
  *
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188eu_led.c b/drivers/staging/rtl8188eu/hal/rtl8188eu_led.c
index 08dfd94163e..77dce584f0d 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188eu_led.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188eu_led.c
@@ -35,16 +35,7 @@ void SwLedOn(struct adapter *padapter, struct LED_871x *pLed)
 	if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
 		return;
 	LedCfg = rtw_read8(padapter, REG_LEDCFG2);
-	switch (pLed->LedPin) {
-	case LED_PIN_LED0:
-		rtw_write8(padapter, REG_LEDCFG2, (LedCfg&0xf0)|BIT5|BIT6); /*  SW control led0 on. */
-		break;
-	case LED_PIN_LED1:
-		rtw_write8(padapter, REG_LEDCFG2, (LedCfg&0x0f)|BIT5); /*  SW control led1 on. */
-		break;
-	default:
-		break;
-	}
+	rtw_write8(padapter, REG_LEDCFG2, (LedCfg&0xf0)|BIT5|BIT6); /*  SW control led0 on. */
 	pLed->bLedOn = true;
 }
 
@@ -60,25 +51,15 @@ void SwLedOff(struct adapter *padapter, struct LED_871x *pLed)
 
 	LedCfg = rtw_read8(padapter, REG_LEDCFG2);/* 0x4E */
 
-	switch (pLed->LedPin) {
-	case LED_PIN_LED0:
-		if (pHalData->bLedOpenDrain) {
+	if (pHalData->bLedOpenDrain) {
 			/*  Open-drain arrangement for controlling the LED) */
-			LedCfg &= 0x90; /*  Set to software control. */
-			rtw_write8(padapter, REG_LEDCFG2, (LedCfg|BIT3));
-			LedCfg = rtw_read8(padapter, REG_MAC_PINMUX_CFG);
-			LedCfg &= 0xFE;
-			rtw_write8(padapter, REG_MAC_PINMUX_CFG, LedCfg);
-		} else {
-			rtw_write8(padapter, REG_LEDCFG2, (LedCfg|BIT3|BIT5|BIT6));
-		}
-		break;
-	case LED_PIN_LED1:
-		LedCfg &= 0x0f; /*  Set to software control. */
+		LedCfg &= 0x90; /*  Set to software control. */
 		rtw_write8(padapter, REG_LEDCFG2, (LedCfg|BIT3));
-		break;
-	default:
-		break;
+		LedCfg = rtw_read8(padapter, REG_MAC_PINMUX_CFG);
+		LedCfg &= 0xFE;
+		rtw_write8(padapter, REG_MAC_PINMUX_CFG, LedCfg);
+	} else {
+		rtw_write8(padapter, REG_LEDCFG2, (LedCfg|BIT3|BIT5|BIT6));
 	}
 exit:
 	pLed->bLedOn = false;
@@ -92,12 +73,13 @@ exit:
 void rtl8188eu_InitSwLeds(struct adapter *padapter)
 {
 	struct led_priv *pledpriv = &(padapter->ledpriv);
+	struct hal_data_8188e   *haldata = GET_HAL_DATA(padapter);
 
+	pledpriv->bRegUseLed = true;
 	pledpriv->LedControlHandler = LedControl8188eu;
+	haldata->bLedOpenDrain = true;
 
-	InitLed871x(padapter, &(pledpriv->SwLed0), LED_PIN_LED0);
-
-	InitLed871x(padapter, &(pledpriv->SwLed1), LED_PIN_LED1);
+	InitLed871x(padapter, &(pledpriv->SwLed0));
 }
 
 /*	Description: */
@@ -107,5 +89,4 @@ void rtl8188eu_DeInitSwLeds(struct adapter *padapter)
 	struct led_priv	*ledpriv = &(padapter->ledpriv);
 
 	DeInitLed871x(&(ledpriv->SwLed0));
-	DeInitLed871x(&(ledpriv->SwLed1));
 }
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188eu_recv.c b/drivers/staging/rtl8188eu/hal/rtl8188eu_recv.c
index b1b1584af1c..0f6222dab4a 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188eu_recv.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188eu_recv.c
@@ -28,22 +28,6 @@
 
 #include <rtl8188e_hal.h>
 
-void rtl8188eu_init_recvbuf(struct adapter *padapter, struct recv_buf *precvbuf)
-{
-	precvbuf->transfer_len = 0;
-
-	precvbuf->len = 0;
-
-	precvbuf->ref_cnt = 0;
-
-	if (precvbuf->pbuf) {
-		precvbuf->pdata = precvbuf->pbuf;
-		precvbuf->phead = precvbuf->pbuf;
-		precvbuf->ptail = precvbuf->pbuf;
-		precvbuf->pend = precvbuf->pdata + MAX_RECVBUF_SZ;
-	}
-}
-
 int	rtl8188eu_init_recv_priv(struct adapter *padapter)
 {
 	struct recv_priv	*precvpriv = &padapter->recvpriv;
@@ -71,13 +55,9 @@ int	rtl8188eu_init_recv_priv(struct adapter *padapter)
 	precvbuf = (struct recv_buf *)precvpriv->precv_buf;
 
 	for (i = 0; i < NR_RECVBUFF; i++) {
-		_rtw_init_listhead(&precvbuf->list);
-		spin_lock_init(&precvbuf->recvbuf_lock);
-		precvbuf->alloc_sz = MAX_RECVBUF_SZ;
 		res = rtw_os_recvbuf_resource_alloc(padapter, precvbuf);
 		if (res == _FAIL)
 			break;
-		precvbuf->ref_cnt = 0;
 		precvbuf->adapter = padapter;
 		precvbuf++;
 	}
@@ -117,7 +97,7 @@ void rtl8188eu_free_recv_priv(struct adapter *padapter)
 	precvbuf = (struct recv_buf *)precvpriv->precv_buf;
 
 	for (i = 0; i < NR_RECVBUFF; i++) {
-		rtw_os_recvbuf_resource_free(padapter, precvbuf);
+		usb_free_urb(precvbuf->purb);
 		precvbuf++;
 	}
 
diff --git a/drivers/staging/rtl8188eu/hal/usb_halinit.c b/drivers/staging/rtl8188eu/hal/usb_halinit.c
index c92067f0ef1..141f85ae561 100644
--- a/drivers/staging/rtl8188eu/hal/usb_halinit.c
+++ b/drivers/staging/rtl8188eu/hal/usb_halinit.c
@@ -420,22 +420,6 @@ static void _InitEDCA(struct adapter *Adapter)
 	rtw_write32(Adapter, REG_EDCA_VO_PARAM, 0x002FA226);
 }
 
-static void _InitBeaconMaxError(struct adapter *Adapter, bool		InfraMode)
-{
-}
-
-static void _InitHWLed(struct adapter *Adapter)
-{
-	struct led_priv *pledpriv = &(Adapter->ledpriv);
-
-	if (pledpriv->LedStrategy != HW_LED)
-		return;
-
-/*  HW led control */
-/*  to do .... */
-/* must consider cases of antenna diversity/ commbo card/solo card/mini card */
-}
-
 static void _InitRDGSetting(struct adapter *Adapter)
 {
 	rtw_write8(Adapter, REG_RD_CTRL, 0xFF);
@@ -599,10 +583,6 @@ static void InitUsbAggregationSetting(struct adapter *Adapter)
 	haldata->UsbRxHighSpeedMode = false;
 }
 
-static void _InitOperationMode(struct adapter *Adapter)
-{
-}
-
 static void _InitBeaconParameters(struct adapter *Adapter)
 {
 	struct hal_data_8188e	*haldata = GET_HAL_DATA(Adapter);
@@ -713,7 +693,6 @@ static u32 rtl8188eu_hal_init(struct adapter *Adapter)
 	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_BEGIN);
 
 	if (Adapter->pwrctrlpriv.bkeepfwalive) {
-		_ps_open_RF(Adapter);
 
 		if (haldata->odmpriv.RFCalibrateInfo.bIQKInitialized) {
 			PHY_IQCalibrate_8188E(Adapter, true);
@@ -840,14 +819,9 @@ static u32 rtl8188eu_hal_init(struct adapter *Adapter)
 	_InitEDCA(Adapter);
 	_InitRetryFunction(Adapter);
 	InitUsbAggregationSetting(Adapter);
-	_InitOperationMode(Adapter);/* todo */
 	_InitBeaconParameters(Adapter);
-	_InitBeaconMaxError(Adapter, true);
-
-	/*  */
 	/*  Init CR MACTXEN, MACRXEN after setting RxFF boundary REG_TRXFF_BNDY to patch */
 	/*  Hw bug which Hw initials RxFF boundary size to a value which is larger than the real Rx buffer size in 88E. */
-	/*  */
 	/*  Enable MACTXEN/MACRXEN block */
 	value16 = rtw_read16(Adapter, REG_CR);
 	value16 |= (MACTXEN | MACRXEN);
@@ -870,8 +844,6 @@ static u32 rtl8188eu_hal_init(struct adapter *Adapter)
 	rtw_write16(Adapter, REG_PKT_VO_VI_LIFE_TIME, 0x0400);	/*  unit: 256us. 256ms */
 	rtw_write16(Adapter, REG_PKT_BE_BK_LIFE_TIME, 0x0400);	/*  unit: 256us. 256ms */
 
-	_InitHWLed(Adapter);
-
 	/* 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);
@@ -970,18 +942,6 @@ HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_END);
 	return status;
 }
 
-void _ps_open_RF(struct adapter *adapt)
-{
-	/* here call with bRegSSPwrLvl 1, bRegSSPwrLvl 2 needs to be verified */
-	/* phy_SsPwrSwitch92CU(adapt, rf_on, 1); */
-}
-
-static void _ps_close_RF(struct adapter *adapt)
-{
-	/* here call with bRegSSPwrLvl 1, bRegSSPwrLvl 2 needs to be verified */
-	/* phy_SsPwrSwitch92CU(adapt, rf_off, 1); */
-}
-
 static void CardDisableRTL8188EU(struct adapter *Adapter)
 {
 	u8 val8;
@@ -1059,7 +1019,6 @@ static u32 rtl8188eu_hal_deinit(struct adapter *Adapter)
 
 	DBG_88E("bkeepfwalive(%x)\n", Adapter->pwrctrlpriv.bkeepfwalive);
 	if (Adapter->pwrctrlpriv.bkeepfwalive) {
-		_ps_close_RF(Adapter);
 		if ((Adapter->pwrctrlpriv.bHWPwrPindetect) && (Adapter->pwrctrlpriv.bHWPowerdown))
 			rtl8192cu_hw_power_down(Adapter);
 	} else {
@@ -1129,16 +1088,6 @@ static unsigned int rtl8188eu_inirp_deinit(struct adapter *Adapter)
 /*	EEPROM/EFUSE Content Parsing */
 /*  */
 /*  */
-static void _ReadLEDSetting(struct adapter *Adapter, u8 *PROMContent, bool AutoloadFail)
-{
-	struct led_priv *pledpriv = &(Adapter->ledpriv);
-	struct hal_data_8188e	*haldata = GET_HAL_DATA(Adapter);
-
-	pledpriv->bRegUseLed = true;
-	pledpriv->LedStrategy = SW_LED_MODE1;
-	haldata->bLedOpenDrain = true;/*  Support Open-drain arrangement for controlling the LED. */
-}
-
 static void Hal_EfuseParsePIDVID_8188EU(struct adapter *adapt, u8 *hwinfo, bool AutoLoadFail)
 {
 	struct hal_data_8188e	*haldata = GET_HAL_DATA(adapt);
@@ -1184,10 +1133,6 @@ static void Hal_EfuseParseMACAddr_8188EU(struct adapter *adapt, u8 *hwinfo, bool
 		 eeprom->mac_addr[4], eeprom->mac_addr[5]));
 }
 
-static void Hal_CustomizeByCustomerID_8188EU(struct adapter *adapt)
-{
-}
-
 static void
 readAdapterInfo_8188EU(
 		struct adapter *adapt
@@ -1214,9 +1159,6 @@ readAdapterInfo_8188EU(
 	/*  The following part initialize some vars by PG info. */
 	/*  */
 	Hal_InitChannelPlan(adapt);
-	Hal_CustomizeByCustomerID_8188EU(adapt);
-
-	_ReadLEDSetting(adapt, eeprom->efuse_eeprom_data, eeprom->bautoload_fail_flag);
 }
 
 static void _ReadPROMContent(
@@ -2284,12 +2226,6 @@ static void rtl8188eu_init_default_value(struct adapter *adapt)
 		haldata->odmpriv.RFCalibrateInfo.ThermalValue_HP[i] = 0;
 }
 
-static u8 rtl8188eu_ps_func(struct adapter *Adapter, enum hal_intf_ps_func efunc_id, u8 *val)
-{
-	u8 bResult = true;
-	return bResult;
-}
-
 void rtl8188eu_set_hal_ops(struct adapter *adapt)
 {
 	struct hal_ops	*halfunc = &adapt->HalFunc;
@@ -2330,7 +2266,5 @@ void rtl8188eu_set_hal_ops(struct adapter *adapt)
 	halfunc->hal_xmit = &rtl8188eu_hal_xmit;
 	halfunc->mgnt_xmit = &rtl8188eu_mgnt_xmit;
 
-	halfunc->interface_ps_func = &rtl8188eu_ps_func;
-
 	rtl8188e_set_hal_ops(halfunc);
 }
diff --git a/drivers/staging/rtl8188eu/hal/usb_ops_linux.c b/drivers/staging/rtl8188eu/hal/usb_ops_linux.c
index 1fa5370f1da..3aadf560de0 100644
--- a/drivers/staging/rtl8188eu/hal/usb_ops_linux.c
+++ b/drivers/staging/rtl8188eu/hal/usb_ops_linux.c
@@ -333,7 +333,6 @@ static int recvbuf2recvframe(struct adapter *adapt, struct sk_buff *pskb)
 		}
 
 		_rtw_init_listhead(&precvframe->list);
-		precvframe->precvbuf = NULL;	/* can't access the precvbuf for new arch. */
 		precvframe->len = 0;
 
 		update_recvframe_attrib_88e(precvframe, prxstat);
@@ -518,7 +517,6 @@ static void usb_read_port_complete(struct urb *purb, struct pt_regs *regs)
 		} else {
 			rtw_reset_continual_urb_error(adapter_to_dvobj(adapt));
 
-			precvbuf->transfer_len = purb->actual_length;
 			skb_put(precvbuf->pskb, purb->actual_length);
 			skb_queue_tail(&precvpriv->rx_skb_queue, precvbuf->pskb);
 
@@ -559,7 +557,7 @@ static void usb_read_port_complete(struct urb *purb, struct pt_regs *regs)
 			rtw_read_port(adapt, precvpriv->ff_hwaddr, 0, (unsigned char *)precvbuf);
 			break;
 		case -EINPROGRESS:
-			DBG_88E("ERROR: URB IS IN PROGRESS!/n");
+			DBG_88E("ERROR: URB IS IN PROGRESS!\n");
 			break;
 		default:
 			break;
@@ -601,8 +599,6 @@ static u32 usb_read_port(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *rmem)
 			precvbuf->reuse = true;
 	}
 
-	rtl8188eu_init_recvbuf(adapter, precvbuf);
-
 	/* re-assign for linux based on skb */
 	if ((!precvbuf->reuse) || (precvbuf->pskb == NULL)) {
 		precvbuf->pskb = netdev_alloc_skb(adapter->pnetdev, MAX_RECVBUF_SZ + RECVBUFF_ALIGN_SZ);
@@ -615,19 +611,7 @@ static u32 usb_read_port(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *rmem)
 		tmpaddr = (size_t)precvbuf->pskb->data;
 		alignment = tmpaddr & (RECVBUFF_ALIGN_SZ-1);
 		skb_reserve(precvbuf->pskb, (RECVBUFF_ALIGN_SZ - alignment));
-
-		precvbuf->phead = precvbuf->pskb->head;
-		precvbuf->pdata = precvbuf->pskb->data;
-		precvbuf->ptail = skb_tail_pointer(precvbuf->pskb);
-		precvbuf->pend = skb_end_pointer(precvbuf->pskb);
-		precvbuf->pbuf = precvbuf->pskb->data;
 	} else { /* reuse skb */
-		precvbuf->phead = precvbuf->pskb->head;
-		precvbuf->pdata = precvbuf->pskb->data;
-		precvbuf->ptail = skb_tail_pointer(precvbuf->pskb);
-		precvbuf->pend = skb_end_pointer(precvbuf->pskb);
-		precvbuf->pbuf = precvbuf->pskb->data;
-
 		precvbuf->reuse = false;
 	}
 
@@ -639,7 +623,7 @@ static u32 usb_read_port(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *rmem)
 	pipe = ffaddr2pipehdl(pdvobj, addr);
 
 	usb_fill_bulk_urb(purb, pusbd, pipe,
-			  precvbuf->pbuf,
+			  precvbuf->pskb->data,
 			  MAX_RECVBUF_SZ,
 			  usb_read_port_complete,
 			  precvbuf);/* context is precvbuf */
@@ -698,9 +682,3 @@ void rtl8188eu_set_intf_ops(struct _io_ops	*pops)
 	pops->_read_port_cancel = &usb_read_port_cancel;
 	pops->_write_port_cancel = &usb_write_port_cancel;
 }
-
-void rtl8188eu_set_hw_type(struct adapter *adapt)
-{
-	adapt->chip_type = RTL8188E;
-	DBG_88E("CHIP TYPE: RTL8188E\n");
-}
diff --git a/drivers/staging/rtl8188eu/include/Hal8188EPhyCfg.h b/drivers/staging/rtl8188eu/include/Hal8188EPhyCfg.h
index 25cae8147e7..260ea6bf9e8 100644
--- a/drivers/staging/rtl8188eu/include/Hal8188EPhyCfg.h
+++ b/drivers/staging/rtl8188eu/include/Hal8188EPhyCfg.h
@@ -224,7 +224,6 @@ 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);
-bool PHY_UpdateTxPowerDbm8188E(struct adapter *adapter, int power);
 
 void PHY_ScanOperationBackup8188E(struct adapter *Adapter, u8 Operation);
 
diff --git a/drivers/staging/rtl8188eu/include/drv_types.h b/drivers/staging/rtl8188eu/include/drv_types.h
index 936c196699a..10cc1a1bcab 100644
--- a/drivers/staging/rtl8188eu/include/drv_types.h
+++ b/drivers/staging/rtl8188eu/include/drv_types.h
@@ -31,7 +31,6 @@
 
 #include <osdep_service.h>
 #include <wlan_bssdef.h>
-#include <drv_types_linux.h>
 #include <rtw_ht.h>
 #include <rtw_cmd.h>
 #include <rtw_xmit.h>
diff --git a/drivers/staging/rtl8188eu/include/hal_intf.h b/drivers/staging/rtl8188eu/include/hal_intf.h
index c274b349f61..c59fccde39c 100644
--- a/drivers/staging/rtl8188eu/include/hal_intf.h
+++ b/drivers/staging/rtl8188eu/include/hal_intf.h
@@ -166,7 +166,6 @@ struct hal_ops {
 	void	(*DeInitSwLeds)(struct adapter *padapter);
 
 	void	(*dm_init)(struct adapter *padapter);
-	void	(*dm_deinit)(struct adapter *padapter);
 	void	(*read_chip_version)(struct adapter *padapter);
 
 	void	(*init_default_value)(struct adapter *padapter);
@@ -218,9 +217,6 @@ struct hal_ops {
 	void	(*AntDivCompareHandler)(struct adapter *adapter,
 					struct wlan_bssid_ex *dst,
 					struct wlan_bssid_ex *src);
-	u8	(*interface_ps_func)(struct adapter *padapter,
-				     enum hal_intf_ps_func efunc_id, u8 *val);
-
 	s32	(*hal_xmit)(struct adapter *padapter,
 			    struct xmit_frame *pxmitframe);
 	s32 (*mgnt_xmit)(struct adapter *padapter,
@@ -299,7 +295,6 @@ void rtw_hal_def_value_init(struct adapter *padapter);
 void	rtw_hal_free_data(struct adapter *padapter);
 
 void rtw_hal_dm_init(struct adapter *padapter);
-void rtw_hal_dm_deinit(struct adapter *padapter);
 void rtw_hal_sw_led_init(struct adapter *padapter);
 void rtw_hal_sw_led_deinit(struct adapter *padapter);
 
@@ -332,9 +327,6 @@ void rtw_hal_disable_interrupt(struct adapter *padapter);
 u32	rtw_hal_inirp_init(struct adapter *padapter);
 u32	rtw_hal_inirp_deinit(struct adapter *padapter);
 
-u8	rtw_hal_intf_ps_func(struct adapter *padapter,
-			     enum hal_intf_ps_func efunc_id, u8 *val);
-
 s32	rtw_hal_xmit(struct adapter *padapter, struct xmit_frame *pxmitframe);
 s32	rtw_hal_mgnt_xmit(struct adapter *padapter,
 			  struct xmit_frame *pmgntframe);
diff --git a/drivers/staging/rtl8188eu/include/odm.h b/drivers/staging/rtl8188eu/include/odm.h
index 9d1a79c21a2..e83812f35ad 100644
--- a/drivers/staging/rtl8188eu/include/odm.h
+++ b/drivers/staging/rtl8188eu/include/odm.h
@@ -90,14 +90,6 @@
 #define ANTTESTA		0x01	/* Ant A will be Testing */
 #define ANTTESTB		0x02	/* Ant B will be testing */
 
-/*  structure and define */
-
-/*  Add for AP/ADSLpseudo DM structuer requirement. */
-/*  We need to remove to other position??? */
-struct rtl8192cd_priv {
-	u8		temp;
-};
-
 struct rtw_dig {
 	u8		Dig_Enable_Flag;
 	u8		Dig_Ext_Port_Stage;
diff --git a/drivers/staging/rtl8188eu/include/odm_RTL8188E.h b/drivers/staging/rtl8188eu/include/odm_RTL8188E.h
index f96ad5af4bd..02ac78d326d 100644
--- a/drivers/staging/rtl8188eu/include/odm_RTL8188E.h
+++ b/drivers/staging/rtl8188eu/include/odm_RTL8188E.h
@@ -51,6 +51,4 @@ void odm_PrimaryCCA_Init(struct odm_dm_struct *pDM_Odm);
 
 bool ODM_DynamicPrimaryCCA_DupRTS(struct odm_dm_struct *pDM_Odm);
 
-void odm_DynamicPrimaryCCA(struct odm_dm_struct *pDM_Odm);
-
 #endif
diff --git a/drivers/staging/rtl8188eu/include/odm_debug.h b/drivers/staging/rtl8188eu/include/odm_debug.h
index e8c4cab2c35..db7b44e16c4 100644
--- a/drivers/staging/rtl8188eu/include/odm_debug.h
+++ b/drivers/staging/rtl8188eu/include/odm_debug.h
@@ -85,7 +85,7 @@
 /*------------------------Export Marco Definition---------------------------*/
 #define DbgPrint	pr_info
 #define RT_PRINTK(fmt, args...)				\
-	DbgPrint( "%s(): " fmt, __func__, ## args);
+	DbgPrint("%s(): " fmt, __func__, ## args);
 
 #ifndef ASSERT
 	#define ASSERT(expr)
@@ -106,8 +106,8 @@
 
 #define ODM_RT_ASSERT(pDM_Odm, expr, fmt)				\
 	if (!(expr)) {							\
-		DbgPrint( "Assertion failed! %s at ......\n", #expr);	\
-		DbgPrint( "      ......%s,%s,line=%d\n", __FILE__,	\
+		DbgPrint("Assertion failed! %s at ......\n", #expr);	\
+		DbgPrint("      ......%s,%s,line=%d\n", __FILE__,	\
 			__func__, __LINE__);				\
 		RT_PRINTK fmt;						\
 		ASSERT(false);						\
diff --git a/drivers/staging/rtl8188eu/include/odm_interface.h b/drivers/staging/rtl8188eu/include/odm_interface.h
deleted file mode 100644
index 548a309db19..00000000000
--- a/drivers/staging/rtl8188eu/include/odm_interface.h
+++ /dev/null
@@ -1,91 +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_INTERFACE_H__
-#define __ODM_INTERFACE_H__
-
-/*  */
-/*  =========== Constant/Structure/Enum/... Define */
-/*  */
-
-/*  */
-/*  =========== Macro Define */
-/*  */
-
-#define _reg_all(_name)			ODM_##_name
-#define _reg_ic(_name, _ic)		ODM_##_name##_ic
-#define _bit_all(_name)			BIT_##_name
-#define _bit_ic(_name, _ic)		BIT_##_name##_ic
-
-/*  _cat: implemented by Token-Pasting Operator. */
-
-/*===================================
-
-#define ODM_REG_DIG_11N		0xC50
-#define ODM_REG_DIG_11AC	0xDDD
-
-ODM_REG(DIG,_pDM_Odm)
-=====================================*/
-
-#define _reg_11N(_name)			ODM_REG_##_name##_11N
-#define _reg_11AC(_name)		ODM_REG_##_name##_11AC
-#define _bit_11N(_name)			ODM_BIT_##_name##_11N
-#define _bit_11AC(_name)		ODM_BIT_##_name##_11AC
-
-#define _cat(_name, _ic_type, _func)					\
-	(								\
-		(_ic_type) ? _func##_11N(_name) :			\
-		_func##_11AC(_name)					\
-	)
-
-/*  _name: name of register or bit. */
-/*  Example: "ODM_REG(R_A_AGC_CORE1, pDM_Odm)" */
-/*         gets "ODM_R_A_AGC_CORE1" or "ODM_R_A_AGC_CORE1_8192C",
- *	   depends on SupportICType. */
-#define ODM_REG(_name, _pDM_Odm) _cat(_name, _pDM_Odm->SupportICType, _reg)
-#define ODM_BIT(_name, _pDM_Odm) _cat(_name, _pDM_Odm->SupportICType, _bit)
-
-enum odm_h2c_cmd {
-	ODM_H2C_RSSI_REPORT = 0,
-	ODM_H2C_PSD_RESULT = 1,
-	ODM_H2C_PathDiv = 2,
-	ODM_MAX_H2CCMD
-};
-
-/*  2012/02/17 MH For non-MP compile pass only. Linux does not support workitem. */
-/*  Suggest HW team to use thread instead of workitem. Windows also support the feature. */
-typedef void (*RT_WORKITEM_CALL_BACK)(void *pContext);
-
-/*  =========== Extern Variable ??? It should be forbidden. */
-
-/*  =========== EXtern Function Prototype */
-
-/*  Memory Relative Function. */
-
-/*  ODM Timer relative API. */
-
-void ODM_CancelTimer(struct odm_dm_struct *pDM_Odm, struct timer_list *pTimer);
-
-/*  ODM FW relative API. */
-u32 ODM_FillH2CCmd(u8 *pH2CBuffer, u32 H2CBufferLen, u32 CmdNum,
-		   u32 *pElementID, u32 *pCmdLen, u8 **pCmbBuffer,
-		   u8 *CmdStartSeq);
-
-#endif	/*  __ODM_INTERFACE_H__ */
diff --git a/drivers/staging/rtl8188eu/include/odm_precomp.h b/drivers/staging/rtl8188eu/include/odm_precomp.h
index 6e6a656b415..2eb769b3f35 100644
--- a/drivers/staging/rtl8188eu/include/odm_precomp.h
+++ b/drivers/staging/rtl8188eu/include/odm_precomp.h
@@ -44,7 +44,6 @@
 #include "Hal8188ERateAdaptive.h"/* for  RA,Power training */
 #include "rtl8188e_hal.h"
 
-#include "odm_interface.h"
 #include "odm_reg.h"
 
 #include "HalHWImg8188E_MAC.h"
diff --git a/drivers/staging/rtl8188eu/include/odm_types.h b/drivers/staging/rtl8188eu/include/odm_types.h
index 78ee2bac0d8..c1355b959c5 100644
--- a/drivers/staging/rtl8188eu/include/odm_types.h
+++ b/drivers/staging/rtl8188eu/include/odm_types.h
@@ -20,31 +20,13 @@
 #ifndef __ODM_TYPES_H__
 #define __ODM_TYPES_H__
 
-/*  */
-/*  Define Different SW team support */
-/*  */
-#define	ODM_AP			0x01	 /* BIT0 */
-#define	ODM_ADSL		0x02	/* BIT1 */
 #define	ODM_CE			0x04	/* BIT2 */
-#define	ODM_MP			0x08	/* BIT3 */
-
-#define		RT_PCI_INTERFACE				1
-#define		RT_USB_INTERFACE				2
-#define		RT_SDIO_INTERFACE				3
 
 enum HAL_STATUS {
 	HAL_STATUS_SUCCESS,
 	HAL_STATUS_FAILURE,
 };
 
-enum RT_SPINLOCK_TYPE {
-	RT_TEMP = 1,
-};
-
-#include <basic_types.h>
-
-#define DEV_BUS_TYPE	RT_USB_INTERFACE
-
 #define SET_TX_DESC_ANTSEL_A_88E(__pTxDesc, __Value)			\
 	SET_BITS_TO_LE_4BYTE(__pTxDesc+8, 24, 1, __Value)
 #define SET_TX_DESC_ANTSEL_B_88E(__pTxDesc, __Value)			\
@@ -52,11 +34,4 @@ enum RT_SPINLOCK_TYPE {
 #define SET_TX_DESC_ANTSEL_C_88E(__pTxDesc, __Value)			\
 	SET_BITS_TO_LE_4BYTE(__pTxDesc+28, 29, 1, __Value)
 
-/* define useless flag to avoid compile warning */
-#define	USE_WORKITEM			0
-#define		FOR_BRAZIL_PRETEST	0
-#define	BT_30_SUPPORT			0
-#define   FPGA_TWO_MAC_VERIFICATION	0
-
-
 #endif /*  __ODM_TYPES_H__ */
diff --git a/drivers/staging/rtl8188eu/include/osdep_service.h b/drivers/staging/rtl8188eu/include/osdep_service.h
index 5889f58ed7d..3859acd358a 100644
--- a/drivers/staging/rtl8188eu/include/osdep_service.h
+++ b/drivers/staging/rtl8188eu/include/osdep_service.h
@@ -119,21 +119,6 @@ static inline int rtw_netif_queue_stopped(struct net_device *pnetdev)
 		netif_tx_queue_stopped(netdev_get_tx_queue(pnetdev, 3));
 }
 
-static inline void rtw_netif_wake_queue(struct net_device *pnetdev)
-{
-	netif_tx_wake_all_queues(pnetdev);
-}
-
-static inline void rtw_netif_start_queue(struct net_device *pnetdev)
-{
-	netif_tx_start_all_queues(pnetdev);
-}
-
-static inline void rtw_netif_stop_queue(struct net_device *pnetdev)
-{
-	netif_tx_stop_all_queues(pnetdev);
-}
-
 #ifndef BIT
 	#define BIT(x)	(1 << (x))
 #endif
@@ -254,11 +239,6 @@ static inline void flush_signals_thread(void)
 		flush_signals(current);
 }
 
-static inline int res_to_status(int res)
-{
-	return res;
-}
-
 #define _RND(sz, r) ((((sz)+((r)-1))/(r))*(r))
 #define RND4(x)	(((x >> 2) + (((x & 3) == 0) ?  0 : 1)) << 2)
 
@@ -359,15 +339,7 @@ u64 rtw_division64(u64 x, u64 y);
 
 #define RTW_GET_BE24(a) ((((u32) (a)[0]) << 16) | (((u32) (a)[1]) << 8) | \
 			 ((u32) (a)[2]))
-#define RTW_PUT_BE24(a, val)					\
-	do {							\
-		(a)[0] = (u8) ((((u32) (val)) >> 16) & 0xff);	\
-		(a)[1] = (u8) ((((u32) (val)) >> 8) & 0xff);	\
-		(a)[2] = (u8) (((u32) (val)) & 0xff);		\
-	} while (0)
 
-#define RTW_GET_BE32(a) ((((u32) (a)[0]) << 24) | (((u32) (a)[1]) << 16) | \
-			 (((u32) (a)[2]) << 8) | ((u32) (a)[3]))
 #define RTW_PUT_BE32(a, val)					\
 	do {							\
 		(a)[0] = (u8) ((((u32) (val)) >> 24) & 0xff);	\
@@ -376,37 +348,6 @@ u64 rtw_division64(u64 x, u64 y);
 		(a)[3] = (u8) (((u32) (val)) & 0xff);		\
 	} while (0)
 
-#define RTW_GET_LE32(a) ((((u32) (a)[3]) << 24) | (((u32) (a)[2]) << 16) | \
-			 (((u32) (a)[1]) << 8) | ((u32) (a)[0]))
-#define RTW_PUT_LE32(a, val)					\
-	do {							\
-		(a)[3] = (u8) ((((u32) (val)) >> 24) & 0xff);	\
-		(a)[2] = (u8) ((((u32) (val)) >> 16) & 0xff);	\
-		(a)[1] = (u8) ((((u32) (val)) >> 8) & 0xff);	\
-		(a)[0] = (u8) (((u32) (val)) & 0xff);		\
-	} while (0)
-
-#define RTW_GET_BE64(a) ((((u64) (a)[0]) << 56) | (((u64) (a)[1]) << 48) | \
-			 (((u64) (a)[2]) << 40) | (((u64) (a)[3]) << 32) | \
-			 (((u64) (a)[4]) << 24) | (((u64) (a)[5]) << 16) | \
-			 (((u64) (a)[6]) << 8) | ((u64) (a)[7]))
-#define RTW_PUT_BE64(a, val)				\
-	do {						\
-		(a)[0] = (u8) (((u64) (val)) >> 56);	\
-		(a)[1] = (u8) (((u64) (val)) >> 48);	\
-		(a)[2] = (u8) (((u64) (val)) >> 40);	\
-		(a)[3] = (u8) (((u64) (val)) >> 32);	\
-		(a)[4] = (u8) (((u64) (val)) >> 24);	\
-		(a)[5] = (u8) (((u64) (val)) >> 16);	\
-		(a)[6] = (u8) (((u64) (val)) >> 8);	\
-		(a)[7] = (u8) (((u64) (val)) & 0xff);	\
-	} while (0)
-
-#define RTW_GET_LE64(a) ((((u64) (a)[7]) << 56) | (((u64) (a)[6]) << 48) | \
-			 (((u64) (a)[5]) << 40) | (((u64) (a)[4]) << 32) | \
-			 (((u64) (a)[3]) << 24) | (((u64) (a)[2]) << 16) | \
-			 (((u64) (a)[1]) << 8) | ((u64) (a)[0]))
-
 void rtw_buf_free(u8 **buf, u32 *buf_len);
 void rtw_buf_update(u8 **buf, u32 *buf_len, u8 *src, u32 src_len);
 
diff --git a/drivers/staging/rtl8188eu/include/recv_osdep.h b/drivers/staging/rtl8188eu/include/recv_osdep.h
index d76cc2db502..a4fd95798f0 100644
--- a/drivers/staging/rtl8188eu/include/recv_osdep.h
+++ b/drivers/staging/rtl8188eu/include/recv_osdep.h
@@ -33,19 +33,15 @@ int rtw_recv_indicatepkt(struct adapter *adapter,
 			 struct recv_frame *recv_frame);
 void rtw_recv_returnpacket(struct  net_device *cnxt, struct sk_buff *retpkt);
 
-void rtw_hostapd_mlme_rx(struct adapter *padapter, struct recv_frame *recv_fr);
 void rtw_handle_tkip_mic_err(struct adapter *padapter, u8 bgroup);
 
 int rtw_init_recv_priv(struct recv_priv *precvpriv, struct adapter *padapter);
 void rtw_free_recv_priv(struct recv_priv *precvpriv);
 
-int rtw_os_recv_resource_init(struct recv_priv *recvpr, struct adapter *adapt);
 int rtw_os_recv_resource_alloc(struct adapter *adapt,
 			       struct recv_frame *recvfr);
-void rtw_os_recv_resource_free(struct recv_priv *precvpriv);
 
 int rtw_os_recvbuf_resource_alloc(struct adapter *adapt, struct recv_buf *buf);
-int rtw_os_recvbuf_resource_free(struct adapter *adapt, struct recv_buf *buf);
 
 void rtw_os_read_port(struct adapter *padapter, struct recv_buf *precvbuf);
 
diff --git a/drivers/staging/rtl8188eu/include/rtl8188e_dm.h b/drivers/staging/rtl8188eu/include/rtl8188e_dm.h
index 97a3175250e..5e0ac31ef46 100644
--- a/drivers/staging/rtl8188eu/include/rtl8188e_dm.h
+++ b/drivers/staging/rtl8188eu/include/rtl8188e_dm.h
@@ -51,7 +51,6 @@ struct	dm_priv {
 };
 
 void rtl8188e_init_dm_priv(struct adapter *adapt);
-void rtl8188e_deinit_dm_priv(struct adapter *adapt);
 void rtl8188e_InitHalDm(struct adapter *adapt);
 void rtl8188e_HalDmWatchDog(struct adapter *adapt);
 
diff --git a/drivers/staging/rtl8188eu/include/rtl8188e_hal.h b/drivers/staging/rtl8188eu/include/rtl8188e_hal.h
index 75e41c4aeb2..fe3b4545cdb 100644
--- a/drivers/staging/rtl8188eu/include/rtl8188e_hal.h
+++ b/drivers/staging/rtl8188eu/include/rtl8188e_hal.h
@@ -471,6 +471,5 @@ void rtl8188e_stop_thread(struct adapter *padapter);
 void rtw_IOL_cmd_tx_pkt_buf_dump(struct adapter  *Adapter, int len);
 s32 rtl8188e_iol_efuse_patch(struct adapter *padapter);
 void rtw_cancel_all_timer(struct adapter *padapter);
-void _ps_open_RF(struct adapter *adapt);
 
 #endif /* __RTL8188E_HAL_H__ */
diff --git a/drivers/staging/rtl8188eu/include/rtl8188e_recv.h b/drivers/staging/rtl8188eu/include/rtl8188e_recv.h
index 07e5f522733..5fed30d389a 100644
--- a/drivers/staging/rtl8188eu/include/rtl8188e_recv.h
+++ b/drivers/staging/rtl8188eu/include/rtl8188e_recv.h
@@ -56,7 +56,6 @@ enum rx_packet_type {
 };
 
 #define INTERRUPT_MSG_FORMAT_LEN 60
-void rtl8188eu_init_recvbuf(struct adapter *padapter, struct recv_buf *buf);
 s32 rtl8188eu_init_recv_priv(struct adapter *padapter);
 void rtl8188eu_free_recv_priv(struct adapter *padapter);
 void rtl8188eu_recv_hdl(struct adapter *padapter, struct recv_buf *precvbuf);
diff --git a/drivers/staging/rtl8188eu/include/rtw_cmd.h b/drivers/staging/rtl8188eu/include/rtw_cmd.h
index 3d347029ff7..66467f711a5 100644
--- a/drivers/staging/rtl8188eu/include/rtw_cmd.h
+++ b/drivers/staging/rtl8188eu/include/rtw_cmd.h
@@ -807,7 +807,6 @@ u8 rtw_chk_hi_queue_cmd(struct adapter *padapter);
 u8 rtw_set_ch_cmd(struct adapter *padapter, u8 ch, u8 bw, u8 ch_offset,
 		  u8 enqueue);
 u8 rtw_set_chplan_cmd(struct adapter *padapter, u8 chplan, u8 enqueue);
-u8 rtw_led_blink_cmd(struct adapter *padapter, struct LED_871x *pLed);
 u8 rtw_set_csa_cmd(struct adapter *padapter, u8 new_ch_no);
 u8 rtw_tdls_cmd(struct adapter *padapter, u8 *addr, u8 option);
 
diff --git a/drivers/staging/rtl8188eu/include/rtw_led.h b/drivers/staging/rtl8188eu/include/rtw_led.h
index 0da4e27a70f..c5194b620da 100644
--- a/drivers/staging/rtl8188eu/include/rtw_led.h
+++ b/drivers/staging/rtl8188eu/include/rtw_led.h
@@ -23,92 +23,43 @@
 #include <osdep_service.h>
 #include <drv_types.h>
 
-#define MSECS(t)        (HZ * ((t) / 1000) + (HZ * ((t) % 1000)) / 1000)
-
-#define LED_BLINK_NORMAL_INTERVAL		100
-#define LED_BLINK_SLOWLY_INTERVAL		200
-#define LED_BLINK_LONG_INTERVAL			400
-
 #define LED_BLINK_NO_LINK_INTERVAL_ALPHA	1000
 #define LED_BLINK_LINK_INTERVAL_ALPHA		500	/* 500 */
 #define LED_BLINK_SCAN_INTERVAL_ALPHA		180	/* 150 */
 #define LED_BLINK_FASTER_INTERVAL_ALPHA		50
 #define LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA	5000
 
-#define LED_BLINK_NORMAL_INTERVAL_NETTRONIX	100
-#define LED_BLINK_SLOWLY_INTERVAL_NETTRONIX	2000
-
-#define LED_BLINK_SLOWLY_INTERVAL_PORNET	1000
-#define LED_BLINK_NORMAL_INTERVAL_PORNET	100
-
-#define LED_BLINK_FAST_INTERVAL_BITLAND		30
-
-/*  060403, rcnjko: Customized for AzWave. */
-#define LED_CM2_BLINK_ON_INTERVAL		250
-#define LED_CM2_BLINK_OFF_INTERVAL		4750
-
-#define LED_CM8_BLINK_INTERVAL			500	/* for QMI */
-#define LED_CM8_BLINK_OFF_INTERVAL		3750	/* for QMI */
-
-/*  080124, lanhsin: Customized for RunTop */
-#define LED_RunTop_BLINK_INTERVAL		300
-
-/*  060421, rcnjko: Customized for Sercomm Printer Server case. */
-#define LED_CM3_BLINK_INTERVAL			1500
-
 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,
-	LED_CTL_START_WPS_BOTTON = 11, /* added for runtop */
-	LED_CTL_STOP_WPS_FAIL = 12, /* added for ALPHA */
-	LED_CTL_STOP_WPS_FAIL_OVERLAP = 13, /* added for BELKIN */
-	LED_CTL_CONNECTION_NO_TRANSFER = 14,
+	LED_CTL_POWER_ON,
+	LED_CTL_LINK,
+	LED_CTL_NO_LINK,
+	LED_CTL_TX,
+	LED_CTL_RX ,
+	LED_CTL_SITE_SURVEY,
+	LED_CTL_POWER_OFF,
+	LED_CTL_START_TO_LINK,
+	LED_CTL_START_WPS,
+	LED_CTL_STOP_WPS,
+	LED_CTL_START_WPS_BOTTON,
+	LED_CTL_STOP_WPS_FAIL
 };
 
 enum LED_STATE_871x {
-	LED_UNKNOWN = 0,
-	RTW_LED_ON = 1,
-	RTW_LED_OFF = 2,
-	LED_BLINK_NORMAL = 3,
-	LED_BLINK_SLOWLY = 4,
-	LED_BLINK_POWER_ON = 5,
-	LED_BLINK_SCAN = 6, /*  LED is blinking during scanning period,
-			     * the # of times to blink is depend on time
-			     * for scanning. */
-	LED_BLINK_NO_LINK = 7, /*  LED is blinking during no link state. */
-	LED_BLINK_StartToBlink = 8,/*  Customzied for Sercomm Printer
-				    * Server case */
-	LED_BLINK_TXRX = 9,
-	LED_BLINK_WPS = 10,	/*  LED is blinkg during WPS communication */
-	LED_BLINK_WPS_STOP = 11,	/* for ALPHA */
-	LED_BLINK_WPS_STOP_OVERLAP = 12,	/* for BELKIN */
-	LED_BLINK_RUNTOP = 13, /*  Customized for RunTop */
-	LED_BLINK_CAMEO = 14,
-	LED_BLINK_XAVI = 15,
-	LED_BLINK_ALWAYS_ON = 16,
-};
-
-enum LED_PIN_871x {
-	LED_PIN_NULL = 0,
-	LED_PIN_LED0 = 1,
-	LED_PIN_LED1 = 2,
-	LED_PIN_LED2 = 3,
-	LED_PIN_GPIO0 = 4,
+	LED_UNKNOWN,
+	RTW_LED_ON,
+	RTW_LED_OFF,
+	LED_BLINK_NORMAL,
+	LED_BLINK_SLOWLY,
+	LED_BLINK_POWER_ON,
+	LED_BLINK_SCAN,
+	LED_BLINK_TXRX,
+	LED_BLINK_WPS,
+	LED_BLINK_WPS_STOP
 };
 
 struct LED_871x {
 	struct adapter *padapter;
 
-	enum LED_PIN_871x	LedPin;	/* Identify how to implement this
-					 * SW led. */
 	enum LED_STATE_871x	CurrLedState; /*  Current LED state. */
 	enum LED_STATE_871x	BlinkingLedState; /*  Next state for blinking,
 				   * either RTW_LED_ON or RTW_LED_OFF are. */
@@ -139,34 +90,11 @@ struct LED_871x {
 	((struct LED_871x *)_LED_871x)->CurrLedState == LED_BLINK_WPS_STOP || \
 	((struct LED_871x *)_LED_871x)->bLedWPSBlinkInProgress)
 
-#define IS_LED_BLINKING(_LED_871x)					\
-	(((struct LED_871x *)_LED_871x)->bLedWPSBlinkInProgress	||	\
-	((struct LED_871x *)_LED_871x)->bLedScanBlinkInProgress)
-
-/*  LED customization. */
-
-enum LED_STRATEGY_871x {
-	SW_LED_MODE0 = 0, /* SW control 1 LED via GPIO0. It is default option.*/
-	SW_LED_MODE1 = 1, /*  2 LEDs, through LED0 and LED1. For ALPHA. */
-	SW_LED_MODE2 = 2, /*  SW control 1 LED via GPIO0, customized for AzWave
-			   * 8187 minicard. */
-	SW_LED_MODE3 = 3, /*  SW control 1 LED via GPIO0, customized for Sercomm
-			   * Printer Server case. */
-	SW_LED_MODE4 = 4, /* for Edimax / Belkin */
-	SW_LED_MODE5 = 5, /* for Sercomm / Belkin */
-	SW_LED_MODE6 = 6, /* for 88CU minicard, porting from ce SW_LED_MODE7 */
-	HW_LED = 50, /*  HW control 2 LEDs, LED0 and LED1 (there are 4
-		      * different control modes, see MAC.CONFIG1 for details.)*/
-	LED_ST_NONE = 99,
-};
-
 void LedControl8188eu(struct adapter *padapter, enum LED_CTL_MODE	LedAction);
 
 struct led_priv{
 	/* add for led control */
 	struct LED_871x			SwLed0;
-	struct LED_871x			SwLed1;
-	enum LED_STRATEGY_871x	LedStrategy;
 	u8	bRegUseLed;
 	void (*LedControlHandler)(struct adapter *padapter,
 				  enum LED_CTL_MODE LedAction);
@@ -184,8 +112,7 @@ void BlinkWorkItemCallback(struct work_struct *work);
 
 void ResetLedStatus(struct LED_871x *pLed);
 
-void InitLed871x(struct adapter *padapter, struct LED_871x *pLed,
-		 enum LED_PIN_871x LedPin);
+void InitLed871x(struct adapter *padapter, struct LED_871x *pLed);
 
 void DeInitLed871x(struct LED_871x *pLed);
 
diff --git a/drivers/staging/rtl8188eu/include/rtw_pwrctrl.h b/drivers/staging/rtl8188eu/include/rtw_pwrctrl.h
index 9a42859d612..54dfbf07564 100644
--- a/drivers/staging/rtl8188eu/include/rtw_pwrctrl.h
+++ b/drivers/staging/rtl8188eu/include/rtw_pwrctrl.h
@@ -192,7 +192,7 @@ struct pwrctrl_priv {
 	u8	ips_mode_req;	/*  used to accept the mode setting request,
 				 *  will update to ipsmode later */
 	uint bips_processing;
-	u32 ips_deny_time; /* will deny IPS when system time less than this */
+	unsigned long ips_deny_time; /* will deny IPS when system time less than this */
 	u8 ps_processing; /* temp used to mark whether in rtw_ps_processor */
 
 	u8	bLeisurePs;
@@ -258,8 +258,6 @@ s32 LPS_RF_ON_check(struct adapter *adapter, u32 delay_ms);
 void LPS_Enter(struct adapter *adapter);
 void LPS_Leave(struct adapter *adapter);
 
-u8 rtw_interface_ps_func(struct adapter *adapter,
-			 enum hal_intf_ps_func efunc_id, u8 *val);
 void rtw_set_ips_deny(struct adapter *adapter, u32 ms);
 int _rtw_pwr_wakeup(struct adapter *adapter, u32 ips_defer_ms,
 		    const char *caller);
diff --git a/drivers/staging/rtl8188eu/include/rtw_recv.h b/drivers/staging/rtl8188eu/include/rtw_recv.h
index bcbce46cec8..f0c26ef8f66 100644
--- a/drivers/staging/rtl8188eu/include/rtw_recv.h
+++ b/drivers/staging/rtl8188eu/include/rtw_recv.h
@@ -175,7 +175,6 @@ recv_thread(passive) ; returnpkt(dispatch)
 using enter_critical section to protect
 */
 struct recv_priv {
-	spinlock_t lock;
 	struct __queue free_recv_queue;
 	struct __queue recv_pending_queue;
 	struct __queue uc_swdec_pending_queue;
@@ -189,11 +188,6 @@ struct recv_priv {
 	u64	rx_drop;
 	u64	last_rx_bytes;
 
-	uint  rx_icv_err;
-	uint  rx_largepacket_crcerr;
-	uint  rx_smallpacket_crcerr;
-	uint  rx_middlepacket_crcerr;
-	struct semaphore allrxreturnevt;
 	uint	ff_hwaddr;
 	u8	rx_pending_cnt;
 
@@ -213,9 +207,7 @@ struct recv_priv {
 	u8 signal_strength;
 	u8 signal_qual;
 	u8 noise;
-	int RxSNRdB[2];
 	s8 RxRssi[2];
-	int FalseAlmCnt_all;
 
 	struct timer_list signal_stat_timer;
 	u32 signal_stat_sampling_interval;
@@ -235,22 +227,8 @@ struct sta_recv_priv {
 };
 
 struct recv_buf {
-	struct list_head list;
-	spinlock_t recvbuf_lock;
-	u32	ref_cnt;
 	struct adapter *adapter;
-	u8	*pbuf;
-	u8	*pallocated_buf;
-	u32	len;
-	u8	*phead;
-	u8	*pdata;
-	u8	*ptail;
-	u8	*pend;
 	struct urb *purb;
-	dma_addr_t dma_transfer_addr;	/* (in) dma addr for transfer_buffer */
-	u32 alloc_sz;
-	u8  irp_pending;
-	int  transfer_len;
 	struct sk_buff *pskb;
 	u8	reuse;
 };
@@ -275,15 +253,12 @@ struct recv_frame {
 	struct sk_buff	 *pkt;
 	struct sk_buff	 *pkt_newalloc;
 	struct adapter  *adapter;
-	u8 fragcnt;
-	int frame_tag;
 	struct rx_pkt_attrib attrib;
 	uint  len;
 	u8 *rx_head;
 	u8 *rx_data;
 	u8 *rx_tail;
 	u8 *rx_end;
-	void *precvbuf;
 	struct sta_info *psta;
 	/* for A-MPDU Rx reordering buffer control */
 	struct recv_reorder_ctrl *preorder_ctrl;
@@ -302,9 +277,6 @@ int rtw_enqueue_recvframe(struct recv_frame *precvframe, struct __queue *queue);
 void rtw_free_recvframe_queue(struct __queue *pframequeue,
 			      struct __queue *pfree_recv_queue);
 u32 rtw_free_uc_swdec_pending_queue(struct adapter *adapter);
-int rtw_enqueue_recvbuf_to_head(struct recv_buf *buf, struct __queue *queue);
-int rtw_enqueue_recvbuf(struct recv_buf *precvbuf, struct __queue *queue);
-struct recv_buf *rtw_dequeue_recvbuf(struct __queue *queue);
 
 void rtw_reordering_ctrl_timeout_handler(void *pcontext);
 
@@ -316,31 +288,6 @@ static inline u8 *get_rxmem(struct recv_frame *precvframe)
 	return precvframe->rx_head;
 }
 
-static inline u8 *get_rx_status(struct recv_frame *precvframe)
-{
-	return get_rxmem(precvframe);
-}
-
-static inline u8 *recvframe_push(struct recv_frame *precvframe, int sz)
-{
-	/*  append data before rx_data */
-
-	/* add data to the start of recv_frame
- *
- *      This function extends the used data area of the recv_frame at the buffer
- *      start. rx_data must be still larger than rx_head, after pushing.
- */
-	if (precvframe == NULL)
-		return NULL;
-	precvframe->rx_data -= sz;
-	if (precvframe->rx_data < precvframe->rx_head) {
-		precvframe->rx_data += sz;
-		return NULL;
-	}
-	precvframe->len += sz;
-	return precvframe->rx_data;
-}
-
 static inline u8 *recvframe_pull(struct recv_frame *precvframe, int sz)
 {
 	/*  rx_data += sz; move rx_data sz bytes  hereafter */
diff --git a/drivers/staging/rtl8188eu/include/usb_ops.h b/drivers/staging/rtl8188eu/include/usb_ops.h
index 7d33477d551..a290e0ff306 100644
--- a/drivers/staging/rtl8188eu/include/usb_ops.h
+++ b/drivers/staging/rtl8188eu/include/usb_ops.h
@@ -63,7 +63,6 @@ enum{
 #include <usb_ops_linux.h>
 
 void rtl8188eu_set_hw_type(struct adapter *padapter);
-#define hal_set_hw_type rtl8188eu_set_hw_type
 void rtl8188eu_set_intf_ops(struct _io_ops *pops);
 #define usb_set_intf_ops rtl8188eu_set_intf_ops
 
diff --git a/drivers/staging/rtl8188eu/include/xmit_osdep.h b/drivers/staging/rtl8188eu/include/xmit_osdep.h
index 2ff622ba24f..13965f2489d 100644
--- a/drivers/staging/rtl8188eu/include/xmit_osdep.h
+++ b/drivers/staging/rtl8188eu/include/xmit_osdep.h
@@ -32,10 +32,6 @@ struct pkt_file {
 	size_t buf_len;
 };
 
-extern int rtw_ht_enable;
-extern int rtw_cbw40_enable;
-extern int rtw_ampdu_enable;/* for enable tx_ampdu */
-
 #define NR_XMITFRAME	256
 
 struct xmit_priv;
@@ -53,8 +49,6 @@ int rtw_os_xmit_resource_alloc(struct adapter *padapter,
 void rtw_os_xmit_resource_free(struct adapter *padapter,
 			       struct xmit_buf *pxmitbuf, u32 free_sz);
 
-void rtw_set_tx_chksum_offload(struct sk_buff *pkt, struct pkt_attrib *pattrib);
-
 uint rtw_remainder_len(struct pkt_file *pfile);
 void _rtw_open_pktfile(struct sk_buff *pkt, struct pkt_file *pfile);
 uint _rtw_pktfile_read(struct pkt_file *pfile, u8 *rmem, uint rlen);
diff --git a/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c b/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c
index cf30a08912d..f04aaa375f0 100644
--- a/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c
+++ b/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c
@@ -2097,7 +2097,8 @@ static int rtw_wx_set_enc_ext(struct net_device *dev,
 		alg_name = "CCMP";
 		break;
 	default:
-		return -1;
+		ret = -1;
+		goto exit;
 	}
 
 	strncpy((char *)param->u.crypt.alg, alg_name, IEEE_CRYPT_ALG_NAME_LEN);
@@ -2124,6 +2125,7 @@ static int rtw_wx_set_enc_ext(struct net_device *dev,
 
 	ret =  wpa_set_encryption(dev, param, param_len);
 
+exit:
 	kfree(param);
 	return ret;
 }
@@ -2154,6 +2156,7 @@ static int rtw_wx_read32(struct net_device *dev,
 	u32 bytes;
 	u8 *ptmp;
 	int rv;
+	int ret = 0;
 
 	padapter = (struct adapter *)rtw_netdev_priv(dev);
 	p = &wrqu->data;
@@ -2163,16 +2166,16 @@ static int rtw_wx_read32(struct net_device *dev,
 		return -ENOMEM;
 
 	if (copy_from_user(ptmp, p->pointer, len)) {
-		kfree(ptmp);
-		return -EFAULT;
+		ret = -EFAULT;
+		goto exit;
 	}
 
 	bytes = 0;
 	addr = 0;
 	rv = sscanf(ptmp, "%d,%x", &bytes, &addr);
 	if (rv != 2) {
-		kfree(ptmp);
-		return -EINVAL;
+		ret = -EINVAL;
+		goto exit;
 	}
 
 	switch (bytes) {
@@ -2190,12 +2193,14 @@ static int rtw_wx_read32(struct net_device *dev,
 		break;
 	default:
 		DBG_88E(KERN_INFO "%s: usage> read [bytes],[address(hex)]\n", __func__);
-		return -EINVAL;
+		ret = -EINVAL;
+		goto exit;
 	}
 	DBG_88E(KERN_INFO "%s: addr = 0x%08X data =%s\n", __func__, addr, extra);
 
+exit:
 	kfree(ptmp);
-	return 0;
+	return ret;
 }
 
 static int rtw_wx_write32(struct net_device *dev,
@@ -7114,15 +7119,15 @@ static int rtw_mp_pwrtrk(struct net_device *dev,
 {
 	u8 enable;
 	u32 thermal;
-	s32 ret;
 	struct adapter *padapter = rtw_netdev_priv(dev);
 	char	*input = kmalloc(wrqu->length, GFP_KERNEL);
+	int ret = 0;
 
 	if (!input)
 		return -ENOMEM;
 	if (copy_from_user(input, wrqu->pointer, wrqu->length)) {
-		kfree(input);
-		return -EFAULT;
+		ret = -EFAULT;
+		goto exit;
 	}
 	_rtw_memset(extra, 0, wrqu->length);
 
@@ -7133,22 +7138,28 @@ static int rtw_mp_pwrtrk(struct net_device *dev,
 			sprintf(extra, "mp tx power tracking stop");
 		} else if (sscanf(input, "ther =%d", &thermal)) {
 				ret = Hal_SetThermalMeter(padapter, (u8)thermal);
-				if (ret == _FAIL)
-					return -EPERM;
+				if (ret == _FAIL) {
+					ret = -EPERM;
+					goto exit;
+				}
 				sprintf(extra, "mp tx power tracking start, target value =%d ok ", thermal);
 		} else {
-			kfree(input);
-			return -EINVAL;
+			ret = -EINVAL;
+			goto exit;
 		}
 	}
 
-	kfree(input);
 	ret = Hal_SetPowerTracking(padapter, enable);
-	if (ret == _FAIL)
-		return -EPERM;
+	if (ret == _FAIL) {
+		ret = -EPERM;
+		goto exit;
+	}
 
 	wrqu->length = strlen(extra);
-	return 0;
+
+exit:
+	kfree(input);
+	return ret;
 }
 
 static int rtw_mp_psd(struct net_device *dev,
@@ -7316,11 +7327,14 @@ static int rtw_mp_SetRFPath(struct net_device *dev,
 	struct adapter *padapter = rtw_netdev_priv(dev);
 	char	*input = kmalloc(wrqu->data.length, GFP_KERNEL);
 	u8 bMain = 1, bTurnoff = 1;
+	int ret = 0;
 
 	if (!input)
 		return -ENOMEM;
-	if (copy_from_user(input, wrqu->data.pointer, wrqu->data.length))
-			return -EFAULT;
+	if (copy_from_user(input, wrqu->data.pointer, wrqu->data.length)) {
+		ret = -EFAULT;
+		goto exit;
+	}
 	DBG_88E("%s:iwpriv in =%s\n", __func__, input);
 
 	bMain = strncmp(input, "1", 2); /*  strncmp true is 0 */
@@ -7333,8 +7347,10 @@ static int rtw_mp_SetRFPath(struct net_device *dev,
 		MP_PHY_SetRFPathSwitch(padapter, false);
 		DBG_88E("%s:PHY_SetRFPathSwitch = false\n", __func__);
 	}
+
+exit:
 	kfree(input);
-	return 0;
+	return ret;
 }
 
 static int rtw_mp_QueryDrv(struct net_device *dev,
@@ -7345,12 +7361,15 @@ static int rtw_mp_QueryDrv(struct net_device *dev,
 	char	*input = kmalloc(wrqu->data.length, GFP_KERNEL);
 	u8 qAutoLoad = 1;
 	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter);
+	int ret = 0;
 
 	if (!input)
 		return -ENOMEM;
 
-	if (copy_from_user(input, wrqu->data.pointer, wrqu->data.length))
-			return -EFAULT;
+	if (copy_from_user(input, wrqu->data.pointer, wrqu->data.length)) {
+		ret = -EFAULT;
+		goto exit;
+	}
 	DBG_88E("%s:iwpriv in =%s\n", __func__, input);
 
 	qAutoLoad = strncmp(input, "autoload", 8); /*  strncmp true is 0 */
@@ -7364,8 +7383,10 @@ static int rtw_mp_QueryDrv(struct net_device *dev,
 		sprintf(extra, "ok");
 	}
 	wrqu->data.length = strlen(extra) + 1;
+
+exit:
 	kfree(input);
-	return 0;
+	return ret;
 }
 
 static int rtw_mp_set(struct net_device *dev,
diff --git a/drivers/staging/rtl8188eu/os_dep/os_intfs.c b/drivers/staging/rtl8188eu/os_dep/os_intfs.c
index b225d1c0721..0e0c32d4643 100644
--- a/drivers/staging/rtl8188eu/os_dep/os_intfs.c
+++ b/drivers/staging/rtl8188eu/os_dep/os_intfs.c
@@ -84,9 +84,9 @@ static int rtw_uapsd_acbe_en;
 static int rtw_uapsd_acvi_en;
 static int rtw_uapsd_acvo_en;
 
-int rtw_ht_enable = 1;
-int rtw_cbw40_enable = 3; /*  0 :disable, bit(0): enable 2.4g, bit(1): enable 5g */
-int rtw_ampdu_enable = 1;/* for enable tx_ampdu */
+static int rtw_ht_enable = 1;
+static int rtw_cbw40_enable = 3; /*  0 :disable, bit(0): enable 2.4g, bit(1): enable 5g */
+static int rtw_ampdu_enable = 1;/* for enable tx_ampdu */
 static int rtw_rx_stbc = 1;/*  0: disable, bit(0):enable 2.4g, bit(1):enable 5g, default is set to enable 2.4GHZ for IOT issue with bufflao's AP at 5GHZ */
 static int rtw_ampdu_amsdu;/*  0: disabled, 1:enabled, 2:auto */
 
@@ -951,8 +951,6 @@ void rtw_cancel_all_timer(struct adapter *padapter)
 	_cancel_timer_ex(&padapter->pwrctrlpriv.pwr_state_check_timer);
 
 	_cancel_timer_ex(&padapter->recvpriv.signal_stat_timer);
-	/* cancel dm timer */
-	rtw_hal_dm_deinit(padapter);
 }
 
 u8 rtw_free_drv_sw(struct adapter *padapter)
@@ -1084,9 +1082,9 @@ int _netdev_open(struct net_device *pnetdev)
 	rtw_set_pwr_state_check_timer(&padapter->pwrctrlpriv);
 
 	if (!rtw_netif_queue_stopped(pnetdev))
-		rtw_netif_start_queue(pnetdev);
+		netif_tx_start_all_queues(pnetdev);
 	else
-		rtw_netif_wake_queue(pnetdev);
+		netif_tx_wake_all_queues(pnetdev);
 
 	netdev_br_init(pnetdev);
 
@@ -1098,7 +1096,7 @@ netdev_open_normal_process:
 netdev_open_error:
 	padapter->bup = false;
 	netif_carrier_off(pnetdev);
-	rtw_netif_stop_queue(pnetdev);
+	netif_tx_stop_all_queues(pnetdev);
 	RT_TRACE(_module_os_intfs_c_, _drv_err_, ("-88eu_drv - dev_open, fail!\n"));
 	DBG_88E("-88eu_drv - drv_open fail, bup =%d\n", padapter->bup);
 	return -1;
@@ -1221,7 +1219,7 @@ int netdev_close(struct net_device *pnetdev)
 		/* s1. */
 		if (pnetdev) {
 			if (!rtw_netif_queue_stopped(pnetdev))
-				rtw_netif_stop_queue(pnetdev);
+				netif_tx_stop_all_queues(pnetdev);
 		}
 
 		/* s2. */
diff --git a/drivers/staging/rtl8188eu/os_dep/recv_linux.c b/drivers/staging/rtl8188eu/os_dep/recv_linux.c
index da397e4c677..c0fa8fdb9d9 100644
--- a/drivers/staging/rtl8188eu/os_dep/recv_linux.c
+++ b/drivers/staging/rtl8188eu/os_dep/recv_linux.c
@@ -28,13 +28,6 @@
 #include <osdep_intf.h>
 #include <usb_ops.h>
 
-/* init os related resource in struct recv_priv */
-int rtw_os_recv_resource_init(struct recv_priv *precvpriv,
-			      struct adapter *padapter)
-{
-	return _SUCCESS;
-}
-
 /* alloc os related resource in struct recv_frame */
 int rtw_os_recv_resource_alloc(struct adapter *padapter,
 			       struct recv_frame *precvframe)
@@ -44,42 +37,20 @@ int rtw_os_recv_resource_alloc(struct adapter *padapter,
 	return _SUCCESS;
 }
 
-/* free os related resource in struct recv_frame */
-void rtw_os_recv_resource_free(struct recv_priv *precvpriv)
-{
-}
-
 /* alloc os related resource in struct recv_buf */
 int rtw_os_recvbuf_resource_alloc(struct adapter *padapter,
 				  struct recv_buf *precvbuf)
 {
 	int res = _SUCCESS;
 
-	precvbuf->irp_pending = false;
 	precvbuf->purb = usb_alloc_urb(0, GFP_KERNEL);
 	if (precvbuf->purb == NULL)
 		res = _FAIL;
 	precvbuf->pskb = NULL;
 	precvbuf->reuse = false;
-	precvbuf->pallocated_buf = NULL;
-	precvbuf->pbuf = NULL;
-	precvbuf->pdata = NULL;
-	precvbuf->phead = NULL;
-	precvbuf->ptail = NULL;
-	precvbuf->pend = NULL;
-	precvbuf->transfer_len = 0;
-	precvbuf->len = 0;
 	return res;
 }
 
-/* free os related resource in struct recv_buf */
-int rtw_os_recvbuf_resource_free(struct adapter *padapter,
-				 struct recv_buf *precvbuf)
-{
-	usb_free_urb(precvbuf->purb);
-	return _SUCCESS;
-}
-
 void rtw_handle_tkip_mic_err(struct adapter *padapter, u8 bgroup)
 {
 	union iwreq_data wrqu;
@@ -116,11 +87,6 @@ void rtw_handle_tkip_mic_err(struct adapter *padapter, u8 bgroup)
 			    &wrqu, (char *)&ev);
 }
 
-void rtw_hostapd_mlme_rx(struct adapter *padapter,
-			 struct recv_frame *precv_frame)
-{
-}
-
 int rtw_recv_indicatepkt(struct adapter *padapter,
 			 struct recv_frame *precv_frame)
 {
@@ -229,14 +195,12 @@ void rtw_os_read_port(struct adapter *padapter, struct recv_buf *precvbuf)
 {
 	struct recv_priv *precvpriv = &padapter->recvpriv;
 
-	precvbuf->ref_cnt--;
 	/* free skb in recv_buf */
 	dev_kfree_skb_any(precvbuf->pskb);
 	precvbuf->pskb = NULL;
 	precvbuf->reuse = false;
-	if (!precvbuf->irp_pending)
-		rtw_read_port(padapter, precvpriv->ff_hwaddr, 0,
-			      (unsigned char *)precvbuf);
+	rtw_read_port(padapter, precvpriv->ff_hwaddr, 0,
+			(unsigned char *)precvbuf);
 }
 
 static void _rtw_reordering_ctrl_timeout_handler(void *func_context)
diff --git a/drivers/staging/rtl8188eu/os_dep/usb_intf.c b/drivers/staging/rtl8188eu/os_dep/usb_intf.c
index 2e49cd58321..7526b989dcb 100644
--- a/drivers/staging/rtl8188eu/os_dep/usb_intf.c
+++ b/drivers/staging/rtl8188eu/os_dep/usb_intf.c
@@ -62,10 +62,6 @@ static struct usb_device_id rtw_usb_id_tbl[] = {
 
 MODULE_DEVICE_TABLE(usb, rtw_usb_id_tbl);
 
-static struct specific_device_id specific_device_id_tbl[] = {
-	{}		/* empty table for now */
-};
-
 struct rtw_usb_drv {
 	struct usb_driver usbdrv;
 	int drv_registered;
@@ -84,46 +80,6 @@ static struct rtw_usb_drv rtl8188e_usb_drv = {
 
 static struct rtw_usb_drv *usb_drv = &rtl8188e_usb_drv;
 
-static inline int RT_usb_endpoint_dir_in(const struct usb_endpoint_descriptor *epd)
-{
-	return (epd->bEndpointAddress & USB_ENDPOINT_DIR_MASK) == USB_DIR_IN;
-}
-
-static inline int RT_usb_endpoint_dir_out(const struct usb_endpoint_descriptor *epd)
-{
-	return (epd->bEndpointAddress & USB_ENDPOINT_DIR_MASK) == USB_DIR_OUT;
-}
-
-static inline int RT_usb_endpoint_xfer_int(const struct usb_endpoint_descriptor *epd)
-{
-	return (epd->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK) == USB_ENDPOINT_XFER_INT;
-}
-
-static inline int RT_usb_endpoint_xfer_bulk(const struct usb_endpoint_descriptor *epd)
-{
-	return (epd->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK) == USB_ENDPOINT_XFER_BULK;
-}
-
-static inline int RT_usb_endpoint_is_bulk_in(const struct usb_endpoint_descriptor *epd)
-{
-	return RT_usb_endpoint_xfer_bulk(epd) && RT_usb_endpoint_dir_in(epd);
-}
-
-static inline int RT_usb_endpoint_is_bulk_out(const struct usb_endpoint_descriptor *epd)
-{
-	return RT_usb_endpoint_xfer_bulk(epd) && RT_usb_endpoint_dir_out(epd);
-}
-
-static inline int usb_endpoint_is_int(const struct usb_endpoint_descriptor *epd)
-{
-	return RT_usb_endpoint_xfer_int(epd) && RT_usb_endpoint_dir_in(epd);
-}
-
-static inline int RT_usb_endpoint_num(const struct usb_endpoint_descriptor *epd)
-{
-	return epd->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK;
-}
-
 static u8 rtw_init_intf_priv(struct dvobj_priv *dvobj)
 {
 	u8 rst = _SUCCESS;
@@ -132,7 +88,7 @@ static u8 rtw_init_intf_priv(struct dvobj_priv *dvobj)
 
 	dvobj->usb_alloc_vendor_req_buf = rtw_zmalloc(MAX_USB_IO_CTL_SIZE);
 	if (dvobj->usb_alloc_vendor_req_buf == NULL) {
-		DBG_88E("alloc usb_vendor_req_buf failed... /n");
+		DBG_88E("alloc usb_vendor_req_buf failed...\n");
 		rst = _FAIL;
 		goto exit;
 	}
@@ -187,60 +143,35 @@ static struct dvobj_priv *usb_dvobj_init(struct usb_interface *usb_intf)
 	pdvobjpriv->nr_endpoint = piface_desc->bNumEndpoints;
 
 	for (i = 0; i < pdvobjpriv->nr_endpoint; i++) {
+		int ep_num;
 		phost_endp = phost_iface->endpoint + i;
+
 		if (phost_endp) {
 			pendp_desc = &phost_endp->desc;
+			ep_num = usb_endpoint_num(pendp_desc);
 
-			DBG_88E("\nusb_endpoint_descriptor(%d):\n", i);
-			DBG_88E("bLength=%x\n", pendp_desc->bLength);
-			DBG_88E("bDescriptorType=%x\n",
-				pendp_desc->bDescriptorType);
-			DBG_88E("bEndpointAddress=%x\n",
-				pendp_desc->bEndpointAddress);
-			DBG_88E("wMaxPacketSize=%d\n",
-				le16_to_cpu(pendp_desc->wMaxPacketSize));
-			DBG_88E("bInterval=%x\n", pendp_desc->bInterval);
-
-			if (RT_usb_endpoint_is_bulk_in(pendp_desc)) {
-				DBG_88E("RT_usb_endpoint_is_bulk_in = %x\n",
-					RT_usb_endpoint_num(pendp_desc));
-				pdvobjpriv->RtInPipe[pdvobjpriv->RtNumInPipes] = RT_usb_endpoint_num(pendp_desc);
+			if (usb_endpoint_is_bulk_in(pendp_desc)) {
+				pdvobjpriv->RtInPipe[pdvobjpriv->RtNumInPipes] = ep_num;
 				pdvobjpriv->RtNumInPipes++;
-			} else if (usb_endpoint_is_int(pendp_desc)) {
-				DBG_88E("usb_endpoint_is_int = %x, Interval = %x\n",
-					RT_usb_endpoint_num(pendp_desc),
-					pendp_desc->bInterval);
-				pdvobjpriv->RtInPipe[pdvobjpriv->RtNumInPipes] = RT_usb_endpoint_num(pendp_desc);
+			} else if (usb_endpoint_is_int_in(pendp_desc)) {
+				pdvobjpriv->RtInPipe[pdvobjpriv->RtNumInPipes] = ep_num;
 				pdvobjpriv->RtNumInPipes++;
-			} else if (RT_usb_endpoint_is_bulk_out(pendp_desc)) {
-				DBG_88E("RT_usb_endpoint_is_bulk_out = %x\n",
-					RT_usb_endpoint_num(pendp_desc));
-				pdvobjpriv->RtOutPipe[pdvobjpriv->RtNumOutPipes] = RT_usb_endpoint_num(pendp_desc);
+			} else if (usb_endpoint_is_bulk_out(pendp_desc)) {
+				pdvobjpriv->RtOutPipe[pdvobjpriv->RtNumOutPipes] = ep_num;
 				pdvobjpriv->RtNumOutPipes++;
 			}
-			pdvobjpriv->ep_num[i] = RT_usb_endpoint_num(pendp_desc);
+			pdvobjpriv->ep_num[i] = ep_num;
 		}
 	}
 
-	DBG_88E("nr_endpoint=%d, in_num=%d, out_num=%d\n\n",
-		pdvobjpriv->nr_endpoint, pdvobjpriv->RtNumInPipes,
-		pdvobjpriv->RtNumOutPipes);
-
-	if (pusbd->speed == USB_SPEED_HIGH) {
+	if (pusbd->speed == USB_SPEED_HIGH)
 		pdvobjpriv->ishighspeed = true;
-		DBG_88E("USB_SPEED_HIGH\n");
-	} else {
+	else
 		pdvobjpriv->ishighspeed = false;
-		DBG_88E("NON USB_SPEED_HIGH\n");
-	}
 
-	if (rtw_init_intf_priv(pdvobjpriv) == _FAIL) {
-		RT_TRACE(_module_os_intfs_c_, _drv_err_,
-			 ("\n Can't INIT rtw_init_intf_priv\n"));
+	if (rtw_init_intf_priv(pdvobjpriv) == _FAIL)
 		goto free_dvobj;
-	}
 
-	/* 3 misc */
 	sema_init(&(pdvobjpriv->usb_suspend_sema), 0);
 	rtw_reset_continual_urb_error(pdvobjpriv);
 
@@ -288,13 +219,6 @@ static void usb_dvobj_deinit(struct usb_interface *usb_intf)
 
 }
 
-static void chip_by_usb_id(struct adapter *padapter,
-			   const struct usb_device_id *pdid)
-{
-	padapter->chip_type = NULL_CHIP_TYPE;
-	hal_set_hw_type(padapter);
-}
-
 static void usb_intf_start(struct adapter *padapter)
 {
 	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("+usb_intf_start\n"));
@@ -360,28 +284,6 @@ static void rtw_dev_unload(struct adapter *padapter)
 	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("-rtw_dev_unload\n"));
 }
 
-static void process_spec_devid(const struct usb_device_id *pdid)
-{
-	u16 vid, pid;
-	u32 flags;
-	int i;
-	int num = sizeof(specific_device_id_tbl) /
-		  sizeof(struct specific_device_id);
-
-	for (i = 0; i < num; i++) {
-		vid = specific_device_id_tbl[i].idVendor;
-		pid = specific_device_id_tbl[i].idProduct;
-		flags = specific_device_id_tbl[i].flags;
-
-		if ((pdid->idVendor == vid) && (pdid->idProduct == pid) &&
-		    (flags&SPEC_DEV_ID_DISABLE_HT)) {
-			rtw_ht_enable = 0;
-			rtw_cbw40_enable = 0;
-			rtw_ampdu_enable = 0;
-		}
-	}
-}
-
 int rtw_hw_suspend(struct adapter *padapter)
 {
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
@@ -396,49 +298,47 @@ int rtw_hw_suspend(struct adapter *padapter)
 		goto error_exit;
 	}
 
-	if (padapter) { /* system suspend */
-		LeaveAllPowerSaveMode(padapter);
+	/* system suspend */
+	LeaveAllPowerSaveMode(padapter);
 
-		DBG_88E("==> rtw_hw_suspend\n");
-		_enter_pwrlock(&pwrpriv->lock);
-		pwrpriv->bips_processing = true;
-		/* s1. */
-		if (pnetdev) {
-			netif_carrier_off(pnetdev);
-			rtw_netif_stop_queue(pnetdev);
-		}
+	DBG_88E("==> rtw_hw_suspend\n");
+	_enter_pwrlock(&pwrpriv->lock);
+	pwrpriv->bips_processing = true;
+	/* s1. */
+	if (pnetdev) {
+		netif_carrier_off(pnetdev);
+		netif_tx_stop_all_queues(pnetdev);
+	}
 
-		/* s2. */
-		rtw_disassoc_cmd(padapter, 500, false);
+	/* s2. */
+	rtw_disassoc_cmd(padapter, 500, false);
 
-		/* s2-2.  indicate disconnect to os */
-		{
-			struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	/* s2-2.  indicate disconnect to os */
+	{
+		struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
-				_clr_fwstate_(pmlmepriv, _FW_LINKED);
+		if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+			_clr_fwstate_(pmlmepriv, _FW_LINKED);
 
-				rtw_led_control(padapter, LED_CTL_NO_LINK);
+			rtw_led_control(padapter, LED_CTL_NO_LINK);
 
-				rtw_os_indicate_disconnect(padapter);
+			rtw_os_indicate_disconnect(padapter);
 
-				/* donnot enqueue cmd */
-				rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_DISCONNECT, 0);
-			}
+			/* donnot enqueue cmd */
+			rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_DISCONNECT, 0);
 		}
-		/* s2-3. */
-		rtw_free_assoc_resources(padapter, 1);
+	}
+	/* s2-3. */
+	rtw_free_assoc_resources(padapter, 1);
 
-		/* s2-4. */
-		rtw_free_network_queue(padapter, true);
-		rtw_ips_dev_unload(padapter);
-		pwrpriv->rf_pwrstate = rf_off;
-		pwrpriv->bips_processing = false;
+	/* s2-4. */
+	rtw_free_network_queue(padapter, true);
+	rtw_ips_dev_unload(padapter);
+	pwrpriv->rf_pwrstate = rf_off;
+	pwrpriv->bips_processing = false;
+
+	_exit_pwrlock(&pwrpriv->lock);
 
-		_exit_pwrlock(&pwrpriv->lock);
-	} else {
-		goto error_exit;
-	}
 	return 0;
 
 error_exit:
@@ -452,35 +352,32 @@ int rtw_hw_resume(struct adapter *padapter)
 	struct net_device *pnetdev = padapter->pnetdev;
 
 
-	if (padapter) { /* system resume */
-		DBG_88E("==> rtw_hw_resume\n");
-		_enter_pwrlock(&pwrpriv->lock);
-		pwrpriv->bips_processing = true;
-		rtw_reset_drv_sw(padapter);
+	/* system resume */
+	DBG_88E("==> rtw_hw_resume\n");
+	_enter_pwrlock(&pwrpriv->lock);
+	pwrpriv->bips_processing = true;
+	rtw_reset_drv_sw(padapter);
 
-		if (pm_netdev_open(pnetdev, false) != 0) {
-			_exit_pwrlock(&pwrpriv->lock);
-			goto error_exit;
-		}
+	if (pm_netdev_open(pnetdev, false) != 0) {
+		_exit_pwrlock(&pwrpriv->lock);
+		goto error_exit;
+	}
 
-		netif_device_attach(pnetdev);
-		netif_carrier_on(pnetdev);
+	netif_device_attach(pnetdev);
+	netif_carrier_on(pnetdev);
 
-		if (!netif_queue_stopped(pnetdev))
-			netif_start_queue(pnetdev);
-		else
-			netif_wake_queue(pnetdev);
+	if (!netif_queue_stopped(pnetdev))
+		netif_start_queue(pnetdev);
+	else
+		netif_wake_queue(pnetdev);
 
-		pwrpriv->bkeepfwalive = false;
-		pwrpriv->brfoffbyhw = false;
+	pwrpriv->bkeepfwalive = false;
+	pwrpriv->brfoffbyhw = false;
 
-		pwrpriv->rf_pwrstate = rf_on;
-		pwrpriv->bips_processing = false;
+	pwrpriv->rf_pwrstate = rf_on;
+	pwrpriv->bips_processing = false;
 
-		_exit_pwrlock(&pwrpriv->lock);
-	} else {
-		goto error_exit;
-	}
+	_exit_pwrlock(&pwrpriv->lock);
 
 
 	return 0;
@@ -519,7 +416,7 @@ static int rtw_suspend(struct usb_interface *pusb_intf, pm_message_t message)
 	/* s1. */
 	if (pnetdev) {
 		netif_carrier_off(pnetdev);
-		rtw_netif_stop_queue(pnetdev);
+		netif_tx_stop_all_queues(pnetdev);
 	}
 
 	/* s2. */
@@ -591,8 +488,7 @@ int rtw_resume_process(struct adapter *padapter)
 
 	_enter_pwrlock(&pwrpriv->lock);
 	rtw_reset_drv_sw(padapter);
-	if (pwrpriv)
-		pwrpriv->bkeepfwalive = false;
+	pwrpriv->bkeepfwalive = false;
 
 	DBG_88E("bkeepfwalive(%x)\n", pwrpriv->bkeepfwalive);
 	if (pm_netdev_open(pnetdev, true) != 0)
@@ -643,18 +539,12 @@ static struct adapter *rtw_usb_if1_init(struct dvobj_priv *dvobj,
 	dvobj->if1 = padapter;
 
 	padapter->bDriverStopped = true;
-
 	padapter->hw_init_mutex = &usb_drv->hw_init_mutex;
-
-	/* step 1-1., decide the chip_type via vid/pid */
-	chip_by_usb_id(padapter, pdid);
-
-	if (rtw_handle_dualmac(padapter, 1) != _SUCCESS)
-		goto free_adapter;
+	padapter->chip_type = RTL8188E;
 
 	pnetdev = rtw_init_netdev(padapter);
 	if (pnetdev == NULL)
-		goto handle_dualmac;
+		goto free_adapter;
 	SET_NETDEV_DEV(pnetdev, dvobj_to_dev(dvobj));
 	padapter = rtw_netdev_priv(pnetdev);
 
@@ -728,9 +618,6 @@ static struct adapter *rtw_usb_if1_init(struct dvobj_priv *dvobj,
 free_hal_data:
 	if (status != _SUCCESS)
 		kfree(padapter->HalData);
-handle_dualmac:
-	if (status != _SUCCESS)
-		rtw_handle_dualmac(padapter, 0);
 free_adapter:
 	if (status != _SUCCESS) {
 		if (pnetdev)
@@ -767,7 +654,6 @@ static void rtw_usb_if1_deinit(struct adapter *if1)
 	rtw_dev_unload(if1);
 	DBG_88E("+r871xu_dev_remove, hw_init_completed=%d\n",
 		if1->hw_init_completed);
-	rtw_handle_dualmac(if1, 0);
 	rtw_free_drv_sw(if1);
 	if (pnetdev)
 		rtw_free_netdev(pnetdev);
@@ -776,14 +662,11 @@ static void rtw_usb_if1_deinit(struct adapter *if1)
 static int rtw_drv_init(struct usb_interface *pusb_intf, const struct usb_device_id *pdid)
 {
 	struct adapter *if1 = NULL;
-	int status;
+	int status = _FAIL;
 	struct dvobj_priv *dvobj;
 
 	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("+rtw_drv_init\n"));
 
-	/* step 0. */
-	process_spec_devid(pdid);
-
 	/* Initialize dvobj_priv */
 	dvobj = usb_dvobj_init(pusb_intf);
 	if (dvobj == NULL) {
@@ -807,8 +690,6 @@ static int rtw_drv_init(struct usb_interface *pusb_intf, const struct usb_device
 
 	status = _SUCCESS;
 
-	if (status != _SUCCESS && if1)
-		rtw_usb_if1_deinit(if1);
 free_dvobj:
 	if (status != _SUCCESS)
 		usb_dvobj_deinit(pusb_intf);
diff --git a/drivers/staging/rtl8188eu/os_dep/usb_ops_linux.c b/drivers/staging/rtl8188eu/os_dep/usb_ops_linux.c
index fb0bba85373..ba2a8ab80d7 100644
--- a/drivers/staging/rtl8188eu/os_dep/usb_ops_linux.c
+++ b/drivers/staging/rtl8188eu/os_dep/usb_ops_linux.c
@@ -39,13 +39,6 @@ unsigned int ffaddr2pipehdl(struct dvobj_priv *pdvobj, u32 addr)
 	return pipe;
 }
 
-struct zero_bulkout_context {
-	void *pbuf;
-	void *purb;
-	void *pirp;
-	void *padapter;
-};
-
 void usb_read_mem(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *rmem)
 {
 }
diff --git a/drivers/staging/rtl8188eu/os_dep/xmit_linux.c b/drivers/staging/rtl8188eu/os_dep/xmit_linux.c
index 2c8e3f72dec..400356831c5 100644
--- a/drivers/staging/rtl8188eu/os_dep/xmit_linux.c
+++ b/drivers/staging/rtl8188eu/os_dep/xmit_linux.c
@@ -19,7 +19,6 @@
  ******************************************************************************/
 #define _XMIT_OSDEP_C_
 
-#include <linux/version.h>
 #include <osdep_service.h>
 #include <drv_types.h>
 
@@ -77,10 +76,6 @@ int rtw_endofpktfile(struct pkt_file *pfile)
 	return false;
 }
 
-void rtw_set_tx_chksum_offload(struct sk_buff *pkt, struct pkt_attrib *pattrib)
-{
-}
-
 int rtw_os_xmit_resource_alloc(struct adapter *padapter, struct xmit_buf *pxmitbuf, u32 alloc_sz)
 {
 	int i;
@@ -117,7 +112,6 @@ void rtw_os_xmit_resource_free(struct adapter *padapter,
 
 void rtw_os_pkt_complete(struct adapter *padapter, struct sk_buff *pkt)
 {
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 35))
 	u16	queue;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 
@@ -130,10 +124,6 @@ void rtw_os_pkt_complete(struct adapter *padapter, struct sk_buff *pkt)
 		if (__netif_subqueue_stopped(padapter->pnetdev, queue))
 			netif_wake_subqueue(padapter->pnetdev, queue);
 	}
-#else
-	if (netif_queue_stopped(padapter->pnetdev))
-		netif_wake_queue(padapter->pnetdev);
-#endif
 
 	dev_kfree_skb_any(pkt);
 }
diff --git a/drivers/staging/rtl8192e/rtl8192e/Kconfig b/drivers/staging/rtl8192e/rtl8192e/Kconfig
index ad82bc348a7..282e293da18 100644
--- a/drivers/staging/rtl8192e/rtl8192e/Kconfig
+++ b/drivers/staging/rtl8192e/rtl8192e/Kconfig
@@ -5,4 +5,5 @@ config RTL8192E
 	select WIRELESS_EXT
 	select WEXT_PRIV
 	select CRYPTO
+	select FW_LOADER
 	---help---
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
index 5f10e4075d3..79d86b9e800 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
@@ -373,7 +373,7 @@ u32 cmpk_message_handle_rx(struct net_device *dev,
 		default:
 
 			RT_TRACE(COMP_CMDPKT, "---->cmpk_message_handle_rx():"
-				 "unknow CMD Element\n");
+				 "unknown CMD Element\n");
 			return 1;
 		}
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
index abcd22f8fdd..1a95d1f87f9 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
@@ -228,13 +228,6 @@ bool init_firmware(struct net_device *dev)
 	struct r8192_priv *priv = rtllib_priv(dev);
 	bool			rt_status = true;
 
-	u8	*firmware_img_buf[3] = { &Rtl8192PciEFwBootArray[0],
-					 &Rtl8192PciEFwMainArray[0],
-					 &Rtl8192PciEFwDataArray[0]};
-
-	u32	firmware_img_len[3] = { sizeof(Rtl8192PciEFwBootArray),
-					sizeof(Rtl8192PciEFwMainArray),
-					sizeof(Rtl8192PciEFwDataArray)};
 	u32	file_length = 0;
 	u8	*mapped_file = NULL;
 	u8	init_step = 0;
@@ -257,77 +250,55 @@ bool init_firmware(struct net_device *dev)
 			 " firmware state\n");
 	}
 
-	priv->firmware_source = FW_SOURCE_IMG_FILE;
 	for (init_step = starting_state; init_step <= FW_INIT_STEP2_DATA;
 	     init_step++) {
 		if (rst_opt == OPT_SYSTEM_RESET) {
-			switch (priv->firmware_source) {
-			case FW_SOURCE_IMG_FILE:
-			{
-				if (pfirmware->firmware_buf_size[init_step] == 0) {
-					const char *fw_name[3] = {
-							RTL8192E_BOOT_IMG_FW,
-							RTL8192E_MAIN_IMG_FW,
-							RTL8192E_DATA_IMG_FW
-					};
-					const struct firmware	*fw_entry;
-					int rc;
-					rc = request_firmware(&fw_entry,
-					 fw_name[init_step], &priv->pdev->dev);
-					if (rc < 0) {
-						RT_TRACE(COMP_FIRMWARE, "request firm"
-						 "ware fail!\n");
-						goto download_firmware_fail;
-					}
-					if (fw_entry->size >
+			if (pfirmware->firmware_buf_size[init_step] == 0) {
+				const char *fw_name[3] = {
+					RTL8192E_BOOT_IMG_FW,
+					RTL8192E_MAIN_IMG_FW,
+					RTL8192E_DATA_IMG_FW
+				};
+				const struct firmware *fw_entry;
+				int rc;
+				rc = request_firmware(&fw_entry,
+						      fw_name[init_step],
+						      &priv->pdev->dev);
+				if (rc < 0) {
+					RT_TRACE(COMP_FIRMWARE, "request firmware fail!\n");
+					goto download_firmware_fail;
+				}
+				if (fw_entry->size >
 				    sizeof(pfirmware->firmware_buf[init_step])) {
-						RT_TRACE(COMP_FIRMWARE, "img file size "
+					RT_TRACE(COMP_FIRMWARE, "img file size "
 						 "exceed the container struct "
 						 "buffer fail!\n");
-						goto download_firmware_fail;
-					}
+					goto download_firmware_fail;
+				}
 
-					if (init_step != FW_INIT_STEP1_MAIN) {
-						memcpy(pfirmware->firmware_buf[init_step],
+				if (init_step != FW_INIT_STEP1_MAIN) {
+					memcpy(pfirmware->firmware_buf[init_step],
 					       fw_entry->data, fw_entry->size);
-						pfirmware->firmware_buf_size[init_step] =
-					       fw_entry->size;
+					pfirmware->firmware_buf_size[init_step] =
+						fw_entry->size;
 
-					} else {
-						memset(pfirmware->firmware_buf[init_step],
+				} else {
+					memset(pfirmware->firmware_buf[init_step],
 					       0, 128);
-						memcpy(&pfirmware->firmware_buf[init_step][128],
+					memcpy(&pfirmware->firmware_buf[init_step][128],
 					       fw_entry->data, fw_entry->size);
-						pfirmware->firmware_buf_size[init_step] =
-							 fw_entry->size + 128;
-					}
-
-					if (rst_opt == OPT_SYSTEM_RESET)
-						release_firmware(fw_entry);
+					pfirmware->firmware_buf_size[init_step] =
+						fw_entry->size + 128;
 				}
-				mapped_file = pfirmware->firmware_buf[init_step];
-				file_length = pfirmware->firmware_buf_size[init_step];
-				break;
-			}
-			case FW_SOURCE_HEADER_FILE:
-				mapped_file =  firmware_img_buf[init_step];
-				file_length  = firmware_img_len[init_step];
-				if (init_step == FW_INIT_STEP2_DATA) {
-					memcpy(pfirmware->firmware_buf[init_step], mapped_file, file_length);
-					pfirmware->firmware_buf_size[init_step] = file_length;
-				}
-				break;
 
-			default:
-				break;
+				if (rst_opt == OPT_SYSTEM_RESET)
+					release_firmware(fw_entry);
 			}
-
-
-		} else if (rst_opt == OPT_FIRMWARE_RESET) {
-			mapped_file = pfirmware->firmware_buf[init_step];
-			file_length = pfirmware->firmware_buf_size[init_step];
 		}
 
+		mapped_file = pfirmware->firmware_buf[init_step];
+		file_length = pfirmware->firmware_buf_size[init_step];
+
 		rt_status = fw_download_code(dev, mapped_file, file_length);
 		if (!rt_status) {
 			goto download_firmware_fail;
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h
index 06d6abc8345..94fa16b4993 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h
@@ -43,11 +43,6 @@ enum desc_packet_type {
 	DESC_PACKET_TYPE_NORMAL = 1,
 };
 
-enum firmware_source {
-	FW_SOURCE_IMG_FILE = 0,
-	FW_SOURCE_HEADER_FILE = 1,
-};
-
 enum firmware_status {
 	FW_STATUS_0_INIT = 0,
 	FW_STATUS_1_MOVE_BOOT_CODE = 1,
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.c
index 08e7dbb6694..6767b5965c1 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.c
@@ -20,2777 +20,6 @@
 
 #include "r8192E_hwimg.h"
 
-u8 Rtl8192PciEFwBootArray[BootArrayLengthPciE] = {
-0x10,0x00,0x00,0x01,0x00,0x00,0x00,0x00,0x3c,0x08,0xbf,0xc0,0x25,0x08,0x00,0x08,
-0x3c,0x09,0xb0,0x03,0xad,0x28,0x00,0x20,0x40,0x80,0x68,0x00,0x00,0x00,0x00,0x00,
-0x3c,0x0a,0xd0,0x00,0x40,0x8a,0x60,0x00,0x00,0x00,0x00,0x00,0x3c,0x08,0x80,0x01,
-0x25,0x08,0xa8,0x04,0x24,0x09,0x00,0x01,0x3c,0x01,0x7f,0xff,0x34,0x21,0xff,0xff,
-0x01,0x01,0x50,0x24,0x00,0x09,0x48,0x40,0x35,0x29,0x00,0x01,0x01,0x2a,0x10,0x2b,
-0x14,0x40,0xff,0xfc,0x00,0x00,0x00,0x00,0x3c,0x0a,0x00,0x00,0x25,0x4a,0x00,0x00,
-0x4c,0x8a,0x00,0x00,0x4c,0x89,0x08,0x00,0x00,0x00,0x00,0x00,0x3c,0x08,0x80,0x01,
-0x25,0x08,0xa8,0x04,0x3c,0x01,0x80,0x00,0x01,0x21,0x48,0x25,0x3c,0x0a,0xbf,0xc0,
-0x25,0x4a,0x00,0x7c,0x3c,0x0b,0xb0,0x03,0xad,0x6a,0x00,0x20,0xad,0x00,0x00,0x00,
-0x21,0x08,0x00,0x04,0x01,0x09,0x10,0x2b,0x14,0x40,0xff,0xf8,0x00,0x00,0x00,0x00,
-0x3c,0x08,0x80,0x01,0x25,0x08,0x7f,0xff,0x24,0x09,0x00,0x01,0x3c,0x01,0x7f,0xff,
-0x34,0x21,0xff,0xff,0x01,0x01,0x50,0x24,0x00,0x09,0x48,0x40,0x35,0x29,0x00,0x01,
-0x01,0x2a,0x10,0x2b,0x14,0x40,0xff,0xfc,0x00,0x00,0x00,0x00,0x3c,0x0a,0x80,0x01,
-0x25,0x4a,0x00,0x00,0x3c,0x01,0x7f,0xff,0x34,0x21,0xff,0xff,0x01,0x41,0x50,0x24,
-0x3c,0x09,0x00,0x01,0x35,0x29,0x7f,0xff,0x4c,0x8a,0x20,0x00,0x4c,0x89,0x28,0x00,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x24,0x08,0x04,0x10,
-0x00,0x00,0x00,0x00,0x40,0x88,0xa0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-0x3c,0x08,0xbf,0xc0,0x00,0x00,0x00,0x00,0x8d,0x09,0x00,0x00,0x00,0x00,0x00,0x00,
-0x3c,0x0a,0xbf,0xc0,0x25,0x4a,0x01,0x20,0x3c,0x0b,0xb0,0x03,0xad,0x6a,0x00,0x20,
-0x3c,0x08,0xb0,0x03,0x8d,0x09,0x00,0x00,0x00,0x00,0x00,0x00,0x35,0x29,0x00,0x10,
-0xad,0x09,0x00,0x00,0x00,0x00,0x00,0x00,0x3c,0x08,0x80,0x00,0x25,0x08,0x4b,0x94,
-0x01,0x00,0x00,0x08,0x00,0x00,0x00,0x00,};
-
-u8 Rtl8192PciEFwMainArray[MainArrayLengthPciE] = {
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-0x40,0x04,0x68,0x00,0x40,0x05,0x70,0x00,0x40,0x06,0x40,0x00,0x0c,0x00,0x12,0x98,
-0x00,0x00,0x00,0x00,0x40,0x1a,0x68,0x00,0x33,0x5b,0x00,0x3c,0x17,0x60,0x00,0x09,
-0x00,0x00,0x00,0x00,0x40,0x1b,0x60,0x00,0x00,0x00,0x00,0x00,0x03,0x5b,0xd0,0x24,
-0x40,0x1a,0x70,0x00,0x03,0x40,0x00,0x08,0x42,0x00,0x00,0x10,0x00,0x00,0x00,0x00,
-0x00,0x00,0x00,0x00,0x3c,0x02,0xff,0xff,0x34,0x42,0xff,0xff,0x8c,0x43,0x00,0x00,
-0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x34,0x63,0x00,0x20,0x24,0x42,0x00,0xd0,
-0xac,0x62,0x00,0x00,0x00,0x00,0x20,0x21,0x27,0x85,0x8b,0x70,0x00,0x85,0x18,0x21,
-0x24,0x84,0x00,0x01,0x28,0x82,0x00,0x0a,0x14,0x40,0xff,0xfc,0xa0,0x60,0x00,0x00,
-0x27,0x82,0x8b,0x7a,0x24,0x04,0x00,0x06,0x24,0x84,0xff,0xff,0xa4,0x40,0x00,0x00,
-0x04,0x81,0xff,0xfd,0x24,0x42,0x00,0x02,0x24,0x02,0x00,0x03,0xa3,0x82,0x8b,0x70,
-0x24,0x02,0x00,0x0a,0x24,0x03,0x09,0xc4,0xa3,0x82,0x8b,0x72,0x24,0x02,0x00,0x04,
-0x24,0x04,0x00,0x01,0x24,0x05,0x00,0x02,0xa7,0x83,0x8b,0x86,0xa3,0x82,0x8b,0x78,
-0x24,0x03,0x04,0x00,0x24,0x02,0x02,0x00,0xaf,0x83,0x8b,0x8c,0xa3,0x85,0x8b,0x79,
-0xa7,0x82,0x8b,0x7a,0xa7,0x84,0x8b,0x7c,0xaf,0x84,0x8b,0x88,0xa3,0x84,0x8b,0x71,
-0xa3,0x80,0x8b,0x73,0xa3,0x80,0x8b,0x74,0xa3,0x80,0x8b,0x75,0xa3,0x84,0x8b,0x76,
-0xa3,0x85,0x8b,0x77,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,
-0x3c,0x02,0x80,0x00,0x24,0x42,0x01,0x7c,0x34,0x63,0x00,0x20,0xac,0x62,0x00,0x00,
-0x27,0x84,0x8b,0x98,0x00,0x00,0x10,0x21,0x24,0x42,0x00,0x01,0x00,0x02,0x16,0x00,
-0x00,0x02,0x16,0x03,0x28,0x43,0x00,0x03,0xac,0x80,0xff,0xfc,0xa0,0x80,0x00,0x00,
-0x14,0x60,0xff,0xf9,0x24,0x84,0x00,0x0c,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,
-0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x34,0x63,0x00,0x20,0x24,0x42,0x01,0xc0,
-0x3c,0x08,0xb0,0x03,0xac,0x62,0x00,0x00,0x35,0x08,0x00,0x70,0x8d,0x02,0x00,0x00,
-0x00,0xa0,0x48,0x21,0x00,0x04,0x26,0x00,0x00,0x02,0x2a,0x43,0x00,0x06,0x36,0x00,
-0x00,0x07,0x3e,0x00,0x00,0x02,0x12,0x03,0x29,0x23,0x00,0x03,0x00,0x04,0x56,0x03,
-0x00,0x06,0x36,0x03,0x00,0x07,0x3e,0x03,0x30,0x48,0x00,0x01,0x10,0x60,0x00,0x11,
-0x30,0xa5,0x00,0x07,0x24,0x02,0x00,0x02,0x00,0x49,0x10,0x23,0x00,0x45,0x10,0x07,
-0x30,0x42,0x00,0x01,0x10,0x40,0x00,0x66,0x00,0x00,0x00,0x00,0x8f,0xa2,0x00,0x10,
-0x00,0x00,0x00,0x00,0x00,0x02,0x21,0x43,0x11,0x00,0x00,0x10,0x00,0x07,0x20,0x0b,
-0x15,0x20,0x00,0x06,0x24,0x02,0x00,0x01,0x3c,0x02,0xb0,0x05,0x34,0x42,0x01,0x20,
-0xa4,0x44,0x00,0x00,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x11,0x22,0x00,0x04,
-0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x05,0x08,0x00,0x00,0x94,0x34,0x42,0x01,0x24,
-0x3c,0x02,0xb0,0x05,0x08,0x00,0x00,0x94,0x34,0x42,0x01,0x22,0x15,0x20,0x00,0x54,
-0x24,0x02,0x00,0x01,0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0x74,0x90,0x43,0x00,0x00,
-0x00,0x00,0x00,0x00,0xaf,0x83,0x8b,0x94,0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0x70,
-0x90,0x43,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x6b,0x00,0x08,0x11,0x60,0x00,0x18,
-0x00,0x09,0x28,0x40,0x00,0x00,0x40,0x21,0x27,0x85,0x8b,0x90,0x8c,0xa3,0x00,0x00,
-0x8c,0xa2,0x00,0x04,0x00,0x00,0x00,0x00,0x00,0x62,0x38,0x23,0x00,0x43,0x10,0x2a,
-0x10,0x40,0x00,0x3d,0x00,0x00,0x00,0x00,0xac,0xa7,0x00,0x00,0x25,0x02,0x00,0x01,
-0x00,0x02,0x16,0x00,0x00,0x02,0x46,0x03,0x29,0x03,0x00,0x03,0x14,0x60,0xff,0xf3,
-0x24,0xa5,0x00,0x0c,0x3c,0x03,0xb0,0x03,0x34,0x63,0x00,0x70,0x90,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x4b,0x10,0x23,0xa0,0x62,0x00,0x00,0x00,0x09,0x28,0x40,
-0x00,0xa9,0x10,0x21,0x00,0x02,0x10,0x80,0x27,0x83,0x8b,0x98,0x00,0x0a,0x20,0x0b,
-0x00,0x43,0x18,0x21,0x10,0xc0,0x00,0x05,0x00,0x00,0x38,0x21,0x80,0x62,0x00,0x01,
-0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x05,0x00,0x00,0x00,0x00,0x80,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x03,0x00,0xa9,0x10,0x21,0x24,0x07,0x00,0x01,
-0x00,0xa9,0x10,0x21,0x00,0x02,0x30,0x80,0x27,0x82,0x8b,0x98,0xa0,0x67,0x00,0x01,
-0x00,0xc2,0x38,0x21,0x80,0xe3,0x00,0x01,0x00,0x00,0x00,0x00,0x10,0x60,0x00,0x07,
-0x00,0x00,0x00,0x00,0x27,0x83,0x8b,0x90,0x00,0xc3,0x18,0x21,0x8c,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x44,0x10,0x21,0xac,0x62,0x00,0x00,0x27,0x85,0x8b,0x94,
-0x27,0x82,0x8b,0x90,0x00,0xc5,0x28,0x21,0x00,0xc2,0x10,0x21,0x8c,0x43,0x00,0x00,
-0x8c,0xa4,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x64,0x18,0x2a,0x14,0x60,0x00,0x03,
-0x24,0x02,0x00,0x01,0x03,0xe0,0x00,0x08,0xa0,0xe2,0x00,0x00,0xa0,0xe0,0x00,0x00,
-0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x08,0x00,0x00,0xb7,0xac,0xa0,0x00,0x00,
-0x11,0x22,0x00,0x08,0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0x7c,
-0x90,0x43,0x00,0x00,0x00,0x00,0x00,0x00,0xaf,0x83,0x8b,0xac,0x08,0x00,0x00,0xa7,
-0x3c,0x02,0xb0,0x03,0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0x78,0x90,0x43,0x00,0x00,
-0x00,0x00,0x00,0x00,0xaf,0x83,0x8b,0xa0,0x08,0x00,0x00,0xa7,0x3c,0x02,0xb0,0x03,
-0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x34,0x63,0x00,0x20,0x24,0x42,0x04,0x10,
-0x3c,0x05,0xb0,0x03,0xac,0x62,0x00,0x00,0x34,0xa5,0x00,0x70,0x8c,0xa2,0x00,0x00,
-0x90,0x84,0x00,0x08,0x3c,0x06,0xb0,0x03,0x00,0x02,0x16,0x00,0x2c,0x83,0x00,0x03,
-0x34,0xc6,0x00,0x72,0x24,0x07,0x00,0x01,0x10,0x60,0x00,0x11,0x00,0x02,0x2f,0xc2,
-0x90,0xc2,0x00,0x00,0x00,0x00,0x18,0x21,0x00,0x02,0x16,0x00,0x10,0xa7,0x00,0x09,
-0x00,0x02,0x16,0x03,0x14,0x80,0x00,0x0c,0x30,0x43,0x00,0x03,0x83,0x82,0x8b,0x98,
-0x00,0x00,0x00,0x00,0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,0x00,0x02,0x16,0x00,
-0x00,0x02,0x1e,0x03,0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0x72,0xa0,0x43,0x00,0x00,
-0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x30,0x45,0x00,0x05,0x10,0x87,0x00,0x04,
-0x30,0x43,0x00,0x06,0x93,0x82,0x8b,0xb0,0x08,0x00,0x01,0x1f,0x00,0x43,0x10,0x21,
-0x83,0x82,0x8b,0xa4,0x00,0x00,0x00,0x00,0x00,0x02,0x10,0x40,0x08,0x00,0x01,0x1f,
-0x00,0x45,0x10,0x21,0x10,0x80,0x00,0x05,0x00,0x00,0x18,0x21,0x24,0x63,0x00,0x01,
-0x00,0x64,0x10,0x2b,0x14,0x40,0xff,0xfd,0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,
-0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x24,0x42,0x04,0xe4,
-0x3c,0x04,0xb0,0x02,0x34,0x63,0x00,0x20,0xac,0x62,0x00,0x00,0x34,0x84,0x00,0x08,
-0x24,0x02,0x00,0x01,0xaf,0x84,0x8b,0xc0,0xa3,0x82,0x8b,0xd0,0xa7,0x80,0x8b,0xc4,
-0xa7,0x80,0x8b,0xc6,0xaf,0x80,0x8b,0xc8,0xaf,0x80,0x8b,0xcc,0x03,0xe0,0x00,0x08,
-0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x34,0x63,0x00,0x20,
-0x24,0x42,0x05,0x24,0x3c,0x04,0xb0,0x03,0xac,0x62,0x00,0x00,0x34,0x84,0x00,0xac,
-0x80,0xa2,0x00,0x15,0x8c,0x83,0x00,0x00,0x27,0xbd,0xff,0xf0,0x00,0x43,0x10,0x21,
-0xac,0x82,0x00,0x00,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x10,0x3c,0x02,0xb0,0x03,
-0x3c,0x03,0x80,0x00,0x34,0x42,0x00,0x20,0x24,0x63,0x05,0x5c,0x27,0xbd,0xff,0xe0,
-0xac,0x43,0x00,0x00,0xaf,0xb1,0x00,0x14,0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x18,
-0x8f,0x90,0x8b,0xc0,0x0c,0x00,0x02,0x98,0x00,0x80,0x88,0x21,0x14,0x40,0x00,0x2a,
-0x3c,0x02,0x00,0x80,0x16,0x20,0x00,0x02,0x34,0x42,0x02,0x01,0x24,0x02,0x02,0x01,
-0xae,0x02,0x00,0x00,0x97,0x84,0x8b,0xc4,0x97,0x82,0x8b,0xc6,0x3c,0x03,0xb0,0x02,
-0x00,0x83,0x20,0x21,0x24,0x42,0x00,0x04,0xa7,0x82,0x8b,0xc6,0xa4,0x82,0x00,0x00,
-0x8f,0x84,0x8b,0xc8,0x8f,0x82,0x8b,0xc0,0x93,0x85,0x8b,0x72,0x24,0x84,0x00,0x01,
-0x24,0x42,0x00,0x04,0x24,0x03,0x8f,0xff,0x3c,0x07,0xb0,0x06,0x3c,0x06,0xb0,0x03,
-0x00,0x43,0x10,0x24,0x00,0x85,0x28,0x2a,0x34,0xe7,0x80,0x18,0xaf,0x82,0x8b,0xc0,
-0xaf,0x84,0x8b,0xc8,0x10,0xa0,0x00,0x08,0x34,0xc6,0x01,0x08,0x8f,0x83,0x8b,0xcc,
-0x8f,0x84,0x8b,0x8c,0x8c,0xc2,0x00,0x00,0x00,0x64,0x18,0x21,0x00,0x43,0x10,0x2b,
-0x14,0x40,0x00,0x09,0x00,0x00,0x00,0x00,0x8c,0xe2,0x00,0x00,0x3c,0x03,0x0f,0x00,
-0x3c,0x04,0x04,0x00,0x00,0x43,0x10,0x24,0x10,0x44,0x00,0x03,0x00,0x00,0x00,0x00,
-0x0c,0x00,0x04,0x96,0x00,0x00,0x00,0x00,0x8f,0xbf,0x00,0x18,0x7b,0xb0,0x00,0xbc,
-0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x20,0x27,0xbd,0xff,0xd8,0x3c,0x02,0xb0,0x03,
-0x3c,0x03,0x80,0x00,0x24,0x63,0x06,0x48,0xaf,0xb0,0x00,0x10,0x34,0x42,0x00,0x20,
-0x8f,0x90,0x8b,0xc0,0xac,0x43,0x00,0x00,0xaf,0xb3,0x00,0x1c,0xaf,0xb2,0x00,0x18,
-0xaf,0xb1,0x00,0x14,0xaf,0xbf,0x00,0x20,0x00,0x80,0x88,0x21,0x00,0xa0,0x90,0x21,
-0x0c,0x00,0x02,0x98,0x00,0xc0,0x98,0x21,0x24,0x07,0x8f,0xff,0x14,0x40,0x00,0x19,
-0x26,0x03,0x00,0x04,0x24,0x02,0x0e,0x03,0xae,0x02,0x00,0x00,0x00,0x67,0x80,0x24,
-0x26,0x02,0x00,0x04,0xae,0x11,0x00,0x00,0x00,0x47,0x80,0x24,0x97,0x86,0x8b,0xc4,
-0x26,0x03,0x00,0x04,0xae,0x12,0x00,0x00,0x00,0x67,0x80,0x24,0xae,0x13,0x00,0x00,
-0x8f,0x84,0x8b,0xc0,0x3c,0x02,0xb0,0x02,0x97,0x85,0x8b,0xc6,0x00,0xc2,0x30,0x21,
-0x8f,0x82,0x8b,0xc8,0x24,0x84,0x00,0x10,0x24,0xa5,0x00,0x10,0x00,0x87,0x20,0x24,
-0x24,0x42,0x00,0x01,0xa7,0x85,0x8b,0xc6,0xaf,0x84,0x8b,0xc0,0xaf,0x82,0x8b,0xc8,
-0xa4,0xc5,0x00,0x00,0x8f,0xbf,0x00,0x20,0x7b,0xb2,0x00,0xfc,0x7b,0xb0,0x00,0xbc,
-0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x28,0x27,0xbd,0xff,0xe8,0xaf,0xbf,0x00,0x10,
-0x94,0x82,0x00,0x04,0x00,0x00,0x00,0x00,0x30,0x42,0xe0,0x00,0x14,0x40,0x00,0x14,
-0x00,0x00,0x00,0x00,0x90,0x82,0x00,0x02,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xfc,
-0x00,0x82,0x28,0x21,0x8c,0xa4,0x00,0x00,0x3c,0x02,0x00,0x70,0x8c,0xa6,0x00,0x08,
-0x00,0x82,0x10,0x21,0x2c,0x43,0x00,0x06,0x10,0x60,0x00,0x09,0x3c,0x03,0x80,0x01,
-0x00,0x02,0x10,0x80,0x24,0x63,0x01,0xe8,0x00,0x43,0x10,0x21,0x8c,0x44,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x80,0x00,0x08,0x00,0x00,0x00,0x00,0xaf,0x86,0x80,0x14,
-0x8f,0xbf,0x00,0x10,0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x18,
-0x8c,0xa4,0x00,0x00,0x0c,0x00,0x17,0x84,0x00,0x00,0x00,0x00,0x08,0x00,0x01,0xdc,
-0x00,0x00,0x00,0x00,0x0c,0x00,0x24,0x49,0x00,0xc0,0x20,0x21,0x08,0x00,0x01,0xdc,
-0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,0x34,0x42,0x01,0x08,0x8c,0x44,0x00,0x00,
-0x8f,0x82,0x80,0x18,0x3c,0x03,0x00,0x0f,0x34,0x63,0x42,0x40,0x00,0x43,0x10,0x21,
-0x00,0x82,0x20,0x2b,0x10,0x80,0x00,0x09,0x24,0x03,0x00,0x05,0x8f,0x82,0x83,0x60,
-0x00,0x00,0x00,0x00,0x24,0x42,0x00,0x01,0xaf,0x82,0x83,0x60,0x10,0x43,0x00,0x03,
-0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,
-0x8c,0x63,0x01,0x08,0x24,0x02,0x00,0x01,0xa3,0x82,0x80,0x11,0xaf,0x80,0x83,0x60,
-0xaf,0x83,0x80,0x18,0x08,0x00,0x01,0xf9,0x00,0x00,0x00,0x00,0x30,0x84,0x00,0xff,
-0x14,0x80,0x00,0x2f,0x00,0x00,0x00,0x00,0x8f,0x82,0x80,0x14,0xa3,0x85,0x83,0x93,
-0x10,0x40,0x00,0x2b,0x2c,0xa2,0x00,0x04,0x14,0x40,0x00,0x06,0x00,0x05,0x10,0x40,
-0x24,0xa2,0xff,0xfc,0x2c,0x42,0x00,0x08,0x10,0x40,0x00,0x09,0x24,0xa2,0xff,0xf0,
-0x00,0x05,0x10,0x40,0x27,0x84,0x83,0x9c,0x00,0x44,0x10,0x21,0x94,0x43,0x00,0x00,
-0x00,0x00,0x00,0x00,0x24,0x63,0x00,0x01,0x03,0xe0,0x00,0x08,0xa4,0x43,0x00,0x00,
-0x2c,0x42,0x00,0x10,0x14,0x40,0x00,0x0a,0x00,0x05,0x10,0x40,0x24,0xa2,0xff,0xe0,
-0x2c,0x42,0x00,0x10,0x14,0x40,0x00,0x06,0x00,0x05,0x10,0x40,0x24,0xa2,0xff,0xd0,
-0x2c,0x42,0x00,0x10,0x10,0x40,0x00,0x09,0x24,0xa2,0xff,0xc0,0x00,0x05,0x10,0x40,
-0x27,0x84,0x83,0x9c,0x00,0x44,0x10,0x21,0x94,0x43,0xff,0xf8,0x00,0x00,0x00,0x00,
-0x24,0x63,0x00,0x01,0x03,0xe0,0x00,0x08,0xa4,0x43,0xff,0xf8,0x2c,0x42,0x00,0x10,
-0x10,0x40,0x00,0x07,0x00,0x05,0x10,0x40,0x27,0x84,0x83,0x9c,0x00,0x44,0x10,0x21,
-0x94,0x43,0xff,0xf8,0x00,0x00,0x00,0x00,0x24,0x63,0x00,0x01,0xa4,0x43,0xff,0xf8,
-0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x8f,0x86,0x8b,0xc0,0x8f,0x82,0x80,0x14,
-0x27,0xbd,0xff,0xe8,0xaf,0xbf,0x00,0x10,0x10,0x40,0x00,0x2a,0x00,0xc0,0x38,0x21,
-0x24,0x02,0x00,0x07,0x24,0x03,0xff,0x9c,0xa3,0x82,0x83,0x9b,0xa3,0x83,0x83,0x9a,
-0x27,0x8a,0x83,0x98,0x00,0x00,0x20,0x21,0x24,0x09,0x8f,0xff,0x00,0x04,0x10,0x80,
-0x00,0x4a,0x28,0x21,0x8c,0xa2,0x00,0x00,0x24,0xe3,0x00,0x04,0x24,0x88,0x00,0x01,
-0xac,0xe2,0x00,0x00,0x10,0x80,0x00,0x02,0x00,0x69,0x38,0x24,0xac,0xa0,0x00,0x00,
-0x31,0x04,0x00,0xff,0x2c,0x82,0x00,0x27,0x14,0x40,0xff,0xf5,0x00,0x04,0x10,0x80,
-0x97,0x83,0x8b,0xc6,0x97,0x85,0x8b,0xc4,0x3c,0x02,0xb0,0x02,0x24,0x63,0x00,0x9c,
-0x00,0xa2,0x28,0x21,0x3c,0x04,0xb0,0x06,0xa7,0x83,0x8b,0xc6,0x34,0x84,0x80,0x18,
-0xa4,0xa3,0x00,0x00,0x8c,0x85,0x00,0x00,0x24,0x02,0x8f,0xff,0x24,0xc6,0x00,0x9c,
-0x3c,0x03,0x0f,0x00,0x00,0xc2,0x30,0x24,0x00,0xa3,0x28,0x24,0x3c,0x02,0x04,0x00,
-0xaf,0x86,0x8b,0xc0,0x10,0xa2,0x00,0x03,0x00,0x00,0x00,0x00,0x0c,0x00,0x04,0x96,
-0x00,0x00,0x00,0x00,0x8f,0xbf,0x00,0x10,0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x18,0x8f,0x86,0x8b,0xc0,0x27,0xbd,0xff,0xc8,0x24,0x02,0x00,0x08,
-0x24,0x03,0x00,0x20,0xaf,0xbf,0x00,0x30,0xa3,0xa2,0x00,0x13,0xa3,0xa3,0x00,0x12,
-0xa7,0xa4,0x00,0x10,0x00,0xc0,0x28,0x21,0x27,0xa9,0x00,0x10,0x00,0x00,0x38,0x21,
-0x24,0x08,0x8f,0xff,0x00,0x07,0x10,0x80,0x00,0x49,0x10,0x21,0x8c,0x44,0x00,0x00,
-0x24,0xe3,0x00,0x01,0x30,0x67,0x00,0xff,0x24,0xa2,0x00,0x04,0x2c,0xe3,0x00,0x08,
-0xac,0xa4,0x00,0x00,0x14,0x60,0xff,0xf7,0x00,0x48,0x28,0x24,0x97,0x83,0x8b,0xc6,
-0x97,0x85,0x8b,0xc4,0x3c,0x02,0xb0,0x02,0x24,0x63,0x00,0x20,0x00,0xa2,0x28,0x21,
-0x3c,0x04,0xb0,0x06,0xa7,0x83,0x8b,0xc6,0x34,0x84,0x80,0x18,0xa4,0xa3,0x00,0x00,
-0x8c,0x85,0x00,0x00,0x24,0x02,0x8f,0xff,0x24,0xc6,0x00,0x20,0x3c,0x03,0x0f,0x00,
-0x00,0xc2,0x30,0x24,0x00,0xa3,0x28,0x24,0x3c,0x02,0x04,0x00,0xaf,0x86,0x8b,0xc0,
-0x10,0xa2,0x00,0x03,0x00,0x00,0x00,0x00,0x0c,0x00,0x04,0x96,0x00,0x00,0x00,0x00,
-0x8f,0xbf,0x00,0x30,0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x38,
-0x93,0x82,0x8b,0xd0,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x11,0x24,0x06,0x00,0x01,
-0x8f,0x82,0x8b,0xc8,0x3c,0x05,0xb0,0x06,0x3c,0x04,0xb0,0x03,0x34,0xa5,0x80,0x18,
-0x34,0x84,0x01,0x08,0x14,0x40,0x00,0x09,0x00,0x00,0x30,0x21,0x97,0x82,0x8b,0xc4,
-0x8c,0x84,0x00,0x00,0x3c,0x03,0xb0,0x02,0x00,0x43,0x10,0x21,0xaf,0x84,0x8b,0xcc,
-0xa7,0x80,0x8b,0xc6,0xac,0x40,0x00,0x00,0xac,0x40,0x00,0x04,0x8c,0xa2,0x00,0x00,
-0x03,0xe0,0x00,0x08,0x00,0xc0,0x10,0x21,0x8f,0x86,0x8b,0xc0,0x8f,0x82,0x8b,0xc8,
-0x27,0xbd,0xff,0xe8,0xaf,0xbf,0x00,0x10,0x00,0xc0,0x40,0x21,0x14,0x40,0x00,0x0a,
-0x00,0x40,0x50,0x21,0x00,0x00,0x38,0x21,0x27,0x89,0x83,0x68,0x24,0xe2,0x00,0x01,
-0x00,0x07,0x18,0x80,0x30,0x47,0x00,0xff,0x00,0x69,0x18,0x21,0x2c,0xe2,0x00,0x0a,
-0x14,0x40,0xff,0xfa,0xac,0x60,0x00,0x00,0x3c,0x02,0x00,0x80,0x10,0x82,0x00,0x6f,
-0x00,0x00,0x00,0x00,0x97,0x82,0x83,0x6e,0x00,0x00,0x00,0x00,0x24,0x42,0x00,0x01,
-0xa7,0x82,0x83,0x6e,0x90,0xa3,0x00,0x15,0x97,0x82,0x83,0x70,0x00,0x03,0x1e,0x00,
-0x00,0x03,0x1e,0x03,0x00,0x43,0x10,0x21,0xa7,0x82,0x83,0x70,0x8c,0xa4,0x00,0x20,
-0x3c,0x02,0x00,0x60,0x3c,0x03,0x00,0x20,0x00,0x82,0x20,0x24,0x10,0x83,0x00,0x54,
-0x00,0x00,0x00,0x00,0x14,0x80,0x00,0x47,0x00,0x00,0x00,0x00,0x97,0x82,0x83,0x74,
-0x00,0x00,0x00,0x00,0x24,0x42,0x00,0x01,0xa7,0x82,0x83,0x74,0x84,0xa3,0x00,0x06,
-0x8f,0x82,0x83,0x84,0x00,0x00,0x00,0x00,0x00,0x43,0x10,0x21,0xaf,0x82,0x83,0x84,
-0x25,0x42,0x00,0x01,0x28,0x43,0x27,0x10,0xaf,0x82,0x8b,0xc8,0x10,0x60,0x00,0x09,
-0x24,0x02,0x00,0x04,0x93,0x83,0x80,0x11,0x24,0x02,0x00,0x01,0x10,0x62,0x00,0x05,
-0x24,0x02,0x00,0x04,0x8f,0xbf,0x00,0x10,0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x18,0x24,0x03,0x00,0x28,0xa3,0x83,0x83,0x6a,0xa3,0x82,0x83,0x6b,
-0x90,0xa2,0x00,0x18,0x93,0x83,0x83,0x93,0x00,0x00,0x38,0x21,0x00,0x02,0x16,0x00,
-0x00,0x02,0x16,0x03,0xa7,0x82,0x83,0x7e,0xa3,0x83,0x83,0x8c,0x27,0x89,0x83,0x68,
-0x24,0x05,0x8f,0xff,0x00,0x07,0x10,0x80,0x00,0x49,0x10,0x21,0x8c,0x44,0x00,0x00,
-0x24,0xe3,0x00,0x01,0x30,0x67,0x00,0xff,0x25,0x02,0x00,0x04,0x2c,0xe3,0x00,0x0a,
-0xad,0x04,0x00,0x00,0x14,0x60,0xff,0xf7,0x00,0x45,0x40,0x24,0x97,0x83,0x8b,0xc6,
-0x97,0x85,0x8b,0xc4,0x3c,0x02,0xb0,0x02,0x24,0x63,0x00,0x28,0x00,0xa2,0x28,0x21,
-0x3c,0x04,0xb0,0x06,0xa7,0x83,0x8b,0xc6,0x34,0x84,0x80,0x18,0xa4,0xa3,0x00,0x00,
-0x8c,0x85,0x00,0x00,0x24,0x02,0x8f,0xff,0x24,0xc6,0x00,0x28,0x3c,0x03,0x0f,0x00,
-0x00,0xc2,0x30,0x24,0x00,0xa3,0x28,0x24,0x3c,0x02,0x04,0x00,0xaf,0x86,0x8b,0xc0,
-0x10,0xa2,0x00,0x03,0x00,0x00,0x00,0x00,0x0c,0x00,0x04,0x96,0x00,0x00,0x00,0x00,
-0x0c,0x00,0x02,0x36,0x00,0x00,0x00,0x00,0xa3,0x80,0x80,0x11,0x08,0x00,0x02,0xe5,
-0x00,0x00,0x00,0x00,0x97,0x82,0x83,0x76,0x00,0x00,0x00,0x00,0x24,0x42,0x00,0x01,
-0xa7,0x82,0x83,0x76,0x84,0xa3,0x00,0x06,0x8f,0x82,0x83,0x88,0x00,0x00,0x00,0x00,
-0x00,0x43,0x10,0x21,0xaf,0x82,0x83,0x88,0x08,0x00,0x02,0xdd,0x25,0x42,0x00,0x01,
-0x97,0x82,0x83,0x72,0x00,0x00,0x00,0x00,0x24,0x42,0x00,0x01,0xa7,0x82,0x83,0x72,
-0x84,0xa3,0x00,0x06,0x8f,0x82,0x83,0x80,0x00,0x00,0x00,0x00,0x00,0x43,0x10,0x21,
-0xaf,0x82,0x83,0x80,0x08,0x00,0x02,0xdd,0x25,0x42,0x00,0x01,0x97,0x82,0x83,0x6c,
-0x00,0x00,0x00,0x00,0x24,0x42,0x00,0x01,0xa7,0x82,0x83,0x6c,0x08,0x00,0x02,0xc5,
-0x00,0x00,0x00,0x00,0x27,0xbd,0xff,0xd0,0xaf,0xbf,0x00,0x28,0x8c,0xa3,0x00,0x20,
-0x8f,0x8a,0x8b,0xc0,0x3c,0x02,0x00,0x10,0x00,0x62,0x10,0x24,0x00,0xa0,0x38,0x21,
-0x01,0x40,0x48,0x21,0x10,0x40,0x00,0x3d,0x00,0x80,0x28,0x21,0x8c,0xe4,0x00,0x1c,
-0x34,0xa5,0x12,0x06,0xaf,0xa5,0x00,0x10,0x8c,0x82,0x00,0x08,0x00,0x03,0x1c,0x42,
-0x30,0x63,0x00,0x30,0x00,0x02,0x13,0x02,0x30,0x42,0x00,0x40,0x00,0x43,0x10,0x25,
-0x90,0xe6,0x00,0x10,0x90,0xe4,0x00,0x13,0x94,0xe8,0x00,0x0c,0x94,0xe3,0x00,0x1a,
-0x00,0x02,0x16,0x00,0x90,0xe7,0x00,0x12,0x00,0xa2,0x28,0x25,0x24,0x02,0x12,0x34,
-0xa7,0xa2,0x00,0x1c,0x24,0x02,0x56,0x78,0xaf,0xa5,0x00,0x10,0xa3,0xa6,0x00,0x18,
-0xa3,0xa7,0x00,0x1f,0xa7,0xa3,0x00,0x1a,0xa3,0xa4,0x00,0x19,0xa7,0xa8,0x00,0x20,
-0xa7,0xa2,0x00,0x22,0x00,0x00,0x28,0x21,0x27,0xa7,0x00,0x10,0x24,0x06,0x8f,0xff,
-0x00,0x05,0x10,0x80,0x00,0x47,0x10,0x21,0x8c,0x44,0x00,0x00,0x24,0xa3,0x00,0x01,
-0x30,0x65,0x00,0xff,0x25,0x22,0x00,0x04,0x2c,0xa3,0x00,0x05,0xad,0x24,0x00,0x00,
-0x14,0x60,0xff,0xf7,0x00,0x46,0x48,0x24,0x97,0x83,0x8b,0xc6,0x97,0x85,0x8b,0xc4,
-0x3c,0x02,0xb0,0x02,0x24,0x63,0x00,0x14,0x00,0xa2,0x28,0x21,0x3c,0x04,0xb0,0x06,
-0xa7,0x83,0x8b,0xc6,0x34,0x84,0x80,0x18,0xa4,0xa3,0x00,0x00,0x8c,0x85,0x00,0x00,
-0x24,0x02,0x8f,0xff,0x25,0x46,0x00,0x14,0x3c,0x03,0x0f,0x00,0x00,0xc2,0x50,0x24,
-0x00,0xa3,0x28,0x24,0x3c,0x02,0x04,0x00,0xaf,0x8a,0x8b,0xc0,0x10,0xa2,0x00,0x03,
-0x00,0x00,0x00,0x00,0x0c,0x00,0x04,0x96,0x00,0x00,0x00,0x00,0x8f,0xbf,0x00,0x28,
-0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x30,0x3c,0x05,0xb0,0x03,
-0x3c,0x02,0x80,0x00,0x27,0xbd,0xff,0xc8,0x00,0x04,0x22,0x00,0x34,0xa5,0x00,0x20,
-0x24,0x42,0x0d,0xfc,0x3c,0x03,0xb0,0x00,0xaf,0xb5,0x00,0x24,0xaf,0xb4,0x00,0x20,
-0xaf,0xb2,0x00,0x18,0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x30,0x00,0x83,0x80,0x21,
-0xaf,0xb7,0x00,0x2c,0xaf,0xb6,0x00,0x28,0xaf,0xb3,0x00,0x1c,0xaf,0xb1,0x00,0x14,
-0xac,0xa2,0x00,0x00,0x8e,0x09,0x00,0x00,0x00,0x00,0x90,0x21,0x26,0x10,0x00,0x08,
-0x00,0x09,0xa6,0x02,0x12,0x80,0x00,0x13,0x00,0x00,0xa8,0x21,0x24,0x13,0x00,0x02,
-0x3c,0x16,0x00,0xff,0x3c,0x17,0xff,0x00,0x8e,0x09,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x09,0x12,0x02,0x24,0x42,0x00,0x02,0x31,0x25,0x00,0xff,0x10,0xb3,0x00,0x76,
-0x30,0x51,0x00,0xff,0x24,0x02,0x00,0x03,0x10,0xa2,0x00,0x18,0x00,0x00,0x00,0x00,
-0x02,0x51,0x10,0x21,0x30,0x52,0xff,0xff,0x02,0x54,0x18,0x2b,0x14,0x60,0xff,0xf2,
-0x02,0x11,0x80,0x21,0x12,0xa0,0x00,0x0a,0x3c,0x02,0xb0,0x06,0x34,0x42,0x80,0x18,
-0x8c,0x43,0x00,0x00,0x3c,0x04,0x0f,0x00,0x3c,0x02,0x04,0x00,0x00,0x64,0x18,0x24,
-0x10,0x62,0x00,0x03,0x00,0x00,0x00,0x00,0x0c,0x00,0x04,0x96,0x00,0x00,0x00,0x00,
-0x8f,0xbf,0x00,0x30,0x7b,0xb6,0x01,0x7c,0x7b,0xb4,0x01,0x3c,0x7b,0xb2,0x00,0xfc,
-0x7b,0xb0,0x00,0xbc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x38,0x8e,0x09,0x00,0x04,
-0x24,0x15,0x00,0x01,0x8e,0x06,0x00,0x0c,0x00,0x09,0x11,0x42,0x00,0x09,0x18,0xc2,
-0x30,0x48,0x00,0x03,0x00,0x09,0x14,0x02,0x30,0x6c,0x00,0x03,0x00,0x09,0x26,0x02,
-0x11,0x15,0x00,0x45,0x30,0x43,0x00,0x0f,0x29,0x02,0x00,0x02,0x14,0x40,0x00,0x26,
-0x00,0x00,0x00,0x00,0x11,0x13,0x00,0x0f,0x00,0x00,0x38,0x21,0x00,0x07,0x22,0x02,
-0x30,0x84,0xff,0x00,0x3c,0x03,0x00,0xff,0x00,0x07,0x2e,0x02,0x00,0x07,0x12,0x00,
-0x00,0x43,0x10,0x24,0x00,0xa4,0x28,0x25,0x00,0xa2,0x28,0x25,0x00,0x07,0x1e,0x00,
-0x00,0xa3,0x28,0x25,0x0c,0x00,0x01,0x92,0x01,0x20,0x20,0x21,0x08,0x00,0x03,0xa5,
-0x02,0x51,0x10,0x21,0x11,0x95,0x00,0x0f,0x00,0x00,0x00,0x00,0x11,0x88,0x00,0x07,
-0x00,0x00,0x00,0x00,0x00,0x04,0x10,0x80,0x27,0x83,0x8b,0x70,0x00,0x43,0x10,0x21,
-0x8c,0x47,0x00,0x18,0x08,0x00,0x03,0xcc,0x00,0x07,0x22,0x02,0x00,0x04,0x10,0x40,
-0x27,0x83,0x8b,0x78,0x00,0x43,0x10,0x21,0x94,0x47,0x00,0x02,0x08,0x00,0x03,0xcc,
-0x00,0x07,0x22,0x02,0x27,0x82,0x8b,0x70,0x00,0x82,0x10,0x21,0x90,0x47,0x00,0x00,
-0x08,0x00,0x03,0xcc,0x00,0x07,0x22,0x02,0x15,0x00,0xff,0xdc,0x00,0x00,0x38,0x21,
-0x10,0x75,0x00,0x05,0x00,0x80,0x38,0x21,0x00,0x65,0x18,0x26,0x24,0x82,0x01,0x00,
-0x00,0x00,0x38,0x21,0x00,0x43,0x38,0x0a,0x24,0x02,0x00,0x01,0x11,0x82,0x00,0x0e,
-0x3c,0x02,0xb0,0x03,0x24,0x02,0x00,0x02,0x11,0x82,0x00,0x06,0x00,0x00,0x00,0x00,
-0x3c,0x02,0xb0,0x03,0x00,0xe2,0x10,0x21,0x8c,0x47,0x00,0x00,0x08,0x00,0x03,0xcc,
-0x00,0x07,0x22,0x02,0x3c,0x02,0xb0,0x03,0x00,0xe2,0x10,0x21,0x94,0x43,0x00,0x00,
-0x08,0x00,0x03,0xcb,0x30,0x67,0xff,0xff,0x00,0xe2,0x10,0x21,0x90,0x43,0x00,0x00,
-0x08,0x00,0x03,0xcb,0x30,0x67,0x00,0xff,0x30,0x62,0x00,0x03,0x00,0x02,0x12,0x00,
-0x11,0x95,0x00,0x07,0x00,0x44,0x38,0x21,0x11,0x93,0x00,0x03,0x00,0x00,0x00,0x00,
-0x08,0x00,0x03,0xfd,0x3c,0x02,0xb0,0x0a,0x08,0x00,0x04,0x02,0x3c,0x02,0xb0,0x0a,
-0x08,0x00,0x04,0x06,0x3c,0x02,0xb0,0x0a,0x8e,0x09,0x00,0x04,0x8e,0x02,0x00,0x08,
-0x8e,0x03,0x00,0x0c,0x00,0x09,0x41,0x42,0x00,0x02,0x22,0x02,0x00,0x03,0x3a,0x02,
-0x30,0x84,0xff,0x00,0x30,0xe7,0xff,0x00,0x00,0x02,0x5e,0x02,0x00,0x02,0x32,0x00,
-0x00,0x03,0x56,0x02,0x00,0x03,0x2a,0x00,0x01,0x64,0x58,0x25,0x00,0xd6,0x30,0x24,
-0x01,0x47,0x50,0x25,0x00,0x02,0x16,0x00,0x00,0xb6,0x28,0x24,0x00,0x03,0x1e,0x00,
-0x01,0x66,0x58,0x25,0x01,0x45,0x50,0x25,0x00,0x57,0x10,0x24,0x00,0x77,0x18,0x24,
-0x01,0x62,0x38,0x25,0x01,0x43,0x30,0x25,0x00,0x09,0x10,0xc2,0x00,0x09,0x1c,0x02,
-0x31,0x08,0x00,0x03,0x30,0x4c,0x00,0x03,0x30,0x63,0x00,0x0f,0x00,0x09,0x26,0x02,
-0x00,0xe0,0x58,0x21,0x15,0x00,0x00,0x28,0x00,0xc0,0x50,0x21,0x24,0x02,0x00,0x01,
-0x10,0x62,0x00,0x06,0x00,0x80,0x28,0x21,0x24,0x02,0x00,0x03,0x14,0x62,0xff,0x69,
-0x02,0x51,0x10,0x21,0x24,0x85,0x01,0x00,0x24,0x02,0x00,0x01,0x11,0x82,0x00,0x15,
-0x24,0x02,0x00,0x02,0x11,0x82,0x00,0x0a,0x3c,0x03,0xb0,0x03,0x00,0xa3,0x18,0x21,
-0x8c,0x62,0x00,0x00,0x00,0x0a,0x20,0x27,0x01,0x6a,0x28,0x24,0x00,0x44,0x10,0x24,
-0x00,0x45,0x10,0x25,0xac,0x62,0x00,0x00,0x08,0x00,0x03,0xa5,0x02,0x51,0x10,0x21,
-0x00,0xa3,0x18,0x21,0x94,0x62,0x00,0x00,0x00,0x0a,0x20,0x27,0x01,0x6a,0x28,0x24,
-0x00,0x44,0x10,0x24,0x00,0x45,0x10,0x25,0xa4,0x62,0x00,0x00,0x08,0x00,0x03,0xa5,
-0x02,0x51,0x10,0x21,0x3c,0x03,0xb0,0x03,0x00,0xa3,0x18,0x21,0x90,0x62,0x00,0x00,
-0x00,0x0a,0x20,0x27,0x01,0x6a,0x28,0x24,0x00,0x44,0x10,0x24,0x00,0x45,0x10,0x25,
-0x08,0x00,0x03,0xa4,0xa0,0x62,0x00,0x00,0x24,0x02,0x00,0x01,0x11,0x02,0x00,0x21,
-0x00,0x00,0x00,0x00,0x15,0x13,0xff,0x42,0x00,0x00,0x00,0x00,0x11,0x82,0x00,0x17,
-0x00,0x00,0x00,0x00,0x11,0x88,0x00,0x0b,0x00,0x00,0x00,0x00,0x27,0x83,0x8b,0x70,
-0x00,0x04,0x20,0x80,0x00,0x83,0x20,0x21,0x8c,0x82,0x00,0x18,0x00,0x06,0x18,0x27,
-0x00,0xe6,0x28,0x24,0x00,0x43,0x10,0x24,0x00,0x45,0x10,0x25,0x08,0x00,0x03,0xa4,
-0xac,0x82,0x00,0x18,0x27,0x83,0x8b,0x78,0x00,0x04,0x20,0x40,0x00,0x83,0x20,0x21,
-0x94,0x82,0x00,0x02,0x00,0x06,0x18,0x27,0x00,0xe6,0x28,0x24,0x00,0x43,0x10,0x24,
-0x00,0x45,0x10,0x25,0x08,0x00,0x03,0xa4,0xa4,0x82,0x00,0x02,0x27,0x83,0x8b,0x70,
-0x00,0x83,0x18,0x21,0x90,0x62,0x00,0x00,0x00,0x06,0x20,0x27,0x08,0x00,0x04,0x5a,
-0x00,0xe6,0x28,0x24,0x30,0x62,0x00,0x07,0x00,0x02,0x12,0x00,0x11,0x88,0x00,0x0f,
-0x00,0x44,0x10,0x21,0x11,0x93,0x00,0x07,0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x0a,
-0x00,0x43,0x18,0x21,0x8c,0x62,0x00,0x00,0x00,0x06,0x20,0x27,0x08,0x00,0x04,0x47,
-0x00,0xe6,0x28,0x24,0x3c,0x03,0xb0,0x0a,0x00,0x43,0x18,0x21,0x94,0x62,0x00,0x00,
-0x00,0x06,0x20,0x27,0x08,0x00,0x04,0x50,0x00,0xe6,0x28,0x24,0x3c,0x03,0xb0,0x0a,
-0x08,0x00,0x04,0x7d,0x00,0x43,0x18,0x21,0x97,0x85,0x8b,0xc4,0x3c,0x07,0xb0,0x02,
-0x3c,0x04,0xb0,0x03,0x3c,0x02,0x80,0x00,0x00,0xa7,0x28,0x21,0x34,0x84,0x00,0x20,
-0x24,0x42,0x12,0x58,0x24,0x03,0xff,0x80,0xac,0x82,0x00,0x00,0xa0,0xa3,0x00,0x07,
-0x97,0x82,0x8b,0xc6,0x97,0x85,0x8b,0xc4,0x3c,0x06,0xb0,0x06,0x30,0x42,0xff,0xf8,
-0x24,0x42,0x00,0x10,0x00,0xa2,0x10,0x21,0x30,0x42,0x0f,0xff,0x24,0x44,0x00,0x08,
-0x30,0x84,0x0f,0xff,0x00,0x05,0x28,0xc2,0x3c,0x03,0x00,0x40,0x00,0xa3,0x28,0x25,
-0x00,0x87,0x20,0x21,0x34,0xc6,0x80,0x18,0xac,0xc5,0x00,0x00,0xaf,0x84,0x8b,0xc0,
-0xa7,0x82,0x8b,0xc4,0xa7,0x80,0x8b,0xc6,0xaf,0x80,0x8b,0xc8,0x03,0xe0,0x00,0x08,
-0x00,0x00,0x00,0x00,0x30,0xa5,0x00,0xff,0x30,0x84,0x00,0xff,0x24,0x02,0x00,0x01,
-0x00,0xe0,0x48,0x21,0x30,0xc6,0x00,0xff,0x8f,0xa7,0x00,0x10,0x10,0x82,0x00,0x07,
-0x00,0xa0,0x40,0x21,0x24,0x02,0x00,0x03,0x10,0x82,0x00,0x03,0x00,0x00,0x00,0x00,
-0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x24,0xa8,0x01,0x00,0x3c,0x03,0xb0,0x03,
-0x24,0x02,0x00,0x01,0x00,0x07,0x20,0x27,0x01,0x27,0x28,0x24,0x10,0xc2,0x00,0x14,
-0x01,0x03,0x18,0x21,0x24,0x02,0x00,0x02,0x10,0xc2,0x00,0x09,0x00,0x07,0x50,0x27,
-0x3c,0x03,0xb0,0x03,0x01,0x03,0x18,0x21,0x8c,0x62,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x4a,0x10,0x24,0x00,0x45,0x10,0x25,0x08,0x00,0x04,0xe1,0xac,0x62,0x00,0x00,
-0x3c,0x03,0xb0,0x03,0x01,0x03,0x18,0x21,0x94,0x62,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x4a,0x10,0x24,0x00,0x45,0x10,0x25,0x03,0xe0,0x00,0x08,0xa4,0x62,0x00,0x00,
-0x90,0x62,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x44,0x10,0x24,0x00,0x45,0x10,0x25,
-0xa0,0x62,0x00,0x00,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x30,0x84,0x00,0x07,
-0x00,0x04,0x22,0x00,0x30,0xa5,0x00,0xff,0x00,0x85,0x28,0x21,0x3c,0x02,0xb0,0x0a,
-0x00,0xa2,0x40,0x21,0x30,0xc6,0x00,0xff,0x24,0x02,0x00,0x01,0x8f,0xa4,0x00,0x10,
-0x10,0xc2,0x00,0x14,0x24,0x02,0x00,0x02,0x00,0x04,0x50,0x27,0x10,0xc2,0x00,0x09,
-0x00,0xe4,0x48,0x24,0x3c,0x03,0xb0,0x0a,0x00,0xa3,0x18,0x21,0x8c,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x4a,0x10,0x24,0x00,0x49,0x10,0x25,0x03,0xe0,0x00,0x08,
-0xac,0x62,0x00,0x00,0x3c,0x03,0xb0,0x0a,0x00,0xa3,0x18,0x21,0x94,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x4a,0x10,0x24,0x00,0x49,0x10,0x25,0x03,0xe0,0x00,0x08,
-0xa4,0x62,0x00,0x00,0x91,0x02,0x00,0x00,0x00,0x04,0x18,0x27,0x00,0xe4,0x20,0x24,
-0x00,0x43,0x10,0x24,0x00,0x44,0x10,0x25,0x03,0xe0,0x00,0x08,0xa1,0x02,0x00,0x00,
-0x30,0xa9,0x00,0xff,0x27,0x83,0x8b,0x70,0x30,0x85,0x00,0xff,0x24,0x02,0x00,0x01,
-0x00,0x07,0x50,0x27,0x00,0xc7,0x40,0x24,0x11,0x22,0x00,0x17,0x00,0xa3,0x18,0x21,
-0x00,0x05,0x20,0x40,0x27,0x82,0x8b,0x70,0x00,0x05,0x28,0x80,0x27,0x83,0x8b,0x78,
-0x00,0x83,0x50,0x21,0x00,0xa2,0x20,0x21,0x24,0x02,0x00,0x02,0x00,0x07,0x40,0x27,
-0x11,0x22,0x00,0x07,0x00,0xc7,0x28,0x24,0x8c,0x82,0x00,0x18,0x00,0x00,0x00,0x00,
-0x00,0x48,0x10,0x24,0x00,0x45,0x10,0x25,0x03,0xe0,0x00,0x08,0xac,0x82,0x00,0x18,
-0x95,0x42,0x00,0x02,0x00,0x00,0x00,0x00,0x00,0x48,0x10,0x24,0x00,0x45,0x10,0x25,
-0x03,0xe0,0x00,0x08,0xa5,0x42,0x00,0x02,0x90,0x62,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x4a,0x10,0x24,0x00,0x48,0x10,0x25,0x03,0xe0,0x00,0x08,0xa0,0x62,0x00,0x00,
-0x00,0x04,0x32,0x02,0x30,0xc6,0xff,0x00,0x00,0x04,0x16,0x02,0x00,0x04,0x1a,0x00,
-0x3c,0x05,0x00,0xff,0x00,0x65,0x18,0x24,0x00,0x46,0x10,0x25,0x00,0x43,0x10,0x25,
-0x00,0x04,0x26,0x00,0x03,0xe0,0x00,0x08,0x00,0x44,0x10,0x25,0x3c,0x03,0xb0,0x03,
-0x3c,0x02,0x80,0x00,0x27,0xbd,0xff,0xe8,0x34,0x63,0x00,0x20,0x24,0x42,0x14,0xdc,
-0x3c,0x04,0xb0,0x03,0xaf,0xbf,0x00,0x14,0xac,0x62,0x00,0x00,0xaf,0xb0,0x00,0x10,
-0x34,0x84,0x00,0x2c,0x8c,0x83,0x00,0x00,0xa7,0x80,0xbc,0x00,0x00,0x03,0x12,0x02,
-0x00,0x03,0x2d,0x02,0x30,0x42,0x0f,0xff,0xa3,0x83,0xbc,0x08,0xa7,0x85,0xbc,0x0c,
-0xa7,0x82,0xbc,0x0a,0xa7,0x80,0xbc,0x02,0xa7,0x80,0xbc,0x04,0xa7,0x80,0xbc,0x06,
-0x0c,0x00,0x06,0xd1,0x24,0x04,0x05,0x00,0x3c,0x05,0x08,0x00,0x00,0x45,0x28,0x25,
-0x24,0x04,0x05,0x00,0x0c,0x00,0x06,0xbf,0x00,0x40,0x80,0x21,0x3c,0x02,0xf7,0xff,
-0x34,0x42,0xff,0xff,0x02,0x02,0x80,0x24,0x02,0x00,0x28,0x21,0x0c,0x00,0x06,0xbf,
-0x24,0x04,0x05,0x00,0x3c,0x02,0xb0,0x03,0x3c,0x03,0xb0,0x03,0x34,0x42,0x01,0x08,
-0x34,0x63,0x01,0x18,0x8c,0x45,0x00,0x00,0x8c,0x64,0x00,0x00,0x3c,0x02,0x00,0x0f,
-0x3c,0x03,0x00,0x4c,0x30,0x84,0x02,0x00,0x34,0x63,0x4b,0x40,0xaf,0x85,0xbc,0x10,
-0x10,0x80,0x00,0x06,0x34,0x42,0x42,0x40,0xaf,0x83,0xbc,0x14,0x8f,0xbf,0x00,0x14,
-0x8f,0xb0,0x00,0x10,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x18,0xaf,0x82,0xbc,0x14,
-0x08,0x00,0x05,0x67,0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,
-0x27,0xbd,0xff,0xc8,0x34,0x63,0x00,0x20,0x24,0x42,0x15,0xb8,0x30,0x84,0x00,0xff,
-0xaf,0xbf,0x00,0x30,0xaf,0xb7,0x00,0x2c,0xaf,0xb6,0x00,0x28,0xaf,0xb5,0x00,0x24,
-0xaf,0xb4,0x00,0x20,0xaf,0xb3,0x00,0x1c,0xaf,0xb2,0x00,0x18,0xaf,0xb1,0x00,0x14,
-0xaf,0xb0,0x00,0x10,0xac,0x62,0x00,0x00,0x10,0x80,0x00,0x1c,0x24,0x02,0x00,0x02,
-0x10,0x82,0x00,0x08,0x00,0x00,0x00,0x00,0x8f,0xbf,0x00,0x30,0x7b,0xb6,0x01,0x7c,
-0x7b,0xb4,0x01,0x3c,0x7b,0xb2,0x00,0xfc,0x7b,0xb0,0x00,0xbc,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x38,0xa7,0x80,0xbc,0x00,0xa7,0x80,0xbc,0x02,0xa7,0x80,0xbc,0x04,
-0xa7,0x80,0xbc,0x06,0x0c,0x00,0x06,0xd1,0x24,0x04,0x05,0x00,0x3c,0x05,0x08,0x00,
-0x00,0x45,0x28,0x25,0x24,0x04,0x05,0x00,0x0c,0x00,0x06,0xbf,0x00,0x40,0x80,0x21,
-0x3c,0x05,0xf7,0xff,0x34,0xa5,0xff,0xff,0x02,0x05,0x28,0x24,0x0c,0x00,0x06,0xbf,
-0x24,0x04,0x05,0x00,0x08,0x00,0x05,0x82,0x00,0x00,0x00,0x00,0x0c,0x00,0x06,0xd1,
-0x24,0x04,0x05,0xa0,0x24,0x04,0x05,0xa4,0x0c,0x00,0x06,0xd1,0x00,0x02,0xbc,0x02,
-0x24,0x04,0x05,0xa8,0x00,0x02,0xb4,0x02,0x0c,0x00,0x06,0xd1,0x30,0x55,0xff,0xff,
-0x00,0x40,0x80,0x21,0x97,0x84,0xbc,0x00,0x97,0x82,0xbc,0x02,0x97,0x83,0xbc,0x06,
-0x02,0xe4,0x20,0x23,0x02,0xa2,0x10,0x23,0x00,0x82,0x20,0x21,0x97,0x82,0xbc,0x04,
-0x32,0x14,0xff,0xff,0x02,0x83,0x18,0x23,0x02,0xc2,0x10,0x23,0x00,0x82,0x20,0x21,
-0x93,0x82,0xbc,0x08,0x00,0x83,0x20,0x21,0x30,0x84,0xff,0xff,0x00,0x82,0x10,0x2b,
-0x14,0x40,0x00,0xaa,0x00,0x00,0x00,0x00,0x97,0x82,0xbc,0x0c,0x00,0x00,0x00,0x00,
-0x00,0x44,0x10,0x2b,0x14,0x40,0x00,0x7f,0x00,0x00,0x00,0x00,0x97,0x82,0xbc,0x0a,
-0x00,0x00,0x00,0x00,0x00,0x44,0x10,0x2b,0x10,0x40,0x00,0x3a,0x00,0x00,0x00,0x00,
-0x0c,0x00,0x06,0xd1,0x24,0x04,0x04,0x50,0x30,0x51,0x00,0x7f,0x00,0x40,0x80,0x21,
-0x2e,0x22,0x00,0x32,0x10,0x40,0x00,0x13,0x24,0x02,0x00,0x20,0x12,0x22,0x00,0x17,
-0x24,0x02,0xff,0x80,0x02,0x02,0x10,0x24,0x26,0x31,0x00,0x01,0x00,0x51,0x80,0x25,
-0x02,0x00,0x28,0x21,0x0c,0x00,0x06,0xbf,0x24,0x04,0x04,0x50,0x02,0x00,0x28,0x21,
-0x0c,0x00,0x06,0xbf,0x24,0x04,0x04,0x58,0x02,0x00,0x28,0x21,0x0c,0x00,0x06,0xbf,
-0x24,0x04,0x04,0x60,0x02,0x00,0x28,0x21,0x24,0x04,0x04,0x68,0x0c,0x00,0x06,0xbf,
-0x00,0x00,0x00,0x00,0xa7,0x97,0xbc,0x00,0xa7,0x95,0xbc,0x02,0xa7,0x96,0xbc,0x04,
-0xa7,0x94,0xbc,0x06,0x08,0x00,0x05,0x82,0x00,0x00,0x00,0x00,0x0c,0x00,0x06,0xd1,
-0x24,0x04,0x02,0x08,0x3c,0x04,0x00,0xc0,0x00,0x40,0x28,0x21,0x00,0x44,0x10,0x24,
-0x00,0x02,0x15,0x82,0x24,0x03,0x00,0x03,0x10,0x43,0x00,0x07,0x00,0x00,0x00,0x00,
-0x3c,0x02,0xff,0x3f,0x34,0x42,0xff,0xff,0x00,0xa2,0x10,0x24,0x00,0x44,0x28,0x25,
-0x0c,0x00,0x06,0xbf,0x24,0x04,0x02,0x08,0x0c,0x00,0x06,0xd1,0x24,0x04,0x02,0x2c,
-0x00,0x40,0x90,0x21,0x3c,0x02,0xff,0xff,0x34,0x42,0x3f,0xff,0x02,0x42,0x90,0x24,
-0x02,0x40,0x28,0x21,0x0c,0x00,0x06,0xbf,0x24,0x04,0x02,0x2c,0x08,0x00,0x05,0xc9,
-0x24,0x02,0xff,0x80,0x0c,0x00,0x06,0xd1,0x24,0x04,0x04,0x50,0x30,0x51,0x00,0x7f,
-0x24,0x02,0x00,0x20,0x16,0x22,0xff,0xdb,0x00,0x00,0x00,0x00,0x0c,0x00,0x06,0xd1,
-0x24,0x04,0x02,0x2c,0x34,0x52,0x40,0x00,0x02,0x40,0x28,0x21,0x0c,0x00,0x06,0xbf,
-0x24,0x04,0x02,0x2c,0x0c,0x00,0x06,0xd1,0x24,0x04,0x02,0x58,0x24,0x04,0x02,0x5c,
-0x0c,0x00,0x06,0xd1,0x00,0x02,0x9e,0x02,0x30,0x43,0x00,0xff,0x00,0x13,0x12,0x00,
-0x00,0x43,0x10,0x25,0x2c,0x43,0x00,0x04,0x14,0x60,0x00,0x1d,0x2c,0x42,0x00,0x11,
-0x10,0x40,0x00,0x0b,0x00,0x00,0x00,0x00,0x3c,0x02,0xff,0xff,0x34,0x42,0x3f,0xff,
-0x02,0x42,0x90,0x24,0x02,0x40,0x28,0x21,0x24,0x04,0x02,0x2c,0x0c,0x00,0x06,0xbf,
-0x36,0x52,0x80,0x00,0x02,0x40,0x28,0x21,0x08,0x00,0x05,0xd7,0x24,0x04,0x02,0x2c,
-0x0c,0x00,0x06,0xd1,0x24,0x04,0x02,0x08,0x3c,0x04,0x00,0xc0,0x00,0x40,0x28,0x21,
-0x00,0x44,0x10,0x24,0x00,0x02,0x15,0x82,0x24,0x03,0x00,0x02,0x14,0x43,0xff,0xee,
-0x3c,0x02,0xff,0x3f,0x34,0x42,0xff,0xff,0x00,0xa2,0x10,0x24,0x00,0x44,0x28,0x25,
-0x0c,0x00,0x06,0xbf,0x24,0x04,0x02,0x08,0x08,0x00,0x06,0x13,0x3c,0x02,0xff,0xff,
-0x0c,0x00,0x06,0xd1,0x24,0x04,0x02,0x08,0x00,0x40,0x28,0x21,0x00,0x02,0x15,0x82,
-0x30,0x42,0x00,0x03,0x24,0x03,0x00,0x03,0x14,0x43,0xff,0xdf,0x3c,0x02,0xff,0x3f,
-0x34,0x42,0xff,0xff,0x00,0xa2,0x10,0x24,0x3c,0x03,0x00,0x80,0x08,0x00,0x06,0x28,
-0x00,0x43,0x28,0x25,0x0c,0x00,0x06,0xd1,0x24,0x04,0x04,0x50,0x30,0x51,0x00,0x7f,
-0x00,0x40,0x80,0x21,0x2e,0x22,0x00,0x32,0x10,0x40,0xff,0x9a,0x24,0x02,0x00,0x20,
-0x12,0x22,0x00,0x04,0x24,0x02,0xff,0x80,0x02,0x02,0x10,0x24,0x08,0x00,0x05,0xcb,
-0x26,0x31,0x00,0x02,0x0c,0x00,0x06,0xd1,0x24,0x04,0x02,0x08,0x3c,0x04,0x00,0xc0,
-0x00,0x40,0x28,0x21,0x00,0x44,0x10,0x24,0x00,0x02,0x15,0x82,0x24,0x03,0x00,0x03,
-0x10,0x43,0x00,0x07,0x00,0x00,0x00,0x00,0x3c,0x02,0xff,0x3f,0x34,0x42,0xff,0xff,
-0x00,0xa2,0x10,0x24,0x00,0x44,0x28,0x25,0x0c,0x00,0x06,0xbf,0x24,0x04,0x02,0x08,
-0x0c,0x00,0x06,0xd1,0x24,0x04,0x02,0x2c,0x00,0x40,0x90,0x21,0x3c,0x02,0xff,0xff,
-0x34,0x42,0x3f,0xff,0x02,0x42,0x90,0x24,0x02,0x40,0x28,0x21,0x0c,0x00,0x06,0xbf,
-0x24,0x04,0x02,0x2c,0x08,0x00,0x06,0x42,0x24,0x02,0xff,0x80,0x0c,0x00,0x06,0xd1,
-0x24,0x04,0x04,0x50,0x00,0x40,0x80,0x21,0x30,0x51,0x00,0x7f,0x24,0x02,0x00,0x20,
-0x12,0x22,0x00,0x1d,0x2e,0x22,0x00,0x21,0x14,0x40,0xff,0x72,0x24,0x02,0xff,0x80,
-0x02,0x02,0x10,0x24,0x26,0x31,0xff,0xff,0x00,0x51,0x80,0x25,0x24,0x04,0x04,0x50,
-0x0c,0x00,0x06,0xbf,0x02,0x00,0x28,0x21,0x24,0x04,0x04,0x58,0x0c,0x00,0x06,0xbf,
-0x02,0x00,0x28,0x21,0x24,0x04,0x04,0x60,0x0c,0x00,0x06,0xbf,0x02,0x00,0x28,0x21,
-0x02,0x00,0x28,0x21,0x0c,0x00,0x06,0xbf,0x24,0x04,0x04,0x68,0x24,0x02,0x00,0x20,
-0x16,0x22,0xff,0x60,0x00,0x00,0x00,0x00,0x0c,0x00,0x06,0xd1,0x24,0x04,0x02,0x2c,
-0x00,0x40,0x90,0x21,0x3c,0x02,0xff,0xff,0x34,0x42,0x3f,0xff,0x02,0x42,0x10,0x24,
-0x08,0x00,0x06,0x19,0x34,0x52,0x80,0x00,0x0c,0x00,0x06,0xd1,0x24,0x04,0x02,0x2c,
-0x34,0x52,0x40,0x00,0x02,0x40,0x28,0x21,0x0c,0x00,0x06,0xbf,0x24,0x04,0x02,0x2c,
-0x0c,0x00,0x06,0xd1,0x24,0x04,0x02,0x58,0x24,0x04,0x02,0x5c,0x0c,0x00,0x06,0xd1,
-0x00,0x02,0x9e,0x02,0x30,0x43,0x00,0xff,0x00,0x13,0x12,0x00,0x00,0x43,0x10,0x25,
-0x2c,0x43,0x00,0x04,0x14,0x60,0x00,0x20,0x2c,0x42,0x00,0x11,0x10,0x40,0x00,0x0d,
-0x00,0x00,0x00,0x00,0x3c,0x02,0xff,0xff,0x34,0x42,0x3f,0xff,0x02,0x42,0x90,0x24,
-0x02,0x40,0x28,0x21,0x24,0x04,0x02,0x2c,0x0c,0x00,0x06,0xbf,0x36,0x52,0x80,0x00,
-0x02,0x40,0x28,0x21,0x0c,0x00,0x06,0xbf,0x24,0x04,0x02,0x2c,0x08,0x00,0x06,0x66,
-0x2e,0x22,0x00,0x21,0x0c,0x00,0x06,0xd1,0x24,0x04,0x02,0x08,0x3c,0x04,0x00,0xc0,
-0x00,0x40,0x28,0x21,0x00,0x44,0x10,0x24,0x00,0x02,0x15,0x82,0x24,0x03,0x00,0x02,
-0x14,0x43,0xff,0xec,0x00,0x00,0x00,0x00,0x3c,0x02,0xff,0x3f,0x34,0x42,0xff,0xff,
-0x00,0xa2,0x10,0x24,0x00,0x44,0x28,0x25,0x0c,0x00,0x06,0xbf,0x24,0x04,0x02,0x08,
-0x08,0x00,0x06,0x96,0x3c,0x02,0xff,0xff,0x0c,0x00,0x06,0xd1,0x24,0x04,0x02,0x08,
-0x00,0x40,0x28,0x21,0x00,0x02,0x15,0x82,0x30,0x42,0x00,0x03,0x24,0x03,0x00,0x03,
-0x14,0x43,0xff,0xdc,0x3c,0x03,0x00,0x80,0x3c,0x02,0xff,0x3f,0x34,0x42,0xff,0xff,
-0x00,0xa2,0x10,0x24,0x08,0x00,0x06,0xae,0x00,0x43,0x28,0x25,0x30,0x83,0x00,0x03,
-0x00,0x04,0x20,0x40,0x00,0x83,0x20,0x23,0x3c,0x02,0xb0,0x0a,0x00,0x82,0x20,0x21,
-0x3c,0x06,0x00,0x01,0xac,0x85,0x00,0x00,0x24,0x07,0x00,0x01,0x00,0x00,0x28,0x21,
-0x34,0xc6,0x86,0x9f,0x8c,0x82,0x10,0x00,0x24,0xa5,0x00,0x01,0x10,0x47,0x00,0x03,
-0x00,0xc5,0x18,0x2b,0x10,0x60,0xff,0xfb,0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,
-0x00,0x00,0x00,0x00,0x30,0x83,0x00,0x03,0x00,0x04,0x20,0x40,0x3c,0x02,0xb0,0x0a,
-0x00,0x83,0x20,0x23,0x00,0x82,0x20,0x21,0x3c,0x06,0x00,0x01,0x24,0x02,0xff,0xff,
-0xac,0x82,0x10,0x00,0x00,0x00,0x28,0x21,0x24,0x07,0x00,0x01,0x34,0xc6,0x86,0x9f,
-0x8c,0x82,0x10,0x00,0x24,0xa5,0x00,0x01,0x10,0x47,0x00,0x03,0x00,0xc5,0x18,0x2b,
-0x10,0x60,0xff,0xfb,0x00,0x00,0x00,0x00,0x8c,0x82,0x00,0x00,0x03,0xe0,0x00,0x08,
-0x00,0x00,0x00,0x00,0x3c,0x05,0xb0,0x03,0x3c,0x02,0x80,0x00,0x24,0x42,0x1b,0x94,
-0x24,0x03,0x00,0x01,0x34,0xa5,0x00,0x20,0x3c,0x06,0xb0,0x03,0xac,0xa2,0x00,0x00,
-0x34,0xc6,0x01,0x04,0xa0,0x83,0x00,0x48,0xa0,0x80,0x00,0x04,0xa0,0x80,0x00,0x05,
-0xa0,0x80,0x00,0x06,0xa0,0x80,0x00,0x07,0xa0,0x80,0x00,0x08,0xa0,0x80,0x00,0x09,
-0xa0,0x80,0x00,0x0a,0xa0,0x80,0x00,0x11,0xa0,0x80,0x00,0x13,0xa0,0x80,0x00,0x49,
-0x94,0xc2,0x00,0x00,0xac,0x80,0x00,0x00,0xa0,0x80,0x00,0x4e,0x00,0x02,0x14,0x00,
-0x00,0x02,0x14,0x03,0x30,0x43,0x00,0xff,0x30,0x42,0xff,0x00,0xa4,0x82,0x00,0x44,
-0xa4,0x83,0x00,0x46,0xac,0x80,0x00,0x24,0xac,0x80,0x00,0x28,0xac,0x80,0x00,0x2c,
-0xac,0x80,0x00,0x30,0xac,0x80,0x00,0x34,0xac,0x80,0x00,0x38,0xac,0x80,0x00,0x3c,
-0x03,0xe0,0x00,0x08,0xac,0x80,0x00,0x40,0x84,0x83,0x00,0x0c,0x3c,0x07,0xb0,0x03,
-0x34,0xe7,0x00,0x20,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,
-0x27,0x83,0x90,0x04,0x00,0x43,0x10,0x21,0x8c,0x48,0x00,0x18,0x3c,0x02,0x80,0x00,
-0x24,0x42,0x1c,0x28,0xac,0xe2,0x00,0x00,0x8d,0x03,0x00,0x08,0x80,0x82,0x00,0x13,
-0x00,0x05,0x2c,0x00,0x00,0x03,0x1e,0x02,0x00,0x02,0x12,0x00,0x30,0x63,0x00,0x7e,
-0x00,0x62,0x18,0x21,0x00,0x65,0x18,0x21,0x3c,0x02,0xc0,0x00,0x3c,0x05,0xb0,0x05,
-0x34,0x42,0x04,0x00,0x24,0x63,0x00,0x01,0x3c,0x07,0xb0,0x05,0x3c,0x08,0xb0,0x05,
-0x34,0xa5,0x04,0x20,0xac,0xa3,0x00,0x00,0x00,0xc2,0x30,0x21,0x34,0xe7,0x04,0x24,
-0x35,0x08,0x02,0x28,0x24,0x02,0x00,0x01,0x24,0x03,0x00,0x20,0xac,0xe6,0x00,0x00,
-0xac,0x82,0x00,0x3c,0x03,0xe0,0x00,0x08,0xa1,0x03,0x00,0x00,0x27,0xbd,0xff,0xa8,
-0x00,0x07,0x60,0x80,0x27,0x82,0xb4,0x00,0xaf,0xbe,0x00,0x50,0xaf,0xb7,0x00,0x4c,
-0xaf,0xb5,0x00,0x44,0xaf,0xb4,0x00,0x40,0xaf,0xbf,0x00,0x54,0xaf,0xb6,0x00,0x48,
-0xaf,0xb3,0x00,0x3c,0xaf,0xb2,0x00,0x38,0xaf,0xb1,0x00,0x34,0xaf,0xb0,0x00,0x30,
-0x01,0x82,0x10,0x21,0x8c,0x43,0x00,0x00,0x00,0xe0,0x70,0x21,0x3c,0x02,0x80,0x00,
-0x94,0x73,0x00,0x14,0x3c,0x07,0xb0,0x03,0x34,0xe7,0x00,0x20,0x24,0x42,0x1c,0xbc,
-0x3c,0x03,0xb0,0x05,0xac,0xe2,0x00,0x00,0x34,0x63,0x01,0x28,0x90,0x67,0x00,0x00,
-0x00,0x13,0xa8,0xc0,0x02,0xb3,0x18,0x21,0x27,0x82,0x90,0x04,0x00,0x03,0x18,0x80,
-0x00,0x62,0x18,0x21,0x00,0x05,0x2c,0x00,0x00,0x07,0x3e,0x00,0x28,0xc2,0x00,0x03,
-0x00,0xc0,0xa0,0x21,0x00,0x80,0x78,0x21,0x00,0x05,0xbc,0x03,0x8c,0x68,0x00,0x18,
-0x02,0xa0,0x58,0x21,0x10,0x40,0x01,0x81,0x00,0x07,0xf6,0x03,0x00,0xde,0x10,0x07,
-0x30,0x5e,0x00,0x01,0x01,0x73,0x10,0x21,0x27,0x83,0x90,0x08,0x00,0x02,0x10,0x80,
-0x00,0x43,0x10,0x21,0x80,0x4d,0x00,0x06,0x8d,0x03,0x00,0x00,0x8d,0x02,0x00,0x04,
-0x8d,0x0a,0x00,0x08,0x8d,0x03,0x00,0x0c,0xaf,0xa2,0x00,0x20,0x11,0xa0,0x01,0x71,
-0xaf,0xa3,0x00,0x18,0x27,0x82,0xb4,0x00,0x01,0x82,0x10,0x21,0x8c,0x44,0x00,0x00,
-0x00,0x00,0x00,0x00,0x90,0x83,0x00,0x16,0x00,0x00,0x00,0x00,0x30,0x63,0x00,0x04,
-0x14,0x60,0x00,0x12,0x00,0x00,0xb0,0x21,0x3c,0x02,0xb0,0x09,0x34,0x42,0x01,0x46,
-0x90,0x43,0x00,0x00,0x2a,0x84,0x00,0x04,0x10,0x80,0x01,0x56,0x30,0x65,0x00,0x01,
-0x91,0xe2,0x00,0x09,0x00,0x00,0x00,0x00,0x12,0x82,0x00,0x02,0x00,0x00,0x00,0x00,
-0x00,0x00,0x28,0x21,0x14,0xa0,0x00,0x03,0x00,0x00,0x38,0x21,0x13,0xc0,0x00,0x03,
-0x38,0xf6,0x00,0x01,0x24,0x07,0x00,0x01,0x38,0xf6,0x00,0x01,0x01,0x73,0x10,0x21,
-0x00,0x02,0x30,0x80,0x27,0x83,0x90,0x10,0x00,0xc3,0x48,0x21,0x91,0x25,0x00,0x00,
-0x8f,0xa4,0x00,0x20,0x2c,0xa3,0x00,0x04,0x00,0x04,0x11,0xc3,0x30,0x42,0x00,0x01,
-0x00,0x03,0xb0,0x0b,0x12,0xc0,0x00,0xd8,0xaf,0xa2,0x00,0x24,0x93,0x90,0xbb,0xea,
-0x00,0x0a,0x16,0x42,0x30,0x52,0x00,0x3f,0x2e,0x06,0x00,0x0c,0x10,0xc0,0x00,0xc0,
-0x00,0xa0,0x20,0x21,0x2c,0xa2,0x00,0x10,0x14,0x40,0x00,0x04,0x00,0x90,0x10,0x2b,
-0x30,0xa2,0x00,0x07,0x24,0x44,0x00,0x04,0x00,0x90,0x10,0x2b,0x10,0x40,0x00,0x0b,
-0x01,0x73,0x10,0x21,0x27,0x85,0xbb,0x1c,0x00,0x10,0x10,0x40,0x00,0x50,0x10,0x21,
-0x00,0x45,0x10,0x21,0x90,0x50,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x90,0x18,0x2b,
-0x14,0x60,0xff,0xfa,0x00,0x10,0x10,0x40,0x01,0x73,0x10,0x21,0x00,0x02,0x10,0x80,
-0x27,0x83,0x90,0x08,0x00,0x43,0x10,0x21,0x31,0xa4,0x00,0x01,0x10,0x80,0x00,0xa5,
-0xa0,0x50,0x00,0x07,0x3c,0x04,0xb0,0x05,0x34,0x84,0x00,0x08,0x24,0x02,0x00,0x01,
-0x3c,0x03,0x80,0x00,0xa1,0xe2,0x00,0x4e,0xac,0x83,0x00,0x00,0x8c,0x85,0x00,0x00,
-0x3c,0x02,0x00,0xf0,0x3c,0x03,0x40,0xf0,0x34,0x42,0xf0,0x00,0x34,0x63,0xf0,0x00,
-0x24,0x17,0x00,0x0e,0x24,0x13,0x01,0x06,0xac,0x82,0x00,0x00,0xac,0x83,0x00,0x00,
-0x27,0x82,0xb4,0x00,0x01,0x82,0x10,0x21,0x8c,0x43,0x00,0x00,0x24,0x05,0x00,0x01,
-0xaf,0xa5,0x00,0x1c,0x90,0x62,0x00,0x16,0x00,0x13,0xa8,0xc0,0x32,0x51,0x00,0x02,
-0x34,0x42,0x00,0x04,0xa0,0x62,0x00,0x16,0x8f,0xa3,0x00,0x20,0x8f,0xa4,0x00,0x18,
-0x00,0x03,0x13,0x43,0x00,0x04,0x1a,0x02,0x30,0x47,0x00,0x01,0x12,0x20,0x00,0x04,
-0x30,0x64,0x07,0xff,0x2e,0x03,0x00,0x04,0x32,0x42,0x00,0x33,0x00,0x43,0x90,0x0b,
-0x8f,0xa5,0x00,0x24,0x8f,0xa6,0x00,0x1c,0x00,0x12,0x10,0x40,0x00,0x05,0x19,0xc0,
-0x00,0x47,0x10,0x21,0x00,0x06,0x2a,0x80,0x00,0x43,0x10,0x21,0x00,0x10,0x32,0x00,
-0x00,0x04,0x24,0x80,0x02,0x65,0x28,0x21,0x00,0xa4,0x28,0x21,0x00,0x46,0x10,0x21,
-0x00,0x17,0x1c,0x00,0x3c,0x04,0xc0,0x00,0x00,0x43,0x30,0x21,0x16,0x80,0x00,0x29,
-0x00,0xa4,0x28,0x21,0x3c,0x02,0xb0,0x05,0x34,0x42,0x04,0x00,0x3c,0x03,0xb0,0x05,
-0x3c,0x04,0xb0,0x05,0xac,0x46,0x00,0x00,0x34,0x63,0x04,0x04,0x34,0x84,0x02,0x28,
-0x24,0x02,0x00,0x01,0xac,0x65,0x00,0x00,0xa0,0x82,0x00,0x00,0x3c,0x02,0xb0,0x09,
-0x34,0x42,0x01,0x46,0x90,0x44,0x00,0x00,0x91,0xe3,0x00,0x09,0x30,0x86,0x00,0x01,
-0x02,0x83,0x18,0x26,0x00,0x03,0x30,0x0b,0x14,0xc0,0x00,0x03,0x00,0x00,0x28,0x21,
-0x13,0xc0,0x00,0x03,0x02,0xb3,0x10,0x21,0x24,0x05,0x00,0x01,0x02,0xb3,0x10,0x21,
-0x27,0x83,0x90,0x08,0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,0x84,0x48,0x00,0x04,
-0x00,0xa0,0x30,0x21,0x00,0xe0,0x20,0x21,0x02,0x80,0x28,0x21,0x02,0xc0,0x38,0x21,
-0x0c,0x00,0x00,0x70,0xaf,0xa8,0x00,0x10,0x7b,0xbe,0x02,0xbc,0x7b,0xb6,0x02,0x7c,
-0x7b,0xb4,0x02,0x3c,0x7b,0xb2,0x01,0xfc,0x7b,0xb0,0x01,0xbc,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x58,0x24,0x02,0x00,0x01,0x12,0x82,0x00,0x3d,0x3c,0x02,0xb0,0x05,
-0x24,0x02,0x00,0x02,0x12,0x82,0x00,0x31,0x3c,0x02,0xb0,0x05,0x24,0x02,0x00,0x03,
-0x12,0x82,0x00,0x25,0x3c,0x02,0xb0,0x05,0x24,0x02,0x00,0x10,0x12,0x82,0x00,0x19,
-0x3c,0x02,0xb0,0x05,0x24,0x02,0x00,0x11,0x12,0x82,0x00,0x0d,0x3c,0x02,0xb0,0x05,
-0x24,0x02,0x00,0x12,0x16,0x82,0xff,0xd1,0x3c,0x02,0xb0,0x05,0x3c,0x03,0xb0,0x05,
-0x34,0x42,0x04,0x20,0x3c,0x04,0xb0,0x05,0x34,0x63,0x04,0x24,0xac,0x46,0x00,0x00,
-0x34,0x84,0x02,0x28,0xac,0x65,0x00,0x00,0x08,0x00,0x07,0xe6,0x24,0x02,0x00,0x20,
-0x34,0x42,0x04,0x40,0x3c,0x03,0xb0,0x05,0x3c,0x04,0xb0,0x05,0xac,0x46,0x00,0x00,
-0x34,0x63,0x04,0x44,0x34,0x84,0x02,0x28,0x24,0x02,0x00,0x40,0x08,0x00,0x07,0xe6,
-0xac,0x65,0x00,0x00,0x34,0x42,0x04,0x28,0x3c,0x03,0xb0,0x05,0x3c,0x04,0xb0,0x05,
-0xac,0x46,0x00,0x00,0x34,0x63,0x04,0x2c,0x34,0x84,0x02,0x28,0x24,0x02,0xff,0x80,
-0x08,0x00,0x07,0xe6,0xac,0x65,0x00,0x00,0x34,0x42,0x04,0x18,0x3c,0x03,0xb0,0x05,
-0x3c,0x04,0xb0,0x05,0xac,0x46,0x00,0x00,0x34,0x63,0x04,0x1c,0x34,0x84,0x02,0x28,
-0x24,0x02,0x00,0x08,0x08,0x00,0x07,0xe6,0xac,0x65,0x00,0x00,0x34,0x42,0x04,0x10,
-0x3c,0x03,0xb0,0x05,0x3c,0x04,0xb0,0x05,0xac,0x46,0x00,0x00,0x34,0x63,0x04,0x14,
-0x34,0x84,0x02,0x28,0x24,0x02,0x00,0x04,0x08,0x00,0x07,0xe6,0xac,0x65,0x00,0x00,
-0x34,0x42,0x04,0x08,0x3c,0x03,0xb0,0x05,0x3c,0x04,0xb0,0x05,0xac,0x46,0x00,0x00,
-0x34,0x63,0x04,0x0c,0x34,0x84,0x02,0x28,0x24,0x02,0x00,0x02,0x08,0x00,0x07,0xe6,
-0xac,0x65,0x00,0x00,0x24,0x17,0x00,0x14,0x08,0x00,0x07,0xb8,0x24,0x13,0x01,0x02,
-0x30,0xa2,0x00,0x07,0x24,0x44,0x00,0x0c,0x00,0x90,0x18,0x2b,0x10,0x60,0x00,0x0c,
-0x26,0x02,0x00,0x04,0x27,0x85,0xbb,0x1c,0x00,0x10,0x10,0x40,0x00,0x50,0x10,0x21,
-0x00,0x45,0x10,0x21,0x90,0x50,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x90,0x18,0x2b,
-0x14,0x60,0xff,0xfa,0x00,0x10,0x10,0x40,0x2e,0x06,0x00,0x0c,0x26,0x02,0x00,0x04,
-0x08,0x00,0x07,0xa2,0x00,0x46,0x80,0x0a,0x27,0x82,0xb4,0x00,0x01,0x82,0x20,0x21,
-0x8c,0x87,0x00,0x00,0x00,0x00,0x00,0x00,0x90,0xe2,0x00,0x19,0x00,0x00,0x00,0x00,
-0x14,0x40,0x00,0x07,0x00,0x00,0x00,0x00,0x27,0x82,0x90,0x20,0x00,0xc2,0x10,0x21,
-0x90,0x43,0x00,0x00,0x00,0x00,0x00,0x00,0x10,0x60,0x00,0x14,0x00,0x00,0x00,0x00,
-0x90,0xe3,0x00,0x16,0x27,0x82,0x90,0x08,0x00,0xc2,0x10,0x21,0x34,0x63,0x00,0x20,
-0x90,0x50,0x00,0x07,0xa0,0xe3,0x00,0x16,0x8c,0x84,0x00,0x00,0x00,0x0a,0x1e,0x42,
-0x24,0x06,0x00,0x01,0x90,0x82,0x00,0x16,0x30,0x71,0x00,0x02,0x30,0x72,0x00,0x3f,
-0x30,0x42,0x00,0xfb,0x24,0x17,0x00,0x18,0x24,0x13,0x01,0x03,0x24,0x15,0x08,0x18,
-0xaf,0xa6,0x00,0x1c,0x08,0x00,0x07,0xc2,0xa0,0x82,0x00,0x16,0x8d,0x02,0x00,0x04,
-0x00,0x0a,0x1c,0x42,0x30,0x42,0x00,0x10,0x14,0x40,0x00,0x15,0x30,0x72,0x00,0x3f,
-0x81,0x22,0x00,0x05,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x11,0x30,0x72,0x00,0x3e,
-0x27,0x83,0x90,0x18,0x00,0xc3,0x18,0x21,0x80,0x64,0x00,0x00,0x27,0x83,0xb5,0x78,
-0x00,0x04,0x11,0x00,0x00,0x44,0x10,0x23,0x00,0x02,0x10,0x80,0x00,0x44,0x10,0x23,
-0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,0x90,0x44,0x00,0x05,0x90,0x43,0x00,0x04,
-0x00,0x00,0x00,0x00,0x00,0x64,0x18,0x24,0x30,0x63,0x00,0x01,0x02,0x43,0x90,0x25,
-0x27,0x85,0xb4,0x00,0x01,0x85,0x28,0x21,0x8c,0xa6,0x00,0x00,0x01,0x73,0x10,0x21,
-0x27,0x83,0x90,0x10,0x90,0xc4,0x00,0x16,0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,
-0x30,0x84,0x00,0xdf,0x90,0x50,0x00,0x00,0xa0,0xc4,0x00,0x16,0x80,0xc6,0x00,0x12,
-0x8c,0xa3,0x00,0x00,0x2d,0xc4,0x00,0x02,0xaf,0xa6,0x00,0x1c,0x90,0x62,0x00,0x16,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xfb,0x14,0x80,0x00,0x06,0xa0,0x62,0x00,0x16,
-0x24,0x02,0x00,0x06,0x11,0xc2,0x00,0x03,0x24,0x02,0x00,0x04,0x15,0xc2,0xff,0x0e,
-0x32,0x51,0x00,0x02,0x32,0x51,0x00,0x02,0x2e,0x02,0x00,0x0c,0x14,0x40,0x00,0x0f,
-0x00,0x11,0x18,0x2b,0x32,0x02,0x00,0x0f,0x34,0x42,0x00,0x10,0x00,0x03,0x19,0x00,
-0x00,0x43,0x18,0x21,0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0xe0,0xa0,0x43,0x00,0x00,
-0x00,0x00,0x20,0x21,0x02,0x00,0x28,0x21,0x0c,0x00,0x02,0x03,0xaf,0xaf,0x00,0x28,
-0x8f,0xaf,0x00,0x28,0x08,0x00,0x07,0xc2,0x00,0x00,0x00,0x00,0x08,0x00,0x08,0xbd,
-0x32,0x03,0x00,0xff,0x3c,0x03,0xb0,0x05,0x34,0x63,0x02,0x42,0x90,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x0f,0x14,0x40,0xfe,0xaa,0x00,0x00,0x00,0x00,
-0x91,0xe2,0x00,0x09,0x00,0x00,0x00,0x00,0x02,0x82,0x10,0x26,0x08,0x00,0x07,0x79,
-0x00,0x02,0x28,0x0b,0x08,0x00,0x07,0x7f,0x00,0x00,0xb0,0x21,0x24,0x02,0x00,0x10,
-0x10,0xc2,0x00,0x08,0x24,0x02,0x00,0x11,0x10,0xc2,0xfe,0x7d,0x00,0x07,0x17,0x83,
-0x24,0x02,0x00,0x12,0x14,0xc2,0xfe,0x7b,0x00,0x07,0x17,0x43,0x08,0x00,0x07,0x59,
-0x30,0x5e,0x00,0x01,0x08,0x00,0x07,0x59,0x00,0x07,0xf7,0xc2,0x00,0x04,0x10,0x40,
-0x27,0x83,0x80,0x1c,0x00,0x43,0x10,0x21,0x00,0x80,0x40,0x21,0x94,0x44,0x00,0x00,
-0x2d,0x07,0x00,0x04,0x24,0xc2,0x00,0x03,0x00,0x47,0x30,0x0a,0x00,0x86,0x00,0x18,
-0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x34,0x63,0x00,0x20,0x24,0x42,0x23,0x8c,
-0xac,0x62,0x00,0x00,0x2d,0x06,0x00,0x10,0x00,0x00,0x20,0x12,0x00,0x04,0x22,0x42,
-0x24,0x84,0x00,0x01,0x24,0x83,0x00,0xc0,0x10,0xe0,0x00,0x0b,0x24,0x82,0x00,0x60,
-0x00,0x40,0x20,0x21,0x00,0x65,0x20,0x0a,0x3c,0x03,0xb0,0x03,0x34,0x63,0x01,0x00,
-0x90,0x62,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x01,0x00,0x44,0x20,0x04,
-0x03,0xe0,0x00,0x08,0x00,0x80,0x10,0x21,0x24,0x85,0x00,0x28,0x24,0x83,0x00,0x24,
-0x31,0x02,0x00,0x08,0x14,0xc0,0xff,0xf4,0x24,0x84,0x00,0x14,0x00,0x60,0x20,0x21,
-0x08,0x00,0x08,0xfa,0x00,0xa2,0x20,0x0b,0x27,0xbd,0xff,0xe0,0x3c,0x03,0xb0,0x03,
-0x3c,0x02,0x80,0x00,0xaf,0xb0,0x00,0x10,0x24,0x42,0x24,0x28,0x00,0x80,0x80,0x21,
-0x34,0x63,0x00,0x20,0x3c,0x04,0xb0,0x03,0xaf,0xb2,0x00,0x18,0xaf,0xb1,0x00,0x14,
-0xaf,0xbf,0x00,0x1c,0x83,0xb1,0x00,0x33,0x83,0xa8,0x00,0x37,0x34,0x84,0x01,0x10,
-0xac,0x62,0x00,0x00,0x2e,0x02,0x00,0x10,0x00,0xe0,0x90,0x21,0x8c,0x87,0x00,0x00,
-0x14,0x40,0x00,0x0c,0x2e,0x02,0x00,0x0c,0x3c,0x02,0x00,0x0f,0x34,0x42,0xf0,0x00,
-0x00,0xe2,0x10,0x24,0x14,0x40,0x00,0x37,0x32,0x02,0x00,0x08,0x32,0x02,0x00,0x07,
-0x27,0x83,0x80,0xcc,0x00,0x43,0x10,0x21,0x90,0x50,0x00,0x00,0x00,0x00,0x00,0x00,
-0x2e,0x02,0x00,0x0c,0x14,0x40,0x00,0x03,0x02,0x00,0x20,0x21,0x32,0x02,0x00,0x0f,
-0x24,0x44,0x00,0x0c,0x00,0x87,0x10,0x06,0x30,0x42,0x00,0x01,0x14,0x40,0x00,0x07,
-0x2c,0x82,0x00,0x0c,0x00,0x04,0x10,0x80,0x27,0x83,0xb4,0x50,0x00,0x43,0x10,0x21,
-0x8c,0x44,0x00,0x00,0x00,0x00,0x00,0x00,0x2c,0x82,0x00,0x0c,0x14,0x40,0x00,0x05,
-0x00,0x05,0x10,0x40,0x00,0x46,0x10,0x21,0x00,0x02,0x11,0x00,0x00,0x82,0x10,0x21,
-0x24,0x44,0x00,0x04,0x15,0x00,0x00,0x02,0x24,0x06,0x00,0x20,0x24,0x06,0x00,0x0e,
-0x0c,0x00,0x08,0xe3,0x00,0x00,0x00,0x00,0x00,0x40,0x30,0x21,0x3c,0x02,0xb0,0x03,
-0x34,0x42,0x01,0x00,0x90,0x43,0x00,0x00,0x2e,0x04,0x00,0x04,0x24,0x02,0x00,0x10,
-0x24,0x05,0x00,0x0a,0x00,0x44,0x28,0x0a,0x30,0x63,0x00,0x01,0x14,0x60,0x00,0x02,
-0x00,0x05,0x10,0x40,0x00,0xa0,0x10,0x21,0x30,0x45,0x00,0xff,0x00,0xc5,0x10,0x21,
-0x24,0x46,0x00,0x46,0x02,0x26,0x18,0x04,0xa6,0x43,0x00,0x00,0x8f,0xbf,0x00,0x1c,
-0x8f,0xb2,0x00,0x18,0x7b,0xb0,0x00,0xbc,0x00,0xc0,0x10,0x21,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x20,0x10,0x40,0xff,0xcf,0x2e,0x02,0x00,0x0c,0x32,0x02,0x00,0x07,
-0x27,0x83,0x80,0xc4,0x00,0x43,0x10,0x21,0x90,0x44,0x00,0x00,0x08,0x00,0x09,0x28,
-0x02,0x04,0x80,0x23,0x27,0xbd,0xff,0xb8,0x00,0x05,0x38,0x80,0x27,0x82,0xb4,0x00,
-0xaf,0xbe,0x00,0x40,0xaf,0xb6,0x00,0x38,0xaf,0xb3,0x00,0x2c,0xaf,0xbf,0x00,0x44,
-0xaf,0xb7,0x00,0x3c,0xaf,0xb5,0x00,0x34,0xaf,0xb4,0x00,0x30,0xaf,0xb2,0x00,0x28,
-0xaf,0xb1,0x00,0x24,0xaf,0xb0,0x00,0x20,0x00,0xe2,0x38,0x21,0x8c,0xe6,0x00,0x00,
-0xaf,0xa5,0x00,0x4c,0x3c,0x02,0x80,0x00,0x3c,0x05,0xb0,0x03,0x34,0xa5,0x00,0x20,
-0x24,0x42,0x25,0x84,0x24,0x03,0x00,0x01,0xac,0xa2,0x00,0x00,0xa0,0xc3,0x00,0x12,
-0x8c,0xe5,0x00,0x00,0x94,0xc3,0x00,0x06,0x90,0xa2,0x00,0x16,0xa4,0xc3,0x00,0x14,
-0x27,0x83,0x90,0x00,0x34,0x42,0x00,0x08,0xa0,0xa2,0x00,0x16,0x8c,0xe8,0x00,0x00,
-0xaf,0xa4,0x00,0x48,0x27,0x82,0x90,0x04,0x95,0x11,0x00,0x14,0x00,0x00,0x00,0x00,
-0x00,0x11,0x98,0xc0,0x02,0x71,0x20,0x21,0x00,0x04,0x20,0x80,0x00,0x82,0x10,0x21,
-0x8c,0x52,0x00,0x18,0x00,0x83,0x18,0x21,0x84,0x75,0x00,0x06,0x8e,0x45,0x00,0x08,
-0x8e,0x46,0x00,0x04,0x8e,0x47,0x00,0x04,0x00,0x05,0x1c,0x82,0x00,0x06,0x31,0x42,
-0x27,0x82,0x90,0x10,0x30,0x63,0x00,0x01,0x30,0xc6,0x00,0x01,0x00,0x82,0x20,0x21,
-0xa5,0x15,0x00,0x1a,0x00,0x05,0x14,0x42,0xaf,0xa3,0x00,0x18,0xaf,0xa6,0x00,0x1c,
-0x30,0xe7,0x00,0x10,0x30,0x56,0x00,0x01,0x80,0x97,0x00,0x06,0x14,0xe0,0x00,0x47,
-0x00,0x05,0xf7,0xc2,0x80,0x82,0x00,0x05,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x44,
-0x02,0x71,0x10,0x21,0x93,0x90,0xbb,0xe9,0x00,0x00,0x00,0x00,0x2e,0x02,0x00,0x0c,
-0x14,0x40,0x00,0x06,0x02,0x00,0x20,0x21,0x00,0x16,0x10,0x40,0x00,0x43,0x10,0x21,
-0x00,0x02,0x11,0x00,0x02,0x02,0x10,0x21,0x24,0x44,0x00,0x04,0x02,0x71,0x10,0x21,
-0x00,0x02,0x10,0x80,0x27,0x83,0x90,0x10,0x00,0x43,0x10,0x21,0x00,0x80,0x80,0x21,
-0xa0,0x44,0x00,0x03,0xa0,0x44,0x00,0x00,0x02,0x00,0x20,0x21,0x02,0xc0,0x28,0x21,
-0x0c,0x00,0x08,0xe3,0x02,0xa0,0x30,0x21,0x02,0x71,0x18,0x21,0x00,0x03,0x88,0x80,
-0x00,0x40,0xa0,0x21,0x27,0x82,0x90,0x20,0x02,0x22,0x10,0x21,0x8c,0x44,0x00,0x00,
-0x26,0xe3,0x00,0x02,0x00,0x03,0x17,0xc2,0x00,0x62,0x18,0x21,0x00,0x04,0x25,0xc2,
-0x00,0x03,0x18,0x43,0x30,0x84,0x00,0x01,0x00,0x03,0x18,0x40,0x03,0xc4,0x20,0x24,
-0x14,0x80,0x00,0x15,0x02,0x43,0x38,0x21,0x3c,0x08,0xb0,0x03,0x35,0x08,0x00,0x28,
-0x8d,0x03,0x00,0x00,0x8f,0xa6,0x00,0x4c,0x8f,0xa4,0x00,0x48,0x27,0x82,0x90,0x08,
-0x02,0x22,0x10,0x21,0x24,0x63,0x00,0x01,0x02,0xa0,0x28,0x21,0xa4,0x54,0x00,0x04,
-0x00,0xc0,0x38,0x21,0x0c,0x00,0x07,0x2f,0xad,0x03,0x00,0x00,0x7b,0xbe,0x02,0x3c,
-0x7b,0xb6,0x01,0xfc,0x7b,0xb4,0x01,0xbc,0x7b,0xb2,0x01,0x7c,0x7b,0xb0,0x01,0x3c,
-0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x48,0x8f,0xa2,0x00,0x1c,0x8f,0xa6,0x00,0x18,
-0x02,0x00,0x20,0x21,0x02,0xc0,0x28,0x21,0xaf,0xa2,0x00,0x10,0x0c,0x00,0x09,0x0a,
-0xaf,0xa0,0x00,0x14,0x08,0x00,0x09,0xc6,0x02,0x82,0xa0,0x21,0x02,0x71,0x10,0x21,
-0x00,0x02,0x10,0x80,0x27,0x83,0x90,0x10,0x00,0x43,0x10,0x21,0x90,0x50,0x00,0x00,
-0x08,0x00,0x09,0xb2,0xa0,0x50,0x00,0x03,0x27,0xbd,0xff,0xb8,0xaf,0xb1,0x00,0x24,
-0x8f,0xb1,0x00,0x5c,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x34,0x63,0x00,0x20,
-0x24,0x42,0x27,0xa8,0xaf,0xbe,0x00,0x40,0xaf,0xb7,0x00,0x3c,0xaf,0xb6,0x00,0x38,
-0xaf,0xb5,0x00,0x34,0xaf,0xb4,0x00,0x30,0xaf,0xa5,0x00,0x4c,0x8f,0xb5,0x00,0x58,
-0xaf,0xbf,0x00,0x44,0xaf,0xb3,0x00,0x2c,0xaf,0xb2,0x00,0x28,0xaf,0xb0,0x00,0x20,
-0x00,0xe0,0xb0,0x21,0xac,0x62,0x00,0x00,0x00,0x80,0xf0,0x21,0x00,0x00,0xb8,0x21,
-0x16,0x20,0x00,0x2b,0x00,0x00,0xa0,0x21,0x27,0x85,0xb4,0x00,0x00,0x07,0x10,0x80,
-0x00,0x45,0x10,0x21,0x8c,0x53,0x00,0x00,0x00,0x15,0x18,0x80,0x00,0x65,0x18,0x21,
-0x92,0x62,0x00,0x16,0x8c,0x72,0x00,0x00,0x30,0x42,0x00,0x03,0x14,0x40,0x00,0x2d,
-0x00,0x00,0x00,0x00,0x92,0x42,0x00,0x16,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x03,
-0x14,0x40,0x00,0x28,0x00,0x00,0x00,0x00,0x8c,0x82,0x00,0x34,0x00,0x00,0x00,0x00,
-0x14,0x40,0x00,0x18,0x02,0x20,0x10,0x21,0x8c,0x82,0x00,0x38,0x00,0x00,0x00,0x00,
-0x14,0x40,0x00,0x14,0x02,0x20,0x10,0x21,0x8c,0x82,0x00,0x3c,0x00,0x00,0x00,0x00,
-0x14,0x40,0x00,0x0f,0x3c,0x03,0xb0,0x09,0x3c,0x05,0xb0,0x05,0x34,0x63,0x01,0x44,
-0x34,0xa5,0x02,0x52,0x94,0x66,0x00,0x00,0x90,0xa2,0x00,0x00,0x8f,0xa3,0x00,0x4c,
-0x00,0x00,0x00,0x00,0x00,0x62,0x10,0x06,0x30,0x42,0x00,0x01,0x10,0x40,0x00,0x04,
-0x30,0xc6,0xff,0xff,0x2c,0xc2,0x00,0x41,0x10,0x40,0x00,0x09,0x24,0x05,0x00,0x14,
-0x02,0x20,0x10,0x21,0x7b,0xbe,0x02,0x3c,0x7b,0xb6,0x01,0xfc,0x7b,0xb4,0x01,0xbc,
-0x7b,0xb2,0x01,0x7c,0x7b,0xb0,0x01,0x3c,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x48,
-0x0c,0x00,0x07,0x0a,0x24,0x06,0x01,0x07,0x24,0x02,0x00,0x01,0x08,0x00,0x0a,0x2c,
-0xa3,0xc2,0x00,0x11,0x10,0xc0,0x00,0x1c,0x24,0x02,0x00,0x01,0x10,0xc2,0x00,0x17,
-0x00,0xc0,0x88,0x21,0x96,0x54,0x00,0x1a,0x02,0xa0,0xb8,0x21,0x12,0x20,0xff,0xed,
-0x02,0x20,0x10,0x21,0x27,0x83,0xb4,0x00,0x00,0x17,0x10,0x80,0x00,0x43,0x10,0x21,
-0x8c,0x44,0x00,0x00,0x3c,0x03,0xb0,0x03,0x34,0x63,0x00,0x28,0x80,0x86,0x00,0x12,
-0x8c,0x62,0x00,0x00,0x00,0x14,0x2c,0x00,0x00,0x05,0x2c,0x03,0x00,0x46,0x10,0x21,
-0x8f,0xa6,0x00,0x4c,0x02,0xe0,0x38,0x21,0x03,0xc0,0x20,0x21,0x0c,0x00,0x07,0x2f,
-0xac,0x62,0x00,0x00,0x08,0x00,0x0a,0x2c,0xaf,0xd1,0x00,0x40,0x96,0x74,0x00,0x1a,
-0x08,0x00,0x0a,0x3f,0x02,0xc0,0xb8,0x21,0x3c,0x02,0xb0,0x03,0x34,0x42,0x01,0x08,
-0x8c,0x50,0x00,0x00,0x02,0x60,0x20,0x21,0x0c,0x00,0x1e,0xf3,0x02,0x00,0x28,0x21,
-0x30,0x42,0x00,0xff,0x02,0x00,0x28,0x21,0x02,0x40,0x20,0x21,0x0c,0x00,0x1e,0xf3,
-0xaf,0xa2,0x00,0x18,0x8f,0xa4,0x00,0x18,0x00,0x00,0x00,0x00,0x10,0x80,0x00,0xed,
-0x30,0x50,0x00,0xff,0x12,0x00,0x00,0x18,0x24,0x11,0x00,0x01,0x96,0x63,0x00,0x14,
-0x96,0x44,0x00,0x14,0x27,0x85,0x90,0x00,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,
-0x00,0x02,0x10,0x80,0x00,0x45,0x10,0x21,0x00,0x04,0x18,0xc0,0x8c,0x46,0x00,0x08,
-0x00,0x64,0x18,0x21,0x00,0x03,0x18,0x80,0x00,0x65,0x18,0x21,0x00,0x06,0x17,0x02,
-0x24,0x04,0x00,0xff,0x8c,0x63,0x00,0x08,0x10,0x44,0x00,0xd6,0x00,0x03,0x17,0x02,
-0x10,0x44,0x00,0xd5,0x3c,0x02,0x80,0x00,0x00,0x66,0x18,0x2b,0x24,0x11,0x00,0x02,
-0x24,0x02,0x00,0x01,0x00,0x43,0x88,0x0a,0x24,0x02,0x00,0x01,0x12,0x22,0x00,0x5a,
-0x24,0x02,0x00,0x02,0x16,0x22,0xff,0xbd,0x00,0x00,0x00,0x00,0x96,0x49,0x00,0x14,
-0x27,0x82,0x90,0x04,0x02,0xa0,0xb8,0x21,0x00,0x09,0x50,0xc0,0x01,0x49,0x18,0x21,
-0x00,0x03,0x40,0x80,0x01,0x02,0x10,0x21,0x8c,0x43,0x00,0x18,0x00,0x00,0x00,0x00,
-0x8c,0x65,0x00,0x08,0x8c,0x62,0x00,0x0c,0x8c,0x62,0x00,0x04,0x00,0x05,0x24,0x42,
-0x00,0x05,0x1c,0x82,0x30,0x42,0x00,0x10,0x30,0x66,0x00,0x01,0x14,0x40,0x00,0x41,
-0x30,0x87,0x00,0x01,0x27,0x82,0x90,0x18,0x01,0x02,0x10,0x21,0x80,0x44,0x00,0x00,
-0x27,0x82,0xb5,0x78,0x00,0x04,0x19,0x00,0x00,0x64,0x18,0x23,0x00,0x03,0x18,0x80,
-0x00,0x64,0x18,0x23,0x00,0x03,0x18,0x80,0x00,0x62,0x10,0x21,0x90,0x45,0x00,0x05,
-0x27,0x84,0xb4,0xa0,0x00,0x64,0x18,0x21,0x90,0x63,0x00,0x00,0x10,0xa0,0x00,0x2b,
-0x2c,0x64,0x00,0x0c,0x14,0x80,0x00,0x04,0x00,0x60,0x10,0x21,0x00,0x06,0x11,0x00,
-0x00,0x62,0x10,0x21,0x24,0x42,0x00,0x24,0x3c,0x01,0xb0,0x03,0xa0,0x22,0x00,0xe1,
-0x14,0x80,0x00,0x06,0x00,0x60,0x28,0x21,0x00,0x07,0x10,0x40,0x00,0x46,0x10,0x21,
-0x00,0x02,0x11,0x00,0x00,0x62,0x10,0x21,0x24,0x45,0x00,0x04,0x01,0x49,0x10,0x21,
-0x27,0x83,0x90,0x10,0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,0x00,0xa0,0x18,0x21,
-0xa0,0x45,0x00,0x03,0xa0,0x45,0x00,0x00,0x24,0x02,0x00,0x08,0x12,0x02,0x00,0x0b,
-0x24,0x02,0x00,0x01,0x00,0x60,0x28,0x21,0x02,0x40,0x20,0x21,0x0c,0x00,0x1f,0x6f,
-0xaf,0xa2,0x00,0x10,0x30,0x54,0xff,0xff,0x92,0x42,0x00,0x16,0x00,0x00,0x00,0x00,
-0x02,0x02,0x10,0x25,0x08,0x00,0x0a,0x3f,0xa2,0x42,0x00,0x16,0x00,0x60,0x28,0x21,
-0x02,0x40,0x20,0x21,0x0c,0x00,0x1f,0x20,0xaf,0xa0,0x00,0x10,0x08,0x00,0x0a,0xc2,
-0x30,0x54,0xff,0xff,0x08,0x00,0x0a,0xaa,0x00,0x60,0x10,0x21,0x14,0x80,0xff,0xfd,
-0x00,0x00,0x00,0x00,0x00,0x06,0x11,0x00,0x00,0x62,0x10,0x21,0x08,0x00,0x0a,0xaa,
-0x24,0x42,0x00,0x04,0x27,0x82,0x90,0x10,0x01,0x02,0x10,0x21,0x90,0x43,0x00,0x00,
-0x08,0x00,0x0a,0xba,0xa0,0x43,0x00,0x03,0x96,0x69,0x00,0x14,0x02,0xc0,0xb8,0x21,
-0x24,0x0b,0x00,0x01,0x00,0x09,0x10,0xc0,0x00,0x49,0x18,0x21,0x00,0x03,0x40,0x80,
-0x00,0x40,0x50,0x21,0x27,0x82,0x90,0x04,0x01,0x02,0x10,0x21,0x8c,0x43,0x00,0x18,
-0x00,0x00,0x00,0x00,0x8c,0x65,0x00,0x08,0x8c,0x62,0x00,0x0c,0x8c,0x62,0x00,0x04,
-0x00,0x05,0x24,0x42,0x00,0x05,0x1c,0x82,0x30,0x42,0x00,0x10,0x30,0x66,0x00,0x01,
-0x10,0x40,0x00,0x0d,0x30,0x87,0x00,0x01,0x27,0x82,0x90,0x18,0x01,0x02,0x10,0x21,
-0x80,0x43,0x00,0x00,0x00,0x00,0x58,0x21,0x00,0x03,0x11,0x00,0x00,0x43,0x10,0x23,
-0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x23,0x00,0x02,0x10,0x80,0x27,0x83,0xb5,0x70,
-0x00,0x43,0x10,0x21,0xa0,0x40,0x00,0x04,0x11,0x60,0x00,0x4f,0x00,0x00,0x00,0x00,
-0x01,0x49,0x10,0x21,0x00,0x02,0x20,0x80,0x27,0x85,0x90,0x10,0x00,0x85,0x10,0x21,
-0x80,0x43,0x00,0x05,0x00,0x00,0x00,0x00,0x14,0x60,0x00,0x42,0x01,0x49,0x10,0x21,
-0x27,0x82,0x90,0x18,0x00,0x82,0x10,0x21,0x80,0x44,0x00,0x00,0x27,0x82,0xb5,0x78,
-0x00,0x04,0x19,0x00,0x00,0x64,0x18,0x23,0x00,0x03,0x18,0x80,0x00,0x64,0x18,0x23,
-0x00,0x03,0x18,0x80,0x00,0x62,0x10,0x21,0x90,0x45,0x00,0x05,0x27,0x84,0xb4,0xa0,
-0x00,0x64,0x18,0x21,0x90,0x63,0x00,0x00,0x10,0xa0,0x00,0x2c,0x2c,0x64,0x00,0x0c,
-0x14,0x80,0x00,0x04,0x00,0x60,0x10,0x21,0x00,0x06,0x11,0x00,0x00,0x62,0x10,0x21,
-0x24,0x42,0x00,0x24,0x3c,0x01,0xb0,0x03,0xa0,0x22,0x00,0xe1,0x14,0x80,0x00,0x06,
-0x00,0x60,0x28,0x21,0x00,0x07,0x10,0x40,0x00,0x46,0x10,0x21,0x00,0x02,0x11,0x00,
-0x00,0x62,0x10,0x21,0x24,0x45,0x00,0x04,0x01,0x49,0x10,0x21,0x27,0x83,0x90,0x10,
-0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,0x00,0xa0,0x18,0x21,0xa0,0x45,0x00,0x03,
-0xa0,0x45,0x00,0x00,0x8f,0xa4,0x00,0x18,0x24,0x02,0x00,0x08,0x10,0x82,0x00,0x0c,
-0x00,0x60,0x28,0x21,0x24,0x02,0x00,0x01,0x02,0x60,0x20,0x21,0x0c,0x00,0x1f,0x6f,
-0xaf,0xa2,0x00,0x10,0x8f,0xa3,0x00,0x18,0x30,0x54,0xff,0xff,0x92,0x62,0x00,0x16,
-0x00,0x00,0x00,0x00,0x00,0x62,0x10,0x25,0x08,0x00,0x0a,0x3f,0xa2,0x62,0x00,0x16,
-0x02,0x60,0x20,0x21,0x0c,0x00,0x1f,0x20,0xaf,0xa0,0x00,0x10,0x08,0x00,0x0b,0x31,
-0x00,0x00,0x00,0x00,0x08,0x00,0x0b,0x19,0x00,0x60,0x10,0x21,0x14,0x80,0xff,0xfd,
-0x00,0x00,0x00,0x00,0x00,0x06,0x11,0x00,0x00,0x62,0x10,0x21,0x08,0x00,0x0b,0x19,
-0x24,0x42,0x00,0x04,0x00,0x02,0x10,0x80,0x00,0x45,0x10,0x21,0x90,0x43,0x00,0x00,
-0x08,0x00,0x0b,0x29,0xa0,0x43,0x00,0x03,0x27,0x85,0x90,0x10,0x08,0x00,0x0b,0x45,
-0x01,0x49,0x10,0x21,0x3c,0x02,0x80,0x00,0x00,0x62,0x18,0x26,0x08,0x00,0x0a,0x7a,
-0x00,0xc2,0x30,0x26,0x12,0x00,0xff,0x2d,0x24,0x02,0x00,0x01,0x08,0x00,0x0a,0x7f,
-0x24,0x11,0x00,0x02,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x27,0xbd,0xff,0xd0,
-0x24,0x42,0x2d,0x54,0x34,0x63,0x00,0x20,0x3c,0x05,0xb0,0x05,0xaf,0xb3,0x00,0x24,
-0xaf,0xb2,0x00,0x20,0xaf,0xb1,0x00,0x1c,0xaf,0xbf,0x00,0x28,0xaf,0xb0,0x00,0x18,
-0xac,0x62,0x00,0x00,0x34,0xa5,0x02,0x42,0x90,0xa2,0x00,0x00,0x00,0x80,0x90,0x21,
-0x24,0x11,0x00,0x10,0x30,0x53,0x00,0xff,0x24,0x02,0x00,0x10,0x12,0x22,0x00,0xcf,
-0x00,0x00,0x18,0x21,0x24,0x02,0x00,0x11,0x12,0x22,0x00,0xc1,0x24,0x02,0x00,0x12,
-0x12,0x22,0x00,0xb4,0x00,0x00,0x00,0x00,0x14,0x60,0x00,0xad,0xae,0x43,0x00,0x40,
-0x3c,0x02,0xb0,0x05,0x34,0x42,0x02,0x2c,0x8c,0x44,0x00,0x00,0x3c,0x03,0x00,0x02,
-0x34,0x63,0x00,0xff,0x00,0x83,0x80,0x24,0x00,0x10,0x14,0x43,0x10,0x40,0x00,0x05,
-0x00,0x00,0x00,0x00,0x8e,0x42,0x00,0x34,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x92,
-0x00,0x00,0x00,0x00,0x93,0x83,0x8b,0x71,0x00,0x00,0x00,0x00,0x30,0x62,0x00,0x02,
-0x10,0x40,0x00,0x04,0x32,0x10,0x00,0xff,0x00,0x10,0x11,0xc3,0x14,0x40,0x00,0x86,
-0x00,0x00,0x00,0x00,0x16,0x00,0x00,0x15,0x02,0x00,0x10,0x21,0x26,0x22,0x00,0x01,
-0x30,0x51,0x00,0xff,0x2e,0x23,0x00,0x13,0x14,0x60,0xff,0xdb,0x24,0x03,0x00,0x02,
-0x12,0x63,0x00,0x73,0x24,0x02,0x00,0x05,0x2a,0x62,0x00,0x03,0x10,0x40,0x00,0x58,
-0x24,0x02,0x00,0x04,0x24,0x02,0x00,0x01,0x12,0x62,0x00,0x4b,0x02,0x40,0x20,0x21,
-0x3c,0x02,0xb0,0x05,0x34,0x42,0x02,0x2c,0x8c,0x43,0x00,0x00,0x00,0x00,0x00,0x00,
-0x30,0x70,0x00,0xff,0x12,0x00,0x00,0x06,0x02,0x00,0x10,0x21,0x8f,0xbf,0x00,0x28,
-0x7b,0xb2,0x01,0x3c,0x7b,0xb0,0x00,0xfc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x30,
-0x92,0x46,0x00,0x04,0x8e,0x43,0x00,0x24,0x24,0x02,0x00,0x07,0x02,0x40,0x20,0x21,
-0x00,0x00,0x28,0x21,0x24,0x07,0x00,0x06,0xaf,0xa2,0x00,0x10,0x0c,0x00,0x09,0xea,
-0xaf,0xa3,0x00,0x14,0xae,0x42,0x00,0x24,0x3c,0x02,0xb0,0x05,0x8c,0x42,0x02,0x2c,
-0x00,0x00,0x00,0x00,0x30,0x50,0x00,0xff,0x16,0x00,0xff,0xec,0x02,0x00,0x10,0x21,
-0x92,0x46,0x00,0x05,0x8e,0x43,0x00,0x28,0x24,0x02,0x00,0x05,0x02,0x40,0x20,0x21,
-0x24,0x05,0x00,0x01,0x24,0x07,0x00,0x04,0xaf,0xa2,0x00,0x10,0x0c,0x00,0x09,0xea,
-0xaf,0xa3,0x00,0x14,0xae,0x42,0x00,0x28,0x3c,0x02,0xb0,0x05,0x8c,0x42,0x02,0x2c,
-0x00,0x00,0x00,0x00,0x30,0x50,0x00,0xff,0x16,0x00,0xff,0xdc,0x02,0x00,0x10,0x21,
-0x92,0x46,0x00,0x06,0x8e,0x43,0x00,0x2c,0x24,0x02,0x00,0x03,0x02,0x40,0x20,0x21,
-0x24,0x05,0x00,0x02,0x00,0x00,0x38,0x21,0xaf,0xa2,0x00,0x10,0x0c,0x00,0x09,0xea,
-0xaf,0xa3,0x00,0x14,0xae,0x42,0x00,0x2c,0x3c,0x02,0xb0,0x05,0x8c,0x42,0x02,0x2c,
-0x00,0x00,0x00,0x00,0x30,0x50,0x00,0xff,0x16,0x00,0xff,0xcc,0x02,0x00,0x10,0x21,
-0x92,0x46,0x00,0x07,0x8e,0x43,0x00,0x30,0x24,0x02,0x00,0x02,0x02,0x40,0x20,0x21,
-0x24,0x05,0x00,0x03,0x24,0x07,0x00,0x01,0xaf,0xa2,0x00,0x10,0x0c,0x00,0x09,0xea,
-0xaf,0xa3,0x00,0x14,0xae,0x42,0x00,0x30,0x3c,0x02,0xb0,0x05,0x8c,0x42,0x02,0x2c,
-0x08,0x00,0x0b,0x9b,0x30,0x42,0x00,0xff,0x92,0x46,0x00,0x04,0x8e,0x43,0x00,0x24,
-0x24,0x02,0x00,0x07,0x00,0x00,0x28,0x21,0x24,0x07,0x00,0x06,0xaf,0xa2,0x00,0x10,
-0x0c,0x00,0x09,0xea,0xaf,0xa3,0x00,0x14,0x08,0x00,0x0b,0x94,0xae,0x42,0x00,0x24,
-0x12,0x62,0x00,0x0d,0x24,0x02,0x00,0x03,0x24,0x02,0x00,0x08,0x16,0x62,0xff,0xa8,
-0x02,0x40,0x20,0x21,0x92,0x46,0x00,0x07,0x8e,0x42,0x00,0x30,0x24,0x05,0x00,0x03,
-0x24,0x07,0x00,0x01,0xaf,0xa3,0x00,0x10,0x0c,0x00,0x09,0xea,0xaf,0xa2,0x00,0x14,
-0x08,0x00,0x0b,0x94,0xae,0x42,0x00,0x30,0x92,0x46,0x00,0x06,0x8e,0x43,0x00,0x2c,
-0x02,0x40,0x20,0x21,0x24,0x05,0x00,0x02,0x00,0x00,0x38,0x21,0xaf,0xa2,0x00,0x10,
-0x0c,0x00,0x09,0xea,0xaf,0xa3,0x00,0x14,0x08,0x00,0x0b,0x94,0xae,0x42,0x00,0x2c,
-0x92,0x46,0x00,0x05,0x8e,0x43,0x00,0x28,0x02,0x40,0x20,0x21,0x24,0x05,0x00,0x01,
-0x24,0x07,0x00,0x04,0xaf,0xa2,0x00,0x10,0x0c,0x00,0x09,0xea,0xaf,0xa3,0x00,0x14,
-0x08,0x00,0x0b,0x94,0xae,0x42,0x00,0x28,0x0c,0x00,0x01,0x57,0x24,0x04,0x00,0x01,
-0x08,0x00,0x0b,0x85,0x00,0x00,0x00,0x00,0x8f,0x84,0xb4,0x40,0xae,0x40,0x00,0x34,
-0x94,0x85,0x00,0x14,0x0c,0x00,0x1b,0x66,0x00,0x00,0x00,0x00,0x93,0x83,0x8b,0x71,
-0x00,0x00,0x00,0x00,0x30,0x62,0x00,0x02,0x10,0x40,0xff,0x69,0x00,0x00,0x00,0x00,
-0x0c,0x00,0x01,0x57,0x00,0x00,0x20,0x21,0x08,0x00,0x0b,0x7d,0x00,0x00,0x00,0x00,
-0x02,0x40,0x20,0x21,0x0c,0x00,0x09,0x61,0x02,0x20,0x28,0x21,0x08,0x00,0x0b,0x71,
-0x3c,0x02,0xb0,0x05,0x8e,0x42,0x00,0x3c,0x00,0x00,0x00,0x00,0x14,0x40,0xff,0x4a,
-0x00,0x00,0x00,0x00,0x8f,0x82,0xb4,0x48,0x00,0x00,0x00,0x00,0x90,0x42,0x00,0x0a,
-0x00,0x00,0x00,0x00,0x00,0x02,0x18,0x2b,0x08,0x00,0x0b,0x6e,0xae,0x43,0x00,0x3c,
-0x8e,0x42,0x00,0x38,0x00,0x00,0x00,0x00,0x14,0x40,0xff,0x3d,0x24,0x02,0x00,0x12,
-0x8f,0x82,0xb4,0x44,0x00,0x00,0x00,0x00,0x90,0x42,0x00,0x0a,0x00,0x00,0x00,0x00,
-0x00,0x02,0x18,0x2b,0x08,0x00,0x0b,0x6e,0xae,0x43,0x00,0x38,0x8e,0x42,0x00,0x34,
-0x00,0x00,0x00,0x00,0x14,0x40,0xff,0x30,0x24,0x02,0x00,0x11,0x8f,0x82,0xb4,0x40,
-0x00,0x00,0x00,0x00,0x90,0x42,0x00,0x0a,0x00,0x00,0x00,0x00,0x00,0x02,0x18,0x2b,
-0x08,0x00,0x0b,0x6e,0xae,0x43,0x00,0x34,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,
-0x27,0xbd,0xff,0xe0,0x34,0x63,0x00,0x20,0x24,0x42,0x31,0x08,0x3c,0x08,0xb0,0x03,
-0xaf,0xb1,0x00,0x14,0xac,0x62,0x00,0x00,0x35,0x08,0x01,0x00,0xaf,0xbf,0x00,0x18,
-0xaf,0xb0,0x00,0x10,0x91,0x03,0x00,0x00,0x00,0xa0,0x48,0x21,0x24,0x11,0x00,0x0a,
-0x2c,0xa5,0x00,0x04,0x24,0x02,0x00,0x10,0x00,0x45,0x88,0x0a,0x30,0x63,0x00,0x01,
-0x00,0xc0,0x28,0x21,0x14,0x60,0x00,0x02,0x00,0x11,0x40,0x40,0x02,0x20,0x40,0x21,
-0x84,0x83,0x00,0x0c,0x31,0x11,0x00,0xff,0x01,0x20,0x20,0x21,0x00,0x03,0x10,0xc0,
-0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,0x27,0x83,0x90,0x08,0x00,0x43,0x10,0x21,
-0x84,0x43,0x00,0x04,0x24,0x06,0x00,0x0e,0x10,0xe0,0x00,0x06,0x02,0x23,0x80,0x21,
-0x02,0x00,0x10,0x21,0x8f,0xbf,0x00,0x18,0x7b,0xb0,0x00,0xbc,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x20,0x0c,0x00,0x08,0xe3,0x00,0x00,0x00,0x00,0x02,0x11,0x18,0x21,
-0x08,0x00,0x0c,0x64,0x00,0x62,0x80,0x21,0x27,0xbd,0xff,0xd0,0xaf,0xbf,0x00,0x28,
-0xaf,0xb4,0x00,0x20,0xaf,0xb3,0x00,0x1c,0xaf,0xb2,0x00,0x18,0xaf,0xb5,0x00,0x24,
-0xaf,0xb1,0x00,0x14,0xaf,0xb0,0x00,0x10,0x84,0x82,0x00,0x0c,0x3c,0x06,0xb0,0x03,
-0x34,0xc6,0x00,0x20,0x00,0x02,0x18,0xc0,0x00,0x62,0x18,0x21,0x00,0x03,0x18,0x80,
-0x27,0x82,0x90,0x04,0x00,0x62,0x10,0x21,0x8c,0x55,0x00,0x18,0x3c,0x02,0x80,0x00,
-0x24,0x42,0x31,0xb8,0xac,0xc2,0x00,0x00,0x8e,0xb0,0x00,0x08,0x27,0x82,0x90,0x08,
-0x00,0x62,0x18,0x21,0x90,0x71,0x00,0x07,0x00,0x10,0x86,0x43,0x32,0x10,0x00,0x01,
-0x00,0xa0,0x38,0x21,0x02,0x00,0x30,0x21,0x00,0xa0,0x98,0x21,0x02,0x20,0x28,0x21,
-0x0c,0x00,0x0c,0x42,0x00,0x80,0x90,0x21,0x02,0x20,0x20,0x21,0x02,0x00,0x28,0x21,
-0x24,0x06,0x00,0x14,0x0c,0x00,0x08,0xe3,0x00,0x40,0xa0,0x21,0x86,0x43,0x00,0x0c,
-0x3c,0x09,0xb0,0x09,0x3c,0x08,0xb0,0x09,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,
-0x00,0x02,0x10,0x80,0x27,0x83,0x90,0x10,0x00,0x43,0x10,0x21,0x80,0x43,0x00,0x06,
-0x3c,0x07,0xb0,0x09,0x3c,0x05,0xb0,0x09,0x28,0x62,0x00,0x00,0x24,0x64,0x00,0x03,
-0x00,0x82,0x18,0x0b,0x00,0x03,0x18,0x83,0x3c,0x02,0xb0,0x09,0x00,0x03,0x18,0x80,
-0x34,0x42,0x01,0x02,0x35,0x29,0x01,0x10,0x35,0x08,0x01,0x14,0x34,0xe7,0x01,0x20,
-0x34,0xa5,0x01,0x24,0xa4,0x54,0x00,0x00,0x12,0x60,0x00,0x11,0x02,0xa3,0xa8,0x21,
-0x8e,0xa2,0x00,0x0c,0x8e,0xa3,0x00,0x08,0x00,0x02,0x14,0x00,0x00,0x03,0x1c,0x02,
-0x00,0x43,0x10,0x21,0xad,0x22,0x00,0x00,0x8e,0xa3,0x00,0x0c,0x00,0x00,0x00,0x00,
-0x00,0x03,0x1c,0x02,0xa5,0x03,0x00,0x00,0x8f,0xbf,0x00,0x28,0x7b,0xb4,0x01,0x3c,
-0x7b,0xb2,0x00,0xfc,0x7b,0xb0,0x00,0xbc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x30,
-0x8e,0xa2,0x00,0x04,0x00,0x00,0x00,0x00,0xad,0x22,0x00,0x00,0x8e,0xa4,0x00,0x08,
-0x00,0x00,0x00,0x00,0xa5,0x04,0x00,0x00,0x7a,0xa2,0x00,0x7c,0x00,0x00,0x00,0x00,
-0x00,0x03,0x1c,0x00,0x00,0x02,0x14,0x02,0x00,0x62,0x18,0x21,0xac,0xe3,0x00,0x00,
-0x8e,0xa2,0x00,0x0c,0x00,0x00,0x00,0x00,0x00,0x02,0x14,0x02,0x08,0x00,0x0c,0xb6,
-0xa4,0xa2,0x00,0x00,0x27,0xbd,0xff,0xe0,0xaf,0xb2,0x00,0x18,0xaf,0xb0,0x00,0x10,
-0xaf,0xbf,0x00,0x1c,0xaf,0xb1,0x00,0x14,0x84,0x82,0x00,0x0c,0x00,0x80,0x90,0x21,
-0x3c,0x05,0xb0,0x03,0x00,0x02,0x20,0xc0,0x00,0x82,0x20,0x21,0x00,0x04,0x20,0x80,
-0x27,0x82,0x90,0x04,0x00,0x82,0x10,0x21,0x8c,0x51,0x00,0x18,0x3c,0x02,0x80,0x00,
-0x34,0xa5,0x00,0x20,0x24,0x42,0x33,0x34,0x27,0x83,0x90,0x08,0xac,0xa2,0x00,0x00,
-0x00,0x83,0x20,0x21,0x3c,0x02,0xb0,0x03,0x90,0x86,0x00,0x07,0x34,0x42,0x01,0x00,
-0x8e,0x23,0x00,0x08,0x90,0x44,0x00,0x00,0x2c,0xc5,0x00,0x04,0x24,0x02,0x00,0x10,
-0x24,0x10,0x00,0x0a,0x00,0x45,0x80,0x0a,0x00,0x03,0x1e,0x43,0x30,0x84,0x00,0x01,
-0x30,0x65,0x00,0x01,0x14,0x80,0x00,0x02,0x00,0x10,0x10,0x40,0x02,0x00,0x10,0x21,
-0x00,0xc0,0x20,0x21,0x24,0x06,0x00,0x20,0x0c,0x00,0x08,0xe3,0x30,0x50,0x00,0xff,
-0x86,0x44,0x00,0x0c,0x27,0x85,0x90,0x10,0x3c,0x06,0xb0,0x09,0x00,0x04,0x18,0xc0,
-0x00,0x64,0x18,0x21,0x00,0x03,0x18,0x80,0x00,0x65,0x18,0x21,0x80,0x64,0x00,0x06,
-0x00,0x50,0x10,0x21,0x34,0xc6,0x01,0x02,0x24,0x85,0x00,0x03,0x28,0x83,0x00,0x00,
-0x00,0xa3,0x20,0x0b,0x00,0x04,0x20,0x83,0x00,0x04,0x20,0x80,0xa4,0xc2,0x00,0x00,
-0x02,0x24,0x20,0x21,0x8c,0x83,0x00,0x04,0x3c,0x02,0xb0,0x09,0x34,0x42,0x01,0x10,
-0xac,0x43,0x00,0x00,0x8c,0x86,0x00,0x08,0x3c,0x02,0xb0,0x09,0x34,0x42,0x01,0x14,
-0xa4,0x46,0x00,0x00,0x8c,0x85,0x00,0x0c,0x8c,0x82,0x00,0x08,0x3c,0x06,0xb0,0x09,
-0x00,0x05,0x2c,0x00,0x00,0x02,0x14,0x02,0x00,0xa2,0x28,0x21,0x34,0xc6,0x01,0x20,
-0xac,0xc5,0x00,0x00,0x8c,0x83,0x00,0x0c,0x3c,0x05,0xb0,0x09,0x34,0xa5,0x01,0x24,
-0x00,0x03,0x1c,0x02,0xa4,0xa3,0x00,0x00,0x92,0x42,0x00,0x0a,0x3c,0x03,0xb0,0x09,
-0x34,0x63,0x01,0x30,0x00,0x02,0x13,0x00,0x24,0x42,0x00,0x04,0x30,0x42,0xff,0xff,
-0xa4,0x62,0x00,0x00,0x86,0x44,0x00,0x0c,0x27,0x83,0x90,0x18,0x8f,0xbf,0x00,0x1c,
-0x00,0x04,0x10,0xc0,0x00,0x44,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,
-0x94,0x44,0x00,0x02,0x8f,0xb2,0x00,0x18,0x7b,0xb0,0x00,0xbc,0x3c,0x05,0xb0,0x09,
-0x34,0xa5,0x01,0x32,0xa4,0xa4,0x00,0x00,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x20,
-0x27,0xbd,0xff,0xe0,0x3c,0x02,0xb0,0x03,0x3c,0x03,0x80,0x00,0xaf,0xb0,0x00,0x10,
-0x34,0x42,0x00,0x20,0x00,0xa0,0x80,0x21,0x24,0x63,0x34,0xc0,0x00,0x05,0x2c,0x43,
-0xaf,0xb1,0x00,0x14,0xaf,0xbf,0x00,0x18,0xac,0x43,0x00,0x00,0x10,0xa0,0x00,0x05,
-0x00,0x80,0x88,0x21,0x8c,0x82,0x00,0x34,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0xb6,
-0x00,0x00,0x00,0x00,0x32,0x10,0x00,0xff,0x12,0x00,0x00,0x4c,0x00,0x00,0x10,0x21,
-0x24,0x02,0x00,0x08,0x12,0x02,0x00,0xa3,0x2a,0x02,0x00,0x09,0x10,0x40,0x00,0x89,
-0x24,0x02,0x00,0x40,0x24,0x04,0x00,0x02,0x12,0x04,0x00,0x79,0x2a,0x02,0x00,0x03,
-0x10,0x40,0x00,0x69,0x24,0x02,0x00,0x04,0x24,0x02,0x00,0x01,0x12,0x02,0x00,0x5a,
-0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x05,0x34,0x42,0x00,0x08,0x3c,0x03,0x80,0x00,
-0xa2,0x20,0x00,0x4e,0xac,0x43,0x00,0x00,0x82,0x24,0x00,0x11,0x92,0x27,0x00,0x11,
-0x10,0x80,0x00,0x4e,0x00,0x00,0x00,0x00,0x92,0x26,0x00,0x0a,0x24,0x02,0x00,0x12,
-0x10,0x46,0x00,0x09,0x30,0xc2,0x00,0xff,0x27,0x83,0xb4,0x00,0x00,0x02,0x10,0x80,
-0x00,0x43,0x10,0x21,0x8c,0x44,0x00,0x00,0x00,0x00,0x00,0x00,0x94,0x83,0x00,0x14,
-0x00,0x00,0x00,0x00,0xa6,0x23,0x00,0x0c,0x3c,0x02,0xb0,0x09,0x34,0x42,0x00,0x40,
-0x90,0x43,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x63,0x00,0x03,0xa2,0x23,0x00,0x10,
-0x14,0x60,0x00,0x2b,0x30,0x65,0x00,0x01,0x30,0xc2,0x00,0xff,0x27,0x83,0xb4,0x00,
-0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,0x8c,0x44,0x00,0x00,0x82,0x23,0x00,0x12,
-0x90,0x82,0x00,0x16,0x00,0x00,0x00,0x00,0x00,0x02,0x11,0x42,0x30,0x42,0x00,0x01,
-0x00,0x62,0x18,0x21,0x00,0x03,0x26,0x00,0x14,0x80,0x00,0x18,0xa2,0x23,0x00,0x12,
-0x00,0x07,0x16,0x00,0x14,0x40,0x00,0x11,0x24,0x02,0x00,0x01,0x96,0x23,0x00,0x0c,
-0x27,0x84,0x90,0x10,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,
-0x00,0x44,0x10,0x21,0x80,0x45,0x00,0x06,0x00,0x03,0x1a,0x00,0x3c,0x02,0xb0,0x00,
-0x00,0x65,0x18,0x21,0x00,0x62,0x18,0x21,0x90,0x64,0x00,0x00,0x90,0x62,0x00,0x04,
-0xa2,0x20,0x00,0x15,0xa3,0x80,0x8b,0xd4,0x24,0x02,0x00,0x01,0x8f,0xbf,0x00,0x18,
-0x7b,0xb0,0x00,0xbc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x20,0x0c,0x00,0x0c,0xcd,
-0x02,0x20,0x20,0x21,0x92,0x27,0x00,0x11,0x08,0x00,0x0d,0x7d,0x00,0x07,0x16,0x00,
-0x0c,0x00,0x0c,0x6e,0x02,0x20,0x20,0x21,0x86,0x23,0x00,0x0c,0x27,0x84,0x90,0x08,
-0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x44,0x20,0x21,
-0x90,0x85,0x00,0x07,0x27,0x83,0x90,0x10,0x00,0x43,0x10,0x21,0xa2,0x25,0x00,0x13,
-0x90,0x83,0x00,0x07,0x08,0x00,0x0d,0x95,0xa0,0x43,0x00,0x02,0x92,0x26,0x00,0x0a,
-0x08,0x00,0x0d,0x5e,0x30,0xc2,0x00,0xff,0x8e,0x22,0x00,0x24,0x00,0x00,0x00,0x00,
-0x10,0x50,0x00,0x07,0xa2,0x20,0x00,0x08,0x24,0x02,0x00,0x07,0xa2,0x22,0x00,0x0a,
-0x92,0x22,0x00,0x27,0xae,0x20,0x00,0x24,0x08,0x00,0x0d,0x51,0xa2,0x22,0x00,0x04,
-0x08,0x00,0x0d,0xaf,0x24,0x02,0x00,0x06,0x16,0x02,0xff,0x9b,0x3c,0x02,0xb0,0x05,
-0x8e,0x23,0x00,0x2c,0x24,0x02,0x00,0x01,0x10,0x62,0x00,0x07,0xa2,0x24,0x00,0x08,
-0x24,0x02,0x00,0x03,0xa2,0x22,0x00,0x0a,0x92,0x22,0x00,0x2f,0xae,0x20,0x00,0x2c,
-0x08,0x00,0x0d,0x51,0xa2,0x22,0x00,0x06,0x08,0x00,0x0d,0xbe,0xa2,0x20,0x00,0x0a,
-0x8e,0x22,0x00,0x28,0x24,0x03,0x00,0x01,0x24,0x04,0x00,0x01,0x10,0x44,0x00,0x07,
-0xa2,0x23,0x00,0x08,0x24,0x02,0x00,0x05,0xa2,0x22,0x00,0x0a,0x92,0x22,0x00,0x2b,
-0xae,0x20,0x00,0x28,0x08,0x00,0x0d,0x51,0xa2,0x22,0x00,0x05,0x08,0x00,0x0d,0xca,
-0x24,0x02,0x00,0x04,0x12,0x02,0x00,0x12,0x2a,0x02,0x00,0x41,0x10,0x40,0x00,0x09,
-0x24,0x02,0x00,0x80,0x24,0x02,0x00,0x20,0x16,0x02,0xff,0x7b,0x3c,0x02,0xb0,0x05,
-0x24,0x02,0x00,0x12,0xa2,0x22,0x00,0x0a,0xa2,0x22,0x00,0x08,0x08,0x00,0x0d,0x51,
-0xae,0x20,0x00,0x3c,0x16,0x02,0xff,0x74,0x3c,0x02,0xb0,0x05,0x24,0x02,0x00,0x10,
-0xa2,0x22,0x00,0x0a,0xa2,0x22,0x00,0x08,0x08,0x00,0x0d,0x51,0xae,0x20,0x00,0x34,
-0x24,0x02,0x00,0x11,0xa2,0x22,0x00,0x0a,0xa2,0x22,0x00,0x08,0x08,0x00,0x0d,0x51,
-0xae,0x20,0x00,0x38,0x8e,0x24,0x00,0x30,0x24,0x02,0x00,0x03,0x24,0x03,0x00,0x01,
-0x10,0x83,0x00,0x07,0xa2,0x22,0x00,0x08,0x24,0x02,0x00,0x02,0xa2,0x22,0x00,0x0a,
-0x92,0x22,0x00,0x33,0xae,0x20,0x00,0x30,0x08,0x00,0x0d,0x51,0xa2,0x22,0x00,0x07,
-0x08,0x00,0x0d,0xf0,0xa2,0x24,0x00,0x0a,0x8f,0x84,0xb4,0x40,0xae,0x20,0x00,0x34,
-0x94,0x85,0x00,0x14,0x0c,0x00,0x1b,0x66,0x32,0x10,0x00,0xff,0x08,0x00,0x0d,0x42,
-0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x24,0x42,0x37,0xf4,
-0x34,0x63,0x00,0x20,0xac,0x62,0x00,0x00,0x80,0xa2,0x00,0x15,0x3c,0x06,0xb0,0x05,
-0x10,0x40,0x00,0x0a,0x34,0xc6,0x02,0x54,0x83,0x83,0x8b,0xd4,0x00,0x00,0x00,0x00,
-0xac,0x83,0x00,0x24,0x8c,0xc2,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x02,0x17,0x42,
-0x30,0x42,0x00,0x01,0x03,0xe0,0x00,0x08,0xac,0x82,0x00,0x28,0x8c,0x82,0x00,0x2c,
-0x3c,0x06,0xb0,0x05,0x34,0xc6,0x04,0x50,0x00,0x02,0x18,0x43,0x30,0x63,0x00,0x01,
-0x10,0x40,0x00,0x04,0x30,0x45,0x00,0x01,0xac,0x83,0x00,0x28,0x03,0xe0,0x00,0x08,
-0xac,0x85,0x00,0x24,0x90,0xc2,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xff,
-0x30,0x43,0x00,0x02,0x30,0x42,0x00,0x01,0xac,0x83,0x00,0x28,0x03,0xe0,0x00,0x08,
-0xac,0x82,0x00,0x24,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x27,0xbd,0xff,0xd8,
-0x34,0x63,0x00,0x20,0x24,0x42,0x38,0x84,0xac,0x62,0x00,0x00,0xaf,0xb1,0x00,0x1c,
-0xaf,0xbf,0x00,0x20,0xaf,0xb0,0x00,0x18,0x90,0xa6,0x00,0x0a,0x27,0x83,0xb4,0x00,
-0x00,0xa0,0x88,0x21,0x00,0x06,0x10,0x80,0x00,0x43,0x10,0x21,0x8c,0x50,0x00,0x00,
-0x80,0xa5,0x00,0x11,0x92,0x03,0x00,0x12,0x10,0xa0,0x00,0x04,0xa2,0x20,0x00,0x15,
-0x24,0x02,0x00,0x12,0x10,0xc2,0x00,0xda,0x00,0x00,0x00,0x00,0x82,0x22,0x00,0x12,
-0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x67,0x00,0x00,0x00,0x00,0xa2,0x20,0x00,0x12,
-0xa2,0x00,0x00,0x19,0x86,0x23,0x00,0x0c,0x00,0x00,0x00,0x00,0x00,0x03,0x10,0xc0,
-0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,0x27,0x83,0x90,0x20,0x00,0x43,0x10,0x21,
-0xa0,0x40,0x00,0x00,0x92,0x03,0x00,0x16,0x00,0x00,0x00,0x00,0x30,0x63,0x00,0xdf,
-0xa2,0x03,0x00,0x16,0x82,0x02,0x00,0x12,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x20,
-0x00,0x00,0x00,0x00,0x92,0x23,0x00,0x08,0x00,0x00,0x00,0x00,0x14,0x60,0x00,0x45,
-0x24,0x02,0x00,0x01,0xa2,0x20,0x00,0x04,0x92,0x08,0x00,0x04,0x00,0x00,0x00,0x00,
-0x15,0x00,0x00,0x1e,0x24,0x02,0x00,0x01,0x92,0x07,0x00,0x0a,0xa2,0x02,0x00,0x17,
-0x92,0x02,0x00,0x16,0x30,0xe3,0x00,0xff,0x30,0x42,0x00,0xe4,0x10,0x60,0x00,0x03,
-0xa2,0x02,0x00,0x16,0x34,0x42,0x00,0x01,0xa2,0x02,0x00,0x16,0x11,0x00,0x00,0x05,
-0x00,0x00,0x00,0x00,0x92,0x02,0x00,0x16,0x00,0x00,0x00,0x00,0x34,0x42,0x00,0x02,
-0xa2,0x02,0x00,0x16,0x92,0x02,0x00,0x17,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x08,
-0x00,0x00,0x00,0x00,0x96,0x02,0x00,0x06,0x00,0x00,0x00,0x00,0xa6,0x02,0x00,0x14,
-0x8f,0xbf,0x00,0x20,0x7b,0xb0,0x00,0xfc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x28,
-0x96,0x02,0x00,0x00,0x08,0x00,0x0e,0x6c,0xa6,0x02,0x00,0x14,0x92,0x07,0x00,0x0a,
-0x00,0x00,0x00,0x00,0x14,0xe0,0x00,0x03,0x00,0x00,0x00,0x00,0x08,0x00,0x0e,0x58,
-0xa2,0x00,0x00,0x17,0x96,0x04,0x00,0x00,0x96,0x05,0x00,0x06,0x27,0x86,0x90,0x00,
-0x00,0x04,0x18,0xc0,0x00,0x64,0x18,0x21,0x00,0x05,0x10,0xc0,0x00,0x45,0x10,0x21,
-0x00,0x03,0x18,0x80,0x00,0x66,0x18,0x21,0x00,0x02,0x10,0x80,0x00,0x46,0x10,0x21,
-0x8c,0x66,0x00,0x08,0x8c,0x45,0x00,0x08,0x3c,0x03,0x80,0x00,0x00,0xc3,0x20,0x24,
-0x10,0x80,0x00,0x08,0x00,0xa3,0x10,0x24,0x10,0x40,0x00,0x04,0x00,0x00,0x18,0x21,
-0x10,0x80,0x00,0x02,0x24,0x03,0x00,0x01,0x00,0xa6,0x18,0x2b,0x08,0x00,0x0e,0x58,
-0xa2,0x03,0x00,0x17,0x10,0x40,0xff,0xfd,0x00,0xa6,0x18,0x2b,0x08,0x00,0x0e,0x8c,
-0x00,0x00,0x00,0x00,0x10,0x62,0x00,0x09,0x24,0x02,0x00,0x02,0x10,0x62,0x00,0x05,
-0x24,0x02,0x00,0x03,0x14,0x62,0xff,0xb8,0x00,0x00,0x00,0x00,0x08,0x00,0x0e,0x52,
-0xa2,0x20,0x00,0x07,0x08,0x00,0x0e,0x52,0xa2,0x20,0x00,0x06,0x08,0x00,0x0e,0x52,
-0xa2,0x20,0x00,0x05,0x82,0x22,0x00,0x10,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x69,
-0x2c,0x62,0x00,0x02,0x10,0x40,0x00,0x49,0x3c,0x02,0xb0,0x09,0x92,0x25,0x00,0x08,
-0x00,0x00,0x00,0x00,0x30,0xa6,0x00,0xff,0x2c,0xc2,0x00,0x04,0x10,0x40,0x00,0x3b,
-0x2c,0xc2,0x00,0x10,0x3c,0x04,0xb0,0x05,0x34,0x84,0x02,0x29,0x90,0x83,0x00,0x00,
-0x24,0x02,0x00,0x01,0x00,0xc2,0x10,0x04,0x00,0x02,0x10,0x27,0x00,0x62,0x18,0x24,
-0xa0,0x83,0x00,0x00,0x86,0x23,0x00,0x0c,0x96,0x26,0x00,0x0c,0x00,0x03,0x10,0xc0,
-0x00,0x43,0x10,0x21,0x00,0x02,0x28,0x80,0x27,0x83,0x90,0x04,0x00,0xa3,0x18,0x21,
-0x8c,0x64,0x00,0x18,0x00,0x00,0x00,0x00,0x8c,0x82,0x00,0x04,0x00,0x00,0x00,0x00,
-0x30,0x42,0x00,0x10,0x10,0x40,0x00,0x18,0x24,0x07,0x00,0x01,0x93,0x82,0x8b,0x71,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x01,0x14,0x40,0x00,0x0a,0x24,0x05,0x00,0x24,
-0x00,0x06,0x2c,0x00,0x00,0x05,0x2c,0x03,0x0c,0x00,0x1b,0x66,0x02,0x00,0x20,0x21,
-0x92,0x02,0x00,0x16,0xa2,0x00,0x00,0x12,0x30,0x42,0x00,0xe7,0x08,0x00,0x0e,0x49,
-0xa2,0x02,0x00,0x16,0xf0,0xc5,0x00,0x06,0x00,0x00,0x28,0x12,0x27,0x82,0x90,0x00,
-0x00,0xa2,0x28,0x21,0x0c,0x00,0x01,0x49,0x3c,0x04,0x00,0x80,0x96,0x26,0x00,0x0c,
-0x08,0x00,0x0e,0xc9,0x00,0x06,0x2c,0x00,0x27,0x83,0x90,0x10,0x27,0x82,0x90,0x18,
-0x00,0xa2,0x10,0x21,0x00,0xa3,0x18,0x21,0x90,0x44,0x00,0x00,0x90,0x65,0x00,0x05,
-0x93,0x82,0x80,0x10,0x00,0x00,0x30,0x21,0x0c,0x00,0x21,0x9a,0xaf,0xa2,0x00,0x10,
-0x96,0x26,0x00,0x0c,0x08,0x00,0x0e,0xc3,0x00,0x00,0x00,0x00,0x14,0x40,0xff,0xcd,
-0x3c,0x04,0xb0,0x05,0x34,0x84,0x02,0x29,0x90,0x83,0x00,0x00,0x30,0xa5,0x00,0x0f,
-0x24,0x02,0x00,0x80,0x08,0x00,0x0e,0xb2,0x00,0xa2,0x10,0x07,0x86,0x26,0x00,0x0c,
-0x3c,0x03,0xb0,0x09,0x34,0x42,0x01,0x72,0x34,0x63,0x01,0x78,0x94,0x47,0x00,0x00,
-0x8c,0x65,0x00,0x00,0x00,0x06,0x10,0xc0,0x00,0x46,0x10,0x21,0x3c,0x04,0xb0,0x09,
-0xae,0x25,0x00,0x1c,0x34,0x84,0x01,0x7c,0x27,0x83,0x90,0x04,0x00,0x02,0x10,0x80,
-0x8c,0x85,0x00,0x00,0x00,0x43,0x10,0x21,0x8c,0x43,0x00,0x18,0xae,0x25,0x00,0x20,
-0xa6,0x27,0x00,0x18,0x8c,0x66,0x00,0x08,0x02,0x20,0x20,0x21,0x0c,0x00,0x0f,0x19,
-0x00,0x00,0x28,0x21,0x86,0x25,0x00,0x18,0x8e,0x26,0x00,0x1c,0x8e,0x27,0x00,0x20,
-0x02,0x20,0x20,0x21,0x0c,0x00,0x1c,0x68,0xaf,0xa2,0x00,0x10,0x08,0x00,0x0e,0x49,
-0xa2,0x02,0x00,0x12,0x92,0x22,0x00,0x08,0x08,0x00,0x0e,0x49,0xa2,0x22,0x00,0x09,
-0xa2,0x20,0x00,0x11,0x80,0x82,0x00,0x50,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x03,
-0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0xd0,0xac,0x40,0x00,0x00,0x08,0x00,0x0e,0x49,
-0xa0,0x80,0x00,0x50,0x94,0x8a,0x00,0x0c,0x24,0x03,0x00,0x24,0x00,0x80,0x70,0x21,
-0x3c,0x02,0x80,0x00,0x3c,0x04,0xb0,0x03,0x24,0x42,0x3c,0x64,0xf1,0x43,0x00,0x06,
-0x34,0x84,0x00,0x20,0x00,0x00,0x18,0x12,0x00,0xa0,0x68,0x21,0xac,0x82,0x00,0x00,
-0x27,0x85,0x90,0x10,0x27,0x82,0x90,0x0f,0x27,0xbd,0xff,0xf8,0x00,0x62,0x60,0x21,
-0x00,0x65,0x58,0x21,0x00,0x00,0xc0,0x21,0x11,0xa0,0x00,0xcc,0x00,0x00,0x78,0x21,
-0x00,0x0a,0x1c,0x00,0x00,0x03,0x1c,0x03,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,
-0x00,0x02,0x10,0x80,0x00,0x45,0x10,0x21,0x91,0x87,0x00,0x00,0x80,0x48,0x00,0x04,
-0x03,0xa0,0x60,0x21,0x00,0x0a,0x1c,0x00,0x00,0x03,0x1c,0x03,0x00,0x03,0x10,0xc0,
-0x00,0x43,0x10,0x21,0x00,0x02,0x48,0x80,0x27,0x83,0x90,0x04,0xa3,0xa7,0x00,0x00,
-0x01,0x23,0x18,0x21,0x8c,0x64,0x00,0x18,0x25,0x02,0xff,0xff,0x00,0x48,0x40,0x0b,
-0x8c,0x83,0x00,0x04,0x2d,0x05,0x00,0x07,0x24,0x02,0x00,0x06,0x30,0x63,0x00,0x08,
-0x14,0x60,0x00,0x35,0x00,0x45,0x40,0x0a,0x93,0xa7,0x00,0x00,0x27,0x82,0x90,0x18,
-0x01,0x22,0x10,0x21,0x30,0xe3,0x00,0xf0,0x38,0x63,0x00,0x50,0x30,0xe5,0x00,0xff,
-0x00,0x05,0x20,0x2b,0x00,0x03,0x18,0x2b,0x00,0x64,0x18,0x24,0x90,0x49,0x00,0x00,
-0x10,0x60,0x00,0x16,0x30,0xe4,0x00,0x0f,0x24,0x02,0x00,0x04,0x10,0xa2,0x00,0x9d,
-0x00,0x00,0x00,0x00,0x11,0xa0,0x00,0x3a,0x2c,0xa2,0x00,0x0c,0x10,0x40,0x00,0x02,
-0x24,0x84,0x00,0x0c,0x00,0xe0,0x20,0x21,0x30,0x84,0x00,0xff,0x00,0x04,0x10,0x40,
-0x27,0x83,0xbb,0x1c,0x00,0x44,0x10,0x21,0x00,0x43,0x10,0x21,0x90,0x47,0x00,0x00,
-0x00,0x00,0x00,0x00,0x2c,0xe3,0x00,0x0c,0xa3,0xa7,0x00,0x00,0x10,0x60,0x00,0x02,
-0x24,0xe2,0x00,0x04,0x00,0xe0,0x10,0x21,0xa3,0xa2,0x00,0x00,0x91,0x65,0x00,0x00,
-0x91,0x82,0x00,0x00,0x30,0xa3,0x00,0xff,0x00,0x62,0x10,0x2b,0x10,0x40,0x00,0x0e,
-0x2c,0x62,0x00,0x0c,0x14,0x40,0x00,0x03,0x00,0x60,0x20,0x21,0x30,0xa2,0x00,0x0f,
-0x24,0x44,0x00,0x0c,0x00,0x04,0x10,0x40,0x00,0x44,0x20,0x21,0x27,0x83,0xbb,0x1c,
-0x00,0x83,0x18,0x21,0x90,0x62,0x00,0x02,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x05,
-0x00,0x09,0x11,0x00,0xa1,0x85,0x00,0x00,0x93,0xa2,0x00,0x00,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x08,0x00,0x49,0x10,0x23,0x00,0x02,0x10,0x80,0x00,0x49,0x10,0x23,
-0x00,0x02,0x10,0x80,0x00,0x44,0x10,0x21,0x27,0x83,0xb4,0xa8,0x00,0x43,0x10,0x21,
-0x90,0x44,0x00,0x00,0x00,0x00,0x00,0x00,0x2c,0x83,0x00,0x0c,0x14,0x60,0x00,0x06,
-0x00,0x80,0x10,0x21,0x00,0x18,0x10,0x40,0x00,0x4f,0x10,0x21,0x00,0x02,0x11,0x00,
-0x00,0x82,0x10,0x21,0x24,0x42,0x00,0x04,0x08,0x00,0x0f,0x7a,0xa1,0x82,0x00,0x00,
-0x8f,0x8d,0x81,0x5c,0x00,0x00,0x00,0x00,0x01,0xa8,0x10,0x21,0x90,0x43,0x00,0x00,
-0x00,0x00,0x00,0x00,0x10,0x60,0xff,0xd1,0x00,0x00,0x28,0x21,0x00,0x06,0x74,0x82,
-0x30,0xe2,0x00,0xff,0x2c,0x42,0x00,0x0c,0x14,0x40,0x00,0x03,0x00,0xe0,0x10,0x21,
-0x30,0xe2,0x00,0x0f,0x24,0x42,0x00,0x0c,0x30,0x44,0x00,0xff,0xa3,0xa2,0x00,0x00,
-0x24,0x02,0x00,0x0c,0x10,0x82,0x00,0x0d,0x00,0x09,0x11,0x00,0x00,0x49,0x10,0x23,
-0x00,0x02,0x10,0x80,0x00,0x04,0x18,0x40,0x00,0x49,0x10,0x23,0x00,0x64,0x18,0x21,
-0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,0x27,0x84,0xb4,0xa8,0x00,0x44,0x10,0x21,
-0x90,0x47,0x00,0x00,0x00,0x00,0x00,0x00,0xa3,0xa7,0x00,0x00,0x00,0x0a,0x1c,0x00,
-0x00,0x03,0x1c,0x03,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,
-0x27,0x83,0x90,0x04,0x00,0x43,0x10,0x21,0x8c,0x44,0x00,0x18,0x00,0x00,0x00,0x00,
-0x8c,0x83,0x00,0x04,0x00,0x00,0x00,0x00,0x30,0x63,0x00,0x10,0x14,0x60,0x00,0x33,
-0x00,0x06,0x14,0x42,0x00,0x09,0x11,0x00,0x00,0x49,0x10,0x23,0x00,0x02,0x10,0x80,
-0x00,0x49,0x10,0x23,0x27,0x83,0xb5,0x78,0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,
-0x90,0x44,0x00,0x04,0x90,0x43,0x00,0x05,0x00,0x00,0x00,0x00,0x00,0x64,0xc0,0x24,
-0x93,0xa7,0x00,0x00,0x00,0x00,0x00,0x00,0x2c,0xe2,0x00,0x0f,0x10,0x40,0x00,0x0f,
-0x31,0xcf,0x00,0x01,0x00,0x0a,0x1c,0x00,0x00,0x03,0x1c,0x03,0x00,0x03,0x10,0xc0,
-0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,0x27,0x84,0x90,0x00,0x00,0x44,0x10,0x21,
-0x84,0x43,0x00,0x06,0x00,0x00,0x00,0x00,0x28,0x63,0x06,0x41,0x14,0x60,0x00,0x04,
-0x30,0xe2,0x00,0xff,0x24,0x07,0x00,0x0f,0xa3,0xa7,0x00,0x00,0x30,0xe2,0x00,0xff,
-0x2c,0x42,0x00,0x0c,0x14,0x40,0x00,0x06,0x00,0xe0,0x10,0x21,0x00,0x18,0x10,0x40,
-0x00,0x4f,0x10,0x21,0x00,0x02,0x11,0x00,0x00,0x47,0x10,0x21,0x24,0x42,0x00,0x04,
-0xa3,0xa2,0x00,0x00,0x00,0x40,0x38,0x21,0x01,0xa8,0x10,0x21,0x90,0x43,0x00,0x00,
-0x24,0xa4,0x00,0x01,0x30,0x85,0xff,0xff,0x00,0xa3,0x18,0x2b,0x14,0x60,0xff,0xad,
-0x30,0xe2,0x00,0xff,0x08,0x00,0x0f,0x67,0x00,0x00,0x00,0x00,0x08,0x00,0x0f,0xc8,
-0x30,0x58,0x00,0x01,0x81,0xc2,0x00,0x48,0x00,0x00,0x00,0x00,0x10,0x40,0xff,0x73,
-0x00,0x00,0x00,0x00,0x08,0x00,0x0f,0x55,0x00,0x00,0x00,0x00,0x00,0x0a,0x1c,0x00,
-0x00,0x03,0x1c,0x03,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,
-0x00,0x45,0x10,0x21,0x80,0x48,0x00,0x05,0x91,0x67,0x00,0x00,0x08,0x00,0x0f,0x35,
-0x03,0xa0,0x58,0x21,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x34,0x63,0x00,0x20,
-0x24,0x42,0x40,0x04,0x03,0xe0,0x00,0x08,0xac,0x62,0x00,0x00,0x27,0xbd,0xff,0xc0,
-0xaf,0xb7,0x00,0x34,0xaf,0xb6,0x00,0x30,0xaf,0xb5,0x00,0x2c,0xaf,0xb4,0x00,0x28,
-0xaf,0xb3,0x00,0x24,0xaf,0xb2,0x00,0x20,0xaf,0xbf,0x00,0x3c,0xaf,0xbe,0x00,0x38,
-0xaf,0xb1,0x00,0x1c,0xaf,0xb0,0x00,0x18,0x84,0x82,0x00,0x0c,0x27,0x93,0x90,0x04,
-0x3c,0x05,0xb0,0x03,0x00,0x02,0x18,0xc0,0x00,0x62,0x18,0x21,0x00,0x03,0x18,0x80,
-0x00,0x73,0x10,0x21,0x8c,0x5e,0x00,0x18,0x3c,0x02,0x80,0x00,0x34,0xa5,0x00,0x20,
-0x24,0x42,0x40,0x1c,0xac,0xa2,0x00,0x00,0x8f,0xd0,0x00,0x08,0x27,0x95,0x90,0x10,
-0x00,0x75,0x18,0x21,0x00,0x00,0x28,0x21,0x02,0x00,0x30,0x21,0x90,0x71,0x00,0x00,
-0x0c,0x00,0x0f,0x19,0x00,0x80,0xb0,0x21,0x00,0x40,0x90,0x21,0x00,0x10,0x14,0x42,
-0x30,0x54,0x00,0x01,0x02,0x40,0x20,0x21,0x00,0x10,0x14,0x82,0x02,0x80,0x28,0x21,
-0x12,0x51,0x00,0x23,0x00,0x10,0xbf,0xc2,0x86,0xc3,0x00,0x0c,0x30,0x50,0x00,0x01,
-0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x55,0x10,0x21,
-0xa0,0x52,0x00,0x00,0x86,0xc3,0x00,0x0c,0x00,0x00,0x00,0x00,0x00,0x03,0x10,0xc0,
-0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x53,0x30,0x21,0x8c,0xc7,0x00,0x18,
-0x27,0x83,0x90,0x00,0x00,0x43,0x10,0x21,0x8c,0xe3,0x00,0x04,0x84,0x46,0x00,0x06,
-0x00,0x03,0x19,0x42,0x0c,0x00,0x08,0xe3,0x30,0x73,0x00,0x01,0x00,0x40,0x88,0x21,
-0x02,0x40,0x20,0x21,0x02,0x80,0x28,0x21,0x16,0xe0,0x00,0x10,0x02,0x00,0x30,0x21,
-0x86,0xc2,0x00,0x0c,0x00,0x00,0x00,0x00,0x00,0x02,0x18,0xc0,0x00,0x62,0x18,0x21,
-0x00,0x03,0x18,0x80,0x27,0x82,0x90,0x08,0x00,0x62,0x18,0x21,0xa4,0x71,0x00,0x04,
-0x7b,0xbe,0x01,0xfc,0x7b,0xb6,0x01,0xbc,0x7b,0xb4,0x01,0x7c,0x7b,0xb2,0x01,0x3c,
-0x7b,0xb0,0x00,0xfc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x40,0x86,0xc3,0x00,0x0c,
-0xaf,0xb3,0x00,0x10,0xaf,0xa0,0x00,0x14,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,
-0x00,0x02,0x10,0x80,0x00,0x55,0x10,0x21,0x80,0x47,0x00,0x06,0x00,0x00,0x00,0x00,
-0x24,0xe7,0x00,0x02,0x00,0x07,0x17,0xc2,0x00,0xe2,0x38,0x21,0x00,0x07,0x38,0x43,
-0x00,0x07,0x38,0x40,0x0c,0x00,0x09,0x0a,0x03,0xc7,0x38,0x21,0x08,0x00,0x10,0x48,
-0x02,0x22,0x88,0x21,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x27,0xbd,0xff,0xd0,
-0x34,0x63,0x00,0x20,0x24,0x42,0x41,0xa4,0xaf,0xb2,0x00,0x20,0xac,0x62,0x00,0x00,
-0xaf,0xbf,0x00,0x28,0xaf,0xb3,0x00,0x24,0xaf,0xb1,0x00,0x1c,0xaf,0xb0,0x00,0x18,
-0x3c,0x02,0xb0,0x03,0x90,0x83,0x00,0x0a,0x34,0x42,0x01,0x04,0x94,0x45,0x00,0x00,
-0x00,0x03,0x18,0x80,0x27,0x82,0xb4,0x00,0x00,0x62,0x18,0x21,0x30,0xa6,0xff,0xff,
-0x8c,0x71,0x00,0x00,0x80,0x85,0x00,0x12,0x30,0xc9,0x00,0xff,0x00,0x06,0x32,0x02,
-0xa4,0x86,0x00,0x44,0xa4,0x89,0x00,0x46,0x82,0x22,0x00,0x12,0x00,0x80,0x90,0x21,
-0x10,0xa0,0x00,0x1b,0xa0,0x80,0x00,0x15,0x00,0xc5,0x10,0x2a,0x10,0x40,0x00,0x14,
-0x00,0x00,0x00,0x00,0xa2,0x20,0x00,0x19,0x84,0x83,0x00,0x0c,0x00,0x00,0x00,0x00,
-0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,0x27,0x83,0x90,0x20,
-0x00,0x43,0x10,0x21,0xa0,0x40,0x00,0x00,0xa0,0x80,0x00,0x12,0x92,0x22,0x00,0x16,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xdf,0xa2,0x22,0x00,0x16,0x8f,0xbf,0x00,0x28,
-0x7b,0xb2,0x01,0x3c,0x7b,0xb0,0x00,0xfc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x30,
-0x0c,0x00,0x10,0x01,0x00,0x00,0x00,0x00,0x08,0x00,0x10,0x97,0x00,0x00,0x00,0x00,
-0x28,0x42,0x00,0x02,0x10,0x40,0x01,0x76,0x00,0x00,0x28,0x21,0x94,0x87,0x00,0x0c,
-0x00,0x00,0x00,0x00,0x00,0xe0,0x10,0x21,0x00,0x02,0x14,0x00,0x00,0x02,0x14,0x03,
-0x00,0x07,0x24,0x00,0x00,0x04,0x24,0x03,0x00,0x02,0x18,0xc0,0x00,0x62,0x18,0x21,
-0x00,0x04,0x28,0xc0,0x00,0xa4,0x28,0x21,0x27,0x82,0x90,0x20,0x00,0x03,0x18,0x80,
-0x00,0x62,0x18,0x21,0x00,0x05,0x28,0x80,0x27,0x82,0x90,0x08,0x00,0xa2,0x10,0x21,
-0x8c,0x68,0x00,0x00,0x80,0x44,0x00,0x06,0x27,0x82,0x90,0x10,0x00,0x08,0x1d,0x02,
-0x00,0xa2,0x28,0x21,0x38,0x84,0x00,0x00,0x30,0x63,0x00,0x01,0x01,0x24,0x30,0x0b,
-0x80,0xaa,0x00,0x04,0x80,0xa9,0x00,0x05,0x10,0x60,0x00,0x02,0x00,0x08,0x14,0x02,
-0x30,0x46,0x00,0x0f,0x15,0x20,0x00,0x28,0x01,0x49,0x10,0x21,0x15,0x40,0x00,0x11,
-0x30,0xe3,0xff,0xff,0x92,0x45,0x00,0x08,0x00,0x00,0x00,0x00,0x30,0xa8,0x00,0xff,
-0x2d,0x02,0x00,0x04,0x10,0x40,0x01,0x46,0x2d,0x02,0x00,0x10,0x3c,0x04,0xb0,0x05,
-0x34,0x84,0x02,0x29,0x90,0x83,0x00,0x00,0x24,0x02,0x00,0x01,0x01,0x02,0x10,0x04,
-0x00,0x62,0x18,0x25,0xa0,0x83,0x00,0x00,0x96,0x47,0x00,0x0c,0x00,0x00,0x00,0x00,
-0x30,0xe3,0xff,0xff,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x27,0x84,0x90,0x10,
-0x00,0x02,0x10,0x80,0x00,0x44,0x10,0x21,0x80,0x45,0x00,0x06,0x00,0x03,0x1a,0x00,
-0x3c,0x04,0xb0,0x00,0x00,0x65,0x18,0x21,0x00,0x64,0x20,0x21,0x94,0x82,0x00,0x00,
-0x82,0x43,0x00,0x10,0x00,0x02,0x14,0x00,0x14,0x60,0x00,0x06,0x00,0x02,0x3c,0x03,
-0x30,0xe2,0x00,0x04,0x14,0x40,0x00,0x04,0x01,0x49,0x10,0x21,0x34,0xe2,0x08,0x00,
-0xa4,0x82,0x00,0x00,0x01,0x49,0x10,0x21,0x00,0x02,0x16,0x00,0x00,0x02,0x16,0x03,
-0x00,0x46,0x10,0x2a,0x10,0x40,0x00,0x7c,0x00,0x00,0x00,0x00,0x82,0x42,0x00,0x10,
-0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x0e,0x00,0x00,0x00,0x00,0x86,0x43,0x00,0x0c,
-0x25,0x44,0x00,0x01,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,
-0x27,0x83,0x90,0x10,0x00,0x43,0x10,0x21,0xa0,0x44,0x00,0x04,0x92,0x23,0x00,0x16,
-0x02,0x40,0x20,0x21,0x30,0x63,0x00,0xfb,0x08,0x00,0x10,0x9c,0xa2,0x23,0x00,0x16,
-0x86,0x43,0x00,0x0c,0x25,0x24,0x00,0x01,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,
-0x00,0x02,0x10,0x80,0x27,0x83,0x90,0x10,0x00,0x43,0x10,0x21,0xa0,0x44,0x00,0x05,
-0x86,0x45,0x00,0x0c,0x0c,0x00,0x1e,0xea,0x02,0x20,0x20,0x21,0x10,0x40,0x00,0x5a,
-0x00,0x00,0x00,0x00,0x92,0x45,0x00,0x08,0x00,0x00,0x00,0x00,0x30,0xa6,0x00,0xff,
-0x2c,0xc2,0x00,0x04,0x10,0x40,0x00,0x4c,0x2c,0xc2,0x00,0x10,0x3c,0x04,0xb0,0x05,
-0x34,0x84,0x02,0x29,0x90,0x83,0x00,0x00,0x24,0x02,0x00,0x01,0x00,0xc2,0x10,0x04,
-0x00,0x02,0x10,0x27,0x00,0x62,0x18,0x24,0xa0,0x83,0x00,0x00,0x92,0x45,0x00,0x08,
-0x00,0x00,0x00,0x00,0x30,0xa5,0x00,0xff,0x14,0xa0,0x00,0x33,0x24,0x02,0x00,0x01,
-0xa2,0x40,0x00,0x04,0x92,0x22,0x00,0x04,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x0c,
-0x24,0x02,0x00,0x01,0xa2,0x22,0x00,0x17,0x92,0x22,0x00,0x17,0x00,0x00,0x00,0x00,
-0x10,0x40,0x00,0x04,0x00,0x00,0x00,0x00,0x96,0x22,0x00,0x06,0x08,0x00,0x10,0x97,
-0xa6,0x22,0x00,0x14,0x96,0x22,0x00,0x00,0x08,0x00,0x10,0x97,0xa6,0x22,0x00,0x14,
-0x92,0x22,0x00,0x0a,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x03,0x00,0x00,0x00,0x00,
-0x08,0x00,0x11,0x26,0xa2,0x20,0x00,0x17,0x96,0x24,0x00,0x00,0x96,0x25,0x00,0x06,
-0x27,0x86,0x90,0x00,0x00,0x04,0x18,0xc0,0x00,0x64,0x18,0x21,0x00,0x05,0x10,0xc0,
-0x00,0x45,0x10,0x21,0x00,0x03,0x18,0x80,0x00,0x66,0x18,0x21,0x00,0x02,0x10,0x80,
-0x00,0x46,0x10,0x21,0x8c,0x65,0x00,0x08,0x8c,0x44,0x00,0x08,0x3c,0x03,0x80,0x00,
-0x00,0xa3,0x30,0x24,0x10,0xc0,0x00,0x08,0x00,0x83,0x10,0x24,0x10,0x40,0x00,0x04,
-0x00,0x00,0x18,0x21,0x10,0xc0,0x00,0x02,0x24,0x03,0x00,0x01,0x00,0x85,0x18,0x2b,
-0x08,0x00,0x11,0x26,0xa2,0x23,0x00,0x17,0x10,0x40,0xff,0xfd,0x00,0x85,0x18,0x2b,
-0x08,0x00,0x11,0x49,0x00,0x00,0x00,0x00,0x10,0xa2,0x00,0x09,0x24,0x02,0x00,0x02,
-0x10,0xa2,0x00,0x05,0x24,0x02,0x00,0x03,0x14,0xa2,0xff,0xca,0x00,0x00,0x00,0x00,
-0x08,0x00,0x11,0x21,0xa2,0x40,0x00,0x07,0x08,0x00,0x11,0x21,0xa2,0x40,0x00,0x06,
-0x08,0x00,0x11,0x21,0xa2,0x40,0x00,0x05,0x14,0x40,0xff,0xbe,0x3c,0x04,0xb0,0x05,
-0x34,0x84,0x02,0x29,0x90,0x83,0x00,0x00,0x30,0xa5,0x00,0x0f,0x24,0x02,0x00,0x80,
-0x08,0x00,0x11,0x18,0x00,0xa2,0x10,0x07,0x0c,0x00,0x10,0x07,0x02,0x40,0x20,0x21,
-0x08,0x00,0x10,0x97,0x00,0x00,0x00,0x00,0x92,0x45,0x00,0x08,0x00,0x00,0x00,0x00,
-0x30,0xa6,0x00,0xff,0x2c,0xc2,0x00,0x04,0x10,0x40,0x00,0x99,0x2c,0xc2,0x00,0x10,
-0x3c,0x04,0xb0,0x05,0x34,0x84,0x02,0x29,0x90,0x83,0x00,0x00,0x24,0x02,0x00,0x01,
-0x00,0xc2,0x10,0x04,0x00,0x02,0x10,0x27,0x00,0x62,0x18,0x24,0xa0,0x83,0x00,0x00,
-0x92,0x45,0x00,0x08,0x00,0x00,0x00,0x00,0x30,0xa5,0x00,0xff,0x14,0xa0,0x00,0x80,
-0x24,0x02,0x00,0x01,0xa2,0x40,0x00,0x04,0x86,0x43,0x00,0x0c,0x27,0x93,0x90,0x04,
-0x96,0x47,0x00,0x0c,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x00,0x02,0x28,0x80,
-0x00,0xb3,0x18,0x21,0x8c,0x64,0x00,0x18,0x00,0x00,0x00,0x00,0x8c,0x82,0x00,0x04,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x10,0x10,0x40,0x00,0x64,0x00,0x00,0x30,0x21,
-0x00,0x07,0x1c,0x00,0x00,0x03,0x1c,0x03,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,
-0x00,0x02,0x10,0x80,0x00,0x53,0x10,0x21,0x8c,0x43,0x00,0x18,0x93,0x82,0x8b,0x71,
-0x8c,0x64,0x00,0x04,0x30,0x42,0x00,0x01,0x00,0x04,0x21,0x42,0x14,0x40,0x00,0x4d,
-0x30,0x90,0x00,0x01,0x00,0x07,0x2c,0x00,0x00,0x05,0x2c,0x03,0x0c,0x00,0x1b,0x66,
-0x02,0x20,0x20,0x21,0x96,0x26,0x00,0x06,0x12,0x00,0x00,0x14,0x30,0xc5,0xff,0xff,
-0x02,0x60,0x90,0x21,0x00,0x05,0x10,0xc0,0x00,0x45,0x10,0x21,0x00,0x02,0x10,0x80,
-0x00,0x52,0x18,0x21,0x92,0x22,0x00,0x0a,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x0b,
-0x02,0x20,0x20,0x21,0x8c,0x63,0x00,0x18,0x00,0x00,0x00,0x00,0x8c,0x62,0x00,0x04,
-0x00,0x00,0x00,0x00,0x00,0x02,0x11,0x42,0x0c,0x00,0x1b,0x66,0x30,0x50,0x00,0x01,
-0x96,0x26,0x00,0x06,0x16,0x00,0xff,0xef,0x30,0xc5,0xff,0xff,0x92,0x22,0x00,0x04,
-0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x0d,0x24,0x02,0x00,0x01,0xa2,0x22,0x00,0x17,
-0x92,0x22,0x00,0x17,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x05,0x00,0x00,0x00,0x00,
-0xa6,0x26,0x00,0x14,0x92,0x22,0x00,0x16,0x08,0x00,0x10,0x96,0x30,0x42,0x00,0xc3,
-0x96,0x22,0x00,0x00,0x08,0x00,0x11,0xbd,0xa6,0x22,0x00,0x14,0x92,0x22,0x00,0x0a,
-0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x03,0x00,0x00,0x00,0x00,0x08,0x00,0x11,0xb8,
-0xa2,0x20,0x00,0x17,0x96,0x24,0x00,0x00,0x30,0xc5,0xff,0xff,0x00,0x05,0x18,0xc0,
-0x00,0x04,0x10,0xc0,0x00,0x44,0x10,0x21,0x00,0x65,0x18,0x21,0x27,0x84,0x90,0x00,
-0x00,0x02,0x10,0x80,0x00,0x44,0x10,0x21,0x00,0x03,0x18,0x80,0x8c,0x45,0x00,0x08,
-0x00,0x64,0x18,0x21,0x8c,0x64,0x00,0x08,0x3c,0x02,0x80,0x00,0x00,0xa2,0x38,0x24,
-0x10,0xe0,0x00,0x08,0x00,0x82,0x10,0x24,0x10,0x40,0x00,0x04,0x00,0x00,0x18,0x21,
-0x10,0xe0,0x00,0x02,0x24,0x03,0x00,0x01,0x00,0x85,0x18,0x2b,0x08,0x00,0x11,0xb8,
-0xa2,0x23,0x00,0x17,0x10,0x40,0xff,0xfd,0x00,0x85,0x18,0x2b,0x08,0x00,0x11,0xdc,
-0x00,0x00,0x00,0x00,0x24,0x05,0x00,0x24,0xf0,0xe5,0x00,0x06,0x00,0x00,0x28,0x12,
-0x27,0x82,0x90,0x00,0x00,0xa2,0x28,0x21,0x0c,0x00,0x01,0x49,0x00,0x00,0x20,0x21,
-0x96,0x47,0x00,0x0c,0x08,0x00,0x11,0x9a,0x00,0x07,0x2c,0x00,0x27,0x83,0x90,0x10,
-0x27,0x82,0x90,0x18,0x00,0xa2,0x10,0x21,0x00,0xa3,0x18,0x21,0x90,0x44,0x00,0x00,
-0x90,0x65,0x00,0x05,0x93,0x82,0x80,0x10,0x24,0x07,0x00,0x01,0x0c,0x00,0x21,0x9a,
-0xaf,0xa2,0x00,0x10,0x96,0x47,0x00,0x0c,0x08,0x00,0x11,0x8d,0x00,0x07,0x1c,0x00,
-0x10,0xa2,0x00,0x09,0x24,0x02,0x00,0x02,0x10,0xa2,0x00,0x05,0x24,0x02,0x00,0x03,
-0x14,0xa2,0xff,0x7d,0x00,0x00,0x00,0x00,0x08,0x00,0x11,0x7e,0xa2,0x40,0x00,0x07,
-0x08,0x00,0x11,0x7e,0xa2,0x40,0x00,0x06,0x08,0x00,0x11,0x7e,0xa2,0x40,0x00,0x05,
-0x14,0x40,0xff,0x71,0x3c,0x04,0xb0,0x05,0x34,0x84,0x02,0x29,0x90,0x83,0x00,0x00,
-0x30,0xa5,0x00,0x0f,0x24,0x02,0x00,0x80,0x08,0x00,0x11,0x75,0x00,0xa2,0x10,0x07,
-0x14,0x40,0xfe,0xc3,0x3c,0x04,0xb0,0x05,0x34,0x84,0x02,0x29,0x90,0x83,0x00,0x00,
-0x30,0xa5,0x00,0x0f,0x24,0x02,0x00,0x80,0x08,0x00,0x10,0xd0,0x00,0xa2,0x10,0x07,
-0x84,0x83,0x00,0x0c,0x00,0x00,0x00,0x00,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,
-0x00,0x02,0x10,0x80,0x27,0x83,0x90,0x04,0x00,0x43,0x10,0x21,0x8c,0x47,0x00,0x18,
-0x00,0x00,0x00,0x00,0x8c,0xe6,0x00,0x08,0x0c,0x00,0x0f,0x19,0x00,0x00,0x00,0x00,
-0x02,0x40,0x20,0x21,0x00,0x00,0x28,0x21,0x00,0x00,0x30,0x21,0x00,0x00,0x38,0x21,
-0x0c,0x00,0x1c,0x68,0xaf,0xa2,0x00,0x10,0x00,0x02,0x1e,0x00,0x14,0x60,0xfe,0x6b,
-0xa2,0x22,0x00,0x12,0x92,0x43,0x00,0x08,0x00,0x00,0x00,0x00,0x14,0x60,0x00,0x40,
-0x24,0x02,0x00,0x01,0xa2,0x40,0x00,0x04,0x92,0x28,0x00,0x04,0x00,0x00,0x00,0x00,
-0x15,0x00,0x00,0x19,0x24,0x02,0x00,0x01,0x92,0x27,0x00,0x0a,0xa2,0x22,0x00,0x17,
-0x92,0x22,0x00,0x17,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x10,0x00,0x00,0x00,0x00,
-0x96,0x22,0x00,0x06,0x00,0x00,0x00,0x00,0xa6,0x22,0x00,0x14,0x92,0x22,0x00,0x16,
-0x30,0xe3,0x00,0xff,0x30,0x42,0x00,0xc0,0x10,0x60,0x00,0x03,0xa2,0x22,0x00,0x16,
-0x34,0x42,0x00,0x01,0xa2,0x22,0x00,0x16,0x11,0x00,0xfe,0x50,0x00,0x00,0x00,0x00,
-0x92,0x22,0x00,0x16,0x08,0x00,0x10,0x96,0x34,0x42,0x00,0x02,0x96,0x22,0x00,0x00,
-0x08,0x00,0x12,0x3f,0xa6,0x22,0x00,0x14,0x92,0x27,0x00,0x0a,0x00,0x00,0x00,0x00,
-0x14,0xe0,0x00,0x03,0x00,0x00,0x00,0x00,0x08,0x00,0x12,0x38,0xa2,0x20,0x00,0x17,
-0x96,0x24,0x00,0x00,0x96,0x25,0x00,0x06,0x27,0x86,0x90,0x00,0x00,0x04,0x18,0xc0,
-0x00,0x64,0x18,0x21,0x00,0x05,0x10,0xc0,0x00,0x45,0x10,0x21,0x00,0x03,0x18,0x80,
-0x00,0x66,0x18,0x21,0x00,0x02,0x10,0x80,0x00,0x46,0x10,0x21,0x8c,0x65,0x00,0x08,
-0x8c,0x44,0x00,0x08,0x3c,0x03,0x80,0x00,0x00,0xa3,0x30,0x24,0x10,0xc0,0x00,0x08,
-0x00,0x83,0x10,0x24,0x10,0x40,0x00,0x04,0x00,0x00,0x18,0x21,0x10,0xc0,0x00,0x02,
-0x24,0x03,0x00,0x01,0x00,0x85,0x18,0x2b,0x08,0x00,0x12,0x38,0xa2,0x23,0x00,0x17,
-0x10,0x40,0xff,0xfd,0x00,0x85,0x18,0x2b,0x08,0x00,0x12,0x67,0x00,0x00,0x00,0x00,
-0x10,0x62,0x00,0x09,0x24,0x02,0x00,0x02,0x10,0x62,0x00,0x05,0x24,0x02,0x00,0x03,
-0x14,0x62,0xff,0xbd,0x00,0x00,0x00,0x00,0x08,0x00,0x12,0x32,0xa2,0x40,0x00,0x07,
-0x08,0x00,0x12,0x32,0xa2,0x40,0x00,0x06,0x08,0x00,0x12,0x32,0xa2,0x40,0x00,0x05,
-0x3c,0x02,0x80,0x00,0x00,0x82,0x30,0x24,0x10,0xc0,0x00,0x08,0x00,0xa2,0x18,0x24,
-0x10,0x60,0x00,0x04,0x00,0x00,0x10,0x21,0x10,0xc0,0x00,0x02,0x24,0x02,0x00,0x01,
-0x00,0xa4,0x10,0x2b,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x10,0x60,0xff,0xfd,
-0x00,0xa4,0x10,0x2b,0x08,0x00,0x12,0x82,0x00,0x00,0x00,0x00,0x30,0x82,0xff,0xff,
-0x00,0x02,0x18,0xc0,0x00,0x62,0x18,0x21,0x27,0x84,0x90,0x10,0x00,0x03,0x18,0x80,
-0x00,0x64,0x18,0x21,0x80,0x66,0x00,0x06,0x00,0x02,0x12,0x00,0x3c,0x03,0xb0,0x00,
-0x00,0x46,0x10,0x21,0x00,0x45,0x10,0x21,0x03,0xe0,0x00,0x08,0x00,0x43,0x10,0x21,
-0x27,0xbd,0xff,0xe0,0x30,0x82,0x00,0x7c,0x30,0x84,0xff,0x00,0xaf,0xbf,0x00,0x1c,
-0xaf,0xb2,0x00,0x18,0xaf,0xb1,0x00,0x14,0xaf,0xb0,0x00,0x10,0x14,0x40,0x00,0x41,
-0x00,0x04,0x22,0x03,0x24,0x02,0x00,0x04,0x3c,0x10,0xb0,0x03,0x8e,0x10,0x00,0x00,
-0x10,0x82,0x00,0x32,0x24,0x02,0x00,0x08,0x10,0x82,0x00,0x03,0x32,0x02,0x00,0x20,
-0x08,0x00,0x12,0xa8,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x17,0x3c,0x02,0xb0,0x06,
-0x34,0x42,0x80,0x24,0x8c,0x43,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x67,0x00,0xff,
-0x10,0xe0,0x00,0x23,0x00,0x00,0x88,0x21,0x8f,0x85,0x8f,0xe0,0x00,0x40,0x30,0x21,
-0x94,0xa2,0x00,0x08,0x8c,0xc3,0x00,0x00,0x26,0x31,0x00,0x01,0x24,0x42,0x00,0x02,
-0x30,0x42,0x01,0xff,0x34,0x63,0x01,0x00,0x02,0x27,0x20,0x2a,0xa4,0xa2,0x00,0x08,
-0x14,0x80,0xff,0xf7,0xac,0xc3,0x00,0x00,0x84,0xa3,0x00,0x08,0x3c,0x02,0xb0,0x03,
-0x34,0x42,0x00,0x30,0xac,0x43,0x00,0x00,0x27,0x92,0xb4,0x00,0x24,0x11,0x00,0x12,
-0x8e,0x44,0x00,0x00,0x26,0x31,0xff,0xff,0x90,0x82,0x00,0x10,0x00,0x00,0x00,0x00,
-0x10,0x40,0x00,0x03,0x26,0x52,0x00,0x04,0x0c,0x00,0x18,0xd0,0x00,0x00,0x00,0x00,
-0x06,0x21,0xff,0xf7,0x24,0x02,0xff,0xdf,0x02,0x02,0x80,0x24,0x3c,0x01,0xb0,0x03,
-0x0c,0x00,0x13,0x1c,0xac,0x30,0x00,0x00,0x08,0x00,0x12,0xa8,0x00,0x00,0x00,0x00,
-0x8f,0x85,0x8f,0xe0,0x08,0x00,0x12,0xbe,0x00,0x00,0x00,0x00,0x24,0x02,0xff,0x95,
-0x3c,0x03,0xb0,0x03,0x02,0x02,0x80,0x24,0x34,0x63,0x00,0x30,0x3c,0x01,0xb0,0x03,
-0xac,0x30,0x00,0x00,0x0c,0x00,0x12,0xe5,0xac,0x60,0x00,0x00,0x08,0x00,0x12,0xa8,
-0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0x50,0x08,0x00,0x12,0xa8,
-0xac,0x46,0x00,0x00,0x3c,0x0a,0x80,0x00,0x25,0x4a,0x4b,0x94,0x3c,0x0b,0xb0,0x03,
-0xad,0x6a,0x00,0x20,0x3c,0x08,0x80,0x01,0x25,0x08,0x00,0x00,0x3c,0x09,0x80,0x01,
-0x25,0x29,0x03,0x50,0x11,0x09,0x00,0x10,0x00,0x00,0x00,0x00,0x3c,0x0a,0x80,0x00,
-0x25,0x4a,0x4b,0xbc,0x3c,0x0b,0xb0,0x03,0xad,0x6a,0x00,0x20,0x3c,0x08,0xb0,0x06,
-0x35,0x08,0x80,0x10,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x8d,0x09,0x00,0x00,
-0x00,0x00,0x00,0x00,0x31,0x29,0x00,0x01,0x00,0x00,0x00,0x00,0x24,0x01,0x00,0x01,
-0x15,0x21,0xff,0xf2,0x00,0x00,0x00,0x00,0x3c,0x0a,0x80,0x00,0x25,0x4a,0x4b,0xf8,
-0x3c,0x0b,0xb0,0x03,0xad,0x6a,0x00,0x20,0x3c,0x02,0xb0,0x03,0x8c,0x43,0x00,0x00,
-0x00,0x00,0x00,0x00,0x34,0x63,0x00,0x40,0x00,0x00,0x00,0x00,0xac,0x43,0x00,0x00,
-0x00,0x00,0x00,0x00,0x3c,0x0a,0x80,0x00,0x25,0x4a,0x4c,0x24,0x3c,0x0b,0xb0,0x03,
-0xad,0x6a,0x00,0x20,0x3c,0x02,0x80,0x01,0x24,0x42,0x00,0x00,0x3c,0x03,0x80,0x01,
-0x24,0x63,0x03,0x50,0x3c,0x04,0xb0,0x00,0x8c,0x85,0x00,0x00,0x00,0x00,0x00,0x00,
-0xac,0x45,0x00,0x00,0x24,0x42,0x00,0x04,0x24,0x84,0x00,0x04,0x00,0x43,0x08,0x2a,
-0x14,0x20,0xff,0xf9,0x00,0x00,0x00,0x00,0x0c,0x00,0x13,0x1c,0x00,0x00,0x00,0x00,
-0x3c,0x0a,0x80,0x00,0x25,0x4a,0x4c,0x70,0x3c,0x0b,0xb0,0x03,0xad,0x6a,0x00,0x20,
-0x3c,0x02,0x80,0x01,0x24,0x42,0x03,0x50,0x3c,0x03,0x80,0x01,0x24,0x63,0x3f,0x24,
-0xac,0x40,0x00,0x00,0xac,0x40,0x00,0x04,0xac,0x40,0x00,0x08,0xac,0x40,0x00,0x0c,
-0x24,0x42,0x00,0x10,0x00,0x43,0x08,0x2a,0x14,0x20,0xff,0xf9,0x00,0x00,0x00,0x00,
-0x3c,0x0a,0x80,0x00,0x25,0x4a,0x4c,0xb0,0x3c,0x0b,0xb0,0x03,0xad,0x6a,0x00,0x20,
-0x3c,0x1c,0x80,0x01,0x27,0x9c,0x7f,0xf0,0x27,0x9d,0x8b,0xe0,0x00,0x00,0x00,0x00,
-0x27,0x9d,0x8f,0xc8,0x3c,0x0a,0x80,0x00,0x25,0x4a,0x4c,0xd4,0x3c,0x0b,0xb0,0x03,
-0xad,0x6a,0x00,0x20,0x40,0x80,0x68,0x00,0x40,0x08,0x60,0x00,0x00,0x00,0x00,0x00,
-0x35,0x08,0xff,0x01,0x40,0x88,0x60,0x00,0x00,0x00,0x00,0x00,0x0c,0x00,0x15,0x62,
-0x00,0x00,0x00,0x00,0x24,0x84,0xf8,0x00,0x30,0x87,0x00,0x03,0x00,0x04,0x30,0x40,
-0x00,0xc7,0x20,0x23,0x3c,0x02,0xb0,0x0a,0x27,0xbd,0xff,0xe0,0x24,0x03,0xff,0xff,
-0x00,0x82,0x20,0x21,0xaf,0xb1,0x00,0x14,0xac,0x83,0x10,0x00,0xaf,0xbf,0x00,0x18,
-0xaf,0xb0,0x00,0x10,0x00,0xa0,0x88,0x21,0x24,0x03,0x00,0x01,0x8c,0x82,0x10,0x00,
-0x00,0x00,0x00,0x00,0x14,0x43,0xff,0xfd,0x00,0xc7,0x10,0x23,0x3c,0x03,0xb0,0x0a,
-0x00,0x43,0x10,0x21,0x8c,0x50,0x00,0x00,0x0c,0x00,0x13,0x99,0x02,0x20,0x20,0x21,
-0x02,0x11,0x80,0x24,0x00,0x50,0x80,0x06,0x02,0x00,0x10,0x21,0x8f,0xbf,0x00,0x18,
-0x7b,0xb0,0x00,0xbc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x20,0x27,0xbd,0xff,0xd8,
-0xaf,0xb2,0x00,0x18,0x00,0xa0,0x90,0x21,0x24,0x05,0xff,0xff,0xaf,0xb3,0x00,0x1c,
-0xaf,0xbf,0x00,0x20,0xaf,0xb1,0x00,0x14,0xaf,0xb0,0x00,0x10,0x00,0xc0,0x98,0x21,
-0x12,0x45,0x00,0x23,0x24,0x84,0xf8,0x00,0x30,0x83,0x00,0x03,0x00,0x04,0x10,0x40,
-0x00,0x40,0x88,0x21,0x00,0x60,0x20,0x21,0x00,0x43,0x10,0x23,0x3c,0x03,0xb0,0x0a,
-0x00,0x43,0x10,0x21,0xac,0x45,0x10,0x00,0x00,0x40,0x18,0x21,0x24,0x05,0x00,0x01,
-0x8c,0x62,0x10,0x00,0x00,0x00,0x00,0x00,0x14,0x45,0xff,0xfd,0x3c,0x02,0xb0,0x0a,
-0x02,0x24,0x88,0x23,0x02,0x22,0x88,0x21,0x8e,0x30,0x00,0x00,0x0c,0x00,0x13,0x99,
-0x02,0x40,0x20,0x21,0x00,0x12,0x18,0x27,0x02,0x03,0x80,0x24,0x00,0x53,0x10,0x04,
-0x02,0x02,0x80,0x25,0xae,0x30,0x00,0x00,0x24,0x03,0x00,0x01,0x8e,0x22,0x10,0x00,
-0x00,0x00,0x00,0x00,0x14,0x43,0xff,0xfd,0x00,0x00,0x00,0x00,0x8f,0xbf,0x00,0x20,
-0x7b,0xb2,0x00,0xfc,0x7b,0xb0,0x00,0xbc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x28,
-0x30,0x82,0x00,0x03,0x00,0x04,0x18,0x40,0x00,0x62,0x18,0x23,0x3c,0x04,0xb0,0x0a,
-0x00,0x64,0x18,0x21,0xac,0x66,0x00,0x00,0x24,0x04,0x00,0x01,0x8c,0x62,0x10,0x00,
-0x00,0x00,0x00,0x00,0x14,0x44,0xff,0xfd,0x00,0x00,0x00,0x00,0x08,0x00,0x13,0x87,
-0x00,0x00,0x00,0x00,0x00,0x00,0x18,0x21,0x00,0x64,0x10,0x06,0x30,0x42,0x00,0x01,
-0x14,0x40,0x00,0x05,0x00,0x00,0x00,0x00,0x24,0x63,0x00,0x01,0x2c,0x62,0x00,0x20,
-0x14,0x40,0xff,0xf9,0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,0x00,0x60,0x10,0x21,
-0x27,0xbd,0xff,0xe0,0x3c,0x03,0xb0,0x05,0xaf,0xb2,0x00,0x18,0xaf,0xb1,0x00,0x14,
-0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x1c,0x00,0x80,0x90,0x21,0x00,0xa0,0x80,0x21,
-0x00,0xc0,0x88,0x21,0x34,0x63,0x02,0x2e,0x90,0x62,0x00,0x00,0x00,0x00,0x00,0x00,
-0x30,0x42,0x00,0x01,0x14,0x40,0xff,0xfc,0x24,0x04,0x08,0x24,0x3c,0x05,0x00,0xc0,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x03,0x24,0x04,0x08,0x34,0x3c,0x05,0x00,0xc0,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x03,0x3c,0x02,0xc0,0x00,0x00,0x10,0x1c,0x00,
-0x34,0x42,0x04,0x00,0x3c,0x04,0xb0,0x05,0x3c,0x05,0xb0,0x05,0x24,0x63,0x16,0x09,
-0x02,0x22,0x10,0x21,0x34,0x84,0x04,0x20,0x34,0xa5,0x04,0x24,0x3c,0x06,0xb0,0x05,
-0xac,0x83,0x00,0x00,0x24,0x07,0x00,0x01,0xac,0xa2,0x00,0x00,0x34,0xc6,0x02,0x28,
-0x24,0x02,0x00,0x20,0xae,0x47,0x00,0x3c,0x24,0x04,0x08,0x24,0xa0,0xc2,0x00,0x00,
-0x3c,0x05,0x00,0xc0,0xa2,0x47,0x00,0x11,0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x01,
-0x24,0x04,0x08,0x34,0x3c,0x05,0x00,0xc0,0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x01,
-0x8f,0xbf,0x00,0x1c,0x8f,0xb2,0x00,0x18,0x7b,0xb0,0x00,0xbc,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x20,0x24,0x02,0x00,0x06,0xac,0x82,0x00,0x0c,0xa0,0x80,0x00,0x50,
-0xac,0x80,0x00,0x00,0xac,0x80,0x00,0x04,0xac,0x80,0x00,0x08,0xac,0x80,0x00,0x14,
-0xac,0x80,0x00,0x18,0xac,0x80,0x00,0x1c,0xa4,0x80,0x00,0x20,0xac,0x80,0x00,0x24,
-0xac,0x80,0x00,0x28,0xac,0x80,0x00,0x2c,0xa0,0x80,0x00,0x30,0xa0,0x80,0x00,0x31,
-0xac,0x80,0x00,0x34,0xac,0x80,0x00,0x38,0xa0,0x80,0x00,0x3c,0xac,0x82,0x00,0x10,
-0xa0,0x80,0x00,0x44,0xac,0x80,0x00,0x48,0x03,0xe0,0x00,0x08,0xac,0x80,0x00,0x4c,
-0x3c,0x04,0xb0,0x06,0x34,0x84,0x80,0x00,0x8c,0x83,0x00,0x00,0x3c,0x02,0x12,0x00,
-0x3c,0x05,0xb0,0x03,0x00,0x62,0x18,0x25,0x34,0xa5,0x00,0x8b,0x24,0x02,0xff,0x80,
-0xac,0x83,0x00,0x00,0x03,0xe0,0x00,0x08,0xa0,0xa2,0x00,0x00,0x3c,0x04,0xb0,0x03,
-0x34,0x84,0x00,0x0b,0x24,0x02,0x00,0x22,0x3c,0x05,0xb0,0x01,0x3c,0x06,0x45,0x67,
-0x3c,0x0a,0xb0,0x09,0xa0,0x82,0x00,0x00,0x34,0xa5,0x00,0x04,0x34,0xc6,0x89,0xaa,
-0x35,0x4a,0x00,0x04,0x24,0x02,0x01,0x23,0x3c,0x0b,0xb0,0x09,0x3c,0x07,0x01,0x23,
-0x3c,0x0c,0xb0,0x09,0x3c,0x01,0xb0,0x01,0xac,0x20,0x00,0x00,0x27,0xbd,0xff,0xe0,
-0xac,0xa0,0x00,0x00,0x35,0x6b,0x00,0x08,0x3c,0x01,0xb0,0x09,0xac,0x26,0x00,0x00,
-0x34,0xe7,0x45,0x66,0xa5,0x42,0x00,0x00,0x35,0x8c,0x00,0x0c,0x24,0x02,0xcd,0xef,
-0x3c,0x0d,0xb0,0x09,0x3c,0x08,0xcd,0xef,0x3c,0x0e,0xb0,0x09,0xad,0x67,0x00,0x00,
-0xaf,0xb7,0x00,0x1c,0xa5,0x82,0x00,0x00,0xaf,0xb6,0x00,0x18,0xaf,0xb5,0x00,0x14,
-0xaf,0xb4,0x00,0x10,0xaf,0xb3,0x00,0x0c,0xaf,0xb2,0x00,0x08,0xaf,0xb1,0x00,0x04,
-0xaf,0xb0,0x00,0x00,0x35,0xad,0x00,0x10,0x35,0x08,0x01,0x22,0x35,0xce,0x00,0x14,
-0x24,0x02,0x89,0xab,0x3c,0x0f,0xb0,0x09,0x3c,0x09,0x89,0xab,0x3c,0x10,0xb0,0x09,
-0x3c,0x11,0xb0,0x09,0x3c,0x12,0xb0,0x09,0x3c,0x13,0xb0,0x09,0x3c,0x14,0xb0,0x09,
-0x3c,0x15,0xb0,0x09,0x3c,0x16,0xb0,0x09,0x3c,0x17,0xb0,0x09,0xad,0xa8,0x00,0x00,
-0x24,0x03,0xff,0xff,0xa5,0xc2,0x00,0x00,0x35,0xef,0x00,0x18,0x35,0x29,0xcd,0xee,
-0x36,0x10,0x00,0x1c,0x36,0x31,0x00,0x20,0x36,0x52,0x00,0x24,0x36,0x73,0x00,0x28,
-0x36,0x94,0x00,0x2c,0x36,0xb5,0x00,0x30,0x36,0xd6,0x00,0x34,0x36,0xf7,0x00,0x38,
-0x24,0x02,0x45,0x67,0xad,0xe9,0x00,0x00,0xa6,0x02,0x00,0x00,0xae,0x23,0x00,0x00,
-0x8f,0xb0,0x00,0x00,0xa6,0x43,0x00,0x00,0x8f,0xb1,0x00,0x04,0xae,0x63,0x00,0x00,
-0x8f,0xb2,0x00,0x08,0xa6,0x83,0x00,0x00,0x8f,0xb3,0x00,0x0c,0xae,0xa3,0x00,0x00,
-0x8f,0xb4,0x00,0x10,0xa6,0xc3,0x00,0x00,0x8f,0xb5,0x00,0x14,0xae,0xe3,0x00,0x00,
-0x7b,0xb6,0x00,0xfc,0x3c,0x18,0xb0,0x09,0x37,0x18,0x00,0x3c,0xa7,0x03,0x00,0x00,
-0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x20,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,
-0x34,0x63,0x00,0x20,0x24,0x42,0x51,0x48,0xac,0x62,0x00,0x00,0x8c,0x83,0x00,0x34,
-0x34,0x02,0xff,0xff,0x00,0x43,0x10,0x2a,0x14,0x40,0x01,0x04,0x00,0x80,0x28,0x21,
-0x8c,0x86,0x00,0x08,0x24,0x02,0x00,0x03,0x10,0xc2,0x00,0xf7,0x00,0x00,0x00,0x00,
-0x8c,0xa2,0x00,0x2c,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x4f,0x24,0x02,0x00,0x06,
-0x3c,0x03,0xb0,0x05,0x34,0x63,0x04,0x50,0x90,0x62,0x00,0x00,0x00,0x00,0x00,0x00,
-0x30,0x42,0x00,0xff,0x14,0x40,0x00,0xdd,0xac,0xa2,0x00,0x2c,0x24,0x02,0x00,0x01,
-0x10,0xc2,0x00,0xdc,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xc2,0x00,0xca,
-0x00,0x00,0x00,0x00,0x8c,0xa7,0x00,0x04,0x24,0x02,0x00,0x02,0x10,0xe2,0x00,0xc0,
-0x00,0x00,0x00,0x00,0x8c,0xa2,0x00,0x14,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x09,
-0x24,0x02,0x00,0x01,0x3c,0x03,0xb0,0x09,0x34,0x63,0x01,0x60,0x90,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xff,0x10,0x40,0x00,0x05,0xac,0xa2,0x00,0x14,
-0x24,0x02,0x00,0x01,0xac,0xa2,0x00,0x00,0x03,0xe0,0x00,0x08,0xac,0xa0,0x00,0x14,
-0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0xd0,0x8c,0x43,0x00,0x00,0x00,0x00,0x00,0x00,
-0x04,0x61,0x00,0x19,0x3c,0x02,0xb0,0x03,0x3c,0x03,0xb0,0x05,0x34,0x63,0x02,0x2e,
-0x90,0x62,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x01,0x14,0x40,0x00,0x12,
-0x3c,0x02,0xb0,0x03,0x3c,0x02,0xb0,0x05,0x34,0x42,0x02,0x42,0x90,0x43,0x00,0x00,
-0x00,0x00,0x00,0x00,0x14,0x60,0x00,0x0c,0x3c,0x02,0xb0,0x03,0x80,0xa2,0x00,0x50,
-0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x08,0x3c,0x02,0xb0,0x03,0x14,0xc0,0x00,0x07,
-0x34,0x42,0x00,0x3f,0x24,0x02,0x00,0x0e,0x24,0x03,0x00,0x01,0xac,0xa2,0x00,0x00,
-0x03,0xe0,0x00,0x08,0xa0,0xa3,0x00,0x50,0x34,0x42,0x00,0x3f,0x90,0x44,0x00,0x00,
-0x24,0x03,0x00,0x01,0x10,0x64,0x00,0x7f,0x3c,0x03,0xb0,0x05,0x80,0xa2,0x00,0x31,
-0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x0a,0x3c,0x02,0xb0,0x06,0x34,0x42,0x80,0x18,
-0x8c,0x43,0x00,0x00,0x3c,0x04,0xf0,0x00,0x3c,0x02,0x80,0x00,0x00,0x64,0x18,0x24,
-0x10,0x62,0x00,0x03,0x24,0x02,0x00,0x09,0x03,0xe0,0x00,0x08,0xac,0xa2,0x00,0x00,
-0x8c,0xa2,0x00,0x40,0x00,0x00,0x00,0x00,0x8c,0x43,0x00,0x00,0x00,0x00,0x00,0x00,
-0x10,0x60,0x00,0x09,0x3c,0x03,0xb0,0x03,0x3c,0x02,0xb0,0x05,0x34,0x42,0x02,0x2c,
-0x8c,0x43,0x00,0x00,0x3c,0x04,0x00,0x02,0x00,0x64,0x18,0x24,0x14,0x60,0xff,0xf2,
-0x24,0x02,0x00,0x10,0x3c,0x03,0xb0,0x03,0x34,0x63,0x02,0x01,0x90,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x80,0x10,0x40,0x00,0x0e,0x00,0x00,0x00,0x00,
-0x8c,0xa3,0x00,0x0c,0x00,0x00,0x00,0x00,0xac,0xa3,0x00,0x10,0x3c,0x02,0xb0,0x03,
-0x90,0x42,0x02,0x01,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x0f,0xac,0xa2,0x00,0x0c,
-0x90,0xa3,0x00,0x0f,0x24,0x02,0x00,0x0d,0x3c,0x01,0xb0,0x03,0x08,0x00,0x14,0xb2,
-0xa0,0x23,0x02,0x01,0x3c,0x02,0xb0,0x09,0x34,0x42,0x01,0x80,0x90,0x44,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x04,0x1e,0x00,0x00,0x03,0x1e,0x03,0x10,0x60,0x00,0x15,
-0xa0,0xa4,0x00,0x44,0x24,0x02,0x00,0x01,0x10,0x62,0x00,0x0b,0x24,0x02,0x00,0x02,
-0x10,0x62,0x00,0x03,0x24,0x03,0x00,0x0d,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,
-0x8c,0xa2,0x00,0x0c,0xac,0xa3,0x00,0x00,0x24,0x03,0x00,0x04,0xac,0xa2,0x00,0x10,
-0x03,0xe0,0x00,0x08,0xac,0xa3,0x00,0x0c,0x24,0x02,0x00,0x0d,0xac,0xa2,0x00,0x00,
-0x24,0x03,0x00,0x04,0x24,0x02,0x00,0x06,0xac,0xa3,0x00,0x10,0x03,0xe0,0x00,0x08,
-0xac,0xa2,0x00,0x0c,0x8c,0xa3,0x00,0x38,0x24,0x04,0x00,0x01,0x10,0x64,0x00,0x2d,
-0x24,0x02,0x00,0x02,0x10,0x60,0x00,0x19,0x00,0x00,0x00,0x00,0x10,0x62,0x00,0x10,
-0x24,0x02,0x00,0x04,0x10,0x62,0x00,0x04,0x00,0x00,0x00,0x00,0xac,0xa0,0x00,0x38,
-0x03,0xe0,0x00,0x08,0xac,0xa0,0x00,0x00,0x10,0xe4,0x00,0x07,0x24,0x02,0x00,0x03,
-0x80,0xa2,0x00,0x30,0x00,0x00,0x00,0x00,0x00,0x02,0x18,0x0b,0xac,0xa3,0x00,0x00,
-0x03,0xe0,0x00,0x08,0xac,0xa0,0x00,0x38,0x08,0x00,0x15,0x04,0xac,0xa2,0x00,0x00,
-0x10,0xe4,0x00,0x02,0x24,0x02,0x00,0x03,0x24,0x02,0x00,0x0c,0xac,0xa2,0x00,0x00,
-0x24,0x02,0x00,0x04,0x03,0xe0,0x00,0x08,0xac,0xa2,0x00,0x38,0x10,0xe4,0x00,0x0e,
-0x3c,0x03,0xb0,0x06,0x34,0x63,0x80,0x24,0x8c,0x62,0x00,0x00,0x00,0x00,0x00,0x00,
-0x30,0x42,0x00,0xff,0x10,0x40,0x00,0x06,0xac,0xa2,0x00,0x18,0x24,0x02,0x00,0x02,
-0xac,0xa2,0x00,0x00,0xac,0xa0,0x00,0x18,0x08,0x00,0x15,0x0d,0x24,0x02,0x00,0x01,
-0x08,0x00,0x15,0x1a,0xac,0xa0,0x00,0x00,0x24,0x02,0x00,0x03,0x08,0x00,0x15,0x1a,
-0xac,0xa2,0x00,0x00,0x24,0x03,0x00,0x0b,0xac,0xa2,0x00,0x38,0x03,0xe0,0x00,0x08,
-0xac,0xa3,0x00,0x00,0x34,0x63,0x02,0x2e,0x90,0x62,0x00,0x00,0x00,0x00,0x00,0x00,
-0x30,0x42,0x00,0x01,0x14,0x40,0xff,0x7d,0x3c,0x02,0xb0,0x05,0x34,0x42,0x02,0x42,
-0x90,0x43,0x00,0x00,0x00,0x00,0x00,0x00,0x14,0x60,0xff,0x78,0x00,0x00,0x00,0x00,
-0x10,0xc0,0xff,0x81,0x24,0x02,0x00,0x0e,0x08,0x00,0x14,0xa7,0x00,0x00,0x00,0x00,
-0x80,0xa2,0x00,0x30,0x00,0x00,0x00,0x00,0x14,0x40,0xff,0x3e,0x24,0x02,0x00,0x04,
-0x08,0x00,0x14,0xb2,0x00,0x00,0x00,0x00,0x84,0xa2,0x00,0x20,0x00,0x00,0x00,0x00,
-0x10,0x40,0xff,0x75,0x24,0x02,0x00,0x06,0x3c,0x02,0xb0,0x05,0x34,0x42,0x02,0x2e,
-0x90,0x43,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x63,0x00,0x01,0x30,0x63,0x00,0xff,
-0x00,0x60,0x10,0x21,0x14,0x40,0xff,0x2b,0xa4,0xa3,0x00,0x20,0x08,0x00,0x14,0xb2,
-0x24,0x02,0x00,0x06,0x8c,0xa2,0x00,0x1c,0x00,0x00,0x00,0x00,0x14,0x40,0xff,0x66,
-0x24,0x02,0x00,0x05,0x3c,0x03,0xb0,0x05,0x34,0x63,0x02,0x2c,0x8c,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xff,0x10,0x40,0xff,0x1b,0xac,0xa2,0x00,0x1c,
-0x08,0x00,0x14,0xb2,0x24,0x02,0x00,0x05,0x3c,0x02,0xb0,0x05,0x8c,0x42,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x02,0x17,0x42,0x30,0x42,0x00,0x01,0x14,0x40,0xff,0x56,
-0x24,0x02,0x00,0x06,0x08,0x00,0x14,0x60,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x0a,
-0x03,0xe0,0x00,0x08,0xac,0x82,0x00,0x00,0x27,0xbd,0xff,0xd8,0xaf,0xb0,0x00,0x10,
-0x27,0x90,0x86,0x58,0xaf,0xbf,0x00,0x20,0xaf,0xb3,0x00,0x1c,0xaf,0xb2,0x00,0x18,
-0x0c,0x00,0x29,0xd5,0xaf,0xb1,0x00,0x14,0xaf,0x90,0x8f,0xe0,0x48,0x02,0x00,0x00,
-0x0c,0x00,0x13,0xf0,0x00,0x00,0x00,0x00,0x0c,0x00,0x18,0x1f,0x02,0x00,0x20,0x21,
-0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0x3a,0x94,0x43,0x00,0x00,0x00,0x00,0x00,0x00,
-0xa3,0x83,0x8f,0xe4,0x0c,0x00,0x00,0x34,0x00,0x00,0x00,0x00,0x0c,0x00,0x13,0xfb,
-0x00,0x00,0x00,0x00,0x27,0x84,0x84,0x98,0x0c,0x00,0x27,0x59,0x00,0x00,0x00,0x00,
-0x93,0x84,0x80,0x10,0x0c,0x00,0x21,0x3f,0x00,0x00,0x00,0x00,0x27,0x84,0x89,0x18,
-0x0c,0x00,0x06,0xe5,0x00,0x00,0x00,0x00,0x0c,0x00,0x01,0x39,0x00,0x00,0x00,0x00,
-0x27,0x84,0x84,0x40,0x0c,0x00,0x13,0xd9,0x00,0x00,0x00,0x00,0x27,0x82,0x89,0x4c,
-0xaf,0x82,0x84,0x80,0x0c,0x00,0x00,0x5f,0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,
-0x34,0x63,0x01,0x08,0x3c,0x04,0xb0,0x09,0x3c,0x05,0xb0,0x09,0x8c,0x66,0x00,0x00,
-0x34,0x84,0x01,0x68,0x34,0xa5,0x01,0x40,0x24,0x02,0xc8,0x80,0x24,0x03,0x00,0x0a,
-0xa4,0x82,0x00,0x00,0xa4,0xa3,0x00,0x00,0x3c,0x04,0xb0,0x03,0x8c,0x82,0x00,0x00,
-0x8f,0x85,0x84,0x40,0xaf,0x86,0x84,0x38,0x34,0x42,0x00,0x20,0xac,0x82,0x00,0x00,
-0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0x58,0x8c,0x43,0x00,0x00,0x2c,0xa4,0x00,0x11,
-0x34,0x63,0x01,0x00,0xac,0x43,0x00,0x00,0x10,0x80,0xff,0xfa,0x3c,0x02,0xb0,0x03,
-0x3c,0x03,0x80,0x01,0x00,0x05,0x10,0x80,0x24,0x63,0x02,0x00,0x00,0x43,0x10,0x21,
-0x8c,0x44,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x00,0x08,0x00,0x00,0x00,0x00,
-0x27,0x84,0x84,0x98,0x0c,0x00,0x26,0x8e,0x00,0x00,0x00,0x00,0x27,0x84,0x84,0x40,
-0x0c,0x00,0x14,0x52,0x00,0x00,0x00,0x00,0x93,0x83,0x81,0xf1,0x24,0x02,0x00,0x01,
-0x10,0x62,0x00,0x08,0x00,0x00,0x00,0x00,0x8f,0x85,0x84,0x40,0x8f,0x82,0x84,0x74,
-0x00,0x00,0x00,0x00,0x24,0x42,0x00,0x01,0xaf,0x82,0x84,0x74,0x08,0x00,0x15,0x9d,
-0x3c,0x02,0xb0,0x03,0x27,0x84,0x84,0x98,0x0c,0x00,0x27,0x0d,0x00,0x00,0x00,0x00,
-0x08,0x00,0x15,0xb6,0x00,0x00,0x00,0x00,0x27,0x84,0x84,0x98,0x0c,0x00,0x28,0xdd,
-0x00,0x00,0x00,0x00,0xa3,0x82,0x84,0x71,0x8f,0x82,0x84,0x74,0xaf,0x80,0x84,0x40,
-0x24,0x42,0x00,0x01,0xaf,0x82,0x84,0x74,0x08,0x00,0x15,0x9c,0x00,0x00,0x28,0x21,
-0x27,0x84,0x86,0x58,0x0c,0x00,0x19,0x5b,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xff,
-0x14,0x40,0x00,0x05,0x3c,0x03,0xb0,0x05,0xaf,0x80,0x84,0x40,0xaf,0x80,0x84,0x44,
-0x08,0x00,0x15,0xb6,0x00,0x00,0x00,0x00,0x34,0x63,0x04,0x50,0x90,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xff,0xaf,0x82,0x84,0x6c,0x14,0x40,0x00,0x20,
-0x24,0x02,0x00,0x01,0x8f,0x84,0x84,0x48,0x00,0x00,0x00,0x00,0x10,0x82,0x00,0x20,
-0x3c,0x03,0xb0,0x09,0x34,0x63,0x01,0x60,0x90,0x62,0x00,0x00,0x00,0x00,0x00,0x00,
-0x30,0x42,0x00,0xff,0xaf,0x82,0x84,0x54,0x14,0x40,0x00,0x15,0x24,0x02,0x00,0x01,
-0x24,0x02,0x00,0x02,0x10,0x82,0x00,0x07,0x00,0x00,0x00,0x00,0x24,0x05,0x00,0x03,
-0x24,0x02,0x00,0x01,0xaf,0x82,0x84,0x44,0xaf,0x85,0x84,0x40,0x08,0x00,0x15,0xb6,
-0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x05,0x34,0x42,0x02,0x2e,0x90,0x43,0x00,0x00,
-0x00,0x00,0x00,0x00,0x30,0x63,0x00,0x01,0x30,0x63,0x00,0xff,0x00,0x60,0x10,0x21,
-0xa7,0x83,0x84,0x60,0x14,0x40,0xff,0xf1,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0xaf,0x82,0x84,0x44,0xaf,0x80,0x84,0x40,0x08,0x00,0x15,0xb6,0x00,0x00,0x00,0x00,
-0x3c,0x03,0xb0,0x05,0x34,0x63,0x02,0x2c,0x8c,0x62,0x00,0x00,0x00,0x00,0x00,0x00,
-0x30,0x42,0x00,0xff,0xaf,0x82,0x84,0x5c,0x14,0x40,0xff,0xf5,0x24,0x02,0x00,0x01,
-0x08,0x00,0x15,0xe1,0x3c,0x03,0xb0,0x09,0x27,0x84,0x86,0x58,0x0c,0x00,0x1a,0xd1,
-0x00,0x00,0x00,0x00,0x83,0x82,0x84,0x70,0x00,0x00,0x00,0x00,0x14,0x40,0xff,0xec,
-0x24,0x02,0x00,0x02,0x3c,0x03,0xb0,0x05,0x34,0x63,0x04,0x50,0x90,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xff,0xaf,0x82,0x84,0x6c,0x14,0x40,0xff,0xe4,
-0x24,0x02,0x00,0x02,0x8f,0x84,0x84,0x48,0x24,0x02,0x00,0x01,0x10,0x82,0x00,0x12,
-0x24,0x02,0x00,0x02,0x10,0x82,0x00,0x04,0x00,0x00,0x00,0x00,0x24,0x05,0x00,0x04,
-0x08,0x00,0x15,0xed,0x24,0x02,0x00,0x02,0x3c,0x02,0xb0,0x05,0x34,0x42,0x02,0x2e,
-0x90,0x43,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x63,0x00,0x01,0x30,0x63,0x00,0xff,
-0x00,0x60,0x10,0x21,0xa7,0x83,0x84,0x60,0x14,0x40,0xff,0xf4,0x00,0x00,0x00,0x00,
-0x08,0x00,0x15,0xfc,0x24,0x02,0x00,0x02,0x3c,0x03,0xb0,0x05,0x34,0x63,0x02,0x2c,
-0x8c,0x62,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xff,0xaf,0x82,0x84,0x5c,
-0x14,0x40,0xff,0xf7,0x00,0x00,0x00,0x00,0x08,0x00,0x16,0x1d,0x24,0x02,0x00,0x02,
-0x27,0x84,0x89,0x18,0x0c,0x00,0x0b,0x55,0x00,0x00,0x00,0x00,0x8f,0x83,0x84,0x44,
-0xaf,0x82,0x84,0x5c,0x38,0x64,0x00,0x02,0x00,0x04,0x18,0x0a,0xaf,0x83,0x84,0x44,
-0x14,0x40,0xff,0xad,0x24,0x05,0x00,0x05,0x8f,0x82,0x89,0x58,0xaf,0x80,0x84,0x40,
-0x10,0x40,0x00,0x02,0x24,0x04,0x00,0x01,0xaf,0x84,0x84,0x48,0x93,0x82,0x89,0x66,
-0x00,0x00,0x00,0x00,0x10,0x40,0xff,0x6c,0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x05,
-0x34,0x42,0x00,0x08,0x8c,0x43,0x00,0x00,0x3c,0x04,0x20,0x00,0x00,0x64,0x18,0x24,
-0x10,0x60,0xff,0x65,0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0xa0,
-0x8c,0x43,0x00,0x00,0x3c,0x04,0x80,0x00,0xaf,0x80,0x89,0x40,0x24,0x63,0x00,0x01,
-0xac,0x43,0x00,0x00,0x3c,0x01,0xb0,0x05,0xac,0x24,0x00,0x08,0xaf,0x80,0x89,0x3c,
-0xaf,0x80,0x89,0x44,0xaf,0x80,0x89,0x48,0xaf,0x80,0x89,0x54,0xaf,0x80,0x89,0x4c,
-0x08,0x00,0x15,0xb6,0x00,0x00,0x00,0x00,0x83,0x82,0x84,0x90,0x00,0x00,0x00,0x00,
-0x10,0x40,0x00,0x02,0x24,0x02,0x00,0x20,0xaf,0x82,0x84,0x5c,0x8f,0x85,0x84,0x5c,
-0x27,0x84,0x89,0x18,0x0c,0x00,0x0d,0x30,0x00,0x00,0x00,0x00,0x00,0x02,0x1e,0x00,
-0xa3,0x82,0x84,0x70,0xaf,0x80,0x84,0x5c,0x10,0x60,0xff,0x8e,0x00,0x00,0x00,0x00,
-0x3c,0x02,0xb0,0x05,0x34,0x42,0x02,0x2e,0x90,0x43,0x00,0x00,0x00,0x00,0x00,0x00,
-0x30,0x63,0x00,0x01,0x30,0x63,0x00,0xff,0x00,0x60,0x10,0x21,0xa7,0x83,0x84,0x60,
-0x10,0x40,0x00,0x04,0x24,0x04,0x00,0x02,0xaf,0x84,0x84,0x48,0x08,0x00,0x15,0xfd,
-0x00,0x00,0x00,0x00,0x08,0x00,0x15,0xee,0x24,0x05,0x00,0x06,0x27,0x84,0x84,0x40,
-0x27,0x85,0x89,0x18,0x0c,0x00,0x0d,0xfd,0x00,0x00,0x00,0x00,0x8f,0x82,0x84,0x64,
-0xaf,0x80,0x84,0x6c,0x14,0x40,0x00,0x19,0x00,0x40,0x18,0x21,0x8f,0x82,0x84,0x68,
-0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x15,0x24,0x02,0x00,0x02,0x8f,0x83,0x84,0x48,
-0x00,0x00,0x00,0x00,0x10,0x62,0x00,0x0b,0x3c,0x02,0x40,0x00,0x8f,0x83,0x84,0x44,
-0x24,0x02,0x00,0x01,0x10,0x62,0x00,0x02,0x24,0x05,0x00,0x03,0x24,0x05,0x00,0x06,
-0xaf,0x85,0x84,0x40,0x24,0x04,0x00,0x03,0xaf,0x84,0x84,0x48,0x08,0x00,0x15,0xb6,
-0x00,0x00,0x00,0x00,0x34,0x42,0x00,0x14,0x3c,0x01,0xb0,0x05,0xac,0x22,0x00,0x00,
-0xaf,0x80,0x84,0x40,0x08,0x00,0x16,0x96,0x24,0x04,0x00,0x03,0x10,0x60,0x00,0x10,
-0x00,0x00,0x00,0x00,0x27,0x85,0x89,0x18,0x27,0x84,0x84,0x40,0x0c,0x00,0x0e,0x21,
-0x00,0x00,0x00,0x00,0x8f,0x83,0x84,0x44,0x24,0x02,0x00,0x01,0xa3,0x80,0x84,0x70,
-0xaf,0x80,0x84,0x48,0x10,0x62,0x00,0x02,0x24,0x05,0x00,0x03,0x24,0x05,0x00,0x04,
-0xaf,0x85,0x84,0x40,0xaf,0x80,0x84,0x64,0x08,0x00,0x15,0xb6,0x00,0x00,0x00,0x00,
-0x83,0x82,0x84,0x90,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x04,0x00,0x00,0x00,0x00,
-0x27,0x84,0x89,0x18,0x0c,0x00,0x10,0x69,0x00,0x00,0x00,0x00,0x8f,0x82,0x84,0x44,
-0xa3,0x80,0x84,0x70,0xaf,0x80,0x84,0x40,0xaf,0x80,0x84,0x48,0x14,0x40,0x00,0x03,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0xaf,0x82,0x84,0x44,0xaf,0x80,0x84,0x68,
-0x08,0x00,0x15,0xb6,0x00,0x00,0x00,0x00,0x27,0x84,0x84,0x40,0x27,0x85,0x89,0x18,
-0x0c,0x00,0x0e,0x21,0x00,0x00,0x00,0x00,0x8f,0x82,0x84,0x44,0xa3,0x80,0x84,0x70,
-0xaf,0x80,0x84,0x40,0xaf,0x80,0x84,0x48,0x14,0x40,0xfe,0xeb,0x00,0x00,0x00,0x00,
-0x24,0x02,0x00,0x02,0xaf,0x82,0x84,0x44,0x08,0x00,0x15,0xb6,0x00,0x00,0x00,0x00,
-0x27,0x84,0x89,0x18,0x0c,0x00,0x10,0x69,0x00,0x00,0x00,0x00,0x08,0x00,0x16,0xc6,
-0x00,0x00,0x00,0x00,0x27,0x84,0x84,0x98,0x0c,0x00,0x29,0x73,0x00,0x00,0x00,0x00,
-0x08,0x00,0x15,0xc5,0x00,0x00,0x00,0x00,0x0c,0x00,0x24,0x05,0x00,0x00,0x00,0x00,
-0x0c,0x00,0x26,0xff,0x00,0x00,0x00,0x00,0x0c,0x00,0x18,0x11,0x00,0x00,0x00,0x00,
-0x93,0x83,0xbc,0x18,0x00,0x00,0x00,0x00,0x14,0x60,0x00,0x2b,0x3c,0x02,0xb0,0x03,
-0x34,0x42,0x01,0x08,0x8c,0x44,0x00,0x00,0x8f,0x83,0xbc,0x10,0x8f,0x82,0xbc,0x14,
-0x00,0x83,0x18,0x23,0x00,0x43,0x10,0x2b,0x10,0x40,0x00,0x23,0x3c,0x02,0xb0,0x03,
-0x24,0x04,0x05,0xa0,0x34,0x42,0x01,0x18,0x8c,0x42,0x00,0x00,0x0c,0x00,0x06,0xd1,
-0x00,0x00,0x00,0x00,0x24,0x04,0x05,0xa4,0x0c,0x00,0x06,0xd1,0x00,0x02,0x84,0x02,
-0x30,0x51,0xff,0xff,0x24,0x04,0x05,0xa8,0x00,0x02,0x94,0x02,0x0c,0x00,0x06,0xd1,
-0x3a,0x10,0xff,0xff,0x3a,0x31,0xff,0xff,0x30,0x42,0xff,0xff,0x2e,0x10,0x00,0x01,
-0x2e,0x31,0x00,0x01,0x3a,0x52,0xff,0xff,0x02,0x11,0x80,0x25,0x2e,0x52,0x00,0x01,
-0x38,0x42,0xff,0xff,0x02,0x12,0x80,0x25,0x2c,0x42,0x00,0x01,0x02,0x02,0x80,0x25,
-0x16,0x00,0x00,0x02,0x24,0x04,0x00,0x02,0x00,0x00,0x20,0x21,0x0c,0x00,0x05,0x6e,
-0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,0x34,0x42,0x01,0x08,0x8c,0x43,0x00,0x00,
-0x00,0x00,0x00,0x00,0xaf,0x83,0xbc,0x10,0x0c,0x00,0x01,0xe9,0x00,0x00,0x00,0x00,
-0xaf,0x80,0x84,0x40,0xaf,0x80,0x84,0x74,0x08,0x00,0x15,0x9c,0x00,0x00,0x28,0x21,
-0x27,0x90,0xb4,0x00,0x24,0x11,0x00,0x12,0x8e,0x04,0x00,0x00,0x00,0x00,0x00,0x00,
-0x90,0x82,0x00,0x10,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x03,0x00,0x00,0x00,0x00,
-0x0c,0x00,0x18,0xd0,0x00,0x00,0x00,0x00,0x26,0x31,0xff,0xff,0x06,0x21,0xff,0xf6,
-0x26,0x10,0x00,0x04,0xaf,0x80,0x84,0x40,0x08,0x00,0x15,0xb7,0x00,0x00,0x28,0x21,
-0x3c,0x02,0xb0,0x03,0x34,0x42,0x01,0x08,0x8c,0x44,0x00,0x00,0x8f,0x82,0x84,0x38,
-0x00,0x04,0x19,0xc2,0x00,0x02,0x11,0xc2,0x10,0x62,0xff,0xf6,0x00,0x00,0x00,0x00,
-0x3c,0x02,0xb0,0x03,0x34,0x42,0x01,0x02,0x90,0x43,0x00,0x00,0x3c,0x12,0xb0,0x05,
-0xaf,0x84,0x84,0x38,0x30,0x63,0x00,0xff,0x00,0x03,0x11,0x40,0x00,0x43,0x10,0x23,
-0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,0x00,0x02,0x99,0x00,0x00,0x00,0x88,0x21,
-0x36,0x52,0x02,0x2c,0x27,0x90,0xb4,0x00,0x8e,0x04,0x00,0x00,0x00,0x00,0x00,0x00,
-0x90,0x83,0x00,0x16,0x00,0x00,0x00,0x00,0x30,0x62,0x00,0x03,0x10,0x40,0x00,0x06,
-0x30,0x62,0x00,0x1c,0x14,0x40,0x00,0x04,0x00,0x00,0x00,0x00,0x8f,0x85,0x84,0x38,
-0x0c,0x00,0x1e,0x94,0x02,0x60,0x30,0x21,0x8e,0x42,0x00,0x00,0x00,0x00,0x00,0x00,
-0x30,0x42,0x00,0xff,0x14,0x40,0xff,0xd7,0x00,0x00,0x00,0x00,0x26,0x31,0x00,0x01,
-0x2a,0x22,0x00,0x13,0x14,0x40,0xff,0xec,0x26,0x10,0x00,0x04,0x08,0x00,0x17,0x21,
-0x00,0x00,0x00,0x00,0x8f,0x84,0x84,0x4c,0x27,0x85,0x89,0x18,0x0c,0x00,0x17,0xa4,
-0x00,0x00,0x00,0x00,0x8f,0x83,0x84,0x4c,0x24,0x02,0x00,0x04,0x14,0x62,0xfe,0xa5,
-0x00,0x00,0x00,0x00,0x08,0x00,0x15,0xee,0x24,0x05,0x00,0x05,0x3c,0x02,0xb0,0x03,
-0x34,0x42,0x00,0x3f,0x90,0x44,0x00,0x00,0x24,0x03,0x00,0x01,0x10,0x64,0x00,0x08,
-0x00,0x00,0x00,0x00,0x27,0x84,0x89,0x18,0x0c,0x00,0x24,0x2c,0x00,0x00,0x00,0x00,
-0x24,0x05,0x00,0x05,0xaf,0x85,0x84,0x40,0x08,0x00,0x15,0xb7,0x00,0x00,0x00,0x00,
-0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0x14,0x8c,0x44,0x00,0x00,0x0c,0x00,0x24,0x49,
-0x00,0x00,0x00,0x00,0x08,0x00,0x17,0x65,0x24,0x05,0x00,0x05,0x8f,0x82,0x89,0x4c,
-0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x0d,0x00,0x00,0x00,0x00,0x8f,0x84,0xb4,0x40,
-0xaf,0x80,0x89,0x4c,0x94,0x85,0x00,0x14,0x0c,0x00,0x1b,0x66,0x00,0x00,0x00,0x00,
-0x93,0x82,0x8b,0x71,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x02,0x10,0x40,0x00,0x03,
-0x00,0x00,0x00,0x00,0x0c,0x00,0x01,0x57,0x00,0x00,0x20,0x21,0x8f,0x84,0xb4,0x40,
-0x0c,0x00,0x18,0xd0,0x00,0x00,0x00,0x00,0x08,0x00,0x17,0x21,0x00,0x00,0x00,0x00,
-0x3c,0x02,0xff,0x90,0x27,0xbd,0xff,0xe8,0x00,0x80,0x18,0x21,0x34,0x42,0x00,0x01,
-0x27,0x84,0x89,0x18,0x10,0x62,0x00,0x05,0xaf,0xbf,0x00,0x10,0x8f,0xbf,0x00,0x10,
-0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x18,0x0c,0x00,0x06,0xe5,
-0x00,0x00,0x00,0x00,0x27,0x84,0x86,0x58,0x0c,0x00,0x18,0x1f,0x00,0x00,0x00,0x00,
-0x27,0x84,0x84,0x40,0x0c,0x00,0x13,0xd9,0x00,0x00,0x00,0x00,0x08,0x00,0x17,0x8b,
-0x00,0x00,0x00,0x00,0x8f,0x82,0x89,0x58,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x05,
-0x00,0x00,0x18,0x21,0x8f,0x82,0x84,0x48,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x02,
-0x00,0x00,0x00,0x00,0x24,0x03,0x00,0x01,0x03,0xe0,0x00,0x08,0x00,0x60,0x10,0x21,
-0x27,0xbd,0xff,0xe0,0x3c,0x06,0xb0,0x03,0xaf,0xb1,0x00,0x14,0xaf,0xb0,0x00,0x10,
-0x34,0xc6,0x00,0x5f,0xaf,0xbf,0x00,0x18,0x90,0xc3,0x00,0x00,0x3c,0x07,0xb0,0x03,
-0x34,0xe7,0x00,0x5d,0x34,0x63,0x00,0x01,0x3c,0x09,0xb0,0x03,0x24,0x02,0x00,0x01,
-0xa0,0xc3,0x00,0x00,0x00,0x80,0x80,0x21,0xa0,0xe2,0x00,0x00,0x00,0xa0,0x88,0x21,
-0x35,0x29,0x00,0x5e,0x00,0xe0,0x40,0x21,0x24,0x04,0x00,0x01,0x91,0x22,0x00,0x00,
-0x91,0x03,0x00,0x00,0x30,0x42,0x00,0x01,0x14,0x83,0x00,0x03,0x30,0x42,0x00,0x01,
-0x14,0x40,0xff,0xfa,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x04,0x12,0x02,0x00,0x2c,
-0x24,0x05,0x0f,0x00,0x24,0x02,0x00,0x06,0x12,0x02,0x00,0x08,0x24,0x05,0x00,0x0f,
-0x3c,0x02,0xb0,0x03,0x34,0x42,0x02,0x00,0xa0,0x50,0x00,0x00,0x8f,0xbf,0x00,0x18,
-0x7b,0xb0,0x00,0xbc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x20,0x24,0x04,0x0c,0x04,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x0f,0x24,0x04,0x0d,0x04,0x24,0x05,0x00,0x0f,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x0f,0x24,0x04,0x08,0x80,0x24,0x05,0x1e,0x00,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x0f,0x24,0x04,0x08,0x8c,0x24,0x05,0x0f,0x00,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x0f,0x24,0x04,0x08,0x24,0x3c,0x05,0x00,0x30,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x02,0x24,0x04,0x08,0x2c,0x3c,0x05,0x00,0x30,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x02,0x24,0x04,0x08,0x34,0x3c,0x05,0x00,0x30,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x02,0x24,0x04,0x08,0x3c,0x3c,0x05,0x00,0x30,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x02,0x08,0x00,0x17,0xc5,0x3c,0x02,0xb0,0x03,
-0x24,0x04,0x08,0x8c,0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x04,0x24,0x04,0x08,0x80,
-0x24,0x05,0x1e,0x00,0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x04,0x24,0x04,0x0c,0x04,
-0x24,0x05,0x00,0x0f,0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x04,0x24,0x04,0x0d,0x04,
-0x24,0x05,0x00,0x0f,0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x04,0x24,0x04,0x08,0x24,
-0x3c,0x05,0x00,0x30,0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x03,0x24,0x04,0x08,0x2c,
-0x3c,0x05,0x00,0x30,0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x03,0x24,0x04,0x08,0x34,
-0x3c,0x05,0x00,0x30,0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x02,0x3c,0x05,0x00,0x30,
-0x24,0x06,0x00,0x03,0x0c,0x00,0x13,0x5f,0x24,0x04,0x08,0x3c,0x02,0x20,0x20,0x21,
-0x24,0x05,0x00,0x14,0x0c,0x00,0x13,0xa4,0x24,0x06,0x01,0x07,0x08,0x00,0x17,0xc5,
-0x3c,0x02,0xb0,0x03,0x3c,0x03,0xb0,0x03,0x34,0x63,0x00,0x73,0x90,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x02,0x14,0x40,0x00,0x04,0x00,0x00,0x00,0x00,
-0xa3,0x80,0x81,0x58,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0xa3,0x82,0x81,0x58,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,
-0x3c,0x02,0x80,0x00,0x00,0x80,0x70,0x21,0x34,0x63,0x00,0x20,0x24,0x42,0x60,0x7c,
-0x3c,0x04,0xb0,0x03,0xac,0x62,0x00,0x00,0x34,0x84,0x00,0x30,0xad,0xc0,0x02,0xb8,
-0x8c,0x83,0x00,0x00,0x24,0x02,0x00,0xff,0xa5,0xc0,0x00,0x0a,0x00,0x00,0x30,0x21,
-0xa7,0x82,0x8f,0xf0,0x27,0x88,0x90,0x00,0xa5,0xc3,0x00,0x08,0x3c,0x07,0xb0,0x08,
-0x30,0xc2,0xff,0xff,0x00,0x02,0x20,0xc0,0x24,0xc3,0x00,0x01,0x00,0x82,0x10,0x21,
-0x00,0x60,0x30,0x21,0x00,0x02,0x10,0x80,0x30,0x63,0xff,0xff,0x00,0x48,0x10,0x21,
-0x00,0x87,0x20,0x21,0x28,0xc5,0x00,0xff,0xac,0x83,0x00,0x00,0x14,0xa0,0xff,0xf4,
-0xa4,0x43,0x00,0x00,0x3c,0x02,0xb0,0x08,0x34,0x03,0xff,0xff,0x25,0xc4,0x00,0x0c,
-0x24,0x0a,0x00,0x02,0x34,0x42,0x07,0xf8,0x3c,0x06,0xb0,0x03,0xa7,0x83,0xb3,0xdc,
-0xac,0x43,0x00,0x00,0xaf,0x84,0xb4,0x00,0x34,0xc6,0x00,0x64,0xa0,0x8a,0x00,0x18,
-0x94,0xc5,0x00,0x00,0x8f,0x82,0xb4,0x00,0x25,0xc4,0x00,0x30,0x24,0x08,0x00,0x03,
-0x3c,0x03,0xb0,0x03,0xa0,0x45,0x00,0x21,0x34,0x63,0x00,0x66,0xaf,0x84,0xb4,0x04,
-0xa0,0x88,0x00,0x18,0x94,0x65,0x00,0x00,0x8f,0x82,0xb4,0x04,0x25,0xc4,0x00,0x54,
-0x25,0xc7,0x00,0x78,0xa0,0x45,0x00,0x21,0xaf,0x84,0xb4,0x08,0xa0,0x88,0x00,0x18,
-0x94,0x65,0x00,0x00,0x8f,0x82,0xb4,0x08,0x25,0xc8,0x00,0x9c,0x24,0x09,0x00,0x01,
-0xa0,0x45,0x00,0x21,0xaf,0x87,0xb4,0x0c,0xa0,0xea,0x00,0x18,0x94,0xc4,0x00,0x00,
-0x8f,0x82,0xb4,0x0c,0x3c,0x03,0xb0,0x03,0x34,0x63,0x00,0x62,0xa0,0x44,0x00,0x21,
-0xaf,0x88,0xb4,0x10,0xa1,0x09,0x00,0x18,0x94,0x65,0x00,0x00,0x8f,0x82,0xb4,0x10,
-0x25,0xc4,0x00,0xc0,0x3c,0x06,0xb0,0x03,0xa0,0x45,0x00,0x21,0xaf,0x84,0xb4,0x14,
-0xa0,0x89,0x00,0x18,0x94,0x65,0x00,0x00,0x8f,0x82,0xb4,0x14,0x25,0xc4,0x00,0xe4,
-0x34,0xc6,0x00,0x60,0xa0,0x45,0x00,0x21,0xaf,0x84,0xb4,0x18,0xa0,0x80,0x00,0x18,
-0x94,0xc5,0x00,0x00,0x8f,0x82,0xb4,0x18,0x25,0xc3,0x01,0x08,0x25,0xc7,0x01,0x2c,
-0xa0,0x45,0x00,0x21,0xaf,0x83,0xb4,0x1c,0xa0,0x60,0x00,0x18,0x94,0xc8,0x00,0x00,
-0x8f,0x82,0xb4,0x1c,0x25,0xc4,0x01,0x50,0x25,0xc5,0x01,0x74,0xa0,0x48,0x00,0x21,
-0x25,0xc6,0x01,0x98,0x25,0xc9,0x01,0xbc,0x25,0xca,0x01,0xe0,0x25,0xcb,0x02,0x04,
-0x25,0xcc,0x02,0x28,0x25,0xcd,0x02,0x4c,0x24,0x02,0x00,0x10,0x3c,0x03,0xb0,0x03,
-0xaf,0x87,0xb4,0x20,0x34,0x63,0x00,0x38,0xa0,0xe0,0x00,0x18,0xaf,0x84,0xb4,0x24,
-0xa0,0x80,0x00,0x18,0xaf,0x85,0xb4,0x28,0xa0,0xa0,0x00,0x18,0xaf,0x86,0xb4,0x2c,
-0xa0,0xc0,0x00,0x18,0xaf,0x89,0xb4,0x30,0xa1,0x20,0x00,0x18,0xaf,0x8a,0xb4,0x34,
-0xa1,0x40,0x00,0x18,0xaf,0x8b,0xb4,0x38,0xa1,0x60,0x00,0x18,0xaf,0x8c,0xb4,0x3c,
-0xa1,0x80,0x00,0x18,0xaf,0x8d,0xb4,0x40,0xa1,0xa2,0x00,0x18,0x94,0x64,0x00,0x00,
-0x8f,0x82,0xb4,0x40,0x25,0xc5,0x02,0x70,0x3c,0x03,0xb0,0x03,0xa0,0x44,0x00,0x21,
-0x24,0x02,0x00,0x11,0xaf,0x85,0xb4,0x44,0x34,0x63,0x00,0x6e,0xa0,0xa2,0x00,0x18,
-0x94,0x64,0x00,0x00,0x8f,0x82,0xb4,0x44,0x25,0xc5,0x02,0x94,0x3c,0x03,0xb0,0x03,
-0xa0,0x44,0x00,0x21,0x24,0x02,0x00,0x12,0xaf,0x85,0xb4,0x48,0x34,0x63,0x00,0x6c,
-0xa0,0xa2,0x00,0x18,0x94,0x64,0x00,0x00,0x8f,0x82,0xb4,0x48,0x24,0x05,0xff,0xff,
-0x24,0x07,0x00,0x01,0xa0,0x44,0x00,0x21,0x24,0x06,0x00,0x12,0x27,0x84,0xb4,0x00,
-0x8c,0x82,0x00,0x00,0x24,0xc6,0xff,0xff,0xa0,0x40,0x00,0x04,0x8c,0x83,0x00,0x00,
-0xa4,0x45,0x00,0x00,0xa4,0x45,0x00,0x02,0xa0,0x60,0x00,0x0a,0x8c,0x82,0x00,0x00,
-0xa4,0x65,0x00,0x06,0xa4,0x65,0x00,0x08,0xa0,0x40,0x00,0x10,0x8c,0x83,0x00,0x00,
-0xa4,0x45,0x00,0x0c,0xa4,0x45,0x00,0x0e,0xa0,0x60,0x00,0x12,0x8c,0x82,0x00,0x00,
-0x00,0x00,0x00,0x00,0xa0,0x40,0x00,0x16,0x8c,0x83,0x00,0x00,0xa4,0x45,0x00,0x14,
-0xa0,0x67,0x00,0x17,0x8c,0x82,0x00,0x00,0x24,0x84,0x00,0x04,0xa0,0x40,0x00,0x20,
-0x04,0xc1,0xff,0xe7,0xac,0x40,0x00,0x1c,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,
-0x3c,0x02,0xb0,0x03,0x3c,0x03,0x80,0x00,0x34,0x42,0x00,0x20,0x24,0x63,0x63,0x40,
-0xac,0x43,0x00,0x00,0x90,0x82,0x00,0x10,0x00,0x80,0x60,0x21,0x10,0x40,0x00,0x56,
-0x00,0x00,0x70,0x21,0x97,0x82,0x8f,0xf0,0x94,0x8a,0x00,0x0c,0x27,0x87,0x90,0x00,
-0x00,0x02,0x40,0xc0,0x01,0x02,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x47,0x10,0x21,
-0x90,0x8b,0x00,0x18,0xa4,0x4a,0x00,0x00,0x94,0x83,0x00,0x0e,0x39,0x64,0x00,0x10,
-0x2c,0x84,0x00,0x01,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x34,0x85,0x00,0x02,
-0x39,0x63,0x00,0x11,0x00,0x83,0x28,0x0b,0x34,0xa3,0x00,0x08,0x39,0x64,0x00,0x12,
-0x00,0x02,0x10,0x80,0x00,0xa4,0x18,0x0b,0x00,0x47,0x10,0x21,0x94,0x49,0x00,0x04,
-0x34,0x64,0x00,0x20,0x00,0x6b,0x20,0x0b,0x34,0x83,0x00,0x40,0x39,0x62,0x00,0x01,
-0x00,0x82,0x18,0x0b,0x00,0x09,0x30,0xc0,0x34,0x64,0x00,0x80,0x00,0xc9,0x28,0x21,
-0x39,0x62,0x00,0x02,0x00,0x60,0x68,0x21,0x00,0x82,0x68,0x0a,0x00,0x05,0x28,0x80,
-0x3c,0x02,0xb0,0x08,0x00,0xa7,0x28,0x21,0x00,0xc2,0x30,0x21,0x01,0x02,0x40,0x21,
-0x34,0x03,0xff,0xff,0x35,0xa4,0x01,0x00,0x39,0x62,0x00,0x03,0x2d,0x67,0x00,0x13,
-0xad,0x0a,0x00,0x00,0xa4,0xa3,0x00,0x00,0xac,0xc3,0x00,0x00,0xa7,0x89,0x8f,0xf0,
-0x10,0xe0,0x00,0x0f,0x00,0x82,0x68,0x0a,0x3c,0x03,0x80,0x01,0x00,0x0b,0x10,0x80,
-0x24,0x63,0x02,0x44,0x00,0x43,0x10,0x21,0x8c,0x44,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x80,0x00,0x08,0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,0x34,0x63,0x00,0x60,
-0x94,0x62,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x02,0x14,0x00,0x00,0x02,0x74,0x03,
-0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0x3a,0x94,0x44,0x00,0x00,0x93,0x83,0x8f,0xe4,
-0x91,0x82,0x00,0x21,0x01,0xc4,0x20,0x21,0x91,0x85,0x00,0x10,0x00,0x04,0x24,0x00,
-0x00,0x62,0x18,0x21,0x00,0x04,0x74,0x03,0x00,0x6e,0x18,0x23,0x00,0x65,0x10,0x2a,
-0x00,0xa2,0x18,0x0a,0x00,0x0d,0x24,0x00,0x3c,0x02,0xb0,0x06,0x24,0x05,0xff,0xff,
-0x00,0x64,0x18,0x25,0x34,0x42,0x80,0x20,0xac,0x43,0x00,0x00,0xa5,0x85,0x00,0x0e,
-0xa1,0x80,0x00,0x10,0xa5,0x85,0x00,0x0c,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,
-0x3c,0x03,0xb0,0x03,0x08,0x00,0x19,0x14,0x34,0x63,0x00,0x62,0x3c,0x03,0xb0,0x03,
-0x08,0x00,0x19,0x14,0x34,0x63,0x00,0x64,0x3c,0x03,0xb0,0x03,0x08,0x00,0x19,0x14,
-0x34,0x63,0x00,0x66,0x3c,0x03,0xb0,0x03,0x08,0x00,0x19,0x14,0x34,0x63,0x00,0x38,
-0x3c,0x03,0xb0,0x03,0x08,0x00,0x19,0x14,0x34,0x63,0x00,0x6e,0x3c,0x03,0xb0,0x03,
-0x08,0x00,0x19,0x14,0x34,0x63,0x00,0x6c,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,
-0x34,0x63,0x00,0x20,0x24,0x42,0x65,0x08,0x00,0x05,0x28,0x40,0xac,0x62,0x00,0x00,
-0x00,0xa6,0x28,0x21,0x2c,0xe2,0x00,0x10,0x14,0x80,0x00,0x06,0x00,0x00,0x18,0x21,
-0x10,0x40,0x00,0x02,0x00,0x00,0x00,0x00,0x00,0xe0,0x18,0x21,0x03,0xe0,0x00,0x08,
-0x00,0x60,0x10,0x21,0x24,0x02,0x00,0x20,0x10,0xe2,0x00,0x06,0x2c,0xe4,0x00,0x10,
-0x24,0xa2,0x00,0x01,0x10,0x80,0xff,0xf9,0x00,0x02,0x11,0x00,0x08,0x00,0x19,0x4f,
-0x00,0x47,0x18,0x21,0x08,0x00,0x19,0x4f,0x24,0xa3,0x00,0x50,0x27,0xbd,0xff,0xc8,
-0xaf,0xb3,0x00,0x1c,0xaf,0xb2,0x00,0x18,0xaf,0xb1,0x00,0x14,0xaf,0xbf,0x00,0x30,
-0xaf,0xb7,0x00,0x2c,0xaf,0xb6,0x00,0x28,0xaf,0xb5,0x00,0x24,0xaf,0xb4,0x00,0x20,
-0xaf,0xb0,0x00,0x10,0x00,0x80,0x88,0x21,0x84,0x84,0x00,0x08,0x3c,0x05,0xb0,0x03,
-0x3c,0x02,0x80,0x00,0x34,0xa5,0x00,0x20,0x24,0x42,0x65,0x6c,0x3c,0x03,0xb0,0x06,
-0x00,0x04,0x20,0x80,0xac,0xa2,0x00,0x00,0x00,0x83,0x20,0x21,0x3c,0x06,0xb0,0x06,
-0x8c,0x82,0x00,0x00,0x34,0xc6,0x80,0x24,0x8c,0x88,0x00,0x00,0x8c,0xc4,0x00,0x00,
-0x96,0x25,0x00,0x08,0x30,0x52,0xff,0xff,0x00,0x08,0x44,0x02,0x34,0x84,0x01,0x00,
-0x3c,0x02,0xb0,0x00,0x00,0x08,0x18,0xc0,0x00,0x12,0x3a,0x00,0xac,0xc4,0x00,0x00,
-0x00,0xe2,0x38,0x21,0xae,0x32,0x02,0xb8,0x00,0x68,0x18,0x21,0x24,0xa5,0x00,0x02,
-0x8c,0xf6,0x00,0x00,0x30,0xa5,0x01,0xff,0x8c,0xf4,0x00,0x04,0x27,0x86,0x90,0x00,
-0x00,0x03,0x18,0x80,0x00,0x12,0x98,0xc0,0xa6,0x25,0x00,0x08,0x00,0x66,0x18,0x21,
-0x02,0x72,0x10,0x21,0x94,0x65,0x00,0x00,0x00,0x02,0x48,0x80,0x01,0x26,0x30,0x21,
-0x24,0x02,0xff,0xff,0x00,0x14,0x1a,0x02,0x27,0x84,0x90,0x10,0xa4,0xc2,0x00,0x02,
-0x30,0x63,0x00,0x1f,0x24,0x02,0x00,0x10,0x01,0x24,0x20,0x21,0xa4,0xc8,0x00,0x04,
-0x8c,0xf0,0x00,0x08,0xa6,0x23,0x00,0x06,0xa6,0x25,0x00,0x0a,0xa0,0x82,0x00,0x06,
-0x86,0x25,0x00,0x06,0x27,0x82,0x90,0x04,0x01,0x22,0x10,0x21,0x24,0x03,0x00,0x13,
-0x10,0xa3,0x00,0xee,0xac,0x47,0x00,0x18,0x3c,0x03,0xb0,0x03,0x34,0x63,0x01,0x00,
-0xa6,0x20,0x00,0x02,0x3c,0x02,0xb0,0x03,0x90,0x64,0x00,0x00,0x34,0x42,0x01,0x08,
-0x8c,0x45,0x00,0x00,0x00,0x10,0x1b,0xc2,0x00,0x04,0x20,0x82,0x30,0x63,0x00,0x01,
-0xac,0xc5,0x00,0x08,0x10,0x60,0x00,0xc7,0x30,0x97,0x00,0x01,0x00,0x10,0x16,0x82,
-0x30,0x46,0x00,0x01,0x00,0x10,0x12,0x02,0x00,0x10,0x19,0xc2,0x00,0x10,0x26,0x02,
-0x00,0x10,0x2e,0x42,0x30,0x48,0x00,0x7f,0x24,0x02,0x00,0x01,0x30,0x75,0x00,0x01,
-0x30,0x84,0x00,0x01,0x10,0xc2,0x00,0xb3,0x30,0xa3,0x00,0x01,0x00,0x60,0x28,0x21,
-0x0c,0x00,0x19,0x42,0x01,0x00,0x38,0x21,0x02,0x72,0x18,0x21,0x00,0x03,0x18,0x80,
-0x2c,0x46,0x00,0x54,0x27,0x85,0x90,0x10,0x27,0x84,0x90,0x08,0x00,0x06,0x10,0x0a,
-0x00,0x65,0x28,0x21,0x26,0xa6,0x00,0x02,0x00,0x64,0x18,0x21,0xa0,0xa2,0x00,0x02,
-0xa0,0x66,0x00,0x06,0xa0,0x62,0x00,0x07,0xa0,0xa2,0x00,0x01,0x02,0x72,0x28,0x21,
-0x00,0x05,0x28,0x80,0x27,0x82,0x90,0x04,0x00,0xa2,0x58,0x21,0x8d,0x64,0x00,0x18,
-0x00,0x10,0x15,0xc2,0x30,0x42,0x00,0x01,0x8c,0x83,0x00,0x0c,0x27,0x84,0x90,0x20,
-0x00,0xa4,0x48,0x21,0xa6,0x22,0x00,0x00,0xa6,0x36,0x00,0x04,0x8d,0x26,0x00,0x00,
-0x00,0x03,0x19,0x42,0x3c,0x02,0xff,0xef,0x34,0x42,0xff,0xff,0x30,0x63,0x00,0x01,
-0x00,0xc2,0x40,0x24,0x00,0x03,0x1d,0x00,0x01,0x03,0x40,0x25,0x00,0x08,0x15,0x02,
-0x00,0x14,0x19,0x82,0x00,0x14,0x25,0x82,0x00,0x10,0x34,0x42,0x00,0x10,0x3c,0x82,
-0x00,0x10,0x2c,0x02,0x30,0x42,0x00,0x01,0x30,0xcd,0x00,0x01,0x30,0x6c,0x00,0x01,
-0x30,0xe6,0x00,0x01,0x30,0x8a,0x00,0x03,0x32,0x94,0x00,0x07,0x30,0xa5,0x00,0x01,
-0xad,0x28,0x00,0x00,0x10,0x40,0x00,0x0b,0x32,0x07,0x00,0x7f,0x8d,0x64,0x00,0x18,
-0x3c,0x03,0xff,0xf0,0x34,0x63,0xff,0xff,0x8c,0x82,0x00,0x0c,0x01,0x03,0x18,0x24,
-0x00,0x02,0x13,0x82,0x30,0x42,0x00,0x0f,0x00,0x02,0x14,0x00,0x00,0x62,0x18,0x25,
-0xad,0x23,0x00,0x00,0x24,0x02,0x00,0x01,0x10,0xc2,0x00,0x6a,0x00,0x00,0x00,0x00,
-0x15,0x80,0x00,0x03,0x00,0x00,0x00,0x00,0x15,0x40,0x00,0x5b,0x24,0x02,0x00,0x01,
-0x96,0x22,0x00,0x04,0x00,0x00,0x00,0x00,0x24,0x42,0x00,0x04,0xa6,0x22,0x00,0x04,
-0x00,0xa0,0x20,0x21,0x0c,0x00,0x19,0x42,0x01,0xa0,0x28,0x21,0x02,0x72,0x18,0x21,
-0x00,0x03,0x40,0x80,0x2c,0x45,0x00,0x54,0x27,0x84,0x90,0x10,0x01,0x04,0x20,0x21,
-0x00,0x05,0x10,0x0a,0xa0,0x82,0x00,0x00,0xa0,0x80,0x00,0x04,0xa0,0x80,0x00,0x05,
-0x96,0x23,0x00,0x04,0x27,0x82,0x90,0x00,0x01,0x02,0x10,0x21,0xa4,0x43,0x00,0x06,
-0x27,0x82,0x90,0x04,0x92,0x26,0x00,0x01,0x01,0x02,0x10,0x21,0x8c,0x45,0x00,0x18,
-0x27,0x83,0x90,0x20,0x01,0x03,0x18,0x21,0xa0,0x60,0x00,0x00,0xa0,0x86,0x00,0x07,
-0x94,0xa2,0x00,0x10,0x24,0x03,0x00,0x04,0x30,0x42,0x00,0x0f,0x10,0x43,0x00,0x36,
-0x24,0xa5,0x00,0x10,0x94,0xa3,0x00,0x16,0x27,0x87,0x90,0x18,0x01,0x07,0x10,0x21,
-0xa4,0x43,0x00,0x02,0x94,0xa2,0x00,0x04,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x01,
-0x14,0x40,0x00,0x24,0x02,0x72,0x20,0x21,0x94,0xa2,0x00,0x00,0x24,0x03,0x00,0xa4,
-0x30,0x42,0x00,0xff,0x10,0x43,0x00,0x1f,0x00,0x00,0x00,0x00,0x94,0xa2,0x00,0x00,
-0x24,0x03,0x00,0x88,0x30,0x42,0x00,0x88,0x10,0x43,0x00,0x14,0x02,0x72,0x18,0x21,
-0x27,0x84,0x90,0x20,0x00,0x03,0x18,0x80,0x00,0x64,0x18,0x21,0x8c,0x62,0x00,0x00,
-0x3c,0x04,0x00,0x80,0x00,0x44,0x10,0x25,0xac,0x62,0x00,0x00,0x02,0x72,0x10,0x21,
-0x00,0x02,0x10,0x80,0x00,0x47,0x10,0x21,0xa0,0x54,0x00,0x00,0x8f,0xbf,0x00,0x30,
-0x7b,0xb6,0x01,0x7c,0x7b,0xb4,0x01,0x3c,0x7b,0xb2,0x00,0xfc,0x7b,0xb0,0x00,0xbc,
-0x24,0x02,0x00,0x01,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x38,0x94,0xa2,0x00,0x18,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x60,0x10,0x40,0xff,0xe9,0x02,0x72,0x18,0x21,
-0x02,0x72,0x20,0x21,0x27,0x82,0x90,0x20,0x00,0x04,0x20,0x80,0x00,0x82,0x20,0x21,
-0x8c,0x83,0x00,0x00,0x3c,0x02,0xff,0x7f,0x34,0x42,0xff,0xff,0x00,0x62,0x18,0x24,
-0x08,0x00,0x1a,0x37,0xac,0x83,0x00,0x00,0x27,0x87,0x90,0x18,0x01,0x07,0x10,0x21,
-0x08,0x00,0x1a,0x21,0xa4,0x40,0x00,0x02,0x11,0x42,0x00,0x07,0x00,0x00,0x00,0x00,
-0x2d,0x42,0x00,0x02,0x14,0x40,0xff,0xa7,0x00,0xa0,0x20,0x21,0x96,0x22,0x00,0x04,
-0x08,0x00,0x19,0xff,0x24,0x42,0x00,0x0c,0x96,0x22,0x00,0x04,0x08,0x00,0x19,0xff,
-0x24,0x42,0x00,0x08,0x16,0xe6,0xff,0x96,0x3c,0x02,0xff,0xfb,0x8d,0x63,0x00,0x18,
-0x34,0x42,0xff,0xff,0x02,0x02,0x10,0x24,0xac,0x62,0x00,0x08,0x08,0x00,0x19,0xf8,
-0x00,0x00,0x30,0x21,0x16,0xe6,0xff,0x4e,0x00,0x60,0x28,0x21,0x3c,0x02,0xfb,0xff,
-0x34,0x42,0xff,0xff,0x02,0x02,0x10,0x24,0xac,0xe2,0x00,0x08,0x08,0x00,0x19,0xb7,
-0x00,0x00,0x30,0x21,0x93,0x87,0xbb,0x14,0x00,0x10,0x1e,0x42,0x00,0x10,0x26,0x82,
-0x27,0x82,0x90,0x08,0x2c,0xe5,0x00,0x0c,0x01,0x22,0x48,0x21,0x30,0x63,0x00,0x01,
-0x30,0x86,0x00,0x01,0x14,0xa0,0x00,0x06,0x00,0xe0,0x40,0x21,0x00,0x03,0x10,0x40,
-0x00,0x46,0x10,0x21,0x00,0x02,0x11,0x00,0x00,0xe2,0x10,0x21,0x24,0x48,0x00,0x04,
-0x02,0x72,0x10,0x21,0x00,0x02,0x10,0x80,0x27,0x84,0x90,0x10,0x27,0x83,0x90,0x08,
-0x00,0x44,0x20,0x21,0x00,0x43,0x10,0x21,0xa1,0x28,0x00,0x07,0xa0,0x40,0x00,0x06,
-0xa0,0x80,0x00,0x02,0x08,0x00,0x19,0xc7,0xa0,0x80,0x00,0x01,0x24,0x02,0x00,0x01,
-0xa6,0x22,0x00,0x02,0x0c,0x00,0x01,0xc2,0x00,0xe0,0x20,0x21,0x08,0x00,0x1a,0x3b,
-0x00,0x00,0x00,0x00,0x30,0xa7,0xff,0xff,0x00,0x07,0x18,0xc0,0x00,0x67,0x18,0x21,
-0x3c,0x06,0xb0,0x03,0x3c,0x02,0x80,0x00,0x24,0x42,0x6a,0x44,0x27,0x85,0x90,0x10,
-0x00,0x03,0x18,0x80,0x34,0xc6,0x00,0x20,0x00,0x65,0x18,0x21,0xac,0xc2,0x00,0x00,
-0x80,0x62,0x00,0x07,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x29,0x00,0x80,0x28,0x21,
-0x90,0x82,0x00,0x16,0x00,0x00,0x00,0x00,0x34,0x42,0x00,0x02,0x30,0x43,0x00,0x01,
-0x14,0x60,0x00,0x02,0xa0,0x82,0x00,0x16,0xa0,0x80,0x00,0x17,0x90,0xa2,0x00,0x04,
-0x3c,0x03,0xb0,0x03,0x27,0x86,0x90,0x00,0x14,0x40,0x00,0x06,0x34,0x63,0x00,0x20,
-0x24,0x02,0x00,0x01,0xa0,0xa2,0x00,0x04,0xa4,0xa7,0x00,0x02,0x03,0xe0,0x00,0x08,
-0xa4,0xa7,0x00,0x00,0x94,0xa4,0x00,0x02,0x3c,0x02,0x80,0x01,0x24,0x42,0x82,0x6c,
-0xac,0x62,0x00,0x00,0x00,0x04,0x18,0xc0,0x00,0x64,0x18,0x21,0x00,0x03,0x18,0x80,
-0x00,0x66,0x18,0x21,0x94,0x62,0x00,0x04,0xa4,0x67,0x00,0x02,0x3c,0x03,0xb0,0x08,
-0x00,0x02,0x20,0xc0,0x00,0x82,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x46,0x10,0x21,
-0x00,0x83,0x20,0x21,0xa4,0x47,0x00,0x00,0xac,0x87,0x00,0x00,0x90,0xa2,0x00,0x04,
-0xa4,0xa7,0x00,0x02,0x24,0x42,0x00,0x01,0x03,0xe0,0x00,0x08,0xa0,0xa2,0x00,0x04,
-0x90,0x82,0x00,0x16,0x24,0x85,0x00,0x06,0x34,0x42,0x00,0x01,0x30,0x43,0x00,0x02,
-0x14,0x60,0xff,0xda,0xa0,0x82,0x00,0x16,0x24,0x02,0x00,0x01,0x08,0x00,0x1a,0xa7,
-0xa0,0x82,0x00,0x17,0x27,0xbd,0xff,0xe8,0xaf,0xbf,0x00,0x10,0x00,0x80,0x38,0x21,
-0x84,0x84,0x00,0x02,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x3c,0x0a,0xb0,0x06,
-0x34,0x63,0x00,0x20,0x24,0x42,0x6b,0x44,0x3c,0x0b,0xb0,0x08,0x27,0x89,0x90,0x00,
-0x34,0x0c,0xff,0xff,0x35,0x4a,0x80,0x20,0x10,0x80,0x00,0x30,0xac,0x62,0x00,0x00,
-0x97,0x82,0x8f,0xf0,0x94,0xe6,0x02,0xba,0x00,0x02,0x18,0xc0,0x00,0x6b,0x28,0x21,
-0xac,0xa6,0x00,0x00,0x8c,0xe4,0x02,0xb8,0x00,0x62,0x18,0x21,0x00,0x03,0x18,0x80,
-0x00,0x04,0x10,0xc0,0x00,0x44,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x49,0x10,0x21,
-0x94,0x48,0x00,0x04,0x00,0x69,0x18,0x21,0xa4,0x66,0x00,0x00,0x00,0x08,0x28,0xc0,
-0x00,0xab,0x10,0x21,0xac,0x4c,0x00,0x00,0x8c,0xe4,0x02,0xb8,0x27,0x82,0x90,0x04,
-0x00,0xa8,0x28,0x21,0x00,0x04,0x18,0xc0,0x00,0x64,0x18,0x21,0x00,0x03,0x18,0x80,
-0x00,0x62,0x10,0x21,0x8c,0x46,0x00,0x18,0x27,0x84,0x90,0x10,0x00,0x64,0x18,0x21,
-0x8c,0xc2,0x00,0x00,0x80,0x67,0x00,0x06,0x00,0x05,0x28,0x80,0x30,0x42,0xff,0xff,
-0x00,0x47,0x10,0x21,0x30,0x43,0x00,0xff,0x00,0x03,0x18,0x2b,0x00,0x02,0x12,0x02,
-0x00,0x43,0x10,0x21,0x3c,0x04,0x00,0x04,0x00,0xa9,0x28,0x21,0x00,0x44,0x10,0x25,
-0xa4,0xac,0x00,0x00,0xad,0x42,0x00,0x00,0xa7,0x88,0x8f,0xf0,0x8f,0xbf,0x00,0x10,
-0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x18,0x84,0xe3,0x00,0x06,
-0x27,0x82,0xb4,0x00,0x94,0xe5,0x02,0xba,0x00,0x03,0x18,0x80,0x00,0x62,0x18,0x21,
-0x8c,0x64,0x00,0x00,0x0c,0x00,0x1a,0x91,0x00,0x00,0x00,0x00,0x08,0x00,0x1b,0x0b,
-0x00,0x00,0x00,0x00,0x94,0x88,0x00,0x00,0x00,0x80,0x58,0x21,0x27,0x8a,0x90,0x00,
-0x00,0x08,0x18,0xc0,0x00,0x68,0x18,0x21,0x3c,0x04,0xb0,0x03,0x00,0x03,0x18,0x80,
-0x3c,0x02,0x80,0x00,0x00,0x6a,0x18,0x21,0x34,0x84,0x00,0x20,0x24,0x42,0x6c,0x64,
-0x30,0xa5,0xff,0xff,0xac,0x82,0x00,0x00,0x94,0x67,0x00,0x02,0x11,0x05,0x00,0x35,
-0x24,0x04,0x00,0x01,0x91,0x66,0x00,0x04,0x00,0x00,0x00,0x00,0x00,0x86,0x10,0x2a,
-0x10,0x40,0x00,0x10,0x00,0xc0,0x48,0x21,0x3c,0x0d,0xb0,0x03,0x01,0x40,0x60,0x21,
-0x35,0xad,0x00,0x20,0x10,0xe5,0x00,0x0d,0x24,0x84,0x00,0x01,0x00,0x07,0x10,0xc0,
-0x00,0x47,0x10,0x21,0x00,0x02,0x10,0x80,0x01,0x20,0x30,0x21,0x00,0x4a,0x10,0x21,
-0x00,0x86,0x18,0x2a,0x00,0xe0,0x40,0x21,0x94,0x47,0x00,0x02,0x14,0x60,0xff,0xf5,
-0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,0x00,0x00,0x10,0x21,0x00,0x08,0x20,0xc0,
-0x00,0x88,0x20,0x21,0x24,0xc2,0xff,0xff,0x00,0x04,0x20,0x80,0xa1,0x62,0x00,0x04,
-0x00,0x8c,0x20,0x21,0x94,0x83,0x00,0x04,0x00,0x07,0x10,0xc0,0x00,0x47,0x10,0x21,
-0x00,0x02,0x10,0x80,0x00,0x4c,0x10,0x21,0x00,0x03,0x28,0xc0,0x94,0x46,0x00,0x02,
-0x00,0xa3,0x18,0x21,0x00,0x03,0x18,0x80,0x00,0x6c,0x18,0x21,0xa4,0x66,0x00,0x00,
-0xa4,0x86,0x00,0x02,0x95,0x64,0x00,0x02,0x3c,0x03,0xb0,0x08,0x3c,0x02,0x80,0x01,
-0x00,0xa3,0x28,0x21,0x24,0x42,0x82,0x6c,0xad,0xa2,0x00,0x00,0x10,0x87,0x00,0x03,
-0xac,0xa6,0x00,0x00,0x03,0xe0,0x00,0x08,0x24,0x02,0x00,0x01,0x08,0x00,0x1b,0x59,
-0xa5,0x68,0x00,0x02,0x91,0x62,0x00,0x04,0xa5,0x67,0x00,0x00,0x24,0x42,0xff,0xff,
-0x30,0x43,0x00,0xff,0x14,0x60,0xff,0xf7,0xa1,0x62,0x00,0x04,0x24,0x02,0xff,0xff,
-0x08,0x00,0x1b,0x59,0xa5,0x62,0x00,0x02,0x00,0x05,0x40,0xc0,0x01,0x05,0x30,0x21,
-0x27,0xbd,0xff,0xd8,0x00,0x06,0x30,0x80,0x27,0x82,0x90,0x04,0xaf,0xb2,0x00,0x18,
-0xaf,0xb1,0x00,0x14,0xaf,0xbf,0x00,0x20,0xaf,0xb3,0x00,0x1c,0xaf,0xb0,0x00,0x10,
-0x00,0xc2,0x10,0x21,0x8c,0x47,0x00,0x18,0x00,0xa0,0x90,0x21,0x3c,0x02,0x80,0x00,
-0x3c,0x05,0xb0,0x03,0x34,0xa5,0x00,0x20,0x24,0x42,0x6d,0x98,0xac,0xa2,0x00,0x00,
-0x27,0x83,0x90,0x10,0x00,0xc3,0x30,0x21,0x8c,0xe2,0x00,0x00,0x80,0xc5,0x00,0x06,
-0x00,0x80,0x88,0x21,0x30,0x42,0xff,0xff,0x00,0x45,0x10,0x21,0x30,0x43,0x00,0xff,
-0x10,0x60,0x00,0x02,0x00,0x02,0x12,0x02,0x24,0x42,0x00,0x01,0x30,0x53,0x00,0xff,
-0x01,0x12,0x10,0x21,0x00,0x02,0x10,0x80,0x27,0x83,0x90,0x10,0x00,0x43,0x10,0x21,
-0x80,0x44,0x00,0x07,0x00,0x00,0x00,0x00,0x10,0x80,0x00,0x4b,0x26,0x24,0x00,0x06,
-0x32,0x50,0xff,0xff,0x02,0x20,0x20,0x21,0x0c,0x00,0x1b,0x19,0x02,0x00,0x28,0x21,
-0x92,0x22,0x00,0x10,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x2e,0x3c,0x03,0xb0,0x08,
-0x3c,0x09,0x80,0x01,0x27,0x88,0x90,0x00,0xa6,0x32,0x00,0x0c,0x00,0x10,0x20,0xc0,
-0x00,0x90,0x20,0x21,0x00,0x04,0x20,0x80,0x00,0x88,0x20,0x21,0x94,0x82,0x00,0x04,
-0x3c,0x03,0xb0,0x08,0x3c,0x07,0xb0,0x03,0x00,0x02,0x28,0xc0,0x00,0xa2,0x10,0x21,
-0x00,0x02,0x10,0x80,0x00,0x48,0x10,0x21,0x00,0xa3,0x28,0x21,0x25,0x26,0x82,0x6c,
-0x34,0x03,0xff,0xff,0x34,0xe7,0x00,0x20,0xac,0xe6,0x00,0x00,0xa4,0x83,0x00,0x02,
-0xa4,0x43,0x00,0x00,0xac,0xa3,0x00,0x00,0x92,0x22,0x00,0x10,0x92,0x23,0x00,0x0a,
-0xa6,0x32,0x00,0x0e,0x02,0x62,0x10,0x21,0x14,0x60,0x00,0x05,0xa2,0x22,0x00,0x10,
-0x92,0x22,0x00,0x16,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xfe,0xa2,0x22,0x00,0x16,
-0x92,0x22,0x00,0x04,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x05,0x00,0x00,0x00,0x00,
-0x92,0x22,0x00,0x16,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xfd,0xa2,0x22,0x00,0x16,
-0x8f,0xbf,0x00,0x20,0x7b,0xb2,0x00,0xfc,0x7b,0xb0,0x00,0xbc,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x28,0x96,0x22,0x00,0x0e,0x27,0x88,0x90,0x00,0x00,0x02,0x20,0xc0,
-0x00,0x82,0x20,0x21,0x00,0x04,0x20,0x80,0x00,0x88,0x20,0x21,0x94,0x82,0x00,0x04,
-0x3c,0x06,0xb0,0x03,0x3c,0x09,0x80,0x01,0x00,0x02,0x28,0xc0,0x00,0xa2,0x10,0x21,
-0x00,0x02,0x10,0x80,0x00,0xa3,0x28,0x21,0x00,0x48,0x10,0x21,0x34,0xc6,0x00,0x20,
-0x25,0x23,0x82,0x6c,0xac,0xc3,0x00,0x00,0xa4,0x50,0x00,0x00,0xac,0xb0,0x00,0x00,
-0x08,0x00,0x1b,0x97,0xa4,0x90,0x00,0x02,0x08,0x00,0x1b,0x8e,0x32,0x50,0xff,0xff,
-0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x24,0x42,0x6f,0x60,0x34,0x63,0x00,0x20,
-0xac,0x62,0x00,0x00,0x90,0x82,0x00,0x04,0x97,0xaa,0x00,0x12,0x00,0x80,0x60,0x21,
-0x30,0xa8,0xff,0xff,0x00,0x4a,0x20,0x23,0x34,0x09,0xff,0xff,0x30,0xcf,0xff,0xff,
-0x30,0xee,0xff,0xff,0x11,0x09,0x00,0x73,0xa1,0x84,0x00,0x04,0x00,0x0e,0xc0,0xc0,
-0x00,0x08,0x10,0xc0,0x00,0x48,0x10,0x21,0x03,0x0e,0x20,0x21,0x27,0x8d,0x90,0x00,
-0x00,0x04,0x20,0x80,0x00,0x02,0x10,0x80,0x00,0x4d,0x10,0x21,0x00,0x8d,0x20,0x21,
-0x94,0x86,0x00,0x02,0x94,0x43,0x00,0x04,0x3c,0x19,0x80,0x01,0xa4,0x46,0x00,0x02,
-0x00,0x03,0x28,0xc0,0x00,0xa3,0x18,0x21,0x94,0x87,0x00,0x02,0x3c,0x02,0xb0,0x08,
-0x00,0x03,0x18,0x80,0x00,0xa2,0x28,0x21,0x00,0x6d,0x18,0x21,0x27,0x22,0x82,0x6c,
-0x3c,0x01,0xb0,0x03,0xac,0x22,0x00,0x20,0xa4,0x66,0x00,0x00,0x10,0xe9,0x00,0x57,
-0xac,0xa6,0x00,0x00,0x01,0xe0,0x30,0x21,0x11,0x40,0x00,0x1d,0x00,0x00,0x48,0x21,
-0x01,0x40,0x38,0x21,0x27,0x8b,0x90,0x04,0x27,0x8a,0x90,0x10,0x00,0x06,0x40,0xc0,
-0x01,0x06,0x18,0x21,0x00,0x03,0x18,0x80,0x00,0x6b,0x10,0x21,0x8c,0x44,0x00,0x18,
-0x00,0x6a,0x18,0x21,0x80,0x65,0x00,0x06,0x8c,0x82,0x00,0x00,0x00,0x00,0x00,0x00,
-0x30,0x42,0xff,0xff,0x00,0x45,0x10,0x21,0x30,0x44,0x00,0xff,0x00,0x02,0x12,0x02,
-0x01,0x22,0x18,0x21,0x24,0x62,0x00,0x01,0x14,0x80,0x00,0x02,0x30,0x49,0x00,0xff,
-0x30,0x69,0x00,0xff,0x01,0x06,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x4d,0x10,0x21,
-0x24,0xe7,0xff,0xff,0x94,0x46,0x00,0x02,0x14,0xe0,0xff,0xe9,0x00,0x06,0x40,0xc0,
-0x91,0x82,0x00,0x10,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x20,0x3c,0x06,0xb0,0x03,
-0xa5,0x8f,0x00,0x0c,0x03,0x0e,0x20,0x21,0x00,0x04,0x20,0x80,0x00,0x8d,0x20,0x21,
-0x94,0x82,0x00,0x04,0x3c,0x03,0xb0,0x08,0x3c,0x07,0xb0,0x03,0x00,0x02,0x28,0xc0,
-0x00,0xa2,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x4d,0x10,0x21,0x00,0xa3,0x28,0x21,
-0x27,0x26,0x82,0x6c,0x34,0x03,0xff,0xff,0x34,0xe7,0x00,0x20,0xac,0xe6,0x00,0x00,
-0xa4,0x83,0x00,0x02,0xa4,0x43,0x00,0x00,0xac,0xa3,0x00,0x00,0x91,0x82,0x00,0x10,
-0x91,0x83,0x00,0x04,0xa5,0x8e,0x00,0x0e,0x01,0x22,0x10,0x21,0x14,0x60,0x00,0x05,
-0xa1,0x82,0x00,0x10,0x91,0x82,0x00,0x16,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xfd,
-0xa1,0x82,0x00,0x16,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x95,0x82,0x00,0x0e,
-0x3c,0x03,0xb0,0x08,0x00,0x02,0x20,0xc0,0x00,0x82,0x20,0x21,0x00,0x04,0x20,0x80,
-0x00,0x8d,0x20,0x21,0x94,0x82,0x00,0x04,0x34,0xc6,0x00,0x20,0x27,0x27,0x82,0x6c,
-0x00,0x02,0x28,0xc0,0x00,0xa2,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0xa3,0x28,0x21,
-0x00,0x4d,0x10,0x21,0xac,0xc7,0x00,0x00,0xa4,0x8f,0x00,0x02,0xa4,0x4f,0x00,0x00,
-0xac,0xaf,0x00,0x00,0x08,0x00,0x1c,0x26,0x03,0x0e,0x20,0x21,0x08,0x00,0x1c,0x01,
-0xa5,0x88,0x00,0x02,0x00,0x0e,0xc0,0xc0,0x03,0x0e,0x10,0x21,0x00,0x02,0x10,0x80,
-0x27,0x8d,0x90,0x00,0x00,0x4d,0x10,0x21,0x94,0x43,0x00,0x02,0x30,0x84,0x00,0xff,
-0x14,0x80,0x00,0x05,0xa5,0x83,0x00,0x00,0x24,0x02,0xff,0xff,0x3c,0x19,0x80,0x01,
-0x08,0x00,0x1c,0x01,0xa5,0x82,0x00,0x02,0x08,0x00,0x1c,0x01,0x3c,0x19,0x80,0x01,
-0x3c,0x08,0xb0,0x03,0x3c,0x02,0x80,0x00,0x27,0xbd,0xff,0x78,0x35,0x08,0x00,0x20,
-0x24,0x42,0x71,0xa0,0xaf,0xb2,0x00,0x68,0xaf,0xb1,0x00,0x64,0xaf,0xb0,0x00,0x60,
-0xad,0x02,0x00,0x00,0xaf,0xbf,0x00,0x84,0xaf,0xbe,0x00,0x80,0xaf,0xb7,0x00,0x7c,
-0xaf,0xb6,0x00,0x78,0xaf,0xb5,0x00,0x74,0xaf,0xb4,0x00,0x70,0xaf,0xb3,0x00,0x6c,
-0xaf,0xa4,0x00,0x88,0x90,0x83,0x00,0x0a,0x27,0x82,0xb4,0x00,0xaf,0xa6,0x00,0x90,
-0x00,0x03,0x18,0x80,0x00,0x62,0x18,0x21,0x8c,0x63,0x00,0x00,0xaf,0xa7,0x00,0x94,
-0x27,0x86,0x90,0x04,0xaf,0xa3,0x00,0x1c,0x94,0x63,0x00,0x14,0x30,0xb1,0xff,0xff,
-0x24,0x08,0x00,0x01,0x00,0x03,0x20,0xc0,0xaf,0xa3,0x00,0x18,0x00,0x83,0x18,0x21,
-0xaf,0xa4,0x00,0x54,0x00,0x03,0x18,0x80,0x27,0x84,0x90,0x10,0x00,0x64,0x20,0x21,
-0x80,0x82,0x00,0x06,0x00,0x66,0x18,0x21,0x8c,0x66,0x00,0x18,0x24,0x42,0x00,0x02,
-0x00,0x02,0x1f,0xc2,0x8c,0xc4,0x00,0x08,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x43,
-0x00,0x02,0x10,0x40,0x00,0x04,0x2f,0xc2,0x00,0x04,0x1c,0x82,0x00,0xc2,0x38,0x21,
-0x00,0x04,0x24,0x42,0x8f,0xa2,0x00,0x1c,0x30,0x63,0x00,0x01,0x30,0x84,0x00,0x01,
-0xaf,0xa5,0x00,0x3c,0xaf,0xa3,0x00,0x34,0xaf,0xa4,0x00,0x38,0xaf,0xa0,0x00,0x40,
-0xaf,0xa0,0x00,0x44,0xaf,0xa0,0x00,0x50,0xaf,0xa8,0x00,0x20,0x80,0x42,0x00,0x12,
-0x8f,0xb2,0x00,0x18,0xaf,0xa2,0x00,0x28,0x8c,0xd0,0x00,0x0c,0x14,0xa0,0x01,0xe4,
-0x00,0x60,0x30,0x21,0x00,0x10,0x10,0x82,0x30,0x45,0x00,0x07,0x10,0xa0,0x00,0x11,
-0xaf,0xa0,0x00,0x30,0x8f,0xa4,0x00,0x98,0x27,0x82,0x80,0x1c,0x00,0x04,0x18,0x40,
-0x00,0x62,0x18,0x21,0x24,0xa2,0x00,0x06,0x8f,0xa5,0x00,0x20,0x94,0x64,0x00,0x00,
-0x00,0x45,0x10,0x04,0x00,0x44,0x00,0x1a,0x14,0x80,0x00,0x02,0x00,0x00,0x00,0x00,
-0x00,0x07,0x00,0x0d,0x00,0x00,0x10,0x12,0x24,0x42,0x00,0x20,0x30,0x42,0xff,0xfc,
-0xaf,0xa2,0x00,0x30,0x8f,0xa3,0x00,0x18,0x8f,0xa4,0x00,0x28,0x34,0x02,0xff,0xff,
-0xaf,0xa0,0x00,0x2c,0xaf,0xa2,0x00,0x48,0xaf,0xa3,0x00,0x4c,0x00,0x60,0xf0,0x21,
-0x00,0x00,0xb8,0x21,0x18,0x80,0x00,0x48,0xaf,0xa0,0x00,0x24,0x00,0x11,0x89,0x02,
-0xaf,0xb1,0x00,0x58,0x00,0x80,0xa8,0x21,0x00,0x12,0x10,0xc0,0x00,0x52,0x18,0x21,
-0x00,0x03,0x80,0x80,0x27,0x85,0x90,0x00,0x02,0x40,0x20,0x21,0x00,0x40,0xa0,0x21,
-0x02,0x05,0x10,0x21,0x94,0x56,0x00,0x02,0x0c,0x00,0x12,0x8b,0x00,0x00,0x28,0x21,
-0x90,0x42,0x00,0x00,0x24,0x03,0x00,0x08,0x30,0x42,0x00,0x0c,0x10,0x43,0x01,0x9e,
-0x24,0x04,0x00,0x01,0x24,0x02,0x00,0x01,0x10,0x82,0x01,0x7c,0x3c,0x02,0xb0,0x03,
-0x8f,0xa6,0x00,0x88,0x34,0x42,0x01,0x04,0x84,0xc5,0x00,0x0c,0x02,0x92,0x18,0x21,
-0x94,0x46,0x00,0x00,0x00,0x05,0x20,0xc0,0x00,0x85,0x20,0x21,0x00,0x03,0x18,0x80,
-0x27,0x82,0x90,0x10,0x27,0x85,0x90,0x08,0x00,0x65,0x28,0x21,0x00,0x62,0x18,0x21,
-0x80,0x71,0x00,0x05,0x80,0x73,0x00,0x04,0x8f,0xa3,0x00,0x88,0x30,0xd0,0xff,0xff,
-0x00,0x10,0x3a,0x03,0x32,0x08,0x00,0xff,0x27,0x82,0x90,0x20,0x00,0x04,0x20,0x80,
-0x80,0xa6,0x00,0x06,0x00,0x82,0x20,0x21,0xa4,0x67,0x00,0x44,0xa4,0x68,0x00,0x46,
-0x8c,0x84,0x00,0x00,0x38,0xc6,0x00,0x00,0x01,0x00,0x80,0x21,0x00,0x04,0x15,0x02,
-0x30,0x42,0x00,0x01,0x10,0x40,0x00,0x03,0x00,0xe6,0x80,0x0a,0x00,0x04,0x14,0x02,
-0x30,0x50,0x00,0x0f,0x12,0x20,0x01,0x50,0x02,0x40,0x20,0x21,0x02,0x71,0x10,0x21,
-0x00,0x50,0x10,0x2a,0x14,0x40,0x00,0xed,0x02,0x92,0x10,0x21,0x93,0x82,0x8b,0x71,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x01,0x14,0x40,0x00,0xe0,0x02,0x92,0x28,0x21,
-0x26,0xe2,0x00,0x01,0x30,0x57,0xff,0xff,0x02,0x40,0xf0,0x21,0x26,0xb5,0xff,0xff,
-0x16,0xa0,0xff,0xbd,0x02,0xc0,0x90,0x21,0x16,0xe0,0x00,0xd0,0x00,0x00,0x00,0x00,
-0x8f,0xa3,0x00,0x98,0x00,0x00,0x00,0x00,0x2c,0x62,0x00,0x10,0x10,0x40,0x00,0x2e,
-0x00,0x00,0x00,0x00,0x8f,0xa4,0x00,0x24,0x00,0x00,0x00,0x00,0x18,0x80,0x00,0x2a,
-0x24,0x03,0x00,0x01,0x8f,0xa5,0x00,0x1c,0x27,0x84,0x90,0x04,0x94,0xb2,0x00,0x14,
-0xa0,0xa3,0x00,0x12,0x8f,0xa6,0x00,0x3c,0x00,0x12,0x10,0xc0,0x00,0x52,0x10,0x21,
-0x00,0x02,0x80,0x80,0x27,0x82,0x90,0x10,0x02,0x02,0x10,0x21,0x80,0x43,0x00,0x06,
-0x02,0x04,0x20,0x21,0x8c,0x85,0x00,0x18,0x24,0x63,0x00,0x02,0x00,0x03,0x17,0xc2,
-0x00,0x62,0x18,0x21,0x00,0x03,0x18,0x43,0x00,0x03,0x18,0x40,0x14,0xc0,0x00,0x0e,
-0x00,0xa3,0x38,0x21,0x27,0x82,0x90,0x00,0x02,0x02,0x10,0x21,0x94,0x43,0x00,0x06,
-0x8f,0xa8,0x00,0x1c,0x24,0x02,0x00,0x01,0xa5,0x03,0x00,0x1a,0x7b,0xbe,0x04,0x3c,
-0x7b,0xb6,0x03,0xfc,0x7b,0xb4,0x03,0xbc,0x7b,0xb2,0x03,0x7c,0x7b,0xb0,0x03,0x3c,
-0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x88,0x8f,0xa4,0x00,0x98,0x8f,0xa5,0x00,0x38,
-0x8f,0xa6,0x00,0x34,0xaf,0xa0,0x00,0x10,0x0c,0x00,0x09,0x0a,0xaf,0xa0,0x00,0x14,
-0x08,0x00,0x1d,0x2d,0x00,0x00,0x00,0x00,0x8f,0xa3,0x00,0x44,0x93,0x82,0x81,0x58,
-0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x61,0x30,0x69,0x00,0x03,0x8f,0xa4,0x00,0x24,
-0x8f,0xa5,0x00,0x28,0x00,0x00,0x00,0x00,0x00,0x85,0x10,0x2a,0x10,0x40,0x00,0x8f,
-0x00,0x00,0x00,0x00,0x8f,0xa6,0x00,0x1c,0x00,0x00,0x00,0x00,0x90,0xc4,0x00,0x04,
-0x00,0x00,0x00,0x00,0x30,0x83,0x00,0xff,0x00,0xa3,0x10,0x2a,0x10,0x40,0x00,0x87,
-0x00,0x00,0x00,0x00,0x8f,0xa8,0x00,0x24,0x00,0x00,0x00,0x00,0x11,0x00,0x00,0x83,
-0x00,0x65,0x10,0x23,0x00,0xa8,0x18,0x23,0x00,0x62,0x10,0x2a,0x14,0x40,0x00,0x7d,
-0x30,0x63,0x00,0xff,0x00,0x85,0x10,0x23,0x30,0x42,0x00,0xff,0xaf,0xa2,0x00,0x50,
-0x8f,0xa2,0x00,0x50,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x73,0x00,0x00,0xa8,0x21,
-0x27,0x8c,0x90,0x00,0x3c,0x0b,0x80,0xff,0x24,0x10,0x00,0x04,0x27,0x91,0x90,0x04,
-0x35,0x6b,0xff,0xff,0x3c,0x0d,0x7f,0x00,0x27,0x8e,0x90,0x10,0x01,0x80,0x78,0x21,
-0x00,0x12,0x30,0xc0,0x00,0xd2,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x4c,0x10,0x21,
-0x94,0x42,0x00,0x06,0x8f,0xa3,0x00,0x2c,0x8f,0xa4,0x00,0x30,0xaf,0xa2,0x00,0x44,
-0x8f,0xa5,0x00,0x44,0x30,0x49,0x00,0x03,0x02,0x09,0x10,0x23,0x30,0x42,0x00,0x03,
-0x00,0xa2,0x10,0x21,0x8f,0xa8,0x00,0x30,0x24,0x42,0x00,0x04,0x30,0x42,0xff,0xff,
-0x00,0x64,0x38,0x21,0x01,0x02,0x28,0x23,0x00,0x62,0x18,0x21,0x00,0x48,0x10,0x2b,
-0x10,0x40,0x00,0x52,0x00,0x00,0x20,0x21,0x30,0xe7,0xff,0xff,0x30,0xa4,0xff,0xff,
-0xaf,0xa7,0x00,0x2c,0x00,0xd2,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x51,0x18,0x21,
-0x8c,0x65,0x00,0x18,0x00,0x04,0x25,0x40,0x00,0x8d,0x20,0x24,0x8c,0xa8,0x00,0x04,
-0x00,0x4e,0x18,0x21,0x00,0x4f,0x50,0x21,0x01,0x0b,0x40,0x24,0x01,0x04,0x40,0x25,
-0xac,0xa8,0x00,0x04,0x8f,0xa4,0x00,0x98,0x8f,0xa2,0x00,0x50,0x26,0xb5,0x00,0x01,
-0xa0,0x64,0x00,0x00,0x8c,0xa4,0x00,0x08,0x00,0x00,0x00,0x00,0x04,0x81,0x00,0x0c,
-0x02,0xa2,0x30,0x2a,0x80,0x62,0x00,0x06,0x00,0x00,0x00,0x00,0x24,0x42,0x00,0x02,
-0x00,0x02,0x1f,0xc2,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x43,0x00,0x02,0x10,0x40,
-0x00,0xa2,0x38,0x21,0x8f,0xa5,0x00,0x40,0x00,0x00,0x00,0x00,0xa4,0xe5,0x00,0x00,
-0x95,0x52,0x00,0x02,0x14,0xc0,0xff,0xc7,0x00,0x12,0x30,0xc0,0x8f,0xa4,0x00,0x24,
-0x8f,0xa5,0x00,0x50,0x8f,0xa6,0x00,0x1c,0x8f,0xa3,0x00,0x2c,0x00,0x85,0x80,0x21,
-0xa0,0xd0,0x00,0x12,0x00,0x09,0x10,0x23,0x30,0x42,0x00,0x03,0x8f,0xa8,0x00,0x88,
-0x00,0x62,0x10,0x23,0xa4,0xc2,0x00,0x1a,0x85,0x03,0x00,0x0c,0x00,0x00,0x00,0x00,
-0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,0x27,0x83,0x90,0x04,
-0x00,0x43,0x10,0x21,0x8c,0x44,0x00,0x18,0x00,0x00,0x00,0x00,0x8c,0x83,0x00,0x04,
-0x00,0x00,0x00,0x00,0x30,0x63,0x00,0x10,0x14,0x60,0xff,0x74,0x02,0x00,0x10,0x21,
-0x8f,0xa3,0x00,0x54,0x8f,0xa4,0x00,0x18,0x8f,0xa5,0x00,0x24,0x00,0x64,0x10,0x21,
-0x00,0x02,0x10,0x80,0x27,0x83,0x90,0x18,0x00,0x43,0x10,0x21,0x90,0x44,0x00,0x00,
-0x10,0xa0,0x00,0x03,0x00,0x00,0x30,0x21,0x08,0x00,0x1d,0x33,0x02,0x00,0x10,0x21,
-0x93,0x82,0x80,0x10,0x00,0x00,0x28,0x21,0x00,0x00,0x38,0x21,0x0c,0x00,0x21,0x9a,
-0xaf,0xa2,0x00,0x10,0x08,0x00,0x1d,0x33,0x02,0x00,0x10,0x21,0x30,0x63,0xff,0xff,
-0x08,0x00,0x1d,0x85,0xaf,0xa3,0x00,0x2c,0x8f,0xa8,0x00,0x44,0x08,0x00,0x1d,0xa7,
-0x31,0x09,0x00,0x03,0x08,0x00,0x1d,0x60,0xaf,0xa3,0x00,0x50,0x8f,0xa6,0x00,0x44,
-0xaf,0xa0,0x00,0x50,0x08,0x00,0x1d,0xa7,0x30,0xc9,0x00,0x03,0x8f,0xa5,0x00,0x48,
-0x8f,0xa6,0x00,0x4c,0x8f,0xa4,0x00,0x1c,0x03,0xc0,0x38,0x21,0x0c,0x00,0x1b,0xd8,
-0xaf,0xb7,0x00,0x10,0x08,0x00,0x1d,0x10,0x00,0x00,0x00,0x00,0x00,0x05,0x28,0x80,
-0x27,0x82,0x90,0x00,0x00,0xa2,0x28,0x21,0x00,0x00,0x20,0x21,0x0c,0x00,0x01,0x49,
-0x00,0x00,0x00,0x00,0x08,0x00,0x1d,0x09,0x26,0xe2,0x00,0x01,0x00,0x02,0x80,0x80,
-0x27,0x83,0x90,0x10,0x8f,0xa4,0x00,0x1c,0x02,0x03,0x18,0x21,0x26,0x31,0x00,0x01,
-0x02,0x40,0x28,0x21,0x0c,0x00,0x1e,0xea,0xa0,0x71,0x00,0x05,0x14,0x40,0xff,0x13,
-0x00,0x00,0x00,0x00,0x16,0xe0,0x00,0x4d,0x03,0xc0,0x38,0x21,0x8f,0xa4,0x00,0x24,
-0x8f,0xa5,0x00,0x20,0x24,0x02,0x00,0x01,0x24,0x84,0x00,0x01,0xaf,0xb2,0x00,0x48,
-0xaf,0xb6,0x00,0x4c,0x02,0xc0,0xf0,0x21,0x10,0xa2,0x00,0x41,0xaf,0xa4,0x00,0x24,
-0x27,0x82,0x90,0x00,0x02,0x02,0x10,0x21,0x94,0x42,0x00,0x06,0x8f,0xa4,0x00,0x30,
-0xaf,0xa0,0x00,0x20,0xaf,0xa2,0x00,0x44,0x30,0x49,0x00,0x03,0x8f,0xa8,0x00,0x44,
-0x00,0x09,0x10,0x23,0x30,0x42,0x00,0x03,0x01,0x02,0x10,0x21,0x24,0x42,0x00,0x04,
-0x30,0x42,0xff,0xff,0x00,0x44,0x18,0x2b,0x10,0x60,0x00,0x2b,0x00,0x00,0x00,0x00,
-0x8f,0xa5,0x00,0x2c,0x00,0x82,0x10,0x23,0x00,0xa4,0x18,0x21,0x30,0x63,0xff,0xff,
-0x30,0x44,0xff,0xff,0xaf,0xa3,0x00,0x2c,0x02,0x92,0x28,0x21,0x00,0x05,0x28,0x80,
-0x27,0x82,0x90,0x04,0x00,0xa2,0x10,0x21,0x8c,0x46,0x00,0x18,0x3c,0x03,0x80,0xff,
-0x3c,0x02,0x7f,0x00,0x8c,0xc8,0x00,0x04,0x00,0x04,0x25,0x40,0x34,0x63,0xff,0xff,
-0x00,0x82,0x20,0x24,0x01,0x03,0x40,0x24,0x01,0x04,0x40,0x25,0xac,0xc8,0x00,0x04,
-0x8f,0xa8,0x00,0x98,0x27,0x82,0x90,0x10,0x00,0xa2,0x10,0x21,0xa0,0x48,0x00,0x00,
-0x8c,0xc4,0x00,0x08,0x00,0x00,0x00,0x00,0x00,0x04,0x27,0xc2,0x10,0x80,0xfe,0xdb,
-0xaf,0xa4,0x00,0x3c,0x80,0x42,0x00,0x06,0x00,0x00,0x00,0x00,0x24,0x42,0x00,0x02,
-0x00,0x02,0x1f,0xc2,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x43,0x00,0x02,0x10,0x40,
-0x00,0xc2,0x38,0x21,0x8f,0xa2,0x00,0x40,0x00,0x00,0x00,0x00,0xa4,0xe2,0x00,0x00,
-0x08,0x00,0x1d,0x0c,0x26,0xb5,0xff,0xff,0x8f,0xa6,0x00,0x2c,0x00,0x00,0x20,0x21,
-0x00,0xc2,0x10,0x21,0x30,0x42,0xff,0xff,0x08,0x00,0x1e,0x1a,0xaf,0xa2,0x00,0x2c,
-0x8f,0xa6,0x00,0x1c,0x08,0x00,0x1e,0x04,0xa4,0xd2,0x00,0x14,0x8f,0xa5,0x00,0x48,
-0x8f,0xa6,0x00,0x4c,0x8f,0xa4,0x00,0x1c,0x0c,0x00,0x1b,0xd8,0xaf,0xb7,0x00,0x10,
-0x08,0x00,0x1d,0xfb,0x00,0x00,0xb8,0x21,0x0c,0x00,0x12,0x8b,0x00,0x00,0x28,0x21,
-0x00,0x40,0x18,0x21,0x94,0x42,0x00,0x00,0x00,0x00,0x00,0x00,0x34,0x42,0x08,0x00,
-0xa4,0x62,0x00,0x00,0x08,0x00,0x1d,0x00,0x02,0x71,0x10,0x21,0x02,0x92,0x18,0x21,
-0x00,0x03,0x80,0x80,0x27,0x82,0x90,0x04,0x02,0x02,0x10,0x21,0x8c,0x44,0x00,0x18,
-0x00,0x00,0x00,0x00,0x8c,0x83,0x00,0x04,0x00,0x00,0x00,0x00,0x30,0x63,0x00,0x10,
-0x10,0x60,0x00,0x09,0x24,0x06,0x00,0x01,0x93,0x82,0x8b,0x71,0x00,0x00,0x00,0x00,
-0x30,0x42,0x00,0x01,0x10,0x40,0xfe,0xa2,0x3c,0x04,0x00,0x80,0x27,0x85,0x90,0x00,
-0x08,0x00,0x1d,0xeb,0x02,0x05,0x28,0x21,0x27,0x83,0x90,0x18,0x27,0x82,0x90,0x10,
-0x02,0x03,0x18,0x21,0x02,0x02,0x10,0x21,0x90,0x64,0x00,0x00,0x90,0x45,0x00,0x05,
-0x93,0x83,0x80,0x10,0x00,0x00,0x38,0x21,0x0c,0x00,0x21,0x9a,0xaf,0xa3,0x00,0x10,
-0x08,0x00,0x1e,0x62,0x00,0x00,0x00,0x00,0x27,0x82,0x90,0x18,0x02,0x02,0x10,0x21,
-0x94,0x43,0x00,0x02,0x8f,0xa6,0x00,0x58,0x00,0x03,0x19,0x02,0x00,0x66,0x18,0x23,
-0x30,0x63,0x0f,0xff,0x28,0x62,0x00,0x20,0x10,0x40,0x00,0x06,0x28,0x62,0x00,0x40,
-0x8f,0xa8,0x00,0x90,0x00,0x00,0x00,0x00,0x00,0x68,0x10,0x06,0x08,0x00,0x1c,0xd9,
-0x30,0x44,0x00,0x01,0x10,0x40,0x00,0x04,0x00,0x00,0x00,0x00,0x8f,0xa4,0x00,0x94,
-0x08,0x00,0x1e,0x83,0x00,0x64,0x10,0x06,0x08,0x00,0x1c,0xd9,0x00,0x00,0x20,0x21,
-0x8f,0xa4,0x00,0x98,0x8f,0xa5,0x00,0x38,0xaf,0xa0,0x00,0x10,0x0c,0x00,0x09,0x0a,
-0xaf,0xa8,0x00,0x14,0x30,0x42,0xff,0xff,0x08,0x00,0x1c,0xa9,0xaf,0xa2,0x00,0x40,
-0x3c,0x02,0xb0,0x03,0x3c,0x03,0x80,0x00,0x27,0xbd,0xff,0xe0,0x34,0x42,0x00,0x20,
-0x24,0x63,0x7a,0x50,0xaf,0xb1,0x00,0x14,0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x18,
-0xac,0x43,0x00,0x00,0x90,0x82,0x00,0x0a,0x00,0x80,0x80,0x21,0x14,0x40,0x00,0x45,
-0x00,0x00,0x88,0x21,0x92,0x02,0x00,0x04,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x3c,
-0x00,0x00,0x00,0x00,0x12,0x20,0x00,0x18,0x00,0x00,0x00,0x00,0x92,0x02,0x00,0x16,
-0x92,0x05,0x00,0x0a,0x30,0x42,0x00,0xfc,0x10,0xa0,0x00,0x03,0xa2,0x02,0x00,0x16,
-0x34,0x42,0x00,0x01,0xa2,0x02,0x00,0x16,0x92,0x04,0x00,0x04,0x00,0x00,0x00,0x00,
-0x30,0x83,0x00,0xff,0x10,0x60,0x00,0x05,0x00,0x00,0x00,0x00,0x92,0x02,0x00,0x16,
-0x00,0x00,0x00,0x00,0x34,0x42,0x00,0x02,0xa2,0x02,0x00,0x16,0x10,0x60,0x00,0x0a,
-0x00,0x00,0x00,0x00,0x14,0xa0,0x00,0x08,0x00,0x00,0x00,0x00,0x96,0x02,0x00,0x00,
-0xa2,0x00,0x00,0x17,0xa6,0x02,0x00,0x14,0x8f,0xbf,0x00,0x18,0x7b,0xb0,0x00,0xbc,
-0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x20,0x14,0x80,0x00,0x05,0x24,0x02,0x00,0x01,
-0x96,0x03,0x00,0x06,0xa2,0x02,0x00,0x17,0x08,0x00,0x1e,0xbe,0xa6,0x03,0x00,0x14,
-0x96,0x04,0x00,0x00,0x96,0x05,0x00,0x06,0x27,0x86,0x90,0x00,0x00,0x04,0x10,0xc0,
-0x00,0x05,0x18,0xc0,0x00,0x44,0x10,0x21,0x00,0x65,0x18,0x21,0x00,0x02,0x10,0x80,
-0x00,0x03,0x18,0x80,0x00,0x66,0x18,0x21,0x00,0x46,0x10,0x21,0x8c,0x65,0x00,0x08,
-0x8c,0x44,0x00,0x08,0x0c,0x00,0x12,0x7c,0x00,0x00,0x00,0x00,0x30,0x43,0x00,0xff,
-0x10,0x60,0x00,0x04,0xa2,0x02,0x00,0x17,0x96,0x02,0x00,0x06,0x08,0x00,0x1e,0xbe,
-0xa6,0x02,0x00,0x14,0x96,0x02,0x00,0x00,0x08,0x00,0x1e,0xbe,0xa6,0x02,0x00,0x14,
-0x96,0x05,0x00,0x00,0x0c,0x00,0x1e,0xea,0x02,0x00,0x20,0x21,0x08,0x00,0x1e,0xa5,
-0x02,0x22,0x88,0x21,0x94,0x85,0x00,0x06,0x0c,0x00,0x1e,0xea,0x00,0x00,0x00,0x00,
-0x08,0x00,0x1e,0xa1,0x00,0x40,0x88,0x21,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,
-0x34,0x63,0x00,0x20,0x24,0x42,0x7b,0xa8,0x27,0xbd,0xff,0xf0,0xac,0x62,0x00,0x00,
-0x00,0x00,0x10,0x21,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x10,0x3c,0x03,0xb0,0x03,
-0x3c,0x02,0x80,0x00,0x34,0x63,0x00,0x20,0x24,0x42,0x7b,0xcc,0xac,0x62,0x00,0x00,
-0x90,0x89,0x00,0x0a,0x00,0x80,0x30,0x21,0x11,0x20,0x00,0x05,0x00,0xa0,0x50,0x21,
-0x90,0x82,0x00,0x17,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x1b,0x00,0x00,0x00,0x00,
-0x90,0xc7,0x00,0x04,0x00,0x00,0x00,0x00,0x10,0xe0,0x00,0x1b,0x00,0x00,0x00,0x00,
-0x94,0xc8,0x00,0x00,0x27,0x83,0x90,0x00,0x93,0x85,0x8b,0x70,0x00,0x08,0x10,0xc0,
-0x00,0x48,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,0x8c,0x44,0x00,0x08,
-0x00,0xe5,0x28,0x2b,0x10,0xa0,0x00,0x06,0x01,0x44,0x18,0x23,0x8f,0x82,0x8b,0x88,
-0x00,0x00,0x00,0x00,0x00,0x43,0x10,0x2b,0x10,0x40,0x00,0x05,0x00,0x00,0x00,0x00,
-0x24,0x03,0x00,0x10,0xa4,0xc8,0x00,0x14,0x03,0xe0,0x00,0x08,0x00,0x60,0x10,0x21,
-0x11,0x20,0x00,0x05,0x00,0x00,0x00,0x00,0x94,0xc2,0x00,0x06,0x24,0x03,0x00,0x08,
-0x08,0x00,0x1f,0x16,0xa4,0xc2,0x00,0x14,0x08,0x00,0x1f,0x16,0x00,0x00,0x18,0x21,
-0x27,0xbd,0xff,0xc8,0xaf,0xb5,0x00,0x2c,0xaf,0xb4,0x00,0x28,0xaf,0xb3,0x00,0x24,
-0xaf,0xb0,0x00,0x18,0xaf,0xbf,0x00,0x30,0xaf,0xb2,0x00,0x20,0xaf,0xb1,0x00,0x1c,
-0x94,0x91,0x00,0x06,0x00,0x80,0xa0,0x21,0x3c,0x02,0x80,0x00,0x3c,0x04,0xb0,0x03,
-0x00,0x11,0xa8,0xc0,0x34,0x84,0x00,0x20,0x24,0x42,0x7c,0x80,0x02,0xb1,0x48,0x21,
-0xac,0x82,0x00,0x00,0x00,0x09,0x48,0x80,0x24,0x03,0x00,0x01,0x27,0x82,0x90,0x10,
-0xa2,0x83,0x00,0x12,0x01,0x22,0x10,0x21,0x27,0x84,0x90,0x04,0x01,0x24,0x20,0x21,
-0x80,0x48,0x00,0x06,0x8c,0x8a,0x00,0x18,0x27,0x83,0x90,0x20,0x01,0x23,0x48,0x21,
-0x8d,0x24,0x00,0x00,0x25,0x08,0x00,0x02,0x8d,0x42,0x00,0x00,0x8d,0x49,0x00,0x04,
-0x00,0x08,0x17,0xc2,0x8d,0x43,0x00,0x08,0x01,0x02,0x40,0x21,0x00,0x04,0x25,0xc2,
-0x00,0x08,0x40,0x43,0x30,0x84,0x00,0x01,0x00,0x03,0x1f,0xc2,0x00,0x08,0x40,0x40,
-0x00,0xe0,0x80,0x21,0x00,0x64,0x18,0x24,0x00,0x09,0x49,0x42,0x01,0x48,0x10,0x21,
-0x00,0xa0,0x98,0x21,0x00,0xa0,0x20,0x21,0x00,0x40,0x38,0x21,0x02,0x00,0x28,0x21,
-0x14,0x60,0x00,0x19,0x31,0x29,0x00,0x01,0x94,0x42,0x00,0x00,0x02,0xb1,0x88,0x21,
-0x02,0x00,0x28,0x21,0x00,0x11,0x88,0x80,0x27,0x90,0x90,0x00,0x02,0x30,0x80,0x21,
-0x96,0x03,0x00,0x06,0x30,0x52,0xff,0xff,0x02,0x60,0x20,0x21,0x00,0x60,0x30,0x21,
-0xa6,0x83,0x00,0x1a,0x27,0x82,0x90,0x08,0x0c,0x00,0x08,0xe3,0x02,0x22,0x88,0x21,
-0x00,0x52,0x10,0x21,0x96,0x03,0x00,0x06,0xa6,0x22,0x00,0x04,0x8f,0xbf,0x00,0x30,
-0x7b,0xb4,0x01,0x7c,0x7b,0xb2,0x01,0x3c,0x7b,0xb0,0x00,0xfc,0x00,0x60,0x10,0x21,
-0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x38,0xaf,0xa9,0x00,0x10,0x0c,0x00,0x09,0x0a,
-0xaf,0xa0,0x00,0x14,0x08,0x00,0x1f,0x54,0x02,0xb1,0x88,0x21,0x27,0xbd,0xff,0xc0,
-0xaf,0xbe,0x00,0x38,0xaf,0xb7,0x00,0x34,0xaf,0xb6,0x00,0x30,0xaf,0xb5,0x00,0x2c,
-0xaf,0xb3,0x00,0x24,0xaf,0xb1,0x00,0x1c,0xaf,0xbf,0x00,0x3c,0xaf,0xb4,0x00,0x28,
-0xaf,0xb2,0x00,0x20,0xaf,0xb0,0x00,0x18,0x94,0x90,0x00,0x00,0x3c,0x08,0xb0,0x03,
-0x35,0x08,0x00,0x20,0x00,0x10,0x10,0xc0,0x00,0x50,0x18,0x21,0x00,0x40,0x88,0x21,
-0x3c,0x02,0x80,0x00,0x00,0x03,0x48,0x80,0x24,0x42,0x7d,0xbc,0x00,0x80,0x98,0x21,
-0x27,0x84,0x90,0x10,0x01,0x24,0x20,0x21,0x93,0xb7,0x00,0x53,0xad,0x02,0x00,0x00,
-0x80,0x83,0x00,0x06,0x27,0x82,0x90,0x04,0x01,0x22,0x10,0x21,0x8c,0x44,0x00,0x18,
-0x24,0x63,0x00,0x02,0x00,0x03,0x17,0xc2,0x8c,0x88,0x00,0x08,0x00,0x62,0x18,0x21,
-0x00,0x03,0x18,0x43,0x00,0x03,0x18,0x40,0xaf,0xa7,0x00,0x4c,0x2c,0xa2,0x00,0x10,
-0x00,0xa0,0xa8,0x21,0x00,0x83,0x50,0x21,0x00,0x08,0x47,0xc2,0x00,0xc0,0x58,0x21,
-0x00,0x00,0xb0,0x21,0x8c,0x92,0x00,0x0c,0x14,0x40,0x00,0x13,0x00,0x00,0xf0,0x21,
-0x92,0x67,0x00,0x04,0x24,0x14,0x00,0x01,0x12,0x87,0x00,0x10,0x02,0x30,0x10,0x21,
-0x27,0x83,0x90,0x18,0x01,0x23,0x18,0x21,0x80,0x64,0x00,0x00,0x27,0x83,0xb5,0x70,
-0x00,0x04,0x11,0x00,0x00,0x44,0x10,0x23,0x00,0x02,0x10,0x80,0x00,0x44,0x10,0x23,
-0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,0x90,0x44,0x00,0x04,0x00,0x00,0x00,0x00,
-0x10,0x80,0x00,0x23,0x00,0x00,0x00,0x00,0x02,0x30,0x10,0x21,0x00,0x02,0x80,0x80,
-0x24,0x04,0x00,0x01,0x27,0x83,0x90,0x20,0xa2,0x64,0x00,0x12,0x02,0x03,0x18,0x21,
-0x8c,0x62,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x02,0x15,0xc2,0x30,0x42,0x00,0x01,
-0x01,0x02,0x10,0x24,0x14,0x40,0x00,0x0e,0x02,0xa0,0x20,0x21,0x27,0x82,0x90,0x00,
-0x02,0x02,0x10,0x21,0x94,0x43,0x00,0x06,0x00,0x00,0x00,0x00,0xa6,0x63,0x00,0x1a,
-0x94,0x42,0x00,0x06,0x7b,0xbe,0x01,0xfc,0x7b,0xb6,0x01,0xbc,0x7b,0xb4,0x01,0x7c,
-0x7b,0xb2,0x01,0x3c,0x7b,0xb0,0x00,0xfc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x40,
-0x8f,0xa5,0x00,0x4c,0x01,0x60,0x30,0x21,0x01,0x40,0x38,0x21,0xaf,0xa0,0x00,0x10,
-0x0c,0x00,0x09,0x0a,0xaf,0xa0,0x00,0x14,0x08,0x00,0x1f,0xbb,0x00,0x00,0x00,0x00,
-0x27,0x83,0x90,0x20,0x01,0x23,0x18,0x21,0x8c,0x62,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x02,0x15,0xc2,0x30,0x42,0x00,0x01,0x01,0x02,0x10,0x24,0x14,0x40,0x00,0xaf,
-0x00,0xa0,0x20,0x21,0x32,0x4f,0x00,0x03,0x00,0x12,0x10,0x82,0x25,0xe3,0x00,0x0d,
-0x30,0x45,0x00,0x07,0x00,0x74,0x78,0x04,0x10,0xa0,0x00,0x0e,0x00,0x00,0x90,0x21,
-0x27,0x82,0x80,0x1c,0x00,0x15,0x18,0x40,0x00,0x62,0x18,0x21,0x94,0x64,0x00,0x00,
-0x24,0xa2,0x00,0x06,0x00,0x54,0x10,0x04,0x00,0x44,0x00,0x1a,0x14,0x80,0x00,0x02,
-0x00,0x00,0x00,0x00,0x00,0x07,0x00,0x0d,0x00,0x00,0x10,0x12,0x24,0x42,0x00,0x20,
-0x30,0x52,0xff,0xfc,0x02,0x30,0x10,0x21,0x27,0x83,0x90,0x10,0x00,0x02,0x10,0x80,
-0x00,0x43,0x10,0x21,0x90,0x44,0x00,0x03,0x00,0x00,0x00,0x00,0x30,0x83,0x00,0xff,
-0x2c,0x62,0x00,0x0c,0x14,0x40,0x00,0x04,0x2c,0x62,0x00,0x19,0x30,0x82,0x00,0x0f,
-0x24,0x43,0x00,0x0c,0x2c,0x62,0x00,0x19,0x10,0x40,0x00,0x19,0x24,0x0e,0x00,0x20,
-0x24,0x62,0xff,0xe9,0x2c,0x42,0x00,0x02,0x14,0x40,0x00,0x15,0x24,0x0e,0x00,0x10,
-0x24,0x62,0xff,0xeb,0x2c,0x42,0x00,0x02,0x14,0x40,0x00,0x11,0x24,0x0e,0x00,0x08,
-0x24,0x02,0x00,0x14,0x10,0x62,0x00,0x0e,0x24,0x0e,0x00,0x02,0x24,0x62,0xff,0xef,
-0x2c,0x42,0x00,0x03,0x14,0x40,0x00,0x0a,0x24,0x0e,0x00,0x10,0x24,0x62,0xff,0xf1,
-0x2c,0x42,0x00,0x02,0x14,0x40,0x00,0x06,0x24,0x0e,0x00,0x08,0x24,0x62,0xff,0xf3,
-0x2c,0x42,0x00,0x02,0x24,0x0e,0x00,0x04,0x24,0x03,0x00,0x02,0x00,0x62,0x70,0x0a,
-0x30,0xe2,0x00,0xff,0x00,0x00,0x48,0x21,0x00,0x00,0x68,0x21,0x10,0x40,0x00,0x6d,
-0x00,0x00,0x58,0x21,0x3c,0x14,0x80,0xff,0x27,0x99,0x90,0x00,0x01,0xf2,0xc0,0x23,
-0x36,0x94,0xff,0xff,0x01,0xc9,0x10,0x2a,0x14,0x40,0x00,0x64,0x24,0x03,0x00,0x04,
-0x00,0x10,0x28,0xc0,0x00,0xb0,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x59,0x10,0x21,
-0x94,0x56,0x00,0x06,0x00,0x00,0x00,0x00,0x32,0xcc,0x00,0x03,0x00,0x6c,0x10,0x23,
-0x30,0x42,0x00,0x03,0x02,0xc2,0x10,0x21,0x24,0x42,0x00,0x04,0x30,0x51,0xff,0xff,
-0x02,0x32,0x18,0x2b,0x10,0x60,0x00,0x4d,0x01,0xf1,0x10,0x23,0x02,0x51,0x10,0x23,
-0x01,0x78,0x18,0x2b,0x10,0x60,0x00,0x34,0x30,0x44,0xff,0xff,0x29,0x22,0x00,0x40,
-0x10,0x40,0x00,0x31,0x01,0x72,0x18,0x21,0x25,0x22,0x00,0x01,0x00,0x02,0x16,0x00,
-0x00,0x02,0x4e,0x03,0x00,0xb0,0x10,0x21,0x00,0x02,0x30,0x80,0x27,0x82,0x90,0x04,
-0x30,0x6b,0xff,0xff,0x00,0xc2,0x18,0x21,0x8c,0x67,0x00,0x18,0x00,0x04,0x25,0x40,
-0x3c,0x03,0x7f,0x00,0x8c,0xe2,0x00,0x04,0x00,0x83,0x20,0x24,0x27,0x83,0x90,0x10,
-0x00,0x54,0x10,0x24,0x00,0xc3,0x28,0x21,0x00,0x44,0x10,0x25,0xac,0xe2,0x00,0x04,
-0x16,0xe0,0x00,0x02,0xa0,0xb5,0x00,0x00,0xa0,0xb5,0x00,0x03,0x27,0x84,0x90,0x20,
-0x00,0xc4,0x18,0x21,0x8c,0x62,0x00,0x00,0x8c,0xe8,0x00,0x08,0x00,0x02,0x15,0xc2,
-0x00,0x08,0x47,0xc2,0x30,0x42,0x00,0x01,0x01,0x02,0x10,0x24,0x10,0x40,0x00,0x0a,
-0x00,0x00,0x00,0x00,0x80,0xa2,0x00,0x06,0x00,0x00,0x00,0x00,0x24,0x42,0x00,0x02,
-0x00,0x02,0x1f,0xc2,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x43,0x00,0x02,0x10,0x40,
-0x00,0xe2,0x50,0x21,0xa5,0x5e,0x00,0x00,0x92,0x62,0x00,0x04,0x25,0xad,0x00,0x01,
-0x27,0x84,0x90,0x00,0x00,0xc4,0x18,0x21,0x01,0xa2,0x10,0x2a,0x94,0x70,0x00,0x02,
-0x14,0x40,0xff,0xb8,0x00,0x00,0x00,0x00,0x96,0x63,0x00,0x14,0x00,0x0c,0x10,0x23,
-0xa2,0x69,0x00,0x12,0x30,0x42,0x00,0x03,0x01,0x62,0x10,0x23,0x00,0x03,0x80,0xc0,
-0x8f,0xa5,0x00,0x4c,0x30,0x4b,0xff,0xff,0x02,0x03,0x80,0x21,0x27,0x82,0x90,0x08,
-0x00,0x10,0x80,0x80,0xa6,0x6b,0x00,0x1a,0x02,0xa0,0x20,0x21,0x01,0x60,0x30,0x21,
-0x01,0x60,0x88,0x21,0x0c,0x00,0x08,0xe3,0x02,0x02,0x80,0x21,0x00,0x5e,0x10,0x21,
-0xa6,0x02,0x00,0x04,0x08,0x00,0x1f,0xc1,0x02,0x20,0x10,0x21,0x01,0x62,0x10,0x2b,
-0x10,0x40,0xff,0xe9,0x00,0x00,0x20,0x21,0x29,0x22,0x00,0x40,0x10,0x40,0xff,0xe6,
-0x01,0x71,0x18,0x21,0x08,0x00,0x20,0x37,0x25,0x22,0x00,0x01,0x08,0x00,0x20,0x66,
-0x32,0xcc,0x00,0x03,0x08,0x00,0x20,0x66,0x00,0x00,0x60,0x21,0x8f,0xa5,0x00,0x4c,
-0x01,0x40,0x38,0x21,0xaf,0xa0,0x00,0x10,0x0c,0x00,0x09,0x0a,0xaf,0xb4,0x00,0x14,
-0x92,0x67,0x00,0x04,0x08,0x00,0x1f,0xd9,0x30,0x5e,0xff,0xff,0x30,0x84,0xff,0xff,
-0x00,0x04,0x30,0xc0,0x00,0xc4,0x20,0x21,0x00,0x04,0x20,0x80,0x27,0x82,0x90,0x00,
-0x3c,0x03,0xb0,0x08,0x30,0xa5,0xff,0xff,0x00,0x82,0x20,0x21,0x00,0xc3,0x30,0x21,
-0xac,0xc5,0x00,0x00,0x03,0xe0,0x00,0x08,0xa4,0x85,0x00,0x00,0x30,0x84,0xff,0xff,
-0x00,0x04,0x30,0xc0,0x00,0xc4,0x30,0x21,0x27,0x88,0x90,0x00,0x00,0x06,0x30,0x80,
-0x00,0xc8,0x30,0x21,0x94,0xc3,0x00,0x04,0x3c,0x02,0xb0,0x08,0x3c,0x07,0xb0,0x03,
-0x00,0x03,0x20,0xc0,0x00,0x83,0x18,0x21,0x00,0x03,0x18,0x80,0x00,0x82,0x20,0x21,
-0x3c,0x02,0x80,0x01,0x30,0xa5,0xff,0xff,0x00,0x68,0x18,0x21,0x34,0xe7,0x00,0x20,
-0x24,0x42,0x82,0x6c,0xac,0xe2,0x00,0x00,0xa4,0xc5,0x00,0x02,0xa4,0x65,0x00,0x00,
-0x03,0xe0,0x00,0x08,0xac,0x85,0x00,0x00,0x30,0x84,0xff,0xff,0x00,0x04,0x10,0xc0,
-0x00,0x44,0x10,0x21,0x27,0x89,0x90,0x00,0x00,0x02,0x10,0x80,0x00,0x49,0x10,0x21,
-0x97,0x83,0x8f,0xf0,0x94,0x4a,0x00,0x04,0x3c,0x02,0xb0,0x08,0x00,0x03,0x38,0xc0,
-0x00,0x0a,0x40,0xc0,0x00,0xe3,0x18,0x21,0x01,0x0a,0x28,0x21,0x00,0xe2,0x38,0x21,
-0x01,0x02,0x40,0x21,0x00,0x03,0x18,0x80,0x00,0x05,0x28,0x80,0x3c,0x06,0xb0,0x03,
-0x3c,0x02,0x80,0x01,0x00,0xa9,0x28,0x21,0x00,0x69,0x18,0x21,0x34,0xc6,0x00,0x20,
-0x34,0x09,0xff,0xff,0x24,0x42,0x82,0xc8,0xac,0xc2,0x00,0x00,0xa4,0x64,0x00,0x00,
-0xac,0xe4,0x00,0x00,0xa4,0xa9,0x00,0x00,0xad,0x09,0x00,0x00,0xa7,0x8a,0x8f,0xf0,
-0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x01,
-0x34,0x63,0x00,0x20,0x24,0x42,0x83,0x48,0x3c,0x04,0xb0,0x03,0xac,0x62,0x00,0x00,
-0x34,0x84,0x01,0x10,0x8c,0x82,0x00,0x00,0x97,0x83,0x81,0x60,0x30,0x42,0xff,0xff,
-0x10,0x62,0x00,0x16,0x24,0x0a,0x00,0x01,0xa7,0x82,0x81,0x60,0xaf,0x80,0xb4,0x50,
-0x00,0x40,0x28,0x21,0x24,0x06,0x00,0x01,0x27,0x84,0xb4,0x54,0x25,0x43,0xff,0xff,
-0x00,0x66,0x10,0x04,0x00,0xa2,0x10,0x24,0x14,0x40,0x00,0x07,0x00,0x00,0x00,0x00,
-0x8c,0x83,0xff,0xfc,0x00,0x00,0x00,0x00,0x00,0x66,0x10,0x04,0x00,0xa2,0x10,0x24,
-0x38,0x42,0x00,0x00,0x01,0x42,0x18,0x0a,0x25,0x4a,0x00,0x01,0x2d,0x42,0x00,0x14,
-0xac,0x83,0x00,0x00,0x14,0x40,0xff,0xf1,0x24,0x84,0x00,0x04,0x3c,0x0b,0xb0,0x03,
-0x00,0x00,0x50,0x21,0x3c,0x0c,0x80,0x00,0x27,0x89,0xb4,0xa0,0x35,0x6b,0x01,0x20,
-0x8d,0x68,0x00,0x00,0x8d,0x23,0x00,0x04,0x01,0x0c,0x10,0x24,0x00,0x02,0x17,0xc2,
-0x11,0x03,0x00,0x37,0xa1,0x22,0x00,0xdc,0xa1,0x20,0x00,0xd5,0xa1,0x20,0x00,0xd6,
-0x01,0x20,0x30,0x21,0x00,0x00,0x38,0x21,0x00,0x00,0x28,0x21,0x01,0x20,0x20,0x21,
-0x00,0xa8,0x10,0x06,0x30,0x42,0x00,0x01,0x10,0xe0,0x00,0x10,0xa0,0x82,0x00,0x0a,
-0x90,0x82,0x00,0x07,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x31,0x24,0xa2,0xff,0xff,
-0xa0,0x82,0x00,0x08,0x90,0x82,0x00,0x0a,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x09,
-0x00,0x00,0x00,0x00,0x90,0x83,0x00,0x08,0x00,0x00,0x00,0x00,0x00,0x03,0x10,0x40,
-0x00,0x43,0x10,0x21,0x00,0x46,0x10,0x21,0xa0,0x45,0x00,0x09,0x90,0x82,0x00,0x0a,
-0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x07,0x00,0x00,0x00,0x00,0x14,0xe0,0x00,0x04,
-0x00,0x00,0x00,0x00,0xa0,0xc5,0x00,0xd5,0x24,0x07,0x00,0x01,0xa0,0x85,0x00,0x08,
-0xa0,0xc5,0x00,0xd6,0x24,0xa5,0x00,0x01,0x2c,0xa2,0x00,0x1c,0x14,0x40,0xff,0xe0,
-0x24,0x84,0x00,0x03,0x90,0xc4,0x00,0xd5,0x00,0x00,0x28,0x21,0x00,0xa4,0x10,0x2b,
-0x10,0x40,0x00,0x0b,0x00,0x00,0x00,0x00,0x00,0xc0,0x18,0x21,0xa0,0x64,0x00,0x08,
-0x90,0xc2,0x00,0xd5,0x24,0xa5,0x00,0x01,0xa0,0x62,0x00,0x09,0x90,0xc4,0x00,0xd5,
-0x00,0x00,0x00,0x00,0x00,0xa4,0x10,0x2b,0x14,0x40,0xff,0xf8,0x24,0x63,0x00,0x03,
-0x25,0x4a,0x00,0x01,0x2d,0x42,0x00,0x08,0xad,0x28,0x00,0x04,0x25,0x6b,0x00,0x04,
-0x14,0x40,0xff,0xbf,0x25,0x29,0x00,0xec,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,
-0x90,0x82,0x00,0x05,0x08,0x00,0x21,0x0d,0xa0,0x82,0x00,0x08,0x97,0x85,0x8b,0x7a,
-0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x01,0x27,0xbd,0xff,0xe8,0x34,0x63,0x00,0x20,
-0x24,0x42,0x84,0xfc,0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x14,0xac,0x62,0x00,0x00,
-0x30,0x90,0x00,0xff,0x00,0x05,0x28,0x42,0x00,0x00,0x48,0x21,0x27,0x8f,0xb4,0xa4,
-0x00,0x00,0x50,0x21,0x00,0x00,0x58,0x21,0x27,0x98,0xb5,0x84,0x27,0x99,0xb5,0x80,
-0x27,0x8e,0xb5,0x7e,0x27,0x8c,0xb4,0xa8,0x27,0x8d,0xb5,0x00,0x27,0x88,0xb5,0x78,
-0x00,0x0a,0x18,0x80,0x01,0x6f,0x10,0x21,0xac,0x40,0x00,0x00,0xac,0x45,0x00,0x58,
-0x00,0x6e,0x20,0x21,0x00,0x78,0x10,0x21,0xa1,0x00,0xff,0xfc,0xad,0x00,0x00,0x00,
-0xa1,0x00,0x00,0x04,0xa1,0x00,0x00,0x05,0xad,0x00,0xff,0xf8,0x00,0x79,0x18,0x21,
-0x24,0x06,0x00,0x01,0x24,0xc6,0xff,0xff,0xa0,0x80,0x00,0x00,0xa4,0x60,0x00,0x00,
-0xac,0x40,0x00,0x00,0x24,0x63,0x00,0x02,0x24,0x42,0x00,0x04,0x04,0xc1,0xff,0xf9,
-0x24,0x84,0x00,0x01,0x00,0x0a,0x10,0x80,0x00,0x4d,0x20,0x21,0x00,0x00,0x30,0x21,
-0x00,0x4c,0x18,0x21,0x27,0x87,0x81,0x64,0x8c,0xe2,0x00,0x00,0x24,0xe7,0x00,0x04,
-0xac,0x82,0x00,0x00,0xa0,0x66,0x00,0x00,0xa0,0x66,0x00,0x01,0x24,0xc6,0x00,0x01,
-0x28,0xc2,0x00,0x1c,0xa0,0x60,0x00,0x02,0x24,0x84,0x00,0x04,0x14,0x40,0xff,0xf6,
-0x24,0x63,0x00,0x03,0x25,0x29,0x00,0x01,0x29,0x22,0x00,0x08,0x25,0x4a,0x00,0x3b,
-0x25,0x08,0x00,0xec,0x14,0x40,0xff,0xd6,0x25,0x6b,0x00,0xec,0xa7,0x80,0x81,0x60,
-0x00,0x00,0x48,0x21,0x27,0x83,0xb4,0x50,0xac,0x69,0x00,0x00,0x25,0x29,0x00,0x01,
-0x29,0x22,0x00,0x0c,0x14,0x40,0xff,0xfc,0x24,0x63,0x00,0x04,0x0c,0x00,0x20,0xd2,
-0x00,0x00,0x00,0x00,0x2e,0x04,0x00,0x14,0x27,0x83,0xb4,0xa0,0x24,0x09,0x00,0x07,
-0x10,0x80,0x00,0x0a,0x00,0x00,0x00,0x00,0x90,0x62,0x00,0xd5,0x25,0x29,0xff,0xff,
-0xa0,0x62,0x00,0x00,0x05,0x21,0xff,0xfa,0x24,0x63,0x00,0xec,0x8f,0xbf,0x00,0x14,
-0x8f,0xb0,0x00,0x10,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x18,0x90,0x62,0x00,0xd6,
-0x08,0x00,0x21,0x90,0x25,0x29,0xff,0xff,0x30,0x84,0x00,0xff,0x00,0x04,0x11,0x00,
-0x00,0x44,0x10,0x23,0x00,0x02,0x10,0x80,0x00,0x44,0x10,0x23,0x00,0x02,0x10,0x80,
-0x27,0x83,0xb4,0xa0,0x00,0x43,0x60,0x21,0x3c,0x04,0xb0,0x03,0x3c,0x02,0x80,0x01,
-0x34,0x84,0x00,0x20,0x24,0x42,0x86,0x68,0x30,0xc6,0x00,0xff,0x93,0xaa,0x00,0x13,
-0x30,0xa5,0x00,0xff,0x30,0xe7,0x00,0xff,0xac,0x82,0x00,0x00,0x10,0xc0,0x00,0xe8,
-0x25,0x8f,0x00,0xd0,0x91,0x82,0x00,0x00,0x00,0x00,0x00,0x00,0x24,0x42,0xff,0xfc,
-0x2c,0x43,0x00,0x18,0x10,0x60,0x00,0xc7,0x3c,0x03,0x80,0x01,0x00,0x02,0x10,0x80,
-0x24,0x63,0x02,0x90,0x00,0x43,0x10,0x21,0x8c,0x44,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x80,0x00,0x08,0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x30,0x14,0x40,0x00,0x1c,
-0x00,0x00,0x00,0x00,0x10,0xa0,0x00,0x17,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0x10,0xa2,0x00,0x11,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0x00,0x0c,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x03,0x10,0xa2,0x00,0x06,0x00,0x00,0x00,0x00,
-0x8d,0x82,0x00,0xd0,0x00,0x00,0x00,0x00,0x24,0x42,0xff,0xe0,0x03,0xe0,0x00,0x08,
-0xad,0x82,0x00,0xd0,0x8d,0x82,0x00,0xd0,0x08,0x00,0x21,0xcb,0x24,0x42,0xff,0xe8,
-0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x8d,0x82,0x00,0xd0,0x08,0x00,0x21,0xcb,
-0x24,0x42,0x00,0x01,0x8d,0x82,0x00,0xd0,0x08,0x00,0x21,0xcb,0x24,0x42,0x00,0x02,
-0x10,0xa0,0xff,0xf9,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,0x10,0xa2,0x00,0x0a,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0xe9,0x00,0x00,0x00,0x00,
-0x24,0x02,0x00,0x03,0x10,0xa2,0xff,0xe6,0x00,0x00,0x00,0x00,0x8d,0x82,0x00,0xd0,
-0x08,0x00,0x21,0xcb,0x24,0x42,0xff,0xd0,0x8d,0x82,0x00,0xd0,0x08,0x00,0x21,0xcb,
-0x24,0x42,0xff,0xfc,0x10,0xa0,0xff,0xeb,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0x10,0xa2,0xff,0xe5,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0xe0,
-0x24,0x02,0x00,0x03,0x14,0xa2,0xff,0xdb,0x00,0x00,0x00,0x00,0x8d,0x82,0x00,0xd0,
-0x08,0x00,0x21,0xcb,0x24,0x42,0xff,0xf8,0x2d,0x42,0x00,0x19,0x14,0x40,0xff,0xc5,
-0x00,0x00,0x00,0x00,0x10,0xa0,0xff,0xdb,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0x10,0xa2,0xff,0xd5,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0xd0,
-0x24,0x02,0x00,0x03,0x10,0xa2,0xff,0xf1,0x00,0x00,0x00,0x00,0x8d,0x82,0x00,0xd0,
-0x08,0x00,0x21,0xcb,0x24,0x42,0xff,0xf0,0x2d,0x42,0x00,0x1b,0x10,0x40,0xff,0xf1,
-0x00,0x00,0x00,0x00,0x10,0xa0,0xff,0xcb,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0x10,0xa2,0xff,0xc5,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x14,0xa2,0xff,0xb5,
-0x00,0x00,0x00,0x00,0x8d,0x82,0x00,0xd0,0x08,0x00,0x21,0xcb,0x24,0x42,0xff,0xf4,
-0x2d,0x42,0x00,0x1e,0x10,0x40,0xff,0xe3,0x00,0x00,0x00,0x00,0x10,0xa0,0xff,0xbd,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0xb5,0x24,0x02,0x00,0x02,
-0x10,0xa2,0xff,0xd6,0x00,0x00,0x00,0x00,0x08,0x00,0x21,0xc6,0x24,0x02,0x00,0x03,
-0x2d,0x42,0x00,0x23,0x10,0x40,0xff,0xd7,0x00,0x00,0x00,0x00,0x10,0xa0,0xff,0xae,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0xa9,0x24,0x02,0x00,0x02,
-0x14,0xa2,0xff,0xb7,0x00,0x00,0x00,0x00,0x08,0x00,0x22,0x03,0x00,0x00,0x00,0x00,
-0x2d,0x42,0x00,0x25,0x10,0x40,0xff,0xcb,0x00,0x00,0x00,0x00,0x08,0x00,0x21,0xd8,
-0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x16,0x14,0x40,0x00,0x0e,0x00,0x00,0x00,0x00,
-0x10,0xa0,0xff,0xa0,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0x9a,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0x95,0x24,0x02,0x00,0x03,
-0x14,0xa2,0xff,0xb6,0x00,0x00,0x00,0x00,0x8d,0x82,0x00,0xd0,0x08,0x00,0x21,0xcb,
-0x24,0x42,0xff,0xfa,0x10,0xa0,0xff,0x93,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0x10,0xa2,0xff,0x8d,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0x88,
-0x00,0x00,0x00,0x00,0x08,0x00,0x21,0xf3,0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x17,
-0x14,0x40,0xff,0xac,0x00,0x00,0x00,0x00,0x08,0x00,0x22,0x34,0x00,0x00,0x00,0x00,
-0x2d,0x42,0x00,0x19,0x10,0x40,0xff,0xe2,0x00,0x00,0x00,0x00,0x10,0xa0,0xff,0x81,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0x7b,0x00,0x00,0x00,0x00,
-0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0x76,0x24,0x02,0x00,0x03,0x10,0xa2,0xff,0x97,
-0x00,0x00,0x00,0x00,0x08,0x00,0x21,0xc8,0x00,0x00,0x00,0x00,0x08,0x00,0x22,0x51,
-0x2d,0x42,0x00,0x1b,0x2d,0x42,0x00,0x1e,0x10,0x40,0xff,0xde,0x00,0x00,0x00,0x00,
-0x10,0xa0,0xff,0x70,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0x6a,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0x65,0x24,0x02,0x00,0x03,
-0x10,0xa2,0xff,0x96,0x00,0x00,0x00,0x00,0x08,0x00,0x21,0xc8,0x00,0x00,0x00,0x00,
-0x2d,0x42,0x00,0x23,0x14,0x40,0xff,0xf2,0x00,0x00,0x00,0x00,0x08,0x00,0x21,0xf9,
-0x00,0x00,0x00,0x00,0x08,0x00,0x21,0xf7,0x2d,0x42,0x00,0x25,0x08,0x00,0x22,0x2d,
-0x2d,0x42,0x00,0x27,0x10,0xa0,0xff,0x5b,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0x10,0xa2,0xff,0x55,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0x50,
-0x24,0x02,0x00,0x03,0x14,0xa2,0xff,0x71,0x00,0x00,0x00,0x00,0x08,0x00,0x21,0xe6,
-0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x27,0x14,0x40,0xff,0xad,0x00,0x00,0x00,0x00,
-0x08,0x00,0x22,0x79,0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x2a,0x14,0x40,0xff,0xd8,
-0x00,0x00,0x00,0x00,0x08,0x00,0x21,0xe9,0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x2c,
-0x14,0x40,0xff,0x78,0x00,0x00,0x00,0x00,0x08,0x00,0x21,0xbd,0x00,0x00,0x00,0x00,
-0x91,0x86,0x00,0x00,0x91,0x83,0x00,0xd4,0x25,0x8d,0x00,0x5c,0x30,0xc4,0x00,0xff,
-0x00,0x04,0x10,0x40,0x00,0x44,0x10,0x21,0x00,0x04,0x48,0x80,0x01,0x82,0x58,0x21,
-0x01,0x89,0x40,0x21,0x25,0x78,0x00,0x08,0x10,0x60,0x00,0x37,0x25,0x0e,0x00,0x60,
-0x2c,0xa2,0x00,0x03,0x14,0x40,0x00,0x25,0x00,0x00,0x00,0x00,0x91,0x82,0x00,0xdd,
-0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x1e,0x00,0x00,0x00,0x00,0x27,0x87,0x81,0x64,
-0x01,0x27,0x10,0x21,0x8c,0x43,0x00,0x00,0x00,0x00,0x00,0x00,0xad,0x03,0x00,0x60,
-0x91,0x62,0x00,0x08,0x00,0x00,0x00,0x00,0x00,0x40,0x30,0x21,0xa1,0x82,0x00,0x00,
-0x30,0xc2,0x00,0xff,0x00,0x02,0x10,0x80,0x00,0x47,0x10,0x21,0x8c,0x43,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x03,0x18,0x42,0xad,0xa3,0x00,0x00,0x91,0x84,0x00,0x00,
-0x8d,0xc5,0x00,0x00,0x00,0x04,0x20,0x80,0x00,0x87,0x10,0x21,0x8c,0x43,0x00,0x00,
-0x00,0x05,0x28,0x40,0x00,0x8c,0x20,0x21,0x00,0x03,0x18,0x80,0x00,0xa3,0x10,0x2b,
-0x00,0x62,0x28,0x0a,0xac,0x85,0x00,0x60,0x03,0xe0,0x00,0x08,0xa1,0x80,0x00,0xd4,
-0x27,0x87,0x81,0x64,0x08,0x00,0x22,0xb0,0xa1,0x80,0x00,0xdd,0x27,0x82,0x81,0xd4,
-0x8d,0x83,0x00,0xd8,0x00,0x82,0x10,0x21,0x90,0x44,0x00,0x00,0x24,0x63,0x00,0x01,
-0x00,0x64,0x20,0x2b,0x14,0x80,0xff,0x02,0xad,0x83,0x00,0xd8,0x8d,0x02,0x00,0x60,
-0xa1,0x80,0x00,0xd4,0x00,0x02,0x1f,0xc2,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x43,
-0x03,0xe0,0x00,0x08,0xad,0x82,0x00,0x5c,0x10,0xe0,0x00,0x1d,0x24,0x83,0xff,0xfc,
-0x2c,0x62,0x00,0x18,0x10,0x40,0x01,0x10,0x00,0x03,0x10,0x80,0x3c,0x03,0x80,0x01,
-0x24,0x63,0x02,0xf0,0x00,0x43,0x10,0x21,0x8c,0x44,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x80,0x00,0x08,0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x30,0x14,0x40,0x00,0x65,
-0x00,0x00,0x00,0x00,0x10,0xa0,0x00,0x60,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0x10,0xa2,0x00,0x5a,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0x00,0x08,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x03,0x10,0xa2,0x00,0x51,0x00,0x00,0x00,0x00,
-0x8d,0x82,0x00,0xd0,0x00,0x00,0x00,0x00,0x24,0x42,0xff,0xe0,0xad,0x82,0x00,0xd0,
-0x8d,0xe3,0x00,0x00,0x8d,0xa2,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x43,0x10,0x21,
-0xad,0xa2,0x00,0x00,0xad,0xe0,0x00,0x00,0x8d,0xa3,0x00,0x00,0x8d,0xc4,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x83,0x10,0x2a,0x10,0x40,0x00,0x22,0x00,0x00,0x00,0x00,
-0x93,0x05,0x00,0x01,0x91,0x82,0x00,0x00,0x00,0x00,0x00,0x00,0x10,0x45,0x00,0x05,
-0x24,0x02,0x00,0x01,0xa1,0x85,0x00,0x00,0xa1,0x82,0x00,0xd4,0x03,0xe0,0x00,0x08,
-0xad,0x80,0x00,0xd8,0x91,0x82,0x00,0xdd,0x24,0x03,0x00,0x01,0x10,0x43,0x00,0x05,
-0x00,0x00,0x00,0x00,0xa1,0x83,0x00,0xd4,0xad,0x80,0x00,0xd8,0x03,0xe0,0x00,0x08,
-0xa1,0x83,0x00,0xdd,0x00,0x04,0x17,0xc2,0x00,0x82,0x10,0x21,0x00,0x02,0x10,0x43,
-0xad,0xa2,0x00,0x00,0x91,0x83,0x00,0x00,0x27,0x82,0x81,0x64,0x8d,0xc5,0x00,0x00,
-0x00,0x03,0x18,0x80,0x00,0x62,0x18,0x21,0x8c,0x64,0x00,0x00,0x00,0x05,0x28,0x40,
-0x00,0x04,0x18,0x80,0x00,0xa3,0x10,0x2b,0x00,0x62,0x28,0x0a,0x08,0x00,0x22,0xc2,
-0xad,0xc5,0x00,0x00,0x97,0x82,0x8b,0x7c,0x00,0x00,0x00,0x00,0x00,0x62,0x10,0x2a,
-0x10,0x40,0xfe,0xab,0x00,0x00,0x00,0x00,0x91,0x82,0x00,0xdd,0x00,0x00,0x00,0x00,
-0x14,0x40,0x00,0x15,0x00,0x00,0x00,0x00,0x91,0x83,0x00,0x00,0x27,0x82,0x81,0x64,
-0x00,0x03,0x18,0x80,0x00,0x62,0x10,0x21,0x8c,0x44,0x00,0x00,0x00,0x6c,0x18,0x21,
-0xac,0x64,0x00,0x60,0x93,0x05,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x05,0x10,0x80,
-0x01,0x82,0x10,0x21,0x24,0x4e,0x00,0x60,0xa1,0x85,0x00,0x00,0x8d,0xc2,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x02,0x1f,0xc2,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x43,
-0x03,0xe0,0x00,0x08,0xad,0xa2,0x00,0x00,0x08,0x00,0x23,0x37,0xa1,0x80,0x00,0xdd,
-0x8d,0x82,0x00,0xd0,0x08,0x00,0x22,0xf3,0x24,0x42,0xff,0xe8,0x8d,0x82,0x00,0xd0,
-0x08,0x00,0x22,0xf3,0x24,0x42,0x00,0x01,0x8d,0x82,0x00,0xd0,0x08,0x00,0x22,0xf3,
-0x24,0x42,0x00,0x02,0x10,0xa0,0xff,0xf9,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0x10,0xa2,0x00,0x0a,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0xa0,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x03,0x10,0xa2,0xff,0x9d,0x00,0x00,0x00,0x00,
-0x8d,0x82,0x00,0xd0,0x08,0x00,0x22,0xf3,0x24,0x42,0xff,0xd0,0x8d,0x82,0x00,0xd0,
-0x08,0x00,0x22,0xf3,0x24,0x42,0xff,0xfc,0x10,0xa0,0xff,0xeb,0x00,0x00,0x00,0x00,
-0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0xe5,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,
-0x10,0xa2,0xff,0x93,0x24,0x02,0x00,0x03,0x14,0xa2,0xff,0xdd,0x00,0x00,0x00,0x00,
-0x8d,0x82,0x00,0xd0,0x08,0x00,0x22,0xf3,0x24,0x42,0xff,0xf8,0x2d,0x42,0x00,0x19,
-0x14,0x40,0xff,0x7c,0x00,0x00,0x00,0x00,0x10,0xa0,0xff,0xdb,0x00,0x00,0x00,0x00,
-0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0xd5,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,
-0x10,0xa2,0xff,0x83,0x24,0x02,0x00,0x03,0x10,0xa2,0xff,0xf1,0x00,0x00,0x00,0x00,
-0x8d,0x82,0x00,0xd0,0x08,0x00,0x22,0xf3,0x24,0x42,0xff,0xf0,0x2d,0x42,0x00,0x1b,
-0x10,0x40,0xff,0xf1,0x00,0x00,0x00,0x00,0x10,0xa0,0xff,0xcb,0x00,0x00,0x00,0x00,
-0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0xc5,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,
-0x14,0xa2,0xff,0x6c,0x00,0x00,0x00,0x00,0x8d,0x82,0x00,0xd0,0x08,0x00,0x22,0xf3,
-0x24,0x42,0xff,0xf4,0x2d,0x42,0x00,0x1e,0x10,0x40,0xff,0xe3,0x00,0x00,0x00,0x00,
-0x10,0xa0,0xff,0xbd,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0x68,
-0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0xd6,0x00,0x00,0x00,0x00,0x08,0x00,0x22,0xee,
-0x24,0x02,0x00,0x03,0x2d,0x42,0x00,0x23,0x10,0x40,0xff,0xd7,0x00,0x00,0x00,0x00,
-0x10,0xa0,0xff,0xae,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0x5c,
-0x24,0x02,0x00,0x02,0x14,0xa2,0xff,0xb7,0x00,0x00,0x00,0x00,0x08,0x00,0x23,0x74,
-0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x25,0x10,0x40,0xff,0xcb,0x00,0x00,0x00,0x00,
-0x08,0x00,0x23,0x49,0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x16,0x14,0x40,0x00,0x0e,
-0x00,0x00,0x00,0x00,0x10,0xa0,0xff,0xa0,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0x10,0xa2,0xff,0x9a,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0x48,
-0x24,0x02,0x00,0x03,0x14,0xa2,0xff,0xb6,0x00,0x00,0x00,0x00,0x8d,0x82,0x00,0xd0,
-0x08,0x00,0x22,0xf3,0x24,0x42,0xff,0xfa,0x10,0xa0,0xff,0x93,0x00,0x00,0x00,0x00,
-0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0x8d,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,
-0x10,0xa2,0xff,0x3b,0x00,0x00,0x00,0x00,0x08,0x00,0x23,0x64,0x00,0x00,0x00,0x00,
-0x2d,0x42,0x00,0x17,0x14,0x40,0xff,0xac,0x00,0x00,0x00,0x00,0x08,0x00,0x23,0xa5,
-0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x19,0x10,0x40,0xff,0xe2,0x00,0x00,0x00,0x00,
-0x10,0xa0,0xff,0x81,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0x7b,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0x29,0x24,0x02,0x00,0x03,
-0x10,0xa2,0xff,0x97,0x00,0x00,0x00,0x00,0x08,0x00,0x22,0xf0,0x00,0x00,0x00,0x00,
-0x08,0x00,0x23,0xc2,0x2d,0x42,0x00,0x1b,0x2d,0x42,0x00,0x1e,0x10,0x40,0xff,0xde,
-0x00,0x00,0x00,0x00,0x10,0xa0,0xff,0x70,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0x10,0xa2,0xff,0x6a,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0x18,
-0x24,0x02,0x00,0x03,0x10,0xa2,0xff,0x96,0x00,0x00,0x00,0x00,0x08,0x00,0x22,0xf0,
-0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x23,0x14,0x40,0xff,0xf2,0x00,0x00,0x00,0x00,
-0x08,0x00,0x23,0x6a,0x00,0x00,0x00,0x00,0x08,0x00,0x23,0x68,0x2d,0x42,0x00,0x25,
-0x08,0x00,0x23,0x9e,0x2d,0x42,0x00,0x27,0x10,0xa0,0xff,0x5b,0x00,0x00,0x00,0x00,
-0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0x55,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,
-0x10,0xa2,0xff,0x03,0x24,0x02,0x00,0x03,0x14,0xa2,0xff,0x71,0x00,0x00,0x00,0x00,
-0x08,0x00,0x23,0x57,0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x27,0x14,0x40,0xff,0xad,
-0x00,0x00,0x00,0x00,0x08,0x00,0x23,0xea,0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x2a,
-0x14,0x40,0xff,0xd8,0x00,0x00,0x00,0x00,0x08,0x00,0x23,0x5a,0x00,0x00,0x00,0x00,
-0x2d,0x42,0x00,0x2c,0x14,0x40,0xff,0x78,0x00,0x00,0x00,0x00,0x08,0x00,0x22,0xe5,
-0x00,0x00,0x00,0x00,0x27,0xbd,0xff,0xe8,0x3c,0x02,0xb0,0x03,0xaf,0xbf,0x00,0x14,
-0xaf,0xb0,0x00,0x10,0x34,0x42,0x01,0x18,0x3c,0x03,0xb0,0x03,0x8c,0x50,0x00,0x00,
-0x34,0x63,0x01,0x2c,0x90,0x62,0x00,0x00,0x32,0x05,0x00,0x01,0xa3,0x82,0x80,0x10,
-0x14,0xa0,0x00,0x14,0x30,0x44,0x00,0xff,0x32,0x02,0x01,0x00,0x14,0x40,0x00,0x09,
-0x00,0x00,0x00,0x00,0x32,0x02,0x08,0x00,0x10,0x40,0x00,0x02,0x24,0x02,0x00,0x01,
-0xa3,0x82,0xbc,0x18,0x8f,0xbf,0x00,0x14,0x8f,0xb0,0x00,0x10,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x18,0x0c,0x00,0x05,0x37,0x00,0x00,0x00,0x00,0x26,0x02,0xff,0x00,
-0xa3,0x80,0xbc,0x18,0x3c,0x01,0xb0,0x03,0xac,0x22,0x01,0x18,0x08,0x00,0x24,0x16,
-0x32,0x02,0x08,0x00,0x0c,0x00,0x21,0x3f,0x00,0x00,0x00,0x00,0x26,0x02,0xff,0xff,
-0x3c,0x01,0xb0,0x03,0xac,0x22,0x01,0x18,0x08,0x00,0x24,0x13,0x32,0x02,0x01,0x00,
-0x27,0xbd,0xff,0xe0,0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0xd0,0xaf,0xbf,0x00,0x18,
-0x8c,0x43,0x00,0x00,0x3c,0x02,0x00,0x40,0x24,0x07,0x0f,0xff,0x00,0x03,0x33,0x02,
-0x00,0x03,0x2d,0x02,0x00,0x03,0x43,0x02,0x30,0x69,0x0f,0xff,0x00,0x62,0x18,0x24,
-0x30,0xa5,0x00,0x03,0x30,0xc6,0x00,0xff,0x10,0x60,0x00,0x08,0x31,0x08,0x00,0xff,
-0x01,0x00,0x30,0x21,0x0c,0x00,0x24,0xdf,0xaf,0xa9,0x00,0x10,0x8f,0xbf,0x00,0x18,
-0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x20,0x0c,0x00,0x25,0x31,
-0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,0x34,0x63,0x00,0xd4,0x08,0x00,0x24,0x3f,
-0xac,0x62,0x00,0x00,0x27,0xbd,0xff,0xc0,0x3c,0x02,0xb0,0x03,0xaf,0xbe,0x00,0x38,
-0xaf,0xb5,0x00,0x2c,0xaf,0xb1,0x00,0x1c,0xaf,0xb0,0x00,0x18,0xaf,0xbf,0x00,0x3c,
-0xaf,0xb7,0x00,0x34,0xaf,0xb6,0x00,0x30,0xaf,0xb4,0x00,0x28,0xaf,0xb3,0x00,0x24,
-0xaf,0xb2,0x00,0x20,0x34,0x42,0x00,0x3f,0x90,0x43,0x00,0x00,0x00,0x80,0x80,0x21,
-0x00,0x00,0xf0,0x21,0x00,0x00,0x88,0x21,0x10,0x60,0x00,0x76,0x00,0x00,0xa8,0x21,
-0x3c,0x01,0xb0,0x03,0xa0,0x20,0x00,0x3f,0x00,0x10,0x12,0x02,0x24,0x04,0x06,0x14,
-0x0c,0x00,0x06,0xd1,0x30,0x54,0x00,0x0f,0x24,0x04,0x06,0x14,0x0c,0x00,0x06,0xd1,
-0xaf,0xa2,0x00,0x10,0x3c,0x03,0x00,0xff,0x34,0x63,0xff,0xff,0x32,0x10,0x00,0x7f,
-0x00,0x43,0x10,0x24,0x00,0x10,0x86,0x00,0x02,0x02,0x80,0x25,0x02,0x00,0x28,0x21,
-0x24,0x04,0x06,0x14,0x3c,0x13,0xbf,0xff,0x0c,0x00,0x06,0xbf,0x3c,0x16,0xb0,0x03,
-0x00,0x00,0x90,0x21,0x3c,0x17,0x40,0x00,0x36,0x73,0xff,0xff,0x36,0xd6,0x00,0x3e,
-0x0c,0x00,0x06,0xd1,0x24,0x04,0x04,0x00,0x00,0x57,0x10,0x25,0x00,0x40,0x28,0x21,
-0x0c,0x00,0x06,0xbf,0x24,0x04,0x04,0x00,0x00,0x00,0x80,0x21,0x0c,0x00,0x25,0xf9,
-0x00,0x00,0x00,0x00,0x26,0x03,0x00,0x01,0x10,0x40,0x00,0x46,0x30,0x70,0x00,0xff,
-0x12,0x00,0xff,0xfa,0x00,0x00,0x00,0x00,0x0c,0x00,0x06,0xd1,0x24,0x04,0x04,0x00,
-0x00,0x53,0x10,0x24,0x00,0x40,0x28,0x21,0x0c,0x00,0x06,0xbf,0x24,0x04,0x04,0x00,
-0x24,0x02,0x00,0x01,0x12,0x82,0x00,0x37,0x00,0x00,0x00,0x00,0x12,0x80,0x00,0x35,
-0x00,0x00,0x00,0x00,0x32,0x31,0x00,0x7f,0x12,0x20,0x00,0x04,0x24,0x03,0x00,0x04,
-0x27,0xc2,0x00,0x01,0x30,0x5e,0x00,0xff,0x02,0xb1,0xa8,0x21,0x12,0x43,0x00,0x2a,
-0x3c,0x03,0xb0,0x03,0x02,0x43,0x10,0x21,0xa0,0x51,0x00,0x34,0x26,0x42,0x00,0x01,
-0x30,0x52,0x00,0xff,0x2e,0x43,0x00,0x05,0x14,0x60,0xff,0xd9,0x00,0x00,0x00,0x00,
-0x8f,0xa5,0x00,0x10,0x0c,0x00,0x06,0xbf,0x24,0x04,0x06,0x14,0x12,0xa0,0x00,0x0e,
-0x3c,0x02,0xb0,0x03,0x13,0xc0,0x00,0x0d,0x34,0x42,0x00,0x3c,0x00,0x15,0x10,0x40,
-0x00,0x55,0x10,0x21,0x00,0x02,0x10,0xc0,0x00,0x55,0x10,0x21,0x00,0x02,0xa8,0x80,
-0x02,0xbe,0x00,0x1b,0x17,0xc0,0x00,0x02,0x00,0x00,0x00,0x00,0x00,0x07,0x00,0x0d,
-0x00,0x00,0xa8,0x12,0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0x3c,0x3c,0x03,0xb0,0x03,
-0x3c,0x04,0xb0,0x03,0xa4,0x55,0x00,0x00,0x34,0x63,0x00,0x1c,0x34,0x84,0x00,0x1d,
-0x24,0x02,0x00,0x01,0xa0,0x60,0x00,0x00,0xa0,0x82,0x00,0x00,0x7b,0xbe,0x01,0xfc,
-0x7b,0xb6,0x01,0xbc,0x7b,0xb4,0x01,0x7c,0x7b,0xb2,0x01,0x3c,0x7b,0xb0,0x00,0xfc,
-0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x40,0xa2,0xd1,0x00,0x00,0x08,0x00,0x24,0x98,
-0x26,0x42,0x00,0x01,0x0c,0x00,0x06,0xd1,0x24,0x04,0x04,0xfc,0x08,0x00,0x24,0x8d,
-0x00,0x40,0x88,0x21,0x3c,0x03,0xb0,0x03,0x34,0x63,0x00,0x3c,0x3c,0x04,0xb0,0x03,
-0x3c,0x05,0xb0,0x03,0xa4,0x60,0x00,0x00,0x34,0x84,0x00,0x1c,0x34,0xa5,0x00,0x1d,
-0x24,0x02,0x00,0x02,0x24,0x03,0x00,0x01,0xa0,0x82,0x00,0x00,0x08,0x00,0x24,0xb7,
-0xa0,0xa3,0x00,0x00,0x0c,0x00,0x17,0x99,0x00,0x00,0x00,0x00,0x10,0x40,0xff,0x8b,
-0x00,0x10,0x12,0x02,0x3c,0x02,0xb0,0x03,0x3c,0x04,0xb0,0x03,0x34,0x42,0x00,0x3c,
-0x34,0x84,0x00,0x14,0x24,0x03,0x00,0x01,0xa4,0x40,0x00,0x00,0x3c,0x01,0xb0,0x03,
-0xa0,0x23,0x00,0x3f,0x08,0x00,0x24,0xb7,0xac,0x90,0x00,0x00,0x27,0xbd,0xff,0xd8,
-0xaf,0xb0,0x00,0x10,0x30,0xd0,0x00,0xff,0x2e,0x02,0x00,0x2e,0xaf,0xb2,0x00,0x18,
-0xaf,0xb1,0x00,0x14,0xaf,0xbf,0x00,0x20,0xaf,0xb3,0x00,0x1c,0x30,0xb1,0x00,0xff,
-0x14,0x40,0x00,0x06,0x00,0x80,0x90,0x21,0x8f,0xbf,0x00,0x20,0x7b,0xb2,0x00,0xfc,
-0x7b,0xb0,0x00,0xbc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x28,0x2e,0x13,0x00,0x10,
-0x24,0x05,0x00,0x14,0x0c,0x00,0x13,0xa4,0x24,0x06,0x01,0x07,0x12,0x60,0x00,0x38,
-0x02,0x00,0x30,0x21,0x8f,0xa2,0x00,0x38,0x30,0xc3,0x00,0x3f,0x3c,0x04,0xb0,0x09,
-0x00,0x02,0x14,0x00,0x00,0x43,0x30,0x25,0x34,0x84,0x01,0x60,0x90,0x82,0x00,0x00,
-0x00,0x00,0x00,0x00,0x14,0x40,0xff,0xfd,0x24,0x02,0x00,0x01,0x12,0x22,0x00,0x2a,
-0x2a,0x22,0x00,0x02,0x14,0x40,0x00,0x24,0x24,0x02,0x00,0x02,0x12,0x22,0x00,0x20,
-0x24,0x02,0x00,0x03,0x12,0x22,0x00,0x19,0x00,0x00,0x00,0x00,0x16,0x60,0xff,0xe2,
-0x24,0x02,0x00,0x01,0x12,0x22,0x00,0x13,0x2a,0x22,0x00,0x02,0x14,0x40,0x00,0x0d,
-0x24,0x02,0x00,0x02,0x12,0x22,0x00,0x09,0x24,0x02,0x00,0x03,0x16,0x22,0xff,0xda,
-0x00,0x00,0x00,0x00,0x24,0x04,0x08,0x4c,0x24,0x05,0xff,0xff,0x0c,0x00,0x13,0x5f,
-0x3c,0x06,0x0c,0xb8,0x08,0x00,0x24,0xea,0x00,0x00,0x00,0x00,0x08,0x00,0x25,0x12,
-0x24,0x04,0x08,0x48,0x16,0x20,0xff,0xd0,0x00,0x00,0x00,0x00,0x08,0x00,0x25,0x12,
-0x24,0x04,0x08,0x40,0x08,0x00,0x25,0x12,0x24,0x04,0x08,0x44,0x24,0x04,0x08,0x4c,
-0x0c,0x00,0x13,0x5f,0x24,0x05,0xff,0xff,0x08,0x00,0x25,0x07,0x00,0x00,0x00,0x00,
-0x08,0x00,0x25,0x20,0x24,0x04,0x08,0x48,0x16,0x20,0xff,0xe0,0x00,0x00,0x00,0x00,
-0x08,0x00,0x25,0x20,0x24,0x04,0x08,0x40,0x08,0x00,0x25,0x20,0x24,0x04,0x08,0x44,
-0x02,0x40,0x20,0x21,0x0c,0x00,0x25,0x71,0x02,0x20,0x28,0x21,0x08,0x00,0x24,0xf5,
-0x00,0x40,0x30,0x21,0x27,0xbd,0xff,0xd8,0x2c,0xc2,0x00,0x2e,0xaf,0xb2,0x00,0x18,
-0xaf,0xb1,0x00,0x14,0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x20,0xaf,0xb3,0x00,0x1c,
-0x00,0xc0,0x80,0x21,0x30,0xb1,0x00,0xff,0x00,0x80,0x90,0x21,0x14,0x40,0x00,0x07,
-0x00,0x00,0x18,0x21,0x8f,0xbf,0x00,0x20,0x7b,0xb2,0x00,0xfc,0x7b,0xb0,0x00,0xbc,
-0x00,0x60,0x10,0x21,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x28,0x2e,0x13,0x00,0x10,
-0x24,0x05,0x00,0x14,0x0c,0x00,0x13,0xa4,0x24,0x06,0x01,0x07,0x12,0x60,0x00,0x24,
-0x02,0x00,0x30,0x21,0x3c,0x03,0xb0,0x09,0x34,0x63,0x01,0x60,0x90,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x14,0x40,0xff,0xfd,0x30,0xc5,0x00,0x3f,0x0c,0x00,0x25,0xae,
-0x02,0x20,0x20,0x21,0x16,0x60,0x00,0x0a,0x00,0x40,0x80,0x21,0x24,0x02,0x00,0x01,
-0x12,0x22,0x00,0x15,0x2a,0x22,0x00,0x02,0x14,0x40,0x00,0x0f,0x24,0x02,0x00,0x02,
-0x12,0x22,0x00,0x0b,0x24,0x02,0x00,0x03,0x12,0x22,0x00,0x03,0x00,0x00,0x00,0x00,
-0x08,0x00,0x25,0x3d,0x02,0x00,0x18,0x21,0x24,0x04,0x08,0x4c,0x24,0x05,0xff,0xff,
-0x0c,0x00,0x13,0x5f,0x3c,0x06,0x0c,0xb8,0x08,0x00,0x25,0x3d,0x02,0x00,0x18,0x21,
-0x08,0x00,0x25,0x5f,0x24,0x04,0x08,0x48,0x16,0x20,0xff,0xf5,0x00,0x00,0x00,0x00,
-0x08,0x00,0x25,0x5f,0x24,0x04,0x08,0x40,0x08,0x00,0x25,0x5f,0x24,0x04,0x08,0x44,
-0x02,0x40,0x20,0x21,0x0c,0x00,0x25,0x71,0x02,0x20,0x28,0x21,0x08,0x00,0x25,0x49,
-0x00,0x40,0x30,0x21,0x27,0xbd,0xff,0xe8,0x2c,0xc2,0x00,0x1f,0xaf,0xb0,0x00,0x10,
-0xaf,0xbf,0x00,0x14,0x00,0xc0,0x80,0x21,0x14,0x40,0x00,0x1d,0x30,0xa5,0x00,0xff,
-0x24,0x02,0x00,0x01,0x10,0xa2,0x00,0x18,0x28,0xa2,0x00,0x02,0x14,0x40,0x00,0x12,
-0x24,0x02,0x00,0x02,0x10,0xa2,0x00,0x0e,0x24,0x02,0x00,0x03,0x10,0xa2,0x00,0x07,
-0x24,0x04,0x08,0x4c,0x26,0x10,0xff,0xe2,0x02,0x00,0x10,0x21,0x8f,0xbf,0x00,0x14,
-0x8f,0xb0,0x00,0x10,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x18,0x24,0x05,0xff,0xff,
-0x0c,0x00,0x13,0x5f,0x3c,0x06,0x0d,0xf8,0x08,0x00,0x25,0x82,0x26,0x10,0xff,0xe2,
-0x08,0x00,0x25,0x87,0x24,0x04,0x08,0x48,0x14,0xa0,0xff,0xf2,0x24,0x04,0x08,0x40,
-0x08,0x00,0x25,0x88,0x24,0x05,0xff,0xff,0x08,0x00,0x25,0x87,0x24,0x04,0x08,0x44,
-0x2c,0xc2,0x00,0x10,0x14,0x40,0xff,0xec,0x24,0x02,0x00,0x01,0x10,0xa2,0x00,0x14,
-0x28,0xa2,0x00,0x02,0x14,0x40,0x00,0x0e,0x24,0x02,0x00,0x02,0x10,0xa2,0x00,0x0a,
-0x24,0x02,0x00,0x03,0x10,0xa2,0x00,0x03,0x24,0x04,0x08,0x4c,0x08,0x00,0x25,0x82,
-0x26,0x10,0xff,0xf1,0x24,0x05,0xff,0xff,0x0c,0x00,0x13,0x5f,0x3c,0x06,0x0d,0xb8,
-0x08,0x00,0x25,0x82,0x26,0x10,0xff,0xf1,0x08,0x00,0x25,0xa1,0x24,0x04,0x08,0x48,
-0x14,0xa0,0xff,0xf6,0x24,0x04,0x08,0x40,0x08,0x00,0x25,0xa2,0x24,0x05,0xff,0xff,
-0x08,0x00,0x25,0xa1,0x24,0x04,0x08,0x44,0x27,0xbd,0xff,0xe8,0x30,0x84,0x00,0xff,
-0x24,0x02,0x00,0x01,0x10,0x82,0x00,0x39,0xaf,0xbf,0x00,0x10,0x28,0x82,0x00,0x02,
-0x14,0x40,0x00,0x27,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0x82,0x00,0x17,
-0x00,0xa0,0x30,0x21,0x24,0x02,0x00,0x03,0x10,0x82,0x00,0x05,0x24,0x04,0x08,0x3c,
-0x8f,0xbf,0x00,0x10,0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x18,
-0x0c,0x00,0x13,0x5f,0x3c,0x05,0x3f,0x00,0x24,0x04,0x08,0x3c,0x3c,0x05,0x80,0x00,
-0x0c,0x00,0x13,0x5f,0x00,0x00,0x30,0x21,0x24,0x04,0x08,0x3c,0x3c,0x05,0x80,0x00,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x01,0x24,0x04,0x08,0xac,0x0c,0x00,0x13,0x41,
-0x24,0x05,0x0f,0xff,0x08,0x00,0x25,0xbc,0x00,0x00,0x00,0x00,0x24,0x04,0x08,0x34,
-0x0c,0x00,0x13,0x5f,0x3c,0x05,0x3f,0x00,0x24,0x04,0x08,0x34,0x3c,0x05,0x80,0x00,
-0x0c,0x00,0x13,0x5f,0x00,0x00,0x30,0x21,0x24,0x04,0x08,0x34,0x3c,0x05,0x80,0x00,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x01,0x08,0x00,0x25,0xcb,0x24,0x04,0x08,0xa8,
-0x14,0x80,0xff,0xdf,0x00,0xa0,0x30,0x21,0x24,0x04,0x08,0x24,0x0c,0x00,0x13,0x5f,
-0x3c,0x05,0x3f,0x00,0x24,0x04,0x08,0x24,0x3c,0x05,0x80,0x00,0x0c,0x00,0x13,0x5f,
-0x00,0x00,0x30,0x21,0x24,0x04,0x08,0x24,0x3c,0x05,0x80,0x00,0x0c,0x00,0x13,0x5f,
-0x24,0x06,0x00,0x01,0x08,0x00,0x25,0xcb,0x24,0x04,0x08,0xa0,0x00,0xa0,0x30,0x21,
-0x24,0x04,0x08,0x2c,0x0c,0x00,0x13,0x5f,0x3c,0x05,0x3f,0x00,0x24,0x04,0x08,0x2c,
-0x3c,0x05,0x80,0x00,0x0c,0x00,0x13,0x5f,0x00,0x00,0x30,0x21,0x24,0x04,0x08,0x2c,
-0x3c,0x05,0x80,0x00,0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x01,0x08,0x00,0x25,0xcb,
-0x24,0x04,0x08,0xa4,0x3c,0x05,0x00,0x14,0x3c,0x02,0xb0,0x05,0x34,0x42,0x04,0x20,
-0x3c,0x06,0xc0,0x00,0x3c,0x03,0xb0,0x05,0x3c,0x04,0xb0,0x05,0x34,0xa5,0x17,0x09,
-0xac,0x45,0x00,0x00,0x34,0xc6,0x05,0x07,0x34,0x63,0x04,0x24,0x34,0x84,0x02,0x28,
-0x3c,0x07,0xb0,0x05,0x24,0x02,0x00,0x20,0xac,0x66,0x00,0x00,0x34,0xe7,0x04,0x50,
-0xa0,0x82,0x00,0x00,0x90,0xe2,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x03,
-0x10,0x40,0xff,0xfc,0x24,0x02,0x00,0x01,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,
-0x93,0x85,0x81,0xf1,0x24,0x02,0x00,0x01,0x14,0xa2,0x00,0x53,0x00,0x80,0x40,0x21,
-0x8c,0x89,0x00,0x04,0x3c,0x02,0xb0,0x01,0x01,0x22,0x30,0x21,0x8c,0xc3,0x00,0x04,
-0x3c,0x02,0x01,0x00,0x00,0x62,0x10,0x24,0x10,0x40,0x00,0x4b,0x30,0x62,0x00,0x08,
-0x10,0x45,0x00,0x59,0x00,0x00,0x00,0x00,0x94,0xc2,0x00,0x38,0x24,0x03,0x00,0xb4,
-0x30,0x44,0x00,0xff,0x10,0x83,0x00,0x61,0x24,0x02,0x00,0xc4,0x10,0x82,0x00,0x54,
-0x24,0x02,0x00,0x94,0x10,0x82,0x00,0x45,0x00,0x00,0x00,0x00,0x94,0xc2,0x00,0x38,
-0x00,0x00,0x00,0x00,0x30,0x47,0xff,0xff,0x30,0xe3,0x40,0xff,0x24,0x02,0x40,0x88,
-0x14,0x62,0x00,0x39,0x30,0xe3,0x03,0x00,0x24,0x02,0x03,0x00,0x10,0x62,0x00,0x38,
-0x00,0x00,0x00,0x00,0x94,0xc2,0x00,0x56,0x00,0x00,0x00,0x00,0x30,0x47,0xff,0xff,
-0x30,0xe2,0x00,0x80,0x14,0x40,0x00,0x30,0x3c,0x02,0xb0,0x01,0x01,0x22,0x30,0x21,
-0x94,0xc3,0x00,0x60,0x24,0x02,0x00,0x08,0x14,0x43,0x00,0x3b,0x00,0x00,0x00,0x00,
-0x90,0xc2,0x00,0x62,0x24,0x03,0x00,0x04,0x00,0x02,0x39,0x02,0x10,0xe3,0x00,0x15,
-0x24,0x02,0x00,0x06,0x14,0xe2,0x00,0x34,0x00,0x00,0x00,0x00,0x8d,0x05,0x01,0xac,
-0x94,0xc4,0x00,0x66,0x27,0x82,0x89,0x68,0x00,0x05,0x28,0x80,0x30,0x87,0xff,0xff,
-0x00,0xa2,0x28,0x21,0x00,0x07,0x1a,0x00,0x8c,0xa4,0x00,0x00,0x00,0x07,0x12,0x02,
-0x00,0x43,0x10,0x25,0x24,0x42,0x00,0x5e,0x24,0x03,0xc0,0x00,0x30,0x47,0xff,0xff,
-0x00,0x83,0x20,0x24,0x00,0x87,0x20,0x25,0xac,0xa4,0x00,0x00,0x08,0x00,0x26,0x76,
-0xad,0x07,0x00,0x10,0x8d,0x05,0x01,0xac,0x94,0xc4,0x00,0x64,0x27,0x82,0x89,0x68,
-0x00,0x05,0x28,0x80,0x30,0x87,0xff,0xff,0x00,0xa2,0x28,0x21,0x00,0x07,0x1a,0x00,
-0x8c,0xa4,0x00,0x00,0x00,0x07,0x12,0x02,0x00,0x43,0x10,0x25,0x24,0x42,0x00,0x36,
-0x3c,0x03,0xff,0xff,0x30,0x47,0xff,0xff,0x00,0x83,0x20,0x24,0x00,0x87,0x20,0x25,
-0xac,0xa4,0x00,0x00,0xad,0x07,0x00,0x10,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,
-0x94,0xc2,0x00,0x50,0x08,0x00,0x26,0x34,0x30,0x47,0xff,0xff,0x8d,0x04,0x01,0xac,
-0x27,0x83,0x89,0x68,0x00,0x04,0x20,0x80,0x00,0x83,0x20,0x21,0x8c,0x82,0x00,0x00,
-0x3c,0x03,0xff,0xff,0x00,0x43,0x10,0x24,0x34,0x42,0x00,0x2e,0xac,0x82,0x00,0x00,
-0x24,0x03,0x00,0x2e,0xad,0x03,0x00,0x10,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,
-0x8d,0x04,0x01,0xac,0x27,0x83,0x89,0x68,0x00,0x04,0x20,0x80,0x00,0x83,0x20,0x21,
-0x8c,0x82,0x00,0x00,0x3c,0x03,0xff,0xff,0x00,0x43,0x10,0x24,0x34,0x42,0x00,0x0e,
-0x24,0x03,0x00,0x0e,0x08,0x00,0x26,0x75,0xac,0x82,0x00,0x00,0x8d,0x04,0x01,0xac,
-0x27,0x83,0x89,0x68,0x00,0x04,0x20,0x80,0x00,0x83,0x20,0x21,0x8c,0x82,0x00,0x00,
-0x3c,0x03,0xff,0xff,0x00,0x43,0x10,0x24,0x34,0x42,0x00,0x14,0x24,0x03,0x00,0x14,
-0x08,0x00,0x26,0x75,0xac,0x82,0x00,0x00,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,
-0x30,0xc6,0x00,0xff,0x00,0x06,0x48,0x40,0x01,0x26,0x10,0x21,0x00,0x02,0x10,0x80,
-0x27,0x8b,0xbc,0x30,0x27,0x83,0xbc,0x36,0x00,0x4b,0x40,0x21,0x00,0x43,0x10,0x21,
-0x94,0x47,0x00,0x00,0x30,0xa2,0x3f,0xff,0x10,0xe2,0x00,0x29,0x30,0x8a,0xff,0xff,
-0x95,0x02,0x00,0x02,0x24,0x03,0x00,0x01,0x00,0x02,0x11,0x82,0x30,0x42,0x00,0x01,
-0x10,0x43,0x00,0x18,0x00,0x00,0x00,0x00,0x01,0x26,0x10,0x21,0x00,0x02,0x10,0x80,
-0x00,0x4b,0x30,0x21,0x94,0xc4,0x00,0x02,0x27,0x83,0xbc,0x36,0x27,0x85,0xbc,0x34,
-0x00,0x45,0x28,0x21,0x30,0x84,0xff,0xdf,0x00,0x43,0x10,0x21,0xa4,0xc4,0x00,0x02,
-0xa4,0x40,0x00,0x00,0xa4,0xa0,0x00,0x00,0x94,0xc3,0x00,0x02,0x3c,0x04,0xb0,0x01,
-0x01,0x44,0x20,0x21,0x30,0x63,0xff,0xbf,0xa4,0xc3,0x00,0x02,0xa0,0xc0,0x00,0x00,
-0x8c,0x82,0x00,0x04,0x24,0x03,0xf0,0xff,0x00,0x43,0x10,0x24,0x03,0xe0,0x00,0x08,
-0xac,0x82,0x00,0x04,0x24,0x02,0xc0,0x00,0x91,0x04,0x00,0x01,0x00,0xa2,0x10,0x24,
-0x00,0x47,0x28,0x25,0x3c,0x03,0xb0,0x01,0x24,0x02,0x00,0x02,0x14,0x82,0xff,0xe2,
-0x01,0x43,0x18,0x21,0xac,0x65,0x00,0x00,0x08,0x00,0x26,0xa3,0x01,0x26,0x10,0x21,
-0x08,0x00,0x26,0xa3,0x01,0x26,0x10,0x21,0x93,0x83,0x81,0xf1,0x24,0x02,0x00,0x01,
-0x14,0x62,0x00,0x0d,0x3c,0x02,0xb0,0x01,0x8c,0x84,0x00,0x04,0x3c,0x06,0xb0,0x09,
-0x00,0x82,0x20,0x21,0x8c,0x85,0x00,0x08,0x8c,0x83,0x00,0x04,0x3c,0x02,0x01,0x00,
-0x34,0xc6,0x01,0x00,0x00,0x62,0x18,0x24,0x14,0x60,0x00,0x05,0x30,0xa5,0x20,0x00,
-0x24,0x02,0x00,0x06,0xa0,0xc2,0x00,0x00,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,
-0x3c,0x03,0xb0,0x09,0x10,0xa0,0xff,0xfc,0x34,0x63,0x01,0x00,0x24,0x02,0x00,0x0e,
-0x08,0x00,0x26,0xd6,0xa0,0x62,0x00,0x00,0x3c,0x02,0xb0,0x01,0x30,0xa5,0xff,0xff,
-0x00,0xa2,0x28,0x21,0x8c,0xa3,0x00,0x00,0x3c,0x02,0x10,0x00,0x00,0x80,0x30,0x21,
-0x00,0x62,0x18,0x24,0x8c,0xa2,0x00,0x04,0x10,0x60,0x00,0x04,0x00,0x00,0x00,0x00,
-0x30,0x42,0x80,0x00,0x10,0x40,0x00,0x13,0x00,0x00,0x00,0x00,0x8c,0xc2,0x01,0xa8,
-0x00,0x00,0x00,0x00,0x24,0x44,0x00,0x01,0x28,0x83,0x00,0x00,0x24,0x42,0x00,0x40,
-0x00,0x83,0x10,0x0a,0x93,0x83,0x81,0xf0,0x00,0x02,0x11,0x83,0x00,0x02,0x11,0x80,
-0x00,0x82,0x20,0x23,0x24,0x63,0xff,0xff,0xac,0xc4,0x01,0xa8,0xa3,0x83,0x81,0xf0,
-0x8c,0xc4,0x01,0xac,0x8c,0xc2,0x01,0xa8,0x00,0x00,0x00,0x00,0x00,0x44,0x10,0x26,
-0x00,0x02,0x10,0x2b,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,
-0x34,0x63,0x00,0x73,0x90,0x62,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x01,
-0x14,0x40,0x00,0x04,0x00,0x00,0x00,0x00,0xa3,0x80,0x81,0xf1,0x03,0xe0,0x00,0x08,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,0xa3,0x82,0x81,0xf1,0x03,0xe0,0x00,0x08,
-0x00,0x00,0x00,0x00,0x8c,0x82,0x00,0x04,0x3c,0x05,0xb0,0x01,0x00,0x80,0x50,0x21,
-0x00,0x45,0x10,0x21,0x8c,0x43,0x00,0x04,0x24,0x02,0x00,0x05,0x00,0x03,0x1a,0x02,
-0x30,0x69,0x00,0x0f,0x11,0x22,0x00,0x0b,0x24,0x02,0x00,0x07,0x11,0x22,0x00,0x09,
-0x24,0x02,0x00,0x0a,0x11,0x22,0x00,0x07,0x24,0x02,0x00,0x0b,0x11,0x22,0x00,0x05,
-0x24,0x02,0x00,0x01,0x93,0x83,0x81,0xf0,0x3c,0x04,0xb0,0x06,0x10,0x62,0x00,0x03,
-0x34,0x84,0x80,0x18,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x8c,0x82,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x02,0x17,0x02,0x14,0x40,0xff,0xfa,0x00,0x00,0x00,0x00,
-0x8d,0x43,0x01,0xa8,0x27,0x82,0x89,0x68,0x00,0x03,0x18,0x80,0x00,0x6a,0x20,0x21,
-0x8c,0x87,0x00,0xa8,0x00,0x62,0x18,0x21,0x8c,0x68,0x00,0x00,0x00,0xe5,0x28,0x21,
-0x8c,0xa9,0x00,0x00,0x3c,0x02,0xff,0xff,0x27,0x83,0x8a,0x68,0x01,0x22,0x10,0x24,
-0x00,0x48,0x10,0x25,0xac,0xa2,0x00,0x00,0x8d,0x44,0x01,0xa8,0x00,0x07,0x30,0xc2,
-0x3c,0x02,0x00,0x80,0x00,0x04,0x20,0x80,0x00,0x83,0x20,0x21,0x00,0x06,0x32,0x00,
-0x8c,0xa9,0x00,0x04,0x00,0xc2,0x30,0x25,0x8c,0x82,0x00,0x00,0x3c,0x03,0x80,0x00,
-0x01,0x22,0x10,0x25,0x00,0x43,0x10,0x25,0xac,0xa2,0x00,0x04,0xaf,0x87,0xbc,0x20,
-0x8c,0xa2,0x00,0x00,0x00,0x00,0x00,0x00,0xaf,0x82,0xbc,0x28,0x8c,0xa3,0x00,0x04,
-0x3c,0x01,0xb0,0x07,0xac,0x26,0x80,0x18,0x8d,0x42,0x01,0xa8,0xaf,0x83,0xbc,0x24,
-0x93,0x85,0x81,0xf0,0x24,0x44,0x00,0x01,0x28,0x83,0x00,0x00,0x24,0x42,0x00,0x40,
-0x00,0x83,0x10,0x0a,0x00,0x02,0x11,0x83,0x00,0x02,0x11,0x80,0x24,0xa5,0xff,0xff,
-0x00,0x82,0x20,0x23,0xad,0x44,0x01,0xa8,0xa3,0x85,0x81,0xf0,0x08,0x00,0x27,0x21,
-0x00,0x00,0x00,0x00,0x3c,0x05,0xb0,0x03,0x3c,0x02,0x80,0x01,0x34,0xa5,0x00,0x20,
-0x24,0x42,0x9d,0x64,0xac,0xa2,0x00,0x00,0x24,0x02,0x00,0x02,0x24,0x03,0x00,0x20,
-0xac,0x82,0x00,0x64,0x3c,0x02,0x80,0x01,0xac,0x83,0x00,0x60,0xac,0x80,0x00,0x00,
-0xac,0x80,0x00,0x04,0xac,0x80,0x00,0x08,0xac,0x80,0x00,0x4c,0xac,0x80,0x00,0x50,
-0xac,0x80,0x00,0x54,0xac,0x80,0x00,0x0c,0xac,0x80,0x00,0x58,0xa0,0x80,0x00,0x5c,
-0x24,0x42,0x9e,0x28,0x24,0x83,0x00,0x68,0x24,0x05,0x00,0x0f,0x24,0xa5,0xff,0xff,
-0xac,0x62,0x00,0x00,0x04,0xa1,0xff,0xfd,0x24,0x63,0x00,0x04,0x3c,0x02,0x80,0x01,
-0x24,0x42,0x9f,0x10,0xac,0x82,0x00,0x78,0x3c,0x03,0x80,0x01,0x3c,0x02,0x80,0x01,
-0x24,0x63,0xa0,0x9c,0x24,0x42,0xa0,0x08,0xac,0x83,0x00,0x88,0xac,0x82,0x00,0x98,
-0x3c,0x03,0x80,0x01,0x3c,0x02,0x80,0x01,0x24,0x63,0xa1,0x44,0x24,0x42,0xa2,0x5c,
-0xac,0x83,0x00,0xa0,0xac,0x82,0x00,0xa4,0xa0,0x80,0x01,0xba,0xac,0x80,0x01,0xa8,
-0xac,0x80,0x01,0xac,0xac,0x80,0x01,0xb0,0xac,0x80,0x01,0xb4,0xa0,0x80,0x01,0xb8,
-0x03,0xe0,0x00,0x08,0xa0,0x80,0x01,0xb9,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x01,
-0x34,0x63,0x00,0x20,0x24,0x42,0x9e,0x28,0x03,0xe0,0x00,0x08,0xac,0x62,0x00,0x00,
-0x3c,0x02,0xb0,0x03,0x3c,0x03,0x80,0x01,0x34,0x42,0x00,0x20,0x24,0x63,0x9e,0x40,
-0xac,0x43,0x00,0x00,0x8c,0x82,0x00,0x10,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x11,
-0x00,0x80,0x28,0x21,0x8c,0x82,0x00,0x14,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x0d,
-0x00,0x00,0x00,0x00,0x8c,0x84,0x00,0x10,0x8c,0xa3,0x00,0x14,0x8c,0xa2,0x00,0x04,
-0x00,0x83,0x20,0x21,0x00,0x44,0x10,0x21,0x30,0x43,0x00,0xff,0x00,0x03,0x18,0x2b,
-0x00,0x02,0x12,0x02,0x00,0x43,0x10,0x21,0x00,0x02,0x12,0x00,0x30,0x42,0x3f,0xff,
-0xac,0xa2,0x00,0x04,0xac,0xa0,0x00,0x00,0xac,0xa0,0x00,0x4c,0xac,0xa0,0x00,0x50,
-0xac,0xa0,0x00,0x54,0x03,0xe0,0x00,0x08,0xac,0xa0,0x00,0x0c,0x3c,0x03,0xb0,0x03,
-0x3c,0x02,0x80,0x01,0x34,0x63,0x00,0x20,0x24,0x42,0x9e,0xbc,0xac,0x62,0x00,0x00,
-0x8c,0x86,0x00,0x04,0x3c,0x02,0xb0,0x01,0x24,0x03,0x00,0x01,0x00,0xc2,0x10,0x21,
-0x8c,0x45,0x00,0x00,0xac,0x83,0x00,0x4c,0x00,0x05,0x14,0x02,0x30,0xa3,0x3f,0xff,
-0x30,0x42,0x00,0xff,0xac,0x83,0x00,0x10,0xac,0x82,0x00,0x14,0x8c,0x83,0x00,0x14,
-0xac,0x85,0x00,0x40,0x00,0xc3,0x30,0x21,0x03,0xe0,0x00,0x08,0xac,0x86,0x00,0x08,
-0x3c,0x02,0xb0,0x03,0x3c,0x03,0x80,0x01,0x27,0xbd,0xff,0xe8,0x34,0x42,0x00,0x20,
-0x24,0x63,0x9f,0x10,0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x14,0xac,0x43,0x00,0x00,
-0x8c,0x82,0x00,0x4c,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x0a,0x00,0x80,0x80,0x21,
-0xae,0x00,0x00,0x00,0xae,0x00,0x00,0x4c,0xae,0x00,0x00,0x50,0xae,0x00,0x00,0x54,
-0xae,0x00,0x00,0x0c,0x8f,0xbf,0x00,0x14,0x8f,0xb0,0x00,0x10,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x18,0x0c,0x00,0x27,0xaf,0x00,0x00,0x00,0x00,0x08,0x00,0x27,0xd1,
-0xae,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,0x3c,0x03,0x80,0x01,0x27,0xbd,0xff,0xe8,
-0x34,0x42,0x00,0x20,0x24,0x63,0x9f,0x74,0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x14,
-0xac,0x43,0x00,0x00,0x8c,0x82,0x00,0x4c,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x16,
-0x00,0x80,0x80,0x21,0x8e,0x03,0x00,0x08,0x3c,0x02,0xb0,0x01,0x8e,0x04,0x00,0x44,
-0x00,0x62,0x18,0x21,0x90,0x65,0x00,0x00,0x24,0x02,0x00,0x01,0xae,0x02,0x00,0x50,
-0x30,0xa3,0x00,0xff,0x00,0x03,0x10,0x82,0x00,0x04,0x23,0x02,0x30,0x84,0x00,0x0f,
-0x30,0x42,0x00,0x03,0x00,0x03,0x19,0x02,0xae,0x04,0x00,0x34,0xae,0x02,0x00,0x2c,
-0xae,0x03,0x00,0x30,0xa2,0x05,0x00,0x48,0x8f,0xbf,0x00,0x14,0x8f,0xb0,0x00,0x10,
-0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x18,0x0c,0x00,0x27,0xaf,0x00,0x00,0x00,0x00,
-0x08,0x00,0x27,0xe9,0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,0x3c,0x03,0x80,0x01,
-0x27,0xbd,0xff,0xe8,0x34,0x42,0x00,0x20,0x24,0x63,0xa0,0x08,0xaf,0xb0,0x00,0x10,
-0xaf,0xbf,0x00,0x14,0xac,0x43,0x00,0x00,0x8c,0x82,0x00,0x50,0x00,0x00,0x00,0x00,
-0x10,0x40,0x00,0x16,0x00,0x80,0x80,0x21,0x92,0x03,0x00,0x44,0x8e,0x02,0x00,0x40,
-0x83,0x85,0x8b,0xd4,0x92,0x04,0x00,0x41,0x30,0x63,0x00,0x01,0x00,0x02,0x16,0x02,
-0xae,0x04,0x00,0x14,0x00,0x00,0x30,0x21,0xae,0x02,0x00,0x18,0x10,0xa0,0x00,0x04,
-0xae,0x03,0x00,0x3c,0x10,0x60,0x00,0x03,0x24,0x02,0x00,0x01,0x24,0x06,0x00,0x01,
-0x24,0x02,0x00,0x01,0xa3,0x86,0x8b,0xd4,0x8f,0xbf,0x00,0x14,0xae,0x02,0x00,0x54,
-0x8f,0xb0,0x00,0x10,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x18,0x0c,0x00,0x27,0xdd,
-0x00,0x00,0x00,0x00,0x08,0x00,0x28,0x0e,0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,
-0x3c,0x03,0x80,0x01,0x27,0xbd,0xff,0xe8,0x34,0x42,0x00,0x20,0x24,0x63,0xa0,0x9c,
-0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x14,0xac,0x43,0x00,0x00,0x8c,0x82,0x00,0x50,
-0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x1b,0x00,0x80,0x80,0x21,0x3c,0x02,0xb0,0x03,
-0x8c,0x42,0x00,0x00,0x92,0x04,0x00,0x44,0x8e,0x03,0x00,0x40,0x83,0x86,0x8b,0xd4,
-0x92,0x05,0x00,0x41,0x30,0x42,0x08,0x00,0x30,0x84,0x00,0x01,0x00,0x02,0x12,0xc2,
-0x00,0x03,0x1e,0x02,0x00,0x82,0x20,0x25,0xae,0x05,0x00,0x14,0x00,0x00,0x38,0x21,
-0xae,0x03,0x00,0x18,0x10,0xc0,0x00,0x04,0xae,0x04,0x00,0x3c,0x10,0x80,0x00,0x03,
-0x24,0x02,0x00,0x01,0x24,0x07,0x00,0x01,0x24,0x02,0x00,0x01,0xa3,0x87,0x8b,0xd4,
-0x8f,0xbf,0x00,0x14,0xae,0x02,0x00,0x54,0x8f,0xb0,0x00,0x10,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x18,0x0c,0x00,0x27,0xdd,0x00,0x00,0x00,0x00,0x08,0x00,0x28,0x33,
-0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,0x3c,0x03,0x80,0x01,0x27,0xbd,0xff,0xe8,
-0x34,0x42,0x00,0x20,0x24,0x63,0xa1,0x44,0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x14,
-0xac,0x43,0x00,0x00,0x8c,0x82,0x00,0x54,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x37,
-0x00,0x80,0x80,0x21,0x8e,0x04,0x00,0x04,0x8e,0x03,0x00,0x44,0x3c,0x02,0x80,0x00,
-0x3c,0x05,0xb0,0x01,0x34,0x42,0x00,0x10,0x00,0x85,0x20,0x21,0x00,0x62,0x18,0x25,
-0xac,0x83,0x00,0x04,0x8e,0x02,0x00,0x04,0x8e,0x03,0x01,0xac,0x02,0x00,0x20,0x21,
-0x00,0x45,0x10,0x21,0x8c,0x46,0x00,0x00,0x00,0x03,0x18,0x80,0x27,0x82,0x89,0x68,
-0x00,0x62,0x18,0x21,0xac,0x66,0x00,0x00,0x8e,0x02,0x00,0x04,0x8e,0x03,0x01,0xac,
-0x00,0x45,0x10,0x21,0x8c,0x46,0x00,0x04,0x00,0x03,0x18,0x80,0x27,0x82,0x8a,0x68,
-0x00,0x62,0x18,0x21,0x0c,0x00,0x26,0x10,0xac,0x66,0x00,0x00,0x8e,0x03,0x01,0xac,
-0x8e,0x07,0x00,0x04,0x3c,0x06,0xb0,0x03,0x24,0x65,0x00,0x01,0x28,0xa4,0x00,0x00,
-0x24,0x62,0x00,0x40,0x00,0xa4,0x10,0x0a,0x00,0x02,0x11,0x83,0x00,0x02,0x11,0x80,
-0x00,0x03,0x18,0x80,0x00,0xa2,0x28,0x23,0x00,0x70,0x18,0x21,0xae,0x05,0x01,0xac,
-0xac,0x67,0x00,0xa8,0x34,0xc6,0x00,0x30,0x8c,0xc3,0x00,0x00,0x93,0x82,0x81,0xf0,
-0x02,0x00,0x20,0x21,0x24,0x63,0x00,0x01,0x24,0x42,0x00,0x01,0xac,0xc3,0x00,0x00,
-0xa3,0x82,0x81,0xf0,0x0c,0x00,0x27,0x90,0x00,0x00,0x00,0x00,0x8f,0xbf,0x00,0x14,
-0x8f,0xb0,0x00,0x10,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x18,0x0c,0x00,0x28,0x27,
-0x00,0x00,0x00,0x00,0x08,0x00,0x28,0x5d,0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,
-0x3c,0x03,0x80,0x01,0x27,0xbd,0xff,0xe8,0x34,0x42,0x00,0x20,0x24,0x63,0xa2,0x5c,
-0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x14,0xac,0x43,0x00,0x00,0x8c,0x82,0x00,0x54,
-0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x37,0x00,0x80,0x80,0x21,0x8e,0x04,0x00,0x04,
-0x8e,0x03,0x00,0x44,0x3c,0x02,0x80,0x00,0x3c,0x05,0xb0,0x01,0x34,0x42,0x00,0x10,
-0x00,0x85,0x20,0x21,0x00,0x62,0x18,0x25,0xac,0x83,0x00,0x04,0x8e,0x02,0x00,0x04,
-0x8e,0x03,0x01,0xac,0x02,0x00,0x20,0x21,0x00,0x45,0x10,0x21,0x8c,0x46,0x00,0x00,
-0x00,0x03,0x18,0x80,0x27,0x82,0x89,0x68,0x00,0x62,0x18,0x21,0xac,0x66,0x00,0x00,
-0x8e,0x02,0x00,0x04,0x8e,0x03,0x01,0xac,0x00,0x45,0x10,0x21,0x8c,0x46,0x00,0x04,
-0x00,0x03,0x18,0x80,0x27,0x82,0x8a,0x68,0x00,0x62,0x18,0x21,0x0c,0x00,0x26,0x10,
-0xac,0x66,0x00,0x00,0x8e,0x03,0x01,0xac,0x8e,0x07,0x00,0x04,0x3c,0x06,0xb0,0x03,
-0x24,0x65,0x00,0x01,0x28,0xa4,0x00,0x00,0x24,0x62,0x00,0x40,0x00,0xa4,0x10,0x0a,
-0x00,0x02,0x11,0x83,0x00,0x02,0x11,0x80,0x00,0x03,0x18,0x80,0x00,0xa2,0x28,0x23,
-0x00,0x70,0x18,0x21,0xae,0x05,0x01,0xac,0xac,0x67,0x00,0xa8,0x34,0xc6,0x00,0x30,
-0x8c,0xc3,0x00,0x00,0x93,0x82,0x81,0xf0,0x02,0x00,0x20,0x21,0x24,0x63,0x00,0x01,
-0x24,0x42,0x00,0x01,0xac,0xc3,0x00,0x00,0xa3,0x82,0x81,0xf0,0x0c,0x00,0x27,0x90,
-0x00,0x00,0x00,0x00,0x8f,0xbf,0x00,0x14,0x8f,0xb0,0x00,0x10,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x18,0x0c,0x00,0x28,0x27,0x00,0x00,0x00,0x00,0x08,0x00,0x28,0xa3,
-0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,0x3c,0x03,0x80,0x01,0x34,0x42,0x00,0x20,
-0x24,0x63,0xa3,0x74,0x27,0xbd,0xff,0xe0,0xac,0x43,0x00,0x00,0x3c,0x02,0x80,0x01,
-0xaf,0xb2,0x00,0x18,0xaf,0xb1,0x00,0x14,0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x1c,
-0x00,0x80,0x80,0x21,0x24,0x52,0x9e,0x28,0x00,0x00,0x88,0x21,0x3c,0x03,0xb0,0x09,
-0x34,0x63,0x00,0x06,0x8e,0x06,0x00,0x04,0x90,0x62,0x00,0x00,0x00,0x06,0x22,0x02,
-0x00,0x44,0x10,0x23,0x24,0x44,0x00,0x40,0x28,0x83,0x00,0x00,0x24,0x42,0x00,0x7f,
-0x00,0x83,0x10,0x0a,0x00,0x02,0x11,0x83,0x00,0x02,0x11,0x80,0x24,0x84,0xff,0xff,
-0x10,0x44,0x00,0x68,0x00,0x00,0x28,0x21,0x3c,0x02,0xb0,0x01,0x00,0xc2,0x10,0x21,
-0x8c,0x44,0x00,0x04,0x3c,0x03,0x7c,0x00,0x34,0x63,0x00,0xf0,0x00,0x83,0x18,0x24,
-0xae,0x04,0x00,0x44,0x8c,0x44,0x00,0x00,0x10,0x60,0x00,0x69,0x00,0x00,0x38,0x21,
-0x3c,0x09,0xb0,0x03,0x3c,0x06,0x7c,0x00,0x35,0x29,0x00,0x99,0x3c,0x0a,0xb0,0x01,
-0x24,0x08,0x00,0x40,0x34,0xc6,0x00,0xf0,0x3c,0x0b,0xff,0xff,0x3c,0x0c,0x28,0x38,
-0x16,0x20,0x00,0x06,0x24,0xa5,0x00,0x01,0x93,0x82,0x81,0xf6,0x24,0x11,0x00,0x01,
-0x24,0x42,0x00,0x01,0xa1,0x22,0x00,0x00,0xa3,0x82,0x81,0xf6,0x8e,0x02,0x00,0x04,
-0x24,0x07,0x00,0x01,0x24,0x42,0x01,0x00,0x30,0x42,0x3f,0xff,0xae,0x02,0x00,0x04,
-0x00,0x4a,0x10,0x21,0x8c,0x43,0x00,0x04,0x00,0x00,0x00,0x00,0xae,0x03,0x00,0x44,
-0x8c,0x44,0x00,0x00,0x10,0xa8,0x00,0x2d,0x00,0x66,0x18,0x24,0x14,0x60,0xff,0xec,
-0x00,0x8b,0x10,0x24,0x14,0x4c,0xff,0xea,0x24,0x02,0x00,0x01,0x10,0xe2,0x00,0x2f,
-0x3c,0x03,0xb0,0x09,0x8e,0x02,0x00,0x44,0x8e,0x04,0x00,0x60,0x00,0x02,0x1e,0x42,
-0x00,0x02,0x12,0x02,0x30,0x42,0x00,0x0f,0x30,0x63,0x00,0x01,0xae,0x02,0x00,0x00,
-0x10,0x44,0x00,0x1a,0xae,0x03,0x00,0x58,0x8e,0x02,0x00,0x64,0x8e,0x04,0x00,0x58,
-0x00,0x00,0x00,0x00,0x10,0x82,0x00,0x05,0x00,0x00,0x00,0x00,0xae,0x00,0x00,0x4c,
-0xae,0x00,0x00,0x50,0xae,0x00,0x00,0x54,0xae,0x00,0x00,0x0c,0x8e,0x03,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x03,0x10,0x80,0x00,0x50,0x10,0x21,0x8c,0x42,0x00,0x68,
-0x00,0x00,0x00,0x00,0x10,0x52,0x00,0x06,0x00,0x00,0x00,0x00,0x00,0x40,0xf8,0x09,
-0x02,0x00,0x20,0x21,0x8e,0x04,0x00,0x58,0x8e,0x03,0x00,0x00,0x00,0x00,0x00,0x00,
-0xae,0x03,0x00,0x60,0x08,0x00,0x28,0xeb,0xae,0x04,0x00,0x64,0x8e,0x02,0x00,0x64,
-0x00,0x00,0x00,0x00,0x14,0x62,0xff,0xe5,0x00,0x00,0x00,0x00,0x7a,0x02,0x0d,0x7c,
-0x8f,0xbf,0x00,0x1c,0x8f,0xb2,0x00,0x18,0x7b,0xb0,0x00,0xbc,0x00,0x43,0x10,0x26,
-0x00,0x02,0x10,0x2b,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x20,0x34,0x63,0x00,0x06,
-0x8e,0x04,0x00,0x04,0x90,0x62,0x00,0x00,0x00,0x04,0x22,0x02,0x00,0x44,0x10,0x23,
-0x24,0x44,0x00,0x40,0x28,0x83,0x00,0x00,0x24,0x42,0x00,0x7f,0x00,0x83,0x10,0x0a,
-0x00,0x02,0x11,0x83,0x00,0x02,0x11,0x80,0x00,0x82,0x20,0x23,0x14,0x87,0xff,0xc5,
-0x00,0x00,0x00,0x00,0x8e,0x03,0x00,0x00,0x00,0x00,0x00,0x00,0x2c,0x62,0x00,0x03,
-0x14,0x40,0x00,0x05,0x24,0x02,0x00,0x0d,0x10,0x62,0x00,0x03,0x24,0x02,0x00,0x01,
-0x08,0x00,0x29,0x4b,0xa2,0x02,0x00,0x5c,0x08,0x00,0x29,0x4b,0xa2,0x00,0x00,0x5c,
-0x3c,0x02,0xff,0xff,0x00,0x82,0x10,0x24,0x3c,0x03,0x28,0x38,0x14,0x43,0xff,0x94,
-0x24,0x02,0x00,0x01,0x08,0x00,0x29,0x23,0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,
-0x3c,0x03,0x80,0x01,0x34,0x42,0x00,0x20,0x24,0x63,0xa5,0xcc,0xac,0x43,0x00,0x00,
-0x8c,0x83,0x01,0xa8,0x8c,0x82,0x01,0xac,0x00,0x80,0x40,0x21,0x10,0x62,0x00,0x20,
-0x00,0x00,0x20,0x21,0x93,0x82,0x81,0xf1,0x00,0x03,0x28,0x80,0x3c,0x07,0xb0,0x06,
-0x00,0xa8,0x18,0x21,0x24,0x04,0x00,0x01,0x8c,0x66,0x00,0xa8,0x10,0x44,0x00,0x1c,
-0x34,0xe7,0x80,0x18,0x3c,0x05,0xb0,0x01,0xaf,0x86,0xbc,0x20,0x00,0xc5,0x28,0x21,
-0x8c,0xa3,0x00,0x00,0x00,0x06,0x20,0xc2,0x3c,0x02,0x00,0x80,0x00,0x04,0x22,0x00,
-0x00,0x82,0x20,0x25,0xaf,0x83,0xbc,0x28,0x8c,0xa2,0x00,0x04,0xac,0xe4,0x00,0x00,
-0x8d,0x03,0x01,0xa8,0xaf,0x82,0xbc,0x24,0x24,0x64,0x00,0x01,0x04,0x80,0x00,0x0a,
-0x00,0x80,0x10,0x21,0x00,0x02,0x11,0x83,0x8d,0x03,0x01,0xac,0x00,0x02,0x11,0x80,
-0x00,0x82,0x10,0x23,0x00,0x43,0x18,0x26,0xad,0x02,0x01,0xa8,0x00,0x03,0x20,0x2b,
-0x03,0xe0,0x00,0x08,0x00,0x80,0x10,0x21,0x08,0x00,0x29,0x95,0x24,0x62,0x00,0x40,
-0x27,0x82,0x89,0x68,0x00,0x06,0x20,0xc2,0x00,0x04,0x22,0x00,0x00,0xa2,0x48,0x21,
-0x3c,0x02,0x00,0x80,0x00,0x82,0x58,0x25,0x93,0x82,0x81,0xf0,0x3c,0x0a,0xb0,0x06,
-0x3c,0x03,0xb0,0x01,0x2c,0x42,0x00,0x02,0x00,0xc3,0x38,0x21,0x35,0x4a,0x80,0x18,
-0x14,0x40,0xff,0xef,0x00,0x00,0x20,0x21,0x8c,0xe5,0x00,0x00,0x8d,0x23,0x00,0x00,
-0x24,0x02,0xc0,0x00,0x00,0xa2,0x10,0x24,0x00,0x43,0x10,0x25,0xac,0xe2,0x00,0x00,
-0x8d,0x04,0x01,0xa8,0x27,0x83,0x8a,0x68,0x8c,0xe5,0x00,0x04,0x00,0x04,0x20,0x80,
-0x00,0x83,0x20,0x21,0x8c,0x82,0x00,0x00,0x3c,0x03,0x80,0x00,0x00,0xa2,0x10,0x25,
-0x00,0x43,0x10,0x25,0xac,0xe2,0x00,0x04,0xaf,0x86,0xbc,0x20,0x8c,0xe2,0x00,0x00,
-0x93,0x85,0x81,0xf0,0xaf,0x82,0xbc,0x28,0x8c,0xe3,0x00,0x04,0xad,0x4b,0x00,0x00,
-0x8d,0x02,0x01,0xa8,0xaf,0x83,0xbc,0x24,0x24,0xa5,0xff,0xff,0x24,0x44,0x00,0x01,
-0x28,0x83,0x00,0x00,0x24,0x42,0x00,0x40,0x00,0x83,0x10,0x0a,0x00,0x02,0x11,0x83,
-0x00,0x02,0x11,0x80,0x00,0x82,0x20,0x23,0xad,0x04,0x01,0xa8,0xa3,0x85,0x81,0xf0,
-0x79,0x02,0x0d,0x7c,0x00,0x00,0x00,0x00,0x00,0x43,0x10,0x26,0x08,0x00,0x29,0x9c,
-0x00,0x02,0x20,0x2b,0x3c,0x04,0xb0,0x03,0x3c,0x06,0xb0,0x07,0x3c,0x02,0x80,0x01,
-0x34,0xc6,0x00,0x18,0x34,0x84,0x00,0x20,0x24,0x42,0xa7,0x54,0x24,0x03,0xff,0x83,
-0xac,0x82,0x00,0x00,0xa0,0xc3,0x00,0x00,0x90,0xc4,0x00,0x00,0x27,0xbd,0xff,0xf8,
-0x3c,0x03,0xb0,0x07,0x24,0x02,0xff,0x82,0xa3,0xa4,0x00,0x00,0xa0,0x62,0x00,0x00,
-0x90,0x64,0x00,0x00,0x3c,0x02,0xb0,0x07,0x34,0x42,0x00,0x08,0xa3,0xa4,0x00,0x01,
-0xa0,0x40,0x00,0x00,0x90,0x43,0x00,0x00,0x24,0x02,0x00,0x03,0x3c,0x05,0xb0,0x07,
-0xa3,0xa3,0x00,0x00,0xa0,0xc2,0x00,0x00,0x90,0xc4,0x00,0x00,0x34,0xa5,0x00,0x10,
-0x24,0x02,0x00,0x06,0x3c,0x03,0xb0,0x07,0xa3,0xa4,0x00,0x00,0x34,0x63,0x00,0x38,
-0xa0,0xa2,0x00,0x00,0x90,0x64,0x00,0x00,0x3c,0x02,0xb0,0x07,0x34,0x42,0x00,0x20,
-0xa3,0xa4,0x00,0x00,0xa0,0xa0,0x00,0x00,0x90,0xa3,0x00,0x00,0xaf,0x82,0xbf,0x30,
-0xa3,0xa3,0x00,0x00,0xa0,0x40,0x00,0x00,0x90,0x43,0x00,0x00,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x08,};
-
-u8 Rtl8192PciEFwDataArray[DataArrayLengthPciE] = {
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x00,0x00,0x00,0x00,0x10,0x00,0x08,0x00,
-0x02,0xe9,0x01,0x74,0x02,0xab,0x01,0xc7,0x01,0x55,0x00,0xe4,0x00,0xab,0x00,0x72,
-0x00,0x55,0x00,0x4c,0x00,0x4c,0x00,0x4c,0x00,0x4c,0x00,0x4c,0x02,0x76,0x01,0x3b,
-0x00,0xd2,0x00,0x9e,0x00,0x69,0x00,0x4f,0x00,0x46,0x00,0x3f,0x01,0x3b,0x00,0x9e,
-0x00,0x69,0x00,0x4f,0x00,0x35,0x00,0x27,0x00,0x23,0x00,0x20,0x01,0x2f,0x00,0x98,
-0x00,0x65,0x00,0x4c,0x00,0x33,0x00,0x26,0x00,0x22,0x00,0x1e,0x00,0x98,0x00,0x4c,
-0x00,0x33,0x00,0x26,0x00,0x19,0x00,0x13,0x00,0x11,0x00,0x0f,0x02,0x39,0x01,0x1c,
-0x00,0xbd,0x00,0x8e,0x00,0x5f,0x00,0x47,0x00,0x3f,0x00,0x39,0x01,0x1c,0x00,0x8e,
-0x00,0x5f,0x00,0x47,0x00,0x2f,0x00,0x23,0x00,0x20,0x00,0x1c,0x01,0x11,0x00,0x89,
-0x00,0x5b,0x00,0x44,0x00,0x2e,0x00,0x22,0x00,0x1e,0x00,0x1b,0x00,0x89,0x00,0x44,
-0x00,0x2e,0x00,0x22,0x00,0x17,0x00,0x11,0x00,0x0f,0x00,0x0e,0x02,0xab,0x02,0xab,
-0x02,0x66,0x02,0x66,0x07,0x06,0x06,0x06,0x05,0x06,0x07,0x08,0x04,0x06,0x07,0x08,
-0x09,0x0a,0x0b,0x0b,0x49,0x6e,0x74,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x54,0x4c,
-0x42,0x4d,0x4f,0x44,0x00,0x00,0x00,0x00,0x54,0x4c,0x42,0x4c,0x5f,0x64,0x61,0x74,
-0x61,0x00,0x54,0x4c,0x42,0x53,0x00,0x00,0x00,0x00,0x00,0x00,0x41,0x64,0x45,0x4c,
-0x5f,0x64,0x61,0x74,0x61,0x00,0x41,0x64,0x45,0x53,0x00,0x00,0x00,0x00,0x00,0x00,
-0x45,0x78,0x63,0x43,0x6f,0x64,0x65,0x36,0x00,0x00,0x45,0x78,0x63,0x43,0x6f,0x64,
-0x65,0x37,0x00,0x00,0x53,0x79,0x73,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x42,0x70,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x52,0x49,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x00,0x43,0x70,0x55,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x4f,0x76,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x01,0x0b,0x63,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x08,0x00,0x00,0x00,0x10,0x00,0x00,0x00,0x2c,
-0x00,0x00,0x00,0x58,0x00,0x00,0x00,0x30,0x00,0x00,0x00,0x48,0x00,0x00,0x00,0x60,
-0x00,0x00,0x00,0x90,0x00,0x00,0x00,0xc0,0x00,0x00,0x01,0x20,0x00,0x00,0x01,0x80,
-0x00,0x00,0x01,0xb0,0x00,0x00,0x00,0x34,0x00,0x00,0x00,0x68,0x00,0x00,0x00,0x9c,
-0x00,0x00,0x00,0xd0,0x00,0x00,0x01,0x38,0x00,0x00,0x01,0xa0,0x00,0x00,0x01,0xd4,
-0x00,0x00,0x02,0x08,0x00,0x00,0x00,0x68,0x00,0x00,0x00,0xd0,0x00,0x00,0x01,0x38,
-0x00,0x00,0x01,0xa0,0x00,0x00,0x02,0x6f,0x00,0x00,0x03,0x40,0x00,0x00,0x03,0xa8,
-0x00,0x00,0x04,0x10,0x01,0x01,0x01,0x02,0x01,0x01,0x02,0x02,0x03,0x03,0x04,0x04,
-0x01,0x01,0x02,0x02,0x03,0x03,0x04,0x04,0x02,0x03,0x03,0x04,0x05,0x06,0x07,0x08,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x00,0x07,0x6c,0x80,0x00,0x07,0x80,
-0x80,0x00,0x07,0x80,0x80,0x00,0x07,0x70,0x80,0x00,0x07,0x70,0x80,0x00,0x07,0x94,
-0x80,0x00,0x56,0xb0,0x80,0x00,0x57,0x08,0x80,0x00,0x57,0x30,0x80,0x00,0x58,0x28,
-0x80,0x00,0x58,0xe0,0x80,0x00,0x59,0x88,0x80,0x00,0x59,0xfc,0x80,0x00,0x5b,0x08,
-0x80,0x00,0x5b,0x40,0x80,0x00,0x5b,0x54,0x80,0x00,0x5b,0x68,0x80,0x00,0x5c,0x50,
-0x80,0x00,0x5c,0x90,0x80,0x00,0x5d,0x44,0x80,0x00,0x5d,0x6c,0x80,0x00,0x56,0x70,
-0x80,0x00,0x5d,0xbc,0x80,0x00,0x64,0x48,0x80,0x00,0x64,0xc0,0x80,0x00,0x64,0xcc,
-0x80,0x00,0x64,0xd8,0x80,0x00,0x64,0x60,0x80,0x00,0x64,0x60,0x80,0x00,0x64,0x60,
-0x80,0x00,0x64,0x60,0x80,0x00,0x64,0x60,0x80,0x00,0x64,0x60,0x80,0x00,0x64,0x60,
-0x80,0x00,0x64,0x60,0x80,0x00,0x64,0x60,0x80,0x00,0x64,0x60,0x80,0x00,0x64,0x60,
-0x80,0x00,0x64,0x60,0x80,0x00,0x64,0xe4,0x80,0x00,0x64,0xf0,0x80,0x00,0x64,0xfc,
-0x80,0x00,0x87,0xa4,0x80,0x00,0x87,0xa4,0x80,0x00,0x87,0xa4,0x80,0x00,0x87,0xd8,
-0x80,0x00,0x88,0x18,0x80,0x00,0x88,0x50,0x80,0x00,0x88,0x80,0x80,0x00,0x88,0xb0,
-0x80,0x00,0x88,0xc4,0x80,0x00,0x89,0x2c,0x80,0x00,0x89,0x40,0x80,0x00,0x89,0x7c,
-0x80,0x00,0x89,0x84,0x80,0x00,0x89,0xc0,0x80,0x00,0x89,0xd4,0x80,0x00,0x89,0xdc,
-0x80,0x00,0x89,0xe4,0x80,0x00,0x89,0xe4,0x80,0x00,0x89,0xe4,0x80,0x00,0x89,0xe4,
-0x80,0x00,0x8a,0x14,0x80,0x00,0x8a,0x28,0x80,0x00,0x8a,0x3c,0x80,0x00,0x86,0xe8,
-0x80,0x00,0x8d,0x68,0x80,0x00,0x8d,0x68,0x80,0x00,0x8d,0x68,0x80,0x00,0x8d,0x9c,
-0x80,0x00,0x8d,0xdc,0x80,0x00,0x8e,0x14,0x80,0x00,0x8e,0x44,0x80,0x00,0x8e,0x74,
-0x80,0x00,0x8e,0x88,0x80,0x00,0x8e,0xf0,0x80,0x00,0x8f,0x04,0x80,0x00,0x8f,0x40,
-0x80,0x00,0x8f,0x48,0x80,0x00,0x8f,0x84,0x80,0x00,0x8f,0x98,0x80,0x00,0x8f,0xa0,
-0x80,0x00,0x8f,0xa8,0x80,0x00,0x8f,0xa8,0x80,0x00,0x8f,0xa8,0x80,0x00,0x8f,0xa8,
-0x80,0x00,0x8f,0xd8,0x80,0x00,0x8f,0xec,0x80,0x00,0x90,0x00,0x80,0x00,0x8b,0x88,
-};
-
 u32 Rtl8192PciEPHY_REGArray[PHY_REGArrayLengthPciE] = {0x0,};
 
 u32 Rtl8192PciEPHY_REG_1T2RArray[PHY_REG_1T2RArrayLengthPciE] = {
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
index 5d6d304c9c0..88489215dc2 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
@@ -729,7 +729,6 @@ u8 rtl8192_phy_ConfigRFWithHeaderFile(struct net_device *dev,
 {
 
 	int i;
-	u8 ret = 0;
 
 	switch (eRFPath) {
 	case RF90_PATH_A:
@@ -787,7 +786,7 @@ u8 rtl8192_phy_ConfigRFWithHeaderFile(struct net_device *dev,
 		break;
 	}
 
-	return ret;
+	return 0;
 
 }
 static void rtl8192_SetTxPowerLevel(struct net_device *dev, u8 channel)
@@ -1555,7 +1554,7 @@ static bool SetRFPowerState8190(struct net_device *dev,
 
 		default:
 			bResult = false;
-			RT_TRACE(COMP_ERR, "SetRFPowerState8190(): unknow state"
+			RT_TRACE(COMP_ERR, "SetRFPowerState8190(): unknown state"
 				 " to set: 0x%X!!!\n", eRFPowerState);
 			break;
 		}
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_core.c b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
index c01abc23213..2920e406030 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
@@ -1915,8 +1915,7 @@ int rtl8192_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	memcpy((unsigned char *)(skb->cb), &dev, sizeof(dev));
 	if (queue_index == TXCMD_QUEUE) {
 		rtl8192_tx_cmd(dev, skb);
-		ret = 0;
-		return ret;
+		return 0;
 	} else {
 		tcb_desc->RATRIndex = 7;
 		tcb_desc->bTxDisableRateFallBack = 1;
@@ -2926,8 +2925,7 @@ static int rtl8192_pci_probe(struct pci_dev *pdev,
 
 	dev->netdev_ops = &rtl8192_netdev_ops;
 
-	dev->wireless_handlers = (struct iw_handler_def *)
-				 &r8192_wx_handlers_def;
+	dev->wireless_handlers = &r8192_wx_handlers_def;
 	dev->ethtool_ops = &rtl819x_ethtool_ops;
 
 	dev->type = ARPHRD_ETHER;
@@ -3030,7 +3028,7 @@ bool NicIFEnableNIC(struct net_device *dev)
 		RT_TRACE(COMP_ERR, "ERR!!! %s(): Driver is already down!\n",
 			 __func__);
 		priv->bdisable_nic = false;
-		return RT_STATUS_FAILURE;
+		return false;
 	}
 
 	RT_TRACE(COMP_PS, "===========>%s()\n", __func__);
@@ -3040,7 +3038,7 @@ bool NicIFEnableNIC(struct net_device *dev)
 		RT_TRACE(COMP_ERR, "ERR!!! %s(): initialization is failed!\n",
 			 __func__);
 		priv->bdisable_nic = false;
-		return -1;
+		return false;
 	}
 	RT_TRACE(COMP_INIT, "start adapter finished\n");
 	RT_CLEAR_PS_LEVEL(pPSC, RT_RF_OFF_LEVL_HALT_NIC);
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_core.h b/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
index 35fc1164eff..d1438c2f898 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
@@ -582,7 +582,6 @@ struct r8192_priv {
 
 	struct rt_firmware			*pFirmware;
 	enum rtl819x_loopback LoopbackMode;
-	enum firmware_source firmware_source;
 
 	struct timer_list			watch_dog_timer;
 	struct timer_list			fsync_timer;
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
index 498995d833e..52870041fcb 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
@@ -19,6 +19,7 @@
 
 #include <linux/string.h>
 #include "rtl_core.h"
+#include "rtl_wx.h"
 
 #define RATE_COUNT 12
 static u32 rtl8192_rates[] = {
@@ -1130,11 +1131,18 @@ static int r8192_wx_set_PromiscuousMode(struct net_device *dev,
 	struct r8192_priv *priv = rtllib_priv(dev);
 	struct rtllib_device *ieee = priv->rtllib;
 
-	u32 *info_buf = (u32 *)(wrqu->data.pointer);
+	u32 info_buf[3];
 
-	u32 oid = info_buf[0];
-	u32 bPromiscuousOn = info_buf[1];
-	u32 bFilterSourceStationFrame = info_buf[2];
+	u32 oid;
+	u32 bPromiscuousOn;
+	u32 bFilterSourceStationFrame;
+
+	if (copy_from_user(info_buf, wrqu->data.pointer, sizeof(info_buf)))
+		return -EFAULT;
+
+	oid = info_buf[0];
+	bPromiscuousOn = info_buf[1];
+	bFilterSourceStationFrame = info_buf[2];
 
 	if (OID_RT_INTEL_PROMISCUOUS_MODE == oid) {
 		ieee->IntelPromiscuousModeInfo.bPromiscuousOn =
@@ -1213,7 +1221,7 @@ static iw_handler r8192_wx_handlers[] = {
 };
 
 /*
- * the following rule need to be follwing,
+ * the following rule need to be following,
  * Odd : get (world access),
  * even : set (root access)
  * */
@@ -1320,7 +1328,7 @@ static struct iw_statistics *r8192_get_wireless_stats(struct net_device *dev)
 	return wstats;
 }
 
-struct iw_handler_def  r8192_wx_handlers_def = {
+const struct iw_handler_def r8192_wx_handlers_def = {
 	.standard = r8192_wx_handlers,
 	.num_standard = ARRAY_SIZE(r8192_wx_handlers),
 	.private = r8192_private_handler,
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.h b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.h
index 6a51a25ec87..58398517f5b 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.h
@@ -24,8 +24,7 @@ struct net_device;
 struct iw_handler_def;
 struct iw_statistics;
 
-extern struct iw_handler_def r8192_wx_handlers_def;
-struct iw_statistics *r8192_get_wireless_stats(struct net_device *dev);
+extern const struct iw_handler_def r8192_wx_handlers_def;
 u16 rtl8192_11n_user_show_rates(struct net_device *dev);
 
 #endif
diff --git a/drivers/staging/rtl8192e/rtl819x_BAProc.c b/drivers/staging/rtl8192e/rtl819x_BAProc.c
index adc6cc7ca3d..48a142b82d9 100644
--- a/drivers/staging/rtl8192e/rtl819x_BAProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_BAProc.c
@@ -109,9 +109,9 @@ static struct sk_buff *rtllib_ADDBA(struct rtllib_device *ieee, u8 *Dst,
 	BAReq->frame_ctl = cpu_to_le16(RTLLIB_STYPE_MANAGE_ACT);
 
 	tag = (u8 *)skb_put(skb, 9);
-	*tag ++= ACT_CAT_BA;
-	*tag ++= type;
-	*tag ++= pBA->DialogToken;
+	*tag++ = ACT_CAT_BA;
+	*tag++ = type;
+	*tag++ = pBA->DialogToken;
 
 	if (ACT_ADDBARSP == type) {
 		RT_TRACE(COMP_DBG, "====>to send ADDBARSP\n");
@@ -175,8 +175,8 @@ static struct sk_buff *rtllib_DELBA(struct rtllib_device *ieee, u8 *dst,
 
 	tag = (u8 *)skb_put(skb, 6);
 
-	*tag ++= ACT_CAT_BA;
-	*tag ++= ACT_DELBA;
+	*tag++ = ACT_CAT_BA;
+	*tag++ = ACT_DELBA;
 
 	tmp = DelbaParamSet.shortData;
 	memcpy(tag, (u8 *)&tmp, 2);
@@ -230,8 +230,8 @@ static void rtllib_send_DELBA(struct rtllib_device *ieee, u8 *dst,
 	if (skb)
 		softmac_mgmt_xmit(skb, ieee);
 	else
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "alloc skb error in func"
-			     "tion %s()\n", __func__);
+		RTLLIB_DEBUG(RTLLIB_DL_ERR, "alloc skb error in function"
+			     " %s()\n", __func__);
 	return ;
 }
 
diff --git a/drivers/staging/rtl8192e/rtl819x_TSProc.c b/drivers/staging/rtl8192e/rtl819x_TSProc.c
index 29608e5488a..52b297724fb 100644
--- a/drivers/staging/rtl8192e/rtl819x_TSProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_TSProc.c
@@ -79,7 +79,7 @@ static void RxPktPendingTimeout(unsigned long data)
 
 		if (index > REORDER_WIN_SIZE) {
 			RTLLIB_DEBUG(RTLLIB_DL_ERR, "RxReorderIndicatePacket():"
-				     " Rx Reorer struct buffer full!!\n");
+				     " Rx Reorder struct buffer full!!\n");
 			spin_unlock_irqrestore(&(ieee->reorder_spinlock),
 					       flags);
 			return;
diff --git a/drivers/staging/rtl8192e/rtllib_rx.c b/drivers/staging/rtl8192e/rtllib_rx.c
index 6c8a8e12b2b..60de54cc601 100644
--- a/drivers/staging/rtl8192e/rtllib_rx.c
+++ b/drivers/staging/rtl8192e/rtllib_rx.c
@@ -712,7 +712,7 @@ static void RxReorderIndicatePacket(struct rtllib_device *ieee,
 
 		if (index > REORDER_WIN_SIZE) {
 			RTLLIB_DEBUG(RTLLIB_DL_ERR, "RxReorderIndicatePacket():"
-				     " Rx Reorer struct buffer full!!\n");
+				     " Rx Reorder struct buffer full!!\n");
 			spin_unlock_irqrestore(&(ieee->reorder_spinlock),
 					       flags);
 			return;
@@ -1589,7 +1589,6 @@ static int rtllib_qos_convert_ac_to_parameters(struct rtllib_qos_parameter_info
 {
 	struct rtllib_qos_ac_parameter *ac_params;
 	struct rtllib_qos_parameters *qos_param = &(qos_data->parameters);
-	int rc = 0;
 	int i;
 	u8 aci;
 	u8 acm;
@@ -1640,7 +1639,7 @@ static int rtllib_qos_convert_ac_to_parameters(struct rtllib_qos_parameter_info
 		    (ac_params->aci_aifsn & 0x10) ? 0x01 : 0x00;
 		qos_param->tx_op_limit[aci] = ac_params->tx_op_limit;
 	}
-	return rc;
+	return 0;
 }
 
 /*
diff --git a/drivers/staging/rtl8192e/rtllib_softmac.c b/drivers/staging/rtl8192e/rtllib_softmac.c
index 4bf72bc1ba7..684ceed1723 100644
--- a/drivers/staging/rtl8192e/rtllib_softmac.c
+++ b/drivers/staging/rtl8192e/rtllib_softmac.c
@@ -543,7 +543,7 @@ void rtllib_softmac_scan_syncro(struct rtllib_device *ieee, u8 is_mesh)
 				goto out; /* scan completed */
 		} while (!ieee->active_channel_map[ch]);
 
-		/* this fuction can be called in two situations
+		/* this function can be called in two situations
 		 * 1- We have switched to ad-hoc mode and we are
 		 *    performing a complete syncro scan before conclude
 		 *    there are no interesting cell and to create a
@@ -3239,7 +3239,6 @@ static int rtllib_wpa_set_auth_algs(struct rtllib_device *ieee, int value)
 	struct rtllib_security sec = {
 		.flags = SEC_AUTH_MODE,
 	};
-	int ret = 0;
 
 	if (value & AUTH_ALG_SHARED_KEY) {
 		sec.auth_mode = WLAN_AUTH_SHARED_KEY;
@@ -3259,7 +3258,7 @@ static int rtllib_wpa_set_auth_algs(struct rtllib_device *ieee, int value)
 	if (ieee->set_security)
 		ieee->set_security(ieee->dev, &sec);
 
-	return ret;
+	return 0;
 }
 
 static int rtllib_wpa_set_param(struct rtllib_device *ieee, u8 name, u32 value)
diff --git a/drivers/staging/rtl8192e/rtllib_softmac_wx.c b/drivers/staging/rtl8192e/rtllib_softmac_wx.c
index e6af8cfab12..65b650cc3d0 100644
--- a/drivers/staging/rtl8192e/rtllib_softmac_wx.c
+++ b/drivers/staging/rtl8192e/rtllib_softmac_wx.c
@@ -628,8 +628,6 @@ int rtllib_wx_get_power(struct rtllib_device *ieee,
 				 struct iw_request_info *info,
 				 union iwreq_data *wrqu, char *extra)
 {
-	int ret = 0;
-
 	down(&ieee->wx_sem);
 
 	if (ieee->ps == RTLLIB_PS_DISABLED) {
@@ -657,7 +655,7 @@ int rtllib_wx_get_power(struct rtllib_device *ieee,
 
 exit:
 	up(&ieee->wx_sem);
-	return ret;
+	return 0;
 
 }
 EXPORT_SYMBOL(rtllib_wx_get_power);
diff --git a/drivers/staging/rtl8192e/rtllib_tx.c b/drivers/staging/rtl8192e/rtllib_tx.c
index 11d0a9d8ee5..b7dd1539bbc 100644
--- a/drivers/staging/rtl8192e/rtllib_tx.c
+++ b/drivers/staging/rtl8192e/rtllib_tx.c
@@ -171,7 +171,7 @@ inline int rtllib_put_snap(u8 *data, u16 h_proto)
 	snap->oui[1] = oui[1];
 	snap->oui[2] = oui[2];
 
-	*(u16 *)(data + SNAP_SIZE) = h_proto;
+	*(__be16 *)(data + SNAP_SIZE) = htons(h_proto);
 
 	return SNAP_SIZE + sizeof(u16);
 }
diff --git a/drivers/staging/rtl8192ee/Kconfig b/drivers/staging/rtl8192ee/Kconfig
new file mode 100644
index 00000000000..beb07ac24e8
--- /dev/null
+++ b/drivers/staging/rtl8192ee/Kconfig
@@ -0,0 +1,15 @@
+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
+	default N
+	---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
new file mode 100644
index 00000000000..31708620d7e
--- /dev/null
+++ b/drivers/staging/rtl8192ee/Makefile
@@ -0,0 +1,40 @@
+
+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
new file mode 100644
index 00000000000..162092a0316
--- /dev/null
+++ b/drivers/staging/rtl8192ee/TODO
@@ -0,0 +1,12 @@
+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
new file mode 100644
index 00000000000..64ade216a15
--- /dev/null
+++ b/drivers/staging/rtl8192ee/base.c
@@ -0,0 +1,1852 @@
+/******************************************************************************
+ *
+ * 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(rtlpriv->cfg->name, 0, 0);
+	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) &&
+	    (map & 0x000c) != 0x000c0) {
+		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
new file mode 100644
index 00000000000..c7929a7b02f
--- /dev/null
+++ b/drivers/staging/rtl8192ee/base.h
@@ -0,0 +1,163 @@
+/******************************************************************************
+ *
+ * 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
new file mode 100644
index 00000000000..56e6c465189
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbt_precomp.h
@@ -0,0 +1,50 @@
+/******************************************************************************
+ *
+ * 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.c b/drivers/staging/rtl8192ee/btcoexist/halbtc8192e2ant.c
new file mode 100644
index 00000000000..ab2cc2fadd8
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtc8192e2ant.c
@@ -0,0 +1,4110 @@
+/**************************************************************
+ * 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 glbt_infosrc8192e2ant[] = {
+	"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 = 0;
+	u8 btrssi_state = coex_sta->pre_bt_rssi_state;
+
+	btrssi = 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)) {
+			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 halbtc_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 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);
+
+	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] |= 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 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 num_diffprofile = 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)
+		num_diffprofile++;
+	if (bt_link_info->hid_exist)
+		num_diffprofile++;
+	if (bt_link_info->pan_exist)
+		num_diffprofile++;
+	if (bt_link_info->a2dp_exist)
+		num_diffprofile++;
+
+	if (num_diffprofile == 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 (num_diffprofile == 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 (num_diffprofile == 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 (num_diffprofile >= 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] |= BIT(0);
+
+	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 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;
+	}
+	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 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);
+	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 halbtc_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 halbtc_coex_table_with_type(struct btc_coexist *btcoexist,
+					bool force_exec, u8 type)
+{
+	switch (type) {
+	case 0:
+		halbtc_coex_table(btcoexist, force_exec, 0x55555555,
+				  0x5a5a5a5a, 0xffffff, 0x3);
+		break;
+	case 1:
+		halbtc_coex_table(btcoexist, force_exec, 0x5a5a5a5a,
+				  0x5a5a5a5a, 0xffffff, 0x3);
+		break;
+	case 2:
+		halbtc_coex_table(btcoexist, force_exec, 0x55555555,
+				  0x5ffb5ffb, 0xffffff, 0x3);
+		break;
+	case 3:
+		halbtc_coex_table(btcoexist, force_exec, 0xdfffdfff,
+				  0x5fdb5fdb, 0xffffff, 0x3);
+		break;
+	case 4:
+		halbtc_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] |= 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 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];
+
+	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 halbtc_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 halbtc_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 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;
+	}
+	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 */
+	halbtc_sw_mechanism1(btcoexist, false, false, false, false);
+	halbtc_sw_mechanism2(btcoexist, false, false, false, 0x18);
+
+	/* hw all off */
+	halbtc_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);
+
+	halbtc_coex_table_with_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);
+}
+
+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);
+
+	halbtc_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);
+
+	halbtc_sw_mechanism1(btcoexist, false, false, false, false);
+	halbtc_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);
+			halbtc_coex_table_with_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);
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						false, 1);
+		}
+
+		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);
+
+		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);
+			halbtc_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);
+
+			halbtc_sw_mechanism1(btcoexist, false, false,
+					     false, false);
+			halbtc_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);
+			halbtc_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);
+
+			halbtc_sw_mechanism1(btcoexist, true, false,
+					     false, false);
+			halbtc_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);
+				halbtc_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);
+				halbtc_sw_mechanism1(btcoexist, false,
+						     false, false,
+						     false);
+				halbtc_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 {
+		/* accquire 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);
+
+	halbtc_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)) {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x6);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(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,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x6);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, false, true,
+					     false, false);
+			halbtc_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);
+
+	halbtc_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)) {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x6);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(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,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x6);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, false, true,
+					     false, false);
+			halbtc_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);
+
+	halbtc_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)) {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(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,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, false, true,
+					     false, false);
+			halbtc_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)
+		halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+	else
+		halbtc_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)) {
+			halbtc_sw_mechanism1(btcoexist, true, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, true, false,
+					     false, false);
+			halbtc_sw_mechanism2(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,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, false, false,
+					     false, false);
+			halbtc_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);
+	halbtc_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)) {
+			halbtc_sw_mechanism1(btcoexist, true, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     true, 0x6);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, true, false,
+					     false, false);
+			halbtc_sw_mechanism2(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,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     true, 0x6);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, false, false,
+					     false, false);
+			halbtc_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);
+
+	halbtc_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)) {
+			halbtc_sw_mechanism1(btcoexist, true, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, true, false,
+					     false, false);
+			halbtc_sw_mechanism2(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,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, false, false,
+					     false, false);
+			halbtc_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);
+
+	halbtc_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)) {
+			halbtc_sw_mechanism1(btcoexist, true, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, true, false,
+					     false, false);
+			halbtc_sw_mechanism2(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,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, false, false,
+					     false, false);
+			halbtc_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);
+
+	halbtc_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)) {
+			halbtc_sw_mechanism1(btcoexist, true, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, true, false,
+					     false, false);
+			halbtc_sw_mechanism2(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,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, false, false,
+					     false, false);
+			halbtc_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);
+
+	halbtc_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)) {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(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,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, false, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     false, 0x18);
+		}
+	}
+}
+
+/* HID+A2DP+PAN(EDR) */
+static void 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);
+
+	halbtc_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)) {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(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,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, false, true,
+					     false, false);
+			halbtc_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);
+
+	halbtc_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)) {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(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,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, false, true,
+					     false, false);
+			halbtc_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");
+			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);
+
+	halbtc_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 |= BIT(9);
+	btcoexist->btc_write_2byte(btcoexist, 0x40, u16tmp);
+
+	/* enable PTA I2C mailbox  */
+	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x101);
+	u8tmp |= BIT(4);
+	btcoexist->btc_write_1byte(btcoexist, 0x101, u8tmp);
+
+	/* enable bt clock when wifi is disabled. */
+	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x93);
+	u8tmp |= BIT(0);
+	btcoexist->btc_write_1byte(btcoexist, 0x93, u8tmp);
+	/* enable bt clock when suspend. */
+	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x7);
+	u8tmp |= BIT(0);
+	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 & BIT(0)) ? "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 ",
+				   glbt_infosrc8192e2ant[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);
+#if (BT_AUTO_REPORT_ONLY_8192E_2ANT == 1)
+	halbtc8192e2ant_monitor_bt_ctr(btcoexist);
+#endif
+	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 & BIT(1))) {
+			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 & BIT(3))) {
+			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 (BT_AUTO_REPORT_ONLY_8192E_2ANT == 0)
+		if ((coex_sta->bt_info_ext & BIT(4))) {
+			/* BT auto report already enabled, do nothing */
+		} else {
+			halbtc8192e2ant_bt_autoreport(btcoexist, FORCE_EXEC,
+						      true);
+		}
+#endif
+	}
+
+	/* check BIT(2) 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)
+{
+}
+
+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");
+	}
+
+#if (BT_AUTO_REPORT_ONLY_8192E_2ANT == 0)
+	halbtc8192e2ant_querybt_info(btcoexist);
+	halbtc8192e2ant_monitor_bt_ctr(btcoexist);
+	halbtc_monitor_bt_enable_disable(btcoexist);
+#else
+	if (halbtc8192e2ant_iswifi_status_changed(btcoexist) ||
+	    coex_dm->auto_tdma_adjust)
+		halbtc8192e2ant_run_coexist_mechanism(btcoexist);
+#endif
+}
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8192e2ant.h b/drivers/staging/rtl8192ee/btcoexist/halbtc8192e2ant.h
new file mode 100644
index 00000000000..ece3e10e6df
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtc8192e2ant.h
@@ -0,0 +1,161 @@
+/*****************************************************************
+ *   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.c b/drivers/staging/rtl8192ee/btcoexist/halbtc8723b1ant.c
new file mode 100644
index 00000000000..153048f6b4d
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtc8723b1ant.c
@@ -0,0 +1,3146 @@
+/***************************************************************
+ * 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_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 glb_infosrc8723b1ant[] = {
+	"BT Info[wifi fw]",
+	"BT Info[bt rsp]",
+	"BT Info[bt auto report]",
+};
+
+static u32 glcoex_ver_date_8723b_1ant = 20130918;
+static u32 glcoex_ver_8723b_1ant = 0x47;
+
+/***************************************************************
+ * 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 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) {
+				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;
+				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;
+				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;
+				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) {
+				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;
+				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) {
+				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;
+				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;
+				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;
+				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;
+				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;
+
+	return 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 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;
+	}
+
+	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] |= 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, 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 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 void set_sw_penalty_tx_rate_adapt(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 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;
+	}
+	set_sw_penalty_tx_rate_adapt(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 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, 0xaaaaaaaa,
+					   0xaaaaaaaa, 0xffffff, 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 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;
+	}
+	set_fw_ignore_wlan_act(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 & BIT(4)) && !(byte1 & BIT(5))) {
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+				  "[BTCoex], FW for 1Ant AP mode\n");
+			real_byte1 &= ~BIT(4);
+			real_byte1 |= BIT(5);
+
+			real_byte5 |= BIT(5);
+			real_byte5 &= ~BIT(6);
+		}
+	}
+
+	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_set_lps_rpwm(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_LPS_VAL, &lps);
+	btcoexist->btc_set(btcoexist, BTC_SET_U1_RPWM_VAL, &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_set_lps_rpwm(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_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);
+
+	halbtc8723b1ant_low_penalty_ra(btcoexist, NORMAL_EXEC, low_penalty_ra);
+}
+
+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;
+	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 &= ~BIT(23);
+		u32tmp &= ~BIT(24);
+		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 */
+			u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
+			u32tmp &= ~BIT(23);
+			u32tmp |= BIT(24);
+			btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
+
+			if (brd_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 (brd_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 (brd_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 (brd_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 (brd_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 |= BIT(23);
+			u32tmp &= ~BIT(24);
+			btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
+
+			if (brd_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 (brd_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 (brd_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 (brd_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 (brd_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:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x3a,
+						       0x03, 0x10, 0x50);
+
+			rssi_adjust_val = 11;
+			break;
+		case 2:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x2b,
+						       0x03, 0x10, 0x50);
+			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:
+			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:
+			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, 0x25,
+						       0x03, 0x11, 0x11);
+			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_sw_mechanism(btcoexist, false);
+		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_sw_mechanism(btcoexist, false);
+		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_sw_mechanism(btcoexist, false);
+		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_sw_mechanism(btcoexist, false);
+		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_sw_mechanism(btcoexist, false);
+		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 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;
+	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 {
+		/*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;
+		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 */
+			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 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:
+		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:
+		pstdmacheckforpowersavestate(btcoexist, false);
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+		break;
+	default:
+		break;
+	}
+}
+
+/***************************************************
+ *
+ *	Software Coex Mechanism start
+ *
+ ***************************************************/
+/* SCO only or SCO+PAN(HS) */
+static void halbtc8723b1ant_action_sco(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_sw_mechanism(btcoexist, true);
+}
+
+static void halbtc8723b1ant_action_hid(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_sw_mechanism(btcoexist, true);
+}
+
+/*A2DP only / PAN(EDR) only/ A2DP+PAN(HS) */
+static void halbtc8723b1ant_action_a2dp(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_sw_mechanism(btcoexist, false);
+}
+
+static void halbtc8723b1ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_sw_mechanism(btcoexist, false);
+}
+
+static void halbtc8723b1ant_action_pan_edr(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_sw_mechanism(btcoexist, false);
+}
+
+/* PAN(HS) only */
+static void halbtc8723b1ant_action_pan_hs(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_sw_mechanism(btcoexist, false);
+}
+
+/*PAN(EDR)+A2DP */
+static void halbtc8723b1ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_sw_mechanism(btcoexist, false);
+}
+
+static void halbtc8723b1ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_sw_mechanism(btcoexist, true);
+}
+
+/* HID+A2DP+PAN(EDR) */
+static void action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_sw_mechanism(btcoexist, true);
+}
+
+static void halbtc8723b1ant_action_hid_a2dp(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_sw_mechanism(btcoexist, true);
+}
+
+/*****************************************************
+ *
+ *	Non-Software Coex Mechanism start
+ *
+ *****************************************************/
+static void halbtc8723b1ant_action_wifi_multiport(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);
+}
+
+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);
+}
+
+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, false, 8);
+		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);
+	} 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);
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	}
+}
+
+static void 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);
+		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);
+	}
+}
+
+static void halbtc8723b1ant_action_wifi_connected_bt_acl_busy(
+					struct btc_coexist *btcoexist,
+					u8 wifi_status)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	u8 rssi_state;
+
+	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);
+		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);
+			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 { /*for low BT RSSI */
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 11);
+			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)) {
+			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;
+		}
+
+		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);
+		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);
+		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);
+		coex_dm->auto_tdma_adjust = false;
+	}
+}
+
+static void 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);
+	coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+}
+
+static void action_wifi_not_connected_scan(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);
+			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);
+		} else {
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 20);
+			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,
+				BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN);
+	} else {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+	}
+}
+
+static void action_wifi_not_connected_asso_auth(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);
+
+	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);
+	} else {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	}
+}
+
+static void action_wifi_connected_scan(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);
+			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);
+		} else {
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 20);
+			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,
+				BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN);
+	} else {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+	}
+}
+
+static void 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_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);
+	} else {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
+		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) {
+		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) {
+		if (scan)
+			action_wifi_connected_scan(btcoexist);
+		else
+			action_wifi_connected_special_packet(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);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+	/* power save state */
+	if (!ap_enable &&
+	    BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status &&
+	    !btcoexist->bt_link_info.hid_only) {
+		if (!wifi_busy && btcoexist->bt_link_info.a2dp_only)
+			halbtc8723b1ant_power_save_state(btcoexist,
+							 BTC_PS_WIFI_NATIVE,
+							 0x0, 0x0);
+		else
+			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 */
+	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)) {
+			action_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);
+		}
+	} 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)) {
+			action_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);
+		}
+	}
+}
+
+static void 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)) {
+		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");
+			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 increase_scan_dev_num = false;
+	bool b_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");
+
+	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))
+		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_U4_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);
+		halbtc8723b1ant_limited_rx(btcoexist, NORMAL_EXEC, false,
+					   b_bt_ctrl_agg_buf_size,
+					   agg_buf_size);
+		halbtc8723b1ant_action_wifi_multiport(btcoexist);
+		return;
+	}
+
+	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);
+
+	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) {
+			if (scan)
+				action_wifi_not_connected_scan(btcoexist);
+			else
+				action_wifi_not_connected_asso_auth(btcoexist);
+		} else {
+			action_wifi_not_connected(btcoexist);
+		}
+	} else { /* wifi LPS/Busy */
+		halbtc8723b1ant_action_wifi_connected(btcoexist);
+	}
+}
+
+static void halbtc8723b1ant_init_coex_dm(struct btc_coexist *btcoexist)
+{
+	/* sw all off */
+	halbtc8723b1ant_sw_mechanism(btcoexist, false);
+
+	halbtc8723b1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 8);
+	coex_table_with_type(btcoexist, FORCE_EXEC, 0);
+}
+
+static void 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->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 & BIT(0)) {
+			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 */
+	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, 0x4);
+}
+
+/**************************************************************
+ * work around function start with wa_halbtc8723b1ant_
+ **************************************************************/
+/**************************************************************
+ * extern function start with EXhalbtc8723b1ant_
+ **************************************************************/
+
+void ex_halbtc8723b1ant_init_hwconfig(struct btc_coexist *btcoexist)
+{
+	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 *brd_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;
+	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, wifi_link_status;
+	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 (!brd_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: ",
+		   brd_info->pg_ant_num, brd_info->btdm_ant_num,
+		   brd_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);
+
+	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",
+		   "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] ",
+		   "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 & BIT(0)) ? "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)",
+				   glb_infosrc8723b1ant[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_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);
+
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\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 ",
+			   "DelBA/ BtCtrlAgg/ AggSize",
+			   (btcoexist->bt_info.reject_agg_pkt ? "Yes" : "No"),
+			   (btcoexist->bt_info.b_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 ",
+			   "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);
+
+		stdmacase = 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],
+			   stdmacase, coex_dm->auto_tdma_adjust);
+		CL_PRINTF(cli_buf);
+
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\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 ",
+			   "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);
+
+	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, "\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);
+		coex_table_with_type(btcoexist,
+						     NORMAL_EXEC, 0);
+		halbtc8723b1ant_wifi_off_hw_cfg(btcoexist);
+	} else if (BTC_IPS_LEAVE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], IPS LEAVE notify\n");
+		coex_sta->under_ips = false;
+
+		init_hw_config(btcoexist, false);
+		halbtc8723b1ant_init_coex_dm(btcoexist);
+		halbtc8723b1ant_query_bt_info(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;
+	u32 wifi_link_status = 0;
+	u32 num_of_wifi_link = 0;
+	bool bt_ctrl_agg_buf_size = false;
+	u8 agg_buf_size = 5;
+
+	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);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_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);
+		halbtc8723b1ant_limited_rx(btcoexist, NORMAL_EXEC, false,
+					   bt_ctrl_agg_buf_size, agg_buf_size);
+		halbtc8723b1ant_action_wifi_multiport(btcoexist);
+		return;
+	}
+
+	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 */
+			action_wifi_not_connected_scan(btcoexist);
+		else	/* wifi is connected */
+			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 */
+			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;
+	u32 wifi_link_status = 0;
+	u32 num_of_wifi_link = 0;
+	bool bt_ctrl_agg_buf_size = false;
+	u8 agg_buf_size = 5;
+
+	if (btcoexist->manual_control || btcoexist->stop_coex_dm ||
+	    btcoexist->bt_info.bt_disabled)
+		return;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_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);
+		halbtc8723b1ant_limited_rx(btcoexist, NORMAL_EXEC, false,
+					   bt_ctrl_agg_buf_size, agg_buf_size);
+		halbtc8723b1ant_action_wifi_multiport(btcoexist);
+		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");
+		action_wifi_not_connected_asso_auth(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 */
+			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;
+	u32 wifi_link_status = 0;
+	u32 num_of_wifi_link = 0;
+	bool bt_ctrl_agg_buf_size = false;
+	u8 agg_buf_size = 5;
+
+	if (btcoexist->manual_control || btcoexist->stop_coex_dm ||
+	    btcoexist->bt_info.bt_disabled)
+		return;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_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);
+		halbtc8723b1ant_limited_rx(btcoexist, NORMAL_EXEC, false,
+					   bt_ctrl_agg_buf_size, agg_buf_size);
+		halbtc8723b1ant_action_wifi_multiport(btcoexist);
+		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);
+		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 & 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)
+				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 & BIT(3)) {
+			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 & BIT(4)) {
+			/* BT auto report already enabled, do nothing */
+		} else {
+			halbtc8723b1ant_bt_auto_report(btcoexist, FORCE_EXEC,
+						       true);
+		}
+#endif
+	}
+
+	/* check BIT(2) 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_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_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_coex_dm(btcoexist);
+		halbtc8723b1ant_query_bt_info(btcoexist);
+	}
+}
+
+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);
+	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);
+}
+
+void ex_halbtc8723b1ant_periodical(struct btc_coexist *btcoexist)
+{
+	struct btc_board_info *brd_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",
+			  brd_info->pg_ant_num, brd_info->btdm_ant_num,
+			  brd_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 (is_wifi_status_changed(btcoexist) ||
+	    coex_dm->auto_tdma_adjust) {
+		halbtc8723b1ant_run_coexist_mechanism(btcoexist);
+	}
+
+	coex_sta->special_pkt_period_cnt++;
+#endif
+}
+
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8723b1ant.h b/drivers/staging/rtl8192ee/btcoexist/halbtc8723b1ant.h
new file mode 100644
index 00000000000..bded3738f24
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtc8723b1ant.h
@@ -0,0 +1,160 @@
+/**********************************************************************
+ * 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
new file mode 100644
index 00000000000..836206357e6
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtc8723b2ant.c
@@ -0,0 +1,3929 @@
+/***************************************************************
+ * 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
new file mode 100644
index 00000000000..8437e1c3ecd
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtc8723b2ant.h
@@ -0,0 +1,145 @@
+/************************************************************************
+ * 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.c b/drivers/staging/rtl8192ee/btcoexist/halbtc8821a1ant.c
new file mode 100644
index 00000000000..fb5286387d8
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtc8821a1ant.c
@@ -0,0 +1,2780 @@
+/*  */
+/*  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 */
+/*  */
+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]",
+};
+
+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_ */
+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;
+
+	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)) {
+				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_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");
+			} 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_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");
+			} 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 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];
+
+	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)) {
+				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_8821A_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_8821A_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 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)) {
+		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)
+{
+	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)) {
+		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_b_mode);
+			if (wifi_under_b_mode) {
+				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 halbtc8821a1ant_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 halbtc8821a1ant_ampdu_max_time(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 halbtc8821a1ant_limited_tx(struct btc_coexist *btcoexist,
+				       bool force_exec, u8 ra_mask_type,
+				       u8 arfr_type, u8 retry_limit_type,
+				       u8 ampdu_time_type)
+{
+	switch (ra_mask_type) {
+	case 0:	/*  normal mode */
+		update_ra_mask(btcoexist, force_exec, 0x0);
+		break;
+	case 1:	/*  disable cck 1/2 */
+		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);
+		break;
+	default:
+		break;
+	}
+
+	auto_rate_fallback_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);
+}
+
+static void halbtc8821a1ant_limited_rx(struct btc_coexist *btcoexist,
+				       bool force_exec, bool rej_ap_agg_pkt,
+				       bool bt_ctrl_agg_buf_size,
+				       u8 agg_buf_size)
+{
+	bool reject_rx_agg = rej_ap_agg_pkt;
+	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. */
+	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 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;
+
+	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 & MASKLWORD;
+	reg_hp_rx = (u4_tmp & MASKHWORD)>>16;
+
+	u4_tmp = btcoexist->btc_read_4byte(btcoexist, reg_lp_tx_rx);
+	reg_lp_tx = u4_tmp & MASKLWORD;
+	reg_lp_rx = (u4_tmp & 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 halbtc8821a1ant_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 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;
+
+	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 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;
+
+	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], 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");
+				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");
+				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");
+					algorithm = BT_8821A_1ANT_COEX_ALGO_PANHS;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = PAN(EDR) only\n");
+					algorithm = BT_8821A_1ANT_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], 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");
+				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");
+					algorithm = BT_8821A_1ANT_COEX_ALGO_SCO;
+				} else {
+					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");
+				algorithm = BT_8821A_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_8821A_1ANT_COEX_ALGO_HID_A2DP;
+				} else {
+					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) {
+				if (bt_hs_on) {
+					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");
+					algorithm = BT_8821A_1ANT_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], 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");
+					algorithm = BT_8821A_1ANT_COEX_ALGO_HID_A2DP;
+				} else {
+					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");
+					algorithm = BT_8821A_1ANT_COEX_ALGO_SCO;
+				} else {
+					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) {
+				if (bt_hs_on) {
+					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");
+					algorithm = BT_8821A_1ANT_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!!! 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_8821A_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		}
+	}
+
+	return algorithm;
+}
+
+static void halbtc8821a1ant_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] |= BIT(0);
+
+	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 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"));
+	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;
+	}
+	halbtc8821a1ant_set_bt_auto_report(btcoexist, coex_dm->cur_bt_auto_report);
+
+	coex_dm->pre_bt_auto_report = coex_dm->cur_bt_auto_report;
+}
+
+static void 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] |= 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  */
+	}
+
+	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 halbtc8821a1ant_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;
+	}
+	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 halbtc8821a1ant_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 halbtc8821a1ant_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_val_0x6c0 = val0x6c0;
+	coex_dm->cur_val_0x6c4 = val0x6c4;
+	coex_dm->cur_val_0x6c8 = val0x6c8;
+	coex_dm->cur_val_0x6cc = val0x6cc;
+
+	if (!force_exec) {
+		if ((coex_dm->pre_val_0x6c0 == coex_dm->cur_val_0x6c0) &&
+		    (coex_dm->pre_val_0x6c4 == coex_dm->cur_val_0x6c4) &&
+		    (coex_dm->pre_val_0x6c8 == coex_dm->cur_val_0x6c8) &&
+		    (coex_dm->pre_val_0x6cc == coex_dm->cur_val_0x6cc))
+			return;
+	}
+	halbtc8821a1ant_set_coex_table(btcoexist, val0x6c0, val0x6c4,
+				       val0x6c8, val0x6cc);
+
+	coex_dm->pre_val_0x6c0 = coex_dm->cur_val_0x6c0;
+	coex_dm->pre_val_0x6c4 = coex_dm->cur_val_0x6c4;
+	coex_dm->pre_val_0x6c8 = coex_dm->cur_val_0x6c8;
+	coex_dm->pre_val_0x6cc = coex_dm->cur_val_0x6cc;
+}
+
+static void halbtc8821a1ant_coex_table_with_type(struct btc_coexist *btcoexist,
+						 bool force_exec, u8 type)
+{
+	switch (type) {
+	case 0:
+		halbtc8821a1ant_coex_table(btcoexist, force_exec,
+					   0x55555555, 0x55555555,
+					   0xffffff, 0x3);
+		break;
+	case 1:
+		halbtc8821a1ant_coex_table(btcoexist, force_exec,
+					   0x55555555, 0x5a5a5a5a,
+					   0xffffff, 0x3);
+		break;
+	case 2:
+		halbtc8821a1ant_coex_table(btcoexist, force_exec,
+					   0x5a5a5a5a, 0x5a5a5a5a,
+					   0xffffff, 0x3);
+		break;
+	case 3:
+		halbtc8821a1ant_coex_table(btcoexist, force_exec,
+					   0x55555555, 0xaaaaaaaa,
+					   0xffffff, 0x3);
+		break;
+	case 4:
+		halbtc8821a1ant_coex_table(btcoexist, force_exec,
+					   0xffffffff, 0xffffffff,
+					   0xffffff, 0x3);
+		break;
+	case 5:
+		halbtc8821a1ant_coex_table(btcoexist, force_exec,
+					   0x5fff5fff, 0x5fff5fff,
+					   0xffffff, 0x3);
+		break;
+	case 6:
+		halbtc8821a1ant_coex_table(btcoexist, force_exec,
+					   0x55ff55ff, 0x5a5a5a5a,
+					   0xffffff, 0x3);
+		break;
+	case 7:
+		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)
+{
+	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 halbtc8821a1ant_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], 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;
+	}
+	halbtc8821a1ant_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
+	)
+{
+	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], 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
+	)
+{
+	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
+	)
+{
+	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;
+		}
+	}
+	halbtc8821a1ant_set_lps_rpwm(btcoexist, lps_val, rpwm_val);
+
+	coex_dm->pre_lps = coex_dm->cur_lps;
+	coex_dm->pre_rpwm = coex_dm->cur_rpwm;
+}
+
+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);
+
+	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
+	)
+{
+	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 */
+		u4_tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
+		u4_tmp &= ~BIT(23);
+		u4_tmp |= BIT(24);
+		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 */
+			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 */
+		} else {
+			/* 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 */
+		}
+	} else if (wifi_off) {
+		/*  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);
+		btcoexist->btc_write_4byte(btcoexist, 0x4c, u4_tmp);
+	}
+
+	/*  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);
+		else
+			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);
+		else
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7, 0x30, 0x1);
+		break;
+	case BTC_ANT_PATH_PTA:
+	default:
+		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7, 0x30, 0x1);
+		else
+			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
+	)
+{
+	u8 rssi_adjust_val = 0;
+
+	coex_dm->cur_ps_tdma_on = turn_on;
+	coex_dm->cur_ps_tdma = type;
+
+	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:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x1a, 0x1a, 0x0, 0x50);
+			break;
+		case 1:
+			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);
+			rssi_adjust_val = 14;
+			break;
+		case 3:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x1d, 0x1d, 0x0, 0x10);
+			break;
+		case 4:
+			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);
+			break;
+		case 6:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x13, 0xa, 0x3, 0x0, 0x0);
+			break;
+		case 7:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x13, 0xc, 0x5, 0x0, 0x0);
+			break;
+		case 8:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x93, 0x25, 0x3, 0x10, 0x0);
+			break;
+		case 9:
+			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);
+			break;
+		case 11:
+			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);
+			break;
+		case 13:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x18, 0x18, 0x0, 0x10);
+			break;
+		case 14:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x21, 0x3, 0x10, 0x10);
+			break;
+		case 15:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x13, 0xa, 0x3, 0x8, 0x0);
+			break;
+		case 16:
+			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);
+			rssi_adjust_val = 14;
+			break;
+		case 20:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x61, 0x35, 0x03, 0x11, 0x10);
+			break;
+		case 21:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x61, 0x15, 0x03, 0x11, 0x10);
+			break;
+		case 22:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x61, 0x25, 0x03, 0x11, 0x10);
+			break;
+		case 23:
+			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);
+			rssi_adjust_val = 22;
+			break;
+		case 25:
+			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);
+			rssi_adjust_val = 22;
+			break;
+		case 27:
+			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);
+			break;
+		case 29:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xab, 0x1a, 0x1a, 0x1, 0x10);
+			break;
+		case 30:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x14, 0x3, 0x10, 0x50);
+			break;
+		case 31:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xd3, 0x1a, 0x1a, 0, 0x58);
+			break;
+		case 32:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x61, 0xa, 0x3, 0x10, 0x0);
+			break;
+		case 33:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xa3, 0x25, 0x3, 0x30, 0x90);
+			break;
+		case 34:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x53, 0x1a, 0x1a, 0x0, 0x10);
+			break;
+		case 35:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x63, 0x1a, 0x1a, 0x0, 0x10);
+			break;
+		case 36:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xd3, 0x12, 0x3, 0x14, 0x50);
+				break;
+		}
+	} else {
+		/*  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);
+			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);
+			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);
+			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);
+			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
+halbtc8821a1ant_is_common_action(
+	struct btc_coexist *btcoexist
+	)
+{
+	bool bCommon = 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);
+
+	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");
+		halbtc8821a1ant_sw_mechanism(btcoexist, false);
+
+		bCommon = 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");
+		halbtc8821a1ant_sw_mechanism(btcoexist, false);
+
+		bCommon = 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");
+		halbtc8821a1ant_sw_mechanism(btcoexist, false);
+
+		bCommon = 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");
+		halbtc8821a1ant_sw_mechanism(btcoexist, false);
+
+		bCommon = 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");
+		halbtc8821a1ant_sw_mechanism(btcoexist, false);
+
+		bCommon = 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");
+		}
+
+		bCommon = false;
+	}
+
+	return bCommon;
+}
+
+static void
+halbtc8821a1ant_tdma_duration_adjust_for_acl(
+	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");
+
+	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;
+
+			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");
+
+		halbtc8821a1ant_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 {
+		/* 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 */
+			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_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;
+			} else if (coex_dm->cur_ps_tdma == 1) {
+				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 2);
+				coex_dm->ps_tdma_du_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;
+			} else if (coex_dm->cur_ps_tdma == 9) {
+				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 11);
+				coex_dm->ps_tdma_du_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;
+			} else if (coex_dm->cur_ps_tdma == 11) {
+				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
+				coex_dm->ps_tdma_du_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;
+			} else if (coex_dm->cur_ps_tdma == 2) {
+				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 1);
+				coex_dm->ps_tdma_du_adj_type = 1;
+			}
+		} else {
+			/* 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);
+		}
+	}
+}
+
+static void
+halbtc8821a1ant_ps_tdma_check_for_power_save_state(
+	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) {
+			/*  will leave LPS state, turn off psTdma first */
+			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 0);
+		}
+	} else {
+		/*  NO PS state */
+		if (new_ps_state) {
+			/*  will enter LPS state, turn off psTdma first */
+			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 0);
+		} else {
+			/*  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
+	)
+{
+	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:
+		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. */
+		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:
+		halbtc8821a1ant_ps_tdma_check_for_power_save_state(btcoexist, false);
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+		break;
+	default:
+		break;
+	}
+}
+
+static void
+halbtc8821a1ant_coex_under_5g(
+	struct btc_coexist *btcoexist
+	)
+{
+	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);
+
+	halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+
+	halbtc8821a1ant_limited_tx(btcoexist, NORMAL_EXEC, 0, 0, 0, 0);
+
+	halbtc8821a1ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 5);
+}
+
+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 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");
+			halbtc8821a1ant_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) {
+			btcoexist->btc_set(btcoexist,
+					   BTC_SET_ACT_LEAVE_LPS, NULL);
+			btcoexist->btc_set(btcoexist,
+					   BTC_SET_ACT_NORMAL_LPS, NULL);
+		}
+	}
+}
+
+/*	Software Coex Mechanism start */
+static void halbtc8821a1ant_action_sco(struct btc_coexist *btcoexist)
+{
+	halbtc8821a1ant_sw_mechanism(btcoexist, true);
+}
+
+static void halbtc8821a1ant_action_hid(struct btc_coexist *btcoexist)
+{
+	halbtc8821a1ant_sw_mechanism(btcoexist, true);
+}
+
+/* A2DP only / PAN(EDR) only/ A2DP+PAN(HS) */
+static void halbtc8821a1ant_action_a2dp(struct btc_coexist *btcoexist)
+{
+	halbtc8821a1ant_sw_mechanism(btcoexist, false);
+}
+
+static void halbtc8821a1ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
+{
+	halbtc8821a1ant_sw_mechanism(btcoexist, false);
+}
+
+static void halbtc8821a1ant_action_pan_edr(struct btc_coexist *btcoexist)
+{
+	halbtc8821a1ant_sw_mechanism(btcoexist, false);
+}
+
+/* PAN(HS) only */
+static void halbtc8821a1ant_action_pan_hs(struct btc_coexist *btcoexist)
+{
+	halbtc8821a1ant_sw_mechanism(btcoexist, false);
+}
+
+/* PAN(EDR)+A2DP */
+static void halbtc8821a1ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
+{
+	halbtc8821a1ant_sw_mechanism(btcoexist, false);
+}
+
+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)
+{
+	halbtc8821a1ant_sw_mechanism(btcoexist, true);
+}
+
+static void halbtc8821a1ant_action_hid_a2dp(struct btc_coexist *btcoexist)
+{
+	halbtc8821a1ant_sw_mechanism(btcoexist, true);
+}
+
+/*	Non-Software Coex Mechanism start */
+static void halbtc8821a1ant_action_hs(struct btc_coexist *btcoexist)
+{
+	halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
+	halbtc8821a1ant_coex_table_with_type(btcoexist, FORCE_EXEC, 2);
+}
+
+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);
+
+	if (!wifi_connected) {
+		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);
+		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_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 */
+	halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
+
+	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)
+{
+	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);
+		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)) {
+			 halbtc8821a1ant_tdma_duration_adjust_for_acl(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 */
+		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			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);
+			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) */
+		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) */
+		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 13);
+		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		coex_dm->auto_tdma_adjust = false;
+	} else {
+		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 11);
+		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		coex_dm->auto_tdma_adjust = false;
+	}
+}
+
+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);
+
+	/*  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)
+{
+	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
+	)
+{
+	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);
+
+	/*  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);
+		} 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,
+			BT_8821A_1ANT_WIFI_STATUS_CONNECTED_SCAN);
+	} else {
+		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
+		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	}
+}
+
+static void 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);
+
+	halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	/*  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);
+		} else {
+			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);
+		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	}
+}
+
+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;
+
+	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) {
+		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) {
+		halbtc8821a1ant_action_wifi_connected_scan(btcoexist);
+		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);
+	else
+		halbtc8821a1ant_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_8821A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
+			action_wifi_connected_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,
+				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);
+		}
+	} else {
+		if (BT_8821A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
+			action_wifi_connected_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,
+				BT_8821A_1ANT_WIFI_STATUS_CONNECTED_BUSY);
+		} else {
+			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 5);
+			halbtc8821a1ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 2);
+		}
+	}
+}
+
+static void run_sw_coexist_mechanism(struct btc_coexist *btcoexist)
+{
+	u8 algorithm = 0;
+
+	algorithm = halbtc8821a1ant_action_algorithm(btcoexist);
+	coex_dm->cur_algorithm = algorithm;
+
+	if (!halbtc8821a1ant_is_common_action(btcoexist)) {
+		switch (coex_dm->cur_algorithm) {
+		case BT_8821A_1ANT_COEX_ALGO_SCO:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = SCO.\n");
+			halbtc8821a1ant_action_sco(btcoexist);
+			break;
+		case BT_8821A_1ANT_COEX_ALGO_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = HID.\n");
+			halbtc8821a1ant_action_hid(btcoexist);
+			break;
+		case BT_8821A_1ANT_COEX_ALGO_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = A2DP.\n");
+			halbtc8821a1ant_action_a2dp(btcoexist);
+			break;
+		case BT_8821A_1ANT_COEX_ALGO_A2DP_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = A2DP+PAN(HS).\n");
+			halbtc8821a1ant_action_a2dp_pan_hs(btcoexist);
+			break;
+		case BT_8821A_1ANT_COEX_ALGO_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = PAN(EDR).\n");
+			halbtc8821a1ant_action_pan_edr(btcoexist);
+			break;
+		case BT_8821A_1ANT_COEX_ALGO_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = HS mode.\n");
+			halbtc8821a1ant_action_pan_hs(btcoexist);
+			break;
+		case BT_8821A_1ANT_COEX_ALGO_PANEDR_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = PAN+A2DP.\n");
+			halbtc8821a1ant_action_pan_edr_a2dp(btcoexist);
+			break;
+		case BT_8821A_1ANT_COEX_ALGO_PANEDR_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = PAN(EDR)+HID.\n");
+			halbtc8821a1ant_action_pan_edr_hid(btcoexist);
+			break;
+		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);
+			break;
+		case BT_8821A_1ANT_COEX_ALGO_HID_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = HID+A2DP.\n");
+			halbtc8821a1ant_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 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;
+
+	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;
+	}
+
+	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");
+		halbtc8821a1ant_coex_under_5g(btcoexist);
+		return;
+	}
+
+	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)) {
+		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);
+
+	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);
+			if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+				halbtc8821a1ant_limited_tx(btcoexist,
+							   NORMAL_EXEC, 1, 1,
+							   1, 1);
+			else
+				halbtc8821a1ant_limited_tx(btcoexist,
+							   NORMAL_EXEC, 1, 1,
+							   1, 1);
+		} else {
+			halbtc8821a1ant_limited_tx(btcoexist, NORMAL_EXEC,
+						   0, 0, 0, 0);
+		}
+	}
+
+	if (bt_link_info->sco_exist) {
+		bt_ctrl_agg_buf_size = true;
+		agg_buf_size = 0x3;
+	} else if (bt_link_info->hid_exist) {
+		bt_ctrl_agg_buf_size = true;
+		agg_buf_size = 0x5;
+	} else if (bt_link_info->a2dp_exist || bt_link_info->pan_exist) {
+		bt_ctrl_agg_buf_size = true;
+		agg_buf_size = 0x8;
+	}
+	halbtc8821a1ant_limited_rx(btcoexist, NORMAL_EXEC, false,
+				   bt_ctrl_agg_buf_size, agg_buf_size);
+
+	run_sw_coexist_mechanism(btcoexist);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	if (coex_sta->c2h_bt_inquiry_page) {
+		halbtc8821a1ant_action_bt_inquiry(btcoexist);
+		return;
+	} else if (bt_hs_on) {
+		halbtc8821a1ant_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)
+			wifi_not_connected_asso_auth_scan(btcoexist);
+		else
+			halbtc8821a1ant_action_wifi_not_connected(btcoexist);
+	} else {
+		/*  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 */
+	halbtc8821a1ant_sw_mechanism(btcoexist, false);
+
+	halbtc8821a1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 8);
+	halbtc8821a1ant_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
+}
+
+static void halbtc8821a1ant_init_hw_config(struct btc_coexist *btcoexist,
+					   bool back_up)
+{
+	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_retry_limit =
+			btcoexist->btc_read_2byte(btcoexist, 0x42a);
+		coex_dm->backup_ampdu_max_time =
+			btcoexist->btc_read_1byte(btcoexist, 0x456);
+	}
+
+	/*  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);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
+
+	/* 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 */
+	halbtc8821a1ant_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);
+}
+
+/*  */
+/*  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
+	)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, "[BTCoex], Coex Mechanism Init!!\n");
+
+	btcoexist->stop_coex_dm = false;
+
+	halbtc8821a1ant_init_coex_dm(btcoexist);
+
+	halbtc8821a1ant_query_bt_info(btcoexist);
+}
+
+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;
+	u8 u1_tmp[4], i, bt_info_ext, ps_tdma_case = 0;
+	u16 u2_tmp[4];
+	u32 u4_tmp[4];
+	bool roam = false, scan = false, link = false, wifi_under_5g = false;
+	bool bt_hs_on = false, wifi_busy = false;
+	long wifi_rssi = 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,
+		   "\n ============[BT Coexist info] ============");
+	CL_PRINTF(cli_buf);
+
+	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);
+	}
+	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);
+	}
+
+	if (!board_info->bt_exist) {
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n BT not exists !!!");
+		CL_PRINTF(cli_buf);
+		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,
+		   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",
+		   ((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_%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);
+
+	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)",
+		   "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 ",
+		   "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",
+		   (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",
+		   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",
+		   (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,
+		   "\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->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);
+	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",
+		   "BT Info A2DP rate",
+		   (bt_info_ext & BIT(0)) ? "Basic rate" : "EDR rate");
+	CL_PRINTF(cli_buf);
+
+	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)",
+				   glbt_info_src_8821a_1ant[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, "\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,
+			   "\n %-35s", "============[Sw mechanism] ============");
+		CL_PRINTF(cli_buf);
+
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\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",
+			   (btcoexist->bt_info.reject_agg_pkt ? "Yes" : "No"),
+			   (btcoexist->bt_info.b_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",
+			   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);
+
+		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)",
+			   "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, "\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",
+			   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);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\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);
+
+	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",
+		   "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]",
+		   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]",
+		   ((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",
+		   "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);
+
+	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",
+		   "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",
+		   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)",
+		   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",
+		   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",
+		   "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)",
+		   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)",
+		   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
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_COEX_STATISTICS);
+}
+
+void ex_halbtc8821a1ant_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;
+		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);
+	} else if (BTC_IPS_LEAVE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], IPS LEAVE notify\n");
+		coex_sta->under_ips = false;
+
+		halbtc8821a1ant_run_coexist_mechanism(btcoexist);
+	}
+}
+
+void ex_halbtc8821a1ant_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_halbtc8821a1ant_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);
+
+	halbtc8821a1ant_query_bt_info(btcoexist);
+
+	if (coex_sta->c2h_bt_inquiry_page) {
+		halbtc8821a1ant_action_bt_inquiry(btcoexist);
+		return;
+	} else if (bt_hs_on) {
+		halbtc8821a1ant_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 */
+			wifi_not_connected_asso_auth_scan(btcoexist);
+		} else {
+			/*  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 */
+			halbtc8821a1ant_action_wifi_not_connected(btcoexist);
+		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;
+
+	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) {
+		halbtc8821a1ant_action_bt_inquiry(btcoexist);
+		return;
+	} else if (bt_hs_on) {
+		halbtc8821a1ant_action_hs(btcoexist);
+		return;
+	}
+
+	if (BTC_ASSOCIATE_START == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], CONNECT START notify\n");
+		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 */
+			halbtc8821a1ant_action_wifi_not_connected(btcoexist);
+		else
+			halbtc8821a1ant_action_wifi_connected(btcoexist);
+	}
+}
+
+void ex_halbtc8821a1ant_media_status_notify(struct btc_coexist *btcoexist,
+					    u8 type)
+{
+	u8 h2c_parameter[3] = {0};
+	u32 wifi_bw;
+	u8 wifi_central_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_central_chnl);
+	if ((BTC_MEDIA_CONNECT == type) &&
+	    (wifi_central_chnl <= 14)) {
+		/* 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);
+		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_halbtc8821a1ant_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) {
+		halbtc8821a1ant_action_bt_inquiry(btcoexist);
+		return;
+	} else if (bt_hs_on) {
+		halbtc8821a1ant_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);
+		action_wifi_connected_special_packet(btcoexist);
+	}
+}
+
+void ex_halbtc8821a1ant_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;
+	bool wifi_under_5g = false;
+
+	coex_sta->c2h_bt_info_req_sent = false;
+
+	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)
+		rsp_source = BT_INFO_SRC_8821A_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_8821A_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 & 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)
+				ex_halbtc8821a1ant_media_status_notify(btcoexist,
+								       BTC_MEDIA_CONNECT);
+			else
+				ex_halbtc8821a1ant_media_status_notify(btcoexist,
+								       BTC_MEDIA_DISCONNECT);
+		}
+
+		if ((coex_sta->bt_info_ext & BIT(3)) && !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);
+			}
+		} 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 {
+			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,
+						       FORCE_EXEC, true);
+		}
+#endif
+	}
+
+	/*  check BIT(2) 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 */
+	if (!(bt_info&BT_INFO_8821A_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_8821A_1ANT_B_FTP)
+			coex_sta->pan_exist = true;
+		else
+			coex_sta->pan_exist = false;
+		if (bt_info & BT_INFO_8821A_1ANT_B_A2DP)
+			coex_sta->a2dp_exist = true;
+		else
+			coex_sta->a2dp_exist = false;
+		if (bt_info & BT_INFO_8821A_1ANT_B_HID)
+			coex_sta->hid_exist = true;
+		else
+			coex_sta->hid_exist = false;
+		if (bt_info & BT_INFO_8821A_1ANT_B_SCO_ESCO)
+			coex_sta->sco_exist = true;
+		else
+			coex_sta->sco_exist = false;
+	}
+
+	halbtc8821a1ant_update_bt_link_info(btcoexist);
+
+	if (!(bt_info&BT_INFO_8821A_1ANT_B_CONNECTION)) {
+		coex_dm->bt_status = BT_8821A_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_8821A_1ANT_B_CONNECTION) {
+		/*  connection exists but not 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)) {
+		coex_dm->bt_status = BT_8821A_1ANT_BT_STATUS_SCO_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT SCO busy!!!\n");
+	} else if (bt_info&BT_INFO_8821A_1ANT_B_ACL_BUSY) {
+		if (BT_8821A_1ANT_BT_STATUS_ACL_BUSY != coex_dm->bt_status)
+			coex_dm->auto_tdma_adjust = false;
+		coex_dm->bt_status = BT_8821A_1ANT_BT_STATUS_ACL_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT ACL busy!!!\n");
+	} else {
+		coex_dm->bt_status = BT_8821A_1ANT_BT_STATUS_MAX;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT Non-Defined state!!!\n");
+	}
+
+	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_busy = true;
+	else
+		bt_busy = false;
+	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");
+
+	btcoexist->stop_coex_dm = 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_ps_tdma(btcoexist, FORCE_EXEC, false, 0);
+
+	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");
+
+	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;
+		halbtc8821a1ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
+		halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+						 0x0, 0x0);
+		halbtc8821a1ant_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;
+		halbtc8821a1ant_init_hw_config(btcoexist, false);
+		halbtc8821a1ant_init_coex_dm(btcoexist);
+		halbtc8821a1ant_query_bt_info(btcoexist);
+	}
+}
+
+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");
+
+	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_8821a_1ant, glcoex_ver_8821a_1ant,
+			  fw_ver, bt_patch_ver, bt_patch_ver);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], ****************************************************************\n");
+	}
+
+#if (BT_AUTO_REPORT_ONLY_8821A_1ANT == 0)
+	halbtc8821a1ant_query_bt_info(btcoexist);
+	halbtc8821a1ant_monitor_bt_ctr(btcoexist);
+	halbtc8821a1ant_monitor_bt_enable_disable(btcoexist);
+#else
+	if (halbtc8821a1ant_Is_wifi_status_changed(btcoexist) ||
+	    coex_dm->auto_tdma_adjust) {
+		if (coex_sta->special_pkt_period_cnt > 2)
+			halbtc8821a1ant_run_coexist_mechanism(btcoexist);
+	}
+
+	coex_sta->special_pkt_period_cnt++;
+#endif
+}
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8821a1ant.h b/drivers/staging/rtl8192ee/btcoexist/halbtc8821a1ant.h
new file mode 100644
index 00000000000..9b991d021a8
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtc8821a1ant.h
@@ -0,0 +1,158 @@
+/*  */
+/*  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.c b/drivers/staging/rtl8192ee/btcoexist/halbtc8821a2ant.c
new file mode 100644
index 00000000000..7fb59073901
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtc8821a2ant.c
@@ -0,0 +1,3438 @@
+/*  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 */
+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;
+static struct coex_sta_8821a_2ant	*coex_sta = &glcoex_sta_8821a_2ant;
+
+static const char *const glbt_info_src_8821a_2ant[] = {
+	"BT Info[wifi fw]",
+	"BT Info[bt rsp]",
+	"BT Info[bt auto report]",
+};
+
+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_ */
+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;
+
+	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)) {
+				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_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");
+			} 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_8821A_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 wifi21a_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];
+
+	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_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_8821A_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_8821A_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 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 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;
+
+	reg_hp_txrx = 0x770;
+	reg_lp_txrx = 0x774;
+
+	u4tmp = btcoexist->btc_read_4byte(btcoexist, reg_hp_txrx);
+	reg_hp_tx = u4tmp & MASKLWORD;
+	reg_hp_rx = (u4tmp & MASKHWORD)>>16;
+
+	u4tmp = btcoexist->btc_read_4byte(btcoexist, reg_lp_txrx);
+	reg_lp_tx = u4tmp & MASKLWORD;
+	reg_lp_rx = (u4tmp & 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 halbtc8821a2ant_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 u8 halbtc8821a2ant_action_algorithm(struct btc_coexist *btcoexist)
+{
+	struct btc_stack_info *stack_info = &btcoexist->stack_info;
+	bool bt_hs_on = false;
+	u8 algorithm = BT_8821A_2ANT_COEX_ALGO_UNDEFINED;
+	u8 num_of_diff_profile = 0;
+
+	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 */
+		stack_info->hid_exist = coex_sta->hid_exist;
+	}
+	/*  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;
+
+	if (!coex_sta->bt_link_exist) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], No profile exists!!!\n");
+		return algorithm;
+	}
+
+	if (coex_sta->sco_exist)
+		num_of_diff_profile++;
+	if (coex_sta->hid_exist)
+		num_of_diff_profile++;
+	if (coex_sta->pan_exist)
+		num_of_diff_profile++;
+	if (coex_sta->a2dp_exist)
+		num_of_diff_profile++;
+
+	if (num_of_diff_profile == 1) {
+		if (coex_sta->sco_exist) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], SCO only\n");
+			algorithm = BT_8821A_2ANT_COEX_ALGO_SCO;
+		} else {
+			if (coex_sta->hid_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], HID only\n");
+				algorithm = BT_8821A_2ANT_COEX_ALGO_HID;
+			} else if (coex_sta->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], A2DP only\n");
+				algorithm = BT_8821A_2ANT_COEX_ALGO_A2DP;
+			} else if (coex_sta->pan_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], PAN(HS) only\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_PANHS;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], PAN(EDR) only\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR;
+				}
+			}
+		}
+	} 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");
+				algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_HID;
+			} else if (coex_sta->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], SCO + A2DP ==> SCO\n");
+				algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_HID;
+			} else if (coex_sta->pan_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], SCO + PAN(HS)\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_SCO;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], SCO + PAN(EDR)\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		} else {
+			if (coex_sta->hid_exist &&
+			    coex_sta->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], HID + A2DP\n");
+				algorithm = BT_8821A_2ANT_COEX_ALGO_HID_A2DP;
+			} else if (coex_sta->hid_exist &&
+				   coex_sta->pan_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], HID + PAN(HS)\n");
+					algorithm =  BT_8821A_2ANT_COEX_ALGO_HID;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], HID + PAN(EDR)\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			} else if (coex_sta->pan_exist &&
+				   coex_sta->a2dp_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], A2DP + PAN(HS)\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_A2DP_PANHS;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], A2DP + PAN(EDR)\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_A2DP;
+				}
+			}
+		}
+	} else if (num_of_diff_profile == 3) {
+		if (coex_sta->sco_exist) {
+			if (coex_sta->hid_exist &&
+			    coex_sta->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], SCO + HID + A2DP ==> HID\n");
+				algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_HID;
+			} else if (coex_sta->hid_exist &&
+				   coex_sta->pan_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], SCO + HID + PAN(HS)\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_HID;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], SCO + HID + PAN(EDR)\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			} else if (coex_sta->pan_exist &&
+				   coex_sta->a2dp_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], SCO + A2DP + PAN(HS)\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_HID;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], SCO + A2DP + PAN(EDR) ==> HID\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		} else {
+			if (coex_sta->hid_exist &&
+			    coex_sta->pan_exist &&
+			    coex_sta->a2dp_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], HID + A2DP + PAN(HS)\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_HID_A2DP;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], HID + A2DP + PAN(EDR)\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_HID_A2DP_PANEDR;
+				}
+			}
+		}
+	} else if (num_of_diff_profile >= 3) {
+		if (coex_sta->sco_exist) {
+			if (coex_sta->hid_exist &&
+			    coex_sta->pan_exist &&
+			    coex_sta->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_8821A_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		}
+	}
+	return algorithm;
+}
+
+static bool halbtc8821a2ant_need_to_dec_bt_pwr(struct btc_coexist *btcoexist)
+{
+	bool ret = false;
+	bool bt_hs_on = false, wifi_connected = false;
+	long 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 = halbtc8821a2ant_bt_rssi_state(2, 35, 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 halbtc8821a2ant_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 halbtc8821a2ant_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], 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;
+	}
+	halbtc8821a2ant_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 set_fw_bt_lna_constrain(struct btc_coexist *btcoexist,
+				    bool bt_lna_cons_on)
+{
+	u8 h2c_parameter[2] = {0};
+
+	h2c_parameter[0] = 0x3;	/*  opCode, 0x3 = BT_SET_LNA_CONSTRAIN */
+
+	if (bt_lna_cons_on)
+		h2c_parameter[1] |= BIT(0);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], set BT LNA Constrain: %s, FW write 0x69 = 0x%x\n",
+		  (bt_lna_cons_on ? "ON!!" : "OFF!!"),
+		  h2c_parameter[0]<<8|h2c_parameter[1]);
+
+	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)
+{
+	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->cur_bt_lna_constrain = bt_lna_cons_on;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], pre_bt_lna_constrain =%d, cur_bt_lna_constrain =%d\n",
+			  coex_dm->pre_bt_lna_constrain,
+			  coex_dm->cur_bt_lna_constrain);
+
+		if (coex_dm->pre_bt_lna_constrain ==
+		    coex_dm->cur_bt_lna_constrain)
+			return;
+	}
+	set_fw_bt_lna_constrain(btcoexist, coex_dm->cur_bt_lna_constrain);
+
+	coex_dm->pre_bt_lna_constrain = coex_dm->cur_bt_lna_constrain;
+}
+
+static void halbtc8821a2ant_set_fw_bt_psd_mode(struct btc_coexist *btcoexist,
+					       u8 bt_psd_mode)
+{
+	u8 h2c_parameter[2] = {0};
+
+	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]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x69, 2, h2c_parameter);
+}
+
+static void halbtc8821a2ant_set_bt_psd_mode(struct btc_coexist *btcoexist,
+					    bool force_exec, u8 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->cur_bt_psd_mode = bt_psd_mode;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], pre_bt_psd_mode = 0x%x, cur_bt_psd_mode = 0x%x\n",
+			  coex_dm->pre_bt_psd_mode, coex_dm->cur_bt_psd_mode);
+
+		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);
+
+	coex_dm->pre_bt_psd_mode = coex_dm->cur_bt_psd_mode;
+}
+
+static void halbtc8821a2ant_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] |= BIT(0);
+
+	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 halbtc8821a2ant_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], 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;
+	}
+	halbtc8821a2ant_set_bt_auto_report(btcoexist,
+					   coex_dm->cur_bt_auto_report);
+
+	coex_dm->pre_bt_auto_report = coex_dm->cur_bt_auto_report;
+}
+
+static void halbtc8821a2ant_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], 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;
+	}
+
+	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->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);
+		}
+	}
+}
+
+static void halbtc8821a2ant_RfShrink(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], 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;
+	}
+	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)
+{
+	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 halbtc8821a2ant_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], 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);
+
+	coex_dm->pre_low_penalty_ra = coex_dm->cur_low_penalty_ra;
+}
+
+static void halbtc8821a2ant_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, 0xc5b, 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)
+		halbtc8821a2ant_set_dac_swing_reg(btcoexist, sw_dac_swing_lvl);
+	else
+		halbtc8821a2ant_set_dac_swing_reg(btcoexist, 0x18);
+}
+
+static void halbtc8821a2ant_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], 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;
+	}
+	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 halbtc8821a2ant_set_adc_back_off(struct btc_coexist *btcoexist,
+					     bool 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);
+	}
+}
+
+static void halbtc8821a2ant_adc_back_off(struct btc_coexist *btcoexist,
+					 bool force_exec, bool 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;
+	}
+	halbtc8821a2ant_set_adc_back_off(btcoexist, coex_dm->cur_adc_back_off);
+
+	coex_dm->pre_adc_back_off = coex_dm->cur_adc_back_off;
+}
+
+static void halbtc8821a2ant_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 halbtc8821a2ant_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], 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;
+	}
+	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;
+	coex_dm->pre_val0x6c8 = coex_dm->cur_val0x6c8;
+	coex_dm->pre_val0x6cc = coex_dm->cur_val0x6cc;
+}
+
+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 halbtc8821a2ant_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], 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);
+
+	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
+}
+
+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};
+
+	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 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;
+	}
+
+	 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); */
+}
+
+static void sw_mechanism2(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);
+	halbtc8821a2ant_dac_swing(btcoexist, NORMAL_EXEC, sw_dac_swing,
+				  sw_dac_swing);
+}
+
+static void halbtc8821a2ant_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 u4tmp = 0;
+	u8 h2c_parameter[2] = {0};
+
+	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);
+		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 */
+			h2c_parameter[0] = 1;
+			h2c_parameter[1] = 1;
+			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2, h2c_parameter);
+		} else {
+			/* 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);
+		}
+	}
+
+	/*  ext switch setting */
+	switch (ant_pos_type) {
+	case BTC_ANT_WIFI_AT_MAIN:
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7, 0x30, 0x1);
+		break;
+	case BTC_ANT_WIFI_AT_AUX:
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7, 0x30, 0x2);
+		break;
+	}
+}
+
+static void ps21a_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);
+	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) {
+		case 1:
+		default:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x1a,
+						      0x1a, 0xe1, 0x90);
+			break;
+		case 2:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x12,
+						      0x12, 0xe1, 0x90);
+			break;
+		case 3:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x1c,
+						      0x3, 0xf1, 0x90);
+			break;
+		case 4:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x10,
+						      0x03, 0xf1, 0x90);
+			break;
+		case 5:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x1a,
+						      0x1a, 0x60, 0x90);
+			break;
+		case 6:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x12,
+						      0x12, 0x60, 0x90);
+			break;
+		case 7:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x1c,
+						      0x3, 0x70, 0x90);
+			break;
+		case 8:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xa3, 0x10,
+						      0x3, 0x70, 0x90);
+			break;
+		case 9:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x1a,
+						      0x1a, 0xe1, 0x90);
+			break;
+		case 10:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x12,
+						      0x12, 0xe1, 0x90);
+			break;
+		case 11:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0xa,
+						      0xa, 0xe1, 0x90);
+			break;
+		case 12:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x5,
+						      0x5, 0xe1, 0x90);
+			break;
+		case 13:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x1a,
+						      0x1a, 0x60, 0x90);
+			break;
+		case 14:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x12,
+						      0x12, 0x60, 0x90);
+			break;
+		case 15:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0xa,
+						      0xa, 0x60, 0x90);
+			break;
+		case 16:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x5,
+						      0x5, 0x60, 0x90);
+			break;
+		case 17:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xa3, 0x2f,
+						      0x2f, 0x60, 0x90);
+			break;
+		case 18:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x5,
+						      0x5, 0xe1, 0x90);
+			break;
+		case 19:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x25,
+						      0x25, 0xe1, 0x90);
+			break;
+		case 20:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x25,
+						      0x25, 0x60, 0x90);
+			break;
+		case 21:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x15,
+						      0x03, 0x70, 0x90);
+			break;
+		case 71:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x1a,
+						      0x1a, 0xe1, 0x90);
+			break;
+		}
+	} else {
+		/*  disable PS tdma */
+		switch (type) {
+		case 0:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0x0, 0x0, 0x0,
+						      0x40, 0x0);
+			break;
+		case 1:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0x0, 0x0, 0x0,
+						      0x48, 0x0);
+			break;
+		default:
+			halbtc8821a2ant_set_fw_pstdma(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 halbtc8821a2ant_coex_all_off(struct btc_coexist *btcoexist)
+{
+	/*  fw all off */
+	ps21a_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);
+
+	/*  hw all off */
+	halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55555555,
+				   0x55555555, 0xffff, 0x3);
+}
+
+static void halbtc8821a2ant_coex_under_5g(struct btc_coexist *btcoexist)
+{
+	halbtc8821a2ant_coex_all_off(btcoexist);
+}
+
+static void halbtc8821a2ant_init_coex_dm(struct btc_coexist *btcoexist)
+{
+	/*  force to reset coex mechanism */
+	halbtc8821a2ant_coex_table(btcoexist, FORCE_EXEC, 0x55555555,
+				   0x55555555, 0xffff, 0x3);
+
+	ps21a_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);
+}
+
+static void halbtc8821a2ant_bt_inquiry_page(struct btc_coexist *btcoexist)
+{
+	bool low_pwr_disable = true;
+
+	btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
+			   &low_pwr_disable);
+
+	halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
+				   0x5afa5afa, 0xffff, 0x3);
+	ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
+}
+
+static bool halbtc8821a2ant_is_common_action(struct btc_coexist *btcoexist)
+{
+	bool common = false, wifi_connected = false, wifi_busy = false;
+	bool low_pwr_disable = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+
+	halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
+				   0x5afa5afa, 0xffff, 0x3);
+
+	if (!wifi_connected &&
+	    BT_8821A_2ANT_BT_STATUS_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 IPS + BT IPS!!\n");
+
+		ps21a_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);
+
+		common = true;
+	} else if (wifi_connected &&
+		   (BT_8821A_2ANT_BT_STATUS_IDLE == coex_dm->bt_status)) {
+		low_pwr_disable = false;
+		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 IPS!!\n");
+			ps21a_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_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);
+
+		common = true;
+	} else if (!wifi_connected &&
+		   (BT_8821A_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);
+
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Wifi IPS + BT LPS!!\n");
+
+		ps21a_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);
+
+		common = true;
+	} else if (wifi_connected &&
+		   (BT_8821A_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 (wifi_busy) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Wifi Busy + BT LPS!!\n");
+			ps21a_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_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);
+
+		common = true;
+	} else if (!wifi_connected &&
+		   (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);
+
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Wifi IPS + BT Busy!!\n");
+
+		ps21a_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);
+
+		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 Busy + BT Busy!!\n");
+			common = false;
+		} else {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Wifi LPS + BT Busy!!\n");
+			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 21);
+
+			if (halbtc8821a2ant_need_to_dec_bt_pwr(btcoexist))
+				halbtc8821a2ant_dec_bt_pwr(btcoexist,
+							   NORMAL_EXEC, true);
+			else
+				halbtc8821a2ant_dec_bt_pwr(btcoexist,
+							   NORMAL_EXEC, false);
+
+			common = true;
+		}
+		sw_mechanism1(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 long up, dn, m, n, wait_count;
+	long result;
+	/* 0: no change, +1: incr WiFi duration, -1: decr WiFi duration */
+	u8 retry_count = 0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], TdmaDurationAdjust()\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");
+		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;
+				} else if (max_interval == 2) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 14);
+					coex_dm->ps_tdma_du_adj_type = 14;
+				} else if (max_interval == 3) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
+					coex_dm->ps_tdma_du_adj_type = 15;
+				} else {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
+					coex_dm->ps_tdma_du_adj_type = 15;
+				}
+			} else {
+				if (max_interval == 1) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 9);
+					coex_dm->ps_tdma_du_adj_type = 9;
+				} else if (max_interval == 2) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 10);
+					coex_dm->ps_tdma_du_adj_type = 10;
+				} else if (max_interval == 3) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 11);
+					coex_dm->ps_tdma_du_adj_type = 11;
+				} else {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 11);
+					coex_dm->ps_tdma_du_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;
+				} else if (max_interval == 2) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 6);
+					coex_dm->ps_tdma_du_adj_type = 6;
+				} else if (max_interval == 3) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
+					coex_dm->ps_tdma_du_adj_type = 7;
+				} else {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
+					coex_dm->ps_tdma_du_adj_type = 7;
+				}
+			} else {
+				if (max_interval == 1) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 1);
+					coex_dm->ps_tdma_du_adj_type = 1;
+				} else if (max_interval == 2) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 2);
+					coex_dm->ps_tdma_du_adj_type = 2;
+				} else if (max_interval == 3) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
+					coex_dm->ps_tdma_du_adj_type = 3;
+				} else {
+					ps21a_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 */
+		retry_count = coex_sta->bt_retry_cnt;
+		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",
+			  (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 */
+			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) {
+			/*  <= 3 retry in the last 2-second duration */
+			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) {
+				/* 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 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!!!, cur_ps_tdma =%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)
+			ps21a_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");
+	}
+
+	/* 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) */
+static void halbtc8821a2ant_action_sco(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = wifi21a_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);
+
+	if (halbtc8821a2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8821a2ant_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 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); */
+
+		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 */
+
+		/*  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);
+		} else {
+			sw_mechanism1(btcoexist, true, true, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	} else {
+		/*  fw mechanism */
+
+		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 */
+
+		/*  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);
+		} else {
+			sw_mechanism1(btcoexist, false, true, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+static void halbtc8821a2ant_action_hid(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = wifi21a_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);
+
+	if (halbtc8821a2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8821a2ant_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 */
+		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
+					   0x5a5a5a5a, 0xffff, 0x3);
+	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 */
+		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 */
+		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);
+		} else {
+			sw_mechanism1(btcoexist, true, true, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	} else {
+		/*  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 */
+		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);
+		} else {
+			sw_mechanism1(btcoexist, false, true, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+/* 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;
+
+	wifi_rssi_state = wifi21a_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); */
+
+	if (halbtc8821a2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8821a2ant_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) {
+		/*  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);
+
+		/*  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);
+		} else {
+			sw_mechanism1(btcoexist, true, false, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	} else {
+		/*  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);
+
+		/*  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);
+		} else {
+			sw_mechanism1(btcoexist, false, false, false, false);
+			sw_mechanism2(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;
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	wifi_rssi_state = wifi21a_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); */
+
+	if (halbtc8821a2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8821a2ant_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) {
+		/*  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);
+
+		/*  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);
+		} else {
+			sw_mechanism1(btcoexist, true, false, false, false);
+			sw_mechanism2(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);
+
+		/*  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);
+		} else {
+			sw_mechanism1(btcoexist, false, false, false, false);
+			sw_mechanism2(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;
+
+	wifi_rssi_state = wifi21a_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);
+
+	if (halbtc8821a2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8821a2ant_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 */
+		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
+					   0x5aff5aff, 0xffff, 0x3);
+	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 ((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);
+
+		/*  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);
+		} else {
+			sw_mechanism1(btcoexist, true, false, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	} else {
+		/*  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);
+
+		/*  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);
+		} else {
+			sw_mechanism1(btcoexist, false, false, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+/* PAN(HS) only */
+static void halbtc8821a2ant_action_pan_hs(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = wifi21a_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);
+
+	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))
+			halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
+						   true);
+		else
+			halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
+						   false);
+		ps21a_tdma(btcoexist, NORMAL_EXEC, false, 1);
+
+		/*  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);
+		} else {
+			sw_mechanism1(btcoexist, true, false, false, false);
+			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))
+			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);
+
+		/*  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);
+		} else {
+			sw_mechanism1(btcoexist, false, false, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+/* 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;
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	wifi_rssi_state = wifi21a_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);
+
+	if (halbtc8821a2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8821a2ant_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 */
+		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
+					   0x5afa5afa, 0xffff, 0x3);
+	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 */
+		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);
+		} 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);
+		}
+
+		/*  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);
+		} else {
+			sw_mechanism1(btcoexist, true, false, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		};
+	} else {
+		/*  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);
+		} 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);
+		}
+
+		/*  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);
+		} else {
+			sw_mechanism1(btcoexist, false, false, false, false);
+			sw_mechanism2(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;
+
+	wifi_rssi_state = wifi21a_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);
+
+	if (halbtc8821a2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8821a2ant_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 */
+		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
+					   0x5a5f5a5f, 0xffff, 0x3);
+	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 */
+		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);
+
+		/*  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);
+		} else {
+			sw_mechanism1(btcoexist, true, true, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	} else {
+		halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+		/*  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);
+
+		/*  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);
+		} else {
+			sw_mechanism1(btcoexist, false, true, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+/*  HID+A2DP+PAN(EDR) */
+static void 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 = wifi21a_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);
+
+	if (halbtc8821a2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8821a2ant_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 */
+		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
+					   0x5a5a5a5a, 0xffff, 0x3);
+	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 */
+		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);
+			} else {
+				/* a2dp edr rate */
+				tdma_duration_adjust(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);
+		}
+
+		/*  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);
+		} else {
+			sw_mechanism1(btcoexist, true, true, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	} else {
+		/*  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);
+		} 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);
+			}
+		}
+
+		/*  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);
+		} else {
+			sw_mechanism1(btcoexist, false, true, false, false);
+			sw_mechanism2(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;
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	wifi_rssi_state = wifi21a_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))
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8821a2ant_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 */
+		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
+					   0x5f5b5f5b, 0xffffff, 0x3);
+	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 */
+		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);
+		} 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);
+		}
+
+		/*  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);
+		} else {
+			sw_mechanism1(btcoexist, true, true, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	} else {
+		/*  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);
+		} 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);
+		}
+
+		/*  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);
+		} else {
+			sw_mechanism1(btcoexist, false, true, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+static void halbtc8821a2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
+{
+	bool wifi_under_5g = false;
+	u8 algorithm = 0;
+
+	if (btcoexist->manual_control) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Manual control!!!\n");
+		return;
+	}
+
+	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(), run 5G coex setting!!<===\n");
+		halbtc8821a2ant_coex_under_5g(btcoexist);
+		return;
+	}
+
+	algorithm = halbtc8821a2ant_action_algorithm(btcoexist);
+	if (coex_sta->c2h_bt_inquiry_page &&
+	    (BT_8821A_2ANT_COEX_ALGO_PANHS != algorithm)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BT is under inquiry/page scan !!\n");
+		halbtc8821a2ant_bt_inquiry_page(btcoexist);
+		return;
+	}
+
+	coex_dm->cur_algorithm = algorithm;
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], Algorithm = %d\n", coex_dm->cur_algorithm);
+
+	if (halbtc8821a2ant_is_common_action(btcoexist)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Action 2-Ant common.\n");
+		coex_dm->reset_tdma_adjust = true;
+	} 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);
+			coex_dm->reset_tdma_adjust = true;
+		}
+		switch (coex_dm->cur_algorithm) {
+		case BT_8821A_2ANT_COEX_ALGO_SCO:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = SCO.\n");
+			halbtc8821a2ant_action_sco(btcoexist);
+			break;
+		case BT_8821A_2ANT_COEX_ALGO_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = HID.\n");
+			halbtc8821a2ant_action_hid(btcoexist);
+			break;
+		case BT_8821A_2ANT_COEX_ALGO_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = A2DP.\n");
+			halbtc8821a2ant_action_a2dp(btcoexist);
+			break;
+		case BT_8821A_2ANT_COEX_ALGO_A2DP_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = A2DP+PAN(HS).\n");
+			halbtc8821a2ant_action_a2dp_pan_hs(btcoexist);
+			break;
+		case BT_8821A_2ANT_COEX_ALGO_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = PAN(EDR).\n");
+			halbtc8821a2ant_action_pan_edr(btcoexist);
+			break;
+		case BT_8821A_2ANT_COEX_ALGO_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = HS mode.\n");
+			halbtc8821a2ant_action_pan_hs(btcoexist);
+			break;
+		case BT_8821A_2ANT_COEX_ALGO_PANEDR_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = PAN+A2DP.\n");
+			halbtc8821a2ant_action_pan_edr_a2dp(btcoexist);
+			break;
+		case BT_8821A_2ANT_COEX_ALGO_PANEDR_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = PAN(EDR)+HID.\n");
+			halbtc8821a2ant_action_pan_edr_hid(btcoexist);
+			break;
+		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);
+			break;
+		case BT_8821A_2ANT_COEX_ALGO_HID_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = HID+A2DP.\n");
+			halbtc8821a2ant_action_hid_a2dp(btcoexist);
+			break;
+		default:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = coexist All Off!!\n");
+			halbtc8821a2ant_coex_all_off(btcoexist);
+			break;
+		}
+		coex_dm->pre_algorithm = coex_dm->cur_algorithm;
+	}
+}
+
+/*  work around function start with wa_halbtc8821a2ant_ */
+/*  extern function start with EXhalbtc8821a2ant_ */
+void ex_halbtc8821a2ant_init_hwconfig(struct btc_coexist *btcoexist)
+{
+	u8 u1tmp = 0;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+		  "[BTCoex], 2Ant Init HW Config!!\n");
+
+	/*  backup rf 0x1e value */
+	coex_dm->bt_rf0x1e_backup =
+		btcoexist->btc_get_rf_reg(btcoexist, BTC_RF_A, 0x1e, 0xfffff);
+
+	/*  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);
+
+	/*  PTA parameter */
+	halbtc8821a2ant_coex_table(btcoexist, FORCE_EXEC,
+				   0x55555555, 0x55555555,
+				   0xffff, 0x3);
+
+	/*  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)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+		  "[BTCoex], Coex Mechanism Init!!\n");
+
+	halbtc8821a2ant_init_coex_dm(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;
+	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;
+	bool bt_hs_on = false, wifi_busy = false;
+	long wifi_rssi = 0, bt_hs_rssi = 0;
+	u32 wifi_bw, wifi_traffic_dir;
+	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);
+
+	if (!board_info->bt_exist) {
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n BT not exists !!!");
+		CL_PRINTF(cli_buf);
+		return;
+	}
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\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);
+	}
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\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)",
+		   "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)",
+		   "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",
+		   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",
+		   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",
+		   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",
+		   (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, "\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);
+
+	if (stack_info->profile_notified) {
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\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",
+		   "BT Info A2DP rate",
+		   (bt_info_ext & BIT(0)) ? "Basic rate" : "EDR rate");
+	CL_PRINTF(cli_buf);
+
+	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)",
+				   glbt_info_src_8821a_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, "\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 ",
+		   "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) ",
+		   "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);
+
+	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",
+			   "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",
+			   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);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\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 ",
+		   "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",
+		   "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",
+		   "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",
+		   "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",
+		   "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",
+		   "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",
+		   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",
+		   "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",
+		   "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",
+		   "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 */
+	u1tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x41b);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\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);
+}
+
+void ex_halbtc8821a2ant_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;
+		halbtc8821a2ant_coex_all_off(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_halbtc8821a2ant_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_halbtc8821a2ant_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_halbtc8821a2ant_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_halbtc8821a2ant_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_halbtc8821a2ant_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_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;
+
+	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 =[",
+		  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_8821A_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))) {
+			btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+					   &wifi_connected);
+			if (wifi_connected)
+				ex_halbtc8821a2ant_media_status_notify(btcoexist,
+								       BTC_MEDIA_CONNECT);
+			else
+				ex_halbtc8821a2ant_media_status_notify(btcoexist,
+								       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);
+			set_bt_psd_mode++;
+		}
+
+		if (coex_dm->cur_bt_lna_constrain) {
+			if (!(coex_sta->bt_info_ext & BIT(2))) {
+				if (set_bt_lna_cnt <= 3) {
+					set_bt_lna_constrain(btcoexist,
+							     FORCE_EXEC, true);
+					set_bt_lna_cnt++;
+				}
+			}
+		} else {
+			set_bt_lna_cnt = 0;
+		}
+
+		if ((coex_sta->bt_info_ext & BIT(3)))
+			halbtc8821a2ant_ignore_wlan_act(btcoexist,
+							FORCE_EXEC, false);
+		else
+			/* BT already NOT ignore Wlan active, do nothing here */
+
+		if (!(coex_sta->bt_info_ext & BIT(4)))
+			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 */
+	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 */
+			coex_sta->bt_link_exist = true;
+			coex_dm->bt_status =
+				BT_8821A_2ANT_BT_STATUS_CONNECTED_IDLE;
+		} else if (bt_info & BT_INFO_8821A_2ANT_B_CONNECTION) {
+			/*  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;
+			else
+				coex_sta->pan_exist = false;
+			if (bt_info & BT_INFO_8821A_2ANT_B_A2DP)
+				coex_sta->a2dp_exist = true;
+			else
+				coex_sta->a2dp_exist = false;
+			if (bt_info & BT_INFO_8821A_2ANT_B_HID)
+				coex_sta->hid_exist = true;
+			else
+				coex_sta->hid_exist = false;
+			if (bt_info & BT_INFO_8821A_2ANT_B_SCO_ESCO)
+				coex_sta->sco_exist = true;
+			else
+				coex_sta->sco_exist = false;
+			coex_dm->bt_status = BT_8821A_2ANT_BT_STATUS_NON_IDLE;
+		} else {
+			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;
+			coex_dm->bt_status = BT_8821A_2ANT_BT_STATUS_IDLE;
+		}
+
+		if (bt_hs_on)
+			coex_dm->bt_status = BT_8821A_2ANT_BT_STATUS_NON_IDLE;
+	}
+
+	if (BT_8821A_2ANT_BT_STATUS_NON_IDLE == 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_8821A_2ANT_BT_STATUS_IDLE != coex_dm->bt_status)
+		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);
+
+	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");
+
+	halbtc8821a2ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
+	ex_halbtc8821a2ant_media_status_notify(btcoexist, BTC_MEDIA_DISCONNECT);
+}
+
+void ex_halbtc8821a2ant_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");
+
+	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_8821a_2ant,
+			  glcoex_ver_8821a_2ant,
+			  fw_ver, bt_patch_ver, bt_patch_ver);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], ****************************************************************\n");
+	}
+
+	halbtc8821a2ant_query_bt_info(btcoexist);
+	halbtc8821a2ant_monitor_bt_ctr(btcoexist);
+	monitor_bt_enable_disable(btcoexist);
+}
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8821a2ant.h b/drivers/staging/rtl8192ee/btcoexist/halbtc8821a2ant.h
new file mode 100644
index 00000000000..745506b48d6
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtc8821a2ant.h
@@ -0,0 +1,179 @@
+/*  */
+/*  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
new file mode 100644
index 00000000000..2d9fc24b1e2
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtcoutsrc.c
@@ -0,0 +1,1297 @@
+/******************************************************************************
+ *
+ * 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
new file mode 100644
index 00000000000..c0a4286430a
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtcoutsrc.h
@@ -0,0 +1,537 @@
+#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	printk
+
+#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
new file mode 100644
index 00000000000..50c012a4c17
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/rtl_btc.c
@@ -0,0 +1,194 @@
+/******************************************************************************
+ *
+ * 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
new file mode 100644
index 00000000000..9530eb1a664
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/rtl_btc.h
@@ -0,0 +1,62 @@
+/******************************************************************************
+ *
+ * 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
new file mode 100644
index 00000000000..e32c329da87
--- /dev/null
+++ b/drivers/staging/rtl8192ee/cam.c
@@ -0,0 +1,337 @@
+/******************************************************************************
+ *
+ * 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
new file mode 100644
index 00000000000..b3a9464e7ed
--- /dev/null
+++ b/drivers/staging/rtl8192ee/cam.h
@@ -0,0 +1,52 @@
+/******************************************************************************
+ *
+ * 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
new file mode 100644
index 00000000000..72a3c13afed
--- /dev/null
+++ b/drivers/staging/rtl8192ee/compat.h
@@ -0,0 +1,70 @@
+#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
new file mode 100644
index 00000000000..76ea356163b
--- /dev/null
+++ b/drivers/staging/rtl8192ee/core.c
@@ -0,0 +1,1600 @@
+/******************************************************************************
+ *
+ * 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;
+	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,
+			 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
new file mode 100644
index 00000000000..ef75ad5f029
--- /dev/null
+++ b/drivers/staging/rtl8192ee/core.h
@@ -0,0 +1,39 @@
+/******************************************************************************
+ *
+ * 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
new file mode 100644
index 00000000000..feec3948585
--- /dev/null
+++ b/drivers/staging/rtl8192ee/debug.c
@@ -0,0 +1,978 @@
+/******************************************************************************
+ *
+ * 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
new file mode 100644
index 00000000000..093128d1f36
--- /dev/null
+++ b/drivers/staging/rtl8192ee/debug.h
@@ -0,0 +1,221 @@
+/******************************************************************************
+ *
+ * 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
new file mode 100644
index 00000000000..3fae1836983
--- /dev/null
+++ b/drivers/staging/rtl8192ee/efuse.c
@@ -0,0 +1,1233 @@
+/******************************************************************************
+ *
+ * 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
new file mode 100644
index 00000000000..cc3e1116b3e
--- /dev/null
+++ b/drivers/staging/rtl8192ee/efuse.h
@@ -0,0 +1,127 @@
+/******************************************************************************
+ *
+ * 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
new file mode 100644
index 00000000000..3fe9b7ba01d
--- /dev/null
+++ b/drivers/staging/rtl8192ee/pci.c
@@ -0,0 +1,2397 @@
+/******************************************************************************
+ *
+ * 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_alloc_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;
+		}
+
+		memset(buffer_desc, 0, sizeof(*buffer_desc) * entries);
+		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_alloc_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;
+	}
+
+	memset(desc, 0, sizeof(*desc) * entries);
+	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_alloc_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;
+		}
+
+		memset(rtlpci->rx_ring[rxring_idx].buffer_desc, 0,
+		       sizeof(*rtlpci->rx_ring[rxring_idx].buffer_desc) *
+		       rtlpci->rxringcount);
+
+		/* 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_alloc_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;
+		}
+		memset(rtlpci->rx_ring[rxring_idx].desc, 0,
+		       sizeof(*rtlpci->rx_ring[rxring_idx].desc) *
+		       rtlpci->rxringcount);
+
+		/* 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
new file mode 100644
index 00000000000..62c23a7f450
--- /dev/null
+++ b/drivers/staging/rtl8192ee/pci.h
@@ -0,0 +1,342 @@
+/******************************************************************************
+ *
+ * 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
new file mode 100644
index 00000000000..90c3fc2e62b
--- /dev/null
+++ b/drivers/staging/rtl8192ee/ps.c
@@ -0,0 +1,983 @@
+/******************************************************************************
+ *
+ * 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
new file mode 100644
index 00000000000..1533661a2f4
--- /dev/null
+++ b/drivers/staging/rtl8192ee/ps.h
@@ -0,0 +1,52 @@
+/******************************************************************************
+ *
+ * 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
new file mode 100644
index 00000000000..c4c34ddcf8c
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rc.c
@@ -0,0 +1,288 @@
+/******************************************************************************
+ *
+ * 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/rc.h b/drivers/staging/rtl8192ee/rc.h
new file mode 100644
index 00000000000..928f570b4b8
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rc.h
@@ -0,0 +1,47 @@
+/******************************************************************************
+ *
+ * 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_RC_H__
+#define __RTL_RC_H__
+
+#define B_MODE_MAX_RIX 3
+#define G_MODE_MAX_RIX 11
+#define A_MODE_MAX_RIX 7
+
+/* in mac80211 mcs0-mcs15 is idx0-idx15*/
+#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 rtl92e_rate_control_register(void);
+void rtl92e_rate_control_unregister(void);
+#endif
diff --git a/drivers/staging/rtl8192ee/regd.c b/drivers/staging/rtl8192ee/regd.c
new file mode 100644
index 00000000000..7272fae68ec
--- /dev/null
+++ b/drivers/staging/rtl8192ee/regd.c
@@ -0,0 +1,448 @@
+/******************************************************************************
+ *
+ * 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
new file mode 100644
index 00000000000..1f26f0e5261
--- /dev/null
+++ b/drivers/staging/rtl8192ee/regd.h
@@ -0,0 +1,63 @@
+/******************************************************************************
+ *
+ * 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/def.h b/drivers/staging/rtl8192ee/rtl8192ee/def.h
new file mode 100644
index 00000000000..7566c1e8bae
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/def.h
@@ -0,0 +1,106 @@
+/******************************************************************************
+ *
+ * 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 __RTL92E_DEF_H__
+#define __RTL92E_DEF_H__
+
+#define RX_DESC_NUM_92E					512
+
+#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 IS_HT_RATE(_rate)	\
+	(_rate >= DESC92C_RATEMCS0)
+#define IS_CCK_RATE(_rate)	\
+	(_rate >= DESC92C_RATE1M && _rate <= DESC92C_RATE11M)
+#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,
+	VERSION_UNKNOWN = 0xFF,
+};
+
+enum rx_packet_type {
+	NORMAL_RX,
+	TX_REPORT1,
+	TX_REPORT2,
+	HIS_REPORT,
+	C2H_PACKET,
+};
+
+enum rtl_desc_qsel {
+	QSLT_BK = 0x2,
+	QSLT_BE = 0x0,
+	QSLT_VI = 0x5,
+	QSLT_VO = 0x7,
+	QSLT_BEACON = 0x10,
+	QSLT_HIGH = 0x11,
+	QSLT_MGNT = 0x12,
+	QSLT_CMD = 0x13,
+};
+
+enum rtl_desc92c_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,
+};
+#endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/dm.c b/drivers/staging/rtl8192ee/rtl8192ee/dm.c
new file mode 100644
index 00000000000..41c2d98e81d
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/dm.c
@@ -0,0 +1,1258 @@
+/******************************************************************************
+ *
+ * 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"
+
+struct dig_t dm_dig;
+
+static const u32 ofdmswing_table[OFDM_TABLE_SIZE] = {
+	0x7f8001fe,		/* 0, +6.0dB */
+	0x788001e2,		/* 1, +5.5dB */
+	0x71c001c7,		/* 2, +5.0dB */
+	0x6b8001ae,		/* 3, +4.5dB */
+	0x65400195,		/* 4, +4.0dB */
+	0x5fc0017f,		/* 5, +3.5dB */
+	0x5a400169,		/* 6, +3.0dB */
+	0x55400155,		/* 7, +2.5dB */
+	0x50800142,		/* 8, +2.0dB */
+	0x4c000130,		/* 9, +1.5dB */
+	0x47c0011f,		/* 10, +1.0dB */
+	0x43c0010f,		/* 11, +0.5dB */
+	0x40000100,		/* 12, +0dB */
+	0x3c8000f2,		/* 13, -0.5dB */
+	0x390000e4,		/* 14, -1.0dB */
+	0x35c000d7,		/* 15, -1.5dB */
+	0x32c000cb,		/* 16, -2.0dB */
+	0x300000c0,		/* 17, -2.5dB */
+	0x2d4000b5,		/* 18, -3.0dB */
+	0x2ac000ab,		/* 19, -3.5dB */
+	0x288000a2,		/* 20, -4.0dB */
+	0x26000098,		/* 21, -4.5dB */
+	0x24000090,		/* 22, -5.0dB */
+	0x22000088,		/* 23, -5.5dB */
+	0x20000080,		/* 24, -6.0dB */
+	0x1e400079,		/* 25, -6.5dB */
+	0x1c800072,		/* 26, -7.0dB */
+	0x1b00006c,		/* 27. -7.5dB */
+	0x19800066,		/* 28, -8.0dB */
+	0x18000060,		/* 29, -8.5dB */
+	0x16c0005b,		/* 30, -9.0dB */
+	0x15800056,		/* 31, -9.5dB */
+	0x14400051,		/* 32, -10.0dB */
+	0x1300004c,		/* 33, -10.5dB */
+	0x12000048,		/* 34, -11.0dB */
+	0x11000044,		/* 35, -11.5dB */
+	0x10000040,		/* 36, -12.0dB */
+	0x0f00003c,		/* 37, -12.5dB */
+	0x0e400039,		/* 38, -13.0dB */
+	0x0d800036,		/* 39, -13.5dB */
+	0x0cc00033,		/* 40, -14.0dB */
+	0x0c000030,		/* 41, -14.5dB */
+	0x0b40002d,		/* 42, -15.0dB */
+};
+
+static const u8 cckswing_table_ch1ch13[CCK_TABLE_SIZE][8] = {
+	{0x36, 0x35, 0x2e, 0x25, 0x1c, 0x12, 0x09, 0x04}, /* 0, +0dB */
+	{0x33, 0x32, 0x2b, 0x23, 0x1a, 0x11, 0x08, 0x04}, /* 1, -0.5dB */
+	{0x30, 0x2f, 0x29, 0x21, 0x19, 0x10, 0x08, 0x03}, /* 2, -1.0dB */
+	{0x2d, 0x2d, 0x27, 0x1f, 0x18, 0x0f, 0x08, 0x03}, /* 3, -1.5dB */
+	{0x2b, 0x2a, 0x25, 0x1e, 0x16, 0x0e, 0x07, 0x03}, /* 4, -2.0dB */
+	{0x28, 0x28, 0x22, 0x1c, 0x15, 0x0d, 0x07, 0x03}, /* 5, -2.5dB */
+	{0x26, 0x25, 0x21, 0x1b, 0x14, 0x0d, 0x06, 0x03}, /* 6, -3.0dB */
+	{0x24, 0x23, 0x1f, 0x19, 0x13, 0x0c, 0x06, 0x03}, /* 7, -3.5dB */
+	{0x22, 0x21, 0x1d, 0x18, 0x11, 0x0b, 0x06, 0x02}, /* 8, -4.0dB */
+	{0x20, 0x20, 0x1b, 0x16, 0x11, 0x08, 0x05, 0x02}, /* 9, -4.5dB */
+	{0x1f, 0x1e, 0x1a, 0x15, 0x10, 0x0a, 0x05, 0x02}, /* 10, -5.0dB */
+	{0x1d, 0x1c, 0x18, 0x14, 0x0f, 0x0a, 0x05, 0x02}, /* 11, -5.5dB */
+	{0x1b, 0x1a, 0x17, 0x13, 0x0e, 0x09, 0x04, 0x02}, /* 12, -6.0dB */
+	{0x1a, 0x19, 0x16, 0x12, 0x0d, 0x09, 0x04, 0x02}, /* 13, -6.5dB */
+	{0x18, 0x17, 0x15, 0x11, 0x0c, 0x08, 0x04, 0x02}, /* 14, -7.0dB */
+	{0x17, 0x16, 0x13, 0x10, 0x0c, 0x08, 0x04, 0x02}, /* 15, -7.5dB */
+	{0x16, 0x15, 0x12, 0x0f, 0x0b, 0x07, 0x04, 0x01}, /* 16, -8.0dB */
+	{0x14, 0x14, 0x11, 0x0e, 0x0b, 0x07, 0x03, 0x02}, /* 17, -8.5dB */
+	{0x13, 0x13, 0x10, 0x0d, 0x0a, 0x06, 0x03, 0x01}, /* 18, -9.0dB */
+	{0x12, 0x12, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01}, /* 19, -9.5dB */
+	{0x11, 0x11, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01}, /* 20, -10.0dB */
+	{0x10, 0x10, 0x0e, 0x0b, 0x08, 0x05, 0x03, 0x01}, /* 21, -10.5dB */
+	{0x0f, 0x0f, 0x0d, 0x0b, 0x08, 0x05, 0x03, 0x01}, /* 22, -11.0dB */
+	{0x0e, 0x0e, 0x0c, 0x0a, 0x08, 0x05, 0x02, 0x01}, /* 23, -11.5dB */
+	{0x0d, 0x0d, 0x0c, 0x0a, 0x07, 0x05, 0x02, 0x01}, /* 24, -12.0dB */
+	{0x0d, 0x0c, 0x0b, 0x09, 0x07, 0x04, 0x02, 0x01}, /* 25, -12.5dB */
+	{0x0c, 0x0c, 0x0a, 0x09, 0x06, 0x04, 0x02, 0x01}, /* 26, -13.0dB */
+	{0x0b, 0x0b, 0x0a, 0x08, 0x06, 0x04, 0x02, 0x01}, /* 27, -13.5dB */
+	{0x0b, 0x0a, 0x09, 0x08, 0x06, 0x04, 0x02, 0x01}, /* 28, -14.0dB */
+	{0x0a, 0x0a, 0x09, 0x07, 0x05, 0x03, 0x02, 0x01}, /* 29, -14.5dB */
+	{0x0a, 0x09, 0x08, 0x07, 0x05, 0x03, 0x02, 0x01}, /* 30, -15.0dB */
+	{0x09, 0x09, 0x08, 0x06, 0x05, 0x03, 0x01, 0x01}, /* 31, -15.5dB */
+	{0x09, 0x08, 0x07, 0x06, 0x04, 0x03, 0x01, 0x01}  /* 32, -16.0dB */
+};
+
+static const u8 cckswing_table_ch14[CCK_TABLE_SIZE][8] = {
+	{0x36, 0x35, 0x2e, 0x1b, 0x00, 0x00, 0x00, 0x00}, /* 0, +0dB */
+	{0x33, 0x32, 0x2b, 0x19, 0x00, 0x00, 0x00, 0x00}, /* 1, -0.5dB */
+	{0x30, 0x2f, 0x29, 0x18, 0x00, 0x00, 0x00, 0x00}, /* 2, -1.0dB */
+	{0x2d, 0x2d, 0x17, 0x17, 0x00, 0x00, 0x00, 0x00}, /* 3, -1.5dB */
+	{0x2b, 0x2a, 0x25, 0x15, 0x00, 0x00, 0x00, 0x00}, /* 4, -2.0dB */
+	{0x28, 0x28, 0x24, 0x14, 0x00, 0x00, 0x00, 0x00}, /* 5, -2.5dB */
+	{0x26, 0x25, 0x21, 0x13, 0x00, 0x00, 0x00, 0x00}, /* 6, -3.0dB */
+	{0x24, 0x23, 0x1f, 0x12, 0x00, 0x00, 0x00, 0x00}, /* 7, -3.5dB */
+	{0x22, 0x21, 0x1d, 0x11, 0x00, 0x00, 0x00, 0x00}, /* 8, -4.0dB */
+	{0x20, 0x20, 0x1b, 0x10, 0x00, 0x00, 0x00, 0x00}, /* 9, -4.5dB */
+	{0x1f, 0x1e, 0x1a, 0x0f, 0x00, 0x00, 0x00, 0x00}, /* 10, -5.0dB */
+	{0x1d, 0x1c, 0x18, 0x0e, 0x00, 0x00, 0x00, 0x00}, /* 11, -5.5dB */
+	{0x1b, 0x1a, 0x17, 0x0e, 0x00, 0x00, 0x00, 0x00}, /* 12, -6.0dB */
+	{0x1a, 0x19, 0x16, 0x0d, 0x00, 0x00, 0x00, 0x00}, /* 13, -6.5dB */
+	{0x18, 0x17, 0x15, 0x0c, 0x00, 0x00, 0x00, 0x00}, /* 14, -7.0dB */
+	{0x17, 0x16, 0x13, 0x0b, 0x00, 0x00, 0x00, 0x00}, /* 15, -7.5dB */
+	{0x16, 0x15, 0x12, 0x0b, 0x00, 0x00, 0x00, 0x00}, /* 16, -8.0dB */
+	{0x14, 0x14, 0x11, 0x0a, 0x00, 0x00, 0x00, 0x00}, /* 17, -8.5dB */
+	{0x13, 0x13, 0x10, 0x0a, 0x00, 0x00, 0x00, 0x00}, /* 18, -9.0dB */
+	{0x12, 0x12, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00}, /* 19, -9.5dB */
+	{0x11, 0x11, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00}, /* 20, -10.0dB */
+	{0x10, 0x10, 0x0e, 0x08, 0x00, 0x00, 0x00, 0x00}, /* 21, -10.5dB */
+	{0x0f, 0x0f, 0x0d, 0x08, 0x00, 0x00, 0x00, 0x00}, /* 22, -11.0dB */
+	{0x0e, 0x0e, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00}, /* 23, -11.5dB */
+	{0x0d, 0x0d, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00}, /* 24, -12.0dB */
+	{0x0d, 0x0c, 0x0b, 0x06, 0x00, 0x00, 0x00, 0x00}, /* 25, -12.5dB */
+	{0x0c, 0x0c, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00}, /* 26, -13.0dB */
+	{0x0b, 0x0b, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00}, /* 27, -13.5dB */
+	{0x0b, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00}, /* 28, -14.0dB */
+	{0x0a, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00}, /* 29, -14.5dB */
+	{0x0a, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00}, /* 30, -15.0dB */
+	{0x09, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00}, /* 31, -15.5dB */
+	{0x09, 0x08, 0x07, 0x04, 0x00, 0x00, 0x00, 0x00}  /* 32, -16.0dB */
+};
+
+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;
+}
+
+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);
+
+	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);
+
+	ret_value = rtl_get_bbreg(hw, DM_REG_OFDM_FA_TYPE1_11N, MASKDWORD);
+	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, DM_REG_OFDM_FA_TYPE2_11N, MASKDWORD);
+	falsealm_cnt->cnt_ofdm_cca = (ret_value & 0xffff);
+	falsealm_cnt->cnt_parity_fail = ((ret_value & 0xffff0000) >> 16);
+
+	ret_value = rtl_get_bbreg(hw, DM_REG_OFDM_FA_TYPE3_11N, MASKDWORD);
+	falsealm_cnt->cnt_rate_illegal = (ret_value & 0xffff);
+	falsealm_cnt->cnt_crc8_fail = ((ret_value & 0xffff0000) >> 16);
+
+	ret_value = rtl_get_bbreg(hw, DM_REG_OFDM_FA_TYPE4_11N, MASKDWORD);
+	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, DM_REG_SC_CNT_11N, MASKDWORD);
+	falsealm_cnt->cnt_bw_lsc = (ret_value & 0xffff);
+	falsealm_cnt->cnt_bw_usc = ((ret_value & 0xffff0000) >> 16);
+
+	rtl_set_bbreg(hw, DM_REG_CCK_FA_RST_11N, BIT(12), 1);
+	rtl_set_bbreg(hw, DM_REG_CCK_FA_RST_11N, BIT(14), 1);
+
+	ret_value = rtl_get_bbreg(hw, DM_REG_CCK_FA_LSB_11N, MASKBYTE0);
+	falsealm_cnt->cnt_cck_fail = ret_value;
+
+	ret_value = rtl_get_bbreg(hw, DM_REG_CCK_FA_MSB_11N, MASKBYTE3);
+	falsealm_cnt->cnt_cck_fail += (ret_value & 0xff) << 8;
+
+	ret_value = rtl_get_bbreg(hw, DM_REG_CCK_CCA_CNT_11N, MASKDWORD);
+	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;
+
+	/*reset false alarm counter registers*/
+	rtl_set_bbreg(hw, DM_REG_OFDM_FA_RSTC_11N, BIT(31), 1);
+	rtl_set_bbreg(hw, DM_REG_OFDM_FA_RSTC_11N, BIT(31), 0);
+	rtl_set_bbreg(hw, DM_REG_OFDM_FA_RSTD_11N, BIT(27), 1);
+	rtl_set_bbreg(hw, DM_REG_OFDM_FA_RSTD_11N, BIT(27), 0);
+	/*update ofdm counter*/
+	rtl_set_bbreg(hw, DM_REG_OFDM_FA_HOLDC_11N, BIT(31), 0);
+	rtl_set_bbreg(hw, DM_REG_OFDM_FA_RSTD_11N, BIT(31), 0);
+	/*reset CCK CCA counter*/
+	rtl_set_bbreg(hw, DM_REG_CCK_FA_RST_11N, BIT(13) | BIT(12), 0);
+	rtl_set_bbreg(hw, DM_REG_CCK_FA_RST_11N, BIT(13) | BIT(12), 2);
+	/*reset CCK FA counter*/
+	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",
+		  falsealm_cnt->cnt_parity_fail,
+		  falsealm_cnt->cnt_rate_illegal,
+		  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",
+		  falsealm_cnt->cnt_ofdm_fail,
+		  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);
+	u8 cur_cck_cca_thresh;
+	if (rtlpriv->mac80211.link_state >= MAC80211_LINKED) {
+		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)) {
+			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;
+	}
+	rtl92ee_dm_write_cck_cca_thres(hw, cur_cck_cca_thresh);
+}
+
+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));
+	u8 dig_dynamic_min , dig_maxofmin;
+	bool bfirstconnect , bfirstdisconnect;
+	u8 dm_dig_max, dm_dig_min;
+	u8 current_igi = dm_dig.cur_igvalue;
+	u8 offset;
+
+	/* AP, BT */
+	if (mac->act_scanning == true)
+		return;
+
+	dig_dynamic_min = dm_dig.dig_dynamic_min_0;
+	bfirstconnect = (mac->link_state >= MAC80211_LINKED) &&
+			(dm_dig.b_media_connect_0 == false);
+	bfirstdisconnect = (mac->link_state < MAC80211_LINKED) &&
+			   (dm_dig.b_media_connect_0 == true);
+
+	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;
+		else
+			dm_dig.rx_gain_range_max = dm_dig.rssi_val_min + 10;
+
+		if (rtlpriv->dm.b_one_entry_only) {
+			offset = 0;
+			if (dm_dig.rssi_val_min - offset < dm_dig_min)
+				dig_dynamic_min = dm_dig_min;
+			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;
+		} else {
+			dig_dynamic_min = dm_dig_min;
+		}
+
+	} else {
+		dm_dig.rx_gain_range_max = dm_dig_max;
+		dig_dynamic_min = dm_dig_min;
+		RT_TRACE(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) {
+			if (dm_dig.forbidden_igi + 1 > dm_dig.rx_gain_range_max)
+				dm_dig.rx_gain_range_min =
+						dm_dig.rx_gain_range_max;
+			else
+				dm_dig.rx_gain_range_min =
+						dm_dig.forbidden_igi + 1;
+			dm_dig.recover_cnt = 3600;
+		}
+	} else {
+		if (dm_dig.recover_cnt != 0) {
+			dm_dig.recover_cnt--;
+		} else {
+			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 =
+								dig_dynamic_min;
+				} else {
+					dm_dig.forbidden_igi--;
+					dm_dig.rx_gain_range_min =
+						dm_dig.forbidden_igi + 1;
+				}
+			} else {
+				dm_dig.large_fa_hit = 0;
+			}
+		}
+	}
+
+	if (rtlpriv->dm.dbginfo.num_qry_beacon_pkt < 5)
+		dm_dig.rx_gain_range_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 (mac->link_state >= MAC80211_LINKED) {
+		if (bfirstconnect) {
+			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;
+		} else {
+			if (rtlpriv->falsealm_cnt.cnt_all > DM_DIG_FA_TH2)
+				current_igi += 4;
+			else if (rtlpriv->falsealm_cnt.cnt_all > DM_DIG_FA_TH1)
+				current_igi += 2;
+			else if (rtlpriv->falsealm_cnt.cnt_all < DM_DIG_FA_TH0)
+				current_igi -= 2;
+
+			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;
+		}
+	} else {
+		if (bfirstdisconnect) {
+			current_igi = dm_dig.rx_gain_range_min;
+		} else {
+			if (rtlpriv->falsealm_cnt.cnt_all > 10000)
+				current_igi += 4;
+			else if (rtlpriv->falsealm_cnt.cnt_all > 8000)
+				current_igi += 2;
+			else if (rtlpriv->falsealm_cnt.cnt_all < 500)
+				current_igi -= 2;
+		}
+	}
+
+	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;
+
+	rtl92ee_dm_write_dig(hw , current_igi);
+	dm_dig.b_media_connect_0 = ((mac->link_state >= MAC80211_LINKED) ?
+				   true : false);
+	dm_dig.dig_dynamic_min_0 = 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)
+		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;
+}
+
+void rtl92ee_dm_write_dig(struct ieee80211_hw *hw, u8 current_igi)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	if (dm_dig.stop_dig)
+		return;
+
+	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;
+}
+
+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.
+	  *According to Windows code,
+	  *these value will only be valid when 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]);
+	/* Rx SNR */
+	rtl_write_byte(rtlpriv, RA_RXSNRDUMP,
+		       (u8)(rtlpriv->stats.rx_snr_db[0]));
+	rtl_write_byte(rtlpriv, RB_RXSNRDUMP,
+		       (u8)(rtlpriv->stats.rx_snr_db[1]));
+	/* Rx Cfo_Short */
+	rtl_write_word(rtlpriv, RA_CFOSHORTDUMP,
+		       rtlpriv->stats.rx_cfo_short[0]);
+	rtl_write_word(rtlpriv, RB_CFOSHORTDUMP,
+		       rtlpriv->stats.rx_cfo_short[1]);
+	/* Rx Cfo_Tail */
+	rtl_write_word(rtlpriv, RA_CFOLONGDUMP, rtlpriv->stats.rx_cfo_tail[0]);
+	rtl_write_word(rtlpriv, RB_CFOLONGDUMP, rtlpriv->stats.rx_cfo_tail[1]);
+}
+
+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 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 disconnet 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));
+}
+
+static void rtl92ee_dm_check_rssi_monitor(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+	struct rtl_dm *dm = rtl_dm(rtlpriv);
+	struct rtl_sta_info *drv_priv;
+	u8 h2c[4] = { 0 };
+	long max = 0, min = 0xff;
+	u8 i = 0;
+
+	if (mac->opmode == NL80211_IFTYPE_AP ||
+	    mac->opmode == NL80211_IFTYPE_ADHOC ||
+	    mac->opmode == NL80211_IFTYPE_MESH_POINT) {
+		/* 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;
+
+			h2c[3] = 0;
+			h2c[2] = (u8) (dm->undecorated_smoothed_pwdb & 0xFF);
+			h2c[1] = 0x20;
+			h2c[0] = ++i;
+			rtl92ee_fill_h2c_cmd(hw, H2C_92E_RSSI_REPORT, 4, h2c);
+		}
+		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
+
+		/* If associated entry is found */
+		if (max != 0) {
+			dm->entry_max_undecoratedsmoothed_pwdb = max;
+			RTPRINT(rtlpriv, FDM, DM_PWDB,
+				"EntryMaxPWDB = 0x%lx(%ld)\n", max, max);
+		} else {
+			dm->entry_max_undecoratedsmoothed_pwdb = 0;
+		}
+		/* If associated entry is found */
+		if (min != 0xff) {
+			dm->entry_min_undecoratedsmoothed_pwdb = min;
+			RTPRINT(rtlpriv, FDM, DM_PWDB,
+				"EntryMinPWDB = 0x%lx(%ld)\n", min, min);
+		} else {
+			dm->entry_min_undecoratedsmoothed_pwdb = 0;
+		}
+	}
+
+	/* Indicate Rx signal strength to FW. */
+	if (dm->b_useramask) {
+		h2c[3] = 0;
+		h2c[2] = (u8) (dm->undecorated_smoothed_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);
+	}
+	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;
+}
+
+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);
+
+	rtlhal->rts_en = 0;
+	primarycca->dup_rts_flag = 0;
+	primarycca->intf_flag = 0;
+	primarycca->intf_type = 0;
+	primarycca->monitor_flag = 0;
+	primarycca->ch_offset = 0;
+	primarycca->mf_state = 0;
+}
+
+static bool rtl92ee_dm_is_edca_turbo_disable(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rtlpriv->mac80211.mode == WIRELESS_MODE_B)
+		return true;
+
+	return false;
+}
+
+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;
+}
+
+static void rtl92ee_dm_check_edca_turbo(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	static u64 last_txok_cnt;
+	static u64 last_rxok_cnt;
+	u64 cur_txok_cnt = 0;
+	u64 cur_rxok_cnt = 0;
+	u32 edca_be_ul = 0x5ea42b;
+	u32 edca_be_dl = 0x5ea42b; /*not sure*/
+	u32 edca_be = 0x5ea42b;
+	bool b_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.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)) ?
+			  true : false;
+
+	if (rtl92ee_dm_is_edca_turbo_disable(hw))
+		goto dm_CheckEdcaTurbo_EXIT;
+
+	if (b_edca_turbo_on) {
+		b_is_cur_rdlstate = (cur_rxok_cnt > cur_txok_cnt * 4) ?
+				    true : false;
+
+		edca_be = b_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;
+	} 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;
+	last_txok_cnt = rtlpriv->stats.txbytesunicast;
+	last_rxok_cnt = rtlpriv->stats.rxbytesunicast;
+}
+
+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;
+
+	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;
+
+	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) {
+			rtl_write_byte(rtlpriv, ROFDM0_ECCATHRESHOLD, 0x03);
+			rtl_write_byte(rtlpriv, ROFDM0_ECCATHRESHOLD + 2, 0x00);
+			rtlpriv->rtlhal.b_pre_edcca_enable = true;
+		}
+	} else if (reg_c50 < 0x25 && reg_c58 < 0x25) {
+		if (rtlpriv->rtlhal.b_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;
+		}
+	}
+}
+
+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)
+{
+	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),
+			      cur_mf_state);
+
+	primarycca->mf_state = cur_mf_state;
+}
+
+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);
+	bool is40mhz = false;
+	u64 ofdm_cca, ofdm_fa, bw_usc_cnt, bw_lsc_cnt;
+	u8 sec_ch_offset;
+	u8 cur_mf_state;
+	static u8 count_down = MONITOR_TIME;
+
+	ofdm_cca = falsealm_cnt->cnt_ofdm_cca;
+	ofdm_fa = falsealm_cnt->cnt_ofdm_fail;
+	bw_usc_cnt = falsealm_cnt->cnt_bw_usc;
+	bw_lsc_cnt = falsealm_cnt->cnt_bw_lsc;
+	is40mhz = rtlpriv->mac80211.bw_40;
+	sec_ch_offset = rtlpriv->mac80211.cur_40_prime_sc;
+	/* NIC: 2: sec is below,  1: sec is above */
+
+	if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_AP) {
+		cur_mf_state = MF_USC_LSC;
+		rtl92ee_dm_write_dynamic_cca(hw, cur_mf_state);
+		return;
+	}
+
+	if (rtlpriv->mac80211.link_state < MAC80211_LINKED)
+		return;
+
+	if (is40mhz)
+		return;
+
+	if (primarycca->pricca_flag == 0) {
+		/* Primary channel is above
+		 * 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)) &&
+			    (ofdm_fa > (ofdm_cca >> 1))) {
+				primarycca->intf_type = 1;
+				primarycca->intf_flag = 1;
+				cur_mf_state = MF_USC;
+				rtl92ee_dm_write_dynamic_cca(hw, cur_mf_state);
+				primarycca->pricca_flag = 1;
+			} else if ((ofdm_cca > OFDMCCA_TH) &&
+				   (bw_lsc_cnt > (bw_usc_cnt + BW_IND_BIAS)) &&
+				   (ofdm_fa < (ofdm_cca >> 1))) {
+				primarycca->intf_type = 2;
+				primarycca->intf_flag = 1;
+				cur_mf_state = MF_USC;
+				rtl92ee_dm_write_dynamic_cca(hw, cur_mf_state);
+				primarycca->pricca_flag = 1;
+				primarycca->dup_rts_flag = 1;
+				rtlpriv->rtlhal.rts_en = 1;
+			} else {
+				primarycca->intf_type = 0;
+				primarycca->intf_flag = 0;
+				cur_mf_state = MF_USC_LSC;
+				rtl92ee_dm_write_dynamic_cca(hw, cur_mf_state);
+				rtlpriv->rtlhal.rts_en = 0;
+				primarycca->dup_rts_flag = 0;
+			}
+		} else if (sec_ch_offset == 1) {
+			if ((ofdm_cca > OFDMCCA_TH) &&
+			    (bw_usc_cnt > (bw_lsc_cnt + BW_IND_BIAS)) &&
+			    (ofdm_fa > (ofdm_cca >> 1))) {
+				primarycca->intf_type = 1;
+				primarycca->intf_flag = 1;
+				cur_mf_state = MF_LSC;
+				rtl92ee_dm_write_dynamic_cca(hw, cur_mf_state);
+				primarycca->pricca_flag = 1;
+			} else if ((ofdm_cca > OFDMCCA_TH) &&
+				   (bw_usc_cnt > (bw_lsc_cnt + BW_IND_BIAS)) &&
+				   (ofdm_fa < (ofdm_cca >> 1))) {
+				primarycca->intf_type = 2;
+				primarycca->intf_flag = 1;
+				cur_mf_state = MF_LSC;
+				rtl92ee_dm_write_dynamic_cca(hw, cur_mf_state);
+				primarycca->pricca_flag = 1;
+				primarycca->dup_rts_flag = 1;
+				rtlpriv->rtlhal.rts_en = 1;
+			} else {
+				primarycca->intf_type = 0;
+				primarycca->intf_flag = 0;
+				cur_mf_state = MF_USC_LSC;
+				rtl92ee_dm_write_dynamic_cca(hw, cur_mf_state);
+				rtlpriv->rtlhal.rts_en = 0;
+				primarycca->dup_rts_flag = 0;
+			}
+		}
+	} else {/* PrimaryCCA->PriCCA_flag == 1 */
+		count_down--;
+		if (count_down == 0) {
+			count_down = MONITOR_TIME;
+			primarycca->pricca_flag = 0;
+			cur_mf_state = MF_USC_LSC;
+			/* default */
+			rtl92ee_dm_write_dynamic_cca(hw, cur_mf_state);
+			rtlpriv->rtlhal.rts_en = 0;
+			primarycca->dup_rts_flag = 0;
+			primarycca->intf_type = 0;
+			primarycca->intf_flag = 0;
+		}
+	}
+}
+
+static void rtl92ee_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) {
+		if (rtldm->atc_status == ATC_STATUS_OFF) {
+			rtl_set_bbreg(hw, ROFDM1_CFOTRACKING, BIT(11),
+				      ATC_STATUS_ON);
+			rtldm->atc_status = ATC_STATUS_ON;
+		}
+		/* 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"));
+				return;
+			}
+		}
+		/* Reset Crystal Cap */
+		if (rtldm->crystal_cap != rtlpriv->efuse.crystalcap) {
+			rtldm->crystal_cap = rtlpriv->efuse.crystalcap;
+			crystal_cap = rtldm->crystal_cap & 0x3f;
+			rtl_set_bbreg(hw, REG_MAC_PHY_CTRL, 0xFFF000,
+				      (crystal_cap | (crystal_cap << 6)));
+		}
+	} else {
+		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;
+
+		if (packet_count == rtldm->packet_count_pre)
+			return;
+
+		rtldm->packet_count_pre = packet_count;
+
+		if (rtlpriv->phy.rf_type == RF_1T1R)
+			cfo_ave = cfo_khz_a;
+		else
+			cfo_ave = (int)(cfo_khz_a + cfo_khz_b) >> 1;
+
+		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) {
+			rtldm->large_cfo_hit = 1;
+			return;
+		} else {
+			rtldm->large_cfo_hit = 0;
+		}
+
+		rtldm->cfo_ave_pre = cfo_ave;
+
+		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;
+			}
+		}
+
+		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;
+
+		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;
+			rtl_set_bbreg(hw, REG_MAC_PHY_CTRL, 0xFFF000,
+				      (crystal_cap | (crystal_cap << 6)));
+		}
+
+		if (cfo_ave < CFO_THRESHOLD_ATC &&
+		    cfo_ave > -CFO_THRESHOLD_ATC) {
+			if (rtldm->atc_status == ATC_STATUS_ON) {
+				rtl_set_bbreg(hw, ROFDM1_CFOTRACKING, BIT(11),
+					      ATC_STATUS_OFF);
+				rtldm->atc_status = ATC_STATUS_OFF;
+			}
+		} else {
+			if (rtldm->atc_status == ATC_STATUS_OFF) {
+				rtl_set_bbreg(hw, ROFDM1_CFOTRACKING, BIT(11),
+					      ATC_STATUS_ON);
+				rtldm->atc_status = ATC_STATUS_ON;
+			}
+		}
+	}
+}
+
+static void rtl92ee_dm_init_txpower_tracking(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm *dm = rtl_dm(rtlpriv);
+	u8 path;
+
+	dm->btxpower_tracking = true;
+	dm->default_ofdm_index = 30;
+	dm->default_cck_index = 20;
+
+	dm->bb_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->ofdm_index[path] = dm->default_ofdm_index;
+		dm->delta_power_index[path] = 0;
+		dm->delta_power_index_last[path] = 0;
+		dm->power_index_offset[path] = 0;
+	}
+}
+
+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);
+
+	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;
+	else
+		rtlpriv->dm.b_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;
+
+}
+
+static bool _rtl92ee_dm_ra_state_check(struct ieee80211_hw *hw,
+				      s32 rssi, u8 *ratr_state)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	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;
+	u8 state;
+
+	/* Threshold Adjustment:
+	 * when RSSI state trends to go up one or two levels,
+	 * make sure RSSI is high enough.
+	 * Here GoUpGap is added to solve
+	 * the boundary's level alternation issue.
+	 */
+	switch (*ratr_state) {
+	case DM_RATR_STA_INIT:
+	case DM_RATR_STA_HIGH:
+			break;
+
+	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:
+			RT_TRACE(COMP_RATR, DBG_DMESG,
+				("wrong rssi level setting %d !", *ratr_state));
+			break;
+	}
+
+	/* Decide RATRState by RSSI. */
+	if (rssi > high_rssithresh_for_ra)
+		state = DM_RATR_STA_HIGH;
+	else if (rssi > low_rssithresh_for_ra)
+		state = DM_RATR_STA_MIDDLE;
+	else
+		state = DM_RATR_STA_LOW;
+
+	if (*ratr_state != state) {
+		*ratr_state = state;
+		return true;
+	}
+
+	return false;
+}
+
+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 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) {
+
+		if (rtlpriv->dm.undecorated_smoothed_pwdb < p_ra->ldpc_thres) {
+			p_ra->use_ldpc = true;
+			p_ra->lower_rts_rate = true;
+		} else if (rtlpriv->dm.undecorated_smoothed_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))) {
+
+			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 rtl92ee_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 rtl92ee_dm_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->dm.dm_type = DM_TYPE_BYDRIVER;
+
+	rtl92ee_dm_diginit(hw);
+	rtl92ee_dm_init_rate_adaptive_mask(hw);
+	rtl92ee_dm_init_primary_cca_check(hw);
+	rtl92ee_dm_init_edca_turbo(hw);
+	rtl92ee_dm_init_txpower_tracking(hw);
+	rtl92ee_dm_init_dynamic_atc_switch(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;
+
+	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;
+	}
+}
+
+void rtl92ee_dm_dynamic_arfb_select(struct ieee80211_hw *hw,
+				    u8 rate, bool collision_state)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rate >= DESC92C_RATEMCS8  && rate <= DESC92C_RATEMCS12) {
+		if (collision_state == 1) {
+			if (rate == DESC92C_RATEMCS12) {
+				rtl_write_dword(rtlpriv, REG_DARFRC, 0x0);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x07060501);
+			} else if (rate == DESC92C_RATEMCS11) {
+				rtl_write_dword(rtlpriv, REG_DARFRC, 0x0);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x07070605);
+			} else if (rate == DESC92C_RATEMCS10) {
+				rtl_write_dword(rtlpriv, REG_DARFRC, 0x0);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x08080706);
+			} else if (rate == DESC92C_RATEMCS9) {
+				rtl_write_dword(rtlpriv, REG_DARFRC, 0x0);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x08080707);
+			} else {
+				rtl_write_dword(rtlpriv, REG_DARFRC, 0x0);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x09090808);
+			}
+		} else {   /* collision_state == 0 */
+			if (rate == DESC92C_RATEMCS12) {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x05010000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x09080706);
+			} else if (rate == DESC92C_RATEMCS11) {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x06050000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x09080807);
+			} else if (rate == DESC92C_RATEMCS10) {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x07060000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x0a090908);
+			} else if (rate == DESC92C_RATEMCS9) {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x07070000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x0a090808);
+			} else {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x08080000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x0b0a0909);
+			}
+		}
+	} else {  /* MCS13~MCS15,  1SS, G-mode */
+		if (collision_state == 1) {
+			if (rate == DESC92C_RATEMCS15) {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x00000000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x05040302);
+			} else if (rate == DESC92C_RATEMCS14) {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x00000000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x06050302);
+			} else if (rate == DESC92C_RATEMCS13) {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x00000000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x07060502);
+			} else {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x00000000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x06050402);
+			}
+		} else{   /* collision_state == 0 */
+			if (rate == DESC92C_RATEMCS15) {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x03020000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x07060504);
+			} else if (rate == DESC92C_RATEMCS14) {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x03020000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x08070605);
+			} else if (rate == DESC92C_RATEMCS13) {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x05020000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x09080706);
+			} else {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x04020000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x08070605);
+			}
+		}
+	}
+}
+
+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;
+
+	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)) {
+		rtl92ee_dm_common_info_self_update(hw);
+		rtl92ee_dm_false_alarm_counter_statistics(hw);
+		rtl92ee_dm_check_rssi_monitor(hw);
+		rtl92ee_dm_dig(hw);
+		rtl92ee_dm_adaptivity(hw);
+		rtl92ee_dm_cck_packet_detection_thresh(hw);
+		rtl92ee_dm_refresh_rate_adaptive_mask(hw);
+		rtl92ee_dm_check_edca_turbo(hw);
+		rtl92ee_dm_dynamic_atc_switch(hw);
+		rtl92ee_dm_dynamic_primary_cca_ckeck(hw);
+	}
+}
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/dm.h b/drivers/staging/rtl8192ee/rtl8192ee/dm.h
new file mode 100644
index 00000000000..30b8fa6d3f7
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/dm.h
@@ -0,0 +1,343 @@
+/******************************************************************************
+ *
+ * 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	__RTL92E_DM_H__
+#define __RTL92E_DM_H__
+
+#define	OFDMCCA_TH				500
+#define	BW_IND_BIAS				500
+#define	MF_USC					2
+#define	MF_LSC					1
+#define	MF_USC_LSC				0
+#define	MONITOR_TIME				30
+
+#define	MAIN_ANT				0
+#define	AUX_ANT					1
+#define	MAIN_ANT_CG_TRX				1
+#define	AUX_ANT_CG_TRX				0
+#define	MAIN_ANT_CGCS_RX			0
+#define	AUX_ANT_CGCS_RX				1
+
+/*RF REG LIST*/
+#define	DM_REG_RF_MODE_11N			0x00
+#define	DM_REG_RF_0B_11N			0x0B
+#define	DM_REG_CHNBW_11N			0x18
+#define	DM_REG_T_METER_11N			0x24
+#define	DM_REG_RF_25_11N			0x25
+#define	DM_REG_RF_26_11N			0x26
+#define	DM_REG_RF_27_11N			0x27
+#define	DM_REG_RF_2B_11N			0x2B
+#define	DM_REG_RF_2C_11N			0x2C
+#define	DM_REG_RXRF_A3_11N			0x3C
+#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
+#define	DM_REG_RF_PIN_11N			0x804
+#define	DM_REG_PSD_CTRL_11N			0x808
+#define	DM_REG_TX_ANT_CTRL_11N			0x80C
+#define	DM_REG_BB_PWR_SAV5_11N			0x818
+#define	DM_REG_CCK_RPT_FORMAT_11N		0x824
+#define	DM_REG_RX_DEFUALT_A_11N			0x858
+#define	DM_REG_RX_DEFUALT_B_11N			0x85A
+#define	DM_REG_BB_PWR_SAV3_11N			0x85C
+#define	DM_REG_ANTSEL_CTRL_11N			0x860
+#define	DM_REG_RX_ANT_CTRL_11N			0x864
+#define	DM_REG_PIN_CTRL_11N			0x870
+#define	DM_REG_BB_PWR_SAV1_11N			0x874
+#define	DM_REG_ANTSEL_PATH_11N			0x878
+#define	DM_REG_BB_3WIRE_11N			0x88C
+#define	DM_REG_SC_CNT_11N			0x8C4
+#define	DM_REG_PSD_DATA_11N			0x8B4
+/*PAGE 9*/
+#define	DM_REG_ANT_MAPPING1_11N			0x914
+#define	DM_REG_ANT_MAPPING2_11N			0x918
+/*PAGE A*/
+#define	DM_REG_CCK_ANTDIV_PARA1_11N		0xA00
+#define	DM_REG_CCK_CCA_11N			0xA0A
+#define	DM_REG_CCK_ANTDIV_PARA2_11N		0xA0C
+#define	DM_REG_CCK_ANTDIV_PARA3_11N		0xA10
+#define	DM_REG_CCK_ANTDIV_PARA4_11N		0xA14
+#define	DM_REG_CCK_FILTER_PARA1_11N		0xA22
+#define	DM_REG_CCK_FILTER_PARA2_11N		0xA23
+#define	DM_REG_CCK_FILTER_PARA3_11N		0xA24
+#define	DM_REG_CCK_FILTER_PARA4_11N		0xA25
+#define	DM_REG_CCK_FILTER_PARA5_11N		0xA26
+#define	DM_REG_CCK_FILTER_PARA6_11N		0xA27
+#define	DM_REG_CCK_FILTER_PARA7_11N		0xA28
+#define	DM_REG_CCK_FILTER_PARA8_11N		0xA29
+#define	DM_REG_CCK_FA_RST_11N			0xA2C
+#define	DM_REG_CCK_FA_MSB_11N			0xA58
+#define	DM_REG_CCK_FA_LSB_11N			0xA5C
+#define	DM_REG_CCK_CCA_CNT_11N			0xA60
+#define	DM_REG_BB_PWR_SAV4_11N			0xA74
+/*PAGE B */
+#define	DM_REG_LNA_SWITCH_11N			0xB2C
+#define	DM_REG_PATH_SWITCH_11N			0xB30
+#define	DM_REG_RSSI_CTRL_11N			0xB38
+#define	DM_REG_CONFIG_ANTA_11N			0xB68
+#define	DM_REG_RSSI_BT_11N			0xB9C
+/*PAGE C */
+#define	DM_REG_OFDM_FA_HOLDC_11N		0xC00
+#define	DM_REG_RX_PATH_11N			0xC04
+#define	DM_REG_TRMUX_11N			0xC08
+#define	DM_REG_OFDM_FA_RSTC_11N			0xC0C
+#define	DM_REG_RXIQI_MATRIX_11N			0xC14
+#define	DM_REG_TXIQK_MATRIX_LSB1_11N		0xC4C
+#define	DM_REG_IGI_A_11N			0xC50
+#define	DM_REG_ANTDIV_PARA2_11N			0xC54
+#define	DM_REG_IGI_B_11N			0xC58
+#define	DM_REG_ANTDIV_PARA3_11N			0xC5C
+#define DM_REG_L1SBD_PD_CH_11N			0XC6C
+#define	DM_REG_BB_PWR_SAV2_11N			0xC70
+#define	DM_REG_RX_OFF_11N			0xC7C
+#define	DM_REG_TXIQK_MATRIXA_11N		0xC80
+#define	DM_REG_TXIQK_MATRIXB_11N		0xC88
+#define	DM_REG_TXIQK_MATRIXA_LSB2_11N		0xC94
+#define	DM_REG_TXIQK_MATRIXB_LSB2_11N		0xC9C
+#define	DM_REG_RXIQK_MATRIX_LSB_11N		0xCA0
+#define	DM_REG_ANTDIV_PARA1_11N			0xCA4
+#define	DM_REG_OFDM_FA_TYPE1_11N		0xCF0
+/*PAGE D */
+#define	DM_REG_OFDM_FA_RSTD_11N			0xD00
+#define	DM_REG_OFDM_FA_TYPE2_11N		0xDA0
+#define	DM_REG_OFDM_FA_TYPE3_11N		0xDA4
+#define	DM_REG_OFDM_FA_TYPE4_11N		0xDA8
+/*PAGE E */
+#define	DM_REG_TXAGC_A_6_18_11N			0xE00
+#define	DM_REG_TXAGC_A_24_54_11N		0xE04
+#define	DM_REG_TXAGC_A_1_MCS32_11N		0xE08
+#define	DM_REG_TXAGC_A_MCS0_3_11N		0xE10
+#define	DM_REG_TXAGC_A_MCS4_7_11N		0xE14
+#define	DM_REG_TXAGC_A_MCS8_11_11N		0xE18
+#define	DM_REG_TXAGC_A_MCS12_15_11N		0xE1C
+#define	DM_REG_FPGA0_IQK_11N			0xE28
+#define	DM_REG_TXIQK_TONE_A_11N			0xE30
+#define	DM_REG_RXIQK_TONE_A_11N			0xE34
+#define	DM_REG_TXIQK_PI_A_11N			0xE38
+#define	DM_REG_RXIQK_PI_A_11N			0xE3C
+#define	DM_REG_TXIQK_11N			0xE40
+#define	DM_REG_RXIQK_11N			0xE44
+#define	DM_REG_IQK_AGC_PTS_11N			0xE48
+#define	DM_REG_IQK_AGC_RSP_11N			0xE4C
+#define	DM_REG_BLUETOOTH_11N			0xE6C
+#define	DM_REG_RX_WAIT_CCA_11N			0xE70
+#define	DM_REG_TX_CCK_RFON_11N			0xE74
+#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_TX2TX_11N			0xE88
+#define	DM_REG_RX_CCK_11N			0xE8C
+#define	DM_REG_RX_OFDM_11N			0xED0
+#define	DM_REG_RX_WAIT_RIFS_11N			0xED4
+#define	DM_REG_RX2RX_11N			0xED8
+#define	DM_REG_STANDBY_11N			0xEDC
+#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
+#define	DM_REG_EARLY_MODE_11N			0x4D0
+#define	DM_REG_RSSI_MONITOR_11N			0x4FE
+#define	DM_REG_EDCA_VO_11N			0x500
+#define	DM_REG_EDCA_VI_11N			0x504
+#define	DM_REG_EDCA_BE_11N			0x508
+#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_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)
+
+#define OFDM_TABLE_LENGTH			43
+#define CCK_TABLE_LENGTH			33
+
+#define OFDM_TABLE_SIZE				43
+#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
+
+#define DM_FALSEALARM_THRESH_LOW		400
+#define DM_FALSEALARM_THRESH_HIGH		1000
+
+#define DM_DIG_MAX				0x3e
+#define DM_DIG_MIN				0x1e
+
+#define DM_DIG_MAX_AP				0x32
+#define DM_DIG_MIN_AP				0x20
+
+#define DM_DIG_FA_UPPER				0x3e
+#define DM_DIG_FA_LOWER				0x1e
+#define DM_DIG_FA_TH0				0x200
+#define DM_DIG_FA_TH1				0x300
+#define DM_DIG_FA_TH2				0x400
+
+#define DM_DIG_BACKOFF_MAX			12
+#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
+#define DM_RATR_STA_HIGH			1
+#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
+#define TX_POWER_NEAR_FIELD_THRESH_LVL1		67
+#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 */
+
+/* RSSI Dump Message */
+#define RA_RSSIDUMP				0xcb0
+#define RB_RSSIDUMP				0xcb1
+#define RS1_RXEVMDUMP				0xcb2
+#define RS2_RXEVMDUMP				0xcb3
+#define RA_RXSNRDUMP				0xcb4
+#define RB_RXSNRDUMP				0xcb5
+#define RA_CFOSHORTDUMP				0xcb6
+#define RB_CFOSHORTDUMP				0xcb8
+#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,
+				    u8 cur_thres);
+void rtl92ee_dm_write_dig(struct ieee80211_hw *hw, u8 current_igi);
+void rtl92ee_dm_init_edca_turbo(struct ieee80211_hw *hw);
+void rtl92ee_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw);
+void rtl92ee_dm_dynamic_arfb_select(struct ieee80211_hw *hw,
+				    u8 rate, bool collision_state);
+#endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/fw.c b/drivers/staging/rtl8192ee/rtl8192ee/fw.c
new file mode 100644
index 00000000000..ea6cafa80f4
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/fw.c
@@ -0,0 +1,945 @@
+/******************************************************************************
+ *
+ * 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 "reg.h"
+#include "def.h"
+#include "fw.h"
+#include "dm.h"
+
+static void _rtl92ee_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);
+	} else {
+
+		tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL);
+		rtl_write_byte(rtlpriv, REG_MCUFWDL, tmp & 0xfe);
+	}
+}
+
+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;
+
+	blockCount = size / blockSize;
+	remainSize = size % blockSize;
+
+	for (i = 0; i < blockCount; i++) {
+		offset = i * blockSize;
+		rtl_write_dword(rtlpriv, (FW_8192C_START_ADDRESS + offset),
+				*(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));
+		}
+	}
+}
+
+static void _rtl92ee_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);
+
+	_rtl92ee_fw_block_write(hw, buffer, size);
+}
+
+static void _rtl92ee_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 _rtl92ee_write_fw(struct ieee80211_hw *hw,
+			      enum version_8192e 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));
+
+	_rtl92ee_fill_dummy(bufferPtr, &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"));
+	}
+
+	for (page = 0; page < pageNums; page++) {
+		offset = page * 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);
+	}
+
+}
+
+static int _rtl92ee_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_8192C_POLLING_TIMEOUT_COUNT) &&
+		 (!(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));
+		goto exit;
+	}
+
+	RT_TRACE(COMP_FW, DBG_TRACE,
+		 ("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);
+
+	rtl92ee_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. count = %d\n", value32, counter));
+			err = 0;
+			goto exit;
+		}
+
+		udelay(FW_8192C_POLLING_DELAY*10);
+
+	} 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));
+
+exit:
+	return err;
+}
+
+int rtl92ee_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 rtl92c_firmware_header *pfwheader;
+	u8 *pfwdata;
+	u32 fwsize;
+	int err;
+	enum version_8192e version = rtlhal->version;
+
+	if (!rtlhal->pfirmware)
+		return 1;
+
+	pfwheader = (struct rtl92c_firmware_header *)rtlhal->pfirmware;
+	rtlhal->fw_version = pfwheader->version;
+	rtlhal->fw_subversion = pfwheader->subversion;
+	pfwdata = (u8 *) rtlhal->pfirmware;
+	fwsize = rtlhal->fwsize;
+	RT_TRACE(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",
+			  pfwheader->version, pfwheader->signature,
+			  (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));
+	}
+
+	if (rtlhal->b_mac_func_enable) {
+		if (rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)) {
+			rtl_write_byte(rtlpriv, REG_MCUFWDL, 0);
+			rtl92ee_firmware_selfreset(hw);
+		}
+	}
+	_rtl92ee_enable_fw_download(hw, true);
+	_rtl92ee_write_fw(hw, version, pfwdata, fwsize);
+	_rtl92ee_enable_fw_download(hw, false);
+
+	err = _rtl92ee_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 _rtl92ee_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 _rtl92ee_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));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	u8 boxnum;
+	u16 box_reg = 0, box_extreg = 0;
+	u8 u1b_tmp;
+	bool isfw_read = false;
+	u8 buf_index = 0;
+	bool bwrite_sucess = false;
+	u8 wait_h2c_limmit = 100;
+	u8 boxcontent[4], boxextcontent[4];
+	u32 h2c_waitcounter = 0;
+	unsigned long flag;
+	u8 idx;
+
+	if (ppsc->dot11_psmode != EACTIVE ||
+		ppsc->inactive_pwrstate == ERFOFF) {
+		RT_TRACE(COMP_CMD, DBG_LOUD ,
+			("FillH2CCommand8192E(): "
+			"Return because RF is off!!!\n"));
+		return;
+	}
+
+	RT_TRACE(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));
+
+			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. */
+		/*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) {
+		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;
+		}
+
+		/* 3. Check if the box content is empty. */
+		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 = _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));
+					break;
+				}
+				udelay(10);
+				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));
+			}
+		}
+
+		/* If Fw has not read the last
+		 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));
+			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));
+
+		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 rtl92ee_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);
+	_rtl92ee_fill_h2c_command(hw, element_id, cmd_len, (u8 *)&tmp_cmdbuf);
+
+	return;
+}
+
+void rtl92ee_firmware_selfreset(struct ieee80211_hw *hw)
+{
+	u8 u1b_tmp;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	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);
+
+	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 ,
+		 ("  _8051Reset92E(): 8051 reset success .\n"));
+}
+
+void rtl92ee_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	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));
+
+	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_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[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);
+
+	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
+#define PROBERSP_PG		4 /* ->5 */
+
+#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,
+	0xFF, 0xFF, 0x00, 0xE0, 0x4C, 0x02, 0xB1, 0x78,
+	0xEC, 0x1A, 0x59, 0x0B, 0xAD, 0xD4, 0x20, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x64, 0x00, 0x10, 0x04, 0x00, 0x05, 0x54, 0x65,
+	0x73, 0x74, 0x32, 0x01, 0x08, 0x82, 0x84, 0x0B,
+	0x16, 0x24, 0x30, 0x48, 0x6C, 0x03, 0x01, 0x06,
+	0x06, 0x02, 0x00, 0x00, 0x2A, 0x01, 0x02, 0x32,
+	0x04, 0x0C, 0x12, 0x18, 0x60, 0x2D, 0x1A, 0x6C,
+	0x09, 0x03, 0xFF, 0x00, 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, 0x00, 0x00, 0xDD, 0x18, 0x00, 0x50,
+	0xF2, 0x01, 0x01, 0x00, 0x00, 0x50, 0xF2, 0x04,
+	0x01, 0x00, 0x00, 0x50, 0xF2, 0x04, 0x01, 0x00,
+
+	/* page 1 beacon */
+	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,
+	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 2  ps-poll */
+	0xA4, 0x10, 0x01, 0xC0, 0xEC, 0x1A, 0x59, 0x0B,
+	0xAD, 0xD4, 0x00, 0xE0, 0x4C, 0x02, 0xB1, 0x78,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	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 3  null */
+	0x48, 0x01, 0x00, 0x00, 0xEC, 0x1A, 0x59, 0x0B,
+	0xAD, 0xD4, 0x00, 0xE0, 0x4C, 0x02, 0xB1, 0x78,
+	0xEC, 0x1A, 0x59, 0x0B, 0xAD, 0xD4, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x72, 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  probe_resp */
+	0x50, 0x00, 0x00, 0x00, 0x00, 0x40, 0x10, 0x10,
+	0x00, 0x03, 0x00, 0xE0, 0x4C, 0x76, 0x00, 0x42,
+	0x00, 0x40, 0x10, 0x10, 0x00, 0x03, 0x00, 0x00,
+	0x9E, 0x46, 0x15, 0x32, 0x27, 0xF2, 0x2D, 0x00,
+	0x64, 0x00, 0x00, 0x04, 0x00, 0x0C, 0x6C, 0x69,
+	0x6E, 0x6B, 0x73, 0x79, 0x73, 0x5F, 0x77, 0x6C,
+	0x61, 0x6E, 0x01, 0x04, 0x82, 0x84, 0x8B, 0x96,
+	0x03, 0x01, 0x01, 0x06, 0x02, 0x00, 0x00, 0x2A,
+	0x01, 0x00, 0x32, 0x08, 0x24, 0x30, 0x48, 0x6C,
+	0x0C, 0x12, 0x18, 0x60, 0x2D, 0x1A, 0x6C, 0x18,
+	0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x3D, 0x00, 0xDD, 0x06, 0x00, 0xE0, 0x4C, 0x02,
+	0x01, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	/* page 5  probe_resp */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	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 rtl92ee_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[BEACON_PG * 128];
+	SET_80211_HDR_ADDRESS2(beacon, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(beacon, mac->bssid);
+
+	/*-------------------------------------------------------
+				(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);
+
+	/*--------------------------------------------------------
+				(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);
+
+	/*---------------------------------------------------------
+				(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);
+
+	totalpacketlen = TOTAL_RESERVED_PKT_LEN;
+
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD ,
+		      "rtl92ee_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
+		      &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);
+
+
+	skb = dev_alloc_skb(totalpacketlen);
+	memcpy((u8 *) skb_put(skb, totalpacketlen),
+	       &reserved_page_packet, totalpacketlen);
+
+	rtstatus = _rtl92ee_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_LOUD ,
+			      "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"));
+}
+
+/*Shoud check FW support p2p or not.*/
+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)
+{
+	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;
+			rtl92ee_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;
+	}
+
+	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,
+					   u8 *cmd_buf, u8 cmd_len)
+{
+	u8 rate = cmd_buf[0] & 0x3F;
+	bool collision_state = cmd_buf[3] & BIT(0);
+
+	rtl92ee_dm_dynamic_arfb_select(hw, rate, collision_state);
+}
+
+static void _rtl92ee_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_8192E_DBG:
+		RT_TRACE(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"));
+		break;
+	case C2H_8192E_TX_REPORT:
+		RT_TRACE(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"));
+		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"));
+		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));
+		break;
+	}
+}
+
+void rtl92ee_c2h_packet_handler(struct ieee80211_hw *hw, u8 *buffer, u8 len)
+{
+	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(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);
+
+	_rtl92ee_c2h_content_parsing(hw, c2h_cmd_id, c2h_cmd_len, tmp_buf);
+}
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/fw.h b/drivers/staging/rtl8192ee/rtl8192ee/fw.h
new file mode 100644
index 00000000000..143992d5f62
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/fw.h
@@ -0,0 +1,213 @@
+/******************************************************************************
+ *
+ * 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 __RTL92E__FW__H__
+#define __RTL92E__FW__H__
+
+#define FW_8192C_SIZE				0x8000
+#define FW_8192C_START_ADDRESS			0x1000
+#define FW_8192C_END_ADDRESS			0x5FFF
+#define FW_8192C_PAGE_SIZE			4096
+#define FW_8192C_POLLING_DELAY			5
+#define FW_8192C_POLLING_TIMEOUT_COUNT		3000
+
+#define IS_FW_HEADER_EXIST(_pfwhdr)	\
+	((_pfwhdr->signature&0xFFF0) == 0x92E0)
+#define USE_OLD_WOWLAN_DEBUG_FW 0
+
+#define H2C_92E_RSVDPAGE_LOC_LEN		5
+#define H2C_92E_PWEMODE_LENGTH			5
+#define H2C_92E_JOINBSSRPT_LENGTH		1
+#define H2C_92E_AP_OFFLOAD_LENGTH		3
+#define H2C_92E_WOWLAN_LENGTH			3
+#define H2C_92E_KEEP_ALIVE_CTRL_LENGTH		3
+#if (USE_OLD_WOWLAN_DEBUG_FW == 0)
+#define H2C_92E_REMOTE_WAKE_CTRL_LEN		1
+#else
+#define H2C_92E_REMOTE_WAKE_CTRL_LEN		3
+#endif
+#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
+*BIT[4] = sub-state
+*/
+#define	FW_PS_RF_ON		BIT(2)
+#define	FW_PS_REGISTER_ACTIVE	BIT(3)
+
+#define	FW_PS_ACK		BIT(6)
+#define	FW_PS_TOGGLE		BIT(7)
+
+ /* 92E 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)
+
+#define	FW_PS_STATE(x)			(FW_PS_STATE_MASK & (x))
+
+#define	FW_PS_STATE_ALL_ON_92E		(FW_PS_CLOCK_ON)
+#define	FW_PS_STATE_RF_ON_92E		(FW_PS_CLOCK_ON)
+#define	FW_PS_STATE_RF_OFF_92E		(FW_PS_CLOCK_ON)
+#define	FW_PS_STATE_RF_OFF_LOW_PWR	(FW_PS_CLOCK_OFF)
+
+/* For 92E 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	IS_IN_LOW_POWER_STATE_92E(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 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;
+};
+
+enum rtl8192c_h2c_cmd {
+	H2C_92E_RSVDPAGE = 0,
+	H2C_92E_MSRRPT = 1,
+	H2C_92E_SCAN = 2,
+	H2C_92E_KEEP_ALIVE_CTRL = 3,
+	H2C_92E_DISCONNECT_DECISION = 4,
+#if (USE_OLD_WOWLAN_DEBUG_FW == 1)
+	H2C_92E_WO_WLAN = 5,
+#endif
+	H2C_92E_INIT_OFFLOAD = 6,
+#if (USE_OLD_WOWLAN_DEBUG_FW == 1)
+	H2C_92E_REMOTE_WAKE_CTRL = 7,
+#endif
+	H2C_92E_AP_OFFLOAD = 8,
+	H2C_92E_BCN_RSVDPAGE = 9,
+	H2C_92E_PROBERSP_RSVDPAGE = 10,
+
+	H2C_92E_SETPWRMODE = 0x20,
+	H2C_92E_PS_TUNING_PARA = 0x21,
+	H2C_92E_PS_TUNING_PARA2 = 0x22,
+	H2C_92E_PS_LPS_PARA = 0x23,
+	H2C_92E_P2P_PS_OFFLOAD = 024,
+
+#if (USE_OLD_WOWLAN_DEBUG_FW == 0)
+	H2C_92E_WO_WLAN = 0x80,
+	H2C_92E_REMOTE_WAKE_CTRL = 0x81,
+	H2C_92E_AOAC_GLOBAL_INFO = 0x82,
+	H2C_92E_AOAC_RSVDPAGE = 0x83,
+#endif
+	H2C_92E_RA_MASK = 0x40,
+	H2C_92E_RSSI_REPORT = 0x42,
+	H2C_92E_SELECTIVE_SUSPEND_ROF_CMD,
+	H2C_92E_P2P_PS_MODE,
+	H2C_92E_PSD_RESULT,
+	/*Not defined CTW CMD for P2P yet*/
+	H2C_92E_P2P_PS_CTW_CMD,
+	MAX_92E_H2CCMD
+};
+
+enum rtl8192e_c2h_evt {
+	C2H_8192E_DBG = 0,
+	C2H_8192E_LB = 1,
+	C2H_8192E_TXBF = 2,
+	C2H_8192E_TX_REPORT = 3,
+	C2H_8192E_BT_INFO = 9,
+	C2H_8192E_BT_MP = 11,
+	C2H_8192E_RA_RPT = 12,
+	MAX_8192E_C2HEVENT
+};
+
+#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_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_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)
+
+/* _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)
+
+
+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);
+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/staging/rtl8192ee/rtl8192ee/hw.c
new file mode 100644
index 00000000000..26af119e2ca
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/hw.c
@@ -0,0 +1,2544 @@
+/******************************************************************************
+ *
+ * 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"
+
+#define LLT_CONFIG	5
+
+static void _rtl92ee_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);
+}
+
+static void _rtl92ee_stop_tx_beacon(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp;
+
+	tmp = rtl_read_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2);
+	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2, tmp & (~BIT(6)));
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 1, 0x64);
+	tmp = rtl_read_byte(rtlpriv, REG_TBTT_PROHIBIT + 2);
+	tmp &= ~(BIT(0));
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 2, tmp);
+}
+
+static void _rtl92ee_resume_tx_beacon(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp;
+
+	tmp = rtl_read_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2);
+	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2, tmp | BIT(6));
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 1, 0xff);
+	tmp = rtl_read_byte(rtlpriv, REG_TBTT_PROHIBIT + 2);
+	tmp |= BIT(0);
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 2, tmp);
+}
+
+static void _rtl92ee_enable_bcn_sub_func(struct ieee80211_hw *hw)
+{
+	_rtl92ee_set_bcn_ctrl_reg(hw, 0, BIT(1));
+}
+
+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];
+
+	while (skb_queue_len(&ring->queue)) {
+		struct rtl_tx_buffer_desc *entry =
+						&ring->buffer_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;
+	}
+
+}
+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)
+{
+	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_92E(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_92E;
+				RT_TRACE(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;
+		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 _rtl92ee_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) {
+		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 _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);
+}
+
+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;
+
+	_rtl92ee_set_fw_ps_rf_off_low_power(hw);
+}
+
+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;
+	u8 rpwm_val = 0, fw_pwrmode = FW_PS_ACTIVE_MODE;
+
+	if (ppsc->b_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;
+		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_92E;	/* 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 _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;
+	u8 rpwm_val;
+
+	if (ppsc->b_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));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
+					      (u8 *) (&ppsc->fwctrl_psmode));
+		rtlhal->ballow_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));
+		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 rtl92ee_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));
+
+	switch (variable) {
+	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_LOUD,
+			 ("switch case not process %x\n", variable));
+		break;
+	}
+}
+
+static void _rtl92ee_download_rsvd_page(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp_regcr, tmp_reg422;
+	u8 bcnvalid_reg, txbc_reg;
+	u8 count = 0, dlbcn_count = 0;
+	bool b_recover = false;
+
+	/*Set REG_CR bit 8. DMA beacon by SW.*/
+	tmp_regcr = rtl_read_byte(rtlpriv, REG_CR + 1);
+	rtl_write_byte(rtlpriv, REG_CR + 1, tmp_regcr | BIT(0));
+
+	/* 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. */
+	_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
+	 * tell Hw the packet is not a real beacon frame.
+	 */
+	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 {
+		/* Clear beacon valid check bit */
+		bcnvalid_reg = rtl_read_byte(rtlpriv, REG_DWBCN0_CTRL + 2);
+		rtl_write_byte(rtlpriv, REG_DWBCN0_CTRL + 2,
+			       bcnvalid_reg | BIT(0));
+
+		/* Return Beacon TCB */
+		_rtl92ee_return_beacon_queue_skb(hw);
+
+		/* download rsvd page */
+		rtl92ee_set_fw_rsvdpagepkt(hw, false);
+
+		txbc_reg = rtl_read_byte(rtlpriv, REG_MGQ_TXBD_NUM + 3);
+		count = 0;
+		while ((txbc_reg & BIT(4)) && count < 20) {
+			count++;
+			udelay(10);
+			txbc_reg = rtl_read_byte(rtlpriv, REG_MGQ_TXBD_NUM + 3);
+		}
+		rtl_write_byte(rtlpriv, REG_MGQ_TXBD_NUM + 3,
+			       txbc_reg | BIT(4));
+
+		/* check rsvd page download OK. */
+		bcnvalid_reg = rtl_read_byte(rtlpriv, REG_DWBCN0_CTRL + 2);
+		count = 0;
+		while (!(bcnvalid_reg & BIT(0)) && count < 20) {
+			count++;
+			udelay(50);
+			bcnvalid_reg = rtl_read_byte(rtlpriv,
+						     REG_DWBCN0_CTRL + 2);
+		}
+
+		if (bcnvalid_reg & BIT(0))
+			rtl_write_byte(rtlpriv, REG_DWBCN0_CTRL + 2, BIT(0));
+
+		dlbcn_count++;
+	} while (!(bcnvalid_reg & BIT(0)) && dlbcn_count < 5);
+
+	if (!(bcnvalid_reg & BIT(0)))
+		RT_TRACE(COMP_INIT, DBG_LOUD, ("Download RSVD page failed!\n"));
+
+	/* Enable Bcn */
+	_rtl92ee_set_bcn_ctrl_reg(hw, BIT(3), 0);
+	_rtl92ee_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 rtl92ee_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 *efuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(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;
+		b_rate_cfg |= 0x01;
+		b_rate_cfg = (b_rate_cfg | 0xd) & (~BIT(1));
+		rtl_write_byte(rtlpriv, REG_RRSR, b_rate_cfg & 0xff);
+		rtl_write_byte(rtlpriv, REG_RRSR + 1, (b_rate_cfg >> 8) & 0xff);
+		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_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));
+		}
+		break; }
+	case HW_VAR_ACK_PREAMBLE:{
+		u8 reg_tmp;
+		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; }
+	case HW_VAR_WPA_CONFIG:
+		rtl_write_byte(rtlpriv, REG_SECCFG, *((u8 *) val));
+		break;
+	case HW_VAR_AMPDU_FACTOR:{
+		u8 regtoset_normal[4] = { 0x41, 0xa8, 0x72, 0xb9 };
+		u8 fac;
+		u8 *reg = NULL;
+		u8 i = 0;
+
+		reg = regtoset_normal;
+
+		fac = *((u8 *) val);
+		if (fac <= 3) {
+			fac = (1 << (fac + 2));
+			if (fac > 0xf)
+				fac = 0xf;
+				for (i = 0; i < 4; i++) {
+				if ((reg[i] & 0xf0) > (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));
+		}
+		break; }
+	case HW_VAR_AC_PARAM:{
+		u8 e_aci = *((u8 *) val);
+
+		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 *aifs = (union aci_aifsn *)(&(mac->ac[0].aifs));
+
+		u8 acm = aifs->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_LOUD,
+					 ("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_RETRY_LIMIT,
+			       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:
+		efuse->efuse_usedbytes = *((u16 *) val);
+		break;
+	case HW_VAR_EFUSE_USAGE:
+		efuse->efuse_usedpercentage = *((u8 *) val);
+		break;
+	case HW_VAR_IO_CMD:
+		rtl92ee_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:
+		rtl92ee_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:
+		_rtl92ee_set_fw_ps_rf_on(hw);
+		break;
+	case HW_VAR_FW_LPS_ACTION:{
+		bool b_enter_fwlps = *((bool *) val);
+
+		if (b_enter_fwlps)
+			_rtl92ee_fwlps_enter(hw);
+		else
+			_rtl92ee_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);
+			_rtl92ee_download_rsvd_page(hw);
+		}
+		rtl92ee_set_fw_media_status_rpt_cmd(hw, mstatus);
+		break; }
+	case HW_VAR_H2C_FW_P2P_PS_OFFLOAD:
+		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; }
+	case HW_VAR_CORRECT_TSF:{
+		u8 btype_ibss = ((u8 *) (val))[0];
+
+		if (btype_ibss == true)
+			_rtl92ee_stop_tx_beacon(hw);
+
+		_rtl92ee_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));
+
+		_rtl92ee_set_bcn_ctrl_reg(hw, BIT(3), 0);
+
+		if (btype_ibss == true)
+			_rtl92ee_resume_tx_beacon(hw);
+		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; }
+	default:
+		RT_TRACE(COMP_ERR, DBG_LOUD,
+			 ("switch case not process %x\n", variable));
+		break;
+	}
+}
+
+static bool _rtl92ee_llt_table_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 txpktbuf_bndy;
+	u8 u8tmp, testcnt = 0;
+
+	txpktbuf_bndy = 0xFA;
+
+	rtl_write_dword(rtlpriv, REG_RQPN, 0x80E90808);
+
+	rtl_write_byte(rtlpriv, REG_TRXFF_BNDY, txpktbuf_bndy);
+	rtl_write_word(rtlpriv, REG_TRXFF_BNDY + 2, 0x3d00 - 1);
+
+	rtl_write_byte(rtlpriv, REG_DWBCN0_CTRL + 1, txpktbuf_bndy);
+	rtl_write_byte(rtlpriv, REG_DWBCN1_CTRL + 1, txpktbuf_bndy);
+
+	rtl_write_byte(rtlpriv, REG_BCNQ_BDNY, txpktbuf_bndy);
+	rtl_write_byte(rtlpriv, REG_BCNQ1_BDNY, txpktbuf_bndy);
+
+	rtl_write_byte(rtlpriv, REG_MGQ_BDNY, txpktbuf_bndy);
+	rtl_write_byte(rtlpriv, 0x45D, txpktbuf_bndy);
+
+	rtl_write_byte(rtlpriv, REG_PBP, 0x31);
+	rtl_write_byte(rtlpriv, REG_RX_DRVINFO_SZ, 0x4);
+
+	u8tmp = rtl_read_byte(rtlpriv, REG_AUTO_LLT + 2);
+	rtl_write_byte(rtlpriv, REG_AUTO_LLT + 2, u8tmp | BIT(0));
+
+	while (u8tmp & BIT(0)) {
+		u8tmp = rtl_read_byte(rtlpriv, REG_AUTO_LLT + 2);
+		udelay(10);
+		testcnt++;
+		if (testcnt > 10)
+			break;
+	}
+
+	return true;
+}
+
+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);
+
+	if (rtlpriv->rtlhal.up_first_time)
+		return;
+
+	if (ppsc->rfoff_reason == RF_CHANGE_BY_IPS)
+		rtl92ee_sw_led_on(hw, pLed0);
+	else if (ppsc->rfoff_reason == RF_CHANGE_BY_INIT)
+		rtl92ee_sw_led_on(hw, pLed0);
+	else
+		rtl92ee_sw_led_off(hw, pLed0);
+}
+
+static bool _rtl92ee_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;
+	u32 dwordtmp;
+
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x0);
+
+	dwordtmp = rtl_read_dword(rtlpriv, REG_SYS_CFG1);
+	if (dwordtmp & BIT(24)) {
+		rtl_write_byte(rtlpriv, 0x7c, 0xc3);
+	} else {
+		bytetmp = rtl_read_byte(rtlpriv, 0x16);
+		rtl_write_byte(rtlpriv, 0x16, bytetmp | BIT(4) | BIT(6));
+		rtl_write_byte(rtlpriv, 0x7c, 0x83);
+	}
+	/* 1. 40Mhz crystal source*/
+	bytetmp = rtl_read_byte(rtlpriv, REG_AFE_CTRL2);
+	bytetmp &= 0xfb;
+	rtl_write_byte(rtlpriv, REG_AFE_CTRL2, bytetmp);
+
+	dwordtmp = rtl_read_dword(rtlpriv, REG_AFE_CTRL4);
+	dwordtmp &= 0xfffffc7f;
+	rtl_write_dword(rtlpriv, REG_AFE_CTRL4, dwordtmp);
+
+	/* 2. 92E AFE parameter
+	  *MP chip then check version */
+	bytetmp = rtl_read_byte(rtlpriv, REG_AFE_CTRL2);
+	bytetmp &= 0xbf;
+	rtl_write_byte(rtlpriv, REG_AFE_CTRL2, bytetmp);
+
+	dwordtmp = rtl_read_dword(rtlpriv, REG_AFE_CTRL4);
+	dwordtmp &= 0xffdfffff;
+	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"));
+		return false;
+	}
+
+	/* Release MAC IO register reset */
+	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);
+
+	/* Add for wakeup online */
+	bytetmp = rtl_read_byte(rtlpriv, REG_SYS_CLKR);
+	rtl_write_byte(rtlpriv, REG_SYS_CLKR, bytetmp | BIT(3));
+	bytetmp = rtl_read_byte(rtlpriv, REG_GPIO_MUXCFG + 1);
+	rtl_write_byte(rtlpriv, REG_GPIO_MUXCFG + 1, bytetmp & (~BIT(4)));
+	/* Release MAC IO register reset */
+	rtl_write_word(rtlpriv, REG_CR, 0x2ff);
+
+	if (!rtlhal->b_mac_func_enable) {
+		if (_rtl92ee_llt_table_init(hw) == false) {
+			RT_TRACE(COMP_INIT, DBG_LOUD,
+				 ("LLT table init fail \n"));
+			return false;
+		}
+	}
+
+	rtl_write_dword(rtlpriv, REG_HISR, 0xffffffff);
+	rtl_write_dword(rtlpriv, REG_HISRE, 0xffffffff);
+
+	wordtmp = rtl_read_word(rtlpriv, REG_TRXDMA_CTRL);
+	wordtmp &= 0xf;
+	wordtmp |= 0xF5B1;
+	rtl_write_word(rtlpriv, REG_TRXDMA_CTRL, wordtmp);
+	/* Reported Tx status from HW for rate adaptive.*/
+	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 1, 0x1F);
+
+	/* Set RCR register */
+	rtl_write_dword(rtlpriv, REG_RCR, rtlpci->receive_config);
+	rtl_write_word(rtlpriv, REG_RXFLTMAP2, 0xffff);
+
+	/* Set TCR register */
+	rtl_write_dword(rtlpriv, REG_TCR, rtlpci->transmit_config);
+
+	/* 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) &
+			DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_MGQ_DESA,
+			(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 &
+			DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_VIQ_DESA,
+			(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 &
+			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 &
+			DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_HQ0_DESA,
+			(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 &
+			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*/
+
+
+	rtl_write_dword(rtlpriv, REG_TSFTIMER_HCI, 0x3fffffff);
+
+	bytetmp = rtl_read_byte(rtlpriv, REG_PCIE_CTRL_REG + 3);
+	rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG + 3, bytetmp | 0xF7);
+
+	rtl_write_dword(rtlpriv, REG_INT_MIG, 0);
+
+	rtl_write_dword(rtlpriv, REG_MCUTST_1, 0x0);
+
+	rtl_write_word(rtlpriv, REG_MGQ_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_VOQ_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_VIQ_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_BEQ_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_VOQ_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_BKQ_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_HI0Q_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_HI1Q_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_HI2Q_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_HI3Q_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_HI4Q_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_HI5Q_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_HI6Q_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_HI7Q_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	/*Rx*/
+#if (DMA_IS_64BIT == 1)
+	rtl_write_word(rtlpriv, REG_RX_RXBD_NUM,
+		       RX_DESC_NUM_92E |
+		       ((RTL8192EE_SEG_NUM << 13) & 0x6000) | 0x8000);
+#else
+	rtl_write_word(rtlpriv, REG_RX_RXBD_NUM,
+		       RX_DESC_NUM_92E |
+		       ((RTL8192EE_SEG_NUM << 13) & 0x6000) | 0x0000);
+#endif
+
+	rtl_write_dword(rtlpriv, REG_TSFTIMER_HCI, 0XFFFFFFFF);
+
+	_rtl92ee_gen_refresh_led_state(hw);
+	return true;
+}
+
+static void _rtl92ee_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;
+	/* 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, 0x00000010);
+	rtl_write_dword(rtlpriv, REG_ARFR0 + 4, 0x3e0ff000);
+
+	/* ARFB table 10 for 11ac 5G 1SS */
+	rtl_write_dword(rtlpriv, REG_ARFR1, 0x00000010);
+	rtl_write_dword(rtlpriv, REG_ARFR1 + 4, 0x000ff000);
+
+	/* Set SLOT time */
+	rtl_write_byte(rtlpriv, REG_SLOT, 0x09);
+
+	/* CF-End setting. */
+	rtl_write_word(rtlpriv, REG_FWHW_TXQ_CTRL, 0x1F80);
+
+	/* Set retry limit */
+	rtl_write_word(rtlpriv, REG_RETRY_LIMIT, 0x0707);
+
+	/* BAR settings */
+	rtl_write_dword(rtlpriv, REG_BAR_MODE_CTRL, 0x0201ffff);
+
+	/* 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);
+
+	/* Beacon related, for rate adaptive */
+	rtl_write_byte(rtlpriv, REG_ATIMWND, 0x2);
+	rtl_write_byte(rtlpriv, REG_BCN_MAX_ERR, 0xff);
+
+	rtlpci->reg_bcn_ctrl_val = 0x1d;
+	rtl_write_byte(rtlpriv, REG_BCN_CTRL, rtlpci->reg_bcn_ctrl_val);
+
+	/* Marked out by Bruce, 2010-09-09.
+	 * This register is configured for the 2nd Beacon (multiple BSSID).
+	 * We shall disable this register if we only support 1 BSSID.
+	 * vivi guess 92d also need this, also 92d now doesnot set this reg
+	 */
+	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_PIFS, 0);
+	rtl_write_byte(rtlpriv, REG_AGGR_BREAK_TIME, 0x16);
+
+	rtl_write_word(rtlpriv, REG_NAV_PROT_LEN, 0x0040);
+	rtl_write_word(rtlpriv, REG_PROT_MODE_CTRL, 0x08ff);
+
+	/* For Rx TP. Suggested by SD1 Richard. Added by tynli. 2010.04.12.*/
+	rtl_write_dword(rtlpriv, REG_FAST_EDCA_CTRL, 0x03086666);
+
+	/* ACKTO for IOT issue. */
+	rtl_write_byte(rtlpriv, REG_ACKTO, 0x40);
+
+	/* Set Spec SIFS (used in NAV) */
+	rtl_write_word(rtlpriv, REG_SPEC_SIFS, 0x100a);
+	rtl_write_word(rtlpriv, REG_MAC_SPEC_SIFS, 0x100a);
+
+	/* Set SIFS for CCK */
+	rtl_write_word(rtlpriv, REG_SIFS_CTX, 0x100a);
+
+	/* Set SIFS for OFDM */
+	rtl_write_word(rtlpriv, REG_SIFS_TRX, 0x100a);
+
+	/* Note Data sheet don't define */
+	rtl_write_word(rtlpriv, 0x4C7, 0x80);
+
+	rtl_write_byte(rtlpriv, REG_RX_PKT_LIMIT, 0x20);
+
+	rtl_write_word(rtlpriv, REG_MAX_AGGR_NUM, 0x1717);
+
+	/* Set Multicast Address. 2009.01.07. by tynli. */
+	rtl_write_dword(rtlpriv, REG_MAR, 0xffffffff);
+	rtl_write_dword(rtlpriv, REG_MAR + 4, 0xffffffff);
+}
+
+static void _rtl92ee_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));
+	u32 tmp32 = 0, count = 0;
+	u8 tmp8 = 0;
+
+	rtl_write_word(rtlpriv, REG_BACKDOOR_DBI_DATA, 0x78);
+	rtl_write_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2, 0x2);
+	tmp8 = rtl_read_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2);
+	count = 0;
+	while (tmp8 && count < 20) {
+		udelay(10);
+		tmp8 = rtl_read_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2);
+		count++;
+	}
+
+	if (0 == tmp8) {
+		tmp32 = rtl_read_dword(rtlpriv, REG_BACKDOOR_DBI_RDATA);
+		if ((tmp32 & 0xff00) != 0x2000) {
+			tmp32 &= 0xffff00ff;
+			rtl_write_dword(rtlpriv, REG_BACKDOOR_DBI_WDATA,
+					tmp32 | BIT(13));
+			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);
+			count = 0;
+			while (tmp8 && count < 20) {
+				udelay(10);
+				tmp8 = rtl_read_byte(rtlpriv,
+						     REG_BACKDOOR_DBI_DATA + 2);
+				count++;
+			}
+		}
+	}
+
+	rtl_write_word(rtlpriv, REG_BACKDOOR_DBI_DATA, 0x70c);
+	rtl_write_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2, 0x2);
+	tmp8 = rtl_read_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2);
+	count = 0;
+	while (tmp8 && count < 20) {
+		udelay(10);
+		tmp8 = rtl_read_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2);
+		count++;
+	}
+	if (0 == tmp8) {
+		tmp32 = rtl_read_dword(rtlpriv, REG_BACKDOOR_DBI_RDATA);
+		rtl_write_dword(rtlpriv, REG_BACKDOOR_DBI_WDATA,
+				tmp32 | BIT(31));
+		rtl_write_word(rtlpriv, REG_BACKDOOR_DBI_DATA, 0xf70c);
+		rtl_write_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2, 0x1);
+	}
+
+	tmp8 = rtl_read_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2);
+	count = 0;
+	while (tmp8 && count < 20) {
+		udelay(10);
+		tmp8 = rtl_read_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2);
+		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);
+	count = 0;
+	while (tmp8 && count < 20) {
+		udelay(10);
+		tmp8 = rtl_read_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2);
+		count++;
+	}
+	if (ppsc->b_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));
+		rtl_write_word(rtlpriv, REG_BACKDOOR_DBI_DATA, 0xf718);
+		rtl_write_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2, 0x1);
+	}
+	tmp8 = rtl_read_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2);
+	count = 0;
+	while (tmp8 && count < 20) {
+		udelay(10);
+		tmp8 = rtl_read_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2);
+		count++;
+	}
+}
+
+void rtl92ee_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);
+
+}
+
+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_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	bool rtstatus = true;
+	int err = 0;
+	u8 tmp_u1b , u1byte;
+	u32 tmp_u4b;
+
+	RT_TRACE(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;
+	} else {
+		rtlhal->b_mac_func_enable = false;
+		rtlhal->fw_ps_state = FW_PS_STATE_ALL_ON_92E;
+	}
+
+	rtstatus = _rtl92ee_init_mac(hw);
+
+	rtl_write_byte(rtlpriv, 0x577, 0x03);
+
+	/*for Crystal 40 Mhz setting */
+	rtl_write_byte(rtlpriv, REG_AFE_CTRL4, 0x2A);
+	rtl_write_byte(rtlpriv, REG_AFE_CTRL4 + 1, 0x00);
+	rtl_write_byte(rtlpriv, REG_AFE_CTRL2, 0x83);
+
+	/*Forced the antenna b to wifi */
+	if (rtlpriv->btcoexist.btc_info.btcoexist == 1) {
+		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"));
+		err = 1;
+		return err;
+	}
+	rtlhal->rx_tag = 0;
+	rtl_write_word(rtlpriv, REG_PCIE_CTRL_REG, 0x8000);
+	err = rtl92ee_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;
+	}
+	/*fw related variable initialize */
+	ppsc->b_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->last_hmeboxnum = 0;
+
+
+	rtl92ee_phy_mac_config(hw);
+
+	rtl92ee_phy_bb_config(hw);
+
+	rtl92ee_phy_rf_config(hw);
+
+	rtlphy->rfreg_chnlval[0] = rtl_get_rfreg(hw, RF90_PATH_A,
+						 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->rfreg_chnlval[0] = (rtlphy->rfreg_chnlval[0] & 0xfffff3ff) |
+				   BIT(10) | BIT(11);
+
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_CHNLBW, RFREG_OFFSET_MASK,
+		      rtlphy->rfreg_chnlval[0]);
+	rtl_set_rfreg(hw, RF90_PATH_B, RF_CHNLBW, RFREG_OFFSET_MASK,
+		      rtlphy->rfreg_chnlval[0]);
+
+	/*---- Set CCK and OFDM Block "ON"----*/
+	rtl_set_bbreg(hw, RFPGA0_RFMOD, BCCKEN, 0x1);
+	rtl_set_bbreg(hw, RFPGA0_RFMOD, BOFDMEN, 0x1);
+
+	/* Must set this,
+	 * otherwise the rx sensitivity will be very pool. Maddest
+	 */
+	rtl_set_rfreg(hw, RF90_PATH_A, 0xB1, RFREG_OFFSET_MASK, 0x54418);
+
+	/*Set Hardware(MAC default setting.)*/
+	_rtl92ee_hw_configure(hw);
+
+	rtlhal->b_mac_func_enable = true;
+
+	stg_rtl_cam_reset_all_entry(hw);
+	rtl92ee_enable_hw_security_config(hw);
+
+	ppsc->rfpwr_state = ERFON;
+
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ETHER_ADDR, mac->mac_addr);
+	_rtl92ee_enable_aspm_back_door(hw);
+	rtlpriv->intf_ops->enable_aspm(hw);
+
+	rtl92ee_bt_hw_init(hw);
+
+	rtlpriv->rtlhal.being_init_adapter = false;
+
+	if (ppsc->rfpwr_state == ERFON) {
+		if (rtlphy->iqk_initialized) {
+			rtl92ee_phy_iq_calibrate(hw, true);
+		} else {
+			rtl92ee_phy_iq_calibrate(hw, false);
+			rtlphy->iqk_initialized = true;
+		}
+	}
+
+	rtlphy->rfpath_rx_enable[0] = true;
+	if (rtlphy->rf_type == RF_2T2R)
+		rtlphy->rfpath_rx_enable[1] = true;
+
+	stg_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"));
+	}
+
+	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"));
+	}
+
+	rtl_write_byte(rtlpriv, REG_NAV_UPPER, ((30000 + 127) / 128));
+
+	/*Fixed LDPC rx hang issue. */
+	tmp_u4b = rtl_read_dword(rtlpriv, REG_SYS_SWR_CTRL1);
+	rtl_write_byte(rtlpriv, REG_SYS_SWR_CTRL2, 0x75);
+	tmp_u4b =  (tmp_u4b & 0xfff00fff) | (0x7E << 12);
+	rtl_write_dword(rtlpriv, REG_SYS_SWR_CTRL1, tmp_u4b);
+
+	rtl92ee_dm_init(hw);
+
+	rtl_write_dword(rtlpriv, 0x4fc, 0);
+
+	RT_TRACE(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);
+	enum version_8192e version = VERSION_UNKNOWN;
+	u32 value32;
+
+	rtlphy->rf_type = RF_2T2R;
+
+	value32 = rtl_read_dword(rtlpriv, REG_SYS_CFG1);
+	if (value32 & TRP_VAUX_EN)
+		version = (enum version_8192e) VERSION_TEST_CHIP_2T2R_8192E;
+	else
+		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"));
+
+	return version;
+}
+
+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);
+	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"));
+		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"));
+		break;
+	case NL80211_IFTYPE_STATION:
+		mode = MSR_INFRA;
+		ledaction = LED_CTL_LINK;
+		RT_TRACE(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"));
+		break;
+	default:
+		RT_TRACE(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) {
+		_rtl92ee_stop_tx_beacon(hw);
+		_rtl92ee_enable_bcn_sub_func(hw);
+	} else if (mode == MSR_ADHOC || mode == MSR_AP) {
+		_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));
+	}
+
+	rtl_write_byte(rtlpriv, (MSR), bt_msr | mode);
+	rtlpriv->cfg->ops->led_control(hw, ledaction);
+	if (mode == MSR_AP)
+		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x00);
+	else
+		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x66);
+	return 0;
+}
+
+void rtl92ee_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));
+		_rtl92ee_set_bcn_ctrl_reg(hw, 0, BIT(4));
+	} else if (check_bssid == false) {
+		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));
+	}
+}
+
+int rtl92ee_set_network_type(struct ieee80211_hw *hw, enum nl80211_iftype type)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (_rtl92ee_set_media_status(hw, type))
+		return -EOPNOTSUPP;
+
+	if (rtlpriv->mac80211.link_state == MAC80211_LINKED) {
+		if (type != NL80211_IFTYPE_AP &&
+		    type != NL80211_IFTYPE_MESH_POINT)
+			rtl92ee_set_check_bssid(hw, true);
+	} else {
+		rtl92ee_set_check_bssid(hw, false);
+	}
+
+	return 0;
+}
+
+/* don't set REG_EDCA_BE_PARAM here because mac80211 will send pkt when scan */
+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:
+		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 rtl92ee_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 rtl92ee_enable_interrupt(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	rtl92ee_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;
+}
+
+void rtl92ee_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 _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"));
+
+	/* 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);
+	/* 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)
+		rtl92ee_firmware_selfreset(hw);
+
+	/* Reset MCU  */
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1, (u1b_tmp & (~BIT(2))));
+
+	/* reset MCU ready status */
+	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);
+
+	/* 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)));
+
+	/* lock ISO/CLK/Power control register */
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x0E);
+}
+
+void rtl92ee_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 , ("RTL8192ee card disable\n"));
+
+	RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
+
+	mac->link_state = MAC80211_NOLINK;
+	opmode = NL80211_IFTYPE_UNSPECIFIED;
+
+	_rtl92ee_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);
+
+	_rtl92ee_poweroff_adapter(hw);
+
+	/* after power off we should do iqk again */
+	rtlpriv->phy.iqk_initialized = false;
+}
+
+void rtl92ee_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 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));
+	u16 bcn_interval, atim_window;
+
+	bcn_interval = mac->beacon_interval;
+	atim_window = 2;	/*FIX MERGE */
+	rtl92ee_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);
+}
+
+void rtl92ee_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));
+	rtl_write_word(rtlpriv, REG_BCN_INTERVAL, bcn_interval);
+}
+
+void rtl92ee_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);
+	rtl92ee_disable_interrupt(hw);
+	rtl92ee_enable_interrupt(hw);
+}
+
+static u8 _rtl92ee_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;
+	}
+	return group;
+}
+
+static void _rtl8192ee_read_power_value_fromprom(struct ieee80211_hw *hw,
+						 struct txpower_info_2g *pwr2g,
+						 struct txpower_info_5g *pwr5g,
+						 bool autoload_fail, u8 *hwinfo)
+{
+	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*/
+		autoload_fail = true;
+
+	if (autoload_fail) {
+		RT_TRACE(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++) {
+				pwr2g->index_cck_base[rf][group] = 0x2D;
+				pwr2g->index_bw40_base[rf][group] = 0x2D;
+			}
+			for (i = 0; i < MAX_TX_COUNT; i++) {
+				if (i == 0) {
+					pwr2g->bw20_diff[rf][0] = 0x02;
+					pwr2g->ofdm_diff[rf][0] = 0x04;
+				} else {
+					pwr2g->bw20_diff[rf][i] = 0xFE;
+					pwr2g->bw40_diff[rf][i] = 0xFE;
+					pwr2g->cck_diff[rf][i] = 0xFE;
+					pwr2g->ofdm_diff[rf][i] = 0xFE;
+				}
+			}
+
+			/*5G default value*/
+			for (group = 0 ; group < MAX_CHNL_GROUP_5G; group++)
+				pwr5g->index_bw40_base[rf][group] = 0x2A;
+
+			for (i = 0; i < MAX_TX_COUNT; i++) {
+				if (i == 0) {
+					pwr5g->ofdm_diff[rf][0] = 0x04;
+					pwr5g->bw20_diff[rf][0] = 0x00;
+					pwr5g->bw80_diff[rf][0] = 0xFE;
+					pwr5g->bw160_diff[rf][0] = 0xFE;
+				} else {
+					pwr5g->ofdm_diff[rf][0] = 0xFE;
+					pwr5g->bw20_diff[rf][0] = 0xFE;
+					pwr5g->bw40_diff[rf][0] = 0xFE;
+					pwr5g->bw80_diff[rf][0] = 0xFE;
+					pwr5g->bw160_diff[rf][0] = 0xFE;
+				}
+			}
+		}
+		return;
+	}
+
+	rtl_priv(hw)->efuse.b_txpwr_fromeprom = true;
+
+	for (rf = 0 ; rf < MAX_RF_PATH ; rf++) {
+		/*2.4G default value*/
+		for (group = 0 ; group < MAX_CHNL_GROUP_24G; group++) {
+			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++];
+			if (pwr2g->index_bw40_base[rf][group] == 0xFF)
+				pwr2g->index_bw40_base[rf][group] = 0x2D;
+		}
+		for (i = 0; i < MAX_TX_COUNT; i++) {
+			if (i == 0) {
+				pwr2g->bw40_diff[rf][i] = 0;
+				if (hwinfo[addr] == 0xFF) {
+					pwr2g->bw20_diff[rf][i] = 0x02;
+				} else {
+					pwr2g->bw20_diff[rf][i] = (hwinfo[addr]
+								   & 0xf0) >> 4;
+					if (pwr2g->bw20_diff[rf][i] & BIT(3))
+						pwr2g->bw20_diff[rf][i] |= 0xF0;
+				}
+
+				if (hwinfo[addr] == 0xFF) {
+					pwr2g->ofdm_diff[rf][i] = 0x04;
+				} else {
+					pwr2g->ofdm_diff[rf][i] = (hwinfo[addr]
+								   & 0x0f);
+					if (pwr2g->ofdm_diff[rf][i] & BIT(3))
+						pwr2g->ofdm_diff[rf][i] |= 0xF0;
+				}
+				pwr2g->cck_diff[rf][i] = 0;
+				addr++;
+			} else {
+				if (hwinfo[addr] == 0xFF) {
+					pwr2g->bw40_diff[rf][i] = 0xFE;
+				} else {
+					pwr2g->bw40_diff[rf][i] = (hwinfo[addr]
+								   & 0xf0) >> 4;
+					if (pwr2g->bw40_diff[rf][i] & BIT(3))
+						pwr2g->bw40_diff[rf][i] |= 0xF0;
+				}
+
+				if (hwinfo[addr] == 0xFF) {
+					pwr2g->bw20_diff[rf][i] = 0xFE;
+				} else {
+					pwr2g->bw20_diff[rf][i] = (hwinfo[addr]
+								   & 0x0f);
+					if (pwr2g->bw20_diff[rf][i] & BIT(3))
+						pwr2g->bw20_diff[rf][i] |= 0xF0;
+				}
+				addr++;
+
+				if (hwinfo[addr] == 0xFF) {
+					pwr2g->ofdm_diff[rf][i] = 0xFE;
+				} else {
+					pwr2g->ofdm_diff[rf][i] = (hwinfo[addr]
+								   & 0xf0) >> 4;
+					if (pwr2g->ofdm_diff[rf][i] & BIT(3))
+						pwr2g->ofdm_diff[rf][i] |= 0xF0;
+				}
+
+				if (hwinfo[addr] == 0xFF) {
+					pwr2g->cck_diff[rf][i] = 0xFE;
+				} else {
+					pwr2g->cck_diff[rf][i] = (hwinfo[addr]
+								  & 0x0f);
+					if (pwr2g->cck_diff[rf][i] & BIT(3))
+						pwr2g->cck_diff[rf][i] |= 0xF0;
+				}
+				addr++;
+			}
+		}
+
+		/*5G default value*/
+		for (group = 0 ; group < MAX_CHNL_GROUP_5G; group++) {
+			pwr5g->index_bw40_base[rf][group] = hwinfo[addr++];
+			if (pwr5g->index_bw40_base[rf][group] == 0xFF)
+				pwr5g->index_bw40_base[rf][group] = 0xFE;
+		}
+
+		for (i = 0; i < MAX_TX_COUNT; i++) {
+			if (i == 0) {
+				pwr5g->bw40_diff[rf][i] = 0;
+
+				if (hwinfo[addr] == 0xFF) {
+					pwr5g->bw20_diff[rf][i] = 0;
+				} else {
+					pwr5g->bw20_diff[rf][0] = (hwinfo[addr]
+								   & 0xf0) >> 4;
+					if (pwr5g->bw20_diff[rf][i] & BIT(3))
+						pwr5g->bw20_diff[rf][i] |= 0xF0;
+				}
+
+				if (hwinfo[addr] == 0xFF) {
+					pwr5g->ofdm_diff[rf][i] = 0x04;
+				} else {
+					pwr5g->ofdm_diff[rf][0] = (hwinfo[addr]
+								   & 0x0f);
+					if (pwr5g->ofdm_diff[rf][i] & BIT(3))
+						pwr5g->ofdm_diff[rf][i] |= 0xF0;
+				}
+				addr++;
+			} else {
+				if (hwinfo[addr] == 0xFF) {
+					pwr5g->bw40_diff[rf][i] = 0xFE;
+				} else {
+					pwr5g->bw40_diff[rf][i] = (hwinfo[addr]
+								  & 0xf0) >> 4;
+					if (pwr5g->bw40_diff[rf][i] & BIT(3))
+					pwr5g->bw40_diff[rf][i] |= 0xF0;
+				}
+
+				if (hwinfo[addr] == 0xFF) {
+					pwr5g->bw20_diff[rf][i] = 0xFE;
+				} else {
+					pwr5g->bw20_diff[rf][i] = (hwinfo[addr]
+								   & 0x0f);
+					if (pwr5g->bw20_diff[rf][i] & BIT(3))
+					pwr5g->bw20_diff[rf][i] |= 0xF0;
+				}
+				addr++;
+			}
+		}
+
+		if (hwinfo[addr] == 0xFF) {
+			pwr5g->ofdm_diff[rf][1] = 0xFE;
+			pwr5g->ofdm_diff[rf][2] = 0xFE;
+		} else {
+			pwr5g->ofdm_diff[rf][1] = (hwinfo[addr] & 0xf0) >> 4;
+			pwr5g->ofdm_diff[rf][2] = (hwinfo[addr] & 0x0f);
+		}
+		addr++;
+
+		if (hwinfo[addr] == 0xFF)
+			pwr5g->ofdm_diff[rf][3] = 0xFE;
+		else
+			pwr5g->ofdm_diff[rf][3] = (hwinfo[addr] & 0x0f);
+		addr++;
+
+		for (i = 1; i < MAX_TX_COUNT; i++) {
+			if (pwr5g->ofdm_diff[rf][i] == 0xFF)
+				pwr5g->ofdm_diff[rf][i] = 0xFE;
+			else if (pwr5g->ofdm_diff[rf][i] & BIT(3))
+				pwr5g->ofdm_diff[rf][i] |= 0xF0;
+		}
+
+		for (i = 0; i < MAX_TX_COUNT; i++) {
+			if (hwinfo[addr] == 0xFF) {
+				pwr5g->bw80_diff[rf][i] = 0xFE;
+			} else {
+				pwr5g->bw80_diff[rf][i] = (hwinfo[addr] & 0xf0)
+							  >> 4;
+				if (pwr5g->bw80_diff[rf][i] & BIT(3))
+					pwr5g->bw80_diff[rf][i] |= 0xF0;
+			}
+
+			if (hwinfo[addr] == 0xFF) {
+				pwr5g->bw160_diff[rf][i] = 0xFE;
+			} else {
+				pwr5g->bw160_diff[rf][i] = (hwinfo[addr] & 0x0f);
+				if (pwr5g->bw160_diff[rf][i] & BIT(3))
+					pwr5g->bw160_diff[rf][i] |= 0xF0;
+			}
+			addr++;
+		}
+	}
+}
+static void _rtl92ee_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
+						 bool autoload_fail, u8 *hwinfo)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *efu = rtl_efuse(rtl_priv(hw));
+	struct txpower_info_2g pwr2g;
+	struct txpower_info_5g pwr5g;
+	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, idx;
+	u8 i;
+
+	_rtl8192ee_read_power_value_fromprom(hw, &pwr2g, &pwr5g,
+					     autoload_fail, hwinfo);
+
+	for (rf = 0; rf < MAX_RF_PATH; rf++) {
+		for (i = 0; i < 14; i++) {
+			idx = _rtl92ee_get_chnl_group(i + 1);
+
+			if (i == CHANNEL_MAX_NUMBER_2G - 1) {
+				efu->txpwrlevel_cck[rf][i] =
+						pwr2g.index_cck_base[rf][5];
+				efu->txpwrlevel_ht40_1s[rf][i] =
+						pwr2g.index_bw40_base[rf][idx];
+			} else {
+				efu->txpwrlevel_cck[rf][i] =
+						pwr2g.index_cck_base[rf][idx];
+				efu->txpwrlevel_ht40_1s[rf][i] =
+						pwr2g.index_bw40_base[rf][idx];
+			}
+		}
+		for (i = 0; i < CHANNEL_MAX_NUMBER_5G; i++) {
+			idx = _rtl92ee_get_chnl_group(channel5g[i]);
+			efu->txpwr_5g_bw40base[rf][i] =
+					pwr5g.index_bw40_base[rf][idx];
+		}
+		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];
+
+			efu->txpwr_5g_bw80base[rf][i] = (upper + lower) / 2;
+		}
+		for (i = 0; i < MAX_TX_COUNT; i++) {
+			efu->txpwr_cckdiff[rf][i] = pwr2g.cck_diff[rf][i];
+			efu->txpwr_legacyhtdiff[rf][i] = pwr2g.ofdm_diff[rf][i];
+			efu->txpwr_ht20diff[rf][i] = pwr2g.bw20_diff[rf][i];
+			efu->txpwr_ht40diff[rf][i] = pwr2g.bw40_diff[rf][i];
+
+			efu->txpwr_5g_ofdmdiff[rf][i] = pwr5g.ofdm_diff[rf][i];
+			efu->txpwr_5g_bw20diff[rf][i] = pwr5g.bw20_diff[rf][i];
+			efu->txpwr_5g_bw40diff[rf][i] = pwr5g.bw40_diff[rf][i];
+			efu->txpwr_5g_bw80diff[rf][i] = pwr5g.bw80_diff[rf][i];
+		}
+	}
+
+	if (!autoload_fail)
+		efu->eeprom_thermalmeter = hwinfo[EEPROM_THERMAL_METER_92E];
+	else
+		efu->eeprom_thermalmeter = EEPROM_DEFAULT_THERMALMETER;
+
+	if (efu->eeprom_thermalmeter == 0xff || autoload_fail) {
+		efu->b_apk_thermalmeterignore = true;
+		efu->eeprom_thermalmeter = EEPROM_DEFAULT_THERMALMETER;
+	}
+
+	efu->thermalmeter[0] = efu->eeprom_thermalmeter;
+	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+		"thermalmeter = 0x%x\n", efu->eeprom_thermalmeter);
+
+	if (!autoload_fail) {
+		efu->eeprom_regulatory = hwinfo[EEPROM_RF_BOARD_OPTION_92E]
+					 & 0x07;
+		if (hwinfo[EEPROM_RF_BOARD_OPTION_92E] == 0xFF)
+			efu->eeprom_regulatory = 0;
+	} else {
+		efu->eeprom_regulatory = 0;
+	}
+	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+		"eeprom_regulatory = 0x%x\n", efu->eeprom_regulatory);
+}
+
+static void _rtl92ee_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));
+	u16 i, usvalue;
+	u8 hwinfo[HWSET_MAX_SIZE];
+	u16 eeprom_id;
+
+	if (rtlefuse->epromtype == EEPROM_BOOT_EFUSE) {
+		stg_rtl_efuse92e_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 !!"));
+		return;
+	}  else {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("boot from neither eeprom nor efuse, check it !!"));
+		return;
+	}
+
+	RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_LOUD, "MAP \n",
+		      hwinfo, HWSET_MAX_SIZE);
+
+	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));
+		rtlefuse->autoload_failflag = true;
+	} else {
+		RT_TRACE(COMP_INIT, DBG_LOUD, ("Autoload OK\n"));
+		rtlefuse->autoload_failflag = false;
+	}
+
+	if (rtlefuse->autoload_failflag == true)
+		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));
+	/*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));
+	/*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;
+	}
+
+	RT_TRACE(COMP_INIT, DBG_DMESG,
+		 ("dev_addr: %pM\n", rtlefuse->dev_addr));
+	/*channel plan */
+	rtlefuse->eeprom_channelplan = *(u8 *) &hwinfo[EEPROM_CHANNELPLAN];
+	/* set channel paln to world wide 13 */
+	rtlefuse->channel_plan = COUNTRY_CODE_WORLD_WIDE_13;
+	/*tx power*/
+	_rtl92ee_read_txpower_info_from_hwpg(hw, rtlefuse->autoload_failflag,
+					     hwinfo);
+
+	rtl92ee_read_bt_coexist_info_from_hwpg(hw, rtlefuse->autoload_failflag,
+					       hwinfo);
+
+	/*board type*/
+	rtlefuse->board_type = (((*(u8 *) &hwinfo[EEPROM_RF_BOARD_OPTION_92E])
+				& 0xE0) >> 5);
+	if ((*(u8 *) &hwinfo[EEPROM_RF_BOARD_OPTION_92E]) == 0xFF)
+		rtlefuse->board_type = 0;
+
+	rtlhal->boad_type = rtlefuse->board_type;
+	/*parse xtal*/
+	rtlefuse->crystalcap = hwinfo[EEPROM_XTAL_92E];
+	if (hwinfo[EEPROM_XTAL_92E] == 0xFF)
+		rtlefuse->crystalcap = 0x20;
+
+	/*antenna diversity*/
+	rtlefuse->antenna_div_type = NO_ANTDIV;
+	rtlefuse->antenna_div_cfg = 0;
+
+	if (rtlhal->oem_id == RT_CID_DEFAULT) {
+		switch (rtlefuse->eeprom_oemid) {
+		case EEPROM_CID_DEFAULT:
+			if (rtlefuse->eeprom_did == 0x818B) {
+				if ((rtlefuse->eeprom_svid == 0x10EC) &&
+				    (rtlefuse->eeprom_smid == 0x001B))
+					rtlhal->oem_id = RT_CID_819x_Lenovo;
+			} else {
+				rtlhal->oem_id = RT_CID_DEFAULT;
+			}
+			break;
+		default:
+			rtlhal->oem_id = RT_CID_DEFAULT;
+			break;
+		}
+	}
+}
+
+static void _rtl92ee_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;
+
+	RT_TRACE(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_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));
+	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;
+		_rtl92ee_read_adapter_info(hw);
+	} else {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("Autoload ERR!!\n"));
+	}
+	_rtl92ee_hal_customized_behavior(hw);
+
+	rtlphy->rfpath_rx_enable[0] = true;
+	if (rtlphy->rf_type == RF_2T2R)
+		rtlphy->rfpath_rx_enable[1] = true;
+}
+
+static u8 _rtl92ee_mrate_idx_to_arfr_id(struct ieee80211_hw *hw, u8 rate_index)
+{
+	u8 ret = 0;
+
+	switch (rate_index) {
+	case RATR_INX_WIRELESS_NGB:
+		ret = 0;
+		break;
+	case RATR_INX_WIRELESS_N:
+	case RATR_INX_WIRELESS_NG:
+		ret = 4;
+		break;
+	case RATR_INX_WIRELESS_NB:
+		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;
+	default:
+		ret = 0;
+		break;
+	}
+	return ret;
+}
+
+static void rtl92ee_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;
+	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] = {0};
+	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);
+
+	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_N_24G:
+		if (b_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 (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 &= 0x0f8f0000;
+				else if (rssi_level == 2)
+					ratr_bitmap &= 0x0ffff000;
+				else
+					ratr_bitmap &= 0x0ffff015;
+			} else {
+				if (rssi_level == 1)
+					ratr_bitmap &= 0x0f8f0000;
+				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;
+	default:
+		ratr_index = RATR_INX_WIRELESS_NGB;
+
+		if (rtlphy->rf_type == RF_1T1R)
+			ratr_bitmap &= 0x000ff0ff;
+		else
+			ratr_bitmap &= 0x0f8ff0ff;
+		break;
+	}
+	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) |
+				       (ratr_index << 28);
+	rate_mask[0] = macid;
+	rate_mask[1] = ratr_index | (b_shortgi ? 0x80 : 0x00);
+	rate_mask[2] = b_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",
+		  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]));
+	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)
+		rtl92ee_update_hal_rate_mask(hw, sta, rssi_level);
+}
+
+void rtl92ee_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 rtl92ee_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid)
+{
+	*valid = 1;
+	return true;
+}
+
+void rtl92ee_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++) {
+			stg_rtl_cam_mark_invalid(hw, cam_offset + idx);
+			stg_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_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 ||
+				    mac->opmode == NL80211_IFTYPE_MESH_POINT) {
+					entry_id = stg_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 ||
+			    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);
+		} else {
+			RT_TRACE(COMP_SEC, DBG_DMESG, ("add one entry\n"));
+			if (is_pairwise) {
+				RT_TRACE(COMP_SEC, DBG_DMESG,
+					 ("set Pairwiase key\n"));
+
+				stg_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) {
+					stg_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]);
+				}
+
+				stg_rtl_cam_add_one_entry(hw, macaddr, key_index,
+						entry_id, enc_algo,
+						CAM_CONFIG_NO_USEDK,
+						rtlpriv->sec.key_buf[entry_id]);
+			}
+		}
+	}
+}
+
+void rtl92ee_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 = hwinfo[EEPROM_RF_BOARD_OPTION_92E];
+		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_RTL8192E;
+		rtlpriv->btcoexist.btc_info.ant_num = ANT_TOTAL_X2;
+	} else {
+		rtlpriv->btcoexist.btc_info.btcoexist = 1;
+		rtlpriv->btcoexist.btc_info.bt_type = BT_RTL8192E;
+		rtlpriv->btcoexist.btc_info.ant_num = ANT_TOTAL_X1;
+	}
+}
+
+void rtl92ee_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 rtl92ee_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 rtl92ee_suspend(struct ieee80211_hw *hw)
+{
+}
+
+void rtl92ee_resume(struct ieee80211_hw *hw)
+{
+}
+
+/* Turn on AAP (RCR:bit 0) for promicuous mode. */
+void rtl92ee_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/rtl8192ee/rtl8192ee/hw.h b/drivers/staging/rtl8192ee/rtl8192ee/hw.h
new file mode 100644
index 00000000000..e99d7bde1c6
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/hw.h
@@ -0,0 +1,67 @@
+/******************************************************************************
+ *
+ * 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 __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,
+				  u32 *p_inta, u32 *p_intb);
+int rtl92ee_hw_init(struct ieee80211_hw *hw);
+void rtl92ee_card_disable(struct ieee80211_hw *hw);
+void rtl92ee_enable_interrupt(struct ieee80211_hw *hw);
+void rtl92ee_disable_interrupt(struct ieee80211_hw *hw);
+int rtl92ee_set_network_type(struct ieee80211_hw *hw, enum nl80211_iftype type);
+void rtl92ee_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid);
+void rtl92ee_set_qos(struct ieee80211_hw *hw, int aci);
+void rtl92ee_set_beacon_related_registers(struct ieee80211_hw *hw);
+void rtl92ee_set_beacon_interval(struct ieee80211_hw *hw);
+void rtl92ee_update_interrupt_mask(struct ieee80211_hw *hw,
+				   u32 add_msr, u32 rm_msr);
+void rtl92ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val);
+void rtl92ee_update_hal_rate_tbl(struct ieee80211_hw *hw,
+				 struct ieee80211_sta *sta, u8 rssi_level);
+void rtl92ee_update_channel_access_setting(struct ieee80211_hw *hw);
+bool rtl92ee_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid);
+void rtl92ee_enable_hw_security_config(struct ieee80211_hw *hw);
+void rtl92ee_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 rtl92ee_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
+					    bool autoload_fail, u8 *hwinfo);
+void rtl92ee_bt_reg_init(struct ieee80211_hw *hw);
+void rtl92ee_bt_hw_init(struct ieee80211_hw *hw);
+void rtl92ee_suspend(struct ieee80211_hw *hw);
+void rtl92ee_resume(struct ieee80211_hw *hw);
+void rtl92ee_allow_all_destaddr(struct ieee80211_hw *hw, bool allow_all_da,
+				bool write_into_reg);
+void rtl92ee_fw_clk_off_timer_callback(unsigned long data);
+#endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/led.c b/drivers/staging/rtl8192ee/rtl8192ee/led.c
new file mode 100644
index 00000000000..3b459c93a84
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/led.c
@@ -0,0 +1,134 @@
+/******************************************************************************
+ *
+ * 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 "led.h"
+#include "reg.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;
+}
+
+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));
+
+	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);
+		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"));
+		break;
+	}
+	pled->b_ledon = true;
+}
+
+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));
+
+	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(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"));
+		break;
+	}
+	pled->b_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);
+}
+
+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);
+	switch (ledaction) {
+	case LED_CTL_POWER_ON:
+	case LED_CTL_LINK:
+	case LED_CTL_NO_LINK:
+		rtl92ee_sw_led_on(hw, pLed0);
+		break;
+	case LED_CTL_POWER_OFF:
+		rtl92ee_sw_led_off(hw, pLed0);
+		break;
+	default:
+		break;
+	}
+}
+
+void rtl92ee_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));
+
+	if ((ppsc->rfoff_reason > RF_CHANGE_BY_PS) &&
+	    (ledaction == LED_CTL_TX ||
+	     ledaction == LED_CTL_RX ||
+	     ledaction == LED_CTL_SITE_SURVEY ||
+	     ledaction == LED_CTL_LINK ||
+	     ledaction == LED_CTL_NO_LINK ||
+	     ledaction == LED_CTL_START_TO_LINK ||
+	     ledaction == LED_CTL_POWER_ON)) {
+		return;
+	}
+	RT_TRACE(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/staging/rtl8192ee/rtl8192ee/led.h
new file mode 100644
index 00000000000..7302eda535d
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/led.h
@@ -0,0 +1,37 @@
+/******************************************************************************
+ *
+ * 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 __RTL92E_LED_H__
+#define __RTL92E_LED_H__
+
+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/staging/rtl8192ee/rtl8192ee/phy.c
new file mode 100644
index 00000000000..beef284615e
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/phy.c
@@ -0,0 +1,3282 @@
+/******************************************************************************
+ *
+ * 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"
+
+static u32 _rtl92ee_phy_rf_serial_read(struct ieee80211_hw *hw,
+				       enum radio_path rfpath, u32 offset);
+static void _rtl92ee_phy_rf_serial_write(struct ieee80211_hw *hw,
+					 enum radio_path rfpath, u32 offset,
+					 u32 data);
+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 _rtl92ee_phy_set_sw_chnl_cmdarray(struct swchnlcmd *cmdtable,
+					      u32 cmdtableidx, u32 cmdtablesz,
+					      enum swchnlcmd_id cmdid,
+					      u32 para1, u32 para2,
+					      u32 msdelay);
+static bool _rtl92ee_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
+					      u8 channel, u8 *stage,
+					      u8 *step, u32 *delay);
+static long _rtl92ee_phy_txpwr_idx_to_dbm(struct ieee80211_hw *hw,
+					  enum wireless_mode wirelessmode,
+					  u8 txpwridx);
+static void rtl92ee_phy_set_rf_on(struct ieee80211_hw *hw);
+static void rtl92ee_phy_set_io(struct ieee80211_hw *hw);
+
+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));
+	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));
+
+	return returnvalue;
+}
+
+void rtl92ee_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 = _rtl92ee_phy_calculate_bit_shift(bitmask);
+		data = ((originalvalue & (~bitmask)) | (data << bitshift));
+	}
+
+	rtl_write_dword(rtlpriv, regaddr, data);
+
+	RT_TRACE(COMP_RF, DBG_TRACE,
+		 ("regaddr(%#x), bitmask(%#x), data(%#x)\n",
+		  regaddr, bitmask, data));
+}
+
+u32 rtl92ee_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 = _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));
+
+	return readback_value;
+}
+
+void rtl92ee_phy_set_rf_reg(struct ieee80211_hw *hw,
+			    enum radio_path rfpath,
+			    u32 addr, 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",
+		  addr, bitmask, data, rfpath));
+
+	spin_lock_irqsave(&rtlpriv->locks.rf_lock, flags);
+
+	if (bitmask != RFREG_OFFSET_MASK) {
+		original_value = _rtl92ee_phy_rf_serial_read(hw, rfpath, addr);
+		bitshift = _rtl92ee_phy_calculate_bit_shift(bitmask);
+		data = (original_value & (~bitmask)) | (data << bitshift);
+	}
+
+	_rtl92ee_phy_rf_serial_write(hw, rfpath, addr, data);
+
+	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));
+}
+
+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 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);
+	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;
+}
+
+static void _rtl92ee_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 _rtl92ee_phy_calculate_bit_shift(u32 bitmask)
+{
+	u32 i;
+
+	for (i = 0; i <= 31; i++) {
+		if (((bitmask >> i) & 0x1) == 1)
+			break;
+	}
+	return i;
+}
+
+bool rtl92ee_phy_mac_config(struct ieee80211_hw *hw)
+{
+	bool rtstatus = _rtl92ee_phy_config_mac_with_headerfile(hw);
+
+	return rtstatus;
+}
+
+bool rtl92ee_phy_bb_config(struct ieee80211_hw *hw)
+{
+	bool rtstatus = true;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u16 regval;
+	u32 tmp;
+	u8 crystal_cap;
+
+	_rtl92ee_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));
+
+	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);
+
+	rtl_write_byte(rtlpriv, REG_AFE_XTAL_CTRL + 1, 0x80);
+
+	tmp = rtl_read_dword(rtlpriv, 0x4c);
+	rtl_write_dword(rtlpriv, 0x4c, tmp | BIT(23));
+
+	rtstatus = _rtl92ee_phy_bb8192ee_config_parafile(hw);
+
+	crystal_cap = rtlpriv->efuse.eeprom_crystalcap & 0x3F;
+	rtl_set_bbreg(hw, REG_MAC_PHY_CTRL, 0xFFF000,
+		      (crystal_cap | (crystal_cap << 6)));
+	return rtstatus;
+}
+
+bool rtl92ee_phy_rf_config(struct ieee80211_hw *hw)
+{
+	return rtl92ee_phy_rf6052_config(hw);
+}
+
+static bool _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) && (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 _rtl92ee_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);
+
+		if (addr == 0xb6) {
+			u32 getvalue;
+			u8 count = 0;
+			getvalue = rtl_get_rfreg(hw, rfpath, addr, MASKDWORD);
+			udelay(1);
+
+			while ((getvalue >> 8) != (data >> 8)) {
+				count++;
+				rtl_set_rfreg(hw, rfpath, regaddr,
+					      RFREG_OFFSET_MASK, data);
+				udelay(1);
+				getvalue = rtl_get_rfreg(hw, rfpath, addr,
+							 MASKDWORD);
+				if (count > 5)
+					break;
+			}
+		}
+
+		if (addr == 0xb2) {
+			u32 getvalue;
+			u8 count = 0;
+			getvalue = rtl_get_rfreg(hw, rfpath, addr, MASKDWORD);
+
+			udelay(1);
+
+			while (getvalue != data) {
+				count++;
+				rtl_set_rfreg(hw, rfpath, regaddr,
+					      RFREG_OFFSET_MASK, data);
+				udelay(1);
+				rtl_set_rfreg(hw, rfpath, 0x18,
+					      RFREG_OFFSET_MASK, 0x0fc07);
+				udelay(1);
+				getvalue = rtl_get_rfreg(hw, rfpath, addr,
+							 MASKDWORD);
+				if (count > 5)
+					break;
+			}
+		}
+	}
+}
+
+static void _rtl92ee_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);
+
+	_rtl92ee_config_rf_reg(hw, addr, data, RF90_PATH_A,
+			       addr | maskforphyset);
+}
+
+static void _rtl92ee_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);
+
+	_rtl92ee_config_rf_reg(hw, addr, data, RF90_PATH_B,
+			       addr | maskforphyset);
+}
+
+static void _rtl92ee_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 _rtl92ee_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 = BAND_ON_2_4G, rf = 0, txnum = 0, sec = 0;
+
+	for (; band <= BAND_ON_5G; ++band)
+		for (; rf < TX_PWR_BY_RATE_NUM_RF; ++rf)
+			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;
+}
+
+static void _rtl92ee_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\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;
+		default:
+			RT_TRACE(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));
+	}
+}
+
+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);
+	u8 value = 0;
+	if (path > RF90_PATH_D) {
+		RT_TRACE(COMP_INIT, DBG_LOUD, ("Invalid Rf Path %d\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;
+		default:
+			RT_TRACE(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));
+	}
+	return value;
+}
+
+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);
+	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;
+			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;
+			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;
+		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;
+		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;
+		base = (raw >> 4) * 10 + (raw & 0xF);
+		_rtl92ee_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path,
+						      HT_MCS8_MCS15, RF_2TX,
+						      base);
+	}
+}
+
+static void _phy_convert_txpower_dbm_to_relative_value(u32 *data, u8 start,
+						       u8 end, u8 base)
+{
+	char i = 0;
+	u8 tmp = 0;
+	u32 temp_data = 0;
+
+	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;
+
+			/* Change the value to a relative value */
+			tmp = (tmp > base) ? tmp - base : base - tmp;
+		} else {
+			tmp = (u8) (*data >> (i * 8)) & 0xFF;
+		}
+		temp_data <<= 8;
+		temp_data |= tmp;
+	}
+	*data = temp_data;
+}
+
+static void _rtl92ee_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, rf = 0, band = BAND_ON_2_4G;
+
+	for (rf = RF90_PATH_A; rf <= RF90_PATH_B; ++rf) {
+		if (rf == RF90_PATH_A) {
+			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][2]),
+				1, 1, base);
+			_phy_convert_txpower_dbm_to_relative_value(
+				&(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]),
+				0, 0, base);
+			_phy_convert_txpower_dbm_to_relative_value(
+				&(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]),
+			0, 3, base);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(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]),
+			0, 3, base);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(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]),
+			0, 3, base);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(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"));
+}
+
+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);
+}
+
+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_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	bool rtstatus;
+
+	rtstatus = _rtl92ee_phy_config_bb_with_headerfile(hw,
+						       BASEBAND_CONFIG_PHY_REG);
+	if (!rtstatus) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("Write BB Reg Fail!!"));
+		return false;
+	}
+
+	_rtl92ee_phy_init_tx_power_by_rate(hw);
+	if (rtlefuse->autoload_failflag == false) {
+		rtlphy->pwrgroup_cnt = 0;
+		rtstatus = _rtl92ee_phy_config_bb_with_pgheaderfile(hw,
+						       BASEBAND_CONFIG_PHY_REG);
+	}
+	_rtl92ee_phy_txpower_by_rate_configuration(hw);
+	if (!rtstatus) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("BB_PG Reg Fail!!"));
+		return false;
+	}
+	rtstatus = _rtl92ee_phy_config_bb_with_headerfile(hw,
+						       BASEBAND_CONFIG_AGC_TAB);
+	if (!rtstatus) {
+		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 _rtl92ee_phy_config_mac_with_headerfile(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+	u32 arraylength;
+	u32 *ptrarray;
+
+	RT_TRACE(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));
+	for (i = 0; i < arraylength; i = i + 2)
+		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)
+
+	int i;
+	u32 *array;
+	u16 len;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 v1 = 0, v2 = 0;
+
+	if (configtype == BASEBAND_CONFIG_PHY_REG) {
+		len = RTL8192EE_PHY_REG_ARRAY_LEN;
+		array = RTL8192EE_PHY_REG_ARRAY;
+
+		for (i = 0; i < len; i = i + 2) {
+			v1 = array[i];
+			v2 = array[i+1];
+			if (v1 < 0xcdcdcdcd) {
+				_rtl92ee_config_bb_reg(hw, v1, v2);
+			} 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. */
+					READ_NEXT_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < len - 2) {
+						_rtl92ee_config_bb_reg(hw, v1,
+								       v2);
+						READ_NEXT_PAIR(v1, v2, i);
+					}
+
+					while (v2 != 0xDEAD && i < len - 2)
+						READ_NEXT_PAIR(v1, v2, i);
+				}
+			}
+		}
+	} 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.*/
+						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]));
+		}
+	}
+	return true;
+}
+
+static u8 _rtl92ee_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 _rtl92ee_store_tx_power_by_rate(struct ieee80211_hw *hw,
+					    enum band_type band,
+					    enum radio_path rfpath,
+					    u32 txnum, u32 regaddr,
+					    u32 bitmask, u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	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));
+		return;
+	}
+
+	if (rfpath > MAX_RF_PATH - 1) {
+		RT_TRACE(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));
+		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)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int i;
+	u32 *phy_regarray_table_pg;
+	u16 phy_regarray_pg_len;
+	u32 v1 = 0, v2 = 0, v3 = 0, v4 = 0, v5 = 0, v6 = 0;
+
+	phy_regarray_pg_len = RTL8192EE_PHY_REG_ARRAY_PG_LEN;
+	phy_regarray_table_pg = RTL8192EE_PHY_REG_ARRAY_PG;
+
+	if (configtype == BASEBAND_CONFIG_PHY_REG) {
+		for (i = 0; i < phy_regarray_pg_len; i = 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) {
+				_rtl92ee_store_tx_power_by_rate(hw, v1, v2, v3,
+								v4, v5, v6);
+				continue;
+			}
+		}
+	} else {
+		RT_TRACE(COMP_SEND, DBG_TRACE,
+			 ("configtype != BaseBand_Config_PHY_REG\n"));
+	}
+	return true;
+}
+
+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)
+
+	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));
+		for (i = 0; i < len; i = i + 2) {
+			v1 = array[i];
+			v2 = array[i+1];
+			if (v1 < 0xcdcdcdcd) {
+				_rtl92ee_config_rf_radio_a(hw, v1, v2);
+				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_RF_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       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.*/
+					READ_NEXT_RF_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < len - 2) {
+						_rtl92ee_config_rf_radio_a(hw,
+									   v1,
+									   v2);
+						READ_NEXT_RF_PAIR(v1, v2, i);
+					}
+
+					while (v2 != 0xDEAD && i < len - 2)
+						READ_NEXT_RF_PAIR(v1, v2, i);
+				}
+			}
+		}
+		break;
+
+	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));
+		for (i = 0; i < len; i = i + 2) {
+			v1 = array[i];
+			v2 = array[i+1];
+			if (v1 < 0xcdcdcdcd) {
+				_rtl92ee_config_rf_radio_b(hw, v1, v2);
+				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_RF_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       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.*/
+					READ_NEXT_RF_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < len - 2) {
+						_rtl92ee_config_rf_radio_b(hw,
+									   v1,
+									   v2);
+						READ_NEXT_RF_PAIR(v1, v2, i);
+					}
+
+					while (v2 != 0xDEAD && i < len - 2)
+						READ_NEXT_RF_PAIR(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;
+}
+
+void rtl92ee_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 _rtl92ee_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 =
+							RFPGA0_XA_LSSIPARAMETER;
+	rtlphy->phyreg_def[RF90_PATH_B].rf3wire_offset =
+							RFPGA0_XB_LSSIPARAMETER;
+
+	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].rflssi_readbackpi =
+						      TRANSCEIVEA_HSPI_READBACK;
+	rtlphy->phyreg_def[RF90_PATH_B].rflssi_readbackpi =
+						      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);
+	u8 txpwr_level;
+	long txpwr_dbm;
+
+	txpwr_level = rtlphy->cur_cck_txpwridx;
+	txpwr_dbm = _rtl92ee_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_B,
+						  txpwr_level);
+	txpwr_level = rtlphy->cur_ofdm24g_txpwridx;
+	if (_rtl92ee_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_G, txpwr_level) >
+	    txpwr_dbm)
+		txpwr_dbm = _rtl92ee_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_G,
+							  txpwr_level);
+	txpwr_level = rtlphy->cur_ofdm24g_txpwridx;
+	if (_rtl92ee_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_N_24G,
+					  txpwr_level) > txpwr_dbm)
+		txpwr_dbm = _rtl92ee_phy_txpwr_idx_to_dbm(hw,
+							  WIRELESS_MODE_N_24G,
+							  txpwr_level);
+	*powerlevel = txpwr_dbm;
+}
+
+static u8 _rtl92ee_phy_get_ratesection_intxpower_byrate(enum radio_path path,
+							u8 rate)
+{
+	u8 rate_section = 0;
+
+	switch (rate) {
+	case DESC92C_RATE1M:
+		rate_section = 2;
+		break;
+
+	case DESC92C_RATE2M:
+	case DESC92C_RATE5_5M:
+		if (path == RF90_PATH_A)
+			rate_section = 3;
+		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;
+}
+
+static u8 _rtl92ee_get_txpower_by_rate(struct ieee80211_hw *hw,
+				       enum band_type band,
+				       enum radio_path rf, u8 rate)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 shift = 0, sec, tx_num;
+	char diff = 0;
+
+	sec = _rtl92ee_phy_get_ratesection_intxpower_byrate(rf, rate);
+	tx_num = RF_TX_NUM_NONIMPLEMENT;
+
+	if (tx_num == RF_TX_NUM_NONIMPLEMENT) {
+		if ((rate >= DESC92C_RATEMCS8 && rate <= DESC92C_RATEMCS15))
+			tx_num = RF_2TX;
+		else
+			tx_num = RF_1TX;
+	}
+
+	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_RATEMCS13:
+		shift = 8;
+		break;
+	case DESC92C_RATEMCS14:
+		shift = 16;
+		break;
+	case DESC92C_RATEMCS15:
+		shift = 24;
+		break;
+
+	default:
+		RT_ASSERT(true, ("Rate_Section is Illegal\n"));
+		break;
+	}
+
+	diff = (u8) (rtlphy->tx_power_by_rate_offset[band][rf][tx_num][sec] >>
+		     shift) & 0xff;
+
+	return	diff;
+}
+
+static u8 _rtl92ee_get_txpower_index(struct ieee80211_hw *hw,
+				     enum radio_path rfpath, u8 rate,
+				     u8 bw, u8 channel)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtlpriv);
+	u8 index = (channel - 1);
+	u8 tx_power = 0;
+	u8 diff = 0;
+
+	if (channel < 1 || channel > 14) {
+		index = 0;
+		RT_TRACE(COMP_POWER_TRACKING, DBG_DMESG,
+			 ("Illegal channel!!\n"));
+	}
+
+	if (IS_CCK_RATE(rate))
+		tx_power = rtlefuse->txpwrlevel_cck[rfpath][index];
+	else if (DESC92C_RATE6M <= rate)
+		tx_power = rtlefuse->txpwrlevel_ht40_1s[rfpath][index];
+
+	/* OFDM-1T*/
+	if (DESC92C_RATE6M <= rate && rate <= DESC92C_RATE54M &&
+	    !IS_CCK_RATE(rate))
+		tx_power += rtlefuse->txpwr_legacyhtdiff[rfpath][TX_1S];
+
+	/* BW20-1S, BW20-2S */
+	if (bw == HT_CHANNEL_WIDTH_20) {
+		if (DESC92C_RATEMCS0 <= rate && rate <= DESC92C_RATEMCS15)
+			tx_power += rtlefuse->txpwr_ht20diff[rfpath][TX_1S];
+		if (DESC92C_RATEMCS8 <= rate && rate <= DESC92C_RATEMCS15)
+			tx_power += rtlefuse->txpwr_ht20diff[rfpath][TX_2S];
+	} else if (bw == HT_CHANNEL_WIDTH_20_40) {/* BW40-1S, BW40-2S */
+		if (DESC92C_RATEMCS0 <= rate && rate <= DESC92C_RATEMCS15)
+			tx_power += rtlefuse->txpwr_ht40diff[rfpath][TX_1S];
+		if (DESC92C_RATEMCS8 <= rate && rate <= DESC92C_RATEMCS15)
+			tx_power += rtlefuse->txpwr_ht40diff[rfpath][TX_2S];
+	}
+
+	if (rtlefuse->eeprom_regulatory != 2)
+		diff = _rtl92ee_get_txpower_by_rate(hw, BAND_ON_2_4G,
+						    rfpath, rate);
+
+	tx_power += diff;
+
+	if (tx_power > MAX_POWER_INDEX)
+		tx_power = MAX_POWER_INDEX;
+
+	return tx_power;
+}
+
+static void _rtl92ee_set_txpower_index(struct ieee80211_hw *hw, u8 pwr_idx,
+				       enum radio_path rfpath, u8 rate)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rfpath == RF90_PATH_A) {
+		switch (rate) {
+		case DESC92C_RATE1M:
+			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);
+			break;
+		case DESC92C_RATE5_5M:
+			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);
+			break;
+		case DESC92C_RATE6M:
+			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);
+			break;
+		case DESC92C_RATE12M:
+			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);
+			break;
+		case DESC92C_RATE24M:
+			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);
+			break;
+		case DESC92C_RATE48M:
+			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);
+			break;
+		case DESC92C_RATEMCS0:
+			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);
+			break;
+		case DESC92C_RATEMCS2:
+			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);
+			break;
+		case DESC92C_RATEMCS4:
+			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);
+			break;
+		case DESC92C_RATEMCS6:
+			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);
+			break;
+		case DESC92C_RATEMCS8:
+			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);
+			break;
+		case DESC92C_RATEMCS10:
+			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);
+			break;
+		case DESC92C_RATEMCS12:
+			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);
+			break;
+		case DESC92C_RATEMCS14:
+			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);
+			break;
+		default:
+			RT_TRACE(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);
+			break;
+		case DESC92C_RATE2M:
+			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);
+			break;
+		case DESC92C_RATE11M:
+			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);
+			break;
+		case DESC92C_RATE9M:
+			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);
+			break;
+		case DESC92C_RATE18M:
+			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);
+			break;
+		case DESC92C_RATE36M:
+			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);
+			break;
+		case DESC92C_RATE54M:
+			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);
+			break;
+		case DESC92C_RATEMCS1:
+			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);
+			break;
+		case DESC92C_RATEMCS3:
+			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);
+			break;
+		case DESC92C_RATEMCS5:
+			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);
+			break;
+		case DESC92C_RATEMCS7:
+			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);
+			break;
+		case DESC92C_RATEMCS9:
+			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);
+			break;
+		case DESC92C_RATEMCS11:
+			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);
+			break;
+		case DESC92C_RATEMCS13:
+			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);
+			break;
+		case DESC92C_RATEMCS15:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12,
+				      MASKBYTE3, pwr_idx);
+			break;
+		default:
+			RT_TRACE(COMP_POWER, DBG_LOUD, ("Invalid Rate!!\n"));
+			break;
+		}
+	} else {
+		RT_TRACE(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)
+{
+	u8 i;
+	u8 power_index;
+	for (i = 0; i < size; i++) {
+		power_index = _rtl92ee_get_txpower_index(hw, rfpath, rates[i],
+							 bw, channel);
+		_rtl92ee_set_txpower_index(hw, power_index, rfpath, rates[i]);
+	}
+}
+
+static void rtl92ee_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);
+
+	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,
+							rtlphy->current_chan_bw,
+							channel, cck_rates, 4);
+	} else if (section == OFDM) {
+		u8 ofdm_rates[] = {DESC92C_RATE6M, DESC92C_RATE9M,
+				   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);
+	} 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);
+	} 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);
+	} else
+		RT_TRACE(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);
+	enum radio_path rfpath;
+
+	if (rtlefuse->b_txpwr_fromeprom == false)
+		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);
+		if (rtlphy->num_total_rfpath >= 2)
+			rtl92ee_phy_set_txpower_index_by_rate_section(hw,
+								rfpath, channel,
+								HT_MCS8_MCS15);
+	}
+}
+
+static long _rtl92ee_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 rtl92ee_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(COMP_ERR, DBG_EMERG,
+				 ("Unknown Scan Backup operation.\n"));
+			break;
+		}
+	}
+}
+
+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_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",
+		  rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20 ?
+		  "20MHz" : "40MHz"));
+
+	if (is_hal_stop(rtlhal)) {
+		rtlphy->set_bwmode_inprogress = false;
+		return;
+	}
+
+	reg_bw_opmode = rtl_read_byte(rtlpriv, REG_BWOPMODE);
+	reg_prsr_rsc = rtl_read_byte(rtlpriv, REG_RRSR + 2);
+
+	switch (rtlphy->current_chan_bw) {
+	case HT_CHANNEL_WIDTH_20:
+		reg_bw_opmode |= BW_OPMODE_20MHZ;
+		rtl_write_byte(rtlpriv, REG_BWOPMODE, reg_bw_opmode);
+		break;
+	case HT_CHANNEL_WIDTH_20_40:
+		reg_bw_opmode &= ~BW_OPMODE_20MHZ;
+		rtl_write_byte(rtlpriv, REG_BWOPMODE, reg_bw_opmode);
+		reg_prsr_rsc = (reg_prsr_rsc & 0x90) |
+			       (mac->cur_40_prime_sc << 5);
+		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));
+		break;
+	}
+
+	switch (rtlphy->current_chan_bw) {
+	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, ROFDM0_TXPSEUDONOISEWGT,
+			      (BIT(31) | BIT(30)), 0);
+		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, 0x818, (BIT(26) | BIT(27)),
+			      (mac->cur_40_prime_sc ==
+			       HAL_PRIME_CHNL_OFFSET_LOWER) ? 2 : 1);
+		break;
+	default:
+		RT_TRACE(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"));
+}
+
+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_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))) {
+		rtl92ee_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 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);
+	u32 delay;
+
+	RT_TRACE(COMP_SCAN, DBG_TRACE,
+		 ("switch to channel%d\n", rtlphy->current_channel));
+	if (is_hal_stop(rtlhal))
+		return;
+	do {
+		if (!rtlphy->sw_chnl_inprogress)
+			break;
+		if (!_rtl92ee_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
+				continue;
+		} else {
+			rtlphy->sw_chnl_inprogress = false;
+		}
+		break;
+	} while (true);
+	RT_TRACE(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_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	if (rtlphy->sw_chnl_inprogress)
+		return 0;
+	if (rtlphy->set_bwmode_inprogress)
+		return 0;
+	RT_ASSERT((rtlphy->current_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));
+		rtlphy->sw_chnl_inprogress = false;
+	} else {
+		RT_TRACE(COMP_CHAN, DBG_LOUD,
+			 ("sw_chnl_inprogress false driver sleep or unload\n"));
+		rtlphy->sw_chnl_inprogress = false;
+	}
+	return 1;
+}
+
+static bool _rtl92ee_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;
+	_rtl92ee_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
+					  MAX_PRECMD_CNT,
+					  CMDID_SET_TXPOWEROWER_LEVEL, 0, 0, 0);
+	_rtl92ee_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
+					  MAX_PRECMD_CNT, CMDID_END, 0, 0, 0);
+
+	postcommoncmdcnt = 0;
+
+	_rtl92ee_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));
+
+	_rtl92ee_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
+					  MAX_RFDEPENDCMD_CNT,
+					  CMDID_RF_WRITEREG,
+					  RF_CHNLBW, channel, 10);
+
+	_rtl92ee_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(COMP_ERR, DBG_EMERG,
+				 ("Invalid 'stage' = %d, Check it!\n" ,
+				  *stage));
+			return true;
+			break;
+		}
+
+		if (currentcmd->cmdid == CMDID_END) {
+			if ((*stage) == 2) {
+				return true;
+			} else {
+				(*stage)++;
+				(*step) = 0;
+				continue;
+			}
+		}
+
+		switch (currentcmd->cmdid) {
+		case CMDID_SET_TXPOWEROWER_LEVEL:
+			rtl92ee_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] &
+					  0xfffff00) | currentcmd->para2);
+
+				rtl_set_rfreg(hw, (enum radio_path)rfpath,
+					      currentcmd->para1,
+					      0x3ff,
+					      rtlphy->rfreg_chnlval[rfpath]);
+			}
+			break;
+		default:
+			RT_TRACE(COMP_ERR, DBG_EMERG,
+				 ("switch case not process\n"));
+			break;
+		}
+
+		break;
+	} while (true);
+
+	(*delay) = currentcmd->msdelay;
+	(*step)++;
+	return false;
+}
+
+static bool _rtl92ee_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 _rtl92ee_phy_path_a_iqk(struct ieee80211_hw *hw, bool config_pathb)
+{
+	u32 reg_eac, reg_e94, reg_e9c;
+	u8 result = 0x00;
+	/* path-A IQK setting */
+	/* PA/PAD controlled by 0x0 */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+	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_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);
+
+	/*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, 0xeac, MASKDWORD);
+	reg_e94 = rtl_get_bbreg(hw, 0xe94, MASKDWORD);
+	reg_e9c = rtl_get_bbreg(hw, 0xe9c, MASKDWORD);
+
+	if (!(reg_eac & BIT(28)) &&
+	    (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
+	    (((reg_e9c & 0x03FF0000) >> 16) != 0x42))
+		result |= 0x01;
+	else
+		return result;
+
+	return result;
+}
+
+static u8 _rtl92ee_phy_path_b_iqk(struct ieee80211_hw *hw)
+{
+	u32 reg_eac, reg_eb4, reg_ebc;
+	u8 result = 0x00;
+
+	/* PA/PAD controlled by 0x0 */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+	rtl_set_rfreg(hw, RF90_PATH_B, 0xdf, RFREG_OFFSET_MASK, 0x180);
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
+
+	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_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);
+
+	/*One shot, path B LOK & IQK*/
+	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xfa000000);
+	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xf8000000);
+
+	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);
+
+	if (!(reg_eac & BIT(31)) &&
+	    (((reg_eb4 & 0x03FF0000) >> 16) != 0x142) &&
+	    (((reg_ebc & 0x03FF0000) >> 16) != 0x42))
+		result |= 0x01;
+	else
+		return result;
+
+	return result;
+}
+
+static u8 _rtl92ee_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);
+
+	/*PA/PAD control by 0x56, and set = 0x0*/
+	rtl_set_rfreg(hw, RF90_PATH_A, 0xdf, RFREG_OFFSET_MASK, 0x980);
+	rtl_set_rfreg(hw, RF90_PATH_A, 0x56, RFREG_OFFSET_MASK, 0x51000);
+
+	/*enter IQK mode*/
+	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, 0x82160c1f);
+	rtl_set_bbreg(hw, RRx_IQK_PI_A, MASKDWORD, 0x68160c1f);
+
+	/*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, 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);
+
+	if (!(reg_eac & BIT(28)) &&
+	    (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
+	    (((reg_e9c & 0x03FF0000) >> 16) != 0x42)) {
+		result |= 0x01;
+	} else {
+		/*	PA/PAD controlled by 0x0 */
+		rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+		rtl_set_rfreg(hw, RF90_PATH_A, 0xdf, RFREG_OFFSET_MASK, 0x180);
+		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);
+
+	/*PA/PAD control by 0x56, and set = 0x0*/
+	rtl_set_rfreg(hw, RF90_PATH_A, 0xdf, RFREG_OFFSET_MASK, 0x980);
+	rtl_set_rfreg(hw, RF90_PATH_A, 0x56, RFREG_OFFSET_MASK, 0x51000);
+
+	/*enter IQK mode*/
+	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, 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);
+
+	/*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);
+
+	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);
+
+	/*PA/PAD controlled by 0x0*/
+	/*leave IQK mode*/
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+	rtl_set_rfreg(hw, RF90_PATH_A, 0xdf, RFREG_OFFSET_MASK, 0x180);
+	/*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;
+
+	return result;
+}
+
+static u8 _rtl92ee_phy_path_b_rx_iqk(struct ieee80211_hw *hw, bool config_pathb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 reg_eac, reg_eb4, reg_ebc, reg_ecc, reg_ec4, 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_B, RF_WE_LUT, RFREG_OFFSET_MASK, 0x800a0);
+	rtl_set_rfreg(hw, RF90_PATH_B, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
+	rtl_set_rfreg(hw, RF90_PATH_B, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0000f);
+	rtl_set_rfreg(hw, RF90_PATH_B, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf117b);
+
+	/*PA/PAD all off*/
+	rtl_set_rfreg(hw, RF90_PATH_B, 0xdf, RFREG_OFFSET_MASK, 0x980);
+	rtl_set_rfreg(hw, RF90_PATH_B, 0x56, RFREG_OFFSET_MASK, 0x51000);
+
+	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, 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);
+
+	/*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, 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);
+
+	if (!(reg_eac & BIT(31)) &&
+	    (((reg_eb4 & 0x03FF0000) >> 16) != 0x142) &&
+	    (((reg_ebc & 0x03FF0000) >> 16) != 0x42)) {
+		result |= 0x01;
+	} else {
+		/*	PA/PAD controlled by 0x0 */
+		rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+		rtl_set_rfreg(hw, RF90_PATH_B, 0xdf, RFREG_OFFSET_MASK, 0x180);
+		return result;
+	}
+
+	u32temp = 0x80007C00 | (reg_eb4 & 0x3FF0000) |
+		  ((reg_ebc & 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_B, RF_WE_LUT, RFREG_OFFSET_MASK, 0x800a0);
+
+	rtl_set_rfreg(hw, RF90_PATH_B, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
+	rtl_set_rfreg(hw, RF90_PATH_B, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0000f);
+	rtl_set_rfreg(hw, RF90_PATH_B, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf7ffa);
+
+	/*PA/PAD all off*/
+	rtl_set_rfreg(hw, RF90_PATH_B, 0xdf, RFREG_OFFSET_MASK, 0x980);
+	rtl_set_rfreg(hw, RF90_PATH_B, 0x56, RFREG_OFFSET_MASK, 0x51000);
+
+	/*enter IQK mode*/
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
+
+	/*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, 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);
+
+	/*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);
+
+	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);
+	/*PA/PAD controlled by 0x0*/
+	/*leave IQK mode*/
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+	rtl_set_rfreg(hw, RF90_PATH_B, 0xdf, RFREG_OFFSET_MASK, 0x180);
+	/*if Tx is OK, check whether Rx is OK*/
+	if (!(reg_eac & BIT(30)) &&
+	    (((reg_ec4 & 0x03FF0000) >> 16) != 0x132) &&
+	    (((reg_ecc & 0x03FF0000) >> 16) != 0x36))
+		result |= 0x02;
+	else
+		RT_TRACE(COMP_RF, DBG_LOUD, ("Path B Rx IQK fail!!\n"));
+
+	return result;
+}
+
+static void _rtl92ee_phy_path_a_fill_iqk_matrix(struct ieee80211_hw *hw,
+						bool b_iqk_ok, long result[][8],
+						u8 final_candidate,
+						bool btxonly)
+{
+	u32 oldval_0, x, tx0_a, reg;
+	long y, tx0_c;
+
+	if (final_candidate == 0xFF) {
+		return;
+	} else if (b_iqk_ok) {
+		oldval_0 = (rtl_get_bbreg(hw, ROFDM0_XATXIQIMBALANCE,
+					  MASKDWORD) >> 22) & 0x3FF;
+		x = result[final_candidate][0];
+		if ((x & 0x00000200) != 0)
+			x = x | 0xFFFFFC00;
+		tx0_a = (x * oldval_0) >> 8;
+		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_candidate][1];
+		if ((y & 0x00000200) != 0)
+			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_XATXIQIMBALANCE, 0x003F0000,
+			      (tx0_c & 0x3F));
+		rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(29),
+			      ((y * oldval_0 >> 7) & 0x1));
+
+		if (btxonly)
+			return;
+
+		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, ROFDM0_RXIQEXTANTA, 0xF0000000, reg);
+	}
+}
+
+static void _rtl92ee_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_XATXIQIMBALANCE,
+					  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_XATXIQIMBALANCE, 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, ROFDM0_AGCRSSITABLE, 0xF0000000, reg);
+	}
+}
+
+static void _rtl92ee_phy_save_adda_registers(struct ieee80211_hw *hw,
+					     u32 *addareg, u32 *addabackup,
+					     u32 registernum)
+{
+	u32 i;
+
+	for (i = 0; i < registernum; i++)
+		addabackup[i] = rtl_get_bbreg(hw, addareg[i], MASKDWORD);
+}
+
+static void _rtl92ee_phy_save_mac_registers(struct ieee80211_hw *hw,
+					    u32 *macreg, u32 *macbackup)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+
+	for (i = 0; i < (IQK_MAC_REG_NUM - 1); i++)
+		macbackup[i] = rtl_read_byte(rtlpriv, macreg[i]);
+
+	macbackup[i] = rtl_read_dword(rtlpriv, macreg[i]);
+}
+
+static void _rtl92ee_phy_reload_adda_registers(struct ieee80211_hw *hw,
+					       u32 *addareg, u32 *addabackup,
+					       u32 regiesternum)
+{
+	u32 i;
+
+	for (i = 0; i < regiesternum; i++)
+		rtl_set_bbreg(hw, addareg[i], MASKDWORD, addabackup[i]);
+}
+
+static void _rtl92ee_phy_reload_mac_registers(struct ieee80211_hw *hw,
+					      u32 *macreg, u32 *macbackup)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+
+	for (i = 0; i < (IQK_MAC_REG_NUM - 1); i++)
+		rtl_write_byte(rtlpriv, macreg[i], (u8) macbackup[i]);
+	rtl_write_dword(rtlpriv, macreg[i], macbackup[i]);
+}
+
+static void _rtl92ee_phy_path_adda_on(struct ieee80211_hw *hw, u32 *addareg,
+				      bool is_patha_on, bool is2t)
+{
+	u32 pathon;
+	u32 i;
+
+	pathon = is_patha_on ? 0x0fc01616 : 0x0fc01616;
+	if (!is2t) {
+		pathon = 0x0fc01616;
+		rtl_set_bbreg(hw, addareg[0], MASKDWORD, 0x0fc01616);
+	} else {
+		rtl_set_bbreg(hw, addareg[0], MASKDWORD, pathon);
+	}
+
+	for (i = 1; i < IQK_ADDA_REG_NUM; i++)
+		rtl_set_bbreg(hw, addareg[i], MASKDWORD, pathon);
+}
+
+static void _rtl92ee_phy_mac_setting_calibration(struct ieee80211_hw *hw,
+						 u32 *macreg, u32 *macbackup)
+{
+	rtl_set_bbreg(hw, 0x520, 0x00ff0000, 0xff);
+}
+
+static void _rtl92ee_phy_path_a_standby(struct ieee80211_hw *hw)
+{
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x0);
+	rtl_set_rfreg(hw, RF90_PATH_A, 0, RFREG_OFFSET_MASK, 0x10000);
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x80800000);
+}
+
+static bool _rtl92ee_phy_simularity_compare(struct ieee80211_hw *hw,
+					    long result[][8], u8 c1, u8 c2)
+{
+	u32 i, j, diff, simularity_bitmap, bound;
+
+	u8 final_candidate[2] = { 0xFF, 0xFF };
+	bool bresult = true/*, is2t = true*/;
+	s32 tmp1, tmp2;
+
+	bound = 8;
+
+	simularity_bitmap = 0;
+
+	for (i = 0; i < bound; 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) {
+				if (result[c1][i] + result[c1][i + 1] == 0)
+					final_candidate[(i / 4)] = c2;
+				else if (result[c2][i] + result[c2][i + 1] == 0)
+					final_candidate[(i / 4)] = c1;
+				else
+					simularity_bitmap |= (1 << i);
+			} else {
+				simularity_bitmap |= (1 << i);
+			}
+		}
+	}
+
+	if (simularity_bitmap == 0) {
+		for (i = 0; i < (bound / 4); i++) {
+			if (final_candidate[i] != 0xFF) {
+				for (j = i * 4; j < (i + 1) * 4 - 2; j++)
+					result[3][j] =
+						result[final_candidate[i]][j];
+				bresult = false;
+			}
+		}
+		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;
+	}
+}
+
+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);
+	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);
+	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,
+						 IQK_ADDA_REG_NUM);
+		_rtl92ee_phy_save_mac_registers(hw, iqk_mac_reg,
+						rtlphy->iqk_mac_backup);
+		_rtl92ee_phy_save_adda_registers(hw, iqk_bb_reg,
+						 rtlphy->iqk_bb_backup,
+						 IQK_BB_REG_NUM);
+	}
+
+	_rtl92ee_phy_path_adda_on(hw, adda_reg, true, is2t);
+
+	/*BB setting*/
+	rtl_set_bbreg(hw, RFPGA0_RFMOD, BIT(24), 0x00);
+	rtl_set_bbreg(hw, ROFDM0_TRXPATHENABLE, MASKDWORD, 0x03a05600);
+	rtl_set_bbreg(hw, ROFDM0_TRMUXPAR, MASKDWORD, 0x000800e4);
+	rtl_set_bbreg(hw, RFPGA0_XCD_RFINTERFACESW, MASKDWORD, 0x22208200);
+
+	rtl_set_bbreg(hw, RFPGA0_XAB_RFINTERFACESW, BIT(10), 0x01);
+	rtl_set_bbreg(hw, RFPGA0_XAB_RFINTERFACESW, BIT(26), 0x01);
+	rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE, BIT(10), 0x01);
+	rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE, BIT(10), 0x01);
+
+	_rtl92ee_phy_mac_setting_calibration(hw, iqk_mac_reg,
+					     rtlphy->iqk_mac_backup);
+	/* 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);
+
+	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"));
+			result[t][0] = (rtl_get_bbreg(hw,
+						      RTx_Power_Before_IQK_A,
+						      MASKDWORD) & 0x3FF0000)
+						      >> 16;
+			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));
+		}
+	}
+
+	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"));
+			result[t][2] = (rtl_get_bbreg(hw,
+						      RRx_Power_Before_IQK_A_2,
+						      MASKDWORD) & 0x3FF0000)
+						      >> 16;
+			result[t][3] = (rtl_get_bbreg(hw,
+						      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));
+		}
+	}
+
+	if (0x00 == patha_ok)
+		RT_TRACE(COMP_RF, DBG_LOUD, ("Path A IQK failed!!, ret = 0\n"));
+
+	if (is2t) {
+		_rtl92ee_phy_path_a_standby(hw);
+		/* Turn Path B ADDA on */
+		_rtl92ee_phy_path_adda_on(hw, adda_reg, false, is2t);
+
+		/* 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);
+
+		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"));
+				result[t][4] = (rtl_get_bbreg(hw,
+							RTx_Power_Before_IQK_B,
+							MASKDWORD) & 0x3FF0000)
+							>> 16;
+				result[t][5] = (rtl_get_bbreg(hw,
+							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));
+			}
+		}
+
+		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"));
+				result[t][6] = (rtl_get_bbreg(hw,
+						       RRx_Power_Before_IQK_B_2,
+						       MASKDWORD) & 0x3FF0000)
+						       >> 16;
+				result[t][7] = (rtl_get_bbreg(hw,
+						       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));
+			}
+		}
+
+		if (0x00 == pathb_ok)
+			RT_TRACE(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"));
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0);
+
+	if (t != 0) {
+		/* Reload ADDA power saving parameters */
+		_rtl92ee_phy_reload_adda_registers(hw, adda_reg,
+						   rtlphy->adda_backup,
+						   IQK_ADDA_REG_NUM);
+
+		/* Reload MAC parameters */
+		_rtl92ee_phy_reload_mac_registers(hw, iqk_mac_reg,
+						  rtlphy->iqk_mac_backup);
+
+		_rtl92ee_phy_reload_adda_registers(hw, iqk_bb_reg,
+						   rtlphy->iqk_bb_backup,
+						   IQK_BB_REG_NUM);
+
+		/* Restore RX initial gain */
+		rtl_set_bbreg(hw, 0xc50, MASKBYTE0, 0x50);
+		rtl_set_bbreg(hw, 0xc50, MASKBYTE0, tmp_0xc50);
+		if (is2t) {
+			rtl_set_bbreg(hw, 0xc50, MASKBYTE0, 0x50);
+			rtl_set_bbreg(hw, 0xc58, MASKBYTE0, tmp_0xc58);
+		}
+
+		/* 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);
+	}
+	RT_TRACE(COMP_RF, DBG_LOUD, ("_rtl92ee_phy_iq_calibrate() <==\n"));
+}
+
+static void _rtl92ee_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, 0x18, MASK12BITS, lc_cal | 0x08000);
+
+	mdelay(100);
+
+	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 _rtl92ee_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(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, 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);
+		}
+	}
+}
+
+#undef IQK_ADDA_REG_NUM
+#undef IQK_DELAY_TIME
+
+static u8 rtl92ee_get_rightchnlplace_for_iqk(u8 chnl)
+{
+	u8 channel_all[59] = {
+		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 rtl92ee_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	long result[4][8];
+	u8 i, final_candidate;
+	bool b_patha_ok, b_pathb_ok;
+	long reg_e94, reg_e9c, reg_ea4, reg_eac;
+	long reg_eb4, reg_ebc, reg_ec4, reg_ecc;
+	bool is12simular, is13simular, is23simular;
+	u8 idx;
+	u32 iqk_bb_reg[IQK_BB_REG_NUM] = {
+		ROFDM0_XARXIQIMBALANCE,
+		ROFDM0_XBRXIQIMBALANCE,
+		ROFDM0_ECCATHRESHOLD,
+		ROFDM0_AGCRSSITABLE,
+		ROFDM0_XATXIQIMBALANCE,
+		ROFDM0_XBTXIQIMBALANCE,
+		ROFDM0_XCTXAFE,
+		ROFDM0_XDTXAFE,
+		ROFDM0_RXIQEXTANTA
+	};
+
+	if (b_recovery) {
+		_rtl92ee_phy_reload_adda_registers(hw, iqk_bb_reg,
+						   rtlphy->iqk_bb_backup, 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_candidate = 0xff;
+	b_patha_ok = false;
+	b_pathb_ok = false;
+	is12simular = false;
+	is23simular = false;
+	is13simular = false;
+	for (i = 0; i < 3; i++) {
+		_rtl92ee_phy_iq_calibrate(hw, result, i, true);
+		if (i == 1) {
+			is12simular = _rtl92ee_phy_simularity_compare(hw,
+								      result,
+								      0, 1);
+			if (is12simular) {
+				final_candidate = 0;
+				break;
+			}
+		}
+
+		if (i == 2) {
+			is13simular = _rtl92ee_phy_simularity_compare(hw,
+								      result,
+								      0, 2);
+			if (is13simular) {
+				final_candidate = 0;
+				break;
+			}
+			is23simular = _rtl92ee_phy_simularity_compare(hw,
+								      result,
+								      1, 2);
+			if (is23simular)
+				final_candidate = 1;
+			else
+				final_candidate = 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_candidate != 0xff) {
+		reg_e94 = result[final_candidate][0];
+		rtlphy->reg_e94 = reg_e94;
+		reg_e9c = result[final_candidate][1];
+		rtlphy->reg_e9c = reg_e9c;
+		reg_ea4 = result[final_candidate][2];
+		reg_eac = result[final_candidate][3];
+		reg_eb4 = result[final_candidate][4];
+		rtlphy->reg_eb4 = reg_eb4;
+		reg_ebc = result[final_candidate][5];
+		rtlphy->reg_ebc = reg_ebc;
+		reg_ec4 = result[final_candidate][6];
+		reg_ecc = result[final_candidate][7];
+		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) */
+		_rtl92ee_phy_path_a_fill_iqk_matrix(hw, b_patha_ok, result,
+						    final_candidate,
+						    (reg_ea4 == 0));
+
+	_rtl92ee_phy_path_b_fill_iqk_matrix(hw, b_pathb_ok, result,
+					    final_candidate,
+					    (reg_ec4 == 0));
+
+	idx = rtl92ee_get_rightchnlplace_for_iqk(rtlphy->current_channel);
+
+	/* To Fix BSOD when final_candidate is 0xff
+	 * by sherry 20120321 */
+	if (final_candidate < 4) {
+		for (i = 0; i < IQK_MATRIX_REG_NUM; i++)
+			rtlphy->iqk_matrix_regsetting[idx].value[0][i] =
+				result[final_candidate][i];
+
+		rtlphy->iqk_matrix_regsetting[idx].b_iqk_done = true;
+	}
+	_rtl92ee_phy_save_adda_registers(hw, iqk_bb_reg,
+					 rtlphy->iqk_bb_backup, 9);
+}
+
+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);
+	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);
+
+	_rtl92ee_phy_lc_calibrate(hw, false);
+
+	rtlphy->lck_inprogress = false;
+}
+
+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)
+{
+	_rtl92ee_phy_set_rfpath_switch(hw, bmain, false);
+}
+
+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;
+
+	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:
+			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;
+	}
+	rtl92ee_phy_set_io(hw);
+	RT_TRACE(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);
+
+	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:
+		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"));
+		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;
+		rtl92ee_dm_write_dig(hw, 0x17);
+		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"));
+		break;
+	}
+	rtlphy->set_io_inprogress = false;
+	RT_TRACE(COMP_CMD, DBG_TRACE,
+		 ("(%#x)\n", rtlphy->current_io_type));
+}
+
+static void rtl92ee_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_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);
+}
+
+static void _rtl92ee_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_SYS_FUNC_EN, 0xE2);
+	rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x22);
+}
+
+static bool _rtl92ee_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;
+			u32 init_count = 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));
+			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;
+			rtl92ee_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"));
+			stg_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;
+		_rtl92ee_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 rtl92ee_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 = _rtl92ee_phy_set_rf_power_state(hw, rfpwr_state);
+	return bresult;
+}
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/phy.h b/drivers/staging/rtl8192ee/rtl8192ee/phy.h
new file mode 100644
index 00000000000..5be6c4866e1
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/phy.h
@@ -0,0 +1,154 @@
+/******************************************************************************
+ *
+ * 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 __RTL92E_PHY_H__
+#define __RTL92E_PHY_H__
+
+/*It must always set to 4, otherwise read efuse table secquence 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_DOZE_WAITING_TIMES_9x		64
+
+#define RT_CANNOT_IO(hw)			false
+#define HIGHPOWER_RADIOA_ARRAYLEN		22
+
+#define IQK_ADDA_REG_NUM			16
+#define IQK_MAC_REG_NUM				4
+#define IQK_BB_REG_NUM				9
+#define MAX_TOLERANCE				5
+#define	IQK_DELAY_TIME				10
+#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 LOOP_LIMIT				5
+#define MAX_STALL_TIME				50
+#define AntennaDiversityValue			0x80
+#define MAX_TXPWR_IDX_NMODE_92S			63
+#define Reset_Cnt_Limit				3
+
+#define RF6052_MAX_PATH				2
+
+#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_HT402S_TX_PWR_IDX_DIFF	0x66
+#define CT_OFFSET_HT20_TX_PWR_IDX_DIFF		0x69
+#define CT_OFFSET_OFDM_TX_PWR_IDX_DIFF		0x6C
+
+#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 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 baseband_config_type {
+	BASEBAND_CONFIG_PHY_REG = 0,
+	BASEBAND_CONFIG_AGC_TAB = 1,
+};
+
+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,
+
+};
+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);
+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);
+#endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/pwrseq.c b/drivers/staging/rtl8192ee/rtl8192ee/pwrseq.c
new file mode 100644
index 00000000000..08a2df8dfd2
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/pwrseq.c
@@ -0,0 +1,108 @@
+/******************************************************************************
+ *
+ * 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 "pwrseqcmd.h"
+#include "pwrseq.h"
+
+
+/*
+    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] = {
+	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] = {
+	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
+	RTL8192E_TRANS_CARDEMU_TO_CARDDIS
+	RTL8192E_TRANS_END
+};
+
+/*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_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] = {
+	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] = {
+	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] = {
+	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] = {
+	/*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] = {
+	/*FW behavior*/
+	RTL8192E_TRANS_LPS_TO_ACT
+	RTL8192E_TRANS_END
+};
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/pwrseq.h b/drivers/staging/rtl8192ee/rtl8192ee/pwrseq.h
new file mode 100644
index 00000000000..52889721322
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/pwrseq.h
@@ -0,0 +1,355 @@
+/******************************************************************************
+ *
+ * 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 __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
+*/
+
+#define	RTL8192E_TRANS_CARDEMU_TO_ACT_STEPS	18
+#define	RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS	18
+#define	RTL8192E_TRANS_CARDEMU_TO_SUS_STEPS	18
+#define	RTL8192E_TRANS_SUS_TO_CARDEMU_STEPS	18
+#define	RTL8192E_TRANS_CARDEMU_TO_PDN_STEPS	18
+#define	RTL8192E_TRANS_PDN_TO_CARDEMU_STEPS	18
+#define	RTL8192E_TRANS_ACT_TO_LPS_STEPS		23
+#define	RTL8192E_TRANS_LPS_TO_ACT_STEPS		23
+#define	RTL8192E_TRANS_END_STEPS		1
+
+
+#define RTL8192E_TRANS_CARDEMU_TO_ACT					\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+	/* 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 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},			\
+	/* 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},		\
+	/* 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)},		\
+	/* 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 RTL8192E_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[23]=0x4E[7]=0, switch DPDT_SEL_P output from register 0x65[2] */\
+	{0x004E, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(7), 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},
+
+
+#define RTL8192E_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)},				\
+	/*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 RTL8192E_TRANS_SUS_TO_CARDEMU					\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+	/*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},
+
+
+#define RTL8192E_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},			\
+	/*Unlock small LDO Register*/					\
+	{0x00CC, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(2), BIT(2)},		\
+	/*Disable small LDO*/						\
+	{0x0011, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 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_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},
+
+
+#define RTL8192E_TRANS_CARDDIS_TO_CARDEMU				\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+	/*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)},		\
+	/*Enable small LDO*/						\
+	{0x0011, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(0), BIT(0)},		\
+	/*Lock small LDO Register*/					\
+	{0x00CC, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(2), 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 RTL8192E_TRANS_CARDEMU_TO_PDN					\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+	/* 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 RTL8192E_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 RTL8192E_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 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},			\
+	/*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},			\
+	/*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},			\
+	/*Clear ISR*/							\
+	{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 */						\
+	/* { 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},
+
+extern struct wlan_pwr_cfg rtl8192E_power_on_flow
+					[RTL8192E_TRANS_CARDEMU_TO_ACT_STEPS +
+					 RTL8192E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8192E_radio_off_flow
+					[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
+					 RTL8192E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8192E_card_disable_flow
+					[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
+					 RTL8192E_TRANS_CARDEMU_TO_PDN_STEPS +
+					 RTL8192E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8192E_card_enable_flow
+					[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
+					 RTL8192E_TRANS_CARDEMU_TO_PDN_STEPS +
+					 RTL8192E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8192E_suspend_flow
+					[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
+					 RTL8192E_TRANS_CARDEMU_TO_SUS_STEPS +
+					 RTL8192E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8192E_resume_flow
+					[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
+					 RTL8192E_TRANS_CARDEMU_TO_SUS_STEPS +
+					 RTL8192E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8192E_hwpdn_flow
+					[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
+					 RTL8192E_TRANS_CARDEMU_TO_PDN_STEPS +
+					 RTL8192E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8192E_enter_lps_flow
+					[RTL8192E_TRANS_ACT_TO_LPS_STEPS +
+					 RTL8192E_TRANS_END_STEPS];
+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
+#endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/pwrseqcmd.c b/drivers/staging/rtl8192ee/rtl8192ee/pwrseqcmd.c
new file mode 100644
index 00000000000..efb00f62236
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/pwrseqcmd.c
@@ -0,0 +1,139 @@
+/******************************************************************************
+ *
+ * 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
new file mode 100644
index 00000000000..ec40ea8b8c2
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/pwrseqcmd.h
@@ -0,0 +1,69 @@
+#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/rtl8192ee/reg.h b/drivers/staging/rtl8192ee/rtl8192ee/reg.h
new file mode 100644
index 00000000000..08c07577239
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/reg.h
@@ -0,0 +1,2240 @@
+/******************************************************************************
+ *
+ * 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 __RTL92E_REG_H__
+#define __RTL92E_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_SYS_SWR_CTRL1			0x0010
+#define REG_SPS0_CTRL				0x0011
+#define REG_SYS_SWR_CTRL2			0x0014
+#define REG_SYS_SWR_CTRL3			0x0018
+#define REG_RSV_CTRL				0x001C
+#define REG_RF_CTRL				0x001F
+#define REG_LPLDO_CTRL				0x0023
+#define REG_AFE_CTRL1				0x0024
+#define REG_AFE_XTAL_CTRL			0x0024
+#define REG_AFE_CTRL2				0x0028
+#define REG_MAC_PHY_CTRL			0x002c
+#define REG_AFE_CTRL3				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_SDIO_CTRL				0x0070
+#define REG_OPT_CTRL				0x0074
+#define REG_GPIO_OUTPUT				0x006c
+#define REG_AFE_CTRL4				0x0078
+#define REG_MCUFWDL				0x0080
+
+#define REG_HIMR				0x00B0
+#define REG_HISR				0x00B4
+#define REG_HIMRE				0x00B8
+#define REG_HISRE				0x00BC
+
+#define REG_EFUSE_ACCESS			0x00CF
+#define REG_HPON_FSM				0x00EC
+#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
+#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_RXPKTBUF_CTRL			0x0142
+#define REG_PKTBUF_DBG_DATA_L			0x0144
+#define REG_PKTBUF_DBG_DATA_H			0x0148
+
+#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_RSVD3				0x0168
+#define REG_C2HEVT_MSG_NORMAL			0x01A0
+#define REG_C2HEVT_CLEAR			0x01AF
+#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_HMEBOX_EXT_0			0x01F0
+#define REG_HMEBOX_EXT_1			0x01F4
+#define REG_HMEBOX_EXT_2			0x01F8
+#define REG_HMEBOX_EXT_3			0x01FC
+
+/*-----------------------------------------------------
+ *
+ *	0x0200h ~ 0x027Fh	TXDMA Configuration
+ *
+ *-----------------------------------------------------*/
+#define REG_RQPN				0x0200
+#define REG_FIFOPAGE				0x0204
+#define REG_DWBCN0_CTRL				0x0208
+#define REG_TXDMA_OFFSET_CHK			0x020C
+#define REG_TXDMA_STATUS			0x0210
+#define REG_RQPN_NPQ				0x0214
+#define REG_AUTO_LLT				0x0224
+#define REG_DWBCN1_CTRL				0x0228
+
+/*-----------------------------------------------------
+ *
+ *	0x0280h ~ 0x02FFh	RXDMA Configuration
+ *
+ *-----------------------------------------------------*/
+#define REG_RXDMA_AGG_PG_TH			0x0280
+#define REG_FW_UPD_RDPTR			0x0284
+#define REG_RXDMA_CONTROL			0x0286
+#define REG_RXPKT_NUM				0x0287
+#define REG_RXDMA_STATUS			0x0288
+#define REG_RXDMA_PRO				0x0290
+#define REG_EARLY_MODE_CONTROL			0x02BC
+#define REG_RSVD5				0x02F0
+#define REG_RSVD6				0x02F4
+
+/*-----------------------------------------------------
+ *
+ *	0x0300h ~ 0x03FFh	PCIe
+ *
+ *-----------------------------------------------------*/
+#define	REG_PCIE_CTRL_REG			0x0300
+#define	REG_INT_MIG				0x0304
+#define	REG_BCNQ_DESA				0x0308
+#define	REG_MGQ_DESA				0x0310
+#define	REG_VOQ_DESA				0x0318
+#define	REG_VIQ_DESA				0x0320
+#define	REG_BEQ_DESA				0x0328
+#define	REG_BKQ_DESA				0x0330
+#define	REG_RX_DESA				0x0338
+#define	REG_HQ0_DESA				0x0340
+#define	REG_HQ1_DESA				0x0348
+#define	REG_HQ2_DESA				0x0350
+#define	REG_HQ3_DESA				0x0358
+#define	REG_HQ4_DESA				0x0360
+#define	REG_HQ5_DESA				0x0368
+#define	REG_HQ6_DESA				0x0370
+#define	REG_HQ7_DESA				0x0378
+#define	REG_MGQ_TXBD_NUM			0x0380
+#define	REG_RX_RXBD_NUM				0x0382
+#define	REG_VOQ_TXBD_NUM			0x0384
+#define	REG_VIQ_TXBD_NUM			0x0386
+#define	REG_BEQ_TXBD_NUM			0x0388
+#define	REG_BKQ_TXBD_NUM			0x038A
+#define	REG_HI0Q_TXBD_NUM			0x038C
+#define	REG_HI1Q_TXBD_NUM			0x038E
+#define	REG_HI2Q_TXBD_NUM			0x0390
+#define	REG_HI3Q_TXBD_NUM			0x0392
+#define	REG_HI4Q_TXBD_NUM			0x0394
+#define	REG_HI5Q_TXBD_NUM			0x0396
+#define	REG_HI6Q_TXBD_NUM			0x0398
+#define	REG_HI7Q_TXBD_NUM			0x039A
+#define	REG_TSFTIMER_HCI			0x039C
+/*Read Write Point*/
+#define	REG_VOQ_TXBD_IDX			0x03A0
+#define	REG_VIQ_TXBD_IDX			0x03A4
+#define	REG_BEQ_TXBD_IDX			0x03A8
+#define	REG_BKQ_TXBD_IDX			0x03AC
+#define	REG_MGQ_TXBD_IDX			0x03B0
+#define	REG_RXQ_TXBD_IDX			0x03B4
+
+#define	REG_HI0Q_TXBD_IDX			0x03B8
+#define	REG_HI1Q_TXBD_IDX			0x03BC
+#define	REG_HI2Q_TXBD_IDX			0x03C0
+#define	REG_HI3Q_TXBD_IDX			0x03C4
+
+#define	REG_HI4Q_TXBD_IDX			0x03C8
+#define	REG_HI5Q_TXBD_IDX			0x03CC
+#define	REG_HI6Q_TXBD_IDX			0x03D0
+#define	REG_HI7Q_TXBD_IDX			0x03D4
+#define	REG_PCIE_HCPWM				0x03D8
+#define	REG_PCIE_CTRL2				0x03DB
+#define	REG_PCIE_HRPWM				0x03DC
+#define	REG_H2C_MSG_DRV2FW_INFO			0x03E0
+#define	REG_PCIE_C2H_MSG_REQUEST		0x03E4
+#define	REG_BACKDOOR_DBI_WDATA			0x03E8
+#define	REG_BACKDOOR_DBI_RDATA			0x03EC
+#define	REG_BACKDOOR_DBI_DATA			0x03F0
+#define	REG_MDIO				0x03F4
+#define	REG_MDIO_DATA				0x03F8
+
+#define	REG_HDAQ_DESA_NODEF			0x0000
+#define	REG_CMDQ_DESA_NODEF			0x0000
+/* spec version 11
+ *-----------------------------------------------------
+ *
+ *	0x0400h ~ 0x047Fh	Protocol Configuration
+ *
+ *-----------------------------------------------------*/
+#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_FWHW_TXQ_CTRL			0x0420
+#define REG_HWSEQ_CTRL				0x0423
+#define REG_BCNQ_BDNY				0x0424
+#define REG_MGQ_BDNY				0x0425
+#define REG_LIFECTRL_CTRL			0x0426
+#define REG_MULTI_BCNQ_OFFSET			0x0427
+#define REG_SPEC_SIFS				0x0428
+#define REG_RETRY_LIMIT				0x042A
+#define REG_TXBF_CTRL				0x042C
+#define REG_DARFRC				0x0430
+#define REG_RARFRC				0x0438
+#define REG_RRSR				0x0440
+#define REG_ARFR0				0x0444
+#define REG_ARFR1				0x044C
+#define REG_AMPDU_MAX_TIME			0x0456
+#define REG_BCNQ1_BDNY				0x0457
+#define REG_AGGLEN_LMT				0x0458
+#define REG_AMPDU_MIN_SPACE			0x045C
+#define REG_TXPKTBUF_WMAC_LBK_BF_HD		0x045D
+#define REG_NDPA_OPT_CTRL			0x045F
+#define REG_FAST_EDCA_CTRL			0x0460
+#define REG_RD_RESP_PKT_TH			0x0463
+#define REG_POWER_STAGE1			0x04B4
+#define REG_POWER_STAGE2			0x04B8
+#define REG_AMPDU_BURST_MODE			0x04BC
+#define REG_PKT_VO_VI_LIFE_TIME			0x04C0
+#define REG_PKT_BE_BK_LIFE_TIME			0x04C2
+#define REG_STBC_SETTING			0x04C4
+#define REG_PROT_MODE_CTRL			0x04C8
+#define REG_MAX_AGGR_NUM			0x04CA
+#define REG_RTS_MAX_AGGR_NUM			0x04CB
+#define REG_BAR_MODE_CTRL			0x04CC
+#define REG_RA_TRY_RATE_AGG_LMT			0x04CF
+#define REG_MACID_PKT_DROP0			0x04D0
+
+/*-----------------------------------------------------
+ *
+ *	0x0500h ~ 0x05FFh	EDCA Configuration
+ *
+ *-----------------------------------------------------*/
+#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_BCN_CTRL_1				0x0551
+#define REG_MBID_NUM				0x0552
+#define REG_DUAL_TSF_RST			0x0553
+#define REG_BCN_INTERVAL			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_CTWND				0x0572
+#define REG_PSTIMER				0x0580
+#define REG_TIMER0				0x0584
+#define REG_TIMER1				0x0588
+#define REG_BCN_PREDL_ITV			0x058F
+#define REG_ACMHWCTRL				0x05C0
+
+/*-----------------------------------------------------
+ *
+ *	0x0600h ~ 0x07FFh	WMAC Configuration
+ *
+ *-----------------------------------------------------*/
+#define REG_MAC_CR				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_UPPER				0x0652
+
+/* Security*/
+#define REG_CAMCMD				0x0670
+#define REG_CAMWRITE				0x0674
+#define REG_CAMREAD				0x0678
+#define REG_CAMDBG				0x067C
+#define REG_SECCFG				0x0680
+
+/* Power*/
+#define REG_WOW_CTRL				0x0690
+#define REG_PS_RX_INFO				0x0692
+#define REG_UAPSD_TID				0x0693
+#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_BT_COEX_TABLE			0x06C0
+#define REG_BFMER0_INFO				0x06E4
+#define REG_BFMER1_INFO				0x06EC
+#define REG_CSI_RPT_PARAM_BW20			0x06F4
+#define REG_CSI_RPT_PARAM_BW40			0x06F8
+#define REG_CSI_RPT_PARAM_BW80			0x06FC
+/* Hardware Port 2*/
+#define REG_MACID1				0x0700
+#define REG_BSSID1				0x0708
+#define REG_BFMEE_SEL				0x0714
+#define REG_SND_PTCL_CTRL			0x0718
+
+
+#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)
+
+#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	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
+
+
+/*********************************************
+*       8192EE IMR/ISR bits
+**********************************************/
+#define	IMR_DISABLED				0x0
+/* IMR DW0(0x0060-0063) Bit 0-31 */
+#define	IMR_TIMER2				BIT(31)
+#define	IMR_TIMER1				BIT(30)
+#define	IMR_PSTIMEOUT				BIT(29)
+#define	IMR_GTINT4				BIT(28)
+#define	IMR_GTINT3				BIT(27)
+#define	IMR_TBDER				BIT(26)
+#define	IMR_TBDOK				BIT(25)
+#define	IMR_TSF_BIT32_TOGGLE			BIT(24)
+#define	IMR_BCNDMAINT0				BIT(20)
+#define	IMR_BCNDOK0				BIT(16)
+#define	IMR_BCNDMAINT_E				BIT(14)
+#define	IMR_ATIMEND				BIT(12)
+#define	IMR_HISR1_IND_INT			BIT(11)
+#define	IMR_C2HCMD				BIT(10)
+#define	IMR_CPWM2				BIT(9)
+#define	IMR_CPWM				BIT(8)
+#define	IMR_HIGHDOK				BIT(7)
+#define	IMR_MGNTDOK				BIT(6)
+#define	IMR_BKDOK				BIT(5)
+#define	IMR_BEDOK				BIT(4)
+#define	IMR_VIDOK				BIT(3)
+#define	IMR_VODOK				BIT(2)
+#define	IMR_RDU					BIT(1)
+#define	IMR_ROK					BIT(0)
+
+/* IMR DW1(0x00B4-00B7) Bit 0-31 */
+#define	IMR_MCUERR				BIT(28)
+#define	IMR_BCNDMAINT7				BIT(27)
+#define	IMR_BCNDMAINT6				BIT(26)
+#define	IMR_BCNDMAINT5				BIT(25)
+#define	IMR_BCNDMAINT4				BIT(24)
+#define	IMR_BCNDMAINT3				BIT(23)
+#define	IMR_BCNDMAINT2				BIT(22)
+#define	IMR_BCNDMAINT1				BIT(21)
+#define	IMR_BCNDOK7				BIT(20)
+#define	IMR_BCNDOK6				BIT(19)
+#define	IMR_BCNDOK5				BIT(18)
+#define	IMR_BCNDOK4				BIT(17)
+#define	IMR_BCNDOK3				BIT(16)
+#define	IMR_BCNDOK2				BIT(15)
+#define	IMR_BCNDOK1				BIT(14)
+#define	IMR_ATIMEND_E				BIT(13)
+#define	IMR_TXERR				BIT(11)
+#define	IMR_RXERR				BIT(10)
+#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
+#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		0x1A
+#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				0x7C
+
+#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	RTL8192E_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_92E				0xB9
+#define	EEPROM_THERMAL_METER_92E		0xBA
+#define	EEPROM_IQK_LCK_92E			0xBB
+
+#define	EEPROM_RF_BOARD_OPTION_92E		0xC1
+#define	EEPROM_RF_FEATURE_OPTION_92E		0xC2
+#define	EEPROM_RF_BT_SETTING_92E		0xC3
+#define	EEPROM_VERSION				0xC4
+#define	EEPROM_CUSTOMER_ID			0xC5
+#define	EEPROM_RF_ANTENNA_OPT_92E		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_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 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
+
+#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	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_RFWAKEUPPARAMETER		0x850
+#define	RFPGA0_RFSLEEPUPPARAMETER		0x854
+
+#define	RFPGA0_XAB_SWITCHCONTROL		0x858
+#define	RFPGA0_XCD_SWITCHCONTROL		0x85c
+
+#define	RFPGA0_XA_RFINTERFACEOE			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	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	RFPGA1_RFMOD				0x900
+
+#define	RFPGA1_TXBLOCK				0x904
+#define	RFPGA1_DEBUGSELECT			0x908
+#define	RFPGA1_TXINFO				0x90c
+
+#define	RCCK0_SYSTEM				0xa00
+
+#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
+
+/*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
+
+#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_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_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	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				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	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)
+#endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/rf.c b/drivers/staging/rtl8192ee/rtl8192ee/rf.c
new file mode 100644
index 00000000000..4f5a49ebca1
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/rf.c
@@ -0,0 +1,150 @@
+/******************************************************************************
+ *
+ * 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 "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "rf.h"
+#include "dm.h"
+
+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);
+
+	switch (bandwidth) {
+	case HT_CHANNEL_WIDTH_20:
+		rtlphy->rfreg_chnlval[0] = ((rtlphy->rfreg_chnlval[0] &
+					     0xfffff3ff) | BIT(10) | BIT(11));
+		rtl_set_rfreg(hw, RF90_PATH_A, RF_CHNLBW, RFREG_OFFSET_MASK,
+			      rtlphy->rfreg_chnlval[0]);
+		rtl_set_rfreg(hw, RF90_PATH_B, RF_CHNLBW, RFREG_OFFSET_MASK,
+			      rtlphy->rfreg_chnlval[0]);
+		break;
+	case HT_CHANNEL_WIDTH_20_40:
+		rtlphy->rfreg_chnlval[0] = ((rtlphy->rfreg_chnlval[0] &
+					     0xfffff3ff) | BIT(10));
+		rtl_set_rfreg(hw, RF90_PATH_A, RF_CHNLBW, RFREG_OFFSET_MASK,
+			      rtlphy->rfreg_chnlval[0]);
+		rtl_set_rfreg(hw, RF90_PATH_B, RF_CHNLBW, RFREG_OFFSET_MASK,
+			      rtlphy->rfreg_chnlval[0]);
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("unknown bandwidth: %#X\n", bandwidth));
+		break;
+	}
+}
+
+bool rtl92ee_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 _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);
+	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];
+
+		switch (rfpath) {
+		case RF90_PATH_A:
+		case RF90_PATH_C:
+			u4_regvalue = rtl_get_bbreg(hw, pphyreg->rfintfs,
+						    BRFSI_RFENV);
+			break;
+		case RF90_PATH_B:
+		case RF90_PATH_D:
+			u4_regvalue = rtl_get_bbreg(hw, pphyreg->rfintfs,
+						    BRFSI_RFENV << 16);
+			break;
+		}
+
+		rtl_set_bbreg(hw, pphyreg->rfintfe, BRFSI_RFENV << 16, 0x1);
+		udelay(1);
+
+		rtl_set_bbreg(hw, pphyreg->rfintfo, BRFSI_RFENV, 0x1);
+		udelay(1);
+
+		rtl_set_bbreg(hw, pphyreg->rfhssi_para2,
+			      B3WIREADDREAALENGTH, 0x0);
+		udelay(1);
+
+		rtl_set_bbreg(hw, pphyreg->rfhssi_para2, B3WIREDATALENGTH, 0x0);
+		udelay(1);
+
+		switch (rfpath) {
+		case RF90_PATH_A:
+			rtstatus = rtl92ee_phy_config_rf_with_headerfile(hw,
+						       (enum radio_path)rfpath);
+			break;
+		case RF90_PATH_B:
+			rtstatus = rtl92ee_phy_config_rf_with_headerfile(hw,
+						       (enum radio_path)rfpath);
+			break;
+		case RF90_PATH_C:
+			break;
+		case RF90_PATH_D:
+			break;
+		}
+
+		switch (rfpath) {
+		case RF90_PATH_A:
+		case RF90_PATH_C:
+			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, u4_regvalue);
+			break;
+		}
+		if (!rtstatus) {
+			RT_TRACE(COMP_INIT, DBG_TRACE,
+				 ("Radio[%d] Fail!!", rfpath));
+			return false;
+		}
+	}
+
+	RT_TRACE(COMP_INIT, DBG_TRACE, ("\n"));
+	return rtstatus;
+}
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/rf.h b/drivers/staging/rtl8192ee/rtl8192ee/rf.h
new file mode 100644
index 00000000000..5bc394af476
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/rf.h
@@ -0,0 +1,39 @@
+/******************************************************************************
+ *
+ * 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 __RTL92E_RF_H__
+#define __RTL92E_RF_H__
+
+#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);
+#endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/sw.c b/drivers/staging/rtl8192ee/rtl8192ee/sw.c
new file mode 100644
index 00000000000..f9c5729e2cd
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/sw.c
@@ -0,0 +1,428 @@
+/******************************************************************************
+ *
+ * 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 "../wifi.h"
+#include "../core.h"
+#include "../pci.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "dm.h"
+#include "hw.h"
+#include "sw.h"
+#include "fw.h"
+#include "trx.h"
+#include "led.h"
+#include "table.h"
+
+#include "../btcoexist/rtl_btc.h"
+
+
+static void rtl92ee_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.
+	 * 0 - Disable ASPM,
+	 * 1 - Enable ASPM without Clock Req,
+	 * 2 - Enable ASPM with Clock Req,
+	 * 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 */
+	rtlpci->const_devicepci_aspm_setting = 0x03;
+
+	/*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,
+	 * 2 - From ASPM setting with low Mac Pwr,
+	 * 3 - Bus D3
+	 * set default to RTL8192CE:0 RTL8192SE:2
+	 */
+	rtlpci->const_hwsw_rfoff_d3 = 0;
+
+	/*
+	 * This setting works for those device with
+	 * backdoor ASPM setting such as EPHY setting.
+	 * 0 - Not support ASPM,
+	 * 1 - Support ASPM,
+	 * 2 - According to chipset.
+	 */
+	rtlpci->const_support_pciaspm = 1;
+}
+
+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;
+
+	rtl92ee_bt_reg_init(hw);
+
+	rtlpci->msi_support = true;
+	rtlpriv->btcoexist.btc_ops = stg_rtl_btc_get_ops_pointer();
+
+	rtlpriv->dm.b_dm_initialgain_enable = 1;
+	rtlpriv->dm.dm_flag = 0;
+	rtlpriv->dm.b_disable_framebursting = 0;
+	/*rtlpriv->dm.thermalvalue = 0;*/
+	rtlpci->transmit_config = CFENDFORM | BIT(15);
+
+	/*just 2.4G band*/
+	rtlpriv->rtlhal.current_bandtype = BAND_ON_2_4G;
+	rtlpriv->rtlhal.bandset = BAND_ON_2_4G;
+	rtlpriv->rtlhal.macphymode = SINGLEMAC_SINGLEPHY;
+
+	rtlpci->receive_config = (RCR_APPFCS			|
+				  RCR_APP_MIC			|
+				  RCR_APP_ICV			|
+				  RCR_APP_PHYST_RXFF		|
+				  RCR_HTC_LOC_CTRL		|
+				  RCR_AMF			|
+				  RCR_ACF			|
+				  RCR_ADF			|
+				  RCR_AICV			|
+				  RCR_ACRC32			|
+				  RCR_AB			|
+				  RCR_AM			|
+				  RCR_APM			|
+				  0);
+
+	rtlpci->irq_mask[0] = (u32) (IMR_PSTIMEOUT		|
+				/*   IMR_TBDER			|
+				     IMR_TBDOK			|
+				     IMR_BCNDMAINT0		|*/
+				     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 | 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.reg_max_lps_awakeintvl = 5;
+	/* for ASPM, you can close aspm through
+	 * set const_support_pciaspm = 0 */
+	rtl92ee_init_aspm_vars(hw);
+
+	if (rtlpriv->psc.b_reg_fwctrl_lps == 1)
+		rtlpriv->psc.fwctrl_psmode = FW_PS_MIN_MODE;
+	else if (rtlpriv->psc.b_reg_fwctrl_lps == 2)
+		rtlpriv->psc.fwctrl_psmode = FW_PS_MAX_MODE;
+	else if (rtlpriv->psc.b_reg_fwctrl_lps == 3)
+		rtlpriv->psc.fwctrl_psmode = FW_PS_DTIM_MODE;
+
+	/* for early mode */
+	rtlpriv->rtlhal.b_earlymode_enable = false;
+
+	/*low power */
+	rtlpriv->psc.b_low_power_enable = false;
+
+
+	/* for firmware buf */
+	rtlpriv->rtlhal.pfirmware = vmalloc(0x8000);
+	if (!rtlpriv->rtlhal.pfirmware) {
+		RT_TRACE(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);
+
+	if (err) {
+		RT_TRACE(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;
+}
+
+void rtl92ee_deinit_sw_vars(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rtlpriv->rtlhal.pfirmware) {
+		vfree(rtlpriv->rtlhal.pfirmware);
+		rtlpriv->rtlhal.pfirmware = NULL;
+	}
+}
+
+/* get bt coexist status */
+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,
+	.read_eeprom_info = rtl92ee_read_eeprom_info,
+	.interrupt_recognized = rtl92ee_interrupt_recognized,/*need check*/
+	.hw_init = rtl92ee_hw_init,
+	.hw_disable = rtl92ee_card_disable,
+	.hw_suspend = rtl92ee_suspend,
+	.hw_resume = rtl92ee_resume,
+	.enable_interrupt = rtl92ee_enable_interrupt,
+	.disable_interrupt = rtl92ee_disable_interrupt,
+	.set_network_type = rtl92ee_set_network_type,
+	.set_chk_bssid = rtl92ee_set_check_bssid,
+	.set_qos = rtl92ee_set_qos,
+	.set_bcn_reg = rtl92ee_set_beacon_related_registers,
+	.set_bcn_intv = rtl92ee_set_beacon_interval,
+	.update_interrupt_mask = rtl92ee_update_interrupt_mask,
+	.get_hw_reg = rtl92ee_get_hw_reg,
+	.set_hw_reg = rtl92ee_set_hw_reg,
+	.update_rate_tbl = rtl92ee_update_hal_rate_tbl,
+	.pre_fill_tx_bd_desc = rtl92ee_pre_fill_tx_bd_desc,
+	.rx_desc_buff_remained_cnt = rtl92ee_rx_desc_buff_remained_cnt,
+	.rx_check_dma_ok = rtl92ee_rx_check_dma_ok,
+	.fill_tx_desc = rtl92ee_tx_fill_desc,
+	.fill_tx_cmddesc = rtl92ee_tx_fill_cmddesc,
+	.query_rx_desc = rtl92ee_rx_query_desc,
+	.set_channel_access = rtl92ee_update_channel_access_setting,
+	.radio_onoff_checking = rtl92ee_gpio_radio_on_off_checking,
+	.set_bw_mode = rtl92ee_phy_set_bw_mode,
+	.switch_channel = rtl92ee_phy_sw_chnl,
+	.dm_watchdog = rtl92ee_dm_watchdog,
+	.scan_operation_backup = rtl92ee_phy_scan_operation_backup,
+	.set_rf_power_state = rtl92ee_phy_set_rf_power_state,
+	.led_control = rtl92ee_led_control,
+	.set_desc = rtl92ee_set_desc,
+	.get_desc = rtl92ee_get_desc,
+	.is_tx_desc_closed = rtl92ee_is_tx_desc_closed,
+	.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,
+	.set_rfreg = rtl92ee_phy_set_rf_reg,
+	.fill_h2c_cmd = rtl92ee_fill_h2c_cmd,
+	.get_btc_status = rtl92ee_get_btc_status,
+	.rx_command_packet = rtl92ee_rx_command_packet,
+};
+
+static struct rtl_mod_params rtl92ee_mod_params = {
+	.sw_crypto = false,
+	.b_inactiveps = true,
+	.b_swctrl_lps = false,
+	.b_fwctrl_lps = true,
+	.debug = DBG_EMERG,
+};
+
+static struct rtl_hal_cfg rtl92ee_hal_cfg = {
+	.bar_id = 2,
+	.write_readback = true,
+	.name = "rtl92ee_pci",
+	.fw_name = "rtlwifi/rtl8192eefw.bin",
+	.ops = &rtl8192ee_hal_ops,
+	.mod_params = &rtl92ee_mod_params,
+
+	.maps[SYS_ISO_CTRL] = REG_SYS_ISO_CTRL,
+	.maps[SYS_FUNC_EN] = REG_SYS_FUNC_EN,
+	.maps[SYS_CLK] = REG_SYS_CLKR,
+	.maps[MAC_RCR_AM] = AM,
+	.maps[MAC_RCR_AB] = AB,
+	.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_ACCESS] = REG_EFUSE_ACCESS,
+
+	.maps[EFUSE_TEST] = REG_EFUSE_TEST,
+	.maps[EFUSE_CTRL] = REG_EFUSE_CTRL,
+	.maps[EFUSE_CLK] = 0,
+	.maps[EFUSE_CLK_CTRL] = REG_EFUSE_CTRL,
+	.maps[EFUSE_PWC_EV12V] = PWC_EV12V,
+	.maps[EFUSE_FEN_ELDR] = FEN_ELDR,
+	.maps[EFUSE_LOADER_CLK_EN] = LOADER_CLK_EN,
+	.maps[EFUSE_ANA8M] = ANA8M,
+	.maps[EFUSE_HWSET_MAX_SIZE] = HWSET_MAX_SIZE,
+	.maps[EFUSE_MAX_SECTION_MAP] = EFUSE_MAX_SECTION,
+	.maps[EFUSE_REAL_CONTENT_SIZE] = EFUSE_REAL_CONTENT_LEN,
+	.maps[EFUSE_OOB_PROTECT_BYTES_LEN] = EFUSE_OOB_PROTECT_BYTES,
+
+	.maps[RWCAM] = REG_CAMCMD,
+	.maps[WCAMI] = REG_CAMWRITE,
+	.maps[RCAMO] = REG_CAMREAD,
+	.maps[CAMDBG] = REG_CAMDBG,
+	.maps[SECR] = REG_SECCFG,
+	.maps[SEC_CAM_NONE] = CAM_NONE,
+	.maps[SEC_CAM_WEP40] = CAM_WEP40,
+	.maps[SEC_CAM_TKIP] = CAM_TKIP,
+	.maps[SEC_CAM_AES] = CAM_AES,
+	.maps[SEC_CAM_WEP104] = CAM_WEP104,
+
+	.maps[RTL_IMR_BCNDMAINT6] = IMR_BCNDMAINT6,
+	.maps[RTL_IMR_BCNDMAINT5] = IMR_BCNDMAINT5,
+	.maps[RTL_IMR_BCNDMAINT4] = IMR_BCNDMAINT4,
+	.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,
+	.maps[RTL_IMR_BCNDOK4] = IMR_BCNDOK4,
+	.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_RXFOVW] = IMR_RXFOVW,
+	.maps[RTL_IMR_RDU] = IMR_RDU,
+	.maps[RTL_IMR_ATIMEND] = IMR_ATIMEND,
+	.maps[RTL_IMR_BDOK] = IMR_BCNDOK0,
+	.maps[RTL_IMR_MGNTDOK] = IMR_MGNTDOK,
+	.maps[RTL_IMR_TBDER] = IMR_TBDER,
+	.maps[RTL_IMR_HIGHDOK] = IMR_HIGHDOK,
+	.maps[RTL_IMR_TBDOK] = IMR_TBDOK,
+	.maps[RTL_IMR_BKDOK] = IMR_BKDOK,
+	.maps[RTL_IMR_BEDOK] = IMR_BEDOK,
+	.maps[RTL_IMR_VIDOK] = IMR_VIDOK,
+	.maps[RTL_IMR_VODOK] = IMR_VODOK,
+	.maps[RTL_IMR_ROK] = IMR_ROK,
+	.maps[RTL_IBSS_INT_MASKS] = (IMR_BCNDMAINT0 | IMR_TBDOK | IMR_TBDER),
+
+	.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 rtl92ee_pci_ids[] = {
+	{RTL_PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0x818B, rtl92ee_hal_cfg)},
+	{},
+};
+
+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_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_PARM_DESC(debug, "Set debug level (0-5) (default 0)");
+
+static SIMPLE_DEV_PM_OPS(rtlwifi_pm_ops, stg_rtl_pci_suspend,
+			 stg_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,
+
+	.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);
diff --git a/drivers/staging/rtl8188eu/include/drv_types_linux.h b/drivers/staging/rtl8192ee/rtl8192ee/sw.h
index 812b7440d4b..0170257a20c 100644
--- a/drivers/staging/rtl8188eu/include/drv_types_linux.h
+++ b/drivers/staging/rtl8192ee/rtl8192ee/sw.h
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ * 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
@@ -8,17 +8,32 @@
  *
  * This program is distributed in the hope that it will be useful, but WITHOUT
  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
  * You should have 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.
  *
- ******************************************************************************/
-#ifndef __DRV_TYPES_LINUX_H__
-#define __DRV_TYPES_LINUX_H__
+ * 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 __RTL92E_SW_H__
+#define __RTL92E_SW_H__
 
+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/staging/rtl8192ee/rtl8192ee/table.c
new file mode 100644
index 00000000000..c7eb9be53cc
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/table.c
@@ -0,0 +1,882 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ * Created on  2010/ 5/18,  1:41
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "table.h"
+u32 RTL8192EE_PHY_REG_ARRAY[] = {
+		0x800, 0x80040000,
+		0x804, 0x00000003,
+		0x808, 0x0000FC00,
+		0x80C, 0x0000000A,
+		0x810, 0x10001331,
+		0x814, 0x020C3D10,
+		0x818, 0x02220385,
+		0x81C, 0x00000000,
+		0x820, 0x01000100,
+		0x824, 0x00390204,
+		0x828, 0x01000100,
+		0x82C, 0x00390204,
+		0x830, 0x32323232,
+		0x834, 0x30303030,
+		0x838, 0x30303030,
+		0x83C, 0x30303030,
+		0x840, 0x00010000,
+		0x844, 0x00010000,
+		0x848, 0x28282828,
+		0x84C, 0x28282828,
+		0x850, 0x00000000,
+		0x854, 0x00000000,
+		0x858, 0x009A009A,
+		0x85C, 0x01000014,
+		0x860, 0x66F60000,
+		0x864, 0x061F0000,
+		0x868, 0x30303030,
+		0x86C, 0x30303030,
+		0x870, 0x00000000,
+		0x874, 0x55004200,
+		0x878, 0x08080808,
+		0x87C, 0x00000000,
+		0x880, 0xB0000C1C,
+		0x884, 0x00000001,
+		0x888, 0x00000000,
+		0x88C, 0xCC0000C0,
+		0x890, 0x00000800,
+		0x894, 0xFFFFFFFE,
+		0x898, 0x40302010,
+		0x900, 0x00000000,
+		0x904, 0x00000023,
+		0x908, 0x00000000,
+		0x90C, 0x81121313,
+		0x910, 0x806C0001,
+		0x914, 0x00000001,
+		0x918, 0x00000000,
+		0x91C, 0x00010000,
+		0x924, 0x00000001,
+		0x928, 0x00000000,
+		0x92C, 0x00000000,
+		0x930, 0x00000000,
+		0x934, 0x00000000,
+		0x938, 0x00000000,
+		0x93C, 0x00000000,
+		0x940, 0x00000000,
+		0x944, 0x00000000,
+		0x94C, 0x00000008,
+		0xA00, 0x00D0C7C8,
+		0xA04, 0x81FF000C,
+		0xA08, 0x8C838300,
+		0xA0C, 0x2E68120F,
+		0xA10, 0x95009B78,
+		0xA14, 0x1114D028,
+		0xA18, 0x00881117,
+		0xA1C, 0x89140F00,
+		0xA20, 0x1A1B0000,
+		0xA24, 0x090E1317,
+		0xA28, 0x00000204,
+		0xA2C, 0x00D30000,
+		0xA70, 0x101FBF00,
+		0xA74, 0x00000007,
+		0xA78, 0x00000900,
+		0xA7C, 0x225B0606,
+		0xA80, 0x218075B1,
+		0xB38, 0x00000000,
+		0xC00, 0x48071D40,
+		0xC04, 0x03A05633,
+		0xC08, 0x000000E4,
+		0xC0C, 0x6C6C6C6C,
+		0xC10, 0x08800000,
+		0xC14, 0x40000100,
+		0xC18, 0x08800000,
+		0xC1C, 0x40000100,
+		0xC20, 0x00000000,
+		0xC24, 0x00000000,
+		0xC28, 0x00000000,
+		0xC2C, 0x00000000,
+		0xC30, 0x69E9AC47,
+		0xC34, 0x469652AF,
+		0xC38, 0x49795994,
+		0xC3C, 0x0A97971C,
+		0xC40, 0x1F7C403F,
+		0xC44, 0x000100B7,
+		0xC48, 0xEC020107,
+		0xC4C, 0x007F037F,
+	0xFF010718, 0xABCD,
+		0xC50, 0x00340220,
+	0xCDCDCDCD, 0xCDCD,
+		0xC50, 0x00340020,
+	0xFF010718, 0xDEAD,
+		0xC54, 0x0080801F,
+	0xFF010718, 0xABCD,
+		0xC58, 0x00000220,
+	0xCDCDCDCD, 0xCDCD,
+		0xC58, 0x00000020,
+	0xFF010718, 0xDEAD,
+		0xC5C, 0x00248492,
+		0xC60, 0x00000000,
+		0xC64, 0x7112848B,
+		0xC68, 0x47C00BFF,
+		0xC6C, 0x00000036,
+		0xC70, 0x00000600,
+		0xC74, 0x02013169,
+		0xC78, 0x0000001F,
+		0xC7C, 0x00B91612,
+	0xFF010718, 0xABCD,
+		0xC80, 0x2D4000B5,
+	0xCDCDCDCD, 0xCDCD,
+		0xC80, 0x40000100,
+	0xFF010718, 0xDEAD,
+		0xC84, 0x21F60000,
+	0xFF010718, 0xABCD,
+		0xC88, 0x2D4000B5,
+	0xCDCDCDCD, 0xCDCD,
+		0xC88, 0x40000100,
+	0xFF010718, 0xDEAD,
+		0xC8C, 0xA0E40000,
+		0xC90, 0x00121820,
+		0xC94, 0x00000000,
+		0xC98, 0x00121820,
+		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, 0x00040000,
+		0xCE8, 0x77644302,
+		0xCEC, 0x2F97D40C,
+		0xD00, 0x00080740,
+		0xD04, 0x00020403,
+		0xD08, 0x0000907F,
+		0xD0C, 0x20010201,
+		0xD10, 0xA0633333,
+		0xD14, 0x3333BC43,
+		0xD18, 0x7A8F5B6B,
+		0xD1C, 0x0000007F,
+		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,
+		0xD80, 0x01081008,
+		0xD84, 0x00000800,
+		0xD88, 0xF0B50000,
+		0xE00, 0x30303030,
+		0xE04, 0x30303030,
+		0xE08, 0x03903030,
+		0xE10, 0x30303030,
+		0xE14, 0x30303030,
+		0xE18, 0x30303030,
+		0xE1C, 0x30303030,
+		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, 0x0FC05656,
+		0xE6C, 0x03C09696,
+		0xE70, 0x03C09696,
+		0xE74, 0x0C005656,
+		0xE78, 0x0C005656,
+		0xE7C, 0x0C005656,
+		0xE80, 0x0C005656,
+		0xE84, 0x03C09696,
+		0xE88, 0x0C005656,
+		0xE8C, 0x03C09696,
+		0xED0, 0x03C09696,
+		0xED4, 0x03C09696,
+		0xED8, 0x03C09696,
+		0xEDC, 0x0000D6D6,
+		0xEE0, 0x0000D6D6,
+		0xEEC, 0x0FC01616,
+		0xEE4, 0xB0000C1C,
+		0xEE8, 0x00000001,
+		0xF14, 0x00000003,
+		0xF4C, 0x00000000,
+		0xF00, 0x00000300,
+};
+
+u32 RTL8192EE_PHY_REG_ARRAY_PG[] = {
+	0, 0, 0, 0x00000e08, 0x0000ff00, 0x00003200,
+	0, 0, 1, 0x00000e08, 0x0000ff00, 0x00003200,
+	0, 0, 0, 0x0000086c, 0xffffff00, 0x32323200,
+	0, 0, 1, 0x0000086c, 0xffffff00, 0x32323200,
+	0, 0, 0, 0x00000e00, 0xffffffff, 0x34343636,
+	0, 0, 1, 0x00000e00, 0xffffffff, 0x34343636,
+	0, 0, 0, 0x00000e04, 0xffffffff, 0x28283032,
+	0, 0, 1, 0x00000e04, 0xffffffff, 0x28283032,
+	0, 0, 0, 0x00000e10, 0xffffffff, 0x34363840,
+	0, 0, 1, 0x00000e10, 0xffffffff, 0x34363840,
+	0, 0, 0, 0x00000e14, 0xffffffff, 0x26283032,
+	0, 0, 1, 0x00000e14, 0xffffffff, 0x26283032,
+	0, 0, 1, 0x00000e18, 0xffffffff, 0x36384040,
+	0, 0, 1, 0x00000e1c, 0xffffffff, 0x24262832,
+	0, 1, 0, 0x00000838, 0xffffff00, 0x32323200,
+	0, 1, 1, 0x00000838, 0xffffff00, 0x32323200,
+	0, 1, 0, 0x0000086c, 0x000000ff, 0x00000032,
+	0, 1, 1, 0x0000086c, 0x000000ff, 0x00000032,
+	0, 1, 0, 0x00000830, 0xffffffff, 0x34343636,
+	0, 1, 1, 0x00000830, 0xffffffff, 0x34343636,
+	0, 1, 0, 0x00000834, 0xffffffff, 0x28283032,
+	0, 1, 1, 0x00000834, 0xffffffff, 0x28283032,
+	0, 1, 0, 0x0000083c, 0xffffffff, 0x34363840,
+	0, 1, 1, 0x0000083c, 0xffffffff, 0x34363840,
+	0, 1, 0, 0x00000848, 0xffffffff, 0x26283032,
+	0, 1, 1, 0x00000848, 0xffffffff, 0x26283032,
+	0, 1, 1, 0x0000084c, 0xffffffff, 0x36384040,
+	0, 1, 1, 0x00000868, 0xffffffff, 0x24262832
+};
+
+u32 RTL8192EE_RADIOA_ARRAY[] = {
+		0x07F, 0x00000082,
+		0x081, 0x0003FC00,
+		0x000, 0x00030000,
+		0x008, 0x00008400,
+		0x018, 0x00000407,
+		0x019, 0x00000012,
+		0x01B, 0x00000064,
+		0x01E, 0x00080009,
+		0x01F, 0x00000880,
+		0x02F, 0x0001A060,
+		0x03F, 0x00000000,
+		0x042, 0x000060C0,
+		0x057, 0x000D0000,
+		0x058, 0x000BE180,
+		0x067, 0x00001552,
+		0x083, 0x00000000,
+		0x0B0, 0x000FF9F1,
+		0x0B1, 0x00055418,
+		0x0B2, 0x0008CC00,
+		0x0B4, 0x00043083,
+		0x0B5, 0x00008166,
+		0x0B6, 0x0000803E,
+		0x0B7, 0x0001C69F,
+		0x0B8, 0x0000407F,
+		0x0B9, 0x00080001,
+		0x0BA, 0x00040001,
+		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, 0x00069545,
+		0x052, 0x0007E45E,
+		0x053, 0x00000071,
+		0x056, 0x00051FF3,
+		0x035, 0x000000A8,
+		0x035, 0x000001E2,
+		0x035, 0x000002A8,
+		0x036, 0x00001C24,
+		0x036, 0x00009C24,
+		0x036, 0x00011C24,
+		0x036, 0x00019C24,
+		0x018, 0x00000C07,
+		0x05A, 0x00048000,
+		0x019, 0x000739D0,
+	0xFF010718, 0xABCD,
+		0x034, 0x0000A093,
+		0x034, 0x0000908F,
+		0x034, 0x0000808C,
+		0x034, 0x0000704D,
+		0x034, 0x0000604A,
+		0x034, 0x00005047,
+		0x034, 0x0000400A,
+		0x034, 0x00003007,
+		0x034, 0x00002004,
+		0x034, 0x00001001,
+		0x034, 0x00000000,
+	0xCDCDCDCD, 0xCDCD,
+		0x034, 0x0000ADD7,
+		0x034, 0x00009DD4,
+		0x034, 0x00008DD1,
+		0x034, 0x00007DCE,
+		0x034, 0x00006DCB,
+		0x034, 0x00005DC8,
+		0x034, 0x00004DC5,
+		0x034, 0x000034CC,
+		0x034, 0x0000244F,
+		0x034, 0x0000144C,
+		0x034, 0x00000014,
+	0xFF010718, 0xDEAD,
+		0x000, 0x00030159,
+		0x084, 0x00068180,
+		0x086, 0x0000014E,
+		0x087, 0x00048E00,
+		0x08E, 0x00065540,
+		0x08F, 0x00088000,
+		0x0EF, 0x000020A0,
+	0xFF010718, 0xABCD,
+		0x03B, 0x000F07B0,
+	0xCDCDCDCD, 0xCDCD,
+		0x03B, 0x000F02B0,
+	0xFF010718, 0xDEAD,
+		0x03B, 0x000EF7B0,
+		0x03B, 0x000D4FB0,
+		0x03B, 0x000CF060,
+		0x03B, 0x000B0090,
+		0x03B, 0x000A0080,
+		0x03B, 0x00090080,
+		0x03B, 0x0008F780,
+	0xFF010718, 0xABCD,
+		0x03B, 0x000787B0,
+	0xCDCDCDCD, 0xCDCD,
+		0x03B, 0x00078730,
+	0xFF010718, 0xDEAD,
+		0x03B, 0x00060FB0,
+		0x03B, 0x0005FFA0,
+		0x03B, 0x00040620,
+		0x03B, 0x00037090,
+		0x03B, 0x00020080,
+		0x03B, 0x0001F060,
+		0x03B, 0x0000FFB0,
+		0x0EF, 0x000000A0,
+		0x0FE, 0x00000000,
+		0x018, 0x0000FC07,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x01E, 0x00000001,
+		0x01F, 0x00080000,
+		0x000, 0x00033E70,
+};
+
+u32 RTL8192EE_RADIOB_ARRAY[] = {
+		0x07F, 0x00000082,
+		0x081, 0x0003FC00,
+		0x000, 0x00030000,
+		0x008, 0x00008400,
+		0x018, 0x00000407,
+		0x019, 0x00000012,
+		0x01B, 0x00000064,
+		0x01E, 0x00080009,
+		0x01F, 0x00000880,
+		0x02F, 0x0001A060,
+		0x03F, 0x00000000,
+		0x042, 0x000060C0,
+		0x057, 0x000D0000,
+		0x058, 0x000BE180,
+		0x067, 0x00001552,
+		0x07F, 0x00000082,
+		0x081, 0x0003F000,
+		0x083, 0x00000000,
+		0x0DF, 0x00000180,
+		0x0EF, 0x000001A0,
+		0x051, 0x00069545,
+		0x052, 0x0007E42E,
+		0x053, 0x00000071,
+		0x056, 0x00051FF3,
+		0x035, 0x000000A8,
+		0x035, 0x000001E0,
+		0x035, 0x000002A8,
+		0x036, 0x00001CA8,
+		0x036, 0x00009C24,
+		0x036, 0x00011C24,
+		0x036, 0x00019C24,
+		0x018, 0x00000C07,
+		0x05A, 0x00048000,
+		0x019, 0x000739D0,
+	0xFF010718, 0xABCD,
+		0x034, 0x0000A093,
+		0x034, 0x0000908F,
+		0x034, 0x0000808C,
+		0x034, 0x0000704D,
+		0x034, 0x0000604A,
+		0x034, 0x00005047,
+		0x034, 0x0000400A,
+		0x034, 0x00003007,
+		0x034, 0x00002004,
+		0x034, 0x00001001,
+		0x034, 0x00000000,
+	0xCDCDCDCD, 0xCDCD,
+		0x034, 0x0000ADD7,
+		0x034, 0x00009DD4,
+		0x034, 0x00008DD1,
+		0x034, 0x00007DCE,
+		0x034, 0x00006DCB,
+		0x034, 0x00005DC8,
+		0x034, 0x00004DC5,
+		0x034, 0x000034CC,
+		0x034, 0x0000244F,
+		0x034, 0x0000144C,
+		0x034, 0x00000014,
+	0xFF010718, 0xDEAD,
+		0x000, 0x00030159,
+		0x084, 0x00068180,
+		0x086, 0x000000CE,
+		0x087, 0x00048A00,
+		0x08E, 0x00065540,
+		0x08F, 0x00088000,
+		0x0EF, 0x000020A0,
+	0xFF010718, 0xABCD,
+		0x03B, 0x000F07B0,
+	0xCDCDCDCD, 0xCDCD,
+		0x03B, 0x000F02B0,
+	0xFF010718, 0xDEAD,
+		0x03B, 0x000EF7B0,
+		0x03B, 0x000D4FB0,
+		0x03B, 0x000CF060,
+		0x03B, 0x000B0090,
+		0x03B, 0x000A0080,
+		0x03B, 0x00090080,
+		0x03B, 0x0008F780,
+	0xFF010718, 0xABCD,
+		0x03B, 0x000787B0,
+	0xCDCDCDCD, 0xCDCD,
+		0x03B, 0x00078730,
+	0xFF010718, 0xDEAD,
+		0x03B, 0x00060FB0,
+		0x03B, 0x0005FFA0,
+		0x03B, 0x00040620,
+		0x03B, 0x00037090,
+		0x03B, 0x00020080,
+		0x03B, 0x0001F060,
+		0x03B, 0x0000FFB0,
+		0x0EF, 0x000000A0,
+		0x000, 0x00010159,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x01E, 0x00000001,
+		0x01F, 0x00080000,
+		0x000, 0x00033E70,
+};
+
+u32 RTL8192EE_MAC_ARRAY[] = {
+		0x011, 0x000000EB,
+		0x012, 0x00000007,
+		0x014, 0x00000075,
+		0x303, 0x000000A7,
+		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,
+		0x540, 0x00000012,
+		0x541, 0x00000064,
+		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 RTL8192EE_AGC_TAB_ARRAY[] = {
+	0xFF010718, 0xABCD,
+		0xC78, 0xFA000001,
+		0xC78, 0xF9010001,
+		0xC78, 0xF8020001,
+		0xC78, 0xF7030001,
+		0xC78, 0xF6040001,
+		0xC78, 0xF5050001,
+		0xC78, 0xF4060001,
+		0xC78, 0xF3070001,
+		0xC78, 0xF2080001,
+		0xC78, 0xF1090001,
+		0xC78, 0xF00A0001,
+		0xC78, 0xEF0B0001,
+		0xC78, 0xEE0C0001,
+		0xC78, 0xED0D0001,
+		0xC78, 0xEC0E0001,
+		0xC78, 0xEB0F0001,
+		0xC78, 0xEA100001,
+		0xC78, 0xE9110001,
+		0xC78, 0xE8120001,
+		0xC78, 0xE7130001,
+		0xC78, 0xE6140001,
+		0xC78, 0xE5150001,
+		0xC78, 0xE4160001,
+		0xC78, 0xE3170001,
+		0xC78, 0xE2180001,
+		0xC78, 0xE1190001,
+		0xC78, 0x8A1A0001,
+		0xC78, 0x891B0001,
+		0xC78, 0x881C0001,
+		0xC78, 0x871D0001,
+		0xC78, 0x861E0001,
+		0xC78, 0x851F0001,
+		0xC78, 0x84200001,
+		0xC78, 0x83210001,
+		0xC78, 0x82220001,
+		0xC78, 0x6A230001,
+		0xC78, 0x69240001,
+		0xC78, 0x68250001,
+		0xC78, 0x67260001,
+		0xC78, 0x66270001,
+		0xC78, 0x65280001,
+		0xC78, 0x64290001,
+		0xC78, 0x632A0001,
+		0xC78, 0x622B0001,
+		0xC78, 0x612C0001,
+		0xC78, 0x602D0001,
+		0xC78, 0x472E0001,
+		0xC78, 0x462F0001,
+		0xC78, 0x45300001,
+		0xC78, 0x44310001,
+		0xC78, 0x43320001,
+		0xC78, 0x42330001,
+		0xC78, 0x41340001,
+		0xC78, 0x40350001,
+		0xC78, 0x40360001,
+		0xC78, 0x40370001,
+		0xC78, 0x40380001,
+		0xC78, 0x40390001,
+		0xC78, 0x403A0001,
+		0xC78, 0x403B0001,
+		0xC78, 0x403C0001,
+		0xC78, 0x403D0001,
+		0xC78, 0x403E0001,
+		0xC78, 0x403F0001,
+	0xCDCDCDCD, 0xCDCD,
+		0xC78, 0xFB000001,
+		0xC78, 0xFB010001,
+		0xC78, 0xFB020001,
+		0xC78, 0xFB030001,
+		0xC78, 0xFB040001,
+		0xC78, 0xFB050001,
+		0xC78, 0xFA060001,
+		0xC78, 0xF9070001,
+		0xC78, 0xF8080001,
+		0xC78, 0xF7090001,
+		0xC78, 0xF60A0001,
+		0xC78, 0xF50B0001,
+		0xC78, 0xF40C0001,
+		0xC78, 0xF30D0001,
+		0xC78, 0xF20E0001,
+		0xC78, 0xF10F0001,
+		0xC78, 0xF0100001,
+		0xC78, 0xEF110001,
+		0xC78, 0xEE120001,
+		0xC78, 0xED130001,
+		0xC78, 0xEC140001,
+		0xC78, 0xEB150001,
+		0xC78, 0xEA160001,
+		0xC78, 0xE9170001,
+		0xC78, 0xE8180001,
+		0xC78, 0xE7190001,
+		0xC78, 0xC81A0001,
+		0xC78, 0xC71B0001,
+		0xC78, 0xC61C0001,
+		0xC78, 0x071D0001,
+		0xC78, 0x061E0001,
+		0xC78, 0x051F0001,
+		0xC78, 0x04200001,
+		0xC78, 0x03210001,
+		0xC78, 0xAA220001,
+		0xC78, 0xA9230001,
+		0xC78, 0xA8240001,
+		0xC78, 0xA7250001,
+		0xC78, 0xA6260001,
+		0xC78, 0x85270001,
+		0xC78, 0x84280001,
+		0xC78, 0x83290001,
+		0xC78, 0x252A0001,
+		0xC78, 0x242B0001,
+		0xC78, 0x232C0001,
+		0xC78, 0x222D0001,
+		0xC78, 0x672E0001,
+		0xC78, 0x662F0001,
+		0xC78, 0x65300001,
+		0xC78, 0x64310001,
+		0xC78, 0x63320001,
+		0xC78, 0x62330001,
+		0xC78, 0x61340001,
+		0xC78, 0x45350001,
+		0xC78, 0x44360001,
+		0xC78, 0x43370001,
+		0xC78, 0x42380001,
+		0xC78, 0x41390001,
+		0xC78, 0x403A0001,
+		0xC78, 0x403B0001,
+		0xC78, 0x403C0001,
+		0xC78, 0x403D0001,
+		0xC78, 0x403E0001,
+		0xC78, 0x403F0001,
+	0xFF010718, 0xDEAD,
+	0xFF010718, 0xABCD,
+		0xC78, 0xFA400001,
+		0xC78, 0xF9410001,
+		0xC78, 0xF8420001,
+		0xC78, 0xF7430001,
+		0xC78, 0xF6440001,
+		0xC78, 0xF5450001,
+		0xC78, 0xF4460001,
+		0xC78, 0xF3470001,
+		0xC78, 0xF2480001,
+		0xC78, 0xF1490001,
+		0xC78, 0xF04A0001,
+		0xC78, 0xEF4B0001,
+		0xC78, 0xEE4C0001,
+		0xC78, 0xED4D0001,
+		0xC78, 0xEC4E0001,
+		0xC78, 0xEB4F0001,
+		0xC78, 0xEA500001,
+		0xC78, 0xE9510001,
+		0xC78, 0xE8520001,
+		0xC78, 0xE7530001,
+		0xC78, 0xE6540001,
+		0xC78, 0xE5550001,
+		0xC78, 0xE4560001,
+		0xC78, 0xE3570001,
+		0xC78, 0xE2580001,
+		0xC78, 0xE1590001,
+		0xC78, 0x8A5A0001,
+		0xC78, 0x895B0001,
+		0xC78, 0x885C0001,
+		0xC78, 0x875D0001,
+		0xC78, 0x865E0001,
+		0xC78, 0x855F0001,
+		0xC78, 0x84600001,
+		0xC78, 0x83610001,
+		0xC78, 0x82620001,
+		0xC78, 0x6A630001,
+		0xC78, 0x69640001,
+		0xC78, 0x68650001,
+		0xC78, 0x67660001,
+		0xC78, 0x66670001,
+		0xC78, 0x65680001,
+		0xC78, 0x64690001,
+		0xC78, 0x636A0001,
+		0xC78, 0x626B0001,
+		0xC78, 0x616C0001,
+		0xC78, 0x606D0001,
+		0xC78, 0x476E0001,
+		0xC78, 0x466F0001,
+		0xC78, 0x45700001,
+		0xC78, 0x44710001,
+		0xC78, 0x43720001,
+		0xC78, 0x42730001,
+		0xC78, 0x41740001,
+		0xC78, 0x40750001,
+		0xC78, 0x40760001,
+		0xC78, 0x40770001,
+		0xC78, 0x40780001,
+		0xC78, 0x40790001,
+		0xC78, 0x407A0001,
+		0xC78, 0x407B0001,
+		0xC78, 0x407C0001,
+		0xC78, 0x407D0001,
+		0xC78, 0x407E0001,
+		0xC78, 0x407F0001,
+		0xC50, 0x00040222,
+		0xC50, 0x00040220,
+	0xCDCDCDCD, 0xCDCD,
+		0xC78, 0xFB400001,
+		0xC78, 0xFB410001,
+		0xC78, 0xFB420001,
+		0xC78, 0xFB430001,
+		0xC78, 0xFB440001,
+		0xC78, 0xFB450001,
+		0xC78, 0xFA460001,
+		0xC78, 0xF9470001,
+		0xC78, 0xF8480001,
+		0xC78, 0xF7490001,
+		0xC78, 0xF64A0001,
+		0xC78, 0xF54B0001,
+		0xC78, 0xF44C0001,
+		0xC78, 0xF34D0001,
+		0xC78, 0xF24E0001,
+		0xC78, 0xF14F0001,
+		0xC78, 0xF0500001,
+		0xC78, 0xEF510001,
+		0xC78, 0xEE520001,
+		0xC78, 0xED530001,
+		0xC78, 0xEC540001,
+		0xC78, 0xEB550001,
+		0xC78, 0xEA560001,
+		0xC78, 0xE9570001,
+		0xC78, 0xE8580001,
+		0xC78, 0xE7590001,
+		0xC78, 0xE65A0001,
+		0xC78, 0xE55B0001,
+		0xC78, 0xE45C0001,
+		0xC78, 0xE35D0001,
+		0xC78, 0xE25E0001,
+		0xC78, 0xE15F0001,
+		0xC78, 0x8A600001,
+		0xC78, 0x89610001,
+		0xC78, 0x88620001,
+		0xC78, 0x87630001,
+		0xC78, 0x86640001,
+		0xC78, 0x85650001,
+		0xC78, 0x84660001,
+		0xC78, 0x83670001,
+		0xC78, 0x82680001,
+		0xC78, 0x6B690001,
+		0xC78, 0x6A6A0001,
+		0xC78, 0x696B0001,
+		0xC78, 0x686C0001,
+		0xC78, 0x676D0001,
+		0xC78, 0x666E0001,
+		0xC78, 0x656F0001,
+		0xC78, 0x64700001,
+		0xC78, 0x63710001,
+		0xC78, 0x62720001,
+		0xC78, 0x61730001,
+		0xC78, 0x49740001,
+		0xC78, 0x48750001,
+		0xC78, 0x47760001,
+		0xC78, 0x46770001,
+		0xC78, 0x45780001,
+		0xC78, 0x44790001,
+		0xC78, 0x437A0001,
+		0xC78, 0x427B0001,
+		0xC78, 0x417C0001,
+		0xC78, 0x407D0001,
+		0xC78, 0x407E0001,
+		0xC78, 0x407F0001,
+		0xC50, 0x00040022,
+		0xC50, 0x00040020,
+	0xFF010718, 0xDEAD,
+};
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/table.h b/drivers/staging/rtl8192ee/rtl8192ee/table.h
new file mode 100644
index 00000000000..e0f419823b3
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/table.h
@@ -0,0 +1,48 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ * Created on  2010/ 5/18,  1:41
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92E_TABLE__H_
+#define __RTL92E_TABLE__H_
+
+#include <linux/types.h>
+#define RTL8192EE_PHY_REG_ARRAY_LEN	448
+extern u32 RTL8192EE_PHY_REG_ARRAY[];
+#define RTL8192EE_PHY_REG_ARRAY_PG_LEN	168
+extern u32 RTL8192EE_PHY_REG_ARRAY_PG[];
+#define	RTL8192EE_RADIOA_ARRAY_LEN	238
+extern u32 RTL8192EE_RADIOA_ARRAY[];
+#define	RTL8192EE_RADIOB_ARRAY_LEN	198
+extern u32 RTL8192EE_RADIOB_ARRAY[];
+#define RTL8192EE_MAC_ARRAY_LEN		202
+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/staging/rtl8192ee/rtl8192ee/trx.c
new file mode 100644
index 00000000000..c930f52ec8c
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/trx.c
@@ -0,0 +1,1286 @@
+/******************************************************************************
+ *
+ * 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 "fw.h"
+
+static u8 _rtl92ee_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,
+ * 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 _rtl92ee_rate_mapping(struct ieee80211_hw *hw,
+				 bool isht, u8 desc_rate)
+{
+	int rate_idx;
+
+	if (!false) {
+		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 _rtl92ee_query_rxphystatus(struct ieee80211_hw *hw,
+				       struct rtl_stats *pstatus, u8 *pdesc,
+				       struct rx_fwinfo *p_drvinfo,
+				       bool bpacket_match_bssid,
+				       bool bpacket_toself,
+				       bool b_packet_beacon)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct phy_status_rpt *p_phystrpt = (struct phy_status_rpt *)p_drvinfo;
+	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_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,
+						 BIT(9));
+
+		lan_idx = ((cck_agc_rpt & 0xE0) >> 5);
+		vga_idx = (cck_agc_rpt & 0x1f);
+		switch (lan_idx) {
+		case 7: /*VGA_idx = 27~2*/
+				if (vga_idx <= 27)
+					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: /*VGA_idx = 5~0*/
+				if (cck_highpwr)
+					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 += 16;
+		pwdb_all = stg_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;
+		}
+
+		pstatus->rx_pwdb_all = pwdb_all;
+		pstatus->bt_rx_rssi_percentage = pwdb_all;
+		pstatus->recvsignalpower = rx_pwr_all;
+
+		/* (3) Get Signal Quality (EVM) */
+		if (bpacket_match_bssid) {
+			u8 sq, sq_rpt;
+
+			if (pstatus->rx_pwdb_all > 40) {
+				sq = 100;
+			} else {
+				sq_rpt = p_phystrpt->cck_sig_qual_ofdm_pwdb_all;
+				if (sq_rpt > 64)
+					sq = 0;
+				else if (sq_rpt < 20)
+					sq = 100;
+				else
+					sq = ((64 - sq_rpt) * 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.brfpath_rxenable[i])
+				rf_rx_num++;
+
+			rx_pwr[i] = ((p_phystrpt->path_agc[i].gain & 0x3f) * 2)
+				    - 110;
+
+			pstatus->rx_pwr[i] = rx_pwr[i];
+			/* Translate DBM to percentage. */
+			rssi = stg_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) */
+		rx_pwr_all = ((p_phystrpt->cck_sig_qual_ofdm_pwdb_all >> 1)
+			      & 0x7f) - 110;
+
+		pwdb_all = stg_rtl_query_rxpwrpercentage(rx_pwr_all);
+		pstatus->rx_pwdb_all = pwdb_all;
+		pstatus->bt_rx_rssi_percentage = pwdb_all;
+		pstatus->rxpower = rx_pwr_all;
+		pstatus->recvsignalpower = rx_pwr_all;
+
+		/* (3)EVM of HT rate */
+		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 = stg_rtl_evm_db_to_percentage(
+						p_phystrpt->stream_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);
+			}
+		}
+
+		if (bpacket_match_bssid) {
+			for (i = RF90_PATH_A; i <= RF90_PATH_B; i++)
+				rtl_priv(hw)->dm.cfo_tail[i] =
+					(int)p_phystrpt->path_cfotail[i];
+
+			if (rtl_priv(hw)->dm.packet_count == 0xffffffff)
+				rtl_priv(hw)->dm.packet_count = 0;
+			else
+				rtl_priv(hw)->dm.packet_count++;
+		}
+	}
+
+	/* 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,
+								     pwdb_all));
+	else if (rf_rx_num != 0)
+		pstatus->signalstrength = (u8)(stg_rtl_signal_scale_mapping(hw,
+						      total_rssi /= rf_rx_num));
+}
+
+static void _rtl92ee_translate_rx_signal_stuff(struct ieee80211_hw *hw,
+					       struct sk_buff *skb,
+					       struct rtl_stats *pstatus,
+					       u8 *pdesc,
+					       struct rx_fwinfo *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 cpu_fc, type;
+	bool b_packet_matchbssid, b_packet_toself, b_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) &&
+			       (ether_addr_equal(mac->bssid,
+						(cpu_fc & IEEE80211_FCTL_TODS) ?
+						hdr->addr1 :
+						(cpu_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++;
+
+	if (b_packet_matchbssid && ieee80211_is_data_qos(fc) &&
+	    !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);
+}
+
+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);
+	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 rtl92ee_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 *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_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->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))
+		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 + 24);
+
+	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 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 (!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 = _rtl92ee_rate_mapping(hw,
+						    status->b_is_ht,
+						    status->rate);
+
+	rx_status->mactime = status->timestamp_low;
+	if (phystatus) {
+		p_drvinfo = (struct rx_fwinfo *)(skb->data +
+						 status->rx_bufshift + 24);
+
+		_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);
+		status->macid_valid_entry[1] =
+			GET_RX_RPT2_DESC_MACID_VALID_2(pdesc);
+	}
+	return true;
+}
+
+/*in Windows, this == Rx_92EE_Interrupt*/
+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;
+	u16 read_cnt = 0;
+	if (header_desc == NULL)
+		return;
+
+	total_len = (u16)GET_RX_BUFFER_DESC_TOTAL_LENGTH(header_desc);
+
+	first_seg = (u8)GET_RX_BUFFER_DESC_FS(header_desc);
+
+	last_seg = (u8)GET_RX_BUFFER_DESC_LS(header_desc);
+
+	while (total_len == 0 && first_seg == 0 && last_seg == 0) {
+		read_cnt++;
+		total_len = (u16)GET_RX_BUFFER_DESC_TOTAL_LENGTH(header_desc);
+		first_seg = (u8)GET_RX_BUFFER_DESC_FS(header_desc);
+		last_seg = (u8)GET_RX_BUFFER_DESC_LS(header_desc);
+
+		if (read_cnt > 20)
+			break;
+	}
+}
+
+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;
+	u32 tmp_4byte = 0;
+	static u16 last_read_point;
+	static bool start_rx;
+
+	tmp_4byte = rtl_read_dword(rtlpriv, REG_RXQ_TXBD_IDX);
+	read_point = (u16)((tmp_4byte>>16) & 0x7ff);
+	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));
+		tmp_4byte = rtl_read_dword(rtlpriv, REG_RXQ_TXBD_IDX);
+		read_point = (u16)((tmp_4byte>>16) & 0x7ff);
+		write_point = (u16)(tmp_4byte & 0x7ff);
+	}
+
+	if (read_point > 0)
+		start_rx = true;
+	if (!start_rx)
+		return 0;
+
+	if ((last_read_point > (RX_DESC_NUM_92E / 2)) &&
+	    (read_point <= (RX_DESC_NUM_92E / 2))) {
+		remind_cnt = RX_DESC_NUM_92E - write_point;
+	} else {
+		remind_cnt = (read_point >= write_point) ?
+			     (read_point - write_point) :
+			     (RX_DESC_NUM_92E - write_point + read_point);
+	}
+
+	if (remind_cnt == 0)
+		return 0;
+
+	rtlpci->rx_ring[queue_index].next_rx_rp = write_point;
+
+	last_read_point = read_point;
+	return remind_cnt;
+}
+
+static u16 get_desc_addr_fr_q(u16 queue_index)
+{
+	u16 desc_address = REG_BEQ_TXBD_IDX;
+
+	switch (queue_index) {
+	case BK_QUEUE:
+			desc_address = REG_BKQ_TXBD_IDX;
+			break;
+	case BE_QUEUE:
+			desc_address = REG_BEQ_TXBD_IDX;
+			break;
+	case VI_QUEUE:
+			desc_address = REG_VIQ_TXBD_IDX;
+			break;
+	case VO_QUEUE:
+			desc_address = REG_VOQ_TXBD_IDX;
+			break;
+	case BEACON_QUEUE:
+			desc_address = REG_BEQ_TXBD_IDX;
+			break;
+	case TXCMD_QUEUE:
+			desc_address = REG_BEQ_TXBD_IDX;
+			break;
+	case MGNT_QUEUE:
+			desc_address = REG_MGQ_TXBD_IDX;
+			break;
+	case HIGH_QUEUE:
+			desc_address = REG_HI0Q_TXBD_IDX;
+			break;
+	case HCCA_QUEUE:
+			desc_address = REG_BEQ_TXBD_IDX;
+			break;
+	default:
+			break;
+	}
+	return desc_address;
+}
+
+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));
+	current_tx_read_point = (u16)((tmp_4byte >> 16) & 0x0fff);
+	current_tx_write_point = (u16)((tmp_4byte) & 0x0fff);
+
+	point_diff = ((current_tx_read_point > current_tx_write_point) ?
+		      (current_tx_read_point - current_tx_write_point) :
+		      (TX_DESC_NUM_92E - current_tx_write_point +
+		       current_tx_read_point));
+
+	rtlpci->tx_ring[q_idx].avl_desc = point_diff;
+}
+
+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)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u32 pkt_len = skb->len;
+	u16 desc_size = 40; /*tx desc size*/
+	u32 psblen = 0;
+	u16 tx_page_size = 0;
+	u32 total_packet_size = 0;
+	u16 current_bd_desc;
+	u8 i = 0;
+	u16 real_desc_size = 0x28;
+	u16	append_early_mode_size = 0;
+#if (RTL8192EE_SEG_NUM == 0)
+	u8 segmentnum = 2;
+#elif (RTL8192EE_SEG_NUM == 1)
+	u8 segmentnum = 4;
+#elif (RTL8192EE_SEG_NUM == 2)
+	u8 segmentnum = 8;
+#endif
+
+	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 (queue_index < BEACON_QUEUE) {
+			append_early_mode_size = 8;
+			total_packet_size += append_early_mode_size;
+		}
+	}
+
+	if (tx_page_size > 0) {
+		psblen = (pkt_len + real_desc_size + append_early_mode_size) /
+			 (tx_page_size * 128);
+
+		if (psblen * (tx_page_size * 128) < total_packet_size)
+			psblen += 1;
+	}
+
+	/* 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);
+
+	for (i = 1; i < segmentnum; i++) {
+		SET_TXBUFFER_DESC_LEN_WITH_OFFSET(tx_bd_desc, i, 0);
+		SET_TXBUFFER_DESC_AMSDU_WITH_OFFSET(tx_bd_desc, i, 0);
+		SET_TXBUFFER_DESC_ADD_LOW_WITH_OFFSET(tx_bd_desc, i, 0);
+#if (DMA_IS_64BIT == 1)
+		SET_TXBUFFER_DESC_ADD_HIGT_WITH_OFFSET(tx_bd_desc, i, 0);
+#endif
+	}
+	SET_TX_BUFF_DESC_LEN_1(tx_bd_desc, 0);
+	SET_TX_BUFF_DESC_AMSDU_1(tx_bd_desc, 0);
+
+	SET_TX_BUFF_DESC_LEN_2(tx_bd_desc, 0);
+	SET_TX_BUFF_DESC_AMSDU_2(tx_bd_desc, 0);
+	SET_TX_BUFF_DESC_LEN_3(tx_bd_desc, 0);
+	SET_TX_BUFF_DESC_AMSDU_3(tx_bd_desc, 0);
+	/* Clear all status */
+	CLEAR_PCI_TX_DESC_CONTENT(desc, TX_DESC_SIZE);
+
+	if (rtlpriv->rtlhal.b_earlymode_enable) {
+		if (queue_index < BEACON_QUEUE) {
+			/* These macros need 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);
+		}
+	} else {
+		SET_TX_BUFF_DESC_LEN_0(tx_bd_desc , desc_size);
+	}
+	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));
+
+	SET_TXBUFFER_DESC_LEN_WITH_OFFSET(tx_bd_desc, 1, pkt_len);
+	/* don't using extendsion mode. */
+	SET_TXBUFFER_DESC_AMSDU_WITH_OFFSET(tx_bd_desc, 1, 0);
+	SET_TXBUFFER_DESC_ADD_LOW_WITH_OFFSET(tx_bd_desc, 1, addr);
+
+	SET_TX_DESC_PKT_SIZE(desc, (u16)(pkt_len));
+	SET_TX_DESC_TX_BUFFER_SIZE(desc, (u16)(pkt_len));
+}
+
+void rtl92ee_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 *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;
+	u8 fw_qsel = _rtl92ee_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;
+	stg_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;
+	}
+	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;
+	}
+	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));
+				_rtl92ee_insert_emcontent(ptcb_desc,
+							  (u8 *)(skb->data));
+			}
+		} else {
+			SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN);
+		}
+
+		SET_TX_DESC_TX_RATE(pdesc, ptcb_desc->hw_rate);
+
+		if (ieee80211_is_mgmt(fc)) {
+			ptcb_desc->use_driver_rate = true;
+		} else {
+			if (rtlpriv->ra.is_special_data) {
+				ptcb_desc->use_driver_rate = true;
+				SET_TX_DESC_TX_RATE(pdesc, DESC92C_RATE11M);
+			} else {
+				ptcb_desc->use_driver_rate = false;
+			}
+		}
+
+		if (ptcb_desc->hw_rate > DESC92C_RATEMCS0)
+			short_gi = (ptcb_desc->use_shortgi) ? 1 : 0;
+		else
+			short_gi = (ptcb_desc->use_shortpreamble) ? 1 : 0;
+
+		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_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)));
+
+		if (ptcb_desc->btx_enable_sw_calc_duration)
+			SET_TX_DESC_NAV_USE_HDR(pdesc, 1);
+
+		if (bw_40) {
+			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);
+			}
+		} else {
+			SET_TX_DESC_DATA_BW(pdesc, 0);
+			SET_TX_DESC_TX_SUB_CARRIER(pdesc, 0);
+		}
+
+		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:
+			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);
+
+		/*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_ADDRESS(pdesc, mapping);
+	if (rtlpriv->dm.b_useramask) {
+		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->ratr_index);
+	}
+
+	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);
+	}
+	RT_TRACE(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)
+{
+	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"));
+		return;
+	}
+	CLEAR_PCI_TX_DESC_CONTENT(pdesc, txdesc_len);
+
+	if (b_firstseg)
+		SET_TX_DESC_OFFSET(pdesc, txdesc_len);
+
+	SET_TX_DESC_TX_RATE(pdesc, DESC92C_RATE1M);
+
+	SET_TX_DESC_SEQ(pdesc, 0);
+
+	SET_TX_DESC_LINIP(pdesc, 0);
+
+	SET_TX_DESC_QUEUE_SEL(pdesc, fw_queue);
+
+	SET_TX_DESC_FIRST_SEG(pdesc, 1);
+	SET_TX_DESC_LAST_SEG(pdesc, 1);
+
+	SET_TX_DESC_TX_BUFFER_SIZE(pdesc, (u16)(skb->len));
+
+	SET_TX_DESC_TX_BUFFER_ADDRESS(pdesc, mapping);
+
+	SET_TX_DESC_RATE_ID(pdesc, 7);
+	SET_TX_DESC_MACID(pdesc, 0);
+
+	SET_TX_DESC_OWN(pdesc, 1);
+
+	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_OFFSET(pdesc, 40);
+
+	SET_TX_DESC_USE_RATE(pdesc, 1);
+
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
+		      "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)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u16 cur_tx_rp = 0;
+	u16 cur_tx_wp = 0;
+	static u16 last_txw_point;
+	static bool over_run;
+	u32 tmp = 0;
+	u8 q_idx = *val;
+
+	if (istx) {
+		switch (desc_name) {
+		case HW_DESC_TX_NEXTDESC_ADDR:
+			SET_TX_DESC_NEXT_DESC_ADDRESS(pdesc, *(u32 *)val);
+			break;
+		case HW_DESC_OWN:{
+			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) {
+				ring->cur_tx_wp = 0;
+				over_run = false;
+			}
+			if (ring->avl_desc > 1) {
+				ring->avl_desc--;
+
+				rtl_write_word(rtlpriv,
+					       get_desc_addr_fr_q(q_idx),
+				       ring->cur_tx_wp);
+
+				if (q_idx == 1)
+					last_txw_point = cur_tx_wp;
+			}
+
+			if (ring->avl_desc < (max_tx_desc - 15)) {
+				u16 point_diff = 0;
+				tmp = rtl_read_dword(rtlpriv,
+						     get_desc_addr_fr_q(q_idx));
+				cur_tx_rp = (u16)((tmp >> 16) & 0x0fff);
+				cur_tx_wp = (u16)(tmp & 0x0fff);
+
+				ring->cur_tx_wp = cur_tx_wp;
+				ring->cur_tx_rp = cur_tx_rp;
+				point_diff = ((cur_tx_rp > cur_tx_wp) ?
+					      (cur_tx_rp - cur_tx_wp) :
+					      (TX_DESC_NUM_92E - 1 -
+					       cur_tx_wp + cur_tx_rp));
+
+				ring->avl_desc = point_diff;
+			}
+		}
+		break;
+		}
+	} 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_DATA_LENGTH(pdesc,
+						       MAX_RECEIVE_BUFFER_SIZE +
+						       RX_DESC_SIZE);
+
+			SET_RX_BUFFER_PHYSICAL_LOW(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 rtl92ee_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_TXBUFFER_DESC_ADDR_LOW(pdesc, 1);
+			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 rtl92ee_is_tx_desc_closed(struct ieee80211_hw *hw, u8 hw_queue, u16 index)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u16 read_point, write_point, available_desc_num;
+	bool ret = false;
+	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) {
+		u16 point_diff = 0;
+		u16 cur_tx_rp, cur_tx_wp;
+		u32 tmpu32;
+
+		tmpu32 = rtl_read_dword(rtlpriv, get_desc_addr_fr_q(hw_queue));
+		cur_tx_rp = (u16)((tmpu32 >> 16) & 0x0fff);
+		cur_tx_wp = (u16)(tmpu32 & 0x0fff);
+
+		ring->cur_tx_wp = cur_tx_wp;
+		ring->cur_tx_rp = cur_tx_rp;
+		point_diff = ((cur_tx_rp > cur_tx_wp) ?
+			      (cur_tx_rp - cur_tx_wp) :
+			      (TX_DESC_NUM_92E - cur_tx_wp + cur_tx_rp));
+
+		ring->avl_desc = point_diff;
+	}
+
+	read_point = ring->cur_tx_rp;
+	write_point = ring->cur_tx_wp;
+	available_desc_num = ring->avl_desc;
+
+	if (write_point > read_point) {
+		if (index < write_point && index >= read_point)
+			ret = false;
+		else
+			ret = true;
+	} else if (write_point < read_point) {
+		if (index > write_point && index < read_point)
+			ret = true;
+		else
+			ret = false;
+	} else {
+		if (index != read_point)
+			ret = true;
+	}
+
+	if (hw_queue == BEACON_QUEUE)
+		ret = true;
+
+	if (rtlpriv->rtlhal.driver_is_goingto_unload ||
+	    rtlpriv->psc.rfoff_reason > RF_CHANGE_BY_PS)
+		ret = true;
+
+	if (hw_queue < BEACON_QUEUE) {
+		if (!ret)
+			stop_report_cnt++;
+		else
+			stop_report_cnt = 0;
+	}
+
+	return ret;
+}
+
+u32 rtl92ee_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:
+		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"));
+		break;
+	}
+
+	return result;
+}
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/trx.h b/drivers/staging/rtl8192ee/rtl8192ee/trx.h
new file mode 100644
index 00000000000..c2cd5813a2b
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/trx.h
@@ -0,0 +1,877 @@
+/******************************************************************************
+ *
+ * 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 __RTL92E_TRX_H__
+#define __RTL92E_TRX_H__
+
+
+
+#if (DMA_IS_64BIT == 1)
+#if (RTL8192EE_SEG_NUM == 2)
+#define TX_BD_DESC_SIZE					128
+#elif (RTL8192EE_SEG_NUM == 1)
+#define TX_BD_DESC_SIZE					64
+#elif (RTL8192EE_SEG_NUM == 0)
+#define TX_BD_DESC_SIZE					32
+#endif
+#else
+#if (RTL8192EE_SEG_NUM == 2)
+#define TX_BD_DESC_SIZE					64
+#elif (RTL8192EE_SEG_NUM == 1)
+#define TX_BD_DESC_SIZE					32
+#elif (RTL8192EE_SEG_NUM == 0)
+#define TX_BD_DESC_SIZE					16
+#endif
+#endif
+
+#define TX_DESC_SIZE					64
+
+#define RX_DRV_INFO_SIZE_UNIT				8
+
+#define	TX_DESC_NEXT_DESC_OFFSET			40
+#define USB_HWDESC_HEADER_LEN				40
+
+#define RX_DESC_SIZE					24
+#define MAX_RECEIVE_BUFFER_SIZE				8192
+
+#define SET_TX_DESC_PKT_SIZE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 0, 16, __val)
+#define SET_TX_DESC_OFFSET(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 16, 8, __val)
+#define SET_TX_DESC_BMC(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 24, 1, __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)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 28, 1, __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)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 30, 1, __val)
+#define SET_TX_DESC_OWN(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 31, 1, __val)
+
+#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)
+#define GET_TX_DESC_BMC(__pdesc)			\
+	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)			\
+	LE_BITS_TO_4BYTE(__pdesc, 26, 1)
+#define GET_TX_DESC_FIRST_SEG(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 27, 1)
+#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)
+#define GET_TX_DESC_GF(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc, 30, 1)
+#define GET_TX_DESC_OWN(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 31, 1)
+
+#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)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 8, 5, __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)		\
+	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, 5, __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)		\
+	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, 24, 5, __val)
+#define SET_TX_DESC_MORE_DATA(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 29, 1, __val)
+#define SET_TX_DESC_TXOP_PS_CAP(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 30, 1, __val)
+#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)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 5, 1)
+#define GET_TX_DESC_AGG_BREAK(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 6, 1)
+#define GET_TX_DESC_RDG_ENABLE(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 7, 1)
+#define GET_TX_DESC_QUEUE_SEL(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 8, 5)
+#define GET_TX_DESC_RDG_NAV_EXT(__pdesc)		\
+	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)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 15, 1)
+#define GET_TX_DESC_RATE_ID(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 16, 4)
+#define GET_TX_DESC_NAV_USE_HDR(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+4, 20, 1)
+#define GET_TX_DESC_EN_DESC_ID(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 21, 1)
+#define GET_TX_DESC_SEC_TYPE(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 22, 2)
+#define GET_TX_DESC_PKT_OFFSET(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 24, 5)
+
+#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)		\
+	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_NULL_0(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 14, 1, __val)
+#define SET_TX_DESC_NULL_1(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 15, 1, __val)
+#define SET_TX_DESC_BK(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 16, 1, __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)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 18, 1, __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)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 20, 3, __val)
+#define SET_TX_DESC_BT_NULL(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 23, 1, __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)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 0, 4, __val)
+#define SET_TX_DESC_CHK_EN(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 4, 1, __val)
+#define SET_TX_DESC_EARLY_RATE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 5, 1, __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)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 8, 1, __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)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 10, 1, __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)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 12, 1, __val)
+#define SET_TX_DESC_HW_RTS_ENABLE(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 13, 1, __val)
+#define SET_TX_DESC_HW_PORT_ID(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 14, 1, __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)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 16, 1, __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)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 22, 2, __val)
+#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)
+#define SET_TX_DESC_TRY_RATE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 7, 1, __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)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 13, 4, __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)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 18, 6, __val)
+#define SET_TX_DESC_RTS_RATE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 24, 5, __val)
+#define SET_TX_DESC_PCTS_ENABLE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 29, 1, __val)
+#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)
+#define SET_TX_DESC_DATA_SHORT(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__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)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 7, 1, __val)
+#define SET_TX_DESC_DATA_STBC(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 8, 2, __val)
+#define SET_TX_DESC_VCS_STBC(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 10, 2, __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)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 13, 4, __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)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 28, 3, __val)
+
+/* Dword 6 */
+#define SET_TX_DESC_SW_DEFINE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 0, 12, __val)
+#define SET_TX_DESC_ANTSEL_A(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 16, 3, __val)
+#define SET_TX_DESC_ANTSEL_B(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 19, 3, __val)
+#define SET_TX_DESC_ANTSEL_C(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 22, 3, __val)
+#define SET_TX_DESC_ANTSEL_D(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 25, 3, __val)
+
+/* Dword 7 */
+#define SET_TX_DESC_TX_BUFFER_SIZE(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+28, 0, 16, __val)
+#define SET_TX_DESC_USB_TXAGG_NUM(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+28, 24, 8, __val)
+
+/* Dword 8 */
+#define SET_TX_DESC_RTS_RC(__pdesc, __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)
+#define SET_TX_DESC_DATA_RC(__pdesc, __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)
+
+/* Dword 9 */
+#define SET_TX_DESC_PADDING_LENGTH(__pdesc, __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)
+#define SET_TX_DESC_SEQ(__pdesc, __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)
+
+/* Dword 10 */
+#define SET_TX_DESC_TX_BUFFER_ADDRESS(__pdesc, __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)
+
+
+/* TX/RX buffer descriptor */
+
+#define SET_TX_EXTBUFF_DESC_LEN(__pdesc, __val, __set)	\
+	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)
+
+
+
+/* for Txfilldescroptor92ee, fill the desc content. */
+#if (DMA_IS_64BIT == 1)
+#define SET_TXBUFFER_DESC_LEN_WITH_OFFSET(__pdesc, __offset, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+(__offset*16), 0, 16, __val)
+#define SET_TXBUFFER_DESC_AMSDU_WITH_OFFSET(__pdesc, __offset, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+(__offset*16), 31, 1, __val)
+#define SET_TXBUFFER_DESC_ADD_LOW_WITH_OFFSET(__pdesc, __offset, __val) \
+	SET_BITS_TO_LE_4BYTE(__pdesc+(__offset*16)+4, 0, 32, __val)
+#define SET_TXBUFFER_DESC_ADD_HIGT_WITH_OFFSET(__pdesc, __offset, __val)\
+	SET_BITS_TO_LE_4BYTE(__pdesc+(__offset*16)+8, 0, 32, __val)
+#define GET_TXBUFFER_DESC_ADDR_LOW(__pdesc, __offset)			\
+	LE_BITS_TO_4BYTE(__pdesc+(__offset*16)+4, 0, 32)
+#else
+#define SET_TXBUFFER_DESC_LEN_WITH_OFFSET(__pdesc, __offset, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+(__offset*8), 0, 16, __val)
+#define SET_TXBUFFER_DESC_AMSDU_WITH_OFFSET(__pdesc, __offset, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+(__offset*8), 31, 1, __val)
+#define SET_TXBUFFER_DESC_ADD_LOW_WITH_OFFSET(__pdesc, __offset, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+(__offset*8)+4, 0, 32, __val)
+#define SET_TXBUFFER_DESC_ADD_HIGT_WITH_OFFSET(__pdesc, __offset, __val)
+#define GET_TXBUFFER_DESC_ADDR_LOW(__pdesc, __offset)			\
+	LE_BITS_TO_4BYTE(__pdesc+(__offset*8)+4, 0, 32)
+#endif
+
+/* Dword 0 */
+#define SET_TX_BUFF_DESC_LEN_0(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 0, 14, __val)
+#define SET_TX_BUFF_DESC_PSB(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 16, 15, __val)
+#define SET_TX_BUFF_DESC_OWN(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 31, 1, __val)
+
+/* Dword 1 */
+#define SET_TX_BUFF_DESC_ADDR_LOW_0(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 0, 32, __val)
+#if (DMA_IS_64BIT == 1)
+/* Dword 2 */
+#define SET_TX_BUFF_DESC_ADDR_HIGH_0(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 0, 32, __val)
+/* Dword 3 / RESERVED 0 */
+/* Dword 4 */
+#define SET_TX_BUFF_DESC_LEN_1(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 0, 16, __val)
+#define SET_TX_BUFF_DESC_AMSDU_1(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 31, 1, __val)
+/* Dword 5 */
+#define SET_TX_BUFF_DESC_ADDR_LOW_1(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 0, 32, __val)
+/* Dword 6 */
+#define SET_TX_BUFF_DESC_ADDR_HIGH_1(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 0, 32, __val)
+/* Dword 7 / RESERVED 0 */
+/* Dword 8 */
+#define SET_TX_BUFF_DESC_LEN_2(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+32, 0, 16, __val)
+#define SET_TX_BUFF_DESC_AMSDU_2(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+32, 31, 1, __val)
+/* Dword 9 */
+#define SET_TX_BUFF_DESC_ADDR_LOW_2(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+36, 0, 32, __val)
+/* Dword 10 */
+#define SET_TX_BUFF_DESC_ADDR_HIGH_2(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+40, 0, 32, __val)
+/* Dword 11 / RESERVED 0 */
+/* Dword 12 */
+#define SET_TX_BUFF_DESC_LEN_3(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+48, 0, 16, __val)
+#define SET_TX_BUFF_DESC_AMSDU_3(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+48, 31, 1, __val)
+/* Dword 13 */
+#define SET_TX_BUFF_DESC_ADDR_LOW_3(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+52, 0, 32, __val)
+/* Dword 14 */
+#define SET_TX_BUFF_DESC_ADDR_HIGH_3(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+56, 0, 32, __val)
+/* Dword 15 / RESERVED 0 */
+#else
+#define SET_TX_BUFF_DESC_ADDR_HIGH_0(__pdesc, __val)
+/* Dword 2 */
+#define SET_TX_BUFF_DESC_LEN_1(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 0, 16, __val)
+#define SET_TX_BUFF_DESC_AMSDU_1(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 31, 1, __val)
+/* Dword 3 */
+#define SET_TX_BUFF_DESC_ADDR_LOW_1(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 0, 32, __val)
+#define SET_TX_BUFF_DESC_ADDR_HIGH_1(__pdesc, __val)
+/* Dword 4 */
+#define SET_TX_BUFF_DESC_LEN_2(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 0, 16, __val)
+#define SET_TX_BUFF_DESC_AMSDU_2(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 31, 1, __val)
+/* Dword 5 */
+#define SET_TX_BUFF_DESC_ADDR_LOW_2(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 0, 32, __val)
+#define SET_TX_BUFF_DESC_ADDR_HIGH_2(__pdesc, __val)
+/* Dword 6 */
+#define SET_TX_BUFF_DESC_LEN_3(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 0, 16, __val)
+#define SET_TX_BUFF_DESC_AMSDU_3(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 31, 1, __val)
+/* Dword 7 */
+#define SET_TX_BUFF_DESC_ADDR_LOW_3(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+28, 0, 32, __val)
+#define SET_TX_BUFF_DESC_ADDR_HIGH_3(__pdesc, __val)
+#endif
+
+/* 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)
+
+
+/* DWORD 1 */
+#define SET_RX_BUFFER_PHYSICAL_LOW(__pRxStatusDesc , __val)	\
+	SET_BITS_TO_LE_4BYTE(__pRxStatusDesc+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 GET_RX_DESC_PKT_LEN(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 0, 14)
+#define GET_RX_DESC_CRC32(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 14, 1)
+#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)			\
+	LE_BITS_TO_4BYTE(__pdesc, 23, 1)
+#define GET_RX_DESC_SHIFT(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 24, 2)
+#define GET_RX_DESC_PHYST(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 26, 1)
+#define GET_RX_DESC_SWDEC(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 27, 1)
+#define GET_RX_DESC_LS(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc, 28, 1)
+#define GET_RX_DESC_FS(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc, 29, 1)
+#define GET_RX_DESC_EOR(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 30, 1)
+#define GET_RX_DESC_OWN(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 31, 1)
+
+#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)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 0, 7)
+#define GET_RX_DESC_TID(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 8, 4)
+#define GET_RX_DESC_MACID_VLD(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 12, 1)
+#define GET_RX_DESC_AMSDU(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 13, 1)
+#define GET_RX_DESC_RXID_MATCH(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 14, 1)
+#define GET_RX_DESC_PAGGR(__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_TCPOFFLOAD_CHKERR(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+4, 20, 1)
+#define GET_RX_DESC_TCPOFFLOAD_IPVER(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+4, 21, 1)
+#define GET_RX_DESC_TCPOFFLOAD_IS_TCPUDP(__pdesc)	\
+	LE_BITS_TO_4BYTE(__pdesc+4, 22, 1)
+#define GET_RX_DESC_TCPOFFLOAD_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)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 25, 1)
+#define GET_RX_DESC_MD(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 26, 1)
+#define GET_RX_DESC_MF(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 27, 1)
+#define GET_RX_DESC_TYPE(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 28, 2)
+#define GET_RX_DESC_MC(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 30, 1)
+#define GET_RX_DESC_BC(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 31, 1)
+#define GET_RX_DESC_SEQ(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+8, 0, 12)
+#define GET_RX_DESC_FRAG(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+8, 12, 4)
+#define GET_RX_DESC_RX_IS_QOS(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+8, 16, 1)
+
+#define GET_RX_DESC_RXMCS(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+12, 0, 7)
+#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)
+#define GET_RX_STATUS_DESC_BSSID_FIT(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+12, 12, 2)
+#define GET_RX_STATUS_DESC_DMA_AGG_NUM(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+12, 16, 8)
+#define GET_RX_STATUS_DESC_PATTERN_MATCH(__pdesc)	\
+	LE_BITS_TO_4BYTE(__pdesc+12, 29, 1)
+#define GET_RX_STATUS_DESC_UNICAST_MATCH(__pdesc)	\
+	LE_BITS_TO_4BYTE(__pdesc+12, 30, 1)
+#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)
+
+#define GET_RX_DESC_BUFF_ADDR(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+24, 0, 32)
+#define GET_RX_DESC_BUFF_ADDR64(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+28, 0, 32)
+
+#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 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);			\
+} while (0)
+
+#define RX_HAL_IS_CCK_RATE(rxmcs)\
+	(rxmcs == DESC92C_RATE1M ||\
+	 rxmcs == DESC92C_RATE2M ||\
+	 rxmcs == DESC92C_RATE5_5M ||\
+	 rxmcs == DESC92C_RATE11M)
+
+#define IS_LITTLE_ENDIAN	1
+
+struct phy_rx_agc_info_t {
+	#if IS_LITTLE_ENDIAN
+		u8 gain:7 , trsw:1;
+	#else
+		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;
+	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;*/
+#endif
+} __packed;
+
+struct rx_fwinfo {
+	u8 gain_trsw[4];
+	u8 pwdb_all;
+	u8 cfosho[4];
+	u8 cfotail[4];
+	char rxevm[2];
+	char rxsnr[4];
+	u8 pdsnr[2];
+	u8 csi_current[2];
+	u8 csi_target[2];
+	u8 sigevm;
+	u8 max_ex_pwr;
+	u8 ex_intf_flag:1;
+	u8 sgi_en:1;
+	u8 rxsc:2;
+	u8 reserve:4;
+} __packed;
+
+struct tx_desc {
+	u32 pktsize:16;
+	u32 offset:8;
+	u32 bmc:1;
+	u32 htc:1;
+	u32 lastseg:1;
+	u32 firstseg:1;
+	u32 linip:1;
+	u32 noacm:1;
+	u32 gf:1;
+	u32 own:1;
+
+	u32 macid:6;
+	u32 rsvd0:2;
+	u32 queuesel:5;
+	u32 rd_nav_ext:1;
+	u32 lsig_txop_en:1;
+	u32 pifs:1;
+	u32 rateid:4;
+	u32 nav_usehdr:1;
+	u32 en_descid:1;
+	u32 sectype:2;
+	u32 pktoffset:8;
+
+	u32 rts_rc:6;
+	u32 data_rc:6;
+	u32 agg_en:1;
+	u32 rdg_en:1;
+	u32 bar_retryht:2;
+	u32 agg_break:1;
+	u32 morefrag:1;
+	u32 raw:1;
+	u32 ccx:1;
+	u32 ampdudensity:3;
+	u32 bt_int:1;
+	u32 ant_sela:1;
+	u32 ant_selb:1;
+	u32 txant_cck:2;
+	u32 txant_l:2;
+	u32 txant_ht:2;
+
+	u32 nextheadpage:8;
+	u32 tailpage:8;
+	u32 seq:12;
+	u32 cpu_handle:1;
+	u32 tag1:1;
+	u32 trigger_int:1;
+	u32 hwseq_en:1;
+
+	u32 rtsrate:5;
+	u32 apdcfe:1;
+	u32 qos:1;
+	u32 hwseq_ssn:1;
+	u32 userrate:1;
+	u32 dis_rtsfb:1;
+	u32 dis_datafb:1;
+	u32 cts2self:1;
+	u32 rts_en:1;
+	u32 hwrts_en:1;
+	u32 portid:1;
+	u32 pwr_status:3;
+	u32 waitdcts:1;
+	u32 cts2ap_en:1;
+	u32 txsc:2;
+	u32 stbc:2;
+	u32 txshort:1;
+	u32 txbw:1;
+	u32 rtsshort:1;
+	u32 rtsbw:1;
+	u32 rtssc:2;
+	u32 rtsstbc:2;
+
+	u32 txrate:6;
+	u32 shortgi:1;
+	u32 ccxt:1;
+	u32 txrate_fb_lmt:5;
+	u32 rtsrate_fb_lmt:4;
+	u32 retrylmt_en:1;
+	u32 txretrylmt:6;
+	u32 usb_txaggnum:8;
+
+	u32 txagca:5;
+	u32 txagcb:5;
+	u32 usemaxlen:1;
+	u32 maxaggnum:5;
+	u32 mcsg1maxlen:4;
+	u32 mcsg2maxlen:4;
+	u32 mcsg3maxlen:4;
+	u32 mcs7sgimaxlen:4;
+
+	u32 txbuffersize:16;
+	u32 sw_offset30:8;
+	u32 sw_offset31:4;
+	u32 rsvd1:1;
+	u32 antsel_c:1;
+	u32 null_0:1;
+	u32 null_1:1;
+
+	u32 txbuffaddr;
+	u32 txbufferaddr64;
+	u32 nextdescaddress;
+	u32 nextdescaddress64;
+
+	u32 reserve_pass_pcie_mm_limit[4];
+} __packed;
+
+struct rx_desc {
+	u32 length:14;
+	u32 crc32:1;
+	u32 icverror:1;
+	u32 drv_infosize:4;
+	u32 security:3;
+	u32 qos:1;
+	u32 shift:2;
+	u32 phystatus:1;
+	u32 swdec:1;
+	u32 lastseg:1;
+	u32 firstseg:1;
+	u32 eor:1;
+	u32 own:1;
+
+	u32 macid:6;
+	u32 tid:4;
+	u32 hwrsvd:5;
+	u32 paggr:1;
+	u32 faggr:1;
+	u32 a1_fit:4;
+	u32 a2_fit:4;
+	u32 pam:1;
+	u32 pwr:1;
+	u32 moredata:1;
+	u32 morefrag:1;
+	u32 type:2;
+	u32 mc:1;
+	u32 bc:1;
+
+	u32 seq:12;
+	u32 frag:4;
+	u32 nextpktlen:14;
+	u32 nextind:1;
+	u32 rsvd:1;
+
+	u32 rxmcs:6;
+	u32 rxht:1;
+	u32 amsdu:1;
+	u32 splcp:1;
+	u32 bandwidth:1;
+	u32 htc:1;
+	u32 tcpchk_rpt:1;
+	u32 ipcchk_rpt:1;
+	u32 tcpchk_valid:1;
+	u32 hwpcerr:1;
+	u32 hwpcind:1;
+	u32 iv0:16;
+
+	u32 iv1;
+
+	u32 tsfl;
+
+	u32 bufferaddress;
+	u32 bufferaddress64;
+
+} __packed;
+
+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_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,
+			  struct ieee80211_tx_info *info,
+			  struct ieee80211_sta *sta,
+			  struct sk_buff *skb,
+			  u8 hw_queue, struct rtl_tcb_desc *ptcb_desc);
+bool rtl92ee_rx_query_desc(struct ieee80211_hw *hw,
+			   struct rtl_stats *status,
+			   struct ieee80211_rx_status *rx_status,
+			   u8 *pdesc, struct sk_buff *skb);
+void rtl92ee_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
+		      u8 desc_name, u8 *val);
+
+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_fill_cmddesc(struct ieee80211_hw *hw, u8 *pdesc,
+			     bool b_firstseg, bool b_lastseg,
+			     struct sk_buff *skb);
+u32 rtl92ee_rx_command_packet(struct ieee80211_hw *hw,
+			      struct rtl_stats status,
+			      struct sk_buff *skb);
+#endif
diff --git a/drivers/staging/rtl8192ee/stats.c b/drivers/staging/rtl8192ee/stats.c
new file mode 100644
index 00000000000..7ac302b1a71
--- /dev/null
+++ b/drivers/staging/rtl8192ee/stats.c
@@ -0,0 +1,290 @@
+/******************************************************************************
+ *
+ * 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
new file mode 100644
index 00000000000..07284278530
--- /dev/null
+++ b/drivers/staging/rtl8192ee/stats.h
@@ -0,0 +1,43 @@
+/******************************************************************************
+ *
+ * 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
new file mode 100644
index 00000000000..96fa261a70a
--- /dev/null
+++ b/drivers/staging/rtl8192ee/wifi.h
@@ -0,0 +1,2644 @@
+/******************************************************************************
+ *
+ * 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,
+				 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/ieee80211/ieee80211.h b/drivers/staging/rtl8192u/ieee80211/ieee80211.h
index cac2056b7f8..e0aa069fe9b 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211.h
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211.h
@@ -354,12 +354,10 @@ enum	_ReasonCode{
 #define ieee80211_wx_get_scan		ieee80211_wx_get_scan_rsl
 #define ieee80211_wx_set_encode		ieee80211_wx_set_encode_rsl
 #define ieee80211_wx_get_encode		ieee80211_wx_get_encode_rsl
-#if WIRELESS_EXT >= 18
 #define ieee80211_wx_set_mlme		ieee80211_wx_set_mlme_rsl
 #define ieee80211_wx_set_auth		ieee80211_wx_set_auth_rsl
 #define ieee80211_wx_set_encode_ext	ieee80211_wx_set_encode_ext_rsl
 #define ieee80211_wx_get_encode_ext	ieee80211_wx_get_encode_ext_rsl
-#endif
 
 
 typedef struct ieee_param {
@@ -392,16 +390,6 @@ typedef struct ieee_param {
 }ieee_param;
 
 
-#if WIRELESS_EXT < 17
-#define IW_QUAL_QUAL_INVALID   0x10
-#define IW_QUAL_LEVEL_INVALID  0x20
-#define IW_QUAL_NOISE_INVALID  0x40
-#define IW_QUAL_QUAL_UPDATED   0x1
-#define IW_QUAL_LEVEL_UPDATED  0x2
-#define IW_QUAL_NOISE_UPDATED  0x4
-#endif
-
-
 // linux under 2.6.9 release may not support it, so modify it for common use
 #define MSECS(t) msecs_to_jiffies(t)
 #define msleep_interruptible_rsl  msleep_interruptible
@@ -539,7 +527,7 @@ do { if (ieee80211_debug_level & (level)) \
 		{	\
 			int i;					\
 			u8 *pdata = (u8 *) data;			\
-			printk(KERN_DEBUG "ieee80211: %s()\n", __FUNCTION__);	\
+			printk(KERN_DEBUG "ieee80211: %s()\n", __func__);	\
 			for(i=0; i<(int)(datalen); i++)			\
 			{						\
 				printk("%2x ", pdata[i]);		\
@@ -2398,7 +2386,6 @@ extern int ieee80211_wx_set_encode(struct ieee80211_device *ieee,
 extern int ieee80211_wx_get_encode(struct ieee80211_device *ieee,
 				   struct iw_request_info *info,
 				   union iwreq_data *wrqu, char *key);
-#if WIRELESS_EXT >= 18
 extern int ieee80211_wx_get_encode_ext(struct ieee80211_device *ieee,
 			    struct iw_request_info *info,
 			    union iwreq_data *wrqu, char *extra);
@@ -2411,7 +2398,6 @@ extern int ieee80211_wx_set_auth(struct ieee80211_device *ieee,
 extern int ieee80211_wx_set_mlme(struct ieee80211_device *ieee,
 			       struct iw_request_info *info,
 			       union iwreq_data *wrqu, char *extra);
-#endif
 extern int ieee80211_wx_set_gen_ie(struct ieee80211_device *ieee, u8 *ie, size_t len);
 
 /* ieee80211_softmac.c */
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_ccmp.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_ccmp.c
index f2b16775a63..af2128b956b 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_ccmp.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_ccmp.c
@@ -444,7 +444,7 @@ static char *ieee80211_ccmp_print_stats(char *p, void *priv)
 
 void ieee80211_ccmp_null(void)
 {
-//    printk("============>%s()\n", __FUNCTION__);
+//    printk("============>%s()\n", __func__);
 	return;
 }
 
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c
index 93121b42f16..fcc90a5700d 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c
@@ -773,6 +773,6 @@ void __exit ieee80211_crypto_tkip_exit(void)
 
 void ieee80211_tkip_null(void)
 {
-//    printk("============>%s()\n", __FUNCTION__);
+//    printk("============>%s()\n", __func__);
 	return;
 }
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_wep.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_wep.c
index f2022369589..8c1bf1f5688 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_wep.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_wep.c
@@ -290,6 +290,6 @@ void __exit ieee80211_crypto_wep_exit(void)
 
 void ieee80211_wep_null(void)
 {
-//	printk("============>%s()\n", __FUNCTION__);
+//	printk("============>%s()\n", __func__);
 	return;
 }
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
index a98414a72bf..d9a8299c48e 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
@@ -549,7 +549,7 @@ void ieee80211_indicate_packets(struct ieee80211_device *ieee, struct ieee80211_
 	u8 i = 0 , j=0;
 	u16 ethertype;
 //	if(index > 1)
-//		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): hahahahhhh, We indicate packet from reorder list, index is %u\n",__FUNCTION__,index);
+//		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): hahahahhhh, We indicate packet from reorder list, index is %u\n",__func__,index);
 	for(j = 0; j<index; j++)
 	{
 //added by amy for reorder
@@ -609,7 +609,7 @@ static void RxReorderIndicatePacket(struct ieee80211_device *ieee,
 	u16			WinEnd = (pTS->RxIndicateSeq + WinSize -1)%4096;
 	u8			index = 0;
 	bool			bMatchWinStart = false, bPktInBuf = false;
-	IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): Seq is %d,pTS->RxIndicateSeq is %d, WinSize is %d\n",__FUNCTION__,SeqNum,pTS->RxIndicateSeq,WinSize);
+	IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): Seq is %d,pTS->RxIndicateSeq is %d, WinSize is %d\n",__func__,SeqNum,pTS->RxIndicateSeq,WinSize);
 	/* Rx Reorder initialize condition.*/
 	if(pTS->RxIndicateSeq == 0xffff) {
 		pTS->RxIndicateSeq = SeqNum;
@@ -662,11 +662,11 @@ static void RxReorderIndicatePacket(struct ieee80211_device *ieee,
 		IEEE80211_DEBUG(IEEE80211_DL_REORDER, "Packets indication!! IndicateSeq: %d, NewSeq: %d\n",\
 				pTS->RxIndicateSeq, SeqNum);
 		prxbIndicateArray[0] = prxb;
-//		printk("========================>%s(): SeqNum is %d\n",__FUNCTION__,SeqNum);
+//		printk("========================>%s(): SeqNum is %d\n",__func__,SeqNum);
 		index = 1;
 	} else {
 		/* Current packet is going to be inserted into pending list.*/
-		//IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): We RX no ordered packed, insert to ordered list\n",__FUNCTION__);
+		//IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): We RX no ordered packed, insert to ordered list\n",__func__);
 		if(!list_empty(&ieee->RxReorder_Unused_List)) {
 			pReorderEntry = (PRX_REORDER_ENTRY)list_entry(ieee->RxReorder_Unused_List.next,RX_REORDER_ENTRY,List);
 			list_del_init(&pReorderEntry->List);
@@ -674,11 +674,11 @@ static void RxReorderIndicatePacket(struct ieee80211_device *ieee,
 			/* Make a reorder entry and insert into a the packet list.*/
 			pReorderEntry->SeqNum = SeqNum;
 			pReorderEntry->prxb = prxb;
-	//		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): pREorderEntry->SeqNum is %d\n",__FUNCTION__,pReorderEntry->SeqNum);
+	//		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): pREorderEntry->SeqNum is %d\n",__func__,pReorderEntry->SeqNum);
 
 			if(!AddReorderEntry(pTS, pReorderEntry)) {
 				IEEE80211_DEBUG(IEEE80211_DL_REORDER, "%s(): Duplicate packet is dropped!! IndicateSeq: %d, NewSeq: %d\n",
-					__FUNCTION__, pTS->RxIndicateSeq, SeqNum);
+					__func__, pTS->RxIndicateSeq, SeqNum);
 				list_add_tail(&pReorderEntry->List,&ieee->RxReorder_Unused_List);
 				{
 					int i;
@@ -713,7 +713,7 @@ static void RxReorderIndicatePacket(struct ieee80211_device *ieee,
 
 	/* Check if there is any packet need indicate.*/
 	while(!list_empty(&pTS->RxPendingPktList)) {
-		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): start RREORDER indicate\n",__FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): start RREORDER indicate\n",__func__);
 		pReorderEntry = (PRX_REORDER_ENTRY)list_entry(pTS->RxPendingPktList.prev,RX_REORDER_ENTRY,List);
 		if( SN_LESS(pReorderEntry->SeqNum, pTS->RxIndicateSeq) ||
 				SN_EQUAL(pReorderEntry->SeqNum, pTS->RxIndicateSeq))
@@ -732,7 +732,7 @@ static void RxReorderIndicatePacket(struct ieee80211_device *ieee,
 
 			IEEE80211_DEBUG(IEEE80211_DL_REORDER,"Packets indication!! IndicateSeq: %d, NewSeq: %d\n",pTS->RxIndicateSeq, SeqNum);
 			prxbIndicateArray[index] = pReorderEntry->prxb;
-		//	printk("========================>%s(): pReorderEntry->SeqNum is %d\n",__FUNCTION__,pReorderEntry->SeqNum);
+		//	printk("========================>%s(): pReorderEntry->SeqNum is %d\n",__func__,pReorderEntry->SeqNum);
 			index++;
 
 			list_add_tail(&pReorderEntry->List,&ieee->RxReorder_Unused_List);
@@ -758,7 +758,7 @@ static void RxReorderIndicatePacket(struct ieee80211_device *ieee,
 
 	if(bPktInBuf && pTS->RxTimeoutIndicateSeq==0xffff) {
 		// Set new pending timer.
-		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): SET rx timeout timer\n", __FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): SET rx timeout timer\n", __func__);
 		pTS->RxTimeoutIndicateSeq = pTS->RxIndicateSeq;
 		if(timer_pending(&pTS->RxPktPendingTimer))
 			del_timer_sync(&pTS->RxPktPendingTimer);
@@ -832,8 +832,8 @@ 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",\
-						__FUNCTION__,rxb->nr_subframes);
-				printk("%s: A-MSDU parse error!! Subframe Length: %d\n",__FUNCTION__, nSubframe_Length);
+						__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);
 				return 0;
@@ -950,7 +950,6 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 
 	//IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA, skb->data, skb->len);
 #ifdef NOT_YET
-#if WIRELESS_EXT > 15
 	/* Put this code here so that we avoid duplicating it in all
 	 * Rx paths. - Jean II */
 #ifdef IW_WIRELESS_SPY		/* defined in iw_handler.h */
@@ -964,18 +963,16 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 		wireless_spy_update(dev, hdr->addr2, &wstats);
 	}
 #endif /* IW_WIRELESS_SPY */
-#endif /* WIRELESS_EXT > 15 */
 	hostap_update_rx_stats(local->ap, hdr, rx_stats);
 #endif
 
-#if WIRELESS_EXT > 15
 	if (ieee->iw_mode == IW_MODE_MONITOR) {
 		ieee80211_monitor_rx(ieee, skb, rx_stats);
 		stats->rx_packets++;
 		stats->rx_bytes += skb->len;
 		return 1;
 	}
-#endif
+
 	if (ieee->host_decrypt) {
 		int idx = 0;
 		if (skb->len >= hdrlen + 3)
@@ -1027,7 +1024,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 	else
 	{
 		PRX_TS_RECORD pRxTS = NULL;
-			//IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): QOS ENABLE AND RECEIVE QOS DATA , we will get Ts, tid:%d\n",__FUNCTION__, tid);
+			//IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): QOS ENABLE AND RECEIVE QOS DATA , we will get Ts, tid:%d\n",__func__, tid);
 		if(GetTs(
 				ieee,
 				(PTS_COMMON_INFO *) &pRxTS,
@@ -1037,7 +1034,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 				true))
 		{
 
-		//	IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): pRxTS->RxLastFragNum is %d,frag is %d,pRxTS->RxLastSeqNum is %d,seq is %d\n",__FUNCTION__,pRxTS->RxLastFragNum,frag,pRxTS->RxLastSeqNum,WLAN_GET_SEQ_SEQ(sc));
+		//	IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): pRxTS->RxLastFragNum is %d,frag is %d,pRxTS->RxLastSeqNum is %d,seq is %d\n",__func__,pRxTS->RxLastFragNum,frag,pRxTS->RxLastSeqNum,WLAN_GET_SEQ_SEQ(sc));
 			if(	(fc & (1<<11))  &&
 					(frag == pRxTS->RxLastFragNum) &&
 					(WLAN_GET_SEQ_SEQ(sc) == pRxTS->RxLastSeqNum)	)
@@ -1052,7 +1049,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 		}
 		else
 		{
-			IEEE80211_DEBUG(IEEE80211_DL_ERR, "%s(): No TS!! Skip the check!!\n",__FUNCTION__);
+			IEEE80211_DEBUG(IEEE80211_DL_ERR, "%s(): No TS!! Skip the check!!\n",__func__);
 			goto rx_dropped;
 		}
 	}
@@ -1297,7 +1294,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 	rxb = kmalloc(sizeof(struct ieee80211_rxb), GFP_ATOMIC);
 	if(rxb == NULL)
 	{
-		IEEE80211_DEBUG(IEEE80211_DL_ERR,"%s(): kmalloc rxb error\n",__FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_ERR,"%s(): kmalloc rxb error\n",__func__);
 		goto rx_dropped;
 	}
 	/* to parse amsdu packets */
@@ -1363,7 +1360,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 	}
 	else
 	{
-		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): REORDER ENABLE AND PTS not NULL, and we will enter RxReorderIndicatePacket()\n",__FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): REORDER ENABLE AND PTS not NULL, and we will enter RxReorderIndicatePacket()\n",__func__);
 		RxReorderIndicatePacket(ieee, rxb, pTS, SeqNum);
 	}
 #ifndef JOHN_NOCPY
@@ -1387,6 +1384,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 	 * hardware as a DMA target */
 	return 0;
 }
+EXPORT_SYMBOL(ieee80211_rx);
 
 #define MGMT_FRAME_FIXED_PART_LENGTH            0x24
 
@@ -1479,7 +1477,6 @@ static int ieee80211_qos_convert_ac_to_parameters(struct
 						  ieee80211_qos_parameters
 						  *qos_param)
 {
-	int rc = 0;
 	int i;
 	struct ieee80211_qos_ac_parameter *ac_params;
 	u8 aci;
@@ -1506,7 +1503,7 @@ static int ieee80211_qos_convert_ac_to_parameters(struct
 		    (ac_params->aci_aifsn & 0x10) ? 0x01 : 0x00;
 		qos_param->tx_op_limit[aci] = le16_to_cpu(ac_params->tx_op_limit);
 	}
-	return rc;
+	return 0;
 }
 
 /*
@@ -1929,7 +1926,7 @@ int ieee80211_parse_info_param(struct ieee80211_device *ieee,
 				info_element->data[1] == 0x13 &&
 				info_element->data[2] == 0x74))
 			{
-				printk("========>%s(): athros AP is exist\n",__FUNCTION__);
+				printk("========>%s(): athros AP is exist\n",__func__);
 				network->atheros_cap_exist = true;
 			}
 			else
@@ -2643,6 +2640,4 @@ void ieee80211_rx_mgt(struct ieee80211_device *ieee,
 
 	}
 }
-
 EXPORT_SYMBOL(ieee80211_rx_mgt);
-EXPORT_SYMBOL(ieee80211_rx);
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
index 2131912113d..c5a0a5da97d 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
@@ -28,11 +28,13 @@ short ieee80211_is_54g(const struct ieee80211_network *net)
 {
 	return (net->rates_ex_len > 0) || (net->rates_len > 4);
 }
+EXPORT_SYMBOL(ieee80211_is_54g);
 
 short ieee80211_is_shortslot(const struct ieee80211_network *net)
 {
 	return net->capability & WLAN_CAPABILITY_SHORT_SLOT;
 }
+EXPORT_SYMBOL(ieee80211_is_shortslot);
 
 /* returns the total length needed for pleacing the RATE MFIE
  * tag and the EXTENDED RATE MFIE tag if needed.
@@ -271,7 +273,7 @@ inline void softmac_mgmt_xmit(struct sk_buff *skb, struct ieee80211_device *ieee
 			/* as for the completion function, it does not need
 			 * to check it any more.
 			 * */
-			printk("%s():insert to waitqueue!\n",__FUNCTION__);
+			printk("%s():insert to waitqueue!\n",__func__);
 			skb_queue_tail(&ieee->skb_waitQ[tcb_desc->queue_index], skb);
 		} else {
 			//printk("TX packet!\n");
@@ -415,6 +417,7 @@ void ieee80211_send_probe_requests(struct ieee80211_device *ieee)
 		ieee80211_send_probe(ieee);
 	}
 }
+EXPORT_SYMBOL(ieee80211_send_probe_requests);
 
 /* this performs syncro scan blocking the caller until all channels
  * in the allowed channel map has been checked.
@@ -485,7 +488,7 @@ out:
 	up(&ieee->scan_sem);
 }
 }
-
+EXPORT_SYMBOL(ieee80211_softmac_scan_syncro);
 
 static void ieee80211_softmac_scan_wq(struct work_struct *work)
 {
@@ -563,7 +566,7 @@ void ieee80211_stop_send_beacons(struct ieee80211_device *ieee)
 	if (ieee->softmac_features & IEEE_SOFTMAC_BEACONS)
 		ieee80211_beacons_stop(ieee);
 }
-
+EXPORT_SYMBOL(ieee80211_stop_send_beacons);
 
 void ieee80211_start_send_beacons(struct ieee80211_device *ieee)
 {
@@ -572,7 +575,7 @@ void ieee80211_start_send_beacons(struct ieee80211_device *ieee)
 	if(ieee->softmac_features & IEEE_SOFTMAC_BEACONS)
 		ieee80211_beacons_start(ieee);
 }
-
+EXPORT_SYMBOL(ieee80211_start_send_beacons);
 
 static void ieee80211_softmac_stop_scan(struct ieee80211_device *ieee)
 {
@@ -600,6 +603,7 @@ void ieee80211_stop_scan(struct ieee80211_device *ieee)
 	else
 		ieee->stop_scan(ieee->dev);
 }
+EXPORT_SYMBOL(ieee80211_stop_scan);
 
 /* called with ieee->lock held */
 static void ieee80211_start_scan(struct ieee80211_device *ieee)
@@ -638,6 +642,7 @@ void ieee80211_start_scan_syncro(struct ieee80211_device *ieee)
 		ieee->scan_syncro(ieee->dev);
 
 }
+EXPORT_SYMBOL(ieee80211_start_scan_syncro);
 
 inline struct sk_buff *ieee80211_authentication_req(struct ieee80211_network *beacon,
 	struct ieee80211_device *ieee, int challengelen)
@@ -670,7 +675,7 @@ inline struct sk_buff *ieee80211_authentication_req(struct ieee80211_network *be
 		auth->algorithm = WLAN_AUTH_SHARED_KEY;
 	else if(ieee->auth_mode == 2)
 		auth->algorithm = WLAN_AUTH_OPEN;//0x80;
-	printk("=================>%s():auth->algorithm is %d\n",__FUNCTION__,auth->algorithm);
+	printk("=================>%s():auth->algorithm is %d\n",__func__,auth->algorithm);
 	auth->transaction = cpu_to_le16(ieee->associate_seq);
 	ieee->associate_seq++;
 
@@ -1212,7 +1217,7 @@ inline struct sk_buff *ieee80211_association_req(struct ieee80211_network *beaco
 			memcpy(tag, realtek_ie_buf,realtek_ie_len -2 );
 		}
 	}
-//	printk("<=====%s(), %p, %p\n", __FUNCTION__, ieee->dev, ieee->dev->dev_addr);
+//	printk("<=====%s(), %p, %p\n", __func__, ieee->dev, ieee->dev->dev_addr);
 //	IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA, skb->data, skb->len);
 	return skb;
 }
@@ -1398,7 +1403,7 @@ static void ieee80211_associate_procedure_wq(struct work_struct *work)
 		ieee->data_hard_stop(ieee->dev);
 
 	ieee80211_stop_scan(ieee);
-	printk("===>%s(), chan:%d\n", __FUNCTION__, ieee->current_network.channel);
+	printk("===>%s(), chan:%d\n", __func__, ieee->current_network.channel);
 	//ieee->set_chan(ieee->dev, ieee->current_network.channel);
 	HTSetConnectBwMode(ieee, HT_CHANNEL_WIDTH_20, HT_EXTCHNL_OFFSET_NO_EXT);
 
@@ -1709,8 +1714,6 @@ ieee80211_rx_assoc_rq(struct ieee80211_device *ieee, struct sk_buff *skb)
 	//FIXME
 }
 
-
-
 static void ieee80211_sta_ps_send_null_frame(struct ieee80211_device *ieee,
 					     short pwr)
 {
@@ -1721,7 +1724,7 @@ static void ieee80211_sta_ps_send_null_frame(struct ieee80211_device *ieee,
 		softmac_ps_mgmt_xmit(buf, ieee);
 
 }
-
+/* EXPORT_SYMBOL(ieee80211_sta_ps_send_null_frame); */
 
 static short ieee80211_sta_ps_sleep(struct ieee80211_device *ieee, u32 *time_h,
 				    u32 *time_l)
@@ -1889,6 +1892,8 @@ void ieee80211_ps_tx_ack(struct ieee80211_device *ieee, short success)
 	}
 	spin_unlock_irqrestore(&ieee->lock, flags);
 }
+EXPORT_SYMBOL(ieee80211_ps_tx_ack);
+
 static void ieee80211_process_action(struct ieee80211_device *ieee,
 				     struct sk_buff *skb)
 {
@@ -1903,20 +1908,17 @@ static void ieee80211_process_action(struct ieee80211_device *ieee,
 	}
 	tmp = *act;
 	act ++;
-	switch (tmp)
-	{
-		case ACT_CAT_BA:
-			if (*act == ACT_ADDBAREQ)
+	switch (tmp) {
+	case ACT_CAT_BA:
+		if (*act == ACT_ADDBAREQ)
 			ieee80211_rx_ADDBAReq(ieee, skb);
-			else if (*act == ACT_ADDBARSP)
+		else if (*act == ACT_ADDBARSP)
 			ieee80211_rx_ADDBARsp(ieee, skb);
-			else if (*act == ACT_DELBA)
+		else if (*act == ACT_DELBA)
 			ieee80211_rx_DELBA(ieee, skb);
-			break;
-		default:
-//			if (net_ratelimit())
-//			IEEE80211_DEBUG(IEEE80211_DL_BA, "unknown action frame(%d)\n", tmp);
-			break;
+		break;
+	default:
+		break;
 	}
 	return;
 
@@ -2191,6 +2193,7 @@ void ieee80211_softmac_xmit(struct ieee80211_txb *txb, struct ieee80211_device *
 	spin_unlock_irqrestore(&ieee->lock,flags);
 
 }
+EXPORT_SYMBOL(ieee80211_softmac_xmit);
 
 /* called with ieee->lock acquired */
 static void ieee80211_resume_tx(struct ieee80211_device *ieee)
@@ -2232,6 +2235,7 @@ void ieee80211_reset_queue(struct ieee80211_device *ieee)
 	spin_unlock_irqrestore(&ieee->lock,flags);
 
 }
+EXPORT_SYMBOL(ieee80211_reset_queue);
 
 void ieee80211_wake_queue(struct ieee80211_device *ieee)
 {
@@ -2272,7 +2276,7 @@ void ieee80211_wake_queue(struct ieee80211_device *ieee)
 exit :
 	spin_unlock_irqrestore(&ieee->lock,flags);
 }
-
+EXPORT_SYMBOL(ieee80211_wake_queue);
 
 void ieee80211_stop_queue(struct ieee80211_device *ieee)
 {
@@ -2287,7 +2291,7 @@ void ieee80211_stop_queue(struct ieee80211_device *ieee)
 	//spin_unlock_irqrestore(&ieee->lock,flags);
 
 }
-
+EXPORT_SYMBOL(ieee80211_stop_queue);
 
 inline void ieee80211_randomize_cell(struct ieee80211_device *ieee)
 {
@@ -2505,6 +2509,8 @@ void ieee80211_disassociate(struct ieee80211_device *ieee)
 	notify_wx_assoc_event(ieee);
 
 }
+EXPORT_SYMBOL(ieee80211_disassociate);
+
 static void ieee80211_associate_retry_wq(struct work_struct *work)
 {
 	struct delayed_work *dwork = container_of(work, struct delayed_work, work);
@@ -2584,6 +2590,7 @@ struct sk_buff *ieee80211_get_beacon(struct ieee80211_device *ieee)
 
 	return skb;
 }
+EXPORT_SYMBOL(ieee80211_get_beacon);
 
 void ieee80211_softmac_stop_protocol(struct ieee80211_device *ieee)
 {
@@ -2592,7 +2599,7 @@ void ieee80211_softmac_stop_protocol(struct ieee80211_device *ieee)
 	ieee80211_stop_protocol(ieee);
 	up(&ieee->wx_sem);
 }
-
+EXPORT_SYMBOL(ieee80211_softmac_stop_protocol);
 
 void ieee80211_stop_protocol(struct ieee80211_device *ieee)
 {
@@ -2618,6 +2625,7 @@ void ieee80211_softmac_start_protocol(struct ieee80211_device *ieee)
 	ieee80211_start_protocol(ieee);
 	up(&ieee->wx_sem);
 }
+EXPORT_SYMBOL(ieee80211_softmac_start_protocol);
 
 void ieee80211_start_protocol(struct ieee80211_device *ieee)
 {
@@ -2639,7 +2647,7 @@ void ieee80211_start_protocol(struct ieee80211_device *ieee)
 
 	if (ieee->current_network.beacon_interval == 0)
 		ieee->current_network.beacon_interval = 100;
-//	printk("===>%s(), chan:%d\n", __FUNCTION__, ieee->current_network.channel);
+//	printk("===>%s(), chan:%d\n", __func__, ieee->current_network.channel);
 //	ieee->set_chan(ieee->dev,ieee->current_network.channel);
 
 	for(i = 0; i < 17; i++) {
@@ -2846,7 +2854,6 @@ static int ieee80211_wpa_set_auth_algs(struct ieee80211_device *ieee, int value)
 	struct ieee80211_security sec = {
 		.flags = SEC_AUTH_MODE,
 	};
-	int ret = 0;
 
 	if (value & AUTH_ALG_SHARED_KEY) {
 		sec.auth_mode = WLAN_AUTH_SHARED_KEY;
@@ -2869,7 +2876,7 @@ static int ieee80211_wpa_set_auth_algs(struct ieee80211_device *ieee, int value)
 	//else
 	//	ret = -EOPNOTSUPP;
 
-	return ret;
+	return 0;
 }
 
 static int ieee80211_wpa_set_param(struct ieee80211_device *ieee, u8 name, u32 value)
@@ -3141,6 +3148,7 @@ SendDisassociation(
 				//dev_kfree_skb_any(skb);//edit by thomas
 		}
 }
+EXPORT_SYMBOL(SendDisassociation);
 
 int ieee80211_wpa_supplicant_ioctl(struct ieee80211_device *ieee, struct iw_point *p)
 {
@@ -3196,6 +3204,7 @@ out:
 
 	return ret;
 }
+EXPORT_SYMBOL(ieee80211_wpa_supplicant_ioctl);
 
 void notify_wx_assoc_event(struct ieee80211_device *ieee)
 {
@@ -3207,25 +3216,4 @@ void notify_wx_assoc_event(struct ieee80211_device *ieee)
 		memset(wrqu.ap_addr.sa_data, 0, ETH_ALEN);
 	wireless_send_event(ieee->dev, SIOCGIWAP, &wrqu, NULL);
 }
-
-EXPORT_SYMBOL(ieee80211_get_beacon);
-EXPORT_SYMBOL(ieee80211_wake_queue);
-EXPORT_SYMBOL(ieee80211_stop_queue);
-EXPORT_SYMBOL(ieee80211_reset_queue);
-EXPORT_SYMBOL(ieee80211_softmac_stop_protocol);
-EXPORT_SYMBOL(ieee80211_softmac_start_protocol);
-EXPORT_SYMBOL(ieee80211_is_shortslot);
-EXPORT_SYMBOL(ieee80211_is_54g);
-EXPORT_SYMBOL(ieee80211_wpa_supplicant_ioctl);
-EXPORT_SYMBOL(ieee80211_ps_tx_ack);
-EXPORT_SYMBOL(ieee80211_softmac_xmit);
-EXPORT_SYMBOL(ieee80211_stop_send_beacons);
 EXPORT_SYMBOL(notify_wx_assoc_event);
-EXPORT_SYMBOL(SendDisassociation);
-EXPORT_SYMBOL(ieee80211_disassociate);
-EXPORT_SYMBOL(ieee80211_start_send_beacons);
-EXPORT_SYMBOL(ieee80211_stop_scan);
-EXPORT_SYMBOL(ieee80211_send_probe_requests);
-EXPORT_SYMBOL(ieee80211_softmac_scan_syncro);
-EXPORT_SYMBOL(ieee80211_start_scan_syncro);
-//EXPORT_SYMBOL(ieee80211_sta_ps_send_null_frame);
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c
index 7b7d929f153..935a8f4adcf 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c
@@ -26,7 +26,7 @@ const long ieee80211_wlan_frequencies[] = {
 	2452, 2457, 2462, 2467,
 	2472, 2484
 };
-
+EXPORT_SYMBOL(ieee80211_wlan_frequencies);
 
 int ieee80211_wx_set_freq(struct ieee80211_device *ieee, struct iw_request_info *a,
 			     union iwreq_data *wrqu, char *b)
@@ -83,7 +83,7 @@ out:
 	up(&ieee->wx_sem);
 	return ret;
 }
-
+EXPORT_SYMBOL(ieee80211_wx_set_freq);
 
 int ieee80211_wx_get_freq(struct ieee80211_device *ieee,
 			     struct iw_request_info *a,
@@ -101,6 +101,7 @@ int ieee80211_wx_get_freq(struct ieee80211_device *ieee,
 
 	return 0;
 }
+EXPORT_SYMBOL(ieee80211_wx_get_freq);
 
 int ieee80211_wx_get_wap(struct ieee80211_device *ieee,
 			    struct iw_request_info *info,
@@ -129,7 +130,7 @@ int ieee80211_wx_get_wap(struct ieee80211_device *ieee,
 
 	return 0;
 }
-
+EXPORT_SYMBOL(ieee80211_wx_get_wap);
 
 int ieee80211_wx_set_wap(struct ieee80211_device *ieee,
 			 struct iw_request_info *info,
@@ -176,6 +177,7 @@ out:
 	up(&ieee->wx_sem);
 	return ret;
 }
+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)
 {
@@ -211,6 +213,7 @@ out:
 	return ret;
 
 }
+EXPORT_SYMBOL(ieee80211_wx_get_essid);
 
 int ieee80211_wx_set_rate(struct ieee80211_device *ieee,
 			     struct iw_request_info *info,
@@ -223,8 +226,7 @@ int ieee80211_wx_set_rate(struct ieee80211_device *ieee,
 	//FIXME: we might want to limit rate also in management protocols.
 	return 0;
 }
-
-
+EXPORT_SYMBOL(ieee80211_wx_set_rate);
 
 int ieee80211_wx_get_rate(struct ieee80211_device *ieee,
 			     struct iw_request_info *info,
@@ -237,7 +239,7 @@ int ieee80211_wx_get_rate(struct ieee80211_device *ieee,
 
 	return 0;
 }
-
+EXPORT_SYMBOL(ieee80211_wx_get_rate);
 
 int ieee80211_wx_set_rts(struct ieee80211_device *ieee,
 			     struct iw_request_info *info,
@@ -254,6 +256,7 @@ int ieee80211_wx_set_rts(struct ieee80211_device *ieee,
 	}
 	return 0;
 }
+EXPORT_SYMBOL(ieee80211_wx_set_rts);
 
 int ieee80211_wx_get_rts(struct ieee80211_device *ieee,
 			     struct iw_request_info *info,
@@ -264,6 +267,8 @@ int ieee80211_wx_get_rts(struct ieee80211_device *ieee,
 	wrqu->rts.disabled = (wrqu->rts.value == DEFAULT_RTS_THRESHOLD);
 	return 0;
 }
+EXPORT_SYMBOL(ieee80211_wx_get_rts);
+
 int ieee80211_wx_set_mode(struct ieee80211_device *ieee, struct iw_request_info *a,
 			     union iwreq_data *wrqu, char *b)
 {
@@ -294,6 +299,7 @@ out:
 	up(&ieee->wx_sem);
 	return 0;
 }
+EXPORT_SYMBOL(ieee80211_wx_set_mode);
 
 void ieee80211_wx_sync_scan_wq(struct work_struct *work)
 {
@@ -378,6 +384,7 @@ out:
 	up(&ieee->wx_sem);
 	return ret;
 }
+EXPORT_SYMBOL(ieee80211_wx_set_scan);
 
 int ieee80211_wx_set_essid(struct ieee80211_device *ieee,
 			      struct iw_request_info *a,
@@ -432,6 +439,7 @@ out:
 	up(&ieee->wx_sem);
 	return ret;
 }
+EXPORT_SYMBOL(ieee80211_wx_set_essid);
 
  int ieee80211_wx_get_mode(struct ieee80211_device *ieee, struct iw_request_info *a,
 			     union iwreq_data *wrqu, char *b)
@@ -440,6 +448,7 @@ out:
 	wrqu->mode = ieee->iw_mode;
 	return 0;
 }
+EXPORT_SYMBOL(ieee80211_wx_get_mode);
 
  int ieee80211_wx_set_rawtx(struct ieee80211_device *ieee,
 			       struct iw_request_info *info,
@@ -477,6 +486,7 @@ out:
 
 	return 0;
 }
+EXPORT_SYMBOL(ieee80211_wx_set_rawtx);
 
 int ieee80211_wx_get_name(struct ieee80211_device *ieee,
 			     struct iw_request_info *info,
@@ -502,7 +512,7 @@ int ieee80211_wx_get_name(struct ieee80211_device *ieee,
 
 	return 0;
 }
-
+EXPORT_SYMBOL(ieee80211_wx_get_name);
 
 /* this is mostly stolen from hostap */
 int ieee80211_wx_set_power(struct ieee80211_device *ieee,
@@ -553,14 +563,13 @@ exit:
 	return ret;
 
 }
+EXPORT_SYMBOL(ieee80211_wx_set_power);
 
 /* this is stolen from hostap */
 int ieee80211_wx_get_power(struct ieee80211_device *ieee,
 				 struct iw_request_info *info,
 				 union iwreq_data *wrqu, char *extra)
 {
-	int ret =0;
-
 	down(&ieee->wx_sem);
 
 	if(ieee->ps == IEEE80211_PS_DISABLED){
@@ -590,24 +599,7 @@ int ieee80211_wx_get_power(struct ieee80211_device *ieee,
 
 exit:
 	up(&ieee->wx_sem);
-	return ret;
+	return 0;
 
 }
-EXPORT_SYMBOL(ieee80211_wx_get_essid);
-EXPORT_SYMBOL(ieee80211_wx_set_essid);
-EXPORT_SYMBOL(ieee80211_wx_set_rate);
-EXPORT_SYMBOL(ieee80211_wx_get_rate);
-EXPORT_SYMBOL(ieee80211_wx_set_wap);
-EXPORT_SYMBOL(ieee80211_wx_get_wap);
-EXPORT_SYMBOL(ieee80211_wx_set_mode);
-EXPORT_SYMBOL(ieee80211_wx_get_mode);
-EXPORT_SYMBOL(ieee80211_wx_set_scan);
-EXPORT_SYMBOL(ieee80211_wx_get_freq);
-EXPORT_SYMBOL(ieee80211_wx_set_freq);
-EXPORT_SYMBOL(ieee80211_wx_set_rawtx);
-EXPORT_SYMBOL(ieee80211_wx_get_name);
-EXPORT_SYMBOL(ieee80211_wx_set_power);
 EXPORT_SYMBOL(ieee80211_wx_get_power);
-EXPORT_SYMBOL(ieee80211_wlan_frequencies);
-EXPORT_SYMBOL(ieee80211_wx_set_rts);
-EXPORT_SYMBOL(ieee80211_wx_get_rts);
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
index 6779bdd561d..029a9765179 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
@@ -188,7 +188,7 @@ int ieee80211_encrypt_fragment(
 
 	if (!(crypt && crypt->ops))
 	{
-		printk("=========>%s(), crypt is null\n", __FUNCTION__);
+		printk("=========>%s(), crypt is null\n", __func__);
 		return -1;
 	}
 #ifdef CONFIG_IEEE80211_CRYPT_TKIP
@@ -236,6 +236,7 @@ void ieee80211_txb_free(struct ieee80211_txb *txb) {
 		return;
 	kfree(txb);
 }
+EXPORT_SYMBOL(ieee80211_txb_free);
 
 static struct ieee80211_txb *ieee80211_alloc_txb(int nr_frags, int txb_size,
 						 gfp_t gfp_mask)
@@ -913,5 +914,3 @@ int ieee80211_xmit(struct sk_buff *skb, struct net_device *dev)
 	return 1;
 
 }
-
-EXPORT_SYMBOL(ieee80211_txb_free);
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_wx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_wx.c
index bdf67ec5df2..698507e00f3 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_wx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_wx.c
@@ -194,7 +194,7 @@ static inline char *rtl819x_translate_scan(struct ieee80211_device *ieee,
 	iwe.u.data.length = p - custom;
 	if (iwe.u.data.length)
 	    start = iwe_stream_add_point(info, start, stop, &iwe, custom);
-#if (WIRELESS_EXT < 18)
+
 	if (ieee->wpa_enabled && network->wpa_ie_len){
 		char buf[MAX_WPA_IE_LEN * 2 + 30];
 	//	printk("WPA IE\n");
@@ -224,26 +224,6 @@ static inline char *rtl819x_translate_scan(struct ieee80211_device *ieee,
 		iwe.u.data.length = strlen(buf);
 		start = iwe_stream_add_point(info, start, stop, &iwe, buf);
 	}
-#else
-	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;
-		start = iwe_stream_add_point(info, start, stop, &iwe, buf);
-	}
-	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;
-		start = iwe_stream_add_point(info, start, stop, &iwe, buf);
-	}
-#endif
 
 
 	/* Add EXTRA: Age to display seconds since last beacon/probe response
@@ -305,6 +285,7 @@ int ieee80211_wx_get_scan(struct ieee80211_device *ieee,
 
 	return err;
 }
+EXPORT_SYMBOL(ieee80211_wx_get_scan);
 
 int ieee80211_wx_set_encode(struct ieee80211_device *ieee,
 			    struct iw_request_info *info,
@@ -479,6 +460,7 @@ int ieee80211_wx_set_encode(struct ieee80211_device *ieee,
 	}
 	return 0;
 }
+EXPORT_SYMBOL(ieee80211_wx_set_encode);
 
 int ieee80211_wx_get_encode(struct ieee80211_device *ieee,
 			    struct iw_request_info *info,
@@ -521,7 +503,8 @@ int ieee80211_wx_get_encode(struct ieee80211_device *ieee,
 
 	return 0;
 }
-#if (WIRELESS_EXT >= 18)
+EXPORT_SYMBOL(ieee80211_wx_get_encode);
+
 int ieee80211_wx_set_encode_ext(struct ieee80211_device *ieee,
 			       struct iw_request_info *info,
 			       union iwreq_data *wrqu, char *extra)
@@ -695,6 +678,7 @@ done:
 	}
 	return ret;
 }
+EXPORT_SYMBOL(ieee80211_wx_set_encode_ext);
 
 int ieee80211_wx_get_encode_ext(struct ieee80211_device *ieee,
 			       struct iw_request_info *info,
@@ -750,6 +734,7 @@ int ieee80211_wx_get_encode_ext(struct ieee80211_device *ieee,
 
 	return 0;
 }
+EXPORT_SYMBOL(ieee80211_wx_get_encode_ext);
 
 int ieee80211_wx_set_mlme(struct ieee80211_device *ieee,
 			       struct iw_request_info *info,
@@ -766,6 +751,7 @@ int ieee80211_wx_set_mlme(struct ieee80211_device *ieee,
 	}
 	return 0;
 }
+EXPORT_SYMBOL(ieee80211_wx_set_mlme);
 
 int ieee80211_wx_set_auth(struct ieee80211_device *ieee,
 			       struct iw_request_info *info,
@@ -792,7 +778,7 @@ int ieee80211_wx_set_auth(struct ieee80211_device *ieee,
 		break;
 
 	case IW_AUTH_80211_AUTH_ALG:
-		//printk("======>%s():data->value is %d\n",__FUNCTION__,data->value);
+		//printk("======>%s():data->value is %d\n",__func__,data->value);
 	//	ieee->open_wep = (data->value&IW_AUTH_ALG_OPEN_SYSTEM)?1:0;
 		if(data->value & IW_AUTH_ALG_SHARED_KEY){
 			ieee->open_wep = 0;
@@ -828,7 +814,8 @@ int ieee80211_wx_set_auth(struct ieee80211_device *ieee,
 	}
 	return 0;
 }
-#endif
+EXPORT_SYMBOL(ieee80211_wx_set_auth);
+
 int ieee80211_wx_set_gen_ie(struct ieee80211_device *ieee, u8 *ie, size_t len)
 {
 	u8 *buf;
@@ -862,14 +849,4 @@ int ieee80211_wx_set_gen_ie(struct ieee80211_device *ieee, u8 *ie, size_t len)
 	return 0;
 
 }
-
 EXPORT_SYMBOL(ieee80211_wx_set_gen_ie);
-#if (WIRELESS_EXT >= 18)
-EXPORT_SYMBOL(ieee80211_wx_set_mlme);
-EXPORT_SYMBOL(ieee80211_wx_set_auth);
-EXPORT_SYMBOL(ieee80211_wx_set_encode_ext);
-EXPORT_SYMBOL(ieee80211_wx_get_encode_ext);
-#endif
-EXPORT_SYMBOL(ieee80211_wx_get_scan);
-EXPORT_SYMBOL(ieee80211_wx_set_encode);
-EXPORT_SYMBOL(ieee80211_wx_get_encode);
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
index 7bf55e39355..b8cc6a537c3 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
@@ -113,7 +113,7 @@ static struct sk_buff *ieee80211_ADDBA(struct ieee80211_device *ieee, u8 *Dst, P
 	u16 tmp = 0;
 	u16 len = ieee->tx_headroom + 9;
 	//category(1) + action field(1) + Dialog Token(1) + BA Parameter Set(2) +  BA Timeout Value(2) +  BA Start SeqCtrl(2)(or StatusCode(2))
-	IEEE80211_DEBUG(IEEE80211_DL_TRACE | IEEE80211_DL_BA, "========>%s(), frame(%d) sentd to:%pM, ieee->dev:%p\n", __FUNCTION__, type, Dst, ieee->dev);
+	IEEE80211_DEBUG(IEEE80211_DL_TRACE | IEEE80211_DL_BA, "========>%s(), frame(%d) sentd to:%pM, ieee->dev:%p\n", __func__, type, Dst, ieee->dev);
 	if (pBA == NULL||ieee == NULL)
 	{
 		IEEE80211_DEBUG(IEEE80211_DL_ERR, "pBA(%p) is NULL or ieee(%p) is NULL\n", pBA, ieee);
@@ -201,7 +201,7 @@ static struct sk_buff *ieee80211_DELBA(
 	u16 len = 6 + ieee->tx_headroom;
 
 	if (net_ratelimit())
-	IEEE80211_DEBUG(IEEE80211_DL_TRACE | IEEE80211_DL_BA, "========>%s(), ReasonCode(%d) sentd to:%pM\n", __FUNCTION__, ReasonCode, dst);
+	IEEE80211_DEBUG(IEEE80211_DL_TRACE | IEEE80211_DL_BA, "========>%s(), ReasonCode(%d) sentd to:%pM\n", __func__, ReasonCode, dst);
 
 	memset(&DelbaParamSet, 0, 2);
 
@@ -240,7 +240,7 @@ static struct sk_buff *ieee80211_DELBA(
 
 	IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA|IEEE80211_DL_BA, skb->data, skb->len);
 	if (net_ratelimit())
-	IEEE80211_DEBUG(IEEE80211_DL_TRACE | IEEE80211_DL_BA, "<=====%s()\n", __FUNCTION__);
+	IEEE80211_DEBUG(IEEE80211_DL_TRACE | IEEE80211_DL_BA, "<=====%s()\n", __func__);
 	return skb;
 }
 
@@ -266,7 +266,7 @@ static void ieee80211_send_ADDBAReq(struct ieee80211_device *ieee,
 	}
 	else
 	{
-		IEEE80211_DEBUG(IEEE80211_DL_ERR, "alloc skb error in function %s()\n", __FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "alloc skb error in function %s()\n", __func__);
 	}
 	return;
 }
@@ -291,7 +291,7 @@ static void ieee80211_send_ADDBARsp(struct ieee80211_device *ieee, u8 *dst,
 	}
 	else
 	{
-		IEEE80211_DEBUG(IEEE80211_DL_ERR, "alloc skb error in function %s()\n", __FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "alloc skb error in function %s()\n", __func__);
 	}
 
 	return;
@@ -320,7 +320,7 @@ static void ieee80211_send_DELBA(struct ieee80211_device *ieee, u8 *dst,
 	}
 	else
 	{
-		IEEE80211_DEBUG(IEEE80211_DL_ERR, "alloc skb error in function %s()\n", __FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "alloc skb error in function %s()\n", __func__);
 	}
 	return ;
 }
@@ -380,7 +380,7 @@ int ieee80211_rx_ADDBAReq( struct ieee80211_device *ieee, struct sk_buff *skb)
 			true)	)
 	{
 		rc = ADDBA_STATUS_REFUSED;
-		IEEE80211_DEBUG(IEEE80211_DL_ERR, "can't get TS in %s()\n", __FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "can't get TS in %s()\n", __func__);
 		goto OnADDBAReq_Fail;
 	}
 	pBA = &pTS->RxAdmittedBARecord;
@@ -391,7 +391,7 @@ int ieee80211_rx_ADDBAReq( struct ieee80211_device *ieee, struct sk_buff *skb)
 	if(pBaParamSet->field.BAPolicy == BA_POLICY_DELAYED)
 	{
 		rc = ADDBA_STATUS_INVALID_PARAM;
-		IEEE80211_DEBUG(IEEE80211_DL_ERR, "BA Policy is not correct in %s()\n", __FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "BA Policy is not correct in %s()\n", __func__);
 		goto OnADDBAReq_Fail;
 	}
 		// Admit the ADDBA Request
@@ -479,7 +479,7 @@ int ieee80211_rx_ADDBARsp( struct ieee80211_device *ieee, struct sk_buff *skb)
 			TX_DIR,
 			false)	)
 	{
-		IEEE80211_DEBUG(IEEE80211_DL_ERR, "can't get TS in %s()\n", __FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "can't get TS in %s()\n", __func__);
 		ReasonCode = DELBA_REASON_UNKNOWN_BA;
 		goto OnADDBARsp_Reject;
 	}
@@ -603,7 +603,7 @@ int ieee80211_rx_DELBA(struct ieee80211_device *ieee,struct sk_buff *skb)
 				RX_DIR,
 				false)	)
 		{
-			IEEE80211_DEBUG(IEEE80211_DL_ERR,  "can't get TS for RXTS in %s()\n", __FUNCTION__);
+			IEEE80211_DEBUG(IEEE80211_DL_ERR,  "can't get TS for RXTS in %s()\n", __func__);
 			return -1;
 		}
 
@@ -621,7 +621,7 @@ int ieee80211_rx_DELBA(struct ieee80211_device *ieee,struct sk_buff *skb)
 			TX_DIR,
 			false)	)
 		{
-			IEEE80211_DEBUG(IEEE80211_DL_ERR,  "can't get TS for TXTS in %s()\n", __FUNCTION__);
+			IEEE80211_DEBUG(IEEE80211_DL_ERR,  "can't get TS for TXTS in %s()\n", __func__);
 			return -1;
 		}
 
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c
index 53ec2d435ff..2694a085e3a 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c
@@ -130,7 +130,7 @@ void HTDebugHTCapability(u8 *CapIE, u8 *TitleString )
 	if(!memcmp(CapIE, EWC11NHTCap, sizeof(EWC11NHTCap)))
 	{
 		//EWC IE
-		IEEE80211_DEBUG(IEEE80211_DL_HT, "EWC IE in %s()\n", __FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_HT, "EWC IE in %s()\n", __func__);
 		pCapELE = (PHT_CAPABILITY_ELE)(&CapIE[4]);
 	}else
 		pCapELE = (PHT_CAPABILITY_ELE)(&CapIE[0]);
@@ -167,7 +167,7 @@ void HTDebugHTInfo(u8 *InfoIE, u8 *TitleString)
 	if(!memcmp(InfoIE, EWC11NHTInfo, sizeof(EWC11NHTInfo)))
 	{
 		// Not EWC IE
-		IEEE80211_DEBUG(IEEE80211_DL_HT, "EWC IE in %s()\n", __FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_HT, "EWC IE in %s()\n", __func__);
 		pHTInfoEle = (PHT_INFORMATION_ELE)(&InfoIE[4]);
 	}else
 		pHTInfoEle = (PHT_INFORMATION_ELE)(&InfoIE[0]);
@@ -415,8 +415,7 @@ static void HTIOTPeerDetermine(struct ieee80211_device *ieee)
   * *****************************************************************************************************************/
 static u8 HTIOTActIsDisableMCS14(struct ieee80211_device *ieee, u8 *PeerMacAddr)
 {
-	u8 ret = 0;
-	return ret;
+	return 0;
  }
 
 
@@ -1134,7 +1133,7 @@ void HTInitializeHTInfo(struct ieee80211_device *ieee)
 	//
 	// These parameters will be reset when receiving deauthentication packet
 	//
-	IEEE80211_DEBUG(IEEE80211_DL_HT, "===========>%s()\n", __FUNCTION__);
+	IEEE80211_DEBUG(IEEE80211_DL_HT, "===========>%s()\n", __func__);
 	pHTInfo->bCurrentHTSupport = false;
 
 	// 40MHz channel support
@@ -1229,7 +1228,7 @@ void HTResetSelfAndSavePeerSetting(struct ieee80211_device *ieee,	struct ieee802
 	//
 	//  Save Peer Setting before Association
 	//
-	IEEE80211_DEBUG(IEEE80211_DL_HT, "==============>%s()\n", __FUNCTION__);
+	IEEE80211_DEBUG(IEEE80211_DL_HT, "==============>%s()\n", __func__);
 	/*unmark bEnableHT flag here is the same reason why unmarked in function ieee80211_softmac_new_net. WB 2008.09.10*/
 //	if( pHTInfo->bEnableHT &&  pNetwork->bssht.bdSupportHT)
 	if (pNetwork->bssht.bdSupportHT)
@@ -1319,6 +1318,7 @@ void HTUpdateSelfAndPeerSetting(struct ieee80211_device *ieee,	struct ieee80211_
 		//
 	}
 }
+EXPORT_SYMBOL(HTUpdateSelfAndPeerSetting);
 
 /********************************************************************************************************************
  *function:  check whether HT control field exists
@@ -1397,7 +1397,7 @@ void HTSetConnectBwModeCallback(struct ieee80211_device *ieee)
 {
 	PRT_HIGH_THROUGHPUT pHTInfo = ieee->pHTInfo;
 
-	IEEE80211_DEBUG(IEEE80211_DL_HT, "======>%s()\n", __FUNCTION__);
+	IEEE80211_DEBUG(IEEE80211_DL_HT, "======>%s()\n", __func__);
 
 	if(pHTInfo->bCurBW40MHz)
 	{
@@ -1416,5 +1416,3 @@ void HTSetConnectBwModeCallback(struct ieee80211_device *ieee)
 
 	pHTInfo->bSwBwInProgress = false;
 }
-
-EXPORT_SYMBOL(HTUpdateSelfAndPeerSetting);
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_TSProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_TSProc.c
index 426f223e6a2..c4514109d0e 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_TSProc.c
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_TSProc.c
@@ -38,7 +38,7 @@ static void RxPktPendingTimeout(unsigned long data)
 
 	spin_lock_irqsave(&(ieee->reorder_spinlock), flags);
 	//PlatformAcquireSpinLock(Adapter, RT_RX_SPINLOCK);
-	IEEE80211_DEBUG(IEEE80211_DL_REORDER,"==================>%s()\n",__FUNCTION__);
+	IEEE80211_DEBUG(IEEE80211_DL_REORDER,"==================>%s()\n",__func__);
 	if(pRxTs->RxTimeoutIndicateSeq != 0xffff)
 	{
 		// Indicate the pending packets sequentially according to SeqNum until meet the gap.
@@ -144,7 +144,7 @@ void TSInitialize(struct ieee80211_device *ieee)
 	PRX_TS_RECORD		pRxTS  = ieee->RxTsRecord;
 	PRX_REORDER_ENTRY	pRxReorderEntry = ieee->RxReorderEntry;
 	u8				count = 0;
-	IEEE80211_DEBUG(IEEE80211_DL_TS, "==========>%s()\n", __FUNCTION__);
+	IEEE80211_DEBUG(IEEE80211_DL_TS, "==========>%s()\n", __func__);
 	// Initialize Tx TS related info.
 	INIT_LIST_HEAD(&ieee->Tx_TS_Admit_List);
 	INIT_LIST_HEAD(&ieee->Tx_TS_Pending_List);
@@ -361,7 +361,7 @@ bool GetTs(
 		// In WMM case: we use 4 TID only
 		if (!IsACValid(TID))
 		{
-			IEEE80211_DEBUG(IEEE80211_DL_ERR, " in %s(), TID(%d) is not valid\n", __FUNCTION__, TID);
+			IEEE80211_DEBUG(IEEE80211_DL_ERR, " in %s(), TID(%d) is not valid\n", __func__, TID);
 			return false;
 		}
 
@@ -463,7 +463,7 @@ bool GetTs(
 			}
 			else
 			{
-				IEEE80211_DEBUG(IEEE80211_DL_ERR, "in function %s() There is not enough TS record to be used!!", __FUNCTION__);
+				IEEE80211_DEBUG(IEEE80211_DL_ERR, "in function %s() There is not enough TS record to be used!!", __func__);
 				return false;
 			}
 		}
@@ -619,5 +619,5 @@ void TsStartAddBaProcess(struct ieee80211_device *ieee, PTX_TS_RECORD	pTxTS)
 		}
 	}
 	else
-		IEEE80211_DEBUG(IEEE80211_DL_ERR, "%s()==>BA timer is already added\n", __FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "%s()==>BA timer is already added\n", __func__);
 }
diff --git a/drivers/staging/rtl8192u/r8180_93cx6.c b/drivers/staging/rtl8192u/r8180_93cx6.c
index cd06054fbed..fb8a7a8ac64 100644
--- a/drivers/staging/rtl8192u/r8180_93cx6.c
+++ b/drivers/staging/rtl8192u/r8180_93cx6.c
@@ -53,7 +53,7 @@ static void eprom_ck_cycle(struct net_device *dev)
 }
 
 
-static void eprom_w(struct net_device *dev,short bit)
+static void eprom_w(struct net_device *dev, short bit)
 {
 	u8 cmdreg;
 
@@ -86,7 +86,7 @@ static void eprom_send_bits_string(struct net_device *dev, short b[], int len)
 {
 	int i;
 
-	for(i=0; i<len; i++){
+	for (i = 0; i < len; i++) {
 		eprom_w(dev, b[i]);
 		eprom_ck_cycle(dev);
 	}
@@ -96,37 +96,37 @@ static void eprom_send_bits_string(struct net_device *dev, short b[], int len)
 u32 eprom_read(struct net_device *dev, u32 addr)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
-	short read_cmd[]={1,1,0};
+	short read_cmd[] = {1, 1, 0};
 	short addr_str[8];
 	int i;
 	int addr_len;
 	u32 ret;
 
-	ret=0;
+	ret = 0;
 	//enable EPROM programming
 	write_nic_byte_E(dev, EPROM_CMD,
 		       (EPROM_CMD_PROGRAM<<EPROM_CMD_OPERATING_MODE_SHIFT));
 	force_pci_posting(dev);
 	udelay(EPROM_DELAY);
 
-	if (priv->epromtype==EPROM_93c56){
-		addr_str[7]=addr & 1;
-		addr_str[6]=addr & (1<<1);
-		addr_str[5]=addr & (1<<2);
-		addr_str[4]=addr & (1<<3);
-		addr_str[3]=addr & (1<<4);
-		addr_str[2]=addr & (1<<5);
-		addr_str[1]=addr & (1<<6);
-		addr_str[0]=addr & (1<<7);
-		addr_len=8;
-	}else{
-		addr_str[5]=addr & 1;
-		addr_str[4]=addr & (1<<1);
-		addr_str[3]=addr & (1<<2);
-		addr_str[2]=addr & (1<<3);
-		addr_str[1]=addr & (1<<4);
-		addr_str[0]=addr & (1<<5);
-		addr_len=6;
+	if (priv->epromtype == EPROM_93c56) {
+		addr_str[7] = addr & 1;
+		addr_str[6] = addr & (1<<1);
+		addr_str[5] = addr & (1<<2);
+		addr_str[4] = addr & (1<<3);
+		addr_str[3] = addr & (1<<4);
+		addr_str[2] = addr & (1<<5);
+		addr_str[1] = addr & (1<<6);
+		addr_str[0] = addr & (1<<7);
+		addr_len = 8;
+	} else {
+		addr_str[5] = addr & 1;
+		addr_str[4] = addr & (1<<1);
+		addr_str[3] = addr & (1<<2);
+		addr_str[2] = addr & (1<<3);
+		addr_str[1] = addr & (1<<4);
+		addr_str[0] = addr & (1<<5);
+		addr_len = 6;
 	}
 	eprom_cs(dev, 1);
 	eprom_ck_cycle(dev);
@@ -137,7 +137,7 @@ u32 eprom_read(struct net_device *dev, u32 addr)
 	//I'm unsure if it is necessary, but anyway shouldn't hurt
 	eprom_w(dev, 0);
 
-	for(i=0;i<16;i++){
+	for (i = 0; i < 16; i++) {
 		//eeprom needs a clk cycle between writing opcode&adr
 		//and reading data. (eeprom outs a dummy 0)
 		eprom_ck_cycle(dev);
diff --git a/drivers/staging/rtl8192u/r8190_rtl8256.c b/drivers/staging/rtl8192u/r8190_rtl8256.c
index 40b14a2d1cd..08e1bc90555 100644
--- a/drivers/staging/rtl8192u/r8190_rtl8256.c
+++ b/drivers/staging/rtl8192u/r8190_rtl8256.c
@@ -32,7 +32,7 @@ void PHY_SetRF8256Bandwidth(struct net_device *dev , HT_CHANNEL_WIDTH Bandwidth)
 	for(eRFPath = 0; eRFPath <RF90_PATH_MAX; eRFPath++)
 	{
 		if (!rtl8192_phy_CheckIsLegalRFPath(dev, eRFPath))
-				continue;
+			continue;
 
 		switch (Bandwidth)
 		{
@@ -125,7 +125,7 @@ void phy_RF8256_Config_ParaFile(struct net_device *dev)
 	for(eRFPath = (RF90_RADIO_PATH_E)RF90_PATH_A; eRFPath <priv->NumTotalRFPath; eRFPath++)
 	{
 		if (!rtl8192_phy_CheckIsLegalRFPath(dev, eRFPath))
-				continue;
+			continue;
 
 		pPhyReg = &priv->PHYRegDef[eRFPath];
 
@@ -249,7 +249,7 @@ void PHY_SetRF8256CCKTxPower(struct net_device *dev, u8 powerlevel)
 		if(priv->CustomerID == RT_CID_819x_Netcore)
 			TxAGC = 0x22;
 		else
-		TxAGC += priv->CckPwEnl;
+			TxAGC += priv->CckPwEnl;
 	}
 
 	if(TxAGC > 0x24)
diff --git a/drivers/staging/rtl8192u/r8192U_core.c b/drivers/staging/rtl8192u/r8192U_core.c
index 1bb6143cdf0..24272c51bc9 100644
--- a/drivers/staging/rtl8192u/r8192U_core.c
+++ b/drivers/staging/rtl8192u/r8192U_core.c
@@ -97,10 +97,6 @@ double __extendsfdf2(float a)
 #include <linux/seq_file.h>
 // FIXME: check if 2.6.7 is ok
 
-#ifdef CONFIG_RTL8192_PM
-#include "r8192_pm.h"
-#endif
-
 #include "dot11d.h"
 //set here to open your trace code. //WB
 u32 rt_global_debug_component = COMP_DOWN	|
@@ -159,13 +155,8 @@ static struct usb_driver rtl8192_usb_driver = {
 	.id_table	= rtl8192_usb_id_tbl,		  /* PCI_ID table  */
 	.probe		= rtl8192_usb_probe,		  /* probe fn      */
 	.disconnect	= rtl8192_usb_disconnect,	  /* remove fn     */
-#ifdef CONFIG_RTL8192_PM
-	.suspend	= rtl8192_suspend,		  /* PM suspend fn */
-	.resume		= rtl8192_resume,                 /* PM resume fn  */
-#else
 	.suspend	= NULL,				  /* PM suspend fn */
 	.resume		= NULL,				  /* PM resume fn  */
-#endif
 };
 
 
@@ -2045,9 +2036,6 @@ static void rtl8192_qos_activate(struct work_struct *work)
 	u32 u4bAcParam;
 	int i;
 
-	if (priv == NULL)
-		return;
-
 	mutex_lock(&priv->mutex);
 	if (priv->ieee80211->state != IEEE80211_LINKED)
 		goto success;
@@ -4981,12 +4969,8 @@ static int rtl8192_usb_probe(struct usb_interface *intf,
 
 	dev->netdev_ops = &rtl8192_netdev_ops;
 
-#if WIRELESS_EXT >= 12
-#if WIRELESS_EXT < 17
-	dev->get_wireless_stats = r8192_get_wireless_stats;
-#endif
 	dev->wireless_handlers = (struct iw_handler_def *) &r8192_wx_handlers_def;
-#endif
+
 	dev->type = ARPHRD_ETHER;
 
 	dev->watchdog_timeo = HZ*3;	//modified by john, 0805
diff --git a/drivers/staging/rtl8192u/r8192U_dm.c b/drivers/staging/rtl8192u/r8192U_dm.c
index d97ad7b909b..b4264e1353c 100644
--- a/drivers/staging/rtl8192u/r8192U_dm.c
+++ b/drivers/staging/rtl8192u/r8192U_dm.c
@@ -183,9 +183,8 @@ void dm_CheckRxAggregation(struct net_device *dev) {
 	curTxOkCnt = priv->stats.txbytesunicast - lastTxOkCnt;
 	curRxOkCnt = priv->stats.rxbytesunicast - lastRxOkCnt;
 
-	if((curTxOkCnt + curRxOkCnt) < 15000000) {
+	if ((curTxOkCnt + curRxOkCnt) < 15000000)
 		return;
-	}
 
 	if(curTxOkCnt > 4*curRxOkCnt) {
 		if (priv->bCurrentRxAggrEnable) {
@@ -1646,9 +1645,8 @@ void dm_restore_dynamic_mechanism_state(struct net_device *dev)
 			write_nic_byte(dev, UFWP, 1);
 	}
 	//Restore TX Power Tracking Index
-	if(priv->btxpower_trackingInit && priv->btxpower_tracking){
+	if (priv->btxpower_trackingInit && priv->btxpower_tracking)
 		dm_txpower_reset_recovery(dev);
-	}
 
 	//
 	//Restore BB Initial Gain
@@ -2525,7 +2523,7 @@ static void dm_check_edca_turbo(
 	if(priv->ieee80211->pHTInfo->IOTAction & HT_IOT_ACT_DISABLE_EDCA_TURBO)
 		goto dm_CheckEdcaTurbo_EXIT;
 
-//	printk("========>%s():bis_any_nonbepkts is %d\n",__FUNCTION__,priv->bis_any_nonbepkts);
+//	printk("========>%s():bis_any_nonbepkts is %d\n",__func__,priv->bis_any_nonbepkts);
 	// Check the status for current condition.
 	if(!priv->ieee80211->bis_any_nonbepkts)
 	{
@@ -3185,7 +3183,7 @@ void dm_fsync_timer_callback(unsigned long data)
 
 static void dm_StartHWFsync(struct net_device *dev)
 {
-	RT_TRACE(COMP_HALDM, "%s\n", __FUNCTION__);
+	RT_TRACE(COMP_HALDM, "%s\n", __func__);
 	write_nic_dword(dev, rOFDM0_RxDetector2, 0x465c12cf);
 	write_nic_byte(dev, 0xc3b, 0x41);
 }
@@ -3194,7 +3192,7 @@ static void dm_EndSWFsync(struct net_device *dev)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
 
-	RT_TRACE(COMP_HALDM, "%s\n", __FUNCTION__);
+	RT_TRACE(COMP_HALDM, "%s\n", __func__);
 	del_timer_sync(&(priv->fsync_timer));
 
 	// Let Register return to default value;
@@ -3218,7 +3216,7 @@ static void dm_StartSWFsync(struct net_device *dev)
 	u32			rateIndex;
 	u32			rateBitmap;
 
-	RT_TRACE(COMP_HALDM,"%s\n", __FUNCTION__);
+	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.
@@ -3253,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", __FUNCTION__);
+	RT_TRACE(COMP_HALDM,"%s\n", __func__);
 	write_nic_dword(dev, rOFDM0_RxDetector2, 0x465c52cd);
 	write_nic_byte(dev, 0xc3b, 0x49);
 
@@ -3485,7 +3483,7 @@ static void dm_dynamic_txpower(struct net_device *dev)
 		txlowpower_threshold = TX_POWER_NEAR_FIELD_THRESH_LOW;
 	}
 
-//	printk("=======>%s(): txhipower_threshhold is %d,txlowpower_threshold is %d\n",__FUNCTION__,txhipower_threshhold,txlowpower_threshold);
+//	printk("=======>%s(): txhipower_threshhold is %d,txlowpower_threshold is %d\n",__func__,txhipower_threshhold,txlowpower_threshold);
 	RT_TRACE(COMP_TXAGC,"priv->undecorated_smoothed_pwdb = %ld \n" , priv->undecorated_smoothed_pwdb);
 
 	if(priv->ieee80211->state == IEEE80211_LINKED)
diff --git a/drivers/staging/rtl8192u/r8192U_wx.c b/drivers/staging/rtl8192u/r8192U_wx.c
index c70af014a31..675a12d6aa8 100644
--- a/drivers/staging/rtl8192u/r8192U_wx.c
+++ b/drivers/staging/rtl8192u/r8192U_wx.c
@@ -24,8 +24,8 @@
 #include "dot11d.h"
 
 #define RATE_COUNT 12
-u32 rtl8180_rates[] = {1000000,2000000,5500000,11000000,
-	6000000,9000000,12000000,18000000,24000000,36000000,48000000,54000000};
+u32 rtl8180_rates[] = {1000000, 2000000, 5500000, 11000000,
+	6000000, 9000000, 12000000, 18000000, 24000000, 36000000, 48000000, 54000000};
 
 
 #ifndef ENETDOWN
@@ -38,16 +38,16 @@ static int r8192_wx_get_freq(struct net_device *dev,
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
 
-	return ieee80211_wx_get_freq(priv->ieee80211,a,wrqu,b);
+	return ieee80211_wx_get_freq(priv->ieee80211, a, wrqu, b);
 }
 
 
 static int r8192_wx_get_mode(struct net_device *dev, struct iw_request_info *a,
 			     union iwreq_data *wrqu, char *b)
 {
-	struct r8192_priv *priv=ieee80211_priv(dev);
+	struct r8192_priv *priv = ieee80211_priv(dev);
 
-	return ieee80211_wx_get_mode(priv->ieee80211,a,wrqu,b);
+	return ieee80211_wx_get_mode(priv->ieee80211, a, wrqu, b);
 }
 
 
@@ -57,7 +57,7 @@ static int r8192_wx_get_rate(struct net_device *dev,
 			     union iwreq_data *wrqu, char *extra)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
-	return ieee80211_wx_get_rate(priv->ieee80211,info,wrqu,extra);
+	return ieee80211_wx_get_rate(priv->ieee80211, info, wrqu, extra);
 }
 
 
@@ -71,7 +71,7 @@ static int r8192_wx_set_rate(struct net_device *dev,
 
 	down(&priv->wx_sem);
 
-	ret = ieee80211_wx_set_rate(priv->ieee80211,info,wrqu,extra);
+	ret = ieee80211_wx_set_rate(priv->ieee80211, info, wrqu, extra);
 
 	up(&priv->wx_sem);
 
@@ -88,7 +88,7 @@ static int r8192_wx_set_rts(struct net_device *dev,
 
 	down(&priv->wx_sem);
 
-	ret = ieee80211_wx_set_rts(priv->ieee80211,info,wrqu,extra);
+	ret = ieee80211_wx_set_rts(priv->ieee80211, info, wrqu, extra);
 
 	up(&priv->wx_sem);
 
@@ -100,7 +100,7 @@ static int r8192_wx_get_rts(struct net_device *dev,
 			     union iwreq_data *wrqu, char *extra)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
-	return ieee80211_wx_get_rts(priv->ieee80211,info,wrqu,extra);
+	return ieee80211_wx_get_rts(priv->ieee80211, info, wrqu, extra);
 }
 
 static int r8192_wx_set_power(struct net_device *dev,
@@ -112,7 +112,7 @@ static int r8192_wx_set_power(struct net_device *dev,
 
 	down(&priv->wx_sem);
 
-	ret = ieee80211_wx_set_power(priv->ieee80211,info,wrqu,extra);
+	ret = ieee80211_wx_set_power(priv->ieee80211, info, wrqu, extra);
 
 	up(&priv->wx_sem);
 
@@ -124,7 +124,7 @@ static int r8192_wx_get_power(struct net_device *dev,
 			     union iwreq_data *wrqu, char *extra)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
-	return ieee80211_wx_get_power(priv->ieee80211,info,wrqu,extra);
+	return ieee80211_wx_get_power(priv->ieee80211, info, wrqu, extra);
 }
 
 static int r8192_wx_force_reset(struct net_device *dev,
@@ -135,7 +135,7 @@ static int r8192_wx_force_reset(struct net_device *dev,
 
 	down(&priv->wx_sem);
 
-	printk("%s(): force reset ! extra is %d\n",__FUNCTION__, *extra);
+	printk("%s(): force reset ! extra is %d\n", __func__, *extra);
 	priv->force_reset = *extra;
 	up(&priv->wx_sem);
 	return 0;
@@ -171,15 +171,15 @@ static int r8192_wx_set_crcmon(struct net_device *dev,
 
 	down(&priv->wx_sem);
 
-	if(enable)
-		priv->crcmon=1;
+	if (enable)
+		priv->crcmon = 1;
 	else
-		priv->crcmon=0;
+		priv->crcmon = 0;
 
 	DMESG("bad CRC in monitor mode are %s",
 	      priv->crcmon ? "accepted" : "rejected");
 
-	if(prev != priv->crcmon && priv->up){
+	if (prev != priv->crcmon && priv->up) {
 		//rtl8180_down(dev);
 		//rtl8180_up(dev);
 	}
@@ -196,7 +196,7 @@ static int r8192_wx_set_mode(struct net_device *dev, struct iw_request_info *a,
 	int ret;
 	down(&priv->wx_sem);
 
-	ret = ieee80211_wx_set_mode(priv->ieee80211,a,wrqu,b);
+	ret = ieee80211_wx_set_mode(priv->ieee80211, a, wrqu, b);
 
 	rtl8192_set_rxconf(dev);
 
@@ -258,7 +258,7 @@ static int rtl8180_wx_get_range(struct net_device *dev,
 //	range->old_num_channels;
 //	range->old_num_frequency;
 //	range->old_freq[6]; /* Filler to keep "version" at the same offset */
-	if(priv->rf_set_sens != NULL)
+	if (priv->rf_set_sens != NULL)
 		range->sensitivity = priv->max_sens;	/* signal level threshold range */
 
 	range->max_qual.qual = 100;
@@ -275,14 +275,13 @@ static int rtl8180_wx_get_range(struct net_device *dev,
 
 	range->num_bitrates = RATE_COUNT;
 
-	for (i = 0; i < RATE_COUNT && i < IW_MAX_BITRATES; i++) {
+	for (i = 0; i < RATE_COUNT && i < IW_MAX_BITRATES; i++)
 		range->bitrate[i] = rtl8180_rates[i];
-	}
 
 	range->min_frag = MIN_FRAG_THRESHOLD;
 	range->max_frag = MAX_FRAG_THRESHOLD;
 
-	range->min_pmp=0;
+	range->min_pmp = 0;
 	range->max_pmp = 5000000;
 	range->min_pmt = 0;
 	range->max_pmt = 65535*1000;
@@ -316,14 +315,12 @@ static int rtl8180_wx_get_range(struct net_device *dev,
 		}
 
 		if (val == IW_MAX_FREQUENCIES)
-		break;
+			break;
 	}
 	range->num_frequency = val;
 	range->num_channels = val;
-#if WIRELESS_EXT > 17
 	range->enc_capa = IW_ENC_CAPA_WPA|IW_ENC_CAPA_WPA2|
 			  IW_ENC_CAPA_CIPHER_TKIP|IW_ENC_CAPA_CIPHER_CCMP;
-#endif
 	tmp->scan_capa = 0x01;
 	return 0;
 }
@@ -336,15 +333,14 @@ static int r8192_wx_set_scan(struct net_device *dev, struct iw_request_info *a,
 	struct ieee80211_device *ieee = priv->ieee80211;
 	int ret = 0;
 
-	if(!priv->up) return -ENETDOWN;
+	if (!priv->up)
+		return -ENETDOWN;
 
 	if (priv->ieee80211->LinkDetectInfo.bBusyTraffic == true)
 		return -EAGAIN;
-	if (wrqu->data.flags & IW_SCAN_THIS_ESSID)
-	{
+	if (wrqu->data.flags & IW_SCAN_THIS_ESSID) {
 		struct iw_scan_req *req = (struct iw_scan_req *)b;
-		if (req->essid_len)
-		{
+		if (req->essid_len) {
 			//printk("==**&*&*&**===>scan set ssid:%s\n", req->essid);
 			ieee->current_network.ssid_len = req->essid_len;
 			memcpy(ieee->current_network.ssid, req->essid, req->essid_len);
@@ -353,13 +349,13 @@ static int r8192_wx_set_scan(struct net_device *dev, struct iw_request_info *a,
 	}
 
 	down(&priv->wx_sem);
-	if(priv->ieee80211->state != IEEE80211_LINKED){
+	if (priv->ieee80211->state != IEEE80211_LINKED) {
 		priv->ieee80211->scanning = 0;
 		ieee80211_softmac_scan_syncro(priv->ieee80211);
 		ret = 0;
+	} else {
+		ret = ieee80211_wx_set_scan(priv->ieee80211, a, wrqu, b);
 	}
-	else
-	ret = ieee80211_wx_set_scan(priv->ieee80211,a,wrqu,b);
 	up(&priv->wx_sem);
 	return ret;
 }
@@ -372,11 +368,12 @@ static int r8192_wx_get_scan(struct net_device *dev, struct iw_request_info *a,
 	int ret;
 	struct r8192_priv *priv = ieee80211_priv(dev);
 
-	if(!priv->up) return -ENETDOWN;
+	if (!priv->up)
+		return -ENETDOWN;
 
 	down(&priv->wx_sem);
 
-	ret = ieee80211_wx_get_scan(priv->ieee80211,a,wrqu,b);
+	ret = ieee80211_wx_get_scan(priv->ieee80211, a, wrqu, b);
 
 	up(&priv->wx_sem);
 
@@ -391,7 +388,7 @@ static int r8192_wx_set_essid(struct net_device *dev,
 	int ret;
 	down(&priv->wx_sem);
 
-	ret = ieee80211_wx_set_essid(priv->ieee80211,a,wrqu,b);
+	ret = ieee80211_wx_set_essid(priv->ieee80211, a, wrqu, b);
 
 	up(&priv->wx_sem);
 
@@ -486,7 +483,7 @@ static int r8192_wx_set_wap(struct net_device *dev,
 //        struct sockaddr *temp = (struct sockaddr *)awrq;
 	down(&priv->wx_sem);
 
-	ret = ieee80211_wx_set_wap(priv->ieee80211,info,awrq,extra);
+	ret = ieee80211_wx_set_wap(priv->ieee80211, info, awrq, extra);
 
 	up(&priv->wx_sem);
 
@@ -501,7 +498,7 @@ static int r8192_wx_get_wap(struct net_device *dev,
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
 
-	return ieee80211_wx_get_wap(priv->ieee80211,info,wrqu,extra);
+	return ieee80211_wx_get_wap(priv->ieee80211, info, wrqu, extra);
 }
 
 
@@ -523,34 +520,37 @@ static int r8192_wx_set_enc(struct net_device *dev,
 	int ret;
 
 	//u32 TargetContent;
-	u32 hwkey[4]={0,0,0,0};
-	u8 mask=0xff;
-	u32 key_idx=0;
+	u32 hwkey[4] = {0, 0, 0, 0};
+	u8 mask = 0xff;
+	u32 key_idx = 0;
 	//u8 broadcast_addr[6] ={	0xff,0xff,0xff,0xff,0xff,0xff};
-	u8 zero_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} };
+	u8 zero_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} };
 	int i;
 
-       if(!priv->up) return -ENETDOWN;
+	if (!priv->up)
+		return -ENETDOWN;
 
 	down(&priv->wx_sem);
 
 	RT_TRACE(COMP_SEC, "Setting SW wep key");
-	ret = ieee80211_wx_set_encode(priv->ieee80211,info,wrqu,key);
+	ret = ieee80211_wx_set_encode(priv->ieee80211, info, wrqu, key);
 
 	up(&priv->wx_sem);
 
 
 
 	//sometimes, the length is zero while we do not type key value
-	if(wrqu->encoding.length!=0){
+	if (wrqu->encoding.length != 0) {
 
-		for(i=0 ; i<4 ; i++){
+		for (i = 0; i < 4; i++) {
 			hwkey[i] |=  key[4*i+0]&mask;
-			if(i==1&&(4*i+1)==wrqu->encoding.length) mask=0x00;
-			if(i==3&&(4*i+1)==wrqu->encoding.length) mask=0x00;
+			if (i == 1 && (4*i+1) == wrqu->encoding.length)
+				mask = 0x00;
+			if (i == 3 && (4*i+1) == wrqu->encoding.length)
+				mask = 0x00;
 			hwkey[i] |= (key[4*i+1]&mask)<<8;
 			hwkey[i] |= (key[4*i+2]&mask)<<16;
 			hwkey[i] |= (key[4*i+3]&mask)<<24;
@@ -559,20 +559,31 @@ static int r8192_wx_set_enc(struct net_device *dev,
 		#define CONF_WEP40  0x4
 		#define CONF_WEP104 0x14
 
-		switch (wrqu->encoding.flags & IW_ENCODE_INDEX){
-		case 0: key_idx = ieee->tx_keyidx; break;
-		case 1:	key_idx = 0; break;
-		case 2:	key_idx = 1; break;
-		case 3:	key_idx = 2; break;
-		case 4:	key_idx	= 3; break;
-		default: break;
+		switch (wrqu->encoding.flags & IW_ENCODE_INDEX) {
+		case 0:
+			key_idx = ieee->tx_keyidx;
+			break;
+		case 1:
+			key_idx = 0;
+			break;
+		case 2:
+			key_idx = 1;
+			break;
+		case 3:
+			key_idx = 2;
+			break;
+		case 4:
+			key_idx	= 3;
+			break;
+		default:
+			break;
 		}
 
-		if(wrqu->encoding.length==0x5){
+		if (wrqu->encoding.length == 0x5) {
 				ieee->pairwise_key_type = KEY_TYPE_WEP40;
 			EnableHWSecurityConfig8192(dev);
 
-			setKey( dev,
+			setKey(dev,
 				key_idx,                //EntryNo
 				key_idx,                //KeyIndex
 				KEY_TYPE_WEP40,         //KeyType
@@ -582,11 +593,11 @@ static int r8192_wx_set_enc(struct net_device *dev,
 
 		}
 
-		else if(wrqu->encoding.length==0xd){
+		else if (wrqu->encoding.length == 0xd) {
 				ieee->pairwise_key_type = KEY_TYPE_WEP104;
 				EnableHWSecurityConfig8192(dev);
 
-			setKey( dev,
+			setKey(dev,
 				key_idx,                //EntryNo
 				key_idx,                //KeyIndex
 				KEY_TYPE_WEP104,        //KeyType
@@ -594,8 +605,9 @@ static int r8192_wx_set_enc(struct net_device *dev,
 				0,                      //DefaultKey
 				hwkey);                 //KeyContent
 
+		} else {
+			printk("wrong type in WEP, not WEP40 and WEP104\n");
 		}
-		else printk("wrong type in WEP, not WEP40 and WEP104\n");
 
 	}
 
@@ -603,12 +615,13 @@ static int r8192_wx_set_enc(struct net_device *dev,
 }
 
 
-static int r8192_wx_set_scan_type(struct net_device *dev, struct iw_request_info *aa, union
- iwreq_data *wrqu, char *p){
+static int r8192_wx_set_scan_type(struct net_device *dev, struct iw_request_info *aa,
+					union iwreq_data *wrqu, char *p)
+{
 
 	struct r8192_priv *priv = ieee80211_priv(dev);
-	int *parms=(int *)p;
-	int mode=parms[0];
+	int *parms = (int *)p;
+	int mode = parms[0];
 
 	priv->ieee80211->active_scan = mode;
 
@@ -631,20 +644,20 @@ static int r8192_wx_set_retry(struct net_device *dev,
 		err = -EINVAL;
 		goto exit;
 	}
-	if (!(wrqu->retry.flags & IW_RETRY_LIMIT)){
+	if (!(wrqu->retry.flags & IW_RETRY_LIMIT)) {
 		err = -EINVAL;
 		goto exit;
 	}
 
-	if(wrqu->retry.value > R8180_MAX_RETRY){
-		err= -EINVAL;
+	if (wrqu->retry.value > R8180_MAX_RETRY) {
+		err = -EINVAL;
 		goto exit;
 	}
 	if (wrqu->retry.flags & IW_RETRY_MAX) {
 		priv->retry_rts = wrqu->retry.value;
 		DMESG("Setting retry for RTS/CTS data to %d", wrqu->retry.value);
 
-	}else {
+	} else {
 		priv->retry_data = wrqu->retry.value;
 		DMESG("Setting retry for non RTS/CTS data to %d", wrqu->retry.value);
 	}
@@ -701,7 +714,7 @@ static int r8192_wx_get_sens(struct net_device *dev,
 				union iwreq_data *wrqu, char *extra)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
-	if(priv->rf_set_sens == NULL)
+	if (priv->rf_set_sens == NULL)
 		return -1; /* we have not this support for this radio */
 	wrqu->sens.value = priv->sens;
 	return 0;
@@ -718,14 +731,14 @@ static int r8192_wx_set_sens(struct net_device *dev,
 	short err = 0;
 	down(&priv->wx_sem);
 	//DMESG("attempt to set sensivity to %ddb",wrqu->sens.value);
-	if(priv->rf_set_sens == NULL) {
-		err= -1; /* we have not this support for this radio */
+	if (priv->rf_set_sens == NULL) {
+		err = -1; /* we have not this support for this radio */
 		goto exit;
 	}
-	if(priv->rf_set_sens(dev, wrqu->sens.value) == 0)
+	if (priv->rf_set_sens(dev, wrqu->sens.value) == 0)
 		priv->sens = wrqu->sens.value;
 	else
-		err= -EINVAL;
+		err = -EINVAL;
 
 exit:
 	up(&priv->wx_sem);
@@ -733,72 +746,66 @@ exit:
 	return err;
 }
 
-#if (WIRELESS_EXT >= 18)
 //hw security need to reorganized.
 static int r8192_wx_set_enc_ext(struct net_device *dev,
 					struct iw_request_info *info,
 					union iwreq_data *wrqu, char *extra)
 {
-	int ret=0;
+	int ret = 0;
 	struct r8192_priv *priv = ieee80211_priv(dev);
 	struct ieee80211_device *ieee = priv->ieee80211;
-	//printk("===>%s()\n", __FUNCTION__);
+	//printk("===>%s()\n", __func__);
 
 
 	down(&priv->wx_sem);
 	ret = ieee80211_wx_set_encode_ext(priv->ieee80211, info, wrqu, extra);
 
 	{
-		u8 broadcast_addr[6] = {0xff,0xff,0xff,0xff,0xff,0xff};
+		u8 broadcast_addr[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 		u8 zero[6] = {0};
 		u32 key[4] = {0};
 		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
+		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;
 
-		alg =  (ext->alg == IW_ENCODE_ALG_CCMP)?KEY_TYPE_CCMP:ext->alg; // as IW_ENCODE_ALG_CCMP is defined to be 3 and KEY_TYPE_CCMP is defined to 4;
+		// as IW_ENCODE_ALG_CCMP is defined to be 3 and KEY_TYPE_CCMP is defined to 4;
+		alg =  (ext->alg == IW_ENCODE_ALG_CCMP)?KEY_TYPE_CCMP:ext->alg;
 		idx = encoding->flags & IW_ENCODE_INDEX;
 		if (idx)
-			idx --;
+			idx--;
 		group = ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY;
 
-		if ((!group) || (IW_MODE_ADHOC == ieee->iw_mode) || (alg ==  KEY_TYPE_WEP40))
-		{
-			if ((ext->key_len == 13) && (alg == KEY_TYPE_WEP40) )
+		if ((!group) || (IW_MODE_ADHOC == ieee->iw_mode) || (alg ==  KEY_TYPE_WEP40)) {
+			if ((ext->key_len == 13) && (alg == KEY_TYPE_WEP40))
 				alg = KEY_TYPE_WEP104;
 			ieee->pairwise_key_type = alg;
 			EnableHWSecurityConfig8192(dev);
 		}
 		memcpy((u8 *)key, ext->key, 16); //we only get 16 bytes key.why? WB 2008.7.1
 
-		if ((alg & KEY_TYPE_WEP40) && (ieee->auth_mode !=2) )
-		{
+		if ((alg & KEY_TYPE_WEP40) && (ieee->auth_mode != 2)) {
 
-			setKey( dev,
+			setKey(dev,
 					idx,//EntryNo
 					idx, //KeyIndex
 					alg,  //KeyType
 					zero, //MacAddr
 					0,              //DefaultKey
 					key);           //KeyContent
-		}
-		else if (group)
-		{
+		} else if (group) {
 			ieee->group_key_type = alg;
-			setKey( dev,
+			setKey(dev,
 					idx,//EntryNo
 					idx, //KeyIndex
 					alg,  //KeyType
 					broadcast_addr, //MacAddr
 					0,              //DefaultKey
 					key);           //KeyContent
-		}
-		else //pairwise key
-		{
-			setKey( dev,
+		} else {//pairwise key
+			setKey(dev,
 					4,//EntryNo
 					idx, //KeyIndex
 					alg,  //KeyType
@@ -820,8 +827,8 @@ static int r8192_wx_set_auth(struct net_device *dev,
 					struct iw_request_info *info,
 					union iwreq_data *data, char *extra)
 {
-	int ret=0;
-	//printk("====>%s()\n", __FUNCTION__);
+	int ret = 0;
+	//printk("====>%s()\n", __func__);
 	struct r8192_priv *priv = ieee80211_priv(dev);
 	down(&priv->wx_sem);
 	ret = ieee80211_wx_set_auth(priv->ieee80211, info, &(data->param), extra);
@@ -833,9 +840,9 @@ static int r8192_wx_set_mlme(struct net_device *dev,
 					struct iw_request_info *info,
 					union iwreq_data *wrqu, char *extra)
 {
-	//printk("====>%s()\n", __FUNCTION__);
+	//printk("====>%s()\n", __func__);
 
-	int ret=0;
+	int ret = 0;
 	struct r8192_priv *priv = ieee80211_priv(dev);
 	down(&priv->wx_sem);
 	ret = ieee80211_wx_set_mlme(priv->ieee80211, info, wrqu, extra);
@@ -843,32 +850,31 @@ static int r8192_wx_set_mlme(struct net_device *dev,
 	up(&priv->wx_sem);
 	return ret;
 }
-#endif
+
 static int r8192_wx_set_gen_ie(struct net_device *dev,
 					struct iw_request_info *info,
 					union iwreq_data *data, char *extra)
 {
-	   //printk("====>%s(), len:%d\n", __FUNCTION__, data->length);
-	int ret=0;
+	   //printk("====>%s(), len:%d\n", __func__, data->length);
+	int ret = 0;
 	struct r8192_priv *priv = ieee80211_priv(dev);
 	down(&priv->wx_sem);
 	ret = ieee80211_wx_set_gen_ie(priv->ieee80211, extra, data->data.length);
 	up(&priv->wx_sem);
-	//printk("<======%s(), ret:%d\n", __FUNCTION__, ret);
+	//printk("<======%s(), ret:%d\n", __func__, ret);
 	return ret;
 
 
 }
 
 static int dummy(struct net_device *dev, struct iw_request_info *a,
-		 union iwreq_data *wrqu,char *b)
+		 union iwreq_data *wrqu, char *b)
 {
 	return -1;
 }
 
 
-static iw_handler r8192_wx_handlers[] =
-{
+static iw_handler r8192_wx_handlers[] = {
 	NULL,                     /* SIOCSIWCOMMIT */
 	r8192_wx_get_name,	  /* SIOCGIWNAME */
 	dummy,                    /* SIOCSIWNWID */
@@ -891,11 +897,7 @@ static iw_handler r8192_wx_handlers[] =
 	NULL,                     /* SIOCWIWTHRSPY */
 	r8192_wx_set_wap,	  /* SIOCSIWAP */
 	r8192_wx_get_wap,         /* SIOCGIWAP */
-#if (WIRELESS_EXT >= 18)
 	r8192_wx_set_mlme,                     /* MLME-- */
-#else
-	 NULL,
-#endif
 	dummy,                     /* SIOCGIWAPLIST -- deprecated */
 	r8192_wx_set_scan,        /* SIOCSIWSCAN */
 	r8192_wx_get_scan,        /* SIOCGIWSCAN */
@@ -924,17 +926,10 @@ static iw_handler r8192_wx_handlers[] =
 	r8192_wx_set_gen_ie,//NULL,			/* SIOCSIWGENIE */
 	NULL,			/* SIOCSIWGENIE */
 
-#if (WIRELESS_EXT >= 18)
 	r8192_wx_set_auth,//NULL,			/* SIOCSIWAUTH */
 	NULL,//r8192_wx_get_auth,//NULL,			/* SIOCSIWAUTH */
 	r8192_wx_set_enc_ext,			/* SIOCSIWENCODEEXT */
 	NULL,//r8192_wx_get_enc_ext,//NULL,			/* SIOCSIWENCODEEXT */
-#else
-	NULL,
-	NULL,
-	NULL,
-	NULL,
-#endif
 	NULL,			/* SIOCSIWPMKSA */
 	NULL,			 /*---hole---*/
 
@@ -978,17 +973,15 @@ static iw_handler r8192_private_handler[] = {
 	r8192_wx_force_reset,
 };
 
-//#if WIRELESS_EXT >= 17
 struct iw_statistics *r8192_get_wireless_stats(struct net_device *dev)
 {
-       struct r8192_priv *priv = ieee80211_priv(dev);
+	struct r8192_priv *priv = ieee80211_priv(dev);
 	struct ieee80211_device *ieee = priv->ieee80211;
 	struct iw_statistics *wstats = &priv->wstats;
 	int tmp_level = 0;
 	int tmp_qual = 0;
 	int tmp_noise = 0;
-	if(ieee->state < IEEE80211_LINKED)
-	{
+	if (ieee->state < IEEE80211_LINKED) {
 		wstats->qual.qual = 0;
 		wstats->qual.level = 0;
 		wstats->qual.noise = 0;
@@ -996,7 +989,7 @@ struct iw_statistics *r8192_get_wireless_stats(struct net_device *dev)
 		return wstats;
 	}
 
-       tmp_level = (&ieee->current_network)->stats.rssi;
+	tmp_level = (&ieee->current_network)->stats.rssi;
 	tmp_qual = (&ieee->current_network)->stats.signal;
 	tmp_noise = (&ieee->current_network)->stats.noise;
 	//printk("level:%d, qual:%d, noise:%d\n", tmp_level, tmp_qual, tmp_noise);
@@ -1007,17 +1000,14 @@ struct iw_statistics *r8192_get_wireless_stats(struct net_device *dev)
 	wstats->qual.updated = IW_QUAL_ALL_UPDATED| IW_QUAL_DBM;
 	return wstats;
 }
-//#endif
 
 
-struct iw_handler_def  r8192_wx_handlers_def={
+struct iw_handler_def  r8192_wx_handlers_def = {
 	.standard = r8192_wx_handlers,
 	.num_standard = ARRAY_SIZE(r8192_wx_handlers),
 	.private = r8192_private_handler,
 	.num_private = ARRAY_SIZE(r8192_private_handler),
 	.num_private_args = sizeof(r8192_private_args) / sizeof(struct iw_priv_args),
-#if WIRELESS_EXT >= 17
 	.get_wireless_stats = r8192_get_wireless_stats,
-#endif
 	.private_args = (struct iw_priv_args *)r8192_private_args,
 };
diff --git a/drivers/staging/rtl8192u/r819xU_cmdpkt.c b/drivers/staging/rtl8192u/r819xU_cmdpkt.c
index 723c8630e9e..545f49ec9c0 100644
--- a/drivers/staging/rtl8192u/r819xU_cmdpkt.c
+++ b/drivers/staging/rtl8192u/r819xU_cmdpkt.c
@@ -28,7 +28,6 @@
 
 rt_status SendTxCommandPacket(struct net_device *dev, void *pData, u32 DataLen)
 {
-	rt_status	rtStatus = RT_STATUS_SUCCESS;
 	struct r8192_priv   *priv = ieee80211_priv(dev);
 	struct sk_buff	    *skb;
 	cb_desc		    *tcb_desc;
@@ -58,7 +57,7 @@ rt_status SendTxCommandPacket(struct net_device *dev, void *pData, u32 DataLen)
 		priv->ieee80211->softmac_hard_start_xmit(skb, dev);
 	}
 
-	return rtStatus;
+	return RT_STATUS_SUCCESS;
 }
 
 /*-----------------------------------------------------------------------------
diff --git a/drivers/staging/rtl8192u/r819xU_firmware.c b/drivers/staging/rtl8192u/r819xU_firmware.c
index ecfb66538eb..04b8f5ec67e 100644
--- a/drivers/staging/rtl8192u/r819xU_firmware.c
+++ b/drivers/staging/rtl8192u/r819xU_firmware.c
@@ -162,7 +162,7 @@ static bool CPUcheck_maincodeok_turnonCPU(struct net_device *dev)
 	return rt_status;
 
 CPUCheckMainCodeOKAndTurnOnCPU_Fail:
-	RT_TRACE(COMP_ERR, "ERR in %s()\n", __FUNCTION__);
+	RT_TRACE(COMP_ERR, "ERR in %s()\n", __func__);
 	rt_status = FALSE;
 	return rt_status;
 }
@@ -191,7 +191,7 @@ static bool CPUcheck_firmware_ready(struct net_device *dev)
 	return rt_status;
 
 CPUCheckFirmwareReady_Fail:
-	RT_TRACE(COMP_ERR, "ERR in %s()\n", __FUNCTION__);
+	RT_TRACE(COMP_ERR, "ERR in %s()\n", __func__);
 	rt_status = false;
 	return rt_status;
 
@@ -277,13 +277,11 @@ bool init_firmware(struct net_device *dev)
 		 *   and Tx descriptor info
 		 * */
 		rt_status = fw_download_code(dev,mapped_file,file_length);
-		if (rst_opt == OPT_SYSTEM_RESET) {
+		if (rst_opt == OPT_SYSTEM_RESET)
 			release_firmware(fw_entry);
-		}
 
-		if (rt_status != TRUE) {
+		if (rt_status != TRUE)
 			goto download_firmware_fail;
-		}
 
 		switch (init_step) {
 		case FW_INIT_STEP0_BOOT:
@@ -336,7 +334,7 @@ bool init_firmware(struct net_device *dev)
 	return rt_status;
 
 download_firmware_fail:
-	RT_TRACE(COMP_ERR, "ERR in %s()\n", __FUNCTION__);
+	RT_TRACE(COMP_ERR, "ERR in %s()\n", __func__);
 	rt_status = FALSE;
 	return rt_status;
 
diff --git a/drivers/staging/rtl8192u/r819xU_firmware_img.c b/drivers/staging/rtl8192u/r819xU_firmware_img.c
index 0785de72a5e..4eb43cfe569 100644
--- a/drivers/staging/rtl8192u/r819xU_firmware_img.c
+++ b/drivers/staging/rtl8192u/r819xU_firmware_img.c
@@ -3,546 +3,546 @@
 #include "r819xU_firmware_img.h"
 
 u32 Rtl8192UsbPHY_REGArray[] = {
-0x0, };
+	0x0, };
 
 u32 Rtl8192UsbPHY_REG_1T2RArray[] = {
-0x800,0x00000000,
-0x804,0x00000001,
-0x808,0x0000fc00,
-0x80c,0x0000001c,
-0x810,0x801010aa,
-0x814,0x008514d0,
-0x818,0x00000040,
-0x81c,0x00000000,
-0x820,0x00000004,
-0x824,0x00690000,
-0x828,0x00000004,
-0x82c,0x00e90000,
-0x830,0x00000004,
-0x834,0x00690000,
-0x838,0x00000004,
-0x83c,0x00e90000,
-0x840,0x00000000,
-0x844,0x00000000,
-0x848,0x00000000,
-0x84c,0x00000000,
-0x850,0x00000000,
-0x854,0x00000000,
-0x858,0x65a965a9,
-0x85c,0x65a965a9,
-0x860,0x001f0010,
-0x864,0x007f0010,
-0x868,0x001f0010,
-0x86c,0x007f0010,
-0x870,0x0f100f70,
-0x874,0x0f100f70,
-0x878,0x00000000,
-0x87c,0x00000000,
-0x880,0x6870e36c,
-0x884,0xe3573600,
-0x888,0x4260c340,
-0x88c,0x0000ff00,
-0x890,0x00000000,
-0x894,0xfffffffe,
-0x898,0x4c42382f,
-0x89c,0x00656056,
-0x8b0,0x00000000,
-0x8e0,0x00000000,
-0x8e4,0x00000000,
-0x900,0x00000000,
-0x904,0x00000023,
-0x908,0x00000000,
-0x90c,0x31121311,
-0xa00,0x00d0c7d8,
-0xa04,0x811f0008,
-0xa08,0x80cd8300,
-0xa0c,0x2e62740f,
-0xa10,0x95009b78,
-0xa14,0x11145008,
-0xa18,0x00881117,
-0xa1c,0x89140fa0,
-0xa20,0x1a1b0000,
-0xa24,0x090e1317,
-0xa28,0x00000204,
-0xa2c,0x00000000,
-0xc00,0x00000040,
-0xc04,0x00005433,
-0xc08,0x000000e4,
-0xc0c,0x6c6c6c6c,
-0xc10,0x08800000,
-0xc14,0x40000100,
-0xc18,0x08000000,
-0xc1c,0x40000100,
-0xc20,0x08000000,
-0xc24,0x40000100,
-0xc28,0x08000000,
-0xc2c,0x40000100,
-0xc30,0x6de9ac44,
-0xc34,0x465c52cd,
-0xc38,0x497f5994,
-0xc3c,0x0a969764,
-0xc40,0x1f7c403f,
-0xc44,0x000100b7,
-0xc48,0xec020000,
-0xc4c,0x00000300,
-0xc50,0x69543420,
-0xc54,0x433c0094,
-0xc58,0x69543420,
-0xc5c,0x433c0094,
-0xc60,0x69543420,
-0xc64,0x433c0094,
-0xc68,0x69543420,
-0xc6c,0x433c0094,
-0xc70,0x2c7f000d,
-0xc74,0x0186175b,
-0xc78,0x0000001f,
-0xc7c,0x00b91612,
-0xc80,0x40000100,
-0xc84,0x20000000,
-0xc88,0x40000100,
-0xc8c,0x20200000,
-0xc90,0x40000100,
-0xc94,0x00000000,
-0xc98,0x40000100,
-0xc9c,0x00000000,
-0xca0,0x00492492,
-0xca4,0x00000000,
-0xca8,0x00000000,
-0xcac,0x00000000,
-0xcb0,0x00000000,
-0xcb4,0x00000000,
-0xcb8,0x00000000,
-0xcbc,0x00492492,
-0xcc0,0x00000000,
-0xcc4,0x00000000,
-0xcc8,0x00000000,
-0xccc,0x00000000,
-0xcd0,0x00000000,
-0xcd4,0x00000000,
-0xcd8,0x64b22427,
-0xcdc,0x00766932,
-0xce0,0x00222222,
-0xd00,0x00000750,
-0xd04,0x00000403,
-0xd08,0x0000907f,
-0xd0c,0x00000001,
-0xd10,0xa0633333,
-0xd14,0x33333c63,
-0xd18,0x6a8f5b6b,
-0xd1c,0x00000000,
-0xd20,0x00000000,
-0xd24,0x00000000,
-0xd28,0x00000000,
-0xd2c,0xcc979975,
-0xd30,0x00000000,
-0xd34,0x00000000,
-0xd38,0x00000000,
-0xd3c,0x00027293,
-0xd40,0x00000000,
-0xd44,0x00000000,
-0xd48,0x00000000,
-0xd4c,0x00000000,
-0xd50,0x6437140a,
-0xd54,0x024dbd02,
-0xd58,0x00000000,
-0xd5c,0x04032064,
-0xe00,0x161a1a1a,
-0xe04,0x12121416,
-0xe08,0x00001800,
-0xe0c,0x00000000,
-0xe10,0x161a1a1a,
-0xe14,0x12121416,
-0xe18,0x161a1a1a,
-0xe1c,0x12121416,
+	0x800, 0x00000000,
+	0x804, 0x00000001,
+	0x808, 0x0000fc00,
+	0x80c, 0x0000001c,
+	0x810, 0x801010aa,
+	0x814, 0x008514d0,
+	0x818, 0x00000040,
+	0x81c, 0x00000000,
+	0x820, 0x00000004,
+	0x824, 0x00690000,
+	0x828, 0x00000004,
+	0x82c, 0x00e90000,
+	0x830, 0x00000004,
+	0x834, 0x00690000,
+	0x838, 0x00000004,
+	0x83c, 0x00e90000,
+	0x840, 0x00000000,
+	0x844, 0x00000000,
+	0x848, 0x00000000,
+	0x84c, 0x00000000,
+	0x850, 0x00000000,
+	0x854, 0x00000000,
+	0x858, 0x65a965a9,
+	0x85c, 0x65a965a9,
+	0x860, 0x001f0010,
+	0x864, 0x007f0010,
+	0x868, 0x001f0010,
+	0x86c, 0x007f0010,
+	0x870, 0x0f100f70,
+	0x874, 0x0f100f70,
+	0x878, 0x00000000,
+	0x87c, 0x00000000,
+	0x880, 0x6870e36c,
+	0x884, 0xe3573600,
+	0x888, 0x4260c340,
+	0x88c, 0x0000ff00,
+	0x890, 0x00000000,
+	0x894, 0xfffffffe,
+	0x898, 0x4c42382f,
+	0x89c, 0x00656056,
+	0x8b0, 0x00000000,
+	0x8e0, 0x00000000,
+	0x8e4, 0x00000000,
+	0x900, 0x00000000,
+	0x904, 0x00000023,
+	0x908, 0x00000000,
+	0x90c, 0x31121311,
+	0xa00, 0x00d0c7d8,
+	0xa04, 0x811f0008,
+	0xa08, 0x80cd8300,
+	0xa0c, 0x2e62740f,
+	0xa10, 0x95009b78,
+	0xa14, 0x11145008,
+	0xa18, 0x00881117,
+	0xa1c, 0x89140fa0,
+	0xa20, 0x1a1b0000,
+	0xa24, 0x090e1317,
+	0xa28, 0x00000204,
+	0xa2c, 0x00000000,
+	0xc00, 0x00000040,
+	0xc04, 0x00005433,
+	0xc08, 0x000000e4,
+	0xc0c, 0x6c6c6c6c,
+	0xc10, 0x08800000,
+	0xc14, 0x40000100,
+	0xc18, 0x08000000,
+	0xc1c, 0x40000100,
+	0xc20, 0x08000000,
+	0xc24, 0x40000100,
+	0xc28, 0x08000000,
+	0xc2c, 0x40000100,
+	0xc30, 0x6de9ac44,
+	0xc34, 0x465c52cd,
+	0xc38, 0x497f5994,
+	0xc3c, 0x0a969764,
+	0xc40, 0x1f7c403f,
+	0xc44, 0x000100b7,
+	0xc48, 0xec020000,
+	0xc4c, 0x00000300,
+	0xc50, 0x69543420,
+	0xc54, 0x433c0094,
+	0xc58, 0x69543420,
+	0xc5c, 0x433c0094,
+	0xc60, 0x69543420,
+	0xc64, 0x433c0094,
+	0xc68, 0x69543420,
+	0xc6c, 0x433c0094,
+	0xc70, 0x2c7f000d,
+	0xc74, 0x0186175b,
+	0xc78, 0x0000001f,
+	0xc7c, 0x00b91612,
+	0xc80, 0x40000100,
+	0xc84, 0x20000000,
+	0xc88, 0x40000100,
+	0xc8c, 0x20200000,
+	0xc90, 0x40000100,
+	0xc94, 0x00000000,
+	0xc98, 0x40000100,
+	0xc9c, 0x00000000,
+	0xca0, 0x00492492,
+	0xca4, 0x00000000,
+	0xca8, 0x00000000,
+	0xcac, 0x00000000,
+	0xcb0, 0x00000000,
+	0xcb4, 0x00000000,
+	0xcb8, 0x00000000,
+	0xcbc, 0x00492492,
+	0xcc0, 0x00000000,
+	0xcc4, 0x00000000,
+	0xcc8, 0x00000000,
+	0xccc, 0x00000000,
+	0xcd0, 0x00000000,
+	0xcd4, 0x00000000,
+	0xcd8, 0x64b22427,
+	0xcdc, 0x00766932,
+	0xce0, 0x00222222,
+	0xd00, 0x00000750,
+	0xd04, 0x00000403,
+	0xd08, 0x0000907f,
+	0xd0c, 0x00000001,
+	0xd10, 0xa0633333,
+	0xd14, 0x33333c63,
+	0xd18, 0x6a8f5b6b,
+	0xd1c, 0x00000000,
+	0xd20, 0x00000000,
+	0xd24, 0x00000000,
+	0xd28, 0x00000000,
+	0xd2c, 0xcc979975,
+	0xd30, 0x00000000,
+	0xd34, 0x00000000,
+	0xd38, 0x00000000,
+	0xd3c, 0x00027293,
+	0xd40, 0x00000000,
+	0xd44, 0x00000000,
+	0xd48, 0x00000000,
+	0xd4c, 0x00000000,
+	0xd50, 0x6437140a,
+	0xd54, 0x024dbd02,
+	0xd58, 0x00000000,
+	0xd5c, 0x04032064,
+	0xe00, 0x161a1a1a,
+	0xe04, 0x12121416,
+	0xe08, 0x00001800,
+	0xe0c, 0x00000000,
+	0xe10, 0x161a1a1a,
+	0xe14, 0x12121416,
+	0xe18, 0x161a1a1a,
+	0xe1c, 0x12121416,
 };
 
 u32 Rtl8192UsbRadioA_Array[] = {
-0x019,0x00000003,
-0x000,0x000000bf,
-0x001,0x00000ee0,
-0x002,0x0000004c,
-0x003,0x000007f1,
-0x004,0x00000975,
-0x005,0x00000c58,
-0x006,0x00000ae6,
-0x007,0x000000ca,
-0x008,0x00000e1c,
-0x009,0x000007f0,
-0x00a,0x000009d0,
-0x00b,0x000001ba,
-0x00c,0x00000240,
-0x00e,0x00000020,
-0x00f,0x00000990,
-0x012,0x00000806,
-0x014,0x000005ab,
-0x015,0x00000f80,
-0x016,0x00000020,
-0x017,0x00000597,
-0x018,0x0000050a,
-0x01a,0x00000f80,
-0x01b,0x00000f5e,
-0x01c,0x00000008,
-0x01d,0x00000607,
-0x01e,0x000006cc,
-0x01f,0x00000000,
-0x020,0x000001a5,
-0x01f,0x00000001,
-0x020,0x00000165,
-0x01f,0x00000002,
-0x020,0x000000c6,
-0x01f,0x00000003,
-0x020,0x00000086,
-0x01f,0x00000004,
-0x020,0x00000046,
-0x01f,0x00000005,
-0x020,0x000001e6,
-0x01f,0x00000006,
-0x020,0x000001a6,
-0x01f,0x00000007,
-0x020,0x00000166,
-0x01f,0x00000008,
-0x020,0x000000c7,
-0x01f,0x00000009,
-0x020,0x00000087,
-0x01f,0x0000000a,
-0x020,0x000000f7,
-0x01f,0x0000000b,
-0x020,0x000000d7,
-0x01f,0x0000000c,
-0x020,0x000000b7,
-0x01f,0x0000000d,
-0x020,0x00000097,
-0x01f,0x0000000e,
-0x020,0x00000077,
-0x01f,0x0000000f,
-0x020,0x00000057,
-0x01f,0x00000010,
-0x020,0x00000037,
-0x01f,0x00000011,
-0x020,0x000000fb,
-0x01f,0x00000012,
-0x020,0x000000db,
-0x01f,0x00000013,
-0x020,0x000000bb,
-0x01f,0x00000014,
-0x020,0x000000ff,
-0x01f,0x00000015,
-0x020,0x000000e3,
-0x01f,0x00000016,
-0x020,0x000000c3,
-0x01f,0x00000017,
-0x020,0x000000a3,
-0x01f,0x00000018,
-0x020,0x00000083,
-0x01f,0x00000019,
-0x020,0x00000063,
-0x01f,0x0000001a,
-0x020,0x00000043,
-0x01f,0x0000001b,
-0x020,0x00000023,
-0x01f,0x0000001c,
-0x020,0x00000003,
-0x01f,0x0000001d,
-0x020,0x000001e3,
-0x01f,0x0000001e,
-0x020,0x000001c3,
-0x01f,0x0000001f,
-0x020,0x000001a3,
-0x01f,0x00000020,
-0x020,0x00000183,
-0x01f,0x00000021,
-0x020,0x00000163,
-0x01f,0x00000022,
-0x020,0x00000143,
-0x01f,0x00000023,
-0x020,0x00000123,
-0x01f,0x00000024,
-0x020,0x00000103,
-0x023,0x00000203,
-0x024,0x00000200,
-0x00b,0x000001ba,
-0x02c,0x000003d7,
-0x02d,0x00000ff0,
-0x000,0x00000037,
-0x004,0x00000160,
-0x007,0x00000080,
-0x002,0x0000088d,
-0x0fe,0x00000000,
-0x0fe,0x00000000,
-0x016,0x00000200,
-0x016,0x00000380,
-0x016,0x00000020,
-0x016,0x000001a0,
-0x000,0x000000bf,
-0x00d,0x0000001f,
-0x00d,0x00000c9f,
-0x002,0x0000004d,
-0x000,0x00000cbf,
-0x004,0x00000975,
-0x007,0x00000700,
+	0x019, 0x00000003,
+	0x000, 0x000000bf,
+	0x001, 0x00000ee0,
+	0x002, 0x0000004c,
+	0x003, 0x000007f1,
+	0x004, 0x00000975,
+	0x005, 0x00000c58,
+	0x006, 0x00000ae6,
+	0x007, 0x000000ca,
+	0x008, 0x00000e1c,
+	0x009, 0x000007f0,
+	0x00a, 0x000009d0,
+	0x00b, 0x000001ba,
+	0x00c, 0x00000240,
+	0x00e, 0x00000020,
+	0x00f, 0x00000990,
+	0x012, 0x00000806,
+	0x014, 0x000005ab,
+	0x015, 0x00000f80,
+	0x016, 0x00000020,
+	0x017, 0x00000597,
+	0x018, 0x0000050a,
+	0x01a, 0x00000f80,
+	0x01b, 0x00000f5e,
+	0x01c, 0x00000008,
+	0x01d, 0x00000607,
+	0x01e, 0x000006cc,
+	0x01f, 0x00000000,
+	0x020, 0x000001a5,
+	0x01f, 0x00000001,
+	0x020, 0x00000165,
+	0x01f, 0x00000002,
+	0x020, 0x000000c6,
+	0x01f, 0x00000003,
+	0x020, 0x00000086,
+	0x01f, 0x00000004,
+	0x020, 0x00000046,
+	0x01f, 0x00000005,
+	0x020, 0x000001e6,
+	0x01f, 0x00000006,
+	0x020, 0x000001a6,
+	0x01f, 0x00000007,
+	0x020, 0x00000166,
+	0x01f, 0x00000008,
+	0x020, 0x000000c7,
+	0x01f, 0x00000009,
+	0x020, 0x00000087,
+	0x01f, 0x0000000a,
+	0x020, 0x000000f7,
+	0x01f, 0x0000000b,
+	0x020, 0x000000d7,
+	0x01f, 0x0000000c,
+	0x020, 0x000000b7,
+	0x01f, 0x0000000d,
+	0x020, 0x00000097,
+	0x01f, 0x0000000e,
+	0x020, 0x00000077,
+	0x01f, 0x0000000f,
+	0x020, 0x00000057,
+	0x01f, 0x00000010,
+	0x020, 0x00000037,
+	0x01f, 0x00000011,
+	0x020, 0x000000fb,
+	0x01f, 0x00000012,
+	0x020, 0x000000db,
+	0x01f, 0x00000013,
+	0x020, 0x000000bb,
+	0x01f, 0x00000014,
+	0x020, 0x000000ff,
+	0x01f, 0x00000015,
+	0x020, 0x000000e3,
+	0x01f, 0x00000016,
+	0x020, 0x000000c3,
+	0x01f, 0x00000017,
+	0x020, 0x000000a3,
+	0x01f, 0x00000018,
+	0x020, 0x00000083,
+	0x01f, 0x00000019,
+	0x020, 0x00000063,
+	0x01f, 0x0000001a,
+	0x020, 0x00000043,
+	0x01f, 0x0000001b,
+	0x020, 0x00000023,
+	0x01f, 0x0000001c,
+	0x020, 0x00000003,
+	0x01f, 0x0000001d,
+	0x020, 0x000001e3,
+	0x01f, 0x0000001e,
+	0x020, 0x000001c3,
+	0x01f, 0x0000001f,
+	0x020, 0x000001a3,
+	0x01f, 0x00000020,
+	0x020, 0x00000183,
+	0x01f, 0x00000021,
+	0x020, 0x00000163,
+	0x01f, 0x00000022,
+	0x020, 0x00000143,
+	0x01f, 0x00000023,
+	0x020, 0x00000123,
+	0x01f, 0x00000024,
+	0x020, 0x00000103,
+	0x023, 0x00000203,
+	0x024, 0x00000200,
+	0x00b, 0x000001ba,
+	0x02c, 0x000003d7,
+	0x02d, 0x00000ff0,
+	0x000, 0x00000037,
+	0x004, 0x00000160,
+	0x007, 0x00000080,
+	0x002, 0x0000088d,
+	0x0fe, 0x00000000,
+	0x0fe, 0x00000000,
+	0x016, 0x00000200,
+	0x016, 0x00000380,
+	0x016, 0x00000020,
+	0x016, 0x000001a0,
+	0x000, 0x000000bf,
+	0x00d, 0x0000001f,
+	0x00d, 0x00000c9f,
+	0x002, 0x0000004d,
+	0x000, 0x00000cbf,
+	0x004, 0x00000975,
+	0x007, 0x00000700,
 };
 
 u32 Rtl8192UsbRadioB_Array[] = {
-0x019,0x00000003,
-0x000,0x000000bf,
-0x001,0x000006e0,
-0x002,0x0000004c,
-0x003,0x000007f1,
-0x004,0x00000975,
-0x005,0x00000c58,
-0x006,0x00000ae6,
-0x007,0x000000ca,
-0x008,0x00000e1c,
-0x000,0x000000b7,
-0x00a,0x00000850,
-0x000,0x000000bf,
-0x00b,0x000001ba,
-0x00c,0x00000240,
-0x00e,0x00000020,
-0x015,0x00000f80,
-0x016,0x00000020,
-0x017,0x00000597,
-0x018,0x0000050a,
-0x01a,0x00000e00,
-0x01b,0x00000f5e,
-0x01d,0x00000607,
-0x01e,0x000006cc,
-0x00b,0x000001ba,
-0x023,0x00000203,
-0x024,0x00000200,
-0x000,0x00000037,
-0x004,0x00000160,
-0x016,0x00000200,
-0x016,0x00000380,
-0x016,0x00000020,
-0x016,0x000001a0,
-0x00d,0x00000ccc,
-0x000,0x000000bf,
-0x002,0x0000004d,
-0x000,0x00000cbf,
-0x004,0x00000975,
-0x007,0x00000700,
+	0x019, 0x00000003,
+	0x000, 0x000000bf,
+	0x001, 0x000006e0,
+	0x002, 0x0000004c,
+	0x003, 0x000007f1,
+	0x004, 0x00000975,
+	0x005, 0x00000c58,
+	0x006, 0x00000ae6,
+	0x007, 0x000000ca,
+	0x008, 0x00000e1c,
+	0x000, 0x000000b7,
+	0x00a, 0x00000850,
+	0x000, 0x000000bf,
+	0x00b, 0x000001ba,
+	0x00c, 0x00000240,
+	0x00e, 0x00000020,
+	0x015, 0x00000f80,
+	0x016, 0x00000020,
+	0x017, 0x00000597,
+	0x018, 0x0000050a,
+	0x01a, 0x00000e00,
+	0x01b, 0x00000f5e,
+	0x01d, 0x00000607,
+	0x01e, 0x000006cc,
+	0x00b, 0x000001ba,
+	0x023, 0x00000203,
+	0x024, 0x00000200,
+	0x000, 0x00000037,
+	0x004, 0x00000160,
+	0x016, 0x00000200,
+	0x016, 0x00000380,
+	0x016, 0x00000020,
+	0x016, 0x000001a0,
+	0x00d, 0x00000ccc,
+	0x000, 0x000000bf,
+	0x002, 0x0000004d,
+	0x000, 0x00000cbf,
+	0x004, 0x00000975,
+	0x007, 0x00000700,
 };
 
 u32 Rtl8192UsbRadioC_Array[] = {
-0x0, };
+	0x0, };
 
 u32 Rtl8192UsbRadioD_Array[] = {
-0x0, };
+	0x0, };
 
 u32 Rtl8192UsbMACPHY_Array[] = {
-0x03c,0xffff0000,0x00000f0f,
-0x340,0xffffffff,0x161a1a1a,
-0x344,0xffffffff,0x12121416,
-0x348,0x0000ffff,0x00001818,
-0x12c,0xffffffff,0x04000802,
-0x318,0x00000fff,0x00000100,
+	0x03c, 0xffff0000, 0x00000f0f,
+	0x340, 0xffffffff, 0x161a1a1a,
+	0x344, 0xffffffff, 0x12121416,
+	0x348, 0x0000ffff, 0x00001818,
+	0x12c, 0xffffffff, 0x04000802,
+	0x318, 0x00000fff, 0x00000100,
 };
 
 u32 Rtl8192UsbMACPHY_Array_PG[] = {
-0x03c,0xffff0000,0x00000f0f,
-0xe00,0xffffffff,0x06090909,
-0xe04,0xffffffff,0x00030306,
-0xe08,0x0000ff00,0x00000000,
-0xe10,0xffffffff,0x0a0c0d0f,
-0xe14,0xffffffff,0x06070809,
-0xe18,0xffffffff,0x0a0c0d0f,
-0xe1c,0xffffffff,0x06070809,
-0x12c,0xffffffff,0x04000802,
-0x318,0x00000fff,0x00000800,
+	0x03c, 0xffff0000, 0x00000f0f,
+	0xe00, 0xffffffff, 0x06090909,
+	0xe04, 0xffffffff, 0x00030306,
+	0xe08, 0x0000ff00, 0x00000000,
+	0xe10, 0xffffffff, 0x0a0c0d0f,
+	0xe14, 0xffffffff, 0x06070809,
+	0xe18, 0xffffffff, 0x0a0c0d0f,
+	0xe1c, 0xffffffff, 0x06070809,
+	0x12c, 0xffffffff, 0x04000802,
+	0x318, 0x00000fff, 0x00000800,
 };
 
 u32 Rtl8192UsbAGCTAB_Array[] = {
-0xc78,0x7d000001,
-0xc78,0x7d010001,
-0xc78,0x7d020001,
-0xc78,0x7d030001,
-0xc78,0x7d040001,
-0xc78,0x7d050001,
-0xc78,0x7c060001,
-0xc78,0x7b070001,
-0xc78,0x7a080001,
-0xc78,0x79090001,
-0xc78,0x780a0001,
-0xc78,0x770b0001,
-0xc78,0x760c0001,
-0xc78,0x750d0001,
-0xc78,0x740e0001,
-0xc78,0x730f0001,
-0xc78,0x72100001,
-0xc78,0x71110001,
-0xc78,0x70120001,
-0xc78,0x6f130001,
-0xc78,0x6e140001,
-0xc78,0x6d150001,
-0xc78,0x6c160001,
-0xc78,0x6b170001,
-0xc78,0x6a180001,
-0xc78,0x69190001,
-0xc78,0x681a0001,
-0xc78,0x671b0001,
-0xc78,0x661c0001,
-0xc78,0x651d0001,
-0xc78,0x641e0001,
-0xc78,0x491f0001,
-0xc78,0x48200001,
-0xc78,0x47210001,
-0xc78,0x46220001,
-0xc78,0x45230001,
-0xc78,0x44240001,
-0xc78,0x43250001,
-0xc78,0x28260001,
-0xc78,0x27270001,
-0xc78,0x26280001,
-0xc78,0x25290001,
-0xc78,0x242a0001,
-0xc78,0x232b0001,
-0xc78,0x222c0001,
-0xc78,0x212d0001,
-0xc78,0x202e0001,
-0xc78,0x0a2f0001,
-0xc78,0x08300001,
-0xc78,0x06310001,
-0xc78,0x05320001,
-0xc78,0x04330001,
-0xc78,0x03340001,
-0xc78,0x02350001,
-0xc78,0x01360001,
-0xc78,0x00370001,
-0xc78,0x00380001,
-0xc78,0x00390001,
-0xc78,0x003a0001,
-0xc78,0x003b0001,
-0xc78,0x003c0001,
-0xc78,0x003d0001,
-0xc78,0x003e0001,
-0xc78,0x003f0001,
-0xc78,0x7d400001,
-0xc78,0x7d410001,
-0xc78,0x7d420001,
-0xc78,0x7d430001,
-0xc78,0x7d440001,
-0xc78,0x7d450001,
-0xc78,0x7c460001,
-0xc78,0x7b470001,
-0xc78,0x7a480001,
-0xc78,0x79490001,
-0xc78,0x784a0001,
-0xc78,0x774b0001,
-0xc78,0x764c0001,
-0xc78,0x754d0001,
-0xc78,0x744e0001,
-0xc78,0x734f0001,
-0xc78,0x72500001,
-0xc78,0x71510001,
-0xc78,0x70520001,
-0xc78,0x6f530001,
-0xc78,0x6e540001,
-0xc78,0x6d550001,
-0xc78,0x6c560001,
-0xc78,0x6b570001,
-0xc78,0x6a580001,
-0xc78,0x69590001,
-0xc78,0x685a0001,
-0xc78,0x675b0001,
-0xc78,0x665c0001,
-0xc78,0x655d0001,
-0xc78,0x645e0001,
-0xc78,0x495f0001,
-0xc78,0x48600001,
-0xc78,0x47610001,
-0xc78,0x46620001,
-0xc78,0x45630001,
-0xc78,0x44640001,
-0xc78,0x43650001,
-0xc78,0x28660001,
-0xc78,0x27670001,
-0xc78,0x26680001,
-0xc78,0x25690001,
-0xc78,0x246a0001,
-0xc78,0x236b0001,
-0xc78,0x226c0001,
-0xc78,0x216d0001,
-0xc78,0x206e0001,
-0xc78,0x0a6f0001,
-0xc78,0x08700001,
-0xc78,0x06710001,
-0xc78,0x05720001,
-0xc78,0x04730001,
-0xc78,0x03740001,
-0xc78,0x02750001,
-0xc78,0x01760001,
-0xc78,0x00770001,
-0xc78,0x00780001,
-0xc78,0x00790001,
-0xc78,0x007a0001,
-0xc78,0x007b0001,
-0xc78,0x007c0001,
-0xc78,0x007d0001,
-0xc78,0x007e0001,
-0xc78,0x007f0001,
-0xc78,0x2e00001e,
-0xc78,0x2e01001e,
-0xc78,0x2e02001e,
-0xc78,0x2e03001e,
-0xc78,0x2e04001e,
-0xc78,0x2e05001e,
-0xc78,0x3006001e,
-0xc78,0x3407001e,
-0xc78,0x3908001e,
-0xc78,0x3c09001e,
-0xc78,0x3f0a001e,
-0xc78,0x420b001e,
-0xc78,0x440c001e,
-0xc78,0x450d001e,
-0xc78,0x460e001e,
-0xc78,0x460f001e,
-0xc78,0x4710001e,
-0xc78,0x4811001e,
-0xc78,0x4912001e,
-0xc78,0x4a13001e,
-0xc78,0x4b14001e,
-0xc78,0x4b15001e,
-0xc78,0x4c16001e,
-0xc78,0x4d17001e,
-0xc78,0x4e18001e,
-0xc78,0x4f19001e,
-0xc78,0x4f1a001e,
-0xc78,0x501b001e,
-0xc78,0x511c001e,
-0xc78,0x521d001e,
-0xc78,0x521e001e,
-0xc78,0x531f001e,
-0xc78,0x5320001e,
-0xc78,0x5421001e,
-0xc78,0x5522001e,
-0xc78,0x5523001e,
-0xc78,0x5624001e,
-0xc78,0x5725001e,
-0xc78,0x5726001e,
-0xc78,0x5827001e,
-0xc78,0x5828001e,
-0xc78,0x5929001e,
-0xc78,0x592a001e,
-0xc78,0x5a2b001e,
-0xc78,0x5b2c001e,
-0xc78,0x5c2d001e,
-0xc78,0x5c2e001e,
-0xc78,0x5d2f001e,
-0xc78,0x5e30001e,
-0xc78,0x5f31001e,
-0xc78,0x6032001e,
-0xc78,0x6033001e,
-0xc78,0x6134001e,
-0xc78,0x6235001e,
-0xc78,0x6336001e,
-0xc78,0x6437001e,
-0xc78,0x6438001e,
-0xc78,0x6539001e,
-0xc78,0x663a001e,
-0xc78,0x673b001e,
-0xc78,0x673c001e,
-0xc78,0x683d001e,
-0xc78,0x693e001e,
-0xc78,0x6a3f001e,
+	0xc78, 0x7d000001,
+	0xc78, 0x7d010001,
+	0xc78, 0x7d020001,
+	0xc78, 0x7d030001,
+	0xc78, 0x7d040001,
+	0xc78, 0x7d050001,
+	0xc78, 0x7c060001,
+	0xc78, 0x7b070001,
+	0xc78, 0x7a080001,
+	0xc78, 0x79090001,
+	0xc78, 0x780a0001,
+	0xc78, 0x770b0001,
+	0xc78, 0x760c0001,
+	0xc78, 0x750d0001,
+	0xc78, 0x740e0001,
+	0xc78, 0x730f0001,
+	0xc78, 0x72100001,
+	0xc78, 0x71110001,
+	0xc78, 0x70120001,
+	0xc78, 0x6f130001,
+	0xc78, 0x6e140001,
+	0xc78, 0x6d150001,
+	0xc78, 0x6c160001,
+	0xc78, 0x6b170001,
+	0xc78, 0x6a180001,
+	0xc78, 0x69190001,
+	0xc78, 0x681a0001,
+	0xc78, 0x671b0001,
+	0xc78, 0x661c0001,
+	0xc78, 0x651d0001,
+	0xc78, 0x641e0001,
+	0xc78, 0x491f0001,
+	0xc78, 0x48200001,
+	0xc78, 0x47210001,
+	0xc78, 0x46220001,
+	0xc78, 0x45230001,
+	0xc78, 0x44240001,
+	0xc78, 0x43250001,
+	0xc78, 0x28260001,
+	0xc78, 0x27270001,
+	0xc78, 0x26280001,
+	0xc78, 0x25290001,
+	0xc78, 0x242a0001,
+	0xc78, 0x232b0001,
+	0xc78, 0x222c0001,
+	0xc78, 0x212d0001,
+	0xc78, 0x202e0001,
+	0xc78, 0x0a2f0001,
+	0xc78, 0x08300001,
+	0xc78, 0x06310001,
+	0xc78, 0x05320001,
+	0xc78, 0x04330001,
+	0xc78, 0x03340001,
+	0xc78, 0x02350001,
+	0xc78, 0x01360001,
+	0xc78, 0x00370001,
+	0xc78, 0x00380001,
+	0xc78, 0x00390001,
+	0xc78, 0x003a0001,
+	0xc78, 0x003b0001,
+	0xc78, 0x003c0001,
+	0xc78, 0x003d0001,
+	0xc78, 0x003e0001,
+	0xc78, 0x003f0001,
+	0xc78, 0x7d400001,
+	0xc78, 0x7d410001,
+	0xc78, 0x7d420001,
+	0xc78, 0x7d430001,
+	0xc78, 0x7d440001,
+	0xc78, 0x7d450001,
+	0xc78, 0x7c460001,
+	0xc78, 0x7b470001,
+	0xc78, 0x7a480001,
+	0xc78, 0x79490001,
+	0xc78, 0x784a0001,
+	0xc78, 0x774b0001,
+	0xc78, 0x764c0001,
+	0xc78, 0x754d0001,
+	0xc78, 0x744e0001,
+	0xc78, 0x734f0001,
+	0xc78, 0x72500001,
+	0xc78, 0x71510001,
+	0xc78, 0x70520001,
+	0xc78, 0x6f530001,
+	0xc78, 0x6e540001,
+	0xc78, 0x6d550001,
+	0xc78, 0x6c560001,
+	0xc78, 0x6b570001,
+	0xc78, 0x6a580001,
+	0xc78, 0x69590001,
+	0xc78, 0x685a0001,
+	0xc78, 0x675b0001,
+	0xc78, 0x665c0001,
+	0xc78, 0x655d0001,
+	0xc78, 0x645e0001,
+	0xc78, 0x495f0001,
+	0xc78, 0x48600001,
+	0xc78, 0x47610001,
+	0xc78, 0x46620001,
+	0xc78, 0x45630001,
+	0xc78, 0x44640001,
+	0xc78, 0x43650001,
+	0xc78, 0x28660001,
+	0xc78, 0x27670001,
+	0xc78, 0x26680001,
+	0xc78, 0x25690001,
+	0xc78, 0x246a0001,
+	0xc78, 0x236b0001,
+	0xc78, 0x226c0001,
+	0xc78, 0x216d0001,
+	0xc78, 0x206e0001,
+	0xc78, 0x0a6f0001,
+	0xc78, 0x08700001,
+	0xc78, 0x06710001,
+	0xc78, 0x05720001,
+	0xc78, 0x04730001,
+	0xc78, 0x03740001,
+	0xc78, 0x02750001,
+	0xc78, 0x01760001,
+	0xc78, 0x00770001,
+	0xc78, 0x00780001,
+	0xc78, 0x00790001,
+	0xc78, 0x007a0001,
+	0xc78, 0x007b0001,
+	0xc78, 0x007c0001,
+	0xc78, 0x007d0001,
+	0xc78, 0x007e0001,
+	0xc78, 0x007f0001,
+	0xc78, 0x2e00001e,
+	0xc78, 0x2e01001e,
+	0xc78, 0x2e02001e,
+	0xc78, 0x2e03001e,
+	0xc78, 0x2e04001e,
+	0xc78, 0x2e05001e,
+	0xc78, 0x3006001e,
+	0xc78, 0x3407001e,
+	0xc78, 0x3908001e,
+	0xc78, 0x3c09001e,
+	0xc78, 0x3f0a001e,
+	0xc78, 0x420b001e,
+	0xc78, 0x440c001e,
+	0xc78, 0x450d001e,
+	0xc78, 0x460e001e,
+	0xc78, 0x460f001e,
+	0xc78, 0x4710001e,
+	0xc78, 0x4811001e,
+	0xc78, 0x4912001e,
+	0xc78, 0x4a13001e,
+	0xc78, 0x4b14001e,
+	0xc78, 0x4b15001e,
+	0xc78, 0x4c16001e,
+	0xc78, 0x4d17001e,
+	0xc78, 0x4e18001e,
+	0xc78, 0x4f19001e,
+	0xc78, 0x4f1a001e,
+	0xc78, 0x501b001e,
+	0xc78, 0x511c001e,
+	0xc78, 0x521d001e,
+	0xc78, 0x521e001e,
+	0xc78, 0x531f001e,
+	0xc78, 0x5320001e,
+	0xc78, 0x5421001e,
+	0xc78, 0x5522001e,
+	0xc78, 0x5523001e,
+	0xc78, 0x5624001e,
+	0xc78, 0x5725001e,
+	0xc78, 0x5726001e,
+	0xc78, 0x5827001e,
+	0xc78, 0x5828001e,
+	0xc78, 0x5929001e,
+	0xc78, 0x592a001e,
+	0xc78, 0x5a2b001e,
+	0xc78, 0x5b2c001e,
+	0xc78, 0x5c2d001e,
+	0xc78, 0x5c2e001e,
+	0xc78, 0x5d2f001e,
+	0xc78, 0x5e30001e,
+	0xc78, 0x5f31001e,
+	0xc78, 0x6032001e,
+	0xc78, 0x6033001e,
+	0xc78, 0x6134001e,
+	0xc78, 0x6235001e,
+	0xc78, 0x6336001e,
+	0xc78, 0x6437001e,
+	0xc78, 0x6438001e,
+	0xc78, 0x6539001e,
+	0xc78, 0x663a001e,
+	0xc78, 0x673b001e,
+	0xc78, 0x673c001e,
+	0xc78, 0x683d001e,
+	0xc78, 0x693e001e,
+	0xc78, 0x6a3f001e,
 };
diff --git a/drivers/staging/rtl8192u/r819xU_phy.c b/drivers/staging/rtl8192u/r819xU_phy.c
index b9f35313c7a..02554c98120 100644
--- a/drivers/staging/rtl8192u/r819xU_phy.c
+++ b/drivers/staging/rtl8192u/r819xU_phy.c
@@ -504,9 +504,8 @@ void rtl8192_phy_configmac(struct net_device *dev)
 		pdwArray = rtl819XMACPHY_Array;
 	}
 	for (i = 0; i < dwArrayLen; i = i+3) {
-		if (pdwArray[i] == 0x318) {
+		if (pdwArray[i] == 0x318)
 			pdwArray[i+2] = 0x00000800;
-		}
 
 		RT_TRACE(COMP_DBG,
 			 "Rtl8190MACPHY_Array[0]=%x Rtl8190MACPHY_Array[1]=%x Rtl8190MACPHY_Array[2]=%x\n",
@@ -992,7 +991,6 @@ u8 rtl8192_phy_ConfigRFWithHeaderFile(struct net_device *dev,
 {
 
 	int i;
-	u8 ret = 0;
 
 	switch (eRFPath) {
 	case RF90_PATH_A:
@@ -1059,7 +1057,7 @@ u8 rtl8192_phy_ConfigRFWithHeaderFile(struct net_device *dev,
 		break;
 	}
 
-	return ret;
+	return 0;
 
 }
 
diff --git a/drivers/staging/rtl8712/hal_init.c b/drivers/staging/rtl8712/hal_init.c
index d801c5af006..36de7e478f3 100644
--- a/drivers/staging/rtl8712/hal_init.c
+++ b/drivers/staging/rtl8712/hal_init.c
@@ -183,7 +183,7 @@ static u8 rtl8712_dl_fw(struct _adapter *padapter)
 		maxlen = (fwhdr.img_IMEM_size > fwhdr.img_SRAM_size) ?
 			  fwhdr.img_IMEM_size : fwhdr.img_SRAM_size;
 		maxlen += txdscp_sz;
-		ptmpchar = _malloc(maxlen + FWBUFF_ALIGN_SZ);
+		ptmpchar = kmalloc(maxlen + FWBUFF_ALIGN_SZ, GFP_ATOMIC);
 		if (ptmpchar == NULL)
 			return ret;
 
diff --git a/drivers/staging/rtl8712/ieee80211.c b/drivers/staging/rtl8712/ieee80211.c
index 57fef70ad98..fe9459e483c 100644
--- a/drivers/staging/rtl8712/ieee80211.c
+++ b/drivers/staging/rtl8712/ieee80211.c
@@ -289,7 +289,7 @@ static int r8712_get_wpa2_cipher_suite(u8 *s)
 int r8712_parse_wpa_ie(u8 *wpa_ie, int wpa_ie_len, int *group_cipher,
 		 int *pairwise_cipher)
 {
-	int i, ret = _SUCCESS;
+	int i;
 	int left, count;
 	u8 *pos;
 
@@ -324,13 +324,13 @@ int r8712_parse_wpa_ie(u8 *wpa_ie, int wpa_ie_len, int *group_cipher,
 		}
 	} else if (left == 1)
 		return _FAIL;
-	return ret;
+	return _SUCCESS;
 }
 
 int r8712_parse_wpa2_ie(u8 *rsn_ie, int rsn_ie_len, int *group_cipher,
 		  int *pairwise_cipher)
 {
-	int i, ret = _SUCCESS;
+	int i;
 	int left, count;
 	u8 *pos;
 
@@ -364,7 +364,7 @@ int r8712_parse_wpa2_ie(u8 *rsn_ie, int rsn_ie_len, int *group_cipher,
 		}
 	} else if (left == 1)
 		return _FAIL;
-	return ret;
+	return _SUCCESS;
 }
 
 int r8712_get_sec_ie(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len,
diff --git a/drivers/staging/rtl8712/mlme_linux.c b/drivers/staging/rtl8712/mlme_linux.c
index f569a7081be..377efb88676 100644
--- a/drivers/staging/rtl8712/mlme_linux.c
+++ b/drivers/staging/rtl8712/mlme_linux.c
@@ -147,10 +147,9 @@ void r8712_report_sec_ie(struct _adapter *adapter, u8 authmode, u8 *sec_ie)
 
 	buff = NULL;
 	if (authmode == _WPA_IE_ID_) {
-		buff = _malloc(IW_CUSTOM_MAX);
+		buff = kzalloc(IW_CUSTOM_MAX, GFP_ATOMIC);
 		if (buff == NULL)
 			return;
-		memset(buff, 0, IW_CUSTOM_MAX);
 		p = buff;
 		p += sprintf(p, "ASSOCINFO(ReqIEs=");
 		len = sec_ie[1] + 2;
diff --git a/drivers/staging/rtl8712/osdep_service.h b/drivers/staging/rtl8712/osdep_service.h
index 566235a14a8..09e156199e7 100644
--- a/drivers/staging/rtl8712/osdep_service.h
+++ b/drivers/staging/rtl8712/osdep_service.h
@@ -168,11 +168,6 @@ static inline void sleep_schedulable(int ms)
 		return;
 }
 
-static inline u8 *_malloc(u32 sz)
-{
-	return	kmalloc(sz, GFP_ATOMIC);
-}
-
 static inline unsigned char _cancel_timer_ex(struct timer_list *ptimer)
 {
 	return del_timer(ptimer);
diff --git a/drivers/staging/rtl8712/rtl8712_cmd.c b/drivers/staging/rtl8712/rtl8712_cmd.c
index 1a4b7a651f9..8ca7d7e68dc 100644
--- a/drivers/staging/rtl8712/rtl8712_cmd.c
+++ b/drivers/staging/rtl8712/rtl8712_cmd.c
@@ -290,8 +290,7 @@ static struct cmd_obj *cmd_hdl_filter(struct _adapter *padapter,
 
 static u8 check_cmd_fifo(struct _adapter *padapter, uint sz)
 {
-	u8 res = _SUCCESS;
-	return res;
+	return _SUCCESS;
 }
 
 u8 r8712_fw_cmd(struct _adapter *pAdapter, u32 cmd)
diff --git a/drivers/staging/rtl8712/rtl8712_recv.c b/drivers/staging/rtl8712/rtl8712_recv.c
index 0723b2f73aa..1f700171af1 100644
--- a/drivers/staging/rtl8712/rtl8712_recv.c
+++ b/drivers/staging/rtl8712/rtl8712_recv.c
@@ -58,12 +58,10 @@ int r8712_init_recv_priv(struct recv_priv *precvpriv, struct _adapter *padapter)
 
 	/*init recv_buf*/
 	_init_queue(&precvpriv->free_recv_buf_queue);
-	precvpriv->pallocated_recv_buf = _malloc(NR_RECVBUFF *
-					 sizeof(struct recv_buf) + 4);
+	precvpriv->pallocated_recv_buf = kzalloc(NR_RECVBUFF * sizeof(struct recv_buf) + 4,
+						 GFP_ATOMIC);
 	if (precvpriv->pallocated_recv_buf == NULL)
 		return _FAIL;
-	memset(precvpriv->pallocated_recv_buf, 0, NR_RECVBUFF *
-		sizeof(struct recv_buf) + 4);
 	precvpriv->precv_buf = precvpriv->pallocated_recv_buf + 4 -
 			      ((addr_t) (precvpriv->pallocated_recv_buf) & 3);
 	precvbuf = (struct recv_buf *)precvpriv->precv_buf;
@@ -123,8 +121,6 @@ void r8712_free_recv_priv(struct recv_priv *precvpriv)
 
 int r8712_init_recvbuf(struct _adapter *padapter, struct recv_buf *precvbuf)
 {
-	int res = _SUCCESS;
-
 	precvbuf->transfer_len = 0;
 	precvbuf->len = 0;
 	precvbuf->ref_cnt = 0;
@@ -134,7 +130,7 @@ int r8712_init_recvbuf(struct _adapter *padapter, struct recv_buf *precvbuf)
 		precvbuf->ptail = precvbuf->pbuf;
 		precvbuf->pend = precvbuf->pdata + MAX_RECVBUF_SZ;
 	}
-	return res;
+	return _SUCCESS;
 }
 
 int r8712_free_recvframe(union recv_frame *precvframe,
@@ -347,7 +343,6 @@ static int amsdu_to_msdu(struct _adapter *padapter, union recv_frame *prframe)
 	_pkt *sub_skb, *subframes[MAX_SUBFRAME_COUNT];
 	struct recv_priv *precvpriv = &padapter->recvpriv;
 	struct  __queue *pfree_recv_queue = &(precvpriv->free_recv_queue);
-	int	ret = _SUCCESS;
 
 	nr_subframes = 0;
 	pattrib = &prframe->u.hdr.attrib;
@@ -435,7 +430,7 @@ static int amsdu_to_msdu(struct _adapter *padapter, union recv_frame *prframe)
 exit:
 	prframe->u.hdr.len = 0;
 	r8712_free_recvframe(prframe, pfree_recv_queue);
-	return ret;
+	return _SUCCESS;
 }
 
 void r8712_rxcmd_event_hdl(struct _adapter *padapter, void *prxcmdbuf)
diff --git a/drivers/staging/rtl8712/rtl871x_cmd.c b/drivers/staging/rtl8712/rtl871x_cmd.c
index a67185db392..7a252200e90 100644
--- a/drivers/staging/rtl8712/rtl871x_cmd.c
+++ b/drivers/staging/rtl8712/rtl871x_cmd.c
@@ -64,13 +64,14 @@ static sint _init_cmd_priv(struct cmd_priv *pcmdpriv)
 
 	/* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
 	pcmdpriv->cmd_seq = 1;
-	pcmdpriv->cmd_allocated_buf = _malloc(MAX_CMDSZ + CMDBUFF_ALIGN_SZ);
+	pcmdpriv->cmd_allocated_buf = kmalloc(MAX_CMDSZ + CMDBUFF_ALIGN_SZ,
+					      GFP_ATOMIC);
 	if (pcmdpriv->cmd_allocated_buf == NULL)
 		return _FAIL;
 	pcmdpriv->cmd_buf = pcmdpriv->cmd_allocated_buf  +  CMDBUFF_ALIGN_SZ -
 			    ((addr_t)(pcmdpriv->cmd_allocated_buf) &
 			    (CMDBUFF_ALIGN_SZ-1));
-	pcmdpriv->rsp_allocated_buf = _malloc(MAX_RSPSZ + 4);
+	pcmdpriv->rsp_allocated_buf = kmalloc(MAX_RSPSZ + 4, GFP_ATOMIC);
 	if (pcmdpriv->rsp_allocated_buf == NULL)
 		return _FAIL;
 	pcmdpriv->rsp_buf = pcmdpriv->rsp_allocated_buf  +  4 -
@@ -85,7 +86,7 @@ static sint _init_evt_priv(struct evt_priv *pevtpriv)
 {
 	/* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
 	pevtpriv->event_seq = 0;
-	pevtpriv->evt_allocated_buf = _malloc(MAX_EVTSZ + 4);
+	pevtpriv->evt_allocated_buf = kmalloc(MAX_EVTSZ + 4, GFP_ATOMIC);
 
 	if (pevtpriv->evt_allocated_buf == NULL)
 		return _FAIL;
@@ -226,11 +227,10 @@ u8 r8712_sitesurvey_cmd(struct _adapter *padapter,
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	psurveyPara = (struct sitesurvey_parm *)_malloc(
-		       sizeof(struct sitesurvey_parm));
+	psurveyPara = kmalloc(sizeof(struct sitesurvey_parm), GFP_ATOMIC);
 	if (psurveyPara == NULL) {
 		kfree((unsigned char *) ph2c);
 		return _FAIL;
@@ -259,11 +259,11 @@ u8 r8712_setdatarate_cmd(struct _adapter *padapter, u8 *rateset)
 	struct setdatarate_parm	*pbsetdataratepara;
 	struct cmd_priv		*pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	pbsetdataratepara = (struct setdatarate_parm *)_malloc(
-			     sizeof(struct setdatarate_parm));
+	pbsetdataratepara = kmalloc(sizeof(struct setdatarate_parm),
+				    GFP_ATOMIC);
 	if (pbsetdataratepara == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
@@ -282,11 +282,11 @@ u8 r8712_set_chplan_cmd(struct _adapter *padapter, int chplan)
 	struct SetChannelPlan_param *psetchplanpara;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	psetchplanpara = (struct SetChannelPlan_param *)
-		_malloc(sizeof(struct SetChannelPlan_param));
+	psetchplanpara = kmalloc(sizeof(struct SetChannelPlan_param),
+				 GFP_ATOMIC);
 	if (psetchplanpara == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
@@ -304,11 +304,11 @@ u8 r8712_setbasicrate_cmd(struct _adapter *padapter, u8 *rateset)
 	struct setbasicrate_parm *pssetbasicratepara;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	pssetbasicratepara = (struct setbasicrate_parm *)_malloc(
-			      sizeof(struct setbasicrate_parm));
+	pssetbasicratepara = kmalloc(sizeof(struct setbasicrate_parm),
+				     GFP_ATOMIC);
 	if (pssetbasicratepara == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
@@ -327,11 +327,10 @@ u8 r8712_setptm_cmd(struct _adapter *padapter, u8 type)
 	struct writePTM_parm	*pwriteptmparm;
 	struct cmd_priv		*pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	pwriteptmparm = (struct writePTM_parm *)
-		_malloc(sizeof(struct writePTM_parm));
+	pwriteptmparm = kmalloc(sizeof(struct writePTM_parm), GFP_ATOMIC);
 	if (pwriteptmparm == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
@@ -348,11 +347,10 @@ u8 r8712_setfwdig_cmd(struct _adapter *padapter, u8 type)
 	struct writePTM_parm *pwriteptmparm;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	pwriteptmparm = (struct writePTM_parm *)
-		_malloc(sizeof(struct setdig_parm));
+	pwriteptmparm = kmalloc(sizeof(struct writePTM_parm), GFP_ATOMIC);
 	if (pwriteptmparm == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
@@ -369,11 +367,10 @@ u8 r8712_setfwra_cmd(struct _adapter *padapter, u8 type)
 	struct writePTM_parm *pwriteptmparm;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	pwriteptmparm = (struct writePTM_parm *)
-		_malloc(sizeof(struct setra_parm));
+	pwriteptmparm = kmalloc(sizeof(struct writePTM_parm), GFP_ATOMIC);
 	if (pwriteptmparm == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
@@ -390,11 +387,10 @@ u8 r8712_setrfreg_cmd(struct _adapter  *padapter, u8 offset, u32 val)
 	struct writeRF_parm *pwriterfparm;
 	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	pwriterfparm = (struct writeRF_parm *)_malloc(
-			sizeof(struct writeRF_parm));
+	pwriterfparm = kmalloc(sizeof(struct writeRF_parm), GFP_ATOMIC);
 	if (pwriterfparm == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
@@ -412,10 +408,10 @@ u8 r8712_getrfreg_cmd(struct _adapter *padapter, u8 offset, u8 *pval)
 	struct readRF_parm *prdrfparm;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	prdrfparm = (struct readRF_parm *)_malloc(sizeof(struct readRF_parm));
+	prdrfparm = kmalloc(sizeof(struct readRF_parm), GFP_ATOMIC);
 	if (prdrfparm == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
@@ -456,7 +452,7 @@ u8 r8712_createbss_cmd(struct _adapter *padapter)
 				 &padapter->registrypriv.dev_network;
 
 	padapter->ledpriv.LedControlHandler(padapter, LED_CTL_START_TO_LINK);
-	pcmd = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	pcmd = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (pcmd == NULL)
 		return _FAIL;
 	_init_listhead(&pcmd->list);
@@ -490,7 +486,7 @@ u8 r8712_joinbss_cmd(struct _adapter  *padapter, struct wlan_network *pnetwork)
 						network.InfrastructureMode;
 
 	padapter->ledpriv.LedControlHandler(padapter, LED_CTL_START_TO_LINK);
-	pcmd = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	pcmd = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (pcmd == NULL)
 		return _FAIL;
 	t_len = sizeof(u32) + 6 * sizeof(unsigned char) + 2 +
@@ -628,11 +624,10 @@ u8 r8712_disassoc_cmd(struct _adapter *padapter) /* for sta_mode */
 	struct disconnect_parm *pdisconnect;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 
-	pdisconnect_cmd = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	pdisconnect_cmd = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (pdisconnect_cmd == NULL)
 		return _FAIL;
-	pdisconnect = (struct disconnect_parm *)_malloc(
-		      sizeof(struct disconnect_parm));
+	pdisconnect = kmalloc(sizeof(struct disconnect_parm), GFP_ATOMIC);
 	if (pdisconnect == NULL) {
 		kfree((u8 *)pdisconnect_cmd);
 		return _FAIL;
@@ -651,11 +646,10 @@ u8 r8712_setopmode_cmd(struct _adapter *padapter,
 
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	psetop = (struct setopmode_parm *)_malloc(
-		  sizeof(struct setopmode_parm));
+	psetop = kmalloc(sizeof(struct setopmode_parm), GFP_ATOMIC);
 	if (psetop == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
@@ -676,17 +670,15 @@ u8 r8712_setstakey_cmd(struct _adapter *padapter, u8 *psta, u8 unicast_key)
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct sta_info *sta = (struct sta_info *)psta;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	psetstakey_para = (struct set_stakey_parm *)_malloc(
-			  sizeof(struct set_stakey_parm));
+	psetstakey_para = kmalloc(sizeof(struct set_stakey_parm), GFP_ATOMIC);
 	if (psetstakey_para == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
 	}
-	psetstakey_rsp = (struct set_stakey_rsp *)_malloc(
-			  sizeof(struct set_stakey_rsp));
+	psetstakey_rsp = kmalloc(sizeof(struct set_stakey_rsp), GFP_ATOMIC);
 	if (psetstakey_rsp == NULL) {
 		kfree((u8 *) ph2c);
 		kfree((u8 *) psetstakey_para);
@@ -718,11 +710,10 @@ u8 r8712_setrfintfs_cmd(struct _adapter *padapter, u8 mode)
 	struct setrfintfs_parm *psetrfintfsparm;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	psetrfintfsparm = (struct setrfintfs_parm *)_malloc(
-			   sizeof(struct setrfintfs_parm));
+	psetrfintfsparm = kmalloc(sizeof(struct setrfintfs_parm), GFP_ATOMIC);
 	if (psetrfintfsparm == NULL) {
 		kfree((unsigned char *) ph2c);
 		return _FAIL;
@@ -741,11 +732,10 @@ u8 r8712_setrttbl_cmd(struct _adapter *padapter,
 	struct setratable_parm *psetrttblparm;
 	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	psetrttblparm = (struct setratable_parm *)_malloc(
-			sizeof(struct setratable_parm));
+	psetrttblparm = kmalloc(sizeof(struct setratable_parm), GFP_ATOMIC);
 	if (psetrttblparm == NULL) {
 		kfree((unsigned char *)ph2c);
 		return _FAIL;
@@ -763,11 +753,10 @@ u8 r8712_gettssi_cmd(struct _adapter *padapter, u8 offset, u8 *pval)
 	struct cmd_obj *ph2c;
 	struct readTSSI_parm *prdtssiparm;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	prdtssiparm = (struct readTSSI_parm *)
-		_malloc(sizeof(struct readTSSI_parm));
+	prdtssiparm = kmalloc(sizeof(struct readTSSI_parm), GFP_ATOMIC);
 	if (prdtssiparm == NULL) {
 		kfree((unsigned char *) ph2c);
 		return _FAIL;
@@ -790,11 +779,11 @@ u8 r8712_setMacAddr_cmd(struct _adapter *padapter, u8 *mac_addr)
 	struct cmd_obj *ph2c;
 	struct SetMacAddr_param	*psetMacAddr_para;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	psetMacAddr_para = (struct SetMacAddr_param *)_malloc(
-			   sizeof(struct SetMacAddr_param));
+	psetMacAddr_para = kmalloc(sizeof(struct SetMacAddr_param),
+				   GFP_ATOMIC);
 	if (psetMacAddr_para == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
@@ -813,17 +802,17 @@ u8 r8712_setassocsta_cmd(struct _adapter *padapter, u8 *mac_addr)
 	struct set_assocsta_parm	*psetassocsta_para;
 	struct set_assocsta_rsp		*psetassocsta_rsp = NULL;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	psetassocsta_para = (struct set_assocsta_parm *)
-			    _malloc(sizeof(struct set_assocsta_parm));
+	psetassocsta_para = kmalloc(sizeof(struct set_assocsta_parm),
+				    GFP_ATOMIC);
 	if (psetassocsta_para == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
 	}
-	psetassocsta_rsp = (struct set_assocsta_rsp *)_malloc(
-			    sizeof(struct set_assocsta_rsp));
+	psetassocsta_rsp = kmalloc(sizeof(struct set_assocsta_rsp),
+				   GFP_ATOMIC);
 	if (psetassocsta_rsp == NULL) {
 		kfree((u8 *)ph2c);
 		kfree((u8 *)psetassocsta_para);
@@ -843,11 +832,10 @@ u8 r8712_addbareq_cmd(struct _adapter *padapter, u8 tid)
 	struct cmd_obj		*ph2c;
 	struct addBaReq_parm	*paddbareq_parm;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	paddbareq_parm = (struct addBaReq_parm *)_malloc(
-			  sizeof(struct addBaReq_parm));
+	paddbareq_parm = kmalloc(sizeof(struct addBaReq_parm), GFP_ATOMIC);
 	if (paddbareq_parm == NULL) {
 		kfree((unsigned char *)ph2c);
 		return _FAIL;
@@ -865,11 +853,10 @@ u8 r8712_wdg_wk_cmd(struct _adapter *padapter)
 	struct drvint_cmd_parm  *pdrvintcmd_param;
 	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	pdrvintcmd_param = (struct drvint_cmd_parm *)_malloc(
-			   sizeof(struct drvint_cmd_parm));
+	pdrvintcmd_param = kmalloc(sizeof(struct drvint_cmd_parm), GFP_ATOMIC);
 	if (pdrvintcmd_param == NULL) {
 		kfree((unsigned char *)ph2c);
 		return _FAIL;
@@ -1040,16 +1027,14 @@ u8 r8712_disconnectCtrlEx_cmd(struct _adapter *adapter, u32 enableDrvCtrl,
 	struct DisconnectCtrlEx_param *param;
 	struct cmd_priv *pcmdpriv = &adapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	param = (struct DisconnectCtrlEx_param *)
-		_malloc(sizeof(struct DisconnectCtrlEx_param));
+	param = kzalloc(sizeof(struct DisconnectCtrlEx_param), GFP_ATOMIC);
 	if (param == NULL) {
 		kfree((unsigned char *) ph2c);
 		return _FAIL;
 	}
-	memset(param, 0, sizeof(struct DisconnectCtrlEx_param));
 
 	param->EnableDrvCtrl = (unsigned char)enableDrvCtrl;
 	param->TryPktCnt = (unsigned char)tryPktCnt;
diff --git a/drivers/staging/rtl8712/rtl871x_io.c b/drivers/staging/rtl8712/rtl871x_io.c
index abc1c97378f..37a841a1488 100644
--- a/drivers/staging/rtl8712/rtl871x_io.c
+++ b/drivers/staging/rtl8712/rtl871x_io.c
@@ -60,8 +60,8 @@ static uint _init_intf_hdl(struct _adapter *padapter,
 	set_intf_funs = &(r8712_usb_set_intf_funs);
 	set_intf_ops = &r8712_usb_set_intf_ops;
 	init_intf_priv = &r8712_usb_init_intf_priv;
-	pintf_priv = pintf_hdl->pintfpriv = (struct intf_priv *)
-		     _malloc(sizeof(struct intf_priv));
+	pintf_priv = pintf_hdl->pintfpriv = kmalloc(sizeof(struct intf_priv),
+						    GFP_ATOMIC);
 	if (pintf_priv == NULL)
 		goto _init_intf_hdl_fail;
 	pintf_hdl->adapter = (u8 *)padapter;
@@ -112,15 +112,16 @@ uint r8712_alloc_io_queue(struct _adapter *adapter)
 	struct io_queue *pio_queue;
 	struct io_req *pio_req;
 
-	pio_queue = (struct io_queue *)_malloc(sizeof(struct io_queue));
+	pio_queue = kmalloc(sizeof(struct io_queue), GFP_ATOMIC);
 	if (pio_queue == NULL)
 		goto alloc_io_queue_fail;
 	_init_listhead(&pio_queue->free_ioreqs);
 	_init_listhead(&pio_queue->processing);
 	_init_listhead(&pio_queue->pending);
 	spin_lock_init(&pio_queue->lock);
-	pio_queue->pallocated_free_ioreqs_buf = (u8 *)_malloc(NUM_IOREQ *
-						(sizeof(struct io_req)) + 4);
+	pio_queue->pallocated_free_ioreqs_buf = kmalloc(NUM_IOREQ *
+							(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_ioctl_linux.c b/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
index 23d539dba7e..e147c4bfe12 100644
--- a/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
+++ b/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
@@ -424,10 +424,9 @@ static int wpa_set_encryption(struct net_device *dev, struct ieee_param *param,
 			wep_key_idx = 0;
 		if (wep_key_len > 0) {
 			wep_key_len = wep_key_len <= 5 ? 5 : 13;
-			pwep = (struct NDIS_802_11_WEP *)_malloc((u32)
-			       (wep_key_len +
-			       FIELD_OFFSET(struct NDIS_802_11_WEP,
-			       KeyMaterial)));
+			pwep = kmalloc((u32)(wep_key_len +
+				       FIELD_OFFSET(struct NDIS_802_11_WEP, KeyMaterial)),
+				       GFP_ATOMIC);
 			if (pwep == NULL)
 				return -ENOMEM;
 			memset(pwep, 0, sizeof(struct NDIS_802_11_WEP));
@@ -518,10 +517,9 @@ static int r871x_set_wpa_ie(struct _adapter *padapter, char *pie,
 	if ((ielen > MAX_WPA_IE_LEN) || (pie == NULL))
 		return -EINVAL;
 	if (ielen) {
-		buf = _malloc(ielen);
+		buf = kmemdup(pie, ielen, GFP_ATOMIC);
 		if (buf == NULL)
 			return -ENOMEM;
-		memcpy(buf, pie , ielen);
 		pos = buf;
 		if (ielen < RSN_HEADER_LEN) {
 			ret  = -EINVAL;
@@ -959,13 +957,9 @@ static int r871x_wx_set_priv(struct net_device *dev,
 	struct iw_point *dwrq = (struct iw_point *)awrq;
 
 	len = dwrq->length;
-	ext = _malloc(len);
-	if (!ext)
-		return -ENOMEM;
-	if (copy_from_user(ext, dwrq->pointer, len)) {
-		kfree(ext);
-		return -EFAULT;
-	}
+	ext = memdup_user(dwrq->pointer, len);
+	if (IS_ERR(ext))
+		return PTR_ERR(ext);
 
 	if (0 == strcasecmp(ext, "RSSI")) {
 		/*Return received signal strength indicator in -db for */
@@ -1801,13 +1795,6 @@ static int r871x_wx_set_enc_ext(struct net_device *dev,
 	u32 param_len;
 	int ret = 0;
 
-	param_len = sizeof(struct ieee_param) + pext->key_len;
-	param = (struct ieee_param *)_malloc(param_len);
-	if (param == NULL)
-		return -ENOMEM;
-	memset(param, 0, param_len);
-	param->cmd = IEEE_CMD_SET_ENCRYPTION;
-	memset(param->sta_addr, 0xff, ETH_ALEN);
 	switch (pext->alg) {
 	case IW_ENCODE_ALG_NONE:
 		alg_name = "none";
@@ -1824,6 +1811,14 @@ static int r871x_wx_set_enc_ext(struct net_device *dev,
 	default:
 		return -EINVAL;
 	}
+
+	param_len = sizeof(struct ieee_param) + pext->key_len;
+	param = kzalloc(param_len, GFP_ATOMIC);
+	if (param == NULL)
+		return -ENOMEM;
+	param->cmd = IEEE_CMD_SET_ENCRYPTION;
+	memset(param->sta_addr, 0xff, ETH_ALEN);
+
 	strncpy((char *)param->u.crypt.alg, alg_name, IEEE_CRYPT_ALG_NAME_LEN);
 	if (pext->ext_flags & IW_ENCODE_EXT_GROUP_KEY)
 		param->u.crypt.set_tx = 0;
@@ -1920,7 +1915,7 @@ static int r871x_mp_ioctl_hdl(struct net_device *dev,
 	bset = (u8)(p->flags & 0xFFFF);
 	len = p->length;
 	pparmbuf = NULL;
-	pparmbuf = (u8 *)_malloc(len);
+	pparmbuf = kmalloc(len, GFP_ATOMIC);
 	if (pparmbuf == NULL) {
 		ret = -ENOMEM;
 		goto _r871x_mp_ioctl_hdl_exit;
@@ -2193,13 +2188,9 @@ static int wpa_supplicant_ioctl(struct net_device *dev, struct iw_point *p)
 
 	if (p->length < sizeof(struct ieee_param) || !p->pointer)
 		return -EINVAL;
-	param = (struct ieee_param *)_malloc(p->length);
-	if (param == NULL)
-		return -ENOMEM;
-	if (copy_from_user(param, p->pointer, p->length)) {
-		kfree((u8 *)param);
-		return -EFAULT;
-	}
+	param = memdup_user(p->pointer, p->length);
+	if (IS_ERR(param))
+		return PTR_ERR(param);
 	switch (param->cmd) {
 	case IEEE_CMD_SET_WPA_PARAM:
 		ret = wpa_set_param(dev, param->u.wpa_param.name,
diff --git a/drivers/staging/rtl8712/rtl871x_mlme.c b/drivers/staging/rtl8712/rtl871x_mlme.c
index 3ea99aea438..02339e10001 100644
--- a/drivers/staging/rtl8712/rtl871x_mlme.c
+++ b/drivers/staging/rtl8712/rtl871x_mlme.c
@@ -62,7 +62,8 @@ static sint _init_mlme_priv(struct _adapter *padapter)
 	_init_queue(&(pmlmepriv->scanned_queue));
 	set_scanned_network_val(pmlmepriv, 0);
 	memset(&pmlmepriv->assoc_ssid, 0, sizeof(struct ndis_802_11_ssid));
-	pbuf = _malloc(MAX_BSS_CNT * (sizeof(struct wlan_network)));
+	pbuf = kmalloc(MAX_BSS_CNT * (sizeof(struct wlan_network)),
+		       GFP_ATOMIC);
 	if (pbuf == NULL)
 		return _FAIL;
 	pmlmepriv->free_bss_buf = pbuf;
@@ -725,8 +726,7 @@ void r8712_joinbss_event_callback(struct _adapter *adapter, u8 *pbuf)
 	struct wlan_network *pnetwork;
 
 	if (sizeof(struct list_head) == 4 * sizeof(u32)) {
-		pnetwork = (struct wlan_network *)
-			_malloc(sizeof(struct wlan_network));
+		pnetwork = kmalloc(sizeof(struct wlan_network), GFP_ATOMIC);
 		memcpy((u8 *)pnetwork+16, (u8 *)pbuf + 8,
 			sizeof(struct wlan_network) - 16);
 	} else
@@ -1211,19 +1211,16 @@ sint r8712_set_auth(struct _adapter *adapter,
 	struct cmd_priv	*pcmdpriv = &adapter->cmdpriv;
 	struct cmd_obj *pcmd;
 	struct setauth_parm *psetauthparm;
-	sint ret = _SUCCESS;
 
-	pcmd = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	pcmd = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (pcmd == NULL)
 		return _FAIL;
 
-	psetauthparm = (struct setauth_parm *)_malloc(
-			sizeof(struct setauth_parm));
+	psetauthparm = kzalloc(sizeof(struct setauth_parm), GFP_ATOMIC);
 	if (psetauthparm == NULL) {
 		kfree((unsigned char *)pcmd);
 		return _FAIL;
 	}
-	memset(psetauthparm, 0, sizeof(struct setauth_parm));
 	psetauthparm->mode = (u8)psecuritypriv->AuthAlgrthm;
 	pcmd->cmdcode = _SetAuth_CMD_;
 	pcmd->parmbuf = (unsigned char *)psetauthparm;
@@ -1232,7 +1229,7 @@ sint r8712_set_auth(struct _adapter *adapter,
 	pcmd->rspsz = 0;
 	_init_listhead(&pcmd->list);
 	r8712_enqueue_cmd(pcmdpriv, pcmd);
-	return ret;
+	return _SUCCESS;
 }
 
 sint r8712_set_key(struct _adapter *adapter,
@@ -1243,16 +1240,16 @@ sint r8712_set_key(struct _adapter *adapter,
 	struct cmd_obj *pcmd;
 	struct setkey_parm *psetkeyparm;
 	u8 keylen;
+	sint ret = _SUCCESS;
 
-	pcmd = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	pcmd = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (pcmd == NULL)
 		return _FAIL;
-	psetkeyparm = (struct setkey_parm *)_malloc(sizeof(struct setkey_parm));
+	psetkeyparm = kzalloc(sizeof(struct setkey_parm), GFP_ATOMIC);
 	if (psetkeyparm == NULL) {
-		kfree((unsigned char *)pcmd);
-		return _FAIL;
+		ret = _FAIL;
+		goto err_free_cmd;
 	}
-	memset(psetkeyparm, 0, sizeof(struct setkey_parm));
 	if (psecuritypriv->AuthAlgrthm == 2) { /* 802.1X */
 		psetkeyparm->algorithm =
 			 (u8)psecuritypriv->XGrpPrivacy;
@@ -1274,23 +1271,28 @@ sint r8712_set_key(struct _adapter *adapter,
 			psecuritypriv->DefKey[keyid].skey, keylen);
 		break;
 	case _TKIP_:
-		if (keyid < 1 || keyid > 2)
-			return _FAIL;
+		if (keyid < 1 || keyid > 2) {
+			ret = _FAIL;
+			goto err_free_parm;
+		}
 		keylen = 16;
 		memcpy(psetkeyparm->key,
 			&psecuritypriv->XGrpKey[keyid - 1], keylen);
 		psetkeyparm->grpkey = 1;
 		break;
 	case _AES_:
-		if (keyid < 1 || keyid > 2)
-			return _FAIL;
+		if (keyid < 1 || keyid > 2) {
+			ret = _FAIL;
+			goto err_free_parm;
+		}
 		keylen = 16;
 		memcpy(psetkeyparm->key,
 			&psecuritypriv->XGrpKey[keyid - 1], keylen);
 		psetkeyparm->grpkey = 1;
 		break;
 	default:
-		return _FAIL;
+		ret = _FAIL;
+		goto err_free_parm;
 	}
 	pcmd->cmdcode = _SetKey_CMD_;
 	pcmd->parmbuf = (u8 *)psetkeyparm;
@@ -1299,7 +1301,13 @@ sint r8712_set_key(struct _adapter *adapter,
 	pcmd->rspsz = 0;
 	_init_listhead(&pcmd->list);
 	r8712_enqueue_cmd(pcmdpriv, pcmd);
-	return _SUCCESS;
+	return ret;
+
+err_free_parm:
+	kfree(psetkeyparm);
+err_free_cmd:
+	kfree(pcmd);
+	return ret;
 }
 
 /* adjust IEs for r8712_joinbss_cmd in WMM */
diff --git a/drivers/staging/rtl8712/rtl871x_mp.c b/drivers/staging/rtl8712/rtl871x_mp.c
index 0563318a19f..389062fe8ea 100644
--- a/drivers/staging/rtl8712/rtl871x_mp.c
+++ b/drivers/staging/rtl8712/rtl871x_mp.c
@@ -53,8 +53,9 @@ static int init_mp_priv(struct mp_priv *pmp_priv)
 	_init_mp_priv_(pmp_priv);
 	_init_queue(&pmp_priv->free_mp_xmitqueue);
 	pmp_priv->pallocated_mp_xmitframe_buf = NULL;
-	pmp_priv->pallocated_mp_xmitframe_buf = _malloc(NR_MP_XMITFRAME *
-					 sizeof(struct mp_xmit_frame) + 4);
+	pmp_priv->pallocated_mp_xmitframe_buf = kmalloc(NR_MP_XMITFRAME *
+							sizeof(struct mp_xmit_frame) + 4,
+							GFP_ATOMIC);
 	if (pmp_priv->pallocated_mp_xmitframe_buf == NULL) {
 		res = _FAIL;
 		goto _exit_init_mp_priv;
@@ -80,9 +81,8 @@ _exit_init_mp_priv:
 
 static int free_mp_priv(struct mp_priv *pmp_priv)
 {
-	int res = 0;
 	kfree(pmp_priv->pallocated_mp_xmitframe_buf);
-	return res;
+	return 0;
 }
 
 void mp871xinit(struct _adapter *padapter)
@@ -281,11 +281,10 @@ void r8712_SetChannel(struct _adapter *pAdapter)
 	struct SetChannel_parm *pparm = NULL;
 	u16 code = GEN_CMD_CODE(_SetChannel);
 
-	pcmd = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	pcmd = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (pcmd == NULL)
 		return;
-	pparm = (struct SetChannel_parm *)_malloc(sizeof(struct
-					 SetChannel_parm));
+	pparm = kmalloc(sizeof(struct SetChannel_parm), GFP_ATOMIC);
 	if (pparm == NULL) {
 		kfree(pcmd);
 		return;
diff --git a/drivers/staging/rtl8712/rtl871x_mp_ioctl.c b/drivers/staging/rtl8712/rtl871x_mp_ioctl.c
index 5bd42966fd5..9827ff8143b 100644
--- a/drivers/staging/rtl8712/rtl871x_mp_ioctl.c
+++ b/drivers/staging/rtl8712/rtl871x_mp_ioctl.c
@@ -66,7 +66,6 @@ uint oid_rt_wireless_mode_hdl(struct oid_par_priv *poid_par_priv)
 
 uint oid_rt_pro_write_bb_reg_hdl(struct oid_par_priv *poid_par_priv)
 {
-	uint status = RNDIS_STATUS_SUCCESS;
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
 	struct bb_reg_param *pbbreg;
@@ -83,12 +82,11 @@ uint oid_rt_pro_write_bb_reg_hdl(struct oid_par_priv *poid_par_priv)
 		offset |= BB_REG_BASE_ADDR;
 	value = pbbreg->value;
 	r8712_bb_reg_write(Adapter, offset, value);
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_pro_read_bb_reg_hdl(struct oid_par_priv *poid_par_priv)
 {
-	uint status = RNDIS_STATUS_SUCCESS;
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
 	struct bb_reg_param *pbbreg;
@@ -106,12 +104,11 @@ uint oid_rt_pro_read_bb_reg_hdl(struct oid_par_priv *poid_par_priv)
 	value = r8712_bb_reg_read(Adapter, offset);
 	pbbreg->value = value;
 	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_pro_write_rf_reg_hdl(struct oid_par_priv *poid_par_priv)
 {
-	uint status = RNDIS_STATUS_SUCCESS;
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
 	struct rf_reg_param *pbbreg;
@@ -130,14 +127,13 @@ uint oid_rt_pro_write_rf_reg_hdl(struct oid_par_priv *poid_par_priv)
 	offset = (u8)pbbreg->offset;
 	value = pbbreg->value;
 	r8712_rf_reg_write(Adapter, path, offset, value);
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_pro_read_rf_reg_hdl(struct oid_par_priv *poid_par_priv)
 {
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
-	uint status = RNDIS_STATUS_SUCCESS;
 	struct rf_reg_param *pbbreg;
 	u8 path;
 	u8 offset;
@@ -155,7 +151,7 @@ uint oid_rt_pro_read_rf_reg_hdl(struct oid_par_priv *poid_par_priv)
 	value = r8712_rf_reg_read(Adapter, path, offset);
 	pbbreg->value = value;
 	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 /*This function initializes the DUT to the MP test mode*/
@@ -266,7 +262,6 @@ uint oid_rt_pro_set_data_rate_hdl(struct oid_par_priv
 {
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
-	uint status = RNDIS_STATUS_SUCCESS;
 	u32 ratevalue;
 
 	if (poid_par_priv->type_of_oid != SET_OID)
@@ -278,7 +273,7 @@ uint oid_rt_pro_set_data_rate_hdl(struct oid_par_priv
 		return RNDIS_STATUS_INVALID_DATA;
 	Adapter->mppriv.curr_rateidx = ratevalue;
 	r8712_SetDataRate(Adapter);
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_pro_start_test_hdl(struct oid_par_priv *poid_par_priv)
@@ -328,7 +323,6 @@ uint oid_rt_pro_set_channel_direct_call_hdl(struct oid_par_priv
 {
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
-	uint status = RNDIS_STATUS_SUCCESS;
 	u32		Channel;
 
 	if (poid_par_priv->type_of_oid != SET_OID)
@@ -340,14 +334,13 @@ uint oid_rt_pro_set_channel_direct_call_hdl(struct oid_par_priv
 		return RNDIS_STATUS_NOT_ACCEPTED;
 	Adapter->mppriv.curr_ch = Channel;
 	r8712_SetChannel(Adapter);
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_pro_set_antenna_bb_hdl(struct oid_par_priv *poid_par_priv)
 {
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
-	uint status = RNDIS_STATUS_SUCCESS;
 	u32 antenna;
 
 	if (poid_par_priv->type_of_oid != SET_OID)
@@ -358,7 +351,7 @@ uint oid_rt_pro_set_antenna_bb_hdl(struct oid_par_priv *poid_par_priv)
 	Adapter->mppriv.antenna_tx = (u16)((antenna & 0xFFFF0000) >> 16);
 	Adapter->mppriv.antenna_rx = (u16)(antenna & 0x0000FFFF);
 	r8712_SwitchAntenna(Adapter);
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_pro_set_tx_power_control_hdl(
@@ -366,7 +359,6 @@ uint oid_rt_pro_set_tx_power_control_hdl(
 {
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
-	uint status = RNDIS_STATUS_SUCCESS;
 	u32 tx_pwr_idx;
 
 	if (poid_par_priv->type_of_oid != SET_OID)
@@ -378,7 +370,7 @@ uint oid_rt_pro_set_tx_power_control_hdl(
 		return RNDIS_STATUS_NOT_ACCEPTED;
 	Adapter->mppriv.curr_txpoweridx = (u8)tx_pwr_idx;
 	r8712_SetTxPower(Adapter);
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_pro_query_tx_packet_sent_hdl(
@@ -839,7 +831,6 @@ uint oid_rt_poll_rx_status_hdl(struct oid_par_priv *poid_par_priv)
 {
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
-	uint status = RNDIS_STATUS_SUCCESS;
 
 	if (poid_par_priv->type_of_oid != QUERY_OID)
 		return RNDIS_STATUS_NOT_ACCEPTED;
@@ -847,7 +838,7 @@ uint oid_rt_poll_rx_status_hdl(struct oid_par_priv *poid_par_priv)
 		(unsigned char *)&Adapter->mppriv.rxstat,
 		sizeof(struct recv_stat));
 	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_pro_cfg_debug_message_hdl(struct oid_par_priv
@@ -875,7 +866,6 @@ uint oid_rt_get_thermal_meter_hdl(struct oid_par_priv *poid_par_priv)
 {
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
-	uint status = RNDIS_STATUS_SUCCESS;
 
 	if (poid_par_priv->type_of_oid != QUERY_OID)
 		return RNDIS_STATUS_NOT_ACCEPTED;
@@ -897,7 +887,7 @@ uint oid_rt_get_thermal_meter_hdl(struct oid_par_priv *poid_par_priv)
 	*(u32 *)poid_par_priv->information_buf =
 				 Adapter->mppriv.workparam.io_value;
 	*poid_par_priv->bytes_rw = sizeof(u32);
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_pro_set_power_tracking_hdl(struct oid_par_priv
@@ -1091,7 +1081,6 @@ uint oid_rt_pro_dele_sta_info_hdl(struct oid_par_priv *poid_par_priv)
 				   (poid_par_priv->adapter_context);
 
 	unsigned long			irqL;
-	uint status = RNDIS_STATUS_SUCCESS;
 
 	struct sta_info		*psta = NULL;
 	u8			*macaddr;
@@ -1113,7 +1102,7 @@ uint oid_rt_pro_dele_sta_info_hdl(struct oid_par_priv *poid_par_priv)
 		spin_unlock_irqrestore(&(Adapter->stapriv.sta_hash_lock), irqL);
 	}
 
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 /*--------------------------------------------------------------------------*/
 static u32 mp_query_drv_var(struct _adapter *padapter, u8 offset, u32 var)
@@ -1126,8 +1115,6 @@ uint oid_rt_pro_query_dr_variable_hdl(struct oid_par_priv *poid_par_priv)
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
 
-	uint status = RNDIS_STATUS_SUCCESS;
-
 	struct DR_VARIABLE_STRUCT *pdrv_var;
 
 	if (poid_par_priv->type_of_oid != QUERY_OID)
@@ -1139,7 +1126,7 @@ uint oid_rt_pro_query_dr_variable_hdl(struct oid_par_priv *poid_par_priv)
 	pdrv_var->variable = mp_query_drv_var(Adapter, pdrv_var->offset,
 					      pdrv_var->variable);
 	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 /*--------------------------------------------------------------------------*/
@@ -1243,7 +1230,6 @@ uint oid_rt_get_efuse_current_size_hdl(struct oid_par_priv
 {
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
-	uint status = RNDIS_STATUS_SUCCESS;
 
 	if (poid_par_priv->type_of_oid != QUERY_OID)
 		return RNDIS_STATUS_NOT_ACCEPTED;
@@ -1254,14 +1240,13 @@ uint oid_rt_get_efuse_current_size_hdl(struct oid_par_priv
 				 r8712_efuse_get_current_size(Adapter);
 	r8712_efuse_reg_uninit(Adapter);
 	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_get_efuse_max_size_hdl(struct oid_par_priv *poid_par_priv)
 {
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
-	uint status = RNDIS_STATUS_SUCCESS;
 
 	if (poid_par_priv->type_of_oid != QUERY_OID)
 		return RNDIS_STATUS_NOT_ACCEPTED;
@@ -1270,7 +1255,7 @@ uint oid_rt_get_efuse_max_size_hdl(struct oid_par_priv *poid_par_priv)
 	*(int *)poid_par_priv->information_buf =
 					 r8712_efuse_get_max_size(Adapter);
 	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_pro_efuse_hdl(struct oid_par_priv *poid_par_priv)
@@ -1320,7 +1305,6 @@ uint oid_rt_set_bandwidth_hdl(struct oid_par_priv *poid_par_priv)
 {
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
-	uint status = RNDIS_STATUS_SUCCESS;
 	u32		bandwidth;
 
 	if (poid_par_priv->type_of_oid != SET_OID)
@@ -1332,14 +1316,13 @@ uint oid_rt_set_bandwidth_hdl(struct oid_par_priv *poid_par_priv)
 		bandwidth = HT_CHANNEL_WIDTH_40;
 	Adapter->mppriv.curr_bandwidth = (u8)bandwidth;
 	r8712_SwitchBandwidth(Adapter);
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_set_crystal_cap_hdl(struct oid_par_priv *poid_par_priv)
 {
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
-	uint status = RNDIS_STATUS_SUCCESS;
 	u32		crystal_cap = 0;
 
 	if (poid_par_priv->type_of_oid != SET_OID)
@@ -1351,7 +1334,7 @@ uint oid_rt_set_crystal_cap_hdl(struct oid_par_priv *poid_par_priv)
 		return RNDIS_STATUS_NOT_ACCEPTED;
 	Adapter->mppriv.curr_crystalcap = crystal_cap;
 	r8712_SetCrystalCap(Adapter);
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_set_rx_packet_type_hdl(struct oid_par_priv
diff --git a/drivers/staging/rtl8712/rtl871x_recv.c b/drivers/staging/rtl8712/rtl871x_recv.c
index 274c359279e..eb775872c93 100644
--- a/drivers/staging/rtl8712/rtl871x_recv.c
+++ b/drivers/staging/rtl8712/rtl871x_recv.c
@@ -72,9 +72,9 @@ sint _r8712_init_recv_priv(struct recv_priv *precvpriv,
 	_init_queue(&precvpriv->recv_pending_queue);
 	precvpriv->adapter = padapter;
 	precvpriv->free_recvframe_cnt = NR_RECVFRAME;
-	precvpriv->pallocated_frame_buf = _malloc(NR_RECVFRAME *
-					   sizeof(union recv_frame) +
-					   RXFRAME_ALIGN_SZ);
+	precvpriv->pallocated_frame_buf = kmalloc(NR_RECVFRAME *
+						  sizeof(union recv_frame) + RXFRAME_ALIGN_SZ,
+						  GFP_ATOMIC);
 	if (precvpriv->pallocated_frame_buf == NULL)
 		return _FAIL;
 	kmemleak_not_leak(precvpriv->pallocated_frame_buf);
@@ -605,8 +605,6 @@ sint r8712_wlanhdr_to_ethhdr(union recv_frame *precvframe)
 	u8	bsnaphdr;
 	u8	*psnap_type;
 	struct ieee80211_snap_hdr *psnap;
-
-	sint ret = _SUCCESS;
 	struct _adapter	*adapter = precvframe->u.hdr.adapter;
 	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
 
@@ -653,7 +651,7 @@ sint r8712_wlanhdr_to_ethhdr(union recv_frame *precvframe)
 		len = htons(len);
 		memcpy(ptr + 12, &len, 2);
 	}
-	return ret;
+	return _SUCCESS;
 }
 
 s32 r8712_recv_entry(union recv_frame *precvframe)
diff --git a/drivers/staging/rtl8712/rtl871x_security.c b/drivers/staging/rtl8712/rtl871x_security.c
index 5ffc489e950..0912f52def9 100644
--- a/drivers/staging/rtl8712/rtl871x_security.c
+++ b/drivers/staging/rtl8712/rtl871x_security.c
@@ -272,6 +272,7 @@ static void secmicputuint32(u8 *p, u32 val)
 /* Convert from Us4Byte32 to Byte[] in a portable way */
 {
 	long i;
+
 	for (i = 0; i < 4; i++) {
 		*p++ = (u8) (val & 0xff);
 		val >>= 8;
@@ -765,6 +766,7 @@ static void xor_128(u8 *a, u8 *b, u8 *out)
 static void xor_32(u8 *a, u8 *b, u8 *out)
 {
 	sint i;
+
 	for (i = 0; i < 4; i++)
 		out[i] = a[i] ^ b[i];
 }
@@ -798,6 +800,7 @@ static void next_key(u8 *key, sint round)
 static void byte_sub(u8 *in, u8 *out)
 {
 	sint i;
+
 	for (i = 0; i < 16; i++)
 		out[i] = sbox(in[i]);
 }
@@ -1164,7 +1167,7 @@ u32 r8712_aes_encrypt(struct _adapter *padapter, u8 *pxmitframe)
 		return _FAIL;
 	pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + TXDESC_OFFSET;
 	/* 4 start to encrypt each fragment */
-	if ((pattrib->encrypt == _AES_)) {
+	if (pattrib->encrypt == _AES_) {
 		if (pattrib->psta)
 			stainfo = pattrib->psta;
 		else
@@ -1175,7 +1178,7 @@ u32 r8712_aes_encrypt(struct _adapter *padapter, u8 *pxmitframe)
 			prwskeylen = 16;
 			for (curfragnum = 0; curfragnum < pattrib->nr_frags;
 			     curfragnum++) {
-				if ((curfragnum + 1) == pattrib->nr_frags) {\
+				if ((curfragnum + 1) == pattrib->nr_frags) {
 					length = pattrib->last_txcmdsz -
 						 pattrib->hdrlen -
 						 pattrib->iv_len -
@@ -1371,7 +1374,7 @@ u32 r8712_aes_decrypt(struct _adapter *padapter, u8 *precvframe)
 	pframe = (unsigned char *)((union recv_frame *)precvframe)->
 		 u.hdr.rx_data;
 	/* 4 start to encrypt each fragment */
-	if ((prxattrib->encrypt == _AES_)) {
+	if (prxattrib->encrypt == _AES_) {
 		stainfo = r8712_get_stainfo(&padapter->stapriv,
 					    &prxattrib->ta[0]);
 		if (stainfo != NULL) {
diff --git a/drivers/staging/rtl8712/rtl871x_sta_mgt.c b/drivers/staging/rtl8712/rtl871x_sta_mgt.c
index 8db6849d4b2..6c649842abd 100644
--- a/drivers/staging/rtl8712/rtl871x_sta_mgt.c
+++ b/drivers/staging/rtl8712/rtl871x_sta_mgt.c
@@ -51,8 +51,8 @@ u32 _r8712_init_sta_priv(struct	sta_priv *pstapriv)
 	struct sta_info *psta;
 	s32 i;
 
-	pstapriv->pallocated_stainfo_buf = _malloc(sizeof(struct sta_info) *
-						   NUM_STA + 4);
+	pstapriv->pallocated_stainfo_buf = kmalloc(sizeof(struct sta_info) *
+						   NUM_STA + 4, GFP_ATOMIC);
 	if (pstapriv->pallocated_stainfo_buf == NULL)
 		return _FAIL;
 	pstapriv->pstainfo_buf = pstapriv->pallocated_stainfo_buf + 4 -
diff --git a/drivers/staging/rtl8712/rtl871x_xmit.c b/drivers/staging/rtl8712/rtl871x_xmit.c
index 78f570b571a..230681a8042 100644
--- a/drivers/staging/rtl8712/rtl871x_xmit.c
+++ b/drivers/staging/rtl8712/rtl871x_xmit.c
@@ -87,8 +87,8 @@ sint _r8712_init_xmit_priv(struct xmit_priv *pxmitpriv,
 	and initialize free_xmit_frame below.
 	Please also apply  free_txobj to link_up all the xmit_frames...
 	*/
-	pxmitpriv->pallocated_frame_buf = _malloc(NR_XMITFRAME *
-					  sizeof(struct xmit_frame) + 4);
+	pxmitpriv->pallocated_frame_buf = kmalloc(NR_XMITFRAME * sizeof(struct xmit_frame) + 4,
+						  GFP_ATOMIC);
 	if (pxmitpriv->pallocated_frame_buf == NULL) {
 		pxmitpriv->pxmit_frame_buf = NULL;
 		return _FAIL;
@@ -126,8 +126,8 @@ sint _r8712_init_xmit_priv(struct xmit_priv *pxmitpriv,
 	/*init xmit_buf*/
 	_init_queue(&pxmitpriv->free_xmitbuf_queue);
 	_init_queue(&pxmitpriv->pending_xmitbuf_queue);
-	pxmitpriv->pallocated_xmitbuf = _malloc(NR_XMITBUFF *
-					sizeof(struct xmit_buf) + 4);
+	pxmitpriv->pallocated_xmitbuf = kmalloc(NR_XMITBUFF * sizeof(struct xmit_buf) + 4,
+						GFP_ATOMIC);
 	if (pxmitpriv->pallocated_xmitbuf  == NULL)
 		return _FAIL;
 	pxmitpriv->pxmitbuf = pxmitpriv->pallocated_xmitbuf + 4 -
@@ -135,8 +135,8 @@ sint _r8712_init_xmit_priv(struct xmit_priv *pxmitpriv,
 	pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf;
 	for (i = 0; i < NR_XMITBUFF; i++) {
 		_init_listhead(&pxmitbuf->list);
-		pxmitbuf->pallocated_buf = _malloc(MAX_XMITBUF_SZ +
-					   XMITBUF_ALIGN_SZ);
+		pxmitbuf->pallocated_buf = kmalloc(MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ,
+						   GFP_ATOMIC);
 		if (pxmitbuf->pallocated_buf == NULL)
 			return _FAIL;
 		pxmitbuf->pbuf = pxmitbuf->pallocated_buf + XMITBUF_ALIGN_SZ -
@@ -955,8 +955,8 @@ static void alloc_hwxmits(struct _adapter *padapter)
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 
 	pxmitpriv->hwxmit_entry = HWXMIT_ENTRY;
-	pxmitpriv->hwxmits = (struct hw_xmit *)_malloc(sizeof(struct hw_xmit) *
-			     pxmitpriv->hwxmit_entry);
+	pxmitpriv->hwxmits = kmalloc(sizeof(struct hw_xmit) * pxmitpriv->hwxmit_entry,
+				     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 bbd5888e316..ba743542bab 100644
--- a/drivers/staging/rtl8712/usb_intf.c
+++ b/drivers/staging/rtl8712/usb_intf.c
@@ -595,7 +595,7 @@ static int r871xu_drv_init(struct usb_interface *pusb_intf,
 error:
 	usb_put_dev(udev);
 	usb_set_intfdata(pusb_intf, NULL);
-	if (padapter->dvobj_deinit != NULL)
+	if (padapter && padapter->dvobj_deinit != NULL)
 		padapter->dvobj_deinit(padapter);
 	if (pnetdev)
 		free_netdev(pnetdev);
diff --git a/drivers/staging/rtl8712/usb_ops_linux.c b/drivers/staging/rtl8712/usb_ops_linux.c
index dca398a0656..a6c2aab0a94 100644
--- a/drivers/staging/rtl8712/usb_ops_linux.c
+++ b/drivers/staging/rtl8712/usb_ops_linux.c
@@ -495,11 +495,11 @@ int r8712_usbctrl_vendorreq(struct intf_priv *pintfpriv, u8 request, u16 value,
 	 */
 	u8 *palloc_buf, *pIo_buf;
 
-	palloc_buf = _malloc((u32) len + 16);
+	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__);
-		return -1;
+		return -ENOMEM;
 	}
 	pIo_buf = palloc_buf + 16 - ((addr_t)(palloc_buf) & 0x0f);
 	if (requesttype == 0x01) {
diff --git a/drivers/staging/rtl8723au/Kconfig b/drivers/staging/rtl8723au/Kconfig
index 07fb5e4e50f..435f3594dab 100644
--- a/drivers/staging/rtl8723au/Kconfig
+++ b/drivers/staging/rtl8723au/Kconfig
@@ -19,14 +19,6 @@ config 8723AU_AP_MODE
 	will never be used as an AP, or the target system has limited memory,
 	"Y" should be selected.
 
-config 8723AU_P2P
-	bool "Realtek RTL8723AU Peer-to-peer mode"
-	default y
-	---help---
-	This option enables peer-to-peer mode for the r8723au driver. Unless you
-	know that peer-to-peer (P2P) mode will never be used, or the target system has
-	limited memory, "Y" should be selected.
-
 config 8723AU_BT_COEXIST
 	bool "Realtek RTL8723AU BlueTooth Coexistence"
 	default y
diff --git a/drivers/staging/rtl8723au/Makefile b/drivers/staging/rtl8723au/Makefile
index 11c6dd48646..a6316af94b7 100644
--- a/drivers/staging/rtl8723au/Makefile
+++ b/drivers/staging/rtl8723au/Makefile
@@ -1,14 +1,11 @@
 r8723au-y :=				\
-		core/rtw_ap.o		\
 		core/rtw_cmd.o		\
 		core/rtw_efuse.o	\
-		core/rtw_io.o		\
 		core/rtw_ioctl_set.o	\
 		core/rtw_ieee80211.o	\
 		core/rtw_led.o		\
 		core/rtw_mlme.o		\
 		core/rtw_mlme_ext.o	\
-		core/rtw_p2p.o		\
 		core/rtw_pwrctrl.o	\
 		core/rtw_recv.o		\
 		core/rtw_security.o	\
@@ -26,7 +23,6 @@ r8723au-y :=				\
 		hal/HalHWImg8723A_RF.o	\
 		hal/HalPwrSeqCmd.o	\
 		hal/odm_RegConfig8723A.o \
-		hal/rtl8723a_bt-coexist.o \
 		hal/odm_debug.o		\
 		hal/odm_interface.o	\
 		hal/odm_HWConfig.o	\
@@ -46,13 +42,15 @@ r8723au-y :=				\
 		hal/usb_ops_linux.o	\
 		os_dep/ioctl_cfg80211.o	\
 		os_dep/mlme_linux.o	\
-		os_dep/osdep_service.o	\
 		os_dep/os_intfs.o	\
 		os_dep/recv_linux.o	\
 		os_dep/usb_intf.o	\
 		os_dep/usb_ops_linux.o	\
 		os_dep/xmit_linux.o
 
+r8723au-$(CONFIG_8723AU_BT_COEXIST) += hal/rtl8723a_bt-coexist.o
+r8723au-$(CONFIG_8723AU_AP_MODE) += core/rtw_ap.o
+
 obj-$(CONFIG_R8723AU)	:= r8723au.o
 
-ccflags-y += -D__CHECK_ENDIAN__ -I$(src)/include
+ccflags-y += -Wtype-limits -D__CHECK_ENDIAN__ -I$(src)/include
diff --git a/drivers/staging/rtl8723au/core/rtw_ap.c b/drivers/staging/rtl8723au/core/rtw_ap.c
index a357e98cb83..c8700b38386 100644
--- a/drivers/staging/rtl8723au/core/rtw_ap.c
+++ b/drivers/staging/rtl8723au/core/rtw_ap.c
@@ -18,10 +18,9 @@
 #include <drv_types.h>
 #include <linux/ieee80211.h>
 #include <wifi.h>
+#include <rtl8723a_cmd.h>
+#include <rtl8723a_hal.h>
 
-#ifdef CONFIG_8723AU_AP_MODE
-
-extern unsigned char RTW_WPA_OUI23A[];
 extern unsigned char WMM_OUI23A[];
 extern unsigned char WPS_OUI23A[];
 extern unsigned char P2P_OUI23A[];
@@ -74,12 +73,13 @@ static void update_BCNTIM(struct rtw_adapter *padapter)
 	struct wlan_bssid_ex *pnetwork_mlmeext = &pmlmeinfo->network;
 	unsigned char *pie = pnetwork_mlmeext->IEs;
 	u8 *p, *dst_ie, *premainder_ie = NULL, *pbackup_remainder_ie = NULL;
-	u16 tim_bitmap_le;
+	__le16 tim_bitmap_le;
 	uint offset, tmp_len, tim_ielen, tim_ie_offset, remainder_ielen;
 
 	tim_bitmap_le = cpu_to_le16(pstapriv->tim_bitmap);
 
-	p = rtw_get_ie23a(pie + _FIXED_IE_LENGTH_, _TIM_IE_, &tim_ielen, pnetwork_mlmeext->IELength - _FIXED_IE_LENGTH_);
+	p = rtw_get_ie23a(pie + _FIXED_IE_LENGTH_, WLAN_EID_TIM, &tim_ielen,
+			  pnetwork_mlmeext->IELength - _FIXED_IE_LENGTH_);
 	if (p != NULL && tim_ielen>0) {
 		tim_ielen += 2;
 
@@ -98,12 +98,16 @@ static void update_BCNTIM(struct rtw_adapter *padapter)
 		offset = _FIXED_IE_LENGTH_;
 
 		/* get ssid_ie len */
-		p = rtw_get_ie23a(pie + _BEACON_IE_OFFSET_, _SSID_IE_, &tmp_len, (pnetwork_mlmeext->IELength - _BEACON_IE_OFFSET_));
+		p = rtw_get_ie23a(pie + _BEACON_IE_OFFSET_, WLAN_EID_SSID,
+				  &tmp_len, (pnetwork_mlmeext->IELength -
+					     _BEACON_IE_OFFSET_));
 		if (p != NULL)
 			offset += tmp_len+2;
 
 		/*  get supported rates len */
-		p = rtw_get_ie23a(pie + _BEACON_IE_OFFSET_, _SUPPORTEDRATES_IE_, &tmp_len, (pnetwork_mlmeext->IELength - _BEACON_IE_OFFSET_));
+		p = rtw_get_ie23a(pie + _BEACON_IE_OFFSET_, WLAN_EID_SUPP_RATES,
+				  &tmp_len, (pnetwork_mlmeext->IELength -
+					     _BEACON_IE_OFFSET_));
 		if (p !=  NULL)
 			offset += tmp_len+2;
 
@@ -124,7 +128,7 @@ static void update_BCNTIM(struct rtw_adapter *padapter)
 			memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);
 	}
 
-	*dst_ie++= _TIM_IE_;
+	*dst_ie++= WLAN_EID_TIM;
 
 	if ((pstapriv->tim_bitmap&0xff00) && (pstapriv->tim_bitmap&0x00fc))
 		tim_ielen = 5;
@@ -182,7 +186,7 @@ void	expire_timeout_chk23a(struct rtw_adapter *padapter)
 	struct sta_info *psta;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	u8 chk_alive_num = 0;
-	char chk_alive_list[NUM_STA];
+	struct sta_info *chk_alive_list[NUM_STA];
 	int i;
 
 	spin_lock_bh(&pstapriv->auth_list_lock);
@@ -248,7 +252,7 @@ void	expire_timeout_chk23a(struct rtw_adapter *padapter)
 
 					/* to update bcn with tim_bitmap for this station */
 					pstapriv->tim_bitmap |= CHKBIT(psta->aid);
-					update_beacon23a(padapter, _TIM_IE_, NULL, false);
+					update_beacon23a(padapter, WLAN_EID_TIM, NULL, false);
 
 					if (!pmlmeext->active_keep_alive_check)
 						continue;
@@ -256,13 +260,7 @@ void	expire_timeout_chk23a(struct rtw_adapter *padapter)
 			}
 
 			if (pmlmeext->active_keep_alive_check) {
-				int stainfo_offset;
-
-				stainfo_offset = rtw_stainfo_offset23a(pstapriv, psta);
-				if (stainfo_offset_valid(stainfo_offset)) {
-					chk_alive_list[chk_alive_num++] = stainfo_offset;
-				}
-
+				chk_alive_list[chk_alive_num++] = psta;
 				continue;
 			}
 
@@ -296,14 +294,14 @@ void	expire_timeout_chk23a(struct rtw_adapter *padapter)
 		if (rtw_get_oper_ch23a(padapter) != pmlmeext->cur_channel) {
 			backup_oper_channel = rtw_get_oper_ch23a(padapter);
 			SelectChannel23a(padapter, pmlmeext->cur_channel);
-	}
+		}
 
 	/* issue null data to check sta alive*/
 	for (i = 0; i < chk_alive_num; i++) {
 
 		int ret = _FAIL;
 
-		psta = rtw_get_stainfo23a_by_offset23a(pstapriv, chk_alive_list[i]);
+		psta = chk_alive_list[i];
 		if (!(psta->state &_FW_LINKED))
 			continue;
 
@@ -357,7 +355,7 @@ void add_RATid23a(struct rtw_adapter *padapter, struct sta_info *psta, u8 rssi_l
 	unsigned int tx_ra_bitmap = 0;
 	struct ht_priv *psta_ht = NULL;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct wlan_bssid_ex *pcur_network = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network;
+	struct wlan_bssid_ex *pcur_network = &pmlmepriv->cur_network.network;
 
 	if (psta)
 		psta_ht = &psta->htpriv;
@@ -374,24 +372,24 @@ void add_RATid23a(struct rtw_adapter *padapter, struct sta_info *psta, u8 rssi_l
 			tx_ra_bitmap |= rtw_get_bit_value_from_ieee_value23a(psta->bssrateset[i]&0x7f);
 	}
 	/* n mode ra_bitmap */
-	if (psta_ht->ht_option)
-	{
-		rtw23a_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+	if (psta_ht->ht_option) {
+		rf_type = rtl8723a_get_rf_type(padapter);
+
 		if (rf_type == RF_2T2R)
 			limit = 16;/*  2R */
 		else
 			limit = 8;/*   1R */
 
-		for (i = 0; i<limit; i++) {
-			if (psta_ht->ht_cap.mcs.rx_mask[i/8] & BIT(i%8))
-				tx_ra_bitmap |= CHKBIT(i+12);
+		for (i = 0; i < limit; i++) {
+			if (psta_ht->ht_cap.mcs.rx_mask[i / 8] & BIT(i % 8))
+				tx_ra_bitmap |= BIT(i + 12);
 		}
 
 		/* max short GI rate */
 		shortGIrate = psta_ht->sgi;
 	}
 
-	if (pcur_network->Configuration.DSConfig > 14) {
+	if (pcur_network->DSConfig > 14) {
 		/*  5G band */
 		if (tx_ra_bitmap & 0xffff000)
 			sta_band |= WIRELESS_11_5N | WIRELESS_11A;
@@ -432,7 +430,7 @@ void add_RATid23a(struct rtw_adapter *padapter, struct sta_info *psta, u8 rssi_l
 		/* bitmap[28:31]= Rate Adaptive id */
 		/* arg[0:4] = macid */
 		/* arg[5] = Short GI */
-		rtw_hal_add_ra_tid23a(padapter, tx_ra_bitmap, arg, rssi_level);
+		rtl8723a_add_rateatid(padapter, tx_ra_bitmap, arg, rssi_level);
 
 		if (shortGIrate == true)
 			init_rate |= BIT(6);
@@ -455,7 +453,7 @@ static void update_bmc_sta(struct rtw_adapter *padapter)
 	int i, supportRateNum = 0;
 	unsigned int tx_ra_bitmap = 0;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct wlan_bssid_ex *pcur_network = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network;
+	struct wlan_bssid_ex *pcur_network = &pmlmepriv->cur_network.network;
 	struct sta_info *psta = rtw_get_bcmc_stainfo23a(padapter);
 
 	if (psta)
@@ -484,7 +482,7 @@ static void update_bmc_sta(struct rtw_adapter *padapter)
 				tx_ra_bitmap |= rtw_get_bit_value_from_ieee_value23a(psta->bssrateset[i]&0x7f);
 		}
 
-		if (pcur_network->Configuration.DSConfig > 14) {
+		if (pcur_network->DSConfig > 14) {
 			/* force to A mode. 5G doesn't support CCK rates */
 			network_type = WIRELESS_11A;
 			tx_ra_bitmap = 0x150; /*  6, 12, 24 Mbps */
@@ -498,7 +496,7 @@ static void update_bmc_sta(struct rtw_adapter *padapter)
 		init_rate = get_highest_rate_idx23a(tx_ra_bitmap&0x0fffffff)&0x3f;
 
 		/* ap mode */
-		rtw_hal_set_odm_var23a(padapter, HAL_ODM_STA_INFO, psta, true);
+		rtl8723a_SetHalODMVar(padapter, HAL_ODM_STA_INFO, psta, true);
 
 		{
 			u8 arg = 0;
@@ -515,16 +513,13 @@ static void update_bmc_sta(struct rtw_adapter *padapter)
 			/* bitmap[28:31]= Rate Adaptive id */
 			/* arg[0:4] = macid */
 			/* arg[5] = Short GI */
-			rtw_hal_add_ra_tid23a(padapter, tx_ra_bitmap, arg, 0);
-
+			rtl8723a_add_rateatid(padapter, tx_ra_bitmap, arg, 0);
 		}
 
 		/* set ra_id, init_rate */
 		psta->raid = raid;
 		psta->init_rate = init_rate;
 
-		rtw_stassoc_hw_rpt23a(padapter, psta);
-
 		spin_lock_bh(&psta->lock);
 		psta->state = _FW_LINKED;
 		spin_unlock_bh(&psta->lock);
@@ -556,7 +551,7 @@ void update_sta_info23a_apmode23a(struct rtw_adapter *padapter, struct sta_info
 	DBG_8723A("%s\n", __func__);
 
 	/* ap mode */
-	rtw_hal_set_odm_var23a(padapter, HAL_ODM_STA_INFO, psta, true);
+	rtl8723a_SetHalODMVar(padapter, HAL_ODM_STA_INFO, psta, true);
 
 	if (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X)
 		psta->ieee8021x_blocked = true;
@@ -632,9 +627,8 @@ static void update_hw_ht_param(struct rtw_adapter *padapter)
 
 	min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) >> 2;
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_AMPDU_MIN_SPACE, (u8 *)(&min_MPDU_spacing));
-
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_AMPDU_FACTOR, (u8 *)(&max_AMPDU_len));
+	rtl8723a_set_ampdu_min_space(padapter, min_MPDU_spacing);
+	rtl8723a_set_ampdu_factor(padapter, max_AMPDU_len);
 
 	/*  Config SM Power Save setting */
 	pmlmeinfo->SM_PS = (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info & 0x0C) >> 2;
@@ -644,25 +638,22 @@ static void update_hw_ht_param(struct rtw_adapter *padapter)
 
 static void start_bss_network(struct rtw_adapter *padapter, u8 *pbuf)
 {
-	u8 *p;
+	const u8 *p;
 	u8 val8, cur_channel, cur_bwmode, cur_ch_offset;
 	u16 bcn_interval;
 	u32 acparm;
-	int ie_len;
 	struct registry_priv *pregpriv = &padapter->registrypriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct security_priv* psecuritypriv = &padapter->securitypriv;
-	struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network;
+	struct wlan_bssid_ex *pnetwork = &pmlmepriv->cur_network.network;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *pnetwork_mlmeext = &pmlmeinfo->network;
 	struct HT_info_element *pht_info = NULL;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif /* CONFIG_8723AU_P2P */
+	int bcn_fixed_size;
 
-	bcn_interval = (u16)pnetwork->Configuration.BeaconPeriod;
-	cur_channel = pnetwork->Configuration.DSConfig;
+	bcn_interval = (u16)pnetwork->BeaconPeriod;
+	cur_channel = pnetwork->DSConfig;
 	cur_bwmode = HT_CHANNEL_WIDTH_20;;
 	cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 
@@ -675,7 +666,7 @@ static void start_bss_network(struct rtw_adapter *padapter, u8 *pbuf)
 	/* todo: update wmm, ht cap */
 	/* pmlmeinfo->WMM_enable; */
 	/* pmlmeinfo->HT_enable; */
-	if (pmlmepriv->qospriv.qos_option)
+	if (pmlmepriv->qos_option)
 		pmlmeinfo->WMM_enable = true;
 	if (pmlmepriv->htpriv.ht_option) {
 		pmlmeinfo->WMM_enable = true;
@@ -687,7 +678,10 @@ static void start_bss_network(struct rtw_adapter *padapter, u8 *pbuf)
 	if (pmlmepriv->cur_network.join_res != true) {
 		/* setting only at  first time */
 		/* WEP Key will be set before this function, do not clear CAM. */
-		if ((psecuritypriv->dot11PrivacyAlgrthm != _WEP40_) && (psecuritypriv->dot11PrivacyAlgrthm != _WEP104_))
+		if (psecuritypriv->dot11PrivacyAlgrthm !=
+		    WLAN_CIPHER_SUITE_WEP40 &&
+		    psecuritypriv->dot11PrivacyAlgrthm !=
+		    WLAN_CIPHER_SUITE_WEP104)
 			flush_all_cam_entry23a(padapter);	/* clear CAM */
 	}
 
@@ -695,27 +689,28 @@ static void start_bss_network(struct rtw_adapter *padapter, u8 *pbuf)
 	Set_MSR23a(padapter, _HW_STATE_AP_);
 
 	/* Set BSSID REG */
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_BSSID, pnetwork->MacAddress);
+	hw_var_set_bssid(padapter, pnetwork->MacAddress);
 
 	/* Set EDCA param reg */
 	acparm = 0x002F3217; /*  VO */
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acparm));
+	rtl8723a_set_ac_param_vo(padapter, acparm);
 	acparm = 0x005E4317; /*  VI */
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acparm));
+	rtl8723a_set_ac_param_vi(padapter, acparm);
 	acparm = 0x005ea42b;
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acparm));
+	rtl8723a_set_ac_param_be(padapter, acparm);
 	acparm = 0x0000A444; /*  BK */
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acparm));
+	rtl8723a_set_ac_param_bk(padapter, acparm);
 
 	/* Set Security */
-	val8 = (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X)? 0xcc: 0xcf;
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+	val8 = (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) ?
+		0xcc: 0xcf;
+	rtl8723a_set_sec_cfg(padapter, val8);
 
 	/* Beacon Control related register */
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&bcn_interval));
+	rtl8723a_set_beacon_interval(padapter, bcn_interval);
 
 	UpdateBrateTbl23a(padapter, pnetwork->SupportedRates);
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_BASIC_RATE, pnetwork->SupportedRates);
+	HalSetBrateCfg23a(padapter, pnetwork->SupportedRates);
 
 	if (!pmlmepriv->cur_network.join_res) {
 		/* setting only at  first time */
@@ -723,21 +718,26 @@ static void start_bss_network(struct rtw_adapter *padapter, u8 *pbuf)
 		/* disable dynamic functions, such as high power, DIG */
 
 		/* turn on all dynamic functions */
-		Switch_DM_Func23a(padapter, DYNAMIC_ALL_FUNC_ENABLE, true);
+		rtl8723a_odm_support_ability_set(padapter,
+						 DYNAMIC_ALL_FUNC_ENABLE);
 	}
 	/* set channel, bwmode */
-	p = rtw_get_ie23a((pnetwork->IEs + sizeof(struct ndis_802_11_fixed_ies)),
-			  _HT_ADD_INFO_IE_, &ie_len, (pnetwork->IELength -
-			  sizeof(struct ndis_802_11_fixed_ies)));
-	if (p && ie_len) {
-		pht_info = (struct HT_info_element *)(p+2);
+	bcn_fixed_size = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+		offsetof(struct ieee80211_mgmt, u.beacon);
+
+	p = cfg80211_find_ie(WLAN_EID_HT_OPERATION,
+			     pnetwork->IEs + bcn_fixed_size,
+			     pnetwork->IELength - bcn_fixed_size);
+	if (p && p[1]) {
+		pht_info = (struct HT_info_element *)(p + 2);
 
-		if ((pregpriv->cbw40_enable) &&	(pht_info->infos[0] & BIT(2))) {
+		if (pregpriv->cbw40_enable && pht_info->infos[0] & BIT(2)) {
 			/* switch to the 40M Hz mode */
 			cur_bwmode = HT_CHANNEL_WIDTH_40;
 			switch (pht_info->infos[0] & 0x3) {
 			case 1:
-				/* pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER; */
+				/* pmlmeext->cur_ch_offset =
+				   HAL_PRIME_CHNL_OFFSET_LOWER; */
 				cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
 				break;
 			case 3:
@@ -764,19 +764,13 @@ static void start_bss_network(struct rtw_adapter *padapter, u8 *pbuf)
 	update_wireless_mode23a(padapter);
 
 	/* udpate capability after cur_wireless_mode updated */
-	update_capinfo23a(padapter, rtw_get_capability23a((struct wlan_bssid_ex *)pnetwork));
+	update_capinfo23a(padapter, rtw_get_capability23a(pnetwork));
 
 	/* let pnetwork_mlmeext == pnetwork_mlme. */
 	memcpy(pnetwork_mlmeext, pnetwork, pnetwork->Length);
 
-#ifdef CONFIG_8723AU_P2P
-	memcpy(pwdinfo->p2p_group_ssid, pnetwork->Ssid.ssid,
-	       pnetwork->Ssid.ssid_len);
-	pwdinfo->p2p_group_ssid_len = pnetwork->Ssid.ssid_len;
-#endif /* CONFIG_8723AU_P2P */
-
 	if (pmlmeext->bstart_bss) {
-		update_beacon23a(padapter, _TIM_IE_, NULL, false);
+		update_beacon23a(padapter, WLAN_EID_TIM, NULL, false);
 
 		/* issue beacon frame */
 		if (send_beacon23a(padapter) == _FAIL)
@@ -787,19 +781,20 @@ static void start_bss_network(struct rtw_adapter *padapter, u8 *pbuf)
 	update_bmc_sta(padapter);
 }
 
-int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
+int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,
+			     unsigned int len)
 {
 	int ret = _SUCCESS;
 	u8 *p;
 	u8 *pHT_caps_ie = NULL;
 	u8 *pHT_info_ie = NULL;
 	struct sta_info *psta = NULL;
+	__le16 *pbeacon;
 	u16 cap, ht_cap = false;
 	uint ie_len = 0;
 	int group_cipher, pairwise_cipher;
 	u8 channel, network_type, supportRate[NDIS_802_11_LENGTH_RATES_EX];
 	int supportRateNum = 0;
-	u8 OUI1[] = {0x00, 0x50, 0xf2, 0x01};
 	u8 WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
 	struct registry_priv *pregistrypriv = &padapter->registrypriv;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
@@ -823,7 +818,7 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 	if (!check_fwstate(pmlmepriv, WIFI_AP_STATE))
 		return _FAIL;
 
-	if (len>MAX_IE_SZ)
+	if (len > MAX_IE_SZ)
 		return _FAIL;
 
 	pbss_network->IELength = len;
@@ -832,7 +827,8 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 
 	memcpy(ie, pbuf, pbss_network->IELength);
 
-	if (pbss_network->InfrastructureMode!= Ndis802_11APMode)
+	if (pbss_network->ifmode != NL80211_IFTYPE_AP &&
+	    pbss_network->ifmode != NL80211_IFTYPE_P2P_GO)
 		return _FAIL;
 
 	pbss_network->Rssi = 0;
@@ -841,14 +837,14 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 
 	/* beacon interval */
 	/* ie + 8;  8: TimeStamp, 2: Beacon Interval 2:Capability */
-	p = rtw_get_beacon_interval23a_from_ie(ie);
-	pbss_network->Configuration.BeaconPeriod = get_unaligned_le16(p);
+	pbeacon = rtw_get_beacon_interval23a_from_ie(ie);
+	pbss_network->BeaconPeriod = get_unaligned_le16(pbeacon);
 
 	/* capability */
 	cap = get_unaligned_le16(ie);
 
 	/* SSID */
-	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, _SSID_IE_, &ie_len,
+	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, WLAN_EID_SSID, &ie_len,
 			  (pbss_network->IELength -_BEACON_IE_OFFSET_));
 	if (p && ie_len > 0) {
 		memset(&pbss_network->Ssid, 0, sizeof(struct cfg80211_ssid));
@@ -858,17 +854,16 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 
 	/* chnnel */
 	channel = 0;
-	pbss_network->Configuration.Length = 0;
-	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, _DSSET_IE_, &ie_len,
+	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, WLAN_EID_DS_PARAMS, &ie_len,
 			  (pbss_network->IELength - _BEACON_IE_OFFSET_));
 	if (p && ie_len > 0)
 		channel = *(p + 2);
 
-	pbss_network->Configuration.DSConfig = channel;
+	pbss_network->DSConfig = channel;
 
 	memset(supportRate, 0, NDIS_802_11_LENGTH_RATES_EX);
 	/*  get supported rates */
-	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, _SUPPORTEDRATES_IE_, &ie_len,
+	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, WLAN_EID_SUPP_RATES, &ie_len,
 			  (pbss_network->IELength - _BEACON_IE_OFFSET_));
 	if (p) {
 		memcpy(supportRate, p+2, ie_len);
@@ -876,7 +871,7 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 	}
 
 	/* get ext_supported rates */
-	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, _EXT_SUPPORTEDRATES_IE_,
+	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, WLAN_EID_EXT_SUPP_RATES,
 			  &ie_len, pbss_network->IELength - _BEACON_IE_OFFSET_);
 	if (p) {
 		memcpy(supportRate+supportRateNum, p+2, ie_len);
@@ -889,10 +884,10 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 	rtw_set_supported_rate23a(pbss_network->SupportedRates, network_type);
 
 	/* parsing ERP_IE */
-	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, _ERPINFO_IE_, &ie_len,
+	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, WLAN_EID_ERP_INFO, &ie_len,
 			  (pbss_network->IELength - _BEACON_IE_OFFSET_));
 	if (p && ie_len > 0)
-		ERP_IE_handler23a(padapter, (struct ndis_802_11_var_ies *)p);
+		ERP_IE_handler23a(padapter, p);
 
 	/* update privacy/security */
 	if (cap & BIT(4))
@@ -904,9 +899,9 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 
 	/* wpa2 */
 	group_cipher = 0; pairwise_cipher = 0;
-	psecuritypriv->wpa2_group_cipher = _NO_PRIVACY_;
-	psecuritypriv->wpa2_pairwise_cipher = _NO_PRIVACY_;
-	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, _RSN_IE_2_, &ie_len,
+	psecuritypriv->wpa2_group_cipher = 0;
+	psecuritypriv->wpa2_pairwise_cipher = 0;
+	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, WLAN_EID_RSN, &ie_len,
 			  (pbss_network->IELength - _BEACON_IE_OFFSET_));
 	if (p && ie_len > 0) {
 		if (rtw_parse_wpa2_ie23a(p, ie_len+2, &group_cipher,
@@ -925,13 +920,13 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 	ie_len = 0;
 	group_cipher = 0;
 	pairwise_cipher = 0;
-	psecuritypriv->wpa_group_cipher = _NO_PRIVACY_;
-	psecuritypriv->wpa_pairwise_cipher = _NO_PRIVACY_;
+	psecuritypriv->wpa_group_cipher = 0;
+	psecuritypriv->wpa_pairwise_cipher = 0;
 	for (p = ie + _BEACON_IE_OFFSET_; ;p += (ie_len + 2)) {
-		p = rtw_get_ie23a(p, _SSN_IE_1_, &ie_len,
+		p = rtw_get_ie23a(p, WLAN_EID_VENDOR_SPECIFIC, &ie_len,
 				  (pbss_network->IELength - _BEACON_IE_OFFSET_ -
 				  (ie_len + 2)));
-		if ((p) && (!memcmp(p+2, OUI1, 4))) {
+		if ((p) && (!memcmp(p+2, RTW_WPA_OUI23A_TYPE, 4))) {
 			if (rtw_parse_wpa_ie23a(p, ie_len+2, &group_cipher,
 						&pairwise_cipher, NULL) == _SUCCESS) {
 				psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
@@ -953,14 +948,14 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 
 	/* wmm */
 	ie_len = 0;
-	pmlmepriv->qospriv.qos_option = 0;
+	pmlmepriv->qos_option = 0;
 	if (pregistrypriv->wmm_enable) {
 		for (p = ie + _BEACON_IE_OFFSET_; ;p += (ie_len + 2)) {
-			p = rtw_get_ie23a(p, _VENDOR_SPECIFIC_IE_, &ie_len,
+			p = rtw_get_ie23a(p, WLAN_EID_VENDOR_SPECIFIC, &ie_len,
 					  (pbss_network->IELength -
 					  _BEACON_IE_OFFSET_ - (ie_len + 2)));
 			if ((p) && !memcmp(p+2, WMM_PARA_IE, 6)) {
-				pmlmepriv->qospriv.qos_option = 1;
+				pmlmepriv->qos_option = 1;
 
 				*(p+8) |= BIT(7);/* QoS Info, support U-APSD */
 
@@ -978,7 +973,7 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 		}
 	}
 	/* parsing HT_CAP_IE */
-	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, _HT_CAPABILITY_IE_, &ie_len,
+	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, WLAN_EID_HT_CAPABILITY, &ie_len,
 			  (pbss_network->IELength - _BEACON_IE_OFFSET_));
 	if (p && ie_len > 0) {
 		u8 rf_type;
@@ -990,7 +985,7 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 		ht_cap = true;
 		network_type |= WIRELESS_11_24N;
 
-		rtw23a_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+		rf_type = rtl8723a_get_rf_type(padapter);
 
 		if ((psecuritypriv->wpa_pairwise_cipher & WPA_CIPHER_CCMP) ||
 		    (psecuritypriv->wpa2_pairwise_cipher & WPA_CIPHER_CCMP))
@@ -1010,29 +1005,11 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 	}
 
 	/* parsing HT_INFO_IE */
-	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, _HT_ADD_INFO_IE_, &ie_len,
+	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, WLAN_EID_HT_OPERATION, &ie_len,
 			  (pbss_network->IELength - _BEACON_IE_OFFSET_));
 	if (p && ie_len > 0)
 		pHT_info_ie = p;
 
-	switch (network_type) {
-	case WIRELESS_11B:
-		pbss_network->NetworkTypeInUse = Ndis802_11DS;
-		break;
-	case WIRELESS_11G:
-	case WIRELESS_11BG:
-            case WIRELESS_11G_24N:
-	case WIRELESS_11BG_24N:
-		pbss_network->NetworkTypeInUse = Ndis802_11OFDM24;
-		break;
-	case WIRELESS_11A:
-		pbss_network->NetworkTypeInUse = Ndis802_11OFDM5;
-		break;
-	default :
-		pbss_network->NetworkTypeInUse = Ndis802_11OFDM24;
-		break;
-	}
-
 	pmlmepriv->cur_network.network_type = network_type;
 
 	pmlmepriv->htpriv.ht_option = false;
@@ -1040,17 +1017,17 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 	/* ht_cap */
 	if (pregistrypriv->ht_enable && ht_cap) {
 		pmlmepriv->htpriv.ht_option = true;
-		pmlmepriv->qospriv.qos_option = 1;
+		pmlmepriv->qos_option = 1;
 
 		if (pregistrypriv->ampdu_enable == 1)
 			pmlmepriv->htpriv.ampdu_enable = true;
 
-		HT_caps_handler23a(padapter, (struct ndis_802_11_var_ies *)pHT_caps_ie);
+		HT_caps_handler23a(padapter, pHT_caps_ie);
 
-		HT_info_handler23a(padapter, (struct ndis_802_11_var_ies *)pHT_info_ie);
+		HT_info_handler23a(padapter, pHT_info_ie);
 	}
 
-	pbss_network->Length = get_wlan_bssid_ex_sz((struct wlan_bssid_ex  *)pbss_network);
+	pbss_network->Length = get_wlan_bssid_ex_sz(pbss_network);
 
 	/* issue beacon to start bss network */
 	start_bss_network(padapter, (u8*)pbss_network);
@@ -1058,7 +1035,9 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 	/* alloc sta_info for ap itself */
 	psta = rtw_get_stainfo23a(&padapter->stapriv, pbss_network->MacAddress);
 	if (!psta) {
-		psta = rtw_alloc_stainfo23a(&padapter->stapriv, pbss_network->MacAddress);
+		psta = rtw_alloc_stainfo23a(&padapter->stapriv,
+					    pbss_network->MacAddress,
+					    GFP_KERNEL);
 		if (!psta)
 			return _FAIL;
 	}
@@ -1200,24 +1179,21 @@ static void update_bcn_erpinfo_ie(struct rtw_adapter *padapter)
 		return;
 
 	/* parsing ERP_IE */
-	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, _ERPINFO_IE_, &len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
-	if (p && len>0)
-	{
-		struct ndis_802_11_var_ies * pIE = (struct ndis_802_11_var_ies *)p;
-
+	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, WLAN_EID_ERP_INFO, &len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
+	if (p && len > 0) {
 		if (pmlmepriv->num_sta_non_erp == 1)
-			pIE->data[0] |= WLAN_ERP_NON_ERP_PRESENT |
+			p[2] |= WLAN_ERP_NON_ERP_PRESENT |
 				WLAN_ERP_USE_PROTECTION;
 		else
-			pIE->data[0] &= ~(WLAN_ERP_NON_ERP_PRESENT |
-					  WLAN_ERP_USE_PROTECTION);
+			p[2] &= ~(WLAN_ERP_NON_ERP_PRESENT |
+				  WLAN_ERP_USE_PROTECTION);
 
 		if (pmlmepriv->num_sta_no_short_preamble > 0)
-			pIE->data[0] |= WLAN_ERP_BARKER_PREAMBLE;
+			p[2] |= WLAN_ERP_BARKER_PREAMBLE;
 		else
-			pIE->data[0] &= ~(WLAN_ERP_BARKER_PREAMBLE);
+			p[2] &= ~(WLAN_ERP_BARKER_PREAMBLE);
 
-		ERP_IE_handler23a(padapter, pIE);
+		ERP_IE_handler23a(padapter, p);
 	}
 }
 
@@ -1259,6 +1235,10 @@ static void update_bcn_wps_ie(struct rtw_adapter *padapter)
 
 	DBG_8723A("%s\n", __func__);
 
+	pwps_ie_src = pmlmepriv->wps_beacon_ie;
+	if (pwps_ie_src == NULL)
+		return;
+
 	pwps_ie = rtw_get_wps_ie23a(ie+_FIXED_IE_LENGTH_, ielen-_FIXED_IE_LENGTH_, NULL, &wps_ielen);
 
 	if (pwps_ie == NULL || wps_ielen == 0)
@@ -1277,10 +1257,6 @@ static void update_bcn_wps_ie(struct rtw_adapter *padapter)
 			       remainder_ielen);
 	}
 
-	pwps_ie_src = pmlmepriv->wps_beacon_ie;
-	if (pwps_ie_src == NULL)
-		return;
-
 	wps_ielen = (uint)pwps_ie_src[1];/* to get ie data len */
 	if ((wps_offset+wps_ielen+2+remainder_ielen)<= MAX_IE_SZ)
 	{
@@ -1306,26 +1282,16 @@ static void update_bcn_vendor_spec_ie(struct rtw_adapter *padapter, u8*oui)
 {
 	DBG_8723A("%s\n", __func__);
 
-	if (!memcmp(RTW_WPA_OUI23A, oui, 4))
-	{
+	if (!memcmp(RTW_WPA_OUI23A_TYPE, oui, 4))
 		update_bcn_wpa_ie(padapter);
-	}
 	else if (!memcmp(WMM_OUI23A, oui, 4))
-	{
 		update_bcn_wmm_ie(padapter);
-	}
 	else if (!memcmp(WPS_OUI23A, oui, 4))
-	{
 		update_bcn_wps_ie(padapter);
-	}
 	else if (!memcmp(P2P_OUI23A, oui, 4))
-	{
 		update_bcn_p2p_ie(padapter);
-	}
 	else
-	{
 		DBG_8723A("unknown OUI type!\n");
-	}
 }
 
 void update_beacon23a(struct rtw_adapter *padapter, u8 ie_id, u8 *oui, u8 tx)
@@ -1350,50 +1316,37 @@ void update_beacon23a(struct rtw_adapter *padapter, u8 ie_id, u8 *oui, u8 tx)
 
 	switch (ie_id)
 	{
-		case 0xFF:
-
-			update_bcn_fixed_ie(padapter);/* 8: TimeStamp, 2: Beacon Interval 2:Capability */
-
-			break;
-
-		case _TIM_IE_:
-
-			update_BCNTIM(padapter);
-
-			break;
-
-		case _ERPINFO_IE_:
-
-			update_bcn_erpinfo_ie(padapter);
-
-			break;
-
-		case _HT_CAPABILITY_IE_:
-
-			update_bcn_htcap_ie(padapter);
-
-			break;
-
-		case _RSN_IE_2_:
-
-			update_bcn_rsn_ie(padapter);
-
-			break;
+	case 0xFF:
+		/* 8: TimeStamp, 2: Beacon Interval 2:Capability */
+		update_bcn_fixed_ie(padapter);
+		break;
 
-		case _HT_ADD_INFO_IE_:
+	case WLAN_EID_TIM:
+		update_BCNTIM(padapter);
+		break;
 
-			update_bcn_htinfo_ie(padapter);
+	case WLAN_EID_ERP_INFO:
+		update_bcn_erpinfo_ie(padapter);
+		break;
 
-			break;
+	case WLAN_EID_HT_CAPABILITY:
+		update_bcn_htcap_ie(padapter);
+		break;
 
-		case _VENDOR_SPECIFIC_IE_:
+	case WLAN_EID_RSN:
+		update_bcn_rsn_ie(padapter);
+		break;
 
-			update_bcn_vendor_spec_ie(padapter, oui);
+	case WLAN_EID_HT_OPERATION:
+		update_bcn_htinfo_ie(padapter);
+		break;
 
-			break;
+	case WLAN_EID_VENDOR_SPECIFIC:
+		update_bcn_vendor_spec_ie(padapter, oui);
+		break;
 
-		default:
-			break;
+	default:
+		break;
 	}
 
 	pmlmepriv->update_bcn = true;
@@ -1432,28 +1385,28 @@ static int rtw_ht_operation_update(struct rtw_adapter *padapter)
 	DBG_8723A("%s current operation mode = 0x%X\n",
 		   __func__, pmlmepriv->ht_op_mode);
 
-	if (!(pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT)
+	if (!(pmlmepriv->ht_op_mode & IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT)
 	    && pmlmepriv->num_sta_ht_no_gf) {
 		pmlmepriv->ht_op_mode |=
-			HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT;
+			IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT;
 		op_mode_changes++;
 	} else if ((pmlmepriv->ht_op_mode &
-		    HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT) &&
+		    IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT) &&
 		   pmlmepriv->num_sta_ht_no_gf == 0) {
 		pmlmepriv->ht_op_mode &=
-			~HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT;
+			~IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT;
 		op_mode_changes++;
 	}
 
-	if (!(pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT) &&
+	if (!(pmlmepriv->ht_op_mode & IEEE80211_HT_OP_MODE_NON_HT_STA_PRSNT) &&
 	    (pmlmepriv->num_sta_no_ht || pmlmepriv->olbc_ht)) {
-		pmlmepriv->ht_op_mode |= HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT;
+		pmlmepriv->ht_op_mode |= IEEE80211_HT_OP_MODE_NON_HT_STA_PRSNT;
 		op_mode_changes++;
 	} else if ((pmlmepriv->ht_op_mode &
-		    HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT) &&
+		    IEEE80211_HT_OP_MODE_NON_HT_STA_PRSNT) &&
 		   (pmlmepriv->num_sta_no_ht == 0 && !pmlmepriv->olbc_ht)) {
 		pmlmepriv->ht_op_mode &=
-			~HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT;
+			~IEEE80211_HT_OP_MODE_NON_HT_STA_PRSNT;
 		op_mode_changes++;
 	}
 
@@ -1461,21 +1414,21 @@ static int rtw_ht_operation_update(struct rtw_adapter *padapter)
 	 * station is associated. Probably it's a theoretical case, since
 	 * it looks like all known HT STAs support greenfield.
 	 */
-	new_op_mode = 0;
 	if (pmlmepriv->num_sta_no_ht ||
-	    (pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT))
-		new_op_mode = OP_MODE_MIXED;
-	else if ((phtpriv_ap->ht_cap.cap_info & IEEE80211_HT_CAP_SUP_WIDTH_20_40)
-		 && pmlmepriv->num_sta_ht_20mhz)
-		new_op_mode = OP_MODE_20MHZ_HT_STA_ASSOCED;
+	    (pmlmepriv->ht_op_mode & IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT))
+		new_op_mode = IEEE80211_HT_OP_MODE_PROTECTION_NONHT_MIXED;
+	else if ((le16_to_cpu(phtpriv_ap->ht_cap.cap_info) &
+		  IEEE80211_HT_CAP_SUP_WIDTH_20_40) &&
+		 pmlmepriv->num_sta_ht_20mhz)
+		new_op_mode = IEEE80211_HT_OP_MODE_PROTECTION_20MHZ;
 	else if (pmlmepriv->olbc_ht)
-		new_op_mode = OP_MODE_MAY_BE_LEGACY_STAS;
+		new_op_mode = IEEE80211_HT_OP_MODE_PROTECTION_NONMEMBER;
 	else
-		new_op_mode = OP_MODE_PURE;
+		new_op_mode = IEEE80211_HT_OP_MODE_PROTECTION_NONE;
 
-	cur_op_mode = pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_OP_MODE_MASK;
+	cur_op_mode = pmlmepriv->ht_op_mode & IEEE80211_HT_OP_MODE_PROTECTION;
 	if (cur_op_mode != new_op_mode) {
-		pmlmepriv->ht_op_mode &= ~HT_INFO_OPERATION_MODE_OP_MODE_MASK;
+		pmlmepriv->ht_op_mode &= ~IEEE80211_HT_OP_MODE_PROTECTION;
 		pmlmepriv->ht_op_mode |= new_op_mode;
 		op_mode_changes++;
 	}
@@ -1562,7 +1515,7 @@ void bss_cap_update_on_sta_join23a(struct rtw_adapter *padapter, struct sta_info
 			if (pmlmepriv->num_sta_non_erp == 1)
 			{
 				beacon_updated = true;
-				update_beacon23a(padapter, _ERPINFO_IE_, NULL, true);
+				update_beacon23a(padapter, WLAN_EID_ERP_INFO, NULL, true);
 			}
 		}
 
@@ -1578,7 +1531,7 @@ void bss_cap_update_on_sta_join23a(struct rtw_adapter *padapter, struct sta_info
 			if (pmlmepriv->num_sta_non_erp == 0)
 			{
 				beacon_updated = true;
-				update_beacon23a(padapter, _ERPINFO_IE_, NULL, true);
+				update_beacon23a(padapter, WLAN_EID_ERP_INFO, NULL, true);
 			}
 		}
 
@@ -1669,8 +1622,8 @@ void bss_cap_update_on_sta_join23a(struct rtw_adapter *padapter, struct sta_info
 
 	if (rtw_ht_operation_update(padapter) > 0)
 	{
-		update_beacon23a(padapter, _HT_CAPABILITY_IE_, NULL, false);
-		update_beacon23a(padapter, _HT_ADD_INFO_IE_, NULL, true);
+		update_beacon23a(padapter, WLAN_EID_HT_CAPABILITY, NULL, false);
+		update_beacon23a(padapter, WLAN_EID_HT_OPERATION, NULL, true);
 	}
 
 	/* update associcated stations cap. */
@@ -1705,7 +1658,8 @@ u8 bss_cap_update_on_sta_leave23a(struct rtw_adapter *padapter, struct sta_info
 		if (pmlmepriv->num_sta_non_erp == 0)
 		{
 			beacon_updated = true;
-			update_beacon23a(padapter, _ERPINFO_IE_, NULL, true);
+			update_beacon23a(padapter, WLAN_EID_ERP_INFO,
+					 NULL, true);
 		}
 	}
 
@@ -1737,8 +1691,8 @@ u8 bss_cap_update_on_sta_leave23a(struct rtw_adapter *padapter, struct sta_info
 
 	if (rtw_ht_operation_update(padapter) > 0)
 	{
-		update_beacon23a(padapter, _HT_CAPABILITY_IE_, NULL, false);
-		update_beacon23a(padapter, _HT_ADD_INFO_IE_, NULL, true);
+		update_beacon23a(padapter, WLAN_EID_HT_CAPABILITY, NULL, false);
+		update_beacon23a(padapter, WLAN_EID_HT_OPERATION, NULL, true);
 	}
 
 	/* update associcated stations cap. */
@@ -1806,8 +1760,8 @@ int rtw_ap_inform_ch_switch23a (struct rtw_adapter *padapter, u8 new_ch, u8 ch_o
 	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
 		return ret;
 
-	DBG_8723A(FUNC_NDEV_FMT" with ch:%u, offset:%u\n",
-		FUNC_NDEV_ARG(padapter->pnetdev), new_ch, ch_offset);
+	DBG_8723A("%s(%s): with ch:%u, offset:%u\n", __func__,
+		  padapter->pnetdev->name, new_ch, ch_offset);
 
 	spin_lock_bh(&pstapriv->asoc_list_lock);
 	phead = &pstapriv->asoc_list;
@@ -1835,10 +1789,10 @@ int rtw_sta_flush23a(struct rtw_adapter *padapter)
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 	u8 chk_alive_num = 0;
-	char chk_alive_list[NUM_STA];
+	struct sta_info *chk_alive_list[NUM_STA];
 	int i;
 
-	DBG_8723A(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev));
+	DBG_8723A("%s(%s)\n", __func__, padapter->pnetdev->name);
 
 	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
 		return ret;
@@ -1847,8 +1801,6 @@ int rtw_sta_flush23a(struct rtw_adapter *padapter)
 	phead = &pstapriv->asoc_list;
 
 	list_for_each_safe(plist, ptmp, phead) {
-		int stainfo_offset;
-
 		psta = container_of(plist, struct sta_info, asoc_list);
 
 		/* Remove sta from asoc_list */
@@ -1856,18 +1808,14 @@ int rtw_sta_flush23a(struct rtw_adapter *padapter)
 		pstapriv->asoc_list_cnt--;
 
 		/* Keep sta for ap_free_sta23a() beyond this asoc_list loop */
-		stainfo_offset = rtw_stainfo_offset23a(pstapriv, psta);
-		if (stainfo_offset_valid(stainfo_offset)) {
-			chk_alive_list[chk_alive_num++] = stainfo_offset;
-		}
+		chk_alive_list[chk_alive_num++] = psta;
 	}
 	spin_unlock_bh(&pstapriv->asoc_list_lock);
 
 	/* For each sta in chk_alive_list, call ap_free_sta23a */
-	for (i = 0; i < chk_alive_num; i++) {
-		psta = rtw_get_stainfo23a_by_offset23a(pstapriv, chk_alive_list[i]);
-		ap_free_sta23a(padapter, psta, true, WLAN_REASON_DEAUTH_LEAVING);
-	}
+	for (i = 0; i < chk_alive_num; i++)
+		ap_free_sta23a(padapter, chk_alive_list[i], true,
+			       WLAN_REASON_DEAUTH_LEAVING);
 
 	issue_deauth23a(padapter, bc_addr, WLAN_REASON_DEAUTH_LEAVING);
 
@@ -1888,7 +1836,7 @@ void sta_info_update23a(struct rtw_adapter *padapter, struct sta_info *psta)
 	else
 		psta->qos_option = 0;
 
-	if (pmlmepriv->qospriv.qos_option == 0)
+	if (pmlmepriv->qos_option == 0)
 		psta->qos_option = 0;
 
 	/* update 802.11n ht cap. */
@@ -1928,25 +1876,29 @@ void rtw_ap_restore_network(struct rtw_adapter *padapter)
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct list_head *phead, *plist, *ptmp;
 	u8 chk_alive_num = 0;
-	char chk_alive_list[NUM_STA];
+	struct sta_info *chk_alive_list[NUM_STA];
 	int i;
 
-	rtw_setopmode_cmd23a(padapter, Ndis802_11APMode);
+	rtw_setopmode_cmd23a(padapter, NL80211_IFTYPE_AP);
 
 	set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
 
 	start_bss_network(padapter, (u8*)&mlmepriv->cur_network.network);
 
-	if ((padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_) ||
-		(padapter->securitypriv.dot11PrivacyAlgrthm == _AES_))
-	{
+	if (padapter->securitypriv.dot11PrivacyAlgrthm ==
+	    WLAN_CIPHER_SUITE_TKIP ||
+	    padapter->securitypriv.dot11PrivacyAlgrthm ==
+	    WLAN_CIPHER_SUITE_CCMP) {
 		/* restore group key, WEP keys is restored in ips_leave23a() */
-		rtw_set_key23a(padapter, psecuritypriv, psecuritypriv->dot118021XGrpKeyid, 0);
+		rtw_set_key23a(padapter, psecuritypriv,
+			       psecuritypriv->dot118021XGrpKeyid, 0);
 	}
 
 	/* per sta pairwise key and settings */
-	if ((padapter->securitypriv.dot11PrivacyAlgrthm != _TKIP_) &&
-		(padapter->securitypriv.dot11PrivacyAlgrthm != _AES_)) {
+	if (padapter->securitypriv.dot11PrivacyAlgrthm !=
+	    WLAN_CIPHER_SUITE_TKIP &&
+	    padapter->securitypriv.dot11PrivacyAlgrthm !=
+	    WLAN_CIPHER_SUITE_CCMP) {
 		return;
 	}
 
@@ -1955,26 +1907,17 @@ void rtw_ap_restore_network(struct rtw_adapter *padapter)
 	phead = &pstapriv->asoc_list;
 
 	list_for_each_safe(plist, ptmp, phead) {
-		int stainfo_offset;
-
 		psta = container_of(plist, struct sta_info, asoc_list);
 
-		stainfo_offset = rtw_stainfo_offset23a(pstapriv, psta);
-		if (stainfo_offset_valid(stainfo_offset)) {
-			chk_alive_list[chk_alive_num++] = stainfo_offset;
-		}
+		chk_alive_list[chk_alive_num++] = psta;
 	}
 
 	spin_unlock_bh(&pstapriv->asoc_list_lock);
 
 	for (i = 0; i < chk_alive_num; i++) {
-		psta = rtw_get_stainfo23a_by_offset23a(pstapriv, chk_alive_list[i]);
+		psta = chk_alive_list[i];
 
-		if (psta == NULL) {
-			DBG_8723A(FUNC_ADPT_FMT" sta_info is null\n", FUNC_ADPT_ARG(padapter));
-		}
-		else 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);
@@ -2083,5 +2026,3 @@ void stop_ap_mode23a(struct rtw_adapter *padapter)
 
 	rtw23a_free_mlme_priv_ie_data(pmlmepriv);
 }
-
-#endif /* CONFIG_8723AU_AP_MODE */
diff --git a/drivers/staging/rtl8723au/core/rtw_cmd.c b/drivers/staging/rtl8723au/core/rtw_cmd.c
index 5e3088a0180..1696cb8b17c 100644
--- a/drivers/staging/rtl8723au/core/rtw_cmd.c
+++ b/drivers/staging/rtl8723au/core/rtw_cmd.c
@@ -17,12 +17,9 @@
 #include <osdep_service.h>
 #include <drv_types.h>
 #include <recv_osdep.h>
-#include <cmd_osdep.h>
 #include <mlme_osdep.h>
-
-#ifdef CONFIG_8723AU_BT_COEXIST
-#include <rtl8723a_hal.h>
-#endif /*  CONFIG_8723AU_BT_COEXIST */
+#include <rtl8723a_cmd.h>
+#include <rtw_sreset.h>
 
 static struct cmd_hdl wlancmds[] = {
 	GEN_DRV_CMD_HANDLER(0, NULL) /*0*/
@@ -175,137 +172,33 @@ int rtw_init_cmd_priv23a(struct cmd_priv *pcmdpriv)
 {
 	int res = _SUCCESS;
 
-	sema_init(&pcmdpriv->cmd_queue_sema, 0);
-	sema_init(&pcmdpriv->terminate_cmdthread_sema, 0);
-
-	_rtw_init_queue23a(&pcmdpriv->cmd_queue);
-
-	pcmdpriv->cmd_seq = 1;
-
-	pcmdpriv->cmd_allocated_buf = kzalloc(MAX_CMDSZ + CMDBUFF_ALIGN_SZ,
-					      GFP_KERNEL);
-
-	if (pcmdpriv->cmd_allocated_buf == NULL) {
-		res = _FAIL;
-		goto exit;
-	}
-
-	pcmdpriv->cmd_buf = pcmdpriv->cmd_allocated_buf + CMDBUFF_ALIGN_SZ -
-			    ((unsigned long)(pcmdpriv->cmd_allocated_buf) &
-			    (CMDBUFF_ALIGN_SZ - 1));
-
-	pcmdpriv->rsp_allocated_buf = kzalloc(MAX_RSPSZ + 4, GFP_KERNEL);
-
-	if (!pcmdpriv->rsp_allocated_buf) {
-		res = _FAIL;
-		goto exit;
-	}
-
-	pcmdpriv->rsp_buf = pcmdpriv->rsp_allocated_buf + 4 -
-			    ((unsigned long)(pcmdpriv->rsp_allocated_buf) & 3);
-
 	pcmdpriv->cmd_issued_cnt = 0;
 	pcmdpriv->cmd_done_cnt = 0;
 	pcmdpriv->rsp_cnt = 0;
 
-exit:
+	pcmdpriv->wq = alloc_workqueue("rtl8723au_cmd", 0, 1);
+	if (!pcmdpriv->wq)
+		res = _FAIL;
 
 	return res;
 }
 
 /* forward definition */
 
-static void c2h_wk_callback(struct work_struct *work);
-int _rtw_init_evt_priv23a(struct evt_priv *pevtpriv)
-{
-	int res = _SUCCESS;
-
-	/* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
-	atomic_set(&pevtpriv->event_seq, 0);
-	pevtpriv->evt_done_cnt = 0;
-
-	INIT_WORK(&pevtpriv->c2h_wk, c2h_wk_callback);
-	pevtpriv->c2h_wk_alive = false;
-	pevtpriv->c2h_queue = rtw_cbuf_alloc23a(C2H_QUEUE_MAX_LEN + 1);
-
-	return res;
-}
-
-void _rtw_free_evt_priv23a (struct evt_priv *pevtpriv)
-{
-	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
-		 ("+_rtw_free_evt_priv23a\n"));
-	cancel_work_sync(&pevtpriv->c2h_wk);
-	while(pevtpriv->c2h_wk_alive)
-		msleep(10);
-
-	while (!rtw_cbuf_empty23a(pevtpriv->c2h_queue)) {
-		void *c2h;
-		if ((c2h = rtw_cbuf_pop23a(pevtpriv->c2h_queue)) != NULL &&
-		    c2h != (void *)pevtpriv) {
-			kfree(c2h);
-		}
-	}
-
-	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
-		 ("-_rtw_free_evt_priv23a\n"));
-}
-
-void _rtw_free_cmd_priv23a(struct cmd_priv *pcmdpriv)
-{
-	if (pcmdpriv) {
-		kfree(pcmdpriv->cmd_allocated_buf);
-		kfree(pcmdpriv->rsp_allocated_buf);
-	}
-}
-
-/*
-Calling Context:
-rtw_enqueue_cmd23a can only be called between kernel thread,
-since only spin_lock is used.
-
-ISR/Call-Back functions can't call this sub-function.
-*/
-
-int _rtw_enqueue_cmd23a(struct rtw_queue *queue, struct cmd_obj *obj)
-{
-	unsigned long irqL;
-
-	if (obj == NULL)
-		goto exit;
-
-	spin_lock_irqsave(&queue->lock, irqL);
-
-	list_add_tail(&obj->list, &queue->queue);
-
-	spin_unlock_irqrestore(&queue->lock, irqL);
-
-exit:
-
-	return _SUCCESS;
-}
+static void rtw_irq_work(struct work_struct *work);
 
 u32 rtw_init_evt_priv23a(struct evt_priv *pevtpriv)
 {
-	int res;
+	pevtpriv->wq = alloc_workqueue("rtl8723au_evt", 0, 1);
 
-	res = _rtw_init_evt_priv23a(pevtpriv);
+	INIT_WORK(&pevtpriv->irq_wk, rtw_irq_work);
 
-	return res;
+	return _SUCCESS;
 }
 
 void rtw_free_evt_priv23a(struct evt_priv *pevtpriv)
 {
-	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
-		 ("rtw_free_evt_priv23a\n"));
-	_rtw_free_evt_priv23a(pevtpriv);
-}
-
-void rtw_free_cmd_priv23a(struct cmd_priv *pcmdpriv)
-{
-	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
-		 ("rtw_free_cmd_priv23a\n"));
-	_rtw_free_cmd_priv23a(pcmdpriv);
+	cancel_work_sync(&pevtpriv->irq_wk);
 }
 
 static int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
@@ -329,21 +222,21 @@ static int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
 	if (cmd_obj->cmdcode == GEN_CMD_CODE(_SetChannelPlan))
 		bAllow = true;
 
-	if ((pcmdpriv->padapter->hw_init_completed == false &&
-	     bAllow == false) || pcmdpriv->cmdthd_running == false)
+	if (pcmdpriv->padapter->hw_init_completed == false && bAllow == false)
 		return _FAIL;
 	return _SUCCESS;
 }
 
-u32 rtw_enqueue_cmd23a(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
+static void rtw_cmd_work(struct work_struct *work);
+
+int rtw_enqueue_cmd23a(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
 {
 	int res = _FAIL;
-	struct rtw_adapter *padapter = pcmdpriv->padapter;
 
 	if (!cmd_obj)
 		goto exit;
 
-	cmd_obj->padapter = padapter;
+	cmd_obj->padapter = pcmdpriv->padapter;
 
 	res = rtw_cmd_filter(pcmdpriv, cmd_obj);
 	if (res == _FAIL) {
@@ -351,32 +244,18 @@ u32 rtw_enqueue_cmd23a(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
 		goto exit;
 	}
 
-	res = _rtw_enqueue_cmd23a(&pcmdpriv->cmd_queue, cmd_obj);
+	INIT_WORK(&cmd_obj->work, rtw_cmd_work);
 
-	if (res == _SUCCESS)
-		up(&pcmdpriv->cmd_queue_sema);
+	res = queue_work(pcmdpriv->wq, &cmd_obj->work);
 
+	if (!res) {
+		printk(KERN_ERR "%s: Call to queue_work() failed\n", __func__);
+		res = _FAIL;
+	} else
+		res = _SUCCESS;
 exit:
-	return res;
-}
 
-static struct cmd_obj *rtw_dequeue_cmd(struct cmd_priv *pcmdpriv)
-{
-	struct cmd_obj *obj;
-	struct rtw_queue *queue = &pcmdpriv->cmd_queue;
-	unsigned long irqL;
-
-	spin_lock_irqsave(&queue->lock, irqL);
-	if (list_empty(&queue->queue))
-		obj = NULL;
-	else {
-		obj = container_of((&queue->queue)->next, struct cmd_obj, list);
-		list_del_init(&obj->list);
-	}
-
-	spin_unlock_irqrestore(&queue->lock, irqL);
-
-	return obj;
+	return res;
 }
 
 void rtw_cmd_clr_isr23a(struct	cmd_priv *pcmdpriv)
@@ -403,135 +282,72 @@ void rtw_free_cmd_obj23a(struct cmd_obj *pcmd)
 	kfree(pcmd);
 }
 
-int rtw_cmd_thread23a(void *context)
+static void rtw_cmd_work(struct work_struct *work)
 {
-	u8 ret;
-	struct cmd_obj *pcmd;
-	u8 *pcmdbuf, *prspbuf;
-	u8 (*cmd_hdl)(struct rtw_adapter *padapter, u8* pbuf);
+	int (*cmd_hdl)(struct rtw_adapter *padapter, const u8 *pbuf);
 	void (*pcmd_callback)(struct rtw_adapter *dev, struct cmd_obj *pcmd);
-	struct rtw_adapter *padapter = (struct rtw_adapter *)context;
-	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct cmd_priv *pcmdpriv;
+	struct cmd_obj *pcmd = container_of(work, struct cmd_obj, work);
 
-	allow_signal(SIGTERM);
+	pcmdpriv = &pcmd->padapter->cmdpriv;
 
-	pcmdbuf = pcmdpriv->cmd_buf;
-	prspbuf = pcmdpriv->rsp_buf;
+	if (rtw_cmd_filter(pcmdpriv, pcmd) == _FAIL) {
+		pcmd->res = H2C_DROPPED;
+		goto post_process;
+	}
 
-	pcmdpriv->cmdthd_running = true;
-	up(&pcmdpriv->terminate_cmdthread_sema);
+	pcmdpriv->cmd_issued_cnt++;
 
-	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
-		 ("start r871x rtw_cmd_thread23a !!!!\n"));
-
-	while(1) {
-		if (down_interruptible(&pcmdpriv->cmd_queue_sema))
-			break;
-_next:
-		if ((padapter->bDriverStopped == true) ||
-		    (padapter->bSurpriseRemoved == true)) {
-			DBG_8723A("%s: DriverStopped(%d) SurpriseRemoved(%d) "
-				  "break at line %d\n",	__func__,
-				  padapter->bDriverStopped,
-				  padapter->bSurpriseRemoved, __LINE__);
-			break;
-		}
+	pcmd->cmdsz = ALIGN(pcmd->cmdsz, 4);
 
-		if (!(pcmd = rtw_dequeue_cmd(pcmdpriv)))
-			continue;
+	if (pcmd->cmdcode < (sizeof(wlancmds)/sizeof(struct cmd_hdl))) {
+		cmd_hdl = wlancmds[pcmd->cmdcode].h2cfuns;
 
-		if (rtw_cmd_filter(pcmdpriv, pcmd) == _FAIL) {
+		if (cmd_hdl)
+			pcmd->res = cmd_hdl(pcmd->padapter, pcmd->parmbuf);
+		else
 			pcmd->res = H2C_DROPPED;
-			goto post_process;
-		}
-
-		pcmdpriv->cmd_issued_cnt++;
-
-		pcmd->cmdsz = ALIGN(pcmd->cmdsz, 4);
-
-		memcpy(pcmdbuf, pcmd->parmbuf, pcmd->cmdsz);
-
-		if (pcmd->cmdcode < (sizeof(wlancmds)/sizeof(struct cmd_hdl))) {
-			cmd_hdl = wlancmds[pcmd->cmdcode].h2cfuns;
-
-			if (cmd_hdl) {
-				ret = cmd_hdl(pcmd->padapter, pcmdbuf);
-				pcmd->res = ret;
-			}
-
-			pcmdpriv->cmd_seq++;
-		} else
-			pcmd->res = H2C_PARAMETERS_ERROR;
-
-		cmd_hdl = NULL;
+	} else
+		pcmd->res = H2C_PARAMETERS_ERROR;
 
 post_process:
-		/* call callback function for post-processed */
-		if (pcmd->cmdcode < (sizeof(rtw_cmd_callback) /
-				     sizeof(struct _cmd_callback))) {
-			pcmd_callback =
-				rtw_cmd_callback[pcmd->cmdcode].callback;
-			if (!pcmd_callback) {
-				RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
-					 ("mlme_cmd_hdl(): pcmd_callback = "
-					  "0x%p, cmdcode = 0x%x\n",
-					  pcmd_callback, pcmd->cmdcode));
-				rtw_free_cmd_obj23a(pcmd);
-			} else {
-				/* todo: !!! fill rsp_buf to pcmd->rsp
-				   if (pcmd->rsp!= NULL) */
-				/* need conider that free cmd_obj in
-				   rtw_cmd_callback */
-				pcmd_callback(pcmd->padapter, pcmd);
-			}
-		} else {
-			RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
-				 ("%s: cmdcode = 0x%x callback not defined!\n",
-				  __func__, pcmd->cmdcode));
+	/* call callback function for post-processed */
+	if (pcmd->cmdcode < (sizeof(rtw_cmd_callback) /
+			     sizeof(struct _cmd_callback))) {
+		pcmd_callback =	rtw_cmd_callback[pcmd->cmdcode].callback;
+		if (!pcmd_callback) {
+			RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
+				 ("mlme_cmd_hdl(): pcmd_callback = 0x%p, "
+				  "cmdcode = 0x%x\n",
+				  pcmd_callback, pcmd->cmdcode));
 			rtw_free_cmd_obj23a(pcmd);
+		} else {
+			/* need conider that free cmd_obj in
+			   rtw_cmd_callback */
+			pcmd_callback(pcmd->padapter, pcmd);
 		}
-
-		if (signal_pending (current))
-			flush_signals(current);
-
-		goto _next;
-
-	}
-	pcmdpriv->cmdthd_running = false;
-
-	/*  free all cmd_obj resources */
-	do {
-		pcmd = rtw_dequeue_cmd(pcmdpriv);
-		if (!pcmd)
-			break;
-
+	} else {
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
+			 ("%s: cmdcode = 0x%x callback not defined!\n",
+			  __func__, pcmd->cmdcode));
 		rtw_free_cmd_obj23a(pcmd);
-	} while(1);
-
-	up(&pcmdpriv->terminate_cmdthread_sema);
-
-	complete_and_exit(NULL, 0);
+	}
 }
 
-u8 rtw_sitesurvey_cmd23a(struct rtw_adapter *padapter,
-			 struct cfg80211_ssid *ssid, int ssid_num,
-			 struct rtw_ieee80211_channel *ch, int ch_num)
+
+int rtw_sitesurvey_cmd23a(struct rtw_adapter *padapter,
+			  struct cfg80211_ssid *ssid, int ssid_num,
+			  struct rtw_ieee80211_channel *ch, int ch_num)
 {
-	u8 res = _FAIL;
+	int res = _FAIL;
 	struct cmd_obj *ph2c;
 	struct sitesurvey_parm *psurveyPara;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
 		rtw_lps_ctrl_wk_cmd23a(padapter, LPS_CTRL_SCAN, 1);
 
-#ifdef CONFIG_8723AU_P2P
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
-		p2p_ps_wk_cmd23a(padapter, P2P_PS_SCAN, 1);
-	}
-#endif /* CONFIG_8723AU_P2P */
-
 	ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (!ph2c)
 		return _FAIL;
@@ -542,7 +358,7 @@ u8 rtw_sitesurvey_cmd23a(struct rtw_adapter *padapter,
 		return _FAIL;
 	}
 
-	rtw_free_network_queue23a(padapter, false);
+	rtw_free_network_queue23a(padapter);
 
 	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
 		 ("%s: flush network queue\n", __func__));
@@ -561,11 +377,6 @@ u8 rtw_sitesurvey_cmd23a(struct rtw_adapter *padapter,
 				memcpy(&psurveyPara->ssid[i], &ssid[i],
 				       sizeof(struct cfg80211_ssid));
 				psurveyPara->ssid_num++;
-				if (0)
-				DBG_8723A(FUNC_ADPT_FMT" ssid:(%s, %d)\n",
-					  FUNC_ADPT_ARG(padapter),
-					  psurveyPara->ssid[i].ssid,
-					  psurveyPara->ssid[i].ssid_len);
 			}
 		}
 	}
@@ -579,10 +390,6 @@ u8 rtw_sitesurvey_cmd23a(struct rtw_adapter *padapter,
 				memcpy(&psurveyPara->ch[i], &ch[i],
 				       sizeof(struct rtw_ieee80211_channel));
 				psurveyPara->ch_num++;
-				if (0)
-				DBG_8723A(FUNC_ADPT_FMT" ch:%u\n",
-					  FUNC_ADPT_ARG(padapter),
-					  psurveyPara->ch[i].hw_value);
 			}
 		}
 	}
@@ -611,7 +418,7 @@ void rtw_getbbrfreg_cmdrsp_callback23a(struct rtw_adapter *padapter,
 	kfree(pcmd);
 }
 
-u8 rtw_createbss_cmd23a(struct rtw_adapter  *padapter)
+int rtw_createbss_cmd23a(struct rtw_adapter  *padapter)
 {
 	struct cmd_obj *pcmd;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
@@ -639,10 +446,9 @@ u8 rtw_createbss_cmd23a(struct rtw_adapter  *padapter)
 		goto exit;
 	}
 
-	INIT_LIST_HEAD(&pcmd->list);
 	pcmd->cmdcode = _CreateBss_CMD_;
 	pcmd->parmbuf = (unsigned char *)pdev_network;
-	pcmd->cmdsz = get_wlan_bssid_ex_sz((struct wlan_bssid_ex*)pdev_network);
+	pcmd->cmdsz = get_wlan_bssid_ex_sz(pdev_network);
 	pcmd->rsp = NULL;
 	pcmd->rspsz = 0;
 
@@ -655,24 +461,23 @@ exit:
 	return res;
 }
 
-u8 rtw_joinbss_cmd23a(struct rtw_adapter *padapter,
-		      struct wlan_network * pnetwork)
+int rtw_joinbss_cmd23a(struct rtw_adapter *padapter,
+		       struct wlan_network *pnetwork)
 {
-	u8 *auth, res = _SUCCESS;
-	uint t_len = 0;
+	u8 *auth;
+	int res = _SUCCESS;
 	struct wlan_bssid_ex *psecnetwork;
 	struct cmd_obj *pcmd;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct qos_priv *pqospriv = &pmlmepriv->qospriv;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct registry_priv *pregistrypriv = &padapter->registrypriv;
 	struct ht_priv *phtpriv = &pmlmepriv->htpriv;
-	enum ndis_802_11_net_infra ndis_network_mode;
+	enum nl80211_iftype ifmode;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
-	ndis_network_mode = pnetwork->network.InfrastructureMode;
+	ifmode = pnetwork->network.ifmode;
 
 	rtw_led_control(padapter, LED_CTL_START_TO_LINK);
 
@@ -693,26 +498,23 @@ u8 rtw_joinbss_cmd23a(struct rtw_adapter *padapter,
 			  "fail!!!\n"));
 		goto exit;
 	}
-	/* for IEs is fix buf size */
-	t_len = sizeof(struct wlan_bssid_ex);
 
 	/* for hidden ap to set fw_state here */
 	if (!check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_ADHOC_STATE)) {
-		switch (ndis_network_mode) {
-		case Ndis802_11IBSS:
+		switch (ifmode) {
+		case NL80211_IFTYPE_ADHOC:
 			set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
 			break;
-		case Ndis802_11Infrastructure:
+		case NL80211_IFTYPE_P2P_CLIENT:
+		case NL80211_IFTYPE_STATION:
 			set_fwstate(pmlmepriv, WIFI_STATION_STATE);
 			break;
-		case Ndis802_11APMode:
-		case Ndis802_11AutoUnknown:
-		case Ndis802_11InfrastructureMax:
+		default:
 			break;
 		}
 	}
 
-	psecnetwork = (struct wlan_bssid_ex *)&psecuritypriv->sec_bss;
+	psecnetwork = &psecuritypriv->sec_bss;
 	if (!psecnetwork) {
 		if (pcmd)
 			kfree(pcmd);
@@ -725,7 +527,7 @@ u8 rtw_joinbss_cmd23a(struct rtw_adapter *padapter,
 		goto exit;
 	}
 
-	memset(psecnetwork, 0, t_len);
+	memset(psecnetwork, 0, sizeof(struct wlan_bssid_ex));
 
 	memcpy(psecnetwork, &pnetwork->network,
 	       get_wlan_bssid_ex_sz(&pnetwork->network));
@@ -758,7 +560,7 @@ u8 rtw_joinbss_cmd23a(struct rtw_adapter *padapter,
 				       &psecnetwork->IEs[0],
 				       pnetwork->network.IELength);
 
-	pqospriv->qos_option = 0;
+	pmlmepriv->qos_option = 0;
 
 	if (pregistrypriv->wmm_enable) {
 		u32 tmp_len;
@@ -772,22 +574,23 @@ u8 rtw_joinbss_cmd23a(struct rtw_adapter *padapter,
 		if (psecnetwork->IELength != tmp_len) {
 			psecnetwork->IELength = tmp_len;
 			/* There is WMM IE in this corresp. beacon */
-			pqospriv->qos_option = 1;
+			pmlmepriv->qos_option = 1;
 		} else {
 			/* There is no WMM IE in this corresp. beacon */
-			pqospriv->qos_option = 0;
+			pmlmepriv->qos_option = 0;
 		}
 	}
 
 	phtpriv->ht_option = false;
 	if (pregistrypriv->ht_enable) {
+		u32 algo = padapter->securitypriv.dot11PrivacyAlgrthm;
 		/*	Added by Albert 2010/06/23 */
 		/*	For the WEP mode, we will use the bg mode to do
 			the connection to avoid some IOT issue. */
 		/*	Especially for Realtek 8192u SoftAP. */
-		if ((padapter->securitypriv.dot11PrivacyAlgrthm != _WEP40_) &&
-		    (padapter->securitypriv.dot11PrivacyAlgrthm != _WEP104_) &&
-		    (padapter->securitypriv.dot11PrivacyAlgrthm != _TKIP_)) {
+		if (algo != WLAN_CIPHER_SUITE_WEP40 &&
+		    algo != WLAN_CIPHER_SUITE_WEP104 &&
+		    algo != WLAN_CIPHER_SUITE_TKIP) {
 			/* rtw_restructure_ht_ie23a */
 			rtw_restructure_ht_ie23a(padapter,
 						 &pnetwork->network.IEs[0],
@@ -813,7 +616,6 @@ u8 rtw_joinbss_cmd23a(struct rtw_adapter *padapter,
 	/* get cmdsz before endian conversion */
 	pcmd->cmdsz = get_wlan_bssid_ex_sz(psecnetwork);
 
-	INIT_LIST_HEAD(&pcmd->list);
 	pcmd->cmdcode = _JoinBss_CMD_;/* GEN_CMD_CODE(_JoinBss) */
 	pcmd->parmbuf = (unsigned char *)psecnetwork;
 	pcmd->rsp = NULL;
@@ -825,13 +627,13 @@ exit:
 	return res;
 }
 
-u8 rtw_disassoc_cmd23a(struct rtw_adapter*padapter, u32 deauth_timeout_ms,
-		       bool enqueue)
+int rtw_disassoc_cmd23a(struct rtw_adapter*padapter, u32 deauth_timeout_ms,
+			bool enqueue)
 {
 	struct cmd_obj *cmdobj = NULL;
 	struct disconnect_parm *param = NULL;
 	struct cmd_priv *cmdpriv = &padapter->cmdpriv;
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_,
 		 ("+rtw_disassoc_cmd23a\n"));
@@ -866,13 +668,13 @@ exit:
 	return res;
 }
 
-u8 rtw_setopmode_cmd23a(struct rtw_adapter *padapter,
-			enum ndis_802_11_net_infra networktype)
+int rtw_setopmode_cmd23a(struct rtw_adapter *padapter,
+			 enum nl80211_iftype ifmode)
 {
 	struct	cmd_obj *ph2c;
 	struct	setopmode_parm *psetop;
 	struct	cmd_priv   *pcmdpriv = &padapter->cmdpriv;
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	ph2c = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
 	if (!ph2c) {
@@ -888,14 +690,14 @@ u8 rtw_setopmode_cmd23a(struct rtw_adapter *padapter,
 	}
 
 	init_h2fwcmd_w_parm_no_rsp(ph2c, psetop, _SetOpMode_CMD_);
-	psetop->mode = (u8)networktype;
+	psetop->mode = ifmode;
 
 	res = rtw_enqueue_cmd23a(pcmdpriv, ph2c);
 exit:
 	return res;
 }
 
-u8 rtw_setstakey_cmd23a(struct rtw_adapter *padapter, u8 *psta, u8 unicast_key)
+int rtw_setstakey_cmd23a(struct rtw_adapter *padapter, u8 *psta, u8 unicast_key)
 {
 	struct cmd_obj *ph2c;
 	struct set_stakey_parm *psetstakey_para;
@@ -904,7 +706,7 @@ u8 rtw_setstakey_cmd23a(struct rtw_adapter *padapter, u8 *psta, u8 unicast_key)
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct sta_info *sta = (struct sta_info*)psta;
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	ph2c = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
 	if (!ph2c) {
@@ -950,7 +752,7 @@ u8 rtw_setstakey_cmd23a(struct rtw_adapter *padapter, u8 *psta, u8 unicast_key)
         }
 
 	/* jeff: set this becasue at least sw key is ready */
-	padapter->securitypriv.busetkipkey = true;
+	padapter->securitypriv.busetkipkey = 1;
 
 	res = rtw_enqueue_cmd23a(pcmdpriv, ph2c);
 
@@ -959,15 +761,15 @@ exit:
 	return res;
 }
 
-u8 rtw_clearstakey_cmd23a(struct rtw_adapter *padapter, u8 *psta, u8 entry,
-			  u8 enqueue)
+int rtw_clearstakey_cmd23a(struct rtw_adapter *padapter, u8 *psta, u8 entry,
+			   u8 enqueue)
 {
 	struct cmd_obj *ph2c;
 	struct set_stakey_parm *psetstakey_para;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 	struct set_stakey_rsp *psetstakey_rsp = NULL;
 	struct sta_info *sta = (struct sta_info *)psta;
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	if (!enqueue) {
 		clear_cam_entry23a(padapter, entry);
@@ -1002,7 +804,7 @@ u8 rtw_clearstakey_cmd23a(struct rtw_adapter *padapter, u8 *psta, u8 entry,
 
 		ether_addr_copy(psetstakey_para->addr, sta->hwaddr);
 
-		psetstakey_para->algorithm = _NO_PRIVACY_;
+		psetstakey_para->algorithm = 0;
 
 		psetstakey_para->id = entry;
 
@@ -1012,12 +814,17 @@ exit:
 	return res;
 }
 
-u8 rtw_addbareq_cmd23a(struct rtw_adapter*padapter, u8 tid, u8 *addr)
+int rtw_addbareq_cmd23a(struct rtw_adapter*padapter, u8 tid, u8 *addr)
 {
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 	struct cmd_obj *ph2c;
 	struct addBaReq_parm *paddbareq_parm;
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
+
+	if (tid >= MAXTID) {
+		res = _FAIL;
+		goto exit;
+	}
 
 	ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (!ph2c) {
@@ -1043,12 +850,12 @@ exit:
 	return res;
 }
 
-u8 rtw_dynamic_chk_wk_cmd23a(struct rtw_adapter*padapter)
+int rtw_dynamic_chk_wk_cmd23a(struct rtw_adapter*padapter)
 {
 	struct cmd_obj *ph2c;
 	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (!ph2c) {
@@ -1080,17 +887,16 @@ exit:
  * This is only ever called from on_action_spct23a_ch_switch () which isn't
  * called from anywhere itself
  */
-u8 rtw_set_ch_cmd23a(struct rtw_adapter*padapter, u8 ch, u8 bw, u8 ch_offset,
-		     u8 enqueue)
+int rtw_set_ch_cmd23a(struct rtw_adapter*padapter, u8 ch, u8 bw, u8 ch_offset,
+		      u8 enqueue)
 {
 	struct cmd_obj *pcmdobj;
 	struct set_ch_parm *set_ch_parm;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	int res = _SUCCESS;
 
-	u8 res = _SUCCESS;
-
-	DBG_8723A(FUNC_NDEV_FMT" ch:%u, bw:%u, ch_offset:%u\n",
-		  FUNC_NDEV_ARG(padapter->pnetdev), ch, bw, ch_offset);
+	DBG_8723A("%s(%s): ch:%u, bw:%u, ch_offset:%u\n", __func__,
+		  padapter->pnetdev->name, ch, bw, ch_offset);
 
 	/* check input parameter */
 
@@ -1128,8 +934,7 @@ u8 rtw_set_ch_cmd23a(struct rtw_adapter*padapter, u8 ch, u8 bw, u8 ch_offset,
 	/* do something based on res... */
 exit:
 
-	DBG_8723A(FUNC_NDEV_FMT" res:%u\n", FUNC_NDEV_ARG(padapter->pnetdev),
-		  res);
+	DBG_8723A("%s(%s): res:%u\n", __func__, padapter->pnetdev->name, res);
 
 	return res;
 }
@@ -1141,25 +946,19 @@ static void traffic_status_watchdog(struct rtw_adapter *padapter)
 	u8 bHigherBusyTraffic = false, bHigherBusyRxTraffic = false;
 	u8 bHigherBusyTxTraffic = false;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-#ifndef CONFIG_8723AU_BT_COEXIST
 	int BusyThreshold = 100;
-#endif
 	/*  */
 	/*  Determine if our traffic is busy now */
 	/*  */
 	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
-#ifdef CONFIG_8723AU_BT_COEXIST
-		if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > 50 ||
-		    pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > 50)
-#else /*  !CONFIG_8723AU_BT_COEXIST */
+		if (rtl8723a_BT_coexist(padapter))
+			BusyThreshold = 50;
+		else if (pmlmepriv->LinkDetectInfo.bBusyTraffic)
+			BusyThreshold = 75;
 		/*  if we raise bBusyTraffic in last watchdog, using
 		    lower threshold. */
-		if (pmlmepriv->LinkDetectInfo.bBusyTraffic)
-			BusyThreshold = 75;
 		if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > BusyThreshold ||
-		    pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > BusyThreshold)
-#endif /*  !CONFIG_8723AU_BT_COEXIST */
-		{
+		    pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > BusyThreshold) {
 			bBusyTraffic = true;
 
 			if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod >
@@ -1181,23 +980,21 @@ static void traffic_status_watchdog(struct rtw_adapter *padapter)
 				bHigherBusyTxTraffic = true;
 		}
 
-#ifdef CONFIG_8723AU_BT_COEXIST
-		if (BT_1Ant(padapter) == false)
-#endif
-		{
+		if (!rtl8723a_BT_coexist(padapter) ||
+		    !rtl8723a_BT_using_antenna_1(padapter)) {
 		/*  check traffic for  powersaving. */
-		if (((pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod +
-		      pmlmepriv->LinkDetectInfo.NumTxOkInPeriod) > 8) ||
-		    (pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod > 2))
-			bEnterPS = false;
-		else
-			bEnterPS = true;
+			if (((pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod +
+			      pmlmepriv->LinkDetectInfo.NumTxOkInPeriod) > 8) ||
+			    pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod >2)
+				bEnterPS = false;
+			else
+				bEnterPS = true;
 
-		/*  LeisurePS only work in infra mode. */
-		if (bEnterPS)
-			LPS_Enter23a(padapter);
-		else
-			LPS_Leave23a(padapter);
+			/*  LeisurePS only work in infra mode. */
+			if (bEnterPS)
+				LPS_Enter23a(padapter);
+			else
+				LPS_Leave23a(padapter);
 		}
 	} else
 		LPS_Leave23a(padapter);
@@ -1213,7 +1010,7 @@ static void traffic_status_watchdog(struct rtw_adapter *padapter)
 	pmlmepriv->LinkDetectInfo.bHigherBusyTxTraffic = bHigherBusyTxTraffic;
 }
 
-void dynamic_chk_wk_hdl(struct rtw_adapter *padapter, u8 *pbuf, int sz)
+static void dynamic_chk_wk_hdl(struct rtw_adapter *padapter, u8 *pbuf, int sz)
 {
 	struct mlme_priv *pmlmepriv;
 
@@ -1221,43 +1018,38 @@ void dynamic_chk_wk_hdl(struct rtw_adapter *padapter, u8 *pbuf, int sz)
 	pmlmepriv = &padapter->mlmepriv;
 
 #ifdef CONFIG_8723AU_AP_MODE
-	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
 		expire_timeout_chk23a(padapter);
 #endif
 
-	rtw_hal_sreset_xmit_status_check23a(padapter);
+	rtl8723a_sreset_xmit_status_check(padapter);
 
 	linked_status_chk23a(padapter);
 	traffic_status_watchdog(padapter);
 
-	rtw_hal_dm_watchdog23a(padapter);
+	rtl8723a_HalDmWatchDog(padapter);
 
-#ifdef CONFIG_8723AU_BT_COEXIST
 	/*  */
 	/*  BT-Coexist */
 	/*  */
-	BT_CoexistMechanism(padapter);
-#endif
+	rtl8723a_BT_do_coexist(padapter);
 }
 
-void lps_ctrl_wk_hdl(struct rtw_adapter *padapter, u8 lps_ctrl_type)
+static void lps_ctrl_wk_hdl(struct rtw_adapter *padapter, u8 lps_ctrl_type)
 {
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	u8 mstatus;
 
-	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) ||
-	    (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true))
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_STATE))
 		return;
 
 	switch (lps_ctrl_type)
 	{
 		case LPS_CTRL_SCAN:
-#ifdef CONFIG_8723AU_BT_COEXIST
-			BT_WifiScanNotify(padapter, true);
-			if (BT_1Ant(padapter) == false)
-#endif
-			{
+			rtl8723a_BT_wifiscan_notify(padapter, true);
+			if (!rtl8723a_BT_using_antenna_1(padapter)) {
 				if (check_fwstate(pmlmepriv, _FW_LINKED))
 					LPS_Leave23a(padapter);
 			}
@@ -1269,42 +1061,26 @@ void lps_ctrl_wk_hdl(struct rtw_adapter *padapter, u8 lps_ctrl_type)
 			mstatus = 1;/* connect */
 			/*  Reset LPS Setting */
 			padapter->pwrctrlpriv.LpsIdleCount = 0;
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_JOINBSSRPT,
-					     (u8 *)&mstatus);
-#ifdef CONFIG_8723AU_BT_COEXIST
-			BT_WifiMediaStatusNotify(padapter, mstatus);
-#endif
+			rtl8723a_set_FwJoinBssReport_cmd(padapter, 1);
+			rtl8723a_BT_mediastatus_notify(padapter, mstatus);
 			break;
 		case LPS_CTRL_DISCONNECT:
 			mstatus = 0;/* disconnect */
-#ifdef CONFIG_8723AU_BT_COEXIST
-			BT_WifiMediaStatusNotify(padapter, mstatus);
-			if (BT_1Ant(padapter) == false)
-#endif
-			{
+			rtl8723a_BT_mediastatus_notify(padapter, mstatus);
+			if (!rtl8723a_BT_using_antenna_1(padapter))
 				LPS_Leave23a(padapter);
-			}
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_JOINBSSRPT,
-					     (u8 *)&mstatus);
+			rtl8723a_set_FwJoinBssReport_cmd(padapter, 0);
 			break;
 		case LPS_CTRL_SPECIAL_PACKET:
 			pwrpriv->DelayLPSLastTimeStamp = jiffies;
-#ifdef CONFIG_8723AU_BT_COEXIST
-			BT_SpecialPacketNotify(padapter);
-			if (BT_1Ant(padapter) == false)
-#endif
-			{
+			rtl8723a_BT_specialpacket_notify(padapter);
+			if (!rtl8723a_BT_using_antenna_1(padapter))
 				LPS_Leave23a(padapter);
-			}
 			break;
 		case LPS_CTRL_LEAVE:
-#ifdef CONFIG_8723AU_BT_COEXIST
-			BT_LpsLeave(padapter);
-			if (BT_1Ant(padapter) == false)
-#endif
-			{
+			rtl8723a_BT_lps_leave(padapter);
+			if (!rtl8723a_BT_using_antenna_1(padapter))
 				LPS_Leave23a(padapter);
-			}
 			break;
 
 		default:
@@ -1312,13 +1088,13 @@ void lps_ctrl_wk_hdl(struct rtw_adapter *padapter, u8 lps_ctrl_type)
 	}
 }
 
-u8 rtw_lps_ctrl_wk_cmd23a(struct rtw_adapter *padapter,
-			  u8 lps_ctrl_type, u8 enqueue)
+int rtw_lps_ctrl_wk_cmd23a(struct rtw_adapter *padapter,
+			   u8 lps_ctrl_type, u8 enqueue)
 {
 	struct cmd_obj *ph2c;
 	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	if (enqueue) {
 		ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
@@ -1355,55 +1131,12 @@ static void power_saving_wk_hdl(struct rtw_adapter *padapter, u8 *pbuf, int sz)
 	 rtw_ps_processor23a(padapter);
 }
 
-#ifdef CONFIG_8723AU_P2P
-u8 p2p_protocol_wk_cmd23a(struct rtw_adapter*padapter, int intCmdType)
-{
-	struct cmd_obj *ph2c;
-	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
-	u8 res = _SUCCESS;
-
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
-	{
-		return res;
-	}
-
-	ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
-	if (!ph2c) {
-		res = _FAIL;
-		goto exit;
-	}
-
-	pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm),
-				     GFP_ATOMIC);
-	if (!pdrvextra_cmd_parm) {
-		kfree(ph2c);
-		res = _FAIL;
-		goto exit;
-	}
-
-	pdrvextra_cmd_parm->ec_id = P2P_PROTO_WK_CID;
-	pdrvextra_cmd_parm->type_size = intCmdType; /* As the command tppe. */
-	pdrvextra_cmd_parm->pbuf = NULL;	    /* Must be NULL here */
-
-	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm,
-				   GEN_CMD_CODE(_Set_Drv_Extra));
-
-	res = rtw_enqueue_cmd23a(pcmdpriv, ph2c);
-exit:
-
-	return res;
-}
-#endif /* CONFIG_8723AU_P2P */
-
-u8 rtw_ps_cmd23a(struct rtw_adapter*padapter)
+int rtw_ps_cmd23a(struct rtw_adapter*padapter)
 {
 	struct cmd_obj *ppscmd;
 	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
-
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	ppscmd = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (!ppscmd) {
@@ -1443,11 +1176,11 @@ static void rtw_chk_hi_queue_hdl(struct rtw_adapter *padapter)
 		return;
 
 	if (psta_bmc->sleepq_len == 0) {
-		u8 val = 0;
+		bool val;
 
-		rtw23a_hal_get_hwreg(padapter, HW_VAR_CHK_HI_QUEUE_EMPTY, &val);
+		val = rtl8723a_chk_hi_queue_empty(padapter);
 
-		while(val == false) {
+		while (val == false) {
 			msleep(100);
 
 			cnt++;
@@ -1455,26 +1188,25 @@ static void rtw_chk_hi_queue_hdl(struct rtw_adapter *padapter)
 			if (cnt>10)
 				break;
 
-			rtw23a_hal_get_hwreg(padapter,
-					     HW_VAR_CHK_HI_QUEUE_EMPTY, &val);
+			val = rtl8723a_chk_hi_queue_empty(padapter);
 		}
 
 		if (cnt <= 10) {
 			pstapriv->tim_bitmap &= ~BIT(0);
 			pstapriv->sta_dz_bitmap &= ~BIT(0);
 
-			update_beacon23a(padapter, _TIM_IE_, NULL, false);
+			update_beacon23a(padapter, WLAN_EID_TIM, NULL, false);
 		} else /* re check again */
 			rtw_chk_hi_queue_cmd23a(padapter);
 	}
 }
 
-u8 rtw_chk_hi_queue_cmd23a(struct rtw_adapter*padapter)
+int rtw_chk_hi_queue_cmd23a(struct rtw_adapter*padapter)
 {
 	struct cmd_obj *ph2c;
 	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (!ph2c) {
@@ -1504,12 +1236,12 @@ exit:
 }
 #endif
 
-u8 rtw_c2h_wk_cmd23a(struct rtw_adapter *padapter, u8 *c2h_evt)
+int rtw_c2h_wk_cmd23a(struct rtw_adapter *padapter, u8 *c2h_evt)
 {
 	struct cmd_obj *ph2c;
 	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (!ph2c) {
@@ -1539,10 +1271,9 @@ exit:
 	return res;
 }
 
-s32 c2h_evt_hdl(struct rtw_adapter *adapter, struct c2h_evt_hdr *c2h_evt,
-		c2h_id_filter filter)
+static int c2h_evt_hdl(struct rtw_adapter *adapter, struct c2h_evt_hdr *c2h_evt)
 {
-	s32 ret = _FAIL;
+	int ret = _FAIL;
 	u8 buf[16];
 
 	if (!c2h_evt) {
@@ -1550,73 +1281,57 @@ s32 c2h_evt_hdl(struct rtw_adapter *adapter, struct c2h_evt_hdr *c2h_evt,
 		if (c2h_evt_read23a(adapter, buf) == _SUCCESS) {
 			c2h_evt = (struct c2h_evt_hdr *)buf;
 
-			if (filter && filter(c2h_evt->id) == false)
-				goto exit;
-
-			ret = rtw_hal_c2h_handler23a(adapter, c2h_evt);
+			ret = c2h_handler_8723a(adapter, c2h_evt);
 		}
-	} else {
-
-		if (filter && filter(c2h_evt->id) == false)
-			goto exit;
+	} else
+		ret = c2h_handler_8723a(adapter, c2h_evt);
 
-		ret = rtw_hal_c2h_handler23a(adapter, c2h_evt);
-	}
-exit:
 	return ret;
 }
 
-static void c2h_wk_callback(struct work_struct *work)
+static void rtw_irq_work(struct work_struct *work)
 {
 	struct evt_priv *evtpriv;
 	struct rtw_adapter *adapter;
-	struct c2h_evt_hdr *c2h_evt;
-	c2h_id_filter ccx_id_filter;
 
-	evtpriv = container_of(work, struct evt_priv, c2h_wk);
+	evtpriv = container_of(work, struct evt_priv, irq_wk);
 	adapter = container_of(evtpriv, struct rtw_adapter, evtpriv);
-	ccx_id_filter = rtw_hal_c2h_id_filter_ccx23a(adapter);
-
-	evtpriv->c2h_wk_alive = true;
-
-	while (!rtw_cbuf_empty23a(evtpriv->c2h_queue)) {
-		c2h_evt = (struct c2h_evt_hdr *)
-			rtw_cbuf_pop23a(evtpriv->c2h_queue);
-		if (c2h_evt) {
-			/* This C2H event is read, clear it */
-			c2h_evt_clear23a(adapter);
-		} else if ((c2h_evt = (struct c2h_evt_hdr *)
-			    kmalloc(16, GFP_ATOMIC))) {
-			/* This C2H event is not read, read & clear now */
-			if (c2h_evt_read23a(adapter, (u8*)c2h_evt) != _SUCCESS)
-				continue;
-		}
 
-		/* Special pointer to trigger c2h_evt_clear23a only */
-		if ((void *)c2h_evt == (void *)evtpriv)
-			continue;
+	c2h_evt_clear23a(adapter);
+}
 
-		if (!c2h_evt_exist(c2h_evt)) {
-			kfree(c2h_evt);
-			continue;
-		}
+void rtw_evt_work(struct work_struct *work)
+{
+	struct evt_work *ework;
+	struct rtw_adapter *adapter;
 
-		if (ccx_id_filter(c2h_evt->id) == true) {
-			/* Handle CCX report here */
-			rtw_hal_c2h_handler23a(adapter, c2h_evt);
-			kfree(c2h_evt);
-		} else {
-			/* Enqueue into cmd_thread for others */
-			rtw_c2h_wk_cmd23a(adapter, (u8 *)c2h_evt);
-		}
+	ework = container_of(work, struct evt_work, work);
+	adapter = ework->adapter;
+
+	c2h_evt_clear23a(adapter);
+
+	if (!c2h_evt_exist(&ework->u.c2h_evt)) {
+		kfree(ework);
+		return;
 	}
 
-	evtpriv->c2h_wk_alive = false;
+	if (c2h_id_filter_ccx_8723a(ework->u.c2h_evt.id) == true) {
+		/* Handle CCX report here */
+		c2h_handler_8723a(adapter, &ework->u.c2h_evt);
+		kfree(ework);
+	} else {
+		/*
+		 * Enqueue into cmd_thread for others.
+		 * ework will be turned into a c2h_evt and freed once it
+		 * has been consumed.
+		 */
+		rtw_c2h_wk_cmd23a(adapter, (u8 *)&ework->u.c2h_evt);
+	}
 }
 
-u8 rtw_drvextra_cmd_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
+int rtw_drvextra_cmd_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
-	struct drvextra_cmd_parm *pdrvextra_cmd;
+	const struct drvextra_cmd_parm *pdrvextra_cmd;
 
 	if (!pbuf)
 		return H2C_PARAMETERS_ERROR;
@@ -1636,16 +1351,6 @@ u8 rtw_drvextra_cmd_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
 	case LPS_CTRL_WK_CID:
 		lps_ctrl_wk_hdl(padapter, (u8)pdrvextra_cmd->type_size);
 		break;
-#ifdef CONFIG_8723AU_P2P
-	case P2P_PS_WK_CID:
-		p2p_ps_wk_hdl23a(padapter, pdrvextra_cmd->type_size);
-		break;
-	case P2P_PROTO_WK_CID:
-		/*	Commented by Albert 2011/07/01 */
-		/*	I used the type_size as the type command */
-		p2p_protocol_wk_hdl23a(padapter, pdrvextra_cmd->type_size);
-		break;
-#endif /*  CONFIG_8723AU_P2P */
 #ifdef CONFIG_8723AU_AP_MODE
 	case CHECK_HIQ_WK_CID:
 		rtw_chk_hi_queue_hdl(padapter);
@@ -1653,7 +1358,7 @@ u8 rtw_drvextra_cmd_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
 #endif /* CONFIG_8723AU_AP_MODE */
 	case C2H_WK_CID:
 		c2h_evt_hdl(padapter,
-			    (struct c2h_evt_hdr *)pdrvextra_cmd->pbuf, NULL);
+			    (struct c2h_evt_hdr *)pdrvextra_cmd->pbuf);
 		break;
 
 	default:
@@ -1662,14 +1367,18 @@ u8 rtw_drvextra_cmd_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
 
 	if (pdrvextra_cmd->pbuf && (pdrvextra_cmd->type_size > 0)) {
 		kfree(pdrvextra_cmd->pbuf);
-		pdrvextra_cmd->pbuf = NULL;
+		/*
+		 * No need to set pdrvextra_cmd->pbuf = NULL as we were
+		 * operating on a copy of the original pcmd->parmbuf
+		 * created in rtw_cmd_work().
+		 */
 	}
 
 	return H2C_SUCCESS;
 }
 
 void rtw_survey_cmd_callback23a(struct rtw_adapter *padapter,
-			     struct cmd_obj *pcmd)
+				struct cmd_obj *pcmd)
 {
 	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
@@ -1748,14 +1457,13 @@ void rtw_createbss_cmd23a_callback(struct rtw_adapter *padapter,
 
 	del_timer_sync(&pmlmepriv->assoc_timer);
 
-	spin_lock_bh(&pmlmepriv->lock);
-
 	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
 		psta = rtw_get_stainfo23a(&padapter->stapriv,
 					  pnetwork->MacAddress);
 		if (!psta) {
 			psta = rtw_alloc_stainfo23a(&padapter->stapriv,
-						 pnetwork->MacAddress);
+						    pnetwork->MacAddress,
+						    GFP_KERNEL);
 			if (!psta) {
 				RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
 					 ("\nCan't alloc sta_info when "
@@ -1764,9 +1472,11 @@ void rtw_createbss_cmd23a_callback(struct rtw_adapter *padapter,
 			}
 		}
 
+		spin_lock_bh(&pmlmepriv->lock);
 		rtw_indicate_connect23a(padapter);
+		spin_unlock_bh(&pmlmepriv->lock);
 	} else {
-		pwlan = rtw_alloc_network(pmlmepriv);
+		pwlan = rtw_alloc_network(pmlmepriv, GFP_KERNEL);
 		spin_lock_bh(&pmlmepriv->scanned_queue.lock);
 		if (!pwlan) {
 			pwlan = rtw_get_oldest_wlan_network23a(&pmlmepriv->scanned_queue);
@@ -1797,7 +1507,7 @@ void rtw_createbss_cmd23a_callback(struct rtw_adapter *padapter,
 
 		/*  reset DSConfig */
 
-		_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+		clr_fwstate(pmlmepriv, _FW_UNDER_LINKING);
 
 		spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 		/*  we will set _FW_LINKED when there is one more sat to
@@ -1806,8 +1516,6 @@ void rtw_createbss_cmd23a_callback(struct rtw_adapter *padapter,
 
 createbss_cmd_fail:
 
-	spin_unlock_bh(&pmlmepriv->lock);
-
 	rtw_free_cmd_obj23a(pcmd);
 }
 
@@ -1858,8 +1566,8 @@ void rtw_setassocsta_cmdrsp_callback23a(struct rtw_adapter *padapter,
 
 	spin_lock_bh(&pmlmepriv->lock);
 
-	if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == true) &&
-	    (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true))
+	if (check_fwstate(pmlmepriv, WIFI_MP_STATE) &&
+	    check_fwstate(pmlmepriv, _FW_UNDER_LINKING))
 		_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
 
 	set_fwstate(pmlmepriv, _FW_LINKED);
@@ -1868,9 +1576,3 @@ void rtw_setassocsta_cmdrsp_callback23a(struct rtw_adapter *padapter,
 exit:
 	rtw_free_cmd_obj23a(pcmd);
 }
-
-void rtw_getrttbl_cmd_cmdrsp_callback(struct rtw_adapter *padapter,
-				      struct cmd_obj *pcmd)
-{
-	rtw_free_cmd_obj23a(pcmd);
-}
diff --git a/drivers/staging/rtl8723au/core/rtw_efuse.c b/drivers/staging/rtl8723au/core/rtw_efuse.c
index 35b177fd051..cc063cbc49b 100644
--- a/drivers/staging/rtl8723au/core/rtw_efuse.c
+++ b/drivers/staging/rtl8723au/core/rtw_efuse.c
@@ -18,6 +18,8 @@
 #include <drv_types.h>
 
 #include <rtw_efuse.h>
+#include <rtl8723a_hal.h>
+#include <usb_ops_linux.h>
 
 /*------------------------Define local variable------------------------------*/
 
@@ -26,8 +28,11 @@
 #define EFUSE_CTRL			REG_EFUSE_CTRL		/*  E-Fuse Control. */
 /*  */
 
+#define VOLTAGE_V25		0x03
+#define LDOE25_SHIFT		28
+
 /*-----------------------------------------------------------------------------
- * Function:	Efuse_PowerSwitch23a
+ * Function:	Efuse_PowerSwitch
  *
  * Overview:	When we want to enable write operation, we should change to
  *				pwr on state. When we stop write, we should switch to 500k mode
@@ -44,13 +49,55 @@
  * 11/17/2008	MHC		Create Version 0.
  *
  *---------------------------------------------------------------------------*/
-void
-Efuse_PowerSwitch23a(
-	struct rtw_adapter *	pAdapter,
-	u8		bWrite,
-	u8		PwrState)
+static void Efuse_PowerSwitch(struct rtw_adapter *padapter,
+			      u8 bWrite, u8 PwrState)
 {
-	pAdapter->HalFunc.EfusePowerSwitch(pAdapter, bWrite, PwrState);
+	u8 tempval;
+	u16 tmpV16;
+
+	if (PwrState == true) {
+		rtl8723au_write8(padapter, REG_EFUSE_ACCESS, EFUSE_ACCESS_ON);
+
+		/*  1.2V Power: From VDDON with Power
+		    Cut(0x0000h[15]), defualt valid */
+		tmpV16 = rtl8723au_read16(padapter, REG_SYS_ISO_CTRL);
+		if (!(tmpV16 & PWC_EV12V)) {
+			tmpV16 |= PWC_EV12V;
+			rtl8723au_write16(padapter, REG_SYS_ISO_CTRL, tmpV16);
+		}
+		/*  Reset: 0x0000h[28], default valid */
+		tmpV16 = rtl8723au_read16(padapter, REG_SYS_FUNC_EN);
+		if (!(tmpV16 & FEN_ELDR)) {
+			tmpV16 |= FEN_ELDR;
+			rtl8723au_write16(padapter, REG_SYS_FUNC_EN, tmpV16);
+		}
+
+		/*  Clock: Gated(0x0008h[5]) 8M(0x0008h[1]) clock
+		    from ANA, default valid */
+		tmpV16 = rtl8723au_read16(padapter, REG_SYS_CLKR);
+		if ((!(tmpV16 & LOADER_CLK_EN)) || (!(tmpV16 & ANA8M))) {
+			tmpV16 |= (LOADER_CLK_EN | ANA8M);
+			rtl8723au_write16(padapter, REG_SYS_CLKR, tmpV16);
+		}
+
+		if (bWrite == true) {
+			/*  Enable LDO 2.5V before read/write action */
+			tempval = rtl8723au_read8(padapter, EFUSE_TEST + 3);
+			tempval &= 0x0F;
+			tempval |= (VOLTAGE_V25 << 4);
+			rtl8723au_write8(padapter, EFUSE_TEST + 3,
+					 tempval | 0x80);
+		}
+	} else {
+		rtl8723au_write8(padapter, REG_EFUSE_ACCESS, EFUSE_ACCESS_OFF);
+
+		if (bWrite == true) {
+			/*  Disable LDO 2.5V after read/write action */
+			tempval = rtl8723au_read8(padapter, EFUSE_TEST + 3);
+			rtl8723au_write8(padapter, EFUSE_TEST + 3,
+					 tempval & 0x7F);
+		}
+	}
 }
 
 /*-----------------------------------------------------------------------------
@@ -74,7 +121,10 @@ Efuse_GetCurrentSize23a(struct rtw_adapter *pAdapter, u8 efuseType)
 {
 	u16 ret = 0;
 
-	ret = pAdapter->HalFunc.EfuseGetCurrentSize(pAdapter, efuseType);
+	if (efuseType == EFUSE_WIFI)
+		ret = rtl8723a_EfuseGetCurrentSize_WiFi(pAdapter);
+	else
+		ret = rtl8723a_EfuseGetCurrentSize_BT(pAdapter);
 
 	return ret;
 }
@@ -110,21 +160,22 @@ ReadEFuseByte23a(struct rtw_adapter *Adapter, u16 _offset, u8 *pbuf)
 	u16	retry;
 
 	/* Write Address */
-	rtw_write8(Adapter, EFUSE_CTRL+1, (_offset & 0xff));
-	readbyte = rtw_read8(Adapter, EFUSE_CTRL+2);
-	rtw_write8(Adapter, EFUSE_CTRL+2, ((_offset >> 8) & 0x03) | (readbyte & 0xfc));
+	rtl8723au_write8(Adapter, EFUSE_CTRL+1, (_offset & 0xff));
+	readbyte = rtl8723au_read8(Adapter, EFUSE_CTRL+2);
+	rtl8723au_write8(Adapter, EFUSE_CTRL+2,
+			 ((_offset >> 8) & 0x03) | (readbyte & 0xfc));
 
 	/* Write bit 32 0 */
-	readbyte = rtw_read8(Adapter, EFUSE_CTRL+3);
-	rtw_write8(Adapter, EFUSE_CTRL+3, (readbyte & 0x7f));
+	readbyte = rtl8723au_read8(Adapter, EFUSE_CTRL+3);
+	rtl8723au_write8(Adapter, EFUSE_CTRL+3, readbyte & 0x7f);
 
 	/* Check bit 32 read-ready */
 	retry = 0;
-	value32 = rtw_read32(Adapter, EFUSE_CTRL);
+	value32 = rtl8723au_read32(Adapter, EFUSE_CTRL);
 	/* while(!(((value32 >> 24) & 0xff) & 0x80)  && (retry<10)) */
 	while(!(((value32 >> 24) & 0xff) & 0x80)  && (retry<10000))
 	{
-		value32 = rtw_read32(Adapter, EFUSE_CTRL);
+		value32 = rtl8723au_read32(Adapter, EFUSE_CTRL);
 		retry++;
 	}
 
@@ -133,46 +184,92 @@ ReadEFuseByte23a(struct rtw_adapter *Adapter, u16 _offset, u8 *pbuf)
 	/*  Designer says that there shall be some delay after ready bit is set, or the */
 	/*  result will always stay on last data we read. */
 	udelay(50);
-	value32 = rtw_read32(Adapter, EFUSE_CTRL);
+	value32 = rtl8723au_read32(Adapter, EFUSE_CTRL);
 
 	*pbuf = (u8)(value32 & 0xff);
 }
 
-/*  */
-/*	Description: */
-/*		1. Execute E-Fuse read byte operation according as map offset and */
-/*		    save to E-Fuse table. */
-/*		2. Refered from SD1 Richard. */
-/*  */
-/*	Assumption: */
-/*		1. Boot from E-Fuse and successfully auto-load. */
-/*		2. PASSIVE_LEVEL (USB interface) */
-/*  */
-/*	Created by Roger, 2008.10.21. */
-/*  */
-/*	2008/12/12 MH	1. Reorganize code flow and reserve bytes. and add description. */
-/*					2. Add efuse utilization collect. */
-/*	2008/12/22 MH	Read Efuse must check if we write section 1 data again!!! Sec1 */
-/*					write addr must be after sec5. */
-/*  */
-
-void
-efuse_ReadEFuse(struct rtw_adapter *Adapter, u8 efuseType,
-		u16 _offset, u16 _size_byte, u8 *pbuf);
-void
-efuse_ReadEFuse(struct rtw_adapter *Adapter, u8 efuseType,
-		u16 _offset, u16 _size_byte, u8 *pbuf)
-{
-	Adapter->HalFunc.ReadEFuse(Adapter, efuseType, _offset,
-				   _size_byte, pbuf);
-}
-
 void
 EFUSE_GetEfuseDefinition23a(struct rtw_adapter *pAdapter, u8 efuseType,
-			 u8 type, void *pOut)
+			    u8 type, void *pOut)
 {
-	pAdapter->HalFunc.EFUSEGetEfuseDefinition(pAdapter, efuseType,
-						  type, pOut);
+	u8 *pu1Tmp;
+	u16 *pu2Tmp;
+	u8 *pMax_section;
+
+	switch (type) {
+	case TYPE_EFUSE_MAX_SECTION:
+		pMax_section = (u8 *) pOut;
+
+		if (efuseType == EFUSE_WIFI)
+			*pMax_section = EFUSE_MAX_SECTION_8723A;
+		else
+			*pMax_section = EFUSE_BT_MAX_SECTION;
+		break;
+
+	case TYPE_EFUSE_REAL_CONTENT_LEN:
+		pu2Tmp = (u16 *) pOut;
+
+		if (efuseType == EFUSE_WIFI)
+			*pu2Tmp = EFUSE_REAL_CONTENT_LEN_8723A;
+		else
+			*pu2Tmp = EFUSE_BT_REAL_CONTENT_LEN;
+		break;
+
+	case TYPE_AVAILABLE_EFUSE_BYTES_BANK:
+		pu2Tmp = (u16 *) pOut;
+
+		if (efuseType == EFUSE_WIFI)
+			*pu2Tmp = (EFUSE_REAL_CONTENT_LEN_8723A -
+				   EFUSE_OOB_PROTECT_BYTES);
+		else
+			*pu2Tmp = (EFUSE_BT_REAL_BANK_CONTENT_LEN -
+				   EFUSE_PROTECT_BYTES_BANK);
+		break;
+
+	case TYPE_AVAILABLE_EFUSE_BYTES_TOTAL:
+		pu2Tmp = (u16 *) pOut;
+
+		if (efuseType == EFUSE_WIFI)
+			*pu2Tmp = (EFUSE_REAL_CONTENT_LEN_8723A -
+				   EFUSE_OOB_PROTECT_BYTES);
+		else
+			*pu2Tmp = (EFUSE_BT_REAL_CONTENT_LEN -
+				   (EFUSE_PROTECT_BYTES_BANK * 3));
+		break;
+
+	case TYPE_EFUSE_MAP_LEN:
+		pu2Tmp = (u16 *) pOut;
+
+		if (efuseType == EFUSE_WIFI)
+			*pu2Tmp = EFUSE_MAP_LEN_8723A;
+		else
+			*pu2Tmp = EFUSE_BT_MAP_LEN;
+		break;
+
+	case TYPE_EFUSE_PROTECT_BYTES_BANK:
+		pu1Tmp = (u8 *) pOut;
+
+		if (efuseType == EFUSE_WIFI)
+			*pu1Tmp = EFUSE_OOB_PROTECT_BYTES;
+		else
+			*pu1Tmp = EFUSE_PROTECT_BYTES_BANK;
+		break;
+
+	case TYPE_EFUSE_CONTENT_LEN_BANK:
+		pu2Tmp = (u16 *) pOut;
+
+		if (efuseType == EFUSE_WIFI)
+			*pu2Tmp = EFUSE_REAL_CONTENT_LEN_8723A;
+		else
+			*pu2Tmp = EFUSE_BT_REAL_BANK_CONTENT_LEN;
+		break;
+
+	default:
+		pu1Tmp = (u8 *) pOut;
+		*pu1Tmp = 0;
+		break;
+	}
 }
 
 /*-----------------------------------------------------------------------------
@@ -208,22 +305,22 @@ EFUSE_Read1Byte23a(struct rtw_adapter *Adapter, u16 Address)
 	{
 		/* Write E-fuse Register address bit0~7 */
 		temp = Address & 0xFF;
-		rtw_write8(Adapter, EFUSE_CTRL+1, temp);
-		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+2);
+		rtl8723au_write8(Adapter, EFUSE_CTRL+1, temp);
+		Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+2);
 		/* Write E-fuse Register address bit8~9 */
 		temp = ((Address >> 8) & 0x03) | (Bytetemp & 0xFC);
-		rtw_write8(Adapter, EFUSE_CTRL+2, temp);
+		rtl8723au_write8(Adapter, EFUSE_CTRL+2, temp);
 
 		/* Write 0x30[31]= 0 */
-		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+		Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+3);
 		temp = Bytetemp & 0x7F;
-		rtw_write8(Adapter, EFUSE_CTRL+3, temp);
+		rtl8723au_write8(Adapter, EFUSE_CTRL+3, temp);
 
 		/* Wait Write-ready (0x30[31]= 1) */
-		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+		Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+3);
 		while(!(Bytetemp & 0x80))
 		{
-			Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+			Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+3);
 			k++;
 			if (k == 1000)
 			{
@@ -231,7 +328,7 @@ EFUSE_Read1Byte23a(struct rtw_adapter *Adapter, u16 Address)
 				break;
 			}
 		}
-		data = rtw_read8(Adapter, EFUSE_CTRL);
+		data = rtl8723au_read8(Adapter, EFUSE_CTRL);
 		return data;
 	}
 	else
@@ -278,27 +375,27 @@ EFUSE_Write1Byte(
 
 	if (Address < contentLen)	/* E-fuse 512Byte */
 	{
-		rtw_write8(Adapter, EFUSE_CTRL, Value);
+		rtl8723au_write8(Adapter, EFUSE_CTRL, Value);
 
 		/* Write E-fuse Register address bit0~7 */
 		temp = Address & 0xFF;
-		rtw_write8(Adapter, EFUSE_CTRL+1, temp);
-		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+2);
+		rtl8723au_write8(Adapter, EFUSE_CTRL+1, temp);
+		Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+2);
 
 		/* Write E-fuse Register address bit8~9 */
 		temp = ((Address >> 8) & 0x03) | (Bytetemp & 0xFC);
-		rtw_write8(Adapter, EFUSE_CTRL+2, temp);
+		rtl8723au_write8(Adapter, EFUSE_CTRL+2, temp);
 
 		/* Write 0x30[31]= 1 */
-		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+		Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+3);
 		temp = Bytetemp | 0x80;
-		rtw_write8(Adapter, EFUSE_CTRL+3, temp);
+		rtl8723au_write8(Adapter, EFUSE_CTRL+3, temp);
 
 		/* Wait Write-ready (0x30[31]= 0) */
-		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+		Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+3);
 		while(Bytetemp & 0x80)
 		{
-			Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+			Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+3);
 			k++;
 			if (k == 100)
 			{
@@ -310,38 +407,38 @@ EFUSE_Write1Byte(
 }/* EFUSE_Write1Byte */
 
 /*  11/16/2008 MH Read one byte from real Efuse. */
-u8
+int
 efuse_OneByteRead23a(struct rtw_adapter *pAdapter, u16 addr, u8 *data)
 {
 	u8	tmpidx = 0;
-	u8	bResult;
+	int	bResult;
 
 	/*  -----------------e-fuse reg ctrl --------------------------------- */
 	/* address */
-	rtw_write8(pAdapter, EFUSE_CTRL+1, (u8)(addr&0xff));
-	rtw_write8(pAdapter, EFUSE_CTRL+2, ((u8)((addr>>8) &0x03)) |
-	(rtw_read8(pAdapter, EFUSE_CTRL+2)&0xFC));
+	rtl8723au_write8(pAdapter, EFUSE_CTRL+1, (u8)(addr&0xff));
+	rtl8723au_write8(pAdapter, EFUSE_CTRL+2, ((u8)((addr>>8) &0x03)) |
+	(rtl8723au_read8(pAdapter, EFUSE_CTRL+2)&0xFC));
 
-	rtw_write8(pAdapter, EFUSE_CTRL+3,  0x72);/* read cmd */
+	rtl8723au_write8(pAdapter, EFUSE_CTRL+3,  0x72);/* read cmd */
 
-	while(!(0x80 &rtw_read8(pAdapter, EFUSE_CTRL+3)) && (tmpidx<100))
+	while(!(0x80 &rtl8723au_read8(pAdapter, EFUSE_CTRL+3)) && (tmpidx<100))
 		tmpidx++;
 	if (tmpidx < 100) {
-		*data = rtw_read8(pAdapter, EFUSE_CTRL);
-		bResult = true;
+		*data = rtl8723au_read8(pAdapter, EFUSE_CTRL);
+		bResult = _SUCCESS;
 	} else {
 		*data = 0xff;
-		bResult = false;
+		bResult = _FAIL;
 	}
 	return bResult;
 }
 
 /*  11/16/2008 MH Write one byte to reald Efuse. */
-u8
+int
 efuse_OneByteWrite23a(struct rtw_adapter *pAdapter, u16 addr, u8 data)
 {
 	u8	tmpidx = 0;
-	u8	bResult;
+	int	bResult;
 
 	/* RT_TRACE(COMP_EFUSE, DBG_LOUD, ("Addr = %x Data =%x\n", addr, data)); */
 
@@ -349,51 +446,26 @@ efuse_OneByteWrite23a(struct rtw_adapter *pAdapter, u16 addr, u8 data)
 
 	/*  -----------------e-fuse reg ctrl --------------------------------- */
 	/* address */
-	rtw_write8(pAdapter, EFUSE_CTRL+1, (u8)(addr&0xff));
-	rtw_write8(pAdapter, EFUSE_CTRL+2,
-	(rtw_read8(pAdapter, EFUSE_CTRL+2)&0xFC)|(u8)((addr>>8)&0x03));
-	rtw_write8(pAdapter, EFUSE_CTRL, data);/* data */
+	rtl8723au_write8(pAdapter, EFUSE_CTRL+1, (u8)(addr&0xff));
+	rtl8723au_write8(pAdapter, EFUSE_CTRL+2,
+	(rtl8723au_read8(pAdapter, EFUSE_CTRL+2)&0xFC)|(u8)((addr>>8)&0x03));
+	rtl8723au_write8(pAdapter, EFUSE_CTRL, data);/* data */
 
-	rtw_write8(pAdapter, EFUSE_CTRL+3, 0xF2);/* write cmd */
+	rtl8723au_write8(pAdapter, EFUSE_CTRL+3, 0xF2);/* write cmd */
 
-	while((0x80 &  rtw_read8(pAdapter, EFUSE_CTRL+3)) && (tmpidx<100)) {
+	while((0x80 & rtl8723au_read8(pAdapter, EFUSE_CTRL+3)) &&
+	      (tmpidx<100)) {
 		tmpidx++;
 	}
 
-	if (tmpidx<100)
-	{
-		bResult = true;
-	}
+	if (tmpidx < 100)
+		bResult = _SUCCESS;
 	else
-	{
-		bResult = false;
-	}
+		bResult = _FAIL;
 
 	return bResult;
 }
 
-int
-Efuse_PgPacketRead23a(struct rtw_adapter *pAdapter, u8 offset, u8 *data)
-{
-	int	ret = 0;
-
-	ret =  pAdapter->HalFunc.Efuse_PgPacketRead23a(pAdapter, offset, data);
-
-	return ret;
-}
-
-int
-Efuse_PgPacketWrite23a(struct rtw_adapter *pAdapter, u8 offset,
-		    u8 word_en, u8 *data)
-{
-	int ret;
-
-	ret =  pAdapter->HalFunc.Efuse_PgPacketWrite23a(pAdapter, offset,
-						     word_en, data);
-
-	return ret;
-}
-
 /*-----------------------------------------------------------------------------
  * Function:	efuse_WordEnableDataRead23a
  *
@@ -438,24 +510,12 @@ efuse_WordEnableDataRead23a(u8	word_en,
 	}
 }
 
-u8
-Efuse_WordEnableDataWrite23a(struct rtw_adapter *pAdapter, u16 efuse_addr,
-			  u8 word_en, u8 *data)
-{
-	u8 ret = 0;
-
-	ret = pAdapter->HalFunc.Efuse_WordEnableDataWrite23a(pAdapter, efuse_addr,
-							  word_en, data);
-
-	return ret;
-}
-
-static u8 efuse_read8(struct rtw_adapter *padapter, u16 address, u8 *value)
+static int efuse_read8(struct rtw_adapter *padapter, u16 address, u8 *value)
 {
 	return efuse_OneByteRead23a(padapter, address, value);
 }
 
-static u8 efuse_write8(struct rtw_adapter *padapter, u16 address, u8 *value)
+static int efuse_write8(struct rtw_adapter *padapter, u16 address, u8 *value)
 {
 	return efuse_OneByteWrite23a(padapter, address, *value);
 }
@@ -463,13 +523,13 @@ static u8 efuse_write8(struct rtw_adapter *padapter, u16 address, u8 *value)
 /*
  * read/wirte raw efuse data
  */
-u8 rtw_efuse_access23a(struct rtw_adapter *padapter, u8 bWrite, u16 start_addr,
-		    u16 cnts, u8 *data)
+int rtw_efuse_access23a(struct rtw_adapter *padapter, u8 bWrite, u16 start_addr,
+			u16 cnts, u8 *data)
 {
 	int i = 0;
-	u16	real_content_len = 0, max_available_size = 0;
-	u8 res = _FAIL ;
-	u8 (*rw8)(struct rtw_adapter *, u16, u8*);
+	u16 real_content_len = 0, max_available_size = 0;
+	int res = _FAIL ;
+	int (*rw8)(struct rtw_adapter *, u16, u8*);
 
 	EFUSE_GetEfuseDefinition23a(padapter, EFUSE_WIFI,
 				 TYPE_EFUSE_REAL_CONTENT_LEN,
@@ -488,7 +548,7 @@ u8 rtw_efuse_access23a(struct rtw_adapter *padapter, u8 bWrite, u16 start_addr,
 	} else
 		rw8 = &efuse_read8;
 
-	Efuse_PowerSwitch23a(padapter, bWrite, true);
+	Efuse_PowerSwitch(padapter, bWrite, true);
 
 	/*  e-fuse one byte read / write */
 	for (i = 0; i < cnts; i++) {
@@ -498,35 +558,37 @@ u8 rtw_efuse_access23a(struct rtw_adapter *padapter, u8 bWrite, u16 start_addr,
 		}
 
 		res = rw8(padapter, start_addr++, data++);
-		if (_FAIL == res) break;
+		if (res == _FAIL)
+			break;
 	}
 
-	Efuse_PowerSwitch23a(padapter, bWrite, false);
+	Efuse_PowerSwitch(padapter, bWrite, false);
 
 	return res;
 }
 /*  */
 u16 efuse_GetMaxSize23a(struct rtw_adapter *padapter)
 {
-	u16	max_size;
+	u16 max_size;
 	EFUSE_GetEfuseDefinition23a(padapter, EFUSE_WIFI,
 				 TYPE_AVAILABLE_EFUSE_BYTES_TOTAL,
 				 (void *)&max_size);
 	return max_size;
 }
 /*  */
-u8 efuse_GetCurrentSize23a(struct rtw_adapter *padapter, u16 *size)
+int efuse_GetCurrentSize23a(struct rtw_adapter *padapter, u16 *size)
 {
-	Efuse_PowerSwitch23a(padapter, false, true);
+	Efuse_PowerSwitch(padapter, false, true);
 	*size = Efuse_GetCurrentSize23a(padapter, EFUSE_WIFI);
-	Efuse_PowerSwitch23a(padapter, false, false);
+	Efuse_PowerSwitch(padapter, false, false);
 
 	return _SUCCESS;
 }
 /*  */
-u8 rtw_efuse_map_read23a(struct rtw_adapter *padapter, u16 addr, u16 cnts, u8 *data)
+int rtw_efuse_map_read23a(struct rtw_adapter *padapter,
+			  u16 addr, u16 cnts, u8 *data)
 {
-	u16	mapLen = 0;
+	u16 mapLen = 0;
 
 	EFUSE_GetEfuseDefinition23a(padapter, EFUSE_WIFI,
 				 TYPE_EFUSE_MAP_LEN, (void *)&mapLen);
@@ -534,18 +596,19 @@ u8 rtw_efuse_map_read23a(struct rtw_adapter *padapter, u16 addr, u16 cnts, u8 *d
 	if ((addr + cnts) > mapLen)
 		return _FAIL;
 
-	Efuse_PowerSwitch23a(padapter, false, true);
+	Efuse_PowerSwitch(padapter, false, true);
 
-	efuse_ReadEFuse(padapter, EFUSE_WIFI, addr, cnts, data);
+	rtl8723a_readefuse(padapter, EFUSE_WIFI, addr, cnts, data);
 
-	Efuse_PowerSwitch23a(padapter, false, false);
+	Efuse_PowerSwitch(padapter, false, false);
 
 	return _SUCCESS;
 }
 
-u8 rtw_BT_efuse_map_read23a(struct rtw_adapter *padapter, u16 addr, u16 cnts, u8 *data)
+int rtw_BT_efuse_map_read23a(struct rtw_adapter *padapter,
+			     u16 addr, u16 cnts, u8 *data)
 {
-	u16	mapLen = 0;
+	u16 mapLen = 0;
 
 	EFUSE_GetEfuseDefinition23a(padapter, EFUSE_BT,
 				 TYPE_EFUSE_MAP_LEN, (void *)&mapLen);
@@ -553,11 +616,11 @@ u8 rtw_BT_efuse_map_read23a(struct rtw_adapter *padapter, u16 addr, u16 cnts, u8
 	if ((addr + cnts) > mapLen)
 		return _FAIL;
 
-	Efuse_PowerSwitch23a(padapter, false, true);
+	Efuse_PowerSwitch(padapter, false, true);
 
-	efuse_ReadEFuse(padapter, EFUSE_BT, addr, cnts, data);
+	rtl8723a_readefuse(padapter, EFUSE_BT, addr, cnts, data);
 
-	Efuse_PowerSwitch23a(padapter, false, false);
+	Efuse_PowerSwitch(padapter, false, false);
 
 	return _SUCCESS;
 }
@@ -585,14 +648,14 @@ Efuse_ReadAllMap(struct rtw_adapter *pAdapter, u8 efuseType, u8 *Efuse)
 {
 	u16	mapLen = 0;
 
-	Efuse_PowerSwitch23a(pAdapter, false, true);
+	Efuse_PowerSwitch(pAdapter, false, true);
 
 	EFUSE_GetEfuseDefinition23a(pAdapter, efuseType, TYPE_EFUSE_MAP_LEN,
 				 (void *)&mapLen);
 
-	efuse_ReadEFuse(pAdapter, efuseType, 0, mapLen, Efuse);
+	rtl8723a_readefuse(pAdapter, efuseType, 0, mapLen, Efuse);
 
-	Efuse_PowerSwitch23a(pAdapter, false, false);
+	Efuse_PowerSwitch(pAdapter, false, false);
 }
 
 /*-----------------------------------------------------------------------------
diff --git a/drivers/staging/rtl8723au/core/rtw_ieee80211.c b/drivers/staging/rtl8723au/core/rtw_ieee80211.c
index a48ab25a7d8..adb86a54bdb 100644
--- a/drivers/staging/rtl8723au/core/rtw_ieee80211.c
+++ b/drivers/staging/rtl8723au/core/rtw_ieee80211.c
@@ -46,21 +46,23 @@ u8 RSN_CIPHER_SUITE_WEP10423A[] = { 0x00, 0x0f, 0xac, 5 };
 /*  for adhoc-master to generate ie and provide supported-rate to fw */
 /*  */
 
-static u8	WIFI_CCKRATES[] =
-{(IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK),
- (IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK),
- (IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK),
- (IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK)};
-
-static u8	WIFI_OFDMRATES[] =
-{(IEEE80211_OFDM_RATE_6MB),
- (IEEE80211_OFDM_RATE_9MB),
- (IEEE80211_OFDM_RATE_12MB),
- (IEEE80211_OFDM_RATE_18MB),
- (IEEE80211_OFDM_RATE_24MB),
- IEEE80211_OFDM_RATE_36MB,
- IEEE80211_OFDM_RATE_48MB,
- IEEE80211_OFDM_RATE_54MB};
+static u8 WIFI_CCKRATES[] = {
+	IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK
+};
+
+static u8 WIFI_OFDMRATES[] = {
+	IEEE80211_OFDM_RATE_6MB,
+	IEEE80211_OFDM_RATE_9MB,
+	IEEE80211_OFDM_RATE_12MB,
+	IEEE80211_OFDM_RATE_18MB,
+	IEEE80211_OFDM_RATE_24MB,
+	IEEE80211_OFDM_RATE_36MB,
+	IEEE80211_OFDM_RATE_48MB,
+	IEEE80211_OFDM_RATE_54MB
+};
 
 int rtw_get_bit_value_from_ieee_value23a(u8 val)
 {
@@ -76,13 +78,13 @@ int rtw_get_bit_value_from_ieee_value23a(u8 val)
 	return 0;
 }
 
-uint rtw_is_cckrates_included23a(u8 *rate)
+static bool rtw_is_cckrates_included(u8 *rate)
 {
 	u32 i = 0;
 
-	while (rate[i] != 0) {
-		if ((((rate[i]) & 0x7f) == 2) || (((rate[i]) & 0x7f) == 4) ||
-		    (((rate[i]) & 0x7f) == 11) || (((rate[i]) & 0x7f) == 22))
+	while (rate[i]) {
+		if ((rate[i] & 0x7f) == 2 || (rate[i] & 0x7f) == 4 ||
+		    (rate[i] & 0x7f) == 11 || (rate[i] & 0x7f) == 22)
 			return true;
 		i++;
 	}
@@ -90,13 +92,13 @@ uint rtw_is_cckrates_included23a(u8 *rate)
 	return false;
 }
 
-uint rtw_is_cckratesonly_included23a(u8 *rate)
+static bool rtw_is_cckratesonly_included(u8 *rate)
 {
 	u32 i = 0;
 
-	while (rate[i] != 0) {
-		if ((((rate[i]) & 0x7f) != 2) && (((rate[i]) & 0x7f) != 4) &&
-		    (((rate[i]) & 0x7f) != 11) && (((rate[i]) & 0x7f) != 22))
+	while (rate[i]) {
+		if ((rate[i] & 0x7f) != 2 && (rate[i] & 0x7f) != 4 &&
+		    (rate[i] & 0x7f) != 11 && (rate[i] & 0x7f) != 22)
 			return false;
 
 		i++;
@@ -108,14 +110,14 @@ uint rtw_is_cckratesonly_included23a(u8 *rate)
 int rtw_check_network_type23a(unsigned char *rate, int ratelen, int channel)
 {
 	if (channel > 14) {
-		if ((rtw_is_cckrates_included23a(rate)) == true)
+		if (rtw_is_cckrates_included(rate))
 			return WIRELESS_INVALID;
 		else
 			return WIRELESS_11A;
 	} else {  /*  could be pure B, pure G, or B/G */
-		if ((rtw_is_cckratesonly_included23a(rate)) == true)
+		if (rtw_is_cckratesonly_included(rate))
 			return WIRELESS_11B;
-		else if ((rtw_is_cckrates_included23a(rate)) == true)
+		else if (rtw_is_cckrates_included(rate))
 			return	WIRELESS_11BG;
 		else
 			return WIRELESS_11G;
@@ -131,7 +133,7 @@ u8 *rtw_set_fixed_ie23a(unsigned char *pbuf, unsigned int len,
 }
 
 /*  rtw_set_ie23a will update frame length */
-u8 *rtw_set_ie23a(u8 *pbuf, int index, uint len, u8 *source, uint *frlen)
+u8 *rtw_set_ie23a(u8 *pbuf, int index, uint len, const u8 *source, uint *frlen)
 {
 
 	*pbuf = (u8)index;
@@ -143,7 +145,6 @@ u8 *rtw_set_ie23a(u8 *pbuf, int index, uint len, u8 *source, uint *frlen)
 
 	*frlen = *frlen + (len + 2);
 
-
 	return pbuf + len + 2;
 }
 
@@ -158,18 +159,6 @@ inline u8 *rtw_set_ie23a_ch_switch (u8 *buf, u32 *buf_len, u8 ch_switch_mode,
 	return rtw_set_ie23a(buf, WLAN_EID_CHANNEL_SWITCH,  3, ie_data, buf_len);
 }
 
-inline u8 secondary_ch_offset_to_hal_ch_offset23a(u8 ch_offset)
-{
-	if (ch_offset == SCN)
-		return HAL_PRIME_CHNL_OFFSET_DONT_CARE;
-	else if (ch_offset == SCA)
-		return HAL_PRIME_CHNL_OFFSET_UPPER;
-	else if (ch_offset == SCB)
-		return HAL_PRIME_CHNL_OFFSET_LOWER;
-
-	return HAL_PRIME_CHNL_OFFSET_DONT_CARE;
-}
-
 inline u8 hal_ch_offset_to_secondary_ch_offset23a(u8 ch_offset)
 {
 	if (ch_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE)
@@ -189,19 +178,6 @@ inline u8 *rtw_set_ie23a_secondary_ch_offset(u8 *buf, u32 *buf_len,
 			  1, &secondary_ch_offset, buf_len);
 }
 
-inline u8 *rtw_set_ie23a_mesh_ch_switch_parm(u8 *buf, u32 *buf_len, u8 ttl,
-					  u8 flags, u16 reason, u16 precedence)
-{
-	u8 ie_data[6];
-
-	ie_data[0] = ttl;
-	ie_data[1] = flags;
-	put_unaligned_le16(reason, (u8*)&ie_data[2]);
-	put_unaligned_le16(precedence, (u8*)&ie_data[4]);
-
-	return rtw_set_ie23a(buf, 0x118,  6, ie_data, buf_len);
-}
-
 /*----------------------------------------------------------------------------
 index: the information element id index, limit is the limit for search
 -----------------------------------------------------------------------------*/
@@ -367,7 +343,7 @@ uint rtw_get_rateset_len23a(u8 *rateset)
 	uint i = 0;
 
 	while(1) {
-		if ((rateset[i]) == 0)
+		if (rateset[i] == 0)
 			break;
 
 		if (i > 12)
@@ -394,31 +370,31 @@ int rtw_generate_ie23a(struct registry_priv *pregistrypriv)
 
 	/* beacon interval : 2bytes */
 	/* BCN_INTERVAL; */
-	*(u16*)ie = cpu_to_le16((u16)pdev_network->Configuration.BeaconPeriod);
+	*(u16*)ie = cpu_to_le16(pdev_network->BeaconPeriod);
 	sz += 2;
 	ie += 2;
 
 	/* capability info */
 	*(u16*)ie = 0;
 
-	*(u16*)ie |= cpu_to_le16(cap_IBSS);
+	*(u16*)ie |= cpu_to_le16(WLAN_CAPABILITY_IBSS);
 
 	if (pregistrypriv->preamble == PREAMBLE_SHORT)
-		*(u16*)ie |= cpu_to_le16(cap_ShortPremble);
+		*(u16*)ie |= cpu_to_le16(WLAN_CAPABILITY_SHORT_PREAMBLE);
 
 	if (pdev_network->Privacy)
-		*(u16*)ie |= cpu_to_le16(cap_Privacy);
+		*(u16*)ie |= cpu_to_le16(WLAN_CAPABILITY_PRIVACY);
 
 	sz += 2;
 	ie += 2;
 
 	/* SSID */
-	ie = rtw_set_ie23a(ie, _SSID_IE_, pdev_network->Ssid.ssid_len,
+	ie = rtw_set_ie23a(ie, WLAN_EID_SSID, pdev_network->Ssid.ssid_len,
 			pdev_network->Ssid.ssid, &sz);
 
 	/* supported rates */
 	if (pregistrypriv->wireless_mode == WIRELESS_11ABGN) {
-		if (pdev_network->Configuration.DSConfig > 14)
+		if (pdev_network->DSConfig > 14)
 			wireless_mode = WIRELESS_11A_5N;
 		else
 			wireless_mode = WIRELESS_11BG_24N;
@@ -431,25 +407,25 @@ int rtw_generate_ie23a(struct registry_priv *pregistrypriv)
 	rateLen = rtw_get_rateset_len23a(pdev_network->SupportedRates);
 
 	if (rateLen > 8) {
-		ie = rtw_set_ie23a(ie, _SUPPORTEDRATES_IE_, 8,
+		ie = rtw_set_ie23a(ie, WLAN_EID_SUPP_RATES, 8,
 				pdev_network->SupportedRates, &sz);
 		/* ie = rtw_set_ie23a(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz); */
 	} else {
-		ie = rtw_set_ie23a(ie, _SUPPORTEDRATES_IE_, rateLen,
+		ie = rtw_set_ie23a(ie, WLAN_EID_SUPP_RATES, rateLen,
 				pdev_network->SupportedRates, &sz);
 	}
 
 	/* DS parameter set */
-	ie = rtw_set_ie23a(ie, _DSSET_IE_, 1,
-			   (u8 *)&pdev_network->Configuration.DSConfig, &sz);
+	ie = rtw_set_ie23a(ie, WLAN_EID_DS_PARAMS, 1,
+			   (u8 *)&pdev_network->DSConfig, &sz);
 
 	/* IBSS Parameter Set */
 
-	ie = rtw_set_ie23a(ie, _IBSS_PARA_IE_, 2,
-			   (u8 *)&pdev_network->Configuration.ATIMWindow, &sz);
+	ie = rtw_set_ie23a(ie, WLAN_EID_IBSS_PARAMS, 2,
+			   (u8 *)&pdev_network->ATIMWindow, &sz);
 
 	if (rateLen > 8) {
-		ie = rtw_set_ie23a(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8),
+		ie = rtw_set_ie23a(ie, WLAN_EID_EXT_SUPP_RATES, (rateLen - 8),
 				(pdev_network->SupportedRates + 8), &sz);
 	}
 
@@ -460,60 +436,7 @@ int rtw_generate_ie23a(struct registry_priv *pregistrypriv)
 	return sz;
 }
 
-unsigned char *rtw_get_wpa_ie23a(unsigned char *pie, int *wpa_ie_len, int limit)
-{
-	int len;
-	u16 val16;
-	unsigned char wpa_oui_type[] = {0x00, 0x50, 0xf2, 0x01};
-	u8 *pbuf = pie;
-	int limit_new = limit;
-
-	while(1) {
-		pbuf = rtw_get_ie23a(pbuf, _WPA_IE_ID_, &len, limit_new);
-
-		if (pbuf) {
-			/* check if oui matches... */
-			if (memcmp((pbuf + 2), wpa_oui_type,
-				   sizeof(wpa_oui_type))) {
-				goto check_next_ie;
-			}
-
-			/* check version... */
-			memcpy((u8 *)&val16, (pbuf + 6), sizeof(val16));
-
-			val16 = le16_to_cpu(val16);
-			if (val16 != 0x0001)
-				goto check_next_ie;
-
-			*wpa_ie_len = *(pbuf + 1);
-
-			return pbuf;
-		} else {
-			*wpa_ie_len = 0;
-			return NULL;
-		}
-
-check_next_ie:
-
-		limit_new = limit - (pbuf - pie) - 2 - len;
-
-		if (limit_new <= 0)
-			break;
-
-		pbuf += (2 + len);
-	}
-
-	*wpa_ie_len = 0;
-
-	return NULL;
-}
-
-unsigned char *rtw_get_wpa2_ie23a(unsigned char *pie, int *rsn_ie_len, int limit)
-{
-	return rtw_get_ie23a(pie, _WPA2_IE_ID_, rsn_ie_len, limit);
-}
-
-int rtw_get_wpa_cipher_suite23a(u8 *s)
+int rtw_get_wpa_cipher_suite23a(const u8 *s)
 {
 	if (!memcmp(s, WPA_CIPHER_SUITE_NONE23A, WPA_SELECTOR_LEN))
 		return WPA_CIPHER_NONE;
@@ -529,7 +452,7 @@ int rtw_get_wpa_cipher_suite23a(u8 *s)
 	return 0;
 }
 
-int rtw_get_wpa2_cipher_suite23a(u8 *s)
+int rtw_get_wpa2_cipher_suite23a(const u8 *s)
 {
 	if (!memcmp(s, RSN_CIPHER_SUITE_NONE23A, RSN_SELECTOR_LEN))
 		return WPA_CIPHER_NONE;
@@ -545,22 +468,19 @@ int rtw_get_wpa2_cipher_suite23a(u8 *s)
 	return 0;
 }
 
-int rtw_parse_wpa_ie23a(u8* wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
+int rtw_parse_wpa_ie23a(const u8* wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
 {
 	int i, ret = _SUCCESS;
 	int left, count;
-	u8 *pos;
-	u8 SUITE_1X[4] = {0x00, 0x50, 0xf2, 1};
+	const u8 *pos;
 
 	if (wpa_ie_len <= 0) {
 		/* No WPA IE - fail silently */
 		return _FAIL;
 	}
 
-	if ((*wpa_ie != _WPA_IE_ID_) || (*(wpa_ie+1) != (u8)(wpa_ie_len - 2)) ||
-	    memcmp(wpa_ie + 2, RTW_WPA_OUI23A_TYPE, WPA_SELECTOR_LEN)) {
+	if (wpa_ie[1] != (u8)(wpa_ie_len - 2))
 		return _FAIL;
-	}
 
 	pos = wpa_ie;
 
@@ -612,7 +532,7 @@ int rtw_parse_wpa_ie23a(u8* wpa_ie, int wpa_ie_len, int *group_cipher, int *pair
 	if (is_8021x) {
 		if (left >= 6) {
 			pos += 2;
-			if (!memcmp(pos, SUITE_1X, 4)) {
+			if (!memcmp(pos, RTW_WPA_OUI23A_TYPE, 4)) {
 				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
 					 ("%s : there has 802.1x auth\n",
 					  __func__));
@@ -624,12 +544,12 @@ int rtw_parse_wpa_ie23a(u8* wpa_ie, int wpa_ie_len, int *group_cipher, int *pair
 	return ret;
 }
 
-int rtw_parse_wpa2_ie23a(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;
 	int left, count;
-	u8 *pos;
+	const u8 *pos;
 	u8 SUITE_1X[4] = {0x00, 0x0f, 0xac, 0x01};
 
 	if (rsn_ie_len <= 0) {
@@ -637,7 +557,7 @@ int rtw_parse_wpa2_ie23a(u8* rsn_ie, int rsn_ie_len, int *group_cipher,
 		return _FAIL;
 	}
 
-	if ((*rsn_ie!= _WPA2_IE_ID_) || (*(rsn_ie+1) != (u8)(rsn_ie_len - 2))) {
+	if (*rsn_ie != _WPA2_IE_ID_ || *(rsn_ie+1) != (u8)(rsn_ie_len - 2)) {
 		return _FAIL;
 	}
 
@@ -705,11 +625,9 @@ int rtw_get_sec_ie23a(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len,
 		   u8 *wpa_ie, u16 *wpa_len)
 {
 	u8 authmode, sec_idx, i;
-	u8 wpa_oui[4] = {0x0, 0x50, 0xf2, 0x01};
 	uint cnt;
 
 
-
 	/* Search required WPA or WPA2 IE and copy to sec_ie[ ] */
 
 	cnt = (_TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_);
@@ -719,8 +637,8 @@ int rtw_get_sec_ie23a(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len,
 	while(cnt < in_len) {
 		authmode = in_ie[cnt];
 
-		if ((authmode == _WPA_IE_ID_) &&
-		    !memcmp(&in_ie[cnt+2], &wpa_oui[0], 4)) {
+		if ((authmode == WLAN_EID_VENDOR_SPECIFIC) &&
+		    !memcmp(&in_ie[cnt+2], RTW_WPA_OUI23A_TYPE, 4)) {
 				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
 					 ("\n rtw_get_wpa_ie23a: sec_idx =%d "
 					  "in_ie[cnt+1]+2 =%d\n",
@@ -778,24 +696,6 @@ int rtw_get_sec_ie23a(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len,
 	return *rsn_len + *wpa_len;
 }
 
-u8 rtw_is_wps_ie23a(u8 *ie_ptr, uint *wps_ielen)
-{
-	u8 match = false;
-	u8 eid, wps_oui[4]= {0x0, 0x50, 0xf2, 0x04};
-
-	if (!ie_ptr)
-		return match;
-
-	eid = ie_ptr[0];
-
-	if ((eid == _WPA_IE_ID_) && !memcmp(&ie_ptr[2], wps_oui, 4)) {
-		/* DBG_8723A("==> found WPS_IE.....\n"); */
-		*wps_ielen = ie_ptr[1] + 2;
-		match = true;
-	}
-	return match;
-}
-
 /**
  * rtw_get_wps_ie23a - Search WPS IE from a series of IEs
  * @in_ie: Address of IEs to search
@@ -824,7 +724,8 @@ u8 *rtw_get_wps_ie23a(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen)
 	while (cnt < in_len) {
 		eid = in_ie[cnt];
 
-		if ((eid == _WPA_IE_ID_) && !memcmp(&in_ie[cnt+2], wps_oui, 4)) {
+		if (eid == WLAN_EID_VENDOR_SPECIFIC &&
+		    !memcmp(&in_ie[cnt+2], wps_oui, 4)) {
 			wpsie_ptr = &in_ie[cnt];
 
 			if (wps_ie)
@@ -866,7 +767,7 @@ u8 *rtw_get_wps_attr23a(u8 *wps_ie, uint wps_ielen, u16 target_attr_id,
 	if (len_attr)
 		*len_attr = 0;
 
-	if ((wps_ie[0] != _VENDOR_SPECIFIC_IE_) ||
+	if (wps_ie[0] != WLAN_EID_VENDOR_SPECIFIC ||
 	    memcmp(wps_ie + 2, wps_oui, 4)) {
 		return attr_ptr;
 	}
@@ -937,664 +838,26 @@ u8 *rtw_get_wps_attr_content23a(u8 *wps_ie, uint wps_ielen, u16 target_attr_id,
 	return NULL;
 }
 
-static int
-rtw_ieee802_11_parse_vendor_specific(u8 *pos, uint elen,
-				     struct rtw_ieee802_11_elems *elems,
-				     int show_errors)
-{
-	unsigned int oui;
-
-	/* first 3 bytes in vendor specific information element are the IEEE
-	 * OUI of the vendor. The following byte is used a vendor specific
-	 * sub-type. */
-	if (elen < 4) {
-		if (show_errors) {
-			DBG_8723A("short vendor specific "
-				   "information element ignored (len =%lu)\n",
-				   (unsigned long) elen);
-		}
-		return -1;
-	}
-
-	oui = RTW_GET_BE24(pos);
-	switch (oui) {
-	case WLAN_OUI_MICROSOFT:
-		/* Microsoft/Wi-Fi information elements are further typed and
-		 * subtyped */
-		switch (pos[3]) {
-		case 1:
-			/* Microsoft OUI (00:50:F2) with OUI Type 1:
-			 * real WPA information element */
-			elems->wpa_ie = pos;
-			elems->wpa_ie_len = elen;
-			break;
-		case WME_OUI_TYPE: /* this is a Wi-Fi WME info. element */
-			if (elen < 5) {
-				DBG_8723A("short WME "
-					   "information element ignored "
-					   "(len =%lu)\n",
-					   (unsigned long) elen);
-				return -1;
-			}
-			switch (pos[4]) {
-			case WME_OUI_SUBTYPE_INFORMATION_ELEMENT:
-			case WME_OUI_SUBTYPE_PARAMETER_ELEMENT:
-				elems->wme = pos;
-				elems->wme_len = elen;
-				break;
-			case WME_OUI_SUBTYPE_TSPEC_ELEMENT:
-				elems->wme_tspec = pos;
-				elems->wme_tspec_len = elen;
-				break;
-			default:
-				DBG_8723A("unknown WME "
-					   "information element ignored "
-					   "(subtype =%d len =%lu)\n",
-					   pos[4], (unsigned long) elen);
-				return -1;
-			}
-			break;
-		case 4:
-			/* Wi-Fi Protected Setup (WPS) IE */
-			elems->wps_ie = pos;
-			elems->wps_ie_len = elen;
-			break;
-		default:
-			DBG_8723A("Unknown Microsoft "
-				   "information element ignored "
-				   "(type =%d len =%lu)\n",
-				   pos[3], (unsigned long) elen);
-			return -1;
-		}
-		break;
-
-	case OUI_BROADCOM:
-		switch (pos[3]) {
-		case VENDOR_HT_CAPAB_OUI_TYPE:
-			elems->vendor_ht_cap = pos;
-			elems->vendor_ht_cap_len = elen;
-			break;
-		default:
-			DBG_8723A("Unknown Broadcom "
-				   "information element ignored "
-				   "(type =%d len =%lu)\n",
-				   pos[3], (unsigned long) elen);
-			return -1;
-		}
-		break;
-
-	default:
-		DBG_8723A("unknown vendor specific information "
-			   "element ignored (vendor OUI %02x:%02x:%02x "
-			   "len =%lu)\n",
-			   pos[0], pos[1], pos[2], (unsigned long) elen);
-		return -1;
-	}
-
-	return 0;
-}
-
-/**
- * ieee802_11_parse_elems - Parse information elements in management frames
- * @start: Pointer to the start of IEs
- * @len: Length of IE buffer in octets
- * @elems: Data structure for parsed elements
- * @show_errors: Whether to show parsing errors in debug log
- * Returns: Parsing result
- */
-enum parse_res rtw_ieee802_11_parse_elems23a(u8 *start, uint len,
-				struct rtw_ieee802_11_elems *elems,
-				int show_errors)
-{
-	uint left = len;
-	u8 *pos = start;
-	int unknown = 0;
-
-	memset(elems, 0, sizeof(*elems));
-
-	while (left >= 2) {
-		u8 id, elen;
-
-		id = *pos++;
-		elen = *pos++;
-		left -= 2;
-
-		if (elen > left) {
-			if (show_errors) {
-				DBG_8723A("IEEE 802.11 element "
-					   "parse failed (id =%d elen =%d "
-					   "left =%lu)\n",
-					   id, elen, (unsigned long) left);
-			}
-			return ParseFailed;
-		}
-
-		switch (id) {
-		case WLAN_EID_SSID:
-			elems->ssid = pos;
-			elems->ssid_len = elen;
-			break;
-		case WLAN_EID_SUPP_RATES:
-			elems->supp_rates = pos;
-			elems->supp_rates_len = elen;
-			break;
-		case WLAN_EID_FH_PARAMS:
-			elems->fh_params = pos;
-			elems->fh_params_len = elen;
-			break;
-		case WLAN_EID_DS_PARAMS:
-			elems->ds_params = pos;
-			elems->ds_params_len = elen;
-			break;
-		case WLAN_EID_CF_PARAMS:
-			elems->cf_params = pos;
-			elems->cf_params_len = elen;
-			break;
-		case WLAN_EID_TIM:
-			elems->tim = pos;
-			elems->tim_len = elen;
-			break;
-		case WLAN_EID_IBSS_PARAMS:
-			elems->ibss_params = pos;
-			elems->ibss_params_len = elen;
-			break;
-		case WLAN_EID_CHALLENGE:
-			elems->challenge = pos;
-			elems->challenge_len = elen;
-			break;
-		case WLAN_EID_ERP_INFO:
-			elems->erp_info = pos;
-			elems->erp_info_len = elen;
-			break;
-		case WLAN_EID_EXT_SUPP_RATES:
-			elems->ext_supp_rates = pos;
-			elems->ext_supp_rates_len = elen;
-			break;
-		case WLAN_EID_VENDOR_SPECIFIC:
-			if (rtw_ieee802_11_parse_vendor_specific(pos, elen,
-								 elems,
-								 show_errors))
-				unknown++;
-			break;
-		case WLAN_EID_RSN:
-			elems->rsn_ie = pos;
-			elems->rsn_ie_len = elen;
-			break;
-		case WLAN_EID_PWR_CAPABILITY:
-			elems->power_cap = pos;
-			elems->power_cap_len = elen;
-			break;
-		case WLAN_EID_SUPPORTED_CHANNELS:
-			elems->supp_channels = pos;
-			elems->supp_channels_len = elen;
-			break;
-		case WLAN_EID_MOBILITY_DOMAIN:
-			elems->mdie = pos;
-			elems->mdie_len = elen;
-			break;
-		case WLAN_EID_FAST_BSS_TRANSITION:
-			elems->ftie = pos;
-			elems->ftie_len = elen;
-			break;
-		case WLAN_EID_TIMEOUT_INTERVAL:
-			elems->timeout_int = pos;
-			elems->timeout_int_len = elen;
-			break;
-		case WLAN_EID_HT_CAPABILITY:
-			elems->ht_capabilities = pos;
-			elems->ht_capabilities_len = elen;
-			break;
-		case WLAN_EID_HT_OPERATION:
-			elems->ht_operation = pos;
-			elems->ht_operation_len = elen;
-			break;
-		default:
-			unknown++;
-			if (!show_errors)
-				break;
-			DBG_8723A("IEEE 802.11 element parse "
-				   "ignored unknown element (id =%d elen =%d)\n",
-				   id, elen);
-			break;
-		}
-
-		left -= elen;
-		pos += elen;
-	}
-
-	if (left)
-		return ParseFailed;
-
-	return unknown ? ParseUnknown : ParseOK;
-}
-
-static u8 key_char2num(u8 ch)
-{
-	if ((ch >= '0') && (ch <= '9'))
-		return ch - '0';
-	else if ((ch >= 'a') && (ch <= 'f'))
-		return ch - 'a' + 10;
-	else if ((ch >= 'A') && (ch <= 'F'))
-		return ch - 'A' + 10;
-	else
-		return 0xff;
-}
-
-u8 str_2char2num23a(u8 hch, u8 lch)
-{
-	return (key_char2num(hch) * 10) + key_char2num(lch);
-}
-
-u8 key_2char2num23a(u8 hch, u8 lch)
-{
-	return (key_char2num(hch) << 4) | key_char2num(lch);
-}
-
-void rtw_macaddr_cfg23a(u8 *mac_addr)
-{
-	u8 mac[ETH_ALEN];
-	if (!mac_addr)
-		return;
-
-	memcpy(mac, mac_addr, ETH_ALEN);
-
-	if (is_broadcast_ether_addr(mac) || is_zero_ether_addr(mac)) {
-		mac[0] = 0x00;
-		mac[1] = 0xe0;
-		mac[2] = 0x4c;
-		mac[3] = 0x87;
-		mac[4] = 0x00;
-		mac[5] = 0x00;
-		/*  use default mac addresss */
-		memcpy(mac_addr, mac, ETH_ALEN);
-		DBG_8723A("MAC Address from efuse error, assign default "
-			  "one !!!\n");
-	}
-	DBG_8723A("rtw_macaddr_cfg23a MAC Address  = "MAC_FMT"\n",
-		  MAC_ARG(mac_addr));
-}
-
-void dump_ies23a(u8 *buf, u32 buf_len) {
-	u8* pos = (u8*)buf;
-	u8 id, len;
-
-	while (pos-buf <= buf_len) {
-		id = *pos;
-		len = *(pos + 1);
-
-		DBG_8723A("%s ID:%u, LEN:%u\n", __func__, id, len);
-#ifdef CONFIG_8723AU_P2P
-		dump_p2p_ie23a(pos, len);
-#endif
-		dump_wps_ie23a(pos, len);
-
-		pos += (2 + len);
-	}
-}
-
-void dump_wps_ie23a(u8 *ie, u32 ie_len) {
-	u8* pos = (u8*)ie;
-	u16 id;
-	u16 len;
-
-	u8 *wps_ie;
-	uint wps_ielen;
-
-	wps_ie = rtw_get_wps_ie23a(ie, ie_len, NULL, &wps_ielen);
-	if (wps_ie != ie || wps_ielen == 0)
-		return;
-
-	pos+= 6;
-	while (pos-ie < ie_len) {
-		id = get_unaligned_be16(pos);
-		len = get_unaligned_be16(pos + 2);
-
-		DBG_8723A("%s ID:0x%04x, LEN:%u\n", __func__, id, len);
-
-		pos += (4 + len);
-	}
-}
-
-#ifdef CONFIG_8723AU_P2P
-void dump_p2p_ie23a(u8 *ie, u32 ie_len) {
-	u8* pos = (u8*)ie;
-	u8 id;
-	u16 len;
-
-	u8 *p2p_ie;
-	uint p2p_ielen;
-
-	p2p_ie = rtw_get_p2p_ie23a(ie, ie_len, NULL, &p2p_ielen);
-	if (p2p_ie != ie || p2p_ielen == 0)
-		return;
-
-	pos += 6;
-	while (pos-ie < ie_len) {
-		id = *pos;
-		len = get_unaligned_le16(pos+1);
-
-		DBG_8723A("%s ID:%u, LEN:%u\n", __func__, id, len);
-
-		pos+= (3+len);
-	}
-}
-
-/**
- * rtw_get_p2p_ie23a - Search P2P IE from a series of IEs
- * @in_ie: Address of IEs to search
- * @in_len: Length limit from in_ie
- * @p2p_ie: If not NULL and P2P IE is found, P2P IE will be copied to the
- *          buf starting from p2p_ie
- * @p2p_ielen: If not NULL and P2P IE is found, will set to the length of
- *             the entire P2P IE
- *
- * Returns: The address of the P2P IE found, or NULL
- */
-u8 *rtw_get_p2p_ie23a(u8 *in_ie, int in_len, u8 *p2p_ie, uint *p2p_ielen)
-{
-	uint cnt = 0;
-	u8 *p2p_ie_ptr;
-	u8 eid, p2p_oui[4]={0x50, 0x6F, 0x9A, 0x09};
-
-	if (p2p_ielen)
-		*p2p_ielen = 0;
-
-	while (cnt<in_len) {
-		eid = in_ie[cnt];
-		if ((in_len < 0) || (cnt > MAX_IE_SZ)) {
-			dump_stack();
-			return NULL;
-		}
-		if ((eid == _VENDOR_SPECIFIC_IE_) &&
-		    !memcmp(&in_ie[cnt + 2], p2p_oui, 4)) {
-			p2p_ie_ptr = in_ie + cnt;
-
-			if (p2p_ie != NULL) {
-				memcpy(p2p_ie, &in_ie[cnt],
-				       in_ie[cnt + 1] + 2);
-			}
-
-			if (p2p_ielen != NULL) {
-				*p2p_ielen = in_ie[cnt + 1] + 2;
-			}
-
-			return p2p_ie_ptr;
-
-			break;
-		} else {
-			cnt += in_ie[cnt + 1] + 2; /* goto next */
-		}
-	}
-
-	return NULL;
-}
-
-/**
- * rtw_get_p2p_attr23a - Search a specific P2P attribute from a given P2P IE
- * @p2p_ie: Address of P2P IE to search
- * @p2p_ielen: Length limit from p2p_ie
- * @target_attr_id: The attribute ID of P2P attribute to search
- * @buf_attr: If not NULL and the P2P attribute is found, P2P attribute will
- *            be copied to the buf starting from buf_attr
- * @len_attr: If not NULL and the P2P attribute is found, will set to the
- *            length of the entire P2P attribute
- *
- * Returns: the address of the specific WPS attribute found, or NULL
- */
-u8 *rtw_get_p2p_attr23a(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id,
-		     u8 *buf_attr, u32 *len_attr)
-{
-	u8 *attr_ptr = NULL;
-	u8 *target_attr_ptr = NULL;
-	u8 p2p_oui[4]={0x50, 0x6F, 0x9A, 0x09};
-
-	if (len_attr)
-		*len_attr = 0;
-
-	if (!p2p_ie || (p2p_ie[0] != _VENDOR_SPECIFIC_IE_) ||
-	    memcmp(p2p_ie + 2, p2p_oui, 4)) {
-		return attr_ptr;
-	}
-
-	/*  6 = 1(Element ID) + 1(Length) + 3 (OUI) + 1(OUI Type) */
-	attr_ptr = p2p_ie + 6; /* goto first attr */
-
-	while (attr_ptr - p2p_ie < p2p_ielen) {
-		/*  3 = 1(Attribute ID) + 2(Length) */
-		u8 attr_id = *attr_ptr;
-		u16 attr_data_len = get_unaligned_le16(attr_ptr + 1);
-		u16 attr_len = attr_data_len + 3;
-
-		/* DBG_8723A("%s attr_ptr:%p, id:%u, length:%u\n", __func__, attr_ptr, attr_id, attr_data_len); */
-		if (attr_id == target_attr_id) {
-			target_attr_ptr = attr_ptr;
-
-			if (buf_attr)
-				memcpy(buf_attr, attr_ptr, attr_len);
-
-			if (len_attr)
-				*len_attr = attr_len;
-
-			break;
-		} else {
-			attr_ptr += attr_len; /* goto next */
-		}
-	}
-
-	return target_attr_ptr;
-}
-
-/**
- * rtw_get_p2p_attr23a_content - Search a specific P2P attribute content from
- * a given P2P IE
- * @p2p_ie: Address of P2P IE to search
- * @p2p_ielen: Length limit from p2p_ie
- * @target_attr_id: The attribute ID of P2P attribute to search
- * @buf_content: If not NULL and the P2P attribute is found, P2P attribute
- *               content will be copied to the buf starting from buf_content
- * @len_content: If not NULL and the P2P attribute is found, will set to the
- *               length of the P2P attribute content
- *
- * Returns: the address of the specific P2P attribute content found, or NULL
- */
-u8 *rtw_get_p2p_attr23a_content(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id,
-			     u8 *buf_content, uint *len_content)
-{
-	u8 *attr_ptr;
-	u32 attr_len;
-
-	if (len_content)
-		*len_content = 0;
-
-	attr_ptr = rtw_get_p2p_attr23a(p2p_ie, p2p_ielen, target_attr_id,
-				    NULL, &attr_len);
-
-	if (attr_ptr && attr_len) {
-		if (buf_content)
-			memcpy(buf_content, attr_ptr + 3, attr_len - 3);
-
-		if (len_content)
-			*len_content = attr_len - 3;
-
-		return attr_ptr+3;
-	}
-
-	return NULL;
-}
-
-u32 rtw_set_p2p_attr_content23a(u8 *pbuf, u8 attr_id, u16 attr_len, u8 *pdata_attr)
-{
-	u32 a_len;
-
-	*pbuf = attr_id;
-
-	/* u16*)(pbuf + 1) = cpu_to_le16(attr_len); */
-	put_unaligned_le16(attr_len, pbuf + 1);
-
-	if (pdata_attr)
-		memcpy(pbuf + 3, pdata_attr, attr_len);
-
-	a_len = attr_len + 3;
-
-	return a_len;
-}
-
-static uint rtw_p2p_attr_remove(u8 *ie, uint ielen_ori, u8 attr_id)
-{
-	u8 *target_attr;
-	u32 target_attr_len;
-	uint ielen = ielen_ori;
-
-	while(1) {
-		target_attr = rtw_get_p2p_attr23a(ie, ielen, attr_id, NULL,
-					     &target_attr_len);
-		if (target_attr && target_attr_len) {
-			u8 *next_attr = target_attr+target_attr_len;
-			uint remain_len = ielen-(next_attr-ie);
-			/* dump_ies23a(ie, ielen); */
-
-			memset(target_attr, 0, target_attr_len);
-			memcpy(target_attr, next_attr, remain_len);
-			memset(target_attr+remain_len, 0, target_attr_len);
-			*(ie + 1) -= target_attr_len;
-			ielen -= target_attr_len;
-		} else {
-			/* if (index>0) */
-			/*	dump_ies23a(ie, ielen); */
-			break;
-		}
-	}
-
-	return ielen;
-}
-
-void rtw_wlan_bssid_ex_remove_p2p_attr23a(struct wlan_bssid_ex *bss_ex, u8 attr_id)
-{
-	u8 *p2p_ie;
-	uint p2p_ielen, p2p_ielen_ori;
-
-	if ((p2p_ie = rtw_get_p2p_ie23a(bss_ex->IEs + _FIXED_IE_LENGTH_,
-				     bss_ex->IELength - _FIXED_IE_LENGTH_,
-				     NULL, &p2p_ielen_ori))) {
-		p2p_ielen = rtw_p2p_attr_remove(p2p_ie, p2p_ielen_ori, attr_id);
-		if (p2p_ielen != p2p_ielen_ori) {
-			u8 *next_ie_ori = p2p_ie+p2p_ielen_ori;
-			u8 *next_ie = p2p_ie+p2p_ielen;
-			uint remain_len;
-			remain_len = bss_ex->IELength-(next_ie_ori-bss_ex->IEs);
-
-			memcpy(next_ie, next_ie_ori, remain_len);
-			memset(next_ie+remain_len, 0, p2p_ielen_ori-p2p_ielen);
-			bss_ex->IELength -= p2p_ielen_ori-p2p_ielen;
-		}
-	}
-}
-
-#endif /* CONFIG_8723AU_P2P */
-
-#ifdef CONFIG_8723AU_P2P
-int rtw_get_wfd_ie(u8 *in_ie, int in_len, u8 *wfd_ie, uint *wfd_ielen)
-{
-	int match;
-	const u8 *ie;
-
-	match = 0;
-
-	if (in_len < 0)
-		return match;
-
-	ie = cfg80211_find_vendor_ie(0x506F9A, 0x0A, in_ie, in_len);
-	if (ie && (ie[1] <= (MAX_WFD_IE_LEN - 2))) {
-		if (wfd_ie) {
-			*wfd_ielen = ie[1] + 2;
-			memcpy(wfd_ie, ie, ie[1] + 2);
-		} else
-			if (wfd_ielen)
-				*wfd_ielen = 0;
-
-		match = 1;
-	}
-
-	return match;
-}
-
-/*	attr_content: The output buffer, contains the "body field" of
-	WFD attribute. */
-/*	attr_contentlen: The data length of the "body field" of WFD
-	attribute. */
-int rtw_get_wfd_attr_content(u8 *wfd_ie, uint wfd_ielen, u8 target_attr_id,
-			     u8 *attr_content, uint *attr_contentlen)
-{
-	int match;
-	uint cnt = 0;
-	u8 attr_id, wfd_oui[4] = {0x50, 0x6F, 0x9A, 0x0A};
-
-	match = false;
-
-	if ((wfd_ie[0] != _VENDOR_SPECIFIC_IE_) ||
-	    memcmp(wfd_ie + 2, wfd_oui, 4)) {
-		return match;
-	}
-
-	/*	1 (WFD IE) + 1 (Length) + 3 (OUI) + 1 (OUI Type) */
-	cnt = 6;
-	while (cnt < wfd_ielen) {
-		u16 attrlen = get_unaligned_be16(wfd_ie + cnt + 1);
-
-		attr_id = wfd_ie[cnt];
-		if (attr_id == target_attr_id) {
-			/*	3 -> 1 byte for attribute ID field, 2
-				bytes for length field */
-			if (attr_content)
-				memcpy(attr_content, &wfd_ie[cnt + 3], attrlen);
-
-			if (attr_contentlen)
-				*attr_contentlen = attrlen;
-
-			cnt += attrlen + 3;
-
-			match = true;
-			break;
-		} else {
-			cnt += attrlen + 3; /* goto next */
-		}
-	}
-
-	return match;
-}
-#endif /*  CONFIG_8723AU_P2P */
-
-/* Baron adds to avoid FreeBSD warning */
-int ieee80211_is_empty_essid23a(const char *essid, int essid_len)
-{
-	/* Single white space is for Linksys APs */
-	if (essid_len == 1 && essid[0] == ' ')
-		return 1;
-
-	/* Otherwise, if the entire essid is 0, we assume it is hidden */
-	while (essid_len) {
-		essid_len--;
-		if (essid[essid_len] != '\0')
-			return 0;
-	}
-
-	return 1;
-}
-
 static int rtw_get_cipher_info(struct wlan_network *pnetwork)
 {
-	u32 wpa_ielen;
-	unsigned char *pbuf;
+	const u8 *pbuf;
 	int group_cipher = 0, pairwise_cipher = 0, is8021x = 0;
 	int ret = _FAIL;
-	int r;
-	pbuf = rtw_get_wpa_ie23a(&pnetwork->network.IEs[12], &wpa_ielen,
-			      pnetwork->network.IELength - 12);
+	int r, offset, plen;
+	char *pie;
+
+	offset = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+		offsetof(struct ieee80211_mgmt, u);
+	pie = &pnetwork->network.IEs[offset];
+	plen = pnetwork->network.IELength - offset;
+
+	pbuf = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
+				       WLAN_OUI_TYPE_MICROSOFT_WPA, pie, plen);
 
-	if (pbuf && (wpa_ielen > 0)) {
+	if (pbuf && pbuf[1] > 0) {
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-			 ("rtw_get_cipher_info: wpa_ielen: %d", wpa_ielen));
-		r = rtw_parse_wpa_ie23a(pbuf, wpa_ielen + 2, &group_cipher,
+			 ("rtw_get_cipher_info: wpa_ielen: %d", pbuf[1]));
+		r = rtw_parse_wpa_ie23a(pbuf, pbuf[1] + 2, &group_cipher,
 				     &pairwise_cipher, &is8021x);
 		if (r == _SUCCESS) {
 			pnetwork->BcnInfo.pairwise_cipher = pairwise_cipher;
@@ -1608,13 +871,12 @@ static int rtw_get_cipher_info(struct wlan_network *pnetwork)
 			ret = _SUCCESS;
 		}
 	} else {
-		pbuf = rtw_get_wpa2_ie23a(&pnetwork->network.IEs[12], &wpa_ielen,
-				       pnetwork->network.IELength - 12);
+		pbuf = cfg80211_find_ie(WLAN_EID_RSN, pie, plen);
 
-		if (pbuf && (wpa_ielen > 0)) {
+		if (pbuf && pbuf[1] > 0) {
 			RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
 				 ("get RSN IE\n"));
-			r = rtw_parse_wpa2_ie23a(pbuf, wpa_ielen + 2,
+			r = rtw_parse_wpa2_ie23a(pbuf, pbuf[1] + 2,
 					      &group_cipher, &pairwise_cipher,
 					      &is8021x);
 			if (r == _SUCCESS) {
@@ -1641,23 +903,22 @@ static int rtw_get_cipher_info(struct wlan_network *pnetwork)
 
 void rtw_get_bcn_info23a(struct wlan_network *pnetwork)
 {
-	unsigned short cap = 0;
+	unsigned short cap;
 	u8 bencrypt = 0;
 	/* u8 wpa_ie[255], rsn_ie[255]; */
 	u16 wpa_len = 0, rsn_len = 0;
-	struct HT_info_element *pht_info = NULL;
-	struct ieee80211_ht_cap *pht_cap = NULL;
-	unsigned int		len;
-	unsigned char		*p;
+	struct HT_info_element *pht_info;
+	struct ieee80211_ht_cap *pht_cap;
+	const u8 *p;
 
-	memcpy(&cap, rtw_get_capability23a_from_ie(pnetwork->network.IEs), 2);
-	cap = le16_to_cpu(cap);
+	cap = get_unaligned_le16(
+		rtw_get_capability23a_from_ie(pnetwork->network.IEs));
 	if (cap & WLAN_CAPABILITY_PRIVACY) {
 		bencrypt = 1;
 		pnetwork->network.Privacy = 1;
-	} else {
+	} else
 		pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_OPENSYS;
-	}
+
 	rtw_get_sec_ie23a(pnetwork->network.IEs, pnetwork->network.IELength,
 		       NULL, &rsn_len, NULL, &wpa_len);
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
@@ -1671,11 +932,11 @@ void rtw_get_bcn_info23a(struct wlan_network *pnetwork)
 		 ("rtw_get_bcn_info23a: wpa_len =%d rsn_len =%d\n",
 		  wpa_len, rsn_len));
 
-	if (rsn_len > 0) {
+	if (rsn_len > 0)
 		pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA2;
-	} else if (wpa_len > 0) {
+	else if (wpa_len > 0)
 		pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA;
-	} else {
+	else {
 		if (bencrypt)
 			pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WEP;
 	}
@@ -1689,25 +950,24 @@ void rtw_get_bcn_info23a(struct wlan_network *pnetwork)
 
 	/* get bwmode and ch_offset */
 	/* parsing HT_CAP_IE */
-	p = rtw_get_ie23a(pnetwork->network.IEs + _FIXED_IE_LENGTH_,
-		       _HT_CAPABILITY_IE_, &len,
-		       pnetwork->network.IELength - _FIXED_IE_LENGTH_);
-	if (p && len > 0) {
+	p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY,
+			     pnetwork->network.IEs + _FIXED_IE_LENGTH_,
+			     pnetwork->network.IELength - _FIXED_IE_LENGTH_);
+	if (p && p[1] > 0) {
 		pht_cap = (struct ieee80211_ht_cap *)(p + 2);
 		pnetwork->BcnInfo.ht_cap_info = pht_cap->cap_info;
-	} else {
+	} else
 		pnetwork->BcnInfo.ht_cap_info = 0;
-	}
+
 	/* parsing HT_INFO_IE */
-	p = rtw_get_ie23a(pnetwork->network.IEs + _FIXED_IE_LENGTH_,
-		       _HT_ADD_INFO_IE_, &len,
+	p = cfg80211_find_ie(WLAN_EID_HT_OPERATION,
+			     pnetwork->network.IEs + _FIXED_IE_LENGTH_,
 		       pnetwork->network.IELength - _FIXED_IE_LENGTH_);
-	if (p && len > 0) {
+	if (p && p[1] > 0) {
 		pht_info = (struct HT_info_element *)(p + 2);
 		pnetwork->BcnInfo.ht_info_infos_0 = pht_info->infos[0];
-	} else {
+	} else
 		pnetwork->BcnInfo.ht_info_infos_0 = 0;
-	}
 }
 
 /* show MCS rate, unit: 100Kbps */
@@ -1781,37 +1041,6 @@ u16 rtw_mcs_rate23a(u8 rf_type, u8 bw_40MHz, u8 short_GI_20, u8 short_GI_40,
 	return max_rate;
 }
 
-int rtw_action_frame_parse23a(const u8 *frame, u32 frame_len, u8* category,
-			   u8 *action)
-{
-	const u8 *frame_body = frame + sizeof(struct ieee80211_hdr_3addr);
-	u16 fc;
-	u8 c, a = 0;
-
-	fc = le16_to_cpu(((struct ieee80211_hdr_3addr *)frame)->frame_control);
-
-	if ((fc & (IEEE80211_FCTL_FTYPE|IEEE80211_FCTL_STYPE)) !=
-	    (IEEE80211_FTYPE_MGMT|IEEE80211_STYPE_ACTION)) {
-		return false;
-	}
-
-	c = frame_body[0];
-
-	switch (c) {
-	case WLAN_CATEGORY_VENDOR_SPECIFIC: /* vendor-specific */
-		break;
-	default:
-		a = frame_body[1];
-	}
-
-	if (category)
-		*category = c;
-	if (action)
-		*action = a;
-
-	return true;
-}
-
 static const char *_action_public_str23a[] = {
 	"ACT_PUB_BSSCOEXIST",
 	"ACT_PUB_DSE_ENABLE",
diff --git a/drivers/staging/rtl8723au/core/rtw_io.c b/drivers/staging/rtl8723au/core/rtw_io.c
deleted file mode 100644
index 1cae8d7659b..00000000000
--- a/drivers/staging/rtl8723au/core/rtw_io.c
+++ /dev/null
@@ -1,266 +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.
- *
- ******************************************************************************/
-/*
-
-The purpose of rtw_io.c
-
-a. provides the API
-
-b. provides the protocol engine
-
-c. provides the software interface between caller and the hardware interface
-
-Compiler Flag Option:
-
-1. For USB:
-   a. USE_ASYNC_IRP: Both sync/async operations are provided.
-
-Only sync read/rtw_write_mem operations are provided.
-
-jackson@realtek.com.tw
-
-*/
-
-#define _RTW_IO_C_
-#include <osdep_service.h>
-#include <drv_types.h>
-#include <rtw_io.h>
-#include <osdep_intf.h>
-
-#include <usb_ops.h>
-
-u8 _rtw_read823a(struct rtw_adapter *adapter, u32 addr)
-{
-	u8 r_val;
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-
-	r_val = pintfhdl->io_ops._read8(pintfhdl, addr);
-
-	return r_val;
-}
-
-u16 _rtw_read1623a(struct rtw_adapter *adapter, u32 addr)
-{
-	u16 r_val;
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-
-	r_val = pintfhdl->io_ops._read16(pintfhdl, addr);
-
-	return le16_to_cpu(r_val);
-}
-
-u32 _rtw_read3223a(struct rtw_adapter *adapter, u32 addr)
-{
-	u32 r_val;
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-
-	r_val = pintfhdl->io_ops._read32(pintfhdl, addr);
-
-	return le32_to_cpu(r_val);
-}
-
-int _rtw_write823a(struct rtw_adapter *adapter, u32 addr, u8 val)
-{
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl		*pintfhdl = &pio_priv->intf;
-	int ret;
-
-	ret = pintfhdl->io_ops._write8(pintfhdl, addr, val);
-
-	return RTW_STATUS_CODE23a(ret);
-}
-
-int _rtw_write1623a(struct rtw_adapter *adapter, u32 addr, u16 val)
-{
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl		*pintfhdl = &pio_priv->intf;
-	int ret;
-
-	val = cpu_to_le16(val);
-	ret = pintfhdl->io_ops._write16(pintfhdl, addr, val);
-
-	return RTW_STATUS_CODE23a(ret);
-}
-int _rtw_write3223a(struct rtw_adapter *adapter, u32 addr, u32 val)
-{
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-	int ret;
-
-	val = cpu_to_le32(val);
-	ret = pintfhdl->io_ops._write32(pintfhdl, addr, val);
-
-	return RTW_STATUS_CODE23a(ret);
-}
-
-int _rtw_writeN23a(struct rtw_adapter *adapter, u32 addr , u32 length , u8 *pdata)
-{
-	struct io_priv *pio_priv = &adapter->iopriv;
-        struct intf_hdl *pintfhdl = (struct intf_hdl*)&pio_priv->intf;
-	int ret;
-
-	ret = pintfhdl->io_ops._writeN(pintfhdl, addr, length, pdata);
-
-	return RTW_STATUS_CODE23a(ret);
-}
-int _rtw_write823a_async23a(struct rtw_adapter *adapter, u32 addr, u8 val)
-{
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-	int ret;
-
-	ret = pintfhdl->io_ops._write8_async(pintfhdl, addr, val);
-
-	return RTW_STATUS_CODE23a(ret);
-}
-int _rtw_write1623a_async(struct rtw_adapter *adapter, u32 addr, u16 val)
-{
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-	int ret;
-
-	val = cpu_to_le16(val);
-	ret = pintfhdl->io_ops._write16_async(pintfhdl, addr, val);
-
-	return RTW_STATUS_CODE23a(ret);
-}
-int _rtw_write3223a_async23a(struct rtw_adapter *adapter, u32 addr, u32 val)
-{
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-	int ret;
-
-	val = cpu_to_le32(val);
-	ret = pintfhdl->io_ops._write32_async(pintfhdl, addr, val);
-
-	return RTW_STATUS_CODE23a(ret);
-}
-
-void _rtw_read_mem23a(struct rtw_adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
-{
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-
-	if ((adapter->bDriverStopped == true) ||
-	    (adapter->bSurpriseRemoved == true)) {
-	     RT_TRACE(_module_rtl871x_io_c_, _drv_info_,
-		      ("rtw_read_mem:bDriverStopped(%d) OR "
-		       "bSurpriseRemoved(%d)", adapter->bDriverStopped,
-		       adapter->bSurpriseRemoved));
-	     return;
-	}
-
-	pintfhdl->io_ops._read_mem(pintfhdl, addr, cnt, pmem);
-}
-
-void _rtw_write_mem23a(struct rtw_adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
-{
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-
-	pintfhdl->io_ops._write_mem(pintfhdl, addr, cnt, pmem);
-}
-
-void _rtw_read_port23a(struct rtw_adapter *adapter, u32 addr, u32 cnt,
-		    struct recv_buf *rbuf)
-{
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-
-	if ((adapter->bDriverStopped == true) ||
-	    (adapter->bSurpriseRemoved == true)) {
-	     RT_TRACE(_module_rtl871x_io_c_, _drv_info_,
-		      ("rtw_read_port:bDriverStopped(%d) OR "
-		       "bSurpriseRemoved(%d)", adapter->bDriverStopped,
-		       adapter->bSurpriseRemoved));
-	     return;
-	}
-
-	pintfhdl->io_ops._read_port(pintfhdl, addr, cnt, rbuf);
-}
-
-void _rtw_read_port23a_cancel(struct rtw_adapter *adapter)
-{
-	void (*_read_port_cancel)(struct intf_hdl *pintfhdl);
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-
-	_read_port_cancel = pintfhdl->io_ops._read_port_cancel;
-
-	if (_read_port_cancel)
-		_read_port_cancel(pintfhdl);
-}
-
-u32 _rtw_write_port23a(struct rtw_adapter *adapter, u32 addr, u32 cnt,
-		    struct xmit_buf *xbuf)
-{
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-	u32 ret = _SUCCESS;
-
-	ret = pintfhdl->io_ops._write_port(pintfhdl, addr, cnt, xbuf);
-
-	return ret;
-}
-
-u32 _rtw_write_port23a_and_wait23a(struct rtw_adapter *adapter, u32 addr, u32 cnt,
-			     struct xmit_buf *pxmitbuf, int timeout_ms)
-{
-	int ret = _SUCCESS;
-	struct submit_ctx sctx;
-
-	rtw_sctx_init23a(&sctx, timeout_ms);
-	pxmitbuf->sctx = &sctx;
-
-	ret = _rtw_write_port23a(adapter, addr, cnt, pxmitbuf);
-
-	if (ret == _SUCCESS)
-		ret = rtw_sctx_wait23a(&sctx);
-
-	return ret;
-}
-
-void _rtw_write_port23a_cancel(struct rtw_adapter *adapter)
-{
-	void (*_write_port_cancel)(struct intf_hdl *pintfhdl);
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-
-	_write_port_cancel = pintfhdl->io_ops._write_port_cancel;
-
-	if (_write_port_cancel)
-		_write_port_cancel(pintfhdl);
-}
-
-int rtw_init_io_priv23a(struct rtw_adapter *padapter,
-		     void (*set_intf_ops)(struct _io_ops *pops))
-{
-	struct io_priv	*piopriv = &padapter->iopriv;
-	struct intf_hdl *pintf = &piopriv->intf;
-
-	if (set_intf_ops == NULL)
-		return _FAIL;
-
-	piopriv->padapter = padapter;
-	pintf->padapter = padapter;
-	pintf->pintf_dev = adapter_to_dvobj(padapter);
-
-	set_intf_ops(&pintf->io_ops);
-
-	return _SUCCESS;
-}
diff --git a/drivers/staging/rtl8723au/core/rtw_ioctl_set.c b/drivers/staging/rtl8723au/core/rtw_ioctl_set.c
index 30d7185e563..cf897c723f4 100644
--- a/drivers/staging/rtl8723au/core/rtw_ioctl_set.c
+++ b/drivers/staging/rtl8723au/core/rtw_ioctl_set.c
@@ -19,17 +19,16 @@
 #include <rtw_ioctl_set.h>
 #include <hal_intf.h>
 
-#include <usb_osintf.h>
 #include <usb_ops.h>
 #include <linux/ieee80211.h>
 
-u8 rtw_do_join23a(struct rtw_adapter *padapter)
+int rtw_do_join23a(struct rtw_adapter *padapter)
 {
 	struct list_head *plist, *phead;
 	u8* pibss = NULL;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct rtw_queue *queue = &pmlmepriv->scanned_queue;
-	u8 ret = _SUCCESS;
+	int ret = _SUCCESS;
 
 	spin_lock_bh(&pmlmepriv->scanned_queue.lock);
 	phead = get_list_head(queue);
@@ -45,7 +44,7 @@ u8 rtw_do_join23a(struct rtw_adapter *padapter)
 
 	pmlmepriv->to_join = true;
 
-	if (_rtw_queue_empty23a(queue) == true) {
+	if (list_empty(&queue->queue)) {
 		spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 		_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
 
@@ -54,7 +53,7 @@ u8 rtw_do_join23a(struct rtw_adapter *padapter)
 		/* we try to issue sitesurvey firstly */
 
 		if (pmlmepriv->LinkDetectInfo.bBusyTraffic == false ||
-		    rtw_to_roaming(padapter) > 0) {
+		    padapter->mlmepriv.to_roaming > 0) {
 			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
 				 ("rtw_do_join23a(): site survey if scanned_queue "
 				  "is empty\n."));
@@ -83,7 +82,7 @@ u8 rtw_do_join23a(struct rtw_adapter *padapter)
 			mod_timer(&pmlmepriv->assoc_timer,
 				  jiffies + msecs_to_jiffies(MAX_JOIN_TIMEOUT));
 		} else {
-			if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) {
+			if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
 				struct wlan_bssid_ex *pdev_network;
 				/*  submit createbss_cmd to change to a
 				    ADHOC_MASTER */
@@ -130,7 +129,7 @@ u8 rtw_do_join23a(struct rtw_adapter *padapter)
 				   queue */
 				/* we try to issue sitesurvey firstly */
 				if (pmlmepriv->LinkDetectInfo.bBusyTraffic ==
-				    false || rtw_to_roaming(padapter) > 0) {
+				    false || padapter->mlmepriv.to_roaming > 0){
 					/* DBG_8723A("rtw_do_join23a() when   no "
 					   "desired bss in scanning queue\n");
 					*/
@@ -152,9 +151,10 @@ exit:
 	return ret;
 }
 
-u8 rtw_set_802_11_ssid23a(struct rtw_adapter* padapter, struct cfg80211_ssid *ssid)
+int rtw_set_802_11_ssid23a(struct rtw_adapter* padapter,
+			   struct cfg80211_ssid *ssid)
 {
-	u8 status = _SUCCESS;
+	int status = _SUCCESS;
 	u32 cur_time = 0;
 
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
@@ -175,22 +175,19 @@ u8 rtw_set_802_11_ssid23a(struct rtw_adapter* padapter, struct cfg80211_ssid *ss
 	spin_lock_bh(&pmlmepriv->lock);
 
 	DBG_8723A("Set SSID under fw_state = 0x%08x\n", get_fwstate(pmlmepriv));
-	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true) {
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY))
 		goto handle_tkip_countermeasure;
-	} else if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true) {
+	else if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING))
 		goto release_mlme_lock;
-	}
 
-	if (check_fwstate(pmlmepriv, _FW_LINKED|WIFI_ADHOC_MASTER_STATE) == true)
-	{
+	if (check_fwstate(pmlmepriv, _FW_LINKED|WIFI_ADHOC_MASTER_STATE)) {
 		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
 			 ("set_ssid: _FW_LINKED||WIFI_ADHOC_MASTER_STATE\n"));
 
 		if ((pmlmepriv->assoc_ssid.ssid_len == ssid->ssid_len) &&
 		    !memcmp(&pmlmepriv->assoc_ssid.ssid, ssid->ssid,
 			    ssid->ssid_len)) {
-			if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == false))
-			{
+			if (!check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
 				RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
 					 ("Set SSID is the same ssid, fw_state = 0x%08x\n",
 					  get_fwstate(pmlmepriv)));
@@ -200,12 +197,12 @@ u8 rtw_set_802_11_ssid23a(struct rtw_adapter* padapter, struct cfg80211_ssid *ss
 					/* if in WIFI_ADHOC_MASTER_STATE | WIFI_ADHOC_STATE, create bss or rejoin again */
 					rtw_disassoc_cmd23a(padapter, 0, true);
 
-					if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
+					if (check_fwstate(pmlmepriv, _FW_LINKED))
 						rtw_indicate_disconnect23a(padapter);
 
 					rtw_free_assoc_resources23a(padapter, 1);
 
-					if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) {
+					if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
 						_clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
 						set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
 					}
@@ -228,12 +225,12 @@ u8 rtw_set_802_11_ssid23a(struct rtw_adapter* padapter, struct cfg80211_ssid *ss
 
 			rtw_disassoc_cmd23a(padapter, 0, true);
 
-			if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
+			if (check_fwstate(pmlmepriv, _FW_LINKED))
 				rtw_indicate_disconnect23a(padapter);
 
 			rtw_free_assoc_resources23a(padapter, 1);
 
-			if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) {
+			if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
 				_clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
 				set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
 			}
@@ -260,12 +257,10 @@ handle_tkip_countermeasure:
 	memcpy(&pmlmepriv->assoc_ssid, ssid, sizeof(struct cfg80211_ssid));
 	pmlmepriv->assoc_by_bssid = false;
 
-	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true) {
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY))
 		pmlmepriv->to_join = true;
-	}
-	else {
+	else
 		status = rtw_do_join23a(padapter);
-	}
 
 release_mlme_lock:
 	spin_unlock_bh(&pmlmepriv->lock);
@@ -279,108 +274,23 @@ exit:
 	return status;
 }
 
-u8 rtw_set_802_11_infrastructure_mode23a(struct rtw_adapter* padapter,
-	enum ndis_802_11_net_infra networktype)
-{
-	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
-	struct	wlan_network	*cur_network = &pmlmepriv->cur_network;
-	enum ndis_802_11_net_infra* pold_state = &cur_network->network.InfrastructureMode;
-
-
-
-	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_notice_,
-		 ("+rtw_set_802_11_infrastructure_mode23a: old =%d new =%d fw_state = 0x%08x\n",
-		  *pold_state, networktype, get_fwstate(pmlmepriv)));
-
-	if (*pold_state != networktype)
-	{
-		spin_lock_bh(&pmlmepriv->lock);
-
-		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, (" change mode!"));
-		/* DBG_8723A("change mode, old_mode =%d, new_mode =%d, fw_state = 0x%x\n", *pold_state, networktype, get_fwstate(pmlmepriv)); */
-
-		if (*pold_state == Ndis802_11APMode)
-		{
-			/* change to other mode from Ndis802_11APMode */
-			cur_network->join_res = -1;
-
-#ifdef CONFIG_8723AU_AP_MODE
-			stop_ap_mode23a(padapter);
-#endif
-		}
-
-		if ((check_fwstate(pmlmepriv, _FW_LINKED) == true) ||(*pold_state == Ndis802_11IBSS))
-			rtw_disassoc_cmd23a(padapter, 0, true);
-
-		if ((check_fwstate(pmlmepriv, _FW_LINKED) == true) ||
-			(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true))
-			rtw_free_assoc_resources23a(padapter, 1);
-
-		if ((*pold_state == Ndis802_11Infrastructure) ||(*pold_state == Ndis802_11IBSS))
-	       {
-			if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-			{
-				rtw_indicate_disconnect23a(padapter); /* will clr Linked_state; before this function, we must have chked whether  issue dis-assoc_cmd or not */
-			}
-	       }
-
-		*pold_state = networktype;
-
-		_clr_fwstate_(pmlmepriv, ~WIFI_NULL_STATE);
-
-		switch (networktype)
-		{
-			case Ndis802_11IBSS:
-				set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
-				break;
-
-			case Ndis802_11Infrastructure:
-				set_fwstate(pmlmepriv, WIFI_STATION_STATE);
-				break;
-
-			case Ndis802_11APMode:
-				set_fwstate(pmlmepriv, WIFI_AP_STATE);
-#ifdef CONFIG_8723AU_AP_MODE
-				start_ap_mode23a(padapter);
-				/* rtw_indicate_connect23a(padapter); */
-#endif
-
-				break;
-
-			case Ndis802_11AutoUnknown:
-			case Ndis802_11InfrastructureMax:
-				break;
-		}
-
-		/* SecClearAllKeys(adapter); */
-
-		/* RT_TRACE(COMP_OID_SET, DBG_LOUD, ("set_infrastructure: fw_state:%x after changing mode\n", */
-		/*									get_fwstate(pmlmepriv))); */
-
-		spin_unlock_bh(&pmlmepriv->lock);
-	}
-
-
-
-	return true;
-}
-
-u8 rtw_set_802_11_bssid23a_list_scan(struct rtw_adapter *padapter,
-				  struct cfg80211_ssid *pssid, int ssid_max_num)
+int rtw_set_802_11_bssid23a_list_scan(struct rtw_adapter *padapter,
+				      struct cfg80211_ssid *pssid,
+				      int ssid_max_num)
 {
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	u8 res = true;
+	int res = _SUCCESS;
 
 	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
 		 ("+rtw_set_802_11_bssid23a_list_scan(), fw_state =%x\n",
 		  get_fwstate(pmlmepriv)));
 
 	if (!padapter) {
-		res = false;
+		res = _FAIL;
 		goto exit;
 	}
 	if (padapter->hw_init_completed == false) {
-		res = false;
+		res = _FAIL;
 		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
 			 ("\n === rtw_set_802_11_bssid23a_list_scan:"
 			  "hw_init_completed == false ===\n"));
@@ -393,7 +303,6 @@ u8 rtw_set_802_11_bssid23a_list_scan(struct rtw_adapter *padapter,
 		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
 			 ("rtw_set_802_11_bssid23a_list_scan fail since fw_state "
 			  "= %x\n", get_fwstate(pmlmepriv)));
-		res = true;
 
 		if (check_fwstate(pmlmepriv,
 				  (_FW_UNDER_SURVEY|_FW_UNDER_LINKING))) {
@@ -406,8 +315,8 @@ u8 rtw_set_802_11_bssid23a_list_scan(struct rtw_adapter *padapter,
 		}
 	} else {
 		if (rtw_is_scan_deny(padapter)) {
-			DBG_8723A(FUNC_ADPT_FMT": scan deny\n",
-				  FUNC_ADPT_ARG(padapter));
+			DBG_8723A("%s(%s): scan deny\n",
+				  __func__, padapter->pnetdev->name);
 			return _SUCCESS;
 		}
 
@@ -422,12 +331,11 @@ exit:
 	return res;
 }
 
-u8 rtw_set_802_11_authentication_mode23a(struct rtw_adapter* padapter,
-				      enum ndis_802_11_auth_mode authmode)
+int rtw_set_802_11_authentication_mode23a(struct rtw_adapter* padapter,
+					  enum ndis_802_11_auth_mode authmode)
 {
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	int res;
-	u8 ret;
 
 	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
 		 ("set_802_11_auth.mode(): mode =%x\n", authmode));
@@ -444,90 +352,7 @@ u8 rtw_set_802_11_authentication_mode23a(struct rtw_adapter* padapter,
 
 	res = rtw_set_auth23a(padapter, psecuritypriv);
 
-	if (res == _SUCCESS)
-		ret = true;
-	else
-		ret = false;
-
-	return ret;
-}
-
-u8 rtw_set_802_11_add_wep23a(struct rtw_adapter* padapter,
-			  struct ndis_802_11_wep *wep)
-{
-	u8 bdefaultkey;
-	u8 btransmitkey;
-	int keyid, res;
-	struct security_priv *psecuritypriv = &padapter->securitypriv;
-	u8 ret = _SUCCESS;
-
-	bdefaultkey = (wep->KeyIndex & 0x40000000) > 0 ? false : true;
-	btransmitkey = (wep->KeyIndex & 0x80000000) > 0 ? true  : false;
-	keyid = wep->KeyIndex & 0x3fffffff;
-
-	if (keyid >= 4) {
-		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
-			 ("MgntActrtw_set_802_11_add_wep23a:keyid>4 =>fail\n"));
-		ret = false;
-		goto exit;
-	}
-
-	switch (wep->KeyLength)
-	{
-	case 5:
-		psecuritypriv->dot11PrivacyAlgrthm = _WEP40_;
-		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
-			 ("MgntActrtw_set_802_11_add_wep23a:wep->KeyLength = 5\n"));
-		break;
-	case 13:
-		psecuritypriv->dot11PrivacyAlgrthm = _WEP104_;
-		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
-			 ("MgntActrtw_set_802_11_add_wep23a:wep->KeyLength = 13\n"));
-		break;
-	default:
-		psecuritypriv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
-		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
-			 ("MgntActrtw_set_802_11_add_wep23a:wep->KeyLength!= 5 "
-			  "or 13\n"));
-			break;
-	}
-
-	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
-		 ("rtw_set_802_11_add_wep23a:befor memcpy, wep->KeyLength = 0x%x "
-		  "wep->KeyIndex = 0x%x  keyid =%x\n",
-		  wep->KeyLength, wep->KeyIndex, keyid));
-
-	memcpy(&psecuritypriv->dot11DefKey[keyid].skey[0],
-	       &wep->KeyMaterial, wep->KeyLength);
-
-	psecuritypriv->dot11DefKeylen[keyid] = wep->KeyLength;
-
-	psecuritypriv->dot11PrivacyKeyIndex = keyid;
-
-	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
-		 ("rtw_set_802_11_add_wep23a:security key material : %x %x %x %x "
-		  "%x %x %x %x %x %x %x %x %x\n",
-		  psecuritypriv->dot11DefKey[keyid].skey[0],
-		  psecuritypriv->dot11DefKey[keyid].skey[1],
-		  psecuritypriv->dot11DefKey[keyid].skey[2],
-		  psecuritypriv->dot11DefKey[keyid].skey[3],
-		  psecuritypriv->dot11DefKey[keyid].skey[4],
-		  psecuritypriv->dot11DefKey[keyid].skey[5],
-		  psecuritypriv->dot11DefKey[keyid].skey[6],
-		  psecuritypriv->dot11DefKey[keyid].skey[7],
-		  psecuritypriv->dot11DefKey[keyid].skey[8],
-		  psecuritypriv->dot11DefKey[keyid].skey[9],
-		  psecuritypriv->dot11DefKey[keyid].skey[10],
-		  psecuritypriv->dot11DefKey[keyid].skey[11],
-		  psecuritypriv->dot11DefKey[keyid].skey[12]));
-
-	res = rtw_set_key23a(padapter, psecuritypriv, keyid, 1);
-
-	if (res == _FAIL)
-		ret = false;
-exit:
-
-	return ret;
+	return res;
 }
 
 /*
@@ -539,7 +364,7 @@ exit:
 u16 rtw_get_cur_max_rate23a(struct rtw_adapter *adapter)
 {
 	int i = 0;
-	u8 *p;
+	const u8 *p;
 	u16 rate = 0, max_rate = 0;
 	struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
@@ -550,16 +375,16 @@ u16 rtw_get_cur_max_rate23a(struct rtw_adapter *adapter)
 	u8 rf_type = 0;
 	u8 bw_40MHz = 0, short_GI_20 = 0, short_GI_40 = 0;
 	u16 mcs_rate = 0;
-	u32 ht_ielen = 0;
 
 	if (!check_fwstate(pmlmepriv, _FW_LINKED) &&
 	    !check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE))
 		return 0;
 
 	if (pmlmeext->cur_wireless_mode & (WIRELESS_11_24N|WIRELESS_11_5N)) {
-		p = rtw_get_ie23a(&pcur_bss->IEs[12], _HT_CAPABILITY_IE_,
-			       &ht_ielen, pcur_bss->IELength - 12);
-		if (p && ht_ielen > 0) {
+		p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY,
+				     &pcur_bss->IEs[12],
+				     pcur_bss->IELength - 12);
+		if (p && p[1] > 0) {
 			pht_capie = (struct ieee80211_ht_cap *)(p + 2);
 
 			memcpy(&mcs_rate, &pht_capie->mcs, 2);
@@ -569,7 +394,7 @@ u16 rtw_get_cur_max_rate23a(struct rtw_adapter *adapter)
 			/* cur_bwmod is updated by beacon, pmlmeinfo is
 			   updated by association response */
 			bw_40MHz = (pmlmeext->cur_bwmode &&
-				    (HT_INFO_HT_PARAM_REC_TRANS_CHNL_WIDTH &
+				    (IEEE80211_HT_PARAM_CHAN_WIDTH_ANY &
 				     pmlmeinfo->HT_info.infos[0])) ? 1:0;
 
 			/* short_GI = (pht_capie->cap_info & (IEEE80211_HT_CAP
@@ -577,8 +402,7 @@ u16 rtw_get_cur_max_rate23a(struct rtw_adapter *adapter)
 			short_GI_20 = (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info&IEEE80211_HT_CAP_SGI_20) ? 1:0;
 			short_GI_40 = (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info&IEEE80211_HT_CAP_SGI_40) ? 1:0;
 
-			rtw23a_hal_get_hwreg(adapter, HW_VAR_RF_TYPE,
-					  (u8 *)(&rf_type));
+			rf_type = rtl8723a_get_rf_type(adapter);
 			max_rate = rtw_mcs_rate23a(rf_type, bw_40MHz &
 						pregistrypriv->cbw40_enable,
 						short_GI_20, short_GI_40,
diff --git a/drivers/staging/rtl8723au/core/rtw_led.c b/drivers/staging/rtl8723au/core/rtw_led.c
index 68532a3b2c1..e21a42c5418 100644
--- a/drivers/staging/rtl8723au/core/rtw_led.c
+++ b/drivers/staging/rtl8723au/core/rtw_led.c
@@ -225,7 +225,7 @@ static void SwLedBlink1(struct led_8723a *pLed)
 		if (pLed->BlinkTimes == 0)
 			bStopBlinking = true;
 		if (bStopBlinking) {
-			if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
 				pLed->bLedLinkBlinkInProgress = true;
 				pLed->CurrLedState = LED_BLINK_NORMAL;
 				if (pLed->bLedOn)
@@ -234,7 +234,7 @@ static void SwLedBlink1(struct led_8723a *pLed)
 					pLed->BlinkingLedState = RTW_LED_ON;
 				delay = LED_BLINK_LINK_INTERVAL_ALPHA;
 				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-			} else if (check_fwstate(pmlmepriv, _FW_LINKED) == false) {
+			} else {
 				pLed->bLedNoLinkBlinkInProgress = true;
 				pLed->CurrLedState = LED_BLINK_SLOWLY;
 				if (pLed->bLedOn)
@@ -258,7 +258,7 @@ static void SwLedBlink1(struct led_8723a *pLed)
 		if (pLed->BlinkTimes == 0)
 			bStopBlinking = true;
 		if (bStopBlinking) {
-			if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
 				pLed->bLedLinkBlinkInProgress = true;
 				pLed->CurrLedState = LED_BLINK_NORMAL;
 				if (pLed->bLedOn)
@@ -267,8 +267,7 @@ static void SwLedBlink1(struct led_8723a *pLed)
 					pLed->BlinkingLedState = RTW_LED_ON;
 				delay = LED_BLINK_LINK_INTERVAL_ALPHA;
 				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-			} else if (check_fwstate(pmlmepriv,
-						 _FW_LINKED) == false) {
+			} else {
 				pLed->bLedNoLinkBlinkInProgress = true;
 				pLed->CurrLedState = LED_BLINK_SLOWLY;
 				if (pLed->bLedOn)
@@ -354,7 +353,7 @@ static void SwLedBlink2(struct led_8723a *pLed)
 				RT_TRACE(_module_rtl8712_led_c_, _drv_info_,
 					 ("stop scan blink CurrLedState %d\n",
 					 pLed->CurrLedState));
-			} else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
+			} else {
 				pLed->CurrLedState = RTW_LED_OFF;
 				pLed->BlinkingLedState = RTW_LED_OFF;
 				SwLedOff23a(padapter, pLed);
@@ -390,7 +389,7 @@ static void SwLedBlink2(struct led_8723a *pLed)
 				RT_TRACE(_module_rtl8712_led_c_, _drv_info_,
 					 ("stop CurrLedState %d\n", pLed->CurrLedState));
 
-			} else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
+			} else {
 				pLed->CurrLedState = RTW_LED_OFF;
 				pLed->BlinkingLedState = RTW_LED_OFF;
 				SwLedOff23a(padapter, pLed);
@@ -450,17 +449,14 @@ static void SwLedBlink3(struct led_8723a *pLed)
 				{
 					SwLedOff23a(padapter, pLed);
 				}
-				else if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-				{
+				else if (check_fwstate(pmlmepriv, _FW_LINKED)) {
 					pLed->CurrLedState = RTW_LED_ON;
 					pLed->BlinkingLedState = RTW_LED_ON;
 					if (!pLed->bLedOn)
 						SwLedOn23a(padapter, pLed);
 
 					RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-				}
-				else if (check_fwstate(pmlmepriv, _FW_LINKED) == false)
-				{
+				} else {
 					pLed->CurrLedState = RTW_LED_OFF;
 					pLed->BlinkingLedState = RTW_LED_OFF;
 					if (pLed->bLedOn)
@@ -499,9 +495,8 @@ static void SwLedBlink3(struct led_8723a *pLed)
 				if (padapter->pwrctrlpriv.rf_pwrstate != rf_on)
 				{
 					SwLedOff23a(padapter, pLed);
-				}
-				else if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-				{
+				} else if (check_fwstate(pmlmepriv,
+							 _FW_LINKED)) {
 					pLed->CurrLedState = RTW_LED_ON;
 					pLed->BlinkingLedState = RTW_LED_ON;
 
@@ -509,9 +504,7 @@ static void SwLedBlink3(struct led_8723a *pLed)
 						SwLedOn23a(padapter, pLed);
 
 					RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-				}
-				else if (check_fwstate(pmlmepriv, _FW_LINKED) == false)
-				{
+				} else {
 					pLed->CurrLedState = RTW_LED_OFF;
 					pLed->BlinkingLedState = RTW_LED_OFF;
 
@@ -914,8 +907,8 @@ SwLedControlMode1(struct rtw_adapter *padapter, enum led_ctl_mode LedAction)
 			break;
 
 		case LED_CTL_SITE_SURVEY:
-			 if ((pmlmepriv->LinkDetectInfo.bBusyTraffic) &&
-			     (check_fwstate(pmlmepriv, _FW_LINKED) == true))
+			 if (pmlmepriv->LinkDetectInfo.bBusyTraffic &&
+			     check_fwstate(pmlmepriv, _FW_LINKED))
 				;
 			 else if (pLed->bLedScanBlinkInProgress == false) {
 				if (IS_LED_WPS_BLINKING(pLed))
@@ -1118,8 +1111,8 @@ SwLedControlMode2(struct rtw_adapter *padapter, enum led_ctl_mode LedAction)
 		 break;
 	case LED_CTL_TX:
 	case LED_CTL_RX:
-		if ((pLed->bLedBlinkInProgress == false) &&
-		    (check_fwstate(pmlmepriv, _FW_LINKED) == true)) {
+		if (pLed->bLedBlinkInProgress == false &&
+		    check_fwstate(pmlmepriv, _FW_LINKED)) {
 			if (pLed->CurrLedState == LED_BLINK_SCAN ||
 			    IS_LED_WPS_BLINKING(pLed)) {
 				return;
@@ -1261,8 +1254,8 @@ SwLedControlMode3(struct rtw_adapter *padapter, enum led_ctl_mode LedAction)
 
 		case LED_CTL_TX:
 		case LED_CTL_RX:
-			if ((pLed->bLedBlinkInProgress == false) &&
-			    (check_fwstate(pmlmepriv, _FW_LINKED) == true)) {
+			if (pLed->bLedBlinkInProgress == false &&
+			    check_fwstate(pmlmepriv, _FW_LINKED)) {
 				if (pLed->CurrLedState == LED_BLINK_SCAN ||
 				    IS_LED_WPS_BLINKING(pLed)) {
 					return;
@@ -1477,8 +1470,8 @@ SwLedControlMode4(struct rtw_adapter *padapter, enum led_ctl_mode LedAction)
 			break;
 
 		case LED_CTL_SITE_SURVEY:
-			if ((pmlmepriv->LinkDetectInfo.bBusyTraffic) &&
-			    (check_fwstate(pmlmepriv, _FW_LINKED) == true))
+			if (pmlmepriv->LinkDetectInfo.bBusyTraffic &&
+			    check_fwstate(pmlmepriv, _FW_LINKED))
 				;
 			else if (pLed->bLedScanBlinkInProgress == false) {
 				if (IS_LED_WPS_BLINKING(pLed))
@@ -1714,7 +1707,8 @@ SwLedControlMode5(struct rtw_adapter *padapter, enum led_ctl_mode LedAction)
 			break;
 
 		case LED_CTL_SITE_SURVEY:
-			if ((pmlmepriv->LinkDetectInfo.bBusyTraffic) && (check_fwstate(pmlmepriv, _FW_LINKED) == true))
+			if (pmlmepriv->LinkDetectInfo.bBusyTraffic &&
+			    check_fwstate(pmlmepriv, _FW_LINKED))
 				;
 			else if (pLed->bLedScanBlinkInProgress == false)
 			{
diff --git a/drivers/staging/rtl8723au/core/rtw_mlme.c b/drivers/staging/rtl8723au/core/rtw_mlme.c
index 6cee78785bd..7170258d260 100644
--- a/drivers/staging/rtl8723au/core/rtw_mlme.c
+++ b/drivers/staging/rtl8723au/core/rtw_mlme.c
@@ -25,8 +25,7 @@
 #include <wifi.h>
 #include <wlan_bssdef.h>
 #include <rtw_ioctl_set.h>
-
-extern u8 rtw_do_join23a(struct rtw_adapter * padapter);
+#include <rtw_sreset.h>
 
 static void rtw_init_mlme_timer(struct rtw_adapter *padapter)
 {
@@ -45,7 +44,7 @@ static void rtw_init_mlme_timer(struct rtw_adapter *padapter)
 		    rtw_set_scan_deny_timer_hdl, (unsigned long)padapter);
 }
 
-int _rtw_init_mlme_priv23a(struct rtw_adapter *padapter)
+int rtw_init_mlme_priv23a(struct rtw_adapter *padapter)
 {
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	int res = _SUCCESS;
@@ -53,16 +52,16 @@ int _rtw_init_mlme_priv23a(struct rtw_adapter *padapter)
 	pmlmepriv->nic_hdl = padapter;
 
 	pmlmepriv->fw_state = 0;
-	pmlmepriv->cur_network.network.InfrastructureMode = Ndis802_11AutoUnknown;
-	pmlmepriv->scan_mode=SCAN_ACTIVE;/*  1: active, 0: pasive. Maybe someday we should rename this varable to "active_mode" (Jeff) */
+	pmlmepriv->cur_network.network.ifmode = NL80211_IFTYPE_UNSPECIFIED;
+	/*  1: active, 0: pasive. Maybe someday we should rename this
+	    varable to "active_mode" (Jeff) */
+	pmlmepriv->scan_mode = SCAN_ACTIVE;
 
 	spin_lock_init(&pmlmepriv->lock);
 	_rtw_init_queue23a(&pmlmepriv->scanned_queue);
 
 	memset(&pmlmepriv->assoc_ssid, 0, sizeof(struct cfg80211_ssid));
 
-	/* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
-
 	rtw_clear_scan_deny(padapter);
 
 	rtw_init_mlme_timer(padapter);
@@ -72,11 +71,10 @@ int _rtw_init_mlme_priv23a(struct rtw_adapter *padapter)
 #ifdef CONFIG_8723AU_AP_MODE
 static void rtw_free_mlme_ie_data(u8 **ppie, u32 *plen)
 {
-	if(*ppie)
-	{
+	if (*ppie) {
 		kfree(*ppie);
 		*plen = 0;
-		*ppie=NULL;
+		*ppie = NULL;
 	}
 }
 #endif
@@ -86,37 +84,52 @@ void rtw23a_free_mlme_priv_ie_data(struct mlme_priv *pmlmepriv)
 #ifdef CONFIG_8723AU_AP_MODE
 	kfree(pmlmepriv->assoc_req);
 	kfree(pmlmepriv->assoc_rsp);
-	rtw_free_mlme_ie_data(&pmlmepriv->wps_beacon_ie, &pmlmepriv->wps_beacon_ie_len);
-	rtw_free_mlme_ie_data(&pmlmepriv->wps_probe_req_ie, &pmlmepriv->wps_probe_req_ie_len);
-	rtw_free_mlme_ie_data(&pmlmepriv->wps_probe_resp_ie, &pmlmepriv->wps_probe_resp_ie_len);
-	rtw_free_mlme_ie_data(&pmlmepriv->wps_assoc_resp_ie, &pmlmepriv->wps_assoc_resp_ie_len);
-
-	rtw_free_mlme_ie_data(&pmlmepriv->p2p_beacon_ie, &pmlmepriv->p2p_beacon_ie_len);
-	rtw_free_mlme_ie_data(&pmlmepriv->p2p_probe_req_ie, &pmlmepriv->p2p_probe_req_ie_len);
-	rtw_free_mlme_ie_data(&pmlmepriv->p2p_probe_resp_ie, &pmlmepriv->p2p_probe_resp_ie_len);
-	rtw_free_mlme_ie_data(&pmlmepriv->p2p_go_probe_resp_ie, &pmlmepriv->p2p_go_probe_resp_ie_len);
-	rtw_free_mlme_ie_data(&pmlmepriv->p2p_assoc_req_ie, &pmlmepriv->p2p_assoc_req_ie_len);
-
-	rtw_free_mlme_ie_data(&pmlmepriv->wfd_beacon_ie, &pmlmepriv->wfd_beacon_ie_len);
-	rtw_free_mlme_ie_data(&pmlmepriv->wfd_probe_req_ie, &pmlmepriv->wfd_probe_req_ie_len);
-	rtw_free_mlme_ie_data(&pmlmepriv->wfd_probe_resp_ie, &pmlmepriv->wfd_probe_resp_ie_len);
-	rtw_free_mlme_ie_data(&pmlmepriv->wfd_go_probe_resp_ie, &pmlmepriv->wfd_go_probe_resp_ie_len);
-	rtw_free_mlme_ie_data(&pmlmepriv->wfd_assoc_req_ie, &pmlmepriv->wfd_assoc_req_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_beacon_ie,
+			      &pmlmepriv->wps_beacon_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_probe_req_ie,
+			      &pmlmepriv->wps_probe_req_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_probe_resp_ie,
+			      &pmlmepriv->wps_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_assoc_resp_ie,
+			      &pmlmepriv->wps_assoc_resp_ie_len);
+
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_beacon_ie,
+			      &pmlmepriv->p2p_beacon_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_probe_req_ie,
+			      &pmlmepriv->p2p_probe_req_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_probe_resp_ie,
+			      &pmlmepriv->p2p_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_go_probe_resp_ie,
+			      &pmlmepriv->p2p_go_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_assoc_req_ie,
+			      &pmlmepriv->p2p_assoc_req_ie_len);
+
+	rtw_free_mlme_ie_data(&pmlmepriv->wfd_beacon_ie,
+			      &pmlmepriv->wfd_beacon_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wfd_probe_req_ie,
+			      &pmlmepriv->wfd_probe_req_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wfd_probe_resp_ie,
+			      &pmlmepriv->wfd_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wfd_go_probe_resp_ie,
+			      &pmlmepriv->wfd_go_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wfd_assoc_req_ie,
+			      &pmlmepriv->wfd_assoc_req_ie_len);
 #endif
 }
 
-void _rtw_free_mlme_priv23a(struct mlme_priv *pmlmepriv)
+void rtw_free_mlme_priv23a(struct mlme_priv *pmlmepriv)
 {
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+		 ("rtw_free_mlme_priv23a\n"));
 
 	rtw23a_free_mlme_priv_ie_data(pmlmepriv);
-
 }
 
-struct wlan_network *rtw_alloc_network(struct mlme_priv *pmlmepriv)
+struct wlan_network *rtw_alloc_network(struct mlme_priv *pmlmepriv, int gfp)
 {
 	struct wlan_network *pnetwork;
 
-	pnetwork = kzalloc(sizeof(struct wlan_network), GFP_ATOMIC);
+	pnetwork = kzalloc(sizeof(struct wlan_network), gfp);
 	if (pnetwork) {
 		INIT_LIST_HEAD(&pnetwork->list);
 		pnetwork->network_type = 0;
@@ -129,48 +142,27 @@ struct wlan_network *rtw_alloc_network(struct mlme_priv *pmlmepriv)
 	return pnetwork;
 }
 
-void _rtw_free_network23a(struct mlme_priv *pmlmepriv,
-		       struct wlan_network *pnetwork, u8 isfreeall)
+static void _rtw_free_network23a(struct mlme_priv *pmlmepriv,
+				 struct wlan_network *pnetwork)
 {
-	u32 lifetime = SCANQUEUE_LIFETIME;
-
 	if (!pnetwork)
 		return;
 
 	if (pnetwork->fixed == true)
 		return;
 
-	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) ||
-	    (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true))
-		lifetime = 1;
-
-	list_del_init(&pnetwork->list);
-
-	kfree(pnetwork);
-}
-
-void _rtw_free_network23a_nolock23a(struct mlme_priv *pmlmepriv,
-			      struct wlan_network *pnetwork)
-{
-
-	if (pnetwork == NULL)
-		return;
-
-	if (pnetwork->fixed == true)
-		return;
-
 	list_del_init(&pnetwork->list);
 
 	kfree(pnetwork);
 }
 
 /*
-	return the wlan_network with the matching addr
+ return the wlan_network with the matching addr
 
-	Shall be calle under atomic context... to avoid possible racing condition...
+ Shall be calle under atomic context... to avoid possible racing condition...
 */
 struct wlan_network *
-_rtw_find_network23a(struct rtw_queue *scanned_queue, u8 *addr)
+rtw_find_network23a(struct rtw_queue *scanned_queue, u8 *addr)
 {
 	struct list_head *phead, *plist;
 	struct wlan_network *pnetwork = NULL;
@@ -194,7 +186,7 @@ _rtw_find_network23a(struct rtw_queue *scanned_queue, u8 *addr)
 		plist = plist->next;
         }
 
-	if(plist == phead)
+	if (plist == phead)
 		pnetwork = NULL;
 
 	/* spin_unlock_bh(&scanned_queue->lock); */
@@ -204,11 +196,11 @@ exit:
 	return pnetwork;
 }
 
-void _rtw_free_network23a_queue23a(struct rtw_adapter *padapter, u8 isfreeall)
+void rtw_free_network_queue23a(struct rtw_adapter *padapter)
 {
 	struct list_head *phead, *plist, *ptmp;
 	struct wlan_network *pnetwork;
-	struct mlme_priv* pmlmepriv = &padapter->mlmepriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct rtw_queue *scanned_queue = &pmlmepriv->scanned_queue;
 
 	spin_lock_bh(&scanned_queue->lock);
@@ -218,24 +210,25 @@ void _rtw_free_network23a_queue23a(struct rtw_adapter *padapter, u8 isfreeall)
 	list_for_each_safe(plist, ptmp, phead) {
 		pnetwork = container_of(plist, struct wlan_network, list);
 
-		_rtw_free_network23a(pmlmepriv,pnetwork, isfreeall);
+		_rtw_free_network23a(pmlmepriv, pnetwork);
 	}
 
 	spin_unlock_bh(&scanned_queue->lock);
-
 }
 
-int rtw_if_up23a(struct rtw_adapter *padapter)	{
-
+int rtw_if_up23a(struct rtw_adapter *padapter)
+{
 	int res;
 
-	if(padapter->bDriverStopped || padapter->bSurpriseRemoved ||
-		(check_fwstate(&padapter->mlmepriv, _FW_LINKED)== false)) {
-		RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_if_up23a:bDriverStopped(%d) OR bSurpriseRemoved(%d)", padapter->bDriverStopped, padapter->bSurpriseRemoved));
-		res=false;
-	}
-	else
-		res=  true;
+	if (padapter->bDriverStopped || padapter->bSurpriseRemoved ||
+	    !check_fwstate(&padapter->mlmepriv, _FW_LINKED)) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+			 ("rtw_if_up23a:bDriverStopped(%d) OR "
+			  "bSurpriseRemoved(%d)", padapter->bDriverStopped,
+			  padapter->bSurpriseRemoved));
+		res = false;
+	} else
+		res =  true;
 
 	return res;
 }
@@ -247,115 +240,114 @@ void rtw_generate_random_ibss23a(u8* pibss)
 	pibss[0] = 0x02;  /* in ad-hoc mode bit1 must set to 1 */
 	pibss[1] = 0x11;
 	pibss[2] = 0x87;
-	pibss[3] = (u8)(curtime & 0xff) ;/* p[0]; */
-	pibss[4] = (u8)((curtime>>8) & 0xff) ;/* p[1]; */
-	pibss[5] = (u8)((curtime>>16) & 0xff) ;/* p[2]; */
+	pibss[3] = curtime & 0xff;/* p[0]; */
+	pibss[4] = (curtime >> 8) & 0xff;/* p[1]; */
+	pibss[5] = (curtime >> 16) & 0xff;/* p[2]; */
 
 	return;
 }
 
-u8 *rtw_get_capability23a_from_ie(u8 *ie)
+void rtw_set_roaming(struct rtw_adapter *adapter, u8 to_roaming)
 {
-	return ie + 8 + 2;
+	if (to_roaming == 0)
+		adapter->mlmepriv.to_join = false;
+	adapter->mlmepriv.to_roaming = to_roaming;
 }
 
-u16 rtw_get_capability23a(struct wlan_bssid_ex *bss)
+static void _rtw_roaming(struct rtw_adapter *padapter,
+			 struct wlan_network *tgt_network)
 {
-	u16	val;
-
-	memcpy((u8 *)&val, rtw_get_capability23a_from_ie(bss->IEs), 2);
-
-	return le16_to_cpu(val);
-}
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_network *pnetwork;
+	int do_join_r;
 
-u8 *rtw_get_timestampe_from_ie23a(u8 *ie)
-{
-	return ie + 0;
-}
+	if (tgt_network)
+		pnetwork = tgt_network;
+	else
+		pnetwork = &pmlmepriv->cur_network;
 
-u8 *rtw_get_beacon_interval23a_from_ie(u8 *ie)
-{
-	return ie + 8;
-}
+	if (padapter->mlmepriv.to_roaming > 0) {
+		DBG_8723A("roaming from %s("MAC_FMT"), length:%d\n",
+			  pnetwork->network.Ssid.ssid,
+			  MAC_ARG(pnetwork->network.MacAddress),
+			  pnetwork->network.Ssid.ssid_len);
+		memcpy(&pmlmepriv->assoc_ssid, &pnetwork->network.Ssid,
+		       sizeof(struct cfg80211_ssid));
 
-int	rtw_init_mlme_priv23a (struct rtw_adapter *padapter)/* struct	mlme_priv *pmlmepriv) */
-{
-	int	res;
+		pmlmepriv->assoc_by_bssid = false;
 
-	res = _rtw_init_mlme_priv23a(padapter);/*  (pmlmepriv); */
+		while (1) {
+			do_join_r = rtw_do_join23a(padapter);
+			if (do_join_r == _SUCCESS)
+				break;
+			else {
+				DBG_8723A("roaming do_join return %d\n",
+					  do_join_r);
+				pmlmepriv->to_roaming--;
 
-	return res;
+				if (padapter->mlmepriv.to_roaming > 0)
+					continue;
+				else {
+					DBG_8723A("%s(%d) -to roaming fail, "
+						  "indicate_disconnect\n",
+						  __func__, __LINE__);
+					rtw_indicate_disconnect23a(padapter);
+					break;
+				}
+			}
+		}
+	}
 }
 
-void rtw_free_mlme_priv23a (struct mlme_priv *pmlmepriv)
+void rtw23a_roaming(struct rtw_adapter *padapter,
+		    struct wlan_network *tgt_network)
 {
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_free_mlme_priv23a\n"));
-	_rtw_free_mlme_priv23a(pmlmepriv);
-
+	spin_lock_bh(&pmlmepriv->lock);
+	_rtw_roaming(padapter, tgt_network);
+	spin_unlock_bh(&pmlmepriv->lock);
 }
 
-void rtw_free_network(struct mlme_priv *pmlmepriv, struct	wlan_network *pnetwork, u8 is_freeall);
-void rtw_free_network(struct mlme_priv *pmlmepriv, struct	wlan_network *pnetwork, u8 is_freeall)/* struct	wlan_network *pnetwork, _queue	*free_queue) */
+__le16 *rtw_get_capability23a_from_ie(u8 *ie)
 {
-
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-		 ("rtw_free_network ==> ssid = %s\n\n" ,
-		  pnetwork->network.Ssid.ssid));
-	_rtw_free_network23a(pmlmepriv, pnetwork, is_freeall);
-
+	return (__le16 *)(ie + 8 + 2);
 }
 
-void rtw_free_network_nolock(struct mlme_priv *pmlmepriv, struct wlan_network *pnetwork);
-void rtw_free_network_nolock(struct mlme_priv *pmlmepriv, struct wlan_network *pnetwork)
+u16 rtw_get_capability23a(struct wlan_bssid_ex *bss)
 {
+	u16 val;
 
-	/* RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_free_network ==> ssid = %s\n\n" , pnetwork->network.Ssid.ssid)); */
-	_rtw_free_network23a_nolock23a(pmlmepriv, pnetwork);
+	memcpy(&val, rtw_get_capability23a_from_ie(bss->IEs), 2);
 
+	return le16_to_cpu(val);
 }
 
-void rtw_free_network_queue23a(struct rtw_adapter* dev, u8 isfreeall)
+__le16 *rtw_get_beacon_interval23a_from_ie(u8 *ie)
 {
-
-	_rtw_free_network23a_queue23a(dev, isfreeall);
-
+	return (__le16 *)(ie + 8);
 }
 
-/*
-	return the wlan_network with the matching addr
-
-	Shall be calle under atomic context... to avoid possible racing condition...
-*/
-struct	wlan_network *
-rtw_find_network23a(struct rtw_queue *scanned_queue, u8 *addr)
+static void rtw_free_network_nolock(struct mlme_priv *pmlmepriv,
+				    struct wlan_network *pnetwork)
 {
-	struct wlan_network *pnetwork;
-
-	pnetwork = _rtw_find_network23a(scanned_queue, addr);
-
-	return pnetwork;
+	_rtw_free_network23a(pmlmepriv, pnetwork);
 }
 
-int rtw_is_same_ibss23a(struct rtw_adapter *adapter, struct wlan_network *pnetwork)
+int rtw_is_same_ibss23a(struct rtw_adapter *adapter,
+			struct wlan_network *pnetwork)
 {
 	int ret = true;
 	struct security_priv *psecuritypriv = &adapter->securitypriv;
 
-	if ((psecuritypriv->dot11PrivacyAlgrthm != _NO_PRIVACY_) &&
-		    (pnetwork->network.Privacy == 0))
-	{
+	if (psecuritypriv->dot11PrivacyAlgrthm != 0 &&
+	    pnetwork->network.Privacy == 0)
 		ret = false;
-	}
-	else if ((psecuritypriv->dot11PrivacyAlgrthm == _NO_PRIVACY_) &&
-		 (pnetwork->network.Privacy == 1))
-	{
+	else if (psecuritypriv->dot11PrivacyAlgrthm == 0 &&
+		 pnetwork->network.Privacy == 1)
 		ret = false;
-	}
 	else
-	{
 		ret = true;
-	}
 
 	return ret;
 }
@@ -363,24 +355,19 @@ int rtw_is_same_ibss23a(struct rtw_adapter *adapter, struct wlan_network *pnetwo
 inline int is_same_ess(struct wlan_bssid_ex *a, struct wlan_bssid_ex *b);
 inline int is_same_ess(struct wlan_bssid_ex *a, struct wlan_bssid_ex *b)
 {
-	/* RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("(%s,%d)(%s,%d)\n", */
-	/*		a->Ssid.Ssid, a->Ssid.SsidLength, b->Ssid.Ssid, b->Ssid.SsidLength)); */
 	return (a->Ssid.ssid_len == b->Ssid.ssid_len) &&
 		!memcmp(a->Ssid.ssid, b->Ssid.ssid, a->Ssid.ssid_len);
 }
 
 int is_same_network23a(struct wlan_bssid_ex *src, struct wlan_bssid_ex *dst)
 {
-	 u16 s_cap, d_cap;
-
-	memcpy((u8 *)&s_cap, rtw_get_capability23a_from_ie(src->IEs), 2);
-	memcpy((u8 *)&d_cap, rtw_get_capability23a_from_ie(dst->IEs), 2);
+	u16 s_cap, d_cap;
 
-	s_cap = le16_to_cpu(s_cap);
-	d_cap = le16_to_cpu(d_cap);
+	s_cap = get_unaligned_le16(rtw_get_capability23a_from_ie(src->IEs));
+	d_cap = get_unaligned_le16(rtw_get_capability23a_from_ie(dst->IEs));
 
 	return ((src->Ssid.ssid_len == dst->Ssid.ssid_len) &&
-		/*	(src->Configuration.DSConfig == dst->Configuration.DSConfig) && */
+		/*	(src->DSConfig == dst->DSConfig) && */
 		ether_addr_equal(src->MacAddress, dst->MacAddress) &&
 		((!memcmp(src->Ssid.ssid, dst->Ssid.ssid, src->Ssid.ssid_len))) &&
 		((s_cap & WLAN_CAPABILITY_IBSS) ==
@@ -389,10 +376,10 @@ int is_same_network23a(struct wlan_bssid_ex *src, struct wlan_bssid_ex *dst)
 		 (d_cap & WLAN_CAPABILITY_ESS)));
 }
 
-struct wlan_network *rtw_get_oldest_wlan_network23a(struct rtw_queue *scanned_queue)
+struct wlan_network *
+rtw_get_oldest_wlan_network23a(struct rtw_queue *scanned_queue)
 {
 	struct list_head *plist, *phead;
-
 	struct wlan_network *pwlan;
 	struct wlan_network *oldest = NULL;
 
@@ -412,7 +399,7 @@ struct wlan_network *rtw_get_oldest_wlan_network23a(struct rtw_queue *scanned_qu
 }
 
 void update_network23a(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src,
-	struct rtw_adapter * padapter, bool update_ie)
+		       struct rtw_adapter *padapter, bool update_ie)
 {
 	u8 ss_ori = dst->PhyInfo.SignalStrength;
 	u8 sq_ori = dst->PhyInfo.SignalQuality;
@@ -426,30 +413,35 @@ void update_network23a(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src,
 	u8 sq_final;
 	long rssi_final;
 
-	DBG_8723A("%s %s(%pM, ch%u) ss_ori:%3u, sq_ori:%3u, rssi_ori:%3ld, ss_smp:%3u, sq_smp:%3u, rssi_smp:%3ld\n",
+	DBG_8723A("%s %s(%pM, ch%u) ss_ori:%3u, sq_ori:%3u, rssi_ori:%3ld, "
+		  "ss_smp:%3u, sq_smp:%3u, rssi_smp:%3ld\n",
 		  __func__, src->Ssid.ssid, src->MacAddress,
-		  src->Configuration.DSConfig, ss_ori, sq_ori, rssi_ori,
+		  src->DSConfig, ss_ori, sq_ori, rssi_ori,
 		  ss_smp, sq_smp, rssi_smp
 	);
 
 	/* The rule below is 1/5 for sample value, 4/5 for history value */
-	if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) && is_same_network23a(&padapter->mlmepriv.cur_network.network, src)) {
+	if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) &&
+	    is_same_network23a(&padapter->mlmepriv.cur_network.network, src)) {
 		/* Take the recvpriv's value for the connected AP*/
 		ss_final = padapter->recvpriv.signal_strength;
 		sq_final = padapter->recvpriv.signal_qual;
-		/* the rssi value here is undecorated, and will be used for antenna diversity */
+		/* the rssi value here is undecorated, and will be
+		   used for antenna diversity */
 		if (sq_smp != 101) /* from the right channel */
 			rssi_final = (src->Rssi+dst->Rssi*4)/5;
 		else
 			rssi_final = rssi_ori;
-	}
-	else {
+	} else {
 		if (sq_smp != 101) { /* from the right channel */
-			ss_final = ((u32)(src->PhyInfo.SignalStrength)+(u32)(dst->PhyInfo.SignalStrength)*4)/5;
-			sq_final = ((u32)(src->PhyInfo.SignalQuality)+(u32)(dst->PhyInfo.SignalQuality)*4)/5;
-			rssi_final = (src->Rssi+dst->Rssi*4)/5;
+			ss_final = ((u32)src->PhyInfo.SignalStrength +
+				    (u32)dst->PhyInfo.SignalStrength * 4) / 5;
+			sq_final = ((u32)src->PhyInfo.SignalQuality +
+				    (u32)dst->PhyInfo.SignalQuality * 4) / 5;
+			rssi_final = src->Rssi+dst->Rssi * 4 / 5;
 		} else {
-			/* bss info not receving from the right channel, use the original RX signal infos */
+			/* bss info not receving from the right channel, use
+			   the original RX signal infos */
 			ss_final = dst->PhyInfo.SignalStrength;
 			sq_final = dst->PhyInfo.SignalQuality;
 			rssi_final = dst->Rssi;
@@ -458,35 +450,37 @@ void update_network23a(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src,
 	}
 
 	if (update_ie)
-		memcpy((u8 *)dst, (u8 *)src, get_wlan_bssid_ex_sz(src));
+		memcpy(dst, src, get_wlan_bssid_ex_sz(src));
 
 	dst->PhyInfo.SignalStrength = ss_final;
 	dst->PhyInfo.SignalQuality = sq_final;
 	dst->Rssi = rssi_final;
 
-	DBG_8723A("%s %s(%pM), SignalStrength:%u, SignalQuality:%u, RawRSSI:%ld\n",
-		  __func__, dst->Ssid.ssid, dst->MacAddress,
+	DBG_8723A("%s %s(%pM), SignalStrength:%u, SignalQuality:%u, "
+		  "RawRSSI:%ld\n",  __func__, dst->Ssid.ssid, dst->MacAddress,
 		  dst->PhyInfo.SignalStrength,
 		  dst->PhyInfo.SignalQuality, dst->Rssi);
-
 }
 
-static void update_current_network(struct rtw_adapter *adapter, struct wlan_bssid_ex *pnetwork)
+static void update_current_network(struct rtw_adapter *adapter,
+				   struct wlan_bssid_ex *pnetwork)
 {
-	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
 
-	if ((check_fwstate(pmlmepriv, _FW_LINKED)== true) && (is_same_network23a(&pmlmepriv->cur_network.network, pnetwork)))
-	{
-		/* RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,"Same Network\n"); */
+	if (check_fwstate(pmlmepriv, _FW_LINKED) &&
+	    is_same_network23a(&pmlmepriv->cur_network.network, pnetwork)) {
+		int bcn_size;
+		update_network23a(&pmlmepriv->cur_network.network,
+				  pnetwork,adapter, true);
 
-		/* if(pmlmepriv->cur_network.network.IELength<= pnetwork->IELength) */
-		{
-			update_network23a(&pmlmepriv->cur_network.network, pnetwork,adapter, true);
-			rtw_update_protection23a(adapter, (pmlmepriv->cur_network.network.IEs) + sizeof (struct ndis_802_11_fixed_ies),
-									pmlmepriv->cur_network.network.IELength);
-		}
-	}
+		bcn_size = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+			offsetof(struct ieee80211_mgmt, u.beacon);
 
+		rtw_update_protection23a(adapter,
+					 pmlmepriv->cur_network.network.IEs +
+					 bcn_size,
+					 pmlmepriv->cur_network.network.IELength);
+	}
 }
 
 /*
@@ -494,7 +488,8 @@ static void update_current_network(struct rtw_adapter *adapter, struct wlan_bssi
 Caller must hold pmlmepriv->lock first.
 
 */
-void rtw_update_scanned_network23a(struct rtw_adapter *adapter, struct wlan_bssid_ex *target)
+void rtw_update_scanned_network23a(struct rtw_adapter *adapter,
+				   struct wlan_bssid_ex *target)
 {
 	struct list_head *plist, *phead;
 	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
@@ -522,7 +517,7 @@ void rtw_update_scanned_network23a(struct rtw_adapter *adapter, struct wlan_bssi
 	/* If we didn't find a match, then get a new network slot to initialize
 	 * with this beacon's information */
 	if (!found) {
-		pnetwork = rtw_alloc_network(pmlmepriv);
+		pnetwork = rtw_alloc_network(pmlmepriv, GFP_ATOMIC);
 		if (!pnetwork) {
 			if (!oldest) {
 				RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
@@ -561,19 +556,20 @@ void rtw_update_scanned_network23a(struct rtw_adapter *adapter, struct wlan_bssi
 
 		/* target.reserved == 1, means that scanned network is
 		 * a bcn frame. */
-		if ((pnetwork->network.IELength>target->IELength) &&
-		    (target->reserved == 1))
+		if (pnetwork->network.IELength > target->IELength &&
+		    target->reserved == 1)
 			update_ie = false;
 
-		update_network23a(&pnetwork->network, target,adapter, update_ie);
+		update_network23a(&pnetwork->network, target,adapter,
+				  update_ie);
 	}
 
 exit:
 	spin_unlock_bh(&queue->lock);
-
 }
 
-void rtw_add_network(struct rtw_adapter *adapter, struct wlan_bssid_ex *pnetwork)
+static void rtw_add_network(struct rtw_adapter *adapter,
+			    struct wlan_bssid_ex *pnetwork)
 {
 	update_current_network(adapter, pnetwork);
 	rtw_update_scanned_network23a(adapter, pnetwork);
@@ -585,7 +581,8 @@ void rtw_add_network(struct rtw_adapter *adapter, struct wlan_bssid_ex *pnetwork
 /*			   (3) WMM */
 /*			   (4) HT */
 /*                      (5) others */
-int rtw_is_desired_network(struct rtw_adapter *adapter, struct wlan_network *pnetwork)
+static int rtw_is_desired_network(struct rtw_adapter *adapter,
+				  struct wlan_network *pnetwork)
 {
 	struct security_priv *psecuritypriv = &adapter->securitypriv;
 	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
@@ -600,31 +597,31 @@ int rtw_is_desired_network(struct rtw_adapter *adapter, struct wlan_network *pne
 	desired_encmode = psecuritypriv->ndisencryptstatus;
 	privacy = pnetwork->network.Privacy;
 
-	if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS))
-	{
-		if (rtw_get_wps_ie23a(pnetwork->network.IEs+_FIXED_IE_LENGTH_, pnetwork->network.IELength-_FIXED_IE_LENGTH_, NULL, &wps_ielen)!= NULL)
-		{
+	if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) {
+		if (rtw_get_wps_ie23a(pnetwork->network.IEs + _FIXED_IE_LENGTH_,
+				      pnetwork->network.IELength -
+				      _FIXED_IE_LENGTH_, NULL, &wps_ielen))
 			return true;
-		}
 		else
-		{
 			return false;
-		}
 	}
-	if (adapter->registrypriv.wifi_spec == 1) /* for  correct flow of 8021X  to do.... */
-	{
-		if ((desired_encmode == Ndis802_11EncryptionDisabled) && (privacy != 0))
+	if (adapter->registrypriv.wifi_spec == 1) {
+		/* for  correct flow of 8021X  to do.... */
+		if (desired_encmode == Ndis802_11EncryptionDisabled &&
+		    privacy != 0)
 	            bselected = false;
 	}
 
-	if ((desired_encmode != Ndis802_11EncryptionDisabled) && (privacy == 0)) {
-		DBG_8723A("desired_encmode: %d, privacy: %d\n", desired_encmode, privacy);
+	if (desired_encmode != Ndis802_11EncryptionDisabled &&
+	    privacy == 0) {
+		DBG_8723A("desired_encmode: %d, privacy: %d\n",
+			  desired_encmode, privacy);
 		bselected = false;
 	}
 
-	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true)
-	{
-		if (pnetwork->network.InfrastructureMode != pmlmepriv->cur_network.network.InfrastructureMode)
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+		if (pnetwork->network.ifmode !=
+		    pmlmepriv->cur_network.network.ifmode)
 			bselected = false;
 	}
 
@@ -632,15 +629,14 @@ int rtw_is_desired_network(struct rtw_adapter *adapter, struct wlan_network *pne
 }
 
 /* TODO: Perry : For Power Management */
-void rtw_atimdone_event_callback23a(struct rtw_adapter	*adapter , u8 *pbuf)
+void rtw_atimdone_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf)
 {
-
 	RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("receive atimdone_evet\n"));
 
 	return;
 }
 
-void rtw_survey_event_cb23a(struct rtw_adapter	*adapter, u8 *pbuf)
+void rtw_survey_event_cb23a(struct rtw_adapter *adapter, const u8 *pbuf)
 {
 	u32 len;
 	struct wlan_bssid_ex *pnetwork;
@@ -648,31 +644,36 @@ void rtw_survey_event_cb23a(struct rtw_adapter	*adapter, u8 *pbuf)
 
 	pnetwork = (struct wlan_bssid_ex *)pbuf;
 
-	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("rtw_survey_event_cb23a, ssid=%s\n",  pnetwork->Ssid.ssid));
+	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_,("\n ****rtw_survey_event_cb23a: return a wrong bss ***\n"));
+	if (len > (sizeof(struct wlan_bssid_ex))) {
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
+			 ("\n ****rtw_survey_event_cb23a: return a wrong "
+			  "bss ***\n"));
 		return;
 	}
 
 	spin_lock_bh(&pmlmepriv->lock);
 
 	/*  update IBSS_network 's timestamp */
-	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) == true)
-	{
-		/* RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,"rtw_survey_event_cb23a : WIFI_ADHOC_MASTER_STATE\n\n"); */
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+		/* RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+		   "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 = NULL;
+			struct wlan_network* ibss_wlan;
 
-			memcpy(pmlmepriv->cur_network.network.IEs, pnetwork->IEs, 8);
+			memcpy(pmlmepriv->cur_network.network.IEs,
+			       pnetwork->IEs, 8);
 			spin_lock_bh(&pmlmepriv->scanned_queue.lock);
-			ibss_wlan = rtw_find_network23a(&pmlmepriv->scanned_queue,  pnetwork->MacAddress);
-			if (ibss_wlan)
-			{
-				memcpy(ibss_wlan->network.IEs , pnetwork->IEs, 8);
+			ibss_wlan = rtw_find_network23a(
+				&pmlmepriv->scanned_queue,
+				pnetwork->MacAddress);
+			if (ibss_wlan) {
+				memcpy(ibss_wlan->network.IEs,
+				       pnetwork->IEs, 8);
 				spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 				goto exit;
 			}
@@ -681,8 +682,7 @@ void rtw_survey_event_cb23a(struct rtw_adapter	*adapter, u8 *pbuf)
 	}
 
 	/*  lock pmlmepriv->lock when you accessing network_q */
-	if ((check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) == false)
-	{
+	if (!check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) {
 	        if (pnetwork->Ssid.ssid[0] == 0)
 			pnetwork->Ssid.ssid_len = 0;
 
@@ -696,10 +696,13 @@ exit:
 	return;
 }
 
-void rtw_surveydone_event_callback23a(struct rtw_adapter	*adapter, u8 *pbuf)
+void
+rtw_surveydone_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf)
 {
 	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
 	struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
+	struct wlan_bssid_ex *pdev_network;
+	u8 *pibss;
 
 	spin_lock_bh(&pmlmepriv->lock);
 
@@ -709,47 +712,62 @@ void rtw_surveydone_event_callback23a(struct rtw_adapter	*adapter, u8 *pbuf)
 		pmlmepriv->wps_probe_req_ie = NULL;
 	}
 
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_surveydone_event_callback23a: fw_state:%x\n\n", get_fwstate(pmlmepriv)));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+		 ("rtw_surveydone_event_callback23a: fw_state:%x\n\n",
+		  get_fwstate(pmlmepriv)));
 
 	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)) {
 		del_timer_sync(&pmlmepriv->scan_to_timer);
 
 		_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
 	} else {
-
-		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("nic status =%x, survey done event comes too late!\n", get_fwstate(pmlmepriv)));
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+			 ("nic status =%x, survey done event comes too late!\n",
+			  get_fwstate(pmlmepriv)));
 	}
 
 	rtw_set_signal_stat_timer(&adapter->recvpriv);
 
 	if (pmlmepriv->to_join == true) {
-		if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true)) {
-			if (check_fwstate(pmlmepriv, _FW_LINKED) == false) {
+		if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+			if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
 				set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
 
-				if (rtw_select_and_join_from_scanned_queue23a(pmlmepriv) == _SUCCESS) {
+				if (rtw_select_and_join_from_scanned_queue23a(
+					    pmlmepriv) == _SUCCESS) {
 					mod_timer(&pmlmepriv->assoc_timer,
 						  jiffies + msecs_to_jiffies(MAX_JOIN_TIMEOUT));
 				} else {
-					struct wlan_bssid_ex *pdev_network = &adapter->registrypriv.dev_network;
-					u8 *pibss = adapter->registrypriv.dev_network.MacAddress;
+					pdev_network = &adapter->registrypriv.dev_network;
+					pibss = adapter->registrypriv.dev_network.MacAddress;
 
-					_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+					_clr_fwstate_(pmlmepriv,
+						      _FW_UNDER_SURVEY);
 
-					RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("switching to adhoc master\n"));
+					RT_TRACE(_module_rtl871x_mlme_c_,
+						 _drv_err_,
+						 ("switching to adhoc "
+						  "master\n"));
 
-					memset(&pdev_network->Ssid, 0, sizeof(struct cfg80211_ssid));
-					memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(struct cfg80211_ssid));
+					memset(&pdev_network->Ssid, 0,
+					       sizeof(struct cfg80211_ssid));
+					memcpy(&pdev_network->Ssid,
+					       &pmlmepriv->assoc_ssid,
+					       sizeof(struct cfg80211_ssid));
 
-					rtw_update_registrypriv_dev_network23a(adapter);
+					rtw_update_registrypriv_dev_network23a(
+						adapter);
 					rtw_generate_random_ibss23a(pibss);
 
-					pmlmepriv->fw_state = WIFI_ADHOC_MASTER_STATE;
+					pmlmepriv->fw_state =
+						WIFI_ADHOC_MASTER_STATE;
 
-					if (rtw_createbss_cmd23a(adapter)!= _SUCCESS)
-					{
-					RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Error =>rtw_createbss_cmd23a status FAIL\n"));
-					}
+					if (rtw_createbss_cmd23a(adapter) !=
+					    _SUCCESS)
+					RT_TRACE(_module_rtl871x_mlme_c_,
+						 _drv_err_,
+						 ("Error =>rtw_createbss_cmd23a"
+						  " status FAIL\n"));
 
 					pmlmepriv->to_join = false;
 				}
@@ -758,27 +776,32 @@ void rtw_surveydone_event_callback23a(struct rtw_adapter	*adapter, u8 *pbuf)
 			int ret;
 			set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
 			pmlmepriv->to_join = false;
-			ret = rtw_select_and_join_from_scanned_queue23a(pmlmepriv);
+			ret = rtw_select_and_join_from_scanned_queue23a(
+				pmlmepriv);
 			if (ret == _SUCCESS) {
 				unsigned long e;
 				e = msecs_to_jiffies(MAX_JOIN_TIMEOUT);
 				mod_timer(&pmlmepriv->assoc_timer, jiffies + e);
-			} else if (ret == 2)/* there is no need to wait for join */
-			{
+			} else if (ret == 2) {/* there is no need to wait */
 				_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
 				rtw_indicate_connect23a(adapter);
 			} else {
-				DBG_8723A("try_to_join, but select scanning queue fail, to_roaming:%d\n", rtw_to_roaming(adapter));
-				if (rtw_to_roaming(adapter) != 0) {
-					if (--pmlmepriv->to_roaming == 0
-						|| _SUCCESS != rtw_sitesurvey_cmd23a(adapter, &pmlmepriv->assoc_ssid, 1, NULL, 0)
-					) {
+				DBG_8723A("try_to_join, but select scanning "
+					  "queue fail, to_roaming:%d\n",
+					  adapter->mlmepriv.to_roaming);
+				if (adapter->mlmepriv.to_roaming) {
+					if (--pmlmepriv->to_roaming == 0 ||
+					    rtw_sitesurvey_cmd23a(
+						    adapter,
+						    &pmlmepriv->assoc_ssid, 1,
+						    NULL, 0) != _SUCCESS) {
 						rtw_set_roaming(adapter, 0);
-						rtw_free_assoc_resources23a(adapter, 1);
-						rtw_indicate_disconnect23a(adapter);
-					} else {
+						rtw_free_assoc_resources23a(
+							adapter, 1);
+						rtw_indicate_disconnect23a(
+							adapter);
+					} else
 						pmlmepriv->to_join = true;
-					}
 				}
 				_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
 			}
@@ -787,29 +810,15 @@ void rtw_surveydone_event_callback23a(struct rtw_adapter	*adapter, u8 *pbuf)
 
 	spin_unlock_bh(&pmlmepriv->lock);
 
-#ifdef CONFIG_8723AU_P2P
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-		p2p_ps_wk_cmd23a(adapter, P2P_PS_SCAN_DONE, 0);
-#endif /*  CONFIG_8723AU_P2P */
-
 	rtw_os_xmit_schedule23a(adapter);
 
-	if(pmlmeext->sitesurvey_res.bss_cnt == 0)
-		rtw_hal_sreset_reset23a(adapter);
+	if (pmlmeext->sitesurvey_res.bss_cnt == 0)
+		rtw_sreset_reset(adapter);
 
 	rtw_cfg80211_surveydone_event_callback(adapter);
-
-}
-
-void rtw_dummy_event_callback23a(struct rtw_adapter *adapter , u8 *pbuf)
-{
 }
 
-void rtw23a_fwdbg_event_callback(struct rtw_adapter *adapter , u8 *pbuf)
-{
-}
-
-static void free_scanqueue(struct	mlme_priv *pmlmepriv)
+static void free_scanqueue(struct mlme_priv *pmlmepriv)
 {
 	struct wlan_network *pnetwork;
 	struct rtw_queue *scan_queue = &pmlmepriv->scanned_queue;
@@ -827,42 +836,38 @@ static void free_scanqueue(struct	mlme_priv *pmlmepriv)
         }
 
 	spin_unlock_bh(&scan_queue->lock);
-
 }
 
 /*
-*rtw_free_assoc_resources23a: the caller has to lock pmlmepriv->lock
-*/
-void rtw_free_assoc_resources23a(struct rtw_adapter *adapter, int lock_scanned_queue)
+ *rtw_free_assoc_resources23a: the caller has to lock pmlmepriv->lock
+ */
+void rtw_free_assoc_resources23a(struct rtw_adapter *adapter,
+				 int lock_scanned_queue)
 {
-	struct wlan_network* pwlan = NULL;
-	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
-	struct	sta_priv *pstapriv = &adapter->stapriv;
+	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;
 
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, ("+rtw_free_assoc_resources23a\n"));
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("tgt_network->network.MacAddress="MAC_FMT" ssid=%s\n",
-		MAC_ARG(tgt_network->network.MacAddress), tgt_network->network.Ssid.ssid));
-
-	if(check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_AP_STATE))
-	{
-		struct sta_info* psta;
-
-		psta = rtw_get_stainfo23a(&adapter->stapriv, tgt_network->network.MacAddress);
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
+		 ("+rtw_free_assoc_resources23a\n"));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+		 ("tgt_network->network.MacAddress="MAC_FMT" ssid=%s\n",
+		  MAC_ARG(tgt_network->network.MacAddress),
+		  tgt_network->network.Ssid.ssid));
 
-		{
-			spin_lock_bh(&pstapriv->sta_hash_lock);
-			rtw_free_stainfo23a(adapter,  psta);
-		}
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_AP_STATE)) {
+		psta = rtw_get_stainfo23a(&adapter->stapriv,
+					  tgt_network->network.MacAddress);
 
+		spin_lock_bh(&pstapriv->sta_hash_lock);
+		rtw_free_stainfo23a(adapter,  psta);
 		spin_unlock_bh(&pstapriv->sta_hash_lock);
-
 	}
 
-	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE))
-	{
-		struct sta_info* psta;
-
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE |
+			  WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE)) {
 		rtw_free_all_stainfo23a(adapter);
 
 		psta = rtw_get_bcmc_stainfo23a(adapter);
@@ -873,23 +878,25 @@ void rtw_free_assoc_resources23a(struct rtw_adapter *adapter, int lock_scanned_q
 		rtw_init_bcmc_stainfo23a(adapter);
 	}
 
-	if(lock_scanned_queue)
+	if (lock_scanned_queue)
 		spin_lock_bh(&pmlmepriv->scanned_queue.lock);
 
-	pwlan = rtw_find_network23a(&pmlmepriv->scanned_queue, tgt_network->network.MacAddress);
-	if(pwlan)
+	pwlan = rtw_find_network23a(&pmlmepriv->scanned_queue,
+				    tgt_network->network.MacAddress);
+	if (pwlan)
 		pwlan->fixed = false;
 	else
-		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("rtw_free_assoc_resources23a : pwlan== NULL\n\n"));
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
+			 ("rtw_free_assoc_resources23a : pwlan== NULL\n"));
 
-	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) && (adapter->stapriv.asoc_sta_count == 1))
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) &&
+	    adapter->stapriv.asoc_sta_count == 1)
 		rtw_free_network_nolock(pmlmepriv, pwlan);
 
-	if(lock_scanned_queue)
+	if (lock_scanned_queue)
 		spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 
 	pmlmepriv->key_mask = 0;
-
 }
 
 /*
@@ -897,46 +904,54 @@ void rtw_free_assoc_resources23a(struct rtw_adapter *adapter, int lock_scanned_q
 */
 void rtw_indicate_connect23a(struct rtw_adapter *padapter)
 {
-	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("+rtw_indicate_connect23a\n"));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+		 ("+rtw_indicate_connect23a\n"));
 
 	pmlmepriv->to_join = false;
 
-	if(!check_fwstate(&padapter->mlmepriv, _FW_LINKED)) {
+	if (!check_fwstate(&padapter->mlmepriv, _FW_LINKED)) {
 		set_fwstate(pmlmepriv, _FW_LINKED);
 
 		rtw_led_control(padapter, LED_CTL_LINK);
 
-		rtw_os_indicate_connect23a(padapter);
+		rtw_cfg80211_indicate_connect(padapter);
+
+		netif_carrier_on(padapter->pnetdev);
+
+		if (padapter->pid[2] != 0)
+			kill_pid(find_vpid(padapter->pid[2]), SIGALRM, 1);
 	}
 
 	rtw_set_roaming(padapter, 0);
 
 	rtw_set_scan_deny(padapter, 3000);
 
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("-rtw_indicate_connect23a: fw_state=0x%08x\n", get_fwstate(pmlmepriv)));
-
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+		 ("-rtw_indicate_connect23a: fw_state=0x%08x\n",
+		  get_fwstate(pmlmepriv)));
 }
 
 /*
-*rtw_indicate_disconnect23a: the caller has to lock pmlmepriv->lock
-*/
+ *rtw_indicate_disconnect23a: the caller has to lock pmlmepriv->lock
+ */
 void rtw_indicate_disconnect23a(struct rtw_adapter *padapter)
 {
 	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("+rtw_indicate_disconnect23a\n"));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+		 ("+rtw_indicate_disconnect23a\n"));
 
 	_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING|WIFI_UNDER_WPS);
 
         /* DBG_8723A("clear wps when %s\n", __func__); */
 
-	if (rtw_to_roaming(padapter) > 0)
+	if (padapter->mlmepriv.to_roaming > 0)
 		_clr_fwstate_(pmlmepriv, _FW_LINKED);
 
 	if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) ||
-	    (rtw_to_roaming(padapter) <= 0)) {
+	    padapter->mlmepriv.to_roaming <= 0) {
 		rtw_os_indicate_disconnect23a(padapter);
 
 		/* set ips_deny_time to avoid enter IPS before LPS leave */
@@ -951,17 +966,7 @@ void rtw_indicate_disconnect23a(struct rtw_adapter *padapter)
 
 	}
 
-#ifdef CONFIG_8723AU_P2P
-	p2p_ps_wk_cmd23a(padapter, P2P_PS_DISABLE, 1);
-#endif /*  CONFIG_8723AU_P2P */
-
 	rtw_lps_ctrl_wk_cmd23a(padapter, LPS_CTRL_DISCONNECT, 1);
-
-}
-
-inline void rtw_indicate_scan_done23a(struct rtw_adapter *padapter, bool aborted)
-{
-	rtw_os_indicate_scan_done23a(padapter, aborted);
 }
 
 void rtw_scan_abort23a(struct rtw_adapter *adapter)
@@ -974,96 +979,105 @@ void rtw_scan_abort23a(struct rtw_adapter *adapter)
 	pmlmeext->scan_abort = true;
 	while (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) &&
 	       jiffies_to_msecs(jiffies - start) <= 200) {
-
 		if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
 			break;
 
-		DBG_8723A(FUNC_NDEV_FMT"fw_state = _FW_UNDER_SURVEY!\n", FUNC_NDEV_ARG(adapter->pnetdev));
+		DBG_8723A("%s(%s): fw_state = _FW_UNDER_SURVEY!\n",
+			  __func__, adapter->pnetdev->name);
 		msleep(20);
 	}
 
 	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)) {
 		if (!adapter->bDriverStopped && !adapter->bSurpriseRemoved)
-			DBG_8723A(FUNC_NDEV_FMT"waiting for scan_abort time out!\n", FUNC_NDEV_ARG(adapter->pnetdev));
-		rtw_indicate_scan_done23a(adapter, true);
+			DBG_8723A("%s(%s): waiting for scan_abort time out!\n",
+				  __func__, adapter->pnetdev->name);
+		rtw_cfg80211_indicate_scan_done(wdev_to_priv(adapter->rtw_wdev),
+						true);
 	}
 	pmlmeext->scan_abort = false;
 }
 
-static struct sta_info *rtw_joinbss_update_stainfo(struct rtw_adapter *padapter, struct wlan_network *pnetwork)
+static struct sta_info *
+rtw_joinbss_update_stainfo(struct rtw_adapter *padapter,
+			   struct wlan_network *pnetwork)
 {
 	int i;
-	struct sta_info *bmc_sta, *psta = NULL;
+	struct sta_info *bmc_sta, *psta;
 	struct recv_reorder_ctrl *preorder_ctrl;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 
 	psta = rtw_get_stainfo23a(pstapriv, pnetwork->network.MacAddress);
-	if (psta == NULL) {
-		psta = rtw_alloc_stainfo23a(pstapriv, pnetwork->network.MacAddress);
-	}
+	if (!psta)
+		psta = rtw_alloc_stainfo23a(pstapriv,
+					    pnetwork->network.MacAddress,
+					    GFP_ATOMIC);
 
-	if (psta) /* update ptarget_sta */
-	{
+	if (psta) { /* update ptarget_sta */
 		DBG_8723A("%s\n", __func__);
 
 		psta->aid  = pnetwork->join_res;
-			psta->mac_id = 0;
+		psta->mac_id = 0;
 
 		/* sta mode */
-		rtw_hal_set_odm_var23a(padapter, HAL_ODM_STA_INFO, psta, true);
+		rtl8723a_SetHalODMVar(padapter, HAL_ODM_STA_INFO, psta, true);
 
 		/* security related */
-		if (padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X)
-		{
-			padapter->securitypriv.binstallGrpkey = false;
-			padapter->securitypriv.busetkipkey = false;
-			padapter->securitypriv.bgrpkey_handshake = false;
+		if (padapter->securitypriv.dot11AuthAlgrthm ==
+		    dot11AuthAlgrthm_8021X) {
+			padapter->securitypriv.binstallGrpkey = 0;
+			padapter->securitypriv.busetkipkey = 0;
 
 			psta->ieee8021x_blocked = true;
-			psta->dot118021XPrivacy = padapter->securitypriv.dot11PrivacyAlgrthm;
+			psta->dot118021XPrivacy =
+				padapter->securitypriv.dot11PrivacyAlgrthm;
 
-			memset((u8 *)&psta->dot118021x_UncstKey, 0, sizeof (union Keytype));
+			memset(&psta->dot118021x_UncstKey, 0,
+			       sizeof (union Keytype));
 
-			memset((u8 *)&psta->dot11tkiprxmickey, 0, sizeof (union Keytype));
-			memset((u8 *)&psta->dot11tkiptxmickey, 0, sizeof (union Keytype));
+			memset(&psta->dot11tkiprxmickey, 0,
+			       sizeof (union Keytype));
+			memset(&psta->dot11tkiptxmickey, 0,
+			       sizeof (union Keytype));
 
-			memset((u8 *)&psta->dot11txpn, 0, sizeof (union pn48));
-			memset((u8 *)&psta->dot11rxpn, 0, sizeof (union pn48));
+			memset(&psta->dot11txpn, 0, sizeof (union pn48));
+			memset(&psta->dot11rxpn, 0, sizeof (union pn48));
 		}
 
 		/*	Commented by Albert 2012/07/21 */
 		/*	When doing the WPS, the wps_ie_len won't equal to 0 */
-		/*	And the Wi-Fi driver shouldn't allow the data packet to be tramsmitted. */
-		if (padapter->securitypriv.wps_ie_len != 0)
-		{
+		/*	And the Wi-Fi driver shouldn't allow the data packet
+			to be tramsmitted. */
+		if (padapter->securitypriv.wps_ie_len != 0) {
 			psta->ieee8021x_blocked = true;
 			padapter->securitypriv.wps_ie_len = 0;
 		}
 
-		/* for A-MPDU Rx reordering buffer control for bmc_sta & sta_info */
-		/* if A-MPDU Rx is enabled, reseting  rx_ordering_ctrl wstart_b(indicate_seq) to default value = 0xffff */
+		/* for A-MPDU Rx reordering buffer control for bmc_sta &
+		 * sta_info */
+		/* if A-MPDU Rx is enabled, reseting
+		   rx_ordering_ctrl wstart_b(indicate_seq) to default
+		   value = 0xffff */
 		/* todo: check if AP can send A-MPDU packets */
-		for (i = 0; i < 16 ; i++)
-		{
+		for (i = 0; i < 16 ; i++) {
 			/* preorder_ctrl = &precvpriv->recvreorder_ctrl[i]; */
 			preorder_ctrl = &psta->recvreorder_ctrl[i];
 			preorder_ctrl->enable = false;
 			preorder_ctrl->indicate_seq = 0xffff;
 			preorder_ctrl->wend_b = 0xffff;
-			preorder_ctrl->wsize_b = 64;/* max_ampdu_sz; ex. 32(kbytes) -> wsize_b = 32 */
+			/* max_ampdu_sz; ex. 32(kbytes) -> wsize_b = 32 */
+			preorder_ctrl->wsize_b = 64;
 		}
 
 		bmc_sta = rtw_get_bcmc_stainfo23a(padapter);
-		if (bmc_sta)
-		{
-			for (i = 0; i < 16 ; i++)
-			{
-				/* preorder_ctrl = &precvpriv->recvreorder_ctrl[i]; */
+		if (bmc_sta) {
+			for (i = 0; i < 16 ; i++) {
 				preorder_ctrl = &bmc_sta->recvreorder_ctrl[i];
 				preorder_ctrl->enable = false;
 				preorder_ctrl->indicate_seq = 0xffff;
 				preorder_ctrl->wend_b = 0xffff;
-				preorder_ctrl->wsize_b = 64;/* max_ampdu_sz; ex. 32(kbytes) -> wsize_b = 32 */
+				/* max_ampdu_sz; ex. 32(kbytes) ->
+				   wsize_b = 32 */
+				preorder_ctrl->wsize_b = 64;
 			}
 		}
 
@@ -1077,81 +1091,108 @@ static struct sta_info *rtw_joinbss_update_stainfo(struct rtw_adapter *padapter,
 
 /* pnetwork : returns from rtw23a_joinbss_event_cb */
 /* ptarget_wlan: found from scanned_queue */
-static void rtw_joinbss_update_network23a(struct rtw_adapter *padapter, struct wlan_network *ptarget_wlan, struct wlan_network  *pnetwork)
+static void
+rtw_joinbss_update_network23a(struct rtw_adapter *padapter,
+			      struct wlan_network *ptarget_wlan,
+			      struct wlan_network  *pnetwork)
 {
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct wlan_network *cur_network = &pmlmepriv->cur_network;
+	int bcn_size;
 
 	DBG_8723A("%s\n", __func__);
 
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("\nfw_state:%x, BSSID:"MAC_FMT"\n"
-		, get_fwstate(pmlmepriv), MAC_ARG(pnetwork->network.MacAddress)));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+		 ("\nfw_state:%x, BSSID:"MAC_FMT"\n", get_fwstate(pmlmepriv),
+		  MAC_ARG(pnetwork->network.MacAddress)));
 
 	/*  why not use ptarget_wlan?? */
-	memcpy(&cur_network->network, &pnetwork->network, pnetwork->network.Length);
+	memcpy(&cur_network->network, &pnetwork->network,
+	       pnetwork->network.Length);
 	/*  some IEs in pnetwork is wrong, so we should use ptarget_wlan IEs */
 	cur_network->network.IELength = ptarget_wlan->network.IELength;
-	memcpy(&cur_network->network.IEs[0], &ptarget_wlan->network.IEs[0], MAX_IE_SZ);
+	memcpy(&cur_network->network.IEs[0], &ptarget_wlan->network.IEs[0],
+	       MAX_IE_SZ);
 
 	cur_network->aid = pnetwork->join_res;
 
 	rtw_set_signal_stat_timer(&padapter->recvpriv);
-	padapter->recvpriv.signal_strength = ptarget_wlan->network.PhyInfo.SignalStrength;
-	padapter->recvpriv.signal_qual = ptarget_wlan->network.PhyInfo.SignalQuality;
-	/* the ptarget_wlan->network.Rssi is raw data, we use ptarget_wlan->network.PhyInfo.SignalStrength instead (has scaled) */
-	padapter->recvpriv.rssi = translate_percentage_to_dbm(ptarget_wlan->network.PhyInfo.SignalStrength);
+	padapter->recvpriv.signal_strength =
+		ptarget_wlan->network.PhyInfo.SignalStrength;
+	padapter->recvpriv.signal_qual =
+		ptarget_wlan->network.PhyInfo.SignalQuality;
+	/*
+	 * the ptarget_wlan->network.Rssi is raw data, we use
+	 * ptarget_wlan->network.PhyInfo.SignalStrength instead (has scaled)
+	 */
+	padapter->recvpriv.rssi = translate_percentage_to_dbm(
+		ptarget_wlan->network.PhyInfo.SignalStrength);
 	DBG_8723A("%s signal_strength:%3u, rssi:%3d, signal_qual:%3u\n",
 		  __func__, padapter->recvpriv.signal_strength,
 		  padapter->recvpriv.rssi, padapter->recvpriv.signal_qual);
 	rtw_set_signal_stat_timer(&padapter->recvpriv);
 
 	/* update fw_state will clr _FW_UNDER_LINKING here indirectly */
-	switch (pnetwork->network.InfrastructureMode) {
-	case Ndis802_11Infrastructure:
-		if (pmlmepriv->fw_state&WIFI_UNDER_WPS)
+	switch (pnetwork->network.ifmode) {
+	case NL80211_IFTYPE_P2P_CLIENT:
+	case NL80211_IFTYPE_STATION:
+		if (pmlmepriv->fw_state & WIFI_UNDER_WPS)
 			pmlmepriv->fw_state = WIFI_STATION_STATE|WIFI_UNDER_WPS;
 		else
 			pmlmepriv->fw_state = WIFI_STATION_STATE;
 		break;
-	case Ndis802_11IBSS:
+	case NL80211_IFTYPE_ADHOC:
 		pmlmepriv->fw_state = WIFI_ADHOC_STATE;
 		break;
 	default:
 		pmlmepriv->fw_state = WIFI_NULL_STATE;
-		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Invalid network_mode\n"));
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+			 ("Invalid network_mode\n"));
 		break;
 	}
 
-	rtw_update_protection23a(padapter, (cur_network->network.IEs) + sizeof (struct ndis_802_11_fixed_ies),
-									(cur_network->network.IELength));
+	bcn_size = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+		offsetof(struct ieee80211_mgmt, u.beacon);
 
-	rtw_update_ht_cap23a(padapter, cur_network->network.IEs, cur_network->network.IELength);
+	rtw_update_protection23a(padapter, cur_network->network.IEs +
+				 bcn_size, cur_network->network.IELength);
+
+	rtw_update_ht_cap23a(padapter, cur_network->network.IEs,
+			     cur_network->network.IELength);
 }
 
-/* Notes: the fucntion could be > passive_level (the same context as Rx tasklet) */
-/* pnetwork : returns from rtw23a_joinbss_event_cb */
-/* ptarget_wlan: found from scanned_queue */
-/* if join_res > 0, for (fw_state==WIFI_STATION_STATE), we check if  "ptarget_sta" & "ptarget_wlan" exist. */
-/* if join_res > 0, for (fw_state==WIFI_ADHOC_STATE), we only check if "ptarget_wlan" exist. */
-/* if join_res > 0, update "cur_network->network" from "pnetwork->network" if (ptarget_wlan !=NULL). */
+/*
+ * Notes:
+ * the fucntion could be > passive_level (the same context as Rx tasklet)
+ * pnetwork : returns from rtw23a_joinbss_event_cb
+ * ptarget_wlan: found from scanned_queue
+ * if join_res > 0, for (fw_state==WIFI_STATION_STATE),
+ * we check if  "ptarget_sta" & "ptarget_wlan" exist.
+ * if join_res > 0, for (fw_state==WIFI_ADHOC_STATE),
+ * we only check if "ptarget_wlan" exist.
+ * if join_res > 0, update "cur_network->network" from "pnetwork->network"
+ * if (ptarget_wlan !=NULL).
+ */
 
 void rtw_joinbss_event_prehandle23a(struct rtw_adapter *adapter, u8 *pbuf)
 {
-	static u8 retry=0;
-	struct sta_info *ptarget_sta= NULL, *pcur_sta = NULL;
-	struct	sta_priv *pstapriv = &adapter->stapriv;
-	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
-	struct wlan_network	*pnetwork	= (struct wlan_network *)pbuf;
+	struct sta_info *ptarget_sta, *pcur_sta;
+	struct sta_priv *pstapriv = &adapter->stapriv;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct wlan_network *pnetwork = (struct wlan_network *)pbuf;
 	struct wlan_network *cur_network = &pmlmepriv->cur_network;
-	struct wlan_network	*pcur_wlan = NULL, *ptarget_wlan = NULL;
-	unsigned int		the_same_macaddr = false;
+	struct wlan_network *pcur_wlan, *ptarget_wlan = NULL;
+	bool the_same_macaddr;
 
-	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("joinbss event call back received with res=%d\n", pnetwork->join_res));
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,
+		 ("joinbss event call back received with res=%d\n",
+		  pnetwork->join_res));
 
 	rtw_get_encrypt_decrypt_from_registrypriv23a(adapter);
 
 	if (pmlmepriv->assoc_ssid.ssid_len == 0) {
-		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("@@@@@   joinbss event call back  for Any SSid\n"));
+		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_,
 			 ("@@@@@   rtw23a_joinbss_event_cb for SSid:%s\n",
@@ -1165,93 +1206,102 @@ void rtw_joinbss_event_prehandle23a(struct rtw_adapter *adapter, u8 *pbuf)
 		the_same_macaddr = false;
 
 	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_,("\n\n ***joinbss_evt_callback return a wrong bss ***\n\n"));
+	if (pnetwork->network.Length > sizeof(struct wlan_bssid_ex)) {
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
+			 ("\n\n ***joinbss_evt_callback return a wrong bss "
+			  "***\n\n"));
 		return;
 	}
 
 	spin_lock_bh(&pmlmepriv->lock);
 
-	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("\n rtw23a_joinbss_event_cb !! _enter_critical\n"));
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,
+		 ("\n rtw23a_joinbss_event_cb !! _enter_critical\n"));
 
-	if(pnetwork->join_res > 0)
-	{
+	if (pnetwork->join_res > 0) {
 		spin_lock_bh(&pmlmepriv->scanned_queue.lock);
-		retry = 0;
-		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)
-				{
+			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+				if (the_same_macaddr == true) {
 					ptarget_wlan = rtw_find_network23a(&pmlmepriv->scanned_queue, cur_network->network.MacAddress);
-				}
-				else
-				{
+				} else {
 					pcur_wlan = rtw_find_network23a(&pmlmepriv->scanned_queue, cur_network->network.MacAddress);
-					if(pcur_wlan)	pcur_wlan->fixed = false;
+					if (pcur_wlan)
+						pcur_wlan->fixed = false;
 
 					pcur_sta = rtw_get_stainfo23a(pstapriv, cur_network->network.MacAddress);
-					if(pcur_sta) {
+					if (pcur_sta) {
 						spin_lock_bh(&pstapriv->sta_hash_lock);
-						rtw_free_stainfo23a(adapter,  pcur_sta);
+						rtw_free_stainfo23a(adapter,
+								    pcur_sta);
 						spin_unlock_bh(&pstapriv->sta_hash_lock);
 					}
 
 					ptarget_wlan = rtw_find_network23a(&pmlmepriv->scanned_queue, pnetwork->network.MacAddress);
-					if(check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
-						if(ptarget_wlan)	ptarget_wlan->fixed = true;
+					if (check_fwstate(pmlmepriv,
+							  WIFI_STATION_STATE)) {
+						if (ptarget_wlan)
+							ptarget_wlan->fixed =
+								true;
 					}
 				}
 
-			}
-			else
-			{
-				ptarget_wlan = rtw_find_network23a(&pmlmepriv->scanned_queue, pnetwork->network.MacAddress);
-				if(check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
-					if(ptarget_wlan)	ptarget_wlan->fixed = true;
+			} else {
+				ptarget_wlan = rtw_find_network23a(
+					&pmlmepriv->scanned_queue,
+					pnetwork->network.MacAddress);
+				if (check_fwstate(pmlmepriv,
+						  WIFI_STATION_STATE)) {
+					if (ptarget_wlan)
+						ptarget_wlan->fixed = true;
 				}
 			}
 
 			/* s2. update cur_network */
-			if(ptarget_wlan)
-			{
-				rtw_joinbss_update_network23a(adapter, ptarget_wlan, pnetwork);
-			}
-			else
-			{
-				RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("Can't find ptarget_wlan when joinbss_event callback\n"));
+			if (ptarget_wlan)
+				rtw_joinbss_update_network23a(adapter,
+							      ptarget_wlan,
+							      pnetwork);
+			else {
+				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);
 				goto ignore_joinbss_callback;
 			}
 
-			/* s3. find ptarget_sta & update ptarget_sta after update cur_network only for station mode */
-			if(check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true)
-			{
-				ptarget_sta = rtw_joinbss_update_stainfo(adapter, pnetwork);
-				if(ptarget_sta==NULL)
-				{
-					RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("Can't update stainfo when joinbss_event callback\n"));
+			/* s3. find ptarget_sta & update ptarget_sta after
+			   update cur_network only for station mode */
+			if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+				ptarget_sta = rtw_joinbss_update_stainfo(
+					adapter, pnetwork);
+				if (!ptarget_sta) {
+					RT_TRACE(_module_rtl871x_mlme_c_,
+						 _drv_err_,
+						 ("Can't update stainfo when "
+						  "joinbss_event callback\n"));
 					spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 					goto ignore_joinbss_callback;
 				}
 			}
 
 			/* s4. indicate connect */
-			if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true)
-			{
+			if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
 				rtw_indicate_connect23a(adapter);
-			} else {
-					/* adhoc mode will rtw_indicate_connect23a when rtw_stassoc_event_callback23a */
-				RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("adhoc mode, fw_state:%x", get_fwstate(pmlmepriv)));
+			else {
+				/* adhoc mode will rtw_indicate_connect23a
+				   when rtw_stassoc_event_callback23a */
+				RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,
+					 ("adhoc mode, fw_state:%x",
+					  get_fwstate(pmlmepriv)));
 			}
 
 			/* s5. Cancle assoc_timer */
 			del_timer_sync(&pmlmepriv->assoc_timer);
 
-			RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("Cancle assoc_timer\n"));
+			RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,
+				 ("Cancle assoc_timer\n"));
 		} else {
 			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
 				 ("rtw23a_joinbss_event_cb err: fw_state:%x",
@@ -1260,20 +1310,19 @@ void rtw_joinbss_event_prehandle23a(struct rtw_adapter *adapter, u8 *pbuf)
 			goto ignore_joinbss_callback;
 		}
 		spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
-	} else if(pnetwork->join_res == -4) {
+	} else if (pnetwork->join_res == -4) {
 		rtw_reset_securitypriv23a(adapter);
 		mod_timer(&pmlmepriv->assoc_timer,
 			  jiffies + msecs_to_jiffies(1));
 
 		/* rtw_free_assoc_resources23a(adapter, 1); */
 
-		if((check_fwstate(pmlmepriv, _FW_UNDER_LINKING))) {
+		if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) {
 			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-				 ("fail! clear _FW_UNDER_LINKING ^^^fw_state=%x\n",
-				 get_fwstate(pmlmepriv)));
+				 ("fail! clear _FW_UNDER_LINKING ^^^fw_state="
+				  "%x\n", get_fwstate(pmlmepriv)));
 			_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
 		}
-
 	} else {
 		/* if join_res < 0 (join fails), then try again */
 		mod_timer(&pmlmepriv->assoc_timer,
@@ -1286,48 +1335,33 @@ ignore_joinbss_callback:
 	spin_unlock_bh(&pmlmepriv->lock);
 }
 
-void rtw23a_joinbss_event_cb(struct rtw_adapter *adapter, u8 *pbuf)
+void rtw23a_joinbss_event_cb(struct rtw_adapter *adapter, const u8 *pbuf)
 {
-	struct wlan_network	*pnetwork	= (struct wlan_network *)pbuf;
+	struct wlan_network *pnetwork = (struct wlan_network *)pbuf;
 
 	mlmeext_joinbss_event_callback23a(adapter, pnetwork->join_res);
 
 	rtw_os_xmit_schedule23a(adapter);
-
-}
-
-/* FOR AP , AD-HOC mode */
-void rtw_stassoc_hw_rpt23a(struct rtw_adapter *adapter, struct sta_info *psta)
-{
-	u16 media_status;
-
-	if (psta == NULL)	return;
-
-	media_status = (psta->mac_id<<8)|1; /*   MACID|OPMODE:1 connect */
-	rtw_hal_set_hwreg23a(adapter, HW_VAR_H2C_MEDIA_STATUS_RPT, (u8 *)&media_status);
 }
 
-void rtw_stassoc_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf)
+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 = NULL;
+	struct wlan_network *ptarget_wlan;
 
-	if(rtw_access_ctrl23a(adapter, pstassoc->macaddr) == false)
+	if (rtw_access_ctrl23a(adapter, pstassoc->macaddr) == false)
 		return;
 
 #ifdef CONFIG_8723AU_AP_MODE
-	if(check_fwstate(pmlmepriv, WIFI_AP_STATE))
-	{
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
 		psta = rtw_get_stainfo23a(&adapter->stapriv, pstassoc->macaddr);
 		if (psta) {
 			/* bss_cap_update_on_sta_join23a(adapter, psta); */
 			/* sta_info_update23a(adapter, psta); */
 			ap_sta_info_defer_update23a(adapter, psta);
-
-			rtw_stassoc_hw_rpt23a(adapter,psta);
 		}
 		return;
 	}
@@ -1336,13 +1370,20 @@ void rtw_stassoc_event_callback23a(struct rtw_adapter *adapter, 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_,("Error: rtw_stassoc_event_callback23a: sta has been in sta_hash_queue\n"));
-		return; /* between drv has received this event before and  fw have not yet to set key to CAM_ENTRY) */
+		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
+		   fw have not yet to set key to CAM_ENTRY) */
+		return;
 	}
 
-	psta = rtw_alloc_stainfo23a(&adapter->stapriv, pstassoc->macaddr);
-	if (psta == NULL) {
-		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("Can't alloc sta_info when rtw_stassoc_event_callback23a\n"));
+	psta = rtw_alloc_stainfo23a(&adapter->stapriv, pstassoc->macaddr,
+		GFP_KERNEL);
+	if (!psta) {
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
+			 ("Can't alloc sta_info when "
+			  "rtw_stassoc_event_callback23a\n"));
 		return;
 	}
 
@@ -1352,25 +1393,25 @@ void rtw_stassoc_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf)
 	/* psta->aid = (uint)pstassoc->cam_id; */
 	DBG_8723A("%s\n",__func__);
 	/* for ad-hoc mode */
-	rtw_hal_set_odm_var23a(adapter,HAL_ODM_STA_INFO,psta,true);
+	rtl8723a_SetHalODMVar(adapter, HAL_ODM_STA_INFO, psta, true);
 
-	rtw_stassoc_hw_rpt23a(adapter,psta);
-
-	if(adapter->securitypriv.dot11AuthAlgrthm==dot11AuthAlgrthm_8021X)
-		psta->dot118021XPrivacy = adapter->securitypriv.dot11PrivacyAlgrthm;
+	if (adapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X)
+		psta->dot118021XPrivacy =
+			adapter->securitypriv.dot11PrivacyAlgrthm;
 
 	psta->ieee8021x_blocked = false;
 
 	spin_lock_bh(&pmlmepriv->lock);
 
-	if ( (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)==true ) ||
-		(check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)==true ) )
-	{
-		if(adapter->stapriv.asoc_sta_count== 2)
-		{
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+		if (adapter->stapriv.asoc_sta_count == 2) {
 			spin_lock_bh(&pmlmepriv->scanned_queue.lock);
-			ptarget_wlan = rtw_find_network23a(&pmlmepriv->scanned_queue, cur_network->network.MacAddress);
-			if(ptarget_wlan)	ptarget_wlan->fixed = true;
+			ptarget_wlan =
+				rtw_find_network23a(&pmlmepriv->scanned_queue,
+						    cur_network->network.MacAddress);
+			if (ptarget_wlan)
+				ptarget_wlan->fixed = true;
 			spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 			/*  a sta + bc/mc_stainfo (not Ibss_stainfo) */
 			rtw_indicate_connect23a(adapter);
@@ -1382,49 +1423,42 @@ void rtw_stassoc_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf)
 	mlmeext_sta_add_event_callback23a(adapter, psta);
 }
 
-void rtw_stadel_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf)
+void rtw_stadel_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf)
 {
-	int mac_id=-1;
+	int mac_id;
 	struct sta_info *psta;
-	struct wlan_network* pwlan = NULL;
-	struct wlan_bssid_ex    *pdev_network=NULL;
-	u8* pibss = NULL;
-	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
-	struct	stadel_event *pstadel	= (struct stadel_event*)pbuf;
-	struct	sta_priv *pstapriv = &adapter->stapriv;
+	struct wlan_network* pwlan;
+	struct wlan_bssid_ex *pdev_network;
+	u8 *pibss;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct stadel_event *pstadel = (struct stadel_event *)pbuf;
+	struct sta_priv *pstapriv = &adapter->stapriv;
 	struct wlan_network *tgt_network = &pmlmepriv->cur_network;
 
 	psta = rtw_get_stainfo23a(&adapter->stapriv, pstadel->macaddr);
-	if(psta)
+	if (psta)
 		mac_id = psta->mac_id;
 	else
 		mac_id = pstadel->mac_id;
 
-	DBG_8723A("%s(mac_id=%d)=" MAC_FMT "\n", __func__, mac_id, MAC_ARG(pstadel->macaddr));
-
-	if(mac_id>=0) {
-		u16 media_status;
-		media_status = (mac_id<<8)|0; /*   MACID|OPMODE:0 means disconnect */
-		/* for STA,AP,ADHOC mode, report disconnect stauts to FW */
-		rtw_hal_set_hwreg23a(adapter, HW_VAR_H2C_MEDIA_STATUS_RPT, (u8 *)&media_status);
-	}
+	DBG_8723A("%s(mac_id=%d)=" MAC_FMT "\n", __func__, mac_id,
+		  MAC_ARG(pstadel->macaddr));
 
         if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
-        {
 		return;
-        }
 
 	mlmeext_sta_del_event_callback23a(adapter);
 
 	spin_lock_bh(&pmlmepriv->lock);
 
-	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
-	{
-		if (rtw_to_roaming(adapter) > 0)
-			pmlmepriv->to_roaming--; /* this stadel_event is caused by roaming, decrease to_roaming */
-		else if (rtw_to_roaming(adapter) == 0)
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+		if (adapter->mlmepriv.to_roaming > 0) {
+			/* this stadel_event is caused by roaming,
+			   decrease to_roaming */
+			pmlmepriv->to_roaming--;
+		} else if (adapter->mlmepriv.to_roaming == 0)
 			rtw_set_roaming(adapter, adapter->registrypriv.max_roaming_times);
-		if (*((unsigned short *)(pstadel->rsvd)) != WLAN_REASON_EXPIRATION_CHK)
+		if (*((u16 *)pstadel->rsvd) != WLAN_REASON_EXPIRATION_CHK)
 			rtw_set_roaming(adapter, 0); /* don't roam */
 
 		rtw_free_uc_swdec_pending_queue23a(adapter);
@@ -1433,32 +1467,33 @@ void rtw_stadel_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf)
 		rtw_indicate_disconnect23a(adapter);
 		spin_lock_bh(&pmlmepriv->scanned_queue.lock);
 		/*  remove the network entry in scanned_queue */
-		pwlan = rtw_find_network23a(&pmlmepriv->scanned_queue, tgt_network->network.MacAddress);
+		pwlan = rtw_find_network23a(&pmlmepriv->scanned_queue,
+					    tgt_network->network.MacAddress);
 		if (pwlan) {
 			pwlan->fixed = false;
 			rtw_free_network_nolock(pmlmepriv, pwlan);
 		}
 		spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 
-		_rtw23a_roaming(adapter, tgt_network);
+		_rtw_roaming(adapter, tgt_network);
 	}
 
 	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) ||
-	      check_fwstate(pmlmepriv, WIFI_ADHOC_STATE))
-	{
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
 
 		spin_lock_bh(&pstapriv->sta_hash_lock);
 		rtw_free_stainfo23a(adapter,  psta);
 		spin_unlock_bh(&pstapriv->sta_hash_lock);
 
-		if (adapter->stapriv.asoc_sta_count == 1) /* a sta + bc/mc_stainfo (not Ibss_stainfo) */
-		{
+		/* a sta + bc/mc_stainfo (not Ibss_stainfo) */
+		if (adapter->stapriv.asoc_sta_count == 1) {
 			spin_lock_bh(&pmlmepriv->scanned_queue.lock);
 			/* free old ibss network */
-			/* pwlan = rtw_find_network23a(&pmlmepriv->scanned_queue, pstadel->macaddr); */
-			pwlan = rtw_find_network23a(&pmlmepriv->scanned_queue, tgt_network->network.MacAddress);
-			if (pwlan)
-			{
+			/* pwlan = rtw_find_network23a(
+			   &pmlmepriv->scanned_queue, pstadel->macaddr); */
+			pwlan = rtw_find_network23a(&pmlmepriv->scanned_queue,
+						    tgt_network->network.MacAddress);
+			if (pwlan) {
 				pwlan->fixed = false;
 				rtw_free_network_nolock(pmlmepriv, pwlan);
 			}
@@ -1467,7 +1502,8 @@ void rtw_stadel_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf)
 			pdev_network = &adapter->registrypriv.dev_network;
 			pibss = adapter->registrypriv.dev_network.MacAddress;
 
-			memcpy(pdev_network, &tgt_network->network, get_wlan_bssid_ex_sz(&tgt_network->network));
+			memcpy(pdev_network, &tgt_network->network,
+			       get_wlan_bssid_ex_sz(&tgt_network->network));
 
 			memset(&pdev_network->Ssid, 0,
 			       sizeof(struct cfg80211_ssid));
@@ -1478,32 +1514,22 @@ void rtw_stadel_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf)
 
 			rtw_generate_random_ibss23a(pibss);
 
-			if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE))
-			{
+			if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
 				set_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
 				_clr_fwstate_(pmlmepriv, WIFI_ADHOC_STATE);
 			}
 
-			if (rtw_createbss_cmd23a(adapter)!= _SUCCESS)
-			{
-
-				RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("***Error =>stadel_event_callback: rtw_createbss_cmd23a status FAIL***\n "));
-
+			if (rtw_createbss_cmd23a(adapter) != _SUCCESS) {
+				RT_TRACE(_module_rtl871x_ioctl_set_c_,
+					 _drv_err_,
+					 ("***Error =>stadel_event_callback: "
+					  "rtw_createbss_cmd23a status "
+					  "FAIL***\n"));
 			}
-
 		}
-
 	}
 
 	spin_unlock_bh(&pmlmepriv->lock);
-
-}
-
-void rtw_cpwm_event_callback23a(struct rtw_adapter *padapter, u8 *pbuf)
-{
-
-	RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("+rtw_cpwm_event_callback23a !!!\n"));
-
 }
 
 /*
@@ -1518,23 +1544,28 @@ void rtw23a_join_to_handler (unsigned long data)
 
 	DBG_8723A("%s, fw_state=%x\n", __func__, get_fwstate(pmlmepriv));
 
-	if(adapter->bDriverStopped ||adapter->bSurpriseRemoved)
+	if (adapter->bDriverStopped ||adapter->bSurpriseRemoved)
 		return;
 
 	spin_lock_bh(&pmlmepriv->lock);
 
-	if (rtw_to_roaming(adapter) > 0) { /* join timeout caused by roaming */
-		while(1) {
+	if (adapter->mlmepriv.to_roaming > 0) {
+		/* join timeout caused by roaming */
+		while (1) {
 			pmlmepriv->to_roaming--;
-			if (rtw_to_roaming(adapter) != 0) { /* try another */
+			if (adapter->mlmepriv.to_roaming != 0) {
+				/* try another */
 				DBG_8723A("%s try another roaming\n", __func__);
-				if (_SUCCESS!= (do_join_r = rtw_do_join23a(adapter))) {
-					DBG_8723A("%s roaming do_join return %d\n", __func__ , do_join_r);
+				do_join_r = rtw_do_join23a(adapter);
+				if (do_join_r != _SUCCESS) {
+					DBG_8723A("%s roaming do_join return "
+						  "%d\n", __func__ , do_join_r);
 					continue;
 				}
 				break;
 			} else {
-				DBG_8723A("%s We've try roaming but fail\n", __func__);
+				DBG_8723A("%s We've try roaming but fail\n",
+					  __func__);
 				rtw_indicate_disconnect23a(adapter);
 				break;
 			}
@@ -1543,7 +1574,8 @@ void rtw23a_join_to_handler (unsigned long data)
 		rtw_indicate_disconnect23a(adapter);
 		free_scanqueue(pmlmepriv);/*  */
 
-		/* indicate disconnect for the case that join_timeout and check_fwstate != FW_LINKED */
+		/* indicate disconnect for the case that join_timeout and
+		   check_fwstate != FW_LINKED */
 		rtw_cfg80211_indicate_disconnect(adapter);
 	}
 
@@ -1560,7 +1592,8 @@ void rtw_scan_timeout_handler23a(unsigned long data)
 	struct rtw_adapter *adapter = (struct rtw_adapter *)data;
 	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
 
-	DBG_8723A(FUNC_ADPT_FMT" fw_state =%x\n", FUNC_ADPT_ARG(adapter), get_fwstate(pmlmepriv));
+	DBG_8723A("%s(%s): fw_state =%x\n", __func__, adapter->pnetdev->name,
+		  get_fwstate(pmlmepriv));
 
 	spin_lock_bh(&pmlmepriv->lock);
 
@@ -1568,7 +1601,7 @@ void rtw_scan_timeout_handler23a(unsigned long data)
 
 	spin_unlock_bh(&pmlmepriv->lock);
 
-	rtw_indicate_scan_done23a(adapter, true);
+	rtw_cfg80211_indicate_scan_done(wdev_to_priv(adapter->rtw_wdev), true);
 }
 
 static void rtw_auto_scan_handler(struct rtw_adapter *padapter)
@@ -1581,7 +1614,8 @@ static void rtw_auto_scan_handler(struct rtw_adapter *padapter)
 		if (pmlmepriv->scan_interval == 0) {
 			DBG_8723A("%s\n", __func__);
 			rtw_set_802_11_bssid23a_list_scan(padapter, NULL, 0);
-			pmlmepriv->scan_interval = SCAN_INTERVAL;/*  30*2 sec = 60sec */
+			/*  30*2 sec = 60sec */
+			pmlmepriv->scan_interval = SCAN_INTERVAL;
 		}
 	}
 }
@@ -1594,7 +1628,8 @@ void rtw_dynamic_check_timer_handler(unsigned long data)
 	if (adapter->hw_init_completed == false)
 		goto out;
 
-	if ((adapter->bDriverStopped == true)||(adapter->bSurpriseRemoved == true))
+	if (adapter->bDriverStopped == true ||
+	    adapter->bSurpriseRemoved == true)
 		goto out;
 
 	if (adapter->net_closed == true)
@@ -1602,16 +1637,9 @@ void rtw_dynamic_check_timer_handler(unsigned long data)
 
 	rtw_dynamic_chk_wk_cmd23a(adapter);
 
-	if (pregistrypriv->wifi_spec == 1)
-	{
-#ifdef CONFIG_8723AU_P2P
-		struct wifidirect_info *pwdinfo = &adapter->wdinfo;
-		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
-#endif
-		{
-			/* auto site survey */
-			rtw_auto_scan_handler(adapter);
-		}
+	if (pregistrypriv->wifi_spec == 1) {
+		/* auto site survey */
+		rtw_auto_scan_handler(adapter);
 	}
 out:
 	mod_timer(&adapter->mlmepriv.dynamic_chk_timer,
@@ -1628,8 +1656,6 @@ void rtw_clear_scan_deny(struct rtw_adapter *adapter)
 {
 	struct mlme_priv *mlmepriv = &adapter->mlmepriv;
 	atomic_set(&mlmepriv->set_scan_deny, 0);
-	if (0)
-	DBG_8723A(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(adapter));
 }
 
 void rtw_set_scan_deny_timer_hdl(unsigned long data)
@@ -1642,12 +1668,9 @@ void rtw_set_scan_deny(struct rtw_adapter *adapter, u32 ms)
 {
 	struct mlme_priv *mlmepriv = &adapter->mlmepriv;
 
-	if (0)
-	DBG_8723A(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(adapter));
 	atomic_set(&mlmepriv->set_scan_deny, 1);
 	mod_timer(&mlmepriv->set_scan_deny_timer,
 		  jiffies + msecs_to_jiffies(ms));
-
 }
 
 #if defined(IEEE80211_SCAN_RESULT_EXPIRE)
@@ -1657,15 +1680,19 @@ void rtw_set_scan_deny(struct rtw_adapter *adapter, u32 ms)
 #endif
 
 /*
-* Select a new join candidate from the original @param candidate and @param competitor
+* Select a new join candidate from the original @param candidate and
+*     @param competitor
 * @return true: candidate is updated
 * @return false: candidate is not updated
 */
-static int rtw_check_join_candidate(struct mlme_priv *pmlmepriv
-	, struct wlan_network **candidate, struct wlan_network *competitor)
+static int rtw_check_join_candidate(struct mlme_priv *pmlmepriv,
+				    struct wlan_network **candidate,
+				    struct wlan_network *competitor)
 {
 	int updated = false;
-	struct rtw_adapter *adapter = container_of(pmlmepriv, struct rtw_adapter, mlmepriv);
+	struct rtw_adapter *adapter;
+
+	adapter = container_of(pmlmepriv, struct rtw_adapter, mlmepriv);
 
 	/* check bssid, if needed */
 	if (pmlmepriv->assoc_by_bssid == true) {
@@ -1684,10 +1711,10 @@ static int rtw_check_join_candidate(struct mlme_priv *pmlmepriv
 			goto exit;
 	}
 
-	if (rtw_is_desired_network(adapter, competitor)  == false)
+	if (rtw_is_desired_network(adapter, competitor) == false)
 		goto exit;
 
-	if (rtw_to_roaming(adapter) > 0) {
+	if (adapter->mlmepriv.to_roaming > 0) {
 		unsigned int passed;
 
 		passed = jiffies_to_msecs(jiffies - competitor->last_scanned);
@@ -1697,19 +1724,21 @@ static int rtw_check_join_candidate(struct mlme_priv *pmlmepriv
 			goto exit;
 	}
 
-	if (*candidate == NULL ||(*candidate)->network.Rssi<competitor->network.Rssi) {
+	if (!*candidate ||
+	    (*candidate)->network.Rssi<competitor->network.Rssi) {
 		*candidate = competitor;
 		updated = true;
 	}
 
 	if (updated) {
-		DBG_8723A("[by_bssid:%u][assoc_ssid:%s][to_roaming:%u] new candidate: %s("MAC_FMT") rssi:%d\n",
-			pmlmepriv->assoc_by_bssid,
-			pmlmepriv->assoc_ssid.ssid,
-			rtw_to_roaming(adapter),
-			(*candidate)->network.Ssid.ssid,
-			MAC_ARG((*candidate)->network.MacAddress),
-			(int)(*candidate)->network.Rssi);
+		DBG_8723A("[by_bssid:%u][assoc_ssid:%s][to_roaming:%u] "
+			  "new candidate: %s("MAC_FMT") rssi:%d\n",
+			  pmlmepriv->assoc_by_bssid,
+			  pmlmepriv->assoc_ssid.ssid,
+			  adapter->mlmepriv.to_roaming,
+			  (*candidate)->network.Ssid.ssid,
+			  MAC_ARG((*candidate)->network.MacAddress),
+			  (int)(*candidate)->network.Rssi);
 	}
 
 exit:
@@ -1732,8 +1761,8 @@ int rtw_select_and_join_from_scanned_queue23a(struct mlme_priv *pmlmepriv)
 	struct list_head *phead, *plist, *ptmp;
 	struct rtw_adapter *adapter;
 	struct rtw_queue *queue = &pmlmepriv->scanned_queue;
-	struct	wlan_network	*pnetwork = NULL;
-	struct	wlan_network	*candidate = NULL;
+	struct wlan_network *pnetwork;
+	struct wlan_network *candidate = NULL;
 
 	spin_lock_bh(&pmlmepriv->scanned_queue.lock);
 	phead = get_list_head(queue);
@@ -1760,11 +1789,11 @@ int rtw_select_and_join_from_scanned_queue23a(struct mlme_priv *pmlmepriv)
 		DBG_8723A("%s: candidate: %s("MAC_FMT", ch:%u)\n", __func__,
 			  candidate->network.Ssid.ssid,
 			  MAC_ARG(candidate->network.MacAddress),
-			  candidate->network.Configuration.DSConfig);
+			  candidate->network.DSConfig);
 	}
 
 	/*  check for situation of  _FW_LINKED */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
 		DBG_8723A("%s: _FW_LINKED while ask_for_joinbss!!!\n",
 			  __func__);
 
@@ -1782,7 +1811,7 @@ exit:
 }
 
 int rtw_set_auth23a(struct rtw_adapter * adapter,
-		 struct security_priv *psecuritypriv)
+		    struct security_priv *psecuritypriv)
 {
 	struct cmd_obj* pcmd;
 	struct setauth_parm *psetauthparm;
@@ -1795,8 +1824,7 @@ int rtw_set_auth23a(struct rtw_adapter * adapter,
 		goto exit;
 	}
 
-	psetauthparm = (struct setauth_parm*)
-		kzalloc(sizeof(struct setauth_parm), GFP_KERNEL);
+	psetauthparm = kzalloc(sizeof(struct setauth_parm), GFP_KERNEL);
 	if (!psetauthparm) {
 		kfree(pcmd);
 		res = _FAIL;
@@ -1811,8 +1839,6 @@ int rtw_set_auth23a(struct rtw_adapter * adapter,
 	pcmd->rsp = NULL;
 	pcmd->rspsz = 0;
 
-	INIT_LIST_HEAD(&pcmd->list);
-
 	RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
 		 ("after enqueue set_auth_cmd, auth_mode=%x\n",
 		  psecuritypriv->dot11AuthAlgrthm));
@@ -1825,7 +1851,7 @@ exit:
 }
 
 int rtw_set_key23a(struct rtw_adapter *adapter,
-		struct security_priv *psecuritypriv, int keyid, u8 set_tx)
+		   struct security_priv *psecuritypriv, int keyid, u8 set_tx)
 {
 	u8 keylen;
 	struct cmd_obj *pcmd;
@@ -1834,7 +1860,12 @@ int rtw_set_key23a(struct rtw_adapter *adapter,
 	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
 	int res = _SUCCESS;
 
-	pcmd = (struct cmd_obj *)kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
+	if (keyid >= 4) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pcmd = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
 	if (!pcmd) {
 		res = _FAIL;  /* try again */
 		goto exit;
@@ -1850,9 +1881,9 @@ int rtw_set_key23a(struct rtw_adapter *adapter,
 		psetkeyparm->algorithm = (unsigned char)
 			psecuritypriv->dot118021XGrpPrivacy;
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-			 ("\n rtw_set_key23a: psetkeyparm->algorithm = (unsigned "
-			  "char)psecuritypriv->dot118021XGrpPrivacy =%d\n",
-			  psetkeyparm->algorithm));
+			 ("\n rtw_set_key23a: psetkeyparm->algorithm = "
+			  "(unsigned char)psecuritypriv->dot118021XGrpPrivacy "
+			  "=%d\n", psetkeyparm->algorithm));
 	} else {
 		psetkeyparm->algorithm = (u8)psecuritypriv->dot11PrivacyAlgrthm;
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
@@ -1860,10 +1891,10 @@ int rtw_set_key23a(struct rtw_adapter *adapter,
 			  "psecuritypriv->dot11PrivacyAlgrthm =%d\n",
 			  psetkeyparm->algorithm));
 	}
-	psetkeyparm->keyid = (u8)keyid;/* 0~3 */
+	psetkeyparm->keyid = keyid;/* 0~3 */
 	psetkeyparm->set_tx = set_tx;
 	if (is_wep_enc(psetkeyparm->algorithm))
-		pmlmepriv->key_mask |= CHKBIT(psetkeyparm->keyid);
+		pmlmepriv->key_mask |= BIT(psetkeyparm->keyid);
 
 	DBG_8723A("==> rtw_set_key23a algorithm(%x), keyid(%x), key_mask(%x)\n",
 		  psetkeyparm->algorithm, psetkeyparm->keyid,
@@ -1873,23 +1904,23 @@ int rtw_set_key23a(struct rtw_adapter *adapter,
 		  "keyid = (u8)keyid =%d\n", psetkeyparm->algorithm, keyid));
 
 	switch (psetkeyparm->algorithm) {
-	case _WEP40_:
+	case WLAN_CIPHER_SUITE_WEP40:
 		keylen = 5;
 		memcpy(&psetkeyparm->key[0],
-		       &psecuritypriv->dot11DefKey[keyid].skey[0], keylen);
+		       &psecuritypriv->wep_key[keyid].key, keylen);
 		break;
-	case _WEP104_:
+	case WLAN_CIPHER_SUITE_WEP104:
 		keylen = 13;
 		memcpy(&psetkeyparm->key[0],
-		       &psecuritypriv->dot11DefKey[keyid].skey[0], keylen);
+		       &psecuritypriv->wep_key[keyid].key, keylen);
 		break;
-	case _TKIP_:
+	case WLAN_CIPHER_SUITE_TKIP:
 		keylen = 16;
 		memcpy(&psetkeyparm->key,
 		       &psecuritypriv->dot118021XGrpKey[keyid], keylen);
 		psetkeyparm->grpkey = 1;
 		break;
-	case _AES_:
+	case WLAN_CIPHER_SUITE_CCMP:
 		keylen = 16;
 		memcpy(&psetkeyparm->key,
 		       &psecuritypriv->dot118021XGrpKey[keyid], keylen);
@@ -1897,8 +1928,8 @@ int rtw_set_key23a(struct rtw_adapter *adapter,
 		break;
 	default:
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-			 ("\n rtw_set_key23a:psecuritypriv->dot11PrivacyAlgrthm = "
-			  "%x (must be 1 or 2 or 4 or 5)\n",
+			 ("\n rtw_set_key23a:psecuritypriv->dot11PrivacyAlgrthm"
+			  " = %x (must be 1 or 2 or 4 or 5)\n",
 			  psecuritypriv->dot11PrivacyAlgrthm));
 		res = _FAIL;
 		kfree(pcmd);
@@ -1912,8 +1943,6 @@ int rtw_set_key23a(struct rtw_adapter *adapter,
 	pcmd->rsp = NULL;
 	pcmd->rspsz = 0;
 
-	INIT_LIST_HEAD(&pcmd->list);
-
 	/* sema_init(&pcmd->cmd_sem, 0); */
 
 	res = rtw_enqueue_cmd23a(pcmdpriv, pcmd);
@@ -1931,7 +1960,7 @@ int rtw_restruct_wmm_ie23a(struct rtw_adapter *adapter, u8 *in_ie,
 	unsigned int i, j;
 
 	i = 12; /* after the fixed IE */
-	while(i < in_len) {
+	while (i < in_len) {
 		ielength = initial_out_len;
 
 		/* WMM element ID and OUI */
@@ -1982,11 +2011,11 @@ static int SecIsInPMKIDList(struct rtw_adapter *Adapter, u8 *bssid)
 			i++;
 			/* continue; */
 		}
-	} while(i < NUM_PMKID_CACHE);
+	} while (i < NUM_PMKID_CACHE);
 
-	if (i == NUM_PMKID_CACHE) {
+	if (i == NUM_PMKID_CACHE)
 		i = -1;/*  Could not find. */
-	} else {
+	else {
 		/*  There is one Pre-Authentication Key for
 		    the specific BSSID. */
 	}
@@ -2023,8 +2052,9 @@ static int rtw_append_pmkid(struct rtw_adapter *Adapter, int iEntry,
 	}
 	return ie_len;
 }
+
 int rtw_restruct_sec_ie23a(struct rtw_adapter *adapter, u8 *in_ie, u8 *out_ie,
-			uint in_len)
+			   uint in_len)
 {
 	u8 authmode;
 	uint ielength;
@@ -2035,41 +2065,39 @@ int rtw_restruct_sec_ie23a(struct rtw_adapter *adapter, u8 *in_ie, u8 *out_ie,
 	uint ndissecuritytype = psecuritypriv->ndisencryptstatus;
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
-		 ("+rtw_restruct_sec_ie23a: ndisauthmode=%d ndissecuritytype=%d\n",
-		  ndisauthmode, ndissecuritytype));
+		 ("+rtw_restruct_sec_ie23a: ndisauthmode=%d "
+		  "ndissecuritytype=%d\n", ndisauthmode, ndissecuritytype));
 
 	/* copy fixed ie only */
 	memcpy(out_ie, in_ie, 12);
 	ielength = 12;
-	if ((ndisauthmode==Ndis802_11AuthModeWPA) ||
-	    (ndisauthmode==Ndis802_11AuthModeWPAPSK))
-		authmode=_WPA_IE_ID_;
-	if ((ndisauthmode==Ndis802_11AuthModeWPA2) ||
-	    (ndisauthmode==Ndis802_11AuthModeWPA2PSK))
-		authmode=_WPA2_IE_ID_;
+	if (ndisauthmode == Ndis802_11AuthModeWPA ||
+	    ndisauthmode == Ndis802_11AuthModeWPAPSK)
+		authmode = WLAN_EID_VENDOR_SPECIFIC;
+	if (ndisauthmode == Ndis802_11AuthModeWPA2 ||
+	    ndisauthmode == Ndis802_11AuthModeWPA2PSK)
+		authmode = _WPA2_IE_ID_;
 
 	if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) {
 		memcpy(out_ie + ielength, psecuritypriv->wps_ie,
 		       psecuritypriv->wps_ie_len);
 
 		ielength += psecuritypriv->wps_ie_len;
-	} else if ((authmode==_WPA_IE_ID_) || (authmode==_WPA2_IE_ID_)) {
+	} else if (authmode == WLAN_EID_VENDOR_SPECIFIC ||
+		   authmode == _WPA2_IE_ID_) {
 		/* copy RSN or SSN */
 		memcpy(&out_ie[ielength], &psecuritypriv->supplicant_ie[0],
 		       psecuritypriv->supplicant_ie[1] + 2);
 		ielength += psecuritypriv->supplicant_ie[1] + 2;
-		rtw_report_sec_ie23a(adapter, authmode,
-				  psecuritypriv->supplicant_ie);
 	}
 
 	iEntry = SecIsInPMKIDList(adapter, pmlmepriv->assoc_bssid);
-	if (iEntry < 0)	{
+	if (iEntry < 0)
 		return ielength;
-	} else {
-		if (authmode == _WPA2_IE_ID_) {
-			ielength=rtw_append_pmkid(adapter, iEntry,
-						  out_ie, ielength);
-		}
+	else {
+		if (authmode == _WPA2_IE_ID_)
+			ielength = rtw_append_pmkid(adapter, iEntry,
+						    out_ie, ielength);
 	}
 
 	return ielength;
@@ -2087,13 +2115,7 @@ void rtw_init_registrypriv_dev_network23a(struct rtw_adapter* adapter)
 	memcpy(&pdev_network->Ssid, &pregistrypriv->ssid,
 	       sizeof(struct cfg80211_ssid));
 
-	pdev_network->Configuration.Length=sizeof(struct ndis_802_11_config);
-	pdev_network->Configuration.BeaconPeriod = 100;
-	pdev_network->Configuration.FHConfig.Length = 0;
-	pdev_network->Configuration.FHConfig.HopPattern = 0;
-	pdev_network->Configuration.FHConfig.HopSet = 0;
-	pdev_network->Configuration.FHConfig.DwellTime = 0;
-
+	pdev_network->BeaconPeriod = 100;
 }
 
 void rtw_update_registrypriv_dev_network23a(struct rtw_adapter* adapter)
@@ -2110,44 +2132,15 @@ void rtw_update_registrypriv_dev_network23a(struct rtw_adapter* adapter)
 
 	pdev_network->Rssi = 0;
 
-	switch (pregistrypriv->wireless_mode)
-	{
-	case WIRELESS_11B:
-		pdev_network->NetworkTypeInUse = Ndis802_11DS;
-		break;
-	case WIRELESS_11G:
-	case WIRELESS_11BG:
-	case WIRELESS_11_24N:
-	case WIRELESS_11G_24N:
-	case WIRELESS_11BG_24N:
-		pdev_network->NetworkTypeInUse = Ndis802_11OFDM24;
-		break;
-	case WIRELESS_11A:
-	case WIRELESS_11A_5N:
-		pdev_network->NetworkTypeInUse = Ndis802_11OFDM5;
-		break;
-	case WIRELESS_11ABGN:
-		if (pregistrypriv->channel > 14)
-			pdev_network->NetworkTypeInUse = Ndis802_11OFDM5;
-		else
-			pdev_network->NetworkTypeInUse = Ndis802_11OFDM24;
-		break;
-	default :
-		/*  TODO */
-		break;
-	}
-
-	pdev_network->Configuration.DSConfig = pregistrypriv->channel;
+	pdev_network->DSConfig = pregistrypriv->channel;
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-		 ("pregistrypriv->channel =%d, pdev_network->Configuration."
-		  "DSConfig = 0x%x\n", pregistrypriv->channel,
-		  pdev_network->Configuration.DSConfig));
+		 ("pregistrypriv->channel =%d, pdev_network->DSConfig = 0x%x\n",
+		  pregistrypriv->channel, pdev_network->DSConfig));
 
-	if (cur_network->network.InfrastructureMode == Ndis802_11IBSS)
-		pdev_network->Configuration.ATIMWindow = 0;
+	if (cur_network->network.ifmode == NL80211_IFTYPE_ADHOC)
+		pdev_network->ATIMWindow = 0;
 
-	pdev_network->InfrastructureMode =
-		cur_network->network.InfrastructureMode;
+	pdev_network->ifmode = cur_network->network.ifmode;
 
 	/*  1. Supported rates */
 	/*  2. IE */
@@ -2157,12 +2150,11 @@ void rtw_update_registrypriv_dev_network23a(struct rtw_adapter* adapter)
 	pdev_network->IELength = sz;
 
 	pdev_network->Length =
-		get_wlan_bssid_ex_sz((struct wlan_bssid_ex *)pdev_network);
+		get_wlan_bssid_ex_sz(pdev_network);
 
 	/* notes: translate IELength & Length after assign the
 	   Length to cmdsz in createbss_cmd(); */
 	/* pdev_network->IELength = cpu_to_le32(sz); */
-
 }
 
 void rtw_get_encrypt_decrypt_from_registrypriv23a(struct rtw_adapter* adapter)
@@ -2193,41 +2185,38 @@ void rtw_joinbss_reset23a(struct rtw_adapter *padapter)
 			threshold = 1;
 		else
 			threshold = 0;
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_RXDMA_AGG_PG_TH,
-				  (u8 *)(&threshold));
-	} else {
+	} else
 		threshold = 1;
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_RXDMA_AGG_PG_TH,
-				  (u8 *)(&threshold));
-	}
+
+	rtl8723a_set_rxdma_agg_pg_th(padapter, threshold);
 }
 
 /* the fucntion is >= passive_level */
 unsigned int rtw_restructure_ht_ie23a(struct rtw_adapter *padapter, u8 *in_ie,
-				   u8 *out_ie, uint in_len, uint *pout_len)
+				      u8 *out_ie, uint in_len, uint *pout_len)
 {
-	u32 ielen, out_len;
+	u32 out_len;
 	int max_rx_ampdu_factor;
-	unsigned char *p, *pframe;
+	unsigned char *pframe;
+	const u8 *p;
 	struct ieee80211_ht_cap ht_capie;
 	unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01, 0x00};
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct qos_priv *pqospriv = &pmlmepriv->qospriv;
 	struct ht_priv *phtpriv = &pmlmepriv->htpriv;
 
 	phtpriv->ht_option = false;
 
-	p = rtw_get_ie23a(in_ie + 12, _HT_CAPABILITY_IE_, &ielen, in_len - 12);
+	p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, in_ie + 12, in_len -12);
 
-	if (p && ielen > 0) {
+	if (p && p[1] > 0) {
 		u32 rx_packet_offset, max_recvbuf_sz;
-		if (pqospriv->qos_option == 0) {
+		if (pmlmepriv->qos_option == 0) {
 			out_len = *pout_len;
 			pframe = rtw_set_ie23a(out_ie + out_len,
-					    _VENDOR_SPECIFIC_IE_,
+					    WLAN_EID_VENDOR_SPECIFIC,
 					    _WMM_IE_Length_, WMM_IE, pout_len);
 
-			pqospriv->qos_option = 1;
+			pmlmepriv->qos_option = 1;
 		}
 
 		out_len = *pout_len;
@@ -2238,33 +2227,36 @@ unsigned int rtw_restructure_ht_ie23a(struct rtw_adapter *padapter, u8 *in_ie,
 			IEEE80211_HT_CAP_SGI_20 | IEEE80211_HT_CAP_SGI_40 |
 			IEEE80211_HT_CAP_TX_STBC | IEEE80211_HT_CAP_DSSSCCK40;
 
-		rtw_hal_get_def_var23a(padapter, HAL_DEF_RX_PACKET_OFFSET,
-				    &rx_packet_offset);
-		rtw_hal_get_def_var23a(padapter, HAL_DEF_MAX_RECVBUF_SZ,
-				    &max_recvbuf_sz);
+		GetHalDefVar8192CUsb(padapter, HAL_DEF_RX_PACKET_OFFSET,
+				     &rx_packet_offset);
+		GetHalDefVar8192CUsb(padapter, HAL_DEF_MAX_RECVBUF_SZ,
+				     &max_recvbuf_sz);
 
-		rtw_hal_get_def_var23a(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR,
-				    &max_rx_ampdu_factor);
+		GetHalDefVar8192CUsb(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR,
+				     &max_rx_ampdu_factor);
 		ht_capie.ampdu_params_info = max_rx_ampdu_factor & 0x03;
 
-		if (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)
+		if (padapter->securitypriv.dot11PrivacyAlgrthm ==
+		    WLAN_CIPHER_SUITE_CCMP)
 			ht_capie.ampdu_params_info |=
 				(IEEE80211_HT_AMPDU_PARM_DENSITY& (0x07 << 2));
 		else
 			ht_capie.ampdu_params_info |=
 				(IEEE80211_HT_AMPDU_PARM_DENSITY & 0x00);
 
-		pframe = rtw_set_ie23a(out_ie + out_len, _HT_CAPABILITY_IE_,
+		pframe = rtw_set_ie23a(out_ie + out_len, WLAN_EID_HT_CAPABILITY,
 				    sizeof(struct ieee80211_ht_cap),
 				    (unsigned char*)&ht_capie, pout_len);
 
 		phtpriv->ht_option = true;
 
-		p = rtw_get_ie23a(in_ie + 12, _HT_ADD_INFO_IE_, &ielen, in_len-12);
-		if (p && (ielen == sizeof(struct ieee80211_ht_addt_info))) {
+		p = cfg80211_find_ie(WLAN_EID_HT_OPERATION, in_ie + 12,
+				     in_len -12);
+		if (p && (p[1] == sizeof(struct ieee80211_ht_addt_info))) {
 			out_len = *pout_len;
-			pframe = rtw_set_ie23a(out_ie + out_len, _HT_ADD_INFO_IE_,
-					    ielen, p + 2 , pout_len);
+			pframe = rtw_set_ie23a(out_ie + out_len,
+					       WLAN_EID_HT_OPERATION,
+					       p[1], p + 2 , pout_len);
 		}
 	}
 
@@ -2274,19 +2266,16 @@ unsigned int rtw_restructure_ht_ie23a(struct rtw_adapter *padapter, u8 *in_ie,
 /* the fucntion is > passive_level (in critical_section) */
 void rtw_update_ht_cap23a(struct rtw_adapter *padapter, u8 *pie, uint ie_len)
 {
-	u8 *p, max_ampdu_sz;
-	int len;
-	/* struct sta_info *bmc_sta, *psta; */
+	u8 max_ampdu_sz;
+	const u8 *p;
 	struct ieee80211_ht_cap *pht_capie;
 	struct ieee80211_ht_addt_info *pht_addtinfo;
-	/* struct recv_reorder_ctrl *preorder_ctrl; */
-	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
-	struct ht_priv		*phtpriv = &pmlmepriv->htpriv;
-	/* struct recv_priv *precvpriv = &padapter->recvpriv; */
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv *phtpriv = &pmlmepriv->htpriv;
 	struct registry_priv *pregistrypriv = &padapter->registrypriv;
-	/* struct wlan_network *pcur_network = &pmlmepriv->cur_network;; */
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+	int bcn_fixed_size;
 
 	if (!phtpriv->ht_option)
 		return;
@@ -2296,51 +2285,54 @@ void rtw_update_ht_cap23a(struct rtw_adapter *padapter, u8 *pie, uint ie_len)
 
 	DBG_8723A("+rtw_update_ht_cap23a()\n");
 
+	bcn_fixed_size = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+		offsetof(struct ieee80211_mgmt, u.beacon);
+
+	/* Adjust pie + ie_len for our searches */
+	pie += bcn_fixed_size;
+	ie_len -= bcn_fixed_size;
+
 	/* maybe needs check if ap supports rx ampdu. */
-	if ((phtpriv->ampdu_enable == false) && (pregistrypriv->ampdu_enable == 1)) {
+	if (phtpriv->ampdu_enable == false &&
+	    pregistrypriv->ampdu_enable == 1) {
 		if (pregistrypriv->wifi_spec == 1)
 			phtpriv->ampdu_enable = false;
 		else
 			phtpriv->ampdu_enable = true;
-	} else if (pregistrypriv->ampdu_enable == 2) {
+	} else if (pregistrypriv->ampdu_enable == 2)
 		phtpriv->ampdu_enable = true;
-	}
 
 	/* check Max Rx A-MPDU Size */
-	len = 0;
-	p = rtw_get_ie23a(pie+sizeof (struct ndis_802_11_fixed_ies), _HT_CAPABILITY_IE_, &len, ie_len-sizeof (struct ndis_802_11_fixed_ies));
-	if (p && len > 0) {
-		pht_capie = (struct ieee80211_ht_cap *)(p+2);
-		max_ampdu_sz = (pht_capie->ampdu_params_info & IEEE80211_HT_AMPDU_PARM_FACTOR);
-		max_ampdu_sz = 1 << (max_ampdu_sz+3); /*  max_ampdu_sz (kbytes); */
-
-		/* DBG_8723A("rtw_update_ht_cap23a(): max_ampdu_sz =%d\n", max_ampdu_sz); */
-		phtpriv->rx_ampdu_maxlen = max_ampdu_sz;
+	p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, pie, ie_len);
 
+	if (p && p[1] > 0) {
+		pht_capie = (struct ieee80211_ht_cap *)(p + 2);
+		max_ampdu_sz = pht_capie->ampdu_params_info &
+			IEEE80211_HT_AMPDU_PARM_FACTOR;
+		/*  max_ampdu_sz (kbytes); */
+		max_ampdu_sz = 1 << (max_ampdu_sz + 3);
+
+		phtpriv->rx_ampdu_maxlen = max_ampdu_sz;
 	}
 
-	len = 0;
-	p = rtw_get_ie23a(pie+sizeof (struct ndis_802_11_fixed_ies), _HT_ADD_INFO_IE_, &len, ie_len-sizeof (struct ndis_802_11_fixed_ies));
-	if (p && len>0)
-	{
-		pht_addtinfo = (struct ieee80211_ht_addt_info *)(p+2);
+	p = cfg80211_find_ie(WLAN_EID_HT_OPERATION, pie, ie_len);
+	if (p && p[1] > 0) {
+		pht_addtinfo = (struct ieee80211_ht_addt_info *)(p + 2);
 		/* todo: */
 	}
 
 	/* update cur_bwmode & cur_ch_offset */
-	if ((pregistrypriv->cbw40_enable) &&
-		(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info & BIT(1)) &&
-		(pmlmeinfo->HT_info.infos[0] & BIT(2)))
-	{
+	if (pregistrypriv->cbw40_enable &&
+	    pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info & BIT(1) &&
+	    pmlmeinfo->HT_info.infos[0] & BIT(2)) {
 		int i;
-		u8	rf_type;
+		u8 rf_type;
 
-		padapter->HalFunc.GetHwRegHandler(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+		rf_type = rtl8723a_get_rf_type(padapter);
 
 		/* update the MCS rates */
-		for (i = 0; i < 16; i++)
-		{
-			if ((rf_type == RF_1T1R) || (rf_type == RF_1T2R))
+		for (i = 0; i < 16; i++) {
+			if (rf_type == RF_1T1R || rf_type == RF_1T2R)
 				pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_1R23A[i];
 			else
 				pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_2R23A[i];
@@ -2349,24 +2341,26 @@ void rtw_update_ht_cap23a(struct rtw_adapter *padapter, u8 *pie, uint ie_len)
 		pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
 		switch ((pmlmeinfo->HT_info.infos[0] & 0x3))
 		{
-			case HT_EXTCHNL_OFFSET_UPPER:
-				pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
-				break;
+		case HT_EXTCHNL_OFFSET_UPPER:
+			pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+			break;
 
-			case HT_EXTCHNL_OFFSET_LOWER:
-				pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
-				break;
+		case HT_EXTCHNL_OFFSET_LOWER:
+			pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+			break;
 
-			default:
-				pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
-				break;
+		default:
+			pmlmeext->cur_ch_offset =
+				HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+			break;
 		}
 	}
 
 	/*  */
 	/*  Config SM Power Save setting */
 	/*  */
-	pmlmeinfo->SM_PS = (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info & 0x0C) >> 2;
+	pmlmeinfo->SM_PS = (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info &
+			    0x0C) >> 2;
 	if (pmlmeinfo->SM_PS == WLAN_HT_CAP_SM_PS_STATIC)
 		DBG_8723A("%s(): WLAN_HT_CAP_SM_PS_STATIC\n", __func__);
 
@@ -2376,124 +2370,64 @@ void rtw_update_ht_cap23a(struct rtw_adapter *padapter, u8 *pie, uint ie_len)
 	pmlmeinfo->HT_protection = pmlmeinfo->HT_info.infos[1] & 0x3;
 }
 
-void rtw_issue_addbareq_cmd23a(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe)
+void rtw_issue_addbareq_cmd23a(struct rtw_adapter *padapter,
+			       struct xmit_frame *pxmitframe)
 {
 	u8 issued;
 	int priority;
-	struct sta_info *psta = NULL;
+	struct sta_info *psta;
 	struct ht_priv	*phtpriv;
 	struct pkt_attrib *pattrib = &pxmitframe->attrib;
 	s32 bmcst = is_multicast_ether_addr(pattrib->ra);
 
-	if (bmcst || (padapter->mlmepriv.LinkDetectInfo.NumTxOkInPeriod<100))
+	if (bmcst || padapter->mlmepriv.LinkDetectInfo.NumTxOkInPeriod < 100)
 		return;
 
 	priority = pattrib->priority;
 
 	if (pattrib->psta)
 		psta = pattrib->psta;
-	else
-	{
+	else {
 		DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__);
 		psta = rtw_get_stainfo23a(&padapter->stapriv, pattrib->ra);
 	}
 
-	if (psta == NULL)
-	{
+	if (!psta) {
 		DBG_8723A("%s, psta == NUL\n", __func__);
 		return;
 	}
 
-	if (!(psta->state &_FW_LINKED))
-	{
-		DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+	if (!(psta->state &_FW_LINKED)) {
+		DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n",
+			  __func__, psta->state);
 		return;
 	}
 
 	phtpriv = &psta->htpriv;
 
-	if ((phtpriv->ht_option == true) && (phtpriv->ampdu_enable == true))
-	{
+	if (phtpriv->ht_option == true && phtpriv->ampdu_enable == true) {
 		issued = (phtpriv->agg_enable_bitmap>>priority)&0x1;
 		issued |= (phtpriv->candidate_tid_bitmap>>priority)&0x1;
 
-		if (0 == issued)
-		{
-			DBG_8723A("rtw_issue_addbareq_cmd23a, p =%d\n", priority);
-			psta->htpriv.candidate_tid_bitmap |= CHKBIT((u8)priority);
-			rtw_addbareq_cmd23a(padapter, (u8) priority, pattrib->ra);
-		}
-	}
-}
-
-inline void rtw_set_roaming(struct rtw_adapter *adapter, u8 to_roaming)
-{
-	if (to_roaming == 0)
-		adapter->mlmepriv.to_join = false;
-	adapter->mlmepriv.to_roaming = to_roaming;
-}
-
-inline u8 rtw_to_roaming(struct rtw_adapter *adapter)
-{
-	return adapter->mlmepriv.to_roaming;
-}
-
-void rtw23a_roaming(struct rtw_adapter *padapter, struct wlan_network *tgt_network)
-{
-	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
-
-	spin_lock_bh(&pmlmepriv->lock);
-	_rtw23a_roaming(padapter, tgt_network);
-	spin_unlock_bh(&pmlmepriv->lock);
-}
-void _rtw23a_roaming(struct rtw_adapter *padapter, struct wlan_network *tgt_network)
-{
-	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
-	struct wlan_network *pnetwork;
-	int do_join_r;
-
-	if (tgt_network != NULL)
-		pnetwork = tgt_network;
-	else
-		pnetwork = &pmlmepriv->cur_network;
-
-	if (0 < rtw_to_roaming(padapter)) {
-		DBG_8723A("roaming from %s("MAC_FMT"), length:%d\n",
-			  pnetwork->network.Ssid.ssid,
-			  MAC_ARG(pnetwork->network.MacAddress),
-			  pnetwork->network.Ssid.ssid_len);
-		memcpy(&pmlmepriv->assoc_ssid, &pnetwork->network.Ssid,
-		       sizeof(struct cfg80211_ssid));
-
-		pmlmepriv->assoc_by_bssid = false;
-
-		while(1) {
-			if (_SUCCESS == (do_join_r = rtw_do_join23a(padapter))) {
-				break;
-			} else {
-				DBG_8723A("roaming do_join return %d\n", do_join_r);
-				pmlmepriv->to_roaming--;
-
-				if (0 < rtw_to_roaming(padapter)) {
-					continue;
-				} else {
-					DBG_8723A("%s(%d) -to roaming fail, indicate_disconnect\n", __func__, __LINE__);
-					rtw_indicate_disconnect23a(padapter);
-					break;
-				}
-			}
+		if (issued == 0) {
+			DBG_8723A("rtw_issue_addbareq_cmd23a, p =%d\n",
+				  priority);
+			psta->htpriv.candidate_tid_bitmap |= BIT(priority);
+			rtw_addbareq_cmd23a(padapter, (u8) priority,
+					    pattrib->ra);
 		}
 	}
 }
 
 int rtw_linked_check(struct rtw_adapter *padapter)
 {
-	if ((check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE)) ||
-	    (check_fwstate(&padapter->mlmepriv, WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE))) {
+	if (check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE) ||
+	    check_fwstate(&padapter->mlmepriv,
+			  WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE)) {
 		if (padapter->stapriv.asoc_sta_count > 2)
 			return true;
 	} else {	/* Station mode */
-		if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) == true)
+		if (check_fwstate(&padapter->mlmepriv, _FW_LINKED))
 			return true;
 	}
 	return false;
diff --git a/drivers/staging/rtl8723au/core/rtw_mlme_ext.c b/drivers/staging/rtl8723au/core/rtw_mlme_ext.c
index 1f3e8a0aece..e1b28a23425 100644
--- a/drivers/staging/rtl8723au/core/rtw_mlme_ext.c
+++ b/drivers/staging/rtl8723au/core/rtw_mlme_ext.c
@@ -21,11 +21,51 @@
 #include <wlan_bssdef.h>
 #include <mlme_osdep.h>
 #include <recv_osdep.h>
-#include <ethernet.h>
 #include <linux/ieee80211.h>
-
-#ifdef CONFIG_8723AU_BT_COEXIST
 #include <rtl8723a_hal.h>
+
+static int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnAssocRsp23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnProbeReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnProbeRsp23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int DoReserved23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnBeacon23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnAtim23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnDisassoc23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnAuth23aClient23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnDeAuth23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnAction23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+
+static int on_action_spct23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnAction23a_qos(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnAction23a_dls(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnAction23a_back23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int on_action_public23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnAction23a_ht(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnAction23a_wmm(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnAction23a_p2p(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+
+static void issue_assocreq(struct rtw_adapter *padapter);
+static void issue_probereq(struct rtw_adapter *padapter,
+			   struct cfg80211_ssid *pssid, u8 *da);
+static int issue_probereq_ex(struct rtw_adapter *padapter,
+			     struct cfg80211_ssid *pssid,
+			     u8 *da, int try_cnt, int wait_ms);
+static void issue_probersp(struct rtw_adapter *padapter, unsigned char *da,
+			   u8 is_valid_p2p_probereq);
+static void issue_auth(struct rtw_adapter *padapter, struct sta_info *psta,
+		       unsigned short status);
+static int issue_deauth_ex(struct rtw_adapter *padapter, u8 *da,
+			   unsigned short reason, int try_cnt, int wait_ms);
+static void start_clnt_assoc(struct rtw_adapter *padapter);
+static void start_clnt_auth(struct rtw_adapter *padapter);
+static void start_clnt_join(struct rtw_adapter *padapter);
+static void start_create_ibss(struct rtw_adapter *padapter);
+
+#ifdef CONFIG_8723AU_AP_MODE
+static int OnAuth23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static void issue_assocrsp(struct rtw_adapter *padapter, unsigned short status,
+			   struct sta_info *pstat, u16 pkt_type);
 #endif
 
 static struct mlme_handler mlme_sta_tbl[]={
@@ -66,7 +106,6 @@ static u8	null_addr[ETH_ALEN]= {0, 0, 0, 0, 0, 0};
 /**************************************************
 OUI definitions for the vendor specific IE
 ***************************************************/
-unsigned char	RTW_WPA_OUI23A[] = {0x00, 0x50, 0xf2, 0x01};
 unsigned char WMM_OUI23A[] = {0x00, 0x50, 0xf2, 0x02};
 unsigned char	WPS_OUI23A[] = {0x00, 0x50, 0xf2, 0x04};
 unsigned char	P2P_OUI23A[] = {0x50, 0x6F, 0x9A, 0x09};
@@ -75,9 +114,7 @@ unsigned char	WFD_OUI23A[] = {0x50, 0x6F, 0x9A, 0x0A};
 unsigned char	WMM_INFO_OUI23A[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
 unsigned char	WMM_PARA_OUI23A[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
 
-unsigned char WPA_TKIP_CIPHER23A[4] = {0x00, 0x50, 0xf2, 0x02};
-unsigned char RSN_TKIP_CIPHER23A[4] = {0x00, 0x0f, 0xac, 0x02};
-
+static unsigned char REALTEK_96B_IE[] = {0x00, 0xe0, 0x4c, 0x02, 0x01, 0x20};
 
 /********************************************************
 MCS rate definitions
@@ -198,9 +235,13 @@ static struct rt_channel_plan_map	RTW_ChannelPlanMap[RT_CHANNEL_DOMAIN_MAX] = {
 
 static struct rt_channel_plan_map	RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE = {0x03, 0x02}; /* use the conbination for max channel numbers */
 
+static void dummy_event_callback(struct rtw_adapter *adapter, const u8 *pbuf)
+{
+}
+
 static struct fwevent wlanevents[] =
 {
-	{0, rtw_dummy_event_callback23a},	/*0*/
+	{0, &dummy_event_callback},	/*0*/
 	{0, NULL},
 	{0, NULL},
 	{0, NULL},
@@ -214,21 +255,32 @@ static struct fwevent wlanevents[] =
 	{0, &rtw23a_joinbss_event_cb},		/*10*/
 	{sizeof(struct stassoc_event), &rtw_stassoc_event_callback23a},
 	{sizeof(struct stadel_event), &rtw_stadel_event_callback23a},
-	{0, &rtw_atimdone_event_callback23a},
-	{0, rtw_dummy_event_callback23a},
+	{0, &dummy_event_callback},
+	{0, &dummy_event_callback},
 	{0, NULL},	/*15*/
 	{0, NULL},
 	{0, NULL},
 	{0, NULL},
-	{0, rtw23a_fwdbg_event_callback},
+	{0, &dummy_event_callback},
 	{0, NULL},	 /*20*/
 	{0, NULL},
 	{0, NULL},
-	{0, &rtw_cpwm_event_callback23a},
+	{0, &dummy_event_callback},
 	{0, NULL},
 };
 
 
+static void rtw_correct_TSF(struct rtw_adapter *padapter)
+{
+	hw_var_set_correct_tsf(padapter);
+}
+
+static void
+rtw_update_TSF(struct mlme_ext_priv *pmlmeext, struct ieee80211_mgmt *mgmt)
+{
+	pmlmeext->TSFValue = get_unaligned_le64(&mgmt->u.beacon.timestamp);
+}
+
 /*
  * Search the @param channel_num in given @param channel_set
  * @ch_set: the given channel set
@@ -310,7 +362,7 @@ static void init_mlme_ext_priv23a_value(struct rtw_adapter* padapter)
 	pmlmeinfo->key_index = 0;
 	pmlmeinfo->iv = 0;
 
-	pmlmeinfo->enc_algo = _NO_PRIVACY_;
+	pmlmeinfo->enc_algo = 0;
 	pmlmeinfo->authModeToggle = 0;
 
 	memset(pmlmeinfo->chg_txt, 0, 128);
@@ -388,96 +440,88 @@ static void init_channel_list(struct rtw_adapter *padapter,
 	channel_list->reg_classes = cla;
 }
 
-static u8 init_channel_set(struct rtw_adapter* padapter, u8 ChannelPlan,
-			   struct rt_channel_info *channel_set)
+static u8 init_channel_set(struct rtw_adapter* padapter, u8 cplan,
+			   struct rt_channel_info *c_set)
 {
-	u8	index, chanset_size = 0;
-	u8	b5GBand = false, b2_4GBand = false;
-	u8	Index2G = 0, Index5G = 0;
+	u8 i, ch_size = 0;
+	u8 b5GBand = false, b2_4GBand = false;
+	u8 Index2G = 0, Index5G = 0;
 
-	memset(channel_set, 0, sizeof(struct rt_channel_info)*MAX_CHANNEL_NUM);
+	memset(c_set, 0, sizeof(struct rt_channel_info) * MAX_CHANNEL_NUM);
 
-	if (ChannelPlan >= RT_CHANNEL_DOMAIN_MAX &&
-	    ChannelPlan != RT_CHANNEL_DOMAIN_REALTEK_DEFINE) {
-		DBG_8723A("ChannelPlan ID %x error !!!!!\n", ChannelPlan);
-		return chanset_size;
+	if (cplan >= RT_CHANNEL_DOMAIN_MAX &&
+	    cplan != RT_CHANNEL_DOMAIN_REALTEK_DEFINE) {
+		DBG_8723A("ChannelPlan ID %x error !!!!!\n", cplan);
+		return ch_size;
 	}
 
 	if (padapter->registrypriv.wireless_mode & WIRELESS_11G) {
 		b2_4GBand = true;
-		if (RT_CHANNEL_DOMAIN_REALTEK_DEFINE == ChannelPlan)
+		if (RT_CHANNEL_DOMAIN_REALTEK_DEFINE == cplan)
 			Index2G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index2G;
 		else
-			Index2G = RTW_ChannelPlanMap[ChannelPlan].Index2G;
+			Index2G = RTW_ChannelPlanMap[cplan].Index2G;
 	}
 
 	if (padapter->registrypriv.wireless_mode & WIRELESS_11A) {
 		b5GBand = true;
-		if (RT_CHANNEL_DOMAIN_REALTEK_DEFINE == ChannelPlan)
+		if (RT_CHANNEL_DOMAIN_REALTEK_DEFINE == cplan)
 			Index5G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index5G;
 		else
-			Index5G = RTW_ChannelPlanMap[ChannelPlan].Index5G;
+			Index5G = RTW_ChannelPlanMap[cplan].Index5G;
 	}
 
 	if (b2_4GBand) {
-		for (index = 0; index<RTW_ChannelPlan2G[Index2G].Len; index++) {
-			channel_set[chanset_size].ChannelNum =
-				RTW_ChannelPlan2G[Index2G].Channel[index];
+		for (i = 0; i < RTW_ChannelPlan2G[Index2G].Len; i++) {
+			c_set[ch_size].ChannelNum =
+				RTW_ChannelPlan2G[Index2G].Channel[i];
 
-			if ((RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN == ChannelPlan) ||
+			if ((RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN == cplan) ||
 			    /* Channel 1~11 is active, and 12~14 is passive */
-			    (RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN_2G == ChannelPlan)){
-				if (channel_set[chanset_size].ChannelNum >= 1 &&
-				    channel_set[chanset_size].ChannelNum <= 11)
-					channel_set[chanset_size].ScanType =
-						SCAN_ACTIVE;
-				else if ((channel_set[chanset_size].ChannelNum >= 12 &&
-					  channel_set[chanset_size].ChannelNum  <= 14))
-					channel_set[chanset_size].ScanType =
-						SCAN_PASSIVE;
-			} else if (RT_CHANNEL_DOMAIN_WORLD_WIDE_13 ==
-				   ChannelPlan ||
-				   RT_CHANNEL_DOMAIN_WORLD_WIDE_5G ==
-				   ChannelPlan ||
+			    RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN_2G == cplan) {
+				if (c_set[ch_size].ChannelNum >= 1 &&
+				    c_set[ch_size].ChannelNum <= 11)
+					c_set[ch_size].ScanType = SCAN_ACTIVE;
+				else if (c_set[ch_size].ChannelNum >= 12 &&
+					 c_set[ch_size].ChannelNum  <= 14)
+					c_set[ch_size].ScanType = SCAN_PASSIVE;
+			} else if (RT_CHANNEL_DOMAIN_WORLD_WIDE_13 == cplan ||
+				   RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == cplan ||
 				   RT_CHANNEL_DOMAIN_2G_WORLD == Index2G) {
 				/*  channel 12~13, passive scan */
-				if (channel_set[chanset_size].ChannelNum <= 11)
-					channel_set[chanset_size].ScanType =
-						SCAN_ACTIVE;
+				if (c_set[ch_size].ChannelNum <= 11)
+					c_set[ch_size].ScanType = SCAN_ACTIVE;
 				else
-					channel_set[chanset_size].ScanType =
-						SCAN_PASSIVE;
+					c_set[ch_size].ScanType = SCAN_PASSIVE;
 			} else
-				channel_set[chanset_size].ScanType =
-					SCAN_ACTIVE;
+				c_set[ch_size].ScanType = SCAN_ACTIVE;
 
-			chanset_size++;
+			ch_size++;
 		}
 	}
 
 	if (b5GBand) {
-		for (index = 0;index<RTW_ChannelPlan5G[Index5G].Len;index++) {
-			if (RTW_ChannelPlan5G[Index5G].Channel[index] <= 48 ||
-			    RTW_ChannelPlan5G[Index5G].Channel[index] >= 149) {
-				channel_set[chanset_size].ChannelNum =
-					RTW_ChannelPlan5G[Index5G].Channel[index];
-				if (RT_CHANNEL_DOMAIN_WORLD_WIDE_5G ==
-				    ChannelPlan) {
+		for (i = 0; i < RTW_ChannelPlan5G[Index5G].Len; i++) {
+			if (RTW_ChannelPlan5G[Index5G].Channel[i] <= 48 ||
+			    RTW_ChannelPlan5G[Index5G].Channel[i] >= 149) {
+				c_set[ch_size].ChannelNum =
+					RTW_ChannelPlan5G[Index5G].Channel[i];
+				if (RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == cplan) {
 					/* passive scan for all 5G channels */
-					channel_set[chanset_size].ScanType =
+					c_set[ch_size].ScanType =
 						SCAN_PASSIVE;
 				} else
-					channel_set[chanset_size].ScanType =
+					c_set[ch_size].ScanType =
 						SCAN_ACTIVE;
 				DBG_8723A("%s(): channel_set[%d].ChannelNum = "
-					  "%d\n", __func__, chanset_size,
-					  channel_set[chanset_size].ChannelNum);
-				chanset_size++;
+					  "%d\n", __func__, ch_size,
+					  c_set[ch_size].ChannelNum);
+				ch_size++;
 			}
 		}
 	}
 
-	return chanset_size;
+	return ch_size;
 }
 
 int init_mlme_ext_priv23a(struct rtw_adapter* padapter)
@@ -547,28 +591,28 @@ _mgt_dispatcher23a(struct rtw_adapter *padapter, struct mlme_handler *ptable,
 void mgt_dispatcher23a(struct rtw_adapter *padapter,
 		    struct recv_frame *precv_frame)
 {
-	int index;
 	struct mlme_handler *ptable;
 #ifdef CONFIG_8723AU_AP_MODE
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 #endif /* CONFIG_8723AU_AP_MODE */
 	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u16 stype;
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
 	struct sta_info *psta;
+	u16 stype;
+	int index;
 
-	if (!ieee80211_is_mgmt(hdr->frame_control))
+	if (!ieee80211_is_mgmt(mgmt->frame_control))
 		return;
 
 	/* receive the frames that ra(a1) is my address or ra(a1) is
 	   bc address. */
-	if (!ether_addr_equal(hdr->addr1, myid(&padapter->eeprompriv)) &&
-	    !is_broadcast_ether_addr(hdr->addr1))
+	if (!ether_addr_equal(mgmt->da, myid(&padapter->eeprompriv)) &&
+	    !is_broadcast_ether_addr(mgmt->da))
 		return;
 
 	ptable = mlme_sta_tbl;
 
-	stype = le16_to_cpu(hdr->frame_control) & IEEE80211_FCTL_STYPE;
+	stype = le16_to_cpu(mgmt->frame_control) & IEEE80211_FCTL_STYPE;
 	index = stype >> 4;
 
 	if (index > 13) {
@@ -579,10 +623,10 @@ void mgt_dispatcher23a(struct rtw_adapter *padapter,
 	}
 	ptable += index;
 
-	psta = rtw_get_stainfo23a(&padapter->stapriv, hdr->addr2);
+	psta = rtw_get_stainfo23a(&padapter->stapriv, mgmt->sa);
 
 	if (psta) {
-		if (ieee80211_has_retry(hdr->frame_control)) {
+		if (ieee80211_has_retry(mgmt->frame_control)) {
 			if (precv_frame->attrib.seq_num ==
 			    psta->RxMgmtFrameSeqNum) {
 				/* drop the duplicate management frame */
@@ -599,7 +643,7 @@ void mgt_dispatcher23a(struct rtw_adapter *padapter,
 	switch (stype)
 	{
 	case IEEE80211_STYPE_AUTH:
-		if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
 			ptable->func = &OnAuth23a;
 		else
 			ptable->func = &OnAuth23aClient23a;
@@ -609,7 +653,7 @@ void mgt_dispatcher23a(struct rtw_adapter *padapter,
 		_mgt_dispatcher23a(padapter, ptable, precv_frame);
 		break;
 	case IEEE80211_STYPE_PROBE_REQ:
-		if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
 			_mgt_dispatcher23a(padapter, ptable, precv_frame);
 		else
 			_mgt_dispatcher23a(padapter, ptable, precv_frame);
@@ -618,13 +662,11 @@ void mgt_dispatcher23a(struct rtw_adapter *padapter,
 		_mgt_dispatcher23a(padapter, ptable, precv_frame);
 		break;
 	case IEEE80211_STYPE_ACTION:
-		/* if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) */
+		/* if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) */
 		_mgt_dispatcher23a(padapter, ptable, precv_frame);
 		break;
 	default:
 		_mgt_dispatcher23a(padapter, ptable, precv_frame);
-		if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
-			rtw_hostapd_mlme_rx23a(padapter, precv_frame);
 		break;
 	}
 #else
@@ -632,186 +674,63 @@ void mgt_dispatcher23a(struct rtw_adapter *padapter,
 #endif
 }
 
-#ifdef CONFIG_8723AU_P2P
-static u32 p2p_listen_state_process(struct rtw_adapter *padapter,
-				    unsigned char *da)
-{
-	bool response = true;
-
-	if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled == false ||
-	    padapter->mlmepriv.wps_probe_resp_ie == NULL ||
-	    padapter->mlmepriv.p2p_probe_resp_ie == NULL) {
-		DBG_8723A("DON'T issue_probersp23a_p2p23a: p2p_enabled:%d, "
-			  "wps_probe_resp_ie:%p, p2p_probe_resp_ie:%p\n",
-			  wdev_to_priv(padapter->rtw_wdev)->p2p_enabled,
-			  padapter->mlmepriv.wps_probe_resp_ie,
-			  padapter->mlmepriv.p2p_probe_resp_ie);
-		response = false;
-	}
-
-	if (response == true)
-		issue_probersp23a_p2p23a(padapter, da);
-
-	return _SUCCESS;
-}
-#endif /* CONFIG_8723AU_P2P */
-
 /****************************************************************************
 
 Following are the callback functions for each subtype of the management frames
 
 *****************************************************************************/
 
-unsigned int OnProbeReq23a(struct rtw_adapter *padapter,
-			   struct recv_frame *precv_frame)
+static int
+OnProbeReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
-	unsigned int	ielen;
-	unsigned char	*p;
+	const u8 *ie;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *cur = &pmlmeinfo->network;
 	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u8 *pframe = skb->data;
-	uint len = skb->len;
-	u8 is_valid_p2p_probereq = false;
-
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	u8 wifi_test_chk_rate = 1;
-
-	if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) &&
-	    !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE) &&
-	    !rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) &&
-	    !rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH) &&
-	    !rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN)) {
-		/*	mcs_rate = 0 -> CCK 1M rate */
-		/*	mcs_rate = 1 -> CCK 2M rate */
-		/*	mcs_rate = 2 -> CCK 5.5M rate */
-		/*	mcs_rate = 3 -> CCK 11M rate */
-		/*	In the P2P mode, the driver should not support
-			the CCK rate */
-
-		/*	IOT issue: Google Nexus7 use 1M rate to send
-			p2p_probe_req after GO nego completed and Nexus7
-			is client */
-		if (wifi_test_chk_rate == 1) {
-			if ((is_valid_p2p_probereq =
-			     process_probe_req_p2p_ie23a(pwdinfo, pframe,
-							 len)) == true) {
-				if (rtw_p2p_chk_role(pwdinfo,
-						     P2P_ROLE_DEVICE)) {
-					u8 *sa = ieee80211_get_SA(hdr);
-					p2p_listen_state_process(padapter, sa);
-					return _SUCCESS;
-				}
-
-				if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
-					goto _continue;
-				}
-			}
-		}
-	}
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
+	int len = skb->len;
 
-_continue:
-#endif /* CONFIG_8723AU_P2P */
-
-	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
 		return _SUCCESS;
-	}
 
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == false &&
-		check_fwstate(pmlmepriv,
-			      WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE) == false) {
+	if (!check_fwstate(pmlmepriv, _FW_LINKED) &&
+	    !check_fwstate(pmlmepriv,
+			   WIFI_ADHOC_MASTER_STATE | WIFI_AP_STATE))
 		return _SUCCESS;
-	}
 
-	p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) +
-			  _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ielen,
-			  len - sizeof(struct ieee80211_hdr_3addr) -
-			  _PROBEREQ_IE_OFFSET_);
+	if (unlikely(!ieee80211_is_probe_req(mgmt->frame_control))) {
+		printk(KERN_WARNING "%s: Received non probe request frame\n",
+		       __func__);
+		return _FAIL;
+	}
 
-	/* check (wildcard) SSID */
-	if (p) {
-		if (is_valid_p2p_probereq == true) {
-			goto _issue_probersp23a;
-		}
+	len -= offsetof(struct ieee80211_mgmt, u.probe_req.variable);
 
-		if ((ielen != 0 &&
-		     memcmp((void *)(p+2), cur->Ssid.ssid,
-			    cur->Ssid.ssid_len)) ||
-		    (ielen == 0 && pmlmeinfo->hidden_ssid_mode)) {
-			return _SUCCESS;
-		}
+	ie = cfg80211_find_ie(WLAN_EID_SSID, mgmt->u.probe_req.variable, len);
 
-_issue_probersp23a:
+	/* check (wildcard) SSID */
+	if (!ie)
+		goto out;
 
-		if (check_fwstate(pmlmepriv, _FW_LINKED) == true &&
-		    pmlmepriv->cur_network.join_res == true) {
-			/* DBG_8723A("+issue_probersp23a during ap mode\n"); */
-			issue_probersp23a(padapter, ieee80211_get_SA(hdr),
-					  is_valid_p2p_probereq);
-		}
+	if ((ie[1] && memcmp(ie + 2, cur->Ssid.ssid, cur->Ssid.ssid_len)) ||
+	    (ie[1] == 0 && pmlmeinfo->hidden_ssid_mode)) {
+		return _SUCCESS;
 	}
 
+	if (check_fwstate(pmlmepriv, _FW_LINKED) &&
+	    pmlmepriv->cur_network.join_res)
+		issue_probersp(padapter, mgmt->sa, false);
+
+out:
 	return _SUCCESS;
 }
 
-unsigned int OnProbeRsp23a(struct rtw_adapter *padapter,
-			   struct recv_frame *precv_frame)
+static int
+OnProbeRsp23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-#ifdef CONFIG_8723AU_P2P
-	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
-#endif
-
-#ifdef CONFIG_8723AU_P2P
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ))	{
-		if (pwdinfo->tx_prov_disc_info.benable == true) {
-			if (ether_addr_equal(pwdinfo->tx_prov_disc_info.peerIFAddr,
-				    hdr->addr2)) {
-				if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
-					pwdinfo->tx_prov_disc_info.benable = false;
-					issue_p2p_provision_request23a(padapter,
-								    				pwdinfo->tx_prov_disc_info.ssid.ssid,
-												pwdinfo->tx_prov_disc_info.ssid.ssid_len,
-												pwdinfo->tx_prov_disc_info.peerDevAddr);
-				}
-				else if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-				{
-					pwdinfo->tx_prov_disc_info.benable = false;
-					issue_p2p_provision_request23a(padapter,
-												NULL,
-												0,
-												pwdinfo->tx_prov_disc_info.peerDevAddr);
-				}
-			}
-		}
-		return _SUCCESS;
-	} else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING)) {
-		if (pwdinfo->nego_req_info.benable == true) {
-			DBG_8723A("[%s] P2P State is GONEGO ING!\n", __func__);
-			if (ether_addr_equal(pwdinfo->nego_req_info.peerDevAddr,
-					     hdr->addr2)) {
-				pwdinfo->nego_req_info.benable = false;
-				issue_p2p_GO_request23a(padapter, pwdinfo->nego_req_info.peerDevAddr);
-			}
-		}
-	} else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ)) {
-		if (pwdinfo->invitereq_info.benable == true) {
-			DBG_8723A("[%s] P2P_STATE_TX_INVITE_REQ!\n", __func__);
-			if (ether_addr_equal(
-				    pwdinfo->invitereq_info.peer_macaddr,
-				    hdr->addr2)) {
-				pwdinfo->invitereq_info.benable = false;
-				issue_p2p_invitation_request23a(padapter, pwdinfo->invitereq_info.peer_macaddr);
-			}
-		}
-	}
-#endif
 
 	if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
 		report_survey_event23a(padapter, precv_frame);
@@ -821,30 +740,30 @@ unsigned int OnProbeRsp23a(struct rtw_adapter *padapter,
 	return _SUCCESS;
 }
 
-unsigned int OnBeacon23a(struct rtw_adapter *padapter,
-			 struct recv_frame *precv_frame)
+static int
+OnBeacon23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
 	int cam_idx;
 	struct sta_info	*psta;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct sta_priv	*pstapriv = &padapter->stapriv;
 	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
 	u8 *pframe = skb->data;
-	uint len = skb->len;
+	int pkt_len = skb->len;
 	struct wlan_bssid_ex *pbss;
 	int ret = _SUCCESS;
-	u8 *p = NULL;
+	u8 *p, *pie;
+	int pie_len;
 	u32 ielen = 0;
 
-	p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) +
-			  _BEACON_IE_OFFSET_, _EXT_SUPPORTEDRATES_IE_, &ielen,
-			  len - sizeof(struct ieee80211_hdr_3addr) -
-			  _BEACON_IE_OFFSET_);
-	if ((p != NULL) && (ielen > 0)) {
-		if ((*(p + 1 + ielen) == 0x2D) && (*(p + 2 + ielen) != 0x2D)) {
+	pie = mgmt->u.beacon.variable;
+	pie_len = pkt_len - offsetof(struct ieee80211_mgmt, u.beacon.variable);
+	p = rtw_get_ie23a(pie, WLAN_EID_EXT_SUPP_RATES, &ielen, pie_len);
+	if (p && ielen > 0) {
+		if (p[1 + ielen] == 0x2D && p[2 + ielen] != 0x2D) {
 			/* Invalid value 0x2D is detected in Extended Supported
 			 * Rates (ESR) IE. Try to fix the IE length to avoid
 			 * failed Beacon parsing.
@@ -852,8 +771,8 @@ unsigned int OnBeacon23a(struct rtw_adapter *padapter,
 			DBG_8723A("[WIFIDBG] Error in ESR IE is detected in "
 				  "Beacon of BSSID: %pM. Fix the length of "
 				  "ESR IE to avoid failed Beacon parsing.\n",
-				  hdr->addr3);
-			*(p + 1) = ielen - 1;
+				  mgmt->bssid);
+			p[1] = ielen - 1;
 		}
 	}
 
@@ -862,141 +781,142 @@ unsigned int OnBeacon23a(struct rtw_adapter *padapter,
 		return _SUCCESS;
 	}
 
-	if (ether_addr_equal(hdr->addr3, get_my_bssid23a(&pmlmeinfo->network))){
-		if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
-			/* we should update current network before auth,
-			   or some IE is wrong */
-			pbss = (struct wlan_bssid_ex *)
-				kmalloc(sizeof(struct wlan_bssid_ex),
-					GFP_ATOMIC);
-			if (pbss) {
-				if (collect_bss_info23a(padapter, precv_frame,
-							pbss) == _SUCCESS) {
-					update_network23a(&pmlmepriv->cur_network.network, pbss, padapter, true);
-					rtw_get_bcn_info23a(&pmlmepriv->cur_network);
-				}
-				kfree(pbss);
-			}
+	if (!ether_addr_equal(mgmt->bssid,
+			      get_my_bssid23a(&pmlmeinfo->network)))
+		goto out;
 
-			/* check the vendor of the assoc AP */
-			pmlmeinfo->assoc_AP_vendor = check_assoc_AP23a(pframe + sizeof(struct ieee80211_hdr_3addr), len-sizeof(struct ieee80211_hdr_3addr));
+	if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
+		/* we should update current network before auth,
+		   or some IE is wrong */
+		pbss = (struct wlan_bssid_ex *)
+			kmalloc(sizeof(struct wlan_bssid_ex), GFP_ATOMIC);
+		if (pbss) {
+			if (collect_bss_info23a(padapter, precv_frame, pbss) ==
+			    _SUCCESS) {
+				update_network23a(
+					&pmlmepriv->cur_network.network, pbss,
+					padapter, true);
+				rtw_get_bcn_info23a(&pmlmepriv->cur_network);
+			}
+			kfree(pbss);
+		}
 
-			/* update TSF Value */
-			update_TSF23a(pmlmeext, pframe, len);
+		/* check the vendor of the assoc AP */
+		pmlmeinfo->assoc_AP_vendor =
+			check_assoc_AP23a((u8 *)&mgmt->u.beacon, pkt_len -
+					  offsetof(struct ieee80211_mgmt, u));
 
-			/* start auth */
-			start_clnt_auth23a(padapter);
+		/* update TSF Value */
+		rtw_update_TSF(pmlmeext, mgmt);
 
-			return _SUCCESS;
-		}
+		/* start auth */
+		start_clnt_auth(padapter);
 
-		if (((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) &&
-		    (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) {
-			psta = rtw_get_stainfo23a(pstapriv, hdr->addr2);
-			if (psta) {
-				ret = rtw_check_bcn_info23a(padapter, pframe,
-							    len);
-				if (!ret) {
-					DBG_8723A_LEVEL(_drv_always_,
-							"ap has changed, "
-							"disconnect now\n");
-					receive_disconnect23a(padapter, pmlmeinfo->network.MacAddress, 65535);
-					return _SUCCESS;
-				}
-				/* update WMM, ERP in the beacon */
-				/* todo: the timer is used instead of
-				   the number of the beacon received */
-				if ((sta_rx_pkts(psta) & 0xf) == 0) {
-					/* DBG_8723A("update_bcn_info\n"); */
-					update_beacon23a_info(padapter, pframe,
-							      len, psta);
-				}
+		return _SUCCESS;
+	}
 
-#ifdef CONFIG_8723AU_P2P
-				process_p2p_ps_ie23a(padapter, (pframe + sizeof(struct ieee80211_hdr_3addr)), (len - sizeof(struct ieee80211_hdr_3addr)));
-#endif /* CONFIG_8723AU_P2P */
+	if (((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE) &&
+	    (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) {
+		psta = rtw_get_stainfo23a(pstapriv, mgmt->sa);
+		if (psta) {
+			ret = rtw_check_bcn_info23a(padapter, mgmt, pkt_len);
+			if (!ret) {
+				DBG_8723A_LEVEL(_drv_always_, "ap has changed, "
+						"disconnect now\n");
+				receive_disconnect23a(padapter, pmlmeinfo->network.MacAddress, 65535);
+				return _SUCCESS;
+			}
+			/* update WMM, ERP in the beacon */
+			/* todo: the timer is used instead of
+			   the number of the beacon received */
+			if ((sta_rx_pkts(psta) & 0xf) == 0) {
+				/* DBG_8723A("update_bcn_info\n"); */
+				update_beacon23a_info(padapter, pframe,
+						      pkt_len, psta);
+			}
+		}
+	} else if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
+		psta = rtw_get_stainfo23a(pstapriv, mgmt->sa);
+		if (psta) {
+			/* update WMM, ERP in the beacon */
+			/* todo: the timer is used instead of the
+			   number of the beacon received */
+			if ((sta_rx_pkts(psta) & 0xf) == 0) {
+				/* DBG_8723A("update_bcn_info\n"); */
+				update_beacon23a_info(padapter, pframe,
+						      pkt_len, psta);
+			}
+		} else {
+			/* allocate a new CAM entry for IBSS station */
+			cam_idx = allocate_fw_sta_entry23a(padapter);
+			if (cam_idx == NUM_STA)
+				goto out;
+
+			/* get supported rate */
+			if (update_sta_support_rate23a(padapter, pie, pie_len,
+						       cam_idx) == _FAIL) {
+				pmlmeinfo->FW_sta_info[cam_idx].status = 0;
+				goto out;
 			}
-		} else if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
-			psta = rtw_get_stainfo23a(pstapriv, hdr->addr2);
-			if (psta) {
-				/* update WMM, ERP in the beacon */
-				/* todo: the timer is used instead of the
-				   number of the beacon received */
-				if ((sta_rx_pkts(psta) & 0xf) == 0) {
-					/* DBG_8723A("update_bcn_info\n"); */
-					update_beacon23a_info(padapter, pframe,
-							      len, psta);
-				}
-			} else {
-				/* allocate a new CAM entry for IBSS station */
-				cam_idx = allocate_fw_sta_entry23a(padapter);
-				if (cam_idx == NUM_STA)
-					goto _END_ONBEACON_;
-
-				/* get supported rate */
-				if (update_sta_support_rate23a(padapter, (pframe + sizeof(struct ieee80211_hdr_3addr) + _BEACON_IE_OFFSET_), (len - sizeof(struct ieee80211_hdr_3addr) - _BEACON_IE_OFFSET_), cam_idx) == _FAIL) {
-					pmlmeinfo->FW_sta_info[cam_idx].status = 0;
-					goto _END_ONBEACON_;
-				}
 
-				/* update TSF Value */
-				update_TSF23a(pmlmeext, pframe, len);
+			/* update TSF Value */
+			rtw_update_TSF(pmlmeext, mgmt);
 
-				/* report sta add event */
-				report_add_sta_event23a(padapter, hdr->addr2,
-							cam_idx);
-			}
+			/* report sta add event */
+			report_add_sta_event23a(padapter, mgmt->sa,
+						cam_idx);
 		}
 	}
 
-_END_ONBEACON_:
+out:
 
 	return _SUCCESS;
 }
 
-unsigned int OnAuth23a(struct rtw_adapter *padapter,
-		       struct recv_frame *precv_frame)
-{
 #ifdef CONFIG_8723AU_AP_MODE
-	unsigned int	auth_mode, seq, ie_len;
-	unsigned char	*sa, *p;
-	u16	algorithm;
-	int	status;
+static int
+OnAuth23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
+{
 	static struct sta_info stat;
-	struct	sta_info	*pstat = NULL;
-	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *pstat = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u8 *pframe = skb->data;
-	uint len = skb->len;
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
+	u8 *pframe;
+	const u8 *p;
+	unsigned char *sa;
+	u16 auth_mode, seq, algorithm;
+	int status, len = skb->len;
 
-	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+	if ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)
 		return _FAIL;
 
 	DBG_8723A("+OnAuth23a\n");
 
-	sa = hdr->addr2;
+	sa = mgmt->sa;
 
 	auth_mode = psecuritypriv->dot11AuthAlgrthm;
-	seq = cpu_to_le16(*(u16*)((unsigned long)pframe +
-				  sizeof(struct ieee80211_hdr_3addr) + 2));
-	algorithm = cpu_to_le16(*(u16*)((unsigned long)pframe +
-					sizeof(struct ieee80211_hdr_3addr)));
+
+	pframe = mgmt->u.auth.variable;
+	len = skb->len - offsetof(struct ieee80211_mgmt, u.auth.variable);
+
+	seq = le16_to_cpu(mgmt->u.auth.auth_transaction);
+	algorithm = le16_to_cpu(mgmt->u.auth.auth_alg);
 
 	DBG_8723A("auth alg =%x, seq =%X\n", algorithm, seq);
 
 	if (auth_mode == 2 &&
-	    psecuritypriv->dot11PrivacyAlgrthm != _WEP40_ &&
-	    psecuritypriv->dot11PrivacyAlgrthm != _WEP104_)
+	    psecuritypriv->dot11PrivacyAlgrthm != WLAN_CIPHER_SUITE_WEP40 &&
+	    psecuritypriv->dot11PrivacyAlgrthm != WLAN_CIPHER_SUITE_WEP104)
 		auth_mode = 0;
 
 	/*  rx a shared-key auth but shared not enabled, or */
 	/*  rx a open-system auth but shared-key is enabled */
-	if ((algorithm > 0 && auth_mode == 0) ||
-	    (algorithm == 0 && auth_mode == 1)) {
+	if ((algorithm != WLAN_AUTH_OPEN && auth_mode == 0) ||
+	    (algorithm == WLAN_AUTH_OPEN && auth_mode == 1)) {
 		DBG_8723A("auth rejected due to bad alg [alg =%d, auth_mib "
 			  "=%d] %02X%02X%02X%02X%02X%02X\n",
 			  algorithm, auth_mode,
@@ -1017,7 +937,7 @@ unsigned int OnAuth23a(struct rtw_adapter *padapter,
 		/*  allocate a new one */
 		DBG_8723A("going to alloc stainfo for sa ="MAC_FMT"\n",
 			  MAC_ARG(sa));
-		pstat = rtw_alloc_stainfo23a(pstapriv, sa);
+		pstat = rtw_alloc_stainfo23a(pstapriv, sa, GFP_ATOMIC);
 		if (!pstat) {
 			DBG_8723A(" Exceed the upper limit of supported "
 				  "clients...\n");
@@ -1035,8 +955,7 @@ unsigned int OnAuth23a(struct rtw_adapter *padapter,
 		if (!list_empty(&pstat->asoc_list)) {
 			list_del_init(&pstat->asoc_list);
 			pstapriv->asoc_list_cnt--;
-			if (pstat->expire_to > 0)
-			{
+			if (pstat->expire_to > 0) {
 				/* TODO: STA re_auth within expire_to */
 			}
 		}
@@ -1064,7 +983,7 @@ unsigned int OnAuth23a(struct rtw_adapter *padapter,
 		goto auth_fail;
 	}
 
-	if (algorithm == 0 && (auth_mode == 0 || auth_mode == 2)) {
+	if (algorithm == WLAN_AUTH_OPEN && (auth_mode == 0 || auth_mode == 2)) {
 		if (seq == 1) {
 			pstat->state &= ~WIFI_FW_AUTH_NULL;
 			pstat->state |= WIFI_FW_AUTH_SUCCESS;
@@ -1088,22 +1007,16 @@ unsigned int OnAuth23a(struct rtw_adapter *padapter,
 			/* checking for challenging txt... */
 			DBG_8723A("checking for challenging txt...\n");
 
-			p = rtw_get_ie23a(pframe +
-					  sizeof(struct ieee80211_hdr_3addr) +
-					  4 + _AUTH_IE_OFFSET_, _CHLGETXT_IE_,
-					  (int *)&ie_len, len -
-					  sizeof(struct ieee80211_hdr_3addr) -
-					  _AUTH_IE_OFFSET_ - 4);
-
-			if ((p == NULL) || (ie_len<= 0)) {
+			p = cfg80211_find_ie(WLAN_EID_CHALLENGE, pframe, len);
+			if (!p || p[1] <= 0) {
 				DBG_8723A("auth rejected because challenge "
 					  "failure!(1)\n");
 				status = WLAN_STATUS_CHALLENGE_FAIL;
 				goto auth_fail;
 			}
 
-			if (!memcmp((void *)(p + 2), pstat->chg_txt, 128)) {
-				pstat->state &= (~WIFI_FW_AUTH_STATE);
+			if (!memcmp(p + 2, pstat->chg_txt, 128)) {
+				pstat->state &= ~WIFI_FW_AUTH_STATE;
 				pstat->state |= WIFI_FW_AUTH_SUCCESS;
 				/*  challenging txt is correct... */
 				pstat->expire_to =  pstapriv->assoc_to;
@@ -1122,10 +1035,10 @@ unsigned int OnAuth23a(struct rtw_adapter *padapter,
 		}
 	}
 
-	/*  Now, we are going to issue_auth23a... */
+	/*  Now, we are going to issue_auth... */
 	pstat->auth_seq = seq + 1;
 
-	issue_auth23a(padapter, pstat, (unsigned short)WLAN_STATUS_SUCCESS);
+	issue_auth(padapter, pstat, WLAN_STATUS_SUCCESS);
 
 	if (pstat->state & WIFI_FW_AUTH_SUCCESS)
 		pstat->auth_seq = 0;
@@ -1142,46 +1055,45 @@ auth_fail:
 	pstat->auth_seq = 2;
 	memcpy(pstat->hwaddr, sa, 6);
 
-	issue_auth23a(padapter, pstat, (unsigned short)status);
+	issue_auth(padapter, pstat, (unsigned short)status);
 
-#endif
 	return _FAIL;
 }
+#endif
 
-unsigned int OnAuth23aClient23a(struct rtw_adapter *padapter,
-				struct recv_frame *precv_frame)
+static int
+OnAuth23aClient23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
-	unsigned int	seq, len, status, algthm, offset;
-	unsigned char	*p;
-	unsigned int	go2asoc = 0;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	unsigned int seq, status, algthm;
+	unsigned int go2asoc = 0;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u8 *pframe = skb->data;
-	uint pkt_len = skb->len;
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
+	const u8 *p;
+	u8 *pie;
+	int plen = skb->len;
 
 	DBG_8723A("%s\n", __func__);
 
 	/* check A1 matches or not */
-	if (!ether_addr_equal(myid(&padapter->eeprompriv),
-			      ieee80211_get_DA(hdr)))
+	if (!ether_addr_equal(myid(&padapter->eeprompriv), mgmt->da))
 		return _SUCCESS;
 
 	if (!(pmlmeinfo->state & WIFI_FW_AUTH_STATE))
 		return _SUCCESS;
 
-	offset = ieee80211_has_protected(hdr->frame_control) ? 4: 0;
+	pie = mgmt->u.auth.variable;
+	plen -= offsetof(struct ieee80211_mgmt, u.auth.variable);
 
-	algthm	= le16_to_cpu(*(unsigned short *)((unsigned long)pframe + sizeof(struct ieee80211_hdr_3addr) + offset));
-	seq	= le16_to_cpu(*(unsigned short *)((unsigned long)pframe + sizeof(struct ieee80211_hdr_3addr) + offset + 2));
-	status	= le16_to_cpu(*(unsigned short *)((unsigned long)pframe + sizeof(struct ieee80211_hdr_3addr) + offset + 4));
+	algthm = le16_to_cpu(mgmt->u.auth.auth_alg);
+	seq = le16_to_cpu(mgmt->u.auth.auth_transaction);
+	status = le16_to_cpu(mgmt->u.auth.status_code);
 
-	if (status != 0)
-	{
+	if (status) {
 		DBG_8723A("clnt auth fail, status: %d\n", status);
-		if (status == 13)/*  pmlmeinfo->auth_algo == dot11AuthAlgrthm_Auto) */
-		{
+		/*  pmlmeinfo->auth_algo == dot11AuthAlgrthm_Auto) */
+		if (status == WLAN_STATUS_NOT_SUPPORTED_AUTH_ALG) {
 			if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
 				pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
 			else
@@ -1193,55 +1105,41 @@ unsigned int OnAuth23aClient23a(struct rtw_adapter *padapter,
 		goto authclnt_fail;
 	}
 
-	if (seq == 2)
-	{
-		if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
-		{
-			 /*  legendary shared system */
-			p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, (int *)&len,
-				pkt_len - sizeof(struct ieee80211_hdr_3addr) - _AUTH_IE_OFFSET_);
+	if (seq == 2) {
+		if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) {
+			/*  legendary shared system */
+			p = cfg80211_find_ie(WLAN_EID_CHALLENGE, pie, plen);
 
-			if (p == NULL)
-			{
+			if (!p) {
 				/* DBG_8723A("marc: no challenge text?\n"); */
 				goto authclnt_fail;
 			}
 
-			memcpy((void *)(pmlmeinfo->chg_txt), (void *)(p + 2), len);
+			memcpy((void *)(pmlmeinfo->chg_txt), p + 2, p[1]);
 			pmlmeinfo->auth_seq = 3;
-			issue_auth23a(padapter, NULL, 0);
+			issue_auth(padapter, NULL, 0);
 			set_link_timer(pmlmeext, REAUTH_TO);
 
 			return _SUCCESS;
-		}
-		else
-		{
+		} else {
 			/*  open system */
 			go2asoc = 1;
 		}
-	}
-	else if (seq == 4)
-	{
+	} else if (seq == 4) {
 		if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
-		{
 			go2asoc = 1;
-		}
 		else
-		{
 			goto authclnt_fail;
-		}
-	}
-	else
-	{
+	} else {
 		/*  this is also illegal */
-		/* DBG_8723A("marc: clnt auth failed due to illegal seq =%x\n", seq); */
+		/* DBG_8723A("marc: clnt auth failed due to illegal seq =%x\n",
+		   seq); */
 		goto authclnt_fail;
 	}
 
-	if (go2asoc)
-	{
+	if (go2asoc) {
 		DBG_8723A_LEVEL(_drv_always_, "auth success, start assoc\n");
-		start_clnt_assoc23a(padapter);
+		start_clnt_assoc(padapter);
 		return _SUCCESS;
 	}
 
@@ -1252,19 +1150,153 @@ authclnt_fail:
 	return _FAIL;
 }
 
-unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
+#ifdef CONFIG_8723AU_AP_MODE
+static int rtw_validate_vendor_specific_ies(const u8 *pos, int elen)
+{
+	unsigned int oui;
+
+	/* first 3 bytes in vendor specific information element are the IEEE
+	 * OUI of the vendor. The following byte is used a vendor specific
+	 * sub-type. */
+	if (elen < 4) {
+		DBG_8723A("short vendor specific information element "
+			  "ignored (len =%i)\n", elen);
+		return -EINVAL;
+	}
+
+	oui = RTW_GET_BE24(pos);
+	switch (oui) {
+	case WLAN_OUI_MICROSOFT:
+		/* Microsoft/Wi-Fi information elements are further typed and
+		 * subtyped */
+		switch (pos[3]) {
+		case 1:
+			/* Microsoft OUI (00:50:F2) with OUI Type 1:
+			 * real WPA information element */
+			break;
+		case WME_OUI_TYPE: /* this is a Wi-Fi WME info. element */
+			if (elen < 5) {
+				DBG_8723A("short WME information element "
+					  "ignored (len =%i)\n", elen);
+				return -EINVAL;
+			}
+			switch (pos[4]) {
+			case WME_OUI_SUBTYPE_INFORMATION_ELEMENT:
+			case WME_OUI_SUBTYPE_PARAMETER_ELEMENT:
+				break;
+			case WME_OUI_SUBTYPE_TSPEC_ELEMENT:
+				break;
+			default:
+				DBG_8723A("unknown WME information element "
+					  "ignored (subtype =%d len =%i)\n",
+					   pos[4], elen);
+				return -EINVAL;
+			}
+			break;
+		case 4:
+			/* Wi-Fi Protected Setup (WPS) IE */
+			break;
+		default:
+			DBG_8723A("Unknown Microsoft information element "
+				  "ignored (type =%d len =%i)\n",
+				  pos[3], elen);
+			return -EINVAL;
+		}
+		break;
+
+	case OUI_BROADCOM:
+		switch (pos[3]) {
+		case VENDOR_HT_CAPAB_OUI_TYPE:
+			break;
+		default:
+			DBG_8723A("Unknown Broadcom information element "
+				  "ignored (type =%d len =%i)\n", pos[3], elen);
+			return -EINVAL;
+		}
+		break;
+
+	default:
+		DBG_8723A("unknown vendor specific information element "
+			  "ignored (vendor OUI %02x:%02x:%02x len =%i)\n",
+			   pos[0], pos[1], pos[2], elen);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int rtw_validate_frame_ies(const u8 *start, uint len)
+{
+	const u8 *pos = start;
+	int left = len;
+	int unknown = 0;
+
+	while (left >= 2) {
+		u8 id, elen;
+
+		id = *pos++;
+		elen = *pos++;
+		left -= 2;
+
+		if (elen > left) {
+			DBG_8723A("%s: IEEE 802.11 failed (id =%d elen =%d "
+				  "left =%i)\n", __func__, id, elen, left);
+			return -EINVAL;
+		}
+
+		switch (id) {
+		case WLAN_EID_SSID:
+		case WLAN_EID_SUPP_RATES:
+		case WLAN_EID_FH_PARAMS:
+		case WLAN_EID_DS_PARAMS:
+		case WLAN_EID_CF_PARAMS:
+		case WLAN_EID_TIM:
+		case WLAN_EID_IBSS_PARAMS:
+		case WLAN_EID_CHALLENGE:
+		case WLAN_EID_ERP_INFO:
+		case WLAN_EID_EXT_SUPP_RATES:
+		case WLAN_EID_VENDOR_SPECIFIC:
+			if (rtw_validate_vendor_specific_ies(pos, elen))
+				unknown++;
+			break;
+		case WLAN_EID_RSN:
+		case WLAN_EID_PWR_CAPABILITY:
+		case WLAN_EID_SUPPORTED_CHANNELS:
+		case WLAN_EID_MOBILITY_DOMAIN:
+		case WLAN_EID_FAST_BSS_TRANSITION:
+		case WLAN_EID_TIMEOUT_INTERVAL:
+		case WLAN_EID_HT_CAPABILITY:
+		case WLAN_EID_HT_OPERATION:
+		default:
+			unknown++;
+			DBG_8723A("%s IEEE 802.11 ignored unknown element "
+				  "(id =%d elen =%d)\n", __func__, id, elen);
+			break;
+		}
+
+		left -= elen;
+		pos += elen;
+	}
+
+	if (left)
+		return -EINVAL;
+
+	return 0;
+}
+#endif
+
+static int
+OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
 #ifdef CONFIG_8723AU_AP_MODE
 	u16 capab_info, listen_interval;
-	struct rtw_ieee802_11_elems elems;
 	struct sta_info	*pstat;
-	unsigned char		reassoc, *p, *pos, *wpa_ie;
+	unsigned char reassoc;
 	unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
-	int		i, ie_len, wpa_ie_len, left;
-	unsigned char		supportRate[16];
-	int					supportRateNum;
-	unsigned short		status = WLAN_STATUS_SUCCESS;
-	unsigned short ie_offset;
+	int i, wpa_ie_len, left;
+	unsigned char supportRate[16];
+	int supportRateNum;
+	unsigned short status = WLAN_STATUS_SUCCESS;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
@@ -1272,69 +1304,56 @@ unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *prec
 	struct wlan_bssid_ex *cur = &pmlmeinfo->network;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct sk_buff *skb = precv_frame->pkt;
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
+	const u8 *pos, *p, *wpa_ie, *wps_ie;
 	u8 *pframe = skb->data;
 	uint pkt_len = skb->len;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u16 frame_control;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	u8 p2p_status_code = P2P_STATUS_SUCCESS;
-	u8 *p2pie;
-	u32 p2pielen = 0;
-	u8	wfd_ie[MAX_WFD_IE_LEN] = { 0x00 };
-	u32	wfd_ielen = 0;
-#endif /* CONFIG_8723AU_P2P */
+	int r;
 
-	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+	if ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)
 		return _FAIL;
 
-	frame_control = hdr->frame_control;
-	if (ieee80211_is_assoc_req(frame_control)) {
+	left = pkt_len - sizeof(struct ieee80211_hdr_3addr);
+	if (ieee80211_is_assoc_req(mgmt->frame_control)) {
 		reassoc = 0;
-		ie_offset = _ASOCREQ_IE_OFFSET_;
+		pos = mgmt->u.assoc_req.variable;
+		left -= offsetof(struct ieee80211_mgmt, u.assoc_req.variable);
 	} else { /*  WIFI_REASSOCREQ */
 		reassoc = 1;
-		ie_offset = _REASOCREQ_IE_OFFSET_;
+		pos = mgmt->u.reassoc_req.variable;
+		left -= offsetof(struct ieee80211_mgmt, u.reassoc_req.variable);
 	}
 
-	if (pkt_len < sizeof(struct ieee80211_hdr_3addr) + ie_offset) {
-		DBG_8723A("handle_assoc(reassoc =%d) - too short payload (len =%lu)"
-		       "\n", reassoc, (unsigned long)pkt_len);
+	if (left < 0) {
+		DBG_8723A("handle_assoc(reassoc =%d) - too short payload "
+			  "(len =%lu)\n", reassoc, (unsigned long)pkt_len);
 		return _FAIL;
 	}
 
-	pstat = rtw_get_stainfo23a(pstapriv, hdr->addr2);
+	pstat = rtw_get_stainfo23a(pstapriv, mgmt->sa);
 	if (!pstat) {
 		status = WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA;
 		goto asoc_class2_error;
 	}
 
-	capab_info = get_unaligned_le16(pframe + sizeof(struct ieee80211_hdr_3addr));
-	/* capab_info = le16_to_cpu(*(unsigned short *)(pframe + sizeof(struct ieee80211_hdr_3addr))); */
-	/* listen_interval = le16_to_cpu(*(unsigned short *)(pframe + sizeof(struct ieee80211_hdr_3addr)+2)); */
-	listen_interval = get_unaligned_le16(pframe + sizeof(struct ieee80211_hdr_3addr)+2);
-
-	left = pkt_len - (sizeof(struct ieee80211_hdr_3addr) + ie_offset);
-	pos = pframe + (sizeof(struct ieee80211_hdr_3addr) + ie_offset);
+	/* These two are located at the same offsets whether it's an
+	 * assoc_req or a reassoc_req */
+	capab_info = get_unaligned_le16(&mgmt->u.assoc_req.capab_info);
+	listen_interval =
+		get_unaligned_le16(&mgmt->u.assoc_req.listen_interval);
 
 	DBG_8723A("%s\n", __func__);
 
 	/*  check if this stat has been successfully authenticated/assocated */
-	if (!((pstat->state) & WIFI_FW_AUTH_SUCCESS))
-	{
-		if (!((pstat->state) & WIFI_FW_ASSOC_SUCCESS))
-		{
+	if (!(pstat->state & WIFI_FW_AUTH_SUCCESS)) {
+		if (!(pstat->state & WIFI_FW_ASSOC_SUCCESS)) {
 			status = WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA;
 			goto asoc_class2_error;
-		}
-		else
-		{
+		} else {
 			pstat->state &= (~WIFI_FW_ASSOC_SUCCESS);
 			pstat->state |= WIFI_FW_ASSOC_STATE;
 		}
-	}
-	else
-	{
+	} else {
 		pstat->state &= (~WIFI_FW_AUTH_SUCCESS);
 		pstat->state |= WIFI_FW_ASSOC_STATE;
 	}
@@ -1342,41 +1361,40 @@ unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *prec
 	pstat->capability = capab_info;
 
 	/* now parse all ieee802_11 ie to point to elems */
-	if (rtw_ieee802_11_parse_elems23a(pos, left, &elems, 1) == ParseFailed ||
-	    !elems.ssid) {
+
+	if (rtw_validate_frame_ies(pos, left)) {
 		DBG_8723A("STA " MAC_FMT " sent invalid association request\n",
-		       MAC_ARG(pstat->hwaddr));
+			  MAC_ARG(pstat->hwaddr));
 		status = WLAN_STATUS_UNSPECIFIED_FAILURE;
 		goto OnAssocReq23aFail;
 	}
 
 	/*  now we should check all the fields... */
 	/*  checking SSID */
-	p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + ie_offset, _SSID_IE_, &ie_len,
-		pkt_len - sizeof(struct ieee80211_hdr_3addr) - ie_offset);
-	if (p == NULL)
-	{
-		status = WLAN_STATUS_UNSPECIFIED_FAILURE;
-	}
-
-	if (ie_len == 0) /*  broadcast ssid, however it is not allowed in assocreq */
+	p = cfg80211_find_ie(WLAN_EID_SSID, pos, left);
+	if (!p || p[1] == 0) {
+		/*  broadcast ssid, however it is not allowed in assocreq */
+		DBG_8723A("STA " MAC_FMT " sent invalid association request "
+			  "lacking an SSID\n", MAC_ARG(pstat->hwaddr));
 		status = WLAN_STATUS_UNSPECIFIED_FAILURE;
-	else {
+		goto OnAssocReq23aFail;
+	} else {
 		/*  check if ssid match */
-		if (memcmp((void *)(p+2), cur->Ssid.ssid, cur->Ssid.ssid_len))
+		if (memcmp(p + 2, cur->Ssid.ssid, cur->Ssid.ssid_len))
 			status = WLAN_STATUS_UNSPECIFIED_FAILURE;
 
-		if (ie_len != cur->Ssid.ssid_len)
+		if (p[1] != cur->Ssid.ssid_len)
 			status = WLAN_STATUS_UNSPECIFIED_FAILURE;
 	}
 
-	if (WLAN_STATUS_SUCCESS != status)
+	if (status != WLAN_STATUS_SUCCESS)
 		goto OnAssocReq23aFail;
 
 	/*  check if the supported rate is ok */
-	p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + ie_offset, _SUPPORTEDRATES_IE_, &ie_len, pkt_len - sizeof(struct ieee80211_hdr_3addr) - ie_offset);
-	if (p == NULL) {
-		DBG_8723A("Rx a sta assoc-req which supported rate is empty!\n");
+	p = cfg80211_find_ie(WLAN_EID_SUPP_RATES, pos, left);
+	if (!p) {
+		DBG_8723A("Rx a sta assoc-req which supported rate is "
+			  "empty!\n");
 		/*  use our own rate set as statoin used */
 		/* memcpy(supportRate, AP_BSSRATE, AP_BSSRATE_LEN); */
 		/* supportRateNum = AP_BSSRATE_LEN; */
@@ -1384,17 +1402,14 @@ unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *prec
 		status = WLAN_STATUS_UNSPECIFIED_FAILURE;
 		goto OnAssocReq23aFail;
 	} else {
-		memcpy(supportRate, p+2, ie_len);
-		supportRateNum = ie_len;
-
-		p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + ie_offset, _EXT_SUPPORTEDRATES_IE_, &ie_len,
-				pkt_len - sizeof(struct ieee80211_hdr_3addr) - ie_offset);
-		if (p !=  NULL) {
-
-			if (supportRateNum<= sizeof(supportRate))
-			{
-				memcpy(supportRate+supportRateNum, p+2, ie_len);
-				supportRateNum += ie_len;
+		memcpy(supportRate, p + 2, p[1]);
+		supportRateNum = p[1];
+
+		p = cfg80211_find_ie(WLAN_EID_EXT_SUPP_RATES, pos, left);
+		if (p) {
+			if (supportRateNum <= sizeof(supportRate)) {
+				memcpy(supportRate+supportRateNum, p + 2, p[1]);
+				supportRateNum += p[1];
 			}
 		}
 	}
@@ -1415,90 +1430,97 @@ unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *prec
 	pstat->wpa_pairwise_cipher = 0;
 	pstat->wpa2_pairwise_cipher = 0;
 	memset(pstat->wpa_ie, 0, sizeof(pstat->wpa_ie));
-	if ((psecuritypriv->wpa_psk & BIT(1)) && elems.rsn_ie) {
 
+	wpa_ie = cfg80211_find_ie(WLAN_EID_RSN, pos, left);
+	if (!wpa_ie)
+		wpa_ie = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
+						 WLAN_OUI_TYPE_MICROSOFT_WPA,
+						 pos, left);
+	if (wpa_ie) {
 		int group_cipher = 0, pairwise_cipher = 0;
 
-		wpa_ie = elems.rsn_ie;
-		wpa_ie_len = elems.rsn_ie_len;
-
-		if (rtw_parse_wpa2_ie23a(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
-			pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
-			pstat->wpa_psk |= BIT(1);
-
-			pstat->wpa2_group_cipher = group_cipher&psecuritypriv->wpa2_group_cipher;
-			pstat->wpa2_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa2_pairwise_cipher;
-
-			if (!pstat->wpa2_group_cipher)
-				status = WLAN_REASON_INVALID_GROUP_CIPHER;
-
-			if (!pstat->wpa2_pairwise_cipher)
-				status = WLAN_REASON_INVALID_PAIRWISE_CIPHER;
+		wpa_ie_len = wpa_ie[1];
+		if (psecuritypriv->wpa_psk & BIT(1)) {
+			r = rtw_parse_wpa2_ie23a(wpa_ie, wpa_ie_len + 2,
+						 &group_cipher,
+						 &pairwise_cipher, NULL);
+			if (r == _SUCCESS) {
+				pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
+				pstat->wpa_psk |= BIT(1);
+
+				pstat->wpa2_group_cipher = group_cipher &
+					psecuritypriv->wpa2_group_cipher;
+				pstat->wpa2_pairwise_cipher = pairwise_cipher &
+					psecuritypriv->wpa2_pairwise_cipher;
+			} else
+				status = WLAN_STATUS_INVALID_IE;
+		} else if (psecuritypriv->wpa_psk & BIT(0)) {
+			r = rtw_parse_wpa_ie23a(wpa_ie, wpa_ie_len + 2,
+						&group_cipher, &pairwise_cipher,
+						NULL);
+			if (r == _SUCCESS) {
+				pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
+				pstat->wpa_psk |= BIT(0);
+
+				pstat->wpa_group_cipher = group_cipher &
+					psecuritypriv->wpa_group_cipher;
+				pstat->wpa_pairwise_cipher = pairwise_cipher &
+					psecuritypriv->wpa_pairwise_cipher;
+			} else
+				status = WLAN_STATUS_INVALID_IE;
 		} else {
-			status = WLAN_STATUS_INVALID_IE;
+			wpa_ie = NULL;
+			wpa_ie_len = 0;
 		}
-
-	} else if ((psecuritypriv->wpa_psk & BIT(0)) && elems.wpa_ie) {
-
-		int group_cipher = 0, pairwise_cipher = 0;
-
-		wpa_ie = elems.wpa_ie;
-		wpa_ie_len = elems.wpa_ie_len;
-
-		if (rtw_parse_wpa_ie23a(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
-			pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
-			pstat->wpa_psk |= BIT(0);
-
-			pstat->wpa_group_cipher = group_cipher&psecuritypriv->wpa_group_cipher;
-			pstat->wpa_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa_pairwise_cipher;
-
+		if (wpa_ie && status == WLAN_STATUS_SUCCESS) {
 			if (!pstat->wpa_group_cipher)
 				status = WLAN_STATUS_INVALID_GROUP_CIPHER;
 
 			if (!pstat->wpa_pairwise_cipher)
 				status = WLAN_STATUS_INVALID_PAIRWISE_CIPHER;
-
-		} else {
-			status = WLAN_STATUS_INVALID_IE;
 		}
-
-	} else {
-		wpa_ie = NULL;
-		wpa_ie_len = 0;
 	}
 
-	if (WLAN_STATUS_SUCCESS != status)
+	if (status != WLAN_STATUS_SUCCESS)
 		goto OnAssocReq23aFail;
 
 	pstat->flags &= ~(WLAN_STA_WPS | WLAN_STA_MAYBE_WPS);
-	if (wpa_ie == NULL) {
-		if (elems.wps_ie) {
-			DBG_8723A("STA included WPS IE in "
-				   "(Re)Association Request - assume WPS is "
-				   "used\n");
+
+	wps_ie = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
+					 WLAN_OUI_TYPE_MICROSOFT_WPS,
+					 pos, left);
+
+	if (!wpa_ie) {
+		if (wps_ie) {
+			DBG_8723A("STA included WPS IE in (Re)Association "
+				  "Request - assume WPS is used\n");
 			pstat->flags |= WLAN_STA_WPS;
 		} else {
-			DBG_8723A("STA did not include WPA/RSN IE "
-				   "in (Re)Association Request - possible WPS "
-				   "use\n");
+			DBG_8723A("STA did not include WPA/RSN IE in (Re)"
+				   "Association Request - possible WPS use\n");
 			pstat->flags |= WLAN_STA_MAYBE_WPS;
 		}
 
-		/*  AP support WPA/RSN, and sta is going to do WPS, but AP is not ready */
+		/*  AP support WPA/RSN, and sta is going to do WPS, but AP
+		    is not ready */
 		/*  that the selected registrar of AP is _FLASE */
-		if ((psecuritypriv->wpa_psk > 0) &&
-		    (pstat->flags & (WLAN_STA_WPS|WLAN_STA_MAYBE_WPS))) {
+		if (psecuritypriv->wpa_psk > 0 &&
+		    pstat->flags & (WLAN_STA_WPS|WLAN_STA_MAYBE_WPS)) {
 			if (pmlmepriv->wps_beacon_ie) {
 				u8 selected_registrar = 0;
 
-				rtw_get_wps_attr_content23a(pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len,
-							 WPS_ATTR_SELECTED_REGISTRAR, &selected_registrar, NULL);
+				rtw_get_wps_attr_content23a(
+					pmlmepriv->wps_beacon_ie,
+					pmlmepriv->wps_beacon_ie_len,
+					WPS_ATTR_SELECTED_REGISTRAR,
+					&selected_registrar, NULL);
 
 				if (!selected_registrar) {
-					DBG_8723A("selected_registrar is false , or AP is not ready to do WPS\n");
+					DBG_8723A("selected_registrar is false,"
+						  "or AP is not ready to do "
+						  "WPS\n");
 
 					status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
-
 					goto OnAssocReq23aFail;
 				}
 			}
@@ -1508,26 +1530,26 @@ unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *prec
 
 		if (psecuritypriv->wpa_psk == 0) {
 			DBG_8723A("STA " MAC_FMT ": WPA/RSN IE in association "
-			"request, but AP don't support WPA/RSN\n", MAC_ARG(pstat->hwaddr));
+			"request, but AP don't support WPA/RSN\n",
+				  MAC_ARG(pstat->hwaddr));
 
 			status = WLAN_STATUS_INVALID_IE;
 
 			goto OnAssocReq23aFail;
 		}
 
-		if (elems.wps_ie) {
-			DBG_8723A("STA included WPS IE in "
-				   "(Re)Association Request - WPS is "
-				   "used\n");
+		if (wps_ie) {
+			DBG_8723A("STA included WPS IE in (Re)Association "
+				  "Request - WPS is used\n");
 			pstat->flags |= WLAN_STA_WPS;
 			copy_len = 0;
 		} else {
-			copy_len = ((wpa_ie_len+2) > sizeof(pstat->wpa_ie)) ? (sizeof(pstat->wpa_ie)):(wpa_ie_len+2);
+			copy_len = ((wpa_ie_len + 2) > sizeof(pstat->wpa_ie)) ?
+				sizeof(pstat->wpa_ie) : (wpa_ie_len + 2);
 		}
 
-		if (copy_len>0)
-			memcpy(pstat->wpa_ie, wpa_ie-2, copy_len);
-
+		if (copy_len > 0)
+			memcpy(pstat->wpa_ie, wpa_ie - 2, copy_len);
 	}
 
 	/*  check if there is WMM IE & support WWM-PS */
@@ -1539,45 +1561,45 @@ unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *prec
 	pstat->uapsd_vi = 0;
 	pstat->uapsd_be = 0;
 	pstat->uapsd_bk = 0;
-	if (pmlmepriv->qospriv.qos_option)
-	{
-		p = pframe + sizeof(struct ieee80211_hdr_3addr) + ie_offset; ie_len = 0;
-		for (;;)
-		{
-			p = rtw_get_ie23a(p, _VENDOR_SPECIFIC_IE_, &ie_len, pkt_len - sizeof(struct ieee80211_hdr_3addr) - ie_offset);
-			if (p != NULL) {
-				if (!memcmp(p+2, WMM_IE, 6)) {
-
+	if (pmlmepriv->qos_option) {
+		const u8 *end = pos + left;
+		p = pos;
+
+		for (;;) {
+			left = end - p;
+			p = cfg80211_find_ie(WLAN_EID_VENDOR_SPECIFIC, p, left);
+			if (p) {
+				if (!memcmp(p + 2, WMM_IE, 6)) {
 					pstat->flags |= WLAN_STA_WME;
 
 					pstat->qos_option = 1;
-					pstat->qos_info = *(p+8);
+					pstat->qos_info = *(p + 8);
 
-					pstat->max_sp_len = (pstat->qos_info>>5)&0x3;
+					pstat->max_sp_len =
+						(pstat->qos_info >> 5) & 0x3;
 
-					if ((pstat->qos_info&0xf) != 0xf)
+					if ((pstat->qos_info & 0xf) != 0xf)
 						pstat->has_legacy_ac = true;
 					else
 						pstat->has_legacy_ac = false;
 
-					if (pstat->qos_info&0xf)
-					{
-						if (pstat->qos_info&BIT(0))
+					if (pstat->qos_info & 0xf) {
+						if (pstat->qos_info & BIT(0))
 							pstat->uapsd_vo = BIT(0)|BIT(1);
 						else
 							pstat->uapsd_vo = 0;
 
-						if (pstat->qos_info&BIT(1))
+						if (pstat->qos_info & BIT(1))
 							pstat->uapsd_vi = BIT(0)|BIT(1);
 						else
 							pstat->uapsd_vi = 0;
 
-						if (pstat->qos_info&BIT(2))
+						if (pstat->qos_info & BIT(2))
 							pstat->uapsd_bk = BIT(0)|BIT(1);
 						else
 							pstat->uapsd_bk = 0;
 
-						if (pstat->qos_info&BIT(3))
+						if (pstat->qos_info & BIT(3))
 							pstat->uapsd_be = BIT(0)|BIT(1);
 						else
 							pstat->uapsd_be = 0;
@@ -1586,45 +1608,42 @@ unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *prec
 
 					break;
 				}
-			}
-			else {
+			} else {
 				break;
 			}
-			p = p + ie_len + 2;
+			p = p + p[1] + 2;
 		}
 	}
 
 	/* save HT capabilities in the sta object */
 	memset(&pstat->htpriv.ht_cap, 0, sizeof(struct ieee80211_ht_cap));
-	if (elems.ht_capabilities && elems.ht_capabilities_len >= sizeof(struct ieee80211_ht_cap))
-	{
+	p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, pos, left);
+
+	if (p && p[1] >= sizeof(struct ieee80211_ht_cap)) {
 		pstat->flags |= WLAN_STA_HT;
 
 		pstat->flags |= WLAN_STA_WME;
 
-		memcpy(&pstat->htpriv.ht_cap, elems.ht_capabilities, sizeof(struct ieee80211_ht_cap));
-
+		memcpy(&pstat->htpriv.ht_cap, p + 2,
+		       sizeof(struct ieee80211_ht_cap));
 	} else
 		pstat->flags &= ~WLAN_STA_HT;
 
-	if ((pmlmepriv->htpriv.ht_option == false) && (pstat->flags&WLAN_STA_HT))
-	{
+	if (pmlmepriv->htpriv.ht_option == false && pstat->flags & WLAN_STA_HT){
 		status = WLAN_STATUS_UNSPECIFIED_FAILURE;
 		goto OnAssocReq23aFail;
 	}
 
-	if ((pstat->flags & WLAN_STA_HT) &&
-		    ((pstat->wpa2_pairwise_cipher&WPA_CIPHER_TKIP) ||
-		      (pstat->wpa_pairwise_cipher&WPA_CIPHER_TKIP)))
-	{
-		DBG_8723A("HT: " MAC_FMT " tried to "
-				   "use TKIP with HT association\n", MAC_ARG(pstat->hwaddr));
+	if (pstat->flags & WLAN_STA_HT &&
+	    (pstat->wpa2_pairwise_cipher & WPA_CIPHER_TKIP ||
+	     pstat->wpa_pairwise_cipher & WPA_CIPHER_TKIP)) {
+		DBG_8723A("HT: " MAC_FMT " tried to use TKIP with HT "
+			  "association\n", MAC_ARG(pstat->hwaddr));
 
 		/* status = WLAN_STATUS_CIPHER_REJECTED_PER_POLICY; */
 		/* goto OnAssocReq23aFail; */
 	}
 
-       /*  */
 	pstat->flags |= WLAN_STA_NONERP;
 	for (i = 0; i < pstat->bssratelen; i++) {
 		if ((pstat->bssrateset[i] & 0x7f) > 22) {
@@ -1641,39 +1660,6 @@ unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *prec
 	if (status != WLAN_STATUS_SUCCESS)
 		goto OnAssocReq23aFail;
 
-#ifdef CONFIG_8723AU_P2P
-	pstat->is_p2p_device = false;
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-	{
-		if ((p2pie = rtw_get_p2p_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + ie_offset, pkt_len - sizeof(struct ieee80211_hdr_3addr) - ie_offset, NULL, &p2pielen)))
-		{
-			pstat->is_p2p_device = true;
-			if ((p2p_status_code = (u8)process_assoc_req_p2p_ie23a(pwdinfo, pframe, pkt_len, pstat))>0)
-			{
-				pstat->p2p_status_code = p2p_status_code;
-				status = WLAN_STATUS_CAPS_UNSUPPORTED;
-				goto OnAssocReq23aFail;
-			}
-		}
-#ifdef CONFIG_8723AU_P2P
-		if (rtw_get_wfd_ie(pframe + sizeof(struct ieee80211_hdr_3addr) + ie_offset, pkt_len - sizeof(struct ieee80211_hdr_3addr) - ie_offset, wfd_ie, &wfd_ielen))
-		{
-			u8	attr_content[ 10 ] = { 0x00 };
-			u32	attr_contentlen = 0;
-
-			DBG_8723A("[%s] WFD IE Found!!\n", __func__);
-			rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen);
-			if (attr_contentlen)
-			{
-				pwdinfo->wfd_info->peer_rtsp_ctrlport = get_unaligned_be16(attr_content + 2);
-				DBG_8723A("[%s] Peer PORT NUM = %d\n", __func__, pwdinfo->wfd_info->peer_rtsp_ctrlport);
-			}
-		}
-#endif
-	}
-	pstat->p2p_status_code = p2p_status_code;
-#endif /* CONFIG_8723AU_P2P */
-
 	/* TODO: identify_proprietary_vendor_ie(); */
 	/*  Realtek proprietary IE */
 	/*  identify if this is Broadcom sta */
@@ -1699,14 +1685,13 @@ unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *prec
 			status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
 
 			goto OnAssocReq23aFail;
-
 		} else {
 			pstapriv->sta_aid[pstat->aid - 1] = pstat;
 			DBG_8723A("allocate new AID = (%d)\n", pstat->aid);
 		}
 	}
 
-	pstat->state &= (~WIFI_FW_ASSOC_STATE);
+	pstat->state &= ~WIFI_FW_ASSOC_STATE;
 	pstat->state |= WIFI_FW_ASSOC_SUCCESS;
 
 	spin_lock_bh(&pstapriv->auth_list_lock);
@@ -1725,18 +1710,20 @@ unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *prec
 	spin_unlock_bh(&pstapriv->asoc_list_lock);
 
 	/*  now the station is qualified to join our BSS... */
-	if (pstat && (pstat->state & WIFI_FW_ASSOC_SUCCESS) &&
-	    (WLAN_STATUS_SUCCESS == status)) {
+	if (pstat && pstat->state & WIFI_FW_ASSOC_SUCCESS &&
+	    status == WLAN_STATUS_SUCCESS) {
 #ifdef CONFIG_8723AU_AP_MODE
 		/* 1 bss_cap_update & sta_info_update23a */
 		bss_cap_update_on_sta_join23a(padapter, pstat);
 		sta_info_update23a(padapter, pstat);
 
 		/* issue assoc rsp before notify station join event. */
-		if (ieee80211_is_assoc_req(frame_control))
-			issue_asocrsp23a(padapter, status, pstat, WIFI_ASSOCRSP);
+		if (ieee80211_is_assoc_req(mgmt->frame_control))
+			issue_assocrsp(padapter, status, pstat,
+				       IEEE80211_STYPE_ASSOC_RESP);
 		else
-			issue_asocrsp23a(padapter, status, pstat, WIFI_REASSOCRSP);
+			issue_assocrsp(padapter, status, pstat,
+				       IEEE80211_STYPE_REASSOC_RESP);
 
 		/* 2 - report to upper layer */
 		DBG_8723A("indicate_sta_join_event to upper layer - hostapd\n");
@@ -1752,19 +1739,20 @@ unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *prec
 asoc_class2_error:
 
 #ifdef CONFIG_8723AU_AP_MODE
-	issue_deauth23a(padapter, hdr->addr2, status);
+	issue_deauth23a(padapter, mgmt->sa, status);
 #endif
-
 	return _FAIL;
 
 OnAssocReq23aFail:
 
 #ifdef CONFIG_8723AU_AP_MODE
 	pstat->aid = 0;
-	if (ieee80211_is_assoc_req(frame_control))
-		issue_asocrsp23a(padapter, status, pstat, WIFI_ASSOCRSP);
+	if (ieee80211_is_assoc_req(mgmt->frame_control))
+		issue_assocrsp(padapter, status, pstat,
+			       IEEE80211_STYPE_ASSOC_RESP);
 	else
-		issue_asocrsp23a(padapter, status, pstat, WIFI_REASSOCRSP);
+		issue_assocrsp(padapter, status, pstat,
+			       IEEE80211_STYPE_REASSOC_RESP);
 #endif
 
 #endif /* CONFIG_8723AU_AP_MODE */
@@ -1772,25 +1760,24 @@ OnAssocReq23aFail:
 	return _FAIL;
 }
 
-unsigned int OnAssocRsp23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
+static int
+OnAssocRsp23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
-	uint i;
-	int res;
-	unsigned short	status;
-	struct ndis_802_11_var_ies *pIE;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
+	struct ieee80211_mgmt *pmgmt = (struct ieee80211_mgmt *) skb->data;
+	int res, i;
+	unsigned short status;
+	u8 *p;
 	u8 *pframe = skb->data;
-	uint pkt_len = skb->len;
+	int pkt_len = skb->len;
 
 	DBG_8723A("%s\n", __func__);
 
 	/* check A1 matches or not */
-	if (!ether_addr_equal(myid(&padapter->eeprompriv),
-			      ieee80211_get_DA(hdr)))
+	if (!ether_addr_equal(myid(&padapter->eeprompriv), pmgmt->da))
 		return _SUCCESS;
 
 	if (!(pmlmeinfo->state & (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE)))
@@ -1802,8 +1789,8 @@ unsigned int OnAssocRsp23a(struct rtw_adapter *padapter, struct recv_frame *prec
 	del_timer_sync(&pmlmeext->link_timer);
 
 	/* status */
-	if ((status = le16_to_cpu(*(unsigned short *)(pframe + sizeof(struct ieee80211_hdr_3addr) + 2))) > 0)
-	{
+	status = le16_to_cpu(pmgmt->u.assoc_resp.status_code);
+	if (status > 0)	{
 		DBG_8723A("assoc reject, status code: %d\n", status);
 		pmlmeinfo->state = WIFI_FW_NULL_STATE;
 		res = -4;
@@ -1811,52 +1798,47 @@ unsigned int OnAssocRsp23a(struct rtw_adapter *padapter, struct recv_frame *prec
 	}
 
 	/* get capabilities */
-	pmlmeinfo->capability = le16_to_cpu(*(unsigned short *)(pframe + sizeof(struct ieee80211_hdr_3addr)));
+	pmlmeinfo->capability = le16_to_cpu(pmgmt->u.assoc_resp.capab_info);
 
 	/* set slot time */
 	pmlmeinfo->slotTime = (pmlmeinfo->capability & BIT(10))? 9: 20;
 
 	/* AID */
-	res = pmlmeinfo->aid = (int)(le16_to_cpu(*(unsigned short *)(pframe + sizeof(struct ieee80211_hdr_3addr) + 4))&0x3fff);
+	res = pmlmeinfo->aid = le16_to_cpu(pmgmt->u.assoc_resp.aid) & 0x3fff;
 
 	/* following are moved to join event callback function */
 	/* to handle HT, WMM, rate adaptive, update MAC reg */
 	/* for not to handle the synchronous IO in the tasklet */
-	for (i = (6 + sizeof(struct ieee80211_hdr_3addr)); i < pkt_len;) {
-		pIE = (struct ndis_802_11_var_ies *)(pframe + i);
+	for (i = offsetof(struct ieee80211_mgmt, u.assoc_resp.variable);
+	     i < pkt_len;) {
+		p = pframe + i;
 
-		switch (pIE->ElementID)
+		switch (p[0])
 		{
-		case _VENDOR_SPECIFIC_IE_:
-			if (!memcmp(pIE->data, WMM_PARA_OUI23A, 6))/* WMM */
-					WMM_param_handler23a(padapter, pIE);
-#if defined(CONFIG_8723AU_P2P)
-			else if (!memcmp(pIE->data, WFD_OUI23A, 4)) { /* WFD */
-				DBG_8723A("[%s] Found WFD IE\n", __func__);
-				WFD_info_handler(padapter, pIE);
-			}
-#endif
+		case WLAN_EID_VENDOR_SPECIFIC:
+			if (!memcmp(p + 2, WMM_PARA_OUI23A, 6))/* WMM */
+				WMM_param_handler23a(padapter, p);
 			break;
 
-		case _HT_CAPABILITY_IE_:	/* HT caps */
-			HT_caps_handler23a(padapter, pIE);
+		case WLAN_EID_HT_CAPABILITY:	/* HT caps */
+			HT_caps_handler23a(padapter, p);
 			break;
 
-		case _HT_EXTRA_INFO_IE_:	/* HT info */
-			HT_info_handler23a(padapter, pIE);
+		case WLAN_EID_HT_OPERATION:	/* HT info */
+			HT_info_handler23a(padapter, p);
 			break;
 
-		case _ERPINFO_IE_:
-			ERP_IE_handler23a(padapter, pIE);
+		case WLAN_EID_ERP_INFO:
+			ERP_IE_handler23a(padapter, p);
 
 		default:
 			break;
 		}
 
-		i += (pIE->Length + 2);
+		i += (p[1] + 2);
 	}
 
-	pmlmeinfo->state &= (~WIFI_FW_ASSOC_STATE);
+	pmlmeinfo->state &= ~WIFI_FW_ASSOC_STATE;
 	pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
 
 	/* Update Basic Rate Table for spec, 2010-12-28 , by thomas */
@@ -1879,44 +1861,33 @@ report_assoc_result:
 	return _SUCCESS;
 }
 
-unsigned int OnDeAuth23a(struct rtw_adapter *padapter,
-			 struct recv_frame *precv_frame)
+static int
+OnDeAuth23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
-	unsigned short	reason;
+	unsigned short reason;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u8 *pframe = skb->data;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif /* CONFIG_8723AU_P2P */
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
 
-	/* check A3 */
-	if (!ether_addr_equal(hdr->addr3, get_my_bssid23a(&pmlmeinfo->network)))
+	if (!ether_addr_equal(mgmt->bssid,
+			      get_my_bssid23a(&pmlmeinfo->network)))
 		return _SUCCESS;
 
-#ifdef CONFIG_8723AU_P2P
-	if (pwdinfo->rx_invitereq_info.scan_op_ch_only) {
-		mod_timer(&pwdinfo->reset_ch_sitesurvey,
-			  jiffies + msecs_to_jiffies(10));
-	}
-#endif /* CONFIG_8723AU_P2P */
-
-	reason = le16_to_cpu(*(unsigned short *)(pframe + sizeof(struct ieee80211_hdr_3addr)));
+	reason = le16_to_cpu(mgmt->u.deauth.reason_code);
 
 	DBG_8723A("%s Reason code(%d)\n", __func__, reason);
 
 #ifdef CONFIG_8723AU_AP_MODE
-	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
 		struct sta_info *psta;
 		struct sta_priv *pstapriv = &padapter->stapriv;
 
 		DBG_8723A_LEVEL(_drv_always_, "ap recv deauth reason code(%d) "
-				"sta:%pM\n", reason, hdr->addr2);
+				"sta:%pM\n", reason, mgmt->sa);
 
-		psta = rtw_get_stainfo23a(pstapriv, hdr->addr2);
+		psta = rtw_get_stainfo23a(pstapriv, mgmt->sa);
 		if (psta) {
 			u8 updated = 0;
 
@@ -1933,46 +1904,34 @@ unsigned int OnDeAuth23a(struct rtw_adapter *padapter,
 		}
 
 		return _SUCCESS;
-	}
-	else
+	} else
 #endif
 	{
 		DBG_8723A_LEVEL(_drv_always_, "sta recv deauth reason code(%d) "
-				"sta:%pM\n", reason, hdr->addr3);
+				"sta:%pM\n", reason, mgmt->bssid);
 
-		receive_disconnect23a(padapter, hdr->addr3, reason);
+		receive_disconnect23a(padapter, mgmt->bssid, reason);
 	}
 	pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
+
 	return _SUCCESS;
 }
 
-unsigned int OnDisassoc23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
+static int
+OnDisassoc23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
 	unsigned short	reason;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u8 *pframe = skb->data;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif /* CONFIG_8723AU_P2P */
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
 
-	/* check A3 */
-	if (!ether_addr_equal(hdr->addr3, get_my_bssid23a(&pmlmeinfo->network)))
+	if (!ether_addr_equal(mgmt->bssid,
+			      get_my_bssid23a(&pmlmeinfo->network)))
 		return _SUCCESS;
 
-#ifdef CONFIG_8723AU_P2P
-	if (pwdinfo->rx_invitereq_info.scan_op_ch_only)
-	{
-		mod_timer(&pwdinfo->reset_ch_sitesurvey,
-			  jiffies + msecs_to_jiffies(10));
-	}
-#endif /* CONFIG_8723AU_P2P */
-
-	reason = le16_to_cpu(*(unsigned short *)
-			     (pframe + sizeof(struct ieee80211_hdr_3addr)));
+	reason = le16_to_cpu(mgmt->u.disassoc.reason_code);
 
         DBG_8723A("%s Reason code(%d)\n", __func__, reason);
 
@@ -1982,9 +1941,9 @@ unsigned int OnDisassoc23a(struct rtw_adapter *padapter, struct recv_frame *prec
 		struct sta_priv *pstapriv = &padapter->stapriv;
 
 		DBG_8723A_LEVEL(_drv_always_, "ap recv disassoc reason code(%d)"
-				" sta:%pM\n", reason, hdr->addr2);
+				" sta:%pM\n", reason, mgmt->sa);
 
-		psta = rtw_get_stainfo23a(pstapriv, hdr->addr2);
+		psta = rtw_get_stainfo23a(pstapriv, mgmt->sa);
 		if (psta) {
 			u8 updated = 0;
 
@@ -1993,7 +1952,7 @@ unsigned int OnDisassoc23a(struct rtw_adapter *padapter, struct recv_frame *prec
 				list_del_init(&psta->asoc_list);
 				pstapriv->asoc_list_cnt--;
 				updated = ap_free_sta23a(padapter, psta,
-						      false, reason);
+							 false, reason);
 			}
 			spin_unlock_bh(&pstapriv->asoc_list_lock);
 
@@ -2001,57 +1960,59 @@ unsigned int OnDisassoc23a(struct rtw_adapter *padapter, struct recv_frame *prec
 		}
 
 		return _SUCCESS;
-	}
-	else
+	} else
 #endif
 	{
 		DBG_8723A_LEVEL(_drv_always_, "ap recv disassoc reason "
-				"code(%d) sta:%pM\n", reason, hdr->addr3);
+				"code(%d) sta:%pM\n", reason, mgmt->bssid);
 
-		receive_disconnect23a(padapter, hdr->addr3, reason);
+		receive_disconnect23a(padapter, mgmt->bssid, reason);
 	}
 	pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
 	return _SUCCESS;
 }
 
-unsigned int OnAtim23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
+static int
+OnAtim23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
 	DBG_8723A("%s\n", __func__);
 	return _SUCCESS;
 }
 
-unsigned int on_action_spct23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
+static int
+on_action_spct23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
 	return _FAIL;
 }
 
-unsigned int OnAction23a_qos(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
+static int
+OnAction23a_qos(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
 	return _SUCCESS;
 }
 
-unsigned int OnAction23a_dls(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
+static int
+OnAction23a_dls(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
 	return _SUCCESS;
 }
 
-unsigned int OnAction23a_back23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
+static int OnAction23a_back23a(struct rtw_adapter *padapter,
+			       struct recv_frame *precv_frame)
 {
 	u8 *addr;
 	struct sta_info *psta = NULL;
 	struct recv_reorder_ctrl *preorder_ctrl;
-	unsigned char		*frame_body;
-	unsigned char		category, action;
-	unsigned short	tid, status, reason_code = 0;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	unsigned char category, action;
+	unsigned short tid, status, capab, params, reason_code = 0;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u8 *pframe = skb->data;
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 
 	/* check RA matches or not */
-	if (!ether_addr_equal(myid(&padapter->eeprompriv), hdr->addr1))
+	if (!ether_addr_equal(myid(&padapter->eeprompriv), mgmt->da))
 		return _SUCCESS;
 
 	DBG_8723A("%s\n", __func__);
@@ -2060,24 +2021,24 @@ unsigned int OnAction23a_back23a(struct rtw_adapter *padapter, struct recv_frame
 		if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
 			return _SUCCESS;
 
-	addr = hdr->addr2;
+	addr = mgmt->sa;
 	psta = rtw_get_stainfo23a(pstapriv, addr);
 
 	if (!psta)
 		return _SUCCESS;
 
-	frame_body = (unsigned char *)
-		(pframe + sizeof(struct ieee80211_hdr_3addr));
-
-	category = frame_body[0];
+	category = mgmt->u.action.category;
 	if (category == WLAN_CATEGORY_BACK) { /*  representing Block Ack */
 		if (!pmlmeinfo->HT_enable)
 			return _SUCCESS;
-		action = frame_body[1];
+		/* action_code is located in the same place for all
+		   action events, so pick any */
+		action = mgmt->u.action.u.wme_action.action_code;
 		DBG_8723A("%s, action =%d\n", __func__, action);
 		switch (action) {
 		case WLAN_ACTION_ADDBA_REQ: /* ADDBA request */
-			memcpy(&pmlmeinfo->ADDBA_req, &frame_body[2],
+			memcpy(&pmlmeinfo->ADDBA_req,
+			       &mgmt->u.action.u.addba_req.dialog_token,
 			       sizeof(struct ADDBA_request));
 			process_addba_req23a(padapter,
 					     (u8 *)&pmlmeinfo->ADDBA_req, addr);
@@ -2091,36 +2052,34 @@ unsigned int OnAction23a_back23a(struct rtw_adapter *padapter, struct recv_frame
 			}
 			break;
 		case WLAN_ACTION_ADDBA_RESP: /* ADDBA response */
-			status = get_unaligned_le16(&frame_body[3]);
-			tid = ((frame_body[5] >> 2) & 0x7);
+			status = get_unaligned_le16(
+				&mgmt->u.action.u.addba_resp.status);
+			capab = get_unaligned_le16(
+				&mgmt->u.action.u.addba_resp.capab);
+			tid = (capab & IEEE80211_ADDBA_PARAM_TID_MASK) >> 2;
 			if (status == 0) {	/* successful */
 				DBG_8723A("agg_enable for TID =%d\n", tid);
-				psta->htpriv.agg_enable_bitmap |= 1 << tid;
-				psta->htpriv.candidate_tid_bitmap &=
-					~CHKBIT(tid);
+				psta->htpriv.agg_enable_bitmap |= BIT(tid);
+				psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
 			} else
-				psta->htpriv.agg_enable_bitmap &= ~CHKBIT(tid);
+				psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
 			break;
 
 		case WLAN_ACTION_DELBA: /* DELBA */
-			if ((frame_body[3] & BIT(3)) == 0) {
-				psta->htpriv.agg_enable_bitmap &=
-					~(1 << ((frame_body[3] >> 4) & 0xf));
-				psta->htpriv.candidate_tid_bitmap &=
-					~(1 << ((frame_body[3] >> 4) & 0xf));
-
-				/* reason_code = frame_body[4] | (frame_body[5] << 8); */
-				reason_code = get_unaligned_le16(&frame_body[4]);
-			} else if ((frame_body[3] & BIT(3)) == BIT(3)) {
-				tid = (frame_body[3] >> 4) & 0x0F;
-
-				preorder_ctrl =  &psta->recvreorder_ctrl[tid];
+			params = get_unaligned_le16(
+				&mgmt->u.action.u.delba.params);
+			tid = params >> 12;
+
+			if (params & IEEE80211_DELBA_PARAM_INITIATOR_MASK) {
+				preorder_ctrl = &psta->recvreorder_ctrl[tid];
 				preorder_ctrl->enable = false;
 				preorder_ctrl->indicate_seq = 0xffff;
+			} else {
+				psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
+				psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
 			}
-
-			DBG_8723A("%s(): DELBA: %x(%x)\n", __func__,
-				  pmlmeinfo->agg_enable_bitmap, reason_code);
+			reason_code = get_unaligned_le16(
+				&mgmt->u.action.u.delba.reason_code);
 			/* todo: how to notify the host while receiving
 			   DELETE BA */
 			break;
@@ -2131,2365 +2090,7 @@ unsigned int OnAction23a_back23a(struct rtw_adapter *padapter, struct recv_frame
 	return _SUCCESS;
 }
 
-#ifdef CONFIG_8723AU_P2P
-
-static int get_reg_classes_full_count(struct p2p_channels channel_list) {
-	int cnt = 0;
-	int i;
-
-	for (i = 0; i < channel_list.reg_classes; i++)
-		cnt += channel_list.reg_class[i].channels;
-
-	return cnt;
-}
-
-void issue_p2p_GO_request23a(struct rtw_adapter *padapter, u8* raddr)
-{
-	unsigned char category = WLAN_CATEGORY_PUBLIC;
-	u8			action = P2P_PUB_ACTION_ACTION;
-	u32			p2poui = cpu_to_be32(P2POUI);
-	u8			oui_subtype = P2P_GO_NEGO_REQ;
-	u8			wpsie[ 255 ] = { 0x00 }, p2pie[ 255 ] = { 0x00 };
-	u8			wpsielen = 0, p2pielen = 0;
-	u16			len_channellist_attr = 0;
-#ifdef CONFIG_8723AU_P2P
-	u32					wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
-
-	struct xmit_frame			*pmgntframe;
-	struct pkt_attrib			*pattrib;
-	unsigned char					*pframe;
-	struct ieee80211_hdr	*pwlanhdr;
-	unsigned short				*fctrl;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-		return;
-
-	DBG_8723A("[%s] In\n", __func__);
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	ether_addr_copy(pwlanhdr->addr1, raddr);
-	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
-	ether_addr_copy(pwlanhdr->addr3, myid(&padapter->eeprompriv));
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
-
-	pframe += sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
-
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *)&p2poui,
-				     &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
-	pwdinfo->negotiation_dialog_token = 1;	/*Initialize the dialog value*/
-	pframe = rtw_set_fixed_ie23a(pframe, 1,
-				     &pwdinfo->negotiation_dialog_token,
-				     &pattrib->pktlen);
-
-	/*	WPS Section */
-	wpsielen = 0;
-	/*	WPS OUI */
-	*(u32*) (wpsie) = cpu_to_be32(WPSOUI);
-	wpsielen += 4;
-
-	/*	WPS version */
-	/*	Type: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
-	wpsielen += 2;
-
-	/*	Length: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0001);
-	wpsielen += 2;
-
-	/*	Value: */
-	wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
-
-	/*	Device Password ID */
-	/*	Type: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID);
-	wpsielen += 2;
-
-	/*	Length: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0002);
-	wpsielen += 2;
-
-	/*	Value: */
-
-	if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN)
-	{
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_DPID_USER_SPEC);
-	}
-	else if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN)
-	{
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_DPID_REGISTRAR_SPEC);
-	}
-	else if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC)
-	{
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_DPID_PBC);
-	}
-
-	wpsielen += 2;
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen);
-
-	/*	P2P IE Section. */
-
-	/*	P2P OUI */
-	p2pielen = 0;
-	p2pie[p2pielen++] = 0x50;
-	p2pie[p2pielen++] = 0x6F;
-	p2pie[p2pielen++] = 0x9A;
-	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
-
-	/*	Commented by Albert 20110306 */
-	/*	According to the P2P Specification, the group negoitation request frame should contain 9 P2P attributes */
-	/*	1. P2P Capability */
-	/*	2. Group Owner Intent */
-	/*	3. Configuration Timeout */
-	/*	4. Listen Channel */
-	/*	5. Extended Listen Timing */
-	/*	6. Intended P2P Interface Address */
-	/*	7. Channel List */
-	/*	8. P2P Device Info */
-	/*	9. Operating Channel */
-
-	/*	P2P Capability */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Device Capability Bitmap, 1 byte */
-	p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
-
-	/*	Group Capability Bitmap, 1 byte */
-	if (pwdinfo->persistent_supported)
-	{
-		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
-	}
-	else
-	{
-		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN;
-	}
-
-	/*	Group Owner Intent */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_GO_INTENT;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0001);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Todo the tie breaker bit. */
-	p2pie[p2pielen++] = ((pwdinfo->intent << 1) | BIT(0));
-
-	/*	Configuration Timeout */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002);
-	p2pielen += 2;
-
-	/*	Value: */
-	p2pie[p2pielen++] = 200;	/*	2 seconds needed to be the P2P GO */
-	p2pie[p2pielen++] = 200;	/*	2 seconds needed to be the P2P Client */
-
-	/*	Listen Channel */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_LISTEN_CH;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0005);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Country String */
-	p2pie[p2pielen++] = 'X';
-	p2pie[p2pielen++] = 'X';
-
-	/*	The third byte should be set to 0x04. */
-	/*	Described in the "Operating Channel Attribute" section. */
-	p2pie[p2pielen++] = 0x04;
-
-	/*	Operating Class */
-	p2pie[p2pielen++] = 0x51;	/*	Copy from SD7 */
-
-	/*	Channel Number */
-	p2pie[p2pielen++] = pwdinfo->listen_channel;	/*	listening channel number */
-
-	/*	Extended Listen Timing ATTR */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0004);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Availability Period */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF);
-	p2pielen += 2;
-
-	/*	Availability Interval */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF);
-	p2pielen += 2;
-
-	/*	Intended P2P Interface Address */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_INTENTED_IF_ADDR;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
-	p2pielen += 2;
-
-	/*	Value: */
-	memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
-	p2pielen += ETH_ALEN;
-
-	/*	Channel List */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
-
-	/*  Length: */
-	/*  Country String(3) */
-	/*  + (Operating Class (1) + Number of Channels(1)) * Operation Classes (?) */
-	/*  + number of channels in all classes */
-	len_channellist_attr = 3
-	   + (1 + 1) * (u16)(pmlmeext->channel_list.reg_classes)
-	   + get_reg_classes_full_count(pmlmeext->channel_list);
-
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Country String */
-	p2pie[p2pielen++] = 'X';
-	p2pie[p2pielen++] = 'X';
-
-	/*	The third byte should be set to 0x04. */
-	/*	Described in the "Operating Channel Attribute" section. */
-	p2pie[p2pielen++] = 0x04;
-
-	/*	Channel Entry List */
-
-	{
-		int i, j;
-		for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
-			/*	Operating Class */
-			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
-
-			/*	Number of Channels */
-			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
-
-			/*	Channel List */
-			for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
-				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
-			}
-		}
-	}
-
-	/*	Device Info */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
-	/*	+ NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	P2P Device Address */
-	memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
-	p2pielen += ETH_ALEN;
-
-	/*	Config Method */
-	/*	This field should be big endian. Noted by P2P specification. */
-
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm);
-
-	p2pielen += 2;
-
-	/*	Primary Device Type */
-	/*	Category ID */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
-	p2pielen += 2;
-
-	/*	OUI */
-	*(u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI);
-	p2pielen += 4;
-
-	/*	Sub Category ID */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
-	p2pielen += 2;
-
-	/*	Number of Secondary Device Types */
-	p2pie[p2pielen++] = 0x00;	/*	No Secondary Device Type List */
-
-	/*	Device Name */
-	/*	Type: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
-	p2pielen += 2;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
-	p2pielen += 2;
-
-	/*	Value: */
-	memcpy(p2pie + p2pielen, pwdinfo->device_name,
-	       pwdinfo->device_name_len);
-	p2pielen += pwdinfo->device_name_len;
-
-	/*	Operating Channel */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0005);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Country String */
-	p2pie[p2pielen++] = 'X';
-	p2pie[p2pielen++] = 'X';
-
-	/*	The third byte should be set to 0x04. */
-	/*	Described in the "Operating Channel Attribute" section. */
-	p2pie[p2pielen++] = 0x04;
-
-	/*	Operating Class */
-	if (pwdinfo->operating_channel <= 14)
-	{
-		/*	Operating Class */
-		p2pie[p2pielen++] = 0x51;
-	}
-	else if ((pwdinfo->operating_channel >= 36) && (pwdinfo->operating_channel <= 48))
-	{
-		/*	Operating Class */
-		p2pie[p2pielen++] = 0x73;
-	}
-	else
-	{
-		/*	Operating Class */
-		p2pie[p2pielen++] = 0x7c;
-	}
-
-	/*	Channel Number */
-	p2pie[p2pielen++] = pwdinfo->operating_channel;	/*	operating channel number */
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen);
-
-#ifdef CONFIG_8723AU_P2P
-	wfdielen = build_nego_req_wfd_ie(pwdinfo, pframe);
-	pframe += wfdielen;
-	pattrib->pktlen += wfdielen;
-#endif /* CONFIG_8723AU_P2P */
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	dump_mgntframe23a(padapter, pmgntframe);
-
-	return;
-}
-
-static void issue_p2p_GO_response(struct rtw_adapter *padapter, u8* raddr, u8* frame_body, uint len, u8 result)
-{
-
-	unsigned char category = WLAN_CATEGORY_PUBLIC;
-	u8 action = P2P_PUB_ACTION_ACTION;
-	u32 p2poui = cpu_to_be32(P2POUI);
-	u8 oui_subtype = P2P_GO_NEGO_RESP;
-	u8 wpsie[255] = { 0x00 }, p2pie[255] = { 0x00 };
-	u8 p2pielen = 0;
-	uint wpsielen = 0;
-	u16 wps_devicepassword_id = 0x0000;
-	uint wps_devicepassword_id_len = 0;
-	u16 len_channellist_attr = 0;
-	int i, j;
-	struct xmit_frame *pmgntframe;
-	struct pkt_attrib *pattrib;
-	unsigned char *pframe;
-	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#ifdef CONFIG_8723AU_P2P
-	u32 wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-		return;
-
-	DBG_8723A("[%s] In, result = %d\n", __func__,  result);
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	ether_addr_copy(pwlanhdr->addr1, raddr);
-	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
-	ether_addr_copy(pwlanhdr->addr3, myid(&padapter->eeprompriv));
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
-
-	pframe += sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
-
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *) &p2poui,
-				     &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
-	/*	The Dialog Token of provisioning discovery request frame. */
-	pwdinfo->negotiation_dialog_token = frame_body[7];
-	pframe = rtw_set_fixed_ie23a(pframe, 1,
-				     &pwdinfo->negotiation_dialog_token,
-				     &pattrib->pktlen);
-
-	/*	Commented by Albert 20110328 */
-	/*	Try to get the device password ID from the WPS IE of group
-		negotiation request frame */
-	/*	WiFi Direct test plan 5.1.15 */
-	rtw_get_wps_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_,
-			  len - _PUBLIC_ACTION_IE_OFFSET_, wpsie, &wpsielen);
-	rtw_get_wps_attr_content23a(wpsie, wpsielen, WPS_ATTR_DEVICE_PWID,
-				    (u8 *)&wps_devicepassword_id,
-				    &wps_devicepassword_id_len);
-	wps_devicepassword_id = be16_to_cpu(wps_devicepassword_id);
-
-	memset(wpsie, 0x00, 255);
-	wpsielen = 0;
-
-	/*	WPS Section */
-	wpsielen = 0;
-	/*	WPS OUI */
-	*(u32*) (wpsie) = cpu_to_be32(WPSOUI);
-	wpsielen += 4;
-
-	/*	WPS version */
-	/*	Type: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
-	wpsielen += 2;
-
-	/*	Length: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0001);
-	wpsielen += 2;
-
-	/*	Value: */
-	wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
-
-	/*	Device Password ID */
-	/*	Type: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID);
-	wpsielen += 2;
-
-	/*	Length: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0002);
-	wpsielen += 2;
-
-	/*	Value: */
-	if (wps_devicepassword_id == WPS_DPID_USER_SPEC) {
-		*(u16*) (wpsie + wpsielen) =
-			cpu_to_be16(WPS_DPID_REGISTRAR_SPEC);
-	} else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC) {
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_DPID_USER_SPEC);
-	} else {
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_DPID_PBC);
-	}
-	wpsielen += 2;
-
-	/*	Commented by Kurt 20120113 */
-	/*	If some device wants to do p2p handshake without sending prov_disc_req */
-	/*	We have to get peer_req_cm from here. */
-	if (!memcmp(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "000", 3)) {
-		if (wps_devicepassword_id == WPS_DPID_USER_SPEC) {
-			memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3);
-		} else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC) {
-			memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3);
-		} else {
-			memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3);
-		}
-	}
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, wpsielen,
-			       (unsigned char *) wpsie, &pattrib->pktlen);
-
-	/*	P2P IE Section. */
-
-	/*	P2P OUI */
-	p2pielen = 0;
-	p2pie[p2pielen++] = 0x50;
-	p2pie[p2pielen++] = 0x6F;
-	p2pie[p2pielen++] = 0x9A;
-	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
-
-	/*	Commented by Albert 20100908 */
-	/*	According to the P2P Specification, the group negoitation
-		response frame should contain 9 P2P attributes */
-	/*	1. Status */
-	/*	2. P2P Capability */
-	/*	3. Group Owner Intent */
-	/*	4. Configuration Timeout */
-	/*	5. Operating Channel */
-	/*	6. Intended P2P Interface Address */
-	/*	7. Channel List */
-	/*	8. Device Info */
-	/*	9. Group ID	(Only GO) */
-
-	/*	ToDo: */
-
-	/*	P2P Status */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_STATUS;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0001);
-	p2pielen += 2;
-
-	/*	Value: */
-	p2pie[p2pielen++] = result;
-
-	/*	P2P Capability */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Device Capability Bitmap, 1 byte */
-
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
-		/*	Commented by Albert 2011/03/08 */
-		/*	According to the P2P specification */
-		/*	if the sending device will be client, the P2P
-			Capability should be reserved of group negotation
-			response frame */
-		p2pie[p2pielen++] = 0;
-	} else {
-		/*	Be group owner or meet the error case */
-		p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
-	}
-
-	/*	Group Capability Bitmap, 1 byte */
-	if (pwdinfo->persistent_supported) {
-		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN |
-			P2P_GRPCAP_PERSISTENT_GROUP;
-	} else {
-		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN;
-	}
-
-	/*	Group Owner Intent */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_GO_INTENT;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0001);
-	p2pielen += 2;
-
-	/*	Value: */
-	if (pwdinfo->peer_intent & 0x01) {
-		/*	Peer's tie breaker bit is 1, our tie breaker
-			bit should be 0 */
-		p2pie[p2pielen++] = (pwdinfo->intent << 1);
-	} else {
-		/* Peer's tie breaker bit is 0, our tie breaker bit
-		   should be 1 */
-		p2pie[p2pielen++] = ((pwdinfo->intent << 1) | BIT(0));
-	}
-
-	/*	Configuration Timeout */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	2 seconds needed to be the P2P GO */
-	p2pie[p2pielen++] = 200;
-	/*	2 seconds needed to be the P2P Client */
-	p2pie[p2pielen++] = 200;
-
-	/*	Operating Channel */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0005);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Country String */
-	p2pie[p2pielen++] = 'X';
-	p2pie[p2pielen++] = 'X';
-
-	/*	The third byte should be set to 0x04. */
-	/*	Described in the "Operating Channel Attribute" section. */
-	p2pie[p2pielen++] = 0x04;
-
-	/*	Operating Class */
-	if (pwdinfo->operating_channel <= 14) {
-		/*	Operating Class */
-		p2pie[p2pielen++] = 0x51;
-	} else if ((pwdinfo->operating_channel >= 36) &&
-		   (pwdinfo->operating_channel <= 48)) {
-		/*	Operating Class */
-		p2pie[p2pielen++] = 0x73;
-	} else {
-		/*	Operating Class */
-		p2pie[p2pielen++] = 0x7c;
-	}
-
-	/*	Channel Number */
-	/*	operating channel number */
-	p2pie[p2pielen++] = pwdinfo->operating_channel;
-
-	/*	Intended P2P Interface Address */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_INTENTED_IF_ADDR;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
-	p2pielen += 2;
-
-	/*	Value: */
-	memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
-	p2pielen += ETH_ALEN;
-
-	/*	Channel List */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
-
-	/*  Country String(3) */
-	/*  + (Operating Class (1) + Number of Channels(1)) *
-	    Operation Classes (?) */
-	/*  + number of channels in all classes */
-	len_channellist_attr = 3 +
-		(1 + 1) * (u16)pmlmeext->channel_list.reg_classes +
-		get_reg_classes_full_count(pmlmeext->channel_list);
-
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
-
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Country String */
-	p2pie[p2pielen++] = 'X';
-	p2pie[p2pielen++] = 'X';
-
-	/*	The third byte should be set to 0x04. */
-	/*	Described in the "Operating Channel Attribute" section. */
-	p2pie[p2pielen++] = 0x04;
-
-	/*	Channel Entry List */
-
-	for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
-		/*	Operating Class */
-		p2pie[p2pielen++] =
-			pmlmeext->channel_list.reg_class[j].reg_class;
-
-		/*	Number of Channels */
-		p2pie[p2pielen++] =
-			pmlmeext->channel_list.reg_class[j].channels;
-
-		/*	Channel List */
-		for (i = 0;
-		     i < pmlmeext->channel_list.reg_class[j].channels; i++) {
-			p2pie[p2pielen++] =
-				pmlmeext->channel_list.reg_class[j].channel[i];
-		}
-	}
-
-	/*	Device Info */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	21 -> P2P Device Address (6bytes) + Config Methods (2bytes) +
-		Primary Device Type (8bytes) */
-	/*	+ NumofSecondDevType (1byte) + WPS Device Name ID field
-		(2bytes) + WPS Device Name Len field (2bytes) */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	P2P Device Address */
-	memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
-	p2pielen += ETH_ALEN;
-
-	/*	Config Method */
-	/*	This field should be big endian. Noted by P2P specification. */
-
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm);
-
-	p2pielen += 2;
-
-	/*	Primary Device Type */
-	/*	Category ID */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
-	p2pielen += 2;
-
-	/*	OUI */
-	*(u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI);
-	p2pielen += 4;
-
-	/*	Sub Category ID */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
-	p2pielen += 2;
-
-	/*	Number of Secondary Device Types */
-	p2pie[p2pielen++] = 0x00;	/*	No Secondary Device Type List */
-
-	/*	Device Name */
-	/*	Type: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
-	p2pielen += 2;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
-	p2pielen += 2;
-
-	/*	Value: */
-	memcpy(p2pie + p2pielen, pwdinfo->device_name,
-	       pwdinfo->device_name_len);
-	p2pielen += pwdinfo->device_name_len;
-
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-	{
-		/*	Group ID Attribute */
-		/*	Type: */
-		p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;
-
-		/*	Length: */
-		*(u16*) (p2pie + p2pielen) =
-			cpu_to_le16(ETH_ALEN + pwdinfo->nego_ssidlen);
-		p2pielen += 2;
-
-		/*	Value: */
-		/*	p2P Device Address */
-		memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);
-		p2pielen += ETH_ALEN;
-
-		/*	SSID */
-		memcpy(p2pie + p2pielen, pwdinfo->nego_ssid,
-		       pwdinfo->nego_ssidlen);
-		p2pielen += pwdinfo->nego_ssidlen;
-
-	}
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen,
-			       (unsigned char *) p2pie, &pattrib->pktlen);
-
-#ifdef CONFIG_8723AU_P2P
-	wfdielen = build_nego_resp_wfd_ie(pwdinfo, pframe);
-	pframe += wfdielen;
-	pattrib->pktlen += wfdielen;
-#endif /* CONFIG_8723AU_P2P */
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	dump_mgntframe23a(padapter, pmgntframe);
-
-	return;
-}
-
-static void issue_p2p_GO_confirm(struct rtw_adapter *padapter, u8* raddr,
-				 u8 result)
-{
-
-	unsigned char category = WLAN_CATEGORY_PUBLIC;
-	u8 action = P2P_PUB_ACTION_ACTION;
-	u32 p2poui = cpu_to_be32(P2POUI);
-	u8 oui_subtype = P2P_GO_NEGO_CONF;
-	u8 p2pie[ 255 ] = { 0x00 };
-	u8 p2pielen = 0;
-	struct xmit_frame *pmgntframe;
-	struct pkt_attrib *pattrib;
-	unsigned char *pframe;
-	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#ifdef CONFIG_8723AU_P2P
-	u32 wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-		return;
-
-	DBG_8723A("[%s] In\n", __func__);
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	ether_addr_copy(pwlanhdr->addr1, raddr);
-	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
-	ether_addr_copy(pwlanhdr->addr3, myid(&padapter->eeprompriv));
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
-
-	pframe += sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
-
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *)&p2poui,
-				     &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1,
-				     &pwdinfo->negotiation_dialog_token,
-				  &pattrib->pktlen);
-	/*	P2P OUI */
-	p2pielen = 0;
-	p2pie[p2pielen++] = 0x50;
-	p2pie[p2pielen++] = 0x6F;
-	p2pie[p2pielen++] = 0x9A;
-	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
-
-	/*	Commented by Albert 20110306 */
-	/*	According to the P2P Specification, the group negoitation
-		request frame should contain 5 P2P attributes */
-	/*	1. Status */
-	/*	2. P2P Capability */
-	/*	3. Operating Channel */
-	/*	4. Channel List */
-	/*	5. Group ID	(if this WiFi is GO) */
-
-	/*	P2P Status */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_STATUS;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0001);
-	p2pielen += 2;
-
-	/*	Value: */
-	p2pie[p2pielen++] = result;
-
-	/*	P2P Capability */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Device Capability Bitmap, 1 byte */
-	p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
-
-	/*	Group Capability Bitmap, 1 byte */
-	if (pwdinfo->persistent_supported) {
-		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN |
-			P2P_GRPCAP_PERSISTENT_GROUP;
-	} else {
-		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN;
-	}
-
-	/*	Operating Channel */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0005);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Country String */
-	p2pie[p2pielen++] = 'X';
-	p2pie[p2pielen++] = 'X';
-
-	/*	The third byte should be set to 0x04. */
-	/*	Described in the "Operating Channel Attribute" section. */
-	p2pie[p2pielen++] = 0x04;
-
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
-		if (pwdinfo->peer_operating_ch <= 14) {
-			/*	Operating Class */
-			p2pie[p2pielen++] = 0x51;
-		} else if ((pwdinfo->peer_operating_ch >= 36) &&
-			 (pwdinfo->peer_operating_ch <= 48)) {
-			/*	Operating Class */
-			p2pie[p2pielen++] = 0x73;
-		} else {
-			/*	Operating Class */
-			p2pie[p2pielen++] = 0x7c;
-		}
-
-		p2pie[p2pielen++] = pwdinfo->peer_operating_ch;
-	} else {
-		if (pwdinfo->operating_channel <= 14) {
-			/*	Operating Class */
-			p2pie[p2pielen++] = 0x51;
-		}
-		else if ((pwdinfo->operating_channel >= 36) &&
-			 (pwdinfo->operating_channel <= 48)) {
-			/*	Operating Class */
-			p2pie[p2pielen++] = 0x73;
-		} else {
-			/*	Operating Class */
-			p2pie[p2pielen++] = 0x7c;
-		}
-
-		/*	Channel Number */
-		/*	Use the listen channel as the operating channel */
-		p2pie[p2pielen++] = pwdinfo->operating_channel;
-	}
-
-	/*	Channel List */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) =
-		cpu_to_le16(pwdinfo->channel_list_attr_len);
-	p2pielen += 2;
-
-	/*	Value: */
-	memcpy(p2pie + p2pielen, pwdinfo->channel_list_attr,
-	       pwdinfo->channel_list_attr_len);
-	p2pielen += pwdinfo->channel_list_attr_len;
-
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
-		/*	Group ID Attribute */
-		/*	Type: */
-		p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;
-
-		/*	Length: */
-		*(u16*) (p2pie + p2pielen) =
-			cpu_to_le16(ETH_ALEN + pwdinfo->nego_ssidlen);
-		p2pielen += 2;
-
-		/*	Value: */
-		/*	p2P Device Address */
-		memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);
-		p2pielen += ETH_ALEN;
-
-		/*	SSID */
-		memcpy(p2pie + p2pielen, pwdinfo->nego_ssid,
-		       pwdinfo->nego_ssidlen);
-		p2pielen += pwdinfo->nego_ssidlen;
-	}
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen,
-			       (unsigned char *)p2pie, &pattrib->pktlen);
-
-#ifdef CONFIG_8723AU_P2P
-	wfdielen = build_nego_confirm_wfd_ie(pwdinfo, pframe);
-	pframe += wfdielen;
-	pattrib->pktlen += wfdielen;
-#endif /* CONFIG_8723AU_P2P */
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	dump_mgntframe23a(padapter, pmgntframe);
-
-	return;
-}
-
-void issue_p2p_invitation_request23a(struct rtw_adapter *padapter, u8* raddr)
-{
-	unsigned char category = WLAN_CATEGORY_PUBLIC;
-	u8 action = P2P_PUB_ACTION_ACTION;
-	u32 p2poui = cpu_to_be32(P2POUI);
-	u8 oui_subtype = P2P_INVIT_REQ;
-	u8 p2pie[ 255 ] = { 0x00 };
-	u8 p2pielen = 0;
-	u8 dialogToken = 3;
-	u16 len_channellist_attr = 0;
-#ifdef CONFIG_8723AU_P2P
-	u32 wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
-	int i, j;
-	struct xmit_frame *pmgntframe;
-	struct pkt_attrib *pattrib;
-	unsigned char *pframe;
-	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-		return;
-
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	ether_addr_copy(pwlanhdr->addr1, raddr);
-	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
-	ether_addr_copy(pwlanhdr->addr3, raddr);
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
-
-	pframe += sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
-
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *) &p2poui,
-				     &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen);
-
-	/*	P2P IE Section. */
-
-	/*	P2P OUI */
-	p2pielen = 0;
-	p2pie[p2pielen++] = 0x50;
-	p2pie[p2pielen++] = 0x6F;
-	p2pie[p2pielen++] = 0x9A;
-	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
-
-	/*	Commented by Albert 20101011 */
-	/*	According to the P2P Specification, the P2P Invitation
-		request frame should contain 7 P2P attributes */
-	/*	1. Configuration Timeout */
-	/*	2. Invitation Flags */
-	/*	3. Operating Channel	(Only GO) */
-	/*	4. P2P Group BSSID	(Should be included if I am the GO) */
-	/*	5. Channel List */
-	/*	6. P2P Group ID */
-	/*	7. P2P Device Info */
-
-	/*	Configuration Timeout */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	2 seconds needed to be the P2P GO */
-	p2pie[p2pielen++] = 200;
-	/*	2 seconds needed to be the P2P Client */
-	p2pie[p2pielen++] = 200;
-
-	/*	Invitation Flags */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_INVITATION_FLAGS;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0001);
-	p2pielen += 2;
-
-	/*	Value: */
-	p2pie[p2pielen++] = P2P_INVITATION_FLAGS_PERSISTENT;
-
-	/*	Operating Channel */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0005);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Country String */
-	p2pie[p2pielen++] = 'X';
-	p2pie[p2pielen++] = 'X';
-
-	/*	The third byte should be set to 0x04. */
-	/*	Described in the "Operating Channel Attribute" section. */
-	p2pie[p2pielen++] = 0x04;
-
-	/*	Operating Class */
-	if (pwdinfo->invitereq_info.operating_ch <= 14)
-		p2pie[p2pielen++] = 0x51;
-	else if ((pwdinfo->invitereq_info.operating_ch >= 36) &&
-		 (pwdinfo->invitereq_info.operating_ch <= 48))
-		p2pie[p2pielen++] = 0x73;
-	else
-		p2pie[p2pielen++] = 0x7c;
-
-	/*	Channel Number */
-	/*	operating channel number */
-	p2pie[p2pielen++] = pwdinfo->invitereq_info.operating_ch;
-
-	if (ether_addr_equal(myid(&padapter->eeprompriv),
-			     pwdinfo->invitereq_info.go_bssid)) {
-		/*	P2P Group BSSID */
-		/*	Type: */
-		p2pie[p2pielen++] = P2P_ATTR_GROUP_BSSID;
-
-		/*	Length: */
-		*(u16*) (p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
-		p2pielen += 2;
-
-		/*	Value: */
-		/*	P2P Device Address for GO */
-		memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid,
-		       ETH_ALEN);
-		p2pielen += ETH_ALEN;
-	}
-
-	/*	Channel List */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
-
-	/*	Length: */
-	/*  Country String(3) */
-	/*  + (Operating Class (1) + Number of Channels(1)) *
-	    Operation Classes (?) */
-	/*  + number of channels in all classes */
-	len_channellist_attr = 3 +
-		(1 + 1) * (u16)pmlmeext->channel_list.reg_classes +
-		get_reg_classes_full_count(pmlmeext->channel_list);
-
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Country String */
-	p2pie[p2pielen++] = 'X';
-	p2pie[p2pielen++] = 'X';
-
-	/*	The third byte should be set to 0x04. */
-	/*	Described in the "Operating Channel Attribute" section. */
-	p2pie[p2pielen++] = 0x04;
-
-	/*	Channel Entry List */
-	for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
-		/*	Operating Class */
-		p2pie[p2pielen++] =
-			pmlmeext->channel_list.reg_class[j].reg_class;
-
-		/*	Number of Channels */
-		p2pie[p2pielen++] =
-			pmlmeext->channel_list.reg_class[j].channels;
-
-		/*	Channel List */
-		for (i = 0;
-		     i < pmlmeext->channel_list.reg_class[j].channels; i++) {
-			p2pie[p2pielen++] =
-				pmlmeext->channel_list.reg_class[j].channel[i];
-		}
-	}
-
-	/*	P2P Group ID */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) =
-		cpu_to_le16(6 + pwdinfo->invitereq_info.ssidlen);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	P2P Device Address for GO */
-	memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN);
-	p2pielen += ETH_ALEN;
-
-	/*	SSID */
-	memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_ssid,
-	       pwdinfo->invitereq_info.ssidlen);
-	p2pielen += pwdinfo->invitereq_info.ssidlen;
-
-	/*	Device Info */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	21 -> P2P Device Address (6bytes) + Config Methods (2bytes) +
-		Primary Device Type (8bytes) */
-	/*	+ NumofSecondDevType (1byte) + WPS Device Name ID field
-		(2bytes) + WPS Device Name Len field (2bytes) */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	P2P Device Address */
-	memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
-	p2pielen += ETH_ALEN;
-
-	/*	Config Method */
-	/*	This field should be big endian. Noted by P2P specification. */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY);
-	p2pielen += 2;
-
-	/*	Primary Device Type */
-	/*	Category ID */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
-	p2pielen += 2;
-
-	/*	OUI */
-	*(u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI);
-	p2pielen += 4;
-
-	/*	Sub Category ID */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
-	p2pielen += 2;
-
-	/*	Number of Secondary Device Types */
-	p2pie[p2pielen++] = 0x00;	/*	No Secondary Device Type List */
-
-	/*	Device Name */
-	/*	Type: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
-	p2pielen += 2;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
-	p2pielen += 2;
-
-	/*	Value: */
-	memcpy(p2pie + p2pielen, pwdinfo->device_name,
-	       pwdinfo->device_name_len);
-	p2pielen += pwdinfo->device_name_len;
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen,
-			       (unsigned char *) p2pie, &pattrib->pktlen);
-
-#ifdef CONFIG_8723AU_P2P
-	wfdielen = build_invitation_req_wfd_ie(pwdinfo, pframe);
-	pframe += wfdielen;
-	pattrib->pktlen += wfdielen;
-#endif /* CONFIG_8723AU_P2P */
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	dump_mgntframe23a(padapter, pmgntframe);
-
-	return;
-}
-
-void issue_p2p_invitation_response23a(struct rtw_adapter *padapter, u8 *raddr,
-				      u8 dialogToken, u8 status_code)
-{
-	unsigned char category = WLAN_CATEGORY_PUBLIC;
-	u8 action = P2P_PUB_ACTION_ACTION;
-	u32 p2poui = cpu_to_be32(P2POUI);
-	u8 oui_subtype = P2P_INVIT_RESP;
-	u8 p2pie[ 255 ] = { 0x00 };
-	u8 p2pielen = 0;
-	u16 len_channellist_attr = 0;
-#ifdef CONFIG_8723AU_P2P
-	u32 wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
-	int i, j;
-
-	struct xmit_frame *pmgntframe;
-	struct pkt_attrib *pattrib;
-	unsigned char *pframe;
-	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-		return;
-
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	ether_addr_copy(pwlanhdr->addr1, raddr);
-	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
-	ether_addr_copy(pwlanhdr->addr3, raddr);
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
-
-	pframe += sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
-
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *)&p2poui,
-				     &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen);
-
-	/*	P2P IE Section. */
-
-	/*	P2P OUI */
-	p2pielen = 0;
-	p2pie[p2pielen++] = 0x50;
-	p2pie[p2pielen++] = 0x6F;
-	p2pie[p2pielen++] = 0x9A;
-	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
-
-	/*	Commented by Albert 20101005 */
-	/*	According to the P2P Specification, the P2P Invitation
-		response frame should contain 5 P2P attributes */
-	/*	1. Status */
-	/*	2. Configuration Timeout */
-	/*	3. Operating Channel	(Only GO) */
-	/*	4. P2P Group BSSID	(Only GO) */
-	/*	5. Channel List */
-
-	/*	P2P Status */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_STATUS;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0001);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	When status code is P2P_STATUS_FAIL_INFO_UNAVAILABLE. */
-	/*	Sent the event receiving the P2P Invitation Req frame
-		to DMP UI. */
-	/*	DMP had to compare the MAC address to find out the profile. */
-	/*	So, the WiFi driver will send the
-		P2P_STATUS_FAIL_INFO_UNAVAILABLE to NB. */
-	/*	If the UI found the corresponding profile, the WiFi driver
-		sends the P2P Invitation Req */
-	/*	to NB to rebuild the persistent group. */
-	p2pie[p2pielen++] = status_code;
-
-	/*	Configuration Timeout */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	2 seconds needed to be the P2P GO */
-	p2pie[p2pielen++] = 200;
-	/*	2 seconds needed to be the P2P Client */
-	p2pie[p2pielen++] = 200;
-
-	if (status_code == P2P_STATUS_SUCCESS) {
-		if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
-			/* The P2P Invitation request frame asks this
-			   Wi-Fi device to be the P2P GO */
-			/* In this case, the P2P Invitation response
-			   frame should carry the two more P2P attributes. */
-			/* First one is operating channel attribute. */
-			/* Second one is P2P Group BSSID attribute. */
-
-			/* Operating Channel */
-			/* Type: */
-			p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
-
-			/* Length: */
-			*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0005);
-			p2pielen += 2;
-
-			/* Value: */
-			/* Country String */
-			p2pie[p2pielen++] = 'X';
-			p2pie[p2pielen++] = 'X';
-
-			/* The third byte should be set to 0x04. */
-			/* Described in the "Operating Channel Attribute"
-			   section. */
-			p2pie[p2pielen++] = 0x04;
-
-			/* Operating Class */
-			/*	Copy from SD7 */
-			p2pie[p2pielen++] = 0x51;
-
-			/* Channel Number */
-			/*	operating channel number */
-			p2pie[p2pielen++] = pwdinfo->operating_channel;
-
-			/*	P2P Group BSSID */
-			/*	Type: */
-			p2pie[p2pielen++] = P2P_ATTR_GROUP_BSSID;
-
-			/*	Length: */
-			*(u16*) (p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
-			p2pielen += 2;
-
-			/*	Value: */
-			/*	P2P Device Address for GO */
-			memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv),
-			       ETH_ALEN);
-			p2pielen += ETH_ALEN;
-		}
-
-		/*	Channel List */
-		/*	Type: */
-		p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
-
-		/*	Length: */
-		/*  Country String(3) */
-		/*  + (Operating Class (1) + Number of Channels(1)) *
-		    Operation Classes (?) */
-		/*  + number of channels in all classes */
-		len_channellist_attr = 3 +
-			(1 + 1) * (u16)pmlmeext->channel_list.reg_classes +
-			get_reg_classes_full_count(pmlmeext->channel_list);
-
-		*(u16*) (p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
-		p2pielen += 2;
-
-		/*	Value: */
-		/*	Country String */
-		p2pie[p2pielen++] = 'X';
-		p2pie[p2pielen++] = 'X';
-
-		/* The third byte should be set to 0x04. */
-		/* Described in the "Operating Channel Attribute" section. */
-		p2pie[p2pielen++] = 0x04;
-
-		/*	Channel Entry List */
-		for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
-			/*	Operating Class */
-			p2pie[p2pielen++] =
-				pmlmeext->channel_list.reg_class[j].reg_class;
-
-			/*	Number of Channels */
-			p2pie[p2pielen++] =
-				pmlmeext->channel_list.reg_class[j].channels;
-
-			/*	Channel List */
-			for (i = 0;
-			     i < pmlmeext->channel_list.reg_class[j].channels;
-			     i++) {
-				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
-			}
-		}
-	}
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen,
-			       (unsigned char *)p2pie, &pattrib->pktlen);
-
-#ifdef CONFIG_8723AU_P2P
-	wfdielen = build_invitation_resp_wfd_ie(pwdinfo, pframe);
-	pframe += wfdielen;
-	pattrib->pktlen += wfdielen;
-#endif /* CONFIG_8723AU_P2P */
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	dump_mgntframe23a(padapter, pmgntframe);
-
-	return;
-}
-
-void issue_p2p_provision_request23a(struct rtw_adapter *padapter, u8 *pssid,
-				    u8 ussidlen, u8 *pdev_raddr)
-{
-	unsigned char category = WLAN_CATEGORY_PUBLIC;
-	u8 action = P2P_PUB_ACTION_ACTION;
-	u8 dialogToken = 1;
-	u32 p2poui = cpu_to_be32(P2POUI);
-	u8 oui_subtype = P2P_PROVISION_DISC_REQ;
-	u8 wpsie[100] = { 0x00 };
-	u8 wpsielen = 0;
-	u32 p2pielen = 0;
-#ifdef CONFIG_8723AU_P2P
-	u32 wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
-	struct xmit_frame *pmgntframe;
-	struct pkt_attrib *pattrib;
-	unsigned char *pframe;
-	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-		return;
-
-	DBG_8723A("[%s] In\n", __func__);
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	ether_addr_copy(pwlanhdr->addr1, pdev_raddr);
-	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
-	ether_addr_copy(pwlanhdr->addr3, pdev_raddr);
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
-
-	pframe += sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
-
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *)&p2poui,
-				     &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen);
-
-	p2pielen = build_prov_disc_request_p2p_ie23a(pwdinfo, pframe, pssid,
-						     ussidlen, pdev_raddr);
-
-	pframe += p2pielen;
-	pattrib->pktlen += p2pielen;
-
-	wpsielen = 0;
-	/*	WPS OUI */
-	*(u32*) (wpsie) = cpu_to_be32(WPSOUI);
-	wpsielen += 4;
-
-	/*	WPS version */
-	/*	Type: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
-	wpsielen += 2;
-
-	/*	Length: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0001);
-	wpsielen += 2;
-
-	/*	Value: */
-	wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
-
-	/*	Config Method */
-	/*	Type: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD);
-	wpsielen += 2;
-
-	/*	Length: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0002);
-	wpsielen += 2;
-
-	/*	Value: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(pwdinfo->tx_prov_disc_info.wps_config_method_request);
-	wpsielen += 2;
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, wpsielen,
-			       (unsigned char *) wpsie, &pattrib->pktlen);
-
-#ifdef CONFIG_8723AU_P2P
-	wfdielen = build_provdisc_req_wfd_ie(pwdinfo, pframe);
-	pframe += wfdielen;
-	pattrib->pktlen += wfdielen;
-#endif /* CONFIG_8723AU_P2P */
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	dump_mgntframe23a(padapter, pmgntframe);
-
-	return;
-}
-
-static u8 is_matched_in_profilelist(u8 *peermacaddr,
-				    struct profile_info *profileinfo)
-{
-	u8 i, match_result = 0;
-
-	DBG_8723A("[%s] peermac = %.2X %.2X %.2X %.2X %.2X %.2X\n", __func__,
-		  peermacaddr[0], peermacaddr[1], peermacaddr[2],
-		  peermacaddr[3], peermacaddr[4], peermacaddr[5]);
-
-	for (i = 0; i < P2P_MAX_PERSISTENT_GROUP_NUM; i++, profileinfo++) {
-	       DBG_8723A("[%s] profileinfo_mac = %.2X %.2X %.2X %.2X %.2X "
-			 "%.2X\n", __func__, profileinfo->peermac[0],
-			 profileinfo->peermac[1], profileinfo->peermac[2],
-			 profileinfo->peermac[3], profileinfo->peermac[4],
-			 profileinfo->peermac[5]);
-		if (ether_addr_equal(peermacaddr, profileinfo->peermac)) {
-			match_result = 1;
-			DBG_8723A("[%s] Match!\n", __func__);
-			break;
-		}
-	}
-
-	return match_result;
-}
-
-void issue_probersp23a_p2p23a(struct rtw_adapter *padapter, unsigned char *da)
-{
-	struct xmit_frame *pmgntframe;
-	struct pkt_attrib *pattrib;
-	unsigned char *pframe;
-	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
-	unsigned char *mac;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	u16 beacon_interval = 100;
-	u16 capInfo = 0;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	u8 wpsie[255] = { 0x00 };
-	u32 wpsielen = 0, p2pielen = 0;
-#ifdef CONFIG_8723AU_P2P
-	u32 wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
-	struct cfg80211_wifidirect_info *pcfg80211_wdinfo =
-		&padapter->cfg80211_wdinfo;
-	struct ieee80211_channel *ieee_ch =
-		&pcfg80211_wdinfo->remain_on_ch_channel;
-	u8 listen_channel =
-		(u8)ieee80211_frequency_to_channel(ieee_ch->center_freq);
-
-	/* DBG_8723A("%s\n", __func__); */
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-	{
-		return;
-	}
-
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	mac = myid(&padapter->eeprompriv);
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-	ether_addr_copy(pwlanhdr->addr1, da);
-	ether_addr_copy(pwlanhdr->addr2, mac);
-
-	/*	Use the device address for BSSID field. */
-	ether_addr_copy(pwlanhdr->addr3, mac);
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(fctrl, WIFI_PROBERSP);
-
-	pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = pattrib->hdrlen;
-	pframe += pattrib->hdrlen;
-
-	/* timestamp will be inserted by hardware */
-	pframe += 8;
-	pattrib->pktlen += 8;
-
-	/*  beacon interval: 2 bytes */
-	memcpy(pframe, (unsigned char *) &beacon_interval, 2);
-	pframe += 2;
-	pattrib->pktlen += 2;
-
-	/*	capability info: 2 bytes */
-	/*	ESS and IBSS bits must be 0 (defined in the 3.1.2.1.1 of
-		WiFi Direct Spec) */
-	capInfo |= cap_ShortPremble;
-	capInfo |= cap_ShortSlot;
-
-	memcpy(pframe, (unsigned char *) &capInfo, 2);
-	pframe += 2;
-	pattrib->pktlen += 2;
-
-	/*  SSID */
-	pframe = rtw_set_ie23a(pframe, _SSID_IE_, 7, pwdinfo->p2p_wildcard_ssid,
-			       &pattrib->pktlen);
-
-	/*  supported rates... */
-	/*	Use the OFDM rate in the P2P probe response frame.
-		(6(B), 9(B), 12, 18, 24, 36, 48, 54) */
-	pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_, 8,
-			       pwdinfo->support_rate, &pattrib->pktlen);
-
-	/*  DS parameter set */
-	if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled &&
-	    listen_channel != 0) {
-		pframe = rtw_set_ie23a(pframe, _DSSET_IE_, 1, (unsigned char *)
-				       &listen_channel, &pattrib->pktlen);
-	} else {
-		pframe = rtw_set_ie23a(pframe, _DSSET_IE_, 1, (unsigned char *)
-				       &pwdinfo->listen_channel,
-				       &pattrib->pktlen);
-	}
-
-	if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) {
-		if (pmlmepriv->wps_probe_resp_ie &&
-		    pmlmepriv->p2p_probe_resp_ie) {
-			/* WPS IE */
-			memcpy(pframe, pmlmepriv->wps_probe_resp_ie,
-			       pmlmepriv->wps_probe_resp_ie_len);
-			pattrib->pktlen += pmlmepriv->wps_probe_resp_ie_len;
-			pframe += pmlmepriv->wps_probe_resp_ie_len;
-
-			/* P2P IE */
-			memcpy(pframe, pmlmepriv->p2p_probe_resp_ie,
-			       pmlmepriv->p2p_probe_resp_ie_len);
-			pattrib->pktlen += pmlmepriv->p2p_probe_resp_ie_len;
-			pframe += pmlmepriv->p2p_probe_resp_ie_len;
-		}
-	} else {
-
-		/*	Todo: WPS IE */
-		/*	Noted by Albert 20100907 */
-		/*	According to the WPS specification, all the WPS
-			attribute is presented by Big Endian. */
-
-		wpsielen = 0;
-		/*	WPS OUI */
-		*(u32*) (wpsie) = cpu_to_be32(WPSOUI);
-		wpsielen += 4;
-
-		/*	WPS version */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0001);
-		wpsielen += 2;
-
-		/*	Value: */
-		wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
-
-		/*	WiFi Simple Config State */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) =
-			cpu_to_be16(WPS_ATTR_SIMPLE_CONF_STATE);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0001);
-		wpsielen += 2;
-
-		/*	Value: */
-		wpsie[wpsielen++] = WPS_WSC_STATE_NOT_CONFIG;
-
-		/*	Response Type */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_RESP_TYPE);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0001);
-		wpsielen += 2;
-
-		/*	Value: */
-		wpsie[wpsielen++] = WPS_RESPONSE_TYPE_8021X;
-
-		/*	UUID-E */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_UUID_E);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0010);
-		wpsielen += 2;
-
-		/*	Value: */
-		memcpy(wpsie + wpsielen, myid(&padapter->eeprompriv), ETH_ALEN);
-		wpsielen += 0x10;
-
-		/*	Manufacturer */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MANUFACTURER);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0007);
-		wpsielen += 2;
-
-		/*	Value: */
-		memcpy(wpsie + wpsielen, "Realtek", 7);
-		wpsielen += 7;
-
-		/*	Model Name */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MODEL_NAME);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0006);
-		wpsielen += 2;
-
-		/*	Value: */
-		memcpy(wpsie + wpsielen, "8192CU", 6);
-		wpsielen += 6;
-
-		/*	Model Number */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MODEL_NUMBER);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0001);
-		wpsielen += 2;
-
-		/*	Value: */
-		wpsie[ wpsielen++ ] = 0x31;		/*	character 1 */
-
-		/*	Serial Number */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) =
-			cpu_to_be16(WPS_ATTR_SERIAL_NUMBER);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(ETH_ALEN);
-		wpsielen += 2;
-
-		/*	Value: */
-		memcpy(wpsie + wpsielen, "123456", ETH_ALEN);
-		wpsielen += ETH_ALEN;
-
-		/*	Primary Device Type */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) =
-			cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0008);
-		wpsielen += 2;
-
-		/*	Value: */
-		/*	Category ID */
-		*(u16*) (wpsie + wpsielen) =
-			cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
-		wpsielen += 2;
-
-		/*	OUI */
-		*(u32*) (wpsie + wpsielen) = cpu_to_be32(WPSOUI);
-		wpsielen += 4;
-
-		/*	Sub Category ID */
-		*(u16*) (wpsie + wpsielen) =
-			cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
-		wpsielen += 2;
-
-		/*	Device Name */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) =
-			cpu_to_be16(pwdinfo->device_name_len);
-		wpsielen += 2;
-
-		/*	Value: */
-		if (pwdinfo->device_name_len) {
-			memcpy(wpsie + wpsielen, pwdinfo->device_name,
-			       pwdinfo->device_name_len);
-			wpsielen += pwdinfo->device_name_len;
-		}
-
-		/*	Config Method */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0002);
-		wpsielen += 2;
-
-		/*	Value: */
-		*(u16*) (wpsie + wpsielen) =
-			cpu_to_be16(pwdinfo->supported_wps_cm);
-		wpsielen += 2;
-
-		pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, wpsielen,
-				       (unsigned char *)wpsie,
-				       &pattrib->pktlen);
-
-		p2pielen = build_probe_resp_p2p_ie23a(pwdinfo, pframe);
-		pframe += p2pielen;
-		pattrib->pktlen += p2pielen;
-	}
-
-#ifdef CONFIG_8723AU_P2P
-	if (pwdinfo->wfd_info->wfd_enable) {
-		wfdielen = build_probe_resp_wfd_ie(pwdinfo, pframe, 0);
-		pframe += wfdielen;
-		pattrib->pktlen += wfdielen;
-	} else if (pmlmepriv->wfd_probe_resp_ie &&
-		 pmlmepriv->wfd_probe_resp_ie_len > 0) {
-		/* WFD IE */
-		memcpy(pframe, pmlmepriv->wfd_probe_resp_ie,
-		       pmlmepriv->wfd_probe_resp_ie_len);
-		pattrib->pktlen += pmlmepriv->wfd_probe_resp_ie_len;
-		pframe += pmlmepriv->wfd_probe_resp_ie_len;
-	}
-#endif /* CONFIG_8723AU_P2P */
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	dump_mgntframe23a(padapter, pmgntframe);
-
-	return;
-}
-
-static int _issue23a_probereq_p2p(struct rtw_adapter *padapter, u8 *da,
-				  int wait_ack)
-{
-	int ret = _FAIL;
-	struct xmit_frame *pmgntframe;
-	struct pkt_attrib *pattrib;
-	unsigned char *pframe;
-	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
-	unsigned char *mac;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	u8	bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
-	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
-	u8 wpsie[255] = {0x00}, p2pie[255] = {0x00};
-	u16 wpsielen = 0, p2pielen = 0;
-#ifdef CONFIG_8723AU_P2P
-	u32 wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-		goto exit;
-
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	mac = myid(&padapter->eeprompriv);
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	if (da) {
-		ether_addr_copy(pwlanhdr->addr1, da);
-		ether_addr_copy(pwlanhdr->addr3, da);
-	} else {
-		if ((pwdinfo->p2p_info.scan_op_ch_only) ||
-		    (pwdinfo->rx_invitereq_info.scan_op_ch_only)) {
-			/*	This two flags will be set when this is
-				only the P2P client mode. */
-			ether_addr_copy(pwlanhdr->addr1,
-					pwdinfo->p2p_peer_interface_addr);
-			ether_addr_copy(pwlanhdr->addr3,
-					pwdinfo->p2p_peer_interface_addr);
-		} else {
-			/*	broadcast probe request frame */
-			ether_addr_copy(pwlanhdr->addr1, bc_addr);
-			ether_addr_copy(pwlanhdr->addr3, bc_addr);
-		}
-	}
-	ether_addr_copy(pwlanhdr->addr2, mac);
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_PROBEREQ);
-
-	pframe += sizeof (struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof (struct ieee80211_hdr_3addr);
-
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ)) {
-		pframe = rtw_set_ie23a(pframe, _SSID_IE_,
-				    pwdinfo->tx_prov_disc_info.ssid.ssid_len,
-				    pwdinfo->tx_prov_disc_info.ssid.ssid,
-				    &pattrib->pktlen);
-	} else {
-		pframe = rtw_set_ie23a(pframe, _SSID_IE_,
-				       P2P_WILDCARD_SSID_LEN,
-				       pwdinfo->p2p_wildcard_ssid,
-				       &pattrib->pktlen);
-	}
-	/*	Use the OFDM rate in the P2P probe request frame.
-		(6(B), 9(B), 12(B), 24(B), 36, 48, 54) */
-	pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_, 8,
-			       pwdinfo->support_rate, &pattrib->pktlen);
-
-	if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) {
-		if (pmlmepriv->wps_probe_req_ie &&
-		    pmlmepriv->p2p_probe_req_ie) {
-			/* WPS IE */
-			memcpy(pframe, pmlmepriv->wps_probe_req_ie,
-			       pmlmepriv->wps_probe_req_ie_len);
-			pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
-			pframe += pmlmepriv->wps_probe_req_ie_len;
-
-			/* P2P IE */
-			memcpy(pframe, pmlmepriv->p2p_probe_req_ie,
-			       pmlmepriv->p2p_probe_req_ie_len);
-			pattrib->pktlen += pmlmepriv->p2p_probe_req_ie_len;
-			pframe += pmlmepriv->p2p_probe_req_ie_len;
-		}
-	} else {
-
-		/*	WPS IE */
-		/*	Noted by Albert 20110221 */
-		/*	According to the WPS specification, all the WPS
-			attribute is presented by Big Endian. */
-
-		wpsielen = 0;
-		/*	WPS OUI */
-		*(u32*) (wpsie) = cpu_to_be32(WPSOUI);
-		wpsielen += 4;
-
-		/*	WPS version */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0001);
-		wpsielen += 2;
-
-		/*	Value: */
-		wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
-
-		if (pmlmepriv->wps_probe_req_ie == NULL) {
-			/*	UUID-E */
-			/*	Type: */
-			*(u16*) (wpsie + wpsielen) =
-				cpu_to_be16(WPS_ATTR_UUID_E);
-			wpsielen += 2;
-
-			/*	Length: */
-			*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0010);
-			wpsielen += 2;
-
-			/*	Value: */
-			memcpy(wpsie + wpsielen, myid(&padapter->eeprompriv),
-			       ETH_ALEN);
-			wpsielen += 0x10;
-
-			/*	Config Method */
-			/*	Type: */
-			*(u16*) (wpsie + wpsielen) =
-				cpu_to_be16(WPS_ATTR_CONF_METHOD);
-			wpsielen += 2;
-
-			/*	Length: */
-			*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0002);
-			wpsielen += 2;
-
-			/*	Value: */
-			*(u16*) (wpsie + wpsielen) =
-				cpu_to_be16(pwdinfo->supported_wps_cm);
-			wpsielen += 2;
-		}
-
-		/*	Device Name */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) =
-			cpu_to_be16(pwdinfo->device_name_len);
-		wpsielen += 2;
-
-		/*	Value: */
-		memcpy(wpsie + wpsielen, pwdinfo->device_name,
-		       pwdinfo->device_name_len);
-		wpsielen += pwdinfo->device_name_len;
-
-		/*	Primary Device Type */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) =
-			cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0008);
-		wpsielen += 2;
-
-		/*	Value: */
-		/*	Category ID */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_PDT_CID_RTK_WIDI);
-		wpsielen += 2;
-
-		/*	OUI */
-		*(u32*) (wpsie + wpsielen) = cpu_to_be32(WPSOUI);
-		wpsielen += 4;
-
-		/*	Sub Category ID */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_PDT_SCID_RTK_DMP);
-		wpsielen += 2;
-
-		/*	Device Password ID */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0002);
-		wpsielen += 2;
-
-		/*	Value: */
-		/*	Registrar-specified */
-		*(u16*) (wpsie + wpsielen) =
-			cpu_to_be16(WPS_DPID_REGISTRAR_SPEC);
-		wpsielen += 2;
-
-		pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, wpsielen,
-				       (unsigned char *)wpsie,
-				       &pattrib->pktlen);
-
-		/*	P2P OUI */
-		p2pielen = 0;
-		p2pie[p2pielen++] = 0x50;
-		p2pie[p2pielen++] = 0x6F;
-		p2pie[p2pielen++] = 0x9A;
-		p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
-
-		/*	Commented by Albert 20110221 */
-		/*	According to the P2P Specification, the probe request
-			frame should contain 5 P2P attributes */
-		/*	1. P2P Capability */
-		/*	2. P2P Device ID if this probe request wants to
-			find the specific P2P device */
-		/*	3. Listen Channel */
-		/*	4. Extended Listen Timing */
-		/*	5. Operating Channel if this WiFi is working as
-			the group owner now */
-
-		/*	P2P Capability */
-		/*	Type: */
-		p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
-
-		/*	Length: */
-		*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002);
-		p2pielen += 2;
-
-		/*	Value: */
-		/*	Device Capability Bitmap, 1 byte */
-		p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
-
-		/*	Group Capability Bitmap, 1 byte */
-		if (pwdinfo->persistent_supported)
-			p2pie[p2pielen++] = P2P_GRPCAP_PERSISTENT_GROUP |
-				DMP_P2P_GRPCAP_SUPPORT;
-		else
-			p2pie[p2pielen++] = DMP_P2P_GRPCAP_SUPPORT;
-
-		/*	Listen Channel */
-		/*	Type: */
-		p2pie[p2pielen++] = P2P_ATTR_LISTEN_CH;
-
-		/*	Length: */
-		*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0005);
-		p2pielen += 2;
-
-		/*	Value: */
-		/*	Country String */
-		p2pie[p2pielen++] = 'X';
-		p2pie[p2pielen++] = 'X';
-
-		/* The third byte should be set to 0x04. */
-		/* Described in the "Operating Channel Attribute" section. */
-		p2pie[p2pielen++] = 0x04;
-
-		/*	Operating Class */
-		p2pie[p2pielen++] = 0x51;	/*	Copy from SD7 */
-
-		/*	Channel Number */
-		/*	listen channel */
-		p2pie[p2pielen++] = pwdinfo->listen_channel;
-
-		/*	Extended Listen Timing */
-		/*	Type: */
-		p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING;
-
-		/*	Length: */
-		*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0004);
-		p2pielen += 2;
-
-		/*	Value: */
-		/*	Availability Period */
-		*(u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF);
-		p2pielen += 2;
-
-		/*	Availability Interval */
-		*(u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF);
-		p2pielen += 2;
-
-		if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
-			/* Operating Channel (if this WiFi is working as
-			   the group owner now) */
-			/* Type: */
-			p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
-
-			/*	Length: */
-			*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0005);
-			p2pielen += 2;
-
-			/*	Value: */
-			/*	Country String */
-			p2pie[p2pielen++] = 'X';
-			p2pie[p2pielen++] = 'X';
-
-			/* The third byte should be set to 0x04. */
-			/* Described in the "Operating Channel Attribute"
-			   section. */
-			p2pie[p2pielen++] = 0x04;
-
-			/*	Operating Class */
-			p2pie[p2pielen++] = 0x51;	/*	Copy from SD7 */
-
-			/*	Channel Number */
-			/*	operating channel number */
-			p2pie[p2pielen++] = pwdinfo->operating_channel;
-		}
-
-		pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen,
-				       (unsigned char *)p2pie,
-				       &pattrib->pktlen);
-
-		if (pmlmepriv->wps_probe_req_ie) {
-			/* WPS IE */
-			memcpy(pframe, pmlmepriv->wps_probe_req_ie,
-			       pmlmepriv->wps_probe_req_ie_len);
-			pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
-			pframe += pmlmepriv->wps_probe_req_ie_len;
-		}
-	}
-
-#ifdef CONFIG_8723AU_P2P
-	if (pwdinfo->wfd_info->wfd_enable) {
-		wfdielen = build_probe_req_wfd_ie(pwdinfo, pframe);
-		pframe += wfdielen;
-		pattrib->pktlen += wfdielen;
-	} else if (pmlmepriv->wfd_probe_req_ie &&
-		   pmlmepriv->wfd_probe_req_ie_len>0) {
-		/* WFD IE */
-		memcpy(pframe, pmlmepriv->wfd_probe_req_ie,
-		       pmlmepriv->wfd_probe_req_ie_len);
-		pattrib->pktlen += pmlmepriv->wfd_probe_req_ie_len;
-		pframe += pmlmepriv->wfd_probe_req_ie_len;
-	}
-#endif /* CONFIG_8723AU_P2P */
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-		 ("issuing probe_req, tx_len =%d\n", pattrib->last_txcmdsz));
-
-	if (wait_ack) {
-		ret = dump_mgntframe23a_and_wait_ack23a(padapter, pmgntframe);
-	} else {
-		dump_mgntframe23a(padapter, pmgntframe);
-		ret = _SUCCESS;
-	}
-
-exit:
-	return ret;
-}
-
-inline void issue23a_probereq_p2p(struct rtw_adapter *adapter, u8 *da)
-{
-	_issue23a_probereq_p2p(adapter, da, false);
-}
-
-int issue23a_probereq_p2p_ex(struct rtw_adapter *adapter, u8 *da,
-			     int try_cnt, int wait_ms)
-{
-	int ret;
-	int i = 0;
-	unsigned long start = jiffies;
-
-	do {
-		ret = _issue23a_probereq_p2p(adapter, da,
-					     wait_ms > 0 ? true : false);
-
-		i++;
-
-		if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
-			break;
-
-		if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
-			msleep(wait_ms);
-
-	} while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
-
-	if (ret != _FAIL) {
-		ret = _SUCCESS;
-		goto exit;
-	}
-
-	if (try_cnt && wait_ms) {
-		if (da)
-			DBG_8723A(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d "
-				  "in %u ms\n", FUNC_ADPT_ARG(adapter),
-				  MAC_ARG(da), rtw_get_oper_ch23a(adapter),
-				  ret == _SUCCESS?", acked":"", i, try_cnt,
-				  jiffies_to_msecs(jiffies - start));
-		else
-			DBG_8723A(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
-				  FUNC_ADPT_ARG(adapter),
-				  rtw_get_oper_ch23a(adapter),
-				  ret == _SUCCESS?", acked":"", i, try_cnt,
-				  jiffies_to_msecs(jiffies - start));
-	}
-exit:
-	return ret;
-}
-
-#endif /* CONFIG_8723AU_P2P */
-
-static s32 rtw_action_public_decache(struct recv_frame *recv_frame, s32 token)
+static int rtw_action_public_decache(struct recv_frame *recv_frame, s32 token)
 {
 	struct rtw_adapter *adapter = recv_frame->adapter;
 	struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv;
@@ -4504,17 +2105,17 @@ static s32 rtw_action_public_decache(struct recv_frame *recv_frame, s32 token)
 		if (token >= 0) {
 			if ((seq_ctrl == mlmeext->action_public_rxseq) &&
 			    (token == mlmeext->action_public_dialog_token)) {
-				DBG_8723A(FUNC_ADPT_FMT" seq_ctrl = 0x%x, "
-					  "rxseq = 0x%x, token:%d\n",
-					  FUNC_ADPT_ARG(adapter), seq_ctrl,
+				DBG_8723A("%s(%s): seq_ctrl = 0x%x, "
+					  "rxseq = 0x%x, token:%d\n", __func__,
+					  adapter->pnetdev->name, seq_ctrl,
 					  mlmeext->action_public_rxseq, token);
 				return _FAIL;
 			}
 		} else {
 			if (seq_ctrl == mlmeext->action_public_rxseq) {
-				DBG_8723A(FUNC_ADPT_FMT" seq_ctrl = 0x%x, "
-					  "rxseq = 0x%x\n",
-					  FUNC_ADPT_ARG(adapter), seq_ctrl,
+				DBG_8723A("%s(%s): seq_ctrl = 0x%x, "
+					  "rxseq = 0x%x\n", __func__,
+					  adapter->pnetdev->name, seq_ctrl,
 					  mlmeext->action_public_rxseq);
 				return _FAIL;
 			}
@@ -4529,21 +2130,12 @@ static s32 rtw_action_public_decache(struct recv_frame *recv_frame, s32 token)
 	return _SUCCESS;
 }
 
-static unsigned int on_action_public23a_p2p(struct recv_frame *precv_frame)
+static int on_action_public23a_p2p(struct recv_frame *precv_frame)
 {
 	struct sk_buff *skb = precv_frame->pkt;
 	u8 *pframe = skb->data;
 	u8 *frame_body;
 	u8 dialogToken = 0;
-#ifdef CONFIG_8723AU_P2P
-	struct rtw_adapter *padapter = precv_frame->adapter;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	uint len = skb->len;
-	u8 *p2p_ie;
-	u32	p2p_ielen;
-	struct	wifidirect_info	*pwdinfo = &padapter->wdinfo;
-	u8	result = P2P_STATUS_SUCCESS;
-#endif /* CONFIG_8723AU_P2P */
 
 	frame_body = (unsigned char *)
 		(pframe + sizeof(struct ieee80211_hdr_3addr));
@@ -4553,349 +2145,10 @@ static unsigned int on_action_public23a_p2p(struct recv_frame *precv_frame)
 	if (rtw_action_public_decache(precv_frame, dialogToken) == _FAIL)
 		return _FAIL;
 
-#ifdef CONFIG_8723AU_P2P
-	del_timer_sync(&pwdinfo->reset_ch_sitesurvey);
-	if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) {
-		rtw_cfg80211_rx_p2p_action_public(padapter, pframe, len);
-	} else {
-		/*	Do nothing if the driver doesn't enable the P2P function. */
-		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
-			return _SUCCESS;
-
-		len -= sizeof(struct ieee80211_hdr_3addr);
-
-		switch (frame_body[ 6 ])/* OUI Subtype */
-		{
-			case P2P_GO_NEGO_REQ:
-				DBG_8723A("[%s] Got GO Nego Req Frame\n", __func__);
-				memset(&pwdinfo->groupid_info, 0x00, sizeof(struct group_id_info));
-
-				if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ))
-				{
-					rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
-				}
-
-				if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
-				{
-					/*	Commented by Albert 20110526 */
-					/*	In this case, this means the previous nego fail doesn't be reset yet. */
-					del_timer_sync(&pwdinfo->restore_p2p_state_timer);
-					/*	Restore the previous p2p state */
-					rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
-					DBG_8723A("[%s] Restore the previous p2p state to %d\n", __func__, rtw_p2p_state(pwdinfo));
-				}
-
-				/*	Commented by Kurt 20110902 */
-				/* Add if statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered. */
-				if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
-					rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
-
-				/*	Commented by Kurt 20120113 */
-				/*	Get peer_dev_addr here if peer doesn't issue prov_disc frame. */
-				if (is_zero_ether_addr(pwdinfo->rx_prov_disc_info.peerDevAddr))
-					ether_addr_copy(pwdinfo->rx_prov_disc_info.peerDevAddr, hdr->addr2);
-
-				result = process_p2p_group_negotation_req23a(pwdinfo, frame_body, len);
-				issue_p2p_GO_response(padapter, hdr->addr2,
-						      frame_body, len, result);
-
-				/*	Commented by Albert 20110718 */
-				/*	No matter negotiating or negotiation failure, the driver should set up the restore P2P state timer. */
-				mod_timer(&pwdinfo->restore_p2p_state_timer,
-					  jiffies + msecs_to_jiffies(5000));
-				break;
-
-			case P2P_GO_NEGO_RESP:
-				DBG_8723A("[%s] Got GO Nego Resp Frame\n", __func__);
-
-				if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
-				{
-					/*	Commented by Albert 20110425 */
-					/*	The restore timer is enabled when issuing the nego request frame of rtw_p2p_connect function. */
-					del_timer_sync(&pwdinfo->restore_p2p_state_timer);
-					pwdinfo->nego_req_info.benable = false;
-					result = process_p2p_group_negotation_resp23a(pwdinfo, frame_body, len);
-					issue_p2p_GO_confirm(pwdinfo->padapter,
-							     hdr->addr2,
-							     result);
-					if (result == P2P_STATUS_SUCCESS) {
-						if (rtw_p2p_role(pwdinfo) ==
-						    P2P_ROLE_CLIENT) {
-							pwdinfo->p2p_info.operation_ch[ 0 ] = pwdinfo->peer_operating_ch;
-							pwdinfo->p2p_info.scan_op_ch_only = 1;
-							mod_timer(&pwdinfo->reset_ch_sitesurvey2, jiffies + msecs_to_jiffies(P2P_RESET_SCAN_CH));
-						}
-					}
-
-					/*	Reset the dialog token for group negotiation frames. */
-					pwdinfo->negotiation_dialog_token = 1;
-
-					if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
-					{
-						mod_timer(&pwdinfo->restore_p2p_state_timer, jiffies + msecs_to_jiffies(5000));
-					}
-				} else {
-					DBG_8723A("[%s] Skipped GO Nego Resp Frame (p2p_state != P2P_STATE_GONEGO_ING)\n", __func__);
-				}
-
-				break;
-
-			case P2P_GO_NEGO_CONF:
-
-				DBG_8723A("[%s] Got GO Nego Confirm Frame\n", __func__);
-				result = process_p2p_group_negotation_confirm23a(pwdinfo, frame_body, len);
-				if (P2P_STATUS_SUCCESS == result)
-				{
-					if (rtw_p2p_role(pwdinfo) == P2P_ROLE_CLIENT)
-					{
-						pwdinfo->p2p_info.operation_ch[ 0 ] = pwdinfo->peer_operating_ch;
-						pwdinfo->p2p_info.scan_op_ch_only = 1;
-						mod_timer(&pwdinfo->reset_ch_sitesurvey2, jiffies + msecs_to_jiffies(P2P_RESET_SCAN_CH));
-					}
-				}
-				break;
-
-			case P2P_INVIT_REQ:
-				/*	Added by Albert 2010/10/05 */
-				/*	Received the P2P Invite Request frame. */
-
-				DBG_8723A("[%s] Got invite request frame!\n", __func__);
-				if ((p2p_ie = rtw_get_p2p_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen)))
-				{
-					/*	Parse the necessary information from the P2P Invitation Request frame. */
-					/*	For example: The MAC address of sending this P2P Invitation Request frame. */
-					u32	attr_contentlen = 0;
-					u8	status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
-					struct group_id_info group_id;
-					u8	invitation_flag = 0;
-
-					rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_INVITATION_FLAGS, &invitation_flag, &attr_contentlen);
-					if (attr_contentlen)
-					{
-
-						rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_BSSID, pwdinfo->p2p_peer_interface_addr, &attr_contentlen);
-						/*	Commented by Albert 20120510 */
-						/*	Copy to the pwdinfo->p2p_peer_interface_addr. */
-						/*	So that the WFD UI (or Sigma) can get the peer interface address by using the following command. */
-						/*	#> iwpriv wlan0 p2p_get peer_ifa */
-						/*	After having the peer interface address, the sigma can find the correct conf file for wpa_supplicant. */
-
-						if (attr_contentlen)
-						{
-							DBG_8723A("[%s] GO's BSSID = %.2X %.2X %.2X %.2X %.2X %.2X\n", __func__,
-									pwdinfo->p2p_peer_interface_addr[0], pwdinfo->p2p_peer_interface_addr[1],
-									pwdinfo->p2p_peer_interface_addr[2], pwdinfo->p2p_peer_interface_addr[3],
-									pwdinfo->p2p_peer_interface_addr[4], pwdinfo->p2p_peer_interface_addr[5]);
-						}
-
-						if (invitation_flag & P2P_INVITATION_FLAGS_PERSISTENT)
-						{
-							/*	Re-invoke the persistent group. */
-
-							memset(&group_id, 0x00, sizeof(struct group_id_info));
-							rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, (u8*) &group_id, &attr_contentlen);
-							if (attr_contentlen) {
-								if (ether_addr_equal(group_id.go_device_addr, myid(&padapter->eeprompriv))) {
-									/*	The p2p device sending this p2p invitation request wants this Wi-Fi device to be the persistent GO. */
-									rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_GO);
-									rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
-									status_code = P2P_STATUS_SUCCESS;
-								}
-								else
-								{
-									/*	The p2p device sending this p2p invitation request wants to be the persistent GO. */
-									if (is_matched_in_profilelist(pwdinfo->p2p_peer_interface_addr, &pwdinfo->profileinfo[ 0 ]))
-									{
-										u8 operatingch_info[5] = { 0x00 };
-										if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
-										{
-											if (rtw_ch_set_search_ch23a(padapter->mlmeextpriv.channel_set, (u32)operatingch_info[4]))
-											{
-												/*	The operating channel is acceptable for this device. */
-												pwdinfo->rx_invitereq_info.operation_ch[0]= operatingch_info[4];
-												pwdinfo->rx_invitereq_info.scan_op_ch_only = 1;
-												mod_timer(&pwdinfo->reset_ch_sitesurvey, jiffies + msecs_to_jiffies(P2P_RESET_SCAN_CH));
-												rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH);
-												rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-												status_code = P2P_STATUS_SUCCESS;
-												}
-											else
-											{
-												/*	The operating channel isn't supported by this device. */
-												rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH);
-												rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-												status_code = P2P_STATUS_FAIL_NO_COMMON_CH;
-												mod_timer(&pwdinfo->restore_p2p_state_timer, jiffies + msecs_to_jiffies(3000));
-											}
-										}
-										else {
-											/*	Commented by Albert 20121130 */
-											/*	Intel will use the different P2P IE to store the operating channel information */
-											/*	Workaround for Intel WiDi 3.5 */
-											rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH);
-											rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-											status_code = P2P_STATUS_SUCCESS;
-										}
-									}
-									else
-									{
-										rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH);
-
-										status_code = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP;
-									}
-								}
-							}
-							else
-							{
-								DBG_8723A("[%s] P2P Group ID Attribute NOT FOUND!\n", __func__);
-								status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
-							}
-						}
-						else
-						{
-							/*	Received the invitation to join a P2P group. */
-
-							memset(&group_id, 0x00, sizeof(struct group_id_info));
-							rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, (u8*) &group_id, &attr_contentlen);
-							if (attr_contentlen)
-							{
-								if (ether_addr_equal(group_id.go_device_addr, myid(&padapter->eeprompriv))) {
-									/*	In this case, the GO can't be myself. */
-									rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH);
-									status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
-								}
-								else
-								{
-									/*	The p2p device sending this p2p invitation request wants to join an existing P2P group */
-									/*	Commented by Albert 2012/06/28 */
-									/*	In this case, this Wi-Fi device should use the iwpriv command to get the peer device address. */
-									/*	The peer device address should be the destination address for the provisioning discovery request. */
-									/*	Then, this Wi-Fi device should use the iwpriv command to get the peer interface address. */
-									/*	The peer interface address should be the address for WPS mac address */
-									ether_addr_copy(pwdinfo->p2p_peer_device_addr, group_id.go_device_addr);
-									rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-									rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_JOIN);
-									status_code = P2P_STATUS_SUCCESS;
-								}
-							}
-							else
-							{
-								DBG_8723A("[%s] P2P Group ID Attribute NOT FOUND!\n", __func__);
-								status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
-							}
-						}
-					}
-					else
-					{
-						DBG_8723A("[%s] P2P Invitation Flags Attribute NOT FOUND!\n", __func__);
-						status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
-					}
-
-					DBG_8723A("[%s] status_code = %d\n", __func__, status_code);
-
-					pwdinfo->inviteresp_info.token = frame_body[ 7 ];
-					issue_p2p_invitation_response23a(padapter, hdr->addr2, pwdinfo->inviteresp_info.token, status_code);
-				}
-				break;
-
-			case P2P_INVIT_RESP:
-			{
-				u8	attr_content = 0x00;
-				u32	attr_contentlen = 0;
-
-				DBG_8723A("[%s] Got invite response frame!\n", __func__);
-				del_timer_sync(&pwdinfo->restore_p2p_state_timer);
-				if ((p2p_ie = rtw_get_p2p_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen)))
-				{
-					rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
-
-					if (attr_contentlen == 1)
-					{
-						DBG_8723A("[%s] Status = %d\n", __func__, attr_content);
-						pwdinfo->invitereq_info.benable = false;
-
-						if (attr_content == P2P_STATUS_SUCCESS)
-						{
-							if (ether_addr_equal(pwdinfo->invitereq_info.go_bssid, myid(&padapter->eeprompriv))) {
-								rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
-							}
-							else
-							{
-								rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-							}
-							rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_OK);
-						}
-						else
-						{
-							rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-							rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL);
-						}
-					}
-					else
-					{
-						rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-						rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL);
-					}
-				}
-				else
-				{
-					rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-					rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL);
-				}
-
-				if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL)) {
-					mod_timer(&pwdinfo->restore_p2p_state_timer, jiffies + msecs_to_jiffies(5000));
-				}
-				break;
-			}
-			case P2P_DEVDISC_REQ:
-
-				process_p2p_devdisc_req23a(pwdinfo, pframe, len);
-
-				break;
-
-			case P2P_DEVDISC_RESP:
-
-				process_p2p_devdisc_resp23a(pwdinfo, pframe, len);
-
-				break;
-
-			case P2P_PROVISION_DISC_REQ:
-				DBG_8723A("[%s] Got Provisioning Discovery Request Frame\n", __func__);
-				process_p2p_provdisc_req23a(pwdinfo, pframe, len);
-				ether_addr_copy(pwdinfo->rx_prov_disc_info.peerDevAddr, hdr->addr2);
-
-				/* 20110902 Kurt */
-				/* Add the following statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered. */
-				if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ))
-					rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
-
-				rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ);
-				mod_timer(&pwdinfo->restore_p2p_state_timer,
-					  jiffies + msecs_to_jiffies(P2P_PROVISION_TIMEOUT));
-				break;
-
-			case P2P_PROVISION_DISC_RESP:
-				/*	Commented by Albert 20110707 */
-				/*	Should we check the pwdinfo->tx_prov_disc_info.bsent flag here?? */
-				DBG_8723A("[%s] Got Provisioning Discovery Response Frame\n", __func__);
-				/*	Commented by Albert 20110426 */
-				/*	The restore timer is enabled when issuing the provisioing request frame in rtw_p2p_prov_disc function. */
-				del_timer_sync(&pwdinfo->restore_p2p_state_timer);
-				rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_RSP);
-				process_p2p_provdisc_resp23a(pwdinfo, pframe);
-				mod_timer(&pwdinfo->restore_p2p_state_timer,
-					  jiffies + msecs_to_jiffies(P2P_PROVISION_TIMEOUT));
-				break;
-
-		}
-	}
-#endif /* CONFIG_8723AU_P2P */
-
 	return _SUCCESS;
 }
 
-static unsigned int on_action_public23a_vendor(struct recv_frame *precv_frame)
+static int on_action_public23a_vendor(struct recv_frame *precv_frame)
 {
 	unsigned int ret = _FAIL;
 	struct sk_buff *skb = precv_frame->pkt;
@@ -4937,10 +2190,10 @@ exit:
 	return ret;
 }
 
-unsigned int on_action_public23a(struct rtw_adapter *padapter,
-				 struct recv_frame *precv_frame)
+static int on_action_public23a(struct rtw_adapter *padapter,
+			       struct recv_frame *precv_frame)
 {
-	unsigned int ret = _FAIL;
+	int ret = _FAIL;
 	struct sk_buff *skb = precv_frame->pkt;
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
 	u8 *pframe = skb->data;
@@ -4969,92 +2222,34 @@ exit:
 	return ret;
 }
 
-unsigned int OnAction23a_ht(struct rtw_adapter *padapter,
-			    struct recv_frame *precv_frame)
+static int
+OnAction23a_ht(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
 	return _SUCCESS;
 }
 
-unsigned int OnAction23a_wmm(struct rtw_adapter *padapter,
-			     struct recv_frame *precv_frame)
+static int
+OnAction23a_wmm(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
 	return _SUCCESS;
 }
 
-unsigned int OnAction23a_p2p(struct rtw_adapter *padapter,
-			     struct recv_frame *precv_frame)
+static int
+OnAction23a_p2p(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
-#ifdef CONFIG_8723AU_P2P
-	u8 *frame_body;
-	u8 category, OUI_Subtype, dialogToken = 0;
-	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u8 *pframe = skb->data;
-	uint len = skb->len;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-
-	DBG_8723A("%s\n", __func__);
-
-	/* check RA matches or not */
-	if (!ether_addr_equal(myid(&padapter->eeprompriv), hdr->addr1))
-		return _SUCCESS;
-
-	frame_body = (unsigned char *)
-		(pframe + sizeof(struct ieee80211_hdr_3addr));
-
-	category = frame_body[0];
-	if (category != WLAN_CATEGORY_VENDOR_SPECIFIC)
-		return _SUCCESS;
-
-	if (cpu_to_be32(*((u32*) (frame_body + 1))) != P2POUI)
-		return _SUCCESS;
-
-	if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) {
-		rtw_cfg80211_rx_action_p2p(padapter, pframe, len);
-		return _SUCCESS;
-	} else {
-		len -= sizeof(struct ieee80211_hdr_3addr);
-		OUI_Subtype = frame_body[5];
-		dialogToken = frame_body[6];
-
-		switch (OUI_Subtype)
-		{
-		case P2P_NOTICE_OF_ABSENCE:
-			break;
-
-		case P2P_PRESENCE_REQUEST:
-			process_p2p_presence_req23a(pwdinfo, pframe, len);
-			break;
-
-		case P2P_PRESENCE_RESPONSE:
-			break;
-
-		case P2P_GO_DISC_REQUEST:
-			break;
-
-		default:
-			break;
-		}
-	}
-#endif /* CONFIG_8723AU_P2P */
-
 	return _SUCCESS;
 }
 
-unsigned int OnAction23a(struct rtw_adapter *padapter,
-			 struct recv_frame *precv_frame)
+static int
+OnAction23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
 	int i;
-	unsigned char	category;
+	u8 category;
 	struct action_handler *ptable;
-	unsigned char	*frame_body;
 	struct sk_buff *skb = precv_frame->pkt;
-	u8 *pframe = skb->data;
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
 
-	frame_body = (unsigned char *)
-		(pframe + sizeof(struct ieee80211_hdr_3addr));
-
-	category = frame_body[0];
+	category = mgmt->u.action.category;
 
 	for (i = 0;
 	     i < sizeof(OnAction23a_tbl) / sizeof(struct action_handler); i++) {
@@ -5067,8 +2262,8 @@ unsigned int OnAction23a(struct rtw_adapter *padapter,
 	return _SUCCESS;
 }
 
-unsigned int DoReserved23a(struct rtw_adapter *padapter,
-			struct recv_frame *precv_frame)
+static int DoReserved23a(struct rtw_adapter *padapter,
+			 struct recv_frame *precv_frame)
 {
 	return _SUCCESS;
 }
@@ -5081,15 +2276,15 @@ struct xmit_frame *alloc_mgtxmitframe23a(struct xmit_priv *pxmitpriv)
 	pmgntframe = rtw_alloc_xmitframe23a_ext(pxmitpriv);
 
 	if (!pmgntframe) {
-		DBG_8723A(FUNC_ADPT_FMT" alloc xmitframe fail\n",
-			  FUNC_ADPT_ARG(pxmitpriv->adapter));
+		DBG_8723A("%s(%s): alloc xmitframe fail\n", __func__,
+			  pxmitpriv->adapter->pnetdev->name);
 		goto exit;
 	}
 
 	pxmitbuf = rtw_alloc_xmitbuf23a_ext(pxmitpriv);
 	if (!pxmitbuf) {
-		DBG_8723A(FUNC_ADPT_FMT" alloc xmitbuf fail\n",
-			  FUNC_ADPT_ARG(pxmitpriv->adapter));
+		DBG_8723A("%s(%s): alloc xmitbuf fail\n", __func__,
+			  pxmitpriv->adapter->pnetdev->name);
 		rtw_free_xmitframe23a(pxmitpriv, pmgntframe);
 		pmgntframe = NULL;
 		goto exit;
@@ -5138,7 +2333,7 @@ void update_mgntframe_attrib23a(struct rtw_adapter *padapter,
 	else
 		pattrib->raid = 5;/* a/g mode */
 
-	pattrib->encrypt = _NO_PRIVACY_;
+	pattrib->encrypt = 0;
 	pattrib->bswenc = false;
 
 	pattrib->qos_en = false;
@@ -5159,13 +2354,13 @@ void dump_mgntframe23a(struct rtw_adapter *padapter,
 	    padapter->bDriverStopped == true)
 		return;
 
-	rtw_hal_mgnt_xmit23a(padapter, pmgntframe);
+	rtl8723au_mgnt_xmit(padapter, pmgntframe);
 }
 
-s32 dump_mgntframe23a_and_wait(struct rtw_adapter *padapter,
+int dump_mgntframe23a_and_wait(struct rtw_adapter *padapter,
 			       struct xmit_frame *pmgntframe, int timeout_ms)
 {
-	s32 ret = _FAIL;
+	int ret = _FAIL;
 	unsigned long irqL;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct xmit_buf *pxmitbuf = pmgntframe->pxmitbuf;
@@ -5178,7 +2373,7 @@ s32 dump_mgntframe23a_and_wait(struct rtw_adapter *padapter,
 	rtw_sctx_init23a(&sctx, timeout_ms);
 	pxmitbuf->sctx = &sctx;
 
-	ret = rtw_hal_mgnt_xmit23a(padapter, pmgntframe);
+	ret = rtl8723au_mgnt_xmit(padapter, pmgntframe);
 
 	if (ret == _SUCCESS)
 		ret = rtw_sctx_wait23a(&sctx);
@@ -5190,24 +2385,23 @@ s32 dump_mgntframe23a_and_wait(struct rtw_adapter *padapter,
 	return ret;
 }
 
-s32 dump_mgntframe23a_and_wait_ack23a(struct rtw_adapter *padapter,
+int dump_mgntframe23a_and_wait_ack23a(struct rtw_adapter *padapter,
 				      struct xmit_frame *pmgntframe)
 {
-	s32 ret = _FAIL;
+	int ret = _FAIL;
 	u32 timeout_ms = 500;/*   500ms */
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 
 	if (padapter->bSurpriseRemoved == true ||
 	    padapter->bDriverStopped == true)
-		return -1;
+		return _FAIL;
 
 	mutex_lock(&pxmitpriv->ack_tx_mutex);
 	pxmitpriv->ack_tx = true;
 
 	pmgntframe->ack_report = 1;
-	if (rtw_hal_mgnt_xmit23a(padapter, pmgntframe) == _SUCCESS) {
+	if (rtl8723au_mgnt_xmit(padapter, pmgntframe) == _SUCCESS)
 		ret = rtw_ack_tx_wait23a(pxmitpriv, timeout_ms);
-	}
 
 	pxmitpriv->ack_tx = false;
 	mutex_unlock(&pxmitpriv->ack_tx_mutex);
@@ -5259,7 +2453,6 @@ void issue_beacon23a(struct rtw_adapter *padapter, int timeout_ms)
 	struct pkt_attrib *pattrib;
 	unsigned char *pframe;
 	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
 	unsigned int rate_len;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
@@ -5267,9 +2460,6 @@ void issue_beacon23a(struct rtw_adapter *padapter, int timeout_ms)
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
 	u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif /* CONFIG_8723AU_P2P */
 	u8 *wps_ie;
 	u32 wps_ielen;
 	u8 sr = 0;
@@ -5277,7 +2467,8 @@ void issue_beacon23a(struct rtw_adapter *padapter, int timeout_ms)
 
 	/* DBG_8723A("%s\n", __func__); */
 
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) {
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe) {
 		DBG_8723A("%s, alloc mgnt frame fail\n", __func__);
 		return;
 	}
@@ -5295,135 +2486,20 @@ void issue_beacon23a(struct rtw_adapter *padapter, int timeout_ms)
 	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					      IEEE80211_STYPE_BEACON);
+	pwlanhdr->seq_ctrl = 0;
 
 	ether_addr_copy(pwlanhdr->addr1, bc_addr);
 	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
 	ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(cur_network));
 
-	SetSeqNum(pwlanhdr, 0 /*pmlmeext->mgnt_seq*/);
-	/* pmlmeext->mgnt_seq++; */
-	SetFrameSubType(pframe, WIFI_BEACON);
-
 	pframe += sizeof(struct ieee80211_hdr_3addr);
 	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
 
 	if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) {
 		/* DBG_8723A("ie len =%d\n", cur_network->IELength); */
-#ifdef CONFIG_8723AU_P2P
-		/*  for P2P : Primary Device Type & Device Name */
-		u32 insert_len = 0;
-		wps_ie = rtw_get_wps_ie23a(cur_network->IEs + _FIXED_IE_LENGTH_,
-					   cur_network->IELength -
-					   _FIXED_IE_LENGTH_, NULL, &wps_ielen);
-
-		if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && wps_ie &&
-		    wps_ielen > 0) {
-			uint wps_offset, remainder_ielen;
-			u8 *premainder_ie, *pframe_wscie;
-
-			wps_offset = (uint)(wps_ie - cur_network->IEs);
-
-			premainder_ie = wps_ie + wps_ielen;
-
-			remainder_ielen = cur_network->IELength - wps_offset -
-				wps_ielen;
-
-			if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) {
-				if (pmlmepriv->wps_beacon_ie &&
-				    pmlmepriv->wps_beacon_ie_len>0) {
-					memcpy(pframe, cur_network->IEs,
-					       wps_offset);
-					pframe += wps_offset;
-					pattrib->pktlen += wps_offset;
-
-					memcpy(pframe, pmlmepriv->wps_beacon_ie,
-					       pmlmepriv->wps_beacon_ie_len);
-					pframe += pmlmepriv->wps_beacon_ie_len;
-					pattrib->pktlen +=
-						pmlmepriv->wps_beacon_ie_len;
-
-					/* copy remainder_ie to pframe */
-					memcpy(pframe, premainder_ie,
-					       remainder_ielen);
-					pframe += remainder_ielen;
-					pattrib->pktlen += remainder_ielen;
-				} else {
-					memcpy(pframe, cur_network->IEs,
-					       cur_network->IELength);
-					pframe += cur_network->IELength;
-					pattrib->pktlen +=
-						cur_network->IELength;
-				}
-			} else {
-				pframe_wscie = pframe + wps_offset;
-				memcpy(pframe, cur_network->IEs,
-				       wps_offset + wps_ielen);
-				pframe += (wps_offset + wps_ielen);
-				pattrib->pktlen += (wps_offset + wps_ielen);
-
-				/* now pframe is end of wsc ie, insert Primary
-				   Device Type & Device Name */
-				/*	Primary Device Type */
-				/*	Type: */
-				*(u16*) (pframe + insert_len) =
-					cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE);
-				insert_len += 2;
-
-				/*	Length: */
-				*(u16*) (pframe + insert_len) =
-					cpu_to_be16(0x0008);
-				insert_len += 2;
-
-				/*	Value: */
-				/*	Category ID */
-				*(u16*) (pframe + insert_len) =
-					cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
-				insert_len += 2;
-
-				/*	OUI */
-				*(u32*) (pframe + insert_len) =
-					cpu_to_be32(WPSOUI);
-				insert_len += 4;
-
-				/*	Sub Category ID */
-				*(u16*) (pframe + insert_len) =
-					cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
-				insert_len += 2;
-
-				/*	Device Name */
-				/*	Type: */
-				*(u16*) (pframe + insert_len) =
-					cpu_to_be16(WPS_ATTR_DEVICE_NAME);
-				insert_len += 2;
-
-				/*	Length: */
-				*(u16*) (pframe + insert_len) =
-					cpu_to_be16(pwdinfo->device_name_len);
-				insert_len += 2;
-
-				/*	Value: */
-				memcpy(pframe + insert_len,
-				       pwdinfo->device_name,
-				       pwdinfo->device_name_len);
-				insert_len += pwdinfo->device_name_len;
-
-				/* update wsc ie length */
-				*(pframe_wscie+1) = (wps_ielen -2) + insert_len;
-
-				/* pframe move to end */
-				pframe+= insert_len;
-				pattrib->pktlen += insert_len;
-
-				/* copy remainder_ie to pframe */
-				memcpy(pframe, premainder_ie, remainder_ielen);
-				pframe += remainder_ielen;
-				pattrib->pktlen += remainder_ielen;
-			}
-		} else
-#endif /* CONFIG_8723AU_P2P */
-			memcpy(pframe, cur_network->IEs, cur_network->IELength);
+		memcpy(pframe, cur_network->IEs, cur_network->IELength);
 		len_diff = update_hidden_ssid(pframe + _BEACON_IE_OFFSET_,
 					      cur_network->IELength -
 					      _BEACON_IE_OFFSET_,
@@ -5447,36 +2523,6 @@ void issue_beacon23a(struct rtw_adapter *padapter, int timeout_ms)
 		else
 			_clr_fwstate_(pmlmepriv, WIFI_UNDER_WPS);
 
-#ifdef CONFIG_8723AU_P2P
-		if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
-			u32 len;
-			if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) {
-				len = pmlmepriv->p2p_beacon_ie_len;
-				if (pmlmepriv->p2p_beacon_ie && len > 0)
-					memcpy(pframe,
-					       pmlmepriv->p2p_beacon_ie, len);
-			} else
-				len = build_beacon_p2p_ie23a(pwdinfo, pframe);
-
-			pframe += len;
-			pattrib->pktlen += len;
-
-			if (true == pwdinfo->wfd_info->wfd_enable) {
-				len = build_beacon_wfd_ie(pwdinfo, pframe);
-			} else {
-				len = 0;
-				if (pmlmepriv->wfd_beacon_ie &&
-				    pmlmepriv->wfd_beacon_ie_len>0) {
-					len = pmlmepriv->wfd_beacon_ie_len;
-					memcpy(pframe,
-					       pmlmepriv->wfd_beacon_ie, len);
-				}
-			}
-			pframe += len;
-			pattrib->pktlen += len;
-		}
-#endif /* CONFIG_8723AU_P2P */
-
 		goto _issue_bcn;
 	}
 
@@ -5503,39 +2549,39 @@ void issue_beacon23a(struct rtw_adapter *padapter, int timeout_ms)
 	pattrib->pktlen += 2;
 
 	/*  SSID */
-	pframe = rtw_set_ie23a(pframe, _SSID_IE_, cur_network->Ssid.ssid_len,
+	pframe = rtw_set_ie23a(pframe, WLAN_EID_SSID,
+			       cur_network->Ssid.ssid_len,
 			       cur_network->Ssid.ssid, &pattrib->pktlen);
 
 	/*  supported rates... */
 	rate_len = rtw_get_rateset_len23a(cur_network->SupportedRates);
-	pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_,
+	pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES,
 			       ((rate_len > 8)? 8: rate_len),
 			       cur_network->SupportedRates, &pattrib->pktlen);
 
 	/*  DS parameter set */
-	pframe = rtw_set_ie23a(pframe, _DSSET_IE_, 1, (unsigned char *)
-			       &cur_network->Configuration.DSConfig,
-			       &pattrib->pktlen);
+	pframe = rtw_set_ie23a(pframe, WLAN_EID_DS_PARAMS, 1, (unsigned char *)
+			       &cur_network->DSConfig, &pattrib->pktlen);
 
 	/* if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) */
 	{
 		u8 erpinfo = 0;
 		u32 ATIMWindow;
 		/*  IBSS Parameter Set... */
-		/* ATIMWindow = cur->Configuration.ATIMWindow; */
+		/* ATIMWindow = cur->ATIMWindow; */
 		ATIMWindow = 0;
-		pframe = rtw_set_ie23a(pframe, _IBSS_PARA_IE_, 2,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_IBSS_PARAMS, 2,
 				       (unsigned char *)&ATIMWindow,
 				       &pattrib->pktlen);
 
 		/* ERP IE */
-		pframe = rtw_set_ie23a(pframe, _ERPINFO_IE_, 1,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_ERP_INFO, 1,
 				       &erpinfo, &pattrib->pktlen);
 	}
 
 	/*  EXTERNDED SUPPORTED RATE */
 	if (rate_len > 8)
-		pframe = rtw_set_ie23a(pframe, _EXT_SUPPORTEDRATES_IE_,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_EXT_SUPP_RATES,
 				       rate_len - 8,
 				       cur_network->SupportedRates + 8,
 				       &pattrib->pktlen);
@@ -5564,14 +2610,13 @@ _issue_bcn:
 		dump_mgntframe23a(padapter, pmgntframe);
 }
 
-void issue_probersp23a(struct rtw_adapter *padapter, unsigned char *da,
-		       u8 is_valid_p2p_probereq)
+static void issue_probersp(struct rtw_adapter *padapter, unsigned char *da,
+			   u8 is_valid_p2p_probereq)
 {
 	struct xmit_frame *pmgntframe;
 	struct pkt_attrib *pattrib;
 	unsigned char *pframe;
 	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
 	unsigned char *mac, *bssid;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 #ifdef CONFIG_8723AU_AP_MODE
@@ -5582,22 +2627,17 @@ void issue_probersp23a(struct rtw_adapter *padapter, unsigned char *da,
 	int ssid_ielen_diff;
 	u8 buf[MAX_IE_SZ];
 	u8 *ies;
-#endif
-#if defined(CONFIG_8723AU_AP_MODE) || defined(CONFIG_8723AU_P2P)
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 #endif
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
 	unsigned int rate_len;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif /* CONFIG_8723AU_P2P */
 
 	/* DBG_8723A("%s\n", __func__); */
 
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-	{
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe) {
 		DBG_8723A("%s, alloc mgnt frame fail\n", __func__);
 		return;
 	}
@@ -5608,21 +2648,22 @@ void issue_probersp23a(struct rtw_adapter *padapter, unsigned char *da,
 
 	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
 
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pframe = (u8 *)pmgntframe->buf_addr + TXDESC_OFFSET;
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
 	mac = myid(&padapter->eeprompriv);
 	bssid = cur_network->MacAddress;
 
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					      IEEE80211_STYPE_PROBE_RESP);
+
 	ether_addr_copy(pwlanhdr->addr1, da);
 	ether_addr_copy(pwlanhdr->addr2, mac);
 	ether_addr_copy(pwlanhdr->addr3, bssid);
 
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
 	pmlmeext->mgnt_seq++;
-	SetFrameSubType(fctrl, WIFI_PROBERSP);
 
 	pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
 	pattrib->pktlen = pattrib->hdrlen;
@@ -5640,8 +2681,7 @@ void issue_probersp23a(struct rtw_adapter *padapter, unsigned char *da,
 					    &wps_ielen);
 
 		/* inerset & update wps_probe_resp_ie */
-		if ((pmlmepriv->wps_probe_resp_ie != NULL) && pwps_ie &&
-		    (wps_ielen > 0)) {
+		if (pmlmepriv->wps_probe_resp_ie && pwps_ie && wps_ielen > 0) {
 			uint wps_offset, remainder_ielen;
 			u8 *premainder_ie;
 
@@ -5658,14 +2698,14 @@ void issue_probersp23a(struct rtw_adapter *padapter, unsigned char *da,
 
 			/* to get ie data len */
 			wps_ielen = (uint)pmlmepriv->wps_probe_resp_ie[1];
-			if ((wps_offset+wps_ielen+2)<= MAX_IE_SZ) {
+			if (wps_offset + wps_ielen + 2 <= MAX_IE_SZ) {
 				memcpy(pframe, pmlmepriv->wps_probe_resp_ie,
 				       wps_ielen+2);
 				pframe += wps_ielen+2;
 				pattrib->pktlen += wps_ielen+2;
 			}
 
-			if ((wps_offset+wps_ielen+2+remainder_ielen) <=
+			if (wps_offset + wps_ielen + 2 + remainder_ielen <=
 			    MAX_IE_SZ) {
 				memcpy(pframe, premainder_ie, remainder_ielen);
 				pframe += remainder_ielen;
@@ -5681,9 +2721,9 @@ void issue_probersp23a(struct rtw_adapter *padapter, unsigned char *da,
 		ies = pmgntframe->buf_addr + TXDESC_OFFSET +
 			sizeof(struct ieee80211_hdr_3addr);
 
-		ssid_ie = rtw_get_ie23a(ies+_FIXED_IE_LENGTH_, _SSID_IE_,
+		ssid_ie = rtw_get_ie23a(ies + _FIXED_IE_LENGTH_, WLAN_EID_SSID,
 					&ssid_ielen,
-					(pframe-ies)-_FIXED_IE_LENGTH_);
+					pframe - ies - _FIXED_IE_LENGTH_);
 
 		ssid_ielen_diff = cur_network->Ssid.ssid_len - ssid_ielen;
 
@@ -5691,20 +2731,19 @@ void issue_probersp23a(struct rtw_adapter *padapter, unsigned char *da,
 			uint remainder_ielen;
 			u8 *remainder_ie;
 			remainder_ie = ssid_ie + 2;
-			remainder_ielen = (pframe-remainder_ie);
+			remainder_ielen = pframe - remainder_ie;
 
-			DBG_8723A_LEVEL(_drv_warning_, FUNC_ADPT_FMT
-					" remainder_ielen > MAX_IE_SZ\n",
-					FUNC_ADPT_ARG(padapter));
-			if (remainder_ielen > MAX_IE_SZ) {
+			DBG_8723A_LEVEL(_drv_warning_, "%s(%s): "
+					"remainder_ielen > MAX_IE_SZ\n",
+					__func__, padapter->pnetdev->name);
+			if (remainder_ielen > MAX_IE_SZ)
 				remainder_ielen = MAX_IE_SZ;
-			}
 
 			memcpy(buf, remainder_ie, remainder_ielen);
-			memcpy(remainder_ie+ssid_ielen_diff, buf,
+			memcpy(remainder_ie + ssid_ielen_diff, buf,
 			       remainder_ielen);
-			*(ssid_ie+1) = cur_network->Ssid.ssid_len;
-			memcpy(ssid_ie+2, cur_network->Ssid.ssid,
+			*(ssid_ie + 1) = cur_network->Ssid.ssid_len;
+			memcpy(ssid_ie + 2, cur_network->Ssid.ssid,
 			       cur_network->Ssid.ssid_len);
 
 			pframe += ssid_ielen_diff;
@@ -5737,40 +2776,41 @@ void issue_probersp23a(struct rtw_adapter *padapter, unsigned char *da,
 		/* below for ad-hoc mode */
 
 		/*  SSID */
-		pframe = rtw_set_ie23a(pframe, _SSID_IE_,
-				    cur_network->Ssid.ssid_len,
-				    cur_network->Ssid.ssid, &pattrib->pktlen);
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_SSID,
+				       cur_network->Ssid.ssid_len,
+				       cur_network->Ssid.ssid,
+				       &pattrib->pktlen);
 
 		/*  supported rates... */
 		rate_len = rtw_get_rateset_len23a(cur_network->SupportedRates);
-		pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES,
 				       ((rate_len > 8)? 8: rate_len),
 				       cur_network->SupportedRates,
 				       &pattrib->pktlen);
 
 		/*  DS parameter set */
-		pframe = rtw_set_ie23a(pframe, _DSSET_IE_, 1, (unsigned char *)
-				       &cur_network->Configuration.DSConfig,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_DS_PARAMS, 1,
+				       (unsigned char *)&cur_network->DSConfig,
 				       &pattrib->pktlen);
 
-		if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
+		if ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) {
 			u8 erpinfo = 0;
 			u32 ATIMWindow;
 			/*  IBSS Parameter Set... */
-			/* ATIMWindow = cur->Configuration.ATIMWindow; */
+			/* ATIMWindow = cur->ATIMWindow; */
 			ATIMWindow = 0;
-			pframe = rtw_set_ie23a(pframe, _IBSS_PARA_IE_, 2,
+			pframe = rtw_set_ie23a(pframe, WLAN_EID_IBSS_PARAMS, 2,
 					       (unsigned char *)&ATIMWindow,
 					       &pattrib->pktlen);
 
 			/* ERP IE */
-			pframe = rtw_set_ie23a(pframe, _ERPINFO_IE_, 1,
+			pframe = rtw_set_ie23a(pframe, WLAN_EID_ERP_INFO, 1,
 					       &erpinfo, &pattrib->pktlen);
 		}
 
 		/*  EXTERNDED SUPPORTED RATE */
 		if (rate_len > 8)
-			pframe = rtw_set_ie23a(pframe, _EXT_SUPPORTEDRATES_IE_,
+			pframe = rtw_set_ie23a(pframe, WLAN_EID_EXT_SUPP_RATES,
 					       rate_len - 8,
 					       cur_network->SupportedRates + 8,
 					       &pattrib->pktlen);
@@ -5778,38 +2818,6 @@ void issue_probersp23a(struct rtw_adapter *padapter, unsigned char *da,
 		/* todo:HT for adhoc */
 	}
 
-#ifdef CONFIG_8723AU_P2P
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && is_valid_p2p_probereq) {
-		u32 len;
-		if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) {
-			/* if pwdinfo->role == P2P_ROLE_DEVICE will call
-			   issue_probersp23a_p2p23a() */
-			len = pmlmepriv->p2p_go_probe_resp_ie_len;
-			if (pmlmepriv->p2p_go_probe_resp_ie && len>0)
-				memcpy(pframe, pmlmepriv->p2p_go_probe_resp_ie,
-				       len);
-		} else
-			len = build_probe_resp_p2p_ie23a(pwdinfo, pframe);
-
-		pframe += len;
-		pattrib->pktlen += len;
-
-		if (true == pwdinfo->wfd_info->wfd_enable) {
-			len = build_probe_resp_wfd_ie(pwdinfo, pframe, 0);
-		} else {
-			len = 0;
-			if (pmlmepriv->wfd_probe_resp_ie &&
-			    pmlmepriv->wfd_probe_resp_ie_len > 0) {
-				len = pmlmepriv->wfd_probe_resp_ie_len;
-				memcpy(pframe, pmlmepriv->wfd_probe_resp_ie,
-				       len);
-			}
-		}
-		pframe += len;
-		pattrib->pktlen += len;
-	}
-#endif /* CONFIG_8723AU_P2P */
-
 	pattrib->last_txcmdsz = pattrib->pktlen;
 
 	dump_mgntframe23a(padapter, pmgntframe);
@@ -5817,15 +2825,14 @@ void issue_probersp23a(struct rtw_adapter *padapter, unsigned char *da,
 	return;
 }
 
-static int _issue_probereq23a(struct rtw_adapter *padapter,
-			      struct cfg80211_ssid *pssid, u8 *da, int wait_ack)
+static int _issue_probereq(struct rtw_adapter *padapter,
+			   struct cfg80211_ssid *pssid, u8 *da, int wait_ack)
 {
 	int ret = _FAIL;
 	struct xmit_frame		*pmgntframe;
 	struct pkt_attrib		*pattrib;
 	unsigned char			*pframe;
 	struct ieee80211_hdr	*pwlanhdr;
-	unsigned short		*fctrl;
 	unsigned char			*mac;
 	unsigned char			bssrate[NumRates];
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
@@ -5835,9 +2842,10 @@ static int _issue_probereq23a(struct rtw_adapter *padapter,
 	u8	bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
-		 ("+issue_probereq23a\n"));
+		 ("+%s\n", __func__));
 
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe)
 		goto exit;
 
 	/* update attribute */
@@ -5851,8 +2859,8 @@ static int _issue_probereq23a(struct rtw_adapter *padapter,
 
 	mac = myid(&padapter->eeprompriv);
 
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					      IEEE80211_STYPE_PROBE_REQ);
 
 	if (da) {
 		/*	unicast probe request frame */
@@ -5866,30 +2874,31 @@ static int _issue_probereq23a(struct rtw_adapter *padapter,
 
 	ether_addr_copy(pwlanhdr->addr2, mac);
 
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
+
 	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_PROBEREQ);
 
 	pframe += sizeof (struct ieee80211_hdr_3addr);
 	pattrib->pktlen = sizeof (struct ieee80211_hdr_3addr);
 
 	if (pssid)
-		pframe = rtw_set_ie23a(pframe, _SSID_IE_, pssid->ssid_len,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_SSID, pssid->ssid_len,
 				       pssid->ssid, &pattrib->pktlen);
 	else
-		pframe = rtw_set_ie23a(pframe, _SSID_IE_, 0, NULL,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_SSID, 0, NULL,
 				       &pattrib->pktlen);
 
 	get_rate_set23a(padapter, bssrate, &bssrate_len);
 
 	if (bssrate_len > 8) {
-		pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_, 8,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES, 8,
 				       bssrate, &pattrib->pktlen);
-		pframe = rtw_set_ie23a(pframe, _EXT_SUPPORTEDRATES_IE_,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_EXT_SUPP_RATES,
 				       (bssrate_len - 8), (bssrate + 8),
 				       &pattrib->pktlen);
 	} else {
-		pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES,
 				       bssrate_len, bssrate, &pattrib->pktlen);
 	}
 
@@ -5917,23 +2926,23 @@ exit:
 	return ret;
 }
 
-inline void issue_probereq23a(struct rtw_adapter *padapter,
-			      struct cfg80211_ssid *pssid, u8 *da)
+static inline void issue_probereq(struct rtw_adapter *padapter,
+				  struct cfg80211_ssid *pssid, u8 *da)
 {
-	_issue_probereq23a(padapter, pssid, da, false);
+	_issue_probereq(padapter, pssid, da, false);
 }
 
-int issue_probereq23a_ex23a(struct rtw_adapter *padapter,
-		      struct cfg80211_ssid *pssid, u8 *da,
-		      int try_cnt, int wait_ms)
+static int issue_probereq_ex(struct rtw_adapter *padapter,
+			     struct cfg80211_ssid *pssid, u8 *da,
+			     int try_cnt, int wait_ms)
 {
 	int ret;
 	int i = 0;
 	unsigned long start = jiffies;
 
 	do {
-		ret = _issue_probereq23a(padapter, pssid, da,
-					 wait_ms > 0 ? true : false);
+		ret = _issue_probereq(padapter, pssid, da,
+				      wait_ms > 0 ? true : false);
 
 		i++;
 
@@ -5952,14 +2961,15 @@ int issue_probereq23a_ex23a(struct rtw_adapter *padapter,
 
 	if (try_cnt && wait_ms) {
 		if (da)
-			DBG_8723A(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d "
-				  "in %u ms\n",	FUNC_ADPT_ARG(padapter),
+			DBG_8723A("%s(%s): to "MAC_FMT", ch:%u%s, %d/%d "
+				  "in %u ms\n",	__func__,
+				  padapter->pnetdev->name,
 				  MAC_ARG(da), rtw_get_oper_ch23a(padapter),
 				  ret == _SUCCESS?", acked":"", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
 		else
-			DBG_8723A(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
-				  FUNC_ADPT_ARG(padapter),
+			DBG_8723A("%s(%s):, ch:%u%s, %d/%d in %u ms\n",
+				  __func__, padapter->pnetdev->name,
 				  rtw_get_oper_ch23a(padapter),
 				  ret == _SUCCESS?", acked":"", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
@@ -5969,14 +2979,13 @@ exit:
 }
 
 /*  if psta == NULL, indiate we are station(client) now... */
-void issue_auth23a(struct rtw_adapter *padapter, struct sta_info *psta,
-		   unsigned short status)
+static void issue_auth(struct rtw_adapter *padapter, struct sta_info *psta,
+		       unsigned short status)
 {
 	struct xmit_frame *pmgntframe;
 	struct pkt_attrib *pattrib;
 	unsigned char *pframe;
 	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
 	unsigned int val32;
 	unsigned short val16;
 	int use_shared_key = 0;
@@ -5984,7 +2993,8 @@ void issue_auth23a(struct rtw_adapter *padapter, struct sta_info *psta,
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe)
 		return;
 
 	/* update attribute */
@@ -5996,12 +3006,11 @@ void issue_auth23a(struct rtw_adapter *padapter, struct sta_info *psta,
 	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					      IEEE80211_STYPE_AUTH);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
 	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_AUTH);
 
 	pframe += sizeof(struct ieee80211_hdr_3addr);
 	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
@@ -6045,7 +3054,7 @@ void issue_auth23a(struct rtw_adapter *padapter, struct sta_info *psta,
 		/*  added challenging text... */
 		if ((psta->auth_seq == 2) &&
 		    (psta->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1))
-			pframe = rtw_set_ie23a(pframe, _CHLGETXT_IE_, 128,
+			pframe = rtw_set_ie23a(pframe, WLAN_EID_CHALLENGE, 128,
 					       psta->chg_txt, &pattrib->pktlen);
 #endif
 	} else {
@@ -6079,7 +3088,7 @@ void issue_auth23a(struct rtw_adapter *padapter, struct sta_info *psta,
 						     (unsigned char *)&val32,
 						     &pattrib->pktlen);
 
-			pattrib->iv_len = 4;
+			pattrib->iv_len = IEEE80211_WEP_IV_LEN;
 		}
 
 		pframe = rtw_set_fixed_ie23a(pframe, _AUTH_ALGM_NUM_,
@@ -6104,17 +3113,18 @@ void issue_auth23a(struct rtw_adapter *padapter, struct sta_info *psta,
 		if ((pmlmeinfo->auth_seq == 3) &&
 		    (pmlmeinfo->state & WIFI_FW_AUTH_STATE) &&
 		    (use_shared_key == 1)) {
-			pframe = rtw_set_ie23a(pframe, _CHLGETXT_IE_, 128,
+			pframe = rtw_set_ie23a(pframe, WLAN_EID_CHALLENGE, 128,
 					       pmlmeinfo->chg_txt,
 					       &pattrib->pktlen);
 
-			SetPrivacy(fctrl);
+			pwlanhdr->frame_control |=
+				cpu_to_le16(IEEE80211_FCTL_PROTECTED);
 
 			pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
 
-			pattrib->encrypt = _WEP40_;
+			pattrib->encrypt = WLAN_CIPHER_SUITE_WEP40;
 
-			pattrib->icv_len = 4;
+			pattrib->icv_len = IEEE80211_WEP_ICV_LEN;
 
 			pattrib->pktlen += pattrib->icv_len;
 		}
@@ -6129,29 +3139,27 @@ void issue_auth23a(struct rtw_adapter *padapter, struct sta_info *psta,
 	return;
 }
 
-void issue_asocrsp23a(struct rtw_adapter *padapter, unsigned short status,
-		      struct sta_info *pstat, int pkt_type)
-{
 #ifdef CONFIG_8723AU_AP_MODE
+static void issue_assocrsp(struct rtw_adapter *padapter, unsigned short status,
+			   struct sta_info *pstat, u16 pkt_type)
+{
 	struct xmit_frame *pmgntframe;
 	struct ieee80211_hdr *pwlanhdr;
 	struct pkt_attrib *pattrib;
-	unsigned char *pbuf, *pframe;
+	unsigned char *pframe;
 	unsigned short val;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
+	const u8 *p;
 	u8 *ie = pnetwork->IEs;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	u32 wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
 
 	DBG_8723A("%s\n", __func__);
 
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe)
 		return;
 
 	/* update attribute */
@@ -6163,18 +3171,16 @@ void issue_asocrsp23a(struct rtw_adapter *padapter, unsigned short status,
 	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
-	pwlanhdr->frame_control = 0;
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT | pkt_type);
 
 	ether_addr_copy(pwlanhdr->addr1, pstat->hwaddr);
 	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
 	ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network));
 
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
+
 	pmlmeext->mgnt_seq++;
-	if ((pkt_type == WIFI_ASSOCRSP) || (pkt_type == WIFI_REASSOCRSP))
-		SetFrameSubType(pwlanhdr, pkt_type);
-	else
-		return;
 
 	pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
 	pattrib->pktlen += pattrib->hdrlen;
@@ -6196,70 +3202,71 @@ void issue_asocrsp23a(struct rtw_adapter *padapter, unsigned short status,
 				     &pattrib->pktlen);
 
 	if (pstat->bssratelen <= 8) {
-		pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES,
 				       pstat->bssratelen, pstat->bssrateset,
 				       &pattrib->pktlen);
 	} else {
-		pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_, 8,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES, 8,
 				       pstat->bssrateset, &pattrib->pktlen);
-		pframe = rtw_set_ie23a(pframe, _EXT_SUPPORTEDRATES_IE_,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_EXT_SUPP_RATES,
 				       pstat->bssratelen - 8,
 				       pstat->bssrateset + 8, &pattrib->pktlen);
 	}
 
-	if ((pstat->flags & WLAN_STA_HT) && (pmlmepriv->htpriv.ht_option)) {
-		uint ie_len = 0;
-
+	if (pstat->flags & WLAN_STA_HT && pmlmepriv->htpriv.ht_option) {
 		/* FILL HT CAP INFO IE */
 		/* p = hostapd_eid_ht_capabilities_info(hapd, p); */
-		pbuf = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_,
-				     _HT_CAPABILITY_IE_, &ie_len,
-				     pnetwork->IELength - _BEACON_IE_OFFSET_);
-		if (pbuf && ie_len>0) {
-			memcpy(pframe, pbuf, ie_len + 2);
-			pframe += (ie_len + 2);
-			pattrib->pktlen += (ie_len + 2);
+		p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY,
+				     ie + _BEACON_IE_OFFSET_,
+				     pnetwork->IELength -_BEACON_IE_OFFSET_);
+		if (p && p[1]) {
+			memcpy(pframe, p, p[1] + 2);
+			pframe += (p[1] + 2);
+			pattrib->pktlen += (p[1] + 2);
 		}
 
 		/* FILL HT ADD INFO IE */
 		/* p = hostapd_eid_ht_operation(hapd, p); */
-		pbuf = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, _HT_ADD_INFO_IE_,
-				     &ie_len,
+		p = cfg80211_find_ie(WLAN_EID_HT_OPERATION,
+				     ie + _BEACON_IE_OFFSET_,
 				     pnetwork->IELength - _BEACON_IE_OFFSET_);
-		if (pbuf && ie_len > 0) {
-			memcpy(pframe, pbuf, ie_len + 2);
-			pframe += (ie_len + 2);
-			pattrib->pktlen += (ie_len + 2);
+		if (p && p[1] > 0) {
+			memcpy(pframe, p, p[1] + 2);
+			pframe += (p[1] + 2);
+			pattrib->pktlen += (p[1] + 2);
 		}
 	}
 
 	/* FILL WMM IE */
-	if ((pstat->flags & WLAN_STA_WME) && pmlmepriv->qospriv.qos_option) {
-		uint ie_len = 0;
+	if (pstat->flags & WLAN_STA_WME && pmlmepriv->qos_option) {
 		unsigned char WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02,
 					       0x01, 0x01};
-
-		for (pbuf = ie + _BEACON_IE_OFFSET_; ; pbuf += (ie_len + 2)) {
-			pbuf = rtw_get_ie23a(pbuf, _VENDOR_SPECIFIC_IE_,
-					     &ie_len, (pnetwork->IELength -
-						       _BEACON_IE_OFFSET_ -
-						       (ie_len + 2)));
-			if (pbuf && !memcmp(pbuf + 2, WMM_PARA_IE, 6)) {
-				memcpy(pframe, pbuf, ie_len + 2);
+		int ie_len = 0;
+
+		for (p = ie + _BEACON_IE_OFFSET_; ; p += (ie_len + 2)) {
+			p = cfg80211_find_ie(WLAN_EID_VENDOR_SPECIFIC, p,
+					     pnetwork->IELength -
+					     _BEACON_IE_OFFSET_ - (ie_len + 2));
+			if (p)
+				ie_len = p[1];
+			else
+				ie_len = 0;
+			if (p && !memcmp(p + 2, WMM_PARA_IE, 6)) {
+				memcpy(pframe, p, ie_len + 2);
 				pframe += (ie_len + 2);
 				pattrib->pktlen += (ie_len + 2);
 
 				break;
 			}
 
-			if ((!pbuf) || (ie_len == 0))
+			if (!p || ie_len == 0)
 				break;
 		}
 	}
 
 	if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK) {
-		pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, 6,
-				       REALTEK_96B_IE23A, &pattrib->pktlen);
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_VENDOR_SPECIFIC, 6,
+				       REALTEK_96B_IE, &pattrib->pktlen);
 	}
 
 	/* add WPS IE ie for wps 2.0 */
@@ -6272,47 +3279,32 @@ void issue_asocrsp23a(struct rtw_adapter *padapter, unsigned short status,
 		pattrib->pktlen += pmlmepriv->wps_assoc_resp_ie_len;
 	}
 
-#ifdef CONFIG_8723AU_P2P
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) &&
-	    pwdinfo->wfd_info->wfd_enable) {
-		wfdielen = build_assoc_resp_wfd_ie(pwdinfo, pframe);
-		pframe += wfdielen;
-		pattrib->pktlen += wfdielen;
-	}
-#endif /* CONFIG_8723AU_P2P */
-
 	pattrib->last_txcmdsz = pattrib->pktlen;
 
 	dump_mgntframe23a(padapter, pmgntframe);
-#endif
 }
+#endif
 
-void issue_assocreq23a(struct rtw_adapter *padapter)
+static void issue_assocreq(struct rtw_adapter *padapter)
 {
 	int ret = _FAIL;
 	struct xmit_frame *pmgntframe;
 	struct pkt_attrib *pattrib;
-	unsigned char *pframe, *p;
+	unsigned char *pframe;
+	const u8 *p;
 	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
-	unsigned short val16;
-	unsigned int i, j, ie_len, index = 0;
+	unsigned int i, j, index = 0;
 	unsigned char rf_type, bssrate[NumRates], sta_bssrate[NumRates];
-	struct ndis_802_11_var_ies *pIE;
 	struct registry_priv *pregpriv = &padapter->registrypriv;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	int bssrate_len = 0, sta_bssrate_len = 0;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	u8 p2pie[255] = { 0x00 };
-	u16 p2pielen = 0;
-	u32 wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
+	int bssrate_len = 0, sta_bssrate_len = 0, pie_len, bcn_fixed_size;
+	u8 *pie;
+
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe)
 		goto exit;
 
 	/* update attribute */
@@ -6321,38 +3313,38 @@ void issue_assocreq23a(struct rtw_adapter *padapter)
 
 	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
 
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pframe = (u8 *)pmgntframe->buf_addr + TXDESC_OFFSET;
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					      IEEE80211_STYPE_ASSOC_REQ);
+
 	ether_addr_copy(pwlanhdr->addr1, get_my_bssid23a(&pmlmeinfo->network));
 	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
 	ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network));
 
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
 	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ASSOCREQ);
 
 	pframe += sizeof(struct ieee80211_hdr_3addr);
 	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
 
 	/* caps */
-	memcpy(pframe, rtw_get_capability23a_from_ie(pmlmeinfo->network.IEs),
-	       2);
+	memcpy(pframe,
+	       rtw_get_capability23a_from_ie(pmlmeinfo->network.IEs), 2);
 
 	pframe += 2;
 	pattrib->pktlen += 2;
 
 	/* listen interval */
 	/* todo: listen interval for power saving */
-	val16 = cpu_to_le16(3);
-	memcpy(pframe, (unsigned char *)&val16, 2);
+	put_unaligned_le16(3, pframe);
 	pframe += 2;
 	pattrib->pktlen += 2;
 
 	/* SSID */
-	pframe = rtw_set_ie23a(pframe, _SSID_IE_,
+	pframe = rtw_set_ie23a(pframe, WLAN_EID_SSID,
 			       pmlmeinfo->network.Ssid.ssid_len,
 			       pmlmeinfo->network.Ssid.ssid, &pattrib->pktlen);
 
@@ -6387,7 +3379,7 @@ void issue_assocreq23a(struct rtw_adapter *padapter)
 			     Handlink WSG-4000 AP */
 			if ((pmlmeinfo->network.SupportedRates[i] |
 			     IEEE80211_BASIC_RATE_MASK) ==
-			    (sta_bssrate[j]|IEEE80211_BASIC_RATE_MASK)) {
+			    (sta_bssrate[j] | IEEE80211_BASIC_RATE_MASK)) {
 				/* DBG_8723A("match i = %d, j =%d\n", i, j); */
 				break;
 			}
@@ -6414,33 +3406,33 @@ void issue_assocreq23a(struct rtw_adapter *padapter)
 	}
 
 	if (bssrate_len > 8) {
-		pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_, 8,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES, 8,
 				       bssrate, &pattrib->pktlen);
-		pframe = rtw_set_ie23a(pframe, _EXT_SUPPORTEDRATES_IE_,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_EXT_SUPP_RATES,
 				       (bssrate_len - 8), (bssrate + 8),
 				       &pattrib->pktlen);
 	} else
-		pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES,
 				       bssrate_len, bssrate, &pattrib->pktlen);
 
 	/* RSN */
-	p = rtw_get_ie23a((pmlmeinfo->network.IEs +
-			   sizeof(struct ndis_802_11_fixed_ies)), _RSN_IE_2_,
-			  &ie_len, (pmlmeinfo->network.IELength -
-				    sizeof(struct ndis_802_11_fixed_ies)));
+	bcn_fixed_size = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+		offsetof(struct ieee80211_mgmt, u.beacon);
+
+	pie = pmlmeinfo->network.IEs + bcn_fixed_size;
+	pie_len = pmlmeinfo->network.IELength - bcn_fixed_size;
+
+	p = cfg80211_find_ie(WLAN_EID_RSN, pie, pie_len);
 	if (p)
-		pframe = rtw_set_ie23a(pframe, _RSN_IE_2_, ie_len, (p + 2),
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_RSN, p[1], p + 2,
 				       &pattrib->pktlen);
 
 	/* HT caps */
 	if (padapter->mlmepriv.htpriv.ht_option == true) {
-		p = rtw_get_ie23a((pmlmeinfo->network.IEs +
-				   sizeof(struct ndis_802_11_fixed_ies)),
-				  _HT_CAPABILITY_IE_, &ie_len,
-				  (pmlmeinfo->network.IELength -
-				   sizeof(struct ndis_802_11_fixed_ies)));
-		if ((p != NULL) && (!(is_ap_in_tkip23a(padapter)))) {
-			memcpy(&pmlmeinfo->HT_caps, (p + 2),
+		p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, pie, pie_len);
+
+		if (p && !is_ap_in_tkip23a(padapter)) {
+			memcpy(&pmlmeinfo->HT_caps, p + 2,
 			       sizeof(struct HT_caps_element));
 
 			/* to disable 40M Hz support while gd_bw_40MHz_en = 0 */
@@ -6454,15 +3446,13 @@ void issue_assocreq23a(struct rtw_adapter *padapter)
 			pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |=
 				0x000c;
 
-			rtw23a_hal_get_hwreg(padapter, HW_VAR_RF_TYPE,
-					     (u8 *)(&rf_type));
+			rf_type = rtl8723a_get_rf_type(padapter);
 			/* switch (pregpriv->rf_config) */
-			switch (rf_type)
-			{
+			switch (rf_type) {
 			case RF_1T1R:
-
+				/* RX STBC One spatial stream */
 				if (pregpriv->rx_stbc)
-					pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(0x0100);/* RX STBC One spatial stream */
+					pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(0x0100);
 
 				memcpy(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_rate_1R23A, 16);
 				break;
@@ -6470,18 +3460,17 @@ void issue_assocreq23a(struct rtw_adapter *padapter)
 			case RF_2T2R:
 			case RF_1T2R:
 			default:
-
 				/* enable for 2.4/5 GHz */
-				if ((pregpriv->rx_stbc == 0x3) ||
-				    ((pmlmeext->cur_wireless_mode &
-				      WIRELESS_11_24N) &&
+				if (pregpriv->rx_stbc == 0x3 ||
+				    (pmlmeext->cur_wireless_mode &
+				     WIRELESS_11_24N &&
 				     /* enable for 2.4GHz */
-				     (pregpriv->rx_stbc == 0x1)) ||
-				    ((pmlmeext->cur_wireless_mode &
-				      WIRELESS_11_5N) &&
-				     (pregpriv->rx_stbc == 0x2)) ||
+				     pregpriv->rx_stbc == 0x1) ||
+				    (pmlmeext->cur_wireless_mode &
+				     WIRELESS_11_5N &&
+				     pregpriv->rx_stbc == 0x2) ||
 				    /* enable for 5GHz */
-				    (pregpriv->wifi_spec == 1)) {
+				    pregpriv->wifi_spec == 1) {
 					DBG_8723A("declare supporting RX "
 						  "STBC\n");
 					pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(0x0200);/* RX STBC two spatial stream */
@@ -6492,44 +3481,40 @@ void issue_assocreq23a(struct rtw_adapter *padapter)
 			pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info =
 				cpu_to_le16(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info);
 
-#ifdef CONFIG_8723AU_BT_COEXIST
-			if (BT_1Ant(padapter) == true) {
+			if (rtl8723a_BT_coexist(padapter) &&
+			    rtl8723a_BT_using_antenna_1(padapter)) {
 				/*  set to 8K */
 				pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para &= (u8)~IEEE80211_HT_AMPDU_PARM_FACTOR;
 /*				pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para |= MAX_AMPDU_FACTOR_8K */
 			}
-#endif
 
-			pframe = rtw_set_ie23a(pframe, _HT_CAPABILITY_IE_,
-					       ie_len,
-					       (u8 *)&pmlmeinfo->HT_caps,
+			pframe = rtw_set_ie23a(pframe, WLAN_EID_HT_CAPABILITY,
+					       p[1], (u8 *)&pmlmeinfo->HT_caps,
 					       &pattrib->pktlen);
 		}
 	}
 
 	/* vendor specific IE, such as WPA, WMM, WPS */
-	for (i = sizeof(struct ndis_802_11_fixed_ies);
-	     i < pmlmeinfo->network.IELength;) {
-		pIE = (struct ndis_802_11_var_ies *)
-			(pmlmeinfo->network.IEs + i);
-
-		switch (pIE->ElementID)
-		{
-		case _VENDOR_SPECIFIC_IE_:
-			if (!memcmp(pIE->data, RTW_WPA_OUI23A, 4) ||
-			    !memcmp(pIE->data, WMM_OUI23A, 4) ||
-			    !memcmp(pIE->data, WPS_OUI23A, 4)) {
+	for (i = bcn_fixed_size;  i < pmlmeinfo->network.IELength;) {
+		p = pmlmeinfo->network.IEs + i;
+
+		switch (p[0]) {
+		case WLAN_EID_VENDOR_SPECIFIC:
+			if (!memcmp(p + 2, RTW_WPA_OUI23A_TYPE, 4) ||
+			    !memcmp(p + 2, WMM_OUI23A, 4) ||
+			    !memcmp(p + 2, WPS_OUI23A, 4)) {
+				u8 plen = p[1];
 				if (!padapter->registrypriv.wifi_spec) {
 					/* Commented by Kurt 20110629 */
 					/* In some older APs, WPS handshake */
 					/* would be fail if we append vender
 					   extensions informations to AP */
-					if (!memcmp(pIE->data, WPS_OUI23A, 4))
-						pIE->Length = 14;
+					if (!memcmp(p + 2, WPS_OUI23A, 4))
+						plen = 14;
 				}
 				pframe = rtw_set_ie23a(pframe,
-						       _VENDOR_SPECIFIC_IE_,
-						       pIE->Length, pIE->data,
+						       WLAN_EID_VENDOR_SPECIFIC,
+						       plen, p + 2,
 						       &pattrib->pktlen);
 			}
 			break;
@@ -6538,197 +3523,12 @@ void issue_assocreq23a(struct rtw_adapter *padapter)
 			break;
 		}
 
-		i += (pIE->Length + 2);
+		i += p[1] + 2;
 	}
 
 	if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
-		pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, 6,
-				       REALTEK_96B_IE23A, &pattrib->pktlen);
-
-#ifdef CONFIG_8723AU_P2P
-
-	if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) {
-		if (pmlmepriv->p2p_assoc_req_ie &&
-		    pmlmepriv->p2p_assoc_req_ie_len>0) {
-			memcpy(pframe, pmlmepriv->p2p_assoc_req_ie,
-			       pmlmepriv->p2p_assoc_req_ie_len);
-			pframe += pmlmepriv->p2p_assoc_req_ie_len;
-			pattrib->pktlen += pmlmepriv->p2p_assoc_req_ie_len;
-		}
-	} else {
-		if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) &&
-		    !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE)) {
-			/*	Should add the P2P IE in the association
-				request frame. */
-			/*	P2P OUI */
-
-			p2pielen = 0;
-			p2pie[p2pielen++] = 0x50;
-			p2pie[p2pielen++] = 0x6F;
-			p2pie[p2pielen++] = 0x9A;
-			p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
-
-			/*	Commented by Albert 20101109 */
-			/*	According to the P2P Specification, the
-				association request frame should contain
-				3 P2P attributes */
-			/*	1. P2P Capability */
-			/*	2. Extended Listen Timing */
-			/*	3. Device Info */
-			/*	Commented by Albert 20110516 */
-			/*	4. P2P Interface */
-
-			/*	P2P Capability */
-			/*	Type: */
-			p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
-
-			/*	Length: */
-			*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002);
-			p2pielen += 2;
-
-			/*	Value: */
-			/*	Device Capability Bitmap, 1 byte */
-			p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
-
-			/*	Group Capability Bitmap, 1 byte */
-			if (pwdinfo->persistent_supported)
-				p2pie[p2pielen++] =
-					P2P_GRPCAP_PERSISTENT_GROUP |
-					DMP_P2P_GRPCAP_SUPPORT;
-			else
-				p2pie[p2pielen++] = DMP_P2P_GRPCAP_SUPPORT;
-
-			/*	Extended Listen Timing */
-			/*	Type: */
-			p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING;
-
-			/*	Length: */
-			*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0004);
-			p2pielen += 2;
-
-			/*	Value: */
-			/*	Availability Period */
-			*(u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF);
-			p2pielen += 2;
-
-			/*	Availability Interval */
-			*(u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF);
-			p2pielen += 2;
-
-			/*	Device Info */
-			/*	Type: */
-			p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
-
-			/*	Length: */
-			/*	21 -> P2P Device Address (6bytes) + Config
-				Methods (2bytes) + Primary Device
-				Type (8bytes) */
-			/*	+ NumofSecondDevType (1byte) + WPS Device
-				Name ID field (2bytes) + WPS Device Name
-				Len field (2bytes) */
-			*(u16*) (p2pie + p2pielen) =
-				cpu_to_le16(21 + pwdinfo->device_name_len);
-			p2pielen += 2;
-
-			/*	Value: */
-			/*	P2P Device Address */
-			memcpy(p2pie + p2pielen,
-			       myid(&padapter->eeprompriv), ETH_ALEN);
-			p2pielen += ETH_ALEN;
-
-			/*	Config Method */
-			/*	This field should be big endian.
-				Noted by P2P specification. */
-			if ((pwdinfo->ui_got_wps_info ==
-			     P2P_GOT_WPSINFO_PEER_DISPLAY_PIN) ||
-			    (pwdinfo->ui_got_wps_info ==
-			     P2P_GOT_WPSINFO_SELF_DISPLAY_PIN))
-				*(u16*) (p2pie + p2pielen) =
-					cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY);
-			else
-				*(u16*) (p2pie + p2pielen) =
-					cpu_to_be16(WPS_CONFIG_METHOD_PBC);
-
-			p2pielen += 2;
-
-			/*	Primary Device Type */
-			/*	Category ID */
-			*(u16*) (p2pie + p2pielen) =
-				cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
-			p2pielen += 2;
-
-			/*	OUI */
-			*(u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI);
-			p2pielen += 4;
-
-			/*	Sub Category ID */
-			*(u16*) (p2pie + p2pielen) =
-				cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
-			p2pielen += 2;
-
-			/*	Number of Secondary Device Types */
-			/*	No Secondary Device Type List */
-			p2pie[p2pielen++] = 0x00;
-
-			/*	Device Name */
-			/*	Type: */
-			*(u16*) (p2pie + p2pielen) =
-				cpu_to_be16(WPS_ATTR_DEVICE_NAME);
-			p2pielen += 2;
-
-			/*	Length: */
-			*(u16*) (p2pie + p2pielen) =
-				cpu_to_be16(pwdinfo->device_name_len);
-			p2pielen += 2;
-
-			/*	Value: */
-			memcpy(p2pie + p2pielen, pwdinfo->device_name,
-			       pwdinfo->device_name_len);
-			p2pielen += pwdinfo->device_name_len;
-
-			/*	P2P Interface */
-			/*	Type: */
-			p2pie[p2pielen++] = P2P_ATTR_INTERFACE;
-
-			/*	Length: */
-			*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x000D);
-			p2pielen += 2;
-
-			/*	Value: */
-			memcpy(p2pie + p2pielen, pwdinfo->device_addr,
-			       ETH_ALEN);	/* P2P Device Address */
-			p2pielen += ETH_ALEN;
-
-			/* P2P Interface Address Count */
-			p2pie[p2pielen++] = 1;
-
-			memcpy(p2pie + p2pielen, pwdinfo->device_addr,
-			       ETH_ALEN);	/* P2P Interface Address List */
-			p2pielen += ETH_ALEN;
-
-			pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_,
-					       p2pielen, (unsigned char *)p2pie,
-					       &pattrib->pktlen);
-
-			/* wfdielen = build_assoc_req_wfd_ie(pwdinfo, pframe);*/
-			/* pframe += wfdielen; */
-			/* pattrib->pktlen += wfdielen; */
-		}
-	}
-
-	if (true == pwdinfo->wfd_info->wfd_enable) {
-		wfdielen = build_assoc_req_wfd_ie(pwdinfo, pframe);
-		pframe += wfdielen;
-		pattrib->pktlen += wfdielen;
-	} else if (pmlmepriv->wfd_assoc_req_ie != NULL &&
-		   pmlmepriv->wfd_assoc_req_ie_len > 0) {
-		/* WFD IE */
-		memcpy(pframe, pmlmepriv->wfd_assoc_req_ie,
-		       pmlmepriv->wfd_assoc_req_ie_len);
-		pattrib->pktlen += pmlmepriv->wfd_assoc_req_ie_len;
-		pframe += pmlmepriv->wfd_assoc_req_ie_len;
-	}
-#endif /* CONFIG_8723AU_P2P */
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_VENDOR_SPECIFIC, 6,
+				       REALTEK_96B_IE, &pattrib->pktlen);
 
 	pattrib->last_txcmdsz = pattrib->pktlen;
 	dump_mgntframe23a(padapter, pmgntframe);
@@ -6741,8 +3541,7 @@ exit:
 		kfree(pmlmepriv->assoc_req);
 		pmlmepriv->assoc_req = kmalloc(pattrib->pktlen, GFP_ATOMIC);
 		if (pmlmepriv->assoc_req) {
-			memcpy(pmlmepriv->assoc_req, pwlanhdr,
-			       pattrib->pktlen);
+			memcpy(pmlmepriv->assoc_req, pwlanhdr, pattrib->pktlen);
 			pmlmepriv->assoc_req_len = pattrib->pktlen;
 		}
 	} else
@@ -6760,7 +3559,6 @@ static int _issue_nulldata23a(struct rtw_adapter *padapter, unsigned char *da,
 	struct pkt_attrib *pattrib;
 	unsigned char *pframe;
 	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
 	struct xmit_priv *pxmitpriv;
 	struct mlme_ext_priv *pmlmeext;
 	struct mlme_ext_info *pmlmeinfo;
@@ -6774,7 +3572,8 @@ static int _issue_nulldata23a(struct rtw_adapter *padapter, unsigned char *da,
 	pmlmeext = &padapter->mlmeextpriv;
 	pmlmeinfo = &pmlmeext->mlmext_info;
 
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe)
 		goto exit;
 
 	/* update attribute */
@@ -6787,24 +3586,24 @@ static int _issue_nulldata23a(struct rtw_adapter *padapter, unsigned char *da,
 	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA |
+					      IEEE80211_STYPE_NULLFUNC);
 
 	if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
-		SetFrDs(fctrl);
+		pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_FROMDS);
 	else if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
-		SetToDs(fctrl);
+		pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_TODS);
 
 	if (power_mode)
-		SetPwrMgt(fctrl);
+		pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_PM);
 
 	ether_addr_copy(pwlanhdr->addr1, da);
 	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
 	ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network));
 
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
 	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_DATA_NULL);
 
 	pframe += sizeof(struct ieee80211_hdr_3addr);
 	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
@@ -6858,14 +3657,15 @@ int issue_nulldata23a(struct rtw_adapter *padapter, unsigned char *da,
 
 	if (try_cnt && wait_ms) {
 		if (da)
-			DBG_8723A(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d "
-				  "in %u ms\n", FUNC_ADPT_ARG(padapter),
+			DBG_8723A("%s(%s): to "MAC_FMT", ch:%u%s, %d/%d "
+				  "in %u ms\n", __func__,
+				  padapter->pnetdev->name,
 				  MAC_ARG(da), rtw_get_oper_ch23a(padapter),
 				  ret == _SUCCESS?", acked":"", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
 		else
-			DBG_8723A(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
-				  FUNC_ADPT_ARG(padapter),
+			DBG_8723A("%s(%s):, ch:%u%s, %d/%d in %u ms\n",
+				  __func__, padapter->pnetdev->name,
 				  rtw_get_oper_ch23a(padapter),
 				  ret == _SUCCESS?", acked":"", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
@@ -6882,15 +3682,15 @@ static int _issue_qos_nulldata23a(struct rtw_adapter *padapter,
 	struct xmit_frame *pmgntframe;
 	struct pkt_attrib *pattrib;
 	unsigned char *pframe;
-	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl, *qc;
+	struct ieee80211_qos_hdr *pwlanhdr;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
 	DBG_8723A("%s\n", __func__);
 
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe)
 		goto exit;
 
 	/* update attribute */
@@ -6906,34 +3706,32 @@ static int _issue_qos_nulldata23a(struct rtw_adapter *padapter,
 	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
 
 	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
+	pwlanhdr = (struct ieee80211_qos_hdr *)pframe;
 
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA |
+					      IEEE80211_STYPE_QOS_NULLFUNC);
 
 	if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
-		SetFrDs(fctrl);
+		pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_FROMDS);
 	else if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
-		SetToDs(fctrl);
+		pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_TODS);
 
 	if (pattrib->mdata)
-		SetMData(fctrl);
-
-	qc = (unsigned short *)(pframe + pattrib->hdrlen - 2);
+		pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_MOREDATA);
 
-	SetPriority(qc, tid);
-
-	SetEOSP(qc, pattrib->eosp);
-
-	SetAckpolicy(qc, pattrib->ack_policy);
+	pwlanhdr->qos_ctrl = cpu_to_le16(tid & IEEE80211_QOS_CTL_TID_MASK);
+	pwlanhdr->qos_ctrl |= cpu_to_le16((pattrib->ack_policy << 5) &
+					  IEEE80211_QOS_CTL_ACK_POLICY_MASK);
+	if (pattrib->eosp)
+		pwlanhdr->qos_ctrl |= cpu_to_le16(IEEE80211_QOS_CTL_EOSP);
 
 	ether_addr_copy(pwlanhdr->addr1, da);
 	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
 	ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network));
 
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
 	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_QOS_DATA_NULL);
 
 	pframe += sizeof(struct ieee80211_qos_hdr);
 	pattrib->pktlen = sizeof(struct ieee80211_qos_hdr);
@@ -6986,14 +3784,15 @@ int issue_qos_nulldata23a(struct rtw_adapter *padapter, unsigned char *da,
 
 	if (try_cnt && wait_ms) {
 		if (da)
-			DBG_8723A(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d "
-				  "in %u ms\n", FUNC_ADPT_ARG(padapter),
+			DBG_8723A("%s(%s): to "MAC_FMT", ch:%u%s, %d/%d "
+				  "in %u ms\n", __func__,
+				  padapter->pnetdev->name,
 				  MAC_ARG(da), rtw_get_oper_ch23a(padapter),
 				  ret == _SUCCESS?", acked":"", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
 		else
-			DBG_8723A(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
-				  FUNC_ADPT_ARG(padapter),
+			DBG_8723A("%s(%s):, ch:%u%s, %d/%d in %u ms\n",
+				  __func__, padapter->pnetdev->name,
 				  rtw_get_oper_ch23a(padapter),
 				  ret == _SUCCESS?", acked":"", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
@@ -7002,33 +3801,22 @@ exit:
 	return ret;
 }
 
-static int _issue_deauth23a(struct rtw_adapter *padapter, unsigned char *da,
-			    unsigned short reason, u8 wait_ack)
+static int _issue_deauth(struct rtw_adapter *padapter, unsigned char *da,
+			 unsigned short reason, u8 wait_ack)
 {
 	struct xmit_frame *pmgntframe;
 	struct pkt_attrib *pattrib;
 	unsigned char *pframe;
 	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	int ret = _FAIL;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif /* CONFIG_8723AU_P2P */
 
 	/* DBG_8723A("%s to "MAC_FMT"\n", __func__, MAC_ARG(da)); */
 
-#ifdef CONFIG_8723AU_P2P
-	if (!(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) &&
-	    (pwdinfo->rx_invitereq_info.scan_op_ch_only)) {
-		mod_timer(&pwdinfo->reset_ch_sitesurvey,
-			  jiffies + msecs_to_jiffies(10));
-	}
-#endif /* CONFIG_8723AU_P2P */
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe)
 		goto exit;
 
 	/* update attribute */
@@ -7041,16 +3829,16 @@ static int _issue_deauth23a(struct rtw_adapter *padapter, unsigned char *da,
 	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					      IEEE80211_STYPE_DEAUTH);
 
 	ether_addr_copy(pwlanhdr->addr1, da);
 	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
 	ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network));
 
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
 	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_DEAUTH);
 
 	pframe += sizeof(struct ieee80211_hdr_3addr);
 	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
@@ -7077,19 +3865,19 @@ int issue_deauth23a(struct rtw_adapter *padapter, unsigned char *da,
 		    unsigned short reason)
 {
 	DBG_8723A("%s to "MAC_FMT"\n", __func__, MAC_ARG(da));
-	return _issue_deauth23a(padapter, da, reason, false);
+	return _issue_deauth(padapter, da, reason, false);
 }
 
-int issue_deauth23a_ex23a(struct rtw_adapter *padapter, u8 *da,
-			  unsigned short reason, int try_cnt, int wait_ms)
+static int issue_deauth_ex(struct rtw_adapter *padapter, u8 *da,
+			   unsigned short reason, int try_cnt, int wait_ms)
 {
 	int ret;
 	int i = 0;
 	unsigned long start = jiffies;
 
 	do {
-		ret = _issue_deauth23a(padapter, da, reason,
-				       wait_ms >0 ? true : false);
+		ret = _issue_deauth(padapter, da, reason,
+				    wait_ms >0 ? true : false);
 
 		i++;
 
@@ -7108,14 +3896,15 @@ int issue_deauth23a_ex23a(struct rtw_adapter *padapter, u8 *da,
 
 	if (try_cnt && wait_ms) {
 		if (da)
-			DBG_8723A(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d "
-				  "in %u ms\n", FUNC_ADPT_ARG(padapter),
+			DBG_8723A("%s(%s): to "MAC_FMT", ch:%u%s, %d/%d "
+				  "in %u ms\n", __func__,
+				  padapter->pnetdev->name,
 				  MAC_ARG(da), rtw_get_oper_ch23a(padapter),
 				  ret == _SUCCESS?", acked":"", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
 		else
-			DBG_8723A(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
-				  FUNC_ADPT_ARG(padapter),
+			DBG_8723A("%s(%s):, ch:%u%s, %d/%d in %u ms\n",
+				  __func__, padapter->pnetdev->name,
 				  rtw_get_oper_ch23a(padapter),
 				  ret == _SUCCESS?", acked":"", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
@@ -7131,16 +3920,15 @@ void issue_action_spct_ch_switch23a(struct rtw_adapter *padapter,
 	struct pkt_attrib *pattrib;
 	unsigned char *pframe;
 	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	u8 category, action;
 
-	DBG_8723A(FUNC_NDEV_FMT" ra ="MAC_FMT", ch:%u, offset:%u\n",
-		FUNC_NDEV_ARG(padapter->pnetdev), MAC_ARG(ra),
-		  new_ch, ch_offset);
+	DBG_8723A("%s(%s): ra ="MAC_FMT", ch:%u, offset:%u\n", __func__,
+		  padapter->pnetdev->name, MAC_ARG(ra), new_ch, ch_offset);
 
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe)
 		return;
 
 	/* update attribute */
@@ -7152,16 +3940,16 @@ void issue_action_spct_ch_switch23a(struct rtw_adapter *padapter,
 	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					      IEEE80211_STYPE_ACTION);
 
 	ether_addr_copy(pwlanhdr->addr1, ra); /* RA */
 	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv)); /* TA */
 	ether_addr_copy(pwlanhdr->addr3, ra); /* DA = RA */
 
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
 	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
 
 	pframe += sizeof(struct ieee80211_hdr_3addr);
 	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
@@ -7183,7 +3971,8 @@ void issue_action_spct_ch_switch23a(struct rtw_adapter *padapter,
 	dump_mgntframe23a(padapter, pmgntframe);
 }
 
-void issue_action_BA23a(struct rtw_adapter *padapter, unsigned char *raddr,
+void issue_action_BA23a(struct rtw_adapter *padapter,
+			const unsigned char *raddr,
 			unsigned char action, unsigned short status)
 {
 	u8 category = WLAN_CATEGORY_BACK;
@@ -7197,21 +3986,19 @@ void issue_action_BA23a(struct rtw_adapter *padapter, unsigned char *raddr,
 	struct pkt_attrib *pattrib;
 	u8 *pframe;
 	struct ieee80211_hdr *pwlanhdr;
-	u16 *fctrl;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct sta_info *psta;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct registry_priv *pregpriv = &padapter->registrypriv;
-#ifdef CONFIG_8723AU_BT_COEXIST
 	u8 tendaAPMac[] = {0xC8, 0x3A, 0x35};
-#endif
 
 	DBG_8723A("%s, category =%d, action =%d, status =%d\n",
 		  __func__, category, action, status);
 
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe)
 		return;
 
 	/* update attribute */
@@ -7223,17 +4010,17 @@ void issue_action_BA23a(struct rtw_adapter *padapter, unsigned char *raddr,
 	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					      IEEE80211_STYPE_ACTION);
 
 	/* memcpy(pwlanhdr->addr1, get_my_bssid23a(&pmlmeinfo->network), ETH_ALEN); */
 	ether_addr_copy(pwlanhdr->addr1, raddr);
 	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
 	ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network));
 
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
 	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
 
 	pframe += sizeof(struct ieee80211_hdr_3addr);
 	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
@@ -7255,9 +4042,9 @@ void issue_action_BA23a(struct rtw_adapter *padapter, unsigned char *raddr,
 		pframe = rtw_set_fixed_ie23a(pframe, 1, &pmlmeinfo->dialogToken,
 					     &pattrib->pktlen);
 
-#ifdef CONFIG_8723AU_BT_COEXIST
-		if ((BT_1Ant(padapter) == true) &&
-		    ((pmlmeinfo->assoc_AP_vendor != broadcomAP) ||
+		if (rtl8723a_BT_coexist(padapter) &&
+		    rtl8723a_BT_using_antenna_1(padapter) &&
+		    (pmlmeinfo->assoc_AP_vendor != broadcomAP ||
 		     memcmp(raddr, tendaAPMac, 3))) {
 			/*  A-MSDU NOT Supported */
 			BA_para_set = 0;
@@ -7270,9 +4057,7 @@ void issue_action_BA23a(struct rtw_adapter *padapter, unsigned char *raddr,
 			/*  max buffer size is 8 MSDU */
 			BA_para_set |= (8 << 6) &
 				IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
-		} else
-#endif
-		{
+		} else {
 			/* immediate ack & 64 buffer size */
 			BA_para_set = (0x1002 | ((status & 0xf) << 2));
 		}
@@ -7309,8 +4094,8 @@ void issue_action_BA23a(struct rtw_adapter *padapter, unsigned char *raddr,
 		pframe = rtw_set_fixed_ie23a(pframe, 2,
 					     (unsigned char *)&status,
 					     &pattrib->pktlen);
-		rtw_hal_get_def_var23a(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR,
-				       &max_rx_ampdu_factor);
+		GetHalDefVar8192CUsb(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR,
+				     &max_rx_ampdu_factor);
 		if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_64K)
 			BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); /* 64 buffer size */
 		else if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_32K)
@@ -7322,16 +4107,15 @@ void issue_action_BA23a(struct rtw_adapter *padapter, unsigned char *raddr,
 		else
 			BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); /* 64 buffer size */
 
-#ifdef CONFIG_8723AU_BT_COEXIST
-		if ((BT_1Ant(padapter) == true) &&
-		    ((pmlmeinfo->assoc_AP_vendor != broadcomAP) ||
+		if (rtl8723a_BT_coexist(padapter) &&
+		    rtl8723a_BT_using_antenna_1(padapter) &&
+		    (pmlmeinfo->assoc_AP_vendor != broadcomAP ||
 		     memcmp(raddr, tendaAPMac, 3))) {
 			/*  max buffer size is 8 MSDU */
 			BA_para_set &= ~IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
 			BA_para_set |= (8 << 6) &
 				IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
 		}
-#endif
 
 		if (pregpriv->ampdu_amsdu == 0)/* disabled */
 			BA_para_set = cpu_to_le16(BA_para_set & ~BIT(0));
@@ -7373,24 +4157,25 @@ static void issue_action_BSSCoexistPacket(struct rtw_adapter *padapter)
 {
 	struct list_head *plist, *phead, *ptmp;
 	unsigned char category, action;
-	struct xmit_frame			*pmgntframe;
-	struct pkt_attrib			*pattrib;
-	unsigned char				*pframe;
-	struct ieee80211_hdr	*pwlanhdr;
-	unsigned short			*fctrl;
-	struct	wlan_network	*pnetwork = NULL;
+	struct xmit_frame *pmgntframe;
+	struct pkt_attrib *pattrib;
+	u8 *pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	struct wlan_network *pnetwork;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	struct rtw_queue	*queue	= &pmlmepriv->scanned_queue;
+	struct rtw_queue *queue	= &pmlmepriv->scanned_queue;
 	u8 InfoContent[16] = {0};
 	u8 ICS[8][15];
+	int i;
 
-	if ((pmlmepriv->num_FortyMHzIntolerant == 0) || (pmlmepriv->num_sta_no_ht == 0))
+	if (pmlmepriv->num_FortyMHzIntolerant == 0 ||
+	    pmlmepriv->num_sta_no_ht == 0)
 		return;
 
-	if (true == pmlmeinfo->bwmode_updated)
+	if (pmlmeinfo->bwmode_updated)
 		return;
 
 	DBG_8723A("%s\n", __func__);
@@ -7398,10 +4183,9 @@ static void issue_action_BSSCoexistPacket(struct rtw_adapter *padapter)
 	category = WLAN_CATEGORY_PUBLIC;
 	action = ACT_PUBLIC_BSSCOEXIST;
 
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-	{
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe)
 		return;
-	}
 
 	/* update attribute */
 	pattrib = &pmgntframe->attrib;
@@ -7409,19 +4193,19 @@ static void issue_action_BSSCoexistPacket(struct rtw_adapter *padapter)
 
 	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
 
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pframe = (u8 *)pmgntframe->buf_addr + TXDESC_OFFSET;
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					      IEEE80211_STYPE_ACTION);
 
 	ether_addr_copy(pwlanhdr->addr1, get_my_bssid23a(&pmlmeinfo->network));
 	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
 	ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network));
 
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
 	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
 
 	pframe += sizeof(struct ieee80211_hdr_3addr);
 	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
@@ -7429,91 +4213,80 @@ static void issue_action_BSSCoexistPacket(struct rtw_adapter *padapter)
 	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
 	pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen);
 
-	/*  */
-	if (pmlmepriv->num_FortyMHzIntolerant>0)
-	{
-		u8 iedata = 0;
-
-		iedata |= BIT(2);/* 20 MHz BSS Width Request */
-
-		pframe = rtw_set_ie23a(pframe, EID_BSSCoexistence,  1, &iedata, &pattrib->pktlen);
+	if (pmlmepriv->num_FortyMHzIntolerant > 0) {
+		u8 iedata = BIT(2);/* 20 MHz BSS Width Request */
 
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_BSS_COEX_2040, 1,
+				       &iedata, &pattrib->pktlen);
 	}
 
-	/*  */
-	memset(ICS, 0, sizeof(ICS));
-	if (pmlmepriv->num_sta_no_ht>0)
-	{
-		int i;
+	if (pmlmepriv->num_sta_no_ht <= 0)
+		goto out;
 
-		spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+	memset(ICS, 0, sizeof(ICS));
 
-		phead = get_list_head(queue);
-		plist = phead->next;
+	spin_lock_bh(&pmlmepriv->scanned_queue.lock);
 
-		list_for_each_safe(plist, ptmp, phead) {
-			int len;
-			u8 *p;
-			struct wlan_bssid_ex *pbss_network;
+	phead = get_list_head(queue);
+	plist = phead->next;
 
-			pnetwork = container_of(plist, struct wlan_network,
-						list);
+	list_for_each_safe(plist, ptmp, phead) {
+		const u8 *p;
+		struct wlan_bssid_ex *pbss_network;
 
-			pbss_network = &pnetwork->network;
+		pnetwork = container_of(plist, struct wlan_network, list);
 
-			p = rtw_get_ie23a(pbss_network->IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pbss_network->IELength - _FIXED_IE_LENGTH_);
-			if ((p == NULL) || (len == 0))/* non-HT */
-			{
-				if ((pbss_network->Configuration.DSConfig<= 0) || (pbss_network->Configuration.DSConfig>14))
-					continue;
+		pbss_network = &pnetwork->network;
 
-				ICS[0][pbss_network->Configuration.DSConfig]= 1;
+		p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY,
+				     pbss_network->IEs + _FIXED_IE_LENGTH_,
+				     pbss_network->IELength -_FIXED_IE_LENGTH_);
+		if (!p || !p[1]) { /* non-HT */
+			if (pbss_network->DSConfig <= 0 ||
+			    pbss_network->DSConfig > 14)
+				continue;
 
-				if (ICS[0][0] == 0)
-					ICS[0][0] = 1;
-			}
+			ICS[0][pbss_network->DSConfig] = 1;
 
+			if (ICS[0][0] == 0)
+				ICS[0][0] = 1;
 		}
 
-		spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+	}
 
-		for (i = 0;i<8;i++)
-		{
-			if (ICS[i][0] == 1)
-			{
-				int j, k = 0;
+	spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 
-				InfoContent[k] = i;
-				/* SET_BSS_INTOLERANT_ELE_REG_CLASS(InfoContent, i); */
-				k++;
+	for (i = 0; i < 8;i++) {
+		if (ICS[i][0] == 1) {
+			int j, k = 0;
 
-				for (j = 1;j<= 14;j++)
-				{
-					if (ICS[i][j]== 1)
-					{
-						if (k<16)
-						{
-							InfoContent[k] = j; /* channel number */
-							/* SET_BSS_INTOLERANT_ELE_CHANNEL(InfoContent+k, j); */
-							k++;
-						}
+			InfoContent[k] = i;
+			/* SET_BSS_INTOLERANT_ELE_REG_CLASS(InfoContent, i); */
+			k++;
+
+			for (j = 1; j <= 14; j++) {
+				if (ICS[i][j] == 1) {
+					if (k < 16) {
+						/* channel number */
+						InfoContent[k] = j;
+						k++;
 					}
 				}
-
-				pframe = rtw_set_ie23a(pframe, EID_BSSIntolerantChlReport, k, InfoContent, &pattrib->pktlen);
-
 			}
 
+			pframe = rtw_set_ie23a(pframe,
+					       EID_BSSIntolerantChlReport, k,
+					       InfoContent, &pattrib->pktlen);
 		}
-
 	}
 
+out:
 	pattrib->last_txcmdsz = pattrib->pktlen;
 
 	dump_mgntframe23a(padapter, pmgntframe);
 }
 
-unsigned int send_delba23a(struct rtw_adapter *padapter, u8 initiator, u8 *addr)
+int send_delba23a(struct rtw_adapter *padapter, u8 initiator, u8 *addr)
 {
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct sta_info *psta = NULL;
@@ -7553,23 +4326,23 @@ unsigned int send_delba23a(struct rtw_adapter *padapter, u8 initiator, u8 *addr)
 	return _SUCCESS;
 }
 
-unsigned int send_beacon23a(struct rtw_adapter *padapter)
+int send_beacon23a(struct rtw_adapter *padapter)
 {
-	u8	bxmitok = false;
+	bool	bxmitok;
 	int	issue = 0;
 	int poll = 0;
 	unsigned long start = jiffies;
 	unsigned int passing_time;
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_BCN_VALID, NULL);
+	rtl8723a_bcn_valid(padapter);
 	do {
 		issue_beacon23a(padapter, 100);
 		issue++;
 		do {
 			yield();
-			rtw23a_hal_get_hwreg(padapter, HW_VAR_BCN_VALID, (u8 *)(&bxmitok));
+			bxmitok = rtl8723a_get_bcn_valid(padapter);
 			poll++;
-		} while ((poll%10)!= 0 && false == bxmitok &&
+		} while ((poll % 10) != 0 && bxmitok == false &&
 			 !padapter->bSurpriseRemoved &&
 			 !padapter->bDriverStopped);
 
@@ -7613,48 +4386,24 @@ bool IsLegal5GChannel(struct rtw_adapter *Adapter, u8 channel)
 	return false;
 }
 
-void site_survey23a(struct rtw_adapter *padapter)
+static void rtw_site_survey(struct rtw_adapter *padapter)
 {
-	unsigned char survey_channel = 0, val8;
+	unsigned char survey_channel = 0;
 	enum rt_scan_type ScanType = SCAN_PASSIVE;
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	u32 initialgain = 0;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-
-	if ((pwdinfo->rx_invitereq_info.scan_op_ch_only) ||
-	    (pwdinfo->p2p_info.scan_op_ch_only)) {
-		if (pwdinfo->rx_invitereq_info.scan_op_ch_only)
-			survey_channel = pwdinfo->rx_invitereq_info.operation_ch[pmlmeext->sitesurvey_res.channel_idx];
-		else
-			survey_channel = pwdinfo->p2p_info.operation_ch[pmlmeext->sitesurvey_res.channel_idx];
-		ScanType = SCAN_ACTIVE;
-	} else if (rtw_p2p_findphase_ex_is_social(pwdinfo)) {
-		/* The driver is in the find phase, it should go through the social channel. */
-		int ch_set_idx;
-		survey_channel = pwdinfo->social_chan[pmlmeext->sitesurvey_res.channel_idx];
-		ch_set_idx = rtw_ch_set_search_ch23a(pmlmeext->channel_set, survey_channel);
-		if (ch_set_idx >= 0)
-			ScanType = pmlmeext->channel_set[ch_set_idx].ScanType;
-		else
-			ScanType = SCAN_ACTIVE;
-	} else
-#endif /* CONFIG_8723AU_P2P */
-	{
-		struct rtw_ieee80211_channel *ch;
-		if (pmlmeext->sitesurvey_res.channel_idx < pmlmeext->sitesurvey_res.ch_num) {
-			ch = &pmlmeext->sitesurvey_res.ch[pmlmeext->sitesurvey_res.channel_idx];
-			survey_channel = ch->hw_value;
-			ScanType = (ch->flags & IEEE80211_CHAN_NO_IR) ? SCAN_PASSIVE : SCAN_ACTIVE;
-}
+	struct rtw_ieee80211_channel *ch;
+
+	if (pmlmeext->sitesurvey_res.channel_idx <
+	    pmlmeext->sitesurvey_res.ch_num) {
+		ch = &pmlmeext->sitesurvey_res.ch[pmlmeext->sitesurvey_res.channel_idx];
+		survey_channel = ch->hw_value;
+		ScanType = (ch->flags & IEEE80211_CHAN_NO_IR) ?
+			SCAN_PASSIVE : SCAN_ACTIVE;
 	}
 
 	if (survey_channel != 0) {
-		/* PAUSE 4-AC Queue when site_survey23a */
-		/* rtw23a_hal_get_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */
-		/* val8 |= 0x0f; */
-		/* rtw_hal_set_hwreg23a(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */
+		/* PAUSE 4-AC Queue when site_survey */
 		if (pmlmeext->sitesurvey_res.channel_idx == 0)
 			set_channel_bwmode23a(padapter, survey_channel,
 					      HAL_PRIME_CHNL_OFFSET_DONT_CARE,
@@ -7664,317 +4413,251 @@ void site_survey23a(struct rtw_adapter *padapter)
 
 		if (ScanType == SCAN_ACTIVE) /* obey the channel plan setting... */
 		{
-#ifdef CONFIG_8723AU_P2P
-			if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) ||
-				rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH)
-			)
-			{
-				issue23a_probereq_p2p(padapter, NULL);
-				issue23a_probereq_p2p(padapter, NULL);
-				issue23a_probereq_p2p(padapter, NULL);
-			}
-			else
-#endif /* CONFIG_8723AU_P2P */
-			{
-				int i;
-				for (i = 0;i<RTW_SSID_SCAN_AMOUNT;i++) {
-					if (pmlmeext->sitesurvey_res.ssid[i].ssid_len) {
-						/* todo: to issue two probe req??? */
-						issue_probereq23a(padapter, &pmlmeext->sitesurvey_res.ssid[i], NULL);
-						/* msleep(SURVEY_TO>>1); */
-						issue_probereq23a(padapter, &pmlmeext->sitesurvey_res.ssid[i], NULL);
-					}
-				}
-
-				if (pmlmeext->sitesurvey_res.scan_mode == SCAN_ACTIVE) {
+			int i;
+			for (i = 0;i<RTW_SSID_SCAN_AMOUNT;i++) {
+				if (pmlmeext->sitesurvey_res.ssid[i].ssid_len) {
 					/* todo: to issue two probe req??? */
-					issue_probereq23a(padapter, NULL, NULL);
+					issue_probereq(padapter, &pmlmeext->sitesurvey_res.ssid[i], NULL);
 					/* msleep(SURVEY_TO>>1); */
-					issue_probereq23a(padapter, NULL, NULL);
+					issue_probereq(padapter, &pmlmeext->sitesurvey_res.ssid[i], NULL);
 				}
 			}
+
+			if (pmlmeext->sitesurvey_res.scan_mode == SCAN_ACTIVE) {
+				/* todo: to issue two probe req??? */
+				issue_probereq(padapter, NULL, NULL);
+				/* msleep(SURVEY_TO>>1); */
+				issue_probereq(padapter, NULL, NULL);
+			}
 		}
 
 		set_survey_timer(pmlmeext, pmlmeext->chan_scan_time);
 	} else {
-
 		/*	channel number is 0 or this channel is not valid. */
+		pmlmeext->sitesurvey_res.state = SCAN_COMPLETE;
 
+		/* switch back to the original channel */
 
-#ifdef CONFIG_8723AU_P2P
-		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH))
-		{
-			if ((pwdinfo->rx_invitereq_info.scan_op_ch_only) || (pwdinfo->p2p_info.scan_op_ch_only))
-			{
-				/*	Set the find_phase_state_exchange_cnt to P2P_FINDPHASE_EX_CNT. */
-				/*	This will let the following flow to run the scanning end. */
-				rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_MAX);
-			}
-		}
-
-		if (rtw_p2p_findphase_ex_is_needed(pwdinfo))
-		{
-			/*	Set the P2P State to the listen state of find phase and set the current channel to the listen channel */
-			set_channel_bwmode23a(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
-			rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_LISTEN);
-			pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
-
-			initialgain = 0xff; /* restore RX GAIN */
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
-			/* turn on dynamic functions */
-			Restore_DM_Func_Flag23a(padapter);
-			/* Switch_DM_Func23a(padapter, DYNAMIC_FUNC_DIG|DYNAMIC_FUNC_HP|DYNAMIC_FUNC_SS, true); */
-
-			mod_timer(&pwdinfo->find_phase_timer, jiffies +
-				  msecs_to_jiffies(pwdinfo->listen_dwell * 100));
-		} else
-#endif /* CONFIG_8723AU_P2P */
-		{
-#ifdef CONFIG_8723AU_P2P
-			if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH))
-				rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
-			rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_NONE);
-#endif /* CONFIG_8723AU_P2P */
-
-			pmlmeext->sitesurvey_res.state = SCAN_COMPLETE;
-
-			/* switch back to the original channel */
-
-			set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+		set_channel_bwmode23a(padapter, pmlmeext->cur_channel,
+				      pmlmeext->cur_ch_offset,
+				      pmlmeext->cur_bwmode);
 
-			/* flush 4-AC Queue after site_survey23a */
-			/* val8 = 0; */
-			/* rtw_hal_set_hwreg23a(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */
+		/* flush 4-AC Queue after rtw_site_survey */
+		/* val8 = 0; */
 
-			/* config MSR */
-			Set_MSR23a(padapter, (pmlmeinfo->state & 0x3));
-
-			initialgain = 0xff; /* restore RX GAIN */
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
-			/* turn on dynamic functions */
-			Restore_DM_Func_Flag23a(padapter);
-			/* Switch_DM_Func23a(padapter, DYNAMIC_ALL_FUNC_ENABLE, true); */
+		/* config MSR */
+		Set_MSR23a(padapter, (pmlmeinfo->state & 0x3));
 
-			if (is_client_associated_to_ap23a(padapter) == true)
-			{
-				issue_nulldata23a(padapter, NULL, 0, 3, 500);
+		/* restore RX GAIN */
+		rtl8723a_set_initial_gain(padapter, 0xff);
+		/* turn on dynamic functions */
+		rtl8723a_odm_support_ability_restore(padapter);
 
-			}
+		if (is_client_associated_to_ap23a(padapter) == true)
+			issue_nulldata23a(padapter, NULL, 0, 3, 500);
 
-			val8 = 0; /* survey done */
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+		rtl8723a_mlme_sitesurvey(padapter, 0);
 
-			report_surveydone_event23a(padapter);
+		report_surveydone_event23a(padapter);
 
-			pmlmeext->chan_scan_time = SURVEY_TO;
-			pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
+		pmlmeext->chan_scan_time = SURVEY_TO;
+		pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
 
-			issue_action_BSSCoexistPacket(padapter);
-			issue_action_BSSCoexistPacket(padapter);
-			issue_action_BSSCoexistPacket(padapter);
-
-		}
+		issue_action_BSSCoexistPacket(padapter);
+		issue_action_BSSCoexistPacket(padapter);
+		issue_action_BSSCoexistPacket(padapter);
 	}
 
 	return;
 }
 
 /* collect bss info from Beacon and Probe request/response frames. */
-u8 collect_bss_info23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame, struct wlan_bssid_ex *bssid)
+int collect_bss_info23a(struct rtw_adapter *padapter,
+			struct recv_frame *precv_frame,
+			struct wlan_bssid_ex *bssid)
 {
-	int	i;
-	u32	len;
-	u8	*p;
-	u16	val16;
+	int i;
+	const u8 *p;
 	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u8	*pframe = skb->data;
-	u32	packet_len = skb->len;
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
+	unsigned int length;
 	u8 ie_offset;
-	struct registry_priv	*pregistrypriv = &padapter->registrypriv;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+	u16 capab_info;
 
-	len = packet_len - sizeof(struct ieee80211_hdr_3addr);
+	length = skb->len - sizeof(struct ieee80211_hdr_3addr);
 
-	if (len > MAX_IE_SZ)
-	{
+	if (length > MAX_IE_SZ) {
 		/* DBG_8723A("IE too long for survey event\n"); */
 		return _FAIL;
 	}
 
 	memset(bssid, 0, sizeof(struct wlan_bssid_ex));
 
-	if (ieee80211_is_beacon(hdr->frame_control)) {
+	if (ieee80211_is_beacon(mgmt->frame_control)) {
 		bssid->reserved = 1;
-		ie_offset = _BEACON_IE_OFFSET_;
+		ie_offset = offsetof(struct ieee80211_mgmt, u.beacon.variable);
+		capab_info = mgmt->u.beacon.capab_info;
+	} else  if (ieee80211_is_probe_req(mgmt->frame_control)) {
+		ie_offset = offsetof(struct ieee80211_mgmt,
+				     u.probe_req.variable);
+		bssid->reserved = 2;
+		capab_info = 0;
+	} else if (ieee80211_is_probe_resp(mgmt->frame_control)) {
+		ie_offset = offsetof(struct ieee80211_mgmt,
+				     u.probe_resp.variable);
+		bssid->reserved = 3;
+		capab_info = mgmt->u.probe_resp.capab_info;
 	} else {
-		/*  FIXME : more type */
-		if (ieee80211_is_probe_req(hdr->frame_control)) {
-			ie_offset = _PROBEREQ_IE_OFFSET_;
-			bssid->reserved = 2;
-		} else if (ieee80211_is_probe_resp(hdr->frame_control)) {
-			ie_offset = _PROBERSP_IE_OFFSET_;
-			bssid->reserved = 3;
-		} else {
-			bssid->reserved = 0;
-			ie_offset = _FIXED_IE_LENGTH_;
-		}
+		bssid->reserved = 0;
+		ie_offset = offsetof(struct ieee80211_mgmt, u.beacon.variable);
+		capab_info = mgmt->u.beacon.capab_info;
 	}
+	ie_offset -= offsetof(struct ieee80211_mgmt, u);
 
-	bssid->Length = sizeof(struct wlan_bssid_ex) - MAX_IE_SZ + len;
+	bssid->Length = offsetof(struct wlan_bssid_ex, IEs) + length;
 
 	/* below is to copy the information element */
-	bssid->IELength = len;
-	memcpy(bssid->IEs, (pframe + sizeof(struct ieee80211_hdr_3addr)), bssid->IELength);
+	bssid->IELength = length;
+	memcpy(bssid->IEs, &mgmt->u, bssid->IELength);
 
 	/* get the signal strength */
-	bssid->Rssi = precv_frame->attrib.phy_info.RecvSignalPower; /*  in dBM.raw data */
-	bssid->PhyInfo.SignalQuality = precv_frame->attrib.phy_info.SignalQuality;/* in percentage */
-	bssid->PhyInfo.SignalStrength = precv_frame->attrib.phy_info.SignalStrength;/* in percentage */
+	/*  in dBM.raw data */
+	bssid->Rssi = precv_frame->attrib.phy_info.RecvSignalPower;
+	bssid->PhyInfo.SignalQuality =
+		precv_frame->attrib.phy_info.SignalQuality;/* in percentage */
+	bssid->PhyInfo.SignalStrength =
+		precv_frame->attrib.phy_info.SignalStrength;/* in percentage */
 
 	/*  checking SSID */
-	if ((p = rtw_get_ie23a(bssid->IEs + ie_offset, _SSID_IE_, &len, bssid->IELength - ie_offset)) == NULL)
-	{
+	p = cfg80211_find_ie(WLAN_EID_SSID, bssid->IEs + ie_offset,
+			     bssid->IELength - ie_offset);
+
+	if (!p) {
 		DBG_8723A("marc: cannot find SSID for survey event\n");
 		return _FAIL;
 	}
 
-	if (*(p + 1)) {
-		if (len > IEEE80211_MAX_SSID_LEN) {
-			DBG_8723A("%s()-%d: IE too long (%d) for survey "
-				  "event\n", __func__, __LINE__, len);
-			return _FAIL;
-		}
-		memcpy(bssid->Ssid.ssid, (p + 2), *(p + 1));
-		bssid->Ssid.ssid_len = *(p + 1);
-	} else {
-		bssid->Ssid.ssid_len = 0;
+	if (p[1] > IEEE80211_MAX_SSID_LEN) {
+		DBG_8723A("%s()-%d: IE too long (%d) for survey "
+			  "event\n", __func__, __LINE__, p[1]);
+		return _FAIL;
 	}
+	memcpy(bssid->Ssid.ssid, p + 2, p[1]);
+	bssid->Ssid.ssid_len = p[1];
 
 	memset(bssid->SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX);
 
 	/* checking rate info... */
 	i = 0;
-	p = rtw_get_ie23a(bssid->IEs + ie_offset, _SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset);
-	if (p != NULL)
-	{
-		if (len > NDIS_802_11_LENGTH_RATES_EX)
-		{
-			DBG_8723A("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len);
+	p = cfg80211_find_ie(WLAN_EID_SUPP_RATES, bssid->IEs + ie_offset,
+			     bssid->IELength - ie_offset);
+	if (p) {
+		if (p[1] > NDIS_802_11_LENGTH_RATES_EX) {
+			DBG_8723A("%s()-%d: IE too long (%d) for survey "
+				  "event\n", __func__, __LINE__, p[1]);
 			return _FAIL;
 		}
-		memcpy(bssid->SupportedRates, (p + 2), len);
-		i = len;
+		memcpy(bssid->SupportedRates, p + 2, p[1]);
+		i = p[1];
 	}
 
-	p = rtw_get_ie23a(bssid->IEs + ie_offset, _EXT_SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset);
-	if (p != NULL)
-	{
-		if (len > (NDIS_802_11_LENGTH_RATES_EX-i))
-		{
-			DBG_8723A("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len);
+	p = cfg80211_find_ie(WLAN_EID_EXT_SUPP_RATES, bssid->IEs + ie_offset,
+			     bssid->IELength - ie_offset);
+	if (p) {
+		if (p[1] > (NDIS_802_11_LENGTH_RATES_EX-i)) {
+			DBG_8723A("%s()-%d: IE too long (%d) for survey "
+				  "event\n", __func__, __LINE__, p[1]);
 			return _FAIL;
 		}
-		memcpy(bssid->SupportedRates + i, (p + 2), len);
-	}
-
-	/* todo: */
-	{
-		bssid->NetworkTypeInUse = Ndis802_11OFDM24;
+		memcpy(bssid->SupportedRates + i, p + 2, p[1]);
 	}
 
 	if (bssid->IELength < 12)
 		return _FAIL;
 
 	/*  Checking for DSConfig */
-	p = rtw_get_ie23a(bssid->IEs + ie_offset, _DSSET_IE_, &len, bssid->IELength - ie_offset);
+	p = cfg80211_find_ie(WLAN_EID_DS_PARAMS, bssid->IEs + ie_offset,
+			     bssid->IELength - ie_offset);
 
-	bssid->Configuration.DSConfig = 0;
-	bssid->Configuration.Length = 0;
+	bssid->DSConfig = 0;
 
-	if (p)
-	{
-		bssid->Configuration.DSConfig = *(p + 2);
-	}
-	else
-	{/*  In 5G, some ap do not have DSSET IE */
+	if (p) {
+		bssid->DSConfig = p[2];
+	} else {/*  In 5G, some ap do not have DSSET IE */
 		/*  checking HT info for channel */
-		p = rtw_get_ie23a(bssid->IEs + ie_offset, _HT_ADD_INFO_IE_, &len, bssid->IELength - ie_offset);
-		if (p)
-		{
-			struct HT_info_element *HT_info = (struct HT_info_element *)(p + 2);
-			bssid->Configuration.DSConfig = HT_info->primary_channel;
-		}
-		else
-		{ /*  use current channel */
-			bssid->Configuration.DSConfig = rtw_get_oper_ch23a(padapter);
-		}
-	}
-
-	if (ieee80211_is_probe_req(hdr->frame_control)) {
+		p = cfg80211_find_ie(WLAN_EID_HT_OPERATION,
+				     bssid->IEs + ie_offset,
+				     bssid->IELength - ie_offset);
+		if (p) {
+			struct HT_info_element *HT_info =
+				(struct HT_info_element *)(p + 2);
+			bssid->DSConfig = HT_info->primary_channel;
+		} else /*  use current channel */
+			bssid->DSConfig = rtw_get_oper_ch23a(padapter);
+	}
+
+	if (ieee80211_is_probe_req(mgmt->frame_control)) {
 		/*  FIXME */
-		bssid->InfrastructureMode = Ndis802_11Infrastructure;
-		ether_addr_copy(bssid->MacAddress, hdr->addr2);
+		bssid->ifmode = NL80211_IFTYPE_STATION;
+		ether_addr_copy(bssid->MacAddress, mgmt->sa);
 		bssid->Privacy = 1;
 		return _SUCCESS;
 	}
 
-	memcpy(&bssid->Configuration.BeaconPeriod, rtw_get_beacon_interval23a_from_ie(bssid->IEs), 2);
-	bssid->Configuration.BeaconPeriod = le32_to_cpu(bssid->Configuration.BeaconPeriod);
+	bssid->BeaconPeriod = get_unaligned_le16(
+		rtw_get_beacon_interval23a_from_ie(bssid->IEs));
 
-	val16 = rtw_get_capability23a(bssid);
-
-	if (val16 & BIT(0)) {
-		bssid->InfrastructureMode = Ndis802_11Infrastructure;
-		ether_addr_copy(bssid->MacAddress, hdr->addr2);
+	if (capab_info & BIT(0)) {
+		bssid->ifmode = NL80211_IFTYPE_STATION;
+		ether_addr_copy(bssid->MacAddress, mgmt->sa);
 	} else {
-		bssid->InfrastructureMode = Ndis802_11IBSS;
-		ether_addr_copy(bssid->MacAddress, hdr->addr3);
+		bssid->ifmode = NL80211_IFTYPE_ADHOC;
+		ether_addr_copy(bssid->MacAddress, mgmt->bssid);
 	}
 
-	if (val16 & BIT(4))
+	if (capab_info & BIT(4))
 		bssid->Privacy = 1;
 	else
 		bssid->Privacy = 0;
 
-	bssid->Configuration.ATIMWindow = 0;
+	bssid->ATIMWindow = 0;
 
 	/* 20/40 BSS Coexistence check */
-	if ((pregistrypriv->wifi_spec == 1) && (false == pmlmeinfo->bwmode_updated))
-	{
+	if (pregistrypriv->wifi_spec == 1 &&
+	    pmlmeinfo->bwmode_updated == false) {
 		struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-		p = rtw_get_ie23a(bssid->IEs + ie_offset, _HT_CAPABILITY_IE_, &len, bssid->IELength - ie_offset);
-		if (p && len > 0) {
-			struct HT_caps_element	*pHT_caps;
-			pHT_caps = (struct HT_caps_element	*)(p + 2);
+		p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY,
+				     bssid->IEs + ie_offset,
+				     bssid->IELength - ie_offset);
+		if (p && p[1] > 0) {
+			struct HT_caps_element *pHT_caps;
+			pHT_caps = (struct HT_caps_element *)(p + 2);
 
 			if (pHT_caps->u.HT_cap_element.HT_caps_info & BIT(14))
 				pmlmepriv->num_FortyMHzIntolerant++;
 		} else
-		{
 			pmlmepriv->num_sta_no_ht++;
-		}
 	}
 
 
 	/*  mark bss info receving from nearby channel as SignalQuality 101 */
-	if (bssid->Configuration.DSConfig != rtw_get_oper_ch23a(padapter))
+	if (bssid->DSConfig != rtw_get_oper_ch23a(padapter))
 		bssid->PhyInfo.SignalQuality = 101;
 
 	return _SUCCESS;
 }
 
-void start_create_ibss23a(struct rtw_adapter* padapter)
+static void start_create_ibss(struct rtw_adapter* padapter)
 {
 	unsigned short	caps;
-	u8	val8;
-	u8	join_type;
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
-	pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
+	pmlmeext->cur_channel = (u8)pnetwork->DSConfig;
 	pmlmeinfo->bcn_interval = get_beacon_interval23a(pnetwork);
 
 	/* update wireless mode */
@@ -7983,16 +4666,14 @@ void start_create_ibss23a(struct rtw_adapter* padapter)
 	/* udpate capability */
 	caps = rtw_get_capability23a(pnetwork);
 	update_capinfo23a(padapter, caps);
-	if (caps&cap_IBSS)/* adhoc master */
-	{
-		val8 = 0xcf;
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+	if (caps & WLAN_CAPABILITY_IBSS) {	/* adhoc master */
+		rtl8723a_set_sec_cfg(padapter, 0xcf);
 
 		/* switch channel */
 		/* SelectChannel23a(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE); */
 		set_channel_bwmode23a(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
 
-		beacon_timing_control23a(padapter);
+		rtl8723a_SetBeaconRelatedRegisters(padapter);
 
 		/* set msr to WIFI_FW_ADHOC_STATE */
 		pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
@@ -8008,9 +4689,8 @@ void start_create_ibss23a(struct rtw_adapter* padapter)
 		}
 		else
 		{
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_BSSID, padapter->registrypriv.dev_network.MacAddress);
-			join_type = 0;
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+			hw_var_set_bssid(padapter, padapter->registrypriv.dev_network.MacAddress);
+			hw_var_set_mlme_join(padapter, 0);
 
 			report_join_res23a(padapter, 1);
 			pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
@@ -8018,12 +4698,12 @@ void start_create_ibss23a(struct rtw_adapter* padapter)
 	}
 	else
 	{
-		DBG_8723A("start_create_ibss23a, invalid cap:%x\n", caps);
+		DBG_8723A("%s: invalid cap:%x\n", __func__, caps);
 		return;
 	}
 }
 
-void start_clnt_join23a(struct rtw_adapter* padapter)
+static void start_clnt_join(struct rtw_adapter* padapter)
 {
 	unsigned short	caps;
 	u8	val8;
@@ -8032,7 +4712,7 @@ void start_clnt_join23a(struct rtw_adapter* padapter)
 	struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
 	int beacon_timeout;
 
-	pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
+	pmlmeext->cur_channel = (u8)pnetwork->DSConfig;
 	pmlmeinfo->bcn_interval = get_beacon_interval23a(pnetwork);
 
 	/* update wireless mode */
@@ -8041,15 +4721,16 @@ void start_clnt_join23a(struct rtw_adapter* padapter)
 	/* udpate capability */
 	caps = rtw_get_capability23a(pnetwork);
 	update_capinfo23a(padapter, caps);
-	if (caps&cap_ESS) {
+	if (caps & WLAN_CAPABILITY_ESS) {
 		/* switch channel */
 		set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
 
 		Set_MSR23a(padapter, WIFI_FW_STATION_STATE);
 
-		val8 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X)? 0xcc: 0xcf;
+		val8 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X) ?
+			0xcc: 0xcf;
 
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+		rtl8723a_set_sec_cfg(padapter, val8);
 
 		/* switch channel */
 		/* set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */
@@ -8061,18 +4742,15 @@ void start_clnt_join23a(struct rtw_adapter* padapter)
 		mod_timer(&padapter->mlmepriv.assoc_timer, jiffies +
 			  msecs_to_jiffies((REAUTH_TO * REAUTH_LIMIT) + (REASSOC_TO*REASSOC_LIMIT) + beacon_timeout));
 		pmlmeinfo->state = WIFI_FW_AUTH_NULL | WIFI_FW_STATION_STATE;
-	}
-	else if (caps&cap_IBSS) /* adhoc client */
-	{
+	} else if (caps & WLAN_CAPABILITY_IBSS) {	/* adhoc client */
 		Set_MSR23a(padapter, WIFI_FW_ADHOC_STATE);
 
-		val8 = 0xcf;
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+		rtl8723a_set_sec_cfg(padapter, 0xcf);
 
 		/* switch channel */
 		set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
 
-		beacon_timing_control23a(padapter);
+		rtl8723a_SetBeaconRelatedRegisters(padapter);
 
 		pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
 
@@ -8085,7 +4763,7 @@ void start_clnt_join23a(struct rtw_adapter* padapter)
 	}
 }
 
-void start_clnt_auth23a(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;
@@ -8109,12 +4787,12 @@ void start_clnt_auth23a(struct rtw_adapter* padapter)
 	issue_deauth23a(padapter, (&pmlmeinfo->network)->MacAddress, WLAN_REASON_DEAUTH_LEAVING);
 
 	DBG_8723A_LEVEL(_drv_always_, "start auth\n");
-	issue_auth23a(padapter, NULL, 0);
+	issue_auth(padapter, NULL, 0);
 
 	set_link_timer(pmlmeext, REAUTH_TO);
 }
 
-void start_clnt_assoc23a(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;
@@ -8124,12 +4802,13 @@ void start_clnt_assoc23a(struct rtw_adapter* padapter)
 	pmlmeinfo->state &= (~(WIFI_FW_AUTH_NULL | WIFI_FW_AUTH_STATE));
 	pmlmeinfo->state |= (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE);
 
-	issue_assocreq23a(padapter);
+	issue_assocreq(padapter);
 
 	set_link_timer(pmlmeext, REASSOC_TO);
 }
 
-unsigned int receive_disconnect23a(struct rtw_adapter *padapter, unsigned char *MacAddr, unsigned short reason)
+int receive_disconnect23a(struct rtw_adapter *padapter,
+			  unsigned char *MacAddr, unsigned short reason)
 {
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
@@ -8158,7 +4837,8 @@ unsigned int receive_disconnect23a(struct rtw_adapter *padapter, unsigned char *
 	return _SUCCESS;
 }
 
-static void process_80211d(struct rtw_adapter *padapter, struct wlan_bssid_ex *bssid)
+static void process_80211d(struct rtw_adapter *padapter,
+			   struct wlan_bssid_ex *bssid)
 {
 	struct registry_priv *pregistrypriv;
 	struct mlme_ext_priv *pmlmeext;
@@ -8171,10 +4851,8 @@ static void process_80211d(struct rtw_adapter *padapter, struct wlan_bssid_ex *b
 
 	/*  Adjust channel plan by AP Country IE */
 	if (pregistrypriv->enable80211d &&
-		(!pmlmeext->update_channel_plan_by_ap_done))
-	{
-		u8 *ie, *p;
-		u32 len;
+	    !pmlmeext->update_channel_plan_by_ap_done) {
+		const u8 *ie, *p;
 		struct rt_channel_plan chplan_ap;
 		struct rt_channel_info chplan_sta[MAX_CHANNEL_NUM];
 		u8 country[4];
@@ -8182,31 +4860,34 @@ static void process_80211d(struct rtw_adapter *padapter, struct wlan_bssid_ex *b
 		u8 noc; /*  number of channel */
 		u8 j, k;
 
-		ie = rtw_get_ie23a(bssid->IEs + _FIXED_IE_LENGTH_, _COUNTRY_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
-		if (!ie) return;
-		if (len < 6) return;
+		ie = cfg80211_find_ie(WLAN_EID_COUNTRY,
+				      bssid->IEs + _FIXED_IE_LENGTH_,
+				      bssid->IELength - _FIXED_IE_LENGTH_);
+		if (!ie || ie[1] < IEEE80211_COUNTRY_IE_MIN_LEN)
+			return;
 
+		p = ie + 2;
+		ie += ie[1];
 		ie += 2;
-		p = ie;
-		ie += len;
 
-		memset(country, 0, 4);
 		memcpy(country, p, 3);
+		country[3] = '\0';
+
 		p += 3;
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
-				("%s: 802.11d country =%s\n", __func__, country));
+			 ("%s: 802.11d country =%s\n", __func__, country));
 
 		i = 0;
-		while ((ie - p) >= 3)
-		{
+		while ((ie - p) >= 3) {
 			fcn = *(p++);
 			noc = *(p++);
 			p++;
 
-			for (j = 0; j < noc; j++)
-			{
-				if (fcn <= 14) channel = fcn + j; /*  2.4 GHz */
-				else channel = fcn + j*4; /*  5 GHz */
+			for (j = 0; j < noc; j++) {
+				if (fcn <= 14)
+					channel = fcn + j; /*  2.4 GHz */
+				else
+					channel = fcn + j * 4; /*  5 GHz */
 
 				chplan_ap.Channel[i++] = channel;
 			}
@@ -8220,45 +4901,55 @@ static void process_80211d(struct rtw_adapter *padapter, struct wlan_bssid_ex *b
 		i = j = k = 0;
 		if (pregistrypriv->wireless_mode & WIRELESS_11G) {
 			do {
-				if ((i == MAX_CHANNEL_NUM) ||
-					(chplan_sta[i].ChannelNum == 0) ||
-					(chplan_sta[i].ChannelNum > 14))
+				if (i == MAX_CHANNEL_NUM ||
+				    chplan_sta[i].ChannelNum == 0 ||
+				    chplan_sta[i].ChannelNum > 14)
 					break;
 
-				if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] > 14))
+				if (j == chplan_ap.Len ||
+				    chplan_ap.Channel[j] > 14)
 					break;
 
-				if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j]) {
-					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+				if (chplan_sta[i].ChannelNum ==
+				    chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum =
+						chplan_ap.Channel[j];
 					chplan_new[k].ScanType = SCAN_ACTIVE;
 					i++;
 					j++;
 					k++;
-				} else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j]) {
-					chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
-					chplan_new[k].ScanType = SCAN_PASSIVE;
+				} else if (chplan_sta[i].ChannelNum <
+					   chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum =
+						chplan_sta[i].ChannelNum;
+					chplan_new[k].ScanType =
+						SCAN_PASSIVE;
 					i++;
 					k++;
-				} else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j]) {
-					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
-					chplan_new[k].ScanType = SCAN_ACTIVE;
+				} else if (chplan_sta[i].ChannelNum >
+					   chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum =
+						chplan_ap.Channel[j];
+					chplan_new[k].ScanType =
+						SCAN_ACTIVE;
 					j++;
 					k++;
 				}
 			} while (1);
 
 			/*  change AP not support channel to Passive scan */
-			while ((i < MAX_CHANNEL_NUM) &&
-				(chplan_sta[i].ChannelNum != 0) &&
-				(chplan_sta[i].ChannelNum <= 14)) {
-				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+			while (i < MAX_CHANNEL_NUM &&
+			       chplan_sta[i].ChannelNum != 0 &&
+			       chplan_sta[i].ChannelNum <= 14) {
+				chplan_new[k].ChannelNum =
+					chplan_sta[i].ChannelNum;
 				chplan_new[k].ScanType = SCAN_PASSIVE;
 				i++;
 				k++;
 			}
 
 			/*  add channel AP supported */
-			while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14)) {
+			while (j < chplan_ap.Len && chplan_ap.Channel[j] <= 14){
 				chplan_new[k].ChannelNum = chplan_ap.Channel[j];
 				chplan_new[k].ScanType = SCAN_ACTIVE;
 				j++;
@@ -8266,49 +4957,50 @@ static void process_80211d(struct rtw_adapter *padapter, struct wlan_bssid_ex *b
 			}
 		} else {
 			/*  keep original STA 2.4G channel plan */
-			while ((i < MAX_CHANNEL_NUM) &&
-				(chplan_sta[i].ChannelNum != 0) &&
-				(chplan_sta[i].ChannelNum <= 14)) {
-				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+			while (i < MAX_CHANNEL_NUM &&
+			       chplan_sta[i].ChannelNum != 0 &&
+			       chplan_sta[i].ChannelNum <= 14) {
+				chplan_new[k].ChannelNum =
+					chplan_sta[i].ChannelNum;
 				chplan_new[k].ScanType = chplan_sta[i].ScanType;
 				i++;
 				k++;
 			}
 
 			/*  skip AP 2.4G channel plan */
-			while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14)) {
+			while (j < chplan_ap.Len && chplan_ap.Channel[j] <= 14)
 				j++;
-			}
 		}
 
 		if (pregistrypriv->wireless_mode & WIRELESS_11A) {
 			do {
-				if ((i == MAX_CHANNEL_NUM) ||
-				    (chplan_sta[i].ChannelNum == 0))
+				if (i == MAX_CHANNEL_NUM ||
+				    chplan_sta[i].ChannelNum == 0)
 					break;
 
-				if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] == 0))
+				if (j == chplan_ap.Len ||
+				    chplan_ap.Channel[j] == 0)
 					break;
 
-				if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j])
-				{
-					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+				if (chplan_sta[i].ChannelNum ==
+				    chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum =
+						chplan_ap.Channel[j];
 					chplan_new[k].ScanType = SCAN_ACTIVE;
 					i++;
 					j++;
 					k++;
-				}
-				else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j])
-				{
-					chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
-/*					chplan_new[k].ScanType = chplan_sta[i].ScanType; */
+				} else if (chplan_sta[i].ChannelNum <
+					   chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum =
+						chplan_sta[i].ChannelNum;
 					chplan_new[k].ScanType = SCAN_PASSIVE;
 					i++;
 					k++;
-				}
-				else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j])
-				{
-					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+				} else if (chplan_sta[i].ChannelNum >
+					   chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum =
+						chplan_ap.Channel[j];
 					chplan_new[k].ScanType = SCAN_ACTIVE;
 					j++;
 					k++;
@@ -8316,15 +5008,17 @@ static void process_80211d(struct rtw_adapter *padapter, struct wlan_bssid_ex *b
 			} while (1);
 
 			/*  change AP not support channel to Passive scan */
-			while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0)) {
-				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+			while (i < MAX_CHANNEL_NUM &&
+			       chplan_sta[i].ChannelNum != 0) {
+				chplan_new[k].ChannelNum =
+					chplan_sta[i].ChannelNum;
 				chplan_new[k].ScanType = SCAN_PASSIVE;
 				i++;
 				k++;
 			}
 
 			/*  add channel AP supported */
-			while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] != 0)) {
+			while (j < chplan_ap.Len && chplan_ap.Channel[j] != 0) {
 				chplan_new[k].ChannelNum = chplan_ap.Channel[j];
 				chplan_new[k].ScanType = SCAN_ACTIVE;
 				j++;
@@ -8332,8 +5026,10 @@ static void process_80211d(struct rtw_adapter *padapter, struct wlan_bssid_ex *b
 			}
 		} else {
 			/*  keep original STA 5G channel plan */
-			while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0)) {
-				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+			while (i < MAX_CHANNEL_NUM &&
+			       chplan_sta[i].ChannelNum != 0) {
+				chplan_new[k].ChannelNum =
+					chplan_sta[i].ChannelNum;
 				chplan_new[k].ScanType = chplan_sta[i].ScanType;
 				i++;
 				k++;
@@ -8343,21 +5039,22 @@ static void process_80211d(struct rtw_adapter *padapter, struct wlan_bssid_ex *b
 	}
 
 	/*  If channel is used by AP, set channel scan type to active */
-	channel = bssid->Configuration.DSConfig;
+	channel = bssid->DSConfig;
 	chplan_new = pmlmeext->channel_set;
 	i = 0;
-	while ((i < MAX_CHANNEL_NUM) && (chplan_new[i].ChannelNum != 0)) {
-		if (chplan_new[i].ChannelNum == channel)
-		{
+	while (i < MAX_CHANNEL_NUM && chplan_new[i].ChannelNum != 0) {
+		if (chplan_new[i].ChannelNum == channel) {
 			if (chplan_new[i].ScanType == SCAN_PASSIVE) {
-				/* 5G Bnad 2, 3 (DFS) doesn't change to active scan */
+				/* 5G Bnad 2, 3 (DFS) doesn't change
+				   to active scan */
 				if (channel >= 52 && channel <= 144)
 					break;
 
 				chplan_new[i].ScanType = SCAN_ACTIVE;
 				RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
-						 ("%s: change channel %d scan type from passive to active\n",
-						  __func__, channel));
+					 ("%s: change channel %d scan type "
+					  "from passive to active\n",
+					  __func__, channel));
 			}
 			break;
 		}
@@ -8399,8 +5096,6 @@ void report_survey_event23a(struct rtw_adapter *padapter, struct recv_frame *pre
 		return;
 	}
 
-	INIT_LIST_HEAD(&pcmd_obj->list);
-
 	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
 	pcmd_obj->cmdsz = cmdsz;
 	pcmd_obj->parmbuf = pevtcmd;
@@ -8452,8 +5147,6 @@ void report_surveydone_event23a(struct rtw_adapter *padapter)
 		return;
 	}
 
-	INIT_LIST_HEAD(&pcmd_obj->list);
-
 	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
 	pcmd_obj->cmdsz = cmdsz;
 	pcmd_obj->parmbuf = pevtcmd;
@@ -8499,8 +5192,6 @@ void report_join_res23a(struct rtw_adapter *padapter, int res)
 		return;
 	}
 
-	INIT_LIST_HEAD(&pcmd_obj->list);
-
 	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
 	pcmd_obj->cmdsz = cmdsz;
 	pcmd_obj->parmbuf = pevtcmd;
@@ -8551,8 +5242,6 @@ void report_del_sta_event23a(struct rtw_adapter *padapter, unsigned char* MacAdd
 		return;
 	}
 
-	INIT_LIST_HEAD(&pcmd_obj->list);
-
 	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
 	pcmd_obj->cmdsz = cmdsz;
 	pcmd_obj->parmbuf = pevtcmd;
@@ -8607,8 +5296,6 @@ void report_add_sta_event23a(struct rtw_adapter *padapter, unsigned char* MacAdd
 		return;
 	}
 
-	INIT_LIST_HEAD(&pcmd_obj->list);
-
 	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
 	pcmd_obj->cmdsz = cmdsz;
 	pcmd_obj->parmbuf = pevtcmd;
@@ -8678,7 +5365,7 @@ void update_sta_info23a(struct rtw_adapter *padapter, struct sta_info *psta)
 	psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */
 
 	/* QoS */
-	if (pmlmepriv->qospriv.qos_option)
+	if (pmlmepriv->qos_option)
 		psta->qos_option = true;
 
 	psta->state = _FW_LINKED;
@@ -8691,17 +5378,14 @@ void mlmeext_joinbss_event_callback23a(struct rtw_adapter *padapter, int join_re
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
 	struct sta_priv		*pstapriv = &padapter->stapriv;
-	u8	join_type;
-	u16 media_status;
 
-	if (join_res < 0)
-	{
-		join_type = 1;
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_BSSID, null_addr);
+	if (join_res < 0) {
+		hw_var_set_mlme_join(padapter, 1);
+		hw_var_set_bssid(padapter, null_addr);
 
 		/* restore to initial setting. */
-		update_tx_basic_rate23a(padapter, padapter->registrypriv.wireless_mode);
+		update_tx_basic_rate23a(padapter,
+					padapter->registrypriv.wireless_mode);
 
 		goto exit_mlmeext_joinbss_event_callback23a;
 	}
@@ -8719,15 +5403,15 @@ void mlmeext_joinbss_event_callback23a(struct rtw_adapter *padapter, int join_re
 	}
 
 	/* turn on dynamic functions */
-	Switch_DM_Func23a(padapter, DYNAMIC_ALL_FUNC_ENABLE, true);
+	rtl8723a_odm_support_ability_set(padapter, DYNAMIC_ALL_FUNC_ENABLE);
 
 	/*  update IOT-releated issue */
 	update_IOT_info23a(padapter);
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_BASIC_RATE, cur_network->SupportedRates);
+	HalSetBrateCfg23a(padapter, cur_network->SupportedRates);
 
 	/* BCN interval */
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&pmlmeinfo->bcn_interval));
+	rtl8723a_set_beacon_interval(padapter, pmlmeinfo->bcn_interval);
 
 	/* udpate capability */
 	update_capinfo23a(padapter, pmlmeinfo->capability);
@@ -8752,18 +5436,13 @@ void mlmeext_joinbss_event_callback23a(struct rtw_adapter *padapter, int join_re
 
 		/* set per sta rate after updating HT cap. */
 		set_sta_rate23a(padapter, psta);
-
-		media_status = (psta->mac_id<<8)|1; /*   MACID|OPMODE: 1 means connect */
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_MEDIA_STATUS_RPT, (u8 *)&media_status);
 	}
 
-	join_type = 2;
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+	hw_var_set_mlme_join(padapter, 2);
 
-	if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
-	{
+	if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) {
 		/*  correcting TSF */
-		correct_TSF23a(padapter, pmlmeext);
+		rtw_correct_TSF(padapter);
 
 		/* set_link_timer(pmlmeext, DISCONNECT_TO); */
 	}
@@ -8778,27 +5457,20 @@ void mlmeext_sta_add_event_callback23a(struct rtw_adapter *padapter, struct sta_
 {
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	u8	join_type;
 
 	DBG_8723A("%s\n", __func__);
 
-	if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
-	{
-		if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)/* adhoc master or sta_count>1 */
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) {
+	/* adhoc master or sta_count>1 */
+		if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
 		{
 			/* nothing to do */
-		}
-		else/* adhoc client */
-		{
-			/* update TSF Value */
-			/* update_TSF23a(pmlmeext, pframe, len); */
-
+		} else { /* adhoc client */
 			/*  correcting TSF */
-			correct_TSF23a(padapter, pmlmeext);
+			rtw_correct_TSF(padapter);
 
 			/* start beacon */
-			if (send_beacon23a(padapter) == _FAIL)
-			{
+			if (send_beacon23a(padapter) != _SUCCESS) {
 				pmlmeinfo->FW_sta_info[psta->mac_id].status = 0;
 
 				pmlmeinfo->state ^= WIFI_FW_ADHOC_STATE;
@@ -8807,11 +5479,8 @@ void mlmeext_sta_add_event_callback23a(struct rtw_adapter *padapter, struct sta_
 			}
 
 			pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
-
 		}
-
-		join_type = 2;
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+		hw_var_set_mlme_join(padapter, 2);
 	}
 
 	pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
@@ -8825,25 +5494,27 @@ void mlmeext_sta_add_event_callback23a(struct rtw_adapter *padapter, struct sta_
 
 void mlmeext_sta_del_event_callback23a(struct rtw_adapter *padapter)
 {
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
-	if (is_client_associated_to_ap23a(padapter) || is_IBSS_empty23a(padapter))
-	{
+	if (is_client_associated_to_ap23a(padapter) ||
+	    is_IBSS_empty23a(padapter)) {
 		/* set_opmode_cmd(padapter, infra_client_with_mlme); */
 
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_DISCONNECT, NULL);
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_BSSID, null_addr);
+		hw_var_set_mlme_disconnect(padapter);
+		hw_var_set_bssid(padapter, null_addr);
 
 		/* restore to initial setting. */
-		update_tx_basic_rate23a(padapter, padapter->registrypriv.wireless_mode);
+		update_tx_basic_rate23a(padapter,
+					padapter->registrypriv.wireless_mode);
 
 		/* switch to the 20M Hz mode after disconnect */
 		pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
 		pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 
-		/* SelectChannel23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset); */
-		set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+		set_channel_bwmode23a(padapter, pmlmeext->cur_channel,
+				      pmlmeext->cur_ch_offset,
+				      pmlmeext->cur_bwmode);
 
 		flush_all_cam_entry23a(padapter);
 
@@ -8856,33 +5527,11 @@ void mlmeext_sta_del_event_callback23a(struct rtw_adapter *padapter)
 	}
 }
 
-/****************************************************************************
-
-Following are the functions for the timer handlers
-
-*****************************************************************************/
-void linked23a_rx_sig_stren_disp(struct rtw_adapter *padapter)
-{
-	struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
-	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	u8 mac_id;
-	int UndecoratedSmoothedPWDB;
-	if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
-		mac_id = 0;
-	else if ((pmlmeinfo->state&0x03) == _HW_STATE_AP_)
-		mac_id = 2;
-
-	rtw_hal_get_def_var23a(padapter, HW_DEF_RA_INFO_DUMP,&mac_id);
-
-	rtw_hal_get_def_var23a(padapter, HAL_DEF_UNDERCORATEDSMOOTHEDPWDB, &UndecoratedSmoothedPWDB);
-	DBG_8723A("UndecoratedSmoothedPWDB:%d\n", UndecoratedSmoothedPWDB);
-}
-
 static u8 chk_ap_is_alive(struct rtw_adapter *padapter, struct sta_info *psta)
 {
 	u8 ret = false;
 
-	if ((sta_rx_data_pkts(psta) == sta_last_rx_data_pkts(psta)) &&
+	if (sta_rx_data_pkts(psta) == sta_last_rx_data_pkts(psta) &&
 	    sta_rx_beacon_pkts(psta) == sta_last_rx_beacon_pkts(psta) &&
 	    sta_rx_probersp_pkts(psta) == sta_last_rx_probersp_pkts(psta))
 		ret = false;
@@ -8895,20 +5544,16 @@ static u8 chk_ap_is_alive(struct rtw_adapter *padapter, struct sta_info *psta)
 
 void linked_status_chk23a(struct rtw_adapter *padapter)
 {
-	u32	i;
-	struct sta_info		*psta;
+	u32 i;
+	struct sta_info *psta;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	struct sta_priv		*pstapriv = &padapter->stapriv;
-
-	if (padapter->bRxRSSIDisplay)
-		 linked23a_rx_sig_stren_disp(padapter);
+	struct sta_priv *pstapriv = &padapter->stapriv;
 
-	rtw_hal_sreset_linked_status_check23a(padapter);
+	rtl8723a_sreset_linked_status_check(padapter);
 
-	if (is_client_associated_to_ap23a(padapter))
-	{
+	if (is_client_associated_to_ap23a(padapter)) {
 		/* linked infrastructure client mode */
 
 		int tx_chk = _SUCCESS, rx_chk = _SUCCESS;
@@ -8916,12 +5561,10 @@ void linked_status_chk23a(struct rtw_adapter *padapter)
 
 		rx_chk_limit = 4;
 
-		if ((psta = rtw_get_stainfo23a(pstapriv, pmlmeinfo->network.MacAddress)) != NULL)
-		{
+		psta = rtw_get_stainfo23a(pstapriv,
+					  pmlmeinfo->network.MacAddress);
+		if (psta) {
 			bool is_p2p_enable = false;
-#ifdef CONFIG_8723AU_P2P
-			is_p2p_enable = !rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE);
-#endif
 
 			if (chk_ap_is_alive(padapter, psta) == false)
 				rx_chk = _FAIL;
@@ -8929,102 +5572,108 @@ void linked_status_chk23a(struct rtw_adapter *padapter)
 			if (pxmitpriv->last_tx_pkts == pxmitpriv->tx_pkts)
 				tx_chk = _FAIL;
 
-			if (pmlmeext->active_keep_alive_check && (rx_chk == _FAIL || tx_chk == _FAIL)) {
+			if (pmlmeext->active_keep_alive_check &&
+			    (rx_chk == _FAIL || tx_chk == _FAIL)) {
 				u8 backup_oper_channel = 0;
 
-				/* switch to correct channel of current network  before issue keep-alive frames */
-				if (rtw_get_oper_ch23a(padapter) != pmlmeext->cur_channel) {
-					backup_oper_channel = rtw_get_oper_ch23a(padapter);
-					SelectChannel23a(padapter, pmlmeext->cur_channel);
+				/* switch to correct channel of current
+				   network  before issue keep-alive frames */
+				if (rtw_get_oper_ch23a(padapter) !=
+				    pmlmeext->cur_channel) {
+					backup_oper_channel =
+						rtw_get_oper_ch23a(padapter);
+					SelectChannel23a(padapter,
+							 pmlmeext->cur_channel);
 				}
 
 				if (rx_chk != _SUCCESS)
-					issue_probereq23a_ex23a(padapter, &pmlmeinfo->network.Ssid, psta->hwaddr, 3, 1);
-
-				if ((tx_chk != _SUCCESS && pmlmeinfo->link_count++ == 0xf) || rx_chk != _SUCCESS) {
-					tx_chk = issue_nulldata23a(padapter, psta->hwaddr, 0, 3, 1);
-					/* if tx acked and p2p disabled, set rx_chk _SUCCESS to reset retry count */
-					if (tx_chk == _SUCCESS && !is_p2p_enable)
+					issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, psta->hwaddr, 3, 1);
+
+				if ((tx_chk != _SUCCESS &&
+				     pmlmeinfo->link_count++ == 0xf) ||
+				    rx_chk != _SUCCESS) {
+					tx_chk = issue_nulldata23a(padapter,
+								   psta->hwaddr,
+								   0, 3, 1);
+					/* if tx acked and p2p disabled,
+					   set rx_chk _SUCCESS to reset retry
+					   count */
+					if (tx_chk == _SUCCESS &&
+					    !is_p2p_enable)
 						rx_chk = _SUCCESS;
 				}
 
 				/* back to the original operation channel */
 				if (backup_oper_channel>0)
-					SelectChannel23a(padapter, backup_oper_channel);
-
+					SelectChannel23a(padapter,
+							 backup_oper_channel);
 			} else {
 				if (rx_chk != _SUCCESS) {
 					if (pmlmeext->retry == 0) {
-						issue_probereq23a(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
-						issue_probereq23a(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
-						issue_probereq23a(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
+						issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
+						issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
+						issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
 					}
 				}
 
-				if (tx_chk != _SUCCESS && pmlmeinfo->link_count++ == 0xf)
-					tx_chk = issue_nulldata23a(padapter, NULL, 0, 1, 0);
+				if (tx_chk != _SUCCESS &&
+				    pmlmeinfo->link_count++ == 0xf)
+					tx_chk = issue_nulldata23a(padapter,
+								   NULL, 0, 1,
+								   0);
 			}
 
 			if (rx_chk == _FAIL) {
 				pmlmeext->retry++;
 				if (pmlmeext->retry > rx_chk_limit) {
-					DBG_8723A_LEVEL(_drv_always_, FUNC_ADPT_FMT" disconnect or roaming\n",
-						FUNC_ADPT_ARG(padapter));
+					DBG_8723A_LEVEL(_drv_always_,
+							"%s(%s): disconnect or "
+							"roaming\n", __func__,
+							padapter->pnetdev->name);
 					receive_disconnect23a(padapter, pmlmeinfo->network.MacAddress,
 						WLAN_REASON_EXPIRATION_CHK);
 					return;
 				}
-			} else {
+			} else
 				pmlmeext->retry = 0;
-			}
 
-			if (tx_chk == _FAIL) {
+			if (tx_chk == _FAIL)
 				pmlmeinfo->link_count &= 0xf;
-			} else {
+			else {
 				pxmitpriv->last_tx_pkts = pxmitpriv->tx_pkts;
 				pmlmeinfo->link_count = 0;
 			}
 
-		} /* end of if ((psta = rtw_get_stainfo23a(pstapriv, passoc_res->network.MacAddress)) != NULL) */
-	}
-	else if (is_client_associated_to_ibss23a(padapter))
-	{
+		}
+	} else if (is_client_associated_to_ibss23a(padapter)) {
 		/* linked IBSS mode */
 		/* for each assoc list entry to check the rx pkt counter */
-		for (i = IBSS_START_MAC_ID; i < NUM_STA; i++)
-		{
-			if (pmlmeinfo->FW_sta_info[i].status == 1)
-			{
+		for (i = IBSS_START_MAC_ID; i < NUM_STA; i++) {
+			if (pmlmeinfo->FW_sta_info[i].status == 1) {
 				psta = pmlmeinfo->FW_sta_info[i].psta;
 
-				if (NULL == psta) continue;
+				if (!psta)
+					continue;
 
-				if (pmlmeinfo->FW_sta_info[i].rx_pkt == sta_rx_pkts(psta))
-				{
+				if (pmlmeinfo->FW_sta_info[i].rx_pkt ==
+				    sta_rx_pkts(psta)) {
 
-					if (pmlmeinfo->FW_sta_info[i].retry<3)
-					{
+					if (pmlmeinfo->FW_sta_info[i].retry<3) {
 						pmlmeinfo->FW_sta_info[i].retry++;
-					}
-					else
-					{
+					} else {
 						pmlmeinfo->FW_sta_info[i].retry = 0;
 						pmlmeinfo->FW_sta_info[i].status = 0;
 						report_del_sta_event23a(padapter, psta->hwaddr,
 							65535/*  indicate disconnect caused by no rx */
 						);
 					}
-				}
-				else
-				{
+				} else {
 					pmlmeinfo->FW_sta_info[i].retry = 0;
 					pmlmeinfo->FW_sta_info[i].rx_pkt = (u32)sta_rx_pkts(psta);
 				}
 			}
 		}
-
 		/* set_link_timer(pmlmeext, DISCONNECT_TO); */
-
 	}
 }
 
@@ -9035,32 +5684,17 @@ static void survey_timer_hdl(unsigned long data)
 	struct sitesurvey_parm *psurveyPara;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif
 
 	/* issue rtw_sitesurvey_cmd23a */
 	if (pmlmeext->sitesurvey_res.state > SCAN_START) {
 		if (pmlmeext->sitesurvey_res.state ==  SCAN_PROCESS)
 			pmlmeext->sitesurvey_res.channel_idx++;
 
-		if (pmlmeext->scan_abort == true)
-		{
-#ifdef CONFIG_8723AU_P2P
-			if (!rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE))
-			{
-				rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_MAX);
-				pmlmeext->sitesurvey_res.channel_idx = 3;
-				DBG_8723A("%s idx:%d, cnt:%u\n", __func__,
-					  pmlmeext->sitesurvey_res.channel_idx,
-					  pwdinfo->find_phase_state_exchange_cnt);
-			} else
-			#endif
-			{
-				pmlmeext->sitesurvey_res.channel_idx = pmlmeext->sitesurvey_res.ch_num;
-				DBG_8723A("%s idx:%d\n", __func__,
-					  pmlmeext->sitesurvey_res.channel_idx);
-			}
+		if (pmlmeext->scan_abort == true) {
+			pmlmeext->sitesurvey_res.channel_idx =
+				pmlmeext->sitesurvey_res.ch_num;
+			DBG_8723A("%s idx:%d\n", __func__,
+				  pmlmeext->sitesurvey_res.channel_idx);
 
 			pmlmeext->scan_abort = false;/* reset */
 		}
@@ -9077,7 +5711,8 @@ static void survey_timer_hdl(unsigned long data)
 			goto exit_survey_timer_hdl;
 		}
 
-		init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, GEN_CMD_CODE(_SiteSurvey));
+		init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara,
+					   GEN_CMD_CODE(_SiteSurvey));
 		rtw_enqueue_cmd23a(pcmdpriv, ph2c);
 	}
 
@@ -9095,17 +5730,13 @@ static void link_timer_hdl(unsigned long data)
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	/* struct sta_priv		*pstapriv = &padapter->stapriv; */
 
-	if (pmlmeinfo->state & WIFI_FW_AUTH_NULL)
-	{
+	if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
 		DBG_8723A("link_timer_hdl:no beacon while connecting\n");
 		pmlmeinfo->state = WIFI_FW_NULL_STATE;
 		report_join_res23a(padapter, -3);
-	}
-	else if (pmlmeinfo->state & WIFI_FW_AUTH_STATE)
-	{
+	} else if (pmlmeinfo->state & WIFI_FW_AUTH_STATE) {
 		/* re-auth timer */
-		if (++pmlmeinfo->reauth_count > REAUTH_LIMIT)
-		{
+		if (++pmlmeinfo->reauth_count > REAUTH_LIMIT) {
 			/* if (pmlmeinfo->auth_algo != dot11AuthAlgrthm_Auto) */
 			/*  */
 				pmlmeinfo->state = 0;
@@ -9114,28 +5745,25 @@ static void link_timer_hdl(unsigned long data)
 			/*  */
 			/* else */
 			/*  */
-			/*	pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared; */
-			/*	pmlmeinfo->reauth_count = 0; */
+			/* pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared; */
+			/* pmlmeinfo->reauth_count = 0; */
 			/*  */
 		}
 
 		DBG_8723A("link_timer_hdl: auth timeout and try again\n");
 		pmlmeinfo->auth_seq = 1;
-		issue_auth23a(padapter, NULL, 0);
+		issue_auth(padapter, NULL, 0);
 		set_link_timer(pmlmeext, REAUTH_TO);
-	}
-	else if (pmlmeinfo->state & WIFI_FW_ASSOC_STATE)
-	{
+	} else if (pmlmeinfo->state & WIFI_FW_ASSOC_STATE) {
 		/* re-assoc timer */
-		if (++pmlmeinfo->reassoc_count > REASSOC_LIMIT)
-		{
+		if (++pmlmeinfo->reassoc_count > REASSOC_LIMIT) {
 			pmlmeinfo->state = WIFI_FW_NULL_STATE;
 			report_join_res23a(padapter, -2);
 			return;
 		}
 
 		DBG_8723A("link_timer_hdl: assoc timeout and try again\n");
-		issue_assocreq23a(padapter);
+		issue_assocreq(padapter);
 		set_link_timer(pmlmeext, REASSOC_TO);
 	}
 
@@ -9152,11 +5780,9 @@ static void addba_timer_hdl(unsigned long data)
 
 	phtpriv = &psta->htpriv;
 
-	if ((phtpriv->ht_option == true) && (phtpriv->ampdu_enable == true))
-	{
+	if (phtpriv->ht_option == true && phtpriv->ampdu_enable == true) {
 		if (phtpriv->candidate_tid_bitmap)
 			phtpriv->candidate_tid_bitmap = 0x0;
-
 	}
 }
 
@@ -9177,57 +5803,58 @@ void init_mlme_ext_timer23a(struct rtw_adapter *padapter)
 		    (unsigned long)padapter);
 }
 
-u8 NULL_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
+int NULL_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
 	return H2C_SUCCESS;
 }
 
-u8 setopmode_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
+int setopmode_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
-	u8	type;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	enum nl80211_iftype type;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	struct setopmode_parm *psetop = (struct setopmode_parm *)pbuf;
+	const struct setopmode_parm *psetop = (struct setopmode_parm *)pbuf;
 
-	if (psetop->mode == Ndis802_11APMode)
-	{
+	switch (psetop->mode) {
+	case NL80211_IFTYPE_P2P_GO:
+	case NL80211_IFTYPE_AP:
 		pmlmeinfo->state = WIFI_FW_AP_STATE;
 		type = _HW_STATE_AP_;
-	}
-	else if (psetop->mode == Ndis802_11Infrastructure)
-	{
-		pmlmeinfo->state &= ~(BIT(0)|BIT(1));/*  clear state */
-		pmlmeinfo->state |= WIFI_FW_STATION_STATE;/* set to	STATION_STATE */
+		break;
+	case NL80211_IFTYPE_P2P_CLIENT:
+	case NL80211_IFTYPE_STATION:
+		/*  clear state */
+		pmlmeinfo->state &= ~(BIT(0)|BIT(1));
+		/* set to STATION_STATE */
+		pmlmeinfo->state |= WIFI_FW_STATION_STATE;
 		type = _HW_STATE_STATION_;
-	}
-	else if (psetop->mode == Ndis802_11IBSS)
-	{
+		break;
+	case NL80211_IFTYPE_ADHOC:
 		type = _HW_STATE_ADHOC_;
-	}
-	else
-	{
+		break;
+	default:
 		type = _HW_STATE_NOLINK_;
+		break;
 	}
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_SET_OPMODE, (u8 *)(&type));
+	hw_var_set_opmode(padapter, type);
 	/* Set_NETYPE0_MSR(padapter, type); */
 
 	return H2C_SUCCESS;
 }
 
-u8 createbss_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
+int createbss_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
-	struct wlan_bssid_ex *pparm = (struct wlan_bssid_ex *)pbuf;
+	const struct wlan_bssid_ex *pparm = (struct wlan_bssid_ex *)pbuf;
 	/* u32	initialgain; */
 
-	if (pparm->InfrastructureMode == Ndis802_11APMode) {
+	if (pparm->ifmode == NL80211_IFTYPE_AP ||
+	    pparm->ifmode == NL80211_IFTYPE_P2P_GO) {
 #ifdef CONFIG_8723AU_AP_MODE
-
-		if (pmlmeinfo->state == WIFI_FW_AP_STATE)
-		{
+		if (pmlmeinfo->state == WIFI_FW_AP_STATE) {
 			/* todo: */
 			return H2C_SUCCESS;
 		}
@@ -9235,7 +5862,7 @@ u8 createbss_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
 	}
 
 	/* below is for ad-hoc master */
-	if (pparm->InfrastructureMode == Ndis802_11IBSS) {
+	if (pparm->ifmode == NL80211_IFTYPE_ADHOC) {
 		rtw_joinbss_reset23a(padapter);
 
 		pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
@@ -9245,16 +5872,12 @@ u8 createbss_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
 		pmlmeinfo->HT_enable = 0;
 		pmlmeinfo->HT_caps_enable = 0;
 		pmlmeinfo->HT_info_enable = 0;
-		pmlmeinfo->agg_enable_bitmap = 0;
-		pmlmeinfo->candidate_tid_bitmap = 0;
 
 		/* disable dynamic functions, such as high power, DIG */
-		Save_DM_Func_Flag23a(padapter);
-		Switch_DM_Func23a(padapter, DYNAMIC_FUNC_DISABLE, false);
+		rtl8723a_odm_support_ability_backup(padapter);
 
-		/* config the initial gain under linking, need to write the BB registers */
-		/* initialgain = 0x1E; */
-		/* rtw_hal_set_hwreg23a(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain)); */
+		rtl8723a_odm_support_ability_clr(padapter,
+						 DYNAMIC_FUNC_DISABLE);
 
 		/* cancel link timer */
 		del_timer_sync(&pmlmeext->link_timer);
@@ -9267,31 +5890,30 @@ u8 createbss_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
 
 		memcpy(pnetwork, pparm, sizeof(struct wlan_bssid_ex));
 
-		start_create_ibss23a(padapter);
+		start_create_ibss(padapter);
 	}
 
 	return H2C_SUCCESS;
 }
 
-u8 join_cmd_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
+int join_cmd_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
-	u8	join_type;
-	struct ndis_802_11_var_ies *	pIE;
-	struct registry_priv	*pregpriv = &padapter->registrypriv;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
-	struct wlan_bssid_ex *pparm = (struct wlan_bssid_ex *)pbuf;
+	const struct wlan_bssid_ex *pparm = (struct wlan_bssid_ex *)pbuf;
 	struct HT_info_element *pht_info;
 	u32 i;
+	int bcn_fixed_size;
+	u8 *p;
         /* u32	initialgain; */
 	/* u32	acparm; */
 
 	/* check already connecting to AP or not */
-	if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
-	{
+	if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {
 		if (pmlmeinfo->state & WIFI_FW_STATION_STATE)
-			issue_deauth23a_ex23a(padapter, pnetwork->MacAddress,
+			issue_deauth_ex(padapter, pnetwork->MacAddress,
 					WLAN_REASON_DEAUTH_LEAVING, 5, 100);
 
 		pmlmeinfo->state = WIFI_FW_NULL_STATE;
@@ -9305,7 +5927,7 @@ u8 join_cmd_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
 		/* Set_MSR23a(padapter, _HW_STATE_NOLINK_); */
 		Set_MSR23a(padapter, _HW_STATE_STATION_);
 
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_DISCONNECT, NULL);
+		hw_var_set_mlme_disconnect(padapter);
 	}
 
 	rtw_joinbss_reset23a(padapter);
@@ -9317,8 +5939,6 @@ u8 join_cmd_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
 	pmlmeinfo->HT_enable = 0;
 	pmlmeinfo->HT_caps_enable = 0;
 	pmlmeinfo->HT_info_enable = 0;
-	pmlmeinfo->agg_enable_bitmap = 0;
-	pmlmeinfo->candidate_tid_bitmap = 0;
 	pmlmeinfo->bwmode_updated = false;
 	/* pmlmeinfo->assoc_AP_vendor = HT_IOT_PEER_MAX; */
 
@@ -9331,27 +5951,28 @@ u8 join_cmd_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
 	/* pmlmeinfo->assoc_AP_vendor = check_assoc_AP23a(pnetwork->IEs,
 	   pnetwork->IELength); */
 
-	for (i = sizeof(struct ndis_802_11_fixed_ies); i < pnetwork->IELength;)
-	{
-		pIE = (struct ndis_802_11_var_ies *)(pnetwork->IEs + i);
+	bcn_fixed_size = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+		offsetof(struct ieee80211_mgmt, u.beacon);
 
-		switch (pIE->ElementID)
-		{
-		case _VENDOR_SPECIFIC_IE_:/* Get WMM IE. */
-			if (!memcmp(pIE->data, WMM_OUI23A, 4))
+	for (i = bcn_fixed_size; i < pnetwork->IELength;) {
+		p = pnetwork->IEs + i;
+
+		switch (p[0]) {
+		case WLAN_EID_VENDOR_SPECIFIC:/* Get WMM IE. */
+			if (!memcmp(p + 2, WMM_OUI23A, 4))
 				pmlmeinfo->WMM_enable = 1;
 			break;
 
-		case _HT_CAPABILITY_IE_:	/* Get HT Cap IE. */
+		case WLAN_EID_HT_CAPABILITY:	/* Get HT Cap IE. */
 			pmlmeinfo->HT_caps_enable = 1;
 			break;
 
-		case _HT_EXTRA_INFO_IE_:	/* Get HT Info IE. */
+		case WLAN_EID_HT_OPERATION:	/* Get HT Info IE. */
 			pmlmeinfo->HT_info_enable = 1;
 
 			/* spec case only for cisco's ap because cisco's ap
 			 * issue assoc rsp using mcs rate @40MHz or @20MHz */
-			pht_info = (struct HT_info_element *)(pIE->data);
+			pht_info = (struct HT_info_element *)(p + 2);
 
 			if ((pregpriv->cbw40_enable) &&
 			    (pht_info->infos[0] & BIT(2))) {
@@ -9383,59 +6004,46 @@ u8 join_cmd_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
 			break;
 		}
 
-		i += (pIE->Length + 2);
+		i += (p[1] + 2);
 	}
-	/* disable dynamic functions, such as high power, DIG */
-	/* Switch_DM_Func23a(padapter, DYNAMIC_FUNC_DISABLE, false); */
-
-	/* config the initial gain under linking, need to write the BB
-	   registers */
-	/* initialgain = 0x1E; */
-	/* rtw_hal_set_hwreg23a(padapter, HW_VAR_INITIAL_GAIN,
-	   (u8 *)(&initialgain)); */
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_BSSID,
-			  pmlmeinfo->network.MacAddress);
-	join_type = 0;
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+	hw_var_set_bssid(padapter, pmlmeinfo->network.MacAddress);
+	hw_var_set_mlme_join(padapter, 0);
 
 	/* cancel link timer */
 	del_timer_sync(&pmlmeext->link_timer);
 
-	start_clnt_join23a(padapter);
+	start_clnt_join(padapter);
 
 	return H2C_SUCCESS;
 }
 
-u8 disconnect_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
+int disconnect_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
-	struct disconnect_parm *param = (struct disconnect_parm *)pbuf;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	const struct disconnect_parm *param = (struct disconnect_parm *)pbuf;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
-	u8	val8;
 
-	if (is_client_associated_to_ap23a(padapter))
-	{
-		issue_deauth23a_ex23a(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, param->deauth_timeout_ms/100, 100);
+	if (is_client_associated_to_ap23a(padapter)) {
+		issue_deauth_ex(padapter, pnetwork->MacAddress,
+				WLAN_REASON_DEAUTH_LEAVING,
+				param->deauth_timeout_ms/100, 100);
 	}
 
 	/* set_opmode_cmd(padapter, infra_client_with_mlme); */
 
 	/* pmlmeinfo->state = WIFI_FW_NULL_STATE; */
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_DISCONNECT, NULL);
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_BSSID, null_addr);
+	hw_var_set_mlme_disconnect(padapter);
+	hw_var_set_bssid(padapter, null_addr);
 
 	/* restore to initial setting. */
 	update_tx_basic_rate23a(padapter, padapter->registrypriv.wireless_mode);
 
-	if (((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) || ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE))
-	{
-		/* Stop BCN */
-		val8 = 0;
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_BCN_FUNC, (u8 *)(&val8));
-	}
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE ||
+	    (pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE)
+		rtl8723a_set_bcn_func(padapter, 0);	/* Stop BCN */
 
 	/* set MSR to no link state -> infra. mode */
 	Set_MSR23a(padapter, _HW_STATE_STATION_);
@@ -9446,7 +6054,8 @@ u8 disconnect_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
 	pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
 	pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 
-	set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+	set_channel_bwmode23a(padapter, pmlmeext->cur_channel,
+			      pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
 
 	flush_all_cam_entry23a(padapter);
 
@@ -9457,8 +6066,10 @@ u8 disconnect_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
 	return	H2C_SUCCESS;
 }
 
-static int rtw_scan_ch_decision(struct rtw_adapter *padapter, struct rtw_ieee80211_channel *out,
-	u32 out_num, struct rtw_ieee80211_channel *in, u32 in_num)
+static int
+rtw_scan_ch_decision(struct rtw_adapter *padapter,
+		     struct rtw_ieee80211_channel *out, u32 out_num,
+		     const struct rtw_ieee80211_channel *in, u32 in_num)
 {
 	int i, j;
 	int scan_ch_num = 0;
@@ -9471,15 +6082,15 @@ static int rtw_scan_ch_decision(struct rtw_adapter *padapter, struct rtw_ieee802
 	/* acquire channels from in */
 	j = 0;
 	for (i = 0;i<in_num;i++) {
-		if (0)
-		DBG_8723A(FUNC_ADPT_FMT" "CHAN_FMT"\n", FUNC_ADPT_ARG(padapter), CHAN_ARG(&in[i]));
-		if (in[i].hw_value && !(in[i].flags & IEEE80211_CHAN_DISABLED)
-			&& (set_idx = rtw_ch_set_search_ch23a(pmlmeext->channel_set, in[i].hw_value)) >= 0
-		)
-		{
-			memcpy(&out[j], &in[i], sizeof(struct rtw_ieee80211_channel));
-
-			if (pmlmeext->channel_set[set_idx].ScanType == SCAN_PASSIVE)
+		if (in[i].hw_value &&
+		    !(in[i].flags & IEEE80211_CHAN_DISABLED) &&
+		    (set_idx = rtw_ch_set_search_ch23a(pmlmeext->channel_set,
+						       in[i].hw_value)) >= 0) {
+			memcpy(&out[j], &in[i],
+			       sizeof(struct rtw_ieee80211_channel));
+
+			if (pmlmeext->channel_set[set_idx].ScanType ==
+			    SCAN_PASSIVE)
 				out[j].flags &= IEEE80211_CHAN_NO_IR;
 
 			j++;
@@ -9500,7 +6111,7 @@ static int rtw_scan_ch_decision(struct rtw_adapter *padapter, struct rtw_ieee802
 		}
 	}
 
-	if (padapter->setband == GHZ_24) {				/*  2.4G */
+	if (padapter->setband == GHZ_24) {			/*  2.4G */
 		for (i = 0; i < j ; i++) {
 			if (out[i].hw_value > 35)
 				memset(&out[i], 0,
@@ -9509,10 +6120,11 @@ static int rtw_scan_ch_decision(struct rtw_adapter *padapter, struct rtw_ieee802
 				scan_ch_num++;
 		}
 		j = scan_ch_num;
-	} else if  (padapter->setband == GHZ_50) {			/*  5G */
+	} else if  (padapter->setband == GHZ_50) {		/*  5G */
 		for (i = 0; i < j ; i++) {
 			if (out[i].hw_value > 35) {
-				memcpy(&out[scan_ch_num++], &out[i], sizeof(struct rtw_ieee80211_channel));
+				memcpy(&out[scan_ch_num++], &out[i],
+				       sizeof(struct rtw_ieee80211_channel));
 			}
 		}
 		j = scan_ch_num;
@@ -9522,19 +6134,15 @@ static int rtw_scan_ch_decision(struct rtw_adapter *padapter, struct rtw_ieee802
 	return j;
 }
 
-u8 sitesurvey_cmd_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
+int sitesurvey_cmd_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	struct sitesurvey_parm *pparm = (struct sitesurvey_parm *)pbuf;
+	const struct sitesurvey_parm *pparm = (struct sitesurvey_parm *)pbuf;
 	u8 bdelayscan = false;
-	u8 val8;
 	u32 initialgain;
 	u32 i;
 
 	if (pmlmeext->sitesurvey_res.state == SCAN_DISABLE) {
-		/* for first time sitesurvey_cmd */
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_CHECK_TXBUF, NULL);
-
 		pmlmeext->sitesurvey_res.state = SCAN_START;
 		pmlmeext->sitesurvey_res.bss_cnt = 0;
 		pmlmeext->sitesurvey_res.channel_idx = 0;
@@ -9564,8 +6172,10 @@ u8 sitesurvey_cmd_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
 
 			/* switch to correct channel of current network
 			   before issue keep-alive frames */
-			if (rtw_get_oper_ch23a(padapter) != pmlmeext->cur_channel)
-				SelectChannel23a(padapter, pmlmeext->cur_channel);
+			if (rtw_get_oper_ch23a(padapter) !=
+			    pmlmeext->cur_channel)
+				SelectChannel23a(padapter,
+						 pmlmeext->cur_channel);
 
 			issue_nulldata23a(padapter, NULL, 1, 3, 500);
 
@@ -9579,83 +6189,80 @@ u8 sitesurvey_cmd_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
 		}
 	}
 
-	if ((pmlmeext->sitesurvey_res.state == SCAN_START) ||
-	    (pmlmeext->sitesurvey_res.state == SCAN_TXNULL)) {
+	if (pmlmeext->sitesurvey_res.state == SCAN_START ||
+	    pmlmeext->sitesurvey_res.state == SCAN_TXNULL) {
 		/* disable dynamic functions, such as high power, DIG */
-		Save_DM_Func_Flag23a(padapter);
-		Switch_DM_Func23a(padapter, DYNAMIC_FUNC_DISABLE, false);
+		rtl8723a_odm_support_ability_backup(padapter);
+		rtl8723a_odm_support_ability_clr(padapter,
+						 DYNAMIC_FUNC_DISABLE);
 
 		/* config the initial gain under scaning, need to
 		   write the BB registers */
-		if ((wdev_to_priv(padapter->rtw_wdev))->p2p_enabled == true) {
+		if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled == true)
 			initialgain = 0x30;
-		} else
+		else
 			initialgain = 0x1E;
 
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_INITIAL_GAIN,
-				  (u8 *)(&initialgain));
+		rtl8723a_set_initial_gain(padapter, initialgain);
 
 		/* set MSR to no link state */
 		Set_MSR23a(padapter, _HW_STATE_NOLINK_);
 
-		val8 = 1; /* under site survey */
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_SITESURVEY,
-				  (u8 *)(&val8));
+		rtl8723a_mlme_sitesurvey(padapter, 1);
 
 		pmlmeext->sitesurvey_res.state = SCAN_PROCESS;
 	}
 
-	site_survey23a(padapter);
+	rtw_site_survey(padapter);
 
 	return H2C_SUCCESS;
 }
 
-u8 setauth_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
+int setauth_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
-	struct setauth_parm		*pparm = (struct setauth_parm *)pbuf;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	const struct setauth_parm *pparm = (struct setauth_parm *)pbuf;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
 	if (pparm->mode < 4)
-	{
 		pmlmeinfo->auth_algo = pparm->mode;
-	}
 
 	return	H2C_SUCCESS;
 }
 
-u8 setkey_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
+int setkey_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
-	unsigned short				ctrl;
-	struct setkey_parm		*pparm = (struct setkey_parm *)pbuf;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	unsigned short ctrl;
+	const struct setkey_parm *pparm = (struct setkey_parm *)pbuf;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	unsigned char					null_sta[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+	unsigned char null_sta[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
 
 	/* main tx key for wep. */
 	if (pparm->set_tx)
 		pmlmeinfo->key_index = pparm->keyid;
 
 	/* write cam */
-	ctrl = BIT(15) | ((pparm->algorithm) << 2) | pparm->keyid;
+	ctrl = BIT(15) | (pparm->algorithm) << 2 | pparm->keyid;
 
-	DBG_8723A_LEVEL(_drv_always_, "set group key to hw: alg:%d(WEP40-1 WEP104-5 TKIP-2 AES-4) "
-			"keyid:%d\n", pparm->algorithm, pparm->keyid);
-	write_cam23a(padapter, pparm->keyid, ctrl, null_sta, pparm->key);
+	DBG_8723A_LEVEL(_drv_always_, "set group key to hw: alg:%d(WEP40-1 "
+			"WEP104-5 TKIP-2 AES-4) keyid:%d\n",
+			pparm->algorithm, pparm->keyid);
+	rtl8723a_cam_write(padapter, pparm->keyid, ctrl, null_sta, pparm->key);
 
 	/* allow multicast packets to driver */
-        padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_ON_RCR_AM, null_addr);
+	rtl8723a_on_rcr_am(padapter);
 
 	return H2C_SUCCESS;
 }
 
-u8 set_stakey_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
+int set_stakey_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
 	u16 ctrl = 0;
 	u8 cam_id;/* cam_entry */
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	struct set_stakey_parm	*pparm = (struct set_stakey_parm *)pbuf;
+	const struct set_stakey_parm *pparm = (struct set_stakey_parm *)pbuf;
 
 	/* cam_entry: */
 	/* 0~3 for default key */
@@ -9672,105 +6279,104 @@ u8 set_stakey_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
 
 	cam_id = 4;
 
-	DBG_8723A_LEVEL(_drv_always_, "set pairwise key to hw: alg:%d(WEP40-1 WEP104-5 TKIP-2 AES-4) camid:%d\n",
+	DBG_8723A_LEVEL(_drv_always_, "set pairwise key to hw: alg:%d(WEP40-1 "
+			"WEP104-5 TKIP-2 AES-4) camid:%d\n",
 			pparm->algorithm, cam_id);
-	if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
-	{
-
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) {
 		struct sta_info *psta;
 		struct sta_priv *pstapriv = &padapter->stapriv;
 
-		if (pparm->algorithm == _NO_PRIVACY_)	/*  clear cam entry */
-		{
+		if (pparm->algorithm == 0) {	/*  clear cam entry */
 			clear_cam_entry23a(padapter, pparm->id);
 			return H2C_SUCCESS_RSP;
 		}
 
 		psta = rtw_get_stainfo23a(pstapriv, pparm->addr);
-		if (psta)
-		{
-			ctrl = (BIT(15) | ((pparm->algorithm) << 2));
+		if (psta) {
+			ctrl = BIT(15) | (pparm->algorithm << 2);
 
-			DBG_8723A("r871x_set_stakey_hdl23a(): enc_algorithm =%d\n", pparm->algorithm);
+			DBG_8723A("r871x_set_stakey_hdl23a(): enc_algorithm "
+				  "=%d\n", pparm->algorithm);
 
-			if ((psta->mac_id<1) || (psta->mac_id>(NUM_STA-4)))
-			{
-				DBG_8723A("r871x_set_stakey_hdl23a():set_stakey failed, mac_id(aid) =%d\n", psta->mac_id);
+			if (psta->mac_id < 1 || psta->mac_id > (NUM_STA - 4)) {
+				DBG_8723A("r871x_set_stakey_hdl23a():set_stakey"
+					  " failed, mac_id(aid) =%d\n",
+					  psta->mac_id);
 				return H2C_REJECTED;
 			}
 
-			cam_id = (psta->mac_id + 3);/* 0~3 for default key, cmd_id = macid + 3, macid = aid+1; */
+			/* 0~3 for default key, cmd_id = macid + 3,
+			   macid = aid+1; */
+			cam_id = (psta->mac_id + 3);
 
-			DBG_8723A("Write CAM, mac_addr =%x:%x:%x:%x:%x:%x, cam_entry =%d\n", pparm->addr[0],
-						pparm->addr[1], pparm->addr[2], pparm->addr[3], pparm->addr[4],
-						pparm->addr[5], cam_id);
+			DBG_8723A("Write CAM, mac_addr =%x:%x:%x:%x:%x:%x, "
+				  "cam_entry =%d\n", pparm->addr[0],
+				  pparm->addr[1], pparm->addr[2],
+				  pparm->addr[3], pparm->addr[4],
+				  pparm->addr[5], cam_id);
 
-			write_cam23a(padapter, cam_id, ctrl, pparm->addr, pparm->key);
+			rtl8723a_cam_write(padapter, cam_id, ctrl,
+					   pparm->addr, pparm->key);
 
 			return H2C_SUCCESS_RSP;
-
-		}
-		else
-		{
-			DBG_8723A("r871x_set_stakey_hdl23a(): sta has been free\n");
+		} else {
+			DBG_8723A("r871x_set_stakey_hdl23a(): sta has been "
+				  "free\n");
 			return H2C_REJECTED;
 		}
-
 	}
 
 	/* below for sta mode */
 
-	if (pparm->algorithm == _NO_PRIVACY_)	/*  clear cam entry */
-	{
+	if (pparm->algorithm == 0) {	/*  clear cam entry */
 		clear_cam_entry23a(padapter, pparm->id);
 		return H2C_SUCCESS;
 	}
 
-	ctrl = BIT(15) | ((pparm->algorithm) << 2);
+	ctrl = BIT(15) | (pparm->algorithm << 2);
 
-	write_cam23a(padapter, cam_id, ctrl, pparm->addr, pparm->key);
+	rtl8723a_cam_write(padapter, cam_id, ctrl, pparm->addr, pparm->key);
 
 	pmlmeinfo->enc_algo = pparm->algorithm;
 
 	return H2C_SUCCESS;
 }
 
-u8 add_ba_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
+int add_ba_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
-	struct addBaReq_parm	*pparm = (struct addBaReq_parm *)pbuf;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	const struct addBaReq_parm *pparm = (struct addBaReq_parm *)pbuf;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+	struct sta_info *psta;
 
-	struct sta_info *psta = rtw_get_stainfo23a(&padapter->stapriv, pparm->addr);
+	psta = rtw_get_stainfo23a(&padapter->stapriv, pparm->addr);
 
 	if (!psta)
 		return	H2C_SUCCESS;
 
 	if (((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) &&
-	     (pmlmeinfo->HT_enable)) ||
-	    ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE)) {
+	     pmlmeinfo->HT_enable) ||
+	    (pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) {
 		issue_action_BA23a(padapter, pparm->addr,
-				WLAN_ACTION_ADDBA_REQ, (u16)pparm->tid);
+				   WLAN_ACTION_ADDBA_REQ, (u16)pparm->tid);
 		mod_timer(&psta->addba_retry_timer,
 			  jiffies + msecs_to_jiffies(ADDBA_TO));
-	} else {
-		psta->htpriv.candidate_tid_bitmap &= ~CHKBIT(pparm->tid);
-	}
+	} else
+		psta->htpriv.candidate_tid_bitmap &= ~BIT(pparm->tid);
+
 	return	H2C_SUCCESS;
 }
 
-u8 set_tx_beacon_cmd23a(struct rtw_adapter* padapter)
+int set_tx_beacon_cmd23a(struct rtw_adapter* padapter)
 {
-	struct cmd_obj	*ph2c;
+	struct cmd_obj *ph2c;
 	struct Tx_Beacon_param	*ptxBeacon_parm;
 	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	u8	res = _SUCCESS;
+	u8 res = _SUCCESS;
 	int len_diff = 0;
 
-
-
 	ph2c = (struct cmd_obj *)kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (!ph2c) {
 		res = _FAIL;
@@ -9794,61 +6400,52 @@ u8 set_tx_beacon_cmd23a(struct rtw_adapter* padapter)
 		pmlmeinfo->hidden_ssid_mode);
 	ptxBeacon_parm->network.IELength += len_diff;
 
-	init_h2fwcmd_w_parm_no_rsp(ph2c, ptxBeacon_parm, GEN_CMD_CODE(_TX_Beacon));
+	init_h2fwcmd_w_parm_no_rsp(ph2c, ptxBeacon_parm,
+				   GEN_CMD_CODE(_TX_Beacon));
 
 	res = rtw_enqueue_cmd23a(pcmdpriv, ph2c);
 
 exit:
-
-
-
 	return res;
 }
 
-u8 mlme_evt_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
+int mlme_evt_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
 	u8 evt_code, evt_seq;
 	u16 evt_sz;
-	uint	*peventbuf;
-	void (*event_callback)(struct rtw_adapter *dev, u8 *pbuf);
-	struct evt_priv *pevt_priv = &padapter->evtpriv;
+	const struct C2HEvent_Header *c2h;
+	void (*event_callback)(struct rtw_adapter *dev, const u8 *pbuf);
 
-	peventbuf = (uint*)pbuf;
-	evt_sz = (u16)(*peventbuf&0xffff);
-	evt_seq = (u8)((*peventbuf>>24)&0x7f);
-	evt_code = (u8)((*peventbuf>>16)&0xff);
+	c2h = (struct C2HEvent_Header *)pbuf;
+	evt_sz = c2h->len;
+	evt_seq = c2h->seq;
+	evt_code = c2h->ID;
 
 	/*  checking if event code is valid */
 	if (evt_code >= MAX_C2HEVT) {
-		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nEvent Code(%d) mismatch!\n", evt_code));
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
+			 ("\nEvent Code(%d) mismatch!\n", evt_code));
 		goto _abort_event_;
 	}
 
 	/*  checking if event size match the event parm size */
-	if ((wlanevents[evt_code].parmsize != 0) &&
-	    (wlanevents[evt_code].parmsize != evt_sz)) {
-		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nEvent(%d) Parm Size mismatch (%d vs %d)!\n",
-			evt_code, wlanevents[evt_code].parmsize, evt_sz));
+	if (wlanevents[evt_code].parmsize != 0 &&
+	    wlanevents[evt_code].parmsize != evt_sz) {
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
+			 ("\nEvent(%d) Parm Size mismatch (%d vs %d)!\n",
+			  evt_code, wlanevents[evt_code].parmsize, evt_sz));
 		goto _abort_event_;
 	}
 
-	atomic_inc(&pevt_priv->event_seq);
-
-	peventbuf += 2;
-
-	if (peventbuf) {
-		event_callback = wlanevents[evt_code].event_callback;
-		event_callback(padapter, (u8*)peventbuf);
-
-		pevt_priv->evt_done_cnt++;
-	}
+	event_callback = wlanevents[evt_code].event_callback;
+	event_callback(padapter, pbuf + sizeof(struct C2HEvent_Header));
 
 _abort_event_:
 
 	return H2C_SUCCESS;
 }
 
-u8 h2c_msg_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
+int h2c_msg_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
 	if (!pbuf)
 		return H2C_PARAMETERS_ERROR;
@@ -9856,16 +6453,14 @@ u8 h2c_msg_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
 	return H2C_SUCCESS;
 }
 
-u8 tx_beacon_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
+int tx_beacon_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
-	if (send_beacon23a(padapter) == _FAIL)
-	{
+	if (send_beacon23a(padapter) == _FAIL) {
 		DBG_8723A("issue_beacon23a, fail!\n");
 		return H2C_PARAMETERS_ERROR;
 	}
 #ifdef CONFIG_8723AU_AP_MODE
-	else /* tx bc/mc frames after update TIM */
-	{
+	else { /* tx bc/mc frames after update TIM */
 		struct sta_info *psta_bmc;
 		struct list_head *plist, *phead, *ptmp;
 		struct xmit_frame *pxmitframe;
@@ -9877,8 +6472,7 @@ u8 tx_beacon_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
 		if (!psta_bmc)
 			return H2C_SUCCESS;
 
-		if ((pstapriv->tim_bitmap&BIT(0)) && (psta_bmc->sleepq_len>0))
-		{
+		if (pstapriv->tim_bitmap & BIT(0) && psta_bmc->sleepq_len > 0) {
 			msleep(10);/*  10ms, ATIM(HIQ) Windows */
 			/* spin_lock_bh(&psta_bmc->sleep_q.lock); */
 			spin_lock_bh(&pxmitpriv->lock);
@@ -9902,22 +6496,22 @@ u8 tx_beacon_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
 
 				pxmitframe->attrib.qsel = 0x11;/* HIQ */
 
-				rtw_hal_xmit23aframe_enqueue(padapter, pxmitframe);
+				rtl8723au_hal_xmitframe_enqueue(padapter,
+								pxmitframe);
 			}
 
 			/* spin_unlock_bh(&psta_bmc->sleep_q.lock); */
 			spin_unlock_bh(&pxmitpriv->lock);
 		}
-
 	}
 #endif
 
 	return H2C_SUCCESS;
 }
 
-u8 set_ch_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
+int set_ch_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
-	struct set_ch_parm *set_ch_parm;
+	const struct set_ch_parm *set_ch_parm;
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 
 	if (!pbuf)
@@ -9925,22 +6519,23 @@ u8 set_ch_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
 
 	set_ch_parm = (struct set_ch_parm *)pbuf;
 
-	DBG_8723A(FUNC_NDEV_FMT" ch:%u, bw:%u, ch_offset:%u\n",
-		FUNC_NDEV_ARG(padapter->pnetdev),
-		set_ch_parm->ch, set_ch_parm->bw, set_ch_parm->ch_offset);
+	DBG_8723A("%s(%s): ch:%u, bw:%u, ch_offset:%u\n", __func__,
+		  padapter->pnetdev->name, set_ch_parm->ch,
+		  set_ch_parm->bw, set_ch_parm->ch_offset);
 
 	pmlmeext->cur_channel = set_ch_parm->ch;
 	pmlmeext->cur_ch_offset = set_ch_parm->ch_offset;
 	pmlmeext->cur_bwmode = set_ch_parm->bw;
 
-	set_channel_bwmode23a(padapter, set_ch_parm->ch, set_ch_parm->ch_offset, set_ch_parm->bw);
+	set_channel_bwmode23a(padapter, set_ch_parm->ch,
+			      set_ch_parm->ch_offset, set_ch_parm->bw);
 
 	return	H2C_SUCCESS;
 }
 
-u8 set_chplan_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
+int set_chplan_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
-	struct SetChannelPlan_param *setChannelPlan_param;
+	const struct SetChannelPlan_param *setChannelPlan_param;
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 
 	if (!pbuf)
@@ -9948,13 +6543,16 @@ u8 set_chplan_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
 
 	setChannelPlan_param = (struct SetChannelPlan_param *)pbuf;
 
-	pmlmeext->max_chan_nums = init_channel_set(padapter, setChannelPlan_param->channel_plan, pmlmeext->channel_set);
-	init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
+	pmlmeext->max_chan_nums =
+		init_channel_set(padapter, setChannelPlan_param->channel_plan,
+				 pmlmeext->channel_set);
+	init_channel_list(padapter, pmlmeext->channel_set,
+			  pmlmeext->max_chan_nums, &pmlmeext->channel_list);
 
 	return	H2C_SUCCESS;
 }
 
-u8 led_blink_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
+int led_blink_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
 	struct LedBlink_param *ledBlink_param;
 
@@ -9966,25 +6564,28 @@ u8 led_blink_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
 	return	H2C_SUCCESS;
 }
 
-u8 set_csa_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
+int set_csa_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
 	return	H2C_REJECTED;
 }
 
 /*  TDLS_WRCR		: write RCR DATA BIT */
 /*  TDLS_SD_PTI		: issue peer traffic indication */
-/*  TDLS_CS_OFF		: go back to the channel linked with AP, terminating channel switch procedure */
-/*  TDLS_INIT_CH_SEN	: init channel sensing, receive all data and mgnt frame */
-/*  TDLS_DONE_CH_SEN: channel sensing and report candidate channel */
+/*  TDLS_CS_OFF		: go back to the channel linked with AP,
+			  terminating channel switch procedure */
+/*  TDLS_INIT_CH_SEN	: init channel sensing, receive all data and
+			  mgnt frame */
+/*  TDLS_DONE_CH_SEN	: channel sensing and report candidate channel */
 /*  TDLS_OFF_CH		: first time set channel to off channel */
-/*  TDLS_BASE_CH		: go back tp the channel linked with AP when set base channel as target channel */
+/*  TDLS_BASE_CH	: go back tp the channel linked with AP when set
+			  base channel as target channel */
 /*  TDLS_P_OFF_CH	: periodically go to off channel */
 /*  TDLS_P_BASE_CH	: periodically go back to base channel */
 /*  TDLS_RS_RCR		: restore RCR */
 /*  TDLS_CKALV_PH1	: check alive timer phase1 */
 /*  TDLS_CKALV_PH2	: check alive timer phase2 */
 /*  TDLS_FREE_STA	: free tdls sta */
-u8 tdls_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
+int tdls_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
 	return H2C_REJECTED;
 }
diff --git a/drivers/staging/rtl8723au/core/rtw_p2p.c b/drivers/staging/rtl8723au/core/rtw_p2p.c
deleted file mode 100644
index 1a961e3f3a5..00000000000
--- a/drivers/staging/rtl8723au/core/rtw_p2p.c
+++ /dev/null
@@ -1,4001 +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.
- *
- ******************************************************************************/
-#define _RTW_P2P_C_
-
-#include <drv_types.h>
-#include <rtw_p2p.h>
-#include <wifi.h>
-
-#ifdef CONFIG_8723AU_P2P
-
-static int rtw_p2p_is_channel_list_ok(u8 desired_ch, u8* ch_list, u8 ch_cnt)
-{
-	int found = 0, i = 0;
-
-	for (i = 0; i < ch_cnt; i++)
-	{
-		if (ch_list[ i ] == desired_ch)
-		{
-			found = 1;
-			break;
-		}
-	}
-	return found;
-}
-
-static int is_any_client_associated(struct rtw_adapter *padapter)
-{
-	return padapter->stapriv.asoc_list_cnt ? true : false;
-}
-
-static u32 go_add_group_info_attr(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	struct list_head *phead, *plist;
-	u32 len = 0;
-	u16 attr_len = 0;
-	u8 tmplen, *pdata_attr, *pstart, *pcur;
-	struct sta_info *psta;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct sta_priv *pstapriv = &padapter->stapriv;
-
-	DBG_8723A("%s\n", __func__);
-
-	pdata_attr = kzalloc(MAX_P2P_IE_LEN, GFP_ATOMIC);
-
-	pstart = pdata_attr;
-	pcur = pdata_attr;
-
-	spin_lock_bh(&pstapriv->asoc_list_lock);
-	phead = &pstapriv->asoc_list;
-
-	list_for_each(plist, phead) {
-		psta = container_of(plist, struct sta_info, asoc_list);
-
-		if (psta->is_p2p_device)
-		{
-			tmplen = 0;
-
-			pcur++;
-
-			/* P2P device address */
-			memcpy(pcur, psta->dev_addr, ETH_ALEN);
-			pcur += ETH_ALEN;
-
-			/* P2P interface address */
-			memcpy(pcur, psta->hwaddr, ETH_ALEN);
-			pcur += ETH_ALEN;
-
-			*pcur = psta->dev_cap;
-			pcur++;
-
-			/* u16*)(pcur) = cpu_to_be16(psta->config_methods); */
-			put_unaligned_be16(psta->config_methods, pcur);
-			pcur += 2;
-
-			memcpy(pcur, psta->primary_dev_type, 8);
-			pcur += 8;
-
-			*pcur = psta->num_of_secdev_type;
-			pcur++;
-
-			memcpy(pcur, psta->secdev_types_list, psta->num_of_secdev_type*8);
-			pcur += psta->num_of_secdev_type*8;
-
-			if (psta->dev_name_len>0)
-			{
-				/* u16*)(pcur) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); */
-				put_unaligned_be16(WPS_ATTR_DEVICE_NAME, pcur);
-				pcur += 2;
-
-				/* u16*)(pcur) = cpu_to_be16(psta->dev_name_len); */
-				put_unaligned_be16(psta->dev_name_len, pcur);
-				pcur += 2;
-
-				memcpy(pcur, psta->dev_name, psta->dev_name_len);
-				pcur += psta->dev_name_len;
-			}
-
-			tmplen = (u8)(pcur-pstart);
-
-			*pstart = (tmplen-1);
-
-			attr_len += tmplen;
-
-			/* pstart += tmplen; */
-			pstart = pcur;
-
-		}
-
-	}
-	spin_unlock_bh(&pstapriv->asoc_list_lock);
-
-	if (attr_len>0)
-	{
-		len = rtw_set_p2p_attr_content23a(pbuf, P2P_ATTR_GROUP_INFO, attr_len, pdata_attr);
-	}
-
-	kfree(pdata_attr);
-
-	return len;
-}
-
-static void issue_group_disc_req(struct wifidirect_info *pwdinfo, u8 *da)
-{
-	struct xmit_frame			*pmgntframe;
-	struct pkt_attrib			*pattrib;
-	unsigned char					*pframe;
-	struct ieee80211_hdr	*pwlanhdr;
-	unsigned short				*fctrl;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	unsigned char category = WLAN_CATEGORY_VENDOR_SPECIFIC;/* P2P action frame */
-	u32	p2poui = cpu_to_be32(P2POUI);
-	u8	oui_subtype = P2P_GO_DISC_REQUEST;
-	u8	dialogToken = 0;
-
-	DBG_8723A("[%s]\n", __func__);
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-	{
-		return;
-	}
-
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
-	memcpy(pwlanhdr->addr2, pwdinfo->interface_addr, ETH_ALEN);
-	memcpy(pwlanhdr->addr3, pwdinfo->interface_addr, ETH_ALEN);
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
-
-	pframe += sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
-
-	/* Build P2P action frame header */
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *)&p2poui, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen);
-
-	/* there is no IE in this P2P action frame */
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	dump_mgntframe23a(padapter, pmgntframe);
-}
-
-static void issue_p2p_devdisc_resp(struct wifidirect_info *pwdinfo, u8 *da, u8 status, u8 dialogToken)
-{
-	struct xmit_frame			*pmgntframe;
-	struct pkt_attrib			*pattrib;
-	unsigned char					*pframe;
-	struct ieee80211_hdr	*pwlanhdr;
-	unsigned short				*fctrl;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	unsigned char category = WLAN_CATEGORY_PUBLIC;
-	u8			action = P2P_PUB_ACTION_ACTION;
-	u32			p2poui = cpu_to_be32(P2POUI);
-	u8			oui_subtype = P2P_DEVDISC_RESP;
-	u8 p2pie[8] = { 0x00 };
-	u32 p2pielen = 0;
-
-	DBG_8723A("[%s]\n", __func__);
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-	{
-		return;
-	}
-
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
-	memcpy(pwlanhdr->addr2, pwdinfo->device_addr, ETH_ALEN);
-	memcpy(pwlanhdr->addr3, pwdinfo->device_addr, ETH_ALEN);
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
-
-	pframe += sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
-
-	/* Build P2P public action frame header */
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *) &p2poui,
-				     &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen);
-
-	/* Build P2P IE */
-	/*	P2P OUI */
-	p2pielen = 0;
-	p2pie[ p2pielen++ ] = 0x50;
-	p2pie[ p2pielen++ ] = 0x6F;
-	p2pie[ p2pielen++ ] = 0x9A;
-	p2pie[ p2pielen++ ] = 0x09;	/*	WFA P2P v1.0 */
-
-	/*  P2P_ATTR_STATUS */
-	p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status);
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, p2pie, &pattrib->pktlen);
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	dump_mgntframe23a(padapter, pmgntframe);
-}
-
-static void issue_p2p_provision_resp(struct wifidirect_info *pwdinfo, u8* raddr, u8* frame_body, u16 config_method)
-{
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	unsigned char category = WLAN_CATEGORY_PUBLIC;
-	u8			action = P2P_PUB_ACTION_ACTION;
-	u8			dialogToken = frame_body[7];	/*	The Dialog Token of provisioning discovery request frame. */
-	u32			p2poui = cpu_to_be32(P2POUI);
-	u8			oui_subtype = P2P_PROVISION_DISC_RESP;
-	u8			wpsie[ 100 ] = { 0x00 };
-	u8			wpsielen = 0;
-#ifdef CONFIG_8723AU_P2P
-	u32					wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
-
-	struct xmit_frame			*pmgntframe;
-	struct pkt_attrib			*pattrib;
-	unsigned char					*pframe;
-	struct ieee80211_hdr	*pwlanhdr;
-	unsigned short				*fctrl;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-		return;
-
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
-	memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
-	memcpy(pwlanhdr->addr3, myid(&padapter->eeprompriv), ETH_ALEN);
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
-
-	pframe += sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
-
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *) &p2poui,
-				     &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen);
-
-	wpsielen = 0;
-	/*	WPS OUI */
-	/* u32*) (wpsie) = cpu_to_be32(WPSOUI); */
-	put_unaligned_be32(WPSOUI, wpsie);
-	wpsielen += 4;
-
-	/*	Config Method */
-	/*	Type: */
-	/* u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD); */
-	put_unaligned_be16(WPS_ATTR_CONF_METHOD, wpsie + wpsielen);
-	wpsielen += 2;
-
-	/*	Length: */
-	/* u16*) (wpsie + wpsielen) = cpu_to_be16(0x0002); */
-	put_unaligned_be16(0x0002, wpsie + wpsielen);
-	wpsielen += 2;
-
-	/*	Value: */
-	/* u16*) (wpsie + wpsielen) = cpu_to_be16(config_method); */
-	put_unaligned_be16(config_method, wpsie + wpsielen);
-	wpsielen += 2;
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen);
-
-#ifdef CONFIG_8723AU_P2P
-	wfdielen = build_provdisc_resp_wfd_ie(pwdinfo, pframe);
-	pframe += wfdielen;
-	pattrib->pktlen += wfdielen;
-#endif /* CONFIG_8723AU_P2P */
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	dump_mgntframe23a(padapter, pmgntframe);
-
-	return;
-}
-
-static void issue_p2p_presence_resp(struct wifidirect_info *pwdinfo, u8 *da, u8 status, u8 dialogToken)
-{
-	struct xmit_frame			*pmgntframe;
-	struct pkt_attrib			*pattrib;
-	unsigned char					*pframe;
-	struct ieee80211_hdr	*pwlanhdr;
-	unsigned short				*fctrl;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	unsigned char category = WLAN_CATEGORY_VENDOR_SPECIFIC;/* P2P action frame */
-	u32	p2poui = cpu_to_be32(P2POUI);
-	u8	oui_subtype = P2P_PRESENCE_RESPONSE;
-	u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
-	u8 noa_attr_content[32] = { 0x00 };
-	u32 p2pielen = 0;
-
-	DBG_8723A("[%s]\n", __func__);
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-	{
-		return;
-	}
-
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
-	memcpy(pwlanhdr->addr2, pwdinfo->interface_addr, ETH_ALEN);
-	memcpy(pwlanhdr->addr3, pwdinfo->interface_addr, ETH_ALEN);
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
-
-	pframe += sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
-
-	/* Build P2P action frame header */
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *) &p2poui,
-				     &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen);
-
-	/* Add P2P IE header */
-	/*	P2P OUI */
-	p2pielen = 0;
-	p2pie[ p2pielen++ ] = 0x50;
-	p2pie[ p2pielen++ ] = 0x6F;
-	p2pie[ p2pielen++ ] = 0x9A;
-	p2pie[ p2pielen++ ] = 0x09;	/*	WFA P2P v1.0 */
-
-	/* Add Status attribute in P2P IE */
-	p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status);
-
-	/* Add NoA attribute in P2P IE */
-	noa_attr_content[0] = 0x1;/* index */
-	noa_attr_content[1] = 0x0;/* CTWindow and OppPS Parameters */
-
-	/* todo: Notice of Absence Descriptor(s) */
-
-	p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_NOA, 2, noa_attr_content);
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, p2pie,
-			       &pattrib->pktlen);
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	dump_mgntframe23a(padapter, pmgntframe);
-}
-
-u32 build_beacon_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
-	u16 capability = 0;
-	u32 len = 0, p2pielen = 0;
-
-	/*	P2P OUI */
-	p2pielen = 0;
-	p2pie[ p2pielen++ ] = 0x50;
-	p2pie[ p2pielen++ ] = 0x6F;
-	p2pie[ p2pielen++ ] = 0x9A;
-	p2pie[ p2pielen++ ] = 0x09;	/*	WFA P2P v1.0 */
-
-	/*	According to the P2P Specification, the beacon frame should contain 3 P2P attributes */
-	/*	1. P2P Capability */
-	/*	2. P2P Device ID */
-	/*	3. Notice of Absence (NOA) */
-
-	/*	P2P Capability ATTR */
-	/*	Type: */
-	/*	Length: */
-	/*	Value: */
-	/*	Device Capability Bitmap, 1 byte */
-	/*	Be able to participate in additional P2P Groups and */
-	/*	support the P2P Invitation Procedure */
-	/*	Group Capability Bitmap, 1 byte */
-	capability = P2P_DEVCAP_INVITATION_PROC|P2P_DEVCAP_CLIENT_DISCOVERABILITY;
-	capability |=  ((P2P_GRPCAP_GO | P2P_GRPCAP_INTRABSS) << 8);
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_PROVISIONING_ING))
-		capability |= (P2P_GRPCAP_GROUP_FORMATION<<8);
-
-	capability = cpu_to_le16(capability);
-
-	p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_CAPABILITY, 2, (u8*)&capability);
-
-	/*  P2P Device ID ATTR */
-	p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_DEVICE_ID, ETH_ALEN, pwdinfo->device_addr);
-
-	/*  Notice of Absence ATTR */
-	/*	Type: */
-	/*	Length: */
-	/*	Value: */
-
-	/* go_add_noa_attr(pwdinfo); */
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len);
-
-	return len;
-}
-
-#ifdef CONFIG_8723AU_P2P
-u32 build_beacon_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
-
-	/*	WFD OUI */
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110812 */
-	/*	According to the WFD Specification, the beacon frame should contain 4 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID */
-	/*	3. Coupled Sink Information */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-
-	if (P2P_ROLE_GO == pwdinfo->role)
-	{
-		if (is_any_client_associated(pwdinfo->padapter))
-		{
-			/*	WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery) */
-			put_unaligned_be16(pwfd_info->wfd_device_type |
-					   WFD_DEVINFO_WSD, wfdie + wfdielen);
-		}
-		else
-		{
-			/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
-			put_unaligned_be16(pwfd_info->wfd_device_type |
-					   WFD_DEVINFO_SESSION_AVAIL |
-					   WFD_DEVINFO_WSD, wfdie + wfdielen);
-		}
-
-	}
-	else
-	{
-		/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
-		put_unaligned_be16(pwfd_info->wfd_device_type |
-				   WFD_DEVINFO_SESSION_AVAIL |
-				   WFD_DEVINFO_WSD, wfdie + wfdielen);
-	}
-
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-u32 build_probe_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
-
-	/*	WFD OUI */
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110812 */
-	/*	According to the WFD Specification, the probe request frame should contain 4 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID */
-	/*	3. Coupled Sink Information */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-
-	if (1 == pwdinfo->wfd_tdls_enable)
-	{
-		/*	WFD primary sink + available for WFD session + WiFi TDLS mode + WSC (WFD Service Discovery) */
-		put_unaligned_be16(pwfd_info->wfd_device_type |
-				   WFD_DEVINFO_SESSION_AVAIL |
-				   WFD_DEVINFO_WSD |
-				   WFD_DEVINFO_PC_TDLS, wfdie + wfdielen);
-	}
-	else
-	{
-		/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSC (WFD Service Discovery) */
-		put_unaligned_be16(pwfd_info->wfd_device_type |
-				   WFD_DEVINFO_SESSION_AVAIL |
-				   WFD_DEVINFO_WSD, wfdie + wfdielen);
-	}
-
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-u32 build_probe_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf, u8 tunneled)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
-
-	/*	WFD OUI */
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110812 */
-	/*	According to the WFD Specification, the probe response frame should contain 4 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID */
-	/*	3. Coupled Sink Information */
-	/*	4. WFD Session Information */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-	/*	WFD primary sink + available for WFD session + WiFi Direct mode */
-
-	if (true == pwdinfo->session_available)
-	{
-		if (P2P_ROLE_GO == pwdinfo->role)
-		{
-			if (is_any_client_associated(pwdinfo->padapter))
-			{
-				if (pwdinfo->wfd_tdls_enable)
-				{
-					/*	TDLS mode + WSD (WFD Service Discovery) */
-					put_unaligned_be16(pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT, wfdie + wfdielen);
-				}
-				else
-				{
-					/*	WiFi Direct mode + WSD (WFD Service Discovery) */
-					put_unaligned_be16(pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT, wfdie + wfdielen);
-				}
-			}
-			else
-			{
-				if (pwdinfo->wfd_tdls_enable)
-				{
-					/*	available for WFD session + TDLS mode + WSD (WFD Service Discovery) */
-					put_unaligned_be16(pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT, wfdie + wfdielen);
-				}
-				else
-				{
-					/*	available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
-					put_unaligned_be16(pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT, wfdie + wfdielen);
-				}
-			}
-		}
-		else
-		{
-			if (pwdinfo->wfd_tdls_enable)
-			{
-				/*	available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
-				put_unaligned_be16(pwfd_info->wfd_device_type |
-						   WFD_DEVINFO_SESSION_AVAIL |
-						   WFD_DEVINFO_WSD |
-						   WFD_DEVINFO_PC_TDLS |
-						   WFD_DEVINFO_HDCP_SUPPORT,
-						   wfdie + wfdielen);
-			}
-			else
-			{
-
-				/*	available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
-				put_unaligned_be16(pwfd_info->wfd_device_type |
-						   WFD_DEVINFO_SESSION_AVAIL |
-						   WFD_DEVINFO_WSD |
-						   WFD_DEVINFO_HDCP_SUPPORT,
-						   wfdie + wfdielen);
-			}
-		}
-	}
-	else
-	{
-		if (pwdinfo->wfd_tdls_enable)
-		{
-			put_unaligned_be16(pwfd_info->wfd_device_type |
-					   WFD_DEVINFO_WSD |
-					   WFD_DEVINFO_PC_TDLS |
-					   WFD_DEVINFO_HDCP_SUPPORT,
-					   wfdie + wfdielen);
-		}
-		else
-		{
-			put_unaligned_be16(pwfd_info->wfd_device_type |
-					   WFD_DEVINFO_WSD |
-					   WFD_DEVINFO_HDCP_SUPPORT,
-					   wfdie + wfdielen);
-		}
-
-	}
-
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-	{
-		/*	WFD Session Information ATTR */
-		/*	Type: */
-		wfdie[ wfdielen++ ] = WFD_ATTR_SESSION_INFO;
-
-		/*	Length: */
-		/*	Note: In the WFD specification, the size of length field is 2. */
-		put_unaligned_be16(0x0000, wfdie + wfdielen);
-		wfdielen += 2;
-
-		/*	Todo: to add the list of WFD device info descriptor in WFD group. */
-
-	}
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-u32 build_assoc_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter					*padapter = NULL;
-	struct mlme_priv			*pmlmepriv = NULL;
-	struct wifi_display_info		*pwfd_info = NULL;
-
-	/*	WFD OUI */
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
-	{
-		return 0;
-	}
-
-	padapter = pwdinfo->padapter;
-	pmlmepriv = &padapter->mlmepriv;
-	pwfd_info = padapter->wdinfo.wfd_info;
-
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110812 */
-	/*	According to the WFD Specification, the probe request frame should contain 4 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID */
-	/*	3. Coupled Sink Information */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-	/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
-	put_unaligned_be16(pwfd_info->wfd_device_type |
-			   WFD_DEVINFO_SESSION_AVAIL |
-			   WFD_DEVINFO_WSD, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-u32 build_assoc_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
-
-	/*	WFD OUI */
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110812 */
-	/*	According to the WFD Specification, the probe request frame should contain 4 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID */
-	/*	3. Coupled Sink Information */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-	/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
-	put_unaligned_be16(pwfd_info->wfd_device_type |
-			   WFD_DEVINFO_SESSION_AVAIL |
-			   WFD_DEVINFO_WSD, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-u32 build_nego_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
-
-	/*	WFD OUI */
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110825 */
-	/*	According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID (Optional) */
-	/*	3. Local IP Adress (Optional) */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-	/*	WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery) + WFD Session Available */
-	put_unaligned_be16(pwfd_info->wfd_device_type |
-			   WFD_DEVINFO_WSD | WFD_DEVINFO_SESSION_AVAIL,
-			   wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-u32 build_nego_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
-
-	/*	WFD OUI */
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110825 */
-	/*	According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID (Optional) */
-	/*	3. Local IP Adress (Optional) */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-	/*	WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery) + WFD Session Available */
-	put_unaligned_be16(pwfd_info->wfd_device_type |
-			   WFD_DEVINFO_WSD | WFD_DEVINFO_SESSION_AVAIL,
-			   wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-u32 build_nego_confirm_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
-
-	/*	WFD OUI */
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110825 */
-	/*	According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID (Optional) */
-	/*	3. Local IP Adress (Optional) */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-	/*	WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery) + WFD Session Available */
-	put_unaligned_be16(pwfd_info->wfd_device_type | WFD_DEVINFO_WSD |
-			   WFD_DEVINFO_SESSION_AVAIL, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-u32 build_invitation_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
-
-	/*	WFD OUI */
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110825 */
-	/*	According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID (Optional) */
-	/*	3. Local IP Adress (Optional) */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-	/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
-	put_unaligned_be16(pwfd_info->wfd_device_type |
-			   WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD,
-			   wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	if (P2P_ROLE_GO == pwdinfo->role)
-	{
-		/*	WFD Session Information ATTR */
-		/*	Type: */
-		wfdie[ wfdielen++ ] = WFD_ATTR_SESSION_INFO;
-
-		/*	Length: */
-		/*	Note: In the WFD specification, the size of length field is 2. */
-		put_unaligned_be16(0x0000, wfdie + wfdielen);
-		wfdielen += 2;
-
-		/*	Todo: to add the list of WFD device info descriptor in WFD group. */
-
-	}
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-u32 build_invitation_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
-
-	/*	WFD OUI */
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110825 */
-	/*	According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID (Optional) */
-	/*	3. Local IP Adress (Optional) */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-	/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
-	put_unaligned_be16(pwfd_info->wfd_device_type |
-			   WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD,
-			   wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	if (P2P_ROLE_GO == pwdinfo->role)
-	{
-		/*	WFD Session Information ATTR */
-		/*	Type: */
-		wfdie[ wfdielen++ ] = WFD_ATTR_SESSION_INFO;
-
-		/*	Length: */
-		/*	Note: In the WFD specification, the size of length field is 2. */
-		put_unaligned_be16(0x0000, wfdie + wfdielen);
-		wfdielen += 2;
-
-		/*	Todo: to add the list of WFD device info descriptor in WFD group. */
-
-	}
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-u32 build_provdisc_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
-
-	/*	WFD OUI */
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110825 */
-	/*	According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID (Optional) */
-	/*	3. Local IP Adress (Optional) */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-	/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
-	put_unaligned_be16(pwfd_info->wfd_device_type |
-			   WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD,
-			   wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-u32 build_provdisc_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
-
-	/*	WFD OUI */
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110825 */
-	/*	According to the WFD Specification, the provision discovery response frame should contain 3 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID (Optional) */
-	/*	3. Local IP Adress (Optional) */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-	/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
-	put_unaligned_be16(pwfd_info->wfd_device_type |
-			   WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD,
-			   wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-#endif /* CONFIG_8723AU_P2P */
-
-u32 build_probe_resp_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
-	u32 len = 0, p2pielen = 0;
-
-	/*	P2P OUI */
-	p2pielen = 0;
-	p2pie[ p2pielen++ ] = 0x50;
-	p2pie[ p2pielen++ ] = 0x6F;
-	p2pie[ p2pielen++ ] = 0x9A;
-	p2pie[ p2pielen++ ] = 0x09;	/*	WFA P2P v1.0 */
-
-	/*	Commented by Albert 20100907 */
-	/*	According to the P2P Specification, the probe response frame should contain 5 P2P attributes */
-	/*	1. P2P Capability */
-	/*	2. Extended Listen Timing */
-	/*	3. Notice of Absence (NOA)	(Only GO needs this) */
-	/*	4. Device Info */
-	/*	5. Group Info	(Only GO need this) */
-
-	/*	P2P Capability ATTR */
-	/*	Type: */
-	p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
-
-	/*	Length: */
-	/* u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002); */
-	put_unaligned_le16(0x0002, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Device Capability Bitmap, 1 byte */
-	p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
-
-	/*	Group Capability Bitmap, 1 byte */
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-	{
-		p2pie[ p2pielen ] = (P2P_GRPCAP_GO | P2P_GRPCAP_INTRABSS);
-
-		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_PROVISIONING_ING))
-			p2pie[ p2pielen ] |= P2P_GRPCAP_GROUP_FORMATION;
-
-		p2pielen++;
-	}
-	else if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE))
-	{
-		/*	Group Capability Bitmap, 1 byte */
-		if (pwdinfo->persistent_supported)
-			p2pie[ p2pielen++ ] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
-		else
-			p2pie[ p2pielen++ ] = DMP_P2P_GRPCAP_SUPPORT;
-	}
-
-	/*	Extended Listen Timing ATTR */
-	/*	Type: */
-	p2pie[ p2pielen++ ] = P2P_ATTR_EX_LISTEN_TIMING;
-
-	/*	Length: */
-	/* u16*) (p2pie + p2pielen) = cpu_to_le16(0x0004); */
-	put_unaligned_le16(0x0004, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Availability Period */
-	/* u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF); */
-	put_unaligned_le16(0xFFFF, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Availability Interval */
-	/* u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF); */
-	put_unaligned_le16(0xFFFF, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*  Notice of Absence ATTR */
-	/*	Type: */
-	/*	Length: */
-	/*	Value: */
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-	{
-		/* go_add_noa_attr(pwdinfo); */
-	}
-
-	/*	Device Info ATTR */
-	/*	Type: */
-	p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
-	/*	+ NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
-	/* u16*) (p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len); */
-	put_unaligned_le16(21 + pwdinfo->device_name_len, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	P2P Device Address */
-	memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);
-	p2pielen += ETH_ALEN;
-
-	/*	Config Method */
-	/*	This field should be big endian. Noted by P2P specification. */
-	/* u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm); */
-	put_unaligned_be16(pwdinfo->supported_wps_cm, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Primary Device Type */
-	/*	Category ID */
-	/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); */
-	put_unaligned_be16(WPS_PDT_CID_MULIT_MEDIA, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	OUI */
-	/* u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI); */
-	put_unaligned_be32(WPSOUI, p2pie + p2pielen);
-	p2pielen += 4;
-
-	/*	Sub Category ID */
-	/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER); */
-	put_unaligned_be16(WPS_PDT_SCID_MEDIA_SERVER, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Number of Secondary Device Types */
-	p2pie[ p2pielen++ ] = 0x00;	/*	No Secondary Device Type List */
-
-	/*	Device Name */
-	/*	Type: */
-	/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); */
-	put_unaligned_be16(WPS_ATTR_DEVICE_NAME, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Length: */
-	/* u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len); */
-	put_unaligned_be16(pwdinfo->device_name_len, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Value: */
-	memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
-	p2pielen += pwdinfo->device_name_len;
-
-	/*  Group Info ATTR */
-	/*	Type: */
-	/*	Length: */
-	/*	Value: */
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-	{
-		p2pielen += go_add_group_info_attr(pwdinfo, p2pie + p2pielen);
-	}
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len);
-
-	return len;
-}
-
-u32 build_prov_disc_request_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf, u8* pssid, u8 ussidlen, u8* pdev_raddr)
-{
-	u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
-	u32 len = 0, p2pielen = 0;
-
-	/*	P2P OUI */
-	p2pielen = 0;
-	p2pie[ p2pielen++ ] = 0x50;
-	p2pie[ p2pielen++ ] = 0x6F;
-	p2pie[ p2pielen++ ] = 0x9A;
-	p2pie[ p2pielen++ ] = 0x09;	/*	WFA P2P v1.0 */
-
-	/*	Commented by Albert 20110301 */
-	/*	According to the P2P Specification, the provision discovery request frame should contain 3 P2P attributes */
-	/*	1. P2P Capability */
-	/*	2. Device Info */
-	/*	3. Group ID (When joining an operating P2P Group) */
-
-	/*	P2P Capability ATTR */
-	/*	Type: */
-	p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
-
-	/*	Length: */
-	/* u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002); */
-	put_unaligned_le16(0x0002, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Device Capability Bitmap, 1 byte */
-	p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
-
-	/*	Group Capability Bitmap, 1 byte */
-	if (pwdinfo->persistent_supported)
-		p2pie[ p2pielen++ ] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
-	else
-		p2pie[ p2pielen++ ] = DMP_P2P_GRPCAP_SUPPORT;
-
-	/*	Device Info ATTR */
-	/*	Type: */
-	p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
-	/*	+ NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
-	/* u16*) (p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len); */
-	put_unaligned_le16(21 + pwdinfo->device_name_len, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	P2P Device Address */
-	memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);
-	p2pielen += ETH_ALEN;
-
-	/*	Config Method */
-	/*	This field should be big endian. Noted by P2P specification. */
-	if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC)
-	{
-		/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_PBC); */
-		put_unaligned_be16(WPS_CONFIG_METHOD_PBC, p2pie + p2pielen);
-	}
-	else
-	{
-		/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY); */
-		put_unaligned_be16(WPS_CONFIG_METHOD_DISPLAY, p2pie + p2pielen);
-	}
-
-	p2pielen += 2;
-
-	/*	Primary Device Type */
-	/*	Category ID */
-	/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); */
-	put_unaligned_be16(WPS_PDT_CID_MULIT_MEDIA, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	OUI */
-	/* u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI); */
-	put_unaligned_be32(WPSOUI, p2pie + p2pielen);
-	p2pielen += 4;
-
-	/*	Sub Category ID */
-	/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER); */
-	put_unaligned_be16(WPS_PDT_SCID_MEDIA_SERVER, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Number of Secondary Device Types */
-	p2pie[ p2pielen++ ] = 0x00;	/*	No Secondary Device Type List */
-
-	/*	Device Name */
-	/*	Type: */
-	/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); */
-	put_unaligned_be16(WPS_ATTR_DEVICE_NAME, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Length: */
-	/* u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len); */
-	put_unaligned_be16(pwdinfo->device_name_len, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Value: */
-	memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
-	p2pielen += pwdinfo->device_name_len;
-
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT))
-	{
-		/*	Added by Albert 2011/05/19 */
-		/*	In this case, the pdev_raddr is the device address of the group owner. */
-
-		/*	P2P Group ID ATTR */
-		/*	Type: */
-		p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_ID;
-
-		/*	Length: */
-		/* u16*) (p2pie + p2pielen) = cpu_to_le16(ETH_ALEN + ussidlen); */
-		put_unaligned_le16(ETH_ALEN + ussidlen, p2pie + p2pielen);
-		p2pielen += 2;
-
-		/*	Value: */
-		memcpy(p2pie + p2pielen, pdev_raddr, ETH_ALEN);
-		p2pielen += ETH_ALEN;
-
-		memcpy(p2pie + p2pielen, pssid, ussidlen);
-		p2pielen += ussidlen;
-
-	}
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len);
-
-	return len;
-}
-
-u32 build_assoc_resp_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf, u8 status_code)
-{
-	u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
-	u32 len = 0, p2pielen = 0;
-
-	/*	P2P OUI */
-	p2pielen = 0;
-	p2pie[ p2pielen++ ] = 0x50;
-	p2pie[ p2pielen++ ] = 0x6F;
-	p2pie[ p2pielen++ ] = 0x9A;
-	p2pie[ p2pielen++ ] = 0x09;	/*	WFA P2P v1.0 */
-
-	/*  According to the P2P Specification, the Association response frame should contain 2 P2P attributes */
-	/*	1. Status */
-	/*	2. Extended Listen Timing (optional) */
-
-	/*	Status ATTR */
-	p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status_code);
-
-	/*  Extended Listen Timing ATTR */
-	/*	Type: */
-	/*	Length: */
-	/*	Value: */
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len);
-
-	return len;
-}
-
-u32 build_deauth_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u32 len = 0;
-
-	return len;
-}
-
-u32 process_probe_req_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
-{
-	u8 *p;
-	u32 ret = false;
-	u8 *p2pie;
-	u32	p2pielen = 0;
-	int ssid_len = 0, rate_cnt = 0;
-
-	p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + _PROBEREQ_IE_OFFSET_, _SUPPORTEDRATES_IE_, (int *)&rate_cnt,
-			len - sizeof(struct ieee80211_hdr_3addr) - _PROBEREQ_IE_OFFSET_);
-
-	if (rate_cnt <= 4)
-	{
-		int i, g_rate = 0;
-
-		for (i = 0; i < rate_cnt; i++)
-		{
-			if (((*(p + 2 + i) & 0xff) != 0x02) &&
-				((*(p + 2 + i) & 0xff) != 0x04) &&
-				((*(p + 2 + i) & 0xff) != 0x0B) &&
-				((*(p + 2 + i) & 0xff) != 0x16))
-			{
-				g_rate = 1;
-			}
-		}
-
-		if (g_rate == 0)
-		{
-			/*	There is no OFDM rate included in SupportedRates IE of this probe request frame */
-			/*	The driver should response this probe request. */
-			return ret;
-		}
-	}
-	else
-	{
-		/*	rate_cnt > 4 means the SupportRates IE contains the OFDM rate because the count of CCK rates are 4. */
-		/*	We should proceed the following check for this probe request. */
-	}
-
-	/*	Added comments by Albert 20100906 */
-	/*	There are several items we should check here. */
-	/*	1. This probe request frame must contain the P2P IE. (Done) */
-	/*	2. This probe request frame must contain the wildcard SSID. (Done) */
-	/*	3. Wildcard BSSID. (Todo) */
-	/*	4. Destination Address. (Done in mgt_dispatcher23a function) */
-	/*	5. Requested Device Type in WSC IE. (Todo) */
-	/*	6. Device ID attribute in P2P IE. (Todo) */
-
-	p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ssid_len,
-			len - sizeof(struct ieee80211_hdr_3addr) - _PROBEREQ_IE_OFFSET_);
-
-	ssid_len &= 0xff;	/*	Just last 1 byte is valid for ssid len of the probe request */
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-	{
-		if ((p2pie = rtw_get_p2p_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + _PROBEREQ_IE_OFFSET_, len - sizeof(struct ieee80211_hdr_3addr) - _PROBEREQ_IE_OFFSET_, NULL, &p2pielen)))
-		{
-			if ((p) && !memcmp((void *)(p+2), (void *)pwdinfo->p2p_wildcard_ssid, 7))
-			{
-				/* todo: */
-				/* Check Requested Device Type attributes in WSC IE. */
-				/* Check Device ID attribute in P2P IE */
-
-				ret = true;
-			}
-			else if ((p != NULL) && (ssid_len == 0))
-			{
-				ret = true;
-			}
-		}
-		else
-		{
-			/* non -p2p device */
-		}
-
-	}
-
-	return ret;
-}
-
-u32 process_assoc_req_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len, struct sta_info *psta)
-{
-	u8 status_code = P2P_STATUS_SUCCESS;
-	u8 *pbuf, *pattr_content = NULL;
-	u32 attr_contentlen = 0;
-	u16 cap_attr = 0;
-	unsigned short ie_offset;
-	u8 * ies;
-	u32 ies_len;
-	u8 * p2p_ie;
-	u32	p2p_ielen = 0;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)pframe;
-
-	if (!rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-		return P2P_STATUS_FAIL_REQUEST_UNABLE;
-
-	if (ieee80211_is_assoc_req(hdr->frame_control))
-		ie_offset = _ASOCREQ_IE_OFFSET_;
-	else /*  WIFI_REASSOCREQ */
-		ie_offset = _REASOCREQ_IE_OFFSET_;
-
-	ies = pframe + sizeof(struct ieee80211_hdr_3addr) + ie_offset;
-	ies_len = len - sizeof(struct ieee80211_hdr_3addr) - ie_offset;
-
-	p2p_ie = rtw_get_p2p_ie23a(ies, ies_len, NULL, &p2p_ielen);
-
-	if (!p2p_ie)
-	{
-		DBG_8723A("[%s] P2P IE not Found!!\n", __func__);
-		status_code =  P2P_STATUS_FAIL_INVALID_PARAM;
-	}
-	else
-	{
-		DBG_8723A("[%s] P2P IE Found!!\n", __func__);
-	}
-
-	while (p2p_ie)
-	{
-		/* Check P2P Capability ATTR */
-		if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8*)&cap_attr, (uint*) &attr_contentlen))
-		{
-			DBG_8723A("[%s] Got P2P Capability Attr!!\n", __func__);
-			cap_attr = le16_to_cpu(cap_attr);
-			psta->dev_cap = cap_attr&0xff;
-		}
-
-		/* Check Extended Listen Timing ATTR */
-
-		/* Check P2P Device Info ATTR */
-		if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO, NULL, (uint*)&attr_contentlen))
-		{
-			DBG_8723A("[%s] Got P2P DEVICE INFO Attr!!\n", __func__);
-			pattr_content = pbuf = kzalloc(attr_contentlen,
-						       GFP_ATOMIC);
-			if (pattr_content) {
-				u8 num_of_secdev_type;
-				u16 dev_name_len;
-
-				rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO, pattr_content, (uint*)&attr_contentlen);
-
-				memcpy(psta->dev_addr,	pattr_content, ETH_ALEN);/* P2P Device Address */
-
-				pattr_content += ETH_ALEN;
-
-				memcpy(&psta->config_methods, pattr_content, 2);/* Config Methods */
-				psta->config_methods = be16_to_cpu(psta->config_methods);
-
-				pattr_content += 2;
-
-				memcpy(psta->primary_dev_type, pattr_content, 8);
-
-				pattr_content += 8;
-
-				num_of_secdev_type = *pattr_content;
-				pattr_content += 1;
-
-				if (num_of_secdev_type == 0)
-				{
-					psta->num_of_secdev_type = 0;
-				}
-				else
-				{
-					u32 len;
-
-					psta->num_of_secdev_type = num_of_secdev_type;
-
-					len = (sizeof(psta->secdev_types_list)<(num_of_secdev_type*8)) ? (sizeof(psta->secdev_types_list)) : (num_of_secdev_type*8);
-
-					memcpy(psta->secdev_types_list, pattr_content, len);
-
-					pattr_content += (num_of_secdev_type*8);
-				}
-
-				/* dev_name_len = attr_contentlen - ETH_ALEN - 2 - 8 - 1 - (num_of_secdev_type*8); */
-				psta->dev_name_len = 0;
-				if (WPS_ATTR_DEVICE_NAME == be16_to_cpu(*(u16*)pattr_content))
-				{
-					dev_name_len = be16_to_cpu(*(u16*)(pattr_content+2));
-
-					psta->dev_name_len = (sizeof(psta->dev_name)<dev_name_len) ? sizeof(psta->dev_name):dev_name_len;
-
-					memcpy(psta->dev_name, pattr_content+4, psta->dev_name_len);
-				}
-
-				kfree(pbuf);
-
-			}
-
-		}
-
-		/* Get the next P2P IE */
-		p2p_ie = rtw_get_p2p_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
-
-	}
-
-	return status_code;
-}
-
-u32 process_p2p_devdisc_req23a(struct wifidirect_info *pwdinfo, u8 *pframe,
-			    uint len)
-{
-	u8 *frame_body;
-	u8 status, dialogToken;
-	struct sta_info *psta = NULL;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct sta_priv *pstapriv = &padapter->stapriv;
-	u8 *p2p_ie;
-	u32	p2p_ielen = 0;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) pframe;
-
-	frame_body = (unsigned char *)
-		(pframe + sizeof(struct ieee80211_hdr_3addr));
-
-	dialogToken = frame_body[7];
-	status = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP;
-
-	if ((p2p_ie = rtw_get_p2p_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_,
-				     len - _PUBLIC_ACTION_IE_OFFSET_, NULL,
-				     &p2p_ielen))) {
-		u8 groupid[38] = { 0x00 };
-		u8 dev_addr[ETH_ALEN] = { 0x00 };
-		u32 attr_contentlen = 0;
-
-		if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen,
-					     P2P_ATTR_GROUP_ID, groupid,
-					     &attr_contentlen)) {
-			if (!memcmp(pwdinfo->device_addr, groupid, ETH_ALEN) &&
-			    !memcmp(pwdinfo->p2p_group_ssid, groupid + ETH_ALEN,
-				    pwdinfo->p2p_group_ssid_len)) {
-				attr_contentlen = 0;
-
-				if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen,
-							     P2P_ATTR_DEVICE_ID,
-							     dev_addr,
-							     &attr_contentlen)) {
-					struct list_head *phead, *plist, *ptmp;
-
-					spin_lock_bh(&pstapriv->asoc_list_lock);
-					phead = &pstapriv->asoc_list;
-
-					list_for_each_safe(plist, ptmp, phead) {
-						psta = container_of(plist, struct sta_info, asoc_list);
-
-						if (psta->is_p2p_device && (psta->dev_cap&P2P_DEVCAP_CLIENT_DISCOVERABILITY) &&
-						   !memcmp(psta->dev_addr, dev_addr, ETH_ALEN))
-						{
-							/* spin_unlock_bh(&pstapriv->asoc_list_lock); */
-							/* issue GO Discoverability Request */
-							issue_group_disc_req(pwdinfo, psta->hwaddr);
-							/* spin_lock_bh(&pstapriv->asoc_list_lock); */
-							status = P2P_STATUS_SUCCESS;
-							break;
-						} else {
-							status = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
-						}
-					}
-					spin_unlock_bh(&pstapriv->asoc_list_lock);
-				} else {
-					status = P2P_STATUS_FAIL_INVALID_PARAM;
-				}
-			} else {
-				status = P2P_STATUS_FAIL_INVALID_PARAM;
-			}
-		}
-	}
-
-	/* issue Device Discoverability Response */
-	issue_p2p_devdisc_resp(pwdinfo, hdr->addr2, status, dialogToken);
-
-	return (status == P2P_STATUS_SUCCESS) ? true:false;
-}
-
-u32 process_p2p_devdisc_resp23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
-{
-	return true;
-}
-
-u8 process_p2p_provdisc_req23a(struct wifidirect_info *pwdinfo,
-			    u8 *pframe, uint len)
-{
-	u8 *frame_body;
-	u8 *wpsie;
-	u8 *ptr = NULL;
-	uint	wps_ielen = 0, attr_contentlen = 0;
-	u16	uconfig_method = 0;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)pframe;
-
-	frame_body = (pframe + sizeof(struct ieee80211_hdr_3addr));
-
-	wpsie = rtw_get_wps_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_,
-			       len - _PUBLIC_ACTION_IE_OFFSET_, NULL,
-			       &wps_ielen);
-	if (!wpsie)
-		goto out;
-
-	if (!rtw_get_wps_attr_content23a(wpsie, wps_ielen, WPS_ATTR_CONF_METHOD,
-				     (u8 *)&uconfig_method, &attr_contentlen))
-		goto out;
-
-	uconfig_method = be16_to_cpu(uconfig_method);
-	ptr = pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req;
-
-	switch (uconfig_method)
-	{
-	case WPS_CM_DISPLYA:
-		memcpy(ptr, "dis", 3);
-		break;
-
-	case WPS_CM_LABEL:
-		memcpy(ptr, "lab", 3);
-		break;
-
-	case WPS_CM_PUSH_BUTTON:
-		memcpy(ptr, "pbc", 3);
-		break;
-
-	case WPS_CM_KEYPAD:
-		memcpy(ptr, "pad", 3);
-		break;
-	}
-	issue_p2p_provision_resp(pwdinfo, hdr->addr2, frame_body,
-				 uconfig_method);
-
-out:
-	DBG_8723A("[%s] config method = %s\n", __func__, ptr);
-
-	return true;
-}
-
-u8 process_p2p_provdisc_resp23a(struct wifidirect_info *pwdinfo,  u8 *pframe)
-{
-
-	return true;
-}
-
-static u8 rtw_p2p_get_peer_ch_list(struct wifidirect_info *pwdinfo, u8 *ch_content, u8 ch_cnt, u8 *peer_ch_list)
-{
-	u8 i = 0, j = 0;
-	u8 temp = 0;
-	u8 ch_no = 0;
-	ch_content += 3;
-	ch_cnt -= 3;
-
-	while(ch_cnt > 0)
-	{
-		ch_content += 1;
-		ch_cnt -= 1;
-		temp = *ch_content;
-		for (i = 0 ; i < temp ; i++, j++)
-		{
-			peer_ch_list[j] = *(ch_content + 1 + i);
-		}
-		ch_content += (temp + 1);
-		ch_cnt -= (temp + 1);
-		ch_no += temp ;
-	}
-
-	return ch_no;
-}
-
-static u8 rtw_p2p_ch_inclusion(struct mlme_ext_priv *pmlmeext, u8 *peer_ch_list, u8 peer_ch_num, u8 *ch_list_inclusioned)
-{
-	int	i = 0, j = 0, temp = 0;
-	u8 ch_no = 0;
-
-	for (i = 0; i < peer_ch_num; i++)
-	{
-		for (j = temp; j < pmlmeext->max_chan_nums; j++)
-		{
-			if (*(peer_ch_list + i) == pmlmeext->channel_set[ j ].ChannelNum)
-			{
-				ch_list_inclusioned[ ch_no++ ] = *(peer_ch_list + i);
-				temp = j;
-				break;
-			}
-		}
-	}
-
-	return ch_no;
-}
-
-u8 process_p2p_group_negotation_req23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
-{
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	u8	result = P2P_STATUS_SUCCESS;
-	u32	p2p_ielen = 0, wps_ielen = 0;
-	u8 * ies;
-	u32 ies_len;
-	u8 *p2p_ie;
-	u8 *wpsie;
-	u16		wps_devicepassword_id = 0x0000;
-	uint	wps_devicepassword_id_len = 0;
-#ifdef CONFIG_8723AU_P2P
-	u8	wfd_ie[MAX_WFD_IE_LEN] = { 0x00 };
-	u32	wfd_ielen = 0;
-#endif /*  CONFIG_8723AU_P2P */
-
-	if ((wpsie = rtw_get_wps_ie23a(pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &wps_ielen)))
-	{
-		/*	Commented by Kurt 20120113 */
-		/*	If some device wants to do p2p handshake without sending prov_disc_req */
-		/*	We have to get peer_req_cm from here. */
-		if (!memcmp(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "000", 3))
-		{
-			rtw_get_wps_attr_content23a(wpsie, wps_ielen, WPS_ATTR_DEVICE_PWID, (u8*) &wps_devicepassword_id, &wps_devicepassword_id_len);
-			wps_devicepassword_id = be16_to_cpu(wps_devicepassword_id);
-
-			if (wps_devicepassword_id == WPS_DPID_USER_SPEC)
-			{
-				memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3);
-			}
-			else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC)
-			{
-				memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3);
-			}
-			else
-			{
-				memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3);
-			}
-		}
-	}
-	else
-	{
-		DBG_8723A("[%s] WPS IE not Found!!\n", __func__);
-		result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
-		return result;
-	}
-
-	if (pwdinfo->ui_got_wps_info == P2P_NO_WPSINFO)
-	{
-		result = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_TX_INFOR_NOREADY);
-		return result;
-	}
-
-	ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
-	ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
-
-	p2p_ie = rtw_get_p2p_ie23a(ies, ies_len, NULL, &p2p_ielen);
-
-	if (!p2p_ie)
-	{
-		DBG_8723A("[%s] P2P IE not Found!!\n", __func__);
-		result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
-	}
-
-	while (p2p_ie)
-	{
-		u8	attr_content = 0x00;
-		u32	attr_contentlen = 0;
-		u8	ch_content[50] = { 0x00 };
-		uint	ch_cnt = 0;
-		u8	peer_ch_list[50] = { 0x00 };
-		u8	peer_ch_num = 0;
-		u8	ch_list_inclusioned[50] = { 0x00 };
-		u8	ch_num_inclusioned = 0;
-		u16	cap_attr;
-
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_ING);
-
-		/* Check P2P Capability ATTR */
-		if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8*)&cap_attr, (uint*)&attr_contentlen))
-			cap_attr = le16_to_cpu(cap_attr);
-
-		if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT, &attr_content, &attr_contentlen))
-		{
-			DBG_8723A("[%s] GO Intent = %d, tie = %d\n", __func__, attr_content >> 1, attr_content & 0x01);
-			pwdinfo->peer_intent = attr_content;	/*	include both intent and tie breaker values. */
-
-			if (pwdinfo->intent == (pwdinfo->peer_intent >> 1))
-			{
-				/*	Try to match the tie breaker value */
-				if (pwdinfo->intent == P2P_MAX_INTENT)
-				{
-					rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-					result = P2P_STATUS_FAIL_BOTH_GOINTENT_15;
-				}
-				else
-				{
-					if (attr_content & 0x01)
-					{
-						rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-					}
-					else
-					{
-						rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
-					}
-				}
-			}
-			else if (pwdinfo->intent > (pwdinfo->peer_intent >> 1))
-			{
-				rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
-			}
-			else
-			{
-				rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-			}
-
-			if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-			{
-				/*	Store the group id information. */
-				memcpy(pwdinfo->groupid_info.go_device_addr, pwdinfo->device_addr, ETH_ALEN);
-				memcpy(pwdinfo->groupid_info.ssid, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen);
-			}
-		}
-
-		attr_contentlen = 0;
-		if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_INTENTED_IF_ADDR, pwdinfo->p2p_peer_interface_addr, &attr_contentlen))
-		{
-			if (attr_contentlen != ETH_ALEN)
-			{
-				memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN);
-			}
-		}
-
-		if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, ch_content, &ch_cnt))
-		{
-			peer_ch_num = rtw_p2p_get_peer_ch_list(pwdinfo, ch_content, ch_cnt, peer_ch_list);
-			ch_num_inclusioned = rtw_p2p_ch_inclusion(&padapter->mlmeextpriv, peer_ch_list, peer_ch_num, ch_list_inclusioned);
-
-			if (ch_num_inclusioned == 0)
-			{
-				DBG_8723A("[%s] No common channel in channel list!\n", __func__);
-				result = P2P_STATUS_FAIL_NO_COMMON_CH;
-				rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
-				break;
-			}
-
-			if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-			{
-				if (!rtw_p2p_is_channel_list_ok(pwdinfo->operating_channel,
-												ch_list_inclusioned, ch_num_inclusioned))
-				{
-					{
-						u8 operatingch_info[5] = { 0x00 }, peer_operating_ch = 0;
-						attr_contentlen = 0;
-
-						if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
-						{
-							peer_operating_ch = operatingch_info[4];
-						}
-
-						if (rtw_p2p_is_channel_list_ok(peer_operating_ch,
-														ch_list_inclusioned, ch_num_inclusioned))
-						{
-							/**
-							 *	Change our operating channel as peer's for compatibility.
-							 */
-							pwdinfo->operating_channel = peer_operating_ch;
-							DBG_8723A("[%s] Change op ch to %02x as peer's\n", __func__, pwdinfo->operating_channel);
-						}
-						else
-						{
-							/*  Take first channel of ch_list_inclusioned as operating channel */
-							pwdinfo->operating_channel = ch_list_inclusioned[0];
-							DBG_8723A("[%s] Change op ch to %02x\n", __func__, pwdinfo->operating_channel);
-						}
-					}
-
-				}
-			}
-		}
-
-		/* Get the next P2P IE */
-		p2p_ie = rtw_get_p2p_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
-	}
-
-#ifdef CONFIG_8723AU_P2P
-	/*	Added by Albert 20110823 */
-	/*	Try to get the TCP port information when receiving the negotiation request. */
-	if (rtw_get_wfd_ie(pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, wfd_ie, &wfd_ielen))
-	{
-		u8	attr_content[ 10 ] = { 0x00 };
-		u32	attr_contentlen = 0;
-
-		DBG_8723A("[%s] WFD IE Found!!\n", __func__);
-		rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen);
-		if (attr_contentlen)
-		{
-			pwdinfo->wfd_info->peer_rtsp_ctrlport = get_unaligned_be16(attr_content + 2);
-			DBG_8723A("[%s] Peer PORT NUM = %d\n", __func__, pwdinfo->wfd_info->peer_rtsp_ctrlport);
-		}
-	}
-#endif /*  CONFIG_8723AU_P2P */
-
-	return result;
-}
-
-u8 process_p2p_group_negotation_resp23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
-{
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	u8	result = P2P_STATUS_SUCCESS;
-	u32	p2p_ielen, wps_ielen;
-	u8 * ies;
-	u32 ies_len;
-	u8 * p2p_ie;
-#ifdef CONFIG_8723AU_P2P
-	u8	wfd_ie[MAX_WFD_IE_LEN] = { 0x00 };
-	u32	wfd_ielen = 0;
-#endif /*  CONFIG_8723AU_P2P */
-
-	ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
-	ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
-
-	/*	Be able to know which one is the P2P GO and which one is P2P client. */
-
-	if (rtw_get_wps_ie23a(ies, ies_len, NULL, &wps_ielen))
-	{
-
-	}
-	else
-	{
-		DBG_8723A("[%s] WPS IE not Found!!\n", __func__);
-		result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
-	}
-
-	p2p_ie = rtw_get_p2p_ie23a(ies, ies_len, NULL, &p2p_ielen);
-	if (!p2p_ie)
-	{
-		rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
-		result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
-	}
-	else
-	{
-
-		u8	attr_content = 0x00;
-		u32	attr_contentlen = 0;
-		u8	operatingch_info[5] = { 0x00 };
-		u8	groupid[ 38 ];
-		u16	cap_attr;
-		u8	peer_ch_list[50] = { 0x00 };
-		u8	peer_ch_num = 0;
-		u8	ch_list_inclusioned[50] = { 0x00 };
-		u8	ch_num_inclusioned = 0;
-
-		while (p2p_ie)	/*	Found the P2P IE. */
-		{
-
-			/* Check P2P Capability ATTR */
-			if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8*)&cap_attr, (uint*)&attr_contentlen))
-				cap_attr = le16_to_cpu(cap_attr);
-
-			rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
-			if (attr_contentlen == 1)
-			{
-				DBG_8723A("[%s] Status = %d\n", __func__, attr_content);
-				if (attr_content == P2P_STATUS_SUCCESS)
-				{
-					/*	Do nothing. */
-				}
-				else
-				{
-					if (P2P_STATUS_FAIL_INFO_UNAVAILABLE == attr_content) {
-						rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INFOR_NOREADY);
-					} else {
-						rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
-					}
-					rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-					result = attr_content;
-					break;
-				}
-			}
-
-			/*	Try to get the peer's interface address */
-			attr_contentlen = 0;
-			if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_INTENTED_IF_ADDR, pwdinfo->p2p_peer_interface_addr, &attr_contentlen))
-			{
-				if (attr_contentlen != ETH_ALEN)
-				{
-					memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN);
-				}
-			}
-
-			/*	Try to get the peer's intent and tie breaker value. */
-			attr_content = 0x00;
-			attr_contentlen = 0;
-			if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT, &attr_content, &attr_contentlen))
-			{
-				DBG_8723A("[%s] GO Intent = %d, tie = %d\n", __func__, attr_content >> 1, attr_content & 0x01);
-				pwdinfo->peer_intent = attr_content;	/*	include both intent and tie breaker values. */
-
-				if (pwdinfo->intent == (pwdinfo->peer_intent >> 1))
-				{
-					/*	Try to match the tie breaker value */
-					if (pwdinfo->intent == P2P_MAX_INTENT)
-					{
-						rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-						result = P2P_STATUS_FAIL_BOTH_GOINTENT_15;
-						rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
-					}
-					else
-					{
-						rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
-						rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
-						if (attr_content & 0x01)
-						{
-							rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-						}
-						else
-						{
-							rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
-						}
-					}
-				}
-				else if (pwdinfo->intent > (pwdinfo->peer_intent >> 1))
-				{
-					rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
-					rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
-					rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
-				}
-				else
-				{
-					rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
-					rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
-					rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-				}
-
-				if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-				{
-					/*	Store the group id information. */
-					memcpy(pwdinfo->groupid_info.go_device_addr, pwdinfo->device_addr, ETH_ALEN);
-					memcpy(pwdinfo->groupid_info.ssid, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen);
-
-				}
-			}
-
-			/*	Try to get the operation channel information */
-
-			attr_contentlen = 0;
-			if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
-			{
-				DBG_8723A("[%s] Peer's operating channel = %d\n", __func__, operatingch_info[4]);
-				pwdinfo->peer_operating_ch = operatingch_info[4];
-			}
-
-			/*	Try to get the channel list information */
-			if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, pwdinfo->channel_list_attr, &pwdinfo->channel_list_attr_len))
-			{
-				DBG_8723A("[%s] channel list attribute found, len = %d\n", __func__,  pwdinfo->channel_list_attr_len);
-
-				peer_ch_num = rtw_p2p_get_peer_ch_list(pwdinfo, pwdinfo->channel_list_attr, pwdinfo->channel_list_attr_len, peer_ch_list);
-				ch_num_inclusioned = rtw_p2p_ch_inclusion(&padapter->mlmeextpriv, peer_ch_list, peer_ch_num, ch_list_inclusioned);
-
-				if (ch_num_inclusioned == 0)
-				{
-					DBG_8723A("[%s] No common channel in channel list!\n", __func__);
-					result = P2P_STATUS_FAIL_NO_COMMON_CH;
-					rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
-					break;
-				}
-
-				if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-				{
-					if (!rtw_p2p_is_channel_list_ok(pwdinfo->operating_channel,
-													ch_list_inclusioned, ch_num_inclusioned))
-					{
-						{
-							u8 operatingch_info[5] = { 0x00 }, peer_operating_ch = 0;
-							attr_contentlen = 0;
-
-							if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
-							{
-								peer_operating_ch = operatingch_info[4];
-							}
-
-							if (rtw_p2p_is_channel_list_ok(peer_operating_ch,
-															ch_list_inclusioned, ch_num_inclusioned))
-							{
-								/**
-								 *	Change our operating channel as peer's for compatibility.
-								 */
-								pwdinfo->operating_channel = peer_operating_ch;
-								DBG_8723A("[%s] Change op ch to %02x as peer's\n", __func__, pwdinfo->operating_channel);
-							}
-							else
-							{
-								/*  Take first channel of ch_list_inclusioned as operating channel */
-								pwdinfo->operating_channel = ch_list_inclusioned[0];
-								DBG_8723A("[%s] Change op ch to %02x\n", __func__, pwdinfo->operating_channel);
-							}
-						}
-
-					}
-				}
-
-			}
-			else
-			{
-				DBG_8723A("[%s] channel list attribute not found!\n", __func__);
-			}
-
-			/*	Try to get the group id information if peer is GO */
-			attr_contentlen = 0;
-			memset(groupid, 0x00, 38);
-			if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen))
-			{
-				memcpy(pwdinfo->groupid_info.go_device_addr, &groupid[0], ETH_ALEN);
-				memcpy(pwdinfo->groupid_info.ssid, &groupid[6], attr_contentlen - ETH_ALEN);
-			}
-
-			/* Get the next P2P IE */
-			p2p_ie = rtw_get_p2p_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
-		}
-
-	}
-
-#ifdef CONFIG_8723AU_P2P
-	/*	Added by Albert 20111122 */
-	/*	Try to get the TCP port information when receiving the negotiation response. */
-	if (rtw_get_wfd_ie(pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, wfd_ie, &wfd_ielen))
-	{
-		u8	attr_content[ 10 ] = { 0x00 };
-		u32	attr_contentlen = 0;
-
-		DBG_8723A("[%s] WFD IE Found!!\n", __func__);
-		rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen);
-		if (attr_contentlen)
-		{
-			pwdinfo->wfd_info->peer_rtsp_ctrlport = get_unaligned_be16(attr_content + 2);
-			DBG_8723A("[%s] Peer PORT NUM = %d\n", __func__, pwdinfo->wfd_info->peer_rtsp_ctrlport);
-		}
-	}
-#endif /*  CONFIG_8723AU_P2P */
-
-	return result;
-}
-
-u8 process_p2p_group_negotation_confirm23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
-{
-	u8 * ies;
-	u32 ies_len;
-	u8 * p2p_ie;
-	u32	p2p_ielen = 0;
-	u8	result = P2P_STATUS_SUCCESS;
-	ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
-	ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
-
-	p2p_ie = rtw_get_p2p_ie23a(ies, ies_len, NULL, &p2p_ielen);
-	while (p2p_ie)	/*	Found the P2P IE. */
-	{
-		u8	attr_content = 0x00, operatingch_info[5] = { 0x00 };
-		u8	groupid[ 38 ] = { 0x00 };
-		u32	attr_contentlen = 0;
-
-		pwdinfo->negotiation_dialog_token = 1;
-		rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
-		if (attr_contentlen == 1)
-		{
-			DBG_8723A("[%s] Status = %d\n", __func__, attr_content);
-			result = attr_content;
-
-			if (attr_content == P2P_STATUS_SUCCESS)
-			{
-				del_timer_sync(&pwdinfo->restore_p2p_state_timer);
-
-				/*	Commented by Albert 20100911 */
-				/*	Todo: Need to handle the case which both Intents are the same. */
-				rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
-				rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
-				if ((pwdinfo->intent) > (pwdinfo->peer_intent >> 1))
-				{
-					rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
-				}
-				else if ((pwdinfo->intent) < (pwdinfo->peer_intent >> 1))
-				{
-					rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-				}
-				else
-				{
-					/*	Have to compare the Tie Breaker */
-					if (pwdinfo->peer_intent & 0x01)
-					{
-						rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-					}
-					else
-					{
-						rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
-					}
-				}
-			}
-			else
-			{
-				rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-				rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
-				break;
-			}
-		}
-
-		/*	Try to get the group id information */
-		attr_contentlen = 0;
-		memset(groupid, 0x00, 38);
-		if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen))
-		{
-			DBG_8723A("[%s] Ssid = %s, ssidlen = %zu\n", __func__, &groupid[ETH_ALEN], strlen(&groupid[ETH_ALEN]));
-			memcpy(pwdinfo->groupid_info.go_device_addr, &groupid[0], ETH_ALEN);
-			memcpy(pwdinfo->groupid_info.ssid, &groupid[6], attr_contentlen - ETH_ALEN);
-		}
-
-		attr_contentlen = 0;
-		if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
-		{
-			DBG_8723A("[%s] Peer's operating channel = %d\n", __func__, operatingch_info[4]);
-			pwdinfo->peer_operating_ch = operatingch_info[4];
-		}
-
-		/* Get the next P2P IE */
-		p2p_ie = rtw_get_p2p_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
-
-	}
-
-	return result;
-}
-
-u8 process_p2p_presence_req23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
-{
-	u8 *frame_body;
-	u8 dialogToken = 0;
-	u8 status = P2P_STATUS_SUCCESS;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) pframe;
-
-	frame_body = (unsigned char *)(pframe + sizeof(struct ieee80211_hdr_3addr));
-
-	dialogToken = frame_body[6];
-
-	/* todo: check NoA attribute */
-
-	issue_p2p_presence_resp(pwdinfo, hdr->addr2, status, dialogToken);
-
-	return true;
-}
-
-static void find_phase_handler(struct rtw_adapter *padapter)
-{
-	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct cfg80211_ssid ssid;
-	u8					_status = 0;
-
-
-
-	memset((unsigned char*)&ssid, 0, sizeof(struct cfg80211_ssid));
-	memcpy(ssid.ssid, pwdinfo->p2p_wildcard_ssid, P2P_WILDCARD_SSID_LEN);
-	ssid.ssid_len = P2P_WILDCARD_SSID_LEN;
-
-	rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH);
-
-	spin_lock_bh(&pmlmepriv->lock);
-	_status = rtw_sitesurvey_cmd23a(padapter, &ssid, 1, NULL, 0);
-	spin_unlock_bh(&pmlmepriv->lock);
-
-
-}
-
-void p2p_concurrent_handler(struct rtw_adapter* padapter);
-
-static void restore_p2p_state_handler(struct rtw_adapter *padapter)
-{
-	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
-
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
-		rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-
-	rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
-
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE)) {
-		/*	In the P2P client mode, the driver should not switch back to its listen channel */
-		/*	because this P2P client should stay at the operating channel of P2P GO. */
-		set_channel_bwmode23a(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
-	}
-}
-
-static void pre_tx_invitereq_handler(struct rtw_adapter *padapter)
-{
-	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
-	u8	val8 = 1;
-
-	set_channel_bwmode23a(padapter, pwdinfo->invitereq_info.peer_ch, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
-	padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
-	issue23a_probereq_p2p(padapter, NULL);
-	mod_timer(&pwdinfo->pre_tx_scan_timer,
-		  jiffies + msecs_to_jiffies(P2P_TX_PRESCAN_TIMEOUT));
-
-
-}
-
-static void pre_tx_provdisc_handler(struct rtw_adapter *padapter)
-{
-	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
-	u8	val8 = 1;
-
-
-	set_channel_bwmode23a(padapter, pwdinfo->tx_prov_disc_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
-	issue23a_probereq_p2p(padapter, NULL);
-	mod_timer(&pwdinfo->pre_tx_scan_timer,
-		  jiffies + msecs_to_jiffies(P2P_TX_PRESCAN_TIMEOUT));
-
-
-}
-
-static void pre_tx_negoreq_handler(struct rtw_adapter *padapter)
-{
-	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
-	u8	val8 = 1;
-
-
-	set_channel_bwmode23a(padapter, pwdinfo->nego_req_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
-	issue23a_probereq_p2p(padapter, NULL);
-	mod_timer(&pwdinfo->pre_tx_scan_timer,
-		  jiffies + msecs_to_jiffies(P2P_TX_PRESCAN_TIMEOUT));
-
-
-}
-
-static void ro_ch_handler(struct rtw_adapter *padapter)
-{
-	struct cfg80211_wifidirect_info *pcfg80211_wdinfo = &padapter->cfg80211_wdinfo;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-
-	if (pcfg80211_wdinfo->restore_channel != pmlmeext->cur_channel) {
-		if (!check_fwstate(&padapter->mlmepriv, _FW_LINKED))
-			pmlmeext->cur_channel = pcfg80211_wdinfo->restore_channel;
-
-		set_channel_bwmode23a(padapter, pmlmeext->cur_channel,
-				      HAL_PRIME_CHNL_OFFSET_DONT_CARE,
-				      HT_CHANNEL_WIDTH_20);
-	}
-
-	rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
-
-	pcfg80211_wdinfo->is_ro_ch = false;
-
-	DBG_8723A("cfg80211_remain_on_channel_expired\n");
-
-	rtw_cfg80211_remain_on_channel_expired(padapter,
-		pcfg80211_wdinfo->remain_on_ch_cookie,
-		&pcfg80211_wdinfo->remain_on_ch_channel,
-		pcfg80211_wdinfo->remain_on_ch_type, GFP_KERNEL);
-}
-
-static void ro_ch_timer_process (unsigned long data)
-{
-	struct rtw_adapter *adapter = (struct rtw_adapter *)data;
-
-	p2p_protocol_wk_cmd23a(adapter, P2P_RO_CH_WK);
-}
-
-#ifdef CONFIG_8723AU_P2P
-void rtw_append_wfd_ie(struct rtw_adapter *padapter, u8 *buf, u32* len)
-{
-	unsigned char	*frame_body;
-	u8 category, action, OUI_Subtype, dialogToken = 0;
-	u32	wfdielen = 0;
-
-	frame_body = (unsigned char *)(buf + sizeof(struct ieee80211_hdr_3addr));
-	category = frame_body[0];
-
-	if (category == WLAN_CATEGORY_PUBLIC) {
-		action = frame_body[1];
-		if (action == ACT_PUBLIC_VENDOR &&
-		    !memcmp(frame_body+2, P2P_OUI23A, 4)) {
-			OUI_Subtype = frame_body[6];
-			dialogToken = frame_body[7];
-			switch (OUI_Subtype)/* OUI Subtype */ {
-			case P2P_GO_NEGO_REQ:
-				wfdielen = build_nego_req_wfd_ie(&padapter->wdinfo, buf + (*len));
-				(*len) += wfdielen;
-				break;
-			case P2P_GO_NEGO_RESP:
-				wfdielen = build_nego_resp_wfd_ie(&padapter->wdinfo, buf + (*len));
-				(*len) += wfdielen;
-				break;
-			case P2P_GO_NEGO_CONF:
-				wfdielen = build_nego_confirm_wfd_ie(&padapter->wdinfo, buf + (*len));
-				(*len) += wfdielen;
-				break;
-			case P2P_INVIT_REQ:
-				wfdielen = build_invitation_req_wfd_ie(&padapter->wdinfo, buf + (*len));
-				(*len) += wfdielen;
-				break;
-			case P2P_INVIT_RESP:
-				wfdielen = build_invitation_resp_wfd_ie(&padapter->wdinfo, buf + (*len));
-				(*len) += wfdielen;
-				break;
-			case P2P_DEVDISC_REQ:
-				break;
-			case P2P_DEVDISC_RESP:
-				break;
-			case P2P_PROVISION_DISC_REQ:
-				wfdielen = build_provdisc_req_wfd_ie(&padapter->wdinfo, buf + (*len));
-				(*len) += wfdielen;
-				break;
-			case P2P_PROVISION_DISC_RESP:
-				wfdielen = build_provdisc_resp_wfd_ie(&padapter->wdinfo, buf + (*len));
-				(*len) += wfdielen;
-				break;
-			default:
-				break;
-			}
-		}
-	} else if (category == WLAN_CATEGORY_VENDOR_SPECIFIC) {
-		OUI_Subtype = frame_body[5];
-		dialogToken = frame_body[6];
-	} else {
-		DBG_8723A("%s, action frame category =%d\n", __func__, category);
-	}
-}
-#endif
-
-int rtw_p2p_check_frames(struct rtw_adapter *padapter, const u8 *buf, u32 len, u8 tx)
-{
-	int is_p2p_frame = (-1);
-	unsigned char	*frame_body;
-	u8 category, action, OUI_Subtype, dialogToken = 0;
-	u8 *p2p_ie = NULL;
-	uint p2p_ielen = 0;
-	struct rtw_wdev_priv *pwdev_priv = wdev_to_priv(padapter->rtw_wdev);
-
-	frame_body = (unsigned char *)(buf + sizeof(struct ieee80211_hdr_3addr));
-	category = frame_body[0];
-	/* just for check */
-	if (category == WLAN_CATEGORY_PUBLIC)
-	{
-		action = frame_body[1];
-		if (action == ACT_PUBLIC_VENDOR &&
-		    !memcmp(frame_body+2, P2P_OUI23A, 4)) {
-			OUI_Subtype = frame_body[6];
-			dialogToken = frame_body[7];
-			is_p2p_frame = OUI_Subtype;
-			p2p_ie = rtw_get_p2p_ie23a(
-				(u8 *)buf+sizeof(struct ieee80211_hdr_3addr)+_PUBLIC_ACTION_IE_OFFSET_,
-				len-sizeof(struct ieee80211_hdr_3addr)-_PUBLIC_ACTION_IE_OFFSET_,
-				NULL, &p2p_ielen);
-
-			switch (OUI_Subtype) {/* OUI Subtype */
-			u8 *cont;
-			uint cont_len;
-			case P2P_GO_NEGO_REQ:
-				DBG_8723A("RTW_%s:P2P_GO_NEGO_REQ, dialogToken =%d\n", (tx == true)?"Tx":"Rx", dialogToken);
-				break;
-			case P2P_GO_NEGO_RESP:
-				cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len);
-				DBG_8723A("RTW_%s:P2P_GO_NEGO_RESP, dialogToken =%d, status:%d\n", (tx == true)?"Tx":"Rx", dialogToken, cont?*cont:-1);
-
-				if (!tx)
-					pwdev_priv->provdisc_req_issued = false;
-				break;
-			case P2P_GO_NEGO_CONF:
-				cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len);
-				DBG_8723A("RTW_%s:P2P_GO_NEGO_CONF, dialogToken =%d, status:%d\n",
-					  (tx == true)?"Tx":"Rx", dialogToken, cont?*cont:-1);
-				break;
-			case P2P_INVIT_REQ:
-			{
-				struct rtw_wdev_invit_info* invit_info = &pwdev_priv->invit_info;
-				int flags = -1;
-				int op_ch = 0;
-
-				if ((cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_INVITATION_FLAGS, NULL, &cont_len)))
-					flags = *cont;
-				if ((cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len)))
-					op_ch = *(cont+4);
-
-				if (invit_info->token != dialogToken)
-					rtw_wdev_invit_info_init(invit_info);
-
-				invit_info->token = dialogToken;
-				invit_info->flags = (flags ==-1) ? 0x0 : flags;
-				invit_info->req_op_ch = op_ch;
-
-				DBG_8723A("RTW_%s:P2P_INVIT_REQ, dialogToken =%d, flags:0x%02x, op_ch:%d\n",
-					  (tx) ? "Tx" : "Rx", dialogToken, flags, op_ch);
-				break;
-			}
-			case P2P_INVIT_RESP:
-			{
-				struct rtw_wdev_invit_info* invit_info = &pwdev_priv->invit_info;
-				int status = -1;
-				int op_ch = 0;
-
-				if ((cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len)))
-					status = *cont;
-				if ((cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len)))
-					op_ch = *(cont+4);
-
-				if (invit_info->token != dialogToken) {
-					rtw_wdev_invit_info_init(invit_info);
-				} else {
-					invit_info->token = 0;
-					invit_info->status = (status ==-1) ? 0xff : status;
-					invit_info->rsp_op_ch = op_ch;
-				}
-
-				DBG_8723A("RTW_%s:P2P_INVIT_RESP, dialogToken =%d, status:%d, op_ch:%d\n",
-					  (tx == true)?"Tx":"Rx", dialogToken, status, op_ch);
-				break;
-			}
-			case P2P_DEVDISC_REQ:
-				DBG_8723A("RTW_%s:P2P_DEVDISC_REQ, dialogToken =%d\n", (tx == true)?"Tx":"Rx", dialogToken);
-				break;
-			case P2P_DEVDISC_RESP:
-				cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len);
-				DBG_8723A("RTW_%s:P2P_DEVDISC_RESP, dialogToken =%d, status:%d\n", (tx == true)?"Tx":"Rx", dialogToken, cont?*cont:-1);
-				break;
-			case P2P_PROVISION_DISC_REQ:
-			{
-				size_t frame_body_len = len - sizeof(struct ieee80211_hdr_3addr);
-				u8 *p2p_ie;
-				uint p2p_ielen = 0;
-				uint contentlen = 0;
-
-				DBG_8723A("RTW_%s:P2P_PROVISION_DISC_REQ, dialogToken =%d\n", (tx == true)?"Tx":"Rx", dialogToken);
-
-				pwdev_priv->provdisc_req_issued = false;
-
-				p2p_ie = rtw_get_p2p_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_,
-							   frame_body_len - _PUBLIC_ACTION_IE_OFFSET_,
-							   NULL, &p2p_ielen);
-				if (p2p_ie) {
-					if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, NULL, &contentlen))
-						pwdev_priv->provdisc_req_issued = false;/* case: p2p_client join p2p GO */
-					else
-						pwdev_priv->provdisc_req_issued = true;/* case: p2p_devices connection before Nego req. */
-				}
-			}
-				break;
-			case P2P_PROVISION_DISC_RESP:
-				DBG_8723A("RTW_%s:P2P_PROVISION_DISC_RESP, dialogToken =%d\n", (tx == true)?"Tx":"Rx", dialogToken);
-				break;
-			default:
-				DBG_8723A("RTW_%s:OUI_Subtype =%d, dialogToken =%d\n", (tx == true)?"Tx":"Rx", OUI_Subtype, dialogToken);
-				break;
-			}
-
-		}
-
-	}
-	else if (category == WLAN_CATEGORY_VENDOR_SPECIFIC)
-	{
-		OUI_Subtype = frame_body[5];
-		dialogToken = frame_body[6];
-
-		is_p2p_frame = OUI_Subtype;
-
-		switch (OUI_Subtype) {
-		case P2P_NOTICE_OF_ABSENCE:
-			DBG_8723A("RTW_%s:P2P_NOTICE_OF_ABSENCE, dialogToken =%d\n", (tx == true)?"TX":"RX", dialogToken);
-			break;
-		case P2P_PRESENCE_REQUEST:
-			DBG_8723A("RTW_%s:P2P_PRESENCE_REQUEST, dialogToken =%d\n", (tx == true)?"TX":"RX", dialogToken);
-			break;
-		case P2P_PRESENCE_RESPONSE:
-			DBG_8723A("RTW_%s:P2P_PRESENCE_RESPONSE, dialogToken =%d\n", (tx == true)?"TX":"RX", dialogToken);
-			break;
-		case P2P_GO_DISC_REQUEST:
-			DBG_8723A("RTW_%s:P2P_GO_DISC_REQUEST, dialogToken =%d\n", (tx == true)?"TX":"RX", dialogToken);
-			break;
-		default:
-			DBG_8723A("RTW_%s:OUI_Subtype =%d, dialogToken =%d\n", (tx == true)?"TX":"RX", OUI_Subtype, dialogToken);
-			break;
-		}
-
-	} else {
-		DBG_8723A("RTW_%s:action frame category =%d\n", (tx == true)?"TX":"RX", category);
-	}
-	return is_p2p_frame;
-}
-
-void rtw_init_cfg80211_wifidirect_info(struct rtw_adapter *padapter)
-{
-	struct cfg80211_wifidirect_info *pcfg80211_wdinfo = &padapter->cfg80211_wdinfo;
-
-	memset(pcfg80211_wdinfo, 0x00, sizeof(struct cfg80211_wifidirect_info));
-
-	setup_timer(&pcfg80211_wdinfo->remain_on_ch_timer,
-		    ro_ch_timer_process, (unsigned long)padapter);
-}
-
-void p2p_protocol_wk_hdl23a(struct rtw_adapter *padapter, int intCmdType)
-{
-	switch (intCmdType) {
-	case P2P_FIND_PHASE_WK:
-		find_phase_handler(padapter);
-		break;
-	case P2P_RESTORE_STATE_WK:
-		restore_p2p_state_handler(padapter);
-		break;
-	case P2P_PRE_TX_PROVDISC_PROCESS_WK:
-		pre_tx_provdisc_handler(padapter);
-		break;
-	case P2P_PRE_TX_INVITEREQ_PROCESS_WK:
-		pre_tx_invitereq_handler(padapter);
-		break;
-	case P2P_PRE_TX_NEGOREQ_PROCESS_WK:
-		pre_tx_negoreq_handler(padapter);
-		break;
-	case P2P_RO_CH_WK:
-		ro_ch_handler(padapter);
-		break;
-	}
-}
-
-#ifdef CONFIG_8723AU_P2P
-void process_p2p_ps_ie23a(struct rtw_adapter *padapter, u8 *IEs, u32 IELength)
-{
-	u8 * ies;
-	u32 ies_len;
-	u8 * p2p_ie;
-	u32	p2p_ielen = 0;
-	u8	noa_attr[MAX_P2P_IE_LEN] = { 0x00 };/*  NoA length should be n*(13) + 2 */
-	u32	attr_contentlen = 0;
-
-	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
-	u8	find_p2p = false, find_p2p_ps = false;
-	u8	noa_offset, noa_num, noa_index;
-
-
-
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
-	{
-		return;
-	}
-	if (IELength <= _BEACON_IE_OFFSET_)
-		return;
-
-	ies = IEs + _BEACON_IE_OFFSET_;
-	ies_len = IELength - _BEACON_IE_OFFSET_;
-
-	p2p_ie = rtw_get_p2p_ie23a(ies, ies_len, NULL, &p2p_ielen);
-
-	while(p2p_ie)
-	{
-		find_p2p = true;
-		/*  Get Notice of Absence IE. */
-		if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_NOA, noa_attr, &attr_contentlen))
-		{
-			find_p2p_ps = true;
-			noa_index = noa_attr[0];
-
-			if ((pwdinfo->p2p_ps_mode == P2P_PS_NONE) ||
-				(noa_index != pwdinfo->noa_index))/*  if index change, driver should reconfigure related setting. */
-			{
-				pwdinfo->noa_index = noa_index;
-				pwdinfo->opp_ps = noa_attr[1] >> 7;
-				pwdinfo->ctwindow = noa_attr[1] & 0x7F;
-
-				noa_offset = 2;
-				noa_num = 0;
-				/*  NoA length should be n*(13) + 2 */
-				if (attr_contentlen > 2)
-				{
-					while(noa_offset < attr_contentlen)
-					{
-						/* memcpy(&wifidirect_info->noa_count[noa_num], &noa_attr[noa_offset], 1); */
-						pwdinfo->noa_count[noa_num] = noa_attr[noa_offset];
-						noa_offset += 1;
-
-						memcpy(&pwdinfo->noa_duration[noa_num], &noa_attr[noa_offset], 4);
-						noa_offset += 4;
-
-						memcpy(&pwdinfo->noa_interval[noa_num], &noa_attr[noa_offset], 4);
-						noa_offset += 4;
-
-						memcpy(&pwdinfo->noa_start_time[noa_num], &noa_attr[noa_offset], 4);
-						noa_offset += 4;
-
-						noa_num++;
-					}
-				}
-				pwdinfo->noa_num = noa_num;
-
-				if (pwdinfo->opp_ps == 1)
-				{
-					pwdinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
-					/*  driver should wait LPS for entering CTWindow */
-					if (padapter->pwrctrlpriv.bFwCurrentInPSMode == true)
-					{
-						p2p_ps_wk_cmd23a(padapter, P2P_PS_ENABLE, 1);
-					}
-				}
-				else if (pwdinfo->noa_num > 0)
-				{
-					pwdinfo->p2p_ps_mode = P2P_PS_NOA;
-					p2p_ps_wk_cmd23a(padapter, P2P_PS_ENABLE, 1);
-				}
-				else if (pwdinfo->p2p_ps_mode > P2P_PS_NONE)
-				{
-					p2p_ps_wk_cmd23a(padapter, P2P_PS_DISABLE, 1);
-				}
-			}
-
-			break; /*  find target, just break. */
-		}
-
-		/* Get the next P2P IE */
-		p2p_ie = rtw_get_p2p_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
-
-	}
-
-	if (find_p2p == true)
-	{
-		if ((pwdinfo->p2p_ps_mode > P2P_PS_NONE) && (find_p2p_ps == false))
-		{
-			p2p_ps_wk_cmd23a(padapter, P2P_PS_DISABLE, 1);
-		}
-	}
-
-
-}
-
-void p2p_ps_wk_hdl23a(struct rtw_adapter *padapter, u8 p2p_ps_state)
-{
-	struct pwrctrl_priv		*pwrpriv = &padapter->pwrctrlpriv;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-
-
-
-	/*  Pre action for p2p state */
-	switch (p2p_ps_state)
-	{
-		case P2P_PS_DISABLE:
-			pwdinfo->p2p_ps_state = p2p_ps_state;
-
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state));
-
-			pwdinfo->noa_index = 0;
-			pwdinfo->ctwindow = 0;
-			pwdinfo->opp_ps = 0;
-			pwdinfo->noa_num = 0;
-			pwdinfo->p2p_ps_mode = P2P_PS_NONE;
-			if (padapter->pwrctrlpriv.bFwCurrentInPSMode == true)
-			{
-				if (pwrpriv->smart_ps == 0)
-				{
-					pwrpriv->smart_ps = 2;
-					rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)&padapter->pwrctrlpriv.pwr_mode);
-				}
-			}
-			break;
-		case P2P_PS_ENABLE:
-			if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) {
-				pwdinfo->p2p_ps_state = p2p_ps_state;
-
-				if (pwdinfo->ctwindow > 0)
-				{
-					if (pwrpriv->smart_ps != 0)
-					{
-						pwrpriv->smart_ps = 0;
-						DBG_8723A("%s(): Enter CTW, change SmartPS\n", __func__);
-						rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)&padapter->pwrctrlpriv.pwr_mode);
-					}
-				}
-				rtw_hal_set_hwreg23a(padapter, 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 (pwdinfo->p2p_ps_mode > P2P_PS_NONE) {
-				pwdinfo->p2p_ps_state = p2p_ps_state;
-				rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state));
-			}
-			break;
-		default:
-			break;
-	}
-
-
-}
-
-u8 p2p_ps_wk_cmd23a(struct rtw_adapter*padapter, u8 p2p_ps_state, u8 enqueue)
-{
-	struct cmd_obj	*ph2c;
-	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
-	u8	res = _SUCCESS;
-
-
-
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
-		return res;
-
-	if (enqueue) {
-		ph2c = (struct cmd_obj *)kzalloc(sizeof(struct cmd_obj),
-						 GFP_ATOMIC);
-		if (!ph2c) {
-			res = _FAIL;
-			goto exit;
-		}
-
-		pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)
-			kzalloc(sizeof(struct drvextra_cmd_parm), GFP_ATOMIC);
-		if (pdrvextra_cmd_parm == NULL) {
-			kfree(ph2c);
-			res = _FAIL;
-			goto exit;
-		}
-
-		pdrvextra_cmd_parm->ec_id = P2P_PS_WK_CID;
-		pdrvextra_cmd_parm->type_size = p2p_ps_state;
-		pdrvextra_cmd_parm->pbuf = NULL;
-
-		init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
-
-		res = rtw_enqueue_cmd23a(pcmdpriv, ph2c);
-	}
-	else
-	{
-		p2p_ps_wk_hdl23a(padapter, p2p_ps_state);
-	}
-
-exit:
-
-
-
-	return res;
-}
-#endif /*  CONFIG_8723AU_P2P */
-
-static void reset_ch_sitesurvey_timer_process(unsigned long data)
-{
-	struct rtw_adapter *adapter = (struct rtw_adapter *)data;
-	struct  wifidirect_info *pwdinfo = &adapter->wdinfo;
-
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
-		return;
-
-	DBG_8723A("[%s] In\n", __func__);
-	/*	Reset the operation channel information */
-	pwdinfo->rx_invitereq_info.operation_ch[0] = 0;
-	pwdinfo->rx_invitereq_info.scan_op_ch_only = 0;
-}
-
-static void reset_ch_sitesurvey_timer_process2(unsigned long data)
-{
-	struct rtw_adapter *adapter = (struct rtw_adapter *)data;
-	struct wifidirect_info *pwdinfo = &adapter->wdinfo;
-
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
-		return;
-
-	DBG_8723A("[%s] In\n", __func__);
-	/*	Reset the operation channel information */
-	pwdinfo->p2p_info.operation_ch[0] = 0;
-	pwdinfo->p2p_info.scan_op_ch_only = 0;
-}
-
-static void restore_p2p_state_timer_process (unsigned long data)
-{
-	struct rtw_adapter *adapter = (struct rtw_adapter *)data;
-	struct	wifidirect_info		*pwdinfo = &adapter->wdinfo;
-
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
-		return;
-
-	p2p_protocol_wk_cmd23a(adapter, P2P_RESTORE_STATE_WK);
-}
-
-static void pre_tx_scan_timer_process (unsigned long data)
-{
-	struct rtw_adapter *adapter = (struct rtw_adapter *)data;
-	struct wifidirect_info *pwdinfo = &adapter->wdinfo;
-	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
-
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
-		return;
-
-	spin_lock_bh(&pmlmepriv->lock);
-
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ))
-	{
-		if (true == pwdinfo->tx_prov_disc_info.benable)	/*	the provision discovery request frame is trigger to send or not */
-		{
-			p2p_protocol_wk_cmd23a(adapter, P2P_PRE_TX_PROVDISC_PROCESS_WK);
-			/* issue23a_probereq_p2p(adapter, NULL); */
-			/* _set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT); */
-		}
-	}
-	else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
-	{
-		if (true == pwdinfo->nego_req_info.benable)
-		{
-			p2p_protocol_wk_cmd23a(adapter, P2P_PRE_TX_NEGOREQ_PROCESS_WK);
-		}
-	}
-	else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ))
-	{
-		if (true == pwdinfo->invitereq_info.benable)
-		{
-			p2p_protocol_wk_cmd23a(adapter, P2P_PRE_TX_INVITEREQ_PROCESS_WK);
-		}
-	}
-	else
-	{
-		DBG_8723A("[%s] p2p_state is %d, ignore!!\n", __func__, rtw_p2p_state(pwdinfo));
-	}
-
-	spin_unlock_bh(&pmlmepriv->lock);
-}
-
-static void find_phase_timer_process (unsigned long data)
-{
-	struct rtw_adapter *adapter = (struct rtw_adapter *)data;
-	struct	wifidirect_info		*pwdinfo = &adapter->wdinfo;
-
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
-		return;
-
-	adapter->wdinfo.find_phase_state_exchange_cnt++;
-
-	p2p_protocol_wk_cmd23a(adapter, P2P_FIND_PHASE_WK);
-}
-
-void reset_global_wifidirect_info23a(struct rtw_adapter *padapter)
-{
-	struct wifidirect_info	*pwdinfo;
-
-	pwdinfo = &padapter->wdinfo;
-	pwdinfo->persistent_supported = 0;
-	pwdinfo->session_available = true;
-	pwdinfo->wfd_tdls_enable = 0;
-	pwdinfo->wfd_tdls_weaksec = 0;
-}
-
-#ifdef CONFIG_8723AU_P2P
-int rtw_init_wifi_display_info(struct rtw_adapter* padapter)
-{
-	int	res = _SUCCESS;
-	struct wifi_display_info *pwfd_info = &padapter->wfd_info;
-
-	/*  Used in P2P and TDLS */
-	pwfd_info->rtsp_ctrlport = 554;
-	pwfd_info->peer_rtsp_ctrlport = 0;	/*	Reset to 0 */
-	pwfd_info->wfd_enable = false;
-	pwfd_info->wfd_device_type = WFD_DEVINFO_PSINK;
-	pwfd_info->scan_result_type = SCAN_RESULT_P2P_ONLY;
-
-	/*  Used in P2P */
-	pwfd_info->peer_session_avail = true;
-	pwfd_info->wfd_pc = false;
-
-	/*  Used in TDLS */
-	memset(pwfd_info->ip_address, 0x00, 4);
-	memset(pwfd_info->peer_ip_address, 0x00, 4);
-	return res;
-}
-#endif /* CONFIG_8723AU_P2P */
-
-void rtw_init_wifidirect_timers23a(struct rtw_adapter* padapter)
-{
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-
-	setup_timer(&pwdinfo->find_phase_timer, find_phase_timer_process,
-		    (unsigned long)padapter);
-	setup_timer(&pwdinfo->restore_p2p_state_timer,
-		    restore_p2p_state_timer_process, (unsigned long)padapter);
-	setup_timer(&pwdinfo->pre_tx_scan_timer, pre_tx_scan_timer_process,
-		    (unsigned long)padapter);
-	setup_timer(&pwdinfo->reset_ch_sitesurvey,
-		    reset_ch_sitesurvey_timer_process, (unsigned long)padapter);
-	setup_timer(&pwdinfo->reset_ch_sitesurvey2,
-		    reset_ch_sitesurvey_timer_process2,
-		    (unsigned long)padapter);
-}
-
-void rtw_init_wifidirect_addrs23a(struct rtw_adapter* padapter, u8 *dev_addr, u8 *iface_addr)
-{
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-
-	/*init device&interface address */
-	if (dev_addr) {
-		memcpy(pwdinfo->device_addr, dev_addr, ETH_ALEN);
-	}
-	if (iface_addr) {
-		memcpy(pwdinfo->interface_addr, iface_addr, ETH_ALEN);
-	}
-#endif
-}
-
-void init_wifidirect_info23a(struct rtw_adapter *padapter, enum P2P_ROLE role)
-{
-	struct wifidirect_info	*pwdinfo;
-#ifdef CONFIG_8723AU_P2P
-	struct wifi_display_info	*pwfd_info = &padapter->wfd_info;
-#endif
-
-	pwdinfo = &padapter->wdinfo;
-
-	pwdinfo->padapter = padapter;
-
-	/*	1, 6, 11 are the social channel defined in the WiFi Direct specification. */
-	pwdinfo->social_chan[0] = 1;
-	pwdinfo->social_chan[1] = 6;
-	pwdinfo->social_chan[2] = 11;
-	pwdinfo->social_chan[3] = 0;	/*	channel 0 for scanning ending in site survey function. */
-
-	/*	Use the channel 11 as the listen channel */
-	pwdinfo->listen_channel = 11;
-
-	if (role == P2P_ROLE_DEVICE)
-	{
-		rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_LISTEN);
-		pwdinfo->intent = 1;
-		rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_LISTEN);
-	}
-	else if (role == P2P_ROLE_CLIENT)
-	{
-		rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
-		pwdinfo->intent = 1;
-		rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
-	}
-	else if (role == P2P_ROLE_GO)
-	{
-		rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
-		pwdinfo->intent = 15;
-		rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
-	}
-
-/*	Use the OFDM rate in the P2P probe response frame. (6(B), 9(B), 12, 18, 24, 36, 48, 54) */
-	pwdinfo->support_rate[0] = 0x8c;	/*	6(B) */
-	pwdinfo->support_rate[1] = 0x92;	/*	9(B) */
-	pwdinfo->support_rate[2] = 0x18;	/*	12 */
-	pwdinfo->support_rate[3] = 0x24;	/*	18 */
-	pwdinfo->support_rate[4] = 0x30;	/*	24 */
-	pwdinfo->support_rate[5] = 0x48;	/*	36 */
-	pwdinfo->support_rate[6] = 0x60;	/*	48 */
-	pwdinfo->support_rate[7] = 0x6c;	/*	54 */
-
-	memcpy((void*) pwdinfo->p2p_wildcard_ssid, "DIRECT-", 7);
-
-	memset(pwdinfo->device_name, 0x00, WPS_MAX_DEVICE_NAME_LEN);
-	pwdinfo->device_name_len = 0;
-
-	memset(&pwdinfo->invitereq_info, 0x00, sizeof(struct tx_invite_req_info));
-	pwdinfo->invitereq_info.token = 3;	/*	Token used for P2P invitation request frame. */
-
-	memset(&pwdinfo->inviteresp_info, 0x00, sizeof(struct tx_invite_resp_info));
-	pwdinfo->inviteresp_info.token = 0;
-
-	pwdinfo->profileindex = 0;
-	memset(&pwdinfo->profileinfo[ 0 ], 0x00, sizeof(struct profile_info) * P2P_MAX_PERSISTENT_GROUP_NUM);
-
-	rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_NONE);
-
-	pwdinfo->listen_dwell = (u8) ((jiffies % 3) + 1);
-	/* DBG_8723A("[%s] listen_dwell time is %d00ms\n", __func__, pwdinfo->listen_dwell); */
-
-	memset(&pwdinfo->tx_prov_disc_info, 0x00, sizeof(struct tx_provdisc_req_info));
-	pwdinfo->tx_prov_disc_info.wps_config_method_request = WPS_CM_NONE;
-
-	memset(&pwdinfo->nego_req_info, 0x00, sizeof(struct tx_nego_req_info));
-
-	pwdinfo->device_password_id_for_nego = WPS_DPID_PBC;
-	pwdinfo->negotiation_dialog_token = 1;
-
-	memset(pwdinfo->nego_ssid, 0x00, IEEE80211_MAX_SSID_LEN);
-	pwdinfo->nego_ssidlen = 0;
-
-	pwdinfo->ui_got_wps_info = P2P_NO_WPSINFO;
-#ifdef CONFIG_8723AU_P2P
-	pwdinfo->supported_wps_cm = WPS_CONFIG_METHOD_DISPLAY  | WPS_CONFIG_METHOD_PBC;
-	pwdinfo->wfd_info = pwfd_info;
-#else
-	pwdinfo->supported_wps_cm = WPS_CONFIG_METHOD_DISPLAY | WPS_CONFIG_METHOD_PBC | WPS_CONFIG_METHOD_KEYPAD;
-#endif /* CONFIG_8723AU_P2P */
-	pwdinfo->channel_list_attr_len = 0;
-	memset(pwdinfo->channel_list_attr, 0x00, 100);
-
-	memset(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, 0x00, 4);
-	memset(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, '0', 3);
-	memset(&pwdinfo->groupid_info, 0x00, sizeof(struct group_id_info));
-	pwdinfo->wfd_tdls_enable = 0;
-	memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN);
-	memset(pwdinfo->p2p_peer_device_addr, 0x00, ETH_ALEN);
-
-	pwdinfo->rx_invitereq_info.operation_ch[0] = 0;
-	pwdinfo->rx_invitereq_info.operation_ch[1] = 0;	/*	Used to indicate the scan end in site survey function */
-	pwdinfo->rx_invitereq_info.scan_op_ch_only = 0;
-	pwdinfo->p2p_info.operation_ch[0] = 0;
-	pwdinfo->p2p_info.operation_ch[1] = 0;			/*	Used to indicate the scan end in site survey function */
-	pwdinfo->p2p_info.scan_op_ch_only = 0;
-}
-
-int rtw_p2p_enable23a(struct rtw_adapter *padapter, enum P2P_ROLE role)
-{
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	int ret = _SUCCESS;
-
-	if (role == P2P_ROLE_DEVICE || role == P2P_ROLE_CLIENT ||
-	    role == P2P_ROLE_GO) {
-		/* leave IPS/Autosuspend */
-		if (_FAIL == rtw_pwr_wakeup(padapter)) {
-			ret = _FAIL;
-			goto exit;
-		}
-
-		/*	Added by Albert 2011/03/22 */
-		/*	In the P2P mode, the driver should not support the b mode. */
-		/*	So, the Tx packet shouldn't use the CCK rate */
-		update_tx_basic_rate23a(padapter, WIRELESS_11AGN);
-
-		/* Enable P2P function */
-		init_wifidirect_info23a(padapter, role);
-
-		rtw_hal_set_odm_var23a(padapter, HAL_ODM_P2P_STATE, NULL, true);
-		#ifdef CONFIG_8723AU_P2P
-		rtw_hal_set_odm_var23a(padapter, HAL_ODM_WIFI_DISPLAY_STATE, NULL, true);
-		#endif
-
-	}
-	else if (role == P2P_ROLE_DISABLE)
-	{
-		if (_FAIL == rtw_pwr_wakeup(padapter)) {
-			ret = _FAIL;
-			goto exit;
-		}
-
-		/* Disable P2P function */
-		if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
-		{
-			del_timer_sync(&pwdinfo->find_phase_timer);
-			del_timer_sync(&pwdinfo->restore_p2p_state_timer);
-			del_timer_sync(&pwdinfo->pre_tx_scan_timer);
-			del_timer_sync(&pwdinfo->reset_ch_sitesurvey);
-			del_timer_sync(&pwdinfo->reset_ch_sitesurvey2);
-			reset_ch_sitesurvey_timer_process((unsigned long)padapter);
-			reset_ch_sitesurvey_timer_process2((unsigned long)padapter);
-			rtw_p2p_set_state(pwdinfo, P2P_STATE_NONE);
-			rtw_p2p_set_role(pwdinfo, P2P_ROLE_DISABLE);
-			memset(&pwdinfo->rx_prov_disc_info, 0x00, sizeof(struct rx_provdisc_req_info));
-		}
-
-		rtw_hal_set_odm_var23a(padapter, HAL_ODM_P2P_STATE, NULL, false);
-		#ifdef CONFIG_8723AU_P2P
-		rtw_hal_set_odm_var23a(padapter, HAL_ODM_WIFI_DISPLAY_STATE, NULL, false);
-		#endif
-
-		/* Restore to initial setting. */
-		update_tx_basic_rate23a(padapter, padapter->registrypriv.wireless_mode);
-	}
-
-exit:
-	return ret;
-}
-
-#endif /* CONFIG_8723AU_P2P */
diff --git a/drivers/staging/rtl8723au/core/rtw_pwrctrl.c b/drivers/staging/rtl8723au/core/rtw_pwrctrl.c
index 8ddd67f194b..dbd01b652e0 100644
--- a/drivers/staging/rtl8723au/core/rtw_pwrctrl.c
+++ b/drivers/staging/rtl8723au/core/rtw_pwrctrl.c
@@ -17,10 +17,11 @@
 #include <osdep_service.h>
 #include <drv_types.h>
 #include <osdep_intf.h>
+#include <rtl8723a_cmd.h>
+#include <rtw_sreset.h>
 
-#ifdef CONFIG_8723AU_BT_COEXIST
-#include <rtl8723a_hal.h>
-#endif
+#include <rtl8723a_bt_intf.h>
+#include <usb_ops_linux.h>
 
 void ips_enter23a(struct rtw_adapter * padapter)
 {
@@ -35,11 +36,9 @@ void ips_enter23a(struct rtw_adapter * padapter)
 
 	pwrpriv->ips_enter23a_cnts++;
 	DBG_8723A("==>ips_enter23a cnts:%d\n", pwrpriv->ips_enter23a_cnts);
-#ifdef CONFIG_8723AU_BT_COEXIST
-	BTDM_TurnOffBtCoexistBeforeEnterIPS(padapter);
-#endif
-	if (rf_off == pwrpriv->change_rfpwrstate)
-	{
+	rtl8723a_BT_disable_coexist(padapter);
+
+	if (pwrpriv->change_rfpwrstate == rf_off) {
 		pwrpriv->bpower_saving = true;
 		DBG_8723A_LEVEL(_drv_always_, "nolinked power save enter\n");
 
@@ -64,25 +63,34 @@ int ips_leave23a(struct rtw_adapter * padapter)
 
 	down(&pwrpriv->lock);
 
-	if ((pwrpriv->rf_pwrstate == rf_off) &&!pwrpriv->bips_processing)
-	{
+	if (pwrpriv->rf_pwrstate == rf_off && !pwrpriv->bips_processing) {
 		pwrpriv->bips_processing = true;
 		pwrpriv->change_rfpwrstate = rf_on;
 		pwrpriv->ips_leave23a_cnts++;
-		DBG_8723A("==>ips_leave23a cnts:%d\n", pwrpriv->ips_leave23a_cnts);
+		DBG_8723A("==>ips_leave23a cnts:%d\n",
+			  pwrpriv->ips_leave23a_cnts);
 
-		if ((result = rtw_ips_pwr_up23a(padapter)) == _SUCCESS) {
+		result = rtw_ips_pwr_up23a(padapter);
+		if (result == _SUCCESS)
 			pwrpriv->rf_pwrstate = rf_on;
-		}
+
 		DBG_8723A_LEVEL(_drv_always_, "nolinked power save leave\n");
 
-		if ((_WEP40_ == psecuritypriv->dot11PrivacyAlgrthm) ||(_WEP104_ == psecuritypriv->dot11PrivacyAlgrthm))
-		{
-			DBG_8723A("==>%s, channel(%d), processing(%x)\n", __func__, padapter->mlmeextpriv.cur_channel, pwrpriv->bips_processing);
-			set_channel_bwmode23a(padapter, padapter->mlmeextpriv.cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
-			for (keyid = 0;keyid<4;keyid++) {
-				if (pmlmepriv->key_mask & CHKBIT(keyid)) {
-					if (keyid == psecuritypriv->dot11PrivacyKeyIndex)
+		if (psecuritypriv->dot11PrivacyAlgrthm ==
+		    WLAN_CIPHER_SUITE_WEP40 ||
+		    psecuritypriv->dot11PrivacyAlgrthm ==
+		    WLAN_CIPHER_SUITE_WEP104) {
+			DBG_8723A("==>%s, channel(%d), processing(%x)\n",
+				  __func__, padapter->mlmeextpriv.cur_channel,
+				  pwrpriv->bips_processing);
+			set_channel_bwmode23a(padapter,
+					      padapter->mlmeextpriv.cur_channel,
+					      HAL_PRIME_CHNL_OFFSET_DONT_CARE,
+					      HT_CHANNEL_WIDTH_20);
+			for (keyid = 0; keyid < 4; keyid++) {
+				if (pmlmepriv->key_mask & BIT(keyid)) {
+					if (keyid ==
+					    psecuritypriv->dot11PrivacyKeyIndex)
 						result = rtw_set_key23a(padapter, psecuritypriv, keyid, 1);
 					else
 						result = rtw_set_key23a(padapter, psecuritypriv, keyid, 0);
@@ -90,7 +98,8 @@ int ips_leave23a(struct rtw_adapter * padapter)
 			}
 		}
 
-		DBG_8723A("==> ips_leave23a.....LED(0x%08x)...\n", rtw_read32(padapter, 0x4c));
+		DBG_8723A("==> ips_leave23a.....LED(0x%08x)...\n",
+			  rtl8723au_read32(padapter, 0x4c));
 		pwrpriv->bips_processing = false;
 
 		pwrpriv->bkeepfwalive = false;
@@ -108,42 +117,42 @@ static bool rtw_pwr_unassociated_idle(struct rtw_adapter *adapter)
 	struct rtw_adapter *buddy = adapter->pbuddy_adapter;
 	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
 	struct xmit_priv *pxmit_priv = &adapter->xmitpriv;
-	struct wifidirect_info *pwdinfo = &adapter->wdinfo;
 
 	bool ret = false;
 
 	if (time_after_eq(adapter->pwrctrlpriv.ips_deny_time, jiffies))
 		goto exit;
 
-	if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE|WIFI_SITE_MONITOR)
-		|| check_fwstate(pmlmepriv, WIFI_UNDER_LINKING|WIFI_UNDER_WPS)
-		|| check_fwstate(pmlmepriv, WIFI_AP_STATE)
-		|| check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_ADHOC_STATE)
-		|| !rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)
-	) {
+	if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE|WIFI_SITE_MONITOR) ||
+	    check_fwstate(pmlmepriv, WIFI_UNDER_LINKING|WIFI_UNDER_WPS) ||
+	    check_fwstate(pmlmepriv, WIFI_AP_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_ADHOC_STATE)){
 		goto exit;
 	}
 
 	/* consider buddy, if exist */
 	if (buddy) {
 		struct mlme_priv *b_pmlmepriv = &buddy->mlmepriv;
-		struct wifidirect_info *b_pwdinfo = &buddy->wdinfo;
-
-		if (check_fwstate(b_pmlmepriv, WIFI_ASOC_STATE|WIFI_SITE_MONITOR)
-			|| check_fwstate(b_pmlmepriv, WIFI_UNDER_LINKING|WIFI_UNDER_WPS)
-			|| check_fwstate(b_pmlmepriv, WIFI_AP_STATE)
-			|| check_fwstate(b_pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_ADHOC_STATE)
-			|| !rtw_p2p_chk_state(b_pwdinfo, P2P_STATE_NONE)
-		) {
+
+		if (check_fwstate(b_pmlmepriv,
+				  WIFI_ASOC_STATE|WIFI_SITE_MONITOR) ||
+		    check_fwstate(b_pmlmepriv,
+				  WIFI_UNDER_LINKING|WIFI_UNDER_WPS) ||
+		    check_fwstate(b_pmlmepriv, WIFI_AP_STATE) ||
+		    check_fwstate(b_pmlmepriv,
+				  WIFI_ADHOC_MASTER_STATE|WIFI_ADHOC_STATE)) {
 			goto exit;
 		}
 	}
 
 	if (pxmit_priv->free_xmitbuf_cnt != NR_XMITBUFF ||
 		pxmit_priv->free_xmit_extbuf_cnt != NR_XMIT_EXTBUFF) {
-		DBG_8723A_LEVEL(_drv_always_, "There are some pkts to transmit\n");
-		DBG_8723A_LEVEL(_drv_info_, "free_xmitbuf_cnt: %d, free_xmit_extbuf_cnt: %d\n",
-			pxmit_priv->free_xmitbuf_cnt, pxmit_priv->free_xmit_extbuf_cnt);
+		DBG_8723A_LEVEL(_drv_always_,
+				"There are some pkts to transmit\n");
+		DBG_8723A_LEVEL(_drv_info_, "free_xmitbuf_cnt: %d, "
+				"free_xmit_extbuf_cnt: %d\n",
+				pxmit_priv->free_xmitbuf_cnt,
+				pxmit_priv->free_xmit_extbuf_cnt);
 		goto exit;
 	}
 
@@ -166,7 +175,8 @@ void rtw_ps_processor23a(struct rtw_adapter*padapter)
 
 	if (padapter->pwrctrlpriv.bHWPwrPindetect) {
 		rfpwrstate = RfOnOffDetect23a(padapter);
-		DBG_8723A("@@@@- #2  %s ==> rfstate:%s\n", __func__, (rfpwrstate == rf_on)?"rf_on":"rf_off");
+		DBG_8723A("@@@@- #2  %s ==> rfstate:%s\n", __func__,
+			  (rfpwrstate == rf_on)?"rf_on":"rf_off");
 
 		if (rfpwrstate!= pwrpriv->rf_pwrstate) {
 			if (rfpwrstate == rf_off) {
@@ -178,7 +188,9 @@ void rtw_ps_processor23a(struct rtw_adapter*padapter)
 				pwrpriv->change_rfpwrstate = rf_on;
 				rtw_hw_resume23a(padapter);
 			}
-			DBG_8723A("current rf_pwrstate(%s)\n", (pwrpriv->rf_pwrstate == rf_off)?"rf_off":"rf_on");
+			DBG_8723A("current rf_pwrstate(%s)\n",
+				  (pwrpriv->rf_pwrstate == rf_off) ?
+				  "rf_off":"rf_on");
 		}
 		pwrpriv->pwr_state_check_cnts ++;
 	}
@@ -189,9 +201,10 @@ void rtw_ps_processor23a(struct rtw_adapter*padapter)
 	if (rtw_pwr_unassociated_idle(padapter) == false)
 		goto exit;
 
-	if ((pwrpriv->rf_pwrstate == rf_on) && ((pwrpriv->pwr_state_check_cnts%4) == 0))
-	{
-		DBG_8723A("==>%s .fw_state(%x)\n", __func__, get_fwstate(pmlmepriv));
+	if (pwrpriv->rf_pwrstate == rf_on &&
+	    (pwrpriv->pwr_state_check_cnts % 4) == 0) {
+		DBG_8723A("==>%s .fw_state(%x)\n", __func__,
+			  get_fwstate(pmlmepriv));
 		pwrpriv->change_rfpwrstate = rf_off;
 		ips_enter23a(padapter);
 	}
@@ -210,36 +223,35 @@ static void pwr_state_check_handler(unsigned long data)
 /*
  *
  * Parameters
- *	padapter
- *	pslv			power state level, only could be PS_STATE_S0 ~ PS_STATE_S4
+ *   padapter
+ *   pslv	power state level, only could be PS_STATE_S0 ~ PS_STATE_S4
  *
  */
 void rtw_set_rpwm23a(struct rtw_adapter *padapter, u8 pslv)
 {
-	u8	rpwm;
+	u8 rpwm;
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 
-
-
 	pslv = PS_STATE(pslv);
 
-	if (true == pwrpriv->btcoex_rfon)
-	{
+	if (pwrpriv->btcoex_rfon) {
 		if (pslv < PS_STATE_S4)
 			pslv = PS_STATE_S3;
 	}
 
 	if (pwrpriv->rpwm == pslv) {
 		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
-			("%s: Already set rpwm[0x%02X], new = 0x%02X!\n", __func__, pwrpriv->rpwm, pslv));
+			("%s: Already set rpwm[0x%02X], new = 0x%02X!\n",
+			 __func__, pwrpriv->rpwm, pslv));
 		return;
 	}
 
-	if ((padapter->bSurpriseRemoved == true) ||
-	    (padapter->hw_init_completed == false)) {
+	if (padapter->bSurpriseRemoved == true ||
+	    padapter->hw_init_completed == false) {
 		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
-				 ("%s: SurpriseRemoved(%d) hw_init_completed(%d)\n",
-				  __func__, padapter->bSurpriseRemoved, padapter->hw_init_completed));
+			 ("%s: SurpriseRemoved(%d) hw_init_completed(%d)\n",
+			  __func__, padapter->bSurpriseRemoved,
+			  padapter->hw_init_completed));
 
 		pwrpriv->cpwm = PS_STATE_S4;
 
@@ -248,52 +260,52 @@ void rtw_set_rpwm23a(struct rtw_adapter *padapter, u8 pslv)
 
 	if (padapter->bDriverStopped == true) {
 		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
-				 ("%s: change power state(0x%02X) when DriverStopped\n", __func__, pslv));
+			 ("%s: change power state(0x%02X) when DriverStopped\n",
+			  __func__, pslv));
 
 		if (pslv < PS_STATE_S2) {
 			RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
-					 ("%s: Reject to enter PS_STATE(0x%02X) lower than S2 when DriverStopped!!\n", __func__, pslv));
+				 ("%s: Reject to enter PS_STATE(0x%02X) lower "
+				  "than S2 when DriverStopped!!\n",
+				  __func__, pslv));
 			return;
 		}
 	}
 
 	rpwm = pslv | pwrpriv->tog;
 	RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
-			 ("rtw_set_rpwm23a: rpwm = 0x%02x cpwm = 0x%02x\n", rpwm, pwrpriv->cpwm));
+		 ("rtw_set_rpwm23a: rpwm = 0x%02x cpwm = 0x%02x\n",
+		  rpwm, pwrpriv->cpwm));
 
 	pwrpriv->rpwm = pslv;
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_SET_RPWM, (u8 *)(&rpwm));
+	rtl8723a_set_rpwm(padapter, rpwm);
 
 	pwrpriv->tog += 0x80;
 	pwrpriv->cpwm = pslv;
-
-
 }
 
-u8 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;
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
 	delta_time = jiffies - pwrpriv->DelayLPSLastTimeStamp;
 
 	if (delta_time < LPS_DELAY_TIME)
-	{
 		return false;
-	}
 
-	if ((check_fwstate(pmlmepriv, _FW_LINKED) == false) ||
-		(check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true) ||
-		(check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) ||
-		(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) ||
-		(check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true))
+	if (!check_fwstate(pmlmepriv, _FW_LINKED) ||
+	    check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) ||
+	    check_fwstate(pmlmepriv, WIFI_AP_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_STATE))
 		return false;
-	if (true == pwrpriv->bInSuspend)
+	if (pwrpriv->bInSuspend)
 		return false;
-	if ((padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) && (padapter->securitypriv.binstallGrpkey == false))
-	{
+	if (padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X &&
+	    !padapter->securitypriv.binstallGrpkey) {
 		DBG_8723A("Group handshake still in progress !!!\n");
 		return false;
 	}
@@ -303,75 +315,51 @@ u8 PS_RDY_CHECK(struct rtw_adapter * padapter)
 	return true;
 }
 
-void rtw_set_ps_mode23a(struct rtw_adapter *padapter, u8 ps_mode, u8 smart_ps, u8 bcn_ant_mode)
+void rtw_set_ps_mode23a(struct rtw_adapter *padapter, u8 ps_mode,
+			u8 smart_ps, u8 bcn_ant_mode)
 {
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif /* CONFIG_8723AU_P2P */
-
-
 
 	RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
 			 ("%s: PowerMode =%d Smart_PS =%d\n",
 			  __func__, ps_mode, smart_ps));
 
 	if (ps_mode > PM_Card_Disable) {
-		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_, ("ps_mode:%d error\n", ps_mode));
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
+			 ("ps_mode:%d error\n", ps_mode));
 		return;
 	}
 
-	if (pwrpriv->pwr_mode == ps_mode)
-	{
-		if (PS_MODE_ACTIVE == ps_mode) return;
+	if (pwrpriv->pwr_mode == ps_mode) {
+		if (PS_MODE_ACTIVE == ps_mode)
+			return;
 
-		if ((pwrpriv->smart_ps == smart_ps) &&
-			(pwrpriv->bcn_ant_mode == bcn_ant_mode))
-		{
+		if (pwrpriv->smart_ps == smart_ps &&
+		    pwrpriv->bcn_ant_mode == bcn_ant_mode)
 			return;
-		}
 	}
 
 	if (ps_mode == PS_MODE_ACTIVE) {
-#ifdef CONFIG_8723AU_P2P
-		if (pwdinfo->opp_ps == 0)
-#endif /* CONFIG_8723AU_P2P */
-		{
-			DBG_8723A("rtw_set_ps_mode23a: Leave 802.11 power save\n");
-
-			pwrpriv->pwr_mode = ps_mode;
-			rtw_set_rpwm23a(padapter, PS_STATE_S4);
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode));
-			pwrpriv->bFwCurrentInPSMode = false;
-		}
-	}
-	else
-	{
-		if (PS_RDY_CHECK(padapter)
-#ifdef CONFIG_8723AU_BT_COEXIST
-			|| (BT_1Ant(padapter) == true)
-#endif
-			)
-		{
+		DBG_8723A("rtw_set_ps_mode23a: Leave 802.11 power save\n");
+
+		pwrpriv->pwr_mode = ps_mode;
+		rtw_set_rpwm23a(padapter, PS_STATE_S4);
+		rtl8723a_set_FwPwrMode_cmd(padapter, ps_mode);
+		pwrpriv->bFwCurrentInPSMode = false;
+	} else {
+		if (PS_RDY_CHECK(padapter) ||
+		    rtl8723a_BT_using_antenna_1(padapter)) {
 			DBG_8723A("%s: Enter 802.11 power save\n", __func__);
 
 			pwrpriv->bFwCurrentInPSMode = true;
 			pwrpriv->pwr_mode = ps_mode;
 			pwrpriv->smart_ps = smart_ps;
 			pwrpriv->bcn_ant_mode = bcn_ant_mode;
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode));
-
-#ifdef CONFIG_8723AU_P2P
-			/*  Set CTWindow after LPS */
-			if (pwdinfo->opp_ps == 1)
-				p2p_ps_wk_cmd23a(padapter, P2P_PS_ENABLE, 0);
-#endif /* CONFIG_8723AU_P2P */
+			rtl8723a_set_FwPwrMode_cmd(padapter, ps_mode);
 
 			rtw_set_rpwm23a(padapter, PS_STATE_S2);
 		}
 	}
-
-
 }
 
 /*
@@ -391,12 +379,11 @@ s32 LPS_RF_ON_check23a(struct rtw_adapter *padapter, u32 delay_ms)
 
 	while (1)
 	{
-		rtw23a_hal_get_hwreg(padapter, HW_VAR_FWLPS_RF_ON, &bAwake);
-		if (true == bAwake)
+		bAwake = rtl8723a_get_fwlps_rf_on(padapter);
+		if (bAwake == true)
 			break;
 
-		if (true == padapter->bSurpriseRemoved)
-		{
+		if (padapter->bSurpriseRemoved == true) {
 			err = -2;
 			DBG_8723A("%s: device surprise removed!!\n", __func__);
 			break;
@@ -404,7 +391,8 @@ s32 LPS_RF_ON_check23a(struct rtw_adapter *padapter, u32 delay_ms)
 
 		if (time_after(jiffies, end_time)) {
 			err = -1;
-			DBG_8723A("%s: Wait for FW LPS leave more than %u ms!!!\n", __func__, delay_ms);
+			DBG_8723A("%s: Wait for FW LPS leave more than %u "
+				  "ms!\n", __func__, delay_ms);
 			break;
 		}
 		udelay(100);
@@ -417,7 +405,7 @@ s32 LPS_RF_ON_check23a(struct rtw_adapter *padapter, u32 delay_ms)
 /*		Enter the leisure power save mode. */
 void LPS_Enter23a(struct rtw_adapter *padapter)
 {
-	struct pwrctrl_priv	*pwrpriv = &padapter->pwrctrlpriv;
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 
 	if (!PS_RDY_CHECK(padapter))
 		return;
@@ -427,13 +415,15 @@ void LPS_Enter23a(struct rtw_adapter *padapter)
 		if (pwrpriv->LpsIdleCount >= 2) { /*   4 Sec */
 			if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) {
 				pwrpriv->bpower_saving = true;
-				DBG_8723A("%s smart_ps:%d\n", __func__, pwrpriv->smart_ps);
+				DBG_8723A("%s smart_ps:%d\n", __func__,
+					  pwrpriv->smart_ps);
 				/* For Tenda W311R IOT issue */
-				rtw_set_ps_mode23a(padapter, pwrpriv->power_mgnt, pwrpriv->smart_ps, 0);
+				rtw_set_ps_mode23a(padapter,
+						   pwrpriv->power_mgnt,
+						   pwrpriv->smart_ps, 0);
 			}
-		} else {
+		} else
 			pwrpriv->LpsIdleCount++;
-		}
 	}
 }
 
@@ -443,14 +433,15 @@ void LPS_Leave23a(struct rtw_adapter *padapter)
 {
 #define LPS_LEAVE_TIMEOUT_MS 100
 
-	struct pwrctrl_priv	*pwrpriv = &padapter->pwrctrlpriv;
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 
 	if (pwrpriv->bLeisurePs) {
 		if (pwrpriv->pwr_mode != PS_MODE_ACTIVE) {
 			rtw_set_ps_mode23a(padapter, PS_MODE_ACTIVE, 0, 0);
 
 			if (pwrpriv->pwr_mode == PS_MODE_ACTIVE)
-				LPS_RF_ON_check23a(padapter, LPS_LEAVE_TIMEOUT_MS);
+				LPS_RF_ON_check23a(padapter,
+						   LPS_LEAVE_TIMEOUT_MS);
 		}
 	}
 
@@ -462,21 +453,11 @@ void LPS_Leave23a(struct rtw_adapter *padapter)
 void LeaveAllPowerSaveMode23a(struct rtw_adapter *Adapter)
 {
 	struct mlme_priv *pmlmepriv = &Adapter->mlmepriv;
-	u8	enqueue = 0;
-
-
+	u8 enqueue = 0;
 
 	/* DBG_8723A("%s.....\n", __func__); */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{ /* connect */
-#ifdef CONFIG_8723AU_P2P
-		p2p_ps_wk_cmd23a(Adapter, P2P_PS_DISABLE, enqueue);
-#endif /* CONFIG_8723AU_P2P */
-
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
 		rtw_lps_ctrl_wk_cmd23a(Adapter, LPS_CTRL_LEAVE, enqueue);
-	}
-
-
 }
 
 void rtw_init_pwrctrl_priv23a(struct rtw_adapter *padapter)
@@ -494,13 +475,15 @@ void rtw_init_pwrctrl_priv23a(struct rtw_adapter *padapter)
 
 	pwrctrlpriv->pwr_state_check_interval = RTW_PWR_STATE_CHK_INTERVAL;
 	pwrctrlpriv->pwr_state_check_cnts = 0;
-	pwrctrlpriv->bInternalAutoSuspend = false;
 	pwrctrlpriv->bInSuspend = false;
 	pwrctrlpriv->bkeepfwalive = false;
 
 	pwrctrlpriv->LpsIdleCount = 0;
-	pwrctrlpriv->power_mgnt = padapter->registrypriv.power_mgnt;/*  PS_MODE_MIN; */
-	pwrctrlpriv->bLeisurePs = (PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt)?true:false;
+
+	/*  PS_MODE_MIN; */
+	pwrctrlpriv->power_mgnt = padapter->registrypriv.power_mgnt;
+	pwrctrlpriv->bLeisurePs =
+		(PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt)?true:false;
 
 	pwrctrlpriv->bFwCurrentInPSMode = false;
 
@@ -517,22 +500,12 @@ void rtw_init_pwrctrl_priv23a(struct rtw_adapter *padapter)
 
 	setup_timer(&pwrctrlpriv->pwr_state_check_timer,
 		    pwr_state_check_handler, (unsigned long)padapter);
-
-
 }
 
 void rtw_free_pwrctrl_priv(struct rtw_adapter *adapter)
 {
 }
 
-u8 rtw_interface_ps_func23a(struct rtw_adapter *padapter, enum hal_intf_ps_func efunc_id, u8* val)
-{
-	u8 bResult = true;
-	rtw_hal_intf_ps_func23a(padapter, efunc_id, val);
-
-	return bResult;
-}
-
 inline void rtw_set_ips_deny23a(struct rtw_adapter *padapter, u32 ms)
 {
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
@@ -570,18 +543,19 @@ int _rtw_pwr_wakeup23a(struct rtw_adapter *padapter, u32 ips_deffer_ms, const ch
 			DBG_8723A("%s wait ps_processing done\n", __func__);
 	}
 
-	if (rtw_hal_sreset_inprogress(padapter)) {
+	if (rtw_sreset_inprogress(padapter)) {
 		DBG_8723A("%s wait sreset_inprogress...\n", __func__);
-		while (rtw_hal_sreset_inprogress(padapter) &&
+		while (rtw_sreset_inprogress(padapter) &&
 		       jiffies_to_msecs(jiffies - start) <= 4000)
 			msleep(10);
-		if (rtw_hal_sreset_inprogress(padapter))
-			DBG_8723A("%s wait sreset_inprogress timeout\n", __func__);
+		if (rtw_sreset_inprogress(padapter))
+			DBG_8723A("%s wait sreset_inprogress timeout\n",
+				  __func__);
 		else
 			DBG_8723A("%s wait sreset_inprogress done\n", __func__);
 	}
 
-	if (pwrpriv->bInternalAutoSuspend == false && pwrpriv->bInSuspend) {
+	if (pwrpriv->bInSuspend) {
 		DBG_8723A("%s wait bInSuspend...\n", __func__);
 		while (pwrpriv->bInSuspend &&
 		       (jiffies_to_msecs(jiffies - start) <= 3000)) {
@@ -594,27 +568,20 @@ int _rtw_pwr_wakeup23a(struct rtw_adapter *padapter, u32 ips_deffer_ms, const ch
 	}
 
 	/* System suspend is not allowed to wakeup */
-	if ((pwrpriv->bInternalAutoSuspend == false) && (true == pwrpriv->bInSuspend)) {
-		ret = _FAIL;
-		goto exit;
-	}
-
-	/* block??? */
-	if ((pwrpriv->bInternalAutoSuspend == true)  && (padapter->net_closed == true)) {
+	if (pwrpriv->bInSuspend) {
 		ret = _FAIL;
 		goto exit;
 	}
 
 	/* I think this should be check in IPS, LPS, autosuspend functions... */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
+	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
 		ret = _SUCCESS;
 		goto exit;
 	}
 
 	if (rf_off == pwrpriv->rf_pwrstate) {
 		DBG_8723A("%s call ips_leave23a....\n", __func__);
-		if (_FAIL ==  ips_leave23a(padapter)) {
+		if (ips_leave23a(padapter)== _FAIL) {
 			DBG_8723A("======> ips_leave23a fail.............\n");
 			ret = _FAIL;
 			goto exit;
@@ -627,7 +594,7 @@ int _rtw_pwr_wakeup23a(struct rtw_adapter *padapter, u32 ips_deffer_ms, const ch
 		DBG_8723A("%s: bDriverStopped =%d, bup =%d, hw_init_completed "
 			  "=%u\n", caller, padapter->bDriverStopped,
 			  padapter->bup, padapter->hw_init_completed);
-		ret = false;
+		ret = _FAIL;
 		goto exit;
 	}
 
@@ -640,29 +607,22 @@ exit:
 
 int rtw_pm_set_lps23a(struct rtw_adapter *padapter, u8 mode)
 {
-	int	ret = 0;
+	int ret = 0;
 	struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv;
 
-	if (mode < PS_MODE_NUM)
-	{
-		if (pwrctrlpriv->power_mgnt != mode)
-		{
+	if (mode < PS_MODE_NUM) {
+		if (pwrctrlpriv->power_mgnt != mode) {
 			if (PS_MODE_ACTIVE == mode)
-			{
 				LeaveAllPowerSaveMode23a(padapter);
-			}
 			else
-			{
 				pwrctrlpriv->LpsIdleCount = 2;
-			}
 			pwrctrlpriv->power_mgnt = mode;
-			pwrctrlpriv->bLeisurePs = (PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt)?true:false;
+			pwrctrlpriv->bLeisurePs =
+				(PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt) ?
+				true:false;
 		}
-	}
-	else
-	{
+	} else
 		ret = -EINVAL;
-	}
 
 	return ret;
 }
@@ -673,17 +633,17 @@ int rtw_pm_set_ips23a(struct rtw_adapter *padapter, u8 mode)
 
 	if (mode == IPS_NORMAL || mode == IPS_LEVEL_2) {
 		rtw_ips_mode_req(pwrctrlpriv, mode);
-		DBG_8723A("%s %s\n", __func__, mode == IPS_NORMAL?"IPS_NORMAL":"IPS_LEVEL_2");
+		DBG_8723A("%s %s\n", __func__,
+			  mode == IPS_NORMAL?"IPS_NORMAL":"IPS_LEVEL_2");
 		return 0;
-	}
-	else if (mode == IPS_NONE) {
+	} else if (mode == IPS_NONE) {
 		rtw_ips_mode_req(pwrctrlpriv, mode);
 		DBG_8723A("%s %s\n", __func__, "IPS_NONE");
-		if ((padapter->bSurpriseRemoved == 0)&&_FAIL == rtw_pwr_wakeup(padapter))
+		if (padapter->bSurpriseRemoved == 0 &&
+		    rtw_pwr_wakeup(padapter) == _FAIL)
 			return -EFAULT;
-	}
-	else {
+	} else
 		return -EINVAL;
-	}
+
 	return 0;
 }
diff --git a/drivers/staging/rtl8723au/core/rtw_recv.c b/drivers/staging/rtl8723au/core/rtw_recv.c
index 0b2455e4f5b..690970efc22 100644
--- a/drivers/staging/rtl8723au/core/rtw_recv.c
+++ b/drivers/staging/rtl8723au/core/rtw_recv.c
@@ -19,10 +19,11 @@
 #include <mlme_osdep.h>
 #include <linux/ip.h>
 #include <linux/if_ether.h>
-#include <ethernet.h>
 #include <usb_ops.h>
 #include <linux/ieee80211.h>
 #include <wifi.h>
+#include <rtl8723a_recv.h>
+#include <rtl8723a_xmit.h>
 
 void rtw_signal_stat_timer_hdl23a(unsigned long data);
 
@@ -48,12 +49,6 @@ int _rtw_init_recv_priv23a(struct recv_priv *precvpriv,
 	int i;
 	int res = _SUCCESS;
 
-
-
-	/*  We don't need to memset padapter->XXX to zero, because
-	    adapter is allocated by rtw_zvmalloc(). */
-	/* memset((unsigned char *)precvpriv, 0, sizeof (struct  recv_priv)); */
-
 	spin_lock_init(&precvpriv->lock);
 
 	_rtw_init_queue23a(&precvpriv->free_recv_queue);
@@ -62,35 +57,23 @@ int _rtw_init_recv_priv23a(struct recv_priv *precvpriv,
 
 	precvpriv->adapter = padapter;
 
-	precvpriv->free_recvframe_cnt = NR_RECVFRAME;
-
-	precvpriv->pallocated_frame_buf =
-		rtw_zvmalloc(NR_RECVFRAME * sizeof(struct recv_frame));
-
-	if (precvpriv->pallocated_frame_buf == NULL) {
-		res = _FAIL;
-		goto exit;
-	}
-
-	precvframe = precvpriv->pallocated_frame_buf;
-
 	for (i = 0; i < NR_RECVFRAME ; i++) {
+		precvframe = kzalloc(sizeof(struct recv_frame), GFP_KERNEL);
+		if (!precvframe)
+			break;
 		INIT_LIST_HEAD(&precvframe->list);
 
 		list_add_tail(&precvframe->list,
 			      &precvpriv->free_recv_queue.queue);
 
-		res = rtw_os_recv_resource_alloc23a(padapter, precvframe);
-
 		precvframe->adapter = padapter;
 		precvframe++;
 	}
 
+	precvpriv->free_recvframe_cnt = i;
 	precvpriv->rx_pending_cnt = 1;
 
-	sema_init(&precvpriv->allrxreturnevt, 0);
-
-	res = rtw_hal_init23a_recv_priv(padapter);
+	res = rtl8723au_init_recv_priv(padapter);
 
 	setup_timer(&precvpriv->signal_stat_timer, rtw_signal_stat_timer_hdl23a,
 		    (unsigned long)padapter);
@@ -99,29 +82,24 @@ int _rtw_init_recv_priv23a(struct recv_priv *precvpriv,
 
 	rtw_set_signal_stat_timer(precvpriv);
 
-exit:
-
-
-
 	return res;
 }
 
 void _rtw_free_recv_priv23a (struct recv_priv *precvpriv)
 {
 	struct rtw_adapter *padapter = precvpriv->adapter;
-
-
+	struct recv_frame *precvframe;
+	struct list_head *plist, *ptmp;
 
 	rtw_free_uc_swdec_pending_queue23a(padapter);
 
-	if (precvpriv->pallocated_frame_buf) {
-		rtw_vmfree(precvpriv->pallocated_frame_buf,
-			   NR_RECVFRAME * sizeof(struct recv_frame));
+	list_for_each_safe(plist, ptmp, &precvpriv->free_recv_queue.queue) {
+		precvframe = container_of(plist, struct recv_frame, list);
+		list_del_init(&precvframe->list);
+		kfree(precvframe);
 	}
 
-	rtw_hal_free_recv_priv23a(padapter);
-
-
+	rtl8723au_free_recv_priv(padapter);
 }
 
 struct recv_frame *rtw_alloc_recvframe23a(struct rtw_queue *pfree_recv_queue)
@@ -133,7 +111,7 @@ struct recv_frame *rtw_alloc_recvframe23a(struct rtw_queue *pfree_recv_queue)
 
 	spin_lock_bh(&pfree_recv_queue->lock);
 
-	if (_rtw_queue_empty23a(pfree_recv_queue) == true)
+	if (list_empty(&pfree_recv_queue->queue))
 		pframe = NULL;
 	else {
 		phead = get_list_head(pfree_recv_queue);
@@ -156,18 +134,18 @@ struct recv_frame *rtw_alloc_recvframe23a(struct rtw_queue *pfree_recv_queue)
 	return pframe;
 }
 
-int rtw_free_recvframe23a(struct recv_frame *precvframe, struct rtw_queue *pfree_recv_queue)
+int rtw_free_recvframe23a(struct recv_frame *precvframe)
 {
 	struct rtw_adapter *padapter = precvframe->adapter;
 	struct recv_priv *precvpriv = &padapter->recvpriv;
-
-
+	struct rtw_queue *pfree_recv_queue;
 
 	if (precvframe->pkt) {
 		dev_kfree_skb_any(precvframe->pkt);/* free skb by driver */
 		precvframe->pkt = NULL;
 	}
 
+	pfree_recv_queue = &precvpriv->free_recv_queue;
 	spin_lock_bh(&pfree_recv_queue->lock);
 
 	list_del_init(&precvframe->list);
@@ -215,12 +193,11 @@ using spinlock to protect
 
 */
 
-void rtw_free_recvframe23a_queue(struct rtw_queue *pframequeue,  struct rtw_queue *pfree_recv_queue)
+static void rtw_free_recvframe23a_queue(struct rtw_queue *pframequeue)
 {
 	struct recv_frame *hdr;
 	struct list_head *plist, *phead, *ptmp;
 
-
 	spin_lock(&pframequeue->lock);
 
 	phead = get_list_head(pframequeue);
@@ -228,12 +205,10 @@ void rtw_free_recvframe23a_queue(struct rtw_queue *pframequeue,  struct rtw_queu
 
 	list_for_each_safe(plist, ptmp, phead) {
 		hdr = container_of(plist, struct recv_frame, list);
-		rtw_free_recvframe23a(hdr, pfree_recv_queue);
+		rtw_free_recvframe23a(hdr);
 	}
 
 	spin_unlock(&pframequeue->lock);
-
-
 }
 
 u32 rtw_free_uc_swdec_pending_queue23a(struct rtw_adapter *adapter)
@@ -241,8 +216,7 @@ u32 rtw_free_uc_swdec_pending_queue23a(struct rtw_adapter *adapter)
 	u32 cnt = 0;
 	struct recv_frame *pending_frame;
 	while ((pending_frame = rtw_alloc_recvframe23a(&adapter->recvpriv.uc_swdec_pending_queue))) {
-		rtw_free_recvframe23a(pending_frame,
-				   &adapter->recvpriv.free_recv_queue);
+		rtw_free_recvframe23a(pending_frame);
 		DBG_8723A("%s: dequeue uc_swdec_pending_queue\n", __func__);
 		cnt++;
 	}
@@ -282,7 +256,7 @@ struct recv_buf *rtw_dequeue_recvbuf23a (struct rtw_queue *queue)
 
 	spin_lock_irqsave(&queue->lock, irqL);
 
-	if (_rtw_queue_empty23a(queue) == true) {
+	if (list_empty(&queue->queue)) {
 		precvbuf = NULL;
 	} else {
 		phead = get_list_head(queue);
@@ -321,9 +295,9 @@ int recvframe_chkmic(struct rtw_adapter *adapter,
 
 	stainfo = rtw_get_stainfo23a(&adapter->stapriv, &prxattrib->ta[0]);
 
-	if (prxattrib->encrypt == _TKIP_) {
+	if (prxattrib->encrypt == WLAN_CIPHER_SUITE_TKIP) {
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-			 ("\n recvframe_chkmic:prxattrib->encrypt == _TKIP_\n"));
+			 ("\n recvframe_chkmic:prxattrib->encrypt == WLAN_CIPHER_SUITE_TKIP\n"));
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
 			 ("\n recvframe_chkmic:da = 0x%02x:0x%02x:0x%02x:0x%02x:"
 			  "0x%02x:0x%02x\n", prxattrib->ra[0],
@@ -338,7 +312,7 @@ int recvframe_chkmic(struct rtw_adapter *adapter,
 				RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
 					 ("\n recvframe_chkmic: bcmc key\n"));
 
-				if (psecuritypriv->binstallGrpkey == false) {
+				if (!psecuritypriv->binstallGrpkey) {
 					res = _FAIL;
 					RT_TRACE(_module_rtl871x_recv_c_,
 						 _drv_err_,
@@ -467,9 +441,9 @@ int recvframe_chkmic(struct rtw_adapter *adapter,
 				res = _FAIL;
 			} else {
 				/* mic checked ok */
-				if ((psecuritypriv->bcheck_grpkey == false) &&
-				    (is_multicast_ether_addr(prxattrib->ra))) {
-					psecuritypriv->bcheck_grpkey = true;
+				if (!psecuritypriv->bcheck_grpkey &&
+				    is_multicast_ether_addr(prxattrib->ra)) {
+					psecuritypriv->bcheck_grpkey = 1;
 					RT_TRACE(_module_rtl871x_recv_c_,
 						 _drv_err_,
 						 ("psecuritypriv->bcheck_grp"
@@ -501,8 +475,7 @@ struct recv_frame *decryptor(struct rtw_adapter *padapter,
 	struct rx_pkt_attrib *prxattrib = &precv_frame->attrib;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct recv_frame *return_packet = precv_frame;
-	u32 res = _SUCCESS;
-
+	int res = _SUCCESS;
 
 	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
 		 ("prxstat->decrypted =%x prxattrib->encrypt = 0x%03x\n",
@@ -517,13 +490,13 @@ struct recv_frame *decryptor(struct rtw_adapter *padapter,
 				  prxattrib->key_index);
 
 			switch (prxattrib->encrypt) {
-			case _WEP40_:
-			case _WEP104_:
+			case WLAN_CIPHER_SUITE_WEP40:
+			case WLAN_CIPHER_SUITE_WEP104:
 				prxattrib->key_index =
 					psecuritypriv->dot11PrivacyKeyIndex;
 				break;
-			case _TKIP_:
-			case _AES_:
+			case WLAN_CIPHER_SUITE_TKIP:
+			case WLAN_CIPHER_SUITE_CCMP:
 			default:
 				prxattrib->key_index =
 					psecuritypriv->dot118021XGrpKeyid;
@@ -533,16 +506,16 @@ struct recv_frame *decryptor(struct rtw_adapter *padapter,
 	}
 
 	if ((prxattrib->encrypt > 0) && ((prxattrib->bdecrypted == 0))) {
-		psecuritypriv->hw_decrypted = false;
+		psecuritypriv->hw_decrypted = 0;
 		switch (prxattrib->encrypt) {
-		case _WEP40_:
-		case _WEP104_:
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
 			rtw_wep_decrypt23a(padapter, precv_frame);
 			break;
-		case _TKIP_:
+		case WLAN_CIPHER_SUITE_TKIP:
 			res = rtw_tkip_decrypt23a(padapter, precv_frame);
 			break;
-		case _AES_:
+		case WLAN_CIPHER_SUITE_CCMP:
 			res = rtw_aes_decrypt23a(padapter, precv_frame);
 			break;
 		default:
@@ -550,13 +523,12 @@ struct recv_frame *decryptor(struct rtw_adapter *padapter,
 		}
 	} else if (prxattrib->bdecrypted == 1 && prxattrib->encrypt > 0 &&
 		   (psecuritypriv->busetkipkey == 1 ||
-		    prxattrib->encrypt != _TKIP_)) {
-			psecuritypriv->hw_decrypted = true;
+		    prxattrib->encrypt != WLAN_CIPHER_SUITE_TKIP)) {
+			psecuritypriv->hw_decrypted = 1;
 	}
 
 	if (res == _FAIL) {
-		rtw_free_recvframe23a(return_packet,
-				   &padapter->recvpriv.free_recv_queue);
+		rtw_free_recvframe23a(return_packet);
 		return_packet = NULL;
 	}
 
@@ -569,31 +541,35 @@ struct recv_frame *decryptor(struct rtw_adapter *padapter,
 static struct recv_frame *portctrl(struct rtw_adapter *adapter,
 				   struct recv_frame *precv_frame)
 {
-	u8 *psta_addr = NULL, *ptr;
+	u8 *psta_addr, *ptr;
 	uint auth_alg;
 	struct recv_frame *pfhdr;
 	struct sta_info *psta;
 	struct sta_priv *pstapriv ;
 	struct recv_frame *prtnframe;
-	u16 ether_type = 0;
-	u16 eapol_type = 0x888e;/* for Funia BD's WPA issue */
+	u16 ether_type;
+	u16 eapol_type = ETH_P_PAE;/* for Funia BD's WPA issue */
 	struct rx_pkt_attrib *pattrib;
 
 	pstapriv = &adapter->stapriv;
-	psta = rtw_get_stainfo23a(pstapriv, psta_addr);
 
 	auth_alg = adapter->securitypriv.dot11AuthAlgrthm;
 
-	ptr = precv_frame->pkt->data;
 	pfhdr = precv_frame;
 	pattrib = &pfhdr->attrib;
 	psta_addr = pattrib->ta;
+	psta = rtw_get_stainfo23a(pstapriv, psta_addr);
 
 	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
 		 ("########portctrl:adapter->securitypriv.dot11AuthAlgrthm ="
 		  "%d\n", adapter->securitypriv.dot11AuthAlgrthm));
 
-	if (auth_alg == 2) {
+	if (auth_alg == dot11AuthAlgrthm_8021X) {
+		/* get ether_type */
+		ptr = pfhdr->pkt->data + pfhdr->attrib.hdrlen;
+
+		ether_type = (ptr[6] << 8) | ptr[7];
+
 		if ((psta != NULL) && (psta->ieee8021x_blocked)) {
 			/* blocked */
 			/* only accept EAPOL frame */
@@ -601,19 +577,11 @@ static struct recv_frame *portctrl(struct rtw_adapter *adapter,
 				 ("########portctrl:psta->ieee8021x_blocked =="
 				  "1\n"));
 
-			prtnframe = precv_frame;
-
-			/* get ether_type */
-			ptr = ptr + pfhdr->attrib.hdrlen + LLC_HEADER_SIZE;
-			memcpy(&ether_type, ptr, 2);
-			ether_type = ntohs((unsigned short)ether_type);
-
 		        if (ether_type == eapol_type) {
 				prtnframe = precv_frame;
 			} else {
 				/* free this frame */
-				rtw_free_recvframe23a(precv_frame,
-						   &adapter->recvpriv.free_recv_queue);
+				rtw_free_recvframe23a(precv_frame);
 				prtnframe = NULL;
 			}
 		} else {
@@ -651,9 +619,7 @@ static struct recv_frame *portctrl(struct rtw_adapter *adapter,
 		prtnframe = precv_frame;
 	}
 
-
-
-		return prtnframe;
+	return prtnframe;
 }
 
 int recv_decache(struct recv_frame *precv_frame, u8 bretry,
@@ -833,8 +799,8 @@ static int sta2sta_data_frame(struct rtw_adapter *adapter,
 
 
 
-	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) ||
-		(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)) {
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
 
 		/*  filter packets that SA is myself or multicast or broadcast */
 		if (ether_addr_equal(myhwaddr, pattrib->src)) {
@@ -857,7 +823,7 @@ static int sta2sta_data_frame(struct rtw_adapter *adapter,
 		}
 
 		sta_addr = pattrib->src;
-	} else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
+	} else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
 		/*  For Station mode, sa and bssid should always be BSSID,
 		    and DA is my mac-address */
 		if (!ether_addr_equal(pattrib->bssid, pattrib->src)) {
@@ -870,7 +836,7 @@ static int sta2sta_data_frame(struct rtw_adapter *adapter,
 
 		sta_addr = pattrib->bssid;
 
-	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
 		if (bmcast) {
 			/*  For AP mode, if DA == MCAST, then BSSID should be also MCAST */
 			if (!is_multicast_ether_addr(pattrib->bssid)) {
@@ -887,7 +853,7 @@ static int sta2sta_data_frame(struct rtw_adapter *adapter,
 
 			sta_addr = pattrib->src;
 		}
-	} else if (check_fwstate(pmlmepriv, WIFI_MP_STATE) == true) {
+	} else if (check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
 		ether_addr_copy(pattrib->dst, hdr->addr1);
 		ether_addr_copy(pattrib->src, hdr->addr2);
 		ether_addr_copy(pattrib->bssid, hdr->addr3);
@@ -934,9 +900,9 @@ int ap2sta_data_frame(struct rtw_adapter *adapter,
 
 
 
-	if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) &&
-	    (check_fwstate(pmlmepriv, _FW_LINKED) == true ||
-	     check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true)) {
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) &&
+	    (check_fwstate(pmlmepriv, _FW_LINKED) ||
+	     check_fwstate(pmlmepriv, _FW_UNDER_LINKING))) {
 
 		/* filter packets that SA is myself or multicast or broadcast */
 		if (ether_addr_equal(myhwaddr, pattrib->src)) {
@@ -999,8 +965,8 @@ int ap2sta_data_frame(struct rtw_adapter *adapter,
 			goto exit;
 		}
 
-	} else if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == true) &&
-		   (check_fwstate(pmlmepriv, _FW_LINKED) == true)) {
+	} else if (check_fwstate(pmlmepriv, WIFI_MP_STATE) &&
+		   check_fwstate(pmlmepriv, _FW_LINKED)) {
 		ether_addr_copy(pattrib->dst, hdr->addr1);
 		ether_addr_copy(pattrib->src, hdr->addr2);
 		ether_addr_copy(pattrib->bssid, hdr->addr3);
@@ -1018,7 +984,7 @@ int ap2sta_data_frame(struct rtw_adapter *adapter,
 			ret = _FAIL;
 			goto exit;
 		}
-	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
 		/* Special case */
 		ret = RTW_RX_HANDLED;
 		goto exit;
@@ -1062,7 +1028,7 @@ int sta2ap_data_frame(struct rtw_adapter *adapter,
 
 
 
-	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
 		/* For AP mode, RA = BSSID, TX = STA(SRC_ADDR), A3 = DST_ADDR */
 		if (!ether_addr_equal(pattrib->bssid, mybssid)) {
 			ret = _FAIL;
@@ -1119,17 +1085,14 @@ exit:
 	return ret;
 }
 
-int validate_recv_ctrl_frame(struct rtw_adapter *padapter,
-			     struct recv_frame *precv_frame);
-int validate_recv_ctrl_frame(struct rtw_adapter *padapter,
-			     struct recv_frame *precv_frame)
+static int validate_recv_ctrl_frame(struct rtw_adapter *padapter,
+				    struct recv_frame *precv_frame)
 {
 #ifdef CONFIG_8723AU_AP_MODE
 	struct rx_pkt_attrib *pattrib = &precv_frame->attrib;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct sk_buff *skb = precv_frame->pkt;
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u8 *pframe = skb->data;
 
 	if (!ieee80211_is_ctl(hdr->frame_control))
 		return _FAIL;
@@ -1140,14 +1103,15 @@ int validate_recv_ctrl_frame(struct rtw_adapter *padapter,
 
 	/* only handle ps-poll */
 	if (ieee80211_is_pspoll(hdr->frame_control)) {
+		struct ieee80211_pspoll *psp = (struct ieee80211_pspoll *)hdr;
 		u16 aid;
 		u8 wmmps_ac = 0;
 		struct sta_info *psta = NULL;
 
-		aid = GetAid(pframe);
+		aid = le16_to_cpu(psp->aid) & 0x3fff;
 		psta = rtw_get_stainfo23a(pstapriv, hdr->addr2);
 
-		if ((!psta) || (psta->aid != aid))
+		if (!psta || psta->aid != aid)
 			return _FAIL;
 
 		/* for rx pkt statistics */
@@ -1213,7 +1177,8 @@ int validate_recv_ctrl_frame(struct rtw_adapter *padapter,
 
 	                        /* DBG_8723A("handling ps-poll, q_len =%d, tim =%x\n", psta->sleepq_len, pstapriv->tim_bitmap); */
 
-				rtw_hal_xmit23aframe_enqueue(padapter, pxmitframe);
+				rtl8723au_hal_xmitframe_enqueue(padapter,
+								pxmitframe);
 
 				if (psta->sleepq_len == 0) {
 					pstapriv->tim_bitmap &= ~CHKBIT(psta->aid);
@@ -1222,8 +1187,8 @@ int validate_recv_ctrl_frame(struct rtw_adapter *padapter,
 
 					/* upate BCN for TIM IE */
 					/* update_BCNTIM(padapter); */
-					update_beacon23a(padapter, _TIM_IE_,
-						      NULL, false);
+					update_beacon23a(padapter, WLAN_EID_TIM,
+							 NULL, false);
 				}
 
 				/* spin_unlock_bh(&psta->sleep_q.lock); */
@@ -1254,8 +1219,8 @@ int validate_recv_ctrl_frame(struct rtw_adapter *padapter,
 
 					/* upate BCN for TIM IE */
 					/* update_BCNTIM(padapter); */
-					update_beacon23a(padapter, _TIM_IE_,
-						      NULL, false);
+					update_beacon23a(padapter, WLAN_EID_TIM,
+							 NULL, false);
 				}
 			}
 		}
@@ -1267,10 +1232,8 @@ int validate_recv_ctrl_frame(struct rtw_adapter *padapter,
 
 struct recv_frame* recvframe_chk_defrag23a(struct rtw_adapter *padapter,
 					struct recv_frame *precv_frame);
-int validate_recv_mgnt_frame(struct rtw_adapter *padapter,
-			     struct recv_frame *precv_frame);
-int validate_recv_mgnt_frame(struct rtw_adapter *padapter,
-			     struct recv_frame *precv_frame)
+static int validate_recv_mgnt_frame(struct rtw_adapter *padapter,
+				    struct recv_frame *precv_frame)
 {
 	struct sta_info *psta;
 	struct sk_buff *skb;
@@ -1316,15 +1279,12 @@ int validate_recv_mgnt_frame(struct rtw_adapter *padapter,
 	return _SUCCESS;
 }
 
-int validate_recv_data_frame(struct rtw_adapter *adapter,
-			     struct recv_frame *precv_frame);
-int validate_recv_data_frame(struct rtw_adapter *adapter,
-			     struct recv_frame *precv_frame)
+static int validate_recv_data_frame(struct rtw_adapter *adapter,
+				    struct recv_frame *precv_frame)
 {
 	u8 bretry;
-	u8 *psa, *pda, *pbssid;
+	u8 *psa, *pda;
 	struct sta_info *psta = NULL;
-	u8 *ptr = precv_frame->pkt->data;
 	struct rx_pkt_attrib *pattrib = & precv_frame->attrib;
 	struct security_priv *psecuritypriv = &adapter->securitypriv;
 	int ret = _SUCCESS;
@@ -1336,48 +1296,44 @@ int validate_recv_data_frame(struct rtw_adapter *adapter,
 	bretry = ieee80211_has_retry(hdr->frame_control);
 	pda = ieee80211_get_DA(hdr);
 	psa = ieee80211_get_SA(hdr);
-	pbssid = get_hdr_bssid(ptr);
-
-	if (pbssid == NULL) {
-		ret = _FAIL;
-		goto exit;
-	}
 
 	ether_addr_copy(pattrib->dst, pda);
 	ether_addr_copy(pattrib->src, psa);
 
-	ether_addr_copy(pattrib->bssid, pbssid);
-
-	switch (pattrib->to_fr_ds)
-	{
-	case 0:
+	switch (hdr->frame_control &
+		cpu_to_le16(IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) {
+	case cpu_to_le16(0):
+		ether_addr_copy(pattrib->bssid, hdr->addr3);
 		ether_addr_copy(pattrib->ra, pda);
 		ether_addr_copy(pattrib->ta, psa);
 		ret = sta2sta_data_frame(adapter, precv_frame, &psta);
 		break;
 
-	case 1:
+	case cpu_to_le16(IEEE80211_FCTL_FROMDS):
+		ether_addr_copy(pattrib->bssid, hdr->addr2);
 		ether_addr_copy(pattrib->ra, pda);
-		ether_addr_copy(pattrib->ta, pbssid);
+		ether_addr_copy(pattrib->ta, hdr->addr2);
 		ret = ap2sta_data_frame(adapter, precv_frame, &psta);
 		break;
 
-	case 2:
-		ether_addr_copy(pattrib->ra, pbssid);
+	case cpu_to_le16(IEEE80211_FCTL_TODS):
+		ether_addr_copy(pattrib->bssid, hdr->addr1);
+		ether_addr_copy(pattrib->ra, hdr->addr1);
 		ether_addr_copy(pattrib->ta, psa);
 		ret = sta2ap_data_frame(adapter, precv_frame, &psta);
 		break;
 
-	case 3:
+	case cpu_to_le16(IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS):
+		/*
+		 * There is no BSSID in this case, but the driver has been
+		 * using addr1 so far, so keep it for now.
+		 */
+		ether_addr_copy(pattrib->bssid, hdr->addr1);
 		ether_addr_copy(pattrib->ra, hdr->addr1);
 		ether_addr_copy(pattrib->ta, hdr->addr2);
 		ret = _FAIL;
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" case 3\n"));
 		break;
-
-	default:
-		ret = _FAIL;
-		break;
 	}
 
 	if ((ret == _FAIL) || (ret == RTW_RX_HANDLED))
@@ -1451,22 +1407,18 @@ int validate_recv_data_frame(struct rtw_adapter *adapter,
 
 		switch (pattrib->encrypt)
 		{
-		case _WEP40_:
-		case _WEP104_:
-			pattrib->iv_len = 4;
-			pattrib->icv_len = 4;
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
+			pattrib->iv_len = IEEE80211_WEP_IV_LEN;
+			pattrib->icv_len = IEEE80211_WEP_ICV_LEN;
 			break;
-		case _TKIP_:
-			pattrib->iv_len = 8;
-			pattrib->icv_len = 4;
+		case WLAN_CIPHER_SUITE_TKIP:
+			pattrib->iv_len = IEEE80211_TKIP_IV_LEN;
+			pattrib->icv_len = IEEE80211_TKIP_ICV_LEN;
 			break;
-		case _AES_:
-			pattrib->iv_len = 8;
-			pattrib->icv_len = 8;
-			break;
-		case _SMS4_:
-			pattrib->iv_len = 18;
-			pattrib->icv_len = 16;
+		case WLAN_CIPHER_SUITE_CCMP:
+			pattrib->iv_len = IEEE80211_CCMP_HDR_LEN;
+			pattrib->icv_len = IEEE80211_CCMP_MIC_LEN;
 			break;
 		default:
 			pattrib->iv_len = 0;
@@ -1538,8 +1490,6 @@ static int validate_recv_frame(struct rtw_adapter *adapter,
 		goto exit;
 	}
 
-	pattrib->to_fr_ds = get_tofr_ds(hdr->frame_control);
-
 	seq_ctrl = le16_to_cpu(hdr->seq_ctrl);
 	pattrib->frag_num = seq_ctrl & IEEE80211_SCTL_FRAG;
 	pattrib->seq_num = seq_ctrl >> 4;
@@ -1550,7 +1500,7 @@ static int validate_recv_frame(struct rtw_adapter *adapter,
 	pattrib->privacy = ieee80211_has_protected(hdr->frame_control);
 	pattrib->order = ieee80211_has_order(hdr->frame_control);
 
-	rtw_hal_get_def_var23a(adapter, HAL_DEF_DBG_DUMP_RXPKT, &bDumpRxPkt);
+	GetHalDefVar8192CUsb(adapter, HAL_DEF_DBG_DUMP_RXPKT, &bDumpRxPkt);
 
 	if (unlikely(bDumpRxPkt == 1))
 		dump_rx_pkt(skb, type, bDumpRxPkt);
@@ -1638,7 +1588,7 @@ static int wlanhdr_to_ethhdr (struct recv_frame *precvframe)
 		  pattrib->hdrlen,  pattrib->iv_len));
 
 	pattrib->eth_type = eth_type;
-	if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == true)) {
+	if (check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
 		ptr += hdrlen;
 		*ptr = 0x87;
 		*(ptr + 1) = 0x12;
@@ -1694,8 +1644,8 @@ struct recv_frame *recvframe_defrag(struct rtw_adapter *adapter,
 	if (curfragnum != prframe->attrib.frag_num) {
 		/* the first fragment number must be 0 */
 		/* free the whole queue */
-		rtw_free_recvframe23a(prframe, pfree_recv_queue);
-		rtw_free_recvframe23a_queue(defrag_q, pfree_recv_queue);
+		rtw_free_recvframe23a(prframe);
+		rtw_free_recvframe23a_queue(defrag_q);
 
 		return NULL;
 	}
@@ -1715,8 +1665,8 @@ struct recv_frame *recvframe_defrag(struct rtw_adapter *adapter,
 			/* the fragment number must be increasing
 			   (after decache) */
 			/* release the defrag_q & prframe */
-			rtw_free_recvframe23a(prframe, pfree_recv_queue);
-			rtw_free_recvframe23a_queue(defrag_q, pfree_recv_queue);
+			rtw_free_recvframe23a(prframe);
+			rtw_free_recvframe23a_queue(defrag_q);
 			return NULL;
 		}
 
@@ -1744,7 +1694,7 @@ struct recv_frame *recvframe_defrag(struct rtw_adapter *adapter,
 	};
 
 	/* free the defrag_q queue and return the prframe */
-	rtw_free_recvframe23a_queue(defrag_q, pfree_recv_queue);
+	rtw_free_recvframe23a_queue(defrag_q);
 
 	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
 		 ("Performance defrag!!!!!\n"));
@@ -1803,9 +1753,9 @@ struct recv_frame* recvframe_chk_defrag23a(struct rtw_adapter *padapter,
 		if (pdefrag_q != NULL) {
 			if (fragnum == 0) {
 				/* the first fragment */
-				if (_rtw_queue_empty23a(pdefrag_q) == false) {
+				if (!list_empty(&pdefrag_q->queue)) {
 					/* free current defrag_q */
-					rtw_free_recvframe23a_queue(pdefrag_q, pfree_recv_queue);
+					rtw_free_recvframe23a_queue(pdefrag_q);
 				}
 			}
 
@@ -1826,7 +1776,7 @@ struct recv_frame* recvframe_chk_defrag23a(struct rtw_adapter *padapter,
 		} else {
 			/* can't find this ta's defrag_queue,
 			   so free this recv_frame */
-			rtw_free_recvframe23a(precv_frame, pfree_recv_queue);
+			rtw_free_recvframe23a(precv_frame);
 			prtnframe = NULL;
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
 				 ("Free because pdefrag_q == NULL: ismfrag = "
@@ -1852,7 +1802,7 @@ struct recv_frame* recvframe_chk_defrag23a(struct rtw_adapter *padapter,
 		} else {
 			/* can't find this ta's defrag_queue,
 			   so free this recv_frame */
-			rtw_free_recvframe23a(precv_frame, pfree_recv_queue);
+			rtw_free_recvframe23a(precv_frame);
 			prtnframe = NULL;
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
 				 ("Free because pdefrag_q == NULL: ismfrag = "
@@ -1867,7 +1817,7 @@ struct recv_frame* recvframe_chk_defrag23a(struct rtw_adapter *padapter,
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
 				 ("recvframe_chkmic(padapter,  prtnframe) =="
 				  "_FAIL\n"));
-			rtw_free_recvframe23a(prtnframe, pfree_recv_queue);
+			rtw_free_recvframe23a(prtnframe);
 			prtnframe = NULL;
 		}
 	}
@@ -1883,8 +1833,6 @@ int amsdu_to_msdu(struct rtw_adapter *padapter, struct recv_frame *prframe)
 	struct rx_pkt_attrib *pattrib;
 	struct sk_buff *skb, *sub_skb;
 	struct sk_buff_head skb_list;
-	struct recv_priv *precvpriv = &padapter->recvpriv;
-	struct rtw_queue *pfree_recv_queue = &precvpriv->free_recv_queue;
 
 	pattrib = &prframe->attrib;
 
@@ -1906,7 +1854,7 @@ int amsdu_to_msdu(struct rtw_adapter *padapter, struct recv_frame *prframe)
 	}
 
 	prframe->pkt = NULL;
-	rtw_free_recvframe23a(prframe, pfree_recv_queue);
+	rtw_free_recvframe23a(prframe);
 	return _SUCCESS;
 }
 
@@ -1942,8 +1890,8 @@ int check_indicate_seq(struct recv_reorder_ctrl *preorder_ctrl, u16 seq_num)
 	return true;
 }
 
-int enqueue_reorder_recvframe23a(struct recv_reorder_ctrl *preorder_ctrl,
-			      struct recv_frame *prframe)
+static int enqueue_reorder_recvframe23a(struct recv_reorder_ctrl *preorder_ctrl,
+				        struct recv_frame *prframe)
 {
 	struct rx_pkt_attrib *pattrib = &prframe->attrib;
 	struct rtw_queue *ppending_recvframe_queue;
@@ -2061,10 +2009,8 @@ int recv_indicatepkts_in_order(struct rtw_adapter *padapter,
 				}
 			} else {
 				if (amsdu_to_msdu(padapter, prframe) !=
-				    _SUCCESS) {
-					rtw_free_recvframe23a(prframe,
-							   &precvpriv->free_recv_queue);
-				}
+				    _SUCCESS)
+					rtw_free_recvframe23a(prframe);
 			}
 
 			/* Update local variables. */
@@ -2102,7 +2048,7 @@ int recv_indicatepkt_reorder(struct rtw_adapter *padapter,
 		/* s1. */
 		wlanhdr_to_ethhdr(prframe);
 
-		if ((pattrib->qos!= 1) || (pattrib->eth_type == 0x0806) ||
+		if ((pattrib->qos!= 1) || (pattrib->eth_type == ETH_P_ARP) ||
 		    (pattrib->ack_policy != 0)) {
 			if ((padapter->bDriverStopped == false) &&
 			    (padapter->bSurpriseRemoved == false)) {
@@ -2269,7 +2215,6 @@ int process_recv_indicatepkts(struct rtw_adapter *padapter,
 static int recv_func_prehandle(struct rtw_adapter *padapter,
 			       struct recv_frame *rframe)
 {
-	struct rtw_queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
 	int ret = _SUCCESS;
 
 	/* check the frame crtl field and decache */
@@ -2277,7 +2222,7 @@ static int recv_func_prehandle(struct rtw_adapter *padapter,
 	if (ret != _SUCCESS) {
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
 			 ("recv_func: validate_recv_frame fail! drop pkt\n"));
-		rtw_free_recvframe23a(rframe, pfree_recv_queue);
+		rtw_free_recvframe23a(rframe);
 		goto exit;
 	}
 
@@ -2291,7 +2236,6 @@ static int recv_func_posthandle(struct rtw_adapter *padapter,
 	int ret = _SUCCESS;
 	struct recv_frame *orig_prframe = prframe;
 	struct recv_priv *precvpriv = &padapter->recvpriv;
-	struct rtw_queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
 
 	/*  DATA FRAME */
 	rtw_led_control(padapter, LED_CTL_RX);
@@ -2337,7 +2281,7 @@ static int recv_func_posthandle(struct rtw_adapter *padapter,
 	if (ret != _SUCCESS) {
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
 			 ("recv_func: process_recv_indicatepkts fail!\n"));
-		rtw_free_recvframe23a(orig_prframe, pfree_recv_queue);/* free this recv_frame */
+		rtw_free_recvframe23a(orig_prframe);/* free this recv_frame */
 		goto _recv_data_drop;
 	}
 	return ret;
@@ -2434,8 +2378,7 @@ void rtw_signal_stat_timer_hdl23a(unsigned long data)
 		}
 
 		/* update value of signal_strength, rssi, signal_qual */
-		if (check_fwstate(&adapter->mlmepriv, _FW_UNDER_SURVEY) ==
-		    false) {
+		if (!check_fwstate(&adapter->mlmepriv, _FW_UNDER_SURVEY)) {
 			tmp_s = (avg_signal_strength + (_alpha - 1) *
 				 recvpriv->signal_strength);
 			if (tmp_s %_alpha)
diff --git a/drivers/staging/rtl8723au/core/rtw_security.c b/drivers/staging/rtl8723au/core/rtw_security.c
index fd43e71bf6d..d6c07adc7bf 100644
--- a/drivers/staging/rtl8723au/core/rtw_security.c
+++ b/drivers/staging/rtl8723au/core/rtw_security.c
@@ -170,16 +170,17 @@ void rtw_wep_encrypt23a(struct rtw_adapter *padapter,
 	pframe = pxmitframe->buf_addr + hw_hdr_offset;
 
 	/* start to encrypt each fragment */
-	if ((pattrib->encrypt != _WEP40_) && (pattrib->encrypt != _WEP104_))
+	if (pattrib->encrypt != WLAN_CIPHER_SUITE_WEP40 &&
+	    pattrib->encrypt != WLAN_CIPHER_SUITE_WEP104)
 		return;
 
 	index = psecuritypriv->dot11PrivacyKeyIndex;
-	keylength = psecuritypriv->dot11DefKeylen[index];
+	keylength = psecuritypriv->wep_key[index].keylen;
 
 	for (curfragnum = 0; curfragnum < pattrib->nr_frags ; curfragnum++) {
 		iv = pframe + pattrib->hdrlen;
 		memcpy(&wepkey[0], iv, 3);
-		memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[index].skey[0],
+		memcpy(&wepkey[3], &psecuritypriv->wep_key[index].key,
 		       keylength);
 		payload = pframe + pattrib->iv_len + pattrib->hdrlen;
 
@@ -225,17 +226,17 @@ void rtw_wep_decrypt23a(struct rtw_adapter *padapter,
 	pframe = skb->data;
 
 	/* start to decrypt recvframe */
-	if ((prxattrib->encrypt != _WEP40_) && (prxattrib->encrypt != _WEP104_))
+	if (prxattrib->encrypt != WLAN_CIPHER_SUITE_WEP40 &&
+	    prxattrib->encrypt != WLAN_CIPHER_SUITE_WEP104)
 		return;
 
 	iv = pframe + prxattrib->hdrlen;
 	/* keyindex = (iv[3]&0x3); */
 	keyindex = prxattrib->key_index;
-	keylength = psecuritypriv->dot11DefKeylen[keyindex];
+	keylength = psecuritypriv->wep_key[keyindex].keylen;
 	memcpy(&wepkey[0], iv, 3);
 	/* memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[psecuritypriv->dot11PrivacyKeyIndex].skey[0], keylength); */
-	memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[keyindex].skey[0],
-		   keylength);
+	memcpy(&wepkey[3], &psecuritypriv->wep_key[keyindex].key, keylength);
 	length = skb->len - prxattrib->hdrlen - prxattrib->iv_len;
 
 	payload = pframe + prxattrib->iv_len + prxattrib->hdrlen;
@@ -248,7 +249,7 @@ void rtw_wep_decrypt23a(struct rtw_adapter *padapter,
 	*((u32 *)crc) = le32_to_cpu(getcrc32(payload, length - 4));
 
 	if (crc[3] != payload[length - 1] || crc[2] != payload[length - 2] ||
-		crc[1] != payload[length - 3] || crc[0] != payload[length - 4]) {
+	    crc[1] != payload[length - 3] || crc[0] != payload[length - 4]) {
 		RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
 			 ("rtw_wep_decrypt23a:icv error crc[3](%x)!= payload"
 			  "[length-1](%x) || crc[2](%x)!= payload[length-2](%x)"
@@ -610,9 +611,9 @@ static void phase2(u8 *rc4key, const u8 *tk, const u16 *p1k, u16 iv16)
 }
 
 /* The hlen isn't include the IV */
-u32 rtw_tkip_encrypt23a(struct rtw_adapter *padapter,
-		     struct xmit_frame *pxmitframe)
-{																	/*  exclude ICV */
+int rtw_tkip_encrypt23a(struct rtw_adapter *padapter,
+			struct xmit_frame *pxmitframe)
+{
 	u16	pnl;
 	u32	pnh;
 	u8	rc4key[16];
@@ -622,14 +623,13 @@ u32 rtw_tkip_encrypt23a(struct rtw_adapter *padapter,
 	struct arc4context mycontext;
 	int			curfragnum, length;
 	u32	prwskeylen;
-
 	u8	*pframe, *payload,*iv,*prwskey;
 	union pn48 dot11txpn;
 	struct	sta_info		*stainfo;
 	struct	pkt_attrib	 *pattrib = &pxmitframe->attrib;
 	struct	security_priv	*psecuritypriv = &padapter->securitypriv;
 	struct	xmit_priv		*pxmitpriv = &padapter->xmitpriv;
-	u32	res = _SUCCESS;
+	int res = _SUCCESS;
 
 	if (!pxmitframe->buf_addr)
 		return _FAIL;
@@ -638,14 +638,10 @@ u32 rtw_tkip_encrypt23a(struct rtw_adapter *padapter,
 
 	pframe = pxmitframe->buf_addr + hw_hdr_offset;
 	/* 4 start to encrypt each fragment */
-	if (pattrib->encrypt == _TKIP_) {
-
+	if (pattrib->encrypt == WLAN_CIPHER_SUITE_TKIP) {
 		if (pattrib->psta)
-		{
 			stainfo = pattrib->psta;
-		}
-		else
-		{
+		else {
 			DBG_8723A("%s, call rtw_get_stainfo()\n", __func__);
 			stainfo = rtw_get_stainfo23a(&padapter->stapriv,
 						     &pattrib->ra[0]);
@@ -716,10 +712,9 @@ u32 rtw_tkip_encrypt23a(struct rtw_adapter *padapter,
 }
 
 /* The hlen isn't include the IV */
-u32 rtw_tkip_decrypt23a(struct rtw_adapter *padapter,
-		     struct recv_frame *precvframe)
+int rtw_tkip_decrypt23a(struct rtw_adapter *padapter,
+			struct recv_frame *precvframe)
 {
-	/*  exclude ICV */
 	u16 pnl;
 	u32 pnh;
 	u8   rc4key[16];
@@ -734,19 +729,19 @@ u32 rtw_tkip_decrypt23a(struct rtw_adapter *padapter,
 	struct	rx_pkt_attrib *prxattrib = &precvframe->attrib;
 	struct	security_priv *psecuritypriv = &padapter->securitypriv;
 	struct sk_buff * skb = precvframe->pkt;
-	u32	res = _SUCCESS;
+	int res = _SUCCESS;
 
 	pframe = skb->data;
 
 	/* 4 start to decrypt recvframe */
-	if (prxattrib->encrypt == _TKIP_) {
+	if (prxattrib->encrypt == WLAN_CIPHER_SUITE_TKIP) {
 
 		stainfo = rtw_get_stainfo23a(&padapter->stapriv,
 					     &prxattrib->ta[0]);
 		if (stainfo!= NULL) {
 
 			if (is_multicast_ether_addr(prxattrib->ra)) {
-				if (psecuritypriv->binstallGrpkey == false) {
+				if (psecuritypriv->binstallGrpkey == 0) {
 					res = _FAIL;
 					DBG_8723A("%s:rx bc/mc packets, but didn't install group key!!!!!!!!!!\n", __func__);
 					goto exit;
@@ -1303,7 +1298,8 @@ static int aes_cipher(u8 *key, uint hdrlen, u8 *pframe, uint plen)
 	return _SUCCESS;
 }
 
-u32 rtw_aes_encrypt23a(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe)
+int rtw_aes_encrypt23a(struct rtw_adapter *padapter,
+		       struct xmit_frame *pxmitframe)
 {	/*  exclude ICV */
 	/* Intermediate Buffers */
 	int curfragnum, length;
@@ -1314,7 +1310,7 @@ u32 rtw_aes_encrypt23a(struct rtw_adapter *padapter, struct xmit_frame *pxmitfra
 	struct pkt_attrib *pattrib = &pxmitframe->attrib;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	u32 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	if (!pxmitframe->buf_addr)
 		return _FAIL;
@@ -1324,7 +1320,7 @@ u32 rtw_aes_encrypt23a(struct rtw_adapter *padapter, struct xmit_frame *pxmitfra
 	pframe = pxmitframe->buf_addr + hw_hdr_offset;
 
 	/* 4 start to encrypt each fragment */
-	if (pattrib->encrypt != _AES_)
+	if (pattrib->encrypt != WLAN_CIPHER_SUITE_CCMP)
 		return _FAIL;
 
 	if (pattrib->psta) {
@@ -1586,7 +1582,8 @@ static int aes_decipher(u8 *key, uint	hdrlen,
 	return res;
 }
 
-u32 rtw_aes_decrypt23a(struct rtw_adapter *padapter, struct recv_frame *precvframe)
+int rtw_aes_decrypt23a(struct rtw_adapter *padapter,
+		       struct recv_frame *precvframe)
 {	/*  exclude ICV */
 	struct sta_info *stainfo;
 	struct rx_pkt_attrib *prxattrib = &precvframe->attrib;
@@ -1594,11 +1591,11 @@ u32 rtw_aes_decrypt23a(struct rtw_adapter *padapter, struct recv_frame *precvfra
 	struct sk_buff *skb = precvframe->pkt;
 	int length;
 	u8 *pframe, *prwskey;	/*  *payload,*iv */
-	u32 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	pframe = skb->data;
 	/* 4 start to encrypt each fragment */
-	if (prxattrib->encrypt != _AES_)
+	if (prxattrib->encrypt != WLAN_CIPHER_SUITE_CCMP)
 		return _FAIL;
 
 	stainfo = rtw_get_stainfo23a(&padapter->stapriv, &prxattrib->ta[0]);
@@ -1645,7 +1642,7 @@ void rtw_use_tkipkey_handler23a(void *FunctionContext)
 	struct rtw_adapter *padapter = (struct rtw_adapter *)FunctionContext;
 
 	RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("^^^rtw_use_tkipkey_handler23a ^^^\n"));
-	padapter->securitypriv.busetkipkey = true;
+	padapter->securitypriv.busetkipkey = 1;
 	RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
 		 ("^^^rtw_use_tkipkey_handler23a padapter->securitypriv.busetkipkey =%d^^^\n",
 		 padapter->securitypriv.busetkipkey));
diff --git a/drivers/staging/rtl8723au/core/rtw_sreset.c b/drivers/staging/rtl8723au/core/rtw_sreset.c
index 4f745920339..18a42a27b48 100644
--- a/drivers/staging/rtl8723au/core/rtw_sreset.c
+++ b/drivers/staging/rtl8723au/core/rtw_sreset.c
@@ -13,11 +13,12 @@
  *
  ******************************************************************************/
 
-#include<rtw_sreset.h>
+#include <rtw_sreset.h>
+#include <usb_ops_linux.h>
 
-void sreset_init_value23a(struct rtw_adapter *padapter)
+void rtw_sreset_init(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a	*pHalData = GET_HAL_DATA(padapter);
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 	struct sreset_priv *psrtpriv = &pHalData->srestpriv;
 
 	mutex_init(&psrtpriv->silentreset_mutex);
@@ -26,9 +27,10 @@ void sreset_init_value23a(struct rtw_adapter *padapter)
 	psrtpriv->last_tx_time = 0;
 	psrtpriv->last_tx_complete_time = 0;
 }
-void sreset_reset_value23a(struct rtw_adapter *padapter)
+
+void rtw_sreset_reset_value(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a	*pHalData = GET_HAL_DATA(padapter);
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 	struct sreset_priv *psrtpriv = &pHalData->srestpriv;
 
 	psrtpriv->silent_reset_inprogress = false;
@@ -37,16 +39,16 @@ void sreset_reset_value23a(struct rtw_adapter *padapter)
 	psrtpriv->last_tx_complete_time = 0;
 }
 
-u8 sreset_get_wifi_status23a(struct rtw_adapter *padapter)
+u8 rtw_sreset_get_wifi_status(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a	*pHalData = GET_HAL_DATA(padapter);
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 	struct sreset_priv *psrtpriv = &pHalData->srestpriv;
 	u8 status = WIFI_STATUS_SUCCESS;
 	u32 val32 = 0;
 
 	if (psrtpriv->silent_reset_inprogress)
 		return status;
-	val32 = rtw_read32(padapter, REG_TXDMA_STATUS);
+	val32 = rtl8723au_read32(padapter, REG_TXDMA_STATUS);
 	if (val32 == 0xeaeaeaea) {
 		psrtpriv->Wifi_Error_Status = WIFI_IF_NOT_EXIST;
 	} else if (val32 != 0) {
@@ -80,9 +82,10 @@ void sreset_set_trigger_point(struct rtw_adapter *padapter, s32 tgp)
 	pHalData->srestpriv.dbg_trigger_point = tgp;
 }
 
-bool sreset_inprogress(struct rtw_adapter *padapter)
+bool rtw_sreset_inprogress(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a	*pHalData = GET_HAL_DATA(padapter);
+	struct rtw_adapter *primary_adapter = GET_PRIMARY_ADAPTER(padapter);
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(primary_adapter);
 
 	return pHalData->srestpriv.silent_reset_inprogress;
 }
@@ -99,10 +102,13 @@ static void sreset_restore_security_station(struct rtw_adapter *padapter)
 		val8 = 0xcc;
 	else
 		val8 = 0xcf;
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
 
-	if ((padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_) ||
-	    (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)) {
+	rtl8723a_set_sec_cfg(padapter, val8);
+
+	if (padapter->securitypriv.dot11PrivacyAlgrthm ==
+	    WLAN_CIPHER_SUITE_TKIP ||
+	    padapter->securitypriv.dot11PrivacyAlgrthm ==
+	    WLAN_CIPHER_SUITE_CCMP) {
 		psta = rtw_get_stainfo23a(pstapriv, get_bssid(mlmepriv));
 		if (psta == NULL) {
 			/* DEBUG_ERR(("Set wpa_set_encryption: Obtain Sta_info fail\n")); */
@@ -122,7 +128,7 @@ static void sreset_restore_network_station(struct rtw_adapter *padapter)
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	u8 threshold;
 
-	rtw_setopmode_cmd23a(padapter, Ndis802_11Infrastructure);
+	rtw_setopmode_cmd23a(padapter, NL80211_IFTYPE_STATION);
 
 	/*  TH = 1 => means that invalidate usb rx aggregation */
 	/*  TH = 0 => means that validate usb rx aggregation, use init value. */
@@ -131,29 +137,22 @@ static void sreset_restore_network_station(struct rtw_adapter *padapter)
 			threshold = 1;
 		else
 			threshold = 0;
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold));
-	} else {
+	} else
 		threshold = 1;
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold));
-	}
 
-	set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+	rtl8723a_set_rxdma_agg_pg_th(padapter, threshold);
 
-	/* disable dynamic functions, such as high power, DIG */
-	/* Switch_DM_Func23a(padapter, DYNAMIC_FUNC_DISABLE, false); */
+	set_channel_bwmode23a(padapter, pmlmeext->cur_channel,
+			      pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_BSSID, pmlmeinfo->network.MacAddress);
-
-	{
-		u8	join_type = 0;
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
-	}
+	hw_var_set_bssid(padapter, pmlmeinfo->network.MacAddress);
+	hw_var_set_mlme_join(padapter, 0);
 
 	Set_MSR23a(padapter, (pmlmeinfo->state & 0x3));
 
 	mlmeext_joinbss_event_callback23a(padapter, 1);
 	/* restore Sequence No. */
-	rtw_write8(padapter, 0x4dc, padapter->xmitpriv.nqos_ssn);
+	rtl8723au_write8(padapter, 0x4dc, padapter->xmitpriv.nqos_ssn);
 
 	sreset_restore_security_station(padapter);
 }
@@ -163,17 +162,24 @@ static void sreset_restore_network_status(struct rtw_adapter *padapter)
 	struct mlme_priv *mlmepriv = &padapter->mlmepriv;
 
 	if (check_fwstate(mlmepriv, WIFI_STATION_STATE)) {
-		DBG_8723A(FUNC_ADPT_FMT" fwstate:0x%08x - WIFI_STATION_STATE\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv));
+		DBG_8723A("%s(%s): fwstate:0x%08x - WIFI_STATION_STATE\n",
+			  __func__, padapter->pnetdev->name,
+			  get_fwstate(mlmepriv));
 		sreset_restore_network_station(padapter);
 #ifdef CONFIG_8723AU_AP_MODE
 	} else if (check_fwstate(mlmepriv, WIFI_AP_STATE)) {
-		DBG_8723A(FUNC_ADPT_FMT" fwstate:0x%08x - WIFI_AP_STATE\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv));
+		DBG_8723A("%s(%s): fwstate:0x%08x - WIFI_AP_STATE\n",
+			  __func__, padapter->pnetdev->name,
+			  get_fwstate(mlmepriv));
 		rtw_ap_restore_network(padapter);
 #endif
 	} else if (check_fwstate(mlmepriv, WIFI_ADHOC_STATE)) {
-		DBG_8723A(FUNC_ADPT_FMT" fwstate:0x%08x - WIFI_ADHOC_STATE\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv));
+		DBG_8723A("%s(%s): fwstate:0x%08x - WIFI_ADHOC_STATE\n",
+			  __func__, padapter->pnetdev->name,
+			  get_fwstate(mlmepriv));
 	} else {
-		DBG_8723A(FUNC_ADPT_FMT" fwstate:0x%08x - ???\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv));
+		DBG_8723A("%s(%s): fwstate:0x%08x - ???\n", __func__,
+			  padapter->pnetdev->name, get_fwstate(mlmepriv));
 	}
 }
 
@@ -185,7 +191,7 @@ static void sreset_stop_adapter(struct rtw_adapter *padapter)
 	if (padapter == NULL)
 		return;
 
-	DBG_8723A(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter));
+	DBG_8723A("%s(%s)\n", __func__, padapter->pnetdev->name);
 
 	if (!rtw_netif_queue_stopped(padapter->pnetdev))
 		netif_tx_stop_all_queues(padapter->pnetdev);
@@ -210,7 +216,7 @@ static void sreset_start_adapter(struct rtw_adapter *padapter)
 	if (padapter == NULL)
 		return;
 
-	DBG_8723A(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter));
+	DBG_8723A("%s(%s)\n", __func__, padapter->pnetdev->name);
 
 	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
 		sreset_restore_network_status(padapter);
@@ -226,9 +232,10 @@ static void sreset_start_adapter(struct rtw_adapter *padapter)
 		netif_tx_wake_all_queues(padapter->pnetdev);
 }
 
-void sreset_reset(struct rtw_adapter *padapter)
+void rtw_sreset_reset(struct rtw_adapter *active_adapter)
 {
-	struct hal_data_8723a	*pHalData = GET_HAL_DATA(padapter);
+	struct rtw_adapter *padapter = GET_PRIMARY_ADAPTER(active_adapter);
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 	struct sreset_priv *psrtpriv = &pHalData->srestpriv;
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 	unsigned long start = jiffies;
diff --git a/drivers/staging/rtl8723au/core/rtw_sta_mgt.c b/drivers/staging/rtl8723au/core/rtw_sta_mgt.c
index 451b58f4728..14a82bea826 100644
--- a/drivers/staging/rtl8723au/core/rtw_sta_mgt.c
+++ b/drivers/staging/rtl8723au/core/rtw_sta_mgt.c
@@ -20,8 +20,9 @@
 #include <xmit_osdep.h>
 #include <mlme_osdep.h>
 #include <sta_info.h>
+#include <rtl8723a_hal.h>
 
-void _rtw_init_stainfo(struct sta_info *psta)
+static void _rtw_init_stainfo(struct sta_info *psta)
 {
 	memset((u8 *)psta, 0, sizeof (struct sta_info));
 	spin_lock_init(&psta->lock);
@@ -48,31 +49,15 @@ void _rtw_init_stainfo(struct sta_info *psta)
 #endif	/*  CONFIG_8723AU_AP_MODE */
 }
 
-u32 _rtw_init_sta_priv23a(struct sta_priv *pstapriv)
+int _rtw_init_sta_priv23a(struct sta_priv *pstapriv)
 {
-	struct sta_info *psta;
-	s32 i;
-
-	pstapriv->pallocated_stainfo_buf = rtw_zvmalloc(sizeof(struct sta_info) * NUM_STA+ 4);
-
-	if (!pstapriv->pallocated_stainfo_buf)
-		return _FAIL;
+	int i;
 
-	pstapriv->pstainfo_buf = pstapriv->pallocated_stainfo_buf + 4 -
-		((unsigned long)(pstapriv->pallocated_stainfo_buf) & 3);
-	_rtw_init_queue23a(&pstapriv->free_sta_queue);
 	spin_lock_init(&pstapriv->sta_hash_lock);
 	pstapriv->asoc_sta_count = 0;
-	_rtw_init_queue23a(&pstapriv->sleep_q);
-	_rtw_init_queue23a(&pstapriv->wakeup_q);
-	psta = (struct sta_info *)(pstapriv->pstainfo_buf);
-
-	for (i = 0; i < NUM_STA; i++) {
-		_rtw_init_stainfo(psta);
+	for (i = 0; i < NUM_STA; i++)
 		INIT_LIST_HEAD(&pstapriv->sta_hash[i]);
-		list_add_tail(&psta->list, get_list_head(&pstapriv->free_sta_queue));
-		psta++;
-	}
+
 #ifdef CONFIG_8723AU_AP_MODE
 	pstapriv->sta_dz_bitmap = 0;
 	pstapriv->tim_bitmap = 0;
@@ -92,49 +77,12 @@ u32 _rtw_init_sta_priv23a(struct sta_priv *pstapriv)
 	return _SUCCESS;
 }
 
-inline int rtw_stainfo_offset23a(struct sta_priv *stapriv, struct sta_info *sta)
-{
-	int offset = (((u8 *)sta) - stapriv->pstainfo_buf)/sizeof(struct sta_info);
-
-	if (!stainfo_offset_valid(offset))
-		DBG_8723A("%s invalid offset(%d), out of range!!!", __func__, offset);
-	return offset;
-}
-
-inline struct sta_info *rtw_get_stainfo23a_by_offset23a(struct sta_priv *stapriv, int offset)
-{
-	if (!stainfo_offset_valid(offset))
-		DBG_8723A("%s invalid offset(%d), out of range!!!", __func__, offset);
-	return (struct sta_info *)(stapriv->pstainfo_buf + offset * sizeof(struct sta_info));
-}
-
-/*  this function is used to free the memory of lock || sema for all stainfos */
-void rtw_mfree_all_stainfo(struct sta_priv *pstapriv)
-{
-	struct list_head *plist, *phead;
-	struct sta_info *psta;
-
-	spin_lock_bh(&pstapriv->sta_hash_lock);
-
-	phead = get_list_head(&pstapriv->free_sta_queue);
-
-	/* we really achieve a lot in this loop .... */
-	list_for_each(plist, phead)
-		psta = container_of(plist, struct sta_info, list);
-	spin_unlock_bh(&pstapriv->sta_hash_lock);
-}
-
-void rtw_mfree_sta_priv_lock(struct	sta_priv *pstapriv)
-{
-	rtw_mfree_all_stainfo(pstapriv); /* be done before free sta_hash_lock */
-}
-
-u32	_rtw_free_sta_priv23a(struct	sta_priv *pstapriv)
+int _rtw_free_sta_priv23a(struct sta_priv *pstapriv)
 {
 	struct list_head *phead, *plist, *ptmp;
 	struct sta_info *psta;
 	struct recv_reorder_ctrl *preorder_ctrl;
-	int	index;
+	int index;
 
 	if (pstapriv) {
 		/*	delete all reordering_ctrl_timer		*/
@@ -154,39 +102,25 @@ u32	_rtw_free_sta_priv23a(struct	sta_priv *pstapriv)
 		}
 		spin_unlock_bh(&pstapriv->sta_hash_lock);
 		/*===============================*/
-
-		rtw_mfree_sta_priv_lock(pstapriv);
-
-		if (pstapriv->pallocated_stainfo_buf)
-			rtw_vmfree(pstapriv->pallocated_stainfo_buf, sizeof(struct sta_info)*NUM_STA+4);
 	}
 	return _SUCCESS;
 }
 
-struct	sta_info *rtw_alloc_stainfo23a(struct sta_priv *pstapriv, u8 *hwaddr)
+struct sta_info *
+rtw_alloc_stainfo23a(struct sta_priv *pstapriv, u8 *hwaddr, gfp_t gfp)
 {
 	struct list_head	*phash_list;
 	struct sta_info	*psta;
-	struct rtw_queue *pfree_sta_queue;
 	struct recv_reorder_ctrl *preorder_ctrl;
-	uint tmp_aid;
 	s32	index;
 	int i = 0;
 	u16  wRxSeqInitialValue = 0xffff;
 
-	pfree_sta_queue = &pstapriv->free_sta_queue;
-
-	spin_lock_bh(&pstapriv->sta_hash_lock);
-
-	if (_rtw_queue_empty23a(pfree_sta_queue)) {
-		spin_unlock_bh(&pstapriv->sta_hash_lock);
+	psta = kmalloc(sizeof(struct sta_info), gfp);
+	if (!psta)
 		return NULL;
-	}
-	psta = container_of((&pfree_sta_queue->queue)->next, struct sta_info, list);
 
-	list_del_init(&psta->list);
-
-	tmp_aid = psta->aid;
+	spin_lock_bh(&pstapriv->sta_hash_lock);
 
 	_rtw_init_stainfo(psta);
 
@@ -252,9 +186,8 @@ exit:
 }
 
 /*  using pstapriv->sta_hash_lock to protect */
-u32	rtw_free_stainfo23a(struct rtw_adapter *padapter, struct sta_info *psta)
+int rtw_free_stainfo23a(struct rtw_adapter *padapter, struct sta_info *psta)
 {
-	struct rtw_queue *pfree_sta_queue;
 	struct recv_reorder_ctrl *preorder_ctrl;
 	struct	sta_xmit_priv	*pstaxmitpriv;
 	struct	xmit_priv	*pxmitpriv = &padapter->xmitpriv;
@@ -269,8 +202,6 @@ u32	rtw_free_stainfo23a(struct rtw_adapter *padapter, struct sta_info *psta)
 	psta->state &= ~_FW_LINKED;
 	spin_unlock_bh(&psta->lock);
 
-	pfree_sta_queue = &pstapriv->free_sta_queue;
-
 	pstaxmitpriv = &psta->sta_xmitpriv;
 
 	spin_lock_bh(&pxmitpriv->lock);
@@ -323,7 +254,6 @@ u32	rtw_free_stainfo23a(struct rtw_adapter *padapter, struct sta_info *psta)
 		struct list_head	*phead, *plist;
 		struct recv_frame *prframe;
 		struct rtw_queue *ppending_recvframe_queue;
-		struct rtw_queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
 
 		preorder_ctrl = &psta->recvreorder_ctrl[i];
 
@@ -339,12 +269,12 @@ u32	rtw_free_stainfo23a(struct rtw_adapter *padapter, struct sta_info *psta)
 			prframe = container_of(plist, struct recv_frame, list);
 			plist = plist->next;
 			list_del_init(&prframe->list);
-			rtw_free_recvframe23a(prframe, pfree_recv_queue);
+			rtw_free_recvframe23a(prframe);
 		}
 		spin_unlock_bh(&ppending_recvframe_queue->lock);
 	}
 	if (!(psta->state & WIFI_AP_STATE))
-		rtw_hal_set_odm_var23a(padapter, HAL_ODM_STA_INFO, psta, false);
+		rtl8723a_SetHalODMVar(padapter, HAL_ODM_STA_INFO, psta, false);
 #ifdef CONFIG_8723AU_AP_MODE
 	spin_lock_bh(&pstapriv->auth_list_lock);
 	if (!list_empty(&psta->auth_list)) {
@@ -374,7 +304,8 @@ u32	rtw_free_stainfo23a(struct rtw_adapter *padapter, struct sta_info *psta)
 		psta->aid = 0;
 	}
 #endif	/*  CONFIG_8723AU_AP_MODE */
-	list_add_tail(&psta->list, get_list_head(pfree_sta_queue));
+
+	kfree(psta);
 exit:
 	return _SUCCESS;
 }
@@ -384,9 +315,11 @@ void rtw_free_all_stainfo23a(struct rtw_adapter *padapter)
 {
 	struct list_head *plist, *phead, *ptmp;
 	struct sta_info *psta;
-	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct sta_info* pbcmc_stainfo = rtw_get_bcmc_stainfo23a(padapter);
-	s32	index;	if (pstapriv->asoc_sta_count == 1)
+	s32 index;
+
+	if (pstapriv->asoc_sta_count == 1)
 		return;
 
 	spin_lock_bh(&pstapriv->sta_hash_lock);
@@ -405,12 +338,12 @@ void rtw_free_all_stainfo23a(struct rtw_adapter *padapter)
 }
 
 /* any station allocated can be searched by hash list */
-struct sta_info *rtw_get_stainfo23a(struct sta_priv *pstapriv, u8 *hwaddr)
+struct sta_info *rtw_get_stainfo23a(struct sta_priv *pstapriv, const u8 *hwaddr)
 {
 	struct list_head *plist, *phead;
 	struct sta_info *psta = NULL;
 	u32	index;
-	u8 *addr;
+	const u8 *addr;
 	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 
 	if (hwaddr == NULL)
@@ -440,15 +373,15 @@ struct sta_info *rtw_get_stainfo23a(struct sta_priv *pstapriv, u8 *hwaddr)
 	return psta;
 }
 
-u32 rtw_init_bcmc_stainfo23a(struct rtw_adapter* padapter)
+int rtw_init_bcmc_stainfo23a(struct rtw_adapter* padapter)
 {
 	struct	sta_priv *pstapriv = &padapter->stapriv;
 	struct sta_info		*psta;
 	struct tx_servq	*ptxservq;
-	u32 res = _SUCCESS;
+	int res = _SUCCESS;
 	unsigned char bcast_addr[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 
-	psta = rtw_alloc_stainfo23a(pstapriv, bcast_addr);
+	psta = rtw_alloc_stainfo23a(pstapriv, bcast_addr, GFP_KERNEL);
 	if (psta == NULL) {
 		res = _FAIL;
 		RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_err_,
@@ -472,13 +405,13 @@ struct sta_info *rtw_get_bcmc_stainfo23a(struct rtw_adapter *padapter)
 	return psta;
 }
 
-u8 rtw_access_ctrl23a(struct rtw_adapter *padapter, u8 *mac_addr)
+bool rtw_access_ctrl23a(struct rtw_adapter *padapter, u8 *mac_addr)
 {
-	u8 res = true;
+	bool res = true;
 #ifdef CONFIG_8723AU_AP_MODE
 	struct list_head *plist, *phead;
 	struct rtw_wlan_acl_node *paclnode;
-	u8 match = false;
+	bool match = false;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
 	struct rtw_queue *pacl_node_q = &pacl_list->acl_node_q;
diff --git a/drivers/staging/rtl8723au/core/rtw_wlan_util.c b/drivers/staging/rtl8723au/core/rtw_wlan_util.c
index 99d81e612e7..579a4a8c827 100644
--- a/drivers/staging/rtl8723au/core/rtw_wlan_util.c
+++ b/drivers/staging/rtl8723au/core/rtw_wlan_util.c
@@ -32,29 +32,36 @@ static unsigned char REALTEK_OUI[] = {0x00, 0xe0, 0x4c};
 static unsigned char AIRGOCAP_OUI[] = {0x00, 0x0a, 0xf5};
 static unsigned char EPIGRAM_OUI[] = {0x00, 0x90, 0x4c};
 
-unsigned char REALTEK_96B_IE23A[] = {0x00, 0xe0, 0x4c, 0x02, 0x01, 0x20};
+static unsigned char WPA_TKIP_CIPHER[4] = {0x00, 0x50, 0xf2, 0x02};
+static unsigned char RSN_TKIP_CIPHER[4] = {0x00, 0x0f, 0xac, 0x02};
 
-#define R2T_PHY_DELAY	(0)
+#define R2T_PHY_DELAY		0
 
-/* define WAIT_FOR_BCN_TO_MIN	(3000) */
-#define WAIT_FOR_BCN_TO_MIN	(6000)
-#define WAIT_FOR_BCN_TO_MAX	(20000)
+/* define WAIT_FOR_BCN_TO_MIN	3000 */
+#define WAIT_FOR_BCN_TO_MIN	6000
+#define WAIT_FOR_BCN_TO_MAX	20000
 
 static u8 rtw_basic_rate_cck[4] = {
-	IEEE80211_CCK_RATE_1MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_2MB|IEEE80211_BASIC_RATE_MASK,
-	IEEE80211_CCK_RATE_5MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_11MB|IEEE80211_BASIC_RATE_MASK
+	IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK
 };
 
 static u8 rtw_basic_rate_ofdm[3] = {
-	IEEE80211_OFDM_RATE_6MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_OFDM_RATE_12MB|IEEE80211_BASIC_RATE_MASK,
-	IEEE80211_OFDM_RATE_24MB|IEEE80211_BASIC_RATE_MASK
+	IEEE80211_OFDM_RATE_6MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_OFDM_RATE_12MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_OFDM_RATE_24MB | IEEE80211_BASIC_RATE_MASK
 };
 
 static u8 rtw_basic_rate_mix[7] = {
-	IEEE80211_CCK_RATE_1MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_2MB|IEEE80211_BASIC_RATE_MASK,
-	IEEE80211_CCK_RATE_5MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_11MB|IEEE80211_BASIC_RATE_MASK,
-	IEEE80211_OFDM_RATE_6MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_OFDM_RATE_12MB|IEEE80211_BASIC_RATE_MASK,
-	IEEE80211_OFDM_RATE_24MB|IEEE80211_BASIC_RATE_MASK
+	IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_OFDM_RATE_6MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_OFDM_RATE_12MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_OFDM_RATE_24MB | IEEE80211_BASIC_RATE_MASK
 };
 
 int cckrates_included23a(unsigned char *rate, int ratelen)
@@ -62,8 +69,8 @@ int cckrates_included23a(unsigned char *rate, int ratelen)
 	int	i;
 
 	for (i = 0; i < ratelen; i++) {
-		if  ((((rate[i]) & 0x7f) == 2)	|| (((rate[i]) & 0x7f) == 4) ||
-		     (((rate[i]) & 0x7f) == 11)  || (((rate[i]) & 0x7f) == 22))
+		if  (((rate[i]) & 0x7f) == 2 || ((rate[i]) & 0x7f) == 4 ||
+		     ((rate[i]) & 0x7f) == 11  || ((rate[i]) & 0x7f) == 22)
 			return true;
 	}
 
@@ -75,8 +82,8 @@ int cckratesonly_included23a(unsigned char *rate, int ratelen)
 	int	i;
 
 	for (i = 0; i < ratelen; i++) {
-		if  ((((rate[i]) & 0x7f) != 2) && (((rate[i]) & 0x7f) != 4) &&
-			   (((rate[i]) & 0x7f) != 11)  && (((rate[i]) & 0x7f) != 22))
+		if  (((rate[i]) & 0x7f) != 2 && ((rate[i]) & 0x7f) != 4 &&
+		     ((rate[i]) & 0x7f) != 11 && ((rate[i]) & 0x7f) != 22)
 		return false;
 	}
 
@@ -116,10 +123,11 @@ unsigned char networktype_to_raid23a(unsigned char network_type)
 	return raid;
 }
 
-u8 judge_network_type23a(struct rtw_adapter *padapter, unsigned char *rate, int ratelen)
+u8 judge_network_type23a(struct rtw_adapter *padapter,
+			 unsigned char *rate, int ratelen)
 {
 	u8 network_type = 0;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
 	if (pmlmeext->cur_channel > 14) {
@@ -140,7 +148,7 @@ u8 judge_network_type23a(struct rtw_adapter *padapter, unsigned char *rate, int
 	return	network_type;
 }
 
-unsigned char ratetbl_val_2wifirate(unsigned char rate)
+static unsigned char ratetbl_val_2wifirate(unsigned char rate)
 {
 	unsigned char val = 0;
 
@@ -185,7 +193,7 @@ unsigned char ratetbl_val_2wifirate(unsigned char rate)
 	return val;
 }
 
-int is_basicrate(struct rtw_adapter *padapter, unsigned char rate)
+static int is_basicrate(struct rtw_adapter *padapter, unsigned char rate)
 {
 	int i;
 	unsigned char val;
@@ -194,7 +202,7 @@ int is_basicrate(struct rtw_adapter *padapter, unsigned char rate)
 	for (i = 0; i < NumRates; i++) {
 		val = pmlmeext->basicrate[i];
 
-		if ((val != 0xff) && (val != 0xfe)) {
+		if (val != 0xff && val != 0xfe) {
 			if (rate == ratetbl_val_2wifirate(val))
 				return true;
 		}
@@ -203,11 +211,12 @@ int is_basicrate(struct rtw_adapter *padapter, unsigned char rate)
 	return false;
 }
 
-unsigned int ratetbl2rateset(struct rtw_adapter *padapter, unsigned char *rateset)
+static unsigned int ratetbl2rateset(struct rtw_adapter *padapter,
+				    unsigned char *rateset)
 {
 	int i;
 	unsigned char rate;
-	unsigned int	len = 0;
+	unsigned int len = 0;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 
 	for (i = 0; i < NumRates; i++) {
@@ -232,7 +241,8 @@ unsigned int ratetbl2rateset(struct rtw_adapter *padapter, unsigned char *ratese
 	return len;
 }
 
-void get_rate_set23a(struct rtw_adapter *padapter, unsigned char *pbssrate, int *bssrate_len)
+void get_rate_set23a(struct rtw_adapter *padapter,
+		     unsigned char *pbssrate, int *bssrate_len)
 {
 	unsigned char supportedrates[NumRates];
 
@@ -243,8 +253,8 @@ void get_rate_set23a(struct rtw_adapter *padapter, unsigned char *pbssrate, int
 
 void UpdateBrateTbl23a(struct rtw_adapter *Adapter, u8 *mBratesOS)
 {
-	u8	i;
-	u8	rate;
+	u8 i;
+	u8 rate;
 
 	/*  1M, 2M, 5.5M, 11M, 6M, 12M, 24M are mandatory. */
 	for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
@@ -267,8 +277,8 @@ void UpdateBrateTbl23a(struct rtw_adapter *Adapter, u8 *mBratesOS)
 
 void Update23aTblForSoftAP(u8 *bssrateset, u32 bssratelen)
 {
-	u8	i;
-	u8	rate;
+	u8 i;
+	u8 rate;
 
 	for (i = 0; i < bssratelen; i++) {
 		rate = bssrateset[i] & 0x7f;
@@ -283,35 +293,9 @@ void Update23aTblForSoftAP(u8 *bssrateset, u32 bssratelen)
 	}
 }
 
-void Save_DM_Func_Flag23a(struct rtw_adapter *padapter)
-{
-	u8	bSaveFlag = true;
-
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_DM_FUNC_OP, (u8 *)(&bSaveFlag));
-}
-
-void Restore_DM_Func_Flag23a(struct rtw_adapter *padapter)
-{
-	u8	bSaveFlag = false;
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_DM_FUNC_OP, (u8 *)(&bSaveFlag));
-}
-
-void Switch_DM_Func23a(struct rtw_adapter *padapter, unsigned long mode, u8 enable)
-{
-	if (enable == true)
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_DM_FUNC_SET, (u8 *)(&mode));
-	else
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_DM_FUNC_CLR, (u8 *)(&mode));
-}
-
-static void Set_NETYPE0_MSR(struct rtw_adapter *padapter, u8 type)
-{
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_MEDIA_STATUS, (u8 *)(&type));
-}
-
 void Set_MSR23a(struct rtw_adapter *padapter, u8 type)
 {
-		Set_NETYPE0_MSR(padapter, type);
+	rtl8723a_set_media_status(padapter, type);
 }
 
 inline u8 rtw_get_oper_ch23a(struct rtw_adapter *adapter)
@@ -351,12 +335,13 @@ void SelectChannel23a(struct rtw_adapter *padapter, unsigned char channel)
 	/* saved channel info */
 	rtw_set_oper_ch23a(padapter, channel);
 
-	rtw_hal_set_chan23a(padapter, channel);
+	PHY_SwChnl8723A(padapter, channel);
 
 	mutex_unlock(&adapter_to_dvobj(padapter)->setch_mutex);
 }
 
-void SetBWMode23a(struct rtw_adapter *padapter, unsigned short bwmode, unsigned char channel_offset)
+static void set_bwmode(struct rtw_adapter *padapter, unsigned short bwmode,
+		       unsigned char channel_offset)
 {
 	mutex_lock(&adapter_to_dvobj(padapter)->setbw_mutex);
 
@@ -364,8 +349,8 @@ void SetBWMode23a(struct rtw_adapter *padapter, unsigned short bwmode, unsigned
 	rtw_set_oper_bw23a(padapter, bwmode);
 	rtw_set_oper_ch23aoffset23a(padapter, channel_offset);
 
-	rtw_hal_set_bwmode23a(padapter, (enum ht_channel_width)bwmode,
-			   channel_offset);
+	PHY_SetBWMode23a8723A(padapter, (enum ht_channel_width)bwmode,
+			      channel_offset);
 
 	mutex_unlock(&adapter_to_dvobj(padapter)->setbw_mutex);
 }
@@ -376,10 +361,11 @@ void set_channel_bwmode23a(struct rtw_adapter *padapter, unsigned char channel,
 	u8 center_ch;
 
 	if (padapter->bNotifyChannelChange)
-		DBG_8723A("[%s] ch = %d, offset = %d, bwmode = %d\n", __func__, channel, channel_offset, bwmode);
+		DBG_8723A("[%s] ch = %d, offset = %d, bwmode = %d\n",
+			  __func__, channel, channel_offset, bwmode);
 
-	if ((bwmode == HT_CHANNEL_WIDTH_20) ||
-	    (channel_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE)) {
+	if (bwmode == HT_CHANNEL_WIDTH_20 ||
+	    channel_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE) {
 		/* SelectChannel23a(padapter, channel); */
 		center_ch = channel;
 	} else {
@@ -401,20 +387,11 @@ void set_channel_bwmode23a(struct rtw_adapter *padapter, unsigned char channel,
 	rtw_set_oper_bw23a(padapter, bwmode);
 	rtw_set_oper_ch23aoffset23a(padapter, channel_offset);
 
-	rtw_hal_set_chan23a(padapter, center_ch); /*  set center channel */
+	PHY_SwChnl8723A(padapter, center_ch); /*  set center channel */
 
 	mutex_unlock(&adapter_to_dvobj(padapter)->setch_mutex);
 
-	SetBWMode23a(padapter, bwmode, channel_offset);
-}
-
-int get_bsstype23a(unsigned short capability)
-{
-	if (capability & BIT(0))
-		return WIFI_FW_AP_STATE;
-	else if (capability & BIT(1))
-		return WIFI_FW_ADHOC_STATE;
-	return 0;
+	set_bwmode(padapter, bwmode, channel_offset);
 }
 
 inline u8 *get_my_bssid23a(struct wlan_bssid_ex *pnetwork)
@@ -425,49 +402,50 @@ inline u8 *get_my_bssid23a(struct wlan_bssid_ex *pnetwork)
 u16 get_beacon_interval23a(struct wlan_bssid_ex *bss)
 {
 	unsigned short val;
-	memcpy((unsigned char *)&val, rtw_get_beacon_interval23a_from_ie(bss->IEs), 2);
+	memcpy(&val, rtw_get_beacon_interval23a_from_ie(bss->IEs), 2);
 
 	return le16_to_cpu(val);
 }
 
-int is_client_associated_to_ap23a(struct rtw_adapter *padapter)
+bool is_client_associated_to_ap23a(struct rtw_adapter *padapter)
 {
-	struct mlme_ext_priv	*pmlmeext;
-	struct mlme_ext_info	*pmlmeinfo;
+	struct mlme_ext_priv *pmlmeext;
+	struct mlme_ext_info *pmlmeinfo;
 
 	if (!padapter)
-		return _FAIL;
+		return false;
 
 	pmlmeext = &padapter->mlmeextpriv;
 	pmlmeinfo = &pmlmeext->mlmext_info;
 
-	if ((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE))
+	if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS &&
+	    (pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE)
 		return true;
 	else
-		return _FAIL;
+		return false;
 }
 
-int is_client_associated_to_ibss23a(struct rtw_adapter *padapter)
+bool is_client_associated_to_ibss23a(struct rtw_adapter *padapter)
 {
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
-	if ((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) &&
-	    ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE))
+	if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS &&
+	    (pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE)
 		return true;
 	else
-		return _FAIL;
+		return false;
 }
 
-int is_IBSS_empty23a(struct rtw_adapter *padapter)
+bool is_IBSS_empty23a(struct rtw_adapter *padapter)
 {
 	unsigned int i;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
 	for (i = IBSS_START_MAC_ID; i < NUM_STA; i++) {
 		if (pmlmeinfo->FW_sta_info[i].status == 1)
-			return _FAIL;
+			return false;
 	}
 
 	return true;
@@ -483,66 +461,26 @@ unsigned int decide_wait_for_beacon_timeout23a(unsigned int bcn_interval)
 		return bcn_interval << 2;
 }
 
-void CAM_empty_entry23a(struct rtw_adapter *Adapter, u8 ucIndex)
-{
-	rtw_hal_set_hwreg23a(Adapter, HW_VAR_CAM_EMPTY_ENTRY, (u8 *)(&ucIndex));
-}
-
 void invalidate_cam_all23a(struct rtw_adapter *padapter)
 {
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_CAM_INVALID_ALL, NULL);
-}
-
-void write_cam23a(struct rtw_adapter *padapter, u8 entry, u16 ctrl, u8 *mac, u8 *key)
-{
-	unsigned int	i, val, addr;
-	int j;
-	u32	cam_val[2];
-
-	addr = entry << 3;
-
-	for (j = 5; j >= 0; j--) {
-		switch (j) {
-		case 0:
-			val = (ctrl | (mac[0] << 16) | (mac[1] << 24));
-			break;
-		case 1:
-			val = (mac[2] | (mac[3] << 8) | (mac[4] << 16) | (mac[5] << 24));
-			break;
-		default:
-			i = (j - 2) << 2;
-			val = (key[i] | (key[i+1] << 8) | (key[i+2] << 16) | (key[i+3] << 24));
-			break;
-		}
-
-		cam_val[0] = val;
-		cam_val[1] = addr + (unsigned int)j;
-
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_CAM_WRITE, (u8 *)cam_val);
-
-		/* rtw_write32(padapter, WCAMI, val); */
-
-		/* cmd = CAM_POLLINIG | CAM_WRITE | (addr + j); */
-		/* rtw_write32(padapter, RWCAM, cmd); */
-
-		/* DBG_8723A("%s => cam write: %x, %x\n", __func__, cmd, val); */
-
-	}
+	rtl8723a_cam_invalid_all(padapter);
 }
 
 void clear_cam_entry23a(struct rtw_adapter *padapter, u8 entry)
 {
 	unsigned char null_sta[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
 
-	unsigned char null_key[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+	unsigned char null_key[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+				    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+				    0x00, 0x00, 0x00, 0x00};
 
-	write_cam23a(padapter, entry, 0, null_sta, null_key);
+	rtl8723a_cam_write(padapter, entry, 0, null_sta, null_key);
 }
 
 int allocate_fw_sta_entry23a(struct rtw_adapter *padapter)
 {
 	unsigned int mac_id;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
 	for (mac_id = IBSS_START_MAC_ID; mac_id < NUM_STA; mac_id++) {
@@ -558,69 +496,43 @@ int allocate_fw_sta_entry23a(struct rtw_adapter *padapter)
 
 void flush_all_cam_entry23a(struct rtw_adapter *padapter)
 {
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_CAM_INVALID_ALL, NULL);
+	rtl8723a_cam_invalid_all(padapter);
 
-	memset((u8 *)(pmlmeinfo->FW_sta_info), 0, sizeof(pmlmeinfo->FW_sta_info));
+	memset(pmlmeinfo->FW_sta_info, 0, sizeof(pmlmeinfo->FW_sta_info));
 }
 
-#if defined(CONFIG_8723AU_P2P) && defined(CONFIG_8723AU_P2P)
-int WFD_info_handler(struct rtw_adapter *padapter, struct ndis_802_11_var_ies *	pIE)
-{
-	struct wifidirect_info	*pwdinfo;
-	u8	wfd_ie[MAX_WFD_IE_LEN] = {0x00};
-	u32	wfd_ielen = 0;
-
-	pwdinfo = &padapter->wdinfo;
-	if (rtw_get_wfd_ie((u8 *) pIE, pIE->Length, wfd_ie, &wfd_ielen)) {
-		u8	attr_content[ 10 ] = { 0x00 };
-		u32	attr_contentlen = 0;
-
-		DBG_8723A("[%s] Found WFD IE\n", __func__);
-		rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen);
-		if (attr_contentlen) {
-			pwdinfo->wfd_info->peer_rtsp_ctrlport = get_unaligned_be16(attr_content + 2);
-			DBG_8723A("[%s] Peer PORT NUM = %d\n", __func__, pwdinfo->wfd_info->peer_rtsp_ctrlport);
-			return true;
-		}
-	} else {
-		DBG_8723A("[%s] NO WFD IE\n", __func__);
-	}
-	return _FAIL;
-}
-#endif
-
-int WMM_param_handler23a(struct rtw_adapter *padapter, struct ndis_802_11_var_ies *	pIE)
+int WMM_param_handler23a(struct rtw_adapter *padapter, u8 *p)
 {
 	/* struct registry_priv	*pregpriv = &padapter->registrypriv; */
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
-	if (pmlmepriv->qospriv.qos_option == 0) {
+	if (pmlmepriv->qos_option == 0) {
 		pmlmeinfo->WMM_enable = 0;
 		return _FAIL;
 	}
 
 	pmlmeinfo->WMM_enable = 1;
-	memcpy(&pmlmeinfo->WMM_param, (pIE->data + 6),
+	memcpy(&pmlmeinfo->WMM_param, p + 2 + 6,
 	       sizeof(struct WMM_para_element));
 	return true;
 }
 
 void WMMOnAssocRsp23a(struct rtw_adapter *padapter)
 {
-	u8	ACI, ACM, AIFS, ECWMin, ECWMax, aSifsTime;
-	u8	acm_mask;
-	u16	TXOP;
-	u32	acParm, i;
-	u32	edca[4], inx[4];
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	u8 ACI, ACM, AIFS, ECWMin, ECWMax, aSifsTime;
+	u8 acm_mask;
+	u16 TXOP;
+	u32 acParm, i;
+	u32 edca[4], inx[4];
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	struct xmit_priv		*pxmitpriv = &padapter->xmitpriv;
-	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct registry_priv *pregpriv = &padapter->registrypriv;
 
 	if (pmlmeinfo->WMM_enable == 0) {
 		padapter->mlmepriv.acm_mask = 0;
@@ -639,9 +551,10 @@ void WMMOnAssocRsp23a(struct rtw_adapter *padapter)
 		ACM = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN >> 4) & 0x01;
 
 		/* AIFS = AIFSN * slot time + SIFS - r2t phy delay */
-		AIFS = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN & 0x0f) * pmlmeinfo->slotTime + aSifsTime;
+		AIFS = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN & 0x0f) *
+			pmlmeinfo->slotTime + aSifsTime;
 
-		ECWMin = (pmlmeinfo->WMM_param.ac_param[i].CW & 0x0f);
+		ECWMin = pmlmeinfo->WMM_param.ac_param[i].CW & 0x0f;
 		ECWMax = (pmlmeinfo->WMM_param.ac_param[i].CW & 0xf0) >> 4;
 		TXOP = le16_to_cpu(pmlmeinfo->WMM_param.ac_param[i].TXOP_limit);
 
@@ -649,22 +562,22 @@ void WMMOnAssocRsp23a(struct rtw_adapter *padapter)
 
 		switch (ACI) {
 		case 0x0:
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acParm));
+			rtl8723a_set_ac_param_be(padapter, acParm);
 			acm_mask |= (ACM? BIT(1):0);
 			edca[XMIT_BE_QUEUE] = acParm;
 			break;
 		case 0x1:
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acParm));
+			rtl8723a_set_ac_param_bk(padapter, acParm);
 			/* acm_mask |= (ACM? BIT(0):0); */
 			edca[XMIT_BK_QUEUE] = acParm;
 			break;
 		case 0x2:
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acParm));
+			rtl8723a_set_ac_param_vi(padapter, acParm);
 			acm_mask |= (ACM? BIT(2):0);
 			edca[XMIT_VI_QUEUE] = acParm;
 			break;
 		case 0x3:
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acParm));
+			rtl8723a_set_ac_param_vo(padapter, acParm);
 			acm_mask |= (ACM? BIT(3):0);
 			edca[XMIT_VO_QUEUE] = acParm;
 			break;
@@ -674,14 +587,14 @@ void WMMOnAssocRsp23a(struct rtw_adapter *padapter)
 	}
 
 	if (padapter->registrypriv.acm_method == 1)
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_ACM_CTRL, (u8 *)(&acm_mask));
+		rtl8723a_set_acm_ctrl(padapter, acm_mask);
 	else
 		padapter->mlmepriv.acm_mask = acm_mask;
 
 	inx[0] = 0; inx[1] = 1; inx[2] = 2; inx[3] = 3;
 
 	if (pregpriv->wifi_spec == 1) {
-		u32	j, tmp, change_inx = false;
+		u32 j, tmp, change_inx = false;
 
 		/* entry indx: 0->vo, 1->vi, 2->be, 3->bk. */
 		for (i = 0; i < 4; i++) {
@@ -689,7 +602,8 @@ void WMMOnAssocRsp23a(struct rtw_adapter *padapter)
 				/* compare CW and AIFS */
 				if ((edca[j] & 0xFFFF) < (edca[i] & 0xFFFF)) {
 					change_inx = true;
-				} else if ((edca[j] & 0xFFFF) == (edca[i] & 0xFFFF)) {
+				} else if ((edca[j] & 0xFFFF) ==
+					   (edca[i] & 0xFFFF)) {
 					/* compare TXOP */
 					if ((edca[j] >> 16) > (edca[i] >> 16))
 						change_inx = true;
@@ -712,31 +626,32 @@ void WMMOnAssocRsp23a(struct rtw_adapter *padapter)
 
 	for (i = 0; i<4; i++) {
 		pxmitpriv->wmm_para_seq[i] = inx[i];
-		DBG_8723A("wmm_para_seq(%d): %d\n", i, pxmitpriv->wmm_para_seq[i]);
+		DBG_8723A("wmm_para_seq(%d): %d\n", i,
+			  pxmitpriv->wmm_para_seq[i]);
 	}
 
 	return;
 }
 
-static void bwmode_update_check(struct rtw_adapter *padapter, struct ndis_802_11_var_ies * pIE)
+static void bwmode_update_check(struct rtw_adapter *padapter, u8 *p)
 {
-	struct HT_info_element	 *pHT_info;
+	struct HT_info_element *pHT_info;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct registry_priv *pregistrypriv = &padapter->registrypriv;
-	struct ht_priv	*phtpriv = &pmlmepriv->htpriv;
-	unsigned char	 new_bwmode;
-	unsigned char  new_ch_offset;
+	struct ht_priv *phtpriv = &pmlmepriv->htpriv;
+	unsigned char new_bwmode;
+	unsigned char new_ch_offset;
 
-	if (!pIE)
+	if (!p)
 		return;
 	if (!phtpriv->ht_option)
 		return;
-	if (pIE->Length > sizeof(struct HT_info_element))
+	if (p[1] > sizeof(struct HT_info_element))
 		return;
 
-	pHT_info = (struct HT_info_element *)pIE->data;
+	pHT_info = (struct HT_info_element *)(p + 2);
 
 	if ((pHT_info->infos[0] & BIT(2)) && pregistrypriv->cbw40_enable) {
 		new_bwmode = HT_CHANNEL_WIDTH_40;
@@ -757,133 +672,144 @@ static void bwmode_update_check(struct rtw_adapter *padapter, struct ndis_802_11
 		new_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 	}
 
-	if ((new_bwmode!= pmlmeext->cur_bwmode) ||
-	    (new_ch_offset!= pmlmeext->cur_ch_offset)) {
+	if (new_bwmode != pmlmeext->cur_bwmode ||
+	    new_ch_offset != pmlmeext->cur_ch_offset) {
 		pmlmeinfo->bwmode_updated = true;
 
 		pmlmeext->cur_bwmode = new_bwmode;
 		pmlmeext->cur_ch_offset = new_ch_offset;
 
 		/* update HT info also */
-		HT_info_handler23a(padapter, pIE);
-	} else {
+		HT_info_handler23a(padapter, p);
+	} else
 		pmlmeinfo->bwmode_updated = false;
-	}
 
 	if (pmlmeinfo->bwmode_updated) {
 		struct sta_info *psta;
 		struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
 		struct sta_priv	*pstapriv = &padapter->stapriv;
 
-		/* set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */
+		/* set_channel_bwmode23a(padapter, pmlmeext->cur_channel,
+		   pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */
 
 		/* update ap's stainfo */
 		psta = rtw_get_stainfo23a(pstapriv, cur_network->MacAddress);
 		if (psta) {
-			struct ht_priv	*phtpriv_sta = &psta->htpriv;
+			struct ht_priv *phtpriv_sta = &psta->htpriv;
 
 			if (phtpriv_sta->ht_option) {
 				/*  bwmode */
 				phtpriv_sta->bwmode = pmlmeext->cur_bwmode;
-				phtpriv_sta->ch_offset = pmlmeext->cur_ch_offset;
+				phtpriv_sta->ch_offset =
+					pmlmeext->cur_ch_offset;
 			} else {
 				phtpriv_sta->bwmode = HT_CHANNEL_WIDTH_20;
-				phtpriv_sta->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+				phtpriv_sta->ch_offset =
+					HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 			}
-
 		}
 	}
 }
 
-void HT_caps_handler23a(struct rtw_adapter *padapter, struct ndis_802_11_var_ies * pIE)
+void HT_caps_handler23a(struct rtw_adapter *padapter, u8 *p)
 {
-	unsigned int	i;
-	u8	rf_type;
-	u8	max_AMPDU_len, min_MPDU_spacing;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	unsigned int i;
+	u8 rf_type;
+	u8 max_AMPDU_len, min_MPDU_spacing;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct ht_priv			*phtpriv = &pmlmepriv->htpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv *phtpriv = &pmlmepriv->htpriv;
 
-	if (pIE == NULL) return;
+	if (!p)
+		return;
 
-	if (phtpriv->ht_option == false)	return;
+	if (phtpriv->ht_option == false)
+		return;
 
 	pmlmeinfo->HT_caps_enable = 1;
 
-	for (i = 0; i < (pIE->Length); i++) {
+	for (i = 0; i < p[1]; i++) {
 		if (i != 2) {
 			/*	Commented by Albert 2010/07/12 */
 			/*	Got the endian issue here. */
-			pmlmeinfo->HT_caps.u.HT_cap[i] &= (pIE->data[i]);
+			pmlmeinfo->HT_caps.u.HT_cap[i] &= p[i + 2];
 		} else {
 			/* modify from  fw by Thomas 2010/11/17 */
-			if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3) > (pIE->data[i] & 0x3))
-				max_AMPDU_len = (pIE->data[i] & 0x3);
+			if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3) > (p[i + 2] & 0x3))
+				max_AMPDU_len = p[i + 2] & 0x3;
 			else
-				max_AMPDU_len = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3);
+				max_AMPDU_len = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3;
 
-			if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) > (pIE->data[i] & 0x1c))
-				min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c);
+			if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) > (p[i + 2] & 0x1c))
+				min_MPDU_spacing = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c;
 			else
-				min_MPDU_spacing = (pIE->data[i] & 0x1c);
+				min_MPDU_spacing = p[i + 2] & 0x1c;
 
-			pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para = max_AMPDU_len | min_MPDU_spacing;
+			pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para =
+				max_AMPDU_len | min_MPDU_spacing;
 		}
 	}
 
 	/*	Commented by Albert 2010/07/12 */
 	/*	Have to handle the endian issue after copying. */
 	/*	HT_ext_caps didn't be used yet. */
-	pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info = le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info);
-	pmlmeinfo->HT_caps.u.HT_cap_element.HT_ext_caps = le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_ext_caps);
+	pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info =
+		le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info);
+	pmlmeinfo->HT_caps.u.HT_cap_element.HT_ext_caps =
+		le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_ext_caps);
 
-	rtw23a_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+	rf_type = rtl8723a_get_rf_type(padapter);
 
 	/* update the MCS rates */
 	for (i = 0; i < 16; i++) {
-		if ((rf_type == RF_1T1R) || (rf_type == RF_1T2R))
-			pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_1R23A[i];
+		if (rf_type == RF_1T1R || rf_type == RF_1T2R)
+			pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &=
+				MCS_rate_1R23A[i];
 		else
-			pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_2R23A[i];
+			pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &=
+				MCS_rate_2R23A[i];
 	}
 	return;
 }
 
-void HT_info_handler23a(struct rtw_adapter *padapter, struct ndis_802_11_var_ies * pIE)
+void HT_info_handler23a(struct rtw_adapter *padapter, u8 *p)
 {
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct ht_priv			*phtpriv = &pmlmepriv->htpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv *phtpriv = &pmlmepriv->htpriv;
 
-	if (pIE == NULL) return;
+	if (!p)
+		return;
 
-	if (phtpriv->ht_option == false)	return;
+	if (phtpriv->ht_option == false)
+		return;
 
-	if (pIE->Length > sizeof(struct HT_info_element))
+	if (p[1] > sizeof(struct HT_info_element))
 		return;
 
 	pmlmeinfo->HT_info_enable = 1;
-	memcpy(&pmlmeinfo->HT_info, pIE->data, pIE->Length);
+	memcpy(&pmlmeinfo->HT_info, p + 2, p[1]);
 	return;
 }
 
 void HTOnAssocRsp23a(struct rtw_adapter *padapter)
 {
-	unsigned char		max_AMPDU_len;
-	unsigned char		min_MPDU_spacing;
+	unsigned char max_AMPDU_len;
+	unsigned char min_MPDU_spacing;
 	/* struct registry_priv	 *pregpriv = &padapter->registrypriv; */
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
 	DBG_8723A("%s\n", __func__);
 
-	if ((pmlmeinfo->HT_info_enable) && (pmlmeinfo->HT_caps_enable)) {
+	if (pmlmeinfo->HT_info_enable && pmlmeinfo->HT_caps_enable)
 		pmlmeinfo->HT_enable = 1;
-	} else {
+	else {
 		pmlmeinfo->HT_enable = 0;
-		/* set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */
+		/* set_channel_bwmode23a(padapter, pmlmeext->cur_channel,
+		   pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */
 		return;
 	}
 
@@ -894,29 +820,29 @@ void HTOnAssocRsp23a(struct rtw_adapter *padapter)
 	*/
 	max_AMPDU_len = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x03;
 
-	min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) >> 2;
+	min_MPDU_spacing =
+		(pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) >> 2;
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_AMPDU_MIN_SPACE, (u8 *)(&min_MPDU_spacing));
-
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_AMPDU_FACTOR, (u8 *)(&max_AMPDU_len));
+	rtl8723a_set_ampdu_min_space(padapter, min_MPDU_spacing);
+	rtl8723a_set_ampdu_factor(padapter, max_AMPDU_len);
 }
 
-void ERP_IE_handler23a(struct rtw_adapter *padapter, struct ndis_802_11_var_ies * pIE)
+void ERP_IE_handler23a(struct rtw_adapter *padapter, u8 *p)
 {
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
-	if (pIE->Length>1)
+	if (p[1] > 1)
 		return;
 
 	pmlmeinfo->ERP_enable = 1;
-	memcpy(&pmlmeinfo->ERP_IE, pIE->data, pIE->Length);
+	memcpy(&pmlmeinfo->ERP_IE, p + 2, p[1]);
 }
 
 void VCS_update23a(struct rtw_adapter *padapter, struct sta_info *psta)
 {
-	struct registry_priv	 *pregpriv = &padapter->registrypriv;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
 	switch (pregpriv->vrtl_carrier_sense) { /* 0:off 1:on 2:auto */
@@ -935,7 +861,7 @@ void VCS_update23a(struct rtw_adapter *padapter, struct sta_info *psta)
 		break;
 	case 2: /* auto */
 	default:
-		if ((pmlmeinfo->ERP_enable) && (pmlmeinfo->ERP_IE & BIT(1))) {
+		if (pmlmeinfo->ERP_enable && pmlmeinfo->ERP_IE & BIT(1)) {
 			if (pregpriv->vcs_type == 1) {
 				psta->rtsen = 1;
 				psta->cts2self = 0;
@@ -951,116 +877,134 @@ void VCS_update23a(struct rtw_adapter *padapter, struct sta_info *psta)
 	}
 }
 
-int rtw_check_bcn_info23a(struct rtw_adapter *Adapter, u8 *pframe, u32 packet_len)
+int rtw_check_bcn_info23a(struct rtw_adapter *Adapter,
+			  struct ieee80211_mgmt *mgmt, u32 pkt_len)
 {
-	unsigned int		len;
-	unsigned char		*p;
-	unsigned short	val16;
 	struct wlan_network *cur_network = &Adapter->mlmepriv.cur_network;
-	u16 wpa_len = 0, rsn_len = 0;
-	u8 encryp_protocol = 0;
+	struct HT_info_element *pht_info;
+	struct ieee80211_ht_cap *pht_cap;
 	struct wlan_bssid_ex *bssid;
-	int group_cipher = 0, pairwise_cipher = 0, is_8021x = 0;
-	unsigned char *pbuf;
-	u32 wpa_ielen = 0;
-	u32 hidden_ssid = 0;
-	struct HT_info_element *pht_info = NULL;
-	struct ieee80211_ht_cap *pht_cap = NULL;
+	unsigned short val16;
+	u16 wpa_len = 0, rsn_len = 0;
+	u8 encryp_protocol;
+	int group_cipher = 0, pairwise_cipher = 0, is_8021x = 0, r;
 	u32 bcn_channel;
-	unsigned short	ht_cap_info;
-	unsigned char	ht_info_infos_0;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) pframe;
-	u8 *pbssid = hdr->addr3;
+	unsigned short ht_cap_info;
+	unsigned char ht_info_infos_0;
+	int len, pie_len, ie_offset;
+	const u8 *p;
+	u8 *pie;
 
 	if (is_client_associated_to_ap23a(Adapter) == false)
 		return true;
 
-	len = packet_len - sizeof(struct ieee80211_hdr_3addr);
+	if (unlikely(!ieee80211_is_beacon(mgmt->frame_control))) {
+		printk(KERN_WARNING "%s: received a non beacon frame!\n",
+		       __func__);
+		return false;
+	}
+
+	len = pkt_len - sizeof(struct ieee80211_hdr_3addr);
 
 	if (len > MAX_IE_SZ) {
 		DBG_8723A("%s IE too long for survey event\n", __func__);
 		return _FAIL;
 	}
 
-	if (memcmp(cur_network->network.MacAddress, pbssid, 6)) {
-		DBG_8723A("Oops: rtw_check_network_encrypt linked but recv other bssid bcn\n" MAC_FMT MAC_FMT,
-				MAC_ARG(pbssid), MAC_ARG(cur_network->network.MacAddress));
+	if (memcmp(cur_network->network.MacAddress, mgmt->bssid, 6)) {
+		DBG_8723A("Oops: rtw_check_network_encrypt linked but recv "
+			  "other bssid bcn\n" MAC_FMT MAC_FMT,
+			  MAC_ARG(mgmt->bssid),
+			  MAC_ARG(cur_network->network.MacAddress));
 		return true;
 	}
 
-	bssid = (struct wlan_bssid_ex *)kzalloc(sizeof(struct wlan_bssid_ex),
-		GFP_ATOMIC);
+	bssid = kzalloc(sizeof(struct wlan_bssid_ex), GFP_ATOMIC);
+	if (!bssid)
+		return _FAIL;
 
-	if (ieee80211_is_beacon(hdr->frame_control))
-		bssid->reserved = 1;
+	bssid->reserved = 1;
 
-	bssid->Length = sizeof(struct wlan_bssid_ex) - MAX_IE_SZ + len;
+	bssid->Length = offsetof(struct wlan_bssid_ex, IEs) + len;
 
 	/* below is to copy the information element */
 	bssid->IELength = len;
-	memcpy(bssid->IEs, (pframe + sizeof(struct ieee80211_hdr_3addr)), bssid->IELength);
+	memcpy(bssid->IEs, &mgmt->u, len);
 
 	/* check bw and channel offset */
 	/* parsing HT_CAP_IE */
-	p = rtw_get_ie23a(bssid->IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
-	if (p && len>0) {
-			pht_cap = (struct ieee80211_ht_cap *)(p + 2);
-			ht_cap_info = pht_cap->cap_info;
+	ie_offset = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+		offsetof(struct ieee80211_mgmt, u);
+	pie = bssid->IEs + ie_offset;
+	pie_len = pkt_len - ie_offset;
+
+	p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, pie, pie_len);
+	if (p && p[1] > 0) {
+		pht_cap = (struct ieee80211_ht_cap *)(p + 2);
+		ht_cap_info = pht_cap->cap_info;
 	} else {
-			ht_cap_info = 0;
+		pht_cap = NULL;
+		ht_cap_info = 0;
 	}
+
 	/* parsing HT_INFO_IE */
-	p = rtw_get_ie23a(bssid->IEs + _FIXED_IE_LENGTH_, _HT_ADD_INFO_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
-	if (p && len>0) {
-			pht_info = (struct HT_info_element *)(p + 2);
-			ht_info_infos_0 = pht_info->infos[0];
+	p = cfg80211_find_ie(WLAN_EID_HT_OPERATION, pie, pie_len);
+	if (p && p[1] > 0) {
+		pht_info = (struct HT_info_element *)(p + 2);
+		ht_info_infos_0 = pht_info->infos[0];
 	} else {
-			ht_info_infos_0 = 0;
+		pht_info = NULL;
+		ht_info_infos_0 = 0;
 	}
+
 	if (ht_cap_info != cur_network->BcnInfo.ht_cap_info ||
-		((ht_info_infos_0&0x03) != (cur_network->BcnInfo.ht_info_infos_0&0x03))) {
-			DBG_8723A("%s bcn now: ht_cap_info:%x ht_info_infos_0:%x\n", __func__,
-							ht_cap_info, ht_info_infos_0);
-			DBG_8723A("%s bcn link: ht_cap_info:%x ht_info_infos_0:%x\n", __func__,
-							cur_network->BcnInfo.ht_cap_info, cur_network->BcnInfo.ht_info_infos_0);
-			DBG_8723A("%s bw mode change, disconnect\n", __func__);
-			/* bcn_info_update */
-			cur_network->BcnInfo.ht_cap_info = ht_cap_info;
-			cur_network->BcnInfo.ht_info_infos_0 = ht_info_infos_0;
-			/* to do : need to check that whether modify related register of BB or not */
+	    ((ht_info_infos_0 & 0x03) !=
+	     (cur_network->BcnInfo.ht_info_infos_0 & 0x03))) {
+		DBG_8723A("%s bcn now: ht_cap_info:%x ht_info_infos_0:%x\n",
+			  __func__, ht_cap_info, ht_info_infos_0);
+		DBG_8723A("%s bcn link: ht_cap_info:%x ht_info_infos_0:%x\n",
+			  __func__, cur_network->BcnInfo.ht_cap_info,
+			  cur_network->BcnInfo.ht_info_infos_0);
+		DBG_8723A("%s bw mode change, disconnect\n", __func__);
+		/* bcn_info_update */
+		cur_network->BcnInfo.ht_cap_info = ht_cap_info;
+		cur_network->BcnInfo.ht_info_infos_0 = ht_info_infos_0;
+		/* to do : need to check that whether modify related
+		   register of BB or not */
 	}
 
 	/* Checking for channel */
-	p = rtw_get_ie23a(bssid->IEs + _FIXED_IE_LENGTH_, _DSSET_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
-	if (p) {
-			bcn_channel = *(p + 2);
-	} else {/* In 5G, some ap do not have DSSET IE checking HT info for channel */
-			p = rtw_get_ie23a(bssid->IEs + _FIXED_IE_LENGTH_, _HT_ADD_INFO_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
-			if (pht_info) {
-					bcn_channel = pht_info->primary_channel;
-			} else { /* we don't find channel IE, so don't check it */
-					DBG_8723A("Oops: %s we don't find channel IE, so don't check it\n", __func__);
-					bcn_channel = Adapter->mlmeextpriv.cur_channel;
-			}
+	p = cfg80211_find_ie(WLAN_EID_DS_PARAMS, pie, pie_len);
+	if (p)
+		bcn_channel = p[2];
+	else {
+		/* In 5G, some ap do not have DSSET IE checking HT
+		   info for channel */
+		p = cfg80211_find_ie(WLAN_EID_HT_OPERATION, pie, pie_len);
+
+		if (pht_info)
+			bcn_channel = pht_info->primary_channel;
+		else { /* we don't find channel IE, so don't check it */
+			DBG_8723A("Oops: %s we don't find channel IE, so don't "
+				  "check it\n", __func__);
+			bcn_channel = Adapter->mlmeextpriv.cur_channel;
+		}
 	}
 	if (bcn_channel != Adapter->mlmeextpriv.cur_channel) {
-			DBG_8723A("%s beacon channel:%d cur channel:%d disconnect\n", __func__,
-						   bcn_channel, Adapter->mlmeextpriv.cur_channel);
-			goto _mismatch;
+		DBG_8723A("%s beacon channel:%d cur channel:%d disconnect\n",
+			  __func__, bcn_channel,
+			  Adapter->mlmeextpriv.cur_channel);
+		goto _mismatch;
 	}
 
 	/* checking SSID */
-	if ((p = rtw_get_ie23a(bssid->IEs + _FIXED_IE_LENGTH_, _SSID_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_)) == NULL) {
-		DBG_8723A("%s marc: cannot find SSID for survey event\n", __func__);
-		hidden_ssid = true;
-	} else {
-		hidden_ssid = false;
-	}
-
-	if ((NULL != p) && (false == hidden_ssid && (*(p + 1)))) {
-		memcpy(bssid->Ssid.ssid, (p + 2), *(p + 1));
-		bssid->Ssid.ssid_len = *(p + 1);
+	p = cfg80211_find_ie(WLAN_EID_SSID, pie, pie_len);
+	if (p && p[1]) {
+		memcpy(bssid->Ssid.ssid, p + 2, p[1]);
+		bssid->Ssid.ssid_len = p[1];
 	} else {
+		DBG_8723A("%s marc: cannot find SSID for survey event\n",
+			  __func__);
 		bssid->Ssid.ssid_len = 0;
 		bssid->Ssid.ssid[0] = '\0';
 	}
@@ -1083,7 +1027,7 @@ int rtw_check_bcn_info23a(struct rtw_adapter *Adapter, u8 *pframe, u32 packet_le
 	}
 
 	/* check encryption info */
-	val16 = rtw_get_capability23a((struct wlan_bssid_ex *)bssid);
+	val16 = rtw_get_capability23a(bssid);
 
 	if (val16 & BIT(4))
 		bssid->Privacy = 1;
@@ -1091,60 +1035,83 @@ int rtw_check_bcn_info23a(struct rtw_adapter *Adapter, u8 *pframe, u32 packet_le
 		bssid->Privacy = 0;
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-			("%s(): cur_network->network.Privacy is %d, bssid.Privacy is %d\n",
-			 __func__, cur_network->network.Privacy, bssid->Privacy));
+		 ("%s(): cur_network->network.Privacy is %d, bssid.Privacy "
+		  "is %d\n", __func__, cur_network->network.Privacy,
+		  bssid->Privacy));
 	if (cur_network->network.Privacy != bssid->Privacy) {
 		DBG_8723A("%s(), privacy is not match return FAIL\n", __func__);
 		goto _mismatch;
 	}
 
-	rtw_get_sec_ie23a(bssid->IEs, bssid->IELength, NULL,&rsn_len, NULL,&wpa_len);
+	rtw_get_sec_ie23a(bssid->IEs, bssid->IELength, NULL, &rsn_len, NULL,
+			  &wpa_len);
 
-	if (rsn_len > 0) {
+	if (rsn_len > 0)
 		encryp_protocol = ENCRYP_PROTOCOL_WPA2;
-	} else if (wpa_len > 0) {
+	else if (wpa_len > 0)
 		encryp_protocol = ENCRYP_PROTOCOL_WPA;
-	} else {
+	else {
 		if (bssid->Privacy)
 			encryp_protocol = ENCRYP_PROTOCOL_WEP;
+		else
+			encryp_protocol = ENCRYP_PROTOCOL_OPENSYS;
 	}
 
 	if (cur_network->BcnInfo.encryp_protocol != encryp_protocol) {
-		DBG_8723A("%s(): enctyp is not match , return FAIL\n", __func__);
+		DBG_8723A("%s(): enctyp is not match, return FAIL\n", __func__);
 		goto _mismatch;
 	}
 
-	if (encryp_protocol == ENCRYP_PROTOCOL_WPA || encryp_protocol == ENCRYP_PROTOCOL_WPA2) {
-		pbuf = rtw_get_wpa_ie23a(&bssid->IEs[12], &wpa_ielen, bssid->IELength-12);
-		if (pbuf && (wpa_ielen>0)) {
-			if (_SUCCESS == rtw_parse_wpa_ie23a(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is_8021x)) {
+	if (encryp_protocol == ENCRYP_PROTOCOL_WPA ||
+	    encryp_protocol == ENCRYP_PROTOCOL_WPA2) {
+		p = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
+					    WLAN_OUI_TYPE_MICROSOFT_WPA,
+					    pie, pie_len);
+		if (p && p[1] > 0) {
+			r = rtw_parse_wpa_ie23a(p, p[1] + 2, &group_cipher,
+						&pairwise_cipher, &is_8021x);
+			if (r == _SUCCESS)
 				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-						("%s pnetwork->pairwise_cipher: %d, group_cipher is %d, is_8021x is %d\n", __func__,
-						 pairwise_cipher, group_cipher, is_8021x));
-			}
+					 ("%s pnetwork->pairwise_cipher: %d, "
+					  "group_cipher is %d, is_8021x is "
+					  "%d\n", __func__, pairwise_cipher,
+					  group_cipher, is_8021x));
 		} else {
-			pbuf = rtw_get_wpa2_ie23a(&bssid->IEs[12], &wpa_ielen, bssid->IELength-12);
-
-			if (pbuf && (wpa_ielen>0)) {
-				if (_SUCCESS == rtw_parse_wpa2_ie23a(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is_8021x)) {
-					RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-							("%s pnetwork->pairwise_cipher: %d, pnetwork->group_cipher is %d, is_802x is %d\n",
-							 __func__, pairwise_cipher, group_cipher, is_8021x));
-				}
+			p = cfg80211_find_ie(WLAN_EID_RSN, pie, pie_len);
+
+			if (p && p[1] > 0) {
+				r = rtw_parse_wpa2_ie23a(p, p[1] + 2,
+							 &group_cipher,
+							 &pairwise_cipher,
+							 &is_8021x);
+				if (r == _SUCCESS)
+					RT_TRACE(_module_rtl871x_mlme_c_,
+						 _drv_info_,
+						 ("%s pnetwork->pairwise_cipher"
+						  ": %d, pnetwork->group_cipher"
+						  " is %d, is_802x is %d\n",
+						  __func__, pairwise_cipher,
+						  group_cipher, is_8021x));
 			}
 		}
 
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-				("%s cur_network->group_cipher is %d: %d\n", __func__, cur_network->BcnInfo.group_cipher, group_cipher));
-		if (pairwise_cipher != cur_network->BcnInfo.pairwise_cipher || group_cipher != cur_network->BcnInfo.group_cipher) {
-			DBG_8723A("%s pairwise_cipher(%x:%x) or group_cipher(%x:%x) is not match , return FAIL\n", __func__,
-					pairwise_cipher, cur_network->BcnInfo.pairwise_cipher,
-					group_cipher, cur_network->BcnInfo.group_cipher);
+			 ("%s cur_network->group_cipher is %d: %d\n", __func__,
+			  cur_network->BcnInfo.group_cipher, group_cipher));
+		if (pairwise_cipher != cur_network->BcnInfo.pairwise_cipher ||
+		    group_cipher != cur_network->BcnInfo.group_cipher) {
+			DBG_8723A("%s pairwise_cipher(%x:%x) or group_cipher "
+				  "(%x:%x) is not match, return FAIL\n",
+				  __func__, pairwise_cipher,
+				  cur_network->BcnInfo.pairwise_cipher,
+				  group_cipher,
+				  cur_network->BcnInfo.group_cipher);
 			goto _mismatch;
 		}
 
 		if (is_8021x != cur_network->BcnInfo.is_8021x) {
-			DBG_8723A("%s authentication is not match , return FAIL\n", __func__);
+			DBG_8723A("%s authentication is not match, return "
+				  "FAIL\n", __func__);
 			goto _mismatch;
 		}
 	}
@@ -1158,93 +1125,106 @@ _mismatch:
 	return _FAIL;
 }
 
-void update_beacon23a_info(struct rtw_adapter *padapter, u8 *pframe, uint pkt_len, struct sta_info *psta)
+void update_beacon23a_info(struct rtw_adapter *padapter, u8 *pframe,
+			   uint pkt_len, struct sta_info *psta)
 {
 	unsigned int i;
 	unsigned int len;
-	struct ndis_802_11_var_ies *	pIE;
+	u8 *p;
 
 	len = pkt_len -
 		(_BEACON_IE_OFFSET_ + sizeof(struct ieee80211_hdr_3addr));
 
 	for (i = 0; i < len;) {
-		pIE = (struct ndis_802_11_var_ies *)(pframe + (_BEACON_IE_OFFSET_ + sizeof(struct ieee80211_hdr_3addr)) + i);
+		p = (u8 *)(pframe + (_BEACON_IE_OFFSET_ + sizeof(struct ieee80211_hdr_3addr)) + i);
 
-		switch (pIE->ElementID) {
-		case _HT_EXTRA_INFO_IE_:	/* HT info */
+		switch (p[0]) {
+		case WLAN_EID_HT_OPERATION:	/* HT info */
 			/* HT_info_handler23a(padapter, pIE); */
-			bwmode_update_check(padapter, pIE);
+			bwmode_update_check(padapter, p);
 			break;
-		case _ERPINFO_IE_:
-			ERP_IE_handler23a(padapter, pIE);
+		case WLAN_EID_ERP_INFO:
+			ERP_IE_handler23a(padapter, p);
 			VCS_update23a(padapter, psta);
 			break;
 		default:
 			break;
 		}
-		i += (pIE->Length + 2);
+		i += (p[1] + 2);
 	}
 }
 
-unsigned int is_ap_in_tkip23a(struct rtw_adapter *padapter)
+bool is_ap_in_tkip23a(struct rtw_adapter *padapter)
 {
 	u32 i;
-	struct ndis_802_11_var_ies *	pIE;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
+	const u8 *p;
+	int bcn_fixed_size;
+
+	bcn_fixed_size = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+		offsetof(struct ieee80211_mgmt, u.beacon);
 
-	if (rtw_get_capability23a((struct wlan_bssid_ex *)cur_network) & WLAN_CAPABILITY_PRIVACY) {
-		for (i = sizeof(struct ndis_802_11_fixed_ies); i < pmlmeinfo->network.IELength;) {
-			pIE = (struct ndis_802_11_var_ies *)(pmlmeinfo->network.IEs + i);
+	if (rtw_get_capability23a(cur_network) & WLAN_CAPABILITY_PRIVACY) {
+		for (i = bcn_fixed_size; i < pmlmeinfo->network.IELength;) {
+			p = pmlmeinfo->network.IEs + i;
 
-			switch (pIE->ElementID) {
-			case _VENDOR_SPECIFIC_IE_:
-				if ((!memcmp(pIE->data, RTW_WPA_OUI23A, 4)) && (!memcmp((pIE->data + 12), WPA_TKIP_CIPHER23A, 4)))
+			switch (p[0]) {
+			case WLAN_EID_VENDOR_SPECIFIC:
+				if (!memcmp(p + 2, RTW_WPA_OUI23A_TYPE, 4) &&
+				    !memcmp(p + 2 + 12, WPA_TKIP_CIPHER, 4))
 					return true;
 				break;
-			case _RSN_IE_2_:
-				if (!memcmp((pIE->data + 8), RSN_TKIP_CIPHER23A, 4))
+			case WLAN_EID_RSN:
+				if (!memcmp(p + 2 + 8, RSN_TKIP_CIPHER, 4))
 					return true;
 				break;
 			default:
 				break;
 			}
-			i += (pIE->Length + 2);
+			i += (p[1] + 2);
 		}
 		return false;
-	} else {
+	} else
 		return false;
-	}
 }
 
-unsigned int should_forbid_n_rate23a(struct rtw_adapter * padapter)
+bool should_forbid_n_rate23a(struct rtw_adapter * padapter)
 {
 	u32 i;
-	struct ndis_802_11_var_ies *	pIE;
-	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct wlan_bssid_ex  *cur_network = &pmlmepriv->cur_network.network;
-
-	if (rtw_get_capability23a((struct wlan_bssid_ex *)cur_network) & WLAN_CAPABILITY_PRIVACY) {
-		for (i = sizeof(struct ndis_802_11_fixed_ies); i < cur_network->IELength;) {
-			pIE = (struct ndis_802_11_var_ies *)(cur_network->IEs + i);
-
-			switch (pIE->ElementID) {
-			case _VENDOR_SPECIFIC_IE_:
-				if (!memcmp(pIE->data, RTW_WPA_OUI23A, 4) &&
-					((!memcmp((pIE->data + 12), WPA_CIPHER_SUITE_CCMP23A, 4)) ||
-					  (!memcmp((pIE->data + 16), WPA_CIPHER_SUITE_CCMP23A, 4))))
+	const u8 *p;
+	int bcn_fixed_size;
+
+	bcn_fixed_size = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+		offsetof(struct ieee80211_mgmt, u.beacon);
+
+	if (rtw_get_capability23a(cur_network) & WLAN_CAPABILITY_PRIVACY) {
+		for (i = bcn_fixed_size; i < cur_network->IELength;) {
+			p = cur_network->IEs + i;
+
+			switch (p[0]) {
+			case WLAN_EID_VENDOR_SPECIFIC:
+				if (!memcmp(p + 2, RTW_WPA_OUI23A_TYPE, 4) &&
+				    (!memcmp(p + 2 + 12,
+					     WPA_CIPHER_SUITE_CCMP23A, 4) ||
+				     !memcmp(p + 2 + 16,
+					     WPA_CIPHER_SUITE_CCMP23A, 4)))
 					return false;
 				break;
-			case _RSN_IE_2_:
-				if  ((!memcmp((pIE->data + 8), RSN_CIPHER_SUITE_CCMP23A, 4))  ||
-				       (!memcmp((pIE->data + 12), RSN_CIPHER_SUITE_CCMP23A, 4)))
+			case WLAN_EID_RSN:
+				if (!memcmp(p + 2 + 8,
+					    RSN_CIPHER_SUITE_CCMP23A, 4) ||
+				    !memcmp(p + 2 + 12,
+					    RSN_CIPHER_SUITE_CCMP23A, 4))
 				return false;
 			default:
 				break;
 			}
 
-			i += (pIE->Length + 2);
+			i += (p[1] + 2);
 		}
 		return true;
 	} else {
@@ -1252,42 +1232,45 @@ unsigned int should_forbid_n_rate23a(struct rtw_adapter * padapter)
 	}
 }
 
-unsigned int is_ap_in_wep23a(struct rtw_adapter *padapter)
+bool is_ap_in_wep23a(struct rtw_adapter *padapter)
 {
 	u32 i;
-	struct ndis_802_11_var_ies *	pIE;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
+	const u8 *p;
+	int bcn_fixed_size;
 
-	if (rtw_get_capability23a((struct wlan_bssid_ex *)cur_network) & WLAN_CAPABILITY_PRIVACY) {
-		for (i = sizeof(struct ndis_802_11_fixed_ies); i < pmlmeinfo->network.IELength;) {
-			pIE = (struct ndis_802_11_var_ies *)(pmlmeinfo->network.IEs + i);
+	bcn_fixed_size = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+		offsetof(struct ieee80211_mgmt, u.beacon);
 
-			switch (pIE->ElementID) {
-			case _VENDOR_SPECIFIC_IE_:
-				if (!memcmp(pIE->data, RTW_WPA_OUI23A, 4))
+	if (rtw_get_capability23a(cur_network) & WLAN_CAPABILITY_PRIVACY) {
+		for (i = bcn_fixed_size; i < pmlmeinfo->network.IELength;) {
+			p = pmlmeinfo->network.IEs + i;
+
+			switch (p[0]) {
+			case WLAN_EID_VENDOR_SPECIFIC:
+				if (!memcmp(p + 2, RTW_WPA_OUI23A_TYPE, 4))
 					return false;
 				break;
-			case _RSN_IE_2_:
+			case WLAN_EID_RSN:
 				return false;
 
 			default:
 				break;
 			}
 
-			i += (pIE->Length + 2);
+			i += (p[1] + 2);
 		}
 
 		return true;
-	} else {
+	} else
 		return false;
-	}
 }
 
-int wifirate2_ratetbl_inx23a(unsigned char rate)
+static int wifirate2_ratetbl_inx23a(unsigned char rate)
 {
-	int	inx = 0;
+	int inx = 0;
 	rate = rate & 0x7f;
 
 	switch (rate) {
@@ -1350,7 +1333,7 @@ unsigned int update_supported_rate23a(unsigned char *ptn, unsigned int ptn_sz)
 	unsigned int i, num_of_rate;
 	unsigned int mask = 0;
 
-	num_of_rate = (ptn_sz > NumRates)? NumRates: ptn_sz;
+	num_of_rate = (ptn_sz > NumRates) ? NumRates : ptn_sz;
 
 	for (i = 0; i < num_of_rate; i++)
 		mask |= 0x1 << wifirate2_ratetbl_inx23a(*(ptn + i));
@@ -1361,21 +1344,22 @@ unsigned int update_MSC_rate23a(struct HT_caps_element *pHT_caps)
 {
 	unsigned int mask = 0;
 
-	mask = ((pHT_caps->u.HT_cap_element.MCS_rate[0] << 12) | (pHT_caps->u.HT_cap_element.MCS_rate[1] << 20));
+	mask = pHT_caps->u.HT_cap_element.MCS_rate[0] << 12 |
+		pHT_caps->u.HT_cap_element.MCS_rate[1] << 20;
 
 	return mask;
 }
 
 int support_short_GI23a(struct rtw_adapter *padapter,
-		     struct HT_caps_element *pHT_caps)
+			struct HT_caps_element *pHT_caps)
 {
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	unsigned char bit_offset;
 
-	if (!(pmlmeinfo->HT_enable))
+	if (!pmlmeinfo->HT_enable)
 		return _FAIL;
-	if ((pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_RALINK))
+	if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_RALINK)
 		return _FAIL;
 	bit_offset = (pmlmeext->cur_bwmode & HT_CHANNEL_WIDTH_40)? 6: 5;
 
@@ -1399,25 +1383,13 @@ unsigned char get_highest_rate_idx23a(u32 mask)
 	return rate_idx;
 }
 
-unsigned char get_highest_mcs_rate(struct HT_caps_element *pHT_caps)
-{
-	int i, mcs_rate;
-
-	mcs_rate = (pHT_caps->u.HT_cap_element.MCS_rate[0] | (pHT_caps->u.HT_cap_element.MCS_rate[1] << 8));
-
-	for (i = 15; i >= 0; i--) {
-		if (mcs_rate & (0x1 << i))
-			break;
-	}
-	return i;
-}
-
 void Update_RA_Entry23a(struct rtw_adapter *padapter, struct sta_info *psta)
 {
 	rtw_hal_update_ra_mask23a(psta, 0);
 }
 
-void enable_rate_adaptive(struct rtw_adapter *padapter, struct sta_info *psta)
+static void enable_rate_adaptive(struct rtw_adapter *padapter,
+				 struct sta_info *psta)
 {
 	Update_RA_Entry23a(padapter, psta);
 }
@@ -1432,19 +1404,10 @@ void set_sta_rate23a(struct rtw_adapter *padapter, struct sta_info *psta)
 void update_tx_basic_rate23a(struct rtw_adapter *padapter, u8 wirelessmode)
 {
 	unsigned char supported_rates[NDIS_802_11_LENGTH_RATES_EX];
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info*	pwdinfo = &padapter->wdinfo;
-
-	/*	Added by Albert 2011/03/22 */
-	/*	In the P2P mode, the driver should not support the b mode. */
-	/*	So, the Tx packet shouldn't use the CCK rate */
-	if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
-		return;
-#endif /* CONFIG_8723AU_P2P */
 
 	memset(supported_rates, 0, NDIS_802_11_LENGTH_RATES_EX);
 
-	if ((wirelessmode & WIRELESS_11B) && (wirelessmode == WIRELESS_11B)) {
+	if (wirelessmode == WIRELESS_11B) {
 		memcpy(supported_rates, rtw_basic_rate_cck, 4);
 	} else if (wirelessmode & WIRELESS_11B) {
 		memcpy(supported_rates, rtw_basic_rate_mix, 7);
@@ -1457,67 +1420,68 @@ void update_tx_basic_rate23a(struct rtw_adapter *padapter, u8 wirelessmode)
 	else
 		update_mgnt_tx_rate23a(padapter, IEEE80211_OFDM_RATE_6MB);
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_BASIC_RATE, supported_rates);
+	HalSetBrateCfg23a(padapter, supported_rates);
 }
 
 unsigned char check_assoc_AP23a(u8 *pframe, uint len)
 {
-	unsigned int	i;
-	struct ndis_802_11_var_ies *	pIE;
-	u8	epigram_vendor_flag;
-	u8	ralink_vendor_flag;
+	int i, bcn_fixed_size;
+	u8 epigram_vendor_flag;
+	u8 ralink_vendor_flag;
+	const u8 *p;
 	epigram_vendor_flag = 0;
 	ralink_vendor_flag = 0;
 
-	for (i = sizeof(struct ndis_802_11_fixed_ies); i < len;) {
-		pIE = (struct ndis_802_11_var_ies *)(pframe + i);
+	bcn_fixed_size = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+		offsetof(struct ieee80211_mgmt, u.beacon);
+
+	for (i = bcn_fixed_size; i < len;) {
+		p = pframe + i;
 
-		switch (pIE->ElementID) {
-		case _VENDOR_SPECIFIC_IE_:
-			if ((!memcmp(pIE->data, ARTHEROS_OUI1, 3)) ||
-			    (!memcmp(pIE->data, ARTHEROS_OUI2, 3))) {
+		switch (p[0]) {
+		case WLAN_EID_VENDOR_SPECIFIC:
+			if (!memcmp(p + 2, ARTHEROS_OUI1, 3) ||
+			    !memcmp(p + 2, ARTHEROS_OUI2, 3)) {
 				DBG_8723A("link to Artheros AP\n");
 				return HT_IOT_PEER_ATHEROS;
-			} else if ((!memcmp(pIE->data, BROADCOM_OUI1, 3)) ||
-				   !memcmp(pIE->data, BROADCOM_OUI2, 3) ||
-				   !memcmp(pIE->data, BROADCOM_OUI2, 3)) {
+			} else if (!memcmp(p + 2, BROADCOM_OUI1, 3) ||
+				   !memcmp(p + 2, BROADCOM_OUI2, 3) ||
+				   !memcmp(p + 2, BROADCOM_OUI2, 3)) {
 				DBG_8723A("link to Broadcom AP\n");
 				return HT_IOT_PEER_BROADCOM;
-			} else if (!memcmp(pIE->data, MARVELL_OUI, 3)) {
+			} else if (!memcmp(p + 2, MARVELL_OUI, 3)) {
 				DBG_8723A("link to Marvell AP\n");
 				return HT_IOT_PEER_MARVELL;
-			} else if (!memcmp(pIE->data, RALINK_OUI, 3)) {
-				if (!ralink_vendor_flag) {
+			} else if (!memcmp(p + 2, RALINK_OUI, 3)) {
+				if (!ralink_vendor_flag)
 					ralink_vendor_flag = 1;
-				} else {
+				else {
 					DBG_8723A("link to Ralink AP\n");
 					return HT_IOT_PEER_RALINK;
 				}
-			} else if (!memcmp(pIE->data, CISCO_OUI, 3)) {
+			} else if (!memcmp(p + 2, CISCO_OUI, 3)) {
 				DBG_8723A("link to Cisco AP\n");
 				return HT_IOT_PEER_CISCO;
-			} else if (!memcmp(pIE->data, REALTEK_OUI, 3)) {
+			} else if (!memcmp(p + 2, REALTEK_OUI, 3)) {
 				DBG_8723A("link to Realtek 96B\n");
 				return HT_IOT_PEER_REALTEK;
-			} else if (!memcmp(pIE->data, AIRGOCAP_OUI, 3)) {
+			} else if (!memcmp(p + 2, AIRGOCAP_OUI, 3)) {
 				DBG_8723A("link to Airgo Cap\n");
 				return HT_IOT_PEER_AIRGO;
-			} else if (!memcmp(pIE->data, EPIGRAM_OUI, 3)) {
+			} else if (!memcmp(p + 2, EPIGRAM_OUI, 3)) {
 				epigram_vendor_flag = 1;
 				if (ralink_vendor_flag) {
 					DBG_8723A("link to Tenda W311R AP\n");
 					return HT_IOT_PEER_TENDA;
-				} else {
+				} else
 					DBG_8723A("Capture EPIGRAM_OUI\n");
-				}
-			} else {
+			} else
 				break;
-			}
 		default:
 			break;
 		}
 
-		i += (pIE->Length + 2);
+		i += (p[1] + 2);
 	}
 
 	if (ralink_vendor_flag && !epigram_vendor_flag) {
@@ -1534,7 +1498,7 @@ unsigned char check_assoc_AP23a(u8 *pframe, uint len)
 
 void update_IOT_info23a(struct rtw_adapter *padapter)
 {
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
 	switch (pmlmeinfo->assoc_AP_vendor) {
@@ -1546,15 +1510,15 @@ void update_IOT_info23a(struct rtw_adapter *padapter)
 		pmlmeinfo->turboMode_cts2self = 0;
 		pmlmeinfo->turboMode_rtsen = 1;
 		/* disable high power */
-		Switch_DM_Func23a(padapter, ~DYNAMIC_BB_DYNAMIC_TXPWR,
-			       false);
+		rtl8723a_odm_support_ability_clr(padapter, (u32)
+						 ~DYNAMIC_BB_DYNAMIC_TXPWR);
 		break;
 	case HT_IOT_PEER_REALTEK:
 		/* rtw_write16(padapter, 0x4cc, 0xffff); */
 		/* rtw_write16(padapter, 0x546, 0x01c0); */
 		/* disable high power */
-		Switch_DM_Func23a(padapter, ~DYNAMIC_BB_DYNAMIC_TXPWR,
-			       false);
+		rtl8723a_odm_support_ability_clr(padapter, (u32)
+						 ~DYNAMIC_BB_DYNAMIC_TXPWR);
 		break;
 	default:
 		pmlmeinfo->turboMode_cts2self = 0;
@@ -1565,24 +1529,21 @@ void update_IOT_info23a(struct rtw_adapter *padapter)
 
 void update_capinfo23a(struct rtw_adapter *Adapter, u16 updateCap)
 {
-	struct mlme_ext_priv	*pmlmeext = &Adapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &Adapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	bool		ShortPreamble;
 
 	if (updateCap & cShortPreamble) {
 		/*  Short Preamble */
 		if (pmlmeinfo->preamble_mode != PREAMBLE_SHORT) {
 			/*  PREAMBLE_LONG or PREAMBLE_AUTO */
-			ShortPreamble = true;
 			pmlmeinfo->preamble_mode = PREAMBLE_SHORT;
-			rtw_hal_set_hwreg23a(Adapter, HW_VAR_ACK_PREAMBLE, (u8 *)&ShortPreamble);
+			rtl8723a_ack_preamble(Adapter, true);
 		}
 	} else { /*  Long Preamble */
 		if (pmlmeinfo->preamble_mode != PREAMBLE_LONG) {
 			/*  PREAMBLE_SHORT or PREAMBLE_AUTO */
-			ShortPreamble = false;
 			pmlmeinfo->preamble_mode = PREAMBLE_LONG;
-			rtw_hal_set_hwreg23a(Adapter, HW_VAR_ACK_PREAMBLE, (u8 *)&ShortPreamble);
+			rtl8723a_ack_preamble(Adapter, false);
 		}
 	}
 	if (updateCap & cIBSS) {
@@ -1590,32 +1551,34 @@ void update_capinfo23a(struct rtw_adapter *Adapter, u16 updateCap)
 		pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME;
 	} else {
 		/* Filen: See 802.11-2007 p.90 */
-		if (pmlmeext->cur_wireless_mode & (WIRELESS_11G | WIRELESS_11_24N)) {
+		if (pmlmeext->cur_wireless_mode &
+		    (WIRELESS_11G | WIRELESS_11_24N)) {
 			if (updateCap & cShortSlotTime) { /*  Short Slot Time */
 				if (pmlmeinfo->slotTime != SHORT_SLOT_TIME)
 					pmlmeinfo->slotTime = SHORT_SLOT_TIME;
 			} else { /*  Long Slot Time */
 				if (pmlmeinfo->slotTime != NON_SHORT_SLOT_TIME)
-					pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME;
+					pmlmeinfo->slotTime =
+						NON_SHORT_SLOT_TIME;
 			}
-		} else if (pmlmeext->cur_wireless_mode & (WIRELESS_11A | WIRELESS_11_5N)) {
+		} else if (pmlmeext->cur_wireless_mode &
+			   (WIRELESS_11A | WIRELESS_11_5N)) {
 			pmlmeinfo->slotTime = SHORT_SLOT_TIME;
 		} else {
 			/* B Mode */
 			pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME;
 		}
 	}
-	rtw_hal_set_hwreg23a(Adapter, HW_VAR_SLOT_TIME, &pmlmeinfo->slotTime);
+	rtl8723a_set_slot_time(Adapter, pmlmeinfo->slotTime);
 }
 
 void update_wireless_mode23a(struct rtw_adapter *padapter)
 {
 	int ratelen, network_type = 0;
-	u32 SIFS_Timer;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
-	unsigned char			*rate = cur_network->SupportedRates;
+	unsigned char *rate = cur_network->SupportedRates;
 
 	ratelen = rtw_get_rateset_len23a(cur_network->SupportedRates);
 
@@ -1630,20 +1593,20 @@ void update_wireless_mode23a(struct rtw_adapter *padapter)
 		if (pmlmeinfo->HT_enable)
 			network_type = WIRELESS_11_24N;
 
-		if ((cckratesonly_included23a(rate, ratelen)) == true)
+		if (cckratesonly_included23a(rate, ratelen) == true)
 			network_type |= WIRELESS_11B;
-		else if ((cckrates_included23a(rate, ratelen)) == true)
+		else if (cckrates_included23a(rate, ratelen) == true)
 			network_type |= WIRELESS_11BG;
 		else
 			network_type |= WIRELESS_11G;
 	}
 
-	pmlmeext->cur_wireless_mode = network_type & padapter->registrypriv.wireless_mode;
+	pmlmeext->cur_wireless_mode =
+		network_type & padapter->registrypriv.wireless_mode;
 
-	SIFS_Timer = 0x0a0a0808; /* 0x0808 -> for CCK, 0x0a0a -> for OFDM */
-                             /* change this value if having IOT issues. */
-
-	padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_RESP_SIFS,  (u8 *)&SIFS_Timer);
+	/* 0x0808 -> for CCK, 0x0a0a -> for OFDM */
+	/* change this value if having IOT issues. */
+	rtl8723a_set_resp_sifs(padapter, 0x08, 0x08, 0x0a, 0x0a);
 
 	if (pmlmeext->cur_wireless_mode & WIRELESS_11B)
 		update_mgnt_tx_rate23a(padapter, IEEE80211_CCK_RATE_1MB);
@@ -1658,41 +1621,45 @@ void update_bmc_sta_support_rate23a(struct rtw_adapter *padapter, u32 mac_id)
 
 	if (pmlmeext->cur_wireless_mode & WIRELESS_11B) {
 		/*  Only B, B/G, and B/G/N AP could use CCK rate */
-		memcpy((pmlmeinfo->FW_sta_info[mac_id].SupportedRates), rtw_basic_rate_cck, 4);
+		memcpy((pmlmeinfo->FW_sta_info[mac_id].SupportedRates),
+		       rtw_basic_rate_cck, 4);
 	} else {
-		memcpy((pmlmeinfo->FW_sta_info[mac_id].SupportedRates), rtw_basic_rate_ofdm, 4);
+		memcpy(pmlmeinfo->FW_sta_info[mac_id].SupportedRates,
+		       rtw_basic_rate_ofdm, 3);
 	}
 }
 
-int update_sta_support_rate23a(struct rtw_adapter *padapter, u8 *pvar_ie, uint var_ie_len, int cam_idx)
+int update_sta_support_rate23a(struct rtw_adapter *padapter, u8 *pvar_ie,
+			       uint var_ie_len, int cam_idx)
 {
-	unsigned int	ie_len;
-	struct ndis_802_11_var_ies *pIE;
-	int	supportRateNum = 0;
+	int supportRateNum = 0;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+	const u8 *p;
 
-	pIE = (struct ndis_802_11_var_ies *)rtw_get_ie23a(pvar_ie, _SUPPORTEDRATES_IE_, &ie_len, var_ie_len);
-	if (pIE == NULL)
+	p = cfg80211_find_ie(WLAN_EID_SUPP_RATES, pvar_ie, var_ie_len);
+	if (!p)
 		return _FAIL;
 
-	memcpy(pmlmeinfo->FW_sta_info[cam_idx].SupportedRates, pIE->data, ie_len);
-	supportRateNum = ie_len;
+	memcpy(pmlmeinfo->FW_sta_info[cam_idx].SupportedRates, p + 2, p[1]);
+	supportRateNum = p[1];
 
-	pIE = (struct ndis_802_11_var_ies *)rtw_get_ie23a(pvar_ie, _EXT_SUPPORTEDRATES_IE_, &ie_len, var_ie_len);
-	if (pIE)
-		memcpy((pmlmeinfo->FW_sta_info[cam_idx].SupportedRates + supportRateNum), pIE->data, ie_len);
+	p = cfg80211_find_ie(WLAN_EID_EXT_SUPP_RATES, pvar_ie, var_ie_len);
+	if (p)
+		memcpy(pmlmeinfo->FW_sta_info[cam_idx].SupportedRates +
+		       supportRateNum, p + 2, p[1]);
 	return _SUCCESS;
 }
 
-void process_addba_req23a(struct rtw_adapter *padapter, u8 *paddba_req, u8 *addr)
+void process_addba_req23a(struct rtw_adapter *padapter,
+			  u8 *paddba_req, u8 *addr)
 {
 	struct sta_info *psta;
 	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 mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct ADDBA_request *preq = (struct ADDBA_request*)paddba_req;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
 	psta = rtw_get_stainfo23a(pstapriv, addr);
@@ -1701,41 +1668,17 @@ void process_addba_req23a(struct rtw_adapter *padapter, u8 *paddba_req, u8 *addr
 		start_seq = le16_to_cpu(preq->BA_starting_seqctrl) >> 4;
 
 		param = le16_to_cpu(preq->BA_para_set);
-		tid = (param>>2)&0x0f;
+		tid = (param >> 2) & 0x0f;
 
 		preorder_ctrl = &psta->recvreorder_ctrl[tid];
 
 		preorder_ctrl->indicate_seq = 0xffff;
 
-		preorder_ctrl->enable = (pmlmeinfo->bAcceptAddbaReq == true)? true :false;
+		preorder_ctrl->enable = (pmlmeinfo->bAcceptAddbaReq == true) ?
+			true : false;
 	}
 }
 
-void update_TSF23a(struct mlme_ext_priv *pmlmeext, u8 *pframe, uint len)
-{
-	u8 *pIE;
-	u32 *pbuf;
-
-	pIE = pframe + sizeof(struct ieee80211_hdr_3addr);
-	pbuf = (u32 *)pIE;
-
-	pmlmeext->TSFValue = le32_to_cpu(*(pbuf+1));
-
-	pmlmeext->TSFValue = pmlmeext->TSFValue << 32;
-
-	pmlmeext->TSFValue |= le32_to_cpu(*pbuf);
-}
-
-void correct_TSF23a(struct rtw_adapter *padapter, struct mlme_ext_priv *pmlmeext)
-{
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_CORRECT_TSF, NULL);
-}
-
-void beacon_timing_control23a(struct rtw_adapter *padapter)
-{
-	rtw_hal_bcn_related_reg_setting23a(padapter);
-}
-
 static struct rtw_adapter *pbuddy_padapter;
 
 int rtw_handle_dualmac23a(struct rtw_adapter *adapter, bool init)
@@ -1745,16 +1688,18 @@ int rtw_handle_dualmac23a(struct rtw_adapter *adapter, bool init)
 	if (init) {
 		if (pbuddy_padapter == NULL) {
 			pbuddy_padapter = adapter;
-			DBG_8723A("%s(): pbuddy_padapter == NULL, Set pbuddy_padapter\n", __func__);
+			DBG_8723A("%s(): pbuddy_padapter == NULL, "
+				  "Set pbuddy_padapter\n", __func__);
 		} else {
 			adapter->pbuddy_adapter = pbuddy_padapter;
 			pbuddy_padapter->pbuddy_adapter = adapter;
 			/*  clear global value */
 			pbuddy_padapter = NULL;
-			DBG_8723A("%s(): pbuddy_padapter exist, Exchange Information\n", __func__);
+			DBG_8723A("%s(): pbuddy_padapter exist, "
+				  "Exchange Information\n", __func__);
 		}
-	  } else {
+	} else
 		pbuddy_padapter = NULL;
-	}
+
 	return status;
 }
diff --git a/drivers/staging/rtl8723au/core/rtw_xmit.c b/drivers/staging/rtl8723au/core/rtw_xmit.c
index 0f10cfa10d3..b917526f570 100644
--- a/drivers/staging/rtl8723au/core/rtw_xmit.c
+++ b/drivers/staging/rtl8723au/core/rtw_xmit.c
@@ -20,6 +20,7 @@
 #include <osdep_intf.h>
 #include <linux/ip.h>
 #include <usb_ops.h>
+#include <rtl8723a_xmit.h>
 
 static u8 P802_1H_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0xf8 };
 static u8 RFC1042_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0x00 };
@@ -50,27 +51,21 @@ void	_rtw_init_sta_xmit_priv23a(struct sta_xmit_priv *psta_xmitpriv)
 
 }
 
-s32	_rtw_init_xmit_priv23a(struct xmit_priv *pxmitpriv, struct rtw_adapter *padapter)
+int _rtw_init_xmit_priv23a(struct xmit_priv *pxmitpriv,
+			   struct rtw_adapter *padapter)
 {
 	int i;
 	struct xmit_buf *pxmitbuf;
 	struct xmit_frame *pxframe;
-	int	res = _SUCCESS;
+	int res = _SUCCESS;
 	u32 max_xmit_extbuf_size = MAX_XMIT_EXTBUF_SZ;
 	u32 num_xmit_extbuf = NR_XMIT_EXTBUFF;
 
-	/*  We don't need to memset padapter->XXX to zero, because adapter is allocated by rtw_zvmalloc(). */
-	/* memset((unsigned char *)pxmitpriv, 0, sizeof(struct xmit_priv)); */
-
 	spin_lock_init(&pxmitpriv->lock);
 	spin_lock_init(&pxmitpriv->lock_sctx);
 	sema_init(&pxmitpriv->xmit_sema, 0);
 	sema_init(&pxmitpriv->terminate_xmitthread_sema, 0);
 
-	/*
-	Please insert all the queue initializaiton using _rtw_init_queue23a below
-	*/
-
 	pxmitpriv->adapter = padapter;
 
 	_rtw_init_queue23a(&pxmitpriv->be_pending);
@@ -81,42 +76,20 @@ s32	_rtw_init_xmit_priv23a(struct xmit_priv *pxmitpriv, struct rtw_adapter *pada
 
 	_rtw_init_queue23a(&pxmitpriv->free_xmit_queue);
 
-	/*
-	Please allocate memory with the sz = (struct xmit_frame) * NR_XMITFRAME,
-	and initialize free_xmit_frame below.
-	Please also apply  free_txobj to link_up all the xmit_frames...
-	*/
-
-	pxmitpriv->pallocated_frame_buf = rtw_zvmalloc(NR_XMITFRAME * sizeof(struct xmit_frame) + 4);
-
-	if (pxmitpriv->pallocated_frame_buf  == NULL) {
-		pxmitpriv->pxmit_frame_buf = NULL;
-		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xmit_frame fail!\n"));
-		res = _FAIL;
-		goto exit;
-	}
-	pxmitpriv->pxmit_frame_buf = PTR_ALIGN(pxmitpriv->pallocated_frame_buf, 4);
-
-	pxframe = (struct xmit_frame*) pxmitpriv->pxmit_frame_buf;
-
 	for (i = 0; i < NR_XMITFRAME; i++) {
+		pxframe = kzalloc(sizeof(struct xmit_frame), GFP_KERNEL);
+		if (!pxframe)
+			break;
 		INIT_LIST_HEAD(&pxframe->list);
 
 		pxframe->padapter = padapter;
 		pxframe->frame_tag = NULL_FRAMETAG;
 
-		pxframe->pkt = NULL;
-
-		pxframe->buf_addr = NULL;
-		pxframe->pxmitbuf = NULL;
-
 		list_add_tail(&pxframe->list,
 			      &pxmitpriv->free_xmit_queue.queue);
-
-		pxframe++;
 	}
 
-	pxmitpriv->free_xmitframe_cnt = NR_XMITFRAME;
+	pxmitpriv->free_xmitframe_cnt = i;
 
 	pxmitpriv->frag_len = MAX_FRAG_THRESHOLD;
 
@@ -152,18 +125,10 @@ s32	_rtw_init_xmit_priv23a(struct xmit_priv *pxmitpriv, struct rtw_adapter *pada
 	/* init xframe_ext queue,  the same count as extbuf  */
 	_rtw_init_queue23a(&pxmitpriv->free_xframe_ext_queue);
 
-	pxmitpriv->xframe_ext_alloc_addr = rtw_zvmalloc(num_xmit_extbuf * sizeof(struct xmit_frame) + 4);
-
-	if (pxmitpriv->xframe_ext_alloc_addr  == NULL) {
-		pxmitpriv->xframe_ext = NULL;
-		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xframe_ext fail!\n"));
-		res = _FAIL;
-		goto exit;
-	}
-	pxmitpriv->xframe_ext = PTR_ALIGN(pxmitpriv->xframe_ext_alloc_addr, 4);
-	pxframe = (struct xmit_frame*)pxmitpriv->xframe_ext;
-
 	for (i = 0; i < num_xmit_extbuf; i++) {
+		pxframe = kzalloc(sizeof(struct xmit_frame), GFP_KERNEL);
+		if (!pxframe)
+			break;
 		INIT_LIST_HEAD(&pxframe->list);
 
 		pxframe->padapter = padapter;
@@ -178,10 +143,8 @@ s32	_rtw_init_xmit_priv23a(struct xmit_priv *pxmitpriv, struct rtw_adapter *pada
 
 		list_add_tail(&pxframe->list,
 			      &pxmitpriv->free_xframe_ext_queue.queue);
-
-		pxframe++;
 	}
-	pxmitpriv->free_xframe_ext_cnt = num_xmit_extbuf;
+	pxmitpriv->free_xframe_ext_cnt = i;
 
 	/*  Init xmit extension buff */
 	_rtw_init_queue23a(&pxmitpriv->free_xmit_extbuf_queue);
@@ -230,7 +193,9 @@ s32	_rtw_init_xmit_priv23a(struct xmit_priv *pxmitpriv, struct rtw_adapter *pada
 	pxmitpriv->ack_tx = false;
 	mutex_init(&pxmitpriv->ack_tx_mutex);
 	rtw_sctx_init23a(&pxmitpriv->ack_tx_ops, 0);
-	rtw_hal_init23a_xmit_priv(padapter);
+	tasklet_init(&padapter->xmitpriv.xmit_tasklet,
+		     (void(*)(unsigned long))rtl8723au_xmit_tasklet,
+		     (unsigned long)padapter);
 
 exit:
 
@@ -242,19 +207,15 @@ fail:
 void _rtw_free_xmit_priv23a (struct xmit_priv *pxmitpriv)
 {
 	struct rtw_adapter *padapter = pxmitpriv->adapter;
-	struct xmit_frame *pxmitframe = (struct xmit_frame*) pxmitpriv->pxmit_frame_buf;
+	struct xmit_frame *pxframe;
 	struct xmit_buf *pxmitbuf;
 	struct list_head *plist, *ptmp;
-	u32 num_xmit_extbuf = NR_XMIT_EXTBUFF;
-	int i;
-
-	rtw_hal_free_xmit_priv23a(padapter);
 
-	if (pxmitpriv->pxmit_frame_buf == NULL)
-		return;
-	for (i = 0; i < NR_XMITFRAME; i++) {
-		rtw_os_xmit_complete23a(padapter, pxmitframe);
-		pxmitframe++;
+	list_for_each_safe(plist, ptmp, &pxmitpriv->free_xmit_queue.queue) {
+		pxframe = container_of(plist, struct xmit_frame, list);
+		list_del_init(&pxframe->list);
+		rtw_os_xmit_complete23a(padapter, pxframe);
+		kfree(pxframe);
 	}
 
 	list_for_each_safe(plist, ptmp, &pxmitpriv->xmitbuf_list) {
@@ -264,19 +225,14 @@ void _rtw_free_xmit_priv23a (struct xmit_priv *pxmitpriv)
 		kfree(pxmitbuf);
 	}
 
-	if (pxmitpriv->pallocated_frame_buf) {
-		rtw_vmfree(pxmitpriv->pallocated_frame_buf, NR_XMITFRAME * sizeof(struct xmit_frame) + 4);
-	}
-
 	/* free xframe_ext queue,  the same count as extbuf  */
-	if ((pxmitframe = (struct xmit_frame*)pxmitpriv->xframe_ext)) {
-		for (i = 0; i<num_xmit_extbuf; i++) {
-			rtw_os_xmit_complete23a(padapter, pxmitframe);
-			pxmitframe++;
-		}
+	list_for_each_safe(plist, ptmp,
+			   &pxmitpriv->free_xframe_ext_queue.queue) {
+		pxframe = container_of(plist, struct xmit_frame, list);
+		list_del_init(&pxframe->list);
+		rtw_os_xmit_complete23a(padapter, pxframe);
+		kfree(pxframe);
 	}
-	if (pxmitpriv->xframe_ext_alloc_addr)
-		rtw_vmfree(pxmitpriv->xframe_ext_alloc_addr, num_xmit_extbuf * sizeof(struct xmit_frame) + 4);
 
 	/*  free xmit extension buff */
 	list_for_each_safe(plist, ptmp, &pxmitpriv->xmitextbuf_list) {
@@ -338,9 +294,10 @@ static void update_attrib_vcs_info(struct rtw_adapter *padapter, struct xmit_fra
 	} else {
 		while (true) {
 			/* IOT action */
-			if ((pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_ATHEROS) &&
-			    (pattrib->ampdu_en) &&
-			    (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)) {
+			if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_ATHEROS &&
+			    pattrib->ampdu_en &&
+			    padapter->securitypriv.dot11PrivacyAlgrthm ==
+			    WLAN_CIPHER_SUITE_CCMP) {
 				pattrib->vcs_mode = CTS_TO_SELF;
 				break;
 			}
@@ -437,27 +394,24 @@ u8 qos_acm23a(u8 acm_mask, u8 priority)
 	default:
 		DBG_8723A("qos_acm23a(): invalid pattrib->priority: %d!!!\n",
 			  priority);
+		change_priority = 0;
 		break;
 	}
 
 	return change_priority;
 }
 
-static void set_qos(struct pkt_file *ppktfile, struct pkt_attrib *pattrib)
+static void set_qos(struct sk_buff *skb, struct pkt_attrib *pattrib)
 {
-	struct ethhdr etherhdr;
-	struct iphdr ip_hdr;
-	s32 UserPriority = 0;
-
-	_rtw_open_pktfile23a(ppktfile->pkt, ppktfile);
-	_rtw_pktfile_read23a(ppktfile, (unsigned char*)&etherhdr, ETH_HLEN);
+	u8 *pframe = skb->data;
+	struct iphdr *ip_hdr;
+	u8 UserPriority = 0;
 
 	/*  get UserPriority from IP hdr */
-	if (pattrib->ether_type == 0x0800) {
-		_rtw_pktfile_read23a(ppktfile, (u8*)&ip_hdr, sizeof(ip_hdr));
-/*		UserPriority = (ntohs(ip_hdr.tos) >> 5) & 0x3; */
-		UserPriority = ip_hdr.tos >> 5;
-	} else if (pattrib->ether_type == 0x888e) {
+	if (pattrib->ether_type == ETH_P_IP) {
+		ip_hdr = (struct iphdr *)(pframe + ETH_HLEN);
+		UserPriority = ip_hdr->tos >> 5;
+	} else if (pattrib->ether_type == ETH_P_PAE) {
 		/*  "When priority processing of data frames is supported, */
 		/*  a STA's SME should send EAPOL-Key frames at the highest
 		    priority." */
@@ -466,61 +420,54 @@ static void set_qos(struct pkt_file *ppktfile, struct pkt_attrib *pattrib)
 
 	pattrib->priority = UserPriority;
 	pattrib->hdrlen = sizeof(struct ieee80211_qos_hdr);
-	pattrib->subtype = WIFI_QOS_DATA_TYPE;
+	pattrib->type = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_QOS_DATA;
 }
 
-static s32 update_attrib(struct rtw_adapter *padapter,
-			 struct sk_buff *pkt, struct pkt_attrib *pattrib)
+static int update_attrib(struct rtw_adapter *padapter,
+			 struct sk_buff *skb, struct pkt_attrib *pattrib)
 {
-	uint i;
-	struct pkt_file pktfile;
 	struct sta_info *psta = NULL;
-	struct ethhdr etherhdr;
-
 	int bmcast;
 	struct sta_priv	*pstapriv = &padapter->stapriv;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct qos_priv	*pqospriv = &pmlmepriv->qospriv;
 	int res = _SUCCESS;
+	struct ethhdr *ehdr = (struct ethhdr *) skb->data;
 
-	_rtw_open_pktfile23a(pkt, &pktfile);
-	i = _rtw_pktfile_read23a(&pktfile, (u8*)&etherhdr, ETH_HLEN);
-
-	pattrib->ether_type = ntohs(etherhdr.h_proto);
+	pattrib->ether_type = ntohs(ehdr->h_proto);
 
-	memcpy(pattrib->dst, &etherhdr.h_dest, ETH_ALEN);
-	memcpy(pattrib->src, &etherhdr.h_source, ETH_ALEN);
+	ether_addr_copy(pattrib->dst, ehdr->h_dest);
+	ether_addr_copy(pattrib->src, ehdr->h_source);
 
 	pattrib->pctrl = 0;
 
-	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) ||
-		(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)) {
-		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
-		memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
-	}
-	else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
-		memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
-		memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
-	}
-	else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
-		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
-		memcpy(pattrib->ta, get_bssid(pmlmepriv), ETH_ALEN);
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+		ether_addr_copy(pattrib->ra, pattrib->dst);
+		ether_addr_copy(pattrib->ta, pattrib->src);
+	} else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+		ether_addr_copy(pattrib->ra, get_bssid(pmlmepriv));
+		ether_addr_copy(pattrib->ta, pattrib->src);
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		ether_addr_copy(pattrib->ra, pattrib->dst);
+		ether_addr_copy(pattrib->ta, get_bssid(pmlmepriv));
 	}
 
-	pattrib->pktlen = pktfile.pkt_len;
+	pattrib->pktlen = skb->len - ETH_HLEN;
 
 	if (pattrib->ether_type == ETH_P_IP) {
 		/*  The following is for DHCP and ARP packet, we use cck1M
 		    to tx these packets and let LPS awake some time */
 		/*  to prevent DHCP protocol fail */
-		u8 tmp[24];
-		_rtw_pktfile_read23a(&pktfile, &tmp[0], 24);
 		pattrib->dhcp_pkt = 0;
-		if (pktfile.pkt_len > 282) {/* MINIMUM_DHCP_PACKET_SIZE) { */
-			if (ETH_P_IP == pattrib->ether_type) {/*  IP header */
-				if (((tmp[21] == 68) && (tmp[23] == 67)) ||
-				    ((tmp[21] == 67) && (tmp[23] == 68))) {
+		/* MINIMUM_DHCP_PACKET_SIZE) { */
+		if (pattrib->pktlen > 282 + 24) {
+			if (pattrib->ether_type == ETH_P_IP) {/*  IP header */
+				u8 *pframe = skb->data;
+				pframe += ETH_HLEN;
+
+				if ((pframe[21] == 68 && pframe[23] == 67) ||
+				    (pframe[21] == 67 && pframe[23] == 68)) {
 					/*  68 : UDP BOOTP client */
 					/*  67 : UDP BOOTP server */
 					RT_TRACE(_module_rtl871x_xmit_c_,
@@ -532,17 +479,17 @@ static s32 update_attrib(struct rtw_adapter *padapter,
 				}
 			}
 		}
-	} else if (0x888e == pattrib->ether_type) {
+	} else if (pattrib->ether_type == ETH_P_PAE) {
 		DBG_8723A_LEVEL(_drv_always_, "send eapol packet\n");
 	}
 
-	if ((pattrib->ether_type == 0x888e) || (pattrib->dhcp_pkt == 1)) {
+	if ((pattrib->ether_type == ETH_P_PAE) || (pattrib->dhcp_pkt == 1)) {
 		rtw_set_scan_deny(padapter, 3000);
 	}
 
 	/*  If EAPOL , ARP , OR DHCP packet, driver must be in active mode. */
-	if ((pattrib->ether_type == 0x0806) ||
-	    (pattrib->ether_type == 0x888e) || (pattrib->dhcp_pkt == 1)) {
+	if ((pattrib->ether_type == ETH_P_ARP) ||
+	    (pattrib->ether_type == ETH_P_PAE) || (pattrib->dhcp_pkt == 1)) {
 		rtw_lps_ctrl_wk_cmd23a(padapter, LPS_CTRL_SPECIAL_PACKET, 1);
 	}
 
@@ -559,7 +506,7 @@ static s32 update_attrib(struct rtw_adapter *padapter,
 				  MAC_FMT"\n", MAC_ARG(pattrib->ra)));
 			res = _FAIL;
 			goto exit;
-		} else if ((check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) &&
+		} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) &&
 			   (!(psta->state & _FW_LINKED))) {
 			res = _FAIL;
 			goto exit;
@@ -586,16 +533,16 @@ static s32 update_attrib(struct rtw_adapter *padapter,
 	pattrib->pkt_hdrlen = ETH_HLEN;
 
 	pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
-	pattrib->subtype = WIFI_DATA_TYPE;
+	pattrib->type = IEEE80211_FTYPE_DATA;
 	pattrib->priority = 0;
 
 	if (check_fwstate(pmlmepriv, WIFI_AP_STATE | WIFI_ADHOC_STATE |
 			  WIFI_ADHOC_MASTER_STATE)) {
 		if (psta->qos_option)
-			set_qos(&pktfile, pattrib);
+			set_qos(skb, pattrib);
 	} else {
-		if (pqospriv->qos_option) {
-			set_qos(&pktfile, pattrib);
+		if (pmlmepriv->qos_option) {
+			set_qos(skb, pattrib);
 
 			if (pmlmepriv->acm_mask != 0) {
 				pattrib->priority = qos_acm23a(pmlmepriv->acm_mask,
@@ -610,8 +557,8 @@ static s32 update_attrib(struct rtw_adapter *padapter,
 
 		pattrib->encrypt = 0;
 
-		if ((pattrib->ether_type != 0x888e) &&
-		    (check_fwstate(pmlmepriv, WIFI_MP_STATE) == false)) {
+		if ((pattrib->ether_type != ETH_P_PAE) &&
+		    !check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
 			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
 				 ("\npsta->ieee8021x_blocked == true,  "
 				  "pattrib->ether_type(%.4x) != 0x888e\n",
@@ -644,31 +591,32 @@ static s32 update_attrib(struct rtw_adapter *padapter,
 	}
 
 	switch (pattrib->encrypt) {
-	case _WEP40_:
-	case _WEP104_:
-		pattrib->iv_len = 4;
-		pattrib->icv_len = 4;
+	case WLAN_CIPHER_SUITE_WEP40:
+	case WLAN_CIPHER_SUITE_WEP104:
+		pattrib->iv_len = IEEE80211_WEP_IV_LEN;
+		pattrib->icv_len = IEEE80211_WEP_ICV_LEN;
 		break;
 
-	case _TKIP_:
-		pattrib->iv_len = 8;
-		pattrib->icv_len = 4;
+	case WLAN_CIPHER_SUITE_TKIP:
+		pattrib->iv_len = IEEE80211_TKIP_IV_LEN;
+		pattrib->icv_len = IEEE80211_TKIP_ICV_LEN;
 
-		if (padapter->securitypriv.busetkipkey == _FAIL) {
+		if (!padapter->securitypriv.busetkipkey) {
 			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
 				 ("\npadapter->securitypriv.busetkip"
-				  "key(%d) == _FAIL drop packet\n",
+				  "key(%d) == false drop packet\n",
 				  padapter->securitypriv.busetkipkey));
 			res = _FAIL;
 			goto exit;
 		}
 
 		break;
-	case _AES_:
+	case WLAN_CIPHER_SUITE_CCMP:
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
-			 ("pattrib->encrypt =%d (_AES_)\n", pattrib->encrypt));
-		pattrib->iv_len = 8;
-		pattrib->icv_len = 8;
+			 ("pattrib->encrypt =%d (WLAN_CIPHER_SUITE_CCMP)\n",
+			  pattrib->encrypt));
+		pattrib->iv_len = IEEE80211_CCMP_HDR_LEN;
+		pattrib->icv_len = IEEE80211_CCMP_MIC_LEN;
 		break;
 
 	default:
@@ -680,7 +628,7 @@ static s32 update_attrib(struct rtw_adapter *padapter,
 	RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
 		 ("update_attrib: encrypt =%d\n", pattrib->encrypt));
 
-	if (pattrib->encrypt && psecuritypriv->hw_decrypted == false) {
+	if (pattrib->encrypt && !psecuritypriv->hw_decrypted) {
 		pattrib->bswenc = true;
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
 			 ("update_attrib: encrypt =%d bswenc = true\n",
@@ -697,7 +645,7 @@ exit:
 	return res;
 }
 
-static s32 xmitframe_addmic(struct rtw_adapter *padapter,
+static int xmitframe_addmic(struct rtw_adapter *padapter,
 			    struct xmit_frame *pxmitframe) {
 	struct mic_data micdata;
 	struct sta_info *stainfo;
@@ -730,7 +678,7 @@ static s32 xmitframe_addmic(struct rtw_adapter *padapter,
 
 	hw_hdr_offset = TXDESC_OFFSET;
 
-	if (pattrib->encrypt == _TKIP_) {
+	if (pattrib->encrypt == WLAN_CIPHER_SUITE_TKIP) {
 		/* encode mic code */
 		if (stainfo) {
 			u8 null_key[16]={0x0, 0x0, 0x0, 0x0,
@@ -775,7 +723,7 @@ static s32 xmitframe_addmic(struct rtw_adapter *padapter,
 							 &pframe[10], 6);
 			}
 
-			/* if (pqospriv->qos_option == 1) */
+			/* if (pmlmepriv->qos_option == 1) */
 			if (pattrib->qos_en)
 				priority[0] = (u8)pxmitframe->attrib.priority;
 
@@ -873,7 +821,7 @@ static s32 xmitframe_addmic(struct rtw_adapter *padapter,
 	return _SUCCESS;
 }
 
-static s32 xmitframe_swencrypt(struct rtw_adapter *padapter,
+static int xmitframe_swencrypt(struct rtw_adapter *padapter,
 			       struct xmit_frame *pxmitframe)
 {
 	struct pkt_attrib *pattrib = &pxmitframe->attrib;
@@ -884,14 +832,14 @@ static s32 xmitframe_swencrypt(struct rtw_adapter *padapter,
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_,
 			 ("### xmitframe_swencrypt\n"));
 		switch (pattrib->encrypt) {
-		case _WEP40_:
-		case _WEP104_:
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
 			rtw_wep_encrypt23a(padapter, pxmitframe);
 			break;
-		case _TKIP_:
+		case WLAN_CIPHER_SUITE_TKIP:
 			rtw_tkip_encrypt23a(padapter, pxmitframe);
 			break;
-		case _AES_:
+		case WLAN_CIPHER_SUITE_CCMP:
 			rtw_aes_encrypt23a(padapter, pxmitframe);
 			break;
 		default:
@@ -906,17 +854,14 @@ static s32 xmitframe_swencrypt(struct rtw_adapter *padapter,
 	return _SUCCESS;
 }
 
-s32 rtw_make_wlanhdr23a(struct rtw_adapter *padapter, u8 *hdr,
-		        struct pkt_attrib *pattrib)
+static int rtw_make_wlanhdr(struct rtw_adapter *padapter, u8 *hdr,
+			    struct pkt_attrib *pattrib)
 {
-	u16 *qc;
-
 	struct ieee80211_hdr *pwlanhdr = (struct ieee80211_hdr *)hdr;
+	struct ieee80211_qos_hdr *qoshdr;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct qos_priv *pqospriv = &pmlmepriv->qospriv;
 	u8 qos_option = false;
 	int res = _SUCCESS;
-	u16 *fctrl = &pwlanhdr->frame_control;
 
 	struct sta_info *psta;
 
@@ -945,36 +890,36 @@ s32 rtw_make_wlanhdr23a(struct rtw_adapter *padapter, u8 *hdr,
 
 	memset(hdr, 0, WLANHDR_OFFSET);
 
-	SetFrameSubType(fctrl, pattrib->subtype);
+	pwlanhdr->frame_control = cpu_to_le16(pattrib->type);
 
-	if (pattrib->subtype & WIFI_DATA_TYPE) {
-		if ((check_fwstate(pmlmepriv,  WIFI_STATION_STATE) == true)) {
+	if (pattrib->type & IEEE80211_FTYPE_DATA) {
+		if (check_fwstate(pmlmepriv,  WIFI_STATION_STATE)) {
 			/* to_ds = 1, fr_ds = 0; */
 			/* Data transfer to AP */
-			SetToDs(fctrl);
-			memcpy(pwlanhdr->addr1, get_bssid(pmlmepriv), ETH_ALEN);
-			memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
-			memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN);
+			pwlanhdr->frame_control |=
+				cpu_to_le16(IEEE80211_FCTL_TODS);
+			ether_addr_copy(pwlanhdr->addr1, get_bssid(pmlmepriv));
+			ether_addr_copy(pwlanhdr->addr2, pattrib->src);
+			ether_addr_copy(pwlanhdr->addr3, pattrib->dst);
 
-			if (pqospriv->qos_option)
+			if (pmlmepriv->qos_option)
 				qos_option = true;
 
-		}
-		else if ((check_fwstate(pmlmepriv,  WIFI_AP_STATE) == true)) {
+		} else if (check_fwstate(pmlmepriv,  WIFI_AP_STATE)) {
 			/* to_ds = 0, fr_ds = 1; */
-			SetFrDs(fctrl);
-			memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
-			memcpy(pwlanhdr->addr2, get_bssid(pmlmepriv), ETH_ALEN);
-			memcpy(pwlanhdr->addr3, pattrib->src, ETH_ALEN);
+			pwlanhdr->frame_control |=
+				cpu_to_le16(IEEE80211_FCTL_FROMDS);
+			ether_addr_copy(pwlanhdr->addr1, pattrib->dst);
+			ether_addr_copy(pwlanhdr->addr2, get_bssid(pmlmepriv));
+			ether_addr_copy(pwlanhdr->addr3, pattrib->src);
 
 			if (psta->qos_option)
 				qos_option = true;
-		}
-		else if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) ||
-		(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)) {
-			memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
-			memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
-			memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv), ETH_ALEN);
+		} else if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
+			   check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+			ether_addr_copy(pwlanhdr->addr1, pattrib->dst);
+			ether_addr_copy(pwlanhdr->addr2, pattrib->src);
+			ether_addr_copy(pwlanhdr->addr3, get_bssid(pmlmepriv));
 
 			if (psta->qos_option)
 				qos_option = true;
@@ -985,15 +930,24 @@ s32 rtw_make_wlanhdr23a(struct rtw_adapter *padapter, u8 *hdr,
 			goto exit;
 		}
 		if (pattrib->mdata)
-			SetMData(fctrl);
+			pwlanhdr->frame_control |=
+				cpu_to_le16(IEEE80211_FCTL_MOREDATA);
 		if (pattrib->encrypt)
-			SetPrivacy(fctrl);
+			pwlanhdr->frame_control |=
+				cpu_to_le16(IEEE80211_FCTL_PROTECTED);
 		if (qos_option) {
-			qc = (unsigned short *)(hdr + pattrib->hdrlen - 2);
-			if (pattrib->priority)
-				SetPriority(qc, pattrib->priority);
-			SetEOSP(qc, pattrib->eosp);
-			SetAckpolicy(qc, pattrib->ack_policy);
+			qoshdr = (struct ieee80211_qos_hdr *)hdr;
+
+			qoshdr->qos_ctrl = cpu_to_le16(
+				pattrib->priority & IEEE80211_QOS_CTL_TID_MASK);
+
+			qoshdr->qos_ctrl |= cpu_to_le16(
+				(pattrib->ack_policy << 5) &
+				IEEE80211_QOS_CTL_ACK_POLICY_MASK);
+
+			if (pattrib->eosp)
+				qoshdr->qos_ctrl |=
+					cpu_to_le16(IEEE80211_QOS_CTL_EOSP);
 		}
 		/* TODO: fill HT Control Field */
 
@@ -1002,11 +956,18 @@ s32 rtw_make_wlanhdr23a(struct rtw_adapter *padapter, u8 *hdr,
 			psta->sta_xmitpriv.txseq_tid[pattrib->priority]++;
 			psta->sta_xmitpriv.txseq_tid[pattrib->priority] &= 0xFFF;
 			pattrib->seqnum = psta->sta_xmitpriv.txseq_tid[pattrib->priority];
-			SetSeqNum(hdr, pattrib->seqnum);
+			/* We dont need to worry about frag bits here */
+			pwlanhdr->seq_ctrl = cpu_to_le16(IEEE80211_SN_TO_SEQ(
+							      pattrib->seqnum));
 			/* check if enable ampdu */
 			if (pattrib->ht_en && psta->htpriv.ampdu_enable) {
-				if (psta->htpriv.agg_enable_bitmap & CHKBIT(pattrib->priority))
-				pattrib->ampdu_en = true;
+				if (pattrib->priority >= 16)
+					printk(KERN_WARNING "%s: Invalid "
+					       "pattrib->priority %i\n",
+					       __func__, pattrib->priority);
+				if (psta->htpriv.agg_enable_bitmap &
+				    BIT(pattrib->priority))
+					pattrib->ampdu_en = true;
 			}
 			/* re-check if enable ampdu by BA_starting_seqctrl */
 			if (pattrib->ampdu_en) {
@@ -1037,10 +998,10 @@ s32 rtw_txframes_pending23a(struct rtw_adapter *padapter)
 {
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 
-	return (!_rtw_queue_empty23a(&pxmitpriv->be_pending)) ||
-	       (!_rtw_queue_empty23a(&pxmitpriv->bk_pending)) ||
-	       (!_rtw_queue_empty23a(&pxmitpriv->vi_pending)) ||
-	       (!_rtw_queue_empty23a(&pxmitpriv->vo_pending));
+	return (!list_empty(&pxmitpriv->be_pending.queue)) ||
+		(!list_empty(&pxmitpriv->bk_pending.queue)) ||
+		(!list_empty(&pxmitpriv->vi_pending.queue)) ||
+		(!list_empty(&pxmitpriv->vo_pending.queue));
 }
 
 s32 rtw_txframes_sta_ac_pending23a(struct rtw_adapter *padapter,
@@ -1098,7 +1059,7 @@ u32 rtw_calculate_wlan_pkt_size_by_attribue23a(struct pkt_attrib *pattrib)
 	len = pattrib->hdrlen + pattrib->iv_len; /*  WLAN Header and IV */
 	len += SNAP_SIZE + sizeof(u16); /*  LLC */
 	len += pattrib->pktlen;
-	if (pattrib->encrypt == _TKIP_) len += 8; /*  MIC */
+	if (pattrib->encrypt == WLAN_CIPHER_SUITE_TKIP) len += 8; /*  MIC */
 	len += ((pattrib->bswenc) ? pattrib->icv_len : 0); /*  ICV */
 
 	return len;
@@ -1116,39 +1077,41 @@ This sub-routine will perform all the following:
 6. apply sw-encrypt, if necessary.
 
 */
-s32 rtw_xmitframe_coalesce23a(struct rtw_adapter *padapter, struct sk_buff *pkt,
+int rtw_xmitframe_coalesce23a(struct rtw_adapter *padapter, struct sk_buff *skb,
 			      struct xmit_frame *pxmitframe)
 {
-	struct pkt_file pktfile;
-	struct sta_info		*psta;
-	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
-	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	struct sta_info *psta;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+	struct ieee80211_hdr *hdr;
 	s32 frg_inx, frg_len, mpdu_len, llc_sz, mem_sz;
 	u8 *pframe, *mem_start;
 	u8 hw_hdr_offset;
 	u8 *pbuf_start;
-
+	u8 *pdata = skb->data;
+	int data_len = skb->len;
 	s32 bmcst = is_multicast_ether_addr(pattrib->ra);
-	s32 res = _SUCCESS;
+	int res = _SUCCESS;
 
-	if (pattrib->psta) {
+	if (pattrib->psta)
 		psta = pattrib->psta;
-	} else {
+	else {
 		DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__);
 		psta = rtw_get_stainfo23a(&padapter->stapriv, pattrib->ra);
 	}
 
-	if (psta == NULL) {
+	if (!psta) {
 		DBG_8723A("%s, psta == NUL\n", __func__);
 		return _FAIL;
 	}
 
 	if (!(psta->state &_FW_LINKED)) {
-		DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+		DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n",
+			  __func__, psta->state);
 		return _FAIL;
 	}
 
-	if (pxmitframe->buf_addr == NULL) {
+	if (!pxmitframe->buf_addr) {
 		DBG_8723A("==> %s buf_addr == NULL\n", __func__);
 		return _FAIL;
 	}
@@ -1157,18 +1120,17 @@ s32 rtw_xmitframe_coalesce23a(struct rtw_adapter *padapter, struct sk_buff *pkt,
 
 	hw_hdr_offset = TXDESC_OFFSET;
 
-	mem_start = pbuf_start +	hw_hdr_offset;
+	mem_start = pbuf_start + hw_hdr_offset;
 
-	if (rtw_make_wlanhdr23a(padapter, mem_start, pattrib) == _FAIL) {
+	if (rtw_make_wlanhdr(padapter, mem_start, pattrib) == _FAIL) {
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
-			 ("rtw_xmitframe_coalesce23a: rtw_make_wlanhdr23a "
-			  "fail; drop pkt\n"));
+			 ("%s: rtw_make_wlanhdr fail; drop pkt\n", __func__));
 		res = _FAIL;
 		goto exit;
 	}
 
-	_rtw_open_pktfile23a(pkt, &pktfile);
-	_rtw_pktfile_read23a(&pktfile, NULL, pattrib->pkt_hdrlen);
+	pdata += pattrib->pkt_hdrlen;
+	data_len -= pattrib->pkt_hdrlen;
 
 	frg_inx = 0;
 	frg_len = pxmitpriv->frag_len - 4;/* 2346-4 = 2342 */
@@ -1179,31 +1141,37 @@ s32 rtw_xmitframe_coalesce23a(struct rtw_adapter *padapter, struct sk_buff *pkt,
 		mpdu_len = frg_len;
 
 		pframe = mem_start;
-
-		SetMFrag(mem_start);
+		hdr = (struct ieee80211_hdr *)mem_start;
 
 		pframe += pattrib->hdrlen;
 		mpdu_len -= pattrib->hdrlen;
 
 		/* adding icv, if necessary... */
 		if (pattrib->iv_len) {
-			if (psta != NULL) {
+			if (psta) {
 				switch (pattrib->encrypt) {
-				case _WEP40_:
-				case _WEP104_:
-					WEP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+				case WLAN_CIPHER_SUITE_WEP40:
+				case WLAN_CIPHER_SUITE_WEP104:
+					WEP_IV(pattrib->iv, psta->dot11txpn,
+					       pattrib->key_idx);
 					break;
-				case _TKIP_:
+				case WLAN_CIPHER_SUITE_TKIP:
 					if (bmcst)
-						TKIP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+						TKIP_IV(pattrib->iv,
+							psta->dot11txpn,
+							pattrib->key_idx);
 					else
-						TKIP_IV(pattrib->iv, psta->dot11txpn, 0);
+						TKIP_IV(pattrib->iv,
+							psta->dot11txpn, 0);
 					break;
-				case _AES_:
+				case WLAN_CIPHER_SUITE_CCMP:
 					if (bmcst)
-						AES_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+						AES_IV(pattrib->iv,
+						       psta->dot11txpn,
+						       pattrib->key_idx);
 					else
-						AES_IV(pattrib->iv, psta->dot11txpn, 0);
+						AES_IV(pattrib->iv,
+						       psta->dot11txpn, 0);
 					break;
 				}
 			}
@@ -1225,16 +1193,20 @@ s32 rtw_xmitframe_coalesce23a(struct rtw_adapter *padapter, struct sk_buff *pkt,
 			mpdu_len -= llc_sz;
 		}
 
-		if ((pattrib->icv_len >0) && (pattrib->bswenc))
+		if (pattrib->icv_len > 0 && pattrib->bswenc)
 			mpdu_len -= pattrib->icv_len;
 
-		if (bmcst) {
+		if (bmcst)
 			/*  don't do fragment to broadcat/multicast packets */
-			mem_sz = _rtw_pktfile_read23a(&pktfile, pframe, pattrib->pktlen);
-		} else {
-			mem_sz = _rtw_pktfile_read23a(&pktfile, pframe, mpdu_len);
-		}
+			mem_sz = min_t(s32, data_len, pattrib->pktlen);
+		else
+			mem_sz = min_t(s32, data_len, mpdu_len);
+
+		memcpy(pframe, pdata, mem_sz);
+
 		pframe += mem_sz;
+		pdata += mem_sz;
+		data_len -= mem_sz;
 
 		if ((pattrib->icv_len >0) && (pattrib->bswenc)) {
 			memcpy(pframe, pattrib->icv, pattrib->icv_len);
@@ -1243,7 +1215,7 @@ s32 rtw_xmitframe_coalesce23a(struct rtw_adapter *padapter, struct sk_buff *pkt,
 
 		frg_inx++;
 
-		if (bmcst || (rtw_endofpktfile23a(&pktfile))) {
+		if (bmcst || data_len <= 0) {
 			pattrib->nr_frags = frg_inx;
 
 			pattrib->last_txcmdsz = pattrib->hdrlen +
@@ -1252,21 +1224,24 @@ s32 rtw_xmitframe_coalesce23a(struct rtw_adapter *padapter, struct sk_buff *pkt,
 						llc_sz : 0) +
 						((pattrib->bswenc) ?
 						pattrib->icv_len : 0) + mem_sz;
-
-			ClearMFrag(mem_start);
+			hdr->frame_control &=
+				~cpu_to_le16(IEEE80211_FCTL_MOREFRAGS);
 
 			break;
 		} else {
-			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("%s: There're still something in packet!\n", __func__));
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
+				 ("%s: There're still something in packet!\n",
+				  __func__));
 		}
+		hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_MOREFRAGS);
 
 		mem_start = PTR_ALIGN(pframe, 4) + hw_hdr_offset;
 		memcpy(mem_start, pbuf_start + hw_hdr_offset, pattrib->hdrlen);
-
 	}
 
 	if (xmitframe_addmic(padapter, pxmitframe) == _FAIL) {
-		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic(padapter, pxmitframe) == _FAIL\n"));
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
+			 ("xmitframe_addmic(padapter, pxmitframe) == _FAIL\n"));
 		DBG_8723A("xmitframe_addmic(padapter, pxmitframe) == _FAIL\n");
 		res = _FAIL;
 		goto exit;
@@ -1313,11 +1288,10 @@ s32 rtw_put_snap23a(u8 *data, u16 h_proto)
 
 void rtw_update_protection23a(struct rtw_adapter *padapter, u8 *ie, uint ie_len)
 {
-	struct	xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct	registry_priv *pregistrypriv = &padapter->registrypriv;
-	uint	protection;
-	u8	*perp;
-	int	 erp_len;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	uint protection;
+	const u8 *p;
 
 	switch (pxmitpriv->vcs_setting) {
 	case DISABLE_VCS:
@@ -1327,11 +1301,11 @@ void rtw_update_protection23a(struct rtw_adapter *padapter, u8 *ie, uint ie_len)
 		break;
 	case AUTO_VCS:
 	default:
-		perp = rtw_get_ie23a(ie, _ERPINFO_IE_, &erp_len, ie_len);
-		if (perp == NULL) {
+		p = cfg80211_find_ie(WLAN_EID_ERP_INFO, ie, ie_len);
+		if (!p)
 			pxmitpriv->vcs = NONE_VCS;
-		} else {
-			protection = (*(perp + 2)) & BIT(1);
+		else {
+			protection = (*(p + 2)) & BIT(1);
 			if (protection) {
 				if (pregistrypriv->vcs_type == RTS_CTS)
 					pxmitpriv->vcs = RTS_CTS;
@@ -1396,7 +1370,8 @@ struct xmit_buf *rtw_alloc_xmitbuf23a_ext(struct xmit_priv *pxmitpriv)
 	return pxmitbuf;
 }
 
-s32 rtw_free_xmitbuf_ext23a(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
+int rtw_free_xmitbuf_ext23a(struct xmit_priv *pxmitpriv,
+			    struct xmit_buf *pxmitbuf)
 {
 	unsigned long irqL;
 	struct rtw_queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue;
@@ -1450,7 +1425,7 @@ struct xmit_buf *rtw_alloc_xmitbuf23a(struct xmit_priv *pxmitpriv)
 	return pxmitbuf;
 }
 
-s32 rtw_free_xmitbuf23a(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
+int rtw_free_xmitbuf23a(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
 {
 	unsigned long irqL;
 	struct rtw_queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
@@ -1512,22 +1487,18 @@ Otherwise, we must use _enter/_exit critical to protect free_xmit_queue...
 Must be very very cautious...
 
 */
-struct xmit_frame *rtw_alloc_xmitframe23a(struct xmit_priv *pxmitpriv)/* _queue *pfree_xmit_queue) */
+static struct xmit_frame *rtw_alloc_xmitframe(struct xmit_priv *pxmitpriv)
 {
-	/*
-		Please remember to use all the osdep_service api,
-		and lock/unlock or _enter/_exit critical to protect
-		pfree_xmit_queue
-	*/
-
 	struct xmit_frame *pxframe = NULL;
 	struct list_head *plist, *phead;
 	struct rtw_queue *pfree_xmit_queue = &pxmitpriv->free_xmit_queue;
 
 	spin_lock_bh(&pfree_xmit_queue->lock);
 
-	if (_rtw_queue_empty23a(pfree_xmit_queue) == true) {
-		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe23a:%d\n", pxmitpriv->free_xmitframe_cnt));
+	if (list_empty(&pfree_xmit_queue->queue)) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
+			 ("rtw_alloc_xmitframe:%d\n",
+			  pxmitpriv->free_xmitframe_cnt));
 		pxframe =  NULL;
 	} else {
 		phead = get_list_head(pfree_xmit_queue);
@@ -1538,7 +1509,9 @@ struct xmit_frame *rtw_alloc_xmitframe23a(struct xmit_priv *pxmitpriv)/* _queue
 
 		list_del_init(&pxframe->list);
 		pxmitpriv->free_xmitframe_cnt--;
-		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe23a():free_xmitframe_cnt =%d\n", pxmitpriv->free_xmitframe_cnt));
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
+			 ("rtw_alloc_xmitframe():free_xmitframe_cnt =%d\n",
+			  pxmitpriv->free_xmitframe_cnt));
 	}
 
 	spin_unlock_bh(&pfree_xmit_queue->lock);
@@ -1556,7 +1529,7 @@ struct xmit_frame *rtw_alloc_xmitframe23a_ext(struct xmit_priv *pxmitpriv)
 
 	spin_lock_bh(&queue->lock);
 
-	if (_rtw_queue_empty23a(queue) == true) {
+	if (list_empty(&queue->queue)) {
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe23a_ext:%d\n", pxmitpriv->free_xframe_ext_cnt));
 		pxframe =  NULL;
 	} else {
@@ -1642,7 +1615,7 @@ void rtw_free_xmitframe_queue23a(struct xmit_priv *pxmitpriv,
 
 }
 
-s32 rtw_xmitframe_enqueue23a(struct rtw_adapter *padapter,
+int rtw_xmitframe_enqueue23a(struct rtw_adapter *padapter,
 			     struct xmit_frame *pxmitframe)
 {
 	if (rtw_xmit23a_classifier(padapter, pxmitframe) == _FAIL) {
@@ -1715,7 +1688,9 @@ rtw_dequeue_xframe23a(struct xmit_priv *pxmitpriv, struct hw_xmit *phwxmit_i,
 				phwxmit->accnt--;
 
 				/* Remove sta node when there is no pending packets. */
-				if (_rtw_queue_empty23a(pframe_queue)) /* must be done after get_next and before break */
+				/* must be done after get_next and
+				   before break */
+				if (list_empty(&pframe_queue->queue))
 					list_del_init(&ptxservq->tx_pending);
 				goto exit;
 			}
@@ -1764,8 +1739,8 @@ struct tx_servq *rtw_get_sta_pending23a(struct rtw_adapter *padapter, struct sta
  * Will enqueue pxmitframe to the proper queue,
  * and indicate it to xx_pending list.....
  */
-s32 rtw_xmit23a_classifier(struct rtw_adapter *padapter,
-			struct xmit_frame *pxmitframe)
+int rtw_xmit23a_classifier(struct rtw_adapter *padapter,
+			   struct xmit_frame *pxmitframe)
 {
 	struct sta_info	*psta;
 	struct tx_servq	*ptxservq;
@@ -1942,9 +1917,9 @@ int rtw_xmit23a(struct rtw_adapter *padapter, struct sk_buff *skb)
 {
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct xmit_frame *pxmitframe = NULL;
-	s32 res;
+	int res;
 
-	pxmitframe = rtw_alloc_xmitframe23a(pxmitpriv);
+	pxmitframe = rtw_alloc_xmitframe(pxmitpriv);
 
 	if (pxmitframe == NULL) {
 		RT_TRACE(_module_xmit_osdep_c_, _drv_err_,
@@ -1974,7 +1949,7 @@ int rtw_xmit23a(struct rtw_adapter *padapter, struct sk_buff *skb)
 	spin_unlock_bh(&pxmitpriv->lock);
 #endif
 
-	if (rtw_hal_xmit23a(padapter, pxmitframe) == false)
+	if (rtl8723au_hal_xmit(padapter, pxmitframe) == false)
 		return 1;
 
 	return 0;
@@ -1991,8 +1966,8 @@ int xmitframe_enqueue_for_sleeping_sta23a(struct rtw_adapter *padapter, struct x
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	int bmcst = is_multicast_ether_addr(pattrib->ra);
 
-	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == false)
-	    return ret;
+	if (!check_fwstate(pmlmepriv, WIFI_AP_STATE))
+		return ret;
 
 	if (pattrib->psta) {
 		psta = pattrib->psta;
@@ -2036,7 +2011,8 @@ int xmitframe_enqueue_for_sleeping_sta23a(struct rtw_adapter *padapter, struct x
 
 			/* DBG_8723A("enqueue, sq_len =%d, tim =%x\n", psta->sleepq_len, pstapriv->tim_bitmap); */
 
-			update_beacon23a(padapter, _TIM_IE_, NULL, false);/* tx bc/mc packets after upate bcn */
+			/* tx bc/mc packets after upate bcn */
+			update_beacon23a(padapter, WLAN_EID_TIM, NULL, false);
 
 			/* spin_unlock_bh(&psta->sleep_q.lock); */
 
@@ -2093,7 +2069,8 @@ int xmitframe_enqueue_for_sleeping_sta23a(struct rtw_adapter *padapter, struct x
 
 				if (psta->sleepq_len == 1) {
 					/* upate BCN for TIM IE */
-					update_beacon23a(padapter, _TIM_IE_, NULL, false);
+					update_beacon23a(padapter, WLAN_EID_TIM,
+							 NULL, false);
 				}
 			}
 
@@ -2244,7 +2221,7 @@ void wakeup_sta_to_xmit23a(struct rtw_adapter *padapter, struct sta_info *psta)
 		}
 
 		pxmitframe->attrib.triggered = 1;
-		rtw_hal_xmit23aframe_enqueue(padapter, pxmitframe);
+		rtl8723au_hal_xmitframe_enqueue(padapter, pxmitframe);
 	}
 
 	if (psta->sleepq_len == 0) {
@@ -2291,7 +2268,7 @@ void wakeup_sta_to_xmit23a(struct rtw_adapter *padapter, struct sta_info *psta)
 				pxmitframe->attrib.mdata = 0;
 
 			pxmitframe->attrib.triggered = 1;
-			rtw_hal_xmit23aframe_enqueue(padapter, pxmitframe);
+			rtl8723au_hal_xmitframe_enqueue(padapter, pxmitframe);
 		}
 		if (psta_bmc->sleepq_len == 0) {
 			pstapriv->tim_bitmap &= ~BIT(0);
@@ -2307,7 +2284,7 @@ void wakeup_sta_to_xmit23a(struct rtw_adapter *padapter, struct sta_info *psta)
 	}
 
 	if (update_mask)
-		update_beacon23a(padapter, _TIM_IE_, NULL, false);
+		update_beacon23a(padapter, WLAN_EID_TIM, NULL, false);
 }
 
 void xmit_delivery_enabled_frames23a(struct rtw_adapter *padapter,
@@ -2365,14 +2342,14 @@ void xmit_delivery_enabled_frames23a(struct rtw_adapter *padapter,
 
 		pxmitframe->attrib.triggered = 1;
 
-		rtw_hal_xmit23aframe_enqueue(padapter, pxmitframe);
+		rtl8723au_hal_xmitframe_enqueue(padapter, pxmitframe);
 
 		if ((psta->sleepq_ac_len == 0) && (!psta->has_legacy_ac) &&
 		    (wmmps_ac)) {
 			pstapriv->tim_bitmap &= ~CHKBIT(psta->aid);
 
 			/* upate BCN for TIM IE */
-			update_beacon23a(padapter, _TIM_IE_, NULL, false);
+			update_beacon23a(padapter, WLAN_EID_TIM, NULL, false);
 		}
 	}
 	spin_unlock_bh(&pxmitpriv->lock);
diff --git a/drivers/staging/rtl8723au/hal/HalDMOutSrc8723A_CE.c b/drivers/staging/rtl8723au/hal/HalDMOutSrc8723A_CE.c
index 9796f2e5c68..4b41bc4ce1e 100644
--- a/drivers/staging/rtl8723au/hal/HalDMOutSrc8723A_CE.c
+++ b/drivers/staging/rtl8723au/hal/HalDMOutSrc8723A_CE.c
@@ -18,6 +18,7 @@
 /*  include files */
 
 #include "odm_precomp.h"
+#include <usb_ops_linux.h>
 
 #define		DPK_DELTA_MAPPING_NUM	13
 #define		index_mapping_HP_NUM	15
@@ -252,35 +253,41 @@ odm_TXPowerTrackingCallback_ThermalMeter_92C(
 				PHY_SetBBReg(Adapter, rOFDM0_XCTxAFE, bMaskH4Bits, value32);
 
 				value32 = ((X * ele_D)>>7)&0x01;
-				PHY_SetBBReg(Adapter, rOFDM0_ECCAThreshold, BIT31, value32);
+				PHY_SetBBReg(Adapter, rOFDM0_ECCAThreshold,
+					     BIT(31), value32);
 
 				value32 = ((Y * ele_D)>>7)&0x01;
-				PHY_SetBBReg(Adapter, rOFDM0_ECCAThreshold, BIT29, value32);
+				PHY_SetBBReg(Adapter, rOFDM0_ECCAThreshold,
+					     BIT(29), value32);
 			} else {
-				PHY_SetBBReg(Adapter, rOFDM0_XATxIQImbalance, bMaskDWord, OFDMSwingTable23A[OFDM_index[0]]);
-				PHY_SetBBReg(Adapter, rOFDM0_XCTxAFE, bMaskH4Bits, 0x00);
-				PHY_SetBBReg(Adapter, rOFDM0_ECCAThreshold, BIT31|BIT29, 0x00);
+				PHY_SetBBReg(Adapter, rOFDM0_XATxIQImbalance,
+					     bMaskDWord,
+					     OFDMSwingTable23A[OFDM_index[0]]);
+				PHY_SetBBReg(Adapter, rOFDM0_XCTxAFE,
+					     bMaskH4Bits, 0x00);
+				PHY_SetBBReg(Adapter, rOFDM0_ECCAThreshold,
+					     BIT(31) | BIT(29), 0x00);
 			}
 
 			/* Adjust CCK according to IQK result */
 			if (!pdmpriv->bCCKinCH14) {
-				rtw_write8(Adapter, 0xa22, CCKSwingTable_Ch1_Ch1323A[CCK_index][0]);
-				rtw_write8(Adapter, 0xa23, CCKSwingTable_Ch1_Ch1323A[CCK_index][1]);
-				rtw_write8(Adapter, 0xa24, CCKSwingTable_Ch1_Ch1323A[CCK_index][2]);
-				rtw_write8(Adapter, 0xa25, CCKSwingTable_Ch1_Ch1323A[CCK_index][3]);
-				rtw_write8(Adapter, 0xa26, CCKSwingTable_Ch1_Ch1323A[CCK_index][4]);
-				rtw_write8(Adapter, 0xa27, CCKSwingTable_Ch1_Ch1323A[CCK_index][5]);
-				rtw_write8(Adapter, 0xa28, CCKSwingTable_Ch1_Ch1323A[CCK_index][6]);
-				rtw_write8(Adapter, 0xa29, CCKSwingTable_Ch1_Ch1323A[CCK_index][7]);
+				rtl8723au_write8(Adapter, 0xa22, CCKSwingTable_Ch1_Ch1323A[CCK_index][0]);
+				rtl8723au_write8(Adapter, 0xa23, CCKSwingTable_Ch1_Ch1323A[CCK_index][1]);
+				rtl8723au_write8(Adapter, 0xa24, CCKSwingTable_Ch1_Ch1323A[CCK_index][2]);
+				rtl8723au_write8(Adapter, 0xa25, CCKSwingTable_Ch1_Ch1323A[CCK_index][3]);
+				rtl8723au_write8(Adapter, 0xa26, CCKSwingTable_Ch1_Ch1323A[CCK_index][4]);
+				rtl8723au_write8(Adapter, 0xa27, CCKSwingTable_Ch1_Ch1323A[CCK_index][5]);
+				rtl8723au_write8(Adapter, 0xa28, CCKSwingTable_Ch1_Ch1323A[CCK_index][6]);
+				rtl8723au_write8(Adapter, 0xa29, CCKSwingTable_Ch1_Ch1323A[CCK_index][7]);
 			} else {
-				rtw_write8(Adapter, 0xa22, CCKSwingTable_Ch1423A[CCK_index][0]);
-				rtw_write8(Adapter, 0xa23, CCKSwingTable_Ch1423A[CCK_index][1]);
-				rtw_write8(Adapter, 0xa24, CCKSwingTable_Ch1423A[CCK_index][2]);
-				rtw_write8(Adapter, 0xa25, CCKSwingTable_Ch1423A[CCK_index][3]);
-				rtw_write8(Adapter, 0xa26, CCKSwingTable_Ch1423A[CCK_index][4]);
-				rtw_write8(Adapter, 0xa27, CCKSwingTable_Ch1423A[CCK_index][5]);
-				rtw_write8(Adapter, 0xa28, CCKSwingTable_Ch1423A[CCK_index][6]);
-				rtw_write8(Adapter, 0xa29, CCKSwingTable_Ch1423A[CCK_index][7]);
+				rtl8723au_write8(Adapter, 0xa22, CCKSwingTable_Ch1423A[CCK_index][0]);
+				rtl8723au_write8(Adapter, 0xa23, CCKSwingTable_Ch1423A[CCK_index][1]);
+				rtl8723au_write8(Adapter, 0xa24, CCKSwingTable_Ch1423A[CCK_index][2]);
+				rtl8723au_write8(Adapter, 0xa25, CCKSwingTable_Ch1423A[CCK_index][3]);
+				rtl8723au_write8(Adapter, 0xa26, CCKSwingTable_Ch1423A[CCK_index][4]);
+				rtl8723au_write8(Adapter, 0xa27, CCKSwingTable_Ch1423A[CCK_index][5]);
+				rtl8723au_write8(Adapter, 0xa28, CCKSwingTable_Ch1423A[CCK_index][6]);
+				rtl8723au_write8(Adapter, 0xa29, CCKSwingTable_Ch1423A[CCK_index][7]);
 			}
 
 			if (is2T) {
@@ -308,14 +315,25 @@ odm_TXPowerTrackingCallback_ThermalMeter_92C(
 					PHY_SetBBReg(Adapter, rOFDM0_XDTxAFE, bMaskH4Bits, value32);
 
 					value32 = ((X * ele_D)>>7)&0x01;
-					PHY_SetBBReg(Adapter, rOFDM0_ECCAThreshold, BIT27, value32);
+					PHY_SetBBReg(Adapter,
+						     rOFDM0_ECCAThreshold,
+						     BIT(27), value32);
 
 					value32 = ((Y * ele_D)>>7)&0x01;
-					PHY_SetBBReg(Adapter, rOFDM0_ECCAThreshold, BIT25, value32);
+					PHY_SetBBReg(Adapter,
+						     rOFDM0_ECCAThreshold,
+						     BIT(25), value32);
 				} else {
-					PHY_SetBBReg(Adapter, rOFDM0_XBTxIQImbalance, bMaskDWord, OFDMSwingTable23A[OFDM_index[1]]);
-					PHY_SetBBReg(Adapter, rOFDM0_XDTxAFE, bMaskH4Bits, 0x00);
-					PHY_SetBBReg(Adapter, rOFDM0_ECCAThreshold, BIT27|BIT25, 0x00);
+					PHY_SetBBReg(Adapter,
+						     rOFDM0_XBTxIQImbalance,
+						     bMaskDWord,
+						     OFDMSwingTable23A[OFDM_index[1]]);
+					PHY_SetBBReg(Adapter,
+						     rOFDM0_XDTxAFE,
+						     bMaskH4Bits, 0x00);
+					PHY_SetBBReg(Adapter,
+						     rOFDM0_ECCAThreshold,
+						     BIT(27) | BIT(25), 0x00);
 				}
 			}
 
@@ -410,14 +428,14 @@ static u8 _PHY_PathA_IQK(struct rtw_adapter *pAdapter, bool configPathB)
 	regE9C = PHY_QueryBBReg(pAdapter, rTx_Power_After_IQK_A, bMaskDWord);
 	regEA4 = PHY_QueryBBReg(pAdapter, rRx_Power_Before_IQK_A_2, bMaskDWord);
 
-	if (!(regEAC & BIT28) &&
+	if (!(regEAC & BIT(28)) &&
 	    (((regE94 & 0x03FF0000)>>16) != 0x142) &&
 	    (((regE9C & 0x03FF0000)>>16) != 0x42))
 		result |= 0x01;
 	else			/* if Tx not OK, ignore Rx */
 		return result;
 
-	if (!(regEAC & BIT27) &&		/* if Tx is OK, check whether Rx is OK */
+	if (!(regEAC & BIT(27)) &&		/* if Tx is OK, check whether Rx is OK */
 	    (((regEA4 & 0x03FF0000)>>16) != 0x132) &&
 	    (((regEAC & 0x03FF0000)>>16) != 0x36))
 		result |= 0x02;
@@ -445,14 +463,14 @@ static u8 _PHY_PathB_IQK(struct rtw_adapter *pAdapter)
 	regEC4 = PHY_QueryBBReg(pAdapter, rRx_Power_Before_IQK_B_2, bMaskDWord);
 	regECC = PHY_QueryBBReg(pAdapter, rRx_Power_After_IQK_B_2, bMaskDWord);
 
-	if (!(regEAC & BIT31) &&
+	if (!(regEAC & BIT(31)) &&
 	    (((regEB4 & 0x03FF0000)>>16) != 0x142) &&
 	    (((regEBC & 0x03FF0000)>>16) != 0x42))
 		result |= 0x01;
 	else
 		return result;
 
-	if (!(regEAC & BIT30) &&
+	if (!(regEAC & BIT(30)) &&
 	    (((regEC4 & 0x03FF0000)>>16) != 0x132) &&
 	    (((regECC & 0x03FF0000)>>16) != 0x36))
 		result |= 0x02;
@@ -564,9 +582,9 @@ static void _PHY_SaveMACRegisters(struct rtw_adapter *pAdapter, u32 *MACReg, u32
 	u32 i;
 
 	for (i = 0 ; i < (IQK_MAC_REG_NUM - 1); i++) {
-		MACBackup[i] = rtw_read8(pAdapter, MACReg[i]);
+		MACBackup[i] = rtl8723au_read8(pAdapter, MACReg[i]);
 	}
-	MACBackup[i] = rtw_read32(pAdapter, MACReg[i]);
+	MACBackup[i] = rtl8723au_read32(pAdapter, MACReg[i]);
 }
 
 static void _PHY_ReloadADDARegisters(struct rtw_adapter *pAdapter, u32 *ADDAReg, u32 *ADDABackup, u32 RegiesterNum)
@@ -582,10 +600,10 @@ static void _PHY_ReloadMACRegisters(struct rtw_adapter *pAdapter, u32 *MACReg, u
 {
 	u32 i;
 
-	for (i = 0 ; i < (IQK_MAC_REG_NUM - 1); i++) {
-		rtw_write8(pAdapter, MACReg[i], (u8)MACBackup[i]);
-	}
-	rtw_write32(pAdapter, MACReg[i], MACBackup[i]);
+	for (i = 0 ; i < (IQK_MAC_REG_NUM - 1); i++)
+		rtl8723au_write8(pAdapter, MACReg[i], (u8)MACBackup[i]);
+
+	rtl8723au_write32(pAdapter, MACReg[i], MACBackup[i]);
 }
 
 static void _PHY_PathADDAOn(struct rtw_adapter *pAdapter, u32 *ADDAReg, bool isPathAOn, bool is2T)
@@ -609,12 +627,13 @@ static void _PHY_MACSettingCalibration(struct rtw_adapter *pAdapter, u32 *MACReg
 {
 	u32 i = 0;
 
-	rtw_write8(pAdapter, MACReg[i], 0x3F);
+	rtl8723au_write8(pAdapter, MACReg[i], 0x3F);
 
 	for (i = 1 ; i < (IQK_MAC_REG_NUM - 1); i++) {
-		rtw_write8(pAdapter, MACReg[i], (u8)(MACBackup[i]&(~BIT3)));
+		rtl8723au_write8(pAdapter, MACReg[i],
+				 (u8)(MACBackup[i] & ~BIT(3)));
 	}
-	rtw_write8(pAdapter, MACReg[i], (u8)(MACBackup[i]&(~BIT5)));
+	rtl8723au_write8(pAdapter, MACReg[i], (u8)(MACBackup[i] & ~BIT(5)));
 }
 
 static void _PHY_PathAStandBy(struct rtw_adapter *pAdapter)
@@ -737,21 +756,23 @@ static void _PHY_IQCalibrate(struct rtw_adapter *pAdapter, int result[][8], u8 t
 	_PHY_PathADDAOn(pAdapter, ADDA_REG, true, is2T);
 
 	if (t == 0)
-		pdmpriv->bRfPiEnable = (u8)PHY_QueryBBReg(pAdapter, rFPGA0_XA_HSSIParameter1, BIT(8));
+		pdmpriv->bRfPiEnable = (u8)
+			PHY_QueryBBReg(pAdapter, rFPGA0_XA_HSSIParameter1,
+				       BIT(8));
 
 	if (!pdmpriv->bRfPiEnable) {
 		/*  Switch BB to PI mode to do IQ Calibration. */
 		_PHY_PIModeSwitch(pAdapter, true);
 	}
 
-	PHY_SetBBReg(pAdapter, rFPGA0_RFMOD, BIT24, 0x00);
+	PHY_SetBBReg(pAdapter, rFPGA0_RFMOD, BIT(24), 0x00);
 	PHY_SetBBReg(pAdapter, rOFDM0_TRxPathEnable, bMaskDWord, 0x03a05600);
 	PHY_SetBBReg(pAdapter, rOFDM0_TRMuxPar, bMaskDWord, 0x000800e4);
 	PHY_SetBBReg(pAdapter, rFPGA0_XCD_RFInterfaceSW, bMaskDWord, 0x22204000);
-	PHY_SetBBReg(pAdapter, rFPGA0_XAB_RFInterfaceSW, BIT10, 0x01);
-	PHY_SetBBReg(pAdapter, rFPGA0_XAB_RFInterfaceSW, BIT26, 0x01);
-	PHY_SetBBReg(pAdapter, rFPGA0_XA_RFInterfaceOE, BIT10, 0x00);
-	PHY_SetBBReg(pAdapter, rFPGA0_XB_RFInterfaceOE, BIT10, 0x00);
+	PHY_SetBBReg(pAdapter, rFPGA0_XAB_RFInterfaceSW, BIT(10), 0x01);
+	PHY_SetBBReg(pAdapter, rFPGA0_XAB_RFInterfaceSW, BIT(26), 0x01);
+	PHY_SetBBReg(pAdapter, rFPGA0_XA_RFInterfaceOE, BIT(10), 0x00);
+	PHY_SetBBReg(pAdapter, rFPGA0_XB_RFInterfaceOE, BIT(10), 0x00);
 
 	if (is2T) {
 		PHY_SetBBReg(pAdapter, rFPGA0_XA_LSSIParameter, bMaskDWord, 0x00010000);
@@ -859,12 +880,17 @@ static void _PHY_LCCalibrate(struct rtw_adapter *pAdapter, bool is2T)
 	u32 RF_Amode = 0, RF_Bmode = 0, LC_Cal;
 
 	/* Check continuous TX and Packet TX */
-	tmpReg = rtw_read8(pAdapter, 0xd03);
+	tmpReg = rtl8723au_read8(pAdapter, 0xd03);
 
-	if ((tmpReg&0x70) != 0)			/* Deal with contisuous TX case */
-		rtw_write8(pAdapter, 0xd03, tmpReg&0x8F);	/* disable all continuous TX */
-	else							/*  Deal with Packet TX case */
-		rtw_write8(pAdapter, REG_TXPAUSE, 0xFF);			/*  block all queues */
+	if ((tmpReg&0x70) != 0) {
+		/* Deal with contisuous TX case */
+		/* disable all continuous TX */
+		rtl8723au_write8(pAdapter, 0xd03, tmpReg&0x8F);
+	} else {
+		/*  Deal with Packet TX case */
+		/*  block all queues */
+		rtl8723au_write8(pAdapter, REG_TXPAUSE, 0xFF);
+	}
 
 	if ((tmpReg&0x70) != 0) {
 		/* 1. Read original RF mode */
@@ -895,15 +921,14 @@ static void _PHY_LCCalibrate(struct rtw_adapter *pAdapter, bool is2T)
 	/* Restore original situation */
 	if ((tmpReg&0x70) != 0) {	/* Deal with contuous TX case  */
 		/* Path-A */
-		rtw_write8(pAdapter, 0xd03, tmpReg);
+		rtl8723au_write8(pAdapter, 0xd03, tmpReg);
 		PHY_SetRFReg(pAdapter, RF_PATH_A, RF_AC, bMask12Bits, RF_Amode);
 
 		/* Path-B */
 		if (is2T)
 			PHY_SetRFReg(pAdapter, RF_PATH_B, RF_AC, bMask12Bits, RF_Bmode);
-	} else { /*  Deal with Packet TX case */
-		rtw_write8(pAdapter, REG_TXPAUSE, 0x00);
-	}
+	} else /*  Deal with Packet TX case */
+		rtl8723au_write8(pAdapter, REG_TXPAUSE, 0x00);
 }
 
 /* Analog Pre-distortion calibration */
diff --git a/drivers/staging/rtl8723au/hal/HalPwrSeqCmd.c b/drivers/staging/rtl8723au/hal/HalPwrSeqCmd.c
index 4f6b4b72f92..1da4eece6f9 100644
--- a/drivers/staging/rtl8723au/hal/HalPwrSeqCmd.c
+++ b/drivers/staging/rtl8723au/hal/HalPwrSeqCmd.c
@@ -30,6 +30,7 @@ Major Change History:
 
 --*/
 #include <HalPwrSeqCmd.h>
+#include <usb_ops_linux.h>
 
 /*  */
 /*	Description: */
@@ -89,14 +90,14 @@ u8 HalPwrSeqCmdParsing23a(struct rtw_adapter *padapter, u8 CutVersion,
 				offset = GET_PWR_CFG_OFFSET(PwrCfgCmd);
 
 				/*  Read the value from system register */
-				value = rtw_read8(padapter, offset);
+				value = rtl8723au_read8(padapter, offset);
 
 				value &= ~(GET_PWR_CFG_MASK(PwrCfgCmd));
 				value |= (GET_PWR_CFG_VALUE(PwrCfgCmd) &
 					  GET_PWR_CFG_MASK(PwrCfgCmd));
 
 				/*  Write the value back to sytem register */
-				rtw_write8(padapter, offset, value);
+				rtl8723au_write8(padapter, offset, value);
 				break;
 
 			case PWR_CMD_POLLING:
@@ -107,7 +108,8 @@ u8 HalPwrSeqCmdParsing23a(struct rtw_adapter *padapter, u8 CutVersion,
 				bPollingBit = false;
 				offset = GET_PWR_CFG_OFFSET(PwrCfgCmd);
 				do {
-					value = rtw_read8(padapter, offset);
+					value = rtl8723au_read8(padapter,
+								offset);
 
 					value &= GET_PWR_CFG_MASK(PwrCfgCmd);
 					if (value ==
diff --git a/drivers/staging/rtl8723au/hal/hal_com.c b/drivers/staging/rtl8723au/hal/hal_com.c
index 0640f352213..9fba0494515 100644
--- a/drivers/staging/rtl8723au/hal/hal_com.c
+++ b/drivers/staging/rtl8723au/hal/hal_com.c
@@ -18,6 +18,7 @@
 #include <hal_intf.h>
 #include <hal_com.h>
 #include <rtl8723a_hal.h>
+#include <usb_ops_linux.h>
 
 #define _HAL_INIT_C_
 
@@ -222,10 +223,10 @@ void HalSetBrateCfg23a(struct rtw_adapter *padapter, u8 *mBratesOS)
 	DBG_8723A("HW_VAR_BASIC_RATE: BrateCfg(%#x)\n", brate_cfg);
 
 	/*  Set RRSR rate table. */
-	rtw_write8(padapter, REG_RRSR, brate_cfg & 0xff);
-	rtw_write8(padapter, REG_RRSR + 1, (brate_cfg >> 8) & 0xff);
-	rtw_write8(padapter, REG_RRSR + 2,
-		   rtw_read8(padapter, REG_RRSR + 2) & 0xf0);
+	rtl8723au_write8(padapter, REG_RRSR, brate_cfg & 0xff);
+	rtl8723au_write8(padapter, REG_RRSR + 1, (brate_cfg >> 8) & 0xff);
+	rtl8723au_write8(padapter, REG_RRSR + 2,
+			 rtl8723au_read8(padapter, REG_RRSR + 2) & 0xf0);
 
 	rate_index = 0;
 	/*  Set RTS initial rate */
@@ -234,7 +235,7 @@ void HalSetBrateCfg23a(struct rtw_adapter *padapter, u8 *mBratesOS)
 		rate_index++;
 	}
 		/*  Ziv - Check */
-	rtw_write8(padapter, REG_INIRTS_RATE_SEL, rate_index);
+	rtl8723au_write8(padapter, REG_INIRTS_RATE_SEL, rate_index);
 
 	return;
 }
@@ -344,12 +345,6 @@ bool Hal_MappingOutPipe23a(struct rtw_adapter *pAdapter, u8 NumOutPipe)
 	return result;
 }
 
-void hal_init_macaddr23a(struct rtw_adapter *adapter)
-{
-	rtw_hal_set_hwreg23a(adapter, HW_VAR_MAC_ADDR,
-			  adapter->eeprompriv.mac_addr);
-}
-
 /*
 * C2H event format:
 * Field	 TRIGGER		CONTENT	   CMD_SEQ	CMD_LEN		 CMD_ID
@@ -358,12 +353,12 @@ void hal_init_macaddr23a(struct rtw_adapter *adapter)
 
 void c2h_evt_clear23a(struct rtw_adapter *adapter)
 {
-	rtw_write8(adapter, REG_C2HEVT_CLEAR, C2H_EVT_HOST_CLOSE);
+	rtl8723au_write8(adapter, REG_C2HEVT_CLEAR, C2H_EVT_HOST_CLOSE);
 }
 
-s32 c2h_evt_read23a(struct rtw_adapter *adapter, u8 *buf)
+int c2h_evt_read23a(struct rtw_adapter *adapter, u8 *buf)
 {
-	s32 ret = _FAIL;
+	int ret = _FAIL;
 	struct c2h_evt_hdr *c2h_evt;
 	int i;
 	u8 trigger;
@@ -371,7 +366,7 @@ s32 c2h_evt_read23a(struct rtw_adapter *adapter, u8 *buf)
 	if (buf == NULL)
 		goto exit;
 
-	trigger = rtw_read8(adapter, REG_C2HEVT_CLEAR);
+	trigger = rtl8723au_read8(adapter, REG_C2HEVT_CLEAR);
 
 	if (trigger == C2H_EVT_HOST_CLOSE)
 		goto exit;	/* Not ready */
@@ -382,8 +377,8 @@ s32 c2h_evt_read23a(struct rtw_adapter *adapter, u8 *buf)
 
 	memset(c2h_evt, 0, 16);
 
-	*buf = rtw_read8(adapter, REG_C2HEVT_MSG_NORMAL);
-	*(buf + 1) = rtw_read8(adapter, REG_C2HEVT_MSG_NORMAL + 1);
+	*buf = rtl8723au_read8(adapter, REG_C2HEVT_MSG_NORMAL);
+	*(buf + 1) = rtl8723au_read8(adapter, REG_C2HEVT_MSG_NORMAL + 1);
 
 	RT_PRINT_DATA(_module_hal_init_c_, _drv_info_, "c2h_evt_read23a(): ",
 		      &c2h_evt, sizeof(c2h_evt));
@@ -396,7 +391,7 @@ s32 c2h_evt_read23a(struct rtw_adapter *adapter, u8 *buf)
 
 	/* Read the content */
 	for (i = 0; i < c2h_evt->plen; i++)
-		c2h_evt->payload[i] = rtw_read8(adapter,
+		c2h_evt->payload[i] = rtl8723au_read8(adapter,
 						REG_C2HEVT_MSG_NORMAL +
 						sizeof(*c2h_evt) + i);
 
@@ -424,15 +419,14 @@ rtl8723a_set_ampdu_min_space(struct rtw_adapter *padapter, u8 MinSpacingToSet)
 
 	if (MinSpacingToSet <= 7) {
 		switch (padapter->securitypriv.dot11PrivacyAlgrthm) {
-		case _NO_PRIVACY_:
-		case _AES_:
+		case 0:
+		case WLAN_CIPHER_SUITE_CCMP:
 			SecMinSpace = 0;
 			break;
 
-		case _WEP40_:
-		case _WEP104_:
-		case _TKIP_:
-		case _TKIP_WTMIC_:
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
+		case WLAN_CIPHER_SUITE_TKIP:
 			SecMinSpace = 6;
 			break;
 		default:
@@ -447,9 +441,9 @@ rtl8723a_set_ampdu_min_space(struct rtw_adapter *padapter, u8 MinSpacingToSet)
 		   ("Set HW_VAR_AMPDU_MIN_SPACE: %#x\n",
 		   padapter->MgntInfo.MinSpaceCfg)); */
 		MinSpacingToSet |=
-			rtw_read8(padapter, REG_AMPDU_MIN_SPACE) & 0xf8;
-		rtw_write8(padapter, REG_AMPDU_MIN_SPACE,
-			   MinSpacingToSet);
+			rtl8723au_read8(padapter, REG_AMPDU_MIN_SPACE) & 0xf8;
+		rtl8723au_write8(padapter, REG_AMPDU_MIN_SPACE,
+				 MinSpacingToSet);
 	}
 }
 
@@ -461,15 +455,12 @@ void rtl8723a_set_ampdu_factor(struct rtw_adapter *padapter, u8 FactorToSet)
 	u8 index = 0;
 
 	pRegToSet = RegToSet_Normal;	/*  0xb972a841; */
-#ifdef CONFIG_8723AU_BT_COEXIST
-	if ((BT_IsBtDisabled(padapter) == false) &&
-	    (BT_1Ant(padapter) == true)) {
+
+	if (rtl8723a_BT_enabled(padapter) &&
+	    rtl8723a_BT_using_antenna_1(padapter))
 		MaxAggNum = 0x8;
-	} else
-#endif /*  CONFIG_8723AU_BT_COEXIST */
-	{
+	else
 		MaxAggNum = 0xF;
-	}
 
 	if (FactorToSet <= 3) {
 		FactorToSet = (1 << (FactorToSet + 2));
@@ -485,8 +476,8 @@ void rtl8723a_set_ampdu_factor(struct rtw_adapter *padapter, u8 FactorToSet)
 				pRegToSet[index] = (pRegToSet[index] & 0xf0) |
 					FactorToSet;
 
-			rtw_write8(padapter, REG_AGGLEN_LMT + index,
-				   pRegToSet[index]);
+			rtl8723au_write8(padapter, REG_AGGLEN_LMT + index,
+					 pRegToSet[index]);
 		}
 
 		/* RT_TRACE(COMP_MLME, DBG_LOUD,
@@ -512,25 +503,25 @@ void rtl8723a_set_acm_ctrl(struct rtw_adapter *padapter, u8 ctrl)
 	}
 
 	DBG_8723A("[HW_VAR_ACM_CTRL] Write 0x%02X\n", hwctrl);
-	rtw_write8(padapter, REG_ACMHWCTRL, hwctrl);
+	rtl8723au_write8(padapter, REG_ACMHWCTRL, hwctrl);
 }
 
 void rtl8723a_set_media_status(struct rtw_adapter *padapter, u8 status)
 {
 	u8 val8;
 
-	val8 = rtw_read8(padapter, MSR) & 0x0c;
+	val8 = rtl8723au_read8(padapter, MSR) & 0x0c;
 	val8 |= status;
-	rtw_write8(padapter, MSR, val8);
+	rtl8723au_write8(padapter, MSR, val8);
 }
 
 void rtl8723a_set_media_status1(struct rtw_adapter *padapter, u8 status)
 {
 	u8 val8;
 
-	val8 = rtw_read8(padapter, MSR) & 0x03;
+	val8 = rtl8723au_read8(padapter, MSR) & 0x03;
 	val8 |= status << 2;
-	rtw_write8(padapter, MSR, val8);
+	rtl8723au_write8(padapter, MSR, val8);
 }
 
 void rtl8723a_set_bcn_func(struct rtw_adapter *padapter, u8 val)
@@ -544,12 +535,12 @@ void rtl8723a_set_bcn_func(struct rtw_adapter *padapter, u8 val)
 void rtl8723a_check_bssid(struct rtw_adapter *padapter, u8 val)
 {
 	u32 val32;
-	val32 = rtw_read32(padapter, REG_RCR);
+	val32 = rtl8723au_read32(padapter, REG_RCR);
 	if (val)
 		val32 |= RCR_CBSSID_DATA | RCR_CBSSID_BCN;
 	else
 		val32 &= ~(RCR_CBSSID_DATA | RCR_CBSSID_BCN);
-	rtw_write32(padapter, REG_RCR, val32);
+	rtl8723au_write32(padapter, REG_RCR, val32);
 }
 
 void rtl8723a_mlme_sitesurvey(struct rtw_adapter *padapter, u8 flag)
@@ -559,11 +550,11 @@ void rtl8723a_mlme_sitesurvey(struct rtw_adapter *padapter, u8 flag)
 
 		/*  config RCR to receive different BSSID & not
 		    to receive data frame */
-		v32 = rtw_read32(padapter, REG_RCR);
+		v32 = rtl8723au_read32(padapter, REG_RCR);
 		v32 &= ~(RCR_CBSSID_BCN);
-		rtw_write32(padapter, REG_RCR, v32);
+		rtl8723au_write32(padapter, REG_RCR, v32);
 		/*  reject all data frame */
-		rtw_write16(padapter, REG_RXFLTMAP2, 0);
+		rtl8723au_write16(padapter, REG_RXFLTMAP2, 0);
 
 		/*  disable update TSF */
 		SetBcnCtrlReg23a(padapter, DIS_TSF_UDT, 0);
@@ -579,35 +570,34 @@ void rtl8723a_mlme_sitesurvey(struct rtw_adapter *padapter, u8 flag)
 		    ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) ||
 		    ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE)) {
 			/*  enable to rx data frame */
-			rtw_write16(padapter, REG_RXFLTMAP2, 0xFFFF);
+			rtl8723au_write16(padapter, REG_RXFLTMAP2, 0xFFFF);
 
 			/*  enable update TSF */
 			SetBcnCtrlReg23a(padapter, 0, DIS_TSF_UDT);
 		}
 
-		v32 = rtw_read32(padapter, REG_RCR);
+		v32 = rtl8723au_read32(padapter, REG_RCR);
 		v32 |= RCR_CBSSID_BCN;
-		rtw_write32(padapter, REG_RCR, v32);
+		rtl8723au_write32(padapter, REG_RCR, v32);
 	}
 
-#ifdef CONFIG_8723AU_BT_COEXIST
-	BT_WifiScanNotify(padapter, flag ? true : false);
-#endif
+	rtl8723a_BT_wifiscan_notify(padapter, flag ? true : false);
 }
 
 void rtl8723a_on_rcr_am(struct rtw_adapter *padapter)
 {
-	rtw_write32(padapter, REG_RCR, rtw_read32(padapter, REG_RCR) | RCR_AM);
-	DBG_8723A("%s, %d, RCR = %x \n", __FUNCTION__, __LINE__,
-		  rtw_read32(padapter, REG_RCR));
+	rtl8723au_write32(padapter, REG_RCR,
+		    rtl8723au_read32(padapter, REG_RCR) | RCR_AM);
+	DBG_8723A("%s, %d, RCR = %x \n", __func__, __LINE__,
+		  rtl8723au_read32(padapter, REG_RCR));
 }
 
 void rtl8723a_off_rcr_am(struct rtw_adapter *padapter)
 {
-	rtw_write32(padapter, REG_RCR,
-		    rtw_read32(padapter, REG_RCR) & (~RCR_AM));
-	DBG_8723A("%s, %d, RCR = %x \n", __FUNCTION__, __LINE__,
-		  rtw_read32(padapter, REG_RCR));
+	rtl8723au_write32(padapter, REG_RCR,
+		    rtl8723au_read32(padapter, REG_RCR) & (~RCR_AM));
+	DBG_8723A("%s, %d, RCR = %x \n", __func__, __LINE__,
+		  rtl8723au_read32(padapter, REG_RCR));
 }
 
 void rtl8723a_set_slot_time(struct rtw_adapter *padapter, u8 slottime)
@@ -616,7 +606,7 @@ void rtl8723a_set_slot_time(struct rtw_adapter *padapter, u8 slottime)
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
-	rtw_write8(padapter, REG_SLOT, slottime);
+	rtl8723au_write8(padapter, REG_SLOT, slottime);
 
 	if (pmlmeinfo->WMM_enable == 0) {
 		if (pmlmeext->cur_wireless_mode == WIRELESS_11B)
@@ -627,10 +617,10 @@ void rtl8723a_set_slot_time(struct rtw_adapter *padapter, u8 slottime)
 		u1bAIFS = aSifsTime + (2 * pmlmeinfo->slotTime);
 
 		/*  <Roger_EXP> Temporary removed, 2008.06.20. */
-		rtw_write8(padapter, REG_EDCA_VO_PARAM, u1bAIFS);
-		rtw_write8(padapter, REG_EDCA_VI_PARAM, u1bAIFS);
-		rtw_write8(padapter, REG_EDCA_BE_PARAM, u1bAIFS);
-		rtw_write8(padapter, REG_EDCA_BK_PARAM, u1bAIFS);
+		rtl8723au_write8(padapter, REG_EDCA_VO_PARAM, u1bAIFS);
+		rtl8723au_write8(padapter, REG_EDCA_VI_PARAM, u1bAIFS);
+		rtl8723au_write8(padapter, REG_EDCA_BE_PARAM, u1bAIFS);
+		rtl8723au_write8(padapter, REG_EDCA_BK_PARAM, u1bAIFS);
 	}
 }
 
@@ -645,12 +635,12 @@ void rtl8723a_ack_preamble(struct rtw_adapter *padapter, u8 bShortPreamble)
 	/* regTmp = 0; */
 	if (bShortPreamble)
 		regTmp |= 0x80;
-	rtw_write8(padapter, REG_RRSR + 2, regTmp);
+	rtl8723au_write8(padapter, REG_RRSR + 2, regTmp);
 }
 
 void rtl8723a_set_sec_cfg(struct rtw_adapter *padapter, u8 sec)
 {
-	rtw_write8(padapter, REG_SECCFG, sec);
+	rtl8723au_write8(padapter, REG_SECCFG, sec);
 }
 
 void rtl8723a_cam_empty_entry(struct rtw_adapter *padapter, u8 ucIndex)
@@ -674,29 +664,54 @@ void rtl8723a_cam_empty_entry(struct rtw_adapter *padapter, u8 ucIndex)
 		ulCommand = ulCommand | CAM_POLLINIG | CAM_WRITE;
 		/*  write content 0 is equall to mark invalid */
 		/* delay_ms(40); */
-		rtw_write32(padapter, WCAMI, ulContent);
+		rtl8723au_write32(padapter, WCAMI, ulContent);
 		/* RT_TRACE(COMP_SEC, DBG_LOUD,
-		   ("CAM_empty_entry23a(): WRITE A4: %lx \n", ulContent));*/
+		   ("rtl8723a_cam_empty_entry(): WRITE A4: %lx \n",
+		   ulContent));*/
 		/* delay_ms(40); */
-		rtw_write32(padapter, RWCAM, ulCommand);
+		rtl8723au_write32(padapter, RWCAM, ulCommand);
 		/* RT_TRACE(COMP_SEC, DBG_LOUD,
-		   ("CAM_empty_entry23a(): WRITE A0: %lx \n", ulCommand));*/
+		   ("rtl8723a_cam_empty_entry(): WRITE A0: %lx \n",
+		   ulCommand));*/
 	}
 }
 
 void rtl8723a_cam_invalid_all(struct rtw_adapter *padapter)
 {
-	rtw_write32(padapter, RWCAM, BIT(31) | BIT(30));
+	rtl8723au_write32(padapter, RWCAM, BIT(31) | BIT(30));
 }
 
-void rtl8723a_cam_write(struct rtw_adapter *padapter, u32 val1, u32 val2)
+void rtl8723a_cam_write(struct rtw_adapter *padapter,
+			u8 entry, u16 ctrl, const u8 *mac, const u8 *key)
 {
 	u32 cmd;
+	unsigned int i, val, addr;
+	int j;
+
+	addr = entry << 3;
+
+	for (j = 5; j >= 0; j--) {
+		switch (j) {
+		case 0:
+			val = ctrl | (mac[0] << 16) | (mac[1] << 24);
+			break;
+		case 1:
+			val = mac[2] | (mac[3] << 8) |
+				(mac[4] << 16) | (mac[5] << 24);
+			break;
+		default:
+			i = (j - 2) << 2;
+			val = key[i] | (key[i+1] << 8) |
+				(key[i+2] << 16) | (key[i+3] << 24);
+			break;
+		}
 
-	rtw_write32(padapter, WCAMI, val1);
+		rtl8723au_write32(padapter, WCAMI, val);
+		cmd = CAM_POLLINIG | CAM_WRITE | (addr + j);
+		rtl8723au_write32(padapter, RWCAM, cmd);
 
-	cmd = CAM_POLLINIG | CAM_WRITE | val2;
-	rtw_write32(padapter, RWCAM, cmd);
+		/* DBG_8723A("%s => cam write: %x, %x\n", __func__, cmd, val);*/
+	}
 }
 
 void rtl8723a_fifo_cleanup(struct rtw_adapter *padapter)
@@ -708,20 +723,21 @@ void rtl8723a_fifo_cleanup(struct rtw_adapter *padapter)
 	u8 trycnt = 100;
 
 	/*  pause tx */
-	rtw_write8(padapter, REG_TXPAUSE, 0xff);
+	rtl8723au_write8(padapter, REG_TXPAUSE, 0xff);
 
 	/*  keep sn */
-	padapter->xmitpriv.nqos_ssn = rtw_read16(padapter, REG_NQOS_SEQ);
+	padapter->xmitpriv.nqos_ssn = rtl8723au_read16(padapter, REG_NQOS_SEQ);
 
 	if (pwrpriv->bkeepfwalive != true) {
 		u32 v32;
 
 		/*  RX DMA stop */
-		v32 = rtw_read32(padapter, REG_RXPKT_NUM);
+		v32 = rtl8723au_read32(padapter, REG_RXPKT_NUM);
 		v32 |= RW_RELEASE_EN;
-		rtw_write32(padapter, REG_RXPKT_NUM, v32);
+		rtl8723au_write32(padapter, REG_RXPKT_NUM, v32);
 		do {
-			v32 = rtw_read32(padapter, REG_RXPKT_NUM) & RXDMA_IDLE;
+			v32 = rtl8723au_read32(padapter,
+					       REG_RXPKT_NUM) & RXDMA_IDLE;
 			if (!v32)
 				break;
 		} while (trycnt--);
@@ -730,36 +746,32 @@ void rtl8723a_fifo_cleanup(struct rtw_adapter *padapter)
 		}
 
 		/*  RQPN Load 0 */
-		rtw_write16(padapter, REG_RQPN_NPQ, 0);
-		rtw_write32(padapter, REG_RQPN, 0x80000000);
+		rtl8723au_write16(padapter, REG_RQPN_NPQ, 0);
+		rtl8723au_write32(padapter, REG_RQPN, 0x80000000);
 		mdelay(10);
 	}
 }
 
-void rtl8723a_set_apfm_on_mac(struct rtw_adapter *padapter, u8 val)
-{
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-
-	pHalData->bMacPwrCtrlOn = val;
-	DBG_8723A("%s: bMacPwrCtrlOn =%d\n", __func__, pHalData->bMacPwrCtrlOn);
-}
-
 void rtl8723a_bcn_valid(struct rtw_adapter *padapter)
 {
 	/* BCN_VALID, BIT16 of REG_TDECTRL = BIT0 of REG_TDECTRL+2,
 	   write 1 to clear, Clear by sw */
-	rtw_write8(padapter, REG_TDECTRL + 2,
-		   rtw_read8(padapter, REG_TDECTRL + 2) | BIT0);
+	rtl8723au_write8(padapter, REG_TDECTRL + 2,
+			 rtl8723au_read8(padapter, REG_TDECTRL + 2) | BIT(0));
 }
 
-void rtl8723a_set_tx_pause(struct rtw_adapter *padapter, u8 pause)
+bool rtl8723a_get_bcn_valid(struct rtw_adapter *padapter)
 {
-	rtw_write8(padapter, REG_TXPAUSE, pause);
+	bool retval;
+
+	retval = (rtl8723au_read8(padapter, REG_TDECTRL + 2) & BIT(0)) ? true : false;
+
+	return retval;
 }
 
 void rtl8723a_set_beacon_interval(struct rtw_adapter *padapter, u16 interval)
 {
-	rtw_write16(padapter, REG_BCN_INTERVAL, interval);
+	rtl8723au_write16(padapter, REG_BCN_INTERVAL, interval);
 }
 
 void rtl8723a_set_resp_sifs(struct rtw_adapter *padapter,
@@ -768,24 +780,24 @@ void rtl8723a_set_resp_sifs(struct rtw_adapter *padapter,
 	/* SIFS_Timer = 0x0a0a0808; */
 	/* RESP_SIFS for CCK */
 	/*  SIFS_T2T_CCK (0x08) */
-	rtw_write8(padapter, REG_R2T_SIFS, r2t1);
+	rtl8723au_write8(padapter, REG_R2T_SIFS, r2t1);
 	/* SIFS_R2T_CCK(0x08) */
-	rtw_write8(padapter, REG_R2T_SIFS + 1, r2t2);
+	rtl8723au_write8(padapter, REG_R2T_SIFS + 1, r2t2);
 	/* RESP_SIFS for OFDM */
 	/* SIFS_T2T_OFDM (0x0a) */
-	rtw_write8(padapter, REG_T2T_SIFS, t2t1);
+	rtl8723au_write8(padapter, REG_T2T_SIFS, t2t1);
 	/* SIFS_R2T_OFDM(0x0a) */
-	rtw_write8(padapter, REG_T2T_SIFS + 1, t2t2);
+	rtl8723au_write8(padapter, REG_T2T_SIFS + 1, t2t2);
 }
 
 void rtl8723a_set_ac_param_vo(struct rtw_adapter *padapter, u32 vo)
 {
-	rtw_write32(padapter, REG_EDCA_VO_PARAM, vo);
+	rtl8723au_write32(padapter, REG_EDCA_VO_PARAM, vo);
 }
 
 void rtl8723a_set_ac_param_vi(struct rtw_adapter *padapter, u32 vi)
 {
-	rtw_write32(padapter, REG_EDCA_VI_PARAM, vi);
+	rtl8723au_write32(padapter, REG_EDCA_VI_PARAM, vi);
 }
 
 void rtl8723a_set_ac_param_be(struct rtw_adapter *padapter, u32 be)
@@ -793,17 +805,17 @@ void rtl8723a_set_ac_param_be(struct rtw_adapter *padapter, u32 be)
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
 	pHalData->AcParam_BE = be;
-	rtw_write32(padapter, REG_EDCA_BE_PARAM, be);
+	rtl8723au_write32(padapter, REG_EDCA_BE_PARAM, be);
 }
 
 void rtl8723a_set_ac_param_bk(struct rtw_adapter *padapter, u32 bk)
 {
-	rtw_write32(padapter, REG_EDCA_BK_PARAM, bk);
+	rtl8723au_write32(padapter, REG_EDCA_BK_PARAM, bk);
 }
 
 void rtl8723a_set_rxdma_agg_pg_th(struct rtw_adapter *padapter, u8 val)
 {
-	rtw_write8(padapter, REG_RXDMA_AGG_PG_TH, val);
+	rtl8723au_write8(padapter, REG_RXDMA_AGG_PG_TH, val);
 }
 
 void rtl8723a_set_nav_upper(struct rtw_adapter *padapter, u32 usNavUpper)
@@ -821,7 +833,7 @@ void rtl8723a_set_nav_upper(struct rtw_adapter *padapter, u32 usNavUpper)
 	/*  is getting the upper integer. */
 	usNavUpper = (usNavUpper + HAL_8723A_NAV_UPPER_UNIT - 1) /
 		HAL_8723A_NAV_UPPER_UNIT;
-	rtw_write8(padapter, REG_NAV_UPPER, (u8) usNavUpper);
+	rtl8723au_write8(padapter, REG_NAV_UPPER, (u8) usNavUpper);
 }
 
 void rtl8723a_set_initial_gain(struct rtw_adapter *padapter, u32 rx_gain)
@@ -837,23 +849,18 @@ void rtl8723a_set_initial_gain(struct rtw_adapter *padapter, u32 rx_gain)
 	}
 }
 
-void rtl8723a_odm_support_ability_write(struct rtw_adapter *padapter, u32 val)
+void rtl8723a_odm_support_ability_restore(struct rtw_adapter *padapter)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
-	pHalData->odmpriv.SupportAbility = val;
+	pHalData->odmpriv.SupportAbility = pHalData->odmpriv.BK_SupportAbility;
 }
 
-void rtl8723a_odm_support_ability_backup(struct rtw_adapter *padapter, u8 val)
+void rtl8723a_odm_support_ability_backup(struct rtw_adapter *padapter)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
-	if (val)	/*  save dm flag */
-		pHalData->odmpriv.BK_SupportAbility =
-			pHalData->odmpriv.SupportAbility;
-	else		/*  restore dm flag */
-		pHalData->odmpriv.SupportAbility =
-			pHalData->odmpriv.BK_SupportAbility;
+	pHalData->odmpriv.BK_SupportAbility = pHalData->odmpriv.SupportAbility;
 }
 
 void rtl8723a_odm_support_ability_set(struct rtw_adapter *padapter, u32 val)
@@ -877,5 +884,44 @@ void rtl8723a_odm_support_ability_clr(struct rtw_adapter *padapter, u32 val)
 
 void rtl8723a_set_rpwm(struct rtw_adapter *padapter, u8 val)
 {
-	rtw_write8(padapter, REG_USB_HRPWM, val);
+	rtl8723au_write8(padapter, REG_USB_HRPWM, val);
+}
+
+u8 rtl8723a_get_rf_type(struct rtw_adapter *padapter)
+{
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
+
+	return pHalData->rf_type;
+}
+
+bool rtl8723a_get_fwlps_rf_on(struct rtw_adapter *padapter)
+{
+	bool retval;
+	u32 valRCR;
+
+	/*  When we halt NIC, we should check if FW LPS is leave. */
+
+	if ((padapter->bSurpriseRemoved == true) ||
+	    (padapter->pwrctrlpriv.rf_pwrstate == rf_off)) {
+		/*  If it is in HW/SW Radio OFF or IPS state, we do
+		    not check Fw LPS Leave, because Fw is unload. */
+		retval = true;
+	} else {
+		valRCR = rtl8723au_read32(padapter, REG_RCR);
+		if (valRCR & 0x00070000)
+			retval = false;
+		else
+			retval = true;
+	}
+
+	return retval;
+}
+
+bool rtl8723a_chk_hi_queue_empty(struct rtw_adapter *padapter)
+{
+	u32 hgq;
+
+	hgq = rtl8723au_read32(padapter, REG_HGQ_INFORMATION);
+
+	return ((hgq & 0x0000ff00) == 0) ? true : false;
 }
diff --git a/drivers/staging/rtl8723au/hal/hal_intf.c b/drivers/staging/rtl8723au/hal/hal_intf.c
index de3608b4010..5383e692546 100644
--- a/drivers/staging/rtl8723au/hal/hal_intf.c
+++ b/drivers/staging/rtl8723au/hal/hal_intf.c
@@ -19,232 +19,7 @@
 
 #include <hal_intf.h>
 
-#include <usb_hal.h>
-
-void rtw_hal_chip_configure23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.intf_chip_configure)
-		padapter->HalFunc.intf_chip_configure(padapter);
-}
-
-void rtw_hal_read_chip_info23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.read_adapter_info)
-		padapter->HalFunc.read_adapter_info(padapter);
-}
-
-void rtw_hal_read_chip_version23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.read_chip_version)
-		padapter->HalFunc.read_chip_version(padapter);
-}
-
-void rtw_hal_def_value_init23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.init_default_value)
-		padapter->HalFunc.init_default_value(padapter);
-}
-void	rtw_hal_free_data23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.free_hal_data)
-		padapter->HalFunc.free_hal_data(padapter);
-}
-void	rtw_hal_dm_init23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.dm_init)
-		padapter->HalFunc.dm_init(padapter);
-}
-void rtw_hal_dm_deinit23a(struct rtw_adapter *padapter)
-{
-	/*  cancel dm  timer */
-	if (padapter->HalFunc.dm_deinit)
-		padapter->HalFunc.dm_deinit(padapter);
-}
-void	rtw_hal_sw_led_init23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.InitSwLeds)
-		padapter->HalFunc.InitSwLeds(padapter);
-}
-
-void rtw_hal_sw_led_deinit23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.DeInitSwLeds)
-		padapter->HalFunc.DeInitSwLeds(padapter);
-}
-
-u32 rtw_hal_power_on23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.hal_power_on)
-		return padapter->HalFunc.hal_power_on(padapter);
-	return _FAIL;
-}
-
-uint	 rtw_hal_init23a(struct rtw_adapter *padapter)
-{
-	uint	status = _SUCCESS;
-
-	padapter->hw_init_completed = false;
-
-	status = padapter->HalFunc.hal_init(padapter);
-
-	if (status == _SUCCESS) {
-		padapter->hw_init_completed = true;
-
-		if (padapter->registrypriv.notch_filter == 1)
-			rtw_hal_notch_filter23a(padapter, 1);
-
-		rtw_hal_reset_security_engine23a(padapter);
-	} else {
-		padapter->hw_init_completed = false;
-		DBG_8723A("rtw_hal_init23a: hal__init fail\n");
-	}
-
-	RT_TRACE(_module_hal_init_c_, _drv_err_, ("-rtl871x_hal_init:status = 0x%x\n", status));
-
-	return status;
-}
-
-uint rtw_hal_deinit23a(struct rtw_adapter *padapter)
-{
-	uint	status = _SUCCESS;
-
-	status = padapter->HalFunc.hal_deinit(padapter);
-
-	if (status == _SUCCESS)
-		padapter->hw_init_completed = false;
-	else
-		DBG_8723A("\n rtw_hal_deinit23a: hal_init fail\n");
-	return status;
-}
-
-void rtw_hal_set_hwreg23a(struct rtw_adapter *padapter, u8 variable, u8 *val)
-{
-	if (padapter->HalFunc.SetHwRegHandler)
-		padapter->HalFunc.SetHwRegHandler(padapter, variable, val);
-}
-
-void rtw23a_hal_get_hwreg(struct rtw_adapter *padapter, u8 variable, u8 *val)
-{
-	if (padapter->HalFunc.GetHwRegHandler)
-		padapter->HalFunc.GetHwRegHandler(padapter, variable, val);
-}
-
-u8 rtw_hal_set_def_var23a(struct rtw_adapter *padapter, enum hal_def_variable eVariable, void *pValue)
-{
-	if (padapter->HalFunc.SetHalDefVarHandler)
-		return padapter->HalFunc.SetHalDefVarHandler(padapter, eVariable, pValue);
-	return _FAIL;
-}
-u8 rtw_hal_get_def_var23a(struct rtw_adapter *padapter, enum hal_def_variable eVariable, void *pValue)
-{
-	if (padapter->HalFunc.GetHalDefVarHandler)
-		return padapter->HalFunc.GetHalDefVarHandler(padapter, eVariable, pValue);
-	return _FAIL;
-}
-
-void rtw_hal_set_odm_var23a(struct rtw_adapter *padapter, enum hal_odm_variable eVariable, void *pValue1, bool bSet)
-{
-	if (padapter->HalFunc.SetHalODMVarHandler)
-		padapter->HalFunc.SetHalODMVarHandler(padapter, eVariable, pValue1, bSet);
-}
-void	rtw_hal_get_odm_var23a(struct rtw_adapter *padapter, enum hal_odm_variable eVariable, void *pValue1, bool bSet)
-{
-	if (padapter->HalFunc.GetHalODMVarHandler)
-		padapter->HalFunc.GetHalODMVarHandler(padapter, eVariable, pValue1, bSet);
-}
-
-void rtw_hal_enable_interrupt23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.enable_interrupt)
-		padapter->HalFunc.enable_interrupt(padapter);
-	else
-		DBG_8723A("%s: HalFunc.enable_interrupt is NULL!\n", __FUNCTION__);
-
-}
-void rtw_hal_disable_interrupt23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.disable_interrupt)
-		padapter->HalFunc.disable_interrupt(padapter);
-	else
-		DBG_8723A("%s: HalFunc.disable_interrupt is NULL!\n", __FUNCTION__);
-
-}
-
-u32	rtw_hal_inirp_init23a(struct rtw_adapter *padapter)
-{
-	u32 rst = _FAIL;
-	if (padapter->HalFunc.inirp_init)
-		rst = padapter->HalFunc.inirp_init(padapter);
-	else
-		DBG_8723A(" %s HalFunc.inirp_init is NULL!!!\n", __FUNCTION__);
-	return rst;
-}
-
-u32	rtw_hal_inirp_deinit23a(struct rtw_adapter *padapter)
-{
-
-	if (padapter->HalFunc.inirp_deinit)
-		return padapter->HalFunc.inirp_deinit(padapter);
-
-	return _FAIL;
-
-}
-
-u8	rtw_hal_intf_ps_func23a(struct rtw_adapter *padapter, enum hal_intf_ps_func efunc_id, u8 *val)
-{
-	if (padapter->HalFunc.interface_ps_func)
-		return padapter->HalFunc.interface_ps_func(padapter, efunc_id, val);
-	return _FAIL;
-}
-
-s32	rtw_hal_xmit23aframe_enqueue(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe)
-{
-	if (padapter->HalFunc.hal_xmitframe_enqueue)
-		return padapter->HalFunc.hal_xmitframe_enqueue(padapter, pxmitframe);
-
-	return false;
-}
-
-s32	rtw_hal_xmit23a(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe)
-{
-	if (padapter->HalFunc.hal_xmit)
-		return padapter->HalFunc.hal_xmit(padapter, pxmitframe);
-
-	return false;
-}
-
-s32	rtw_hal_mgnt_xmit23a(struct rtw_adapter *padapter, struct xmit_frame *pmgntframe)
-{
-	s32 ret = _FAIL;
-	if (padapter->HalFunc.mgnt_xmit)
-		ret = padapter->HalFunc.mgnt_xmit(padapter, pmgntframe);
-	return ret;
-}
-
-s32	rtw_hal_init23a_xmit_priv(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.init_xmit_priv != NULL)
-		return padapter->HalFunc.init_xmit_priv(padapter);
-	return _FAIL;
-}
-void	rtw_hal_free_xmit_priv23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.free_xmit_priv != NULL)
-		padapter->HalFunc.free_xmit_priv(padapter);
-}
-
-s32	rtw_hal_init23a_recv_priv(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.init_recv_priv)
-		return padapter->HalFunc.init_recv_priv(padapter);
-
-	return _FAIL;
-}
-void	rtw_hal_free_recv_priv23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.free_recv_priv)
-		padapter->HalFunc.free_recv_priv(padapter);
-}
+#include <rtl8723a_hal.h>
 
 void rtw_hal_update_ra_mask23a(struct sta_info *psta, u8 rssi_level)
 {
@@ -262,159 +37,6 @@ void rtw_hal_update_ra_mask23a(struct sta_info *psta, u8 rssi_level)
 #ifdef CONFIG_8723AU_AP_MODE
 		add_RATid23a(padapter, psta, rssi_level);
 #endif
-	} else {
-		if (padapter->HalFunc.UpdateRAMaskHandler)
-			padapter->HalFunc.UpdateRAMaskHandler(padapter, psta->mac_id, rssi_level);
-	}
-}
-
-void	rtw_hal_add_ra_tid23a(struct rtw_adapter *padapter, u32 bitmap, u8 arg, u8 rssi_level)
-{
-	if (padapter->HalFunc.Add_RateATid)
-		padapter->HalFunc.Add_RateATid(padapter, bitmap, arg, rssi_level);
-}
-
-/*	Start specifical interface thread		*/
-void	rtw_hal_start_thread23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.run_thread)
-		padapter->HalFunc.run_thread(padapter);
-}
-/*	Start specifical interface thread		*/
-void	rtw_hal_stop_thread23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.cancel_thread)
-		padapter->HalFunc.cancel_thread(padapter);
-}
-
-u32	rtw_hal_read_bbreg23a(struct rtw_adapter *padapter, u32 RegAddr, u32 BitMask)
-{
-	u32 data = 0;
-	if (padapter->HalFunc.read_bbreg)
-		 data = padapter->HalFunc.read_bbreg(padapter, RegAddr, BitMask);
-	return data;
-}
-void	rtw_hal_write_bbreg23a(struct rtw_adapter *padapter, u32 RegAddr, u32 BitMask, u32 Data)
-{
-	if (padapter->HalFunc.write_bbreg)
-		padapter->HalFunc.write_bbreg(padapter, RegAddr, BitMask, Data);
-}
-
-u32	rtw_hal_read_rfreg23a(struct rtw_adapter *padapter, u32 eRFPath, u32 RegAddr, u32 BitMask)
-{
-	u32 data = 0;
-	if (padapter->HalFunc.read_rfreg)
-		data = padapter->HalFunc.read_rfreg(padapter, eRFPath, RegAddr, BitMask);
-	return data;
-}
-void	rtw_hal_write_rfreg23a(struct rtw_adapter *padapter, u32 eRFPath, u32 RegAddr, u32 BitMask, u32 Data)
-{
-	if (padapter->HalFunc.write_rfreg)
-		padapter->HalFunc.write_rfreg(padapter, eRFPath, RegAddr, BitMask, Data);
-}
-
-s32	rtw_hal_interrupt_handler23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.interrupt_handler)
-		return padapter->HalFunc.interrupt_handler(padapter);
-	return _FAIL;
-}
-
-void	rtw_hal_set_bwmode23a(struct rtw_adapter *padapter,
-			   enum ht_channel_width Bandwidth, u8 offset)
-{
-	if (padapter->HalFunc.set_bwmode_handler)
-		padapter->HalFunc.set_bwmode_handler(padapter, Bandwidth,
-						     offset);
-}
-
-void	rtw_hal_set_chan23a(struct rtw_adapter *padapter, u8 channel)
-{
-	if (padapter->HalFunc.set_channel_handler)
-		padapter->HalFunc.set_channel_handler(padapter, channel);
-}
-
-void	rtw_hal_dm_watchdog23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.hal_dm_watchdog)
-		padapter->HalFunc.hal_dm_watchdog(padapter);
-}
-
-void rtw_hal_bcn_related_reg_setting23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.SetBeaconRelatedRegistersHandler)
-		padapter->HalFunc.SetBeaconRelatedRegistersHandler(padapter);
-}
-
-void	rtw_hal_sreset_init23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.sreset_init_value23a)
-		padapter->HalFunc.sreset_init_value23a(padapter);
-}
-void rtw_hal_sreset_reset23a(struct rtw_adapter *padapter)
-{
-	padapter = GET_PRIMARY_ADAPTER(padapter);
-
-	if (padapter->HalFunc.silentreset)
-		padapter->HalFunc.silentreset(padapter);
-}
-
-void rtw_hal_sreset_reset23a_value23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.sreset_reset_value23a)
-		padapter->HalFunc.sreset_reset_value23a(padapter);
-}
-
-void rtw_hal_sreset_xmit_status_check23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.sreset_xmit_status_check)
-		padapter->HalFunc.sreset_xmit_status_check(padapter);
-}
-void rtw_hal_sreset_linked_status_check23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.sreset_linked_status_check)
-		padapter->HalFunc.sreset_linked_status_check(padapter);
-}
-u8   rtw_hal_sreset_get_wifi_status23a(struct rtw_adapter *padapter)
-{
-	u8 status = 0;
-	if (padapter->HalFunc.sreset_get_wifi_status23a)
-		status = padapter->HalFunc.sreset_get_wifi_status23a(padapter);
-	return status;
-}
-
-bool rtw_hal_sreset_inprogress(struct rtw_adapter *padapter)
-{
-	bool inprogress = false;
-
-	padapter = GET_PRIMARY_ADAPTER(padapter);
-
-	if (padapter->HalFunc.sreset_inprogress)
-		inprogress = padapter->HalFunc.sreset_inprogress(padapter);
-	return inprogress;
-}
-
-void rtw_hal_notch_filter23a(struct rtw_adapter *adapter, bool enable)
-{
-	if (adapter->HalFunc.hal_notch_filter)
-		adapter->HalFunc.hal_notch_filter(adapter, enable);
-}
-
-void rtw_hal_reset_security_engine23a(struct rtw_adapter *adapter)
-{
-	if (adapter->HalFunc.hal_reset_security_engine)
-		adapter->HalFunc.hal_reset_security_engine(adapter);
-}
-
-s32 rtw_hal_c2h_handler23a(struct rtw_adapter *adapter, struct c2h_evt_hdr *c2h_evt)
-{
-	s32 ret = _FAIL;
-	if (adapter->HalFunc.c2h_handler)
-		ret = adapter->HalFunc.c2h_handler(adapter, c2h_evt);
-	return ret;
-}
-
-c2h_id_filter rtw_hal_c2h_id_filter_ccx23a(struct rtw_adapter *adapter)
-{
-	return adapter->HalFunc.c2h_id_filter_ccx;
+	} else
+		rtl8723a_update_ramask(padapter, psta->mac_id, rssi_level);
 }
diff --git a/drivers/staging/rtl8723au/hal/odm.c b/drivers/staging/rtl8723au/hal/odm.c
index 584a74ed294..e15ebfe1881 100644
--- a/drivers/staging/rtl8723au/hal/odm.c
+++ b/drivers/staging/rtl8723au/hal/odm.c
@@ -14,6 +14,7 @@
  ******************************************************************************/
 
 #include "odm_precomp.h"
+#include "usb_ops_linux.h"
 
 static const u16 dB_Invert_Table[8][12] = {
 	{1, 1, 1, 2, 2, 2, 2, 3, 3, 3, 4, 4},
@@ -199,17 +200,6 @@ void odm_RefreshRateAdaptiveMask23aAPADSL23a(struct dm_odm_t *pDM_Odm);
 
 void odm_DynamicTxPower23aInit(struct dm_odm_t *pDM_Odm);
 
-void odm_DynamicTxPower23aRestorePowerIndex(struct dm_odm_t *pDM_Odm);
-
-void odm_DynamicTxPower23aSavePowerIndex(struct dm_odm_t *pDM_Odm);
-
-void odm_DynamicTxPower23aWritePowerIndex(struct dm_odm_t *pDM_Odm,
-	u8 Value);
-
-void odm_DynamicTxPower23a_92C(struct dm_odm_t *pDM_Odm);
-
-void odm_DynamicTxPower23a_92D(struct dm_odm_t *pDM_Odm);
-
 void odm_RSSIMonitorInit(struct dm_odm_t *pDM_Odm);
 
 void odm_RSSIMonitorCheck23aMP(struct dm_odm_t *pDM_Odm);
@@ -232,8 +222,6 @@ void odm_SwAntDivChkAntSwitchNIC(struct dm_odm_t *pDM_Odm,
 
 void odm_SwAntDivChkAntSwitchCallback23a(unsigned long data);
 
-void odm_GlobalAdapterCheck(void);
-
 void odm_RefreshRateAdaptiveMask23a(struct dm_odm_t *pDM_Odm);
 
 void ODM_TXPowerTrackingCheck23a(struct dm_odm_t *pDM_Odm);
@@ -306,7 +294,6 @@ void ODM23a_DMInit(struct dm_odm_t *pDM_Odm)
 void ODM_DMWatchdog23a(struct dm_odm_t *pDM_Odm)
 {
 	/* 2012.05.03 Luke: For all IC series */
-	odm_GlobalAdapterCheck();
 	odm_CmnInfoHook_Debug23a(pDM_Odm);
 	odm_CmnInfoUpdate_Debug23a(pDM_Odm);
 	odm_CommonInfoSelfUpdate23a(pDM_Odm);
@@ -343,7 +330,6 @@ void ODM_DMWatchdog23a(struct dm_odm_t *pDM_Odm)
 	if (pDM_Odm->SupportICType & ODM_IC_11N_SERIES) {
 		ODM_TXPowerTrackingCheck23a(pDM_Odm);
 	      odm_EdcaTurboCheck23a(pDM_Odm);
-		odm_DynamicTxPower23a(pDM_Odm);
 	}
 
 	odm_dtc(pDM_Odm);
@@ -563,7 +549,7 @@ void ODM_CmnInfoUpdate23a(struct dm_odm_t *pDM_Odm, u32 CmnInfo, u64 Value)
 void odm_CommonInfoSelfInit23a(struct dm_odm_t *pDM_Odm
 	)
 {
-	pDM_Odm->bCckHighPower = (bool) ODM_GetBBReg(pDM_Odm, 0x824, BIT9);
+	pDM_Odm->bCckHighPower = (bool) ODM_GetBBReg(pDM_Odm, 0x824, BIT(9));
 	pDM_Odm->RFPathRxEnable = (u8) ODM_GetBBReg(pDM_Odm, 0xc04, 0x0F);
 	if (pDM_Odm->SupportICType & (ODM_RTL8723A))
 		pDM_Odm->AntDivType = CGCS_RX_SW_ANTDIV;
@@ -941,8 +927,8 @@ void odm_FalseAlarmCounterStatistics23a(struct dm_odm_t *pDM_Odm)
 
 	if (pDM_Odm->SupportICType & ODM_IC_11N_SERIES) {
 		/* hold ofdm counter */
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_HOLDC_11N, BIT31, 1); /* hold page C counter */
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTD_11N, BIT31, 1); /* hold page D counter */
+		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_HOLDC_11N, BIT(31), 1); /* hold page C counter */
+		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTD_11N, BIT(31), 1); /* hold page D counter */
 
 		ret_value = ODM_GetBBReg(pDM_Odm, ODM_REG_OFDM_FA_TYPE1_11N, bMaskDWord);
 		FalseAlmCnt->Cnt_Fast_Fsync = (ret_value&0xffff);
@@ -962,9 +948,9 @@ void odm_FalseAlarmCounterStatistics23a(struct dm_odm_t *pDM_Odm)
 					     FalseAlmCnt->Cnt_Mcs_fail +
 					     FalseAlmCnt->Cnt_Fast_Fsync +
 					     FalseAlmCnt->Cnt_SB_Search_fail;
-	/* hold cck counter */
-	ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N, BIT12, 1);
-	ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N, BIT14, 1);
+		/* hold cck counter */
+		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N, BIT(12), 1);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N, BIT(14), 1);
 
 	ret_value = ODM_GetBBReg(pDM_Odm, ODM_REG_CCK_FA_LSB_11N, bMaskByte0);
 	FalseAlmCnt->Cnt_Cck_fail = ret_value;
@@ -986,20 +972,24 @@ void odm_FalseAlarmCounterStatistics23a(struct dm_odm_t *pDM_Odm)
 
 	if (pDM_Odm->SupportICType >= ODM_RTL8723A) {
 		/* reset false alarm counter registers */
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTC_11N, BIT31, 1);
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTC_11N, BIT31, 0);
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTD_11N, BIT27, 1);
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTD_11N, BIT27, 0);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTC_11N, BIT(31), 1);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTC_11N, BIT(31), 0);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTD_11N, BIT(27), 1);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTD_11N, BIT(27), 0);
 		/* update ofdm counter */
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_HOLDC_11N, BIT31, 0); /* update page C counter */
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTD_11N, BIT31, 0); /* update page D counter */
+		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_HOLDC_11N, BIT(31), 0); /* update page C counter */
+		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTD_11N, BIT(31), 0); /* update page D counter */
 
 		/* reset CCK CCA counter */
-		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N, BIT13|BIT12, 0);
-		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N, BIT13|BIT12, 2);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N,
+			     BIT(13) | BIT(12), 0);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N,
+			     BIT(13) | BIT(12), 2);
 		/* reset CCK FA counter */
-		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N, BIT15|BIT14, 0);
-		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N, BIT15|BIT14, 2);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N,
+			     BIT(15) | BIT(14), 0);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N,
+			     BIT(15) | BIT(14), 2);
 	}
 
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_FA_CNT, ODM_DBG_LOUD, ("Enter odm_FalseAlarmCounterStatistics23a\n"));
@@ -1016,11 +1006,11 @@ void odm_FalseAlarmCounterStatistics23a(struct dm_odm_t *pDM_Odm)
 		FalseAlmCnt->Cnt_all = FalseAlmCnt->Cnt_Ofdm_fail + FalseAlmCnt->Cnt_Cck_fail;
 
 		/*  reset OFDM FA coutner */
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RST_11AC, BIT17, 1);
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RST_11AC, BIT17, 0);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RST_11AC, BIT(17), 1);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RST_11AC, BIT(17), 0);
 		/*  reset CCK FA counter */
-		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11AC, BIT15, 0);
-		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11AC, BIT15, 1);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11AC, BIT(15), 0);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11AC, BIT(15), 1);
 	}
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_FA_CNT, ODM_DBG_LOUD, ("Cnt_Cck_fail =%d\n", FalseAlmCnt->Cnt_Cck_fail));
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_FA_CNT, ODM_DBG_LOUD, ("Cnt_Ofdm_fail =%d\n", FalseAlmCnt->Cnt_Ofdm_fail));
@@ -1140,7 +1130,8 @@ void ODM_RF_Saving23a(struct dm_odm_t *pDM_Odm, u8 bForceInNormal)
 	if (pDM_PSTable->initialize == 0) {
 
 		pDM_PSTable->Reg874 = (ODM_GetBBReg(pDM_Odm, 0x874, bMaskDWord)&0x1CC000)>>14;
-		pDM_PSTable->RegC70 = (ODM_GetBBReg(pDM_Odm, 0xc70, bMaskDWord)&BIT3)>>3;
+		pDM_PSTable->RegC70 =
+			(ODM_GetBBReg(pDM_Odm, 0xc70, bMaskDWord) & BIT(3)) >>3;
 		pDM_PSTable->Reg85C = (ODM_GetBBReg(pDM_Odm, 0x85c, bMaskDWord)&0xFF000000)>>24;
 		pDM_PSTable->RegA74 = (ODM_GetBBReg(pDM_Odm, 0xa74, bMaskDWord)&0xF000)>>12;
 		/* Reg818 = PHY_QueryBBReg(pAdapter, 0x818, bMaskDWord); */
@@ -1172,23 +1163,23 @@ void ODM_RF_Saving23a(struct dm_odm_t *pDM_Odm, u8 bForceInNormal)
 			/*  <tynli_note> 8723 RSSI report will be wrong. Set 0x874[5]= 1 when enter BB power saving mode. */
 			/*  Suggested by SD3 Yu-Nan. 2011.01.20. */
 			if (pDM_Odm->SupportICType == ODM_RTL8723A)
-				ODM_SetBBReg(pDM_Odm, 0x874, BIT5, 0x1); /* Reg874[5]= 1b'1 */
+				ODM_SetBBReg(pDM_Odm, 0x874, BIT(5), 0x1); /* Reg874[5]= 1b'1 */
 			ODM_SetBBReg(pDM_Odm, 0x874, 0x1C0000, 0x2); /* Reg874[20:18]= 3'b010 */
-			ODM_SetBBReg(pDM_Odm, 0xc70, BIT3, 0); /* RegC70[3]= 1'b0 */
+			ODM_SetBBReg(pDM_Odm, 0xc70, BIT(3), 0); /* RegC70[3]= 1'b0 */
 			ODM_SetBBReg(pDM_Odm, 0x85c, 0xFF000000, 0x63); /* Reg85C[31:24]= 0x63 */
 			ODM_SetBBReg(pDM_Odm, 0x874, 0xC000, 0x2); /* Reg874[15:14]= 2'b10 */
 			ODM_SetBBReg(pDM_Odm, 0xa74, 0xF000, 0x3); /* RegA75[7:4]= 0x3 */
-			ODM_SetBBReg(pDM_Odm, 0x818, BIT28, 0x0); /* Reg818[28]= 1'b0 */
-			ODM_SetBBReg(pDM_Odm, 0x818, BIT28, 0x1); /* Reg818[28]= 1'b1 */
+			ODM_SetBBReg(pDM_Odm, 0x818, BIT(28), 0x0); /* Reg818[28]= 1'b0 */
+			ODM_SetBBReg(pDM_Odm, 0x818, BIT(28), 0x1); /* Reg818[28]= 1'b1 */
 		} else {
 			ODM_SetBBReg(pDM_Odm, 0x874, 0x1CC000, pDM_PSTable->Reg874);
-			ODM_SetBBReg(pDM_Odm, 0xc70, BIT3, pDM_PSTable->RegC70);
+			ODM_SetBBReg(pDM_Odm, 0xc70, BIT(3), pDM_PSTable->RegC70);
 			ODM_SetBBReg(pDM_Odm, 0x85c, 0xFF000000, pDM_PSTable->Reg85C);
 			ODM_SetBBReg(pDM_Odm, 0xa74, 0xF000, pDM_PSTable->RegA74);
-			ODM_SetBBReg(pDM_Odm, 0x818, BIT28, 0x0);
+			ODM_SetBBReg(pDM_Odm, 0x818, BIT(28), 0x0);
 
 			if (pDM_Odm->SupportICType == ODM_RTL8723A)
-				ODM_SetBBReg(pDM_Odm, 0x874, BIT5, 0x0); /* Reg874[5]= 1b'0 */
+				ODM_SetBBReg(pDM_Odm, 0x874, BIT(5), 0x0); /* Reg874[5]= 1b'0 */
 		}
 		pDM_PSTable->PreRFState = pDM_PSTable->CurRFState;
 	}
@@ -1289,7 +1280,7 @@ u32 ODM_Get_Rate_Bitmap23a(struct dm_odm_t *pDM_Odm,
 		break;
 	}
 
-	/* printk("%s ==> rssi_level:0x%02x, WirelessMode:0x%02x, rate_bitmap:0x%08x \n", __FUNCTION__, rssi_level, WirelessMode, rate_bitmap); */
+	/* printk("%s ==> rssi_level:0x%02x, WirelessMode:0x%02x, rate_bitmap:0x%08x \n", __func__, rssi_level, WirelessMode, rate_bitmap); */
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_RA_MASK, ODM_DBG_LOUD, (" ==> rssi_level:0x%02x, WirelessMode:0x%02x, rate_bitmap:0x%08x \n", rssi_level, WirelessMode, rate_bitmap));
 
 	return rate_bitmap;
@@ -1345,7 +1336,7 @@ void odm_RefreshRateAdaptiveMask23aCE23a(struct dm_odm_t *pDM_Odm)
 		return;
 	}
 
-	/* printk("==> %s \n", __FUNCTION__); */
+	/* printk("==> %s \n", __func__); */
 
 	for (i = 0; i < ODM_ASSOCIATE_ENTRY_NUM; i++) {
 		struct sta_info *pstat = pDM_Odm->pODM_StaInfo[i];
@@ -1424,60 +1415,13 @@ void odm_DynamicTxPower23aInit(struct dm_odm_t *pDM_Odm)
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 	struct dm_priv *pdmpriv = &pHalData->dmpriv;
 
-	pdmpriv->bDynamicTxPowerEnable = false;
-
-	pdmpriv->LastDTPLvl = TxHighPwrLevel_Normal;
+	/*
+	 * This is never changed, so we should be able to clean up the
+	 * code checking for different values in rtl8723a_rf6052.c
+	 */
 	pdmpriv->DynamicTxHighPowerLvl = TxHighPwrLevel_Normal;
 }
 
-void odm_DynamicTxPower23aSavePowerIndex(struct dm_odm_t *pDM_Odm)
-{
-	u8 index;
-	u32 Power_Index_REG[6] = {0xc90, 0xc91, 0xc92, 0xc98, 0xc99, 0xc9a};
-
-	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
-	struct dm_priv *pdmpriv = &pHalData->dmpriv;
-	for (index = 0; index < 6; index++)
-		pdmpriv->PowerIndex_backup[index] = rtw_read8(Adapter, Power_Index_REG[index]);
-}
-
-void odm_DynamicTxPower23aRestorePowerIndex(struct dm_odm_t *pDM_Odm)
-{
-	u8 index;
-	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
-
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
-	u32 Power_Index_REG[6] = {0xc90, 0xc91, 0xc92, 0xc98, 0xc99, 0xc9a};
-	struct dm_priv *pdmpriv = &pHalData->dmpriv;
-	for (index = 0; index < 6; index++)
-		rtw_write8(Adapter, Power_Index_REG[index], pdmpriv->PowerIndex_backup[index]);
-}
-
-void odm_DynamicTxPower23aWritePowerIndex(struct dm_odm_t *pDM_Odm,
-	u8 Value)
-{
-
-	u8 index;
-	u32 Power_Index_REG[6] = {0xc90, 0xc91, 0xc92, 0xc98, 0xc99, 0xc9a};
-
-	for (index = 0; index < 6; index++)
-		ODM_Write1Byte(pDM_Odm, Power_Index_REG[index], Value);
-
-}
-
-void odm_DynamicTxPower23a(struct dm_odm_t *pDM_Odm)
-{
-}
-
-void odm_DynamicTxPower23a_92C(struct dm_odm_t *pDM_Odm)
-{
-}
-
-void odm_DynamicTxPower23a_92D(struct dm_odm_t *pDM_Odm)
-{
-}
-
 /* 3 ============================================================ */
 /* 3 RSSI Monitor */
 /* 3 ============================================================ */
@@ -1576,22 +1520,6 @@ void odm_RSSIMonitorCheck23aAP(struct dm_odm_t *pDM_Odm)
 {
 }
 
-void ODM_InitAllTimers(struct dm_odm_t *pDM_Odm)
-{
-	setup_timer(&pDM_Odm->DM_SWAT_Table.SwAntennaSwitchTimer,
-		    odm_SwAntDivChkAntSwitchCallback23a, (unsigned long)pDM_Odm);
-}
-
-void ODM_CancelAllTimers(struct dm_odm_t *pDM_Odm)
-{
-	del_timer_sync(&pDM_Odm->DM_SWAT_Table.SwAntennaSwitchTimer);
-}
-
-void ODM_ReleaseAllTimers(struct dm_odm_t *pDM_Odm)
-{
-	ODM_ReleaseTimer(pDM_Odm, &pDM_Odm->DM_SWAT_Table.SwAntennaSwitchTimer);
-}
-
 /* endif */
 /* 3 ============================================================ */
 /* 3 Tx Power Tracking */
@@ -1655,7 +1583,8 @@ void odm_SwAntDivInit(struct dm_odm_t *pDM_Odm)
 {
 }
 
-void ODM_SwAntDivChkPerPktRssi(struct dm_odm_t *pDM_Odm, u8 StationID, struct odm_phy_info *pPhyInfo)
+void ODM_SwAntDivChkPerPktRssi(struct dm_odm_t *pDM_Odm, u8 StationID,
+			       struct phy_info *pPhyInfo)
 {
 }
 
@@ -1739,10 +1668,8 @@ void odm_EdcaTurboCheck23aCE23a(struct dm_odm_t *pDM_Odm)
 	if (pmlmeinfo->assoc_AP_vendor >=  HT_IOT_PEER_MAX)
 		goto dm_CheckEdcaTurbo_EXIT;
 
-#ifdef CONFIG_8723AU_BT_COEXIST
-	if (BT_DisableEDCATurbo(Adapter))
+	if (rtl8723a_BT_disable_EDCA_turbo(Adapter))
 		goto dm_CheckEdcaTurbo_EXIT;
-#endif
 
 	/*  Check if the status needs to be changed. */
 	if ((bbtchange) || (!precvpriv->bIsAnyNonBEPkts)) {
@@ -1774,7 +1701,8 @@ void odm_EdcaTurboCheck23aCE23a(struct dm_odm_t *pDM_Odm)
 				edca_param = EDCAParam[pmlmeinfo->assoc_AP_vendor][trafficIndex];
 			else
 				edca_param = EDCAParam[HT_IOT_PEER_UNKNOWN][trafficIndex];
-			rtw_write32(Adapter, REG_EDCA_BE_PARAM, edca_param);
+			rtl8723au_write32(Adapter, REG_EDCA_BE_PARAM,
+					  edca_param);
 
 			pDM_Odm->DM_EDCA_Table.prv_traffic_idx = trafficIndex;
 		}
@@ -1784,7 +1712,8 @@ void odm_EdcaTurboCheck23aCE23a(struct dm_odm_t *pDM_Odm)
 		/*  Turn Off EDCA turbo here. */
 		/*  Restore original EDCA according to the declaration of AP. */
 		if (pDM_Odm->DM_EDCA_Table.bCurrentTurboEDCA) {
-			rtw_write32(Adapter, REG_EDCA_BE_PARAM, pHalData->AcParam_BE);
+			rtl8723au_write32(Adapter, REG_EDCA_BE_PARAM,
+					  pHalData->AcParam_BE);
 			pDM_Odm->DM_EDCA_Table.bCurrentTurboEDCA = false;
 		}
 	}
@@ -1804,10 +1733,10 @@ u32 GetPSDData(struct dm_odm_t *pDM_Odm, unsigned int point, u8 initial_gain_psd
 	ODM_SetBBReg(pDM_Odm, 0x808, 0x3FF, point);
 
 	/* Start PSD calculation, Reg808[22]= 0->1 */
-	ODM_SetBBReg(pDM_Odm, 0x808, BIT22, 1);
+	ODM_SetBBReg(pDM_Odm, 0x808, BIT(22), 1);
 	/* Need to wait for HW PSD report */
 	udelay(30);
-	ODM_SetBBReg(pDM_Odm, 0x808, BIT22, 0);
+	ODM_SetBBReg(pDM_Odm, 0x808, BIT(22), 0);
 	/* Read PSD report, Reg8B4[15:0] */
 	psd_report = ODM_GetBBReg(pDM_Odm, 0x8B4, bMaskDWord) & 0x0000FFFF;
 
@@ -1845,16 +1774,6 @@ ConvertTo_dB23a(
 }
 
 /*  */
-/*  2011/09/22 MH Add for 92D global spin lock utilization. */
-/*  */
-void
-odm_GlobalAdapterCheck(
-		void
-	)
-{
-}	/*  odm_GlobalAdapterCheck */
-
-/*  */
 /*  Description: */
 /*Set Single/Dual Antenna default setting for products that do not do detection in advance. */
 /*  */
@@ -1941,7 +1860,7 @@ bool ODM_SingleDualAntennaDetection(struct dm_odm_t *pDM_Odm, u8 mode)
 	odm_PHY_SaveAFERegisters(pDM_Odm, AFE_REG_8723A, AFE_Backup, 16);
 
 	/* Set PSD 128 pts */
-	ODM_SetBBReg(pDM_Odm, rFPGA0_PSDFunction, BIT14|BIT15, 0x0);  /* 128 pts */
+	ODM_SetBBReg(pDM_Odm, rFPGA0_PSDFunction, BIT(14) | BIT(15), 0x0);
 
 	/*  To SET CH1 to do */
 	ODM_SetRFReg(pDM_Odm, RF_PATH_A, ODM_CHANNEL, bRFRegOffsetMask, 0x01);     /* Channel 1 */
diff --git a/drivers/staging/rtl8723au/hal/odm_HWConfig.c b/drivers/staging/rtl8723au/hal/odm_HWConfig.c
index 72441709697..82b1b834889 100644
--- a/drivers/staging/rtl8723au/hal/odm_HWConfig.c
+++ b/drivers/staging/rtl8723au/hal/odm_HWConfig.c
@@ -91,7 +91,7 @@ odm_EVMdbToPercentage(
 }
 
 static void odm_RxPhyStatus92CSeries_Parsing(struct dm_odm_t *pDM_Odm,
-					     struct odm_phy_info *pPhyInfo,
+					     struct phy_info *pPhyInfo,
 					     u8 *pPhyStatus,
 					     struct odm_packet_info *pPktinfo)
 {
@@ -281,7 +281,7 @@ void odm_Init_RSSIForDM23a(struct dm_odm_t *pDM_Odm)
 }
 
 static void odm_Process_RSSIForDM(struct dm_odm_t *pDM_Odm,
-				  struct odm_phy_info *pPhyInfo,
+				  struct phy_info *pPhyInfo,
 				  struct odm_packet_info *pPktinfo)
 {
 	s32 UndecoratedSmoothedPWDB, UndecoratedSmoothedCCK;
@@ -347,7 +347,8 @@ static void odm_Process_RSSIForDM(struct dm_odm_t *pDM_Odm,
 							(RSSI_Ave)) / (Rx_Smooth_Factor);
 				}
 			}
-			pEntry->rssi_stat.PacketMap = (pEntry->rssi_stat.PacketMap<<1) | BIT0;
+			pEntry->rssi_stat.PacketMap =
+				(pEntry->rssi_stat.PacketMap<<1) | BIT(0);
 		} else {
 			RSSI_Ave = pPhyInfo->RxPWDBAll;
 
@@ -377,7 +378,8 @@ static void odm_Process_RSSIForDM(struct dm_odm_t *pDM_Odm,
 			pEntry->rssi_stat.ValidBit++;
 
 		for (i = 0; i < pEntry->rssi_stat.ValidBit; i++)
-			OFDM_pkt += (u8)(pEntry->rssi_stat.PacketMap>>i)&BIT0;
+			OFDM_pkt +=
+				(u8)(pEntry->rssi_stat.PacketMap>>i) & BIT(0);
 
 		if (pEntry->rssi_stat.ValidBit == 64) {
 			Weighting = ((OFDM_pkt<<4) > 64)?64:(OFDM_pkt<<4);
@@ -396,7 +398,7 @@ static void odm_Process_RSSIForDM(struct dm_odm_t *pDM_Odm,
 
 /*  Endianness before calling this API */
 static void ODM_PhyStatusQuery23a_92CSeries(struct dm_odm_t *pDM_Odm,
-					 struct odm_phy_info *pPhyInfo,
+					 struct phy_info *pPhyInfo,
 					 u8 *pPhyStatus,
 					 struct odm_packet_info *pPktinfo)
 {
@@ -411,7 +413,7 @@ static void ODM_PhyStatusQuery23a_92CSeries(struct dm_odm_t *pDM_Odm,
 	}
 }
 
-void ODM_PhyStatusQuery23a(struct dm_odm_t *pDM_Odm, struct odm_phy_info *pPhyInfo,
+void ODM_PhyStatusQuery23a(struct dm_odm_t *pDM_Odm, struct phy_info *pPhyInfo,
 			   u8 *pPhyStatus, struct odm_packet_info *pPktinfo)
 {
 	ODM_PhyStatusQuery23a_92CSeries(pDM_Odm, pPhyInfo, pPhyStatus, pPktinfo);
@@ -426,12 +428,9 @@ void ODM_MacStatusQuery23a(struct dm_odm_t *pDM_Odm, u8 *pMacStatus, u8 MacID,
 
 }
 
-enum hal_status
-ODM_ConfigRFWithHeaderFile23a(
-	struct dm_odm_t *pDM_Odm,
-	enum RF_RADIO_PATH	Content,
-	enum RF_RADIO_PATH	eRFPath
-  )
+int ODM_ConfigRFWithHeaderFile23a(struct dm_odm_t *pDM_Odm,
+				  enum RF_RADIO_PATH Content,
+				  enum RF_RADIO_PATH eRFPath)
 {
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_LOUD,
 		     ("===>ODM_ConfigRFWithHeaderFile23a\n"));
@@ -446,14 +445,11 @@ ODM_ConfigRFWithHeaderFile23a(
 	}
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_TRACE,
 		     ("ODM_ConfigRFWithHeaderFile23a: Radio No %x\n", eRFPath));
-	return HAL_STATUS_SUCCESS;
+	return _SUCCESS;
 }
 
-enum hal_status
-ODM_ConfigBBWithHeaderFile23a(
-	struct dm_odm_t *pDM_Odm,
-	enum odm_bb_config_type		ConfigType
-	)
+int ODM_ConfigBBWithHeaderFile23a(struct dm_odm_t *pDM_Odm,
+				  enum odm_bb_config_type ConfigType)
 {
 	if (pDM_Odm->SupportICType == ODM_RTL8723A) {
 		if (ConfigType == CONFIG_BB_PHY_REG)
@@ -465,17 +461,12 @@ ODM_ConfigBBWithHeaderFile23a(
 		ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_LOUD,
 			     (" ===> phy_ConfigBBWithHeaderFile() agc:Rtl8723PHY_REG_1TArray\n"));
 	}
-	return HAL_STATUS_SUCCESS;
+	return _SUCCESS;
 }
 
-enum hal_status
-ODM_ConfigMACWithHeaderFile23a(
-	struct dm_odm_t *pDM_Odm
-	)
+int ODM_ConfigMACWithHeaderFile23a(struct dm_odm_t *pDM_Odm)
 {
-	u8 result = HAL_STATUS_SUCCESS;
-
 	if (pDM_Odm->SupportICType == ODM_RTL8723A)
 		READ_AND_CONFIG_MP(8723A, _MAC_REG_);
-	return result;
+	return _SUCCESS;
 }
diff --git a/drivers/staging/rtl8723au/hal/odm_interface.c b/drivers/staging/rtl8723au/hal/odm_interface.c
index bef1269749d..f03f6d4a388 100644
--- a/drivers/staging/rtl8723au/hal/odm_interface.c
+++ b/drivers/staging/rtl8723au/hal/odm_interface.c
@@ -21,6 +21,7 @@
 /*  */
 /*  ODM IO Relative API. */
 /*  */
+#include <usb_ops_linux.h>
 
 u8 ODM_Read1Byte(struct dm_odm_t *pDM_Odm,
 	u32			RegAddr
@@ -28,59 +29,42 @@ u8 ODM_Read1Byte(struct dm_odm_t *pDM_Odm,
 {
 	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
 
-	return rtw_read8(Adapter, RegAddr);
+	return rtl8723au_read8(Adapter, RegAddr);
 }
 
-u16 ODM_Read2Byte(struct dm_odm_t *pDM_Odm,
-	u32			RegAddr
-	)
+u16 ODM_Read2Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr)
 {
 	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
 
-	return rtw_read16(Adapter, RegAddr);
+	return rtl8723au_read16(Adapter, RegAddr);
 }
 
-u32 ODM_Read4Byte(struct dm_odm_t *pDM_Odm,
-	u32			RegAddr
-	)
+u32 ODM_Read4Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr)
 {
 	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
 
-	return rtw_read32(Adapter, RegAddr);
+	return rtl8723au_read32(Adapter, RegAddr);
 }
 
-void ODM_Write1Byte(
-	struct dm_odm_t *pDM_Odm,
-	u32			RegAddr,
-	u8			Data
-	)
+void ODM_Write1Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr, u8 Data)
 {
 	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
 
-	rtw_write8(Adapter, RegAddr, Data);
+	rtl8723au_write8(Adapter, RegAddr, Data);
 }
 
-void ODM_Write2Byte(
-	struct dm_odm_t *pDM_Odm,
-	u32			RegAddr,
-	u16			Data
-	)
+void ODM_Write2Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr, u16 Data)
 {
 	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
 
-	rtw_write16(Adapter, RegAddr, Data);
+	rtl8723au_write16(Adapter, RegAddr, Data);
 }
 
-void ODM_Write4Byte(
-	struct dm_odm_t *pDM_Odm,
-	u32			RegAddr,
-	u32			Data
-	)
+void ODM_Write4Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr, u32 Data)
 {
 	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
 
-	rtw_write32(Adapter, RegAddr, Data);
-
+	rtl8723au_write32(Adapter, RegAddr, Data);
 }
 
 void ODM_SetMACReg(
@@ -153,84 +137,3 @@ u32 ODM_GetRFReg(
 
 	return PHY_QueryRFReg(Adapter, eRFPath, RegAddr, BitMask);
 }
-
-/*  */
-/*  ODM Memory relative API. */
-/*  */
-void ODM_AllocateMemory(
-	struct dm_odm_t *pDM_Odm,
-	void **pPtr,
-	u32		length
-	)
-{
-	*pPtr = rtw_zvmalloc(length);
-}
-
-/*  length could be ignored, used to detect memory leakage. */
-void ODM_FreeMemory(
-	struct dm_odm_t *pDM_Odm,
-	void *pPtr,
-	u32		length
-	)
-{
-	rtw_vmfree(pPtr, length);
-}
-
-/*  */
-/*  ODM MISC relative API. */
-/*  */
-void
-ODM_AcquireSpinLock(
-	struct dm_odm_t *pDM_Odm,
-	enum rt_spinlock_type	type
-	)
-{
-}
-
-void ODM_ReleaseSpinLock(
-	struct dm_odm_t *pDM_Odm,
-	enum rt_spinlock_type	type
-	)
-{
-}
-
-/*  */
-/*  Work item relative API. FOr MP driver only~! */
-/*  */
-void ODM_InitializeWorkItem(
-	struct dm_odm_t *pDM_Odm,
-	void *pRtWorkItem,
-	RT_WORKITEM_CALL_BACK		RtWorkItemCallback,
-	void *pContext,
-	const char *szID
-	)
-{
-}
-
-/*  */
-/*  ODM Timer relative API. */
-/*  */
-void ODM_SetTimer(struct dm_odm_t *pDM_Odm, struct timer_list *pTimer, u32 msDelay)
-{
-	mod_timer(pTimer, jiffies + msecs_to_jiffies(msDelay)); /* ms */
-}
-
-void ODM_ReleaseTimer(struct dm_odm_t *pDM_Odm, struct timer_list *pTimer)
-{
-}
-
-/*  */
-/*  ODM FW relative API. */
-/*  */
-u32 ODM_FillH2CCmd(
-	u8 *pH2CBuffer,
-	u32		H2CBufferLen,
-	u32		CmdNum,
-	u32 *pElementID,
-	u32 *pCmdLen,
-	u8 **pCmbBuffer,
-	u8 *CmdStartSeq
-	)
-{
-	return	true;
-}
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_bt-coexist.c b/drivers/staging/rtl8723au/hal/rtl8723a_bt-coexist.c
index 9d738d79de4..c0010538909 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_bt-coexist.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_bt-coexist.c
@@ -15,11 +15,10 @@
 #include <drv_types.h>
 #include <rtl8723a_hal.h>
 #include <rtw_ioctl_set.h>
+#include <usb_ops_linux.h>
 
 #define DIS_PS_RX_BCN
 
-#ifdef CONFIG_8723AU_BT_COEXIST
-
 u32 BTCoexDbgLevel = _bt_dbg_off_;
 
 #define RTPRINT(_Comp, _Level, Fmt)\
@@ -70,7 +69,6 @@ if ((BTCoexDbgLevel == _bt_dbg_on_)) {\
 		printk(_TitleString);					\
 		printk(": %d, <%s>\n", _Len, buffer);			\
 	}
-#endif
 
 #define DCMD_Printf(...)
 #define RT_ASSERT(...)
@@ -100,7 +98,6 @@ enum {
 
 /*  power saving */
 
-#ifdef __BT_C__ /*  COMMOM/BT.c */
 /*  ===== Below this line is sync from SD7 driver COMMOM/BT.c ===== */
 
 static u8 BT_Operation(struct rtw_adapter *padapter)
@@ -138,14 +135,14 @@ void BT_SignalCompensation(struct rtw_adapter *padapter, u8 *rssi_wifi, u8 *rssi
 	BTDM_SignalCompensation(padapter, rssi_wifi, rssi_bt);
 }
 
-void BT_WifiScanNotify(struct rtw_adapter *padapter, u8 scanType)
+void rtl8723a_BT_wifiscan_notify(struct rtw_adapter *padapter, u8 scanType)
 {
 	BTHCI_WifiScanNotify(padapter, scanType);
 	BTDM_CheckAntSelMode(padapter);
 	BTDM_WifiScanNotify(padapter, scanType);
 }
 
-void BT_WifiAssociateNotify(struct rtw_adapter *padapter, u8 action)
+void rtl8723a_BT_wifiassociate_notify(struct rtw_adapter *padapter, u8 action)
 {
 	/*  action : */
 	/*  true = associate start */
@@ -156,36 +153,12 @@ void BT_WifiAssociateNotify(struct rtw_adapter *padapter, u8 action)
 	BTDM_WifiAssociateNotify(padapter, action);
 }
 
-void BT_WifiMediaStatusNotify(struct rtw_adapter *padapter, enum rt_media_status mstatus)
-{
-	BTDM_MediaStatusNotify(padapter, mstatus);
-}
-
-void BT_SpecialPacketNotify(struct rtw_adapter *padapter)
-{
-	BTDM_ForDhcp(padapter);
-}
-
 void BT_HaltProcess(struct rtw_adapter *padapter)
 {
 	BTDM_ForHalt(padapter);
 }
 
-void BT_LpsLeave(struct rtw_adapter *padapter)
-{
-	BTDM_LpsLeave(padapter);
-}
-
 /*  ===== End of sync from SD7 driver COMMOM/BT.c ===== */
-#endif
-
-#ifdef __BT_HANDLEPACKET_C__ /*  COMMOM/bt_handlepacket.c */
-/*  ===== Below this line is sync from SD7 driver COMMOM/bt_handlepacket.c ===== */
-
-/*  ===== End of sync from SD7 driver COMMOM/bt_handlepacket.c ===== */
-#endif
-
-#ifdef __BT_HCI_C__ /*  COMMOM/bt_hci.c */
 
 #define i64fmt		"ll"
 #define UINT64_C(v)  (v)
@@ -3944,7 +3917,6 @@ bthci_CmdAMPTestEnd(struct rtw_adapter *padapter,
 	enum hci_status status = HCI_STATUS_SUCCESS;
 	struct bt_30info *pBTInfo = GET_BT_INFO(padapter);
 	struct bt_hci_info *pBtHciInfo = &pBTInfo->BtHciInfo;
-	u8 bFilterOutNonAssociatedBSSID = true;
 
 	if (!pBtHciInfo->bInTestMode) {
 		RTPRINT(FIOCTL, (IOCTL_BT_EVENT|IOCTL_BT_LOGO), ("Not in Test mode, return status = HCI_STATUS_CMD_DISALLOW\n"));
@@ -3956,7 +3928,7 @@ bthci_CmdAMPTestEnd(struct rtw_adapter *padapter,
 
 	del_timer_sync(&pBTInfo->BTTestSendPacketTimer);
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_CHECK_BSSID, (u8 *)(&bFilterOutNonAssociatedBSSID));
+	rtl8723a_check_bssid(padapter, true);
 
 	/* send command complete event here when all data are received. */
 	{
@@ -4057,8 +4029,7 @@ bthci_CmdAMPTestCommand(struct rtw_adapter *padapter,
 			  jiffies + msecs_to_jiffies(50));
 		RTPRINT(FIOCTL, (IOCTL_BT_EVENT|IOCTL_BT_LOGO), ("TX Single Test \n"));
 	} else if (pBtHciInfo->TestScenario == 0x02) {
-		u8 bFilterOutNonAssociatedBSSID = false;
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_CHECK_BSSID, (u8 *)(&bFilterOutNonAssociatedBSSID));
+		rtl8723a_check_bssid(padapter, false);
 		RTPRINT(FIOCTL, (IOCTL_BT_EVENT|IOCTL_BT_LOGO), ("Receive Frame Test \n"));
 	}
 
@@ -4674,10 +4645,10 @@ bthci_StateConnected(struct rtw_adapter *padapter,
 
 		/*  for rate adaptive */
 
-		if (padapter->HalFunc.UpdateRAMaskHandler)
-			padapter->HalFunc.UpdateRAMaskHandler(padapter, MAX_FW_SUPPORT_MACID_NUM-1-EntryNum, 0);
+		rtl8723a_update_ramask(padapter,
+				       MAX_FW_SUPPORT_MACID_NUM-1-EntryNum, 0);
 
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_BASIC_RATE, padapter->mlmepriv.cur_network.network.SupportedRates);
+		HalSetBrateCfg23a(padapter, padapter->mlmepriv.cur_network.network.SupportedRates);
 		BTDM_SetFwChnlInfo(padapter, RT_MEDIA_CONNECT);
 		break;
 	default:
@@ -5215,9 +5186,6 @@ BTHCI_HandleHCICMD(
 }
 
 /*  ===== End of sync from SD7 driver COMMOM/bt_hci.c ===== */
-#endif
-
-#ifdef __HALBTC87231ANT_C__ /*  HAL/BTCoexist/HalBtc87231Ant.c */
 
 static const char *const BtStateString[] = {
 	"BT_DISABLED",
@@ -5286,7 +5254,7 @@ static void btdm_1AntTSFSwitch(struct rtw_adapter *padapter, u8 enable)
 {
 	u8 oldVal, newVal;
 
-	oldVal = rtw_read8(padapter, 0x550);
+	oldVal = rtl8723au_read8(padapter, 0x550);
 
 	if (enable)
 		newVal = oldVal | EN_BCN_FUNCTION;
@@ -5294,7 +5262,7 @@ static void btdm_1AntTSFSwitch(struct rtw_adapter *padapter, u8 enable)
 		newVal = oldVal & ~EN_BCN_FUNCTION;
 
 	if (oldVal != newVal)
-		rtw_write8(padapter, 0x550, newVal);
+		rtl8723au_write8(padapter, 0x550, newVal);
 }
 
 static u8 btdm_Is1AntPsTdmaStateChange(struct rtw_adapter *padapter)
@@ -5393,8 +5361,8 @@ btdm_1AntPsTdma(
 		case 29: /*  WiFi DHCP/Site Survey & BT ACL busy */
 			if (btdm_Is1AntPsTdmaStateChange(padapter)) {
 				BTDM_SetFw3a(padapter, 0xeb, 0x1a, 0x1a, 0x01, 0x18);
-				rtw_write32(padapter, 0x6c0, 0x5afa5afa);
-				rtw_write32(padapter, 0x6c4, 0x5afa5afa);
+				rtl8723au_write32(padapter, 0x6c0, 0x5afa5afa);
+				rtl8723au_write32(padapter, 0x6c4, 0x5afa5afa);
 			}
 			break;
 		case 30: /*  WiFi idle & BT Inquiry */
@@ -5437,7 +5405,8 @@ btdm_1AntPsTdma(
 				/*  Antenna control by PTA, 0x870 = 0x310 */
 				BTDM_SetFw3a(padapter, 0x0, 0x0, 0x0, 0x8, 0x0);
 			}
-			rtw_write16(padapter, 0x860, 0x210); /*  Switch Antenna to BT */
+			/*  Switch Antenna to BT */
+			rtl8723au_write16(padapter, 0x860, 0x210);
 			RTPRINT(FBT, BT_TRACE, ("[BTCoex], 0x860 = 0x210, Switch Antenna to BT\n"));
 			break;
 		case 9:
@@ -5445,7 +5414,8 @@ btdm_1AntPsTdma(
 				/*  Antenna control by PTA, 0x870 = 0x310 */
 				BTDM_SetFw3a(padapter, 0x0, 0x0, 0x0, 0x8, 0x0);
 			}
-			rtw_write16(padapter, 0x860, 0x110); /*  Switch Antenna to WiFi */
+			/*  Switch Antenna to WiFi */
+			rtl8723au_write16(padapter, 0x860, 0x110);
 			RTPRINT(FBT, BT_TRACE, ("[BTCoex], 0x860 = 0x110, Switch Antenna to WiFi\n"));
 			break;
 		}
@@ -5531,7 +5501,7 @@ _btdm_1AntSetPSTDMA(struct rtw_adapter *padapter, u8 bPSEn, u8 smartps,
 			if (!bTDMAOn) {
 				btdm_1AntPsTdma(padapter, false, tdmaType);
 			} else {
-				if ((BT_IsBtDisabled(padapter)) ||
+				if (!rtl8723a_BT_enabled(padapter) ||
 				    (pHalData->bt_coexist.halCoex8723.c2hBtInfo == BT_INFO_STATE_NO_CONNECTION) ||
 				    (pHalData->bt_coexist.halCoex8723.c2hBtInfo == BT_INFO_STATE_CONNECT_IDLE) ||
 				    (tdmaType == 29))
@@ -5575,24 +5545,28 @@ static void btdm_1AntWifiParaAdjust(struct rtw_adapter *padapter, u8 bEnable)
 static void btdm_1AntPtaParaReload(struct rtw_adapter *padapter)
 {
 	/*  PTA parameter */
-	rtw_write8(padapter, 0x6cc, 0x0);			/*  1-Ant coex */
-	rtw_write32(padapter, 0x6c8, 0xffff);		/*  wifi break table */
-	rtw_write32(padapter, 0x6c4, 0x55555555);	/*  coex table */
+	rtl8723au_write8(padapter, 0x6cc, 0x0);		/*  1-Ant coex */
+	rtl8723au_write32(padapter, 0x6c8, 0xffff);	/*  wifi break table */
+	rtl8723au_write32(padapter, 0x6c4, 0x55555555);	/*  coex table */
 
 	/*  Antenna switch control parameter */
-	rtw_write32(padapter, 0x858, 0xaaaaaaaa);
+	rtl8723au_write32(padapter, 0x858, 0xaaaaaaaa);
 	if (IS_8723A_A_CUT(GET_HAL_DATA(padapter)->VersionID)) {
-		rtw_write32(padapter, 0x870, 0x0);	/*  SPDT(connected with TRSW) control by hardware PTA */
-		rtw_write8(padapter, 0x40, 0x24);
+		/*  SPDT(connected with TRSW) control by hardware PTA */
+		rtl8723au_write32(padapter, 0x870, 0x0);
+		rtl8723au_write8(padapter, 0x40, 0x24);
 	} else {
-		rtw_write8(padapter, 0x40, 0x20);
-		rtw_write16(padapter, 0x860, 0x210);	/*  set antenna at bt side if ANTSW is software control */
-		rtw_write32(padapter, 0x870, 0x300);	/*  SPDT(connected with TRSW) control by hardware PTA */
-		rtw_write32(padapter, 0x874, 0x22804000);	/*  ANTSW keep by GNT_BT */
+		rtl8723au_write8(padapter, 0x40, 0x20);
+		/*  set antenna at bt side if ANTSW is software control */
+		rtl8723au_write16(padapter, 0x860, 0x210);
+		/*  SPDT(connected with TRSW) control by hardware PTA */
+		rtl8723au_write32(padapter, 0x870, 0x300);
+		/*  ANTSW keep by GNT_BT */
+		rtl8723au_write32(padapter, 0x874, 0x22804000);
 	}
 
 	/*  coexistence parameters */
-	rtw_write8(padapter, 0x778, 0x1);	/*  enable RTK mode PTA */
+	rtl8723au_write8(padapter, 0x778, 0x1);	/*  enable RTK mode PTA */
 
 	/*  BT don't ignore WLAN_Act */
 	btdm_SetFwIgnoreWlanAct(padapter, false);
@@ -5764,13 +5738,17 @@ static void btdm_1AntCoexProcessForWifiConnect(struct rtw_adapter *padapter)
 	pBtdm8723 = &pBtCoex->btdm1Ant;
 	BtState = pBtCoex->c2hBtInfo;
 
-	RTPRINT(FBT, BT_TRACE, ("[BTCoex], WiFi is %s\n", BTDM_IsWifiBusy(padapter)?"Busy":"IDLE"));
-	RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT is %s\n", BtStateString[BtState]));
+	RTPRINT(FBT, BT_TRACE, ("[BTCoex], WiFi is %s\n",
+				BTDM_IsWifiBusy(padapter)?"Busy":"IDLE"));
+	RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT is %s\n",
+				BtStateString[BtState]));
 
 	padapter->pwrctrlpriv.btcoex_rfon = false;
 
-	if ((!BTDM_IsWifiBusy(padapter)) && (!check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE)) &&
-	    ((BtState == BT_INFO_STATE_NO_CONNECTION) || (BtState == BT_INFO_STATE_CONNECT_IDLE))) {
+	if (!BTDM_IsWifiBusy(padapter) &&
+	    !check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE) &&
+	    (BtState == BT_INFO_STATE_NO_CONNECTION ||
+	     BtState == BT_INFO_STATE_CONNECT_IDLE)) {
 		switch (BtState) {
 		case BT_INFO_STATE_NO_CONNECTION:
 			_btdm_1AntSetPSTDMA(padapter, true, 2, 0x26, false, 9);
@@ -5785,45 +5763,59 @@ static void btdm_1AntCoexProcessForWifiConnect(struct rtw_adapter *padapter)
 		case BT_INFO_STATE_CONNECT_IDLE:
 			/*  WiFi is Busy */
 			btdm_1AntSetPSTDMA(padapter, false, 0, true, 5);
-			rtw_write32(padapter, 0x6c0, 0x5a5a5a5a);
-			rtw_write32(padapter, 0x6c4, 0x5a5a5a5a);
+			rtl8723au_write32(padapter, 0x6c0, 0x5a5a5a5a);
+			rtl8723au_write32(padapter, 0x6c4, 0x5a5a5a5a);
 			break;
 		case BT_INFO_STATE_ACL_INQ_OR_PAG:
-			RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT PROFILE is BT_INFO_STATE_ACL_INQ_OR_PAG\n"));
+			RTPRINT(FBT, BT_TRACE,
+				("[BTCoex], BT PROFILE is "
+				 "BT_INFO_STATE_ACL_INQ_OR_PAG\n"));
 		case BT_INFO_STATE_INQ_OR_PAG:
 			padapter->pwrctrlpriv.btcoex_rfon = true;
 			btdm_1AntSetPSTDMA(padapter, true, 0, true, 30);
 			break;
 		case BT_INFO_STATE_SCO_ONLY_BUSY:
 		case BT_INFO_STATE_ACL_SCO_BUSY:
-			if (true == pBtCoex->bC2hBtInquiryPage) {
-				btdm_1AntSetPSTDMA(padapter, false, 0, true, 32);
-			} else {
+			if (true == pBtCoex->bC2hBtInquiryPage)
+				btdm_1AntSetPSTDMA(padapter, false, 0,
+						   true, 32);
+			else {
 #ifdef BTCOEX_CMCC_TEST
-				btdm_1AntSetPSTDMA(padapter, false, 0, true, 23);
+				btdm_1AntSetPSTDMA(padapter, false, 0,
+						   true, 23);
 #else /*  !BTCOEX_CMCC_TEST */
-				btdm_1AntSetPSTDMA(padapter, false, 0, false, 8);
-				rtw_write32(padapter, 0x6c0, 0x5a5a5a5a);
-				rtw_write32(padapter, 0x6c4, 0x5a5a5a5a);
+				btdm_1AntSetPSTDMA(padapter, false, 0,
+						   false, 8);
+				rtl8723au_write32(padapter, 0x6c0, 0x5a5a5a5a);
+				rtl8723au_write32(padapter, 0x6c4, 0x5a5a5a5a);
 #endif /*  !BTCOEX_CMCC_TEST */
 			}
 			break;
 		case BT_INFO_STATE_ACL_ONLY_BUSY:
 			padapter->pwrctrlpriv.btcoex_rfon = true;
 			if (pBtCoex->c2hBtProfile == BT_INFO_HID) {
-				RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT PROFILE is HID\n"));
+				RTPRINT(FBT, BT_TRACE,
+					("[BTCoex], BT PROFILE is HID\n"));
 				btdm_1AntSetPSTDMA(padapter, true, 0, true, 31);
 			} else if (pBtCoex->c2hBtProfile == BT_INFO_FTP) {
-				RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT PROFILE is FTP/OPP\n"));
+				RTPRINT(FBT, BT_TRACE,
+					("[BTCoex], BT PROFILE is FTP/OPP\n"));
 				btdm_1AntSetPSTDMA(padapter, true, 0, true, 3);
 			} else if (pBtCoex->c2hBtProfile == (BT_INFO_A2DP|BT_INFO_FTP)) {
-				RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT PROFILE is A2DP_FTP\n"));
+				RTPRINT(FBT, BT_TRACE,
+					("[BTCoex], BT PROFILE is A2DP_FTP\n"));
 				btdm_1AntSetPSTDMA(padapter, true, 0, true, 11);
 			} else {
 				if (pBtCoex->c2hBtProfile == BT_INFO_A2DP)
-					RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT PROFILE is A2DP\n"));
+					RTPRINT(FBT, BT_TRACE,
+						("[BTCoex], BT PROFILE is "
+						 "A2DP\n"));
 				else
-					RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT PROFILE is UNKNOWN(0x%02X)! Use A2DP Profile\n", pBtCoex->c2hBtProfile));
+					RTPRINT(FBT, BT_TRACE,
+						("[BTCoex], BT PROFILE is "
+						 "UNKNOWN(0x%02X)! Use A2DP "
+						 "Profile\n",
+						 pBtCoex->c2hBtProfile));
 				btdm_1AntTdmaDurationAdjustForACL(padapter);
 			}
 			break;
@@ -5833,13 +5825,14 @@ static void btdm_1AntCoexProcessForWifiConnect(struct rtw_adapter *padapter)
 	pBtdm8723->psTdmaGlobalCnt++;
 }
 
-static void btdm_1AntUpdateHalRAMask(struct rtw_adapter *padapter, u32 mac_id, u32 filter)
+static void
+btdm_1AntUpdateHalRAMask(struct rtw_adapter *padapter, u32 mac_id, u32 filter)
 {
 	u8 init_rate = 0;
 	u8 raid;
 	u32 mask;
 	u8 shortGIrate = false;
-	int	supportRateNum = 0;
+	int supportRateNum = 0;
 	struct sta_info	*psta;
 	struct hal_data_8723a *pHalData;
 	struct dm_priv *pdmpriv;
@@ -5847,7 +5840,8 @@ static void btdm_1AntUpdateHalRAMask(struct rtw_adapter *padapter, u32 mac_id, u
 	struct mlme_ext_info *pmlmeinfo;
 	struct wlan_bssid_ex *cur_network;
 
-	RTPRINT(FBT, BT_TRACE, ("[BTCoex], %s, MACID =%d, filter = 0x%08x!!\n", __func__, mac_id, filter));
+	RTPRINT(FBT, BT_TRACE, ("[BTCoex], %s, MACID =%d, filter = 0x%08x!!\n",
+				__func__, mac_id, filter));
 
 	pHalData = GET_HAL_DATA(padapter);
 	pdmpriv = &pHalData->dmpriv;
@@ -5856,13 +5850,15 @@ static void btdm_1AntUpdateHalRAMask(struct rtw_adapter *padapter, u32 mac_id, u
 	cur_network = &pmlmeinfo->network;
 
 	if (mac_id >= NUM_STA) { /* CAM_SIZE */
-		RTPRINT(FBT, BT_TRACE, ("[BTCoex], %s, MACID =%d illegal!!\n", __func__, mac_id));
+		RTPRINT(FBT, BT_TRACE, ("[BTCoex], %s, MACID =%d illegal!!\n",
+					__func__, mac_id));
 		return;
 	}
 
 	psta = pmlmeinfo->FW_sta_info[mac_id].psta;
-	if (psta == NULL) {
-		RTPRINT(FBT, BT_TRACE, ("[BTCoex], %s, Can't find station!!\n", __func__));
+	if (!psta) {
+		RTPRINT(FBT, BT_TRACE, ("[BTCoex], %s, Can't find station!!\n",
+					__func__));
 		return;
 	}
 
@@ -5870,19 +5866,26 @@ static void btdm_1AntUpdateHalRAMask(struct rtw_adapter *padapter, u32 mac_id, u
 
 	switch (mac_id) {
 	case 0:/*  for infra mode */
-		supportRateNum = rtw_get_rateset_len23a(cur_network->SupportedRates);
-		mask = update_supported_rate23a(cur_network->SupportedRates, supportRateNum);
-		mask |= (pmlmeinfo->HT_enable) ? update_MSC_rate23a(&pmlmeinfo->HT_caps):0;
+		supportRateNum =
+			rtw_get_rateset_len23a(cur_network->SupportedRates);
+		mask = update_supported_rate23a(cur_network->SupportedRates,
+						supportRateNum);
+		mask |= (pmlmeinfo->HT_enable) ?
+			update_MSC_rate23a(&pmlmeinfo->HT_caps):0;
 		if (support_short_GI23a(padapter, &pmlmeinfo->HT_caps))
 			shortGIrate = true;
 		break;
 	case 1:/* for broadcast/multicast */
-		supportRateNum = rtw_get_rateset_len23a(pmlmeinfo->FW_sta_info[mac_id].SupportedRates);
-		mask = update_basic_rate23a(cur_network->SupportedRates, supportRateNum);
+		supportRateNum = rtw_get_rateset_len23a(
+			pmlmeinfo->FW_sta_info[mac_id].SupportedRates);
+		mask = update_basic_rate23a(cur_network->SupportedRates,
+					    supportRateNum);
 		break;
 	default: /* for each sta in IBSS */
-		supportRateNum = rtw_get_rateset_len23a(pmlmeinfo->FW_sta_info[mac_id].SupportedRates);
-		mask = update_supported_rate23a(cur_network->SupportedRates, supportRateNum);
+		supportRateNum = rtw_get_rateset_len23a(
+			pmlmeinfo->FW_sta_info[mac_id].SupportedRates);
+		mask = update_supported_rate23a(cur_network->SupportedRates,
+						supportRateNum);
 		break;
 	}
 	mask |= ((raid<<28)&0xf0000000);
@@ -5899,22 +5902,24 @@ static void btdm_1AntUpdateHalRAMask(struct rtw_adapter *padapter, u32 mac_id, u
 			arg |= BIT(5);
 
 		RTPRINT(FBT, BT_TRACE,
-			("[BTCoex], Update FW RAID entry, MASK = 0x%08x, arg = 0x%02x\n",
-			mask, arg));
+			("[BTCoex], Update FW RAID entry, MASK = 0x%08x, "
+			 "arg = 0x%02x\n", mask, arg));
 
 		rtl8723a_set_raid_cmd(padapter, mask, arg);
 	} else {
 		if (shortGIrate)
 			init_rate |= BIT(6);
 
-		rtw_write8(padapter, (REG_INIDATA_RATE_SEL+mac_id), init_rate);
+		rtl8723au_write8(padapter, REG_INIDATA_RATE_SEL + mac_id,
+				 init_rate);
 	}
 
 	psta->init_rate = init_rate;
 	pdmpriv->INIDATA_RATE[mac_id] = init_rate;
 }
 
-static void btdm_1AntUpdateHalRAMaskForSCO(struct rtw_adapter *padapter, u8 forceUpdate)
+static void
+btdm_1AntUpdateHalRAMaskForSCO(struct rtw_adapter *padapter, u8 forceUpdate)
 {
 	struct btdm_8723a_1ant *pBtdm8723;
 	struct sta_priv *pstapriv;
@@ -5925,7 +5930,7 @@ static void btdm_1AntUpdateHalRAMaskForSCO(struct rtw_adapter *padapter, u8 forc
 
 	pBtdm8723 = &GET_HAL_DATA(padapter)->bt_coexist.halCoex8723.btdm1Ant;
 
-	if ((pBtdm8723->bRAChanged == true) && (forceUpdate == false))
+	if (pBtdm8723->bRAChanged == true && forceUpdate == false)
 		return;
 
 	pstapriv = &padapter->stapriv;
@@ -5968,9 +5973,13 @@ static void btdm_1AntRecoverHalRAMask(struct rtw_adapter *padapter)
 
 static void
 btdm_1AntBTStateChangeHandler(struct rtw_adapter *padapter,
-			      enum bt_state_1ant oldState, enum bt_state_1ant newState)
+			      enum bt_state_1ant oldState,
+			      enum bt_state_1ant newState)
 {
-	RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT state change, %s => %s\n", BtStateString[oldState], BtStateString[newState]));
+	struct hal_data_8723a *phaldata;
+	RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT state change, %s => %s\n",
+				BtStateString[oldState],
+				BtStateString[newState]));
 
 	/*  BT default ignore wlan active, */
 	/*  WiFi MUST disable this when BT is enable */
@@ -5987,22 +5996,23 @@ btdm_1AntBTStateChangeHandler(struct rtw_adapter *padapter,
 			btdm_1AntRecoverHalRAMask(padapter);
 		}
 	} else {
-		GET_HAL_DATA(padapter)->bt_coexist.halCoex8723.btdm1Ant.bRAChanged = false;
+		phaldata = GET_HAL_DATA(padapter);
+		phaldata->bt_coexist.halCoex8723.btdm1Ant.bRAChanged = false;
 	}
 
 	if (oldState == newState)
 		return;
 
 	if (oldState == BT_INFO_STATE_ACL_ONLY_BUSY) {
-		struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-		pHalData->bt_coexist.halCoex8723.btdm1Ant.psTdmaMonitorCnt = 0;
-		pHalData->bt_coexist.halCoex8723.btdm1Ant.psTdmaMonitorCntForSCO = 0;
+		struct hal_data_8723a *Hal = GET_HAL_DATA(padapter);
+		Hal->bt_coexist.halCoex8723.btdm1Ant.psTdmaMonitorCnt = 0;
+		Hal->bt_coexist.halCoex8723.btdm1Ant.psTdmaMonitorCntForSCO = 0;
 	}
 
 	if ((oldState == BT_INFO_STATE_SCO_ONLY_BUSY) ||
 	    (oldState == BT_INFO_STATE_ACL_SCO_BUSY)) {
-		struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-		pHalData->bt_coexist.halCoex8723.btdm1Ant.psTdmaMonitorCntForSCO = 0;
+		struct hal_data_8723a *Hal = GET_HAL_DATA(padapter);
+		Hal->bt_coexist.halCoex8723.btdm1Ant.psTdmaMonitorCntForSCO = 0;
 	}
 
 	/*  Active 2Ant mechanism when BT Connected */
@@ -6010,14 +6020,16 @@ btdm_1AntBTStateChangeHandler(struct rtw_adapter *padapter,
 	    (oldState == BT_INFO_STATE_NO_CONNECTION)) {
 		if ((newState != BT_INFO_STATE_DISABLED) &&
 		    (newState != BT_INFO_STATE_NO_CONNECTION)) {
-			BTDM_SetSwRfRxLpfCorner(padapter, BT_RF_RX_LPF_CORNER_SHRINK);
+			BTDM_SetSwRfRxLpfCorner(padapter,
+						BT_RF_RX_LPF_CORNER_SHRINK);
 			BTDM_AGCTable(padapter, BT_AGCTABLE_ON);
 			BTDM_BBBackOffLevel(padapter, BT_BB_BACKOFF_ON);
 		}
 	} else {
 		if ((newState == BT_INFO_STATE_DISABLED) ||
 		    (newState == BT_INFO_STATE_NO_CONNECTION)) {
-			BTDM_SetSwRfRxLpfCorner(padapter, BT_RF_RX_LPF_CORNER_RESUME);
+			BTDM_SetSwRfRxLpfCorner(padapter,
+						BT_RF_RX_LPF_CORNER_RESUME);
 			BTDM_AGCTable(padapter, BT_AGCTABLE_OFF);
 			BTDM_BBBackOffLevel(padapter, BT_BB_BACKOFF_OFF);
 		}
@@ -6034,21 +6046,27 @@ static void btdm_1AntBtCoexistHandler(struct rtw_adapter *padapter)
 	pBtCoex8723 = &pHalData->bt_coexist.halCoex8723;
 	pBtdm8723 = &pBtCoex8723->btdm1Ant;
 	padapter->pwrctrlpriv.btcoex_rfon = false;
-	if (BT_IsBtDisabled(padapter)) {
+	if (!rtl8723a_BT_enabled(padapter)) {
 		RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT is disabled\n"));
 
 		if (BTDM_IsWifiConnectionExist(padapter)) {
-			RTPRINT(FBT, BT_TRACE, ("[BTCoex], wifi is connected\n"));
+			RTPRINT(FBT, BT_TRACE,
+				("[BTCoex], wifi is connected\n"));
 
 			if (BTDM_IsWifiBusy(padapter)) {
-				RTPRINT(FBT, BT_TRACE, ("[BTCoex], Wifi is busy\n"));
-				btdm_1AntSetPSTDMA(padapter, false, 0, false, 9);
+				RTPRINT(FBT, BT_TRACE,
+					("[BTCoex], Wifi is busy\n"));
+				btdm_1AntSetPSTDMA(padapter, false, 0,
+						   false, 9);
 			} else {
-				RTPRINT(FBT, BT_TRACE, ("[BTCoex], Wifi is idle\n"));
-				_btdm_1AntSetPSTDMA(padapter, true, 2, 1, false, 9);
+				RTPRINT(FBT, BT_TRACE,
+					("[BTCoex], Wifi is idle\n"));
+				_btdm_1AntSetPSTDMA(padapter, true, 2, 1,
+						    false, 9);
 			}
 		} else {
-			RTPRINT(FBT, BT_TRACE, ("[BTCoex], wifi is disconnected\n"));
+			RTPRINT(FBT, BT_TRACE,
+				("[BTCoex], wifi is disconnected\n"));
 
 			btdm_1AntSetPSTDMA(padapter, false, 0, false, 9);
 		}
@@ -6056,24 +6074,29 @@ static void btdm_1AntBtCoexistHandler(struct rtw_adapter *padapter)
 		RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT is enabled\n"));
 
 		if (BTDM_IsWifiConnectionExist(padapter)) {
-			RTPRINT(FBT, BT_TRACE, ("[BTCoex], wifi is connected\n"));
+			RTPRINT(FBT, BT_TRACE,
+				("[BTCoex], wifi is connected\n"));
 
 			btdm_1AntWifiParaAdjust(padapter, true);
 			btdm_1AntCoexProcessForWifiConnect(padapter);
 		} else {
-			RTPRINT(FBT, BT_TRACE, ("[BTCoex], wifi is disconnected\n"));
+			RTPRINT(FBT, BT_TRACE,
+				("[BTCoex], wifi is disconnected\n"));
 
-			/*  Antenna switch at BT side(0x870 = 0x300, 0x860 = 0x210) after PSTDMA off */
+			/*  Antenna switch at BT side(0x870 = 0x300,
+			    0x860 = 0x210) after PSTDMA off */
 			btdm_1AntWifiParaAdjust(padapter, false);
 			btdm_1AntSetPSTDMA(padapter, false, 0, false, 0);
 		}
 	}
 
-	btdm_1AntBTStateChangeHandler(padapter, pBtCoex8723->prec2hBtInfo, pBtCoex8723->c2hBtInfo);
+	btdm_1AntBTStateChangeHandler(padapter, pBtCoex8723->prec2hBtInfo,
+				      pBtCoex8723->c2hBtInfo);
 	pBtCoex8723->prec2hBtInfo = pBtCoex8723->c2hBtInfo;
 }
 
-void BTDM_1AntSignalCompensation(struct rtw_adapter *padapter, u8 *rssi_wifi, u8 *rssi_bt)
+void BTDM_1AntSignalCompensation(struct rtw_adapter *padapter,
+				 u8 *rssi_wifi, u8 *rssi_bt)
 {
 	struct hal_data_8723a *pHalData;
 	struct btdm_8723a_1ant *pBtdm8723;
@@ -6119,14 +6142,19 @@ void BTDM_1AntSignalCompensation(struct rtw_adapter *padapter, u8 *rssi_wifi, u8
 	}
 
 	if (rssi_wifi && RSSI_WiFi_Cmpnstn) {
-		RTPRINT(FBT, BT_TRACE, ("[BTCoex], 1AntSgnlCmpnstn, case %d, WiFiCmpnstn =%d(%d => %d)\n",
-				pBtdm8723->curPsTdma, RSSI_WiFi_Cmpnstn, *rssi_wifi, *rssi_wifi+RSSI_WiFi_Cmpnstn));
+		RTPRINT(FBT, BT_TRACE,
+			("[BTCoex], 1AntSgnlCmpnstn, case %d, WiFiCmpnstn "
+			 "=%d(%d => %d)\n", pBtdm8723->curPsTdma,
+			 RSSI_WiFi_Cmpnstn, *rssi_wifi,
+			 *rssi_wifi+RSSI_WiFi_Cmpnstn));
 		*rssi_wifi += RSSI_WiFi_Cmpnstn;
 	}
 
 	if (rssi_bt && RSSI_BT_Cmpnstn) {
-		RTPRINT(FBT, BT_TRACE, ("[BTCoex], 1AntSgnlCmpnstn, case %d, BTCmpnstn =%d(%d => %d)\n",
-				pBtdm8723->curPsTdma, RSSI_BT_Cmpnstn, *rssi_bt, *rssi_bt+RSSI_BT_Cmpnstn));
+		RTPRINT(FBT, BT_TRACE,
+			("[BTCoex], 1AntSgnlCmpnstn, case %d, BTCmpnstn "
+			 "=%d(%d => %d)\n", pBtdm8723->curPsTdma,
+			 RSSI_BT_Cmpnstn, *rssi_bt, *rssi_bt+RSSI_BT_Cmpnstn));
 		*rssi_bt += RSSI_BT_Cmpnstn;
 	}
 }
@@ -6142,7 +6170,7 @@ static void BTDM_1AntParaInit(struct rtw_adapter *padapter)
 	pBtdm8723 = &pBtCoex->btdm1Ant;
 
 	/*  Enable counter statistics */
-	rtw_write8(padapter, 0x76e, 0x4);
+	rtl8723au_write8(padapter, 0x76e, 0x4);
 	btdm_1AntPtaParaReload(padapter);
 
 	pBtdm8723->wifiRssiThresh = 48;
@@ -6162,7 +6190,8 @@ static void BTDM_1AntForHalt(struct rtw_adapter *padapter)
 {
 	RTPRINT(FBT, BT_TRACE, ("\n[BTCoex], 1Ant for halt\n"));
 
-	GET_HAL_DATA(padapter)->bt_coexist.halCoex8723.btdm1Ant.bWiFiHalt = true;
+	GET_HAL_DATA(padapter)->bt_coexist.halCoex8723.btdm1Ant.bWiFiHalt =
+		true;
 
 	btdm_1AntWifiParaAdjust(padapter, false);
 
@@ -6180,7 +6209,8 @@ static void BTDM_1AntLpsLeave(struct rtw_adapter *padapter)
 	RTPRINT(FBT, BT_TRACE, ("\n[BTCoex], 1Ant for LPS Leave\n"));
 
 	/*  Prevent from entering LPS again */
-	GET_HAL_DATA(padapter)->bt_coexist.halCoex8723.btdm1Ant.bWiFiHalt = true;
+	GET_HAL_DATA(padapter)->bt_coexist.halCoex8723.btdm1Ant.bWiFiHalt =
+		true;
 
 	btdm_1AntSetPSTDMA(padapter, false, 0, false, 8);
 /*btdm_1AntPsTdma(padapter, false, 8); */
@@ -6190,13 +6220,14 @@ static void BTDM_1AntWifiAssociateNotify(struct rtw_adapter *padapter, u8 type)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
-	RTPRINT(FBT, BT_TRACE, ("\n[BTCoex], 1Ant for associate, type =%d\n", type));
+	RTPRINT(FBT, BT_TRACE,
+		("\n[BTCoex], 1Ant for associate, type =%d\n", type));
 
 	if (type) {
 		rtl8723a_CheckAntenna_Selection(padapter);
-		if (BT_IsBtDisabled(padapter)) {
+		if (!rtl8723a_BT_enabled(padapter))
 			btdm_1AntSetPSTDMA(padapter, false, 0, false, 9);
-		} else {
+		else {
 			struct bt_coexist_8723a *pBtCoex;
 			u8 BtState;
 
@@ -6205,24 +6236,28 @@ static void BTDM_1AntWifiAssociateNotify(struct rtw_adapter *padapter, u8 type)
 
 			btdm_1AntTSFSwitch(padapter, true);
 
-			if ((BtState == BT_INFO_STATE_NO_CONNECTION) ||
-			    (BtState == BT_INFO_STATE_CONNECT_IDLE)) {
-				btdm_1AntSetPSTDMA(padapter, false, 0, true, 28);
-			} else if ((BtState == BT_INFO_STATE_SCO_ONLY_BUSY) ||
-				   (BtState == BT_INFO_STATE_ACL_SCO_BUSY)) {
-				btdm_1AntSetPSTDMA(padapter, false, 0, false, 8);
-				rtw_write32(padapter, 0x6c0, 0x5a5a5a5a);
-				rtw_write32(padapter, 0x6c4, 0x5a5a5a5a);
-			} else if ((BtState == BT_INFO_STATE_ACL_ONLY_BUSY) ||
-				   (BtState == BT_INFO_STATE_ACL_INQ_OR_PAG)) {
+			if (BtState == BT_INFO_STATE_NO_CONNECTION ||
+			    BtState == BT_INFO_STATE_CONNECT_IDLE) {
+				btdm_1AntSetPSTDMA(padapter, false, 0,
+						   true, 28);
+			} else if (BtState == BT_INFO_STATE_SCO_ONLY_BUSY ||
+				   BtState == BT_INFO_STATE_ACL_SCO_BUSY) {
+				btdm_1AntSetPSTDMA(padapter, false, 0,
+						   false, 8);
+				rtl8723au_write32(padapter, 0x6c0, 0x5a5a5a5a);
+				rtl8723au_write32(padapter, 0x6c4, 0x5a5a5a5a);
+			} else if (BtState == BT_INFO_STATE_ACL_ONLY_BUSY ||
+				   BtState == BT_INFO_STATE_ACL_INQ_OR_PAG) {
 				if (pBtCoex->c2hBtProfile == BT_INFO_HID)
-					btdm_1AntSetPSTDMA(padapter, false, 0, true, 35);
+					btdm_1AntSetPSTDMA(padapter, false, 0,
+							   true, 35);
 				else
-					btdm_1AntSetPSTDMA(padapter, false, 0, true, 29);
+					btdm_1AntSetPSTDMA(padapter, false, 0,
+							   true, 29);
 			}
 		}
 	} else {
-		if (BT_IsBtDisabled(padapter)) {
+		if (!rtl8723a_BT_enabled(padapter)) {
 			if (!BTDM_IsWifiConnectionExist(padapter)) {
 				btdm_1AntPsTdma(padapter, false, 0);
 				btdm_1AntTSFSwitch(padapter, false);
@@ -6241,22 +6276,24 @@ BTDM_1AntMediaStatusNotify(struct rtw_adapter *padapter,
 
 	pBtCoex = &GET_HAL_DATA(padapter)->bt_coexist.halCoex8723;
 
-	RTPRINT(FBT, BT_TRACE, ("\n\n[BTCoex]******************************\n"));
+	RTPRINT(FBT, BT_TRACE,
+		("\n\n[BTCoex]******************************\n"));
 	RTPRINT(FBT, BT_TRACE, ("[BTCoex], MediaStatus, WiFi %s !!\n",
 			mstatus == RT_MEDIA_CONNECT?"CONNECT":"DISCONNECT"));
 	RTPRINT(FBT, BT_TRACE, ("[BTCoex]******************************\n"));
 
 	if (RT_MEDIA_CONNECT == mstatus) {
 		if (check_fwstate(&padapter->mlmepriv, WIFI_STATION_STATE)) {
-			if ((pBtCoex->c2hBtInfo == BT_INFO_STATE_SCO_ONLY_BUSY) ||
-			    (pBtCoex->c2hBtInfo == BT_INFO_STATE_ACL_SCO_BUSY))
+			if (pBtCoex->c2hBtInfo == BT_INFO_STATE_SCO_ONLY_BUSY ||
+			    pBtCoex->c2hBtInfo == BT_INFO_STATE_ACL_SCO_BUSY)
 				btdm_1AntUpdateHalRAMaskForSCO(padapter, true);
 		}
 
 		padapter->pwrctrlpriv.DelayLPSLastTimeStamp = jiffies;
 		BTDM_1AntForDhcp(padapter);
 	} else {
-		/* DBG_8723A("%s rtl8723a_DeinitAntenna_Selection\n", __func__); */
+		/* DBG_8723A("%s rtl8723a_DeinitAntenna_Selection\n",
+		   __func__); */
 		rtl8723a_DeinitAntenna_Selection(padapter);
 		btdm_1AntBtCoexistHandler(padapter);
 		pBtCoex->btdm1Ant.bRAChanged = false;
@@ -6276,8 +6313,10 @@ void BTDM_1AntForDhcp(struct rtw_adapter *padapter)
 	pBtdm8723 = &pBtCoex->btdm1Ant;
 
 	RTPRINT(FBT, BT_TRACE, ("\n[BTCoex], 1Ant for DHCP\n"));
-	RTPRINT(FBT, BT_TRACE, ("[BTCoex], 1Ant for DHCP, WiFi is %s\n", BTDM_IsWifiBusy(padapter)?"Busy":"IDLE"));
-	RTPRINT(FBT, BT_TRACE, ("[BTCoex], 1Ant for DHCP, %s\n", BtStateString[BtState]));
+	RTPRINT(FBT, BT_TRACE, ("[BTCoex], 1Ant for DHCP, WiFi is %s\n",
+				BTDM_IsWifiBusy(padapter)?"Busy":"IDLE"));
+	RTPRINT(FBT, BT_TRACE, ("[BTCoex], 1Ant for DHCP, %s\n",
+				BtStateString[BtState]));
 
 	BTDM_1AntWifiAssociateNotify(padapter, true);
 }
@@ -6294,13 +6333,16 @@ static void BTDM_1AntWifiScanNotify(struct rtw_adapter *padapter, u8 scanType)
 	pBtCoex = &pHalData->bt_coexist.halCoex8723;
 	pBtdm8723 = &pBtCoex->btdm1Ant;
 
-	RTPRINT(FBT, BT_TRACE, ("\n[BTCoex], 1Ant for wifi scan =%d!!\n", scanType));
-	RTPRINT(FBT, BT_TRACE, ("[BTCoex], 1Ant for wifi scan, WiFi is %s\n", BTDM_IsWifiBusy(padapter)?"Busy":"IDLE"));
-	RTPRINT(FBT, BT_TRACE, ("[BTCoex], 1Ant for wifi scan, %s\n", BtStateString[BtState]));
+	RTPRINT(FBT, BT_TRACE, ("\n[BTCoex], 1Ant for wifi scan =%d!!\n",
+				scanType));
+	RTPRINT(FBT, BT_TRACE, ("[BTCoex], 1Ant for wifi scan, WiFi is %s\n",
+				BTDM_IsWifiBusy(padapter)?"Busy":"IDLE"));
+	RTPRINT(FBT, BT_TRACE, ("[BTCoex], 1Ant for wifi scan, %s\n",
+				BtStateString[BtState]));
 
 	if (scanType) {
 		rtl8723a_CheckAntenna_Selection(padapter);
-		if (BT_IsBtDisabled(padapter)) {
+		if (!rtl8723a_BT_enabled(padapter)) {
 			btdm_1AntSetPSTDMA(padapter, false, 0, false, 9);
 		} else if (BTDM_IsWifiConnectionExist(padapter) == false) {
 			BTDM_1AntWifiAssociateNotify(padapter, true);
@@ -6308,10 +6350,13 @@ static void BTDM_1AntWifiScanNotify(struct rtw_adapter *padapter, u8 scanType)
 			if ((BtState == BT_INFO_STATE_SCO_ONLY_BUSY) ||
 			    (BtState == BT_INFO_STATE_ACL_SCO_BUSY)) {
 				if (pBtCoex->bC2hBtInquiryPage) {
-					btdm_1AntSetPSTDMA(padapter, false, 0, true, 32);
+					btdm_1AntSetPSTDMA(padapter, false, 0,
+							   true, 32);
 				} else {
-					padapter->pwrctrlpriv.btcoex_rfon = true;
-					btdm_1AntSetPSTDMA(padapter, true, 0, true, 33);
+					padapter->pwrctrlpriv.btcoex_rfon =
+						true;
+					btdm_1AntSetPSTDMA(padapter, true, 0,
+							   true, 33);
 				}
 			} else if (true == pBtCoex->bC2hBtInquiryPage) {
 				padapter->pwrctrlpriv.btcoex_rfon = true;
@@ -6319,9 +6364,11 @@ static void BTDM_1AntWifiScanNotify(struct rtw_adapter *padapter, u8 scanType)
 			} else if (BtState == BT_INFO_STATE_ACL_ONLY_BUSY) {
 				padapter->pwrctrlpriv.btcoex_rfon = true;
 				if (pBtCoex->c2hBtProfile == BT_INFO_HID)
-					btdm_1AntSetPSTDMA(padapter, true, 0, true, 34);
+					btdm_1AntSetPSTDMA(padapter, true, 0,
+							   true, 34);
 				else
-					btdm_1AntSetPSTDMA(padapter, true, 0, true, 4);
+					btdm_1AntSetPSTDMA(padapter, true, 0,
+							   true, 4);
 			} else {
 				padapter->pwrctrlpriv.btcoex_rfon = true;
 				btdm_1AntSetPSTDMA(padapter, true, 0, true, 5);
@@ -6365,16 +6412,18 @@ static void BTDM_1AntFwC2hBtInfo8723A(struct rtw_adapter *padapter)
 		pBtCoex->bC2hBtInquiryPage = false;
 	btState &= ~BIT(2);
 
-	if (!(btState & BIT(0))) {
+	if (!(btState & BIT(0)))
 		pBtCoex->c2hBtInfo = BT_INFO_STATE_NO_CONNECTION;
-	} else {
-		if (btState == 0x1) {
+	else {
+		if (btState == 0x1)
 			pBtCoex->c2hBtInfo = BT_INFO_STATE_CONNECT_IDLE;
-		} else if (btState == 0x9) {
+		else if (btState == 0x9) {
 			if (pBtCoex->bC2hBtInquiryPage == true)
-				pBtCoex->c2hBtInfo = BT_INFO_STATE_ACL_INQ_OR_PAG;
+				pBtCoex->c2hBtInfo =
+					BT_INFO_STATE_ACL_INQ_OR_PAG;
 			else
-				pBtCoex->c2hBtInfo = BT_INFO_STATE_ACL_ONLY_BUSY;
+				pBtCoex->c2hBtInfo =
+					BT_INFO_STATE_ACL_ONLY_BUSY;
 			pBtMgnt->ExtConfig.bBTBusy = true;
 		} else if (btState == 0x3) {
 			pBtCoex->c2hBtInfo = BT_INFO_STATE_SCO_ONLY_BUSY;
@@ -6382,15 +6431,15 @@ static void BTDM_1AntFwC2hBtInfo8723A(struct rtw_adapter *padapter)
 		} else if (btState == 0xb) {
 			pBtCoex->c2hBtInfo = BT_INFO_STATE_ACL_SCO_BUSY;
 			pBtMgnt->ExtConfig.bBTBusy = true;
-		} else {
+		} else
 			pBtCoex->c2hBtInfo = BT_INFO_STATE_MAX;
-		}
 		if (pBtMgnt->ExtConfig.bBTBusy)
-			pHalData->bt_coexist.CurrentState &= ~BT_COEX_STATE_BT_IDLE;
+			pHalData->bt_coexist.CurrentState &=
+				~BT_COEX_STATE_BT_IDLE;
 	}
 
-	if ((BT_INFO_STATE_NO_CONNECTION == pBtCoex->c2hBtInfo) ||
-	    (BT_INFO_STATE_CONNECT_IDLE == pBtCoex->c2hBtInfo)) {
+	if (BT_INFO_STATE_NO_CONNECTION == pBtCoex->c2hBtInfo ||
+	    BT_INFO_STATE_CONNECT_IDLE == pBtCoex->c2hBtInfo) {
 		if (pBtCoex->bC2hBtInquiryPage)
 			pBtCoex->c2hBtInfo = BT_INFO_STATE_INQ_OR_PAG;
 	}
@@ -6413,12 +6462,14 @@ void BTDM_1AntBtCoexist8723A(struct rtw_adapter *padapter)
 
 	if (check_fwstate(pmlmepriv, WIFI_SITE_MONITOR)) {
 		/*  already done in BTDM_1AntForScan() */
-		RTPRINT(FBT, BT_TRACE, ("[BTCoex], wifi is under scan progress!!\n"));
+		RTPRINT(FBT, BT_TRACE,
+			("[BTCoex], wifi is under scan progress!!\n"));
 		return;
 	}
 
 	if (check_fwstate(pmlmepriv, WIFI_UNDER_LINKING)) {
-		RTPRINT(FBT, BT_TRACE, ("[BTCoex], wifi is under link progress!!\n"));
+		RTPRINT(FBT, BT_TRACE,
+			("[BTCoex], wifi is under link progress!!\n"));
 		return;
 	}
 
@@ -6437,9 +6488,7 @@ void BTDM_1AntBtCoexist8723A(struct rtw_adapter *padapter)
 }
 
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtc87231Ant.c ===== */
-#endif
 
-#ifdef __HALBTC87232ANT_C__ /*  HAL/BTCoexist/HalBtc87232Ant.c */
 /*  ===== Below this line is sync from SD7 driver HAL/BTCoexist/HalBtc87232Ant.c ===== */
 
 /*  local function start with btdm_ */
@@ -6691,13 +6740,13 @@ btdm_SetCoexTable(struct rtw_adapter *padapter, u32 val0x6c0,
 		  u32 val0x6c8, u8 val0x6cc)
 {
 	RTPRINT(FBT, BT_TRACE, ("set coex table, set 0x6c0 = 0x%x\n", val0x6c0));
-	rtw_write32(padapter, 0x6c0, val0x6c0);
+	rtl8723au_write32(padapter, 0x6c0, val0x6c0);
 
 	RTPRINT(FBT, BT_TRACE, ("set coex table, set 0x6c8 = 0x%x\n", val0x6c8));
-	rtw_write32(padapter, 0x6c8, val0x6c8);
+	rtl8723au_write32(padapter, 0x6c8, val0x6c8);
 
 	RTPRINT(FBT, BT_TRACE, ("set coex table, set 0x6cc = 0x%x\n", val0x6cc));
-	rtw_write8(padapter, 0x6cc, val0x6cc);
+	rtl8723au_write8(padapter, 0x6cc, val0x6cc);
 }
 
 static void
@@ -8520,9 +8569,9 @@ static void BTDM_2AntParaInit(struct rtw_adapter *padapter)
 	RTPRINT(FBT, BT_TRACE, ("[BTCoex], 2Ant Parameter Init!!\n"));
 
 	/*  Enable counter statistics */
-	rtw_write8(padapter, 0x76e, 0x4);
-	rtw_write8(padapter, 0x778, 0x3);
-	rtw_write8(padapter, 0x40, 0x20);
+	rtl8723au_write8(padapter, 0x76e, 0x4);
+	rtl8723au_write8(padapter, 0x778, 0x3);
+	rtl8723au_write8(padapter, 0x40, 0x20);
 
 	/*  force to reset coex mechanism */
 	pBtdm8723->preVal0x6c0 = 0x0;
@@ -8919,9 +8968,7 @@ void BTDM_2AntBtCoexist8723A(struct rtw_adapter *padapter)
 }
 
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtc87232Ant.c ===== */
-#endif
 
-#ifdef __HALBTC8723_C__ /*  HAL/BTCoexist/HalBtc8723.c */
 /*  ===== Below this line is sync from SD7 driver HAL/BTCoexist/HalBtc8723.c ===== */
 
 static u8 btCoexDbgBuf[BT_TMP_BUF_SIZE];
@@ -8974,11 +9021,11 @@ static void btdm_BtHwCountersMonitor(struct rtw_adapter *padapter)
 	regHPTxRx = REG_HIGH_PRIORITY_TXRX;
 	regLPTxRx = REG_LOW_PRIORITY_TXRX;
 
-	u4Tmp = rtw_read32(padapter, regHPTxRx);
+	u4Tmp = rtl8723au_read32(padapter, regHPTxRx);
 	regHPTx = u4Tmp & bMaskLWord;
 	regHPRx = (u4Tmp & bMaskHWord)>>16;
 
-	u4Tmp = rtw_read32(padapter, regLPTxRx);
+	u4Tmp = rtl8723au_read32(padapter, regLPTxRx);
 	regLPTx = u4Tmp & bMaskLWord;
 	regLPRx = (u4Tmp & bMaskHWord)>>16;
 
@@ -8991,7 +9038,7 @@ static void btdm_BtHwCountersMonitor(struct rtw_adapter *padapter)
 	RTPRINT(FBT, BT_TRACE, ("Low Priority Tx/Rx = %d / %d\n", regLPTx, regLPRx));
 
 	/*  reset counter */
-	rtw_write8(padapter, 0x76e, 0xc);
+	rtl8723au_write8(padapter, 0x76e, 0xc);
 }
 
 /*  This function check if 8723 bt is disabled */
@@ -9004,7 +9051,7 @@ static void btdm_BtEnableDisableCheck8723A(struct rtw_adapter *padapter)
 	u8 val8;
 
 	/*  ox68[28]= 1 => BT enable; otherwise disable */
-	val8 = rtw_read8(padapter, 0x6B);
+	val8 = rtl8723au_read8(padapter, 0x6B);
 	if (!(val8 & BIT(4)))
 		btAlife = false;
 
@@ -9197,7 +9244,7 @@ void BTDM_SetFw3a(
 {
 	u8 H2C_Parameter[5] = {0};
 
-	if (BTDM_1Ant8723A(padapter)) {
+	if (rtl8723a_BT_using_antenna_1(padapter)) {
 		if ((!check_fwstate(&padapter->mlmepriv, WIFI_STATION_STATE)) &&
 		    (get_fwstate(&padapter->mlmepriv) != WIFI_NULL_STATE)) {
 			/*  for softap mode */
@@ -9241,7 +9288,7 @@ void BTDM_QueryBtInformation(struct rtw_adapter *padapter)
 	pHalData = GET_HAL_DATA(padapter);
 	pBtCoex = &pHalData->bt_coexist.halCoex8723;
 
-	if (BT_IsBtDisabled(padapter)) {
+	if (!rtl8723a_BT_enabled(padapter)) {
 		pBtCoex->c2hBtInfo = BT_INFO_STATE_DISABLED;
 		pBtCoex->bC2hBtInfoReqSent = false;
 		return;
@@ -9288,7 +9335,7 @@ BTDM_SetSwPenaltyTxRateAdaptive(
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 	u8 tmpU1;
 
-	tmpU1 = rtw_read8(padapter, 0x4fd);
+	tmpU1 = rtl8723au_read8(padapter, 0x4fd);
 	tmpU1 |= BIT(0);
 	if (BT_TX_RATE_ADAPTIVE_LOW_PENALTY == raType) {
 		tmpU1 &= ~BIT(2);
@@ -9297,7 +9344,7 @@ BTDM_SetSwPenaltyTxRateAdaptive(
 		tmpU1 |= BIT(2);
 	}
 
-	rtw_write8(padapter, 0x4fd, tmpU1);
+	rtl8723au_write8(padapter, 0x4fd, tmpU1);
 }
 
 void BTDM_SetFwDecBtPwr(struct rtw_adapter *padapter, u8 bDecBtPwr)
@@ -9349,8 +9396,8 @@ static void BTDM_FwC2hBtRssi8723A(struct rtw_adapter *padapter, u8 *tmpBuf)
 /*RTPRINT(FBT, BT_TRACE, ("[BTC2H], BT RSSI =%d\n", percent)); */
 }
 
-static void
-BTDM_FwC2hBtInfo8723A(struct rtw_adapter *padapter, u8 *tmpBuf, u8 length)
+void
+rtl8723a_fw_c2h_BT_info(struct rtw_adapter *padapter, u8 *tmpBuf, u8 length)
 {
 	struct hal_data_8723a *pHalData;
 	struct bt_30info *pBTInfo;
@@ -9419,7 +9466,7 @@ static void BTDM_Display8723ABtCoexInfo(struct rtw_adapter *padapter)
 	rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n ============[BT Coexist info]============");
 	DCMD_Printf(btCoexDbgBuf);
 
-	if (!pHalData->bt_coexist.BluetoothCoexist) {
+	if (!rtl8723a_BT_coexist(padapter)) {
 		rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n BT not exists !!!");
 		DCMD_Printf(btCoexDbgBuf);
 		return;
@@ -9473,7 +9520,8 @@ static void BTDM_Display8723ABtCoexInfo(struct rtw_adapter *padapter)
 
 					btInfoExt = pHalData->bt_coexist.halCoex8723.btInfoExt;
 					rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s", "A2DP rate", \
-						(btInfoExt&BIT0) ? "Basic rate" : "EDR rate");
+						 (btInfoExt & BIT(0)) ?
+						 "Basic rate" : "EDR rate");
 					DCMD_Printf(btCoexDbgBuf);
 				} else {
 					rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s/ %s", "Bt link type/spec", \
@@ -9527,9 +9575,9 @@ static void BTDM_Display8723ABtCoexInfo(struct rtw_adapter *padapter)
 			pBtCoex->btdm2Ant.bCurDecBtPwr);
 		DCMD_Printf(btCoexDbgBuf);
 	}
-	u1Tmp = rtw_read8(padapter, 0x778);
-	u1Tmp1 = rtw_read8(padapter, 0x783);
-	u1Tmp2 = rtw_read8(padapter, 0x796);
+	u1Tmp = rtl8723au_read8(padapter, 0x778);
+	u1Tmp1 = rtl8723au_read8(padapter, 0x783);
+	u1Tmp2 = rtl8723au_read8(padapter, 0x796);
 	rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x", "0x778/ 0x783/ 0x796", \
 		u1Tmp, u1Tmp1, u1Tmp2);
 	DCMD_Printf(btCoexDbgBuf);
@@ -9539,7 +9587,7 @@ static void BTDM_Display8723ABtCoexInfo(struct rtw_adapter *padapter)
 			pBtCoex->btdm2Ant.bCurDacSwingOn, pBtCoex->btdm2Ant.curDacSwingLvl);
 		DCMD_Printf(btCoexDbgBuf);
 	}
-	u4Tmp[0] =  rtw_read32(padapter, 0x880);
+	u4Tmp[0] =  rtl8723au_read32(padapter, 0x880);
 	rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x880", \
 		u4Tmp[0]);
 	DCMD_Printf(btCoexDbgBuf);
@@ -9550,56 +9598,56 @@ static void BTDM_Display8723ABtCoexInfo(struct rtw_adapter *padapter)
 		DCMD_Printf(btCoexDbgBuf);
 	}
 
-	u1Tmp = rtw_read8(padapter, 0x40);
+	u1Tmp = rtl8723au_read8(padapter, 0x40);
 	rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x40", \
 		u1Tmp);
 	DCMD_Printf(btCoexDbgBuf);
 
-	u4Tmp[0] = rtw_read32(padapter, 0x550);
-	u1Tmp = rtw_read8(padapter, 0x522);
+	u4Tmp[0] = rtl8723au_read32(padapter, 0x550);
+	u1Tmp = rtl8723au_read8(padapter, 0x522);
 	rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/0x%x", "0x550(bcn contrl)/0x522", \
 		u4Tmp[0], u1Tmp);
 	DCMD_Printf(btCoexDbgBuf);
 
-	u4Tmp[0] = rtw_read32(padapter, 0x484);
+	u4Tmp[0] = rtl8723au_read32(padapter, 0x484);
 	rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x484(rate adaptive)", \
 		u4Tmp[0]);
 	DCMD_Printf(btCoexDbgBuf);
 
-	u4Tmp[0] = rtw_read32(padapter, 0x50);
+	u4Tmp[0] = rtl8723au_read32(padapter, 0x50);
 	rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0xc50(dig)", \
 		u4Tmp[0]);
 	DCMD_Printf(btCoexDbgBuf);
 
-	u4Tmp[0] = rtw_read32(padapter, 0xda0);
-	u4Tmp[1] = rtw_read32(padapter, 0xda4);
-	u4Tmp[2] = rtw_read32(padapter, 0xda8);
-	u4Tmp[3] = rtw_read32(padapter, 0xdac);
+	u4Tmp[0] = rtl8723au_read32(padapter, 0xda0);
+	u4Tmp[1] = rtl8723au_read32(padapter, 0xda4);
+	u4Tmp[2] = rtl8723au_read32(padapter, 0xda8);
+	u4Tmp[3] = rtl8723au_read32(padapter, 0xdac);
 	rsprintf(btCoexDbgBuf, 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]);
 	DCMD_Printf(btCoexDbgBuf);
 
-	u4Tmp[0] = rtw_read32(padapter, 0x6c0);
-	u4Tmp[1] = rtw_read32(padapter, 0x6c4);
-	u4Tmp[2] = rtw_read32(padapter, 0x6c8);
-	u1Tmp = rtw_read8(padapter, 0x6cc);
+	u4Tmp[0] = rtl8723au_read32(padapter, 0x6c0);
+	u4Tmp[1] = rtl8723au_read32(padapter, 0x6c4);
+	u4Tmp[2] = rtl8723au_read32(padapter, 0x6c8);
+	u1Tmp = rtl8723au_read8(padapter, 0x6cc);
 	rsprintf(btCoexDbgBuf, 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);
 	DCMD_Printf(btCoexDbgBuf);
 
-	/* u4Tmp = rtw_read32(padapter, 0x770); */
+	/* u4Tmp = rtl8723au_read32(padapter, 0x770); */
 	rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d", "0x770(Hi pri Rx[31:16]/Tx[15:0])", \
 		pHalData->bt_coexist.halCoex8723.highPriorityRx,
 		pHalData->bt_coexist.halCoex8723.highPriorityTx);
 	DCMD_Printf(btCoexDbgBuf);
-	/* u4Tmp = rtw_read32(padapter, 0x774); */
+	/* u4Tmp = rtl8723au_read32(padapter, 0x774); */
 	rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d", "0x774(Lo pri Rx[31:16]/Tx[15:0])", \
 		pHalData->bt_coexist.halCoex8723.lowPriorityRx,
 		pHalData->bt_coexist.halCoex8723.lowPriorityTx);
 	DCMD_Printf(btCoexDbgBuf);
 
 	/*  Tx mgnt queue hang or not, 0x41b should = 0xf, ex: 0xd ==>hang */
-	u1Tmp = rtw_read8(padapter, 0x41b);
+	u1Tmp = rtl8723au_read8(padapter, 0x41b);
 	rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x41b (hang chk == 0xf)", \
 		u1Tmp);
 	DCMD_Printf(btCoexDbgBuf);
@@ -9672,7 +9720,7 @@ BTDM_Set8723ABtCoexCurrAntNum(struct rtw_adapter *padapter, u8 antNum)
 		pBtCoex->TotalAntNum = Ant_x2;
 }
 
-void BTDM_LpsLeave(struct rtw_adapter *padapter)
+void rtl8723a_BT_lps_leave(struct rtw_adapter *padapter)
 {
 	struct bt_30info *pBTInfo = GET_BT_INFO(padapter);
 	struct bt_mgnt *pBtMgnt = &pBTInfo->BtMgnt;
@@ -9752,7 +9800,7 @@ static void BTDM_ForDhcp8723A(struct rtw_adapter *padapter)
 		BTDM_1AntForDhcp(padapter);
 }
 
-u8 BTDM_1Ant8723A(struct rtw_adapter *padapter)
+bool rtl8723a_BT_using_antenna_1(struct rtw_adapter *padapter)
 {
 	if (btdm_BtWifiAntNum(padapter) == Ant_x1)
 		return true;
@@ -9785,7 +9833,7 @@ static void BTDM_BTCoexist8723A(struct rtw_adapter *padapter)
 	}
 
 	if (pBtCoex->bC2hBtInfoReqSent) {
-		if (BT_IsBtDisabled(padapter)) {
+		if (!rtl8723a_BT_enabled(padapter)) {
 			pBtCoex->c2hBtInfo = BT_INFO_STATE_DISABLED;
 		} else {
 			if (pBtCoex->c2hBtInfo == BT_INFO_STATE_DISABLED)
@@ -9793,7 +9841,7 @@ static void BTDM_BTCoexist8723A(struct rtw_adapter *padapter)
 		}
 
 		btdm_BTCoexist8723AHandler(padapter);
-	} else if (BT_IsBtDisabled(padapter) == true) {
+	} else if (!rtl8723a_BT_enabled(padapter)) {
 		pBtCoex->c2hBtInfo = BT_INFO_STATE_DISABLED;
 		btdm_BTCoexist8723AHandler(padapter);
 	}
@@ -9802,9 +9850,7 @@ static void BTDM_BTCoexist8723A(struct rtw_adapter *padapter)
 }
 
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtc8723.c ===== */
-#endif
 
-#ifdef __HALBTCCSR1ANT_C__ /*  HAL/BTCoexist/HalBtcCsr1Ant.c */
 /*  ===== Below this line is sync from SD7 driver HAL/BTCoexist/HalBtcCsr1Ant.c ===== */
 
 /*  local function start with btdm_ */
@@ -9866,7 +9912,7 @@ void BTDM_CheckBTIdleChange1Ant(struct rtw_adapter *padapter)
 	u32				BT_Active, BT_State;
 	u32				regBTActive = 0, regBTState = 0, regBTPolling = 0;
 
-	if (!pHalData->bt_coexist.BluetoothCoexist)
+	if (!rtl8723a_BT_coexist(padapter))
 		return;
 	if (pBtMgnt->ExtConfig.bManualControl)
 		return;
@@ -9885,15 +9931,15 @@ void BTDM_CheckBTIdleChange1Ant(struct rtw_adapter *padapter)
 	else
 		regBTPolling = REG_BT_POLLING;
 
-	BT_Active = rtw_read32(padapter, regBTActive);
+	BT_Active = rtl8723au_read32(padapter, regBTActive);
 	RTPRINT(FBT, BT_TRACE, ("[DM][BT], BT_Active(0x%x) =%x\n", regBTActive, BT_Active));
 	BT_Active = BT_Active & 0x00ffffff;
 
-	BT_State = rtw_read32(padapter, regBTState);
+	BT_State = rtl8723au_read32(padapter, regBTState);
 	RTPRINT(FBT, BT_TRACE, ("[DM][BT], BT_State(0x%x) =%x\n", regBTState, BT_State));
 	BT_State = BT_State & 0x00ffffff;
 
-	BT_Polling = rtw_read32(padapter, regBTPolling);
+	BT_Polling = rtl8723au_read32(padapter, regBTPolling);
 	RTPRINT(FBT, BT_TRACE, ("[DM][BT], BT_Polling(0x%x) =%x\n", regBTPolling, BT_Polling));
 
 	if (BT_Active == 0xffffffff && BT_State == 0xffffffff && BT_Polling == 0xffffffff)
@@ -9991,9 +10037,7 @@ void BTDM_CheckBTIdleChange1Ant(struct rtw_adapter *padapter)
 }
 
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtcCsr1Ant.c ===== */
-#endif
 
-#ifdef __HALBTCCSR2ANT_C__ /*  HAL/BTCoexist/HalBtcCsr2Ant.c */
 /*  ===== Below this line is sync from SD7 driver HAL/BTCoexist/HalBtcCsr2Ant.c ===== */
 
 /*  local function start with btdm_ */
@@ -10048,9 +10092,7 @@ BTDM_DiminishWiFi(
 }
 
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtcCsr2Ant.c ===== */
-#endif
 
-#ifdef __HALBTCOEXIST_C__ /*  HAL/BTCoexist/HalBtCoexist.c */
 /*  ===== Below this line is sync from SD7 driver HAL/BTCoexist/HalBtCoexist.c ===== */
 
 /*  local function */
@@ -10089,11 +10131,6 @@ void BTDM_FwC2hBtRssi(struct rtw_adapter *padapter, u8 *tmpBuf)
 	BTDM_FwC2hBtRssi8723A(padapter, tmpBuf);
 }
 
-void BTDM_FwC2hBtInfo(struct rtw_adapter *padapter, u8 *tmpBuf, u8 length)
-{
-	BTDM_FwC2hBtInfo8723A(padapter, tmpBuf, length);
-}
-
 void BTDM_DisplayBtCoexInfo(struct rtw_adapter *padapter)
 {
 	BTDM_Display8723ABtCoexInfo(padapter);
@@ -10489,18 +10526,18 @@ u8 BTDM_CheckCoexRSSIState(struct rtw_adapter *padapter, u8 levelNum,
 	return btRssiState;
 }
 
-u8 BTDM_DisableEDCATurbo(struct rtw_adapter *padapter)
+bool rtl8723a_BT_disable_EDCA_turbo(struct rtw_adapter *padapter)
 {
 	struct bt_mgnt *pBtMgnt;
 	struct hal_data_8723a *pHalData;
 	u8 bBtChangeEDCA = false;
 	u32 EDCA_BT_BE = 0x5ea42b, cur_EDCA_reg;
-	u8 bRet = false;
+	bool bRet = false;
 
 	pHalData = GET_HAL_DATA(padapter);
 	pBtMgnt = &pHalData->BtInfo.BtMgnt;
 
-	if (!pHalData->bt_coexist.BluetoothCoexist) {
+	if (!rtl8723a_BT_coexist(padapter)) {
 		bRet = false;
 		pHalData->bt_coexist.lastBtEdca = 0;
 		return bRet;
@@ -10512,7 +10549,7 @@ u8 BTDM_DisableEDCATurbo(struct rtw_adapter *padapter)
 		return bRet;
 	}
 
-	if (BT_1Ant(padapter)) {
+	if (rtl8723a_BT_using_antenna_1(padapter)) {
 		bRet = false;
 		pHalData->bt_coexist.lastBtEdca = 0;
 		return bRet;
@@ -10533,12 +10570,13 @@ u8 BTDM_DisableEDCATurbo(struct rtw_adapter *padapter)
 			pHalData->odmpriv.DM_EDCA_Table.bCurrentTurboEDCA = false;
 			pHalData->dmpriv.prv_traffic_idx = 3;
 		}
-		cur_EDCA_reg = rtw_read32(padapter, REG_EDCA_BE_PARAM);
+		cur_EDCA_reg = rtl8723au_read32(padapter, REG_EDCA_BE_PARAM);
 
 		if (cur_EDCA_reg != EDCA_BT_BE)
 			bBtChangeEDCA = true;
 		if (bBtChangeEDCA || !pHalData->bt_coexist.bEDCAInitialized) {
-			rtw_write32(padapter, REG_EDCA_BE_PARAM, EDCA_BT_BE);
+			rtl8723au_write32(padapter, REG_EDCA_BE_PARAM,
+					  EDCA_BT_BE);
 			pHalData->bt_coexist.lastBtEdca = EDCA_BT_BE;
 		}
 		bRet = true;
@@ -10585,11 +10623,11 @@ void BTDM_AGCTable(struct rtw_adapter *padapter, u8 type)
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 	if (type == BT_AGCTABLE_OFF) {
 		RTPRINT(FBT, BT_TRACE, ("[BT]AGCTable Off!\n"));
-		rtw_write32(padapter, 0xc78, 0x641c0001);
-		rtw_write32(padapter, 0xc78, 0x631d0001);
-		rtw_write32(padapter, 0xc78, 0x621e0001);
-		rtw_write32(padapter, 0xc78, 0x611f0001);
-		rtw_write32(padapter, 0xc78, 0x60200001);
+		rtl8723au_write32(padapter, 0xc78, 0x641c0001);
+		rtl8723au_write32(padapter, 0xc78, 0x631d0001);
+		rtl8723au_write32(padapter, 0xc78, 0x621e0001);
+		rtl8723au_write32(padapter, 0xc78, 0x611f0001);
+		rtl8723au_write32(padapter, 0xc78, 0x60200001);
 
 		PHY_SetRFReg(padapter, PathA, RF_RX_AGC_HP, bRFRegOffsetMask, 0x32000);
 		PHY_SetRFReg(padapter, PathA, RF_RX_AGC_HP, bRFRegOffsetMask, 0x71000);
@@ -10600,11 +10638,11 @@ void BTDM_AGCTable(struct rtw_adapter *padapter, u8 type)
 		pHalData->bt_coexist.b8723aAgcTableOn = false;
 	} else if (type == BT_AGCTABLE_ON) {
 		RTPRINT(FBT, BT_TRACE, ("[BT]AGCTable On!\n"));
-		rtw_write32(padapter, 0xc78, 0x4e1c0001);
-		rtw_write32(padapter, 0xc78, 0x4d1d0001);
-		rtw_write32(padapter, 0xc78, 0x4c1e0001);
-		rtw_write32(padapter, 0xc78, 0x4b1f0001);
-		rtw_write32(padapter, 0xc78, 0x4a200001);
+		rtl8723au_write32(padapter, 0xc78, 0x4e1c0001);
+		rtl8723au_write32(padapter, 0xc78, 0x4d1d0001);
+		rtl8723au_write32(padapter, 0xc78, 0x4c1e0001);
+		rtl8723au_write32(padapter, 0xc78, 0x4b1f0001);
+		rtl8723au_write32(padapter, 0xc78, 0x4a200001);
 
 		PHY_SetRFReg(padapter, PathA, RF_RX_AGC_HP, bRFRegOffsetMask, 0xdc000);
 		PHY_SetRFReg(padapter, PathA, RF_RX_AGC_HP, bRFRegOffsetMask, 0x90000);
@@ -10624,10 +10662,10 @@ void BTDM_BBBackOffLevel(struct rtw_adapter *padapter, u8 type)
 
 	if (type == BT_BB_BACKOFF_OFF) {
 		RTPRINT(FBT, BT_TRACE, ("[BT]BBBackOffLevel Off!\n"));
-		rtw_write32(padapter, 0xc04, 0x3a05611);
+		rtl8723au_write32(padapter, 0xc04, 0x3a05611);
 	} else if (type == BT_BB_BACKOFF_ON) {
 		RTPRINT(FBT, BT_TRACE, ("[BT]BBBackOffLevel On!\n"));
-		rtw_write32(padapter, 0xc04, 0x3a07611);
+		rtl8723au_write32(padapter, 0xc04, 0x3a07611);
 		pHalData->bt_coexist.bSWCoexistAllOff = false;
 	}
 }
@@ -10680,16 +10718,15 @@ void BTDM_CoexAllOff(struct rtw_adapter *padapter)
 	BTDM_HWCoexAllOff(padapter);
 }
 
-void BTDM_TurnOffBtCoexistBeforeEnterIPS(struct rtw_adapter *padapter)
+void rtl8723a_BT_disable_coexist(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 	struct pwrctrl_priv *ppwrctrl = &padapter->pwrctrlpriv;
 
-	if (!pHalData->bt_coexist.BluetoothCoexist)
+	if (!rtl8723a_BT_coexist(padapter))
 		return;
 
 	/*  8723 1Ant doesn't need to turn off bt coexist mechanism. */
-	if (BTDM_1Ant8723A(padapter))
+	if (rtl8723a_BT_using_antenna_1(padapter))
 		return;
 
 	/*  Before enter IPS, turn off FW BT Co-exist mechanism */
@@ -10706,11 +10743,11 @@ void BTDM_SignalCompensation(struct rtw_adapter *padapter, u8 *rssi_wifi, u8 *rs
 	BTDM_8723ASignalCompensation(padapter, rssi_wifi, rssi_bt);
 }
 
-void BTDM_Coexist(struct rtw_adapter *padapter)
+void rtl8723a_BT_do_coexist(struct rtw_adapter *padapter)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
-	if (!pHalData->bt_coexist.BluetoothCoexist) {
+	if (!rtl8723a_BT_coexist(padapter)) {
 		RTPRINT(FBT, BT_TRACE, ("[DM][BT], BT not exists!!\n"));
 		return;
 	}
@@ -10920,9 +10957,7 @@ void BTDM_SetBtCoexCurrAntNum(struct rtw_adapter *padapter, u8 antNum)
 
 void BTDM_ForHalt(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-
-	if (!pHalData->bt_coexist.BluetoothCoexist)
+	if (!rtl8723a_BT_coexist(padapter))
 		return;
 
 	BTDM_ForHalt8723A(padapter);
@@ -10931,9 +10966,7 @@ void BTDM_ForHalt(struct rtw_adapter *padapter)
 
 void BTDM_WifiScanNotify(struct rtw_adapter *padapter, u8 scanType)
 {
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-
-	if (!pHalData->bt_coexist.BluetoothCoexist)
+	if (!rtl8723a_BT_coexist(padapter))
 		return;
 
 	BTDM_WifiScanNotify8723A(padapter, scanType);
@@ -10941,29 +10974,24 @@ void BTDM_WifiScanNotify(struct rtw_adapter *padapter, u8 scanType)
 
 void BTDM_WifiAssociateNotify(struct rtw_adapter *padapter, u8 action)
 {
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-
-	if (!pHalData->bt_coexist.BluetoothCoexist)
+	if (!rtl8723a_BT_coexist(padapter))
 		return;
 
 	BTDM_WifiAssociateNotify8723A(padapter, action);
 }
 
-void BTDM_MediaStatusNotify(struct rtw_adapter *padapter, enum rt_media_status mstatus)
+void rtl8723a_BT_mediastatus_notify(struct rtw_adapter *padapter,
+				    enum rt_media_status mstatus)
 {
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-
-	if (!pHalData->bt_coexist.BluetoothCoexist)
+	if (!rtl8723a_BT_coexist(padapter))
 		return;
 
 	BTDM_MediaStatusNotify8723A(padapter, mstatus);
 }
 
-void BTDM_ForDhcp(struct rtw_adapter *padapter)
+void rtl8723a_BT_specialpacket_notify(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-
-	if (!pHalData->bt_coexist.BluetoothCoexist)
+	if (!rtl8723a_BT_coexist(padapter))
 		return;
 
 	BTDM_ForDhcp8723A(padapter);
@@ -11175,20 +11203,18 @@ u8 BTDM_IsActionPANA2DP(struct rtw_adapter *padapter)
 	return bRet;
 }
 
-u8 BTDM_IsBtDisabled(struct rtw_adapter *padapter)
+bool rtl8723a_BT_enabled(struct rtw_adapter *padapter)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
 	if (pHalData->bt_coexist.bCurBtDisabled)
-		return true;
-	else
 		return false;
+	else
+		return true;
 }
 
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtCoexist.c ===== */
-#endif
 
-#ifdef __HALBT_C__ /*  HAL/HalBT.c */
 /*  ===== Below this line is sync from SD7 driver HAL/HalBT.c ===== */
 
 /*  */
@@ -11221,7 +11247,9 @@ void HALBT_SetKey(struct rtw_adapter *padapter, u8 EntryNum)
 	pBtAssocEntry->HwCAMIndex = BT_HWCAM_STAR + EntryNum;
 
 	usConfig = CAM_VALID | (CAM_AES << 2);
-	write_cam23a(padapter, pBtAssocEntry->HwCAMIndex, usConfig, pBtAssocEntry->BTRemoteMACAddr, pBtAssocEntry->PTK + TKIP_ENC_KEY_POS);
+	rtl8723a_cam_write(padapter, pBtAssocEntry->HwCAMIndex, usConfig,
+			   pBtAssocEntry->BTRemoteMACAddr,
+			   pBtAssocEntry->PTK + TKIP_ENC_KEY_POS);
 }
 
 void HALBT_RemoveKey(struct rtw_adapter *padapter, u8 EntryNum)
@@ -11234,13 +11262,15 @@ void HALBT_RemoveKey(struct rtw_adapter *padapter, u8 EntryNum)
 
 	if (pBTinfo->BtAsocEntry[EntryNum].HwCAMIndex != 0) {
 		/*  ToDo : add New HALBT_RemoveKey function !! */
-		if (pBtAssocEntry->HwCAMIndex >= BT_HWCAM_STAR && pBtAssocEntry->HwCAMIndex < HALF_CAM_ENTRY)
-			CAM_empty_entry23a(padapter, pBtAssocEntry->HwCAMIndex);
+		if (pBtAssocEntry->HwCAMIndex >= BT_HWCAM_STAR &&
+		    pBtAssocEntry->HwCAMIndex < HALF_CAM_ENTRY)
+			rtl8723a_cam_empty_entry(padapter,
+						 pBtAssocEntry->HwCAMIndex);
 		pBTinfo->BtAsocEntry[EntryNum].HwCAMIndex = 0;
 	}
 }
 
-void HALBT_InitBTVars8723A(struct rtw_adapter *padapter)
+void rtl8723a_BT_init_hal_vars(struct rtw_adapter *padapter)
 {
 	struct hal_data_8723a *pHalData;
 
@@ -11252,8 +11282,10 @@ void HALBT_InitBTVars8723A(struct rtw_adapter *padapter)
 	pHalData->bt_coexist.BT_Ant_isolation = pHalData->EEPROMBluetoothAntIsolation;
 	pHalData->bt_coexist.bt_radiosharedtype = pHalData->EEPROMBluetoothRadioShared;
 
-	RT_TRACE(_module_hal_init_c_, _drv_info_, ("BT Coexistance = 0x%x\n", pHalData->bt_coexist.BluetoothCoexist));
-	if (pHalData->bt_coexist.BluetoothCoexist) {
+	RT_TRACE(_module_hal_init_c_, _drv_info_,
+		 ("BT Coexistance = 0x%x\n", rtl8723a_BT_coexist(padapter)));
+
+	if (rtl8723a_BT_coexist(padapter)) {
 		if (pHalData->bt_coexist.BT_Ant_Num == Ant_x2) {
 			BTDM_SetBtCoexCurrAntNum(padapter, 2);
 			RT_TRACE(_module_hal_init_c_, _drv_info_, ("BlueTooth BT_Ant_Num = Antx2\n"));
@@ -11273,7 +11305,7 @@ void HALBT_InitBTVars8723A(struct rtw_adapter *padapter)
 	}
 }
 
-u8 HALBT_IsBTExist(struct rtw_adapter *padapter)
+bool rtl8723a_BT_coexist(struct rtw_adapter *padapter)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
@@ -11290,10 +11322,10 @@ u8 HALBT_BTChipType(struct rtw_adapter *padapter)
 	return pHalData->bt_coexist.BT_CoexistType;
 }
 
-void HALBT_InitHwConfig(struct rtw_adapter *padapter)
+void rtl8723a_BT_init_hwconfig(struct rtw_adapter *padapter)
 {
 	halbt_InitHwConfig8723A(padapter);
-	BTDM_Coexist(padapter);
+	rtl8723a_BT_do_coexist(padapter);
 }
 
 void HALBT_SetRtsCtsNoLenLimit(struct rtw_adapter *padapter)
@@ -11301,4 +11333,49 @@ void HALBT_SetRtsCtsNoLenLimit(struct rtw_adapter *padapter)
 }
 
 /*  ===== End of sync from SD7 driver HAL/HalBT.c ===== */
-#endif
+
+void rtl8723a_dual_antenna_detection(struct rtw_adapter *padapter)
+{
+	struct hal_data_8723a *pHalData;
+	struct dm_odm_t *pDM_Odm;
+	struct sw_ant_sw *pDM_SWAT_Table;
+	u8 i;
+
+	pHalData = GET_HAL_DATA(padapter);
+	pDM_Odm = &pHalData->odmpriv;
+	pDM_SWAT_Table = &pDM_Odm->DM_SWAT_Table;
+
+	/*  */
+	/*  <Roger_Notes> RTL8723A Single and Dual antenna dynamic detection
+	    mechanism when RF power state is on. */
+	/*  We should take power tracking, IQK, LCK, RCK RF read/write
+	    operation into consideration. */
+	/*  2011.12.15. */
+	/*  */
+	if (!pHalData->bAntennaDetected) {
+		u8 btAntNum = BT_GetPGAntNum(padapter);
+
+		/*  Set default antenna B status */
+		if (btAntNum == Ant_x2)
+			pDM_SWAT_Table->ANTB_ON = true;
+		else if (btAntNum == Ant_x1)
+			pDM_SWAT_Table->ANTB_ON = false;
+		else
+			pDM_SWAT_Table->ANTB_ON = true;
+
+		if (pHalData->CustomerID != RT_CID_TOSHIBA) {
+			for (i = 0; i < MAX_ANTENNA_DETECTION_CNT; i++) {
+				if (ODM_SingleDualAntennaDetection
+				    (&pHalData->odmpriv, ANTTESTALL) == true)
+					break;
+			}
+
+			/*  Set default antenna number for BT coexistence */
+			if (btAntNum == Ant_x2)
+				BT_SetBtCoexCurrAntNum(padapter,
+						       pDM_SWAT_Table->
+						       ANTB_ON ? 2 : 1);
+		}
+		pHalData->bAntennaDetected = true;
+	}
+}
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_cmd.c b/drivers/staging/rtl8723au/hal/rtl8723a_cmd.c
index 0b205e1204f..e8737916c7b 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_cmd.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_cmd.c
@@ -17,10 +17,10 @@
 #include <osdep_service.h>
 #include <drv_types.h>
 #include <recv_osdep.h>
-#include <cmd_osdep.h>
 #include <mlme_osdep.h>
 #include <rtw_ioctl_set.h>
 #include <rtl8723a_hal.h>
+#include <usb_ops_linux.h>
 
 #define RTL92C_MAX_H2C_BOX_NUMS		4
 #define RTL92C_MAX_CMD_LEN		5
@@ -34,7 +34,7 @@ static u8 _is_fw_read_cmd_down(struct rtw_adapter *padapter, u8 msgbox_num)
 	u8 valid;
 
 	do {
-		valid = rtw_read8(padapter, REG_HMETFR) & BIT(msgbox_num);
+		valid = rtl8723au_read8(padapter, REG_HMETFR) & BIT(msgbox_num);
 		if (0 == valid)
 			read_down = true;
 	} while ((!read_down) && (retry_cnts--));
@@ -48,7 +48,8 @@ static u8 _is_fw_read_cmd_down(struct rtw_adapter *padapter, u8 msgbox_num)
 *| h2c_msg	|Ext_bit	|CMD_ID	|
 *
 ******************************************/
-s32 FillH2CCmd(struct rtw_adapter *padapter, u8 ElementID, u32 CmdLen, u8 *pCmdBuffer)
+int FillH2CCmd(struct rtw_adapter *padapter, u8 ElementID, u32 CmdLen,
+	       u8 *pCmdBuffer)
 {
 	u8 bcmd_down = false;
 	s32 retry_cnts = 100;
@@ -58,7 +59,7 @@ s32 FillH2CCmd(struct rtw_adapter *padapter, u8 ElementID, u32 CmdLen, u8 *pCmdB
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 	u32 h2c_cmd = 0;
 	u16 h2c_cmd_ex = 0;
-	s32 ret = _FAIL;
+	int ret = _FAIL;
 
 	padapter = GET_PRIMARY_ADAPTER(padapter);
 	pHalData = GET_HAL_DATA(padapter);
@@ -94,11 +95,11 @@ s32 FillH2CCmd(struct rtw_adapter *padapter, u8 ElementID, u32 CmdLen, u8 *pCmdB
 		if (h2c_cmd & BIT(7)) {
 			msgbox_ex_addr = REG_HMEBOX_EXT_0 + (h2c_box_num * EX_MESSAGE_BOX_SIZE);
 			h2c_cmd_ex = le16_to_cpu(h2c_cmd_ex);
-			rtw_write16(padapter, msgbox_ex_addr, h2c_cmd_ex);
+			rtl8723au_write16(padapter, msgbox_ex_addr, h2c_cmd_ex);
 		}
 		msgbox_addr = REG_HMEBOX_0 + (h2c_box_num * MESSAGE_BOX_SIZE);
 		h2c_cmd = le32_to_cpu(h2c_cmd);
-		rtw_write32(padapter, msgbox_addr, h2c_cmd);
+		rtl8723au_write32(padapter, msgbox_addr, h2c_cmd);
 
 		bcmd_down = true;
 
@@ -113,9 +114,9 @@ exit:
 	return ret;
 }
 
-u8 rtl8723a_set_rssi_cmd(struct rtw_adapter *padapter, u8 *param)
+int rtl8723a_set_rssi_cmd(struct rtw_adapter *padapter, u8 *param)
 {
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	*((u32 *)param) = cpu_to_le32(*((u32 *)param));
 
@@ -124,10 +125,10 @@ u8 rtl8723a_set_rssi_cmd(struct rtw_adapter *padapter, u8 *param)
 	return res;
 }
 
-u8 rtl8723a_set_raid_cmd(struct rtw_adapter *padapter, u32 mask, u8 arg)
+int rtl8723a_set_raid_cmd(struct rtw_adapter *padapter, u32 mask, u8 arg)
 {
 	u8 buf[5];
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	memset(buf, 0, 5);
 	mask = cpu_to_le32(mask);
@@ -137,7 +138,6 @@ u8 rtl8723a_set_raid_cmd(struct rtw_adapter *padapter, u32 mask, u8 arg)
 	FillH2CCmd(padapter, MACID_CONFIG_EID, 5, buf);
 
 	return res;
-
 }
 
 /* bitmap[0:27] = tx_rate_bitmap */
@@ -168,7 +168,8 @@ void rtl8723a_add_rateatid(struct rtw_adapter *pAdapter, u32 bitmap, u8 arg, u8
 		if (shortGIrate == true)
 			init_rate |= BIT(6);
 
-		rtw_write8(pAdapter, (REG_INIDATA_RATE_SEL+macid), (u8)init_rate);
+		rtl8723au_write8(pAdapter, REG_INIDATA_RATE_SEL + macid,
+				 init_rate);
 	}
 }
 
@@ -178,7 +179,7 @@ void rtl8723a_set_FwPwrMode_cmd(struct rtw_adapter *padapter, u8 Mode)
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
-	DBG_8723A("%s: Mode =%d SmartPS =%d UAPSD =%d BcnMode = 0x%02x\n", __FUNCTION__,
+	DBG_8723A("%s: Mode =%d SmartPS =%d UAPSD =%d BcnMode = 0x%02x\n", __func__,
 			Mode, pwrpriv->smart_ps, padapter->registrypriv.uapsd_enable, pwrpriv->bcn_ant_mode);
 
 	/*  Forece leave RF low power mode for 1T1R to
@@ -203,27 +204,26 @@ void rtl8723a_set_FwPwrMode_cmd(struct rtw_adapter *padapter, u8 Mode)
 static void ConstructBeacon(struct rtw_adapter *padapter, u8 *pframe, u32 *pLength)
 {
 	struct ieee80211_hdr *pwlanhdr;
-	u16 *fctrl;
 	u32 rate_len, pktlen;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
 	u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	int bcn_fixed_size;
 
-	/* DBG_8723A("%s\n", __FUNCTION__); */
+	/* DBG_8723A("%s\n", __func__); */
 
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
-	fctrl = &pwlanhdr->frame_control;
-	*(fctrl) = 0;
+	pwlanhdr->frame_control =
+		cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_BEACON);
 
 	memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
 	memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
 	memcpy(pwlanhdr->addr3, get_my_bssid23a(cur_network), ETH_ALEN);
 
-	SetSeqNum(pwlanhdr, 0/*pmlmeext->mgnt_seq*/);
-	/* pmlmeext->mgnt_seq++; */
-	SetFrameSubType(pframe, WIFI_BEACON);
+	/* A Beacon frame shouldn't have fragment bits set */
+	pwlanhdr->seq_ctrl = 0;
 
 	pframe += sizeof(struct ieee80211_hdr_3addr);
 	pktlen = sizeof (struct ieee80211_hdr_3addr);
@@ -245,9 +245,13 @@ static void ConstructBeacon(struct rtw_adapter *padapter, u8 *pframe, u32 *pLeng
 	pktlen += 2;
 
 	if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) {
+		bcn_fixed_size =
+			offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+			offsetof(struct ieee80211_mgmt, u.beacon);
+
 		/* DBG_8723A("ie len =%d\n", cur_network->IELength); */
-		pktlen += cur_network->IELength - sizeof(struct ndis_802_11_fixed_ies);
-		memcpy(pframe, cur_network->IEs+sizeof(struct ndis_802_11_fixed_ies), pktlen);
+		pktlen += cur_network->IELength - bcn_fixed_size;
+		memcpy(pframe, cur_network->IEs + bcn_fixed_size, pktlen);
 
 		goto _ConstructBeacon;
 	}
@@ -255,30 +259,36 @@ static void ConstructBeacon(struct rtw_adapter *padapter, u8 *pframe, u32 *pLeng
 	/* below for ad-hoc mode */
 
 	/*  SSID */
-	pframe = rtw_set_ie23a(pframe, _SSID_IE_, cur_network->Ssid.ssid_len,
-			    cur_network->Ssid.ssid, &pktlen);
+	pframe = rtw_set_ie23a(pframe, WLAN_EID_SSID,
+			       cur_network->Ssid.ssid_len,
+			       cur_network->Ssid.ssid, &pktlen);
 
 	/*  supported rates... */
 	rate_len = rtw_get_rateset_len23a(cur_network->SupportedRates);
-	pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8) ?
+	pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES, ((rate_len > 8) ?
 			       8 : rate_len), cur_network->SupportedRates, &pktlen);
 
 	/*  DS parameter set */
-	pframe = rtw_set_ie23a(pframe, _DSSET_IE_, 1, (unsigned char *)&cur_network->Configuration.DSConfig, &pktlen);
+	pframe = rtw_set_ie23a(pframe, WLAN_EID_DS_PARAMS, 1, (unsigned char *)
+			       &cur_network->DSConfig, &pktlen);
 
 	if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
 		u32 ATIMWindow;
 		/*  IBSS Parameter Set... */
-		/* ATIMWindow = cur->Configuration.ATIMWindow; */
+		/* ATIMWindow = cur->ATIMWindow; */
 		ATIMWindow = 0;
-		pframe = rtw_set_ie23a(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pktlen);
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_IBSS_PARAMS, 2,
+				       (unsigned char *)&ATIMWindow, &pktlen);
 	}
 
 	/* todo: ERP IE */
 
 	/*  EXTERNDED SUPPORTED RATE */
 	if (rate_len > 8)
-		pframe = rtw_set_ie23a(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pktlen);
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_EXT_SUPP_RATES,
+				       (rate_len - 8),
+				       (cur_network->SupportedRates + 8),
+				       &pktlen);
 
 	/* todo:HT for adhoc */
 
@@ -291,27 +301,26 @@ _ConstructBeacon:
 
 	*pLength = pktlen;
 
-	/* DBG_8723A("%s bcn_sz =%d\n", __FUNCTION__, pktlen); */
+	/* DBG_8723A("%s bcn_sz =%d\n", __func__, pktlen); */
 
 }
 
-static void ConstructPSPoll(struct rtw_adapter *padapter, u8 *pframe, u32 *pLength)
+static void ConstructPSPoll(struct rtw_adapter *padapter,
+			    u8 *pframe, u32 *pLength)
 {
 	struct ieee80211_hdr *pwlanhdr;
-	u16 *fctrl;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
 	/*  Frame control. */
-	fctrl = &pwlanhdr->frame_control;
-	*(fctrl) = 0;
-	SetPwrMgt(fctrl);
-	SetFrameSubType(pframe, WIFI_PSPOLL);
+	pwlanhdr->frame_control =
+		cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_PSPOLL);
+	pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_PM);
 
 	/*  AID. */
-	SetDuration(pframe, (pmlmeinfo->aid | 0xc000));
+	pwlanhdr->duration_id = cpu_to_le16(pmlmeinfo->aid | 0xc000);
 
 	/*  BSSID. */
 	memcpy(pwlanhdr->addr1, get_my_bssid23a(&pmlmeinfo->network), ETH_ALEN);
@@ -322,49 +331,46 @@ static void ConstructPSPoll(struct rtw_adapter *padapter, u8 *pframe, u32 *pLeng
 	*pLength = 16;
 }
 
-static void ConstructNullFunctionData(
-	struct rtw_adapter *padapter,
-	u8 *pframe,
-	u32 *pLength,
-	u8 *StaAddr,
-	u8 bQoS,
-	u8 AC,
-	u8 bEosp,
-	u8 bForcePowerSave)
+static void
+ConstructNullFunctionData(struct rtw_adapter *padapter, u8 *pframe,
+			  u32 *pLength, u8 *StaAddr, u8 bQoS, u8 AC,
+			  u8 bEosp, u8 bForcePowerSave)
 {
 	struct ieee80211_hdr *pwlanhdr;
-	u16 *fctrl;
 	u32 pktlen;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct wlan_network		*cur_network = &pmlmepriv->cur_network;
+	struct wlan_network *cur_network = &pmlmepriv->cur_network;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
-	fctrl = &pwlanhdr->frame_control;
-	*(fctrl) = 0;
+	pwlanhdr->frame_control = 0;
+	pwlanhdr->seq_ctrl = 0;
+
 	if (bForcePowerSave)
-		SetPwrMgt(fctrl);
+		pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_PM);
 
-	switch (cur_network->network.InfrastructureMode) {
-	case Ndis802_11Infrastructure:
-		SetToDs(fctrl);
+	switch (cur_network->network.ifmode) {
+	case NL80211_IFTYPE_P2P_CLIENT:
+	case NL80211_IFTYPE_STATION:
+		pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_TODS);
 		memcpy(pwlanhdr->addr1,
 		       get_my_bssid23a(&pmlmeinfo->network), ETH_ALEN);
 		memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv),
 		       ETH_ALEN);
 		memcpy(pwlanhdr->addr3, StaAddr, ETH_ALEN);
 		break;
-	case Ndis802_11APMode:
-		SetFrDs(fctrl);
+	case NL80211_IFTYPE_P2P_GO:
+	case NL80211_IFTYPE_AP:
+		pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_FROMDS);
 		memcpy(pwlanhdr->addr1, StaAddr, ETH_ALEN);
 		memcpy(pwlanhdr->addr2,
 		       get_my_bssid23a(&pmlmeinfo->network), ETH_ALEN);
 		memcpy(pwlanhdr->addr3, myid(&padapter->eeprompriv),
 		       ETH_ALEN);
 		break;
-	case Ndis802_11IBSS:
+	case NL80211_IFTYPE_ADHOC:
 	default:
 		memcpy(pwlanhdr->addr1, StaAddr, ETH_ALEN);
 		memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
@@ -373,20 +379,23 @@ static void ConstructNullFunctionData(
 		break;
 	}
 
-	SetSeqNum(pwlanhdr, 0);
-
 	if (bQoS == true) {
-		struct ieee80211_qos_hdr *pwlanqoshdr;
+		struct ieee80211_qos_hdr *qoshdr;
+		qoshdr = (struct ieee80211_qos_hdr *)pframe;
 
-		SetFrameSubType(pframe, WIFI_QOS_DATA_NULL);
+		qoshdr->frame_control |=
+			cpu_to_le16(IEEE80211_FTYPE_DATA |
+				    IEEE80211_STYPE_QOS_NULLFUNC);
 
-		pwlanqoshdr = (struct ieee80211_qos_hdr *)pframe;
-		SetPriority(&pwlanqoshdr->qos_ctrl, AC);
-		SetEOSP(&pwlanqoshdr->qos_ctrl, bEosp);
+		qoshdr->qos_ctrl = cpu_to_le16(AC & IEEE80211_QOS_CTL_TID_MASK);
+		if (bEosp)
+			qoshdr->qos_ctrl |= cpu_to_le16(IEEE80211_QOS_CTL_EOSP);
 
 		pktlen = sizeof(struct ieee80211_qos_hdr);
 	} else {
-		SetFrameSubType(pframe, WIFI_DATA_NULL);
+		pwlanhdr->frame_control |=
+			cpu_to_le16(IEEE80211_FTYPE_DATA |
+				    IEEE80211_STYPE_NULLFUNC);
 
 		pktlen = sizeof(struct ieee80211_hdr_3addr);
 	}
@@ -394,32 +403,32 @@ static void ConstructNullFunctionData(
 	*pLength = pktlen;
 }
 
-static void ConstructProbeRsp(struct rtw_adapter *padapter, u8 *pframe, u32 *pLength, u8 *StaAddr, bool bHideSSID)
+static void ConstructProbeRsp(struct rtw_adapter *padapter, u8 *pframe,
+			      u32 *pLength, u8 *StaAddr, bool bHideSSID)
 {
 	struct ieee80211_hdr *pwlanhdr;
-	u16 *fctrl;
 	u8 *mac, *bssid;
 	u32 pktlen;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
 
-	/* DBG_8723A("%s\n", __FUNCTION__); */
+	/* DBG_8723A("%s\n", __func__); */
 
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
 	mac = myid(&padapter->eeprompriv);
 	bssid = cur_network->MacAddress;
 
-	fctrl = &pwlanhdr->frame_control;
-	*(fctrl) = 0;
+	pwlanhdr->frame_control =
+		cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_PROBE_RESP);
+
+	pwlanhdr->seq_ctrl = 0;
+
 	memcpy(pwlanhdr->addr1, StaAddr, ETH_ALEN);
 	memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
 	memcpy(pwlanhdr->addr3, bssid, ETH_ALEN);
 
-	SetSeqNum(pwlanhdr, 0);
-	SetFrameSubType(fctrl, WIFI_PROBERSP);
-
 	pktlen = sizeof(struct ieee80211_hdr_3addr);
 	pframe += pktlen;
 
@@ -464,11 +473,11 @@ static void SetFwRsvdPagePkt(struct rtw_adapter *padapter, bool bDLFinished)
 	u32 TotalPacketLen;
 	struct rsvdpage_loc	RsvdPageLoc;
 
-	DBG_8723A("%s\n", __FUNCTION__);
+	DBG_8723A("%s\n", __func__);
 
 	ReservedPagePacket = kzalloc(1000, GFP_KERNEL);
 	if (ReservedPagePacket == NULL) {
-		DBG_8723A("%s: alloc ReservedPagePacket fail!\n", __FUNCTION__);
+		DBG_8723A("%s: alloc ReservedPagePacket fail!\n", __func__);
 		return;
 	}
 
@@ -507,13 +516,13 @@ static void SetFwRsvdPagePkt(struct rtw_adapter *padapter, bool bDLFinished)
 
 	/* 3 (3) null data */
 	RsvdPageLoc.LocNullData = PageNum;
-	ConstructNullFunctionData(
-		padapter,
-		&ReservedPagePacket[BufIndex],
-		&NullDataLength,
-		get_my_bssid23a(&pmlmeinfo->network),
-		false, 0, 0, false);
-	rtl8723a_fill_fake_txdesc(padapter, &ReservedPagePacket[BufIndex-TxDescLen], NullDataLength, false, false);
+	ConstructNullFunctionData(padapter, &ReservedPagePacket[BufIndex],
+				  &NullDataLength,
+				  get_my_bssid23a(&pmlmeinfo->network),
+				  false, 0, 0, false);
+	rtl8723a_fill_fake_txdesc(padapter,
+				  &ReservedPagePacket[BufIndex-TxDescLen],
+				  NullDataLength, false, false);
 
 	PageNeed = (u8)PageNum_128(TxDescLen + NullDataLength);
 	PageNum += PageNeed;
@@ -573,9 +582,9 @@ static void SetFwRsvdPagePkt(struct rtw_adapter *padapter, bool bDLFinished)
 	pattrib->pktlen = pattrib->last_txcmdsz = TotalPacketLen - TXDESC_OFFSET;
 	memcpy(pmgntframe->buf_addr, ReservedPagePacket, TotalPacketLen);
 
-	rtw_hal_mgnt_xmit23a(padapter, pmgntframe);
+	rtl8723au_mgnt_xmit(padapter, pmgntframe);
 
-	DBG_8723A("%s: Set RSVD page location to Fw\n", __FUNCTION__);
+	DBG_8723A("%s: Set RSVD page location to Fw\n", __func__);
 	FillH2CCmd(padapter, RSVD_PAGE_EID, sizeof(RsvdPageLoc), (u8 *)&RsvdPageLoc);
 
 exit:
@@ -589,7 +598,7 @@ void rtl8723a_set_FwJoinBssReport_cmd(struct rtw_adapter *padapter, u8 mstatus)
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
-	DBG_8723A("%s mstatus(%x)\n", __FUNCTION__, mstatus);
+	DBG_8723A("%s mstatus(%x)\n", __func__, mstatus);
 
 	if (mstatus == 1) {
 		bool bRecover = false;
@@ -597,17 +606,18 @@ void rtl8723a_set_FwJoinBssReport_cmd(struct rtw_adapter *padapter, u8 mstatus)
 
 		/*  We should set AID, correct TSF, HW seq enable before set JoinBssReport to Fw in 88/92C. */
 		/*  Suggested by filen. Added by tynli. */
-		rtw_write16(padapter, REG_BCN_PSR_RPT, (0xC000|pmlmeinfo->aid));
+		rtl8723au_write16(padapter, REG_BCN_PSR_RPT,
+				  0xC000|pmlmeinfo->aid);
 		/*  Do not set TSF again here or vWiFi beacon DMA INT will not work. */
 		/* correct_TSF23a(padapter, pmlmeext); */
 		/*  Hw sequende enable by dedault. 2010.06.23. by tynli. */
-		/* rtw_write16(padapter, REG_NQOS_SEQ, ((pmlmeext->mgnt_seq+100)&0xFFF)); */
-		/* rtw_write8(padapter, REG_HWSEQ_CTRL, 0xFF); */
+		/* rtl8723au_write16(padapter, REG_NQOS_SEQ, ((pmlmeext->mgnt_seq+100)&0xFFF)); */
+		/* rtl8723au_write8(padapter, REG_HWSEQ_CTRL, 0xFF); */
 
 		/*  set REG_CR bit 8 */
-		v8 = rtw_read8(padapter, REG_CR+1);
+		v8 = rtl8723au_read8(padapter, REG_CR+1);
 		v8 |= BIT(0); /*  ENSWBCN */
-		rtw_write8(padapter,  REG_CR+1, v8);
+		rtl8723au_write8(padapter,  REG_CR+1, v8);
 
 		/*  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. */
@@ -621,8 +631,9 @@ void rtl8723a_set_FwJoinBssReport_cmd(struct rtw_adapter *padapter, u8 mstatus)
 			bRecover = true;
 
 		/*  To tell Hw the packet is not a real beacon frame. */
-		/* U1bTmp = rtw_read8(padapter, REG_FWHW_TXQ_CTRL+2); */
-		rtw_write8(padapter, REG_FWHW_TXQ_CTRL+2, pHalData->RegFwHwTxQCtrl & ~BIT(6));
+		/* U1bTmp = rtl8723au_read8(padapter, REG_FWHW_TXQ_CTRL+2); */
+		rtl8723au_write8(padapter, REG_FWHW_TXQ_CTRL + 2,
+				 pHalData->RegFwHwTxQCtrl & ~BIT(6));
 		pHalData->RegFwHwTxQCtrl &= ~BIT(6);
 		SetFwRsvdPagePkt(padapter, 0);
 
@@ -635,14 +646,15 @@ void rtl8723a_set_FwJoinBssReport_cmd(struct rtw_adapter *padapter, u8 mstatus)
 		/*  the beacon cannot be sent by HW. */
 		/*  2010.06.23. Added by tynli. */
 		if (bRecover) {
-			rtw_write8(padapter, REG_FWHW_TXQ_CTRL+2, pHalData->RegFwHwTxQCtrl | BIT(6));
+			rtl8723au_write8(padapter, REG_FWHW_TXQ_CTRL + 2,
+					 pHalData->RegFwHwTxQCtrl | BIT(6));
 			pHalData->RegFwHwTxQCtrl |= BIT(6);
 		}
 
 		/*  Clear CR[8] or beacon packet will not be send to TxBuf anymore. */
-		v8 = rtw_read8(padapter, REG_CR+1);
+		v8 = rtl8723au_read8(padapter, REG_CR+1);
 		v8 &= ~BIT(0); /*  ~ENSWBCN */
-		rtw_write8(padapter, REG_CR+1, v8);
+		rtl8723au_write8(padapter, REG_CR+1, v8);
 	}
 
 	JoinBssRptParm.OpMode = mstatus;
@@ -668,11 +680,11 @@ static void SetFwRsvdPagePkt_BTCoex(struct rtw_adapter *padapter)
 	u32 TotalPacketLen;
 	struct rsvdpage_loc	RsvdPageLoc;
 
-	DBG_8723A("+%s\n", __FUNCTION__);
+	DBG_8723A("+%s\n", __func__);
 
 	ReservedPagePacket = kzalloc(1024, GFP_KERNEL);
 	if (ReservedPagePacket == NULL) {
-		DBG_8723A("%s: alloc ReservedPagePacket fail!\n", __FUNCTION__);
+		DBG_8723A("%s: alloc ReservedPagePacket fail!\n", __func__);
 		return;
 	}
 
@@ -732,9 +744,9 @@ static void SetFwRsvdPagePkt_BTCoex(struct rtw_adapter *padapter)
 	pattrib->pktlen = pattrib->last_txcmdsz = TotalPacketLen - TXDESC_OFFSET;
 	memcpy(pmgntframe->buf_addr, ReservedPagePacket, TotalPacketLen);
 
-	rtw_hal_mgnt_xmit23a(padapter, pmgntframe);
+	rtl8723au_mgnt_xmit(padapter, pmgntframe);
 
-	DBG_8723A("%s: Set RSVD page location to Fw\n", __FUNCTION__);
+	DBG_8723A("%s: Set RSVD page location to Fw\n", __func__);
 	FillH2CCmd(padapter, RSVD_PAGE_EID, sizeof(RsvdPageLoc), (u8 *)&RsvdPageLoc);
 
 exit:
@@ -746,7 +758,7 @@ void rtl8723a_set_BTCoex_AP_mode_FwRsvdPkt_cmd(struct rtw_adapter *padapter)
 	struct hal_data_8723a *pHalData;
 	u8 bRecover = false;
 
-	DBG_8723A("+%s\n", __FUNCTION__);
+	DBG_8723A("+%s\n", __func__);
 
 	pHalData = GET_HAL_DATA(padapter);
 
@@ -756,7 +768,8 @@ void rtl8723a_set_BTCoex_AP_mode_FwRsvdPkt_cmd(struct rtw_adapter *padapter)
 
 	/*  To tell Hw the packet is not a real beacon frame. */
 	pHalData->RegFwHwTxQCtrl &= ~BIT(6);
-	rtw_write8(padapter, REG_FWHW_TXQ_CTRL+2, pHalData->RegFwHwTxQCtrl);
+	rtl8723au_write8(padapter, REG_FWHW_TXQ_CTRL + 2,
+			 pHalData->RegFwHwTxQCtrl);
 	SetFwRsvdPagePkt_BTCoex(padapter);
 
 	/*  To make sure that if there exists an adapter which would like to send beacon. */
@@ -766,80 +779,8 @@ void rtl8723a_set_BTCoex_AP_mode_FwRsvdPkt_cmd(struct rtw_adapter *padapter)
 	/*  2010.06.23. Added by tynli. */
 	if (bRecover) {
 		pHalData->RegFwHwTxQCtrl |= BIT(6);
-		rtw_write8(padapter, REG_FWHW_TXQ_CTRL+2, pHalData->RegFwHwTxQCtrl);
+		rtl8723au_write8(padapter, REG_FWHW_TXQ_CTRL + 2,
+				 pHalData->RegFwHwTxQCtrl);
 	}
 }
 #endif
-
-#ifdef CONFIG_8723AU_P2P
-void rtl8723a_set_p2p_ps_offload_cmd(struct rtw_adapter *padapter, u8 p2p_ps_state)
-{
-	struct hal_data_8723a	*pHalData = GET_HAL_DATA(padapter);
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	struct P2P_PS_Offload_t	*p2p_ps_offload = &pHalData->p2p_ps_offload;
-	u8 i;
-
-	switch (p2p_ps_state) {
-	case P2P_PS_DISABLE:
-		DBG_8723A("P2P_PS_DISABLE \n");
-		memset(p2p_ps_offload, 0, 1);
-		break;
-	case P2P_PS_ENABLE:
-		DBG_8723A("P2P_PS_ENABLE \n");
-		/*  update CTWindow value. */
-		if (pwdinfo->ctwindow > 0) {
-			p2p_ps_offload->CTWindow_En = 1;
-			rtw_write8(padapter, REG_P2P_CTWIN, pwdinfo->ctwindow);
-		}
-
-		/*  hw only support 2 set of NoA */
-		for (i = 0; i < pwdinfo->noa_num; i++) {
-			/*  To control the register setting for which NOA */
-			rtw_write8(padapter, REG_NOA_DESC_SEL, (i << 4));
-			if (i == 0)
-				p2p_ps_offload->NoA0_En = 1;
-			else
-				p2p_ps_offload->NoA1_En = 1;
-
-			/*  config P2P NoA Descriptor Register */
-			rtw_write32(padapter, REG_NOA_DESC_DURATION, pwdinfo->noa_duration[i]);
-
-			rtw_write32(padapter, REG_NOA_DESC_INTERVAL, pwdinfo->noa_interval[i]);
-
-			rtw_write32(padapter, REG_NOA_DESC_START, pwdinfo->noa_start_time[i]);
-
-			rtw_write8(padapter, REG_NOA_DESC_COUNT, pwdinfo->noa_count[i]);
-		}
-
-		if ((pwdinfo->opp_ps == 1) || (pwdinfo->noa_num > 0)) {
-			/*  rst p2p circuit */
-			rtw_write8(padapter, REG_DUAL_TSF_RST, BIT(4));
-
-			p2p_ps_offload->Offload_En = 1;
-
-			if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
-				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:
-		DBG_8723A("P2P_PS_SCAN \n");
-		p2p_ps_offload->discovery = 1;
-		break;
-	case P2P_PS_SCAN_DONE:
-		DBG_8723A("P2P_PS_SCAN_DONE \n");
-		p2p_ps_offload->discovery = 0;
-		pwdinfo->p2p_ps_state = P2P_PS_ENABLE;
-		break;
-	default:
-		break;
-	}
-
-	FillH2CCmd(padapter, P2P_PS_OFFLOAD_EID, 1, (u8 *)p2p_ps_offload);
-}
-#endif /* CONFIG_8723AU_P2P */
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_dm.c b/drivers/staging/rtl8723au/hal/rtl8723a_dm.c
index f204ab1714e..ac47a971108 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_dm.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_dm.c
@@ -28,6 +28,7 @@
 #include <drv_types.h>
 
 #include <rtl8723a_hal.h>
+#include <usb_ops_linux.h>
 
 /*  */
 /*  Global var */
@@ -45,18 +46,21 @@ static void dm_CheckPbcGPIO(struct rtw_adapter *padapter)
 	if (!padapter->registrypriv.hw_wps_pbc)
 		return;
 
-	tmp1byte = rtw_read8(padapter, GPIO_IO_SEL);
+	tmp1byte = rtl8723au_read8(padapter, GPIO_IO_SEL);
 	tmp1byte |= (HAL_8192C_HW_GPIO_WPS_BIT);
-	rtw_write8(padapter, GPIO_IO_SEL, tmp1byte);	/* enable GPIO[2] as output mode */
+	/* enable GPIO[2] as output mode */
+	rtl8723au_write8(padapter, GPIO_IO_SEL, tmp1byte);
 
 	tmp1byte &= ~(HAL_8192C_HW_GPIO_WPS_BIT);
-	rtw_write8(padapter,  GPIO_IN, tmp1byte);		/* reset the floating voltage level */
+	/* reset the floating voltage level */
+	rtl8723au_write8(padapter,  GPIO_IN, tmp1byte);
 
-	tmp1byte = rtw_read8(padapter, GPIO_IO_SEL);
+	tmp1byte = rtl8723au_read8(padapter, GPIO_IO_SEL);
 	tmp1byte &= ~(HAL_8192C_HW_GPIO_WPS_BIT);
-	rtw_write8(padapter, GPIO_IO_SEL, tmp1byte);	/* enable GPIO[2] as input mode */
+	/* enable GPIO[2] as input mode */
+	rtl8723au_write8(padapter, GPIO_IO_SEL, tmp1byte);
 
-	tmp1byte = rtw_read8(padapter, GPIO_IN);
+	tmp1byte = rtl8723au_read8(padapter, GPIO_IN);
 
 	if (tmp1byte == 0xff)
 		return;
@@ -76,7 +80,7 @@ static void dm_CheckPbcGPIO(struct rtw_adapter *padapter)
 			return;
 		}
 
-		rtw_signal_process(padapter->pid[0], SIGUSR1);
+		kill_pid(find_vpid(padapter->pid[0]), SIGUSR1, 1);
 	}
 }
 
@@ -197,7 +201,7 @@ void rtl8723a_InitHalDm(struct rtw_adapter *Adapter)
 	ODM23a_DMInit(pDM_Odm);
 	/*  Save REG_INIDATA_RATE_SEL value for TXDESC. */
 	for (i = 0; i < 32; i++)
-		pdmpriv->INIDATA_RATE[i] = rtw_read8(Adapter, REG_INIDATA_RATE_SEL+i) & 0x3f;
+		pdmpriv->INIDATA_RATE[i] = rtl8723au_read8(Adapter, REG_INIDATA_RATE_SEL+i) & 0x3f;
 }
 
 void
@@ -217,14 +221,7 @@ rtl8723a_HalDmWatchDog(
 		goto skip_dm;
 
 	bFwCurrentInPSMode = Adapter->pwrctrlpriv.bFwCurrentInPSMode;
-	rtw23a_hal_get_hwreg(Adapter, HW_VAR_FWLPS_RF_ON, (u8 *)(&bFwPSAwake));
-
-#ifdef CONFIG_8723AU_P2P
-	/*  Fw is under p2p powersaving mode, driver should stop dynamic mechanism. */
-	/*  modifed by thomas. 2011.06.11. */
-	if (Adapter->wdinfo.p2p_ps_mode)
-		bFwPSAwake = false;
-#endif /* CONFIG_8723AU_P2P */
+	bFwPSAwake = rtl8723a_get_fwlps_rf_on(Adapter);
 
 	if ((hw_init_completed) && ((!bFwCurrentInPSMode) && bFwPSAwake)) {
 		/*  Calculate Tx/Rx statistics. */
@@ -232,11 +229,11 @@ rtl8723a_HalDmWatchDog(
 
 		/*  Read REG_INIDATA_RATE_SEL value for TXDESC. */
 		if (check_fwstate(&Adapter->mlmepriv, WIFI_STATION_STATE)) {
-			pdmpriv->INIDATA_RATE[0] = rtw_read8(Adapter, REG_INIDATA_RATE_SEL) & 0x3f;
+			pdmpriv->INIDATA_RATE[0] = rtl8723au_read8(Adapter, REG_INIDATA_RATE_SEL) & 0x3f;
 		} else {
 			u8	i;
 			for (i = 1 ; i < (Adapter->stapriv.asoc_sta_count + 1); i++)
-				pdmpriv->INIDATA_RATE[i] = rtw_read8(Adapter, (REG_INIDATA_RATE_SEL+i)) & 0x3f;
+				pdmpriv->INIDATA_RATE[i] = rtl8723au_read8(Adapter, (REG_INIDATA_RATE_SEL+i)) & 0x3f;
 		}
 	}
 
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_hal_init.c b/drivers/staging/rtl8723au/hal/rtl8723a_hal_init.c
index 0982b0a4ab9..0acacab95a4 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_hal_init.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_hal_init.c
@@ -19,6 +19,7 @@
 #include <rtw_efuse.h>
 
 #include <rtl8723a_hal.h>
+#include <usb_ops_linux.h>
 
 static void _FWDownloadEnable(struct rtw_adapter *padapter, bool enable)
 {
@@ -26,23 +27,23 @@ static void _FWDownloadEnable(struct rtw_adapter *padapter, bool enable)
 
 	if (enable) {
 		/*  8051 enable */
-		tmp = rtw_read8(padapter, REG_SYS_FUNC_EN + 1);
-		rtw_write8(padapter, REG_SYS_FUNC_EN + 1, tmp | 0x04);
+		tmp = rtl8723au_read8(padapter, REG_SYS_FUNC_EN + 1);
+		rtl8723au_write8(padapter, REG_SYS_FUNC_EN + 1, tmp | 0x04);
 
 		/*  MCU firmware download enable. */
-		tmp = rtw_read8(padapter, REG_MCUFWDL);
-		rtw_write8(padapter, REG_MCUFWDL, tmp | 0x01);
+		tmp = rtl8723au_read8(padapter, REG_MCUFWDL);
+		rtl8723au_write8(padapter, REG_MCUFWDL, tmp | 0x01);
 
 		/*  8051 reset */
-		tmp = rtw_read8(padapter, REG_MCUFWDL + 2);
-		rtw_write8(padapter, REG_MCUFWDL + 2, tmp & 0xf7);
+		tmp = rtl8723au_read8(padapter, REG_MCUFWDL + 2);
+		rtl8723au_write8(padapter, REG_MCUFWDL + 2, tmp & 0xf7);
 	} else {
 		/*  MCU firmware download disable. */
-		tmp = rtw_read8(padapter, REG_MCUFWDL);
-		rtw_write8(padapter, REG_MCUFWDL, tmp & 0xfe);
+		tmp = rtl8723au_read8(padapter, REG_MCUFWDL);
+		rtl8723au_write8(padapter, REG_MCUFWDL, tmp & 0xfe);
 
 		/*  Reserved for fw extension. */
-		rtw_write8(padapter, REG_MCUFWDL + 1, 0x00);
+		rtl8723au_write8(padapter, REG_MCUFWDL + 1, 0x00);
 	}
 }
 
@@ -75,9 +76,10 @@ static int _BlockWrite(struct rtw_adapter *padapter, void *buffer, u32 buffSize)
 	}
 
 	for (i = 0; i < blockCount_p1; i++) {
-		ret = rtw_writeN(padapter,
-				 (FW_8723A_START_ADDRESS + i * blockSize_p1),
-				 blockSize_p1, (bufferPtr + i * blockSize_p1));
+		ret = rtl8723au_writeN(padapter, (FW_8723A_START_ADDRESS +
+						  i * blockSize_p1),
+				       blockSize_p1,
+				       (bufferPtr + i * blockSize_p1));
 		if (ret == _FAIL)
 			goto exit;
 	}
@@ -99,11 +101,12 @@ static int _BlockWrite(struct rtw_adapter *padapter, void *buffer, u32 buffSize)
 		}
 
 		for (i = 0; i < blockCount_p2; i++) {
-			ret = rtw_writeN(padapter,
-					 (FW_8723A_START_ADDRESS + offset +
-					  i * blockSize_p2), blockSize_p2,
-					 (bufferPtr + offset +
-					  i * blockSize_p2));
+			ret = rtl8723au_writeN(padapter,
+					       (FW_8723A_START_ADDRESS +
+						offset + i * blockSize_p2),
+					       blockSize_p2,
+					       (bufferPtr + offset +
+						i * blockSize_p2));
 
 			if (ret == _FAIL)
 				goto exit;
@@ -123,9 +126,9 @@ static int _BlockWrite(struct rtw_adapter *padapter, void *buffer, u32 buffSize)
 			  (buffSize - offset), blockSize_p3, blockCount_p3));
 
 		for (i = 0; i < blockCount_p3; i++) {
-			ret = rtw_write8(padapter,
-					 (FW_8723A_START_ADDRESS + offset + i),
-					 *(bufferPtr + offset + i));
+			ret = rtl8723au_write8(padapter,
+					       (FW_8723A_START_ADDRESS + offset + i),
+					       *(bufferPtr + offset + i));
 
 			if (ret == _FAIL)
 				goto exit;
@@ -142,8 +145,8 @@ _PageWrite(struct rtw_adapter *padapter, u32 page, void *buffer, u32 size)
 	u8 value8;
 	u8 u8Page = (u8) (page & 0x07);
 
-	value8 = (rtw_read8(padapter, REG_MCUFWDL + 2) & 0xF8) | u8Page;
-	rtw_write8(padapter, REG_MCUFWDL + 2, value8);
+	value8 = (rtl8723au_read8(padapter, REG_MCUFWDL + 2) & 0xF8) | u8Page;
+	rtl8723au_write8(padapter, REG_MCUFWDL + 2, value8);
 
 	return _BlockWrite(padapter, buffer, size);
 }
@@ -187,14 +190,14 @@ exit:
 	return ret;
 }
 
-static s32 _FWFreeToGo(struct rtw_adapter *padapter)
+static int _FWFreeToGo(struct rtw_adapter *padapter)
 {
 	u32 counter = 0;
 	u32 value32;
 
 	/*  polling CheckSum report */
 	do {
-		value32 = rtw_read32(padapter, REG_MCUFWDL);
+		value32 = rtl8723au_read32(padapter, REG_MCUFWDL);
 		if (value32 & FWDL_ChkSum_rpt)
 			break;
 	} while (counter++ < POLLING_READY_TIMEOUT_COUNT);
@@ -209,15 +212,15 @@ static s32 _FWFreeToGo(struct rtw_adapter *padapter)
 		 ("%s: Checksum report OK! REG_MCUFWDL:0x%08x\n", __func__,
 		  value32));
 
-	value32 = rtw_read32(padapter, REG_MCUFWDL);
+	value32 = rtl8723au_read32(padapter, REG_MCUFWDL);
 	value32 |= MCUFWDL_RDY;
 	value32 &= ~WINTINI_RDY;
-	rtw_write32(padapter, REG_MCUFWDL, value32);
+	rtl8723au_write32(padapter, REG_MCUFWDL, value32);
 
 	/*  polling for FW ready */
 	counter = 0;
 	do {
-		value32 = rtw_read32(padapter, REG_MCUFWDL);
+		value32 = rtl8723au_read32(padapter, REG_MCUFWDL);
 		if (value32 & WINTINI_RDY) {
 			RT_TRACE(_module_hal_init_c_, _drv_info_,
 				 ("%s: Polling FW ready success!! "
@@ -248,15 +251,15 @@ void rtl8723a_FirmwareSelfReset(struct rtw_adapter *padapter)
 		pHalData->FirmwareSubVersion < 0x01)))) {
 		/*  after 88C Fw v33.1 */
 		/* 0x1cf = 0x20. Inform 8051 to reset. 2009.12.25. tynli_test */
-		rtw_write8(padapter, REG_HMETFR + 3, 0x20);
+		rtl8723au_write8(padapter, REG_HMETFR + 3, 0x20);
 
-		u1bTmp = rtw_read8(padapter, REG_SYS_FUNC_EN + 1);
-		while (u1bTmp & BIT2) {
+		u1bTmp = rtl8723au_read8(padapter, REG_SYS_FUNC_EN + 1);
+		while (u1bTmp & BIT(2)) {
 			Delay--;
 			if (Delay == 0)
 				break;
 			udelay(50);
-			u1bTmp = rtw_read8(padapter, REG_SYS_FUNC_EN + 1);
+			u1bTmp = rtl8723au_read8(padapter, REG_SYS_FUNC_EN + 1);
 		}
 		RT_TRACE(_module_hal_init_c_, _drv_info_,
 			 ("-%s: 8051 reset success (%d)\n", __func__,
@@ -264,9 +267,9 @@ void rtl8723a_FirmwareSelfReset(struct rtw_adapter *padapter)
 
 		if ((Delay == 0)) {
 			/* force firmware reset */
-			u1bTmp = rtw_read8(padapter, REG_SYS_FUNC_EN + 1);
-			rtw_write8(padapter, REG_SYS_FUNC_EN + 1,
-				   u1bTmp & (~BIT2));
+			u1bTmp = rtl8723au_read8(padapter, REG_SYS_FUNC_EN + 1);
+			rtl8723au_write8(padapter, REG_SYS_FUNC_EN + 1,
+					 u1bTmp & ~BIT(2));
 		}
 	}
 }
@@ -276,9 +279,9 @@ void rtl8723a_FirmwareSelfReset(struct rtw_adapter *padapter)
 /*		Download 8192C firmware code. */
 /*  */
 /*  */
-s32 rtl8723a_FirmwareDownload(struct rtw_adapter *padapter)
+int rtl8723a_FirmwareDownload(struct rtw_adapter *padapter)
 {
-	s32 rtStatus = _SUCCESS;
+	int rtStatus = _SUCCESS;
 	u8 writeFW_retry = 0;
 	unsigned long fwdl_start_time;
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
@@ -306,15 +309,15 @@ s32 rtl8723a_FirmwareDownload(struct rtw_adapter *padapter)
 			DBG_8723A(" Rtl8723_FwUMCBCutImageArrayWithoutBT for "
 				  "RTL8723A B CUT\n");
 		} else {
-#ifdef CONFIG_8723AU_BT_COEXIST
-			fw_name = "rtlwifi/rtl8723aufw_B.bin";
-			DBG_8723A(" Rtl8723_FwUMCBCutImageArrayWithBT for "
-				  "RTL8723A B CUT\n");
-#else
-			fw_name = "rtlwifi/rtl8723aufw_B_NoBT.bin";
-			DBG_8723A(" Rtl8723_FwUMCBCutImageArrayWithoutBT for "
-				  "RTL8723A B CUT\n");
-#endif
+			if (rtl8723a_BT_coexist(padapter)) {
+				fw_name = "rtlwifi/rtl8723aufw_B.bin";
+				DBG_8723A(" Rtl8723_FwUMCBCutImageArrayWithBT "
+					  "for RTL8723A B CUT\n");
+			} else {
+				fw_name = "rtlwifi/rtl8723aufw_B_NoBT.bin";
+				DBG_8723A(" Rtl8723_FwUMCBCutImageArrayWithout "
+					  "BT for RTL8723A B CUT\n");
+			}
 		}
 	} else {
 		/*  <Roger_TODO> We should download proper RAM Code here
@@ -336,12 +339,11 @@ s32 rtl8723a_FirmwareDownload(struct rtw_adapter *padapter)
 		rtStatus = _FAIL;
 		goto Exit;
 	}
-	firmware_buf = kzalloc(fw->size, GFP_KERNEL);
+	firmware_buf = kmemdup(fw->data, fw->size, GFP_KERNEL);
 	if (!firmware_buf) {
 		rtStatus = _FAIL;
 		goto Exit;
 	}
-	memcpy(firmware_buf, fw->data, fw->size);
 	buf = firmware_buf;
 	fw_size = fw->size;
 	release_firmware(fw);
@@ -372,18 +374,19 @@ s32 rtl8723a_FirmwareDownload(struct rtw_adapter *padapter)
 	/*  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 (rtw_read8(padapter, REG_MCUFWDL) & RAM_DL_SEL) {
+	if (rtl8723au_read8(padapter, REG_MCUFWDL) & RAM_DL_SEL) {
 		/* 8051 RAM code */
 		rtl8723a_FirmwareSelfReset(padapter);
-		rtw_write8(padapter, REG_MCUFWDL, 0x00);
+		rtl8723au_write8(padapter, REG_MCUFWDL, 0x00);
 	}
 
 	_FWDownloadEnable(padapter, true);
 	fwdl_start_time = jiffies;
 	while (1) {
 		/* reset the FWDL chksum */
-		rtw_write8(padapter, REG_MCUFWDL,
-			   rtw_read8(padapter, REG_MCUFWDL) | FWDL_ChkSum_rpt);
+		rtl8723au_write8(padapter, REG_MCUFWDL,
+				 rtl8723au_read8(padapter, REG_MCUFWDL) |
+				 FWDL_ChkSum_rpt);
 
 		rtStatus = _WriteFW(padapter, buf, fw_size);
 
@@ -427,14 +430,6 @@ void rtl8723a_InitializeFirmwareVars(struct rtw_adapter *padapter)
 	pHalData->LastHMEBoxNum = 0;
 }
 
-static void rtl8723a_free_hal_data(struct rtw_adapter *padapter)
-{
-
-	kfree(padapter->HalData);
-	padapter->HalData = NULL;
-
-}
-
 /*  */
 /*				Efuse related code */
 /*  */
@@ -445,7 +440,7 @@ hal_EfuseSwitchToBank(struct rtw_adapter *padapter, u8 bank)
 	u32 value32 = 0;
 
 	DBG_8723A("%s: Efuse switch bank to %d\n", __func__, bank);
-	value32 = rtw_read32(padapter, EFUSE_TEST);
+	value32 = rtl8723au_read32(padapter, EFUSE_TEST);
 	bRet = true;
 	switch (bank) {
 	case 0:
@@ -470,147 +465,12 @@ hal_EfuseSwitchToBank(struct rtw_adapter *padapter, u8 bank)
 		bRet = false;
 		break;
 	}
-	rtw_write32(padapter, EFUSE_TEST, value32);
+	rtl8723au_write32(padapter, EFUSE_TEST, value32);
 
 	return bRet;
 }
 
 static void
-Hal_GetEfuseDefinition(struct rtw_adapter *padapter,
-		       u8 efuseType, u8 type, void *pOut)
-{
-	u8 *pu1Tmp;
-	u16 *pu2Tmp;
-	u8 *pMax_section;
-
-	switch (type) {
-	case TYPE_EFUSE_MAX_SECTION:
-		pMax_section = (u8 *) pOut;
-
-		if (efuseType == EFUSE_WIFI)
-			*pMax_section = EFUSE_MAX_SECTION_8723A;
-		else
-			*pMax_section = EFUSE_BT_MAX_SECTION;
-		break;
-
-	case TYPE_EFUSE_REAL_CONTENT_LEN:
-		pu2Tmp = (u16 *) pOut;
-
-		if (efuseType == EFUSE_WIFI)
-			*pu2Tmp = EFUSE_REAL_CONTENT_LEN_8723A;
-		else
-			*pu2Tmp = EFUSE_BT_REAL_CONTENT_LEN;
-		break;
-
-	case TYPE_AVAILABLE_EFUSE_BYTES_BANK:
-		pu2Tmp = (u16 *) pOut;
-
-		if (efuseType == EFUSE_WIFI)
-			*pu2Tmp = (EFUSE_REAL_CONTENT_LEN_8723A -
-				   EFUSE_OOB_PROTECT_BYTES);
-		else
-			*pu2Tmp = (EFUSE_BT_REAL_BANK_CONTENT_LEN -
-				   EFUSE_PROTECT_BYTES_BANK);
-		break;
-
-	case TYPE_AVAILABLE_EFUSE_BYTES_TOTAL:
-		pu2Tmp = (u16 *) pOut;
-
-		if (efuseType == EFUSE_WIFI)
-			*pu2Tmp = (EFUSE_REAL_CONTENT_LEN_8723A -
-				   EFUSE_OOB_PROTECT_BYTES);
-		else
-			*pu2Tmp = (EFUSE_BT_REAL_CONTENT_LEN -
-				   (EFUSE_PROTECT_BYTES_BANK * 3));
-		break;
-
-	case TYPE_EFUSE_MAP_LEN:
-		pu2Tmp = (u16 *) pOut;
-
-		if (efuseType == EFUSE_WIFI)
-			*pu2Tmp = EFUSE_MAP_LEN_8723A;
-		else
-			*pu2Tmp = EFUSE_BT_MAP_LEN;
-		break;
-
-	case TYPE_EFUSE_PROTECT_BYTES_BANK:
-		pu1Tmp = (u8 *) pOut;
-
-		if (efuseType == EFUSE_WIFI)
-			*pu1Tmp = EFUSE_OOB_PROTECT_BYTES;
-		else
-			*pu1Tmp = EFUSE_PROTECT_BYTES_BANK;
-		break;
-
-	case TYPE_EFUSE_CONTENT_LEN_BANK:
-		pu2Tmp = (u16 *) pOut;
-
-		if (efuseType == EFUSE_WIFI)
-			*pu2Tmp = EFUSE_REAL_CONTENT_LEN_8723A;
-		else
-			*pu2Tmp = EFUSE_BT_REAL_BANK_CONTENT_LEN;
-		break;
-
-	default:
-		pu1Tmp = (u8 *) pOut;
-		*pu1Tmp = 0;
-		break;
-	}
-}
-
-#define VOLTAGE_V25		0x03
-#define LDOE25_SHIFT	28
-
-static void
-Hal_EfusePowerSwitch(struct rtw_adapter *padapter, u8 bWrite, u8 PwrState)
-{
-	u8 tempval;
-	u16 tmpV16;
-
-	if (PwrState == true) {
-		rtw_write8(padapter, REG_EFUSE_ACCESS, EFUSE_ACCESS_ON);
-
-		/*  1.2V Power: From VDDON with Power
-		    Cut(0x0000h[15]), defualt valid */
-		tmpV16 = rtw_read16(padapter, REG_SYS_ISO_CTRL);
-		if (!(tmpV16 & PWC_EV12V)) {
-			tmpV16 |= PWC_EV12V;
-			rtw_write16(padapter, REG_SYS_ISO_CTRL, tmpV16);
-		}
-		/*  Reset: 0x0000h[28], default valid */
-		tmpV16 = rtw_read16(padapter, REG_SYS_FUNC_EN);
-		if (!(tmpV16 & FEN_ELDR)) {
-			tmpV16 |= FEN_ELDR;
-			rtw_write16(padapter, REG_SYS_FUNC_EN, tmpV16);
-		}
-
-		/*  Clock: Gated(0x0008h[5]) 8M(0x0008h[1]) clock
-		    from ANA, default valid */
-		tmpV16 = rtw_read16(padapter, REG_SYS_CLKR);
-		if ((!(tmpV16 & LOADER_CLK_EN)) || (!(tmpV16 & ANA8M))) {
-			tmpV16 |= (LOADER_CLK_EN | ANA8M);
-			rtw_write16(padapter, REG_SYS_CLKR, tmpV16);
-		}
-
-		if (bWrite == true) {
-			/*  Enable LDO 2.5V before read/write action */
-			tempval = rtw_read8(padapter, EFUSE_TEST + 3);
-			tempval &= 0x0F;
-			tempval |= (VOLTAGE_V25 << 4);
-			rtw_write8(padapter, EFUSE_TEST + 3, (tempval | 0x80));
-		}
-	} else {
-		rtw_write8(padapter, REG_EFUSE_ACCESS, EFUSE_ACCESS_OFF);
-
-		if (bWrite == true) {
-			/*  Disable LDO 2.5V after read/write action */
-			tempval = rtw_read8(padapter, EFUSE_TEST + 3);
-			rtw_write8(padapter, EFUSE_TEST + 3, (tempval & 0x7F));
-		}
-	}
-}
-
-static void
 hal_ReadEFuse_WiFi(struct rtw_adapter *padapter,
 		   u16 _offset, u16 _size_byte, u8 *pbuf)
 {
@@ -619,6 +479,7 @@ hal_ReadEFuse_WiFi(struct rtw_adapter *padapter,
 	u8 offset, wden;
 	u8 efuseHeader, efuseExtHdr, efuseData;
 	u16 i, total, used;
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
 	/*  Do NOT excess total size of EFuse table.
 	    Added by Roger, 2008.11.10. */
@@ -628,7 +489,7 @@ hal_ReadEFuse_WiFi(struct rtw_adapter *padapter,
 		return;
 	}
 
-	efuseTbl = (u8 *) kmalloc(EFUSE_MAP_LEN_8723A, GFP_KERNEL);
+	efuseTbl = kmalloc(EFUSE_MAP_LEN_8723A, GFP_KERNEL);
 	if (efuseTbl == NULL) {
 		DBG_8723A("%s: alloc efuseTbl fail!\n", __func__);
 		return;
@@ -696,7 +557,7 @@ hal_ReadEFuse_WiFi(struct rtw_adapter *padapter,
 	EFUSE_GetEfuseDefinition23a(padapter, EFUSE_WIFI,
 				 TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, &total);
 	used = eFuse_Addr - 1;
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_EFUSE_BYTES, (u8 *)&used);
+	pHalData->EfuseUsedBytes = used;
 
 	kfree(efuseTbl);
 }
@@ -711,6 +572,7 @@ hal_ReadEFuse_BT(struct rtw_adapter *padapter,
 	u8 efuseHeader, efuseExtHdr, efuseData;
 	u8 offset, wden;
 	u16 i, total, used;
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
 	/*  Do NOT excess total size of EFuse table.
 	    Added by Roger, 2008.11.10. */
@@ -812,15 +674,15 @@ hal_ReadEFuse_BT(struct rtw_adapter *padapter,
 	EFUSE_GetEfuseDefinition23a(padapter, EFUSE_BT,
 				 TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, &total);
 	used = (EFUSE_BT_REAL_BANK_CONTENT_LEN * (bank - 1)) + eFuse_Addr - 1;
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_EFUSE_BT_BYTES, (u8 *) &used);
+	pHalData->BTEfuseUsedBytes = used;
 
 exit:
 	kfree(efuseTbl);
 }
 
-static void
-Hal_ReadEFuse(struct rtw_adapter *padapter,
-	      u8 efuseType, u16 _offset, u16 _size_byte, u8 *pbuf)
+void
+rtl8723a_readefuse(struct rtw_adapter *padapter,
+		   u8 efuseType, u16 _offset, u16 _size_byte, u8 *pbuf)
 {
 	if (efuseType == EFUSE_WIFI)
 		hal_ReadEFuse_WiFi(padapter, _offset, _size_byte, pbuf);
@@ -828,14 +690,14 @@ Hal_ReadEFuse(struct rtw_adapter *padapter,
 		hal_ReadEFuse_BT(padapter, _offset, _size_byte, pbuf);
 }
 
-static u16
-hal_EfuseGetCurrentSize_WiFi(struct rtw_adapter *padapter)
+u16 rtl8723a_EfuseGetCurrentSize_WiFi(struct rtw_adapter *padapter)
 {
 	u16 efuse_addr = 0;
 	u8 hoffset = 0, hworden = 0;
 	u8 efuse_data, word_cnts = 0;
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
-	rtw23a_hal_get_hwreg(padapter, HW_VAR_EFUSE_BYTES, (u8 *) &efuse_addr);
+	efuse_addr = pHalData->EfuseUsedBytes;
 
 	DBG_8723A("%s: start_efuse_addr = 0x%X\n", __func__, efuse_addr);
 
@@ -844,7 +706,7 @@ hal_EfuseGetCurrentSize_WiFi(struct rtw_adapter *padapter)
 
 	while (AVAILABLE_EFUSE_ADDR(efuse_addr)) {
 		if (efuse_OneByteRead23a(padapter, efuse_addr, &efuse_data) ==
-		    false) {
+		    _FAIL) {
 			DBG_8723A(KERN_ERR "%s: efuse_OneByteRead23a Fail! "
 				  "addr = 0x%X !!\n", __func__, efuse_addr);
 			break;
@@ -872,15 +734,14 @@ hal_EfuseGetCurrentSize_WiFi(struct rtw_adapter *padapter)
 		efuse_addr += (word_cnts * 2) + 1;
 	}
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_EFUSE_BYTES, (u8 *) &efuse_addr);
+	pHalData->EfuseUsedBytes = efuse_addr;
 
 	DBG_8723A("%s: CurrentSize =%d\n", __func__, efuse_addr);
 
 	return efuse_addr;
 }
 
-static u16
-hal_EfuseGetCurrentSize_BT(struct rtw_adapter *padapter)
+u16 rtl8723a_EfuseGetCurrentSize_BT(struct rtw_adapter *padapter)
 {
 	u16 btusedbytes;
 	u16 efuse_addr;
@@ -888,8 +749,9 @@ hal_EfuseGetCurrentSize_BT(struct rtw_adapter *padapter)
 	u8 hoffset = 0, hworden = 0;
 	u8 efuse_data, word_cnts = 0;
 	u16 retU2 = 0;
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
-	rtw23a_hal_get_hwreg(padapter, HW_VAR_EFUSE_BT_BYTES, (u8 *) &btusedbytes);
+	btusedbytes = pHalData->BTEfuseUsedBytes;
 
 	efuse_addr = (u16) ((btusedbytes % EFUSE_BT_REAL_BANK_CONTENT_LEN));
 	startBank = (u8) (1 + (btusedbytes / EFUSE_BT_REAL_BANK_CONTENT_LEN));
@@ -915,7 +777,7 @@ hal_EfuseGetCurrentSize_BT(struct rtw_adapter *padapter)
 
 		while (AVAILABLE_EFUSE_ADDR(efuse_addr)) {
 			if (efuse_OneByteRead23a(padapter, efuse_addr,
-					      &efuse_data) == false) {
+					      &efuse_data) == _FAIL) {
 				DBG_8723A(KERN_ERR "%s: efuse_OneByteRead23a Fail!"
 					  " addr = 0x%X !!\n",
 					  __func__, efuse_addr);
@@ -954,86 +816,14 @@ hal_EfuseGetCurrentSize_BT(struct rtw_adapter *padapter)
 	}
 
 	retU2 = ((bank - 1) * EFUSE_BT_REAL_BANK_CONTENT_LEN) + efuse_addr;
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_EFUSE_BT_BYTES, (u8 *)&retU2);
+	pHalData->BTEfuseUsedBytes = retU2;
 
 	DBG_8723A("%s: CurrentSize =%d\n", __func__, retU2);
 	return retU2;
 }
 
-static u16
-Hal_EfuseGetCurrentSize(struct rtw_adapter *pAdapter, u8 efuseType)
-{
-	u16 ret = 0;
-
-	if (efuseType == EFUSE_WIFI)
-		ret = hal_EfuseGetCurrentSize_WiFi(pAdapter);
-	else
-		ret = hal_EfuseGetCurrentSize_BT(pAdapter);
-
-	return ret;
-}
-
-static u8
-Hal_EfuseWordEnableDataWrite(struct rtw_adapter *padapter,
-			     u16 efuse_addr, u8 word_en, u8 *data)
-{
-	u16 tmpaddr = 0;
-	u16 start_addr = efuse_addr;
-	u8 badworden = 0x0F;
-	u8 tmpdata[PGPKT_DATA_SIZE];
-
-	memset(tmpdata, 0xFF, PGPKT_DATA_SIZE);
-
-	if (!(word_en & BIT(0))) {
-		tmpaddr = start_addr;
-		efuse_OneByteWrite23a(padapter, start_addr++, data[0]);
-		efuse_OneByteWrite23a(padapter, start_addr++, data[1]);
-
-		efuse_OneByteRead23a(padapter, tmpaddr, &tmpdata[0]);
-		efuse_OneByteRead23a(padapter, 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_OneByteWrite23a(padapter, start_addr++, data[2]);
-		efuse_OneByteWrite23a(padapter, start_addr++, data[3]);
-
-		efuse_OneByteRead23a(padapter, tmpaddr, &tmpdata[2]);
-		efuse_OneByteRead23a(padapter, 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_OneByteWrite23a(padapter, start_addr++, data[4]);
-		efuse_OneByteWrite23a(padapter, start_addr++, data[5]);
-
-		efuse_OneByteRead23a(padapter, tmpaddr, &tmpdata[4]);
-		efuse_OneByteRead23a(padapter, 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_OneByteWrite23a(padapter, start_addr++, data[6]);
-		efuse_OneByteWrite23a(padapter, start_addr++, data[7]);
-
-		efuse_OneByteRead23a(padapter, tmpaddr, &tmpdata[6]);
-		efuse_OneByteRead23a(padapter, tmpaddr + 1, &tmpdata[7]);
-		if ((data[6] != tmpdata[6]) || (data[7] != tmpdata[7])) {
-			badworden &= (~BIT(3));
-		}
-	}
-
-	return badworden;
-}
-
-static s32
-Hal_EfusePgPacketRead(struct rtw_adapter *padapter, u8 offset, u8 *data)
+bool
+rtl8723a_EfusePgPacketRead(struct rtw_adapter *padapter, u8 offset, u8 *data)
 {
 	u8 efuse_data, word_cnts = 0;
 	u16 efuse_addr = 0;
@@ -1064,7 +854,7 @@ Hal_EfusePgPacketRead(struct rtw_adapter *padapter, u8 offset, u8 *data)
 	/*  */
 	while (AVAILABLE_EFUSE_ADDR(efuse_addr)) {
 		if (efuse_OneByteRead23a(padapter, efuse_addr++, &efuse_data) ==
-		    false) {
+		    _FAIL) {
 			ret = false;
 			break;
 		}
@@ -1110,281 +900,7 @@ Hal_EfusePgPacketRead(struct rtw_adapter *padapter, u8 offset, u8 *data)
 	return ret;
 }
 
-static u8
-hal_EfusePgCheckAvailableAddr(struct rtw_adapter *pAdapter, u8 efuseType)
-{
-	u16 max_available = 0;
-	u16 current_size;
-
-	EFUSE_GetEfuseDefinition23a(pAdapter, efuseType,
-				 TYPE_AVAILABLE_EFUSE_BYTES_TOTAL,
-				 &max_available);
-
-	current_size = Efuse_GetCurrentSize23a(pAdapter, efuseType);
-	if (current_size >= max_available) {
-		DBG_8723A("%s: Error!! current_size(%d)>max_available(%d)\n",
-			  __func__, current_size, max_available);
-		return false;
-	}
-	return true;
-}
-
-static void
-hal_EfuseConstructPGPkt(u8 offset, u8 word_en, u8 *pData,
-			struct pg_pkt_struct *pTargetPkt)
-{
-	memset(pTargetPkt->data, 0xFF, PGPKT_DATA_SIZE);
-	pTargetPkt->offset = offset;
-	pTargetPkt->word_en = word_en;
-	efuse_WordEnableDataRead23a(word_en, pData, pTargetPkt->data);
-	pTargetPkt->word_cnts = Efuse_CalculateWordCnts23a(pTargetPkt->word_en);
-}
-
-static u8
-hal_EfusePartialWriteCheck(struct rtw_adapter *padapter, u8 efuseType,
-			   u16 *pAddr, struct pg_pkt_struct *pTargetPkt)
-{
-	u8 bRet = false;
-	u16 startAddr = 0, efuse_max_available_len = 0, efuse_max = 0;
-	u8 efuse_data = 0;
-
-	EFUSE_GetEfuseDefinition23a(padapter, efuseType,
-				 TYPE_AVAILABLE_EFUSE_BYTES_TOTAL,
-				 &efuse_max_available_len);
-	EFUSE_GetEfuseDefinition23a(padapter, efuseType,
-				 TYPE_EFUSE_CONTENT_LEN_BANK, &efuse_max);
-
-	if (efuseType == EFUSE_WIFI) {
-		rtw23a_hal_get_hwreg(padapter, HW_VAR_EFUSE_BYTES,
-				  (u8 *) &startAddr);
-	} else {
-		rtw23a_hal_get_hwreg(padapter, HW_VAR_EFUSE_BT_BYTES,
-				  (u8 *) &startAddr);
-	}
-	startAddr %= efuse_max;
-
-	while (1) {
-		if (startAddr >= efuse_max_available_len) {
-			bRet = false;
-			DBG_8723A("%s: startAddr(%d) >= efuse_max_available_"
-				  "len(%d)\n", __func__, startAddr,
-				  efuse_max_available_len);
-			break;
-		}
-
-		if (efuse_OneByteRead23a(padapter, startAddr, &efuse_data) &&
-		    (efuse_data != 0xFF)) {
-			bRet = false;
-			DBG_8723A("%s: Something Wrong! last bytes(%#X = 0x%02X) "
-				  "is not 0xFF\n", __func__,
-				  startAddr, efuse_data);
-			break;
-		} else {
-			/*  not used header, 0xff */
-			*pAddr = startAddr;
-			bRet = true;
-			break;
-		}
-	}
-
-	return bRet;
-}
-
-static u8
-hal_EfusePgPacketWrite1ByteHeader(struct rtw_adapter *pAdapter, u8 efuseType,
-				  u16 *pAddr, struct pg_pkt_struct *pTargetPkt)
-{
-	u8 pg_header = 0, tmp_header = 0;
-	u16 efuse_addr = *pAddr;
-	u8 repeatcnt = 0;
-
-	pg_header = ((pTargetPkt->offset << 4) & 0xf0) | pTargetPkt->word_en;
-
-	do {
-		efuse_OneByteWrite23a(pAdapter, efuse_addr, pg_header);
-		efuse_OneByteRead23a(pAdapter, efuse_addr, &tmp_header);
-		if (tmp_header != 0xFF)
-			break;
-		if (repeatcnt++ > EFUSE_REPEAT_THRESHOLD_) {
-			DBG_8723A("%s: Repeat over limit for pg_header!!\n",
-				  __func__);
-			return false;
-		}
-	} while (1);
-
-	if (tmp_header != pg_header) {
-		DBG_8723A(KERN_ERR "%s: PG Header Fail!!(pg = 0x%02X "
-			  "read = 0x%02X)\n", __func__,
-			  pg_header, tmp_header);
-		return false;
-	}
-
-	*pAddr = efuse_addr;
-
-	return true;
-}
-
-static u8
-hal_EfusePgPacketWrite2ByteHeader(struct rtw_adapter *padapter, u8 efuseType,
-				  u16 *pAddr, struct pg_pkt_struct *pTargetPkt)
-{
-	u16 efuse_addr, efuse_max_available_len = 0;
-	u8 pg_header = 0, tmp_header = 0;
-	u8 repeatcnt = 0;
-
-	EFUSE_GetEfuseDefinition23a(padapter, efuseType,
-				 TYPE_AVAILABLE_EFUSE_BYTES_BANK,
-				 &efuse_max_available_len);
-
-	efuse_addr = *pAddr;
-	if (efuse_addr >= efuse_max_available_len) {
-		DBG_8723A("%s: addr(%d) over avaliable(%d)!!\n", __func__,
-			  efuse_addr, efuse_max_available_len);
-		return false;
-	}
-
-	pg_header = ((pTargetPkt->offset & 0x07) << 5) | 0x0F;
-
-	do {
-		efuse_OneByteWrite23a(padapter, efuse_addr, pg_header);
-		efuse_OneByteRead23a(padapter, efuse_addr, &tmp_header);
-		if (tmp_header != 0xFF)
-			break;
-		if (repeatcnt++ > EFUSE_REPEAT_THRESHOLD_) {
-			DBG_8723A("%s: Repeat over limit for pg_header!!\n",
-				  __func__);
-			return false;
-		}
-	} while (1);
-
-	if (tmp_header != pg_header) {
-		DBG_8723A(KERN_ERR
-			  "%s: PG Header Fail!!(pg = 0x%02X read = 0x%02X)\n",
-			  __func__, pg_header, tmp_header);
-		return false;
-	}
-
-	/*  to write ext_header */
-	efuse_addr++;
-	pg_header = ((pTargetPkt->offset & 0x78) << 1) | pTargetPkt->word_en;
-
-	do {
-		efuse_OneByteWrite23a(padapter, efuse_addr, pg_header);
-		efuse_OneByteRead23a(padapter, efuse_addr, &tmp_header);
-		if (tmp_header != 0xFF)
-			break;
-		if (repeatcnt++ > EFUSE_REPEAT_THRESHOLD_) {
-			DBG_8723A("%s: Repeat over limit for ext_header!!\n",
-				  __func__);
-			return false;
-		}
-	} while (1);
-
-	if (tmp_header != pg_header) {	/* offset PG fail */
-		DBG_8723A(KERN_ERR
-			  "%s: PG EXT Header Fail!!(pg = 0x%02X read = 0x%02X)\n",
-			  __func__, pg_header, tmp_header);
-		return false;
-	}
-
-	*pAddr = efuse_addr;
-
-	return true;
-}
-
-static u8
-hal_EfusePgPacketWriteHeader(struct rtw_adapter *padapter, u8 efuseType,
-			     u16 *pAddr, struct pg_pkt_struct *pTargetPkt)
-{
-	u8 bRet = false;
-
-	if (pTargetPkt->offset >= EFUSE_MAX_SECTION_BASE) {
-		bRet = hal_EfusePgPacketWrite2ByteHeader(padapter, efuseType,
-							 pAddr, pTargetPkt);
-	} else {
-		bRet = hal_EfusePgPacketWrite1ByteHeader(padapter, efuseType,
-							 pAddr, pTargetPkt);
-	}
-
-	return bRet;
-}
-
-static u8
-hal_EfusePgPacketWriteData(struct rtw_adapter *pAdapter, u8 efuseType,
-			   u16 *pAddr, struct pg_pkt_struct *pTargetPkt)
-{
-	u16 efuse_addr;
-	u8 badworden;
-
-	efuse_addr = *pAddr;
-	badworden =
-	    Efuse_WordEnableDataWrite23a(pAdapter, efuse_addr + 1,
-				      pTargetPkt->word_en, pTargetPkt->data);
-	if (badworden != 0x0F) {
-		DBG_8723A("%s: Fail!!\n", __func__);
-		return false;
-	}
-
-	return true;
-}
-
-static s32
-Hal_EfusePgPacketWrite(struct rtw_adapter *padapter,
-		       u8 offset, u8 word_en, u8 *pData)
-{
-	struct pg_pkt_struct targetPkt;
-	u16 startAddr = 0;
-	u8 efuseType = EFUSE_WIFI;
-
-	if (!hal_EfusePgCheckAvailableAddr(padapter, efuseType))
-		return false;
-
-	hal_EfuseConstructPGPkt(offset, word_en, pData, &targetPkt);
-
-	if (!hal_EfusePartialWriteCheck(padapter, efuseType,
-					&startAddr, &targetPkt))
-		return false;
-
-	if (!hal_EfusePgPacketWriteHeader(padapter, efuseType,
-					  &startAddr, &targetPkt))
-		return false;
-
-	if (!hal_EfusePgPacketWriteData(padapter, efuseType,
-					&startAddr, &targetPkt))
-		return false;
-
-	return true;
-}
-
-static bool
-Hal_EfusePgPacketWrite_BT(struct rtw_adapter *pAdapter,
-			  u8 offset, u8 word_en, u8 *pData)
-{
-	struct pg_pkt_struct targetPkt;
-	u16 startAddr = 0;
-	u8 efuseType = EFUSE_BT;
-
-	if (!hal_EfusePgCheckAvailableAddr(pAdapter, efuseType))
-		return false;
-
-	hal_EfuseConstructPGPkt(offset, word_en, pData, &targetPkt);
-
-	if (!hal_EfusePartialWriteCheck(pAdapter, efuseType,
-					&startAddr, &targetPkt))
-		return false;
-
-	if (!hal_EfusePgPacketWriteHeader(pAdapter, efuseType,
-					  &startAddr, &targetPkt))
-		return false;
-
-	if (!hal_EfusePgPacketWriteData(pAdapter, efuseType,
-					&startAddr, &targetPkt))
-		return false;
-
-	return true;
-}
-
-static struct hal_version ReadChipVersion8723A(struct rtw_adapter *padapter)
+void rtl8723a_read_chip_version(struct rtw_adapter *padapter)
 {
 	u32 value32;
 	struct hal_version ChipVersion;
@@ -1392,7 +908,7 @@ static struct hal_version ReadChipVersion8723A(struct rtw_adapter *padapter)
 
 	pHalData = GET_HAL_DATA(padapter);
 
-	value32 = rtw_read32(padapter, REG_SYS_CFG);
+	value32 = rtl8723au_read32(padapter, REG_SYS_CFG);
 	ChipVersion.ICType = CHIP_8723A;
 	ChipVersion.ChipType = ((value32 & RTL_ID) ? TEST_CHIP : NORMAL_CHIP);
 	ChipVersion.RFType = RF_TYPE_1T1R;
@@ -1404,13 +920,13 @@ static struct hal_version ReadChipVersion8723A(struct rtw_adapter *padapter)
 	pHalData->RegulatorMode = ((value32 & SPS_SEL) ?
 				   RT_LDO_REGULATOR : RT_SWITCHING_REGULATOR);
 
-	value32 = rtw_read32(padapter, REG_GPIO_OUTSTS);
+	value32 = rtl8723au_read32(padapter, REG_GPIO_OUTSTS);
 	/*  ROM code version. */
 	ChipVersion.ROMVer = ((value32 & RF_RL_ID) >> 20);
 
 	/*  For multi-function consideration. Added by Roger, 2010.10.06. */
 	pHalData->MultiFunc = RT_MULTI_FUNC_NONE;
-	value32 = rtw_read32(padapter, REG_MULTI_FUNC_CTRL);
+	value32 = rtl8723au_read32(padapter, REG_MULTI_FUNC_CTRL);
 	pHalData->MultiFunc |=
 		((value32 & WL_FUNC_EN) ? RT_MULTI_FUNC_WIFI : 0);
 	pHalData->MultiFunc |= ((value32 & BT_FUNC_EN) ? RT_MULTI_FUNC_BT : 0);
@@ -1430,13 +946,6 @@ static struct hal_version ReadChipVersion8723A(struct rtw_adapter *padapter)
 		pHalData->rf_type = RF_1T1R;
 
 	MSG_8723A("RF_Type is %x!!\n", pHalData->rf_type);
-
-	return ChipVersion;
-}
-
-static void rtl8723a_read_chip_version(struct rtw_adapter *padapter)
-{
-	ReadChipVersion8723A(padapter);
 }
 
 /*  */
@@ -1461,33 +970,34 @@ void SetBcnCtrlReg23a(struct rtw_adapter *padapter, u8 SetBits, u8 ClearBits)
 
 	addr = REG_BCN_CTRL;
 
-	*pRegBcnCtrlVal = rtw_read8(padapter, addr);
+	*pRegBcnCtrlVal = rtl8723au_read8(padapter, addr);
 	*pRegBcnCtrlVal |= SetBits;
 	*pRegBcnCtrlVal &= ~ClearBits;
 
-	rtw_write8(padapter, addr, *pRegBcnCtrlVal);
+	rtl8723au_write8(padapter, addr, *pRegBcnCtrlVal);
 }
 
 void rtl8723a_InitBeaconParameters(struct rtw_adapter *padapter)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
-	rtw_write16(padapter, REG_BCN_CTRL, 0x1010);
+	rtl8723au_write16(padapter, REG_BCN_CTRL, 0x1010);
 	pHalData->RegBcnCtrlVal = 0x1010;
 
 	/*  TODO: Remove these magic number */
-	rtw_write16(padapter, REG_TBTT_PROHIBIT, 0x6404);	/*  ms */
+	rtl8723au_write16(padapter, REG_TBTT_PROHIBIT, 0x6404);	/*  ms */
 	/*  Firmware will control REG_DRVERLYINT when power saving is enable, */
 	/*  so don't set this register on STA mode. */
 	if (check_fwstate(&padapter->mlmepriv, WIFI_STATION_STATE) == false)
-		rtw_write8(padapter, REG_DRVERLYINT, DRIVER_EARLY_INT_TIME);
+		rtl8723au_write8(padapter, REG_DRVERLYINT,
+				 DRIVER_EARLY_INT_TIME);
 	/*  2ms */
-	rtw_write8(padapter, REG_BCNDMATIM, BCN_DMA_ATIME_INT_TIME);
+	rtl8723au_write8(padapter, REG_BCNDMATIM, BCN_DMA_ATIME_INT_TIME);
 
 	/*  Suggested by designer timchen. Change beacon AIFS to the
 	    largest number beacause test chip does not contension before
 	    sending beacon. by tynli. 2009.11.03 */
-	rtw_write16(padapter, REG_BCNTCFG, 0x660F);
+	rtl8723au_write16(padapter, REG_BCNTCFG, 0x660F);
 }
 
 static void ResumeTxBeacon(struct rtw_adapter *padapter)
@@ -1501,10 +1011,11 @@ static void ResumeTxBeacon(struct rtw_adapter *padapter)
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("+ResumeTxBeacon\n"));
 
 	pHalData->RegFwHwTxQCtrl |= BIT(6);
-	rtw_write8(padapter, REG_FWHW_TXQ_CTRL + 2, pHalData->RegFwHwTxQCtrl);
-	rtw_write8(padapter, REG_TBTT_PROHIBIT + 1, 0xff);
+	rtl8723au_write8(padapter, REG_FWHW_TXQ_CTRL + 2,
+			 pHalData->RegFwHwTxQCtrl);
+	rtl8723au_write8(padapter, REG_TBTT_PROHIBIT + 1, 0xff);
 	pHalData->RegReg542 |= BIT(0);
-	rtw_write8(padapter, REG_TBTT_PROHIBIT + 2, pHalData->RegReg542);
+	rtl8723au_write8(padapter, REG_TBTT_PROHIBIT + 2, pHalData->RegReg542);
 }
 
 static void StopTxBeacon(struct rtw_adapter *padapter)
@@ -1518,10 +1029,11 @@ static void StopTxBeacon(struct rtw_adapter *padapter)
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("+StopTxBeacon\n"));
 
 	pHalData->RegFwHwTxQCtrl &= ~BIT(6);
-	rtw_write8(padapter, REG_FWHW_TXQ_CTRL + 2, pHalData->RegFwHwTxQCtrl);
-	rtw_write8(padapter, REG_TBTT_PROHIBIT + 1, 0x64);
+	rtl8723au_write8(padapter, REG_FWHW_TXQ_CTRL + 2,
+			 pHalData->RegFwHwTxQCtrl);
+	rtl8723au_write8(padapter, REG_TBTT_PROHIBIT + 1, 0x64);
 	pHalData->RegReg542 &= ~BIT(0);
-	rtw_write8(padapter, REG_TBTT_PROHIBIT + 2, pHalData->RegReg542);
+	rtl8723au_write8(padapter, REG_TBTT_PROHIBIT + 2, pHalData->RegReg542);
 
 	CheckFwRsvdPageContent23a(padapter); /*  2010.06.23. Added by tynli. */
 }
@@ -1531,10 +1043,10 @@ static void _BeaconFunctionEnable(struct rtw_adapter *padapter, u8 Enable,
 {
 	SetBcnCtrlReg23a(padapter, DIS_TSF_UDT | EN_BCN_FUNCTION | DIS_BCNQ_SUB,
 		      0);
-	rtw_write8(padapter, REG_RD_CTRL + 1, 0x6F);
+	rtl8723au_write8(padapter, REG_RD_CTRL + 1, 0x6F);
 }
 
-static void rtl8723a_SetBeaconRelatedRegisters(struct rtw_adapter *padapter)
+void rtl8723a_SetBeaconRelatedRegisters(struct rtw_adapter *padapter)
 {
 	u32 value32;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
@@ -1555,32 +1067,32 @@ static void rtl8723a_SetBeaconRelatedRegisters(struct rtw_adapter *padapter)
 	/*  */
 	/*  ATIM window */
 	/*  */
-	rtw_write16(padapter, REG_ATIMWND, 2);
+	rtl8723au_write16(padapter, REG_ATIMWND, 2);
 
 	/*  */
 	/*  Beacon interval (in unit of TU). */
 	/*  */
-	rtw_write16(padapter, REG_BCN_INTERVAL, pmlmeinfo->bcn_interval);
+	rtl8723au_write16(padapter, REG_BCN_INTERVAL, pmlmeinfo->bcn_interval);
 
 	rtl8723a_InitBeaconParameters(padapter);
 
-	rtw_write8(padapter, REG_SLOT, 0x09);
+	rtl8723au_write8(padapter, REG_SLOT, 0x09);
 
 	/*  */
 	/*  Reset TSF Timer to zero, added by Roger. 2008.06.24 */
 	/*  */
-	value32 = rtw_read32(padapter, REG_TCR);
+	value32 = rtl8723au_read32(padapter, REG_TCR);
 	value32 &= ~TSFRST;
-	rtw_write32(padapter, REG_TCR, value32);
+	rtl8723au_write32(padapter, REG_TCR, value32);
 
 	value32 |= TSFRST;
-	rtw_write32(padapter, REG_TCR, value32);
+	rtl8723au_write32(padapter, REG_TCR, value32);
 
 	/*  NOTE: Fix test chip's bug (about contention windows's randomness) */
 	if (check_fwstate(&padapter->mlmepriv, WIFI_ADHOC_STATE |
 			  WIFI_ADHOC_MASTER_STATE | WIFI_AP_STATE) == true) {
-		rtw_write8(padapter, REG_RXTSF_OFFSET_CCK, 0x50);
-		rtw_write8(padapter, REG_RXTSF_OFFSET_OFDM, 0x50);
+		rtl8723au_write8(padapter, REG_RXTSF_OFFSET_CCK, 0x50);
+		rtl8723au_write8(padapter, REG_RXTSF_OFFSET_OFDM, 0x50);
 	}
 
 	_BeaconFunctionEnable(padapter, true, true);
@@ -1589,21 +1101,9 @@ static void rtl8723a_SetBeaconRelatedRegisters(struct rtw_adapter *padapter)
 	SetBcnCtrlReg23a(padapter, DIS_BCNQ_SUB, 0);
 }
 
-static void rtl8723a_GetHalODMVar(struct rtw_adapter *Adapter,
-				  enum hal_odm_variable eVariable,
-				  void *pValue1, bool bSet)
-{
-	switch (eVariable) {
-	case HAL_ODM_STA_INFO:
-		break;
-	default:
-		break;
-	}
-}
-
-static void rtl8723a_SetHalODMVar(struct rtw_adapter *Adapter,
-				  enum hal_odm_variable eVariable,
-				  void *pValue1, bool bSet)
+void rtl8723a_SetHalODMVar(struct rtw_adapter *Adapter,
+			   enum hal_odm_variable eVariable,
+			   void *pValue1, bool bSet)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 	struct dm_odm_t *podmpriv = &pHalData->odmpriv;
@@ -1636,32 +1136,33 @@ static void rtl8723a_SetHalODMVar(struct rtw_adapter *Adapter,
 	}
 }
 
-static void hal_notch_filter_8723a(struct rtw_adapter *adapter, bool enable)
+void rtl8723a_notch_filter(struct rtw_adapter *adapter, bool enable)
 {
 	if (enable) {
 		DBG_8723A("Enable notch filter\n");
-		rtw_write8(adapter, rOFDM0_RxDSP + 1,
-			   rtw_read8(adapter, rOFDM0_RxDSP + 1) | BIT1);
+		rtl8723au_write8(adapter, rOFDM0_RxDSP + 1,
+				 rtl8723au_read8(adapter, rOFDM0_RxDSP + 1) |
+				 BIT(1));
 	} else {
 		DBG_8723A("Disable notch filter\n");
-		rtw_write8(adapter, rOFDM0_RxDSP + 1,
-			   rtw_read8(adapter, rOFDM0_RxDSP + 1) & ~BIT1);
+		rtl8723au_write8(adapter, rOFDM0_RxDSP + 1,
+			   rtl8723au_read8(adapter, rOFDM0_RxDSP + 1) &
+				 ~BIT(1));
 	}
 }
 
-s32 c2h_id_filter_ccx_8723a(u8 id)
+bool c2h_id_filter_ccx_8723a(u8 id)
 {
-	s32 ret = false;
+	bool ret = false;
 	if (id == C2H_CCX_TX_RPT)
 		ret = true;
 
 	return ret;
 }
 
-static s32 c2h_handler_8723a(struct rtw_adapter *padapter,
-			     struct c2h_evt_hdr *c2h_evt)
+int c2h_handler_8723a(struct rtw_adapter *padapter, struct c2h_evt_hdr *c2h_evt)
 {
-	s32 ret = _SUCCESS;
+	int ret = _SUCCESS;
 	u8 i = 0;
 
 	if (c2h_evt == NULL) {
@@ -1701,12 +1202,11 @@ static s32 c2h_handler_8723a(struct rtw_adapter *padapter,
 			  c2h_evt->payload[3], c2h_evt->payload[4]));
 		break;
 
-#ifdef CONFIG_8723AU_BT_COEXIST
 	case C2H_BT_INFO:
 		DBG_8723A("%s ,  Got  C2H_BT_INFO \n", __func__);
-		BT_FwC2hBtInfo(padapter, c2h_evt->payload, c2h_evt->plen);
+		rtl8723a_fw_c2h_BT_info(padapter,
+					c2h_evt->payload, c2h_evt->plen);
 		break;
-#endif
 
 	default:
 		ret = _FAIL;
@@ -1717,97 +1217,36 @@ exit:
 	return ret;
 }
 
-void rtl8723a_set_hal_ops(struct hal_ops *pHalFunc)
-{
-	pHalFunc->free_hal_data = &rtl8723a_free_hal_data;
-
-	pHalFunc->dm_init = &rtl8723a_init_dm_priv;
-	pHalFunc->dm_deinit = &rtl8723a_deinit_dm_priv;
-
-	pHalFunc->read_chip_version = &rtl8723a_read_chip_version;
-
-	pHalFunc->set_bwmode_handler = &PHY_SetBWMode23a8723A;
-	pHalFunc->set_channel_handler = &PHY_SwChnl8723A;
-
-	pHalFunc->hal_dm_watchdog = &rtl8723a_HalDmWatchDog;
-
-	pHalFunc->SetBeaconRelatedRegistersHandler =
-		&rtl8723a_SetBeaconRelatedRegisters;
-
-	pHalFunc->Add_RateATid = &rtl8723a_add_rateatid;
-	pHalFunc->run_thread = &rtl8723a_start_thread;
-	pHalFunc->cancel_thread = &rtl8723a_stop_thread;
-
-	pHalFunc->read_bbreg = &PHY_QueryBBReg;
-	pHalFunc->write_bbreg = &PHY_SetBBReg;
-	pHalFunc->read_rfreg = &PHY_QueryRFReg;
-	pHalFunc->write_rfreg = &PHY_SetRFReg;
-
-	/*  Efuse related function */
-	pHalFunc->EfusePowerSwitch = &Hal_EfusePowerSwitch;
-	pHalFunc->ReadEFuse = &Hal_ReadEFuse;
-	pHalFunc->EFUSEGetEfuseDefinition = &Hal_GetEfuseDefinition;
-	pHalFunc->EfuseGetCurrentSize = &Hal_EfuseGetCurrentSize;
-	pHalFunc->Efuse_PgPacketRead23a = &Hal_EfusePgPacketRead;
-	pHalFunc->Efuse_PgPacketWrite23a = &Hal_EfusePgPacketWrite;
-	pHalFunc->Efuse_WordEnableDataWrite23a = &Hal_EfuseWordEnableDataWrite;
-	pHalFunc->Efuse_PgPacketWrite23a_BT = &Hal_EfusePgPacketWrite_BT;
-
-	pHalFunc->sreset_init_value23a = &sreset_init_value23a;
-	pHalFunc->sreset_reset_value23a = &sreset_reset_value23a;
-	pHalFunc->silentreset = &sreset_reset;
-	pHalFunc->sreset_xmit_status_check = &rtl8723a_sreset_xmit_status_check;
-	pHalFunc->sreset_linked_status_check =
-		&rtl8723a_sreset_linked_status_check;
-	pHalFunc->sreset_get_wifi_status23a = &sreset_get_wifi_status23a;
-	pHalFunc->sreset_inprogress = &sreset_inprogress;
-	pHalFunc->GetHalODMVarHandler = &rtl8723a_GetHalODMVar;
-	pHalFunc->SetHalODMVarHandler = &rtl8723a_SetHalODMVar;
-
-	pHalFunc->hal_notch_filter = &hal_notch_filter_8723a;
-
-	pHalFunc->c2h_handler = c2h_handler_8723a;
-	pHalFunc->c2h_id_filter_ccx = c2h_id_filter_ccx_8723a;
-}
-
 void rtl8723a_InitAntenna_Selection(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a *pHalData;
 	u8 val;
 
-	pHalData = GET_HAL_DATA(padapter);
-
-	val = rtw_read8(padapter, REG_LEDCFG2);
+	val = rtl8723au_read8(padapter, REG_LEDCFG2);
 	/*  Let 8051 take control antenna settting */
 	val |= BIT(7);		/*  DPDT_SEL_EN, 0x4C[23] */
-	rtw_write8(padapter, REG_LEDCFG2, val);
+	rtl8723au_write8(padapter, REG_LEDCFG2, val);
 }
 
 void rtl8723a_CheckAntenna_Selection(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a *pHalData;
 	u8 val;
 
-	pHalData = GET_HAL_DATA(padapter);
-
-	val = rtw_read8(padapter, REG_LEDCFG2);
+	val = rtl8723au_read8(padapter, REG_LEDCFG2);
 	/*  Let 8051 take control antenna settting */
 	if (!(val & BIT(7))) {
 		val |= BIT(7);	/*  DPDT_SEL_EN, 0x4C[23] */
-		rtw_write8(padapter, REG_LEDCFG2, val);
+		rtl8723au_write8(padapter, REG_LEDCFG2, val);
 	}
 }
 
 void rtl8723a_DeinitAntenna_Selection(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a *pHalData;
 	u8 val;
 
-	pHalData = GET_HAL_DATA(padapter);
-	val = rtw_read8(padapter, REG_LEDCFG2);
+	val = rtl8723au_read8(padapter, REG_LEDCFG2);
 	/*  Let 8051 take control antenna settting */
 	val &= ~BIT(7);		/*  DPDT_SEL_EN, clear 0x4C[23] */
-	rtw_write8(padapter, REG_LEDCFG2, val);
+	rtl8723au_write8(padapter, REG_LEDCFG2, val);
 }
 
 void rtl8723a_init_default_value(struct rtw_adapter *padapter)
@@ -1847,7 +1286,7 @@ u8 GetEEPROMSize8723A(struct rtw_adapter *padapter)
 	u8 size = 0;
 	u32 cr;
 
-	cr = rtw_read16(padapter, REG_9346CR);
+	cr = rtl8723au_read16(padapter, REG_9346CR);
 	/*  6: EEPROM used is 93C46, 4: boot from E-Fuse. */
 	size = (cr & BOOT_FROM_EEPROM) ? 6 : 4;
 
@@ -1861,19 +1300,19 @@ u8 GetEEPROMSize8723A(struct rtw_adapter *padapter)
 /*  LLT R/W/Init function */
 /*  */
 /*  */
-static s32 _LLTWrite(struct rtw_adapter *padapter, u32 address, u32 data)
+static int _LLTWrite(struct rtw_adapter *padapter, u32 address, u32 data)
 {
-	s32 status = _SUCCESS;
+	int status = _SUCCESS;
 	s32 count = 0;
 	u32 value = _LLT_INIT_ADDR(address) | _LLT_INIT_DATA(data) |
 		    _LLT_OP(_LLT_WRITE_ACCESS);
 	u16 LLTReg = REG_LLT_INIT;
 
-	rtw_write32(padapter, LLTReg, value);
+	rtl8723au_write32(padapter, LLTReg, value);
 
 	/* polling */
 	do {
-		value = rtw_read32(padapter, LLTReg);
+		value = rtl8723au_read32(padapter, LLTReg);
 		if (_LLT_NO_ACTIVE == _LLT_OP_VALUE(value)) {
 			break;
 		}
@@ -1890,23 +1329,23 @@ static s32 _LLTWrite(struct rtw_adapter *padapter, u32 address, u32 data)
 	return status;
 }
 
-s32 InitLLTTable23a(struct rtw_adapter *padapter, u32 boundary)
+int InitLLTTable23a(struct rtw_adapter *padapter, u32 boundary)
 {
-	s32 status = _SUCCESS;
+	int status = _SUCCESS;
 	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 = _LLTWrite(padapter, i, i + 1);
-		if (_SUCCESS != status) {
+		if (status != _SUCCESS) {
 			return status;
 		}
 	}
 
 	/*  end of list */
 	status = _LLTWrite(padapter, (txpktbuf_bndy - 1), 0xFF);
-	if (_SUCCESS != status) {
+	if (status != _SUCCESS) {
 		return status;
 	}
 
@@ -1923,7 +1362,7 @@ s32 InitLLTTable23a(struct rtw_adapter *padapter, u32 boundary)
 
 	/*  Let last entry point to the start entry of ring buffer */
 	status = _LLTWrite(padapter, Last_Entry_Of_TxPktBuf, txpktbuf_bndy);
-	if (_SUCCESS != status) {
+	if (status != _SUCCESS) {
 		return status;
 	}
 
@@ -1943,11 +1382,11 @@ n. LEDCFG 0x4C[15:0] = 0x8080
 	u32 u4bTmp;
 
 	/* 1. Disable GPIO[7:0] */
-	rtw_write16(padapter, REG_GPIO_PIN_CTRL + 2, 0x0000);
-	value32 = rtw_read32(padapter, REG_GPIO_PIN_CTRL) & 0xFFFF00FF;
+	rtl8723au_write16(padapter, REG_GPIO_PIN_CTRL + 2, 0x0000);
+	value32 = rtl8723au_read32(padapter, REG_GPIO_PIN_CTRL) & 0xFFFF00FF;
 	u4bTmp = value32 & 0x000000FF;
 	value32 |= ((u4bTmp << 8) | 0x00FF0000);
-	rtw_write32(padapter, REG_GPIO_PIN_CTRL, value32);
+	rtl8723au_write32(padapter, REG_GPIO_PIN_CTRL, value32);
 
 	/*  */
 	/*  <Roger_Notes> For RTL8723u multi-function configuration which
@@ -1958,15 +1397,15 @@ n. LEDCFG 0x4C[15:0] = 0x8080
 	/* 2. Disable GPIO[8] and GPIO[12] */
 
 	/*  Configure all pins as input mode. */
-	rtw_write16(padapter, REG_GPIO_IO_SEL_2, 0x0000);
-	value32 = rtw_read32(padapter, REG_GPIO_PIN_CTRL_2) & 0xFFFF001F;
+	rtl8723au_write16(padapter, REG_GPIO_IO_SEL_2, 0x0000);
+	value32 = rtl8723au_read32(padapter, REG_GPIO_PIN_CTRL_2) & 0xFFFF001F;
 	u4bTmp = value32 & 0x0000001F;
 	/*  Set pin 8, 10, 11 and pin 12 to output mode. */
 	value32 |= ((u4bTmp << 8) | 0x001D0000);
-	rtw_write32(padapter, REG_GPIO_PIN_CTRL_2, value32);
+	rtl8723au_write32(padapter, REG_GPIO_PIN_CTRL_2, value32);
 
 	/* 3. Disable LED0 & 1 */
-	rtw_write16(padapter, REG_LEDCFG0, 0x8080);
+	rtl8723au_write16(padapter, REG_LEDCFG0, 0x8080);
 }				/* end of _DisableGPIO() */
 
 static void _DisableRFAFEAndResetBB8192C(struct rtw_adapter *padapter)
@@ -1980,21 +1419,21 @@ e.	SYS_FUNC_EN 0x02[7:0] = 0x14		reset BB state machine
 ***************************************/
 	u8 eRFPath = 0, value8 = 0;
 
-	rtw_write8(padapter, REG_TXPAUSE, 0xFF);
+	rtl8723au_write8(padapter, REG_TXPAUSE, 0xFF);
 
 	PHY_SetRFReg(padapter, (enum RF_RADIO_PATH) eRFPath, 0x0, bMaskByte0, 0x0);
 
 	value8 |= APSDOFF;
-	rtw_write8(padapter, REG_APSD_CTRL, value8);	/* 0x40 */
+	rtl8723au_write8(padapter, REG_APSD_CTRL, value8);	/* 0x40 */
 
 	/*  Set BB reset at first */
 	value8 = 0;
 	value8 |= (FEN_USBD | FEN_USBA | FEN_BB_GLB_RSTn);
-	rtw_write8(padapter, REG_SYS_FUNC_EN, value8);	/* 0x16 */
+	rtl8723au_write8(padapter, REG_SYS_FUNC_EN, value8);	/* 0x16 */
 
 	/*  Set global reset. */
 	value8 &= ~FEN_BB_GLB_RSTn;
-	rtw_write8(padapter, REG_SYS_FUNC_EN, value8);	/* 0x14 */
+	rtl8723au_write8(padapter, REG_SYS_FUNC_EN, value8);	/* 0x14 */
 
 	/*  2010/08/12 MH We need to set BB/GLBAL reset to save power
 	    for SS mode. */
@@ -2021,19 +1460,22 @@ static void _ResetDigitalProcedure1_92C(struct rtw_adapter *padapter,
 						(8051 enable)
 	******************************/
 		u16 valu16 = 0;
-		rtw_write8(padapter, REG_MCUFWDL, 0);
+		rtl8723au_write8(padapter, REG_MCUFWDL, 0);
 
-		valu16 = rtw_read16(padapter, REG_SYS_FUNC_EN);
+		valu16 = rtl8723au_read16(padapter, REG_SYS_FUNC_EN);
 		/* reset MCU , 8051 */
-		rtw_write16(padapter, REG_SYS_FUNC_EN, (valu16 & (~FEN_CPUEN)));
+		rtl8723au_write16(padapter, REG_SYS_FUNC_EN,
+				  valu16 & (~FEN_CPUEN));
 
-		valu16 = rtw_read16(padapter, REG_SYS_FUNC_EN) & 0x0FFF;
-		rtw_write16(padapter, REG_SYS_FUNC_EN,
-			    (valu16 | (FEN_HWPDN | FEN_ELDR)));	/* reset MAC */
+		valu16 = rtl8723au_read16(padapter, REG_SYS_FUNC_EN) & 0x0FFF;
+		/* reset MAC */
+		rtl8723au_write16(padapter, REG_SYS_FUNC_EN,
+				  valu16 | (FEN_HWPDN | FEN_ELDR));
 
-		valu16 = rtw_read16(padapter, REG_SYS_FUNC_EN);
+		valu16 = rtl8723au_read16(padapter, REG_SYS_FUNC_EN);
 		/* enable MCU , 8051 */
-		rtw_write16(padapter, REG_SYS_FUNC_EN, (valu16 | FEN_CPUEN));
+		rtl8723au_write16(padapter, REG_SYS_FUNC_EN,
+				  valu16 | FEN_CPUEN);
 	} else {
 		u8 retry_cnts = 0;
 
@@ -2041,41 +1483,43 @@ static void _ResetDigitalProcedure1_92C(struct rtw_adapter *padapter,
 		    are trying to enter IPS/HW&SW radio off. For
 		    S3/S4/S5/Disable, we can stop 8051 because */
 		/*  we will init FW when power on again. */
-		/* if (!pDevice->RegUsbSS) */
 		/*  If we want to SS mode, we can not reset 8051. */
-		if (rtw_read8(padapter, REG_MCUFWDL) & BIT1) {
+		if (rtl8723au_read8(padapter, REG_MCUFWDL) & BIT(1)) {
 			/* IF fw in RAM code, do reset */
 			if (padapter->bFWReady) {
 				/*  2010/08/25 MH Accordign to RD alfred's
 				    suggestion, we need to disable other */
 				/*  HRCV INT to influence 8051 reset. */
-				rtw_write8(padapter, REG_FWIMR, 0x20);
+				rtl8723au_write8(padapter, REG_FWIMR, 0x20);
 				/*  2011/02/15 MH According to Alex's
 				    suggestion, close mask to prevent
 				    incorrect FW write operation. */
-				rtw_write8(padapter, REG_FTIMR, 0x00);
-				rtw_write8(padapter, REG_FSIMR, 0x00);
+				rtl8723au_write8(padapter, REG_FTIMR, 0x00);
+				rtl8723au_write8(padapter, REG_FSIMR, 0x00);
 
 				/* 8051 reset by self */
-				rtw_write8(padapter, REG_HMETFR + 3, 0x20);
+				rtl8723au_write8(padapter, REG_HMETFR + 3,
+						 0x20);
 
 				while ((retry_cnts++ < 100) &&
 				       (FEN_CPUEN &
-					rtw_read16(padapter, REG_SYS_FUNC_EN))) {
+					rtl8723au_read16(padapter,
+							 REG_SYS_FUNC_EN))) {
 					udelay(50);	/* us */
 				}
 
 				if (retry_cnts >= 100) {
 					/* Reset MAC and Enable 8051 */
-					rtw_write8(padapter,
-						   REG_SYS_FUNC_EN + 1, 0x50);
+					rtl8723au_write8(padapter,
+							 REG_SYS_FUNC_EN + 1,
+							 0x50);
 					mdelay(10);
 				}
 			}
 		}
 		/* Reset MAC and Enable 8051 */
-		rtw_write8(padapter, REG_SYS_FUNC_EN + 1, 0x54);
-		rtw_write8(padapter, REG_MCUFWDL, 0);
+		rtl8723au_write8(padapter, REG_SYS_FUNC_EN + 1, 0x54);
+		rtl8723au_write8(padapter, REG_MCUFWDL, 0);
 	}
 
 	if (bWithoutHWSM) {
@@ -2087,13 +1531,13 @@ static void _ResetDigitalProcedure1_92C(struct rtw_adapter *padapter,
 	j.	SYS_ISO_CTRL 0x00[7:0] = 0xF9		isolated digital to PON
 	******************************/
 		/* modify to 0x70A3 by Scott. */
-		rtw_write16(padapter, REG_SYS_CLKR, 0x70A3);
-		rtw_write8(padapter, REG_AFE_PLL_CTRL, 0x80);
-		rtw_write16(padapter, REG_AFE_XTAL_CTRL, 0x880F);
-		rtw_write8(padapter, REG_SYS_ISO_CTRL, 0xF9);
+		rtl8723au_write16(padapter, REG_SYS_CLKR, 0x70A3);
+		rtl8723au_write8(padapter, REG_AFE_PLL_CTRL, 0x80);
+		rtl8723au_write16(padapter, REG_AFE_XTAL_CTRL, 0x880F);
+		rtl8723au_write8(padapter, REG_SYS_ISO_CTRL, 0xF9);
 	} else {
 		/*  Disable all RF/BB power */
-		rtw_write8(padapter, REG_RF_CTRL, 0x00);
+		rtl8723au_write8(padapter, REG_RF_CTRL, 0x00);
 	}
 }
 
@@ -2111,9 +1555,9 @@ l.	SYS_CLKR 0x08[15:0] = 0x3083		disable ELDR clock
 m.	SYS_ISO_CTRL 0x01[7:0] = 0x83		isolated ELDR to PON
 ******************************/
 	/* modify to 0x70a3 by Scott. */
-	rtw_write16(padapter, REG_SYS_CLKR, 0x70a3);
+	rtl8723au_write16(padapter, REG_SYS_CLKR, 0x70a3);
 	/* modify to 0x82 by Scott. */
-	rtw_write8(padapter, REG_SYS_ISO_CTRL + 1, 0x82);
+	rtl8723au_write8(padapter, REG_SYS_ISO_CTRL + 1, 0x82);
 }
 
 static void _DisableAnalog(struct rtw_adapter *padapter, bool bWithoutHWSM)
@@ -2130,12 +1574,12 @@ static void _DisableAnalog(struct rtw_adapter *padapter, bool bWithoutHWSM)
 		clock automatically
 	******************************/
 
-		rtw_write8(padapter, REG_LDOA15_CTRL, 0x04);
-		/* rtw_write8(padapter, REG_LDOV12D_CTRL, 0x54); */
+		rtl8723au_write8(padapter, REG_LDOA15_CTRL, 0x04);
+		/* rtl8723au_write8(padapter, REG_LDOV12D_CTRL, 0x54); */
 
-		value8 = rtw_read8(padapter, REG_LDOV12D_CTRL);
+		value8 = rtl8723au_read8(padapter, REG_LDOV12D_CTRL);
 		value8 &= (~LDV12_EN);
-		rtw_write8(padapter, REG_LDOV12D_CTRL, value8);
+		rtl8723au_write8(padapter, REG_LDOV12D_CTRL, value8);
 /*		RT_TRACE(COMP_INIT, DBG_LOUD,
 		(" REG_LDOV12D_CTRL Reg0x21:0x%02x.\n", value8)); */
 	}
@@ -2146,9 +1590,9 @@ static void _DisableAnalog(struct rtw_adapter *padapter, bool bWithoutHWSM)
 	******************************/
 	value8 = 0x23;
 	if (IS_81xxC_VENDOR_UMC_B_CUT(pHalData->VersionID))
-		value8 |= BIT3;
+		value8 |= BIT(3);
 
-	rtw_write8(padapter, REG_SPS0_CTRL, value8);
+	rtl8723au_write8(padapter, REG_SPS0_CTRL, value8);
 
 	if (bWithoutHWSM) {
 		/* value16 |= (APDM_HOST | FSM_HSUS |/PFM_ALDN); */
@@ -2161,13 +1605,13 @@ static void _DisableAnalog(struct rtw_adapter *padapter, bool bWithoutHWSM)
 		value16 |= (APDM_HOST | AFSM_HSUS | PFM_ALDN);
 	}
 
-	rtw_write16(padapter, REG_APS_FSMCO, value16);	/* 0x4802 */
+	rtl8723au_write16(padapter, REG_APS_FSMCO, value16);	/* 0x4802 */
 
-	rtw_write8(padapter, REG_RSV_CTRL, 0x0e);
+	rtl8723au_write8(padapter, REG_RSV_CTRL, 0x0e);
 }
 
 /*  HW Auto state machine */
-s32 CardDisableHWSM(struct rtw_adapter *padapter, u8 resetMCU)
+int CardDisableHWSM(struct rtw_adapter *padapter, u8 resetMCU)
 {
 	int rtStatus = _SUCCESS;
 
@@ -2193,9 +1637,9 @@ s32 CardDisableHWSM(struct rtw_adapter *padapter, u8 resetMCU)
 }
 
 /*  without HW Auto state machine */
-s32 CardDisableWithoutHWSM(struct rtw_adapter *padapter)
+int CardDisableWithoutHWSM(struct rtw_adapter *padapter)
 {
-	s32 rtStatus = _SUCCESS;
+	int rtStatus = _SUCCESS;
 
 	/* RT_TRACE(COMP_INIT, DBG_LOUD,
 	   ("======> Card Disable Without HWSM .\n")); */
@@ -2274,9 +1718,9 @@ static void Hal_EEValueCheck(u8 EEType, void *pInValue, void *pOutValue)
 		u8 *pIn, *pOut;
 		pIn = (u8 *) pInValue;
 		pOut = (u8 *) pOutValue;
-		if (*pIn >= 0 && *pIn <= 63) {
+		if (*pIn <= 63)
 			*pOut = *pIn;
-		} else {
+		else {
 			RT_TRACE(_module_hci_hal_init_c_, _drv_err_,
 				 ("EETYPE_TX_PWR, value =%d is invalid, set "
 				  "to default = 0x%x\n",
@@ -2346,7 +1790,7 @@ Hal_ReadPowerValueFromPROM_8723A(struct txpowerinfo *pwrInfo,
 				 [EEPROM_HT20_TX_PWR_INX_DIFF_8723A +
 				  group] >> (rfPath * 4)) & 0xF;
 			/* 4bit sign number to 8 bit sign number */
-			if (pwrInfo->HT20IndexDiff[rfPath][group] & BIT3)
+			if (pwrInfo->HT20IndexDiff[rfPath][group] & BIT(3))
 				pwrInfo->HT20IndexDiff[rfPath][group] |= 0xF0;
 
 			pwrInfo->OFDMIndexDiff[rfPath][group] =
@@ -2478,7 +1922,7 @@ Hal_EfuseParseBTCoexistInfo_8723A(struct rtw_adapter *padapter,
 	u32 tmpu4;
 
 	if (!AutoLoadFail) {
-		tmpu4 = rtw_read32(padapter, REG_MULTI_FUNC_CTRL);
+		tmpu4 = rtl8723au_read32(padapter, REG_MULTI_FUNC_CTRL);
 		if (tmpu4 & BT_FUNC_EN)
 			pHalData->EEPROMBluetoothCoexist = 1;
 		else
@@ -2498,9 +1942,8 @@ Hal_EfuseParseBTCoexistInfo_8723A(struct rtw_adapter *padapter,
 		pHalData->EEPROMBluetoothAntIsolation = 0;
 		pHalData->EEPROMBluetoothRadioShared = BT_Radio_Shared;
 	}
-#ifdef CONFIG_8723AU_BT_COEXIST
-	BT_InitHalVars(padapter);
-#endif
+
+	rtl8723a_BT_init_hal_vars(padapter);
 }
 
 void
@@ -2586,20 +2029,20 @@ Hal_EfuseParseXtal_8723A(struct rtw_adapter *pAdapter,
 
 void
 Hal_EfuseParseThermalMeter_8723A(struct rtw_adapter *padapter,
-				 u8 *PROMContent, u8 AutoloadFail)
+				 u8 *PROMContent, bool AutoloadFail)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
 	/*  */
 	/*  ThermalMeter from EEPROM */
 	/*  */
-	if (false == AutoloadFail)
+	if (AutoloadFail == false)
 		pHalData->EEPROMThermalMeter =
 		    PROMContent[EEPROM_THERMAL_METER_8723A];
 	else
 		pHalData->EEPROMThermalMeter = EEPROM_Default_ThermalMeter;
 
-	if ((pHalData->EEPROMThermalMeter == 0xff) || (true == AutoloadFail)) {
+	if ((pHalData->EEPROMThermalMeter == 0xff) || (AutoloadFail == true)) {
 		pHalData->bAPKThermalMeterIgnore = true;
 		pHalData->EEPROMThermalMeter = EEPROM_Default_ThermalMeter;
 	}
@@ -2635,18 +2078,17 @@ static void fill_txdesc_sectype(struct pkt_attrib *pattrib,
 	if ((pattrib->encrypt > 0) && !pattrib->bswenc) {
 		switch (pattrib->encrypt) {
 			/*  SEC_TYPE */
-		case _WEP40_:
-		case _WEP104_:
-		case _TKIP_:
-		case _TKIP_WTMIC_:
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
+		case WLAN_CIPHER_SUITE_TKIP:
 			ptxdesc->sectype = 1;
 			break;
 
-		case _AES_:
+		case WLAN_CIPHER_SUITE_CCMP:
 			ptxdesc->sectype = 3;
 			break;
 
-		case _NO_PRIVACY_:
+		case 0:
 		default:
 			break;
 		}
@@ -2935,7 +2377,7 @@ void rtl8723a_fill_fake_txdesc(struct rtw_adapter *padapter, u8 *pDesc,
 	rtl8723a_cal_txdesc_chksum(ptxdesc);
 }
 
-static void hw_var_set_opmode(struct rtw_adapter *padapter, u8 mode)
+void hw_var_set_opmode(struct rtw_adapter *padapter, u8 mode)
 {
 	u8 val8;
 
@@ -2951,10 +2393,8 @@ static void hw_var_set_opmode(struct rtw_adapter *padapter, u8 mode)
 		val8 = DIS_TSF_UDT | EN_BCN_FUNCTION | DIS_BCNQ_SUB;
 		SetBcnCtrlReg23a(padapter, val8, ~val8);
 	} else if (mode == _HW_STATE_AP_) {
-#ifdef CONFIG_8723AU_BT_COEXIST
 		/*  add NULL Data and BT NULL Data Packets to FW RSVD Page */
 		rtl8723a_set_BTCoex_AP_mode_FwRsvdPkt_cmd(padapter);
-#endif
 
 		ResumeTxBeacon(padapter);
 
@@ -2962,26 +2402,29 @@ static void hw_var_set_opmode(struct rtw_adapter *padapter, u8 mode)
 		SetBcnCtrlReg23a(padapter, val8, ~val8);
 
 		/*  Set RCR */
-		/* rtw_write32(padapter, REG_RCR, 0x70002a8e);
+		/* rtl8723au_write32(padapter, REG_RCR, 0x70002a8e);
 		   CBSSID_DATA must set to 0 */
 		/* CBSSID_DATA must set to 0 */
-		rtw_write32(padapter, REG_RCR, 0x7000228e);
+		rtl8723au_write32(padapter, REG_RCR, 0x7000228e);
 		/*  enable to rx data frame */
-		rtw_write16(padapter, REG_RXFLTMAP2, 0xFFFF);
+		rtl8723au_write16(padapter, REG_RXFLTMAP2, 0xFFFF);
 		/*  enable to rx ps-poll */
-		rtw_write16(padapter, REG_RXFLTMAP1, 0x0400);
+		rtl8723au_write16(padapter, REG_RXFLTMAP1, 0x0400);
 
 		/*  Beacon Control related register for first time */
-		rtw_write8(padapter, REG_BCNDMATIM, 0x02);	/*  2ms */
-		rtw_write8(padapter, REG_DRVERLYINT, 0x05);	/*  5ms */
-		rtw_write8(padapter, REG_ATIMWND, 0x0a); /*  10ms for port0 */
-		rtw_write16(padapter, REG_BCNTCFG, 0x00);
-		rtw_write16(padapter, REG_TBTT_PROHIBIT, 0xff04);
+		/*  2ms */
+		rtl8723au_write8(padapter, REG_BCNDMATIM, 0x02);
+		/*  5ms */
+		rtl8723au_write8(padapter, REG_DRVERLYINT, 0x05);
+		/*  10ms for port0 */
+		rtl8723au_write8(padapter, REG_ATIMWND, 0x0a);
+		rtl8723au_write16(padapter, REG_BCNTCFG, 0x00);
+		rtl8723au_write16(padapter, REG_TBTT_PROHIBIT, 0xff04);
 		/*  +32767 (~32ms) */
-		rtw_write16(padapter, REG_TSFTR_SYN_OFFSET, 0x7fff);
+		rtl8723au_write16(padapter, REG_TSFTR_SYN_OFFSET, 0x7fff);
 
 		/*  reset TSF */
-		rtw_write8(padapter, REG_DUAL_TSF_RST, BIT(0));
+		rtl8723au_write8(padapter, REG_DUAL_TSF_RST, BIT(0));
 
 		/*  enable BCN Function */
 		/*  don't enable update TSF (due to TSF update when
@@ -2991,12 +2434,12 @@ static void hw_var_set_opmode(struct rtw_adapter *padapter, u8 mode)
 		SetBcnCtrlReg23a(padapter, val8, ~val8);
 	}
 
-	val8 = rtw_read8(padapter, MSR);
+	val8 = rtl8723au_read8(padapter, MSR);
 	val8 = (val8 & 0xC) | mode;
-	rtw_write8(padapter, MSR, val8);
+	rtl8723au_write8(padapter, MSR, val8);
 }
 
-static void hw_var_set_macaddr(struct rtw_adapter *padapter, u8 *val)
+void hw_var_set_macaddr(struct rtw_adapter *padapter, u8 *val)
 {
 	u8 idx = 0;
 	u32 reg_macid;
@@ -3004,10 +2447,10 @@ static void hw_var_set_macaddr(struct rtw_adapter *padapter, u8 *val)
 	reg_macid = REG_MACID;
 
 	for (idx = 0; idx < 6; idx++)
-		rtw_write8(padapter, (reg_macid + idx), val[idx]);
+		rtl8723au_write8(padapter, (reg_macid + idx), val[idx]);
 }
 
-static void hw_var_set_bssid(struct rtw_adapter *padapter, u8 *val)
+void hw_var_set_bssid(struct rtw_adapter *padapter, u8 *val)
 {
 	u8 idx = 0;
 	u32 reg_bssid;
@@ -3015,10 +2458,10 @@ static void hw_var_set_bssid(struct rtw_adapter *padapter, u8 *val)
 	reg_bssid = REG_BSSID;
 
 	for (idx = 0; idx < 6; idx++)
-		rtw_write8(padapter, (reg_bssid + idx), val[idx]);
+		rtl8723au_write8(padapter, (reg_bssid + idx), val[idx]);
 }
 
-static void hw_var_set_correct_tsf(struct rtw_adapter *padapter)
+void hw_var_set_correct_tsf(struct rtw_adapter *padapter)
 {
 	u64 tsf;
 	u32 reg_tsftr;
@@ -3028,14 +2471,14 @@ static void hw_var_set_correct_tsf(struct rtw_adapter *padapter)
 	/* tsf = pmlmeext->TSFValue - ((u32)pmlmeext->TSFValue %
 	   (pmlmeinfo->bcn_interval*1024)) - 1024; us */
 	tsf = pmlmeext->TSFValue -
-		rtw_modular6423a(pmlmeext->TSFValue,
-			      (pmlmeinfo->bcn_interval * 1024)) - 1024;	/* us */
+		do_div(pmlmeext->TSFValue,
+		       (pmlmeinfo->bcn_interval * 1024)) - 1024;	/* us */
 
 	if (((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) ||
 	    ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE)) {
 		/* pHalData->RegTxPause |= STOP_BCNQ;BIT(6) */
-		/* rtw_write8(padapter, REG_TXPAUSE,
-		   (rtw_read8(Adapter, REG_TXPAUSE)|BIT(6))); */
+		/* rtl8723au_write8(padapter, REG_TXPAUSE,
+		   (rtl8723au_read8(Adapter, REG_TXPAUSE)|BIT(6))); */
 		StopTxBeacon(padapter);
 	}
 
@@ -3044,8 +2487,8 @@ static void hw_var_set_correct_tsf(struct rtw_adapter *padapter)
 	/*  disable related TSF function */
 	SetBcnCtrlReg23a(padapter, 0, EN_BCN_FUNCTION);
 
-	rtw_write32(padapter, reg_tsftr, tsf);
-	rtw_write32(padapter, reg_tsftr + 4, tsf >> 32);
+	rtl8723au_write32(padapter, reg_tsftr, tsf);
+	rtl8723au_write32(padapter, reg_tsftr + 4, tsf >> 32);
 
 	/* enable related TSF function */
 	SetBcnCtrlReg23a(padapter, EN_BCN_FUNCTION, 0);
@@ -3055,19 +2498,19 @@ static void hw_var_set_correct_tsf(struct rtw_adapter *padapter)
 		ResumeTxBeacon(padapter);
 }
 
-static void hw_var_set_mlme_disconnect(struct rtw_adapter *padapter)
+void hw_var_set_mlme_disconnect(struct rtw_adapter *padapter)
 {
 	/*  reject all data frames */
-	rtw_write16(padapter, REG_RXFLTMAP2, 0);
+	rtl8723au_write16(padapter, REG_RXFLTMAP2, 0);
 
 	/*  reset TSF */
-	rtw_write8(padapter, REG_DUAL_TSF_RST, BIT(0));
+	rtl8723au_write8(padapter, REG_DUAL_TSF_RST, BIT(0));
 
 	/*  disable update TSF */
 	SetBcnCtrlReg23a(padapter, DIS_TSF_UDT, 0);
 }
 
-static void hw_var_set_mlme_join(struct rtw_adapter *padapter, u8 type)
+void hw_var_set_mlme_join(struct rtw_adapter *padapter, u8 type)
 {
 	u8 RetryLimit = 0x30;
 
@@ -3078,13 +2521,13 @@ static void hw_var_set_mlme_join(struct rtw_adapter *padapter, u8 type)
 		u32 v32;
 
 		/*  enable to rx data frame.Accept all data frame */
-		/* rtw_write32(padapter, REG_RCR,
-		   rtw_read32(padapter, REG_RCR)|RCR_ADF); */
-		rtw_write16(padapter, REG_RXFLTMAP2, 0xFFFF);
+		/* rtl8723au_write32(padapter, REG_RCR,
+		   rtl8723au_read32(padapter, REG_RCR)|RCR_ADF); */
+		rtl8723au_write16(padapter, REG_RXFLTMAP2, 0xFFFF);
 
-		v32 = rtw_read32(padapter, REG_RCR);
+		v32 = rtl8723au_read32(padapter, REG_RCR);
 		v32 |= RCR_CBSSID_DATA | RCR_CBSSID_BCN;
-		rtw_write32(padapter, REG_RCR, v32);
+		rtl8723au_write32(padapter, REG_RCR, v32);
 
 		if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true)
 			RetryLimit =
@@ -3094,7 +2537,7 @@ static void hw_var_set_mlme_join(struct rtw_adapter *padapter, u8 type)
 	} else if (type == 1) {	/*  joinbss_event callback when join res < 0 */
 		/*  config RCR to receive different BSSID & not to
 		    receive data frame during linking */
-		rtw_write16(padapter, REG_RXFLTMAP2, 0);
+		rtl8723au_write16(padapter, REG_RXFLTMAP2, 0);
 	} else if (type == 2) {	/*  sta add event callback */
 		/*  enable update TSF */
 		SetBcnCtrlReg23a(padapter, 0, DIS_TSF_UDT);
@@ -3102,351 +2545,27 @@ static void hw_var_set_mlme_join(struct rtw_adapter *padapter, u8 type)
 		if (check_fwstate(pmlmepriv,
 				  WIFI_ADHOC_STATE | WIFI_ADHOC_MASTER_STATE)) {
 			/*  fixed beacon issue for 8191su........... */
-			rtw_write8(padapter, 0x542, 0x02);
+			rtl8723au_write8(padapter, 0x542, 0x02);
 			RetryLimit = 0x7;
 		}
 	}
 
-	rtw_write16(padapter, REG_RL,
-		    RetryLimit << RETRY_LIMIT_SHORT_SHIFT | RetryLimit <<
-		    RETRY_LIMIT_LONG_SHIFT);
+	rtl8723au_write16(padapter, REG_RL,
+			  RetryLimit << RETRY_LIMIT_SHORT_SHIFT | RetryLimit <<
+			  RETRY_LIMIT_LONG_SHIFT);
 
-#ifdef CONFIG_8723AU_BT_COEXIST
 	switch (type) {
 	case 0:
 		/*  prepare to join */
-		BT_WifiAssociateNotify(padapter, true);
+		rtl8723a_BT_wifiassociate_notify(padapter, true);
 		break;
 	case 1:
 		/*  joinbss_event callback when join res < 0 */
-		BT_WifiAssociateNotify(padapter, false);
+		rtl8723a_BT_wifiassociate_notify(padapter, false);
 		break;
 	case 2:
 		/*  sta add event callback */
 /*		BT_WifiMediaStatusNotify(padapter, RT_MEDIA_CONNECT); */
 		break;
 	}
-#endif
-}
-
-void SetHwReg8723A(struct rtw_adapter *padapter, u8 variable, u8 *val)
-{
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-	u32 *val32 = (u32 *)val;
-
-	switch (variable) {
-	case HW_VAR_MEDIA_STATUS:
-		rtl8723a_set_media_status(padapter, *val);
-		break;
-
-	case HW_VAR_MEDIA_STATUS1:
-		rtl8723a_set_media_status1(padapter, *val);
-		break;
-
-	case HW_VAR_SET_OPMODE:
-		hw_var_set_opmode(padapter, *val);
-		break;
-
-	case HW_VAR_MAC_ADDR:
-		hw_var_set_macaddr(padapter, val);
-		break;
-
-	case HW_VAR_BSSID:
-		hw_var_set_bssid(padapter, val);
-		break;
-
-	case HW_VAR_BASIC_RATE:
-		HalSetBrateCfg23a(padapter, val);
-		break;
-
-	case HW_VAR_TXPAUSE:
-		rtl8723a_set_tx_pause(padapter, *val);
-		break;
-
-	case HW_VAR_BCN_FUNC:
-		rtl8723a_set_bcn_func(padapter, *val);
-		break;
-
-	case HW_VAR_CORRECT_TSF:
-		hw_var_set_correct_tsf(padapter);
-		break;
-
-	case HW_VAR_CHECK_BSSID:
-		rtl8723a_check_bssid(padapter, *val);
-		break;
-
-	case HW_VAR_MLME_DISCONNECT:
-		hw_var_set_mlme_disconnect(padapter);
-		break;
-
-	case HW_VAR_MLME_SITESURVEY:
-		rtl8723a_mlme_sitesurvey(padapter, *val);
-		break;
-
-	case HW_VAR_MLME_JOIN:
-		hw_var_set_mlme_join(padapter, *val);
-		break;
-
-	case HW_VAR_ON_RCR_AM:
-		rtl8723a_on_rcr_am(padapter);
-		break;
-
-	case HW_VAR_OFF_RCR_AM:
-		rtl8723a_off_rcr_am(padapter);
-		break;
-
-	case HW_VAR_BEACON_INTERVAL:
-		rtl8723a_set_beacon_interval(padapter, *((u16 *) val));
-		break;
-
-	case HW_VAR_SLOT_TIME:
-		rtl8723a_set_slot_time(padapter, *val);
-		break;
-
-	case HW_VAR_RESP_SIFS:
-		rtl8723a_set_resp_sifs(padapter, val[0], val[1],
-				       val[2], val[3]);
-		break;
-
-	case HW_VAR_ACK_PREAMBLE:
-		rtl8723a_ack_preamble(padapter, *val);
-		break;
-
-	case HW_VAR_SEC_CFG:
-		rtl8723a_set_sec_cfg(padapter, *val);
-		break;
-
-	case HW_VAR_DM_FLAG:
-		rtl8723a_odm_support_ability_write(padapter, *val32);
-		break;
-	case HW_VAR_DM_FUNC_OP:
-		rtl8723a_odm_support_ability_backup(padapter, *val);
-		break;
-	case HW_VAR_DM_FUNC_SET:
-		rtl8723a_odm_support_ability_set(padapter, *val32);
-		break;
-
-	case HW_VAR_DM_FUNC_CLR:
-		rtl8723a_odm_support_ability_clr(padapter, *val32);
-		break;
-
-	case HW_VAR_CAM_EMPTY_ENTRY:
-		rtl8723a_cam_empty_entry(padapter, *val);
-		break;
-
-	case HW_VAR_CAM_INVALID_ALL:
-		rtl8723a_cam_invalid_all(padapter);
-		break;
-
-	case HW_VAR_CAM_WRITE:
-		rtl8723a_cam_write(padapter, val32[0], val32[1]);
-		break;
-
-	case HW_VAR_AC_PARAM_VO:
-		rtl8723a_set_ac_param_vo(padapter, *val32);
-		break;
-
-	case HW_VAR_AC_PARAM_VI:
-		rtl8723a_set_ac_param_vi(padapter, *val32);
-		break;
-
-	case HW_VAR_AC_PARAM_BE:
-		rtl8723a_set_ac_param_be(padapter, *val32);
-		break;
-
-	case HW_VAR_AC_PARAM_BK:
-		rtl8723a_set_ac_param_bk(padapter, *val32);
-		break;
-
-	case HW_VAR_ACM_CTRL:
-		rtl8723a_set_acm_ctrl(padapter, *val);
-		break;
-
-	case HW_VAR_AMPDU_MIN_SPACE:
-		rtl8723a_set_ampdu_min_space(padapter, *val);
-		break;
-
-	case HW_VAR_AMPDU_FACTOR:
-		rtl8723a_set_ampdu_factor(padapter, *val);
-		break;
-
-	case HW_VAR_RXDMA_AGG_PG_TH:
-		rtl8723a_set_rxdma_agg_pg_th(padapter, *val);
-		break;
-
-	case HW_VAR_H2C_FW_PWRMODE:
-		rtl8723a_set_FwPwrMode_cmd(padapter, *val);
-		break;
-
-	case HW_VAR_H2C_FW_JOINBSSRPT:
-		rtl8723a_set_FwJoinBssReport_cmd(padapter, *val);
-		break;
-
-#ifdef CONFIG_8723AU_P2P
-	case HW_VAR_H2C_FW_P2P_PS_OFFLOAD:
-		rtl8723a_set_p2p_ps_offload_cmd(padapter, *val);
-		break;
-#endif /* CONFIG_8723AU_P2P */
-
-	case HW_VAR_INITIAL_GAIN:
-		rtl8723a_set_initial_gain(padapter, *val32);
-		break;
-	case HW_VAR_EFUSE_BYTES:
-		pHalData->EfuseUsedBytes = *((u16 *) val);
-		break;
-	case HW_VAR_EFUSE_BT_BYTES:
-		pHalData->BTEfuseUsedBytes = *((u16 *) val);
-		break;
-	case HW_VAR_FIFO_CLEARN_UP:
-		rtl8723a_fifo_cleanup(padapter);
-		break;
-	case HW_VAR_CHECK_TXBUF:
-		break;
-	case HW_VAR_APFM_ON_MAC:
-		rtl8723a_set_apfm_on_mac(padapter, *val);
-		break;
-
-	case HW_VAR_NAV_UPPER:
-		rtl8723a_set_nav_upper(padapter, *val32);
-		break;
-	case HW_VAR_BCN_VALID:
-		rtl8723a_bcn_valid(padapter);
-		break;
-	default:
-		break;
-	}
-
-}
-
-void GetHwReg8723A(struct rtw_adapter *padapter, u8 variable, u8 *val)
-{
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-
-	switch (variable) {
-	case HW_VAR_BASIC_RATE:
-		*((u16 *) val) = pHalData->BasicRateSet;
-		break;
-
-	case HW_VAR_TXPAUSE:
-		*val = rtw_read8(padapter, REG_TXPAUSE);
-		break;
-
-	case HW_VAR_BCN_VALID:
-		/* BCN_VALID, BIT16 of REG_TDECTRL = BIT0 of REG_TDECTRL+2 */
-		val[0] = (BIT0 & rtw_read8(padapter, REG_TDECTRL + 2)) ? true :
-			false;
-		break;
-
-	case HW_VAR_RF_TYPE:
-		*val = pHalData->rf_type;
-		break;
-
-	case HW_VAR_DM_FLAG:
-	{
-		struct dm_odm_t *podmpriv = &pHalData->odmpriv;
-		*((u32 *) val) = podmpriv->SupportAbility;
-	}
-		break;
-
-	case HW_VAR_FWLPS_RF_ON:
-	{
-		/*  When we halt NIC, we should check if FW LPS is leave. */
-		u32 valRCR;
-
-		if ((padapter->bSurpriseRemoved == true) ||
-		    (padapter->pwrctrlpriv.rf_pwrstate == rf_off)) {
-			/*  If it is in HW/SW Radio OFF or IPS state, we do
-			    not check Fw LPS Leave, because Fw is unload. */
-			*val = true;
-		} else {
-			valRCR = rtw_read32(padapter, REG_RCR);
-			valRCR &= 0x00070000;
-			if (valRCR)
-				*val = false;
-			else
-				*val = true;
-		}
-	}
-		break;
-	case HW_VAR_EFUSE_BYTES:
-		*((u16 *) val) = pHalData->EfuseUsedBytes;
-		break;
-
-	case HW_VAR_EFUSE_BT_BYTES:
-		*((u16 *) val) = pHalData->BTEfuseUsedBytes;
-		break;
-
-	case HW_VAR_APFM_ON_MAC:
-		*val = pHalData->bMacPwrCtrlOn;
-		break;
-	case HW_VAR_CHK_HI_QUEUE_EMPTY:
-		*val =
-		    ((rtw_read32(padapter, REG_HGQ_INFORMATION) & 0x0000ff00) ==
-		     0) ? true : false;
-		break;
-	}
-}
-
-#ifdef CONFIG_8723AU_BT_COEXIST
-
-void rtl8723a_SingleDualAntennaDetection(struct rtw_adapter *padapter)
-{
-	struct hal_data_8723a *pHalData;
-	struct dm_odm_t *pDM_Odm;
-	struct sw_ant_sw *pDM_SWAT_Table;
-	u8 i;
-
-	pHalData = GET_HAL_DATA(padapter);
-	pDM_Odm = &pHalData->odmpriv;
-	pDM_SWAT_Table = &pDM_Odm->DM_SWAT_Table;
-
-	/*  */
-	/*  <Roger_Notes> RTL8723A Single and Dual antenna dynamic detection
-	    mechanism when RF power state is on. */
-	/*  We should take power tracking, IQK, LCK, RCK RF read/write
-	    operation into consideration. */
-	/*  2011.12.15. */
-	/*  */
-	if (!pHalData->bAntennaDetected) {
-		u8 btAntNum = BT_GetPGAntNum(padapter);
-
-		/*  Set default antenna B status */
-		if (btAntNum == Ant_x2)
-			pDM_SWAT_Table->ANTB_ON = true;
-		else if (btAntNum == Ant_x1)
-			pDM_SWAT_Table->ANTB_ON = false;
-		else
-			pDM_SWAT_Table->ANTB_ON = true;
-
-		if (pHalData->CustomerID != RT_CID_TOSHIBA) {
-			for (i = 0; i < MAX_ANTENNA_DETECTION_CNT; i++) {
-				if (ODM_SingleDualAntennaDetection
-				    (&pHalData->odmpriv, ANTTESTALL) == true)
-					break;
-			}
-
-			/*  Set default antenna number for BT coexistence */
-			if (btAntNum == Ant_x2)
-				BT_SetBtCoexCurrAntNum(padapter,
-						       pDM_SWAT_Table->
-						       ANTB_ON ? 2 : 1);
-		}
-		pHalData->bAntennaDetected = true;
-	}
-}
-#endif /*  CONFIG_8723AU_BT_COEXIST */
-
-void rtl8723a_clone_haldata(struct rtw_adapter *dst_adapter,
-			    struct rtw_adapter *src_adapter)
-{
-	memcpy(dst_adapter->HalData, src_adapter->HalData,
-	       dst_adapter->hal_data_sz);
-}
-
-void rtl8723a_start_thread(struct rtw_adapter *padapter)
-{
-}
-
-void rtl8723a_stop_thread(struct rtw_adapter *padapter)
-{
 }
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_phycfg.c b/drivers/staging/rtl8723au/hal/rtl8723a_phycfg.c
index 8400e6e2fca..d23525e664f 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_phycfg.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_phycfg.c
@@ -18,6 +18,7 @@
 #include <drv_types.h>
 
 #include <rtl8723a_hal.h>
+#include <usb_ops_linux.h>
 
 /*---------------------------Define Local Constant---------------------------*/
 /* Channel switch:The size of command tables for switch channel*/
@@ -87,7 +88,7 @@ PHY_QueryBBReg(struct rtw_adapter *Adapter, u32 RegAddr, u32 BitMask)
 {
 	u32	ReturnValue = 0, OriginalValue, BitShift;
 
-	OriginalValue = rtw_read32(Adapter, RegAddr);
+	OriginalValue = rtl8723au_read32(Adapter, RegAddr);
 	BitShift = phy_CalculateBitShift(BitMask);
 	ReturnValue = (OriginalValue & BitMask) >> BitShift;
 	return ReturnValue;
@@ -123,12 +124,12 @@ PHY_SetBBReg(struct rtw_adapter *Adapter, u32 RegAddr, u32 BitMask, u32	Data)
 	/* RT_TRACE(COMP_RF, DBG_TRACE, ("--->PHY_SetBBReg(): RegAddr(%#lx), BitMask(%#lx), Data(%#lx)\n", RegAddr, BitMask, Data)); */
 
 	if (BitMask != bMaskDWord) {/* if not "double word" write */
-		OriginalValue = rtw_read32(Adapter, RegAddr);
+		OriginalValue = rtl8723au_read32(Adapter, RegAddr);
 		BitShift = phy_CalculateBitShift(BitMask);
 		Data = ((OriginalValue & (~BitMask)) | (Data << BitShift));
 	}
 
-	rtw_write32(Adapter, RegAddr, Data);
+	rtl8723au_write32(Adapter, RegAddr, Data);
 
 	/* RTPRINT(FPHY, PHY_BBW, ("BBW MASK = 0x%lx Addr[0x%lx]= 0x%lx\n", BitMask, RegAddr, Data)); */
 	/* RT_TRACE(COMP_RF, DBG_TRACE, ("<---PHY_SetBBReg(): RegAddr(%#lx), BitMask(%#lx), Data(%#lx)\n", RegAddr, BitMask, Data)); */
@@ -212,10 +213,12 @@ phy_RFSerialRead(struct rtw_adapter *Adapter, enum RF_RADIO_PATH eRFPath,
 
 	if (eRFPath == RF_PATH_A)
 		RfPiEnable = (u8)PHY_QueryBBReg(Adapter,
-						rFPGA0_XA_HSSIParameter1, BIT8);
+						rFPGA0_XA_HSSIParameter1,
+						BIT(8));
 	else if (eRFPath == RF_PATH_B)
 		RfPiEnable = (u8)PHY_QueryBBReg(Adapter,
-						rFPGA0_XB_HSSIParameter1, BIT8);
+						rFPGA0_XB_HSSIParameter1,
+						BIT(8));
 
 	if (RfPiEnable)	{
 		/* Read from BBreg8b8, 12 bits for 8190, 20bits for T65 RF */
@@ -413,28 +416,23 @@ PHY_SetRFReg(struct rtw_adapter *Adapter, enum RF_RADIO_PATH eRFPath,
  *  08/12/2008	MHC		Create Version 0.
  *
  *---------------------------------------------------------------------------*/
-s32 PHY_MACConfig8723A(struct rtw_adapter *Adapter)
+int PHY_MACConfig8723A(struct rtw_adapter *Adapter)
 {
 	int rtStatus = _SUCCESS;
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
-	s8 *pszMACRegFile;
-	s8 sz8723MACRegFile[] = RTL8723_PHY_MACREG;
 	bool is92C = IS_92C_SERIAL(pHalData->VersionID);
 
-	pszMACRegFile = sz8723MACRegFile;
-
 	/*  */
 	/*  Config MAC */
 	/*  */
-	if (HAL_STATUS_FAILURE ==
-	    ODM_ConfigMACWithHeaderFile23a(&pHalData->odmpriv))
+	if (ODM_ConfigMACWithHeaderFile23a(&pHalData->odmpriv) == _FAIL)
 		rtStatus = _FAIL;
 
 	/*  2010.07.13 AMPDU aggregation number 9 */
 	/* rtw_write16(Adapter, REG_MAX_AGGR_NUM, MAX_AGGR_NUM); */
-	rtw_write8(Adapter, REG_MAX_AGGR_NUM, 0x0A); /* By tynli. 2010.11.18. */
+	rtl8723au_write8(Adapter, REG_MAX_AGGR_NUM, 0x0A);
 	if (is92C && (BOARD_USB_DONGLE == pHalData->BoardType))
-		rtw_write8(Adapter, 0x40, 0x04);
+		rtl8723au_write8(Adapter, 0x40, 0x04);
 
 	return rtStatus;
 }
@@ -751,27 +749,12 @@ phy_BB8723a_Config_ParaFile(struct rtw_adapter *Adapter)
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 	int rtStatus = _SUCCESS;
 
-	u8 sz8723BBRegFile[] = RTL8723_PHY_REG;
-	u8 sz8723AGCTableFile[] = RTL8723_AGC_TAB;
-	u8 sz8723BBRegPgFile[] = RTL8723_PHY_REG_PG;
-	u8 sz8723BBRegMpFile[] = RTL8723_PHY_REG_MP;
-
-	u8 *pszBBRegFile = NULL, *pszAGCTableFile = NULL;
-	u8 *pszBBRegPgFile = NULL, *pszBBRegMpFile = NULL;
-
-	/* RT_TRACE(COMP_INIT, DBG_TRACE, ("==>phy_BB8192S_Config_ParaFile\n")); */
-
-	pszBBRegFile = sz8723BBRegFile ;
-	pszAGCTableFile = sz8723AGCTableFile;
-	pszBBRegPgFile = sz8723BBRegPgFile;
-	pszBBRegMpFile = sz8723BBRegMpFile;
-
 	/*  */
 	/*  1. Read PHY_REG.TXT BB INIT!! */
 	/*  We will seperate as 88C / 92C according to chip version */
 	/*  */
-	if (HAL_STATUS_FAILURE == ODM_ConfigBBWithHeaderFile23a(&pHalData->odmpriv,
-							     CONFIG_BB_PHY_REG))
+	if (ODM_ConfigBBWithHeaderFile23a(&pHalData->odmpriv,
+					  CONFIG_BB_PHY_REG) == _FAIL)
 		rtStatus = _FAIL;
 	if (rtStatus != _SUCCESS)
 		goto phy_BB8190_Config_ParaFile_Fail;
@@ -801,8 +784,8 @@ phy_BB8723a_Config_ParaFile(struct rtw_adapter *Adapter)
 	/*  */
 	/*  3. BB AGC table Initialization */
 	/*  */
-	if (HAL_STATUS_FAILURE == ODM_ConfigBBWithHeaderFile23a(&pHalData->odmpriv,
-							     CONFIG_BB_AGC_TAB))
+	if (ODM_ConfigBBWithHeaderFile23a(&pHalData->odmpriv,
+					  CONFIG_BB_AGC_TAB) == _FAIL)
 		rtStatus = _FAIL;
 
 phy_BB8190_Config_ParaFile_Fail:
@@ -822,30 +805,30 @@ PHY_BBConfig8723A(struct rtw_adapter *Adapter)
 
 	/*  Suggested by Scott. tynli_test. 2010.12.30. */
 	/* 1. 0x28[1] = 1 */
-	TmpU1B = rtw_read8(Adapter, REG_AFE_PLL_CTRL);
+	TmpU1B = rtl8723au_read8(Adapter, REG_AFE_PLL_CTRL);
 	udelay(2);
-	rtw_write8(Adapter, REG_AFE_PLL_CTRL, (TmpU1B|BIT1));
+	rtl8723au_write8(Adapter, REG_AFE_PLL_CTRL, TmpU1B | BIT(1));
 	udelay(2);
 
 	/* 2. 0x29[7:0] = 0xFF */
-	rtw_write8(Adapter, REG_AFE_PLL_CTRL+1, 0xff);
+	rtl8723au_write8(Adapter, REG_AFE_PLL_CTRL+1, 0xff);
 	udelay(2);
 
 	/* 3. 0x02[1:0] = 2b'11 */
-	TmpU1B = rtw_read8(Adapter, REG_SYS_FUNC_EN);
-	rtw_write8(Adapter, REG_SYS_FUNC_EN,
-		   (TmpU1B | FEN_BB_GLB_RSTn | FEN_BBRSTB));
+	TmpU1B = rtl8723au_read8(Adapter, REG_SYS_FUNC_EN);
+	rtl8723au_write8(Adapter, REG_SYS_FUNC_EN,
+			 (TmpU1B | FEN_BB_GLB_RSTn | FEN_BBRSTB));
 
 	/* 4. 0x25[6] = 0 */
-	TmpU1B = rtw_read8(Adapter, REG_AFE_XTAL_CTRL + 1);
-	rtw_write8(Adapter, REG_AFE_XTAL_CTRL+1, (TmpU1B & (~BIT6)));
+	TmpU1B = rtl8723au_read8(Adapter, REG_AFE_XTAL_CTRL + 1);
+	rtl8723au_write8(Adapter, REG_AFE_XTAL_CTRL+1, TmpU1B & ~BIT(6));
 
 	/* 5. 0x24[20] = 0	Advised by SD3 Alex Wang. 2011.02.09. */
-	TmpU1B = rtw_read8(Adapter, REG_AFE_XTAL_CTRL+2);
-	rtw_write8(Adapter, REG_AFE_XTAL_CTRL+2, (TmpU1B & (~BIT4)));
+	TmpU1B = rtl8723au_read8(Adapter, REG_AFE_XTAL_CTRL+2);
+	rtl8723au_write8(Adapter, REG_AFE_XTAL_CTRL+2, TmpU1B & ~BIT(4));
 
 	/* 6. 0x1f[7:0] = 0x07 */
-	rtw_write8(Adapter, REG_RF_CTRL, 0x07);
+	rtl8723au_write8(Adapter, REG_RF_CTRL, 0x07);
 
 	/*  */
 	/*  Config BB and AGC */
@@ -974,20 +957,20 @@ _PHY_SetBWMode23a92C(struct rtw_adapter *Adapter)
 	/* 3<1>Set MAC register */
 	/* 3 */
 
-	regBwOpMode = rtw_read8(Adapter, REG_BWOPMODE);
-	regRRSR_RSC = rtw_read8(Adapter, REG_RRSR+2);
+	regBwOpMode = rtl8723au_read8(Adapter, REG_BWOPMODE);
+	regRRSR_RSC = rtl8723au_read8(Adapter, REG_RRSR+2);
 
 	switch (pHalData->CurrentChannelBW) {
 	case HT_CHANNEL_WIDTH_20:
 		regBwOpMode |= BW_OPMODE_20MHZ;
-		rtw_write8(Adapter, REG_BWOPMODE, regBwOpMode);
+		rtl8723au_write8(Adapter, REG_BWOPMODE, regBwOpMode);
 		break;
 	case HT_CHANNEL_WIDTH_40:
 		regBwOpMode &= ~BW_OPMODE_20MHZ;
-		rtw_write8(Adapter, REG_BWOPMODE, regBwOpMode);
+		rtl8723au_write8(Adapter, REG_BWOPMODE, regBwOpMode);
 		regRRSR_RSC = (regRRSR_RSC & 0x90) |
 			(pHalData->nCur40MhzPrimeSC << 5);
-		rtw_write8(Adapter, REG_RRSR+2, regRRSR_RSC);
+		rtl8723au_write8(Adapter, REG_RRSR+2, regRRSR_RSC);
 		break;
 
 	default:
@@ -1002,7 +985,7 @@ _PHY_SetBWMode23a92C(struct rtw_adapter *Adapter)
 	case HT_CHANNEL_WIDTH_20:
 		PHY_SetBBReg(Adapter, rFPGA0_RFMOD, bRFMOD, 0x0);
 		PHY_SetBBReg(Adapter, rFPGA1_RFMOD, bRFMOD, 0x0);
-		PHY_SetBBReg(Adapter, rFPGA0_AnalogParameter2, BIT10, 1);
+		PHY_SetBBReg(Adapter, rFPGA0_AnalogParameter2, BIT(10), 1);
 
 		break;
 
@@ -1017,9 +1000,9 @@ _PHY_SetBWMode23a92C(struct rtw_adapter *Adapter)
 			     (pHalData->nCur40MhzPrimeSC >> 1));
 		PHY_SetBBReg(Adapter, rOFDM1_LSTF, 0xC00,
 			     pHalData->nCur40MhzPrimeSC);
-		PHY_SetBBReg(Adapter, rFPGA0_AnalogParameter2, BIT10, 0);
+		PHY_SetBBReg(Adapter, rFPGA0_AnalogParameter2, BIT(10), 0);
 
-		PHY_SetBBReg(Adapter, 0x818, (BIT26 | BIT27),
+		PHY_SetBBReg(Adapter, 0x818, BIT(26) | BIT(27),
 			     (pHalData->nCur40MhzPrimeSC ==
 			      HAL_PRIME_CHNL_OFFSET_LOWER) ? 2:1);
 		break;
@@ -1116,7 +1099,7 @@ static void _PHY_SwChnl8723A(struct rtw_adapter *Adapter, u8 channel)
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 
 	if (Adapter->bNotifyChannelChange)
-		DBG_8723A("[%s] ch = %d\n", __FUNCTION__, channel);
+		DBG_8723A("[%s] ch = %d\n", __func__, channel);
 
 	/* s1. pre common command - CmdID_SetTxPowerLevel */
 	PHY_SetTxPowerLevel8723A(Adapter, channel);
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_rf6052.c b/drivers/staging/rtl8723au/hal/rtl8723a_rf6052.c
index ed39c18c3f8..c30bd232bf6 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_rf6052.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_rf6052.c
@@ -40,6 +40,7 @@
 #include <drv_types.h>
 
 #include <rtl8723a_hal.h>
+#include <usb_ops_linux.h>
 
 /*---------------------------Define Local Constant---------------------------*/
 /*  Define local structure for debug!!!!! */
@@ -368,7 +369,8 @@ static void writeOFDMPowerReg(struct rtw_adapter *Adapter, u8 index, u32 *pValue
 					writeVal = (writeVal > 8) ? (writeVal-8) : 0;
 				else
 					writeVal = (writeVal > 6) ? (writeVal-6) : 0;
-				rtw_write8(Adapter, (u32)(RegOffset+i), (u8)writeVal);
+				rtl8723au_write8(Adapter, RegOffset + i,
+						 (u8)writeVal);
 			}
 		}
 	}
@@ -417,12 +419,6 @@ static int phy_RF6052_Config_ParaFile(struct rtw_adapter *Adapter)
 	struct bb_reg_define	*pPhyReg;
 	int rtStatus = _SUCCESS;
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
-	static char sz8723RadioAFile[] = RTL8723_PHY_RADIO_A;
-	static char sz8723RadioBFile[] = RTL8723_PHY_RADIO_B;
-	char *pszRadioAFile, *pszRadioBFile;
-
-	pszRadioAFile = sz8723RadioAFile;
-	pszRadioBFile = sz8723RadioBFile;
 
 	/* 3----------------------------------------------------------------- */
 	/* 3 <2> Initialize RF */
@@ -459,11 +455,19 @@ static int phy_RF6052_Config_ParaFile(struct rtw_adapter *Adapter)
 		/*----Initialize RF fom connfiguration file----*/
 		switch (eRFPath) {
 		case RF_PATH_A:
-			if (HAL_STATUS_FAILURE == ODM_ConfigRFWithHeaderFile23a(&pHalData->odmpriv, (enum RF_RADIO_PATH)eRFPath, (enum RF_RADIO_PATH)eRFPath))
+			if (ODM_ConfigRFWithHeaderFile23a(&pHalData->odmpriv,
+							  (enum RF_RADIO_PATH)
+							  eRFPath,
+							  (enum RF_RADIO_PATH)
+							  eRFPath) == _FAIL)
 				rtStatus = _FAIL;
 			break;
 		case RF_PATH_B:
-			if (HAL_STATUS_FAILURE == ODM_ConfigRFWithHeaderFile23a(&pHalData->odmpriv, (enum RF_RADIO_PATH)eRFPath, (enum RF_RADIO_PATH)eRFPath))
+			if (ODM_ConfigRFWithHeaderFile23a(&pHalData->odmpriv,
+							  (enum RF_RADIO_PATH)
+							  eRFPath,
+							  (enum RF_RADIO_PATH)
+							  eRFPath) == _FAIL)
 				rtStatus = _FAIL;
 			break;
 		}
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_sreset.c b/drivers/staging/rtl8723au/hal/rtl8723a_sreset.c
index c0218e734b9..0680e29a552 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_sreset.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_sreset.c
@@ -16,6 +16,7 @@
 
 #include <rtl8723a_sreset.h>
 #include <rtl8723a_hal.h>
+#include <usb_ops_linux.h>
 
 void rtl8723a_sreset_xmit_status_check(struct rtw_adapter *padapter)
 {
@@ -27,10 +28,10 @@ void rtl8723a_sreset_xmit_status_check(struct rtw_adapter *padapter)
 	unsigned int diff_time;
 	u32 txdma_status;
 
-	txdma_status = rtw_read32(padapter, REG_TXDMA_STATUS);
+	txdma_status = rtl8723au_read32(padapter, REG_TXDMA_STATUS);
 	if (txdma_status != 0) {
 		DBG_8723A("%s REG_TXDMA_STATUS:0x%08x\n", __func__, txdma_status);
-		rtw_hal_sreset_reset23a(padapter);
+		rtw_sreset_reset(padapter);
 	}
 
 	current_time = jiffies;
@@ -47,7 +48,7 @@ void rtl8723a_sreset_xmit_status_check(struct rtw_adapter *padapter)
 				if (diff_time > 4000) {
 					/* padapter->Wifi_Error_Status = WIFI_TX_HANG; */
 					DBG_8723A("%s tx hang\n", __func__);
-					rtw_hal_sreset_reset23a(padapter);
+					rtw_sreset_reset(padapter);
 				}
 			}
 		}
@@ -55,7 +56,7 @@ void rtl8723a_sreset_xmit_status_check(struct rtw_adapter *padapter)
 
 	if (psrtpriv->dbg_trigger_point == SRESET_TGP_XMIT_STATUS) {
 		psrtpriv->dbg_trigger_point = SRESET_TGP_NULL;
-		rtw_hal_sreset_reset23a(padapter);
+		rtw_sreset_reset(padapter);
 		return;
 	}
 }
@@ -67,7 +68,7 @@ void rtl8723a_sreset_linked_status_check(struct rtw_adapter *padapter)
 
 	if (psrtpriv->dbg_trigger_point == SRESET_TGP_LINK_STATUS) {
 		psrtpriv->dbg_trigger_point = SRESET_TGP_NULL;
-		rtw_hal_sreset_reset23a(padapter);
+		rtw_sreset_reset(padapter);
 		return;
 	}
 }
diff --git a/drivers/staging/rtl8723au/hal/rtl8723au_led.c b/drivers/staging/rtl8723au/hal/rtl8723au_led.c
index 4d5c909487f..b946636af9b 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723au_led.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723au_led.c
@@ -16,6 +16,7 @@
 #include "drv_types.h"
 #include "rtl8723a_hal.h"
 #include "rtl8723a_led.h"
+#include "usb_ops_linux.h"
 
 /*  */
 /*  LED object. */
@@ -41,14 +42,18 @@ void SwLedOn23a(struct rtw_adapter *padapter, struct led_8723a *pLed)
 	case LED_PIN_GPIO0:
 		break;
 	case LED_PIN_LED0:
-		rtw_write8(padapter, REG_LEDCFG0, (LedCfg&0xf0)|BIT5|BIT6); /*  SW control led0 on. */
+		/*  SW control led0 on. */
+		rtl8723au_write8(padapter, REG_LEDCFG0,
+				 (LedCfg&0xf0)|BIT(5)|BIT(6));
 		break;
 	case LED_PIN_LED1:
-		rtw_write8(padapter, REG_LEDCFG1, (LedCfg&0x00)|BIT6); /*  SW control led1 on. */
+		 /*  SW control led1 on. */
+		rtl8723au_write8(padapter, REG_LEDCFG1, (LedCfg&0x00)|BIT(6));
 		break;
 	case LED_PIN_LED2:
-		LedCfg = rtw_read8(padapter, REG_LEDCFG2);
-		rtw_write8(padapter, REG_LEDCFG2, (LedCfg&0x80)|BIT5); /*  SW control led1 on. */
+		LedCfg = rtl8723au_read8(padapter, REG_LEDCFG2);
+		 /*  SW control led1 on. */
+		rtl8723au_write8(padapter, REG_LEDCFG2, (LedCfg&0x80)|BIT(5));
 		break;
 	default:
 		break;
@@ -70,14 +75,20 @@ void SwLedOff23a(struct rtw_adapter *padapter, struct led_8723a *pLed)
 	case LED_PIN_GPIO0:
 		break;
 	case LED_PIN_LED0:
-		rtw_write8(padapter, REG_LEDCFG0, (LedCfg&0xf0)|BIT5|BIT6); /*  SW control led0 on. */
+		/*  SW control led0 on. */
+		rtl8723au_write8(padapter, REG_LEDCFG0,
+				 (LedCfg&0xf0)|BIT(5)|BIT(6));
 		break;
 	case LED_PIN_LED1:
-		rtw_write8(padapter, REG_LEDCFG1, (LedCfg&0x00)|BIT5|BIT6); /*  SW control led1 on. */
+		/*  SW control led1 on. */
+		rtl8723au_write8(padapter, REG_LEDCFG1,
+				 (LedCfg&0x00)|BIT(5)|BIT(6));
 		break;
 	case LED_PIN_LED2:
-		LedCfg = rtw_read8(padapter, REG_LEDCFG2);
-		rtw_write8(padapter, REG_LEDCFG2, (LedCfg&0x80)|BIT3|BIT5); /*  SW control led1 on. */
+		LedCfg = rtl8723au_read8(padapter, REG_LEDCFG2);
+		/*  SW control led1 on. */
+		rtl8723au_write8(padapter, REG_LEDCFG2,
+				 (LedCfg&0x80)|BIT(3)|BIT(5));
 		break;
 	default:
 		break;
diff --git a/drivers/staging/rtl8723au/hal/rtl8723au_recv.c b/drivers/staging/rtl8723au/hal/rtl8723au_recv.c
index 213d1936109..6075b6dc1be 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723au_recv.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723au_recv.c
@@ -19,16 +19,10 @@
 #include <mlme_osdep.h>
 #include <linux/ip.h>
 #include <linux/if_ether.h>
-#include <ethernet.h>
 #include <usb_ops.h>
 #include <wifi.h>
 #include <rtl8723a_hal.h>
 
-void rtl8723au_init_recvbuf(struct rtw_adapter *padapter,
-			    struct recv_buf *precvbuf)
-{
-}
-
 int rtl8723au_init_recv_priv(struct rtw_adapter *padapter)
 {
 	struct recv_priv *precvpriv = &padapter->recvpriv;
@@ -49,9 +43,6 @@ int rtl8723au_init_recv_priv(struct rtw_adapter *padapter)
 	if (!precvpriv->int_in_buf)
 		DBG_8723A("alloc_mem for interrupt in endpoint fail !!!!\n");
 
-	/* init recv_buf */
-	_rtw_init_queue23a(&precvpriv->free_recv_buf_queue);
-
 	size = NR_RECVBUFF * sizeof(struct recv_buf);
 	precvpriv->precv_buf = kzalloc(size, GFP_KERNEL);
 	if (!precvpriv->precv_buf) {
@@ -66,8 +57,8 @@ int rtl8723au_init_recv_priv(struct rtw_adapter *padapter)
 	for (i = 0; i < NR_RECVBUFF; i++) {
 		INIT_LIST_HEAD(&precvbuf->list);
 
-		res = rtw_os_recvbuf_resource_alloc23a(padapter, precvbuf);
-		if (res == _FAIL)
+		precvbuf->purb = usb_alloc_urb(0, GFP_KERNEL);
+		if (!precvbuf->purb)
 			break;
 
 		precvbuf->adapter = padapter;
@@ -75,8 +66,6 @@ int rtl8723au_init_recv_priv(struct rtw_adapter *padapter)
 		precvbuf++;
 	}
 
-	precvpriv->free_recv_buf_queue_cnt = NR_RECVBUFF;
-
 	skb_queue_head_init(&precvpriv->rx_skb_queue);
 	skb_queue_head_init(&precvpriv->free_recv_skb_queue);
 
@@ -110,7 +99,11 @@ void rtl8723au_free_recv_priv(struct rtw_adapter *padapter)
 	precvbuf = (struct recv_buf *)precvpriv->precv_buf;
 
 	for (i = 0; i < NR_RECVBUFF; i++) {
-		rtw_os_recvbuf_resource_free23a(padapter, precvbuf);
+		usb_free_urb(precvbuf->purb);
+
+		if (precvbuf->pskb)
+			dev_kfree_skb_any(precvbuf->pskb);
+
 		precvbuf++;
 	}
 
@@ -132,11 +125,20 @@ void rtl8723au_free_recv_priv(struct rtw_adapter *padapter)
 	skb_queue_purge(&precvpriv->free_recv_skb_queue);
 }
 
+struct recv_stat_cpu {
+	u32 rxdw0;
+	u32 rxdw1;
+	u32 rxdw2;
+	u32 rxdw3;
+	u32 rxdw4;
+	u32 rxdw5;
+};
+
 void update_recvframe_attrib(struct recv_frame *precvframe,
 			     struct recv_stat *prxstat)
 {
 	struct rx_pkt_attrib *pattrib;
-	struct recv_stat report;
+	struct recv_stat_cpu report;
 	struct rxreport_8723a *prxreport;
 
 	report.rxdw0 = le32_to_cpu(prxstat->rxdw0);
@@ -182,25 +184,43 @@ void update_recvframe_phyinfo(struct recv_frame *precvframe,
 	struct rtw_adapter *padapter = precvframe->adapter;
 	struct rx_pkt_attrib *pattrib = &precvframe->attrib;
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-	struct odm_phy_info *pPHYInfo = (struct odm_phy_info *)(&pattrib->phy_info);
+	struct phy_info *pPHYInfo = &pattrib->phy_info;
 	struct odm_packet_info pkt_info;
 	u8 *sa = NULL, *da;
 	struct sta_priv *pstapriv;
 	struct sta_info *psta;
 	struct sk_buff *skb = precvframe->pkt;
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u8 *wlanhdr = skb->data;
+	bool matchbssid = false;
+	u8 *bssid;
+
+	matchbssid = (!ieee80211_is_ctl(hdr->frame_control) &&
+		      !pattrib->icv_err && !pattrib->crc_err);
+
+	if (matchbssid) {
+		switch (hdr->frame_control &
+			cpu_to_le16(IEEE80211_FCTL_TODS |
+				    IEEE80211_FCTL_FROMDS)) {
+		case cpu_to_le16(IEEE80211_FCTL_TODS):
+			bssid = hdr->addr1;
+			break;
+		case cpu_to_le16(IEEE80211_FCTL_FROMDS):
+			bssid = hdr->addr2;
+			break;
+		case cpu_to_le16(0):
+			bssid = hdr->addr3;
+			break;
+		default:
+			bssid = NULL;
+			matchbssid = false;
+		}
 
-	pkt_info.bPacketMatchBSSID = false;
-	pkt_info.bPacketToSelf = false;
-	pkt_info.bPacketBeacon = false;
+		if (bssid)
+			matchbssid = ether_addr_equal(
+				get_bssid(&padapter->mlmepriv), bssid);
+	}
 
-	pkt_info.bPacketMatchBSSID =
-		(!ieee80211_is_ctl(hdr->frame_control) &&
-		 !pattrib->icv_err &&
-		 !pattrib->crc_err &&
-		 !memcmp(get_hdr_bssid(wlanhdr),
-			 get_bssid(&padapter->mlmepriv), ETH_ALEN));
+	pkt_info.bPacketMatchBSSID = matchbssid;
 
 	da = ieee80211_get_DA(hdr);
 	pkt_info.bPacketToSelf = pkt_info.bPacketMatchBSSID &&
@@ -222,12 +242,12 @@ void update_recvframe_phyinfo(struct recv_frame *precvframe,
 	psta = rtw_get_stainfo23a(pstapriv, sa);
 	if (psta) {
 		pkt_info.StationID = psta->mac_id;
-		/* printk("%s ==> StationID(%d)\n", __FUNCTION__, pkt_info.StationID); */
+		/* printk("%s ==> StationID(%d)\n", __func__, pkt_info.StationID); */
 	}
 	pkt_info.Rate = pattrib->mcs_rate;
 
 	ODM_PhyStatusQuery23a(&pHalData->odmpriv, pPHYInfo,
-			   (u8 *)pphy_status, &pkt_info);
+			      (u8 *)pphy_status, &pkt_info);
 	precvframe->psta = NULL;
 	if (pkt_info.bPacketMatchBSSID &&
 	    (check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE) == true)) {
diff --git a/drivers/staging/rtl8723au/hal/rtl8723au_xmit.c b/drivers/staging/rtl8723au/hal/rtl8723au_xmit.c
index 2af2e3ee1ab..a67850fe6e5 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723au_xmit.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723au_xmit.c
@@ -21,20 +21,6 @@
 /* include <rtl8192c_hal.h> */
 #include <rtl8723a_hal.h>
 
-s32	rtl8723au_init_xmit_priv(struct rtw_adapter *padapter)
-{
-	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
-
-	tasklet_init(&pxmitpriv->xmit_tasklet,
-	     (void(*)(unsigned long))rtl8723au_xmit_tasklet,
-	     (unsigned long)padapter);
-	return _SUCCESS;
-}
-
-void	rtl8723au_free_xmit_priv(struct rtw_adapter *padapter)
-{
-}
-
 static void do_queue_select(struct rtw_adapter	*padapter, struct pkt_attrib *pattrib)
 {
 	u8 qsel;
@@ -87,19 +73,18 @@ static void fill_txdesc_sectype(struct pkt_attrib *pattrib, struct tx_desc *ptxd
 	if ((pattrib->encrypt > 0) && !pattrib->bswenc) {
 		switch (pattrib->encrypt) {
 		/* SEC_TYPE */
-		case _WEP40_:
-		case _WEP104_:
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
 			ptxdesc->txdw1 |= cpu_to_le32((0x01<<22)&0x00c00000);
 			break;
-		case _TKIP_:
-		case _TKIP_WTMIC_:
+		case WLAN_CIPHER_SUITE_TKIP:
 			/* ptxdesc->txdw1 |= cpu_to_le32((0x02<<22)&0x00c00000); */
 			ptxdesc->txdw1 |= cpu_to_le32((0x01<<22)&0x00c00000);
 			break;
-		case _AES_:
+		case WLAN_CIPHER_SUITE_CCMP:
 			ptxdesc->txdw1 |= cpu_to_le32((0x03<<22)&0x00c00000);
 			break;
-		case _NO_PRIVACY_:
+		case 0:
 		default:
 			break;
 		}
@@ -309,10 +294,11 @@ static s32 update_txdesc(struct xmit_frame *pxmitframe, u8 *pmem, s32 sz, u8 bag
 	return pull;
 }
 
-static s32 rtw_dump_xframe(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe)
+static int rtw_dump_xframe(struct rtw_adapter *padapter,
+			   struct xmit_frame *pxmitframe)
 {
-	s32 ret = _SUCCESS;
-	s32 inner_ret = _SUCCESS;
+	int ret = _SUCCESS;
+	int inner_ret = _SUCCESS;
 	int t, sz, w_sz, pull = 0;
 	u8 *mem_addr;
 	u32 ff_hwaddr;
@@ -358,7 +344,8 @@ static s32 rtw_dump_xframe(struct rtw_adapter *padapter, struct xmit_frame *pxmi
 		}
 
 		ff_hwaddr = rtw_get_ff_hwaddr23a(pxmitframe);
-		inner_ret = rtw_write_port(padapter, ff_hwaddr, w_sz, pxmitbuf);
+		inner_ret = rtl8723au_write_port(padapter, ff_hwaddr,
+						 w_sz, pxmitbuf);
 		rtw_count_tx_stats23a(padapter, pxmitframe, sz);
 
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
@@ -377,8 +364,9 @@ static s32 rtw_dump_xframe(struct rtw_adapter *padapter, struct xmit_frame *pxmi
 	return ret;
 }
 
-s32 rtl8723au_xmitframe_complete(struct rtw_adapter *padapter,
-				 struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
+bool rtl8723au_xmitframe_complete(struct rtw_adapter *padapter,
+				  struct xmit_priv *pxmitpriv,
+				  struct xmit_buf *pxmitbuf)
 {
 	struct hw_xmit *phwxmits;
 	struct xmit_frame *pxmitframe;
@@ -427,9 +415,10 @@ s32 rtl8723au_xmitframe_complete(struct rtw_adapter *padapter,
 	return true;
 }
 
-static s32 xmitframe_direct(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe)
+static int xmitframe_direct(struct rtw_adapter *padapter,
+			    struct xmit_frame *pxmitframe)
 {
-	s32 res = _SUCCESS;
+	int res;
 
 	res = rtw_xmitframe_coalesce23a(padapter, pxmitframe->pkt, pxmitframe);
 	if (res == _SUCCESS)
@@ -442,9 +431,10 @@ static s32 xmitframe_direct(struct rtw_adapter *padapter, struct xmit_frame *pxm
  *	true	dump packet directly
  *	false	enqueue packet
  */
-static s32 pre_xmitframe(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe)
+bool rtl8723au_hal_xmit(struct rtw_adapter *padapter,
+			struct xmit_frame *pxmitframe)
 {
-	s32 res;
+	int res;
 	struct xmit_buf *pxmitbuf = NULL;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct pkt_attrib *pattrib = &pxmitframe->attrib;
@@ -513,26 +503,17 @@ enqueue:
 	return false;
 }
 
-s32 rtl8723au_mgnt_xmit(struct rtw_adapter *padapter, struct xmit_frame *pmgntframe)
+int rtl8723au_mgnt_xmit(struct rtw_adapter *padapter,
+			struct xmit_frame *pmgntframe)
 {
 	return rtw_dump_xframe(padapter, pmgntframe);
 }
 
-/*
- * Return
- *	true	dump packet directly ok
- *	false	temporary can't transmit packets to hardware
- */
-s32 rtl8723au_hal_xmit(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe)
-{
-	return pre_xmitframe(padapter, pxmitframe);
-}
-
-s32	rtl8723au_hal_xmitframe_enqueue(struct rtw_adapter *padapter,
-					struct xmit_frame *pxmitframe)
+int rtl8723au_hal_xmitframe_enqueue(struct rtw_adapter *padapter,
+				    struct xmit_frame *pxmitframe)
 {
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	s32 err;
+	int err;
 
 	err = rtw_xmitframe_enqueue23a(padapter, pxmitframe);
 	if (err != _SUCCESS) {
diff --git a/drivers/staging/rtl8723au/hal/usb_halinit.c b/drivers/staging/rtl8723au/hal/usb_halinit.c
index e206829d50f..6a7fb28e03d 100644
--- a/drivers/staging/rtl8723au/hal/usb_halinit.c
+++ b/drivers/staging/rtl8723au/hal/usb_halinit.c
@@ -25,8 +25,6 @@
 #include <linux/ieee80211.h>
 
 #include <usb_ops.h>
-#include <usb_hal.h>
-#include <usb_osintf.h>
 
 static void
 _ConfigChipOutEP(struct rtw_adapter *pAdapter, u8 NumOutPipe)
@@ -38,7 +36,7 @@ _ConfigChipOutEP(struct rtw_adapter *pAdapter, u8 NumOutPipe)
 	pHalData->OutEpNumber = 0;
 
 	/*  Normal and High queue */
-	value8 = rtw_read8(pAdapter, (REG_NORMAL_SIE_EP + 1));
+	value8 = rtl8723au_read8(pAdapter, (REG_NORMAL_SIE_EP + 1));
 
 	if (value8 & USB_NORMAL_SIE_EP_MASK) {
 		pHalData->OutEpQueueSel |= TX_SELE_HQ;
@@ -51,7 +49,7 @@ _ConfigChipOutEP(struct rtw_adapter *pAdapter, u8 NumOutPipe)
 	}
 
 	/*  Low queue */
-	value8 = rtw_read8(pAdapter, (REG_NORMAL_SIE_EP + 2));
+	value8 = rtl8723au_read8(pAdapter, (REG_NORMAL_SIE_EP + 2));
 	if (value8 & USB_NORMAL_SIE_EP_MASK) {
 		pHalData->OutEpQueueSel |= TX_SELE_LQ;
 		pHalData->OutEpNumber++;
@@ -82,19 +80,11 @@ static bool rtl8723au_set_queue_pipe_mapping(struct rtw_adapter *pAdapter,
 	return result;
 }
 
-static void rtl8723au_interface_configure(struct rtw_adapter *padapter)
+void rtl8723au_chip_configure(struct rtw_adapter *padapter)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter);
 
-	if (pdvobjpriv->ishighspeed == true) {
-		/* 512 bytes */
-		pHalData->UsbBulkOutSize = USB_HIGH_SPEED_BULK_SIZE;
-	} else {
-		/* 64 bytes */
-		pHalData->UsbBulkOutSize = USB_FULL_SPEED_BULK_SIZE;
-	}
-
 	pHalData->interfaceIndex = pdvobjpriv->InterfaceNumber;
 
 	rtl8723au_set_queue_pipe_mapping(padapter,
@@ -102,15 +92,15 @@ static void rtl8723au_interface_configure(struct rtw_adapter *padapter)
 					 pdvobjpriv->RtNumOutPipes);
 }
 
-static u8 _InitPowerOn(struct rtw_adapter *padapter)
+static int _InitPowerOn(struct rtw_adapter *padapter)
 {
-	u8 status = _SUCCESS;
+	int status = _SUCCESS;
 	u16 value16 = 0;
 	u8 value8 = 0;
 
 	/*  RSV_CTRL 0x1C[7:0] = 0x00
 	    unlock ISO/CLK/Power control register */
-	rtw_write8(padapter, REG_RSV_CTRL, 0x0);
+	rtl8723au_write8(padapter, REG_RSV_CTRL, 0x0);
 
 	/*  HW Power on sequence */
 	if (!HalPwrSeqCmdParsing23a(padapter, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
@@ -118,20 +108,20 @@ static u8 _InitPowerOn(struct rtw_adapter *padapter)
 		return _FAIL;
 
 	/*  0x04[19] = 1, suggest by Jackie 2011.05.09, reset 8051 */
-	value8 = rtw_read8(padapter, REG_APS_FSMCO+2);
-	rtw_write8(padapter, REG_APS_FSMCO + 2, (value8 | BIT3));
+	value8 = rtl8723au_read8(padapter, REG_APS_FSMCO+2);
+	rtl8723au_write8(padapter, REG_APS_FSMCO + 2, value8 | BIT(3));
 
 	/*  Enable MAC DMA/WMAC/SCHEDULE/SEC block */
 	/*  Set CR bit10 to enable 32k calibration. Suggested by SD1 Gimmy.
 	    Added by tynli. 2011.08.31. */
-	value16 = rtw_read16(padapter, REG_CR);
+	value16 = rtl8723au_read16(padapter, REG_CR);
 	value16 |= (HCI_TXDMA_EN | HCI_RXDMA_EN | TXDMA_EN | RXDMA_EN |
 		    PROTOCOL_EN | SCHEDULE_EN | MACTXEN | MACRXEN |
 		    ENSEC | CALTMR_EN);
-	rtw_write16(padapter, REG_CR, value16);
+	rtl8723au_write16(padapter, REG_CR, value16);
 
 	/* for Efuse PG, suggest by Jackie 2011.11.23 */
-	PHY_SetBBReg(padapter, REG_EFUSE_CTRL, BIT28|BIT29|BIT30, 0x06);
+	PHY_SetBBReg(padapter, REG_EFUSE_CTRL, BIT(28)|BIT(29)|BIT(30), 0x06);
 
 	return status;
 }
@@ -143,10 +133,10 @@ static void _InitInterrupt(struct rtw_adapter *Adapter)
 
 	/*  HISR - turn all on */
 	value32 = 0xFFFFFFFF;
-	rtw_write32(Adapter, REG_HISR, value32);
+	rtl8723au_write32(Adapter, REG_HISR, value32);
 
 	/*  HIMR - turn all on */
-	rtw_write32(Adapter, REG_HIMR, value32);
+	rtl8723au_write32(Adapter, REG_HIMR, value32);
 }
 
 static void _InitQueueReservedPage(struct rtw_adapter *Adapter)
@@ -160,37 +150,33 @@ static void _InitQueueReservedPage(struct rtw_adapter *Adapter)
 	u32 value32;
 	u8 value8;
 	bool bWiFiConfig = pregistrypriv->wifi_spec;
-	/* u32			txQPageNum, txQPageUnit, txQRemainPage; */
 
-	{ /* for WMM */
-		/* RT_ASSERT((outEPNum>= 2), ("for WMM , number of out-ep "
-		   "must more than or equal to 2!\n")); */
+	/* RT_ASSERT((outEPNum>= 2), ("for WMM , number of out-ep "
+	   "must more than or equal to 2!\n")); */
 
-		numPubQ = bWiFiConfig ?
-			WMM_NORMAL_PAGE_NUM_PUBQ : NORMAL_PAGE_NUM_PUBQ;
+	numPubQ = bWiFiConfig ? WMM_NORMAL_PAGE_NUM_PUBQ : NORMAL_PAGE_NUM_PUBQ;
 
-		if (pHalData->OutEpQueueSel & TX_SELE_HQ) {
-			numHQ = bWiFiConfig ?
-				WMM_NORMAL_PAGE_NUM_HPQ : NORMAL_PAGE_NUM_HPQ;
-		}
+	if (pHalData->OutEpQueueSel & TX_SELE_HQ) {
+		numHQ = bWiFiConfig ?
+			WMM_NORMAL_PAGE_NUM_HPQ : NORMAL_PAGE_NUM_HPQ;
+	}
 
-		if (pHalData->OutEpQueueSel & TX_SELE_LQ) {
-			numLQ = bWiFiConfig ?
-				WMM_NORMAL_PAGE_NUM_LPQ : NORMAL_PAGE_NUM_LPQ;
-		}
-		/*  NOTE: This step shall be proceed before
-		    writting REG_RQPN. */
-		if (pHalData->OutEpQueueSel & TX_SELE_NQ) {
-			numNQ = bWiFiConfig ?
-				WMM_NORMAL_PAGE_NUM_NPQ : NORMAL_PAGE_NUM_NPQ;
-		}
-		value8 = (u8)_NPQ(numNQ);
-		rtw_write8(Adapter, REG_RQPN_NPQ, value8);
+	if (pHalData->OutEpQueueSel & TX_SELE_LQ) {
+		numLQ = bWiFiConfig ?
+			WMM_NORMAL_PAGE_NUM_LPQ : NORMAL_PAGE_NUM_LPQ;
 	}
+	/*  NOTE: This step shall be proceed before
+	    writting REG_RQPN. */
+	if (pHalData->OutEpQueueSel & TX_SELE_NQ) {
+		numNQ = bWiFiConfig ?
+			WMM_NORMAL_PAGE_NUM_NPQ : NORMAL_PAGE_NUM_NPQ;
+	}
+	value8 = (u8)_NPQ(numNQ);
+	rtl8723au_write8(Adapter, REG_RQPN_NPQ, value8);
 
 	/*  TX DMA */
 	value32 = _HPQ(numHQ) | _LPQ(numLQ) | _PUBQ(numPubQ) | LD_RQPN;
-	rtw_write32(Adapter, REG_RQPN, value32);
+	rtl8723au_write32(Adapter, REG_RQPN, value32);
 }
 
 static void _InitTxBufferBoundary(struct rtw_adapter *Adapter)
@@ -204,11 +190,11 @@ static void _InitTxBufferBoundary(struct rtw_adapter *Adapter)
 	else /* for WMM */
 		txpktbuf_bndy = WMM_NORMAL_TX_PAGE_BOUNDARY;
 
-	rtw_write8(Adapter, REG_TXPKTBUF_BCNQ_BDNY, txpktbuf_bndy);
-	rtw_write8(Adapter, REG_TXPKTBUF_MGQ_BDNY, txpktbuf_bndy);
-	rtw_write8(Adapter, REG_TXPKTBUF_WMAC_LBK_BF_HD, txpktbuf_bndy);
-	rtw_write8(Adapter, REG_TRXFF_BNDY, txpktbuf_bndy);
-	rtw_write8(Adapter, REG_TDECTRL+1, txpktbuf_bndy);
+	rtl8723au_write8(Adapter, REG_TXPKTBUF_BCNQ_BDNY, txpktbuf_bndy);
+	rtl8723au_write8(Adapter, REG_TXPKTBUF_MGQ_BDNY, txpktbuf_bndy);
+	rtl8723au_write8(Adapter, REG_TXPKTBUF_WMAC_LBK_BF_HD, txpktbuf_bndy);
+	rtl8723au_write8(Adapter, REG_TRXFF_BNDY, txpktbuf_bndy);
+	rtl8723au_write8(Adapter, REG_TDECTRL+1, txpktbuf_bndy);
 }
 
 static void _InitPageBoundary(struct rtw_adapter *Adapter)
@@ -217,7 +203,7 @@ static void _InitPageBoundary(struct rtw_adapter *Adapter)
 	/* srand(static_cast<unsigned int>(time(NULL))); */
 	u16 rxff_bndy = 0x27FF;/* rand() % 1) ? 0x27FF : 0x23FF; */
 
-	rtw_write16(Adapter, (REG_TRXFF_BNDY + 2), rxff_bndy);
+	rtl8723au_write16(Adapter, (REG_TRXFF_BNDY + 2), rxff_bndy);
 
 	/*  TODO: ?? shall we set tx boundary? */
 }
@@ -226,13 +212,13 @@ static void
 _InitNormalChipRegPriority(struct rtw_adapter *Adapter, u16 beQ, u16 bkQ,
 			   u16 viQ, u16 voQ, u16 mgtQ, u16 hiQ)
 {
-	u16 value16 = rtw_read16(Adapter, REG_TRXDMA_CTRL) & 0x7;
+	u16 value16 = rtl8723au_read16(Adapter, REG_TRXDMA_CTRL) & 0x7;
 
 	value16 |= _TXDMA_BEQ_MAP(beQ) | _TXDMA_BKQ_MAP(bkQ) |
 		_TXDMA_VIQ_MAP(viQ) | _TXDMA_VOQ_MAP(voQ) |
 		_TXDMA_MGQ_MAP(mgtQ) | _TXDMA_HIQ_MAP(hiQ);
 
-	rtw_write16(Adapter, REG_TRXDMA_CTRL, value16);
+	rtl8723au_write16(Adapter, REG_TRXDMA_CTRL, value16);
 }
 
 static void _InitNormalChipOneOutEpPriority(struct rtw_adapter *Adapter)
@@ -356,11 +342,11 @@ static void _InitNetworkType(struct rtw_adapter *Adapter)
 {
 	u32 value32;
 
-	value32 = rtw_read32(Adapter, REG_CR);
+	value32 = rtl8723au_read32(Adapter, REG_CR);
 
 	/*  TODO: use the other function to set network type */
 	value32 = (value32 & ~MASK_NETTYPE) | _NETTYPE(NT_LINK_AP);
-	rtw_write32(Adapter, REG_CR, value32);
+	rtl8723au_write32(Adapter, REG_CR, value32);
 }
 
 static void _InitTransferPageSize(struct rtw_adapter *Adapter)
@@ -369,12 +355,12 @@ static void _InitTransferPageSize(struct rtw_adapter *Adapter)
 
 	u8 value8;
 	value8 = _PSRX(PBP_128) | _PSTX(PBP_128);
-	rtw_write8(Adapter, REG_PBP, value8);
+	rtl8723au_write8(Adapter, REG_PBP, value8);
 }
 
 static void _InitDriverInfoSize(struct rtw_adapter *Adapter, u8 drvInfoSize)
 {
-	rtw_write8(Adapter, REG_RX_DRVINFO_SZ, drvInfoSize);
+	rtl8723au_write8(Adapter, REG_RX_DRVINFO_SZ, drvInfoSize);
 }
 
 static void _InitWMACSetting(struct rtw_adapter *Adapter)
@@ -389,15 +375,15 @@ static void _InitWMACSetting(struct rtw_adapter *Adapter)
 
 	/*  some REG_RCR will be modified later by
 	    phy_ConfigMACWithHeaderFile() */
-	rtw_write32(Adapter, REG_RCR, pHalData->ReceiveConfig);
+	rtl8723au_write32(Adapter, REG_RCR, pHalData->ReceiveConfig);
 
 	/*  Accept all multicast address */
-	rtw_write32(Adapter, REG_MAR, 0xFFFFFFFF);
-	rtw_write32(Adapter, REG_MAR + 4, 0xFFFFFFFF);
+	rtl8723au_write32(Adapter, REG_MAR, 0xFFFFFFFF);
+	rtl8723au_write32(Adapter, REG_MAR + 4, 0xFFFFFFFF);
 
 	/*  Accept all data frames */
 	/* value16 = 0xFFFF; */
-	/* rtw_write16(Adapter, REG_RXFLTMAP2, value16); */
+	/* rtl8723au_write16(Adapter, REG_RXFLTMAP2, value16); */
 
 	/*  2010.09.08 hpfan */
 	/*  Since ADF is removed from RCR, ps-poll will not be indicate
@@ -405,14 +391,14 @@ static void _InitWMACSetting(struct rtw_adapter *Adapter)
 	/*  RxFilterMap should mask ps-poll to gurantee AP mode can
 	    rx ps-poll. */
 	/* value16 = 0x400; */
-	/* rtw_write16(Adapter, REG_RXFLTMAP1, value16); */
+	/* rtl8723au_write16(Adapter, REG_RXFLTMAP1, value16); */
 
 	/*  Accept all management frames */
 	/* value16 = 0xFFFF; */
-	/* rtw_write16(Adapter, REG_RXFLTMAP0, value16); */
+	/* rtl8723au_write16(Adapter, REG_RXFLTMAP0, value16); */
 
 	/* enable RX_SHIFT bits */
-	/* rtw_write8(Adapter, REG_TRXDMA_CTRL, rtw_read8(Adapter,
+	/* rtl8723au_write8(Adapter, REG_TRXDMA_CTRL, rtl8723au_read8(Adapter,
 	   REG_TRXDMA_CTRL)|BIT(1)); */
 }
 
@@ -422,49 +408,49 @@ static void _InitAdaptiveCtrl(struct rtw_adapter *Adapter)
 	u32 value32;
 
 	/*  Response Rate Set */
-	value32 = rtw_read32(Adapter, REG_RRSR);
+	value32 = rtl8723au_read32(Adapter, REG_RRSR);
 	value32 &= ~RATE_BITMAP_ALL;
 	value32 |= RATE_RRSR_CCK_ONLY_1M;
-	rtw_write32(Adapter, REG_RRSR, value32);
+	rtl8723au_write32(Adapter, REG_RRSR, value32);
 
 	/*  CF-END Threshold */
-	/* m_spIoBase->rtw_write8(REG_CFEND_TH, 0x1); */
+	/* m_spIoBase->rtl8723au_write8(REG_CFEND_TH, 0x1); */
 
 	/*  SIFS (used in NAV) */
 	value16 = _SPEC_SIFS_CCK(0x10) | _SPEC_SIFS_OFDM(0x10);
-	rtw_write16(Adapter, REG_SPEC_SIFS, value16);
+	rtl8723au_write16(Adapter, REG_SPEC_SIFS, value16);
 
 	/*  Retry Limit */
 	value16 = _LRL(0x30) | _SRL(0x30);
-	rtw_write16(Adapter, REG_RL, value16);
+	rtl8723au_write16(Adapter, REG_RL, value16);
 }
 
 static void _InitRateFallback(struct rtw_adapter *Adapter)
 {
 	/*  Set Data Auto Rate Fallback Retry Count register. */
-	rtw_write32(Adapter, REG_DARFRC, 0x00000000);
-	rtw_write32(Adapter, REG_DARFRC+4, 0x10080404);
-	rtw_write32(Adapter, REG_RARFRC, 0x04030201);
-	rtw_write32(Adapter, REG_RARFRC+4, 0x08070605);
+	rtl8723au_write32(Adapter, REG_DARFRC, 0x00000000);
+	rtl8723au_write32(Adapter, REG_DARFRC+4, 0x10080404);
+	rtl8723au_write32(Adapter, REG_RARFRC, 0x04030201);
+	rtl8723au_write32(Adapter, REG_RARFRC+4, 0x08070605);
 }
 
 static void _InitEDCA(struct rtw_adapter *Adapter)
 {
 	/*  Set Spec SIFS (used in NAV) */
-	rtw_write16(Adapter, REG_SPEC_SIFS, 0x100a);
-	rtw_write16(Adapter, REG_MAC_SPEC_SIFS, 0x100a);
+	rtl8723au_write16(Adapter, REG_SPEC_SIFS, 0x100a);
+	rtl8723au_write16(Adapter, REG_MAC_SPEC_SIFS, 0x100a);
 
 	/*  Set SIFS for CCK */
-	rtw_write16(Adapter, REG_SIFS_CTX, 0x100a);
+	rtl8723au_write16(Adapter, REG_SIFS_CTX, 0x100a);
 
 	/*  Set SIFS for OFDM */
-	rtw_write16(Adapter, REG_SIFS_TRX, 0x100a);
+	rtl8723au_write16(Adapter, REG_SIFS_TRX, 0x100a);
 
 	/*  TXOP */
-	rtw_write32(Adapter, REG_EDCA_BE_PARAM, 0x005EA42B);
-	rtw_write32(Adapter, REG_EDCA_BK_PARAM, 0x0000A44F);
-	rtw_write32(Adapter, REG_EDCA_VI_PARAM, 0x005EA324);
-	rtw_write32(Adapter, REG_EDCA_VO_PARAM, 0x002FA226);
+	rtl8723au_write32(Adapter, REG_EDCA_BE_PARAM, 0x005EA42B);
+	rtl8723au_write32(Adapter, REG_EDCA_BK_PARAM, 0x0000A44F);
+	rtl8723au_write32(Adapter, REG_EDCA_VI_PARAM, 0x005EA324);
+	rtl8723au_write32(Adapter, REG_EDCA_VO_PARAM, 0x002FA226);
 }
 
 static void _InitHWLed(struct rtw_adapter *Adapter)
@@ -481,21 +467,21 @@ static void _InitHWLed(struct rtw_adapter *Adapter)
 
 static void _InitRDGSetting(struct rtw_adapter *Adapter)
 {
-	rtw_write8(Adapter, REG_RD_CTRL, 0xFF);
-	rtw_write16(Adapter, REG_RD_NAV_NXT, 0x200);
-	rtw_write8(Adapter, REG_RD_RESP_PKT_TH, 0x05);
+	rtl8723au_write8(Adapter, REG_RD_CTRL, 0xFF);
+	rtl8723au_write16(Adapter, REG_RD_NAV_NXT, 0x200);
+	rtl8723au_write8(Adapter, REG_RD_RESP_PKT_TH, 0x05);
 }
 
 static void _InitRetryFunction(struct rtw_adapter *Adapter)
 {
 	u8 value8;
 
-	value8 = rtw_read8(Adapter, REG_FWHW_TXQ_CTRL);
+	value8 = rtl8723au_read8(Adapter, REG_FWHW_TXQ_CTRL);
 	value8 |= EN_AMPDU_RTY_NEW;
-	rtw_write8(Adapter, REG_FWHW_TXQ_CTRL, value8);
+	rtl8723au_write8(Adapter, REG_FWHW_TXQ_CTRL, value8);
 
 	/*  Set ACK timeout */
-	rtw_write8(Adapter, REG_ACKTO, 0x40);
+	rtl8723au_write8(Adapter, REG_ACKTO, 0x40);
 }
 
 /*-----------------------------------------------------------------------------
@@ -599,35 +585,33 @@ enum rt_rf_power_state RfOnOffDetect23a(struct rtw_adapter *pAdapter)
 	enum rt_rf_power_state rfpowerstate = rf_off;
 
 	if (pAdapter->pwrctrlpriv.bHWPowerdown) {
-		val8 = rtw_read8(pAdapter, REG_HSISR);
+		val8 = rtl8723au_read8(pAdapter, REG_HSISR);
 		DBG_8723A("pwrdown, 0x5c(BIT7) =%02x\n", val8);
-		rfpowerstate = (val8 & BIT7) ? rf_off : rf_on;
+		rfpowerstate = (val8 & BIT(7)) ? rf_off : rf_on;
 	} else { /*  rf on/off */
-		rtw_write8(pAdapter, REG_MAC_PINMUX_CFG,
-			   rtw_read8(pAdapter, REG_MAC_PINMUX_CFG) & ~BIT3);
-		val8 = rtw_read8(pAdapter, REG_GPIO_IO_SEL);
+		rtl8723au_write8(pAdapter, REG_MAC_PINMUX_CFG,
+				 rtl8723au_read8(pAdapter, REG_MAC_PINMUX_CFG) &
+				 ~BIT(3));
+		val8 = rtl8723au_read8(pAdapter, REG_GPIO_IO_SEL);
 		DBG_8723A("GPIO_IN =%02x\n", val8);
-		rfpowerstate = (val8 & BIT3) ? rf_on : rf_off;
+		rfpowerstate = (val8 & BIT(3)) ? rf_on : rf_off;
 	}
 	return rfpowerstate;
 }	/*  HalDetectPwrDownMode */
 
 void _ps_open_RF23a(struct rtw_adapter *padapter);
 
-static u32 rtl8723au_hal_init(struct rtw_adapter *Adapter)
+static int rtl8723au_hal_init(struct rtw_adapter *Adapter)
 {
-	u8	val8 = 0;
-	u32	boundary, status = _SUCCESS;
+	u8 val8 = 0;
+	u32 boundary;
+	int status = _SUCCESS;
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 	struct pwrctrl_priv *pwrctrlpriv = &Adapter->pwrctrlpriv;
 	struct registry_priv *pregistrypriv = &Adapter->registrypriv;
-	u32 NavUpper = WiFiNavUpperUs;
 
 	unsigned long init_start_time = jiffies;
 
-#define HAL_INIT_PROFILE_TAG(stage) do {} while (0)
-
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_BEGIN);
 	if (Adapter->pwrctrlpriv.bkeepfwalive) {
 		_ps_open_RF23a(Adapter);
 
@@ -644,7 +628,7 @@ static u32 rtl8723au_hal_init(struct rtw_adapter *Adapter)
 	}
 
 	/*  Check if MAC has already power on. by tynli. 2011.05.27. */
-	val8 = rtw_read8(Adapter, REG_CR);
+	val8 = rtl8723au_read8(Adapter, REG_CR);
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
 		 ("%s: REG_CR 0x100 = 0x%02x\n", __func__, val8));
 	/* Fix 92DU-VC S3 hang with the reason is that secondary mac is not
@@ -659,7 +643,6 @@ static u32 rtl8723au_hal_init(struct rtw_adapter *Adapter)
 			 ("%s: MAC has already power on\n", __func__));
 	}
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_INIT_PW_ON);
 	status = _InitPowerOn(Adapter);
 	if (status == _FAIL) {
 		RT_TRACE(_module_hci_hal_init_c_, _drv_err_,
@@ -667,7 +650,6 @@ static u32 rtl8723au_hal_init(struct rtw_adapter *Adapter)
 		goto exit;
 	}
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_INIT_LLTT);
 	if (!pregistrypriv->wifi_spec) {
 		boundary = TX_PAGE_BOUNDARY;
 	} else {
@@ -684,11 +666,9 @@ static u32 rtl8723au_hal_init(struct rtw_adapter *Adapter)
 		}
 	}
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_MISC01);
 	if (pHalData->bRDGEnable)
 		_InitRDGSetting(Adapter);
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_DOWNLOAD_FW);
 	status = rtl8723a_FirmwareDownload(Adapter);
 	if (status != _SUCCESS) {
 		Adapter->bFWReady = false;
@@ -720,14 +700,12 @@ static u32 rtl8723au_hal_init(struct rtw_adapter *Adapter)
 	/*  <Roger_Notes> Current Channel will be updated again later. */
 	pHalData->CurrentChannel = 6;/* default set to 6 */
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_MAC);
 	status = PHY_MACConfig8723A(Adapter);
 	if (status == _FAIL) {
 		DBG_8723A("PHY_MACConfig8723A fault !!\n");
 		goto exit;
 	}
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_BB);
 	/*  */
 	/* d. Initialize BB related configurations. */
 	/*  */
@@ -740,7 +718,6 @@ static u32 rtl8723au_hal_init(struct rtw_adapter *Adapter)
 	/*  Add for tx power by rate fine tune. We need to call the function after BB config. */
 	/*  Because the tx power by rate table is inited in BB config. */
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_RF);
 	status = PHY_RFConfig8723A(Adapter);
 	if (status == _FAIL) {
 		DBG_8723A("PHY_RFConfig8723A fault !!\n");
@@ -766,7 +743,6 @@ static u32 rtl8723au_hal_init(struct rtw_adapter *Adapter)
 	pHalData->RfRegChnlVal[0] = PHY_QueryRFReg(Adapter, (enum RF_RADIO_PATH)0, RF_CHNLBW, bRFRegOffsetMask);
 	pHalData->RfRegChnlVal[1] = PHY_QueryRFReg(Adapter, (enum RF_RADIO_PATH)1, RF_CHNLBW, bRFRegOffsetMask);
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_MISC02);
 	if (!pHalData->bMACFuncEnable) {
 		_InitQueueReservedPage(Adapter);
 		_InitTxBufferBoundary(Adapter);
@@ -779,7 +755,7 @@ static u32 rtl8723au_hal_init(struct rtw_adapter *Adapter)
 	_InitDriverInfoSize(Adapter, DRVINFO_SZ);
 
 	_InitInterrupt(Adapter);
-	hal_init_macaddr23a(Adapter);/* set mac_address */
+	hw_var_set_macaddr(Adapter, Adapter->eeprompriv.mac_addr);
 	_InitNetworkType(Adapter);/* set msr */
 	_InitWMACSetting(Adapter);
 	_InitAdaptiveCtrl(Adapter);
@@ -792,14 +768,11 @@ static u32 rtl8723au_hal_init(struct rtw_adapter *Adapter)
 
 	_InitHWLed(Adapter);
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_TURN_ON_BLOCK);
 	_BBTurnOnBlock(Adapter);
 	/* NicIFSetMacAddress(padapter, padapter->PermanentAddress); */
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_INIT_SECURITY);
 	invalidate_cam_all23a(Adapter);
 
-	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_SetTxPowerLevel8723A(Adapter, pHalData->CurrentChannel);
 
@@ -807,101 +780,89 @@ static u32 rtl8723au_hal_init(struct rtw_adapter *Adapter)
 
 	/*  HW SEQ CTRL */
 	/* set 0x0 to 0xFF by tynli. Default enable HW SEQ NUM. */
-	rtw_write8(Adapter, REG_HWSEQ_CTRL, 0xFF);
+	rtl8723au_write8(Adapter, REG_HWSEQ_CTRL, 0xFF);
 
 	/*  */
 	/*  Disable BAR, suggested by Scott */
 	/*  2010.04.09 add by hpfan */
 	/*  */
-	rtw_write32(Adapter, REG_BAR_MODE_CTRL, 0x0201ffff);
+	rtl8723au_write32(Adapter, REG_BAR_MODE_CTRL, 0x0201ffff);
 
 	if (pregistrypriv->wifi_spec)
-		rtw_write16(Adapter, REG_FAST_EDCA_CTRL, 0);
+		rtl8723au_write16(Adapter, REG_FAST_EDCA_CTRL, 0);
 
 	/*  Move by Neo for USB SS from above setp */
 	_RfPowerSave(Adapter);
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_IQK);
-		/*  2010/08/26 MH Merge from 8192CE. */
-		/* sherry masked that it has been done in _RfPowerSave */
-		/* 20110927 */
-		/* recovery for 8192cu and 9723Au 20111017 */
-		if (pwrctrlpriv->rf_pwrstate == rf_on) {
-			if (pHalData->bIQKInitialized) {
-				rtl8723a_phy_iq_calibrate(Adapter, true);
-			} else {
-				rtl8723a_phy_iq_calibrate(Adapter, false);
-				pHalData->bIQKInitialized = true;
-			}
+	/*  2010/08/26 MH Merge from 8192CE. */
+	/* sherry masked that it has been done in _RfPowerSave */
+	/* 20110927 */
+	/* recovery for 8192cu and 9723Au 20111017 */
+	if (pwrctrlpriv->rf_pwrstate == rf_on) {
+		if (pHalData->bIQKInitialized) {
+			rtl8723a_phy_iq_calibrate(Adapter, true);
+		} else {
+			rtl8723a_phy_iq_calibrate(Adapter, false);
+			pHalData->bIQKInitialized = true;
+		}
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_PW_TRACK);
-			rtl8723a_odm_check_tx_power_tracking(Adapter);
+		rtl8723a_odm_check_tx_power_tracking(Adapter);
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_LCK);
-			rtl8723a_phy_lc_calibrate(Adapter);
+		rtl8723a_phy_lc_calibrate(Adapter);
 
-#ifdef CONFIG_8723AU_BT_COEXIST
-			rtl8723a_SingleDualAntennaDetection(Adapter);
-#endif
-		}
+		rtl8723a_dual_antenna_detection(Adapter);
+	}
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_MISC21);
- /* fixed USB interface interference issue */
-	rtw_write8(Adapter, 0xfe40, 0xe0);
-	rtw_write8(Adapter, 0xfe41, 0x8d);
-	rtw_write8(Adapter, 0xfe42, 0x80);
-	rtw_write32(Adapter, 0x20c, 0xfd0320);
+	/* fixed USB interface interference issue */
+	rtl8723au_write8(Adapter, 0xfe40, 0xe0);
+	rtl8723au_write8(Adapter, 0xfe41, 0x8d);
+	rtl8723au_write8(Adapter, 0xfe42, 0x80);
+	rtl8723au_write32(Adapter, 0x20c, 0xfd0320);
 	/* Solve too many protocol error on USB bus */
 	if (!IS_81xxC_VENDOR_UMC_A_CUT(pHalData->VersionID)) {
 		/*  0xE6 = 0x94 */
-		rtw_write8(Adapter, 0xFE40, 0xE6);
-		rtw_write8(Adapter, 0xFE41, 0x94);
-		rtw_write8(Adapter, 0xFE42, 0x80);
+		rtl8723au_write8(Adapter, 0xFE40, 0xE6);
+		rtl8723au_write8(Adapter, 0xFE41, 0x94);
+		rtl8723au_write8(Adapter, 0xFE42, 0x80);
 
 		/*  0xE0 = 0x19 */
-		rtw_write8(Adapter, 0xFE40, 0xE0);
-		rtw_write8(Adapter, 0xFE41, 0x19);
-		rtw_write8(Adapter, 0xFE42, 0x80);
+		rtl8723au_write8(Adapter, 0xFE40, 0xE0);
+		rtl8723au_write8(Adapter, 0xFE41, 0x19);
+		rtl8723au_write8(Adapter, 0xFE42, 0x80);
 
 		/*  0xE5 = 0x91 */
-		rtw_write8(Adapter, 0xFE40, 0xE5);
-		rtw_write8(Adapter, 0xFE41, 0x91);
-		rtw_write8(Adapter, 0xFE42, 0x80);
+		rtl8723au_write8(Adapter, 0xFE40, 0xE5);
+		rtl8723au_write8(Adapter, 0xFE41, 0x91);
+		rtl8723au_write8(Adapter, 0xFE42, 0x80);
 
 		/*  0xE2 = 0x81 */
-		rtw_write8(Adapter, 0xFE40, 0xE2);
-		rtw_write8(Adapter, 0xFE41, 0x81);
-		rtw_write8(Adapter, 0xFE42, 0x80);
+		rtl8723au_write8(Adapter, 0xFE40, 0xE2);
+		rtl8723au_write8(Adapter, 0xFE41, 0x81);
+		rtl8723au_write8(Adapter, 0xFE42, 0x80);
 
 	}
 
-/* HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_INIT_PABIAS); */
 /*	_InitPABias(Adapter); */
 
-#ifdef CONFIG_8723AU_BT_COEXIST
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_BT_COEXIST);
 	/*  Init BT hw config. */
-	BT_InitHwConfig(Adapter);
-#endif
+	rtl8723a_BT_init_hwconfig(Adapter);
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_INIT_HAL_DM);
 	rtl8723a_InitHalDm(Adapter);
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_MISC31);
-	rtw_hal_set_hwreg23a(Adapter, HW_VAR_NAV_UPPER, (u8 *)&NavUpper);
+	rtl8723a_set_nav_upper(Adapter, WiFiNavUpperUs);
 
 	/*  2011/03/09 MH debug only, UMC-B cut pass 2500 S5 test, but we need to fin root cause. */
-	if (((rtw_read32(Adapter, rFPGA0_RFMOD) & 0xFF000000) != 0x83000000)) {
+	if (((rtl8723au_read32(Adapter, rFPGA0_RFMOD) & 0xFF000000) !=
+	     0x83000000)) {
 		PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT(24), 1);
 		RT_TRACE(_module_hci_hal_init_c_, _drv_err_, ("%s: IQK fail recorver\n", __func__));
 	}
 
 	/* ack for xmit mgmt frames. */
-	rtw_write32(Adapter, REG_FWHW_TXQ_CTRL, rtw_read32(Adapter, REG_FWHW_TXQ_CTRL)|BIT(12));
+	rtl8723au_write32(Adapter, REG_FWHW_TXQ_CTRL,
+			  rtl8723au_read32(Adapter, REG_FWHW_TXQ_CTRL)|BIT(12));
 
 exit:
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_END);
-
 	DBG_8723A("%s in %dms\n", __func__,
 		  jiffies_to_msecs(jiffies - init_start_time));
 	return status;
@@ -923,9 +884,9 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 			   ReadXBYTE(REG_SYS_CLKR+1) | BIT(3)); */
 
 			/*  2. Force PWM, Enable SPS18_LDO_Marco_Block */
-			rtw_write8(Adapter, REG_SPS0_CTRL,
-				   rtw_read8(Adapter, REG_SPS0_CTRL) |
-				   (BIT0|BIT3));
+			rtl8723au_write8(Adapter, REG_SPS0_CTRL,
+					 rtl8723au_read8(Adapter, REG_SPS0_CTRL) |
+					 BIT(0) | BIT(3));
 
 			/*  3. restore BB, AFE control register. */
 			/* RF */
@@ -936,7 +897,7 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 				PHY_SetBBReg(Adapter, rFPGA0_XAB_RFParameter,
 					     0x38, 1);
 			PHY_SetBBReg(Adapter, rOFDM0_TRxPathEnable, 0xf0, 1);
-			PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT1, 0);
+			PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT(1), 0);
 
 			/* AFE */
 			if (pHalData->rf_type ==  RF_2T2R)
@@ -959,17 +920,17 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 		} else {		/*  Level 2 or others. */
 			/* h.	AFE_PLL_CTRL 0x28[7:0] = 0x80
 			   disable AFE PLL */
-			rtw_write8(Adapter, REG_AFE_PLL_CTRL, 0x81);
+			rtl8723au_write8(Adapter, REG_AFE_PLL_CTRL, 0x81);
 
 			/*  i.	AFE_XTAL_CTRL 0x24[15:0] = 0x880F
 			    gated AFE DIG_CLOCK */
-			rtw_write16(Adapter, REG_AFE_XTAL_CTRL, 0x800F);
+			rtl8723au_write16(Adapter, REG_AFE_XTAL_CTRL, 0x800F);
 			mdelay(1);
 
 			/*  2. Force PWM, Enable SPS18_LDO_Marco_Block */
-			rtw_write8(Adapter, REG_SPS0_CTRL,
-				   rtw_read8(Adapter, REG_SPS0_CTRL) |
-				   (BIT0|BIT3));
+			rtl8723au_write8(Adapter, REG_SPS0_CTRL,
+					 rtl8723au_read8(Adapter, REG_SPS0_CTRL) |
+					 BIT(0) | BIT(3));
 
 			/*  3. restore BB, AFE control register. */
 			/* RF */
@@ -980,7 +941,7 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 				PHY_SetBBReg(Adapter, rFPGA0_XAB_RFParameter,
 					     0x38, 1);
 			PHY_SetBBReg(Adapter, rOFDM0_TRxPathEnable, 0xf0, 1);
-			PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT1, 0);
+			PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT(1), 0);
 
 			/* AFE */
 			if (pHalData->rf_type ==  RF_2T2R)
@@ -1002,25 +963,27 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 			}
 
 			/*  5. gated MAC Clock */
-			bytetmp = rtw_read8(Adapter, REG_APSD_CTRL);
-			rtw_write8(Adapter, REG_APSD_CTRL, bytetmp & ~BIT6);
+			bytetmp = rtl8723au_read8(Adapter, REG_APSD_CTRL);
+			rtl8723au_write8(Adapter, REG_APSD_CTRL,
+					 bytetmp & ~BIT(6));
 
 			mdelay(10);
 
 			/*  Set BB reset at first */
-			rtw_write8(Adapter, REG_SYS_FUNC_EN, 0x17); /* 0x16 */
+			/* 0x16 */
+			rtl8723au_write8(Adapter, REG_SYS_FUNC_EN, 0x17);
 
 			/*  Enable TX */
-			rtw_write8(Adapter, REG_TXPAUSE, 0x0);
+			rtl8723au_write8(Adapter, REG_TXPAUSE, 0x0);
 		}
 		break;
 	case rf_sleep:
 	case rf_off:
-		value8 = rtw_read8(Adapter, REG_SPS0_CTRL) ;
+		value8 = rtl8723au_read8(Adapter, REG_SPS0_CTRL) ;
 		if (IS_81xxC_VENDOR_UMC_B_CUT(pHalData->VersionID))
-			value8 &= ~(BIT0);
+			value8 &= ~BIT(0);
 		else
-			value8 &= ~(BIT0|BIT3);
+			value8 &= ~(BIT(0) | BIT(3));
 		if (bRegSSPwrLvl == 1) {
 			RT_TRACE(_module_hal_init_c_, _drv_err_, ("SS LVL1\n"));
 			/*  Disable RF and BB only for SelectSuspend. */
@@ -1050,7 +1013,7 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 					     0x38, 0);
 			}
 			PHY_SetBBReg(Adapter, rOFDM0_TRxPathEnable, 0xf0, 0);
-			PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT1, 1);
+			PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT(1), 1);
 
 			/*  2 .AFE control register to power down. bit[30:22] */
 			Adapter->pwrctrlpriv.PS_BBRegBackup[PSBBREG_AFE0] =
@@ -1070,18 +1033,19 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 					     bRFRegOffsetMask, 0);
 
 			/*  4. Force PFM , disable SPS18_LDO_Marco_Block */
-			rtw_write8(Adapter, REG_SPS0_CTRL, value8);
+			rtl8723au_write8(Adapter, REG_SPS0_CTRL, value8);
 		} else {	/*  Level 2 or others. */
 			RT_TRACE(_module_hal_init_c_, _drv_err_, ("SS LVL2\n"));
 			{
 				u8 eRFPath = RF_PATH_A, value8 = 0;
-				rtw_write8(Adapter, REG_TXPAUSE, 0xFF);
+				rtl8723au_write8(Adapter, REG_TXPAUSE, 0xFF);
 				PHY_SetRFReg(Adapter,
 					     (enum RF_RADIO_PATH)eRFPath,
 					     0x0, bMaskByte0, 0x0);
 				value8 |= APSDOFF;
 				/* 0x40 */
-				rtw_write8(Adapter, REG_APSD_CTRL, value8);
+				rtl8723au_write8(Adapter, REG_APSD_CTRL,
+						 value8);
 
 				/*  After switch APSD, we need to delay
 				    for stability */
@@ -1092,7 +1056,8 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 				value8 |= (FEN_USBD | FEN_USBA |
 					   FEN_BB_GLB_RSTn);
 				/* 0x16 */
-				rtw_write8(Adapter, REG_SYS_FUNC_EN, value8);
+				rtl8723au_write8(Adapter, REG_SYS_FUNC_EN,
+						 value8);
 			}
 
 			/*  Disable RF and BB only for SelectSuspend. */
@@ -1121,7 +1086,7 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 				PHY_SetBBReg(Adapter, rFPGA0_XAB_RFParameter,
 					     0x38, 0);
 			PHY_SetBBReg(Adapter, rOFDM0_TRxPathEnable, 0xf0, 0);
-			PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT1, 1);
+			PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT(1), 1);
 
 			/*  2 .AFE control register to power down. bit[30:22] */
 			Adapter->pwrctrlpriv.PS_BBRegBackup[PSBBREG_AFE0] =
@@ -1141,17 +1106,17 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 					     bRFRegOffsetMask, 0);
 
 			/*  4. Force PFM , disable SPS18_LDO_Marco_Block */
-			rtw_write8(Adapter, REG_SPS0_CTRL, value8);
+			rtl8723au_write8(Adapter, REG_SPS0_CTRL, value8);
 
 			/*  2010/10/13 MH/Isaachsu exchange sequence. */
 			/* h.	AFE_PLL_CTRL 0x28[7:0] = 0x80
 				disable AFE PLL */
-			rtw_write8(Adapter, REG_AFE_PLL_CTRL, 0x80);
+			rtl8723au_write8(Adapter, REG_AFE_PLL_CTRL, 0x80);
 			mdelay(1);
 
 			/*  i.	AFE_XTAL_CTRL 0x24[15:0] = 0x880F
 				gated AFE DIG_CLOCK */
-			rtw_write16(Adapter, REG_AFE_XTAL_CTRL, 0xA80F);
+			rtl8723au_write16(Adapter, REG_AFE_XTAL_CTRL, 0xA80F);
 		}
 		break;
 	default:
@@ -1177,19 +1142,19 @@ static void CardDisableRTL8723U(struct rtw_adapter *Adapter)
 			    PWR_INTF_USB_MSK, rtl8723AU_enter_lps_flow);
 
 	/*  2. 0x1F[7:0] = 0		turn off RF */
-	rtw_write8(Adapter, REG_RF_CTRL, 0x00);
+	rtl8723au_write8(Adapter, REG_RF_CTRL, 0x00);
 
 	/*	==== Reset digital sequence   ====== */
-	if ((rtw_read8(Adapter, REG_MCUFWDL)&BIT7) &&
+	if ((rtl8723au_read8(Adapter, REG_MCUFWDL) & BIT(7)) &&
 	    Adapter->bFWReady) /* 8051 RAM code */
 		rtl8723a_FirmwareSelfReset(Adapter);
 
 	/*  Reset MCU. Suggested by Filen. 2011.01.26. by tynli. */
-	u1bTmp = rtw_read8(Adapter, REG_SYS_FUNC_EN+1);
-	rtw_write8(Adapter, REG_SYS_FUNC_EN+1, (u1bTmp & (~BIT2)));
+	u1bTmp = rtl8723au_read8(Adapter, REG_SYS_FUNC_EN+1);
+	rtl8723au_write8(Adapter, REG_SYS_FUNC_EN+1, u1bTmp & ~BIT(2));
 
 	/*  g.	MCUFWDL 0x80[1:0]= 0		reset MCU ready status */
-	rtw_write8(Adapter, REG_MCUFWDL, 0x00);
+	rtl8723au_write8(Adapter, REG_MCUFWDL, 0x00);
 
 	/*	==== Reset digital sequence end ====== */
 	/*  Card disable power action flow */
@@ -1198,16 +1163,16 @@ static void CardDisableRTL8723U(struct rtw_adapter *Adapter)
 			       rtl8723AU_card_disable_flow);
 
 	/*  Reset MCU IO Wrapper, added by Roger, 2011.08.30. */
-	u1bTmp = rtw_read8(Adapter, REG_RSV_CTRL + 1);
-	rtw_write8(Adapter, REG_RSV_CTRL+1, (u1bTmp & (~BIT0)));
-	u1bTmp = rtw_read8(Adapter, REG_RSV_CTRL + 1);
-	rtw_write8(Adapter, REG_RSV_CTRL+1, u1bTmp | BIT0);
+	u1bTmp = rtl8723au_read8(Adapter, REG_RSV_CTRL + 1);
+	rtl8723au_write8(Adapter, REG_RSV_CTRL+1, u1bTmp & ~BIT(0));
+	u1bTmp = rtl8723au_read8(Adapter, REG_RSV_CTRL + 1);
+	rtl8723au_write8(Adapter, REG_RSV_CTRL+1, u1bTmp | BIT(0));
 
 	/*  7. RSV_CTRL 0x1C[7:0] = 0x0E  lock ISO/CLK/Power control register */
-	rtw_write8(Adapter, REG_RSV_CTRL, 0x0e);
+	rtl8723au_write8(Adapter, REG_RSV_CTRL, 0x0e);
 }
 
-static u32 rtl8723au_hal_deinit(struct rtw_adapter *padapter)
+static int rtl8723au_hal_deinit(struct rtw_adapter *padapter)
 {
 	DBG_8723A("==> %s\n", __func__);
 
@@ -1223,67 +1188,57 @@ static u32 rtl8723au_hal_deinit(struct rtw_adapter *padapter)
 	return _SUCCESS;
 }
 
-static unsigned int rtl8723au_inirp_init(struct rtw_adapter *Adapter)
+int rtl8723au_inirp_init(struct rtw_adapter *Adapter)
 {
 	u8 i;
 	struct recv_buf *precvbuf;
-	uint	status;
-	struct intf_hdl *pintfhdl = &Adapter->iopriv.intf;
+	int status;
 	struct recv_priv *precvpriv = &Adapter->recvpriv;
-	u32 (*_read_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt,
-			  struct recv_buf *rbuf);
-	u32 (*_read_interrupt)(struct intf_hdl *pintfhdl, u32 addr);
-	struct hal_data_8723a	*pHalData = GET_HAL_DATA(Adapter);
-
-	_read_port = pintfhdl->io_ops._read_port;
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 
 	status = _SUCCESS;
 
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("===> usb_inirp_init\n"));
 
-	precvpriv->ff_hwaddr = RECV_BULK_IN_ADDR;
-
 	/* issue Rx irp to receive data */
 	precvbuf = (struct recv_buf *)precvpriv->precv_buf;
 	for (i = 0; i < NR_RECVBUFF; i++) {
-		if (_read_port(pintfhdl, precvpriv->ff_hwaddr, 0, precvbuf) ==
-		    false) {
+		if (rtl8723au_read_port(Adapter, RECV_BULK_IN_ADDR, 0,
+					precvbuf) == _FAIL) {
 			RT_TRACE(_module_hci_hal_init_c_, _drv_err_,
 				 ("usb_rx_init: usb_read_port error\n"));
 			status = _FAIL;
 			goto exit;
 		}
 		precvbuf++;
-		precvpriv->free_recv_buf_queue_cnt--;
 	}
-	_read_interrupt = pintfhdl->io_ops._read_interrupt;
-	if (_read_interrupt(pintfhdl, RECV_INT_IN_ADDR) == false) {
+	if (rtl8723au_read_interrupt(Adapter, RECV_INT_IN_ADDR) == _FAIL) {
 		RT_TRACE(_module_hci_hal_init_c_, _drv_err_,
 			 ("usb_rx_init: usb_read_interrupt error\n"));
 		status = _FAIL;
 	}
-	pHalData->IntrMask[0] = rtw_read32(Adapter, REG_USB_HIMR);
+	pHalData->IntrMask[0] = rtl8723au_read32(Adapter, REG_USB_HIMR);
 	MSG_8723A("pHalData->IntrMask = 0x%04x\n", pHalData->IntrMask[0]);
 	pHalData->IntrMask[0] |= UHIMR_C2HCMD|UHIMR_CPWM;
-	rtw_write32(Adapter, REG_USB_HIMR, pHalData->IntrMask[0]);
+	rtl8723au_write32(Adapter, REG_USB_HIMR, pHalData->IntrMask[0]);
 exit:
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
 		 ("<=== usb_inirp_init\n"));
 	return status;
 }
 
-static unsigned int rtl8723au_inirp_deinit(struct rtw_adapter *Adapter)
+int rtl8723au_inirp_deinit(struct rtw_adapter *Adapter)
 {
 	struct hal_data_8723a	*pHalData = GET_HAL_DATA(Adapter);
 
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
 		 ("\n ===> usb_rx_deinit\n"));
-	rtw_read_port_cancel(Adapter);
-	pHalData->IntrMask[0] = rtw_read32(Adapter, REG_USB_HIMR);
+	rtl8723au_read_port_cancel(Adapter);
+	pHalData->IntrMask[0] = rtl8723au_read32(Adapter, REG_USB_HIMR);
 	MSG_8723A("%s pHalData->IntrMask = 0x%04x\n", __func__,
 		  pHalData->IntrMask[0]);
 	pHalData->IntrMask[0] = 0x0;
-	rtw_write32(Adapter, REG_USB_HIMR, pHalData->IntrMask[0]);
+	rtl8723au_write32(Adapter, REG_USB_HIMR, pHalData->IntrMask[0]);
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
 		 ("\n <=== usb_rx_deinit\n"));
 	return _SUCCESS;
@@ -1420,7 +1375,7 @@ static void _ReadPROMContent(struct rtw_adapter *Adapter)
 	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(Adapter);
 	u8 eeValue;
 
-	eeValue = rtw_read8(Adapter, REG_9346CR);
+	eeValue = rtl8723au_read8(Adapter, REG_9346CR);
 	/*  To check system boot selection. */
 	pEEPROM->EepromOrEfuse = (eeValue & BOOT_FROM_EEPROM) ? true : false;
 	pEEPROM->bautoload_fail_flag = (eeValue & EEPROM_EN) ? false : true;
@@ -1459,16 +1414,18 @@ static void hal_EfuseCellSel(struct rtw_adapter *Adapter)
 {
 	u32 value32;
 
-	value32 = rtw_read32(Adapter, EFUSE_TEST);
+	value32 = rtl8723au_read32(Adapter, EFUSE_TEST);
 	value32 = (value32 & ~EFUSE_SEL_MASK) | EFUSE_SEL(EFUSE_WIFI_SEL_0);
-	rtw_write32(Adapter, EFUSE_TEST, value32);
+	rtl8723au_write32(Adapter, EFUSE_TEST, value32);
 }
 
-static int _ReadAdapterInfo8723AU(struct rtw_adapter *Adapter)
+void rtl8723a_read_adapter_info(struct rtw_adapter *Adapter)
 {
-	/* struct hal_data_8723a	*pHalData = GET_HAL_DATA(Adapter); */
 	unsigned long start = jiffies;
 
+	/*  Read EEPROM size before call any EEPROM function */
+	Adapter->EepromAddressSize = GetEEPROMSize8723A(Adapter);
+
 	MSG_8723A("====> _ReadAdapterInfo8723AU\n");
 
 	hal_EfuseCellSel(Adapter);
@@ -1485,73 +1442,17 @@ static int _ReadAdapterInfo8723AU(struct rtw_adapter *Adapter)
 
 	MSG_8723A("<==== _ReadAdapterInfo8723AU in %d ms\n",
 		  jiffies_to_msecs(jiffies - start));
-
-	return _SUCCESS;
-}
-
-static void ReadAdapterInfo8723AU(struct rtw_adapter *Adapter)
-{
-	/*  Read EEPROM size before call any EEPROM function */
-	Adapter->EepromAddressSize = GetEEPROMSize8723A(Adapter);
-
-	_ReadAdapterInfo8723AU(Adapter);
-}
-
-#define GPIO_DEBUG_PORT_NUM 0
-static void rtl8723au_trigger_gpio_0(struct rtw_adapter *padapter)
-{
-	u32 gpioctrl;
-	DBG_8723A("==> trigger_gpio_0...\n");
-	rtw_write16_async(padapter, REG_GPIO_PIN_CTRL, 0);
-	rtw_write8_async(padapter, REG_GPIO_PIN_CTRL+2, 0xFF);
-	gpioctrl = (BIT(GPIO_DEBUG_PORT_NUM) << 24)|
-		(BIT(GPIO_DEBUG_PORT_NUM) << 16);
-	rtw_write32_async(padapter, REG_GPIO_PIN_CTRL, gpioctrl);
-	gpioctrl |= (BIT(GPIO_DEBUG_PORT_NUM)<<8);
-	rtw_write32_async(padapter, REG_GPIO_PIN_CTRL, gpioctrl);
-	DBG_8723A("<=== trigger_gpio_0...\n");
-}
-
-/*
- * If variable not handled here,
- * some variables will be processed in SetHwReg8723A()
- */
-static void SetHwReg8723AU(struct rtw_adapter *Adapter, u8 variable, u8 *val)
-{
-	switch (variable) {
-	case HW_VAR_RXDMA_AGG_PG_TH:
-		break;
-	case HW_VAR_SET_RPWM:
-		rtl8723a_set_rpwm(Adapter, *val);
-		break;
-	case HW_VAR_TRIGGER_GPIO_0:
-		rtl8723au_trigger_gpio_0(Adapter);
-		break;
-	default:
-		SetHwReg8723A(Adapter, variable, val);
-		break;
-	}
-
-}
-
-/*
- * If variable not handled here,
- * some variables will be processed in GetHwReg8723A()
- */
-static void GetHwReg8723AU(struct rtw_adapter *Adapter, u8 variable, u8 *val)
-{
-	GetHwReg8723A(Adapter, variable, val);
 }
 
 /*  */
 /*	Description: */
 /*		Query setting of specified variable. */
 /*  */
-static u8 GetHalDefVar8192CUsb(struct rtw_adapter *Adapter,
-			       enum hal_def_variable eVariable, void *pValue)
+int GetHalDefVar8192CUsb(struct rtw_adapter *Adapter,
+			 enum hal_def_variable eVariable, void *pValue)
 {
-	struct hal_data_8723a	*pHalData = GET_HAL_DATA(Adapter);
-	u8			bResult = _SUCCESS;
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
+	int bResult = _SUCCESS;
 
 	switch (eVariable) {
 	case HAL_DEF_UNDERCORATEDSMOOTHEDPWDB:
@@ -1596,79 +1497,8 @@ static u8 GetHalDefVar8192CUsb(struct rtw_adapter *Adapter,
 	return bResult;
 }
 
-/*	Change default setting of specified variable. */
-static u8 SetHalDefVar8192CUsb(struct rtw_adapter *Adapter,
-			       enum hal_def_variable eVariable, void *pValue)
-{
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
-	u8 bResult = _SUCCESS;
-
-	switch (eVariable) {
-	case HAL_DEF_DBG_DUMP_RXPKT:
-		pHalData->bDumpRxPkt = *((u8 *)pValue);
-		break;
-	case HAL_DEF_DBG_DM_FUNC:
-	{
-		u8 dm_func = *((u8 *)pValue);
-		struct dm_priv	*pdmpriv = &pHalData->dmpriv;
-		struct dm_odm_t *podmpriv = &pHalData->odmpriv;
-
-		if (dm_func == 0) { /* disable all dynamic func */
-			podmpriv->SupportAbility = DYNAMIC_FUNC_DISABLE;
-			DBG_8723A("==> Disable all dynamic function...\n");
-		} else if (dm_func == 1) {/* disable DIG */
-			podmpriv->SupportAbility &= (~DYNAMIC_BB_DIG);
-			DBG_8723A("==> Disable DIG...\n");
-		} else if (dm_func == 2) {/* disable High power */
-			podmpriv->SupportAbility &= (~DYNAMIC_BB_DYNAMIC_TXPWR);
-		} else if (dm_func == 3) {/* disable tx power tracking */
-			podmpriv->SupportAbility &= (~DYNAMIC_RF_CALIBRATION);
-			DBG_8723A("==> Disable tx power tracking...\n");
-		} else if (dm_func == 4) {/* disable BT coexistence */
-			pdmpriv->DMFlag &= (~DYNAMIC_FUNC_BT);
-		} else if (dm_func == 5) {/* disable antenna diversity */
-			podmpriv->SupportAbility &= (~DYNAMIC_BB_ANT_DIV);
-		} else if (dm_func == 6) {/* turn on all dynamic func */
-			if (!(podmpriv->SupportAbility & DYNAMIC_BB_DIG)) {
-				struct dig_t *pDigTable =
-					&podmpriv->DM_DigTable;
-				pDigTable->CurIGValue = rtw_read8(Adapter, 0xc50);
-			}
-			pdmpriv->DMFlag |= DYNAMIC_FUNC_BT;
-			podmpriv->SupportAbility = DYNAMIC_ALL_FUNC_ENABLE;
-			DBG_8723A("==> Turn on all dynamic function...\n");
-		}
-	}
-		break;
-	case HW_DEF_FA_CNT_DUMP:
-	{
-		u8 bRSSIDump = *((u8 *)pValue);
-		struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
-		if (bRSSIDump)
-			pDM_Odm->DebugComponents = ODM_COMP_DIG|ODM_COMP_FA_CNT;
-		else
-			pDM_Odm->DebugComponents = 0;
-	}
-		break;
-	case HW_DEF_ODM_DBG_FLAG:
-	{
-		u64 DebugComponents = *((u64 *)pValue);
-		struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
-		pDM_Odm->DebugComponents = DebugComponents;
-	}
-		break;
-	default:
-		/* RT_TRACE(COMP_INIT, DBG_TRACE, ("SetHalDefVar819xUsb(): "
-		   "Unkown variable: %d!\n", eVariable)); */
-		bResult = _FAIL;
-		break;
-	}
-
-	return bResult;
-}
-
-static void UpdateHalRAMask8192CUsb(struct rtw_adapter *padapter,
-				    u32 mac_id, u8 rssi_level)
+void rtl8723a_update_ramask(struct rtw_adapter *padapter,
+			    u32 mac_id, u8 rssi_level)
 {
 	u8	init_rate = 0;
 	u8	networkType, raid;
@@ -1741,10 +1571,9 @@ static void UpdateHalRAMask8192CUsb(struct rtw_adapter *padapter,
 	rate_bitmap = 0x0fffffff;
 	rate_bitmap = ODM_Get_Rate_Bitmap23a(&pHalData->odmpriv,
 					  mac_id, mask, rssi_level);
-	printk(KERN_DEBUG "%s => mac_id:%d, networkType:0x%02x, "
-	       "mask:0x%08x\n\t ==> rssi_level:%d, rate_bitmap:0x%08x\n",
-	       __func__,
-	       mac_id, networkType, mask, rssi_level, rate_bitmap);
+	DBG_8723A("%s => mac_id:%d, networkType:0x%02x, "
+		  "mask:0x%08x\n\t ==> rssi_level:%d, rate_bitmap:0x%08x\n",
+		  __func__, mac_id, networkType, mask, rssi_level, rate_bitmap);
 
 	mask &= rate_bitmap;
 	mask |= ((raid<<28)&0xf0000000);
@@ -1770,7 +1599,8 @@ static void UpdateHalRAMask8192CUsb(struct rtw_adapter *padapter,
 		if (shortGIrate == true)
 			init_rate |= BIT(6);
 
-		rtw_write8(padapter, (REG_INIDATA_RATE_SEL+mac_id), init_rate);
+		rtl8723au_write8(padapter, (REG_INIDATA_RATE_SEL+mac_id),
+				 init_rate);
 	}
 
 	/* set ra_id */
@@ -1781,54 +1611,39 @@ static void UpdateHalRAMask8192CUsb(struct rtw_adapter *padapter,
 	pdmpriv->INIDATA_RATE[mac_id] = init_rate;
 }
 
-static void rtl8723au_init_default_value(struct rtw_adapter *padapter)
+int rtw_hal_init23a(struct rtw_adapter *padapter)
 {
-	rtl8723a_init_default_value(padapter);
-}
+	int status;
 
-static u8 rtl8192cu_ps_func(struct rtw_adapter *Adapter,
-			    enum hal_intf_ps_func efunc_id, u8 *val)
-{
-	return true;
+	padapter->hw_init_completed = false;
+
+	status = rtl8723au_hal_init(padapter);
+
+	if (status == _SUCCESS) {
+		padapter->hw_init_completed = true;
+
+		if (padapter->registrypriv.notch_filter == 1)
+			rtl8723a_notch_filter(padapter, 1);
+	} else {
+		padapter->hw_init_completed = false;
+		DBG_8723A("rtw_hal_init23a: hal__init fail\n");
+	}
+
+	RT_TRACE(_module_hal_init_c_, _drv_err_,
+		 ("-rtl871x_hal_init:status = 0x%x\n", status));
+
+	return status;
 }
 
-int rtl8723au_set_hal_ops(struct rtw_adapter *padapter)
+int rtw_hal_deinit23a(struct rtw_adapter *padapter)
 {
-	struct hal_ops	*pHalFunc = &padapter->HalFunc;
+	int status;
 
-	padapter->HalData = kzalloc(sizeof(struct hal_data_8723a), GFP_KERNEL);
-	if (!padapter->HalData) {
-		DBG_8723A("cannot alloc memory for HAL DATA\n");
-		return -ENOMEM;
-	}
-	padapter->hal_data_sz = sizeof(struct hal_data_8723a);
-
-	pHalFunc->hal_init = &rtl8723au_hal_init;
-	pHalFunc->hal_deinit = &rtl8723au_hal_deinit;
-
-	pHalFunc->inirp_init = &rtl8723au_inirp_init;
-	pHalFunc->inirp_deinit = &rtl8723au_inirp_deinit;
-
-	pHalFunc->init_xmit_priv = &rtl8723au_init_xmit_priv;
-	pHalFunc->free_xmit_priv = &rtl8723au_free_xmit_priv;
-
-	pHalFunc->init_recv_priv = &rtl8723au_init_recv_priv;
-	pHalFunc->free_recv_priv = &rtl8723au_free_recv_priv;
-	pHalFunc->InitSwLeds = NULL;
-	pHalFunc->DeInitSwLeds = NULL;
-
-	pHalFunc->init_default_value = &rtl8723au_init_default_value;
-	pHalFunc->intf_chip_configure = &rtl8723au_interface_configure;
-	pHalFunc->read_adapter_info = &ReadAdapterInfo8723AU;
-	pHalFunc->SetHwRegHandler = &SetHwReg8723AU;
-	pHalFunc->GetHwRegHandler = &GetHwReg8723AU;
-	pHalFunc->GetHalDefVarHandler = &GetHalDefVar8192CUsb;
-	pHalFunc->SetHalDefVarHandler = &SetHalDefVar8192CUsb;
-	pHalFunc->UpdateRAMaskHandler = &UpdateHalRAMask8192CUsb;
-	pHalFunc->hal_xmit = &rtl8723au_hal_xmit;
-	pHalFunc->mgnt_xmit = &rtl8723au_mgnt_xmit;
-	pHalFunc->hal_xmitframe_enqueue = &rtl8723au_hal_xmitframe_enqueue;
-	pHalFunc->interface_ps_func = &rtl8192cu_ps_func;
-	rtl8723a_set_hal_ops(pHalFunc);
-	return 0;
+	status = rtl8723au_hal_deinit(padapter);
+
+	if (status == _SUCCESS)
+		padapter->hw_init_completed = false;
+	else
+		DBG_8723A("\n rtw_hal_deinit23a: hal_init fail\n");
+	return status;
 }
diff --git a/drivers/staging/rtl8723au/hal/usb_ops_linux.c b/drivers/staging/rtl8723au/hal/usb_ops_linux.c
index 0311cdf77ff..8e9e61c74bd 100644
--- a/drivers/staging/rtl8723au/hal/usb_ops_linux.c
+++ b/drivers/staging/rtl8723au/hal/usb_ops_linux.c
@@ -22,9 +22,10 @@
 #include <rtl8723a_hal.h>
 #include <rtl8723a_recv.h>
 
-static int usbctrl_vendorreq(struct intf_hdl *pintfhdl, u8 request, u16 value, u16 index, void *pdata, u16 len, u8 requesttype)
+static int usbctrl_vendorreq(struct rtw_adapter *padapter, u8 request,
+			     u16 value, u16 index, void *pdata, u16 len,
+			     u8 requesttype)
 {
-	struct rtw_adapter		*padapter = pintfhdl->padapter ;
 	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter);
 	struct usb_device *udev = pdvobjpriv->pusbdev;
 
@@ -34,15 +35,17 @@ static int usbctrl_vendorreq(struct intf_hdl *pintfhdl, u8 request, u16 value, u
 	u8 *pIo_buf;
 	int vendorreq_times = 0;
 
-	if ((padapter->bSurpriseRemoved) || (padapter->pwrctrlpriv.pnp_bstop_trx)) {
+	if (padapter->bSurpriseRemoved || padapter->pwrctrlpriv.pnp_bstop_trx) {
 		RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-			 ("usbctrl_vendorreq:(padapter->bSurpriseRemoved||adapter->pwrctrlpriv.pnp_bstop_trx)!!!\n"));
+			 ("usbctrl_vendorreq:(padapter->bSurpriseRemoved||"
+			  "adapter->pwrctrlpriv.pnp_bstop_trx)!!!\n"));
 		status = -EPERM;
 		goto exit;
 	}
 
 	if (len > MAX_VENDOR_REQ_CMD_SIZE) {
-		DBG_8723A("[%s] Buffer len error , vendor request failed\n", __FUNCTION__);
+		DBG_8723A("[%s] Buffer len error , vendor request failed\n",
+			  __func__);
 		status = -EINVAL;
 		goto exit;
 	}
@@ -53,7 +56,7 @@ static int usbctrl_vendorreq(struct intf_hdl *pintfhdl, u8 request, u16 value, u
 	pIo_buf = pdvobjpriv->usb_vendor_req_buf;
 
 	if (pIo_buf == NULL) {
-		DBG_8723A("[%s] pIo_buf == NULL \n", __FUNCTION__);
+		DBG_8723A("[%s] pIo_buf == NULL \n", __func__);
 		status = -ENOMEM;
 		goto release_mutex;
 	}
@@ -70,9 +73,9 @@ static int usbctrl_vendorreq(struct intf_hdl *pintfhdl, u8 request, u16 value, u
 			memcpy(pIo_buf, pdata, len);
 		}
 
-		status = rtw_usb_control_msg(udev, pipe, request, reqtype,
-					     value, index, pIo_buf, len,
-					     RTW_USB_CONTROL_MSG_TIMEOUT);
+		status = usb_control_msg(udev, pipe, request, reqtype,
+					 value, index, pIo_buf, len,
+					 RTW_USB_CONTROL_MSG_TIMEOUT);
 
 		if (status == len) {   /*  Success this control transfer. */
 			rtw_reset_continual_urb_error(pdvobjpriv);
@@ -85,21 +88,26 @@ static int usbctrl_vendorreq(struct intf_hdl *pintfhdl, u8 request, u16 value, u
 		} else { /*  error cases */
 			DBG_8723A("reg 0x%x, usb %s %u fail, status:%d value ="
 				  " 0x%x, vendorreq_times:%d\n",
-				  value, (requesttype == 0x01) ? "read" : "write",
+				  value, (requesttype == 0x01) ?
+				  "read" : "write",
 				  len, status, *(u32 *)pdata, vendorreq_times);
 
 			if (status < 0) {
-				if (status == (-ESHUTDOWN) || status == -ENODEV) {
+				if (status == -ESHUTDOWN || status == -ENODEV)
 					padapter->bSurpriseRemoved = true;
-				} else {
-					struct hal_data_8723a	*pHalData = GET_HAL_DATA(padapter);
-					pHalData->srestpriv.Wifi_Error_Status = USB_VEN_REQ_CMD_FAIL;
+				else {
+					struct hal_data_8723a *pHalData;
+					pHalData = GET_HAL_DATA(padapter);
+					pHalData->srestpriv.Wifi_Error_Status =
+						USB_VEN_REQ_CMD_FAIL;
 				}
 			} else { /*  status != len && status >= 0 */
 				if (status > 0) {
 					if (requesttype == 0x01) {
-						/*  For Control read transfer, we have to copy
-						 * the read data from pIo_buf to pdata.
+						/*
+						 * For Control read transfer,
+						 * we have to copy the read
+						 * data from pIo_buf to pdata.
 						 */
 						memcpy(pdata, pIo_buf,  len);
 					}
@@ -110,11 +118,11 @@ static int usbctrl_vendorreq(struct intf_hdl *pintfhdl, u8 request, u16 value, u
 				padapter->bSurpriseRemoved = true;
 				break;
 			}
-
 		}
 
 		/*  firmware download is checksumed, don't retry */
-		if ((value >= FW_8723A_START_ADDRESS && value <= FW_8723A_END_ADDRESS) || status == len)
+		if ((value >= FW_8723A_START_ADDRESS &&
+		     value <= FW_8723A_END_ADDRESS) || status == len)
 			break;
 	}
 
@@ -124,7 +132,7 @@ exit:
 	return status;
 }
 
-static u8 usb_read8(struct intf_hdl *pintfhdl, u32 addr)
+u8 rtl8723au_read8(struct rtw_adapter *padapter, u32 addr)
 {
 	u8 request;
 	u8 requesttype;
@@ -140,19 +148,20 @@ static u8 usb_read8(struct intf_hdl *pintfhdl, u32 addr)
 	wvalue = (u16)(addr&0x0000ffff);
 	len = 1;
 
-	usbctrl_vendorreq(pintfhdl, request, wvalue, index, &data, len, requesttype);
+	usbctrl_vendorreq(padapter, request, wvalue, index, &data,
+			  len, requesttype);
 
 	return data;
 }
 
-static u16 usb_read16(struct intf_hdl *pintfhdl, u32 addr)
+u16 rtl8723au_read16(struct rtw_adapter *padapter, u32 addr)
 {
 	u8 request;
 	u8 requesttype;
 	u16 wvalue;
 	u16 index;
 	u16 len;
-	u16 data = 0;
+	__le16 data;
 
 	request = 0x05;
 	requesttype = 0x01;/* read_in */
@@ -161,19 +170,20 @@ static u16 usb_read16(struct intf_hdl *pintfhdl, u32 addr)
 	wvalue = (u16)(addr&0x0000ffff);
 	len = 2;
 
-	usbctrl_vendorreq(pintfhdl, request, wvalue, index, &data, len, requesttype);
+	usbctrl_vendorreq(padapter, request, wvalue, index, &data,
+			  len, requesttype);
 
-	return data;
+	return le16_to_cpu(data);
 }
 
-static u32 usb_read32(struct intf_hdl *pintfhdl, u32 addr)
+u32 rtl8723au_read32(struct rtw_adapter *padapter, u32 addr)
 {
 	u8 request;
 	u8 requesttype;
 	u16 wvalue;
 	u16 index;
 	u16 len;
-	u32 data = 0;
+	__le32 data;
 
 	request = 0x05;
 	requesttype = 0x01;/* read_in */
@@ -182,12 +192,13 @@ static u32 usb_read32(struct intf_hdl *pintfhdl, u32 addr)
 	wvalue = (u16)(addr&0x0000ffff);
 	len = 4;
 
-	usbctrl_vendorreq(pintfhdl, request, wvalue, index, &data, len, requesttype);
+	usbctrl_vendorreq(padapter, request, wvalue, index, &data,
+			  len, requesttype);
 
-	return data;
+	return le32_to_cpu(data);
 }
 
-static int usb_write8(struct intf_hdl *pintfhdl, u32 addr, u8 val)
+int rtl8723au_write8(struct rtw_adapter *padapter, u32 addr, u8 val)
 {
 	u8 request;
 	u8 requesttype;
@@ -206,19 +217,20 @@ static int usb_write8(struct intf_hdl *pintfhdl, u32 addr, u8 val)
 
 	data = val;
 
-	ret = usbctrl_vendorreq(pintfhdl, request, wvalue, index, &data, len, requesttype);
+	ret = usbctrl_vendorreq(padapter, request, wvalue, index, &data,
+				len, requesttype);
 
 	return ret;
 }
 
-static int usb_write16(struct intf_hdl *pintfhdl, u32 addr, u16 val)
+int rtl8723au_write16(struct rtw_adapter *padapter, u32 addr, u16 val)
 {
 	u8 request;
 	u8 requesttype;
 	u16 wvalue;
 	u16 index;
 	u16 len;
-	u16 data;
+	__le16 data;
 	int ret;
 
 	request = 0x05;
@@ -228,20 +240,21 @@ static int usb_write16(struct intf_hdl *pintfhdl, u32 addr, u16 val)
 	wvalue = (u16)(addr&0x0000ffff);
 	len = 2;
 
-	data = val;
+	data = cpu_to_le16(val);
 
-	ret = usbctrl_vendorreq(pintfhdl, request, wvalue, index, &data, len, requesttype);
+	ret = usbctrl_vendorreq(padapter, request, wvalue, index, &data,
+				len, requesttype);
 	return ret;
 }
 
-static int usb_write32(struct intf_hdl *pintfhdl, u32 addr, u32 val)
+int rtl8723au_write32(struct rtw_adapter *padapter, u32 addr, u32 val)
 {
 	u8 request;
 	u8 requesttype;
 	u16 wvalue;
 	u16 index;
 	u16 len;
-	u32 data;
+	__le32 data;
 	int ret;
 
 	request = 0x05;
@@ -250,14 +263,16 @@ static int usb_write32(struct intf_hdl *pintfhdl, u32 addr, u32 val)
 
 	wvalue = (u16)(addr&0x0000ffff);
 	len = 4;
-	data = val;
+	data = cpu_to_le32(val);
 
-	ret = usbctrl_vendorreq(pintfhdl, request, wvalue, index, &data, len, requesttype);
+	ret = usbctrl_vendorreq(padapter, request, wvalue, index, &data,
+				len, requesttype);
 
 	return ret;
 }
 
-static int usb_writeN(struct intf_hdl *pintfhdl, u32 addr, u32 length, u8 *pdata)
+int rtl8723au_writeN(struct rtw_adapter *padapter,
+		     u32 addr, u32 length, u8 *pdata)
 {
 	u8 request;
 	u8 requesttype;
@@ -273,9 +288,10 @@ static int usb_writeN(struct intf_hdl *pintfhdl, u32 addr, u32 length, u8 *pdata
 
 	wvalue = (u16)(addr&0x0000ffff);
 	len = length;
-	 memcpy(buf, pdata, len);
+	memcpy(buf, pdata, len);
 
-	ret = usbctrl_vendorreq(pintfhdl, request, wvalue, index, buf, len, requesttype);
+	ret = usbctrl_vendorreq(padapter, request, wvalue, index, buf,
+				len, requesttype);
 
 	return ret;
 }
@@ -323,11 +339,11 @@ InterruptRecognized8723AU(struct rtw_adapter *Adapter, void *pContent,
 
 	memcpy(&report.state, &buffer[USB_INTR_CPWM_OFFSET], 1);
 
-	return ((pHalData->IntArray[0])&pHalData->IntrMask[0]) != 0 ||
-		((pHalData->IntArray[1])&pHalData->IntrMask[1]) != 0;
+	return (pHalData->IntArray[0] & pHalData->IntrMask[0]) != 0 ||
+		(pHalData->IntArray[1] & pHalData->IntrMask[1]) != 0;
 }
 
-static void usb_read_interrupt_complete(struct urb *purb, struct pt_regs *regs)
+static void usb_read_interrupt_complete(struct urb *purb)
 {
 	int err;
 	struct rtw_adapter *padapter = (struct rtw_adapter *)purb->context;
@@ -336,7 +352,7 @@ static void usb_read_interrupt_complete(struct urb *purb, struct pt_regs *regs)
 	    padapter->bReadPortCancel) {
 		DBG_8723A("%s() RX Warning! bDriverStopped(%d) OR "
 			  "bSurpriseRemoved(%d) bReadPortCancel(%d)\n",
-			  __FUNCTION__, padapter->bDriverStopped,
+			  __func__, padapter->bDriverStopped,
 			  padapter->bSurpriseRemoved,
 			  padapter->bReadPortCancel);
 		return;
@@ -359,30 +375,37 @@ static void usb_read_interrupt_complete(struct urb *purb, struct pt_regs *regs)
 		if (c2h_evt_exist(c2h_evt)) {
 			if (c2h_id_filter_ccx_8723a(c2h_evt->id)) {
 				/* Handle CCX report here */
-				handle_txrpt_ccx_8723a(padapter, (void *)(c2h_evt->payload));
-				/* Replace with special pointer to
-				   trigger c2h_evt_clear23a */
-				if (rtw_cbuf_push23a(padapter->evtpriv.c2h_queue,
-						  (void *)&padapter->evtpriv) !=
-				    _SUCCESS)
-					DBG_8723A("%s rtw_cbuf_push23a fail\n",
-						  __func__);
-				schedule_work(&padapter->evtpriv.c2h_wk);
-			} else if ((c2h_evt = (struct c2h_evt_hdr *)
-				    kmalloc(16, GFP_ATOMIC))) {
-				memcpy(c2h_evt, purb->transfer_buffer, 16);
-				if (rtw_cbuf_push23a(padapter->evtpriv.c2h_queue,
-						  (void *)c2h_evt) != _SUCCESS)
-					DBG_8723A("%s rtw_cbuf_push23a fail\n",
-						  __func__);
-				schedule_work(&padapter->evtpriv.c2h_wk);
+				handle_txrpt_ccx_8723a(padapter, (void *)
+						       c2h_evt->payload);
+				schedule_work(&padapter->evtpriv.irq_wk);
 			} else {
-				/* Error handling for malloc fail */
-				if (rtw_cbuf_push23a(padapter->evtpriv.c2h_queue,
-						  (void *)NULL) != _SUCCESS)
-					DBG_8723A("%s rtw_cbuf_push23a fail\n",
-						  __func__);
-				schedule_work(&padapter->evtpriv.c2h_wk);
+				struct evt_work *c2w;
+				int res;
+
+				c2w = kmalloc(sizeof(struct evt_work),
+						GFP_ATOMIC);
+
+				if (!c2w) {
+					printk(KERN_WARNING "%s: unable to "
+					       "allocate work buffer\n",
+					       __func__);
+					goto urb_submit;
+				}
+
+				c2w->adapter = padapter;
+				INIT_WORK(&c2w->work, rtw_evt_work);
+				memcpy(c2w->u.buf, purb->transfer_buffer, 16);
+
+				res = queue_work(padapter->evtpriv.wq,
+						 &c2w->work);
+
+				if (!res) {
+					printk(KERN_ERR "%s: Call to "
+					       "queue_work() failed\n",
+					       __func__);
+					kfree(c2w);
+					goto urb_submit;
+				}
 			}
 		}
 
@@ -415,7 +438,7 @@ urb_submit:
 		case -EPROTO:
 			break;
 		case -EINPROGRESS:
-			DBG_8723A("ERROR: URB IS IN PROGRESS!/n");
+			DBG_8723A("ERROR: URB IS IN PROGRESS!\n");
 			break;
 		default:
 			break;
@@ -423,18 +446,17 @@ urb_submit:
 	}
 }
 
-static u32 usb_read_interrupt(struct intf_hdl *pintfhdl, u32 addr)
+int rtl8723au_read_interrupt(struct rtw_adapter *adapter, u32 addr)
 {
 	int err;
 	unsigned int pipe;
-	u32 ret = _SUCCESS;
-	struct rtw_adapter *adapter = pintfhdl->padapter;
+	int ret = _SUCCESS;
 	struct dvobj_priv *pdvobj = adapter_to_dvobj(adapter);
 	struct recv_priv *precvpriv = &adapter->recvpriv;
 	struct usb_device *pusbd = pdvobj->pusbdev;
 
 	/* translate DMA FIFO addr to pipehandle */
-	pipe = ffaddr2pipehdl23a(pdvobj, addr);
+	pipe = usb_rcvintpipe(pusbd, pdvobj->RtInPipe[1]);
 
 	usb_fill_int_urb(precvpriv->int_in_urb, pusbd, pipe,
 			 precvpriv->int_in_buf, USB_INTR_CONTENT_LENGTH,
@@ -457,16 +479,16 @@ static int recvbuf2recvframe(struct rtw_adapter *padapter, struct sk_buff *pskb)
 	u8	shift_sz = 0;
 	u16	pkt_cnt;
 	u32	pkt_offset, skb_len, alloc_sz;
-	s32	transfer_len;
-	struct recv_stat	*prxstat;
-	struct phy_stat	*pphy_info = NULL;
-	struct sk_buff		*pkt_copy = NULL;
-	struct recv_frame	*precvframe = NULL;
-	struct rx_pkt_attrib	*pattrib = NULL;
-	struct recv_priv	*precvpriv = &padapter->recvpriv;
-	struct rtw_queue	*pfree_recv_queue = &precvpriv->free_recv_queue;
-
-	transfer_len = (s32)pskb->len;
+	int	transfer_len;
+	struct recv_stat *prxstat;
+	struct phy_stat	*pphy_info;
+	struct sk_buff *pkt_copy;
+	struct recv_frame *precvframe;
+	struct rx_pkt_attrib *pattrib;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+	struct rtw_queue *pfree_recv_queue = &precvpriv->free_recv_queue;
+
+	transfer_len = (int)pskb->len;
 	pbuf = pskb->data;
 
 	prxstat = (struct recv_stat *)pbuf;
@@ -485,7 +507,7 @@ static int recvbuf2recvframe(struct rtw_adapter *padapter, struct sk_buff *pskb)
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
 				 ("recvbuf2recvframe: precvframe == NULL\n"));
 			DBG_8723A("%s()-%d: rtw_alloc_recvframe23a() failed! RX "
-				  "Drop!\n", __FUNCTION__, __LINE__);
+				  "Drop!\n", __func__, __LINE__);
 			goto _exit_recvbuf2recvframe;
 		}
 
@@ -497,31 +519,30 @@ static int recvbuf2recvframe(struct rtw_adapter *padapter, struct sk_buff *pskb)
 
 		if (pattrib->crc_err) {
 			DBG_8723A("%s()-%d: RX Warning! rx CRC ERROR !!\n",
-				  __FUNCTION__, __LINE__);
-			rtw_free_recvframe23a(precvframe, pfree_recv_queue);
+				  __func__, __LINE__);
+			rtw_free_recvframe23a(precvframe);
 			goto _exit_recvbuf2recvframe;
 		}
 
 		pkt_offset = RXDESC_SIZE + pattrib->drvinfo_sz +
 			pattrib->shift_sz + pattrib->pkt_len;
 
-		if ((pattrib->pkt_len <= 0) || (pkt_offset > transfer_len)) {
+		if (pattrib->pkt_len <= 0 || pkt_offset > transfer_len) {
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
 				 ("recvbuf2recvframe: pkt_len<= 0\n"));
 			DBG_8723A("%s()-%d: RX Warning!\n",
-				  __FUNCTION__, __LINE__);
-			rtw_free_recvframe23a(precvframe, pfree_recv_queue);
+				  __func__, __LINE__);
+			rtw_free_recvframe23a(precvframe);
 			goto _exit_recvbuf2recvframe;
 		}
 
 		/*	Modified by Albert 20101213 */
 		/*	For 8 bytes IP header alignment. */
 		/*	Qos data, wireless lan header length is 26 */
-		if (pattrib->qos) {
+		if (pattrib->qos)
 			shift_sz = 6;
-		} else {
+		else
 			shift_sz = 0;
-		}
 
 		skb_len = pattrib->pkt_len;
 
@@ -529,7 +550,7 @@ static int recvbuf2recvframe(struct rtw_adapter *padapter, struct sk_buff *pskb)
 		 * 1536+drvinfo_sz+RXDESC_SIZE to defrag packet.
 		 * modify alloc_sz for recvive crc error packet
 		 * by thomas 2011-06-02 */
-		if ((pattrib->mfrag == 1) && (pattrib->frag_num == 0)) {
+		if (pattrib->mfrag == 1 && pattrib->frag_num == 0) {
 			/* alloc_sz = 1664;	1664 is 128 alignment. */
 			if (skb_len <= 1650)
 				alloc_sz = 1664;
@@ -546,17 +567,20 @@ static int recvbuf2recvframe(struct rtw_adapter *padapter, struct sk_buff *pskb)
 		if (pkt_copy) {
 			pkt_copy->dev = padapter->pnetdev;
 			precvframe->pkt = pkt_copy;
-			skb_reserve(pkt_copy, 8 - ((unsigned long)(pkt_copy->data) & 7));/* force pkt_copy->data at 8-byte alignment address */
-	/*force ip_hdr at 8-byte alignment address according to shift_sz. */
+			/* force pkt_copy->data at 8-byte alignment address */
+			skb_reserve(pkt_copy, 8 -
+				    ((unsigned long)(pkt_copy->data) & 7));
+			/*force ip_hdr at 8-byte alignment address
+			  according to shift_sz. */
 			skb_reserve(pkt_copy, shift_sz);
-			memcpy(pkt_copy->data, (pbuf + pattrib->shift_sz + pattrib->drvinfo_sz + RXDESC_SIZE), skb_len);
+			memcpy(pkt_copy->data, pbuf + pattrib->shift_sz +
+			       pattrib->drvinfo_sz + RXDESC_SIZE, skb_len);
 			skb_put(pkt_copy, skb_len);
 		} else {
-			if ((pattrib->mfrag == 1) && (pattrib->frag_num == 0)) {
+			if (pattrib->mfrag == 1 && pattrib->frag_num == 0) {
 				DBG_8723A("recvbuf2recvframe: alloc_skb fail, "
 					  "drop frag frame \n");
-				rtw_free_recvframe23a(precvframe,
-						   pfree_recv_queue);
+				rtw_free_recvframe23a(precvframe);
 				goto _exit_recvbuf2recvframe;
 			}
 
@@ -564,8 +588,7 @@ static int recvbuf2recvframe(struct rtw_adapter *padapter, struct sk_buff *pskb)
 			if (!precvframe->pkt) {
 				DBG_8723A("recvbuf2recvframe: skb_clone "
 					  "fail\n");
-				rtw_free_recvframe23a(precvframe,
-						   pfree_recv_queue);
+				rtw_free_recvframe23a(precvframe);
 				goto _exit_recvbuf2recvframe;
 			}
 		}
@@ -589,7 +612,7 @@ static int recvbuf2recvframe(struct rtw_adapter *padapter, struct sk_buff *pskb)
 		if (transfer_len > 0 && pkt_cnt == 0)
 			pkt_cnt = (le32_to_cpu(prxstat->rxdw2)>>16) & 0xff;
 
-	} while ((transfer_len > 0) && (pkt_cnt > 0));
+	} while (transfer_len > 0 && pkt_cnt > 0);
 
 _exit_recvbuf2recvframe:
 
@@ -603,8 +626,7 @@ void rtl8723au_recv_tasklet(void *priv)
 	struct recv_priv *precvpriv = &padapter->recvpriv;
 
 	while (NULL != (pskb = skb_dequeue(&precvpriv->rx_skb_queue))) {
-		if ((padapter->bDriverStopped) ||
-		    (padapter->bSurpriseRemoved)) {
+		if (padapter->bDriverStopped || padapter->bSurpriseRemoved) {
 			DBG_8723A("recv_tasklet => bDriverStopped or "
 				  "bSurpriseRemoved \n");
 			dev_kfree_skb_any(pskb);
@@ -620,7 +642,7 @@ void rtl8723au_recv_tasklet(void *priv)
 	}
 }
 
-static void usb_read_port_complete(struct urb *purb, struct pt_regs *regs)
+static void usb_read_port_complete(struct urb *purb)
 {
 	struct recv_buf *precvbuf = (struct recv_buf *)purb->context;
 	struct rtw_adapter *padapter = (struct rtw_adapter *)precvbuf->adapter;
@@ -641,22 +663,22 @@ static void usb_read_port_complete(struct urb *purb, struct pt_regs *regs)
 
 		DBG_8723A("%s()-%d: RX Warning! bDriverStopped(%d) OR "
 			  "bSurpriseRemoved(%d) bReadPortCancel(%d)\n",
-			  __FUNCTION__, __LINE__, padapter->bDriverStopped,
+			  __func__, __LINE__, padapter->bDriverStopped,
 			  padapter->bSurpriseRemoved, padapter->bReadPortCancel);
 		return;
 	}
 
 	if (purb->status == 0) {
-		if ((purb->actual_length > MAX_RECVBUF_SZ) ||
-		    (purb->actual_length < RXDESC_SIZE)) {
+		if (purb->actual_length > MAX_RECVBUF_SZ ||
+		    purb->actual_length < RXDESC_SIZE) {
 			RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
 				 ("usb_read_port_complete: (purb->actual_"
 				  "length > MAX_RECVBUF_SZ) || (purb->actual_"
 				  "length < RXDESC_SIZE)\n"));
-			rtw_read_port(padapter, precvpriv->ff_hwaddr, 0,
-				      precvbuf);
+			rtl8723au_read_port(padapter, RECV_BULK_IN_ADDR, 0,
+					    precvbuf);
 			DBG_8723A("%s()-%d: RX Warning!\n",
-				  __FUNCTION__, __LINE__);
+				  __func__, __LINE__);
 		} else {
 			rtw_reset_continual_urb_error(
 				adapter_to_dvobj(padapter));
@@ -669,8 +691,8 @@ static void usb_read_port_complete(struct urb *purb, struct pt_regs *regs)
 				tasklet_schedule(&precvpriv->recv_tasklet);
 
 			precvbuf->pskb = NULL;
-			rtw_read_port(padapter, precvpriv->ff_hwaddr, 0,
-				      precvbuf);
+			rtl8723au_read_port(padapter, RECV_BULK_IN_ADDR, 0,
+					    precvbuf);
 		}
 	} else {
 		RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
@@ -707,32 +729,30 @@ static void usb_read_port_complete(struct urb *purb, struct pt_regs *regs)
 			pHalData = GET_HAL_DATA(padapter);
 			pHalData->srestpriv.Wifi_Error_Status =
 				USB_READ_PORT_FAIL;
-			rtw_read_port(padapter, precvpriv->ff_hwaddr,
-				      0, precvbuf);
+			rtl8723au_read_port(padapter, RECV_BULK_IN_ADDR, 0,
+					    precvbuf);
 			break;
 		case -EINPROGRESS:
-			DBG_8723A("ERROR: URB IS IN PROGRESS!/n");
+			DBG_8723A("ERROR: URB IS IN PROGRESS!\n");
 			break;
 		default:
 			break;
 		}
-
 	}
 }
 
-static u32 usb_read_port(struct intf_hdl *pintfhdl, u32 addr, u32 cnt,
-			 struct recv_buf *precvbuf)
+int rtl8723au_read_port(struct rtw_adapter *adapter, u32 addr, u32 cnt,
+			struct recv_buf *precvbuf)
 {
 	int err;
 	unsigned int pipe;
-	unsigned long tmpaddr = 0;
-	unsigned long alignment = 0;
-	u32 ret = _SUCCESS;
-	struct urb *purb = NULL;
-	struct rtw_adapter		*adapter = pintfhdl->padapter;
-	struct dvobj_priv	*pdvobj = adapter_to_dvobj(adapter);
-	struct recv_priv	*precvpriv = &adapter->recvpriv;
-	struct usb_device	*pusbd = pdvobj->pusbdev;
+	unsigned long tmpaddr;
+	unsigned long alignment;
+	int ret = _SUCCESS;
+	struct urb *purb;
+	struct dvobj_priv *pdvobj = adapter_to_dvobj(adapter);
+	struct recv_priv *precvpriv = &adapter->recvpriv;
+	struct usb_device *pusbd = pdvobj->pusbdev;
 
 	if (adapter->bDriverStopped || adapter->bSurpriseRemoved ||
 	    adapter->pwrctrlpriv.pnp_bstop_trx) {
@@ -752,8 +772,6 @@ static u32 usb_read_port(struct intf_hdl *pintfhdl, u32 addr, u32 cnt,
 	if (!precvbuf->pskb)
 		precvbuf->pskb = skb_dequeue(&precvpriv->free_recv_skb_queue);
 
-	rtl8723au_init_recvbuf(adapter, precvbuf);
-
 	/* re-assign for linux based on skb */
 	if (!precvbuf->pskb) {
 		precvbuf->pskb = netdev_alloc_skb(adapter->pnetdev, MAX_RECVBUF_SZ + RECVBUFF_ALIGN_SZ);
@@ -772,7 +790,7 @@ static u32 usb_read_port(struct intf_hdl *pintfhdl, u32 addr, u32 cnt,
 	purb = precvbuf->purb;
 
 	/* translate DMA FIFO addr to pipehandle */
-	pipe = ffaddr2pipehdl23a(pdvobj, addr);
+	pipe = usb_rcvbulkpipe(pusbd, pdvobj->RtInPipe[0]);
 
 	usb_fill_bulk_urb(purb, pusbd, pipe, precvbuf->pskb->data,
 			  MAX_RECVBUF_SZ, usb_read_port_complete,
@@ -792,7 +810,7 @@ static u32 usb_read_port(struct intf_hdl *pintfhdl, u32 addr, u32 cnt,
 
 void rtl8723au_xmit_tasklet(void *priv)
 {
-	int ret = false;
+	int ret;
 	struct rtw_adapter *padapter = (struct rtw_adapter *)priv;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 
@@ -800,9 +818,8 @@ void rtl8723au_xmit_tasklet(void *priv)
 		return;
 
 	while (1) {
-		if ((padapter->bDriverStopped) ||
-		    (padapter->bSurpriseRemoved) ||
-		    (padapter->bWritePortCancel)) {
+		if (padapter->bDriverStopped || padapter->bSurpriseRemoved ||
+		    padapter->bWritePortCancel) {
 			DBG_8723A("xmit_tasklet => bDriverStopped or "
 				  "bSurpriseRemoved or bWritePortCancel\n");
 			break;
@@ -815,31 +832,6 @@ void rtl8723au_xmit_tasklet(void *priv)
 	}
 }
 
-void rtl8723au_set_intf_ops(struct _io_ops *pops)
-{
-
-	memset((u8 *)pops, 0, sizeof(struct _io_ops));
-
-	pops->_read8 = &usb_read8;
-	pops->_read16 = &usb_read16;
-	pops->_read32 = &usb_read32;
-	pops->_read_mem = &usb_read_mem23a;
-	pops->_read_port = &usb_read_port;
-
-	pops->_write8 = &usb_write8;
-	pops->_write16 = &usb_write16;
-	pops->_write32 = &usb_write32;
-	pops->_writeN = &usb_writeN;
-
-	pops->_write_mem = &usb_write_mem23a;
-	pops->_write_port = &usb_write_port23a;
-
-	pops->_read_port_cancel = &usb_read_port_cancel23a;
-	pops->_write_port_cancel = &usb_write_port23a_cancel;
-
-	pops->_read_interrupt = &usb_read_interrupt;
-}
-
 void rtl8723au_set_hw_type(struct rtw_adapter *padapter)
 {
 	padapter->chip_type = RTL8723A;
diff --git a/drivers/staging/rtl8723au/include/Hal8723APhyCfg.h b/drivers/staging/rtl8723au/include/Hal8723APhyCfg.h
index 4b7f3479c0a..299598b5387 100644
--- a/drivers/staging/rtl8723au/include/Hal8723APhyCfg.h
+++ b/drivers/staging/rtl8723au/include/Hal8723APhyCfg.h
@@ -68,13 +68,13 @@ enum RF_RADIO_PATH {
 
 enum WIRELESS_MODE {
 	WIRELESS_MODE_UNKNOWN	= 0x00,
-	WIRELESS_MODE_A		= BIT2,
-	WIRELESS_MODE_B		= BIT0,
-	WIRELESS_MODE_G		= BIT1,
-	WIRELESS_MODE_AUTO	= BIT5,
-	WIRELESS_MODE_N_24G	= BIT3,
-	WIRELESS_MODE_N_5G	= BIT4,
-	WIRELESS_MODE_AC	= BIT6
+	WIRELESS_MODE_A		= BIT(2),
+	WIRELESS_MODE_B		= BIT(0),
+	WIRELESS_MODE_G		= BIT(1),
+	WIRELESS_MODE_AUTO	= BIT(5),
+	WIRELESS_MODE_N_24G	= BIT(3),
+	WIRELESS_MODE_N_5G	= BIT(4),
+	WIRELESS_MODE_AC	= BIT(6)
 };
 
 enum baseband_config_type {
diff --git a/drivers/staging/rtl8723au/include/Hal8723PwrSeq.h b/drivers/staging/rtl8723au/include/Hal8723PwrSeq.h
index 7f3bdea6a55..4a1f58f2982 100644
--- a/drivers/staging/rtl8723au/include/Hal8723PwrSeq.h
+++ b/drivers/staging/rtl8723au/include/Hal8723PwrSeq.h
@@ -39,71 +39,71 @@
  * { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },  comments here
  */
 #define RTL8723A_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*/	\
+	{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)}, /*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, BIT(4), 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, 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*/	\
-	{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},/**/	\
-	{0x004E, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, 1},/*0x4C[23] = 0x4E[7] = 1, switch DPDT_SEL_P output from WL BB */\
+	{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}, /*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, 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},/**/	\
+	{0x004E, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), 1},/*0x4C[23] = 0x4E[7] = 1, switch DPDT_SEL_P output from WL BB */\
 
 #define RTL8723A_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*/	\
-	{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] */\
-	{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*/   \
+	{0x004E, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), 0},/*0x4C[23] = 0x4E[7] = 0, switch DPDT_SEL_P output from register 0x65[2] */\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, 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, PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(1), 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, BIT(5), BIT(5)}, /*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, BIT(0), 0}, /*0x20[0] = 1b'0 disable LDOA12 MACRO block*/   \
 
 
 #define RTL8723A_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*/   \
+	{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'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*/	\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), BIT(4)}, /*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*/
+	{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*/
 
 #define RTL8723A_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*/
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3) | BIT(7), 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, 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*/\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 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, BIT(3)|BIT(4), 0}, /*0x04[12:11] = 2b'01enable WL suspend*/
 
 #define RTL8723A_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*/
+	{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*/	\
+	{0x004A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 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, BIT(4), BIT(4)}, /*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, 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*/
 
 #define RTL8723A_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*/   \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3) | BIT(7), 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, 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*/\
+	{0x004A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 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, BIT(3)|BIT(4), 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, BIT(4), 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 RTL8723A_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*/   \
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), BIT(4)}, /*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*/
+	{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_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*/
+	{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_PCI_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF},/*PCIe DMA stop*/	\
@@ -112,25 +112,25 @@
 	{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_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, BIT1, 0},/*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},/*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*/	\
+	{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*/	\
 	{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*/
+	{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															\
 	{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*/\
+	{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, BIT1|BIT0, BIT1|BIT0}, /*.	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)}, /*.	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 RTL8723A_TRANS_END															\
diff --git a/drivers/staging/rtl8723au/include/cmd_osdep.h b/drivers/staging/rtl8723au/include/cmd_osdep.h
deleted file mode 100644
index 4866bee0405..00000000000
--- a/drivers/staging/rtl8723au/include/cmd_osdep.h
+++ /dev/null
@@ -1,26 +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 __CMD_OSDEP_H_
-#define __CMD_OSDEP_H_
-
-#include <osdep_service.h>
-#include <drv_types.h>
-
-int _rtw_init_evt_priv23a(struct evt_priv *pevtpriv);
-void _rtw_free_evt_priv23a(struct	evt_priv *pevtpriv);
-void _rtw_free_cmd_priv23a(struct	cmd_priv *pcmdpriv);
-int _rtw_enqueue_cmd23a(struct rtw_queue *queue, struct cmd_obj *obj);
-
-#endif
diff --git a/drivers/staging/rtl8723au/include/drv_types.h b/drivers/staging/rtl8723au/include/drv_types.h
index 53eecea48ce..a94857df2de 100644
--- a/drivers/staging/rtl8723au/include/drv_types.h
+++ b/drivers/staging/rtl8723au/include/drv_types.h
@@ -38,12 +38,10 @@ enum _NIC_VERSION {
 #include <rtw_ht.h>
 
 #include <rtw_cmd.h>
-#include <wlan_bssdef.h>
 #include <rtw_xmit.h>
 #include <rtw_recv.h>
 #include <hal_intf.h>
 #include <hal_com.h>
-#include <rtw_qos.h>
 #include <rtw_security.h>
 #include <rtw_pwrctrl.h>
 #include <rtw_io.h>
@@ -55,26 +53,10 @@ enum _NIC_VERSION {
 #include <rtw_event.h>
 #include <rtw_led.h>
 #include <rtw_mlme_ext.h>
-#include <rtw_p2p.h>
 #include <rtw_ap.h>
 
 #include "ioctl_cfg80211.h"
 
-#define SPEC_DEV_ID_NONE BIT(0)
-#define SPEC_DEV_ID_DISABLE_HT BIT(1)
-#define SPEC_DEV_ID_ENABLE_PS BIT(2)
-#define SPEC_DEV_ID_RF_CONFIG_1T1R BIT(3)
-#define SPEC_DEV_ID_RF_CONFIG_2T2R BIT(4)
-#define SPEC_DEV_ID_ASSIGN_IFNAME BIT(5)
-
-struct specific_device_id {
-	u32		flags;
-
-	u16		idVendor;
-	u16		idProduct;
-
-};
-
 struct registry_priv {
 	u8	chip_version;
 	u8	rfintfs;
@@ -187,8 +169,6 @@ struct dvobj_priv {
 	int	RtOutPipe[3];
 	u8	Queue2Pipe[HW_QUEUE_ENTRY];/* for out pipe mapping */
 
-	u8	irq_alloc;
-
 /*-------- below is for USB INTERFACE --------*/
 
 	u8	nr_endpoint;
@@ -197,10 +177,6 @@ struct dvobj_priv {
 	u8	RtNumOutPipes;
 	int	ep_num[5]; /* endpoint number */
 
-	int	RegUsbSS;
-
-	struct semaphore usb_suspend_sema;
-
 	struct mutex  usb_vendor_req_mutex;
 
 	u8 *usb_alloc_vendor_req_buf;
@@ -243,8 +219,6 @@ struct rtw_adapter {
 	struct	mlme_ext_priv mlmeextpriv;
 	struct	cmd_priv	cmdpriv;
 	struct	evt_priv	evtpriv;
-	/* struct	io_queue	*pio_queue; */
-	struct	io_priv	iopriv;
 	struct	xmit_priv	xmitpriv;
 	struct	recv_priv	recvpriv;
 	struct	sta_priv	stapriv;
@@ -254,21 +228,9 @@ struct rtw_adapter {
 	struct	eeprom_priv eeprompriv;
 	struct	led_priv	ledpriv;
 
-#ifdef CONFIG_8723AU_AP_MODE
-	struct	hostapd_priv	*phostapdpriv;
-#endif
-
-	struct cfg80211_wifidirect_info	cfg80211_wdinfo;
 	u32	setband;
-	struct wifidirect_info	wdinfo;
-
-#ifdef CONFIG_8723AU_P2P
-	struct wifi_display_info wfd_info;
-#endif /* CONFIG_8723AU_P2P */
 
 	void *HalData;
-	u32 hal_data_sz;
-	struct hal_ops	HalFunc;
 
 	s32	bDriverStopped;
 	s32	bSurpriseRemoved;
@@ -283,69 +245,26 @@ struct rtw_adapter {
 	u8	init_adpt_in_progress;
 	u8	bHaltInProgress;
 
-	void *cmdThread;
-	void *evtThread;
-	void *xmitThread;
-	void *recvThread;
-
-	void (*intf_start)(struct rtw_adapter *adapter);
-	void (*intf_stop)(struct rtw_adapter *adapter);
-
 	struct net_device *pnetdev;
 
 	/*  used by rtw_rereg_nd_name related function */
-	struct rereg_nd_name_data {
-		struct net_device *old_pnetdev;
-		char old_ifname[IFNAMSIZ];
-		u8 old_ips_mode;
-		u8 old_bRegUseLed;
-	} rereg_nd_name_priv;
-
 	int bup;
 	struct net_device_stats stats;
-	struct iw_statistics iwstats;
-	struct proc_dir_entry *dir_dev;/*  for proc directory */
 
 	struct wireless_dev *rtw_wdev;
 	int net_closed;
 
 	u8 bFWReady;
-	u8 bBTFWReady;
 	u8 bReadPortCancel;
 	u8 bWritePortCancel;
-	u8 bRxRSSIDisplay;
 	/* The driver will show the desired chan nor when this flag is 1. */
 	u8 bNotifyChannelChange;
-#ifdef CONFIG_8723AU_P2P
-	/* driver will show current P2P status when the  application reads it*/
-	u8 bShowGetP2PState;
-#endif
 	struct rtw_adapter *pbuddy_adapter;
 
 	/* extend to support multi interface */
 	/* IFACE_ID0 is equals to PRIMARY_ADAPTER */
 	/* IFACE_ID1 is equals to SECONDARY_ADAPTER */
 	u8 iface_id;
-
-#ifdef CONFIG_BR_EXT
-	_lock					br_ext_lock;
-	/* unsigned int			macclone_completed; */
-	struct nat25_network_db_entry	*nethash[NAT25_HASH_SIZE];
-	int				pppoe_connection_in_progress;
-	unsigned char			pppoe_addr[MACADDRLEN];
-	unsigned char			scdb_mac[MACADDRLEN];
-	unsigned char			scdb_ip[4];
-	struct nat25_network_db_entry	*scdb_entry;
-	unsigned char			br_mac[MACADDRLEN];
-	unsigned char			br_ip[4];
-
-	struct br_ext_info		ethBrExtInfo;
-#endif	/*  CONFIG_BR_EXT */
-
-	u8    fix_rate;
-
-	unsigned char     in_cta_test;
-
 };
 
 #define adapter_to_dvobj(adapter) (adapter->dvobj)
diff --git a/drivers/staging/rtl8723au/include/ethernet.h b/drivers/staging/rtl8723au/include/ethernet.h
deleted file mode 100644
index 39fc6df8818..00000000000
--- a/drivers/staging/rtl8723au/include/ethernet.h
+++ /dev/null
@@ -1,22 +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.
- *
- *
- ******************************************************************************/
-/*! \file */
-#ifndef __INC_ETHERNET_H
-#define __INC_ETHERNET_H
-
-#define LLC_HEADER_SIZE			6	/*  LLC Header Length */
-
-#endif /*  #ifndef __INC_ETHERNET_H */
diff --git a/drivers/staging/rtl8723au/include/hal_com.h b/drivers/staging/rtl8723au/include/hal_com.h
index 20f983cfc2b..4a161e26c8d 100644
--- a/drivers/staging/rtl8723au/include/hal_com.h
+++ b/drivers/staging/rtl8723au/include/hal_com.h
@@ -165,8 +165,6 @@ void HalSetBrateCfg23a(struct rtw_adapter *padapter, u8 *mBratesOS);
 bool
 Hal_MappingOutPipe23a(struct rtw_adapter *pAdapter, u8 NumOutPipe);
 
-void hal_init_macaddr23a(struct rtw_adapter *adapter);
-
 void c2h_evt_clear23a(struct rtw_adapter *adapter);
 s32 c2h_evt_read23a(struct rtw_adapter *adapter, u8 *buf);
 
@@ -185,11 +183,12 @@ void rtl8723a_ack_preamble(struct rtw_adapter *padapter, u8 bShortPreamble);
 void rtl8723a_set_sec_cfg(struct rtw_adapter *padapter, u8 sec);
 void rtl8723a_cam_empty_entry(struct rtw_adapter *padapter, u8 ucIndex);
 void rtl8723a_cam_invalid_all(struct rtw_adapter *padapter);
-void rtl8723a_cam_write(struct rtw_adapter *padapter, u32 val1, u32 val2);
+void rtl8723a_cam_write(struct rtw_adapter *padapter,
+			u8 entry, u16 ctrl, const u8 *mac, const u8 *key);
 void rtl8723a_fifo_cleanup(struct rtw_adapter *padapter);
 void rtl8723a_set_apfm_on_mac(struct rtw_adapter *padapter, u8 val);
 void rtl8723a_bcn_valid(struct rtw_adapter *padapter);
-void rtl8723a_set_tx_pause(struct rtw_adapter *padapter, u8 pause);
+bool rtl8723a_get_bcn_valid(struct rtw_adapter *padapter);
 void rtl8723a_set_beacon_interval(struct rtw_adapter *padapter, u16 interval);
 void rtl8723a_set_resp_sifs(struct rtw_adapter *padapter,
 			    u8 r2t1, u8 r2t2, u8 t2t1, u8 t2t2);
@@ -202,10 +201,14 @@ void rtl8723a_set_nav_upper(struct rtw_adapter *padapter, u32 usNavUpper);
 void rtl8723a_set_initial_gain(struct rtw_adapter *padapter, u32 rx_gain);
 
 void rtl8723a_odm_support_ability_write(struct rtw_adapter *padapter, u32 val);
-void rtl8723a_odm_support_ability_backup(struct rtw_adapter *padapter, u8 val);
+void rtl8723a_odm_support_ability_backup(struct rtw_adapter *padapter);
+void rtl8723a_odm_support_ability_restore(struct rtw_adapter *padapter);
 void rtl8723a_odm_support_ability_set(struct rtw_adapter *padapter, u32 val);
 void rtl8723a_odm_support_ability_clr(struct rtw_adapter *padapter, u32 val);
 
 void rtl8723a_set_rpwm(struct rtw_adapter *padapter, u8 val);
+u8 rtl8723a_get_rf_type(struct rtw_adapter *padapter);
+bool rtl8723a_get_fwlps_rf_on(struct rtw_adapter *padapter);
+bool rtl8723a_chk_hi_queue_empty(struct rtw_adapter *padapter);
 
 #endif /* __HAL_COMMON_H__ */
diff --git a/drivers/staging/rtl8723au/include/hal_intf.h b/drivers/staging/rtl8723au/include/hal_intf.h
index d183f4ba1ec..04223b5c047 100644
--- a/drivers/staging/rtl8723au/include/hal_intf.h
+++ b/drivers/staging/rtl8723au/include/hal_intf.h
@@ -19,10 +19,10 @@
 #include <drv_types.h>
 
 enum RTL871X_HCI_TYPE {
-	RTW_PCIE	= BIT0,
-	RTW_USB		= BIT1,
-	RTW_SDIO	= BIT2,
-	RTW_GSPI	= BIT3,
+	RTW_PCIE	= BIT(0),
+	RTW_USB		= BIT(1),
+	RTW_SDIO	= BIT(2),
+	RTW_GSPI	= BIT(3),
 };
 
 enum _CHIP_TYPE {
@@ -35,78 +35,6 @@ enum _CHIP_TYPE {
 	MAX_CHIP_TYPE
 };
 
-enum HW_VARIABLES {
-	HW_VAR_MEDIA_STATUS,
-	HW_VAR_MEDIA_STATUS1,
-	HW_VAR_SET_OPMODE,
-	HW_VAR_MAC_ADDR,
-	HW_VAR_BSSID,
-	HW_VAR_INIT_RTS_RATE,
-	HW_VAR_BASIC_RATE,
-	HW_VAR_TXPAUSE,
-	HW_VAR_BCN_FUNC,
-	HW_VAR_CORRECT_TSF,
-	HW_VAR_CHECK_BSSID,
-	HW_VAR_MLME_DISCONNECT,
-	HW_VAR_MLME_SITESURVEY,
-	HW_VAR_MLME_JOIN,
-	HW_VAR_ON_RCR_AM,
-	HW_VAR_OFF_RCR_AM,
-	HW_VAR_BEACON_INTERVAL,
-	HW_VAR_SLOT_TIME,
-	HW_VAR_RESP_SIFS,
-	HW_VAR_ACK_PREAMBLE,
-	HW_VAR_SEC_CFG,
-	HW_VAR_BCN_VALID,
-	HW_VAR_RF_TYPE,
-	HW_VAR_DM_FLAG,
-	HW_VAR_DM_FUNC_OP,
-	HW_VAR_DM_FUNC_SET,
-	HW_VAR_DM_FUNC_CLR,
-	HW_VAR_CAM_EMPTY_ENTRY,
-	HW_VAR_CAM_INVALID_ALL,
-	HW_VAR_CAM_WRITE,
-	HW_VAR_CAM_READ,
-	HW_VAR_AC_PARAM_VO,
-	HW_VAR_AC_PARAM_VI,
-	HW_VAR_AC_PARAM_BE,
-	HW_VAR_AC_PARAM_BK,
-	HW_VAR_ACM_CTRL,
-	HW_VAR_AMPDU_MIN_SPACE,
-	HW_VAR_AMPDU_FACTOR,
-	HW_VAR_RXDMA_AGG_PG_TH,
-	HW_VAR_SET_RPWM,
-	HW_VAR_H2C_FW_PWRMODE,
-	HW_VAR_H2C_FW_JOINBSSRPT,
-	HW_VAR_FWLPS_RF_ON,
-	HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
-	HW_VAR_TDLS_WRCR,
-	HW_VAR_TDLS_INIT_CH_SEN,
-	HW_VAR_TDLS_RS_RCR,
-	HW_VAR_TDLS_DONE_CH_SEN,
-	HW_VAR_INITIAL_GAIN,
-	HW_VAR_TRIGGER_GPIO_0,
-	HW_VAR_BT_SET_COEXIST,
-	HW_VAR_BT_ISSUE_DELBA,
-	HW_VAR_CURRENT_ANTENNA,
-	HW_VAR_ANTENNA_DIVERSITY_LINK,
-	HW_VAR_ANTENNA_DIVERSITY_SELECT,
-	HW_VAR_SWITCH_EPHY_WoWLAN,
-	HW_VAR_EFUSE_BYTES,
-	HW_VAR_EFUSE_BT_BYTES,
-	HW_VAR_FIFO_CLEARN_UP,
-	HW_VAR_CHECK_TXBUF,
-	HW_VAR_APFM_ON_MAC, /* Auto FSM to Turn On, include clock, isolation, power control for MAC only */
-	/*  The valid upper nav range for the HW updating, if the true value is larger than the upper range, the HW won't update it. */
-	/*  Unit in microsecond. 0 means disable this function. */
-	HW_VAR_NAV_UPPER,
-	HW_VAR_RPT_TIMER_SETTING,
-	HW_VAR_TX_RPT_MAX_MACID,
-	HW_VAR_H2C_MEDIA_STATUS_RPT,
-	HW_VAR_CHK_HI_QUEUE_EMPTY,
-	HW_VAR_READ_LLT_TAB,
-};
-
 enum hal_def_variable {
 	HAL_DEF_UNDERCORATEDSMOOTHEDPWDB,
 	HAL_DEF_IS_SUPPORT_ANT_DIV,
@@ -132,127 +60,6 @@ enum hal_odm_variable {
 	HAL_ODM_WIFI_DISPLAY_STATE,
 };
 
-enum hal_intf_ps_func {
-	HAL_USB_SELECT_SUSPEND,
-	HAL_MAX_ID,
-};
-
-struct hal_ops {
-	u32 (*hal_power_on)(struct rtw_adapter *padapter);
-	u32 (*hal_init)(struct rtw_adapter *padapter);
-	u32 (*hal_deinit)(struct rtw_adapter *padapter);
-
-	void (*free_hal_data)(struct rtw_adapter *padapter);
-
-	u32 (*inirp_init)(struct rtw_adapter *padapter);
-	u32 (*inirp_deinit)(struct rtw_adapter *padapter);
-
-	s32 (*init_xmit_priv)(struct rtw_adapter *padapter);
-	void (*free_xmit_priv)(struct rtw_adapter *padapter);
-
-	s32 (*init_recv_priv)(struct rtw_adapter *padapter);
-	void (*free_recv_priv)(struct rtw_adapter *padapter);
-
-	void (*InitSwLeds)(struct rtw_adapter *padapter);
-	void (*DeInitSwLeds)(struct rtw_adapter *padapter);
-
-	void (*dm_init)(struct rtw_adapter *padapter);
-	void (*dm_deinit)(struct rtw_adapter *padapter);
-	void (*read_chip_version)(struct rtw_adapter *padapter);
-
-	void (*init_default_value)(struct rtw_adapter *padapter);
-
-	void (*intf_chip_configure)(struct rtw_adapter *padapter);
-
-	void (*read_adapter_info)(struct rtw_adapter *padapter);
-
-	void (*enable_interrupt)(struct rtw_adapter *padapter);
-	void (*disable_interrupt)(struct rtw_adapter *padapter);
-	s32 (*interrupt_handler)(struct rtw_adapter *padapter);
-	void (*set_bwmode_handler)(struct rtw_adapter *padapter,
-				   enum ht_channel_width Bandwidth, u8 Offset);
-	void (*set_channel_handler)(struct rtw_adapter *padapter, u8 channel);
-
-	void (*hal_dm_watchdog)(struct rtw_adapter *padapter);
-
-	void (*SetHwRegHandler)(struct rtw_adapter *padapter,
-				u8 variable, u8 *val);
-	void (*GetHwRegHandler)(struct rtw_adapter *padapter,
-				u8 variable, u8 *val);
-
-	u8 (*GetHalDefVarHandler)(struct rtw_adapter *padapter,
-				  enum hal_def_variable eVariable,
-				  void *pValue);
-	u8 (*SetHalDefVarHandler)(struct rtw_adapter *padapter,
-				  enum hal_def_variable eVariable,
-				  void *pValue);
-
-	void (*GetHalODMVarHandler)(struct rtw_adapter *padapter,
-				    enum hal_odm_variable eVariable,
-				    void *pValue1, bool bSet);
-	void (*SetHalODMVarHandler)(struct rtw_adapter *padapter,
-				    enum hal_odm_variable eVariable,
-				    void *pValue1, bool bSet);
-
-	void (*UpdateRAMaskHandler)(struct rtw_adapter *padapter,
-				    u32 mac_id, u8 rssi_level);
-	void (*SetBeaconRelatedRegistersHandler)(struct rtw_adapter *padapter);
-
-	void (*Add_RateATid)(struct rtw_adapter *padapter, u32 bitmap,
-			     u8 arg, u8 rssi_level);
-	void (*run_thread)(struct rtw_adapter *padapter);
-	void (*cancel_thread)(struct rtw_adapter *padapter);
-
-	u8 (*interface_ps_func)(struct rtw_adapter *padapter,
-				enum hal_intf_ps_func efunc_id, u8 *val);
-
-	s32 (*hal_xmit)(struct rtw_adapter *padapter,
-			struct xmit_frame *pxmitframe);
-	s32 (*mgnt_xmit)(struct rtw_adapter *padapter,
-			 struct xmit_frame *pmgntframe);
-	s32 (*hal_xmitframe_enqueue)(struct rtw_adapter *padapter,
-				     struct xmit_frame *pxmitframe);
-
-	u32 (*read_bbreg)(struct rtw_adapter *padapter, u32 RegAddr,
-			  u32 BitMask);
-	void (*write_bbreg)(struct rtw_adapter *padapter, u32 RegAddr,
-			    u32 BitMask, u32 Data);
-	u32 (*read_rfreg)(struct rtw_adapter *padapter, u32 eRFPath,
-			  u32 RegAddr, u32 BitMask);
-	void (*write_rfreg)(struct rtw_adapter *padapter, u32 eRFPath,
-			    u32 RegAddr, u32 BitMask, u32 Data);
-
-	void (*EfusePowerSwitch)(struct rtw_adapter *padapter, u8 bWrite,
-				 u8 PwrState);
-	void (*ReadEFuse)(struct rtw_adapter *padapter, u8 efuseType,
-			  u16 _offset, u16 _size_byte, u8 *pbuf);
-	void (*EFUSEGetEfuseDefinition)(struct rtw_adapter *padapter,
-					u8 efuseType, u8 type, void *pOut);
-	u16 (*EfuseGetCurrentSize)(struct rtw_adapter *padapter, u8 efuseType);
-	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);
-	u8 (*Efuse_WordEnableDataWrite23a)(struct rtw_adapter *padapter,
-					   u16 efuse_addr, u8 word_en,
-					   u8 *data);
-	bool (*Efuse_PgPacketWrite23a_BT)(struct rtw_adapter *padapter,
-					  u8 offset, u8 word_en, u8 *data);
-
-	void (*sreset_init_value23a)(struct rtw_adapter *padapter);
-	void (*sreset_reset_value23a)(struct rtw_adapter *padapter);
-	void (*silentreset)(struct rtw_adapter *padapter);
-	void (*sreset_xmit_status_check)(struct rtw_adapter *padapter);
-	void (*sreset_linked_status_check) (struct rtw_adapter *padapter);
-	u8 (*sreset_get_wifi_status23a)(struct rtw_adapter *padapter);
-	bool (*sreset_inprogress)(struct rtw_adapter *padapter);
-
-	void (*hal_notch_filter)(struct rtw_adapter *adapter, bool enable);
-	void (*hal_reset_security_engine)(struct rtw_adapter *adapter);
-	s32 (*c2h_handler)(struct rtw_adapter *padapter, struct c2h_evt_hdr *c2h_evt);
-	c2h_id_filter c2h_id_filter_ccx;
-};
-
 enum rt_eeprom_type {
 	EEPROM_93C46,
 	EEPROM_93C56,
@@ -262,10 +69,10 @@ enum rt_eeprom_type {
 
 
 #define RF_CHANGE_BY_INIT	0
-#define RF_CHANGE_BY_IPS	BIT28
-#define RF_CHANGE_BY_PS		BIT29
-#define RF_CHANGE_BY_HW		BIT30
-#define RF_CHANGE_BY_SW		BIT31
+#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)
 
 enum hardware_type {
 	HARDWARE_TYPE_RTL8180,
@@ -291,102 +98,26 @@ enum hardware_type {
 };
 
 #define GET_EEPROM_EFUSE_PRIV(adapter) (&adapter->eeprompriv)
-#define is_boot_from_eeprom(adapter) (adapter->eeprompriv.EepromOrEfuse)
-
-extern int rtw_ht_enable23A;
-extern int rtw_cbw40_enable23A;
-extern int rtw_ampdu_enable23A;/* for enable tx_ampdu */
 
 void rtw_hal_def_value_init23a(struct rtw_adapter *padapter);
 int pm_netdev_open23a(struct net_device *pnetdev, u8 bnormal);
 int rtw_resume_process23a(struct rtw_adapter *padapter);
 
-void	rtw_hal_free_data23a(struct rtw_adapter *padapter);
-
-void rtw_hal_dm_init23a(struct rtw_adapter *padapter);
-void rtw_hal_dm_deinit23a(struct rtw_adapter *padapter);
-void rtw_hal_sw_led_init23a(struct rtw_adapter *padapter);
-void rtw_hal_sw_led_deinit23a(struct rtw_adapter *padapter);
-
-u32 rtw_hal_power_on23a(struct rtw_adapter *padapter);
-uint rtw_hal_init23a(struct rtw_adapter *padapter);
-uint rtw_hal_deinit23a(struct rtw_adapter *padapter);
+int rtw_hal_init23a(struct rtw_adapter *padapter);
+int rtw_hal_deinit23a(struct rtw_adapter *padapter);
 void rtw_hal_stop(struct rtw_adapter *padapter);
-void rtw_hal_set_hwreg23a(struct rtw_adapter *padapter, u8 variable, u8 *val);
-void rtw23a_hal_get_hwreg(struct rtw_adapter *padapter, u8 variable, u8 *val);
-
-void rtw_hal_chip_configure23a(struct rtw_adapter *padapter);
-void rtw_hal_read_chip_info23a(struct rtw_adapter *padapter);
-void rtw_hal_read_chip_version23a(struct rtw_adapter *padapter);
-
-u8 rtw_hal_set_def_var23a(struct rtw_adapter *padapter,
-			  enum hal_def_variable eVariable,
-			  void *pValue);
-u8 rtw_hal_get_def_var23a(struct rtw_adapter *padapter,
-			  enum hal_def_variable eVariable,
-			  void *pValue);
-
-void rtw_hal_set_odm_var23a(struct rtw_adapter *padapter,
-			    enum hal_odm_variable eVariable,
-			    void *pValue1, bool bSet);
-void rtw_hal_get_odm_var23a(struct rtw_adapter *padapter,
-			    enum hal_odm_variable eVariable,
-			    void *pValue1, bool bSet);
-
-void rtw_hal_enable_interrupt23a(struct rtw_adapter *padapter);
-void rtw_hal_disable_interrupt23a(struct rtw_adapter *padapter);
-
-u32 rtw_hal_inirp_init23a(struct rtw_adapter *padapter);
-u32 rtw_hal_inirp_deinit23a(struct rtw_adapter *padapter);
-
-u8 rtw_hal_intf_ps_func23a(struct rtw_adapter *padapter,
-			   enum hal_intf_ps_func efunc_id, u8 *val);
-
-s32 rtw_hal_xmit23aframe_enqueue(struct rtw_adapter *padapter,
-				 struct xmit_frame *pxmitframe);
-s32 rtw_hal_xmit23a(struct rtw_adapter *padapter,
-		    struct xmit_frame *pxmitframe);
-s32 rtw_hal_mgnt_xmit23a(struct rtw_adapter *padapter,
-			 struct xmit_frame *pmgntframe);
-
-s32	rtw_hal_init23a_xmit_priv(struct rtw_adapter *padapter);
-void	rtw_hal_free_xmit_priv23a(struct rtw_adapter *padapter);
-
-s32	rtw_hal_init23a_recv_priv(struct rtw_adapter *padapter);
-void	rtw_hal_free_recv_priv23a(struct rtw_adapter *padapter);
 
 void rtw_hal_update_ra_mask23a(struct sta_info *psta, u8 rssi_level);
-void	rtw_hal_add_ra_tid23a(struct rtw_adapter *padapter, u32 bitmap, u8 arg, u8 rssi_level);
 void	rtw_hal_clone_data(struct rtw_adapter *dst_padapter, struct rtw_adapter *src_padapter);
-void	rtw_hal_start_thread23a(struct rtw_adapter *padapter);
-void	rtw_hal_stop_thread23a(struct rtw_adapter *padapter);
-
-void rtw_hal_bcn_related_reg_setting23a(struct rtw_adapter *padapter);
-
-u32	rtw_hal_read_bbreg23a(struct rtw_adapter *padapter, u32 RegAddr, u32 BitMask);
-void	rtw_hal_write_bbreg23a(struct rtw_adapter *padapter, u32 RegAddr, u32 BitMask, u32 Data);
-u32	rtw_hal_read_rfreg23a(struct rtw_adapter *padapter, u32 eRFPath, u32 RegAddr, u32 BitMask);
-void	rtw_hal_write_rfreg23a(struct rtw_adapter *padapter, u32 eRFPath, u32 RegAddr, u32 BitMask, u32 Data);
-
-s32	rtw_hal_interrupt_handler23a(struct rtw_adapter *padapter);
-
-void	rtw_hal_set_bwmode23a(struct rtw_adapter *padapter,
-			   enum ht_channel_width Bandwidth, u8 Offset);
-void	rtw_hal_set_chan23a(struct rtw_adapter *padapter, u8 channel);
-void	rtw_hal_dm_watchdog23a(struct rtw_adapter *padapter);
-
-void rtw_hal_sreset_init23a(struct rtw_adapter *padapter);
-void rtw_hal_sreset_reset23a(struct rtw_adapter *padapter);
-void rtw_hal_sreset_reset23a_value23a(struct rtw_adapter *padapter);
-void rtw_hal_sreset_xmit_status_check23a(struct rtw_adapter *padapter);
-void rtw_hal_sreset_linked_status_check23a (struct rtw_adapter *padapter);
-u8   rtw_hal_sreset_get_wifi_status23a(struct rtw_adapter *padapter);
-bool rtw_hal_sreset_inprogress(struct rtw_adapter *padapter);
 
-void rtw_hal_notch_filter23a(struct rtw_adapter *adapter, bool enable);
-void rtw_hal_reset_security_engine23a(struct rtw_adapter *adapter);
+void hw_var_set_correct_tsf(struct rtw_adapter *padapter);
+void hw_var_set_mlme_disconnect(struct rtw_adapter *padapter);
+void hw_var_set_opmode(struct rtw_adapter *padapter, u8 mode);
+void hw_var_set_macaddr(struct rtw_adapter *padapter, u8 *val);
+void hw_var_set_bssid(struct rtw_adapter *padapter, u8 *val);
+void hw_var_set_mlme_join(struct rtw_adapter *padapter, u8 type);
 
-s32 rtw_hal_c2h_handler23a(struct rtw_adapter *adapter, struct c2h_evt_hdr *c2h_evt);
-c2h_id_filter rtw_hal_c2h_id_filter_ccx23a(struct rtw_adapter *adapter);
+int GetHalDefVar8192CUsb(struct rtw_adapter *Adapter,
+			 enum hal_def_variable eVariable, void *pValue);
 
 #endif /* __HAL_INTF_H__ */
diff --git a/drivers/staging/rtl8723au/include/ieee80211.h b/drivers/staging/rtl8723au/include/ieee80211.h
index 28e4ab239fb..69c0f5c94fc 100644
--- a/drivers/staging/rtl8723au/include/ieee80211.h
+++ b/drivers/staging/rtl8723au/include/ieee80211.h
@@ -58,7 +58,7 @@
 
 #define WPA_CIPHER_NONE		BIT(0)
 #define WPA_CIPHER_WEP40	BIT(1)
-#define WPA_CIPHER_WEP104 BIT(2)
+#define WPA_CIPHER_WEP104	BIT(2)
 #define WPA_CIPHER_TKIP		BIT(3)
 #define WPA_CIPHER_CCMP		BIT(4)
 
@@ -105,41 +105,53 @@ enum ratr_table_mode {
 
 enum NETWORK_TYPE
 {
-    WIRELESS_INVALID = 0,
-    /* Sub-Element */
-    WIRELESS_11B = BIT(0), /*  tx: cck only , rx: cck only, hw: cck */
-    WIRELESS_11G = BIT(1), /*  tx: ofdm only, rx: ofdm & cck, hw: cck & ofdm */
-    WIRELESS_11A = BIT(2), /*  tx: ofdm only, rx: ofdm only, hw: ofdm only */
-    WIRELESS_11_24N = BIT(3), /*  tx: MCS only, rx: MCS & cck, hw: MCS & cck */
-    WIRELESS_11_5N = BIT(4), /*  tx: MCS only, rx: MCS & ofdm, hw: ofdm only */
+	WIRELESS_INVALID = 0,
+	/* Sub-Element */
+	/*  tx: cck only , rx: cck only, hw: cck */
+	WIRELESS_11B = BIT(0),
+	/*  tx: ofdm only, rx: ofdm & cck, hw: cck & ofdm */
+	WIRELESS_11G = BIT(1),
+	/*  tx: ofdm only, rx: ofdm only, hw: ofdm only */
+	WIRELESS_11A = BIT(2),
+	/*  tx: MCS only, rx: MCS & cck, hw: MCS & cck */
+	WIRELESS_11_24N = BIT(3),
+	/*  tx: MCS only, rx: MCS & ofdm, hw: ofdm only */
+	WIRELESS_11_5N = BIT(4),
 	/* WIRELESS_AUTO		= BIT(5), */
-	WIRELESS_AC		= BIT(6),
-
-    /* Combination */
-    WIRELESS_11BG = (WIRELESS_11B|WIRELESS_11G), /*  tx: cck & ofdm, rx: cck & ofdm & MCS, hw: cck & ofdm */
-    WIRELESS_11G_24N = (WIRELESS_11G|WIRELESS_11_24N), /*  tx: ofdm & MCS, rx: ofdm & cck & MCS, hw: cck & ofdm */
-    WIRELESS_11A_5N = (WIRELESS_11A|WIRELESS_11_5N), /*  tx: ofdm & MCS, rx: ofdm & MCS, hw: ofdm only */
-    WIRELESS_11BG_24N = (WIRELESS_11B|WIRELESS_11G|WIRELESS_11_24N), /*  tx: ofdm & cck & MCS, rx: ofdm & cck & MCS, hw: ofdm & cck */
-    WIRELESS_11AGN = (WIRELESS_11A|WIRELESS_11G|WIRELESS_11_24N|WIRELESS_11_5N), /*  tx: ofdm & MCS, rx: ofdm & MCS, hw: ofdm only */
-    WIRELESS_11ABGN = (WIRELESS_11A|WIRELESS_11B|WIRELESS_11G|WIRELESS_11_24N|WIRELESS_11_5N),
+	WIRELESS_AC = BIT(6),
+
+	/* Combination */
+	 /*  tx: cck & ofdm, rx: cck & ofdm & MCS, hw: cck & ofdm */
+	WIRELESS_11BG = WIRELESS_11B|WIRELESS_11G,
+	/*  tx: ofdm & MCS, rx: ofdm & cck & MCS, hw: cck & ofdm */
+	WIRELESS_11G_24N = WIRELESS_11G | WIRELESS_11_24N,
+	/*  tx: ofdm & MCS, rx: ofdm & MCS, hw: ofdm only */
+	WIRELESS_11A_5N = WIRELESS_11A | WIRELESS_11_5N,
+	/*  tx: ofdm & cck & MCS, rx: ofdm & cck & MCS, hw: ofdm & cck */
+	WIRELESS_11BG_24N = WIRELESS_11B | WIRELESS_11G | WIRELESS_11_24N,
+	/*  tx: ofdm & MCS, rx: ofdm & MCS, hw: ofdm only */
+	WIRELESS_11AGN = WIRELESS_11A | WIRELESS_11G | WIRELESS_11_24N |
+				WIRELESS_11_5N,
+	WIRELESS_11ABGN = WIRELESS_11A | WIRELESS_11B | WIRELESS_11G |
+				WIRELESS_11_24N | WIRELESS_11_5N,
 };
 
 #define SUPPORTED_24G_NETTYPE_MSK (WIRELESS_11B | WIRELESS_11G | WIRELESS_11_24N)
 #define SUPPORTED_5G_NETTYPE_MSK (WIRELESS_11A | WIRELESS_11_5N)
 
-#define IsSupported24G(NetType) ((NetType) & SUPPORTED_24G_NETTYPE_MSK ? true : false)
-#define IsSupported5G(NetType) ((NetType) & SUPPORTED_5G_NETTYPE_MSK ? true : false)
+#define IsSupported24G(NetType) (NetType & SUPPORTED_24G_NETTYPE_MSK ? true : false)
+#define IsSupported5G(NetType) (NetType & SUPPORTED_5G_NETTYPE_MSK ? true : false)
 
 #define IsEnableHWCCK(NetType) IsSupported24G(NetType)
-#define IsEnableHWOFDM(NetType) ((NetType) & (WIRELESS_11G|WIRELESS_11_24N|SUPPORTED_5G_NETTYPE_MSK) ? true : false)
+#define IsEnableHWOFDM(NetType) (NetType & (WIRELESS_11G|WIRELESS_11_24N|SUPPORTED_5G_NETTYPE_MSK) ? true : false)
 
 #define IsSupportedRxCCK(NetType) IsEnableHWCCK(NetType)
 #define IsSupportedRxOFDM(NetType) IsEnableHWOFDM(NetType)
 #define IsSupportedRxMCS(NetType) IsEnableHWOFDM(NetType)
 
-#define IsSupportedTxCCK(NetType) ((NetType) & (WIRELESS_11B) ? true : false)
-#define IsSupportedTxOFDM(NetType) ((NetType) & (WIRELESS_11G|WIRELESS_11A) ? true : false)
-#define IsSupportedTxMCS(NetType) ((NetType) & (WIRELESS_11_24N|WIRELESS_11_5N) ? true : false)
+#define IsSupportedTxCCK(NetType) (NetType & (WIRELESS_11B) ? true : false)
+#define IsSupportedTxOFDM(NetType) (NetType & (WIRELESS_11G|WIRELESS_11A) ? true : false)
+#define IsSupportedTxMCS(NetType) (NetType & (WIRELESS_11_24N|WIRELESS_11_5N) ? true : false)
 
 
 struct ieee_param {
@@ -192,7 +204,7 @@ struct ieee_param {
 /* QoS,QOS */
 #define NORMAL_ACK			0
 #define NO_ACK				1
-#define NON_EXPLICIT_ACK	2
+#define NON_EXPLICIT_ACK		2
 #define BLOCK_ACK			3
 
 /* IEEE 802.11 defines */
@@ -200,24 +212,22 @@ struct ieee_param {
 #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 */
-
 } __attribute__ ((packed));
 
 
 #define SNAP_SIZE sizeof(struct ieee80211_snap_hdr)
 
-#define WLAN_FC_GET_TYPE(fc) ((fc) & IEEE80211_FCTL_FTYPE)
-#define WLAN_FC_GET_STYPE(fc) ((fc) & IEEE80211_FCTL_STYPE)
+#define WLAN_FC_GET_TYPE(fc)		(fc & IEEE80211_FCTL_FTYPE)
+#define WLAN_FC_GET_STYPE(fc)		(fc & IEEE80211_FCTL_STYPE)
 
-#define WLAN_QC_GET_TID(qc) ((qc) & 0x0f)
+#define WLAN_QC_GET_TID(qc)		(qc & 0x0f)
 
-#define WLAN_GET_SEQ_FRAG(seq) ((seq) & RTW_IEEE80211_SCTL_FRAG)
-#define WLAN_GET_SEQ_SEQ(seq)  ((seq) & RTW_IEEE80211_SCTL_SEQ)
+#define WLAN_GET_SEQ_FRAG(seq)		(seq & RTW_IEEE80211_SCTL_FRAG)
+#define WLAN_GET_SEQ_SEQ(seq)		(seq & RTW_IEEE80211_SCTL_SEQ)
 
 
 #define WLAN_REASON_JOIN_WRONG_CHANNEL       65534
@@ -298,67 +308,20 @@ struct ieee80211_snap_hdr {
 #define WEP_KEY_LEN 13
 
 
-
-/*
-
- 802.11 data frame from AP
-
-      ,-------------------------------------------------------------------.
-Bytes |  2   |  2   |    6    |    6    |    6    |  2   | 0..2312 |   4  |
-      |------|------|---------|---------|---------|------|---------|------|
-Desc. | ctrl | dura |  DA/RA  |   TA    |    SA   | Sequ |  frame  |  fcs |
-      |      | tion | (BSSID) |         |         | ence |  data   |      |
-      `-------------------------------------------------------------------'
-
-Total: 28-2340 bytes
-
-*/
-
-struct ieee80211_header_data {
-	u16 frame_ctl;
-	u16 duration_id;
-	u8 addr1[6];
-	u8 addr2[6];
-	u8 addr3[6];
-	u16 seq_ctrl;
-};
-
-struct ieee80211_info_element_hdr {
-	u8 id;
-	u8 len;
-} __attribute__ ((packed));
-
-struct ieee80211_info_element {
-	u8 id;
-	u8 len;
-	u8 data[0];
-} __attribute__ ((packed));
-
-
-struct ieee80211_txb {
-	u8 nr_frags;
-	u8 encrypted;
-	u16 reserved;
-	u16 frag_size;
-	u16 payload_size;
-	struct sk_buff *fragments[0];
-};
-
-
 /* MAX_RATES_LENGTH needs to be 12.  The spec says 8, and many APs
  * only use 8, and then use extended rates for the remaining supported
  * rates.  Other APs, however, stick all of their supported rates on the
  * main rates information element... */
-#define MAX_RATES_LENGTH                  ((u8)12)
-#define MAX_RATES_EX_LENGTH               ((u8)16)
-#define MAX_CHANNEL_NUMBER                 161
+#define MAX_RATES_LENGTH	12
+#define MAX_RATES_EX_LENGTH	16
+#define MAX_CHANNEL_NUMBER	161
 
-#define MAX_WPA_IE_LEN (256)
-#define MAX_WPS_IE_LEN (512)
-#define MAX_P2P_IE_LEN (256)
-#define MAX_WFD_IE_LEN (128)
+#define MAX_WPA_IE_LEN		256
+#define MAX_WPS_IE_LEN		512
+#define MAX_P2P_IE_LEN		256
+#define MAX_WFD_IE_LEN		128
 
-#define IW_ESSID_MAX_SIZE 32
+#define IW_ESSID_MAX_SIZE	32
 
 /*
 join_res:
@@ -367,24 +330,11 @@ join_res:
 > 0: TID
 */
 
-#define DEFAULT_MAX_SCAN_AGE (15 * HZ)
-#define DEFAULT_FTS 2346
 #define MAC_FMT "%02x:%02x:%02x:%02x:%02x:%02x"
 #define MAC_ARG(x) ((u8*)(x))[0],((u8*)(x))[1],((u8*)(x))[2],((u8*)(x))[3],((u8*)(x))[4],((u8*)(x))[5]
 
-#define CFG_IEEE80211_RESERVE_FCS (1<<0)
-#define CFG_IEEE80211_COMPUTE_FCS (1<<1)
-
 #define MAXTID	16
 
-#define IEEE_A            (1<<0)
-#define IEEE_B            (1<<1)
-#define IEEE_G            (1<<2)
-#define IEEE_MODE_MASK    (IEEE_A|IEEE_B|IEEE_G)
-
-/* Baron move to ieee80211.c */
-int ieee80211_is_empty_essid23a(const char *essid, int essid_len);
-
 enum _PUBLIC_ACTION{
 	ACT_PUBLIC_BSSCOEXIST = 0, /*  20/40 BSS Coexistence */
 	ACT_PUBLIC_DSE_ENABLE = 1,
@@ -457,79 +407,17 @@ struct rtw_ieee80211_channel {
 	/*, (channel)->orig_mag*/ \
 	/*, (channel)->orig_mpwr*/ \
 
-/* Parsed Information Elements */
-struct rtw_ieee802_11_elems {
-	u8 *ssid;
-	u8 ssid_len;
-	u8 *supp_rates;
-	u8 supp_rates_len;
-	u8 *fh_params;
-	u8 fh_params_len;
-	u8 *ds_params;
-	u8 ds_params_len;
-	u8 *cf_params;
-	u8 cf_params_len;
-	u8 *tim;
-	u8 tim_len;
-	u8 *ibss_params;
-	u8 ibss_params_len;
-	u8 *challenge;
-	u8 challenge_len;
-	u8 *erp_info;
-	u8 erp_info_len;
-	u8 *ext_supp_rates;
-	u8 ext_supp_rates_len;
-	u8 *wpa_ie;
-	u8 wpa_ie_len;
-	u8 *rsn_ie;
-	u8 rsn_ie_len;
-	u8 *wme;
-	u8 wme_len;
-	u8 *wme_tspec;
-	u8 wme_tspec_len;
-	u8 *wps_ie;
-	u8 wps_ie_len;
-	u8 *power_cap;
-	u8 power_cap_len;
-	u8 *supp_channels;
-	u8 supp_channels_len;
-	u8 *mdie;
-	u8 mdie_len;
-	u8 *ftie;
-	u8 ftie_len;
-	u8 *timeout_int;
-	u8 timeout_int_len;
-	u8 *ht_capabilities;
-	u8 ht_capabilities_len;
-	u8 *ht_operation;
-	u8 ht_operation_len;
-	u8 *vendor_ht_cap;
-	u8 vendor_ht_cap_len;
-};
-
-enum parse_res {
-	ParseOK = 0,
-	ParseUnknown = 1,
-	ParseFailed = -1
-};
-
-enum parse_res rtw_ieee802_11_parse_elems23a(u8 *start, uint len,
-				struct rtw_ieee802_11_elems *elems,
-				int show_errors);
-
 u8 *rtw_set_fixed_ie23a(unsigned char *pbuf, unsigned int len, unsigned char *source, unsigned int *frlen);
-u8 *rtw_set_ie23a(u8 *pbuf, int index, uint len, u8 *source, uint *frlen);
+u8 *rtw_set_ie23a(u8 *pbuf, int index, uint len, const u8 *source, uint *frlen);
 
 enum secondary_ch_offset {
 	SCN = 0, /* no secondary channel */
 	SCA = 1, /* secondary channel above */
 	SCB = 3,  /* secondary channel below */
 };
-u8 secondary_ch_offset_to_hal_ch_offset23a(u8 ch_offset);
 u8 hal_ch_offset_to_secondary_ch_offset23a(u8 ch_offset);
 u8 *rtw_set_ie23a_ch_switch(u8 *buf, u32 *buf_len, u8 ch_switch_mode, u8 new_ch, u8 ch_switch_cnt);
 u8 *rtw_set_ie23a_secondary_ch_offset(u8 *buf, u32 *buf_len, u8 secondary_ch_offset);
-u8 *rtw_set_ie23a_mesh_ch_switch_parm(u8 *buf, u32 *buf_len, u8 ttl, u8 flags, u16 reason, u16 precedence);
 
 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);
@@ -537,46 +425,17 @@ int rtw_ies_remove_ie23a(u8 *ies, uint *ies_len, uint offset, u8 eid, u8 *oui, u
 
 void rtw_set_supported_rate23a(u8* SupportedRates, uint mode) ;
 
-unsigned char *rtw_get_wpa_ie23a(unsigned char *pie, int *wpa_ie_len, int limit);
-unsigned char *rtw_get_wpa2_ie23a(unsigned char *pie, int *rsn_ie_len, int limit);
-int rtw_get_wpa_cipher_suite23a(u8 *s);
-int rtw_get_wpa2_cipher_suite23a(u8 *s);
-int rtw_parse_wpa_ie23a(u8* wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x);
-int rtw_parse_wpa2_ie23a(u8* wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x);
+int rtw_get_wpa_cipher_suite23a(const u8 *s);
+int rtw_get_wpa2_cipher_suite23a(const u8 *s);
+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);
 
 int rtw_get_sec_ie23a(u8 *in_ie,uint in_len,u8 *rsn_ie,u16 *rsn_len,u8 *wpa_ie,u16 *wpa_len);
 
-u8 rtw_is_wps_ie23a(u8 *ie_ptr, uint *wps_ielen);
 u8 *rtw_get_wps_ie23a(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen);
 u8 *rtw_get_wps_attr23a(u8 *wps_ie, uint wps_ielen, u16 target_attr_id ,u8 *buf_attr, u32 *len_attr);
 u8 *rtw_get_wps_attr_content23a(u8 *wps_ie, uint wps_ielen, u16 target_attr_id ,u8 *buf_content, uint *len_content);
 
-/**
- * for_each_ie - iterate over continuous IEs
- * @ie:
- * @buf:
- * @buf_len:
- */
-#define for_each_ie(ie, buf, buf_len) \
-	for (ie = (void*)buf; (((u8*)ie) - ((u8*)buf) + 1) < buf_len; ie = (void*)(((u8*)ie) + *(((u8*)ie)+1) + 2))
-
-void dump_ies23a(u8 *buf, u32 buf_len);
-void dump_wps_ie23a(u8 *ie, u32 ie_len);
-
-#ifdef CONFIG_8723AU_P2P
-void dump_p2p_ie23a(u8 *ie, u32 ie_len);
-u8 *rtw_get_p2p_ie23a(u8 *in_ie, int in_len, u8 *p2p_ie, uint *p2p_ielen);
-u8 *rtw_get_p2p_attr23a(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id ,u8 *buf_attr, u32 *len_attr);
-u8 *rtw_get_p2p_attr23a_content(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id ,u8 *buf_content, uint *len_content);
-u32 rtw_set_p2p_attr_content23a(u8 *pbuf, u8 attr_id, u16 attr_len, u8 *pdata_attr);
-void rtw_wlan_bssid_ex_remove_p2p_attr23a(struct wlan_bssid_ex *bss_ex, u8 attr_id);
-#endif
-
-#ifdef CONFIG_8723AU_P2P
-int rtw_get_wfd_ie(u8 *in_ie, int in_len, u8 *wfd_ie, uint *wfd_ielen);
-int rtw_get_wfd_attr_content(u8 *wfd_ie, uint wfd_ielen, u8 target_attr_id ,u8 *attr_content, uint *attr_contentlen);
-#endif /*  CONFIG_8723AU_P2P */
-
 uint	rtw_get_rateset_len23a(u8	*rateset);
 
 struct registry_priv;
@@ -585,19 +444,12 @@ int rtw_generate_ie23a(struct registry_priv *pregistrypriv);
 
 int rtw_get_bit_value_from_ieee_value23a(u8 val);
 
-uint rtw_is_cckrates_included23a(u8 *rate);
-
-uint rtw_is_cckratesonly_included23a(u8 *rate);
-
 int rtw_check_network_type23a(unsigned char *rate, int ratelen, int channel);
 
 void rtw_get_bcn_info23a(struct wlan_network *pnetwork);
 
-void rtw_macaddr_cfg23a(u8 *mac_addr);
-
 u16 rtw_mcs_rate23a(u8 rf_type, u8 bw_40MHz, u8 short_GI_20, u8 short_GI_40, unsigned char * MCS_rate);
 
-int rtw_action_frame_parse23a(const u8 *frame, u32 frame_len, u8* category, u8 *action);
 const char *action_public_str23a(u8 action);
 
 #endif /* IEEE80211_H */
diff --git a/drivers/staging/rtl8723au/include/ioctl_cfg80211.h b/drivers/staging/rtl8723au/include/ioctl_cfg80211.h
index 0eb9036d725..63e921f921d 100644
--- a/drivers/staging/rtl8723au/include/ioctl_cfg80211.h
+++ b/drivers/staging/rtl8723au/include/ioctl_cfg80211.h
@@ -15,23 +15,6 @@
 #ifndef __IOCTL_CFG80211_H__
 #define __IOCTL_CFG80211_H__
 
-struct rtw_wdev_invit_info {
-	u8 token;
-	u8 flags;
-	u8 status;
-	u8 req_op_ch;
-	u8 rsp_op_ch;
-};
-
-#define rtw_wdev_invit_info_init(invit_info) \
-	do { \
-		(invit_info)->token = 0; \
-		(invit_info)->flags = 0x00; \
-		(invit_info)->status = 0xff; \
-		(invit_info)->req_op_ch = 0; \
-		(invit_info)->rsp_op_ch = 0; \
-	} while (0)
-
 struct rtw_wdev_priv {
 	struct wireless_dev *rtw_wdev;
 
@@ -45,11 +28,6 @@ struct rtw_wdev_priv {
 
 	u8 p2p_enabled;
 
-	u8 provdisc_req_issued;
-
-	struct rtw_wdev_invit_info invit_info;
-
-	bool block;
 	bool power_mgmt;
 };
 
@@ -83,37 +61,9 @@ void rtw_cfg80211_indicate_sta_disassoc(struct rtw_adapter *padapter,
 					unsigned char *da, unsigned short reason);
 #endif /* CONFIG_8723AU_AP_MODE */
 
-void rtw_cfg80211_issue_p2p_provision_request23a(struct rtw_adapter *padapter,
-					      const u8 *buf, size_t len);
-void rtw_cfg80211_rx_p2p_action_public(struct rtw_adapter *padapter,
-				       u8 *pmgmt_frame, uint frame_len);
-void rtw_cfg80211_rx_action_p2p(struct rtw_adapter *padapter,
-				u8 *pmgmt_frame, uint frame_len);
 void rtw_cfg80211_rx_action(struct rtw_adapter *adapter, u8 *frame,
 			    uint frame_len, const char*msg);
 
-int rtw_cfg80211_set_mgnt_wpsp2pie(struct net_device *net, char *buf, int len,
-				   int type);
-
 bool rtw_cfg80211_pwr_mgmt(struct rtw_adapter *adapter);
 
-#define rtw_cfg80211_rx_mgmt(adapter, freq, sig_dbm, buf, len, gfp)	\
-	cfg80211_rx_mgmt((adapter)->rtw_wdev, freq, sig_dbm, buf, len, 0, gfp)
-
-#define rtw_cfg80211_send_rx_assoc(adapter, bss, buf, len)		\
-	cfg80211_send_rx_assoc((adapter)->pnetdev, bss, buf, len)
-
-#define rtw_cfg80211_mgmt_tx_status(adapter, cookie, buf, len, ack, gfp) \
-	cfg80211_mgmt_tx_status((adapter)->rtw_wdev, cookie, buf,	\
-				len, ack, gfp)
-
-#define rtw_cfg80211_ready_on_channel(adapter, cookie, chan,		\
-				      channel_type, duration, gfp)	\
-	cfg80211_ready_on_channel((adapter)->rtw_wdev, cookie, chan,	\
-				  duration, gfp)
-#define rtw_cfg80211_remain_on_channel_expired(adapter, cookie, chan,	\
-					       chan_type, gfp)		\
-	cfg80211_remain_on_channel_expired((adapter)->rtw_wdev,		\
-					   cookie, chan, gfp)
-
 #endif /* __IOCTL_CFG80211_H__ */
diff --git a/drivers/staging/rtl8723au/include/mlme_osdep.h b/drivers/staging/rtl8723au/include/mlme_osdep.h
index b7132a9a137..4bb5525b7a6 100644
--- a/drivers/staging/rtl8723au/include/mlme_osdep.h
+++ b/drivers/staging/rtl8723au/include/mlme_osdep.h
@@ -19,10 +19,6 @@
 #include <drv_types.h>
 
 void rtw_os_indicate_disconnect23a(struct rtw_adapter *adapter);
-void rtw_os_indicate_connect23a(struct rtw_adapter *adapter);
-void rtw_os_indicate_scan_done23a(struct rtw_adapter *padapter, bool aborted);
-void rtw_report_sec_ie23a(struct rtw_adapter *adapter, u8 authmode, u8 *sec_ie);
-
 void rtw_reset_securitypriv23a(struct rtw_adapter *adapter);
 
 #endif	/* _MLME_OSDEP_H_ */
diff --git a/drivers/staging/rtl8723au/include/mp_custom_oid.h b/drivers/staging/rtl8723au/include/mp_custom_oid.h
deleted file mode 100644
index da197cf678a..00000000000
--- a/drivers/staging/rtl8723au/include/mp_custom_oid.h
+++ /dev/null
@@ -1,342 +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	__CUSTOM_OID_H
-#define __CUSTOM_OID_H
-
-/*  0xFF818000 - 0xFF81802F		RTL8180 Mass Production Kit */
-/*  0xFF818500 - 0xFF81850F		RTL8185 Setup Utility */
-/*  0xFF818580 - 0xFF81858F		RTL8185 Phy Status Utility */
-
-/*  For Production Kit with Agilent Equipments */
-/*  in order to make our custom oids hopefully somewhat unique */
-/*  we will use 0xFF (indicating implementation specific OID) */
-/*                81(first byte of non zero Realtek unique identifier) */
-/*                80 (second byte of non zero Realtek unique identifier) */
-/*                XX (the custom OID number - providing 255 possible custom oids) */
-
-#define OID_RT_PRO_RESET_DUT		0xFF818000
-#define OID_RT_PRO_SET_DATA_RATE		0xFF818001
-#define OID_RT_PRO_START_TEST		0xFF818002
-#define OID_RT_PRO_STOP_TEST		0xFF818003
-#define OID_RT_PRO_SET_PREAMBLE		0xFF818004
-#define OID_RT_PRO_SET_SCRAMBLER		0xFF818005
-#define OID_RT_PRO_SET_FILTER_BB		0xFF818006
-#define OID_RT_PRO_SET_MANUAL_DIVERSITY_BB		0xFF818007
-#define OID_RT_PRO_SET_CHANNEL_DIRECT_CALL		0xFF818008
-#define OID_RT_PRO_SET_SLEEP_MODE_DIRECT_CALL		0xFF818009
-#define OID_RT_PRO_SET_WAKE_MODE_DIRECT_CALL		0xFF81800A
-
-#define OID_RT_PRO_SET_TX_ANTENNA_BB		0xFF81800D
-#define OID_RT_PRO_SET_ANTENNA_BB		0xFF81800E
-#define OID_RT_PRO_SET_CR_SCRAMBLER		0xFF81800F
-#define OID_RT_PRO_SET_CR_NEW_FILTER		0xFF818010
-#define OID_RT_PRO_SET_TX_POWER_CONTROL		0xFF818011
-#define OID_RT_PRO_SET_CR_TX_CONFIG		0xFF818012
-#define OID_RT_PRO_GET_TX_POWER_CONTROL		0xFF818013
-#define OID_RT_PRO_GET_CR_SIGNAL_QUALITY		0xFF818014
-#define OID_RT_PRO_SET_CR_SETPOINT		0xFF818015
-#define OID_RT_PRO_SET_INTEGRATOR		0xFF818016
-#define OID_RT_PRO_SET_SIGNAL_QUALITY		0xFF818017
-#define OID_RT_PRO_GET_INTEGRATOR		0xFF818018
-#define OID_RT_PRO_GET_SIGNAL_QUALITY		0xFF818019
-#define OID_RT_PRO_QUERY_EEPROM_TYPE		0xFF81801A
-#define OID_RT_PRO_WRITE_MAC_ADDRESS		0xFF81801B
-#define OID_RT_PRO_READ_MAC_ADDRESS		0xFF81801C
-#define OID_RT_PRO_WRITE_CIS_DATA		0xFF81801D
-#define OID_RT_PRO_READ_CIS_DATA		0xFF81801E
-#define OID_RT_PRO_WRITE_POWER_CONTROL		0xFF81801F
-#define OID_RT_PRO_READ_POWER_CONTROL		0xFF818020
-#define OID_RT_PRO_WRITE_EEPROM		0xFF818021
-#define OID_RT_PRO_READ_EEPROM		0xFF818022
-#define OID_RT_PRO_RESET_TX_PACKET_SENT		0xFF818023
-#define OID_RT_PRO_QUERY_TX_PACKET_SENT		0xFF818024
-#define OID_RT_PRO_RESET_RX_PACKET_RECEIVED		0xFF818025
-#define OID_RT_PRO_QUERY_RX_PACKET_RECEIVED		0xFF818026
-#define OID_RT_PRO_QUERY_RX_PACKET_CRC32_ERROR		0xFF818027
-#define OID_RT_PRO_QUERY_CURRENT_ADDRESS		0xFF818028
-#define OID_RT_PRO_QUERY_PERMANENT_ADDRESS		0xFF818029
-#define OID_RT_PRO_SET_PHILIPS_RF_PARAMETERS		0xFF81802A
-#define OID_RT_PRO_RECEIVE_PACKET		0xFF81802C
-/*  added by Owen on 04/08/03 for Cameo's request */
-#define OID_RT_PRO_WRITE_EEPROM_BYTE		0xFF81802D
-#define OID_RT_PRO_READ_EEPROM_BYTE		0xFF81802E
-#define OID_RT_PRO_SET_MODULATION		0xFF81802F
-/*  */
-
-#define OID_RT_DRIVER_OPTION		0xFF818080
-#define OID_RT_RF_OFF		0xFF818081
-#define OID_RT_AUTH_STATUS		0xFF818082
-
-/*  */
-#define OID_RT_PRO_SET_CONTINUOUS_TX		0xFF81800B
-#define OID_RT_PRO_SET_SINGLE_CARRIER_TX		0xFF81800C
-#define OID_RT_PRO_SET_CARRIER_SUPPRESSION_TX		0xFF81802B
-#define OID_RT_PRO_SET_SINGLE_TONE_TX		0xFF818043
-/*  */
-
-
-/*  by Owen for RTL8185 Phy Status Report Utility */
-#define OID_RT_UTILITYfalse_ALARM_COUNTERS		0xFF818580
-#define OID_RT_UTILITY_SELECT_DEBUG_MODE		0xFF818581
-#define OID_RT_UTILITY_SELECT_SUBCARRIER_NUMBER		0xFF818582
-#define OID_RT_UTILITY_GET_RSSI_STATUS		0xFF818583
-#define OID_RT_UTILITY_GET_FRAME_DETECTION_STATUS		0xFF818584
-#define OID_RT_UTILITY_GET_AGC_AND_FREQUENCY_OFFSET_ESTIMATION_STATUS	0xFF818585
-#define OID_RT_UTILITY_GET_CHANNEL_ESTIMATION_STATUS		0xFF818586
-
-/*  by Owen on 03/09/19-03/09/22 for RTL8185 */
-#define OID_RT_WIRELESS_MODE		0xFF818500
-#define OID_RT_SUPPORTED_RATES		0xFF818501
-#define OID_RT_DESIRED_RATES		0xFF818502
-#define OID_RT_WIRELESS_MODE_STARTING_ADHOC		0xFF818503
-/*  */
-
-#define OID_RT_GET_CONNECT_STATE		0xFF030001
-#define OID_RT_RESCAN		0xFF030002
-#define OID_RT_SET_KEY_LENGTH		0xFF030003
-#define OID_RT_SET_DEFAULT_KEY_ID		0xFF030004
-
-#define OID_RT_SET_CHANNEL		0xFF010182
-#define OID_RT_SET_SNIFFER_MODE		0xFF010183
-#define OID_RT_GET_SIGNAL_QUALITY		0xFF010184
-#define OID_RT_GET_SMALL_PACKET_CRC		0xFF010185
-#define OID_RT_GET_MIDDLE_PACKET_CRC		0xFF010186
-#define OID_RT_GET_LARGE_PACKET_CRC		0xFF010187
-#define OID_RT_GET_TX_RETRY		0xFF010188
-#define OID_RT_GET_RX_RETRY		0xFF010189
-#define OID_RT_PRO_SET_FW_DIG_STATE		0xFF01018A/* S */
-#define OID_RT_PRO_SET_FW_RA_STATE		0xFF01018B/* S */
-
-#define OID_RT_GET_RX_TOTAL_PACKET		0xFF010190
-#define OID_RT_GET_TX_BEACON_OK		0xFF010191
-#define OID_RT_GET_TX_BEACON_ERR		0xFF010192
-#define OID_RT_GET_RX_ICV_ERR		0xFF010193
-#define OID_RT_SET_ENCRYPTION_ALGORITHM		0xFF010194
-#define OID_RT_SET_NO_AUTO_RESCAN		0xFF010195
-#define OID_RT_GET_PREAMBLE_MODE		0xFF010196
-#define OID_RT_GET_DRIVER_UP_DELTA_TIME		0xFF010197
-#define OID_RT_GET_AP_IP		0xFF010198
-#define OID_RT_GET_CHANNELPLAN		0xFF010199
-#define OID_RT_SET_PREAMBLE_MODE		0xFF01019A
-#define OID_RT_SET_BCN_INTVL		0xFF01019B
-#define OID_RT_GET_RF_VENDER		0xFF01019C
-#define OID_RT_DEDICATE_PROBE		0xFF01019D
-#define OID_RT_PRO_RX_FILTER_PATTERN		0xFF01019E
-
-#define OID_RT_GET_DCST_CURRENT_THRESHOLD		0xFF01019F
-
-#define OID_RT_GET_CCA_ERR		0xFF0101A0
-#define OID_RT_GET_CCA_UPGRADE_THRESHOLD		0xFF0101A1
-#define OID_RT_GET_CCA_FALLBACK_THRESHOLD		0xFF0101A2
-
-#define OID_RT_GET_CCA_UPGRADE_EVALUATE_TIMES		0xFF0101A3
-#define OID_RT_GET_CCA_FALLBACK_EVALUATE_TIMES		0xFF0101A4
-
-/*  by Owen on 03/31/03 for Cameo's request */
-#define OID_RT_SET_RATE_ADAPTIVE		0xFF0101A5
-/*  */
-#define OID_RT_GET_DCST_EVALUATE_PERIOD		0xFF0101A5
-#define OID_RT_GET_DCST_TIME_UNIT_INDEX		0xFF0101A6
-#define OID_RT_GET_TOTAL_TX_BYTES		0xFF0101A7
-#define OID_RT_GET_TOTAL_RX_BYTES		0xFF0101A8
-#define OID_RT_CURRENT_TX_POWER_LEVEL		0xFF0101A9
-#define OID_RT_GET_ENC_KEY_MISMATCH_COUNT		0xFF0101AA
-#define OID_RT_GET_ENC_KEY_MATCH_COUNT		0xFF0101AB
-#define OID_RT_GET_CHANNEL		0xFF0101AC
-
-#define OID_RT_SET_CHANNELPLAN		0xFF0101AD
-#define OID_RT_GET_HARDWARE_RADIO_OFF		0xFF0101AE
-#define OID_RT_CHANNELPLAN_BY_COUNTRY		0xFF0101AF
-#define OID_RT_SCAN_AVAILABLE_BSSID		0xFF0101B0
-#define OID_RT_GET_HARDWARE_VERSION		0xFF0101B1
-#define OID_RT_GET_IS_ROAMING		0xFF0101B2
-#define OID_RT_GET_IS_PRIVACY		0xFF0101B3
-#define OID_RT_GET_KEY_MISMATCH		0xFF0101B4
-#define OID_RT_SET_RSSI_ROAM_TRAFFIC_TH		0xFF0101B5
-#define OID_RT_SET_RSSI_ROAM_SIGNAL_TH		0xFF0101B6
-#define OID_RT_RESET_LOG		0xFF0101B7
-#define OID_RT_GET_LOG		0xFF0101B8
-#define OID_RT_SET_INDICATE_HIDDEN_AP		0xFF0101B9
-#define OID_RT_GET_HEADER_FAIL		0xFF0101BA
-#define OID_RT_SUPPORTED_WIRELESS_MODE		0xFF0101BB
-#define OID_RT_GET_CHANNEL_LIST		0xFF0101BC
-#define OID_RT_GET_SCAN_IN_PROGRESS		0xFF0101BD
-#define OID_RT_GET_TX_INFO		0xFF0101BE
-#define OID_RT_RF_READ_WRITE_OFFSET		0xFF0101BF
-#define OID_RT_RF_READ_WRITE		0xFF0101C0
-
-/*  For Netgear request. 2005.01.13, by rcnjko. */
-#define OID_RT_FORCED_DATA_RATE		0xFF0101C1
-#define OID_RT_WIRELESS_MODE_FOR_SCAN_LIST	0xFF0101C2
-/*  For Netgear request. 2005.02.17, by rcnjko. */
-#define OID_RT_GET_BSS_WIRELESS_MODE		0xFF0101C3
-/*  For AZ project. 2005.06.27, by rcnjko. */
-#define OID_RT_SCAN_WITH_MAGIC_PACKET		0xFF0101C4
-
-/*  Vincent 8185MP */
-#define OID_RT_PRO_RX_FILTER		0xFF0111C0
-
-/* Andy TEST */
-/* define OID_RT_PRO_WRITE_REGISTRY		0xFF0111C1 */
-/* define OID_RT_PRO_READ_REGISTRY		0xFF0111C2 */
-#define OID_CE_USB_WRITE_REGISTRY		0xFF0111C1
-#define OID_CE_USB_READ_REGISTRY		0xFF0111C2
-
-
-#define OID_RT_PRO_SET_INITIAL_GAIN		0xFF0111C3
-#define OID_RT_PRO_SET_BB_RF_STANDBY_MODE		0xFF0111C4
-#define OID_RT_PRO_SET_BB_RF_SHUTDOWN_MODE		0xFF0111C5
-#define OID_RT_PRO_SET_TX_CHARGE_PUMP		0xFF0111C6
-#define OID_RT_PRO_SET_RX_CHARGE_PUMP		0xFF0111C7
-#define OID_RT_PRO_RF_WRITE_REGISTRY		0xFF0111C8
-#define OID_RT_PRO_RF_READ_REGISTRY		0xFF0111C9
-#define OID_RT_PRO_QUERY_RF_TYPE		0xFF0111CA
-
-/*  AP OID */
-#define OID_RT_AP_GET_ASSOCIATED_STATION_LIST		0xFF010300
-#define OID_RT_AP_GET_CURRENT_TIME_STAMP		0xFF010301
-#define OID_RT_AP_SWITCH_INTO_AP_MODE		0xFF010302
-#define OID_RT_AP_SET_DTIM_PERIOD		0xFF010303
-#define OID_RT_AP_SUPPORTED		0xFF010304	/*  Determine if driver supports AP mode. 2004.08.27, by rcnjko. */
-#define OID_RT_AP_SET_PASSPHRASE		0xFF010305	/*  Set WPA-PSK passphrase into authenticator. 2005.07.08, byrcnjko. */
-
-/*  8187MP. 2004.09.06, by rcnjko. */
-#define OID_RT_PRO8187_WI_POLL		0xFF818780
-#define OID_RT_PRO_WRITE_BB_REG		0xFF818781
-#define OID_RT_PRO_READ_BB_REG		0xFF818782
-#define OID_RT_PRO_WRITE_RF_REG		0xFF818783
-#define OID_RT_PRO_READ_RF_REG		0xFF818784
-
-/*  Meeting House. added by Annie, 2005-07-20. */
-#define OID_RT_MH_VENDER_ID		0xFFEDC100
-
-/* 8711 MP OID added 20051230. */
-#define OID_RT_PRO8711_JOIN_BSS		0xFF871100/* S */
-
-#define OID_RT_PRO_READ_REGISTER		0xFF871101 /* Q */
-#define OID_RT_PRO_WRITE_REGISTER		0xFF871102 /* S */
-
-#define OID_RT_PRO_BURST_READ_REGISTER		0xFF871103 /* Q */
-#define OID_RT_PRO_BURST_WRITE_REGISTER		0xFF871104 /* S */
-
-#define OID_RT_PRO_WRITE_TXCMD		0xFF871105 /* S */
-
-#define OID_RT_PRO_READ16_EEPROM		0xFF871106 /* Q */
-#define OID_RT_PRO_WRITE16_EEPROM		0xFF871107 /* S */
-
-#define OID_RT_PRO_H2C_SET_COMMAND		0xFF871108 /* S */
-#define OID_RT_PRO_H2C_QUERY_RESULT		0xFF871109 /* Q */
-
-#define OID_RT_PRO8711_WI_POLL		0xFF87110A /* Q */
-#define OID_RT_PRO8711_PKT_LOSS		0xFF87110B /* Q */
-#define OID_RT_RD_ATTRIB_MEM		0xFF87110C/* Q */
-#define OID_RT_WR_ATTRIB_MEM		0xFF87110D/* S */
-
-
-/* Method 2 for H2C/C2H */
-#define OID_RT_PRO_H2C_CMD_MODE		0xFF871110 /* S */
-#define OID_RT_PRO_H2C_CMD_RSP_MODE		0xFF871111 /* Q */
-#define OID_RT_PRO_H2C_CMD_EVENT_MODE		0xFF871112 /* S */
-#define OID_RT_PRO_WAIT_C2H_EVENT		0xFF871113 /* Q */
-#define OID_RT_PRO_RW_ACCESS_PROTOCOL_TEST		0xFF871114/* Q */
-
-#define OID_RT_PRO_SCSI_ACCESS_TEST		0xFF871115 /* Q, S */
-
-#define OID_RT_PRO_SCSI_TCPIPOFFLOAD_OUT		0xFF871116 /* S */
-#define OID_RT_PRO_SCSI_TCPIPOFFLOAD_IN		0xFF871117 /* Q,S */
-#define OID_RT_RRO_RX_PKT_VIA_IOCTRL		0xFF871118 /* Q */
-#define OID_RT_RRO_RX_PKTARRAY_VIA_IOCTRL		0xFF871119 /* Q */
-
-#define OID_RT_RPO_SET_PWRMGT_TEST		0xFF87111A /* S */
-#define OID_RT_PRO_QRY_PWRMGT_TEST		0XFF87111B /* Q */
-#define OID_RT_RPO_ASYNC_RWIO_TEST		0xFF87111C /* S */
-#define OID_RT_RPO_ASYNC_RWIO_POLL		0xFF87111D /* Q */
-#define OID_RT_PRO_SET_RF_INTFS		0xFF87111E /* S */
-#define OID_RT_POLL_RX_STATUS		0xFF87111F /* Q */
-
-#define OID_RT_PRO_CFG_DEBUG_MESSAGE		0xFF871120 /* Q,S */
-#define OID_RT_PRO_SET_DATA_RATE_EX		0xFF871121/* S */
-#define OID_RT_PRO_SET_BASIC_RATE		0xFF871122/* S */
-#define OID_RT_PRO_READ_TSSI		0xFF871123/* S */
-#define OID_RT_PRO_SET_POWER_TRACKING		0xFF871124/* S */
-
-
-#define OID_RT_PRO_QRY_PWRSTATE		0xFF871150 /* Q */
-#define OID_RT_PRO_SET_PWRSTATE		0xFF871151 /* S */
-
-/* Method 2 , using workitem */
-#define OID_RT_SET_READ_REG		0xFF871181 /* S */
-#define OID_RT_SET_WRITE_REG		0xFF871182 /* S */
-#define OID_RT_SET_BURST_READ_REG		0xFF871183 /* S */
-#define OID_RT_SET_BURST_WRITE_REG		0xFF871184 /* S */
-#define OID_RT_SET_WRITE_TXCMD		0xFF871185 /* S */
-#define OID_RT_SET_READ16_EEPROM		0xFF871186 /* S */
-#define OID_RT_SET_WRITE16_EEPROM		0xFF871187 /* S */
-#define OID_RT_QRY_POLL_WKITEM		0xFF871188 /* Q */
-
-/* For SDIO INTERFACE only */
-#define OID_RT_PRO_SYNCPAGERW_SRAM		0xFF8711A0 /* Q, S */
-#define OID_RT_PRO_871X_DRV_EXT		0xFF8711A1
-
-/* For USB INTERFACE only */
-#define OID_RT_PRO_USB_VENDOR_REQ		0xFF8711B0 /* Q, S */
-#define OID_RT_PRO_SCSI_AUTO_TEST		0xFF8711B1 /* S */
-#define OID_RT_PRO_USB_MAC_AC_FIFO_WRITE		0xFF8711B2 /* S */
-#define OID_RT_PRO_USB_MAC_RX_FIFO_READ		0xFF8711B3 /* Q */
-#define OID_RT_PRO_USB_MAC_RX_FIFO_POLLING		0xFF8711B4 /* Q */
-
-#define OID_RT_PRO_H2C_SET_RATE_TABLE		0xFF8711FB /* S */
-#define OID_RT_PRO_H2C_GET_RATE_TABLE		0xFF8711FC /* S */
-#define OID_RT_PRO_H2C_C2H_LBK_TEST		0xFF8711FE
-
-#define OID_RT_PRO_ENCRYPTION_CTRL		0xFF871200 /* Q, S */
-#define OID_RT_PRO_ADD_STA_INFO		0xFF871201 /* S */
-#define OID_RT_PRO_DELE_STA_INFO		0xFF871202 /* S */
-#define OID_RT_PRO_QUERY_DR_VARIABLE		0xFF871203 /* Q */
-
-#define OID_RT_PRO_RX_PACKET_TYPE		0xFF871204 /* Q, S */
-
-#define OID_RT_PRO_READ_EFUSE		0xFF871205 /* Q */
-#define OID_RT_PRO_WRITE_EFUSE		0xFF871206 /* S */
-#define OID_RT_PRO_RW_EFUSE_PGPKT		0xFF871207 /* Q, S */
-#define OID_RT_GET_EFUSE_CURRENT_SIZE		0xFF871208 /* Q */
-
-#define OID_RT_SET_BANDWIDTH		0xFF871209 /* S */
-#define OID_RT_SET_CRYSTAL_CAP		0xFF87120A /* S */
-
-#define OID_RT_SET_RX_PACKET_TYPE		0xFF87120B /* S */
-
-#define OID_RT_GET_EFUSE_MAX_SIZE		0xFF87120C /* Q */
-
-#define OID_RT_PRO_SET_TX_AGC_OFFSET		0xFF87120D /* S */
-
-#define OID_RT_PRO_SET_PKT_TEST_MODE		0xFF87120E /* S */
-
-#define OID_RT_PRO_FOR_EVM_TEST_SETTING		0xFF87120F /* S */
-
-#define OID_RT_PRO_GET_THERMAL_METER		0xFF871210 /* Q */
-
-#define OID_RT_RESET_PHY_RX_PACKET_COUNT	0xFF871211 /* S */
-#define OID_RT_GET_PHY_RX_PACKET_RECEIVED	0xFF871212 /* Q */
-#define OID_RT_GET_PHY_RX_PACKET_CRC32_ERROR	0xFF871213 /* Q */
-
-#define OID_RT_SET_POWER_DOWN		0xFF871214 /* S */
-
-#define OID_RT_GET_POWER_MODE		0xFF871215 /* Q */
-
-#define OID_RT_PRO_EFUSE		0xFF871216 /* Q, S */
-#define OID_RT_PRO_EFUSE_MAP		0xFF871217 /* Q, S */
-
-#endif /* ifndef	__CUSTOM_OID_H */
diff --git a/drivers/staging/rtl8723au/include/odm.h b/drivers/staging/rtl8723au/include/odm.h
index dfedfbb48fc..183d8ff1ab9 100644
--- a/drivers/staging/rtl8723au/include/odm.h
+++ b/drivers/staging/rtl8723au/include/odm.h
@@ -86,15 +86,6 @@
 /*  structure and define */
 /*  */
 
-/*  */
-/*  2011/09/20 MH Add for AP/ADSLpseudo DM structuer requirement. */
-/*  We need to remove to other position??? */
-/*  */
-struct rtl8723a_priv {
-	u8		temp;
-};
-
-
 struct  dig_t {
 	u8		Dig_Enable_Flag;
 	u8		Dig_Ext_Port_Stage;
@@ -187,7 +178,6 @@ struct rx_hp {
 	bool		First_time_enter;
 	bool		RXHP_enable;
 	u8		TP_Mode;
-	struct timer_list PSDTimer;
 };
 
 #define ASSOCIATE_ENTRY_NUM					32 /*  Max size of AsocEntry[]. */
@@ -234,14 +224,12 @@ struct sw_ant_sw {
 	u64		RXByteCnt_A;
 	u64		RXByteCnt_B;
 	u8		TrafficLoad;
-	struct timer_list SwAntennaSwitchTimer;
 };
 
 struct edca_turbo {
 	bool bCurrentTurboEDCA;
 	bool bIsCurRDLState;
 	u32	prv_traffic_idx; /*  edca turbo */
-
 };
 
 struct odm_rate_adapt {
@@ -267,20 +255,6 @@ struct odm_rate_adapt {
 
 /*  Declare for common info */
 
-struct odm_phy_info {
-	u8		RxPWDBAll;
-	u8		SignalQuality;	 /*  in 0-100 index. */
-	u8		RxMIMOSignalQuality[RF_PATH_MAX]; /* EVM */
-	u8		RxMIMOSignalStrength[RF_PATH_MAX];/*  in 0~100 index */
-	s8		RxPower; /*  in dBm Translate from PWdB */
-	s8		RecvSignalPower;/*  Real power in dBm for this packet, no beautification and aggregation. Keep this raw info to be used for the other procedures. */
-	u8		BTRxRSSIPercentage;
-	u8		SignalStrength; /*  in 0-100 index. */
-	u8		RxPwr[RF_PATH_MAX];/* per-path's pwdb */
-	u8		RxSNR[RF_PATH_MAX];/* per-path's SNR */
-};
-
-
 struct odm_phy_dbg_info {
 	/* ODM Write,debug info */
 	s8		RxSNRdB[RF_PATH_MAX];
@@ -300,11 +274,6 @@ struct odm_packet_info {
 	bool		bPacketBeacon;
 };
 
-struct odm_mac_info {
-	u8	test;
-
-};
-
 
 enum {
 	/*  BB Team */
@@ -323,32 +292,6 @@ enum {
 };
 
 /*  */
-/*  2011/20/20 MH For MP driver RT_WLAN_STA =  struct sta_info */
-/*  Please declare below ODM relative info in your STA info structure. */
-/*  */
-struct odm_sta_info {
-	/*  Driver Write */
-	bool		bUsed;				/*  record the sta status link or not? */
-	u8		IOTPeer;			/*  Enum value.	HT_IOT_PEER_E */
-
-	/*  ODM Write */
-	/* 1 PHY_STATUS_INFO */
-	u8		RSSI_Path[4];		/*  */
-	u8		RSSI_Ave;
-	u8		RXEVM[4];
-	u8		RXSNR[4];
-
-	/*  ODM Write */
-	/* 1 TX_INFO (may changed by IC) */
-
-	/*  */
-	/*	Please use compile flag to disable the structure for other IC except 88E. */
-	/*	Move To lower layer. */
-	/*  */
-	/*  ODM Write Wilson will handle this part(said by Luke.Lee) */
-};
-
-/*  */
 /*  2011/10/20 MH Define Common info enum for all team. */
 /*  */
 
@@ -428,28 +371,28 @@ enum odm_cmninfo {
 /*  Define ODM support ability.  ODM_CMNINFO_ABILITY */
 enum {
 	/*  BB ODM section BIT 0-15 */
-	ODM_BB_DIG				= BIT0,
-	ODM_BB_RA_MASK				= BIT1,
-	ODM_BB_DYNAMIC_TXPWR			= BIT2,
-	ODM_BB_FA_CNT				= BIT3,
-	ODM_BB_RSSI_MONITOR			= BIT4,
-	ODM_BB_CCK_PD				= BIT5,
-	ODM_BB_ANT_DIV				= BIT6,
-	ODM_BB_PWR_SAVE				= BIT7,
-	ODM_BB_PWR_TRAIN			= BIT8,
-	ODM_BB_RATE_ADAPTIVE			= BIT9,
-	ODM_BB_PATH_DIV				= BIT10,
-	ODM_BB_PSD				= BIT11,
-	ODM_BB_RXHP				= BIT12,
+	ODM_BB_DIG				= BIT(0),
+	ODM_BB_RA_MASK				= BIT(1),
+	ODM_BB_DYNAMIC_TXPWR			= BIT(2),
+	ODM_BB_FA_CNT				= BIT(3),
+	ODM_BB_RSSI_MONITOR			= BIT(4),
+	ODM_BB_CCK_PD				= BIT(5),
+	ODM_BB_ANT_DIV				= BIT(6),
+	ODM_BB_PWR_SAVE				= BIT(7),
+	ODM_BB_PWR_TRAIN			= BIT(8),
+	ODM_BB_RATE_ADAPTIVE			= BIT(9),
+	ODM_BB_PATH_DIV				= BIT(10),
+	ODM_BB_PSD				= BIT(11),
+	ODM_BB_RXHP				= BIT(12),
 
 	/*  MAC DM section BIT 16-23 */
-	ODM_MAC_EDCA_TURBO			= BIT16,
-	ODM_MAC_EARLY_MODE			= BIT17,
+	ODM_MAC_EDCA_TURBO			= BIT(16),
+	ODM_MAC_EARLY_MODE			= BIT(17),
 
 	/*  RF ODM section BIT 24-31 */
-	ODM_RF_TX_PWR_TRACK			= BIT24,
-	ODM_RF_RX_GAIN_TRACK			= BIT25,
-	ODM_RF_CALIBRATION			= BIT26,
+	ODM_RF_TX_PWR_TRACK			= BIT(24),
+	ODM_RF_RX_GAIN_TRACK			= BIT(25),
+	ODM_RF_CALIBRATION			= BIT(26),
 
 };
 
@@ -463,13 +406,13 @@ enum odm_interface_def {
 
 /*  ODM_CMNINFO_IC_TYPE */
 enum odm_ic_type_def {
-	ODM_RTL8192S	=	BIT0,
-	ODM_RTL8192C	=	BIT1,
-	ODM_RTL8192D	=	BIT2,
-	ODM_RTL8723A	=	BIT3,
-	ODM_RTL8188E	=	BIT4,
-	ODM_RTL8812	=	BIT5,
-	ODM_RTL8821	=	BIT6,
+	ODM_RTL8192S	=	BIT(0),
+	ODM_RTL8192C	=	BIT(1),
+	ODM_RTL8192D	=	BIT(2),
+	ODM_RTL8723A	=	BIT(3),
+	ODM_RTL8188E	=	BIT(4),
+	ODM_RTL8812	=	BIT(5),
+	ODM_RTL8821	=	BIT(6),
 };
 
 #define ODM_IC_11N_SERIES			\
@@ -496,14 +439,14 @@ enum odm_fab_version {
 /*  ODM_CMNINFO_RF_TYPE */
 /*  For example 1T2R (A+AB = BIT0|BIT4|BIT5) */
 enum rf_path_def {
-	ODM_RF_TX_A	=	BIT0,
-	ODM_RF_TX_B	=	BIT1,
-	ODM_RF_TX_C	=	BIT2,
-	ODM_RF_TX_D	=	BIT3,
-	ODM_RF_RX_A	=	BIT4,
-	ODM_RF_RX_B	=	BIT5,
-	ODM_RF_RX_C	=	BIT6,
-	ODM_RF_RX_D	=	BIT7,
+	ODM_RF_TX_A	=	BIT(0),
+	ODM_RF_TX_B	=	BIT(1),
+	ODM_RF_TX_C	=	BIT(2),
+	ODM_RF_TX_D	=	BIT(3),
+	ODM_RF_RX_A	=	BIT(4),
+	ODM_RF_RX_B	=	BIT(5),
+	ODM_RF_RX_C	=	BIT(6),
+	ODM_RF_RX_D	=	BIT(7),
 };
 
 
@@ -536,33 +479,33 @@ enum odm_bt_coexist {
 
 /*  ODM_CMNINFO_OP_MODE */
 enum odm_operation_mode {
-	ODM_NO_LINK		= BIT0,
-	ODM_LINK		= BIT1,
-	ODM_SCAN		= BIT2,
-	ODM_POWERSAVE		= BIT3,
-	ODM_AP_MODE		= BIT4,
-	ODM_CLIENT_MODE		= BIT5,
-	ODM_AD_HOC		= BIT6,
-	ODM_WIFI_DIRECT		= BIT7,
-	ODM_WIFI_DISPLAY	= BIT8,
+	ODM_NO_LINK		= BIT(0),
+	ODM_LINK		= BIT(1),
+	ODM_SCAN		= BIT(2),
+	ODM_POWERSAVE		= BIT(3),
+	ODM_AP_MODE		= BIT(4),
+	ODM_CLIENT_MODE		= BIT(5),
+	ODM_AD_HOC		= BIT(6),
+	ODM_WIFI_DIRECT		= BIT(7),
+	ODM_WIFI_DISPLAY	= BIT(8),
 };
 
 /*  ODM_CMNINFO_WM_MODE */
 enum odm_wireless_mode {
 	ODM_WM_UNKNOW		= 0x0,
-	ODM_WM_B		= BIT0,
-	ODM_WM_G		= BIT1,
-	ODM_WM_A		= BIT2,
-	ODM_WM_N24G		= BIT3,
-	ODM_WM_N5G		= BIT4,
-	ODM_WM_AUTO		= BIT5,
-	ODM_WM_AC		= BIT6,
+	ODM_WM_B		= BIT(0),
+	ODM_WM_G		= BIT(1),
+	ODM_WM_A		= BIT(2),
+	ODM_WM_N24G		= BIT(3),
+	ODM_WM_N5G		= BIT(4),
+	ODM_WM_AUTO		= BIT(5),
+	ODM_WM_AC		= BIT(6),
 };
 
 /*  ODM_CMNINFO_BAND */
 enum odm_band_type {
-	ODM_BAND_2_4G		= BIT0,
-	ODM_BAND_5G		= BIT1,
+	ODM_BAND_2_4G		= BIT(0),
+	ODM_BAND_5G		= BIT(1),
 
 };
 
@@ -573,18 +516,6 @@ enum odm_sec_chnl_offset {
 	ODM_ABOVE		= 2
 };
 
-/*  ODM_CMNINFO_SEC_MODE */
-enum odm_security {
-	ODM_SEC_OPEN		= 0,
-	ODM_SEC_WEP40		= 1,
-	ODM_SEC_TKIP		= 2,
-	ODM_SEC_RESERVE		= 3,
-	ODM_SEC_AESCCMP		= 4,
-	ODM_SEC_WEP104		= 5,
-	ODM_WEP_WPA_MIXED	= 6, /*  WEP + WPA */
-	ODM_SEC_SMS4		= 7,
-};
-
 /*  ODM_CMNINFO_BW */
 enum odm_band_width {
 	ODM_BW20M		= 0,
@@ -613,38 +544,6 @@ enum odm_cca_path {
 	ODM_CCA_1R_B			= 2,
 };
 
-struct odm_ra_info {
-	u8 RateID;
-	u32 RateMask;
-	u32 RAUseRate;
-	u8 RateSGI;
-	u8 RssiStaRA;
-	u8 PreRssiStaRA;
-	u8 SGIEnable;
-	u8 DecisionRate;
-	u8 PreRate;
-	u8 HighestRate;
-	u8 LowestRate;
-	u32 NscUp;
-	u32 NscDown;
-	u16 RTY[5];
-	u32 TOTAL;
-	u16 DROP;
-	u8 Active;
-	u16 RptTime;
-	u8 RAWaitingCounter;
-	u8 RAPendingCounter;
-	u8 PTActive;  /*  on or off */
-	u8 PTTryState;  /*  0 trying state, 1 for decision state */
-	u8 PTStage;  /*  0~6 */
-	u8 PTStopCount; /* Stop PT counter */
-	u8 PTPreRate;  /*  if rate change do PT */
-	u8 PTPreRssi; /*  if RSSI change 5% do PT */
-	u8 PTModeSS;  /*  decide whitch rate should do PT */
-	u8 RAstage;  /*  StageRA, decide how many times RA will be done between PT */
-	u8 PTSmoothFactor;
-};
-
 struct iqk_matrix_regs_set {
 	bool	bIQKDone;
 	s32	Value[1][IQK_Matrix_REG_NUM];
@@ -762,16 +661,10 @@ enum ant_dif_type {
 
 /*  2011/09/22 MH Copy from SD4 defined structure. We use to support PHY DM integration. */
 struct dm_odm_t {
-	/* struct timer_list FastAntTrainingTimer; */
 	/*  */
 	/*	Add for different team use temporarily */
 	/*  */
 	struct rtw_adapter	*Adapter;		/*  For CE/NIC team */
-	struct rtl8723a_priv	*priv;			/*  For AP/ADSL team */
-	/*  WHen you use Adapter or priv pointer, you must make sure the pointer is ready. */
-	bool			odm_ready;
-
-	struct rtl8723a_priv fake_priv;
 
 	u64			DebugComponents;
 	u32			DebugLevel;
@@ -891,7 +784,6 @@ struct dm_odm_t {
 	/* PHY_INFO_88E		PhyInfo; */
 
 	/*  Latest packet phy info (ODM write) */
-	struct odm_mac_info		*pMacInfo;
 	/* MAC_INFO_88E		MacInfo; */
 
 	/*  Different Team independt structure?? */
@@ -928,7 +820,6 @@ struct dm_odm_t {
 
 	/* PSD */
 	bool			bUserAssignLevel;
-	struct timer_list	PSDTimer;
 	u8			RSSI_BT;			/* come from BT */
 	bool			bPSDinProcess;
 	bool			bDMInitialGainEnable;
@@ -955,14 +846,6 @@ struct dm_odm_t {
 	/*  */
 	/*  ODM system resource. */
 	/*  */
-
-	/*  ODM relative time. */
-	struct timer_list PathDivSwitchTimer;
-	/* 2011.09.27 add for Path Diversity */
-	struct timer_list CCKPathDiversityTimer;
-	struct timer_list FastAntTrainingTimer;
-
-	/*  ODM relative workitem. */
 };	/*  DM_Dynamic_Mechanism_Structure */
 
 enum odm_rf_content {
@@ -1163,7 +1046,7 @@ bool ODM_RAStateCheck23a(struct dm_odm_t *pDM_Odm, s32 RSSI, bool bForceUpdate,
 
 #define dm_SWAW_RSSI_Check	ODM_SwAntDivChkPerPktRssi
 void ODM_SwAntDivChkPerPktRssi(struct dm_odm_t *pDM_Odm, u8 StationID,
-			       struct odm_phy_info *pPhyInfo);
+			       struct phy_info *pPhyInfo);
 
 u32 ConvertTo_dB23a(u32 Value);
 
@@ -1186,12 +1069,6 @@ void ODM_CmnInfoPtrArrayHook23a(struct dm_odm_t *pDM_Odm, enum odm_cmninfo	CmnIn
 
 void ODM_CmnInfoUpdate23a(struct dm_odm_t *pDM_Odm, u32 CmnInfo, u64 Value);
 
-void ODM_InitAllTimers(struct dm_odm_t *pDM_Odm);
-
-void ODM_CancelAllTimers(struct dm_odm_t *pDM_Odm);
-
-void ODM_ReleaseAllTimers(struct dm_odm_t *pDM_Odm);
-
 void ODM_ResetIQKResult(struct dm_odm_t *pDM_Odm);
 
 void ODM_AntselStatistics_88C(struct dm_odm_t *pDM_Odm, u8 MacId, u32 PWDBAll, bool isCCKrate);
diff --git a/drivers/staging/rtl8723au/include/odm_HWConfig.h b/drivers/staging/rtl8723au/include/odm_HWConfig.h
index 147855c96ad..6b98e05d046 100644
--- a/drivers/staging/rtl8723au/include/odm_HWConfig.h
+++ b/drivers/staging/rtl8723au/include/odm_HWConfig.h
@@ -147,7 +147,7 @@ void odm_Init_RSSIForDM23a(struct dm_odm_t *pDM_Odm);
 void
 ODM_PhyStatusQuery23a(
 	struct dm_odm_t *pDM_Odm,
-	struct odm_phy_info *pPhyInfo,
+	struct phy_info *pPhyInfo,
 	u8 *						pPhyStatus,
 	struct odm_packet_info *pPktinfo
 	);
@@ -160,15 +160,13 @@ void ODM_MacStatusQuery23a(struct dm_odm_t *pDM_Odm,
 	bool bPacketBeacon
 );
 
-enum hal_status ODM_ConfigRFWithHeaderFile23a(struct dm_odm_t *pDM_Odm,
-	enum RF_RADIO_PATH	Content,
-	enum RF_RADIO_PATH	eRFPath
-);
+int ODM_ConfigRFWithHeaderFile23a(struct dm_odm_t *pDM_Odm,
+				  enum RF_RADIO_PATH Content,
+				  enum RF_RADIO_PATH eRFPath);
 
-enum hal_status ODM_ConfigBBWithHeaderFile23a(struct dm_odm_t *pDM_Odm,
-	enum odm_bb_config_type		ConfigType
-);
+int ODM_ConfigBBWithHeaderFile23a(struct dm_odm_t *pDM_Odm,
+				  enum odm_bb_config_type ConfigType);
 
-enum hal_status ODM_ConfigMACWithHeaderFile23a(struct dm_odm_t *pDM_Odm);
+int ODM_ConfigMACWithHeaderFile23a(struct dm_odm_t *pDM_Odm);
 
 #endif
diff --git a/drivers/staging/rtl8723au/include/odm_debug.h b/drivers/staging/rtl8723au/include/odm_debug.h
index 5bc51d09e52..4d935a33ccb 100644
--- a/drivers/staging/rtl8723au/include/odm_debug.h
+++ b/drivers/staging/rtl8723au/include/odm_debug.h
@@ -65,30 +65,30 @@
 /*  */
 /*  */
 /* BB Functions */
-#define ODM_COMP_DIG				BIT0
-#define ODM_COMP_RA_MASK			BIT1
-#define ODM_COMP_DYNAMIC_TXPWR			BIT2
-#define ODM_COMP_FA_CNT				BIT3
-#define ODM_COMP_RSSI_MONITOR			BIT4
-#define ODM_COMP_CCK_PD				BIT5
-#define ODM_COMP_ANT_DIV			BIT6
-#define ODM_COMP_PWR_SAVE			BIT7
-#define ODM_COMP_PWR_TRAIN			BIT8
-#define ODM_COMP_RATE_ADAPTIVE			BIT9
-#define ODM_COMP_PATH_DIV			BIT10
-#define ODM_COMP_PSD				BIT11
-#define ODM_COMP_DYNAMIC_PRICCA			BIT12
-#define ODM_COMP_RXHP				BIT13
+#define ODM_COMP_DIG				BIT(0)
+#define ODM_COMP_RA_MASK			BIT(1)
+#define ODM_COMP_DYNAMIC_TXPWR			BIT(2)
+#define ODM_COMP_FA_CNT				BIT(3)
+#define ODM_COMP_RSSI_MONITOR			BIT(4)
+#define ODM_COMP_CCK_PD				BIT(5)
+#define ODM_COMP_ANT_DIV			BIT(6)
+#define ODM_COMP_PWR_SAVE			BIT(7)
+#define ODM_COMP_PWR_TRAIN			BIT(8)
+#define ODM_COMP_RATE_ADAPTIVE			BIT(9)
+#define ODM_COMP_PATH_DIV			BIT(10)
+#define ODM_COMP_PSD				BIT(11)
+#define ODM_COMP_DYNAMIC_PRICCA			BIT(12)
+#define ODM_COMP_RXHP				BIT(13)
 /* MAC Functions */
-#define ODM_COMP_EDCA_TURBO			BIT16
-#define ODM_COMP_EARLY_MODE			BIT17
+#define ODM_COMP_EDCA_TURBO			BIT(16)
+#define ODM_COMP_EARLY_MODE			BIT(17)
 /* RF Functions */
-#define ODM_COMP_TX_PWR_TRACK			BIT24
-#define ODM_COMP_RX_GAIN_TRACK			BIT25
-#define ODM_COMP_CALIBRATION			BIT26
+#define ODM_COMP_TX_PWR_TRACK			BIT(24)
+#define ODM_COMP_RX_GAIN_TRACK			BIT(25)
+#define ODM_COMP_CALIBRATION			BIT(26)
 /* Common Functions */
-#define ODM_COMP_COMMON				BIT30
-#define ODM_COMP_INIT				BIT31
+#define ODM_COMP_COMMON				BIT(30)
+#define ODM_COMP_INIT				BIT(31)
 
 /*------------------------Export Macro Definition---------------------------*/
 	#define DbgPrint	printk
diff --git a/drivers/staging/rtl8723au/include/odm_interface.h b/drivers/staging/rtl8723au/include/odm_interface.h
index f216b5846f9..c260d6bd8ff 100644
--- a/drivers/staging/rtl8723au/include/odm_interface.h
+++ b/drivers/staging/rtl8723au/include/odm_interface.h
@@ -18,22 +18,6 @@
 #define __ODM_INTERFACE_H__
 
 
-
-/*  */
-/*  =========== Constant/Structure/Enum/... Define */
-/*  */
-
-
-
-/*  */
-/*  =========== Macro Define */
-/*  */
-
-#define _reg_all(_name)			ODM_##_name
-#define _reg_ic(_name, _ic)		ODM_##_name##_ic
-#define _bit_all(_name)			BIT_##_name
-#define _bit_ic(_name, _ic)		BIT_##_name##_ic
-
 /*  _cat: implemented by Token-Pasting Operator. */
 
 /*===================================
@@ -68,64 +52,23 @@ ODM_REG(DIG,_pDM_Odm)
 typedef void (*RT_WORKITEM_CALL_BACK)(struct work_struct *pContext);
 
 /*  */
-/*  =========== Extern Variable ??? It should be forbidden. */
-/*  */
-
-
-/*  */
 /*  =========== EXtern Function Prototype */
 /*  */
 
 
 u8 ODM_Read1Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr);
-
 u16 ODM_Read2Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr);
-
 u32 ODM_Read4Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr);
-
 void ODM_Write1Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr, u8 Data);
-
 void ODM_Write2Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr, u16 Data);
-
 void ODM_Write4Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr, u32 Data);
-
 void ODM_SetMACReg(struct dm_odm_t *pDM_Odm, u32 RegAddr, u32 BitMask, u32 Data);
-
 u32 ODM_GetMACReg(struct dm_odm_t *pDM_Odm, u32 RegAddr, u32 BitMask);
-
 void ODM_SetBBReg(struct dm_odm_t *pDM_Odm, u32 RegAddr, u32 BitMask, u32 Data);
-
 u32 ODM_GetBBReg(struct dm_odm_t *pDM_Odm, u32 RegAddr, u32 BitMask);
-
 void ODM_SetRFReg(struct dm_odm_t *pDM_Odm, enum RF_RADIO_PATH eRFPath,
 		  u32 RegAddr, u32 BitMask, u32 Data);
-
 u32 ODM_GetRFReg(struct dm_odm_t *pDM_Odm, enum RF_RADIO_PATH eRFPath,
 		 u32 RegAddr, u32 BitMask);
 
-/*  Memory Relative Function. */
-void ODM_AllocateMemory(struct dm_odm_t *pDM_Odm, void **pPtr, u32 length);
-void ODM_FreeMemory(struct dm_odm_t *pDM_Odm, void *pPtr, u32 length);
-
-s32 ODM_CompareMemory(struct dm_odm_t *pDM_Odm, void *pBuf1, void *pBuf2, u32 length);
-
-/*  ODM MISC-spin lock relative API. */
-void ODM_AcquireSpinLock(struct dm_odm_t *pDM_Odm, enum rt_spinlock_type type);
-
-void ODM_ReleaseSpinLock(struct dm_odm_t *pDM_Odm, enum rt_spinlock_type type);
-
-/*  ODM MISC-workitem relative API. */
-void ODM_InitializeWorkItem(struct dm_odm_t *pDM_Odm, void *pRtWorkItem,
-			    RT_WORKITEM_CALL_BACK RtWorkItemCallback, void *pContext, const char *szID);
-
-/*  ODM Timer relative API. */
-void ODM_SetTimer(struct dm_odm_t *pDM_Odm, struct timer_list *pTimer, u32 msDelay);
-
-void ODM_ReleaseTimer(struct dm_odm_t *pDM_Odm, struct timer_list *pTimer);
-
-/*  ODM FW relative API. */
-u32 ODM_FillH2CCmd(u8 *pH2CBuffer, u32 H2CBufferLen, u32 CmdNum,
-		   u32 *pElementID, u32 *pCmdLen, u8 **pCmbBuffer,
-		   u8 *CmdStartSeq);
-
 #endif	/*  __ODM_INTERFACE_H__ */
diff --git a/drivers/staging/rtl8723au/include/odm_precomp.h b/drivers/staging/rtl8723au/include/odm_precomp.h
index f3fc2fad988..69de888d884 100644
--- a/drivers/staging/rtl8723au/include/odm_precomp.h
+++ b/drivers/staging/rtl8723au/include/odm_precomp.h
@@ -16,10 +16,6 @@
 #ifndef	__ODM_PRECOMP_H__
 #define __ODM_PRECOMP_H__
 
-#include "odm_types.h"
-
-#define		TEST_FALG___		1
-
 /* 2 Config Flags and Structs - defined by each ODM Type */
 
 #include <osdep_service.h>
diff --git a/drivers/staging/rtl8723au/include/odm_reg.h b/drivers/staging/rtl8723au/include/odm_reg.h
index 56191e9fdcd..c18433120fe 100644
--- a/drivers/staging/rtl8723au/include/odm_reg.h
+++ b/drivers/staging/rtl8723au/include/odm_reg.h
@@ -102,13 +102,10 @@
 #define	PATHDIV_REG					0xB30
 #define	PATHDIV_TRI					0xBA0
 
-
 /*  */
 /*  Bitmap Definition */
 /*  */
 
-#define	BIT_FA_RESET					BIT0
-
-
+#define	BIT_FA_RESET					BIT(0)
 
 #endif
diff --git a/drivers/staging/rtl8723au/include/odm_types.h b/drivers/staging/rtl8723au/include/odm_types.h
deleted file mode 100644
index a866769ea17..00000000000
--- a/drivers/staging/rtl8723au/include/odm_types.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.
- *
- ******************************************************************************/
-#ifndef __ODM_TYPES_H__
-#define __ODM_TYPES_H__
-
-/*  Define Different SW team support */
-
-enum hal_status {
-	HAL_STATUS_SUCCESS,
-	HAL_STATUS_FAILURE,
-};
-
-enum rt_spinlock_type {
-	RT_TEMP =1,
-};
-
-#define SET_TX_DESC_ANTSEL_A_88E(__pTxDesc, __Value)		\
-	SET_BITS_TO_LE_4BYTE(__pTxDesc+8, 24, 1, __Value)
-#define SET_TX_DESC_ANTSEL_B_88E(__pTxDesc, __Value)		\
-	SET_BITS_TO_LE_4BYTE(__pTxDesc+8, 25, 1, __Value)
-#define SET_TX_DESC_ANTSEL_C_88E(__pTxDesc, __Value)		\
-	SET_BITS_TO_LE_4BYTE(__pTxDesc+28, 29, 1, __Value)
-
-#endif /*  __ODM_TYPES_H__ */
diff --git a/drivers/staging/rtl8723au/include/osdep_intf.h b/drivers/staging/rtl8723au/include/osdep_intf.h
index b603cf53290..33afa62f31b 100644
--- a/drivers/staging/rtl8723au/include/osdep_intf.h
+++ b/drivers/staging/rtl8723au/include/osdep_intf.h
@@ -22,12 +22,10 @@
 int rtw_hw_suspend23a(struct rtw_adapter *padapter);
 int rtw_hw_resume23a(struct rtw_adapter *padapter);
 
-u8 rtw_init_drv_sw23a(struct rtw_adapter *padapter);
-u8 rtw_free_drv_sw23a(struct rtw_adapter *padapter);
-u8 rtw_reset_drv_sw23a(struct rtw_adapter *padapter);
+int rtw_init_drv_sw23a(struct rtw_adapter *padapter);
+int rtw_free_drv_sw23a(struct rtw_adapter *padapter);
+int rtw_reset_drv_sw23a(struct rtw_adapter *padapter);
 
-u32 rtw_start_drv_threads23a(struct rtw_adapter *padapter);
-void rtw_stop_drv_threads23a (struct rtw_adapter *padapter);
 void rtw_cancel_all_timer23a(struct rtw_adapter *padapter);
 
 int rtw_init_netdev23a_name23a(struct net_device *pnetdev, const char *ifname);
@@ -43,4 +41,8 @@ void rtw_ips_pwr_down23a(struct rtw_adapter *padapter);
 int rtw_drv_register_netdev(struct rtw_adapter *padapter);
 void rtw_ndev_destructor(struct net_device *ndev);
 
+int rtl8723au_inirp_init(struct rtw_adapter *Adapter);
+int rtl8723au_inirp_deinit(struct rtw_adapter *Adapter);
+void rtl8723a_usb_intf_stop(struct rtw_adapter *padapter);
+
 #endif	/* _OSDEP_INTF_H_ */
diff --git a/drivers/staging/rtl8723au/include/osdep_service.h b/drivers/staging/rtl8723au/include/osdep_service.h
index 039bc7285ed..a5ebdb81ce3 100644
--- a/drivers/staging/rtl8723au/include/osdep_service.h
+++ b/drivers/staging/rtl8723au/include/osdep_service.h
@@ -19,7 +19,6 @@
 #define _SUCCESS	1
 #define RTW_RX_HANDLED 2
 
-#include <linux/version.h>
 #include <linux/spinlock.h>
 #include <linux/compiler.h>
 #include <linux/kernel.h>
@@ -30,7 +29,6 @@
 #include <linux/kref.h>
 #include <linux/netdevice.h>
 #include <linux/skbuff.h>
-#include <linux/circ_buf.h>
 #include <asm/uaccess.h>
 #include <asm/byteorder.h>
 #include <asm/atomic.h>
@@ -40,26 +38,14 @@
 #include <linux/sched.h>
 #include <linux/etherdevice.h>
 #include <linux/wireless.h>
-#include <net/iw_handler.h>
 #include <linux/if_arp.h>
 #include <linux/rtnetlink.h>
 #include <linux/delay.h>
-#include <linux/proc_fs.h>	/*  Necessary because we use the proc fs */
 #include <linux/interrupt.h>	/*  for struct tasklet_struct */
 #include <linux/ip.h>
-#include <linux/kthread.h>
 
-
-/*	#include <linux/ieee80211.h> */
 #include <net/ieee80211_radiotap.h>
 #include <net/cfg80211.h>
-#include <linux/usb.h>
-#include <linux/usb/ch9.h>
-
-struct rtw_adapter;
-struct c2h_evt_hdr;
-
-typedef s32 (*c2h_id_filter)(u8 id);
 
 struct rtw_queue {
 	struct	list_head	queue;
@@ -79,9 +65,6 @@ static inline int rtw_netif_queue_stopped(struct net_device *pnetdev)
 		netif_tx_queue_stopped(netdev_get_tx_queue(pnetdev, 3)) );
 }
 
-#ifndef BIT
-#define BIT(x)	( 1 << (x))
-#endif
 static inline u32 CHKBIT(u32 x)
 {
 	WARN_ON(x >= 32);
@@ -90,118 +73,16 @@ static inline u32 CHKBIT(u32 x)
 	return BIT(x);
 }
 
-#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 BIT32	0x0100000000
-#define BIT33	0x0200000000
-#define BIT34	0x0400000000
-#define BIT35	0x0800000000
-#define BIT36	0x1000000000
-
-int RTW_STATUS_CODE23a(int error_code);
-
-u8*	_rtw_vmalloc(u32 sz);
-u8*	_rtw_zvmalloc(u32 sz);
-void	_rtw_vmfree(u8 *pbuf, u32 sz);
-#define rtw_vmalloc(sz)			_rtw_vmalloc((sz))
-#define rtw_zvmalloc(sz)			_rtw_zvmalloc((sz))
-#define rtw_vmfree(pbuf, sz)		_rtw_vmfree((pbuf), (sz))
-
-extern unsigned char REALTEK_96B_IE23A[];
 extern unsigned char MCS_rate_2R23A[16];
-extern unsigned char RTW_WPA_OUI23A[];
-extern unsigned char WPA_TKIP_CIPHER23A[4];
-extern unsigned char RSN_TKIP_CIPHER23A[4];
 
 extern unsigned char	MCS_rate_2R23A[16];
 extern unsigned char	MCS_rate_1R23A[16];
 
 void	_rtw_init_queue23a(struct rtw_queue *pqueue);
-u32	_rtw_queue_empty23a(struct rtw_queue *pqueue);
-
-static inline u32 bitshift(u32 bitmask)
-{
-	u32 i;
-
-	for (i = 0; i <= 31; i++)
-		if (((bitmask>>i) &  0x1) == 1) break;
-
-	return i;
-}
-
-void rtw_suspend_lock_init(void);
-void rtw_suspend_lock_uninit(void);
-void rtw_lock_suspend(void);
-void rtw_unlock_suspend(void);
-
-
-#define NDEV_FMT "%s"
-#define NDEV_ARG(ndev) ndev->name
-#define ADPT_FMT "%s"
-#define ADPT_ARG(adapter) adapter->pnetdev->name
-#define FUNC_NDEV_FMT "%s(%s)"
-#define FUNC_NDEV_ARG(ndev) __func__, ndev->name
-#define FUNC_ADPT_FMT "%s(%s)"
-#define FUNC_ADPT_ARG(adapter) __func__, adapter->pnetdev->name
-
-#define rtw_signal_process(pid, sig) kill_pid(find_vpid((pid)),(sig), 1)
-
-u64 rtw_modular6423a(u64 x, u64 y);
-u64 rtw_division6423a(u64 x, u64 y);
-
 
 /* Macros for handling unaligned memory accesses */
 
 #define RTW_GET_BE24(a) ((((u32) (a)[0]) << 16) | (((u32) (a)[1]) << 8) | \
 			 ((u32) (a)[2]))
 
-
-struct rtw_cbuf {
-	u32 write;
-	u32 read;
-	u32 size;
-	void *bufs[0];
-};
-
-bool rtw_cbuf_full23a(struct rtw_cbuf *cbuf);
-bool rtw_cbuf_empty23a(struct rtw_cbuf *cbuf);
-bool rtw_cbuf_push23a(struct rtw_cbuf *cbuf, void *buf);
-void *rtw_cbuf_pop23a(struct rtw_cbuf *cbuf);
-struct rtw_cbuf *rtw_cbuf_alloc23a(u32 size);
-void rtw_cbuf_free(struct rtw_cbuf *cbuf);
-int rtw_change_ifname(struct rtw_adapter *padapter, const char *ifname);
-s32 c2h_evt_hdl(struct rtw_adapter *adapter, struct c2h_evt_hdr *c2h_evt, c2h_id_filter filter);
-void indicate_wx_scan_complete_event(struct rtw_adapter *padapter);
-u8 rtw_do_join23a(struct rtw_adapter *padapter);
-
 #endif
diff --git a/drivers/staging/rtl8723au/include/recv_osdep.h b/drivers/staging/rtl8723au/include/recv_osdep.h
index 15c94b6168b..c2d3f1bd594 100644
--- a/drivers/staging/rtl8723au/include/recv_osdep.h
+++ b/drivers/staging/rtl8723au/include/recv_osdep.h
@@ -23,22 +23,13 @@ void _rtw_free_recv_priv23a (struct recv_priv *precvpriv);
 
 int rtw_recv_entry23a(struct recv_frame *precv_frame);
 int rtw_recv_indicatepkt23a(struct rtw_adapter *adapter, struct recv_frame *precv_frame);
-void rtw_recv_returnpacket(struct net_device *cnxt, struct sk_buff *preturnedpkt);
 
-void rtw_hostapd_mlme_rx23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
 void rtw_handle_tkip_mic_err23a(struct rtw_adapter *padapter, u8 bgroup);
 
 int	rtw_init_recv_priv(struct recv_priv *precvpriv, struct rtw_adapter *padapter);
 void rtw_free_recv_priv (struct recv_priv *precvpriv);
 
 int rtw_os_recv_resource_init(struct recv_priv *precvpriv, struct rtw_adapter *padapter);
-int rtw_os_recv_resource_alloc23a(struct rtw_adapter *padapter, struct recv_frame *precvframe);
-void rtw_os_recv_resource_free(struct recv_priv *precvpriv);
-
-int rtw_os_recvbuf_resource_alloc23a(struct rtw_adapter *padapter, struct recv_buf *precvbuf);
-int rtw_os_recvbuf_resource_free23a(struct rtw_adapter *padapter, struct recv_buf *precvbuf);
-
-void rtw_os_read_port23a(struct rtw_adapter *padapter, struct recv_buf *precvbuf);
 
 void rtw_init_recv_timer23a(struct recv_reorder_ctrl *preorder_ctrl);
 
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_bt-coexist.h b/drivers/staging/rtl8723au/include/rtl8723a_bt-coexist.h
index 6d1edc6ef84..05069652bae 100644
--- a/drivers/staging/rtl8723au/include/rtl8723a_bt-coexist.h
+++ b/drivers/staging/rtl8723au/include/rtl8723a_bt-coexist.h
@@ -19,19 +19,6 @@
 #include "odm_precomp.h"
 
 
-#define __BT_C__ 1
-#define __BT_HANDLEPACKET_C__ 1
-#define __BT_HCI_C__ 1
-#define __HALBTC87231ANT_C__ 1
-#define __HALBTC87232ANT_C__ 1
-#define __HALBTC8723_C__ 1
-#define __HALBTCCSR1ANT_C__ 1
-#define __HALBTCCSR2ANT_C__ 1
-#define __HALBTCOEXIST_C__ 1
-#define __HALBT_C__ 1
-
-#ifdef __BT_C__ /*  COMMON/BT.h */
-
 /*  HEADER/PlatformDef.h */
 enum rt_media_status {
 	RT_MEDIA_DISCONNECT	= 0,
@@ -44,20 +31,12 @@ enum rt_media_status {
 
 void BT_SignalCompensation(struct rtw_adapter *padapter,
 			   u8 *rssi_wifi, u8 *rssi_bt);
-void BT_WifiScanNotify(struct rtw_adapter *padapter, u8 scanType);
-void BT_WifiAssociateNotify(struct rtw_adapter *padapter, u8 action);
-void BT_WifiMediaStatusNotify(struct rtw_adapter *padapter,
-			      enum rt_media_status mstatus);
-void BT_SpecialPacketNotify(struct rtw_adapter * padapter);
 void BT_HaltProcess(struct rtw_adapter * padapter);
 void BT_LpsLeave(struct rtw_adapter * padapter);
 
 
 #define	BT_HsConnectionEstablished(Adapter)		false
 /*  ===== End of sync from SD7 driver COMMON/BT.h ===== */
-#endif /*  __BT_C__ */
-
-#ifdef __BT_HCI_C__ /*  COMMON/bt_hci.h */
 
 /*  HEADER/SecurityType.h */
 #define TKIP_ENC_KEY_POS		32		/* KEK_LEN+KEK_LEN) */
@@ -1107,15 +1086,9 @@ enum hci_ext_bp_operation {
 	HCI_BT_OP_MAX
 };
 
-/*	Function proto type */
-struct btdata_entry {
-	struct list_head	List;
-	void			*pDataBlock;
-};
-
 #define BTHCI_SM_WITH_INFO(_Adapter, _StateToEnter, _StateCmd, _EntryNum)	\
 {										\
-	RTPRINT(FIOCTL, IOCTL_STATE, ("[BT state change] caused by ""%s"", line =%d\n", __FUNCTION__, __LINE__));							\
+	RTPRINT(FIOCTL, IOCTL_STATE, ("[BT state change] caused by ""%s"", line =%d\n", __func__, __LINE__));							\
 	BTHCI_StateMachine(_Adapter, _StateToEnter, _StateCmd, _EntryNum);\
 }
 
@@ -1132,9 +1105,7 @@ void BTHCI_DisconnectAll(struct rtw_adapter * padapter);
 enum hci_status BTHCI_HandleHCICMD(struct rtw_adapter * padapter, struct packet_irp_hcicmd_data *pHciCmd);
 
 /*  ===== End of sync from SD7 driver COMMON/bt_hci.h ===== */
-#endif /*  __BT_HCI_C__ */
 
-#ifdef __HALBTC87231ANT_C__ /*  HAL/BTCoexist/HalBtc87231Ant.h */
 /*  ===== Below this line is sync from SD7 driver HAL/BTCoexist/HalBtc87231Ant.h ===== */
 #define GET_BT_INFO(padapter)	(&GET_HAL_DATA(padapter)->BtInfo)
 
@@ -1191,9 +1162,7 @@ void BTDM_1AntForDhcp(struct rtw_adapter * padapter);
 void BTDM_1AntBtCoexist8723A(struct rtw_adapter * padapter);
 
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtc87231Ant.h ===== */
-#endif /*  __HALBTC87231ANT_C__ */
 
-#ifdef __HALBTC87232ANT_C__ /*  HAL/BTCoexist/HalBtc87232Ant.h */
 /*  ===== Below this line is sync from SD7 driver HAL/BTCoexist/HalBtc87232Ant.h ===== */
 enum bt_2ant_bt_status {
 	BT_2ANT_BT_STATUS_IDLE			= 0x0,
@@ -1274,9 +1243,7 @@ struct btdm_8723a_2ant {
 
 void BTDM_2AntBtCoexist8723A(struct rtw_adapter * padapter);
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtc87232Ant.h ===== */
-#endif /*  __HALBTC87232ANT_C__ */
 
-#ifdef __HALBTC8723_C__ /*  HAL/BTCoexist/HalBtc8723.h */
 /*  ===== Below this line is sync from SD7 driver HAL/BTCoexist/HalBtc8723.h ===== */
 
 #define	BT_Q_PKT_OFF		0
@@ -1352,13 +1319,9 @@ void BTDM_SetSwPenaltyTxRateAdaptive(struct rtw_adapter * padapter, u8 raType);
 void BTDM_SetFwDecBtPwr(struct rtw_adapter * padapter, u8 bDecBtPwr);
 u8 BTDM_BtProfileSupport(struct rtw_adapter * padapter);
 void BTDM_LpsLeave(struct rtw_adapter * padapter);
-u8 BTDM_1Ant8723A(struct rtw_adapter * padapter);
-#define BT_1Ant BTDM_1Ant8723A
 
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtc8723.h ===== */
-#endif /*  __HALBTC8723_C__ */
 
-#ifdef __HALBTCCSR1ANT_C__ /*  HAL/BTCoexist/HalBtcCsr1Ant.h */
 /*  ===== Below this line is sync from SD7 driver HAL/BTCoexist/HalBtcCsr1Ant.h ===== */
 
 enum BT_A2DP_INDEX{
@@ -1381,19 +1344,17 @@ void BTDM_SingleAnt(struct rtw_adapter * padapter, u8 bSingleAntOn, u8 bInterrup
 void BTDM_CheckBTIdleChange1Ant(struct rtw_adapter * padapter);
 
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtcCsr1Ant.h ===== */
-#endif /*  __HALBTCCSR1ANT_C__ */
 
-#ifdef __HALBTCCSR2ANT_C__ /*  HAL/BTCoexist/HalBtcCsr2Ant.h */
 /*  ===== Below this line is sync from SD7 driver HAL/BTCoexist/HalBtcCsr2Ant.h ===== */
 
 /*  */
 /*  For old core stack before v251 */
 /*  */
-#define BT_RSSI_STATE_NORMAL_POWER	BIT0
-#define BT_RSSI_STATE_AMDPU_OFF		BIT1
-#define BT_RSSI_STATE_SPECIAL_LOW	BIT2
-#define BT_RSSI_STATE_BG_EDCA_LOW	BIT3
-#define BT_RSSI_STATE_TXPOWER_LOW	BIT4
+#define BT_RSSI_STATE_NORMAL_POWER	BIT(0)
+#define BT_RSSI_STATE_AMDPU_OFF		BIT(1)
+#define BT_RSSI_STATE_SPECIAL_LOW	BIT(2)
+#define BT_RSSI_STATE_BG_EDCA_LOW	BIT(3)
+#define BT_RSSI_STATE_TXPOWER_LOW	BIT(4)
 
 #define	BT_DACSWING_OFF				0
 #define	BT_DACSWING_M4				1
@@ -1403,9 +1364,6 @@ void BTDM_CheckBTIdleChange1Ant(struct rtw_adapter * padapter);
 void BTDM_DiminishWiFi(struct rtw_adapter * Adapter, u8 bDACOn, u8 bInterruptOn, u8 DACSwingLevel, u8 bNAVOn);
 
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtcCsr2Ant.h ===== */
-#endif /*  __HALBTCCSR2ANT_C__ */
-
-#ifdef __HALBTCOEXIST_C__ /*  HAL/BTCoexist/HalBtCoexist.h */
 
 /*  HEADER/TypeDef.h */
 #define MAX_FW_SUPPORT_MACID_NUM			64
@@ -1440,61 +1398,61 @@ void BTDM_DiminishWiFi(struct rtw_adapter * Adapter, u8 bDACOn, u8 bInterruptOn,
 #define BT_FW_COEX_THRESH_55				55
 #define BT_FW_COEX_THRESH_65				65
 
-#define BT_COEX_STATE_BT30					BIT(0)
+#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_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_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_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_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)
 /*  */
 /*  Todo: Remove these definitions */
-#define BT_COEX_STATE_BT_PAN_IDLE			BIT(15)
+#define BT_COEX_STATE_BT_PAN_IDLE		BIT(15)
 #define BT_COEX_STATE_BT_PAN_UPLINK		BIT(16)
-#define BT_COEX_STATE_BT_PAN_DOWNLINK	BIT(17)
+#define BT_COEX_STATE_BT_PAN_DOWNLINK		BIT(17)
 #define BT_COEX_STATE_BT_A2DP_IDLE		BIT(18)
 /*  */
-#define BT_COEX_STATE_BT_RSSI_LOW			BIT(19)
+#define BT_COEX_STATE_BT_RSSI_LOW		BIT(19)
 
-#define BT_COEX_STATE_PROFILE_HID			BIT(20)
+#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_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_WIFI_RSSI_BEACON_LOW		BIT(27)
-#define BT_COEX_STATE_WIFI_RSSI_BEACON_MEDIUM		BIT(28)
-#define BT_COEX_STATE_WIFI_RSSI_BEACON_HIGH		BIT(29)
+#define BT_COEX_STATE_WIFI_RSSI_BEACON_LOW	BIT(27)
+#define BT_COEX_STATE_WIFI_RSSI_BEACON_MEDIUM	BIT(28)
+#define BT_COEX_STATE_WIFI_RSSI_BEACON_HIGH	BIT(29)
 
 
-#define BT_COEX_STATE_BTINFO_COMMON				BIT30
-#define BT_COEX_STATE_BTINFO_B_HID_SCOESCO		BIT31
-#define BT_COEX_STATE_BTINFO_B_FTP_A2DP			BIT32
+#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(32)
 
-#define BT_COEX_STATE_BT_CNT_LEVEL_0				BIT33
-#define BT_COEX_STATE_BT_CNT_LEVEL_1				BIT34
-#define BT_COEX_STATE_BT_CNT_LEVEL_2				BIT35
-#define BT_COEX_STATE_BT_CNT_LEVEL_3				BIT36
+#define BT_COEX_STATE_BT_CNT_LEVEL_0		BIT(33)
+#define BT_COEX_STATE_BT_CNT_LEVEL_1		BIT(34)
+#define BT_COEX_STATE_BT_CNT_LEVEL_2		BIT(35)
+#define BT_COEX_STATE_BT_CNT_LEVEL_3		BIT(36)
 
-#define BT_RSSI_STATE_HIGH					0
-#define BT_RSSI_STATE_MEDIUM				1
-#define BT_RSSI_STATE_LOW					2
+#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_MEDIUM		4
 #define BT_RSSI_STATE_STAY_LOW			5
 
 #define	BT_AGCTABLE_OFF				0
@@ -1579,8 +1537,6 @@ struct bt_coexist_str {
 void BTDM_CheckAntSelMode(struct rtw_adapter * padapter);
 void BTDM_FwC2hBtRssi(struct rtw_adapter * padapter, u8 *tmpBuf);
 #define BT_FwC2hBtRssi BTDM_FwC2hBtRssi
-void BTDM_FwC2hBtInfo(struct rtw_adapter * padapter, u8 *tmpBuf, u8 length);
-#define BT_FwC2hBtInfo BTDM_FwC2hBtInfo
 void BTDM_DisplayBtCoexInfo(struct rtw_adapter * padapter);
 #define BT_DisplayBtCoexInfo BTDM_DisplayBtCoexInfo
 void BTDM_RejectAPAggregatedPacket(struct rtw_adapter * padapter, u8 bReject);
@@ -1591,8 +1547,6 @@ s32 BTDM_GetRxSS(struct rtw_adapter * padapter);
 u8 BTDM_CheckCoexBcnRssiState(struct rtw_adapter * padapter, u8 levelNum, u8 RssiThresh, u8 RssiThresh1);
 u8 BTDM_CheckCoexRSSIState1(struct rtw_adapter * padapter, u8 levelNum, u8 RssiThresh, u8 RssiThresh1);
 u8 BTDM_CheckCoexRSSIState(struct rtw_adapter * padapter, u8 levelNum, u8 RssiThresh, u8 RssiThresh1);
-u8 BTDM_DisableEDCATurbo(struct rtw_adapter * padapter);
-#define BT_DisableEDCATurbo BTDM_DisableEDCATurbo
 void BTDM_Balance(struct rtw_adapter * padapter, u8 bBalanceOn, u8 ms0, u8 ms1);
 void BTDM_AGCTable(struct rtw_adapter * padapter, u8 type);
 void BTDM_BBBackOffLevel(struct rtw_adapter * padapter, u8 type);
@@ -1602,8 +1556,6 @@ void BTDM_HWCoexAllOff(struct rtw_adapter * padapter);
 void BTDM_CoexAllOff(struct rtw_adapter * padapter);
 void BTDM_TurnOffBtCoexistBeforeEnterIPS(struct rtw_adapter * padapter);
 void BTDM_SignalCompensation(struct rtw_adapter * padapter, u8 *rssi_wifi, u8 *rssi_bt);
-void BTDM_Coexist(struct rtw_adapter * padapter);
-#define BT_CoexistMechanism BTDM_Coexist
 void BTDM_UpdateCoexState(struct rtw_adapter * padapter);
 u8 BTDM_IsSameCoexistState(struct rtw_adapter * padapter);
 void BTDM_PWDBMonitor(struct rtw_adapter * padapter);
@@ -1632,15 +1584,11 @@ u8 BTDM_IsActionPAN(struct rtw_adapter * padapter);
 u8 BTDM_IsActionHIDA2DP(struct rtw_adapter * padapter);
 u8 BTDM_IsActionHIDPAN(struct rtw_adapter * padapter);
 u8 BTDM_IsActionPANA2DP(struct rtw_adapter * padapter);
-u8 BTDM_IsBtDisabled(struct rtw_adapter * padapter);
-#define BT_IsBtDisabled BTDM_IsBtDisabled
 u32 BTDM_BtTxRxCounterH(struct rtw_adapter * padapter);
 u32 BTDM_BtTxRxCounterL(struct rtw_adapter * padapter);
 
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtCoexist.h ===== */
-#endif /*  __HALBTCOEXIST_C__ */
 
-#ifdef __HALBT_C__ /*  HAL/HalBT.h */
 /*  ===== Below this line is sync from SD7 driver HAL/HalBT.h ===== */
 
 #define RTS_CTS_NO_LEN_LIMIT	0
@@ -1649,18 +1597,12 @@ u8 HALBT_GetPGAntNum(struct rtw_adapter * padapter);
 #define BT_GetPGAntNum HALBT_GetPGAntNum
 void HALBT_SetKey(struct rtw_adapter * padapter, u8 EntryNum);
 void HALBT_RemoveKey(struct rtw_adapter * padapter, u8 EntryNum);
-void HALBT_InitBTVars8723A(struct rtw_adapter * padapter);
-#define HALBT_InitHalVars HALBT_InitBTVars8723A
-#define BT_InitHalVars HALBT_InitHalVars
 u8 HALBT_IsBTExist(struct rtw_adapter * padapter);
 #define BT_IsBtExist HALBT_IsBTExist
 u8 HALBT_BTChipType(struct rtw_adapter * padapter);
-void HALBT_InitHwConfig(struct rtw_adapter * padapter);
-#define BT_InitHwConfig HALBT_InitHwConfig
 void HALBT_SetRtsCtsNoLenLimit(struct rtw_adapter * padapter);
 
 /*  ===== End of sync from SD7 driver HAL/HalBT.c ===== */
-#endif /*  __HALBT_C__ */
 
 #define _bt_dbg_off_		0
 #define _bt_dbg_on_		1
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_bt_intf.h b/drivers/staging/rtl8723au/include/rtl8723a_bt_intf.h
new file mode 100644
index 00000000000..4733559970e
--- /dev/null
+++ b/drivers/staging/rtl8723au/include/rtl8723a_bt_intf.h
@@ -0,0 +1,69 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ * Copyright(c) 2014, Jes Sorensen <Jes.Sorensen@redhat.com>
+ *
+ * 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 __RTL8723A_BT_INTF_H__
+#define __RTL8723A_BT_INTF_H__
+
+#include <drv_types.h>
+
+#ifdef CONFIG_8723AU_BT_COEXIST
+enum rt_media_status;
+bool rtl8723a_BT_using_antenna_1(struct rtw_adapter *padapter);
+bool rtl8723a_BT_enabled(struct rtw_adapter *padapter);
+bool rtl8723a_BT_coexist(struct rtw_adapter *padapter);
+void rtl8723a_BT_do_coexist(struct rtw_adapter *padapter);
+void rtl8723a_BT_wifiscan_notify(struct rtw_adapter *padapter, u8 scanType);
+void rtl8723a_BT_mediastatus_notify(struct rtw_adapter *padapter,
+				    enum rt_media_status mstatus);
+void rtl8723a_BT_specialpacket_notify(struct rtw_adapter *padapter);
+void rtl8723a_BT_lps_leave(struct rtw_adapter *padapter);
+void rtl8723a_BT_disable_coexist(struct rtw_adapter *padapter);
+bool rtl8723a_BT_disable_EDCA_turbo(struct rtw_adapter *padapter);
+void rtl8723a_dual_antenna_detection(struct rtw_adapter *padapter);
+void rtl8723a_BT_init_hwconfig(struct rtw_adapter *padapter);
+void rtl8723a_BT_wifiassociate_notify(struct rtw_adapter *padapter, u8 action);
+void rtl8723a_BT_init_hal_vars(struct rtw_adapter *padapter);
+void rtl8723a_fw_c2h_BT_info(struct rtw_adapter *padapter, u8 *tmpBuf, u8 length);
+#else
+static inline bool rtl8723a_BT_using_antenna_1(struct rtw_adapter *padapter)
+{
+	return false;
+}
+static inline bool rtl8723a_BT_enabled(struct rtw_adapter *padapter)
+{
+	return false;
+}
+static inline bool rtl8723a_BT_coexist(struct rtw_adapter *padapter)
+{
+	return false;
+}
+#define rtl8723a_BT_do_coexist(padapter)			do {} while(0)
+#define rtl8723a_BT_wifiscan_notify(padapter, scanType)		do {} while(0)
+#define rtl8723a_BT_mediastatus_notify(padapter, mstatus)	do {} while(0)
+#define rtl8723a_BT_specialpacket_notify(padapter)		do {} while(0)
+#define rtl8723a_BT_lps_leave(padapter)				do {} while(0)
+#define rtl8723a_BT_disable_coexist(padapter)			do {} while(0)
+static inline bool rtl8723a_BT_disable_EDCA_turbo(struct rtw_adapter *padapter)
+{
+	return false;
+}
+#define rtl8723a_dual_antenna_detection(padapter)		do {} while(0)
+#define rtl8723a_BT_init_hwconfig(padapter)			do {} while(0)
+#define rtl8723a_BT_wifiassociate_notify(padapter, action)	do {} while(0)
+#define rtl8723a_BT_init_hal_vars(padapter)			do {} while(0)
+#define rtl8723a_fw_c2h_BT_info(padapter, tmpBuf, length)	do {} while(0)
+#endif
+
+#endif
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_cmd.h b/drivers/staging/rtl8723au/include/rtl8723a_cmd.h
index 8fccbfc3a45..900baccc3c2 100644
--- a/drivers/staging/rtl8723au/include/rtl8723a_cmd.h
+++ b/drivers/staging/rtl8723au/include/rtl8723a_cmd.h
@@ -142,18 +142,16 @@ struct lowpwr_lps_parm{
 
 
 /*  host message to firmware cmd */
-void rtl8723a_set_FwPwrMode_cmd(struct rtw_adapter * padapter, u8 Mode);
-void rtl8723a_set_FwJoinBssReport_cmd(struct rtw_adapter * padapter, u8 mstatus);
+void rtl8723a_set_FwPwrMode_cmd(struct rtw_adapter *padapter, u8 Mode);
+void rtl8723a_set_FwJoinBssReport_cmd(struct rtw_adapter *padapter, u8 mstatus);
 #ifdef CONFIG_8723AU_BT_COEXIST
-void rtl8723a_set_BTCoex_AP_mode_FwRsvdPkt_cmd(struct rtw_adapter * padapter);
+void rtl8723a_set_BTCoex_AP_mode_FwRsvdPkt_cmd(struct rtw_adapter *padapter);
+#else
+#define rtl8723a_set_BTCoex_AP_mode_FwRsvdPkt_cmd(padapter) do {} while(0)
 #endif
-u8 rtl8723a_set_rssi_cmd(struct rtw_adapter * padapter, u8 *param);
-u8 rtl8723a_set_raid_cmd(struct rtw_adapter * padapter, u32 mask, u8 arg);
-void rtl8723a_add_rateatid(struct rtw_adapter * padapter, u32 bitmap, u8 arg, u8 rssi_level);
-
-#ifdef CONFIG_8723AU_P2P
-void rtl8723a_set_p2p_ps_offload_cmd(struct rtw_adapter * padapter, u8 p2p_ps_state);
-#endif /* CONFIG_8723AU_P2P */
+int rtl8723a_set_rssi_cmd(struct rtw_adapter *padapter, u8 *param);
+int rtl8723a_set_raid_cmd(struct rtw_adapter *padapter, u32 mask, u8 arg);
+void rtl8723a_add_rateatid(struct rtw_adapter *padapter, u32 bitmap, u8 arg, u8 rssi_level);
 
 void CheckFwRsvdPageContent23a(struct rtw_adapter *padapter);
 
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_dm.h b/drivers/staging/rtl8723au/include/rtl8723a_dm.h
index 47e887f5bb2..ccac672c9c0 100644
--- a/drivers/staging/rtl8723au/include/rtl8723a_dm.h
+++ b/drivers/staging/rtl8723au/include/rtl8723a_dm.h
@@ -59,8 +59,6 @@ struct dm_priv
 	#endif
 
 	/* for High Power */
-	u8 bDynamicTxPowerEnable;
-	u8 LastDTPLvl;
 	u8 DynamicTxHighPowerLvl;/* Add by Jacken Tx Power Control for Near/Far Range 2008/03/06 */
 
 	/* for tx power tracking */
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_hal.h b/drivers/staging/rtl8723au/include/rtl8723a_hal.h
index c20248bab71..344b7085f87 100644
--- a/drivers/staging/rtl8723au/include/rtl8723a_hal.h
+++ b/drivers/staging/rtl8723au/include/rtl8723a_hal.h
@@ -20,6 +20,7 @@
 #include "Hal8723APhyReg.h"
 #include "Hal8723APhyCfg.h"
 #include "rtl8723a_rf.h"
+#include "rtl8723a_bt_intf.h"
 #ifdef CONFIG_8723AU_BT_COEXIST
 #include "rtl8723a_bt-coexist.h"
 #endif
@@ -29,25 +30,16 @@
 #include "rtl8723a_cmd.h"
 #include "rtl8723a_sreset.h"
 #include "rtw_efuse.h"
+#include "rtw_eeprom.h"
 
 #include "odm_precomp.h"
+#include "odm.h"
 
 
 /* 2TODO: We should define 8192S firmware related macro settings here!! */
 #define RTL819X_DEFAULT_RF_TYPE			RF_1T2R
 #define RTL819X_TOTAL_RF_PATH				2
 
-/* TODO:  The following need to check!! */
-#define RTL8723_FW_UMC_IMG				"rtl8192CU\\rtl8723fw.bin"
-#define RTL8723_FW_UMC_B_IMG			"rtl8192CU\\rtl8723fw_B.bin"
-#define RTL8723_PHY_REG					"rtl8723S\\PHY_REG_1T.txt"
-#define RTL8723_PHY_RADIO_A				"rtl8723S\\radio_a_1T.txt"
-#define RTL8723_PHY_RADIO_B				"rtl8723S\\radio_b_1T.txt"
-#define RTL8723_AGC_TAB					"rtl8723S\\AGC_TAB_1T.txt"
-#define RTL8723_PHY_MACREG				"rtl8723S\\MAC_REG.txt"
-#define RTL8723_PHY_REG_PG				"rtl8723S\\PHY_REG_PG.txt"
-#define RTL8723_PHY_REG_MP				"rtl8723S\\PHY_REG_MP.txt"
-
 /*  */
 /*		RTL8723S From header */
 /*  */
@@ -424,27 +416,9 @@ struct hal_data_8723a {
 
 	bool				bMACFuncEnable;
 
-#ifdef CONFIG_8723AU_P2P
-	struct P2P_PS_Offload_t	p2p_ps_offload;
-#endif
-
-
-	/*  */
-	/*  For USB Interface HAL related */
-	/*  */
-	u32	UsbBulkOutSize;
-
 	/*  Interrupt related register information. */
 	u32	IntArray[2];
 	u32	IntrMask[2];
-
-	/*  */
-	/*  For SDIO Interface HAL related */
-	/*  */
-
-	/*  Auto FSM to Turn On, include clock, isolation, power control for MAC only */
-	u8			bMacPwrCtrlOn;
-
 };
 
 #define GET_HAL_DATA(__pAdapter)	((struct hal_data_8723a *)((__pAdapter)->HalData))
@@ -551,25 +525,33 @@ void Hal_EfuseParseCustomerID(struct rtw_adapter *padapter, u8 *hwinfo, bool Aut
 void Hal_EfuseParseAntennaDiversity(struct rtw_adapter *padapter, u8 *hwinfo, bool AutoLoadFail);
 void Hal_EfuseParseRateIndicationOption(struct rtw_adapter *padapter, u8 *hwinfo, bool AutoLoadFail);
 void Hal_EfuseParseXtal_8723A(struct rtw_adapter *pAdapter, u8 *hwinfo, u8 AutoLoadFail);
-void Hal_EfuseParseThermalMeter_8723A(struct rtw_adapter *padapter, u8 *hwinfo, u8 AutoLoadFail);
+void Hal_EfuseParseThermalMeter_8723A(struct rtw_adapter *padapter, u8 *hwinfo, bool AutoLoadFail);
 
 void Hal_InitChannelPlan23a(struct rtw_adapter *padapter);
 
-void rtl8723a_set_hal_ops(struct hal_ops *pHalFunc);
-void SetHwReg8723A(struct rtw_adapter *padapter, u8 variable, u8 *val);
-void GetHwReg8723A(struct rtw_adapter *padapter, u8 variable, u8 *val);
-#ifdef CONFIG_8723AU_BT_COEXIST
-void rtl8723a_SingleDualAntennaDetection(struct rtw_adapter *padapter);
-#endif
-
 /*  register */
 void SetBcnCtrlReg23a(struct rtw_adapter *padapter, u8 SetBits, u8 ClearBits);
 void rtl8723a_InitBeaconParameters(struct rtw_adapter *padapter);
 
-void rtl8723a_clone_haldata(struct rtw_adapter *dst_adapter, struct rtw_adapter *src_adapter);
 void rtl8723a_start_thread(struct rtw_adapter *padapter);
 void rtl8723a_stop_thread(struct rtw_adapter *padapter);
 
-s32 c2h_id_filter_ccx_8723a(u8 id);
+bool c2h_id_filter_ccx_8723a(u8 id);
+int c2h_handler_8723a(struct rtw_adapter *padapter, struct c2h_evt_hdr *c2h_evt);
+
+void rtl8723a_read_adapter_info(struct rtw_adapter *Adapter);
+void rtl8723a_read_chip_version(struct rtw_adapter *padapter);
+void rtl8723a_notch_filter(struct rtw_adapter *adapter, bool enable);
+void rtl8723a_SetBeaconRelatedRegisters(struct rtw_adapter *padapter);
+void rtl8723a_SetHalODMVar(struct rtw_adapter *Adapter,
+			   enum hal_odm_variable eVariable,
+			   void *pValue1, bool bSet);
+void
+rtl8723a_readefuse(struct rtw_adapter *padapter,
+		   u8 efuseType, u16 _offset, u16 _size_byte, u8 *pbuf);
+u16 rtl8723a_EfuseGetCurrentSize_WiFi(struct rtw_adapter *padapter);
+u16 rtl8723a_EfuseGetCurrentSize_BT(struct rtw_adapter *padapter);
+void rtl8723a_update_ramask(struct rtw_adapter *padapter,
+			    u32 mac_id, u8 rssi_level);
 
 #endif
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_recv.h b/drivers/staging/rtl8723au/include/rtl8723a_recv.h
index 6bf6904f4d4..885d4d3859f 100644
--- a/drivers/staging/rtl8723au/include/rtl8723a_recv.h
+++ b/drivers/staging/rtl8723au/include/rtl8723a_recv.h
@@ -18,21 +18,21 @@
 #include <osdep_service.h>
 #include <drv_types.h>
 
-#define NR_RECVBUFF (4)
+#define NR_RECVBUFF			4
 
-#define NR_PREALLOC_RECV_SKB (8)
+#define NR_PREALLOC_RECV_SKB		8
 
-#define RECV_BLK_SZ 512
-#define RECV_BLK_CNT 16
-#define RECV_BLK_TH RECV_BLK_CNT
+#define RECV_BLK_SZ			512
+#define RECV_BLK_CNT			16
+#define RECV_BLK_TH			RECV_BLK_CNT
 
-#define MAX_RECVBUF_SZ (15360) /*  15k < 16k */
+#define MAX_RECVBUF_SZ			15360 /*  15k < 16k */
 
 #define RECV_BULK_IN_ADDR		0x80
 #define RECV_INT_IN_ADDR		0x81
 
-#define PHY_RSSI_SLID_WIN_MAX				100
-#define PHY_LINKQUALITY_SLID_WIN_MAX		20
+#define PHY_RSSI_SLID_WIN_MAX		100
+#define PHY_LINKQUALITY_SLID_WIN_MAX	20
 
 
 struct phy_stat
@@ -48,7 +48,7 @@ struct phy_stat
 };
 
 /*  Rx smooth factor */
-#define	Rx_Smooth_Factor (20)
+#define	Rx_Smooth_Factor		20
 
 struct interrupt_msg_format {
 	unsigned int C2H_MSG0;
@@ -60,8 +60,7 @@ struct interrupt_msg_format {
 	unsigned int  MSG_EX;
 };
 
-void rtl8723au_init_recvbuf(struct rtw_adapter *padapter, struct recv_buf *precvbuf);
-int	rtl8723au_init_recv_priv(struct rtw_adapter * padapter);
+int rtl8723au_init_recv_priv(struct rtw_adapter * padapter);
 void rtl8723au_free_recv_priv(struct rtw_adapter * padapter);
 void rtl8723a_process_phy_info(struct rtw_adapter *padapter, void *prframe);
 void update_recvframe_attrib(struct recv_frame *precvframe, struct recv_stat *prxstat);
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_spec.h b/drivers/staging/rtl8723au/include/rtl8723a_spec.h
index 3595c27907d..b5d7123f787 100644
--- a/drivers/staging/rtl8723au/include/rtl8723a_spec.h
+++ b/drivers/staging/rtl8723au/include/rtl8723a_spec.h
@@ -481,11 +481,11 @@
 /* 8192C Cmd9346CR bits			(Offset 0xA, 16bit) */
 /*  */
 	 /*  EEPROM enable when set 1 */
-#define	CmdEEPROM_En			BIT5
+#define	CmdEEPROM_En			BIT(5)
 	/*  System EEPROM select, 0: boot from E-FUSE,
 	    1: The EEPROM used is 9346 */
-#define	CmdEERPOMSEL			BIT4
-#define	Cmd9346CR_9356SEL		BIT4
+#define	CmdEERPOMSEL			BIT(4)
+#define	Cmd9346CR_9356SEL		BIT(4)
 #define	AutoLoadEEPROM			(CmdEEPROM_En|CmdEERPOMSEL)
 #define	AutoLoadEFUSE			CmdEEPROM_En
 
@@ -493,7 +493,7 @@
 /* 8192C GPIO MUX Configuration Register (offset 0x40, 4 byte) */
 /*  */
 #define	GPIOSEL_GPIO			0
-#define	GPIOSEL_ENBT			BIT5
+#define	GPIOSEL_ENBT			BIT(5)
 
 /*  */
 /*        8192C GPIO PIN Control Register (offset 0x44, 4 byte) */
@@ -535,43 +535,43 @@ Default: 00b.
 #define	RRSR_RSC_UPSUBCHNL		0x400000
 #define	RRSR_RSC_LOWSUBCHNL		0x200000
 #define	RRSR_SHORT			0x800000
-#define	RRSR_1M				BIT0
-#define	RRSR_2M				BIT1
-#define	RRSR_5_5M			BIT2
-#define	RRSR_11M			BIT3
-#define	RRSR_6M				BIT4
-#define	RRSR_9M				BIT5
-#define	RRSR_12M			BIT6
-#define	RRSR_18M			BIT7
-#define	RRSR_24M			BIT8
-#define	RRSR_36M			BIT9
-#define	RRSR_48M			BIT10
-#define	RRSR_54M			BIT11
-#define	RRSR_MCS0			BIT12
-#define	RRSR_MCS1			BIT13
-#define	RRSR_MCS2			BIT14
-#define	RRSR_MCS3			BIT15
-#define	RRSR_MCS4			BIT16
-#define	RRSR_MCS5			BIT17
-#define	RRSR_MCS6			BIT18
-#define	RRSR_MCS7			BIT19
-#define	BRSR_AckShortPmb		BIT23
+#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)
 /*  CCK ACK: use Short Preamble or not */
 
 /*  */
 /* 8192C BW_OPMODE bits			(Offset 0x203, 8bit) */
 /*  */
-#define	BW_OPMODE_20MHZ			BIT2
-#define	BW_OPMODE_5G			BIT1
-#define	BW_OPMODE_11J			BIT0
+#define	BW_OPMODE_20MHZ			BIT(2)
+#define	BW_OPMODE_5G			BIT(1)
+#define	BW_OPMODE_11J			BIT(0)
 
 
 /*  */
 /* 8192C CAM Config Setting (offset 0x250, 1 byte) */
 /*  */
-#define	CAM_VALID			BIT15
+#define	CAM_VALID			BIT(15)
 #define	CAM_NOTVALID			0x0000
-#define	CAM_USEDK			BIT5
+#define	CAM_USEDK			BIT(5)
 
 #define	CAM_CONTENT_COUNT		8
 
@@ -587,9 +587,9 @@ Default: 00b.
 #define	CAM_CONFIG_USEDK		true
 #define	CAM_CONFIG_NO_USEDK		false
 
-#define	CAM_WRITE			BIT16
+#define	CAM_WRITE			BIT(16)
 #define	CAM_READ			0x00000000
-#define	CAM_POLLINIG			BIT31
+#define	CAM_POLLINIG			BIT(31)
 
 #define	SCR_UseDK			0x01
 #define	SCR_TxSecEnable			0x02
@@ -605,54 +605,54 @@ Default: 00b.
 #define	IMR8190_DISABLED		0x0
 /*  IMR DW0 Bit 0-31 */
 
-#define	IMR_BCNDMAINT6			BIT31	/*  Beacon DMA Interrupt 6 */
-#define	IMR_BCNDMAINT5			BIT30	/*  Beacon DMA Interrupt 5 */
-#define	IMR_BCNDMAINT4			BIT29	/*  Beacon DMA Interrupt 4 */
-#define	IMR_BCNDMAINT3			BIT28	/*  Beacon DMA Interrupt 3 */
-#define	IMR_BCNDMAINT2			BIT27	/*  Beacon DMA Interrupt 2 */
-#define	IMR_BCNDMAINT1			BIT26	/*  Beacon DMA Interrupt 1 */
-#define	IMR_BCNDOK8			BIT25	/*  Beacon Queue DMA OK
+#define	IMR_BCNDMAINT6			BIT(31)	/*  Beacon DMA Interrupt 6 */
+#define	IMR_BCNDMAINT5			BIT(30)	/*  Beacon DMA Interrupt 5 */
+#define	IMR_BCNDMAINT4			BIT(29)	/*  Beacon DMA Interrupt 4 */
+#define	IMR_BCNDMAINT3			BIT(28)	/*  Beacon DMA Interrupt 3 */
+#define	IMR_BCNDMAINT2			BIT(27)	/*  Beacon DMA Interrupt 2 */
+#define	IMR_BCNDMAINT1			BIT(26)	/*  Beacon DMA Interrupt 1 */
+#define	IMR_BCNDOK8			BIT(25)	/*  Beacon Queue DMA OK
 						    Interrupt 8 */
-#define	IMR_BCNDOK7			BIT24	/*  Beacon Queue DMA OK
+#define	IMR_BCNDOK7			BIT(24)	/*  Beacon Queue DMA OK
 						    Interrupt 7 */
-#define	IMR_BCNDOK6			BIT23	/*  Beacon Queue DMA OK
+#define	IMR_BCNDOK6			BIT(23)	/*  Beacon Queue DMA OK
 						    Interrupt 6 */
-#define	IMR_BCNDOK5			BIT22	/*  Beacon Queue DMA OK
+#define	IMR_BCNDOK5			BIT(22)	/*  Beacon Queue DMA OK
 						    Interrupt 5 */
-#define	IMR_BCNDOK4			BIT21	/*  Beacon Queue DMA OK
+#define	IMR_BCNDOK4			BIT(21)	/*  Beacon Queue DMA OK
 						    Interrupt 4 */
-#define	IMR_BCNDOK3			BIT20	/*  Beacon Queue DMA OK
+#define	IMR_BCNDOK3			BIT(20)	/*  Beacon Queue DMA OK
 						    Interrupt 3 */
-#define	IMR_BCNDOK2			BIT19	/*  Beacon Queue DMA OK
+#define	IMR_BCNDOK2			BIT(19)	/*  Beacon Queue DMA OK
 						    Interrupt 2 */
-#define	IMR_BCNDOK1			BIT18	/*  Beacon Queue DMA OK
+#define	IMR_BCNDOK1			BIT(18)	/*  Beacon Queue DMA OK
 						    Interrupt 1 */
-#define	IMR_TIMEOUT2			BIT17	/*  Timeout interrupt 2 */
-#define	IMR_TIMEOUT1			BIT16	/*  Timeout interrupt 1 */
-#define	IMR_TXFOVW			BIT15	/*  Transmit FIFO Overflow */
-#define	IMR_PSTIMEOUT			BIT14	/*  Power save time out
+#define	IMR_TIMEOUT2			BIT(17)	/*  Timeout interrupt 2 */
+#define	IMR_TIMEOUT1			BIT(16)	/*  Timeout interrupt 1 */
+#define	IMR_TXFOVW			BIT(15)	/*  Transmit FIFO Overflow */
+#define	IMR_PSTIMEOUT			BIT(14)	/*  Power save time out
 						    interrupt */
-#define	IMR_BcnInt			BIT13	/*  Beacon DMA Interrupt 0 */
-#define	IMR_RXFOVW			BIT12	/*  Receive FIFO Overflow */
-#define	IMR_RDU				BIT11	/*  Receive Descriptor
+#define	IMR_BcnInt			BIT(13)	/*  Beacon DMA Interrupt 0 */
+#define	IMR_RXFOVW			BIT(12)	/*  Receive FIFO Overflow */
+#define	IMR_RDU				BIT(11)	/*  Receive Descriptor
 						    Unavailable */
-#define	IMR_ATIMEND			BIT10	/*  For 92C,ATIM Window
+#define	IMR_ATIMEND			BIT(10)	/*  For 92C,ATIM Window
 						    End Interrupt */
-#define	IMR_BDOK			BIT9	/*  Beacon Queue DMA OK
+#define	IMR_BDOK			BIT(9)	/*  Beacon Queue DMA OK
 						    Interrup */
-#define	IMR_HIGHDOK			BIT8	/*  High Queue DMA OK
+#define	IMR_HIGHDOK			BIT(8)	/*  High Queue DMA OK
 						    Interrupt */
-#define	IMR_TBDOK			BIT7	/*  Transmit Beacon OK
+#define	IMR_TBDOK			BIT(7)	/*  Transmit Beacon OK
 						    interrup */
-#define	IMR_MGNTDOK			BIT6	/*  Management Queue DMA OK
+#define	IMR_MGNTDOK			BIT(6)	/*  Management Queue DMA OK
 						    Interrupt */
-#define	IMR_TBDER			BIT5	/*  For 92C,Transmit Beacon
+#define	IMR_TBDER			BIT(5)	/*  For 92C,Transmit Beacon
 						    Error Interrupt */
-#define	IMR_BKDOK			BIT4	/*  AC_BK DMA OK Interrupt */
-#define	IMR_BEDOK			BIT3	/*  AC_BE DMA OK Interrupt */
-#define	IMR_VIDOK			BIT2	/*  AC_VI DMA OK Interrupt */
-#define	IMR_VODOK			BIT1	/*  AC_VO DMA Interrupt */
-#define	IMR_ROK				BIT0	/*  Receive DMA OK Interrupt */
+#define	IMR_BKDOK			BIT(4)	/*  AC_BK DMA OK Interrupt */
+#define	IMR_BEDOK			BIT(3)	/*  AC_BE DMA OK Interrupt */
+#define	IMR_VIDOK			BIT(2)	/*  AC_VI DMA OK Interrupt */
+#define	IMR_VODOK			BIT(1)	/*  AC_VO DMA Interrupt */
+#define	IMR_ROK				BIT(0)	/*  Receive DMA OK Interrupt */
 
 #define	IMR_RX_MASK			(IMR_ROK|IMR_RDU|IMR_RXFOVW)
 #define	IMR_TX_MASK			(IMR_VODOK|IMR_VIDOK|IMR_BEDOK| \
@@ -660,14 +660,14 @@ Default: 00b.
 					 IMR_BDOK)
 
 /*  13. Host Interrupt Status Extension Register (Offset: 0x012C-012Eh) */
-#define	IMR_BcnInt_E			BIT12
-#define	IMR_TXERR			BIT11
-#define	IMR_RXERR			BIT10
-#define	IMR_C2HCMD			BIT9
-#define	IMR_CPWM			BIT8
+#define	IMR_BcnInt_E			BIT(12)
+#define	IMR_TXERR			BIT(11)
+#define	IMR_RXERR			BIT(10)
+#define	IMR_C2HCMD			BIT(9)
+#define	IMR_CPWM			BIT(8)
 /* RSVD [2-7] */
-#define	IMR_OCPINT			BIT1
-#define	IMR_WLANOFF			BIT0
+#define	IMR_OCPINT			BIT(1)
+#define	IMR_WLANOFF			BIT(0)
 
 
 /*        8192C EEPROM/EFUSE share register definition. */
@@ -913,35 +913,35 @@ Current IOREG MAP
 /*  */
 /* 8192C (RCR) Receive Configuration Register	(Offset 0x608, 32 bits) */
 /*  */
-#define	RCR_APPFCS			BIT31 /* WMAC append FCS after payload*/
-#define	RCR_APP_MIC			BIT30
-#define	RCR_APP_PHYSTS			BIT28
-#define	RCR_APP_ICV			BIT29
-#define	RCR_APP_PHYST_RXFF		BIT28
-#define	RCR_APP_BA_SSN			BIT27 /* Accept BA SSN */
-#define	RCR_ENMBID			BIT24 /* Enable Multiple BssId. */
-#define	RCR_LSIGEN			BIT23
-#define	RCR_MFBEN			BIT22
-#define	RCR_HTC_LOC_CTRL		BIT14 /* MFC<--HTC=1 MFC-->HTC=0 */
-#define	RCR_AMF				BIT13 /* Accept management type frame */
-#define	RCR_ACF				BIT12 /* Accept control type frame */
-#define	RCR_ADF				BIT11 /* Accept data type frame */
-#define	RCR_AICV			BIT9  /* Accept ICV error packet */
-#define	RCR_ACRC32			BIT8  /* Accept CRC32 error packet */
-#define	RCR_CBSSID_BCN			BIT7  /* Accept BSSID match packet
+#define	RCR_APPFCS			BIT(31) /* WMAC append FCS after payload*/
+#define	RCR_APP_MIC			BIT(30)
+#define	RCR_APP_PHYSTS			BIT(28)
+#define	RCR_APP_ICV			BIT(29)
+#define	RCR_APP_PHYST_RXFF		BIT(28)
+#define	RCR_APP_BA_SSN			BIT(27) /* Accept BA SSN */
+#define	RCR_ENMBID			BIT(24) /* Enable Multiple BssId. */
+#define	RCR_LSIGEN			BIT(23)
+#define	RCR_MFBEN			BIT(22)
+#define	RCR_HTC_LOC_CTRL		BIT(14) /* MFC<--HTC=1 MFC-->HTC=0 */
+#define	RCR_AMF				BIT(13) /* Accept management type frame */
+#define	RCR_ACF				BIT(12) /* Accept control type frame */
+#define	RCR_ADF				BIT(11) /* Accept data type frame */
+#define	RCR_AICV			BIT(9)  /* Accept ICV error packet */
+#define	RCR_ACRC32			BIT(8)  /* Accept CRC32 error packet */
+#define	RCR_CBSSID_BCN			BIT(7)  /* Accept BSSID match packet
 						 (Rx beacon, probe rsp) */
-#define	RCR_CBSSID_DATA			BIT6  /* Accept BSSID match packet
+#define	RCR_CBSSID_DATA			BIT(6)  /* Accept BSSID match packet
 						 (Data) */
 #define	RCR_CBSSID			RCR_CBSSID_DATA	/* Accept BSSID match
 							   packet */
-#define	RCR_APWRMGT			BIT5  /* Accept power management
+#define	RCR_APWRMGT			BIT(5)  /* Accept power management
 						 packet */
-#define	RCR_ADD3			BIT4  /* Accept address 3 match
+#define	RCR_ADD3			BIT(4)  /* Accept address 3 match
 						 packet */
-#define	RCR_AB				BIT3  /* Accept broadcast packet */
-#define	RCR_AM				BIT2  /* Accept multicast packet */
-#define	RCR_APM				BIT1  /* Accept physical match packet */
-#define	RCR_AAP				BIT0  /* Accept all unicast packet */
+#define	RCR_AB				BIT(3)  /* Accept broadcast packet */
+#define	RCR_AM				BIT(2)  /* Accept multicast packet */
+#define	RCR_APM				BIT(1)  /* Accept physical match packet */
+#define	RCR_AAP				BIT(0)  /* Accept all unicast packet */
 #define	RCR_MXDMA_OFFSET		8
 #define	RCR_FIFO_OFFSET			13
 
@@ -1714,33 +1714,33 @@ Current IOREG MAP
 
 /* 2REG_MULTI_FUNC_CTRL(For RTL8723 Only) */
 	/*  Enable GPIO[9] as WiFi HW PDn source */
-#define	WL_HWPDN_EN			BIT0
+#define	WL_HWPDN_EN			BIT(0)
 	/*  WiFi HW PDn polarity control */
-#define	WL_HWPDN_SL			BIT1
+#define	WL_HWPDN_SL			BIT(1)
 	/*  WiFi function enable */
-#define	WL_FUNC_EN			BIT2
+#define	WL_FUNC_EN			BIT(2)
 	/*  Enable GPIO[9] as WiFi RF HW PDn source */
-#define	WL_HWROF_EN			BIT3
+#define	WL_HWROF_EN			BIT(3)
 	/*  Enable GPIO[11] as BT HW PDn source */
-#define	BT_HWPDN_EN			BIT16
+#define	BT_HWPDN_EN			BIT(16)
 	/*  BT HW PDn polarity control */
-#define	BT_HWPDN_SL			BIT17
+#define	BT_HWPDN_SL			BIT(17)
 	/*  BT function enable */
-#define	BT_FUNC_EN			BIT18
+#define	BT_FUNC_EN			BIT(18)
 	/*  Enable GPIO[11] as BT/GPS RF HW PDn source */
-#define	BT_HWROF_EN			BIT19
+#define	BT_HWROF_EN			BIT(19)
 	/*  Enable GPIO[10] as GPS HW PDn source */
-#define	GPS_HWPDN_EN			BIT20
+#define	GPS_HWPDN_EN			BIT(20)
 	/*  GPS HW PDn polarity control */
-#define	GPS_HWPDN_SL			BIT21
+#define	GPS_HWPDN_SL			BIT(21)
 	/*  GPS function enable */
-#define	GPS_FUNC_EN			BIT22
+#define	GPS_FUNC_EN			BIT(22)
 
 /* 3 REG_LIFECTRL_CTRL */
-#define	HAL92C_EN_PKT_LIFE_TIME_BK	BIT3
-#define	HAL92C_EN_PKT_LIFE_TIME_BE	BIT2
-#define	HAL92C_EN_PKT_LIFE_TIME_VI	BIT1
-#define	HAL92C_EN_PKT_LIFE_TIME_VO	BIT0
+#define	HAL92C_EN_PKT_LIFE_TIME_BK	BIT(3)
+#define	HAL92C_EN_PKT_LIFE_TIME_BE	BIT(2)
+#define	HAL92C_EN_PKT_LIFE_TIME_VI	BIT(1)
+#define	HAL92C_EN_PKT_LIFE_TIME_VO	BIT(0)
 
 #define	HAL92C_MSDU_LIFE_TIME_UNIT	128	/*  in us, said by Tim. */
 
@@ -1933,7 +1933,7 @@ Current IOREG MAP
 					 HSISR_GPIO9_INT)
 
 /* 2 MCUFWDL */
-#define RAM_DL_SEL			BIT7	/*  1:RAM, 0:ROM */
+#define RAM_DL_SEL			BIT(7)	/*  1:RAM, 0:ROM */
 
 /* 2 HPON_FSM */
 
@@ -2083,60 +2083,60 @@ Current IOREG MAP
 
 
 /* USB interrupt */
-#define	UHIMR_TIMEOUT2			BIT31
-#define	UHIMR_TIMEOUT1			BIT30
-#define	UHIMR_PSTIMEOUT			BIT29
-#define	UHIMR_GTINT4			BIT28
-#define	UHIMR_GTINT3			BIT27
-#define	UHIMR_TXBCNERR			BIT26
-#define	UHIMR_TXBCNOK			BIT25
-#define	UHIMR_TSF_BIT32_TOGGLE		BIT24
-#define	UHIMR_BCNDMAINT3		BIT23
-#define	UHIMR_BCNDMAINT2		BIT22
-#define	UHIMR_BCNDMAINT1		BIT21
-#define	UHIMR_BCNDMAINT0		BIT20
-#define	UHIMR_BCNDOK3			BIT19
-#define	UHIMR_BCNDOK2			BIT18
-#define	UHIMR_BCNDOK1			BIT17
-#define	UHIMR_BCNDOK0			BIT16
-#define	UHIMR_HSISR_IND			BIT15
-#define	UHIMR_BCNDMAINT_E		BIT14
-/* RSVD	BIT13 */
-#define	UHIMR_CTW_END			BIT12
-/* RSVD	BIT11 */
-#define	UHIMR_C2HCMD			BIT10
-#define	UHIMR_CPWM2			BIT9
-#define	UHIMR_CPWM			BIT8
-#define	UHIMR_HIGHDOK			BIT7	/*  High Queue DMA OK
+#define	UHIMR_TIMEOUT2			BIT(31)
+#define	UHIMR_TIMEOUT1			BIT(30)
+#define	UHIMR_PSTIMEOUT			BIT(29)
+#define	UHIMR_GTINT4			BIT(28)
+#define	UHIMR_GTINT3			BIT(27)
+#define	UHIMR_TXBCNERR			BIT(26)
+#define	UHIMR_TXBCNOK			BIT(25)
+#define	UHIMR_TSF_BIT32_TOGGLE		BIT(24)
+#define	UHIMR_BCNDMAINT3		BIT(23)
+#define	UHIMR_BCNDMAINT2		BIT(22)
+#define	UHIMR_BCNDMAINT1		BIT(21)
+#define	UHIMR_BCNDMAINT0		BIT(20)
+#define	UHIMR_BCNDOK3			BIT(19)
+#define	UHIMR_BCNDOK2			BIT(18)
+#define	UHIMR_BCNDOK1			BIT(17)
+#define	UHIMR_BCNDOK0			BIT(16)
+#define	UHIMR_HSISR_IND			BIT(15)
+#define	UHIMR_BCNDMAINT_E		BIT(14)
+/* RSVD	BIT(13) */
+#define	UHIMR_CTW_END			BIT(12)
+/* RSVD	BIT(11) */
+#define	UHIMR_C2HCMD			BIT(10)
+#define	UHIMR_CPWM2			BIT(9)
+#define	UHIMR_CPWM			BIT(8)
+#define	UHIMR_HIGHDOK			BIT(7)	/*  High Queue DMA OK
 						    Interrupt */
-#define	UHIMR_MGNTDOK			BIT6	/*  Management Queue DMA OK
+#define	UHIMR_MGNTDOK			BIT(6)	/*  Management Queue DMA OK
 						    Interrupt */
-#define	UHIMR_BKDOK			BIT5	/*  AC_BK DMA OK Interrupt */
-#define	UHIMR_BEDOK			BIT4	/*  AC_BE DMA OK Interrupt */
-#define	UHIMR_VIDOK			BIT3	/*  AC_VI DMA OK Interrupt */
-#define	UHIMR_VODOK			BIT2	/*  AC_VO DMA Interrupt */
-#define	UHIMR_RDU			BIT1	/*  Receive Descriptor
+#define	UHIMR_BKDOK			BIT(5)	/*  AC_BK DMA OK Interrupt */
+#define	UHIMR_BEDOK			BIT(4)	/*  AC_BE DMA OK Interrupt */
+#define	UHIMR_VIDOK			BIT(3)	/*  AC_VI DMA OK Interrupt */
+#define	UHIMR_VODOK			BIT(2)	/*  AC_VO DMA Interrupt */
+#define	UHIMR_RDU			BIT(1)	/*  Receive Descriptor
 						    Unavailable */
-#define	UHIMR_ROK			BIT0	/*  Receive DMA OK Interrupt */
+#define	UHIMR_ROK			BIT(0)	/*  Receive DMA OK Interrupt */
 
 /*  USB Host Interrupt Status Extension bit */
-#define	UHIMR_BCNDMAINT7		BIT23
-#define	UHIMR_BCNDMAINT6		BIT22
-#define	UHIMR_BCNDMAINT5		BIT21
-#define	UHIMR_BCNDMAINT4		BIT20
-#define	UHIMR_BCNDOK7			BIT19
-#define	UHIMR_BCNDOK6			BIT18
-#define	UHIMR_BCNDOK5			BIT17
-#define	UHIMR_BCNDOK4			BIT16
+#define	UHIMR_BCNDMAINT7		BIT(23)
+#define	UHIMR_BCNDMAINT6		BIT(22)
+#define	UHIMR_BCNDMAINT5		BIT(21)
+#define	UHIMR_BCNDMAINT4		BIT(20)
+#define	UHIMR_BCNDOK7			BIT(19)
+#define	UHIMR_BCNDOK6			BIT(18)
+#define	UHIMR_BCNDOK5			BIT(17)
+#define	UHIMR_BCNDOK4			BIT(16)
 /*  bit14-15: RSVD */
-#define	UHIMR_ATIMEND_E			BIT13
-#define	UHIMR_ATIMEND			BIT12
-#define	UHIMR_TXERR			BIT11
-#define	UHIMR_RXERR			BIT10
-#define	UHIMR_TXFOVW			BIT9
-#define	UHIMR_RXFOVW			BIT8
+#define	UHIMR_ATIMEND_E			BIT(13)
+#define	UHIMR_ATIMEND			BIT(12)
+#define	UHIMR_TXERR			BIT(11)
+#define	UHIMR_RXERR			BIT(10)
+#define	UHIMR_TXFOVW			BIT(9)
+#define	UHIMR_RXFOVW			BIT(8)
 /*  bit2-7: RSVD */
-#define	UHIMR_OCPINT			BIT1
+#define	UHIMR_OCPINT			BIT(1)
 /*  bit0: RSVD */
 
 #define	REG_USB_HIMR			0xFE38
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_xmit.h b/drivers/staging/rtl8723au/include/rtl8723a_xmit.h
index 3b6fdc3a9b7..79883d7b867 100644
--- a/drivers/staging/rtl8723au/include/rtl8723a_xmit.h
+++ b/drivers/staging/rtl8723au/include/rtl8723a_xmit.h
@@ -216,14 +216,12 @@ void handle_txrpt_ccx_8723a(struct rtw_adapter *adapter, void *buf);
 void rtl8723a_update_txdesc(struct xmit_frame *pxmitframe, u8 *pmem);
 void rtl8723a_fill_fake_txdesc(struct rtw_adapter *padapter, u8 *pDesc, u32 BufferLen, u8 IsPsPoll, u8 IsBTQosNull);
 
-s32	rtl8723au_hal_xmitframe_enqueue(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe);
+int rtl8723au_hal_xmitframe_enqueue(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe);
 s32 rtl8723au_xmit_buf_handler(struct rtw_adapter *padapter);
 #define hal_xmit_handler rtl8723au_xmit_buf_handler
-s32	rtl8723au_init_xmit_priv(struct rtw_adapter * padapter);
-void	rtl8723au_free_xmit_priv(struct rtw_adapter * padapter);
-s32 rtl8723au_hal_xmit(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe);
-s32 rtl8723au_mgnt_xmit(struct rtw_adapter *padapter, struct xmit_frame *pmgntframe);
-s32 rtl8723au_xmitframe_complete(struct rtw_adapter *padapter, struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf);
+bool rtl8723au_hal_xmit(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe);
+int rtl8723au_mgnt_xmit(struct rtw_adapter *padapter, struct xmit_frame *pmgntframe);
+bool rtl8723au_xmitframe_complete(struct rtw_adapter *padapter, struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf);
 
 
 #endif
diff --git a/drivers/staging/rtl8723au/include/rtw_ap.h b/drivers/staging/rtl8723au/include/rtw_ap.h
index 76f82d68f63..8d9be5a36ea 100644
--- a/drivers/staging/rtl8723au/include/rtw_ap.h
+++ b/drivers/staging/rtl8723au/include/rtw_ap.h
@@ -22,8 +22,6 @@
 #ifdef CONFIG_8723AU_AP_MODE
 
 /* external function */
-void rtw_indicate_sta_assoc_event23a(struct rtw_adapter *padapter, struct sta_info *psta);
-void rtw_indicate_sta_disassoc_event23a(struct rtw_adapter *padapter, struct sta_info *psta);
 
 void init_mlme_ap_info23a(struct rtw_adapter *padapter);
 void free_mlme_ap_info23a(struct rtw_adapter *padapter);
@@ -34,7 +32,7 @@ void update_beacon23a(struct rtw_adapter *padapter, u8 ie_id, u8 *oui, u8 tx);
 void add_RATid23a(struct rtw_adapter *padapter, struct sta_info *psta, u8 rssi_level);
 void expire_timeout_chk23a(struct rtw_adapter *padapter);
 void update_sta_info23a_apmode23a(struct rtw_adapter *padapter, struct sta_info *psta);
-int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len);
+int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf, unsigned int len);
 void rtw_ap_restore_network(struct rtw_adapter *padapter);
 void rtw_set_macaddr_acl23a(struct rtw_adapter *padapter, int mode);
 int rtw_acl_add_sta23a(struct rtw_adapter *padapter, u8 *addr);
diff --git a/drivers/staging/rtl8723au/include/rtw_cmd.h b/drivers/staging/rtl8723au/include/rtw_cmd.h
index f9caa3e35f5..c49237b04a3 100644
--- a/drivers/staging/rtl8723au/include/rtw_cmd.h
+++ b/drivers/staging/rtl8723au/include/rtw_cmd.h
@@ -25,8 +25,6 @@
 #include <ieee80211.h> /*  <ieee80211/ieee80211.h> */
 
 
-#define FREE_CMDOBJ_SZ	128
-
 #define MAX_CMDSZ	1024
 #define MAX_RSPSZ	512
 #define MAX_EVTSZ	1024
@@ -34,50 +32,33 @@
 #define CMDBUFF_ALIGN_SZ 512
 
 struct cmd_obj {
+	struct work_struct work;
 	struct rtw_adapter *padapter;
 	u16	cmdcode;
-	u8	res;
-	u8	*parmbuf;
+	int	res;
 	u32	cmdsz;
+	u8	*parmbuf;
 	u8	*rsp;
 	u32	rspsz;
-	/* struct semaphore		cmd_sem; */
-	struct list_head	list;
 };
 
 struct cmd_priv {
-	struct semaphore	cmd_queue_sema;
-	/* struct semaphore	cmd_done_sema; */
-	struct semaphore	terminate_cmdthread_sema;
-	struct rtw_queue	cmd_queue;
-	u8	cmd_seq;
-	u8	*cmd_buf;	/* shall be non-paged, and 4 bytes aligned */
-	u8	*cmd_allocated_buf;
-	u8	*rsp_buf;	/* shall be non-paged, and 4 bytes aligned */
-	u8	*rsp_allocated_buf;
+	struct workqueue_struct *wq;
 	u32	cmd_issued_cnt;
 	u32	cmd_done_cnt;
 	u32	rsp_cnt;
-	u8 cmdthd_running;
 	struct rtw_adapter *padapter;
 };
 
 #define C2H_QUEUE_MAX_LEN 10
 
 struct	evt_priv {
-	struct work_struct c2h_wk;
-	bool c2h_wk_alive;
-	struct rtw_cbuf *c2h_queue;
-
-	atomic_t event_seq;
-	u8	*evt_buf;	/* shall be non-paged, and 4 bytes aligned */
-	u8	*evt_allocated_buf;
-	u32	evt_done_cnt;
+	struct workqueue_struct *wq;
+	struct work_struct irq_wk;
 };
 
 #define init_h2fwcmd_w_parm_no_rsp(pcmd, pparm, code) \
 do {\
-	INIT_LIST_HEAD(&pcmd->list);\
 	pcmd->cmdcode = code;\
 	pcmd->parmbuf = (u8 *)(pparm);\
 	pcmd->cmdsz = sizeof (*pparm);\
@@ -92,23 +73,35 @@ struct c2h_evt_hdr {
 	u8 payload[0];
 };
 
+/*
+ * Do not reorder - this allows for struct evt_work to be passed on to
+ * rtw_c2h_wk_cmd23a() as a 'struct c2h_evt_hdr *' without making an
+ * additional copy.
+ */
+struct evt_work {
+	union {
+		struct c2h_evt_hdr c2h_evt;
+		u8 buf[16];
+	} u;
+	struct work_struct work;
+	struct rtw_adapter *adapter;
+};
+
 #define c2h_evt_exist(c2h_evt) ((c2h_evt)->id || (c2h_evt)->plen)
 
-u32 rtw_enqueue_cmd23a(struct cmd_priv *pcmdpriv, struct cmd_obj *obj);
+void rtw_evt_work(struct work_struct *work);
+
+int rtw_enqueue_cmd23a(struct cmd_priv *pcmdpriv, struct cmd_obj *obj);
 void rtw_free_cmd_obj23a(struct cmd_obj *pcmd);
 
 int rtw_cmd_thread23a(void *context);
 
 int rtw_init_cmd_priv23a(struct cmd_priv *pcmdpriv);
-void rtw_free_cmd_priv23a (struct cmd_priv *pcmdpriv);
 
 u32 rtw_init_evt_priv23a (struct evt_priv *pevtpriv);
 void rtw_free_evt_priv23a (struct evt_priv *pevtpriv);
 void rtw_cmd_clr_isr23a(struct cmd_priv *pcmdpriv);
 void rtw_evt_notify_isr(struct evt_priv *pevtpriv);
-#ifdef CONFIG_8723AU_P2P
-u8 p2p_protocol_wk_cmd23a(struct rtw_adapter*padapter, int intCmdType );
-#endif /* CONFIG_8723AU_P2P */
 
 enum rtw_drvextra_cmd_id
 {
@@ -177,8 +170,7 @@ struct disconnect_parm {
 };
 
 struct	setopmode_parm {
-	u8	mode;
-	u8	rsvd[3];
+	enum nl80211_iftype mode;
 };
 
 /*
@@ -227,7 +219,7 @@ when 802.1x ==> keyid > 2 ==> unicast key
 
 */
 struct setkey_parm {
-	u8	algorithm;	/*  encryption algorithm, could be none, wep40, TKIP, CCMP, wep104 */
+	u32	algorithm;	/*  encryption algorithm, could be none, wep40, TKIP, CCMP, wep104 */
 	u8	keyid;
 	u8	grpkey;		/*  1: this is the grpkey for 802.1x. 0: this is the unicast key for 802.1x */
 	u8	set_tx;		/*  1: main tx key for wep. 0: other key. */
@@ -245,8 +237,8 @@ when shared key ==> algorithm/keyid
 */
 struct set_stakey_parm {
 	u8	addr[ETH_ALEN];
-	u8	algorithm;
 	u8	id;/*  currently for erasing cam entry if algorithm == _NO_PRIVACY_ */
+	u32	algorithm;
 	u8	key[16];
 };
 
@@ -686,52 +678,51 @@ Result:
 #define H2C_CMD_OVERFLOW		0x06
 #define H2C_RESERVED			0x07
 
-u8 rtw_setassocsta_cmd(struct rtw_adapter  *padapter, u8 *mac_addr);
-u8 rtw_setstandby_cmd(struct rtw_adapter *padapter, uint action);
-u8 rtw_sitesurvey_cmd23a(struct rtw_adapter  *padapter, struct cfg80211_ssid *ssid, int ssid_num, struct rtw_ieee80211_channel *ch, int ch_num);
-u8 rtw_createbss_cmd23a(struct rtw_adapter  *padapter);
-u8 rtw_createbss_cmd23a_ex(struct rtw_adapter  *padapter, unsigned char *pbss, unsigned int sz);
-u8 rtw_setphy_cmd(struct rtw_adapter  *padapter, u8 modem, u8 ch);
-u8 rtw_setstakey_cmd23a(struct rtw_adapter  *padapter, u8 *psta, u8 unicast_key);
-u8 rtw_clearstakey_cmd23a(struct rtw_adapter *padapter, u8 *psta, u8 entry, u8 enqueue);
-u8 rtw_joinbss_cmd23a(struct rtw_adapter  *padapter, struct wlan_network* pnetwork);
-u8 rtw_disassoc_cmd23a(struct rtw_adapter *padapter, u32 deauth_timeout_ms, bool enqueue);
-u8 rtw_setopmode_cmd23a(struct rtw_adapter  *padapter, enum ndis_802_11_net_infra networktype);
-u8 rtw_setdatarate_cmd(struct rtw_adapter  *padapter, u8 *rateset);
-u8 rtw_setbasicrate_cmd(struct rtw_adapter  *padapter, u8 *rateset);
-u8 rtw_setbbreg_cmd(struct rtw_adapter * padapter, u8 offset, u8 val);
-u8 rtw_setrfreg_cmd(struct rtw_adapter * padapter, u8 offset, u32 val);
-u8 rtw_getbbreg_cmd(struct rtw_adapter * padapter, u8 offset, u8 * pval);
-u8 rtw_getrfreg_cmd(struct rtw_adapter * padapter, u8 offset, u8 * pval);
-u8 rtw_setrfintfs_cmd(struct rtw_adapter  *padapter, u8 mode);
-u8 rtw_setrttbl_cmd(struct rtw_adapter  *padapter, struct setratable_parm *prate_table);
-u8 rtw_getrttbl_cmd(struct rtw_adapter  *padapter, struct getratable_rsp *pval);
-
-u8 rtw_gettssi_cmd(struct rtw_adapter  *padapter, u8 offset,u8 *pval);
-u8 rtw_setfwdig_cmd(struct rtw_adapter*padapter, u8 type);
-u8 rtw_setfwra_cmd(struct rtw_adapter*padapter, u8 type);
-
-u8 rtw_addbareq_cmd23a(struct rtw_adapter*padapter, u8 tid, u8 *addr);
-
-u8 rtw_dynamic_chk_wk_cmd23a(struct rtw_adapter *adapter);
-
-u8 rtw_lps_ctrl_wk_cmd23a(struct rtw_adapter*padapter, u8 lps_ctrl_type, u8 enqueue);
-
-u8 rtw_ps_cmd23a(struct rtw_adapter*padapter);
+int rtw_setassocsta_cmd(struct rtw_adapter  *padapter, u8 *mac_addr);
+int rtw_setstandby_cmd(struct rtw_adapter *padapter, uint action);
+int rtw_sitesurvey_cmd23a(struct rtw_adapter  *padapter, struct cfg80211_ssid *ssid, int ssid_num, struct rtw_ieee80211_channel *ch, int ch_num);
+int rtw_createbss_cmd23a(struct rtw_adapter  *padapter);
+int rtw_createbss_cmd23a_ex(struct rtw_adapter  *padapter, unsigned char *pbss, unsigned int sz);
+int rtw_setphy_cmd(struct rtw_adapter  *padapter, u8 modem, u8 ch);
+int rtw_setstakey_cmd23a(struct rtw_adapter  *padapter, u8 *psta, u8 unicast_key);
+int rtw_clearstakey_cmd23a(struct rtw_adapter *padapter, u8 *psta, u8 entry, u8 enqueue);
+int rtw_joinbss_cmd23a(struct rtw_adapter  *padapter, struct wlan_network* pnetwork);
+int rtw_disassoc_cmd23a(struct rtw_adapter *padapter, u32 deauth_timeout_ms, bool enqueue);
+int rtw_setopmode_cmd23a(struct rtw_adapter *padapter, enum nl80211_iftype ifmode);
+int rtw_setdatarate_cmd(struct rtw_adapter  *padapter, u8 *rateset);
+int rtw_setbasicrate_cmd(struct rtw_adapter  *padapter, u8 *rateset);
+int rtw_setbbreg_cmd(struct rtw_adapter * padapter, u8 offset, u8 val);
+int rtw_setrfreg_cmd(struct rtw_adapter * padapter, u8 offset, u32 val);
+int rtw_getbbreg_cmd(struct rtw_adapter * padapter, u8 offset, u8 * pval);
+int rtw_getrfreg_cmd(struct rtw_adapter * padapter, u8 offset, u8 * pval);
+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_setfwdig_cmd(struct rtw_adapter*padapter, u8 type);
+int rtw_setfwra_cmd(struct rtw_adapter*padapter, u8 type);
+
+int rtw_addbareq_cmd23a(struct rtw_adapter*padapter, u8 tid, u8 *addr);
+
+int rtw_dynamic_chk_wk_cmd23a(struct rtw_adapter *adapter);
+
+int rtw_lps_ctrl_wk_cmd23a(struct rtw_adapter*padapter, u8 lps_ctrl_type, u8 enqueue);
+
+int rtw_ps_cmd23a(struct rtw_adapter*padapter);
 
 #ifdef CONFIG_8723AU_AP_MODE
-u8 rtw_chk_hi_queue_cmd23a(struct rtw_adapter*padapter);
+int rtw_chk_hi_queue_cmd23a(struct rtw_adapter*padapter);
 #endif
 
-u8 rtw_set_ch_cmd23a(struct rtw_adapter*padapter, u8 ch, u8 bw, u8 ch_offset, u8 enqueue);
-u8 rtw_set_chplan_cmd(struct rtw_adapter*padapter, u8 chplan, u8 enqueue);
-u8 rtw_led_blink_cmd(struct rtw_adapter*padapter, struct led_8723a *pLed);
-u8 rtw_set_csa_cmd(struct rtw_adapter*padapter, u8 new_ch_no);
-u8 rtw_tdls_cmd(struct rtw_adapter*padapter, u8 *addr, u8 option);
+int rtw_set_ch_cmd23a(struct rtw_adapter*padapter, u8 ch, u8 bw, u8 ch_offset, u8 enqueue);
+int rtw_set_chplan_cmd(struct rtw_adapter*padapter, u8 chplan, u8 enqueue);
+int rtw_led_blink_cmd(struct rtw_adapter*padapter, struct led_8723a *pLed);
+int rtw_set_csa_cmd(struct rtw_adapter*padapter, u8 new_ch_no);
 
-u8 rtw_c2h_wk_cmd23a(struct rtw_adapter *padapter, u8 *c2h_evt);
+int rtw_c2h_wk_cmd23a(struct rtw_adapter *padapter, u8 *c2h_evt);
 
-u8 rtw_drvextra_cmd_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf);
+int rtw_drvextra_cmd_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
 
 void rtw_survey_cmd_callback23a(struct rtw_adapter  *padapter, struct cmd_obj *pcmd);
 void rtw_disassoc_cmd23a_callback(struct rtw_adapter  *padapter, struct cmd_obj *pcmd);
@@ -825,11 +816,6 @@ enum rtw_h2c_cmd {
 	MAX_H2CCMD
 };
 
-#define _GetBBReg_CMD_		_Read_BBREG_CMD_
-#define _SetBBReg_CMD_		_Write_BBREG_CMD_
-#define _GetRFReg_CMD_		_Read_RFREG_CMD_
-#define _SetRFReg_CMD_		_Write_RFREG_CMD_
-
 extern struct _cmd_callback	rtw_cmd_callback[];
 
 #endif /*  _CMD_H_ */
diff --git a/drivers/staging/rtl8723au/include/rtw_efuse.h b/drivers/staging/rtl8723au/include/rtw_efuse.h
index a7755056163..07bdc346444 100644
--- a/drivers/staging/rtl8723au/include/rtw_efuse.h
+++ b/drivers/staging/rtl8723au/include/rtw_efuse.h
@@ -81,20 +81,19 @@ struct pg_pkt_struct {
 
 /*------------------------Export global variable----------------------------*/
 
-u8	efuse_GetCurrentSize23a(struct rtw_adapter *padapter, u16 *size);
 u16	efuse_GetMaxSize23a(struct rtw_adapter *padapter);
-u8	rtw_efuse_access23a(struct rtw_adapter *padapter, u8 bRead, u16 start_addr, u16 cnts, u8 *data);
-u8	rtw_efuse_map_read23a(struct rtw_adapter *padapter, u16 addr, u16 cnts, u8 *data);
+int	rtw_efuse_access23a(struct rtw_adapter *padapter, u8 bRead, u16 start_addr, u16 cnts, u8 *data);
+int	rtw_efuse_map_read23a(struct rtw_adapter *padapter, u16 addr, u16 cnts, u8 *data);
 u8	rtw_efuse_map_write(struct rtw_adapter *padapter, u16 addr, u16 cnts, u8 *data);
-u8	rtw_BT_efuse_map_read23a(struct rtw_adapter *padapter, u16 addr, u16 cnts, u8 *data);
+int	rtw_BT_efuse_map_read23a(struct rtw_adapter *padapter, u16 addr, u16 cnts, u8 *data);
 u8	rtw_BT_efuse_map_write(struct rtw_adapter *padapter, u16 addr, u16 cnts, u8 *data);
 
 u16	Efuse_GetCurrentSize23a(struct rtw_adapter *pAdapter, u8 efuseType);
 u8	Efuse_CalculateWordCnts23a(u8 word_en);
 void	ReadEFuseByte23a(struct rtw_adapter *Adapter, u16 _offset, u8 *pbuf);
 void	EFUSE_GetEfuseDefinition23a(struct rtw_adapter *pAdapter, u8 efuseType, u8 type, void *pOut);
-u8	efuse_OneByteRead23a(struct rtw_adapter *pAdapter, u16 addr, u8 *data);
-u8	efuse_OneByteWrite23a(struct rtw_adapter *pAdapter, u16 addr, u8 data);
+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);
 int	Efuse_PgPacketRead23a(struct rtw_adapter *pAdapter, u8 offset, u8 *data);
diff --git a/drivers/staging/rtl8723au/include/rtw_event.h b/drivers/staging/rtl8723au/include/rtw_event.h
index bb20640e685..807cc83bc71 100644
--- a/drivers/staging/rtl8723au/include/rtw_event.h
+++ b/drivers/staging/rtl8723au/include/rtw_event.h
@@ -16,99 +16,59 @@
 #define _RTW_EVENT_H_
 
 #include <osdep_service.h>
-
 #include <wlan_bssdef.h>
 
 /*
 Used to report a bss has been scanned
-
 */
-struct survey_event	{
+struct survey_event {
 	struct wlan_bssid_ex bss;
 };
 
 /*
 Used to report that the requested site survey has been done.
-
 bss_cnt indicates the number of bss that has been reported.
-
-
 */
 struct surveydone_event {
-	unsigned int	bss_cnt;
-
+	unsigned int bss_cnt;
 };
 
 /*
 Used to report the link result of joinning the given bss
-
-
 join_res:
 -1: authentication fail
 -2: association fail
 > 0: TID
-
 */
 struct joinbss_event {
-	struct	wlan_network	network;
+	struct wlan_network network;
 };
 
 /*
 Used to report a given STA has joinned the created BSS.
 It is used in AP/Ad-HoC(M) mode.
-
-
 */
 struct stassoc_event {
 	unsigned char macaddr[6];
 	unsigned char rsvd[2];
-	int    cam_id;
-
+	int cam_id;
 };
 
 struct stadel_event {
- unsigned char macaddr[6];
- unsigned char rsvd[2]; /* for reason */
- int mac_id;
+	unsigned char macaddr[6];
+	unsigned char rsvd[2]; /* for reason */
+	int mac_id;
 };
 
-struct addba_event
-{
+struct addba_event {
 	unsigned int tid;
 };
 
 #define GEN_EVT_CODE(event)	event ## _EVT_
 
 struct fwevent {
-	u32	parmsize;
-	void (*event_callback)(struct rtw_adapter *dev, u8 *pbuf);
-};
-
-
-#define C2HEVENT_SZ			32
-
-struct event_node{
-	unsigned char *node;
-	unsigned char evt_code;
-	unsigned short evt_sz;
-	volatile int	*caller_ff_tail;
-	int	caller_ff_sz;
+	u32 parmsize;
+	void (*event_callback)(struct rtw_adapter *dev, const u8 *pbuf);
 };
 
-struct c2hevent_queue {
-	volatile int	head;
-	volatile int	tail;
-	struct	event_node	nodes[C2HEVENT_SZ];
-	unsigned char	seq;
-};
-
-#define NETWORK_QUEUE_SZ	4
-
-struct network_queue {
-	volatile int	head;
-	volatile int	tail;
-	struct wlan_bssid_ex networks[NETWORK_QUEUE_SZ];
-};
-
-
 #endif /*  _WLANEVENT_H_ */
diff --git a/drivers/staging/rtl8723au/include/rtw_ht.h b/drivers/staging/rtl8723au/include/rtw_ht.h
index 7fe0aa46f70..86ce86b6b69 100644
--- a/drivers/staging/rtl8723au/include/rtw_ht.h
+++ b/drivers/staging/rtl8723au/include/rtw_ht.h
@@ -33,9 +33,9 @@ struct ht_priv
 	u8	sgi;/* short GI */
 
 	/* for processing Tx A-MPDU */
-	u8	agg_enable_bitmap;
+	u16	agg_enable_bitmap;
 	/* u8	ADDBA_retry_count; */
-	u8	candidate_tid_bitmap;
+	u16	candidate_tid_bitmap;
 
 	struct ieee80211_ht_cap ht_cap;
 };
diff --git a/drivers/staging/rtl8723au/include/rtw_io.h b/drivers/staging/rtl8723au/include/rtw_io.h
index 8d39d800267..cc151b08d31 100644
--- a/drivers/staging/rtl8723au/include/rtw_io.h
+++ b/drivers/staging/rtl8723au/include/rtw_io.h
@@ -97,40 +97,6 @@
 #define _INTF_ASYNC_	BIT(0)	/* support async io */
 
 struct intf_priv;
-struct intf_hdl;
-struct io_queue;
-
-struct _io_ops
-{
-		u8 (*_read8)(struct intf_hdl *pintfhdl, u32 addr);
-		u16 (*_read16)(struct intf_hdl *pintfhdl, u32 addr);
-		u32 (*_read32)(struct intf_hdl *pintfhdl, u32 addr);
-
-		int (*_write8)(struct intf_hdl *pintfhdl, u32 addr, u8 val);
-		int (*_write16)(struct intf_hdl *pintfhdl, u32 addr, u16 val);
-		int (*_write32)(struct intf_hdl *pintfhdl, u32 addr, u32 val);
-		int (*_writeN)(struct intf_hdl *pintfhdl, u32 addr, u32 length, u8 *pdata);
-
-		int (*_write8_async)(struct intf_hdl *pintfhdl, u32 addr, u8 val);
-		int (*_write16_async)(struct intf_hdl *pintfhdl, u32 addr, u16 val);
-		int (*_write32_async)(struct intf_hdl *pintfhdl, u32 addr, u32 val);
-
-		void (*_read_mem)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
-		void (*_write_mem)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
-
-		void (*_sync_irp_protocol_rw)(struct io_queue *pio_q);
-
-		u32 (*_read_interrupt)(struct intf_hdl *pintfhdl, u32 addr);
-
-		u32 (*_read_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, struct recv_buf *rbuf);
-		u32 (*_write_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, struct xmit_buf *pmem);
-
-		u32 (*_write_scsi)(struct intf_hdl *pintfhdl,u32 cnt, u8 *pmem);
-
-		void (*_read_port_cancel)(struct intf_hdl *pintfhdl);
-		void (*_write_port_cancel)(struct intf_hdl *pintfhdl);
-
-};
 
 struct io_req {
 	struct list_head	list;
@@ -145,14 +111,6 @@ struct io_req {
 	u8 *cnxt;
 };
 
-struct	intf_hdl {
-	struct rtw_adapter *padapter;
-	struct dvobj_priv *pintf_dev;/* 	pointer to &(padapter->dvobjpriv); */
-
-	struct _io_ops	io_ops;
-
-};
-
 struct reg_protocol_rd {
 
 #ifdef __LITTLE_ENDIAN
@@ -265,152 +223,15 @@ struct reg_protocol_wt {
 
 };
 
-
-
-/*
-Below is the data structure used by _io_handler
-
-*/
-
-struct io_queue {
-	spinlock_t	lock;
-	struct list_head free_ioreqs;
-	struct list_head pending;		/* The io_req list that will be served in the single protocol read/write. */
-	struct list_head processing;
-	u8	*free_ioreqs_buf; /*  4-byte aligned */
-	u8	*pallocated_free_ioreqs_buf;
-	struct	intf_hdl	intf;
-};
-
-struct io_priv{
-
-	struct rtw_adapter *padapter;
-
-	struct intf_hdl intf;
-
-};
-
-uint ioreq_flush(struct rtw_adapter *adapter, struct io_queue *ioqueue);
-void sync_ioreq_enqueue(struct io_req *preq,struct io_queue *ioqueue);
-uint sync_ioreq_flush(struct rtw_adapter *adapter, struct io_queue *ioqueue);
-
-uint free_ioreq(struct io_req *preq, struct io_queue *pio_queue);
-struct io_req *alloc_ioreq(struct io_queue *pio_q);
-
-uint register_intf_hdl(u8 *dev, struct intf_hdl *pintfhdl);
-void unregister_intf_hdl(struct intf_hdl *pintfhdl);
-
-void _rtw_attrib_read(struct rtw_adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
-void _rtw_attrib_write(struct rtw_adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
-
-u8 _rtw_read823a(struct rtw_adapter *adapter, u32 addr);
-u16 _rtw_read1623a(struct rtw_adapter *adapter, u32 addr);
-u32 _rtw_read3223a(struct rtw_adapter *adapter, u32 addr);
-void _rtw_read_mem23a(struct rtw_adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
-void _rtw_read_port23a(struct rtw_adapter *adapter, u32 addr, u32 cnt, struct recv_buf *rbuf);
-void _rtw_read_port23a_cancel(struct rtw_adapter *adapter);
-
-int _rtw_write823a(struct rtw_adapter *adapter, u32 addr, u8 val);
-int _rtw_write1623a(struct rtw_adapter *adapter, u32 addr, u16 val);
-int _rtw_write3223a(struct rtw_adapter *adapter, u32 addr, u32 val);
-int _rtw_writeN23a(struct rtw_adapter *adapter, u32 addr, u32 length, u8 *pdata);
-
-int _rtw_write823a_async23a(struct rtw_adapter *adapter, u32 addr, u8 val);
-int _rtw_write1623a_async(struct rtw_adapter *adapter, u32 addr, u16 val);
-int _rtw_write3223a_async23a(struct rtw_adapter *adapter, u32 addr, u32 val);
-
-void _rtw_write_mem23a(struct rtw_adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
-u32 _rtw_write_port23a(struct rtw_adapter *adapter, u32 addr, u32 cnt, struct xmit_buf *pmem);
-u32 _rtw_write_port23a_and_wait23a(struct rtw_adapter *adapter, u32 addr, u32 cnt, struct xmit_buf *pmem, int timeout_ms);
-void _rtw_write_port23a_cancel(struct rtw_adapter *adapter);
-
-#ifdef DBG_IO
-bool match_read_sniff_ranges(u16 addr, u16 len);
-bool match_write_sniff_ranges(u16 addr, u16 len);
-
-u8 dbg_rtw_read823a(struct rtw_adapter *adapter, u32 addr, const char *caller, const int line);
-u16 dbg_rtw_read1623a(struct rtw_adapter *adapter, u32 addr, const char *caller, const int line);
-u32 dbg_rtw_read3223a(struct rtw_adapter *adapter, u32 addr, const char *caller, const int line);
-
-int dbg_rtw_write823a(struct rtw_adapter *adapter, u32 addr, u8 val, const char *caller, const int line);
-int dbg_rtw_write1623a(struct rtw_adapter *adapter, u32 addr, u16 val, const char *caller, const int line);
-int dbg_rtw_write3223a(struct rtw_adapter *adapter, u32 addr, u32 val, const char *caller, const int line);
-int dbg_rtw_writeN23a(struct rtw_adapter *adapter, u32 addr ,u32 length , u8 *data, const char *caller, const int line);
-
-#define rtw_read8(adapter, addr) dbg_rtw_read823a((adapter), (addr), __FUNCTION__, __LINE__)
-#define rtw_read16(adapter, addr) dbg_rtw_read1623a((adapter), (addr), __FUNCTION__, __LINE__)
-#define rtw_read32(adapter, addr) dbg_rtw_read3223a((adapter), (addr), __FUNCTION__, __LINE__)
-#define rtw_read_mem(adapter, addr, cnt, mem) _rtw_read_mem23a((adapter), (addr), (cnt), (mem))
-#define rtw_read_port(adapter, addr, cnt, mem) _rtw_read_port23a((adapter), (addr), (cnt), (mem))
-#define rtw_read_port_cancel(adapter) _rtw_read_port23a_cancel((adapter))
-
-#define  rtw_write8(adapter, addr, val) dbg_rtw_write823a((adapter), (addr), (val), __FUNCTION__, __LINE__)
-#define  rtw_write16(adapter, addr, val) dbg_rtw_write1623a((adapter), (addr), (val), __FUNCTION__, __LINE__)
-#define  rtw_write32(adapter, addr, val) dbg_rtw_write3223a((adapter), (addr), (val), __FUNCTION__, __LINE__)
-#define  rtw_writeN(adapter, addr, length, data) dbg_rtw_writeN23a((adapter), (addr), (length), (data), __FUNCTION__, __LINE__)
-
-#define rtw_write8_async(adapter, addr, val) _rtw_write823a_async23a((adapter), (addr), (val))
-#define rtw_write16_async(adapter, addr, val) _rtw_write1623a_async((adapter), (addr), (val))
-#define rtw_write32_async(adapter, addr, val) _rtw_write3223a_async23a((adapter), (addr), (val))
-
-#define rtw_write_mem(adapter, addr, cnt, mem) _rtw_write_mem23a((adapter), addr, cnt, mem)
-#define rtw_write_port(adapter, addr, cnt, mem) _rtw_write_port23a(adapter, addr, cnt, mem)
-#define rtw_write_port_and_wait(adapter, addr, cnt, mem, timeout_ms) _rtw_write_port23a_and_wait23a((adapter), (addr), (cnt), (mem), (timeout_ms))
-#define rtw_write_port_cancel(adapter) _rtw_write_port23a_cancel(adapter)
-#else /* DBG_IO */
-#define rtw_read8(adapter, addr) _rtw_read823a((adapter), (addr))
-#define rtw_read16(adapter, addr) _rtw_read1623a((adapter), (addr))
-#define rtw_read32(adapter, addr) _rtw_read3223a((adapter), (addr))
-#define rtw_read_mem(adapter, addr, cnt, mem) _rtw_read_mem23a((adapter), (addr), (cnt), (mem))
-#define rtw_read_port(adapter, addr, cnt, mem) _rtw_read_port23a((adapter), (addr), (cnt), (mem))
-#define rtw_read_port_cancel(adapter) _rtw_read_port23a_cancel((adapter))
-
-#define  rtw_write8(adapter, addr, val) _rtw_write823a((adapter), (addr), (val))
-#define  rtw_write16(adapter, addr, val) _rtw_write1623a((adapter), (addr), (val))
-#define  rtw_write32(adapter, addr, val) _rtw_write3223a((adapter), (addr), (val))
-#define  rtw_writeN(adapter, addr, length, data) _rtw_writeN23a((adapter), (addr), (length), (data))
-
-#define rtw_write8_async(adapter, addr, val) _rtw_write823a_async23a((adapter), (addr), (val))
-#define rtw_write16_async(adapter, addr, val) _rtw_write1623a_async((adapter), (addr), (val))
-#define rtw_write32_async(adapter, addr, val) _rtw_write3223a_async23a((adapter), (addr), (val))
-
-#define rtw_write_mem(adapter, addr, cnt, mem) _rtw_write_mem23a((adapter), (addr), (cnt), (mem))
-#define rtw_write_port(adapter, addr, cnt, mem) _rtw_write_port23a((adapter), (addr), (cnt), (mem))
-#define rtw_write_port_and_wait(adapter, addr, cnt, mem, timeout_ms) _rtw_write_port23a_and_wait23a((adapter), (addr), (cnt), (mem), (timeout_ms))
-#define rtw_write_port_cancel(adapter) _rtw_write_port23a_cancel((adapter))
-#endif /* DBG_IO */
-
-void rtw_write_scsi(struct rtw_adapter *adapter, u32 cnt, u8 *pmem);
-
-/* ioreq */
-void ioreq_read8(struct rtw_adapter *adapter, u32 addr, u8 *pval);
-void ioreq_read16(struct rtw_adapter *adapter, u32 addr, u16 *pval);
-void ioreq_read32(struct rtw_adapter *adapter, u32 addr, u32 *pval);
-void ioreq_write8(struct rtw_adapter *adapter, u32 addr, u8 val);
-void ioreq_write16(struct rtw_adapter *adapter, u32 addr, u16 val);
-void ioreq_write32(struct rtw_adapter *adapter, u32 addr, u32 val);
-
-int rtw_init_io_priv23a(struct rtw_adapter *padapter, void (*set_intf_ops)(struct _io_ops *pops));
-
-uint alloc_io_queue(struct rtw_adapter *adapter);
-void free_io_queue(struct rtw_adapter *adapter);
-void async_bus_io(struct io_queue *pio_q);
-void bus_sync_io(struct io_queue *pio_q);
-u32 _ioreq2rwmem(struct io_queue *pio_q);
-void dev_power_down(struct rtw_adapter * Adapter, u8 bpwrup);
-
 #define PlatformEFIOWrite1Byte(_a,_b,_c)		\
-	rtw_write8(_a,_b,_c)
+	rtl8723au_write8(_a,_b,_c)
 #define PlatformEFIOWrite2Byte(_a,_b,_c)		\
-	rtw_write16(_a,_b,_c)
+	rtl8723au_write16(_a,_b,_c)
 #define PlatformEFIOWrite4Byte(_a,_b,_c)		\
-	rtw_write32(_a,_b,_c)
-
-#define PlatformEFIORead1Byte(_a,_b)		\
-		rtw_read8(_a,_b)
-#define PlatformEFIORead2Byte(_a,_b)		\
-		rtw_read16(_a,_b)
-#define PlatformEFIORead4Byte(_a,_b)		\
-		rtw_read32(_a,_b)
+	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_ioctl.h b/drivers/staging/rtl8723au/include/rtw_ioctl.h
deleted file mode 100644
index 629eec8a702..00000000000
--- a/drivers/staging/rtl8723au/include/rtw_ioctl.h
+++ /dev/null
@@ -1,26 +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 _RTW_IOCTL_H_
-#define _RTW_IOCTL_H_
-
-#include <osdep_service.h>
-#include <drv_types.h>
-
-
-#if defined(CONFIG_WIRELESS_EXT)
-extern struct iw_handler_def  rtw_handlers_def;
-#endif
-
-#endif /*  #ifndef __INC_CEINFO_ */
diff --git a/drivers/staging/rtl8723au/include/rtw_ioctl_set.h b/drivers/staging/rtl8723au/include/rtw_ioctl_set.h
index 18ad2a87335..040543bff1b 100644
--- a/drivers/staging/rtl8723au/include/rtw_ioctl_set.h
+++ b/drivers/staging/rtl8723au/include/rtw_ioctl_set.h
@@ -17,23 +17,16 @@
 
 #include <drv_types.h>
 
-
-struct bssid_info {
-	unsigned char  BSSID[6];
-	u8  PMKID[16];
-};
-
-u8 rtw_set_802_11_authentication_mode23a(struct rtw_adapter *pdapter,
-				      enum ndis_802_11_auth_mode authmode);
-u8 rtw_set_802_11_add_wep23a(struct rtw_adapter * padapter,
-			  struct ndis_802_11_wep *wep);
-u8 rtw_set_802_11_bssid23a_list_scan(struct rtw_adapter *padapter,
-				  struct cfg80211_ssid *pssid, int ssid_max_num);
-u8 rtw_set_802_11_infrastructure_mode23a(struct rtw_adapter *padapter,
-				      enum ndis_802_11_net_infra networktype);
-u8 rtw_set_802_11_ssid23a(struct rtw_adapter * padapter, struct cfg80211_ssid * ssid);
+int rtw_set_802_11_authentication_mode23a(struct rtw_adapter *pdapter,
+					  enum ndis_802_11_auth_mode authmode);
+int rtw_set_802_11_bssid23a_list_scan(struct rtw_adapter *padapter,
+				      struct cfg80211_ssid *pssid,
+				      int ssid_max_num);
+int rtw_set_802_11_ssid23a(struct rtw_adapter * padapter,
+			   struct cfg80211_ssid * ssid);
 
 u16 rtw_get_cur_max_rate23a(struct rtw_adapter *adapter);
 s32 FillH2CCmd(struct rtw_adapter *padapter, u8 ElementID, u32 CmdLen, u8 *pCmdBuffer);
+int rtw_do_join23a(struct rtw_adapter *padapter);
 
 #endif
diff --git a/drivers/staging/rtl8723au/include/rtw_mlme.h b/drivers/staging/rtl8723au/include/rtw_mlme.h
index 31f96f39b49..4d327ba775b 100644
--- a/drivers/staging/rtl8723au/include/rtw_mlme.h
+++ b/drivers/staging/rtl8723au/include/rtw_mlme.h
@@ -83,13 +83,6 @@ enum {
 	GHZ_24,
 };
 
-enum SCAN_RESULT_TYPE {
-	SCAN_RESULT_P2P_ONLY = 0,	/*	Will return all the P2P devices. */
-	SCAN_RESULT_ALL = 1,		/*	Will return all the scanned device, include AP. */
-	SCAN_RESULT_WFD_TYPE = 2	/*	Will just return the correct WFD device. */
-					/*	If this device is Miracast sink device, it will just return all the Miracast source devices. */
-};
-
 /*
 
 there are several "locks" in mlme_priv,
@@ -104,16 +97,6 @@ To avoid possible dead lock, any thread trying to modifiying mlme_priv
 SHALL not lock up more than one locks at a time!
 */
 
-#define traffic_threshold	10
-#define	traffic_scan_period	500
-
-struct sitesurvey_ctrl {
-	u64	last_tx_pkts;
-	uint	last_rx_pkts;
-	int	traffic_busy;
-	struct timer_list	sitesurvey_ctrl_timer;
-};
-
 struct rt_link_detect {
 	u32	NumTxOkInPeriod;
 	u32	NumRxOkInPeriod;
@@ -126,209 +109,6 @@ struct rt_link_detect {
 	bool	bHigherBusyTxTraffic; /*  We may disable Tx interrupt according as Tx traffic. */
 };
 
-struct profile_info {
-	u8	ssidlen;
-	u8	ssid[IEEE80211_MAX_SSID_LEN];
-	u8	peermac[ETH_ALEN];
-};
-
-struct tx_invite_req_info {
-	u8	token;
-	u8	benable;
-	u8	go_ssid[IEEE80211_MAX_SSID_LEN];
-	u8	ssidlen;
-	u8	go_bssid[ETH_ALEN];
-	u8	peer_macaddr[ETH_ALEN];
-	u8	operating_ch;	/* This information will be set by using the p2p_set op_ch = x */
-	u8	peer_ch;	/* The listen channel for peer P2P device */
-
-};
-
-struct tx_invite_resp_info {
-	u8	token;	/*	Used to record the dialog token of p2p invitation request frame. */
-};
-
-#ifdef CONFIG_8723AU_P2P
-
-struct wifi_display_info {
-	u16	wfd_enable;		/* Enable/Disable the WFD function. */
-	u16	rtsp_ctrlport;		/* TCP port number at which the this WFD device listens for RTSP messages */
-	u16	peer_rtsp_ctrlport;	/* TCP port number at which the peer WFD device listens for RTSP messages */
-					/* This filed should be filled when receiving the gropu negotiation request */
-
-	u8	peer_session_avail;	/* WFD session is available or not for the peer wfd device. */
-					/* This variable will be set when sending the provisioning discovery request to peer WFD device. */
-					/* And this variable will be reset when it is read by using the iwpriv p2p_get wfd_sa command. */
-	u8	ip_address[4];
-	u8	peer_ip_address[4];
-	u8	wfd_pc;		/* WFD preferred connection */
-				/* 0 -> Prefer to use the P2P for WFD connection on peer side. */
-				/* 1 -> Prefer to use the TDLS for WFD connection on peer side. */
-
-	u8	wfd_device_type;/* WFD Device Type */
-				/* 0 -> WFD Source Device */
-				/* 1 -> WFD Primary Sink Device */
-	enum	SCAN_RESULT_TYPE scan_result_type;	/* Used when P2P is enable. This parameter will impact the scan result. */
-};
-#endif /* CONFIG_8723AU_P2P */
-
-struct tx_provdisc_req_info {
-	u16	wps_config_method_request;	/* Used when sending the provisioning request frame */
-	u16	peer_channel_num[2];		/* The channel number which the receiver stands. */
-	struct	cfg80211_ssid ssid;
-	u8	peerDevAddr[ETH_ALEN];	/* Peer device address */
-	u8	peerIFAddr[ETH_ALEN];		/* Peer interface address */
-	u8	benable;			/* This provision discovery request frame is trigger to send or not */
-};
-
-struct rx_provdisc_req_info {	/* When peer device issue prov_disc_req first, we should store the following informations */
-	u8	peerDevAddr[ETH_ALEN];		/*	Peer device address */
-	u8	strconfig_method_desc_of_prov_disc_req[4];	/*	description for the config method located in the provisioning discovery request frame. */
-																	/*	The UI must know this information to know which config method the remote p2p device is requiring. */
-};
-
-struct tx_nego_req_info {
-	u16	peer_channel_num[2];	/* The channel number which the receiver stands. */
-	u8	peerDevAddr[ETH_ALEN];/* Peer device address */
-	u8	benable;		/* This negoitation request frame is trigger to send or not */
-};
-
-struct group_id_info {
-	u8 go_device_addr[ETH_ALEN]; /*The GO's device address of P2P group */
-	u8 ssid[IEEE80211_MAX_SSID_LEN]; /* The SSID of this P2P group */
-};
-
-struct scan_limit_info {
-	u8	scan_op_ch_only;	/* When this flag is set, the driver should just scan the operation channel */
-	u8	operation_ch[2];	/* Store the operation channel of invitation request frame */
-};
-
-struct cfg80211_wifidirect_info {
-	struct timer_list		remain_on_ch_timer;
-	u8		restore_channel;
-	struct ieee80211_channel	remain_on_ch_channel;
-	enum nl80211_channel_type	remain_on_ch_type;
-	u64	remain_on_ch_cookie;
-	bool is_ro_ch;
-};
-
-struct wifidirect_info {
-	struct rtw_adapter	*padapter;
-	struct timer_list	find_phase_timer;
-	struct timer_list	restore_p2p_state_timer;
-
-	/*	Used to do the scanning. After confirming the peer is availalble, the driver transmits the P2P frame to peer. */
-	struct timer_list	pre_tx_scan_timer;
-	struct timer_list	reset_ch_sitesurvey;
-	struct timer_list	reset_ch_sitesurvey2;	/*	Just for resetting the scan limit function by using p2p nego */
-	struct tx_provdisc_req_info	tx_prov_disc_info;
-	struct rx_provdisc_req_info rx_prov_disc_info;
-	struct tx_invite_req_info	invitereq_info;
-	struct profile_info	profileinfo[P2P_MAX_PERSISTENT_GROUP_NUM];	/*	Store the profile information of persistent group */
-	struct tx_invite_resp_info	inviteresp_info;
-	struct tx_nego_req_info	nego_req_info;
-	struct group_id_info	groupid_info;	/*	Store the group id information when doing the group negotiation handshake. */
-	struct scan_limit_info	rx_invitereq_info;	/*	Used for get the limit scan channel from the Invitation procedure */
-	struct scan_limit_info	p2p_info;		/*	Used for get the limit scan channel from the P2P negotiation handshake */
-#ifdef CONFIG_8723AU_P2P
-	struct wifi_display_info	*wfd_info;
-#endif
-	enum P2P_ROLE	role;
-	enum P2P_STATE	pre_p2p_state;
-	enum P2P_STATE	p2p_state;
-	u8	device_addr[ETH_ALEN];	/*	The device address should be the mac address of this device. */
-	u8	interface_addr[ETH_ALEN];
-	u8	social_chan[4];
-	u8	listen_channel;
-	u8	operating_channel;
-	u8	listen_dwell;		/*	This value should be between 1 and 3 */
-	u8	support_rate[8];
-	u8	p2p_wildcard_ssid[P2P_WILDCARD_SSID_LEN];
-	u8	intent;		/*	should only include the intent value. */
-	u8	p2p_peer_interface_addr[ETH_ALEN];
-	u8	p2p_peer_device_addr[ETH_ALEN];
-	u8	peer_intent;	/*	Included the intent value and tie breaker value. */
-	u8	device_name[WPS_MAX_DEVICE_NAME_LEN];	/*	Device name for displaying on searching device screen */
-	u8	device_name_len;
-	u8	profileindex;	/*	Used to point to the index of profileinfo array */
-	u8	peer_operating_ch;
-	u8	find_phase_state_exchange_cnt;
-	u16	device_password_id_for_nego;	/*	The device password ID for group negotation */
-	u8	negotiation_dialog_token;
-	/*	SSID information for group negotitation */
-	u8 nego_ssid[IEEE80211_MAX_SSID_LEN];
-	u8 nego_ssidlen;
-	u8 p2p_group_ssid[IEEE80211_MAX_SSID_LEN];
-	u8 p2p_group_ssid_len;
-	u8	persistent_supported;	/*	Flag to know the persistent function should be supported or not. */
-					/*	In the Sigma test, the Sigma will provide this enable from the sta_set_p2p CAPI. */
-					/*	0: disable */
-					/*	1: enable */
-	u8	session_available;	/*	Flag to set the WFD session available to enable or disable "by Sigma" */
-					/*	In the Sigma test, the Sigma will disable the session available by using the sta_preset CAPI. */
-					/*	0: disable */
-					/*	1: enable */
-
-	u8	wfd_tdls_enable;	/*	Flag to enable or disable the TDLS by WFD Sigma */
-					/*	0: disable */
-					/*	1: enable */
-	u8	wfd_tdls_weaksec;	/*	Flag to enable or disable the weak security function for TDLS by WFD Sigma */
-				/*	0: disable */
-				/*	In this case, the driver can't issue the tdsl setup request frame. */
-				/*	1: enable */
-				/*	In this case, the driver can issue the tdls setup request frame */
-				/*	even the current security is weak security. */
-
-	enum	P2P_WPSINFO		ui_got_wps_info;			/*	This field will store the WPS value (PIN value or PBC) that UI had got from the user. */
-	u16	supported_wps_cm;			/*	This field describes the WPS config method which this driver supported. */
-														/*	The value should be the combination of config method defined in page104 of WPS v2.0 spec. */
-	uint	channel_list_attr_len;		/*	This field will contain the length of body of P2P Channel List attribute of group negotitation response frame. */
-	u8	channel_list_attr[100];		/*	This field will contain the body of P2P Channel List attribute of group negotitation response frame. */
-														/*	We will use the channel_cnt and channel_list fields when constructing the group negotitation confirm frame. */
-#ifdef CONFIG_8723AU_P2P
-	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. */
-	u8	ctwindow; /*  Client traffic window. A period of time in TU after TBTT. */
-	u8	opp_ps; /*  opportunistic power save. */
-	u8	noa_num; /*  number of NoA descriptor in P2P IE. */
-	u8	noa_count[P2P_MAX_NOA_NUM]; /*  Count for owner, Type of client. */
-	u32	noa_duration[P2P_MAX_NOA_NUM]; /*  Max duration for owner, preferred or min acceptable duration for client. */
-	u32	noa_interval[P2P_MAX_NOA_NUM]; /*  Length of interval for owner, preferred or max acceptable interval of client. */
-	u32	noa_start_time[P2P_MAX_NOA_NUM]; /*  schedule expressed in terms of the lower 4 bytes of the TSF timer. */
-#endif /*  CONFIG_8723AU_P2P */
-};
-
-struct tdls_ss_record {	/* signal strength record */
-	u8	macaddr[ETH_ALEN];
-	u8	RxPWDBAll;
-	u8	is_tdls_sta;	/*  true: direct link sta, false: else */
-};
-
-struct tdls_info {
-	u8	ap_prohibited;
-	uint	setup_state;
-	u8	sta_cnt;
-	/* 1:tdls sta == (NUM_STA-1), reach max direct link no; 0: else; */
-	u8	sta_maximum;
-	struct tdls_ss_record	ss_record;
-	u8	macid_index;	/* macid entry that is ready to write */
-	/* cam entry that is trying to clear, using it in direct link teardown*/
-	u8	clear_cam;
-	u8	ch_sensing;
-	u8	cur_channel;
-	u8	candidate_ch;
-	u8	collect_pkt_num[MAX_CHANNEL_NUM];
-	spinlock_t	cmd_lock;
-	spinlock_t	hdl_lock;
-	u8	watchdog_count;
-	u8	dev_discovered;		/* WFD_TDLS: for sigma test */
-	u8	enable;
-#ifdef CONFIG_8723AU_P2P
-	struct wifi_display_info		*wfd_info;
-#endif
-};
-
 struct mlme_priv {
 	spinlock_t	lock;
 	int	fw_state;
@@ -360,7 +140,7 @@ struct mlme_priv {
 	struct timer_list set_scan_deny_timer;
 	atomic_t set_scan_deny; /* 0: allowed, 1: deny */
 
-	struct qos_priv qospriv;
+	unsigned int qos_option;
 
 	/* Number of non-HT AP/stations */
 	int num_sta_no_ht;
@@ -445,30 +225,17 @@ struct mlme_priv {
 	u32 wfd_go_probe_resp_ie_len; /* for GO */
 };
 
-#ifdef CONFIG_8723AU_AP_MODE
-
-struct hostapd_priv {
-	struct rtw_adapter *padapter;
-};
-
-int hostapd_mode_init(struct rtw_adapter *padapter);
-void hostapd_mode_unload(struct rtw_adapter *padapter);
-#endif
-
 void rtw_joinbss_event_prehandle23a(struct rtw_adapter *adapter, u8 *pbuf);
-void rtw_survey_event_cb23a(struct rtw_adapter *adapter, u8 *pbuf);
-void rtw_surveydone_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf);
-void rtw23a_joinbss_event_cb(struct rtw_adapter *adapter, u8 *pbuf);
-void rtw_stassoc_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf);
-void rtw_stadel_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf);
-void rtw_atimdone_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf);
-void rtw_cpwm_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf);
-
+void rtw_survey_event_cb23a(struct rtw_adapter *adapter, const u8 *pbuf);
+void rtw_surveydone_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf);
+void rtw23a_joinbss_event_cb(struct rtw_adapter *adapter, const u8 *pbuf);
+void rtw_stassoc_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf);
+void rtw_stadel_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf);
 
 int event_thread(void *context);
 void rtw23a_join_to_handler(unsigned long);
 
-void rtw_free_network_queue23a(struct rtw_adapter *adapter, u8 isfreeall);
+void rtw_free_network_queue23a(struct rtw_adapter *adapter);
 int rtw_init_mlme_priv23a(struct rtw_adapter *adapter);
 
 void rtw_free_mlme_priv23a(struct mlme_priv *pmlmepriv);
@@ -485,7 +252,7 @@ static inline u8 *get_bssid(struct mlme_priv *pmlmepriv)
 	return pmlmepriv->cur_network.network.MacAddress;
 }
 
-static inline int check_fwstate(struct mlme_priv *pmlmepriv, int state)
+static inline bool check_fwstate(struct mlme_priv *pmlmepriv, int state)
 {
 	if (pmlmepriv->fw_state & state)
 		return true;
@@ -553,7 +320,6 @@ void rtw_free_assoc_resources23a(struct rtw_adapter *adapter,
 			      int lock_scanned_queue);
 void rtw_indicate_disconnect23a(struct rtw_adapter *adapter);
 void rtw_indicate_connect23a(struct rtw_adapter *adapter);
-void rtw_indicate_scan_done23a(struct rtw_adapter *padapter, bool aborted);
 void rtw_scan_abort23a(struct rtw_adapter *adapter);
 
 int rtw_restruct_sec_ie23a(struct rtw_adapter *adapter, u8 *in_ie, u8 *out_ie,
@@ -574,30 +340,18 @@ void rtw_clear_scan_deny(struct rtw_adapter *adapter);
 void rtw_set_scan_deny_timer_hdl(unsigned long data);
 void rtw_set_scan_deny(struct rtw_adapter *adapter, u32 ms);
 
-int _rtw_init_mlme_priv23a(struct rtw_adapter *padapter);
-
 void rtw23a_free_mlme_priv_ie_data(struct mlme_priv *pmlmepriv);
 
 void _rtw_free_mlme_priv23a(struct mlme_priv *pmlmepriv);
 
-struct wlan_network *rtw_alloc_network(struct mlme_priv *pmlmepriv);
-
-void _rtw_free_network23a(struct mlme_priv *pmlmepriv,
-		       struct wlan_network *pnetwork, u8 isfreeall);
-void _rtw_free_network23a_nolock23a(struct mlme_priv *pmlmepriv,
-			      struct wlan_network *pnetwork);
-
-struct wlan_network *_rtw_find_network23a(struct rtw_queue *scanned_queue, u8 *addr);
-
-void _rtw_free_network23a_queue23a(struct rtw_adapter *padapter, u8 isfreeall);
+struct wlan_network *rtw_alloc_network(struct mlme_priv *pmlmepriv, int gfp);
 
 int rtw_if_up23a(struct rtw_adapter *padapter);
 
 int rtw_linked_check(struct rtw_adapter *padapter);
 
-u8 *rtw_get_capability23a_from_ie(u8 *ie);
-u8 *rtw_get_timestampe_from_ie23a(u8 *ie);
-u8 *rtw_get_beacon_interval23a_from_ie(u8 *ie);
+__le16 *rtw_get_capability23a_from_ie(u8 *ie);
+__le16 *rtw_get_beacon_interval23a_from_ie(u8 *ie);
 
 
 void rtw_joinbss_reset23a(struct rtw_adapter *padapter);
@@ -613,12 +367,8 @@ int rtw_is_same_ibss23a(struct rtw_adapter *adapter,
 		     struct wlan_network *pnetwork);
 int is_same_network23a(struct wlan_bssid_ex *src, struct wlan_bssid_ex *dst);
 
-void _rtw23a_roaming(struct rtw_adapter *adapter,
-		  struct wlan_network *tgt_network);
 void rtw23a_roaming(struct rtw_adapter *adapter,
 		 struct wlan_network *tgt_network);
 void rtw_set_roaming(struct rtw_adapter *adapter, u8 to_roaming);
-u8 rtw_to_roaming(struct rtw_adapter *adapter);
-void rtw_stassoc_hw_rpt23a(struct rtw_adapter *adapter, struct sta_info *psta);
 
 #endif /* __RTL871X_MLME_H_ */
diff --git a/drivers/staging/rtl8723au/include/rtw_mlme_ext.h b/drivers/staging/rtl8723au/include/rtw_mlme_ext.h
index 0aaf0d5d8ae..badbce09559 100644
--- a/drivers/staging/rtl8723au/include/rtw_mlme_ext.h
+++ b/drivers/staging/rtl8723au/include/rtw_mlme_ext.h
@@ -89,7 +89,6 @@
 #define		_54M_RATE_	11
 
 
-extern unsigned char RTW_WPA_OUI23A[];
 extern unsigned char WMM_OUI23A[];
 extern unsigned char WPS_OUI23A[];
 extern unsigned char WFD_OUI23A[];
@@ -262,13 +261,13 @@ enum SCAN_STATE {
 
 struct mlme_handler {
 	char *str;
-	unsigned int (*func)(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+	int (*func)(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
 };
 
 struct action_handler {
 	unsigned int   num;
 	char* str;
-	unsigned int (*func)(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+	int (*func)(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
 };
 
 struct	ss_res
@@ -358,9 +357,7 @@ struct mlme_ext_info
 	u8	turboMode_cts2self;
 	u8	turboMode_rtsen;
 	u8	SM_PS;
-	u8	agg_enable_bitmap;
 	u8	ADDBA_retry_count;
-	u8	candidate_tid_bitmap;
 	u8	dialogToken;
 	/*  Accept ADDBA Request */
 	bool bAcceptAddbaReq;
@@ -474,10 +471,6 @@ void get_rate_set23a(struct rtw_adapter *padapter, unsigned char *pbssrate,
 void UpdateBrateTbl23a(struct rtw_adapter *padapter,u8 *mBratesOS);
 void Update23aTblForSoftAP(u8 *bssrateset, u32 bssratelen);
 
-void Save_DM_Func_Flag23a(struct rtw_adapter *padapter);
-void Restore_DM_Func_Flag23a(struct rtw_adapter *padapter);
-void Switch_DM_Func23a(struct rtw_adapter *padapter, unsigned long mode, u8 enable);
-
 void Set_MSR23a(struct rtw_adapter *padapter, u8 type);
 
 u8 rtw_get_oper_ch23a(struct rtw_adapter *adapter);
@@ -490,61 +483,47 @@ void rtw_set_oper_ch23aoffset23a(struct rtw_adapter *adapter, u8 offset);
 void set_channel_bwmode23a(struct rtw_adapter *padapter, unsigned char channel,
 			unsigned char channel_offset, unsigned short bwmode);
 void SelectChannel23a(struct rtw_adapter *padapter, unsigned char channel);
-void SetBWMode23a(struct rtw_adapter *padapter, unsigned short bwmode,
-	       unsigned char channel_offset);
 
 unsigned int decide_wait_for_beacon_timeout23a(unsigned int bcn_interval);
 
-void write_cam23a(struct rtw_adapter *padapter, u8 entry, u16 ctrl,
-	       u8 *mac, u8 *key);
 void clear_cam_entry23a(struct rtw_adapter *padapter, u8 entry);
 
 void invalidate_cam_all23a(struct rtw_adapter *padapter);
-void CAM_empty_entry23a(struct rtw_adapter *Adapter, u8 ucIndex);
 
 int allocate_fw_sta_entry23a(struct rtw_adapter *padapter);
 void flush_all_cam_entry23a(struct rtw_adapter *padapter);
 
 bool IsLegal5GChannel(struct rtw_adapter *Adapter, u8 channel);
 
-void site_survey23a(struct rtw_adapter *padapter);
-u8 collect_bss_info23a(struct rtw_adapter *padapter,
-		    struct recv_frame *precv_frame,
-		    struct wlan_bssid_ex *bssid);
+int collect_bss_info23a(struct rtw_adapter *padapter,
+			struct recv_frame *precv_frame,
+			struct wlan_bssid_ex *bssid);
 void update_network23a(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src,
 		    struct rtw_adapter *padapter, bool update_ie);
 
-int get_bsstype23a(unsigned short capability);
 u8 *get_my_bssid23a(struct wlan_bssid_ex *pnetwork);
 u16 get_beacon_interval23a(struct wlan_bssid_ex *bss);
 
-int is_client_associated_to_ap23a(struct rtw_adapter *padapter);
-int is_client_associated_to_ibss23a(struct rtw_adapter *padapter);
-int is_IBSS_empty23a(struct rtw_adapter *padapter);
+bool is_client_associated_to_ap23a(struct rtw_adapter *padapter);
+bool is_client_associated_to_ibss23a(struct rtw_adapter *padapter);
+bool is_IBSS_empty23a(struct rtw_adapter *padapter);
 
 unsigned char check_assoc_AP23a(u8 *pframe, uint len);
 
-int WMM_param_handler23a(struct rtw_adapter *padapter,
-		      struct ndis_802_11_var_ies *pIE);
-#ifdef CONFIG_8723AU_P2P
-int WFD_info_handler(struct rtw_adapter *padapter,
-		     struct ndis_802_11_var_ies *pIE);
-#endif
+int WMM_param_handler23a(struct rtw_adapter *padapter, u8 *p);
 void WMMOnAssocRsp23a(struct rtw_adapter *padapter);
 
-void HT_caps_handler23a(struct rtw_adapter *padapter,
-		     struct ndis_802_11_var_ies *pIE);
-void HT_info_handler23a(struct rtw_adapter *padapter,
-		     struct ndis_802_11_var_ies *pIE);
+void HT_caps_handler23a(struct rtw_adapter *padapter, u8 *p);
+void HT_info_handler23a(struct rtw_adapter *padapter, u8 *p);
 void HTOnAssocRsp23a(struct rtw_adapter *padapter);
 
-void ERP_IE_handler23a(struct rtw_adapter *padapter,
-		    struct ndis_802_11_var_ies *pIE);
+void ERP_IE_handler23a(struct rtw_adapter *padapter, u8 *p);
 void VCS_update23a(struct rtw_adapter *padapter, struct sta_info *psta);
 
 void update_beacon23a_info(struct rtw_adapter *padapter, u8 *pframe, uint len,
 			struct sta_info *psta);
-int rtw_check_bcn_info23a(struct rtw_adapter *Adapter, u8 *pframe, u32 packet_len);
+int rtw_check_bcn_info23a(struct rtw_adapter *Adapter,
+			  struct ieee80211_mgmt *mgmt, u32 packet_len);
 void update_IOT_info23a(struct rtw_adapter *padapter);
 void update_capinfo23a(struct rtw_adapter *Adapter, u16 updateCap);
 void update_wireless_mode23a(struct rtw_adapter * padapter);
@@ -561,15 +540,15 @@ unsigned int update_MSC_rate23a(struct HT_caps_element *pHT_caps);
 void Update_RA_Entry23a(struct rtw_adapter *padapter, struct sta_info *psta);
 void set_sta_rate23a(struct rtw_adapter *padapter, struct sta_info *psta);
 
-unsigned int receive_disconnect23a(struct rtw_adapter *padapter,
-				unsigned char *MacAddr, unsigned short reason);
+int receive_disconnect23a(struct rtw_adapter *padapter,
+			  unsigned char *MacAddr, unsigned short reason);
 
 unsigned char get_highest_rate_idx23a(u32 mask);
 int support_short_GI23a(struct rtw_adapter *padapter,
 		     struct HT_caps_element *pHT_caps);
-unsigned int is_ap_in_tkip23a(struct rtw_adapter *padapter);
-unsigned int is_ap_in_wep23a(struct rtw_adapter *padapter);
-unsigned int should_forbid_n_rate23a(struct rtw_adapter *padapter);
+bool is_ap_in_tkip23a(struct rtw_adapter *padapter);
+bool is_ap_in_wep23a(struct rtw_adapter *padapter);
+bool should_forbid_n_rate23a(struct rtw_adapter *padapter);
 
 void report_join_res23a(struct rtw_adapter *padapter, int res);
 void report_survey_event23a(struct rtw_adapter *padapter,
@@ -580,8 +559,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);
 
-void beacon_timing_control23a(struct rtw_adapter *padapter);
-u8 set_tx_beacon_cmd23a(struct rtw_adapter*padapter);
+int set_tx_beacon_cmd23a(struct rtw_adapter*padapter);
 unsigned int setup_beacon_frame(struct rtw_adapter *padapter,
 				unsigned char *beacon_frame);
 void update_mgnt_tx_rate23a(struct rtw_adapter *padapter, u8 rate);
@@ -594,72 +572,20 @@ s32 dump_mgntframe23a_and_wait(struct rtw_adapter *padapter,
 s32 dump_mgntframe23a_and_wait_ack23a(struct rtw_adapter *padapter,
 				struct xmit_frame *pmgntframe);
 
-#ifdef CONFIG_8723AU_P2P
-void issue_probersp23a_p2p23a(struct rtw_adapter *padapter, unsigned char *da);
-void issue_p2p_provision_request23a(struct rtw_adapter *padapter, u8 *pssid,
-				 u8 ussidlen, u8* pdev_raddr);
-void issue_p2p_GO_request23a(struct rtw_adapter *padapter, u8* raddr);
-void issue23a_probereq_p2p(struct rtw_adapter *padapter, u8 *da);
-int issue23a_probereq_p2p_ex(struct rtw_adapter *adapter, u8 *da, int try_cnt,
-			  int wait_ms);
-void issue_p2p_invitation_response23a(struct rtw_adapter *padapter, u8* raddr,
-				   u8 dialogToken, u8 success);
-void issue_p2p_invitation_request23a(struct rtw_adapter *padapter, u8* raddr);
-#endif /* CONFIG_8723AU_P2P */
 void issue_beacon23a(struct rtw_adapter *padapter, int timeout_ms);
-void issue_probersp23a(struct rtw_adapter *padapter, unsigned char *da,
-		    u8 is_valid_p2p_probereq);
-void issue_assocreq23a(struct rtw_adapter *padapter);
-void issue_asocrsp23a(struct rtw_adapter *padapter, unsigned short status,
-		   struct sta_info *pstat, int pkt_type);
-void issue_auth23a(struct rtw_adapter *padapter, struct sta_info *psta,
-		unsigned short status);
-void issue_probereq23a(struct rtw_adapter *padapter, struct cfg80211_ssid *pssid,
-		    u8 *da);
-s32 issue_probereq23a_ex23a(struct rtw_adapter *padapter, struct cfg80211_ssid *pssid,
-		      u8 *da, int try_cnt, int wait_ms);
 int issue_nulldata23a(struct rtw_adapter *padapter, unsigned char *da,
 		   unsigned int power_mode, int try_cnt, int wait_ms);
 int issue_qos_nulldata23a(struct rtw_adapter *padapter, unsigned char *da, u16 tid,
 		       int try_cnt, int wait_ms);
 int issue_deauth23a(struct rtw_adapter *padapter, unsigned char *da,
 		 unsigned short reason);
-int issue_deauth23a_ex23a(struct rtw_adapter *padapter, u8 *da, unsigned short reason,
-		    int try_cnt, int wait_ms);
 void issue_action_spct_ch_switch23a(struct rtw_adapter *padapter, u8 *ra,
 				 u8 new_ch, u8 ch_offset);
-void issue_action_BA23a(struct rtw_adapter *padapter, unsigned char *raddr,
-		     unsigned char action, unsigned short status);
-unsigned int send_delba23a(struct rtw_adapter *padapter, u8 initiator, u8 *addr);
-unsigned int send_beacon23a(struct rtw_adapter *padapter);
-
-void start_clnt_assoc23a(struct rtw_adapter *padapter);
-void start_clnt_auth23a(struct rtw_adapter *padapter);
-void start_clnt_join23a(struct rtw_adapter *padapter);
-void start_create_ibss23a(struct rtw_adapter *padapter);
-
-unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnAssocRsp23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnProbeReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnProbeRsp23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int DoReserved23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnBeacon23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnAtim23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnDisassoc23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnAuth23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnAuth23aClient23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnDeAuth23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnAction23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-
-unsigned int on_action_spct23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnAction23a_qos(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnAction23a_dls(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnAction23a_back23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int on_action_public23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnAction23a_ht(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnAction23a_wmm(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnAction23a_p2p(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-
+void issue_action_BA23a(struct rtw_adapter *padapter,
+			const unsigned char *raddr,
+			unsigned char action, unsigned short status);
+int send_delba23a(struct rtw_adapter *padapter, u8 initiator, u8 *addr);
+int send_beacon23a(struct rtw_adapter *padapter);
 
 void mlmeext_joinbss_event_callback23a(struct rtw_adapter *padapter, int join_res);
 void mlmeext_sta_del_event_callback23a(struct rtw_adapter *padapter);
@@ -668,11 +594,11 @@ void mlmeext_sta_add_event_callback23a(struct rtw_adapter *padapter, struct sta_
 void linked_status_chk23a(struct rtw_adapter *padapter);
 
 #define set_survey_timer(mlmeext, ms) \
-	/*DBG_8723A("%s set_survey_timer(%p, %d)\n", __FUNCTION__, (mlmeext), (ms));*/ \
+	/*DBG_8723A("%s set_survey_timer(%p, %d)\n", __func__, (mlmeext), (ms));*/ \
 	mod_timer(&mlmeext->survey_timer, jiffies + msecs_to_jiffies(ms));
 
 #define set_link_timer(mlmeext, ms) \
-	/*DBG_8723A("%s set_link_timer(%p, %d)\n", __FUNCTION__, (mlmeext), (ms));*/ \
+	/*DBG_8723A("%s set_link_timer(%p, %d)\n", __func__, (mlmeext), (ms));*/ \
 	mod_timer(&mlmeext->link_timer, jiffies + msecs_to_jiffies(ms));
 
 int cckrates_included23a(unsigned char *rate, int ratelen);
@@ -680,44 +606,43 @@ int cckratesonly_included23a(unsigned char *rate, int ratelen);
 
 void process_addba_req23a(struct rtw_adapter *padapter, u8 *paddba_req, u8 *addr);
 
-void update_TSF23a(struct mlme_ext_priv *pmlmeext, u8 *pframe, uint len);
 void correct_TSF23a(struct rtw_adapter *padapter, struct mlme_ext_priv *pmlmeext);
 
 struct cmd_hdl {
 	uint	parmsize;
-	u8 (*h2cfuns)(struct rtw_adapter *padapter, u8 *pbuf);
+	int (*h2cfuns)(struct rtw_adapter *padapter, const u8 *pbuf);
 };
 
 
-u8 read_macreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
-u8 write_macreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
-u8 read_bbreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
-u8 write_bbreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
-u8 read_rfreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
-u8 write_rfreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
-
-
-u8 NULL_hdl23a(struct rtw_adapter *padapter, u8 *pbuf);
-u8 join_cmd_hdl23a(struct rtw_adapter *padapter, u8 *pbuf);
-u8 disconnect_hdl23a(struct rtw_adapter *padapter, u8 *pbuf);
-u8 createbss_hdl23a(struct rtw_adapter *padapter, u8 *pbuf);
-u8 setopmode_hdl23a(struct rtw_adapter *padapter, u8 *pbuf);
-u8 sitesurvey_cmd_hdl23a(struct rtw_adapter *padapter, u8 *pbuf);
-u8 setauth_hdl23a(struct rtw_adapter *padapter, u8 *pbuf);
-u8 setkey_hdl23a(struct rtw_adapter *padapter, u8 *pbuf);
-u8 set_stakey_hdl23a(struct rtw_adapter *padapter, u8 *pbuf);
-u8 set_assocsta_hdl(struct rtw_adapter *padapter, u8 *pbuf);
-u8 del_assocsta_hdl(struct rtw_adapter *padapter, u8 *pbuf);
-u8 add_ba_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf);
-
-u8 mlme_evt_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf);
-u8 h2c_msg_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf);
-u8 tx_beacon_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf);
-u8 set_ch_hdl23a(struct rtw_adapter *padapter, u8 *pbuf);
-u8 set_chplan_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf);
-u8 led_blink_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf);
-u8 set_csa_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf);	/* Kurt: Handling DFS channel switch announcement ie. */
-u8 tdls_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf);
+int read_macreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
+int write_macreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
+int read_bbreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
+int write_bbreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
+int read_rfreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
+int write_rfreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
+
+
+int NULL_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int join_cmd_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int disconnect_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int createbss_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int setopmode_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int sitesurvey_cmd_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int setauth_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int setkey_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int set_stakey_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int set_assocsta_hdl(struct rtw_adapter *padapter, const u8 *pbuf);
+int del_assocsta_hdl(struct rtw_adapter *padapter, const u8 *pbuf);
+int add_ba_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+
+int mlme_evt_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int h2c_msg_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int tx_beacon_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int set_ch_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int set_chplan_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int led_blink_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int set_csa_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);	/* Kurt: Handling DFS channel switch announcement ie. */
+int tdls_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
 
 #define GEN_DRV_CMD_HANDLER(size, cmd)	{size, &cmd ## _hdl23a},
 #define GEN_MLME_EXT_HANDLER(size, cmd)	{size, cmd},
@@ -744,9 +669,6 @@ struct C2HEvent_Header {
 	unsigned int rsvd;
 };
 
-void rtw_dummy_event_callback23a(struct rtw_adapter *adapter , u8 *pbuf);
-void rtw23a_fwdbg_event_callback(struct rtw_adapter *adapter , u8 *pbuf);
-
 enum rtw_c2h_event {
 	GEN_EVT_CODE(_Read_MACREG) = 0, /*0*/
 	GEN_EVT_CODE(_Read_BBREG),
diff --git a/drivers/staging/rtl8723au/include/rtw_p2p.h b/drivers/staging/rtl8723au/include/rtw_p2p.h
deleted file mode 100644
index 93fdc658ff4..00000000000
--- a/drivers/staging/rtl8723au/include/rtw_p2p.h
+++ /dev/null
@@ -1,158 +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 __RTW_P2P_H_
-#define __RTW_P2P_H_
-
-#include <drv_types.h>
-
-u32 build_beacon_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_probe_resp_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_prov_disc_request_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf,
-				   u8 *pssid, u8 ussidlen, u8 *pdev_raddr);
-u32 build_assoc_resp_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf,
-			    u8 status_code);
-u32 build_deauth_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf);
-#ifdef CONFIG_8723AU_P2P
-u32 build_probe_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_probe_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf,
-			    u8 tunneled);
-u32 build_beacon_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_nego_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_nego_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_nego_confirm_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_invitation_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_invitation_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_assoc_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_assoc_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_provdisc_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_provdisc_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
-#endif /* CONFIG_8723AU_P2P */
-
-u32 process_probe_req_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pframe,
-			     uint len);
-u32 process_assoc_req_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pframe,
-			     uint len, struct sta_info *psta);
-u32 process_p2p_devdisc_req23a(struct wifidirect_info *pwdinfo, u8 *pframe,
-			    uint len);
-u32 process_p2p_devdisc_resp23a(struct wifidirect_info *pwdinfo, u8 *pframe,
-			     uint len);
-u8 process_p2p_provdisc_req23a(struct wifidirect_info *pwdinfo, u8 *pframe,
-			    uint len);
-u8 process_p2p_provdisc_resp23a(struct wifidirect_info *pwdinfo, u8 *pframe);
-u8 process_p2p_group_negotation_req23a(struct wifidirect_info *pwdinfo,
-				    u8 *pframe, uint len);
-u8 process_p2p_group_negotation_resp23a(struct wifidirect_info *pwdinfo,
-				     u8 *pframe, uint len);
-u8 process_p2p_group_negotation_confirm23a(struct wifidirect_info *pwdinfo,
-					u8 *pframe, uint len);
-u8 process_p2p_presence_req23a(struct wifidirect_info *pwdinfo,
-			    u8 *pframe, uint len);
-
-void p2p_protocol_wk_hdl23a(struct rtw_adapter *padapter, int cmdtype);
-
-#ifdef CONFIG_8723AU_P2P
-void	process_p2p_ps_ie23a(struct rtw_adapter *padapter, u8 *IEs, u32 IELength);
-void	p2p_ps_wk_hdl23a(struct rtw_adapter *padapter, u8 p2p_ps_state);
-u8 p2p_ps_wk_cmd23a(struct rtw_adapter *padapter, u8 p2p_ps_state, u8 enqueue);
-#endif /*  CONFIG_8723AU_P2P */
-
-void rtw_init_cfg80211_wifidirect_info(struct rtw_adapter *padapter);
-int rtw_p2p_check_frames(struct rtw_adapter *padapter, const u8 *buf,
-			 u32 len, u8 tx);
-void rtw_append_wfd_ie(struct rtw_adapter *padapter, u8 *buf, u32 *len);
-
-void reset_global_wifidirect_info23a(struct rtw_adapter *padapter);
-int rtw_init_wifi_display_info(struct rtw_adapter *padapter);
-void rtw_init_wifidirect_timers23a(struct rtw_adapter *padapter);
-void rtw_init_wifidirect_addrs23a(struct rtw_adapter *padapter, u8 *dev_addr,
-			       u8 *iface_addr);
-void init_wifidirect_info23a(struct rtw_adapter *padapter, enum P2P_ROLE role);
-int rtw_p2p_enable23a(struct rtw_adapter *padapter, enum P2P_ROLE role);
-
-static inline void _rtw_p2p_set_state(struct wifidirect_info *wdinfo,
-				      enum P2P_STATE state)
-{
-	if (wdinfo->p2p_state != state) {
-		/* wdinfo->pre_p2p_state = wdinfo->p2p_state; */
-		wdinfo->p2p_state = state;
-	}
-}
-
-static inline void _rtw_p2p_set_pre_state(struct wifidirect_info *wdinfo,
-					  enum P2P_STATE state)
-{
-	if (wdinfo->pre_p2p_state != state)
-		wdinfo->pre_p2p_state = state;
-}
-
-static inline void _rtw_p2p_set_role(struct wifidirect_info *wdinfo,
-				     enum P2P_ROLE role)
-{
-	if (wdinfo->role != role)
-		wdinfo->role = role;
-}
-
-static inline int _rtw_p2p_state(struct wifidirect_info *wdinfo)
-{
-	return wdinfo->p2p_state;
-}
-
-static inline int _rtw_p2p_pre_state(struct wifidirect_info *wdinfo)
-{
-	return wdinfo->pre_p2p_state;
-}
-
-static inline int _rtw_p2p_role(struct wifidirect_info *wdinfo)
-{
-	return wdinfo->role;
-}
-
-static inline bool _rtw_p2p_chk_state(struct wifidirect_info *wdinfo,
-				      enum P2P_STATE state)
-{
-	return wdinfo->p2p_state == state;
-}
-
-static inline bool _rtw_p2p_chk_role(struct wifidirect_info *wdinfo,
-				     enum P2P_ROLE role)
-{
-	return wdinfo->role == role;
-}
-
-#define rtw_p2p_set_state(wdinfo, state) _rtw_p2p_set_state(wdinfo, state)
-#define rtw_p2p_set_pre_state(wdinfo, state)			\
-	_rtw_p2p_set_pre_state(wdinfo, state)
-#define rtw_p2p_set_role(wdinfo, role) _rtw_p2p_set_role(wdinfo, role)
-
-#define rtw_p2p_state(wdinfo) _rtw_p2p_state(wdinfo)
-#define rtw_p2p_pre_state(wdinfo) _rtw_p2p_pre_state(wdinfo)
-#define rtw_p2p_role(wdinfo) _rtw_p2p_role(wdinfo)
-#define rtw_p2p_chk_state(wdinfo, state) _rtw_p2p_chk_state(wdinfo, state)
-#define rtw_p2p_chk_role(wdinfo, role) _rtw_p2p_chk_role(wdinfo, role)
-
-#define rtw_p2p_findphase_ex_set(wdinfo, value) \
-	((wdinfo)->find_phase_state_exchange_cnt = (value))
-
-/* is this find phase exchange for social channel scan? */
-#define rtw_p2p_findphase_ex_is_social(wdinfo)			\
-	((wdinfo)->find_phase_state_exchange_cnt >=		\
-	 P2P_FINDPHASE_EX_SOCIAL_FIRST)
-
-/* should we need find phase exchange anymore? */
-#define rtw_p2p_findphase_ex_is_needed(wdinfo) \
-	((wdinfo)->find_phase_state_exchange_cnt < P2P_FINDPHASE_EX_MAX && \
-	(wdinfo)->find_phase_state_exchange_cnt != P2P_FINDPHASE_EX_NONE)
-
-#endif
diff --git a/drivers/staging/rtl8723au/include/rtw_pwrctrl.h b/drivers/staging/rtl8723au/include/rtw_pwrctrl.h
index e0da87d4d3d..a458af97814 100644
--- a/drivers/staging/rtl8723au/include/rtw_pwrctrl.h
+++ b/drivers/staging/rtl8723au/include/rtw_pwrctrl.h
@@ -165,7 +165,6 @@ struct pwrctrl_priv {
 	struct work_struct cpwm_event;
 	u8	bpower_saving;
 
-	u8	b_hw_radio_off;
 	u8	reg_rfoff;
 	u8	reg_pdnmode; /* powerdown mode */
 	u32	rfoff_reason;
@@ -192,7 +191,6 @@ struct pwrctrl_priv {
 	s32		pnp_current_pwr_state;
 	u8		pnp_bstop_trx;
 
-	u8		bInternalAutoSuspend;
 	u8		bInSuspend;
 #ifdef	CONFIG_8723AU_BT_COEXIST
 	u8		bAutoResume;
@@ -250,8 +248,6 @@ s32 LPS_RF_ON_check23a(struct rtw_adapter *padapter, u32 delay_ms);
 void LPS_Enter23a(struct rtw_adapter *padapter);
 void LPS_Leave23a(struct rtw_adapter *padapter);
 
-u8 rtw_interface_ps_func23a(struct rtw_adapter *padapter,
-			 enum hal_intf_ps_func efunc_id, u8 *val);
 void rtw_set_ips_deny23a(struct rtw_adapter *padapter, u32 ms);
 int _rtw_pwr_wakeup23a(struct rtw_adapter *padapter, u32 ips_deffer_ms,
 		    const char *caller);
diff --git a/drivers/staging/rtl8723au/include/rtw_qos.h b/drivers/staging/rtl8723au/include/rtw_qos.h
deleted file mode 100644
index 68fc5ba1844..00000000000
--- a/drivers/staging/rtl8723au/include/rtw_qos.h
+++ /dev/null
@@ -1,26 +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 _RTW_QOS_H_
-#define _RTW_QOS_H_
-
-#include <osdep_service.h>
-
-struct	qos_priv	{
-	/* bit mask option: u-apsd, s-apsd, ts, block ack... */
-	unsigned int	  qos_option;
-};
-
-#endif	/* _RTL871X_QOS_H_ */
diff --git a/drivers/staging/rtl8723au/include/rtw_recv.h b/drivers/staging/rtl8723au/include/rtw_recv.h
index d1866a6e831..f846bb5e7ab 100644
--- a/drivers/staging/rtl8723au/include/rtw_recv.h
+++ b/drivers/staging/rtl8723au/include/rtw_recv.h
@@ -99,7 +99,6 @@ struct rx_pkt_attrib	{
 	u8	drvinfo_sz;
 	u8	shift_sz;
 	u8	hdrlen; /* the WLAN Header Len */
-	u8	to_fr_ds;
 	u8	amsdu;
 	u8	qos;
 	u8	priority;
@@ -112,7 +111,7 @@ struct rx_pkt_attrib	{
 	u8	privacy; /* in frame_ctrl field */
 	u8	bdecrypted;
 	/* when 0 indicate no encrypt. when non-zero, indicate the algorith */
-	u8	encrypt;
+	u32	encrypt;
 	u8	iv_len;
 	u8	icv_len;
 	u8	crc_err;
@@ -152,12 +151,12 @@ struct rx_pkt_attrib	{
 #define RXDESC_OFFSET RXDESC_SIZE
 
 struct recv_stat {
-	unsigned int rxdw0;
-	unsigned int rxdw1;
-	unsigned int rxdw2;
-	unsigned int rxdw3;
-	unsigned int rxdw4;
-	unsigned int rxdw5;
+	__le32 rxdw0;
+	__le32 rxdw1;
+	__le32 rxdw2;
+	__le32 rxdw3;
+	__le32 rxdw4;
+	__le32 rxdw5;
 };
 
 /* accesser of recv_priv: rtw_recv_entry23a(dispatch / passive level);	\
@@ -172,9 +171,7 @@ struct recv_priv {
 	struct rtw_queue	recv_pending_queue;
 	struct rtw_queue	uc_swdec_pending_queue;
 
-	void *pallocated_frame_buf;
-
-	uint free_recvframe_cnt;
+	int free_recvframe_cnt;
 
 	struct rtw_adapter	*adapter;
 
@@ -190,8 +187,6 @@ struct recv_priv {
 	uint  rx_middlepacket_crcerr;
 
 	/* u8 *pallocated_urb_buf; */
-	struct semaphore allrxreturnevt;
-	uint	ff_hwaddr;
 	u8	rx_pending_cnt;
 
 	struct urb *int_in_urb;
@@ -203,8 +198,6 @@ struct recv_priv {
 	struct sk_buff_head free_recv_skb_queue;
 	struct sk_buff_head rx_skb_queue;
 	u8 *precv_buf;
-	struct rtw_queue	free_recv_buf_queue;
-	u32	free_recv_buf_queue_cnt;
 
 	/* For display the phy informatiom */
 	u8 is_signal_dbg;	/*  for debug */
@@ -283,11 +276,10 @@ struct recv_frame {
 
 /* get a free recv_frame from pfree_recv_queue */
 struct recv_frame *rtw_alloc_recvframe23a(struct rtw_queue *pfree_recv_queue);
-int rtw_free_recvframe23a(struct recv_frame *precvframe, struct rtw_queue *pfree_recv_queue);
+int rtw_free_recvframe23a(struct recv_frame *precvframe);
 
 int rtw_enqueue_recvframe23a(struct recv_frame *precvframe, struct rtw_queue *queue);
 
-void rtw_free_recvframe23a_queue(struct rtw_queue *pframequeue, struct rtw_queue *pfree_recv_queue);
 u32 rtw_free_uc_swdec_pending_queue23a(struct rtw_adapter *adapter);
 
 int rtw_enqueue_recvbuf23a_to_head(struct recv_buf *precvbuf, struct rtw_queue *queue);
diff --git a/drivers/staging/rtl8723au/include/rtw_security.h b/drivers/staging/rtl8723au/include/rtw_security.h
index 75bbb934a53..8b8433365e3 100644
--- a/drivers/staging/rtl8723au/include/rtw_security.h
+++ b/drivers/staging/rtl8723au/include/rtw_security.h
@@ -17,20 +17,12 @@
 
 #include <osdep_service.h>
 #include <drv_types.h>
+#include <net/lib80211.h>
 
 
-#define _NO_PRIVACY_		0x0
-#define _WEP40_			0x1
-#define _TKIP_			0x2
-#define _TKIP_WTMIC_		0x3
-#define _AES_			0x4
-#define _WEP104_		0x5
-#define _WEP_WPA_MIXED_	0x07  /*  WEP + WPA */
-#define _SMS4_			0x06
+#define is_wep_enc(alg) (alg == WLAN_CIPHER_SUITE_WEP40 || \
+			 alg == WLAN_CIPHER_SUITE_WEP104)
 
-#define is_wep_enc(alg) (((alg) == _WEP40_) || ((alg) == _WEP104_))
-
-#define _WPA_IE_ID_	0xdd
 #define _WPA2_IE_ID_	0x30
 
 #define SHA256_MAC_LEN 32
@@ -93,6 +85,10 @@ union Keytype {
 	u32    lkey[4];
 };
 
+struct rtw_wep_key {
+	u8 key[WLAN_KEY_LEN_WEP104 + 1]; /* 14 */
+	u16 keylen;
+};
 
 struct rt_pmkid_list {
 	u8	bUsed;
@@ -113,8 +109,7 @@ struct security_priv {
 	u32	  dot11PrivacyKeyIndex;	/*  this is only valid for legendary
 					 * wep, 0~3 for key id. (tx key index)
 					 */
-	union Keytype dot11DefKey[4];	/*  this is only valid for def. key */
-	u32	dot11DefKeylen[4];
+	struct rtw_wep_key wep_key[NUM_WEP_KEYS];
 
 	u32 dot118021XGrpPrivacy;	/* specify the privacy algthm.
 					 * used for Grp key
@@ -140,15 +135,13 @@ struct security_priv {
 
 	u8 wps_ie[MAX_WPS_IE_LEN];/* added in assoc req */
 	int wps_ie_len;
-	u8	binstallGrpkey;
-	u8	busetkipkey;
-	u8	bcheck_grpkey;
-	u8	bgrpkey_handshake;
-	s32	hw_decrypted;
+	unsigned int binstallGrpkey:1;
+	unsigned int busetkipkey:1;
+	unsigned int bcheck_grpkey:1;
+	unsigned int hw_decrypted:1;
 	u32 ndisauthtype;	/*  enum ndis_802_11_auth_mode */
 	u32 ndisencryptstatus;	/*  NDIS_802_11_ENCRYPTION_STATUS */
 	struct wlan_bssid_ex sec_bss;  /* for joinbss (h2c buffer) usage */
-	struct ndis_802_11_wep ndiswep;
 	u8 assoc_info[600];
 	u8 szofcapability[256]; /* for wpa2 usage */
 	u8 oidassociation[512]; /* for wpa/wpa2 usage */
@@ -179,13 +172,13 @@ do {\
 	case dot11AuthAlgrthm_Open:\
 	case dot11AuthAlgrthm_Shared:\
 	case dot11AuthAlgrthm_Auto:\
-		encry_algo = (u8)psecuritypriv->dot11PrivacyAlgrthm;\
+		encry_algo = psecuritypriv->dot11PrivacyAlgrthm;\
 		break;\
 	case dot11AuthAlgrthm_8021X:\
 		if (bmcst)\
-			encry_algo = (u8)psecuritypriv->dot118021XGrpPrivacy;\
+			encry_algo = psecuritypriv->dot118021XGrpPrivacy;\
 		else\
-			encry_algo = (u8)psta->dot118021XPrivacy;\
+			encry_algo = psta->dot118021XPrivacy;\
 		break;\
 	}	\
 } while (0)
@@ -316,22 +309,6 @@ static const unsigned long K[64] = {
 	0x90befffaUL, 0xa4506cebUL, 0xbef9a3f7UL, 0xc67178f2UL
 };
 
-/* Various logical functions */
-#define RORc(x, y) \
-(((((unsigned long)(x) & 0xFFFFFFFFUL) >> (unsigned long) ((y) & 31)) | \
-((unsigned long)(x) << (unsigned long) (32 - ((y) & 31)))) & 0xFFFFFFFFUL)
-#define Ch(x, y, z)     (z ^ (x & (y ^ z)))
-#define Maj(x, y, z)    (((x | y) & z) | (x & y))
-#define S(x, n)         RORc((x), (n))
-#define R(x, n)         (((x)&0xFFFFFFFFUL)>>(n))
-#define Sigma0(x)       (S(x, 2) ^ S(x, 13) ^ S(x, 22))
-#define Sigma1(x)       (S(x, 6) ^ S(x, 11) ^ S(x, 25))
-#define Gamma0(x)       (S(x, 7) ^ S(x, 18) ^ R(x, 3))
-#define Gamma1(x)       (S(x, 17) ^ S(x, 19) ^ R(x, 10))
-#ifndef MIN
-#define MIN(x, y) (((x) < (y)) ? (x) : (y))
-#endif
-
 void rtw_secmicsetkey23a(struct mic_data *pmicdata, u8 *key);
 void rtw_secmicappend23abyte23a(struct mic_data *pmicdata, u8 b);
 void rtw_secmicappend23a(struct mic_data *pmicdata, u8 *src, u32 nbBytes);
@@ -340,15 +317,15 @@ void rtw_secgetmic23a(struct mic_data *pmicdata, u8 *dst);
 void rtw_seccalctkipmic23a(u8 *key, u8 *header, u8 *data, u32 data_len,
 			u8 *Miccode, u8 priorityi);
 
-u32 rtw_aes_encrypt23a(struct rtw_adapter *padapter,
+int rtw_aes_encrypt23a(struct rtw_adapter *padapter,
 		    struct xmit_frame *pxmitframe);
-u32 rtw_tkip_encrypt23a(struct rtw_adapter *padapter,
+int rtw_tkip_encrypt23a(struct rtw_adapter *padapter,
 		     struct xmit_frame *pxmitframe);
 void rtw_wep_encrypt23a(struct rtw_adapter *padapter,
 		     struct xmit_frame *pxmitframe);
-u32 rtw_aes_decrypt23a(struct rtw_adapter *padapter,
+int rtw_aes_decrypt23a(struct rtw_adapter *padapter,
 		    struct recv_frame *precvframe);
-u32 rtw_tkip_decrypt23a(struct rtw_adapter *padapter,
+int rtw_tkip_decrypt23a(struct rtw_adapter *padapter,
 		     struct recv_frame *precvframe);
 void rtw_wep_decrypt23a(struct rtw_adapter *padapter, struct recv_frame *precvframe);
 
diff --git a/drivers/staging/rtl8723au/include/rtw_sreset.h b/drivers/staging/rtl8723au/include/rtw_sreset.h
index 4c523722dd1..77cc7941b54 100644
--- a/drivers/staging/rtl8723au/include/rtw_sreset.h
+++ b/drivers/staging/rtl8723au/include/rtw_sreset.h
@@ -36,21 +36,21 @@ struct sreset_priv {
 
 #include <rtl8723a_hal.h>
 
-#define	WIFI_STATUS_SUCCESS		0
-#define	USB_VEN_REQ_CMD_FAIL	BIT0
-#define	USB_READ_PORT_FAIL		BIT1
-#define	USB_WRITE_PORT_FAIL		BIT2
-#define	WIFI_MAC_TXDMA_ERROR	BIT3
-#define   WIFI_TX_HANG				BIT4
-#define	WIFI_RX_HANG				BIT5
-#define		WIFI_IF_NOT_EXIST			BIT6
-
-void sreset_init_value23a(struct rtw_adapter *padapter);
-void sreset_reset_value23a(struct rtw_adapter *padapter);
-u8 sreset_get_wifi_status23a(struct rtw_adapter *padapter);
+#define	WIFI_STATUS_SUCCESS	0
+#define	USB_VEN_REQ_CMD_FAIL	BIT(0)
+#define	USB_READ_PORT_FAIL	BIT(1)
+#define	USB_WRITE_PORT_FAIL	BIT(2)
+#define	WIFI_MAC_TXDMA_ERROR	BIT(3)
+#define	WIFI_TX_HANG		BIT(4)
+#define	WIFI_RX_HANG		BIT(5)
+#define	WIFI_IF_NOT_EXIST	BIT(6)
+
+void rtw_sreset_init(struct rtw_adapter *padapter);
+void rtw_sreset_reset_value(struct rtw_adapter *padapter);
+u8 rtw_sreset_get_wifi_status(struct rtw_adapter *padapter);
+bool rtw_sreset_inprogress(struct rtw_adapter *padapter);
 void sreset_set_wifi_error_status23a(struct rtw_adapter *padapter, u32 status);
 void sreset_set_trigger_point(struct rtw_adapter *padapter, s32 tgp);
-bool sreset_inprogress(struct rtw_adapter *padapter);
-void sreset_reset(struct rtw_adapter *padapter);
+void rtw_sreset_reset(struct rtw_adapter *active_adapter);
 
 #endif
diff --git a/drivers/staging/rtl8723au/include/rtw_xmit.h b/drivers/staging/rtl8723au/include/rtw_xmit.h
index 65a33a07c8e..32a84417032 100644
--- a/drivers/staging/rtl8723au/include/rtw_xmit.h
+++ b/drivers/staging/rtl8723au/include/rtw_xmit.h
@@ -92,14 +92,14 @@ do {									\
 
 struct tx_desc {
 	/* DWORD 0 */
-	unsigned int txdw0;
-	unsigned int txdw1;
-	unsigned int txdw2;
-	unsigned int txdw3;
-	unsigned int txdw4;
-	unsigned int txdw5;
-	unsigned int txdw6;
-	unsigned int txdw7;
+	__le32 txdw0;
+	__le32 txdw1;
+	__le32 txdw2;
+	__le32 txdw3;
+	__le32 txdw4;
+	__le32 txdw5;
+	__le32 txdw6;
+	__le32 txdw7;
 };
 
 union txdesc {
@@ -114,8 +114,7 @@ struct	hw_xmit	{
 
 /* reduce size */
 struct pkt_attrib {
-	u8	type;
-	u8	subtype;
+	u16	type;
 	u8	bswenc;
 	u8	dhcp_pkt;
 	u16	ether_type;
@@ -124,8 +123,8 @@ struct pkt_attrib {
 	u16	hdrlen;		/* the WLAN Header Len */
 	u32	pktlen;		/* the original 802.3 pkt raw_data len */
 	u32	last_txcmdsz;
+	u32	encrypt;	/* when 0 indicate no encrypt. */
 	u8	nr_frags;
-	u8	encrypt;	/* when 0 indicate no encrypt. */
 	u8	iv_len;
 	u8	icv_len;
 	u8	iv[18];
@@ -286,14 +285,10 @@ struct	xmit_priv {
 	struct rtw_queue	vo_pending;
 	struct rtw_queue	bm_pending;
 
-	u8 *pallocated_frame_buf;
-	u8 *pxmit_frame_buf;
-	uint free_xmitframe_cnt;
+	int free_xmitframe_cnt;
 	struct rtw_queue	free_xmit_queue;
 
-	u8 *xframe_ext_alloc_addr;
-	u8 *xframe_ext;
-	uint free_xframe_ext_cnt;
+	int free_xframe_ext_cnt;
 	struct rtw_queue free_xframe_ext_queue;
 
 	uint	frag_len;
@@ -354,10 +349,7 @@ s32 rtw_free_xmitbuf23a(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf);
 void rtw_count_tx_stats23a(struct rtw_adapter *padapter,
 			struct xmit_frame *pxmitframe, int sz);
 void rtw_update_protection23a(struct rtw_adapter *padapter, u8 *ie, uint ie_len);
-s32 rtw_make_wlanhdr23a(struct rtw_adapter *padapter, u8 *hdr,
-		     struct pkt_attrib *pattrib);
 s32 rtw_put_snap23a(u8 *data, u16 h_proto);
-struct xmit_frame *rtw_alloc_xmitframe23a(struct xmit_priv *pxmitpriv);
 struct xmit_frame *rtw_alloc_xmitframe23a_ext(struct xmit_priv *pxmitpriv);
 struct xmit_frame *rtw_alloc_xmitframe23a_once(struct xmit_priv *pxmitpriv);
 s32 rtw_free_xmitframe23a(struct xmit_priv *pxmitpriv,
@@ -382,7 +374,7 @@ s32 rtw_txframes_pending23a(struct rtw_adapter *padapter);
 s32 rtw_txframes_sta_ac_pending23a(struct rtw_adapter *padapter,
 				struct pkt_attrib *pattrib);
 void rtw_init_hwxmits23a(struct hw_xmit *phwxmit, int entry);
-s32 _rtw_init_xmit_priv23a(struct xmit_priv *pxmitpriv,
+int _rtw_init_xmit_priv23a(struct xmit_priv *pxmitpriv,
 			struct rtw_adapter *padapter);
 void _rtw_free_xmit_priv23a(struct xmit_priv *pxmitpriv);
 void rtw_alloc_hwxmits23a(struct rtw_adapter *padapter);
diff --git a/drivers/staging/rtl8723au/include/sta_info.h b/drivers/staging/rtl8723au/include/sta_info.h
index ffbc9e3f215..d1939a67390 100644
--- a/drivers/staging/rtl8723au/include/sta_info.h
+++ b/drivers/staging/rtl8723au/include/sta_info.h
@@ -97,7 +97,7 @@ struct sta_info {
 	u8	hwaddr[ETH_ALEN];
 
 	uint	ieee8021x_blocked;	/* 0: allowed, 1:blocked */
-	uint	dot118021XPrivacy; /* aes, tkip... */
+	u32	dot118021XPrivacy; /* aes, tkip... */
 	union Keytype	dot11tkiptxmickey;
 	union Keytype	dot11tkiprxmickey;
 	union Keytype	dot118021x_UncstKey;
@@ -211,7 +211,6 @@ struct sta_info {
 	/*  */
 	/*  Driver Write */
 	u8		bValid;				/*  record the sta status link or not? */
-	u8		IOTPeer;			/*  Enum value.	HT_IOT_PEER_E */
 	u8		rssi_level;			/* for Refresh RA mask */
 	/*  ODM Write */
 	/* 1 PHY_STATUS_INFO */
@@ -310,16 +309,10 @@ struct sta_info {
 
 #define STA_PKTS_FMT "(m:%llu, c:%llu, d:%llu)"
 
-struct	sta_priv {
-	u8 *pallocated_stainfo_buf;
-	u8 *pstainfo_buf;
-	struct rtw_queue	free_sta_queue;
-
+struct sta_priv {
 	spinlock_t sta_hash_lock;
-	struct list_head   sta_hash[NUM_STA];
+	struct list_head sta_hash[NUM_STA];
 	int asoc_sta_count;
-	struct rtw_queue sleep_q;
-	struct rtw_queue wakeup_q;
 
 	struct rtw_adapter *padapter;
 	struct list_head asoc_list;
@@ -349,7 +342,7 @@ struct	sta_priv {
 	struct wlan_acl_pool acl_list;
 };
 
-static inline u32 wifi_mac_hash(u8 *mac)
+static inline u32 wifi_mac_hash(const u8 *mac)
 {
 	u32 x;
 
@@ -366,20 +359,15 @@ static inline u32 wifi_mac_hash(u8 *mac)
 	return x;
 }
 
-u32	_rtw_init_sta_priv23a(struct sta_priv *pstapriv);
-u32	_rtw_free_sta_priv23a(struct sta_priv *pstapriv);
-
-#define stainfo_offset_valid(offset) (offset < NUM_STA && offset >= 0)
-int rtw_stainfo_offset23a(struct sta_priv *stapriv, struct sta_info *sta);
-struct sta_info *rtw_get_stainfo23a_by_offset23a(struct sta_priv *stapriv,
-					   int offset);
+int _rtw_init_sta_priv23a(struct sta_priv *pstapriv);
+int _rtw_free_sta_priv23a(struct sta_priv *pstapriv);
 
-struct sta_info *rtw_alloc_stainfo23a(struct sta_priv *pstapriv, u8 *hwaddr);
-u32 rtw_free_stainfo23a(struct rtw_adapter *padapter, struct sta_info *psta);
+struct sta_info *rtw_alloc_stainfo23a(struct sta_priv *pstapriv, u8 *hwaddr, gfp_t gfp);
+int rtw_free_stainfo23a(struct rtw_adapter *padapter, struct sta_info *psta);
 void rtw_free_all_stainfo23a(struct rtw_adapter *padapter);
-struct sta_info *rtw_get_stainfo23a(struct sta_priv *pstapriv, u8 *hwaddr);
-u32 rtw_init_bcmc_stainfo23a(struct rtw_adapter *padapter);
+struct sta_info *rtw_get_stainfo23a(struct sta_priv *pstapriv, const u8 *hwaddr);
+int rtw_init_bcmc_stainfo23a(struct rtw_adapter *padapter);
 struct sta_info *rtw_get_bcmc_stainfo23a(struct rtw_adapter *padapter);
-u8 rtw_access_ctrl23a(struct rtw_adapter *padapter, u8 *mac_addr);
+bool rtw_access_ctrl23a(struct rtw_adapter *padapter, u8 *mac_addr);
 
 #endif /* _STA_INFO_H_ */
diff --git a/drivers/staging/rtl8723au/include/usb_hal.h b/drivers/staging/rtl8723au/include/usb_hal.h
deleted file mode 100644
index 4edec3b539b..00000000000
--- a/drivers/staging/rtl8723au/include/usb_hal.h
+++ /dev/null
@@ -1,20 +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 __USB_HAL_H__
-#define __USB_HAL_H__
-
-int rtl8723au_set_hal_ops(struct rtw_adapter *padapter);
-
-#endif /* __USB_HAL_H__ */
diff --git a/drivers/staging/rtl8723au/include/usb_ops.h b/drivers/staging/rtl8723au/include/usb_ops.h
index 55d1380f903..ade8bc71572 100644
--- a/drivers/staging/rtl8723au/include/usb_ops.h
+++ b/drivers/staging/rtl8723au/include/usb_ops.h
@@ -34,19 +34,7 @@ enum {
 #define MAX_VENDOR_REQ_CMD_SIZE	254		/* 8188cu SIE Support */
 #define MAX_USB_IO_CTL_SIZE	(MAX_VENDOR_REQ_CMD_SIZE +ALIGNMENT_UNIT)
 
-#define rtw_usb_control_msg(dev, pipe, request, requesttype, value,	\
-			    index, data, size, timeout_ms)		\
-	usb_control_msg((dev), (pipe), (request), (requesttype),	\
-			(value), (index), (data), (size), (timeout_ms))
-#define rtw_usb_bulk_msg(usb_dev, pipe, data, len, actual_length, timeout_ms) \
-	usb_bulk_msg((usb_dev), (pipe), (data), (len), (actual_length),	\
-		     (timeout_ms))
-
 void rtl8723au_set_hw_type(struct rtw_adapter *padapter);
-#define hal_set_hw_type rtl8723au_set_hw_type
-
-void rtl8723au_set_intf_ops(struct _io_ops *pops);
-#define usb_set_intf_ops rtl8723au_set_intf_ops
 
 void rtl8723au_recv_tasklet(void *priv);
 
@@ -75,23 +63,6 @@ static inline void rtw_reset_continual_urb_error(struct dvobj_priv *dvobj)
 	atomic_set(&dvobj->continual_urb_error, 0);
 }
 
-#define USB_HIGH_SPEED_BULK_SIZE	512
-#define USB_FULL_SPEED_BULK_SIZE	64
-
-static inline u8 rtw_usb_bulk_size_boundary(struct rtw_adapter *padapter,
-					    int buf_len)
-{
-	u8 rst = true;
-	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter);
-
-	if (pdvobjpriv->ishighspeed)
-		rst = (0 == (buf_len) % USB_HIGH_SPEED_BULK_SIZE) ?
-		      true : false;
-	else
-		rst = (0 == (buf_len) % USB_FULL_SPEED_BULK_SIZE) ?
-		      true : false;
-	return rst;
-}
-
+void rtl8723au_chip_configure(struct rtw_adapter *padapter);
 
 #endif /* __USB_OPS_H_ */
diff --git a/drivers/staging/rtl8723au/include/usb_ops_linux.h b/drivers/staging/rtl8723au/include/usb_ops_linux.h
index 8f5c59eace5..e540a4bad08 100644
--- a/drivers/staging/rtl8723au/include/usb_ops_linux.h
+++ b/drivers/staging/rtl8723au/include/usb_ops_linux.h
@@ -17,30 +17,25 @@
 
 #define VENDOR_CMD_MAX_DATA_LEN	254
 
-#define RTW_USB_CONTROL_MSG_TIMEOUT_TEST	10/* ms */
 #define RTW_USB_CONTROL_MSG_TIMEOUT	500/* ms */
 
 #define MAX_USBCTRL_VENDORREQ_TIMES	10
 
-#define RTW_USB_BULKOUT_TIMEOUT	5000/* ms */
-
-#define _usbctrl_vendorreq_async_callback(urb, regs)		\
-	_usbctrl_vendorreq_async_callback(urb)
-#define usb_write_mem23a_complete(purb, regs)	usb_write_mem23a_complete(purb)
-#define usb_write_port23a_complete(purb, regs)	usb_write_port23a_complete(purb)
-#define usb_read_port_complete(purb, regs)	usb_read_port_complete(purb)
-#define usb_read_interrupt_complete(purb, regs)			\
-	usb_read_interrupt_complete(purb)
-
-unsigned int ffaddr2pipehdl23a(struct dvobj_priv *pdvobj, u32 addr);
-
-void usb_read_mem23a(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *rmem);
-void usb_write_mem23a(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *wmem);
-
-void usb_read_port_cancel23a(struct intf_hdl *pintfhdl);
-
-u32 usb_write_port23a(struct intf_hdl *pintfhdl, u32 addr, u32 cnt,
-		   struct xmit_buf *wmem);
-void usb_write_port23a_cancel(struct intf_hdl *pintfhdl);
+int rtl8723au_read_port(struct rtw_adapter *adapter, u32 addr, u32 cnt,
+			struct recv_buf *precvbuf);
+void rtl8723au_read_port_cancel(struct rtw_adapter *padapter);
+int rtl8723au_write_port(struct rtw_adapter *padapter, u32 addr, u32 cnt,
+			 struct xmit_buf *pxmitbuf);
+void rtl8723au_write_port_cancel(struct rtw_adapter *padapter);
+int rtl8723au_read_interrupt(struct rtw_adapter *adapter, u32 addr);
+
+u8 rtl8723au_read8(struct rtw_adapter *padapter, u32 addr);
+u16 rtl8723au_read16(struct rtw_adapter *padapter, u32 addr);
+u32 rtl8723au_read32(struct rtw_adapter *padapter, u32 addr);
+int rtl8723au_write8(struct rtw_adapter *padapter, u32 addr, u8 val);
+int rtl8723au_write16(struct rtw_adapter *padapter, u32 addr, u16 val);
+int rtl8723au_write32(struct rtw_adapter *padapter, u32 addr, u32 val);
+int rtl8723au_writeN(struct rtw_adapter *padapter,
+		     u32 addr, u32 length, u8 *pdata);
 
 #endif
diff --git a/drivers/staging/rtl8723au/include/usb_osintf.h b/drivers/staging/rtl8723au/include/usb_osintf.h
deleted file mode 100644
index 46087662834..00000000000
--- a/drivers/staging/rtl8723au/include/usb_osintf.h
+++ /dev/null
@@ -1,24 +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 __USB_OSINTF_H
-#define __USB_OSINTF_H
-
-#include <osdep_service.h>
-#include <drv_types.h>
-#include <usb_vendor_req.h>
-
-#define USBD_HALTED(_status) ((u32)(_status) >> 30 == 3)
-
-#endif
diff --git a/drivers/staging/rtl8723au/include/usb_vendor_req.h b/drivers/staging/rtl8723au/include/usb_vendor_req.h
deleted file mode 100644
index eb4508ef191..00000000000
--- a/drivers/staging/rtl8723au/include/usb_vendor_req.h
+++ /dev/null
@@ -1,31 +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 _USB_VENDOR_REQUEST_H_
-#define _USB_VENDOR_REQUEST_H_
-
-/* 4	Set/Get Register related wIndex/Data */
-#define	RT_USB_RESET_MASK_OFF		0
-#define	RT_USB_RESET_MASK_ON		1
-#define	RT_USB_SLEEP_MASK_OFF		0
-#define	RT_USB_SLEEP_MASK_ON		1
-#define	RT_USB_LDO_ON			1
-#define	RT_USB_LDO_OFF			0
-
-/* 4	Set/Get SYSCLK related	wValue or Data */
-#define	RT_USB_SYSCLK_32KHZ		0
-#define	RT_USB_SYSCLK_40MHZ		1
-#define	RT_USB_SYSCLK_60MHZ		2
-
-#endif
diff --git a/drivers/staging/rtl8723au/include/wifi.h b/drivers/staging/rtl8723au/include/wifi.h
index b5034c6ef1d..cccea6a50b3 100644
--- a/drivers/staging/rtl8723au/include/wifi.h
+++ b/drivers/staging/rtl8723au/include/wifi.h
@@ -23,189 +23,6 @@
  */
 #define	WiFiNavUpperUs		30000	/*  30 ms */
 
-enum WIFI_FRAME_TYPE {
-	WIFI_MGT_TYPE  =	(0),
-	WIFI_CTRL_TYPE =	(BIT(2)),
-	WIFI_DATA_TYPE =	(BIT(3)),
-	WIFI_QOS_DATA_TYPE	= (BIT(7)|BIT(3)),	/*  QoS Data */
-};
-
-enum WIFI_FRAME_SUBTYPE {
-	/*  below is for mgt frame */
-	WIFI_ASSOCREQ = (0 | WIFI_MGT_TYPE),
-	WIFI_ASSOCRSP = (BIT(4) | WIFI_MGT_TYPE),
-	WIFI_REASSOCREQ = (BIT(5) | WIFI_MGT_TYPE),
-	WIFI_REASSOCRSP = (BIT(5) | BIT(4) | WIFI_MGT_TYPE),
-	WIFI_PROBEREQ = (BIT(6) | WIFI_MGT_TYPE),
-	WIFI_PROBERSP = (BIT(6) | BIT(4) | WIFI_MGT_TYPE),
-	WIFI_BEACON = (BIT(7) | WIFI_MGT_TYPE),
-	WIFI_ATIM = (BIT(7) | BIT(4) | WIFI_MGT_TYPE),
-	WIFI_DISASSOC = (BIT(7) | BIT(5) | WIFI_MGT_TYPE),
-	WIFI_AUTH = (BIT(7) | BIT(5) | BIT(4) | WIFI_MGT_TYPE),
-	WIFI_DEAUTH = (BIT(7) | BIT(6) | WIFI_MGT_TYPE),
-	WIFI_ACTION = (BIT(7) | BIT(6) | BIT(4) | WIFI_MGT_TYPE),
-
-	/*  below is for control frame */
-	WIFI_PSPOLL = (BIT(7) | BIT(5) | WIFI_CTRL_TYPE),
-	WIFI_RTS = (BIT(7) | BIT(5) | BIT(4) | WIFI_CTRL_TYPE),
-	WIFI_CTS = (BIT(7) | BIT(6) | WIFI_CTRL_TYPE),
-	WIFI_ACK = (BIT(7) | BIT(6) | BIT(4) | WIFI_CTRL_TYPE),
-	WIFI_CFEND = (BIT(7) | BIT(6) | BIT(5) | WIFI_CTRL_TYPE),
-	WIFI_CFEND_CFACK = (BIT(7) | BIT(6) | BIT(5) | BIT(4) | WIFI_CTRL_TYPE),
-
-	/*  below is for data frame */
-	WIFI_DATA = (0 | WIFI_DATA_TYPE),
-	WIFI_DATA_CFACK = (BIT(4) | WIFI_DATA_TYPE),
-	WIFI_DATA_CFPOLL = (BIT(5) | WIFI_DATA_TYPE),
-	WIFI_DATA_CFACKPOLL = (BIT(5) | BIT(4) | WIFI_DATA_TYPE),
-	WIFI_DATA_NULL = (BIT(6) | WIFI_DATA_TYPE),
-	WIFI_CF_ACK = (BIT(6) | BIT(4) | WIFI_DATA_TYPE),
-	WIFI_CF_POLL = (BIT(6) | BIT(5) | WIFI_DATA_TYPE),
-	WIFI_CF_ACKPOLL = (BIT(6) | BIT(5) | BIT(4) | WIFI_DATA_TYPE),
-	WIFI_QOS_DATA_NULL = (BIT(6) | WIFI_QOS_DATA_TYPE),
-};
-
-
-enum WIFI_REG_DOMAIN {
-	DOMAIN_FCC		= 1,
-	DOMAIN_IC		= 2,
-	DOMAIN_ETSI		= 3,
-	DOMAIN_SPAIN		= 4,
-	DOMAIN_FRANCE		= 5,
-	DOMAIN_MKK		= 6,
-	DOMAIN_ISRAEL		= 7,
-	DOMAIN_MKK1		= 8,
-	DOMAIN_MKK2		= 9,
-	DOMAIN_MKK3		= 10,
-	DOMAIN_MAX
-};
-
-
-#define SetToDs(pbuf)	\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16(IEEE80211_FCTL_TODS))
-
-#define SetFrDs(pbuf)	\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16(IEEE80211_FCTL_FROMDS))
-
-#define get_tofr_ds(pframe)	((ieee80211_has_tods(pframe) << 1) | \
-				 ieee80211_has_fromds(pframe))
-
-#define SetMFrag(pbuf)	\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16(IEEE80211_FCTL_MOREFRAGS))
-
-#define ClearMFrag(pbuf)	\
-	(*(unsigned short *)(pbuf) &= (~cpu_to_le16(IEEE80211_FCTL_MOREFRAGS)))
-
-#define SetRetry(pbuf)	\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16(IEEE80211_FCTL_RETRY))
-
-#define SetPwrMgt(pbuf)	\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16(IEEE80211_FCTL_PM))
-
-#define SetMData(pbuf)	\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16(IEEE80211_FCTL_MOREDATA))
-
-#define SetPrivacy(pbuf)	\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16(IEEE80211_FCTL_PROTECTED))
-
-#define SetFrameType(pbuf, type)	\
-	do {	\
-		*(unsigned short *)(pbuf) &= __constant_cpu_to_le16(~(BIT(3) | BIT(2))); \
-		*(unsigned short *)(pbuf) |= __constant_cpu_to_le16(type); \
-	} while (0)
-
-#define SetFrameSubType(pbuf, type) \
-	do {    \
-		*(unsigned short *)(pbuf) &= cpu_to_le16(~(BIT(7) | BIT(6) | BIT(5) | BIT(4) | BIT(3) | BIT(2))); \
-		*(unsigned short *)(pbuf) |= cpu_to_le16(type); \
-	} while (0)
-
-#define GetTupleCache(pbuf)	(cpu_to_le16(*(unsigned short *)((unsigned long)(pbuf) + 22)))
-
-#define SetFragNum(pbuf, num) \
-	do {    \
-		*(unsigned short *)((unsigned long)(pbuf) + 22) = \
-			((*(unsigned short *)((unsigned long)(pbuf) + 22)) & le16_to_cpu(~(0x000f))) | \
-			cpu_to_le16(0x0f & (num));     \
-	} while (0)
-
-#define SetSeqNum(pbuf, num) \
-	do {    \
-		*(unsigned short *)((unsigned long)(pbuf) + 22) = \
-			((*(unsigned short *)((unsigned long)(pbuf) + 22)) & le16_to_cpu((unsigned short)0x000f)) | \
-			le16_to_cpu((unsigned short)(0xfff0 & (num << 4))); \
-	} while (0)
-
-#define SetDuration(pbuf, dur) \
-	(*(unsigned short *)((unsigned long)(pbuf) + 2) =		\
-	 cpu_to_le16(0xffff & (dur)))
-
-#define SetPriority(pbuf, tid)	\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16(tid & 0xf))
-
-#define SetEOSP(pbuf, eosp)	\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16((eosp & 1) << 4))
-
-#define SetAckpolicy(pbuf, ack)	\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16((ack & 3) << 5))
-
-#define SetAMsdu(pbuf, amsdu)	\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16((amsdu & 1) << 7))
-
-#define GetAid(pbuf)							\
-	(cpu_to_le16(*(unsigned short *)((unsigned long)(pbuf) + 2)) &	\
-	 0x3fff)
-
-#define GetTid(pbuf)							\
-	(cpu_to_le16(*(unsigned short *)((unsigned long)(pbuf) +	\
-	 (((ieee80211_has_tods(pbuf)<<1) |				\
-	 ieee80211_has_fromds(pbuf)) == 3 ? 30 : 24))) & 0x000f)
-
-static inline unsigned char *get_hdr_bssid(unsigned char *pframe)
-{
-	unsigned char	*sa;
-	unsigned int	to_fr_ds;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) pframe;
-
-	to_fr_ds = (ieee80211_has_tods(hdr->frame_control) << 1) |
-		    ieee80211_has_fromds(hdr->frame_control);
-
-	switch (to_fr_ds) {
-	case 0x00:	/*  ToDs=0, FromDs=0 */
-		sa = hdr->addr3;
-		break;
-	case 0x01:	/*  ToDs=0, FromDs=1 */
-		sa = hdr->addr2;
-		break;
-	case 0x02:	/*  ToDs=1, FromDs=0 */
-		sa = hdr->addr1;
-		break;
-	case 0x03:	/*  ToDs=1, FromDs=1 */
-		sa = hdr->addr1;
-		break;
-	default:
-		sa = NULL; /*  */
-		break;
-	}
-	return sa;
-}
-
-/*-----------------------------------------------------------------------------
-			Below is for the security related definition
-------------------------------------------------------------------------------*/
-#define _RESERVED_FRAME_TYPE_		0
-#define _SKB_FRAME_TYPE_		2
-#define _PRE_ALLOCMEM_			1
-#define _PRE_ALLOCHDR_			3
-#define _PRE_ALLOCLLCHDR_		4
-#define _PRE_ALLOCICVHDR_		5
-#define _PRE_ALLOCMICHDR_		6
-
-#define _SIFSTIME_					\
-	((priv->pmib->dot11BssType.net_work_type & WIRELESS_11A) ? 16 : 10)
-#define _ACKCTSLNG_			14	/* 14 bytes long, including crclng */
-#define _CRCLNG_			4
-
 #define _ASOCREQ_IE_OFFSET_		4	/*  excluding wlan_hdr */
 #define	_ASOCRSP_IE_OFFSET_		6
 #define _REASOCREQ_IE_OFFSET_		10
@@ -219,39 +36,8 @@ static inline unsigned char *get_hdr_bssid(unsigned char *pframe)
 
 #define _FIXED_IE_LENGTH_		_BEACON_IE_OFFSET_
 
-#define _SSID_IE_			0
-#define _SUPPORTEDRATES_IE_		1
-#define _DSSET_IE_			3
-#define _TIM_IE_			5
-#define _IBSS_PARA_IE_			6
-#define _COUNTRY_IE_			7
-#define _CHLGETXT_IE_			16
-#define _SUPPORTED_CH_IE_		36
-#define _CH_SWTICH_ANNOUNCE_	37	/* Secondary Channel Offset */
-#define _RSN_IE_2_			48
-#define _SSN_IE_1_			221
-#define _ERPINFO_IE_			42
-#define _EXT_SUPPORTEDRATES_IE_		50
-
-#define _HT_CAPABILITY_IE_		45
-#define _FTIE_				55
-#define _TIMEOUT_ITVL_IE_		56
-#define _SRC_IE_			59
-#define _HT_EXTRA_INFO_IE_		61
-#define _HT_ADD_INFO_IE_		61 /* _HT_EXTRA_INFO_IE_ */
 
-
-#define	EID_BSSCoexistence		72 /*  20/40 BSS Coexistence */
 #define	EID_BSSIntolerantChlReport	73
-#define _RIC_Descriptor_IE_		75
-
-#define _LINK_ID_IE_		101
-#define _CH_SWITCH_TIMING_	104
-#define _PTI_BUFFER_STATUS_	106
-#define _EXT_CAP_IE_		127
-#define _VENDOR_SPECIFIC_IE_	221
-
-#define	_RESERVED47_		47
 
 /* ---------------------------------------------------------------------------
 					Below is the fixed elements...
@@ -266,41 +52,6 @@ static inline unsigned char *get_hdr_bssid(unsigned char *pframe)
 #define _STATUS_CODE_		2
 #define _TIMESTAMP_		8
 
-#define AUTH_ODD_TO		0
-#define AUTH_EVEN_TO		1
-
-#define WLAN_ETHCONV_ENCAP	1
-#define WLAN_ETHCONV_RFC1042	2
-#define WLAN_ETHCONV_8021h	3
-
-#define cap_ESS		BIT(0)
-#define cap_IBSS	BIT(1)
-#define cap_CFPollable	BIT(2)
-#define cap_CFRequest	BIT(3)
-#define cap_Privacy	BIT(4)
-#define cap_ShortPremble BIT(5)
-#define cap_PBCC	BIT(6)
-#define cap_ChAgility	BIT(7)
-#define cap_SpecMgmt	BIT(8)
-#define cap_QoS		BIT(9)
-#define cap_ShortSlot	BIT(10)
-
-/*-----------------------------------------------------------------------------
-				Below is the definition for 802.11i / 802.1x
-------------------------------------------------------------------------------*/
-#define _IEEE8021X_MGT_			1	/*  WPA */
-#define _IEEE8021X_PSK_			2	/*  WPA with pre-shared key */
-
-/*
-#define _NO_PRIVACY_			0
-#define _WEP_40_PRIVACY_		1
-#define _TKIP_PRIVACY_			2
-#define _WRAP_PRIVACY_			3
-#define _CCMP_PRIVACY_			4
-#define _WEP_104_PRIVACY_		5
-#define _WEP_WPA_MIXED_PRIVACY_ 6	WEP + WPA
-*/
-
 /*-----------------------------------------------------------------------------
 				Below is the definition for WMM
 ------------------------------------------------------------------------------*/
@@ -312,13 +63,6 @@ static inline unsigned char *get_hdr_bssid(unsigned char *pframe)
 				Below is the definition for 802.11n
 ------------------------------------------------------------------------------*/
 
-#define SetOrderBit(pbuf)						\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16(_ORDER_))
-
-#define GetOrderBit(pbuf)		\
-	(((*(unsigned short *)(pbuf)) & le16_to_cpu(_ORDER_)) != 0)
-
-
 /* struct rtw_ieee80211_ht_cap - HT additional information
  *
  * This structure refers to "HT information element" as
@@ -372,38 +116,7 @@ struct ADDBA_request {
 }  __packed;
 
 
-#define OP_MODE_PURE                    0
-#define OP_MODE_MAY_BE_LEGACY_STAS      1
-#define OP_MODE_20MHZ_HT_STA_ASSOCED    2
-#define OP_MODE_MIXED                   3
-
-#define HT_INFO_HT_PARAM_SECONDARY_CHNL_OFF_MASK	((u8) BIT(0) | BIT(1))
-#define HT_INFO_HT_PARAM_SECONDARY_CHNL_ABOVE		((u8) BIT(0))
-#define HT_INFO_HT_PARAM_SECONDARY_CHNL_BELOW		((u8) BIT(0) | BIT(1))
-#define HT_INFO_HT_PARAM_REC_TRANS_CHNL_WIDTH		((u8) BIT(2))
-#define HT_INFO_HT_PARAM_RIFS_MODE			((u8) BIT(3))
-#define HT_INFO_HT_PARAM_CTRL_ACCESS_ONLY		((u8) BIT(4))
-#define HT_INFO_HT_PARAM_SRV_INTERVAL_GRANULARITY	((u8) BIT(5))
-
-#define HT_INFO_OPERATION_MODE_OP_MODE_MASK	\
-		((u16) (0x0001 | 0x0002))
-#define HT_INFO_OPERATION_MODE_OP_MODE_OFFSET		0
-#define HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT	((u8) BIT(2))
-#define HT_INFO_OPERATION_MODE_TRANSMIT_BURST_LIMIT	((u8) BIT(3))
-#define HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT	((u8) BIT(4))
-
-#define HT_INFO_STBC_PARAM_DUAL_BEACON		((u16) BIT(6))
-#define HT_INFO_STBC_PARAM_DUAL_STBC_PROTECT	((u16) BIT(7))
-#define HT_INFO_STBC_PARAM_SECONDARY_BCN	((u16) BIT(8))
-#define HT_INFO_STBC_PARAM_LSIG_TXOP_PROTECT_ALLOWED	((u16) BIT(9))
-#define HT_INFO_STBC_PARAM_PCO_ACTIVE		((u16) BIT(10))
-#define HT_INFO_STBC_PARAM_PCO_PHASE		((u16) BIT(11))
-
-
-
 /*	===============WPS Section=============== */
-/*	For WPSv1.0 */
-#define WPSOUI					0x0050f204
 /*	WPS attribute ID */
 #define WPS_ATTR_VER1				0x104A
 #define WPS_ATTR_SIMPLE_CONF_STATE		0x1044
@@ -425,182 +138,6 @@ struct ADDBA_request {
 #define WPS_ATTR_VENDOR_EXT			0x1049
 #define WPS_ATTR_SELECTED_REGISTRAR		0x1041
 
-/*	Value of WPS attribute "WPS_ATTR_DEVICE_NAME */
-#define WPS_MAX_DEVICE_NAME_LEN			32
-
-/*	Value of WPS Request Type Attribute */
-#define WPS_REQ_TYPE_ENROLLEE_INFO_ONLY		0x00
-#define WPS_REQ_TYPE_ENROLLEE_OPEN_8021X	0x01
-#define WPS_REQ_TYPE_REGISTRAR			0x02
-#define WPS_REQ_TYPE_WLAN_MANAGER_REGISTRAR	0x03
-
-/*	Value of WPS Response Type Attribute */
-#define WPS_RESPONSE_TYPE_INFO_ONLY		0x00
-#define WPS_RESPONSE_TYPE_8021X			0x01
-#define WPS_RESPONSE_TYPE_REGISTRAR		0x02
-#define WPS_RESPONSE_TYPE_AP			0x03
-
-/*	Value of WPS WiFi Simple Configuration State Attribute */
-#define WPS_WSC_STATE_NOT_CONFIG		0x01
-#define WPS_WSC_STATE_CONFIG			0x02
-
-/*	Value of WPS Version Attribute */
-#define WPS_VERSION_1				0x10
-
-/*	Value of WPS Configuration Method Attribute */
-#define WPS_CONFIG_METHOD_FLASH			0x0001
-#define WPS_CONFIG_METHOD_ETHERNET		0x0002
-#define WPS_CONFIG_METHOD_LABEL			0x0004
-#define WPS_CONFIG_METHOD_DISPLAY		0x0008
-#define WPS_CONFIG_METHOD_E_NFC			0x0010
-#define WPS_CONFIG_METHOD_I_NFC			0x0020
-#define WPS_CONFIG_METHOD_NFC			0x0040
-#define WPS_CONFIG_METHOD_PBC			0x0080
-#define WPS_CONFIG_METHOD_KEYPAD		0x0100
-#define WPS_CONFIG_METHOD_VPBC			0x0280
-#define WPS_CONFIG_METHOD_PPBC			0x0480
-#define WPS_CONFIG_METHOD_VDISPLAY		0x2008
-#define WPS_CONFIG_METHOD_PDISPLAY		0x4008
-
-/*	Value of Category ID of WPS Primary Device Type Attribute */
-#define WPS_PDT_CID_DISPLAYS			0x0007
-#define WPS_PDT_CID_MULIT_MEDIA			0x0008
-#define WPS_PDT_CID_RTK_WIDI			WPS_PDT_CID_MULIT_MEDIA
-
-/*	Value of Sub Category ID of WPS Primary Device Type Attribute */
-#define WPS_PDT_SCID_MEDIA_SERVER		0x0005
-#define WPS_PDT_SCID_RTK_DMP			WPS_PDT_SCID_MEDIA_SERVER
-
-/*	Value of Device Password ID */
-#define WPS_DPID_PIN				0x0000
-#define WPS_DPID_USER_SPEC			0x0001
-#define WPS_DPID_MACHINE_SPEC			0x0002
-#define WPS_DPID_REKEY				0x0003
-#define WPS_DPID_PBC				0x0004
-#define WPS_DPID_REGISTRAR_SPEC			0x0005
-
-/*	Value of WPS RF Bands Attribute */
-#define WPS_RF_BANDS_2_4_GHZ			0x01
-#define WPS_RF_BANDS_5_GHZ			0x02
-
-/*	Value of WPS Association State Attribute */
-#define WPS_ASSOC_STATE_NOT_ASSOCIATED		0x00
-#define WPS_ASSOC_STATE_CONNECTION_SUCCESS	0x01
-#define WPS_ASSOC_STATE_CONFIGURATION_FAILURE	0x02
-#define WPS_ASSOC_STATE_ASSOCIATION_FAILURE	0x03
-#define WPS_ASSOC_STATE_IP_FAILURE		0x04
-
-/*	=====================P2P Section===================== */
-/*	For P2P */
-#define	P2POUI					0x506F9A09
-
-/*	P2P Attribute ID */
-#define	P2P_ATTR_STATUS				0x00
-#define	P2P_ATTR_MINOR_REASON_CODE		0x01
-#define	P2P_ATTR_CAPABILITY			0x02
-#define	P2P_ATTR_DEVICE_ID			0x03
-#define	P2P_ATTR_GO_INTENT			0x04
-#define	P2P_ATTR_CONF_TIMEOUT			0x05
-#define	P2P_ATTR_LISTEN_CH			0x06
-#define	P2P_ATTR_GROUP_BSSID			0x07
-#define	P2P_ATTR_EX_LISTEN_TIMING		0x08
-#define	P2P_ATTR_INTENTED_IF_ADDR		0x09
-#define	P2P_ATTR_MANAGEABILITY			0x0A
-#define	P2P_ATTR_CH_LIST			0x0B
-#define	P2P_ATTR_NOA				0x0C
-#define	P2P_ATTR_DEVICE_INFO			0x0D
-#define	P2P_ATTR_GROUP_INFO			0x0E
-#define	P2P_ATTR_GROUP_ID			0x0F
-#define	P2P_ATTR_INTERFACE			0x10
-#define	P2P_ATTR_OPERATING_CH			0x11
-#define	P2P_ATTR_INVITATION_FLAGS		0x12
-
-/*	Value of Status Attribute */
-#define	P2P_STATUS_SUCCESS			0x00
-#define	P2P_STATUS_FAIL_INFO_UNAVAILABLE	0x01
-#define	P2P_STATUS_FAIL_INCOMPATIBLE_PARAM	0x02
-#define	P2P_STATUS_FAIL_LIMIT_REACHED		0x03
-#define	P2P_STATUS_FAIL_INVALID_PARAM		0x04
-#define	P2P_STATUS_FAIL_REQUEST_UNABLE		0x05
-#define	P2P_STATUS_FAIL_PREVOUS_PROTO_ERR	0x06
-#define	P2P_STATUS_FAIL_NO_COMMON_CH		0x07
-#define	P2P_STATUS_FAIL_UNKNOWN_P2PGROUP	0x08
-#define	P2P_STATUS_FAIL_BOTH_GOINTENT_15	0x09
-#define	P2P_STATUS_FAIL_INCOMPATIBLE_PROVSION	0x0A
-#define	P2P_STATUS_FAIL_USER_REJECT		0x0B
-
-/*	Value of Inviation Flags Attribute */
-#define	P2P_INVITATION_FLAGS_PERSISTENT		BIT(0)
-
-#define	DMP_P2P_DEVCAP_SUPPORT	(P2P_DEVCAP_SERVICE_DISCOVERY | \
-				 P2P_DEVCAP_CLIENT_DISCOVERABILITY | \
-				 P2P_DEVCAP_CONCURRENT_OPERATION | \
-				 P2P_DEVCAP_INVITATION_PROC)
-
-#define	DMP_P2P_GRPCAP_SUPPORT	(P2P_GRPCAP_INTRABSS)
-
-/*	Value of Device Capability Bitmap */
-#define	P2P_DEVCAP_SERVICE_DISCOVERY		BIT(0)
-#define	P2P_DEVCAP_CLIENT_DISCOVERABILITY	BIT(1)
-#define	P2P_DEVCAP_CONCURRENT_OPERATION		BIT(2)
-#define	P2P_DEVCAP_INFRA_MANAGED		BIT(3)
-#define	P2P_DEVCAP_DEVICE_LIMIT			BIT(4)
-#define	P2P_DEVCAP_INVITATION_PROC		BIT(5)
-
-/*	Value of Group Capability Bitmap */
-#define	P2P_GRPCAP_GO				BIT(0)
-#define	P2P_GRPCAP_PERSISTENT_GROUP		BIT(1)
-#define	P2P_GRPCAP_GROUP_LIMIT			BIT(2)
-#define	P2P_GRPCAP_INTRABSS			BIT(3)
-#define	P2P_GRPCAP_CROSS_CONN			BIT(4)
-#define	P2P_GRPCAP_PERSISTENT_RECONN		BIT(5)
-#define	P2P_GRPCAP_GROUP_FORMATION		BIT(6)
-
-/*	P2P Public Action Frame ( Management Frame ) */
-#define	P2P_PUB_ACTION_ACTION			0x09
-
-/*	P2P Public Action Frame Type */
-#define	P2P_GO_NEGO_REQ				0
-#define	P2P_GO_NEGO_RESP			1
-#define	P2P_GO_NEGO_CONF			2
-#define	P2P_INVIT_REQ				3
-#define	P2P_INVIT_RESP				4
-#define	P2P_DEVDISC_REQ				5
-#define	P2P_DEVDISC_RESP			6
-#define	P2P_PROVISION_DISC_REQ			7
-#define	P2P_PROVISION_DISC_RESP			8
-
-/*	P2P Action Frame Type */
-#define	P2P_NOTICE_OF_ABSENCE			0
-#define	P2P_PRESENCE_REQUEST			1
-#define	P2P_PRESENCE_RESPONSE			2
-#define	P2P_GO_DISC_REQUEST			3
-
-
-#define	P2P_MAX_PERSISTENT_GROUP_NUM		10
-
-#define	P2P_PROVISIONING_SCAN_CNT		3
-
-#define	P2P_WILDCARD_SSID_LEN			7
-
-#define	P2P_FINDPHASE_EX_NONE			0	/*  default value, used when: (1)p2p disabed or (2)p2p enabled but only do 1 scan phase */
-#define	P2P_FINDPHASE_EX_FULL			1	/*  used when p2p enabled and want to do 1 scan phase and P2P_FINDPHASE_EX_MAX-1 find phase */
-#define	P2P_FINDPHASE_EX_SOCIAL_FIRST		(P2P_FINDPHASE_EX_FULL+1)
-#define	P2P_FINDPHASE_EX_MAX					4
-#define	P2P_FINDPHASE_EX_SOCIAL_LAST		P2P_FINDPHASE_EX_MAX
-
-#define	P2P_PROVISION_TIMEOUT			5000	/*5 sec timeout for sending the provision discovery request */
-#define	P2P_CONCURRENT_PROVISION_TIMEOUT	3000	/*3 sec timeout for sending the provision discovery request under concurrent mode */
-#define	P2P_GO_NEGO_TIMEOUT			5000	/*5 sec timeout for receiving the group negotation response */
-#define	P2P_CONCURRENT_GO_NEGO_TIMEOUT		3000	/*3 sec timeout for sending the negotiation request under concurrent mode */
-#define	P2P_TX_PRESCAN_TIMEOUT			100	/*100ms */
-#define	P2P_INVITE_TIMEOUT			5000	/*5 sec timeout for sending the invitation request */
-#define	P2P_CONCURRENT_INVITE_TIMEOUT		3000	/*3 sec timeout for sending the invitation request under concurrent mode */
-#define	P2P_RESET_SCAN_CH			25000	/*25 sec t/o to reset the scan channel ( based on channel plan ) */
-#define	P2P_MAX_INTENT				15
-
-#define	P2P_MAX_NOA_NUM				2
-
 /*	WPS Configuration Method */
 #define	WPS_CM_NONE					0x0000
 #define	WPS_CM_LABEL					0x0004
@@ -615,93 +152,4 @@ struct ADDBA_request {
 #define	WPS_CM_SW_DISPLAY_PIN				0x2008
 #define	WPS_CM_LCD_DISPLAY_PIN				0x4008
 
-enum P2P_ROLE {
-	P2P_ROLE_DISABLE = 0,
-	P2P_ROLE_DEVICE = 1,
-	P2P_ROLE_CLIENT = 2,
-	P2P_ROLE_GO = 3
-};
-
-enum P2P_STATE {
-	P2P_STATE_NONE = 0,			/*P2P disable */
-	P2P_STATE_IDLE = 1,			/*P2P had enabled and do nothing */
-	P2P_STATE_LISTEN = 2,			/*In pure listen state */
-	P2P_STATE_SCAN = 3,			/*In scan phase */
-	P2P_STATE_FIND_PHASE_LISTEN = 4,	/*In the listen state of find phase */
-	P2P_STATE_FIND_PHASE_SEARCH = 5,	/*In the search state of find phase */
-	P2P_STATE_TX_PROVISION_DIS_REQ = 6,	/*In P2P provisioning discovery */
-	P2P_STATE_RX_PROVISION_DIS_RSP = 7,
-	P2P_STATE_RX_PROVISION_DIS_REQ = 8,
-	P2P_STATE_GONEGO_ING = 9,		/*Doing the group owner negoitation handshake */
-	P2P_STATE_GONEGO_OK = 10,		/*finish the group negoitation handshake with success */
-	P2P_STATE_GONEGO_FAIL = 11,		/*finish the group negoitation handshake with failure */
-	P2P_STATE_RECV_INVITE_REQ_MATCH = 12,	/*receiving the P2P Inviation request and match with the profile. */
-	P2P_STATE_PROVISIONING_ING = 13,	/*Doing the P2P WPS */
-	P2P_STATE_PROVISIONING_DONE = 14,	/*Finish the P2P WPS */
-	P2P_STATE_TX_INVITE_REQ = 15,		/*Transmit the P2P Invitation request */
-	P2P_STATE_RX_INVITE_RESP_OK = 16,	/*Receiving the P2P Invitation response */
-	P2P_STATE_RECV_INVITE_REQ_DISMATCH = 17,/*receiving the P2P Inviation request and dismatch with the profile. */
-	P2P_STATE_RECV_INVITE_REQ_GO = 18,	/*receiving the P2P Inviation request and this wifi is GO. */
-	P2P_STATE_RECV_INVITE_REQ_JOIN = 19,	/*receiving the P2P Inviation request to join an existing P2P Group. */
-	P2P_STATE_RX_INVITE_RESP_FAIL = 20,	/*receiving the P2P Inviation response with failure */
-	P2P_STATE_RX_INFOR_NOREADY = 21,	/*receiving p2p negotiation response with information is not available */
-	P2P_STATE_TX_INFOR_NOREADY = 22,	/*sending p2p negotiation response with information is not available */
-};
-
-enum P2P_WPSINFO {
-	P2P_NO_WPSINFO				= 0,
-	P2P_GOT_WPSINFO_PEER_DISPLAY_PIN	= 1,
-	P2P_GOT_WPSINFO_SELF_DISPLAY_PIN	= 2,
-	P2P_GOT_WPSINFO_PBC			= 3,
-};
-
-#define	P2P_PRIVATE_IOCTL_SET_LEN		64
-
-enum P2P_PROTO_WK_ID {
-	P2P_FIND_PHASE_WK = 0,
-	P2P_RESTORE_STATE_WK = 1,
-	P2P_PRE_TX_PROVDISC_PROCESS_WK = 2,
-	P2P_PRE_TX_NEGOREQ_PROCESS_WK = 3,
-	P2P_PRE_TX_INVITEREQ_PROCESS_WK = 4,
-	P2P_AP_P2P_CH_SWITCH_PROCESS_WK = 5,
-	P2P_RO_CH_WK = 6,
-};
-
-#ifdef CONFIG_8723AU_P2P
-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 */
-};
-#endif /*  CONFIG_8723AU_P2P */
-
-/*	=====================WFD Section===================== */
-/*	For Wi-Fi Display */
-#define	WFD_ATTR_DEVICE_INFO			0x00
-#define	WFD_ATTR_ASSOC_BSSID			0x01
-#define	WFD_ATTR_COUPLED_SINK_INFO	0x06
-#define	WFD_ATTR_LOCAL_IP_ADDR		0x08
-#define	WFD_ATTR_SESSION_INFO		0x09
-#define	WFD_ATTR_ALTER_MAC			0x0a
-
-/*	For WFD Device Information Attribute */
-#define	WFD_DEVINFO_SOURCE					0x0000
-#define	WFD_DEVINFO_PSINK					0x0001
-#define	WFD_DEVINFO_SSINK					0x0002
-#define	WFD_DEVINFO_DUAL					0x0003
-
-#define	WFD_DEVINFO_SESSION_AVAIL			0x0010
-#define	WFD_DEVINFO_WSD						0x0040
-#define	WFD_DEVINFO_PC_TDLS					0x0080
-#define	WFD_DEVINFO_HDCP_SUPPORT			0x0100
-
 #endif /*  _WIFI_H_ */
diff --git a/drivers/staging/rtl8723au/include/wlan_bssdef.h b/drivers/staging/rtl8723au/include/wlan_bssdef.h
index 92287ebe5b9..664015d049e 100644
--- a/drivers/staging/rtl8723au/include/wlan_bssdef.h
+++ b/drivers/staging/rtl8723au/include/wlan_bssdef.h
@@ -22,54 +22,6 @@
 #define NDIS_802_11_LENGTH_RATES        8
 #define NDIS_802_11_LENGTH_RATES_EX     16
 
-enum ndis_802_11_net_type {
-	Ndis802_11FH,
-	Ndis802_11DS,
-	Ndis802_11OFDM5,
-	Ndis802_11OFDM24,
-	Ndis802_11NetworkTypeMax    /*  just an upper bound */
-};
-
-struct ndis_802_11_configuration_fh {
-	u32           Length;             /*  Length of structure */
-	u32           HopPattern;         /*  As defined by 802.11, MSB set */
-	u32           HopSet;             /*  to one if non-802.11 */
-	u32           DwellTime;          /*  units are Kusec */
-};
-
-
-/*
-	FW will only save the channel number in DSConfig.
-	ODI Handler will convert the channel number to freq. number.
-*/
-struct ndis_802_11_config {
-	u32           Length;             /*  Length of structure */
-	u32           BeaconPeriod;       /*  units are Kusec */
-	u32           ATIMWindow;         /*  units are Kusec */
-	u32           DSConfig;           /*  Frequency, units are kHz */
-	struct ndis_802_11_configuration_fh    FHConfig;
-};
-
-enum ndis_802_11_net_infra {
-	Ndis802_11IBSS,
-	Ndis802_11Infrastructure,
-	Ndis802_11AutoUnknown,
-	Ndis802_11InfrastructureMax,     /*  Not a real value, defined as upper bound */
-	Ndis802_11APMode
-};
-
-struct ndis_802_11_fixed_ies {
-	u8  Timestamp[8];
-	u16  BeaconInterval;
-	u16  Capabilities;
-};
-
-struct ndis_802_11_var_ies {
-	u8  ElementID;
-	u8  Length;
-	u8  data[1];
-};
-
 /* Length is the 4 bytes multiples of the sum of
  * sizeof(6 * sizeof(unsigned char)) + 2 + sizeof(struct ndis_802_11_ssid) +
  * sizeof(u32) + sizeof(long) + sizeof(enum ndis_802_11_net_type) +
@@ -115,34 +67,6 @@ struct ndis_802_11_key {
 	u8 KeyMaterial[32]; /*  variable length depending on above field */
 };
 
-struct ndis_802_11_wep {
-	u32     Length;        /*  Length of this structure */
-	u32     KeyIndex;      /*  0 is the per-client key, 1-N are global */
-	u32     KeyLength;     /*  length of key in bytes */
-	u8     KeyMaterial[16];/*  variable length depending on above field */
-};
-
-enum NDIS_802_11_STATUS_TYPE {
-	Ndis802_11StatusType_Authentication,
-	Ndis802_11StatusType_MediaStreamMode,
-	Ndis802_11StatusType_PMKID_CandidateList,
-	Ndis802_11StatusTypeMax    /*  not a real type, just an upper bound */
-};
-
-/*  mask for authentication/integrity fields */
-#define NDIS_802_11_AUTH_REQUEST_AUTH_FIELDS        0x0f
-#define NDIS_802_11_AUTH_REQUEST_REAUTH			0x01
-#define NDIS_802_11_AUTH_REQUEST_KEYUPDATE		0x02
-#define NDIS_802_11_AUTH_REQUEST_PAIRWISE_ERROR		0x06
-#define NDIS_802_11_AUTH_REQUEST_GROUP_ERROR		0x0E
-
-/*  MIC check time, 60 seconds. */
-#define MIC_CHECK_TIME	60000000
-
-#ifndef Ndis802_11APMode
-#define Ndis802_11APMode (Ndis802_11InfrastructureMax+1)
-#endif
-
 struct wlan_phy_info {
 	u8	SignalStrength;/* in percentage) */
 	u8	SignalQuality;/* in percentage) */
@@ -170,9 +94,10 @@ struct wlan_bssid_ex {
 	struct cfg80211_ssid Ssid;
 	u32  Privacy;
 	long  Rssi;/* in dBM, raw data , get from PHY) */
-	enum ndis_802_11_net_type  NetworkTypeInUse;
-	struct ndis_802_11_config  Configuration;
-	enum ndis_802_11_net_infra  InfrastructureMode;
+	u16 BeaconPeriod;       /*  units are Kusec */
+	u32 ATIMWindow;         /*  units are Kusec */
+	u32 DSConfig;           /*  Frequency, units are kHz */
+	enum nl80211_iftype ifmode;
 	unsigned char SupportedRates[NDIS_802_11_LENGTH_RATES_EX];
 	struct wlan_phy_info	PhyInfo;
 	u32  IELength;
diff --git a/drivers/staging/rtl8723au/include/xmit_osdep.h b/drivers/staging/rtl8723au/include/xmit_osdep.h
index 0eca53ece75..2be04c48656 100644
--- a/drivers/staging/rtl8723au/include/xmit_osdep.h
+++ b/drivers/staging/rtl8723au/include/xmit_osdep.h
@@ -18,24 +18,9 @@
 #include <osdep_service.h>
 #include <drv_types.h>
 
-struct pkt_file {
-	struct sk_buff *pkt;
-	__kernel_size_t pkt_len; /* the remainder length of the open_file */
-	unsigned char *cur_buffer;
-	u8 *buf_start;
-	u8 *cur_addr;
-	__kernel_size_t buf_len;
-};
-
 
 #define NR_XMITFRAME	256
 
-struct xmit_priv;
-struct pkt_attrib;
-struct sta_xmit_priv;
-struct xmit_frame;
-struct xmit_buf;
-
 int rtw_xmit23a_entry23a(struct sk_buff *pkt, struct net_device *pnetdev);
 
 void rtw_os_xmit_schedule23a(struct rtw_adapter *padapter);
@@ -44,10 +29,6 @@ int rtw_os_xmit_resource_alloc23a(struct rtw_adapter *padapter,
 			       struct xmit_buf *pxmitbuf, u32 alloc_sz);
 void rtw_os_xmit_resource_free23a(struct rtw_adapter *padapter,
 			       struct xmit_buf *pxmitbuf);
-uint rtw_remainder_len23a(struct pkt_file *pfile);
-void _rtw_open_pktfile23a(struct sk_buff *pkt, struct pkt_file *pfile);
-uint _rtw_pktfile_read23a(struct pkt_file *pfile, u8 *rmem, uint rlen);
-int rtw_endofpktfile23a(struct pkt_file *pfile);
 
 void rtw_os_pkt_complete23a(struct rtw_adapter *padapter, struct sk_buff *pkt);
 void rtw_os_xmit_complete23a(struct rtw_adapter *padapter,
diff --git a/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c b/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c
index 50840b9a11f..0c9f5cebfb4 100644
--- a/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c
+++ b/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c
@@ -20,7 +20,6 @@
 #include <xmit_osdep.h>
 
 #include "ioctl_cfg80211.h"
-#include <linux/version.h>
 
 #define RTW_MAX_MGMT_TX_CNT 8
 
@@ -260,26 +259,21 @@ static int rtw_cfg80211_inform_bss(struct rtw_adapter *padapter,
 	size_t notify_ielen;
 	s32 notify_signal;
 	u8 buf[MAX_BSSINFO_LEN], *pbuf;
-	size_t len, bssinf_len = 0;
+	size_t len;
 	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
-	u8 bc_addr[] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
-
 	struct wireless_dev *wdev = padapter->rtw_wdev;
 	struct wiphy *wiphy = wdev->wiphy;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
 	/* DBG_8723A("%s\n", __func__); */
 
-	bssinf_len =
-		pnetwork->network.IELength + sizeof(struct ieee80211_hdr_3addr);
-	if (bssinf_len > MAX_BSSINFO_LEN) {
+	if (pnetwork->network.IELength > MAX_IE_SZ) {
 		DBG_8723A("%s IE Length too long > %d byte\n", __func__,
-			  MAX_BSSINFO_LEN);
+			  MAX_IE_SZ);
 		goto exit;
 	}
 
-	channel = pnetwork->network.Configuration.DSConfig;
+	channel = pnetwork->network.DSConfig;
 	if (channel <= RTW_CH_MAX_2G_CHANNEL)
 		freq = ieee80211_channel_to_frequency(channel,
 						      IEEE80211_BAND_2GHZ);
@@ -289,14 +283,13 @@ static int rtw_cfg80211_inform_bss(struct rtw_adapter *padapter,
 
 	notify_channel = ieee80211_get_channel(wiphy, freq);
 
-	/* rtw_get_timestampe_from_ie23a() */
 	notify_timestamp = jiffies_to_msecs(jiffies) * 1000;	/* uSec */
 
 	notify_interval =
-	    le16_to_cpu(*(u16 *)
+		get_unaligned_le16(
 			rtw_get_beacon_interval23a_from_ie(pnetwork->network.IEs));
 	notify_capability =
-	    le16_to_cpu(*(u16 *)
+		get_unaligned_le16(
 			rtw_get_capability23a_from_ie(pnetwork->network.IEs));
 
 	notify_ie = pnetwork->network.IEs + _FIXED_IE_LENGTH_;
@@ -315,21 +308,22 @@ static int rtw_cfg80211_inform_bss(struct rtw_adapter *padapter,
 	pbuf = buf;
 
 	pwlanhdr = (struct ieee80211_hdr *)pbuf;
-	fctrl = &pwlanhdr->frame_control;
-	*(fctrl) = 0;
 
-	SetSeqNum(pwlanhdr, 0);
+	pwlanhdr->seq_ctrl = 0;
 
 	if (pnetwork->network.reserved == 1) {	/*  WIFI_BEACON */
-		memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
-		SetFrameSubType(pbuf, WIFI_BEACON);
+		eth_broadcast_addr(pwlanhdr->addr1);
+		pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+						      IEEE80211_STYPE_BEACON);
 	} else {
-		memcpy(pwlanhdr->addr1, myid(&padapter->eeprompriv), ETH_ALEN);
-		SetFrameSubType(pbuf, WIFI_PROBERSP);
+		ether_addr_copy(pwlanhdr->addr1, myid(&padapter->eeprompriv));
+		pwlanhdr->frame_control =
+			cpu_to_le16(IEEE80211_FTYPE_MGMT |
+				    IEEE80211_STYPE_PROBE_RESP);
 	}
 
-	memcpy(pwlanhdr->addr2, pnetwork->network.MacAddress, ETH_ALEN);
-	memcpy(pwlanhdr->addr3, pnetwork->network.MacAddress, ETH_ALEN);
+	ether_addr_copy(pwlanhdr->addr2, pnetwork->network.MacAddress);
+	ether_addr_copy(pwlanhdr->addr3, pnetwork->network.MacAddress);
 
 	pbuf += sizeof(struct ieee80211_hdr_3addr);
 	len = sizeof(struct ieee80211_hdr_3addr);
@@ -357,9 +351,6 @@ void rtw_cfg80211_indicate_connect(struct rtw_adapter *padapter)
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct wlan_network *cur_network = &pmlmepriv->cur_network;
 	struct wireless_dev *pwdev = padapter->rtw_wdev;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif
 
 	DBG_8723A("%s(padapter =%p)\n", __func__, padapter);
 
@@ -370,22 +361,11 @@ void rtw_cfg80211_indicate_connect(struct rtw_adapter *padapter)
 	if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
 		return;
 
-#ifdef CONFIG_8723AU_P2P
-	if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
-		rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
-		rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
-		DBG_8723A("%s, role =%d, p2p_state =%d, pre_p2p_state =%d\n",
-			  __func__, rtw_p2p_role(pwdinfo),
-			  rtw_p2p_state(pwdinfo), rtw_p2p_pre_state(pwdinfo));
-	}
-#endif /* CONFIG_8723AU_P2P */
-
-	if (rtw_to_roaming(padapter) > 0) {
+	if (padapter->mlmepriv.to_roaming > 0) {
 		struct wiphy *wiphy = pwdev->wiphy;
 		struct ieee80211_channel *notify_channel;
 		u32 freq;
-		u16 channel = cur_network->network.Configuration.DSConfig;
+		u16 channel = cur_network->network.DSConfig;
 
 		if (channel <= RTW_CH_MAX_2G_CHANNEL)
 			freq =
@@ -429,9 +409,6 @@ void rtw_cfg80211_indicate_disconnect(struct rtw_adapter *padapter)
 {
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct wireless_dev *pwdev = padapter->rtw_wdev;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif
 
 	DBG_8723A("%s(padapter =%p)\n", __func__, padapter);
 
@@ -442,21 +419,6 @@ void rtw_cfg80211_indicate_disconnect(struct rtw_adapter *padapter)
 	if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
 		return;
 
-#ifdef CONFIG_8723AU_P2P
-	if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
-		del_timer_sync(&pwdinfo->find_phase_timer);
-		del_timer_sync(&pwdinfo->restore_p2p_state_timer);
-		del_timer_sync(&pwdinfo->pre_tx_scan_timer);
-
-		rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
-		rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-
-		DBG_8723A("%s, role =%d, p2p_state =%d, pre_p2p_state =%d\n",
-			  __func__, rtw_p2p_role(pwdinfo),
-			  rtw_p2p_state(pwdinfo), rtw_p2p_pre_state(pwdinfo));
-	}
-#endif /* CONFIG_8723AU_P2P */
-
 	if (!padapter->mlmepriv.not_indic_disco) {
 		if (check_fwstate(&padapter->mlmepriv, WIFI_UNDER_LINKING)) {
 			cfg80211_connect_result(padapter->pnetdev, NULL, NULL,
@@ -471,12 +433,12 @@ void rtw_cfg80211_indicate_disconnect(struct rtw_adapter *padapter)
 }
 
 #ifdef CONFIG_8723AU_AP_MODE
-static u8 set_pairwise_key(struct rtw_adapter *padapter, struct sta_info *psta)
+static int set_pairwise_key(struct rtw_adapter *padapter, struct sta_info *psta)
 {
 	struct cmd_obj *ph2c;
 	struct set_stakey_parm *psetstakey_para;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	ph2c = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
 	if (ph2c == NULL) {
@@ -493,9 +455,9 @@ static u8 set_pairwise_key(struct rtw_adapter *padapter, struct sta_info *psta)
 
 	init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
 
-	psetstakey_para->algorithm = (u8) psta->dot118021XPrivacy;
+	psetstakey_para->algorithm = psta->dot118021XPrivacy;
 
-	memcpy(psetstakey_para->addr, psta->hwaddr, ETH_ALEN);
+	ether_addr_copy(psetstakey_para->addr, psta->hwaddr);
 
 	memcpy(psetstakey_para->key, &psta->dot118021x_UncstKey, 16);
 
@@ -505,8 +467,8 @@ exit:
 	return res;
 }
 
-static int set_group_key(struct rtw_adapter *padapter, u8 *key, u8 alg,
-			 int keyid)
+static int set_group_key(struct rtw_adapter *padapter, u8 *key, u32 alg,
+			 u8 keyid)
 {
 	u8 keylen;
 	struct cmd_obj *pcmd;
@@ -516,6 +478,11 @@ static int set_group_key(struct rtw_adapter *padapter, u8 *key, u8 alg,
 
 	DBG_8723A("%s\n", __func__);
 
+	if (keyid >= 4) {
+		res = _FAIL;
+		goto exit;
+	}
+
 	pcmd = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
 	if (!pcmd) {
 		res = _FAIL;
@@ -528,24 +495,23 @@ static int set_group_key(struct rtw_adapter *padapter, u8 *key, u8 alg,
 		goto exit;
 	}
 
-	psetkeyparm->keyid = (u8) keyid;
+	psetkeyparm->keyid = keyid;
 	if (is_wep_enc(alg))
-		padapter->mlmepriv.key_mask |= CHKBIT(psetkeyparm->keyid);
+		padapter->mlmepriv.key_mask |= BIT(psetkeyparm->keyid);
 
 	psetkeyparm->algorithm = alg;
 
 	psetkeyparm->set_tx = 1;
 
 	switch (alg) {
-	case _WEP40_:
+	case WLAN_CIPHER_SUITE_WEP40:
 		keylen = 5;
 		break;
-	case _WEP104_:
+	case WLAN_CIPHER_SUITE_WEP104:
 		keylen = 13;
 		break;
-	case _TKIP_:
-	case _TKIP_WTMIC_:
-	case _AES_:
+	case WLAN_CIPHER_SUITE_TKIP:
+	case WLAN_CIPHER_SUITE_CCMP:
 	default:
 		keylen = 16;
 	}
@@ -558,28 +524,26 @@ static int set_group_key(struct rtw_adapter *padapter, u8 *key, u8 alg,
 	pcmd->rsp = NULL;
 	pcmd->rspsz = 0;
 
-	INIT_LIST_HEAD(&pcmd->list);
-
 	res = rtw_enqueue_cmd23a(pcmdpriv, pcmd);
 
 exit:
 	return res;
 }
 
-static int set_wep_key(struct rtw_adapter *padapter, u8 *key, u8 keylen,
-		       int keyid)
+static int set_wep_key(struct rtw_adapter *padapter, u8 *key, u16 keylen,
+		       u8 keyid)
 {
-	u8 alg;
+	u32 alg;
 
 	switch (keylen) {
 	case 5:
-		alg = _WEP40_;
+		alg = WLAN_CIPHER_SUITE_WEP40;
 		break;
 	case 13:
-		alg = _WEP104_;
+		alg = WLAN_CIPHER_SUITE_WEP104;
 		break;
 	default:
-		alg = _NO_PRIVACY_;
+		alg = 0;
 	}
 
 	return set_group_key(padapter, key, alg, keyid);
@@ -590,7 +554,8 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev,
 					  u32 param_len)
 {
 	int ret = 0;
-	u32 wep_key_idx, wep_key_len;
+	u16 wep_key_len;
+	u8 wep_key_idx;
 	struct sta_info *psta = NULL, *pbcmc_sta = NULL;
 	struct rtw_adapter *padapter = netdev_priv(dev);
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
@@ -658,21 +623,21 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev,
 
 			psecuritypriv->ndisencryptstatus =
 				Ndis802_11Encryption1Enabled;
-			psecuritypriv->dot11PrivacyAlgrthm = _WEP40_;
-			psecuritypriv->dot118021XGrpPrivacy = _WEP40_;
+			psecuritypriv->dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_WEP40;
+			psecuritypriv->dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_WEP40;
 
 			if (wep_key_len == 13) {
-				psecuritypriv->dot11PrivacyAlgrthm = _WEP104_;
-				psecuritypriv->dot118021XGrpPrivacy = _WEP104_;
+				psecuritypriv->dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_WEP104;
+				psecuritypriv->dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_WEP104;
 			}
 
 			psecuritypriv->dot11PrivacyKeyIndex = wep_key_idx;
 		}
 
-		memcpy(&psecuritypriv->dot11DefKey[wep_key_idx].skey[0],
+		memcpy(&psecuritypriv->wep_key[wep_key_idx].key,
 		       param->u.crypt.key, wep_key_len);
 
-		psecuritypriv->dot11DefKeylen[wep_key_idx] = wep_key_len;
+		psecuritypriv->wep_key[wep_key_idx].keylen = wep_key_len;
 
 		set_wep_key(padapter, param->u.crypt.key, wep_key_len,
 			    wep_key_idx);
@@ -684,8 +649,7 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev,
 	if (!psta && check_fwstate(pmlmepriv, WIFI_AP_STATE)) {	/*  group key */
 		if (param->u.crypt.set_tx == 0) {	/* group key */
 			if (strcmp(param->u.crypt.alg, "WEP") == 0) {
-				DBG_8723A("%s, set group_key, WEP\n",
-					  __func__);
+				DBG_8723A("%s, set group_key, WEP\n", __func__);
 
 				memcpy(psecuritypriv->
 				       dot118021XGrpKey[param->u.crypt.idx].
@@ -693,17 +657,17 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev,
 				       (param->u.crypt.key_len >
 					16 ? 16 : param->u.crypt.key_len));
 
-				psecuritypriv->dot118021XGrpPrivacy = _WEP40_;
+				psecuritypriv->dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_WEP40;
 				if (param->u.crypt.key_len == 13) {
 					psecuritypriv->dot118021XGrpPrivacy =
-					    _WEP104_;
+					    WLAN_CIPHER_SUITE_WEP104;
 				}
 
 			} else if (strcmp(param->u.crypt.alg, "TKIP") == 0) {
 				DBG_8723A("%s, set group_key, TKIP\n",
 					  __func__);
 
-				psecuritypriv->dot118021XGrpPrivacy = _TKIP_;
+				psecuritypriv->dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_TKIP;
 
 				memcpy(psecuritypriv->
 				       dot118021XGrpKey[param->u.crypt.idx].
@@ -722,13 +686,13 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev,
 							     idx].skey,
 				       &param->u.crypt.key[24], 8);
 
-				psecuritypriv->busetkipkey = true;
+				psecuritypriv->busetkipkey = 1;
 
 			} else if (strcmp(param->u.crypt.alg, "CCMP") == 0) {
 				DBG_8723A("%s, set group_key, CCMP\n",
 					  __func__);
 
-				psecuritypriv->dot118021XGrpPrivacy = _AES_;
+				psecuritypriv->dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_CCMP;
 
 				memcpy(psecuritypriv->
 				       dot118021XGrpKey[param->u.crypt.idx].
@@ -740,12 +704,12 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev,
 					  __func__);
 
 				psecuritypriv->dot118021XGrpPrivacy =
-				    _NO_PRIVACY_;
+				    0;
 			}
 
 			psecuritypriv->dot118021XGrpKeyid = param->u.crypt.idx;
 
-			psecuritypriv->binstallGrpkey = true;
+			psecuritypriv->binstallGrpkey = 1;
 
 			psecuritypriv->dot11PrivacyAlgrthm =
 				psecuritypriv->dot118021XGrpPrivacy;
@@ -782,16 +746,16 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev,
 					DBG_8723A("%s, set pairwise key, WEP\n",
 						  __func__);
 
-					psta->dot118021XPrivacy = _WEP40_;
+					psta->dot118021XPrivacy = WLAN_CIPHER_SUITE_WEP40;
 					if (param->u.crypt.key_len == 13) {
 						psta->dot118021XPrivacy =
-							_WEP104_;
+							WLAN_CIPHER_SUITE_WEP104;
 					}
 				} else if (!strcmp(param->u.crypt.alg, "TKIP")) {
 					DBG_8723A("%s, set pairwise key, "
 						  "TKIP\n", __func__);
 
-					psta->dot118021XPrivacy = _TKIP_;
+					psta->dot118021XPrivacy = WLAN_CIPHER_SUITE_TKIP;
 
 					/* DEBUG_ERR("set key length :param->u.crypt.key_len =%d\n", param->u.crypt.key_len); */
 					/* set mic key */
@@ -800,19 +764,19 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev,
 					memcpy(psta->dot11tkiprxmickey.skey,
 					       &param->u.crypt.key[24], 8);
 
-					psecuritypriv->busetkipkey = true;
+					psecuritypriv->busetkipkey = 1;
 
 				} else if (!strcmp(param->u.crypt.alg, "CCMP")) {
 
 					DBG_8723A("%s, set pairwise key, "
 						  "CCMP\n", __func__);
 
-					psta->dot118021XPrivacy = _AES_;
+					psta->dot118021XPrivacy = WLAN_CIPHER_SUITE_CCMP;
 				} else {
 					DBG_8723A("%s, set pairwise key, "
 						  "none\n", __func__);
 
-					psta->dot118021XPrivacy = _NO_PRIVACY_;
+					psta->dot118021XPrivacy = 0;
 				}
 
 				set_pairwise_key(padapter, psta);
@@ -831,15 +795,15 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev,
 						key_len));
 
 					psecuritypriv->dot118021XGrpPrivacy =
-						_WEP40_;
+						WLAN_CIPHER_SUITE_WEP40;
 					if (param->u.crypt.key_len == 13) {
 						psecuritypriv->
 						    dot118021XGrpPrivacy =
-							_WEP104_;
+							WLAN_CIPHER_SUITE_WEP104;
 					}
 				} else if (!strcmp(param->u.crypt.alg, "TKIP")) {
 					psecuritypriv->dot118021XGrpPrivacy =
-					    _TKIP_;
+					    WLAN_CIPHER_SUITE_TKIP;
 
 					memcpy(psecuritypriv->
 					       dot118021XGrpKey[param->u.crypt.
@@ -864,11 +828,11 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev,
 					       skey, &param->u.crypt.key[24],
 					       8);
 
-					psecuritypriv->busetkipkey = true;
+					psecuritypriv->busetkipkey = 1;
 
 				} else if (!strcmp(param->u.crypt.alg, "CCMP")) {
 					psecuritypriv->dot118021XGrpPrivacy =
-						_AES_;
+						WLAN_CIPHER_SUITE_CCMP;
 
 					memcpy(psecuritypriv->
 					       dot118021XGrpKey[param->u.crypt.
@@ -879,13 +843,13 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev,
 						key_len));
 				} else {
 					psecuritypriv->dot118021XGrpPrivacy =
-						_NO_PRIVACY_;
+						0;
 				}
 
 				psecuritypriv->dot118021XGrpKeyid =
 					param->u.crypt.idx;
 
-				psecuritypriv->binstallGrpkey = true;
+				psecuritypriv->binstallGrpkey = 1;
 
 				psecuritypriv->dot11PrivacyAlgrthm =
 					psecuritypriv->dot118021XGrpPrivacy;
@@ -917,15 +881,11 @@ static int rtw_cfg80211_set_encryption(struct net_device *dev,
 				       struct ieee_param *param, u32 param_len)
 {
 	int ret = 0;
-	u32 wep_key_idx, wep_key_len;
+	u32 wep_key_idx;
+	u16 wep_key_len;
 	struct rtw_adapter *padapter = netdev_priv(dev);
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif /* CONFIG_8723AU_P2P */
-
-
 
 	DBG_8723A("%s\n", __func__);
 
@@ -970,21 +930,21 @@ static int rtw_cfg80211_set_encryption(struct net_device *dev,
 
 			psecuritypriv->ndisencryptstatus =
 				Ndis802_11Encryption1Enabled;
-			psecuritypriv->dot11PrivacyAlgrthm = _WEP40_;
-			psecuritypriv->dot118021XGrpPrivacy = _WEP40_;
+			psecuritypriv->dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_WEP40;
+			psecuritypriv->dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_WEP40;
 
 			if (wep_key_len == 13) {
-				psecuritypriv->dot11PrivacyAlgrthm = _WEP104_;
-				psecuritypriv->dot118021XGrpPrivacy = _WEP104_;
+				psecuritypriv->dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_WEP104;
+				psecuritypriv->dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_WEP104;
 			}
 
 			psecuritypriv->dot11PrivacyKeyIndex = wep_key_idx;
 		}
 
-		memcpy(&psecuritypriv->dot11DefKey[wep_key_idx].skey[0],
+		memcpy(&psecuritypriv->wep_key[wep_key_idx].key,
 		       param->u.crypt.key, wep_key_len);
 
-		psecuritypriv->dot11DefKeylen[wep_key_idx] = wep_key_len;
+		psecuritypriv->wep_key[wep_key_idx].keylen = wep_key_len;
 
 		rtw_set_key23a(padapter, psecuritypriv, wep_key_idx, 0);
 
@@ -1041,7 +1001,7 @@ static int rtw_cfg80211_set_encryption(struct net_device *dev,
 						       8);
 
 						padapter->securitypriv.
-							busetkipkey = false;
+							busetkipkey = 0;
 					}
 					DBG_8723A(" ~~~~set sta key:unicastkey\n");
 
@@ -1067,7 +1027,7 @@ static int rtw_cfg80211_set_encryption(struct net_device *dev,
 					       skey, &param->u.crypt.key[24],
 					       8);
 					padapter->securitypriv.binstallGrpkey =
-					    true;
+						1;
 					/* DEBUG_ERR((" param->u.crypt.key_len"
 					   "=%d\n", param->u.crypt.key_len)); */
 					DBG_8723A
@@ -1080,15 +1040,6 @@ static int rtw_cfg80211_set_encryption(struct net_device *dev,
 					rtw_set_key23a(padapter,
 						    &padapter->securitypriv,
 						    param->u.crypt.idx, 1);
-#ifdef CONFIG_8723AU_P2P
-					if (rtw_p2p_chk_state
-					    (pwdinfo,
-					     P2P_STATE_PROVISIONING_ING)) {
-						rtw_p2p_set_state(pwdinfo,
-								  P2P_STATE_PROVISIONING_DONE);
-					}
-#endif /* CONFIG_8723AU_P2P */
-
 				}
 			}
 
@@ -1127,13 +1078,13 @@ static int cfg80211_rtw_add_key(struct wiphy *wiphy, struct net_device *ndev,
 {
 	char *alg_name;
 	u32 param_len;
-	struct ieee_param *param = NULL;
+	struct ieee_param *param;
 	int ret = 0;
 	struct wireless_dev *rtw_wdev = wiphy_to_wdev(wiphy);
 	struct rtw_adapter *padapter = wiphy_to_adapter(wiphy);
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-	DBG_8723A(FUNC_NDEV_FMT " adding key for %pM\n", FUNC_NDEV_ARG(ndev),
+	DBG_8723A("%s(%s): adding key for %pM\n", __func__, ndev->name,
 		  mac_addr);
 	DBG_8723A("cipher = 0x%x\n", params->cipher);
 	DBG_8723A("key_len = 0x%x\n", params->key_len);
@@ -1143,11 +1094,11 @@ static int cfg80211_rtw_add_key(struct wiphy *wiphy, struct net_device *ndev,
 
 	param_len = sizeof(struct ieee_param) + params->key_len;
 	param = kzalloc(param_len, GFP_KERNEL);
-	if (param == NULL)
-		return -1;
+	if (!param)
+		return -ENOMEM;
 
 	param->cmd = IEEE_CMD_SET_ENCRYPTION;
-	memset(param->sta_addr, 0xff, ETH_ALEN);
+	eth_broadcast_addr(param->sta_addr);
 
 	switch (params->cipher) {
 	case IW_AUTH_CIPHER_NONE:
@@ -1196,7 +1147,7 @@ static int cfg80211_rtw_add_key(struct wiphy *wiphy, struct net_device *ndev,
 	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
 #ifdef CONFIG_8723AU_AP_MODE
 		if (mac_addr)
-			memcpy(param->sta_addr, (void *)mac_addr, ETH_ALEN);
+			ether_addr_copy(param->sta_addr, mac_addr);
 
 		ret = rtw_cfg80211_ap_set_encryption(ndev, param, param_len);
 #endif
@@ -1218,7 +1169,7 @@ cfg80211_rtw_get_key(struct wiphy *wiphy, struct net_device *ndev,
 		     void *cookie,
 		     void (*callback) (void *cookie, struct key_params *))
 {
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 	return 0;
 }
 
@@ -1229,8 +1180,7 @@ static int cfg80211_rtw_del_key(struct wiphy *wiphy, struct net_device *ndev,
 	struct rtw_adapter *padapter = netdev_priv(ndev);
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 
-	DBG_8723A(FUNC_NDEV_FMT " key_index =%d\n", FUNC_NDEV_ARG(ndev),
-		  key_index);
+	DBG_8723A("%s(%s): key_index =%d\n", __func__, ndev->name, key_index);
 
 	if (key_index == psecuritypriv->dot11PrivacyKeyIndex) {
 		/* clear the flag of wep default key set. */
@@ -1247,23 +1197,24 @@ static int cfg80211_rtw_set_default_key(struct wiphy *wiphy,
 	struct rtw_adapter *padapter = netdev_priv(ndev);
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 
-	DBG_8723A(FUNC_NDEV_FMT " key_index =%d"
-		  ", unicast =%d, multicast =%d.\n", FUNC_NDEV_ARG(ndev),
-		  key_index, unicast, multicast);
+	DBG_8723A("%s(%s): key_index =%d, unicast =%d, multicast =%d.\n",
+		  __func__, ndev->name, key_index, unicast, multicast);
 
-	if ((key_index < WEP_KEYS) &&
-	    ((psecuritypriv->dot11PrivacyAlgrthm == _WEP40_) ||
-	     (psecuritypriv->dot11PrivacyAlgrthm == _WEP104_))) {
+	if (key_index < NUM_WEP_KEYS &&
+	    (psecuritypriv->dot11PrivacyAlgrthm == WLAN_CIPHER_SUITE_WEP40 ||
+	     psecuritypriv->dot11PrivacyAlgrthm == WLAN_CIPHER_SUITE_WEP104)) {
 		/* set wep default key */
 		psecuritypriv->ndisencryptstatus = Ndis802_11Encryption1Enabled;
 
 		psecuritypriv->dot11PrivacyKeyIndex = key_index;
 
-		psecuritypriv->dot11PrivacyAlgrthm = _WEP40_;
-		psecuritypriv->dot118021XGrpPrivacy = _WEP40_;
-		if (psecuritypriv->dot11DefKeylen[key_index] == 13) {
-			psecuritypriv->dot11PrivacyAlgrthm = _WEP104_;
-			psecuritypriv->dot118021XGrpPrivacy = _WEP104_;
+		psecuritypriv->dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_WEP40;
+		psecuritypriv->dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_WEP40;
+		if (psecuritypriv->wep_key[key_index].keylen == 13) {
+			psecuritypriv->dot11PrivacyAlgrthm =
+				WLAN_CIPHER_SUITE_WEP104;
+			psecuritypriv->dot118021XGrpPrivacy =
+				WLAN_CIPHER_SUITE_WEP104;
 		}
 
 		/* set the flag to represent that wep default key
@@ -1287,7 +1238,7 @@ static int cfg80211_rtw_get_station(struct wiphy *wiphy,
 	sinfo->filled = 0;
 
 	if (!mac) {
-		DBG_8723A(FUNC_NDEV_FMT " mac ==%p\n", FUNC_NDEV_ARG(ndev), mac);
+		DBG_8723A("%s(%s): mac ==%p\n", __func__, ndev->name, mac);
 		ret = -ENOENT;
 		goto exit;
 	}
@@ -1298,17 +1249,15 @@ static int cfg80211_rtw_get_station(struct wiphy *wiphy,
 		ret = -ENOENT;
 		goto exit;
 	}
-#ifdef CONFIG_DEBUG_CFG80211
-	DBG_8723A(FUNC_NDEV_FMT " mac =" MAC_FMT "\n", FUNC_NDEV_ARG(ndev),
+	DBG_8723A("%s(%s): mac =" MAC_FMT "\n", __func__, ndev->name,
 		  MAC_ARG(mac));
-#endif
 
 	/* for infra./P2PClient mode */
 	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) &&
 	    check_fwstate(pmlmepriv, _FW_LINKED)) {
 		struct wlan_network *cur_network = &pmlmepriv->cur_network;
 
-		if (memcmp(mac, cur_network->network.MacAddress, ETH_ALEN)) {
+		if (!ether_addr_equal(mac, cur_network->network.MacAddress)) {
 			DBG_8723A("%s, mismatch bssid =" MAC_FMT "\n", __func__,
 				  MAC_ARG(cur_network->network.MacAddress));
 			ret = -ENOENT;
@@ -1342,78 +1291,122 @@ exit:
 	return ret;
 }
 
+int cfg80211_infrastructure_mode(struct rtw_adapter* padapter,
+				 enum nl80211_iftype ifmode)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_network *cur_network = &pmlmepriv->cur_network;
+	enum nl80211_iftype old_mode;
+
+	old_mode = cur_network->network.ifmode;
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_notice_,
+		 ("+%s: old =%d new =%d fw_state = 0x%08x\n", __func__,
+		  old_mode, ifmode, get_fwstate(pmlmepriv)));
+
+	if (old_mode != ifmode) {
+		spin_lock_bh(&pmlmepriv->lock);
+
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+			 (" change mode!"));
+
+		if (old_mode == NL80211_IFTYPE_AP ||
+		    old_mode == NL80211_IFTYPE_P2P_GO) {
+			/* change to other mode from Ndis802_11APMode */
+			cur_network->join_res = -1;
+
+#ifdef CONFIG_8723AU_AP_MODE
+			stop_ap_mode23a(padapter);
+#endif
+		}
+
+		if (check_fwstate(pmlmepriv, _FW_LINKED) ||
+		    old_mode == NL80211_IFTYPE_ADHOC)
+			rtw_disassoc_cmd23a(padapter, 0, true);
+
+		if (check_fwstate(pmlmepriv, _FW_LINKED) ||
+		    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE))
+			rtw_free_assoc_resources23a(padapter, 1);
+
+		if (old_mode == NL80211_IFTYPE_STATION ||
+		    old_mode == NL80211_IFTYPE_P2P_CLIENT ||
+		    old_mode == NL80211_IFTYPE_ADHOC) {
+			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+				/* will clr Linked_state; before this function,
+				   we must have chked whether issue
+				   dis-assoc_cmd or not */
+				rtw_indicate_disconnect23a(padapter);
+			}
+	       }
+
+		cur_network->network.ifmode = ifmode;
+
+		_clr_fwstate_(pmlmepriv, ~WIFI_NULL_STATE);
+
+		switch (ifmode) {
+		case NL80211_IFTYPE_ADHOC:
+			set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+			break;
+
+		case NL80211_IFTYPE_P2P_CLIENT:
+		case NL80211_IFTYPE_STATION:
+			set_fwstate(pmlmepriv, WIFI_STATION_STATE);
+			break;
+
+		case NL80211_IFTYPE_P2P_GO:
+		case NL80211_IFTYPE_AP:
+			set_fwstate(pmlmepriv, WIFI_AP_STATE);
+#ifdef CONFIG_8723AU_AP_MODE
+			start_ap_mode23a(padapter);
+			/* rtw_indicate_connect23a(padapter); */
+#endif
+			break;
+
+		default:
+			break;
+		}
+
+		/* SecClearAllKeys(adapter); */
+
+		/* RT_TRACE(COMP_OID_SET, DBG_LOUD,
+		   ("set_infrastructure: fw_state:%x after changing mode\n", */
+		/* get_fwstate(pmlmepriv))); */
+
+		spin_unlock_bh(&pmlmepriv->lock);
+	}
+
+	return _SUCCESS;
+}
+
 static int cfg80211_rtw_change_iface(struct wiphy *wiphy,
 				     struct net_device *ndev,
 				     enum nl80211_iftype type, u32 *flags,
 				     struct vif_params *params)
 {
 	enum nl80211_iftype old_type;
-	enum ndis_802_11_net_infra networkType;
 	struct rtw_adapter *padapter = wiphy_to_adapter(wiphy);
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct wireless_dev *rtw_wdev = wiphy_to_wdev(wiphy);
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif
 	int ret = 0;
-	u8 change = false;
 
-	DBG_8723A(FUNC_NDEV_FMT " call netdev_open23a\n", FUNC_NDEV_ARG(ndev));
-	if (netdev_open23a(ndev) != 0) {
-		ret = -EPERM;
-		goto exit;
-	}
-
-	if (_FAIL == rtw_pwr_wakeup(padapter)) {
-		ret = -EPERM;
-		goto exit;
-	}
+	DBG_8723A("%s(%s): call netdev_open23a\n", __func__, ndev->name);
 
 	old_type = rtw_wdev->iftype;
-	DBG_8723A(FUNC_NDEV_FMT " old_iftype =%d, new_iftype =%d\n",
-		  FUNC_NDEV_ARG(ndev), old_type, type);
+	DBG_8723A("%s(%s): old_iftype =%d, new_iftype =%d\n",
+		  __func__, ndev->name, old_type, type);
 
 	if (old_type != type) {
-		change = true;
 		pmlmeext->action_public_rxseq = 0xffff;
 		pmlmeext->action_public_dialog_token = 0xff;
 	}
 
 	switch (type) {
 	case NL80211_IFTYPE_ADHOC:
-		networkType = Ndis802_11IBSS;
-		break;
 	case NL80211_IFTYPE_P2P_CLIENT:
 	case NL80211_IFTYPE_STATION:
-		networkType = Ndis802_11Infrastructure;
-#ifdef CONFIG_8723AU_P2P
-		if (change && rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
-			del_timer_sync(&pwdinfo->find_phase_timer);
-			del_timer_sync(&pwdinfo->restore_p2p_state_timer);
-			del_timer_sync(&pwdinfo->pre_tx_scan_timer);
-
-			/* it means remove GO and change mode from AP(GO)
-			   to station(P2P DEVICE) */
-			rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-			rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
-
-			DBG_8723A("%s, role =%d, p2p_state =%d, pre_p2p_state ="
-				  "%d\n", __func__, rtw_p2p_role(pwdinfo),
-				  rtw_p2p_state(pwdinfo),
-				  rtw_p2p_pre_state(pwdinfo));
-		}
-#endif /* CONFIG_8723AU_P2P */
-		break;
 	case NL80211_IFTYPE_P2P_GO:
 	case NL80211_IFTYPE_AP:
-		networkType = Ndis802_11APMode;
-#ifdef CONFIG_8723AU_P2P
-		if (change && !rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
-			/* it means P2P Group created, we will be GO
-			   and change mode from  P2P DEVICE to AP(GO) */
-			rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
-		}
-#endif /* CONFIG_8723AU_P2P */
+	case NL80211_IFTYPE_UNSPECIFIED:
 		break;
 	default:
 		return -EOPNOTSUPP;
@@ -1421,13 +1414,13 @@ static int cfg80211_rtw_change_iface(struct wiphy *wiphy,
 
 	rtw_wdev->iftype = type;
 
-	if (rtw_set_802_11_infrastructure_mode23a(padapter, networkType) == false) {
+	if (cfg80211_infrastructure_mode(padapter, type) != _SUCCESS) {
 		rtw_wdev->iftype = old_type;
 		ret = -EPERM;
 		goto exit;
 	}
 
-	rtw_setopmode_cmd23a(padapter, networkType);
+	rtw_setopmode_cmd23a(padapter, type);
 
 exit:
 	return ret;
@@ -1438,9 +1431,8 @@ void rtw_cfg80211_indicate_scan_done(struct rtw_wdev_priv *pwdev_priv,
 {
 	spin_lock_bh(&pwdev_priv->scan_req_lock);
 	if (pwdev_priv->scan_request != NULL) {
-#ifdef CONFIG_DEBUG_CFG80211
 		DBG_8723A("%s with scan req\n", __func__);
-#endif
+
 		if (pwdev_priv->scan_request->wiphy !=
 		    pwdev_priv->rtw_wdev->wiphy)
 			DBG_8723A("error wiphy compare\n");
@@ -1449,9 +1441,7 @@ void rtw_cfg80211_indicate_scan_done(struct rtw_wdev_priv *pwdev_priv,
 
 		pwdev_priv->scan_request = NULL;
 	} else {
-#ifdef CONFIG_DEBUG_CFG80211
 		DBG_8723A("%s without scan req\n", __func__);
-#endif
 	}
 	spin_unlock_bh(&pwdev_priv->scan_req_lock);
 }
@@ -1463,10 +1453,6 @@ void rtw_cfg80211_surveydone_event_callback(struct rtw_adapter *padapter)
 	struct rtw_queue *queue = &pmlmepriv->scanned_queue;
 	struct wlan_network *pnetwork;
 
-#ifdef CONFIG_DEBUG_CFG80211
-	DBG_8723A("%s\n", __func__);
-#endif
-
 	spin_lock_bh(&pmlmepriv->scanned_queue.lock);
 
 	phead = get_list_head(queue);
@@ -1478,7 +1464,7 @@ void rtw_cfg80211_surveydone_event_callback(struct rtw_adapter *padapter)
 		   contains the channel to which this network belongs */
 		if (rtw_ch_set_search_ch23a
 		    (padapter->mlmeextpriv.channel_set,
-		     pnetwork->network.Configuration.DSConfig) >= 0)
+		     pnetwork->network.DSConfig) >= 0)
 			rtw_cfg80211_inform_bss(padapter, pnetwork);
 	}
 
@@ -1495,93 +1481,31 @@ static int rtw_cfg80211_set_probe_req_wpsp2pie(struct rtw_adapter *padapter,
 	int ret = 0;
 	uint wps_ielen = 0;
 	u8 *wps_ie;
-#ifdef CONFIG_8723AU_P2P
-	u32 p2p_ielen = 0;
-	u8 *p2p_ie;
-	u32 wfd_ielen = 0;
-#endif
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-#ifdef CONFIG_DEBUG_CFG80211
 	DBG_8723A("%s, ielen =%d\n", __func__, len);
-#endif
 
 	if (len > 0) {
 		wps_ie = rtw_get_wps_ie23a(buf, len, NULL, &wps_ielen);
 		if (wps_ie) {
-#ifdef CONFIG_DEBUG_CFG80211
 			DBG_8723A("probe_req_wps_ielen =%d\n", wps_ielen);
-#endif
+
 			if (pmlmepriv->wps_probe_req_ie) {
 				pmlmepriv->wps_probe_req_ie_len = 0;
 				kfree(pmlmepriv->wps_probe_req_ie);
 				pmlmepriv->wps_probe_req_ie = NULL;
 			}
 
-			pmlmepriv->wps_probe_req_ie =
-				kmalloc(wps_ielen, GFP_KERNEL);
+			pmlmepriv->wps_probe_req_ie = kmemdup(wps_ie,
+							      wps_ielen,
+							      GFP_KERNEL);
 			if (pmlmepriv->wps_probe_req_ie == NULL) {
 				DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
 					  __func__, __LINE__);
 				return -EINVAL;
 			}
-			memcpy(pmlmepriv->wps_probe_req_ie, wps_ie, wps_ielen);
 			pmlmepriv->wps_probe_req_ie_len = wps_ielen;
 		}
-#ifdef CONFIG_8723AU_P2P
-		p2p_ie = rtw_get_p2p_ie23a(buf, len, NULL, &p2p_ielen);
-		if (p2p_ie) {
-#ifdef CONFIG_DEBUG_CFG80211
-			DBG_8723A("probe_req_p2p_ielen =%d\n", p2p_ielen);
-#endif
-
-			if (pmlmepriv->p2p_probe_req_ie) {
-				pmlmepriv->p2p_probe_req_ie_len = 0;
-				kfree(pmlmepriv->p2p_probe_req_ie);
-				pmlmepriv->p2p_probe_req_ie = NULL;
-			}
-
-			pmlmepriv->p2p_probe_req_ie =
-				kmalloc(p2p_ielen, GFP_KERNEL);
-			if (pmlmepriv->p2p_probe_req_ie == NULL) {
-				DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-					  __func__, __LINE__);
-				return -EINVAL;
-
-			}
-			memcpy(pmlmepriv->p2p_probe_req_ie, p2p_ie, p2p_ielen);
-			pmlmepriv->p2p_probe_req_ie_len = p2p_ielen;
-		}
-#endif /* CONFIG_8723AU_P2P */
-
-		/* buf += p2p_ielen; */
-		/* len -= p2p_ielen; */
-
-#ifdef CONFIG_8723AU_P2P
-		if (rtw_get_wfd_ie(buf, len, NULL, &wfd_ielen)) {
-#ifdef CONFIG_DEBUG_CFG80211
-			DBG_8723A("probe_req_wfd_ielen =%d\n", wfd_ielen);
-#endif
-
-			if (pmlmepriv->wfd_probe_req_ie) {
-				pmlmepriv->wfd_probe_req_ie_len = 0;
-				kfree(pmlmepriv->wfd_probe_req_ie);
-				pmlmepriv->wfd_probe_req_ie = NULL;
-			}
-
-			pmlmepriv->wfd_probe_req_ie =
-				kmalloc(wfd_ielen, GFP_KERNEL);
-			if (pmlmepriv->wfd_probe_req_ie == NULL) {
-				DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-					  __func__, __LINE__);
-				return -EINVAL;
-
-			}
-			rtw_get_wfd_ie(buf, len, pmlmepriv->wfd_probe_req_ie,
-				       &pmlmepriv->wfd_probe_req_ie_len);
-		}
-#endif /* CONFIG_8723AU_P2P */
-
 	}
 
 	return ret;
@@ -1599,25 +1523,16 @@ static int cfg80211_rtw_scan(struct wiphy *wiphy,
 	struct rtw_ieee80211_channel ch[RTW_CHANNEL_SCAN_AMOUNT];
 	struct rtw_wdev_priv *pwdev_priv = wdev_to_priv(padapter->rtw_wdev);
 	struct cfg80211_ssid *ssids = request->ssids;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	int social_channel = 0;
-#endif /* CONFIG_8723AU_P2P */
 	bool need_indicate_scan_done = false;
 
-#ifdef CONFIG_DEBUG_CFG80211
-	DBG_8723A(FUNC_ADPT_FMT "\n", FUNC_ADPT_ARG(padapter));
-#endif
+	DBG_8723A("%s(%s)\n", __func__, padapter->pnetdev->name);
 
 	spin_lock_bh(&pwdev_priv->scan_req_lock);
 	pwdev_priv->scan_request = request;
 	spin_unlock_bh(&pwdev_priv->scan_req_lock);
 
 	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
-
-#ifdef CONFIG_DEBUG_CFG80211
 		DBG_8723A("%s under WIFI_AP_STATE\n", __func__);
-#endif
 		/* need_indicate_scan_done = true; */
 		/* goto check_need_indicate_scan_done; */
 	}
@@ -1626,29 +1541,6 @@ static int cfg80211_rtw_scan(struct wiphy *wiphy,
 		need_indicate_scan_done = true;
 		goto check_need_indicate_scan_done;
 	}
-#ifdef CONFIG_8723AU_P2P
-	if (!memcmp(ssids->ssid, "DIRECT-", 7) &&
-	    rtw_get_p2p_ie23a((u8 *) request->ie, request->ie_len, NULL, NULL)) {
-		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
-			rtw_p2p_enable23a(padapter, P2P_ROLE_DEVICE);
-			wdev_to_priv(padapter->rtw_wdev)->p2p_enabled = true;
-		} else {
-			rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
-#ifdef CONFIG_DEBUG_CFG80211
-			DBG_8723A("%s, role =%d, p2p_state =%d\n", __func__,
-				  rtw_p2p_role(pwdinfo),
-				  rtw_p2p_state(pwdinfo));
-#endif
-		}
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_LISTEN);
-
-		if (request->n_channels == 3 &&
-		    request->channels[0]->hw_value == 1 &&
-		    request->channels[1]->hw_value == 6 &&
-		    request->channels[2]->hw_value == 11)
-			social_channel = 1;
-	}
-#endif /* CONFIG_8723AU_P2P */
 
 	if (request->ie && request->ie_len > 0) {
 		rtw_cfg80211_set_probe_req_wpsp2pie(padapter,
@@ -1662,8 +1554,8 @@ static int cfg80211_rtw_scan(struct wiphy *wiphy,
 		goto check_need_indicate_scan_done;
 	}
 	if (rtw_is_scan_deny(padapter)) {
-		DBG_8723A(FUNC_ADPT_FMT ": scan deny\n",
-			  FUNC_ADPT_ARG(padapter));
+		DBG_8723A("%s(%s): scan deny\n", __func__,
+			  padapter->pnetdev->name);
 		need_indicate_scan_done = true;
 		goto check_need_indicate_scan_done;
 	}
@@ -1674,28 +1566,12 @@ static int cfg80211_rtw_scan(struct wiphy *wiphy,
 		need_indicate_scan_done = true;
 		goto check_need_indicate_scan_done;
 	}
-#ifdef CONFIG_8723AU_P2P
-	if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) &&
-	    !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE)) {
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH);
-		rtw_free_network_queue23a(padapter, true);
-
-		if (social_channel == 0)
-			rtw_p2p_findphase_ex_set(pwdinfo,
-						 P2P_FINDPHASE_EX_NONE);
-		else
-			rtw_p2p_findphase_ex_set(pwdinfo,
-						 P2P_FINDPHASE_EX_SOCIAL_LAST);
-	}
-#endif /* CONFIG_8723AU_P2P */
 
 	memset(ssid, 0, sizeof(struct cfg80211_ssid) * RTW_SSID_SCAN_AMOUNT);
 	/* parsing request ssids, n_ssids */
 	for (i = 0; i < request->n_ssids && i < RTW_SSID_SCAN_AMOUNT; i++) {
-#ifdef CONFIG_DEBUG_CFG80211
 		DBG_8723A("ssid =%s, len =%d\n", ssids[i].ssid,
 			  ssids[i].ssid_len);
-#endif
 		memcpy(ssid[i].ssid, ssids[i].ssid, ssids[i].ssid_len);
 		ssid[i].ssid_len = ssids[i].ssid_len;
 	}
@@ -1707,11 +1583,9 @@ static int cfg80211_rtw_scan(struct wiphy *wiphy,
 	if (request->n_channels == 1) {
 		for (i = 0; i < request->n_channels &&
 		     i < RTW_CHANNEL_SCAN_AMOUNT; i++) {
-#ifdef CONFIG_DEBUG_CFG80211
-			DBG_8723A(FUNC_ADPT_FMT CHAN_FMT "\n",
-				  FUNC_ADPT_ARG(padapter),
+			DBG_8723A("%s:(%s):" CHAN_FMT "\n",
+				  __func__, padapter->pnetdev->name,
 				  CHAN_ARG(request->channels[i]));
-#endif
 			ch[i].hw_value = request->channels[i]->hw_value;
 			ch[i].flags = request->channels[i]->flags;
 		}
@@ -1747,13 +1621,13 @@ static int cfg80211_rtw_set_wiphy_params(struct wiphy *wiphy, u32 changed)
 static int cfg80211_rtw_join_ibss(struct wiphy *wiphy, struct net_device *ndev,
 				  struct cfg80211_ibss_params *params)
 {
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 	return 0;
 }
 
 static int cfg80211_rtw_leave_ibss(struct wiphy *wiphy, struct net_device *ndev)
 {
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 	return 0;
 }
 
@@ -1822,30 +1696,30 @@ static int rtw_cfg80211_set_cipher(struct security_priv *psecuritypriv,
 	DBG_8723A("%s, ucast =%d, cipher = 0x%x\n", __func__, ucast, cipher);
 
 	if (!cipher) {
-		*profile_cipher = _NO_PRIVACY_;
+		*profile_cipher = 0;
 		psecuritypriv->ndisencryptstatus = ndisencryptstatus;
 		return 0;
 	}
 
 	switch (cipher) {
 	case IW_AUTH_CIPHER_NONE:
-		*profile_cipher = _NO_PRIVACY_;
+		*profile_cipher = 0;
 		ndisencryptstatus = Ndis802_11EncryptionDisabled;
 		break;
 	case WLAN_CIPHER_SUITE_WEP40:
-		*profile_cipher = _WEP40_;
+		*profile_cipher = WLAN_CIPHER_SUITE_WEP40;
 		ndisencryptstatus = Ndis802_11Encryption1Enabled;
 		break;
 	case WLAN_CIPHER_SUITE_WEP104:
-		*profile_cipher = _WEP104_;
+		*profile_cipher = WLAN_CIPHER_SUITE_WEP104;
 		ndisencryptstatus = Ndis802_11Encryption1Enabled;
 		break;
 	case WLAN_CIPHER_SUITE_TKIP:
-		*profile_cipher = _TKIP_;
+		*profile_cipher = WLAN_CIPHER_SUITE_TKIP;
 		ndisencryptstatus = Ndis802_11Encryption2Enabled;
 		break;
 	case WLAN_CIPHER_SUITE_CCMP:
-		*profile_cipher = _AES_;
+		*profile_cipher = WLAN_CIPHER_SUITE_CCMP;
 		ndisencryptstatus = Ndis802_11Encryption3Enabled;
 		break;
 	default:
@@ -1877,13 +1751,10 @@ static int rtw_cfg80211_set_key_mgt(struct security_priv *psecuritypriv,
 static int rtw_cfg80211_set_wpa_ie(struct rtw_adapter *padapter, const u8 *pie,
 				   size_t ielen)
 {
-	u8 *buf = NULL, *pos = NULL;
+	u8 *buf = NULL;
 	int group_cipher = 0, pairwise_cipher = 0;
 	int ret = 0;
-	int wpa_ielen = 0;
-	int wpa2_ielen = 0;
-	u8 *pwpa, *pwpa2;
-	u8 null_addr[] = { 0, 0, 0, 0, 0, 0 };
+	const u8 *pwpa, *pwpa2;
 	int i;
 
 	if (!pie || !ielen) {
@@ -1896,12 +1767,11 @@ static int rtw_cfg80211_set_wpa_ie(struct rtw_adapter *padapter, const u8 *pie,
 		ret = -EINVAL;
 		goto exit;
 	}
-	buf = kzalloc(ielen, GFP_KERNEL);
+	buf = kmemdup(pie, ielen, GFP_KERNEL);
 	if (buf == NULL) {
 		ret = -ENOMEM;
 		goto exit;
 	}
-	memcpy(buf, pie, ielen);
 
 	/* dump */
 	DBG_8723A("set wpa_ie(length:%zu):\n", ielen);
@@ -1910,7 +1780,6 @@ static int rtw_cfg80211_set_wpa_ie(struct rtw_adapter *padapter, const u8 *pie,
 			  buf[i], buf[i + 1],
 			  buf[i + 2], buf[i + 3], buf[i + 4],
 			  buf[i + 5], buf[i + 6], buf[i + 7]);
-	pos = buf;
 	if (ielen < RSN_HEADER_LEN) {
 		RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_err_,
 			 ("Ie len too short %d\n", (int)ielen));
@@ -1918,33 +1787,35 @@ static int rtw_cfg80211_set_wpa_ie(struct rtw_adapter *padapter, const u8 *pie,
 		goto exit;
 	}
 
-	pwpa = rtw_get_wpa_ie23a(buf, &wpa_ielen, ielen);
-	if (pwpa && wpa_ielen > 0) {
-		if (rtw_parse_wpa_ie23a(pwpa, wpa_ielen + 2, &group_cipher,
-				     &pairwise_cipher, NULL) == _SUCCESS) {
+	pwpa = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
+				       WLAN_OUI_TYPE_MICROSOFT_WPA,
+				       buf, ielen);
+	if (pwpa && pwpa[1] > 0) {
+		if (rtw_parse_wpa_ie23a(pwpa, pwpa[1] + 2, &group_cipher,
+					&pairwise_cipher, NULL) == _SUCCESS) {
 			padapter->securitypriv.dot11AuthAlgrthm =
 				dot11AuthAlgrthm_8021X;
 			padapter->securitypriv.ndisauthtype =
 				Ndis802_11AuthModeWPAPSK;
-			memcpy(padapter->securitypriv.supplicant_ie, &pwpa[0],
-			       wpa_ielen + 2);
+			memcpy(padapter->securitypriv.supplicant_ie, pwpa,
+			       pwpa[1] + 2);
 
-			DBG_8723A("got wpa_ie, wpa_ielen:%u\n", wpa_ielen);
+			DBG_8723A("got wpa_ie, wpa_ielen:%u\n", pwpa[1]);
 		}
 	}
 
-	pwpa2 = rtw_get_wpa2_ie23a(buf, &wpa2_ielen, ielen);
-	if (pwpa2 && wpa2_ielen > 0) {
-		if (rtw_parse_wpa2_ie23a (pwpa2, wpa2_ielen + 2, &group_cipher,
-				       &pairwise_cipher, NULL) == _SUCCESS) {
+	pwpa2 = cfg80211_find_ie(WLAN_EID_RSN, buf, ielen);
+	if (pwpa2 && pwpa2[1] > 0) {
+		if (rtw_parse_wpa2_ie23a (pwpa2, pwpa2[1] + 2, &group_cipher,
+					  &pairwise_cipher, NULL) == _SUCCESS) {
 			padapter->securitypriv.dot11AuthAlgrthm =
 				dot11AuthAlgrthm_8021X;
 			padapter->securitypriv.ndisauthtype =
 				Ndis802_11AuthModeWPA2PSK;
-			memcpy(padapter->securitypriv.supplicant_ie, &pwpa2[0],
-			       wpa2_ielen + 2);
+			memcpy(padapter->securitypriv.supplicant_ie, pwpa2,
+			       pwpa2[1] + 2);
 
-			DBG_8723A("got wpa2_ie, wpa2_ielen:%u\n", wpa2_ielen);
+			DBG_8723A("got wpa2_ie, wpa2_ielen:%u\n", pwpa2[1]);
 		}
 	}
 
@@ -1957,27 +1828,27 @@ static int rtw_cfg80211_set_wpa_ie(struct rtw_adapter *padapter, const u8 *pie,
 
 	switch (group_cipher) {
 	case WPA_CIPHER_NONE:
-		padapter->securitypriv.dot118021XGrpPrivacy = _NO_PRIVACY_;
+		padapter->securitypriv.dot118021XGrpPrivacy = 0;
 		padapter->securitypriv.ndisencryptstatus =
 			Ndis802_11EncryptionDisabled;
 		break;
 	case WPA_CIPHER_WEP40:
-		padapter->securitypriv.dot118021XGrpPrivacy = _WEP40_;
+		padapter->securitypriv.dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_WEP40;
 		padapter->securitypriv.ndisencryptstatus =
 			Ndis802_11Encryption1Enabled;
 		break;
 	case WPA_CIPHER_TKIP:
-		padapter->securitypriv.dot118021XGrpPrivacy = _TKIP_;
+		padapter->securitypriv.dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_TKIP;
 		padapter->securitypriv.ndisencryptstatus =
 			Ndis802_11Encryption2Enabled;
 		break;
 	case WPA_CIPHER_CCMP:
-		padapter->securitypriv.dot118021XGrpPrivacy = _AES_;
+		padapter->securitypriv.dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_CCMP;
 		padapter->securitypriv.ndisencryptstatus =
 			Ndis802_11Encryption3Enabled;
 		break;
 	case WPA_CIPHER_WEP104:
-		padapter->securitypriv.dot118021XGrpPrivacy = _WEP104_;
+		padapter->securitypriv.dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_WEP104;
 		padapter->securitypriv.ndisencryptstatus =
 			Ndis802_11Encryption1Enabled;
 		break;
@@ -1985,27 +1856,27 @@ static int rtw_cfg80211_set_wpa_ie(struct rtw_adapter *padapter, const u8 *pie,
 
 	switch (pairwise_cipher) {
 	case WPA_CIPHER_NONE:
-		padapter->securitypriv.dot11PrivacyAlgrthm = _NO_PRIVACY_;
+		padapter->securitypriv.dot11PrivacyAlgrthm = 0;
 		padapter->securitypriv.ndisencryptstatus =
 			Ndis802_11EncryptionDisabled;
 		break;
 	case WPA_CIPHER_WEP40:
-		padapter->securitypriv.dot11PrivacyAlgrthm = _WEP40_;
+		padapter->securitypriv.dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_WEP40;
 		padapter->securitypriv.ndisencryptstatus =
 			Ndis802_11Encryption1Enabled;
 		break;
 	case WPA_CIPHER_TKIP:
-		padapter->securitypriv.dot11PrivacyAlgrthm = _TKIP_;
+		padapter->securitypriv.dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_TKIP;
 		padapter->securitypriv.ndisencryptstatus =
 			Ndis802_11Encryption2Enabled;
 		break;
 	case WPA_CIPHER_CCMP:
-		padapter->securitypriv.dot11PrivacyAlgrthm = _AES_;
+		padapter->securitypriv.dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_CCMP;
 		padapter->securitypriv.ndisencryptstatus =
 			Ndis802_11Encryption3Enabled;
 		break;
 	case WPA_CIPHER_WEP104:
-		padapter->securitypriv.dot11PrivacyAlgrthm = _WEP104_;
+		padapter->securitypriv.dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_WEP104;
 		padapter->securitypriv.ndisencryptstatus =
 			Ndis802_11Encryption1Enabled;
 		break;
@@ -2029,75 +1900,14 @@ static int rtw_cfg80211_set_wpa_ie(struct rtw_adapter *padapter, const u8 *pie,
 		}
 	}
 
-#ifdef CONFIG_8723AU_P2P
-	{			/* check p2p_ie for assoc req; */
-		uint p2p_ielen = 0;
-		u8 *p2p_ie;
-		struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-
-		p2p_ie = rtw_get_p2p_ie23a(buf, ielen, NULL, &p2p_ielen);
-		if (p2p_ie) {
-#ifdef CONFIG_DEBUG_CFG80211
-			DBG_8723A("%s p2p_assoc_req_ielen =%d\n", __func__,
-				  p2p_ielen);
-#endif
-
-			if (pmlmepriv->p2p_assoc_req_ie) {
-				pmlmepriv->p2p_assoc_req_ie_len = 0;
-				kfree(pmlmepriv->p2p_assoc_req_ie);
-				pmlmepriv->p2p_assoc_req_ie = NULL;
-			}
-
-			pmlmepriv->p2p_assoc_req_ie =
-				kmalloc(p2p_ielen, GFP_KERNEL);
-			if (pmlmepriv->p2p_assoc_req_ie == NULL) {
-				DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-					  __func__, __LINE__);
-				goto exit;
-			}
-			memcpy(pmlmepriv->p2p_assoc_req_ie, p2p_ie, p2p_ielen);
-			pmlmepriv->p2p_assoc_req_ie_len = p2p_ielen;
-		}
-	}
-#endif /* CONFIG_8723AU_P2P */
-
-#ifdef CONFIG_8723AU_P2P
-	{			/* check wfd_ie for assoc req; */
-		uint wfd_ielen = 0;
-		struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-
-		if (rtw_get_wfd_ie(buf, ielen, NULL, &wfd_ielen)) {
-#ifdef CONFIG_DEBUG_CFG80211
-			DBG_8723A("%s wfd_assoc_req_ielen =%d\n", __func__,
-				  wfd_ielen);
-#endif
-
-			if (pmlmepriv->wfd_assoc_req_ie) {
-				pmlmepriv->wfd_assoc_req_ie_len = 0;
-				kfree(pmlmepriv->wfd_assoc_req_ie);
-				pmlmepriv->wfd_assoc_req_ie = NULL;
-			}
-
-			pmlmepriv->wfd_assoc_req_ie =
-				kmalloc(wfd_ielen, GFP_KERNEL);
-			if (pmlmepriv->wfd_assoc_req_ie == NULL) {
-				DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-					  __func__, __LINE__);
-				goto exit;
-			}
-			rtw_get_wfd_ie(buf, ielen, pmlmepriv->wfd_assoc_req_ie,
-				       &pmlmepriv->wfd_assoc_req_ie_len);
-		}
-	}
-#endif /* CONFIG_8723AU_P2P */
-
 	/* TKIP and AES disallow multicast packets until installing group key */
-	if (padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_ ||
-	    padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_WTMIC_ ||
-	    padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)
+	if (padapter->securitypriv.dot11PrivacyAlgrthm ==
+	    WLAN_CIPHER_SUITE_TKIP ||
+	    padapter->securitypriv.dot11PrivacyAlgrthm ==
+	    WLAN_CIPHER_SUITE_CCMP)
 		/* WPS open need to enable multicast */
 		/* check_fwstate(&padapter->mlmepriv, WIFI_UNDER_WPS) == true)*/
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_OFF_RCR_AM, null_addr);
+		rtl8723a_off_rcr_am(padapter);
 
 	RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_info_,
 		 ("rtw_set_wpa_ie: pairwise_cipher = 0x%08x padapter->"
@@ -2113,6 +1923,70 @@ exit:
 	return ret;
 }
 
+static int rtw_cfg80211_add_wep(struct rtw_adapter *padapter,
+				struct rtw_wep_key *wep, u8 keyid)
+{
+	int res;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	if (keyid >= NUM_WEP_KEYS) {
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+			 ("%s:keyid>4 =>fail\n", __func__));
+		res = _FAIL;
+		goto exit;
+	}
+
+	switch (wep->keylen) {
+	case 5:
+		psecuritypriv->dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_WEP40;
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+			 ("%s:wep->KeyLength = 5\n", __func__));
+		break;
+	case 13:
+		psecuritypriv->dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_WEP104;
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+			 ("%s:wep->KeyLength = 13\n", __func__));
+		break;
+	default:
+		psecuritypriv->dot11PrivacyAlgrthm = 0;
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+			 ("%s:wep->KeyLength!= 5 or 13\n", __func__));
+		res = _FAIL;
+		goto exit;
+	}
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+		 ("%s:before memcpy, wep->KeyLength = 0x%x keyid =%x\n",
+		  __func__, wep->keylen, keyid));
+
+	memcpy(&psecuritypriv->wep_key[keyid], wep, sizeof(struct rtw_wep_key));
+
+	psecuritypriv->dot11PrivacyKeyIndex = keyid;
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+		 ("%s:security key material : "
+		  "%x %x %x %x %x %x %x %x %x %x %x %x %x\n", __func__,
+		  psecuritypriv->wep_key[keyid].key[0],
+		  psecuritypriv->wep_key[keyid].key[1],
+		  psecuritypriv->wep_key[keyid].key[2],
+		  psecuritypriv->wep_key[keyid].key[3],
+		  psecuritypriv->wep_key[keyid].key[4],
+		  psecuritypriv->wep_key[keyid].key[5],
+		  psecuritypriv->wep_key[keyid].key[6],
+		  psecuritypriv->wep_key[keyid].key[7],
+		  psecuritypriv->wep_key[keyid].key[8],
+		  psecuritypriv->wep_key[keyid].key[9],
+		  psecuritypriv->wep_key[keyid].key[10],
+		  psecuritypriv->wep_key[keyid].key[11],
+		  psecuritypriv->wep_key[keyid].key[12]));
+
+	res = rtw_set_key23a(padapter, psecuritypriv, keyid, 1);
+
+exit:
+
+	return res;
+}
+
 static int cfg80211_rtw_connect(struct wiphy *wiphy, struct net_device *ndev,
 				struct cfg80211_connect_params *sme)
 {
@@ -2133,16 +2007,10 @@ static int cfg80211_rtw_connect(struct wiphy *wiphy, struct net_device *ndev,
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct rtw_queue *queue = &pmlmepriv->scanned_queue;
 
-	DBG_8723A("=>" FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("=>" "%s(%s)\n", __func__, ndev->name);
 	DBG_8723A("privacy =%d, key =%p, key_len =%d, key_idx =%d\n",
 		  sme->privacy, sme->key, sme->key_len, sme->key_idx);
 
-	if (wdev_to_priv(padapter->rtw_wdev)->block) {
-		ret = -EBUSY;
-		DBG_8723A("%s wdev_priv.block is set\n", __func__);
-		goto exit;
-	}
-
 	if (_FAIL == rtw_pwr_wakeup(padapter)) {
 		ret = -EPERM;
 		goto exit;
@@ -2193,8 +2061,8 @@ static int cfg80211_rtw_connect(struct wiphy *wiphy, struct net_device *ndev,
 		dst_bssid = pnetwork->network.MacAddress;
 
 		if (sme->bssid) {
-			if (memcmp(pnetwork->network.MacAddress,
-				   sme->bssid, ETH_ALEN))
+			if (!ether_addr_equal(pnetwork->network.MacAddress,
+					      sme->bssid))
 				continue;
 		}
 
@@ -2208,7 +2076,7 @@ static int cfg80211_rtw_connect(struct wiphy *wiphy, struct net_device *ndev,
 		if (sme->bssid) {
 			src_bssid = sme->bssid;
 
-			if ((!memcmp(dst_bssid, src_bssid, ETH_ALEN))) {
+			if (ether_addr_equal(dst_bssid, src_bssid)) {
 				DBG_8723A("matched by bssid\n");
 
 				ndis_ssid.ssid_len =
@@ -2242,15 +2110,15 @@ static int cfg80211_rtw_connect(struct wiphy *wiphy, struct net_device *ndev,
 		goto exit;
 	}
 
-	if (rtw_set_802_11_infrastructure_mode23a
-	    (padapter, pnetwork->network.InfrastructureMode) == false) {
+	if (cfg80211_infrastructure_mode(
+		    padapter, pnetwork->network.ifmode) != _SUCCESS) {
 		ret = -EPERM;
 		goto exit;
 	}
 
 	psecuritypriv->ndisencryptstatus = Ndis802_11EncryptionDisabled;
-	psecuritypriv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
-	psecuritypriv->dot118021XGrpPrivacy = _NO_PRIVACY_;
+	psecuritypriv->dot11PrivacyAlgrthm = 0;
+	psecuritypriv->dot118021XGrpPrivacy = 0;
 	psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_Open;
 	psecuritypriv->ndisauthtype = Ndis802_11AuthModeOpen;
 
@@ -2283,58 +2151,42 @@ static int cfg80211_rtw_connect(struct wiphy *wiphy, struct net_device *ndev,
 	if ((psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_Shared ||
 	     psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_Auto) &&
 	    sme->key) {
-		u32 wep_key_idx, wep_key_len, wep_total_len;
-		struct ndis_802_11_wep *pwep = NULL;
+		struct rtw_wep_key wep_key;
+		u8 wep_key_idx, wep_key_len;
 		DBG_8723A("%s(): Shared/Auto WEP\n", __func__);
 
 		wep_key_idx = sme->key_idx;
 		wep_key_len = sme->key_len;
 
-		if (sme->key_idx > WEP_KEYS) {
+		if (wep_key_idx > WEP_KEYS || !wep_key_len ||
+		    wep_key_len > WLAN_KEY_LEN_WEP104) {
 			ret = -EINVAL;
 			goto exit;
 		}
 
-		if (wep_key_len > 0) {
-			wep_key_len = wep_key_len <= 5 ? 5 : 13;
-			wep_total_len =
-				wep_key_len +
-				offsetof(struct ndis_802_11_wep, KeyMaterial);
-			pwep = (struct ndis_802_11_wep *)kmalloc(wep_total_len,
-								 GFP_KERNEL);
-			if (pwep == NULL) {
-				DBG_8723A(" wpa_set_encryption: pwep "
-					  "allocate fail !!!\n");
-				ret = -ENOMEM;
-				goto exit;
-			}
+		wep_key_len = wep_key_len <= 5 ? 5 : 13;
 
-			memset(pwep, 0, wep_total_len);
+		memset(&wep_key, 0, sizeof(struct rtw_wep_key));
 
-			pwep->KeyLength = wep_key_len;
-			pwep->Length = wep_total_len;
+		wep_key.keylen = wep_key_len;
 
-			if (wep_key_len == 13) {
-				padapter->securitypriv.dot11PrivacyAlgrthm =
-				    _WEP104_;
-				padapter->securitypriv.dot118021XGrpPrivacy =
-				    _WEP104_;
-			}
+		if (wep_key_len == 13) {
+			padapter->securitypriv.dot11PrivacyAlgrthm =
+				WLAN_CIPHER_SUITE_WEP104;
+			padapter->securitypriv.dot118021XGrpPrivacy =
+				WLAN_CIPHER_SUITE_WEP104;
 		} else {
-			ret = -EINVAL;
-			goto exit;
+			padapter->securitypriv.dot11PrivacyAlgrthm =
+				WLAN_CIPHER_SUITE_WEP40;
+			padapter->securitypriv.dot118021XGrpPrivacy =
+				WLAN_CIPHER_SUITE_WEP40;
 		}
 
-		pwep->KeyIndex = wep_key_idx;
-		pwep->KeyIndex |= 0x80000000;
+		memcpy(wep_key.key, (void *)sme->key, wep_key.keylen);
 
-		memcpy(pwep->KeyMaterial, (void *)sme->key, pwep->KeyLength);
-
-		if (rtw_set_802_11_add_wep23a(padapter, pwep) == (u8) _FAIL) {
+		if (rtw_cfg80211_add_wep(padapter, &wep_key, wep_key_idx) !=
+		    _SUCCESS)
 			ret = -EOPNOTSUPP;
-		}
-
-		kfree(pwep);
 
 		if (ret < 0)
 			goto exit;
@@ -2380,7 +2232,7 @@ static int cfg80211_rtw_disconnect(struct wiphy *wiphy, struct net_device *ndev,
 {
 	struct rtw_adapter *padapter = wiphy_to_adapter(wiphy);
 
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 
 	rtw_set_roaming(padapter, 0);
 
@@ -2430,8 +2282,8 @@ static int cfg80211_rtw_set_power_mgmt(struct wiphy *wiphy,
 	struct rtw_adapter *padapter = wiphy_to_adapter(wiphy);
 	struct rtw_wdev_priv *rtw_wdev_priv = wdev_to_priv(padapter->rtw_wdev);
 
-	DBG_8723A(FUNC_NDEV_FMT " enabled:%u, timeout:%d\n",
-		  FUNC_NDEV_ARG(ndev), enabled, timeout);
+	DBG_8723A("%s(%s): enabled:%u, timeout:%d\n",
+		  __func__, ndev->name, enabled, timeout);
 
 	rtw_wdev_priv->power_mgmt = enabled;
 
@@ -2448,25 +2300,22 @@ static int cfg80211_rtw_set_pmksa(struct wiphy *wiphy,
 	u8 index, blInserted = false;
 	struct rtw_adapter *padapter = wiphy_to_adapter(wiphy);
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
-	u8 strZeroMacAddress[ETH_ALEN] = { 0x00 };
 
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(netdev));
+	DBG_8723A("%s(%s)\n", __func__, netdev->name);
 
-	if (!memcmp(pmksa->bssid, strZeroMacAddress, ETH_ALEN)) {
+	if (is_zero_ether_addr(pmksa->bssid))
 		return -EINVAL;
-	}
 
 	blInserted = false;
 
 	/* overwrite PMKID */
 	for (index = 0; index < NUM_PMKID_CACHE; index++) {
-		if (!memcmp(psecuritypriv->PMKIDList[index].Bssid,
-			    pmksa->bssid, ETH_ALEN)) {
+		if (ether_addr_equal(psecuritypriv->PMKIDList[index].Bssid,
+				     pmksa->bssid)) {
 			/* BSSID is matched, the same AP => rewrite with
 			   new PMKID. */
-			DBG_8723A(FUNC_NDEV_FMT
-				  " BSSID exists in the PMKList.\n",
-				  FUNC_NDEV_ARG(netdev));
+			DBG_8723A("%s(%s):  BSSID exists in the PMKList.\n",
+				  __func__, netdev->name);
 
 			memcpy(psecuritypriv->PMKIDList[index].PMKID,
 			       pmksa->pmkid, WLAN_PMKID_LEN);
@@ -2479,12 +2328,12 @@ static int cfg80211_rtw_set_pmksa(struct wiphy *wiphy,
 
 	if (!blInserted) {
 		/*  Find a new entry */
-		DBG_8723A(FUNC_NDEV_FMT
-			  " Use the new entry index = %d for this PMKID.\n",
-			  FUNC_NDEV_ARG(netdev), psecuritypriv->PMKIDIndex);
+		DBG_8723A("%s(%s): Use new entry index = %d for this PMKID\n",
+			  __func__, netdev->name, psecuritypriv->PMKIDIndex);
 
-		memcpy(psecuritypriv->PMKIDList[psecuritypriv->PMKIDIndex].
-		       Bssid, pmksa->bssid, ETH_ALEN);
+		ether_addr_copy(
+			psecuritypriv->PMKIDList[psecuritypriv->PMKIDIndex].
+			Bssid, pmksa->bssid);
 		memcpy(psecuritypriv->PMKIDList[psecuritypriv->PMKIDIndex].
 		       PMKID, pmksa->pmkid, WLAN_PMKID_LEN);
 
@@ -2507,14 +2356,14 @@ static int cfg80211_rtw_del_pmksa(struct wiphy *wiphy,
 	struct rtw_adapter *padapter = wiphy_to_adapter(wiphy);
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(netdev));
+	DBG_8723A("%s(%s)\n", __func__, netdev->name);
 
 	for (index = 0; index < NUM_PMKID_CACHE; index++) {
-		if (!memcmp(psecuritypriv->PMKIDList[index].Bssid,
-			    pmksa->bssid, ETH_ALEN)) {
-			/* BSSID is matched, the same AP => Remove this PMKID information and reset it. */
-			memset(psecuritypriv->PMKIDList[index].Bssid, 0x00,
-			       ETH_ALEN);
+		if (ether_addr_equal(psecuritypriv->PMKIDList[index].Bssid,
+				     pmksa->bssid)) {
+			/* BSSID is matched, the same AP => Remove this PMKID
+			   information and reset it. */
+			eth_zero_addr(psecuritypriv->PMKIDList[index].Bssid);
 			memset(psecuritypriv->PMKIDList[index].PMKID, 0x00,
 			       WLAN_PMKID_LEN);
 			psecuritypriv->PMKIDList[index].bUsed = false;
@@ -2524,8 +2373,8 @@ static int cfg80211_rtw_del_pmksa(struct wiphy *wiphy,
 	}
 
 	if (false == bMatched) {
-		DBG_8723A(FUNC_NDEV_FMT " do not have matched BSSID\n",
-			  FUNC_NDEV_ARG(netdev));
+		DBG_8723A("%s(%s): do not have matched BSSID\n", __func__,
+			  netdev->name);
 		return -EINVAL;
 	}
 
@@ -2538,7 +2387,7 @@ static int cfg80211_rtw_flush_pmksa(struct wiphy *wiphy,
 	struct rtw_adapter *padapter = wiphy_to_adapter(wiphy);
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(netdev));
+	DBG_8723A("%s(%s)\n", __func__, netdev->name);
 
 	memset(&psecuritypriv->PMKIDList[0], 0x00,
 	       sizeof(struct rt_pmkid_list) * NUM_PMKID_CACHE);
@@ -2583,8 +2432,8 @@ void rtw_cfg80211_indicate_sta_assoc(struct rtw_adapter *padapter,
 		freq = ieee80211_channel_to_frequency(channel,
 						      IEEE80211_BAND_5GHZ);
 
-	rtw_cfg80211_rx_mgmt(padapter, freq, 0, pmgmt_frame, frame_len,
-			     GFP_ATOMIC);
+	cfg80211_rx_mgmt(padapter->rtw_wdev, freq, 0, pmgmt_frame, frame_len,
+			 0, GFP_ATOMIC);
 #endif /* defined(RTW_USE_CFG80211_STA_EVENT) */
 }
 
@@ -2597,14 +2446,15 @@ void rtw_cfg80211_indicate_sta_disassoc(struct rtw_adapter *padapter,
 	u8 *pmgmt_frame;
 	uint frame_len;
 	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
-	u8 mgmt_buf[128] = { 0 };
+	u8 mgmt_buf[128];
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct net_device *ndev = padapter->pnetdev;
 
 	DBG_8723A("%s(padapter =%p,%s)\n", __func__, padapter, ndev->name);
 
+	memset(mgmt_buf, 0, 128);
+
 #if defined(RTW_USE_CFG80211_STA_EVENT)
 	cfg80211_del_sta(ndev, da, GFP_ATOMIC);
 #else /* defined(RTW_USE_CFG80211_STA_EVENT) */
@@ -2619,16 +2469,16 @@ void rtw_cfg80211_indicate_sta_disassoc(struct rtw_adapter *padapter,
 	pmgmt_frame = mgmt_buf;
 	pwlanhdr = (struct ieee80211_hdr *)pmgmt_frame;
 
-	fctrl = &pwlanhdr->frame_control;
-	*(fctrl) = 0;
+	pwlanhdr->frame_control =
+		cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_DEAUTH);
 
-	memcpy(pwlanhdr->addr1, myid(&padapter->eeprompriv), ETH_ALEN);
-	memcpy(pwlanhdr->addr2, da, ETH_ALEN);
-	memcpy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network), ETH_ALEN);
+	ether_addr_copy(pwlanhdr->addr1, myid(&padapter->eeprompriv));
+	ether_addr_copy(pwlanhdr->addr2, da);
+	ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network));
 
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
 	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pmgmt_frame, WIFI_DEAUTH);
 
 	pmgmt_frame += sizeof(struct ieee80211_hdr_3addr);
 	frame_len = sizeof(struct ieee80211_hdr_3addr);
@@ -2638,8 +2488,8 @@ void rtw_cfg80211_indicate_sta_disassoc(struct rtw_adapter *padapter,
 				       WLAN_REASON_PREV_AUTH_NOT_VALID,
 				       (unsigned char *)&reason, &frame_len);
 
-	rtw_cfg80211_rx_mgmt(padapter, freq, 0, mgmt_buf, frame_len,
-			     GFP_ATOMIC);
+	cfg80211_rx_mgmt(padapter->rtw_wdev, freq, 0, mgmt_buf, frame_len,
+			 0, GFP_ATOMIC);
 #endif /* defined(RTW_USE_CFG80211_STA_EVENT) */
 }
 
@@ -2676,7 +2526,7 @@ static int rtw_cfg80211_monitor_if_xmit_entry(struct sk_buff *skb,
 	struct ieee80211_radiotap_header *rtap_hdr;
 	struct rtw_adapter *padapter = netdev_priv(ndev);
 
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 
 	if (unlikely(skb->len < sizeof(struct ieee80211_radiotap_header)))
 		goto fail;
@@ -2718,8 +2568,8 @@ static int rtw_cfg80211_monitor_if_xmit_entry(struct sk_buff *skb,
 		skb_pull(skb, dot11_hdr_len + qos_len + snap_len -
 			 ETH_ALEN * 2);
 		pdata = (unsigned char *)skb->data;
-		memcpy(pdata, dst_mac_addr, ETH_ALEN);
-		memcpy(pdata + ETH_ALEN, src_mac_addr, ETH_ALEN);
+		ether_addr_copy(pdata, dst_mac_addr);
+		ether_addr_copy(pdata + ETH_ALEN, src_mac_addr);
 
 		DBG_8723A("should be eapol packet\n");
 
@@ -2729,6 +2579,7 @@ static int rtw_cfg80211_monitor_if_xmit_entry(struct sk_buff *skb,
 		return ret;
 
 	} else if (ieee80211_is_action(dot11_hdr->frame_control)) {
+		struct ieee80211_mgmt *mgmt;
 		/* only for action frames */
 		struct xmit_frame *pmgntframe;
 		struct pkt_attrib *pattrib;
@@ -2739,33 +2590,19 @@ static int rtw_cfg80211_monitor_if_xmit_entry(struct sk_buff *skb,
 		struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 		u32 len = skb->len;
 		u8 category, action;
-#ifdef CONFIG_8723AU_P2P
-		int type = -1;
-#endif
 
-		if (rtw_action_frame_parse23a(skb->data, len, &category,
-					   &action) == false) {
-			DBG_8723A(FUNC_NDEV_FMT " frame_control:0x%x\n",
-				  FUNC_NDEV_ARG(ndev),
-				  le16_to_cpu(dot11_hdr->frame_control));
-			goto fail;
-		}
+		mgmt = (struct ieee80211_mgmt *)dot11_hdr;
 
-		DBG_8723A("RTW_Tx:da =" MAC_FMT " via " FUNC_NDEV_FMT "\n",
-			  MAC_ARG(dot11_hdr->addr1), FUNC_NDEV_ARG(ndev));
-#ifdef CONFIG_8723AU_P2P
-		type = rtw_p2p_check_frames(padapter, skb->data, len, true);
-		if (type >= 0)
-			goto dump;
-#endif
-		if (category == WLAN_CATEGORY_PUBLIC)
+		DBG_8723A("RTW_Tx:da =" MAC_FMT " via %s(%s)\n",
+			  MAC_ARG(mgmt->da), __func__, ndev->name);
+		category = mgmt->u.action.category;
+		action = mgmt->u.action.u.wme_action.action_code;
+		if (mgmt->u.action.category == WLAN_CATEGORY_PUBLIC)
 			DBG_8723A("RTW_Tx:%s\n", action_public_str23a(action));
 		else
 			DBG_8723A("RTW_Tx:category(%u), action(%u)\n", category,
 				  action);
-#ifdef CONFIG_8723AU_P2P
-dump:
-#endif
+
 		/* starting alloc mgmt frame to dump it */
 		pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
 		if (pmgntframe == NULL)
@@ -2781,16 +2618,6 @@ dump:
 		pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 
 		memcpy(pframe, skb->data, len);
-#ifdef CONFIG_8723AU_P2P
-		if (type >= 0) {
-			struct wifi_display_info *pwfd_info;
-
-			pwfd_info = padapter->wdinfo.wfd_info;
-
-			if (pwfd_info->wfd_enable)
-				rtw_append_wfd_ie(padapter, pframe, &len);
-		}
-#endif /*  CONFIG_8723AU_P2P */
 		pattrib->pktlen = len;
 
 		/* update seq number */
@@ -2836,24 +2663,23 @@ static int rtw_cfg80211_add_monitor_if(struct rtw_adapter *padapter, char *name,
 	struct rtw_wdev_priv *pwdev_priv = wdev_to_priv(padapter->rtw_wdev);
 
 	if (!name) {
-		DBG_8723A(FUNC_ADPT_FMT " without specific name\n",
-			  FUNC_ADPT_ARG(padapter));
+		DBG_8723A("%s(%s): without specific name\n",
+			  __func__, padapter->pnetdev->name);
 		ret = -EINVAL;
 		goto out;
 	}
 
 	if (pwdev_priv->pmon_ndev) {
-		DBG_8723A(FUNC_ADPT_FMT " monitor interface exist: " NDEV_FMT
-			  "\n", FUNC_ADPT_ARG(padapter),
-			  NDEV_ARG(pwdev_priv->pmon_ndev));
+		DBG_8723A("%s(%s): monitor interface exist: %s\n", __func__,
+			  padapter->pnetdev->name, pwdev_priv->pmon_ndev->name);
 		ret = -EBUSY;
 		goto out;
 	}
 
 	mon_ndev = alloc_etherdev(sizeof(struct rtw_adapter));
 	if (!mon_ndev) {
-		DBG_8723A(FUNC_ADPT_FMT " allocate ndev fail\n",
-			  FUNC_ADPT_ARG(padapter));
+		DBG_8723A("%s(%s): allocate ndev fail\n", __func__,
+			  padapter->pnetdev->name);
 		ret = -ENOMEM;
 		goto out;
 	}
@@ -2868,8 +2694,8 @@ static int rtw_cfg80211_add_monitor_if(struct rtw_adapter *padapter, char *name,
 	/*  wdev */
 	mon_wdev = kzalloc(sizeof(struct wireless_dev), GFP_KERNEL);
 	if (!mon_wdev) {
-		DBG_8723A(FUNC_ADPT_FMT " allocate mon_wdev fail\n",
-			  FUNC_ADPT_ARG(padapter));
+		DBG_8723A("%s(%s): allocate mon_wdev fail\n", __func__,
+			  padapter->pnetdev->name);
 		ret = -ENOMEM;
 		goto out;
 	}
@@ -2910,8 +2736,8 @@ cfg80211_rtw_add_virtual_intf(struct wiphy *wiphy, const char *name,
 	struct net_device *ndev = NULL;
 	struct rtw_adapter *padapter = wiphy_to_adapter(wiphy);
 
-	DBG_8723A(FUNC_ADPT_FMT " wiphy:%s, name:%s, type:%d\n",
-		  FUNC_ADPT_ARG(padapter), wiphy_name(wiphy), name, type);
+	DBG_8723A("%s(%s): wiphy:%s, name:%s, type:%d\n", __func__,
+		  padapter->pnetdev->name, wiphy_name(wiphy), name, type);
 
 	switch (type) {
 	case NL80211_IFTYPE_ADHOC:
@@ -2940,7 +2766,8 @@ cfg80211_rtw_add_virtual_intf(struct wiphy *wiphy, const char *name,
 		break;
 	}
 
-	DBG_8723A(FUNC_ADPT_FMT " ndev:%p, ret:%d\n", FUNC_ADPT_ARG(padapter),
+	DBG_8723A("%s(%s): ndev:%p, ret:%d\n", __func__,
+		  padapter->pnetdev->name,
 		  ndev, ret);
 
 	return ndev ? ndev->ieee80211_ptr : ERR_PTR(ret);
@@ -2962,8 +2789,8 @@ static int cfg80211_rtw_del_virtual_intf(struct wiphy *wiphy,
 	if (ndev == pwdev_priv->pmon_ndev) {
 		pwdev_priv->pmon_ndev = NULL;
 		pwdev_priv->ifname_mon[0] = '\0';
-		DBG_8723A(FUNC_NDEV_FMT " remove monitor interface\n",
-			  FUNC_NDEV_ARG(ndev));
+		DBG_8723A("%s(%s): remove monitor interface\n",
+			  __func__, ndev->name);
 	}
 
 exit:
@@ -2976,10 +2803,6 @@ static int rtw_add_beacon(struct rtw_adapter *adapter, const u8 *head,
 	int ret = 0;
 	u8 *pbuf = NULL;
 	uint len, wps_ielen = 0;
-#ifdef CONFIG_8723AU_P2P
-	uint p2p_ielen = 0;
-	u8 got_p2p_ie = false;
-#endif
 	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
 	/* struct sta_priv *pstapriv = &padapter->stapriv; */
 
@@ -3007,55 +2830,14 @@ static int rtw_add_beacon(struct rtw_adapter *adapter, const u8 *head,
 	     &wps_ielen))
 		DBG_8723A("add bcn, wps_ielen =%d\n", wps_ielen);
 
-#ifdef CONFIG_8723AU_P2P
-	/* check p2p ie if inclued */
-	if (rtw_get_p2p_ie23a
-	    (pbuf + _FIXED_IE_LENGTH_, len - _FIXED_IE_LENGTH_, NULL,
-	     &p2p_ielen)) {
-		DBG_8723A("got p2p_ie, len =%d\n", p2p_ielen);
-		got_p2p_ie = true;
-	}
-#endif
-
 	/* pbss_network->IEs will not include p2p_ie, wfd ie */
-	rtw_ies_remove_ie23a(pbuf, &len, _BEACON_IE_OFFSET_, _VENDOR_SPECIFIC_IE_,
-			  P2P_OUI23A, 4);
-	rtw_ies_remove_ie23a(pbuf, &len, _BEACON_IE_OFFSET_, _VENDOR_SPECIFIC_IE_,
-			  WFD_OUI23A, 4);
+	rtw_ies_remove_ie23a(pbuf, &len, _BEACON_IE_OFFSET_,
+			     WLAN_EID_VENDOR_SPECIFIC, P2P_OUI23A, 4);
+	rtw_ies_remove_ie23a(pbuf, &len, _BEACON_IE_OFFSET_,
+			     WLAN_EID_VENDOR_SPECIFIC, WFD_OUI23A, 4);
 
 	if (rtw_check_beacon_data23a(adapter, pbuf, len) == _SUCCESS) {
-#ifdef CONFIG_8723AU_P2P
-		/* check p2p if enable */
-		if (got_p2p_ie == true) {
-			struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
-			struct wifidirect_info *pwdinfo = &adapter->wdinfo;
-
-			if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
-				DBG_8723A("Enable P2P function for the first "
-					  "time\n");
-				rtw_p2p_enable23a(adapter, P2P_ROLE_GO);
-				wdev_to_priv(adapter->rtw_wdev)->p2p_enabled =
-					true;
-			} else {
-				del_timer_sync(&pwdinfo->find_phase_timer);
-				del_timer_sync(&pwdinfo->
-					       restore_p2p_state_timer);
-				del_timer_sync(&pwdinfo->pre_tx_scan_timer);
-
-				DBG_8723A("enter GO Mode, p2p_ielen =%d\n",
-					  p2p_ielen);
-
-				rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
-				rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
-				pwdinfo->intent = 15;
-			}
-
-			pwdinfo->operating_channel = pmlmeext->cur_channel;
-		}
-#endif /* CONFIG_8723AU_P2P */
-
 		ret = 0;
-
 	} else {
 		ret = -EINVAL;
 	}
@@ -3071,8 +2853,8 @@ static int cfg80211_rtw_start_ap(struct wiphy *wiphy, struct net_device *ndev,
 	int ret = 0;
 	struct rtw_adapter *adapter = wiphy_to_adapter(wiphy);
 
-	DBG_8723A(FUNC_NDEV_FMT " hidden_ssid:%d, auth_type:%d\n",
-		  FUNC_NDEV_ARG(ndev), settings->hidden_ssid,
+	DBG_8723A("%s(%s): hidden_ssid:%d, auth_type:%d\n",
+		  __func__, ndev->name, settings->hidden_ssid,
 		  settings->auth_type);
 
 	ret = rtw_add_beacon(adapter, settings->beacon.head,
@@ -3088,29 +2870,12 @@ static int cfg80211_rtw_start_ap(struct wiphy *wiphy, struct net_device *ndev,
 		struct wlan_bssid_ex *pbss_network_ext =
 			&adapter->mlmeextpriv.mlmext_info.network;
 
-		if (0)
-			DBG_8723A(FUNC_ADPT_FMT
-				  " ssid:(%s,%d), from ie:(%s,%d)\n",
-				  FUNC_ADPT_ARG(adapter), settings->ssid,
-				  (int)settings->ssid_len,
-				  pbss_network->Ssid.ssid,
-				  pbss_network->Ssid.ssid_len);
-
 		memcpy(pbss_network->Ssid.ssid, (void *)settings->ssid,
 		       settings->ssid_len);
 		pbss_network->Ssid.ssid_len = settings->ssid_len;
 		memcpy(pbss_network_ext->Ssid.ssid, (void *)settings->ssid,
 		       settings->ssid_len);
 		pbss_network_ext->Ssid.ssid_len = settings->ssid_len;
-
-		if (0)
-			DBG_8723A(FUNC_ADPT_FMT
-				  " after ssid:(%s,%d), (%s,%d)\n",
-				  FUNC_ADPT_ARG(adapter),
-				  pbss_network->Ssid.ssid,
-				  pbss_network->Ssid.ssid_len,
-				  pbss_network_ext->Ssid.ssid,
-				  pbss_network_ext->Ssid.ssid_len);
 	}
 
 	return ret;
@@ -3123,7 +2888,7 @@ static int cfg80211_rtw_change_beacon(struct wiphy *wiphy,
 	int ret = 0;
 	struct rtw_adapter *adapter = wiphy_to_adapter(wiphy);
 
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 
 	ret = rtw_add_beacon(adapter, info->head, info->head_len, info->tail,
 			     info->tail_len);
@@ -3133,7 +2898,7 @@ static int cfg80211_rtw_change_beacon(struct wiphy *wiphy,
 
 static int cfg80211_rtw_stop_ap(struct wiphy *wiphy, struct net_device *ndev)
 {
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 	return 0;
 }
 
@@ -3141,7 +2906,7 @@ static int cfg80211_rtw_add_station(struct wiphy *wiphy,
 				    struct net_device *ndev, u8 *mac,
 				    struct station_parameters *params)
 {
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 
 	return 0;
 }
@@ -3157,7 +2922,7 @@ static int cfg80211_rtw_del_station(struct wiphy *wiphy,
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 
-	DBG_8723A("+" FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("+%s(%s)\n", __func__, ndev->name);
 
 	if (check_fwstate(pmlmepriv, (_FW_LINKED | WIFI_AP_STATE)) != true) {
 		DBG_8723A("%s, fw_state != FW_LINKED|WIFI_AP_STATE\n",
@@ -3188,7 +2953,7 @@ static int cfg80211_rtw_del_station(struct wiphy *wiphy,
 	list_for_each_safe(plist, ptmp, phead) {
 		psta = container_of(plist, struct sta_info, asoc_list);
 
-		if (!memcmp(mac, psta->hwaddr, ETH_ALEN)) {
+		if (ether_addr_equal(mac, psta->hwaddr)) {
 			if (psta->dot8021xalg == 1 &&
 			    psta->bpairwise_key_installed == false) {
 				DBG_8723A("%s, sta's dot8021xalg = 1 and "
@@ -3217,7 +2982,7 @@ static int cfg80211_rtw_del_station(struct wiphy *wiphy,
 
 	associated_clients_update23a(padapter, updated);
 
-	DBG_8723A("-" FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("-%s(%s)\n", __func__, ndev->name);
 
 	return ret;
 }
@@ -3226,7 +2991,7 @@ static int cfg80211_rtw_change_station(struct wiphy *wiphy,
 				       struct net_device *ndev, u8 *mac,
 				       struct station_parameters *params)
 {
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 	return 0;
 }
 
@@ -3234,7 +2999,7 @@ static int cfg80211_rtw_dump_station(struct wiphy *wiphy,
 				     struct net_device *ndev, int idx, u8 *mac,
 				     struct station_info *sinfo)
 {
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 
 	/* TODO: dump scanned queue */
 
@@ -3244,104 +3009,27 @@ static int cfg80211_rtw_dump_station(struct wiphy *wiphy,
 static int cfg80211_rtw_change_bss(struct wiphy *wiphy, struct net_device *ndev,
 				   struct bss_parameters *params)
 {
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 	return 0;
 }
 #endif /* CONFIG_8723AU_AP_MODE */
 
-void rtw_cfg80211_rx_action_p2p(struct rtw_adapter *padapter, u8 *pmgmt_frame,
-				uint frame_len)
-{
-#ifdef CONFIG_8723AU_P2P
-	int type;
-#endif
-	s32 freq;
-	int channel;
-	u8 category, action;
-
-	channel = rtw_get_oper_ch23a(padapter);
-
-	DBG_8723A("RTW_Rx:cur_ch =%d\n", channel);
-#ifdef CONFIG_8723AU_P2P
-	type = rtw_p2p_check_frames(padapter, pmgmt_frame, frame_len, false);
-	if (type >= 0)
-		goto indicate;
-#endif
-	rtw_action_frame_parse23a(pmgmt_frame, frame_len, &category, &action);
-	DBG_8723A("RTW_Rx:category(%u), action(%u)\n", category, action);
-
-#ifdef CONFIG_8723AU_P2P
-indicate:
-#endif
-	if (channel <= RTW_CH_MAX_2G_CHANNEL)
-		freq = ieee80211_channel_to_frequency(channel,
-						      IEEE80211_BAND_2GHZ);
-	else
-		freq = ieee80211_channel_to_frequency(channel,
-						      IEEE80211_BAND_5GHZ);
-
-	rtw_cfg80211_rx_mgmt(padapter, freq, 0, pmgmt_frame, frame_len,
-			     GFP_ATOMIC);
-}
-
-void rtw_cfg80211_rx_p2p_action_public(struct rtw_adapter *padapter,
-				       u8 *pmgmt_frame, uint frame_len)
-{
-#ifdef CONFIG_8723AU_P2P
-	int type;
-#endif
-	s32 freq;
-	int channel;
-	u8 category, action;
-
-	channel = rtw_get_oper_ch23a(padapter);
-
-	DBG_8723A("RTW_Rx:cur_ch =%d\n", channel);
-#ifdef CONFIG_8723AU_P2P
-	type = rtw_p2p_check_frames(padapter, pmgmt_frame, frame_len, false);
-	if (type >= 0) {
-		switch (type) {
-		case P2P_GO_NEGO_CONF:
-		case P2P_PROVISION_DISC_RESP:
-			rtw_clear_scan_deny(padapter);
-		}
-		goto indicate;
-	}
-#endif
-	rtw_action_frame_parse23a(pmgmt_frame, frame_len, &category, &action);
-	DBG_8723A("RTW_Rx:category(%u), action(%u)\n", category, action);
-
-#ifdef CONFIG_8723AU_P2P
-indicate:
-#endif
-	if (channel <= RTW_CH_MAX_2G_CHANNEL)
-		freq = ieee80211_channel_to_frequency(channel,
-						      IEEE80211_BAND_2GHZ);
-	else
-		freq = ieee80211_channel_to_frequency(channel,
-						      IEEE80211_BAND_5GHZ);
-
-	rtw_cfg80211_rx_mgmt(padapter, freq, 0, pmgmt_frame, frame_len,
-			     GFP_ATOMIC);
-}
-
 void rtw_cfg80211_rx_action(struct rtw_adapter *adapter, u8 *frame,
 			    uint frame_len, const char *msg)
 {
+	struct ieee80211_mgmt *hdr = (struct ieee80211_mgmt *)frame;
 	s32 freq;
 	int channel;
-	u8 category, action;
 
 	channel = rtw_get_oper_ch23a(adapter);
 
-	rtw_action_frame_parse23a(frame, frame_len, &category, &action);
-
 	DBG_8723A("RTW_Rx:cur_ch =%d\n", channel);
 	if (msg)
 		DBG_8723A("RTW_Rx:%s\n", msg);
 	else
-		DBG_8723A("RTW_Rx:category(%u), action(%u)\n", category,
-			  action);
+		DBG_8723A("RTW_Rx:category(%u), action(%u)\n",
+			  hdr->u.action.category,
+			  hdr->u.action.u.wme_action.action_code);
 
 	if (channel <= RTW_CH_MAX_2G_CHANNEL)
 		freq = ieee80211_channel_to_frequency(channel,
@@ -3350,353 +3038,10 @@ void rtw_cfg80211_rx_action(struct rtw_adapter *adapter, u8 *frame,
 		freq = ieee80211_channel_to_frequency(channel,
 						      IEEE80211_BAND_5GHZ);
 
-	rtw_cfg80211_rx_mgmt(adapter, freq, 0, frame, frame_len, GFP_ATOMIC);
+	cfg80211_rx_mgmt(adapter->rtw_wdev, freq, 0, frame, frame_len,
+			 0, GFP_ATOMIC);
 }
 
-#ifdef CONFIG_8723AU_P2P
-void rtw_cfg80211_issue_p2p_provision_request23a(struct rtw_adapter *padapter,
-					      const u8 *buf, size_t len)
-{
-	u16 wps_devicepassword_id = 0x0000;
-	uint wps_devicepassword_id_len = 0;
-	u8 wpsie[255] = { 0x00 }, p2p_ie[255] = { 0x00 };
-	uint p2p_ielen = 0;
-	uint wpsielen = 0;
-	u32 devinfo_contentlen = 0;
-	u8 devinfo_content[64] = { 0x00 };
-	u16 capability = 0;
-	uint capability_len = 0;
-
-	unsigned char category = WLAN_CATEGORY_PUBLIC;
-	u8 action = P2P_PUB_ACTION_ACTION;
-	u8 dialogToken = 1;
-	u32 p2poui = cpu_to_be32(P2POUI);
-	u8 oui_subtype = P2P_PROVISION_DISC_REQ;
-	u32 p2pielen = 0;
-#ifdef CONFIG_8723AU_P2P
-	u32 wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
-
-	struct xmit_frame *pmgntframe;
-	struct pkt_attrib *pattrib;
-	unsigned char *pframe;
-	struct ieee80211_hdr *pwlanhdr, *hdr;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	u8 *frame_body =
-	    (unsigned char *)(buf + sizeof(struct ieee80211_hdr_3addr));
-	size_t frame_body_len = len - sizeof(struct ieee80211_hdr_3addr);
-
-	DBG_8723A("[%s] In\n", __func__);
-
-	hdr = (struct ieee80211_hdr *)buf;
-	/* prepare for building provision_request frame */
-	memcpy(pwdinfo->tx_prov_disc_info.peerIFAddr, hdr->addr1, ETH_ALEN);
-	memcpy(pwdinfo->tx_prov_disc_info.peerDevAddr, hdr->addr1, ETH_ALEN);
-
-	pwdinfo->tx_prov_disc_info.wps_config_method_request =
-	    WPS_CM_PUSH_BUTTON;
-
-	rtw_get_wps_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_,
-		       frame_body_len - _PUBLIC_ACTION_IE_OFFSET_, wpsie,
-		       &wpsielen);
-	rtw_get_wps_attr_content23a(wpsie, wpsielen, WPS_ATTR_DEVICE_PWID,
-				    (u8 *)&wps_devicepassword_id,
-				    &wps_devicepassword_id_len);
-	wps_devicepassword_id = be16_to_cpu(wps_devicepassword_id);
-
-	switch (wps_devicepassword_id) {
-	case WPS_DPID_PIN:
-		pwdinfo->tx_prov_disc_info.wps_config_method_request =
-			WPS_CM_LABEL;
-		break;
-	case WPS_DPID_USER_SPEC:
-		pwdinfo->tx_prov_disc_info.wps_config_method_request =
-			WPS_CM_DISPLYA;
-		break;
-	case WPS_DPID_MACHINE_SPEC:
-		break;
-	case WPS_DPID_REKEY:
-		break;
-	case WPS_DPID_PBC:
-		pwdinfo->tx_prov_disc_info.wps_config_method_request =
-			WPS_CM_PUSH_BUTTON;
-		break;
-	case WPS_DPID_REGISTRAR_SPEC:
-		pwdinfo->tx_prov_disc_info.wps_config_method_request =
-			WPS_CM_KEYPAD;
-		break;
-	default:
-		break;
-	}
-
-	if (rtw_get_p2p_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_,
-			   frame_body_len - _PUBLIC_ACTION_IE_OFFSET_,
-			   p2p_ie, &p2p_ielen)) {
-		rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen,
-					 P2P_ATTR_DEVICE_INFO, devinfo_content,
-					 &devinfo_contentlen);
-		rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY,
-					 (u8 *)&capability, &capability_len);
-	}
-
-	/* start to build provision_request frame */
-	memset(wpsie, 0, sizeof(wpsie));
-	memset(p2p_ie, 0, sizeof(p2p_ie));
-	p2p_ielen = 0;
-
-	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
-	if (pmgntframe == NULL)
-		return;
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *) (pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	pwlanhdr->frame_control = 0;
-
-	memcpy(pwlanhdr->addr1, pwdinfo->tx_prov_disc_info.peerDevAddr,
-	       ETH_ALEN);
-	memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
-	memcpy(pwlanhdr->addr3, pwdinfo->tx_prov_disc_info.peerDevAddr,
-	       ETH_ALEN);
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
-
-	pframe += sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
-
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *)&p2poui,
-				  &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen);
-
-	/* build_prov_disc_request_p2p_ie23a */
-	/*      P2P OUI */
-	p2pielen = 0;
-	p2p_ie[p2pielen++] = 0x50;
-	p2p_ie[p2pielen++] = 0x6F;
-	p2p_ie[p2pielen++] = 0x9A;
-	p2p_ie[p2pielen++] = 0x09;	/*      WFA P2P v1.0 */
-
-	/*      Commented by Albert 20110301 */
-	/*      According to the P2P Specification, the provision discovery request frame should contain 3 P2P attributes */
-	/*      1. P2P Capability */
-	/*      2. Device Info */
-	/*      3. Group ID ( When joining an operating P2P Group ) */
-
-	/*      P2P Capability ATTR */
-	/*      Type: */
-	p2p_ie[p2pielen++] = P2P_ATTR_CAPABILITY;
-
-	/*      Length: */
-	put_unaligned_le16(0x0002, p2p_ie + p2pielen);
-	p2pielen += 2;
-
-	/*      Value: */
-	/*      Device Capability Bitmap, 1 byte */
-	/*      Group Capability Bitmap, 1 byte */
-	memcpy(p2p_ie + p2pielen, &capability, 2);
-	p2pielen += 2;
-
-	/*      Device Info ATTR */
-	/*      Type: */
-	p2p_ie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
-
-	/*      Length: */
-	put_unaligned_le16(devinfo_contentlen, p2p_ie + p2pielen);
-	p2pielen += 2;
-
-	/*      Value: */
-	memcpy(p2p_ie + p2pielen, devinfo_content, devinfo_contentlen);
-	p2pielen += devinfo_contentlen;
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen,
-			    (unsigned char *)p2p_ie, &p2p_ielen);
-	pattrib->pktlen += p2p_ielen;
-
-	wpsielen = 0;
-	/*      WPS OUI */
-	*(u32 *)(wpsie) = cpu_to_be32(WPSOUI);
-	wpsielen += 4;
-
-	/*      WPS version */
-	/*      Type: */
-	*(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
-	wpsielen += 2;
-
-	/*      Length: */
-	*(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
-	wpsielen += 2;
-
-	/*      Value: */
-	wpsie[wpsielen++] = WPS_VERSION_1;	/*      Version 1.0 */
-
-	/*      Config Method */
-	/*      Type: */
-	*(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD);
-	wpsielen += 2;
-
-	/*      Length: */
-	*(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
-	wpsielen += 2;
-
-	/*      Value: */
-	*(u16 *)(wpsie + wpsielen) =
-	    cpu_to_be16(pwdinfo->tx_prov_disc_info.wps_config_method_request);
-	wpsielen += 2;
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, wpsielen,
-			    (unsigned char *)wpsie, &pattrib->pktlen);
-
-#ifdef CONFIG_8723AU_P2P
-	wfdielen = build_provdisc_req_wfd_ie(pwdinfo, pframe);
-	pframe += wfdielen;
-	pattrib->pktlen += wfdielen;
-#endif /* CONFIG_8723AU_P2P */
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	/* dump_mgntframe23a(padapter, pmgntframe); */
-	if (dump_mgntframe23a_and_wait_ack23a(padapter, pmgntframe) != _SUCCESS)
-		DBG_8723A("%s, ack to\n", __func__);
-}
-
-static s32 cfg80211_rtw_remain_on_channel(struct wiphy *wiphy,
-					  struct wireless_dev *wdev,
-					  struct ieee80211_channel *channel,
-					  unsigned int duration, u64 *cookie)
-{
-	s32 err = 0;
-	struct rtw_adapter *padapter = wiphy_to_adapter(wiphy);
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	struct cfg80211_wifidirect_info *pcfg80211_wdinfo =
-	    &padapter->cfg80211_wdinfo;
-	u8 remain_ch =
-		(u8) ieee80211_frequency_to_channel(channel->center_freq);
-	u8 ready_on_channel = false;
-
-	DBG_8723A(FUNC_ADPT_FMT " ch:%u duration:%d\n", FUNC_ADPT_ARG(padapter),
-		  remain_ch, duration);
-
-	if (pcfg80211_wdinfo->is_ro_ch == true) {
-		DBG_8723A("%s, cancel ro ch timer\n", __func__);
-
-		del_timer_sync(&padapter->cfg80211_wdinfo.remain_on_ch_timer);
-
-#ifdef CONFIG_8723AU_P2P
-		p2p_protocol_wk_hdl23a(padapter, P2P_RO_CH_WK);
-#endif
-	}
-
-	pcfg80211_wdinfo->is_ro_ch = true;
-
-	if (_FAIL == rtw_pwr_wakeup(padapter)) {
-		err = -EFAULT;
-		goto exit;
-	}
-
-	memcpy(&pcfg80211_wdinfo->remain_on_ch_channel, channel,
-	       sizeof(struct ieee80211_channel));
-	pcfg80211_wdinfo->remain_on_ch_cookie = *cookie;
-
-	rtw_scan_abort23a(padapter);
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
-		rtw_p2p_enable23a(padapter, P2P_ROLE_DEVICE);
-		wdev_to_priv(padapter->rtw_wdev)->p2p_enabled = true;
-	} else {
-		rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
-#ifdef CONFIG_DEBUG_CFG80211
-		DBG_8723A("%s, role =%d, p2p_state =%d\n", __func__,
-			  rtw_p2p_role(pwdinfo), rtw_p2p_state(pwdinfo));
-#endif
-	}
-
-	rtw_p2p_set_state(pwdinfo, P2P_STATE_LISTEN);
-
-	if (duration < 400)
-		duration = duration * 3;	/* extend from exper. */
-
-	pcfg80211_wdinfo->restore_channel = pmlmeext->cur_channel;
-
-	if (rtw_ch_set_search_ch23a(pmlmeext->channel_set, remain_ch) >= 0) {
-		if (remain_ch != pmlmeext->cur_channel) {
-			ready_on_channel = true;
-		}
-	} else {
-		DBG_8723A("%s remain_ch:%u not in channel plan!!!!\n",
-			  __func__, remain_ch);
-	}
-
-	/* call this after other things have been done */
-	if (ready_on_channel == true) {
-		if (!check_fwstate(&padapter->mlmepriv, _FW_LINKED)) {
-			pmlmeext->cur_channel = remain_ch;
-
-			set_channel_bwmode23a(padapter, remain_ch,
-					   HAL_PRIME_CHNL_OFFSET_DONT_CARE,
-					   HT_CHANNEL_WIDTH_20);
-		}
-	}
-	DBG_8723A("%s, set ro ch timer, duration =%d\n", __func__, duration);
-	mod_timer(&pcfg80211_wdinfo->remain_on_ch_timer,
-		  jiffies + msecs_to_jiffies(duration));
-
-	rtw_cfg80211_ready_on_channel(padapter, *cookie, channel, channel_type,
-				      duration, GFP_KERNEL);
-
-	pwdinfo->listen_channel = pmlmeext->cur_channel;
-
-exit:
-	if (err)
-		pcfg80211_wdinfo->is_ro_ch = false;
-
-	return err;
-}
-
-static s32 cfg80211_rtw_cancel_remain_on_channel(struct wiphy *wiphy,
-						 struct wireless_dev *wdev,
-						 u64 cookie)
-{
-	s32 err = 0;
-	struct rtw_adapter *padapter = wiphy_to_adapter(wiphy);
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	struct cfg80211_wifidirect_info *pcfg80211_wdinfo =
-	    &padapter->cfg80211_wdinfo;
-
-	DBG_8723A(FUNC_ADPT_FMT "\n", FUNC_ADPT_ARG(padapter));
-
-	if (pcfg80211_wdinfo->is_ro_ch == true) {
-		DBG_8723A("%s, cancel ro ch timer\n", __func__);
-		del_timer_sync(&padapter->cfg80211_wdinfo.remain_on_ch_timer);
-#ifdef CONFIG_8723AU_P2P
-		p2p_protocol_wk_hdl23a(padapter, P2P_RO_CH_WK);
-#endif
-	}
-
-	rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
-#ifdef CONFIG_DEBUG_CFG80211
-	DBG_8723A("%s, role =%d, p2p_state =%d\n", __func__,
-		  rtw_p2p_role(pwdinfo), rtw_p2p_state(pwdinfo));
-#endif
-	pcfg80211_wdinfo->is_ro_ch = false;
-
-	return err;
-}
-
-#endif /* CONFIG_8723AU_P2P */
-
 static int _cfg80211_rtw_mgmt_tx(struct rtw_adapter *padapter, u8 tx_ch,
 				 const u8 *buf, size_t len)
 {
@@ -3704,11 +3049,9 @@ static int _cfg80211_rtw_mgmt_tx(struct rtw_adapter *padapter, u8 tx_ch,
 	struct pkt_attrib *pattrib;
 	unsigned char *pframe;
 	int ret = _FAIL;
-	bool ack = true;
 	struct ieee80211_hdr *pwlanhdr;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	/* struct cfg80211_wifidirect_info *pcfg80211_wdinfo = &padapter->cfg80211_wdinfo; */
 
 	if (_FAIL == rtw_pwr_wakeup(padapter)) {
 		ret = -EFAULT;
@@ -3729,7 +3072,7 @@ static int _cfg80211_rtw_mgmt_tx(struct rtw_adapter *padapter, u8 tx_ch,
 
 	/* starting alloc mgmt frame to dump it */
 	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
-	if (pmgntframe == NULL) {
+	if (!pmgntframe) {
 		/* ret = -ENOMEM; */
 		ret = _FAIL;
 		goto exit;
@@ -3753,139 +3096,76 @@ static int _cfg80211_rtw_mgmt_tx(struct rtw_adapter *padapter, u8 tx_ch,
 	pattrib->seqnum = pmlmeext->mgnt_seq;
 	pmlmeext->mgnt_seq++;
 
-#ifdef CONFIG_8723AU_P2P
-	{
-		struct wifi_display_info *pwfd_info;
-
-		pwfd_info = padapter->wdinfo.wfd_info;
-
-		if (true == pwfd_info->wfd_enable) {
-			rtw_append_wfd_ie(padapter, pframe, &pattrib->pktlen);
-		}
-	}
-#endif /*  CONFIG_8723AU_P2P */
-
 	pattrib->last_txcmdsz = pattrib->pktlen;
 
-	if (dump_mgntframe23a_and_wait_ack23a(padapter, pmgntframe) != _SUCCESS) {
-		ack = false;
-		ret = _FAIL;
+	ret = dump_mgntframe23a_and_wait_ack23a(padapter, pmgntframe);
 
-#ifdef CONFIG_DEBUG_CFG80211
-		DBG_8723A("%s, ack == _FAIL\n", __func__);
-#endif
-	} else {
-#ifdef CONFIG_DEBUG_CFG80211
-		DBG_8723A("%s, ack =%d, ok!\n", __func__, ack);
-#endif
-		ret = _SUCCESS;
-	}
+	if (ret  != _SUCCESS)
+		DBG_8723A("%s, ack == false\n", __func__);
+	else
+		DBG_8723A("%s, ack == true\n", __func__);
 
 exit:
 
-#ifdef CONFIG_DEBUG_CFG80211
 	DBG_8723A("%s, ret =%d\n", __func__, ret);
-#endif
 
 	return ret;
 }
 
 static int cfg80211_rtw_mgmt_tx(struct wiphy *wiphy, struct wireless_dev *wdev,
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3, 14, 0))
-				struct ieee80211_channel *chan,
-				bool offchan,
-				unsigned int wait,
-				const u8 *buf, size_t len,
-				bool no_cck, bool dont_wait_for_ack,
-#else
 				struct cfg80211_mgmt_tx_params *params,
-#endif
 				u64 *cookie)
 {
 	struct rtw_adapter *padapter =
 		(struct rtw_adapter *)wiphy_to_adapter(wiphy);
-	struct rtw_wdev_priv *pwdev_priv = wdev_to_priv(padapter->rtw_wdev);
 	int ret = 0;
 	int tx_ret;
 	u32 dump_limit = RTW_MAX_MGMT_TX_CNT;
 	u32 dump_cnt = 0;
 	bool ack = true;
 	u8 category, action;
-	int type = (-1);
 	unsigned long start = jiffies;
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 14, 0))
 	size_t len = params->len;
 	struct ieee80211_channel *chan = params->chan;
 	const u8 *buf = params->buf;
-#endif
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)buf;
+	struct ieee80211_mgmt *hdr = (struct ieee80211_mgmt *)buf;
 	u8 tx_ch = (u8) ieee80211_frequency_to_channel(chan->center_freq);
 
+	if (!ieee80211_is_action(hdr->frame_control))
+		return -EINVAL;
+
 	/* cookie generation */
 	*cookie = (unsigned long)buf;
 
-#ifdef CONFIG_DEBUG_CFG80211
-	DBG_8723A(FUNC_ADPT_FMT " len =%zu, ch =%d"
-		  "\n", FUNC_ADPT_ARG(padapter), len, tx_ch);
-#endif /* CONFIG_DEBUG_CFG80211 */
+	DBG_8723A("%s(%s): len =%zu, ch =%d\n", __func__,
+		  padapter->pnetdev->name, len, tx_ch);
 
 	/* indicate ack before issue frame to avoid racing with rsp frame */
-	rtw_cfg80211_mgmt_tx_status(padapter, *cookie, buf, len, ack,
-				    GFP_KERNEL);
-
-	if (rtw_action_frame_parse23a(buf, len, &category, &action) == false) {
-		DBG_8723A(FUNC_ADPT_FMT " frame_control:0x%x\n",
-			  FUNC_ADPT_ARG(padapter),
-			  le16_to_cpu(hdr->frame_control));
-		goto exit;
-	}
+	cfg80211_mgmt_tx_status(padapter->rtw_wdev, *cookie, buf, len, ack,
+				GFP_KERNEL);
 
 	DBG_8723A("RTW_Tx:tx_ch =%d, da =" MAC_FMT "\n", tx_ch,
-		  MAC_ARG(hdr->addr1));
-#ifdef CONFIG_8723AU_P2P
-	type = rtw_p2p_check_frames(padapter, buf, len, true);
-	if (type >= 0)
-		goto dump;
-#endif
+		  MAC_ARG(hdr->da));
+	category = hdr->u.action.category;
+	action = hdr->u.action.u.wme_action.action_code;
 	if (category == WLAN_CATEGORY_PUBLIC)
 		DBG_8723A("RTW_Tx:%s\n", action_public_str23a(action));
 	else
 		DBG_8723A("RTW_Tx:category(%u), action(%u)\n",
 			  category, action);
 
-#ifdef CONFIG_8723AU_P2P
-dump:
-#endif
 	do {
 		dump_cnt++;
 		tx_ret = _cfg80211_rtw_mgmt_tx(padapter, tx_ch, buf, len);
 	} while (dump_cnt < dump_limit && tx_ret != _SUCCESS);
 
 	if (tx_ret != _SUCCESS || dump_cnt > 1) {
-		DBG_8723A(FUNC_ADPT_FMT " %s (%d/%d) in %d ms\n",
-			  FUNC_ADPT_ARG(padapter),
+		DBG_8723A("%s(%s): %s (%d/%d) in %d ms\n",
+			  __func__, padapter->pnetdev->name,
 			  tx_ret == _SUCCESS ? "OK" : "FAIL", dump_cnt,
 			  dump_limit, jiffies_to_msecs(jiffies - start));
 	}
 
-	switch (type) {
-	case P2P_GO_NEGO_CONF:
-		rtw_clear_scan_deny(padapter);
-		break;
-	case P2P_INVIT_RESP:
-		if (pwdev_priv->invit_info.flags & BIT(0)
-		    && pwdev_priv->invit_info.status == 0) {
-			DBG_8723A(FUNC_ADPT_FMT " agree with invitation of "
-				  "persistent group\n",
-				  FUNC_ADPT_ARG(padapter));
-			rtw_set_scan_deny(padapter, 5000);
-			rtw_pwr_wakeup_ex(padapter, 5000);
-			rtw_clear_scan_deny(padapter);
-		}
-		break;
-	}
-
-exit:
 	return ret;
 }
 
@@ -3893,358 +3173,12 @@ static void cfg80211_rtw_mgmt_frame_register(struct wiphy *wiphy,
 					     struct wireless_dev *wdev,
 					     u16 frame_type, bool reg)
 {
-
-#ifdef CONFIG_DEBUG_CFG80211
-	DBG_8723A(FUNC_ADPT_FMT " frame_type:%x, reg:%d\n",
-		  FUNC_ADPT_ARG(adapter), frame_type, reg);
-#endif
-
 	if (frame_type != (IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_PROBE_REQ))
 		return;
 
 	return;
 }
 
-static int rtw_cfg80211_set_beacon_wpsp2pie(struct net_device *ndev, char *buf,
-					    int len)
-{
-	int ret = 0;
-	uint wps_ielen = 0;
-	u8 *wps_ie;
-#ifdef CONFIG_8723AU_P2P
-	u32 p2p_ielen = 0;
-	u32 wfd_ielen = 0;
-	u8 *p2p_ie;
-#endif
-#ifdef CONFIG_8723AU_AP_MODE
-	u8 wps_oui[8] = { 0x0, 0x50, 0xf2, 0x04 };
-#endif
-	struct rtw_adapter *padapter = netdev_priv(ndev);
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-
-	DBG_8723A(FUNC_NDEV_FMT " ielen =%d\n", FUNC_NDEV_ARG(ndev), len);
-
-	if (len > 0) {
-		wps_ie = rtw_get_wps_ie23a(buf, len, NULL, &wps_ielen);
-		if (wps_ie) {
-#ifdef CONFIG_DEBUG_CFG80211
-			DBG_8723A("bcn_wps_ielen =%d\n", wps_ielen);
-#endif
-
-			if (pmlmepriv->wps_beacon_ie) {
-				pmlmepriv->wps_beacon_ie_len = 0;
-				kfree(pmlmepriv->wps_beacon_ie);
-				pmlmepriv->wps_beacon_ie = NULL;
-			}
-
-			pmlmepriv->wps_beacon_ie =
-				kmalloc(wps_ielen, GFP_KERNEL);
-			if (pmlmepriv->wps_beacon_ie == NULL) {
-				DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-					  __func__, __LINE__);
-				return -EINVAL;
-			}
-			memcpy(pmlmepriv->wps_beacon_ie, wps_ie, wps_ielen);
-			pmlmepriv->wps_beacon_ie_len = wps_ielen;
-
-#ifdef CONFIG_8723AU_AP_MODE
-			update_beacon23a(padapter, _VENDOR_SPECIFIC_IE_, wps_oui,
-				      true);
-#endif
-		}
-#ifdef CONFIG_8723AU_P2P
-		p2p_ie = rtw_get_p2p_ie23a(buf, len, NULL, &p2p_ielen);
-		if (p2p_ie) {
-#ifdef CONFIG_DEBUG_CFG80211
-			DBG_8723A("bcn_p2p_ielen =%d\n", p2p_ielen);
-#endif
-
-			if (pmlmepriv->p2p_beacon_ie) {
-				pmlmepriv->p2p_beacon_ie_len = 0;
-				kfree(pmlmepriv->p2p_beacon_ie);
-				pmlmepriv->p2p_beacon_ie = NULL;
-			}
-
-			pmlmepriv->p2p_beacon_ie =
-				kmalloc(p2p_ielen, GFP_KERNEL);
-			if (pmlmepriv->p2p_beacon_ie == NULL) {
-				DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-					  __func__, __LINE__);
-				return -EINVAL;
-			}
-
-			memcpy(pmlmepriv->p2p_beacon_ie, p2p_ie, p2p_ielen);
-			pmlmepriv->p2p_beacon_ie_len = p2p_ielen;
-		}
-#endif /* CONFIG_8723AU_P2P */
-
-		/* buf += p2p_ielen; */
-		/* len -= p2p_ielen; */
-
-#ifdef CONFIG_8723AU_P2P
-		if (rtw_get_wfd_ie(buf, len, NULL, &wfd_ielen)) {
-#ifdef CONFIG_DEBUG_CFG80211
-			DBG_8723A("bcn_wfd_ielen =%d\n", wfd_ielen);
-#endif
-
-			if (pmlmepriv->wfd_beacon_ie) {
-				pmlmepriv->wfd_beacon_ie_len = 0;
-				kfree(pmlmepriv->wfd_beacon_ie);
-				pmlmepriv->wfd_beacon_ie = NULL;
-			}
-
-			pmlmepriv->wfd_beacon_ie =
-				kmalloc(wfd_ielen, GFP_KERNEL);
-			if (pmlmepriv->wfd_beacon_ie == NULL) {
-				DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-					  __func__, __LINE__);
-				return -EINVAL;
-
-			}
-			rtw_get_wfd_ie(buf, len, pmlmepriv->wfd_beacon_ie,
-				       &pmlmepriv->wfd_beacon_ie_len);
-		}
-#endif /* CONFIG_8723AU_P2P */
-
-		pmlmeext->bstart_bss = true;
-
-	}
-
-	return ret;
-}
-
-static int rtw_cfg80211_set_probe_resp_wpsp2pie(struct net_device *net,
-						char *buf, int len)
-{
-	struct rtw_adapter *padapter = netdev_priv(net);
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-#ifdef CONFIG_8723AU_P2P
-	u32 p2p_ielen = 0;
-	u8 *p2p_ie;
-	u32 wfd_ielen = 0;
-#endif
-	int ret = 0;
-	uint wps_ielen = 0;
-	u8 *wps_ie;
-
-	if (len > 0) {
-		wps_ie = rtw_get_wps_ie23a(buf, len, NULL, &wps_ielen);
-		if (wps_ie) {
-			uint attr_contentlen = 0;
-			u16 uconfig_method, *puconfig_method = NULL;
-
-			if (pmlmepriv->wps_probe_resp_ie) {
-				pmlmepriv->wps_probe_resp_ie_len = 0;
-				kfree(pmlmepriv->wps_probe_resp_ie);
-				pmlmepriv->wps_probe_resp_ie = NULL;
-			}
-
-			pmlmepriv->wps_probe_resp_ie =
-				kmalloc(wps_ielen, GFP_KERNEL);
-			if (pmlmepriv->wps_probe_resp_ie == NULL) {
-				DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-					  __func__, __LINE__);
-				return -EINVAL;
-
-			}
-
-			/* add PUSH_BUTTON config_method by driver self in
-			   wpsie of probe_resp at GO Mode */
-			puconfig_method = (u16 *)rtw_get_wps_attr_content23a(wps_ie, wps_ielen,
-							      WPS_ATTR_CONF_METHOD,
-							      NULL,
-							      &attr_contentlen);
-			if (puconfig_method) {
-				uconfig_method = WPS_CM_PUSH_BUTTON;
-				uconfig_method = cpu_to_be16(uconfig_method);
-
-				*puconfig_method |= uconfig_method;
-			}
-
-			memcpy(pmlmepriv->wps_probe_resp_ie, wps_ie, wps_ielen);
-			pmlmepriv->wps_probe_resp_ie_len = wps_ielen;
-
-		}
-
-		/* buf += wps_ielen; */
-		/* len -= wps_ielen; */
-
-#ifdef CONFIG_8723AU_P2P
-		p2p_ie = rtw_get_p2p_ie23a(buf, len, NULL, &p2p_ielen);
-		if (p2p_ie) {
-			u8 is_GO = false;
-			u32 attr_contentlen = 0;
-			u16 cap_attr = 0;
-
-#ifdef CONFIG_DEBUG_CFG80211
-			DBG_8723A("probe_resp_p2p_ielen =%d\n", p2p_ielen);
-#endif
-
-			/* Check P2P Capability ATTR */
-			if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen,
-						     P2P_ATTR_CAPABILITY,
-						     (u8 *) &cap_attr,
-						     (uint *) &attr_contentlen)) {
-				u8 grp_cap = 0;
-				/* DBG_8723A( "[%s] Got P2P Capability Attr!!\n", __func__ ); */
-				cap_attr = le16_to_cpu(cap_attr);
-				grp_cap = (u8) ((cap_attr >> 8) & 0xff);
-
-				is_GO = (grp_cap & BIT(0)) ? true : false;
-
-				if (is_GO)
-					DBG_8723A
-					    ("Got P2P Capability Attr, grp_cap"
-					     "= 0x%x, is_GO\n", grp_cap);
-			}
-
-			if (is_GO == false) {
-				if (pmlmepriv->p2p_probe_resp_ie) {
-					pmlmepriv->p2p_probe_resp_ie_len = 0;
-					kfree(pmlmepriv->p2p_probe_resp_ie);
-					pmlmepriv->p2p_probe_resp_ie = NULL;
-				}
-
-				pmlmepriv->p2p_probe_resp_ie =
-				    kmalloc(p2p_ielen, GFP_KERNEL);
-				if (pmlmepriv->p2p_probe_resp_ie == NULL) {
-					DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-						  __func__, __LINE__);
-					return -EINVAL;
-				}
-				memcpy(pmlmepriv->p2p_probe_resp_ie, p2p_ie,
-				       p2p_ielen);
-				pmlmepriv->p2p_probe_resp_ie_len = p2p_ielen;
-			} else {
-				if (pmlmepriv->p2p_go_probe_resp_ie) {
-					pmlmepriv->p2p_go_probe_resp_ie_len = 0;
-					kfree(pmlmepriv->p2p_go_probe_resp_ie);
-					pmlmepriv->p2p_go_probe_resp_ie = NULL;
-				}
-
-				pmlmepriv->p2p_go_probe_resp_ie =
-				    kmalloc(p2p_ielen, GFP_KERNEL);
-				if (pmlmepriv->p2p_go_probe_resp_ie == NULL) {
-					DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-						  __func__, __LINE__);
-					return -EINVAL;
-
-				}
-				memcpy(pmlmepriv->p2p_go_probe_resp_ie,
-				       p2p_ie, p2p_ielen);
-				pmlmepriv->p2p_go_probe_resp_ie_len = p2p_ielen;
-			}
-		}
-#endif /* CONFIG_8723AU_P2P */
-
-		/* buf += p2p_ielen; */
-		/* len -= p2p_ielen; */
-
-#ifdef CONFIG_8723AU_P2P
-		if (rtw_get_wfd_ie(buf, len, NULL, &wfd_ielen)) {
-#ifdef CONFIG_DEBUG_CFG80211
-			DBG_8723A("probe_resp_wfd_ielen =%d\n", wfd_ielen);
-#endif
-
-			if (pmlmepriv->wfd_probe_resp_ie) {
-				pmlmepriv->wfd_probe_resp_ie_len = 0;
-				kfree(pmlmepriv->wfd_probe_resp_ie);
-				pmlmepriv->wfd_probe_resp_ie = NULL;
-			}
-
-			pmlmepriv->wfd_probe_resp_ie =
-			    kmalloc(wfd_ielen, GFP_KERNEL);
-			if (pmlmepriv->wfd_probe_resp_ie == NULL) {
-				DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-					  __func__, __LINE__);
-				return -EINVAL;
-
-			}
-			rtw_get_wfd_ie(buf, len, pmlmepriv->wfd_probe_resp_ie,
-				       &pmlmepriv->wfd_probe_resp_ie_len);
-		}
-#endif /* CONFIG_8723AU_P2P */
-	}
-
-	return ret;
-}
-
-static int rtw_cfg80211_set_assoc_resp_wpsp2pie(struct net_device *net,
-						char *buf, int len)
-{
-	int ret = 0;
-	struct rtw_adapter *padapter = netdev_priv(net);
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-
-	DBG_8723A("%s, ielen =%d\n", __func__, len);
-
-	if (len > 0) {
-		if (pmlmepriv->wps_assoc_resp_ie) {
-			pmlmepriv->wps_assoc_resp_ie_len = 0;
-			kfree(pmlmepriv->wps_assoc_resp_ie);
-			pmlmepriv->wps_assoc_resp_ie = NULL;
-		}
-
-		pmlmepriv->wps_assoc_resp_ie = kmalloc(len, GFP_KERNEL);
-		if (pmlmepriv->wps_assoc_resp_ie == NULL) {
-			DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-				  __func__, __LINE__);
-			return -EINVAL;
-
-		}
-		memcpy(pmlmepriv->wps_assoc_resp_ie, buf, len);
-		pmlmepriv->wps_assoc_resp_ie_len = len;
-	}
-
-	return ret;
-}
-
-int rtw_cfg80211_set_mgnt_wpsp2pie(struct net_device *net, char *buf, int len,
-				   int type)
-{
-	int ret = 0;
-	uint wps_ielen = 0;
-#ifdef CONFIG_8723AU_P2P
-	u32 p2p_ielen = 0;
-#endif
-
-#ifdef CONFIG_DEBUG_CFG80211
-	DBG_8723A("%s, ielen =%d\n", __func__, len);
-#endif
-
-	if ((rtw_get_wps_ie23a(buf, len, NULL, &wps_ielen) && (wps_ielen > 0))
-#ifdef CONFIG_8723AU_P2P
-	    || (rtw_get_p2p_ie23a(buf, len, NULL, &p2p_ielen) && (p2p_ielen > 0))
-#endif
-	    ) {
-		if (net) {
-			switch (type) {
-			case 0x1:	/* BEACON */
-				ret =
-				    rtw_cfg80211_set_beacon_wpsp2pie(net, buf,
-								     len);
-				break;
-			case 0x2:	/* PROBE_RESP */
-				ret =
-				    rtw_cfg80211_set_probe_resp_wpsp2pie(net,
-									 buf,
-									 len);
-				break;
-			case 0x4:	/* ASSOC_RESP */
-				ret =
-				    rtw_cfg80211_set_assoc_resp_wpsp2pie(net,
-									 buf,
-									 len);
-				break;
-			}
-		}
-	}
-
-	return ret;
-
-}
-
 static struct cfg80211_ops rtw_cfg80211_ops = {
 	.change_virtual_intf = cfg80211_rtw_change_iface,
 	.add_key = cfg80211_rtw_add_key,
@@ -4280,11 +3214,6 @@ static struct cfg80211_ops rtw_cfg80211_ops = {
 	.change_bss = cfg80211_rtw_change_bss,
 #endif /* CONFIG_8723AU_AP_MODE */
 
-#ifdef CONFIG_8723AU_P2P
-	.remain_on_channel = cfg80211_rtw_remain_on_channel,
-	.cancel_remain_on_channel = cfg80211_rtw_cancel_remain_on_channel,
-#endif
-
 	.mgmt_tx = cfg80211_rtw_mgmt_tx,
 	.mgmt_frame_register = cfg80211_rtw_mgmt_frame_register,
 };
@@ -4348,7 +3277,7 @@ void rtw_cfg80211_init_wiphy(struct rtw_adapter *padapter)
 	struct wireless_dev *pwdev = padapter->rtw_wdev;
 	struct wiphy *wiphy = pwdev->wiphy;
 
-	rtw23a_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+	rf_type = rtl8723a_get_rf_type(padapter);
 
 	DBG_8723A("%s:rf_type =%d\n", __func__, rf_type);
 
@@ -4388,9 +3317,6 @@ static void rtw_cfg80211_preinit_wiphy(struct rtw_adapter *padapter,
 #ifdef CONFIG_8723AU_AP_MODE
 	    BIT(NL80211_IFTYPE_AP) | BIT(NL80211_IFTYPE_MONITOR) |
 #endif
-#if defined(CONFIG_8723AU_P2P)
-	    BIT(NL80211_IFTYPE_P2P_CLIENT) | BIT(NL80211_IFTYPE_P2P_GO) |
-#endif
 	    0;
 
 #ifdef CONFIG_8723AU_AP_MODE
@@ -4440,22 +3366,24 @@ int rtw_wdev_alloc(struct rtw_adapter *padapter, struct device *dev)
 		ret = -ENOMEM;
 		goto exit;
 	}
+
+	/*  wdev */
+	wdev = kzalloc(sizeof(struct wireless_dev), GFP_KERNEL);
+	if (!wdev) {
+		DBG_8723A("Couldn't allocate wireless device\n");
+		ret = -ENOMEM;
+		goto free_wiphy;
+	}
+
 	set_wiphy_dev(wiphy, dev);
 	rtw_cfg80211_preinit_wiphy(padapter, wiphy);
 
 	ret = wiphy_register(wiphy);
 	if (ret < 0) {
 		DBG_8723A("Couldn't register wiphy device\n");
-		goto free_wiphy;
+		goto free_wdev;
 	}
 
-	/*  wdev */
-	wdev = kzalloc(sizeof(struct wireless_dev), GFP_KERNEL);
-	if (!wdev) {
-		DBG_8723A("Couldn't allocate wireless device\n");
-		ret = -ENOMEM;
-		goto unregister_wiphy;
-	}
 	wdev->wiphy = wiphy;
 	wdev->netdev = pnetdev;
 	/* wdev->iftype = NL80211_IFTYPE_STATION; */
@@ -4474,8 +3402,6 @@ int rtw_wdev_alloc(struct rtw_adapter *padapter, struct device *dev)
 	spin_lock_init(&pwdev_priv->scan_req_lock);
 
 	pwdev_priv->p2p_enabled = false;
-	pwdev_priv->provdisc_req_issued = false;
-	rtw_wdev_invit_info_init(&pwdev_priv->invit_info);
 
 	if (padapter->registrypriv.power_mgnt != PS_MODE_ACTIVE)
 		pwdev_priv->power_mgmt = true;
@@ -4483,8 +3409,8 @@ int rtw_wdev_alloc(struct rtw_adapter *padapter, struct device *dev)
 		pwdev_priv->power_mgmt = false;
 
 	return ret;
-unregister_wiphy:
-	wiphy_unregister(wiphy);
+free_wdev:
+	kfree(wdev);
 free_wiphy:
 	wiphy_free(wiphy);
 exit:
@@ -4493,15 +3419,11 @@ exit:
 
 void rtw_wdev_free(struct wireless_dev *wdev)
 {
-	struct rtw_wdev_priv *pwdev_priv;
-
 	DBG_8723A("%s(wdev =%p)\n", __func__, wdev);
 
 	if (!wdev)
 		return;
 
-	pwdev_priv = wdev_to_priv(wdev);
-
 	kfree(wdev->wiphy->bands[IEEE80211_BAND_2GHZ]);
 	kfree(wdev->wiphy->bands[IEEE80211_BAND_5GHZ]);
 
diff --git a/drivers/staging/rtl8723au/os_dep/mlme_linux.c b/drivers/staging/rtl8723au/os_dep/mlme_linux.c
index b30d4d37556..ac618fb8b76 100644
--- a/drivers/staging/rtl8723au/os_dep/mlme_linux.c
+++ b/drivers/staging/rtl8723au/os_dep/mlme_linux.c
@@ -20,22 +20,6 @@
 #include <mlme_osdep.h>
 #include <rtw_ioctl_set.h>
 
-void rtw_os_indicate_connect23a(struct rtw_adapter *adapter)
-{
-	rtw_cfg80211_indicate_connect(adapter);
-
-	netif_carrier_on(adapter->pnetdev);
-
-	if (adapter->pid[2] != 0)
-		rtw_signal_process(adapter->pid[2], SIGALRM);
-}
-
-void rtw_os_indicate_scan_done23a(struct rtw_adapter *padapter, bool aborted)
-{
-	rtw_cfg80211_indicate_scan_done(wdev_to_priv(padapter->rtw_wdev),
-					aborted);
-}
-
 static struct rt_pmkid_list backupPMKIDList[NUM_PMKID_CACHE];
 
 void rtw_reset_securitypriv23a(struct rtw_adapter *adapter)
@@ -52,9 +36,6 @@ void rtw_reset_securitypriv23a(struct rtw_adapter *adapter)
 		 *  When the countermeasure is trigger, the driver have to
 		 *  disconnect with AP for 60 seconds.
 		 */
-		memset(&backupPMKIDList[0], 0x00, sizeof(struct rt_pmkid_list) *
-		       NUM_PMKID_CACHE);
-
 		memcpy(&backupPMKIDList[0], &adapter->securitypriv.PMKIDList[0],
 		       sizeof(struct rt_pmkid_list) * NUM_PMKID_CACHE);
 		backupPMKIDIndex = adapter->securitypriv.PMKIDIndex;
@@ -79,10 +60,10 @@ void rtw_reset_securitypriv23a(struct rtw_adapter *adapter)
 
 		/* open system */
 		psec_priv->dot11AuthAlgrthm = dot11AuthAlgrthm_Open;
-		psec_priv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
+		psec_priv->dot11PrivacyAlgrthm = 0;
 		psec_priv->dot11PrivacyKeyIndex = 0;
 
-		psec_priv->dot118021XGrpPrivacy = _NO_PRIVACY_;
+		psec_priv->dot118021XGrpPrivacy = 0;
 		psec_priv->dot118021XGrpKeyid = 1;
 
 		psec_priv->ndisauthtype = Ndis802_11AuthModeOpen;
@@ -99,89 +80,3 @@ void rtw_os_indicate_disconnect23a(struct rtw_adapter *adapter)
 
 	rtw_reset_securitypriv23a(adapter);
 }
-
-void rtw_report_sec_ie23a(struct rtw_adapter *adapter, u8 authmode, u8 *sec_ie)
-{
-	uint	len;
-	u8	*buff, *p, i;
-	union iwreq_data wrqu;
-
-	RT_TRACE(_module_mlme_osdep_c_, _drv_info_,
-		 ("+rtw_report_sec_ie23a, authmode =%d\n", authmode));
-
-	buff = NULL;
-	if (authmode == _WPA_IE_ID_) {
-		RT_TRACE(_module_mlme_osdep_c_, _drv_info_,
-			 ("rtw_report_sec_ie23a, authmode =%d\n", authmode));
-
-		buff = kzalloc(IW_CUSTOM_MAX, GFP_KERNEL);
-		if (!buff)
-			return;
-		p = buff;
-
-		p += sprintf(p, "ASSOCINFO(ReqIEs =");
-
-		len = sec_ie[1]+2;
-		len =  (len < IW_CUSTOM_MAX) ? len : IW_CUSTOM_MAX;
-
-		for (i = 0; i < len; i++)
-			p += sprintf(p, "%02x", sec_ie[i]);
-
-		p += sprintf(p, ")");
-
-		memset(&wrqu, 0, sizeof(wrqu));
-
-		wrqu.data.length = p-buff;
-
-		wrqu.data.length = (wrqu.data.length < IW_CUSTOM_MAX) ?
-				   wrqu.data.length : IW_CUSTOM_MAX;
-
-		kfree(buff);
-	}
-}
-
-#ifdef CONFIG_8723AU_AP_MODE
-void rtw_indicate_sta_assoc_event23a(struct rtw_adapter *padapter,
-				  struct sta_info *psta)
-{
-	struct sta_priv *pstapriv = &padapter->stapriv;
-	union iwreq_data wrqu;
-
-	if (psta == NULL)
-		return;
-
-	if (psta->aid > NUM_STA)
-		return;
-
-	if (pstapriv->sta_aid[psta->aid - 1] != psta)
-		return;
-
-	wrqu.addr.sa_family = ARPHRD_ETHER;
-
-	memcpy(wrqu.addr.sa_data, psta->hwaddr, ETH_ALEN);
-
-	DBG_8723A("+rtw_indicate_sta_assoc_event23a\n");
-}
-
-void rtw_indicate_sta_disassoc_event23a(struct rtw_adapter *padapter,
-				     struct sta_info *psta)
-{
-	struct sta_priv *pstapriv = &padapter->stapriv;
-	union iwreq_data wrqu;
-
-	if (psta == NULL)
-		return;
-
-	if (psta->aid > NUM_STA)
-		return;
-
-	if (pstapriv->sta_aid[psta->aid - 1] != psta)
-		return;
-
-	wrqu.addr.sa_family = ARPHRD_ETHER;
-
-	memcpy(wrqu.addr.sa_data, psta->hwaddr, ETH_ALEN);
-
-	DBG_8723A("+rtw_indicate_sta_disassoc_event23a\n");
-}
-#endif
diff --git a/drivers/staging/rtl8723au/os_dep/os_intfs.c b/drivers/staging/rtl8723au/os_dep/os_intfs.c
index 57eca7a4567..4e32003a443 100644
--- a/drivers/staging/rtl8723au/os_dep/os_intfs.c
+++ b/drivers/staging/rtl8723au/os_dep/os_intfs.c
@@ -20,10 +20,8 @@
 #include <recv_osdep.h>
 #include <hal_intf.h>
 #include <rtw_version.h>
-#include <ethernet.h>
 
-#include <usb_osintf.h>
-#include <linux/version.h>
+#include <rtl8723a_hal.h>
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("Realtek Wireless Lan Driver");
@@ -66,10 +64,10 @@ static int rtw_acm_method;/*  0:By SW 1:By HW. */
 static int rtw_wmm_enable = 1;/*  default is set to enable the wmm. */
 static int rtw_uapsd_enable;
 
-int rtw_ht_enable23A = 1;
+static int rtw_ht_enable = 1;
 /* 0 :diable, bit(0): enable 2.4g, bit(1): enable 5g */
-int rtw_cbw40_enable23A = 3;
-int rtw_ampdu_enable23A = 1;/* for enable tx_ampdu */
+static int rtw_cbw40_enable = 3;
+static int rtw_ampdu_enable = 1;/* for enable tx_ampdu */
 /*  0: disable, bit(0):enable 2.4g, bit(1):enable 5g, default is set to enable
  * 2.4GHZ for IOT issue with bufflao's AP at 5GHZ
  */
@@ -130,9 +128,9 @@ module_param(rtw_wmm_enable, int, 0644);
 module_param(rtw_vrtl_carrier_sense, int, 0644);
 module_param(rtw_vcs_type, int, 0644);
 module_param(rtw_busy_thresh, int, 0644);
-module_param(rtw_ht_enable23A, int, 0644);
-module_param(rtw_cbw40_enable23A, int, 0644);
-module_param(rtw_ampdu_enable23A, int, 0644);
+module_param(rtw_ht_enable, int, 0644);
+module_param(rtw_cbw40_enable, int, 0644);
+module_param(rtw_ampdu_enable, int, 0644);
 module_param(rtw_rx_stbc, int, 0644);
 module_param(rtw_ampdu_amsdu, int, 0644);
 
@@ -172,10 +170,10 @@ MODULE_PARM_DESC(debug, "Set debug level (1-9) (default 1)");
 
 static int netdev_close(struct net_device *pnetdev);
 
-static uint loadparam(struct rtw_adapter *padapter,  struct net_device *pnetdev)
+static int loadparam(struct rtw_adapter *padapter,  struct net_device *pnetdev)
 {
 	struct registry_priv  *registry_par = &padapter->registrypriv;
-	uint status = _SUCCESS;
+	int status = _SUCCESS;
 
 	GlobalDebugLevel23A = rtw_debug;
 	registry_par->chip_version = (u8)rtw_chip_version;
@@ -203,9 +201,9 @@ static uint loadparam(struct rtw_adapter *padapter,  struct net_device *pnetdev)
 	 /* UAPSD */
 	registry_par->wmm_enable = (u8)rtw_wmm_enable;
 	registry_par->uapsd_enable = (u8)rtw_uapsd_enable;
-	registry_par->ht_enable = (u8)rtw_ht_enable23A;
-	registry_par->cbw40_enable = (u8)rtw_cbw40_enable23A;
-	registry_par->ampdu_enable = (u8)rtw_ampdu_enable23A;
+	registry_par->ht_enable = (u8)rtw_ht_enable;
+	registry_par->cbw40_enable = (u8)rtw_cbw40_enable;
+	registry_par->ampdu_enable = (u8)rtw_ampdu_enable;
 	registry_par->rx_stbc = (u8)rtw_rx_stbc;
 	registry_par->ampdu_amsdu = (u8)rtw_ampdu_amsdu;
 	registry_par->lowrate_two_xmit = (u8)rtw_lowrate_two_xmit;
@@ -274,9 +272,9 @@ static struct net_device_stats *rtw_net_get_stats(struct net_device *pnetdev)
 static const u16 rtw_1d_to_queue[8] = { 2, 3, 3, 2, 1, 1, 0, 0 };
 
 /* Given a data frame determine the 802.1p/1d tag to use. */
-static unsigned int rtw_classify8021d(struct sk_buff *skb)
+static u32 rtw_classify8021d(struct sk_buff *skb)
 {
-	unsigned int dscp;
+	u32 dscp;
 
 	/* skb->priority values from 256->263 are magic values to
 	 * directly indicate a specific 802.1d priority.  This is used
@@ -312,14 +310,14 @@ static u16 rtw_select_queue(struct net_device *dev, struct sk_buff *skb,
 u16 rtw_recv_select_queue23a(struct sk_buff *skb)
 {
 	struct iphdr *piphdr;
+	struct ethhdr *eth = (struct ethhdr *)skb->data;
 	unsigned int dscp;
-	u16 eth_type;
+	u16 eth_type = get_unaligned_be16(&eth->h_proto);
 	u32 priority;
 	u8 *pdata = skb->data;
 
-	memcpy(&eth_type, pdata + (ETH_ALEN << 1), 2);
 	switch (eth_type) {
-	case htons(ETH_P_IP):
+	case ETH_P_IP:
 		piphdr = (struct iphdr *)(pdata + ETH_HLEN);
 		dscp = piphdr->tos & 0xfc;
 		priority = dscp >> 5;
@@ -378,42 +376,13 @@ struct net_device *rtw_init_netdev23a(struct rtw_adapter *old_padapter)
 	return pnetdev;
 }
 
-u32 rtw_start_drv_threads23a(struct rtw_adapter *padapter)
-{
-	u32 _status = _SUCCESS;
-
-	RT_TRACE(_module_os_intfs_c_, _drv_info_,
-		 ("+rtw_start_drv_threads23a\n"));
-	padapter->cmdThread = kthread_run(rtw_cmd_thread23a, padapter,
-					  "RTW_CMD_THREAD");
-	if (IS_ERR(padapter->cmdThread)) {
-		_status = _FAIL;
-	} else {
-		/* wait for cmd_thread to run */
-		down(&padapter->cmdpriv.terminate_cmdthread_sema);
-	}
-	rtw_hal_start_thread23a(padapter);
-	return _status;
-}
-
-void rtw_stop_drv_threads23a(struct rtw_adapter *padapter)
-{
-	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+rtw_stop_drv_threads23a\n"));
-
-	/* Below is to termindate rtw_cmd_thread23a & event_thread... */
-	up(&padapter->cmdpriv.cmd_queue_sema);
-	if (padapter->cmdThread)
-		down(&padapter->cmdpriv.terminate_cmdthread_sema);
-	rtw_hal_stop_thread23a(padapter);
-}
-
-static u8 rtw_init_default_value(struct rtw_adapter *padapter)
+static int rtw_init_default_value(struct rtw_adapter *padapter)
 {
 	struct registry_priv *pregistrypriv = &padapter->registrypriv;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
-	u8 ret = _SUCCESS;
+	int ret = _SUCCESS;
 
 	/* xmit_priv */
 	pxmitpriv->vcs_setting = pregistrypriv->vrtl_carrier_sense;
@@ -430,15 +399,15 @@ static u8 rtw_init_default_value(struct rtw_adapter *padapter)
 	pmlmepriv->htpriv.ampdu_enable = false;/* set to disabled */
 
 	/* security_priv */
-	psecuritypriv->binstallGrpkey = _FAIL;
+	psecuritypriv->binstallGrpkey = 0;
 
 	 /* open system */
 	psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_Open;
-	psecuritypriv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
+	psecuritypriv->dot11PrivacyAlgrthm = 0;
 
 	psecuritypriv->dot11PrivacyKeyIndex = 0;
 
-	psecuritypriv->dot118021XGrpPrivacy = _NO_PRIVACY_;
+	psecuritypriv->dot118021XGrpPrivacy = 0;
 	psecuritypriv->dot118021XGrpKeyid = 1;
 
 	psecuritypriv->ndisauthtype = Ndis802_11AuthModeOpen;
@@ -449,30 +418,24 @@ static u8 rtw_init_default_value(struct rtw_adapter *padapter)
 	rtw_update_registrypriv_dev_network23a(padapter);
 
 	/* hal_priv */
-	rtw_hal_def_value_init23a(padapter);
+	rtl8723a_init_default_value(padapter);
 
 	/* misc. */
 	padapter->bReadPortCancel = false;
 	padapter->bWritePortCancel = false;
-	padapter->bRxRSSIDisplay = 0;
 	padapter->bNotifyChannelChange = 0;
-#ifdef CONFIG_8723AU_P2P
-	padapter->bShowGetP2PState = 1;
-#endif
 	return ret;
 }
 
-u8 rtw_reset_drv_sw23a(struct rtw_adapter *padapter)
+int rtw_reset_drv_sw23a(struct rtw_adapter *padapter)
 {
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv;
-	u8 ret8 = _SUCCESS;
 
 	/* hal_priv */
-	rtw_hal_def_value_init23a(padapter);
+	rtl8723a_init_default_value(padapter);
 	padapter->bReadPortCancel = false;
 	padapter->bWritePortCancel = false;
-	padapter->bRxRSSIDisplay = 0;
 	pmlmepriv->scan_interval = SCAN_INTERVAL;/*  30*2 sec = 60sec */
 
 	padapter->xmitpriv.tx_pkts = 0;
@@ -482,23 +445,23 @@ u8 rtw_reset_drv_sw23a(struct rtw_adapter *padapter)
 
 	_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY | _FW_UNDER_LINKING);
 
-	rtw_hal_sreset_reset23a_value23a(padapter);
+	rtw_sreset_reset_value(padapter);
 	pwrctrlpriv->pwr_state_check_cnts = 0;
 
 	/* mlmeextpriv */
 	padapter->mlmeextpriv.sitesurvey_res.state = SCAN_DISABLE;
 
 	rtw_set_signal_stat_timer(&padapter->recvpriv);
-	return ret8;
+	return _SUCCESS;
 }
 
-u8 rtw_init_drv_sw23a(struct rtw_adapter *padapter)
+int rtw_init_drv_sw23a(struct rtw_adapter *padapter)
 {
-	u8 ret8 = _SUCCESS;
+	int ret8 = _SUCCESS;
 
 	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+rtw_init_drv_sw23a\n"));
 
-	if ((rtw_init_cmd_priv23a(&padapter->cmdpriv)) == _FAIL) {
+	if (rtw_init_cmd_priv23a(&padapter->cmdpriv) == _FAIL) {
 		RT_TRACE(_module_os_intfs_c_, _drv_err_,
 			 ("\n Can't init cmd_priv\n"));
 		ret8 = _FAIL;
@@ -521,17 +484,6 @@ u8 rtw_init_drv_sw23a(struct rtw_adapter *padapter)
 		goto exit;
 	}
 
-#ifdef CONFIG_8723AU_P2P
-	rtw_init_wifidirect_timers23a(padapter);
-	init_wifidirect_info23a(padapter, P2P_ROLE_DISABLE);
-	reset_global_wifidirect_info23a(padapter);
-	rtw_init_cfg80211_wifidirect_info(padapter);
-#ifdef CONFIG_8723AU_P2P
-	if (rtw_init_wifi_display_info(padapter) == _FAIL)
-		RT_TRACE(_module_os_intfs_c_, _drv_err_,
-			 ("\n Can't init init_wifi_display_info\n"));
-#endif
-#endif /* CONFIG_8723AU_P2P */
 
 	if (init_mlme_ext_priv23a(padapter) == _FAIL) {
 		RT_TRACE(_module_os_intfs_c_, _drv_err_,
@@ -566,10 +518,9 @@ u8 rtw_init_drv_sw23a(struct rtw_adapter *padapter)
 
 	ret8 = rtw_init_default_value(padapter);
 
-	rtw_hal_dm_init23a(padapter);
-	rtw_hal_sw_led_init23a(padapter);
+	rtl8723a_init_dm_priv(padapter);
 
-	rtw_hal_sreset_init23a(padapter);
+	rtw_sreset_init(padapter);
 
 exit:
 
@@ -579,89 +530,60 @@ exit:
 
 void rtw_cancel_all_timer23a(struct rtw_adapter *padapter)
 {
-	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+rtw_cancel_all_timer23a\n"));
+	RT_TRACE(_module_os_intfs_c_, _drv_info_,
+		 ("+rtw_cancel_all_timer23a\n"));
 
 	del_timer_sync(&padapter->mlmepriv.assoc_timer);
 	RT_TRACE(_module_os_intfs_c_, _drv_info_,
-		 ("rtw_cancel_all_timer23a:cancel association timer complete!\n"));
+		 ("%s:cancel association timer complete!\n", __func__));
 
 	del_timer_sync(&padapter->mlmepriv.scan_to_timer);
 	RT_TRACE(_module_os_intfs_c_, _drv_info_,
-		 ("rtw_cancel_all_timer23a:cancel scan_to_timer!\n"));
+		 ("%s:cancel scan_to_timer!\n", __func__));
 
 	del_timer_sync(&padapter->mlmepriv.dynamic_chk_timer);
 	RT_TRACE(_module_os_intfs_c_, _drv_info_,
-		 ("rtw_cancel_all_timer23a:cancel dynamic_chk_timer!\n"));
+		 ("%s:cancel dynamic_chk_timer!\n", __func__));
 
-	/*  cancel sw led timer */
-	rtw_hal_sw_led_deinit23a(padapter);
 	RT_TRACE(_module_os_intfs_c_, _drv_info_,
-		 ("rtw_cancel_all_timer23a:cancel DeInitSwLeds!\n"));
+		 ("%s:cancel DeInitSwLeds!\n", __func__));
 
 	del_timer_sync(&padapter->pwrctrlpriv.pwr_state_check_timer);
 
-#ifdef CONFIG_8723AU_P2P
-	del_timer_sync(&padapter->cfg80211_wdinfo.remain_on_ch_timer);
-#endif /* CONFIG_8723AU_P2P */
-
 	del_timer_sync(&padapter->mlmepriv.set_scan_deny_timer);
 	rtw_clear_scan_deny(padapter);
 	RT_TRACE(_module_os_intfs_c_, _drv_info_,
-		 ("rtw_cancel_all_timer23a:cancel set_scan_deny_timer!\n"));
+		 ("%s:cancel set_scan_deny_timer!\n", __func__));
 
 	del_timer_sync(&padapter->recvpriv.signal_stat_timer);
 	/* cancel dm timer */
-	rtw_hal_dm_deinit23a(padapter);
+	rtl8723a_deinit_dm_priv(padapter);
 }
 
-u8 rtw_free_drv_sw23a(struct rtw_adapter *padapter)
+int rtw_free_drv_sw23a(struct rtw_adapter *padapter)
 {
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo;
-#endif
-
 	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("==>rtw_free_drv_sw23a"));
 
-	/* we can call rtw_p2p_enable23a here, but:
-	 *  1. rtw_p2p_enable23a may have IO operation
-	 *  2. rtw_p2p_enable23a is bundled with wext interface
-	 */
-#ifdef CONFIG_8723AU_P2P
-	pwdinfo = &padapter->wdinfo;
-	if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
-		del_timer_sync(&pwdinfo->find_phase_timer);
-		del_timer_sync(&pwdinfo->restore_p2p_state_timer);
-		del_timer_sync(&pwdinfo->pre_tx_scan_timer);
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_NONE);
-	}
-#endif
-
 	free_mlme_ext_priv23a(&padapter->mlmeextpriv);
 
-	rtw_free_cmd_priv23a(&padapter->cmdpriv);
-
 	rtw_free_evt_priv23a(&padapter->evtpriv);
 
 	rtw_free_mlme_priv23a(&padapter->mlmepriv);
 
 	_rtw_free_xmit_priv23a(&padapter->xmitpriv);
 
-	_rtw_free_sta_priv23a(&padapter->stapriv);/* will free bcmc_stainfo here */
+	/* will free bcmc_stainfo here */
+	_rtw_free_sta_priv23a(&padapter->stapriv);
 
 	_rtw_free_recv_priv23a(&padapter->recvpriv);
 
 	rtw_free_pwrctrl_priv(padapter);
 
-	rtw_hal_free_data23a(padapter);
+	kfree(padapter->HalData);
+	padapter->HalData = NULL;
 
 	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("<== rtw_free_drv_sw23a\n"));
 
-	/* free the old_pnetdev */
-	if (padapter->rereg_nd_name_priv.old_pnetdev) {
-		free_netdev(padapter->rereg_nd_name_priv.old_pnetdev);
-		padapter->rereg_nd_name_priv.old_pnetdev = NULL;
-	}
-
 	/*  clear pbuddy_adapter to avoid access wrong pointer. */
 	if (padapter->pbuddy_adapter != NULL)
 		padapter->pbuddy_adapter->pbuddy_adapter = NULL;
@@ -681,7 +603,7 @@ static int _rtw_drv_register_netdev(struct rtw_adapter *padapter, char *name)
 
 	/* Tell the network stack we exist */
 	if (register_netdev(pnetdev)) {
-		DBG_8723A(FUNC_NDEV_FMT "Failed!\n", FUNC_NDEV_ARG(pnetdev));
+		DBG_8723A("%s(%s): Failed!\n", __func__, pnetdev->name);
 		ret = _FAIL;
 		goto error_register_netdev;
 	}
@@ -704,26 +626,30 @@ int rtw_drv_register_netdev(struct rtw_adapter *if1)
 	struct dvobj_priv *dvobj = if1->dvobj;
 	int i, status = _SUCCESS;
 
-	if (dvobj->iface_nums < IFACE_ID_MAX) {
-		for (i = 0; i < dvobj->iface_nums; i++) {
-			struct rtw_adapter *padapter = dvobj->padapters[i];
-
-			if (padapter) {
-				char *name;
-
-				if (padapter->iface_id == IFACE_ID0)
-					name = if1->registrypriv.ifname;
-				else if (padapter->iface_id == IFACE_ID1)
-					name = if1->registrypriv.if2name;
-				else
-					name = "wlan%d";
-				status = _rtw_drv_register_netdev(padapter,
-								  name);
-				if (status != _SUCCESS)
-					break;
-			}
+	if (dvobj->iface_nums >= IFACE_ID_MAX) {
+		status = _FAIL; /* -EINVAL */
+		goto exit;
+	}
+
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		struct rtw_adapter *padapter = dvobj->padapters[i];
+
+		if (padapter) {
+			char *name;
+
+			if (padapter->iface_id == IFACE_ID0)
+				name = if1->registrypriv.ifname;
+			else if (padapter->iface_id == IFACE_ID1)
+				name = if1->registrypriv.if2name;
+			else
+				name = "wlan%d";
+			status = _rtw_drv_register_netdev(padapter, name);
+			if (status != _SUCCESS)
+				break;
 		}
 	}
+
+exit:
 	return status;
 }
 
@@ -732,7 +658,7 @@ int netdev_open23a(struct net_device *pnetdev)
 	struct rtw_adapter *padapter = netdev_priv(pnetdev);
 	struct pwrctrl_priv *pwrctrlpriv;
 	int ret = 0;
-	uint status;
+	int status;
 
 	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+871x_drv - dev_open\n"));
 	DBG_8723A("+871x_drv - drv_open, bup =%d\n", padapter->bup);
@@ -760,19 +686,12 @@ int netdev_open23a(struct net_device *pnetdev)
 		DBG_8723A("MAC Address = "MAC_FMT"\n",
 			  MAC_ARG(pnetdev->dev_addr));
 
-		status = rtw_start_drv_threads23a(padapter);
-		if (status == _FAIL) {
-			DBG_8723A("Initialize driver software resource Failed!\n");
-			goto netdev_open23a_error;
-		}
-
 		if (init_hw_mlme_ext23a(padapter) == _FAIL) {
 			DBG_8723A("can't init mlme_ext_priv\n");
 			goto netdev_open23a_error;
 		}
 
-		if (padapter->intf_start)
-			padapter->intf_start(padapter);
+		rtl8723au_inirp_init(padapter);
 
 		rtw_cfg80211_init_wiphy(padapter);
 
@@ -816,7 +735,7 @@ netdev_open23a_error:
 	goto exit;
 }
 
-static int  ips_netdrv_open(struct rtw_adapter *padapter)
+static int ips_netdrv_open(struct rtw_adapter *padapter)
 {
 	int status = _SUCCESS;
 
@@ -834,8 +753,7 @@ static int  ips_netdrv_open(struct rtw_adapter *padapter)
 		goto netdev_open23a_error;
 	}
 
-	if (padapter->intf_start)
-		padapter->intf_start(padapter);
+	rtl8723au_inirp_init(padapter);
 
 	rtw_set_pwr_state_check_timer(&padapter->pwrctrlpriv);
 	mod_timer(&padapter->mlmepriv.dynamic_chk_timer,
@@ -887,10 +805,9 @@ void rtw_ips_pwr_down23a(struct rtw_adapter *padapter)
 
 void rtw_ips_dev_unload23a(struct rtw_adapter *padapter)
 {
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_FIFO_CLEARN_UP, NULL);
+	rtl8723a_fifo_cleanup(padapter);
 
-	if (padapter->intf_stop)
-		padapter->intf_stop(padapter);
+	rtl8723a_usb_intf_stop(padapter);
 
 	/* s5. */
 	if (!padapter->bSurpriseRemoved)
@@ -916,15 +833,11 @@ static int netdev_close(struct net_device *pnetdev)
 
 	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+871x_drv - drv_close\n"));
 
-	if (padapter->pwrctrlpriv.bInternalAutoSuspend) {
-		if (padapter->pwrctrlpriv.rf_pwrstate == rf_off)
-			padapter->pwrctrlpriv.ps_flag = true;
-	}
 	padapter->net_closed = true;
 
 	if (padapter->pwrctrlpriv.rf_pwrstate == rf_on) {
-		DBG_8723A("(2)871x_drv - drv_close, bup =%d, hw_init_completed =%d\n",
-			  padapter->bup,
+		DBG_8723A("(2)871x_drv - drv_close, bup =%d, "
+			  "hw_init_completed =%d\n", padapter->bup,
 			  padapter->hw_init_completed);
 
 		/* s1. */
@@ -941,20 +854,12 @@ static int netdev_close(struct net_device *pnetdev)
 		/* s2-3. */
 		rtw_free_assoc_resources23a(padapter, 1);
 		/* s2-4. */
-		rtw_free_network_queue23a(padapter, true);
+		rtw_free_network_queue23a(padapter);
 		/*  Close LED */
 		rtw_led_control(padapter, LED_CTL_POWER_OFF);
 	}
 
-#ifdef CONFIG_8723AU_P2P
-	if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled)
-		wdev_to_priv(padapter->rtw_wdev)->p2p_enabled = false;
-	rtw_p2p_enable23a(padapter, P2P_ROLE_DISABLE);
-#endif /* CONFIG_8723AU_P2P */
-
 	rtw_scan_abort23a(padapter);
-	 /* set this at the end */
-	padapter->rtw_wdev->iftype = NL80211_IFTYPE_MONITOR;
 
 	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("-871x_drv - drv_close\n"));
 	DBG_8723A("-871x_drv - drv_close, bup =%d\n", padapter->bup);
@@ -964,7 +869,13 @@ static int netdev_close(struct net_device *pnetdev)
 
 void rtw_ndev_destructor(struct net_device *ndev)
 {
-	DBG_8723A(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 	kfree(ndev->ieee80211_ptr);
 	free_netdev(ndev);
 }
+
+void _rtw_init_queue23a(struct rtw_queue *pqueue)
+{
+	INIT_LIST_HEAD(&pqueue->queue);
+	spin_lock_init(&pqueue->lock);
+}
diff --git a/drivers/staging/rtl8723au/os_dep/osdep_service.c b/drivers/staging/rtl8723au/os_dep/osdep_service.c
deleted file mode 100644
index 97fc27dce19..00000000000
--- a/drivers/staging/rtl8723au/os_dep/osdep_service.c
+++ /dev/null
@@ -1,175 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2012 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.
- *
- ******************************************************************************/
-
-
-#define _OSDEP_SERVICE_C_
-
-#include <osdep_service.h>
-#include <drv_types.h>
-#include <recv_osdep.h>
-#include <linux/vmalloc.h>
-
-#define RT_TAG	('1178')
-
-/*
-* Translate the OS dependent @param error_code to OS independent RTW_STATUS_CODE23a
-* @return: one of RTW_STATUS_CODE23a
-*/
-inline int RTW_STATUS_CODE23a(int error_code)
-{
-	if (error_code >= 0)
-		return _SUCCESS;
-	return _FAIL;
-}
-
-inline u8 *_rtw_vmalloc(u32 sz)
-{
-	u8	*pbuf;
-	pbuf = vmalloc(sz);
-
-	return pbuf;
-}
-
-inline u8 *_rtw_zvmalloc(u32 sz)
-{
-	u8	*pbuf;
-	pbuf = _rtw_vmalloc(sz);
-	if (pbuf != NULL)
-		memset(pbuf, 0, sz);
-
-	return pbuf;
-}
-
-inline void _rtw_vmfree(u8 *pbuf, u32 sz)
-{
-	vfree(pbuf);
-}
-
-void _rtw_init_queue23a(struct rtw_queue *pqueue)
-{
-	INIT_LIST_HEAD(&pqueue->queue);
-	spin_lock_init(&pqueue->lock);
-}
-
-u32 _rtw_queue_empty23a(struct rtw_queue *pqueue)
-{
-	if (list_empty(&pqueue->queue))
-		return true;
-	else
-		return false;
-}
-
-u64 rtw_modular6423a(u64 x, u64 y)
-{
-	return do_div(x, y);
-}
-
-u64 rtw_division6423a(u64 x, u64 y)
-{
-	do_div(x, y);
-	return x;
-}
-
-/* rtw_cbuf_full23a - test if cbuf is full
- * @cbuf: pointer of struct rtw_cbuf
- *
- * Returns: true if cbuf is full
- */
-inline bool rtw_cbuf_full23a(struct rtw_cbuf *cbuf)
-{
-	return (cbuf->write == cbuf->read-1) ? true : false;
-}
-
-/* rtw_cbuf_empty23a - test if cbuf is empty
- * @cbuf: pointer of struct rtw_cbuf
- *
- * Returns: true if cbuf is empty
- */
-inline bool rtw_cbuf_empty23a(struct rtw_cbuf *cbuf)
-{
-	return (cbuf->write == cbuf->read) ? true : false;
-}
-
-/**
- * rtw_cbuf_push23a - push a pointer into cbuf
- * @cbuf: pointer of struct rtw_cbuf
- * @buf: pointer to push in
- *
- * Lock free operation, be careful of the use scheme
- * Returns: true push success
- */
-bool rtw_cbuf_push23a(struct rtw_cbuf *cbuf, void *buf)
-{
-	if (rtw_cbuf_full23a(cbuf))
-		return _FAIL;
-
-	if (0)
-		DBG_8723A("%s on %u\n", __func__, cbuf->write);
-	cbuf->bufs[cbuf->write] = buf;
-	cbuf->write = (cbuf->write+1)%cbuf->size;
-
-	return _SUCCESS;
-}
-
-/**
- * rtw_cbuf_pop23a - pop a pointer from cbuf
- * @cbuf: pointer of struct rtw_cbuf
- *
- * Lock free operation, be careful of the use scheme
- * Returns: pointer popped out
- */
-void *rtw_cbuf_pop23a(struct rtw_cbuf *cbuf)
-{
-	void *buf;
-	if (rtw_cbuf_empty23a(cbuf))
-		return NULL;
-
-	if (0)
-		DBG_8723A("%s on %u\n", __func__, cbuf->read);
-	buf = cbuf->bufs[cbuf->read];
-	cbuf->read = (cbuf->read+1)%cbuf->size;
-
-	return buf;
-}
-
-/**
- * rtw_cbuf_alloc23a - allocte a rtw_cbuf with given size and do initialization
- * @size: size of pointer
- *
- * Returns: pointer of srtuct rtw_cbuf, NULL for allocation failure
- */
-struct rtw_cbuf *rtw_cbuf_alloc23a(u32 size)
-{
-	struct rtw_cbuf *cbuf;
-
-	cbuf = kmalloc(sizeof(*cbuf) + sizeof(void *)*size, GFP_KERNEL);
-
-	if (cbuf) {
-		cbuf->write = 0;
-		cbuf->read = 0;
-		cbuf->size = size;
-	}
-
-	return cbuf;
-}
-
-/**
- * rtw_cbuf_free - free the given rtw_cbuf
- * @cbuf: pointer of struct rtw_cbuf to free
- */
-void rtw_cbuf_free(struct rtw_cbuf *cbuf)
-{
-	kfree(cbuf);
-}
diff --git a/drivers/staging/rtl8723au/os_dep/recv_linux.c b/drivers/staging/rtl8723au/os_dep/recv_linux.c
index 84402a589f2..bcd970baf40 100644
--- a/drivers/staging/rtl8723au/os_dep/recv_linux.c
+++ b/drivers/staging/rtl8723au/os_dep/recv_linux.c
@@ -21,50 +21,9 @@
 #include <recv_osdep.h>
 
 #include <osdep_intf.h>
-#include <ethernet.h>
 
 #include <usb_ops.h>
 
-/* alloc os related resource in struct recv_frame */
-int rtw_os_recv_resource_alloc23a(struct rtw_adapter *padapter,
-			       struct recv_frame *precvframe)
-{
-	int res = _SUCCESS;
-
-	precvframe->pkt = NULL;
-
-	return res;
-}
-
-/* alloc os related resource in struct recv_buf */
-int rtw_os_recvbuf_resource_alloc23a(struct rtw_adapter *padapter,
-				  struct recv_buf *precvbuf)
-{
-	int res = _SUCCESS;
-
-	precvbuf->purb = usb_alloc_urb(0, GFP_KERNEL);
-	if (precvbuf->purb == NULL)
-		res = _FAIL;
-
-	precvbuf->pskb = NULL;
-
-	return res;
-}
-
-/* free os related resource in struct recv_buf */
-int rtw_os_recvbuf_resource_free23a(struct rtw_adapter *padapter,
-				 struct recv_buf *precvbuf)
-{
-	int ret = _SUCCESS;
-
-	usb_free_urb(precvbuf->purb);
-
-	if (precvbuf->pskb)
-		dev_kfree_skb_any(precvbuf->pskb);
-
-	return ret;
-}
-
 void rtw_handle_tkip_mic_err23a(struct rtw_adapter *padapter, u8 bgroup)
 {
 	enum nl80211_key_type key_type = 0;
@@ -110,21 +69,14 @@ void rtw_handle_tkip_mic_err23a(struct rtw_adapter *padapter, u8 bgroup)
 	wrqu.data.length = sizeof(ev);
 }
 
-void rtw_hostapd_mlme_rx23a(struct rtw_adapter *padapter,
-			 struct recv_frame *precv_frame)
-{
-}
-
 int rtw_recv_indicatepkt23a(struct rtw_adapter *padapter,
 			 struct recv_frame *precv_frame)
 {
 	struct recv_priv *precvpriv;
-	struct rtw_queue *pfree_recv_queue;
 	struct sk_buff *skb;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-	precvpriv = &(padapter->recvpriv);
-	pfree_recv_queue = &(precvpriv->free_recv_queue);
+	precvpriv = &padapter->recvpriv;
 
 	skb = precv_frame->pkt;
 	if (!skb) {
@@ -193,7 +145,7 @@ _recv_indicatepkt_end:
 
 	precv_frame->pkt = NULL; /*  pointers to NULL before rtw_free_recvframe23a() */
 
-	rtw_free_recvframe23a(precv_frame, pfree_recv_queue);
+	rtw_free_recvframe23a(precv_frame);
 
 	RT_TRACE(_module_recv_osdep_c_, _drv_info_,
 		 ("\n rtw_recv_indicatepkt23a :after netif_rx!!!!\n"));
@@ -201,22 +153,10 @@ _recv_indicatepkt_end:
 
 _recv_indicatepkt_drop:
 
-	 rtw_free_recvframe23a(precv_frame, pfree_recv_queue);
+	 rtw_free_recvframe23a(precv_frame);
 	 return _FAIL;
 }
 
-void rtw_os_read_port23a(struct rtw_adapter *padapter, struct recv_buf *precvbuf)
-{
-	struct recv_priv *precvpriv = &padapter->recvpriv;
-
-	/* free skb in recv_buf */
-	dev_kfree_skb_any(precvbuf->pskb);
-
-	precvbuf->pskb = NULL;
-
-	rtw_read_port(padapter, precvpriv->ff_hwaddr, 0, precvbuf);
-}
-
 void rtw_init_recv_timer23a(struct recv_reorder_ctrl *preorder_ctrl)
 {
 	setup_timer(&preorder_ctrl->reordering_ctrl_timer,
diff --git a/drivers/staging/rtl8723au/os_dep/usb_intf.c b/drivers/staging/rtl8723au/os_dep/usb_intf.c
index 612806e0de2..8b25c1aa202 100644
--- a/drivers/staging/rtl8723au/os_dep/usb_intf.c
+++ b/drivers/staging/rtl8723au/os_dep/usb_intf.c
@@ -21,10 +21,8 @@
 #include <hal_intf.h>
 #include <rtw_version.h>
 #include <osdep_intf.h>
-#include <usb_vendor_req.h>
 #include <usb_ops.h>
-#include <usb_osintf.h>
-#include <usb_hal.h>
+#include <rtl8723a_hal.h>
 
 static int rtw_suspend(struct usb_interface *intf, pm_message_t message);
 static int rtw_resume(struct usb_interface *intf);
@@ -101,15 +99,15 @@ static inline int RT_usb_endpoint_num(const struct usb_endpoint_descriptor *epd)
 	return epd->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK;
 }
 
-static u8 rtw_init_intf_priv(struct dvobj_priv *dvobj)
+static int rtw_init_intf_priv(struct dvobj_priv *dvobj)
 {
-	u8 rst = _SUCCESS;
+	int rst = _SUCCESS;
 
 	mutex_init(&dvobj->usb_vendor_req_mutex);
 	dvobj->usb_alloc_vendor_req_buf = kzalloc(MAX_USB_IO_CTL_SIZE,
 						  GFP_KERNEL);
 	if (dvobj->usb_alloc_vendor_req_buf == NULL) {
-		DBG_8723A("alloc usb_vendor_req_buf failed... /n");
+		DBG_8723A("alloc usb_vendor_req_buf failed...\n");
 		rst = _FAIL;
 		goto exit;
 	}
@@ -119,9 +117,9 @@ exit:
 	return rst;
 }
 
-static u8 rtw_deinit_intf_priv(struct dvobj_priv *dvobj)
+static int rtw_deinit_intf_priv(struct dvobj_priv *dvobj)
 {
-	u8 rst = _SUCCESS;
+	int rst = _SUCCESS;
 
 	kfree(dvobj->usb_alloc_vendor_req_buf);
 
@@ -229,7 +227,6 @@ static struct dvobj_priv *usb_dvobj_init(struct usb_interface *usb_intf)
 		goto free_dvobj;
 	}
 	/* 3 misc */
-	sema_init(&(pdvobjpriv->usb_suspend_sema), 0);
 	rtw_reset_continual_urb_error(pdvobjpriv);
 	usb_get_dev(pusbd);
 	status = _SUCCESS;
@@ -278,21 +275,7 @@ static void usb_dvobj_deinit(struct usb_interface *usb_intf)
 	usb_put_dev(interface_to_usbdev(usb_intf));
 }
 
-static void decide_chip_type_by_usb_device_id(struct rtw_adapter *padapter,
-					      const struct usb_device_id *pdid)
-{
-	padapter->chip_type = NULL_CHIP_TYPE;
-	hal_set_hw_type(padapter);
-}
-
-static void usb_intf_start(struct rtw_adapter *padapter)
-{
-	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("+usb_intf_start\n"));
-	rtw_hal_inirp_init23a(padapter);
-	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("-usb_intf_start\n"));
-}
-
-static void usb_intf_stop(struct rtw_adapter *padapter)
+void rtl8723a_usb_intf_stop(struct rtw_adapter *padapter)
 {
 	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("+usb_intf_stop\n"));
 
@@ -306,10 +289,10 @@ static void usb_intf_stop(struct rtw_adapter *padapter)
 	}
 
 	/* cancel in irp */
-	rtw_hal_inirp_deinit23a(padapter);
+	rtl8723au_inirp_deinit(padapter);
 
 	/* cancel out irp */
-	rtw_write_port_cancel(padapter);
+	rtl8723au_write_port_cancel(padapter);
 
 	/* todo:cancel other irps */
 	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("-usb_intf_stop\n"));
@@ -328,12 +311,10 @@ static void rtw_dev_unload(struct rtw_adapter *padapter)
 					RTW_SCTX_DONE_DRV_STOP);
 
 		/* s3. */
-		if (padapter->intf_stop)
-			padapter->intf_stop(padapter);
+		rtl8723a_usb_intf_stop(padapter);
 
 		/* s4. */
-		if (!padapter->pwrctrlpriv.bInternalAutoSuspend)
-			rtw_stop_drv_threads23a(padapter);
+		flush_workqueue(padapter->cmdpriv.wq);
 
 		/* s5. */
 		if (!padapter->bSurpriseRemoved) {
@@ -353,6 +334,7 @@ int rtw_hw_suspend23a(struct rtw_adapter *padapter)
 {
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 	struct net_device *pnetdev = padapter->pnetdev;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
 	if ((!padapter->bup) || (padapter->bDriverStopped) ||
 	    (padapter->bSurpriseRemoved)) {
@@ -380,26 +362,22 @@ int rtw_hw_suspend23a(struct rtw_adapter *padapter)
 
 		/* s2-2.  indicate disconnect to os */
 		/* rtw_indicate_disconnect23a(padapter); */
-		{
-			struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+		if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+			_clr_fwstate_(pmlmepriv, _FW_LINKED);
 
-			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
-				_clr_fwstate_(pmlmepriv, _FW_LINKED);
+			rtw_led_control(padapter, LED_CTL_NO_LINK);
 
-				rtw_led_control(padapter, LED_CTL_NO_LINK);
+			rtw_os_indicate_disconnect23a(padapter);
 
-				rtw_os_indicate_disconnect23a(padapter);
-
-				/* donnot enqueue cmd */
-				rtw_lps_ctrl_wk_cmd23a(padapter,
-						    LPS_CTRL_DISCONNECT, 0);
-			}
+			/* donnot enqueue cmd */
+			rtw_lps_ctrl_wk_cmd23a(padapter,
+					       LPS_CTRL_DISCONNECT, 0);
 		}
 		/* s2-3. */
 		rtw_free_assoc_resources23a(padapter, 1);
 
 		/* s2-4. */
-		rtw_free_network_queue23a(padapter, true);
+		rtw_free_network_queue23a(padapter);
 		rtw_ips_dev_unload23a(padapter);
 		pwrpriv->rf_pwrstate = rf_off;
 		pwrpriv->bips_processing = false;
@@ -503,13 +481,14 @@ static int rtw_suspend(struct usb_interface *pusb_intf, pm_message_t message)
 	/* s2-3. */
 	rtw_free_assoc_resources23a(padapter, 1);
 	/* s2-4. */
-	rtw_free_network_queue23a(padapter, true);
+	rtw_free_network_queue23a(padapter);
 
 	rtw_dev_unload(padapter);
 	up(&pwrpriv->lock);
 
 	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY))
-		rtw_indicate_scan_done23a(padapter, 1);
+		rtw_cfg80211_indicate_scan_done(
+			wdev_to_priv(padapter->rtw_wdev), true);
 
 	if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING))
 		rtw_indicate_disconnect23a(padapter);
@@ -525,13 +504,9 @@ static int rtw_resume(struct usb_interface *pusb_intf)
 {
 	struct dvobj_priv *dvobj = usb_get_intfdata(pusb_intf);
 	struct rtw_adapter *padapter = dvobj->if1;
-	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
-	int ret = 0;
+	int ret;
 
-	if (pwrpriv->bInternalAutoSuspend)
-		ret = rtw_resume_process23a(padapter);
-	else
-		ret = rtw_resume_process23a(padapter);
+	ret = rtw_resume_process23a(padapter);
 
 	return ret;
 }
@@ -565,7 +540,7 @@ int rtw_resume_process23a(struct rtw_adapter *padapter)
 
 	if (padapter->pid[1] != 0) {
 		DBG_8723A("pid[1]:%d\n", padapter->pid[1]);
-		rtw_signal_process(padapter->pid[1], SIGUSR2);
+		kill_pid(find_vpid(padapter->pid[1]), SIGUSR2, 1);
 	}
 
 	rtw23a_roaming(padapter, NULL);
@@ -606,8 +581,7 @@ static struct rtw_adapter *rtw_usb_if1_init(struct dvobj_priv *dvobj,
 	dvobj->padapters[dvobj->iface_nums++] = padapter;
 	padapter->iface_id = IFACE_ID0;
 
-	/* step 1-1., decide the chip_type via vid/pid */
-	decide_chip_type_by_usb_device_id(padapter, pdid);
+	rtl8723au_set_hw_type(padapter);
 
 	if (rtw_handle_dualmac23a(padapter, 1) != _SUCCESS)
 		goto free_adapter;
@@ -617,24 +591,19 @@ static struct rtw_adapter *rtw_usb_if1_init(struct dvobj_priv *dvobj,
 	if (rtw_wdev_alloc(padapter, dvobj_to_dev(dvobj)))
 		goto handle_dualmac;
 
-	/* step 2. hook HalFunc, allocate HalData */
-	if (rtl8723au_set_hal_ops(padapter))
-		return NULL;
-
-	padapter->intf_start = &usb_intf_start;
-	padapter->intf_stop = &usb_intf_stop;
-
-	/* step init_io_priv */
-	rtw_init_io_priv23a(padapter, usb_set_intf_ops);
+	/* step 2. allocate HalData */
+	padapter->HalData = kzalloc(sizeof(struct hal_data_8723a), GFP_KERNEL);
+	if (!padapter->HalData)
+		goto free_wdev;
 
 	/* step read_chip_version */
-	rtw_hal_read_chip_version23a(padapter);
+	rtl8723a_read_chip_version(padapter);
 
 	/* step usb endpoint mapping */
-	rtw_hal_chip_configure23a(padapter);
+	rtl8723au_chip_configure(padapter);
 
 	/* step read efuse/eeprom data and get mac_addr */
-	rtw_hal_read_chip_info23a(padapter);
+	rtl8723a_read_adapter_info(padapter);
 
 	/* step 5. */
 	if (rtw_init_drv_sw23a(padapter) == _FAIL) {
@@ -662,12 +631,10 @@ static struct rtw_adapter *rtw_usb_if1_init(struct dvobj_priv *dvobj,
 	padapter->pwrctrlpriv.autopm_cnt = 1;
 #endif
 
-	/*  set mac addr */
-	rtw_macaddr_cfg23a(padapter->eeprompriv.mac_addr);
-#ifdef CONFIG_8723AU_P2P
-	rtw_init_wifidirect_addrs23a(padapter, padapter->eeprompriv.mac_addr,
-				  padapter->eeprompriv.mac_addr);
-#endif
+	/* If the eeprom mac address is corrupted, assign a random address */
+	if (is_broadcast_ether_addr(padapter->eeprompriv.mac_addr) ||
+	    is_zero_ether_addr(padapter->eeprompriv.mac_addr))
+		eth_random_addr(padapter->eeprompriv.mac_addr);
 
 	DBG_8723A("bDriverStopped:%d, bSurpriseRemoved:%d, bup:%d, hw_init_completed:%d\n",
 		  padapter->bDriverStopped, padapter->bSurpriseRemoved,
@@ -678,6 +645,7 @@ static struct rtw_adapter *rtw_usb_if1_init(struct dvobj_priv *dvobj,
 free_hal_data:
 	if (status != _SUCCESS)
 		kfree(padapter->HalData);
+free_wdev:
 	if (status != _SUCCESS) {
 		rtw_wdev_unregister(padapter->rtw_wdev);
 		rtw_wdev_free(padapter->rtw_wdev);
diff --git a/drivers/staging/rtl8723au/os_dep/usb_ops_linux.c b/drivers/staging/rtl8723au/os_dep/usb_ops_linux.c
index c49160e477d..d081449318c 100644
--- a/drivers/staging/rtl8723au/os_dep/usb_ops_linux.c
+++ b/drivers/staging/rtl8723au/os_dep/usb_ops_linux.c
@@ -18,22 +18,6 @@
 #include <usb_ops_linux.h>
 #include <rtw_sreset.h>
 
-unsigned int ffaddr2pipehdl23a(struct dvobj_priv *pdvobj, u32 addr)
-{
-	struct usb_device *pusbd = pdvobj->pusbdev;
-	unsigned int pipe = 0, ep_num = 0;
-
-	if (addr == RECV_BULK_IN_ADDR) {
-		pipe = usb_rcvbulkpipe(pusbd, pdvobj->RtInPipe[0]);
-	} else if (addr == RECV_INT_IN_ADDR) {
-		pipe = usb_rcvbulkpipe(pusbd, pdvobj->RtInPipe[1]);
-	} else if (addr < HW_QUEUE_ENTRY) {
-		ep_num = pdvobj->Queue2Pipe[addr];
-		pipe = usb_sndbulkpipe(pusbd, ep_num);
-	}
-	return pipe;
-}
-
 struct zero_bulkout_context {
 	void *pbuf;
 	void *purb;
@@ -41,18 +25,9 @@ struct zero_bulkout_context {
 	void *padapter;
 };
 
-void usb_read_mem23a(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *rmem)
-{
-}
-
-void usb_write_mem23a(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *wmem)
-{
-}
-
-void usb_read_port_cancel23a(struct intf_hdl *pintfhdl)
+void rtl8723au_read_port_cancel(struct rtw_adapter *padapter)
 {
 	struct recv_buf *precvbuf;
-	struct rtw_adapter *padapter = pintfhdl->padapter;
 	int i;
 
 	precvbuf = (struct recv_buf *)padapter->recvpriv.precv_buf;
@@ -69,7 +44,7 @@ void usb_read_port_cancel23a(struct intf_hdl *pintfhdl)
 	usb_kill_urb(padapter->recvpriv.int_in_urb);
 }
 
-static void usb_write_port23a_complete(struct urb *purb, struct pt_regs *regs)
+static void usb_write_port23a_complete(struct urb *purb)
 {
 	struct xmit_buf *pxmitbuf = (struct xmit_buf *)purb->context;
 	struct rtw_adapter *padapter = pxmitbuf->padapter;
@@ -102,23 +77,25 @@ static void usb_write_port23a_complete(struct urb *purb, struct pt_regs *regs)
 	if (padapter->bSurpriseRemoved || padapter->bDriverStopped ||
 	    padapter->bWritePortCancel) {
 		RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-			 ("usb_write_port23a_complete:bDriverStopped(%d) OR bSurpriseRemoved(%d)",
-			 padapter->bDriverStopped, padapter->bSurpriseRemoved));
-		DBG_8723A("%s(): TX Warning! bDriverStopped(%d) OR bSurpriseRemoved(%d) bWritePortCancel(%d) pxmitbuf->ext_tag(%x)\n",
-			  __func__, padapter->bDriverStopped,
-			  padapter->bSurpriseRemoved, padapter->bReadPortCancel,
-			  pxmitbuf->ext_tag);
+			 ("usb_write_port23a_complete:bDriverStopped(%d) OR "
+			  "bSurpriseRemoved(%d)", padapter->bDriverStopped,
+			  padapter->bSurpriseRemoved));
+		DBG_8723A("%s(): TX Warning! bDriverStopped(%d) OR "
+			  "bSurpriseRemoved(%d) bWritePortCancel(%d) "
+			  "pxmitbuf->ext_tag(%x)\n", __func__,
+			  padapter->bDriverStopped, padapter->bSurpriseRemoved,
+			  padapter->bReadPortCancel, pxmitbuf->ext_tag);
 
 		goto check_completion;
 	}
 
 	if (purb->status) {
 		RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-			 ("usb_write_port23a_complete : purb->status(%d) != 0\n",
-			 purb->status));
+			 ("usb_write_port23a_complete : purb->status(%d) "
+			  "!= 0\n", purb->status));
 		DBG_8723A("###=> urb_write_port_complete status(%d)\n",
 			  purb->status);
-		if ((purb->status == -EPIPE) || (purb->status == -EPROTO)) {
+		if (purb->status == -EPIPE || purb->status == -EPROTO) {
 			sreset_set_wifi_error_status23a(padapter,
 						     USB_WRITE_PORT_FAIL);
 		} else if (purb->status == -EINPROGRESS) {
@@ -136,13 +113,15 @@ static void usb_write_port23a_complete(struct urb *purb, struct pt_regs *regs)
 				 ("usb_write_port23a_complete: ESHUTDOWN\n"));
 			padapter->bDriverStopped = true;
 			RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-				 ("usb_write_port23a_complete:bDriverStopped = true\n"));
+				 ("usb_write_port23a_complete:bDriverStopped "
+				  "= true\n"));
 			goto check_completion;
 		} else {
 			padapter->bSurpriseRemoved = true;
 			DBG_8723A("bSurpriseRemoved = true\n");
 			RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-				 ("usb_write_port23a_complete:bSurpriseRemoved = true\n"));
+				 ("usb_write_port23a_complete:bSurpriseRemoved "
+				  "= true\n"));
 			goto check_completion;
 		}
 	}
@@ -152,8 +131,8 @@ static void usb_write_port23a_complete(struct urb *purb, struct pt_regs *regs)
 check_completion:
 	spin_lock_irqsave(&pxmitpriv->lock_sctx, irqL);
 	rtw23a_sctx_done_err(&pxmitbuf->sctx,
-			  purb->status ? RTW_SCTX_DONE_WRITE_PORT_ERR :
-			  RTW_SCTX_DONE_SUCCESS);
+			     purb->status ? RTW_SCTX_DONE_WRITE_PORT_ERR :
+			     RTW_SCTX_DONE_SUCCESS);
 	spin_unlock_irqrestore(&pxmitpriv->lock_sctx, irqL);
 
 	rtw_free_xmitbuf23a(pxmitpriv, pxmitbuf);
@@ -161,30 +140,32 @@ check_completion:
 	tasklet_hi_schedule(&pxmitpriv->xmit_tasklet);
 }
 
-u32 usb_write_port23a(struct intf_hdl *pintfhdl, u32 addr, u32 cnt,
-		   struct xmit_buf *pxmitbuf)
+int rtl8723au_write_port(struct rtw_adapter *padapter, u32 addr, u32 cnt,
+			 struct xmit_buf *pxmitbuf)
 {
 	struct urb *purb = NULL;
-	struct rtw_adapter *padapter = (struct rtw_adapter *)pintfhdl->padapter;
 	struct dvobj_priv *pdvobj = adapter_to_dvobj(padapter);
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct xmit_frame *pxmitframe = (struct xmit_frame *)pxmitbuf->priv_data;
+	struct xmit_frame *pxmitframe;
 	struct usb_device *pusbd = pdvobj->pusbdev;
 	unsigned long irqL;
-	unsigned int pipe;
+	unsigned int pipe, ep_num;
 	int status;
-	u32 ret = _FAIL;
+	int ret = _FAIL;
 
 	RT_TRACE(_module_hci_ops_os_c_, _drv_err_, ("+usb_write_port23a\n"));
 
-	if ((padapter->bDriverStopped) || (padapter->bSurpriseRemoved) ||
-	    (padapter->pwrctrlpriv.pnp_bstop_trx)) {
+	if (padapter->bDriverStopped || padapter->bSurpriseRemoved ||
+	    padapter->pwrctrlpriv.pnp_bstop_trx) {
 		RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-			 ("usb_write_port23a:( padapter->bDriverStopped ||padapter->bSurpriseRemoved ||adapter->pwrctrlpriv.pnp_bstop_trx)!!!\n"));
+			 ("usb_write_port23a:( padapter->bDriverStopped || "
+			  "padapter->bSurpriseRemoved || "
+			  "adapter->pwrctrlpriv.pnp_bstop_trx)!!!\n"));
 		rtw23a_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_TX_DENY);
 		goto exit;
 	}
 
+	pxmitframe = (struct xmit_frame *)pxmitbuf->priv_data;
 	spin_lock_irqsave(&pxmitpriv->lock, irqL);
 
 	switch (addr) {
@@ -214,10 +195,11 @@ u32 usb_write_port23a(struct intf_hdl *pintfhdl, u32 addr, u32 cnt,
 
 	spin_unlock_irqrestore(&pxmitpriv->lock, irqL);
 
-	purb	= pxmitbuf->pxmit_urb[0];
+	purb = pxmitbuf->pxmit_urb[0];
 
 	/* translate DMA FIFO addr to pipehandle */
-	pipe = ffaddr2pipehdl23a(pdvobj, addr);
+	ep_num = pdvobj->Queue2Pipe[addr];
+	pipe = usb_sndbulkpipe(pusbd, ep_num);
 
 	usb_fill_bulk_urb(purb, pusbd, pipe,
 			  pxmitframe->buf_addr, /*  pxmitbuf->pbuf */
@@ -230,7 +212,7 @@ u32 usb_write_port23a(struct intf_hdl *pintfhdl, u32 addr, u32 cnt,
 		phaldata->srestpriv.last_tx_time = jiffies;
 	} else {
 		rtw23a_sctx_done_err(&pxmitbuf->sctx,
-				  RTW_SCTX_DONE_WRITE_PORT_ERR);
+				     RTW_SCTX_DONE_WRITE_PORT_ERR);
 		DBG_8723A("usb_write_port23a, status =%d\n", status);
 		RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
 			 ("usb_write_port23a(): usb_submit_urb, status =%x\n",
@@ -255,9 +237,8 @@ exit:
 	return ret;
 }
 
-void usb_write_port23a_cancel(struct intf_hdl *pintfhdl)
+void rtl8723au_write_port_cancel(struct rtw_adapter *padapter)
 {
-	struct rtw_adapter *padapter = pintfhdl->padapter;
 	struct xmit_buf *pxmitbuf;
 	struct list_head *plist;
 	int j;
diff --git a/drivers/staging/rtl8723au/os_dep/xmit_linux.c b/drivers/staging/rtl8723au/os_dep/xmit_linux.c
index e1c6fc74623..69f88848715 100644
--- a/drivers/staging/rtl8723au/os_dep/xmit_linux.c
+++ b/drivers/staging/rtl8723au/os_dep/xmit_linux.c
@@ -24,47 +24,6 @@
 #include <xmit_osdep.h>
 #include <osdep_intf.h>
 
-uint rtw_remainder_len23a(struct pkt_file *pfile)
-{
-	return pfile->buf_len - ((unsigned long)(pfile->cur_addr) -
-	       (unsigned long)(pfile->buf_start));
-}
-
-void _rtw_open_pktfile23a(struct sk_buff *pktptr, struct pkt_file *pfile)
-{
-	pfile->pkt = pktptr;
-	pfile->buf_start = pktptr->data;
-	pfile->cur_addr = pktptr->data;
-	pfile->buf_len = pktptr->len;
-	pfile->pkt_len = pktptr->len;
-
-	pfile->cur_buffer = pfile->buf_start;
-}
-
-uint _rtw_pktfile_read23a(struct pkt_file *pfile, u8 *rmem, uint rlen)
-{
-	uint	len = 0;
-
-	len =  rtw_remainder_len23a(pfile);
-	len = (rlen > len) ? len : rlen;
-
-	if (rmem)
-		skb_copy_bits(pfile->pkt, pfile->buf_len-pfile->pkt_len,
-			      rmem, len);
-
-	pfile->cur_addr += len;
-	pfile->pkt_len -= len;
-
-	return len;
-}
-
-int rtw_endofpktfile23a(struct pkt_file *pfile)
-{
-	if (pfile->pkt_len == 0)
-		return true;
-	return false;
-}
-
 int rtw_os_xmit_resource_alloc23a(struct rtw_adapter *padapter,
 			       struct xmit_buf *pxmitbuf, u32 alloc_sz)
 {
diff --git a/drivers/staging/rtl8821ae/base.c b/drivers/staging/rtl8821ae/base.c
index a4c9cc437bc..4a36da06c01 100644
--- a/drivers/staging/rtl8821ae/base.c
+++ b/drivers/staging/rtl8821ae/base.c
@@ -320,9 +320,6 @@ static void _rtl_init_mac80211(struct ieee80211_hw *hw)
 	/* <5> set hw caps */
 	hw->flags = IEEE80211_HW_SIGNAL_DBM |
 	    IEEE80211_HW_RX_INCLUDES_FCS |
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3, 4, 0))
-	    IEEE80211_HW_BEACON_FILTER |
-#endif
 	    IEEE80211_HW_AMPDU_AGGREGATION |
 	    IEEE80211_HW_REPORTS_TX_ACK_STATUS |
 	    IEEE80211_HW_CONNECTION_MONITOR |
@@ -335,8 +332,6 @@ static void _rtl_init_mac80211(struct ieee80211_hw *hw)
 			IEEE80211_HW_PS_NULLFUNC_STACK |
 			/* IEEE80211_HW_SUPPORTS_DYNAMIC_PS | */
 			0;
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37))
 	hw->wiphy->interface_modes =
 	    BIT(NL80211_IFTYPE_AP) |
 	    BIT(NL80211_IFTYPE_STATION) |
@@ -344,23 +339,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);
-#else
-/*<delete in kernel end>*/
-	hw->wiphy->interface_modes =
-	    BIT(NL80211_IFTYPE_AP) |
-	    BIT(NL80211_IFTYPE_STATION) |
-	    BIT(NL80211_IFTYPE_ADHOC) |
-	    BIT(NL80211_IFTYPE_MESH_POINT) ;
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
-#if (LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 39))
+
 	hw->wiphy->flags |= WIPHY_FLAG_IBSS_RSN;
-#endif
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 3, 0))
 	hw->wiphy->flags |= WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL;
-#endif
 
 	hw->wiphy->rts_threshold = 2347;
 
@@ -401,15 +383,8 @@ static int _rtl_init_deferred_work(struct ieee80211_hw *hw)
 		    rtl_easy_concurrent_retrytimer_callback, (unsigned long)hw);
 	/* <2> work queue */
 	rtlpriv->works.hw = hw;
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37))
-/*<delete in kernel end>*/
-	rtlpriv->works.rtl_wq = alloc_workqueue(rtlpriv->cfg->name, 0, 0);
-/*<delete in kernel start>*/
-#else
-	rtlpriv->works.rtl_wq = create_workqueue(rtlpriv->cfg->name);
-#endif
-/*<delete in kernel end>*/
+	rtlpriv->works.rtl_wq = alloc_workqueue("%s", 0, 0,
+						rtlpriv->cfg->name);
 	if (!rtlpriv->works.rtl_wq)
 		return -ENOMEM;
 
@@ -900,13 +875,8 @@ bool rtl_action_proc(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
 								    hdr->addr3,
 								    tid);
 					if (skb_delba) {
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 10, 0))
 						rx_status.freq = hw->conf.chandef.chan->center_freq;
 						rx_status.band = hw->conf.chandef.chan->band;
-#else
-						rx_status.freq = hw->conf.channel->center_freq;
-						rx_status.band = hw->conf.channel->band;
-#endif
 						rx_status.flag |= RX_FLAG_DECRYPTED;
 						rx_status.flag |= RX_FLAG_MACTIME_MPDU;
 						rx_status.rate_idx = 0;
@@ -1481,21 +1451,8 @@ int rtl_send_smps_action(struct ieee80211_hw *hw,
 		/* rtlpriv->cfg->ops->update_rate_tbl(hw, sta, 0); */
 
 		info->control.rates[0].idx = 0;
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 10, 0))
 		info->band = hw->conf.chandef.chan->band;
-#else
-		info->band = hw->conf.channel->band;
-#endif
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3, 7, 0))
-		info->control.sta = sta;
-		rtlpriv->intf_ops->adapter_tx(hw, skb, &tcb_desc);
-#else
-/*<delete in kernel end>*/
 		rtlpriv->intf_ops->adapter_tx(hw, sta, skb, &tcb_desc);
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 	}
 	return 1;
 
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.c b/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.c
index 115908928ae..d2381794ecc 100644
--- a/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.c
+++ b/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.c
@@ -12,7 +12,7 @@
  *   include files
  **************************************************************/
 #include "halbt_precomp.h"
-#if 1
+
 /**************************************************************
  *   Global variables, these are static variables
  **************************************************************/
@@ -21,14 +21,14 @@ 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;
 
-const char *const GLBtInfoSrc8192e2Ant[]={
+static const char *const GLBtInfoSrc8192e2Ant[] = {
 	"BT Info[wifi fw]",
 	"BT Info[bt rsp]",
 	"BT Info[bt auto report]",
 };
 
-u32 glcoex_ver_date_8192e_2ant = 20130902;
-u32 glcoex_ver_8192e_2ant = 0x34;
+static u32 glcoex_ver_date_8192e_2ant = 20130902;
+static u32 glcoex_ver_8192e_2ant = 0x34;
 
 /**************************************************************
  *   local function proto type if needed
@@ -36,13 +36,12 @@ u32 glcoex_ver_8192e_2ant = 0x34;
 /**************************************************************
  *   local function start with halbtc8192e2ant_
  **************************************************************/
-u8 halbtc8192e2ant_btrssi_state(u8 level_num, u8 rssi_thresh, u8 rssi_thresh1)
+static u8 halbtc8192e2ant_btrssi_state(u8 level_num, u8 rssi_thresh,
+				       u8 rssi_thresh1)
 {
-	int btrssi=0;
+	int btrssi = coex_sta->bt_rssi;
 	u8 btrssi_state = coex_sta->pre_bt_rssi_state;
 
-	btrssi = 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)) {
@@ -82,7 +81,7 @@ u8 halbtc8192e2ant_btrssi_state(u8 level_num, u8 rssi_thresh, u8 rssi_thresh1)
 		    (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 +
+			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,
@@ -132,8 +131,9 @@ u8 halbtc8192e2ant_btrssi_state(u8 level_num, u8 rssi_thresh, u8 rssi_thresh1)
 	return btrssi_state;
 }
 
-u8 halbtc8192e2ant_wifirssi_state(struct btc_coexist * btcoexist, u8 index,
-				  u8 level_num, u8 rssi_thresh, u8 rssi_thresh1)
+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];
@@ -156,8 +156,8 @@ u8 halbtc8192e2ant_wifirssi_state(struct btc_coexist * btcoexist, u8 index,
 				BTC_PRINT(BTC_MSG_ALGORITHM,
 					  ALGO_WIFI_RSSI_STATE,
 					  "wifi RSSI state stay at Low\n");
- 			}
- 		} else {
+			}
+		} else {
 			if (wifirssi < rssi_thresh) {
 				wifirssi_state = BTC_RSSI_STATE_LOW;
 				BTC_PRINT(BTC_MSG_ALGORITHM,
@@ -234,10 +234,10 @@ u8 halbtc8192e2ant_wifirssi_state(struct btc_coexist * btcoexist, u8 index,
 	return wifirssi_state;
 }
 
-void halbtc8192e2ant_monitor_bt_enable_disable(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_monitor_bt_enable_disable(struct btc_coexist *btcoexist)
 {
-	static bool pre_bt_disabled = false;
-	static u32 bt_disable_cnt = 0;
+	static bool pre_bt_disabled;
+	static u32 bt_disable_cnt;
 	bool bt_active = true, bt_disabled = false;
 
 	/* This function check if bt is disabled */
@@ -283,7 +283,7 @@ void halbtc8192e2ant_monitor_bt_enable_disable(struct btc_coexist *btcoexist)
 	}
 }
 
-u32 halbtc8192e2ant_decidera_mask(struct btc_coexist *btcoexist,
+static u32 halbtc8192e2ant_decidera_mask(struct btc_coexist *btcoexist,
 				  u8 sstype, u32 ra_masktype)
 {
 	u32 disra_mask = 0x0;
@@ -296,13 +296,13 @@ u32 halbtc8192e2ant_decidera_mask(struct btc_coexist *btcoexist,
 			disra_mask = 0xfff00000;/* disable 2ss */
 		break;
 	case 1: /* disable cck 1/2 */
-		if(sstype == 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)
+		if (sstype == 2)
 			disra_mask = 0x0001f1f7;/* enable 2ss */
 		else
 			disra_mask = 0xfff1f1f7;/* disable 2ss */
@@ -314,8 +314,8 @@ u32 halbtc8192e2ant_decidera_mask(struct btc_coexist *btcoexist,
 	return disra_mask;
 }
 
-void halbtc8192e2ant_Updatera_mask(struct btc_coexist *btcoexist,
-				   bool force_exec, u32 dis_ratemask)
+static void halbtc8192e2ant_Updatera_mask(struct btc_coexist *btcoexist,
+					  bool force_exec, u32 dis_ratemask)
 {
 	coex_dm->curra_mask = dis_ratemask;
 
@@ -325,8 +325,8 @@ void halbtc8192e2ant_Updatera_mask(struct btc_coexist *btcoexist,
 	coex_dm->prera_mask = coex_dm->curra_mask;
 }
 
-void halbtc8192e2ant_autorate_fallback_retry(struct btc_coexist *btcoexist,
-					     bool force_exec, u8 type)
+static void halbtc8192e2ant_autorate_fallback_retry(struct btc_coexist *btcoexist,
+						    bool force_exec, u8 type)
 {
 	bool wifi_under_bmode = false;
 
@@ -364,32 +364,32 @@ void halbtc8192e2ant_autorate_fallback_retry(struct btc_coexist *btcoexist,
 	coex_dm->pre_arfrtype = coex_dm->cur_arfrtype;
 }
 
-void halbtc8192e2ant_retrylimit(struct btc_coexist *btcoexist,
-				bool force_exec, u8 type)
+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;
+		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;
 }
 
-void halbtc8192e2ant_ampdu_maxtime(struct btc_coexist *btcoexist,
-				   bool force_exec, u8 type)
+static void halbtc8192e2ant_ampdu_maxtime(struct btc_coexist *btcoexist,
+					  bool force_exec, u8 type)
 {
 	coex_dm->cur_ampdutime_type = type;
 
@@ -411,9 +411,10 @@ void halbtc8192e2ant_ampdu_maxtime(struct btc_coexist *btcoexist,
 	coex_dm->pre_ampdutime_type = coex_dm->cur_ampdutime_type;
 }
 
-void halbtc8192e2ant_limited_tx(struct btc_coexist *btcoexist,
-				bool force_exec, u8 ra_masktype, u8 arfr_type,
-				u8 retrylimit_type, u8 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;
 
@@ -429,10 +430,10 @@ void halbtc8192e2ant_limited_tx(struct btc_coexist *btcoexist,
 	halbtc8192e2ant_ampdu_maxtime(btcoexist, force_exec, ampdutime_type);
 }
 
-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)
+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;
@@ -455,7 +456,7 @@ void halbtc8192e2ant_limited_rx(struct btc_coexist *btcoexist,
 
 }
 
-void halbtc8192e2ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
+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;
@@ -487,9 +488,9 @@ void halbtc8192e2ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
 	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
 }
 
-void halbtc8192e2ant_querybt_info(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_querybt_info(struct btc_coexist *btcoexist)
 {
-	u8 h2c_parameter[1] ={0};
+	u8 h2c_parameter[1] = {0};
 
 	coex_sta->c2h_bt_info_req_sent = true;
 
@@ -502,39 +503,7 @@ void halbtc8192e2ant_querybt_info(struct btc_coexist *btcoexist)
 	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
 }
 
-bool halbtc8192e2ant_iswifi_status_changed(struct btc_coexist *btcoexist)
-{
-	static bool pre_wifi_busy = false;
-	static bool pre_under_4way = false, pre_bt_hson = false;
-	bool wifi_busy = false, under_4way = false, bt_hson = 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_hson);
-	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_hson != pre_bt_hson) {
-			pre_bt_hson = bt_hson;
-			return true;
-		}
-	}
-
-	return false;
-}
-
-void halbtc8192e2ant_update_btlink_info(struct btc_coexist *btcoexist)
+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;
@@ -590,11 +559,11 @@ void halbtc8192e2ant_update_btlink_info(struct btc_coexist *btcoexist)
 		bt_link_info->hid_only = false;
 }
 
-u8 halbtc8192e2ant_action_algorithm(struct btc_coexist *btcoexist)
+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;
+	bool bt_hson = false;
 	u8 algorithm = BT_8192E_2ANT_COEX_ALGO_UNDEFINED;
 	u8 numOfDiffProfile = 0;
 
@@ -777,10 +746,10 @@ u8 halbtc8192e2ant_action_algorithm(struct btc_coexist *btcoexist)
 	return algorithm;
 }
 
-void halbtc8192e2ant_setfw_dac_swinglevel(struct btc_coexist *btcoexist,
-					  u8 dac_swinglvl)
+static void halbtc8192e2ant_setfw_dac_swinglevel(struct btc_coexist *btcoexist,
+						 u8 dac_swinglvl)
 {
-	u8 h2c_parameter[1] ={0};
+	u8 h2c_parameter[1] = {0};
 
 	/* There are several type of dacswing
 	 * 0x18/ 0x10/ 0xc/ 0x8/ 0x4/ 0x6 */
@@ -794,10 +763,10 @@ void halbtc8192e2ant_setfw_dac_swinglevel(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x64, 1, h2c_parameter);
 }
 
-void halbtc8192e2ant_set_fwdec_btpwr(struct btc_coexist *btcoexist,
-				     u8 dec_btpwr_lvl)
+static void halbtc8192e2ant_set_fwdec_btpwr(struct btc_coexist *btcoexist,
+					    u8 dec_btpwr_lvl)
 {
-	u8 h2c_parameter[1] ={0};
+	u8 h2c_parameter[1] = {0};
 
 	h2c_parameter[0] = dec_btpwr_lvl;
 
@@ -808,12 +777,12 @@ void halbtc8192e2ant_set_fwdec_btpwr(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x62, 1, h2c_parameter);
 }
 
-void halbtc8192e2ant_dec_btpwr(struct btc_coexist *btcoexist,
-			       bool force_exec, u8 dec_btpwr_lvl)
+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);
+		  (force_exec ? "force to":""), dec_btpwr_lvl);
 	coex_dm->cur_dec_bt_pwr = dec_btpwr_lvl;
 
 	if (!force_exec) {
@@ -826,10 +795,10 @@ void halbtc8192e2ant_dec_btpwr(struct btc_coexist *btcoexist,
 	coex_dm->pre_dec_bt_pwr = coex_dm->cur_dec_bt_pwr;
 }
 
-void halbtc8192e2ant_set_bt_autoreport(struct btc_coexist *btcoexist,
-				       bool enable_autoreport)
+static void halbtc8192e2ant_set_bt_autoreport(struct btc_coexist *btcoexist,
+					      bool enable_autoreport)
 {
-	u8 h2c_parameter[1] ={0};
+	u8 h2c_parameter[1] = {0};
 
 	h2c_parameter[0] = 0;
 
@@ -838,19 +807,20 @@ void halbtc8192e2ant_set_bt_autoreport(struct btc_coexist *btcoexist,
 
 	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!!"),
+		  (enable_autoreport ? "Enabled!!":"Disabled!!"),
 		  h2c_parameter[0]);
 
 	btcoexist->btc_fill_h2c(btcoexist, 0x68, 1, h2c_parameter);
 }
 
-void halbtc8192e2ant_bt_autoreport(struct btc_coexist *btcoexist,
-				   bool force_exec, bool enable_autoreport)
+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"));
+		  (force_exec ? "force to":""),
+		  ((enable_autoreport) ? "Enabled":"Disabled"));
 	coex_dm->cur_bt_auto_report = enable_autoreport;
 
 	if (!force_exec) {
@@ -868,12 +838,12 @@ void halbtc8192e2ant_bt_autoreport(struct btc_coexist *btcoexist,
 	coex_dm->pre_bt_auto_report = coex_dm->cur_bt_auto_report;
 }
 
-void halbtc8192e2ant_fw_dac_swinglvl(struct btc_coexist *btcoexist,
-				     bool force_exec, u8 fw_dac_swinglvl)
+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);
+		  (force_exec ? "force to":""), fw_dac_swinglvl);
 	coex_dm->cur_fw_dac_swing_lvl = fw_dac_swinglvl;
 
 	if (!force_exec) {
@@ -893,8 +863,8 @@ void halbtc8192e2ant_fw_dac_swinglvl(struct btc_coexist *btcoexist,
 	coex_dm->pre_fw_dac_swing_lvl = coex_dm->cur_fw_dac_swing_lvl;
 }
 
-void halbtc8192e2ant_set_sw_rf_rx_lpf_corner(struct btc_coexist *btcoexist,
-					     bool rx_rf_shrink_on)
+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 */
@@ -915,12 +885,12 @@ void halbtc8192e2ant_set_sw_rf_rx_lpf_corner(struct btc_coexist *btcoexist,
 	}
 }
 
-void halbtc8192e2ant_rf_shrink(struct btc_coexist *btcoexist,
-			       bool force_exec, bool rx_rf_shrink_on)
+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"));
+		  (force_exec ? "force to":""), ((rx_rf_shrink_on) ? "ON":"OFF"));
 	coex_dm->cur_rf_rx_lpf_shrink = rx_rf_shrink_on;
 
 	if (!force_exec) {
@@ -939,56 +909,8 @@ void halbtc8192e2ant_rf_shrink(struct btc_coexist *btcoexist,
 	coex_dm->pre_rf_rx_lpf_shrink = coex_dm->cur_rf_rx_lpf_shrink;
 }
 
-void halbtc8192e2ant_set_sw_penalty_tx_rateadaptive(
-						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);
-}
-
-void halbtc8192e2ant_low_penalty_ra(struct btc_coexist *btcoexist,
-				    bool force_exec, bool low_penalty_ra)
-{
-	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;
-	}
-	halbtc8192e2ant_set_sw_penalty_tx_rateadaptive(btcoexist,
-						coex_dm->cur_low_penalty_ra);
-
-	coex_dm->pre_low_penalty_ra = coex_dm->cur_low_penalty_ra;
-}
-
-void halbtc8192e2ant_set_dac_swingreg(struct btc_coexist *btcoexist,
-				      u32 level)
+static void halbtc8192e2ant_set_dac_swingreg(struct btc_coexist *btcoexist,
+					     u32 level)
 {
 	u8 val = (u8)level;
 
@@ -997,9 +919,9 @@ void halbtc8192e2ant_set_dac_swingreg(struct btc_coexist *btcoexist,
 	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x883, 0x3e, val);
 }
 
-void halbtc8192e2ant_setsw_fulltime_dacswing(struct btc_coexist *btcoexist,
-					     bool sw_dac_swingon,
-					     u32 sw_dac_swinglvl)
+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);
@@ -1007,15 +929,14 @@ void halbtc8192e2ant_setsw_fulltime_dacswing(struct btc_coexist *btcoexist,
 		halbtc8192e2ant_set_dac_swingreg(btcoexist, 0x18);
 }
 
-
-void halbtc8192e2ant_DacSwing(struct btc_coexist *btcoexist,
-			      bool force_exec, bool dac_swingon,
-			      u32 dac_swinglvl)
+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);
+		  (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;
 
@@ -1041,46 +962,8 @@ void halbtc8192e2ant_DacSwing(struct btc_coexist *btcoexist,
 	coex_dm->pre_dac_swing_lvl = coex_dm->cur_dac_swing_lvl;
 }
 
-void halbtc8192e2ant_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);
-	}
-}
-
-void halbtc8192e2ant_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;
-	}
-	halbtc8192e2ant_set_adc_backoff(btcoexist, coex_dm->cur_adc_back_off);
-
-	coex_dm->pre_adc_back_off = coex_dm->cur_adc_back_off;
-}
-
-void halbtc8192e2ant_set_agc_table(struct btc_coexist *btcoexist,
-				   bool agc_table_en)
+static void halbtc8192e2ant_set_agc_table(struct btc_coexist *btcoexist,
+					  bool agc_table_en)
 {
 
 	/* BB AGC Gain Table */
@@ -1096,7 +979,7 @@ void halbtc8192e2ant_set_agc_table(struct btc_coexist *btcoexist,
 	} 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, 0xaa1A0001);
 		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa91B0001);
 		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa81C0001);
 		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa71D0001);
@@ -1105,13 +988,13 @@ void halbtc8192e2ant_set_agc_table(struct btc_coexist *btcoexist,
 	}
 }
 
-void halbtc8192e2ant_AgcTable(struct btc_coexist *btcoexist,
-			      bool force_exec, bool agc_table_en)
+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"));
+		  (force_exec ? "force to":""),
+		  ((agc_table_en) ? "Enable":"Disable"));
 	coex_dm->cur_agc_table_en = agc_table_en;
 
 	if (!force_exec) {
@@ -1127,9 +1010,9 @@ void halbtc8192e2ant_AgcTable(struct btc_coexist *btcoexist,
 	coex_dm->pre_agc_table_en = coex_dm->cur_agc_table_en;
 }
 
-void halbtc8192e2ant_set_coex_table(struct btc_coexist *btcoexist,
-				    u32 val0x6c0, u32 val0x6c4,
-				    u32 val0x6c8, u8 val0x6cc)
+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);
@@ -1148,13 +1031,14 @@ void halbtc8192e2ant_set_coex_table(struct btc_coexist *btcoexist,
 	btcoexist->btc_write_1byte(btcoexist, 0x6cc, val0x6cc);
 }
 
-void halbtc8192e2ant_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, ",
-		  (force_exec? "force to":""), val0x6c0);
+		  (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);
@@ -1171,7 +1055,7 @@ void halbtc8192e2ant_coex_table(struct btc_coexist *btcoexist, bool force_exec,
 			  "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",
@@ -1192,8 +1076,8 @@ void halbtc8192e2ant_coex_table(struct btc_coexist *btcoexist, bool force_exec,
 	coex_dm->pre_val0x6cc = coex_dm->cur_val0x6cc;
 }
 
-void halbtc8192e2ant_coex_table_with_type(struct btc_coexist *btcoexist,
-					  bool force_exec, u8 type)
+static void halbtc8192e2ant_coex_table_with_type(struct btc_coexist *btcoexist,
+						 bool force_exec, u8 type)
 {
 	switch (type) {
 	case 0:
@@ -1221,10 +1105,10 @@ void halbtc8192e2ant_coex_table_with_type(struct btc_coexist *btcoexist,
 	}
 }
 
-void halbtc8192e2ant_set_fw_ignore_wlanact(struct btc_coexist *btcoexist,
-					   bool enable)
+static void halbtc8192e2ant_set_fw_ignore_wlanact(struct btc_coexist *btcoexist,
+						  bool enable)
 {
-	u8 h2c_parameter[1] ={0};
+	u8 h2c_parameter[1] = {0};
 
 	if (enable)
 		h2c_parameter[0] |= BIT0; /* function enable */
@@ -1236,12 +1120,12 @@ void halbtc8192e2ant_set_fw_ignore_wlanact(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x63, 1, h2c_parameter);
 }
 
-void halbtc8192e2ant_IgnoreWlanAct(struct btc_coexist *btcoexist,
-				   bool force_exec, bool enable)
+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"));
+		  (force_exec ? "force to":""), (enable ? "ON":"OFF"));
 	coex_dm->cur_ignore_wlan_act = enable;
 
 	if (!force_exec) {
@@ -1261,10 +1145,10 @@ void halbtc8192e2ant_IgnoreWlanAct(struct btc_coexist *btcoexist,
 	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
 }
 
-void halbtc8192e2ant_SetFwPstdma(struct btc_coexist *btcoexist, u8 byte1,
-				 u8 byte2, u8 byte3, u8 byte4, u8 byte5)
+static void halbtc8192e2ant_SetFwPstdma(struct btc_coexist *btcoexist, u8 byte1,
+					u8 byte2, u8 byte3, u8 byte4, u8 byte5)
 {
-	u8 h2c_parameter[5] ={0};
+	u8 h2c_parameter[5] = {0};
 
 	h2c_parameter[0] = byte1;
 	h2c_parameter[1] = byte2;
@@ -1287,29 +1171,33 @@ void halbtc8192e2ant_SetFwPstdma(struct btc_coexist *btcoexist, u8 byte1,
 	btcoexist->btc_fill_h2c(btcoexist, 0x60, 5, h2c_parameter);
 }
 
-void halbtc8192e2ant_sw_mechanism1(struct btc_coexist *btcoexist,
-				   bool shrink_rx_lpf, bool low_penalty_ra,
-				   bool limited_dig, bool btlan_constrain)
+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);
 }
 
-void halbtc8192e2ant_sw_mechanism2(struct btc_coexist *btcoexist,
-				   bool agc_table_shift, bool adc_backoff,
-				   bool sw_dac_swing, u32 dac_swinglvl)
+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);
 }
 
-void halbtc8192e2ant_ps_tdma(struct btc_coexist *btcoexist,
-			     bool force_exec, bool turn_on, u8 type)
+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);
+		  (force_exec ? "force to":""), (turn_on ? "ON":"OFF"), type);
 	coex_dm->cur_ps_tdma_on = turn_on;
 	coex_dm->cur_ps_tdma = type;
 
@@ -1440,7 +1328,8 @@ void halbtc8192e2ant_ps_tdma(struct btc_coexist *btcoexist,
 	coex_dm->pre_ps_tdma = coex_dm->cur_ps_tdma;
 }
 
-void halbtc8192e2ant_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;
@@ -1461,7 +1350,7 @@ void halbtc8192e2ant_set_switch_sstype(struct btc_coexist *btcoexist, u8 sstype)
 		/* switch cck patch */
 		btcoexist->btc_write_1byte_bitmask(btcoexist, 0xe77, 0x4, 0x1);
 		btcoexist->btc_write_1byte(btcoexist, 0xa07, 0x81);
-		mimops=BTC_MIMO_PS_STATIC;
+		mimops = BTC_MIMO_PS_STATIC;
 	} else if (sstype == 2) {
 		halbtc8192e2ant_ps_tdma(btcoexist, FORCE_EXEC, false, 0);
 		btcoexist->btc_write_1byte(btcoexist, 0xc04, 0x33);
@@ -1469,18 +1358,18 @@ void halbtc8192e2ant_set_switch_sstype(struct btc_coexist *btcoexist, u8 sstype)
 		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;
+		mimops = BTC_MIMO_PS_DYNAMIC;
 	}
 	/* set rx 1ss or 2ss */
 	btcoexist->btc_set(btcoexist, BTC_SET_ACT_SEND_MIMO_PS, &mimops);
 }
 
-void halbtc8192e2ant_switch_sstype(struct btc_coexist *btcoexist,
+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);
+		  (force_exec ? "force to":""), new_sstype);
 	coex_dm->cur_sstype = new_sstype;
 
 	if (!force_exec) {
@@ -1492,7 +1381,7 @@ void halbtc8192e2ant_switch_sstype(struct btc_coexist *btcoexist,
 	coex_dm->pre_sstype = coex_dm->cur_sstype;
 }
 
-void halbtc8192e2ant_coex_alloff(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_coex_alloff(struct btc_coexist *btcoexist)
 {
 	/* fw all off */
 	halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
@@ -1507,7 +1396,7 @@ void halbtc8192e2ant_coex_alloff(struct btc_coexist *btcoexist)
 	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
 }
 
-void halbtc8192e2ant_init_coex_dm(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_init_coex_dm(struct btc_coexist *btcoexist)
 {
 	/* force to reset coex mechanism */
 
@@ -1522,7 +1411,7 @@ void halbtc8192e2ant_init_coex_dm(struct btc_coexist *btcoexist)
 	halbtc8192e2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
 }
 
-void halbtc8192e2ant_action_bt_inquiry(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_bt_inquiry(struct btc_coexist *btcoexist)
 {
 	bool low_pwr_disable = true;
 
@@ -1540,7 +1429,7 @@ void halbtc8192e2ant_action_bt_inquiry(struct btc_coexist *btcoexist)
 	halbtc8192e2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
 }
 
-bool halbtc8192e2ant_is_common_action(struct btc_coexist *btcoexist)
+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;
@@ -1586,7 +1475,7 @@ bool halbtc8192e2ant_is_common_action(struct btc_coexist *btcoexist)
 		halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
 		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
 
- 		halbtc8192e2ant_sw_mechanism1(btcoexist, false, false, false,
+		halbtc8192e2ant_sw_mechanism1(btcoexist, false, false, false,
 					      false);
 		halbtc8192e2ant_sw_mechanism2(btcoexist, false, false, false,
 					      0x18);
@@ -1685,9 +1574,9 @@ bool halbtc8192e2ant_is_common_action(struct btc_coexist *btcoexist)
 	return common;
 }
 
-void halbtc8192e2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
-					  bool sco_hid, bool tx_pause,
-					  u8 max_interval)
+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,
@@ -1799,7 +1688,7 @@ void halbtc8192e2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
 		up = 0;
 		dn = 0;
 		m = 1;
-		n= 3;
+		n = 3;
 		result = 0;
 		wait_cnt = 0;
 	} else {
@@ -2116,7 +2005,7 @@ void halbtc8192e2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
 									     12;
 					}
 				} else if (result == 1) {
- 					if (coex_dm->cur_ps_tdma == 4) {
+					if (coex_dm->cur_ps_tdma == 4) {
 						halbtc8192e2ant_ps_tdma(
 								    btcoexist,
 								    NORMAL_EXEC,
@@ -2416,21 +2305,21 @@ void halbtc8192e2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
 								    true, 2);
 						coex_dm->ps_tdma_du_adj_type =
 									     2;
-					} else if(coex_dm->cur_ps_tdma == 12) {
+					} 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) {
+					} 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) {
+					} else if (coex_dm->cur_ps_tdma == 10) {
 						halbtc8192e2ant_ps_tdma(
 								    btcoexist,
 								    NORMAL_EXEC,
@@ -2721,7 +2610,7 @@ void halbtc8192e2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
 	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!!!, " );
+			  "[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);
@@ -2730,7 +2619,7 @@ void halbtc8192e2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
 		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
 		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
 
-		if ( !scan && !link && !roam)
+		if (!scan && !link && !roam)
 			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						true,
 						coex_dm->ps_tdma_du_adj_type);
@@ -2742,7 +2631,7 @@ void halbtc8192e2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
 }
 
 /* SCO only or SCO+PAN(HS) */
-void halbtc8192e2ant_action_sco(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_sco(struct btc_coexist *btcoexist)
 {
 	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_STAY_LOW;
 	u32 wifi_bw;
@@ -2804,7 +2693,7 @@ void halbtc8192e2ant_action_sco(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8192e2ant_action_sco_pan(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_sco_pan(struct btc_coexist *btcoexist)
 {
 	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_STAY_LOW;
 	u32 wifi_bw;
@@ -2866,9 +2755,9 @@ void halbtc8192e2ant_action_sco_pan(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8192e2ant_action_hid(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_hid(struct btc_coexist *btcoexist)
 {
-	u8 wifirssi_state, btrssi_state=BTC_RSSI_STATE_HIGH;
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
 	u32 wifi_bw;
 
 	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
@@ -2906,7 +2795,7 @@ void halbtc8192e2ant_action_hid(struct btc_coexist *btcoexist)
 			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
 						      false, 0x18);
 		} else {
- 			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
 						      false, false);
 			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
 						      false, 0x18);
@@ -2918,8 +2807,8 @@ void halbtc8192e2ant_action_hid(struct btc_coexist *btcoexist)
 						      false, false);
 			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
 						      false, 0x18);
- 		} else {
- 			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
+		} else {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
 						      false, false);
 			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
 						      false, 0x18);
@@ -2928,7 +2817,7 @@ void halbtc8192e2ant_action_hid(struct btc_coexist *btcoexist)
 }
 
 /* A2DP only / PAN(EDR) only/ A2DP+PAN(HS) */
-void halbtc8192e2ant_action_a2dp(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_a2dp(struct btc_coexist *btcoexist)
 {
 	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
 	u32 wifi_bw;
@@ -2991,7 +2880,7 @@ 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)) {
- 			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
 						      false, false);
 			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
 						      false, 0x18);
@@ -3017,7 +2906,7 @@ void halbtc8192e2ant_action_a2dp(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8192e2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
 {
 	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
 	u32 wifi_bw;
@@ -3052,7 +2941,7 @@ 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)) {
- 			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
 						      false, false);
 			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
 						      true, 0x6);
@@ -3078,7 +2967,7 @@ void halbtc8192e2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8192e2ant_action_pan_edr(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_pan_edr(struct btc_coexist *btcoexist)
 {
 	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
 	u32 wifi_bw;
@@ -3139,7 +3028,7 @@ void halbtc8192e2ant_action_pan_edr(struct btc_coexist *btcoexist)
 }
 
 /* PAN(HS) only */
-void halbtc8192e2ant_action_pan_hs(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_pan_hs(struct btc_coexist *btcoexist)
 {
 	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
 	u32 wifi_bw;
@@ -3197,9 +3086,9 @@ void halbtc8192e2ant_action_pan_hs(struct btc_coexist *btcoexist)
 }
 
 /* PAN(EDR)+A2DP */
-void halbtc8192e2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
 {
-	u8 wifirssi_state, btrssi_state=BTC_RSSI_STATE_HIGH;
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
 	u32 wifi_bw;
 
 	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
@@ -3260,7 +3149,7 @@ void halbtc8192e2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8192e2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
 {
 	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
 	u32 wifi_bw;
@@ -3280,15 +3169,15 @@ void halbtc8192e2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
 	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);
+		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_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_ps_tdma(btcoexist, NORMAL_EXEC, true, 10);
 	}
 
 	/* sw mechanism */
@@ -3322,7 +3211,7 @@ void halbtc8192e2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
 }
 
 /* HID+A2DP+PAN(EDR) */
-void halbtc8192e2ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
 {
 	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
 	u32 wifi_bw;
@@ -3383,7 +3272,7 @@ void halbtc8192e2ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8192e2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
 {
 	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
 	u32 wifi_bw;
@@ -3403,7 +3292,7 @@ void halbtc8192e2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
 		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)) 	{
+		   (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) ||
@@ -3442,7 +3331,7 @@ void halbtc8192e2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8192e2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 {
 	u8 algorithm = 0;
 
@@ -3472,7 +3361,7 @@ void halbtc8192e2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 
 	coex_dm->cur_algorithm = algorithm;
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-		  "[BTCoex], Algorithm = %d \n", coex_dm->cur_algorithm);
+		  "[BTCoex], Algorithm = %d\n", coex_dm->cur_algorithm);
 
 	if (halbtc8192e2ant_is_common_action(btcoexist)) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
@@ -3552,7 +3441,8 @@ void halbtc8192e2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8192e2ant_init_hwconfig(struct btc_coexist *btcoexist, bool backup)
+static void halbtc8192e2ant_init_hwconfig(struct btc_coexist *btcoexist,
+					  bool backup)
 {
 	u16 u16tmp = 0;
 	u8 u8tmp = 0;
@@ -3649,7 +3539,7 @@ void ex_halbtc8192e2ant_init_coex_dm(struct btc_coexist *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;
+	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];
@@ -3735,9 +3625,9 @@ void ex_halbtc8192e2ant_display_coex_info(struct btc_coexist *btcoexist)
 	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"))),
+			(((BTC_WIFI_BW_HT40 == wifi_bw) ? "HT40" : "HT20"))),
 		   ((!wifi_busy) ? "idle" :
-		   	((BTC_WIFI_TRAFFIC_TX == wifi_traffic_dir) ?
+			((BTC_WIFI_TRAFFIC_TX == wifi_traffic_dir) ?
 				"uplink" : "downlink")));
 	CL_PRINTF(cli_buf);
 
@@ -3766,7 +3656,7 @@ void ex_halbtc8192e2ant_display_coex_info(struct btc_coexist *btcoexist)
 		   (bt_info_ext&BIT0) ? "Basic rate" : "EDR rate");
 	CL_PRINTF(cli_buf);
 
-	for (i=0; i<BT_INFO_SRC_8192E_2ANT_MAX; i++) {
+	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 ",
@@ -3911,9 +3801,6 @@ void ex_halbtc8192e2ant_display_coex_info(struct btc_coexist *btcoexist)
 		   "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
 	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_COEX_STATISTICS);
 }
 
@@ -3950,7 +3837,7 @@ 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)
+	else if (BTC_SCAN_FINISH == type)
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], SCAN FINISH notify\n");
 }
@@ -3960,7 +3847,7 @@ 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)
+	else if (BTC_ASSOCIATE_FINISH == type)
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], CONNECT FINISH notify\n");
 }
@@ -3968,7 +3855,7 @@ void ex_halbtc8192e2ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
 void ex_halbtc8192e2ant_media_status_notify(struct btc_coexist *btcoexist,
 					    u8 type)
 {
-	u8 h2c_parameter[3] ={0};
+	u8 h2c_parameter[3] = {0};
 	u32 wifi_bw;
 	u8 wifi_center_chnl;
 
@@ -4014,12 +3901,12 @@ void ex_halbtc8192e2ant_special_packet_notify(struct btc_coexist *btcoexist,
 					      u8 type)
 {
 	if (type == BTC_PACKET_DHCP)
- 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+		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 *tmp_buf, u8 length)
 {
 	u8 bt_info = 0;
 	u8 i, rspSource = 0;
@@ -4089,18 +3976,16 @@ void ex_halbtc8192e2ant_bt_info_notify(struct btc_coexist *btcoexist,
 			 * do nothing here. */
 		}
 
-#if(BT_AUTO_REPORT_ONLY_8192E_2ANT == 0)
 		if ((coex_sta->bt_info_ext & BIT4)) {
 			/* BT auto report already enabled, do nothing */
 		} else {
 			halbtc8192e2ant_bt_autoreport(btcoexist, FORCE_EXEC,
 						      true);
 		}
-#endif
 	}
 
 	/* check BIT2 first ==> check if bt is under inquiry or page scan */
-	if(bt_info & BT_INFO_8192E_2ANT_B_INQ_PAGE)
+	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;
@@ -4180,10 +4065,10 @@ void ex_halbtc8192e2ant_stack_operation_notify(struct btc_coexist *btcoexist,
 {
 	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)
+			  "[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");
+			  "[BTCoex] StackOP Inquiry/page/pair finish notify\n");
 }
 
 void ex_halbtc8192e2ant_halt_notify(struct btc_coexist *btcoexist)
@@ -4196,10 +4081,10 @@ void ex_halbtc8192e2ant_halt_notify(struct btc_coexist *btcoexist)
 
 void ex_halbtc8192e2ant_periodical(struct btc_coexist *btcoexist)
 {
-	static u8 dis_ver_info_cnt = 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;
+	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");
@@ -4226,17 +4111,8 @@ void ex_halbtc8192e2ant_periodical(struct btc_coexist *btcoexist)
 			  "************************************************\n");
 	}
 
-#if(BT_AUTO_REPORT_ONLY_8192E_2ANT == 0)
 	halbtc8192e2ant_querybt_info(btcoexist);
 	halbtc8192e2ant_monitor_bt_ctr(btcoexist);
 	halbtc8192e2ant_monitor_bt_enable_disable(btcoexist);
-#else
-	if (halbtc8192e2ant_iswifi_status_changed(btcoexist) ||
-	    coex_dm->auto_tdma_adjust)
-		halbtc8192e2ant_run_coexist_mechanism(btcoexist);
-#endif
 }
 
-
-#endif
-
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.h b/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.h
index 6d109edb895..416d3ddaed3 100644
--- a/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.h
+++ b/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.h
@@ -1,8 +1,6 @@
 /*****************************************************************
  *   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
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8723b1ant.c b/drivers/staging/rtl8821ae/btcoexist/halbtc8723b1ant.c
index 9677943fc20..c9693944c13 100644
--- a/drivers/staging/rtl8821ae/btcoexist/halbtc8723b1ant.c
+++ b/drivers/staging/rtl8821ae/btcoexist/halbtc8723b1ant.c
@@ -22,14 +22,14 @@ 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;
 
-const char *const GLBtInfoSrc8723b1Ant[]={
+static const char *const GLBtInfoSrc8723b1Ant[] = {
 	"BT Info[wifi fw]",
 	"BT Info[bt rsp]",
 	"BT Info[bt auto report]",
 };
 
-u32 glcoex_ver_date_8723b_1ant = 20130906;
-u32 glcoex_ver_8723b_1ant = 0x45;
+static u32 glcoex_ver_date_8723b_1ant = 20130906;
+static u32 glcoex_ver_8723b_1ant = 0x45;
 
 /***************************************************************
  * local function proto type if needed
@@ -37,7 +37,8 @@ u32 glcoex_ver_8723b_1ant = 0x45;
 /***************************************************************
  * local function start with halbtc8723b1ant_
  ***************************************************************/
-u8 halbtc8723b1ant_bt_rssi_state(u8 level_num, u8 rssi_thresh, u8 rssi_thresh1)
+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;
@@ -134,9 +135,9 @@ u8 halbtc8723b1ant_bt_rssi_state(u8 level_num, u8 rssi_thresh, u8 rssi_thresh1)
 	return bt_rssi_state;
 }
 
-u8 halbtc8723b1ant_wifi_rssi_state(struct btc_coexist *btcoexist,
-				   u8 index, u8 level_num,
-				   u8 rssi_thresh, u8 rssi_thresh1)
+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];
@@ -249,8 +250,8 @@ u8 halbtc8723b1ant_wifi_rssi_state(struct btc_coexist *btcoexist,
 	return wifi_rssi_state;
 }
 
-void halbtc8723b1ant_updatera_mask(struct btc_coexist *btcoexist,
-				   bool force_exec, u32 dis_rate_mask)
+static void halbtc8723b1ant_updatera_mask(struct btc_coexist *btcoexist,
+					bool force_exec, u32 dis_rate_mask)
 {
 	coex_dm->curra_mask = dis_rate_mask;
 
@@ -261,8 +262,9 @@ void halbtc8723b1ant_updatera_mask(struct btc_coexist *btcoexist,
 	coex_dm->prera_mask = coex_dm->curra_mask;
 }
 
-void halbtc8723b1ant_auto_rate_fallback_retry(struct btc_coexist *btcoexist,
-					      bool force_exec, u8 type)
+static void halbtc8723b1ant_auto_rate_fallback_retry(
+						struct btc_coexist *btcoexist,
+						bool force_exec, u8 type)
 {
 	bool wifi_under_bmode = false;
 
@@ -300,8 +302,8 @@ void halbtc8723b1ant_auto_rate_fallback_retry(struct btc_coexist *btcoexist,
 	coex_dm->pre_arfr_type = coex_dm->cur_arfr_type;
 }
 
-void halbtc8723b1ant_retry_limit(struct btc_coexist *btcoexist,
-				 bool force_exec, u8 type)
+static void halbtc8723b1ant_retry_limit(struct btc_coexist *btcoexist,
+					bool force_exec, u8 type)
 {
 	coex_dm->cur_retry_limit_type = type;
 
@@ -324,8 +326,8 @@ void halbtc8723b1ant_retry_limit(struct btc_coexist *btcoexist,
 	coex_dm->pre_retry_limit_type = coex_dm->cur_retry_limit_type;
 }
 
-void halbtc8723b1ant_ampdu_maxtime(struct btc_coexist *btcoexist,
-				   bool force_exec, u8 type)
+static void halbtc8723b1ant_ampdu_maxtime(struct btc_coexist *btcoexist,
+					bool force_exec, u8 type)
 {
 	coex_dm->cur_ampdu_time_type = type;
 
@@ -348,7 +350,7 @@ void halbtc8723b1ant_ampdu_maxtime(struct btc_coexist *btcoexist,
 	coex_dm->pre_ampdu_time_type = coex_dm->cur_ampdu_time_type;
 }
 
-void halbtc8723b1ant_limited_tx(struct btc_coexist *btcoexist,
+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)
 {
@@ -375,7 +377,7 @@ void halbtc8723b1ant_limited_tx(struct btc_coexist *btcoexist,
 	halbtc8723b1ant_ampdu_maxtime(btcoexist, force_exec, ampdu_time_type);
 }
 
-void halbtc8723b1ant_limited_rx(struct btc_coexist *btcoexist,
+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)
 {
@@ -398,7 +400,7 @@ void halbtc8723b1ant_limited_rx(struct btc_coexist *btcoexist,
 	btcoexist->btc_set(btcoexist, BTC_SET_ACT_AGGREGATE_CTRL, NULL);
 }
 
-void halbtc8723b1ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
+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;
@@ -424,7 +426,7 @@ void halbtc8723b1ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
 	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
 }
 
-void halbtc8723b1ant_query_bt_info(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_query_bt_info(struct btc_coexist *btcoexist)
 {
 	u8 h2c_parameter[1] = {0};
 
@@ -439,7 +441,8 @@ void halbtc8723b1ant_query_bt_info(struct btc_coexist *btcoexist)
 	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
 }
 
-bool halbtc8723b1ant_is_wifi_status_changed(struct btc_coexist *btcoexist)
+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;
@@ -471,7 +474,7 @@ bool halbtc8723b1ant_is_wifi_status_changed(struct btc_coexist *btcoexist)
 	return false;
 }
 
-void halbtc8723b1ant_update_bt_link_info(struct btc_coexist *btcoexist)
+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;
@@ -519,7 +522,7 @@ void halbtc8723b1ant_update_bt_link_info(struct btc_coexist *btcoexist)
 		bt_link_info->hid_only = false;
 }
 
-u8 halbtc8723b1ant_action_algorithm(struct btc_coexist *btcoexist)
+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;
@@ -719,7 +722,7 @@ u8 halbtc8723b1ant_action_algorithm(struct btc_coexist *btcoexist)
 	return algorithm;
 }
 
-bool halbtc8723b1ant_need_to_dec_bt_pwr(struct btc_coexist *btcoexist)
+static bool halbtc8723b1ant_need_to_dec_bt_pwr(struct btc_coexist *btcoexist)
 {
 	bool ret = false;
 	bool bt_hs_on = false, wifi_connected = false;
@@ -750,8 +753,9 @@ bool halbtc8723b1ant_need_to_dec_bt_pwr(struct btc_coexist *btcoexist)
 	return ret;
 }
 
-void halbtc8723b1ant_set_fw_dac_swing_level(struct btc_coexist *btcoexist,
-					    u8 dac_swing_lvl)
+static void halbtc8723b1ant_set_fw_dac_swing_level(
+						struct btc_coexist *btcoexist,
+						u8 dac_swing_lvl)
 {
 	u8 h2c_parameter[1] = {0};
 
@@ -767,7 +771,7 @@ void halbtc8723b1ant_set_fw_dac_swing_level(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x64, 1, h2c_parameter);
 }
 
-void halbtc8723b1ant_set_fw_dec_bt_pwr(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_set_fw_dec_bt_pwr(struct btc_coexist *btcoexist,
 				       bool dec_bt_pwr)
 {
 	u8 h2c_parameter[1] = {0};
@@ -784,7 +788,7 @@ void halbtc8723b1ant_set_fw_dec_bt_pwr(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x62, 1, h2c_parameter);
 }
 
-void halbtc8723b1ant_dec_bt_pwr(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_dec_bt_pwr(struct btc_coexist *btcoexist,
 				bool force_exec, bool dec_bt_pwr)
 {
 	return;
@@ -806,7 +810,8 @@ void halbtc8723b1ant_dec_bt_pwr(struct btc_coexist *btcoexist,
 	coex_dm->pre_dec_bt_pwr = coex_dm->cur_dec_bt_pwr;
 }
 
-void halbtc8723b1ant_set_bt_auto_report(struct btc_coexist *btcoexist,
+#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};
@@ -824,7 +829,7 @@ void halbtc8723b1ant_set_bt_auto_report(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x68, 1, h2c_parameter);
 }
 
-void halbtc8723b1ant_bt_auto_report(struct btc_coexist *btcoexist,
+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,
@@ -848,8 +853,9 @@ void halbtc8723b1ant_bt_auto_report(struct btc_coexist *btcoexist,
 
 	coex_dm->pre_bt_auto_report = coex_dm->cur_bt_auto_report;
 }
+#endif
 
-void halbtc8723b1ant_fw_dac_swing_lvl(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_fw_dac_swing_lvl(struct btc_coexist *btcoexist,
 				      bool force_exec, u8 fw_dac_swing_lvl)
 {
 	return;
@@ -876,55 +882,7 @@ void halbtc8723b1ant_fw_dac_swing_lvl(struct btc_coexist *btcoexist,
 	coex_dm->pre_fw_dac_swing_lvl = coex_dm->cur_fw_dac_swing_lvl;
 }
 
-void halbtc8723b1ant_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, 0xffff7);
-	} 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);
-		}
-	}
-}
-
-void halbtc8723b1ant_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;
-	}
-	halbtc8723b1ant_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;
-}
-
-void halbtc8723b1ant_set_sw_penalty_tx_rate_adaptive(
+static void halbtc8723b1ant_set_sw_penalty_tx_rate_adaptive(
 					struct btc_coexist *btcoexist,
 					bool low_penalty_ra)
 {
@@ -948,7 +906,7 @@ void halbtc8723b1ant_set_sw_penalty_tx_rate_adaptive(
 	btcoexist->btc_fill_h2c(btcoexist, 0x69, 6, h2c_parameter);
 }
 
-void halbtc8723b1ant_low_penalty_ra(struct btc_coexist *btcoexist,
+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;
@@ -963,152 +921,7 @@ void halbtc8723b1ant_low_penalty_ra(struct btc_coexist *btcoexist,
 	coex_dm->pre_low_penalty_ra = coex_dm->cur_low_penalty_ra;
 }
 
-void halbtc8723b1ant_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);
-}
-
-void halbtc8723b1ant_set_sw_full_time_dac_swing(struct btc_coexist *btcoexist,
-						bool sw_dac_swing_on,
-						u32 sw_dac_swing_lvl)
-{
-	if (sw_dac_swing_on)
-		halbtc8723b1ant_set_dac_swing_reg(btcoexist, sw_dac_swing_lvl);
-	else
-		halbtc8723b1ant_set_dac_swing_reg(btcoexist, 0x18);
-}
-
-
-void halbtc8723b1ant_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);
-	halbtc8723b1ant_set_sw_full_time_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;
-}
-
-void halbtc8723b1ant_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, 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 halbtc8723b1ant_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_backoff = adc_backoff;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], bPreAdcBackOff=%d, bCurAdcBackOff=%d\n",
-			  coex_dm->pre_adc_backoff, coex_dm->cur_adc_backoff);
-
-		if(coex_dm->pre_adc_backoff == coex_dm->cur_adc_backoff)
-			return;
-	}
-	halbtc8723b1ant_set_adc_backoff(btcoexist, coex_dm->cur_adc_backoff);
-
-	coex_dm->pre_adc_backoff =
-		coex_dm->cur_adc_backoff;
-}
-
-void halbtc8723b1ant_set_agc_table(struct btc_coexist *btcoexist,
-				   bool adc_table_en)
-{
-	u8 rssi_adjust_val = 0;
-
-	btcoexist->btc_set_rf_reg(btcoexist,
-		BTC_RF_A, 0xef, 0xfffff, 0x02000);
-	if (adc_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 halbtc8723b1ant_agc_table(struct btc_coexist *btcoexist,
-			       bool force_exec, bool adc_table_en)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s %s Agc Table\n",
-		  (force_exec ? "force to" : ""),
-		  (adc_table_en ? "Enable" : "Disable"));
-	coex_dm->cur_agc_table_en = adc_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;
-	}
-	halbtc8723b1ant_set_agc_table(btcoexist, adc_table_en);
-
-	coex_dm->pre_agc_table_en = coex_dm->cur_agc_table_en;
-}
-
-void halbtc8723b1ant_set_coex_table(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_set_coex_table(struct btc_coexist *btcoexist,
 				    u32 val0x6c0, u32 val0x6c4,
 				    u32 val0x6c8, u8 val0x6cc)
 {
@@ -1129,7 +942,7 @@ void halbtc8723b1ant_set_coex_table(struct btc_coexist *btcoexist,
 	btcoexist->btc_write_1byte(btcoexist, 0x6cc, val0x6cc);
 }
 
-void halbtc8723b1ant_coex_table(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_coex_table(struct btc_coexist *btcoexist,
  				bool force_exec, u32 val0x6c0,
  				u32 val0x6c4, u32 val0x6c8,
  				u8 val0x6cc)
@@ -1159,7 +972,7 @@ void halbtc8723b1ant_coex_table(struct btc_coexist *btcoexist,
 	coex_dm->pre_val0x6cc = coex_dm->cur_val0x6cc;
 }
 
-void halbtc8723b1ant_coex_table_with_type(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_coex_table_with_type(struct btc_coexist *btcoexist,
 					  bool force_exec, u8 type)
 {
 	switch (type) {
@@ -1200,7 +1013,7 @@ void halbtc8723b1ant_coex_table_with_type(struct btc_coexist *btcoexist,
 	}
 }
 
-void halbtc8723b1ant_SetFwIgnoreWlanAct(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_SetFwIgnoreWlanAct(struct btc_coexist *btcoexist,
 					bool enable)
 {
 	u8 h2c_parameter[1] = {0};
@@ -1215,7 +1028,7 @@ void halbtc8723b1ant_SetFwIgnoreWlanAct(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x63, 1, h2c_parameter);
 }
 
-void halbtc8723b1ant_ignore_wlan_act(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_ignore_wlan_act(struct btc_coexist *btcoexist,
 				     bool force_exec, bool enable)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
@@ -1239,7 +1052,7 @@ void halbtc8723b1ant_ignore_wlan_act(struct btc_coexist *btcoexist,
 	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
 }
 
-void halbtc8723b1ant_set_fw_ps_tdma(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_set_fw_ps_tdma(struct btc_coexist *btcoexist,
 				    u8 byte1, u8 byte2, u8 byte3,
 				    u8 byte4, u8 byte5)
 {
@@ -1285,7 +1098,7 @@ void halbtc8723b1ant_set_fw_ps_tdma(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x60, 5, h2c_parameter);
 }
 
-void halbtc8723b1ant_SetLpsRpwm(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_SetLpsRpwm(struct btc_coexist *btcoexist,
 				u8 lps_val, u8 rpwm_val)
 {
 	u8 lps = lps_val;
@@ -1295,8 +1108,8 @@ void halbtc8723b1ant_SetLpsRpwm(struct btc_coexist *btcoexist,
 	btcoexist->btc_set(btcoexist, BTC_SET_U1_1ANT_RPWM, &rpwm);
 }
 
-void halbtc8723b1ant_LpsRpwm(struct btc_coexist *btcoexist, bool force_exec,
-			     u8 lps_val, u8 rpwm_val)
+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,
@@ -1326,7 +1139,7 @@ void halbtc8723b1ant_LpsRpwm(struct btc_coexist *btcoexist, bool force_exec,
 	coex_dm->pre_rpwm = coex_dm->cur_rpwm;
 }
 
-void halbtc8723b1ant_sw_mechanism1(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_sw_mechanism1(struct btc_coexist *btcoexist,
 				   bool shrink_rx_lpf, bool low_penalty_ra,
 				   bool limited_dig, bool bt_lna_constrain)
 {
@@ -1337,7 +1150,7 @@ void halbtc8723b1ant_sw_mechanism1(struct btc_coexist *btcoexist,
 	halbtc8723b1ant_low_penalty_ra(btcoexist, NORMAL_EXEC, low_penalty_ra);
 }
 
-void halbtc8723b1ant_sw_mechanism2(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_sw_mechanism2(struct btc_coexist *btcoexist,
 				   bool agc_table_shift, bool adc_backoff,
 				   bool sw_dac_swing, u32 dac_swing_lvl)
 {
@@ -1346,7 +1159,7 @@ void halbtc8723b1ant_sw_mechanism2(struct btc_coexist *btcoexist,
 		  agc_table_shift, adc_backoff, sw_dac_swing);
 }
 
-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)
 {
@@ -1537,8 +1350,8 @@ void halbtc8723b1ant_SetAntPath(struct btc_coexist *btcoexist,
 	}
 }
 
-void halbtc8723b1ant_ps_tdma(struct btc_coexist *btcoexist, bool force_exec,
-			     bool turn_on, u8 type)
+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;
@@ -1783,22 +1596,7 @@ void halbtc8723b1ant_ps_tdma(struct btc_coexist *btcoexist, bool force_exec,
 	coex_dm->pre_ps_tdma = coex_dm->cur_ps_tdma;
 }
 
-void halbtc8723b1ant_coex_alloff(struct btc_coexist *btcoexist)
-{
-	/* fw all off */
-	halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-	halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	/* sw all off */
-	halbtc8723b1ant_sw_mechanism1(btcoexist, false, false, false, false);
-	halbtc8723b1ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
-
-
-	/* hw all off */
-	halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
-}
-
-bool halbtc8723b1ant_is_common_action(struct btc_coexist *btcoexist)
+static bool halbtc8723b1ant_is_common_action(struct btc_coexist *btcoexist)
 {
 	bool commom = false, wifi_connected = false;
 	bool wifi_busy = false;
@@ -1896,8 +1694,9 @@ bool halbtc8723b1ant_is_common_action(struct btc_coexist *btcoexist)
 }
 
 
-void halbtc8723b1ant_tdma_duration_adjust_for_acl(struct btc_coexist *btcoexist,
-						  u8 wifi_status)
+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,
@@ -2082,28 +1881,8 @@ void halbtc8723b1ant_tdma_duration_adjust_for_acl(struct btc_coexist *btcoexist,
 	}
 }
 
-u8 halbtc8723b1ant_ps_tdma_type_by_wifi_rssi(s32 wifi_rssi, s32 pre_wifi_rssi,
-					     u8 wifi_rssi_thresh)
-{
-	u8 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 halbtc8723b1ant_PsTdmaCheckForPowerSaveState(struct btc_coexist *btcoexist,
-						  bool new_ps_state)
+static void halbtc8723b1ant_PsTdmaCheckForPowerSaveState(
+			struct btc_coexist *btcoexist, bool new_ps_state)
 {
 	u8 lps_mode = 0x0;
 
@@ -2128,7 +1907,7 @@ void halbtc8723b1ant_PsTdmaCheckForPowerSaveState(struct btc_coexist *btcoexist,
 	}
 }
 
-void halbtc8723b1ant_power_save_state(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_power_save_state(struct btc_coexist *btcoexist,
 				      u8 ps_type, u8 lps_val,
 				      u8 rpwm_val)
 {
@@ -2162,13 +1941,15 @@ void halbtc8723b1ant_power_save_state(struct btc_coexist *btcoexist,
 	}
 }
 
-void halbtc8723b1ant_action_wifi_only(struct btc_coexist *btcoexist)
+#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);
 }
 
-void halbtc8723b1ant_monitor_bt_enable_disable(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_monitor_bt_enable_disable(
+						struct btc_coexist *btcoexist)
 {
 	static bool pre_bt_disabled = false;
 	static u32 bt_disable_cnt = 0;
@@ -2224,6 +2005,7 @@ void halbtc8723b1ant_monitor_bt_enable_disable(struct btc_coexist *btcoexist)
 		}
 	}
 }
+#endif
 
 /***************************************************
  *
@@ -2231,7 +2013,7 @@ void halbtc8723b1ant_monitor_bt_enable_disable(struct btc_coexist *btcoexist)
  *
  ***************************************************/
 /* SCO only or SCO+PAN(HS) */
-void halbtc8723b1ant_action_sco(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_sco(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state;
 	u32 wifi_bw;
@@ -2280,7 +2062,7 @@ void halbtc8723b1ant_action_sco(struct btc_coexist *btcoexist)
 }
 
 
-void halbtc8723b1ant_action_hid(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_hid(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state;
 	u32 wifi_bw;
@@ -2331,7 +2113,7 @@ void halbtc8723b1ant_action_hid(struct btc_coexist *btcoexist)
 }
 
 /*A2DP only / PAN(EDR) only/ A2DP+PAN(HS) */
-void halbtc8723b1ant_action_a2dp(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_a2dp(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state;
 	u32 wifi_bw;
@@ -2380,7 +2162,7 @@ void halbtc8723b1ant_action_a2dp(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b1ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
 	u32 wifi_bw;
@@ -2430,7 +2212,7 @@ void halbtc8723b1ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b1ant_action_pan_edr(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_pan_edr(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state;
 	u32 wifi_bw;
@@ -2481,7 +2263,7 @@ void halbtc8723b1ant_action_pan_edr(struct btc_coexist *btcoexist)
 
 
 /* PAN(HS) only */
-void halbtc8723b1ant_action_pan_hs(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_pan_hs(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state;
 	u32 wifi_bw;
@@ -2544,7 +2326,7 @@ void halbtc8723b1ant_action_pan_hs(struct btc_coexist *btcoexist)
 }
 
 /*PAN(EDR)+A2DP */
-void halbtc8723b1ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
 	u32 wifi_bw;
@@ -2595,7 +2377,7 @@ void halbtc8723b1ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b1ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state;
 	u32 wifi_bw;
@@ -2645,7 +2427,8 @@ void halbtc8723b1ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
 }
 
 /* HID+A2DP+PAN(EDR) */
-void halbtc8723b1ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
+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;
@@ -2695,7 +2478,7 @@ void halbtc8723b1ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b1ant_action_hid_a2dp(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_hid_a2dp(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
 	u32 wifi_bw;
@@ -2748,13 +2531,13 @@ void halbtc8723b1ant_action_hid_a2dp(struct btc_coexist *btcoexist)
  *	Non-Software Coex Mechanism start
  *
  *****************************************************/
-void halbtc8723b1ant_action_hs(struct btc_coexist *btcoexist)
+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);
 }
 
-void halbtc8723b1ant_action_bt_inquiry(struct btc_coexist *btcoexist)
+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;
@@ -2790,8 +2573,9 @@ void halbtc8723b1ant_action_bt_inquiry(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b1ant_action_bt_sco_hid_only_busy(struct btc_coexist * btcoexist,
-						 u8 wifi_status)
+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;
@@ -2810,7 +2594,7 @@ void halbtc8723b1ant_action_bt_sco_hid_only_busy(struct btc_coexist * btcoexist,
 	}
 }
 
-void halbtc8723b1ant_action_wifi_connected_bt_acl_busy(
+static void halbtc8723b1ant_action_wifi_connected_bt_acl_busy(
 					struct btc_coexist *btcoexist,
 					u8 wifi_status)
 {
@@ -2870,7 +2654,8 @@ void halbtc8723b1ant_action_wifi_connected_bt_acl_busy(
 	}
 }
 
-void halbtc8723b1ant_action_wifi_not_connected(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_wifi_not_connected(
+						struct btc_coexist *btcoexist)
 {
 	/* power save state */
 	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
@@ -2881,7 +2666,7 @@ void halbtc8723b1ant_action_wifi_not_connected(struct btc_coexist *btcoexist)
 	halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
 }
 
-void halbtc8723b1ant_action_wifi_not_connected_asso_auth_scan(
+static void halbtc8723b1ant_action_wifi_not_connected_asso_auth_scan(
 						struct btc_coexist *btcoexist)
 {
 	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
@@ -2891,7 +2676,8 @@ void halbtc8723b1ant_action_wifi_not_connected_asso_auth_scan(
 	halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
 }
 
-void halbtc8723b1ant_ActionWifiConnectedScan(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_ActionWifiConnectedScan(
+						struct btc_coexist *btcoexist)
 {
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
 
@@ -2921,7 +2707,7 @@ void halbtc8723b1ant_ActionWifiConnectedScan(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b1ant_action_wifi_connected_special_packet(
+static void halbtc8723b1ant_action_wifi_connected_special_packet(
 						struct btc_coexist *btcoexist)
 {
 	bool hs_connecting = false;
@@ -2951,7 +2737,7 @@ void halbtc8723b1ant_action_wifi_connected_special_packet(
 	}
 }
 
-void halbtc8723b1ant_action_wifi_connected(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_wifi_connected(struct btc_coexist *btcoexist)
 {
 	bool wifi_busy = false;
 	bool scan = false, link = false, roam = false;
@@ -3031,7 +2817,8 @@ void halbtc8723b1ant_action_wifi_connected(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b1ant_run_sw_coexist_mechanism(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_run_sw_coexist_mechanism(
+						struct btc_coexist *btcoexist)
 {
 	u8 algorithm = 0;
 
@@ -3104,7 +2891,7 @@ void halbtc8723b1ant_run_sw_coexist_mechanism(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b1ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
+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;
@@ -3218,7 +3005,7 @@ void halbtc8723b1ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b1ant_init_coex_dm(struct btc_coexist *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);
@@ -3232,7 +3019,8 @@ void halbtc8723b1ant_init_coex_dm(struct btc_coexist *btcoexist)
 	halbtc8723b1ant_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
 }
 
-void halbtc8723b1ant_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;
@@ -3304,7 +3092,7 @@ void halbtc8723b1ant_init_hw_config(struct btc_coexist *btcoexist, bool backup)
 
 }
 
-void halbtc8723b1ant_wifi_off_hw_cfg(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_wifi_off_hw_cfg(struct btc_coexist *btcoexist)
 {
 	/* set wlan_act to low */
 	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0);
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8723b2ant.c b/drivers/staging/rtl8821ae/btcoexist/halbtc8723b2ant.c
index d337bd0b3c1..da3f62da4e4 100644
--- a/drivers/staging/rtl8821ae/btcoexist/halbtc8723b2ant.c
+++ b/drivers/staging/rtl8821ae/btcoexist/halbtc8723b2ant.c
@@ -20,14 +20,14 @@ 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;
 
-const char *const glbt_info_src_8723b_2ant[] = {
+static const char *const glbt_info_src_8723b_2ant[] = {
 	"BT Info[wifi fw]",
 	"BT Info[bt rsp]",
 	"BT Info[bt auto report]",
 };
 
-u32 glcoex_ver_date_8723b_2ant = 20131113;
-u32 glcoex_ver_8723b_2ant = 0x3f;
+static u32 glcoex_ver_date_8723b_2ant = 20131113;
+static u32 glcoex_ver_8723b_2ant = 0x3f;
 
 /**************************************************************
  * local function proto type if needed
@@ -35,7 +35,8 @@ u32 glcoex_ver_8723b_2ant = 0x3f;
 /**************************************************************
  * local function start with halbtc8723b2ant_
  **************************************************************/
-u8 halbtc8723b2ant_bt_rssi_state(u8 level_num, u8 rssi_thresh, u8 rssi_thresh1)
+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;
@@ -132,7 +133,7 @@ u8 halbtc8723b2ant_bt_rssi_state(u8 level_num, u8 rssi_thresh, u8 rssi_thresh1)
 	return bt_rssi_state;
 }
 
-u8 halbtc8723b2ant_wifi_rssi_state(struct btc_coexist *btcoexist,
+static u8 halbtc8723b2ant_wifi_rssi_state(struct btc_coexist *btcoexist,
 				   u8 index, u8 level_num,
 				   u8 rssi_thresh, u8 rssi_thresh1)
 {
@@ -246,7 +247,9 @@ u8 halbtc8723b2ant_wifi_rssi_state(struct btc_coexist *btcoexist,
 	return wifi_rssi_state;
 }
 
-void halbtc8723b2ant_monitor_bt_enable_disable(struct btc_coexist *btcoexist)
+#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;
@@ -298,8 +301,9 @@ void halbtc8723b2ant_monitor_bt_enable_disable(struct btc_coexist *btcoexist)
 		}
 	}
 }
+#endif
 
-void halbtc8723b2ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
+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;
@@ -332,7 +336,7 @@ void halbtc8723b2ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
 	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
 }
 
-void halbtc8723b2ant_query_bt_info(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_query_bt_info(struct btc_coexist *btcoexist)
 {
 	u8 h2c_parameter[1] ={0};
 
@@ -347,7 +351,8 @@ void halbtc8723b2ant_query_bt_info(struct btc_coexist *btcoexist)
 	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
 }
 
-bool halbtc8723b2ant_is_wifi_status_changed(struct btc_coexist *btcoexist)
+static bool halbtc8723b2ant_is_wifi_status_changed(
+						struct btc_coexist *btcoexist)
 {
 	static bool pre_wifi_busy = false;
 	static bool pre_under_4way = false;
@@ -382,7 +387,7 @@ bool halbtc8723b2ant_is_wifi_status_changed(struct btc_coexist *btcoexist)
 	return false;
 }
 
-void halbtc8723b2ant_update_bt_link_info(struct btc_coexist *btcoexist)
+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;
@@ -446,7 +451,7 @@ void halbtc8723b2ant_update_bt_link_info(struct btc_coexist *btcoexist)
 		bt_link_info->hid_only = false;
 }
 
-u8 halbtc8723b2ant_action_algorithm(struct btc_coexist *btcoexist)
+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;
@@ -634,7 +639,7 @@ u8 halbtc8723b2ant_action_algorithm(struct btc_coexist *btcoexist)
 	return algorithm;
 }
 
-bool halbtc8723b2ant_need_to_dec_bt_pwr(struct btc_coexist *btcoexist)
+static bool halbtc8723b2ant_need_to_dec_bt_pwr(struct btc_coexist *btcoexist)
 {
 	bool bRet = false;
 	bool bt_hs_on = false, wifi_connected = false;
@@ -675,8 +680,9 @@ bool halbtc8723b2ant_need_to_dec_bt_pwr(struct btc_coexist *btcoexist)
 	return bRet;
 }
 
-void halbtc8723b2ant_set_fw_dac_swing_level(struct btc_coexist *btcoexist,
-					    u8 dac_swing_lvl)
+static void halbtc8723b2ant_set_fw_dac_swing_level(
+						struct btc_coexist *btcoexist,
+						u8 dac_swing_lvl)
 {
 	u8 h2c_parameter[1] ={0};
 
@@ -692,7 +698,7 @@ void halbtc8723b2ant_set_fw_dac_swing_level(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x64, 1, h2c_parameter);
 }
 
-void halbtc8723b2ant_set_fw_dec_bt_pwr(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_set_fw_dec_bt_pwr(struct btc_coexist *btcoexist,
 				       bool dec_bt_pwr)
 {
 	u8 h2c_parameter[1] = {0};
@@ -709,8 +715,8 @@ void halbtc8723b2ant_set_fw_dec_bt_pwr(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x62, 1, h2c_parameter);
 }
 
-void halbtc8723b2ant_dec_bt_pwr(struct btc_coexist *btcoexist,
-				bool force_exec, bool dec_bt_pwr)
+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",
@@ -730,7 +736,8 @@ void halbtc8723b2ant_dec_bt_pwr(struct btc_coexist *btcoexist,
 	coex_dm->pre_dec_bt_pwr = coex_dm->cur_dec_bt_pwr;
 }
 
-void halbtc8723b2ant_set_bt_auto_report(struct btc_coexist *btcoexist,
+#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};
@@ -747,7 +754,7 @@ void halbtc8723b2ant_set_bt_auto_report(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x68, 1, h2c_parameter);
 }
 
-void halbtc8723b2ant_bt_auto_report(struct btc_coexist *btcoexist,
+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,
@@ -771,8 +778,9 @@ void halbtc8723b2ant_bt_auto_report(struct btc_coexist *btcoexist,
 
 	coex_dm->pre_bt_auto_report = coex_dm->cur_bt_auto_report;
 }
+#endif
 
-void halbtc8723b2ant_fw_dac_swing_lvl(struct btc_coexist *btcoexist,
+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,
@@ -797,8 +805,9 @@ void halbtc8723b2ant_fw_dac_swing_lvl(struct btc_coexist *btcoexist,
 	coex_dm->pre_fw_dac_swing_lvl = coex_dm->cur_fw_dac_swing_lvl;
 }
 
-void halbtc8723b2ant_set_sw_rf_rx_lpf_corner(struct btc_coexist *btcoexist,
-					     bool rx_rf_shrink_on)
+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 */
@@ -819,7 +828,7 @@ void halbtc8723b2ant_set_sw_rf_rx_lpf_corner(struct btc_coexist *btcoexist,
 	}
 }
 
-void halbtc8723b2ant_rf_shrink(struct btc_coexist *btcoexist,
+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,
@@ -844,7 +853,7 @@ void halbtc8723b2ant_rf_shrink(struct btc_coexist *btcoexist,
 	coex_dm->pre_rf_rx_lpf_shrink = coex_dm->cur_rf_rx_lpf_shrink;
 }
 
-void halbtc8723b2ant_set_sw_penalty_txrate_adaptive(
+static void halbtc8723b2ant_set_sw_penalty_txrate_adaptive(
 						struct btc_coexist *btcoexist,
 						bool low_penalty_ra)
 {
@@ -868,7 +877,7 @@ void halbtc8723b2ant_set_sw_penalty_txrate_adaptive(
 	btcoexist->btc_fill_h2c(btcoexist, 0x69, 6, h2c_parameter);
 }
 
-void halbtc8723b2ant_low_penalty_ra(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_low_penalty_ra(struct btc_coexist *btcoexist,
 				    bool force_exec, bool low_penalty_ra)
 {
 	/*return; */
@@ -893,7 +902,7 @@ void halbtc8723b2ant_low_penalty_ra(struct btc_coexist *btcoexist,
 	coex_dm->pre_low_penalty_ra = coex_dm->cur_low_penalty_ra;
 }
 
-void halbtc8723b2ant_set_dac_swing_reg(struct btc_coexist * btcoexist,
+static void halbtc8723b2ant_set_dac_swing_reg(struct btc_coexist *btcoexist,
 				       u32 level)
 {
 	u8 val = (u8) level;
@@ -902,9 +911,10 @@ void halbtc8723b2ant_set_dac_swing_reg(struct btc_coexist * btcoexist,
 	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x883, 0x3e, val);
 }
 
-void halbtc8723b2ant_set_sw_fulltime_dac_swing(struct btc_coexist *btcoexist,
-					       bool sw_dac_swing_on,
-					       u32 sw_dac_swing_lvl)
+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);
@@ -913,7 +923,7 @@ void halbtc8723b2ant_set_sw_fulltime_dac_swing(struct btc_coexist *btcoexist,
 }
 
 
-void halbtc8723b2ant_dac_swing(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_dac_swing(struct btc_coexist *btcoexist,
 			       bool force_exec, bool dac_swing_on,
 			       u32 dac_swing_lvl)
 {
@@ -944,7 +954,8 @@ void halbtc8723b2ant_dac_swing(struct btc_coexist *btcoexist,
 	coex_dm->pre_dac_swing_lvl = coex_dm->cur_dac_swing_lvl;
 }
 
-void halbtc8723b2ant_set_adc_backoff(struct btc_coexist *btcoexist,
+#if 0
+static void halbtc8723b2ant_set_adc_backoff(struct btc_coexist *btcoexist,
 				     bool adc_backoff)
 {
 	if (adc_backoff) {
@@ -958,7 +969,7 @@ void halbtc8723b2ant_set_adc_backoff(struct btc_coexist *btcoexist,
 	}
 }
 
-void halbtc8723b2ant_adc_backoff(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_adc_backoff(struct btc_coexist *btcoexist,
 				 bool force_exec, bool adc_backoff)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
@@ -979,8 +990,9 @@ void halbtc8723b2ant_adc_backoff(struct btc_coexist *btcoexist,
 
 	coex_dm->pre_adc_back_off = coex_dm->cur_adc_back_off;
 }
+#endif
 
-void halbtc8723b2ant_set_agc_table(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_set_agc_table(struct btc_coexist *btcoexist,
 				   bool agc_table_en)
 {
 	u8 rssi_adjust_val = 0;
@@ -1054,7 +1066,7 @@ void halbtc8723b2ant_set_agc_table(struct btc_coexist *btcoexist,
 			   &rssi_adjust_val);
 }
 
-void halbtc8723b2ant_agc_table(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_agc_table(struct btc_coexist *btcoexist,
 			       bool force_exec, bool agc_table_en)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
@@ -1076,7 +1088,7 @@ void halbtc8723b2ant_agc_table(struct btc_coexist *btcoexist,
 	coex_dm->pre_agc_table_en = coex_dm->cur_agc_table_en;
 }
 
-void halbtc8723b2ant_set_coex_table(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_set_coex_table(struct btc_coexist *btcoexist,
 				    u32 val0x6c0, u32 val0x6c4,
 				    u32 val0x6c8, u8 val0x6cc)
 {
@@ -1097,7 +1109,7 @@ void halbtc8723b2ant_set_coex_table(struct btc_coexist *btcoexist,
 	btcoexist->btc_write_1byte(btcoexist, 0x6cc, val0x6cc);
 }
 
-void halbtc8723b2ant_coex_table(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_coex_table(struct btc_coexist *btcoexist,
 				bool force_exec, u32 val0x6c0,
 				u32 val0x6c4, u32 val0x6c8,
 				u8 val0x6cc)
@@ -1141,7 +1153,7 @@ void halbtc8723b2ant_coex_table(struct btc_coexist *btcoexist,
 	coex_dm->pre_val0x6cc = coex_dm->cur_val0x6cc;
 }
 
-void halbtc8723b2ant_coex_table_with_type(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_coex_table_with_type(struct btc_coexist *btcoexist,
 					  bool force_exec, u8 type)
 {
 	switch (type) {
@@ -1202,8 +1214,9 @@ void halbtc8723b2ant_coex_table_with_type(struct btc_coexist *btcoexist,
 	}
 }
 
-void halbtc8723b2ant_set_fw_ignore_wlan_act(struct btc_coexist *btcoexist,
-					    bool enable)
+static void halbtc8723b2ant_set_fw_ignore_wlan_act(
+						struct btc_coexist *btcoexist,
+						bool enable)
 {
 	u8 h2c_parameter[1] ={0};
 
@@ -1217,7 +1230,7 @@ void halbtc8723b2ant_set_fw_ignore_wlan_act(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x63, 1, h2c_parameter);
 }
 
-void halbtc8723b2ant_ignore_wlan_act(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_ignore_wlan_act(struct btc_coexist *btcoexist,
 				     bool force_exec, bool enable)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
@@ -1241,8 +1254,8 @@ void halbtc8723b2ant_ignore_wlan_act(struct btc_coexist *btcoexist,
 	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
 }
 
-void halbtc8723b2ant_set_fw_ps_tdma(struct btc_coexist *btcoexist, u8 byte1,
-				    u8 byte2, u8 byte3, u8 byte4, u8 byte5)
+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};
 
@@ -1267,7 +1280,7 @@ void halbtc8723b2ant_set_fw_ps_tdma(struct btc_coexist *btcoexist, u8 byte1,
 	btcoexist->btc_fill_h2c(btcoexist, 0x60, 5, h2c_parameter);
 }
 
-void halbtc8723b2ant_sw_mechanism1(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_sw_mechanism1(struct btc_coexist *btcoexist,
 				   bool shrink_rx_lpf, bool low_penalty_ra,
 				   bool limited_dig, bool bt_lna_constrain)
 {
@@ -1287,7 +1300,7 @@ void halbtc8723b2ant_sw_mechanism1(struct btc_coexist *btcoexist,
 	halbtc8723b2ant_low_penalty_ra(btcoexist, NORMAL_EXEC, low_penalty_ra);
 }
 
-void halbtc8723b2ant_sw_mechanism2(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_sw_mechanism2(struct btc_coexist *btcoexist,
 				   bool agc_table_shift, bool adc_backoff,
 				   bool sw_dac_swing, u32 dac_swing_lvl)
 {
@@ -1297,7 +1310,7 @@ void halbtc8723b2ant_sw_mechanism2(struct btc_coexist *btcoexist,
 				  dac_swing_lvl);
 }
 
-void halbtc8723b2ant_set_ant_path(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_set_ant_path(struct btc_coexist *btcoexist,
 				  u8 antpos_type, bool init_hwcfg,
 				  bool wifi_off)
 {
@@ -1377,8 +1390,9 @@ void halbtc8723b2ant_set_ant_path(struct btc_coexist *btcoexist,
 }
 
 
-void halbtc8723b2ant_ps_tdma(struct btc_coexist *btcoexist, bool force_exec,
-			     bool turn_on, u8 type)
+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,
@@ -1514,7 +1528,7 @@ void halbtc8723b2ant_ps_tdma(struct btc_coexist *btcoexist, bool force_exec,
 	coex_dm->pre_ps_tdma = coex_dm->cur_ps_tdma;
 }
 
-void halbtc8723b2ant_coex_alloff(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_coex_alloff(struct btc_coexist *btcoexist)
 {
 	/* fw all off */
 	halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
@@ -1530,7 +1544,7 @@ void halbtc8723b2ant_coex_alloff(struct btc_coexist *btcoexist)
 	halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
 }
 
-void halbtc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist)
 {
 	/* force to reset coex mechanism*/
 
@@ -1542,7 +1556,7 @@ void halbtc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist)
 	halbtc8723b2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
 }
 
-void halbtc8723b2ant_action_bt_inquiry(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_action_bt_inquiry(struct btc_coexist *btcoexist)
 {
 	bool wifi_connected = false;
 	bool low_pwr_disable = true;
@@ -1572,7 +1586,7 @@ void halbtc8723b2ant_action_bt_inquiry(struct btc_coexist *btcoexist)
 				     false, false);
 }
 
-bool halbtc8723b2ant_is_common_action(struct btc_coexist *btcoexist)
+static bool halbtc8723b2ant_is_common_action(struct btc_coexist *btcoexist)
 {
 	bool bCommon = false, wifi_connected = false;
 	bool wifi_busy = false;
@@ -1713,7 +1727,8 @@ bool halbtc8723b2ant_is_common_action(struct btc_coexist *btcoexist)
 
 	return bCommon;
 }
-void halbtc8723b2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
+
+static void halbtc8723b2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
 					  bool sco_hid, bool tx_pause,
 					  u8 max_interval)
 {
@@ -2772,7 +2787,7 @@ void halbtc8723b2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
 }
 
 /* SCO only or SCO+PAN(HS) */
-void halbtc8723b2ant_action_sco(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_action_sco(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state;
 	u32 wifi_bw;
@@ -2830,8 +2845,7 @@ void halbtc8723b2ant_action_sco(struct btc_coexist *btcoexist)
 	}
 }
 
-
-void halbtc8723b2ant_action_hid(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_action_hid(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state;
 	u32 wifi_bw;
@@ -2893,7 +2907,7 @@ void halbtc8723b2ant_action_hid(struct btc_coexist *btcoexist)
 }
 
 /*A2DP only / PAN(EDR) only/ A2DP+PAN(HS)*/
-void halbtc8723b2ant_action_a2dp(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_action_a2dp(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, wifi_rssi_state1, bt_rssi_state;
 	u32 wifi_bw;
@@ -2981,7 +2995,7 @@ void halbtc8723b2ant_action_a2dp(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state;
 	u32 wifi_bw;
@@ -3034,7 +3048,7 @@ void halbtc8723b2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b2ant_action_pan_edr(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_action_pan_edr(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state;
 	u32 wifi_bw;
@@ -3091,9 +3105,8 @@ void halbtc8723b2ant_action_pan_edr(struct btc_coexist *btcoexist)
 	}
 }
 
-
 /*PAN(HS) only*/
-void halbtc8723b2ant_action_pan_hs(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_action_pan_hs(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state;
 	u32 wifi_bw;
@@ -3146,7 +3159,7 @@ void halbtc8723b2ant_action_pan_hs(struct btc_coexist *btcoexist)
 }
 
 /*PAN(EDR)+A2DP*/
-void halbtc8723b2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state;
 	u32 wifi_bw;
@@ -3210,7 +3223,7 @@ void halbtc8723b2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state;
 	u32 wifi_bw;
@@ -3282,7 +3295,8 @@ void halbtc8723b2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
 }
 
 /* HID+A2DP+PAN(EDR) */
-void halbtc8723b2ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_action_hid_a2dp_pan_edr(
+						struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state;
 	u32 wifi_bw;
@@ -3346,7 +3360,7 @@ void halbtc8723b2ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state;
 	u32 wifi_bw;
@@ -3404,7 +3418,8 @@ void halbtc8723b2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_run_coexist_mechanism(
+						struct btc_coexist *btcoexist)
 {
 	u8 algorithm = 0;
 
@@ -3525,7 +3540,7 @@ void halbtc8723b2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b2ant_wifioff_hwcfg(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_wifioff_hwcfg(struct btc_coexist *btcoexist)
 {
 	/* set wlan_act to low */
 	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0x4);
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtcoutsrc.c b/drivers/staging/rtl8821ae/btcoexist/halbtcoutsrc.c
index c4e83773ec9..01f5a74028a 100644
--- a/drivers/staging/rtl8821ae/btcoexist/halbtcoutsrc.c
+++ b/drivers/staging/rtl8821ae/btcoexist/halbtcoutsrc.c
@@ -25,7 +25,7 @@
 /***********************************************
  *		Global variables
  ***********************************************/
-const char *const bt_profile_string[]={
+static const char *const bt_profile_string[] = {
 	"NONE",
 	"A2DP",
 	"PAN",
@@ -33,7 +33,7 @@ const char *const bt_profile_string[]={
 	"SCO",
 };
 
-const char *const bt_spec_string[]={
+static const char *const bt_spec_string[] = {
 	"1.0b",
 	"1.1",
 	"1.2",
@@ -43,19 +43,19 @@ const char *const bt_spec_string[]={
 	"4.0",
 };
 
-const char *const bt_link_role_string[]={
+static const char *const bt_link_role_string[] = {
 	"Master",
 	"Slave",
 };
 
-const char *const h2c_state_string[]={
+static const char *const h2c_state_string[] = {
 	"successful",
 	"h2c busy",
 	"rf off",
 	"fw not read",
 };
 
-const char *const io_state_string[]={
+static const char *const io_state_string[] = {
 	"IO_STATUS_SUCCESS",
 	"IO_STATUS_FAIL_CANNOT_IO",
 	"IO_STATUS_FAIL_RF_OFF",
@@ -72,12 +72,12 @@ const char *const io_state_string[]={
 struct btc_coexist gl_bt_coexist;
 
 u32 btc_dbg_type[BTC_MSG_MAX];
-u8 btc_dbg_buf[100];
+static u8 btc_dbg_buf[100];
 
 /***************************************************
  *		Debug related function
  ***************************************************/
-bool halbtc_is_bt_coexist_available(struct btc_coexist *btcoexist)
+static bool halbtc_is_bt_coexist_available(struct btc_coexist *btcoexist)
 {
 	if (!btcoexist->binded || NULL == btcoexist->adapter)
 		return false;
@@ -85,7 +85,7 @@ bool halbtc_is_bt_coexist_available(struct btc_coexist *btcoexist)
 	return true;
 }
 
-bool halbtc_is_wifi_busy(struct rtl_priv *rtlpriv)
+static bool halbtc_is_wifi_busy(struct rtl_priv *rtlpriv)
 {
 
 	if (rtlpriv->link_info.b_busytraffic)
@@ -95,7 +95,7 @@ bool halbtc_is_wifi_busy(struct rtl_priv *rtlpriv)
 }
 
 
-void halbtc_dbg_init(void)
+static void halbtc_dbg_init(void)
 {
 	u8 i;
 
@@ -121,12 +121,7 @@ void halbtc_dbg_init(void)
 			0;
 }
 
-bool halbtc_is_hw_mailbox_exist(struct btc_coexist *btcoexist)
-{
-	return true;
-}
-
-bool halbtc_is_bt40(struct rtl_priv *adapter)
+static bool halbtc_is_bt40(struct rtl_priv *adapter)
 {
 	struct rtl_priv *rtlpriv = adapter;
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
@@ -142,7 +137,7 @@ bool halbtc_is_bt40(struct rtl_priv *adapter)
 	return is_ht40;
 }
 
-bool halbtc_legacy(struct rtl_priv *adapter)
+static bool halbtc_legacy(struct rtl_priv *adapter)
 {
 	struct rtl_priv *rtlpriv = adapter;
 	struct rtl_mac *mac = rtl_mac(rtlpriv);
@@ -165,7 +160,7 @@ bool halbtc_is_wifi_uplink(struct rtl_priv *adapter)
 		return false;
 }
 
-u32 halbtc_get_wifi_bw(struct btc_coexist *btcoexist)
+static u32 halbtc_get_wifi_bw(struct btc_coexist *btcoexist)
 {
 	struct rtl_priv *rtlpriv = 
 		(struct rtl_priv *)btcoexist->adapter;
@@ -182,7 +177,7 @@ u32 halbtc_get_wifi_bw(struct btc_coexist *btcoexist)
 	return wifi_bw;
 }
 
-u8 halbtc_get_wifi_central_chnl(struct btc_coexist *btcoexist)
+static u8 halbtc_get_wifi_central_chnl(struct btc_coexist *btcoexist)
 {
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
 	struct rtl_phy 	*rtlphy = &(rtlpriv->phy);
@@ -196,7 +191,7 @@ u8 halbtc_get_wifi_central_chnl(struct btc_coexist *btcoexist)
 	return chnl;
 }
 
-void halbtc_leave_lps(struct btc_coexist *btcoexist)
+static void halbtc_leave_lps(struct btc_coexist *btcoexist)
 {
 	struct rtl_priv *rtlpriv;
 	struct rtl_ps_ctl *ppsc;
@@ -217,7 +212,7 @@ void halbtc_leave_lps(struct btc_coexist *btcoexist)
 	btcoexist->bt_info.bt_lps_on = false;
 }
 
-void halbtc_enter_lps(struct btc_coexist *btcoexist)
+static void halbtc_enter_lps(struct btc_coexist *btcoexist)
 {
 	struct rtl_priv *rtlpriv;
 	struct rtl_ps_ctl *ppsc;
@@ -238,7 +233,7 @@ void halbtc_enter_lps(struct btc_coexist *btcoexist)
 	btcoexist->bt_info.bt_lps_on = false;
 }
 
-void halbtc_normal_lps(struct btc_coexist *btcoexist)
+static void halbtc_normal_lps(struct btc_coexist *btcoexist)
 {
 	if (btcoexist->bt_info.bt_ctrl_lps) {
 		btcoexist->bt_info.bt_lps_on = false;
@@ -247,29 +242,29 @@ void halbtc_normal_lps(struct btc_coexist *btcoexist)
 		
 }
 
-void halbtc_leave_low_power(void)
+static void halbtc_leave_low_power(void)
 {
 }
 
-void halbtc_nomal_low_power(void)
+static void halbtc_nomal_low_power(void)
 {
 }
 
-void halbtc_disable_low_power(void)
+static void halbtc_disable_low_power(void)
 {
 }
 
-void halbtc_aggregation_check(void)
+static void halbtc_aggregation_check(void)
 {
 }
 
 
-u32 halbtc_get_bt_patch_version(struct btc_coexist *btcoexist)
+static u32 halbtc_get_bt_patch_version(struct btc_coexist *btcoexist)
 {
 	return 0;
 }
 
-s32 halbtc_get_wifi_rssi(struct rtl_priv *adapter)
+static s32 halbtc_get_wifi_rssi(struct rtl_priv *adapter)
 {
 	struct rtl_priv *rtlpriv = adapter;
 	s32	undecorated_smoothed_pwdb = 0;
@@ -283,7 +278,7 @@ s32 halbtc_get_wifi_rssi(struct rtl_priv *adapter)
 	return undecorated_smoothed_pwdb;
 }
 
-bool halbtc_get(void *void_btcoexist, u8 get_type, void *out_buf)
+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;
@@ -422,7 +417,7 @@ bool halbtc_get(void *void_btcoexist, u8 get_type, void *out_buf)
 	return true;
 }
 
-bool halbtc_set(void *void_btcoexist, u8 set_type, void *in_buf)
+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;
@@ -516,26 +511,26 @@ bool halbtc_set(void *void_btcoexist, u8 set_type, void *in_buf)
 	return true;
 }
 
-void halbtc_display_coex_statistics(struct btc_coexist *btcoexist)
+static void halbtc_display_coex_statistics(struct btc_coexist *btcoexist)
 {
 }
 
-void halbtc_display_bt_link_info(struct btc_coexist *btcoexist)
+static void halbtc_display_bt_link_info(struct btc_coexist *btcoexist)
 {
 }
 
-void halbtc_display_bt_fw_info(struct btc_coexist *btcoexist)
+static void halbtc_display_bt_fw_info(struct btc_coexist *btcoexist)
 {
 }
 
-void halbtc_display_fw_pwr_mode_cmd(struct btc_coexist *btcoexist)
+static void halbtc_display_fw_pwr_mode_cmd(struct btc_coexist *btcoexist)
 {
 }
 
 /************************************************************
  *		IO related function
  ************************************************************/
-u8 halbtc_read_1byte(void *bt_context, u32 reg_addr)
+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;
@@ -544,7 +539,7 @@ u8 halbtc_read_1byte(void *bt_context, u32 reg_addr)
 }
 
 
-u16 halbtc_read_2byte(void *bt_context, u32 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;
@@ -553,7 +548,7 @@ u16 halbtc_read_2byte(void *bt_context, u32 reg_addr)
 }
 
 
-u32 halbtc_read_4byte(void *bt_context, u32 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;
@@ -562,7 +557,7 @@ u32 halbtc_read_4byte(void *bt_context, u32 reg_addr)
 }
 
 
-void halbtc_write_1byte(void *bt_context, u32 reg_addr, u8 data)
+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;
@@ -570,7 +565,7 @@ void halbtc_write_1byte(void *bt_context, u32 reg_addr, u8 data)
 	rtl_write_byte(rtlpriv, reg_addr, data);
 }
 
-void halbtc_bitmask_write_1byte(void *bt_context, u32 reg_addr, 
+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;
@@ -592,7 +587,7 @@ void halbtc_bitmask_write_1byte(void *bt_context, u32 reg_addr,
 }
 
 
-void halbtc_write_2byte(void *bt_context, u32 reg_addr, u16 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;
@@ -601,7 +596,7 @@ void halbtc_write_2byte(void *bt_context, u32 reg_addr, u16 data)
 }
 
 
-void halbtc_write_4byte(void *bt_context, u32 reg_addr, u32 data)
+static void halbtc_write_4byte(void *bt_context, u32 reg_addr, u32 data)
 {
 	struct btc_coexist *btcoexist = 
 		(struct btc_coexist *)bt_context;
@@ -610,26 +605,8 @@ void halbtc_write_4byte(void *bt_context, u32 reg_addr, u32 data)
 	rtl_write_dword(rtlpriv, reg_addr, data);
 }
 
-
-void halbtc_set_macreg(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);
-}
-
-
-u32 halbtc_get_macreg(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);
-}
-
-
-void halbtc_set_bbreg(void *bt_context, u32 reg_addr, u32 bit_mask, u32 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;
@@ -638,7 +615,7 @@ void halbtc_set_bbreg(void *bt_context, u32 reg_addr, u32 bit_mask, u32 data)
 }
 
 
-u32 halbtc_get_bbreg(void *bt_context, u32 reg_addr, u32 bit_mask)
+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;
@@ -647,7 +624,7 @@ u32 halbtc_get_bbreg(void *bt_context, u32 reg_addr, u32 bit_mask)
 }
 
 
-void halbtc_set_rfreg(void *bt_context, u8 rf_path, u32 reg_addr, 
+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;
@@ -657,7 +634,8 @@ void halbtc_set_rfreg(void *bt_context, u8 rf_path, u32 reg_addr,
 }
 
 
-u32 halbtc_get_rfreg(void *bt_context, u8 rf_path, u32 reg_addr, u32 bit_mask)
+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;
@@ -666,7 +644,7 @@ u32 halbtc_get_rfreg(void *bt_context, u8 rf_path, u32 reg_addr, u32 bit_mask)
 }
 
 
-void halbtc_fill_h2c_cmd(void *bt_context, u8 element_id, 
+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;
@@ -676,7 +654,7 @@ void halbtc_fill_h2c_cmd(void *bt_context, u8 element_id,
 					cmd_len, cmd_buf);
 }
 
-void halbtc_display_dbg_msg(void *bt_context, u8 disp_type)
+static void halbtc_display_dbg_msg(void *bt_context, u8 disp_type)
 {
 	struct btc_coexist *btcoexist =	(struct btc_coexist *)bt_context;
 	switch (disp_type) {
@@ -697,25 +675,6 @@ void halbtc_display_dbg_msg(void *bt_context, u8 disp_type)
 	}
 }
 
-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
  *****************************************************************/
diff --git a/drivers/staging/rtl8821ae/btcoexist/rtl_btc.c b/drivers/staging/rtl8821ae/btcoexist/rtl_btc.c
index 6653f147757..6be0acafba5 100644
--- a/drivers/staging/rtl8821ae/btcoexist/rtl_btc.c
+++ b/drivers/staging/rtl8821ae/btcoexist/rtl_btc.c
@@ -32,7 +32,7 @@
 #include "rtl_btc.h"
 #include "halbt_precomp.h"
 
-struct rtl_btc_ops rtl_btc_operation ={
+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,
diff --git a/drivers/staging/rtl8821ae/cam.c b/drivers/staging/rtl8821ae/cam.c
index 72743e78954..3bc6b3d0651 100644
--- a/drivers/staging/rtl8821ae/cam.c
+++ b/drivers/staging/rtl8821ae/cam.c
@@ -28,9 +28,7 @@
  *****************************************************************************/
 #include "wifi.h"
 #include "cam.h"
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0))
 #include <linux/export.h>
-#endif
 
 void rtl_cam_reset_sec_info(struct ieee80211_hw *hw)
 {
diff --git a/drivers/staging/rtl8821ae/compat.h b/drivers/staging/rtl8821ae/compat.h
index 68269cc2d47..ffb5f8b07bd 100644
--- a/drivers/staging/rtl8821ae/compat.h
+++ b/drivers/staging/rtl8821ae/compat.h
@@ -1,65 +1,8 @@
 #ifndef __RTL_COMPAT_H__
 #define __RTL_COMPAT_H__
 
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,29))
-/*
- * Use this if you want to use the same suspend and resume callbacks for suspend
- * to RAM and hibernation.
- */
-#define SIMPLE_DEV_PM_OPS(name, suspend_fn, resume_fn) \
-struct dev_pm_ops name = { \
-	.suspend = suspend_fn, \
-	.resume = resume_fn, \
-	.freeze = suspend_fn, \
-	.thaw = resume_fn, \
-	.poweroff = suspend_fn, \
-	.restore = resume_fn, \
-}
-
-#define compat_pci_suspend(fn)						\
-	int fn##_compat(struct pci_dev *pdev, pm_message_t state) 	\
-	{								\
-		int r;							\
-									\
-		r = fn(&pdev->dev);					\
-		if (r)							\
-			return r;					\
-									\
-		pci_save_state(pdev);					\
-		pci_disable_device(pdev);				\
-		pci_set_power_state(pdev, PCI_D3hot);			\
-									\
-		return 0;						\
-	}
-
-#define compat_pci_resume(fn)						\
-	int fn##_compat(struct pci_dev *pdev)				\
-	{								\
-		int r;							\
-									\
-		pci_set_power_state(pdev, PCI_D0);			\
-		r = pci_enable_device(pdev);				\
-		if (r)							\
-			return r;					\
-		pci_restore_state(pdev);				\
-									\
-		return fn(&pdev->dev);					\
-	}
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,39))
-#define RX_FLAG_MACTIME_MPDU RX_FLAG_TSFT
-#else
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,8,0))
 #define RX_FLAG_MACTIME_MPDU RX_FLAG_MACTIME_START
-#else
-#endif
-//#define NETDEV_TX_OK
-#endif
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,7,0))
 #define IEEE80211_KEY_FLAG_SW_MGMT IEEE80211_KEY_FLAG_SW_MGMT_TX
-#endif
 
 struct ieee80211_mgmt_compat {
 	__le16 frame_control;
diff --git a/drivers/staging/rtl8821ae/core.c b/drivers/staging/rtl8821ae/core.c
index ff3139b6da6..9a37408708f 100644
--- a/drivers/staging/rtl8821ae/core.c
+++ b/drivers/staging/rtl8821ae/core.c
@@ -88,42 +88,9 @@ static void rtl_op_stop(struct ieee80211_hw *hw)
 	mutex_unlock(&rtlpriv->locks.conf_mutex);
 }
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,39))
-static int rtl_op_tx(struct ieee80211_hw *hw, 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, skb))
-		rtlpriv->intf_ops->adapter_tx(hw, skb, &tcb_desc);
-
-	return NETDEV_TX_OK;
-
-err_free:
-	dev_kfree_skb_any(skb);
-	return NETDEV_TX_OK;
-}
-#else
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-static void rtl_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
-#else
-/*<delete in kernel end>*/
 static void rtl_op_tx(struct ieee80211_hw *hw,
 		      struct ieee80211_tx_control *control,
 		      struct sk_buff *skb)
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
@@ -137,26 +104,14 @@ static void rtl_op_tx(struct ieee80211_hw *hw,
 	if (!test_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status))
 		goto err_free;
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-	if (!rtlpriv->intf_ops->waitq_insert(hw, skb))
-		rtlpriv->intf_ops->adapter_tx(hw, skb, &tcb_desc);
-#else
-/*<delete in kernel end>*/
 	if (!rtlpriv->intf_ops->waitq_insert(hw, control->sta, skb))
 	        rtlpriv->intf_ops->adapter_tx(hw, control->sta, skb, &tcb_desc);
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 	return;
 
 err_free:
 	dev_kfree_skb_any(skb);
 	return;
 }
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 
 static int rtl_op_add_interface(struct ieee80211_hw *hw,
 		struct ieee80211_vif *vif)
@@ -171,26 +126,15 @@ static int rtl_op_add_interface(struct ieee80211_hw *hw,
 		return -EOPNOTSUPP;
 	}
 
-/*This flag is not defined before kernel 3.4*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,4,0))
 	vif->driver_flags |= IEEE80211_VIF_BEACON_FILTER;
-#endif
 
 	rtl_ips_nic_on(hw);
 
 	mutex_lock(&rtlpriv->locks.conf_mutex);
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37))
 	switch (ieee80211_vif_type_p2p(vif)) {
 	case NL80211_IFTYPE_P2P_CLIENT:
 		mac->p2p = P2P_ROLE_CLIENT;
 		/*fall through*/
-#else
-/*<delete in kernel end>*/
-	switch (vif->type) {
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 	case NL80211_IFTYPE_STATION:
 		if (mac->beacon_enabled == 1) {
 			RT_TRACE(COMP_MAC80211, DBG_LOUD,
@@ -214,13 +158,9 @@ static int rtl_op_add_interface(struct ieee80211_hw *hw,
 				(u8 *) (&mac->basic_rates));
 
 		break;
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37))
 	case NL80211_IFTYPE_P2P_GO:
 		mac->p2p = P2P_ROLE_GO;
 		/*fall through*/
-#endif
-/*<delete in kernel end>*/
 	case NL80211_IFTYPE_AP:
 		RT_TRACE(COMP_MAC80211, DBG_LOUD,
 			 ("NL80211_IFTYPE_AP \n"));
@@ -310,9 +250,7 @@ static void rtl_op_remove_interface(struct ieee80211_hw *hw,
 
 	mutex_unlock(&rtlpriv->locks.conf_mutex);
 }
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37))
-/*<delete in kernel end>*/
+
 static int rtl_op_change_interface(struct ieee80211_hw *hw,
 				   struct ieee80211_vif *vif,
 				   enum nl80211_iftype new_type, bool p2p)
@@ -328,9 +266,7 @@ static int rtl_op_change_interface(struct ieee80211_hw *hw,
 		 (" p2p  %x\n",p2p));
 	return ret;
 }
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
+
 static int rtl_op_config(struct ieee80211_hw *hw, u32 changed)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -398,14 +334,9 @@ static int rtl_op_config(struct ieee80211_hw *hw, u32 changed)
 	}
 
 	if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,10,0))
 		struct ieee80211_channel *channel = hw->conf.chandef.chan;
 		enum nl80211_channel_type channel_type =
 				cfg80211_get_chandef_type(&(hw->conf.chandef));
-#else
-		struct ieee80211_channel *channel = hw->conf.channel;
-		enum nl80211_channel_type channel_type = hw->conf.channel_type;
-#endif
 		u8 wide_chan = (u8) channel->hw_value;
 
 		if (mac->act_scanning)
@@ -661,14 +592,9 @@ static int _rtl_get_hal_qnum(u16 queue)
  *for mac80211 VO=0, VI=1, BE=2, BK=3
  *for rtl819x  BE=0, BK=1, VI=2, VO=3
  */
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0))
 static int rtl_op_conf_tx(struct ieee80211_hw *hw,
 			  struct ieee80211_vif *vif, u16 queue,
 			  const struct ieee80211_tx_queue_params *param)
-#else
-static int rtl_op_conf_tx(struct ieee80211_hw *hw, u16 queue,
-			  const struct ieee80211_tx_queue_params *param)
-#endif
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
@@ -964,11 +890,7 @@ out:
 	mutex_unlock(&rtlpriv->locks.conf_mutex);
 }
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0))
 static u64 rtl_op_get_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
-#else
-static u64 rtl_op_get_tsf(struct ieee80211_hw *hw)
-#endif
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u64 tsf;
@@ -977,12 +899,8 @@ static u64 rtl_op_get_tsf(struct ieee80211_hw *hw)
 	return tsf;
 }
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0))
 static void rtl_op_set_tsf(struct ieee80211_hw *hw,
 			   struct ieee80211_vif *vif, u64 tsf)
-#else
-static void rtl_op_set_tsf(struct ieee80211_hw *hw, u64 tsf)
-#endif
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
@@ -992,11 +910,7 @@ static void rtl_op_set_tsf(struct ieee80211_hw *hw, u64 tsf)
 	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_CORRECT_TSF, (u8 *) (&bibss));
 }
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0))
 static void rtl_op_reset_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
-#else
-static void rtl_op_reset_tsf(struct ieee80211_hw *hw)
-#endif
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 tmp = 0;
@@ -1023,13 +937,7 @@ 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
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,39))
-/*<delete in kernel end>*/
 			       ,u8 buf_size
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 			       )
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -1040,13 +948,9 @@ static int rtl_op_ampdu_action(struct ieee80211_hw *hw,
 			 ("IEEE80211_AMPDU_TX_START: TID:%d\n", tid));
 		return rtl_tx_agg_start(hw, vif, sta, tid, ssn);
 		break;
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,9,0))
 	case IEEE80211_AMPDU_TX_STOP_CONT:
 	case IEEE80211_AMPDU_TX_STOP_FLUSH:
 	case IEEE80211_AMPDU_TX_STOP_FLUSH_CONT:
-#else
-	case IEEE80211_AMPDU_TX_STOP:
-#endif
 		RT_TRACE(COMP_MAC80211, DBG_TRACE,
 			 ("IEEE80211_AMPDU_TX_STOP: TID:%d\n", tid));
 		return rtl_tx_agg_stop(hw, vif, sta, tid);
@@ -1174,9 +1078,6 @@ static int rtl_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 	mutex_lock(&rtlpriv->locks.conf_mutex);
 	/* <1> get encryption alg */
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37))
-/*<delete in kernel end>*/
 	switch (key->cipher) {
 	case WLAN_CIPHER_SUITE_WEP40:
 		key_type = WEP40_ENCRYPTION;
@@ -1209,43 +1110,6 @@ static int rtl_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 			 ("alg_err:%x!!!!:\n", key->cipher));
 		goto out_unlock;
 	}
-/*<delete in kernel start>*/
-#else
-	switch (key->alg) {
-	case ALG_WEP:
-		if (key->keylen == WLAN_KEY_LEN_WEP40) {
-			key_type = WEP40_ENCRYPTION;
-			RT_TRACE(COMP_SEC, DBG_DMESG, ("alg:WEP40\n"));
-		} else {
-			RT_TRACE(COMP_SEC, DBG_DMESG,
-				 ("alg:WEP104\n"));
-			key_type = WEP104_ENCRYPTION;
-		}
-		break;
-	case ALG_TKIP:
-		key_type = TKIP_ENCRYPTION;
-		RT_TRACE(COMP_SEC, DBG_DMESG, ("alg:TKIP\n"));
-		break;
-	case ALG_CCMP:
-		key_type = AESCCMP_ENCRYPTION;
-		RT_TRACE(COMP_SEC, DBG_DMESG, ("alg:CCMP\n"));
-		break;
-	case ALG_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->alg));
-		goto out_unlock;
-	}
-#endif
-/*<delete in kernel end>*/
 	if(key_type == WEP40_ENCRYPTION ||
 			key_type == WEP104_ENCRYPTION ||
 			vif->type == NL80211_IFTYPE_ADHOC)
@@ -1414,7 +1278,6 @@ static void rtl_op_rfkill_poll(struct ieee80211_hw *hw)
  * before switch channel or power save, or tx buffer packet
  * maybe send after offchannel or rf sleep, this may cause
  * dis-association by AP */
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,10,0))
 static void rtl_op_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -1422,15 +1285,6 @@ static void rtl_op_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
 	if (rtlpriv->intf_ops->flush)
 		rtlpriv->intf_ops->flush(hw, queues, drop);
 }
-#else
-static void rtl_op_flush(struct ieee80211_hw *hw, bool drop)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	if (rtlpriv->intf_ops->flush)
-		rtlpriv->intf_ops->flush(hw, drop);
-}
-#endif
 
 const struct ieee80211_ops rtl_ops = {
 	.start = rtl_op_start,
@@ -1438,13 +1292,7 @@ const struct ieee80211_ops rtl_ops = {
 	.tx = rtl_op_tx,
 	.add_interface = rtl_op_add_interface,
 	.remove_interface = rtl_op_remove_interface,
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37))
-/*<delete in kernel end>*/
 	.change_interface = rtl_op_change_interface,
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 	.config = rtl_op_config,
 	.configure_filter = rtl_op_configure_filter,
 	.set_key = rtl_op_set_key,
diff --git a/drivers/staging/rtl8821ae/debug.c b/drivers/staging/rtl8821ae/debug.c
index 8a6c794bda4..8aefbf10a45 100644
--- a/drivers/staging/rtl8821ae/debug.c
+++ b/drivers/staging/rtl8821ae/debug.c
@@ -30,12 +30,7 @@
 #include "wifi.h"
 #include "cam.h"
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,10,0))
 #define GET_INODE_DATA(__node)		PDE_DATA(__node)
-#else
-#define GET_INODE_DATA(__node)		PDE(__node)->data
-#endif
-
 
 void rtl_dbgp_flag_init(struct ieee80211_hw *hw)
 {
diff --git a/drivers/staging/rtl8821ae/efuse.c b/drivers/staging/rtl8821ae/efuse.c
index 250aae1ce63..206012cf360 100644
--- a/drivers/staging/rtl8821ae/efuse.c
+++ b/drivers/staging/rtl8821ae/efuse.c
@@ -29,9 +29,7 @@
 #include "wifi.h"
 #include "efuse.h"
 #include "btcoexist/halbt_precomp.h"
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0))
 #include <linux/export.h>
-#endif
 
 static const u8 MAX_PGPKT_SIZE = 9;
 static const u8 PGPKT_DATA_SIZE = 8;
diff --git a/drivers/staging/rtl8821ae/pci.c b/drivers/staging/rtl8821ae/pci.c
index a562aa60d59..e194ffe58ac 100644
--- a/drivers/staging/rtl8821ae/pci.c
+++ b/drivers/staging/rtl8821ae/pci.c
@@ -33,9 +33,7 @@
 #include "base.h"
 #include "ps.h"
 #include "efuse.h"
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0))
 #include <linux/export.h>
-#endif
 
 static const u16 pcibridge_vendors[PCI_BRIDGE_VENDOR_MAX] = {
 	INTEL_VENDOR_ID,
@@ -364,47 +362,6 @@ static bool rtl_pci_get_amd_l1_patch(struct ieee80211_hw *hw)
 	return status;
 }
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35))
-static u8 _rtl_pci_get_pciehdr_offset(struct ieee80211_hw *hw)
-{
-	u8 capability_offset;
-	u8 num4bytes = 0x34/4;
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	u32 pcicfg_addr_port = (pcipriv->ndis_adapter.pcibridge_busnum << 16)|
-			       (pcipriv->ndis_adapter.pcibridge_devnum << 11)|
-			       (pcipriv->ndis_adapter.pcibridge_funcnum << 8)|
-			       (1 << 31);
-
-	rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS , pcicfg_addr_port
-							+ (num4bytes << 2));
-	rtl_pci_raw_read_port_uchar(PCI_CONF_DATA, &capability_offset);
-	while (capability_offset != 0) {
-		struct rtl_pci_capabilities_header capability_hdr;
-
-		num4bytes = capability_offset / 4;
-		/* Read the header of the capability at  this offset.
-		 * If the retrieved capability is not the power management
-		 * capability that we are looking for, follow the link to
-		 * the next capability and continue looping.
-		 */
-		rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS ,
-					     pcicfg_addr_port +
-					     (num4bytes << 2));
-		rtl_pci_raw_read_port_ushort(PCI_CONF_DATA,
-					     (u16*)&capability_hdr);
-		/* Found the PCI express capability. */
-		if (capability_hdr.capability_id ==
-		    PCI_CAPABILITY_ID_PCI_EXPRESS)
-			break;
-		else
-        		capability_offset = capability_hdr.next;
-	}
-	return capability_offset;
-}
-#endif
-/*<delete in kernel end>*/
-
 bool rtl_pci_check_buddy_priv(struct ieee80211_hw *hw,
         		      struct rtl_priv **buddy_priv)
 {
@@ -610,14 +567,7 @@ static void _rtl_pci_tx_chk_waitq(struct ieee80211_hw *hw)
 				_rtl_pci_update_earlymode_info(hw, skb,
 							       &tcb_desc, tid);
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-			rtlpriv->intf_ops->adapter_tx(hw, skb, &tcb_desc);
-#else
-/*<delete in kernel end>*/
 			rtlpriv->intf_ops->adapter_tx(hw, NULL, skb, &tcb_desc);
-#endif
-/*<delete in kernel end>*/
 		}
 	}
 }
@@ -1201,19 +1151,9 @@ static void _rtl_pci_prepare_bcn_tasklet(struct ieee80211_hw *hw)
 	if (rtlpriv->use_new_trx_flow)
 		pbuffer_desc = &ring->buffer_desc[0];
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-	rtlpriv->cfg->ops->fill_tx_desc(hw, hdr, (u8 *) pdesc,
-				        (u8 *)pbuffer_desc, info, pskb,
-				        BEACON_QUEUE, &tcb_desc);
-#else
-/*<delete in kernel end>*/
 	rtlpriv->cfg->ops->fill_tx_desc(hw, hdr, (u8 *) pdesc,
 					(u8 *)pbuffer_desc, info, NULL, pskb,
 					BEACON_QUEUE, &tcb_desc);
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 
 	__skb_queue_tail(&ring->queue, pskb);
 
@@ -1616,26 +1556,11 @@ int rtl_pci_reset_trx_ring(struct ieee80211_hw *hw)
 	return 0;
 }
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-static bool rtl_pci_tx_chk_waitq_insert(struct ieee80211_hw *hw,
-					struct sk_buff *skb)
-#else
-/*<delete in kernel end>*/
 static bool rtl_pci_tx_chk_waitq_insert(struct ieee80211_hw *hw,
 					struct ieee80211_sta *sta,
 					struct sk_buff *skb)
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
-	struct ieee80211_sta *sta = info->control.sta;
-#endif
-/*<delete in kernel end>*/
 	struct rtl_sta_info *sta_entry = NULL;
 	u8 tid = rtl_get_tid(skb);
 	u16 fc = rtl_get_fc(skb);
@@ -1671,28 +1596,14 @@ static bool rtl_pci_tx_chk_waitq_insert(struct ieee80211_hw *hw,
 	return true;
 }
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-int rtl_pci_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
-	       struct rtl_tcb_desc *ptcb_desc)
-#else
-/*<delete in kernel end>*/
 static int rtl_pci_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>*/
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_sta_info *sta_entry = NULL;
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-	struct ieee80211_sta *sta = info->control.sta;
-#endif
-/*<delete in kernel end>*/
 	struct rtl8192_tx_ring *ring;
 	struct rtl_tx_desc *pdesc;
 	struct rtl_tx_buffer_desc *ptx_bd_desc = NULL;
@@ -1777,19 +1688,9 @@ static int rtl_pci_tx(struct ieee80211_hw *hw,
 	if (ieee80211_is_data(fc))
 		rtlpriv->cfg->ops->led_control(hw, LED_CTL_TX);
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-	rtlpriv->cfg->ops->fill_tx_desc(hw, hdr, (u8 *) pdesc,
-					(u8 *)ptx_bd_desc, info, skb,
-					hw_queue, ptcb_desc);
-#else
-/*<delete in kernel end>*/
 	rtlpriv->cfg->ops->fill_tx_desc(hw, hdr, (u8 *) pdesc,
 					(u8 *)ptx_bd_desc, info, sta, skb,
 					hw_queue, ptcb_desc);
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 
 	__skb_queue_tail(&ring->queue, skb);
 	if (rtlpriv->use_new_trx_flow) {
@@ -1819,11 +1720,7 @@ static int rtl_pci_tx(struct ieee80211_hw *hw,
 
 	return 0;
 }
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,10,0))
 static void rtl_pci_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
-#else
-static void rtl_pci_flush(struct ieee80211_hw *hw, bool drop)
-#endif
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
@@ -1838,12 +1735,10 @@ 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 (LINUX_VERSION_CODE >= KERNEL_VERSION(3,10,0))
 		if (((queues >> queue_id) & 0x1) == 0) {
 			queue_id--;
 			continue;
 		}
-#endif
 		ring = &pcipriv->dev.tx_ring[queue_id];
 		queue_len = skb_queue_len(&ring->queue);
 		if (queue_len == 0 || queue_id == BEACON_QUEUE ||
@@ -2122,17 +2017,8 @@ static bool _rtl_pci_find_adapter(struct pci_dev *pdev,
 		    (pcipriv->ndis_adapter.pcibridge_busnum << 16) |
 		    (pcipriv->ndis_adapter.pcibridge_devnum << 11) |
 		    (pcipriv->ndis_adapter.pcibridge_funcnum << 8) | (1 << 31);
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,35))
-/*<delete in kernel end>*/
 		pcipriv->ndis_adapter.pcibridge_pciehdr_offset =
 		    pci_pcie_cap(bridge_pdev);
-/*<delete in kernel start>*/
-#else
-		pcipriv->ndis_adapter.pcibridge_pciehdr_offset =
-			_rtl_pci_get_pciehdr_offset(hw);
-#endif
-/*<delete in kernel end>*/
 		pcipriv->ndis_adapter.num4bytes =
 		    (pcipriv->ndis_adapter.pcibridge_pciehdr_offset + 0x10) / 4;
 
@@ -2230,14 +2116,8 @@ static int rtl_pci_intr_mode_decide(struct ieee80211_hw *hw)
  * hw pointer in rtl_pci_get_hw_pointer */
 struct ieee80211_hw *hw_export = NULL;
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,8,0))
 int rtl_pci_probe(struct pci_dev *pdev,
                   const struct pci_device_id *id)
-
-#else
-int __devinit rtl_pci_probe(struct pci_dev *pdev,
-			    const struct pci_device_id *id)
-#endif
 {
 	struct ieee80211_hw *hw = NULL;
 
@@ -2313,16 +2193,16 @@ int __devinit rtl_pci_probe(struct pci_dev *pdev,
 
 	/*shared mem start */
 	rtlpriv->io.pci_mem_start =
-			(unsigned long)pci_iomap(pdev,
+			pci_iomap(pdev,
 			rtlpriv->cfg->bar_id, pmem_len);
-	if (rtlpriv->io.pci_mem_start == 0) {
+	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%08lx\n",
+		  "flags:%08lx, after map:0x%p\n",
 		  pmem_start, pmem_len, pmem_flags,
 		  rtlpriv->io.pci_mem_start));
 
@@ -2415,8 +2295,8 @@ fail3:
 	rtl_deinit_core(hw);
 	ieee80211_free_hw(hw);
 
-	if (rtlpriv->io.pci_mem_start != 0)
-		pci_iounmap(pdev, (void *)rtlpriv->io.pci_mem_start);
+	if (rtlpriv->io.pci_mem_start != NULL)
+		pci_iounmap(pdev, rtlpriv->io.pci_mem_start);
 
 fail2:
 	pci_release_regions(pdev);
@@ -2478,8 +2358,8 @@ void rtl_pci_disconnect(struct pci_dev *pdev)
 		pci_disable_msi(rtlpci->pdev);
 
 	list_del(&rtlpriv->list);
-	if (rtlpriv->io.pci_mem_start != 0) {
-		pci_iounmap(pdev, (void *)rtlpriv->io.pci_mem_start);
+	if (rtlpriv->io.pci_mem_start != NULL) {
+		pci_iounmap(pdev, rtlpriv->io.pci_mem_start);
 		pci_release_regions(pdev);
 	}
 
diff --git a/drivers/staging/rtl8821ae/pci.h b/drivers/staging/rtl8821ae/pci.h
index 06eaa521e0e..3f16ec90296 100644
--- a/drivers/staging/rtl8821ae/pci.h
+++ b/drivers/staging/rtl8821ae/pci.h
@@ -282,13 +282,8 @@ int rtl_pci_reset_trx_ring(struct ieee80211_hw *hw);
 
 extern struct rtl_intf_ops rtl_pci_ops;
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,8,0))
 int rtl_pci_probe(struct pci_dev *pdev,
                   const struct pci_device_id *id);
-#else
-int __devinit rtl_pci_probe(struct pci_dev *pdev,
-			    const struct pci_device_id *id);
-#endif
 void rtl_pci_disconnect(struct pci_dev *pdev);
 int rtl_pci_suspend(struct device *dev);
 int rtl_pci_resume(struct device *dev);
diff --git a/drivers/staging/rtl8821ae/ps.c b/drivers/staging/rtl8821ae/ps.c
index 7876442417f..5a9bbf025e2 100644
--- a/drivers/staging/rtl8821ae/ps.c
+++ b/drivers/staging/rtl8821ae/ps.c
@@ -30,9 +30,7 @@
 #include "wifi.h"
 #include "base.h"
 #include "ps.h"
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0))
 #include <linux/export.h>
-#endif
 #include "btcoexist/rtl_btc.h"
 
 bool rtl_ps_enable_nic(struct ieee80211_hw *hw)
@@ -542,17 +540,8 @@ void rtl_swlps_beacon(struct ieee80211_hw *hw, void *data, unsigned int len)
 	tim_len = tim[1];
 	tim_ie = (struct ieee80211_tim_ie *) &tim[2];
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,35))
-/*<delete in kernel end>*/
 	if (!WARN_ON_ONCE(!hw->conf.ps_dtim_period))
 		rtlpriv->psc.dtim_counter = tim_ie->dtim_count;
-/*<delete in kernel start>*/
-#else
-	if (!WARN_ON_ONCE(!mac->vif->bss_conf.dtim_period))
-		rtlpriv->psc.dtim_counter = tim_ie->dtim_count;
-#endif
-/*<delete in kernel end>*/
 
 	/* Check whenever the PHY can be turned off again. */
 
@@ -656,9 +645,6 @@ void rtl_swlps_rf_sleep(struct ieee80211_hw *hw)
 	 * time to sleep_intv = rtlpriv->psc.dtim_counter or
 	 * MAX_SW_LPS_SLEEP_INTV(default set to 5) */
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,35))
-/*<delete in kernel end>*/
 	if (rtlpriv->psc.dtim_counter == 0) {
 		if (hw->conf.ps_dtim_period == 1)
 			sleep_intv = hw->conf.ps_dtim_period * 2;
@@ -667,18 +653,6 @@ void rtl_swlps_rf_sleep(struct ieee80211_hw *hw)
 	} else {
 		sleep_intv = rtlpriv->psc.dtim_counter;
 	}
-/*<delete in kernel start>*/
-#else
-	if (rtlpriv->psc.dtim_counter == 0) {
-		if (mac->vif->bss_conf.dtim_period == 1)
-			sleep_intv = mac->vif->bss_conf.dtim_period * 2;
-		else
-			sleep_intv = mac->vif->bss_conf.dtim_period;
-	} else {
-		sleep_intv = rtlpriv->psc.dtim_counter;
-	}
-#endif
-/*<delete in kernel end>*/
 
 	if (sleep_intv > MAX_SW_LPS_SLEEP_INTV)
 		sleep_intv = MAX_SW_LPS_SLEEP_INTV;
diff --git a/drivers/staging/rtl8821ae/rc.c b/drivers/staging/rtl8821ae/rc.c
index 0cc32c60dde..0b4f3218508 100644
--- a/drivers/staging/rtl8821ae/rc.c
+++ b/drivers/staging/rtl8821ae/rc.c
@@ -210,16 +210,8 @@ static void rtl_tx_status(void *ppriv,
 						       tid)) {
 					sta_entry->tids[tid].agg.agg_state =
 						RTL_AGG_PROGRESS;
-					/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,38))
-					/*<delete in kernel end>*/
 					ieee80211_start_tx_ba_session(sta, tid,
 								      5000);
-					/*<delete in kernel start>*/
-#else
-					ieee80211_start_tx_ba_session(sta, tid);
-#endif
-					/*<delete in kernel end>*/
 				}
 			}
 		}
@@ -232,15 +224,6 @@ static void rtl_rate_init(void *ppriv,
 			  struct ieee80211_sta *sta, void *priv_sta)
 {
 }
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,5,0))
-static void rtl_rate_update(void *ppriv,
-			    struct ieee80211_supported_band *sband,
-			    struct ieee80211_sta *sta, void *priv_sta,
-			    u32 changed,
-			    enum nl80211_channel_type oper_chan_type)
-{
-}
-#else
 static void rtl_rate_update(void *ppriv,
 			    struct ieee80211_supported_band *sband,
 			    struct cfg80211_chan_def *chandef,
@@ -248,7 +231,6 @@ static void rtl_rate_update(void *ppriv,
 			    u32 changed)
 {
 }
-#endif
 static void *rtl_rate_alloc(struct ieee80211_hw *hw, struct dentry *debugfsdir)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -286,7 +268,7 @@ static void rtl_rate_free_sta(void *rtlpriv,
 }
 
 static struct rate_control_ops rtl_rate_ops = {
-	.name = "rtl_rc",
+	.name = "rtl_rc_21ae",
 	.alloc = rtl_rate_alloc,
 	.free = rtl_rate_free,
 	.alloc_sta = rtl_rate_alloc_sta,
diff --git a/drivers/staging/rtl8821ae/regd.c b/drivers/staging/rtl8821ae/regd.c
index 0a4b3984b7e..2efa5f31822 100644
--- a/drivers/staging/rtl8821ae/regd.c
+++ b/drivers/staging/rtl8821ae/regd.c
@@ -158,11 +158,6 @@ static void _rtl_reg_apply_beaconing_flags(struct wiphy *wiphy,
 	const struct ieee80211_reg_rule *reg_rule;
 	struct ieee80211_channel *ch;
 	unsigned int i;
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,9,0))
-	u32 bandwidth = 0;
-	int r;
-#endif
-
 	for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
 
 		if (!wiphy->bands[band])
@@ -176,16 +171,9 @@ static void _rtl_reg_apply_beaconing_flags(struct wiphy *wiphy,
 			    (ch->flags & IEEE80211_CHAN_RADAR))
 				continue;
 			if (initiator == NL80211_REGDOM_SET_BY_COUNTRY_IE) {
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,9,0))
 				reg_rule = freq_reg_info(wiphy, ch->center_freq);
 				if (IS_ERR(reg_rule))
 					continue;
-#else
-				r = freq_reg_info(wiphy, ch->center_freq,
-						  bandwidth, &reg_rule);
-				if (r)
-					continue;
-#endif
 
 				/*
 				 *If 11d had a rule for this channel ensure
@@ -219,10 +207,6 @@ static void _rtl_reg_apply_active_scan_flags(struct wiphy *wiphy,
 	struct ieee80211_supported_band *sband;
 	struct ieee80211_channel *ch;
 	const struct ieee80211_reg_rule *reg_rule;
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,9,0))
-	u32 bandwidth = 0;
-	int r;
-#endif
 
 	if (!wiphy->bands[IEEE80211_BAND_2GHZ])
 		return;
@@ -250,26 +234,16 @@ static void _rtl_reg_apply_active_scan_flags(struct wiphy *wiphy,
 	 */
 
 	ch = &sband->channels[11];	/* CH 12 */
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,9,0))
 	reg_rule = freq_reg_info(wiphy, ch->center_freq);
 	if (!IS_ERR(reg_rule)) {
-#else
-	r = freq_reg_info(wiphy, ch->center_freq, bandwidth, &reg_rule);
-	if (!r) {
-#endif
 		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 */
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,9,0))
 	reg_rule = freq_reg_info(wiphy, ch->center_freq);
 	if (!IS_ERR(reg_rule)) {
-#else
-	r = freq_reg_info(wiphy, ch->center_freq, bandwidth, &reg_rule);
-	if (!r) {
-#endif
 		if (!(reg_rule->flags & NL80211_RRF_PASSIVE_SCAN))
 			if (ch->flags & IEEE80211_CHAN_PASSIVE_SCAN)
 				ch->flags &= ~IEEE80211_CHAN_PASSIVE_SCAN;
@@ -389,19 +363,11 @@ static const struct ieee80211_regdomain *_rtl_regdomain_select(
 	}
 }
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,9,0))
 static int _rtl_regd_init_wiphy(struct rtl_regulatory *reg,
 				struct wiphy *wiphy,
 				void (*reg_notifier) (struct wiphy * wiphy,
 						     struct regulatory_request *
 						     request))
-#else
-static int _rtl_regd_init_wiphy(struct rtl_regulatory *reg,
-				struct wiphy *wiphy,
-				int (*reg_notifier) (struct wiphy * wiphy,
-						     struct regulatory_request *
-						     request))
-#endif
 {
 	const struct ieee80211_regdomain *regd;
 
@@ -429,15 +395,9 @@ static struct country_code_to_enum_rd *_rtl_regd_find_country(u16 countrycode)
 	return NULL;
 }
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,9,0))
 int rtl_regd_init(struct ieee80211_hw *hw,
 		  void (*reg_notifier) (struct wiphy *wiphy,
 				        struct regulatory_request *request))
-#else
-int rtl_regd_init(struct ieee80211_hw *hw,
-		  int (*reg_notifier) (struct wiphy *wiphy,
-				       struct regulatory_request *request))
-#endif
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct wiphy *wiphy = hw->wiphy;
@@ -480,7 +440,6 @@ int rtl_regd_init(struct ieee80211_hw *hw,
 	return 0;
 }
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,9,0))
 void rtl_reg_notifier(struct wiphy *wiphy, struct regulatory_request *request)
 {
 	struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
@@ -490,14 +449,3 @@ void rtl_reg_notifier(struct wiphy *wiphy, struct regulatory_request *request)
 
 	_rtl_reg_notifier_apply(wiphy, request, &rtlpriv->regd);
 }
-#else
-int 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"));
-
-	return _rtl_reg_notifier_apply(wiphy, request, &rtlpriv->regd);
-}
-#endif
diff --git a/drivers/staging/rtl8821ae/regd.h b/drivers/staging/rtl8821ae/regd.h
index dceb3f18200..1cfcb97b0ab 100644
--- a/drivers/staging/rtl8821ae/regd.h
+++ b/drivers/staging/rtl8821ae/regd.h
@@ -60,16 +60,8 @@ enum country_code_type_t {
 	COUNTRY_CODE_MAX
 };
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,9,0))
 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);
-#else
-int rtl_regd_init(struct ieee80211_hw *hw,
-		  int (*reg_notifier) (struct wiphy *wiphy,
-				       struct regulatory_request *request));
-int rtl_reg_notifier(struct wiphy *wiphy, struct regulatory_request *request);
-#endif
-
 #endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/hw.h b/drivers/staging/rtl8821ae/rtl8821ae/hw.h
index 4fb6bf0d1da..256e514f424 100644
--- a/drivers/staging/rtl8821ae/rtl8821ae/hw.h
+++ b/drivers/staging/rtl8821ae/rtl8821ae/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,42 +30,43 @@ void rtl8821ae_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val);
 void rtl8821ae_read_eeprom_info(struct ieee80211_hw *hw);
 
 void rtl8821ae_interrupt_recognized(struct ieee80211_hw *hw,
-											 u32 *p_inta, u32 *p_intb);
+				    u32 *p_inta, u32 *p_intb);
 int rtl8821ae_hw_init(struct ieee80211_hw *hw);
 void rtl8821ae_card_disable(struct ieee80211_hw *hw);
 void rtl8821ae_enable_interrupt(struct ieee80211_hw *hw);
 void rtl8821ae_disable_interrupt(struct ieee80211_hw *hw);
-int rtl8821ae_set_network_type(struct ieee80211_hw *hw, enum nl80211_iftype type);
+int rtl8821ae_set_network_type(struct ieee80211_hw *hw,
+			       enum nl80211_iftype type);
 void rtl8821ae_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid);
 void rtl8821ae_set_qos(struct ieee80211_hw *hw, int aci);
 void rtl8821ae_set_beacon_related_registers(struct ieee80211_hw *hw);
 void rtl8821ae_set_beacon_interval(struct ieee80211_hw *hw);
 void rtl8821ae_update_interrupt_mask(struct ieee80211_hw *hw,
-												u32 add_msr, u32 rm_msr);
+				     u32 add_msr, u32 rm_msr);
 void rtl8821ae_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val);
 void rtl8821ae_update_hal_rate_tbl(struct ieee80211_hw *hw,
-											struct ieee80211_sta *sta,
-											u8 rssi_level);
+				   struct ieee80211_sta *sta,
+				   u8 rssi_level);
 void rtl8821ae_update_channel_access_setting(struct ieee80211_hw *hw);
 bool rtl8821ae_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid);
 void rtl8821ae_enable_hw_security_config(struct ieee80211_hw *hw);
 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);
+		       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);
+					      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);
+					      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);
 void rtl8821ae_resume(struct ieee80211_hw *hw);
 void rtl8821ae_allow_all_destaddr(struct ieee80211_hw *hw,
-										  bool allow_all_da,
-										  bool write_into_reg);
+				  bool allow_all_da,
+				  bool write_into_reg);
 void _rtl8821ae_stop_tx_beacon(struct ieee80211_hw *hw);
 void _rtl8821ae_resume_tx_beacon(struct ieee80211_hw *hw);
 #endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/sw.c b/drivers/staging/rtl8821ae/rtl8821ae/sw.c
index a8d17556977..26212755f88 100644
--- a/drivers/staging/rtl8821ae/rtl8821ae/sw.c
+++ b/drivers/staging/rtl8821ae/rtl8821ae/sw.c
@@ -45,7 +45,7 @@
 #include "hal_btc.h"
 #include "../btcoexist/rtl_btc.h"
 
-void rtl8821ae_init_aspm_vars(struct ieee80211_hw *hw)
+static void rtl8821ae_init_aspm_vars(struct ieee80211_hw *hw)
 {
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 
@@ -225,7 +225,7 @@ void rtl8821ae_deinit_sw_vars(struct ieee80211_hw *hw)
 	//printk("<=========rtl8821ae_deinit_sw_vars().\n");
 }
 
-u32 rtl8812ae_rx_command_packet_handler(
+static u32 rtl8812ae_rx_command_packet_handler(
 	struct ieee80211_hw *hw,
 	struct rtl_stats status,
 	struct sk_buff *skb
@@ -260,7 +260,7 @@ bool rtl8821ae_get_btc_status(void)
 	return true;
 }
 
-struct rtl_hal_ops rtl8821ae_hal_ops = {
+static struct rtl_hal_ops rtl8821ae_hal_ops = {
 	.init_sw_vars = rtl8821ae_init_sw_vars,
 	.deinit_sw_vars = rtl8821ae_deinit_sw_vars,
 	.read_eeprom_info = rtl8821ae_read_eeprom_info,
@@ -311,14 +311,14 @@ struct rtl_hal_ops rtl8821ae_hal_ops = {
 	.rx_command_packet_handler = rtl8812ae_rx_command_packet_handler,
 };
 
-struct rtl_mod_params rtl8821ae_mod_params = {
+static struct rtl_mod_params rtl8821ae_mod_params = {
 	.sw_crypto = false,
 	.b_inactiveps = false,//true,
 	.b_swctrl_lps = false,
 	.b_fwctrl_lps = false, //true,
 };
 
-struct rtl_hal_cfg rtl8821ae_hal_cfg = {
+static struct rtl_hal_cfg rtl8821ae_hal_cfg = {
 	.bar_id = 2,
 	.write_readback = true,
 	.name = "rtl8821ae_pci",
@@ -415,19 +415,11 @@ struct rtl_hal_cfg rtl8821ae_hal_cfg = {
 	.maps[RTL_RC_HT_RATEMCS15] =  DESC_RATEMCS15,
 };
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,8,0))
 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)},
         {},
 };
-#else
-static struct pci_device_id rtl8821ae_pci_ids[] __devinitdata = {
-	{RTL_PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0x8812, rtl8821ae_hal_cfg)},
-	{RTL_PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0x8821, rtl8821ae_hal_cfg)},
-	{},
-};
-#endif
 
 MODULE_DEVICE_TABLE(pci, rtl8821ae_pci_ids);
 
@@ -445,14 +437,7 @@ 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");
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29))
-static const SIMPLE_DEV_PM_OPS(rtlwifi_pm_ops, rtl_pci_suspend, rtl_pci_resume);
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,29))
-compat_pci_suspend(rtl_pci_suspend)
-compat_pci_resume(rtl_pci_resume)
-#endif
+static SIMPLE_DEV_PM_OPS(rtlwifi_pm_ops, rtl_pci_suspend, rtl_pci_resume);
 
 static struct pci_driver rtl8821ae_driver = {
 	.name = KBUILD_MODNAME,
@@ -460,13 +445,7 @@ static struct pci_driver rtl8821ae_driver = {
 	.probe = rtl_pci_probe,
 	.remove = rtl_pci_disconnect,
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29))
 	.driver.pm = &rtlwifi_pm_ops,
-#elif defined(CONFIG_PM)
-	.suspend = rtl_pci_suspend_compat,
-	.resume = rtl_pci_resume_compat,
-#endif
-
 };
 
 
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/trx.c b/drivers/staging/rtl8821ae/rtl8821ae/trx.c
index f40a93c0d2b..b4943e8b7f8 100644
--- a/drivers/staging/rtl8821ae/rtl8821ae/trx.c
+++ b/drivers/staging/rtl8821ae/rtl8821ae/trx.c
@@ -77,11 +77,7 @@ static int _rtl8821ae_rate_mapping(struct ieee80211_hw *hw,
 	int rate_idx;
 
 	if (false == isht) {
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,10,0))
 		if (IEEE80211_BAND_2GHZ == hw->conf.chandef.chan->band) {
-#else
-		if (IEEE80211_BAND_2GHZ == hw->conf.channel->band) {
-#endif
 			switch (desc_rate) {
 			case DESC_RATE1M:
 				rate_idx = 0;
@@ -579,13 +575,8 @@ bool rtl8821ae_rx_query_desc(struct ieee80211_hw *hw,
 	if (status->wake_match)
 		RT_TRACE(COMP_RXDESC,DBG_LOUD,
 		("GGGGGGGGGGGGGet Wakeup Packet!! WakeMatch=%d\n",status->wake_match ));
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,10,0))
 	rx_status->freq = hw->conf.chandef.chan->center_freq;
 	rx_status->band = hw->conf.chandef.chan->band;
-#else
-	rx_status->freq = hw->conf.channel->center_freq;
-	rx_status->band = hw->conf.channel->band;
-#endif
 
 	hdr = (struct ieee80211_hdr *)(skb->data + status->rx_drvinfo_size
 			+ status->rx_bufshift);
@@ -650,33 +641,17 @@ bool rtl8821ae_rx_query_desc(struct ieee80211_hw *hw,
 	return true;
 }
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-void rtl8821ae_tx_fill_desc(struct ieee80211_hw *hw,
-			  struct ieee80211_hdr *hdr, u8 *pdesc_tx, u8 *txbd,
-			  struct ieee80211_tx_info *info, struct sk_buff *skb,
-			  u8 hw_queue, struct rtl_tcb_desc *ptcb_desc)
-#else
-/*<delete in kernel end>*/
 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)
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 {
 	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);
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-	struct ieee80211_sta *sta = info->control.sta;
-#endif
-/*<delete in kernel end>*/
 	u8 *pdesc = (u8 *) pdesc_tx;
 	u16 seq_number;
 	u16 fc = le16_to_cpu(hdr->frame_control);
@@ -783,9 +758,6 @@ void rtl8821ae_tx_fill_desc(struct ieee80211_hw *hw,
 		}
 		if (info->control.hw_key) {
 			struct ieee80211_key_conf *keyconf = info->control.hw_key;
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37))
-/*<delete in kernel end>*/
 			switch (keyconf->cipher) {
 			case WLAN_CIPHER_SUITE_WEP40:
 			case WLAN_CIPHER_SUITE_WEP104:
@@ -800,23 +772,6 @@ void rtl8821ae_tx_fill_desc(struct ieee80211_hw *hw,
 				break;
 
 			}
-/*<delete in kernel start>*/
-#else
-			switch (keyconf->alg) {
-			case ALG_WEP:
-			case ALG_TKIP:
-				SET_TX_DESC_SEC_TYPE(pdesc, 0x1);
-				break;
-			case ALG_CCMP:
-				SET_TX_DESC_SEC_TYPE(pdesc, 0x3);
-				break;
-			default:
-				SET_TX_DESC_SEC_TYPE(pdesc, 0x0);
-				break;
-
-			}
-#endif
-/*<delete in kernel end>*/
 		}
 
 		SET_TX_DESC_QUEUE_SEL(pdesc, fw_qsel);
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/trx.h b/drivers/staging/rtl8821ae/rtl8821ae/trx.h
index da93e5c7ece..af017844d82 100644
--- a/drivers/staging/rtl8821ae/rtl8821ae/trx.h
+++ b/drivers/staging/rtl8821ae/rtl8821ae/trx.h
@@ -609,23 +609,12 @@ struct rx_desc_8821ae {
 
 } __packed;
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-void rtl8821ae_tx_fill_desc(struct ieee80211_hw *hw,
-			  struct ieee80211_hdr *hdr, u8 *pdesc_tx, u8 *txbd,
-			  struct ieee80211_tx_info *info, struct sk_buff *skb,
-			  u8 hw_queue, struct rtl_tcb_desc *ptcb_desc);
-#else
-/*<delete in kernel end>*/
 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);
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 bool rtl8821ae_rx_query_desc(struct ieee80211_hw *hw,
 			   struct rtl_stats *status,
 			   struct ieee80211_rx_status *rx_status,
diff --git a/drivers/staging/rtl8821ae/stats.c b/drivers/staging/rtl8821ae/stats.c
index a20c0f8f65e..4d383d11574 100644
--- a/drivers/staging/rtl8821ae/stats.c
+++ b/drivers/staging/rtl8821ae/stats.c
@@ -28,9 +28,7 @@
  *****************************************************************************/
 #include "wifi.h"
 #include "stats.h"
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0))
 #include <linux/export.h>
-#endif
 
 u8 rtl_query_rxpwrpercentage(char antpower)
 {
diff --git a/drivers/staging/rtl8821ae/wifi.h b/drivers/staging/rtl8821ae/wifi.h
index 17a9d9f8781..e8250dad613 100644
--- a/drivers/staging/rtl8821ae/wifi.h
+++ b/drivers/staging/rtl8821ae/wifi.h
@@ -1086,8 +1086,8 @@ 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 */
+	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 */
diff --git a/drivers/staging/rts5139/Kconfig b/drivers/staging/rts5139/Kconfig
deleted file mode 100644
index afd526bf422..00000000000
--- a/drivers/staging/rts5139/Kconfig
+++ /dev/null
@@ -1,16 +0,0 @@
-config RTS5139
-	tristate "Realtek RTS5139 USB card reader support"
-	depends on USB && SCSI
-	help
-	  Say Y here to include driver code to support the Realtek
-	  RTS5139 USB card readers.
-
-	  If this driver is compiled as a module, it will be named rts5139.
-
-config RTS5139_DEBUG
-	bool "Realtek RTS5139 Card Reader verbose debug"
-	depends on RTS5139
-	help
-	  Say Y here in order to have the rts5139 code generate
-	  verbose debugging messages.
-
diff --git a/drivers/staging/rts5139/Makefile b/drivers/staging/rts5139/Makefile
deleted file mode 100644
index 75dd31224e6..00000000000
--- a/drivers/staging/rts5139/Makefile
+++ /dev/null
@@ -1,43 +0,0 @@
-# Driver for Realtek RTS51xx USB card reader
-#
-# Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
-#
-# Author:
-#   wwang (wei_wang@realsil.com.cn)
-#   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
-# Maintainer:
-#   Edwin Rong (edwin_rong@realsil.com.cn)
-#   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
-#
-# Makefile for the RTS51xx USB Card Reader drivers.
-#
-
-obj-$(CONFIG_RTS5139) := rts5139.o
-
-ccflags-y := -Idrivers/scsi
-
-rts5139-y :=				\
-		rts51x_transport.o 	\
-		rts51x_scsi.o		\
-		rts51x_fop.o		\
-		rts51x.o		\
-		rts51x_chip.o		\
-		rts51x_card.o		\
-		xd.o			\
-		sd.o			\
-		ms.o			\
-		sd_cprm.o		\
-		ms_mg.o
diff --git a/drivers/staging/rts5139/TODO b/drivers/staging/rts5139/TODO
deleted file mode 100644
index dd5fabb8ea7..00000000000
--- a/drivers/staging/rts5139/TODO
+++ /dev/null
@@ -1,9 +0,0 @@
-TODO:
-- support more USB card reader of Realtek family
-- use kernel coding style
-- checkpatch.pl fixes
-- stop having thousands of lines of code duplicated with staging/rts_pstor
-- This driver contains an entire SD/MMC stack -- it should use the stack in
-  drivers/mmc instead, as a host driver e.g. drivers/mmc/host/realtek-usb.c;
-  see drivers/mmc/host/ushc.c as an example.
-- This driver presents cards as SCSI devices, but they should be MMC devices.
diff --git a/drivers/staging/rts5139/debug.h b/drivers/staging/rts5139/debug.h
deleted file mode 100644
index 73dec133a1b..00000000000
--- a/drivers/staging/rts5139/debug.h
+++ /dev/null
@@ -1,46 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_DEBUG_H
-#define __RTS51X_DEBUG_H
-
-#include <linux/kernel.h>
-
-#define RTS51X_TIP "rts51x: "
-
-#ifdef CONFIG_RTS5139_DEBUG
-#define RTS51X_DEBUGP(x...) printk(KERN_DEBUG RTS51X_TIP x)
-#define RTS51X_DEBUGPN(x...) printk(KERN_DEBUG x)
-#define RTS51X_DEBUGPX(x...) printk(x)
-#define RTS51X_DEBUG(x) x
-#else
-#define RTS51X_DEBUGP(x...)
-#define RTS51X_DEBUGPN(x...)
-#define RTS51X_DEBUGPX(x...)
-#define RTS51X_DEBUG(x)
-#endif
-
-#endif /* __RTS51X_DEBUG_H */
diff --git a/drivers/staging/rts5139/ms.c b/drivers/staging/rts5139/ms.c
deleted file mode 100644
index 390292ad456..00000000000
--- a/drivers/staging/rts5139/ms.c
+++ /dev/null
@@ -1,4185 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#include <linux/blkdev.h>
-#include <linux/kthread.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-#include <linux/vmalloc.h>
-
-#include "debug.h"
-#include "trace.h"
-#include "rts51x.h"
-#include "rts51x_transport.h"
-#include "rts51x_scsi.h"
-#include "rts51x_card.h"
-#include "ms.h"
-
-static inline void ms_set_err_code(struct rts51x_chip *chip, u8 err_code)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-
-	ms_card->err_code = err_code;
-}
-
-static inline int ms_check_err_code(struct rts51x_chip *chip, u8 err_code)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-
-	return ms_card->err_code == err_code;
-}
-
-static int ms_parse_err_code(struct rts51x_chip *chip)
-{
-	TRACE_RET(chip, STATUS_FAIL);
-}
-
-static int ms_transfer_tpc(struct rts51x_chip *chip, u8 trans_mode, u8 tpc,
-			   u8 cnt, u8 cfg)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-
-	RTS51X_DEBUGP("ms_transfer_tpc: tpc = 0x%x\n", tpc);
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       PINGPONG_BUFFER);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
-		       MS_TRANSFER_START | trans_mode);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER, MS_TRANSFER_END,
-		       MS_TRANSFER_END);
-
-	rts51x_add_cmd(chip, READ_REG_CMD, MS_TRANS_CFG, 0, 0);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 2, 5000);
-	if (CHECK_MS_TRANS_FAIL(chip, retval)) {
-		rts51x_clear_ms_error(chip);
-		ms_set_err_code(chip, MS_TO_ERROR);
-		TRACE_RET(chip, ms_parse_err_code(chip));
-	}
-
-	if (!(tpc & 0x08)) {	/* Read Packet */
-		/* Check CRC16 & Ready Timeout */
-		if (chip->rsp_buf[1] & MS_CRC16_ERR) {
-			ms_set_err_code(chip, MS_CRC16_ERROR);
-			TRACE_RET(chip, ms_parse_err_code(chip));
-		}
-	} else { /* Write Packet */
-		if (CHK_MSPRO(ms_card) && !(chip->rsp_buf[1] & 0x80)) {
-			if (chip->rsp_buf[1] & (MS_INT_ERR | MS_INT_CMDNK)) {
-				ms_set_err_code(chip, MS_CMD_NK);
-				TRACE_RET(chip, ms_parse_err_code(chip));
-			}
-		}
-	}
-
-	/* Check Timeout of Ready Signal */
-	if (chip->rsp_buf[1] & MS_RDY_TIMEOUT) {
-		rts51x_clear_ms_error(chip);
-		ms_set_err_code(chip, MS_TO_ERROR);
-		TRACE_RET(chip, ms_parse_err_code(chip));
-	}
-
-	return STATUS_SUCCESS;
-}
-
-int ms_transfer_data(struct rts51x_chip *chip, u8 trans_mode, u8 tpc,
-		     u16 sec_cnt, u8 cfg, int mode_2k, int use_sg, void *buf,
-		     int buf_len)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	u8 val, err_code = 0, flag = 0;
-	enum dma_data_direction dir;
-	unsigned int pipe;
-
-	if (!buf || !buf_len)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	if (trans_mode == MS_TM_AUTO_READ) {
-		pipe = RCV_BULK_PIPE(chip);
-		dir = DMA_FROM_DEVICE;
-		flag = MODE_CDIR;
-		err_code = MS_FLASH_READ_ERROR;
-	} else if (trans_mode == MS_TM_AUTO_WRITE) {
-		pipe = SND_BULK_PIPE(chip);
-		dir = DMA_TO_DEVICE;
-		flag = MODE_CDOR;
-		err_code = MS_FLASH_WRITE_ERROR;
-	} else {
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_SECTOR_CNT_H, 0xFF,
-		       (u8) (sec_cnt >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_SECTOR_CNT_L, 0xFF,
-		       (u8) sec_cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       RING_BUFFER);
-
-	if (mode_2k)
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_CFG, MS_2K_SECTOR_MODE,
-			       MS_2K_SECTOR_MODE);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_CFG, MS_2K_SECTOR_MODE,
-			       0);
-
-	rts51x_trans_dma_enable(dir, chip, sec_cnt * 512, DMA_512);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
-		       MS_TRANSFER_START | trans_mode);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER, MS_TRANSFER_END,
-		       MS_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, flag | STAGE_MS_STATUS, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval =
-	    rts51x_transfer_data_rcc(chip, pipe, buf, buf_len, use_sg, NULL,
-				     15000, flag);
-	if (retval != STATUS_SUCCESS) {
-		ms_set_err_code(chip, err_code);
-		rts51x_clear_ms_error(chip);
-		TRACE_RET(chip, retval);
-	}
-
-	retval = rts51x_get_rsp(chip, 3, 15000);
-	if (CHECK_MS_TRANS_FAIL(chip, retval)) {
-		ms_set_err_code(chip, err_code);
-		rts51x_clear_ms_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	ms_card->last_rw_int = val = chip->rsp_buf[1];
-	if (val & (MS_INT_CMDNK | MS_INT_ERR | MS_CRC16_ERR | MS_RDY_TIMEOUT))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-int ms_write_bytes(struct rts51x_chip *chip, u8 tpc, u8 cnt, u8 cfg, u8 *data,
-		   int data_len)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-
-	if (!data || (data_len < cnt))
-		TRACE_RET(chip, STATUS_ERROR);
-
-	rts51x_init_cmd(chip);
-
-	for (i = 0; i < cnt; i++) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, PPBUF_BASE2 + i, 0xFF,
-			       data[i]);
-	}
-	if (cnt % 2)
-		rts51x_add_cmd(chip, WRITE_REG_CMD, PPBUF_BASE2 + i,
-				0xFF, 0xFF);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       PINGPONG_BUFFER);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
-		       MS_TRANSFER_START | MS_TM_WRITE_BYTES);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER, MS_TRANSFER_END,
-		       MS_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 1, 5000);
-	if (CHECK_MS_TRANS_FAIL(chip, retval)) {
-		u8 val = 0;
-
-		rts51x_ep0_read_register(chip, MS_TRANS_CFG, &val);
-		RTS51X_DEBUGP("MS_TRANS_CFG: 0x%02x\n", val);
-
-		rts51x_clear_ms_error(chip);
-
-		if (!(tpc & 0x08)) { /* Read Packet */
-			/* Check CRC16 & Ready Timeout */
-			if (val & MS_CRC16_ERR) {
-				ms_set_err_code(chip, MS_CRC16_ERROR);
-				TRACE_RET(chip, ms_parse_err_code(chip));
-			}
-		} else { /* Write Packet */
-			if (CHK_MSPRO(ms_card) && !(val & 0x80)) {
-				if (val & (MS_INT_ERR | MS_INT_CMDNK)) {
-					ms_set_err_code(chip, MS_CMD_NK);
-					TRACE_RET(chip,
-						  ms_parse_err_code(chip));
-				}
-			}
-		}
-
-		/* Check Timeout of Ready Signal */
-		if (val & MS_RDY_TIMEOUT) {
-			ms_set_err_code(chip, MS_TO_ERROR);
-			TRACE_RET(chip, ms_parse_err_code(chip));
-		}
-
-		ms_set_err_code(chip, MS_TO_ERROR);
-		TRACE_RET(chip, ms_parse_err_code(chip));
-	}
-
-	return STATUS_SUCCESS;
-}
-
-int ms_read_bytes(struct rts51x_chip *chip, u8 tpc, u8 cnt, u8 cfg, u8 *data,
-		  int data_len)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-
-	if (!data)
-		TRACE_RET(chip, STATUS_ERROR);
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       PINGPONG_BUFFER);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
-		       MS_TRANSFER_START | MS_TM_READ_BYTES);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER, MS_TRANSFER_END,
-		       MS_TRANSFER_END);
-
-	for (i = 0; i < data_len - 1; i++)
-		rts51x_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + i, 0, 0);
-
-	if (data_len % 2)
-		rts51x_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + data_len, 0,
-			       0);
-	else
-		rts51x_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + data_len - 1,
-			       0, 0);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, data_len + 1, 5000);
-	if (CHECK_MS_TRANS_FAIL(chip, retval)) {
-		u8 val = 0;
-
-		rts51x_ep0_read_register(chip, MS_TRANS_CFG, &val);
-		RTS51X_DEBUGP("MS_TRANS_CFG: 0x%02x\n", val);
-
-		rts51x_clear_ms_error(chip);
-
-		if (!(tpc & 0x08)) { /* Read Packet */
-			/* Check CRC16 & Ready Timeout */
-			if (val & MS_CRC16_ERR) {
-				ms_set_err_code(chip, MS_CRC16_ERROR);
-				TRACE_RET(chip, ms_parse_err_code(chip));
-			}
-		} else { /* Write Packet */
-			if (CHK_MSPRO(ms_card) && !(val & 0x80)) {
-				if (val & (MS_INT_ERR | MS_INT_CMDNK)) {
-					ms_set_err_code(chip, MS_CMD_NK);
-					TRACE_RET(chip,
-						  ms_parse_err_code(chip));
-				}
-			}
-		}
-
-		/* Check Timeout of Ready Signal */
-		if (val & MS_RDY_TIMEOUT) {
-			ms_set_err_code(chip, MS_TO_ERROR);
-			TRACE_RET(chip, ms_parse_err_code(chip));
-		}
-
-		ms_set_err_code(chip, MS_TO_ERROR);
-		TRACE_RET(chip, ms_parse_err_code(chip));
-	}
-
-	rts51x_read_rsp_buf(chip, 1, data, data_len);
-
-	return STATUS_SUCCESS;
-}
-
-int ms_set_rw_reg_addr(struct rts51x_chip *chip,
-		       u8 read_start, u8 read_cnt, u8 write_start, u8 write_cnt)
-{
-	int retval, i;
-	u8 data[4];
-
-	data[0] = read_start;
-	data[1] = read_cnt;
-	data[2] = write_start;
-	data[3] = write_cnt;
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_write_bytes(chip, SET_RW_REG_ADRS, 4, NO_WAIT_INT, data,
-				   4);
-		if (retval == STATUS_SUCCESS)
-			return STATUS_SUCCESS;
-		rts51x_clear_ms_error(chip);
-	}
-
-	TRACE_RET(chip, STATUS_FAIL);
-}
-
-static int ms_send_cmd(struct rts51x_chip *chip, u8 cmd, u8 cfg)
-{
-	u8 data[2];
-
-	data[0] = cmd;
-	data[1] = 0;
-
-	return ms_write_bytes(chip, PRO_SET_CMD, 1, cfg, data, 1);
-}
-
-static int ms_set_cmd(struct rts51x_chip *chip,
-		      u8 read_start, u8 read_count,
-		      u8 write_start, u8 write_count,
-		      u8 cmd, u8 cfg, u8 *data, int data_len, u8 *int_stat)
-{
-	int retval, i;
-	u8 val;
-
-	if (!data || (data_len <= 0) || (data_len > 128)) {
-		RTS51X_DEBUGP("ms_set_cmd (data_len = %d)\n", data_len);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	retval =
-	    ms_set_rw_reg_addr(chip, read_start, read_count, write_start,
-			       write_count);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_write_bytes(chip, WRITE_REG, write_count, NO_WAIT_INT,
-				   data, data_len);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	ms_set_err_code(chip, MS_NO_ERROR);
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval = ms_send_cmd(chip, cmd, WAIT_INT);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
-	/* GET_INT Register */
-	ms_set_err_code(chip, MS_NO_ERROR);
-	retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (int_stat)
-		*int_stat = val;
-
-	return STATUS_SUCCESS;
-}
-
-#ifdef MS_SPEEDUP
-static int ms_auto_set_cmd(struct rts51x_chip *chip,
-			   u8 read_start, u8 read_count,
-			   u8 write_start, u8 write_count,
-			   u8 cmd, u8 cfg, u8 *data, int data_len,
-			   u8 *int_stat)
-{
-	int retval;
-	int i;
-
-	if (!data || (data_len <= 0) || (data_len > 128)) {
-		RTS51X_DEBUGP("ms_auto_set_cmd (data_len = %d)\n", data_len);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_READ_START, 0xFF, read_start);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_READ_COUNT, 0xFF, read_count);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_WRITE_START, 0xFF, write_start);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_WRITE_COUNT, 0xFF, write_count);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_COMMAND, 0xFF, cmd);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       PINGPONG_BUFFER);
-
-	for (i = 0; i < data_len; i++) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, PPBUF_BASE2 + i, 0xFF,
-			       data[i]);
-	}
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
-		       MS_TRANSFER_START | MS_TM_SET_CMD);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER, MS_TRANSFER_END,
-		       MS_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, MODE_CR | STAGE_MS_STATUS, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 3, 5000);
-
-	if (CHECK_MS_TRANS_FAIL(chip, retval)) {
-		rts51x_clear_ms_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (int_stat)
-		*int_stat = chip->rsp_buf[2];
-
-	return STATUS_SUCCESS;
-}
-#endif
-
-static int ms_set_init_para(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-
-	if (CHK_HG8BIT(ms_card)) {
-		if (chip->asic_code)
-			ms_card->ms_clock = chip->option.asic_ms_hg_clk;
-		else
-			ms_card->ms_clock = chip->option.fpga_ms_hg_clk;
-	} else if (CHK_MSPRO(ms_card) || CHK_MS4BIT(ms_card)) {
-		if (chip->asic_code)
-			ms_card->ms_clock = chip->option.asic_ms_4bit_clk;
-		else
-			ms_card->ms_clock = chip->option.fpga_ms_4bit_clk;
-	} else {
-		if (chip->asic_code)
-			ms_card->ms_clock = 38;
-		else
-			ms_card->ms_clock = CLK_40;
-	}
-
-	retval = switch_clock(chip, ms_card->ms_clock);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_select_card(chip, MS_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-int ms_switch_clock(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-
-	retval = rts51x_select_card(chip, MS_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = switch_clock(chip, ms_card->ms_clock);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static void ms_pull_ctl_disable(struct rts51x_chip *chip)
-{
-	if (CHECK_PKG(chip, LQFP48)) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0xA5);
-	} else {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x65);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x56);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0x59);
-	}
-}
-
-static void ms_pull_ctl_enable(struct rts51x_chip *chip)
-{
-	if (CHECK_PKG(chip, LQFP48)) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0xA5);
-	} else {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x65);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0x59);
-	}
-}
-
-static int ms_prepare_reset(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-
-	ms_card->ms_type = 0;
-	ms_card->check_ms_flow = 0;
-	ms_card->switch_8bit_fail = 0;
-	ms_card->delay_write.delay_write_flag = 0;
-
-	ms_card->pro_under_formatting = 0;
-
-	rts51x_init_cmd(chip);
-
-	if (chip->asic_code) {
-		ms_pull_ctl_enable(chip);
-	} else {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, FPGA_PULL_CTL,
-			       FPGA_MS_PULL_CTL_BIT | 0x20, 0);
-	}
-	/* Tri-state MS output */
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_OE, MS_OUTPUT_EN, 0);
-
-	if (!chip->option.FT2_fast_mode) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, POWER_MASK,
-			       POWER_OFF);
-	}
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (!chip->option.FT2_fast_mode) {
-		wait_timeout(250);
-
-		rts51x_card_power_on(chip, MS_CARD);
-		wait_timeout(150);
-
-#ifdef SUPPORT_OCP
-		rts51x_get_card_status(chip, &(chip->card_status));
-		/* get OCP status */
-		chip->ocp_stat = (chip->card_status >> 4) & 0x03;
-
-		if (chip->ocp_stat & (MS_OCP_NOW | MS_OCP_EVER)) {
-			RTS51X_DEBUGP("Over current, OCPSTAT is 0x%x\n",
-				       chip->ocp_stat);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-#endif
-	}
-
-	rts51x_init_cmd(chip);
-
-	/* Enable MS Output */
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_OE, MS_OUTPUT_EN,
-		       MS_OUTPUT_EN);
-
-	/* Reset Registers */
-	if (chip->asic_code)
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_CFG, 0xFF,
-			       SAMPLE_TIME_RISING | PUSH_TIME_DEFAULT |
-			       NO_EXTEND_TOGGLE | MS_BUS_WIDTH_1);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_CFG, 0xFF,
-			       SAMPLE_TIME_FALLING | PUSH_TIME_DEFAULT |
-			       NO_EXTEND_TOGGLE | MS_BUS_WIDTH_1);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF,
-		       NO_WAIT_INT | NO_AUTO_READ_INT_REG);
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return ms_set_init_para(chip);
-}
-
-static int ms_identify_media_type(struct rts51x_chip *chip, int switch_8bit_bus)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-	u8 val;
-
-	retval = ms_set_rw_reg_addr(chip, Pro_StatusReg, 6, SystemParm, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	/* Get Register form MS-PRO card */
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_transfer_tpc(chip, MS_TM_READ_BYTES, READ_REG, 6,
-				    NO_WAIT_INT);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	RTS51X_READ_REG(chip, PPBUF_BASE2 + 2, &val);
-	RTS51X_DEBUGP("Type register: 0x%x\n", val);
-	if (val != 0x01) {
-		if (val != 0x02)
-			ms_card->check_ms_flow = 1;
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-	/* Category Register */
-	RTS51X_READ_REG(chip, PPBUF_BASE2 + 4, &val);
-	RTS51X_DEBUGP("Category register: 0x%x\n", val);
-	if (val != 0) {
-		ms_card->check_ms_flow = 1;
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-	/* Class Register */
-	RTS51X_READ_REG(chip, PPBUF_BASE2 + 5, &val);
-	RTS51X_DEBUGP("Class register: 0x%x\n", val);
-	if (val == 0) {
-		RTS51X_READ_REG(chip, PPBUF_BASE2, &val);
-		if (val & WRT_PRTCT)
-			chip->card_wp |= MS_CARD;
-		else
-			chip->card_wp &= ~MS_CARD;
-	} else if ((val == 0x01) || (val == 0x02) || (val == 0x03)) {
-		chip->card_wp |= MS_CARD;
-	} else {
-		ms_card->check_ms_flow = 1;
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	ms_card->ms_type |= TYPE_MSPRO;
-
-	/* Check MSPro-HG Card, use IF Mode Register to distinguish */
-	RTS51X_READ_REG(chip, PPBUF_BASE2 + 3, &val);
-	RTS51X_DEBUGP("IF Mode register: 0x%x\n", val);
-	if (val == 0) {
-		ms_card->ms_type &= 0x0F;
-	} else if (val == 7) {
-		if (switch_8bit_bus)
-			ms_card->ms_type |= MS_HG;
-		else
-			ms_card->ms_type &= 0x0F;
-	} else {
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	/* end Procedure to identify Media Type */
-	return STATUS_SUCCESS;
-}
-
-static int ms_confirm_cpu_startup(struct rts51x_chip *chip)
-{
-	int retval, i, k;
-	u8 val;
-
-	/* Confirm CPU StartUp */
-	k = 0;
-	do {
-		if (monitor_card_cd(chip, MS_CARD) == CD_NOT_EXIST)
-			TRACE_RET(chip, STATUS_FAIL);
-
-		for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-			retval =
-			    ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val,
-					  1);
-			if (retval == STATUS_SUCCESS)
-				break;
-		}
-		if (i == MS_MAX_RETRY_COUNT)
-			TRACE_RET(chip, STATUS_FAIL);
-
-		if (k > 100)
-			TRACE_RET(chip, STATUS_FAIL);
-		k++;
-		wait_timeout(100);
-	} while (!(val & INT_REG_CED));
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	if (val & INT_REG_ERR) {
-		if (val & INT_REG_CMDNK) {	/* CMDNK = 1 */
-			chip->card_wp |= (MS_CARD);
-		} else {	/* CMDNK = 0 */
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-	/*--  end confirm CPU startup */
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_switch_parallel_bus(struct rts51x_chip *chip)
-{
-	int retval, i;
-	u8 data[2];
-
-	data[0] = PARALLEL_4BIT_IF;
-	data[1] = 0;
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_write_bytes(chip, WRITE_REG, 1, NO_WAIT_INT, data, 2);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_switch_8bit_bus(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-	u8 data[2];
-
-	data[0] = PARALLEL_8BIT_IF;
-	data[1] = 0;
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_write_bytes(chip, WRITE_REG, 1, NO_WAIT_INT, data, 2);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	RTS51X_WRITE_REG(chip, MS_CFG, 0x98,
-			 MS_BUS_WIDTH_8 | SAMPLE_TIME_FALLING);
-	ms_card->ms_type |= MS_8BIT;
-
-	retval = ms_set_init_para(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_transfer_tpc(chip, MS_TM_READ_BYTES, GET_INT, 1,
-				    NO_WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_pro_reset_flow(struct rts51x_chip *chip, int switch_8bit_bus)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-
-	for (i = 0; i < 3; i++) {
-		retval = ms_prepare_reset(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval = ms_identify_media_type(chip, switch_8bit_bus);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval = ms_confirm_cpu_startup(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval = ms_switch_parallel_bus(chip);
-		if (retval != STATUS_SUCCESS) {
-			if (monitor_card_cd(chip, MS_CARD) == CD_NOT_EXIST)
-				TRACE_RET(chip, STATUS_FAIL);
-			continue;
-		} else {
-			break;
-		}
-	}
-
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_WRITE_REG(chip, MS_CFG, 0x18, MS_BUS_WIDTH_4);
-
-	RTS51X_WRITE_REG(chip, MS_CFG, PUSH_TIME_ODD, PUSH_TIME_ODD);
-
-	retval = ms_set_init_para(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (CHK_MSHG(ms_card) && switch_8bit_bus) {
-		retval = ms_switch_8bit_bus(chip);
-		if (retval != STATUS_SUCCESS) {
-			ms_card->switch_8bit_fail = 1;
-			TRACE_RET(chip, retval);
-		}
-	}
-
-	return STATUS_SUCCESS;
-}
-
-#ifdef XC_POWERCLASS
-static int msxc_change_power(struct rts51x_chip *chip, u8 mode)
-{
-	int retval;
-	u8 buf[6];
-
-	rts51x_ms_cleanup_work(chip);
-
-	/* Set Parameter Register */
-	retval = ms_set_rw_reg_addr(chip, 0, 0, Pro_DataCount1, 6);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	buf[0] = 0;
-	buf[1] = mode;
-	buf[2] = 0;
-	buf[3] = 0;
-	buf[4] = 0;
-	buf[5] = 0;
-
-	retval = ms_write_bytes(chip, PRO_WRITE_REG, 6, NO_WAIT_INT, buf, 6);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = ms_send_cmd(chip, XC_CHG_POWER, WAIT_INT);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_READ_REG(chip, MS_TRANS_CFG, buf);
-	if (buf[0] & (MS_INT_CMDNK | MS_INT_ERR))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-#endif
-
-static int ms_read_attribute_info(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-	u8 val, *buf, class_code, device_type, sub_class, data[16];
-	u16 total_blk = 0, blk_size = 0;
-#ifdef SUPPORT_MSXC
-	u32 xc_total_blk = 0, xc_blk_size = 0;
-#endif
-	u32 sys_info_addr = 0, sys_info_size;
-#ifdef SUPPORT_PCGL_1P18
-	u32 model_name_addr = 0, model_name_size;
-	int found_sys_info = 0, found_model_name = 0;
-#endif
-
-	retval = ms_set_rw_reg_addr(chip, Pro_IntReg, 2, Pro_SystemParm, 7);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (CHK_MS8BIT(ms_card))
-		data[0] = PARALLEL_8BIT_IF;
-	else
-		data[0] = PARALLEL_4BIT_IF;
-	data[1] = 0;
-
-	data[2] = 0x40;
-	data[3] = 0;
-	data[4] = 0;
-	data[5] = 0;
-	/* Start address 0 */
-	data[6] = 0;
-	data[7] = 0;
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_write_bytes(chip, PRO_WRITE_REG, 7, NO_WAIT_INT, data,
-				   8);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	buf = kmalloc(64 * 512, GFP_KERNEL);
-	if (buf == NULL)
-		TRACE_RET(chip, STATUS_NOMEM);
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval = ms_send_cmd(chip, PRO_READ_ATRB, WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			continue;
-
-		retval = rts51x_read_register(chip, MS_TRANS_CFG, &val);
-		if (retval != STATUS_SUCCESS) {
-			kfree(buf);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-		if (!(val & MS_INT_BREQ)) {
-			kfree(buf);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		retval =
-		    ms_transfer_data(chip, MS_TM_AUTO_READ, PRO_READ_LONG_DATA,
-				     0x40, WAIT_INT, 0, 0, buf, 64 * 512);
-		if (retval == STATUS_SUCCESS)
-			break;
-		else
-			rts51x_clear_ms_error(chip);
-	}
-	if (retval != STATUS_SUCCESS) {
-		kfree(buf);
-		TRACE_RET(chip, retval);
-	}
-
-	i = 0;
-	do {
-		retval = rts51x_read_register(chip, MS_TRANS_CFG, &val);
-		if (retval != STATUS_SUCCESS) {
-			kfree(buf);
-			TRACE_RET(chip, retval);
-		}
-
-		if ((val & MS_INT_CED) || !(val & MS_INT_BREQ))
-			break;
-
-		retval =
-		    ms_transfer_tpc(chip, MS_TM_NORMAL_READ, PRO_READ_LONG_DATA,
-				    0, WAIT_INT);
-		if (retval != STATUS_SUCCESS) {
-			kfree(buf);
-			TRACE_RET(chip, retval);
-		}
-
-		i++;
-	} while (i < 1024);
-
-	if (retval != STATUS_SUCCESS) {
-		kfree(buf);
-		TRACE_RET(chip, retval);
-	}
-
-	if ((buf[0] != 0xa5) && (buf[1] != 0xc3)) {
-		/* Signature code is wrong */
-		kfree(buf);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if ((buf[4] < 1) || (buf[4] > 12)) {
-		kfree(buf);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	for (i = 0; i < buf[4]; i++) {
-		int cur_addr_off = 16 + i * 12;
-
-#ifdef SUPPORT_MSXC
-		if ((buf[cur_addr_off + 8] == 0x10)
-		    || (buf[cur_addr_off + 8] == 0x13)) {
-#else
-		if (buf[cur_addr_off + 8] == 0x10) {
-#endif
-			sys_info_addr = ((u32) buf[cur_addr_off + 0] << 24) |
-			    ((u32) buf[cur_addr_off + 1] << 16) |
-			    ((u32) buf[cur_addr_off + 2] << 8) |
-			    buf[cur_addr_off + 3];
-			sys_info_size =
-			    ((u32) buf[cur_addr_off + 4] << 24) |
-			    ((u32) buf[cur_addr_off + 5] << 16) |
-			    ((u32) buf[cur_addr_off + 6] << 8) |
-			    buf[cur_addr_off + 7];
-			RTS51X_DEBUGP("sys_info_addr = 0x%x, sys_info_size = 0x%x\n",
-						sys_info_addr, sys_info_size);
-			if (sys_info_size != 96) {
-				kfree(buf);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-			if (sys_info_addr < 0x1A0) {
-				kfree(buf);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-			if ((sys_info_size + sys_info_addr) > 0x8000) {
-				kfree(buf);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-#ifdef SUPPORT_MSXC
-			if (buf[cur_addr_off + 8] == 0x13)
-				ms_card->ms_type |= MS_XC;
-#endif
-#ifdef SUPPORT_PCGL_1P18
-			found_sys_info = 1;
-#else
-			break;
-#endif
-		}
-#ifdef SUPPORT_PCGL_1P18
-		if (buf[cur_addr_off + 8] == 0x15) {
-			model_name_addr = ((u32) buf[cur_addr_off + 0] << 24) |
-			    ((u32) buf[cur_addr_off + 1] << 16) |
-			    ((u32) buf[cur_addr_off + 2] << 8) |
-			    buf[cur_addr_off + 3];
-			model_name_size =
-			    ((u32) buf[cur_addr_off + 4] << 24) |
-			    ((u32) buf[cur_addr_off + 5] << 16) |
-			    ((u32) buf[cur_addr_off + 6] << 8) |
-			    buf[cur_addr_off + 7];
-			RTS51X_DEBUGP("model_name_addr = 0x%x, model_name_size = 0x%x\n",
-					model_name_addr, model_name_size);
-			if (model_name_size != 48) {
-				kfree(buf);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-			if (model_name_addr < 0x1A0) {
-				kfree(buf);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-			if ((model_name_size + model_name_addr) > 0x8000) {
-				kfree(buf);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-
-			found_model_name = 1;
-		}
-
-		if (found_sys_info && found_model_name)
-			break;
-#endif
-	}
-
-	if (i == buf[4]) {
-		kfree(buf);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	class_code = buf[sys_info_addr + 0];
-	device_type = buf[sys_info_addr + 56];
-	sub_class = buf[sys_info_addr + 46];
-#ifdef SUPPORT_MSXC
-	if (CHK_MSXC(ms_card)) {
-		xc_total_blk = ((u32) buf[sys_info_addr + 6] << 24) |
-		    ((u32) buf[sys_info_addr + 7] << 16) |
-		    ((u32) buf[sys_info_addr + 8] << 8) |
-		    buf[sys_info_addr + 9];
-		xc_blk_size = ((u32) buf[sys_info_addr + 32] << 24) |
-		    ((u32) buf[sys_info_addr + 33] << 16) |
-		    ((u32) buf[sys_info_addr + 34] << 8) |
-		    buf[sys_info_addr + 35];
-		RTS51X_DEBUGP("xc_total_blk = 0x%x, xc_blk_size = 0x%x\n",
-			       xc_total_blk, xc_blk_size);
-	} else {
-		total_blk =
-		    ((u16) buf[sys_info_addr + 6] << 8) | buf[sys_info_addr +
-							      7];
-		blk_size =
-		    ((u16) buf[sys_info_addr + 2] << 8) | buf[sys_info_addr +
-							      3];
-		RTS51X_DEBUGP("total_blk = 0x%x, blk_size = 0x%x\n", total_blk,
-			       blk_size);
-	}
-#else
-	total_blk =
-	    ((u16) buf[sys_info_addr + 6] << 8) | buf[sys_info_addr + 7];
-	blk_size = ((u16) buf[sys_info_addr + 2] << 8) | buf[sys_info_addr + 3];
-	RTS51X_DEBUGP("total_blk = 0x%x, blk_size = 0x%x\n", total_blk,
-		       blk_size);
-#endif
-
-	RTS51X_DEBUGP("class_code = 0x%x, device_type = 0x%x,"
-			"sub_class = 0x%x\n",
-				class_code, device_type, sub_class);
-
-	memcpy(ms_card->raw_sys_info, buf + sys_info_addr, 96);
-#ifdef SUPPORT_PCGL_1P18
-	memcpy(ms_card->raw_model_name, buf + model_name_addr, 48);
-#endif
-
-	kfree(buf);
-
-	/* Confirm System Information */
-#ifdef SUPPORT_MSXC
-	if (CHK_MSXC(ms_card)) {
-		if (class_code != 0x03)
-			TRACE_RET(chip, STATUS_FAIL);
-	} else {
-		if (class_code != 0x02)
-			TRACE_RET(chip, STATUS_FAIL);
-	}
-#else
-	if (class_code != 0x02)
-		TRACE_RET(chip, STATUS_FAIL);
-#endif
-
-	if (device_type != 0x00) {
-		if ((device_type == 0x01) || (device_type == 0x02)
-		    || (device_type == 0x03))
-			chip->card_wp |= MS_CARD;
-		else
-			TRACE_RET(chip, STATUS_FAIL);
-	}
-	if (sub_class & 0xC0)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	RTS51X_DEBUGP("class_code: 0x%x, device_type: 0x%x, sub_class: 0x%x\n",
-		       class_code, device_type, sub_class);
-
-#ifdef SUPPORT_MSXC
-	if (CHK_MSXC(ms_card)) {
-		chip->capacity[chip->card2lun[MS_CARD]] = ms_card->capacity =
-		    xc_total_blk * xc_blk_size;
-	} else {
-		chip->capacity[chip->card2lun[MS_CARD]] = ms_card->capacity =
-		    total_blk * blk_size;
-	}
-#else
-	chip->capacity[chip->card2lun[MS_CARD]] = ms_card->capacity =
-	    total_blk * blk_size;
-#endif
-
-	return STATUS_SUCCESS;
-}
-
-#ifdef SUPPORT_MAGIC_GATE
-int mg_set_tpc_para_sub(struct rts51x_chip *chip, int type, u8 mg_entry_num);
-#endif
-
-static int reset_ms_pro(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-#ifdef XC_POWERCLASS
-	u8 change_power_class = 2;
-#endif
-
-#ifdef XC_POWERCLASS
-Retry:
-#endif
-	retval = ms_pro_reset_flow(chip, 1);
-	if (retval != STATUS_SUCCESS) {
-		if (ms_card->switch_8bit_fail) {
-			retval = ms_pro_reset_flow(chip, 0);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-		} else {
-			TRACE_RET(chip, retval);
-		}
-	}
-
-	retval = ms_read_attribute_info(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-#ifdef XC_POWERCLASS
-	if (CHK_HG8BIT(ms_card))
-		change_power_class = 0;
-
-	if (change_power_class && CHK_MSXC(ms_card)) {
-		u8 power_class_mode = (ms_card->raw_sys_info[46] & 0x18) >> 3;
-		RTS51X_DEBUGP("power_class_mode = 0x%x", power_class_mode);
-		if (change_power_class > power_class_mode)
-			change_power_class = power_class_mode;
-		if (change_power_class) {
-			retval = msxc_change_power(chip, change_power_class);
-			if (retval != STATUS_SUCCESS) {
-				change_power_class--;
-				goto Retry;
-			}
-		}
-	}
-#endif
-
-#ifdef SUPPORT_MAGIC_GATE
-	retval = mg_set_tpc_para_sub(chip, 0, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-#endif
-
-	if (CHK_HG8BIT(ms_card))
-		chip->card_bus_width[chip->card2lun[MS_CARD]] = 8;
-	else
-		chip->card_bus_width[chip->card2lun[MS_CARD]] = 4;
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_read_status_reg(struct rts51x_chip *chip)
-{
-	int retval;
-	u8 val[2];
-
-	retval = ms_set_rw_reg_addr(chip, StatusReg0, 2, 0, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = ms_read_bytes(chip, READ_REG, 2, NO_WAIT_INT, val, 2);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	if (val[1] & (STS_UCDT | STS_UCEX | STS_UCFG)) {
-		ms_set_err_code(chip, MS_FLASH_READ_ERROR);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_check_boot_block(struct rts51x_chip *chip, u16 block_addr)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	u8 extra[MS_EXTRA_SIZE], data[10], val = 0;
-
-	if (CHK_MS4BIT(ms_card)) {
-		/* Parallel interface */
-		data[0] = 0x88;
-	} else {
-		/* Serial interface */
-		data[0] = 0x80;
-	}
-	/* Block Address */
-	data[1] = 0;
-	data[2] = (u8) (block_addr >> 8);
-	data[3] = (u8) block_addr;
-	/* Page Number
-	 * Extra data access mode */
-	data[4] = 0x40;
-	data[5] = 0;
-
-	retval = ms_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 6,
-			    BLOCK_READ, WAIT_INT, data, 6, &val);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (val & INT_REG_CMDNK) {
-		ms_set_err_code(chip, MS_CMD_NK);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-	if (val & INT_REG_CED) {
-		if (val & INT_REG_ERR) {
-			retval = ms_read_status_reg(chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-			retval =
-			    ms_set_rw_reg_addr(chip, OverwriteFlag,
-					       MS_EXTRA_SIZE, SystemParm, 6);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-		}
-	}
-
-	retval =
-	    ms_read_bytes(chip, READ_REG, MS_EXTRA_SIZE, NO_WAIT_INT, extra,
-			  MS_EXTRA_SIZE);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (!(extra[0] & BLOCK_OK) || (extra[1] & NOT_BOOT_BLOCK))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_read_extra_data(struct rts51x_chip *chip,
-			      u16 block_addr, u8 page_num, u8 *buf,
-			      int buf_len)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	u8 val = 0, data[10];
-
-	if (CHK_MS4BIT(ms_card)) {
-		/* Parallel interface */
-		data[0] = 0x88;
-	} else {
-		/* Serial interface */
-		data[0] = 0x80;
-	}
-	/* Block Address */
-	data[1] = 0;
-	data[2] = (u8) (block_addr >> 8);
-	data[3] = (u8) block_addr;
-	/* Page Number
-	 * Extra data access mode */
-	data[4] = 0x40;
-	data[5] = page_num;
-
-#ifdef MS_SPEEDUP
-	retval =
-	    ms_auto_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 6,
-			    BLOCK_READ, WAIT_INT, data, 6, &val);
-#else
-	retval = ms_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 6,
-			    BLOCK_READ, WAIT_INT, data, 6, &val);
-#endif
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (val & INT_REG_CMDNK) {
-		ms_set_err_code(chip, MS_CMD_NK);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-	if (val & INT_REG_CED) {
-		if (val & INT_REG_ERR) {
-			retval = ms_read_status_reg(chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-			retval =
-			    ms_set_rw_reg_addr(chip, OverwriteFlag,
-					       MS_EXTRA_SIZE, SystemParm, 6);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-		}
-	}
-
-	retval =
-	    ms_read_bytes(chip, READ_REG, MS_EXTRA_SIZE, NO_WAIT_INT, data,
-			  MS_EXTRA_SIZE);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (buf && buf_len) {
-		if (buf_len > MS_EXTRA_SIZE)
-			buf_len = MS_EXTRA_SIZE;
-		memcpy(buf, data, buf_len);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_write_extra_data(struct rts51x_chip *chip,
-			       u16 block_addr, u8 page_num, u8 *buf,
-			       int buf_len)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-	u8 val = 0, data[16];
-
-	if (!buf || (buf_len < MS_EXTRA_SIZE))
-		TRACE_RET(chip, STATUS_FAIL);
-	/* Write REG */
-	if (CHK_MS4BIT(ms_card)) {
-		/* Parallel interface */
-		data[0] = 0x88;
-	} else {
-		/* Serial interface */
-		data[0] = 0x80;
-	}
-	/* Block Address */
-	data[1] = 0;
-	data[2] = (u8) (block_addr >> 8);
-	data[3] = (u8) block_addr;
-	/* Page Number
-	 * Extra data access mode */
-	data[4] = 0x40;
-	data[5] = page_num;
-
-	for (i = 6; i < MS_EXTRA_SIZE + 6; i++)
-		data[i] = buf[i - 6];
-
-#ifdef MS_SPEEDUP
-	retval =
-	    ms_auto_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm,
-			    6 + MS_EXTRA_SIZE, BLOCK_WRITE, WAIT_INT, data, 16,
-			    &val);
-#else
-	retval =
-	    ms_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm,
-		       6 + MS_EXTRA_SIZE, BLOCK_WRITE, WAIT_INT, data, 16,
-		       &val);
-#endif
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (val & INT_REG_CMDNK) {
-		ms_set_err_code(chip, MS_CMD_NK);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-	if (val & INT_REG_CED) {
-		if (val & INT_REG_ERR) {
-			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_read_page(struct rts51x_chip *chip, u16 block_addr, u8 page_num)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	u8 val = 0, data[6];
-
-	if (CHK_MS4BIT(ms_card)) {
-		/* Parallel interface */
-		data[0] = 0x88;
-	} else {
-		/* Serial interface */
-		data[0] = 0x80;
-	}
-	/* Block Address */
-	data[1] = 0;
-	data[2] = (u8) (block_addr >> 8);
-	data[3] = (u8) block_addr;
-	/* Page Number
-	 * Single page access mode */
-	data[4] = 0x20;
-	data[5] = page_num;
-
-	retval = ms_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 6,
-			    BLOCK_READ, WAIT_INT, data, 6, &val);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (val & INT_REG_CMDNK) {
-		ms_set_err_code(chip, MS_CMD_NK);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (val & INT_REG_CED) {
-		if (val & INT_REG_ERR) {
-			if (!(val & INT_REG_BREQ)) {
-				ms_set_err_code(chip, MS_FLASH_READ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-			retval = ms_read_status_reg(chip);
-			if (retval != STATUS_SUCCESS)
-				ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-		} else {
-			if (!(val & INT_REG_BREQ)) {
-				ms_set_err_code(chip, MS_BREQ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-	}
-
-	retval =
-	    ms_transfer_tpc(chip, MS_TM_NORMAL_READ, READ_PAGE_DATA, 0,
-			    NO_WAIT_INT);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	if (ms_check_err_code(chip, MS_FLASH_WRITE_ERROR))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_set_bad_block(struct rts51x_chip *chip, u16 phy_blk)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	u8 val = 0, data[8], extra[MS_EXTRA_SIZE];
-
-	retval = ms_read_extra_data(chip, phy_blk, 0, extra, MS_EXTRA_SIZE);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	ms_set_err_code(chip, MS_NO_ERROR);
-
-	if (CHK_MS4BIT(ms_card)) {
-		/* Parallel interface */
-		data[0] = 0x88;
-	} else {
-		/* Serial interface */
-		data[0] = 0x80;
-	}
-	/* Block Address */
-	data[1] = 0;
-	data[2] = (u8) (phy_blk >> 8);
-	data[3] = (u8) phy_blk;
-	data[4] = 0x80;
-	data[5] = 0;
-	data[6] = extra[0] & 0x7F;
-	data[7] = 0xFF;
-
-#ifdef MS_SPEEDUP
-	retval = ms_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 7,
-			    BLOCK_WRITE, WAIT_INT, data, 7, &val);
-#else
-	retval = ms_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 7,
-			    BLOCK_WRITE, WAIT_INT, data, 7, &val);
-#endif
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (val & INT_REG_CMDNK) {
-		ms_set_err_code(chip, MS_CMD_NK);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (val & INT_REG_CED) {
-		if (val & INT_REG_ERR) {
-			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_erase_block(struct rts51x_chip *chip, u16 phy_blk)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i = 0;
-	u8 val = 0, data[6];
-
-	retval =
-	    ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm,
-			       6);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	ms_set_err_code(chip, MS_NO_ERROR);
-
-	if (CHK_MS4BIT(ms_card)) {
-		/* Parallel interface */
-		data[0] = 0x88;
-	} else {
-		/* Serial interface */
-		data[0] = 0x80;
-	}
-	/* Block Address */
-	data[1] = 0;
-	data[2] = (u8) (phy_blk >> 8);
-	data[3] = (u8) phy_blk;
-	data[4] = 0;
-	data[5] = 0;
-
-ERASE_RTY:
-#ifdef MS_SPEEDUP
-	retval =
-	    ms_auto_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 6,
-			    BLOCK_ERASE, WAIT_INT, data, 6, &val);
-#else
-	retval = ms_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 6,
-			    BLOCK_ERASE, WAIT_INT, data, 6, &val);
-#endif
-
-	if (val & INT_REG_CMDNK) {
-		if (i < 3) {
-			i++;
-			goto ERASE_RTY;
-		}
-		ms_set_err_code(chip, MS_CMD_NK);
-		ms_set_bad_block(chip, phy_blk);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (val & INT_REG_CED) {
-		if (val & INT_REG_ERR) {
-			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static void ms_set_page_status(u16 log_blk, u8 type, u8 *extra, int extra_len)
-{
-	if (!extra || (extra_len < MS_EXTRA_SIZE))
-		return;
-
-	memset(extra, 0xFF, MS_EXTRA_SIZE);
-
-	if (type == setPS_NG)
-		extra[0] = 0xB8;
-	else
-		extra[0] = 0x98;
-
-	extra[2] = (u8) (log_blk >> 8);
-	extra[3] = (u8) log_blk;
-}
-
-static int ms_init_page(struct rts51x_chip *chip, u16 phy_blk, u16 log_blk,
-			u8 start_page, u8 end_page)
-{
-	int retval;
-	u8 extra[MS_EXTRA_SIZE], i;
-
-	memset(extra, 0xff, MS_EXTRA_SIZE);
-
-	extra[0] = 0xf8; /* Block, page OK, data erased */
-	extra[1] = 0xff;
-	extra[2] = (u8) (log_blk >> 8);
-	extra[3] = (u8) log_blk;
-
-	for (i = start_page; i < end_page; i++) {
-		if (monitor_card_cd(chip, MS_CARD) == CD_NOT_EXIST)
-			TRACE_RET(chip, STATUS_FAIL);
-
-		retval =
-		    ms_write_extra_data(chip, phy_blk, i, extra, MS_EXTRA_SIZE);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_copy_page(struct rts51x_chip *chip, u16 old_blk, u16 new_blk,
-			u16 log_blk, u8 start_page, u8 end_page)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, rty_cnt, uncorrect_flag = 0;
-	u8 extra[MS_EXTRA_SIZE], val, i, j, data[16];
-
-	RTS51X_DEBUGP("Copy page from 0x%x to 0x%x, logical block is 0x%x\n",
-		       old_blk, new_blk, log_blk);
-	RTS51X_DEBUGP("start_page = %d, end_page = %d\n", start_page,
-		       end_page);
-
-	retval = ms_read_extra_data(chip, new_blk, 0, extra, MS_EXTRA_SIZE);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = ms_read_status_reg(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_READ_REG(chip, PPBUF_BASE2, &val);
-
-	if (val & BUF_FULL) {
-		/* Clear Buffer */
-		retval = ms_send_cmd(chip, CLEAR_BUF, WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		/* GET_INT Register */
-		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		if (!(val & INT_REG_CED)) {
-			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-
-	for (i = start_page; i < end_page; i++) {
-		if (monitor_card_cd(chip, MS_CARD) == CD_NOT_EXIST)
-			TRACE_RET(chip, STATUS_FAIL);
-
-		ms_read_extra_data(chip, old_blk, i, extra, MS_EXTRA_SIZE);
-
-		retval =
-		    ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE,
-				       SystemParm, 6);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		/* Write REG */
-		ms_set_err_code(chip, MS_NO_ERROR);
-
-		if (CHK_MS4BIT(ms_card)) {
-			/* Parallel interface */
-			data[0] = 0x88;
-		} else {
-			/* Serial interface */
-			data[0] = 0x80;
-		}
-		/* Block Address */
-		data[1] = 0;
-		data[2] = (u8) (old_blk >> 8);
-		data[3] = (u8) old_blk;
-		data[4] = 0x20;
-		data[5] = i;
-
-		retval =
-		    ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT, data, 6);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		ms_set_err_code(chip, MS_NO_ERROR);
-		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		if (val & INT_REG_CMDNK) {
-			ms_set_err_code(chip, MS_CMD_NK);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		if (val & INT_REG_CED) {
-			if (val & INT_REG_ERR) {
-				retval = ms_read_status_reg(chip);
-				if (retval != STATUS_SUCCESS) {
-					uncorrect_flag = 1;
-					RTS51X_DEBUGP("Uncorrectable error\n");
-				} else {
-					uncorrect_flag = 0;
-				}
-
-				retval =
-				    ms_transfer_tpc(chip, MS_TM_NORMAL_READ,
-					READ_PAGE_DATA, 0, NO_WAIT_INT);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, retval);
-				if (uncorrect_flag) {
-					ms_set_page_status(log_blk, setPS_NG,
-						extra, MS_EXTRA_SIZE);
-					if (i == 0)
-						extra[0] &= 0xEF;
-					ms_write_extra_data(chip, old_blk, i,
-							    extra,
-							    MS_EXTRA_SIZE);
-					RTS51X_DEBUGP("page %d : extra[0] = 0x%x\n",
-							i, extra[0]);
-					MS_SET_BAD_BLOCK_FLG(ms_card);
-
-					ms_set_page_status(log_blk, setPS_Error,
-							extra, MS_EXTRA_SIZE);
-					ms_write_extra_data(chip, new_blk, i,
-						extra, MS_EXTRA_SIZE);
-					continue;
-				}
-
-				for (rty_cnt = 0; rty_cnt < MS_MAX_RETRY_COUNT;
-				     rty_cnt++) {
-					retval =
-					    ms_transfer_tpc(chip,
-							    MS_TM_NORMAL_WRITE,
-							    WRITE_PAGE_DATA, 0,
-							    NO_WAIT_INT);
-					if (retval == STATUS_SUCCESS)
-						break;
-				}
-				if (rty_cnt == MS_MAX_RETRY_COUNT)
-					TRACE_RET(chip, STATUS_FAIL);
-			}
-
-			if (!(val & INT_REG_BREQ)) {
-				ms_set_err_code(chip, MS_BREQ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-
-		retval = ms_set_rw_reg_addr(chip, OverwriteFlag,
-					    MS_EXTRA_SIZE, SystemParm,
-					    (6 + MS_EXTRA_SIZE));
-
-		/* Write REG */
-		ms_set_err_code(chip, MS_NO_ERROR);
-
-		if (CHK_MS4BIT(ms_card)) {
-			/* Parallel interface */
-			data[0] = 0x88;
-		} else {
-			/* Serial interface */
-			data[0] = 0x80;
-		}
-		/* Block Address */
-		data[1] = 0;
-		data[2] = (u8) (new_blk >> 8);
-		data[3] = (u8) new_blk;
-		data[4] = 0x20;
-		data[5] = i;
-
-		/* for MS check procedure */
-		if ((extra[0] & 0x60) != 0x60)
-			data[6] = extra[0];
-		else
-			data[6] = 0xF8;
-
-		data[6 + 1] = 0xFF;
-		data[6 + 2] = (u8) (log_blk >> 8);
-		data[6 + 3] = (u8) log_blk;
-
-		for (j = 4; j <= MS_EXTRA_SIZE; j++)
-			data[6 + j] = 0xFF;
-
-		retval =
-		    ms_write_bytes(chip, WRITE_REG, (6 + MS_EXTRA_SIZE),
-				   NO_WAIT_INT, data, 16);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		/* GET_INT Register */
-		ms_set_err_code(chip, MS_NO_ERROR);
-		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		if (val & INT_REG_CMDNK) {
-			ms_set_err_code(chip, MS_CMD_NK);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		if (val & INT_REG_CED) {
-			if (val & INT_REG_ERR) {
-				ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-
-		if (i == 0) {
-			retval =
-			    ms_set_rw_reg_addr(chip, OverwriteFlag,
-					       MS_EXTRA_SIZE, SystemParm, 7);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-
-			ms_set_err_code(chip, MS_NO_ERROR);
-
-			if (CHK_MS4BIT(ms_card)) {
-				/* Parallel interface */
-				data[0] = 0x88;
-			} else {
-				/* Serial interface */
-				data[0] = 0x80;
-			}
-			/* Block Address */
-			data[1] = 0;
-			data[2] = (u8) (old_blk >> 8);
-			data[3] = (u8) old_blk;
-			data[4] = 0x80;
-			data[5] = 0;
-			data[6] = 0xEF;
-			data[7] = 0xFF;
-
-			retval =
-			    ms_write_bytes(chip, WRITE_REG, 7, NO_WAIT_INT,
-					   data, 8);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-
-			retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-
-			ms_set_err_code(chip, MS_NO_ERROR);
-			retval =
-			    ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val,
-					  1);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-
-			if (val & INT_REG_CMDNK) {
-				ms_set_err_code(chip, MS_CMD_NK);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-
-			if (val & INT_REG_CED) {
-				if (val & INT_REG_ERR) {
-					ms_set_err_code(chip,
-							MS_FLASH_WRITE_ERROR);
-					TRACE_RET(chip, STATUS_FAIL);
-				}
-			}
-		}
-	}
-
-	return STATUS_SUCCESS;
-}
-
-#ifdef MS_SPEEDUP
-static int ms_auto_copy_page(struct rts51x_chip *chip, u16 old_blk, u16 new_blk,
-			     u16 log_blk, u8 start_page, u8 end_page)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	u8 page_len, bus_width, val = 0;
-	u8 extra[MS_EXTRA_SIZE];
-
-	RTS51X_DEBUGP("Auto copy page from 0x%x to 0x%x, logical block is 0x%x\n",
-				old_blk, new_blk, log_blk);
-	RTS51X_DEBUGP("start_page = %d, end_page = %d\n", start_page,
-		       end_page);
-
-	page_len = end_page - start_page;
-
-	retval = ms_read_extra_data(chip, new_blk, 0, extra, MS_EXTRA_SIZE);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = ms_read_status_reg(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_READ_REG(chip, PPBUF_BASE2, &val);
-
-	if (val & BUF_FULL) {
-		retval = ms_send_cmd(chip, CLEAR_BUF, WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		if (!(val & INT_REG_CED)) {
-			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-
-	if (CHK_MS4BIT(ms_card)) {
-		/* Parallel interface */
-		bus_width = 0x88;
-	} else {
-		/* Serial interface */
-		bus_width = 0x80;
-	}
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_OLD_BLOCK_0, 0xFF, (u8) old_blk);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_OLD_BLOCK_1, 0xFF,
-		       (u8) (old_blk >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_NEW_BLOCK_0, 0xFF, (u8) new_blk);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_NEW_BLOCK_1, 0xFF,
-		       (u8) (new_blk >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_LOG_BLOCK_0, 0xFF, (u8) log_blk);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_LOG_BLOCK_1, 0xFF,
-		       (u8) (log_blk >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_PAGE_START, 0xFF, start_page);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_PAGE_LENGTH, 0xFF, page_len);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_BUS_WIDTH, 0xFF, bus_width);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
-		       MS_TRANSFER_START | MS_TM_COPY_PAGE);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER, MS_TRANSFER_END,
-		       MS_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_ms_error(chip);
-		TRACE_RET(chip, retval);
-	}
-
-	retval = rts51x_get_rsp(chip, 1, 5000);
-
-	if (CHECK_MS_TRANS_FAIL(chip, retval)) {
-		rts51x_clear_ms_error(chip);
-		if (retval == STATUS_TIMEDOUT)
-			TRACE_RET(chip, retval);
-		TRACE_GOTO(chip, Fail);
-	}
-
-	return STATUS_SUCCESS;
-
-Fail:
-	retval = ms_erase_block(chip, new_blk);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval =
-	    ms_copy_page(chip, old_blk, new_blk, log_blk, start_page, end_page);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-#endif
-
-static int reset_ms(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	u16 i, reg_addr, block_size;
-	u8 val, j, *ptr;
-#ifndef SUPPORT_MAGIC_GATE
-	u16 eblock_cnt;
-#endif
-
-	retval = ms_prepare_reset(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	ms_card->ms_type |= TYPE_MS;
-
-	retval = ms_send_cmd(chip, MS_RESET, NO_WAIT_INT);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = ms_read_status_reg(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_READ_REG(chip, PPBUF_BASE2, &val);
-	if (val & WRT_PRTCT)
-		chip->card_wp |= MS_CARD;
-	else
-		chip->card_wp &= ~MS_CARD;
-
-	i = 0;
-
-RE_SEARCH:
-	/* Search For Boot Block */
-	while (i < (MAX_DEFECTIVE_BLOCK + 2)) {
-		if (monitor_card_cd(chip, MS_CARD) == CD_NOT_EXIST)
-			TRACE_RET(chip, STATUS_FAIL);
-
-		retval = ms_check_boot_block(chip, i);
-		if (retval != STATUS_SUCCESS) {
-			i++;
-			continue;
-		}
-
-		ms_card->boot_block = i;
-		break;
-	}
-
-	if (i == (MAX_DEFECTIVE_BLOCK + 2)) {
-		RTS51X_DEBUGP("No boot block found!");
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-	for (j = 0; j < 3; j++) {
-		retval = ms_read_page(chip, ms_card->boot_block, j);
-		if (retval != STATUS_SUCCESS) {
-			if (ms_check_err_code(chip, MS_FLASH_WRITE_ERROR)) {
-				i = ms_card->boot_block + 1;
-				ms_set_err_code(chip, MS_NO_ERROR);
-				goto RE_SEARCH;
-			}
-		}
-	}
-
-	/* Read boot block contents */
-	retval = ms_read_page(chip, ms_card->boot_block, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	/* Read MS system information as sys_info */
-	retval =
-	    rts51x_seq_read_register(chip, PPBUF_BASE2 + 0x1A0, 96,
-				     ms_card->raw_sys_info);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	/* Read useful block contents */
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, READ_REG_CMD, HEADER_ID0, 0, 0);
-	rts51x_add_cmd(chip, READ_REG_CMD, HEADER_ID1, 0, 0);
-
-	for (reg_addr = DISABLED_BLOCK0; reg_addr <= DISABLED_BLOCK3;
-	     reg_addr++) {
-		rts51x_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
-	}
-
-	for (reg_addr = BLOCK_SIZE_0; reg_addr <= PAGE_SIZE_1; reg_addr++)
-		rts51x_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
-
-	rts51x_add_cmd(chip, READ_REG_CMD, MS_Device_Type, 0, 0);
-	rts51x_add_cmd(chip, READ_REG_CMD, MS_4bit_Support, 0, 0);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 16, 100);
-
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	ptr = rts51x_get_rsp_data(chip);
-
-	RTS51X_DEBUGP("Boot block data:\n");
-	RTS51X_DUMP(ptr, 16);
-
-	if (ptr[0] != 0x00 || ptr[1] != 0x01) {
-		i = ms_card->boot_block + 1;
-		goto RE_SEARCH;
-	}
-	if (ptr[12] != 0x02 || ptr[13] != 0x00) {
-		i = ms_card->boot_block + 1;
-		goto RE_SEARCH;
-	}
-	if ((ptr[14] == 1) || (ptr[14] == 3))
-		chip->card_wp |= MS_CARD;
-	block_size = ((u16) ptr[6] << 8) | ptr[7];
-	if (block_size == 0x0010) {
-		ms_card->block_shift = 5;
-		ms_card->page_off = 0x1F;
-	} else if (block_size == 0x0008) {
-		ms_card->block_shift = 4;
-		ms_card->page_off = 0x0F;
-	}
-	ms_card->total_block = ((u16) ptr[8] << 8) | ptr[9];
-
-#ifdef SUPPORT_MAGIC_GATE
-	j = ptr[10];
-
-	if (ms_card->block_shift == 4) {
-		if (j < 2)
-			ms_card->capacity = 0x1EE0;
-		else
-			ms_card->capacity = 0x3DE0;
-	} else {
-		if (j < 5)
-			ms_card->capacity = 0x7BC0;
-		else if (j < 0xA)
-			ms_card->capacity = 0xF7C0;
-		else if (j < 0x11)
-			ms_card->capacity = 0x1EF80;
-		else
-			ms_card->capacity = 0x3DF00;
-	}
-#else
-	eblock_cnt = ((u16) ptr[10] << 8) | ptr[11];
-
-	ms_card->capacity = ((u32) eblock_cnt - 2) << ms_card->block_shift;
-#endif
-
-	chip->capacity[chip->card2lun[MS_CARD]] = ms_card->capacity;
-
-	if (ptr[15]) {
-		retval = ms_set_rw_reg_addr(chip, 0, 0, SystemParm, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
-		RTS51X_WRITE_REG(chip, PPBUF_BASE2, 0xFF, 0x88);
-		RTS51X_WRITE_REG(chip, PPBUF_BASE2 + 1, 0xFF, 0);
-
-		retval =
-		    ms_transfer_tpc(chip, MS_TM_WRITE_BYTES, WRITE_REG, 1,
-				    NO_WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
-		RTS51X_WRITE_REG(chip, MS_CFG, 0x58 | MS_NO_CHECK_INT,
-				 MS_BUS_WIDTH_4 | PUSH_TIME_ODD |
-				 MS_NO_CHECK_INT);
-
-		ms_card->ms_type |= MS_4BIT;
-	}
-
-	if (CHK_MS4BIT(ms_card))
-		chip->card_bus_width[chip->card2lun[MS_CARD]] = 4;
-	else
-		chip->card_bus_width[chip->card2lun[MS_CARD]] = 1;
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_init_l2p_tbl(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int size, i, seg_no, retval;
-	u16 defect_block, reg_addr;
-	u8 val1, val2;
-
-	ms_card->segment_cnt = ms_card->total_block >> 9;
-	RTS51X_DEBUGP("ms_card->segment_cnt = %d\n", ms_card->segment_cnt);
-
-	size = ms_card->segment_cnt * sizeof(struct zone_entry);
-	ms_card->segment = vmalloc(size);
-	if (ms_card->segment == NULL)
-		TRACE_RET(chip, STATUS_FAIL);
-	memset(ms_card->segment, 0, size);
-
-	retval = ms_read_page(chip, ms_card->boot_block, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_GOTO(chip, INIT_FAIL);
-
-	reg_addr = PPBUF_BASE2;
-	for (i = 0; i < (((ms_card->total_block >> 9) * 10) + 1); i++) {
-		retval = rts51x_read_register(chip, reg_addr++, &val1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, INIT_FAIL);
-		retval = rts51x_read_register(chip, reg_addr++, &val2);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, INIT_FAIL);
-
-		defect_block = ((u16) val1 << 8) | val2;
-		if (defect_block == 0xFFFF)
-			break;
-		seg_no = defect_block / 512;
-		ms_card->segment[seg_no].defect_list[ms_card->segment[seg_no].
-						     disable_count++] =
-		    defect_block;
-	}
-
-	for (i = 0; i < ms_card->segment_cnt; i++) {
-		ms_card->segment[i].build_flag = 0;
-		ms_card->segment[i].l2p_table = NULL;
-		ms_card->segment[i].free_table = NULL;
-		ms_card->segment[i].get_index = 0;
-		ms_card->segment[i].set_index = 0;
-		ms_card->segment[i].unused_blk_cnt = 0;
-
-		RTS51X_DEBUGP("defective block count of segment %d is %d\n",
-			       i, ms_card->segment[i].disable_count);
-	}
-
-	return STATUS_SUCCESS;
-
-INIT_FAIL:
-	if (ms_card->segment) {
-		vfree(ms_card->segment);
-		ms_card->segment = NULL;
-	}
-
-	return STATUS_FAIL;
-}
-
-static u16 ms_get_l2p_tbl(struct rts51x_chip *chip, int seg_no, u16 log_off)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	struct zone_entry *segment;
-
-	if (ms_card->segment == NULL)
-		return 0xFFFF;
-
-	segment = &(ms_card->segment[seg_no]);
-
-	if (segment->l2p_table)
-		return segment->l2p_table[log_off];
-
-	return 0xFFFF;
-}
-
-static void ms_set_l2p_tbl(struct rts51x_chip *chip, int seg_no, u16 log_off,
-			   u16 phy_blk)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	struct zone_entry *segment;
-
-	if (ms_card->segment == NULL)
-		return;
-
-	segment = &(ms_card->segment[seg_no]);
-	if (segment->l2p_table)
-		segment->l2p_table[log_off] = phy_blk;
-}
-
-static void ms_set_unused_block(struct rts51x_chip *chip, u16 phy_blk)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	struct zone_entry *segment;
-	int seg_no;
-
-	seg_no = (int)phy_blk >> 9;
-	segment = &(ms_card->segment[seg_no]);
-
-	segment->free_table[segment->set_index++] = phy_blk;
-	if (segment->set_index >= MS_FREE_TABLE_CNT)
-		segment->set_index = 0;
-	segment->unused_blk_cnt++;
-}
-
-static u16 ms_get_unused_block(struct rts51x_chip *chip, int seg_no)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	struct zone_entry *segment;
-	u16 phy_blk;
-
-	segment = &(ms_card->segment[seg_no]);
-
-	if (segment->unused_blk_cnt <= 0)
-		return 0xFFFF;
-
-	phy_blk = segment->free_table[segment->get_index];
-	segment->free_table[segment->get_index++] = 0xFFFF;
-	if (segment->get_index >= MS_FREE_TABLE_CNT)
-		segment->get_index = 0;
-	segment->unused_blk_cnt--;
-
-	return phy_blk;
-}
-
-static const unsigned short ms_start_idx[] = {
-	0, 494, 990, 1486, 1982, 2478, 2974, 3470,
-	3966, 4462, 4958, 5454, 5950, 6446, 6942, 7438, 7934
-};
-
-static int ms_arbitrate_l2p(struct rts51x_chip *chip, u16 phy_blk, u16 log_off,
-			    u8 us1, u8 us2)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	struct zone_entry *segment;
-	int seg_no;
-	u16 tmp_blk;
-
-	seg_no = (int)phy_blk >> 9;
-	segment = &(ms_card->segment[seg_no]);
-	tmp_blk = segment->l2p_table[log_off];
-
-	if (us1 != us2) {
-		if (us1 == 0) {
-			if (!(chip->card_wp & MS_CARD))
-				ms_erase_block(chip, tmp_blk);
-			ms_set_unused_block(chip, tmp_blk);
-			segment->l2p_table[log_off] = phy_blk;
-		} else {
-			if (!(chip->card_wp & MS_CARD))
-				ms_erase_block(chip, phy_blk);
-			ms_set_unused_block(chip, phy_blk);
-		}
-	} else {
-		if (phy_blk < tmp_blk) {
-			if (!(chip->card_wp & MS_CARD))
-				ms_erase_block(chip, phy_blk);
-			ms_set_unused_block(chip, phy_blk);
-		} else {
-			if (!(chip->card_wp & MS_CARD))
-				ms_erase_block(chip, tmp_blk);
-			ms_set_unused_block(chip, tmp_blk);
-			segment->l2p_table[log_off] = phy_blk;
-		}
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_build_l2p_tbl(struct rts51x_chip *chip, int seg_no)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	struct zone_entry *segment;
-	int retval, table_size, disable_cnt, defect_flag, i;
-	u16 start, end, phy_blk, log_blk, tmp_blk;
-	u8 extra[MS_EXTRA_SIZE], us1, us2;
-
-	RTS51X_DEBUGP("ms_build_l2p_tbl: %d\n", seg_no);
-
-	if (ms_card->segment == NULL) {
-		retval = ms_init_l2p_tbl(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	if (ms_card->segment[seg_no].build_flag) {
-		RTS51X_DEBUGP("l2p table of segment %d has been built\n",
-			       seg_no);
-		return STATUS_SUCCESS;
-	}
-
-	if (seg_no == 0)
-		table_size = 494;
-	else
-		table_size = 496;
-
-	segment = &(ms_card->segment[seg_no]);
-
-	if (segment->l2p_table == NULL) {
-		segment->l2p_table = vmalloc(table_size * 2);
-		if (segment->l2p_table == NULL)
-			TRACE_GOTO(chip, BUILD_FAIL);
-	}
-	memset((u8 *) (segment->l2p_table), 0xff, table_size * 2);
-
-	if (segment->free_table == NULL) {
-		segment->free_table = vmalloc(MS_FREE_TABLE_CNT * 2);
-		if (segment->free_table == NULL)
-			TRACE_GOTO(chip, BUILD_FAIL);
-	}
-	memset((u8 *) (segment->free_table), 0xff, MS_FREE_TABLE_CNT * 2);
-
-	start = (u16) seg_no << 9;
-	end = (u16) (seg_no + 1) << 9;
-
-	disable_cnt = segment->disable_count;
-
-	segment->get_index = segment->set_index = 0;
-	segment->unused_blk_cnt = 0;
-
-	for (phy_blk = start; phy_blk < end; phy_blk++) {
-		if (disable_cnt) {
-			defect_flag = 0;
-			for (i = 0; i < segment->disable_count; i++) {
-				if (phy_blk == segment->defect_list[i]) {
-					defect_flag = 1;
-					break;
-				}
-			}
-			if (defect_flag) {
-				disable_cnt--;
-				continue;
-			}
-		}
-
-		retval =
-		    ms_read_extra_data(chip, phy_blk, 0, extra, MS_EXTRA_SIZE);
-		if (retval != STATUS_SUCCESS) {
-			RTS51X_DEBUGP("read extra data fail\n");
-			ms_set_bad_block(chip, phy_blk);
-			continue;
-		}
-
-		if (seg_no == ms_card->segment_cnt - 1) {
-			if (!(extra[1] & NOT_TRANSLATION_TABLE)) {
-				if (!(chip->card_wp & MS_CARD)) {
-					retval = ms_erase_block(chip, phy_blk);
-					if (retval != STATUS_SUCCESS)
-						continue;
-					extra[2] = 0xff;
-					extra[3] = 0xff;
-				}
-			}
-		}
-
-		if (!(extra[0] & BLOCK_OK))
-			continue;
-		if (!(extra[1] & NOT_BOOT_BLOCK))
-			continue;
-		if ((extra[0] & PAGE_OK) != PAGE_OK)
-			continue;
-
-		log_blk = ((u16) extra[2] << 8) | extra[3];
-
-		if (log_blk == 0xFFFF) {
-			if (!(chip->card_wp & MS_CARD)) {
-				retval = ms_erase_block(chip, phy_blk);
-				if (retval != STATUS_SUCCESS)
-					continue;
-			}
-			ms_set_unused_block(chip, phy_blk);
-			continue;
-		}
-
-		if ((log_blk < ms_start_idx[seg_no]) ||
-		    (log_blk >= ms_start_idx[seg_no + 1])) {
-			if (!(chip->card_wp & MS_CARD)) {
-				retval = ms_erase_block(chip, phy_blk);
-				if (retval != STATUS_SUCCESS)
-					continue;
-			}
-			ms_set_unused_block(chip, phy_blk);
-			continue;
-		}
-
-		if (segment->l2p_table[log_blk - ms_start_idx[seg_no]] ==
-		    0xFFFF) {
-			segment->l2p_table[log_blk - ms_start_idx[seg_no]] =
-			    phy_blk;
-			continue;
-		}
-
-		us1 = extra[0] & 0x10;
-		tmp_blk = segment->l2p_table[log_blk - ms_start_idx[seg_no]];
-		retval =
-		    ms_read_extra_data(chip, tmp_blk, 0, extra, MS_EXTRA_SIZE);
-		if (retval != STATUS_SUCCESS)
-			continue;
-		us2 = extra[0] & 0x10;
-
-		(void)ms_arbitrate_l2p(chip, phy_blk,
-				       log_blk - ms_start_idx[seg_no], us1,
-				       us2);
-		continue;
-	}
-
-	segment->build_flag = 1;
-
-	RTS51X_DEBUGP("unused block count: %d\n", segment->unused_blk_cnt);
-
-	if (seg_no == ms_card->segment_cnt - 1) {
-		if (segment->unused_blk_cnt < 2)
-			chip->card_wp |= MS_CARD;
-	} else {
-		if (segment->unused_blk_cnt < 1)
-			chip->card_wp |= MS_CARD;
-	}
-
-	if (chip->card_wp & MS_CARD)
-		return STATUS_SUCCESS;
-
-	for (log_blk = ms_start_idx[seg_no]; log_blk < ms_start_idx[seg_no + 1];
-	     log_blk++) {
-		if (segment->l2p_table[log_blk - ms_start_idx[seg_no]] ==
-		    0xFFFF) {
-			phy_blk = ms_get_unused_block(chip, seg_no);
-			if (phy_blk == 0xFFFF) {
-				chip->card_wp |= MS_CARD;
-				return STATUS_SUCCESS;
-			}
-			retval = ms_init_page(chip, phy_blk, log_blk, 0, 1);
-			if (retval != STATUS_SUCCESS)
-				TRACE_GOTO(chip, BUILD_FAIL);
-			segment->l2p_table[log_blk - ms_start_idx[seg_no]] =
-			    phy_blk;
-			if (seg_no == ms_card->segment_cnt - 1) {
-				if (segment->unused_blk_cnt < 2) {
-					chip->card_wp |= MS_CARD;
-					return STATUS_SUCCESS;
-				}
-			} else {
-				if (segment->unused_blk_cnt < 1) {
-					chip->card_wp |= MS_CARD;
-					return STATUS_SUCCESS;
-				}
-			}
-		}
-	}
-
-	if (seg_no == 0) {
-		for (log_blk = 0; log_blk < 494; log_blk++) {
-			tmp_blk = segment->l2p_table[log_blk];
-			if (tmp_blk < ms_card->boot_block) {
-				RTS51X_DEBUGP("Boot block is not the first normal block.\n");
-
-				if (chip->card_wp & MS_CARD)
-					break;
-
-				phy_blk = ms_get_unused_block(chip, 0);
-#ifdef MS_SPEEDUP
-				retval =
-				    ms_auto_copy_page(chip, tmp_blk, phy_blk,
-						      log_blk, 0,
-						      ms_card->page_off + 1);
-#else
-				retval = ms_copy_page(chip, tmp_blk, phy_blk,
-						      log_blk, 0,
-						      ms_card->page_off + 1);
-#endif
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, retval);
-
-				segment->l2p_table[log_blk] = phy_blk;
-
-				retval = ms_set_bad_block(chip, tmp_blk);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, retval);
-			}
-		}
-	}
-
-	return STATUS_SUCCESS;
-
-BUILD_FAIL:
-	segment->build_flag = 0;
-	if (segment->l2p_table) {
-		vfree(segment->l2p_table);
-		segment->l2p_table = NULL;
-	}
-	if (segment->free_table) {
-		vfree(segment->free_table);
-		segment->free_table = NULL;
-	}
-
-	return STATUS_FAIL;
-}
-
-int rts51x_reset_ms_card(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-
-	memset(ms_card, 0, sizeof(struct ms_info));
-
-	rts51x_enable_card_clock(chip, MS_CARD);
-
-	retval = rts51x_select_card(chip, MS_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	ms_card->ms_type = 0;
-	ms_card->last_rw_int = 0;
-
-	retval = reset_ms_pro(chip);
-	if (retval != STATUS_SUCCESS) {
-		if (ms_card->check_ms_flow) {
-			retval = reset_ms(chip);
-			if (retval != STATUS_SUCCESS) {
-				if (chip->option.reset_or_rw_fail_set_pad_drive) {
-					rts51x_write_register(chip,
-						CARD_DRIVE_SEL, SD20_DRIVE_MASK,
-						DRIVE_8mA);
-				}
-				TRACE_RET(chip, retval);
-			}
-		} else {
-			if (chip->option.reset_or_rw_fail_set_pad_drive) {
-				rts51x_write_register(chip, CARD_DRIVE_SEL,
-						      SD20_DRIVE_MASK,
-						      DRIVE_8mA);
-			}
-			TRACE_RET(chip, retval);
-		}
-	}
-
-	retval = ms_set_init_para(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (!CHK_MSPRO(ms_card)) {
-		retval = ms_build_l2p_tbl(chip, ms_card->total_block / 512 - 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	RTS51X_DEBUGP("ms_card->ms_type = 0x%x\n", ms_card->ms_type);
-
-	return STATUS_SUCCESS;
-}
-
-static int mspro_set_rw_cmd(struct rts51x_chip *chip, u32 start_sec,
-			    u16 sec_cnt, u8 cmd)
-{
-	int retval, i;
-	u8 data[8];
-
-	data[0] = cmd;
-	data[1] = (u8) (sec_cnt >> 8);
-	data[2] = (u8) sec_cnt;
-	data[3] = (u8) (start_sec >> 24);
-	data[4] = (u8) (start_sec >> 16);
-	data[5] = (u8) (start_sec >> 8);
-	data[6] = (u8) start_sec;
-	data[7] = 0;
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_write_bytes(chip, PRO_EX_SET_CMD, 7, WAIT_INT, data, 8);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-static void mspro_stop_seq_mode(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-
-	if (ms_card->seq_mode) {
-		retval = ms_switch_clock(chip);
-		if (retval != STATUS_SUCCESS)
-			return;
-
-		ms_card->seq_mode = 0;
-		ms_card->total_sec_cnt = 0;
-		ms_card->last_rw_int = 0;
-		ms_send_cmd(chip, PRO_STOP, WAIT_INT);
-
-		rts51x_ep0_write_register(chip, MC_FIFO_CTL, FIFO_FLUSH,
-					  FIFO_FLUSH);
-	}
-}
-
-static inline int ms_auto_tune_clock(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-
-	if (chip->asic_code) {
-		if (ms_card->ms_clock > 30)
-			ms_card->ms_clock -= 20;
-	} else {
-		if (ms_card->ms_clock == CLK_80)
-			ms_card->ms_clock = CLK_60;
-		else if (ms_card->ms_clock == CLK_60)
-			ms_card->ms_clock = CLK_40;
-	}
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static int mspro_rw_multi_sector(struct scsi_cmnd *srb,
-				 struct rts51x_chip *chip, u32 start_sector,
-				 u16 sector_cnt)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, mode_2k = 0;
-	u16 count;
-	u8 val, trans_mode, rw_tpc, rw_cmd;
-
-	ms_set_err_code(chip, MS_NO_ERROR);
-
-	ms_card->counter = 0;
-
-	if (CHK_MSHG(ms_card)) {
-		if ((start_sector % 4) || (sector_cnt % 4)) {
-			if (srb->sc_data_direction == DMA_FROM_DEVICE) {
-				rw_tpc = PRO_READ_LONG_DATA;
-				rw_cmd = PRO_READ_DATA;
-			} else {
-				rw_tpc = PRO_WRITE_LONG_DATA;
-				rw_cmd = PRO_WRITE_DATA;
-			}
-		} else {
-			if (srb->sc_data_direction == DMA_FROM_DEVICE) {
-				rw_tpc = PRO_READ_QUAD_DATA;
-				rw_cmd = PRO_READ_2K_DATA;
-			} else {
-				rw_tpc = PRO_WRITE_QUAD_DATA;
-				rw_cmd = PRO_WRITE_2K_DATA;
-			}
-			mode_2k = 1;
-		}
-	} else {
-		if (srb->sc_data_direction == DMA_FROM_DEVICE) {
-			rw_tpc = PRO_READ_LONG_DATA;
-			rw_cmd = PRO_READ_DATA;
-		} else {
-			rw_tpc = PRO_WRITE_LONG_DATA;
-			rw_cmd = PRO_WRITE_DATA;
-		}
-	}
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (srb->sc_data_direction == DMA_FROM_DEVICE)
-		trans_mode = MS_TM_AUTO_READ;
-	else
-		trans_mode = MS_TM_AUTO_WRITE;
-
-	val = ms_card->last_rw_int;
-
-	if (ms_card->seq_mode) {
-		if ((ms_card->pre_dir != srb->sc_data_direction)
-		    || ((ms_card->pre_sec_addr + ms_card->pre_sec_cnt) !=
-			start_sector)
-		    || (mode_2k && (ms_card->seq_mode & MODE_512_SEQ))
-		    || (!mode_2k && (ms_card->seq_mode & MODE_2K_SEQ))
-		    || !(val & MS_INT_BREQ)
-		    || ((ms_card->total_sec_cnt + sector_cnt) > 0xFE00)) {
-			ms_card->seq_mode = 0;
-			ms_card->total_sec_cnt = 0;
-			ms_card->last_rw_int = 0;
-			if (val & MS_INT_BREQ) {
-				retval = ms_send_cmd(chip, PRO_STOP, WAIT_INT);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, retval);
-
-				rts51x_ep0_write_register(chip, MC_FIFO_CTL,
-					FIFO_FLUSH, FIFO_FLUSH);
-			}
-		}
-	}
-
-	if (!ms_card->seq_mode) {
-		ms_card->total_sec_cnt = 0;
-		if (sector_cnt >= 0x80) {
-			if ((ms_card->capacity - start_sector) > 0xFE00)
-				count = 0xFE00;
-			else
-				count =
-				    (u16) (ms_card->capacity - start_sector);
-			if (count > sector_cnt) {
-				if (mode_2k)
-					ms_card->seq_mode |= MODE_2K_SEQ;
-				else
-					ms_card->seq_mode |= MODE_512_SEQ;
-			}
-		} else {
-			count = sector_cnt;
-		}
-		retval = mspro_set_rw_cmd(chip, start_sector, count, rw_cmd);
-		if (retval != STATUS_SUCCESS) {
-			ms_card->seq_mode = 0;
-			TRACE_RET(chip, retval);
-		}
-	}
-
-	retval =
-	    ms_transfer_data(chip, trans_mode, rw_tpc, sector_cnt, WAIT_INT,
-			     mode_2k, scsi_sg_count(srb), scsi_sglist(srb),
-			     scsi_bufflen(srb));
-	if (retval != STATUS_SUCCESS) {
-		ms_card->seq_mode = 0;
-		rts51x_ep0_read_register(chip, MS_TRANS_CFG, &val);
-		rts51x_clear_ms_error(chip);
-		if (val & MS_INT_BREQ)
-			ms_send_cmd(chip, PRO_STOP, WAIT_INT);
-		if (val & (MS_CRC16_ERR | MS_RDY_TIMEOUT)) {
-			RTS51X_DEBUGP("MSPro CRC error, tune clock!\n");
-			ms_auto_tune_clock(chip);
-		}
-
-		TRACE_RET(chip, retval);
-	}
-
-	ms_card->pre_sec_addr = start_sector;
-	ms_card->pre_sec_cnt = sector_cnt;
-	ms_card->pre_dir = srb->sc_data_direction;
-	ms_card->total_sec_cnt += sector_cnt;
-
-	return STATUS_SUCCESS;
-}
-
-static int mspro_read_format_progress(struct rts51x_chip *chip,
-				      const int short_data_len)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-	u32 total_progress, cur_progress;
-	u8 cnt, tmp;
-	u8 data[8];
-
-	ms_card->format_status = FORMAT_FAIL;
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_READ_REG(chip, MS_TRANS_CFG, &tmp);
-
-	if ((tmp & (MS_INT_CED | MS_INT_CMDNK | MS_INT_ERR)) == MS_INT_CED) {
-		ms_card->format_status = FORMAT_SUCCESS;
-		ms_card->pro_under_formatting = 0;
-		return STATUS_SUCCESS;
-	}
-	if (!
-	    ((tmp & (MS_INT_BREQ | MS_INT_CED | MS_INT_CMDNK | MS_INT_ERR)) ==
-	     MS_INT_BREQ)) {
-		ms_card->pro_under_formatting = 0;
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (short_data_len >= 256)
-		cnt = 0;
-	else
-		cnt = (u8) short_data_len;
-
-	retval =
-	    ms_read_bytes(chip, PRO_READ_SHORT_DATA, cnt, WAIT_INT, data, 8);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	total_progress =
-	    (data[0] << 24) | (data[1] << 16) | (data[2] << 8) | data[3];
-	cur_progress =
-	    (data[4] << 24) | (data[5] << 16) | (data[6] << 8) | data[7];
-
-	RTS51X_DEBUGP("total_progress = %d, cur_progress = %d\n",
-		       total_progress, cur_progress);
-
-	if (total_progress == 0) {
-		ms_card->progress = 0;
-	} else {
-		u64 ulltmp = (u64) cur_progress * (u64) 65535;
-		do_div(ulltmp, total_progress);
-		ms_card->progress = (u16) ulltmp;
-	}
-	RTS51X_DEBUGP("progress = %d\n", ms_card->progress);
-
-	for (i = 0; i < 2500; i++) {
-		RTS51X_READ_REG(chip, MS_TRANS_CFG, &tmp);
-		if (tmp &
-		    (MS_INT_CED | MS_INT_CMDNK | MS_INT_BREQ | MS_INT_ERR))
-			break;
-
-		wait_timeout(1);
-	}
-
-	if (i == 2500)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	RTS51X_DEBUGP("MSPro format tmp:%d\n", tmp);
-
-	if (tmp & (MS_INT_CMDNK | MS_INT_ERR))
-		TRACE_RET(chip, STATUS_FAIL);
-	if (tmp & MS_INT_CED) {
-		ms_card->format_status = FORMAT_SUCCESS;
-		ms_card->pro_under_formatting = 0;
-	} else if (tmp & MS_INT_BREQ) {
-		ms_card->format_status = FORMAT_IN_PROGRESS;
-	} else {
-		ms_card->format_status = FORMAT_FAIL;
-		ms_card->pro_under_formatting = 0;
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	RTS51X_DEBUGP("MSPro format format_status:%d\n",
-		       ms_card->format_status);
-
-	return STATUS_SUCCESS;
-}
-
-void rts51x_mspro_polling_format_status(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int i;
-
-	if (ms_card->pro_under_formatting) {
-		for (i = 0; i < 65535; i++) {
-			mspro_read_format_progress(chip, MS_SHORT_DATA_LEN);
-			if (ms_card->format_status != FORMAT_IN_PROGRESS)
-				break;
-		}
-	}
-
-	return;
-}
-
-void rts51x_mspro_format_sense(struct rts51x_chip *chip, unsigned int lun)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-
-	if (CHK_FORMAT_STATUS(ms_card, FORMAT_SUCCESS)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_NO_SENSE);
-		ms_card->pro_under_formatting = 0;
-		ms_card->progress = 0;
-	} else if (CHK_FORMAT_STATUS(ms_card, FORMAT_IN_PROGRESS)) {
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x02, 0, 0x04, 0x04,
-			       0, (u16) (ms_card->progress));
-	} else {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_FORMAT_CMD_FAILED);
-		ms_card->pro_under_formatting = 0;
-		ms_card->progress = 0;
-	}
-}
-
-int rts51x_mspro_format(struct scsi_cmnd *srb, struct rts51x_chip *chip,
-		 int short_data_len, int quick_format)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-	u8 buf[8], tmp;
-	u16 para;
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = ms_set_rw_reg_addr(chip, 0x00, 0x00, Pro_TPCParm, 0x01);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	memset(buf, 0, 2);
-	switch (short_data_len) {
-	case 32:
-		buf[0] = 0;
-		break;
-	case 64:
-		buf[0] = 1;
-		break;
-	case 128:
-		buf[0] = 2;
-		break;
-	case 256:
-	default:
-		buf[0] = 3;
-		break;
-	}
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_write_bytes(chip, PRO_WRITE_REG, 1, NO_WAIT_INT, buf, 2);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
-	/* Format command */
-	if (quick_format)
-		para = 0x0000;
-	else
-		para = 0x0001;
-	retval = mspro_set_rw_cmd(chip, 0, para, PRO_FORMAT);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	/* Check INT */
-	RTS51X_READ_REG(chip, MS_TRANS_CFG, &tmp);
-	if (tmp & (MS_INT_CMDNK | MS_INT_ERR))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	if ((tmp & (MS_INT_BREQ | MS_INT_CED)) == MS_INT_BREQ) {
-		ms_card->pro_under_formatting = 1;
-		ms_card->progress = 0;
-		ms_card->format_status = FORMAT_IN_PROGRESS;
-		return STATUS_SUCCESS;
-	}
-
-	if (tmp & MS_INT_CED) {
-		ms_card->pro_under_formatting = 0;
-		ms_card->progress = 0;
-		ms_card->format_status = FORMAT_SUCCESS;
-		rts51x_set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_NO_SENSE);
-		return STATUS_SUCCESS;
-	}
-
-	TRACE_RET(chip, STATUS_FAIL);
-}
-
-#ifdef MS_SPEEDUP
-static int ms_read_multiple_pages(struct rts51x_chip *chip, u16 phy_blk,
-				  u16 log_blk, u8 start_page, u8 end_page,
-				  u8 *buf, void **ptr, unsigned int *offset)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	int send_blkend;
-	u8 extra[MS_EXTRA_SIZE], val1, val2, data[6];
-	u8 page_cnt = end_page - start_page, page_addr, sec_cnt;
-
-	if (end_page != (ms_card->page_off + 1))
-		send_blkend = 1;
-	else
-		send_blkend = 0;
-
-	retval =
-	    ms_read_extra_data(chip, phy_blk, start_page, extra, MS_EXTRA_SIZE);
-	if (retval == STATUS_SUCCESS) {
-		if ((extra[1] & 0x30) != 0x30) {
-			ms_set_err_code(chip, MS_FLASH_READ_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-
-	if (CHK_MS4BIT(ms_card)) {
-		/* Parallel interface */
-		data[0] = 0x88;
-	} else {
-		/* Serial interface */
-		data[0] = 0x80;
-	}
-	/* Block Address */
-	data[1] = 0;
-	data[2] = (u8) (phy_blk >> 8);
-	data[3] = (u8) phy_blk;
-	/* Page Number
-	 * Extra data access mode */
-	data[4] = 0;
-	data[5] = start_page;
-
-	retval =
-	    ms_auto_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 6,
-			    BLOCK_READ, WAIT_INT, data, 6, &val1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	rts51x_init_cmd(chip);
-
-	if (send_blkend)
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_BLKEND, SET_BLKEND,
-			       SET_BLKEND);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_BLKEND, SET_BLKEND, 0);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, WAIT_INT,
-		       NO_WAIT_INT);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_SECTOR_CNT_L, 0xFF,
-		       (u8) page_cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_SECTOR_CNT_H, 0xFF, 0);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, READ_PAGE_DATA);
-
-	rts51x_trans_dma_enable(DMA_FROM_DEVICE, chip, 512 * page_cnt, DMA_512);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
-		       MS_TRANSFER_START | MS_TM_MULTI_READ);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER, MS_TRANSFER_END,
-		       MS_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, MODE_CDIR | STAGE_MS_STATUS, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval =
-	    rts51x_transfer_data_partial(chip, RCV_BULK_PIPE(chip), (void *)buf,
-					 ptr, offset, 512 * page_cnt,
-					 scsi_sg_count(chip->srb), NULL, 2000);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_ms_error(chip);
-		if (retval == STATUS_TIMEDOUT)
-			TRACE_RET(chip, retval);
-		TRACE_GOTO(chip, Fail);
-	}
-	retval = rts51x_get_rsp(chip, 3, 200);
-	if (CHECK_MS_TRANS_FAIL(chip, retval)) {
-		rts51x_clear_ms_error(chip);
-		if (retval == STATUS_TIMEDOUT)
-			TRACE_RET(chip, retval);
-		TRACE_GOTO(chip, Fail);
-	}
-
-	return STATUS_SUCCESS;
-
-Fail:
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, READ_REG_CMD, MS_SECTOR_CNT_L, 0, 0);
-
-	retval = rts51x_send_cmd(chip, MODE_CR | STAGE_MS_STATUS, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 3, 200);
-
-	if (CHECK_MS_TRANS_FAIL(chip, retval))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	sec_cnt = chip->rsp_buf[0];
-	RTS51X_DEBUGP("%d pages need be transferred, %d pages remained\n",
-		       (int)page_cnt, (int)sec_cnt);
-	page_addr = start_page + (page_cnt - sec_cnt);
-
-	if (CHK_MS4BIT(ms_card)) {
-		val1 = chip->rsp_buf[1];
-		RTS51X_DEBUGP("MS_TRANS_CFG: 0x%x\n", val1);
-	} else {
-		val1 = 0;
-	}
-
-	val2 = chip->rsp_buf[2];
-	RTS51X_DEBUGP("GET_INT: 0x%x\n", val2);
-
-	if ((val1 & INT_CMDNK) || (val2 & INT_REG_CMDNK)) {
-		ms_set_err_code(chip, MS_CMD_NK);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if ((val1 & INT_ERR) || (val2 & INT_REG_ERR)) {
-		if ((val1 & INT_BREQ) || (val2 & INT_REG_BREQ)) {
-			retval = ms_read_status_reg(chip);
-			if (retval != STATUS_SUCCESS) {
-				if (!(chip->card_wp & MS_CARD)) {
-					reset_ms(chip);
-					ms_set_page_status(log_blk, setPS_NG,
-						extra, MS_EXTRA_SIZE);
-					ms_write_extra_data(chip, phy_blk,
-						page_addr, extra,
-						MS_EXTRA_SIZE);
-				}
-				ms_set_err_code(chip, MS_FLASH_READ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		} else {
-			ms_set_err_code(chip, MS_FLASH_READ_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	} else {
-		if (CHK_MS4BIT(ms_card)) {
-			if (!(val1 & INT_BREQ) && !(val2 & INT_REG_BREQ)) {
-				ms_set_err_code(chip, MS_BREQ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		} else {
-			if (!(val2 & INT_REG_BREQ)) {
-				ms_set_err_code(chip, MS_BREQ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-	}
-
-	TRACE_RET(chip, STATUS_FAIL);
-}
-
-static int ms_write_multiple_pages(struct rts51x_chip *chip, u16 old_blk,
-				   u16 new_blk, u16 log_blk, u8 start_page,
-				   u8 end_page, u8 *buf, void **ptr,
-				   unsigned int *offset)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-	int send_blkend;
-	u8 val, data[16];
-	u8 page_cnt = end_page - start_page;
-
-	if ((end_page == (ms_card->page_off + 1)) || (page_cnt == 1))
-		send_blkend = 0;
-	else
-		send_blkend = 1;
-
-	if (!start_page) {
-		if (CHK_MS4BIT(ms_card)) {
-			/* Parallel interface */
-			data[0] = 0x88;
-		} else {
-			/* Serial interface */
-			data[0] = 0x80;
-		}
-		/* Block Address */
-		data[1] = 0;
-		data[2] = (u8) (old_blk >> 8);
-		data[3] = (u8) old_blk;
-		data[4] = 0x80;
-		data[5] = 0;
-		data[6] = 0xEF;
-		data[7] = 0xFF;
-
-		retval =
-		    ms_auto_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE,
-				    SystemParm, 7, BLOCK_WRITE, WAIT_INT, data,
-				    7, &val);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	retval =
-	    ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm,
-			       (6 + MS_EXTRA_SIZE));
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	ms_set_err_code(chip, MS_NO_ERROR);
-
-	if (CHK_MS4BIT(ms_card)) {
-		/* Parallel interface */
-		data[0] = 0x88;
-	} else {
-		/* Serial interface */
-		data[0] = 0x80;
-	}
-	/* Block Address */
-	data[1] = 0;
-	data[2] = (u8) (new_blk >> 8);
-	data[3] = (u8) new_blk;
-	/* Page Number
-	 * Extra data access mode */
-	if (page_cnt == 1) {
-		/* Single page access mode */
-		data[4] = 0x20;
-	} else {
-		/* Block access mode */
-		data[4] = 0;
-	}
-	data[5] = start_page;
-	data[6] = 0xF8;
-	data[7] = 0xFF;
-	data[8] = (u8) (log_blk >> 8);
-	data[9] = (u8) log_blk;
-
-	for (i = 0x0A; i < 0x10; i++) {
-		/* ECC */
-		data[i] = 0xFF;
-	}
-
-	retval =
-	    ms_auto_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm,
-			    (6 + MS_EXTRA_SIZE), BLOCK_WRITE, WAIT_INT, data,
-			    16, &val);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	rts51x_init_cmd(chip);
-
-	if (send_blkend)
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_BLKEND, SET_BLKEND,
-			       SET_BLKEND);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_BLKEND, SET_BLKEND, 0);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, WAIT_INT,
-		       NO_WAIT_INT);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_SECTOR_CNT_L, 0xFF,
-		       (u8) page_cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_SECTOR_CNT_H, 0xFF, 0);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, WRITE_PAGE_DATA);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       RING_BUFFER);
-
-	rts51x_trans_dma_enable(DMA_TO_DEVICE, chip, 512 * page_cnt, DMA_512);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
-		       MS_TRANSFER_START | MS_TM_MULTI_WRITE);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER, MS_TRANSFER_END,
-		       MS_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, MODE_CDOR | STAGE_MS_STATUS, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval =
-	    rts51x_transfer_data_partial(chip, SND_BULK_PIPE(chip), (void *)buf,
-					 ptr, offset, 512 * page_cnt,
-					 scsi_sg_count(chip->srb), NULL, 2000);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_ms_error(chip);
-		TRACE_RET(chip, retval);
-	}
-
-	retval = rts51x_get_rsp(chip, 3, 2000);
-
-
-	if (CHECK_MS_TRANS_FAIL(chip, retval)) {
-		rts51x_clear_ms_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-#else
-
-static int ms_read_multiple_pages(struct rts51x_chip *chip, u16 phy_blk,
-				  u16 log_blk, u8 start_page, u8 end_page,
-				  u8 *buf, void **ptr, unsigned int *offset)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-	u8 extra[MS_EXTRA_SIZE], page_addr, val, trans_cfg, data[6];
-
-	retval =
-	    ms_read_extra_data(chip, phy_blk, start_page, extra, MS_EXTRA_SIZE);
-	if (retval == STATUS_SUCCESS) {
-		if ((extra[1] & 0x30) != 0x30) {
-			ms_set_err_code(chip, MS_FLASH_READ_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-
-	retval =
-	    ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm,
-			       6);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	/* Write REG */
-	if (CHK_MS4BIT(ms_card)) {
-		/* Parallel interface */
-		data[0] = 0x88;
-	} else {
-		/* Serial interface */
-		data[0] = 0x80;
-	}
-	/* Block Address */
-	data[1] = 0;
-	data[2] = (u8) (phy_blk >> 8);
-	data[3] = (u8) phy_blk;
-	/* Page Number
-	 * Extra data access mode */
-	data[4] = 0;
-	data[5] = start_page;
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT, data, 6);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	ms_set_err_code(chip, MS_NO_ERROR);
-
-	retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	for (page_addr = start_page; page_addr < end_page; page_addr++) {
-		ms_set_err_code(chip, MS_NO_ERROR);
-
-		if (monitor_card_cd(chip, MS_CARD) == CD_NOT_EXIST) {
-			ms_set_err_code(chip, MS_NO_CARD);
-			chip->card_exist &= ~MS_CARD;
-			chip->card_ready &= ~MS_CARD;
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-		/* GET_INT Register */
-		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		if (val & INT_REG_CMDNK) {
-			ms_set_err_code(chip, MS_CMD_NK);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-		if (val & INT_REG_ERR) {
-			if (val & INT_REG_BREQ) {
-				retval = ms_read_status_reg(chip);
-				if (retval != STATUS_SUCCESS) {
-					if (!(chip->card_wp & MS_CARD)) {
-						reset_ms(chip);
-						ms_set_page_status(log_blk,
-							setPS_NG, extra,
-							MS_EXTRA_SIZE);
-						ms_write_extra_data(chip,
-							phy_blk, page_addr,
-							extra, MS_EXTRA_SIZE);
-					}
-					ms_set_err_code(chip,
-							MS_FLASH_READ_ERROR);
-					TRACE_RET(chip, STATUS_FAIL);
-				}
-			} else {
-				ms_set_err_code(chip, MS_FLASH_READ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		} else {
-			if (!(val & INT_REG_BREQ)) {
-				ms_set_err_code(chip, MS_BREQ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-
-		if (page_addr == (end_page - 1)) {
-			if (!(val & INT_REG_CED)) {
-				retval = ms_send_cmd(chip, BLOCK_END, WAIT_INT);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, retval);
-			}
-			retval =
-			    ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val,
-					  1);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-			if (!(val & INT_REG_CED)) {
-				ms_set_err_code(chip, MS_FLASH_READ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-
-			trans_cfg = NO_WAIT_INT;
-		} else {
-			trans_cfg = WAIT_INT;
-		}
-
-		rts51x_init_cmd(chip);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF,
-			       READ_PAGE_DATA);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF,
-			       trans_cfg);
-
-		rts51x_trans_dma_enable(DMA_FROM_DEVICE, chip, 512, DMA_512);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
-			       MS_TRANSFER_START | MS_TM_NORMAL_READ);
-		rts51x_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
-			       MS_TRANSFER_END, MS_TRANSFER_END);
-
-		retval = rts51x_send_cmd(chip, MODE_CDIR, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval =
-		    rts51x_transfer_data_partial(chip, RCV_BULK_PIPE(chip),
-						 (void *)buf, ptr, offset, 512,
-						 scsi_sg_count(chip->srb), NULL,
-						 2000);
-		if (retval != STATUS_SUCCESS) {
-			if (retval == STATUS_TIMEDOUT) {
-				ms_set_err_code(chip, MS_TO_ERROR);
-				rts51x_clear_ms_error(chip);
-				TRACE_RET(chip, retval);
-			}
-
-			retval =
-			    rts51x_ep0_read_register(chip, MS_TRANS_CFG, &val);
-			if (retval != STATUS_SUCCESS) {
-				ms_set_err_code(chip, MS_TO_ERROR);
-				rts51x_clear_ms_error(chip);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-			if (val & (MS_CRC16_ERR | MS_RDY_TIMEOUT)) {
-				ms_set_err_code(chip, MS_CRC16_ERROR);
-				rts51x_clear_ms_error(chip);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-
-		retval = rts51x_get_rsp(chip, 1, 2000);
-		if (CHECK_MS_TRANS_FAIL(chip, retval)) {
-			if (retval == STATUS_TIMEDOUT) {
-				ms_set_err_code(chip, MS_TO_ERROR);
-				rts51x_clear_ms_error(chip);
-				TRACE_RET(chip, retval);
-			}
-
-			retval =
-			    rts51x_ep0_read_register(chip, MS_TRANS_CFG, &val);
-			if (retval != STATUS_SUCCESS) {
-				ms_set_err_code(chip, MS_TO_ERROR);
-				rts51x_clear_ms_error(chip);
-				TRACE_RET(chip, retval);
-			}
-			if (val & (MS_CRC16_ERR | MS_RDY_TIMEOUT)) {
-				ms_set_err_code(chip, MS_CRC16_ERROR);
-				rts51x_clear_ms_error(chip);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_write_multiple_pages(struct rts51x_chip *chip, u16 old_blk,
-				   u16 new_blk, u16 log_blk, u8 start_page,
-				   u8 end_page, u8 *buf, void **ptr,
-				   unsigned int *offset)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-	u8 page_addr, val, data[16];
-
-	if (!start_page) {
-		retval =
-		    ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE,
-				       SystemParm, 7);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		if (CHK_MS4BIT(ms_card)) {
-			/* Parallel interface */
-			data[0] = 0x88;
-		} else {
-			/* Serial interface */
-			data[0] = 0x80;
-		}
-		/* Block Address */
-		data[1] = 0;
-		data[2] = (u8) (old_blk >> 8);
-		data[3] = (u8) old_blk;
-		data[4] = 0x80;
-		data[5] = 0;
-		data[6] = 0xEF;
-		data[7] = 0xFF;
-
-		retval =
-		    ms_write_bytes(chip, WRITE_REG, 7, NO_WAIT_INT, data, 8);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		/* GET_INT Register */
-		ms_set_err_code(chip, MS_NO_ERROR);
-		retval =
-		    ms_transfer_tpc(chip, MS_TM_READ_BYTES, GET_INT, 1,
-				    NO_WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	retval =
-	    ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm,
-			       (6 + MS_EXTRA_SIZE));
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	ms_set_err_code(chip, MS_NO_ERROR);
-
-	if (CHK_MS4BIT(ms_card)) {
-		/* Parallel interface */
-		data[0] = 0x88;
-	} else {
-		/* Serial interface */
-		data[0] = 0x80;
-	}
-	/* Block Address */
-	data[1] = 0;
-	data[2] = (u8) (new_blk >> 8);
-	data[3] = (u8) new_blk;
-	/* Page Number
-	 * Extra data access mode */
-	if ((end_page - start_page) == 1) {
-		/* Single page access mode */
-		data[4] = 0x20;
-	} else {
-		/* Block access mode */
-		data[4] = 0;
-	}
-	data[5] = start_page;
-	data[6] = 0xF8;
-	data[7] = 0xFF;
-	data[8] = (u8) (log_blk >> 8);
-	data[9] = (u8) log_blk;
-
-	for (i = 0x0A; i < 0x10; i++) {
-		/* ECC */
-		data[i] = 0xFF;
-	}
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_write_bytes(chip, WRITE_REG, 6 + MS_EXTRA_SIZE,
-				   NO_WAIT_INT, data, 16);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
-	/* GET_INT Register */
-	retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	for (page_addr = start_page; page_addr < end_page; page_addr++) {
-		ms_set_err_code(chip, MS_NO_ERROR);
-
-		if (monitor_card_cd(chip, MS_CARD) == CD_NOT_EXIST) {
-			ms_set_err_code(chip, MS_NO_CARD);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		if (val & INT_REG_CMDNK) {
-			ms_set_err_code(chip, MS_CMD_NK);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-		if (val & INT_REG_ERR) {
-			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-		if (!(val & INT_REG_BREQ)) {
-			ms_set_err_code(chip, MS_BREQ_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		udelay(30);
-
-		rts51x_init_cmd(chip);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF,
-			       WRITE_PAGE_DATA);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF,
-			       WAIT_INT);
-
-		rts51x_trans_dma_enable(DMA_TO_DEVICE, chip, 512, DMA_512);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
-			       MS_TRANSFER_START | MS_TM_NORMAL_WRITE);
-		rts51x_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
-			       MS_TRANSFER_END, MS_TRANSFER_END);
-
-		retval = rts51x_send_cmd(chip, MODE_CDOR, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval =
-		    rts51x_transfer_data_partial(chip, SND_BULK_PIPE(chip),
-						 (void *)buf, ptr, offset, 512,
-						 scsi_sg_count(chip->srb), NULL,
-						 2000);
-		if (retval != STATUS_SUCCESS) {
-			ms_set_err_code(chip, MS_TO_ERROR);
-			rts51x_clear_ms_error(chip);
-
-			if (retval == STATUS_TIMEDOUT)
-				TRACE_RET(chip, STATUS_TIMEDOUT);
-			else
-				TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		retval = rts51x_get_rsp(chip, 1, 2000);
-		if (CHECK_MS_TRANS_FAIL(chip, retval)) {
-			ms_set_err_code(chip, MS_TO_ERROR);
-			rts51x_clear_ms_error(chip);
-
-			if (retval == STATUS_TIMEDOUT)
-				TRACE_RET(chip, STATUS_TIMEDOUT);
-			else
-				TRACE_RET(chip, STATUS_FAIL);
-		}
-		/* GET_INT Register */
-		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		if ((end_page - start_page) == 1) {
-			if (!(val & INT_REG_CED)) {
-				/* Command can not be executed */
-				ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		} else {
-			if (page_addr == (end_page - 1)) {
-				if (!(val & INT_REG_CED)) {
-					retval =
-					    ms_send_cmd(chip, BLOCK_END,
-							WAIT_INT);
-					if (retval != STATUS_SUCCESS)
-						TRACE_RET(chip, retval);
-				}
-				/* GET_INT Register */
-				retval =
-				    ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT,
-						  &val, 1);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, retval);
-			}
-
-			if ((page_addr == (end_page - 1))
-			    || (page_addr == ms_card->page_off)) {
-				if (!(val & INT_REG_CED)) {
-					ms_set_err_code(chip,
-							MS_FLASH_WRITE_ERROR);
-					TRACE_RET(chip, STATUS_FAIL);
-				}
-			}
-		}
-	}
-
-	return STATUS_SUCCESS;
-}
-#endif
-
-static int ms_finish_write(struct rts51x_chip *chip, u16 old_blk, u16 new_blk,
-			   u16 log_blk, u8 page_off)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, seg_no;
-
-#ifdef MS_SPEEDUP
-	retval = ms_auto_copy_page(chip, old_blk, new_blk, log_blk,
-				   page_off, ms_card->page_off + 1);
-#else
-	retval = ms_copy_page(chip, old_blk, new_blk, log_blk,
-			      page_off, ms_card->page_off + 1);
-#endif
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	seg_no = old_blk >> 9;
-
-	if (MS_TST_BAD_BLOCK_FLG(ms_card)) {
-		MS_CLR_BAD_BLOCK_FLG(ms_card);
-		ms_set_bad_block(chip, old_blk);
-	} else {
-		retval = ms_erase_block(chip, old_blk);
-		if (retval == STATUS_SUCCESS)
-			ms_set_unused_block(chip, old_blk);
-	}
-
-	ms_set_l2p_tbl(chip, seg_no, log_blk - ms_start_idx[seg_no], new_blk);
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_prepare_write(struct rts51x_chip *chip, u16 old_blk, u16 new_blk,
-			    u16 log_blk, u8 start_page)
-{
-	int retval;
-
-	if (start_page) {
-#ifdef MS_SPEEDUP
-		retval =
-		    ms_auto_copy_page(chip, old_blk, new_blk, log_blk, 0,
-				      start_page);
-#else
-		retval =
-		    ms_copy_page(chip, old_blk, new_blk, log_blk, 0,
-				 start_page);
-#endif
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_ms_delay_write(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	struct rts51x_ms_delay_write_tag *delay_write = &(ms_card->delay_write);
-	int retval;
-
-	if (delay_write->delay_write_flag) {
-		retval = ms_set_init_para(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		delay_write->delay_write_flag = 0;
-		retval = ms_finish_write(chip,
-					 delay_write->old_phyblock,
-					 delay_write->new_phyblock,
-					 delay_write->logblock,
-					 delay_write->pageoff);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static inline void rts51x_ms_rw_fail(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	if (srb->sc_data_direction == DMA_FROM_DEVICE)
-		rts51x_set_sense_type(chip, SCSI_LUN(srb),
-			       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-	else
-		rts51x_set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_MEDIA_WRITE_ERR);
-}
-
-static int rts51x_ms_rw_multi_sector(struct scsi_cmnd *srb, struct rts51x_chip *chip,
-			      u32 start_sector, u16 sector_cnt)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	unsigned int lun = SCSI_LUN(srb);
-	int retval, seg_no;
-	unsigned int offset = 0;
-	u16 old_blk = 0, new_blk = 0, log_blk, total_sec_cnt = sector_cnt;
-	u8 start_page, end_page = 0, page_cnt;
-	u8 *buf;
-	void *ptr = NULL;
-	struct rts51x_ms_delay_write_tag *delay_write = &(ms_card->delay_write);
-
-	ms_set_err_code(chip, MS_NO_ERROR);
-
-	ms_card->counter = 0;
-
-	buf = (u8 *) scsi_sglist(srb);
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_ms_rw_fail(srb, chip);
-		TRACE_RET(chip, retval);
-	}
-
-	log_blk = (u16) (start_sector >> ms_card->block_shift);
-	start_page = (u8) (start_sector & ms_card->page_off);
-
-	for (seg_no = 0; seg_no < ARRAY_SIZE(ms_start_idx) - 1; seg_no++) {
-		if (log_blk < ms_start_idx[seg_no + 1])
-			break;
-	}
-
-	if (ms_card->segment[seg_no].build_flag == 0) {
-		retval = ms_build_l2p_tbl(chip, seg_no);
-		if (retval != STATUS_SUCCESS) {
-			chip->card_fail |= MS_CARD;
-			rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-			TRACE_RET(chip, retval);
-		}
-	}
-
-	if (srb->sc_data_direction == DMA_TO_DEVICE) {
-		if (delay_write->delay_write_flag &&
-		    (delay_write->logblock == log_blk) &&
-		    (start_page > delay_write->pageoff)) {
-			delay_write->delay_write_flag = 0;
-#ifdef MS_SPEEDUP
-			retval = ms_auto_copy_page(chip,
-						   delay_write->old_phyblock,
-						   delay_write->new_phyblock,
-						   log_blk,
-						   delay_write->pageoff,
-						   start_page);
-#else
-			retval = ms_copy_page(chip,
-					      delay_write->old_phyblock,
-					      delay_write->new_phyblock,
-					      log_blk, delay_write->pageoff,
-					      start_page);
-#endif
-			if (retval != STATUS_SUCCESS) {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, retval);
-			}
-			old_blk = delay_write->old_phyblock;
-			new_blk = delay_write->new_phyblock;
-		} else if (delay_write->delay_write_flag &&
-			   (delay_write->logblock == log_blk) &&
-			   (start_page == delay_write->pageoff)) {
-			delay_write->delay_write_flag = 0;
-			old_blk = delay_write->old_phyblock;
-			new_blk = delay_write->new_phyblock;
-		} else {
-			retval = rts51x_ms_delay_write(chip);
-			if (retval != STATUS_SUCCESS) {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, retval);
-			}
-			old_blk =
-			    ms_get_l2p_tbl(chip, seg_no,
-					   log_blk - ms_start_idx[seg_no]);
-			new_blk = ms_get_unused_block(chip, seg_no);
-			if ((old_blk == 0xFFFF) || (new_blk == 0xFFFF)) {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-
-			retval =
-			    ms_prepare_write(chip, old_blk, new_blk, log_blk,
-					     start_page);
-			if (retval != STATUS_SUCCESS) {
-				if (monitor_card_cd(chip, MS_CARD) ==
-				    CD_NOT_EXIST) {
-					rts51x_set_sense_type(chip, lun,
-						SENSE_TYPE_MEDIA_NOT_PRESENT);
-					TRACE_RET(chip, STATUS_FAIL);
-				}
-
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, retval);
-			}
-		}
-	} else {
-		retval = rts51x_ms_delay_write(chip);
-		if (retval != STATUS_SUCCESS) {
-			if (monitor_card_cd(chip, MS_CARD) == CD_NOT_EXIST) {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_NOT_PRESENT);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-			TRACE_RET(chip, retval);
-		}
-		old_blk =
-		    ms_get_l2p_tbl(chip, seg_no,
-				   log_blk - ms_start_idx[seg_no]);
-		if (old_blk == 0xFFFF) {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-
-	RTS51X_DEBUGP("seg_no = %d, old_blk = 0x%x, new_blk = 0x%x\n", seg_no,
-		       old_blk, new_blk);
-
-	while (total_sec_cnt) {
-		if ((start_page + total_sec_cnt) > (ms_card->page_off + 1))
-			end_page = ms_card->page_off + 1;
-		else
-			end_page = start_page + (u8) total_sec_cnt;
-		page_cnt = end_page - start_page;
-
-		RTS51X_DEBUGP("start_page = %d, end_page = %d, page_cnt = %d\n",
-					start_page, end_page, page_cnt);
-
-		if (srb->sc_data_direction == DMA_FROM_DEVICE)
-			retval = ms_read_multiple_pages(chip,
-							old_blk, log_blk,
-							start_page, end_page,
-							buf, &ptr, &offset);
-		else
-			retval = ms_write_multiple_pages(chip, old_blk,
-							 new_blk, log_blk,
-							 start_page, end_page,
-							 buf, &ptr, &offset);
-
-		if (retval != STATUS_SUCCESS) {
-			if (monitor_card_cd(chip, MS_CARD) == CD_NOT_EXIST) {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_NOT_PRESENT);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-
-			rts51x_ms_rw_fail(srb, chip);
-			TRACE_RET(chip, retval);
-		}
-		/* Update L2P table if need */
-		if (srb->sc_data_direction == DMA_TO_DEVICE) {
-			if (end_page == (ms_card->page_off + 1)) {
-				retval = ms_erase_block(chip, old_blk);
-				if (retval == STATUS_SUCCESS)
-					ms_set_unused_block(chip, old_blk);
-				ms_set_l2p_tbl(chip, seg_no,
-					       log_blk - ms_start_idx[seg_no],
-					       new_blk);
-			}
-		}
-
-		total_sec_cnt -= page_cnt;
-
-		if (total_sec_cnt == 0)
-			break;
-
-		log_blk++;
-
-		for (seg_no = 0; seg_no < ARRAY_SIZE(ms_start_idx) - 1;
-				seg_no++) {
-			if (log_blk < ms_start_idx[seg_no + 1])
-				break;
-		}
-
-		if (ms_card->segment[seg_no].build_flag == 0) {
-			retval = ms_build_l2p_tbl(chip, seg_no);
-			if (retval != STATUS_SUCCESS) {
-				chip->card_fail |= MS_CARD;
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_NOT_PRESENT);
-				TRACE_RET(chip, retval);
-			}
-		}
-
-		old_blk =
-		    ms_get_l2p_tbl(chip, seg_no,
-				   log_blk - ms_start_idx[seg_no]);
-		if (old_blk == 0xFFFF) {
-			rts51x_ms_rw_fail(srb, chip);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		if (srb->sc_data_direction == DMA_TO_DEVICE) {
-			new_blk = ms_get_unused_block(chip, seg_no);
-			if (new_blk == 0xFFFF) {
-				rts51x_ms_rw_fail(srb, chip);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-
-		RTS51X_DEBUGP("seg_no = %d, old_blk = 0x%x, new_blk = 0x%x\n",
-			       seg_no, old_blk, new_blk);
-
-		start_page = 0;
-	}
-
-	if (srb->sc_data_direction == DMA_TO_DEVICE) {
-		if (end_page < (ms_card->page_off + 1)) {
-			delay_write->delay_write_flag = 1;
-			delay_write->old_phyblock = old_blk;
-			delay_write->new_phyblock = new_blk;
-			delay_write->logblock = log_blk;
-			delay_write->pageoff = end_page;
-		}
-	}
-
-	scsi_set_resid(srb, 0);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_ms_rw(struct scsi_cmnd *srb, struct rts51x_chip *chip, u32 start_sector,
-	  u16 sector_cnt)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-
-	if (CHK_MSPRO(ms_card))
-		retval =
-		    mspro_rw_multi_sector(srb, chip, start_sector, sector_cnt);
-	else
-		retval =
-		    rts51x_ms_rw_multi_sector(srb, chip, start_sector, sector_cnt);
-
-	return retval;
-}
-
-void rts51x_ms_free_l2p_tbl(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int i = 0;
-
-	if (ms_card->segment != NULL) {
-		for (i = 0; i < ms_card->segment_cnt; i++) {
-			if (ms_card->segment[i].l2p_table != NULL) {
-				vfree(ms_card->segment[i].l2p_table);
-				ms_card->segment[i].l2p_table = NULL;
-			}
-			if (ms_card->segment[i].free_table != NULL) {
-				vfree(ms_card->segment[i].free_table);
-				ms_card->segment[i].free_table = NULL;
-			}
-		}
-		vfree(ms_card->segment);
-		ms_card->segment = NULL;
-	}
-}
-
-void rts51x_ms_cleanup_work(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-
-	if (CHK_MSPRO(ms_card)) {
-		if (ms_card->seq_mode) {
-			RTS51X_DEBUGP("MS Pro: stop transmission\n");
-			mspro_stop_seq_mode(chip);
-			ms_card->counter = 0;
-		}
-		if (CHK_MSHG(ms_card)) {
-			u8 value;
-			rts51x_read_register(chip, MS_CFG, &value);
-			if (value & MS_2K_SECTOR_MODE)
-				rts51x_write_register(chip, MS_CFG,
-						      MS_2K_SECTOR_MODE, 0x00);
-		}
-	} else if ((!CHK_MSPRO(ms_card))
-		   && ms_card->delay_write.delay_write_flag) {
-		RTS51X_DEBUGP("MS: delay write\n");
-		rts51x_ms_delay_write(chip);
-		ms_card->counter = 0;
-	}
-}
-
-static int ms_power_off_card3v3(struct rts51x_chip *chip)
-{
-	int retval;
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_CLK_EN, MS_CLK_EN, 0);
-	if (chip->asic_code)
-		ms_pull_ctl_disable(chip);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, FPGA_PULL_CTL,
-			       FPGA_MS_PULL_CTL_BIT | 0x20,
-			       FPGA_MS_PULL_CTL_BIT);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_OE, MS_OUTPUT_EN, 0);
-	if (!chip->option.FT2_fast_mode) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, POWER_MASK,
-			       POWER_OFF);
-	}
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_release_ms_card(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-
-	RTS51X_DEBUGP("rts51x_release_ms_card\n");
-
-	ms_card->delay_write.delay_write_flag = 0;
-	ms_card->pro_under_formatting = 0;
-
-	chip->card_ready &= ~MS_CARD;
-	chip->card_fail &= ~MS_CARD;
-	chip->card_wp &= ~MS_CARD;
-
-	rts51x_ms_free_l2p_tbl(chip);
-
-	rts51x_write_register(chip, SFSM_ED, HW_CMD_STOP, HW_CMD_STOP);
-
-	memset(ms_card->raw_sys_info, 0, 96);
-#ifdef SUPPORT_PCGL_1P18
-	memset(ms_card->raw_model_name, 0, 48);
-#endif
-
-	retval = ms_power_off_card3v3(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
diff --git a/drivers/staging/rts5139/ms.h b/drivers/staging/rts5139/ms.h
deleted file mode 100644
index 857c1974ef2..00000000000
--- a/drivers/staging/rts5139/ms.h
+++ /dev/null
@@ -1,261 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_MS_H
-#define __RTS51X_MS_H
-
-#include "rts51x_chip.h"
-
-#define	MS_MAX_RETRY_COUNT		3
-
-#define	MS_EXTRA_SIZE			0x9
-
-#define	WRT_PRTCT			0x01
-
-/* Error Code */
-#define	MS_NO_ERROR				0x00
-#define	MS_CRC16_ERROR				0x80
-#define	MS_TO_ERROR				0x40
-#define	MS_NO_CARD				0x20
-#define	MS_NO_MEMORY				0x10
-#define	MS_CMD_NK				0x08
-#define	MS_FLASH_READ_ERROR			0x04
-#define	MS_FLASH_WRITE_ERROR			0x02
-#define	MS_BREQ_ERROR				0x01
-#define	MS_NOT_FOUND				0x03
-
-/* Transfer Protocol Command */
-#define READ_PAGE_DATA				0x02
-#define READ_REG				0x04
-#define	GET_INT					0x07
-#define WRITE_PAGE_DATA				0x0D
-#define WRITE_REG				0x0B
-#define SET_RW_REG_ADRS				0x08
-#define SET_CMD					0x0E
-
-#define	PRO_READ_LONG_DATA			0x02
-#define	PRO_READ_SHORT_DATA			0x03
-#define PRO_READ_REG				0x04
-#define	PRO_READ_QUAD_DATA			0x05
-#define PRO_GET_INT				0x07
-#define	PRO_WRITE_LONG_DATA			0x0D
-#define	PRO_WRITE_SHORT_DATA			0x0C
-#define	PRO_WRITE_QUAD_DATA			0x0A
-#define PRO_WRITE_REG				0x0B
-#define PRO_SET_RW_REG_ADRS			0x08
-#define PRO_SET_CMD				0x0E
-#define PRO_EX_SET_CMD				0x09
-
-#ifdef SUPPORT_MAGIC_GATE
-#define MG_GET_ID		0x40
-#define MG_SET_LID		0x41
-#define MG_GET_LEKB		0x42
-#define MG_SET_RD		0x43
-#define MG_MAKE_RMS		0x44
-#define MG_MAKE_KSE		0x45
-#define MG_SET_IBD		0x46
-#define MG_GET_IBD		0x47
-#endif
-
-#ifdef XC_POWERCLASS
-#define XC_CHG_POWER		0x16
-#endif
-
-/* ++ CMD over Memory Stick */
-/*  Flash CMD */
-#define BLOCK_READ		0xAA
-#define	BLOCK_WRITE		0x55
-#define BLOCK_END		0x33
-#define BLOCK_ERASE		0x99
-#define FLASH_STOP		0xCC
-
-/*  Function CMD */
-#define SLEEP			0x5A
-#define CLEAR_BUF		0xC3
-#define MS_RESET		0x3C
-/* -- CMD over Memory Stick */
-
-/* ++ CMD over Memory Stick Pro */
-/*  Flash CMD */
-#define PRO_READ_DATA		0x20
-#define	PRO_WRITE_DATA		0x21
-#define PRO_READ_ATRB		0x24
-#define PRO_STOP		0x25
-#define PRO_ERASE		0x26
-#define	PRO_READ_2K_DATA	0x27
-#define	PRO_WRITE_2K_DATA	0x28
-
-/*  Function CMD */
-#define PRO_FORMAT		0x10
-#define PRO_SLEEP		0x11
-/* -- CMD over Memory Stick Pro */
-
-/*  register inside memory stick */
-#define	IntReg			0x01
-#define StatusReg0		0x02
-#define StatusReg1		0x03
-
-#define SystemParm		0x10
-#define BlockAdrs		0x11
-#define CMDParm			0x14
-#define PageAdrs		0x15
-
-#define OverwriteFlag		0x16
-#define ManagemenFlag		0x17
-#define LogicalAdrs		0x18
-#define ReserveArea		0x1A
-
-/*  register inside memory pro */
-#define	Pro_IntReg		0x01
-#define Pro_StatusReg		0x02
-#define Pro_TypeReg		0x04
-#define	Pro_IFModeReg		0x05
-#define Pro_CatagoryReg		0x06
-#define Pro_ClassReg		0x07
-
-#define Pro_SystemParm		0x10
-#define Pro_DataCount1		0x11
-#define Pro_DataCount0		0x12
-#define Pro_DataAddr3		0x13
-#define Pro_DataAddr2		0x14
-#define Pro_DataAddr1		0x15
-#define Pro_DataAddr0		0x16
-
-#define Pro_TPCParm		0x17
-#define Pro_CMDParm		0x18
-
-/*  define for INT Register */
-#define	INT_REG_CED		0x80
-#define	INT_REG_ERR		0x40
-#define	INT_REG_BREQ		0x20
-#define	INT_REG_CMDNK		0x01
-
-/*  INT signal */
-#define	INT_CED			0x01
-#define	INT_ERR			0x02
-#define	INT_BREQ		0x04
-#define	INT_CMDNK		0x08
-
-/*  define for OverwriteFlag Register */
-#define	BLOCK_BOOT		0xC0
-#define	BLOCK_OK		0x80
-#define	PAGE_OK			0x60
-#define	DATA_COMPL		0x10
-
-/*  define for ManagemenFlag Register */
-#define	NOT_BOOT_BLOCK		0x4
-#define	NOT_TRANSLATION_TABLE	0x8
-
-/*  Header */
-#define	HEADER_ID0		(PPBUF_BASE2)			/* 0 */
-#define	HEADER_ID1		(PPBUF_BASE2 + 1)		/* 1 */
-/*  System Entry */
-#define	DISABLED_BLOCK0		(PPBUF_BASE2 + 0x170 + 4)	/* 2 */
-#define	DISABLED_BLOCK1		(PPBUF_BASE2 + 0x170 + 5)	/* 3 */
-#define	DISABLED_BLOCK2		(PPBUF_BASE2 + 0x170 + 6)	/* 4 */
-#define	DISABLED_BLOCK3		(PPBUF_BASE2 + 0x170 + 7)	/* 5 */
-/*  Boot & Attribute Information */
-#define	BLOCK_SIZE_0		(PPBUF_BASE2 + 0x1a0 + 2)	/* 6 */
-#define	BLOCK_SIZE_1		(PPBUF_BASE2 + 0x1a0 + 3)	/* 7 */
-#define	BLOCK_COUNT_0		(PPBUF_BASE2 + 0x1a0 + 4)	/* 8 */
-#define	BLOCK_COUNT_1		(PPBUF_BASE2 + 0x1a0 + 5)	/* 9 */
-#define	EBLOCK_COUNT_0		(PPBUF_BASE2 + 0x1a0 + 6)	/* 10 */
-#define	EBLOCK_COUNT_1		(PPBUF_BASE2 + 0x1a0 + 7)	/* 11 */
-#define	PAGE_SIZE_0		(PPBUF_BASE2 + 0x1a0 + 8)	/* 12 */
-#define	PAGE_SIZE_1		(PPBUF_BASE2 + 0x1a0 + 9)	/* 13 */
-
-/* joey 2004-08-07 for MS check Procedure */
-#define MS_Device_Type	(PPBUF_BASE2 + 0x1D8)	/* 14 */
-/* end */
-
-/* joey 2004-05-03 */
-#define	MS_4bit_Support	(PPBUF_BASE2 + 0x1D3)	/* 15 */
-/* end */
-
-#define setPS_NG	1
-#define setPS_Error	0
-
-/*  define for Pro_SystemParm Register */
-#define	PARALLEL_8BIT_IF	0x40
-#define	PARALLEL_4BIT_IF	0x00
-#define	SERIAL_IF		0x80
-
-/*  define for StatusReg0 Register */
-#define BUF_FULL	0x10
-#define BUF_EMPTY	0x20
-
-/*  define for StatusReg1 Register */
-#define	MEDIA_BUSY	0x80
-#define	FLASH_BUSY	0x40
-#define	DATA_ERROR	0x20
-#define	STS_UCDT	0x10
-#define	EXTRA_ERROR	0x08
-#define	STS_UCEX	0x04
-#define	FLAG_ERROR	0x02
-#define	STS_UCFG	0x01
-
-#define MS_SHORT_DATA_LEN	32
-
-#define FORMAT_SUCCESS		0
-#define FORMAT_FAIL		1
-#define FORMAT_IN_PROGRESS	2
-
-#define	MS_SET_BAD_BLOCK_FLG(ms_card)	((ms_card)->multi_flag |= 0x80)
-#define MS_CLR_BAD_BLOCK_FLG(ms_card)	((ms_card)->multi_flag &= 0x7F)
-#define MS_TST_BAD_BLOCK_FLG(ms_card)	((ms_card)->multi_flag & 0x80)
-
-#define CHECK_MS_TRANS_FAIL(chip, retval)	\
-	(((retval) != STATUS_SUCCESS) || \
-	(chip->rsp_buf[0] & MS_TRANSFER_ERR))
-
-void rts51x_mspro_polling_format_status(struct rts51x_chip *chip);
-void rts51x_mspro_format_sense(struct rts51x_chip *chip, unsigned int lun);
-
-int rts51x_reset_ms_card(struct rts51x_chip *chip);
-int rts51x_ms_rw(struct scsi_cmnd *srb, struct rts51x_chip *chip, u32 start_sector,
-	  u16 sector_cnt);
-int rts51x_mspro_format(struct scsi_cmnd *srb, struct rts51x_chip *chip,
-		 int short_data_len, int quick_format);
-void rts51x_ms_free_l2p_tbl(struct rts51x_chip *chip);
-void rts51x_ms_cleanup_work(struct rts51x_chip *chip);
-int rts51x_release_ms_card(struct rts51x_chip *chip);
-int rts51x_ms_delay_write(struct rts51x_chip *chip);
-
-#ifdef SUPPORT_MAGIC_GATE
-
-int ms_switch_clock(struct rts51x_chip *chip);
-int ms_write_bytes(struct rts51x_chip *chip, u8 tpc, u8 cnt, u8 cfg, u8 *data,
-		   int data_len);
-int ms_read_bytes(struct rts51x_chip *chip, u8 tpc, u8 cnt, u8 cfg, u8 *data,
-		  int data_len);
-int ms_set_rw_reg_addr(struct rts51x_chip *chip, u8 read_start, u8 read_cnt,
-		       u8 write_start, u8 write_cnt);
-int ms_transfer_data(struct rts51x_chip *chip, u8 trans_mode, u8 tpc,
-		     u16 sec_cnt, u8 cfg, int mode_2k, int use_sg, void *buf,
-		     int buf_len);
-#endif
-
-#endif /* __RTS51X_MS_H */
diff --git a/drivers/staging/rts5139/ms_mg.c b/drivers/staging/rts5139/ms_mg.c
deleted file mode 100644
index 00862c1a36d..00000000000
--- a/drivers/staging/rts5139/ms_mg.c
+++ /dev/null
@@ -1,643 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#include <linux/blkdev.h>
-#include <linux/kthread.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-
-#include "debug.h"
-#include "trace.h"
-#include "rts51x.h"
-#include "rts51x_transport.h"
-#include "rts51x_scsi.h"
-#include "rts51x_card.h"
-#include "ms.h"
-#include "ms_mg.h"
-
-#ifdef SUPPORT_MAGIC_GATE
-
-static int mg_check_int_error(struct rts51x_chip *chip)
-{
-	u8 value;
-
-	rts51x_read_register(chip, MS_TRANS_CFG, &value);
-	if (value & (INT_ERR | INT_CMDNK))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-static int mg_send_ex_cmd(struct rts51x_chip *chip, u8 cmd, u8 entry_num)
-{
-	int retval, i;
-	u8 data[8];
-
-	data[0] = cmd;
-	data[1] = 0;
-	data[2] = 0;
-	data[3] = 0;
-	data[4] = 0;
-	data[5] = 0;
-	data[6] = entry_num;
-	data[7] = 0;
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_write_bytes(chip, PRO_EX_SET_CMD, 7, WAIT_INT, data, 8);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
-	retval = mg_check_int_error(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-int mg_set_tpc_para_sub(struct rts51x_chip *chip, int type, u8 mg_entry_num)
-{
-	int retval;
-	u8 buf[6];
-
-	RTS51X_DEBUGP("--%s--\n", __func__);
-
-	if (type == 0)
-		retval = ms_set_rw_reg_addr(chip, 0, 0, Pro_TPCParm, 1);
-	else
-		retval = ms_set_rw_reg_addr(chip, 0, 0, Pro_DataCount1, 6);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	buf[0] = 0;
-	buf[1] = 0;
-	if (type == 1) {
-		buf[2] = 0;
-		buf[3] = 0;
-		buf[4] = 0;
-		buf[5] = mg_entry_num;
-	}
-	retval =
-	    ms_write_bytes(chip, PRO_WRITE_REG, (type == 0) ? 1 : 6,
-			   NO_WAIT_INT, buf, 6);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-/**
-  * Get MagciGate ID and set Leaf ID to medium.
-
-  * After receiving this SCSI command, adapter shall fulfill 2 tasks
-  * below in order:
-  * 1. send GET_ID TPC command to get MagicGate ID and hold it till
-  * Response&challenge CMD.
-  * 2. send SET_ID TPC command to medium with Leaf ID released by host
-  * in this SCSI CMD.
-  */
-int rts51x_mg_set_leaf_id(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	int retval;
-	int i;
-	unsigned int lun = SCSI_LUN(srb);
-	u8 buf1[32], buf2[12];
-
-	RTS51X_DEBUGP("--%s--\n", __func__);
-
-	if (scsi_bufflen(srb) < 12) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-	rts51x_ms_cleanup_work(chip);
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = mg_send_ex_cmd(chip, MG_SET_LID, 0);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
-		TRACE_RET(chip, retval);
-	}
-
-	memset(buf1, 0, 32);
-	rts51x_get_xfer_buf(buf2, min(12, (int)scsi_bufflen(srb)), srb);
-	for (i = 0; i < 8; i++)
-		buf1[8 + i] = buf2[4 + i];
-	retval =
-	    ms_write_bytes(chip, PRO_WRITE_SHORT_DATA, 32, WAIT_INT, buf1, 32);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
-		TRACE_RET(chip, retval);
-	}
-	retval = mg_check_int_error(chip);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
-		TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-/**
-  * Send Local EKB to host.
-
-  * After receiving this SCSI command, adapter shall read the divided
-  * data(1536 bytes totally) from medium by using READ_LONG_DATA TPC
-  * for 3 times, and report data to host with data-length is 1052 bytes.
-  */
-int rts51x_mg_get_local_EKB(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	int retval = STATUS_FAIL;
-	int bufflen;
-	unsigned int lun = SCSI_LUN(srb);
-	u8 *buf = NULL;
-
-	RTS51X_DEBUGP("--%s--\n", __func__);
-
-	rts51x_ms_cleanup_work(chip);
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	buf = kmalloc(1540, GFP_KERNEL);
-	if (!buf)
-		TRACE_RET(chip, STATUS_NOMEM);
-
-	buf[0] = 0x04;
-	buf[1] = 0x1A;
-	buf[2] = 0x00;
-	buf[3] = 0x00;
-
-	retval = mg_send_ex_cmd(chip, MG_GET_LEKB, 0);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_GOTO(chip, GetEKBFinish);
-	}
-
-	retval = ms_transfer_data(chip, MS_TM_AUTO_READ, PRO_READ_LONG_DATA,
-				  3, WAIT_INT, 0, 0, buf + 4, 1536);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		rts51x_write_register(chip, CARD_STOP, MS_STOP | MS_CLR_ERR,
-				      MS_STOP | MS_CLR_ERR);
-		TRACE_GOTO(chip, GetEKBFinish);
-	}
-	retval = mg_check_int_error(chip);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_GOTO(chip, GetEKBFinish);
-	}
-
-	bufflen = min(1052, (int)scsi_bufflen(srb));
-	rts51x_set_xfer_buf(buf, bufflen, srb);
-
-GetEKBFinish:
-	kfree(buf);
-	return retval;
-}
-
-/**
-  * Send challenge(host) to medium.
-
-  * After receiving this SCSI command, adapter shall sequentially issues
-  * TPC commands to the medium for writing 8-bytes data as challenge
-  * by host within a short data packet.
-  */
-int rts51x_mg_chg(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	int bufflen;
-	int i;
-	unsigned int lun = SCSI_LUN(srb);
-	u8 buf[32], tmp;
-
-	RTS51X_DEBUGP("--%s--\n", __func__);
-
-	rts51x_ms_cleanup_work(chip);
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = mg_send_ex_cmd(chip, MG_GET_ID, 0);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, retval);
-	}
-
-	retval =
-	    ms_read_bytes(chip, PRO_READ_SHORT_DATA, 32, WAIT_INT, buf, 32);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, retval);
-	}
-	retval = mg_check_int_error(chip);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, retval);
-	}
-
-	memcpy(ms_card->magic_gate_id, buf, 16);
-
-	for (i = 0; i < 2500; i++) {
-		RTS51X_READ_REG(chip, MS_TRANS_CFG, &tmp);
-		if (tmp &
-		    (MS_INT_CED | MS_INT_CMDNK | MS_INT_BREQ | MS_INT_ERR))
-			break;
-
-		wait_timeout(1);
-	}
-
-	if (i == 2500) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	retval = mg_send_ex_cmd(chip, MG_SET_RD, 0);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, retval);
-	}
-
-	bufflen = min(12, (int)scsi_bufflen(srb));
-	rts51x_get_xfer_buf(buf, bufflen, srb);
-
-	for (i = 0; i < 8; i++)
-		buf[i] = buf[4 + i];
-	for (i = 0; i < 24; i++)
-		buf[8 + i] = 0;
-	retval =
-	    ms_write_bytes(chip, PRO_WRITE_SHORT_DATA, 32, WAIT_INT, buf, 32);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, retval);
-	}
-	retval = mg_check_int_error(chip);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, retval);
-	}
-
-	ms_card->mg_auth = 0;
-
-	return STATUS_SUCCESS;
-}
-
-/**
-  * Send Response and Challenge data  to host.
-
-  * After receiving this SCSI command, adapter shall communicates with
-  * the medium, get parameters(HRd, Rms, MagicGateID) by using READ_SHORT_DATA
-  * TPC and send the data to host according to certain format required by
-  * MG-R specification.
-  * The paremeter MagicGateID is the one that adapter has obtained from
-  * the medium by TPC commands in Set Leaf ID command phase previously.
-  */
-int rts51x_mg_get_rsp_chg(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-	int bufflen;
-	unsigned int lun = SCSI_LUN(srb);
-	u8 buf1[32], buf2[36], tmp;
-
-	RTS51X_DEBUGP("--%s--\n", __func__);
-
-	rts51x_ms_cleanup_work(chip);
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = mg_send_ex_cmd(chip, MG_MAKE_RMS, 0);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_RET(chip, retval);
-	}
-
-	retval =
-	    ms_read_bytes(chip, PRO_READ_SHORT_DATA, 32, WAIT_INT, buf1, 32);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_RET(chip, retval);
-	}
-	retval = mg_check_int_error(chip);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_RET(chip, retval);
-	}
-
-	buf2[0] = 0x00;
-	buf2[1] = 0x22;
-	buf2[2] = 0x00;
-	buf2[3] = 0x00;
-
-	memcpy(buf2 + 4, ms_card->magic_gate_id, 16);
-	memcpy(buf2 + 20, buf1, 16);
-
-	bufflen = min(36, (int)scsi_bufflen(srb));
-	rts51x_set_xfer_buf(buf2, bufflen, srb);
-
-	for (i = 0; i < 2500; i++) {
-		RTS51X_READ_REG(chip, MS_TRANS_CFG, &tmp);
-		if (tmp & (MS_INT_CED | MS_INT_CMDNK |
-				MS_INT_BREQ | MS_INT_ERR))
-			break;
-
-		wait_timeout(1);
-	}
-
-	if (i == 2500) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-/**
-  * Send response(host) to medium.
-
-  * After receiving this SCSI command, adapter shall sequentially
-  * issues TPC commands to the medium for writing 8-bytes data as
-  * challenge by host within a short data packet.
-  */
-int rts51x_mg_rsp(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	int i;
-	int bufflen;
-	unsigned int lun = SCSI_LUN(srb);
-	u8 buf[32];
-
-	RTS51X_DEBUGP("--%s--\n", __func__);
-
-	rts51x_ms_cleanup_work(chip);
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = mg_send_ex_cmd(chip, MG_MAKE_KSE, 0);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_RET(chip, retval);
-	}
-
-	bufflen = min(12, (int)scsi_bufflen(srb));
-	rts51x_get_xfer_buf(buf, bufflen, srb);
-
-	for (i = 0; i < 8; i++)
-		buf[i] = buf[4 + i];
-	for (i = 0; i < 24; i++)
-		buf[8 + i] = 0;
-	retval =
-	    ms_write_bytes(chip, PRO_WRITE_SHORT_DATA, 32, WAIT_INT, buf, 32);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_RET(chip, retval);
-	}
-	retval = mg_check_int_error(chip);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_RET(chip, retval);
-	}
-
-	ms_card->mg_auth = 1;
-
-	return STATUS_SUCCESS;
-}
-
-/** * Send ICV data to host.
-
-  * After receiving this SCSI command, adapter shall read the divided
-  * data(1024 bytes totally) from medium by using READ_LONG_DATA TPC
-  * for 2 times, and report data to host with data-length is 1028 bytes.
-  *
-  * Since the extra 4 bytes data is just only a prefix to original data
-  * that read from medium, so that the 4-byte data pushed into Ring buffer
-  * precedes data transmission from medium to Ring buffer by DMA mechanism
-  * in order to get maximum performance and minimum code size simultaneously.
-  */
-int rts51x_mg_get_ICV(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	int bufflen;
-	unsigned int lun = SCSI_LUN(srb);
-	u8 *buf = NULL;
-
-	RTS51X_DEBUGP("--%s--\n", __func__);
-
-	rts51x_ms_cleanup_work(chip);
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	buf = kmalloc(1028, GFP_KERNEL);
-	if (!buf)
-		TRACE_RET(chip, STATUS_NOMEM);
-
-	buf[0] = 0x04;
-	buf[1] = 0x02;
-	buf[2] = 0x00;
-	buf[3] = 0x00;
-
-	retval = mg_send_ex_cmd(chip, MG_GET_IBD, ms_card->mg_entry_num);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-		TRACE_GOTO(chip, GetICVFinish);
-	}
-
-	retval = ms_transfer_data(chip, MS_TM_AUTO_READ, PRO_READ_LONG_DATA,
-				  2, WAIT_INT, 0, 0, buf + 4, 1024);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-		rts51x_write_register(chip, CARD_STOP, MS_STOP | MS_CLR_ERR,
-				      MS_STOP | MS_CLR_ERR);
-		TRACE_GOTO(chip, GetICVFinish);
-	}
-	retval = mg_check_int_error(chip);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-		TRACE_GOTO(chip, GetICVFinish);
-	}
-
-	bufflen = min(1028, (int)scsi_bufflen(srb));
-	rts51x_set_xfer_buf(buf, bufflen, srb);
-
-GetICVFinish:
-	kfree(buf);
-	return retval;
-}
-
-/**
-  * Send ICV data to medium.
-
-  * After receiving this SCSI command, adapter shall receive 1028 bytes
-  * and write the later 1024 bytes to medium by WRITE_LONG_DATA TPC
-  * consecutively.
-  *
-  * Since the first 4-bytes data is just only a prefix to original data
-  * that sent by host, and it should be skipped by shifting DMA pointer
-  * before writing 1024 bytes to medium.
-  */
-int rts51x_mg_set_ICV(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	int bufflen;
-#ifdef MG_SET_ICV_SLOW
-	int i;
-#endif
-	unsigned int lun = SCSI_LUN(srb);
-	u8 *buf = NULL;
-
-	RTS51X_DEBUGP("--%s--\n", __func__);
-
-	rts51x_ms_cleanup_work(chip);
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	buf = kmalloc(1028, GFP_KERNEL);
-	if (!buf)
-		TRACE_RET(chip, STATUS_NOMEM);
-
-	bufflen = min(1028, (int)scsi_bufflen(srb));
-	rts51x_get_xfer_buf(buf, bufflen, srb);
-
-	retval = mg_send_ex_cmd(chip, MG_SET_IBD, ms_card->mg_entry_num);
-	if (retval != STATUS_SUCCESS) {
-		if (ms_card->mg_auth == 0) {
-			if ((buf[5] & 0xC0) != 0)
-				rts51x_set_sense_type(chip, lun,
-					SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
-			else
-				rts51x_set_sense_type(chip, lun,
-					SENSE_TYPE_MG_WRITE_ERR);
-		} else {
-			rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_WRITE_ERR);
-		}
-		TRACE_GOTO(chip, SetICVFinish);
-	}
-
-#ifdef MG_SET_ICV_SLOW
-	for (i = 0; i < 2; i++) {
-		udelay(50);
-
-		rts51x_init_cmd(chip);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF,
-			       PRO_WRITE_LONG_DATA);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF,
-			       WAIT_INT);
-
-		rts51x_trans_dma_enable(DMA_TO_DEVICE, chip, 512, DMA_512);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
-			       MS_TRANSFER_START | MS_TM_NORMAL_WRITE);
-		rts51x_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
-			       MS_TRANSFER_END, MS_TRANSFER_END);
-
-		retval = rts51x_send_cmd(chip, MODE_CDOR, 100);
-		if (retval != STATUS_SUCCESS) {
-			rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_WRITE_ERR);
-			TRACE_GOTO(chip, SetICVFinish);
-		}
-
-		retval = rts51x_transfer_data_rcc(chip, SND_BULK_PIPE(chip),
-						  buf + 4 + i * 512, 512, 0,
-						  NULL, 3000, STAGE_DO);
-		if (retval != STATUS_SUCCESS) {
-			rts51x_clear_ms_error(chip);
-			if (ms_card->mg_auth == 0) {
-				if ((buf[5] & 0xC0) != 0)
-					rts51x_set_sense_type(chip, lun,
-						SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
-				else
-					rts51x_set_sense_type(chip, lun,
-						SENSE_TYPE_MG_WRITE_ERR);
-			} else {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MG_WRITE_ERR);
-			}
-			retval = STATUS_FAIL;
-			TRACE_GOTO(chip, SetICVFinish);
-		}
-
-		retval = rts51x_get_rsp(chip, 1, 3000);
-		if (CHECK_MS_TRANS_FAIL(chip, retval)
-		    || mg_check_int_error(chip)) {
-			rts51x_clear_ms_error(chip);
-			if (ms_card->mg_auth == 0) {
-				if ((buf[5] & 0xC0) != 0)
-					rts51x_set_sense_type(chip, lun,
-						SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
-				else
-					rts51x_set_sense_type(chip, lun,
-						SENSE_TYPE_MG_WRITE_ERR);
-			} else {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MG_WRITE_ERR);
-			}
-			retval = STATUS_FAIL;
-			TRACE_GOTO(chip, SetICVFinish);
-		}
-	}
-#else
-	retval = ms_transfer_data(chip, MS_TM_AUTO_WRITE, PRO_WRITE_LONG_DATA,
-				  2, WAIT_INT, 0, 0, buf + 4, 1024);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_ms_error(chip);
-		if (ms_card->mg_auth == 0) {
-			if ((buf[5] & 0xC0) != 0)
-				rts51x_set_sense_type(chip, lun,
-					SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
-			else
-				rts51x_set_sense_type(chip, lun,
-					SENSE_TYPE_MG_WRITE_ERR);
-		} else {
-			rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_WRITE_ERR);
-		}
-		TRACE_GOTO(chip, SetICVFinish);
-	}
-#endif
-
-SetICVFinish:
-	kfree(buf);
-	return retval;
-}
-
-#endif /* SUPPORT_MAGIC_GATE */
diff --git a/drivers/staging/rts5139/ms_mg.h b/drivers/staging/rts5139/ms_mg.h
deleted file mode 100644
index d15733a992a..00000000000
--- a/drivers/staging/rts5139/ms_mg.h
+++ /dev/null
@@ -1,41 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_MS_MG_H
-#define __RTS51X_MS_MG_H
-
-#include "rts51x_chip.h"
-#include "ms.h"
-
-int rts51x_mg_set_leaf_id(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-int rts51x_mg_get_local_EKB(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-int rts51x_mg_chg(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-int rts51x_mg_get_rsp_chg(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-int rts51x_mg_rsp(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-int rts51x_mg_get_ICV(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-int rts51x_mg_set_ICV(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-
-#endif /* __RTS51X_MS_MG_H */
diff --git a/drivers/staging/rts5139/rts51x.c b/drivers/staging/rts5139/rts51x.c
deleted file mode 100644
index c8d06d4fddd..00000000000
--- a/drivers/staging/rts5139/rts51x.c
+++ /dev/null
@@ -1,857 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#include <linux/blkdev.h>
-#include <linux/kthread.h>
-#include <linux/sched.h>
-#include <linux/workqueue.h>
-#include <linux/errno.h>
-#include <linux/freezer.h>
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/mutex.h>
-#include <linux/utsname.h>
-#include <linux/usb.h>
-
-#include <scsi/scsi.h>
-#include <scsi/scsi_cmnd.h>
-#include <scsi/scsi_device.h>
-#include <scsi/scsi_devinfo.h>
-#include <scsi/scsi_eh.h>
-#include <scsi/scsi_host.h>
-
-#include "debug.h"
-#include "ms.h"
-#include "rts51x.h"
-#include "rts51x_chip.h"
-#include "rts51x_card.h"
-#include "rts51x_scsi.h"
-#include "rts51x_transport.h"
-#include "rts51x_fop.h"
-
-MODULE_DESCRIPTION(RTS51X_DESC);
-MODULE_LICENSE("GPL");
-MODULE_VERSION(DRIVER_VERSION);
-
-static int auto_delink_en;
-module_param(auto_delink_en, int, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(auto_delink_en, "enable auto delink");
-
-static int ss_en;
-module_param(ss_en, int, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(ss_en, "enable selective suspend");
-
-static int ss_delay = 50;
-module_param(ss_delay, int, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(ss_delay,
-		 "seconds to delay before entering selective suspend");
-
-static int needs_remote_wakeup;
-module_param(needs_remote_wakeup, int, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(needs_remote_wakeup, "ss state needs remote wakeup supported");
-
-#ifdef SUPPORT_FILE_OP
-static const struct file_operations rts51x_fops = {
-	.owner = THIS_MODULE,
-	.read = rts51x_read,
-	.write = rts51x_write,
-	.unlocked_ioctl = rts51x_ioctl,
-	.open = rts51x_open,
-	.release = rts51x_release,
-};
-
-/*
- * usb class driver info in order to get a minor number from the usb core,
- * and to have the device registered with the driver core
- */
-static struct usb_class_driver rts51x_class = {
-	.name = "rts51x%d",
-	.fops = &rts51x_fops,
-	.minor_base = 192,
-};
-#endif
-
-#ifdef CONFIG_PM		/* Minimal support for suspend and resume */
-
-static inline void usb_autopm_enable(struct usb_interface *intf)
-{
-	atomic_set(&intf->pm_usage_cnt, 1);
-	usb_autopm_put_interface(intf);
-}
-
-static inline void usb_autopm_disable(struct usb_interface *intf)
-{
-	atomic_set(&intf->pm_usage_cnt, 0);
-	usb_autopm_get_interface(intf);
-}
-
-static void rts51x_try_to_enter_ss(struct rts51x_chip *chip)
-{
-	RTS51X_DEBUGP("Ready to enter SS state\n");
-	usb_autopm_enable(chip->usb->pusb_intf);
-}
-
-void rts51x_try_to_exit_ss(struct rts51x_chip *chip)
-{
-	RTS51X_DEBUGP("Exit from SS state\n");
-	usb_autopm_disable(chip->usb->pusb_intf);
-}
-
-int rts51x_suspend(struct usb_interface *iface, pm_message_t message)
-{
-	struct rts51x_chip *chip = usb_get_intfdata(iface);
-
-	RTS51X_DEBUGP("%s, message.event = 0x%x\n", __func__, message.event);
-
-	/* Wait until no command is running */
-	mutex_lock(&chip->usb->dev_mutex);
-
-	chip->fake_card_ready = chip->card_ready;
-	rts51x_do_before_power_down(chip);
-
-	if (message.event == PM_EVENT_AUTO_SUSPEND) {
-		RTS51X_DEBUGP("Enter SS state");
-		chip->resume_from_scsi = 0;
-		RTS51X_SET_STAT(chip, STAT_SS);
-	} else {
-		RTS51X_DEBUGP("Enter SUSPEND state");
-		RTS51X_SET_STAT(chip, STAT_SUSPEND);
-	}
-
-	/* When runtime PM is working, we'll set a flag to indicate
-	 * whether we should autoresume when a SCSI request arrives. */
-
-	mutex_unlock(&chip->usb->dev_mutex);
-	return 0;
-}
-
-int rts51x_resume(struct usb_interface *iface)
-{
-	struct rts51x_chip *chip = usb_get_intfdata(iface);
-
-	RTS51X_DEBUGP("%s\n", __func__);
-
-	if (!RTS51X_CHK_STAT(chip, STAT_SS) || !chip->resume_from_scsi) {
-		mutex_lock(&chip->usb->dev_mutex);
-
-		if (chip->option.ss_en) {
-			if (GET_PM_USAGE_CNT(chip) <= 0) {
-				/* Remote wake up, increase pm_usage_cnt */
-				RTS51X_DEBUGP("Incr pm_usage_cnt\n");
-				SET_PM_USAGE_CNT(chip, 1);
-			}
-		}
-
-		RTS51X_SET_STAT(chip, STAT_RUN);
-
-		rts51x_init_chip(chip);
-		rts51x_init_cards(chip);
-
-		mutex_unlock(&chip->usb->dev_mutex);
-	}
-
-	return 0;
-}
-
-int rts51x_reset_resume(struct usb_interface *iface)
-{
-	struct rts51x_chip *chip = usb_get_intfdata(iface);
-
-	RTS51X_DEBUGP("%s\n", __func__);
-
-	mutex_lock(&chip->usb->dev_mutex);
-
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	if (chip->option.ss_en)
-		SET_PM_USAGE_CNT(chip, 1);
-
-	rts51x_init_chip(chip);
-	rts51x_init_cards(chip);
-
-	mutex_unlock(&chip->usb->dev_mutex);
-
-	/* FIXME: Notify the subdrivers that they need to reinitialize
-	 * the device */
-	return 0;
-}
-
-#else /* CONFIG_PM */
-
-static void rts51x_try_to_enter_ss(struct rts51x_chip *chip)
-{
-}
-
-void rts51x_try_to_exit_ss(struct rts51x_chip *chip)
-{
-}
-
-#endif /* CONFIG_PM */
-
-/*
- * The next two routines get called just before and just after
- * a USB port reset, whether from this driver or a different one.
- */
-
-static int rts51x_pre_reset(struct usb_interface *iface)
-{
-	struct rts51x_chip *chip = usb_get_intfdata(iface);
-
-	RTS51X_DEBUGP("%s\n", __func__);
-
-	/* Make sure no command runs during the reset */
-	mutex_lock(&chip->usb->dev_mutex);
-	return 0;
-}
-
-static int rts51x_post_reset(struct usb_interface *iface)
-{
-	struct rts51x_chip *chip = usb_get_intfdata(iface);
-
-	RTS51X_DEBUGP("%s\n", __func__);
-
-	/* Report the reset to the SCSI core */
-	/* usb_stor_report_bus_reset(us); */
-
-	/* FIXME: Notify the subdrivers that they need to reinitialize
-	 * the device */
-
-	mutex_unlock(&chip->usb->dev_mutex);
-	return 0;
-}
-
-static int rts51x_control_thread(void *__chip)
-{
-	struct rts51x_chip *chip = (struct rts51x_chip *)__chip;
-	struct Scsi_Host *host = rts51x_to_host(chip);
-
-	for (;;) {
-		if (wait_for_completion_interruptible(&chip->usb->cmnd_ready))
-			break;
-
-		if (test_bit(FLIDX_DISCONNECTING, &chip->usb->dflags)) {
-			RTS51X_DEBUGP("-- exiting from rts51x-control\n");
-			break;
-		}
-
-		/* lock the device pointers */
-		mutex_lock(&(chip->usb->dev_mutex));
-
-		/* lock access to the state */
-		scsi_lock(host);
-
-		/* When we are called with no command pending, we're done */
-		if (chip->srb == NULL) {
-			scsi_unlock(host);
-			mutex_unlock(&chip->usb->dev_mutex);
-			RTS51X_DEBUGP("-- exiting from control thread\n");
-			break;
-		}
-
-		/* has the command timed out *already* ? */
-		if (test_bit(FLIDX_TIMED_OUT, &chip->usb->dflags)) {
-			chip->srb->result = DID_ABORT << 16;
-			goto SkipForAbort;
-		}
-
-		scsi_unlock(host);
-
-		/* reject the command if the direction indicator
-		 * is UNKNOWN
-		 */
-		if (chip->srb->sc_data_direction == DMA_BIDIRECTIONAL) {
-			RTS51X_DEBUGP("UNKNOWN data direction\n");
-			chip->srb->result = DID_ERROR << 16;
-		}
-
-		/* reject if target != 0 or if LUN is higher than
-		 * the maximum known LUN
-		 */
-		else if (chip->srb->device->id) {
-			RTS51X_DEBUGP("Bad target number (%d:%d)\n",
-				       chip->srb->device->id,
-				       chip->srb->device->lun);
-			chip->srb->result = DID_BAD_TARGET << 16;
-		}
-
-		else if (chip->srb->device->lun > chip->max_lun) {
-			RTS51X_DEBUGP("Bad LUN (%d:%d)\n",
-				       chip->srb->device->id,
-				       chip->srb->device->lun);
-			chip->srb->result = DID_BAD_TARGET << 16;
-		}
-
-		/* we've got a command, let's do it! */
-		else {
-			RTS51X_DEBUG(rts51x_scsi_show_command(chip->srb));
-			rts51x_invoke_transport(chip->srb, chip);
-		}
-
-		/* lock access to the state */
-		scsi_lock(host);
-
-		/* indicate that the command is done */
-		if (chip->srb->result != DID_ABORT << 16)
-			chip->srb->scsi_done(chip->srb);
-		else
-SkipForAbort :
-			RTS51X_DEBUGP("scsi command aborted\n");
-
-		/* If an abort request was received we need to signal that
-		 * the abort has finished.  The proper test for this is
-		 * the TIMED_OUT flag, not srb->result == DID_ABORT, because
-		 * the timeout might have occurred after the command had
-		 * already completed with a different result code. */
-		if (test_bit(FLIDX_TIMED_OUT, &chip->usb->dflags)) {
-			complete(&(chip->usb->notify));
-
-			/* Allow USB transfers to resume */
-			clear_bit(FLIDX_ABORTING, &chip->usb->dflags);
-			clear_bit(FLIDX_TIMED_OUT, &chip->usb->dflags);
-		}
-
-		/* finished working on this command */
-		chip->srb = NULL;
-		scsi_unlock(host);
-
-		/* unlock the device pointers */
-		mutex_unlock(&chip->usb->dev_mutex);
-	}			/* for (;;) */
-
-	complete(&chip->usb->control_exit);
-
-	/* Wait until we are told to stop */
-/*	for (;;) {
-		set_current_state(TASK_INTERRUPTIBLE);
-		if (kthread_should_stop())
-			break;
-		schedule();
-	}
-	__set_current_state(TASK_RUNNING);*/
-	return 0;
-}
-
-static int rts51x_polling_thread(void *__chip)
-{
-	struct rts51x_chip *chip = (struct rts51x_chip *)__chip;
-
-	for (;;) {
-		wait_timeout(POLLING_INTERVAL);
-
-		/* if the device has disconnected, we are free to exit */
-		if (test_bit(FLIDX_DISCONNECTING, &chip->usb->dflags)) {
-			RTS51X_DEBUGP("-- exiting from rts51x-polling\n");
-			break;
-		}
-
-		/* if the device has disconnected, we are free to exit */
-		/* if (kthread_should_stop()) {
-			printk(KERN_INFO "Stop polling thread!\n");
-			break;
-		} */
-
-#ifdef CONFIG_PM
-		if (RTS51X_CHK_STAT(chip, STAT_SS) ||
-		    RTS51X_CHK_STAT(chip, STAT_SS_PRE) ||
-		    RTS51X_CHK_STAT(chip, STAT_SUSPEND)) {
-			continue;
-		}
-
-		if (ss_en) {
-			if (RTS51X_CHK_STAT(chip, STAT_IDLE)) {
-				if (chip->ss_counter <
-				    (ss_delay * 1000 / POLLING_INTERVAL)) {
-					chip->ss_counter++;
-				} else {
-					/* Prepare SS state */
-					RTS51X_SET_STAT(chip, STAT_SS_PRE);
-					rts51x_try_to_enter_ss(chip);
-					continue;
-				}
-			} else {
-				chip->ss_counter = 0;
-			}
-		}
-#endif
-
-		rts51x_mspro_polling_format_status(chip);
-
-		/* lock the device pointers */
-		mutex_lock(&(chip->usb->dev_mutex));
-
-		rts51x_polling_func(chip);
-
-		/* unlock the device pointers */
-		mutex_unlock(&chip->usb->dev_mutex);
-	}			/* for (;;) */
-
-	complete(&chip->usb->polling_exit);
-
-	/* Wait until we are told to stop */
-	/* for (;;) {
-		set_current_state(TASK_INTERRUPTIBLE);
-		if (kthread_should_stop())
-		break;
-		schedule();
-		}
-	__set_current_state(TASK_RUNNING); */
-	return 0;
-}
-
-/* Associate our private data with the USB device */
-static int associate_dev(struct rts51x_chip *chip, struct usb_interface *intf)
-{
-	struct rts51x_usb *rts51x = chip->usb;
-#ifdef SUPPORT_FILE_OP
-	int retval;
-#endif
-
-	/* Fill in the device-related fields */
-	rts51x->pusb_dev = interface_to_usbdev(intf);
-	rts51x->pusb_intf = intf;
-	rts51x->ifnum = intf->cur_altsetting->desc.bInterfaceNumber;
-	RTS51X_DEBUGP("Vendor: 0x%04x, Product: 0x%04x, Revision: 0x%04x\n",
-		       le16_to_cpu(rts51x->pusb_dev->descriptor.idVendor),
-		       le16_to_cpu(rts51x->pusb_dev->descriptor.idProduct),
-		       le16_to_cpu(rts51x->pusb_dev->descriptor.bcdDevice));
-	RTS51X_DEBUGP("Interface Subclass: 0x%02x, Protocol: 0x%02x\n",
-		       intf->cur_altsetting->desc.bInterfaceSubClass,
-		       intf->cur_altsetting->desc.bInterfaceProtocol);
-
-	/* Store our private data in the interface */
-	usb_set_intfdata(intf, chip);
-
-#ifdef SUPPORT_FILE_OP
-	/* we can register the device now, as it is ready */
-	retval = usb_register_dev(intf, &rts51x_class);
-	if (retval) {
-		/* something prevented us from registering this driver */
-		RTS51X_DEBUGP("Not able to get a minor for this device.");
-		usb_set_intfdata(intf, NULL);
-		return -ENOMEM;
-	}
-#endif
-
-	/* Allocate the device-related DMA-mapped buffers */
-	rts51x->cr = usb_buffer_alloc(rts51x->pusb_dev, sizeof(*rts51x->cr),
-				      GFP_KERNEL, &rts51x->cr_dma);
-	if (!rts51x->cr) {
-		RTS51X_DEBUGP("usb_ctrlrequest allocation failed\n");
-		usb_set_intfdata(intf, NULL);
-		return -ENOMEM;
-	}
-
-	rts51x->iobuf = usb_buffer_alloc(rts51x->pusb_dev, RTS51X_IOBUF_SIZE,
-					 GFP_KERNEL, &rts51x->iobuf_dma);
-	if (!rts51x->iobuf) {
-		RTS51X_DEBUGP("I/O buffer allocation failed\n");
-		usb_set_intfdata(intf, NULL);
-		return -ENOMEM;
-	}
-	return 0;
-}
-
-static void rts51x_init_options(struct rts51x_chip *chip)
-{
-	struct rts51x_option *option = &(chip->option);
-
-	option->rts51x_mspro_formatter_enable = 1;
-
-	option->fpga_sd_sdr104_clk = CLK_100;
-	option->fpga_sd_sdr50_clk = CLK_100;
-	option->fpga_sd_ddr50_clk = CLK_100;
-	option->fpga_sd_hs_clk = CLK_100;
-	option->fpga_mmc_52m_clk = CLK_80;
-	option->fpga_ms_hg_clk = CLK_80;
-	option->fpga_ms_4bit_clk = CLK_80;
-
-	option->asic_sd_sdr104_clk = 98;
-	option->asic_sd_sdr50_clk = 98;
-	option->asic_sd_ddr50_clk = 98;
-	option->asic_sd_hs_clk = 97;
-	option->asic_mmc_52m_clk = 95;
-	option->asic_ms_hg_clk = 116;
-	option->asic_ms_4bit_clk = 77;
-
-	option->sd_ddr_tx_phase = 0;
-	option->mmc_ddr_tx_phase = 1;
-
-	option->sd_speed_prior = 0;
-	option->sd_ctl =
-	    SD_PUSH_POINT_AUTO | SD_SAMPLE_POINT_AUTO | SUPPORT_UHS50_MMC44;
-
-	option->ss_en = ss_en;
-	option->ss_delay = ss_delay;
-
-	option->auto_delink_en = auto_delink_en;
-
-	option->FT2_fast_mode = 0;
-	option->pwr_delay = 800;
-	option->rts51x_xd_rw_step = 0;
-	option->D3318_off_delay = 50;
-	option->delink_delay = 100;
-	option->rts5129_D3318_off_enable = 0;
-	option->sd20_pad_drive = 0;
-	option->reset_or_rw_fail_set_pad_drive = 1;
-	option->debounce_num = 2;
-	option->led_toggle_interval = 6;
-	option->rts51x_xd_rwn_step = 0;
-	option->sd_send_status_en = 0;
-	option->sdr50_tx_phase = 0x01;
-	option->sdr50_rx_phase = 0x05;
-	option->ddr50_tx_phase = 0x09;
-	option->ddr50_rx_phase = 0x06;
-	option->sdr50_phase_sel = 0;
-	option->sd30_pad_drive = 1;
-	option->ms_errreg_fix = 0;
-	option->reset_mmc_first = 0;
-	option->speed_mmc = 1;
-	option->led_always_on = 0;
-}
-
-/* Get the pipe settings */
-static int get_pipes(struct rts51x_chip *chip)
-{
-	struct rts51x_usb *rts51x = chip->usb;
-	struct usb_host_interface *altsetting =
-	    rts51x->pusb_intf->cur_altsetting;
-	int i;
-	struct usb_endpoint_descriptor *ep;
-	struct usb_endpoint_descriptor *ep_in = NULL;
-	struct usb_endpoint_descriptor *ep_out = NULL;
-	struct usb_endpoint_descriptor *ep_int = NULL;
-
-	/*
-	 * Find the first endpoint of each type we need.
-	 * We are expecting a minimum of 2 endpoints - in and out (bulk).
-	 * An optional interrupt-in is OK (necessary for CBI protocol).
-	 * We will ignore any others.
-	 */
-	for (i = 0; i < altsetting->desc.bNumEndpoints; i++) {
-		ep = &altsetting->endpoint[i].desc;
-
-		if (usb_endpoint_xfer_bulk(ep)) {
-			if (usb_endpoint_dir_in(ep)) {
-				if (!ep_in)
-					ep_in = ep;
-			} else {
-				if (!ep_out)
-					ep_out = ep;
-			}
-		}
-
-		else if (usb_endpoint_is_int_in(ep)) {
-			if (!ep_int)
-				ep_int = ep;
-		}
-	}
-
-	if (!ep_in || !ep_out) {
-		RTS51X_DEBUGP("Endpoint sanity check failed!"
-					"Rejecting dev.\n");
-		return -EIO;
-	}
-
-	/* Calculate and store the pipe values */
-	rts51x->send_ctrl_pipe = usb_sndctrlpipe(rts51x->pusb_dev, 0);
-	rts51x->recv_ctrl_pipe = usb_rcvctrlpipe(rts51x->pusb_dev, 0);
-	rts51x->send_bulk_pipe = usb_sndbulkpipe(rts51x->pusb_dev,
-						 usb_endpoint_num(ep_out));
-	rts51x->recv_bulk_pipe = usb_rcvbulkpipe(rts51x->pusb_dev,
-						 usb_endpoint_num(ep_in));
-	if (ep_int) {
-		rts51x->recv_intr_pipe = usb_rcvintpipe(rts51x->pusb_dev,
-							usb_endpoint_num
-							(ep_int));
-		rts51x->ep_bInterval = ep_int->bInterval;
-	}
-	return 0;
-}
-
-/* Initialize all the dynamic resources we need */
-static int rts51x_acquire_resources(struct rts51x_chip *chip)
-{
-	struct rts51x_usb *rts51x = chip->usb;
-	int retval;
-
-	rts51x->current_urb = usb_alloc_urb(0, GFP_KERNEL);
-	if (!rts51x->current_urb) {
-		RTS51X_DEBUGP("URB allocation failed\n");
-		return -ENOMEM;
-	}
-
-	rts51x->intr_urb = usb_alloc_urb(0, GFP_KERNEL);
-	if (!rts51x->intr_urb) {
-		RTS51X_DEBUGP("URB allocation failed\n");
-		return -ENOMEM;
-	}
-
-	chip->cmd_buf = chip->rsp_buf = rts51x->iobuf;
-
-	rts51x_init_options(chip);
-
-	/* Init rts51xx device */
-	retval = rts51x_init_chip(chip);
-	if (retval != STATUS_SUCCESS)
-		return -EIO;
-
-	return 0;
-}
-
-/* Release all our dynamic resources */
-static void rts51x_release_resources(struct rts51x_chip *chip)
-{
-	RTS51X_DEBUGP("-- %s\n", __func__);
-
-	/* Tell the control thread to exit.  The SCSI host must
-	 * already have been removed and the DISCONNECTING flag set
-	 * so that we won't accept any more commands.
-	 */
-	RTS51X_DEBUGP("-- sending exit command to thread\n");
-	complete(&chip->usb->cmnd_ready);
-	if (chip->usb->ctl_thread)
-		wait_for_completion(&chip->usb->control_exit);
-		/* kthread_stop(chip->usb->ctl_thread); */
-	if (chip->usb->polling_thread)
-		wait_for_completion(&chip->usb->polling_exit);
-
-	/* if (chip->usb->polling_thread)
-		kthread_stop(chip->usb->polling_thread); */
-
-	wait_timeout(200);
-
-	/* Release rts51xx device here */
-	rts51x_release_chip(chip);
-
-	usb_free_urb(chip->usb->current_urb);
-	usb_free_urb(chip->usb->intr_urb);
-}
-
-/* Dissociate from the USB device */
-static void dissociate_dev(struct rts51x_chip *chip)
-{
-	struct rts51x_usb *rts51x = chip->usb;
-
-	RTS51X_DEBUGP("-- %s\n", __func__);
-
-	/* Free the device-related DMA-mapped buffers */
-	if (rts51x->cr)
-		usb_buffer_free(rts51x->pusb_dev, sizeof(*rts51x->cr),
-				rts51x->cr, rts51x->cr_dma);
-	if (rts51x->iobuf)
-		usb_buffer_free(rts51x->pusb_dev, RTS51X_IOBUF_SIZE,
-				rts51x->iobuf, rts51x->iobuf_dma);
-
-	/* Remove our private data from the interface */
-	usb_set_intfdata(rts51x->pusb_intf, NULL);
-
-#ifdef SUPPORT_FILE_OP
-	/* give back our minor */
-	usb_deregister_dev(rts51x->pusb_intf, &rts51x_class);
-#endif
-
-	kfree(rts51x);
-	chip->usb = NULL;
-}
-
-/* First stage of disconnect processing: stop SCSI scanning,
- * remove the host, and stop accepting new commands
- */
-static void quiesce_and_remove_host(struct rts51x_chip *chip)
-{
-	struct rts51x_usb *rts51x = chip->usb;
-	struct Scsi_Host *host = rts51x_to_host(chip);
-
-	/* If the device is really gone, cut short reset delays */
-	if (rts51x->pusb_dev->state == USB_STATE_NOTATTACHED)
-		set_bit(FLIDX_DISCONNECTING, &rts51x->dflags);
-
-	/* Removing the host will perform an orderly shutdown: caches
-	 * synchronized, disks spun down, etc.
-	 */
-	scsi_remove_host(host);
-
-	/* Prevent any new commands from being accepted and cut short
-	 * reset delays.
-	 */
-	scsi_lock(host);
-	set_bit(FLIDX_DISCONNECTING, &rts51x->dflags);
-	scsi_unlock(host);
-}
-
-/* Second stage of disconnect processing: deallocate all resources */
-static void release_everything(struct rts51x_chip *chip)
-{
-	rts51x_release_resources(chip);
-	dissociate_dev(chip);
-
-	/* Drop our reference to the host; the SCSI core will free it
-	 * (and "chip" along with it) when the refcount becomes 0. */
-	scsi_host_put(rts51x_to_host(chip));
-}
-
-static int rts51x_probe(struct usb_interface *intf,
-			const struct usb_device_id *id)
-{
-	struct Scsi_Host *host;
-	struct rts51x_chip *chip;
-	struct rts51x_usb *rts51x;
-	int result;
-	struct task_struct *th;
-
-	RTS51X_DEBUGP("%s detected\n", RTS51X_NAME);
-
-	rts51x = kzalloc(sizeof(struct rts51x_usb), GFP_KERNEL);
-	if (!rts51x) {
-		printk(KERN_WARNING RTS51X_TIP
-		       "Unable to allocate rts51x_usb\n");
-		return -ENOMEM;
-	}
-
-	/*
-	 * Ask the SCSI layer to allocate a host structure, with extra
-	 * space at the end for our private us_data structure.
-	 */
-	host = scsi_host_alloc(&rts51x_host_template, sizeof(*chip));
-	if (!host) {
-		printk(KERN_WARNING RTS51X_TIP
-		       "Unable to allocate the scsi host\n");
-		kfree(rts51x);
-		return -ENOMEM;
-	}
-
-	/*
-	 * Allow 16-byte CDBs and thus > 2TB
-	 */
-	host->max_cmd_len = 16;
-	chip = host_to_rts51x(host);
-	memset(chip, 0, sizeof(struct rts51x_chip));
-
-	chip->vendor_id = id->idVendor;
-	chip->product_id = id->idProduct;
-
-	mutex_init(&(rts51x->dev_mutex));
-	init_completion(&rts51x->cmnd_ready);
-	init_completion(&rts51x->control_exit);
-	init_completion(&rts51x->polling_exit);
-	init_completion(&(rts51x->notify));
-
-	chip->usb = rts51x;
-
-	/* Associate the us_data structure with the USB device */
-	result = associate_dev(chip, intf);
-	if (result)
-		goto BadDevice;
-
-	/* Find the endpoints and calculate pipe values */
-	result = get_pipes(chip);
-	if (result)
-		goto BadDevice;
-
-	/* Acquire all the other resources and add the host */
-	result = rts51x_acquire_resources(chip);
-	if (result)
-		goto BadDevice;
-
-	/* Start up our control thread */
-	th = kthread_run(rts51x_control_thread, chip, RTS51X_CTL_THREAD);
-	if (IS_ERR(th)) {
-		printk(KERN_WARNING RTS51X_TIP
-		       "Unable to start control thread\n");
-		result = PTR_ERR(th);
-		goto BadDevice;
-	}
-	rts51x->ctl_thread = th;
-
-	result = scsi_add_host(rts51x_to_host(chip), &rts51x->pusb_intf->dev);
-	if (result) {
-		printk(KERN_WARNING RTS51X_TIP "Unable to add the scsi host\n");
-		goto BadDevice;
-	}
-	scsi_scan_host(rts51x_to_host(chip));
-
-	/* Start up our polling thread */
-	th = kthread_run(rts51x_polling_thread, chip, RTS51X_POLLING_THREAD);
-	if (IS_ERR(th)) {
-		printk(KERN_WARNING RTS51X_TIP
-		       "Unable to start polling thread\n");
-		result = PTR_ERR(th);
-		goto BadDevice;
-	}
-	rts51x->polling_thread = th;
-
-#ifdef CONFIG_PM
-	if (ss_en) {
-		rts51x->pusb_intf->needs_remote_wakeup = needs_remote_wakeup;
-		SET_PM_USAGE_CNT(chip, 1);
-		RTS51X_DEBUGP("pm_usage_cnt = %d\n", GET_PM_USAGE_CNT(chip));
-	}
-#endif
-
-	return 0;
-
-	/* We come here if there are any problems */
-BadDevice:
-	RTS51X_DEBUGP("rts51x_probe() failed\n");
-	release_everything(chip);
-	return result;
-}
-
-static void rts51x_disconnect(struct usb_interface *intf)
-{
-	struct rts51x_chip *chip = (struct rts51x_chip *)usb_get_intfdata(intf);
-
-	RTS51X_DEBUGP("rts51x_disconnect() called\n");
-	quiesce_and_remove_host(chip);
-	release_everything(chip);
-}
-
-/***********************************************************************
- * Initialization and registration
- ***********************************************************************/
-
-static struct usb_device_id rts5139_usb_ids[] = {
-	{USB_DEVICE(0x0BDA, 0x0139)},
-	{USB_DEVICE(0x0BDA, 0x0129)},
-	{}			/* Terminating entry */
-};
-EXPORT_SYMBOL_GPL(rts5139_usb_ids);
-
-MODULE_DEVICE_TABLE(usb, rts5139_usb_ids);
-
-struct usb_driver rts51x_driver = {
-	.name = RTS51X_NAME,
-	.probe = rts51x_probe,
-	.disconnect = rts51x_disconnect,
-	.suspend = rts51x_suspend,
-	.resume = rts51x_resume,
-	.reset_resume = rts51x_reset_resume,
-	.pre_reset = rts51x_pre_reset,
-	.post_reset = rts51x_post_reset,
-	.id_table = rts5139_usb_ids,
-	.soft_unbind = 1,
-};
-
-module_usb_driver(rts51x_driver);
diff --git a/drivers/staging/rts5139/rts51x.h b/drivers/staging/rts5139/rts51x.h
deleted file mode 100644
index ecc0109a5b1..00000000000
--- a/drivers/staging/rts5139/rts51x.h
+++ /dev/null
@@ -1,194 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_H
-#define __RTS51X_H
-
-#include <linux/usb.h>
-#include <linux/usb_usual.h>
-#include <linux/blkdev.h>
-#include <linux/completion.h>
-#include <linux/mutex.h>
-#include <linux/cdrom.h>
-#include <linux/kernel.h>
-
-#include <scsi/scsi.h>
-#include <scsi/scsi_cmnd.h>
-#include <scsi/scsi_device.h>
-#include <scsi/scsi_devinfo.h>
-#include <scsi/scsi_eh.h>
-#include <scsi/scsi_host.h>
-
-#define DRIVER_VERSION		"v1.04"
-
-#define RTS51X_DESC		"Realtek RTS5139/29 USB card reader driver"
-#define RTS51X_NAME		"rts5139"
-#define RTS51X_CTL_THREAD	"rts5139-control"
-#define RTS51X_POLLING_THREAD	"rts5139-polling"
-
-#define POLLING_IN_THREAD
-#define SUPPORT_FILE_OP
-
-#define wait_timeout_x(task_state, msecs)	\
-do {						\
-	set_current_state((task_state));	\
-	schedule_timeout((msecs) * HZ / 1000);	\
-} while (0)
-
-#define wait_timeout(msecs)	wait_timeout_x(TASK_INTERRUPTIBLE, (msecs))
-
-#define SCSI_LUN(srb)		((srb)->device->lun)
-
-/* Size of the DMA-mapped I/O buffer */
-#define RTS51X_IOBUF_SIZE	1024
-
-/* Dynamic bitflag definitions (dflags): used in set_bit() etc. */
-#define FLIDX_URB_ACTIVE	0	/* current_urb is in use    */
-#define FLIDX_SG_ACTIVE		1	/* current_sg is in use     */
-#define FLIDX_ABORTING		2	/* abort is in progress     */
-#define FLIDX_DISCONNECTING	3	/* disconnect in progress   */
-#define FLIDX_RESETTING		4	/* device reset in progress */
-#define FLIDX_TIMED_OUT		5	/* SCSI midlayer timed out  */
-
-struct rts51x_chip;
-
-struct rts51x_usb {
-	/* The device we're working with
-	 * It's important to note:
-	 *    (o) you must hold dev_mutex to change pusb_dev
-	 */
-	struct mutex dev_mutex;	/* protect pusb_dev */
-	struct usb_device *pusb_dev;	/* this usb_device */
-	struct usb_interface *pusb_intf;	/* this interface */
-
-	unsigned long dflags;	/* dynamic atomic bitflags */
-
-	unsigned int send_bulk_pipe;	/* cached pipe values */
-	unsigned int recv_bulk_pipe;
-	unsigned int send_ctrl_pipe;
-	unsigned int recv_ctrl_pipe;
-	unsigned int recv_intr_pipe;
-
-	u8 ifnum;		/* interface number   */
-	u8 ep_bInterval;	/* interrupt interval */
-
-	/* control and bulk communications data */
-	struct urb *current_urb;	/* USB requests         */
-	struct urb *intr_urb;	/* Interrupt USB request */
-	struct usb_ctrlrequest *cr;	/* control requests     */
-	struct usb_sg_request current_sg;	/* scatter-gather req.  */
-	unsigned char *iobuf;	/* I/O buffer           */
-	dma_addr_t cr_dma;	/* buffer DMA addresses */
-	dma_addr_t iobuf_dma;
-	struct task_struct *ctl_thread;	/* the control thread   */
-	struct task_struct *polling_thread;	/* the polling thread   */
-
-	/* mutual exclusion and synchronization structures */
-	struct completion cmnd_ready;	/* to sleep thread on      */
-	struct completion control_exit;	/* control thread exit     */
-	struct completion polling_exit;	/* polling thread exit     */
-	struct completion notify;	/* thread begin/end        */
-};
-
-extern struct usb_driver rts51x_driver;
-
-static inline void get_current_time(u8 *timeval_buf, int buf_len)
-{
-	struct timeval tv;
-
-	if (!timeval_buf || (buf_len < 8))
-		return;
-
-	do_gettimeofday(&tv);
-
-	timeval_buf[0] = (u8) (tv.tv_sec >> 24);
-	timeval_buf[1] = (u8) (tv.tv_sec >> 16);
-	timeval_buf[2] = (u8) (tv.tv_sec >> 8);
-	timeval_buf[3] = (u8) (tv.tv_sec);
-	timeval_buf[4] = (u8) (tv.tv_usec >> 24);
-	timeval_buf[5] = (u8) (tv.tv_usec >> 16);
-	timeval_buf[6] = (u8) (tv.tv_usec >> 8);
-	timeval_buf[7] = (u8) (tv.tv_usec);
-}
-
-#define SND_CTRL_PIPE(chip)	((chip)->usb->send_ctrl_pipe)
-#define RCV_CTRL_PIPE(chip)	((chip)->usb->recv_ctrl_pipe)
-#define SND_BULK_PIPE(chip)	((chip)->usb->send_bulk_pipe)
-#define RCV_BULK_PIPE(chip)	((chip)->usb->recv_bulk_pipe)
-#define RCV_INTR_PIPE(chip)	((chip)->usb->recv_intr_pipe)
-
-/* The scsi_lock() and scsi_unlock() macros protect the sm_state and the
- * single queue element srb for write access */
-#define scsi_unlock(host)	spin_unlock_irq(host->host_lock)
-#define scsi_lock(host)		spin_lock_irq(host->host_lock)
-
-#define GET_PM_USAGE_CNT(chip)	\
-	atomic_read(&((chip)->usb->pusb_intf->pm_usage_cnt))
-#define SET_PM_USAGE_CNT(chip, cnt)	\
-	atomic_set(&((chip)->usb->pusb_intf->pm_usage_cnt), (cnt))
-
-/* Compatible macros while we switch over */
-static inline void *usb_buffer_alloc(struct usb_device *dev, size_t size,
-				     gfp_t mem_flags, dma_addr_t *dma)
-{
-	return usb_alloc_coherent(dev, size, mem_flags, dma);
-}
-
-static inline void usb_buffer_free(struct usb_device *dev, size_t size,
-				   void *addr, dma_addr_t dma)
-{
-	return usb_free_coherent(dev, size, addr, dma);
-}
-
-/* Convert between us_data and the corresponding Scsi_Host */
-static inline struct Scsi_Host *rts51x_to_host(struct rts51x_chip *chip)
-{
-	return container_of((void *)chip, struct Scsi_Host, hostdata);
-}
-
-static inline struct rts51x_chip *host_to_rts51x(struct Scsi_Host *host)
-{
-	return (struct rts51x_chip *)(host->hostdata);
-}
-
-/* struct scsi_cmnd transfer buffer access utilities */
-enum xfer_buf_dir { TO_XFER_BUF, FROM_XFER_BUF };
-
-/* General routines provided by the usb-storage standard core */
-#ifdef CONFIG_PM
-void rts51x_try_to_exit_ss(struct rts51x_chip *chip);
-int rts51x_suspend(struct usb_interface *iface, pm_message_t message);
-int rts51x_resume(struct usb_interface *iface);
-int rts51x_reset_resume(struct usb_interface *iface);
-#else
-#define rts51x_suspend		NULL
-#define rts51x_resume		NULL
-#define rts51x_reset_resume	NULL
-#endif
-
-extern struct scsi_host_template rts51x_host_template;
-
-#endif /* __RTS51X_H */
diff --git a/drivers/staging/rts5139/rts51x_card.c b/drivers/staging/rts5139/rts51x_card.c
deleted file mode 100644
index 03456d9873e..00000000000
--- a/drivers/staging/rts5139/rts51x_card.c
+++ /dev/null
@@ -1,940 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#include <linux/blkdev.h>
-#include <linux/kthread.h>
-#include <linux/sched.h>
-#include <linux/workqueue.h>
-
-#include <scsi/scsi.h>
-#include <scsi/scsi_eh.h>
-#include <scsi/scsi_device.h>
-
-#include "debug.h"
-#include "rts51x.h"
-#include "rts51x_chip.h"
-#include "rts51x_card.h"
-#include "rts51x_transport.h"
-#include "xd.h"
-#include "sd.h"
-#include "ms.h"
-
-void rts51x_do_remaining_work(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	struct xd_info *xd_card = &(chip->xd_card);
-	struct ms_info *ms_card = &(chip->ms_card);
-
-	if (chip->card_ready & SD_CARD) {
-		if (sd_card->seq_mode) {
-			RTS51X_SET_STAT(chip, STAT_RUN);
-			sd_card->counter++;
-		} else {
-			sd_card->counter = 0;
-		}
-	}
-
-	if (chip->card_ready & XD_CARD) {
-		if (xd_card->delay_write.delay_write_flag) {
-			RTS51X_SET_STAT(chip, STAT_RUN);
-			xd_card->counter++;
-		} else {
-			xd_card->counter = 0;
-		}
-	}
-
-	if (chip->card_ready & MS_CARD) {
-		if (CHK_MSPRO(ms_card)) {
-			if (ms_card->seq_mode) {
-				RTS51X_SET_STAT(chip, STAT_RUN);
-				ms_card->counter++;
-			} else {
-				ms_card->counter = 0;
-			}
-		} else {
-			if (ms_card->delay_write.delay_write_flag) {
-				RTS51X_SET_STAT(chip, STAT_RUN);
-				ms_card->counter++;
-			} else {
-				ms_card->counter = 0;
-			}
-		}
-	}
-
-	if (sd_card->counter > POLLING_WAIT_CNT)
-		rts51x_sd_cleanup_work(chip);
-
-	if (xd_card->counter > POLLING_WAIT_CNT)
-		rts51x_xd_cleanup_work(chip);
-
-	if (ms_card->counter > POLLING_WAIT_CNT)
-		rts51x_ms_cleanup_work(chip);
-}
-
-static void do_rts51x_reset_xd_card(struct rts51x_chip *chip)
-{
-	int retval;
-
-	if (chip->card2lun[XD_CARD] >= MAX_ALLOWED_LUN_CNT)
-		return;
-
-	retval = rts51x_reset_xd_card(chip);
-	if (retval == STATUS_SUCCESS) {
-		chip->card_ready |= XD_CARD;
-		chip->card_fail &= ~XD_CARD;
-		chip->rw_card[chip->card2lun[XD_CARD]] = rts51x_xd_rw;
-	} else {
-		chip->card_ready &= ~XD_CARD;
-		chip->card_fail |= XD_CARD;
-		chip->capacity[chip->card2lun[XD_CARD]] = 0;
-		chip->rw_card[chip->card2lun[XD_CARD]] = NULL;
-
-		rts51x_init_cmd(chip);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_OE, XD_OUTPUT_EN, 0);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, POWER_MASK,
-			       POWER_OFF);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_CLK_EN, XD_CLK_EN, 0);
-		rts51x_send_cmd(chip, MODE_C, 100);
-	}
-}
-
-void rts51x_do_rts51x_reset_sd_card(struct rts51x_chip *chip)
-{
-	int retval;
-
-	if (chip->card2lun[SD_CARD] >= MAX_ALLOWED_LUN_CNT)
-		return;
-
-	retval = rts51x_reset_sd_card(chip);
-	if (retval == STATUS_SUCCESS) {
-		chip->card_ready |= SD_CARD;
-		chip->card_fail &= ~SD_CARD;
-		chip->rw_card[chip->card2lun[SD_CARD]] = rts51x_sd_rw;
-	} else {
-		chip->card_ready &= ~SD_CARD;
-		chip->card_fail |= SD_CARD;
-		chip->capacity[chip->card2lun[SD_CARD]] = 0;
-		chip->rw_card[chip->card2lun[SD_CARD]] = NULL;
-
-		rts51x_init_cmd(chip);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_OE, SD_OUTPUT_EN, 0);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, POWER_MASK,
-			       POWER_OFF);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_CLK_EN, SD_CLK_EN, 0);
-		rts51x_send_cmd(chip, MODE_C, 100);
-	}
-}
-
-static void do_rts51x_reset_ms_card(struct rts51x_chip *chip)
-{
-	int retval;
-
-	if (chip->card2lun[MS_CARD] >= MAX_ALLOWED_LUN_CNT)
-		return;
-
-	retval = rts51x_reset_ms_card(chip);
-	if (retval == STATUS_SUCCESS) {
-		chip->card_ready |= MS_CARD;
-		chip->card_fail &= ~MS_CARD;
-		chip->rw_card[chip->card2lun[MS_CARD]] = rts51x_ms_rw;
-	} else {
-		chip->card_ready &= ~MS_CARD;
-		chip->card_fail |= MS_CARD;
-		chip->capacity[chip->card2lun[MS_CARD]] = 0;
-		chip->rw_card[chip->card2lun[MS_CARD]] = NULL;
-
-		rts51x_init_cmd(chip);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_OE, MS_OUTPUT_EN, 0);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, POWER_MASK,
-			       POWER_OFF);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_CLK_EN, MS_CLK_EN, 0);
-		rts51x_send_cmd(chip, MODE_C, 100);
-	}
-}
-
-static void card_cd_debounce(struct rts51x_chip *chip, u8 *need_reset,
-		      u8 *need_release)
-{
-	int retval;
-	u8 release_map = 0, reset_map = 0;
-	u8 value;
-
-	retval = rts51x_get_card_status(chip, &(chip->card_status));
-#ifdef SUPPORT_OCP
-	chip->ocp_stat = (chip->card_status >> 4) & 0x03;
-#endif
-
-	if (retval != STATUS_SUCCESS)
-		goto Exit_Debounce;
-
-	if (chip->card_exist) {
-		retval = rts51x_read_register(chip, CARD_INT_PEND, &value);
-		if (retval != STATUS_SUCCESS) {
-			rts51x_ep0_write_register(chip, MC_FIFO_CTL, FIFO_FLUSH,
-						  FIFO_FLUSH);
-			rts51x_ep0_write_register(chip, SFSM_ED, 0xf8, 0xf8);
-			value = 0;
-		}
-
-		if (chip->card_exist & XD_CARD) {
-			if (!(chip->card_status & XD_CD))
-				release_map |= XD_CARD;
-		} else if (chip->card_exist & SD_CARD) {
-			/* if (!(chip->card_status & SD_CD)) { */
-			if (!(chip->card_status & SD_CD) || (value & SD_INT))
-				release_map |= SD_CARD;
-		} else if (chip->card_exist & MS_CARD) {
-			/* if (!(chip->card_status & MS_CD)) { */
-			if (!(chip->card_status & MS_CD) || (value & MS_INT))
-				release_map |= MS_CARD;
-		}
-	} else {
-		if (chip->card_status & XD_CD)
-			reset_map |= XD_CARD;
-		else if (chip->card_status & SD_CD)
-			reset_map |= SD_CARD;
-		else if (chip->card_status & MS_CD)
-			reset_map |= MS_CARD;
-	}
-
-	if (CHECK_PKG(chip, QFN24) && reset_map) {
-		if (chip->card_exist & XD_CARD) {
-			reset_map = 0;
-			goto Exit_Debounce;
-		}
-	}
-
-	if (reset_map) {
-		int xd_cnt = 0, sd_cnt = 0, ms_cnt = 0;
-		int i;
-
-		for (i = 0; i < (chip->option.debounce_num); i++) {
-			retval =
-			    rts51x_get_card_status(chip, &(chip->card_status));
-			if (retval != STATUS_SUCCESS) {
-				reset_map = release_map = 0;
-				goto Exit_Debounce;
-			}
-			if (chip->card_status & XD_CD)
-				xd_cnt++;
-			else
-				xd_cnt = 0;
-			if (chip->card_status & SD_CD)
-				sd_cnt++;
-			else
-				sd_cnt = 0;
-			if (chip->card_status & MS_CD)
-				ms_cnt++;
-			else
-				ms_cnt = 0;
-			wait_timeout(30);
-		}
-
-		reset_map = 0;
-		if (!(chip->card_exist & XD_CARD)
-		    && (xd_cnt > (chip->option.debounce_num - 1))) {
-			reset_map |= XD_CARD;
-		}
-		if (!(chip->card_exist & SD_CARD)
-		    && (sd_cnt > (chip->option.debounce_num - 1))) {
-			reset_map |= SD_CARD;
-		}
-		if (!(chip->card_exist & MS_CARD)
-		    && (ms_cnt > (chip->option.debounce_num - 1))) {
-			reset_map |= MS_CARD;
-		}
-	}
-	rts51x_write_register(chip, CARD_INT_PEND, XD_INT | MS_INT | SD_INT,
-			      XD_INT | MS_INT | SD_INT);
-
-Exit_Debounce:
-	if (need_reset)
-		*need_reset = reset_map;
-	if (need_release)
-		*need_release = release_map;
-}
-
-void rts51x_init_cards(struct rts51x_chip *chip)
-{
-	u8 need_reset = 0, need_release = 0;
-
-	card_cd_debounce(chip, &need_reset, &need_release);
-
-	if (need_release) {
-		RTS51X_DEBUGP("need_release = 0x%x\n", need_release);
-
-		rts51x_prepare_run(chip);
-		RTS51X_SET_STAT(chip, STAT_RUN);
-
-#ifdef SUPPORT_OCP
-		if (chip->ocp_stat & (MS_OCP_NOW | MS_OCP_EVER)) {
-			rts51x_write_register(chip, OCPCTL, MS_OCP_CLEAR,
-					      MS_OCP_CLEAR);
-			chip->ocp_stat = 0;
-			RTS51X_DEBUGP("Clear OCP status.\n");
-		}
-#endif
-
-		if (need_release & XD_CARD) {
-			chip->card_exist &= ~XD_CARD;
-			chip->card_ejected = 0;
-			if (chip->card_ready & XD_CARD) {
-				rts51x_release_xd_card(chip);
-				chip->rw_card[chip->card2lun[XD_CARD]] = NULL;
-				clear_bit(chip->card2lun[XD_CARD],
-					  &(chip->lun_mc));
-			}
-		}
-
-		if (need_release & SD_CARD) {
-			chip->card_exist &= ~SD_CARD;
-			chip->card_ejected = 0;
-			if (chip->card_ready & SD_CARD) {
-				rts51x_release_sd_card(chip);
-				chip->rw_card[chip->card2lun[SD_CARD]] = NULL;
-				clear_bit(chip->card2lun[SD_CARD],
-					  &(chip->lun_mc));
-			}
-		}
-
-		if (need_release & MS_CARD) {
-			chip->card_exist &= ~MS_CARD;
-			chip->card_ejected = 0;
-			if (chip->card_ready & MS_CARD) {
-				rts51x_release_ms_card(chip);
-				chip->rw_card[chip->card2lun[MS_CARD]] = NULL;
-				clear_bit(chip->card2lun[MS_CARD],
-					  &(chip->lun_mc));
-			}
-		}
-	}
-
-	if (need_reset && !chip->card_ready) {
-		RTS51X_DEBUGP("need_reset = 0x%x\n", need_reset);
-
-		rts51x_prepare_run(chip);
-		RTS51X_SET_STAT(chip, STAT_RUN);
-
-		if (need_reset & XD_CARD) {
-			chip->card_exist |= XD_CARD;
-			do_rts51x_reset_xd_card(chip);
-		} else if (need_reset & SD_CARD) {
-			chip->card_exist |= SD_CARD;
-			rts51x_do_rts51x_reset_sd_card(chip);
-		} else if (need_reset & MS_CARD) {
-			chip->card_exist |= MS_CARD;
-			do_rts51x_reset_ms_card(chip);
-		}
-	}
-}
-
-void rts51x_release_cards(struct rts51x_chip *chip)
-{
-	if (chip->card_ready & SD_CARD) {
-		rts51x_sd_cleanup_work(chip);
-		rts51x_release_sd_card(chip);
-		chip->card_ready &= ~SD_CARD;
-	}
-
-	if (chip->card_ready & XD_CARD) {
-		rts51x_xd_cleanup_work(chip);
-		rts51x_release_xd_card(chip);
-		chip->card_ready &= ~XD_CARD;
-	}
-
-	if (chip->card_ready & MS_CARD) {
-		rts51x_ms_cleanup_work(chip);
-		rts51x_release_ms_card(chip);
-		chip->card_ready &= ~MS_CARD;
-	}
-}
-
-static inline u8 double_depth(u8 depth)
-{
-	return (depth > 1) ? (depth - 1) : depth;
-}
-
-int rts51x_switch_ssc_clock(struct rts51x_chip *chip, int clk)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	u8 N = (u8) (clk - 2), min_N, max_N;
-	u8 mcu_cnt, div, max_div, ssc_depth;
-	int sd_vpclk_phase_reset = 0;
-
-	if (chip->cur_clk == clk)
-		return STATUS_SUCCESS;
-
-	min_N = 60;
-	max_N = 120;
-	max_div = CLK_DIV_4;
-
-	RTS51X_DEBUGP("Switch SSC clock to %dMHz\n", clk);
-
-	if ((clk <= 2) || (N > max_N))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	mcu_cnt = (u8) (60 / clk + 3);
-	if (mcu_cnt > 15)
-		mcu_cnt = 15;
-	/* To make sure that the SSC clock div_n is
-	 * equal or greater than min_N */
-	div = CLK_DIV_1;
-	while ((N < min_N) && (div < max_div)) {
-		N = (N + 2) * 2 - 2;
-		div++;
-	}
-	RTS51X_DEBUGP("N = %d, div = %d\n", N, div);
-
-	if (chip->option.ssc_en) {
-		if (chip->cur_card == SD_CARD) {
-			if (CHK_SD_SDR104(sd_card)) {
-				ssc_depth = chip->option.ssc_depth_sd_sdr104;
-			} else if (CHK_SD_SDR50(sd_card)) {
-				ssc_depth = chip->option.ssc_depth_sd_sdr50;
-			} else if (CHK_SD_DDR50(sd_card)) {
-				ssc_depth =
-				    double_depth(chip->option.
-						 ssc_depth_sd_ddr50);
-			} else if (CHK_SD_HS(sd_card)) {
-				ssc_depth =
-				    double_depth(chip->option.ssc_depth_sd_hs);
-			} else if (CHK_MMC_52M(sd_card)
-				   || CHK_MMC_DDR52(sd_card)) {
-				ssc_depth =
-				    double_depth(chip->option.
-						 ssc_depth_mmc_52m);
-			} else {
-				ssc_depth =
-				    double_depth(chip->option.
-						 ssc_depth_low_speed);
-			}
-		} else if (chip->cur_card == MS_CARD) {
-			if (CHK_MSPRO(ms_card)) {
-				if (CHK_HG8BIT(ms_card)) {
-					ssc_depth =
-					    double_depth(chip->option.
-							 ssc_depth_ms_hg);
-				} else {
-					ssc_depth =
-					    double_depth(chip->option.
-							 ssc_depth_ms_4bit);
-				}
-			} else {
-				if (CHK_MS4BIT(ms_card)) {
-					ssc_depth =
-					    double_depth(chip->option.
-							 ssc_depth_ms_4bit);
-				} else {
-					ssc_depth =
-					    double_depth(chip->option.
-							 ssc_depth_low_speed);
-				}
-			}
-		} else {
-			ssc_depth =
-			    double_depth(chip->option.ssc_depth_low_speed);
-		}
-
-		if (ssc_depth) {
-			if (div == CLK_DIV_2) {
-				/* If clock divided by 2, ssc depth must
-				 * be multiplied by 2 */
-				if (ssc_depth > 1)
-					ssc_depth -= 1;
-				else
-					ssc_depth = SSC_DEPTH_2M;
-			} else if (div == CLK_DIV_4) {
-				/* If clock divided by 4, ssc depth must
-				 * be multiplied by 4 */
-				if (ssc_depth > 2)
-					ssc_depth -= 2;
-				else
-					ssc_depth = SSC_DEPTH_2M;
-			}
-		}
-	} else {
-		/* Disable SSC */
-		ssc_depth = 0;
-	}
-
-	RTS51X_DEBUGP("ssc_depth = %d\n", ssc_depth);
-
-	rts51x_init_cmd(chip);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CLK_DIV, CLK_CHANGE, CLK_CHANGE);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CLK_DIV, 0x3F,
-		       (div << 4) | mcu_cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SSC_CTL1, SSC_RSTB, 0);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SSC_CTL2, SSC_DEPTH_MASK,
-		       ssc_depth);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SSC_DIV_N_0, 0xFF, N);
-	if (sd_vpclk_phase_reset) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_VPCLK0_CTL,
-			       PHASE_NOT_RESET, 0);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_VPCLK0_CTL,
-			       PHASE_NOT_RESET, PHASE_NOT_RESET);
-	}
-
-	retval = rts51x_send_cmd(chip, MODE_C, 2000);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	if (chip->option.ssc_en && ssc_depth)
-		rts51x_write_register(chip, SSC_CTL1, 0xff, 0xD0);
-	else
-		rts51x_write_register(chip, SSC_CTL1, 0xff, 0x50);
-	udelay(100);
-	RTS51X_WRITE_REG(chip, CLK_DIV, CLK_CHANGE, 0);
-
-	chip->cur_clk = clk;
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_switch_normal_clock(struct rts51x_chip *chip, int clk)
-{
-	int retval;
-	u8 sel, div, mcu_cnt;
-	int sd_vpclk_phase_reset = 0;
-
-	if (chip->cur_clk == clk)
-		return STATUS_SUCCESS;
-
-	if (chip->cur_card == SD_CARD) {
-		struct sd_info *sd_card = &(chip->sd_card);
-		if (CHK_SD30_SPEED(sd_card) || CHK_MMC_DDR52(sd_card))
-			sd_vpclk_phase_reset = 1;
-	}
-
-	switch (clk) {
-	case CLK_20:
-		RTS51X_DEBUGP("Switch clock to 20MHz\n");
-		sel = SSC_80;
-		div = CLK_DIV_4;
-		mcu_cnt = 5;
-		break;
-
-	case CLK_30:
-		RTS51X_DEBUGP("Switch clock to 30MHz\n");
-		sel = SSC_60;
-		div = CLK_DIV_2;
-		mcu_cnt = 4;
-		break;
-
-	case CLK_40:
-		RTS51X_DEBUGP("Switch clock to 40MHz\n");
-		sel = SSC_80;
-		div = CLK_DIV_2;
-		mcu_cnt = 3;
-		break;
-
-	case CLK_50:
-		RTS51X_DEBUGP("Switch clock to 50MHz\n");
-		sel = SSC_100;
-		div = CLK_DIV_2;
-		mcu_cnt = 3;
-		break;
-
-	case CLK_60:
-		RTS51X_DEBUGP("Switch clock to 60MHz\n");
-		sel = SSC_60;
-		div = CLK_DIV_1;
-		mcu_cnt = 3;
-		break;
-
-	case CLK_80:
-		RTS51X_DEBUGP("Switch clock to 80MHz\n");
-		sel = SSC_80;
-		div = CLK_DIV_1;
-		mcu_cnt = 2;
-		break;
-
-	case CLK_100:
-		RTS51X_DEBUGP("Switch clock to 100MHz\n");
-		sel = SSC_100;
-		div = CLK_DIV_1;
-		mcu_cnt = 2;
-		break;
-
-	/* case CLK_120:
-		RTS51X_DEBUGP("Switch clock to 120MHz\n");
-		sel = SSC_120;
-		div = CLK_DIV_1;
-		mcu_cnt = 2;
-		break;
-
-	case CLK_150:
-		RTS51X_DEBUGP("Switch clock to 150MHz\n");
-		sel = SSC_150;
-		div = CLK_DIV_1;
-		mcu_cnt = 2;
-		break; */
-
-	default:
-		RTS51X_DEBUGP("Try to switch to an illegal clock (%d)\n",
-			       clk);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (!sd_vpclk_phase_reset) {
-		rts51x_init_cmd(chip);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CLK_DIV, CLK_CHANGE,
-			       CLK_CHANGE);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CLK_DIV, 0x3F,
-			       (div << 4) | mcu_cnt);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SSC_CLK_FPGA_SEL, 0xFF,
-			       sel);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CLK_DIV, CLK_CHANGE, 0);
-
-		retval = rts51x_send_cmd(chip, MODE_C, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	} else {
-		rts51x_init_cmd(chip);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CLK_DIV, CLK_CHANGE,
-			       CLK_CHANGE);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_VPCLK0_CTL,
-			       PHASE_NOT_RESET, 0);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_VPCLK1_CTL,
-			       PHASE_NOT_RESET, 0);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CLK_DIV, 0x3F,
-			       (div << 4) | mcu_cnt);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SSC_CLK_FPGA_SEL, 0xFF,
-			       sel);
-
-		retval = rts51x_send_cmd(chip, MODE_C, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		udelay(200);
-
-		rts51x_init_cmd(chip);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_VPCLK0_CTL,
-			       PHASE_NOT_RESET, PHASE_NOT_RESET);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_VPCLK1_CTL,
-			       PHASE_NOT_RESET, PHASE_NOT_RESET);
-
-		retval = rts51x_send_cmd(chip, MODE_C, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		udelay(200);
-
-		RTS51X_WRITE_REG(chip, CLK_DIV, CLK_CHANGE, 0);
-	}
-
-	chip->cur_clk = clk;
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_card_rw(struct scsi_cmnd *srb, struct rts51x_chip *chip,
-		u32 sec_addr, u16 sec_cnt)
-{
-	int retval;
-	unsigned int lun = SCSI_LUN(srb);
-	int i;
-
-	if (chip->rw_card[lun] == NULL)
-		return STATUS_FAIL;
-
-	RTS51X_DEBUGP("%s card, sector addr: 0x%x, sector cnt: %d\n",
-		       (srb->sc_data_direction ==
-			DMA_TO_DEVICE) ? "Write" : "Read", sec_addr, sec_cnt);
-
-	chip->rw_need_retry = 0;
-	for (i = 0; i < 3; i++) {
-		retval = chip->rw_card[lun] (srb, chip, sec_addr, sec_cnt);
-		if (retval != STATUS_SUCCESS) {
-			CATCH_TRIGGER(chip);
-			if (chip->option.reset_or_rw_fail_set_pad_drive) {
-				rts51x_write_register(chip, CARD_DRIVE_SEL,
-						      SD20_DRIVE_MASK,
-						      DRIVE_8mA);
-			}
-		}
-
-		if (!chip->rw_need_retry)
-			break;
-
-		RTS51X_DEBUGP("Retry RW, (i = %d\n)", i);
-	}
-
-	return retval;
-}
-
-u8 rts51x_get_lun_card(struct rts51x_chip *chip, unsigned int lun)
-{
-	if ((chip->card_ready & chip->lun2card[lun]) == XD_CARD)
-		return (u8) XD_CARD;
-	else if ((chip->card_ready & chip->lun2card[lun]) == SD_CARD)
-		return (u8) SD_CARD;
-	else if ((chip->card_ready & chip->lun2card[lun]) == MS_CARD)
-		return (u8) MS_CARD;
-
-	return 0;
-}
-
-static int card_share_mode(struct rts51x_chip *chip, int card)
-{
-	u8 value;
-
-	if (card == SD_CARD)
-		value = CARD_SHARE_SD;
-	else if (card == MS_CARD)
-		value = CARD_SHARE_MS;
-	else if (card == XD_CARD)
-		value = CARD_SHARE_XD;
-	else
-		TRACE_RET(chip, STATUS_FAIL);
-
-	RTS51X_WRITE_REG(chip, CARD_SHARE_MODE, CARD_SHARE_MASK, value);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_select_card(struct rts51x_chip *chip, int card)
-{
-	int retval;
-
-	if (chip->cur_card != card) {
-		u8 mod;
-
-		if (card == SD_CARD)
-			mod = SD_MOD_SEL;
-		else if (card == MS_CARD)
-			mod = MS_MOD_SEL;
-		else if (card == XD_CARD)
-			mod = XD_MOD_SEL;
-		else
-			TRACE_RET(chip, STATUS_FAIL);
-		RTS51X_WRITE_REG(chip, CARD_SELECT, 0x07, mod);
-		chip->cur_card = card;
-
-		retval = card_share_mode(chip, card);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-void rts51x_eject_card(struct rts51x_chip *chip, unsigned int lun)
-{
-	RTS51X_DEBUGP("eject card\n");
-	RTS51X_SET_STAT(chip, STAT_RUN);
-	rts51x_do_remaining_work(chip);
-
-	if ((chip->card_ready & chip->lun2card[lun]) == SD_CARD) {
-		rts51x_release_sd_card(chip);
-		chip->card_ejected |= SD_CARD;
-		chip->card_ready &= ~SD_CARD;
-		chip->capacity[lun] = 0;
-	} else if ((chip->card_ready & chip->lun2card[lun]) == XD_CARD) {
-		rts51x_release_xd_card(chip);
-		chip->card_ejected |= XD_CARD;
-		chip->card_ready &= ~XD_CARD;
-		chip->capacity[lun] = 0;
-	} else if ((chip->card_ready & chip->lun2card[lun]) == MS_CARD) {
-		rts51x_release_ms_card(chip);
-		chip->card_ejected |= MS_CARD;
-		chip->card_ready &= ~MS_CARD;
-		chip->capacity[lun] = 0;
-	}
-	rts51x_write_register(chip, CARD_INT_PEND, XD_INT | MS_INT | SD_INT,
-			      XD_INT | MS_INT | SD_INT);
-}
-
-void rts51x_trans_dma_enable(enum dma_data_direction dir,
-		struct rts51x_chip *chip, u32 byte_cnt, u8 pack_size)
-{
-	if (pack_size > DMA_1024)
-		pack_size = DMA_512;
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       RING_BUFFER);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MC_DMA_TC3, 0xFF,
-		       (u8) (byte_cnt >> 24));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MC_DMA_TC2, 0xFF,
-		       (u8) (byte_cnt >> 16));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MC_DMA_TC1, 0xFF,
-		       (u8) (byte_cnt >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MC_DMA_TC0, 0xFF, (u8) byte_cnt);
-
-	if (dir == DMA_FROM_DEVICE) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MC_DMA_CTL,
-			       0x03 | DMA_PACK_SIZE_MASK,
-			       DMA_DIR_FROM_CARD | DMA_EN | pack_size);
-	} else {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MC_DMA_CTL,
-			       0x03 | DMA_PACK_SIZE_MASK,
-			       DMA_DIR_TO_CARD | DMA_EN | pack_size);
-	}
-}
-
-int rts51x_enable_card_clock(struct rts51x_chip *chip, u8 card)
-{
-	u8 clk_en = 0;
-
-	if (card & XD_CARD)
-		clk_en |= XD_CLK_EN;
-	if (card & SD_CARD)
-		clk_en |= SD_CLK_EN;
-	if (card & MS_CARD)
-		clk_en |= MS_CLK_EN;
-
-	RTS51X_WRITE_REG(chip, CARD_CLK_EN, clk_en, clk_en);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_card_power_on(struct rts51x_chip *chip, u8 card)
-{
-	u8 mask, val1, val2;
-
-	mask = POWER_MASK;
-	val1 = PARTIAL_POWER_ON;
-	val2 = POWER_ON;
-
-#ifdef SD_XD_IO_FOLLOW_PWR
-	if ((card == SD_CARD) || (card == XD_CARD)) {
-		RTS51X_WRITE_REG(chip, CARD_PWR_CTL, mask | LDO3318_PWR_MASK,
-				 val1 | LDO_SUSPEND);
-	} else {
-#endif
-		RTS51X_WRITE_REG(chip, CARD_PWR_CTL, mask, val1);
-#ifdef SD_XD_IO_FOLLOW_PWR
-	}
-#endif
-	udelay(chip->option.pwr_delay);
-	RTS51X_WRITE_REG(chip, CARD_PWR_CTL, mask, val2);
-#ifdef SD_XD_IO_FOLLOW_PWR
-	if (card == SD_CARD) {
-		rts51x_write_register(chip, CARD_PWR_CTL, LDO3318_PWR_MASK,
-				      LDO_ON);
-	}
-#endif
-
-	return STATUS_SUCCESS;
-}
-
-int monitor_card_cd(struct rts51x_chip *chip, u8 card)
-{
-	int retval;
-	u8 card_cd[32] = { 0 };
-
-	card_cd[SD_CARD] = SD_CD;
-	card_cd[XD_CARD] = XD_CD;
-	card_cd[MS_CARD] = MS_CD;
-
-	retval = rts51x_get_card_status(chip, &(chip->card_status));
-	if (retval != STATUS_SUCCESS)
-		return CD_NOT_EXIST;
-
-	if (chip->card_status & card_cd[card])
-		return CD_EXIST;
-
-	return CD_NOT_EXIST;
-}
-
-int rts51x_toggle_gpio(struct rts51x_chip *chip, u8 gpio)
-{
-	int retval;
-	u8 temp_reg;
-	u8 gpio_output[4] = {
-		0x01,
-	};
-	u8 gpio_oe[4] = {
-		0x02,
-	};
-	if (chip->rts5179) {
-		retval = rts51x_ep0_read_register(chip, CARD_GPIO, &temp_reg);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
-		temp_reg ^= gpio_oe[gpio];
-		temp_reg &= 0xfe; /* bit 0 always set 0 */
-		retval =
-		    rts51x_ep0_write_register(chip, CARD_GPIO, 0x03, temp_reg);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
-	} else {
-		retval = rts51x_ep0_read_register(chip, CARD_GPIO, &temp_reg);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
-		temp_reg ^= gpio_output[gpio];
-		retval =
-		    rts51x_ep0_write_register(chip, CARD_GPIO, 0xFF,
-					      temp_reg | gpio_oe[gpio]);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_turn_on_led(struct rts51x_chip *chip, u8 gpio)
-{
-	int retval;
-	u8 gpio_oe[4] = {
-		0x02,
-	};
-	u8 gpio_mask[4] = {
-		0x03,
-	};
-
-	retval =
-	    rts51x_ep0_write_register(chip, CARD_GPIO, gpio_mask[gpio],
-				      gpio_oe[gpio]);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_turn_off_led(struct rts51x_chip *chip, u8 gpio)
-{
-	int retval;
-	u8 gpio_output[4] = {
-		0x01,
-	};
-	u8 gpio_oe[4] = {
-		0x02,
-	};
-	u8 gpio_mask[4] = {
-		0x03,
-	};
-
-	retval =
-	    rts51x_ep0_write_register(chip, CARD_GPIO, gpio_mask[gpio],
-				      gpio_oe[gpio] | gpio_output[gpio]);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
diff --git a/drivers/staging/rts5139/rts51x_card.h b/drivers/staging/rts5139/rts51x_card.h
deleted file mode 100644
index df8816e0f84..00000000000
--- a/drivers/staging/rts5139/rts51x_card.h
+++ /dev/null
@@ -1,870 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_CARD_H
-#define __RTS51X_CARD_H
-
-#include "rts51x_chip.h"
-
-/* Register bit definition */
-
-/* Card Power Control Register */
-#define POWER_OFF			0x03
-#define PARTIAL_POWER_ON		0x02
-#define POWER_ON			0x00
-#define POWER_MASK			0x03
-#define LDO3318_PWR_MASK		0x0C
-#define LDO_ON				0x00
-#define LDO_SUSPEND			0x08
-#define LDO_OFF				0x0C
-#define DV3318_AUTO_PWR_OFF		0x10
-#define FORCE_LDO_POWERB	0x60
-
-/* Card Output Enable Register */
-#define XD_OUTPUT_EN			0x02
-#define SD_OUTPUT_EN			0x04
-#define MS_OUTPUT_EN			0x08
-
-/* System Clock Control Register */
-
-/* System Clock Divider Register */
-#define CLK_CHANGE			0x80
-#define CLK_DIV_1			0x00
-#define CLK_DIV_2			0x01
-#define CLK_DIV_4			0x02
-#define CLK_DIV_8			0x03
-
-/* System Clock Select Register */
-#define SSC_60				0
-#define SSC_80				1
-#define SSC_100				2
-#define SSC_120				3
-#define SSC_150				4
-
-/* Card Clock Enable Register */
-#define XD_CLK_EN			0x02
-#define SD_CLK_EN			0x04
-#define MS_CLK_EN			0x08
-
-/* Card Select Register */
-#define XD_MOD_SEL			1
-#define SD_MOD_SEL			2
-#define MS_MOD_SEL			3
-
-/* Card Transfer Reset Register */
-#define XD_STOP				0x02
-#define SD_STOP				0x04
-#define MS_STOP				0x08
-#define XD_CLR_ERR			0x20
-#define SD_CLR_ERR			0x40
-#define MS_CLR_ERR			0x80
-
-/* SD30_drive_sel */
-#define SD30_DRIVE_MASK	0x07
-
-/* CARD_DRIVE_SEL */
-#define SD20_DRIVE_MASK	0x03
-#define DRIVE_4mA			0x00
-#define DRIVE_8mA			0x01
-#define DRIVE_12mA			0x02
-
-/* FPGA_PULL_CTL */
-#define FPGA_MS_PULL_CTL_EN		0xEF
-#define FPGA_SD_PULL_CTL_EN		0xF7
-#define FPGA_XD_PULL_CTL_EN1		0xFE
-#define FPGA_XD_PULL_CTL_EN2		0xFD
-#define FPGA_XD_PULL_CTL_EN3		0xFB
-
-#define FPGA_MS_PULL_CTL_BIT		0x10
-#define FPGA_SD_PULL_CTL_BIT		0x08
-
-/* Card Data Source Register */
-#define PINGPONG_BUFFER			0x01
-#define RING_BUFFER			0x00
-
-/* SFSM_ED */
-#define HW_CMD_STOP			0x80
-#define CLR_STAGE_STALL			0x08
-#define CARD_ERR				0x10
-
-/* CARD_SHARE_MODE */
-#define	CARD_SHARE_LQFP48		0x04
-#define	CARD_SHARE_QFN24		0x00
-#define CARD_SHARE_LQFP_SEL		0x04
-#define	CARD_SHARE_XD			0x00
-#define	CARD_SHARE_SD			0x01
-#define	CARD_SHARE_MS			0x02
-#define CARD_SHARE_MASK			0x03
-
-/* CARD_AUTO_BLINK */
-#define BLINK_ENABLE			0x08
-#define BLINK_SPEED_MASK		0x07
-
-/* CARD_GPIO */
-#define GPIO_OE				0x02
-#define GPIO_OUTPUT			0x01
-
-/* CARD_CLK_SOURCE */
-#define CRC_FIX_CLK			(0x00 << 0)
-#define CRC_VAR_CLK0			(0x01 << 0)
-#define CRC_VAR_CLK1			(0x02 << 0)
-#define SD30_FIX_CLK			(0x00 << 2)
-#define SD30_VAR_CLK0			(0x01 << 2)
-#define SD30_VAR_CLK1			(0x02 << 2)
-#define SAMPLE_FIX_CLK			(0x00 << 4)
-#define SAMPLE_VAR_CLK0			(0x01 << 4)
-#define SAMPLE_VAR_CLK1			(0x02 << 4)
-
-/* DCM_DRP_CTL */
-#define DCM_RESET			0x08
-#define DCM_LOCKED			0x04
-#define DCM_208M			0x00
-#define DCM_TX			        0x01
-#define DCM_RX			        0x02
-
-/* DCM_DRP_TRIG */
-#define DRP_START			0x80
-#define DRP_DONE			0x40
-
-/* DCM_DRP_CFG */
-#define DRP_WRITE			0x80
-#define DRP_READ			0x00
-#define DCM_WRITE_ADDRESS_50		0x50
-#define DCM_WRITE_ADDRESS_51		0x51
-#define DCM_READ_ADDRESS_00		0x00
-#define DCM_READ_ADDRESS_51		0x51
-
-/* HW_VERSION */
-#define FPGA_VER			0x80
-#define HW_VER_MASK			0x0F
-
-/* CD_DEGLITCH_EN */
-#define DISABLE_SD_CD			0x08
-#define DISABLE_MS_CD			0x10
-#define DISABLE_XD_CD			0x20
-#define SD_CD_DEGLITCH_EN		0x01
-#define MS_CD_DEGLITCH_EN		0x02
-#define XD_CD_DEGLITCH_EN		0x04
-
-/* OCPCTL */
-#define CARD_OC_DETECT_EN		0x08
-#define CARD_OC_CLR			0x01
-
-/* CARD_DMA1_CTL */
-#define EXTEND_DMA1_ASYNC_SIGNAL	0x02
-
-/* HS_USB_STAT */
-#define USB_HI_SPEED			0x01
-
-/* CFG_MODE_1 */
-#define RTS5179				0x02
-
-/* SYS_DUMMY0 */
-#define NYET_EN				0x01
-#define NYET_MSAK			0x01
-
-/* SSC_CTL1 */
-#define SSC_RSTB			0x80
-#define SSC_8X_EN			0x40
-#define SSC_FIX_FRAC			0x20
-#define SSC_SEL_1M			0x00
-#define SSC_SEL_2M			0x08
-#define SSC_SEL_4M			0x10
-#define SSC_SEL_8M			0x18
-
-/* SSC_CTL2 */
-#define SSC_DEPTH_MASK			0x03
-#define SSC_DEPTH_DISALBE		0x00
-#define SSC_DEPTH_2M			0x01
-#define SSC_DEPTH_1M			0x02
-#define SSC_DEPTH_512K			0x03
-
-/* LDO_POWER_CFG */
-#define TUNE_SD18_MASK			0x1C
-#define TUNE_SD18_1V8			(0x01 << 2)
-#define TUNE_SD18_3V3			(0x07 << 2)
-
-/* XD_CP_WAITTIME */
-#define WAIT_1F				0x00
-#define WAIT_3F				0x01
-#define WAIT_7F				0x02
-#define WAIT_FF				0x03
-
-/* XD_INIT */
-#define	XD_PWR_OFF_DELAY0		0x00
-#define	XD_PWR_OFF_DELAY1		0x02
-#define	XD_PWR_OFF_DELAY2		0x04
-#define	XD_PWR_OFF_DELAY3		0x06
-#define	XD_AUTO_PWR_OFF_EN		0xF7
-#define	XD_NO_AUTO_PWR_OFF		0x08
-
-/* XD_DTCTL */
-/* XD_CATCTL */
-#define	XD_TIME_RWN_1			0x00
-#define	XD_TIME_RWN_STEP		0x20
-#define	XD_TIME_RW_1			0x00
-#define	XD_TIME_RW_STEP			0x04
-#define	XD_TIME_SETUP_1			0x00
-#define	XD_TIME_SETUP_STEP		0x01
-
-/* XD_CTL */
-#define	XD_ECC2_UNCORRECTABLE		0x80
-#define	XD_ECC2_ERROR			0x40
-#define	XD_ECC1_UNCORRECTABLE		0x20
-#define	XD_ECC1_ERROR			0x10
-#define	XD_RDY				0x04
-#define	XD_CE_EN			0xFD
-#define	XD_CE_DISEN			0x02
-#define	XD_WP_EN			0xFE
-#define	XD_WP_DISEN			0x01
-
-/* XD_TRANSFER */
-#define	XD_TRANSFER_START		0x80
-#define	XD_TRANSFER_END			0x40
-#define	XD_PPB_EMPTY			0x20
-#define	XD_ERR				0x10
-#define	XD_RESET			0x00
-#define	XD_ERASE			0x01
-#define	XD_READ_STATUS			0x02
-#define	XD_READ_ID			0x03
-#define	XD_READ_REDUNDANT		0x04
-#define	XD_READ_PAGES			0x05
-#define	XD_SET_CMD			0x06
-#define	XD_NORMAL_READ			0x07
-#define	XD_WRITE_PAGES			0x08
-#define	XD_NORMAL_WRITE			0x09
-#define	XD_WRITE_REDUNDANT		0x0A
-#define	XD_SET_ADDR			0x0B
-#define XD_COPY_PAGES			0x0C
-
-/* XD_CFG */
-#define	XD_PPB_TO_SIE			0x80
-#define	XD_TO_PPB_ONLY			0x00
-#define	XD_BA_TRANSFORM			0x40
-#define	XD_BA_NO_TRANSFORM		0x00
-#define	XD_NO_CALC_ECC			0x20
-#define	XD_CALC_ECC			0x00
-#define	XD_IGNORE_ECC			0x10
-#define	XD_CHECK_ECC			0x00
-#define	XD_DIRECT_TO_RB			0x08
-#define XD_ADDR_MASK			0x07
-#define	XD_ADDR_LENGTH_0		0x00
-#define	XD_ADDR_LENGTH_1		0x01
-#define	XD_ADDR_LENGTH_2		0x02
-#define	XD_ADDR_LENGTH_3		0x03
-#define	XD_ADDR_LENGTH_4		0x04
-
-/* XD_PAGE_STATUS */
-#define	XD_GPG				0xFF
-#define	XD_BPG				0x00
-
-/* XD_BLOCK_STATUS */
-#define	XD_GBLK				0xFF
-#define	XD_LATER_BBLK			0xF0
-
-/* XD_PARITY */
-#define	XD_ECC2_ALL1			0x80
-#define	XD_ECC1_ALL1			0x40
-#define	XD_BA2_ALL0			0x20
-#define	XD_BA1_ALL0			0x10
-#define	XD_BA1_BA2_EQL			0x04
-#define	XD_BA2_VALID			0x02
-#define	XD_BA1_VALID			0x01
-
-/* XD_CHK_DATA_STATUS */
-#define	XD_PGSTS_ZEROBIT_OVER4		0x00
-#define	XD_PGSTS_NOT_FF			0x02
-#define	XD_AUTO_CHK_DATA_STATUS		0x01
-
-/* SD_CFG1 */
-#define SD_CLK_DIVIDE_0			0x00
-#define	SD_CLK_DIVIDE_256		0xC0
-#define	SD_CLK_DIVIDE_128		0x80
-#define SD_CLK_DIVIDE_MASK		0xC0
-#define	SD_BUS_WIDTH_1			0x00
-#define	SD_BUS_WIDTH_4			0x01
-#define	SD_BUS_WIDTH_8			0x02
-#define	SD_ASYNC_FIFO_RST		0x10
-#define	SD_20_MODE			0x00
-#define	SD_DDR_MODE			0x04
-#define	SD_30_MODE			0x08
-
-/* SD_CFG2 */
-#define	SD_CALCULATE_CRC7		0x00
-#define	SD_NO_CALCULATE_CRC7		0x80
-#define	SD_CHECK_CRC16			0x00
-#define	SD_NO_CHECK_CRC16		0x40
-#define SD_WAIT_CRC_TO_EN		0x20
-#define	SD_WAIT_BUSY_END		0x08
-#define	SD_NO_WAIT_BUSY_END		0x00
-#define	SD_CHECK_CRC7			0x00
-#define	SD_NO_CHECK_CRC7		0x04
-#define	SD_RSP_LEN_0			0x00
-#define	SD_RSP_LEN_6			0x01
-#define	SD_RSP_LEN_17			0x02
-/* SD/MMC Response Type Definition */
-/* SD_CALCULATE_CRC7, SD_CHECK_CRC16,
- * SD_NO_WAIT_BUSY_END, SD_NO_CHECK_CRC7,
- * SD_RSP_LEN_0 */
-#define	SD_RSP_TYPE_R0			0x04
-/* SD_CALCULATE_CRC7, SD_CHECK_CRC16,
- * SD_NO_WAIT_BUSY_END, SD_CHECK_CRC7,
- * SD_RSP_LEN_6 */
-#define	SD_RSP_TYPE_R1			0x01
-/* SD_CALCULATE_CRC7, SD_CHECK_CRC16,
- * SD_WAIT_BUSY_END, SD_CHECK_CRC7,
- * SD_RSP_LEN_6 */
-#define	SD_RSP_TYPE_R1b			0x09
-/* SD_CALCULATE_CRC7, SD_CHECK_CRC16,
- * SD_NO_WAIT_BUSY_END, SD_CHECK_CRC7,
- * SD_RSP_LEN_17 */
-#define	SD_RSP_TYPE_R2			0x02
-/* SD_CALCULATE_CRC7, SD_CHECK_CRC16,
- * SD_NO_WAIT_BUSY_END, SD_NO_CHECK_CRC7,
- * SD_RSP_LEN_6 */
-#define	SD_RSP_TYPE_R3			0x05
-/* SD_CALCULATE_CRC7, SD_CHECK_CRC16,
- * SD_NO_WAIT_BUSY_END, SD_NO_CHECK_CRC7,
- * SD_RSP_LEN_6 */
-#define	SD_RSP_TYPE_R4			0x05
-/* SD_CALCULATE_CRC7, SD_CHECK_CRC16,
- * SD_NO_WAIT_BUSY_END, SD_CHECK_CRC7,
- * SD_RSP_LEN_6 */
-#define	SD_RSP_TYPE_R5			0x01
-/* SD_CALCULATE_CRC7, SD_CHECK_CRC16,
- * SD_NO_WAIT_BUSY_END, SD_CHECK_CRC7,
- * SD_RSP_LEN_6 */
-#define	SD_RSP_TYPE_R6			0x01
-/* SD_CALCULATE_CRC7, SD_CHECK_CRC16,
- * SD_NO_WAIT_BUSY_END, SD_CHECK_CRC7,
- * SD_RSP_LEN_6  */
-#define	SD_RSP_TYPE_R7			0x01
-
-/* SD_CFG3 */
-#define	SD_RSP_80CLK_TIMEOUT_EN		0x01
-
-/* SD_STAT1 */
-#define	SD_CRC7_ERR			0x80
-#define	SD_CRC16_ERR			0x40
-#define	SD_CRC_WRITE_ERR		0x20
-#define	SD_CRC_WRITE_ERR_MASK		0x1C
-#define	GET_CRC_TIME_OUT		0x02
-#define	SD_TUNING_COMPARE_ERR		0x01
-
-/* SD_STAT2 */
-#define	SD_RSP_80CLK_TIMEOUT		0x01
-
-/* SD_BUS_STAT */
-#define	SD_CLK_TOGGLE_EN		0x80
-#define	SD_CLK_FORCE_STOP	        0x40
-#define	SD_DAT3_STATUS		        0x10
-#define	SD_DAT2_STATUS		        0x08
-#define	SD_DAT1_STATUS		        0x04
-#define	SD_DAT0_STATUS		        0x02
-#define	SD_CMD_STATUS			0x01
-
-/* SD_PAD_CTL */
-#define	SD_IO_USING_1V8		        0x80
-#define	SD_IO_USING_3V3		        0x7F
-#define	TYPE_A_DRIVING		        0x00
-#define	TYPE_B_DRIVING			0x01
-#define	TYPE_C_DRIVING			0x02
-#define	TYPE_D_DRIVING		        0x03
-
-/* SD_SAMPLE_POINT_CTL */
-#define	DDR_FIX_RX_DAT			0x00
-#define	DDR_VAR_RX_DAT			0x80
-#define	DDR_FIX_RX_DAT_EDGE		0x00
-#define	DDR_FIX_RX_DAT_14_DELAY		0x40
-#define	DDR_FIX_RX_CMD			0x00
-#define	DDR_VAR_RX_CMD			0x20
-#define	DDR_FIX_RX_CMD_POS_EDGE		0x00
-#define	DDR_FIX_RX_CMD_14_DELAY		0x10
-#define	SD20_RX_POS_EDGE		0x00
-#define	SD20_RX_14_DELAY		0x08
-#define SD20_RX_SEL_MASK		0x08
-
-/* SD_PUSH_POINT_CTL */
-#define	DDR_FIX_TX_CMD_DAT		0x00
-#define	DDR_VAR_TX_CMD_DAT		0x80
-#define	DDR_FIX_TX_DAT_14_TSU		0x00
-#define	DDR_FIX_TX_DAT_12_TSU		0x40
-#define	DDR_FIX_TX_CMD_NEG_EDGE		0x00
-#define	DDR_FIX_TX_CMD_14_AHEAD		0x20
-#define	SD20_TX_NEG_EDGE		0x00
-#define	SD20_TX_14_AHEAD		0x10
-#define SD20_TX_SEL_MASK		0x10
-#define	DDR_VAR_SDCLK_POL_SWAP		0x01
-
-/* SD_TRANSFER */
-#define	SD_TRANSFER_START		0x80
-#define	SD_TRANSFER_END			0x40
-#define SD_STAT_IDLE			0x20
-#define	SD_TRANSFER_ERR			0x10
-/* SD Transfer Mode definition */
-#define	SD_TM_NORMAL_WRITE		0x00
-#define	SD_TM_AUTO_WRITE_3		0x01
-#define	SD_TM_AUTO_WRITE_4		0x02
-#define	SD_TM_AUTO_READ_3		0x05
-#define	SD_TM_AUTO_READ_4		0x06
-#define	SD_TM_CMD_RSP			0x08
-#define	SD_TM_AUTO_WRITE_1		0x09
-#define	SD_TM_AUTO_WRITE_2		0x0A
-#define	SD_TM_NORMAL_READ		0x0C
-#define	SD_TM_AUTO_READ_1		0x0D
-#define	SD_TM_AUTO_READ_2		0x0E
-#define	SD_TM_AUTO_TUNING		0x0F
-
-/* SD_VPTX_CTL / SD_VPRX_CTL */
-#define PHASE_CHANGE			0x80
-#define PHASE_NOT_RESET			0x40
-
-/* SD_DCMPS_TX_CTL / SD_DCMPS_RX_CTL */
-#define DCMPS_CHANGE			0x80
-#define DCMPS_CHANGE_DONE		0x40
-#define DCMPS_ERROR			0x20
-#define DCMPS_CURRENT_PHASE		0x1F
-
-/* SD_CMD_STATE */
-#define SD_CMD_IDLE			0x80
-
-/* SD_DATA_STATE */
-#define SD_DATA_IDLE			0x80
-
-/* MS_BLKEND */
-#define SET_BLKEND			0x01
-
-/* MS_CFG */
-#define	SAMPLE_TIME_RISING		0x00
-#define	SAMPLE_TIME_FALLING		0x80
-#define	PUSH_TIME_DEFAULT		0x00
-#define	PUSH_TIME_ODD			0x40
-#define	NO_EXTEND_TOGGLE		0x00
-#define	EXTEND_TOGGLE_CHK		0x20
-#define	MS_BUS_WIDTH_1			0x00
-#define	MS_BUS_WIDTH_4			0x10
-#define	MS_BUS_WIDTH_8			0x18
-#define	MS_2K_SECTOR_MODE		0x04
-#define	MS_512_SECTOR_MODE		0x00
-#define	MS_TOGGLE_TIMEOUT_EN		0x00
-#define	MS_TOGGLE_TIMEOUT_DISEN		0x01
-#define MS_NO_CHECK_INT			0x02
-
-/* MS_TRANS_CFG */
-#define	WAIT_INT			0x80
-#define	NO_WAIT_INT			0x00
-#define	NO_AUTO_READ_INT_REG		0x00
-#define	AUTO_READ_INT_REG		0x40
-#define	MS_CRC16_ERR			0x20
-#define	MS_RDY_TIMEOUT			0x10
-#define	MS_INT_CMDNK			0x08
-#define	MS_INT_BREQ			0x04
-#define	MS_INT_ERR			0x02
-#define	MS_INT_CED			0x01
-
-/* MS_TRANSFER */
-#define	MS_TRANSFER_START		0x80
-#define	MS_TRANSFER_END			0x40
-#define	MS_TRANSFER_ERR			0x20
-#define	MS_BS_STATE			0x10
-#define	MS_TM_READ_BYTES		0x00
-#define	MS_TM_NORMAL_READ		0x01
-#define	MS_TM_WRITE_BYTES		0x04
-#define	MS_TM_NORMAL_WRITE		0x05
-#define	MS_TM_AUTO_READ			0x08
-#define	MS_TM_AUTO_WRITE		0x0C
-#define MS_TM_SET_CMD			0x06
-#define MS_TM_COPY_PAGE			0x07
-#define MS_TM_MULTI_READ		0x02
-#define MS_TM_MULTI_WRITE		0x03
-
-/* MC_DMA_CTL */
-#define DMA_TC_EQ_0			0x80
-#define DMA_DIR_TO_CARD			0x00
-#define DMA_DIR_FROM_CARD		0x02
-#define DMA_EN				0x01
-#define DMA_128				(0 << 2)
-#define DMA_256				(1 << 2)
-#define DMA_512				(2 << 2)
-#define DMA_1024			(3 << 2)
-#define DMA_PACK_SIZE_MASK		0x0C
-
-/* CARD_INT_PEND */
-#define XD_INT				0x10
-#define MS_INT				0x08
-#define SD_INT				0x04
-
-/* MC_FIFO_CTL */
-#define FIFO_FLUSH			0x01
-
-/* AUTO_DELINK_EN */
-#define AUTO_DELINK			0x02
-#define FORCE_DELINK			0x01
-
-/* MC_DMA_RST */
-#define DMA_RESET  0x01
-
-#define SSC_POWER_MASK			0x01
-#define SSC_POWER_DOWN			0x01
-#define SSC_POWER_ON			0x00
-
-/* OCPCTL */
-#define MS_OCP_DETECT_EN		0x08
-#define	MS_OCP_INT_EN			0x04
-#define	MS_OCP_INT_CLR			0x02
-#define	MS_OCP_CLEAR			0x01
-
-/* OCPSTAT */
-#define MS_OCP_DETECT			0x80
-#define MS_OCP_NOW			0x02
-#define MS_OCP_EVER			0x01
-
-/* MC_FIFO_STAT */
-#define FIFO_FULL		0x01
-#define FIFO_EMPTY		0x02
-
-/* RCCTL */
-#define U_HW_CMD_EN_MASK		0x02
-#define U_HW_CMD_EN			0x02
-#define U_HW_CMD_DIS			0x00
-
-/* Register address */
-#define FPDCTL				0xFC00
-#define SSC_DIV_N_0			0xFC07
-#define SSC_CTL1			0xFC09
-#define SSC_CTL2			0xFC0A
-#define CFG_MODE_1		0xFC0F
-#define RCCTL			0xFC14
-#define SYS_DUMMY0			0xFC30
-#define XD_CP_WAITTIME			0xFD00
-#define XD_CP_PAGELEN			0xFD01
-#define XD_CP_READADDR0			0xFD02
-#define XD_CP_READADDR1			0xFD03
-#define XD_CP_READADDR2			0xFD04
-#define XD_CP_READADDR3			0xFD05
-#define XD_CP_READADDR4			0xFD06
-#define XD_CP_WRITEADDR0		0xFD07
-#define XD_CP_WRITEADDR1		0xFD08
-#define XD_CP_WRITEADDR2		0xFD09
-#define XD_CP_WRITEADDR3		0xFD0A
-#define XD_CP_WRITEADDR4		0xFD0B
-#define XD_INIT				0xFD10
-#define XD_DTCTL			0xFD11
-#define XD_CTL				0xFD12
-#define XD_TRANSFER			0xFD13
-#define XD_CFG				0xFD14
-#define XD_ADDRESS0			0xFD15
-#define XD_ADDRESS1			0xFD16
-#define XD_ADDRESS2			0xFD17
-#define XD_ADDRESS3			0xFD18
-#define XD_ADDRESS4			0xFD19
-#define XD_DAT				0xFD1A
-#define XD_PAGE_CNT			0xFD1B
-#define XD_PAGE_STATUS			0xFD1C
-#define XD_BLOCK_STATUS			0xFD1D
-#define XD_BLOCK_ADDR1_L		0xFD1E
-#define XD_BLOCK_ADDR1_H		0xFD1F
-#define XD_BLOCK_ADDR2_L		0xFD20
-#define XD_BLOCK_ADDR2_H		0xFD21
-#define XD_BYTE_CNT_L			0xFD22
-#define XD_BYTE_CNT_H			0xFD23
-#define	XD_PARITY			0xFD24
-#define XD_ECC_BIT1			0xFD25
-#define XD_ECC_BYTE1			0xFD26
-#define XD_ECC_BIT2			0xFD27
-#define XD_ECC_BYTE2			0xFD28
-#define XD_RESERVED0			0xFD29
-#define XD_RESERVED1			0xFD2A
-#define XD_RESERVED2			0xFD2B
-#define XD_RESERVED3			0xFD2C
-#define XD_CHK_DATA_STATUS		0xFD2D
-#define XD_CATCTL			0xFD2E
-
-#define MS_BLKEND			0xFD30
-#define MS_READ_START			0xFD31
-#define MS_READ_COUNT			0xFD32
-#define MS_WRITE_START			0xFD33
-#define MS_WRITE_COUNT			0xFD34
-#define MS_COMMAND			0xFD35
-#define MS_OLD_BLOCK_0			0xFD36
-#define MS_OLD_BLOCK_1			0xFD37
-#define MS_NEW_BLOCK_0			0xFD38
-#define MS_NEW_BLOCK_1			0xFD39
-#define MS_LOG_BLOCK_0			0xFD3A
-#define MS_LOG_BLOCK_1			0xFD3B
-#define MS_BUS_WIDTH			0xFD3C
-#define MS_PAGE_START			0xFD3D
-#define MS_PAGE_LENGTH			0xFD3E
-#define MS_CFG				0xFD40
-#define MS_TPC				0xFD41
-#define MS_TRANS_CFG			0xFD42
-#define MS_TRANSFER			0xFD43
-#define MS_INT_REG			0xFD44
-#define MS_BYTE_CNT			0xFD45
-#define MS_SECTOR_CNT_L			0xFD46
-#define MS_SECTOR_CNT_H			0xFD47
-#define MS_DBUS_H			0xFD48
-
-#define CARD_DMA1_CTL			0xFD5C
-#define CARD_PULL_CTL1			0xFD60
-#define CARD_PULL_CTL2			0xFD61
-#define CARD_PULL_CTL3			0xFD62
-#define CARD_PULL_CTL4			0xFD63
-#define CARD_PULL_CTL5			0xFD64
-#define CARD_PULL_CTL6			0xFD65
-#define CARD_EXIST				0xFD6F
-#define CARD_INT_PEND			0xFD71
-
-#define LDO_POWER_CFG			0xFD7B
-
-#define SD_CFG1				0xFDA0
-#define SD_CFG2				0xFDA1
-#define SD_CFG3				0xFDA2
-#define SD_STAT1			0xFDA3
-#define SD_STAT2			0xFDA4
-#define SD_BUS_STAT			0xFDA5
-#define SD_PAD_CTL			0xFDA6
-#define SD_SAMPLE_POINT_CTL		0xFDA7
-#define SD_PUSH_POINT_CTL		0xFDA8
-#define SD_CMD0				0xFDA9
-#define SD_CMD1				0xFDAA
-#define SD_CMD2				0xFDAB
-#define SD_CMD3				0xFDAC
-#define SD_CMD4				0xFDAD
-#define SD_CMD5				0xFDAE
-#define SD_BYTE_CNT_L			0xFDAF
-#define SD_BYTE_CNT_H			0xFDB0
-#define SD_BLOCK_CNT_L			0xFDB1
-#define SD_BLOCK_CNT_H			0xFDB2
-#define SD_TRANSFER			0xFDB3
-#define SD_CMD_STATE			0xFDB5
-#define SD_DATA_STATE			0xFDB6
-#define SD_VPCLK0_CTL			0xFC2A
-#define SD_VPCLK1_CTL			0xFC2B
-#define SD_DCMPS0_CTL			0xFC2C
-#define SD_DCMPS1_CTL			0xFC2D
-
-#define CARD_DMA1_CTL			0xFD5C
-
-#define HW_VERSION			0xFC01
-
-#define SSC_CLK_FPGA_SEL		0xFC02
-#define CLK_DIV				0xFC03
-#define SFSM_ED				0xFC04
-
-#define CD_DEGLITCH_WIDTH		0xFC20
-#define CD_DEGLITCH_EN			0xFC21
-#define AUTO_DELINK_EN			0xFC23
-
-#define FPGA_PULL_CTL			0xFC1D
-#define CARD_CLK_SOURCE			0xFC2E
-
-#define CARD_SHARE_MODE			0xFD51
-#define CARD_DRIVE_SEL			0xFD52
-#define CARD_STOP			0xFD53
-#define CARD_OE				0xFD54
-#define CARD_AUTO_BLINK			0xFD55
-#define CARD_GPIO			0xFD56
-#define SD30_DRIVE_SEL		0xFD57
-
-#define CARD_DATA_SOURCE		0xFD5D
-#define CARD_SELECT			0xFD5E
-
-#define CARD_CLK_EN			0xFD79
-#define CARD_PWR_CTL			0xFD7A
-
-#define OCPCTL				0xFD80
-#define OCPPARA1			0xFD81
-#define OCPPARA2			0xFD82
-#define OCPSTAT				0xFD83
-
-#define HS_USB_STAT			0xFE01
-#define HS_VCONTROL			0xFE26
-#define HS_VSTAIN			0xFE27
-#define HS_VLOADM			0xFE28
-#define HS_VSTAOUT			0xFE29
-
-#define MC_IRQ				0xFF00
-#define MC_IRQEN			0xFF01
-#define MC_FIFO_CTL			0xFF02
-#define MC_FIFO_BC0			0xFF03
-#define MC_FIFO_BC1			0xFF04
-#define MC_FIFO_STAT			0xFF05
-#define MC_FIFO_MODE			0xFF06
-#define MC_FIFO_RD_PTR0		0xFF07
-#define MC_FIFO_RD_PTR1		0xFF08
-#define MC_DMA_CTL			0xFF10
-#define MC_DMA_TC0			0xFF11
-#define MC_DMA_TC1			0xFF12
-#define MC_DMA_TC2			0xFF13
-#define MC_DMA_TC3			0xFF14
-#define MC_DMA_RST			0xFF15
-
-/* Memory mapping */
-#define RBUF_SIZE_MASK		0xFBFF
-#define RBUF_BASE			0xF000
-#define PPBUF_BASE1			0xF800
-#define PPBUF_BASE2			0xFA00
-
-/* int monitor_card_cd */
-#define CD_EXIST			0
-#define CD_NOT_EXIST			1
-
-#define DEBOUNCE_CNT			5
-
-int monitor_card_cd(struct rts51x_chip *chip, u8 card);
-
-void rts51x_do_remaining_work(struct rts51x_chip *chip);
-void rts51x_do_rts51x_reset_sd_card(struct rts51x_chip *chip);
-void rts51x_init_cards(struct rts51x_chip *chip);
-void rts51x_release_cards(struct rts51x_chip *chip);
-int rts51x_switch_ssc_clock(struct rts51x_chip *chip, int clk);
-int rts51x_switch_normal_clock(struct rts51x_chip *chip, int clk);
-int rts51x_card_rw(struct scsi_cmnd *srb, struct rts51x_chip *chip,
-		u32 sec_addr, u16 sec_cnt);
-u8 rts51x_get_lun_card(struct rts51x_chip *chip, unsigned int lun);
-int rts51x_select_card(struct rts51x_chip *chip, int card);
-void rts51x_eject_card(struct rts51x_chip *chip, unsigned int lun);
-void rts51x_trans_dma_enable(enum dma_data_direction dir,
-		struct rts51x_chip *chip, u32 byte_cnt, u8 pack_size);
-int rts51x_enable_card_clock(struct rts51x_chip *chip, u8 card);
-int rts51x_card_power_on(struct rts51x_chip *chip, u8 card);
-int rts51x_toggle_gpio(struct rts51x_chip *chip, u8 gpio);
-int rts51x_turn_on_led(struct rts51x_chip *chip, u8 gpio);
-int rts51x_turn_off_led(struct rts51x_chip *chip, u8 gpio);
-
-static inline int check_card_ready(struct rts51x_chip *chip, unsigned int lun)
-{
-	if (chip->card_ready & chip->lun2card[lun])
-		return 1;
-
-	return 0;
-}
-
-static inline int check_card_exist(struct rts51x_chip *chip, unsigned int lun)
-{
-	if (chip->card_exist & chip->lun2card[lun])
-		return 1;
-
-	return 0;
-}
-
-static inline int check_card_wp(struct rts51x_chip *chip, unsigned int lun)
-{
-	if (chip->card_wp & chip->lun2card[lun])
-		return 1;
-
-	return 0;
-}
-
-static inline int check_card_fail(struct rts51x_chip *chip, unsigned int lun)
-{
-	if (chip->card_fail & chip->lun2card[lun])
-		return 1;
-
-	return 0;
-}
-
-static inline int check_card_ejected(struct rts51x_chip *chip, unsigned int lun)
-{
-	if (chip->card_ejected & chip->lun2card[lun])
-		return 1;
-
-	return 0;
-}
-
-static inline int check_fake_card_ready(struct rts51x_chip *chip,
-					unsigned int lun)
-{
-	if (chip->fake_card_ready & chip->lun2card[lun])
-		return 1;
-
-	return 0;
-}
-
-static inline u8 get_lun2card(struct rts51x_chip *chip, unsigned int lun)
-{
-	return chip->lun2card[lun];
-}
-
-static inline int check_lun_mc(struct rts51x_chip *chip, unsigned int lun)
-{
-	return CHK_BIT(chip->lun_mc, lun);
-}
-
-static inline void set_lun_mc(struct rts51x_chip *chip, unsigned int lun)
-{
-	SET_BIT(chip->lun_mc, lun);
-}
-
-static inline void clear_lun_mc(struct rts51x_chip *chip, unsigned int lun)
-{
-	CLR_BIT(chip->lun_mc, lun);
-}
-
-static inline int switch_clock(struct rts51x_chip *chip, int clk)
-{
-	int retval = 0;
-
-	if (chip->asic_code)
-		retval = rts51x_switch_ssc_clock(chip, clk);
-	else
-		retval = rts51x_switch_normal_clock(chip, clk);
-
-	return retval;
-}
-
-static inline void rts51x_clear_xd_error(struct rts51x_chip *chip)
-{
-	rts51x_ep0_write_register(chip, CARD_STOP,
-				  XD_STOP | XD_CLR_ERR, XD_STOP | XD_CLR_ERR);
-
-	rts51x_ep0_write_register(chip, MC_FIFO_CTL, FIFO_FLUSH, FIFO_FLUSH);
-	rts51x_ep0_write_register(chip, MC_DMA_RST, DMA_RESET, DMA_RESET);
-	rts51x_ep0_write_register(chip, SFSM_ED, 0xf8, 0xf8);
-}
-
-static inline void rts51x_clear_sd_error(struct rts51x_chip *chip)
-{
-	rts51x_ep0_write_register(chip, CARD_STOP,
-				  SD_STOP | SD_CLR_ERR, SD_STOP | SD_CLR_ERR);
-
-	rts51x_ep0_write_register(chip, MC_FIFO_CTL, FIFO_FLUSH, FIFO_FLUSH);
-	rts51x_ep0_write_register(chip, MC_DMA_RST, DMA_RESET, DMA_RESET);
-	rts51x_ep0_write_register(chip, SFSM_ED, 0xf8, 0xf8);
-}
-
-static inline void rts51x_clear_ms_error(struct rts51x_chip *chip)
-{
-	rts51x_ep0_write_register(chip, CARD_STOP,
-				  MS_STOP | MS_CLR_ERR, MS_STOP | MS_CLR_ERR);
-
-	rts51x_ep0_write_register(chip, MC_FIFO_CTL, FIFO_FLUSH, FIFO_FLUSH);
-	rts51x_ep0_write_register(chip, MC_DMA_RST, DMA_RESET, DMA_RESET);
-	rts51x_ep0_write_register(chip, SFSM_ED, 0xf8, 0xf8);
-}
-
-#endif /* __RTS51X_CARD_H */
diff --git a/drivers/staging/rts5139/rts51x_chip.c b/drivers/staging/rts5139/rts51x_chip.c
deleted file mode 100644
index 7d7510de170..00000000000
--- a/drivers/staging/rts5139/rts51x_chip.c
+++ /dev/null
@@ -1,1014 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#include <linux/blkdev.h>
-#include <linux/kthread.h>
-#include <linux/sched.h>
-#include <linux/workqueue.h>
-
-#include "debug.h"
-#include "trace.h"
-#include "rts51x.h"
-#include "rts51x_chip.h"
-#include "rts51x_card.h"
-#include "rts51x_transport.h"
-#include "xd.h"
-#include "ms.h"
-#include "sd.h"
-
-static int check_sd_speed_prior(u32 sd_speed_prior)
-{
-	int i, fake_para = 0;
-
-	/* Check the legality of 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;
-		}
-	}
-
-	return !fake_para;
-}
-
-int rts51x_reset_chip(struct rts51x_chip *chip)
-{
-	int retval;
-
-	if (CHECK_PKG(chip, LQFP48)) {
-		RTS51X_WRITE_REG(chip, CARD_PWR_CTL, LDO3318_PWR_MASK,
-				 LDO_SUSPEND);
-		RTS51X_WRITE_REG(chip, CARD_PWR_CTL, FORCE_LDO_POWERB,
-				 FORCE_LDO_POWERB);
-		RTS51X_WRITE_REG(chip, CARD_PULL_CTL1, 0x30, 0x10);
-		RTS51X_WRITE_REG(chip, CARD_PULL_CTL5, 0x03, 0x01);
-		RTS51X_WRITE_REG(chip, CARD_PULL_CTL6, 0x0C, 0x04);
-	}
-	if (chip->asic_code) {
-		RTS51X_WRITE_REG(chip, SYS_DUMMY0, NYET_MSAK, NYET_EN);
-		RTS51X_WRITE_REG(chip, CD_DEGLITCH_WIDTH, 0xFF, 0x08);
-		rts51x_write_register(chip, CD_DEGLITCH_EN, XD_CD_DEGLITCH_EN,
-				      0x00);
-		rts51x_write_register(chip, SD30_DRIVE_SEL, SD30_DRIVE_MASK,
-				      chip->option.sd30_pad_drive);
-		rts51x_write_register(chip, CARD_DRIVE_SEL, SD20_DRIVE_MASK,
-				      chip->option.sd20_pad_drive);
-		if (chip->rts5179)
-			rts51x_write_register(chip, CARD_PULL_CTL5, 0x03, 0x01);
-		if (CHECK_PKG(chip, LQFP48)) {
-			rts51x_write_register(chip, CARD_PULL_CTL3,
-					      0x80, 0x80);
-			rts51x_write_register(chip, CARD_PULL_CTL6,
-					      0xf0, 0xA0);
-		} else {
-			rts51x_write_register(chip, CARD_PULL_CTL1,
-					      0x30, 0x20);
-			rts51x_write_register(chip, CARD_PULL_CTL3,
-					      0x80, 0x80);
-			rts51x_write_register(chip, CARD_PULL_CTL6,
-					      0x0c, 0x08);
-		}
-	}
-	if (chip->option.sd_ctl & SUPPORT_UHS50_MMC44) {
-		SET_UHS50(chip);
-		RTS51X_DEBUGP("option enable UHS50&MMC44,sd_ctl:0x%x\n",
-			chip->option.sd_ctl);
-	} else {
-		/* if(CHECK_PID(chip, 0x0139)&&CHECK_PKG(chip, LQFP48)) */
-		if ((CHECK_PID(chip, 0x0139) && CHECK_PKG(chip, LQFP48))
-		    || chip->rts5179) {
-			SET_UHS50(chip);
-			RTS51X_DEBUGP("PID enable UHS50&MMC44\n");
-		} else {
-			CLEAR_UHS50(chip);
-			RTS51X_DEBUGP("PID disable UHS50&MMC44\n");
-		}
-	}
-
-	if (chip->option.ms_errreg_fix && (chip->ic_version > 1))
-		rts51x_write_register(chip, 0xFD4D, 0x01, 0x01);
-	retval = rts51x_write_phy_register(chip, 0xC2, 0x7C);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	rts51x_init_cmd(chip);
-
-	/* GPIO OE */
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_GPIO, GPIO_OE, GPIO_OE);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DMA1_CTL,
-		       EXTEND_DMA1_ASYNC_SIGNAL, EXTEND_DMA1_ASYNC_SIGNAL);
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-#ifdef SUPPORT_OCP
-	if (chip->asic_code) {
-		rts51x_write_register(chip, OCPCTL, MS_OCP_DETECT_EN,
-				      MS_OCP_DETECT_EN);
-		RTS51X_DEBUGP("Enable OCP detect!\n");
-	}
-#endif
-	if (chip->option.FT2_fast_mode) {
-		rts51x_card_power_on(chip, SD_CARD | MS_CARD | XD_CARD);
-		wait_timeout(10);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_init_chip(struct rts51x_chip *chip)
-{
-	int retval;
-	u8 val;
-
-	chip->max_lun = 0;
-	chip->cur_clk = 0;
-	chip->cur_card = 0;
-
-	chip->card2lun[XD_CARD] = 0;
-	chip->card2lun[SD_CARD] = 0;
-	chip->card2lun[MS_CARD] = 0;
-	chip->card_ejected = 0;
-
-	chip->lun2card[0] = XD_CARD | SD_CARD | MS_CARD;
-#ifdef CLOSE_SSC_POWER
-	rts51x_write_register(chip, FPDCTL, SSC_POWER_MASK, SSC_POWER_ON);
-	udelay(100);
-	rts51x_write_register(chip, CLK_DIV, CLK_CHANGE, 0x00);
-#endif
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	RTS51X_READ_REG(chip, HW_VERSION, &val);
-	RTS51X_DEBUGP("HW_VERSION: 0x%x\n", val);
-	if (val & FPGA_VER) {
-		chip->asic_code = 0;
-		RTS51X_DEBUGP("FPGA!\n");
-	} else {
-		chip->asic_code = 1;
-		RTS51X_DEBUGP("ASIC!\n");
-	}
-	chip->ic_version = val & HW_VER_MASK;
-
-	if (!check_sd_speed_prior(chip->option.sd_speed_prior))
-		chip->option.sd_speed_prior = 0x01020403;
-	RTS51X_DEBUGP("sd_speed_prior = 0x%08x\n",
-		       chip->option.sd_speed_prior);
-
-	RTS51X_READ_REG(chip, CARD_SHARE_MODE, &val);
-	if (val & CARD_SHARE_LQFP_SEL) {
-		chip->package = LQFP48;
-		RTS51X_DEBUGP("Package: LQFP48\n");
-	} else {
-		chip->package = QFN24;
-		RTS51X_DEBUGP("Package: QFN24\n");
-	}
-
-	RTS51X_READ_REG(chip, HS_USB_STAT, &val);
-	if (val & USB_HI_SPEED) {
-		chip->usb_speed = USB_20;
-		RTS51X_DEBUGP("USB High Speed\n");
-	} else {
-		chip->usb_speed = USB_11;
-		RTS51X_DEBUGP("USB Full Speed\n");
-	}
-
-	RTS51X_READ_REG(chip, CFG_MODE_1, &val);
-	if (val & RTS5179) {
-		chip->rts5179 = 1;
-		RTS51X_DEBUGP("device is rts5179\n");
-	} else {
-		chip->rts5179 = 0;
-	}
-
-	retval = rts51x_reset_chip(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_release_chip(struct rts51x_chip *chip)
-{
-	rts51x_xd_free_l2p_tbl(chip);
-	rts51x_ms_free_l2p_tbl(chip);
-	chip->card_ready = 0;
-	return STATUS_SUCCESS;
-}
-
-static inline void rts51x_blink_led(struct rts51x_chip *chip)
-{
-	/* Read/Write */
-	if (chip->card_ready) {
-		if (chip->led_toggle_counter <
-				chip->option.led_toggle_interval) {
-			chip->led_toggle_counter++;
-		} else {
-			chip->led_toggle_counter = 0;
-			rts51x_toggle_gpio(chip, LED_GPIO);
-		}
-	}
-}
-
-static void rts51x_auto_delink_cmd(struct rts51x_chip *chip)
-{
-	rts51x_write_register(chip, AUTO_DELINK_EN,
-			AUTO_DELINK, AUTO_DELINK);
-}
-
-static void rts51x_auto_delink_force_cmd(struct rts51x_chip *chip)
-{
-	rts51x_write_register(chip, AUTO_DELINK_EN,
-			AUTO_DELINK | FORCE_DELINK,
-			AUTO_DELINK | FORCE_DELINK);
-}
-
-#ifdef USING_POLLING_CYCLE_DELINK
-/* using polling cycle as delink time */
-static void rts51x_auto_delink_polling_cycle(struct rts51x_chip *chip)
-{
-	if (chip->auto_delink_counter <=
-			chip->option.delink_delay * 2) {
-		if (chip->auto_delink_counter ==
-		    chip->option.delink_delay) {
-			if (chip->card_exist) {
-				/* False card */
-				if (!chip->card_ejected) {
-					/* if card is not ejected or safely
-					 * remove,then do force delink */
-					RTS51X_DEBUGP("False card inserted,"
-							"do force delink\n");
-					rts51x_auto_delink_force_cmd(chip);
-					chip->auto_delink_counter =
-					    chip->option.delink_delay * 2 + 1;
-				}
-			} else {
-				RTS51X_DEBUGP("No card inserted, do delink\n");
-				/* rts51x_write_register(chip, CARD_PWR_CTL,
-						DV3318_AUTO_PWR_OFF, 0); */
-				rts51x_auto_delink_cmd(chip);
-			}
-		}
-		if (chip->auto_delink_counter ==
-		    chip->option.delink_delay * 2) {
-			RTS51X_DEBUGP("Try to do force delink\n");
-			rts51x_auto_delink_force_cmd(chip);
-		}
-		chip->auto_delink_counter++;
-	}
-}
-
-static void rts51x_auto_delink(struct rts51x_chip *chip)
-{
-	rts51x_auto_delink_polling_cycle(chip);
-}
-#else
-/* some of called funcs are not implemented, so comment it out */
-static void rts51x_auto_delink(struct rts51x_chip *chip)
-{
-}
-#endif
-
-void rts51x_polling_func(struct rts51x_chip *chip)
-{
-
-	rts51x_init_cards(chip);
-
-#ifdef SUPPORT_OCP
-	/* if OCP happen and card exist, then close card OE */
-	if ((chip->ocp_stat & (MS_OCP_NOW | MS_OCP_EVER)) &&
-			(chip->card_exist)) {
-
-		rts51x_prepare_run(chip);
-
-		if (chip->card_exist & SD_CARD)
-			rts51x_write_register(chip, CARD_OE, SD_OUTPUT_EN, 0);
-		else if (chip->card_exist & MS_CARD)
-			rts51x_write_register(chip, CARD_OE, MS_OUTPUT_EN, 0);
-		else if (chip->card_exist & XD_CARD)
-			rts51x_write_register(chip, CARD_OE, XD_OUTPUT_EN, 0);
-	}
-#endif
-
-	if (chip->idle_counter < IDLE_MAX_COUNT) {
-		chip->idle_counter++;
-	} else {
-		if (!RTS51X_CHK_STAT(chip, STAT_IDLE)) {
-			RTS51X_DEBUGP("Idle state!\n");
-			RTS51X_SET_STAT(chip, STAT_IDLE);
-			chip->led_toggle_counter = 0;
-			/* Idle state, turn off LED
-			 * to reduce power consumption */
-			if (chip->option.led_always_on
-			    && (chip->card_exist &
-				(SD_CARD | MS_CARD | XD_CARD))
-			    && (!chip->card_ejected)) {
-				rts51x_turn_on_led(chip, LED_GPIO);
-			} else {
-				if (chip->rts5179) {
-					rts51x_ep0_write_register(chip,
-								  CARD_GPIO,
-								  0x03, 0x00);
-				} else {
-					rts51x_turn_off_led(chip, LED_GPIO);
-				}
-
-			}
-
-#ifdef CLOSE_SSC_POWER
-			if (!chip->card_ready) {
-				rts51x_write_register(chip, CLK_DIV, CLK_CHANGE,
-						      CLK_CHANGE);
-				rts51x_write_register(chip, FPDCTL,
-						      SSC_POWER_MASK,
-						      SSC_POWER_DOWN);
-				RTS51X_DEBUGP("Close SSC clock power!\n");
-			}
-#endif
-		}
-	}
-
-	switch (RTS51X_GET_STAT(chip)) {
-	case STAT_RUN:
-		rts51x_blink_led(chip);
-		rts51x_do_remaining_work(chip);
-		break;
-
-	case STAT_IDLE:
-		break;
-
-	default:
-		break;
-	}
-
-	if (chip->option.auto_delink_en && !chip->card_ready)
-		rts51x_auto_delink(chip);
-	else
-		chip->auto_delink_counter = 0;
-}
-
-void rts51x_add_cmd(struct rts51x_chip *chip,
-		    u8 cmd_type, u16 reg_addr, u8 mask, u8 data)
-{
-	int i;
-
-	if (chip->cmd_idx < ((CMD_BUF_LEN - CMD_OFFSET) / 4)) {
-		i = CMD_OFFSET + chip->cmd_idx * 4;
-		chip->cmd_buf[i++] =
-		    ((cmd_type & 0x03) << 6) | (u8) ((reg_addr >> 8) & 0x3F);
-		chip->cmd_buf[i++] = (u8) reg_addr;
-		chip->cmd_buf[i++] = mask;
-		chip->cmd_buf[i++] = data;
-		chip->cmd_idx++;
-	}
-}
-
-int rts51x_send_cmd(struct rts51x_chip *chip, u8 flag, int timeout)
-{
-	int result;
-
-	chip->cmd_buf[CNT_H] = (u8) (chip->cmd_idx >> 8);
-	chip->cmd_buf[CNT_L] = (u8) (chip->cmd_idx);
-	chip->cmd_buf[STAGE_FLAG] = flag;
-
-	result = rts51x_transfer_data_rcc(chip, SND_BULK_PIPE(chip),
-					  (void *)(chip->cmd_buf),
-					  chip->cmd_idx * 4 + CMD_OFFSET,
-					  0, NULL, timeout, MODE_C);
-	if (result != STATUS_SUCCESS)
-		TRACE_RET(chip, result);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_get_rsp(struct rts51x_chip *chip, int rsp_len, int timeout)
-{
-	int result;
-
-	if (rsp_len <= 0)
-		TRACE_RET(chip, STATUS_ERROR);
-	/* rsp_len must aligned to dword */
-	if (rsp_len % 4)
-		rsp_len += (4 - rsp_len % 4);
-
-	result = rts51x_transfer_data_rcc(chip, RCV_BULK_PIPE(chip),
-					  (void *)chip->rsp_buf, rsp_len,
-					  0, NULL, timeout, STAGE_R);
-	if (result != STATUS_SUCCESS)
-		TRACE_RET(chip, result);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_get_card_status(struct rts51x_chip *chip, u16 *status)
-{
-	int retval;
-	u16 val;
-
-#ifdef GET_CARD_STATUS_USING_EPC
-	retval = rts51x_get_epc_status(chip, &val);
-
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-#else
-	retval = rts51x_ctrl_transfer(chip, RCV_CTRL_PIPE(chip), 0x02, 0xC0,
-				      0, 0, &val, 2, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-#endif
-
-	if (status)
-		*status = val;
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_write_register(struct rts51x_chip *chip, u16 addr, u8 mask, u8 data)
-{
-	int retval;
-
-	rts51x_init_cmd(chip);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, addr, mask, data);
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_read_register(struct rts51x_chip *chip, u16 addr, u8 *data)
-{
-	int retval;
-
-	if (data)
-		*data = 0;
-	rts51x_init_cmd(chip);
-	rts51x_add_cmd(chip, READ_REG_CMD, addr, 0, 0);
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	retval = rts51x_get_rsp(chip, 1, 100);
-
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	if (data)
-		*data = chip->rsp_buf[0];
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_ep0_write_register(struct rts51x_chip *chip, u16 addr, u8 mask,
-			      u8 data)
-{
-	int retval;
-	u16 value = 0, index = 0;
-
-	value |= (u16) (3 & 0x03) << 14;
-	value |= (u16) (addr & 0x3FFF);
-	index |= (u16) mask << 8;
-	index |= (u16) data;
-
-	retval = rts51x_ctrl_transfer(chip, SND_CTRL_PIPE(chip), 0x00, 0x40,
-				      cpu_to_be16(value), cpu_to_be16(index),
-				      NULL, 0, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_ep0_read_register(struct rts51x_chip *chip, u16 addr, u8 *data)
-{
-	int retval;
-	u16 value = 0;
-	u8 val;
-
-	if (data)
-		*data = 0;
-
-	value |= (u16) (2 & 0x03) << 14;
-	value |= (u16) (addr & 0x3FFF);
-
-	retval = rts51x_ctrl_transfer(chip, RCV_CTRL_PIPE(chip), 0x00, 0xC0,
-				      cpu_to_be16(value), 0, &val, 1, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (data)
-		*data = val;
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_seq_write_register(struct rts51x_chip *chip, u16 addr, u16 len,
-			      u8 *data)
-{
-	int result;
-	u16 cmd_len = len + 12;
-
-	if (!data)
-		TRACE_RET(chip, STATUS_ERROR);
-
-	cmd_len = (cmd_len <= CMD_BUF_LEN) ? cmd_len : CMD_BUF_LEN;
-
-	/* cmd_len must aligned to dword */
-	if (cmd_len % 4)
-		cmd_len += (4 - cmd_len % 4);
-
-	chip->cmd_buf[0] = 'R';
-	chip->cmd_buf[1] = 'T';
-	chip->cmd_buf[2] = 'C';
-	chip->cmd_buf[3] = 'R';
-	chip->cmd_buf[PACKET_TYPE] = SEQ_WRITE;
-	chip->cmd_buf[5] = (u8) (len >> 8);
-	chip->cmd_buf[6] = (u8) len;
-	chip->cmd_buf[STAGE_FLAG] = 0;
-	chip->cmd_buf[8] = (u8) (addr >> 8);
-	chip->cmd_buf[9] = (u8) addr;
-
-	memcpy(chip->cmd_buf + 12, data, len);
-
-	result = rts51x_transfer_data_rcc(chip, SND_BULK_PIPE(chip),
-					  (void *)(chip->cmd_buf), cmd_len, 0,
-					  NULL, 100, MODE_C);
-	if (result != STATUS_SUCCESS)
-		TRACE_RET(chip, result);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_seq_read_register(struct rts51x_chip *chip, u16 addr, u16 len,
-			     u8 *data)
-{
-	int result;
-	u16 rsp_len;
-
-	if (!data)
-		TRACE_RET(chip, STATUS_ERROR);
-	/* rsp_len must aligned to dword */
-	if (len % 4)
-		rsp_len = len + (4 - len % 4);
-	else
-		rsp_len = len;
-
-	chip->cmd_buf[0] = 'R';
-	chip->cmd_buf[1] = 'T';
-	chip->cmd_buf[2] = 'C';
-	chip->cmd_buf[3] = 'R';
-	chip->cmd_buf[PACKET_TYPE] = SEQ_READ;
-	chip->cmd_buf[5] = (u8) (rsp_len >> 8);
-	chip->cmd_buf[6] = (u8) rsp_len;
-	chip->cmd_buf[STAGE_FLAG] = STAGE_R;
-	chip->cmd_buf[8] = (u8) (addr >> 8);
-	chip->cmd_buf[9] = (u8) addr;
-
-	result = rts51x_transfer_data_rcc(chip, SND_BULK_PIPE(chip),
-					  (void *)(chip->cmd_buf), 12, 0, NULL,
-					  100, MODE_C);
-	if (result != STATUS_SUCCESS)
-		TRACE_RET(chip, result);
-
-	result = rts51x_transfer_data_rcc(chip, RCV_BULK_PIPE(chip),
-					  (void *)data, rsp_len, 0, NULL, 100,
-					  STAGE_DI);
-	if (result != STATUS_SUCCESS)
-		TRACE_RET(chip, result);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_read_ppbuf(struct rts51x_chip *chip, u8 *buf, int buf_len)
-{
-	int retval;
-
-	if (!buf)
-		TRACE_RET(chip, STATUS_ERROR);
-
-	retval =
-	    rts51x_seq_read_register(chip, PPBUF_BASE2, (u16) buf_len, buf);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_write_ppbuf(struct rts51x_chip *chip, u8 *buf, int buf_len)
-{
-	int retval;
-
-	if (!buf)
-		TRACE_RET(chip, STATUS_ERROR);
-
-	retval =
-	    rts51x_seq_write_register(chip, PPBUF_BASE2, (u16) buf_len, buf);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_write_phy_register(struct rts51x_chip *chip, u8 addr, u8 val)
-{
-	int retval;
-
-	RTS51X_DEBUGP("Write 0x%x to phy register 0x%x\n", val, addr);
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VSTAIN, 0xFF, val);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VCONTROL, 0xFF, addr & 0x0F);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x00);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x00);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x01);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VCONTROL, 0xFF,
-		       (addr >> 4) & 0x0F);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x00);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x00);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x01);
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_read_phy_register(struct rts51x_chip *chip, u8 addr, u8 *val)
-{
-	int retval;
-
-	RTS51X_DEBUGP("Read from phy register 0x%x\n", addr);
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VCONTROL, 0xFF, 0x07);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x00);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x00);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x01);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VCONTROL, 0xFF,
-		       (addr >> 4) & 0x0F);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x00);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x00);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x01);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VCONTROL, 0xFF, addr & 0x0F);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x00);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x00);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x01);
-	rts51x_add_cmd(chip, READ_REG_CMD, HS_VSTAOUT, 0, 0);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 1, 100);
-
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (val)
-		*val = chip->rsp_buf[0];
-
-	RTS51X_DEBUGP("Return value: 0x%x\n", chip->rsp_buf[0]);
-
-	return STATUS_SUCCESS;
-}
-
-void rts51x_do_before_power_down(struct rts51x_chip *chip)
-{
-	RTS51X_DEBUGP("rts51x_do_before_power_down\n");
-
-	rts51x_prepare_run(chip);
-
-	rts51x_release_cards(chip);
-	if (chip->rts5179)
-		rts51x_ep0_write_register(chip, CARD_GPIO, 0x03, 0x00);
-	else
-		rts51x_turn_off_led(chip, LED_GPIO);
-
-	chip->cur_clk = 0;
-	chip->card_exist = 0;
-	chip->cur_card = 0;
-	if (chip->asic_code) {
-		if (CHECK_PKG(chip, LQFP48)) {
-			rts51x_write_register(chip, CARD_PULL_CTL3, 0x80, 0x00);
-			rts51x_write_register(chip, CARD_PULL_CTL6, 0xf0, 0x50);
-		} else {
-			rts51x_write_register(chip, CARD_PULL_CTL1, 0x30, 0x10);
-			rts51x_write_register(chip, CARD_PULL_CTL3, 0x80, 0x00);
-			rts51x_write_register(chip, CARD_PULL_CTL6, 0x0c, 0x04);
-		}
-	}
-	if (CHECK_PKG(chip, LQFP48))
-		rts51x_write_register(chip, CARD_PWR_CTL, LDO3318_PWR_MASK,
-				      LDO_OFF);
-}
-
-void rts51x_clear_hw_error(struct rts51x_chip *chip)
-{
-	rts51x_ep0_write_register(chip, SFSM_ED, 0xf8, 0xf8);
-}
-
-void rts51x_prepare_run(struct rts51x_chip *chip)
-{
-#ifdef CLOSE_SSC_POWER
-	if (RTS51X_CHK_STAT(chip, STAT_IDLE) && (!chip->card_ready)) {
-		rts51x_write_register(chip, FPDCTL, SSC_POWER_MASK,
-				      SSC_POWER_ON);
-		udelay(100);
-		RTS51X_DEBUGP("Open SSC clock power.\n");
-
-		rts51x_write_register(chip, CLK_DIV, CLK_CHANGE, 0x00);
-	}
-#endif
-}
-
-#ifdef _MSG_TRACE
-void rts51x_trace_msg(struct rts51x_chip *chip, unsigned char *buf, int clear)
-{
-	unsigned char *ptr;
-	int i, msg_cnt;
-
-	if (!buf)
-		return;
-
-	ptr = buf;
-
-	if (chip->trace_msg[chip->msg_idx].valid)
-		msg_cnt = TRACE_ITEM_CNT;
-	else
-		msg_cnt = chip->msg_idx;
-	*(ptr++) = (u8) (msg_cnt >> 24);
-	*(ptr++) = (u8) (msg_cnt >> 16);
-	*(ptr++) = (u8) (msg_cnt >> 8);
-	*(ptr++) = (u8) msg_cnt;
-	RTS51X_DEBUGP("Trace message count is %d\n", msg_cnt);
-
-	for (i = 1; i <= msg_cnt; i++) {
-		int j, idx;
-
-		idx = chip->msg_idx - i;
-		if (idx < 0)
-			idx += TRACE_ITEM_CNT;
-
-		*(ptr++) = (u8) (chip->trace_msg[idx].line >> 8);
-		*(ptr++) = (u8) (chip->trace_msg[idx].line);
-		for (j = 0; j < MSG_FUNC_LEN; j++)
-			*(ptr++) = chip->trace_msg[idx].func[j];
-		for (j = 0; j < MSG_FILE_LEN; j++)
-			*(ptr++) = chip->trace_msg[idx].file[j];
-		for (j = 0; j < TIME_VAL_LEN; j++)
-			*(ptr++) = chip->trace_msg[idx].timeval_buf[j];
-	}
-
-	if (clear) {
-		chip->msg_idx = 0;
-		for (i = 0; i < TRACE_ITEM_CNT; i++)
-			chip->trace_msg[i].valid = 0;
-	}
-}
-#endif
-
-void rts51x_pp_status(struct rts51x_chip *chip, unsigned int lun, u8 *status,
-		      u8 status_len)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	struct ms_info *ms_card = &(chip->ms_card);
-	u8 card = rts51x_get_lun_card(chip, lun);
-#ifdef SUPPORT_OC
-	u8 oc_now_mask = 0, oc_ever_mask = 0;
-#endif
-
-	if (!status || (status_len < 32))
-		return;
-	/* IC Version */
-	status[0] = (u8) RTS51X_GET_PID(chip);
-	status[1] = (u8) (chip->ic_version);
-
-	/* Auto delink mode */
-	if (chip->option.auto_delink_en)
-		status[2] = 0x10;
-	else
-		status[2] = 0x00;
-
-	/* Spec version */
-	status[3] = 20;
-	status[4] = 10;
-	status[5] = 05;
-	status[6] = 21;
-
-	/* Card WP */
-	if (chip->card_wp)
-		status[7] = 0x20;
-	else
-		status[7] = 0x00;
-
-#ifdef SUPPORT_OC
-	/* Over current status */
-	status[8] = 0;
-	oc_now_mask = MS_OCP_NOW;
-	oc_ever_mask = MS_OCP_EVER;
-
-	if (chip->ocp_stat & oc_now_mask)
-		status[8] |= 0x02;
-	if (chip->ocp_stat & oc_ever_mask)
-		status[8] |= 0x01;
-#endif
-
-	if (card == SD_CARD) {
-		if (CHK_SD(sd_card)) {
-			if (CHK_SD_HCXC(sd_card)) {
-				if (sd_card->capacity > 0x4000000)
-					/* SDXC */
-					status[0x0E] = 0x02;
-				else /* SDHC */
-					status[0x0E] = 0x01;
-			} else { /* SDSC */
-				status[0x0E] = 0x00;
-			}
-
-			if (CHK_SD_SDR104(sd_card))
-				status[0x0F] = 0x03;
-			else if (CHK_SD_DDR50(sd_card))
-				status[0x0F] = 0x04;
-			else if (CHK_SD_SDR50(sd_card))
-				status[0x0F] = 0x02;
-			else if (CHK_SD_HS(sd_card))
-				status[0x0F] = 0x01;
-			else
-				status[0x0F] = 0x00; /* Normal speed */
-		} else {
-			if (CHK_MMC_SECTOR_MODE(sd_card))
-				status[0x0E] = 0x01; /* High capacity */
-			else
-				status[0x0E] = 0x00; /* Normal capacity */
-
-			if (CHK_MMC_DDR52(sd_card))
-				status[0x0F] = 0x03; /* DDR 52M */
-			else if (CHK_MMC_52M(sd_card))
-				status[0x0F] = 0x02; /* SDR 52M */
-			else if (CHK_MMC_26M(sd_card))
-				status[0x0F] = 0x01; /* SDR 26M */
-			else
-				status[0x0F] = 0x00; /* Normal speed */
-		}
-	} else if (card == MS_CARD) {
-		if (CHK_MSPRO(ms_card)) {
-			if (CHK_MSXC(ms_card))
-				status[0x0E] = 0x01; /* XC */
-			else
-				status[0x0E] = 0x00;
-
-			if (CHK_HG8BIT(ms_card))
-				status[0x0F] = 0x01;
-			else
-				status[0x0F] = 0x00;
-		}
-	}
-
-	/* Function 0
-	 * Support Magic Gate, CPRM and PhyRegister R/W */
-	status[0x18] = 0x8A;
-
-	/* Function 2
-	 * Support OC LUN status & WP LUN status */
-	status[0x1A] = 0x28;
-
-	/* Function 2
-	 * Support OC LUN status & WP LUN status */
-	status[0x1A] = 0x28;
-}
-
-void rts51x_read_status(struct rts51x_chip *chip, unsigned int lun,
-			u8 *rts51x_status, u8 status_len)
-{
-	if (!rts51x_status || (status_len < 16))
-		return;
-	/* VID */
-	rts51x_status[0] = (u8) (RTS51X_GET_VID(chip) >> 8);
-	rts51x_status[1] = (u8) RTS51X_GET_VID(chip);
-
-	/* PID */
-	rts51x_status[2] = (u8) (RTS51X_GET_PID(chip) >> 8);
-	rts51x_status[3] = (u8) RTS51X_GET_PID(chip);
-
-	/* gbLUN */
-	rts51x_status[4] = (u8) lun;
-
-	/* Lun Card Number */
-	if (chip->card_exist) {
-		if (chip->card_exist & XD_CARD)
-			rts51x_status[5] = 4; /* xD Card */
-		else if (chip->card_exist & SD_CARD)
-			rts51x_status[5] = 2; /* SD Card */
-		else if (chip->card_exist & MS_CARD)
-			rts51x_status[5] = 3; /* MS Card */
-		else
-			rts51x_status[5] = 7; /* Multi */
-	} else {
-		rts51x_status[5] = 7;	/* Multi */
-	}
-
-	/* Total LUNs */
-	rts51x_status[6] = 1;
-
-	/* IC Version */
-	rts51x_status[7] = (u8) RTS51X_GET_PID(chip);
-	rts51x_status[8] = chip->ic_version;
-
-	/* Physical Exist */
-	if (check_card_exist(chip, lun))
-		rts51x_status[9] = 1;
-	else
-		rts51x_status[9] = 0;
-
-	/* Multi Flag */
-	rts51x_status[10] = 1;
-
-	/* LUN Valid Map */
-	rts51x_status[11] = XD_CARD | SD_CARD | MS_CARD;
-
-	/* Logical Exist */
-	if (check_card_ready(chip, lun))
-		rts51x_status[12] = 1;
-	else
-		rts51x_status[12] = 0;
-
-	/* Detailed Type */
-	if (rts51x_get_lun_card(chip, lun) == XD_CARD) {
-		rts51x_status[13] = 0x40;
-	} else if (rts51x_get_lun_card(chip, lun) == SD_CARD) {
-		struct sd_info *sd_card = &(chip->sd_card);
-
-		rts51x_status[13] = 0x20;
-		if (CHK_SD(sd_card)) {
-			if (CHK_SD_HCXC(sd_card))
-				rts51x_status[13] |= 0x04; /* Hi capacity SD */
-			if (CHK_SD_HS(sd_card))
-				rts51x_status[13] |= 0x02; /* Hi speed SD */
-		} else {
-			rts51x_status[13] |= 0x08; /* MMC card */
-			if (CHK_MMC_52M(sd_card))
-				rts51x_status[13] |= 0x02; /* Hi speed */
-			if (CHK_MMC_SECTOR_MODE(sd_card))
-				rts51x_status[13] |= 0x04; /* Hi capacity */
-		}
-	} else if (rts51x_get_lun_card(chip, lun) == MS_CARD) {
-		struct ms_info *ms_card = &(chip->ms_card);
-
-		if (CHK_MSPRO(ms_card)) {
-			rts51x_status[13] = 0x38; /* MS Pro */
-			if (CHK_HG8BIT(ms_card))
-				rts51x_status[13] |= 0x04; /* HG */
-#ifdef SUPPORT_MSXC
-			if (CHK_MSXC(ms_card))
-				rts51x_status[13] |= 0x01; /* MSXC */
-#endif
-		} else {
-			rts51x_status[13] = 0x30;
-		}
-	} else {
-		rts51x_status[13] = 0x70;
-	}
-/* Support OC, auto delink, vendor r/w, get bus width */
-	rts51x_status[14] = 0x78;
-
-	rts51x_status[15] = 0x82;
-}
-
-int rts51x_transfer_data_rcc(struct rts51x_chip *chip, unsigned int pipe,
-			     void *buf, unsigned int len, int use_sg,
-			     unsigned int *act_len, int timeout, u8 stage_flag)
-{
-	int retval;
-
-	retval =
-	    rts51x_transfer_data(chip, pipe, buf, len, use_sg, act_len,
-				 timeout);
-
-	return retval;
-
-}
diff --git a/drivers/staging/rts5139/rts51x_chip.h b/drivers/staging/rts5139/rts51x_chip.h
deleted file mode 100644
index 12deb24cfbb..00000000000
--- a/drivers/staging/rts5139/rts51x_chip.h
+++ /dev/null
@@ -1,821 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_CHIP_H
-#define __RTS51X_CHIP_H
-
-#include <linux/usb.h>
-#include <linux/usb_usual.h>
-#include <linux/blkdev.h>
-#include <linux/completion.h>
-#include <linux/mutex.h>
-#include <scsi/scsi_host.h>
-
-#include "trace.h"
-
-#define SUPPORT_CPRM
-#define SUPPORT_MAGIC_GATE
-#define SUPPORT_MSXC
-#define USING_POLLING_CYCLE_DELINK
-
-#ifdef SUPPORT_MAGIC_GA
-/* Using NORMAL_WRITE instead of AUTO_WRITE to set ICVTE */
-#define MG_SET_ICV_SLOW
-#endif
-
-#ifdef SUPPORT_MSXC
-#define XC_POWERCLASS
-#define SUPPORT_PCGL_1P18
-#endif
-
-#define GET_CARD_STATUS_USING_EPC
-
-#define CLOSE_SSC_POWER
-
-#define SUPPORT_OCP
-
-#define MS_SPEEDUP
-
-#define SD_XD_IO_FOLLOW_PWR
-
-#define SD_NR		2
-#define MS_NR		3
-#define XD_NR		4
-#define SD_CARD		(1 << SD_NR)
-#define MS_CARD		(1 << MS_NR)
-#define XD_CARD		(1 << XD_NR)
-
-#define SD_CD		0x01
-#define MS_CD		0x02
-#define XD_CD		0x04
-#define SD_WP		0x08
-
-#define MAX_ALLOWED_LUN_CNT	8
-#define CMD_BUF_LEN		1024
-#define POLLING_INTERVAL	50	/* 50ms */
-
-#define XD_FREE_TABLE_CNT	1200
-#define MS_FREE_TABLE_CNT	512
-
-/* Bit Operation */
-#define SET_BIT(data, idx)	((data) |= 1 << (idx))
-#define CLR_BIT(data, idx)	((data) &= ~(1 << (idx)))
-#define CHK_BIT(data, idx)	((data) & (1 << (idx)))
-
-/* Command type */
-#define READ_REG_CMD		0
-#define WRITE_REG_CMD		1
-#define CHECK_REG_CMD		2
-
-#define PACKET_TYPE		4
-#define CNT_H			5
-#define CNT_L			6
-#define STAGE_FLAG		7
-#define CMD_OFFSET		8
-
-/* Packet type */
-#define BATCH_CMD		0
-#define SEQ_READ		1
-#define SEQ_WRITE		2
-
-/* Stage flag */
-#define STAGE_R			0x01
-#define STAGE_DI		0x02
-#define STAGE_DO		0x04
-/* Return MS_TRANS_CFG, GET_INT */
-#define STAGE_MS_STATUS		0x08
-/* Return XD_CFG, XD_CTL, XD_PAGE_STATUS */
-#define STAGE_XD_STATUS		0x10
-/* Command stage mode */
-#define MODE_C			0x00
-#define MODE_CR			(STAGE_R)
-#define MODE_CDIR		(STAGE_R | STAGE_DI)
-#define MODE_CDOR		(STAGE_R | STAGE_DO)
-
-/* Function return code */
-#ifndef STATUS_SUCCESS
-#define STATUS_SUCCESS		0
-#endif
-
-#define STATUS_FAIL		1
-#define STATUS_TIMEDOUT		4
-#define STATUS_NOMEM		5
-#define STATUS_TRANS_SHORT	6
-#define STATUS_TRANS_LONG	7
-#define STATUS_STALLED		8
-#define STATUS_ERROR		10
-
-#define IDLE_MAX_COUNT		10
-#define POLLING_WAIT_CNT	1
-#define LED_GPIO		0
-
-/* package */
-#define QFN24			0
-#define LQFP48			1
-
-#define USB_11			0
-#define USB_20			1
-
-/*
- * Transport return codes
- */
-/* Transport good, command good */
-#define TRANSPORT_GOOD		0
-/* Transport good, command failed */
-#define TRANSPORT_FAILED	1
-/* Transport bad (i.e. device dead) */
-#define TRANSPORT_ERROR		3
-
-/* Supported Clock */
-enum card_clock { CLK_20 = 1, CLK_30, CLK_40, CLK_50, CLK_60, CLK_80, CLK_100 };
-
-#ifdef _MSG_TRACE
-
-#define TRACE_ITEM_CNT		64
-
-struct trace_msg_t {
-	u16 line;
-#define MSG_FUNC_LEN 64
-	char func[MSG_FUNC_LEN];
-#define MSG_FILE_LEN 32
-	char file[MSG_FILE_LEN];
-#define TIME_VAL_LEN 16
-	u8 timeval_buf[TIME_VAL_LEN];
-	u8 valid;
-};
-
-#endif /* _MSG_TRACE */
-
-/* Size of the autosense data buffer */
-#define SENSE_SIZE		18
-
-/* Sense type */
-#define	SENSE_TYPE_NO_SENSE				0
-#define	SENSE_TYPE_MEDIA_CHANGE				1
-#define	SENSE_TYPE_MEDIA_NOT_PRESENT			2
-#define	SENSE_TYPE_MEDIA_LBA_OVER_RANGE			3
-#define	SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT		4
-#define	SENSE_TYPE_MEDIA_WRITE_PROTECT			5
-#define	SENSE_TYPE_MEDIA_INVALID_CMD_FIELD		6
-#define	SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR		7
-#define	SENSE_TYPE_MEDIA_WRITE_ERR			8
-#define SENSE_TYPE_FORMAT_CMD_FAILED			10
-#ifdef SUPPORT_MAGIC_GATE
-/* COPY PROTECTION KEY EXCHANGE FAILURE - KEY NOT ESTABLISHED */
-#define SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB		0x0b
-/* COPY PROTECTION KEY EXCHANGE FAILURE - AUTHENTICATION FAILURE */
-#define SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN		0x0c
-/* INCOMPATIBLE MEDIUM INSTALLED */
-#define SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM		0x0d
-/* WRITE ERROR */
-#define SENSE_TYPE_MG_WRITE_ERR				0x0e
-#endif
-
-/*---- sense key ----*/
-#define ILGAL_REQ               0x05	/* CDB/parameter/identify msg error */
-
-/*-----------------------------------
-    SENSE_DATA
------------------------------------*/
-
-/*---- error code ----*/
-#define CUR_ERR                 0x70	/* current error                    */
-
-/*---- sense key Information ----*/
-
-#define SKSV                    0x80
-#define CDB_ILLEGAL             0x40
-
-/*---- ASC ----*/
-#define ASC_INVLD_CDB           0x24
-
-/*---- ASQC ----*/
-#define ASCQ_INVLD_CDB          0x00
-
-struct sense_data_t {
-	unsigned char err_code;	/* error code */
-	/* bit7 : valid                    */
-	/*   (1 : SCSI2)                    */
-	/*   (0 : Vendor specific)          */
-	/* bit6-0 : error code             */
-	/*  (0x70 : current error)          */
-	/*  (0x71 : specific command error) */
-	unsigned char seg_no;	/* segment No.                      */
-	unsigned char sense_key;	/* byte5 : ILI                      */
-	/* bit3-0 : sense key              */
-	unsigned char info[4];	/* information                       */
-	unsigned char ad_sense_len;	/* additional sense data length     */
-	unsigned char cmd_info[4];	/* command specific information      */
-	unsigned char asc;	/* ASC                              */
-	unsigned char ascq;	/* ASCQ                             */
-	unsigned char rfu;	/* FRU                              */
-	unsigned char sns_key_info[3];	/* sense key specific information    */
-};
-
-/* sd_ctl bit map */
-/* SD push point control, bit 0, 1 */
-#define SD_PUSH_POINT_CTL_MASK		0x03
-#define SD_PUSH_POINT_DELAY		0x01
-#define SD_PUSH_POINT_AUTO		0x02
-/* SD sample point control, bit 2, 3 */
-#define SD_SAMPLE_POINT_CTL_MASK	0x0C
-#define SD_SAMPLE_POINT_DELAY		0x04
-#define SD_SAMPLE_POINT_AUTO		0x08
-/* SD DDR Tx phase set by user, bit 4 */
-#define SD_DDR_TX_PHASE_SET_BY_USER	0x10
-/* MMC DDR Tx phase set by user, bit 5 */
-#define MMC_DDR_TX_PHASE_SET_BY_USER	0x20
-/* Support MMC DDR mode, bit 6 */
-/*#define SUPPORT_MMC_DDR_MODE          0x40 */
-#define SUPPORT_UHS50_MMC44		0x40
-
-struct rts51x_option {
-	int rts51x_mspro_formatter_enable;
-
-	/* card clock expected by user for fpga platform */
-	int fpga_sd_sdr104_clk;
-	int fpga_sd_ddr50_clk;
-	int fpga_sd_sdr50_clk;
-	int fpga_sd_hs_clk;
-	int fpga_mmc_52m_clk;
-	int fpga_ms_hg_clk;
-	int fpga_ms_4bit_clk;
-
-	/* card clock expected by user for asic platform */
-	int asic_sd_sdr104_clk;
-	int asic_sd_ddr50_clk;
-	int asic_sd_sdr50_clk;
-	int asic_sd_hs_clk;
-	int asic_mmc_52m_clk;
-	int asic_ms_hg_clk;
-	int asic_ms_4bit_clk;
-
-	u8 ssc_depth_sd_sdr104;	/* sw */
-	u8 ssc_depth_sd_ddr50;	/* sw */
-	u8 ssc_depth_sd_sdr50;	/* sw */
-	u8 ssc_depth_sd_hs;	/* sw */
-	u8 ssc_depth_mmc_52m;	/* sw */
-	u8 ssc_depth_ms_hg;	/* sw */
-	u8 ssc_depth_ms_4bit;	/* sw */
-	u8 ssc_depth_low_speed;	/* sw */
-
-	/* SD/MMC Tx phase */
-	int sd_ddr_tx_phase;	/* Enabled by bit 4 of sd_ctl */
-	int mmc_ddr_tx_phase;	/* Enabled by bit 5 of sd_ctl */
-
-	/* priority of choosing sd speed funciton */
-	u32 sd_speed_prior;
-
-	/* sd card control */
-	u32 sd_ctl;
-
-	/* Enable Selective Suspend */
-	int ss_en;
-	/* Interval to enter SS from IDLE state (second) */
-	int ss_delay;
-
-	/* Enable SSC clock */
-	int ssc_en;
-
-	int auto_delink_en;
-
-	/* sangdy2010-07-13:add FT2 fast mode */
-	int FT2_fast_mode;
-	/* sangdy2010-07-15:
-	 * add for config delay between 1/4 PMOS and 3/4 PMOS */
-	int pwr_delay;
-
-	int rts51x_xd_rw_step;		/* add to tune xd tRP */
-	int D3318_off_delay;	/* add to tune D3318 off delay time */
-	int delink_delay;	/* add to tune delink delay time */
-	/* add for rts5129 to enable/disable D3318 off */
-	u8 rts5129_D3318_off_enable;
-	u8 sd20_pad_drive;	/* add to config SD20 PAD drive */
-	u8 sd30_pad_drive;	/* add to config SD30 pad drive */
-	/*if reset or rw fail,then set SD20 pad drive again */
-	u8 reset_or_rw_fail_set_pad_drive;
-
-	u8 debounce_num;	/* debounce number */
-	u8 led_toggle_interval;	/* used to control led toggle speed */
-	int rts51x_xd_rwn_step;
-	u8 sd_send_status_en;
-	/* used to store default phase which is
-	 * used when phase tune all pass. */
-	u8 ddr50_tx_phase;
-	u8 ddr50_rx_phase;
-	u8 sdr50_tx_phase;
-	u8 sdr50_rx_phase;
-	/* used to enable select sdr50 tx phase according to proportion. */
-	u8 sdr50_phase_sel;
-	u8 ms_errreg_fix;
-	u8 reset_mmc_first;
-	u8 speed_mmc;		/* when set, then try CMD55 only twice */
-	u8 led_always_on;	/* if set, then led always on when card exist */
-	u8 dv18_voltage;	/* add to tune dv18 voltage */
-};
-
-#define MS_FORMATTER_ENABLED(chip)	((chip)->option.rts51x_mspro_formatter_enable)
-
-struct rts51x_chip;
-
-typedef int (*rts51x_card_rw_func) (struct scsi_cmnd *srb, struct rts51x_chip *chip,
-			     u32 sec_addr, u16 sec_cnt);
-
-/* For MS Card */
-#define    MAX_DEFECTIVE_BLOCK     10
-
-struct zone_entry {
-	u16 *l2p_table;
-	u16 *free_table;
-	u16 defect_list[MAX_DEFECTIVE_BLOCK];	/* For MS card only */
-	int set_index;
-	int get_index;
-	int unused_blk_cnt;
-	int disable_count;
-	/* To indicate whether the L2P table of this zone has been built. */
-	int build_flag;
-};
-
-struct xd_delay_write_tag {
-	u32 old_phyblock;
-	u32 new_phyblock;
-	u32 logblock;
-	u8 pageoff;
-	u8 delay_write_flag;
-};
-
-struct xd_info {
-	u8 maker_code;
-	u8 device_code;
-	u8 block_shift;
-	u8 page_off;
-	u8 addr_cycle;
-	u16 cis_block;
-	u8 multi_flag;
-	u8 err_code;
-	u32 capacity;
-
-	struct zone_entry *zone;
-	int zone_cnt;
-
-	struct xd_delay_write_tag delay_write;
-
-	int counter;
-
-	int xd_clock;
-};
-
-#define TYPE_SD			0x0000
-#define TYPE_MMC		0x0001
-
-/* TYPE_SD */
-#define SD_HS			0x0100
-#define SD_SDR50		0x0200
-#define SD_DDR50		0x0400
-#define SD_SDR104		0x0800
-#define SD_HCXC			0x1000
-
-/* TYPE_MMC */
-#define MMC_26M			0x0100
-#define MMC_52M			0x0200
-#define MMC_4BIT		0x0400
-#define MMC_8BIT		0x0800
-#define MMC_SECTOR_MODE		0x1000
-#define MMC_DDR52		0x2000
-
-/* SD card */
-#define CHK_SD(sd_card)			(((sd_card)->sd_type & 0xFF) == TYPE_SD)
-#define CHK_SD_HS(sd_card)	\
-	(CHK_SD(sd_card) && ((sd_card)->sd_type & SD_HS))
-#define CHK_SD_SDR50(sd_card)		\
-	(CHK_SD(sd_card) && ((sd_card)->sd_type & SD_SDR50))
-#define CHK_SD_DDR50(sd_card)	\
-	(CHK_SD(sd_card) && ((sd_card)->sd_type & SD_DDR50))
-#define CHK_SD_SDR104(sd_card)	\
-	(CHK_SD(sd_card) && ((sd_card)->sd_type & SD_SDR104))
-#define CHK_SD_HCXC(sd_card)	\
-	(CHK_SD(sd_card) && ((sd_card)->sd_type & SD_HCXC))
-#define CHK_SD30_SPEED(sd_card)	\
-	(CHK_SD_SDR50(sd_card) || CHK_SD_DDR50(sd_card) ||\
-	 CHK_SD_SDR104(sd_card))
-
-#define SET_SD(sd_card)			((sd_card)->sd_type = TYPE_SD)
-#define SET_SD_HS(sd_card)		((sd_card)->sd_type |= SD_HS)
-#define SET_SD_SDR50(sd_card)		((sd_card)->sd_type |= SD_SDR50)
-#define SET_SD_DDR50(sd_card)		((sd_card)->sd_type |= SD_DDR50)
-#define SET_SD_SDR104(sd_card)		((sd_card)->sd_type |= SD_SDR104)
-#define SET_SD_HCXC(sd_card)		((sd_card)->sd_type |= SD_HCXC)
-
-#define CLR_SD_HS(sd_card)		((sd_card)->sd_type &= ~SD_HS)
-#define CLR_SD_SDR50(sd_card)		((sd_card)->sd_type &= ~SD_SDR50)
-#define CLR_SD_DDR50(sd_card)		((sd_card)->sd_type &= ~SD_DDR50)
-#define CLR_SD_SDR104(sd_card)		((sd_card)->sd_type &= ~SD_SDR104)
-#define CLR_SD_HCXC(sd_card)		((sd_card)->sd_type &= ~SD_HCXC)
-#define CLR_SD30_SPEED(sd_card)	\
-	((sd_card)->sd_type &= ~(SD_SDR50|SD_DDR50|SD_SDR104))
-
-/* MMC card */
-#define CHK_MMC(sd_card)	\
-	(((sd_card)->sd_type & 0xFF) == TYPE_MMC)
-#define CHK_MMC_26M(sd_card)	\
-	(CHK_MMC(sd_card) && ((sd_card)->sd_type & MMC_26M))
-#define CHK_MMC_52M(sd_card)	\
-	(CHK_MMC(sd_card) && ((sd_card)->sd_type & MMC_52M))
-#define CHK_MMC_4BIT(sd_card)	\
-	(CHK_MMC(sd_card) && ((sd_card)->sd_type & MMC_4BIT))
-#define CHK_MMC_8BIT(sd_card)	\
-	(CHK_MMC(sd_card) && ((sd_card)->sd_type & MMC_8BIT))
-#define CHK_MMC_SECTOR_MODE(sd_card)\
-	(CHK_MMC(sd_card) && ((sd_card)->sd_type & MMC_SECTOR_MODE))
-#define CHK_MMC_DDR52(sd_card)	\
-	(CHK_MMC(sd_card) && ((sd_card)->sd_type & MMC_DDR52))
-
-#define SET_MMC(sd_card)		((sd_card)->sd_type = TYPE_MMC)
-#define SET_MMC_26M(sd_card)		((sd_card)->sd_type |= MMC_26M)
-#define SET_MMC_52M(sd_card)		((sd_card)->sd_type |= MMC_52M)
-#define SET_MMC_4BIT(sd_card)		((sd_card)->sd_type |= MMC_4BIT)
-#define SET_MMC_8BIT(sd_card)		((sd_card)->sd_type |= MMC_8BIT)
-#define SET_MMC_SECTOR_MODE(sd_card)	((sd_card)->sd_type |= MMC_SECTOR_MODE)
-#define SET_MMC_DDR52(sd_card)		((sd_card)->sd_type |= MMC_DDR52)
-
-#define CLR_MMC_26M(sd_card)		((sd_card)->sd_type &= ~MMC_26M)
-#define CLR_MMC_52M(sd_card)		((sd_card)->sd_type &= ~MMC_52M)
-#define CLR_MMC_4BIT(sd_card)		((sd_card)->sd_type &= ~MMC_4BIT)
-#define CLR_MMC_8BIT(sd_card)		((sd_card)->sd_type &= ~MMC_8BIT)
-#define CLR_MMC_SECTOR_MODE(sd_card)	((sd_card)->sd_type &= ~MMC_SECTOR_MODE)
-#define CLR_MMC_DDR52(sd_card)		((sd_card)->sd_type &= ~MMC_DDR52)
-
-#define CHK_MMC_HS(sd_card)	\
-	(CHK_MMC_52M(sd_card) && CHK_MMC_26M(sd_card))
-#define CLR_MMC_HS(sd_card)			\
-do {						\
-	CLR_MMC_DDR52(sd_card);			\
-	CLR_MMC_52M(sd_card);			\
-	CLR_MMC_26M(sd_card);			\
-} while (0)
-
-#define SD_SUPPORT_CLASS_TEN		0x01
-#define SD_SUPPORT_1V8			0x02
-
-#define SD_SET_CLASS_TEN(sd_card)	\
-	((sd_card)->sd_setting |= SD_SUPPORT_CLASS_TEN)
-#define SD_CHK_CLASS_TEN(sd_card)	\
-	((sd_card)->sd_setting & SD_SUPPORT_CLASS_TEN)
-#define SD_CLR_CLASS_TEN(sd_card)	\
-	((sd_card)->sd_setting &= ~SD_SUPPORT_CLASS_TEN)
-#define SD_SET_1V8(sd_card)		\
-	((sd_card)->sd_setting |= SD_SUPPORT_1V8)
-#define SD_CHK_1V8(sd_card)		\
-	((sd_card)->sd_setting & SD_SUPPORT_1V8)
-#define SD_CLR_1V8(sd_card)		\
-	((sd_card)->sd_setting &= ~SD_SUPPORT_1V8)
-#define CLR_RETRY_SD20_MODE(sd_card)		\
-	((sd_card)->retry_SD20_mode = 0)
-#define SET_RETRY_SD20_MODE(sd_card)		\
-	((sd_card)->retry_SD20_mode = 1)
-#define CHK_RETRY_SD20_MODE(sd_card)		\
-	((sd_card)->retry_SD20_mode == 1)
-
-struct sd_info {
-	u16 sd_type;
-	u8 err_code;
-	u8 sd_data_buf_ready;
-	u32 sd_addr;
-	u32 capacity;
-
-	u8 raw_csd[16];
-	u8 raw_scr[8];
-
-	/* Sequential RW */
-	int seq_mode;
-	enum dma_data_direction pre_dir;
-	u32 pre_sec_addr;
-	u16 pre_sec_cnt;
-
-	int counter;
-
-	int sd_clock;
-
-#ifdef SUPPORT_CPRM
-	int sd_pass_thru_en;
-	int pre_cmd_err;
-	u8 last_rsp_type;
-	u8 rsp[17];
-#endif
-
-	u8 func_group1_mask;
-	u8 func_group2_mask;
-	u8 func_group3_mask;
-	u8 func_group4_mask;
-
-	u8 sd_switch_fail;
-	u8 sd_read_phase;
-	u8 retry_SD20_mode;	/* sangdy2010-06-10 */
-	u8 sd_reset_fail;	/* sangdy2010-07-01 */
-	u8 sd_send_status_en;
-
-};
-
-#define MODE_512_SEQ		0x01
-#define MODE_2K_SEQ		0x02
-
-#define TYPE_MS			0x0000
-#define TYPE_MSPRO		0x0001
-
-#define MS_4BIT			0x0100
-#define MS_8BIT			0x0200
-#define MS_HG			0x0400
-#define MS_XC			0x0800
-
-#define HG8BIT			(MS_HG | MS_8BIT)
-
-#define CHK_MSPRO(ms_card)	\
-	(((ms_card)->ms_type & 0xFF) == TYPE_MSPRO)
-#define CHK_HG8BIT(ms_card)	\
-	(CHK_MSPRO(ms_card) && (((ms_card)->ms_type & HG8BIT) == HG8BIT))
-#define CHK_MSXC(ms_card)	\
-	(CHK_MSPRO(ms_card) && ((ms_card)->ms_type & MS_XC))
-#define CHK_MSHG(ms_card)	\
-	(CHK_MSPRO(ms_card) && ((ms_card)->ms_type & MS_HG))
-
-#define CHK_MS8BIT(ms_card)	(((ms_card)->ms_type & MS_8BIT))
-#define CHK_MS4BIT(ms_card)	(((ms_card)->ms_type & MS_4BIT))
-
-struct rts51x_ms_delay_write_tag {
-	u16 old_phyblock;
-	u16 new_phyblock;
-	u16 logblock;
-	u8 pageoff;
-	u8 delay_write_flag;
-};
-
-struct ms_info {
-	u16 ms_type;
-	u8 block_shift;
-	u8 page_off;
-	u16 total_block;
-	u16 boot_block;
-	u32 capacity;
-
-	u8 check_ms_flow;
-	u8 switch_8bit_fail;
-	u8 err_code;
-
-	struct zone_entry *segment;
-	int segment_cnt;
-
-	int pro_under_formatting;
-	int format_status;
-	u16 progress;
-	u8 raw_sys_info[96];
-#ifdef SUPPORT_PCGL_1P18
-	u8 raw_model_name[48];
-#endif
-
-	u8 multi_flag;
-
-	/* Sequential RW */
-	u8 seq_mode;
-	enum dma_data_direction pre_dir;
-	u32 pre_sec_addr;
-	u16 pre_sec_cnt;
-	u32 total_sec_cnt;
-	u8 last_rw_int;
-
-	struct rts51x_ms_delay_write_tag delay_write;
-
-	int counter;
-
-	int ms_clock;
-
-#ifdef SUPPORT_MAGIC_GATE
-	u8 magic_gate_id[16];
-	u8 mg_entry_num;
-	int mg_auth;		/* flag to indicate authentication process */
-#endif
-};
-
-#define PRO_UNDER_FORMATTING(ms_card)		\
-	((ms_card)->pro_under_formatting)
-#define SET_FORMAT_STATUS(ms_card, status)	\
-	((ms_card)->format_status = (status))
-#define CHK_FORMAT_STATUS(ms_card, status)	\
-	((ms_card)->format_status == (status))
-
-struct scsi_cmnd;
-
-enum CHIP_STAT { STAT_INIT, STAT_IDLE, STAT_RUN, STAT_SS_PRE, STAT_SS,
-	    STAT_SUSPEND };
-
-struct rts51x_chip {
-	u16 vendor_id;
-	u16 product_id;
-	char max_lun;
-
-	struct scsi_cmnd *srb;
-	struct sense_data_t sense_buffer[MAX_ALLOWED_LUN_CNT];
-
-	int led_toggle_counter;
-
-	int ss_counter;
-	int idle_counter;
-	int auto_delink_counter;
-	enum CHIP_STAT chip_stat;
-
-	int resume_from_scsi;
-
-	/* Card information */
-	struct xd_info xd_card;
-	struct sd_info sd_card;
-	struct ms_info ms_card;
-
-	int cur_clk;		/* current card clock */
-	int cur_card;		/* Current card module */
-
-	u8 card_exist;		/* card exist bit map (physical exist) */
-	u8 card_ready;		/* card ready bit map (reset successfully) */
-	u8 card_fail;		/* card reset fail bit map */
-	u8 card_ejected;	/* card ejected bit map */
-	u8 card_wp;		/* card write protected bit map */
-
-	u8 fake_card_ready;
-	/* flag to indicate whether to answer MediaChange */
-	unsigned long lun_mc;
-
-	/* card bus width */
-	u8 card_bus_width[MAX_ALLOWED_LUN_CNT];
-	/* card capacity */
-	u32 capacity[MAX_ALLOWED_LUN_CNT];
-
-	/* read/write card function pointer */
-	rts51x_card_rw_func rw_card[MAX_ALLOWED_LUN_CNT];
-	/* read/write capacity, used for GPIO Toggle */
-	u32 rw_cap[MAX_ALLOWED_LUN_CNT];
-	/* card to lun mapping table */
-	u8 card2lun[32];
-	/* lun to card mapping table */
-	u8 lun2card[MAX_ALLOWED_LUN_CNT];
-
-#ifdef _MSG_TRACE
-	struct trace_msg_t trace_msg[TRACE_ITEM_CNT];
-	int msg_idx;
-#endif
-
-	int rw_need_retry;
-
-	/* ASIC or FPGA */
-	int asic_code;
-
-	/* QFN24 or LQFP48 */
-	int package;
-
-	/* Full Speed or High Speed */
-	int usb_speed;
-
-	/*sangdy:enable or disable UHS50 and MMC4.4 */
-	int uhs50_mmc44_en;
-
-	u8 ic_version;
-
-	/* Command buffer */
-	u8 *cmd_buf;
-	unsigned int cmd_idx;
-	/* Response buffer */
-	u8 *rsp_buf;
-
-	u16 card_status;
-
-#ifdef SUPPORT_OCP
-	u16 ocp_stat;
-#endif
-
-	struct rts51x_option option;
-	struct rts51x_usb *usb;
-
-	u8 rcc_read_response;
-	int reset_need_retry;
-	u8 rts5179;
-};
-
-#define UHS50_EN 0x0001
-#define UHS50_DIS 0x0000
-#define SET_UHS50(chip)   ((chip)->uhs50_mmc44_en = UHS50_EN)
-#define CLEAR_UHS50(chip)  ((chip)->uhs50_mmc44_en = UHS50_DIS)
-#define CHECK_UHS50(chip)  (((chip)->uhs50_mmc44_en&0xff) == UHS50_EN)
-
-#define RTS51X_GET_VID(chip)		((chip)->vendor_id)
-#define RTS51X_GET_PID(chip)		((chip)->product_id)
-
-#define RTS51X_SET_STAT(chip, stat)			\
-do {							\
-	if ((stat) != STAT_IDLE) {			\
-		(chip)->idle_counter = 0;		\
-	}						\
-	(chip)->chip_stat = (enum CHIP_STAT)(stat);	\
-} while (0)
-#define RTS51X_CHK_STAT(chip, stat)	((chip)->chip_stat == (stat))
-#define RTS51X_GET_STAT(chip)		((chip)->chip_stat)
-
-#define CHECK_PID(chip, pid)		(RTS51X_GET_PID(chip) == (pid))
-#define CHECK_PKG(chip, pkg)		((chip)->package == (pkg))
-#define CHECK_USB(chip, speed)		((chip)->usb_speed == (speed))
-
-int rts51x_reset_chip(struct rts51x_chip *chip);
-int rts51x_init_chip(struct rts51x_chip *chip);
-int rts51x_release_chip(struct rts51x_chip *chip);
-void rts51x_polling_func(struct rts51x_chip *chip);
-
-static inline void rts51x_init_cmd(struct rts51x_chip *chip)
-{
-	chip->cmd_idx = 0;
-	chip->cmd_buf[0] = 'R';
-	chip->cmd_buf[1] = 'T';
-	chip->cmd_buf[2] = 'C';
-	chip->cmd_buf[3] = 'R';
-	chip->cmd_buf[PACKET_TYPE] = BATCH_CMD;
-}
-
-void rts51x_add_cmd(struct rts51x_chip *chip,
-		    u8 cmd_type, u16 reg_addr, u8 mask, u8 data);
-int rts51x_send_cmd(struct rts51x_chip *chip, u8 flag, int timeout);
-int rts51x_get_rsp(struct rts51x_chip *chip, int rsp_len, int timeout);
-
-static inline void rts51x_read_rsp_buf(struct rts51x_chip *chip, int offset,
-				       u8 *buf, int buf_len)
-{
-	memcpy(buf, chip->rsp_buf + offset, buf_len);
-}
-
-static inline u8 *rts51x_get_rsp_data(struct rts51x_chip *chip)
-{
-	return chip->rsp_buf;
-}
-
-int rts51x_get_card_status(struct rts51x_chip *chip, u16 *status);
-int rts51x_write_register(struct rts51x_chip *chip, u16 addr, u8 mask, u8 data);
-int rts51x_read_register(struct rts51x_chip *chip, u16 addr, u8 *data);
-int rts51x_ep0_write_register(struct rts51x_chip *chip, u16 addr, u8 mask,
-			      u8 data);
-int rts51x_ep0_read_register(struct rts51x_chip *chip, u16 addr, u8 *data);
-int rts51x_seq_write_register(struct rts51x_chip *chip, u16 addr, u16 len,
-			      u8 *data);
-int rts51x_seq_read_register(struct rts51x_chip *chip, u16 addr, u16 len,
-			     u8 *data);
-int rts51x_read_ppbuf(struct rts51x_chip *chip, u8 *buf, int buf_len);
-int rts51x_write_ppbuf(struct rts51x_chip *chip, u8 *buf, int buf_len);
-int rts51x_write_phy_register(struct rts51x_chip *chip, u8 addr, u8 val);
-int rts51x_read_phy_register(struct rts51x_chip *chip, u8 addr, u8 *val);
-void rts51x_do_before_power_down(struct rts51x_chip *chip);
-void rts51x_clear_hw_error(struct rts51x_chip *chip);
-void rts51x_prepare_run(struct rts51x_chip *chip);
-void rts51x_trace_msg(struct rts51x_chip *chip, unsigned char *buf, int clear);
-void rts51x_pp_status(struct rts51x_chip *chip, unsigned int lun, u8 *status,
-		      u8 status_len);
-void rts51x_read_status(struct rts51x_chip *chip, unsigned int lun,
-			u8 *rts51x_status, u8 status_len);
-int rts51x_transfer_data_rcc(struct rts51x_chip *chip, unsigned int pipe,
-			     void *buf, unsigned int len, int use_sg,
-			     unsigned int *act_len, int timeout, u8 stage_flag);
-
-#define RTS51X_WRITE_REG(chip, addr, mask, data)	\
-do {							\
-	int _retval = rts51x_write_register((chip),	\
-			(addr), (mask), (data));	\
-	if (_retval != STATUS_SUCCESS) {		\
-		TRACE_RET((chip), _retval);		\
-	}						\
-} while (0)
-
-#define RTS51X_READ_REG(chip, addr, data)		\
-do {							\
-	int _retval = rts51x_read_register((chip),	\
-			(addr), (data));		\
-	if (_retval != STATUS_SUCCESS) {		\
-		TRACE_RET((chip), _retval);		\
-	}						\
-} while (0)
-
-#endif /* __RTS51X_CHIP_H */
diff --git a/drivers/staging/rts5139/rts51x_fop.c b/drivers/staging/rts5139/rts51x_fop.c
deleted file mode 100644
index 677d18b3dcd..00000000000
--- a/drivers/staging/rts5139/rts51x_fop.c
+++ /dev/null
@@ -1,292 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#include "rts51x.h"
-
-#ifdef SUPPORT_FILE_OP
-
-#include <linux/types.h>
-#include <linux/stat.h>
-#include <linux/kref.h>
-#include <linux/slab.h>
-
-#include "rts51x_chip.h"
-#include "rts51x_card.h"
-#include "rts51x_fop.h"
-#include "sd_cprm.h"
-
-#define RTS5139_IOC_MAGIC		0x39
-
-#define RTS5139_IOC_SD_DIRECT		_IOWR(RTS5139_IOC_MAGIC, 0xA0, int)
-#define RTS5139_IOC_SD_GET_RSP		_IOWR(RTS5139_IOC_MAGIC, 0xA1, int)
-
-static int rts51x_sd_direct_cmnd(struct rts51x_chip *chip,
-				 struct sd_direct_cmnd *cmnd)
-{
-	int retval;
-	u8 dir, cmd12, standby, acmd, cmd_idx, rsp_code;
-	u8 *buf;
-	u32 arg, len;
-
-	dir = (cmnd->cmnd[0] >> 3) & 0x03;
-	cmd12 = (cmnd->cmnd[0] >> 2) & 0x01;
-	standby = (cmnd->cmnd[0] >> 1) & 0x01;
-	acmd = cmnd->cmnd[0] & 0x01;
-	cmd_idx = cmnd->cmnd[1];
-	arg = ((u32) (cmnd->cmnd[2]) << 24) | ((u32) (cmnd->cmnd[3]) << 16) |
-	    ((u32) (cmnd->cmnd[4]) << 8) | cmnd->cmnd[5];
-	len =
-	    ((u32) (cmnd->cmnd[6]) << 16) | ((u32) (cmnd->cmnd[7]) << 8) |
-	    cmnd->cmnd[8];
-	rsp_code = cmnd->cmnd[9];
-
-	if (dir) {
-		if (!cmnd->buf || (cmnd->buf_len < len))
-			TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	switch (dir) {
-	case 0:
-		/* No data */
-		retval = ext_rts51x_sd_execute_no_data(chip, chip->card2lun[SD_CARD],
-						cmd_idx, standby, acmd,
-						rsp_code, arg);
-		if (retval != TRANSPORT_GOOD)
-			TRACE_RET(chip, STATUS_FAIL);
-		break;
-
-	case 1:
-		/* Read from card */
-		buf = kzalloc(cmnd->buf_len, GFP_KERNEL);
-		if (!buf)
-			TRACE_RET(chip, STATUS_NOMEM);
-
-		retval = ext_rts51x_sd_execute_read_data(chip, chip->card2lun[SD_CARD],
-						  cmd_idx, cmd12, standby, acmd,
-						  rsp_code, arg, len, buf,
-						  cmnd->buf_len, 0);
-		if (retval != TRANSPORT_GOOD) {
-			kfree(buf);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		retval =
-		    copy_to_user(cmnd->buf, (void *)buf, cmnd->buf_len);
-		if (retval) {
-			kfree(buf);
-			TRACE_RET(chip, STATUS_NOMEM);
-		}
-
-		kfree(buf);
-		break;
-
-	case 2:
-		/* Write to card */
-		buf = kmalloc(cmnd->buf_len, GFP_KERNEL);
-		if (!buf)
-			TRACE_RET(chip, STATUS_NOMEM);
-
-		retval =
-		    copy_from_user((void *)buf, cmnd->buf,
-				   cmnd->buf_len);
-		if (retval) {
-			kfree(buf);
-			TRACE_RET(chip, STATUS_NOMEM);
-		}
-
-		retval =
-		    ext_rts51x_sd_execute_write_data(chip, chip->card2lun[SD_CARD],
-					      cmd_idx, cmd12, standby, acmd,
-					      rsp_code, arg, len, buf,
-					      cmnd->buf_len, 0);
-		if (retval != TRANSPORT_GOOD) {
-			kfree(buf);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		kfree(buf);
-
-		break;
-
-	default:
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int rts51x_sd_get_rsp(struct rts51x_chip *chip, struct sd_rsp *rsp)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int count = 0, retval;
-
-	if (sd_card->pre_cmd_err) {
-		sd_card->pre_cmd_err = 0;
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (sd_card->last_rsp_type == SD_RSP_TYPE_R0)
-		TRACE_RET(chip, STATUS_FAIL);
-	else if (sd_card->last_rsp_type == SD_RSP_TYPE_R2)
-		count = (rsp->rsp_len < 17) ? rsp->rsp_len : 17;
-	else
-		count = (rsp->rsp_len < 6) ? rsp->rsp_len : 6;
-
-	retval = copy_to_user(rsp->rsp, (void *)sd_card->rsp, count);
-	if (retval)
-		TRACE_RET(chip, STATUS_NOMEM);
-
-	RTS51X_DEBUGP("Response length: %d\n", count);
-	RTS51X_DEBUGP("Response: 0x%x 0x%x 0x%x 0x%x\n",
-		       sd_card->rsp[0], sd_card->rsp[1], sd_card->rsp[2],
-		       sd_card->rsp[3]);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_open(struct inode *inode, struct file *filp)
-{
-	struct rts51x_chip *chip;
-	struct usb_interface *interface;
-	int subminor;
-	int retval = 0;
-
-	subminor = iminor(inode);
-
-	interface = usb_find_interface(&rts51x_driver, subminor);
-	if (!interface) {
-		RTS51X_DEBUGP("%s - error, can't find device for minor %d\n",
-			       __func__, subminor);
-		retval = -ENODEV;
-		goto exit;
-	}
-
-	chip = (struct rts51x_chip *)usb_get_intfdata(interface);
-	if (!chip) {
-		RTS51X_DEBUGP("Can't find chip\n");
-		retval = -ENODEV;
-		goto exit;
-	}
-
-	/* Increase our reference to the host */
-	scsi_host_get(rts51x_to_host(chip));
-
-	/* lock the device pointers */
-	mutex_lock(&(chip->usb->dev_mutex));
-
-	/* save our object in the file's private structure */
-	filp->private_data = chip;
-
-	/* unlock the device pointers */
-	mutex_unlock(&chip->usb->dev_mutex);
-
-exit:
-	return retval;
-}
-
-int rts51x_release(struct inode *inode, struct file *filp)
-{
-	struct rts51x_chip *chip;
-
-	chip = (struct rts51x_chip *)filp->private_data;
-	if (chip == NULL)
-		return -ENODEV;
-
-	/* Drop our reference to the host; the SCSI core will free it
-	 * (and "chip" along with it) when the refcount becomes 0. */
-	scsi_host_put(rts51x_to_host(chip));
-
-	return 0;
-}
-
-ssize_t rts51x_read(struct file *filp, char __user *buf, size_t count,
-		    loff_t *f_pos)
-{
-	return 0;
-}
-
-ssize_t rts51x_write(struct file *filp, const char __user *buf, size_t count,
-		     loff_t *f_pos)
-{
-	return 0;
-}
-
-long rts51x_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
-{
-	struct rts51x_chip *chip;
-	struct sd_direct_cmnd cmnd;
-	struct sd_rsp rsp;
-	int retval = 0;
-
-	chip = (struct rts51x_chip *)filp->private_data;
-	if (chip == NULL)
-		return -ENODEV;
-
-	/* lock the device pointers */
-	mutex_lock(&(chip->usb->dev_mutex));
-
-	switch (cmd) {
-	case RTS5139_IOC_SD_DIRECT:
-		retval =
-		    copy_from_user((void *)&cmnd, (void __user *)arg,
-				   sizeof(struct sd_direct_cmnd));
-		if (retval) {
-			retval = -ENOMEM;
-			TRACE_GOTO(chip, exit);
-		}
-		retval = rts51x_sd_direct_cmnd(chip, &cmnd);
-		if (retval != STATUS_SUCCESS) {
-			retval = -EIO;
-			TRACE_GOTO(chip, exit);
-		}
-		break;
-
-	case RTS5139_IOC_SD_GET_RSP:
-		retval =
-		    copy_from_user(&rsp, (void __user *)arg,
-				   sizeof(struct sd_rsp));
-		if (retval) {
-			retval = -ENOMEM;
-			TRACE_GOTO(chip, exit);
-		}
-		retval = rts51x_sd_get_rsp(chip, &rsp);
-		if (retval != STATUS_SUCCESS) {
-			retval = -EIO;
-			TRACE_GOTO(chip, exit);
-		}
-		break;
-
-	default:
-		break;
-	}
-
-exit:
-	/* unlock the device pointers */
-	mutex_unlock(&chip->usb->dev_mutex);
-
-	return retval;
-}
-
-#endif
diff --git a/drivers/staging/rts5139/rts51x_fop.h b/drivers/staging/rts5139/rts51x_fop.h
deleted file mode 100644
index c691ee99720..00000000000
--- a/drivers/staging/rts5139/rts51x_fop.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_FOP_H
-#define __RTS51X_FOP_H
-
-#include "rts51x.h"
-
-#ifdef SUPPORT_FILE_OP
-
-#include <linux/fs.h>
-#include <linux/types.h>
-
-struct sd_direct_cmnd {
-	u8 cmnd[12];
-	void __user *buf;
-	int buf_len;
-};
-
-struct sd_rsp {
-	void __user *rsp;
-	int rsp_len;
-};
-
-int rts51x_open(struct inode *inode, struct file *filp);
-int rts51x_release(struct inode *inode, struct file *filp);
-ssize_t rts51x_read(struct file *filp, char __user *buf, size_t count,
-		    loff_t *f_pos);
-ssize_t rts51x_write(struct file *filp, const char __user *buf, size_t count,
-		     loff_t *f_pos);
-long rts51x_ioctl(struct file *filp, unsigned int cmd, unsigned long arg);
-
-#endif
-
-#endif /* __RTS51X_FOP_H */
diff --git a/drivers/staging/rts5139/rts51x_scsi.c b/drivers/staging/rts5139/rts51x_scsi.c
deleted file mode 100644
index 75282fea38f..00000000000
--- a/drivers/staging/rts5139/rts51x_scsi.c
+++ /dev/null
@@ -1,2135 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#include <linux/blkdev.h>
-#include <linux/kthread.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-#include <linux/vmalloc.h>
-#include <linux/export.h>
-
-#include <scsi/scsi.h>
-#include <scsi/scsi_eh.h>
-#include <scsi/scsi_device.h>
-
-#include "debug.h"
-#include "rts51x.h"
-#include "rts51x_chip.h"
-#include "rts51x_scsi.h"
-#include "rts51x_card.h"
-#include "rts51x_transport.h"
-#include "sd_cprm.h"
-#include "ms_mg.h"
-#include "trace.h"
-
-void rts51x_scsi_show_command(struct scsi_cmnd *srb)
-{
-	char *what = NULL;
-	int i, unknown_cmd = 0;
-
-	switch (srb->cmnd[0]) {
-	case TEST_UNIT_READY:
-		what = (char *)"TEST_UNIT_READY";
-		break;
-	case REZERO_UNIT:
-		what = (char *)"REZERO_UNIT";
-		break;
-	case REQUEST_SENSE:
-		what = (char *)"REQUEST_SENSE";
-		break;
-	case FORMAT_UNIT:
-		what = (char *)"FORMAT_UNIT";
-		break;
-	case READ_BLOCK_LIMITS:
-		what = (char *)"READ_BLOCK_LIMITS";
-		break;
-	case 0x07:
-		what = (char *)"REASSIGN_BLOCKS";
-		break;
-	case READ_6:
-		what = (char *)"READ_6";
-		break;
-	case WRITE_6:
-		what = (char *)"WRITE_6";
-		break;
-	case SEEK_6:
-		what = (char *)"SEEK_6";
-		break;
-	case READ_REVERSE:
-		what = (char *)"READ_REVERSE";
-		break;
-	case WRITE_FILEMARKS:
-		what = (char *)"WRITE_FILEMARKS";
-		break;
-	case SPACE:
-		what = (char *)"SPACE";
-		break;
-	case INQUIRY:
-		what = (char *)"INQUIRY";
-		break;
-	case RECOVER_BUFFERED_DATA:
-		what = (char *)"RECOVER_BUFFERED_DATA";
-		break;
-	case MODE_SELECT:
-		what = (char *)"MODE_SELECT";
-		break;
-	case RESERVE:
-		what = (char *)"RESERVE";
-		break;
-	case RELEASE:
-		what = (char *)"RELEASE";
-		break;
-	case COPY:
-		what = (char *)"COPY";
-		break;
-	case ERASE:
-		what = (char *)"ERASE";
-		break;
-	case MODE_SENSE:
-		what = (char *)"MODE_SENSE";
-		break;
-	case START_STOP:
-		what = (char *)"START_STOP";
-		break;
-	case RECEIVE_DIAGNOSTIC:
-		what = (char *)"RECEIVE_DIAGNOSTIC";
-		break;
-	case SEND_DIAGNOSTIC:
-		what = (char *)"SEND_DIAGNOSTIC";
-		break;
-	case ALLOW_MEDIUM_REMOVAL:
-		what = (char *)"ALLOW_MEDIUM_REMOVAL";
-		break;
-	case SET_WINDOW:
-		what = (char *)"SET_WINDOW";
-		break;
-	case READ_CAPACITY:
-		what = (char *)"READ_CAPACITY";
-		break;
-	case READ_10:
-		what = (char *)"READ_10";
-		break;
-	case WRITE_10:
-		what = (char *)"WRITE_10";
-		break;
-	case SEEK_10:
-		what = (char *)"SEEK_10";
-		break;
-	case WRITE_VERIFY:
-		what = (char *)"WRITE_VERIFY";
-		break;
-	case VERIFY:
-		what = (char *)"VERIFY";
-		break;
-	case SEARCH_HIGH:
-		what = (char *)"SEARCH_HIGH";
-		break;
-	case SEARCH_EQUAL:
-		what = (char *)"SEARCH_EQUAL";
-		break;
-	case SEARCH_LOW:
-		what = (char *)"SEARCH_LOW";
-		break;
-	case SET_LIMITS:
-		what = (char *)"SET_LIMITS";
-		break;
-	case READ_POSITION:
-		what = (char *)"READ_POSITION";
-		break;
-	case SYNCHRONIZE_CACHE:
-		what = (char *)"SYNCHRONIZE_CACHE";
-		break;
-	case LOCK_UNLOCK_CACHE:
-		what = (char *)"LOCK_UNLOCK_CACHE";
-		break;
-	case READ_DEFECT_DATA:
-		what = (char *)"READ_DEFECT_DATA";
-		break;
-	case MEDIUM_SCAN:
-		what = (char *)"MEDIUM_SCAN";
-		break;
-	case COMPARE:
-		what = (char *)"COMPARE";
-		break;
-	case COPY_VERIFY:
-		what = (char *)"COPY_VERIFY";
-		break;
-	case WRITE_BUFFER:
-		what = (char *)"WRITE_BUFFER";
-		break;
-	case READ_BUFFER:
-		what = (char *)"READ_BUFFER";
-		break;
-	case UPDATE_BLOCK:
-		what = (char *)"UPDATE_BLOCK";
-		break;
-	case READ_LONG:
-		what = (char *)"READ_LONG";
-		break;
-	case WRITE_LONG:
-		what = (char *)"WRITE_LONG";
-		break;
-	case CHANGE_DEFINITION:
-		what = (char *)"CHANGE_DEFINITION";
-		break;
-	case WRITE_SAME:
-		what = (char *)"WRITE_SAME";
-		break;
-	case GPCMD_READ_SUBCHANNEL:
-		what = (char *)"READ SUBCHANNEL";
-		break;
-	case READ_TOC:
-		what = (char *)"READ_TOC";
-		break;
-	case GPCMD_READ_HEADER:
-		what = (char *)"READ HEADER";
-		break;
-	case GPCMD_PLAY_AUDIO_10:
-		what = (char *)"PLAY AUDIO (10)";
-		break;
-	case GPCMD_PLAY_AUDIO_MSF:
-		what = (char *)"PLAY AUDIO MSF";
-		break;
-	case GPCMD_GET_EVENT_STATUS_NOTIFICATION:
-		what = (char *)"GET EVENT/STATUS NOTIFICATION";
-		break;
-	case GPCMD_PAUSE_RESUME:
-		what = (char *)"PAUSE/RESUME";
-		break;
-	case LOG_SELECT:
-		what = (char *)"LOG_SELECT";
-		break;
-	case LOG_SENSE:
-		what = (char *)"LOG_SENSE";
-		break;
-	case GPCMD_STOP_PLAY_SCAN:
-		what = (char *)"STOP PLAY/SCAN";
-		break;
-	case GPCMD_READ_DISC_INFO:
-		what = (char *)"READ DISC INFORMATION";
-		break;
-	case GPCMD_READ_TRACK_RZONE_INFO:
-		what = (char *)"READ TRACK INFORMATION";
-		break;
-	case GPCMD_RESERVE_RZONE_TRACK:
-		what = (char *)"RESERVE TRACK";
-		break;
-	case GPCMD_SEND_OPC:
-		what = (char *)"SEND OPC";
-		break;
-	case MODE_SELECT_10:
-		what = (char *)"MODE_SELECT_10";
-		break;
-	case GPCMD_REPAIR_RZONE_TRACK:
-		what = (char *)"REPAIR TRACK";
-		break;
-	case 0x59:
-		what = (char *)"READ MASTER CUE";
-		break;
-	case MODE_SENSE_10:
-		what = (char *)"MODE_SENSE_10";
-		break;
-	case GPCMD_CLOSE_TRACK:
-		what = (char *)"CLOSE TRACK/SESSION";
-		break;
-	case 0x5C:
-		what = (char *)"READ BUFFER CAPACITY";
-		break;
-	case 0x5D:
-		what = (char *)"SEND CUE SHEET";
-		break;
-	case GPCMD_BLANK:
-		what = (char *)"BLANK";
-		break;
-	case REPORT_LUNS:
-		what = (char *)"REPORT LUNS";
-		break;
-	case MOVE_MEDIUM:
-		what = (char *)"MOVE_MEDIUM or PLAY AUDIO (12)";
-		break;
-	case READ_12:
-		what = (char *)"READ_12";
-		break;
-	case WRITE_12:
-		what = (char *)"WRITE_12";
-		break;
-	case WRITE_VERIFY_12:
-		what = (char *)"WRITE_VERIFY_12";
-		break;
-	case SEARCH_HIGH_12:
-		what = (char *)"SEARCH_HIGH_12";
-		break;
-	case SEARCH_EQUAL_12:
-		what = (char *)"SEARCH_EQUAL_12";
-		break;
-	case SEARCH_LOW_12:
-		what = (char *)"SEARCH_LOW_12";
-		break;
-	case SEND_VOLUME_TAG:
-		what = (char *)"SEND_VOLUME_TAG";
-		break;
-	case READ_ELEMENT_STATUS:
-		what = (char *)"READ_ELEMENT_STATUS";
-		break;
-	case GPCMD_READ_CD_MSF:
-		what = (char *)"READ CD MSF";
-		break;
-	case GPCMD_SCAN:
-		what = (char *)"SCAN";
-		break;
-	case GPCMD_SET_SPEED:
-		what = (char *)"SET CD SPEED";
-		break;
-	case GPCMD_MECHANISM_STATUS:
-		what = (char *)"MECHANISM STATUS";
-		break;
-	case GPCMD_READ_CD:
-		what = (char *)"READ CD";
-		break;
-	case 0xE1:
-		what = (char *)"WRITE CONTINUE";
-		break;
-	case WRITE_LONG_2:
-		what = (char *)"WRITE_LONG_2";
-		break;
-	case VENDOR_CMND:
-		what = (char *)"Realtek's vendor command";
-		break;
-	default:
-		what = (char *)"(unknown command)";
-		unknown_cmd = 1;
-		break;
-	}
-
-	if (srb->cmnd[0] != TEST_UNIT_READY)
-		RTS51X_DEBUGP("Command %s (%d bytes)\n", what, srb->cmd_len);
-	if (unknown_cmd) {
-		RTS51X_DEBUGP("");
-		for (i = 0; i < srb->cmd_len && i < 16; i++)
-			RTS51X_DEBUGPN(" %02x", srb->cmnd[i]);
-		RTS51X_DEBUGPN("\n");
-	}
-}
-
-void rts51x_set_sense_type(struct rts51x_chip *chip, unsigned int lun, int sense_type)
-{
-	switch (sense_type) {
-	case SENSE_TYPE_MEDIA_CHANGE:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x06, 0, 0x28, 0, 0, 0);
-		break;
-
-	case SENSE_TYPE_MEDIA_NOT_PRESENT:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x02, 0, 0x3A, 0, 0, 0);
-		break;
-
-	case SENSE_TYPE_MEDIA_LBA_OVER_RANGE:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x05, 0, 0x21, 0, 0, 0);
-		break;
-
-	case SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x05, 0, 0x25, 0, 0, 0);
-		break;
-
-	case SENSE_TYPE_MEDIA_WRITE_PROTECT:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x07, 0, 0x27, 0, 0, 0);
-		break;
-
-	case SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x03, 0, 0x11, 0, 0, 0);
-		break;
-
-	case SENSE_TYPE_MEDIA_WRITE_ERR:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x03, 0, 0x0C, 0x02, 0, 0);
-		break;
-
-	case SENSE_TYPE_MEDIA_INVALID_CMD_FIELD:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, ILGAL_REQ, 0,
-			       ASC_INVLD_CDB, ASCQ_INVLD_CDB, CDB_ILLEGAL, 1);
-		break;
-
-	case SENSE_TYPE_FORMAT_CMD_FAILED:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x03, 0, 0x31, 0x01, 0, 0);
-		break;
-
-#ifdef SUPPORT_MAGIC_GATE
-	case SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x05, 0, 0x6F, 0x02, 0, 0);
-		break;
-
-	case SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x05, 0, 0x6F, 0x00, 0, 0);
-		break;
-
-	case SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x02, 0, 0x30, 0x00, 0, 0);
-		break;
-
-	case SENSE_TYPE_MG_WRITE_ERR:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x03, 0, 0x0C, 0x00, 0, 0);
-		break;
-#endif
-
-	case SENSE_TYPE_NO_SENSE:
-	default:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0, 0, 0, 0, 0, 0);
-		break;
-	}
-}
-
-void rts51x_set_sense_data(struct rts51x_chip *chip, unsigned int lun, u8 err_code,
-		    u8 sense_key, u32 info, u8 asc, u8 ascq, u8 sns_key_info0,
-		    u16 sns_key_info1)
-{
-	struct sense_data_t *sense = &(chip->sense_buffer[lun]);
-
-	sense->err_code = err_code;
-	sense->sense_key = sense_key;
-	sense->info[0] = (u8) (info >> 24);
-	sense->info[1] = (u8) (info >> 16);
-	sense->info[2] = (u8) (info >> 8);
-	sense->info[3] = (u8) info;
-
-	sense->ad_sense_len = sizeof(struct sense_data_t) - 8;
-	sense->asc = asc;
-	sense->ascq = ascq;
-	if (sns_key_info0 != 0) {
-		sense->sns_key_info[0] = SKSV | sns_key_info0;
-		sense->sns_key_info[1] = (sns_key_info1 & 0xf0) >> 8;
-		sense->sns_key_info[2] = sns_key_info1 & 0x0f;
-	}
-}
-
-static int test_unit_ready(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned int lun = SCSI_LUN(srb);
-
-	rts51x_init_cards(chip);
-
-	if (!check_card_ready(chip, lun)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		return TRANSPORT_FAILED;
-	}
-
-	if (!check_lun_mc(chip, lun)) {
-		set_lun_mc(chip, lun);
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		return TRANSPORT_FAILED;
-	}
-
-	return TRANSPORT_GOOD;
-}
-
-static unsigned char formatter_inquiry_str[20] = {
-	'M', 'E', 'M', 'O', 'R', 'Y', 'S', 'T', 'I', 'C', 'K',
-	'-', 'M', 'G',		/* Byte[47:49] */
-	0x0B,			/* Byte[50]: MG, MS, MSPro, MSXC */
-	0x00,			/* Byte[51]: Category Specific Commands */
-	0x00,			/* Byte[52]: Access Control and feature */
-	0x20, 0x20, 0x20,	/* Byte[53:55] */
-};
-
-static int inquiry(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned int lun = SCSI_LUN(srb);
-	char *inquiry_default = (char *)"Generic-xD/SD/M.S.      1.00 ";
-	char *inquiry_string;
-	unsigned char sendbytes;
-	unsigned char *buf;
-	u8 card = rts51x_get_lun_card(chip, lun);
-	int pro_formatter_flag = 0;
-	unsigned char inquiry_buf[] = {
-		QULIFIRE | DRCT_ACCESS_DEV,
-		RMB_DISC | 0x0D,
-		0x00,
-		0x01,
-		0x1f,
-		0x02,
-		0,
-		REL_ADR | WBUS_32 | WBUS_16 | SYNC | LINKED | CMD_QUE | SFT_RE,
-	};
-
-	inquiry_string = inquiry_default;
-
-	buf = vmalloc(scsi_bufflen(srb));
-	if (buf == NULL)
-		TRACE_RET(chip, TRANSPORT_ERROR);
-
-	if (MS_FORMATTER_ENABLED(chip) && (get_lun2card(chip, lun) & MS_CARD)) {
-		if (!card || (card == MS_CARD))
-			pro_formatter_flag = 1;
-	}
-
-	if (pro_formatter_flag) {
-		if (scsi_bufflen(srb) < 56)
-			sendbytes = (unsigned char)(scsi_bufflen(srb));
-		else
-			sendbytes = 56;
-	} else {
-		if (scsi_bufflen(srb) < 36)
-			sendbytes = (unsigned char)(scsi_bufflen(srb));
-		else
-			sendbytes = 36;
-	}
-
-	if (sendbytes > 8) {
-		memcpy(buf, inquiry_buf, 8);
-		memcpy(buf + 8, inquiry_string, sendbytes - 8);
-		if (pro_formatter_flag)
-			buf[4] = 0x33;	/* Additional Length */
-	} else {
-		memcpy(buf, inquiry_buf, sendbytes);
-	}
-
-	if (pro_formatter_flag) {
-		if (sendbytes > 36)
-			memcpy(buf + 36, formatter_inquiry_str, sendbytes - 36);
-	}
-
-	scsi_set_resid(srb, 0);
-
-	rts51x_set_xfer_buf(buf, scsi_bufflen(srb), srb);
-	vfree(buf);
-
-	return TRANSPORT_GOOD;
-}
-
-static int start_stop_unit(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned int lun = SCSI_LUN(srb);
-
-	scsi_set_resid(srb, scsi_bufflen(srb));
-
-	if (srb->cmnd[1] == 1)
-		return TRANSPORT_GOOD;
-
-	switch (srb->cmnd[0x4]) {
-	case STOP_MEDIUM:
-		/* Media disabled */
-		return TRANSPORT_GOOD;
-
-	case UNLOAD_MEDIUM:
-		/* Media shall be unload */
-		if (check_card_ready(chip, lun))
-			rts51x_eject_card(chip, lun);
-		return TRANSPORT_GOOD;
-
-	case MAKE_MEDIUM_READY:
-	case LOAD_MEDIUM:
-		if (check_card_ready(chip, lun)) {
-			return TRANSPORT_GOOD;
-		} else {
-			rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-
-		break;
-	}
-
-	TRACE_RET(chip, TRANSPORT_ERROR);
-}
-
-static int allow_medium_removal(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	int prevent;
-
-	prevent = srb->cmnd[4] & 0x1;
-
-	scsi_set_resid(srb, 0);
-
-	if (prevent) {
-		rts51x_set_sense_type(chip, SCSI_LUN(srb),
-			       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	return TRANSPORT_GOOD;
-}
-
-static void ms_mode_sense(struct rts51x_chip *chip, u8 cmd,
-			  int lun, u8 *buf, int buf_len)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int sys_info_offset;
-	int data_size = buf_len;
-	int support_format = 0;
-	int i = 0;
-
-	if (cmd == MODE_SENSE) {
-		sys_info_offset = 8;
-		if (data_size > 0x68)
-			data_size = 0x68;
-		buf[i++] = 0x67;	/* Mode Data Length */
-	} else {
-		sys_info_offset = 12;
-		if (data_size > 0x6C)
-			data_size = 0x6C;
-		buf[i++] = 0x00;	/* Mode Data Length (MSB) */
-		buf[i++] = 0x6A;	/* Mode Data Length (LSB) */
-	}
-
-	/* Medium Type Code */
-	if (check_card_ready(chip, lun)) {
-		if (CHK_MSXC(ms_card)) {
-			support_format = 1;
-			buf[i++] = 0x40;
-		} else if (CHK_MSPRO(ms_card)) {
-			support_format = 1;
-			buf[i++] = 0x20;
-		} else {
-			buf[i++] = 0x10;
-		}
-
-		/* WP */
-		if (check_card_wp(chip, lun))
-			buf[i++] = 0x80;
-		else
-			buf[i++] = 0x00;
-	} else {
-		buf[i++] = 0x00;	/* MediaType */
-		buf[i++] = 0x00;	/* WP */
-	}
-
-	buf[i++] = 0x00;	/* Reserved */
-
-	if (cmd == MODE_SENSE_10) {
-		buf[i++] = 0x00;	/* Reserved */
-		buf[i++] = 0x00;	/* Block descriptor length(MSB) */
-		buf[i++] = 0x00;	/* Block descriptor length(LSB) */
-
-		/* The Following Data is the content of "Page 0x20" */
-		if (data_size >= 9)
-			buf[i++] = 0x20;	/* Page Code */
-		if (data_size >= 10)
-			buf[i++] = 0x62;	/* Page Length */
-		if (data_size >= 11)
-			buf[i++] = 0x00;	/* No Access Control */
-		if (data_size >= 12) {
-			if (support_format)
-				buf[i++] = 0xC0;	/* SF, SGM */
-			else
-				buf[i++] = 0x00;
-		}
-	} else {
-		/* The Following Data is the content of "Page 0x20" */
-		if (data_size >= 5)
-			buf[i++] = 0x20;	/* Page Code */
-		if (data_size >= 6)
-			buf[i++] = 0x62;	/* Page Length */
-		if (data_size >= 7)
-			buf[i++] = 0x00;	/* No Access Control */
-		if (data_size >= 8) {
-			if (support_format)
-				buf[i++] = 0xC0;	/* SF, SGM */
-			else
-				buf[i++] = 0x00;
-		}
-	}
-
-	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);
-	}
-}
-
-static int mode_sense(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned int lun = SCSI_LUN(srb);
-	unsigned int dataSize;
-	int status;
-	int pro_formatter_flag;
-	unsigned char pageCode, *buf;
-	u8 card = rts51x_get_lun_card(chip, lun);
-
-	if (!check_card_ready(chip, lun)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		scsi_set_resid(srb, scsi_bufflen(srb));
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	pro_formatter_flag = 0;
-	dataSize = 8;
-	/* In Combo mode, device responses ModeSense command as a MS LUN
-	 * when no card is inserted */
-	if ((get_lun2card(chip, lun) & MS_CARD)) {
-		if (!card || (card == MS_CARD)) {
-			dataSize = 108;
-			if (chip->option.rts51x_mspro_formatter_enable)
-				pro_formatter_flag = 1;
-		}
-	}
-
-	buf = kmalloc(dataSize, GFP_KERNEL);
-	if (buf == NULL)
-		TRACE_RET(chip, TRANSPORT_ERROR);
-
-	pageCode = srb->cmnd[2] & 0x3f;
-
-	if ((pageCode == 0x3F) || (pageCode == 0x1C) ||
-	    (pageCode == 0x00) || (pro_formatter_flag && (pageCode == 0x20))) {
-		if (srb->cmnd[0] == MODE_SENSE) {
-			if ((pageCode == 0x3F) || (pageCode == 0x20)) {
-				ms_mode_sense(chip, srb->cmnd[0], lun, buf,
-					      dataSize);
-			} else {
-				dataSize = 4;
-				buf[0] = 0x03;
-				buf[1] = 0x00;
-				if (check_card_wp(chip, lun))
-					buf[2] = 0x80;
-				else
-				buf[3] = 0x00;
-			}
-		} else {
-			if ((pageCode == 0x3F) || (pageCode == 0x20)) {
-				ms_mode_sense(chip, srb->cmnd[0], lun, buf,
-					      dataSize);
-			} else {
-				dataSize = 8;
-				buf[0] = 0x00;
-				buf[1] = 0x06;
-				buf[2] = 0x00;
-				if (check_card_wp(chip, lun))
-					buf[3] = 0x80;
-				else
-					buf[3] = 0x00;
-				buf[4] = 0x00;
-				buf[5] = 0x00;
-				buf[6] = 0x00;
-				buf[7] = 0x00;
-			}
-		}
-		status = TRANSPORT_GOOD;
-	} else {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		scsi_set_resid(srb, scsi_bufflen(srb));
-		status = TRANSPORT_FAILED;
-	}
-
-	if (status == TRANSPORT_GOOD) {
-		unsigned int len = min(scsi_bufflen(srb), dataSize);
-		rts51x_set_xfer_buf(buf, len, srb);
-		scsi_set_resid(srb, scsi_bufflen(srb) - len);
-	}
-	kfree(buf);
-
-	return status;
-}
-
-static int request_sense(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct sense_data_t *sense;
-	unsigned int lun = SCSI_LUN(srb);
-	struct ms_info *ms_card = &(chip->ms_card);
-	unsigned char *tmp, *buf;
-
-	sense = &(chip->sense_buffer[lun]);
-
-	if ((rts51x_get_lun_card(chip, lun) == MS_CARD)
-	    && PRO_UNDER_FORMATTING(ms_card)) {
-		rts51x_mspro_format_sense(chip, lun);
-	}
-
-	buf = vmalloc(scsi_bufflen(srb));
-	if (buf == NULL)
-		TRACE_RET(chip, TRANSPORT_ERROR);
-
-	tmp = (unsigned char *)sense;
-	memcpy(buf, tmp, scsi_bufflen(srb));
-
-	rts51x_set_xfer_buf(buf, scsi_bufflen(srb), srb);
-	vfree(buf);
-
-	scsi_set_resid(srb, 0);
-	/* Reset Sense Data */
-	rts51x_set_sense_type(chip, lun, SENSE_TYPE_NO_SENSE);
-	return TRANSPORT_GOOD;
-}
-
-static int read_write(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned int lun = SCSI_LUN(srb);
-	int retval;
-	u32 start_sec;
-	u16 sec_cnt;
-
-	if (!check_card_ready(chip, lun) || (chip->capacity[lun] == 0)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (!check_lun_mc(chip, lun)) {
-		set_lun_mc(chip, lun);
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		return TRANSPORT_FAILED;
-	}
-
-	rts51x_prepare_run(chip);
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	if ((srb->cmnd[0] == READ_10) || (srb->cmnd[0] == WRITE_10)) {
-		start_sec =
-		    ((u32) srb->cmnd[2] << 24) |
-		    ((u32) srb->cmnd[3] << 16) |
-		    ((u32) srb->cmnd[4] << 8) |
-		    ((u32) srb->cmnd[5]);
-		sec_cnt = ((u16) (srb->cmnd[7]) << 8) | srb->cmnd[8];
-	} else if ((srb->cmnd[0] == READ_6) || (srb->cmnd[0] == WRITE_6)) {
-		start_sec = ((u32) (srb->cmnd[1] & 0x1F) << 16) |
-		    ((u32) srb->cmnd[2] << 8) | ((u32) srb->cmnd[3]);
-		sec_cnt = srb->cmnd[4];
-	} else if ((srb->cmnd[0] == VENDOR_CMND) &&
-			(srb->cmnd[1] == SCSI_APP_CMD) &&
-			((srb->cmnd[2] == PP_READ10) ||
-			 (srb->cmnd[2] == PP_WRITE10))) {
-		start_sec = ((u32) srb->cmnd[4] << 24) |
-			((u32) srb->cmnd[5] << 16) |
-			((u32) srb->cmnd[6] << 8) |
-			((u32) srb->cmnd[7]);
-		sec_cnt = ((u16) (srb->cmnd[9]) << 8) | srb->cmnd[10];
-	} else {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if ((start_sec > chip->capacity[lun]) ||
-	    ((start_sec + sec_cnt) > chip->capacity[lun])) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LBA_OVER_RANGE);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (sec_cnt == 0) {
-		scsi_set_resid(srb, 0);
-		return TRANSPORT_GOOD;
-	}
-
-	if ((srb->sc_data_direction == DMA_TO_DEVICE)
-	    && check_card_wp(chip, lun)) {
-		RTS51X_DEBUGP("Write protected card!\n");
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_PROTECT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	retval = rts51x_card_rw(srb, chip, start_sec, sec_cnt);
-	if (retval != STATUS_SUCCESS) {
-		if (srb->sc_data_direction == DMA_FROM_DEVICE) {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-		} else {
-			rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
-		}
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	scsi_set_resid(srb, 0);
-
-	return TRANSPORT_GOOD;
-}
-
-static int read_format_capacity(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned char *buf;
-	unsigned int lun = SCSI_LUN(srb);
-	unsigned int buf_len;
-	u8 card = rts51x_get_lun_card(chip, lun);
-	int desc_cnt;
-	int i = 0;
-
-	if (!check_card_ready(chip, lun)) {
-		if (!chip->option.rts51x_mspro_formatter_enable) {
-			rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-	}
-
-	buf_len = (scsi_bufflen(srb) > 12) ? 0x14 : 12;
-
-	buf = kmalloc(buf_len, GFP_KERNEL);
-	if (buf == NULL)
-		TRACE_RET(chip, TRANSPORT_ERROR);
-
-	buf[i++] = 0;
-	buf[i++] = 0;
-	buf[i++] = 0;
-
-	/* Capacity List Length */
-	if ((buf_len > 12) && chip->option.rts51x_mspro_formatter_enable &&
-	    (chip->lun2card[lun] & MS_CARD) && (!card || (card == MS_CARD))) {
-		buf[i++] = 0x10;
-		desc_cnt = 2;
-	} else {
-		buf[i++] = 0x08;
-		desc_cnt = 1;
-	}
-
-	while (desc_cnt) {
-		if (check_card_ready(chip, lun)) {
-			buf[i++] = (unsigned char)((chip->capacity[lun]) >> 24);
-			buf[i++] = (unsigned char)((chip->capacity[lun]) >> 16);
-			buf[i++] = (unsigned char)((chip->capacity[lun]) >> 8);
-			buf[i++] = (unsigned char)(chip->capacity[lun]);
-
-			if (desc_cnt == 2)
-				/* Byte[8]: Descriptor Type: Formatted medium */
-				buf[i++] = 2;
-			else
-				buf[i++] = 0;	/* Byte[16] */
-		} else {
-			buf[i++] = 0xFF;
-			buf[i++] = 0xFF;
-			buf[i++] = 0xFF;
-			buf[i++] = 0xFF;
-
-			if (desc_cnt == 2)
-				/* Byte[8]: Descriptor Type: No medium */
-				buf[i++] = 3;
-			else
-				buf[i++] = 0;	/*Byte[16] */
-		}
-
-		buf[i++] = 0x00;
-		buf[i++] = 0x02;
-		buf[i++] = 0x00;
-
-		desc_cnt--;
-	}
-
-	buf_len = min(scsi_bufflen(srb), buf_len);
-	rts51x_set_xfer_buf(buf, buf_len, srb);
-	kfree(buf);
-
-	scsi_set_resid(srb, scsi_bufflen(srb) - buf_len);
-
-	return TRANSPORT_GOOD;
-}
-
-static int read_capacity(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned char *buf;
-	unsigned int lun = SCSI_LUN(srb);
-
-	if (!check_card_ready(chip, lun)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (!check_lun_mc(chip, lun)) {
-		set_lun_mc(chip, lun);
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		return TRANSPORT_FAILED;
-	}
-
-	buf = kmalloc(8, GFP_KERNEL);
-	if (buf == NULL)
-		TRACE_RET(chip, TRANSPORT_ERROR);
-
-	buf[0] = (unsigned char)((chip->capacity[lun] - 1) >> 24);
-	buf[1] = (unsigned char)((chip->capacity[lun] - 1) >> 16);
-	buf[2] = (unsigned char)((chip->capacity[lun] - 1) >> 8);
-	buf[3] = (unsigned char)(chip->capacity[lun] - 1);
-
-	buf[4] = 0x00;
-	buf[5] = 0x00;
-	buf[6] = 0x02;
-	buf[7] = 0x00;
-
-	rts51x_set_xfer_buf(buf, scsi_bufflen(srb), srb);
-	kfree(buf);
-
-	scsi_set_resid(srb, 0);
-
-	return TRANSPORT_GOOD;
-}
-
-static int get_dev_status(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned int lun = SCSI_LUN(srb);
-	unsigned int buf_len;
-	u8 status[32] = { 0 };
-
-	rts51x_pp_status(chip, lun, status, 32);
-
-	buf_len = min_t(unsigned int, scsi_bufflen(srb), sizeof(status));
-	rts51x_set_xfer_buf(status, buf_len, srb);
-	scsi_set_resid(srb, scsi_bufflen(srb) - buf_len);
-
-	return TRANSPORT_GOOD;
-}
-
-static int read_status(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	u8 rts51x_status[16];
-	unsigned int buf_len;
-	unsigned int lun = SCSI_LUN(srb);
-
-	rts51x_read_status(chip, lun, rts51x_status, 16);
-
-	buf_len = min_t(unsigned int, scsi_bufflen(srb), sizeof(rts51x_status));
-	rts51x_set_xfer_buf(rts51x_status, buf_len, srb);
-	scsi_set_resid(srb, scsi_bufflen(srb) - buf_len);
-
-	return TRANSPORT_GOOD;
-}
-
-static int read_mem(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned int lun = SCSI_LUN(srb);
-	unsigned short addr, len, i;
-	int retval;
-	u8 *buf;
-
-	rts51x_prepare_run(chip);
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	addr = ((u16) srb->cmnd[2] << 8) | srb->cmnd[3];
-	len = ((u16) srb->cmnd[4] << 8) | srb->cmnd[5];
-
-	if (addr < 0xe000) {
-		RTS51X_DEBUGP("filter!addr=0x%x\n", addr);
-		return TRANSPORT_GOOD;
-	}
-
-	buf = vmalloc(len);
-	if (!buf)
-		TRACE_RET(chip, TRANSPORT_ERROR);
-
-	for (i = 0; i < len; i++) {
-		retval = rts51x_ep0_read_register(chip, addr + i, buf + i);
-		if (retval != STATUS_SUCCESS) {
-			vfree(buf);
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-	}
-
-	len = (unsigned short)min(scsi_bufflen(srb), (unsigned int)len);
-	rts51x_set_xfer_buf(buf, len, srb);
-	scsi_set_resid(srb, scsi_bufflen(srb) - len);
-
-	vfree(buf);
-
-	return TRANSPORT_GOOD;
-}
-
-static int write_mem(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned int lun = SCSI_LUN(srb);
-	unsigned short addr, len, i;
-	int retval;
-	u8 *buf;
-
-	rts51x_prepare_run(chip);
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	addr = ((u16) srb->cmnd[2] << 8) | srb->cmnd[3];
-	len = ((u16) srb->cmnd[4] << 8) | srb->cmnd[5];
-
-	if (addr < 0xe000) {
-		RTS51X_DEBUGP("filter!addr=0x%x\n", addr);
-		return TRANSPORT_GOOD;
-	}
-
-	len = (unsigned short)min(scsi_bufflen(srb), (unsigned int)len);
-	buf = vmalloc(len);
-	if (!buf)
-		TRACE_RET(chip, TRANSPORT_ERROR);
-
-	rts51x_get_xfer_buf(buf, len, srb);
-
-	for (i = 0; i < len; i++) {
-		retval =
-		    rts51x_ep0_write_register(chip, addr + i, 0xFF, buf[i]);
-		if (retval != STATUS_SUCCESS) {
-			vfree(buf);
-			rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-	}
-
-	vfree(buf);
-	scsi_set_resid(srb, scsi_bufflen(srb) - len);
-
-	return TRANSPORT_GOOD;
-}
-
-static int get_sd_csd(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	unsigned int lun = SCSI_LUN(srb);
-
-	if (!check_card_ready(chip, lun)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (rts51x_get_lun_card(chip, lun) != SD_CARD) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	scsi_set_resid(srb, 0);
-	rts51x_set_xfer_buf(sd_card->raw_csd, scsi_bufflen(srb), srb);
-
-	return TRANSPORT_GOOD;
-}
-
-static int read_phy_register(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	int retval;
-	u8 addr, len, i;
-	u8 *buf;
-
-	rts51x_prepare_run(chip);
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	addr = srb->cmnd[5];
-	len = srb->cmnd[7];
-
-	if (len) {
-		buf = vmalloc(len);
-		if (!buf)
-			TRACE_RET(chip, TRANSPORT_ERROR);
-
-		for (i = 0; i < len; i++) {
-			retval =
-			    rts51x_read_phy_register(chip, addr + i, buf + i);
-			if (retval != STATUS_SUCCESS) {
-				vfree(buf);
-				rts51x_set_sense_type(chip, SCSI_LUN(srb),
-					SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-				TRACE_RET(chip, TRANSPORT_FAILED);
-			}
-		}
-
-		len = min(scsi_bufflen(srb), (unsigned int)len);
-		rts51x_set_xfer_buf(buf, len, srb);
-		scsi_set_resid(srb, scsi_bufflen(srb) - len);
-
-		vfree(buf);
-	}
-
-	return TRANSPORT_GOOD;
-}
-
-static int write_phy_register(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	int retval;
-	u8 addr, len, i;
-	u8 *buf;
-
-	rts51x_prepare_run(chip);
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	addr = srb->cmnd[5];
-	len = srb->cmnd[7];
-
-	if (len) {
-		len = min(scsi_bufflen(srb), (unsigned int)len);
-
-		buf = vmalloc(len);
-		if (buf == NULL)
-			TRACE_RET(chip, TRANSPORT_ERROR);
-
-		rts51x_get_xfer_buf(buf, len, srb);
-		scsi_set_resid(srb, scsi_bufflen(srb) - len);
-
-		for (i = 0; i < len; i++) {
-			retval =
-			    rts51x_write_phy_register(chip, addr + i, buf[i]);
-			if (retval != STATUS_SUCCESS) {
-				vfree(buf);
-				rts51x_set_sense_type(chip, SCSI_LUN(srb),
-					       SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, TRANSPORT_FAILED);
-			}
-		}
-
-		vfree(buf);
-	}
-
-	return TRANSPORT_GOOD;
-}
-
-static int get_card_bus_width(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned int lun = SCSI_LUN(srb);
-	u8 card, bus_width;
-
-	if (!check_card_ready(chip, lun)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	card = rts51x_get_lun_card(chip, lun);
-	if ((card == SD_CARD) || (card == MS_CARD)) {
-		bus_width = chip->card_bus_width[lun];
-	} else {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	scsi_set_resid(srb, 0);
-	rts51x_set_xfer_buf(&bus_width, scsi_bufflen(srb), srb);
-
-	return TRANSPORT_GOOD;
-}
-
-#ifdef _MSG_TRACE
-static int trace_msg_cmd(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned char *buf = NULL;
-	u8 clear;
-	unsigned int buf_len;
-
-	buf_len =
-	    4 +
-	    ((2 + MSG_FUNC_LEN + MSG_FILE_LEN + TIME_VAL_LEN) * TRACE_ITEM_CNT);
-
-	if ((scsi_bufflen(srb) < buf_len) || (scsi_sglist(srb) == NULL)) {
-		rts51x_set_sense_type(chip, SCSI_LUN(srb),
-			       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	clear = srb->cmnd[2];
-
-	buf = vmalloc(scsi_bufflen(srb));
-	if (buf == NULL)
-		TRACE_RET(chip, TRANSPORT_ERROR);
-
-	rts51x_trace_msg(chip, buf, clear);
-
-	rts51x_set_xfer_buf(buf, scsi_bufflen(srb), srb);
-	vfree(buf);
-
-	scsi_set_resid(srb, 0);
-	return TRANSPORT_GOOD;
-}
-#endif
-
-static int rw_mem_cmd_buf(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	int retval = STATUS_SUCCESS;
-	unsigned int lun = SCSI_LUN(srb);
-	u8 cmd_type, mask, value, idx, mode, len;
-	u16 addr;
-	u32 timeout;
-
-	rts51x_prepare_run(chip);
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	switch (srb->cmnd[3]) {
-	case INIT_BATCHCMD:
-		rts51x_init_cmd(chip);
-		break;
-
-	case ADD_BATCHCMD:
-		cmd_type = srb->cmnd[4];
-		if (cmd_type > 2) {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		addr = (srb->cmnd[5] << 8) | srb->cmnd[6];
-		mask = srb->cmnd[7];
-		value = srb->cmnd[8];
-		rts51x_add_cmd(chip, cmd_type, addr, mask, value);
-		break;
-
-	case SEND_BATCHCMD:
-		mode = srb->cmnd[4];
-		len = srb->cmnd[5];
-		timeout =
-		    ((u32) srb->cmnd[6] << 24) | ((u32) srb->
-						  cmnd[7] << 16) | ((u32) srb->
-								    cmnd[8] <<
-								    8) | ((u32)
-									  srb->
-									  cmnd
-									  [9]);
-		retval = rts51x_send_cmd(chip, mode, 1000);
-		if (retval != STATUS_SUCCESS) {
-			rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		if (mode & STAGE_R) {
-			retval = rts51x_get_rsp(chip, len, timeout);
-			if (retval != STATUS_SUCCESS) {
-				rts51x_set_sense_type(chip, lun,
-					SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-				TRACE_RET(chip, TRANSPORT_FAILED);
-			}
-		}
-		break;
-
-	case GET_BATCHRSP:
-		idx = srb->cmnd[4];
-		value = chip->rsp_buf[idx];
-		if (scsi_bufflen(srb) < 1) {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		rts51x_set_xfer_buf(&value, 1, srb);
-		scsi_set_resid(srb, 0);
-		break;
-
-	default:
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	return TRANSPORT_GOOD;
-}
-
-static int suit_cmd(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	int result;
-
-	switch (srb->cmnd[3]) {
-	case INIT_BATCHCMD:
-	case ADD_BATCHCMD:
-	case SEND_BATCHCMD:
-	case GET_BATCHRSP:
-		result = rw_mem_cmd_buf(srb, chip);
-		break;
-	default:
-		result = TRANSPORT_ERROR;
-	}
-
-	return result;
-}
-
-static int app_cmd(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	int result;
-
-	switch (srb->cmnd[2]) {
-	case PP_READ10:
-	case PP_WRITE10:
-		result = read_write(srb, chip);
-		break;
-
-	case SUIT_CMD:
-		result = suit_cmd(srb, chip);
-		break;
-
-	case READ_PHY:
-		result = read_phy_register(srb, chip);
-		break;
-
-	case WRITE_PHY:
-		result = write_phy_register(srb, chip);
-		break;
-
-	case GET_DEV_STATUS:
-		result = get_dev_status(srb, chip);
-		break;
-
-	default:
-		rts51x_set_sense_type(chip, SCSI_LUN(srb),
-			       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	return result;
-}
-
-static int vendor_cmnd(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	int result = TRANSPORT_GOOD;
-
-	switch (srb->cmnd[1]) {
-	case READ_STATUS:
-		result = read_status(srb, chip);
-		break;
-
-	case READ_MEM:
-		result = read_mem(srb, chip);
-		break;
-
-	case WRITE_MEM:
-		result = write_mem(srb, chip);
-		break;
-
-	case GET_BUS_WIDTH:
-		result = get_card_bus_width(srb, chip);
-		break;
-
-	case GET_SD_CSD:
-		result = get_sd_csd(srb, chip);
-		break;
-
-#ifdef _MSG_TRACE
-	case TRACE_MSG:
-		result = trace_msg_cmd(srb, chip);
-		break;
-#endif
-
-	case SCSI_APP_CMD:
-		result = app_cmd(srb, chip);
-		break;
-
-	default:
-		rts51x_set_sense_type(chip, SCSI_LUN(srb),
-			       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	return result;
-}
-
-static int ms_format_cmnd(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	unsigned int lun = SCSI_LUN(srb);
-	int retval, quick_format;
-
-	if (rts51x_get_lun_card(chip, lun) != MS_CARD) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if ((srb->cmnd[3] != 0x4D) || (srb->cmnd[4] != 0x47)
-	    || (srb->cmnd[5] != 0x66) || (srb->cmnd[6] != 0x6D)
-	    || (srb->cmnd[7] != 0x74)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (srb->cmnd[8] & 0x01)
-		quick_format = 0;
-	else
-		quick_format = 1;
-
-	if (!(chip->card_ready & MS_CARD)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (chip->card_wp & MS_CARD) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_PROTECT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (!CHK_MSPRO(ms_card)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	rts51x_prepare_run(chip);
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	retval = rts51x_mspro_format(srb, chip, MS_SHORT_DATA_LEN, quick_format);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_FORMAT_CMD_FAILED);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	scsi_set_resid(srb, 0);
-	return TRANSPORT_GOOD;
-}
-
-#ifdef SUPPORT_PCGL_1P18
-static int get_ms_information(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	unsigned int lun = SCSI_LUN(srb);
-	u8 dev_info_id, data_len;
-	u8 *buf;
-	unsigned int buf_len;
-	int i;
-
-	if (!check_card_ready(chip, lun)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-	if ((rts51x_get_lun_card(chip, lun) != MS_CARD)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if ((srb->cmnd[2] != 0xB0) || (srb->cmnd[4] != 0x4D) ||
-	    (srb->cmnd[5] != 0x53) || (srb->cmnd[6] != 0x49) ||
-	    (srb->cmnd[7] != 0x44)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	dev_info_id = srb->cmnd[3];
-	if ((CHK_MSXC(ms_card) && (dev_info_id == 0x10)) ||
-	    (!CHK_MSXC(ms_card) && (dev_info_id == 0x13)) ||
-	    !CHK_MSPRO(ms_card)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (dev_info_id == 0x15)
-		buf_len = data_len = 0x3A;
-	else
-		buf_len = data_len = 0x6A;
-
-	buf = kmalloc(buf_len, GFP_KERNEL);
-	if (!buf)
-		TRACE_RET(chip, TRANSPORT_ERROR);
-
-	i = 0;
-	/* GET Memory Stick Media Information Response Header */
-	buf[i++] = 0x00;	/* Data length MSB */
-	buf[i++] = data_len;	/* Data length LSB */
-	/* Device Information Type Code */
-	if (CHK_MSXC(ms_card))
-		buf[i++] = 0x03;
-	else
-		buf[i++] = 0x02;
-	/* SGM bit */
-	buf[i++] = 0x01;
-	/* Reserved */
-	buf[i++] = 0x00;
-	buf[i++] = 0x00;
-	buf[i++] = 0x00;
-	/* Number of Device Information */
-	buf[i++] = 0x01;
-
-	/*  Device Information Body
-	 *  Device Information ID Number */
-	buf[i++] = dev_info_id;
-	/* Device Information Length */
-	if (dev_info_id == 0x15)
-		data_len = 0x31;
-	else
-		data_len = 0x61;
-	buf[i++] = 0x00;	/* Data length MSB */
-	buf[i++] = data_len;	/* Data length LSB */
-	/* Valid Bit */
-	buf[i++] = 0x80;
-	if ((dev_info_id == 0x10) || (dev_info_id == 0x13)) {
-		/* System Information */
-		memcpy(buf + i, ms_card->raw_sys_info, 96);
-	} else {
-		/* Model Name */
-		memcpy(buf + i, ms_card->raw_model_name, 48);
-	}
-
-	rts51x_set_xfer_buf(buf, buf_len, srb);
-
-	if (dev_info_id == 0x15)
-		scsi_set_resid(srb, scsi_bufflen(srb) - 0x3C);
-	else
-		scsi_set_resid(srb, scsi_bufflen(srb) - 0x6C);
-
-	kfree(buf);
-	return STATUS_SUCCESS;
-}
-#endif
-
-static int ms_sp_cmnd(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	int retval = TRANSPORT_ERROR;
-
-	if (srb->cmnd[2] == MS_FORMAT)
-		retval = ms_format_cmnd(srb, chip);
-#ifdef SUPPORT_PCGL_1P18
-	else if (srb->cmnd[2] == GET_MS_INFORMATION)
-		retval = get_ms_information(srb, chip);
-#endif
-
-	return retval;
-}
-
-#ifdef SUPPORT_CPRM
-static int sd_extention_cmnd(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned int lun = SCSI_LUN(srb);
-	int result;
-
-	rts51x_prepare_run(chip);
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	rts51x_sd_cleanup_work(chip);
-
-	if (!check_card_ready(chip, lun)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-	if ((rts51x_get_lun_card(chip, lun) != SD_CARD)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	switch (srb->cmnd[0]) {
-	case SD_PASS_THRU_MODE:
-		result = rts51x_sd_pass_thru_mode(srb, chip);
-		break;
-
-	case SD_EXECUTE_NO_DATA:
-		result = rts51x_sd_execute_no_data(srb, chip);
-		break;
-
-	case SD_EXECUTE_READ:
-		result = rts51x_sd_execute_read_data(srb, chip);
-		break;
-
-	case SD_EXECUTE_WRITE:
-		result = rts51x_sd_execute_write_data(srb, chip);
-		break;
-
-	case SD_GET_RSP:
-		result = rts51x_sd_get_cmd_rsp(srb, chip);
-		break;
-
-	case SD_HW_RST:
-		result = rts51x_sd_hw_rst(srb, chip);
-		break;
-
-	default:
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	return result;
-}
-#endif
-
-#ifdef SUPPORT_MAGIC_GATE
-static int mg_report_key(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	unsigned int lun = SCSI_LUN(srb);
-	int retval;
-	u8 key_format;
-
-	rts51x_prepare_run(chip);
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	rts51x_ms_cleanup_work(chip);
-
-	if (!check_card_ready(chip, lun)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-	if ((rts51x_get_lun_card(chip, lun) != MS_CARD)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (srb->cmnd[7] != KC_MG_R_PRO) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (!CHK_MSPRO(ms_card)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	key_format = srb->cmnd[10] & 0x3F;
-
-	switch (key_format) {
-	case KF_GET_LOC_EKB:
-		if ((scsi_bufflen(srb) == 0x41C) &&
-		    (srb->cmnd[8] == 0x04) && (srb->cmnd[9] == 0x1C)) {
-			retval = rts51x_mg_get_local_EKB(srb, chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
-		} else {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		break;
-
-	case KF_RSP_CHG:
-		if ((scsi_bufflen(srb) == 0x24) &&
-		    (srb->cmnd[8] == 0x00) && (srb->cmnd[9] == 0x24)) {
-			retval = rts51x_mg_get_rsp_chg(srb, chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
-		} else {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		break;
-
-	case KF_GET_ICV:
-		ms_card->mg_entry_num = srb->cmnd[5];
-		if ((scsi_bufflen(srb) == 0x404) &&
-		    (srb->cmnd[8] == 0x04) &&
-		    (srb->cmnd[9] == 0x04) &&
-		    (srb->cmnd[2] == 0x00) &&
-		    (srb->cmnd[3] == 0x00) &&
-		    (srb->cmnd[4] == 0x00) && (srb->cmnd[5] < 32)) {
-			retval = rts51x_mg_get_ICV(srb, chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
-		} else {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		break;
-
-	default:
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	scsi_set_resid(srb, 0);
-	return TRANSPORT_GOOD;
-}
-
-static int mg_send_key(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	unsigned int lun = SCSI_LUN(srb);
-	int retval;
-	u8 key_format;
-
-	rts51x_prepare_run(chip);
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	rts51x_ms_cleanup_work(chip);
-
-	if (!check_card_ready(chip, lun)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-	if (check_card_wp(chip, lun)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_PROTECT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-	if ((rts51x_get_lun_card(chip, lun) != MS_CARD)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (srb->cmnd[7] != KC_MG_R_PRO) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (!CHK_MSPRO(ms_card)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	key_format = srb->cmnd[10] & 0x3F;
-
-	switch (key_format) {
-	case KF_SET_LEAF_ID:
-		if ((scsi_bufflen(srb) == 0x0C) &&
-		    (srb->cmnd[8] == 0x00) && (srb->cmnd[9] == 0x0C)) {
-			retval = rts51x_mg_set_leaf_id(srb, chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
-		} else {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		break;
-
-	case KF_CHG_HOST:
-		if ((scsi_bufflen(srb) == 0x0C) &&
-		    (srb->cmnd[8] == 0x00) && (srb->cmnd[9] == 0x0C)) {
-			retval = rts51x_mg_chg(srb, chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
-		} else {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		break;
-
-	case KF_RSP_HOST:
-		if ((scsi_bufflen(srb) == 0x0C) &&
-		    (srb->cmnd[8] == 0x00) && (srb->cmnd[9] == 0x0C)) {
-			retval = rts51x_mg_rsp(srb, chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
-		} else {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		break;
-
-	case KF_SET_ICV:
-		ms_card->mg_entry_num = srb->cmnd[5];
-		if ((scsi_bufflen(srb) == 0x404) &&
-		    (srb->cmnd[8] == 0x04) &&
-		    (srb->cmnd[9] == 0x04) &&
-		    (srb->cmnd[2] == 0x00) &&
-		    (srb->cmnd[3] == 0x00) &&
-		    (srb->cmnd[4] == 0x00) && (srb->cmnd[5] < 32)) {
-			retval = rts51x_mg_set_ICV(srb, chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
-		} else {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		break;
-
-	default:
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	scsi_set_resid(srb, 0);
-	return TRANSPORT_GOOD;
-}
-#endif
-
-int rts51x_scsi_handler(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	unsigned int lun = SCSI_LUN(srb);
-	int result = TRANSPORT_GOOD;
-
-	if ((rts51x_get_lun_card(chip, lun) == MS_CARD) &&
-	    (ms_card->format_status == FORMAT_IN_PROGRESS)) {
-		if ((srb->cmnd[0] != REQUEST_SENSE)
-		    && (srb->cmnd[0] != INQUIRY)) {
-			/* Logical Unit Not Ready Format in Progress */
-			rts51x_set_sense_data(chip, lun, CUR_ERR, 0x02, 0, 0x04, 0x04,
-				       0, (u16) (ms_card->progress));
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-	}
-
-	switch (srb->cmnd[0]) {
-	case READ_10:
-	case WRITE_10:
-	case READ_6:
-	case WRITE_6:
-		result = read_write(srb, chip);
-		break;
-
-	case TEST_UNIT_READY:
-		result = test_unit_ready(srb, chip);
-		break;
-
-	case INQUIRY:
-		result = inquiry(srb, chip);
-		break;
-
-	case READ_CAPACITY:
-		result = read_capacity(srb, chip);
-		break;
-
-	case START_STOP:
-		result = start_stop_unit(srb, chip);
-		break;
-
-	case ALLOW_MEDIUM_REMOVAL:
-		result = allow_medium_removal(srb, chip);
-		break;
-
-	case REQUEST_SENSE:
-		result = request_sense(srb, chip);
-		break;
-
-	case MODE_SENSE:
-	case MODE_SENSE_10:
-		result = mode_sense(srb, chip);
-		break;
-
-	case 0x23:
-		result = read_format_capacity(srb, chip);
-		break;
-
-	case VENDOR_CMND:
-		result = vendor_cmnd(srb, chip);
-		break;
-
-	case MS_SP_CMND:
-		result = ms_sp_cmnd(srb, chip);
-		break;
-
-#ifdef SUPPORT_CPRM
-	case SD_PASS_THRU_MODE:
-	case SD_EXECUTE_NO_DATA:
-	case SD_EXECUTE_READ:
-	case SD_EXECUTE_WRITE:
-	case SD_GET_RSP:
-	case SD_HW_RST:
-		result = sd_extention_cmnd(srb, chip);
-		break;
-#endif
-
-#ifdef SUPPORT_MAGIC_GATE
-	case CMD_MSPRO_MG_RKEY:
-		result = mg_report_key(srb, chip);
-		break;
-
-	case CMD_MSPRO_MG_SKEY:
-		result = mg_send_key(srb, chip);
-		break;
-#endif
-
-	case FORMAT_UNIT:
-	case MODE_SELECT:
-	case VERIFY:
-		result = TRANSPORT_GOOD;
-		break;
-
-	default:
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		result = TRANSPORT_FAILED;
-	}
-
-	return result;
-}
-
-/***********************************************************************
- * Host functions
- ***********************************************************************/
-
-int slave_alloc(struct scsi_device *sdev)
-{
-	/*
-	 * Set the INQUIRY transfer length to 36.  We don't use any of
-	 * the extra data and many devices choke if asked for more or
-	 * less than 36 bytes.
-	 */
-	sdev->inquiry_len = 36;
-	return 0;
-}
-
-int slave_configure(struct scsi_device *sdev)
-{
-	/* Scatter-gather buffers (all but the last) must have a length
-	 * divisible by the bulk maxpacket size.  Otherwise a data packet
-	 * would end up being short, causing a premature end to the data
-	 * transfer.  Since high-speed bulk pipes have a maxpacket size
-	 * of 512, we'll use that as the scsi device queue's DMA alignment
-	 * mask.  Guaranteeing proper alignment of the first buffer will
-	 * have the desired effect because, except at the beginning and
-	 * the end, scatter-gather buffers follow page boundaries. */
-	blk_queue_dma_alignment(sdev->request_queue, (512 - 1));
-
-	/* Set the SCSI level to at least 2.  We'll leave it at 3 if that's
-	 * what is originally reported.  We need this to avoid confusing
-	 * the SCSI layer with devices that report 0 or 1, but need 10-byte
-	 * commands (ala ATAPI devices behind certain bridges, or devices
-	 * which simply have broken INQUIRY data).
-	 *
-	 * NOTE: This means /dev/sg programs (ala cdrecord) will get the
-	 * actual information.  This seems to be the preference for
-	 * programs like that.
-	 *
-	 * NOTE: This also means that /proc/scsi/scsi and sysfs may report
-	 * the actual value or the modified one, depending on where the
-	 * data comes from.
-	 */
-	if (sdev->scsi_level < SCSI_2)
-		sdev->scsi_level = sdev->sdev_target->scsi_level = SCSI_2;
-
-	return 0;
-}
-
-/***********************************************************************
- * /proc/scsi/ functions
- ***********************************************************************/
-
-/* we use this macro to help us write into the buffer */
-#undef SPRINTF
-#define SPRINTF(args...) seq_printf(m, ##args)
-
-static int write_info(struct Scsi_Host *host, char *buffer, int length)
-{
-	/* if someone is sending us data, just throw it away */
-	return length;
-}
-
-static int show_info(struct seq_file *m, struct Scsi_Host *host)
-{
-	/* print the controller name */
-	SPRINTF("   Host scsi%d: %s\n", host->host_no, RTS51X_NAME);
-
-	/* print product, vendor, and driver version strings */
-	SPRINTF("       Vendor: Realtek Corp.\n");
-	SPRINTF("      Product: RTS51xx USB Card Reader\n");
-	SPRINTF("      Version: %s\n", DRIVER_VERSION);
-	return 0;
-}
-
-/* queue a command */
-/* This is always called with scsi_lock(host) held */
-static int queuecommand_lck(struct scsi_cmnd *srb, void (*done) (struct scsi_cmnd *))
-{
-	struct rts51x_chip *chip = host_to_rts51x(srb->device->host);
-
-	/* check for state-transition errors */
-	if (chip->srb != NULL) {
-		RTS51X_DEBUGP("Error in %s: chip->srb = %p\n",
-			       __func__, chip->srb);
-		return SCSI_MLQUEUE_HOST_BUSY;
-	}
-
-	/* fail the command if we are disconnecting */
-	if (test_bit(FLIDX_DISCONNECTING, &chip->usb->dflags)) {
-		RTS51X_DEBUGP("Fail command during disconnect\n");
-		srb->result = DID_NO_CONNECT << 16;
-		done(srb);
-		return 0;
-	}
-
-	/* enqueue the command and wake up the control thread */
-	srb->scsi_done = done;
-	chip->srb = srb;
-	complete(&chip->usb->cmnd_ready);
-
-	return 0;
-}
-
-DEF_SCSI_QCMD(queuecommand)
-/***********************************************************************
- * Error handling functions
- ***********************************************************************/
-/* Command timeout and abort */
-int command_abort(struct scsi_cmnd *srb)
-{
-	struct rts51x_chip *chip = host_to_rts51x(srb->device->host);
-
-	RTS51X_DEBUGP("%s called\n", __func__);
-
-	/* us->srb together with the TIMED_OUT, RESETTING, and ABORTING
-	 * bits are protected by the host lock. */
-	scsi_lock(rts51x_to_host(chip));
-
-	/* Is this command still active? */
-	if (chip->srb != srb) {
-		scsi_unlock(rts51x_to_host(chip));
-		RTS51X_DEBUGP("-- nothing to abort\n");
-		return FAILED;
-	}
-
-	/* Set the TIMED_OUT bit.  Also set the ABORTING bit, but only if
-	 * a device reset isn't already in progress (to avoid interfering
-	 * with the reset).  Note that we must retain the host lock while
-	 * calling usb_stor_stop_transport(); otherwise it might interfere
-	 * with an auto-reset that begins as soon as we release the lock. */
-	set_bit(FLIDX_TIMED_OUT, &chip->usb->dflags);
-	if (!test_bit(FLIDX_RESETTING, &chip->usb->dflags)) {
-		set_bit(FLIDX_ABORTING, &chip->usb->dflags);
-		/* rts51x_stop_transport(us); */
-	}
-	scsi_unlock(rts51x_to_host(chip));
-
-	/* Wait for the aborted command to finish */
-	wait_for_completion(&chip->usb->notify);
-	return SUCCESS;
-}
-
-/* This invokes the transport reset mechanism to reset the state of the
- * device */
-static int device_reset(struct scsi_cmnd *srb)
-{
-	int result = 0;
-
-	RTS51X_DEBUGP("%s called\n", __func__);
-
-	return result < 0 ? FAILED : SUCCESS;
-}
-
-/* Simulate a SCSI bus reset by resetting the device's USB port. */
-int bus_reset(struct scsi_cmnd *srb)
-{
-	int result = 0;
-
-	RTS51X_DEBUGP("%s called\n", __func__);
-
-	return result < 0 ? FAILED : SUCCESS;
-}
-
-static const char *rts5139_info(struct Scsi_Host *host)
-{
-	return "SCSI emulation for RTS5139 USB card reader";
-}
-
-struct scsi_host_template rts51x_host_template = {
-	/* basic userland interface stuff */
-	.name = RTS51X_NAME,
-	.proc_name = RTS51X_NAME,
-	.show_info = show_info,
-	.write_info = write_info,
-	.info = rts5139_info,
-
-	/* command interface -- queued only */
-	.queuecommand = queuecommand,
-
-	/* error and abort handlers */
-	.eh_abort_handler = command_abort,
-	.eh_device_reset_handler = device_reset,
-	.eh_bus_reset_handler = bus_reset,
-
-	/* queue commands only, only one command per LUN */
-	.can_queue = 1,
-	.cmd_per_lun = 1,
-
-	/* unknown initiator id */
-	.this_id = -1,
-
-	.slave_alloc = slave_alloc,
-	.slave_configure = slave_configure,
-
-	/* lots of sg segments can be handled */
-	.sg_tablesize = SG_ALL,
-
-	/* limit the total size of a transfer to 120 KB */
-	.max_sectors = 240,
-
-	/* merge commands... this seems to help performance, but
-	 * periodically someone should test to see which setting is more
-	 * optimal.
-	 */
-	.use_clustering = 1,
-
-	/* emulated HBA */
-	.emulated = 1,
-
-	/* we do our own delay after a device or bus reset */
-	.skip_settle_delay = 1,
-
-	/* sysfs device attributes */
-	/* .sdev_attrs = sysfs_device_attr_list, */
-
-	/* module management */
-	.module = THIS_MODULE
-};
-
diff --git a/drivers/staging/rts5139/rts51x_scsi.h b/drivers/staging/rts5139/rts51x_scsi.h
deleted file mode 100644
index 1a0d7056618..00000000000
--- a/drivers/staging/rts5139/rts51x_scsi.h
+++ /dev/null
@@ -1,154 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_SCSI_H
-#define __RTS51X_SCSI_H
-
-#include <linux/usb.h>
-#include <linux/usb_usual.h>
-#include <linux/blkdev.h>
-#include <linux/completion.h>
-#include <linux/mutex.h>
-#include <scsi/scsi_host.h>
-
-#include "rts51x_chip.h"
-
-#define MS_SP_CMND		0xFA
-#define MS_FORMAT		0xA0
-#define GET_MS_INFORMATION	0xB0
-
-#define VENDOR_CMND		0xF0
-
-#define READ_STATUS		0x09
-
-#define READ_MEM		0x0D
-#define WRITE_MEM		0x0E
-#define GET_BUS_WIDTH		0x13
-#define GET_SD_CSD		0x14
-#define TOGGLE_GPIO		0x15
-#define TRACE_MSG		0x18
-
-#define SCSI_APP_CMD		0x10
-
-#define PP_READ10		0x1A
-#define PP_WRITE10		0x0A
-#define READ_HOST_REG		0x1D
-#define WRITE_HOST_REG		0x0D
-#define SET_VAR			0x05
-#define GET_VAR			0x15
-#define DMA_READ		0x16
-#define DMA_WRITE		0x06
-#define GET_DEV_STATUS		0x10
-#define SET_CHIP_MODE		0x27
-#define SUIT_CMD		0xE0
-#define WRITE_PHY		0x07
-#define READ_PHY		0x17
-
-#define INIT_BATCHCMD		0x41
-#define ADD_BATCHCMD		0x42
-#define SEND_BATCHCMD		0x43
-#define GET_BATCHRSP		0x44
-
-#ifdef SUPPORT_CPRM
-/* SD Pass Through Command Extension */
-#define SD_PASS_THRU_MODE	0xD0
-#define SD_EXECUTE_NO_DATA	0xD1
-#define SD_EXECUTE_READ		0xD2
-#define SD_EXECUTE_WRITE	0xD3
-#define SD_GET_RSP		0xD4
-#define SD_HW_RST		0xD6
-#endif
-
-#ifdef SUPPORT_MAGIC_GATE
-#define CMD_MSPRO_MG_RKEY	0xA4	/* Report Key Command */
-#define CMD_MSPRO_MG_SKEY	0xA3	/* Send Key Command */
-
-/* CBWCB field: key class */
-#define KC_MG_R_PRO		0xBE	/* MG-R PRO */
-
-/* CBWCB field: key format */
-#define KF_SET_LEAF_ID		0x31	/* Set Leaf ID */
-#define KF_GET_LOC_EKB		0x32	/* Get Local EKB */
-#define KF_CHG_HOST		0x33	/* Challenge (host) */
-#define KF_RSP_CHG		0x34	/* Response and Challenge (device)  */
-#define KF_RSP_HOST		0x35	/* Response (host) */
-#define KF_GET_ICV		0x36	/* Get ICV */
-#define KF_SET_ICV		0x37	/* SSet ICV */
-#endif
-
-struct rts51x_chip;
-
-/*-----------------------------------
-    Start-Stop-Unit
------------------------------------*/
-#define STOP_MEDIUM			0x00	/* access disable */
-#define MAKE_MEDIUM_READY		0x01	/* access enable */
-#define UNLOAD_MEDIUM			0x02	/* unload */
-#define LOAD_MEDIUM			0x03	/* load */
-
-/*-----------------------------------
-    STANDARD_INQUIRY
------------------------------------*/
-#define QULIFIRE                0x00
-#define AENC_FNC                0x00
-#define TRML_IOP                0x00
-#define REL_ADR                 0x00
-#define WBUS_32                 0x00
-#define WBUS_16                 0x00
-#define SYNC                    0x00
-#define LINKED                  0x00
-#define CMD_QUE                 0x00
-#define SFT_RE                  0x00
-
-#define VEN_ID_LEN              8	/* Vendor ID Length         */
-#define PRDCT_ID_LEN            16	/* Product ID Length        */
-#define PRDCT_REV_LEN           4	/* Product LOT Length       */
-
-#define DRCT_ACCESS_DEV         0x00	/* Direct Access Device             */
-#define RMB_DISC                0x80	/* The Device is Removable          */
-#define ANSI_SCSI2              0x02	/* Based on ANSI-SCSI2              */
-
-#define SCSI                    0x00	/* Interface ID                     */
-
-void rts51x_scsi_show_command(struct scsi_cmnd *srb);
-void rts51x_set_sense_type(struct rts51x_chip *chip, unsigned int lun, int sense_type);
-void rts51x_set_sense_data(struct rts51x_chip *chip, unsigned int lun, u8 err_code,
-		    u8 sense_key, u32 info, u8 asc, u8 ascq, u8 sns_key_info0,
-		    u16 sns_key_info1);
-
-int rts51x_scsi_handler(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-
-struct Scsi_Host;
-struct scsi_device;
-struct scsi_cmnd;
-
-int slave_alloc(struct scsi_device *sdev);
-int slave_configure(struct scsi_device *sdev);
-int queuecommand(struct Scsi_Host *, struct scsi_cmnd *);
-int command_abort(struct scsi_cmnd *srb);
-int bus_reset(struct scsi_cmnd *srb);
-
-#endif /* __RTS51X_SCSI_H */
diff --git a/drivers/staging/rts5139/rts51x_transport.c b/drivers/staging/rts5139/rts51x_transport.c
deleted file mode 100644
index 74588d249b3..00000000000
--- a/drivers/staging/rts5139/rts51x_transport.c
+++ /dev/null
@@ -1,736 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#include <linux/blkdev.h>
-#include <linux/kthread.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-
-#include <scsi/scsi.h>
-#include <scsi/scsi_eh.h>
-#include <scsi/scsi_device.h>
-
-#include "debug.h"
-#include "rts51x.h"
-#include "rts51x_chip.h"
-#include "rts51x_card.h"
-#include "rts51x_scsi.h"
-#include "rts51x_transport.h"
-#include "trace.h"
-
-/***********************************************************************
- * Scatter-gather transfer buffer access routines
- ***********************************************************************/
-
-/* Copy a buffer of length buflen to/from the srb's transfer buffer.
- * Update the **sgptr and *offset variables so that the next copy will
- * pick up from where this one left off.
- */
-
-unsigned int rts51x_access_sglist(unsigned char *buffer,
-				  unsigned int buflen, void *sglist,
-				  void **sgptr, unsigned int *offset,
-				  enum xfer_buf_dir dir)
-{
-	unsigned int cnt;
-	struct scatterlist *sg = (struct scatterlist *)*sgptr;
-
-	/* We have to go through the list one entry
-	 * at a time.  Each s-g entry contains some number of pages, and
-	 * each page has to be kmap()'ed separately.  If the page is already
-	 * in kernel-addressable memory then kmap() will return its address.
-	 * If the page is not directly accessible -- such as a user buffer
-	 * located in high memory -- then kmap() will map it to a temporary
-	 * position in the kernel's virtual address space.
-	 */
-
-	if (!sg)
-		sg = (struct scatterlist *)sglist;
-
-	/* This loop handles a single s-g list entry, which may
-	 * include multiple pages.  Find the initial page structure
-	 * and the starting offset within the page, and update
-	 * the *offset and **sgptr values for the next loop.
-	 */
-	cnt = 0;
-	while (cnt < buflen && sg) {
-		struct page *page = sg_page(sg) +
-		    ((sg->offset + *offset) >> PAGE_SHIFT);
-		unsigned int poff = (sg->offset + *offset) & (PAGE_SIZE - 1);
-		unsigned int sglen = sg->length - *offset;
-
-		if (sglen > buflen - cnt) {
-
-			/* Transfer ends within this s-g entry */
-			sglen = buflen - cnt;
-			*offset += sglen;
-		} else {
-
-			/* Transfer continues to next s-g entry */
-			*offset = 0;
-			sg = sg_next(sg);
-		}
-
-		/* Transfer the data for all the pages in this
-		 * s-g entry.  For each page: call kmap(), do the
-		 * transfer, and call kunmap() immediately after. */
-		while (sglen > 0) {
-			unsigned int plen = min(sglen, (unsigned int)
-						PAGE_SIZE - poff);
-			unsigned char *ptr = kmap(page);
-
-			if (dir == TO_XFER_BUF)
-				memcpy(ptr + poff, buffer + cnt, plen);
-			else
-				memcpy(buffer + cnt, ptr + poff, plen);
-			kunmap(page);
-
-			/* Start at the beginning of the next page */
-			poff = 0;
-			++page;
-			cnt += plen;
-			sglen -= plen;
-		}
-	}
-	*sgptr = sg;
-
-	/* Return the amount actually transferred */
-	return cnt;
-}
-
-static unsigned int rts51x_access_xfer_buf(unsigned char *buffer,
-				    unsigned int buflen, struct scsi_cmnd *srb,
-				    struct scatterlist **sgptr,
-				    unsigned int *offset, enum xfer_buf_dir dir)
-{
-	return rts51x_access_sglist(buffer, buflen, (void *)scsi_sglist(srb),
-				    (void **)sgptr, offset, dir);
-}
-
-/* Store the contents of buffer into srb's transfer buffer and set the
- * SCSI residue.
- */
-void rts51x_set_xfer_buf(unsigned char *buffer,
-			 unsigned int buflen, struct scsi_cmnd *srb)
-{
-	unsigned int offset = 0;
-	struct scatterlist *sg = NULL;
-
-	buflen = min(buflen, scsi_bufflen(srb));
-	buflen = rts51x_access_xfer_buf(buffer, buflen, srb, &sg, &offset,
-					TO_XFER_BUF);
-	if (buflen < scsi_bufflen(srb))
-		scsi_set_resid(srb, scsi_bufflen(srb) - buflen);
-}
-
-void rts51x_get_xfer_buf(unsigned char *buffer,
-			 unsigned int buflen, struct scsi_cmnd *srb)
-{
-	unsigned int offset = 0;
-	struct scatterlist *sg = NULL;
-
-	buflen = min(buflen, scsi_bufflen(srb));
-	buflen = rts51x_access_xfer_buf(buffer, buflen, srb, &sg, &offset,
-					FROM_XFER_BUF);
-	if (buflen < scsi_bufflen(srb))
-		scsi_set_resid(srb, scsi_bufflen(srb) - buflen);
-}
-
-/* This is the completion handler which will wake us up when an URB
- * completes.
- */
-static void urb_done_completion(struct urb *urb)
-{
-	struct completion *urb_done_ptr = urb->context;
-
-	if (urb_done_ptr)
-		complete(urb_done_ptr);
-}
-
-/* This is the common part of the URB message submission code
- *
- * All URBs from the driver involved in handling a queued scsi
- * command _must_ pass through this function (or something like it) for the
- * abort mechanisms to work properly.
- */
-static int rts51x_msg_common(struct rts51x_chip *chip, struct urb *urb,
-			     int timeout)
-{
-	struct rts51x_usb *rts51x = chip->usb;
-	struct completion urb_done;
-	long timeleft;
-	int status;
-
-	/* don't submit URBs during abort processing */
-	if (test_bit(FLIDX_ABORTING, &rts51x->dflags))
-		TRACE_RET(chip, -EIO);
-
-	/* set up data structures for the wakeup system */
-	init_completion(&urb_done);
-
-	/* fill the common fields in the URB */
-	urb->context = &urb_done;
-	urb->actual_length = 0;
-	urb->error_count = 0;
-	urb->status = 0;
-
-	/* we assume that if transfer_buffer isn't us->iobuf then it
-	 * hasn't been mapped for DMA.  Yes, this is clunky, but it's
-	 * easier than always having the caller tell us whether the
-	 * transfer buffer has already been mapped. */
-	urb->transfer_flags = URB_NO_SETUP_DMA_MAP;
-	if (urb->transfer_buffer == rts51x->iobuf) {
-		urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
-		urb->transfer_dma = rts51x->iobuf_dma;
-	}
-	urb->setup_dma = rts51x->cr_dma;
-
-	/* submit the URB */
-	status = usb_submit_urb(urb, GFP_NOIO);
-	if (status) {
-		/* something went wrong */
-		TRACE_RET(chip, status);
-	}
-
-	/* since the URB has been submitted successfully, it's now okay
-	 * to cancel it */
-	set_bit(FLIDX_URB_ACTIVE, &rts51x->dflags);
-
-	/* did an abort occur during the submission? */
-	if (test_bit(FLIDX_ABORTING, &rts51x->dflags)) {
-
-		/* cancel the URB, if it hasn't been cancelled already */
-		if (test_and_clear_bit(FLIDX_URB_ACTIVE, &rts51x->dflags)) {
-			RTS51X_DEBUGP("-- cancelling URB\n");
-			usb_unlink_urb(urb);
-		}
-	}
-
-	/* wait for the completion of the URB */
-	timeleft =
-	    wait_for_completion_interruptible_timeout(&urb_done,
-						      (timeout * HZ /
-						       1000) ? :
-						      MAX_SCHEDULE_TIMEOUT);
-
-	clear_bit(FLIDX_URB_ACTIVE, &rts51x->dflags);
-
-	if (timeleft <= 0) {
-		RTS51X_DEBUGP("%s -- cancelling URB\n",
-			       timeleft == 0 ? "Timeout" : "Signal");
-		usb_kill_urb(urb);
-		if (timeleft == 0)
-			status = -ETIMEDOUT;
-		else
-			status = -EINTR;
-	} else {
-		status = urb->status;
-	}
-
-	return status;
-}
-
-static int rts51x_clear_halt(struct rts51x_chip *chip, unsigned int pipe);
-
-/*
- * Interpret the results of a URB transfer
- */
-static int interpret_urb_result(struct rts51x_chip *chip, unsigned int pipe,
-				unsigned int length, int result,
-				unsigned int partial)
-{
-	int retval = STATUS_SUCCESS;
-
-	/* RTS51X_DEBUGP("Status code %d; transferred %u/%u\n",
-				result, partial, length); */
-	switch (result) {
-		/* no error code; did we send all the data? */
-	case 0:
-		if (partial != length) {
-			RTS51X_DEBUGP("-- short transfer\n");
-			TRACE_RET(chip, STATUS_TRANS_SHORT);
-		}
-		/* RTS51X_DEBUGP("-- transfer complete\n"); */
-		return STATUS_SUCCESS;
-		/* stalled */
-	case -EPIPE:
-		/* for control endpoints, (used by CB[I]) a stall indicates
-		 * a failed command */
-		if (usb_pipecontrol(pipe)) {
-			RTS51X_DEBUGP("-- stall on control pipe\n");
-			TRACE_RET(chip, STATUS_STALLED);
-		}
-		/* for other sorts of endpoint, clear the stall */
-		RTS51X_DEBUGP("clearing endpoint halt for pipe 0x%x\n", pipe);
-		if (rts51x_clear_halt(chip, pipe) < 0)
-			TRACE_RET(chip, STATUS_ERROR);
-		retval = STATUS_STALLED;
-		TRACE_GOTO(chip, Exit);
-
-		/* babble - the device tried to send more than
-		 * we wanted to read */
-	case -EOVERFLOW:
-		RTS51X_DEBUGP("-- babble\n");
-		retval = STATUS_TRANS_LONG;
-		TRACE_GOTO(chip, Exit);
-
-		/* the transfer was cancelled by abort,
-		 * disconnect, or timeout */
-	case -ECONNRESET:
-		RTS51X_DEBUGP("-- transfer cancelled\n");
-		retval = STATUS_ERROR;
-		TRACE_GOTO(chip, Exit);
-
-		/* short scatter-gather read transfer */
-	case -EREMOTEIO:
-		RTS51X_DEBUGP("-- short read transfer\n");
-		retval = STATUS_TRANS_SHORT;
-		TRACE_GOTO(chip, Exit);
-
-		/* abort or disconnect in progress */
-	case -EIO:
-		RTS51X_DEBUGP("-- abort or disconnect in progress\n");
-		retval = STATUS_ERROR;
-		TRACE_GOTO(chip, Exit);
-
-	case -ETIMEDOUT:
-		RTS51X_DEBUGP("-- time out\n");
-		retval = STATUS_TIMEDOUT;
-		TRACE_GOTO(chip, Exit);
-
-		/* the catch-all error case */
-	default:
-		RTS51X_DEBUGP("-- unknown error\n");
-		retval = STATUS_ERROR;
-		TRACE_GOTO(chip, Exit);
-	}
-
-Exit:
-	if ((retval != STATUS_SUCCESS) && !usb_pipecontrol(pipe))
-		rts51x_clear_hw_error(chip);
-
-	return retval;
-}
-
-int rts51x_ctrl_transfer(struct rts51x_chip *chip, unsigned int pipe,
-			 u8 request, u8 requesttype, u16 value, u16 index,
-			 void *data, u16 size, int timeout)
-{
-	struct rts51x_usb *rts51x = chip->usb;
-	int result;
-
-	RTS51X_DEBUGP("%s: rq=%02x rqtype=%02x value=%04x index=%02x len=%u\n",
-		       __func__, request, requesttype, value, index, size);
-
-	/* fill in the devrequest structure */
-	rts51x->cr->bRequestType = requesttype;
-	rts51x->cr->bRequest = request;
-	rts51x->cr->wValue = cpu_to_le16(value);
-	rts51x->cr->wIndex = cpu_to_le16(index);
-	rts51x->cr->wLength = cpu_to_le16(size);
-
-	/* fill and submit the URB */
-	usb_fill_control_urb(rts51x->current_urb, rts51x->pusb_dev, pipe,
-			     (unsigned char *)rts51x->cr, data, size,
-			     urb_done_completion, NULL);
-	result = rts51x_msg_common(chip, rts51x->current_urb, timeout);
-
-	return interpret_urb_result(chip, pipe, size, result,
-				    rts51x->current_urb->actual_length);
-}
-
-static int rts51x_clear_halt(struct rts51x_chip *chip, unsigned int pipe)
-{
-	int result;
-	int endp = usb_pipeendpoint(pipe);
-
-	if (usb_pipein(pipe))
-		endp |= USB_DIR_IN;
-
-	result = rts51x_ctrl_transfer(chip, SND_CTRL_PIPE(chip),
-				      USB_REQ_CLEAR_FEATURE, USB_RECIP_ENDPOINT,
-				      USB_ENDPOINT_HALT, endp, NULL, 0, 3000);
-	if (result != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	usb_reset_endpoint(chip->usb->pusb_dev, endp);
-
-	return STATUS_SUCCESS;
-}
-
-static void rts51x_sg_clean(struct usb_sg_request *io)
-{
-	if (io->urbs) {
-		while (io->entries--)
-			usb_free_urb(io->urbs[io->entries]);
-		kfree(io->urbs);
-		io->urbs = NULL;
-	}
-	io->dev = NULL;
-}
-
-static int rts51x_sg_init(struct usb_sg_request *io, struct usb_device *dev,
-		   unsigned pipe, unsigned period, struct scatterlist *sg,
-		   int nents, size_t length, gfp_t mem_flags)
-{
-	return usb_sg_init(io, dev, pipe, period, sg, nents, length, mem_flags);
-}
-
-static int rts51x_sg_wait(struct usb_sg_request *io, int timeout)
-{
-	long timeleft;
-	int i;
-	int entries = io->entries;
-
-	/* queue the urbs.  */
-	spin_lock_irq(&io->lock);
-	i = 0;
-	while (i < entries && !io->status) {
-		int retval;
-
-		io->urbs[i]->dev = io->dev;
-		retval = usb_submit_urb(io->urbs[i], GFP_ATOMIC);
-
-		/* after we submit, let completions or cancelations fire;
-		 * we handshake using io->status.
-		 */
-		spin_unlock_irq(&io->lock);
-		switch (retval) {
-			/* maybe the retry will recover */
-		case -ENXIO:	/* hc didn't queue this one */
-		case -EAGAIN:
-		case -ENOMEM:
-			io->urbs[i]->dev = NULL;
-			retval = 0;
-			yield();
-			break;
-
-			/* no error? continue immediately.
-			 *
-			 * NOTE: to work better with UHCI (4K I/O buffer may
-			 * need 3K of TDs) it may be good to limit how many
-			 * URBs are queued at once; N milliseconds?
-			 */
-		case 0:
-			++i;
-			cpu_relax();
-			break;
-
-			/* fail any uncompleted urbs */
-		default:
-			io->urbs[i]->dev = NULL;
-			io->urbs[i]->status = retval;
-			dev_dbg(&io->dev->dev, "%s, submit --> %d\n",
-				__func__, retval);
-			usb_sg_cancel(io);
-		}
-		spin_lock_irq(&io->lock);
-		if (retval && (io->status == 0 || io->status == -ECONNRESET))
-			io->status = retval;
-	}
-	io->count -= entries - i;
-	if (io->count == 0)
-		complete(&io->complete);
-	spin_unlock_irq(&io->lock);
-
-	timeleft =
-	    wait_for_completion_interruptible_timeout(&io->complete,
-						      (timeout * HZ /
-						       1000) ? :
-						      MAX_SCHEDULE_TIMEOUT);
-	if (timeleft <= 0) {
-		RTS51X_DEBUGP("%s -- cancelling SG request\n",
-			       timeleft == 0 ? "Timeout" : "Signal");
-		usb_sg_cancel(io);
-		if (timeleft == 0)
-			io->status = -ETIMEDOUT;
-		else
-			io->status = -EINTR;
-	}
-
-	rts51x_sg_clean(io);
-	return io->status;
-}
-
-/*
- * Transfer a scatter-gather list via bulk transfer
- *
- * This function does basically the same thing as usb_stor_bulk_transfer_buf()
- * above, but it uses the usbcore scatter-gather library.
- */
-static int rts51x_bulk_transfer_sglist(struct rts51x_chip *chip,
-				       unsigned int pipe,
-				       struct scatterlist *sg, int num_sg,
-				       unsigned int length,
-				       unsigned int *act_len, int timeout)
-{
-	int result;
-
-	/* don't submit s-g requests during abort processing */
-	if (test_bit(FLIDX_ABORTING, &chip->usb->dflags))
-		TRACE_RET(chip, STATUS_ERROR);
-
-	/* initialize the scatter-gather request block */
-	RTS51X_DEBUGP("%s: xfer %u bytes, %d entries\n", __func__,
-		       length, num_sg);
-	result =
-	    rts51x_sg_init(&chip->usb->current_sg, chip->usb->pusb_dev, pipe, 0,
-			   sg, num_sg, length, GFP_NOIO);
-	if (result) {
-		RTS51X_DEBUGP("rts51x_sg_init returned %d\n", result);
-		TRACE_RET(chip, STATUS_ERROR);
-	}
-
-	/* since the block has been initialized successfully, it's now
-	 * okay to cancel it */
-	set_bit(FLIDX_SG_ACTIVE, &chip->usb->dflags);
-
-	/* did an abort occur during the submission? */
-	if (test_bit(FLIDX_ABORTING, &chip->usb->dflags)) {
-
-		/* cancel the request, if it hasn't been cancelled already */
-		if (test_and_clear_bit(FLIDX_SG_ACTIVE, &chip->usb->dflags)) {
-			RTS51X_DEBUGP("-- cancelling sg request\n");
-			usb_sg_cancel(&chip->usb->current_sg);
-		}
-	}
-
-	/* wait for the completion of the transfer */
-	result = rts51x_sg_wait(&chip->usb->current_sg, timeout);
-
-	clear_bit(FLIDX_SG_ACTIVE, &chip->usb->dflags);
-
-	/* result = us->current_sg.status; */
-	if (act_len)
-		*act_len = chip->usb->current_sg.bytes;
-	return interpret_urb_result(chip, pipe, length, result,
-				    chip->usb->current_sg.bytes);
-}
-
-static int rts51x_bulk_transfer_buf(struct rts51x_chip *chip,
-			     unsigned int pipe,
-			     void *buf, unsigned int length,
-			     unsigned int *act_len, int timeout)
-{
-	int result;
-
-	/* fill and submit the URB */
-	usb_fill_bulk_urb(chip->usb->current_urb, chip->usb->pusb_dev, pipe,
-			  buf, length, urb_done_completion, NULL);
-	result = rts51x_msg_common(chip, chip->usb->current_urb, timeout);
-
-	/* store the actual length of the data transferred */
-	if (act_len)
-		*act_len = chip->usb->current_urb->actual_length;
-	return interpret_urb_result(chip, pipe, length, result,
-				    chip->usb->current_urb->actual_length);
-}
-
-int rts51x_transfer_data(struct rts51x_chip *chip, unsigned int pipe,
-			 void *buf, unsigned int len, int use_sg,
-			 unsigned int *act_len, int timeout)
-{
-	int result;
-
-	if (timeout < 600)
-		timeout = 600;
-
-	if (use_sg) {
-		result =
-		    rts51x_bulk_transfer_sglist(chip, pipe,
-						(struct scatterlist *)buf,
-						use_sg, len, act_len, timeout);
-	} else {
-		result =
-		    rts51x_bulk_transfer_buf(chip, pipe, buf, len, act_len,
-					     timeout);
-	}
-
-	return result;
-}
-
-int rts51x_transfer_data_partial(struct rts51x_chip *chip, unsigned int pipe,
-				 void *buf, void **ptr, unsigned int *offset,
-				 unsigned int len, int use_sg,
-				 unsigned int *act_len, int timeout)
-{
-	int result;
-
-	if (timeout < 600)
-		timeout = 600;
-
-	if (use_sg) {
-		void *tmp_buf = kmalloc(len, GFP_KERNEL);
-		if (!tmp_buf)
-			TRACE_RET(chip, STATUS_NOMEM);
-
-		if (usb_pipeout(pipe)) {
-			rts51x_access_sglist(tmp_buf, len, buf, ptr, offset,
-					     FROM_XFER_BUF);
-		}
-		result =
-		    rts51x_bulk_transfer_buf(chip, pipe, tmp_buf, len, act_len,
-					     timeout);
-		if (result == STATUS_SUCCESS) {
-			if (usb_pipein(pipe)) {
-				rts51x_access_sglist(tmp_buf, len, buf, ptr,
-						     offset, TO_XFER_BUF);
-			}
-		}
-
-		kfree(tmp_buf);
-	} else {
-		unsigned int step = 0;
-		if (offset)
-			step = *offset;
-		result =
-		    rts51x_bulk_transfer_buf(chip, pipe, buf + step, len,
-					     act_len, timeout);
-		if (act_len)
-			step += *act_len;
-		else
-			step += len;
-		if (offset)
-			*offset = step;
-	}
-
-	return result;
-}
-
-int rts51x_get_epc_status(struct rts51x_chip *chip, u16 *status)
-{
-	unsigned int pipe = RCV_INTR_PIPE(chip);
-	struct usb_host_endpoint *ep;
-	struct completion urb_done;
-	int result;
-
-	if (!status)
-		TRACE_RET(chip, STATUS_ERROR);
-
-	/* set up data structures for the wakeup system */
-	init_completion(&urb_done);
-
-	ep = chip->usb->pusb_dev->ep_in[usb_pipeendpoint(pipe)];
-
-	/* fill and submit the URB */
-	/* Set interval to 10 here to match the endpoint descriptor,
-	 * the polling interval is controlled by the polling thread */
-	usb_fill_int_urb(chip->usb->intr_urb, chip->usb->pusb_dev, pipe,
-			 status, 2, urb_done_completion, &urb_done, 10);
-
-	result = rts51x_msg_common(chip, chip->usb->intr_urb, 100);
-
-	return interpret_urb_result(chip, pipe, 2, result,
-				    chip->usb->intr_urb->actual_length);
-}
-
-static u8 media_not_present[] = {
-	0x70, 0, 0x02, 0, 0, 0, 0, 10, 0, 0, 0, 0, 0x3A, 0, 0, 0, 0, 0 };
-static u8 invalid_cmd_field[] = {
-	0x70, 0, 0x05, 0, 0, 0, 0, 10, 0, 0, 0, 0, 0x24, 0, 0, 0, 0, 0 };
-
-void rts51x_invoke_transport(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	int result;
-
-#ifdef CONFIG_PM
-	if (chip->option.ss_en) {
-		if (srb->cmnd[0] == TEST_UNIT_READY) {
-			if (RTS51X_CHK_STAT(chip, STAT_SS)) {
-				if (check_fake_card_ready(chip,
-							SCSI_LUN(srb))) {
-					srb->result = SAM_STAT_GOOD;
-				} else {
-					srb->result = SAM_STAT_CHECK_CONDITION;
-					memcpy(srb->sense_buffer,
-					       media_not_present, SENSE_SIZE);
-				}
-				return;
-			}
-		} else if (srb->cmnd[0] == ALLOW_MEDIUM_REMOVAL) {
-			if (RTS51X_CHK_STAT(chip, STAT_SS)) {
-				int prevent = srb->cmnd[4] & 0x1;
-
-				if (prevent) {
-					srb->result = SAM_STAT_CHECK_CONDITION;
-					memcpy(srb->sense_buffer,
-					       invalid_cmd_field, SENSE_SIZE);
-				} else {
-					srb->result = SAM_STAT_GOOD;
-				}
-				return;
-			}
-		} else {
-			if (RTS51X_CHK_STAT(chip, STAT_SS)
-			    || RTS51X_CHK_STAT(chip, STAT_SS_PRE)) {
-				/* Wake up device */
-				RTS51X_DEBUGP("Try to wake up device\n");
-				chip->resume_from_scsi = 1;
-
-				rts51x_try_to_exit_ss(chip);
-
-				if (RTS51X_CHK_STAT(chip, STAT_SS)) {
-					wait_timeout(3000);
-
-					rts51x_init_chip(chip);
-					rts51x_init_cards(chip);
-				}
-			}
-		}
-	}
-#endif
-
-	result = rts51x_scsi_handler(srb, chip);
-
-	/* if there is a transport error, reset and don't auto-sense */
-	if (result == TRANSPORT_ERROR) {
-		RTS51X_DEBUGP("-- transport indicates error, resetting\n");
-		srb->result = DID_ERROR << 16;
-		goto Handle_Errors;
-	}
-
-	srb->result = SAM_STAT_GOOD;
-
-	/*
-	 * If we have a failure, we're going to do a REQUEST_SENSE
-	 * automatically.  Note that we differentiate between a command
-	 * "failure" and an "error" in the transport mechanism.
-	 */
-	if (result == TRANSPORT_FAILED) {
-		/* set the result so the higher layers expect this data */
-		srb->result = SAM_STAT_CHECK_CONDITION;
-		memcpy(srb->sense_buffer,
-		       (unsigned char *)&(chip->sense_buffer[SCSI_LUN(srb)]),
-		       sizeof(struct sense_data_t));
-	}
-
-	return;
-
-	/* Error and abort processing: try to resynchronize with the device
-	 * by issuing a port reset.  If that fails, try a class-specific
-	 * device reset. */
-Handle_Errors:
-	return;
-}
diff --git a/drivers/staging/rts5139/rts51x_transport.h b/drivers/staging/rts5139/rts51x_transport.h
deleted file mode 100644
index 024f115540a..00000000000
--- a/drivers/staging/rts5139/rts51x_transport.h
+++ /dev/null
@@ -1,67 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_TRANSPORT_H
-#define __RTS51X_TRANSPORT_H
-
-#include <linux/kernel.h>
-
-#include "rts51x.h"
-#include "rts51x_chip.h"
-
-#if 1 /* LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 34) */
-#define URB_NO_SETUP_DMA_MAP		0
-#endif
-
-unsigned int rts51x_access_sglist(unsigned char *buffer,
-				  unsigned int buflen, void *sglist,
-				  void **sgptr, unsigned int *offset,
-				  enum xfer_buf_dir dir);
-void rts51x_set_xfer_buf(unsigned char *buffer, unsigned int buflen,
-			 struct scsi_cmnd *srb);
-void rts51x_get_xfer_buf(unsigned char *buffer, unsigned int buflen,
-			 struct scsi_cmnd *srb);
-
-int rts51x_ctrl_transfer(struct rts51x_chip *chip, unsigned int pipe,
-			 u8 request, u8 requesttype, u16 value, u16 index,
-			 void *data, u16 size, int timeout);
-int rts51x_transfer_data(struct rts51x_chip *chip, unsigned int pipe,
-			 void *buf, unsigned int len, int use_sg,
-			 unsigned int *act_len, int timeout);
-int rts51x_transfer_data_partial(struct rts51x_chip *chip, unsigned int pipe,
-				 void *buf, void **ptr, unsigned int *offset,
-				 unsigned int len, int use_sg,
-				 unsigned int *act_len, int timeout);
-
-#ifndef POLLING_IN_THREAD
-int rts51x_start_epc_transfer(struct rts51x_chip *chip);
-void rts51x_cancel_epc_transfer(struct rts51x_chip *chip);
-#endif
-
-int rts51x_get_epc_status(struct rts51x_chip *chip, u16 *status);
-void rts51x_invoke_transport(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-
-#endif /* __RTS51X_TRANSPORT_H */
diff --git a/drivers/staging/rts5139/sd.c b/drivers/staging/rts5139/sd.c
deleted file mode 100644
index da5a9b8fb69..00000000000
--- a/drivers/staging/rts5139/sd.c
+++ /dev/null
@@ -1,3274 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#include <linux/blkdev.h>
-#include <linux/kthread.h>
-#include <linux/sched.h>
-
-#include "debug.h"
-#include "trace.h"
-#include "rts51x.h"
-#include "rts51x_transport.h"
-#include "rts51x_scsi.h"
-#include "rts51x_card.h"
-#include "sd.h"
-
-static inline void sd_set_reset_fail(struct rts51x_chip *chip, u8 err_code)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-
-	sd_card->sd_reset_fail |= err_code;
-}
-
-static inline void sd_clear_reset_fail(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-
-	sd_card->sd_reset_fail = 0;
-}
-
-static inline int sd_check_reset_fail(struct rts51x_chip *chip, u8 err_code)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-
-	return sd_card->sd_reset_fail & err_code;
-}
-
-static inline void sd_set_err_code(struct rts51x_chip *chip, u8 err_code)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-
-	sd_card->err_code |= err_code;
-}
-
-static inline void sd_clr_err_code(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-
-	sd_card->err_code = 0;
-}
-
-static inline int sd_check_err_code(struct rts51x_chip *chip, u8 err_code)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-
-	return sd_card->err_code & err_code;
-}
-
-static int sd_parse_err_code(struct rts51x_chip *chip)
-{
-	TRACE_RET(chip, STATUS_FAIL);
-}
-
-int sd_check_data0_status(struct rts51x_chip *chip)
-{
-	int retval;
-	u8 stat;
-
-	retval = rts51x_ep0_read_register(chip, SD_BUS_STAT, &stat);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	if (!(stat & SD_DAT0_STATUS)) {
-		sd_set_err_code(chip, SD_BUSY);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_send_cmd_get_rsp(struct rts51x_chip *chip, u8 cmd_idx,
-			       u32 arg, u8 rsp_type, u8 *rsp, int rsp_len)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	int timeout = 50;
-	u16 reg_addr;
-	u8 buf[17], stat;
-	int len = 2;
-	int rty_cnt = 0;
-
-	sd_clr_err_code(chip);
-
-	RTS51X_DEBUGP("SD/MMC CMD %d, arg = 0x%08x\n", cmd_idx, arg);
-
-	if (rsp_type == SD_RSP_TYPE_R1b)
-		timeout = 3000;
-
-RTY_SEND_CMD:
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD0, 0xFF, 0x40 | cmd_idx);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD1, 0xFF, (u8) (arg >> 24));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD2, 0xFF, (u8) (arg >> 16));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD3, 0xFF, (u8) (arg >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD4, 0xFF, (u8) arg);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF, rsp_type);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       PINGPONG_BUFFER);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-		       SD_TM_CMD_RSP | SD_TRANSFER_START);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER,
-		       SD_TRANSFER_END | SD_STAT_IDLE,
-		       SD_TRANSFER_END | SD_STAT_IDLE);
-
-	rts51x_add_cmd(chip, READ_REG_CMD, SD_STAT1, 0, 0);
-
-	if (CHECK_USB(chip, USB_20)) {
-		if (rsp_type == SD_RSP_TYPE_R2) {
-			/* Read data from ping-pong buffer */
-			for (reg_addr = PPBUF_BASE2;
-			     reg_addr < PPBUF_BASE2 + 16; reg_addr++) {
-				rts51x_add_cmd(chip, READ_REG_CMD, reg_addr, 0,
-					       0);
-			}
-			len = 18;
-		} else if (rsp_type != SD_RSP_TYPE_R0) {
-			/* Read data from SD_CMDx registers */
-			for (reg_addr = SD_CMD0; reg_addr <= SD_CMD4;
-			     reg_addr++) {
-				rts51x_add_cmd(chip, READ_REG_CMD, reg_addr, 0,
-					       0);
-			}
-			len = 7;
-		} else {
-			len = 2;
-		}
-	} else {
-		len = 2;
-	}
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, len, timeout);
-
-	if (CHECK_SD_TRANS_FAIL(chip, retval)) {
-		u8 val;
-
-		rts51x_ep0_read_register(chip, SD_STAT1, &val);
-		RTS51X_DEBUGP("SD_STAT1: 0x%x\n", val);
-
-		rts51x_ep0_read_register(chip, SD_STAT2, &val);
-		RTS51X_DEBUGP("SD_STAT2: 0x%x\n", val);
-
-		if (val & SD_RSP_80CLK_TIMEOUT)
-			sd_set_err_code(chip, SD_RSP_TIMEOUT);
-
-		rts51x_ep0_read_register(chip, SD_BUS_STAT, &val);
-		RTS51X_DEBUGP("SD_BUS_STAT: 0x%x\n", val);
-
-		if (retval == STATUS_TIMEDOUT) {
-			if (rsp_type & SD_WAIT_BUSY_END) {
-				retval = sd_check_data0_status(chip);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, retval);
-			} else {
-				sd_set_err_code(chip, SD_TO_ERR);
-			}
-		}
-		rts51x_clear_sd_error(chip);
-
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (rsp_type == SD_RSP_TYPE_R0)
-		return STATUS_SUCCESS;
-
-	if (CHECK_USB(chip, USB_20)) {
-		rts51x_read_rsp_buf(chip, 2, buf, len - 2);
-	} else {
-		if (rsp_type == SD_RSP_TYPE_R2) {
-			reg_addr = PPBUF_BASE2;
-			len = 16;
-		} else {
-			reg_addr = SD_CMD0;
-			len = 5;
-		}
-		retval = rts51x_seq_read_register(chip, reg_addr,
-						     (unsigned short)len, buf);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-	stat = chip->rsp_buf[1];
-
-	/* Check (Start,Transmission) bit of Response */
-	if ((buf[0] & 0xC0) != 0) {
-		sd_set_err_code(chip, SD_STS_ERR);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-	/* Check CRC7 */
-	if (!(rsp_type & SD_NO_CHECK_CRC7)) {
-		if (stat & SD_CRC7_ERR) {
-			if (cmd_idx == WRITE_MULTIPLE_BLOCK) {
-				sd_set_err_code(chip, SD_CRC_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-			if (rty_cnt < SD_MAX_RETRY_COUNT) {
-				wait_timeout(20);
-				rty_cnt++;
-				goto RTY_SEND_CMD;
-			} else {
-				sd_set_err_code(chip, SD_CRC_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-	}
-	/* Check Status */
-	if ((rsp_type == SD_RSP_TYPE_R1) || (rsp_type == SD_RSP_TYPE_R1b)) {
-		if ((cmd_idx != SEND_RELATIVE_ADDR)
-		    && (cmd_idx != SEND_IF_COND)) {
-			if (cmd_idx != STOP_TRANSMISSION) {
-				if (buf[1] & 0x80)
-					TRACE_RET(chip, STATUS_FAIL);
-			}
-			if (buf[1] & 0x7F) {
-				RTS51X_DEBUGP("buf[1]: 0x%02x\n", buf[1]);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-			if (buf[2] & 0xFF) {
-				RTS51X_DEBUGP("buf[2]: 0x%02x\n", buf[2]);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-			if (buf[3] & 0x80) {
-				RTS51X_DEBUGP("buf[3]: 0x%02x\n", buf[3]);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-			if (buf[3] & 0x01) {
-				/* Get "READY_FOR_DATA" bit */
-				sd_card->sd_data_buf_ready = 1;
-			} else {
-				sd_card->sd_data_buf_ready = 0;
-			}
-		}
-	}
-
-	if (rsp && rsp_len)
-		memcpy(rsp, buf, rsp_len);
-
-	return STATUS_SUCCESS;
-}
-
-static inline void sd_print_debug_reg(struct rts51x_chip *chip)
-{
-#ifdef CONFIG_RTS5139_DEBUG
-	u8 val = 0;
-
-	rts51x_ep0_read_register(chip, SD_STAT1, &val);
-	RTS51X_DEBUGP("SD_STAT1: 0x%x\n", val);
-	rts51x_ep0_read_register(chip, SD_STAT2, &val);
-	RTS51X_DEBUGP("SD_STAT2: 0x%x\n", val);
-	rts51x_ep0_read_register(chip, SD_BUS_STAT, &val);
-	RTS51X_DEBUGP("SD_BUS_STAT: 0x%x\n", val);
-#endif
-}
-
-int sd_read_data(struct rts51x_chip *chip, u8 trans_mode, u8 *cmd, int cmd_len,
-		 u16 byte_cnt, u16 blk_cnt, u8 bus_width, u8 *buf, int buf_len,
-		 int timeout)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	int i;
-
-	sd_clr_err_code(chip);
-
-	if (!buf)
-		buf_len = 0;
-
-	if (buf_len > 512)
-		/* This function can't read data more than one page */
-		TRACE_RET(chip, STATUS_FAIL);
-
-	rts51x_init_cmd(chip);
-
-	if (cmd_len) {
-		RTS51X_DEBUGP("SD/MMC CMD %d\n", cmd[0] - 0x40);
-		for (i = 0; i < (cmd_len < 6 ? cmd_len : 6); i++) {
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD0 + i, 0xFF,
-				       cmd[i]);
-		}
-	}
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, (u8) byte_cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_H, 0xFF,
-		       (u8) (byte_cnt >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L, 0xFF, (u8) blk_cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H, 0xFF,
-		       (u8) (blk_cnt >> 8));
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0x03, bus_width);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF,
-		       SD_CALCULATE_CRC7 | SD_CHECK_CRC16 | SD_NO_WAIT_BUSY_END
-		       | SD_CHECK_CRC7 | SD_RSP_LEN_6);
-	if (trans_mode != SD_TM_AUTO_TUNING) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-			       PINGPONG_BUFFER);
-	}
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-		       trans_mode | SD_TRANSFER_START);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER, SD_TRANSFER_END,
-		       SD_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 1, timeout);
-
-	if (CHECK_SD_TRANS_FAIL(chip, retval)) {
-		sd_print_debug_reg(chip);
-		if (retval == STATUS_TIMEDOUT) {
-			sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
-					    SD_RSP_TYPE_R1, NULL, 0);
-		}
-
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (buf && buf_len) {
-		retval = rts51x_read_ppbuf(chip, buf, buf_len);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_write_data(struct rts51x_chip *chip, u8 trans_mode,
-			 u8 *cmd, int cmd_len, u16 byte_cnt, u16 blk_cnt,
-			 u8 bus_width, u8 *buf, int buf_len, int timeout)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	int i;
-
-	sd_clr_err_code(chip);
-
-	if (!buf)
-		buf_len = 0;
-
-	/* This function can't write data more than one page */
-	if (buf_len > 512)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	if (buf && buf_len) {
-		retval = rts51x_write_ppbuf(chip, buf, buf_len);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	rts51x_init_cmd(chip);
-
-	if (cmd_len) {
-		RTS51X_DEBUGP("SD/MMC CMD %d\n", cmd[0] - 0x40);
-		for (i = 0; i < (cmd_len < 6 ? cmd_len : 6); i++) {
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD0 + i, 0xFF,
-				       cmd[i]);
-		}
-	}
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, (u8) byte_cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_H, 0xFF,
-		       (u8) (byte_cnt >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L, 0xFF, (u8) blk_cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H, 0xFF,
-		       (u8) (blk_cnt >> 8));
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0x03, bus_width);
-
-	if (cmd_len) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF,
-			       SD_CALCULATE_CRC7 | SD_CHECK_CRC16 |
-			       SD_WAIT_BUSY_END | SD_CHECK_CRC7 | SD_RSP_LEN_6);
-
-	} else {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF,
-			       SD_CALCULATE_CRC7 | SD_CHECK_CRC16 |
-			       SD_NO_WAIT_BUSY_END | SD_CHECK_CRC7 |
-			       SD_RSP_LEN_6);
-	}
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-		       trans_mode | SD_TRANSFER_START);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER, SD_TRANSFER_END,
-		       SD_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 1, timeout);
-
-	if (CHECK_SD_TRANS_FAIL(chip, retval)) {
-		sd_print_debug_reg(chip);
-
-		if (retval == STATUS_TIMEDOUT)
-			sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
-					    SD_RSP_TYPE_R1, NULL, 0);
-
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_check_csd(struct rts51x_chip *chip, char check_wp)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	int i;
-	u8 csd_ver, trans_speed;
-	u8 rsp[16];
-
-	for (i = 0; i < 6; i++) {
-		if (monitor_card_cd(chip, SD_CARD) == CD_NOT_EXIST) {
-			sd_set_reset_fail(chip, SD_RESET_FAIL);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		retval =
-		    sd_send_cmd_get_rsp(chip, SEND_CSD, sd_card->sd_addr,
-					SD_RSP_TYPE_R2, rsp, 16);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-
-	if (i == 6)
-		TRACE_RET(chip, STATUS_FAIL);
-	memcpy(sd_card->raw_csd, rsp + 1, 15);
-	/* Get CRC7 */
-	RTS51X_READ_REG(chip, SD_CMD5, sd_card->raw_csd + 15);
-
-	RTS51X_DEBUGP("CSD Response:\n");
-	RTS51X_DUMP(rsp, 16);
-
-	/* Get CSD Version */
-	csd_ver = (rsp[1] & 0xc0) >> 6;
-	RTS51X_DEBUGP("csd_ver = %d\n", csd_ver);
-
-	trans_speed = rsp[4];
-	if ((trans_speed & 0x07) == 0x02) {	/* 10Mbits/s */
-		if ((trans_speed & 0xf8) >= 0x30) {	/* >25Mbits/s */
-			if (chip->asic_code)
-				sd_card->sd_clock = 46;
-			else
-				sd_card->sd_clock = CLK_50;
-		} else if ((trans_speed & 0xf8) == 0x28) { /* 20Mbits/s */
-			if (chip->asic_code)
-				sd_card->sd_clock = 39;
-			else
-				sd_card->sd_clock = CLK_40;
-		} else if ((trans_speed & 0xf8) == 0x20) { /* 15Mbits/s */
-			if (chip->asic_code)
-				sd_card->sd_clock = 29;
-			else
-				sd_card->sd_clock = CLK_30;
-		} else if ((trans_speed & 0xf8) >= 0x10) { /* 12Mbits/s */
-			if (chip->asic_code)
-				sd_card->sd_clock = 23;
-			else
-				sd_card->sd_clock = CLK_20;
-		} else if ((trans_speed & 0x08) >= 0x08) { /* 10Mbits/s */
-			if (chip->asic_code)
-				sd_card->sd_clock = 19;
-			else
-				sd_card->sd_clock = CLK_20;
-		} /*else { */
-			/*If this ,then slow card will use 30M clock */
-			/* TRACE_RET(chip, STATUS_FAIL); */
-		/* } */
-	}
-	/*else {
-	   TRACE_RET(chip, STATUS_FAIL);
-	   } */
-	if (CHK_MMC_SECTOR_MODE(sd_card)) {
-		sd_card->capacity = 0;
-	} else {
-		/* For High-Capacity Card, CSD_STRUCTURE always be "0x1" */
-		if ((!CHK_SD_HCXC(sd_card)) || (csd_ver == 0)) {
-			/* Calculate total sector according to C_SIZE,
-			 * C_SIZE_MULT & READ_BL_LEN */
-			u8 blk_size, c_size_mult;
-			u16 c_size;
-			/* Get READ_BL_LEN */
-			blk_size = rsp[6] & 0x0F;
-			/* Get C_SIZE */
-			c_size = ((u16) (rsp[7] & 0x03) << 10)
-			    + ((u16) rsp[8] << 2)
-			    + ((u16) (rsp[9] & 0xC0) >> 6);
-			/* Get C_SIZE_MUL */
-			c_size_mult = (u8) ((rsp[10] & 0x03) << 1);
-			c_size_mult += (rsp[11] & 0x80) >> 7;
-			/* Calculate total Capacity  */
-			sd_card->capacity =
-			    (((u32) (c_size + 1)) *
-			     (1 << (c_size_mult + 2))) << (blk_size - 9);
-		} else {
-			/* High Capacity Card and Use CSD2.0 Version */
-			u32 total_sector = 0;
-			total_sector = (((u32) rsp[8] & 0x3f) << 16) |
-			    ((u32) rsp[9] << 8) | (u32) rsp[10];
-			/* Total Capacity= (C_SIZE+1) *
-			 * 512K Byte = (C_SIZE+1)K Sector,1K = 1024 Bytes */
-			sd_card->capacity = (total_sector + 1) << 10;
-		}
-	}
-
-	/* We need check Write-Protected Status by Field PERM WP or TEMP WP */
-	if (check_wp) {
-		if (rsp[15] & 0x30)
-			chip->card_wp |= SD_CARD;
-		RTS51X_DEBUGP("CSD WP Status: 0x%x\n", rsp[15]);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_set_sample_push_timing(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-
-	rts51x_init_cmd(chip);
-
-	if (CHK_SD_SDR104(sd_card) || CHK_SD_SDR50(sd_card)) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG1,
-			       0x0C | SD_ASYNC_FIFO_RST,
-			       SD_30_MODE | SD_ASYNC_FIFO_RST);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_CLK_SOURCE, 0xFF,
-			       CRC_VAR_CLK0 | SD30_FIX_CLK | SAMPLE_VAR_CLK1);
-	} else if (CHK_SD_DDR50(sd_card) || CHK_MMC_DDR52(sd_card)) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG1,
-			       0x0C | SD_ASYNC_FIFO_RST,
-			       SD_DDR_MODE | SD_ASYNC_FIFO_RST);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_CLK_SOURCE, 0xFF,
-			       CRC_VAR_CLK0 | SD30_FIX_CLK | SAMPLE_VAR_CLK1);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_PUSH_POINT_CTL,
-			       DDR_VAR_TX_CMD_DAT, DDR_VAR_TX_CMD_DAT);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_SAMPLE_POINT_CTL,
-			       DDR_VAR_RX_DAT | DDR_VAR_RX_CMD,
-			       DDR_VAR_RX_DAT | DDR_VAR_RX_CMD);
-	} else {
-		u8 val = 0;
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0x0C, SD_20_MODE);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_CLK_SOURCE, 0xFF,
-			       CRC_FIX_CLK | SD30_VAR_CLK0 | SAMPLE_VAR_CLK1);
-
-		if ((chip->option.sd_ctl & SD_PUSH_POINT_CTL_MASK) ==
-		    SD_PUSH_POINT_AUTO) {
-			val = SD20_TX_NEG_EDGE;
-		} else if ((chip->option.sd_ctl & SD_PUSH_POINT_CTL_MASK) ==
-			   SD_PUSH_POINT_DELAY) {
-			val = SD20_TX_14_AHEAD;
-		} else {
-			val = SD20_TX_NEG_EDGE;
-		}
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_PUSH_POINT_CTL,
-			       SD20_TX_SEL_MASK, val);
-
-		if ((chip->option.sd_ctl & SD_SAMPLE_POINT_CTL_MASK) ==
-		    SD_SAMPLE_POINT_AUTO) {
-			if (chip->asic_code) {
-				if (CHK_SD_HS(sd_card) || CHK_MMC_52M(sd_card))
-					val = SD20_RX_14_DELAY;
-				else
-					val = SD20_RX_POS_EDGE;
-			} else {
-				val = SD20_RX_14_DELAY;
-			}
-		} else if ((chip->option.sd_ctl & SD_SAMPLE_POINT_CTL_MASK) ==
-			   SD_SAMPLE_POINT_DELAY) {
-			val = SD20_RX_14_DELAY;
-		} else {
-			val = SD20_RX_POS_EDGE;
-		}
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_SAMPLE_POINT_CTL,
-			       SD20_RX_SEL_MASK, val);
-	}
-
-	if (CHK_MMC_DDR52(sd_card) && CHK_MMC_8BIT(sd_card)) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DMA1_CTL,
-			       EXTEND_DMA1_ASYNC_SIGNAL, 0);
-	}
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static void sd_choose_proper_clock(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-
-	if (CHK_SD_SDR104(sd_card)) {
-		if (chip->asic_code)
-			sd_card->sd_clock = chip->option.asic_sd_sdr104_clk;
-		else
-			sd_card->sd_clock = chip->option.fpga_sd_sdr104_clk;
-	} else if (CHK_SD_DDR50(sd_card)) {
-		if (chip->asic_code)
-			sd_card->sd_clock = chip->option.asic_sd_ddr50_clk;
-		else
-			sd_card->sd_clock = chip->option.fpga_sd_ddr50_clk;
-	} else if (CHK_SD_SDR50(sd_card)) {
-		if (chip->asic_code)
-			sd_card->sd_clock = chip->option.asic_sd_sdr50_clk;
-		else
-			sd_card->sd_clock = chip->option.fpga_sd_sdr50_clk;
-	} else if (CHK_SD_HS(sd_card)) {
-		if (chip->asic_code)
-			sd_card->sd_clock = chip->option.asic_sd_hs_clk;
-		else
-			sd_card->sd_clock = chip->option.fpga_sd_hs_clk;
-	} else if (CHK_MMC_52M(sd_card) || CHK_MMC_DDR52(sd_card)) {
-		if (chip->asic_code)
-			sd_card->sd_clock = chip->option.asic_mmc_52m_clk;
-		else
-			sd_card->sd_clock = chip->option.fpga_mmc_52m_clk;
-	} else if (CHK_MMC_26M(sd_card)) {
-		if (chip->asic_code) {
-			sd_card->sd_clock = 46;
-			RTS51X_DEBUGP("Set MMC clock to 22.5MHz\n");
-		} else {
-			sd_card->sd_clock = CLK_50;
-		}
-	}
-}
-
-static int sd_set_init_para(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-
-	retval = sd_set_sample_push_timing(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	sd_choose_proper_clock(chip);
-
-	retval = switch_clock(chip, sd_card->sd_clock);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_sd_select_card(struct rts51x_chip *chip, int select)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	u8 cmd_idx, cmd_type;
-	u32 addr;
-
-	if (select) {
-		cmd_idx = SELECT_CARD;
-		cmd_type = SD_RSP_TYPE_R1;
-		addr = sd_card->sd_addr;
-	} else {
-		cmd_idx = DESELECT_CARD;
-		cmd_type = SD_RSP_TYPE_R0;
-		addr = 0;
-	}
-
-	retval = sd_send_cmd_get_rsp(chip, cmd_idx, addr, cmd_type, NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_wait_currentstate_dataready(struct rts51x_chip *chip, u8 statechk,
-				   u8 rdychk, u16 pollingcnt)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	u8 rsp[5];
-	u16 i;
-
-	for (i = 0; i < pollingcnt; i++) {
-
-		retval =
-		    sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
-					SD_RSP_TYPE_R1, rsp, 5);
-		if (retval == STATUS_SUCCESS) {
-			if (((rsp[3] & 0x1E) == statechk)
-			    && ((rsp[3] & 0x01) == rdychk)) {
-				return STATUS_SUCCESS;
-			}
-		} else {
-			rts51x_clear_sd_error(chip);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-
-	return STATUS_TIMEDOUT;
-}
-
-static int sd_voltage_switch(struct rts51x_chip *chip)
-{
-	int retval;
-	u8 stat;
-
-	RTS51X_WRITE_REG(chip, SD_BUS_STAT,
-			 SD_CLK_TOGGLE_EN | SD_CLK_FORCE_STOP,
-			 SD_CLK_TOGGLE_EN);
-
-	retval =
-	    sd_send_cmd_get_rsp(chip, VOLTAGE_SWITCH, 0, SD_RSP_TYPE_R1, NULL,
-				0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_READ_REG(chip, SD_BUS_STAT, &stat);
-	if (stat & (SD_CMD_STATUS | SD_DAT3_STATUS | SD_DAT2_STATUS |
-		    SD_DAT1_STATUS | SD_DAT0_STATUS))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	rts51x_init_cmd(chip);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BUS_STAT, 0xFF,
-		       SD_CLK_FORCE_STOP);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_PAD_CTL, SD_IO_USING_1V8,
-		       SD_IO_USING_1V8);
-	if (chip->asic_code)
-		rts51x_add_cmd(chip, WRITE_REG_CMD, LDO_POWER_CFG,
-			       TUNE_SD18_MASK, TUNE_SD18_1V8);
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	wait_timeout(chip->option.D3318_off_delay);
-
-	RTS51X_WRITE_REG(chip, SD_BUS_STAT, 0xFF, SD_CLK_TOGGLE_EN);
-	wait_timeout(10);
-
-	RTS51X_READ_REG(chip, SD_BUS_STAT, &stat);
-	if ((stat & (SD_CMD_STATUS | SD_DAT3_STATUS | SD_DAT2_STATUS |
-		     SD_DAT1_STATUS | SD_DAT0_STATUS)) !=
-	    (SD_CMD_STATUS | SD_DAT3_STATUS | SD_DAT2_STATUS |
-	     SD_DAT1_STATUS | SD_DAT0_STATUS)) {
-		rts51x_init_cmd(chip);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BUS_STAT, 0xFF,
-			       SD_CLK_FORCE_STOP);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_CLK_EN, 0xFF, 0);
-		rts51x_send_cmd(chip, MODE_C, 100);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-	RTS51X_WRITE_REG(chip, SD_BUS_STAT,
-			 SD_CLK_TOGGLE_EN | SD_CLK_FORCE_STOP, 0);
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_change_phase(struct rts51x_chip *chip, u8 sample_point,
-			   u8 tune_dir)
-{
-	u16 SD_VP_CTL, SD_DCMPS_CTL;
-	u8 val;
-	int retval;
-
-	RTS51X_DEBUGP("sd_change_phase (sample_point = %d, tune_dir = %d)\n",
-		       sample_point, tune_dir);
-
-	if (tune_dir == TUNE_RX) {
-		SD_VP_CTL = SD_VPCLK1_CTL;
-		SD_DCMPS_CTL = SD_DCMPS1_CTL;
-	} else {
-		SD_VP_CTL = SD_VPCLK0_CTL;
-		SD_DCMPS_CTL = SD_DCMPS0_CTL;
-	}
-
-	if (chip->asic_code) {
-		RTS51X_WRITE_REG(chip, CLK_DIV, CLK_CHANGE, CLK_CHANGE);
-		RTS51X_WRITE_REG(chip, SD_VP_CTL, 0x1F, sample_point);
-		RTS51X_WRITE_REG(chip, SD_VPCLK0_CTL, PHASE_NOT_RESET, 0);
-		RTS51X_WRITE_REG(chip, SD_VPCLK0_CTL, PHASE_NOT_RESET,
-				 PHASE_NOT_RESET);
-		RTS51X_WRITE_REG(chip, CLK_DIV, CLK_CHANGE, 0);
-	} else {
-#ifdef CONFIG_RTS5139_DEBUG
-		RTS51X_READ_REG(chip, SD_VP_CTL, &val);
-		RTS51X_DEBUGP("SD_VP_CTL: 0x%x\n", val);
-		RTS51X_READ_REG(chip, SD_DCMPS_CTL, &val);
-		RTS51X_DEBUGP("SD_DCMPS_CTL: 0x%x\n", val);
-#endif
-
-		RTS51X_WRITE_REG(chip, CLK_DIV, CLK_CHANGE, CLK_CHANGE);
-		udelay(100);
-		RTS51X_WRITE_REG(chip, SD_VP_CTL, 0xFF,
-				 PHASE_NOT_RESET | sample_point);
-		udelay(200);
-
-		rts51x_init_cmd(chip);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_DCMPS_CTL, DCMPS_CHANGE,
-			       DCMPS_CHANGE);
-		rts51x_add_cmd(chip, CHECK_REG_CMD, SD_DCMPS_CTL,
-			       DCMPS_CHANGE_DONE, DCMPS_CHANGE_DONE);
-		retval = rts51x_send_cmd(chip, MODE_CR, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, Fail);
-
-		retval = rts51x_get_rsp(chip, 1, 500);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, Fail);
-
-		val = chip->rsp_buf[0];
-		if (val & DCMPS_ERROR)
-			TRACE_GOTO(chip, Fail);
-		if ((val & DCMPS_CURRENT_PHASE) != sample_point)
-			TRACE_GOTO(chip, Fail);
-		RTS51X_WRITE_REG(chip, SD_DCMPS_CTL, DCMPS_CHANGE, 0);
-		RTS51X_WRITE_REG(chip, CLK_DIV, CLK_CHANGE, 0);
-		udelay(100);
-	}
-
-	RTS51X_WRITE_REG(chip, SD_CFG1, SD_ASYNC_FIFO_RST, 0);
-
-	return STATUS_SUCCESS;
-
-Fail:
-#ifdef CONFIG_RTS5139_DEBUG
-	rts51x_ep0_read_register(chip, SD_VP_CTL, &val);
-	RTS51X_DEBUGP("SD_VP_CTL: 0x%x\n", val);
-	rts51x_ep0_read_register(chip, SD_DCMPS_CTL, &val);
-	RTS51X_DEBUGP("SD_DCMPS_CTL: 0x%x\n", val);
-#endif
-
-	RTS51X_WRITE_REG(chip, SD_DCMPS_CTL, DCMPS_CHANGE, 0);
-	RTS51X_WRITE_REG(chip, SD_VP_CTL, PHASE_CHANGE, 0);
-	wait_timeout(10);
-
-	return STATUS_FAIL;
-}
-
-static int sd_check_spec(struct rts51x_chip *chip, u8 bus_width)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	u8 cmd[5], buf[8];
-
-	retval =
-	    sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr, SD_RSP_TYPE_R1,
-				NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	cmd[0] = 0x40 | SEND_SCR;
-	cmd[1] = 0;
-	cmd[2] = 0;
-	cmd[3] = 0;
-	cmd[4] = 0;
-
-	retval =
-	    sd_read_data(chip, SD_TM_NORMAL_READ, cmd, 5, 8, 1, bus_width, buf,
-			 8, 250);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_sd_error(chip);
-		TRACE_RET(chip, retval);
-	}
-
-	memcpy(sd_card->raw_scr, buf, 8);
-
-	if ((buf[0] & 0x0F) == 0)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_query_switch_result(struct rts51x_chip *chip, u8 func_group,
-				  u8 func_to_switch, u8 *buf, int buf_len)
-{
-	u8 support_mask = 0, query_switch = 0, switch_busy = 0;
-	int support_offset = 0, query_switch_offset = 0, check_busy_offset = 0;
-
-	if (func_group == SD_FUNC_GROUP_1) {
-		support_offset = FUNCTION_GROUP1_SUPPORT_OFFSET;
-		query_switch_offset = FUNCTION_GROUP1_QUERY_SWITCH_OFFSET;
-		check_busy_offset = FUNCTION_GROUP1_CHECK_BUSY_OFFSET;
-
-		switch (func_to_switch) {
-		case HS_SUPPORT:
-			support_mask = HS_SUPPORT_MASK;
-			query_switch = HS_QUERY_SWITCH_OK;
-			switch_busy = HS_SWITCH_BUSY;
-			break;
-
-		case SDR50_SUPPORT:
-			support_mask = SDR50_SUPPORT_MASK;
-			query_switch = SDR50_QUERY_SWITCH_OK;
-			switch_busy = SDR50_SWITCH_BUSY;
-			break;
-
-		case SDR104_SUPPORT:
-			support_mask = SDR104_SUPPORT_MASK;
-			query_switch = SDR104_QUERY_SWITCH_OK;
-			switch_busy = SDR104_SWITCH_BUSY;
-			break;
-
-		case DDR50_SUPPORT:
-			support_mask = DDR50_SUPPORT_MASK;
-			query_switch = DDR50_QUERY_SWITCH_OK;
-			switch_busy = DDR50_SWITCH_BUSY;
-			break;
-
-		default:
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	} else if (func_group == SD_FUNC_GROUP_3) {
-		support_offset = FUNCTION_GROUP3_SUPPORT_OFFSET;
-		query_switch_offset = FUNCTION_GROUP3_QUERY_SWITCH_OFFSET;
-		check_busy_offset = FUNCTION_GROUP3_CHECK_BUSY_OFFSET;
-
-		switch (func_to_switch) {
-		case DRIVING_TYPE_A:
-			support_mask = DRIVING_TYPE_A_MASK;
-			query_switch = TYPE_A_QUERY_SWITCH_OK;
-			switch_busy = TYPE_A_SWITCH_BUSY;
-			break;
-
-		case DRIVING_TYPE_C:
-			support_mask = DRIVING_TYPE_C_MASK;
-			query_switch = TYPE_C_QUERY_SWITCH_OK;
-			switch_busy = TYPE_C_SWITCH_BUSY;
-			break;
-
-		case DRIVING_TYPE_D:
-			support_mask = DRIVING_TYPE_D_MASK;
-			query_switch = TYPE_D_QUERY_SWITCH_OK;
-			switch_busy = TYPE_D_SWITCH_BUSY;
-			break;
-
-		default:
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	} else if (func_group == SD_FUNC_GROUP_4) {
-		support_offset = FUNCTION_GROUP4_SUPPORT_OFFSET;
-		query_switch_offset = FUNCTION_GROUP4_QUERY_SWITCH_OFFSET;
-		check_busy_offset = FUNCTION_GROUP4_CHECK_BUSY_OFFSET;
-
-		switch (func_to_switch) {
-		case CURRENT_LIMIT_400:
-			support_mask = CURRENT_LIMIT_400_MASK;
-			query_switch = CURRENT_LIMIT_400_QUERY_SWITCH_OK;
-			switch_busy = CURRENT_LIMIT_400_SWITCH_BUSY;
-			break;
-
-		case CURRENT_LIMIT_600:
-			support_mask = CURRENT_LIMIT_600_MASK;
-			query_switch = CURRENT_LIMIT_600_QUERY_SWITCH_OK;
-			switch_busy = CURRENT_LIMIT_600_SWITCH_BUSY;
-			break;
-
-		case CURRENT_LIMIT_800:
-			support_mask = CURRENT_LIMIT_800_MASK;
-			query_switch = CURRENT_LIMIT_800_QUERY_SWITCH_OK;
-			switch_busy = CURRENT_LIMIT_800_SWITCH_BUSY;
-			break;
-
-		default:
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	} else {
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (func_group == SD_FUNC_GROUP_4)
-		buf[query_switch_offset] =
-		    (buf[query_switch_offset] & 0xf0) >> 4;
-	if (!(buf[support_offset] & support_mask) ||
-	    ((buf[query_switch_offset] & 0x0F) != query_switch))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	if ((buf[DATA_STRUCTURE_VER_OFFSET] == 0x01) &&
-	    ((buf[check_busy_offset] & switch_busy) == switch_busy))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_check_switch_mode(struct rts51x_chip *chip, u8 mode,
-				u8 func_group, u8 func_to_switch, u8 bus_width)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	u8 cmd[5], buf[64];
-
-	RTS51X_DEBUGP("sd_check_switch_mode (mode = %d, func_group = %d,"
-		"func_to_switch = %d)\n", mode, func_group, func_to_switch);
-
-	cmd[0] = 0x40 | SWITCH;
-	cmd[1] = mode;
-
-	if (func_group == SD_FUNC_GROUP_1) {
-		cmd[2] = 0xFF;
-		cmd[3] = 0xFF;
-		cmd[4] = 0xF0 + func_to_switch;
-	} else if (func_group == SD_FUNC_GROUP_3) {
-		cmd[2] = 0xFF;
-		cmd[3] = 0xF0 + func_to_switch;
-		cmd[4] = 0xFF;
-	} else if (func_group == SD_FUNC_GROUP_4) {
-		cmd[2] = 0xFF;
-		cmd[3] = 0x0F + (func_to_switch << 4);
-		cmd[4] = 0xFF;
-	} else {
-		cmd[1] = SD_CHECK_MODE;
-		cmd[2] = 0xFF;
-		cmd[3] = 0xFF;
-		cmd[4] = 0xFF;
-	}
-
-	retval =
-	    sd_read_data(chip, SD_TM_NORMAL_READ, cmd, 5, 64, 1, bus_width, buf,
-			 64, 250);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_sd_error(chip);
-		TRACE_RET(chip, retval);
-	}
-
-	if (func_group == NO_ARGUMENT) {
-		sd_card->func_group1_mask = buf[0x0D];
-		sd_card->func_group2_mask = buf[0x0B];
-		sd_card->func_group3_mask = buf[0x09];
-		sd_card->func_group4_mask = buf[0x07];
-
-		RTS51X_DEBUGP("func_group1_mask = 0x%02x\n", buf[0x0D]);
-		RTS51X_DEBUGP("func_group2_mask = 0x%02x\n", buf[0x0B]);
-		RTS51X_DEBUGP("func_group3_mask = 0x%02x\n", buf[0x09]);
-		RTS51X_DEBUGP("func_group4_mask = 0x%02x\n", buf[0x07]);
-	} else {
-		if ((buf[0] == 0) && (buf[1] == 0))
-			TRACE_RET(chip, STATUS_FAIL);
-		retval =
-		    sd_query_switch_result(chip, func_group, func_to_switch,
-					   buf, 64);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_check_switch(struct rts51x_chip *chip,
-			   u8 func_group, u8 func_to_switch, u8 bus_width)
-{
-	int retval;
-	int i;
-	int switch_good = 0;
-
-	for (i = 0; i < 3; i++) {
-		if (monitor_card_cd(chip, SD_CARD) == CD_NOT_EXIST) {
-			sd_set_reset_fail(chip, SD_RESET_FAIL);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		retval = sd_check_switch_mode(chip, SD_CHECK_MODE, func_group,
-					      func_to_switch, bus_width);
-		if (retval == STATUS_SUCCESS) {
-			u8 stat;
-
-			retval = sd_check_switch_mode(chip, SD_SWITCH_MODE,
-					func_group, func_to_switch, bus_width);
-			if (retval == STATUS_SUCCESS) {
-				switch_good = 1;
-				break;
-			}
-
-			RTS51X_READ_REG(chip, SD_STAT1, &stat);
-
-			if (stat & SD_CRC16_ERR) {
-				RTS51X_DEBUGP("SD CRC16 error when switching"
-							"mode\n");
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-
-		wait_timeout(20);
-	}
-
-	if (!switch_good)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_switch_function(struct rts51x_chip *chip, u8 bus_width)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	int i;
-	u8 func_to_switch = 0;
-
-	/* Get supported functions */
-	retval = sd_check_switch_mode(chip, SD_CHECK_MODE,
-				      NO_ARGUMENT, NO_ARGUMENT, bus_width);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	sd_card->func_group1_mask &= ~(sd_card->sd_switch_fail);
-
-	for (i = 0; i < 4; i++) {
-		switch ((u8) (chip->option.sd_speed_prior >> (i * 8))) {
-		case DDR50_SUPPORT:
-			if ((sd_card->func_group1_mask & DDR50_SUPPORT_MASK)
-			    && (CHECK_UHS50(chip)))
-				func_to_switch = DDR50_SUPPORT;
-			break;
-
-		case SDR50_SUPPORT:
-			if ((sd_card->func_group1_mask & SDR50_SUPPORT_MASK)
-			    && (CHECK_UHS50(chip)))
-				func_to_switch = SDR50_SUPPORT;
-			break;
-
-		case HS_SUPPORT:
-			if (sd_card->func_group1_mask & HS_SUPPORT_MASK)
-				func_to_switch = HS_SUPPORT;
-			break;
-
-		default:
-			continue;
-		}
-
-		if (func_to_switch)
-			break;
-	}
-	RTS51X_DEBUGP("SD_FUNC_GROUP_1: func_to_switch = 0x%02x",
-		       func_to_switch);
-
-	if (func_to_switch) {
-		retval =
-		    sd_check_switch(chip, SD_FUNC_GROUP_1, func_to_switch,
-				    bus_width);
-		if (retval != STATUS_SUCCESS) {
-			if (func_to_switch == SDR104_SUPPORT)
-				sd_card->sd_switch_fail = SDR104_SUPPORT_MASK;
-			else if (func_to_switch == DDR50_SUPPORT)
-				sd_card->sd_switch_fail = DDR50_SUPPORT_MASK;
-			else if (func_to_switch == SDR50_SUPPORT)
-				sd_card->sd_switch_fail = SDR50_SUPPORT_MASK;
-			else if (func_to_switch == HS_SUPPORT)
-				sd_card->sd_switch_fail = HS_SUPPORT_MASK;
-
-			TRACE_RET(chip, retval);
-		}
-
-		if (func_to_switch == SDR104_SUPPORT)
-			SET_SD_SDR104(sd_card);
-		else if (func_to_switch == DDR50_SUPPORT)
-			SET_SD_DDR50(sd_card);
-		else if (func_to_switch == SDR50_SUPPORT)
-			SET_SD_SDR50(sd_card);
-		else
-			SET_SD_HS(sd_card);
-	}
-
-	if (CHK_SD_DDR50(sd_card))
-		RTS51X_WRITE_REG(chip, SD_CFG1, 0x0C, SD_DDR_MODE);
-
-	func_to_switch = 0;
-	if (sd_card->func_group4_mask & CURRENT_LIMIT_400_MASK)
-		func_to_switch = CURRENT_LIMIT_400;
-
-	if (func_to_switch) {
-		RTS51X_DEBUGP("Try to switch current_limit_400\n");
-		retval =
-		    sd_check_switch(chip, SD_FUNC_GROUP_4, func_to_switch,
-				    bus_width);
-		RTS51X_DEBUGP("Switch current_limit_400 status: (%d)\n",
-			       retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_wait_data_idle(struct rts51x_chip *chip)
-{
-	int retval = STATUS_TIMEDOUT;
-	int i;
-	u8 val = 0;
-
-	for (i = 0; i < 100; i++) {
-		retval = rts51x_ep0_read_register(chip, SD_DATA_STATE, &val);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
-		if (val & SD_DATA_IDLE) {
-			retval = STATUS_SUCCESS;
-			break;
-		}
-		udelay(100);
-	}
-	RTS51X_DEBUGP("SD_DATA_STATE: 0x%02x\n", val);
-
-	return retval;
-}
-
-static int sd_sdr_tuning_rx_cmd(struct rts51x_chip *chip, u8 sample_point)
-{
-	int retval;
-	u8 cmd[5];
-
-	retval = sd_change_phase(chip, sample_point, TUNE_RX);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	cmd[0] = 0x40 | SEND_TUNING_PATTERN;
-	cmd[1] = 0;
-	cmd[2] = 0;
-	cmd[3] = 0;
-	cmd[4] = 0;
-
-	retval = sd_read_data(chip, SD_TM_AUTO_TUNING,
-			      cmd, 5, 0x40, 1, SD_BUS_WIDTH_4, NULL, 0, 100);
-	if (retval != STATUS_SUCCESS) {
-		/* Wait till SD DATA IDLE */
-		(void)sd_wait_data_idle(chip);
-
-		rts51x_clear_sd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_ddr_tuning_rx_cmd(struct rts51x_chip *chip, u8 sample_point)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	u8 cmd[5];
-
-	retval = sd_change_phase(chip, sample_point, TUNE_RX);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_DEBUGP("sd ddr tuning rx\n");
-
-	retval =
-	    sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr, SD_RSP_TYPE_R1,
-				NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	cmd[0] = 0x40 | SD_STATUS;
-	cmd[1] = 0;
-	cmd[2] = 0;
-	cmd[3] = 0;
-	cmd[4] = 0;
-
-	retval = sd_read_data(chip, SD_TM_NORMAL_READ,
-			      cmd, 5, 64, 1, SD_BUS_WIDTH_4, NULL, 0, 100);
-	if (retval != STATUS_SUCCESS) {
-		/* Wait till SD DATA IDLE */
-		(void)sd_wait_data_idle(chip);
-
-		rts51x_clear_sd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int mmc_ddr_tunning_rx_cmd(struct rts51x_chip *chip, u8 sample_point)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	u8 cmd[5], bus_width;
-
-	if (CHK_MMC_8BIT(sd_card))
-		bus_width = SD_BUS_WIDTH_8;
-	else if (CHK_MMC_4BIT(sd_card))
-		bus_width = SD_BUS_WIDTH_4;
-	else
-		bus_width = SD_BUS_WIDTH_1;
-
-	retval = sd_change_phase(chip, sample_point, TUNE_RX);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_DEBUGP("mmc ddr tuning rx\n");
-
-	cmd[0] = 0x40 | SEND_EXT_CSD;
-	cmd[1] = 0;
-	cmd[2] = 0;
-	cmd[3] = 0;
-	cmd[4] = 0;
-
-	retval = sd_read_data(chip, SD_TM_NORMAL_READ,
-			      cmd, 5, 0x200, 1, bus_width, NULL, 0, 100);
-	if (retval != STATUS_SUCCESS) {
-		/* Wait till SD DATA IDLE */
-		(void)sd_wait_data_idle(chip);
-
-		rts51x_clear_sd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_sdr_tuning_tx_cmd(struct rts51x_chip *chip, u8 sample_point)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-
-	retval = sd_change_phase(chip, sample_point, TUNE_TX);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
-			 SD_RSP_80CLK_TIMEOUT_EN);
-
-	retval = sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
-				     SD_RSP_TYPE_R1, NULL, 0);
-	if (retval != STATUS_SUCCESS) {
-		if (sd_check_err_code(chip, SD_RSP_TIMEOUT)) {
-			/* Tunning TX fail */
-			rts51x_ep0_write_register(chip, SD_CFG3,
-						  SD_RSP_80CLK_TIMEOUT_EN, 0);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-
-	RTS51X_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN, 0);
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_ddr_tuning_tx_cmd(struct rts51x_chip *chip, u8 sample_point)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	u8 cmd[5], bus_width;
-
-	retval = sd_change_phase(chip, sample_point, TUNE_TX);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (CHK_SD(sd_card)) {
-		bus_width = SD_BUS_WIDTH_4;
-	} else {
-		if (CHK_MMC_8BIT(sd_card))
-			bus_width = SD_BUS_WIDTH_8;
-		else if (CHK_MMC_4BIT(sd_card))
-			bus_width = SD_BUS_WIDTH_4;
-		else
-			bus_width = SD_BUS_WIDTH_1;
-	}
-	retval = sd_wait_currentstate_dataready(chip, 0x08, 1, 20);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	RTS51X_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
-			 SD_RSP_80CLK_TIMEOUT_EN);
-
-	cmd[0] = 0x40 | PROGRAM_CSD;
-	cmd[1] = 0;
-	cmd[2] = 0;
-	cmd[3] = 0;
-	cmd[4] = 0;
-
-	retval = sd_write_data(chip, SD_TM_AUTO_WRITE_2,
-			cmd, 5, 16, 1, bus_width, sd_card->raw_csd, 16, 100);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_sd_error(chip);
-		/* Tunning TX fail */
-		rts51x_ep0_write_register(chip, SD_CFG3,
-					  SD_RSP_80CLK_TIMEOUT_EN, 0);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	RTS51X_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN, 0);
-
-	sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr, SD_RSP_TYPE_R1,
-			    NULL, 0);
-
-	return STATUS_SUCCESS;
-}
-
-static u8 sd_search_final_phase(struct rts51x_chip *chip, u32 phase_map,
-				u8 tune_dir)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	struct timing_phase_path path[MAX_PHASE + 1];
-	int i, j, cont_path_cnt;
-	int new_block, max_len;
-	u8 final_phase = 0xFF;
-	int final_path_idx;
-
-	if (phase_map == 0xffff) {
-		if (CHK_SD_DDR50(sd_card)) {
-			if (tune_dir == TUNE_TX)
-				final_phase = chip->option.ddr50_tx_phase;
-			else
-				final_phase = chip->option.ddr50_rx_phase;
-			RTS51X_DEBUGP("DDR50 tuning dir:%d all pass,"
-					"so select default phase:0x%x.\n",
-					tune_dir, final_phase);
-		} else {
-			if (tune_dir == TUNE_TX)
-				final_phase = chip->option.sdr50_tx_phase;
-			else
-				final_phase = chip->option.sdr50_rx_phase;
-			RTS51X_DEBUGP("SDR50 tuning dir:%d all pass,"
-					"so select default phase:0x%x.\n",
-					tune_dir, final_phase);
-		}
-		goto Search_Finish;
-	}
-
-	cont_path_cnt = 0;
-	new_block = 1;
-	j = 0;
-	for (i = 0; i < MAX_PHASE + 1; i++) {
-		if (phase_map & (1 << i)) {
-			if (new_block) {
-				new_block = 0;
-				j = cont_path_cnt++;
-				path[j].start = i;
-				path[j].end = i;
-			} else {
-				path[j].end = i;
-			}
-		} else {
-			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 + path[idx].len / 2;
-			}
-		}
-	}
-
-	if (cont_path_cnt == 0) {
-		RTS51X_DEBUGP("No continuous phase path\n");
-		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;
-	}
-
-	if ((path[0].start == 0) &&
-			(path[cont_path_cnt - 1].end == MAX_PHASE)) {
-		path[0].start = path[cont_path_cnt - 1].start - MAX_PHASE - 1;
-		path[0].len += path[cont_path_cnt - 1].len;
-		path[0].mid = path[0].start + path[0].len / 2;
-		if (path[0].mid < 0)
-			path[0].mid += MAX_PHASE + 1;
-		cont_path_cnt--;
-	}
-	max_len = 0;
-	final_phase = 0;
-	final_path_idx = 0;
-	for (i = 0; i < cont_path_cnt; i++) {
-		if (path[i].len > max_len) {
-			max_len = path[i].len;
-			final_phase = (u8) path[i].mid;
-			final_path_idx = i;
-		}
-
-		RTS51X_DEBUGP("path[%d].start = %d\n", i, path[i].start);
-		RTS51X_DEBUGP("path[%d].end = %d\n", i, path[i].end);
-		RTS51X_DEBUGP("path[%d].len = %d\n", i, path[i].len);
-		RTS51X_DEBUGP("path[%d].mid = %d\n", i, path[i].mid);
-		RTS51X_DEBUGP("\n");
-	}
-
-	if ((tune_dir == TUNE_TX) && (CHK_SD_SDR50(sd_card))
-	    && chip->option.sdr50_phase_sel) {
-		if (max_len > 6) {
-			int temp_mid = (max_len - 6) / 2;
-			int temp_final_phase =
-			    path[final_path_idx].end - (max_len -
-							(3 + temp_mid));
-
-			if (temp_final_phase < 0)
-				final_phase = temp_final_phase + MAX_PHASE + 1;
-			else
-				final_phase = (u8) temp_final_phase;
-		}
-	}
-
-Search_Finish:
-	RTS51X_DEBUGP("Final chosen phase: %d\n", final_phase);
-	return final_phase;
-}
-
-static int sd_tuning_rx(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	int i, j;
-	u32 raw_phase_map[3], phase_map;
-	u8 final_phase;
-	int (*tuning_cmd)(struct rts51x_chip *chip, u8 sample_point);
-
-	if (CHK_SD(sd_card)) {
-		if (CHK_SD_DDR50(sd_card))
-			tuning_cmd = sd_ddr_tuning_rx_cmd;
-		else
-			tuning_cmd = sd_sdr_tuning_rx_cmd;
-	} else {
-		if (CHK_MMC_DDR52(sd_card))
-			tuning_cmd = mmc_ddr_tunning_rx_cmd;
-		else
-			TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	for (i = 0; i < 3; i++) {
-		raw_phase_map[i] = 0;
-		for (j = MAX_PHASE; j >= 0; j--) {
-			if (monitor_card_cd(chip, SD_CARD) == CD_NOT_EXIST) {
-				sd_set_reset_fail(chip, SD_RESET_FAIL);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-
-			retval = tuning_cmd(chip, (u8) j);
-			if (retval == STATUS_SUCCESS)
-				raw_phase_map[i] |= 1 << j;
-			else
-				RTS51X_DEBUGP("Tuning phase %d fail\n", j);
-		}
-	}
-
-	phase_map = raw_phase_map[0] & raw_phase_map[1] & raw_phase_map[2];
-	for (i = 0; i < 3; i++)
-		RTS51X_DEBUGP("RX raw_phase_map[%d] = 0x%04x\n", i,
-			       raw_phase_map[i]);
-	RTS51X_DEBUGP("RX phase_map = 0x%04x\n", phase_map);
-
-	final_phase = sd_search_final_phase(chip, phase_map, TUNE_RX);
-	if (final_phase == 0xFF)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	retval = tuning_cmd(chip, final_phase);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_ddr_pre_tuning_tx(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	u8 i;
-	u8 pre_tune_tx_phase;
-	u32 pre_tune_phase_map;
-
-	RTS51X_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
-			 SD_RSP_80CLK_TIMEOUT_EN);
-
-	pre_tune_tx_phase = 0xFF;
-	pre_tune_phase_map = 0x0000;
-	for (i = 0; i < MAX_PHASE + 1; i++) {
-		if (monitor_card_cd(chip, SD_CARD) == CD_NOT_EXIST) {
-			sd_set_reset_fail(chip, SD_RESET_FAIL);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		retval = sd_change_phase(chip, (u8) i, TUNE_TX);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval =
-		    sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
-					SD_RSP_TYPE_R1, NULL, 0);
-		if ((retval == STATUS_SUCCESS)
-		    || !sd_check_err_code(chip, SD_RSP_TIMEOUT))
-			pre_tune_phase_map |= (u32) 1 << i;
-	}
-
-	RTS51X_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN, 0);
-
-	pre_tune_tx_phase =
-	    sd_search_final_phase(chip, pre_tune_phase_map, TUNE_TX);
-	if (pre_tune_tx_phase == 0xFF)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	sd_change_phase(chip, pre_tune_tx_phase, TUNE_TX);
-	RTS51X_DEBUGP("DDR TX pre tune phase: %d\n", (int)pre_tune_tx_phase);
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_tuning_tx(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	int i, j;
-	u32 raw_phase_map[3], phase_map;
-	u8 final_phase;
-	int (*tuning_cmd)(struct rts51x_chip *chip, u8 sample_point);
-
-	if (CHK_SD(sd_card)) {
-		if (CHK_SD_DDR50(sd_card))
-			tuning_cmd = sd_ddr_tuning_tx_cmd;
-		else
-			tuning_cmd = sd_sdr_tuning_tx_cmd;
-	} else {
-		if (CHK_MMC_DDR52(sd_card))
-			tuning_cmd = sd_ddr_tuning_tx_cmd;
-		else
-			TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	for (i = 0; i < 3; i++) {
-		raw_phase_map[i] = 0;
-		for (j = MAX_PHASE; j >= 0; j--) {
-			if (monitor_card_cd(chip, SD_CARD) == CD_NOT_EXIST) {
-				sd_set_reset_fail(chip, SD_RESET_FAIL);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-
-			retval = tuning_cmd(chip, (u8) j);
-			if (retval == STATUS_SUCCESS)
-				raw_phase_map[i] |= 1 << j;
-			else
-				RTS51X_DEBUGP("Tuning phase %d fail\n", j);
-		}
-	}
-
-	phase_map = raw_phase_map[0] & raw_phase_map[1] & raw_phase_map[2];
-	for (i = 0; i < 3; i++)
-		RTS51X_DEBUGP("TX raw_phase_map[%d] = 0x%04x\n", i,
-			       raw_phase_map[i]);
-	RTS51X_DEBUGP("TX phase_map = 0x%04x\n", phase_map);
-
-	final_phase = sd_search_final_phase(chip, phase_map, TUNE_TX);
-	if (final_phase == 0xFF)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	retval = tuning_cmd(chip, final_phase);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_sdr_tuning(struct rts51x_chip *chip)
-{
-	int retval;
-
-	retval = sd_tuning_tx(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = sd_tuning_rx(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_ddr_tuning(struct rts51x_chip *chip)
-{
-	int retval;
-
-	if (!(chip->option.sd_ctl & SD_DDR_TX_PHASE_SET_BY_USER)) {
-		retval = sd_ddr_pre_tuning_tx(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	} else {
-		retval =
-		    sd_change_phase(chip, (u8) chip->option.sd_ddr_tx_phase,
-				    TUNE_TX);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	retval = sd_tuning_rx(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (!(chip->option.sd_ctl & SD_DDR_TX_PHASE_SET_BY_USER)) {
-		retval = sd_tuning_tx(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int mmc_ddr_tuning(struct rts51x_chip *chip)
-{
-	int retval;
-
-	if (!(chip->option.sd_ctl & MMC_DDR_TX_PHASE_SET_BY_USER)) {
-		retval = sd_ddr_pre_tuning_tx(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	} else {
-		retval =
-		    sd_change_phase(chip, (u8) chip->option.mmc_ddr_tx_phase,
-				    TUNE_TX);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	retval = sd_tuning_rx(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (!(chip->option.sd_ctl & MMC_DDR_TX_PHASE_SET_BY_USER)) {
-		retval = sd_tuning_tx(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_sd_switch_clock(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	int re_tuning = 0;
-
-	retval = rts51x_select_card(chip, SD_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (CHK_SD30_SPEED(sd_card) || CHK_MMC_DDR52(sd_card)) {
-		if (sd_card->sd_clock != chip->cur_clk)
-			re_tuning = 1;
-	}
-
-	retval = switch_clock(chip, sd_card->sd_clock);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (re_tuning) {
-		if (CHK_SD(sd_card)) {
-			if (CHK_SD_DDR50(sd_card))
-				retval = sd_ddr_tuning(chip);
-			else
-				retval = sd_sdr_tuning(chip);
-		} else {
-			if (CHK_MMC_DDR52(sd_card))
-				retval = mmc_ddr_tuning(chip);
-		}
-
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_prepare_reset(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-
-	if (chip->asic_code)
-		sd_card->sd_clock = 29;
-	else
-		sd_card->sd_clock = CLK_30;
-
-	/* Set SD Clocks */
-	retval = sd_set_init_para(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0xFF,
-		       SD_CLK_DIVIDE_128 | SD_20_MODE | SD_BUS_WIDTH_1);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_SAMPLE_POINT_CTL, 0xFF,
-		       SD20_RX_POS_EDGE);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_PUSH_POINT_CTL, 0xFF, 0);
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_select_card(chip, SD_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static void sd_pull_ctl_disable(struct rts51x_chip *chip)
-{
-	if (CHECK_PKG(chip, LQFP48)) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0xA5);
-	} else {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x65);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x56);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0x59);
-	}
-}
-
-static void sd_pull_ctl_enable(struct rts51x_chip *chip)
-{
-	if (CHECK_PKG(chip, LQFP48)) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0xAA);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0xAA);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0xA9);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0xA5);
-	} else {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0xA5);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x9A);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0xA5);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x9A);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x65);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0x5A);
-	}
-}
-
-static int sd_init_power(struct rts51x_chip *chip)
-{
-	int retval;
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, LDO3318_PWR_MASK,
-		       LDO_ON);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_PAD_CTL, SD_IO_USING_1V8,
-		       SD_IO_USING_3V3);
-	if (chip->asic_code)
-		rts51x_add_cmd(chip, WRITE_REG_CMD, LDO_POWER_CFG,
-			       TUNE_SD18_MASK, TUNE_SD18_3V3);
-	if (chip->asic_code)
-		sd_pull_ctl_disable(chip);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, FPGA_PULL_CTL,
-			       FPGA_SD_PULL_CTL_BIT | 0x20,
-			       FPGA_SD_PULL_CTL_BIT);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_OE, SD_OUTPUT_EN, 0);
-	if (!chip->option.FT2_fast_mode)
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, POWER_MASK,
-			       POWER_OFF);
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	if (!chip->option.FT2_fast_mode) {
-#ifdef SD_XD_IO_FOLLOW_PWR
-		if (CHECK_PKG(chip, LQFP48)
-		    || chip->option.rts5129_D3318_off_enable)
-			rts51x_write_register(chip, CARD_PWR_CTL,
-					LDO_OFF, LDO_OFF);
-#endif
-		wait_timeout(250);
-
-#ifdef SD_XD_IO_FOLLOW_PWR
-		if (CHECK_PKG(chip, LQFP48)
-		    || chip->option.rts5129_D3318_off_enable) {
-			rts51x_init_cmd(chip);
-			if (chip->asic_code)
-				sd_pull_ctl_enable(chip);
-			else
-				rts51x_add_cmd(chip, WRITE_REG_CMD,
-					       FPGA_PULL_CTL,
-					       FPGA_SD_PULL_CTL_BIT | 0x20, 0);
-			retval = rts51x_send_cmd(chip, MODE_C, 100);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-		} else {
-			if (chip->asic_code)
-				rts51x_write_register(chip, CARD_PULL_CTL6,
-						      0x03, 0x00);
-		}
-#endif
-
-		/* Power on card */
-		retval = rts51x_card_power_on(chip, SD_CARD);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		wait_timeout(260);
-
-#ifdef SUPPORT_OCP
-		rts51x_get_card_status(chip, &(chip->card_status));
-		chip->ocp_stat = (chip->card_status >> 4) & 0x03;
-
-		if (chip->ocp_stat & (MS_OCP_NOW | MS_OCP_EVER)) {
-			RTS51X_DEBUGP("Over current, OCPSTAT is 0x%x\n",
-				       chip->ocp_stat);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-#endif
-	}
-
-	rts51x_init_cmd(chip);
-	if (chip->asic_code) {
-		sd_pull_ctl_enable(chip);
-	} else {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, FPGA_PULL_CTL,
-			       FPGA_SD_PULL_CTL_BIT | 0x20, 0);
-	}
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-#ifdef SD_XD_IO_FOLLOW_PWR
-	rts51x_write_register(chip, CARD_INT_PEND, XD_INT | MS_INT | SD_INT,
-			      XD_INT | MS_INT | SD_INT);
-#endif
-
-	RTS51X_WRITE_REG(chip, CARD_OE, SD_OUTPUT_EN, SD_OUTPUT_EN);
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_dummy_clock(struct rts51x_chip *chip)
-{
-	RTS51X_WRITE_REG(chip, SD_BUS_STAT, SD_CLK_TOGGLE_EN, SD_CLK_TOGGLE_EN);
-	wait_timeout(5);
-	RTS51X_WRITE_REG(chip, SD_BUS_STAT, SD_CLK_TOGGLE_EN, 0x00);
-
-	return STATUS_SUCCESS;
-}
-
-int reset_sd(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval, i = 0, j = 0, k = 0, hi_cap_flow = 0;
-	int sd_dont_switch = 0;
-	int support_1v8 = 0;
-	u8 rsp[16];
-	u8 switch_bus_width;
-	u32 voltage = 0;
-	u8 cmd[5], buf[64];
-	u16 sd_card_type;
-
-	SET_SD(sd_card);
-	CLR_RETRY_SD20_MODE(sd_card);
-Switch_Fail:
-	i = 0;
-	j = 0;
-	k = 0;
-	hi_cap_flow = 0;
-	support_1v8 = 0;
-
-	retval = sd_prepare_reset(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	sd_dummy_clock(chip);
-
-	/* Start Initialization Process of SD Card */
-RTY_SD_RST:
-	retval =
-	    sd_send_cmd_get_rsp(chip, GO_IDLE_STATE, 0, SD_RSP_TYPE_R0, NULL,
-				0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	wait_timeout(20);
-
-	retval =
-	    sd_send_cmd_get_rsp(chip, SEND_IF_COND, 0x000001AA, SD_RSP_TYPE_R7,
-				rsp, 5);
-	if (retval == STATUS_SUCCESS) {
-		if ((rsp[4] == 0xAA) && ((rsp[3] & 0x0f) == 0x01)) {
-			hi_cap_flow = 1;
-			if (CHK_RETRY_SD20_MODE(sd_card)) {
-				voltage =
-				    SUPPORT_VOLTAGE |
-				    SUPPORT_HIGH_AND_EXTENDED_CAPACITY;
-			} else {
-				voltage =
-				    SUPPORT_VOLTAGE |
-				    SUPPORT_HIGH_AND_EXTENDED_CAPACITY |
-				    SUPPORT_MAX_POWER_PERMANCE | SUPPORT_1V8;
-			}
-		}
-	}
-
-	if (!hi_cap_flow) {
-		voltage = SUPPORT_VOLTAGE;
-
-		retval =
-		    sd_send_cmd_get_rsp(chip, GO_IDLE_STATE, 0, SD_RSP_TYPE_R0,
-					NULL, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		wait_timeout(20);
-	}
-
-	/* ACMD41 */
-	do {
-		{
-			u8 temp = 0;
-			rts51x_read_register(chip, CARD_INT_PEND, &temp);
-			RTS51X_DEBUGP("CARD_INT_PEND:%x\n", temp);
-			if (temp & SD_INT) {
-				chip->reset_need_retry = 1;
-				rts51x_write_register(chip, CARD_INT_PEND,
-						      XD_INT | SD_INT | MS_INT,
-						      XD_INT | SD_INT | MS_INT);
-				sd_set_reset_fail(chip, SD_RESET_FAIL);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-
-RTY_CMD55:
-		retval =
-		    sd_send_cmd_get_rsp(chip, APP_CMD, 0, SD_RSP_TYPE_R1, NULL,
-					0);
-		if (retval != STATUS_SUCCESS) {
-			if (monitor_card_cd(chip, SD_CARD) == CD_NOT_EXIST) {
-				sd_set_reset_fail(chip, SD_RESET_FAIL);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-
-			j++;
-			if (chip->option.speed_mmc) {
-				if (j < 2)
-					goto RTY_CMD55;
-				else
-					TRACE_RET(chip, STATUS_FAIL);
-			} else {
-				if (j < 3)
-					goto RTY_SD_RST;
-				else
-					TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-
-		retval =
-		    sd_send_cmd_get_rsp(chip, SD_APP_OP_COND, voltage,
-					SD_RSP_TYPE_R3, rsp, 5);
-		if (retval != STATUS_SUCCESS) {
-			k++;
-			if (k < 3)
-				goto RTY_SD_RST;
-			else
-				TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		i++;
-		wait_timeout(20);
-	} while (!(rsp[1] & 0x80) && (i < 255)); /* Not complete power on */
-
-	if (i == 255) {
-		/* Time out */
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (hi_cap_flow) {
-		if (rsp[1] & 0x40)
-			SET_SD_HCXC(sd_card);
-		else
-			CLR_SD_HCXC(sd_card);
-		if (!CHK_RETRY_SD20_MODE(sd_card)) {
-			if ((CHK_SD_HCXC(sd_card)) && (CHECK_UHS50(chip))) {
-				support_1v8 = (rsp[1] & 0x01) ? 1 : 0;
-				RTS51X_DEBUGP("support_1v8 = %d\n",
-					       support_1v8);
-			}
-		}
-	} else {
-		CLR_SD_HCXC(sd_card);
-		support_1v8 = 0;
-	}
-
-	/* CMD11: Switch Voltage */
-	if (support_1v8 && CHECK_UHS50(chip)
-	    && !(((u8) chip->option.sd_speed_prior & SDR104_SUPPORT) ==
-		 HS_SUPPORT)) {
-		retval = sd_voltage_switch(chip);
-		if (retval != STATUS_SUCCESS) {
-			SET_RETRY_SD20_MODE(sd_card);
-			sd_init_power(chip);
-			RTS51X_DEBUGP("1.8v switch fail\n");
-			goto Switch_Fail;
-		}
-	}
-
-	/* CMD 2 */
-	retval =
-	    sd_send_cmd_get_rsp(chip, ALL_SEND_CID, 0, SD_RSP_TYPE_R2, NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	/* CMD 3 */
-	retval =
-	    sd_send_cmd_get_rsp(chip, SEND_RELATIVE_ADDR, 0, SD_RSP_TYPE_R6,
-				rsp, 5);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	sd_card->sd_addr = (u32) rsp[1] << 24;
-	sd_card->sd_addr += (u32) rsp[2] << 16;
-
-	/* Get CSD register for Calculating Timing,Capacity,
-	 * Check CSD to determine as if this is the SD ROM card */
-	retval = sd_check_csd(chip, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	/* Select SD card */
-	retval = rts51x_sd_select_card(chip, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	/* ACMD42 */
-	retval =
-	    sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr, SD_RSP_TYPE_R1,
-				NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval =
-	    sd_send_cmd_get_rsp(chip, SET_CLR_CARD_DETECT, 0, SD_RSP_TYPE_R1,
-				NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (support_1v8) {
-		/* ACMD6 */
-		retval =
-		    sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
-					SD_RSP_TYPE_R1, NULL, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		/* Enable 4 bit data bus */
-		retval =
-		    sd_send_cmd_get_rsp(chip, SET_BUS_WIDTH, 2, SD_RSP_TYPE_R1,
-					NULL, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		switch_bus_width = SD_BUS_WIDTH_4;
-	} else {
-		switch_bus_width = SD_BUS_WIDTH_1;
-	}
-
-	/* Set block length 512 bytes for all block commands */
-	retval = sd_send_cmd_get_rsp(chip, SET_BLOCKLEN,
-			0x200, SD_RSP_TYPE_R1, NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_WRITE_REG(chip, SD_CFG1, SD_CLK_DIVIDE_MASK, SD_CLK_DIVIDE_0);
-
-	if (!(sd_card->raw_csd[4] & 0x40)) {
-		sd_dont_switch = 1;
-		RTS51X_DEBUGP("Not support class ten\n");
-	}
-
-	if (!sd_dont_switch) {
-		/* Check the card whether flow SD1.1 spec or higher */
-		retval = sd_check_spec(chip, switch_bus_width);
-		if (retval == STATUS_SUCCESS) {
-			retval = sd_switch_function(chip, switch_bus_width);
-			if (retval != STATUS_SUCCESS) {
-				if ((sd_card->sd_switch_fail ==
-				     SDR104_SUPPORT_MASK)
-				    || (sd_card->sd_switch_fail ==
-					DDR50_SUPPORT_MASK)
-				    || (sd_card->sd_switch_fail ==
-					    SDR50_SUPPORT_MASK)) {
-					sd_init_power(chip);
-					SET_RETRY_SD20_MODE(sd_card);
-				} else if (sd_card->sd_switch_fail ==
-						HS_SUPPORT_MASK) {
-					sd_dont_switch = 1;
-				}
-				goto Switch_Fail;
-			}
-		} else {
-			if (support_1v8) {
-				SET_RETRY_SD20_MODE(sd_card);
-				sd_init_power(chip);
-				sd_dont_switch = 1;
-
-				goto Switch_Fail;
-			}
-		}
-	}
-
-	if (!support_1v8) {
-		/* ACMD6 */
-		retval =
-		    sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
-					SD_RSP_TYPE_R1, NULL, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		/* Enable 4 bit data bus */
-		retval =
-		    sd_send_cmd_get_rsp(chip, SET_BUS_WIDTH, 2, SD_RSP_TYPE_R1,
-					NULL, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	if (CHK_SD30_SPEED(sd_card)) {
-		rts51x_write_register(chip, SD30_DRIVE_SEL, SD30_DRIVE_MASK,
-				      0x03);
-
-		retval = sd_set_init_para(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		if (CHK_SD_DDR50(sd_card))
-			retval = sd_ddr_tuning(chip);
-		else
-			retval = sd_sdr_tuning(chip);
-
-		if (retval != STATUS_SUCCESS) {
-			SET_RETRY_SD20_MODE(sd_card);
-			RTS51X_DEBUGP("tuning phase fail,goto SD20 mode\n");
-			sd_init_power(chip);
-			CLR_SD30_SPEED(sd_card);
-			goto Switch_Fail;
-		}
-		if (STATUS_SUCCESS ==
-		    sd_wait_currentstate_dataready(chip, 0x08, 1, 20)) {
-			cmd[0] = 0x40 | READ_SINGLE_BLOCK;
-			cmd[1] = 0x00;
-			cmd[2] = 0x00;
-			cmd[3] = 0x00;
-			cmd[4] = 0x00;
-			retval =
-			    sd_read_data(chip, SD_TM_NORMAL_READ, cmd, 5, 512,
-					 1, SD_BUS_WIDTH_4, NULL, 0, 600);
-			if (retval != STATUS_SUCCESS) {
-				SET_RETRY_SD20_MODE(sd_card);
-				RTS51X_DEBUGP("read lba0 fail,"
-							"goto SD20 mode\n");
-				sd_init_power(chip);
-				CLR_SD30_SPEED(sd_card);
-				goto Switch_Fail;
-			}
-		}
-	}
-	sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr, SD_RSP_TYPE_R1,
-			    NULL, 0);
-
-	retval = sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
-			SD_RSP_TYPE_R1, NULL, 0);
-	if (retval == STATUS_SUCCESS) {
-		int ret;
-		cmd[0] = 0x40 | SEND_STATUS;
-		cmd[1] = 0x00;
-		cmd[2] = 0x00;
-		cmd[3] = 0x00;
-		cmd[4] = 0x00;
-		ret =
-		    sd_read_data(chip, SD_TM_NORMAL_READ, cmd, 5, 64, 1,
-				 SD_BUS_WIDTH_4, buf, 64, 600);
-		if (ret == STATUS_SUCCESS) {
-			sd_card_type = ((u16) buf[2] << 8) | (u16) buf[3];
-			RTS51X_DEBUGP("sd_card_type:0x%4x\n", sd_card_type);
-			if ((sd_card_type == 0x0001)
-			    || (sd_card_type == 0x0002))
-				chip->card_wp |= SD_CARD;
-		} else {
-			rts51x_clear_sd_error(chip);
-			sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
-					    SD_RSP_TYPE_R1, NULL, 0);
-		}
-	} else {
-		rts51x_clear_sd_error(chip);
-		sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
-				    SD_RSP_TYPE_R1, NULL, 0);
-	}
-
-	/* Check SD Machanical Write-Protect Switch */
-	retval = rts51x_get_card_status(chip, &(chip->card_status));
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	if (chip->card_status & SD_WP)
-		chip->card_wp |= SD_CARD;
-
-	chip->card_bus_width[chip->card2lun[SD_CARD]] = 4;
-
-	return STATUS_SUCCESS;
-}
-
-static int mmc_test_switch_bus(struct rts51x_chip *chip, u8 width)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	u8 buf[8] = { 0 }, bus_width;
-	u16 byte_cnt;
-	int len;
-
-	retval =
-	    sd_send_cmd_get_rsp(chip, BUSTEST_W, 0, SD_RSP_TYPE_R1, NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (width == MMC_8BIT_BUS) {
-		buf[0] = 0x55;
-		buf[1] = 0xAA;
-		len = 8;
-		byte_cnt = 8;
-		bus_width = SD_BUS_WIDTH_8;
-	} else {
-		buf[0] = 0x5A;
-		len = 4;
-		byte_cnt = 4;
-		bus_width = SD_BUS_WIDTH_4;
-	}
-
-	retval = sd_write_data(chip, SD_TM_AUTO_WRITE_3,
-			       NULL, 0, byte_cnt, 1, bus_width, buf, len, 100);
-	if (retval != STATUS_SUCCESS) {
-		u8 val1 = 0, val2 = 0;
-		rts51x_ep0_read_register(chip, SD_STAT1, &val1);
-		rts51x_ep0_read_register(chip, SD_STAT2, &val2);
-		rts51x_clear_sd_error(chip);
-		if ((val1 & 0xE0) || val2)
-			TRACE_RET(chip, STATUS_FAIL);
-	}
-	RTS51X_DEBUGP("SD/MMC CMD %d\n", BUSTEST_R);
-
-	rts51x_init_cmd(chip);
-
-	/* CMD14 */
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD0, 0xFF, 0x40 | BUSTEST_R);
-
-	if (width == MMC_8BIT_BUS)
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, 0x08);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, 0x04);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L, 0xFF, 1);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H, 0xFF, 0);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF,
-		       SD_CALCULATE_CRC7 | SD_NO_CHECK_CRC16 |
-		       SD_NO_WAIT_BUSY_END | SD_CHECK_CRC7 | SD_RSP_LEN_6);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       PINGPONG_BUFFER);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-		       SD_TM_NORMAL_READ | SD_TRANSFER_START);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER, SD_TRANSFER_END,
-		       SD_TRANSFER_END);
-
-	rts51x_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2, 0, 0);
-	if (width == MMC_8BIT_BUS) {
-		len = 3;
-		rts51x_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 1, 0, 0);
-	} else {
-		len = 2;
-	}
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, len, 100);
-	if (CHECK_SD_TRANS_FAIL(chip, retval)) {
-		rts51x_clear_sd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	rts51x_read_rsp_buf(chip, 1, buf, 2);
-
-	if (width == MMC_8BIT_BUS) {
-		RTS51X_DEBUGP("BUSTEST_R [8bits]: 0x%02x 0x%02x\n",
-					buf[0], buf[1]);
-		if ((buf[0] == 0xAA) && (buf[1] == 0x55)) {
-			u8 rsp[5];
-			u32 arg;
-
-			if (CHK_MMC_DDR52(sd_card))
-				arg = 0x03B70600;
-			else
-				arg = 0x03B70200;
-			/* Switch MMC to  8-bit mode */
-			retval =
-			    sd_send_cmd_get_rsp(chip, SWITCH, arg,
-						SD_RSP_TYPE_R1b, rsp, 5);
-			if ((retval == STATUS_SUCCESS)
-			    && !(rsp[4] & MMC_SWITCH_ERR))
-				return STATUS_SUCCESS;
-		}
-	} else {
-		RTS51X_DEBUGP("BUSTEST_R [4bits]: 0x%02x\n", buf[0]);
-		if (buf[0] == 0xA5) {
-			u8 rsp[5];
-			u32 arg;
-
-			if (CHK_MMC_DDR52(sd_card))
-				arg = 0x03B70500;
-			else
-				arg = 0x03B70100;
-			/* Switch MMC to  4-bit mode */
-			retval =
-			    sd_send_cmd_get_rsp(chip, SWITCH, arg,
-						SD_RSP_TYPE_R1b, rsp, 5);
-			if ((retval == STATUS_SUCCESS)
-			    && !(rsp[4] & MMC_SWITCH_ERR))
-				return STATUS_SUCCESS;
-		}
-	}
-
-	TRACE_RET(chip, STATUS_FAIL);
-}
-
-static int mmc_switch_timing_bus(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	u8 card_type, card_type_mask = 0;
-	u8 buf[6];
-
-	CLR_MMC_HS(sd_card);
-
-	RTS51X_DEBUGP("SD/MMC CMD %d\n", SEND_EXT_CSD);
-
-	rts51x_init_cmd(chip);
-
-	/* SEND_EXT_CSD command */
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD0, 0xFF,
-			0x40 | SEND_EXT_CSD);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD1, 0xFF, 0);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD2, 0xFF, 0);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD3, 0xFF, 0);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD4, 0xFF, 0);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, 0);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_H, 0xFF, 2);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L, 0xFF, 1);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H, 0xFF, 0);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF,
-		       SD_CALCULATE_CRC7 | SD_CHECK_CRC16 | SD_NO_WAIT_BUSY_END
-		       | SD_CHECK_CRC7 | SD_RSP_LEN_6);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       PINGPONG_BUFFER);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-		       SD_TM_NORMAL_READ | SD_TRANSFER_START);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER, SD_TRANSFER_END,
-		       SD_TRANSFER_END);
-
-	rts51x_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 196, 0xFF, 0);
-	rts51x_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 212, 0xFF, 0);
-	rts51x_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 213, 0xFF, 0);
-	rts51x_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 214, 0xFF, 0);
-	rts51x_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 215, 0xFF, 0);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 6, 1000);
-
-	if (CHECK_SD_TRANS_FAIL(chip, retval)) {
-		if (retval == STATUS_TIMEDOUT) {
-			rts51x_clear_sd_error(chip);
-			sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
-					    SD_RSP_TYPE_R1, NULL, 0);
-		}
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	rts51x_read_rsp_buf(chip, 0, buf, 6);
-
-	if (buf[0] & SD_TRANSFER_ERR) {
-		sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
-				    SD_RSP_TYPE_R1, NULL, 0);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-	if (CHK_MMC_SECTOR_MODE(sd_card))
-		sd_card->capacity =
-		    ((u32) buf[5] << 24) | ((u32) buf[4] << 16) |
-		    ((u32) buf[3] << 8) | ((u32) buf[2]);
-	if (CHECK_UHS50(chip))
-		card_type_mask = 0x07;
-	else
-		card_type_mask = 0x03;
-
-	card_type = buf[1] & card_type_mask;
-	if (card_type) {
-		/* CARD TYPE FIELD = DDR52MHz, 52MHz or 26MHz */
-		u8 rsp[5];
-
-		if (card_type & 0x04)
-			SET_MMC_DDR52(sd_card);
-		else if (card_type & 0x02)
-			SET_MMC_52M(sd_card);
-		else
-			SET_MMC_26M(sd_card);
-
-		retval =
-		    sd_send_cmd_get_rsp(chip, SWITCH, 0x03B90100,
-					SD_RSP_TYPE_R1b, rsp, 5);
-		if ((retval != STATUS_SUCCESS) || (rsp[4] & MMC_SWITCH_ERR))
-			CLR_MMC_HS(sd_card);
-	}
-	sd_choose_proper_clock(chip);
-	retval = switch_clock(chip, sd_card->sd_clock);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	/* Test Bus Procedure */
-	if (mmc_test_switch_bus(chip, MMC_8BIT_BUS) == STATUS_SUCCESS) {
-		SET_MMC_8BIT(sd_card);
-		chip->card_bus_width[chip->card2lun[SD_CARD]] = 8;
-	} else if (mmc_test_switch_bus(chip, MMC_4BIT_BUS) == STATUS_SUCCESS) {
-		SET_MMC_4BIT(sd_card);
-		chip->card_bus_width[chip->card2lun[SD_CARD]] = 4;
-	} else {
-		CLR_MMC_8BIT(sd_card);
-		CLR_MMC_4BIT(sd_card);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int reset_mmc(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval, i = 0, j = 0, k = 0;
-	u8 rsp[16];
-	u8 spec_ver = 0;
-	u8 change_to_ddr52 = 1;
-	u8 cmd[5];
-
-MMC_DDR_FAIL:
-
-	retval = sd_prepare_reset(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	SET_MMC(sd_card);
-
-RTY_MMC_RST:
-	retval =
-	    sd_send_cmd_get_rsp(chip, GO_IDLE_STATE, 0, SD_RSP_TYPE_R0, NULL,
-				0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	do {
-		{
-			u8 temp = 0;
-			rts51x_read_register(chip, CARD_INT_PEND, &temp);
-			if (temp & SD_INT) {
-				chip->reset_need_retry = 1;
-				rts51x_write_register(chip, CARD_INT_PEND,
-						      XD_INT | SD_INT | MS_INT,
-						      XD_INT | SD_INT | MS_INT);
-				sd_set_reset_fail(chip, MMC_RESET_FAIL);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-
-		/* CMD  1 */
-		retval = sd_send_cmd_get_rsp(chip, SEND_OP_COND,
-					     (SUPPORT_VOLTAGE | 0x40000000),
-					     SD_RSP_TYPE_R3, rsp, 5);
-		if (retval != STATUS_SUCCESS) {
-			if (monitor_card_cd(chip, SD_CARD) == CD_NOT_EXIST) {
-				sd_set_reset_fail(chip, MMC_RESET_FAIL);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-
-			if (sd_check_err_code(chip, SD_BUSY)
-			    || sd_check_err_code(chip, SD_TO_ERR)) {
-				k++;
-				if (k < 20) {
-					sd_clr_err_code(chip);
-					goto RTY_MMC_RST;
-				} else {
-					TRACE_RET(chip, STATUS_FAIL);
-				}
-			} else {
-				j++;
-				if (j < 100) {
-					sd_clr_err_code(chip);
-					goto RTY_MMC_RST;
-				} else {
-					TRACE_RET(chip, STATUS_FAIL);
-				}
-			}
-		}
-
-		wait_timeout(20);
-		i++;
-	} while (!(rsp[1] & 0x80) && (i < 100)); /* Not complete power on */
-
-	if (i == 100) {
-		/* Time out */
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if ((rsp[1] & 0x60) == 0x40)
-		SET_MMC_SECTOR_MODE(sd_card);
-	else
-		CLR_MMC_SECTOR_MODE(sd_card);
-
-	/* CMD 2 */
-	retval =
-	    sd_send_cmd_get_rsp(chip, ALL_SEND_CID, 0, SD_RSP_TYPE_R2, NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	/* CMD 3 */
-	sd_card->sd_addr = 0x00100000;
-	retval =
-	    sd_send_cmd_get_rsp(chip, SET_RELATIVE_ADDR, sd_card->sd_addr,
-				SD_RSP_TYPE_R6, rsp, 5);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	/* Get CSD register for Calculating Timing,Capacity
-	 * Check CSD to determine as if this is the SD ROM card */
-	retval = sd_check_csd(chip, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	/* Get MMC Spec_Ver in the CSD register */
-	spec_ver = (sd_card->raw_csd[0] & 0x3C) >> 2;
-
-	/* Select MMC card */
-	retval = rts51x_sd_select_card(chip, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	/* Set block length 512 bytes for all block commands */
-	retval =
-	    sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, 0x200, SD_RSP_TYPE_R1, NULL,
-				0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_WRITE_REG(chip, SD_CFG1, SD_CLK_DIVIDE_MASK, SD_CLK_DIVIDE_0);
-
-	if (chip->ic_version < 2)
-		rts51x_write_register(chip, SD30_DRIVE_SEL, SD30_DRIVE_MASK,
-				      0x02);
-	rts51x_write_register(chip, CARD_DRIVE_SEL, SD20_DRIVE_MASK, DRIVE_8mA);
-
-	chip->card_bus_width[chip->card2lun[SD_CARD]] = 1;
-	if (spec_ver == 4) {
-		/* MMC 4.x Cards */
-		(void)mmc_switch_timing_bus(chip);
-	}
-
-	if (CHK_MMC_SECTOR_MODE(sd_card) && (sd_card->capacity == 0))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	if (CHK_MMC_DDR52(sd_card) && change_to_ddr52) {
-		/* Card is extracted while identifying */
-		if (monitor_card_cd(chip, SD_CARD) == CD_NOT_EXIST)
-			TRACE_RET(chip, STATUS_FAIL);
-
-		retval = sd_set_init_para(chip);
-		if (retval != STATUS_SUCCESS) {
-			CLR_MMC_DDR52(sd_card);
-			sd_init_power(chip);
-			change_to_ddr52 = 0;
-			goto MMC_DDR_FAIL;
-		}
-
-		retval = mmc_ddr_tuning(chip);
-		if (retval != STATUS_SUCCESS) {
-			CLR_MMC_DDR52(sd_card);
-			sd_init_power(chip);
-			change_to_ddr52 = 0;
-			goto MMC_DDR_FAIL;
-		}
-
-		if (STATUS_SUCCESS ==
-		    sd_wait_currentstate_dataready(chip, 0x08, 1, 20)) {
-			cmd[0] = 0x40 | READ_SINGLE_BLOCK;
-			cmd[1] = 0x00;
-			cmd[2] = 0x00;
-			cmd[3] = 0x00;
-			cmd[4] = 0x00;
-			if (CHK_MMC_8BIT(sd_card)) {
-				retval =
-				    sd_read_data(chip, SD_TM_NORMAL_READ, cmd,
-						 5, 512, 1, SD_BUS_WIDTH_8,
-						 NULL, 0, 600);
-			} else if (CHK_MMC_4BIT(sd_card)) {
-				retval =
-				    sd_read_data(chip, SD_TM_NORMAL_READ, cmd,
-						 5, 512, 1, SD_BUS_WIDTH_4,
-						 NULL, 0, 600);
-			} else {
-				retval =
-				    sd_read_data(chip, SD_TM_NORMAL_READ, cmd,
-						 5, 512, 1, SD_BUS_WIDTH_1,
-						 NULL, 0, 600);
-			}
-
-			if (retval != STATUS_SUCCESS) {
-				CLR_MMC_DDR52(sd_card);
-				change_to_ddr52 = 0;
-				RTS51X_DEBUGP("read lba0 fail,"
-							"goto SD20 mode\n");
-				sd_init_power(chip);
-				goto MMC_DDR_FAIL;
-			}
-		}
-	}
-
-	retval = rts51x_get_card_status(chip, &(chip->card_status));
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	if (chip->card_status & SD_WP)
-		chip->card_wp |= SD_CARD;
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_reset_sd_card(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	int i;
-
-	memset(sd_card, 0, sizeof(struct sd_info));
-
-	/* Init variables */
-	sd_card->sd_type = 0;
-	sd_card->seq_mode = 0;
-	sd_card->sd_data_buf_ready = 0;
-	sd_card->capacity = 0;
-	sd_card->sd_switch_fail = 0;
-
-	sd_clear_reset_fail(chip);
-	rts51x_enable_card_clock(chip, SD_CARD);
-
-	sd_init_power(chip);
-
-	chip->reset_need_retry = 0;
-	for (i = 0; i < 3; i++) {
-		if (!chip->option.reset_mmc_first) { /* reset sd first */
-			retval = reset_sd(chip);
-			if (retval != STATUS_SUCCESS) {
-				/* Switch SD bus to 3V3 signal */
-				RTS51X_WRITE_REG(chip, SD_PAD_CTL,
-						 SD_IO_USING_1V8, 0);
-				if (sd_check_reset_fail(chip, SD_RESET_FAIL))
-					sd_clear_reset_fail(chip);
-				else
-					retval = reset_mmc(chip);
-			}
-		} else { /* reset MMC first */
-			retval = reset_mmc(chip);
-			if (retval != STATUS_SUCCESS) {
-				if (sd_check_reset_fail(chip, MMC_RESET_FAIL)) {
-					sd_clear_reset_fail(chip);
-				} else {
-					retval = reset_sd(chip);
-					if (retval != STATUS_SUCCESS) {
-						/* Switch SD bus to
-						 * 3V3 signal */
-						RTS51X_WRITE_REG(chip,
-							SD_PAD_CTL,
-							SD_IO_USING_1V8, 0);
-					}
-				}
-			}
-		}
-
-		if ((retval == STATUS_SUCCESS) || (!chip->reset_need_retry)) {
-			/* if reset success or don't need retry,then break */
-			break;
-		}
-		if (monitor_card_cd(chip, SD_CARD) == CD_NOT_EXIST) {
-			/* card is extracted */
-			break;
-		}
-		RTS51X_DEBUGP("retry reset sd card,%d\n", i);
-		chip->reset_need_retry = 0;
-	}
-
-	sd_clear_reset_fail(chip);
-	chip->reset_need_retry = 0;
-
-	if (retval == STATUS_SUCCESS) {
-		rts51x_init_cmd(chip);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, SD_CLK_DIVIDE_MASK,
-			       SD_CLK_DIVIDE_0);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, 0);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_H, 0xFF, 2);
-		retval = rts51x_send_cmd(chip, MODE_C, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	} else {
-		chip->capacity[chip->card2lun[SD_CARD]] = sd_card->capacity = 0;
-		if (chip->option.reset_or_rw_fail_set_pad_drive) {
-			rts51x_write_register(chip, CARD_DRIVE_SEL,
-					      SD20_DRIVE_MASK, DRIVE_8mA);
-		}
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	chip->capacity[chip->card2lun[SD_CARD]] = sd_card->capacity;
-
-	if (chip->option.sd_send_status_en) {
-		sd_card->sd_send_status_en = 1;
-	} else {
-		if (sd_card->capacity > 0x20000) { /* 64MB */
-			sd_card->sd_send_status_en = 0;
-		} else {
-			sd_card->sd_send_status_en = 1;
-		}
-	}
-	RTS51X_DEBUGP("sd_card->sd_send_status = %d\n",
-		       sd_card->sd_send_status_en);
-
-	retval = sd_set_init_para(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_DEBUGP("sd_card->sd_type = 0x%x\n", sd_card->sd_type);
-
-	return STATUS_SUCCESS;
-}
-
-#define WAIT_DATA_READY_RTY_CNT		255
-
-static int wait_data_buf_ready(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int i, retval;
-
-	for (i = 0; i < WAIT_DATA_READY_RTY_CNT; i++) {
-		if (monitor_card_cd(chip, SD_CARD) == CD_NOT_EXIST)
-			TRACE_RET(chip, STATUS_FAIL);
-
-		sd_card->sd_data_buf_ready = 0;
-
-		retval = sd_send_cmd_get_rsp(chip, SEND_STATUS,
-					     sd_card->sd_addr, SD_RSP_TYPE_R1,
-					     NULL, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		if (sd_card->sd_data_buf_ready)
-			return sd_send_cmd_get_rsp(chip, SEND_STATUS,
-						   sd_card->sd_addr,
-						   SD_RSP_TYPE_R1, NULL, 0);
-	}
-
-	sd_set_err_code(chip, SD_TO_ERR);
-
-	TRACE_RET(chip, STATUS_FAIL);
-}
-
-static void sd_stop_seq_mode(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-
-	if (sd_card->seq_mode) {
-		retval = rts51x_sd_switch_clock(chip);
-		if (retval != STATUS_SUCCESS)
-			return;
-
-		retval = sd_send_cmd_get_rsp(chip, STOP_TRANSMISSION, 0,
-					     SD_RSP_TYPE_R1b, NULL, 0);
-		if (retval != STATUS_SUCCESS)
-			sd_set_err_code(chip, SD_STS_ERR);
-		sd_card->seq_mode = 0;
-
-		rts51x_ep0_write_register(chip, MC_FIFO_CTL, FIFO_FLUSH,
-					  FIFO_FLUSH);
-	}
-}
-
-static inline int sd_auto_tune_clock(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-
-	if (chip->asic_code) {
-		if (sd_card->sd_clock > 30)
-			sd_card->sd_clock -= 20;
-	} else {
-		if (sd_card->sd_clock == CLK_100)
-			sd_card->sd_clock = CLK_80;
-		else if (sd_card->sd_clock == CLK_80)
-			sd_card->sd_clock = CLK_60;
-		else if (sd_card->sd_clock == CLK_60)
-			sd_card->sd_clock = CLK_50;
-	}
-
-	retval = rts51x_sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_sd_rw(struct scsi_cmnd *srb, struct rts51x_chip *chip, u32 start_sector,
-	  u16 sector_cnt)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	u32 data_addr;
-	int retval;
-	u8 flag;
-	unsigned int pipe;
-	u8 stageflag;
-
-	sd_card->counter = 0;
-
-	if (!CHK_SD_HCXC(sd_card) && !CHK_MMC_SECTOR_MODE(sd_card))
-		data_addr = start_sector << 9;
-	else
-		data_addr = start_sector;
-
-	RTS51X_DEBUGP("rts51x_sd_rw, data_addr = 0x%x\n", data_addr);
-
-	sd_clr_err_code(chip);
-
-	retval = rts51x_sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (sd_card->seq_mode && ((sd_card->pre_dir != srb->sc_data_direction)
-				  ||
-				  ((sd_card->pre_sec_addr +
-				    sd_card->pre_sec_cnt) != start_sector))) {
-		if ((sd_card->pre_dir == DMA_FROM_DEVICE)
-		    && !CHK_SD30_SPEED(sd_card)
-		    && !CHK_SD_HS(sd_card)
-		    && !CHK_MMC_HS(sd_card)
-		    && sd_card->sd_send_status_en) {
-			sd_send_cmd_get_rsp(chip, SEND_STATUS,
-					    sd_card->sd_addr, SD_RSP_TYPE_R1,
-					    NULL, 0);
-		}
-
-		retval =
-		    sd_send_cmd_get_rsp(chip, STOP_TRANSMISSION, 0,
-					SD_RSP_TYPE_R1b, NULL, 0);
-		if (retval != STATUS_SUCCESS) {
-			sd_set_err_code(chip, SD_STS_ERR);
-			TRACE_RET(chip, sd_parse_err_code(chip));
-		}
-
-		sd_card->seq_mode = 0;
-
-		RTS51X_WRITE_REG(chip, MC_FIFO_CTL, FIFO_FLUSH, FIFO_FLUSH);
-
-		if (!CHK_SD30_SPEED(sd_card)
-		    && !CHK_SD_HS(sd_card)
-		    && !CHK_MMC_HS(sd_card)
-		    && sd_card->sd_send_status_en) {
-			/* random rw, so pre_sec_cnt < 0x80 */
-			sd_send_cmd_get_rsp(chip, SEND_STATUS,
-					    sd_card->sd_addr, SD_RSP_TYPE_R1,
-					    NULL, 0);
-		}
-	}
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, 0x00);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_H, 0xFF, 0x02);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L, 0xFF,
-		       (u8) sector_cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H, 0xFF,
-		       (u8) (sector_cnt >> 8));
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       RING_BUFFER);
-
-	if (CHK_MMC_8BIT(sd_card))
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0x03,
-			       SD_BUS_WIDTH_8);
-	else if (CHK_MMC_4BIT(sd_card) || CHK_SD(sd_card))
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0x03,
-			       SD_BUS_WIDTH_4);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0x03,
-			       SD_BUS_WIDTH_1);
-
-	if (sd_card->seq_mode) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF,
-			       SD_NO_CALCULATE_CRC7 | SD_CHECK_CRC16 |
-			       SD_NO_WAIT_BUSY_END | SD_NO_CHECK_CRC7 |
-			       SD_RSP_LEN_0);
-
-		rts51x_trans_dma_enable(srb->sc_data_direction, chip, sector_cnt * 512,
-				 DMA_512);
-
-		if (srb->sc_data_direction == DMA_FROM_DEVICE) {
-			flag = MODE_CDIR;
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-				       SD_TM_AUTO_READ_3 | SD_TRANSFER_START);
-		} else {
-			flag = MODE_CDOR;
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-				       SD_TM_AUTO_WRITE_3 | SD_TRANSFER_START);
-		}
-
-		rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER,
-			       SD_TRANSFER_END, SD_TRANSFER_END);
-
-		retval = rts51x_send_cmd(chip, flag, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	} else {
-		if (srb->sc_data_direction == DMA_FROM_DEVICE) {
-			RTS51X_DEBUGP("SD/MMC CMD %d\n", READ_MULTIPLE_BLOCK);
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD0, 0xFF,
-				       0x40 | READ_MULTIPLE_BLOCK);
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD1, 0xFF,
-				       (u8) (data_addr >> 24));
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD2, 0xFF,
-				       (u8) (data_addr >> 16));
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD3, 0xFF,
-				       (u8) (data_addr >> 8));
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD4, 0xFF,
-				       (u8) data_addr);
-
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF,
-				       SD_CALCULATE_CRC7 | SD_CHECK_CRC16 |
-				       SD_NO_WAIT_BUSY_END | SD_CHECK_CRC7 |
-				       SD_RSP_LEN_6);
-
-			rts51x_trans_dma_enable(srb->sc_data_direction, chip,
-					 sector_cnt * 512, DMA_512);
-
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-				       SD_TM_AUTO_READ_2 | SD_TRANSFER_START);
-			rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER,
-				       SD_TRANSFER_END, SD_TRANSFER_END);
-
-			retval = rts51x_send_cmd(chip, MODE_CDIR, 100);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-		} else {
-			retval = rts51x_send_cmd(chip, MODE_C, 50);
-			if (retval != STATUS_SUCCESS) {
-				rts51x_clear_sd_error(chip);
-
-				sd_set_err_code(chip, SD_TO_ERR);
-				TRACE_RET(chip, sd_parse_err_code(chip));
-			}
-
-			retval = wait_data_buf_ready(chip);
-			if (retval != STATUS_SUCCESS) {
-				sd_set_err_code(chip, SD_TO_ERR);
-				TRACE_RET(chip, sd_parse_err_code(chip));
-			}
-
-			retval = sd_send_cmd_get_rsp(chip, WRITE_MULTIPLE_BLOCK,
-						     data_addr, SD_RSP_TYPE_R1,
-						     NULL, 0);
-			if (retval != STATUS_SUCCESS) {
-				sd_set_err_code(chip, SD_CRC_ERR);
-				TRACE_RET(chip, sd_parse_err_code(chip));
-			}
-
-			rts51x_init_cmd(chip);
-
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF,
-				       SD_NO_CALCULATE_CRC7 | SD_CHECK_CRC16 |
-				       SD_NO_WAIT_BUSY_END | SD_NO_CHECK_CRC7 |
-				       SD_RSP_LEN_0);
-
-			rts51x_trans_dma_enable(srb->sc_data_direction, chip,
-					 sector_cnt * 512, DMA_512);
-
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-				       SD_TM_AUTO_WRITE_3 | SD_TRANSFER_START);
-			rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER,
-				       SD_TRANSFER_END, SD_TRANSFER_END);
-
-			retval = rts51x_send_cmd(chip, MODE_CDOR, 100);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-		}
-
-		sd_card->seq_mode = 1;
-	}
-
-	if (srb->sc_data_direction == DMA_FROM_DEVICE) {
-		pipe = RCV_BULK_PIPE(chip);
-		stageflag = STAGE_DI;
-	} else {
-		pipe = SND_BULK_PIPE(chip);
-		stageflag = STAGE_DO;
-	}
-
-	retval =
-	    rts51x_transfer_data_rcc(chip, pipe, scsi_sglist(srb),
-				     scsi_bufflen(srb), scsi_sg_count(srb),
-				     NULL, 10000, stageflag);
-	if (retval != STATUS_SUCCESS) {
-		u8 stat = 0;
-		int err = retval;
-
-		sd_print_debug_reg(chip);
-
-		rts51x_ep0_read_register(chip, SD_STAT1, &stat);
-		RTS51X_DEBUGP("SD_STAT1: 0x%x\n", stat);
-
-		rts51x_clear_sd_error(chip);
-
-		retval =
-		    sd_send_cmd_get_rsp(chip, STOP_TRANSMISSION, 0,
-					SD_RSP_TYPE_R1b, NULL, 0);
-		if (retval != STATUS_SUCCESS) {
-			sd_set_err_code(chip, SD_STS_ERR);
-			TRACE_RET(chip, retval);
-		}
-
-		if (stat & (SD_CRC7_ERR | SD_CRC16_ERR | SD_CRC_WRITE_ERR)) {
-			RTS51X_DEBUGP("SD CRC error, tune clock!\n");
-			sd_auto_tune_clock(chip);
-		}
-
-		sd_card->seq_mode = 0;
-
-		TRACE_RET(chip, err);
-	}
-	retval = rts51x_get_rsp(chip, 1, 2000);
-	if (CHECK_SD_TRANS_FAIL(chip, retval)) {
-		rts51x_clear_sd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	sd_card->pre_sec_addr = start_sector;
-	sd_card->pre_sec_cnt = sector_cnt;
-	sd_card->pre_dir = srb->sc_data_direction;
-
-	return STATUS_SUCCESS;
-}
-
-void rts51x_sd_cleanup_work(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-
-	if (sd_card->seq_mode) {
-		RTS51X_DEBUGP("SD: stop transmission\n");
-		sd_stop_seq_mode(chip);
-		sd_card->counter = 0;
-	}
-}
-
-static inline void sd_fill_power_off_card3v3(struct rts51x_chip *chip)
-{
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_CLK_EN, SD_CLK_EN, 0);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_OE, SD_OUTPUT_EN, 0);
-	if (!chip->option.FT2_fast_mode) {
-#ifdef SD_XD_IO_FOLLOW_PWR
-		if (CHECK_PKG(chip, LQFP48)
-		    || chip->option.rts5129_D3318_off_enable)
-			rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL,
-				       POWER_MASK | LDO_OFF,
-				       POWER_OFF | LDO_OFF);
-		else
-			rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL,
-				       POWER_MASK, POWER_OFF);
-#else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, POWER_MASK,
-			       POWER_OFF);
-#endif
-	}
-}
-
-static int sd_power_off_card3v3(struct rts51x_chip *chip)
-{
-	int retval;
-
-	rts51x_init_cmd(chip);
-
-	sd_fill_power_off_card3v3(chip);
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-#ifdef SD_XD_IO_FOLLOW_PWR
-	if (!chip->option.FT2_fast_mode)
-		wait_timeout(chip->option.D3318_off_delay);
-#endif
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_release_sd_card(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-
-	RTS51X_DEBUGP("rts51x_release_sd_card\n");
-
-	chip->card_ready &= ~SD_CARD;
-	chip->card_fail &= ~SD_CARD;
-	chip->card_wp &= ~SD_CARD;
-
-	memset(sd_card->raw_csd, 0, 16);
-	memset(sd_card->raw_scr, 0, 8);
-
-	rts51x_write_register(chip, SFSM_ED, HW_CMD_STOP, HW_CMD_STOP);
-	rts51x_write_register(chip, SD_PAD_CTL, SD_IO_USING_1V8, 0);
-	if (CHECK_PKG(chip, LQFP48) || chip->option.rts5129_D3318_off_enable)
-		sd_power_off_card3v3(chip);
-
-	rts51x_init_cmd(chip);
-	if (!(CHECK_PKG(chip, LQFP48) || chip->option.rts5129_D3318_off_enable))
-		sd_fill_power_off_card3v3(chip);
-
-	if (chip->asic_code)
-		sd_pull_ctl_disable(chip);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, FPGA_PULL_CTL,
-			       FPGA_SD_PULL_CTL_BIT | 0x20,
-			       FPGA_SD_PULL_CTL_BIT);
-
-	/* Switch LDO3318 to 3.3V */
-	rts51x_add_cmd(chip, WRITE_REG_CMD, LDO_POWER_CFG, TUNE_SD18_MASK,
-		       TUNE_SD18_3V3);
-
-	if (CHK_MMC_DDR52(sd_card) && CHK_MMC_8BIT(sd_card))
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DMA1_CTL,
-			       EXTEND_DMA1_ASYNC_SIGNAL,
-			       EXTEND_DMA1_ASYNC_SIGNAL);
-	if (CHK_SD30_SPEED(sd_card) || CHK_MMC(sd_card))
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD30_DRIVE_SEL,
-			       SD30_DRIVE_MASK, chip->option.sd30_pad_drive);
-	/* Suspend LDO3318 */
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, LDO3318_PWR_MASK,
-		       LDO_SUSPEND);
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	wait_timeout(20);
-
-	return STATUS_SUCCESS;
-}
diff --git a/drivers/staging/rts5139/sd.h b/drivers/staging/rts5139/sd.h
deleted file mode 100644
index 7dd943f54c7..00000000000
--- a/drivers/staging/rts5139/sd.h
+++ /dev/null
@@ -1,275 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_SD_H
-#define __RTS51X_SD_H
-
-#include "rts51x_chip.h"
-
-#define SD_MAX_RETRY_COUNT	3
-
-#define SUPPORT_VOLTAGE	0x003C0000
-
-#define SD_RESET_FAIL	0x01
-#define MMC_RESET_FAIL  0x02
-
-/* Error Code */
-#define	SD_NO_ERROR		0x0
-#define	SD_CRC_ERR		0x80
-#define	SD_TO_ERR		0x40
-#define	SD_NO_CARD		0x20
-#define SD_BUSY			0x10
-#define	SD_STS_ERR		0x08
-#define SD_RSP_TIMEOUT		0x04
-
-/* MMC/SD Command Index */
-/* Basic command (class 0) */
-#define GO_IDLE_STATE		0
-#define	SEND_OP_COND		1 /* reserved for SD */
-#define	ALL_SEND_CID		2
-#define	SET_RELATIVE_ADDR	3
-#define	SEND_RELATIVE_ADDR	3
-#define	SET_DSR			4
-#define IO_SEND_OP_COND		5
-#define	SWITCH			6
-#define	SELECT_CARD		7
-#define	DESELECT_CARD		7
-/* CMD8 is "SEND_EXT_CSD" for MMC4.x Spec
- * while is "SEND_IF_COND" for SD 2.0 */
-#define	SEND_EXT_CSD		8
-#define	SEND_IF_COND		8
-/* end  */
-#define	SEND_CSD		9
-#define	SEND_CID		10
-#define	VOLTAGE_SWITCH		11
-#define	READ_DAT_UTIL_STOP	11 /* reserved for SD */
-#define	STOP_TRANSMISSION	12
-#define	SEND_STATUS		13
-#define	GO_INACTIVE_STATE	15
-
-/* Block oriented read commands (class 2) */
-#define	SET_BLOCKLEN		16
-#define	READ_SINGLE_BLOCK	17
-#define	READ_MULTIPLE_BLOCK	18
-#define	SEND_TUNING_PATTERN	19
-
-/* Bus Width Test */
-#define	BUSTEST_R		14
-#define	BUSTEST_W		19
-/* end */
-
-/* Block oriented write commands (class 4) */
-#define	WRITE_BLOCK		24
-#define	WRITE_MULTIPLE_BLOCK	25
-#define	PROGRAM_CSD		27
-
-/* Erase commands */
-#define	ERASE_WR_BLK_START	32
-#define	ERASE_WR_BLK_END	33
-#define	ERASE_CMD		38
-
-/* Block Oriented Write Protection Commands */
-#define LOCK_UNLOCK		42
-
-#define	IO_RW_DIRECT		52
-
-/* Application specific commands (class 8) */
-#define	APP_CMD			55
-#define	GEN_CMD			56
-
-/* SD Application command Index */
-#define	SET_BUS_WIDTH			6
-#define	SD_STATUS			13
-#define	SEND_NUM_WR_BLOCKS		22
-#define	SET_WR_BLK_ERASE_COUNT		23
-#define	SD_APP_OP_COND			41
-#define	SET_CLR_CARD_DETECT		42
-#define	SEND_SCR			51
-
-/* SD TIMEOUT function return error */
-#define	SD_READ_COMPLETE	0x00
-#define	SD_READ_TO		0x01
-#define	SD_READ_ADVENCE		0x02
-
-/* SD v1.1 CMD6 SWITCH function */
-#define	SD_CHECK_MODE		0x00
-#define	SD_SWITCH_MODE		0x80
-#define	SD_FUNC_GROUP_1		0x01
-#define	SD_FUNC_GROUP_2		0x02
-#define	SD_FUNC_GROUP_3		0x03
-#define	SD_FUNC_GROUP_4		0x04
-#define	SD_CHECK_SPEC_V1_1	0xFF
-
-/* SD Command Argument */
-#define	NO_ARGUMENT	                        0x00
-#define	CHECK_PATTERN				0x000000AA
-#define	VOLTAGE_SUPPLY_RANGE			0x00000100 /* 2.7~3.6V */
-#define	SUPPORT_HIGH_AND_EXTENDED_CAPACITY	0x40000000
-#define	SUPPORT_MAX_POWER_PERMANCE	        0x10000000
-#define	SUPPORT_1V8	                        0x01000000
-
-/* Switch Command Error Code */
-#define	SWTICH_NO_ERR	  0x00
-#define	CARD_NOT_EXIST	  0x01
-#define	SPEC_NOT_SUPPORT  0x02
-#define	CHECK_MODE_ERR	  0x03
-#define	CHECK_NOT_READY	  0x04
-#define	SWITCH_CRC_ERR	  0x05
-#define	SWITCH_MODE_ERR	  0x06
-#define	SWITCH_PASS	  0x07
-
-/* Function Group Definition */
-/* Function Group 1 */
-#define	HS_SUPPORT			0x01
-#define	SDR50_SUPPORT			0x02
-#define	SDR104_SUPPORT			0x03
-#define	DDR50_SUPPORT			0x04
-#define	HS_SUPPORT_MASK			0x02
-#define	SDR50_SUPPORT_MASK		0x04
-#define	SDR104_SUPPORT_MASK		0x08
-#define	DDR50_SUPPORT_MASK		0x10
-#define	HS_QUERY_SWITCH_OK		0x01
-#define	SDR50_QUERY_SWITCH_OK		0x02
-#define	SDR104_QUERY_SWITCH_OK		0x03
-#define	DDR50_QUERY_SWITCH_OK		0x04
-#define	HS_SWITCH_BUSY			0x02
-#define	SDR50_SWITCH_BUSY		0x04
-#define	SDR104_SWITCH_BUSY		0x08
-#define	DDR50_SWITCH_BUSY		0x10
-#define	FUNCTION_GROUP1_SUPPORT_OFFSET       0x0D
-#define FUNCTION_GROUP1_QUERY_SWITCH_OFFSET  0x10
-#define FUNCTION_GROUP1_CHECK_BUSY_OFFSET    0x1D
-/* Function Group 3 */
-#define	DRIVING_TYPE_A	        0x01
-#define	DRIVING_TYPE_B		    0x00
-#define	DRIVING_TYPE_C		    0x02
-#define	DRIVING_TYPE_D	        0x03
-#define	DRIVING_TYPE_A_MASK	    0x02
-#define	DRIVING_TYPE_B_MASK	    0x01
-#define	DRIVING_TYPE_C_MASK	    0x04
-#define	DRIVING_TYPE_D_MASK	    0x08
-#define	TYPE_A_QUERY_SWITCH_OK	0x01
-#define	TYPE_B_QUERY_SWITCH_OK	0x00
-#define	TYPE_C_QUERY_SWITCH_OK  0x02
-#define	TYPE_D_QUERY_SWITCH_OK  0x03
-#define	TYPE_A_SWITCH_BUSY	    0x02
-#define	TYPE_B_SWITCH_BUSY	    0x01
-#define	TYPE_C_SWITCH_BUSY      0x04
-#define	TYPE_D_SWITCH_BUSY      0x08
-#define	FUNCTION_GROUP3_SUPPORT_OFFSET       0x09
-#define FUNCTION_GROUP3_QUERY_SWITCH_OFFSET  0x0F
-#define FUNCTION_GROUP3_CHECK_BUSY_OFFSET    0x19
-/* Function Group 4 */
-#define	CURRENT_LIMIT_200	    0x00
-#define	CURRENT_LIMIT_400	    0x01
-#define	CURRENT_LIMIT_600	    0x02
-#define	CURRENT_LIMIT_800	    0x03
-#define	CURRENT_LIMIT_200_MASK	0x01
-#define	CURRENT_LIMIT_400_MASK	0x02
-#define	CURRENT_LIMIT_600_MASK	0x04
-#define	CURRENT_LIMIT_800_MASK	0x08
-#define	CURRENT_LIMIT_200_QUERY_SWITCH_OK    0x00
-#define	CURRENT_LIMIT_400_QUERY_SWITCH_OK    0x01
-#define	CURRENT_LIMIT_600_QUERY_SWITCH_OK    0x02
-#define	CURRENT_LIMIT_800_QUERY_SWITCH_OK    0x03
-#define	CURRENT_LIMIT_200_SWITCH_BUSY        0x01
-#define	CURRENT_LIMIT_400_SWITCH_BUSY	     0x02
-#define	CURRENT_LIMIT_600_SWITCH_BUSY        0x04
-#define	CURRENT_LIMIT_800_SWITCH_BUSY        0x08
-#define	FUNCTION_GROUP4_SUPPORT_OFFSET       0x07
-#define FUNCTION_GROUP4_QUERY_SWITCH_OFFSET  0x0F
-#define FUNCTION_GROUP4_CHECK_BUSY_OFFSET    0x17
-/* Switch Function Status Offset */
-#define	DATA_STRUCTURE_VER_OFFSET   0x11 /* The high offset */
-#define MAX_PHASE		15
-/* #define      TOTAL_READ_PHASE    0x20 */
-/* #define      TOTAL_WRITE_PHASE    0x20 */
-/* MMC v4.0 */
-/* #define MMC_52MHZ_SPEED                       0x0001 */
-/* #define MMC_26MHZ_SPEED                       0x0002 */
-#define MMC_8BIT_BUS			0x0010
-#define MMC_4BIT_BUS			0x0020
-/* #define MMC_SECTOR_MODE                       0x0100 */
-#define MMC_SWITCH_ERR			0x80
-/* Tuning direction RX or TX */
-#define TUNE_TX    0x00
-#define TUNE_RX	   0x01
-/* For Change_DCM_FreqMode Function */
-#define CHANGE_TX  0x00
-#define CHANGE_RX  0x01
-#define DCM_HIGH_FREQUENCY_MODE  0x00
-#define DCM_LOW_FREQUENCY_MODE   0x01
-#define DCM_HIGH_FREQUENCY_MODE_SET  0x0C
-#define DCM_Low_FREQUENCY_MODE_SET   0x00
-/* For Change_FPGA_SSCClock Function */
-#define MULTIPLY_BY_1    0x00
-#define MULTIPLY_BY_2    0x01
-#define MULTIPLY_BY_3    0x02
-#define MULTIPLY_BY_4    0x03
-#define MULTIPLY_BY_5    0x04
-#define MULTIPLY_BY_6    0x05
-#define MULTIPLY_BY_7    0x06
-#define MULTIPLY_BY_8    0x07
-#define MULTIPLY_BY_9    0x08
-#define MULTIPLY_BY_10   0x09
-#define DIVIDE_BY_2      0x01
-#define DIVIDE_BY_3      0x02
-#define DIVIDE_BY_4      0x03
-#define DIVIDE_BY_5      0x04
-#define DIVIDE_BY_6      0x05
-#define DIVIDE_BY_7      0x06
-#define DIVIDE_BY_8      0x07
-#define DIVIDE_BY_9      0x08
-#define DIVIDE_BY_10     0x09
-#define CHECK_SD_TRANS_FAIL(chip, retval)	\
-	(((retval) != STATUS_SUCCESS) || \
-			(chip->rsp_buf[0] & SD_TRANSFER_ERR))
-/* SD Tuning Data Structure */
-/* Record continuous timing phase path */
-struct timing_phase_path {
-	int start;
-	int end;
-	int mid;
-	int len;
-};
-
-int rts51x_sd_select_card(struct rts51x_chip *chip, int select);
-int rts51x_reset_sd_card(struct rts51x_chip *chip);
-int rts51x_sd_switch_clock(struct rts51x_chip *chip);
-int rts51x_sd_rw(struct scsi_cmnd *srb, struct rts51x_chip *chip, u32 start_sector,
-	  u16 sector_cnt);
-void rts51x_sd_cleanup_work(struct rts51x_chip *chip);
-int rts51x_release_sd_card(struct rts51x_chip *chip);
-
-#ifdef SUPPORT_CPRM
-extern int reset_sd(struct rts51x_chip *chip);
-extern int sd_check_data0_status(struct rts51x_chip *chip);
-extern int sd_read_data(struct rts51x_chip *chip, u8 trans_mode, u8 *cmd,
-		int cmd_len, u16 byte_cnt, u16 blk_cnt, u8 bus_width,
-		u8 *buf, int buf_len, int timeout);
-#endif
-
-#endif /* __RTS51X_SD_H */
diff --git a/drivers/staging/rts5139/sd_cprm.c b/drivers/staging/rts5139/sd_cprm.c
deleted file mode 100644
index cede6c07394..00000000000
--- a/drivers/staging/rts5139/sd_cprm.c
+++ /dev/null
@@ -1,1056 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#include <linux/blkdev.h>
-#include <linux/kthread.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-
-#include "debug.h"
-#include "trace.h"
-#include "rts51x.h"
-#include "rts51x_transport.h"
-#include "rts51x_scsi.h"
-#include "rts51x_card.h"
-#include "rts51x_chip.h"
-#include "sd_cprm.h"
-#include "sd.h"
-
-#ifdef SUPPORT_CPRM
-
-static inline int get_rsp_type(u8 rsp_code, u8 *rsp_type, int *rsp_len)
-{
-	if (!rsp_type || !rsp_len)
-		return STATUS_FAIL;
-
-	switch (rsp_code) {
-	case 0x03:
-		*rsp_type = SD_RSP_TYPE_R0; /* no response */
-		*rsp_len = 0;
-		break;
-
-	case 0x04:
-		*rsp_type = SD_RSP_TYPE_R1; /* R1,R6(,R4,R5) */
-		*rsp_len = 6;
-		break;
-
-	case 0x05:
-		*rsp_type = SD_RSP_TYPE_R1b;	/* R1b */
-		*rsp_len = 6;
-		break;
-
-	case 0x06:
-		*rsp_type = SD_RSP_TYPE_R2;	/* R2 */
-		*rsp_len = 17;
-		break;
-
-	case 0x07:
-		*rsp_type = SD_RSP_TYPE_R3;	/* R3 */
-		*rsp_len = 6;
-		break;
-
-	default:
-		return STATUS_FAIL;
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int ext_sd_send_cmd_get_rsp(struct rts51x_chip *chip, u8 cmd_idx,
-			    u32 arg, u8 rsp_type, u8 *rsp, int rsp_len,
-			    int special_check)
-{
-	int retval;
-	int timeout = 50;
-	u16 reg_addr;
-	u8 buf[17], stat;
-	int len = 2;
-	int rty_cnt = 0;
-
-	RTS51X_DEBUGP("EXT SD/MMC CMD %d\n", cmd_idx);
-
-	if (rsp_type == SD_RSP_TYPE_R1b)
-		timeout = 3000;
-
-RTY_SEND_CMD:
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD0, 0xFF, 0x40 | cmd_idx);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD1, 0xFF, (u8) (arg >> 24));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD2, 0xFF, (u8) (arg >> 16));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD3, 0xFF, (u8) (arg >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD4, 0xFF, (u8) arg);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF, rsp_type);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
-		       0x01, PINGPONG_BUFFER);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER,
-		       0xFF, SD_TM_CMD_RSP | SD_TRANSFER_START);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER, SD_TRANSFER_END,
-		       SD_TRANSFER_END);
-
-	rts51x_add_cmd(chip, READ_REG_CMD, SD_STAT1, 0, 0);
-
-	if (CHECK_USB(chip, USB_20)) {
-		if (rsp_type == SD_RSP_TYPE_R2) {
-			for (reg_addr = PPBUF_BASE2;
-			     reg_addr < PPBUF_BASE2 + 16; reg_addr++) {
-				rts51x_add_cmd(chip, READ_REG_CMD, reg_addr, 0,
-					       0);
-			}
-			len = 19;
-		} else if (rsp_type != SD_RSP_TYPE_R0) {
-			/* Read data from SD_CMDx registers */
-			for (reg_addr = SD_CMD0; reg_addr <= SD_CMD4;
-			     reg_addr++) {
-				rts51x_add_cmd(chip, READ_REG_CMD, reg_addr, 0,
-					       0);
-			}
-			len = 8;
-		} else {
-			len = 3;
-		}
-		rts51x_add_cmd(chip, READ_REG_CMD, SD_CMD5, 0, 0);
-	} else {
-		len = 2;
-	}
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, len, timeout);
-
-	if (CHECK_SD_TRANS_FAIL(chip, retval)) {
-		rts51x_clear_sd_error(chip);
-
-		if (retval == STATUS_TIMEDOUT) {
-			if (rsp_type & SD_WAIT_BUSY_END) {
-				retval = sd_check_data0_status(chip);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, retval);
-			}
-		}
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (rsp_type == SD_RSP_TYPE_R0)
-		return STATUS_SUCCESS;
-
-	if (CHECK_USB(chip, USB_20)) {
-		rts51x_read_rsp_buf(chip, 2, buf, len - 2);
-	} else {
-		if (rsp_type == SD_RSP_TYPE_R2) {
-			reg_addr = PPBUF_BASE2;
-			len = 16;
-		} else {
-			reg_addr = SD_CMD0;
-			len = 5;
-		}
-		retval =
-		    rts51x_seq_read_register(chip, reg_addr,
-						     (unsigned short)len, buf);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		RTS51X_READ_REG(chip, SD_CMD5, buf + len);
-	}
-	stat = chip->rsp_buf[1];
-
-	if ((buf[0] & 0xC0) != 0)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	if (!(rsp_type & SD_NO_CHECK_CRC7)) {
-		if (stat & SD_CRC7_ERR) {
-			if (cmd_idx == WRITE_MULTIPLE_BLOCK)
-				TRACE_RET(chip, STATUS_FAIL);
-			if (rty_cnt < SD_MAX_RETRY_COUNT) {
-				wait_timeout(20);
-				rty_cnt++;
-				goto RTY_SEND_CMD;
-			} else {
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-	}
-
-	if ((cmd_idx == SELECT_CARD) || (cmd_idx == APP_CMD) ||
-	    (cmd_idx == SEND_STATUS) || (cmd_idx == STOP_TRANSMISSION)) {
-		if ((cmd_idx != STOP_TRANSMISSION) && (special_check == 0)) {
-			if (buf[1] & 0x80)
-				TRACE_RET(chip, STATUS_FAIL);
-		}
-		if (buf[1] & 0x7F)
-			TRACE_RET(chip, STATUS_FAIL);
-		if (buf[2] & 0xF8)
-			TRACE_RET(chip, STATUS_FAIL);
-
-		if (cmd_idx == SELECT_CARD) {
-			if (rsp_type == SD_RSP_TYPE_R2) {
-				if ((buf[3] & 0x1E) != 0x04)
-					TRACE_RET(chip, STATUS_FAIL);
-			} else if (rsp_type == SD_RSP_TYPE_R2) {
-				if ((buf[3] & 0x1E) != 0x03)
-					TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-	}
-
-	if (rsp && rsp_len)
-		memcpy(rsp, buf, rsp_len);
-
-	return STATUS_SUCCESS;
-}
-
-static int ext_sd_get_rsp(struct rts51x_chip *chip, int len,
-			u8 *rsp, u8 rsp_type)
-{
-	int retval, rsp_len;
-	u16 reg_addr;
-
-	if (rsp_type == SD_RSP_TYPE_R0)
-		return STATUS_SUCCESS;
-
-	rts51x_init_cmd(chip);
-
-	if (rsp_type == SD_RSP_TYPE_R2) {
-		for (reg_addr = PPBUF_BASE2; reg_addr < PPBUF_BASE2 + 16;
-		     reg_addr++) {
-			rts51x_add_cmd(chip, READ_REG_CMD, reg_addr, 0xFF, 0);
-		}
-		rsp_len = 17;
-	} else if (rsp_type != SD_RSP_TYPE_R0) {
-		for (reg_addr = SD_CMD0; reg_addr <= SD_CMD4; reg_addr++)
-			rts51x_add_cmd(chip, READ_REG_CMD, reg_addr, 0xFF, 0);
-		rsp_len = 6;
-	}
-	rts51x_add_cmd(chip, READ_REG_CMD, SD_CMD5, 0xFF, 0);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, rsp_len, 100);
-
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (rsp) {
-		int min_len = (rsp_len < len) ? rsp_len : len;
-
-		memcpy(rsp, rts51x_get_rsp_data(chip), min_len);
-
-		RTS51X_DEBUGP("min_len = %d\n", min_len);
-		RTS51X_DEBUGP("Response in cmd buf: 0x%x 0x%x 0x%x 0x%x\n",
-			       rsp[0], rsp[1], rsp[2], rsp[3]);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-int ext_rts51x_sd_execute_no_data(struct rts51x_chip *chip, unsigned int lun,
-			   u8 cmd_idx, u8 standby, u8 acmd, u8 rsp_code,
-			   u32 arg)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval, rsp_len;
-	u8 rsp_type;
-
-	retval = rts51x_sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, TRANSPORT_FAILED);
-
-	if (sd_card->pre_cmd_err) {
-		sd_card->pre_cmd_err = 0;
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-	retval = get_rsp_type(rsp_code, &rsp_type, &rsp_len);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-	sd_card->last_rsp_type = rsp_type;
-
-	retval = rts51x_sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	/* Set H/W SD/MMC Bus Width */
-	rts51x_write_register(chip, SD_CFG1, 0x03, SD_BUS_WIDTH_4);
-
-	if (standby) {
-		retval = rts51x_sd_select_card(chip, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Cmd_Failed);
-	}
-
-	if (acmd) {
-		retval =
-		    ext_sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
-					    SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Cmd_Failed);
-	}
-
-	retval = ext_sd_send_cmd_get_rsp(chip, cmd_idx, arg, rsp_type,
-					 sd_card->rsp, rsp_len, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_GOTO(chip, SD_Execute_Cmd_Failed);
-
-	if (standby) {
-		retval = rts51x_sd_select_card(chip, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Cmd_Failed);
-	}
-
-	return TRANSPORT_GOOD;
-
-SD_Execute_Cmd_Failed:
-	sd_card->pre_cmd_err = 1;
-	rts51x_set_sense_type(chip, lun, SENSE_TYPE_NO_SENSE);
-	rts51x_release_sd_card(chip);
-	rts51x_do_rts51x_reset_sd_card(chip);
-	if (!(chip->card_ready & SD_CARD))
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-
-	TRACE_RET(chip, TRANSPORT_FAILED);
-}
-
-int ext_rts51x_sd_execute_read_data(struct rts51x_chip *chip, unsigned int lun,
-			     u8 cmd_idx, u8 cmd12, u8 standby,
-			     u8 acmd, u8 rsp_code, u32 arg, u32 data_len,
-			     void *data_buf, unsigned int buf_len, int use_sg)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval, rsp_len, i;
-	int cmd13_checkbit = 0, read_err = 0;
-	u8 rsp_type, bus_width;
-
-	if (sd_card->pre_cmd_err) {
-		sd_card->pre_cmd_err = 0;
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	retval = rts51x_sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-	retval = get_rsp_type(rsp_code, &rsp_type, &rsp_len);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-	sd_card->last_rsp_type = rsp_type;
-
-	retval = rts51x_sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	bus_width = SD_BUS_WIDTH_4;
-
-	if (data_len < 512) {
-		retval = ext_sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, data_len,
-						 SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-	}
-
-	if (standby) {
-		retval = rts51x_sd_select_card(chip, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-	}
-
-	if (acmd) {
-		retval =
-		    ext_sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
-					    SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-	}
-
-	if (data_len <= 512) {
-		int min_len;
-		u8 *buf;
-		u16 byte_cnt, blk_cnt;
-		u8 cmd[5];
-		unsigned int offset = 0;
-		void *sg = NULL;
-
-		byte_cnt = (u16) (data_len & 0x3FF);
-		blk_cnt = 1;
-
-		cmd[0] = 0x40 | cmd_idx;
-		cmd[1] = (u8) (arg >> 24);
-		cmd[2] = (u8) (arg >> 16);
-		cmd[3] = (u8) (arg >> 8);
-		cmd[4] = (u8) arg;
-
-		buf = kmalloc(data_len, GFP_KERNEL);
-		if (buf == NULL)
-			TRACE_RET(chip, TRANSPORT_ERROR);
-
-		retval = sd_read_data(chip, SD_TM_NORMAL_READ, cmd, 5, byte_cnt,
-				      blk_cnt, bus_width, buf, data_len, 2000);
-		if (retval != STATUS_SUCCESS) {
-			read_err = 1;
-			kfree(buf);
-			rts51x_write_register(chip, CARD_STOP,
-					      SD_STOP | SD_CLR_ERR,
-					      SD_STOP | SD_CLR_ERR);
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-		}
-
-		min_len = min(data_len, buf_len);
-		if (use_sg)
-			rts51x_access_sglist(buf, min_len, (void *)data_buf,
-					     &sg, &offset, TO_XFER_BUF);
-		else
-			memcpy(data_buf, buf, min_len);
-
-		kfree(buf);
-	} else if (!(data_len & 0x1FF)) {
-		rts51x_init_cmd(chip);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_H, 0xFF, 0x02);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, 0x00);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H,
-			       0xFF, (u8) (data_len >> 17));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L,
-			       0xFF, (u8) ((data_len & 0x0001FE00) >> 9));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD0, 0xFF,
-			       0x40 | cmd_idx);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD1, 0xFF,
-			       (u8) (arg >> 24));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD2, 0xFF,
-			       (u8) (arg >> 16));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD3, 0xFF,
-			       (u8) (arg >> 8));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD4, 0xFF, (u8) arg);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0x03, bus_width);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF, rsp_type);
-		rts51x_trans_dma_enable(DMA_FROM_DEVICE, chip, data_len, DMA_512);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-			       SD_TM_AUTO_READ_2 | SD_TRANSFER_START);
-		rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER,
-			       SD_TRANSFER_END, SD_TRANSFER_END);
-		retval = rts51x_send_cmd(chip, MODE_CDIR, 100);
-		if (retval != STATUS_SUCCESS) {
-			read_err = 1;
-			rts51x_ep0_write_register(chip, CARD_STOP,
-						  SD_STOP | SD_CLR_ERR,
-						  SD_STOP | SD_CLR_ERR);
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-		}
-
-		retval =
-		    rts51x_transfer_data_rcc(chip, RCV_BULK_PIPE(chip),
-					     data_buf, buf_len, use_sg, NULL,
-					     10000, STAGE_DI);
-		if (retval != STATUS_SUCCESS) {
-			read_err = 1;
-			rts51x_ep0_write_register(chip, CARD_STOP,
-						  SD_STOP | SD_CLR_ERR,
-						  SD_STOP | SD_CLR_ERR);
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-		}
-		retval = rts51x_get_rsp(chip, 1, 500);
-		if (CHECK_SD_TRANS_FAIL(chip, retval)) {
-			read_err = 1;
-			rts51x_ep0_write_register(chip, CARD_STOP,
-						  SD_STOP | SD_CLR_ERR,
-						  SD_STOP | SD_CLR_ERR);
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-		}
-	} else {
-		TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-	}
-
-	retval = ext_sd_get_rsp(chip, rsp_len, sd_card->rsp, rsp_type);
-	if (retval != STATUS_SUCCESS)
-		TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-
-	if (standby) {
-		retval = rts51x_sd_select_card(chip, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-	}
-
-	if (cmd12) {
-		retval = ext_sd_send_cmd_get_rsp(chip, STOP_TRANSMISSION,
-						 0, SD_RSP_TYPE_R1b, NULL, 0,
-						 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-	}
-
-	if (data_len < 512) {
-		retval = ext_sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, 0x200,
-						 SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-
-		rts51x_write_register(chip, SD_BYTE_CNT_H, 0xFF, 0x02);
-		rts51x_write_register(chip, SD_BYTE_CNT_L, 0xFF, 0x00);
-	}
-
-	if (standby || cmd12)
-		cmd13_checkbit = 1;
-
-	for (i = 0; i < 3; i++) {
-		retval =
-		    ext_sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
-					    SD_RSP_TYPE_R1, NULL, 0,
-					    cmd13_checkbit);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (retval != STATUS_SUCCESS)
-		TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-
-	return TRANSPORT_GOOD;
-
-SD_Execute_Read_Cmd_Failed:
-	sd_card->pre_cmd_err = 1;
-	rts51x_set_sense_type(chip, lun, SENSE_TYPE_NO_SENSE);
-	if (read_err)
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-	rts51x_release_sd_card(chip);
-	rts51x_do_rts51x_reset_sd_card(chip);
-	if (!(chip->card_ready & SD_CARD))
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-
-	TRACE_RET(chip, TRANSPORT_FAILED);
-}
-
-int ext_rts51x_sd_execute_write_data(struct rts51x_chip *chip, unsigned int lun,
-			      u8 cmd_idx, u8 cmd12, u8 standby, u8 acmd,
-			      u8 rsp_code, u32 arg, u32 data_len,
-			      void *data_buf, unsigned int buf_len, int use_sg)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval, rsp_len;
-	int cmd13_checkbit = 0, write_err = 0;
-	u8 rsp_type;
-	u32 i;
-
-	if (sd_card->pre_cmd_err) {
-		sd_card->pre_cmd_err = 0;
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	retval = rts51x_sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	retval = get_rsp_type(rsp_code, &rsp_type, &rsp_len);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-	sd_card->last_rsp_type = rsp_type;
-
-	retval = rts51x_sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	rts51x_write_register(chip, SD_CFG1, 0x03, SD_BUS_WIDTH_4);
-
-	if (data_len < 512) {
-		retval = ext_sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, data_len,
-						 SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-	}
-
-	if (standby) {
-		retval = rts51x_sd_select_card(chip, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-	}
-
-	if (acmd) {
-		retval =
-		    ext_sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
-					    SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-	}
-
-	retval = ext_sd_send_cmd_get_rsp(chip, cmd_idx, arg, rsp_type,
-					 sd_card->rsp, rsp_len, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-
-	if (data_len <= 512) {
-		u8 *buf;
-		unsigned int offset = 0;
-		void *sg = NULL;
-
-		buf = kmalloc(data_len, GFP_KERNEL);
-		if (buf == NULL)
-			TRACE_RET(chip, TRANSPORT_ERROR);
-
-		if (use_sg)
-			rts51x_access_sglist(buf, data_len, (void *)data_buf,
-					     &sg, &offset, FROM_XFER_BUF);
-		else
-			memcpy(buf, data_buf, data_len);
-
-
-		if (data_len > 256) {
-			rts51x_init_cmd(chip);
-			for (i = 0; i < 256; i++) {
-				rts51x_add_cmd(chip, WRITE_REG_CMD,
-					       (u16) (PPBUF_BASE2 + i), 0xFF,
-					       buf[i]);
-			}
-			retval = rts51x_send_cmd(chip, MODE_C, 250);
-			if (retval != STATUS_SUCCESS) {
-				kfree(buf);
-				TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-			}
-
-			rts51x_init_cmd(chip);
-			for (i = 256; i < data_len; i++) {
-				rts51x_add_cmd(chip, WRITE_REG_CMD,
-					       (u16) (PPBUF_BASE2 + i), 0xFF,
-					       buf[i]);
-			}
-			retval = rts51x_send_cmd(chip, MODE_C, 250);
-			if (retval != STATUS_SUCCESS) {
-				kfree(buf);
-				TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-			}
-		} else {
-			rts51x_init_cmd(chip);
-			for (i = 0; i < data_len; i++) {
-				rts51x_add_cmd(chip, WRITE_REG_CMD,
-					       (u16) (PPBUF_BASE2 + i), 0xFF,
-					       buf[i]);
-			}
-			retval = rts51x_send_cmd(chip, MODE_C, 250);
-			if (retval != STATUS_SUCCESS) {
-				kfree(buf);
-				TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-			}
-		}
-
-		kfree(buf);
-
-		rts51x_init_cmd(chip);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_H, 0xFF,
-			       (u8) ((data_len >> 8) & 0x03));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF,
-			       (u8) data_len);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H, 0xFF, 0x00);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L, 0xFF, 0x01);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-			       PINGPONG_BUFFER);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-			       SD_TM_AUTO_WRITE_3 | SD_TRANSFER_START);
-		rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER,
-			       SD_TRANSFER_END, SD_TRANSFER_END);
-
-		retval = rts51x_send_cmd(chip, MODE_CR, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-
-		retval = rts51x_get_rsp(chip, 1, 250);
-		if (CHECK_SD_TRANS_FAIL(chip, retval))
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-	} else if (!(data_len & 0x1FF)) {
-		rts51x_init_cmd(chip);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_H, 0xFF, 0x02);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, 0x00);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H,
-			       0xFF, (u8) (data_len >> 17));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L,
-			       0xFF, (u8) ((data_len & 0x0001FE00) >> 9));
-
-		rts51x_trans_dma_enable(DMA_TO_DEVICE, chip, data_len, DMA_512);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-			       SD_TM_AUTO_WRITE_3 | SD_TRANSFER_START);
-		rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER,
-			       SD_TRANSFER_END, SD_TRANSFER_END);
-
-		retval = rts51x_send_cmd(chip, MODE_CDOR, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-
-		retval =
-		    rts51x_transfer_data_rcc(chip, SND_BULK_PIPE(chip),
-					     data_buf, buf_len, use_sg, NULL,
-					     10000, STAGE_DO);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-
-		retval = rts51x_get_rsp(chip, 1, 10000);
-		if (CHECK_SD_TRANS_FAIL(chip, retval))
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-
-	} else {
-		TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-	}
-
-	if (retval < 0) {
-		write_err = 1;
-		rts51x_write_register(chip, CARD_STOP, SD_STOP | SD_CLR_ERR,
-				      SD_STOP | SD_CLR_ERR);
-		TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-	}
-
-	if (standby) {
-		retval = rts51x_sd_select_card(chip, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-	}
-
-	if (cmd12) {
-		retval = ext_sd_send_cmd_get_rsp(chip, STOP_TRANSMISSION,
-						 0, SD_RSP_TYPE_R1b, NULL, 0,
-						 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-	}
-
-	if (data_len < 512) {
-		retval = ext_sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, 0x200,
-						 SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-
-		rts51x_write_register(chip, SD_BYTE_CNT_H, 0xFF, 0x02);
-		rts51x_write_register(chip, SD_BYTE_CNT_L, 0xFF, 0x00);
-	}
-
-	if (cmd12 || standby) {
-		/* There is CMD7 or CMD12 sent before CMD13 */
-		cmd13_checkbit = 1;
-	}
-
-	for (i = 0; i < 3; i++) {
-		retval =
-		    ext_sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
-					    SD_RSP_TYPE_R1, NULL, 0,
-					    cmd13_checkbit);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (retval != STATUS_SUCCESS)
-		TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-
-	return TRANSPORT_GOOD;
-
-SD_Execute_Write_Cmd_Failed:
-	sd_card->pre_cmd_err = 1;
-	rts51x_set_sense_type(chip, lun, SENSE_TYPE_NO_SENSE);
-	if (write_err)
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
-	rts51x_release_sd_card(chip);
-	rts51x_do_rts51x_reset_sd_card(chip);
-	if (!(chip->card_ready & SD_CARD))
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-
-	TRACE_RET(chip, TRANSPORT_FAILED);
-}
-
-int rts51x_sd_pass_thru_mode(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	unsigned int lun = SCSI_LUN(srb);
-	int len;
-	u8 buf[18] = {
-		0x00,
-		0x00,
-		0x00,
-		0x0E,
-		0x00,		/* Version Number */
-		0x00,		/* WP | Media Type */
-		0x00,		/* RCA (Low byte) */
-		0x00,		/* RCA (High byte) */
-		0x53,		/* 'S' */
-		0x44,		/* 'D' */
-		0x20,		/* ' ' */
-		0x43,		/* 'C' */
-		0x61,		/* 'a' */
-		0x72,		/* 'r' */
-		0x64,		/* 'd' */
-		0x00,		/* Max LUN Number */
-		0x00,
-		0x00,
-	};
-
-	sd_card->pre_cmd_err = 0;
-
-	if (!(CHK_BIT(chip->lun_mc, lun))) {
-		SET_BIT(chip->lun_mc, lun);
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if ((0x53 != srb->cmnd[2]) || (0x44 != srb->cmnd[3])
-	    || (0x20 != srb->cmnd[4]) || (0x43 != srb->cmnd[5])
-	    || (0x61 != srb->cmnd[6]) || (0x72 != srb->cmnd[7])
-	    || (0x64 != srb->cmnd[8])) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	switch (srb->cmnd[1] & 0x0F) {
-	case 0:
-		sd_card->sd_pass_thru_en = 0;
-		break;
-
-	case 1:
-		sd_card->sd_pass_thru_en = 1;
-		break;
-
-	default:
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	/* 0x01:SD Memory Card; 0x02:Other Media; 0x03:Illegal Media; */
-	buf[5] = (1 == CHK_SD(sd_card)) ? 0x01 : 0x02;
-	if (chip->card_wp & SD_CARD)
-		buf[5] |= 0x80;
-
-	buf[6] = (u8) (sd_card->sd_addr >> 16);
-	buf[7] = (u8) (sd_card->sd_addr >> 24);
-
-	buf[15] = chip->max_lun;
-
-	len = min_t(unsigned, 18, scsi_bufflen(srb));
-	rts51x_set_xfer_buf(buf, len, srb);
-
-	return TRANSPORT_GOOD;
-}
-
-int rts51x_sd_execute_no_data(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	unsigned int lun = SCSI_LUN(srb);
-	int retval;
-	u8 cmd_idx, rsp_code;
-	u8 standby = 0, acmd = 0;
-	u32 arg;
-
-	if (!sd_card->sd_pass_thru_en) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	cmd_idx = srb->cmnd[2] & 0x3F;
-	if (srb->cmnd[1] & 0x02)
-		standby = 1;
-	if (srb->cmnd[1] & 0x01)
-		acmd = 1;
-
-	arg = ((u32) srb->cmnd[3] << 24) | ((u32) srb->cmnd[4] << 16) |
-	    ((u32) srb->cmnd[5] << 8) | srb->cmnd[6];
-
-	rsp_code = srb->cmnd[10];
-
-	retval =
-	    ext_rts51x_sd_execute_no_data(chip, lun, cmd_idx, standby, acmd, rsp_code,
-				   arg);
-	scsi_set_resid(srb, 0);
-	return retval;
-}
-
-int rts51x_sd_execute_read_data(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	unsigned int lun = SCSI_LUN(srb);
-	u8 cmd_idx, rsp_code, send_cmd12 = 0, standby = 0, acmd = 0;
-	u32 arg, data_len;
-
-	if (!sd_card->sd_pass_thru_en) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	cmd_idx = srb->cmnd[2] & 0x3F;
-	if (srb->cmnd[1] & 0x04)
-		send_cmd12 = 1;
-	if (srb->cmnd[1] & 0x02)
-		standby = 1;
-	if (srb->cmnd[1] & 0x01)
-		acmd = 1;
-
-	arg = ((u32) srb->cmnd[3] << 24) | ((u32) srb->cmnd[4] << 16) |
-	    ((u32) srb->cmnd[5] << 8) | srb->cmnd[6];
-
-	data_len =
-	    ((u32) srb->cmnd[7] << 16) | ((u32) srb->cmnd[8] << 8) |
-	    srb->cmnd[9];
-	rsp_code = srb->cmnd[10];
-
-	retval =
-	    ext_rts51x_sd_execute_read_data(chip, lun, cmd_idx, send_cmd12, standby,
-				     acmd, rsp_code, arg, data_len,
-				     scsi_sglist(srb), scsi_bufflen(srb),
-				     scsi_sg_count(srb));
-	scsi_set_resid(srb, 0);
-	return retval;
-}
-
-int rts51x_sd_execute_write_data(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	unsigned int lun = SCSI_LUN(srb);
-	u8 cmd_idx, rsp_code, send_cmd12 = 0, standby = 0, acmd = 0;
-	u32 data_len, arg;
-
-	if (!sd_card->sd_pass_thru_en) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	cmd_idx = srb->cmnd[2] & 0x3F;
-	if (srb->cmnd[1] & 0x04)
-		send_cmd12 = 1;
-	if (srb->cmnd[1] & 0x02)
-		standby = 1;
-	if (srb->cmnd[1] & 0x01)
-		acmd = 1;
-
-	data_len =
-	    ((u32) srb->cmnd[7] << 16) | ((u32) srb->cmnd[8] << 8) |
-	    srb->cmnd[9];
-	arg =
-	    ((u32) srb->cmnd[3] << 24) | ((u32) srb->cmnd[4] << 16) |
-	    ((u32) srb->cmnd[5] << 8) | srb->cmnd[6];
-	rsp_code = srb->cmnd[10];
-
-	retval =
-	    ext_rts51x_sd_execute_write_data(chip, lun, cmd_idx, send_cmd12, standby,
-				      acmd, rsp_code, arg, data_len,
-				      scsi_sglist(srb), scsi_bufflen(srb),
-				      scsi_sg_count(srb));
-	scsi_set_resid(srb, 0);
-	return retval;
-}
-
-int rts51x_sd_get_cmd_rsp(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	unsigned int lun = SCSI_LUN(srb);
-	int count;
-	u16 data_len;
-
-	if (!sd_card->sd_pass_thru_en) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (sd_card->pre_cmd_err) {
-		sd_card->pre_cmd_err = 0;
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	data_len = ((u16) srb->cmnd[7] << 8) | srb->cmnd[8];
-
-	if (sd_card->last_rsp_type == SD_RSP_TYPE_R0) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	} else if (sd_card->last_rsp_type == SD_RSP_TYPE_R2) {
-		count = (data_len < 17) ? data_len : 17;
-	} else {
-		count = (data_len < 6) ? data_len : 6;
-	}
-	rts51x_set_xfer_buf(sd_card->rsp, count, srb);
-
-	RTS51X_DEBUGP("Response length: %d\n", data_len);
-	RTS51X_DEBUGP("Response: 0x%x 0x%x 0x%x 0x%x\n",
-		       sd_card->rsp[0], sd_card->rsp[1], sd_card->rsp[2],
-		       sd_card->rsp[3]);
-
-	scsi_set_resid(srb, 0);
-	return TRANSPORT_GOOD;
-}
-
-int rts51x_sd_hw_rst(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	unsigned int lun = SCSI_LUN(srb);
-	int retval;
-
-	if (!sd_card->sd_pass_thru_en) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (sd_card->pre_cmd_err) {
-		sd_card->pre_cmd_err = 0;
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if ((0x53 != srb->cmnd[2]) || (0x44 != srb->cmnd[3])
-	    || (0x20 != srb->cmnd[4]) || (0x43 != srb->cmnd[5])
-	    || (0x61 != srb->cmnd[6]) || (0x72 != srb->cmnd[7])
-	    || (0x64 != srb->cmnd[8])) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	switch (srb->cmnd[1] & 0x0F) {
-	case 0:
-		/* SD Card Power Off -> ON and Initialization */
-		retval = rts51x_reset_sd_card(chip);
-		if (retval != STATUS_SUCCESS) {
-			rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-			sd_card->pre_cmd_err = 1;
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		break;
-
-	case 1:
-		/* reset CMD(CMD0) and Initialization
-		 * (without SD Card Power Off -> ON) */
-		retval = reset_sd(chip);
-		if (retval != STATUS_SUCCESS) {
-			rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-			sd_card->pre_cmd_err = 1;
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		break;
-
-	default:
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	scsi_set_resid(srb, 0);
-	return TRANSPORT_GOOD;
-}
-#endif
diff --git a/drivers/staging/rts5139/sd_cprm.h b/drivers/staging/rts5139/sd_cprm.h
deleted file mode 100644
index 79dfd27db41..00000000000
--- a/drivers/staging/rts5139/sd_cprm.h
+++ /dev/null
@@ -1,54 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_SD_CPRM_H
-#define __RTS51X_SD_CPRM_H
-
-#include "rts51x_chip.h"
-#include "sd.h"
-
-#ifdef SUPPORT_CPRM
-int ext_rts51x_sd_execute_no_data(struct rts51x_chip *chip, unsigned int lun,
-			   u8 cmd_idx, u8 standby, u8 acmd, u8 rsp_code,
-			   u32 arg);
-int ext_rts51x_sd_execute_read_data(struct rts51x_chip *chip, unsigned int lun,
-			     u8 cmd_idx, u8 cmd12, u8 standby, u8 acmd,
-			     u8 rsp_code, u32 arg, u32 data_len, void *data_buf,
-			     unsigned int buf_len, int use_sg);
-int ext_rts51x_sd_execute_write_data(struct rts51x_chip *chip, unsigned int lun,
-			      u8 cmd_idx, u8 cmd12, u8 standby, u8 acmd,
-			      u8 rsp_code, u32 arg, u32 data_len,
-			      void *data_buf, unsigned int buf_len, int use_sg);
-
-int rts51x_sd_pass_thru_mode(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-int rts51x_sd_execute_no_data(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-int rts51x_sd_execute_read_data(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-int rts51x_sd_execute_write_data(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-int rts51x_sd_get_cmd_rsp(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-int rts51x_sd_hw_rst(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-#endif
-
-#endif /* __RTS51X_SD_CPRM_H */
diff --git a/drivers/staging/rts5139/trace.h b/drivers/staging/rts5139/trace.h
deleted file mode 100644
index ac58b452ecb..00000000000
--- a/drivers/staging/rts5139/trace.h
+++ /dev/null
@@ -1,95 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_TRACE_H
-#define __RTS51X_TRACE_H
-
-#include <linux/string.h>
-
-#include "debug.h"
-
-#define _MSG_TRACE
-
-#ifdef _MSG_TRACE
-#define TRACE_RET(chip, ret)						\
-do {									\
-	const char *_file = kbasename(__FILE__);			\
-	RTS51X_DEBUGP("[%s][%s]:[%d]\n", _file, __func__, __LINE__);	\
-	(chip)->trace_msg[(chip)->msg_idx].line = (u16)(__LINE__);	\
-	strncpy((chip)->trace_msg[(chip)->msg_idx].func,		\
-			__func__, MSG_FUNC_LEN-1);			\
-	strncpy((chip)->trace_msg[(chip)->msg_idx].file,		\
-			_file, MSG_FILE_LEN-1);				\
-	get_current_time((chip)->trace_msg[(chip)->msg_idx].timeval_buf,\
-			TIME_VAL_LEN);	\
-	(chip)->trace_msg[(chip)->msg_idx].valid = 1;			\
-	(chip)->msg_idx++;						\
-	if ((chip)->msg_idx >= TRACE_ITEM_CNT) {			\
-		(chip)->msg_idx = 0;					\
-	}								\
-	return ret;							\
-} while (0)
-
-#define TRACE_GOTO(chip, label)						\
-do {									\
-	const char *_file = kbasename(__FILE__);			\
-	RTS51X_DEBUGP("[%s][%s]:[%d]\n", _file, __func__, __LINE__);	\
-	(chip)->trace_msg[(chip)->msg_idx].line = (u16)(__LINE__);	\
-	strncpy((chip)->trace_msg[(chip)->msg_idx].func,		\
-			__func__, MSG_FUNC_LEN-1);			\
-	strncpy((chip)->trace_msg[(chip)->msg_idx].file,		\
-			_file, MSG_FILE_LEN-1);				\
-	get_current_time((chip)->trace_msg[(chip)->msg_idx].timeval_buf,\
-			TIME_VAL_LEN);					\
-	(chip)->trace_msg[(chip)->msg_idx].valid = 1;			\
-	(chip)->msg_idx++;						\
-	if ((chip)->msg_idx >= TRACE_ITEM_CNT) {			\
-		(chip)->msg_idx = 0;					\
-	}								\
-	goto label;							\
-} while (0)
-#else
-#define TRACE_RET(chip, ret)	return (ret)
-#define TRACE_GOTO(chip, label)	goto label
-#endif
-
-#ifdef CONFIG_RTS5139_DEBUG
-#define RTS51X_DUMP(buf, buf_len)					\
-	print_hex_dump(KERN_DEBUG, RTS51X_TIP, DUMP_PREFIX_NONE,	\
-				16, 1, (buf), (buf_len), false)
-
-#define CATCH_TRIGGER(chip)					\
-do {								\
-	rts51x_ep0_write_register((chip), 0xFC31, 0x01, 0x01);	\
-	RTS51X_DEBUGP("Catch trigger!\n");			\
-} while (0)
-
-#else
-#define RTS51X_DUMP(buf, buf_len)
-#define CATCH_TRIGGER(chip)
-#endif
-
-#endif /* __RTS51X_TRACE_H */
diff --git a/drivers/staging/rts5139/xd.c b/drivers/staging/rts5139/xd.c
deleted file mode 100644
index be432351be8..00000000000
--- a/drivers/staging/rts5139/xd.c
+++ /dev/null
@@ -1,2145 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#include <linux/blkdev.h>
-#include <linux/kthread.h>
-#include <linux/sched.h>
-#include <linux/vmalloc.h>
-
-#include "debug.h"
-#include "trace.h"
-#include "rts51x.h"
-#include "rts51x_transport.h"
-#include "rts51x_scsi.h"
-#include "rts51x_card.h"
-#include "xd.h"
-
-static int xd_build_l2p_tbl(struct rts51x_chip *chip, int zone_no);
-static int xd_init_page(struct rts51x_chip *chip, u32 phy_blk, u16 logoff,
-			u8 start_page, u8 end_page);
-
-static inline void xd_set_err_code(struct rts51x_chip *chip, u8 err_code)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-
-	xd_card->err_code = err_code;
-}
-
-static int xd_set_init_para(struct rts51x_chip *chip)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	int retval;
-
-	if (chip->asic_code)
-		xd_card->xd_clock = 47;
-	else
-		xd_card->xd_clock = CLK_50;
-
-	retval = switch_clock(chip, xd_card->xd_clock);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_switch_clock(struct rts51x_chip *chip)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	int retval;
-
-	retval = rts51x_select_card(chip, XD_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = switch_clock(chip, xd_card->xd_clock);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_read_id(struct rts51x_chip *chip, u8 id_cmd, u8 *id_buf,
-		      u8 buf_len)
-{
-	int retval, i;
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_DAT, 0xFF, id_cmd);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-		       XD_TRANSFER_START | XD_READ_ID);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER, XD_TRANSFER_END,
-		       XD_TRANSFER_END);
-
-	for (i = 0; i < 4; i++) {
-		rts51x_add_cmd(chip, READ_REG_CMD, (u16) (XD_ADDRESS1 + i), 0,
-			       0);
-	}
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 20);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 5, 20);
-
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_xd_error(chip);
-		TRACE_RET(chip, retval);
-	}
-
-	if (id_buf && buf_len) {
-		if (buf_len > 4)
-			buf_len = 4;
-		rts51x_read_rsp_buf(chip, 1, id_buf, buf_len);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static void xd_assign_phy_addr(struct rts51x_chip *chip, u32 addr, u8 mode)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-
-	switch (mode) {
-	case XD_RW_ADDR:
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_ADDRESS0, 0xFF, 0);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_ADDRESS1, 0xFF,
-			       (u8) addr);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_ADDRESS2, 0xFF,
-			       (u8) (addr >> 8));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_ADDRESS3, 0xFF,
-			       (u8) (addr >> 16));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CFG, 0xFF,
-			       xd_card->addr_cycle | XD_CALC_ECC |
-			       XD_BA_NO_TRANSFORM);
-		break;
-
-	case XD_ERASE_ADDR:
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_ADDRESS0, 0xFF,
-			       (u8) addr);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_ADDRESS1, 0xFF,
-			       (u8) (addr >> 8));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_ADDRESS2, 0xFF,
-			       (u8) (addr >> 16));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CFG, 0xFF,
-			       (xd_card->addr_cycle - 1) |
-			       XD_CALC_ECC | XD_BA_NO_TRANSFORM);
-		break;
-
-	default:
-		break;
-	}
-}
-
-static int xd_read_redundant(struct rts51x_chip *chip, u32 page_addr, u8 *buf,
-			     int buf_len)
-{
-	int retval, i;
-
-	rts51x_init_cmd(chip);
-
-	xd_assign_phy_addr(chip, page_addr, XD_RW_ADDR);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-		       XD_TRANSFER_START | XD_READ_REDUNDANT);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER, XD_TRANSFER_END,
-		       XD_TRANSFER_END);
-
-	for (i = 0; i < 6; i++) {
-		rts51x_add_cmd(chip, READ_REG_CMD, (u16) (XD_PAGE_STATUS + i),
-			       0, 0);
-	}
-	for (i = 0; i < 4; i++) {
-		rts51x_add_cmd(chip, READ_REG_CMD, (u16) (XD_RESERVED0 + i), 0,
-			       0);
-	}
-	rts51x_add_cmd(chip, READ_REG_CMD, XD_PARITY, 0, 0);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 11, 500);
-
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_xd_error(chip);
-		TRACE_RET(chip, retval);
-	}
-
-	if (buf && buf_len) {
-		if (buf_len > 11)
-			buf_len = 11;
-		rts51x_read_rsp_buf(chip, 1, buf, buf_len);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_read_data_from_ppb(struct rts51x_chip *chip, int offset, u8 *buf,
-				 int buf_len)
-{
-	int retval, i;
-
-	if (!buf || (buf_len <= 0))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	rts51x_init_cmd(chip);
-
-	for (i = 0; i < buf_len; i++) {
-		rts51x_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + offset + i, 0,
-			       0);
-	}
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, buf_len, 200);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	rts51x_read_rsp_buf(chip, 0, buf, buf_len);
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_read_cis(struct rts51x_chip *chip, u32 page_addr, u8 *buf,
-		       int buf_len)
-{
-	int retval;
-	u8 reg;
-
-	if (!buf || (buf_len < 10))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	rts51x_init_cmd(chip);
-
-	xd_assign_phy_addr(chip, page_addr, XD_RW_ADDR);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       PINGPONG_BUFFER);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_CNT, 0xFF, 1);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CHK_DATA_STATUS,
-		       XD_AUTO_CHK_DATA_STATUS, XD_AUTO_CHK_DATA_STATUS);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-		       XD_TRANSFER_START | XD_READ_PAGES);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER,
-		       XD_TRANSFER_END | XD_PPB_EMPTY,
-		       XD_TRANSFER_END | XD_PPB_EMPTY);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 1, 500);
-	if (retval == STATUS_TIMEDOUT) {
-		rts51x_clear_xd_error(chip);
-		TRACE_RET(chip, retval);
-	}
-
-	RTS51X_READ_REG(chip, XD_PAGE_STATUS, &reg);
-	if (reg != XD_GPG) {
-		rts51x_clear_xd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	RTS51X_READ_REG(chip, XD_CTL, &reg);
-
-	if (!(reg & XD_ECC1_ERROR) || !(reg & XD_ECC1_UNCORRECTABLE)) {
-		retval = xd_read_data_from_ppb(chip, 0, buf, buf_len);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		if (reg & XD_ECC1_ERROR) {	/* correctable error */
-			u8 ecc_bit, ecc_byte;
-
-			RTS51X_READ_REG(chip, XD_ECC_BIT1, &ecc_bit);
-			RTS51X_READ_REG(chip, XD_ECC_BYTE1, &ecc_byte);
-
-			RTS51X_DEBUGP("ECC_BIT1 = 0x%x, ECC_BYTE1 = 0x%x\n",
-				       ecc_bit, ecc_byte);
-			if (ecc_byte < buf_len) {
-				RTS51X_DEBUGP("Before correct: 0x%x\n",
-					       buf[ecc_byte]);
-				buf[ecc_byte] ^= (1 << ecc_bit);
-				RTS51X_DEBUGP("After correct: 0x%x\n",
-					       buf[ecc_byte]);
-			}
-		}
-	} else if (!(reg & XD_ECC2_ERROR) || !(reg & XD_ECC2_UNCORRECTABLE)) {
-		RTS51X_WRITE_REG(chip, CARD_STOP, XD_STOP | XD_CLR_ERR,
-				 XD_STOP | XD_CLR_ERR);
-
-		retval = xd_read_data_from_ppb(chip, 256, buf, buf_len);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		if (reg & XD_ECC2_ERROR) {
-			u8 ecc_bit, ecc_byte;
-
-			RTS51X_READ_REG(chip, XD_ECC_BIT2, &ecc_bit);
-			RTS51X_READ_REG(chip, XD_ECC_BYTE2, &ecc_byte);
-
-			RTS51X_DEBUGP("ECC_BIT2 = 0x%x, ECC_BYTE2 = 0x%x\n",
-				       ecc_bit, ecc_byte);
-			if (ecc_byte < buf_len) {
-				RTS51X_DEBUGP("Before correct: 0x%x\n",
-					       buf[ecc_byte]);
-				buf[ecc_byte] ^= (1 << ecc_bit);
-				RTS51X_DEBUGP("After correct: 0x%x\n",
-					       buf[ecc_byte]);
-			}
-		}
-	} else {
-		rts51x_clear_xd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static void xd_pull_ctl_disable(struct rts51x_chip *chip)
-{
-	if (CHECK_PKG(chip, LQFP48)) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0xA5);
-	} else {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x65);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x56);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0x59);
-	}
-}
-
-static void xd_pull_ctl_enable(struct rts51x_chip *chip)
-{
-	if (CHECK_PKG(chip, LQFP48)) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0xAA);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0xA5);
-	} else {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0xA5);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x59);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0x59);
-	}
-}
-
-static int reset_xd(struct rts51x_chip *chip)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	int retval, i, j;
-	u8 id_buf[4], redunt[11];
-
-	retval = rts51x_select_card(chip, XD_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CHK_DATA_STATUS, 0xFF,
-		       XD_PGSTS_NOT_FF);
-	if (chip->asic_code)
-		xd_pull_ctl_disable(chip);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, FPGA_PULL_CTL, 0xFF,
-			       (FPGA_XD_PULL_CTL_EN1 & FPGA_XD_PULL_CTL_EN3));
-
-	if (!chip->option.FT2_fast_mode) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_INIT, XD_NO_AUTO_PWR_OFF,
-			       0);
-		if (CHECK_PKG(chip, LQFP48) ||
-				chip->option.rts5129_D3318_off_enable) {
-			rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL,
-				       DV3318_AUTO_PWR_OFF,
-				       DV3318_AUTO_PWR_OFF);
-		}
-	}
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_OE, XD_OUTPUT_EN, 0);
-	if (!chip->option.FT2_fast_mode) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, POWER_MASK,
-			       POWER_OFF);
-	}
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	if (!chip->option.FT2_fast_mode) {
-#ifdef SD_XD_IO_FOLLOW_PWR
-		if (CHECK_PKG(chip, LQFP48)
-		    || chip->option.rts5129_D3318_off_enable) {
-			rts51x_write_register(chip, CARD_PWR_CTL,
-					LDO_OFF, LDO_OFF);
-		}
-#endif
-
-		wait_timeout(250);
-
-#ifdef SD_XD_IO_FOLLOW_PWR
-		if (CHECK_PKG(chip, LQFP48)
-		    || chip->option.rts5129_D3318_off_enable) {
-			rts51x_init_cmd(chip);
-			if (chip->asic_code) {
-				xd_pull_ctl_enable(chip);
-			} else {
-				rts51x_add_cmd(chip, WRITE_REG_CMD,
-					FPGA_PULL_CTL, 0xFF,
-					(FPGA_XD_PULL_CTL_EN1 &
-						FPGA_XD_PULL_CTL_EN2));
-			}
-			retval = rts51x_send_cmd(chip, MODE_C, 100);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
-		}
-#endif
-
-		retval = rts51x_card_power_on(chip, XD_CARD);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-#ifdef SUPPORT_OCP
-		wait_timeout(50);
-		rts51x_get_card_status(chip, &(chip->card_status));
-		chip->ocp_stat = (chip->card_status >> 4) & 0x03;
-
-		if (chip->ocp_stat & (MS_OCP_NOW | MS_OCP_EVER)) {
-			RTS51X_DEBUGP("Over current, OCPSTAT is 0x%x\n",
-				       chip->ocp_stat);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-#endif
-	}
-
-	rts51x_init_cmd(chip);
-
-	if (chip->asic_code)
-		xd_pull_ctl_enable(chip);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, FPGA_PULL_CTL, 0xFF,
-			       (FPGA_XD_PULL_CTL_EN1 & FPGA_XD_PULL_CTL_EN2));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_OE, XD_OUTPUT_EN,
-		       XD_OUTPUT_EN);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CTL, XD_CE_DISEN, XD_CE_DISEN);
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	if (!chip->option.FT2_fast_mode)
-		wait_timeout(200);
-
-	retval = xd_set_init_para(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-	/* Read ID to check if the timing setting is right */
-	for (i = 0; i < 4; i++) {
-		u8 xd_dat, xd_ctl;
-
-		if (monitor_card_cd(chip, XD_CARD) == CD_NOT_EXIST)
-			TRACE_RET(chip, STATUS_FAIL);
-
-		rts51x_init_cmd(chip);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_DTCTL, 0xFF,
-			XD_TIME_SETUP_STEP * 3 + XD_TIME_RW_STEP *
-			(2 + i + chip->option.rts51x_xd_rw_step)
-			+ XD_TIME_RWN_STEP *
-			(i + chip->option.rts51x_xd_rwn_step));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CATCTL, 0xFF,
-			XD_TIME_SETUP_STEP * 3 + XD_TIME_RW_STEP * (4 +
-			i) + XD_TIME_RWN_STEP * (3 + i));
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-			       XD_TRANSFER_START | XD_RESET);
-		rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER,
-			       XD_TRANSFER_END, XD_TRANSFER_END);
-
-		rts51x_add_cmd(chip, READ_REG_CMD, XD_DAT, 0, 0);
-		rts51x_add_cmd(chip, READ_REG_CMD, XD_CTL, 0, 0);
-
-		retval = rts51x_send_cmd(chip, MODE_CR, 100);
-		if (retval != STATUS_SUCCESS) {
-			rts51x_clear_xd_error(chip);
-			TRACE_RET(chip, retval);
-		}
-
-		retval = rts51x_get_rsp(chip, 3, 100);
-		if (retval != STATUS_SUCCESS) {
-			rts51x_clear_xd_error(chip);
-			TRACE_RET(chip, retval);
-		}
-
-		xd_dat = chip->rsp_buf[1];
-		xd_ctl = chip->rsp_buf[2];
-		RTS51X_DEBUGP("XD_DAT: 0x%x, XD_CTL: 0x%x\n", xd_dat, xd_ctl);
-
-		if (((xd_dat & READY_FLAG) != READY_STATE)
-		    || !(xd_ctl & XD_RDY))
-			continue;
-
-		retval = xd_read_id(chip, READ_ID, id_buf, 4);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		RTS51X_DEBUGP("READ_ID: 0x%x 0x%x 0x%x 0x%x\n",
-			       id_buf[0], id_buf[1], id_buf[2], id_buf[3]);
-
-		xd_card->device_code = id_buf[1];
-
-		switch (xd_card->device_code) {
-		case XD_4M_X8_512_1:
-		case XD_4M_X8_512_2:
-			xd_card->block_shift = 4;	/* 16 pages per block */
-			xd_card->page_off = 0x0F;
-			xd_card->addr_cycle = 3;
-			xd_card->zone_cnt = 1;
-			xd_card->capacity = 8000;	/* 500 * 2 ^ 4 */
-			XD_SET_4MB(xd_card);
-			break;
-		case XD_8M_X8_512:
-			xd_card->block_shift = 4;
-			xd_card->page_off = 0x0F;
-			xd_card->addr_cycle = 3;
-			xd_card->zone_cnt = 1;
-			xd_card->capacity = 16000;	/* 1000 * 2 ^ 4 */
-			break;
-		case XD_16M_X8_512:
-			XD_PAGE_512(xd_card);	/* 32 pages per block */
-			xd_card->addr_cycle = 3;
-			xd_card->zone_cnt = 1;
-			xd_card->capacity = 32000;	/* 1000 * 2 ^ 5 */
-			break;
-		case XD_32M_X8_512:
-			XD_PAGE_512(xd_card);
-			xd_card->addr_cycle = 3;
-			xd_card->zone_cnt = 2;
-			xd_card->capacity = 64000;	/* 2000 * 2 ^ 5 */
-			break;
-		case XD_64M_X8_512:
-			XD_PAGE_512(xd_card);
-			xd_card->addr_cycle = 4;
-			xd_card->zone_cnt = 4;
-			xd_card->capacity = 128000;	/* 4000 * 2 ^ 5 */
-			break;
-		case XD_128M_X8_512:
-			XD_PAGE_512(xd_card);
-			xd_card->addr_cycle = 4;
-			xd_card->zone_cnt = 8;
-			xd_card->capacity = 256000;	/* 8000 * 2 ^ 5 */
-			break;
-		case XD_256M_X8_512:
-			XD_PAGE_512(xd_card);
-			xd_card->addr_cycle = 4;
-			xd_card->zone_cnt = 16;
-			xd_card->capacity = 512000;	/* 16000 * 2 ^ 5 */
-			break;
-		case XD_512M_X8:
-			XD_PAGE_512(xd_card);
-			xd_card->addr_cycle = 4;
-			xd_card->zone_cnt = 32;
-			xd_card->capacity = 1024000;	/* 32000 * 2 ^ 5 */
-			break;
-		case xD_1G_X8_512:
-			XD_PAGE_512(xd_card);
-			xd_card->addr_cycle = 4;
-			xd_card->zone_cnt = 64;
-			xd_card->capacity = 2048000;	/* 64000 * 2 ^ 5 */
-			break;
-		case xD_2G_X8_512:
-			XD_PAGE_512(xd_card);
-			xd_card->addr_cycle = 4;
-			xd_card->zone_cnt = 128;
-			xd_card->capacity = 4096000;	/* 128000 * 2 ^ 5 */
-			break;
-		default:
-			continue;
-		}
-
-		/* Confirm timing setting */
-		for (j = 0; j < 10; j++) {
-			retval = xd_read_id(chip, READ_ID, id_buf, 4);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-
-			if (id_buf[1] != xd_card->device_code)
-				break;
-		}
-
-		/* Current timing pass */
-		if (j == 10)
-			break;
-	}
-
-	if (i == 4) {
-		xd_card->block_shift = 0;
-		xd_card->page_off = 0;
-		xd_card->addr_cycle = 0;
-		xd_card->capacity = 0;
-
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	retval = xd_read_id(chip, READ_xD_ID, id_buf, 4);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	RTS51X_DEBUGP("READ_xD_ID: 0x%x 0x%x 0x%x 0x%x\n",
-		       id_buf[0], id_buf[1], id_buf[2], id_buf[3]);
-	if (id_buf[2] != XD_ID_CODE)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	/* Search CIS block */
-	for (i = 0; i < 24; i++) {
-		u32 page_addr;
-
-		if (monitor_card_cd(chip, XD_CARD) == CD_NOT_EXIST)
-			TRACE_RET(chip, STATUS_FAIL);
-
-		page_addr = (u32) i << xd_card->block_shift;
-
-		for (j = 0; j < 3; j++) {
-			retval = xd_read_redundant(chip, page_addr, redunt, 11);
-			if (retval == STATUS_SUCCESS)
-				break;
-		}
-		if (j == 3)
-			continue;
-
-		if (redunt[BLOCK_STATUS] != XD_GBLK)
-			continue;
-
-		j = 0;
-		/* Check page status */
-		if (redunt[PAGE_STATUS] != XD_GPG) {
-			for (j = 1; j <= 8; j++) {
-				retval =
-				    xd_read_redundant(chip, page_addr + j,
-						      redunt, 11);
-				if (retval == STATUS_SUCCESS) {
-					if (redunt[PAGE_STATUS] == XD_GPG)
-						break;
-				}
-			}
-
-			if (j == 9)
-				break;
-		}
-
-		if ((redunt[BLOCK_STATUS] == XD_GBLK)
-		    && (redunt[PARITY] & XD_BA1_ALL0)) {
-			u8 buf[10];
-
-			page_addr += j;
-
-			retval = xd_read_cis(chip, page_addr, buf, 10);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-
-			if ((buf[0] == 0x01) && (buf[1] == 0x03)
-			    && (buf[2] == 0xD9)
-			    && (buf[3] == 0x01) && (buf[4] == 0xFF)
-			    && (buf[5] == 0x18) && (buf[6] == 0x02)
-			    && (buf[7] == 0xDF) && (buf[8] == 0x01)
-			    && (buf[9] == 0x20)) {
-				xd_card->cis_block = (u16) i;
-			}
-		}
-
-		break;
-	}
-
-	RTS51X_DEBUGP("CIS block: 0x%x\n", xd_card->cis_block);
-	if (xd_card->cis_block == 0xFFFF)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	chip->capacity[chip->card2lun[XD_CARD]] = xd_card->capacity;
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_check_data_blank(u8 *redunt)
-{
-	int i;
-
-	for (i = 0; i < 6; i++) {
-		if (redunt[PAGE_STATUS + i] != 0xFF)
-			return 0;
-	}
-
-	if ((redunt[PARITY] & (XD_ECC1_ALL1 | XD_ECC2_ALL1)) !=
-	    (XD_ECC1_ALL1 | XD_ECC2_ALL1))
-		return 0;
-
-	for (i = 0; i < 4; i++) {
-		if (redunt[RESERVED0 + i] != 0xFF)
-			return 0;
-	}
-
-	return 1;
-}
-
-static u16 xd_load_log_block_addr(u8 *redunt)
-{
-	u16 addr = 0xFFFF;
-
-	if (redunt[PARITY] & XD_BA1_BA2_EQL)
-		addr =
-		    ((u16) redunt[BLOCK_ADDR1_H] << 8) | redunt[BLOCK_ADDR1_L];
-	else if (redunt[PARITY] & XD_BA1_VALID)
-		addr =
-		    ((u16) redunt[BLOCK_ADDR1_H] << 8) | redunt[BLOCK_ADDR1_L];
-	else if (redunt[PARITY] & XD_BA2_VALID)
-		addr =
-		    ((u16) redunt[BLOCK_ADDR2_H] << 8) | redunt[BLOCK_ADDR2_L];
-
-	return addr;
-}
-
-static int xd_init_l2p_tbl(struct rts51x_chip *chip)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	int size, i;
-
-	RTS51X_DEBUGP("xd_init_l2p_tbl: zone_cnt = %d\n", xd_card->zone_cnt);
-
-	if (xd_card->zone_cnt < 1)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	size = xd_card->zone_cnt * sizeof(struct zone_entry);
-	RTS51X_DEBUGP("Buffer size for l2p table is %d\n", size);
-
-	xd_card->zone = vmalloc(size);
-	if (!xd_card->zone)
-		TRACE_RET(chip, STATUS_NOMEM);
-
-	for (i = 0; i < xd_card->zone_cnt; i++) {
-		xd_card->zone[i].build_flag = 0;
-		xd_card->zone[i].l2p_table = NULL;
-		xd_card->zone[i].free_table = NULL;
-		xd_card->zone[i].get_index = 0;
-		xd_card->zone[i].set_index = 0;
-		xd_card->zone[i].unused_blk_cnt = 0;
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static inline void free_zone(struct zone_entry *zone)
-{
-	RTS51X_DEBUGP("free_zone\n");
-	if (!zone)
-		return;
-	zone->build_flag = 0;
-	zone->set_index = 0;
-	zone->get_index = 0;
-	zone->unused_blk_cnt = 0;
-	if (zone->l2p_table) {
-		vfree(zone->l2p_table);
-		zone->l2p_table = NULL;
-	}
-	if (zone->free_table) {
-		vfree(zone->free_table);
-		zone->free_table = NULL;
-	}
-}
-
-static void xd_set_unused_block(struct rts51x_chip *chip, u32 phy_blk)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	struct zone_entry *zone;
-	int zone_no;
-
-	zone_no = (int)phy_blk >> 10;
-	if (zone_no >= xd_card->zone_cnt) {
-		RTS51X_DEBUGP("Set unused block to invalid zone"
-					"(zone_no = %d, zone_cnt = %d)\n",
-					zone_no, xd_card->zone_cnt);
-		return;
-	}
-	zone = &(xd_card->zone[zone_no]);
-
-	if (zone->free_table == NULL) {
-		if (xd_build_l2p_tbl(chip, zone_no) != STATUS_SUCCESS)
-			return;
-	}
-
-	if ((zone->set_index >= XD_FREE_TABLE_CNT)
-	    || (zone->set_index < 0)) {
-		free_zone(zone);
-		RTS51X_DEBUGP("Set unused block fail, invalid set_index\n");
-		return;
-	}
-
-	RTS51X_DEBUGP("Set unused block to index %d\n", zone->set_index);
-
-	zone->free_table[zone->set_index++] = (u16) (phy_blk & 0x3ff);
-	if (zone->set_index >= XD_FREE_TABLE_CNT)
-		zone->set_index = 0;
-	zone->unused_blk_cnt++;
-}
-
-static u32 xd_get_unused_block(struct rts51x_chip *chip, int zone_no)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	struct zone_entry *zone;
-	u32 phy_blk;
-
-	if (zone_no >= xd_card->zone_cnt) {
-		RTS51X_DEBUGP("Get unused block from invalid zone"
-					"(zone_no = %d, zone_cnt = %d)\n",
-					zone_no, xd_card->zone_cnt);
-		TRACE_RET(chip, BLK_NOT_FOUND);
-	}
-	zone = &(xd_card->zone[zone_no]);
-
-	if ((zone->unused_blk_cnt == 0) ||
-			(zone->set_index == zone->get_index)) {
-		free_zone(zone);
-		RTS51X_DEBUGP("Get unused block fail,"
-					"no unused block available\n");
-		TRACE_RET(chip, BLK_NOT_FOUND);
-	}
-	if ((zone->get_index >= XD_FREE_TABLE_CNT) || (zone->get_index < 0)) {
-		free_zone(zone);
-		RTS51X_DEBUGP("Get unused block fail, invalid get_index\n");
-		TRACE_RET(chip, BLK_NOT_FOUND);
-	}
-
-	RTS51X_DEBUGP("Get unused block from index %d\n", zone->get_index);
-
-	phy_blk = zone->free_table[zone->get_index];
-	zone->free_table[zone->get_index++] = 0xFFFF;
-	if (zone->get_index >= XD_FREE_TABLE_CNT)
-		zone->get_index = 0;
-	zone->unused_blk_cnt--;
-
-	phy_blk += ((u32) (zone_no) << 10);
-	return phy_blk;
-}
-
-static void xd_set_l2p_tbl(struct rts51x_chip *chip, int zone_no, u16 log_off,
-			   u16 phy_off)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	struct zone_entry *zone;
-
-	zone = &(xd_card->zone[zone_no]);
-	zone->l2p_table[log_off] = phy_off;
-}
-
-static int xd_delay_write(struct rts51x_chip *chip);
-
-static u32 xd_get_l2p_tbl(struct rts51x_chip *chip, int zone_no, u16 log_off)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	struct zone_entry *zone;
-	int retval;
-
-	zone = &(xd_card->zone[zone_no]);
-	if (zone->l2p_table[log_off] == 0xFFFF) {
-		u32 phy_blk = 0;
-		int i;
-
-		retval = xd_delay_write(chip);
-		if (retval != STATUS_SUCCESS) {
-			RTS51X_DEBUGP("In xd_get_l2p_tbl,"
-						"delay write fail!\n");
-			TRACE_RET(chip, BLK_NOT_FOUND);
-		}
-
-		if (zone->unused_blk_cnt <= 0) {
-			RTS51X_DEBUGP("No unused block!\n");
-			TRACE_RET(chip, BLK_NOT_FOUND);
-		}
-
-		for (i = 0; i < zone->unused_blk_cnt; i++) {
-			phy_blk = xd_get_unused_block(chip, zone_no);
-			if (phy_blk == BLK_NOT_FOUND) {
-				RTS51X_DEBUGP("No unused block available!\n");
-				TRACE_RET(chip, BLK_NOT_FOUND);
-			}
-
-			retval =
-			    xd_init_page(chip, phy_blk, log_off, 0,
-					 xd_card->page_off + 1);
-			if (retval == STATUS_SUCCESS)
-				break;
-		}
-		if (i >= zone->unused_blk_cnt) {
-			RTS51X_DEBUGP("No good unused block available!\n");
-			TRACE_RET(chip, BLK_NOT_FOUND);
-		}
-
-		xd_set_l2p_tbl(chip, zone_no, log_off, (u16) (phy_blk & 0x3FF));
-		return phy_blk;
-	}
-
-	return (u32) zone->l2p_table[log_off] + ((u32) (zone_no) << 10);
-}
-
-int rts51x_reset_xd_card(struct rts51x_chip *chip)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	int retval;
-
-	memset(xd_card, 0, sizeof(struct xd_info));
-
-	xd_card->block_shift = 0;
-	xd_card->page_off = 0;
-	xd_card->addr_cycle = 0;
-	xd_card->capacity = 0;
-	xd_card->zone_cnt = 0;
-	xd_card->cis_block = 0xFFFF;
-	xd_card->delay_write.delay_write_flag = 0;
-
-	rts51x_enable_card_clock(chip, XD_CARD);
-
-	retval = reset_xd(chip);
-	if (retval != STATUS_SUCCESS) {
-		if (chip->option.reset_or_rw_fail_set_pad_drive) {
-			rts51x_write_register(chip, CARD_DRIVE_SEL,
-					      SD20_DRIVE_MASK, DRIVE_8mA);
-		}
-		TRACE_RET(chip, retval);
-	}
-
-	retval = xd_init_l2p_tbl(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_mark_bad_block(struct rts51x_chip *chip, u32 phy_blk)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	int retval;
-	u32 page_addr;
-	u8 reg = 0;
-
-	RTS51X_DEBUGP("mark block 0x%x as bad block\n", phy_blk);
-
-	if (phy_blk == BLK_NOT_FOUND)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_STATUS, 0xFF, XD_GPG);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_STATUS, 0xFF,
-		       XD_LATER_BBLK);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR1_H, 0xFF, 0xFF);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR1_L, 0xFF, 0xFF);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR2_H, 0xFF, 0xFF);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR2_L, 0xFF, 0xFF);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_RESERVED0, 0xFF, 0xFF);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_RESERVED1, 0xFF, 0xFF);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_RESERVED2, 0xFF, 0xFF);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_RESERVED3, 0xFF, 0xFF);
-
-	page_addr = phy_blk << xd_card->block_shift;
-
-	xd_assign_phy_addr(chip, page_addr, XD_RW_ADDR);
-
-	/* Specify page count */
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_CNT, 0xFF,
-		       xd_card->page_off + 1);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-		       XD_TRANSFER_START | XD_WRITE_REDUNDANT);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER, XD_TRANSFER_END,
-		       XD_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	retval = rts51x_get_rsp(chip, 1, 100);
-
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_xd_error(chip);
-		rts51x_ep0_read_register(chip, XD_DAT, &reg);
-		if (reg & PROGRAM_ERROR)
-			xd_set_err_code(chip, XD_PRG_ERROR);
-		else
-			xd_set_err_code(chip, XD_TO_ERROR);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_init_page(struct rts51x_chip *chip, u32 phy_blk, u16 logoff,
-			u8 start_page, u8 end_page)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	int retval;
-	u32 page_addr;
-	u8 reg = 0;
-
-	RTS51X_DEBUGP("Init block 0x%x\n", phy_blk);
-
-	if (start_page > end_page)
-		TRACE_RET(chip, STATUS_FAIL);
-	if (phy_blk == BLK_NOT_FOUND)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_STATUS, 0xFF, 0xFF);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_STATUS, 0xFF, 0xFF);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR1_H, 0xFF,
-		       (u8) (logoff >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR1_L, 0xFF,
-		       (u8) logoff);
-
-	page_addr = (phy_blk << xd_card->block_shift) + start_page;
-
-	xd_assign_phy_addr(chip, page_addr, XD_RW_ADDR);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CFG, XD_BA_TRANSFORM,
-		       XD_BA_TRANSFORM);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_CNT, 0xFF,
-		       (end_page - start_page));
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-		       XD_TRANSFER_START | XD_WRITE_REDUNDANT);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER, XD_TRANSFER_END,
-		       XD_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	retval = rts51x_get_rsp(chip, 1, 500);
-
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_xd_error(chip);
-		rts51x_ep0_read_register(chip, XD_DAT, &reg);
-		if (reg & PROGRAM_ERROR) {
-			xd_mark_bad_block(chip, phy_blk);
-			xd_set_err_code(chip, XD_PRG_ERROR);
-		} else {
-			xd_set_err_code(chip, XD_TO_ERROR);
-		}
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_copy_page(struct rts51x_chip *chip,
-			u32 old_blk, u32 new_blk, u8 start_page, u8 end_page)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	u32 old_page, new_page;
-	u8 i, reg = 0;
-	int retval;
-
-	RTS51X_DEBUGP("Copy page from block 0x%x to block 0x%x\n", old_blk,
-		       new_blk);
-
-	if (start_page > end_page)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	if ((old_blk == BLK_NOT_FOUND) || (new_blk == BLK_NOT_FOUND))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	old_page = (old_blk << xd_card->block_shift) + start_page;
-	new_page = (new_blk << xd_card->block_shift) + start_page;
-
-	XD_CLR_BAD_NEWBLK(xd_card);
-
-	RTS51X_WRITE_REG(chip, CARD_DATA_SOURCE, 0x01, PINGPONG_BUFFER);
-
-	for (i = start_page; i < end_page; i++) {
-		if (monitor_card_cd(chip, XD_CARD) == CD_NOT_EXIST) {
-			RTS51X_WRITE_REG(chip, CARD_STOP, XD_STOP | XD_CLR_ERR,
-					 XD_STOP | XD_CLR_ERR);
-			xd_set_err_code(chip, XD_NO_CARD);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		rts51x_init_cmd(chip);
-
-		xd_assign_phy_addr(chip, old_page, XD_RW_ADDR);
-
-		/* Single page read */
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_CNT, 0xFF, 1);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CHK_DATA_STATUS,
-			       XD_AUTO_CHK_DATA_STATUS, 0);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-			       XD_TRANSFER_START | XD_READ_PAGES);
-		rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER,
-			       XD_TRANSFER_END, XD_TRANSFER_END);
-
-		retval = rts51x_send_cmd(chip, MODE_CR | STAGE_XD_STATUS, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval = rts51x_get_rsp(chip, 4, 500);
-		if ((retval != STATUS_SUCCESS) ||
-		    (chip->rsp_buf[2] & (XD_ECC1_ERROR | XD_ECC2_ERROR))) {
-			rts51x_clear_xd_error(chip);
-			reg = 0;
-			rts51x_ep0_read_register(chip, XD_CTL, &reg);
-			if (reg & (XD_ECC1_ERROR | XD_ECC2_ERROR)) {
-				wait_timeout(100);
-
-				if (monitor_card_cd(chip, XD_CARD) ==
-				    CD_NOT_EXIST) {
-					xd_set_err_code(chip, XD_NO_CARD);
-					TRACE_RET(chip, STATUS_FAIL);
-				}
-
-				if (((reg &
-				      (XD_ECC1_ERROR | XD_ECC1_UNCORRECTABLE))
-				     == (XD_ECC1_ERROR | XD_ECC1_UNCORRECTABLE))
-				    || ((reg & (XD_ECC2_ERROR |
-					XD_ECC2_UNCORRECTABLE)) ==
-				     (XD_ECC2_ERROR | XD_ECC2_UNCORRECTABLE))) {
-					RTS51X_WRITE_REG(chip, XD_PAGE_STATUS,
-							 0xFF, XD_BPG);
-					RTS51X_WRITE_REG(chip, XD_BLOCK_STATUS,
-							 0xFF, XD_GBLK);
-					XD_SET_BAD_OLDBLK(xd_card);
-					RTS51X_DEBUGP("old block 0x%x"
-						"ecc error\n", old_blk);
-				}
-			} else {
-				xd_set_err_code(chip, XD_TO_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-		if (XD_CHK_BAD_OLDBLK(xd_card))
-			rts51x_clear_xd_error(chip);
-
-		rts51x_init_cmd(chip);
-
-		xd_assign_phy_addr(chip, new_page, XD_RW_ADDR);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_CNT, 0xFF, 1);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-			       XD_TRANSFER_START | XD_WRITE_PAGES);
-		rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER,
-			       XD_TRANSFER_END, XD_TRANSFER_END);
-
-		retval = rts51x_send_cmd(chip, MODE_CR, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval = rts51x_get_rsp(chip, 1, 300);
-		if (retval != STATUS_SUCCESS) {
-			rts51x_clear_xd_error(chip);
-			reg = 0;
-			rts51x_ep0_read_register(chip, XD_DAT, &reg);
-			if (reg & PROGRAM_ERROR) {
-				xd_mark_bad_block(chip, new_blk);
-				xd_set_err_code(chip, XD_PRG_ERROR);
-				XD_SET_BAD_NEWBLK(xd_card);
-			} else {
-				xd_set_err_code(chip, XD_TO_ERROR);
-			}
-			TRACE_RET(chip, retval);
-		}
-
-		old_page++;
-		new_page++;
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_reset_cmd(struct rts51x_chip *chip)
-{
-	int retval;
-	u8 xd_dat, xd_ctl;
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-		       XD_TRANSFER_START | XD_RESET);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER, XD_TRANSFER_END,
-		       XD_TRANSFER_END);
-	rts51x_add_cmd(chip, READ_REG_CMD, XD_DAT, 0, 0);
-	rts51x_add_cmd(chip, READ_REG_CMD, XD_CTL, 0, 0);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 3, 100);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_xd_error(chip);
-		TRACE_RET(chip, retval);
-	}
-
-	xd_dat = chip->rsp_buf[1];
-	xd_ctl = chip->rsp_buf[2];
-	if (((xd_dat & READY_FLAG) == READY_STATE) && (xd_ctl & XD_RDY))
-		return STATUS_SUCCESS;
-
-	TRACE_RET(chip, STATUS_FAIL);
-}
-
-static int xd_erase_block(struct rts51x_chip *chip, u32 phy_blk)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	u32 page_addr;
-	u8 reg = 0, xd_dat;
-	int i, retval;
-
-	if (phy_blk == BLK_NOT_FOUND)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	page_addr = phy_blk << xd_card->block_shift;
-
-	for (i = 0; i < 3; i++) {
-		rts51x_init_cmd(chip);
-
-		xd_assign_phy_addr(chip, page_addr, XD_ERASE_ADDR);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-			       XD_TRANSFER_START | XD_ERASE);
-		rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER,
-			       XD_TRANSFER_END, XD_TRANSFER_END);
-		rts51x_add_cmd(chip, READ_REG_CMD, XD_DAT, 0, 0);
-
-		retval = rts51x_send_cmd(chip, MODE_CR, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval = rts51x_get_rsp(chip, 2, 300);
-		if (retval != STATUS_SUCCESS) {
-			rts51x_clear_xd_error(chip);
-			rts51x_ep0_read_register(chip, XD_DAT, &reg);
-			if (reg & PROGRAM_ERROR) {
-				xd_mark_bad_block(chip, phy_blk);
-				xd_set_err_code(chip, XD_PRG_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			} else {
-				xd_set_err_code(chip, XD_ERASE_FAIL);
-			}
-			retval = xd_reset_cmd(chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-			continue;
-		}
-		xd_dat = chip->rsp_buf[1];
-		if (xd_dat & PROGRAM_ERROR) {
-			xd_mark_bad_block(chip, phy_blk);
-			xd_set_err_code(chip, XD_PRG_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		return STATUS_SUCCESS;
-	}
-
-	xd_mark_bad_block(chip, phy_blk);
-	xd_set_err_code(chip, XD_ERASE_FAIL);
-	TRACE_RET(chip, STATUS_FAIL);
-}
-
-static int xd_build_l2p_tbl(struct rts51x_chip *chip, int zone_no)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	struct zone_entry *zone;
-	int retval;
-	u32 start, end, i;
-	u16 max_logoff, cur_fst_page_logoff, cur_lst_page_logoff,
-	    ent_lst_page_logoff;
-	u8 redunt[11];
-
-	RTS51X_DEBUGP("xd_build_l2p_tbl: %d\n", zone_no);
-
-	if (xd_card->zone == NULL) {
-		retval = xd_init_l2p_tbl(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	if (xd_card->zone[zone_no].build_flag) {
-		RTS51X_DEBUGP("l2p table of zone %d has been built\n",
-			       zone_no);
-		return STATUS_SUCCESS;
-	}
-
-	zone = &(xd_card->zone[zone_no]);
-
-	if (zone->l2p_table == NULL) {
-		zone->l2p_table = vmalloc(2000);
-		if (zone->l2p_table == NULL)
-			TRACE_GOTO(chip, Build_Fail);
-	}
-	memset((u8 *) (zone->l2p_table), 0xff, 2000);
-
-	if (zone->free_table == NULL) {
-		zone->free_table = vmalloc(XD_FREE_TABLE_CNT * 2);
-		if (zone->free_table == NULL)
-			TRACE_GOTO(chip, Build_Fail);
-	}
-	memset((u8 *) (zone->free_table), 0xff, XD_FREE_TABLE_CNT * 2);
-
-	if (zone_no == 0) {
-		if (xd_card->cis_block == 0xFFFF)
-			start = 0;
-		else
-			start = xd_card->cis_block + 1;
-		if (XD_CHK_4MB(xd_card)) {
-			end = 0x200;
-			max_logoff = 499;
-		} else {
-			end = 0x400;
-			max_logoff = 999;
-		}
-	} else {
-		start = (u32) (zone_no) << 10;
-		end = (u32) (zone_no + 1) << 10;
-		max_logoff = 999;
-	}
-
-	RTS51X_DEBUGP("start block 0x%x, end block 0x%x\n", start, end);
-
-	zone->set_index = zone->get_index = 0;
-	zone->unused_blk_cnt = 0;
-
-	for (i = start; i < end; i++) {
-		u32 page_addr = i << xd_card->block_shift;
-		u32 phy_block;
-
-		retval = xd_read_redundant(chip, page_addr, redunt, 11);
-		if (retval != STATUS_SUCCESS)
-			continue;
-
-		if (redunt[BLOCK_STATUS] != 0xFF) {
-			RTS51X_DEBUGP("bad block\n");
-			continue;
-		}
-
-		if (xd_check_data_blank(redunt)) {
-			RTS51X_DEBUGP("blank block\n");
-			xd_set_unused_block(chip, i);
-			continue;
-		}
-
-		cur_fst_page_logoff = xd_load_log_block_addr(redunt);
-		if ((cur_fst_page_logoff == 0xFFFF)
-		    || (cur_fst_page_logoff > max_logoff)) {
-			retval = xd_erase_block(chip, i);
-			if (retval == STATUS_SUCCESS)
-				xd_set_unused_block(chip, i);
-			continue;
-		}
-		if ((zone_no == 0) && (cur_fst_page_logoff == 0)
-		    && (redunt[PAGE_STATUS] != XD_GPG))
-			XD_SET_MBR_FAIL(xd_card);
-
-		if (zone->l2p_table[cur_fst_page_logoff] == 0xFFFF) {
-			zone->l2p_table[cur_fst_page_logoff] =
-			    (u16) (i & 0x3FF);
-			continue;
-		}
-
-		phy_block =
-		    zone->l2p_table[cur_fst_page_logoff] +
-		    ((u32) ((zone_no) << 10));
-
-		page_addr = ((i + 1) << xd_card->block_shift) - 1;
-
-		retval = xd_read_redundant(chip, page_addr, redunt, 11);
-		if (retval != STATUS_SUCCESS)
-			continue;
-
-		cur_lst_page_logoff = xd_load_log_block_addr(redunt);
-		if (cur_lst_page_logoff == cur_fst_page_logoff) {
-			int m;
-
-			page_addr =
-			    ((phy_block + 1) << xd_card->block_shift) - 1;
-
-			for (m = 0; m < 3; m++) {
-				retval =
-				    xd_read_redundant(chip, page_addr, redunt,
-						      11);
-				if (retval == STATUS_SUCCESS)
-					break;
-			}
-
-			if (m == 3) {
-				zone->l2p_table[cur_fst_page_logoff] =
-				    (u16) (i & 0x3FF);
-				retval = xd_erase_block(chip, phy_block);
-				if (retval == STATUS_SUCCESS)
-					xd_set_unused_block(chip, phy_block);
-				continue;
-			}
-
-			ent_lst_page_logoff = xd_load_log_block_addr(redunt);
-			if (ent_lst_page_logoff != cur_fst_page_logoff) {
-				zone->l2p_table[cur_fst_page_logoff] =
-				    (u16) (i & 0x3FF);
-				retval = xd_erase_block(chip, phy_block);
-				if (retval == STATUS_SUCCESS)
-					xd_set_unused_block(chip, phy_block);
-				continue;
-			} else {
-				retval = xd_erase_block(chip, i);
-				if (retval == STATUS_SUCCESS)
-					xd_set_unused_block(chip, i);
-			}
-		} else {
-			retval = xd_erase_block(chip, i);
-			if (retval == STATUS_SUCCESS)
-				xd_set_unused_block(chip, i);
-		}
-	}
-
-	if (XD_CHK_4MB(xd_card))
-		end = 500;
-	else
-		end = 1000;
-
-	i = 0;
-	for (start = 0; start < end; start++) {
-		if (zone->l2p_table[start] == 0xFFFF)
-			i++;
-	}
-
-	RTS51X_DEBUGP("Block count %d, invalid L2P entry %d\n", end, i);
-	RTS51X_DEBUGP("Total unused block: %d\n", zone->unused_blk_cnt);
-
-	if ((zone->unused_blk_cnt - i) < 1)
-		chip->card_wp |= XD_CARD;
-
-	zone->build_flag = 1;
-
-	return STATUS_SUCCESS;
-
-Build_Fail:
-	if (zone->l2p_table) {
-		vfree(zone->l2p_table);
-		zone->l2p_table = NULL;
-	}
-	if (zone->free_table) {
-		vfree(zone->free_table);
-		zone->free_table = NULL;
-	}
-
-	return STATUS_FAIL;
-}
-
-static int xd_send_cmd(struct rts51x_chip *chip, u8 cmd)
-{
-	int retval;
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_DAT, 0xFF, cmd);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-		       XD_TRANSFER_START | XD_SET_CMD);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER, XD_TRANSFER_END,
-		       XD_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 1, 200);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_xd_error(chip);
-		TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_read_multiple_pages(struct rts51x_chip *chip, u32 phy_blk,
-				  u32 log_blk, u8 start_page, u8 end_page,
-				  u8 *buf, void **ptr, unsigned int *offset)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	u32 page_addr, new_blk;
-	u16 log_off;
-	u8 reg_val, page_cnt;
-	int zone_no, retval, i;
-
-	if (start_page > end_page)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	page_cnt = end_page - start_page;
-	zone_no = (int)(log_blk / 1000);
-	log_off = (u16) (log_blk % 1000);
-
-	if ((phy_blk & 0x3FF) == 0x3FF) {
-		for (i = 0; i < 256; i++) {
-			page_addr = ((u32) i) << xd_card->block_shift;
-
-			retval = xd_read_redundant(chip, page_addr, NULL, 0);
-			if (retval == STATUS_SUCCESS)
-				break;
-
-			if (monitor_card_cd(chip, XD_CARD) == CD_NOT_EXIST) {
-				xd_set_err_code(chip, XD_NO_CARD);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-	}
-
-	page_addr = (phy_blk << xd_card->block_shift) + start_page;
-
-	rts51x_init_cmd(chip);
-
-	xd_assign_phy_addr(chip, page_addr, XD_RW_ADDR);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CFG, XD_PPB_TO_SIE,
-		       XD_PPB_TO_SIE);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       RING_BUFFER);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_CNT, 0xFF, page_cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CHK_DATA_STATUS,
-		       XD_AUTO_CHK_DATA_STATUS, XD_AUTO_CHK_DATA_STATUS);
-
-	rts51x_trans_dma_enable(chip->srb->sc_data_direction, chip,
-			page_cnt * 512, DMA_512);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-		       XD_TRANSFER_START | XD_READ_PAGES);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER,
-		       XD_TRANSFER_END | XD_PPB_EMPTY,
-		       XD_TRANSFER_END | XD_PPB_EMPTY);
-
-	retval = rts51x_send_cmd(chip, MODE_CDIR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval =
-	    rts51x_transfer_data_partial(chip, RCV_BULK_PIPE(chip), (void *)buf,
-					 ptr, offset, page_cnt * 512,
-					 scsi_sg_count(chip->srb), NULL, 2000);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_xd_error(chip);
-
-		if (retval == STATUS_TIMEDOUT) {
-			xd_set_err_code(chip, XD_TO_ERROR);
-			TRACE_RET(chip, retval);
-		} else {
-			TRACE_GOTO(chip, Fail);
-		}
-	}
-	retval = rts51x_get_rsp(chip, 1, 200);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_xd_error(chip);
-
-		if (retval == STATUS_TIMEDOUT) {
-			xd_set_err_code(chip, XD_TO_ERROR);
-			TRACE_RET(chip, retval);
-		} else {
-			TRACE_GOTO(chip, Fail);
-		}
-	}
-
-	return STATUS_SUCCESS;
-
-Fail:
-	rts51x_ep0_read_register(chip, XD_PAGE_STATUS, &reg_val);
-	RTS51X_DEBUGP("XD_PAGE_STATUS: 0x%x\n", reg_val);
-
-	if (reg_val != XD_GPG)
-		xd_set_err_code(chip, XD_PRG_ERROR);
-
-	rts51x_ep0_read_register(chip, XD_CTL, &reg_val);
-	RTS51X_DEBUGP("XD_CTL: 0x%x\n", reg_val);
-
-	/* Handle uncorrectable ECC error */
-	if (((reg_val & (XD_ECC1_ERROR | XD_ECC1_UNCORRECTABLE))
-	     == (XD_ECC1_ERROR | XD_ECC1_UNCORRECTABLE))
-	    || ((reg_val & (XD_ECC2_ERROR | XD_ECC2_UNCORRECTABLE))
-		== (XD_ECC2_ERROR | XD_ECC2_UNCORRECTABLE))) {
-		wait_timeout(100);
-
-		if (monitor_card_cd(chip, XD_CARD) == CD_NOT_EXIST) {
-			xd_set_err_code(chip, XD_NO_CARD);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		xd_set_err_code(chip, XD_ECC_ERROR);
-
-		new_blk = xd_get_unused_block(chip, zone_no);
-		if (new_blk == NO_NEW_BLK) {
-			XD_CLR_BAD_OLDBLK(xd_card);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-		retval =
-		    xd_copy_page(chip, phy_blk, new_blk, 0,
-				 xd_card->page_off + 1);
-		if (retval != STATUS_SUCCESS) {
-			if (!XD_CHK_BAD_NEWBLK(xd_card)) {
-				retval = xd_erase_block(chip, new_blk);
-				if (retval == STATUS_SUCCESS)
-					xd_set_unused_block(chip, new_blk);
-			} else {
-				XD_CLR_BAD_NEWBLK(xd_card);
-			}
-			XD_CLR_BAD_OLDBLK(xd_card);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-		xd_set_l2p_tbl(chip, zone_no, log_off, (u16) (new_blk & 0x3FF));
-		xd_erase_block(chip, phy_blk);
-		xd_mark_bad_block(chip, phy_blk);
-		XD_CLR_BAD_OLDBLK(xd_card);
-	}
-
-	TRACE_RET(chip, STATUS_FAIL);
-}
-
-static int xd_finish_write(struct rts51x_chip *chip,
-			   u32 old_blk, u32 new_blk, u32 log_blk, u8 page_off)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	int retval, zone_no;
-	u16 log_off;
-
-	RTS51X_DEBUGP("xd_finish_write, old_blk = 0x%x, new_blk = 0x%x,"
-				"log_blk = 0x%x\n", old_blk, new_blk, log_blk);
-
-	if (page_off > xd_card->page_off)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	zone_no = (int)(log_blk / 1000);
-	log_off = (u16) (log_blk % 1000);
-
-	if (old_blk == BLK_NOT_FOUND) {
-		retval = xd_init_page(chip, new_blk, log_off,
-				      page_off, xd_card->page_off + 1);
-		if (retval != STATUS_SUCCESS) {
-			retval = xd_erase_block(chip, new_blk);
-			if (retval == STATUS_SUCCESS)
-				xd_set_unused_block(chip, new_blk);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	} else {
-		retval = xd_copy_page(chip, old_blk, new_blk,
-				      page_off, xd_card->page_off + 1);
-		if (retval != STATUS_SUCCESS) {
-			if (!XD_CHK_BAD_NEWBLK(xd_card)) {
-				retval = xd_erase_block(chip, new_blk);
-				if (retval == STATUS_SUCCESS)
-					xd_set_unused_block(chip, new_blk);
-			}
-			XD_CLR_BAD_NEWBLK(xd_card);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		retval = xd_erase_block(chip, old_blk);
-		if (retval == STATUS_SUCCESS) {
-			if (XD_CHK_BAD_OLDBLK(xd_card)) {
-				xd_mark_bad_block(chip, old_blk);
-				XD_CLR_BAD_OLDBLK(xd_card);
-			} else {
-				/* Add source block to unused block */
-				xd_set_unused_block(chip, old_blk);
-			}
-		} else {
-			xd_set_err_code(chip, XD_NO_ERROR);
-			XD_CLR_BAD_OLDBLK(xd_card);
-		}
-	}
-
-	/* Add target block to L2P table */
-	xd_set_l2p_tbl(chip, zone_no, log_off, (u16) (new_blk & 0x3FF));
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_prepare_write(struct rts51x_chip *chip,
-			    u32 old_blk, u32 new_blk, u32 log_blk, u8 page_off)
-{
-	int retval;
-
-	RTS51X_DEBUGP("xd_prepare_write, old_blk = 0x%x, new_blk = 0x%x,"
-				"log_blk = 0x%x, page_off = %d\n",
-				old_blk, new_blk, log_blk, (int)page_off);
-
-	if (page_off) {
-		retval = xd_copy_page(chip, old_blk, new_blk, 0, page_off);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_write_multiple_pages(struct rts51x_chip *chip, u32 old_blk,
-				   u32 new_blk, u32 log_blk, u8 start_page,
-				   u8 end_page, u8 *buf, void **ptr,
-				   unsigned int *offset)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	u32 page_addr;
-	int zone_no, retval;
-	u16 log_off;
-	u8 page_cnt, reg_val;
-
-	RTS51X_DEBUGP("xd_write_multiple_pages, old_blk = 0x%x,"
-				"new_blk = 0x%x, log_blk = 0x%x\n",
-				old_blk, new_blk, log_blk);
-
-	if (start_page > end_page)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	page_cnt = end_page - start_page;
-	zone_no = (int)(log_blk / 1000);
-	log_off = (u16) (log_blk % 1000);
-
-	page_addr = (new_blk << xd_card->block_shift) + start_page;
-
-	/* Send index command */
-	retval = xd_send_cmd(chip, READ1_1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	rts51x_init_cmd(chip);
-
-	/* Prepare redundant field */
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR1_H, 0xFF,
-		       (u8) (log_off >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR1_L, 0xFF,
-		       (u8) log_off);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_STATUS, 0xFF, XD_GBLK);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_STATUS, 0xFF, XD_GPG);
-
-	xd_assign_phy_addr(chip, page_addr, XD_RW_ADDR);
-
-	/* Transform the block address by hardware */
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CFG, XD_BA_TRANSFORM,
-		       XD_BA_TRANSFORM);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_CNT, 0xFF, page_cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       RING_BUFFER);
-
-	rts51x_trans_dma_enable(chip->srb->sc_data_direction, chip,
-			page_cnt * 512, DMA_512);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-		       XD_TRANSFER_START | XD_WRITE_PAGES);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER, XD_TRANSFER_END,
-		       XD_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, MODE_CDOR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval =
-	    rts51x_transfer_data_partial(chip, SND_BULK_PIPE(chip), (void *)buf,
-					 ptr, offset, page_cnt * 512,
-					 scsi_sg_count(chip->srb), NULL, 2000);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_xd_error(chip);
-
-		if (retval == STATUS_TIMEDOUT) {
-			xd_set_err_code(chip, XD_TO_ERROR);
-			TRACE_RET(chip, retval);
-		} else {
-			TRACE_GOTO(chip, Fail);
-		}
-	}
-	retval = rts51x_get_rsp(chip, 1, 200);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_xd_error(chip);
-
-		if (retval == STATUS_TIMEDOUT) {
-			xd_set_err_code(chip, XD_TO_ERROR);
-			TRACE_RET(chip, retval);
-		} else {
-			TRACE_GOTO(chip, Fail);
-		}
-	}
-
-	if (end_page == (xd_card->page_off + 1)) {
-		xd_card->delay_write.delay_write_flag = 0;
-
-		if (old_blk != BLK_NOT_FOUND) {
-			retval = xd_erase_block(chip, old_blk);
-			if (retval == STATUS_SUCCESS) {
-				if (XD_CHK_BAD_OLDBLK(xd_card)) {
-					xd_mark_bad_block(chip, old_blk);
-					XD_CLR_BAD_OLDBLK(xd_card);
-				} else {
-					xd_set_unused_block(chip, old_blk);
-				}
-			} else {
-				xd_set_err_code(chip, XD_NO_ERROR);
-				XD_CLR_BAD_OLDBLK(xd_card);
-			}
-		}
-		xd_set_l2p_tbl(chip, zone_no, log_off, (u16) (new_blk & 0x3FF));
-	}
-
-	return STATUS_SUCCESS;
-
-Fail:
-	rts51x_ep0_read_register(chip, XD_DAT, &reg_val);
-	RTS51X_DEBUGP("XD_DAT: 0x%x\n", reg_val);
-
-	if (reg_val & PROGRAM_ERROR) {
-		xd_set_err_code(chip, XD_PRG_ERROR);
-		xd_mark_bad_block(chip, new_blk);
-	}
-
-	TRACE_RET(chip, STATUS_FAIL);
-}
-
-static int xd_delay_write(struct rts51x_chip *chip)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	struct xd_delay_write_tag *delay_write = &(xd_card->delay_write);
-	int retval;
-
-	if (delay_write->delay_write_flag) {
-		RTS51X_DEBUGP("xd_delay_write\n");
-		retval = xd_switch_clock(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		delay_write->delay_write_flag = 0;
-		retval = xd_finish_write(chip,
-					 delay_write->old_phyblock,
-					 delay_write->new_phyblock,
-					 delay_write->logblock,
-					 delay_write->pageoff);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_xd_rw(struct scsi_cmnd *srb, struct rts51x_chip *chip,
-	u32 start_sector, u16 sector_cnt)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	unsigned int lun = SCSI_LUN(srb);
-	struct xd_delay_write_tag *delay_write = &(xd_card->delay_write);
-	int retval, zone_no;
-	u32 log_blk, old_blk = 0, new_blk = 0;
-	u16 log_off, total_sec_cnt = sector_cnt;
-	u8 start_page, end_page = 0, page_cnt;
-	u8 *buf;
-	void *ptr = NULL;
-	unsigned int offset = 0;
-
-	xd_set_err_code(chip, XD_NO_ERROR);
-
-	xd_card->counter = 0;
-
-	RTS51X_DEBUGP("rts51x_xd_rw: scsi_bufflen = %d, scsi_sg_count = %d\n",
-		       scsi_bufflen(srb), scsi_sg_count(srb));
-	RTS51X_DEBUGP("Data direction: %s\n",
-		       (srb->sc_data_direction ==
-			DMA_TO_DEVICE) ? "write" : "read");
-
-	buf = (u8 *) scsi_sglist(srb);
-
-	retval = xd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	log_blk = start_sector >> xd_card->block_shift;
-	start_page = (u8) start_sector & xd_card->page_off;
-	zone_no = (int)(log_blk / 1000);
-	log_off = (u16) (log_blk % 1000);
-
-	RTS51X_DEBUGP("log_blk = 0x%x\n", log_blk);
-
-	if (xd_card->zone[zone_no].build_flag == 0) {
-		retval = xd_build_l2p_tbl(chip, zone_no);
-		if (retval != STATUS_SUCCESS) {
-			chip->card_fail |= XD_CARD;
-			rts51x_set_sense_type(chip, lun,
-				SENSE_TYPE_MEDIA_NOT_PRESENT);
-			TRACE_RET(chip, retval);
-		}
-	}
-
-	if (srb->sc_data_direction == DMA_TO_DEVICE) {
-		if (delay_write->delay_write_flag &&
-		    (delay_write->logblock == log_blk) &&
-		    (start_page > delay_write->pageoff)) {
-			delay_write->delay_write_flag = 0;
-			if (delay_write->old_phyblock != BLK_NOT_FOUND) {
-				retval = xd_copy_page(chip,
-						      delay_write->old_phyblock,
-						      delay_write->new_phyblock,
-						      delay_write->pageoff,
-						      start_page);
-				if (retval != STATUS_SUCCESS) {
-					rts51x_set_sense_type(chip, lun,
-						SENSE_TYPE_MEDIA_WRITE_ERR);
-					TRACE_RET(chip, retval);
-				}
-			}
-			old_blk = delay_write->old_phyblock;
-			new_blk = delay_write->new_phyblock;
-		} else if (delay_write->delay_write_flag &&
-			   (delay_write->logblock == log_blk) &&
-			   (start_page == delay_write->pageoff)) {
-			delay_write->delay_write_flag = 0;
-			old_blk = delay_write->old_phyblock;
-			new_blk = delay_write->new_phyblock;
-		} else {
-			retval = xd_delay_write(chip);
-			if (retval != STATUS_SUCCESS) {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, retval);
-			}
-			old_blk = xd_get_l2p_tbl(chip, zone_no, log_off);
-			new_blk = xd_get_unused_block(chip, zone_no);
-			if ((old_blk == BLK_NOT_FOUND)
-			    || (new_blk == BLK_NOT_FOUND)) {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, retval);
-			}
-
-			retval =
-			    xd_prepare_write(chip, old_blk, new_blk, log_blk,
-					     start_page);
-			if (retval != STATUS_SUCCESS) {
-				if (monitor_card_cd(chip, XD_CARD) ==
-				    CD_NOT_EXIST) {
-					rts51x_set_sense_type(chip, lun,
-						SENSE_TYPE_MEDIA_NOT_PRESENT);
-					TRACE_RET(chip, STATUS_FAIL);
-				}
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, retval);
-			}
-		}
-	} else {
-		retval = xd_delay_write(chip);
-		if (retval != STATUS_SUCCESS) {
-			if (monitor_card_cd(chip, XD_CARD) == CD_NOT_EXIST) {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_NOT_PRESENT);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-			TRACE_RET(chip, retval);
-		}
-
-		old_blk = xd_get_l2p_tbl(chip, zone_no, log_off);
-		if (old_blk == BLK_NOT_FOUND) {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-
-	RTS51X_DEBUGP("old_blk = 0x%x\n", old_blk);
-	if (srb->sc_data_direction == DMA_TO_DEVICE)
-		RTS51X_DEBUGP("new_blk = 0x%x\n", new_blk);
-
-	while (total_sec_cnt) {
-		if ((start_page + total_sec_cnt) > (xd_card->page_off + 1))
-			end_page = xd_card->page_off + 1;
-		else
-			end_page = start_page + (u8) total_sec_cnt;
-		page_cnt = end_page - start_page;
-		if (srb->sc_data_direction == DMA_FROM_DEVICE) {
-			retval = xd_read_multiple_pages(chip, old_blk, log_blk,
-							start_page, end_page,
-							buf, &ptr, &offset);
-			if (retval != STATUS_SUCCESS) {
-				rts51x_set_sense_type(chip, lun,
-					SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		} else {
-			retval =
-			    xd_write_multiple_pages(chip, old_blk, new_blk,
-						    log_blk, start_page,
-						    end_page, buf, &ptr,
-						    &offset);
-			if (retval != STATUS_SUCCESS) {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-
-		total_sec_cnt -= page_cnt;
-
-		if (total_sec_cnt == 0)
-			break;
-
-		log_blk++;
-		zone_no = (int)(log_blk / 1000);
-		log_off = (u16) (log_blk % 1000);
-
-		if (xd_card->zone[zone_no].build_flag == 0) {
-			retval = xd_build_l2p_tbl(chip, zone_no);
-			if (retval != STATUS_SUCCESS) {
-				chip->card_fail |= XD_CARD;
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_NOT_PRESENT);
-				TRACE_RET(chip, retval);
-			}
-		}
-
-		old_blk = xd_get_l2p_tbl(chip, zone_no, log_off);
-		if (old_blk == BLK_NOT_FOUND) {
-			if (srb->sc_data_direction == DMA_FROM_DEVICE) {
-				rts51x_set_sense_type(chip, lun,
-					SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-			} else {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_WRITE_ERR);
-			}
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		if (srb->sc_data_direction == DMA_TO_DEVICE) {
-			new_blk = xd_get_unused_block(chip, zone_no);
-			if (new_blk == BLK_NOT_FOUND) {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-
-		start_page = 0;
-	}
-
-	if ((srb->sc_data_direction == DMA_TO_DEVICE) &&
-	    (end_page != (xd_card->page_off + 1))) {
-		delay_write->delay_write_flag = 1;
-		delay_write->old_phyblock = old_blk;
-		delay_write->new_phyblock = new_blk;
-		delay_write->logblock = log_blk;
-		delay_write->pageoff = end_page;
-	}
-
-	scsi_set_resid(srb, 0);
-
-	return STATUS_SUCCESS;
-}
-
-void rts51x_xd_free_l2p_tbl(struct rts51x_chip *chip)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	int i = 0;
-
-	if (xd_card->zone != NULL) {
-		for (i = 0; i < xd_card->zone_cnt; i++) {
-			if (xd_card->zone[i].l2p_table != NULL) {
-				vfree(xd_card->zone[i].l2p_table);
-				xd_card->zone[i].l2p_table = NULL;
-			}
-			if (xd_card->zone[i].free_table != NULL) {
-				vfree(xd_card->zone[i].free_table);
-				xd_card->zone[i].free_table = NULL;
-			}
-		}
-		vfree(xd_card->zone);
-		xd_card->zone = NULL;
-	}
-}
-
-void rts51x_xd_cleanup_work(struct rts51x_chip *chip)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-
-	if (xd_card->delay_write.delay_write_flag) {
-		RTS51X_DEBUGP("xD: delay write\n");
-		xd_delay_write(chip);
-		xd_card->counter = 0;
-	}
-}
-
-static int xd_power_off_card3v3(struct rts51x_chip *chip)
-{
-	int retval;
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_CLK_EN, XD_CLK_EN, 0);
-
-	if (chip->asic_code)
-		xd_pull_ctl_disable(chip);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, FPGA_PULL_CTL, 0xFF, 0xDF);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_OE, XD_OUTPUT_EN, 0);
-	if (!chip->option.FT2_fast_mode) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, POWER_MASK,
-			       POWER_OFF);
-		if (CHECK_PKG(chip, LQFP48)
-		    || chip->option.rts5129_D3318_off_enable)
-			rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL,
-				       DV3318_AUTO_PWR_OFF, 0);
-	}
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_release_xd_card(struct rts51x_chip *chip)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	int retval;
-
-	RTS51X_DEBUGP("rts51x_release_xd_card\n");
-
-	chip->card_ready &= ~XD_CARD;
-	chip->card_fail &= ~XD_CARD;
-	chip->card_wp &= ~XD_CARD;
-
-	xd_card->delay_write.delay_write_flag = 0;
-
-	rts51x_xd_free_l2p_tbl(chip);
-
-	rts51x_write_register(chip, SFSM_ED, HW_CMD_STOP, HW_CMD_STOP);
-
-	retval = xd_power_off_card3v3(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (chip->asic_code && CHECK_PKG(chip, QFN24))
-		wait_timeout(20);
-
-	return STATUS_SUCCESS;
-}
diff --git a/drivers/staging/rts5139/xd.h b/drivers/staging/rts5139/xd.h
deleted file mode 100644
index 695a0b4d7e5..00000000000
--- a/drivers/staging/rts5139/xd.h
+++ /dev/null
@@ -1,191 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_XD_H
-#define __RTS51X_XD_H
-
-/* Error Codes */
-#define	XD_NO_ERROR			0x00
-#define	XD_NO_MEMORY			0x80
-#define	XD_PRG_ERROR			0x40
-#define	XD_NO_CARD			0x20
-#define	XD_READ_FAIL			0x10
-#define	XD_ERASE_FAIL			0x08
-#define	XD_WRITE_FAIL			0x04
-#define	XD_ECC_ERROR			0x02
-#define	XD_TO_ERROR			0x01
-
-/* XD Commands */
-#define	READ1_1				0x00
-#define	READ1_2				0x01
-#define	READ2				0x50
-#define READ_ID				0x90
-#define RESET				0xff
-#define PAGE_PRG_1			0x80
-#define PAGE_PRG_2			0x10
-#define	BLK_ERASE_1			0x60
-#define	BLK_ERASE_2			0xD0
-#define READ_STS			0x70
-#define READ_xD_ID			0x9A
-#define	COPY_BACK_512			0x8A
-#define	COPY_BACK_2K			0x85
-#define	READ1_1_2			0x30
-#define	READ1_1_3			0x35
-#define	CHG_DAT_OUT_1			0x05
-#define RDM_DAT_OUT_1			0x05
-#define	CHG_DAT_OUT_2			0xE0
-#define RDM_DAT_OUT_2			0xE0
-#define	CHG_DAT_OUT_2			0xE0
-#define	CHG_DAT_IN_1			0x85
-#define	CACHE_PRG			0x15
-
-/* Redundant Area Related */
-#define XD_EXTRA_SIZE			0x10
-#define XD_2K_EXTRA_SIZE		0x40
-
-/* Define for XD Status  */
-#define	NOT_WRITE_PROTECTED		0x80
-#define	READY_STATE			0x40
-#define	PROGRAM_ERROR			0x01
-#define	PROGRAM_ERROR_N_1		0x02
-#define	INTERNAL_READY			0x20
-#define	READY_FLAG			0x5F
-
-/* Define for device code */
-#define	XD_8M_X8_512			0xE6
-#define	XD_16M_X8_512			0x73
-#define	XD_32M_X8_512			0x75
-#define	XD_64M_X8_512			0x76
-#define	XD_128M_X8_512			0x79
-#define	XD_256M_X8_512			0x71
-#define	XD_128M_X8_2048			0xF1
-#define	XD_256M_X8_2048			0xDA
-#define	XD_512M_X8			0xDC
-#define	XD_128M_X16_2048		0xC1
-#define	XD_4M_X8_512_1			0xE3
-#define	XD_4M_X8_512_2			0xE5
-#define	xD_1G_X8_512			0xD3
-#define	xD_2G_X8_512			0xD5
-
-#define	XD_ID_CODE			0xB5
-
-#define	VENDOR_BLOCK			0xEFFF
-#define	CIS_BLOCK			0xDFFF
-
-#define	BLK_NOT_FOUND			0xFFFFFFFF
-
-#define	NO_NEW_BLK			0xFFFFFFFF
-
-#define	PAGE_CORRECTABLE		0x0
-#define	PAGE_NOTCORRECTABLE		0x1
-
-#define	NO_OFFSET			0x0
-#define	WITH_OFFSET			0x1
-
-#define	Sect_Per_Page			4
-#define	XD_ADDR_MODE_2C			XD_ADDR_MODE_2A
-
-#define ZONE0_BAD_BLOCK			23
-#define NOT_ZONE0_BAD_BLOCK		24
-
-/* Assign address mode */
-#define	XD_RW_ADDR			0x01
-#define	XD_ERASE_ADDR			0x02
-
-/* Macro Definition */
-#define	XD_PAGE_512(xd_card)		\
-	do {	\
-		(xd_card)->block_shift = 5;	\
-		(xd_card)->page_off = 0x1F;	\
-	} while (0)
-
-#define	XD_SET_BAD_NEWBLK(xd_card)	((xd_card)->multi_flag |= 0x01)
-#define	XD_CLR_BAD_NEWBLK(xd_card)	((xd_card)->multi_flag &= ~0x01)
-#define	XD_CHK_BAD_NEWBLK(xd_card)	((xd_card)->multi_flag & 0x01)
-
-#define	XD_SET_BAD_OLDBLK(xd_card)	((xd_card)->multi_flag |= 0x02)
-#define	XD_CLR_BAD_OLDBLK(xd_card)	((xd_card)->multi_flag &= ~0x02)
-#define	XD_CHK_BAD_OLDBLK(xd_card)	((xd_card)->multi_flag & 0x02)
-
-#define	XD_SET_MBR_FAIL(xd_card)	((xd_card)->multi_flag |= 0x04)
-#define	XD_CLR_MBR_FAIL(xd_card)	((xd_card)->multi_flag &= ~0x04)
-#define	XD_CHK_MBR_FAIL(xd_card)	((xd_card)->multi_flag & 0x04)
-
-#define	XD_SET_ECC_FLD_ERR(xd_card)	((xd_card)->multi_flag |= 0x08)
-#define	XD_CLR_ECC_FLD_ERR(xd_card)	((xd_card)->multi_flag &= ~0x08)
-#define	XD_CHK_ECC_FLD_ERR(xd_card)	((xd_card)->multi_flag & 0x08)
-
-#define	XD_SET_4MB(xd_card)		((xd_card)->multi_flag |= 0x10)
-#define	XD_CLR_4MB(xd_card)		((xd_card)->multi_flag &= ~0x10)
-#define	XD_CHK_4MB(xd_card)		((xd_card)->multi_flag & 0x10)
-
-#define	XD_SET_ECC_ERR(xd_card)		((xd_card)->multi_flag |= 0x40)
-#define	XD_CLR_ECC_ERR(xd_card)		((xd_card)->multi_flag &= ~0x40)
-#define	XD_CHK_ECC_ERR(xd_card)		((xd_card)->multi_flag & 0x40)
-
-/* Offset in xD redundant buffer */
-#define PAGE_STATUS		0
-#define BLOCK_STATUS		1
-#define BLOCK_ADDR1_L		2
-#define BLOCK_ADDR1_H		3
-#define BLOCK_ADDR2_L		4
-#define BLOCK_ADDR2_H		5
-#define RESERVED0		6
-#define RESERVED1		7
-#define RESERVED2		8
-#define RESERVED3		9
-#define PARITY			10
-
-/* For CIS block */
-#define	CIS0_0			0
-#define	CIS0_1			1
-#define	CIS0_2			2
-#define	CIS0_3			3
-#define	CIS0_4			4
-#define	CIS0_5			5
-#define	CIS0_6			6
-#define	CIS0_7			7
-#define	CIS0_8			8
-#define	CIS0_9			9
-#define	CIS1_0			256
-#define	CIS1_1			(256 + 1)
-#define	CIS1_2			(256 + 2)
-#define	CIS1_3			(256 + 3)
-#define	CIS1_4			(256 + 4)
-#define	CIS1_5			(256 + 5)
-#define	CIS1_6			(256 + 6)
-#define	CIS1_7			(256 + 7)
-#define	CIS1_8			(256 + 8)
-#define	CIS1_9			(256 + 9)
-
-int rts51x_reset_xd_card(struct rts51x_chip *chip);
-int rts51x_xd_rw(struct scsi_cmnd *srb, struct rts51x_chip *chip, u32 start_sector,
-	  u16 sector_cnt);
-void rts51x_xd_free_l2p_tbl(struct rts51x_chip *chip);
-void rts51x_xd_cleanup_work(struct rts51x_chip *chip);
-int rts51x_release_xd_card(struct rts51x_chip *chip);
-
-#endif /* __RTS51X_XD_H */
diff --git a/drivers/staging/rts5208/rtsx.c b/drivers/staging/rts5208/rtsx.c
index d2d1345fb06..c0a0e601037 100644
--- a/drivers/staging/rts5208/rtsx.c
+++ b/drivers/staging/rts5208/rtsx.c
@@ -1030,7 +1030,7 @@ static void rtsx_remove(struct pci_dev *pci)
 }
 
 /* PCI IDs */
-static DEFINE_PCI_DEVICE_TABLE(rtsx_ids) = {
+static const struct pci_device_id rtsx_ids[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0x5208),
 		PCI_CLASS_OTHERS << 16, 0xFF0000 },
 	{ PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0x5288),
diff --git a/drivers/staging/rts5208/rtsx_chip.c b/drivers/staging/rts5208/rtsx_chip.c
index 6426807a906..7907e931a35 100644
--- a/drivers/staging/rts5208/rtsx_chip.c
+++ b/drivers/staging/rts5208/rtsx_chip.c
@@ -104,7 +104,7 @@ void rtsx_enable_bus_int(struct rtsx_chip *chip)
 	if (chip->ic_version >= IC_VER_C)
 		reg |= DELINK_INT_EN;
 #ifdef SUPPORT_OCP
-		reg |= OC_INT_EN;
+	reg |= OC_INT_EN;
 #endif
 	if (!chip->adma_mode)
 		reg |= DATA_DONE_INT_EN;
diff --git a/drivers/staging/sbe-2t3e3/2t3e3.h b/drivers/staging/sbe-2t3e3/2t3e3.h
deleted file mode 100644
index e7bf721f3fd..00000000000
--- a/drivers/staging/sbe-2t3e3/2t3e3.h
+++ /dev/null
@@ -1,889 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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 code is based on a driver written by SBE Inc.
- */
-
-#ifndef T3E3_H
-#define T3E3_H
-
-#include <linux/hdlc.h>
-#include <linux/interrupt.h>
-#include <linux/netdevice.h>
-#include <linux/pci.h>
-#include <linux/io.h>
-#include "ctrl.h"
-
-/**************************************************************
- *  21143
- **************************************************************/
-
-/* CSR */
-#define SBE_2T3E3_21143_REG_BUS_MODE					0
-#define SBE_2T3E3_21143_REG_TRANSMIT_POLL_DEMAND			1
-#define SBE_2T3E3_21143_REG_RECEIVE_POLL_DEMAND				2
-#define SBE_2T3E3_21143_REG_RECEIVE_LIST_BASE_ADDRESS			3
-#define SBE_2T3E3_21143_REG_TRANSMIT_LIST_BASE_ADDRESS			4
-#define SBE_2T3E3_21143_REG_STATUS					5
-#define SBE_2T3E3_21143_REG_OPERATION_MODE				6
-#define SBE_2T3E3_21143_REG_INTERRUPT_ENABLE				7
-#define SBE_2T3E3_21143_REG_MISSED_FRAMES_AND_OVERFLOW_COUNTER		8
-#define SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT	9
-#define SBE_2T3E3_21143_REG_BOOT_ROM_PROGRAMMING_ADDRESS		10
-#define SBE_2T3E3_21143_REG_GENERAL_PURPOSE_TIMER_AND_INTERRUPT_MITIGATION_CONTROL 11
-#define SBE_2T3E3_21143_REG_SIA_STATUS					12
-#define SBE_2T3E3_21143_REG_SIA_CONNECTIVITY				13
-#define SBE_2T3E3_21143_REG_SIA_TRANSMIT_AND_RECEIVE			14
-#define SBE_2T3E3_21143_REG_SIA_AND_GENERAL_PURPOSE_PORT		15
-#define SBE_2T3E3_21143_REG_MAX						16
-
-/* CSR0 - BUS_MODE */
-#define SBE_2T3E3_21143_VAL_WRITE_AND_INVALIDATE_ENABLE		0x01000000
-#define SBE_2T3E3_21143_VAL_READ_LINE_ENABLE			0x00800000
-#define SBE_2T3E3_21143_VAL_READ_MULTIPLE_ENABLE		0x00200000
-#define SBE_2T3E3_21143_VAL_TRANSMIT_AUTOMATIC_POLLING_200us	0x00020000
-#define SBE_2T3E3_21143_VAL_TRANSMIT_AUTOMATIC_POLLING_DISABLED	0x00000000
-#define SBE_2T3E3_21143_VAL_CACHE_ALIGNMENT_32			0x0000c000
-#define SBE_2T3E3_21143_VAL_CACHE_ALIGNMENT_16			0x00008000
-#define SBE_2T3E3_21143_VAL_CACHE_ALIGNMENT_8			0x00004000
-#define SBE_2T3E3_21143_VAL_BUS_ARBITRATION_RR			0x00000002
-#define SBE_2T3E3_21143_VAL_SOFTWARE_RESET			0x00000001
-
-/* CSR5 - STATUS */
-#define SBE_2T3E3_21143_VAL_GENERAL_PURPOSE_PORT_INTERRUPT	0x04000000
-#define SBE_2T3E3_21143_VAL_ERROR_BITS				0x03800000
-#define SBE_2T3E3_21143_VAL_PARITY_ERROR			0x00000000
-#define SBE_2T3E3_21143_VAL_MASTER_ABORT			0x00800000
-#define SBE_2T3E3_21143_VAL_TARGET_ABORT			0x01000000
-#define SBE_2T3E3_21143_VAL_TRANSMISSION_PROCESS_STATE		0x00700000
-#define SBE_2T3E3_21143_VAL_TX_STOPPED				0x00000000
-#define SBE_2T3E3_21143_VAL_TX_SUSPENDED			0x00600000
-#define SBE_2T3E3_21143_VAL_RECEIVE_PROCESS_STATE		0x000e0000
-#define SBE_2T3E3_21143_VAL_RX_STOPPED				0x00000000
-#define SBE_2T3E3_21143_VAL_RX_SUSPENDED			0x000a0000
-#define SBE_2T3E3_21143_VAL_NORMAL_INTERRUPT_SUMMARY		0x00010000
-#define SBE_2T3E3_21143_VAL_ABNORMAL_INTERRUPT_SUMMARY		0x00008000
-#define SBE_2T3E3_21143_VAL_EARLY_RECEIVE_INTERRUPT		0x00004000
-#define SBE_2T3E3_21143_VAL_FATAL_BUS_ERROR			0x00002000
-#define SBE_2T3E3_21143_VAL_GENERAL_PURPOSE_TIMER_EXPIRED	0x00000800
-#define SBE_2T3E3_21143_VAL_EARLY_TRANSMIT_INTERRUPT		0x00000400
-#define SBE_2T3E3_21143_VAL_RECEIVE_WATCHDOG_TIMEOUT		0x00000200
-#define SBE_2T3E3_21143_VAL_RECEIVE_PROCESS_STOPPED		0x00000100
-#define SBE_2T3E3_21143_VAL_RECEIVE_BUFFER_UNAVAILABLE		0x00000080
-#define SBE_2T3E3_21143_VAL_RECEIVE_INTERRUPT			0x00000040
-#define SBE_2T3E3_21143_VAL_TRANSMIT_UNDERFLOW			0x00000020
-#define SBE_2T3E3_21143_VAL_TRANSMIT_JABBER_TIMEOUT		0x00000008
-#define SBE_2T3E3_21143_VAL_TRANSMIT_BUFFER_UNAVAILABLE		0x00000004
-#define SBE_2T3E3_21143_VAL_TRANSMIT_PROCESS_STOPPED		0x00000002
-#define SBE_2T3E3_21143_VAL_TRANSMIT_INTERRUPT			0x00000001
-
-/* CSR6 - OPERATION_MODE */
-#define SBE_2T3E3_21143_VAL_SPECIAL_CAPTURE_EFFECT_ENABLE	0x80000000
-#define SBE_2T3E3_21143_VAL_RECEIVE_ALL				0x40000000
-#define SBE_2T3E3_21143_VAL_MUST_BE_ONE				0x02000000
-#define SBE_2T3E3_21143_VAL_SCRAMBLER_MODE			0x01000000
-#define SBE_2T3E3_21143_VAL_PCS_FUNCTION			0x00800000
-#define SBE_2T3E3_21143_VAL_TRANSMIT_THRESHOLD_MODE_10Mbs	0x00400000
-#define SBE_2T3E3_21143_VAL_TRANSMIT_THRESHOLD_MODE_100Mbs	0x00000000
-#define SBE_2T3E3_21143_VAL_STORE_AND_FORWARD			0x00200000
-#define SBE_2T3E3_21143_VAL_HEARTBEAT_DISABLE			0x00080000
-#define SBE_2T3E3_21143_VAL_PORT_SELECT				0x00040000
-#define SBE_2T3E3_21143_VAL_CAPTURE_EFFECT_ENABLE		0x00020000
-#define SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS		0x0000c000
-#define SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_1		0x00000000
-#define SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_2		0x00004000
-#define SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_3		0x00008000
-#define SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_4		0x0000c000
-#define SBE_2T3E3_21143_VAL_TRANSMISSION_START			0x00002000
-#define SBE_2T3E3_21143_VAL_OPERATING_MODE			0x00000c00
-#define SBE_2T3E3_21143_VAL_LOOPBACK_OFF			0x00000000
-#define SBE_2T3E3_21143_VAL_LOOPBACK_EXTERNAL			0x00000800
-#define SBE_2T3E3_21143_VAL_LOOPBACK_INTERNAL			0x00000400
-#define SBE_2T3E3_21143_VAL_FULL_DUPLEX_MODE			0x00000200
-#define SBE_2T3E3_21143_VAL_PASS_ALL_MULTICAST			0x00000080
-#define SBE_2T3E3_21143_VAL_PROMISCUOUS_MODE			0x00000040
-#define SBE_2T3E3_21143_VAL_PASS_BAD_FRAMES			0x00000008
-#define SBE_2T3E3_21143_VAL_RECEIVE_START			0x00000002
-
-/* CSR7 - INTERRUPT_ENABLE */
-#define SBE_2T3E3_21143_VAL_LINK_CHANGED_ENABLE			0x08000000
-#define SBE_2T3E3_21143_VAL_GENERAL_PURPOSE_PORT_ENABLE		0x04000000
-#define SBE_2T3E3_21143_VAL_NORMAL_INTERRUPT_SUMMARY_ENABLE	0x00010000
-#define SBE_2T3E3_21143_VAL_ABNORMAL_INTERRUPT_SUMMARY_ENABLE	0x00008000
-#define SBE_2T3E3_21143_VAL_EARLY_RECEIVE_INTERRUPT_ENABLE	0x00004000
-#define SBE_2T3E3_21143_VAL_FATAL_BUS_ERROR_ENABLE		0x00002000
-#define SBE_2T3E3_21143_VAL_LINK_FAIL_ENABLE			0x00001000
-#define SBE_2T3E3_21143_VAL_GENERAL_PURPOSE_TIMER_ENABLE	0x00000800
-#define SBE_2T3E3_21143_VAL_EARLY_TRANSMIT_INTERRUPT_ENABLE	0x00000400
-#define SBE_2T3E3_21143_VAL_RECEIVE_WATCHDOG_TIMEOUT_ENABLE	0x00000200
-#define SBE_2T3E3_21143_VAL_RECEIVE_STOPPED_ENABLE		0x00000100
-#define SBE_2T3E3_21143_VAL_RECEIVE_BUFFER_UNAVAILABLE_ENABLE	0x00000080
-#define SBE_2T3E3_21143_VAL_RECEIVE_INTERRUPT_ENABLE		0x00000040
-#define SBE_2T3E3_21143_VAL_TRANSMIT_UNDERFLOW_INTERRUPT_ENABLE	0x00000020
-#define SBE_2T3E3_21143_VAL_TRANSMIT_JABBER_TIMEOUT_ENABLE	0x00000008
-#define SBE_2T3E3_21143_VAL_TRANSMIT_BUFFER_UNAVAILABLE_ENABLE	0x00000004
-#define SBE_2T3E3_21143_VAL_TRANSMIT_STOPPED_ENABLE		0x00000002
-#define SBE_2T3E3_21143_VAL_TRANSMIT_INTERRUPT_ENABLE		0x00000001
-
-/* CSR8 - MISSED_FRAMES_AND_OVERFLOW_COUNTER */
-#define SBE_2T3E3_21143_VAL_OVERFLOW_COUNTER_OVERFLOW		0x10000000
-#define SBE_2T3E3_21143_VAL_OVERFLOW_COUNTER			0x0ffe0000
-#define SBE_2T3E3_21143_VAL_MISSED_FRAME_OVERFLOW		0x00010000
-#define SBE_2T3E3_21143_VAL_MISSED_FRAMES_COUNTER		0x0000ffff
-
-/* CSR9 - BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT */
-#define SBE_2T3E3_21143_VAL_MII_MANAGEMENT_DATA_IN		0x00080000
-#define SBE_2T3E3_21143_VAL_MII_MANAGEMENT_READ_MODE		0x00040000
-#define SBE_2T3E3_21143_VAL_MII_MANAGEMENT_DATA_OUT		0x00020000
-#define SBE_2T3E3_21143_VAL_MII_MANAGEMENT_CLOCK		0x00010000
-#define SBE_2T3E3_21143_VAL_READ_OPERATION			0x00004000
-#define SBE_2T3E3_21143_VAL_WRITE_OPERATION			0x00002000
-#define SBE_2T3E3_21143_VAL_BOOT_ROM_SELECT			0x00001000
-#define SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT			0x00000800
-#define SBE_2T3E3_21143_VAL_BOOT_ROM_DATA			0x000000ff
-#define SBE_2T3E3_21143_VAL_SERIAL_ROM_DATA_OUT			0x00000008
-#define SBE_2T3E3_21143_VAL_SERIAL_ROM_DATA_IN			0x00000004
-#define SBE_2T3E3_21143_VAL_SERIAL_ROM_CLOCK			0x00000002
-#define SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT		0x00000001
-
-/* CSR11 - GENERAL_PURPOSE_TIMER_AND_INTERRUPT_MITIGATION_CONTROL */
-#define SBE_2T3E3_21143_VAL_CYCLE_SIZE				0x80000000
-#define SBE_2T3E3_21143_VAL_TRANSMIT_TIMER			0x78000000
-#define SBE_2T3E3_21143_VAL_NUMBER_OF_TRANSMIT_PACKETS		0x07000000
-#define SBE_2T3E3_21143_VAL_RECEIVE_TIMER			0x00f00000
-#define SBE_2T3E3_21143_VAL_NUMBER_OF_RECEIVE_PACKETS		0x000e0000
-#define SBE_2T3E3_21143_VAL_CONTINUOUS_MODE			0x00010000
-#define SBE_2T3E3_21143_VAL_TIMER_VALUE				0x0000ffff
-
-/* CSR12 - SIA_STATUS */
-#define SBE_2T3E3_21143_VAL_10BASE_T_RECEIVE_PORT_ACTIVITY	0x00000200
-#define SBE_2T3E3_21143_VAL_AUI_RECEIVE_PORT_ACTIVITY		0x00000100
-#define SBE_2T3E3_21143_VAL_10Mbs_LINK_STATUS			0x00000004
-#define SBE_2T3E3_21143_VAL_100Mbs_LINK_STATUS			0x00000002
-#define SBE_2T3E3_21143_VAL_MII_RECEIVE_PORT_ACTIVITY		0x00000001
-
-/* CSR13 - SIA_CONNECTIVITY */
-#define SBE_2T3E3_21143_VAL_10BASE_T_OR_AUI			0x00000008
-#define SBE_2T3E3_21143_VAL_SIA_RESET				0x00000001
-
-/* CSR14 - SIA_TRANSMIT_AND_RECEIVE */
-#define SBE_2T3E3_21143_VAL_100BASE_TX_FULL_DUPLEX		0x00020000
-#define SBE_2T3E3_21143_VAL_COLLISION_DETECT_ENABLE		0x00000400
-#define SBE_2T3E3_21143_VAL_COLLISION_SQUELCH_ENABLE		0x00000200
-#define SBE_2T3E3_21143_VAL_RECEIVE_SQUELCH_ENABLE		0x00000100
-#define SBE_2T3E3_21143_VAL_LINK_PULSE_SEND_ENABLE		0x00000004
-#define SBE_2T3E3_21143_VAL_ENCODER_ENABLE			0x00000001
-
-/* CSR15 - SIA_AND_GENERAL_PURPOSE_PORT */
-#define SBE_2T3E3_21143_VAL_RECEIVE_WATCHDOG_DISABLE		0x00000010
-#define SBE_2T3E3_21143_VAL_AUI_BNC_MODE			0x00000008
-#define SBE_2T3E3_21143_VAL_HOST_UNJAB				0x00000002
-#define SBE_2T3E3_21143_VAL_JABBER_DISABLE			0x00000001
-
-/**************************************************************
- *  CPLD
- **************************************************************/
-
-/* reg_map indexes */
-#define SBE_2T3E3_CPLD_REG_PCRA				0
-#define SBE_2T3E3_CPLD_REG_PCRB				1
-#define SBE_2T3E3_CPLD_REG_PLCR				2
-#define SBE_2T3E3_CPLD_REG_PLTR				3
-#define SBE_2T3E3_CPLD_REG_PPFR				4
-#define SBE_2T3E3_CPLD_REG_BOARD_ID			5
-#define SBE_2T3E3_CPLD_REG_FPGA_VERSION			6
-#define SBE_2T3E3_CPLD_REG_FRAMER_BASE_ADDRESS		7
-#define SBE_2T3E3_CPLD_REG_SERIAL_CHIP_SELECT		8
-#define SBE_2T3E3_CPLD_REG_STATIC_RESET			9
-#define SBE_2T3E3_CPLD_REG_PULSE_RESET			10
-#define SBE_2T3E3_CPLD_REG_FPGA_RECONFIGURATION		11
-#define SBE_2T3E3_CPLD_REG_LEDR				12
-#define SBE_2T3E3_CPLD_REG_PICSR			13
-#define SBE_2T3E3_CPLD_REG_PIER				14
-#define SBE_2T3E3_CPLD_REG_PCRC				15
-#define SBE_2T3E3_CPLD_REG_PBWF				16
-#define SBE_2T3E3_CPLD_REG_PBWL				17
-
-#define SBE_2T3E3_CPLD_REG_MAX				18
-
-/**********/
-
-/* val_map indexes */
-#define SBE_2T3E3_CPLD_VAL_LIU_SELECT			0
-#define SBE_2T3E3_CPLD_VAL_DAC_SELECT			1
-#define SBE_2T3E3_CPLD_VAL_LOOP_TIMING_SOURCE		2
-#define SBE_2T3E3_CPLD_VAL_LIU_FRAMER_RESET		3
-
-/* PCRA */
-#define SBE_2T3E3_CPLD_VAL_CRC32				0x40
-#define SBE_2T3E3_CPLD_VAL_TRANSPARENT_MODE			0x20
-#define SBE_2T3E3_CPLD_VAL_REAR_PANEL				0x10
-#define SBE_2T3E3_CPLD_VAL_RAW_MODE				0x08
-#define SBE_2T3E3_CPLD_VAL_ALT					0x04
-#define SBE_2T3E3_CPLD_VAL_LOOP_TIMING				0x02
-#define SBE_2T3E3_CPLD_VAL_LOCAL_CLOCK_E3			0x01
-
-/* PCRB */
-#define SBE_2T3E3_CPLD_VAL_PAD_COUNT				0x30
-#define SBE_2T3E3_CPLD_VAL_PAD_COUNT_1				0x00
-#define SBE_2T3E3_CPLD_VAL_PAD_COUNT_2				0x10
-#define SBE_2T3E3_CPLD_VAL_PAD_COUNT_3				0x20
-#define SBE_2T3E3_CPLD_VAL_PAD_COUNT_4				0x30
-#define SBE_2T3E3_CPLD_VAL_SCRAMBLER_TYPE			0x02
-#define SBE_2T3E3_CPLD_VAL_SCRAMBLER_ENABLE			0x01
-
-/* PCRC */
-#define SBE_2T3E3_CPLD_VAL_FRACTIONAL_MODE_NONE			0x00
-#define SBE_2T3E3_CPLD_VAL_FRACTIONAL_MODE_0			0x01
-#define SBE_2T3E3_CPLD_VAL_FRACTIONAL_MODE_1			0x11
-#define SBE_2T3E3_CPLD_VAL_FRACTIONAL_MODE_2			0x21
-
-/* PLTR */
-#define SBE_2T3E3_CPLD_VAL_LCV_COUNTER				0xff
-
-/* SCSR */
-#define SBE_2T3E3_CPLD_VAL_EEPROM_SELECT			0x10
-
-/* PICSR */
-#define SBE_2T3E3_CPLD_VAL_LOSS_OF_SIGNAL_THRESHOLD_LEVEL_1	0x80
-#define SBE_2T3E3_CPLD_VAL_RECEIVE_LOSS_OF_SIGNAL_CHANGE	0x40
-#define SBE_2T3E3_CPLD_VAL_INTERRUPT_FROM_ETHERNET_ASSERTED	0x20
-#define SBE_2T3E3_CPLD_VAL_INTERRUPT_FROM_FRAMER_ASSERTED	0x10
-#define SBE_2T3E3_CPLD_VAL_LCV_LIMIT_EXCEEDED			0x08
-#define SBE_2T3E3_CPLD_VAL_DMO_SIGNAL_DETECTED			0x04
-#define SBE_2T3E3_CPLD_VAL_RECEIVE_LOSS_OF_LOCK_DETECTED	0x02
-#define SBE_2T3E3_CPLD_VAL_RECEIVE_LOSS_OF_SIGNAL_DETECTED	0x01
-
-/* PIER */
-#define SBE_2T3E3_CPLD_VAL_RECEIVE_LOS_CHANGE_ENABLE		0x40
-#define SBE_2T3E3_CPLD_VAL_INTERRUPT_FROM_ETHERNET_ENABLE	0x20
-#define SBE_2T3E3_CPLD_VAL_INTERRUPT_FROM_FRAMER_ENABLE		0x10
-#define SBE_2T3E3_CPLD_VAL_LCV_INTERRUPT_ENABLE			0x08
-#define SBE_2T3E3_CPLD_VAL_DMO_ENABLE				0x04
-#define SBE_2T3E3_CPLD_VAL_RECEIVE_LOSS_OF_LOCK_ENABLE		0x02
-#define SBE_2T3E3_CPLD_VAL_RECEIVE_LOSS_OF_SIGNAL_ENABLE	0x01
-
-/**************************************************************
- *  Framer
- **************************************************************/
-
-/* reg_map indexes */
-/* common */
-#define SBE_2T3E3_FRAMER_REG_OPERATING_MODE				0
-#define SBE_2T3E3_FRAMER_REG_IO_CONTROL					1
-#define SBE_2T3E3_FRAMER_REG_BLOCK_INTERRUPT_ENABLE			2
-#define SBE_2T3E3_FRAMER_REG_BLOCK_INTERRUPT_STATUS			3
-#define SBE_2T3E3_FRAMER_REG_PMON_LCV_EVENT_COUNT_MSB			28
-#define SBE_2T3E3_FRAMER_REG_PMON_LCV_EVENT_COUNT_LSB			29
-#define SBE_2T3E3_FRAMER_REG_PMON_FRAMING_BIT_ERROR_EVENT_COUNT_MSB	30
-#define SBE_2T3E3_FRAMER_REG_PMON_FRAMING_BIT_ERROR_EVENT_COUNT_LSB	31
-#define SBE_2T3E3_FRAMER_REG_PMON_PARITY_ERROR_EVENT_COUNT_MSB		32
-#define SBE_2T3E3_FRAMER_REG_PMON_PARITY_ERROR_EVENT_COUNT_LSB		33
-#define SBE_2T3E3_FRAMER_REG_PMON_FEBE_EVENT_COUNT_MSB			34
-#define SBE_2T3E3_FRAMER_REG_PMON_FEBE_EVENT_COUNT_LSB			35
-#define SBE_2T3E3_FRAMER_REG_PMON_CP_BIT_ERROR_EVENT_COUNT_MSB		36
-#define SBE_2T3E3_FRAMER_REG_PMON_CP_BIT_ERROR_EVENT_COUNT_LSB		37
-#define SBE_2T3E3_FRAMER_REG_PMON_HOLDING_REGISTER			38
-#define SBE_2T3E3_FRAMER_REG_ONE_SECOND_ERROR_STATUS			39
-#define SBE_2T3E3_FRAMER_REG_LCV_ONE_SECOND_ACCUMULATOR_MSB		40
-#define SBE_2T3E3_FRAMER_REG_LCV_ONE_SECOND_ACCUMULATOR_LSB		41
-#define SBE_2T3E3_FRAMER_REG_FRAME_PARITY_ERROR_ONE_SECOND_ACCUMULATOR_MSB  42
-#define SBE_2T3E3_FRAMER_REG_FRAME_PARITY_ERROR_ONE_SECOND_ACCUMULATOR_LSB  43
-#define SBE_2T3E3_FRAMER_REG_FRAME_CP_BIT_ERROR_ONE_SECOND_ACCUMULATOR_MSB  44
-#define SBE_2T3E3_FRAMER_REG_FRAME_CP_BIT_ERROR_ONE_SECOND_ACCUMULATOR_LSB  45
-#define SBE_2T3E3_FRAMER_REG_LINE_INTERFACE_DRIVE			46
-#define SBE_2T3E3_FRAMER_REG_LINE_INTERFACE_SCAN			47
-
-/* T3 */
-#define SBE_2T3E3_FRAMER_REG_T3_RX_CONFIGURATION_STATUS			4
-#define SBE_2T3E3_FRAMER_REG_T3_RX_STATUS				5
-#define SBE_2T3E3_FRAMER_REG_T3_RX_INTERRUPT_ENABLE			6
-#define SBE_2T3E3_FRAMER_REG_T3_RX_INTERRUPT_STATUS			7
-#define SBE_2T3E3_FRAMER_REG_T3_RX_SYNC_DETECT_ENABLE			8
-#define SBE_2T3E3_FRAMER_REG_T3_RX_FEAC					10
-#define SBE_2T3E3_FRAMER_REG_T3_RX_FEAC_INTERRUPT_ENABLE_STATUS		11
-#define SBE_2T3E3_FRAMER_REG_T3_RX_LAPD_CONTROL				12
-#define SBE_2T3E3_FRAMER_REG_T3_RX_LAPD_STATUS				13
-#define SBE_2T3E3_FRAMER_REG_T3_TX_CONFIGURATION			16
-#define SBE_2T3E3_FRAMER_REG_T3_TX_FEAC_CONFIGURATION_STATUS		17
-#define SBE_2T3E3_FRAMER_REG_T3_TX_FEAC					18
-#define SBE_2T3E3_FRAMER_REG_T3_TX_LAPD_CONFIGURATION			19
-#define SBE_2T3E3_FRAMER_REG_T3_TX_LAPD_STATUS				20
-#define SBE_2T3E3_FRAMER_REG_T3_TX_MBIT_MASK				21
-#define SBE_2T3E3_FRAMER_REG_T3_TX_FBIT_MASK				22
-#define SBE_2T3E3_FRAMER_REG_T3_TX_FBIT_MASK_2				23
-#define SBE_2T3E3_FRAMER_REG_T3_TX_FBIT_MASK_3				24
-
-/* E3 */
-#define SBE_2T3E3_FRAMER_REG_E3_RX_CONFIGURATION_STATUS_1		4
-#define SBE_2T3E3_FRAMER_REG_E3_RX_CONFIGURATION_STATUS_2		5
-#define SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_ENABLE_1			6
-#define SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_ENABLE_2			7
-#define SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_STATUS_1			8
-#define SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_STATUS_2			9
-#define SBE_2T3E3_FRAMER_REG_E3_RX_LAPD_CONTROL				12
-#define SBE_2T3E3_FRAMER_REG_E3_RX_LAPD_STATUS				13
-#define SBE_2T3E3_FRAMER_REG_E3_RX_NR_BYTE				14
-#define SBE_2T3E3_FRAMER_REG_E3_RX_SERVICE_BITS				14
-#define SBE_2T3E3_FRAMER_REG_E3_RX_GC_BYTE				15
-#define SBE_2T3E3_FRAMER_REG_E3_TX_CONFIGURATION			16
-#define SBE_2T3E3_FRAMER_REG_E3_TX_LAPD_CONFIGURATION			19
-#define SBE_2T3E3_FRAMER_REG_E3_TX_LAPD_STATUS				19
-#define SBE_2T3E3_FRAMER_REG_E3_TX_GC_BYTE				21
-#define SBE_2T3E3_FRAMER_REG_E3_TX_SERVICE_BITS				21
-#define SBE_2T3E3_FRAMER_REG_E3_TX_MA_BYTE				22
-#define SBE_2T3E3_FRAMER_REG_E3_TX_NR_BYTE				23
-#define SBE_2T3E3_FRAMER_REG_E3_TX_FA1_ERROR_MASK			25
-#define SBE_2T3E3_FRAMER_REG_E3_TX_FAS_ERROR_MASK_UPPER			25
-#define SBE_2T3E3_FRAMER_REG_E3_TX_FA2_ERROR_MASK			26
-#define SBE_2T3E3_FRAMER_REG_E3_TX_FAS_ERROR_MASK_LOWER			26
-#define SBE_2T3E3_FRAMER_REG_E3_TX_BIP8_MASK				27
-#define SBE_2T3E3_FRAMER_REG_E3_TX_BIP4_MASK				27
-
-#define SBE_2T3E3_FRAMER_REG_MAX					48
-
-/**********/
-
-/* OPERATING_MODE */
-#define SBE_2T3E3_FRAMER_VAL_LOCAL_LOOPBACK_MODE		0x80
-#define SBE_2T3E3_FRAMER_VAL_T3_E3_SELECT			0x40
-#define SBE_2T3E3_FRAMER_VAL_INTERNAL_LOS_ENABLE		0x20
-#define SBE_2T3E3_FRAMER_VAL_RESET				0x10
-#define SBE_2T3E3_FRAMER_VAL_INTERRUPT_ENABLE_RESET		0x08
-#define SBE_2T3E3_FRAMER_VAL_FRAME_FORMAT_SELECT		0x04
-#define SBE_2T3E3_FRAMER_VAL_TIMING_ASYNCH_TXINCLK		0x03
-#define SBE_2T3E3_FRAMER_VAL_E3_G751				0x00
-#define SBE_2T3E3_FRAMER_VAL_E3_G832				0x04
-#define SBE_2T3E3_FRAMER_VAL_T3_CBIT				0x40
-#define SBE_2T3E3_FRAMER_VAL_T3_M13				0x44
-#define SBE_2T3E3_FRAMER_VAL_LOOPBACK_ON			0x80
-#define SBE_2T3E3_FRAMER_VAL_LOOPBACK_OFF			0x00
-
-/* IO_CONTROL */
-#define SBE_2T3E3_FRAMER_VAL_DISABLE_TX_LOSS_OF_CLOCK		0x80
-#define SBE_2T3E3_FRAMER_VAL_LOSS_OF_CLOCK_STATUS		0x40
-#define SBE_2T3E3_FRAMER_VAL_DISABLE_RX_LOSS_OF_CLOCK		0x20
-#define SBE_2T3E3_FRAMER_VAL_AMI_LINE_CODE			0x10
-#define SBE_2T3E3_FRAMER_VAL_UNIPOLAR				0x08
-#define SBE_2T3E3_FRAMER_VAL_TX_LINE_CLOCK_INVERT		0x04
-#define SBE_2T3E3_FRAMER_VAL_RX_LINE_CLOCK_INVERT		0x02
-#define SBE_2T3E3_FRAMER_VAL_REFRAME				0x01
-
-/* BLOCK_INTERRUPT_ENABLE */
-#define SBE_2T3E3_FRAMER_VAL_RX_INTERRUPT_ENABLE		0x80
-#define SBE_2T3E3_FRAMER_VAL_TX_INTERRUPT_ENABLE		0x02
-#define SBE_2T3E3_FRAMER_VAL_ONE_SECOND_INTERRUPT_ENABLE	0x01
-
-/* BLOCK_INTERRUPT_STATUS */
-#define SBE_2T3E3_FRAMER_VAL_RX_INTERRUPT_STATUS		0x80
-#define SBE_2T3E3_FRAMER_VAL_TX_INTERRUPT_STATUS		0x02
-#define SBE_2T3E3_FRAMER_VAL_ONE_SECOND_INTERRUPT_STATUS	0x01
-
-/**********/
-
-/* T3_RX_CONFIGURATION_STATUS */
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_AIS				0x80
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_LOS				0x40
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_IDLE				0x20
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_OOF				0x10
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FRAMING_ON_PARITY		0x04
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_F_SYNC_ALGO			0x02
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_M_SYNC_ALGO			0x01
-
-/* T3_RX_STATUS */
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FERF				0x10
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_AIC				0x04
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FEBE				0x07
-
-/* T3_RX_INTERRUPT_ENABLE */
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_CP_BIT_ERROR_INTERRUPT_ENABLE 0x80
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_LOS_INTERRUPT_ENABLE		0x40
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_AIS_INTERRUPT_ENABLE		0x20
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_IDLE_INTERRUPT_ENABLE	0x10
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FERF_INTERRUPT_ENABLE	0x08
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_AIC_INTERRUPT_ENABLE		0x04
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_OOF_INTERRUPT_ENABLE		0x02
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_P_BIT_INTERRUPT_ENABLE	0x01
-
-/* T3_RX_INTERRUPT_STATUS */
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_CP_BIT_ERROR_INTERRUPT_STATUS 0x80
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_LOS_INTERRUPT_STATUS		0x40
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_AIS_INTERRUPT_STATUS		0x20
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_IDLE_INTERRUPT_STATUS	0x10
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FERF_INTERRUPT_STATUS	0x08
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_AIC_INTERRUPT_STATUS		0x04
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_OOF_INTERRUPT_STATUS		0x02
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_P_BIT_INTERRUPT_STATUS	0x01
-
-/* T3_RX_FEAC_INTERRUPT_ENABLE_STATUS */
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FEAC_VALID			0x10
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FEAC_REMOVE_INTERRUPT_ENABLE	0x08
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FEAC_REMOVE_INTERRUPT_STATUS	0x04
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FEAC_VALID_INTERRUPT_ENABLE	0x02
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FEAC_VALID_INTERRUPT_STATUS	0x01
-
-/* T3_RX_LAPD_CONTROL */
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_LAPD_ENABLE			0x04
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_LAPD_INTERRUPT_ENABLE	0x02
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_LAPD_INTERRUPT_STATUS	0x01
-
-/* T3_RX_LAPD_STATUS */
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_ABORT			0x40
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_LAPD_TYPE			0x30
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_CR_TYPE			0x08
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FCS_ERROR			0x04
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_END_OF_MESSAGE		0x02
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FLAG_PRESENT			0x01
-
-/* T3_TX_CONFIGURATION */
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_YELLOW_ALARM			0x80
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_X_BIT			0x40
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_IDLE				0x20
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_AIS				0x10
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_LOS				0x08
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_FERF_ON_LOS			0x04
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_FERF_ON_OOF			0x02
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_FERF_ON_AIS			0x01
-
-/* T3_TX_FEAC_CONFIGURATION_STATUS */
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_FEAC_INTERRUPT_ENABLE	0x10
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_FEAC_INTERRUPT_STATUS	0x08
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_FEAC_ENABLE			0x04
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_FEAC_GO			0x02
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_FEAC_BUSY			0x01
-
-/* T3_TX_LAPD_STATUS */
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_DL_START			0x08
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_DL_BUSY			0x04
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_LAPD_INTERRUPT_ENABLE	0x02
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_LAPD_INTERRUPT_STATUS	0x01
-
-/**********/
-
-/* E3_RX_CONFIGURATION_STATUS_1 */
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_PAYLOAD_TYPE			0xe0
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_FERF_ALGO			0x10
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_T_MARK_ALGO			0x08
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_PAYLOAD_EXPECTED		0x07
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_BIP4				0x01
-
-/* E3_RX_CONFIGURATION_STATUS_2 */
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_LOF_ALGO			0x80
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_LOF				0x40
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_OOF				0x20
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_LOS				0x10
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_AIS				0x08
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_PAYLOAD_UNSTABLE		0x04
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_T_MARK			0x02
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_FERF				0x01
-
-/* E3_RX_INTERRUPT_ENABLE_1 */
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_COFA_INTERRUPT_ENABLE	0x10
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_OOF_INTERRUPT_ENABLE		0x08
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_LOF_INTERRUPT_ENABLE		0x04
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_LOS_INTERRUPT_ENABLE		0x02
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_AIS_INTERRUPT_ENABLE		0x01
-
-/* E3_RX_INTERRUPT_ENABLE_2 */
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_TTB_CHANGE_INTERRUPT_ENABLE	0x40
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_FEBE_INTERRUPT_ENABLE	0x10
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_FERF_INTERRUPT_ENABLE	0x08
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_BIP8_ERROR_INTERRUPT_ENABLE	0x04
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_BIP4_ERROR_INTERRUPT_ENABLE	0x04
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_FRAMING_BYTE_ERROR_INTERRUPT_ENABLE 0x02
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_PAYLOAD_MISMATCH_INTERRUPT_ENABLE 0x01
-
-/* E3_RX_INTERRUPT_STATUS_1 */
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_COFA_INTERRUPT_STATUS	0x10
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_OOF_INTERRUPT_STATUS		0x08
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_LOF_INTERRUPT_STATUS		0x04
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_LOS_INTERRUPT_STATUS		0x02
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_AIS_INTERRUPT_STATUS		0x01
-
-/* E3_RX_INTERRUPT_STATUS_2 */
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_TTB_CHANGE_INTERRUPT_STATUS	0x40
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_FEBE_INTERRUPT_STATUS	0x10
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_FERF_INTERRUPT_STATUS	0x08
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_BIP8_ERROR_INTERRUPT_STATUS	0x04
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_BIP4_ERROR_INTERRUPT_STATUS	0x04
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_FRAMING_BYTE_ERROR_INTERRUPT_STATUS 0x02
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_PAYLOAD_MISMATCH_INTERRUPT_STATUS 0x01
-
-/* E3_RX_LAPD_CONTROL */
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_DL_FROM_NR			0x08
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_LAPD_ENABLE			0x04
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_LAPD_INTERRUPT_ENABLE	0x02
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_LAPD_INTERRUPT_STATUS	0x01
-
-/* E3_RX_LAPD_STATUS */
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_ABORT			0x40
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_LAPD_TYPE			0x30
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_CR_TYPE			0x08
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_FCS_ERROR			0x04
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_END_OF_MESSAGE		0x02
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_FLAG_PRESENT			0x01
-
-/* E3_TX_CONFIGURATION */
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_BIP4_ENABLE			0x80
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_A_SOURCE_SELECT		0x60
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_DL_IN_NR			0x10
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_N_SOURCE_SELECT		0x18
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_AIS_ENABLE			0x04
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_LOS_ENABLE			0x02
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_MA_RX			0x01
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_FAS_SOURCE_SELECT		0x01
-
-/* E3_TX_LAPD_CONFIGURATION */
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_AUTO_RETRANSMIT		0x08
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_LAPD_MESSAGE_LENGTH		0x02
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_LAPD_ENABLE			0x01
-
-/* E3_TX_LAPD_STATUS_INTERRUPT */
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_DL_START			0x08
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_DL_BUSY			0x04
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_LAPD_INTERRUPT_ENABLE	0x02
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_LAPD_INTERRUPT_STATUS	0x01
-
-
-
-
-
-
-/**************************************************************
- *  LIU
- **************************************************************/
-
-/* reg_map indexes */
-#define SBE_2T3E3_LIU_REG_REG0			0
-#define SBE_2T3E3_LIU_REG_REG1			1
-#define SBE_2T3E3_LIU_REG_REG2			2
-#define SBE_2T3E3_LIU_REG_REG3			3
-#define SBE_2T3E3_LIU_REG_REG4			4
-
-#define	SBE_2T3E3_LIU_REG_MAX			5
-
-/**********/
-
-/* REG0 */
-#define SBE_2T3E3_LIU_VAL_RECEIVE_LOSS_OF_LOCK_STATUS		0x10
-#define SBE_2T3E3_LIU_VAL_RECEIVE_LOSS_OF_SIGNAL_STATUS		0x08
-#define SBE_2T3E3_LIU_VAL_ANALOG_LOSS_OF_SIGNAL_STATUS		0x04
-#define SBE_2T3E3_LIU_VAL_DIGITAL_LOSS_OF_SIGNAL_STATUS		0x02
-#define SBE_2T3E3_LIU_VAL_DMO_STATUS				0x01
-
-/* REG1 */
-#define SBE_2T3E3_LIU_VAL_TRANSMITTER_OFF			0x10
-#define SBE_2T3E3_LIU_VAL_TRANSMIT_ALL_ONES			0x08
-#define SBE_2T3E3_LIU_VAL_TRANSMIT_CLOCK_INVERT			0x04
-#define SBE_2T3E3_LIU_VAL_TRANSMIT_LEVEL_SELECT			0x02
-#define SBE_2T3E3_LIU_VAL_TRANSMIT_BINARY_DATA			0x01
-
-/* REG2 */
-#define SBE_2T3E3_LIU_VAL_DECODER_DISABLE			0x10
-#define SBE_2T3E3_LIU_VAL_ENCODER_DISABLE			0x08
-#define SBE_2T3E3_LIU_VAL_ANALOG_LOSS_OF_SIGNAL_DISABLE		0x04
-#define SBE_2T3E3_LIU_VAL_DIGITAL_LOSS_OF_SIGNAL_DISABLE	0x02
-#define SBE_2T3E3_LIU_VAL_RECEIVE_EQUALIZATION_DISABLE		0x01
-
-/* REG3 */
-#define SBE_2T3E3_LIU_VAL_RECEIVE_BINARY_DATA			0x10
-#define SBE_2T3E3_LIU_VAL_RECOVERED_DATA_MUTING			0x08
-#define SBE_2T3E3_LIU_VAL_RECEIVE_CLOCK_OUTPUT_2		0x04
-#define SBE_2T3E3_LIU_VAL_INVERT_RECEIVE_CLOCK_2		0x02
-#define SBE_2T3E3_LIU_VAL_INVERT_RECEIVE_CLOCK_1		0x01
-
-/* REG4 */
-#define SBE_2T3E3_LIU_VAL_T3_MODE_SELECT			0x00
-#define SBE_2T3E3_LIU_VAL_E3_MODE_SELECT			0x04
-#define SBE_2T3E3_LIU_VAL_LOCAL_LOOPBACK			0x02
-#define SBE_2T3E3_LIU_VAL_REMOTE_LOOPBACK			0x01
-#define SBE_2T3E3_LIU_VAL_LOOPBACK_OFF				0x00
-#define SBE_2T3E3_LIU_VAL_LOOPBACK_REMOTE			0x01
-#define SBE_2T3E3_LIU_VAL_LOOPBACK_ANALOG			0x02
-#define SBE_2T3E3_LIU_VAL_LOOPBACK_DIGITAL			0x03
-
-/**********************************************************************
- *
- * descriptor list and data buffer
- *
- **********************************************************************/
-struct t3e3_rx_desc {
-	u32 rdes0;
-	u32 rdes1;
-	u32 rdes2;
-	u32 rdes3;
-};
-
-#define SBE_2T3E3_RX_DESC_RING_SIZE			64
-
-/* RDES0 */
-#define SBE_2T3E3_RX_DESC_21143_OWN			0X80000000
-#define SBE_2T3E3_RX_DESC_FRAME_LENGTH			0x3fff0000
-#define SBE_2T3E3_RX_DESC_FRAME_LENGTH_SHIFT		16
-#define SBE_2T3E3_RX_DESC_ERROR_SUMMARY			0x00008000
-#define SBE_2T3E3_RX_DESC_DESC_ERROR			0x00004000
-#define SBE_2T3E3_RX_DESC_DATA_TYPE			0x00003000
-#define SBE_2T3E3_RX_DESC_RUNT_FRAME			0x00000800
-#define SBE_2T3E3_RX_DESC_FIRST_DESC			0x00000200
-#define SBE_2T3E3_RX_DESC_LAST_DESC			0x00000100
-#define SBE_2T3E3_RX_DESC_FRAME_TOO_LONG		0x00000080
-#define SBE_2T3E3_RX_DESC_COLLISION_SEEN		0x00000040
-#define SBE_2T3E3_RX_DESC_FRAME_TYPE			0x00000020
-#define SBE_2T3E3_RX_DESC_RECEIVE_WATCHDOG		0x00000010
-#define SBE_2T3E3_RX_DESC_MII_ERROR			0x00000008
-#define SBE_2T3E3_RX_DESC_DRIBBLING_BIT			0x00000004
-#define SBE_2T3E3_RX_DESC_CRC_ERROR			0x00000002
-
-/* RDES1 */
-#define SBE_2T3E3_RX_DESC_END_OF_RING			0x02000000
-#define SBE_2T3E3_RX_DESC_SECOND_ADDRESS_CHAINED	0x01000000
-#define SBE_2T3E3_RX_DESC_BUFFER_2_SIZE			0x003ff800
-#define SBE_2T3E3_RX_DESC_BUFFER_1_SIZE			0x000007ff
-
-/*********************/
-
-struct t3e3_tx_desc {
-	u32 tdes0;
-	u32 tdes1;
-	u32 tdes2;
-	u32 tdes3;
-};
-
-#define SBE_2T3E3_TX_DESC_RING_SIZE			256
-
-/* TDES0 */
-#define SBE_2T3E3_TX_DESC_21143_OWN			0x80000000
-#define SBE_2T3E3_TX_DESC_ERROR_SUMMARY			0x00008000
-#define SBE_2T3E3_TX_DESC_TRANSMIT_JABBER_TIMEOUT	0x00004000
-#define SBE_2T3E3_TX_DESC_LOSS_OF_CARRIER		0x00000800
-#define SBE_2T3E3_TX_DESC_NO_CARRIER			0x00000400
-#define SBE_2T3E3_TX_DESC_LINK_FAIL_REPORT		0x00000004
-#define SBE_2T3E3_TX_DESC_UNDERFLOW_ERROR		0x00000002
-#define SBE_2T3E3_TX_DESC_DEFFERED			0x00000001
-
-/* TDES1 */
-#define SBE_2T3E3_TX_DESC_INTERRUPT_ON_COMPLETION	0x80000000
-#define SBE_2T3E3_TX_DESC_LAST_SEGMENT			0x40000000
-#define SBE_2T3E3_TX_DESC_FIRST_SEGMENT			0x20000000
-#define SBE_2T3E3_TX_DESC_CRC_DISABLE			0x04000000
-#define SBE_2T3E3_TX_DESC_END_OF_RING			0x02000000
-#define SBE_2T3E3_TX_DESC_SECOND_ADDRESS_CHAINED	0x01000000
-#define SBE_2T3E3_TX_DESC_DISABLE_PADDING		0x00800000
-#define SBE_2T3E3_TX_DESC_BUFFER_2_SIZE			0x003ff800
-#define SBE_2T3E3_TX_DESC_BUFFER_1_SIZE			0x000007ff
-
-
-#define SBE_2T3E3_MTU					1600
-#define SBE_2T3E3_CRC16_LENGTH				2
-#define SBE_2T3E3_CRC32_LENGTH				4
-
-#define MCLBYTES (SBE_2T3E3_MTU + 128)
-
-struct channel {
-	struct pci_dev *pdev;
-	struct net_device *dev;
-	struct card *card;
-	unsigned long addr;	/* DECchip */
-
-	int leds;
-
-	/* pci specific */
-	struct {
-		u32 slot;           /* should be 0 or 1 */
-		u32 command;
-		u8 cache_size;
-	} h;
-
-	/* statistics */
-	struct t3e3_stats s;
-
-	/* running */
-	struct {
-		u32 flags;
-	} r;
-
-	/* parameters */
-	struct t3e3_param p;
-
-	u32 liu_regs[SBE_2T3E3_LIU_REG_MAX];	   /* LIU registers */
-	u32 framer_regs[SBE_2T3E3_FRAMER_REG_MAX]; /* Framer registers */
-
-	/* Ethernet Controller */
-	struct {
-		u_int16_t card_serial_number[3];
-
-		u32 reg[SBE_2T3E3_21143_REG_MAX]; /* registers i.e. CSR */
-
-		u32 interrupt_enable_mask;
-
-		/* receive chain/ring */
-		struct t3e3_rx_desc *rx_ring;
-		struct sk_buff *rx_data[SBE_2T3E3_RX_DESC_RING_SIZE];
-		u32 rx_ring_current_read;
-
-		/* transmit chain/ring */
-		struct t3e3_tx_desc *tx_ring;
-		struct sk_buff *tx_data[SBE_2T3E3_TX_DESC_RING_SIZE];
-		u32 tx_ring_current_read;
-		u32 tx_ring_current_write;
-		int tx_full;
-		int tx_free_cnt;
-		spinlock_t tx_lock;
-	} ether;
-
-	int32_t interrupt_active;
-	int32_t rcv_count;
-};
-
-struct card {
-	spinlock_t bootrom_lock;
-	unsigned long bootrom_addr;
-	struct timer_list timer; /* for updating LEDs */
-	struct channel channels[0];
-};
-
-#define SBE_2T3E3_FLAG_NETWORK_UP		0x00000001
-#define SBE_2T3E3_FLAG_NO_ERROR_MESSAGES	0x00000002
-
-extern const u32 cpld_reg_map[][2];
-extern const u32 cpld_val_map[][2];
-extern const u32 t3e3_framer_reg_map[];
-extern const u32 t3e3_liu_reg_map[];
-
-void t3e3_init(struct channel *);
-void t3e3_if_up(struct channel *);
-void t3e3_if_down(struct channel *);
-int t3e3_if_start_xmit(struct sk_buff *skb, struct net_device *dev);
-void t3e3_if_config(struct channel *, u32, char *, struct t3e3_resp *, int *);
-void t3e3_set_frame_type(struct channel *, u32);
-u32 t3e3_eeprom_read_word(struct channel *, u32);
-void t3e3_read_card_serial_number(struct channel *);
-
-/* interrupt handlers */
-irqreturn_t t3e3_intr(int irq, void *dev_instance);
-void dc_intr(struct channel *);
-void dc_intr_rx(struct channel *);
-void dc_intr_tx(struct channel *);
-void dc_intr_tx_underflow(struct channel *);
-void exar7250_intr(struct channel *);
-void exar7250_E3_intr(struct channel *, u32);
-void exar7250_T3_intr(struct channel *, u32);
-
-/* Ethernet controller */
-u32 bootrom_read(struct channel *, u32);
-void bootrom_write(struct channel *, u32, u32);
-void dc_init(struct channel *);
-void dc_start(struct channel *);
-void dc_stop(struct channel *);
-void dc_start_intr(struct channel *);
-void dc_stop_intr(struct channel *);
-void dc_reset(struct channel *);
-void dc_restart(struct channel *);
-void dc_receiver_onoff(struct channel *, u32);
-void dc_transmitter_onoff(struct channel *, u32);
-void dc_set_loopback(struct channel *, u32);
-void dc_clear_descriptor_list(struct channel *);
-void dc_drop_descriptor_list(struct channel *);
-void dc_set_output_port(struct channel *);
-void t3e3_sc_init(struct channel *);
-
-/* CPLD */
-void cpld_init(struct channel *sc);
-u32 cpld_read(struct channel *sc, u32 reg);
-void cpld_set_crc(struct channel *, u32);
-void cpld_start_intr(struct channel *);
-void cpld_stop_intr(struct channel *);
-void cpld_set_clock(struct channel *sc, u32 mode);
-void cpld_set_scrambler(struct channel *, u32);
-void cpld_select_panel(struct channel *, u32);
-void cpld_set_frame_mode(struct channel *, u32);
-void cpld_set_frame_type(struct channel *, u32);
-void cpld_set_pad_count(struct channel *, u32);
-void cpld_set_fractional_mode(struct channel *, u32, u32, u32);
-void cpld_LOS_update(struct channel *);
-
-/* Framer */
-extern u32 exar7250_read(struct channel *, u32);
-extern void exar7250_write(struct channel *, u32, u32);
-void exar7250_init(struct channel *);
-void exar7250_start_intr(struct channel *, u32);
-void exar7250_stop_intr(struct channel *, u32);
-void exar7250_set_frame_type(struct channel *, u32);
-void exar7250_set_loopback(struct channel *, u32);
-void exar7250_unipolar_onoff(struct channel *, u32);
-
-/* LIU */
-u32 exar7300_read(struct channel *, u32);
-void exar7300_write(struct channel *, u32, u32);
-void exar7300_init(struct channel *);
-void exar7300_line_build_out_onoff(struct channel *, u32);
-void exar7300_set_frame_type(struct channel *, u32);
-void exar7300_set_loopback(struct channel *, u32);
-void exar7300_transmit_all_ones_onoff(struct channel *, u32);
-void exar7300_receive_equalization_onoff(struct channel *, u32);
-void exar7300_unipolar_onoff(struct channel *, u32);
-
-void update_led(struct channel *, int);
-int setup_device(struct net_device *dev, struct channel *sc);
-
-static inline int has_two_ports(struct pci_dev *pdev)
-{
-	return pdev->subsystem_device == PCI_SUBDEVICE_ID_SBE_2T3E3_P0;
-}
-
-#define dev_to_priv(dev) (*(struct channel **) ((hdlc_device *)(dev) + 1))
-
-static inline u32 dc_read(unsigned long addr, u32 reg)
-{
-	return inl(addr + (reg << 3));
-}
-
-static inline void dc_write(unsigned long addr, u32 reg, u32 val)
-{
-	outl(val, addr + (reg << 3));
-}
-
-static inline void dc_set_bits(unsigned long addr, u32 reg, u32 bits)
-{
-	dc_write(addr, reg, dc_read(addr, reg) | bits);
-}
-
-static inline void dc_clear_bits(unsigned long addr, u32 reg, u32 bits)
-{
-	dc_write(addr, reg, dc_read(addr, reg) & ~bits);
-}
-
-#define CPLD_MAP_REG(reg, sc)	(cpld_reg_map[(reg)][(sc)->h.slot])
-
-static inline void cpld_write(struct channel *channel, unsigned reg, u32 val)
-{
-	unsigned long flags;
-	spin_lock_irqsave(&channel->card->bootrom_lock, flags);
-	bootrom_write(channel, CPLD_MAP_REG(reg, channel), val);
-	spin_unlock_irqrestore(&channel->card->bootrom_lock, flags);
-}
-
-#define exar7250_set_bit(sc, reg, bit)			\
-	exar7250_write((sc), (reg),			\
-		       exar7250_read(sc, reg) | (bit))
-
-#define exar7250_clear_bit(sc, reg, bit)		\
-	exar7250_write((sc), (reg),			\
-		       exar7250_read(sc, reg) & ~(bit))
-
-#define exar7300_set_bit(sc, reg, bit)			\
-	exar7300_write((sc), (reg),			\
-		       exar7300_read(sc, reg) | (bit))
-
-#define exar7300_clear_bit(sc, reg, bit)		\
-	exar7300_write((sc), (reg),			\
-		       exar7300_read(sc, reg) & ~(bit))
-
-
-#endif /* T3E3_H */
diff --git a/drivers/staging/sbe-2t3e3/Kconfig b/drivers/staging/sbe-2t3e3/Kconfig
deleted file mode 100644
index 8ec86cfc6bf..00000000000
--- a/drivers/staging/sbe-2t3e3/Kconfig
+++ /dev/null
@@ -1,13 +0,0 @@
-config SBE_2T3E3
-	tristate "SBE wanPMC-2T3E3 support"
-	depends on HDLC && PCI
-	help
-	  Driver for wanPMC-2T3E3 cards by SBE Inc.
-
-	  If you have such a card, say Y here and see
-	  <http://www.kernel.org/pub/linux/utils/net/hdlc/>.
-
-	  To compile this as a module, choose M here: the
-	  module will be called sbe-2t3e3.
-
-	  If unsure, say N.
diff --git a/drivers/staging/sbe-2t3e3/Makefile b/drivers/staging/sbe-2t3e3/Makefile
deleted file mode 100644
index cce2a7af626..00000000000
--- a/drivers/staging/sbe-2t3e3/Makefile
+++ /dev/null
@@ -1,4 +0,0 @@
-obj-$(CONFIG_SBE_2T3E3) += sbe-2t3e3.o
-
-sbe-2t3e3-y := module.o netdev.o maps.o	\
-	main.o cpld.o intr.o ctrl.o io.o dc.o exar7250.o exar7300.o
diff --git a/drivers/staging/sbe-2t3e3/TODO b/drivers/staging/sbe-2t3e3/TODO
deleted file mode 100644
index 624b20f70cf..00000000000
--- a/drivers/staging/sbe-2t3e3/TODO
+++ /dev/null
@@ -1,6 +0,0 @@
-TODO:
-	- additional cleaning and tests
-	- wait for the new configuration interface in generic HDLC layer and
-	  when available, convert the driver to it
-
-Please send patches to Krzysztof Halasa <khc@pm.waw.pl>.
\ No newline at end of file
diff --git a/drivers/staging/sbe-2t3e3/cpld.c b/drivers/staging/sbe-2t3e3/cpld.c
deleted file mode 100644
index c6370d3d637..00000000000
--- a/drivers/staging/sbe-2t3e3/cpld.c
+++ /dev/null
@@ -1,351 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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 code is based on a driver written by SBE Inc.
- */
-
-#include <linux/delay.h>
-#include "2t3e3.h"
-#include "ctrl.h"
-
-#define bootrom_set_bit(sc, reg, bit)				\
-	bootrom_write((sc), (reg),				\
-		      bootrom_read((sc), (reg)) | (bit))
-
-#define bootrom_clear_bit(sc, reg, bit)				\
-	bootrom_write((sc), (reg),				\
-		      bootrom_read((sc), (reg)) & ~(bit))
-
-static inline void cpld_set_bit(struct channel *channel, unsigned reg, u32 bit)
-{
-	unsigned long flags;
-	spin_lock_irqsave(&channel->card->bootrom_lock, flags);
-	bootrom_set_bit(channel, CPLD_MAP_REG(reg, channel), bit);
-	spin_unlock_irqrestore(&channel->card->bootrom_lock, flags);
-}
-
-static inline void cpld_clear_bit(struct channel *channel, unsigned reg, u32 bit)
-{
-	unsigned long flags;
-	spin_lock_irqsave(&channel->card->bootrom_lock, flags);
-	bootrom_clear_bit(channel, CPLD_MAP_REG(reg, channel), bit);
-	spin_unlock_irqrestore(&channel->card->bootrom_lock, flags);
-}
-
-void cpld_init(struct channel *sc)
-{
-	u32 val;
-
-	/* PCRA */
-	val = SBE_2T3E3_CPLD_VAL_CRC32 |
-		cpld_val_map[SBE_2T3E3_CPLD_VAL_LOOP_TIMING_SOURCE][sc->h.slot];
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PCRA, val);
-
-	/* PCRB */
-	val = 0;
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PCRB, val);
-
-	/* PCRC */
-	val = 0;
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PCRC, val);
-
-	/* PBWF */
-	val = 0;
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PBWF, val);
-
-	/* PBWL */
-	val = 0;
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PBWL, val);
-
-	/* PLTR */
-	val = SBE_2T3E3_CPLD_VAL_LCV_COUNTER;
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PLTR, val);
-	udelay(1000);
-
-	/* PLCR */
-	val = 0;
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PLCR, val);
-	udelay(1000);
-
-	/* PPFR */
-	val = 0x55;
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PPFR, val);
-	/* TODO: this doesn't work!!! */
-
-	/* SERIAL_CHIP_SELECT */
-	val = 0;
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_SERIAL_CHIP_SELECT, val);
-
-	/* PICSR */
-	val = SBE_2T3E3_CPLD_VAL_DMO_SIGNAL_DETECTED |
-		SBE_2T3E3_CPLD_VAL_RECEIVE_LOSS_OF_LOCK_DETECTED |
-		SBE_2T3E3_CPLD_VAL_RECEIVE_LOSS_OF_SIGNAL_DETECTED;
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PICSR, val);
-
-	cpld_start_intr(sc);
-
-	udelay(1000);
-}
-
-void cpld_start_intr(struct channel *sc)
-{
-	u32 val;
-
-	/* PIER */
-	val = SBE_2T3E3_CPLD_VAL_INTERRUPT_FROM_ETHERNET_ENABLE |
-		SBE_2T3E3_CPLD_VAL_INTERRUPT_FROM_FRAMER_ENABLE;
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PIER, val);
-}
-
-void cpld_stop_intr(struct channel *sc)
-{
-	u32 val;
-
-	/* PIER */
-	val = 0;
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PIER, val);
-}
-
-void cpld_set_frame_mode(struct channel *sc, u32 mode)
-{
-	if (sc->p.frame_mode == mode)
-		return;
-
-	switch (mode) {
-	case SBE_2T3E3_FRAME_MODE_HDLC:
-		cpld_clear_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			       SBE_2T3E3_CPLD_VAL_TRANSPARENT_MODE |
-			       SBE_2T3E3_CPLD_VAL_RAW_MODE);
-		exar7250_unipolar_onoff(sc, SBE_2T3E3_OFF);
-		exar7300_unipolar_onoff(sc, SBE_2T3E3_OFF);
-		break;
-	case SBE_2T3E3_FRAME_MODE_TRANSPARENT:
-		cpld_clear_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			       SBE_2T3E3_CPLD_VAL_RAW_MODE);
-		cpld_set_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			     SBE_2T3E3_CPLD_VAL_TRANSPARENT_MODE);
-		exar7250_unipolar_onoff(sc, SBE_2T3E3_OFF);
-		exar7300_unipolar_onoff(sc, SBE_2T3E3_OFF);
-		break;
-	case SBE_2T3E3_FRAME_MODE_RAW:
-		cpld_set_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			     SBE_2T3E3_CPLD_VAL_RAW_MODE);
-		exar7250_unipolar_onoff(sc, SBE_2T3E3_ON);
-		exar7300_unipolar_onoff(sc, SBE_2T3E3_ON);
-		break;
-	default:
-		return;
-	}
-
-	sc->p.frame_mode = mode;
-}
-
-/* set rate of the local clock */
-void cpld_set_frame_type(struct channel *sc, u32 type)
-{
-	switch (type) {
-	case SBE_2T3E3_FRAME_TYPE_E3_G751:
-	case SBE_2T3E3_FRAME_TYPE_E3_G832:
-		cpld_set_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			     SBE_2T3E3_CPLD_VAL_LOCAL_CLOCK_E3);
-		break;
-	case SBE_2T3E3_FRAME_TYPE_T3_CBIT:
-	case SBE_2T3E3_FRAME_TYPE_T3_M13:
-		cpld_clear_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			       SBE_2T3E3_CPLD_VAL_LOCAL_CLOCK_E3);
-		break;
-	default:
-		return;
-	}
-}
-
-void cpld_set_scrambler(struct channel *sc, u32 mode)
-{
-	if (sc->p.scrambler == mode)
-		return;
-
-	switch (mode) {
-	case SBE_2T3E3_SCRAMBLER_OFF:
-		cpld_clear_bit(sc, SBE_2T3E3_CPLD_REG_PCRB,
-			       SBE_2T3E3_CPLD_VAL_SCRAMBLER_ENABLE);
-		break;
-	case SBE_2T3E3_SCRAMBLER_LARSCOM:
-		cpld_clear_bit(sc, SBE_2T3E3_CPLD_REG_PCRB,
-			       SBE_2T3E3_CPLD_VAL_SCRAMBLER_TYPE);
-		cpld_set_bit(sc, SBE_2T3E3_CPLD_REG_PCRB,
-			     SBE_2T3E3_CPLD_VAL_SCRAMBLER_ENABLE);
-		break;
-	case SBE_2T3E3_SCRAMBLER_ADC_KENTROX_DIGITAL:
-		cpld_set_bit(sc, SBE_2T3E3_CPLD_REG_PCRB,
-			     SBE_2T3E3_CPLD_VAL_SCRAMBLER_TYPE);
-		cpld_set_bit(sc, SBE_2T3E3_CPLD_REG_PCRB,
-			     SBE_2T3E3_CPLD_VAL_SCRAMBLER_ENABLE);
-		break;
-	default:
-		return;
-	}
-
-	sc->p.scrambler = mode;
-}
-
-
-void cpld_set_crc(struct channel *sc, u32 crc)
-{
-	if (sc->p.crc == crc)
-		return;
-
-	switch (crc) {
-	case SBE_2T3E3_CRC_16:
-		cpld_clear_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			       SBE_2T3E3_CPLD_VAL_CRC32);
-		break;
-	case SBE_2T3E3_CRC_32:
-		cpld_set_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			     SBE_2T3E3_CPLD_VAL_CRC32);
-		break;
-	default:
-		return;
-	}
-
-	sc->p.crc = crc;
-}
-
-
-void cpld_select_panel(struct channel *sc, u32 panel)
-{
-	if (sc->p.panel == panel)
-		return;
-	switch (panel) {
-	case SBE_2T3E3_PANEL_FRONT:
-		cpld_clear_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			       SBE_2T3E3_CPLD_VAL_REAR_PANEL);
-		break;
-	case SBE_2T3E3_PANEL_REAR:
-		cpld_set_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			     SBE_2T3E3_CPLD_VAL_REAR_PANEL);
-		break;
-	default:
-		return;
-	}
-
-	udelay(100);
-
-	sc->p.panel = panel;
-}
-
-
-void cpld_set_clock(struct channel *sc, u32 mode)
-{
-	if (sc->p.clock_source == mode)
-		return;
-
-	switch (mode) {
-	case SBE_2T3E3_TIMING_LOCAL:
-		cpld_set_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			     SBE_2T3E3_CPLD_VAL_ALT);
-		break;
-	case SBE_2T3E3_TIMING_LOOP:
-		cpld_clear_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			       SBE_2T3E3_CPLD_VAL_ALT);
-		break;
-	default:
-		return;
-	}
-
-	sc->p.clock_source = mode;
-}
-
-void cpld_set_pad_count(struct channel *sc, u32 count)
-{
-	u32 val;
-
-	if (sc->p.pad_count == count)
-		return;
-
-	switch (count) {
-	case SBE_2T3E3_PAD_COUNT_1:
-		val = SBE_2T3E3_CPLD_VAL_PAD_COUNT_1;
-		break;
-	case SBE_2T3E3_PAD_COUNT_2:
-		val = SBE_2T3E3_CPLD_VAL_PAD_COUNT_2;
-		break;
-	case SBE_2T3E3_PAD_COUNT_3:
-		val = SBE_2T3E3_CPLD_VAL_PAD_COUNT_3;
-		break;
-	case SBE_2T3E3_PAD_COUNT_4:
-		val = SBE_2T3E3_CPLD_VAL_PAD_COUNT_4;
-		break;
-	default:
-		return;
-	}
-
-	cpld_clear_bit(sc, SBE_2T3E3_CPLD_REG_PCRB,
-		       SBE_2T3E3_CPLD_VAL_PAD_COUNT);
-	cpld_set_bit(sc, SBE_2T3E3_CPLD_REG_PCRB, val);
-	sc->p.pad_count = count;
-}
-
-void cpld_LOS_update(struct channel *sc)
-{
-	u_int8_t los;
-
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PICSR,
-		   SBE_2T3E3_CPLD_VAL_DMO_SIGNAL_DETECTED |
-		   SBE_2T3E3_CPLD_VAL_RECEIVE_LOSS_OF_LOCK_DETECTED |
-		   SBE_2T3E3_CPLD_VAL_RECEIVE_LOSS_OF_SIGNAL_DETECTED);
-	los = cpld_read(sc, SBE_2T3E3_CPLD_REG_PICSR) &
-		SBE_2T3E3_CPLD_VAL_RECEIVE_LOSS_OF_SIGNAL_DETECTED;
-
-	if (los != sc->s.LOS)
-		dev_info(&sc->pdev->dev, "SBE 2T3E3: LOS status: %s\n",
-			 los ? "Loss of signal" : "Signal OK");
-	sc->s.LOS = los;
-}
-
-void cpld_set_fractional_mode(struct channel *sc, u32 mode,
-			      u32 start, u32 stop)
-{
-	if (mode == SBE_2T3E3_FRACTIONAL_MODE_NONE) {
-		start = 0;
-		stop = 0;
-	}
-
-	if (sc->p.fractional_mode == mode && sc->p.bandwidth_start == start &&
-	    sc->p.bandwidth_stop == stop)
-		return;
-
-	switch (mode) {
-	case SBE_2T3E3_FRACTIONAL_MODE_NONE:
-		cpld_write(sc, SBE_2T3E3_CPLD_REG_PCRC,
-			   SBE_2T3E3_CPLD_VAL_FRACTIONAL_MODE_NONE);
-		break;
-	case SBE_2T3E3_FRACTIONAL_MODE_0:
-		cpld_write(sc, SBE_2T3E3_CPLD_REG_PCRC,
-			   SBE_2T3E3_CPLD_VAL_FRACTIONAL_MODE_0);
-		break;
-	case SBE_2T3E3_FRACTIONAL_MODE_1:
-		cpld_write(sc, SBE_2T3E3_CPLD_REG_PCRC,
-			   SBE_2T3E3_CPLD_VAL_FRACTIONAL_MODE_1);
-		break;
-	case SBE_2T3E3_FRACTIONAL_MODE_2:
-		cpld_write(sc, SBE_2T3E3_CPLD_REG_PCRC,
-			   SBE_2T3E3_CPLD_VAL_FRACTIONAL_MODE_2);
-		break;
-	default:
-		netdev_err(sc->dev, "wrong mode in set_fractional_mode\n");
-		return;
-	}
-
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PBWF, start);
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PBWL, stop);
-
-	sc->p.fractional_mode = mode;
-	sc->p.bandwidth_start = start;
-	sc->p.bandwidth_stop = stop;
-}
diff --git a/drivers/staging/sbe-2t3e3/ctrl.c b/drivers/staging/sbe-2t3e3/ctrl.c
deleted file mode 100644
index e0964ac9e7d..00000000000
--- a/drivers/staging/sbe-2t3e3/ctrl.c
+++ /dev/null
@@ -1,350 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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 code is based on a driver written by SBE Inc.
- */
-
-#include <linux/types.h>
-#include "2t3e3.h"
-#include "ctrl.h"
-
-void t3e3_set_frame_type(struct channel *sc, u32 mode)
-{
-	if (sc->p.frame_type == mode)
-		return;
-
-	if (sc->r.flags & SBE_2T3E3_FLAG_NETWORK_UP) {
-		dev_err(&sc->pdev->dev, "SBE 2T3E3: changing frame type during active connection\n");
-		return;
-	}
-
-	exar7300_set_frame_type(sc, mode);
-	exar7250_set_frame_type(sc, mode);
-	cpld_set_frame_type(sc, mode);
-
-	sc->p.frame_type = mode;
-}
-
-static void t3e3_set_loopback(struct channel *sc, u32 mode)
-{
-	u32 tx, rx;
-
-	if (sc->p.loopback == mode)
-		return;
-
-	tx = sc->p.transmitter_on;
-	rx = sc->p.receiver_on;
-	if (tx == SBE_2T3E3_ON)
-		dc_transmitter_onoff(sc, SBE_2T3E3_OFF);
-	if (rx == SBE_2T3E3_ON)
-		dc_receiver_onoff(sc, SBE_2T3E3_OFF);
-
-	/* stop current loopback if any exists */
-	switch (sc->p.loopback) {
-	case SBE_2T3E3_LOOPBACK_NONE:
-		break;
-	case SBE_2T3E3_LOOPBACK_ETHERNET:
-		dc_set_loopback(sc, SBE_2T3E3_21143_VAL_LOOPBACK_OFF);
-		break;
-	case SBE_2T3E3_LOOPBACK_FRAMER:
-		exar7250_set_loopback(sc, SBE_2T3E3_FRAMER_VAL_LOOPBACK_OFF);
-		break;
-	case SBE_2T3E3_LOOPBACK_LIU_DIGITAL:
-	case SBE_2T3E3_LOOPBACK_LIU_ANALOG:
-	case SBE_2T3E3_LOOPBACK_LIU_REMOTE:
-		exar7300_set_loopback(sc, SBE_2T3E3_LIU_VAL_LOOPBACK_OFF);
-		break;
-	default:
-		return;
-	}
-
-	switch (mode) {
-	case SBE_2T3E3_LOOPBACK_NONE:
-		break;
-	case SBE_2T3E3_LOOPBACK_ETHERNET:
-		dc_set_loopback(sc, SBE_2T3E3_21143_VAL_LOOPBACK_INTERNAL);
-		break;
-	case SBE_2T3E3_LOOPBACK_FRAMER:
-		exar7250_set_loopback(sc, SBE_2T3E3_FRAMER_VAL_LOOPBACK_ON);
-		break;
-	case SBE_2T3E3_LOOPBACK_LIU_DIGITAL:
-		exar7300_set_loopback(sc, SBE_2T3E3_LIU_VAL_LOOPBACK_DIGITAL);
-		break;
-	case SBE_2T3E3_LOOPBACK_LIU_ANALOG:
-		exar7300_set_loopback(sc, SBE_2T3E3_LIU_VAL_LOOPBACK_ANALOG);
-		break;
-	case SBE_2T3E3_LOOPBACK_LIU_REMOTE:
-		exar7300_set_loopback(sc, SBE_2T3E3_LIU_VAL_LOOPBACK_REMOTE);
-		break;
-	default:
-		return;
-	}
-
-	sc->p.loopback = mode;
-
-	if (tx == SBE_2T3E3_ON)
-		dc_transmitter_onoff(sc, SBE_2T3E3_ON);
-	if (rx == SBE_2T3E3_ON)
-		dc_receiver_onoff(sc, SBE_2T3E3_ON);
-}
-
-
-static void t3e3_reg_read(struct channel *sc, u32 *reg, u32 *val)
-{
-	u32 i;
-
-	*val = 0;
-
-	switch (reg[0]) {
-	case SBE_2T3E3_CHIP_21143:
-		if (!(reg[1] & 7))
-			*val = dc_read(sc->addr, reg[1] / 8);
-		break;
-	case SBE_2T3E3_CHIP_CPLD:
-		for (i = 0; i < SBE_2T3E3_CPLD_REG_MAX; i++)
-			if (cpld_reg_map[i][sc->h.slot] == reg[1]) {
-				*val = cpld_read(sc, i);
-				break;
-			}
-		break;
-	case SBE_2T3E3_CHIP_FRAMER:
-		for (i = 0; i < SBE_2T3E3_FRAMER_REG_MAX; i++)
-			if (t3e3_framer_reg_map[i] == reg[1]) {
-				*val = exar7250_read(sc, i);
-				break;
-			}
-		break;
-	case SBE_2T3E3_CHIP_LIU:
-		for (i = 0; i < SBE_2T3E3_LIU_REG_MAX; i++)
-			if (t3e3_liu_reg_map[i] == reg[1]) {
-				*val = exar7300_read(sc, i);
-				break;
-			}
-		break;
-	default:
-		break;
-	}
-}
-
-static void t3e3_reg_write(struct channel *sc, u32 *reg)
-{
-	u32 i;
-
-	switch (reg[0]) {
-	case SBE_2T3E3_CHIP_21143:
-		dc_write(sc->addr, reg[1], reg[2]);
-		break;
-	case SBE_2T3E3_CHIP_CPLD:
-		for (i = 0; i < SBE_2T3E3_CPLD_REG_MAX; i++)
-			if (cpld_reg_map[i][sc->h.slot] == reg[1]) {
-				cpld_write(sc, i, reg[2]);
-				break;
-			}
-		break;
-	case SBE_2T3E3_CHIP_FRAMER:
-		for (i = 0; i < SBE_2T3E3_FRAMER_REG_MAX; i++)
-			if (t3e3_framer_reg_map[i] == reg[1]) {
-				exar7250_write(sc, i, reg[2]);
-				break;
-			}
-		break;
-	case SBE_2T3E3_CHIP_LIU:
-		for (i = 0; i < SBE_2T3E3_LIU_REG_MAX; i++)
-			if (t3e3_liu_reg_map[i] == reg[1]) {
-				exar7300_write(sc, i, reg[2]);
-				break;
-			}
-		break;
-	}
-}
-
-static void t3e3_port_get(struct channel *sc, struct t3e3_param *param)
-{
-	memcpy(param, &(sc->p), sizeof(struct t3e3_param));
-}
-
-static void t3e3_port_set(struct channel *sc, struct t3e3_param *param)
-{
-	if (param->frame_mode != 0xff)
-		cpld_set_frame_mode(sc, param->frame_mode);
-
-	if (param->fractional_mode != 0xff)
-		cpld_set_fractional_mode(sc, param->fractional_mode,
-					 param->bandwidth_start,
-					 param->bandwidth_stop);
-
-	if (param->pad_count != 0xff)
-		cpld_set_pad_count(sc, param->pad_count);
-
-	if (param->crc != 0xff)
-		cpld_set_crc(sc, param->crc);
-
-	if (param->receiver_on != 0xff)
-		dc_receiver_onoff(sc, param->receiver_on);
-
-	if (param->transmitter_on != 0xff)
-		dc_transmitter_onoff(sc, param->transmitter_on);
-
-	if (param->frame_type != 0xff)
-		t3e3_set_frame_type(sc, param->frame_type);
-
-	if (param->panel != 0xff)
-		cpld_select_panel(sc, param->panel);
-
-	if (param->line_build_out != 0xff)
-		exar7300_line_build_out_onoff(sc, param->line_build_out);
-
-	if (param->receive_equalization != 0xff)
-		exar7300_receive_equalization_onoff(sc, param->receive_equalization);
-
-	if (param->transmit_all_ones != 0xff)
-		exar7300_transmit_all_ones_onoff(sc, param->transmit_all_ones);
-
-	if (param->loopback != 0xff)
-		t3e3_set_loopback(sc, param->loopback);
-
-	if (param->clock_source != 0xff)
-		cpld_set_clock(sc, param->clock_source);
-
-	if (param->scrambler != 0xff)
-		cpld_set_scrambler(sc, param->scrambler);
-}
-
-static void t3e3_port_get_stats(struct channel *sc, struct t3e3_stats *stats)
-{
-	u32 result;
-
-	sc->s.LOC = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_IO_CONTROL)
-		& SBE_2T3E3_FRAMER_VAL_LOSS_OF_CLOCK_STATUS ? 1 : 0;
-
-	switch (sc->p.frame_type) {
-	case SBE_2T3E3_FRAME_TYPE_E3_G751:
-	case SBE_2T3E3_FRAME_TYPE_E3_G832:
-		result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_CONFIGURATION_STATUS_2);
-		sc->s.LOF = result & SBE_2T3E3_FRAMER_VAL_E3_RX_LOF ? 1 : 0;
-		sc->s.OOF = result & SBE_2T3E3_FRAMER_VAL_E3_RX_OOF ? 1 : 0;
-
-		cpld_LOS_update(sc);
-
-		sc->s.AIS = result & SBE_2T3E3_FRAMER_VAL_E3_RX_AIS ? 1 : 0;
-		sc->s.FERF = result & SBE_2T3E3_FRAMER_VAL_E3_RX_FERF ? 1 : 0;
-		break;
-
-	case SBE_2T3E3_FRAME_TYPE_T3_CBIT:
-	case SBE_2T3E3_FRAME_TYPE_T3_M13:
-		result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_CONFIGURATION_STATUS);
-		sc->s.AIS = result & SBE_2T3E3_FRAMER_VAL_T3_RX_AIS ? 1 : 0;
-
-		cpld_LOS_update(sc);
-
-		sc->s.IDLE = result & SBE_2T3E3_FRAMER_VAL_T3_RX_IDLE ? 1 : 0;
-		sc->s.OOF = result & SBE_2T3E3_FRAMER_VAL_T3_RX_OOF ? 1 : 0;
-
-		result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_STATUS);
-		sc->s.FERF = result & SBE_2T3E3_FRAMER_VAL_T3_RX_FERF ? 1 : 0;
-		sc->s.AIC = result & SBE_2T3E3_FRAMER_VAL_T3_RX_AIC ? 1 : 0;
-		sc->s.FEBE_code = result & SBE_2T3E3_FRAMER_VAL_T3_RX_FEBE;
-
-		sc->s.FEAC = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_FEAC);
-		break;
-
-	default:
-		break;
-	}
-
-	result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_PMON_LCV_EVENT_COUNT_MSB) << 8;
-	result += exar7250_read(sc, SBE_2T3E3_FRAMER_REG_PMON_HOLDING_REGISTER);
-	sc->s.LCV += result;
-
-	result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_PMON_FRAMING_BIT_ERROR_EVENT_COUNT_MSB) << 8;
-	result += exar7250_read(sc, SBE_2T3E3_FRAMER_REG_PMON_HOLDING_REGISTER);
-	sc->s.FRAMING_BIT += result;
-
-	result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_PMON_PARITY_ERROR_EVENT_COUNT_MSB) << 8;
-	result += exar7250_read(sc, SBE_2T3E3_FRAMER_REG_PMON_HOLDING_REGISTER);
-	sc->s.PARITY_ERROR += result;
-
-	result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_PMON_FEBE_EVENT_COUNT_MSB) << 8;
-	result += exar7250_read(sc, SBE_2T3E3_FRAMER_REG_PMON_HOLDING_REGISTER);
-	sc->s.FEBE_count += result;
-
-	result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_PMON_CP_BIT_ERROR_EVENT_COUNT_MSB) << 8;
-	result += exar7250_read(sc, SBE_2T3E3_FRAMER_REG_PMON_HOLDING_REGISTER);
-	sc->s.CP_BIT += result;
-
-	memcpy(stats, &(sc->s), sizeof(struct t3e3_stats));
-}
-
-static void t3e3_port_del_stats(struct channel *sc)
-{
-	memset(&(sc->s), 0, sizeof(struct t3e3_stats));
-}
-
-void t3e3_if_config(struct channel *sc, u32 cmd, char *set,
-		    struct t3e3_resp *ret, int *rlen)
-{
-	struct t3e3_param *param = (struct t3e3_param *)set;
-	u32 *data = (u32 *)set;
-
-	/* turn off all interrupt */
-	/* cpld_stop_intr(sc); */
-
-	switch (cmd) {
-	case SBE_2T3E3_PORT_GET:
-		t3e3_port_get(sc, &(ret->u.param));
-		*rlen = sizeof(ret->u.param);
-		break;
-	case SBE_2T3E3_PORT_SET:
-		t3e3_port_set(sc, param);
-		*rlen = 0;
-		break;
-	case SBE_2T3E3_PORT_GET_STATS:
-		t3e3_port_get_stats(sc, &(ret->u.stats));
-		*rlen = sizeof(ret->u.stats);
-		break;
-	case SBE_2T3E3_PORT_DEL_STATS:
-		t3e3_port_del_stats(sc);
-		*rlen = 0;
-		break;
-	case SBE_2T3E3_PORT_READ_REGS:
-		t3e3_reg_read(sc, data, &(ret->u.data));
-		*rlen = sizeof(ret->u.data);
-		break;
-	case SBE_2T3E3_PORT_WRITE_REGS:
-		t3e3_reg_write(sc, data);
-		*rlen = 0;
-		break;
-	case SBE_2T3E3_LOG_LEVEL:
-		*rlen = 0;
-		break;
-	default:
-		*rlen = 0;
-		break;
-	}
-}
-
-void t3e3_sc_init(struct channel *sc)
-{
-	memset(sc, 0, sizeof(*sc));
-
-	sc->p.frame_mode = SBE_2T3E3_FRAME_MODE_HDLC;
-	sc->p.fractional_mode = SBE_2T3E3_FRACTIONAL_MODE_NONE;
-	sc->p.crc = SBE_2T3E3_CRC_32;
-	sc->p.receiver_on = SBE_2T3E3_OFF;
-	sc->p.transmitter_on = SBE_2T3E3_OFF;
-	sc->p.frame_type = SBE_2T3E3_FRAME_TYPE_T3_CBIT;
-	sc->p.panel = SBE_2T3E3_PANEL_FRONT;
-	sc->p.line_build_out = SBE_2T3E3_OFF;
-	sc->p.receive_equalization = SBE_2T3E3_OFF;
-	sc->p.transmit_all_ones = SBE_2T3E3_OFF;
-	sc->p.loopback = SBE_2T3E3_LOOPBACK_NONE;
-	sc->p.clock_source = SBE_2T3E3_TIMING_LOCAL;
-	sc->p.scrambler = SBE_2T3E3_SCRAMBLER_OFF;
-	sc->p.pad_count = SBE_2T3E3_PAD_COUNT_1;
-}
diff --git a/drivers/staging/sbe-2t3e3/ctrl.h b/drivers/staging/sbe-2t3e3/ctrl.h
deleted file mode 100644
index 41f144d75c3..00000000000
--- a/drivers/staging/sbe-2t3e3/ctrl.h
+++ /dev/null
@@ -1,131 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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 code is based on a driver written by SBE Inc.
- */
-
-#ifndef CTRL_H
-#define CTRL_H
-
-#define SBE_2T3E3_OFF					0
-#define SBE_2T3E3_ON					1
-
-#define SBE_2T3E3_LED_NONE				0
-#define SBE_2T3E3_LED_GREEN				1
-#define SBE_2T3E3_LED_YELLOW				2
-
-#define SBE_2T3E3_CABLE_LENGTH_LESS_THAN_255_FEET	0
-#define SBE_2T3E3_CABLE_LENGTH_GREATER_THAN_255_FEET	1
-
-#define SBE_2T3E3_CRC_16				0
-#define SBE_2T3E3_CRC_32				1
-
-#define SBE_2T3E3_PANEL_FRONT				0
-#define SBE_2T3E3_PANEL_REAR				1
-
-#define SBE_2T3E3_FRAME_MODE_HDLC			0
-#define SBE_2T3E3_FRAME_MODE_TRANSPARENT		1
-#define SBE_2T3E3_FRAME_MODE_RAW			2
-
-#define SBE_2T3E3_FRAME_TYPE_E3_G751			0
-#define SBE_2T3E3_FRAME_TYPE_E3_G832			1
-#define SBE_2T3E3_FRAME_TYPE_T3_CBIT			2
-#define SBE_2T3E3_FRAME_TYPE_T3_M13			3
-
-#define SBE_2T3E3_FRACTIONAL_MODE_NONE			0
-#define SBE_2T3E3_FRACTIONAL_MODE_0			1
-#define SBE_2T3E3_FRACTIONAL_MODE_1			2
-#define SBE_2T3E3_FRACTIONAL_MODE_2			3
-
-#define SBE_2T3E3_SCRAMBLER_OFF				0
-#define SBE_2T3E3_SCRAMBLER_LARSCOM			1
-#define SBE_2T3E3_SCRAMBLER_ADC_KENTROX_DIGITAL		2
-
-#define SBE_2T3E3_TIMING_LOCAL				0
-#define SBE_2T3E3_TIMING_LOOP				1
-
-#define SBE_2T3E3_LOOPBACK_NONE				0
-#define SBE_2T3E3_LOOPBACK_ETHERNET			1
-#define SBE_2T3E3_LOOPBACK_FRAMER			2
-#define SBE_2T3E3_LOOPBACK_LIU_DIGITAL			3
-#define SBE_2T3E3_LOOPBACK_LIU_ANALOG			4
-#define SBE_2T3E3_LOOPBACK_LIU_REMOTE			5
-
-#define SBE_2T3E3_PAD_COUNT_1				1
-#define SBE_2T3E3_PAD_COUNT_2				2
-#define SBE_2T3E3_PAD_COUNT_3				3
-#define SBE_2T3E3_PAD_COUNT_4				4
-
-#define SBE_2T3E3_CHIP_21143				0
-#define SBE_2T3E3_CHIP_CPLD				1
-#define SBE_2T3E3_CHIP_FRAMER				2
-#define SBE_2T3E3_CHIP_LIU				3
-
-#define SBE_2T3E3_LOG_LEVEL_NONE			0
-#define SBE_2T3E3_LOG_LEVEL_ERROR			1
-#define SBE_2T3E3_LOG_LEVEL_WARNING			2
-#define SBE_2T3E3_LOG_LEVEL_INFO			3
-
-/* commands */
-#define SBE_2T3E3_PORT_GET				0
-#define SBE_2T3E3_PORT_SET				1
-#define SBE_2T3E3_PORT_GET_STATS			2
-#define SBE_2T3E3_PORT_DEL_STATS			3
-#define SBE_2T3E3_PORT_READ_REGS			4
-#define SBE_2T3E3_LOG_LEVEL				5
-#define SBE_2T3E3_PORT_WRITE_REGS			6
-
-#define NG_SBE_2T3E3_NODE_TYPE  "sbe2T3E3"
-#define NG_SBE_2T3E3_COOKIE     0x03800891
-
-struct t3e3_param {
-	u_int8_t frame_mode;		/* FRAME_MODE_* */
-	u_int8_t crc;			/* CRC_* */
-	u_int8_t receiver_on;		/* ON/OFF */
-	u_int8_t transmitter_on;	/* ON/OFF */
-	u_int8_t frame_type;		/* FRAME_TYPE_* */
-	u_int8_t panel;			/* PANEL_* */
-	u_int8_t line_build_out;	/* ON/OFF */
-	u_int8_t receive_equalization;	/* ON/OFF */
-	u_int8_t transmit_all_ones;	/* ON/OFF */
-	u_int8_t loopback;		/* LOOPBACK_* */
-	u_int8_t clock_source;		/* TIMING_* */
-	u_int8_t scrambler;		/* SCRAMBLER_* */
-	u_int8_t pad_count;		/* PAD_COUNT_* */
-	u_int8_t log_level;		/* LOG_LEVEL_* - unused */
-	u_int8_t fractional_mode;	/* FRACTIONAL_MODE_* */
-	u_int8_t bandwidth_start;	/* 0-255 */
-	u_int8_t bandwidth_stop;	/* 0-255 */
-};
-
-struct t3e3_stats {
-	u_int64_t in_bytes;
-	u32 in_packets, in_dropped;
-	u32 in_errors, in_error_desc, in_error_coll, in_error_drib,
-		in_error_crc, in_error_mii;
-	u_int64_t out_bytes;
-	u32 out_packets, out_dropped;
-	u32 out_errors, out_error_jab, out_error_lost_carr,
-		out_error_no_carr, out_error_link_fail, out_error_underflow,
-		out_error_dereferred;
-	u_int8_t LOC, LOF, OOF, LOS, AIS, FERF, IDLE, AIC, FEAC;
-	u_int16_t FEBE_code;
-	u32 LCV, FRAMING_BIT, PARITY_ERROR, FEBE_count, CP_BIT;
-};
-
-
-struct t3e3_resp {
-	union {
-		struct t3e3_param param;
-		struct t3e3_stats stats;
-		u32 data;
-	} u;
-};
-
-#endif /* CTRL_H */
diff --git a/drivers/staging/sbe-2t3e3/dc.c b/drivers/staging/sbe-2t3e3/dc.c
deleted file mode 100644
index 02510f67ac4..00000000000
--- a/drivers/staging/sbe-2t3e3/dc.c
+++ /dev/null
@@ -1,460 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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 code is based on a driver written by SBE Inc.
- */
-
-#include <linux/netdevice.h>
-#include <linux/types.h>
-#include <linux/errno.h>
-#include <linux/io.h>
-#include "2t3e3.h"
-#include "ctrl.h"
-
-static int dc_init_descriptor_list(struct channel *sc);
-
-void dc_init(struct channel *sc)
-{
-	u32 val;
-
-	dc_stop(sc);
-	/*dc_reset(sc);*/ /* do not want to reset here */
-
-	/*
-	 * BUS_MODE (CSR0)
-	 */
-	val = SBE_2T3E3_21143_VAL_READ_LINE_ENABLE |
-		SBE_2T3E3_21143_VAL_READ_MULTIPLE_ENABLE |
-		SBE_2T3E3_21143_VAL_TRANSMIT_AUTOMATIC_POLLING_200us |
-		SBE_2T3E3_21143_VAL_BUS_ARBITRATION_RR;
-
-	if (sc->h.command & 16)
-		val |= SBE_2T3E3_21143_VAL_WRITE_AND_INVALIDATE_ENABLE;
-
-	switch (sc->h.cache_size) {
-	case 32:
-		val |= SBE_2T3E3_21143_VAL_CACHE_ALIGNMENT_32;
-		break;
-	case 16:
-		val |= SBE_2T3E3_21143_VAL_CACHE_ALIGNMENT_16;
-		break;
-	case 8:
-		val |= SBE_2T3E3_21143_VAL_CACHE_ALIGNMENT_8;
-		break;
-	default:
-		break;
-	}
-
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_BUS_MODE, val);
-
-	/* OPERATION_MODE (CSR6) */
-	val = SBE_2T3E3_21143_VAL_RECEIVE_ALL |
-		SBE_2T3E3_21143_VAL_MUST_BE_ONE |
-		SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_1 |
-		SBE_2T3E3_21143_VAL_LOOPBACK_OFF |
-		SBE_2T3E3_21143_VAL_PASS_ALL_MULTICAST |
-		SBE_2T3E3_21143_VAL_PROMISCUOUS_MODE |
-		SBE_2T3E3_21143_VAL_PASS_BAD_FRAMES;
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE, val);
-	if (sc->p.loopback == SBE_2T3E3_LOOPBACK_ETHERNET)
-		sc->p.loopback = SBE_2T3E3_LOOPBACK_NONE;
-
-	/*
-	 * GENERAL_PURPOSE_TIMER_AND_INTERRUPT_MITIGATION_CONTROL (CSR11)
-	 */
-	val = SBE_2T3E3_21143_VAL_CYCLE_SIZE |
-		SBE_2T3E3_21143_VAL_TRANSMIT_TIMER |
-		SBE_2T3E3_21143_VAL_NUMBER_OF_TRANSMIT_PACKETS |
-		SBE_2T3E3_21143_VAL_RECEIVE_TIMER |
-		SBE_2T3E3_21143_VAL_NUMBER_OF_RECEIVE_PACKETS;
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_GENERAL_PURPOSE_TIMER_AND_INTERRUPT_MITIGATION_CONTROL, val);
-
-	/* prepare descriptors and data for receive and transmit processes */
-	if (dc_init_descriptor_list(sc) != 0)
-		return;
-
-	/* clear ethernet interrupts status */
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_STATUS, 0xFFFFFFFF);
-
-	/* SIA mode registers */
-	dc_set_output_port(sc);
-}
-
-void dc_start(struct channel *sc)
-{
-	u32 val;
-
-	if (!(sc->r.flags & SBE_2T3E3_FLAG_NETWORK_UP))
-		return;
-
-	dc_init(sc);
-
-	/* get actual LOS and OOF status */
-	switch (sc->p.frame_type) {
-	case SBE_2T3E3_FRAME_TYPE_E3_G751:
-	case SBE_2T3E3_FRAME_TYPE_E3_G832:
-		val = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_CONFIGURATION_STATUS_2);
-		dev_dbg(&sc->pdev->dev, "Start Framer Rx Status = %02X\n", val);
-		sc->s.OOF = val & SBE_2T3E3_FRAMER_VAL_E3_RX_OOF ? 1 : 0;
-		break;
-	case SBE_2T3E3_FRAME_TYPE_T3_CBIT:
-	case SBE_2T3E3_FRAME_TYPE_T3_M13:
-		val = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_CONFIGURATION_STATUS);
-		dev_dbg(&sc->pdev->dev, "Start Framer Rx Status = %02X\n", val);
-		sc->s.OOF = val & SBE_2T3E3_FRAMER_VAL_T3_RX_OOF ? 1 : 0;
-		break;
-	default:
-		break;
-	}
-	cpld_LOS_update(sc);
-
-	/* start receive and transmit processes */
-	dc_transmitter_onoff(sc, SBE_2T3E3_ON);
-	dc_receiver_onoff(sc, SBE_2T3E3_ON);
-
-	/* start interrupts */
-	dc_start_intr(sc);
-}
-
-#define MAX_INT_WAIT_CNT	12000
-void dc_stop(struct channel *sc)
-{
-	int wcnt;
-
-	/* stop receive and transmit processes */
-	dc_receiver_onoff(sc, SBE_2T3E3_OFF);
-	dc_transmitter_onoff(sc, SBE_2T3E3_OFF);
-
-	/* turn off ethernet interrupts */
-	dc_stop_intr(sc);
-
-	/* wait to ensure the interrupts have been completed */
-	for (wcnt = 0; wcnt < MAX_INT_WAIT_CNT; wcnt++) {
-		udelay(5);
-		if (!sc->interrupt_active)
-			break;
-	}
-	if (wcnt >= MAX_INT_WAIT_CNT)
-		dev_warn(&sc->pdev->dev, "SBE 2T3E3: Interrupt active too long\n");
-
-	/* clear all receive/transmit data */
-	dc_drop_descriptor_list(sc);
-}
-
-void dc_start_intr(struct channel *sc)
-{
-	if (sc->p.loopback == SBE_2T3E3_LOOPBACK_NONE && sc->s.OOF)
-		return;
-
-	if (sc->p.receiver_on || sc->p.transmitter_on) {
-		if (!sc->ether.interrupt_enable_mask)
-			dc_write(sc->addr, SBE_2T3E3_21143_REG_STATUS, 0xFFFFFFFF);
-
-		sc->ether.interrupt_enable_mask =
-			SBE_2T3E3_21143_VAL_NORMAL_INTERRUPT_SUMMARY_ENABLE |
-			SBE_2T3E3_21143_VAL_ABNORMAL_INTERRUPT_SUMMARY_ENABLE |
-			SBE_2T3E3_21143_VAL_RECEIVE_STOPPED_ENABLE |
-			SBE_2T3E3_21143_VAL_RECEIVE_BUFFER_UNAVAILABLE_ENABLE |
-			SBE_2T3E3_21143_VAL_RECEIVE_INTERRUPT_ENABLE |
-			SBE_2T3E3_21143_VAL_TRANSMIT_UNDERFLOW_INTERRUPT_ENABLE |
-			SBE_2T3E3_21143_VAL_TRANSMIT_BUFFER_UNAVAILABLE_ENABLE |
-			SBE_2T3E3_21143_VAL_TRANSMIT_STOPPED_ENABLE |
-			SBE_2T3E3_21143_VAL_TRANSMIT_INTERRUPT_ENABLE;
-
-		dc_write(sc->addr, SBE_2T3E3_21143_REG_INTERRUPT_ENABLE,
-			 sc->ether.interrupt_enable_mask);
-	}
-}
-
-void dc_stop_intr(struct channel *sc)
-{
-	sc->ether.interrupt_enable_mask = 0;
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_INTERRUPT_ENABLE, 0);
-}
-
-void dc_reset(struct channel *sc)
-{
-	/* turn off ethernet interrupts */
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_INTERRUPT_ENABLE, 0);
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_STATUS, 0xFFFFFFFF);
-
-	/* software reset */
-	dc_set_bits(sc->addr, SBE_2T3E3_21143_REG_BUS_MODE,
-		   SBE_2T3E3_21143_VAL_SOFTWARE_RESET);
-	udelay(4); /* 50 PCI cycles < 2us */
-
-	/* clear hardware configuration */
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_BUS_MODE, 0);
-
-	/* clear software configuration */
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE, 0);
-
-	/* turn off SIA reset */
-	dc_set_bits(sc->addr, SBE_2T3E3_21143_REG_SIA_CONNECTIVITY,
-		   SBE_2T3E3_21143_VAL_SIA_RESET);
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_SIA_TRANSMIT_AND_RECEIVE, 0);
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_SIA_AND_GENERAL_PURPOSE_PORT, 0);
-}
-
-
-void dc_receiver_onoff(struct channel *sc, u32 mode)
-{
-	u32 i, state = 0;
-
-	if (sc->p.receiver_on == mode)
-		return;
-
-	switch (mode) {
-	case SBE_2T3E3_OFF:
-		if (dc_read(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE) &
-		    SBE_2T3E3_21143_VAL_RECEIVE_START) {
-			dc_clear_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-				      SBE_2T3E3_21143_VAL_RECEIVE_START);
-
-			for (i = 0; i < 16; i++) {
-				state = dc_read(sc->addr, SBE_2T3E3_21143_REG_STATUS) &
-					SBE_2T3E3_21143_VAL_RECEIVE_PROCESS_STATE;
-				if (state == SBE_2T3E3_21143_VAL_RX_STOPPED)
-					break;
-				udelay(5);
-			}
-			if (state != SBE_2T3E3_21143_VAL_RX_STOPPED)
-				dev_warn(&sc->pdev->dev, "SBE 2T3E3: Rx failed to stop\n");
-			else
-				dev_info(&sc->pdev->dev, "SBE 2T3E3: Rx off\n");
-		}
-		break;
-	case SBE_2T3E3_ON:
-		dc_set_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-			   SBE_2T3E3_21143_VAL_RECEIVE_START);
-		udelay(100);
-		dc_write(sc->addr, SBE_2T3E3_21143_REG_RECEIVE_POLL_DEMAND, 0xFFFFFFFF);
-		break;
-	default:
-		return;
-	}
-
-	sc->p.receiver_on = mode;
-}
-
-void dc_transmitter_onoff(struct channel *sc, u32 mode)
-{
-	u32 i, state = 0;
-
-	if (sc->p.transmitter_on == mode)
-		return;
-
-	switch (mode) {
-	case SBE_2T3E3_OFF:
-		if (dc_read(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE) &
-		    SBE_2T3E3_21143_VAL_TRANSMISSION_START) {
-			dc_clear_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-				      SBE_2T3E3_21143_VAL_TRANSMISSION_START);
-
-			for (i = 0; i < 16; i++) {
-				state = dc_read(sc->addr, SBE_2T3E3_21143_REG_STATUS) &
-					SBE_2T3E3_21143_VAL_TRANSMISSION_PROCESS_STATE;
-				if (state == SBE_2T3E3_21143_VAL_TX_STOPPED)
-					break;
-				udelay(5);
-			}
-			if (state != SBE_2T3E3_21143_VAL_TX_STOPPED)
-				dev_warn(&sc->pdev->dev, "SBE 2T3E3: Tx failed to stop\n");
-		}
-		break;
-	case SBE_2T3E3_ON:
-		dc_set_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-			   SBE_2T3E3_21143_VAL_TRANSMISSION_START);
-		udelay(100);
-		dc_write(sc->addr, SBE_2T3E3_21143_REG_TRANSMIT_POLL_DEMAND, 0xFFFFFFFF);
-		break;
-	default:
-		return;
-	}
-
-	sc->p.transmitter_on = mode;
-}
-
-
-
-void dc_set_loopback(struct channel *sc, u32 mode)
-{
-	u32 val;
-
-	switch (mode) {
-	case SBE_2T3E3_21143_VAL_LOOPBACK_OFF:
-	case SBE_2T3E3_21143_VAL_LOOPBACK_INTERNAL:
-		break;
-	default:
-		return;
-	}
-
-	/* select loopback mode */
-	val = dc_read(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE) &
-		~SBE_2T3E3_21143_VAL_OPERATING_MODE;
-	val |= mode;
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE, val);
-
-	if (mode == SBE_2T3E3_21143_VAL_LOOPBACK_OFF)
-		dc_set_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-			   SBE_2T3E3_21143_VAL_FULL_DUPLEX_MODE);
-	else
-		dc_clear_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-			      SBE_2T3E3_21143_VAL_FULL_DUPLEX_MODE);
-}
-
-static int dc_init_descriptor_list(struct channel *sc)
-{
-	u32 i, j;
-	struct sk_buff *m;
-
-	if (sc->ether.rx_ring == NULL)
-		sc->ether.rx_ring = kcalloc(SBE_2T3E3_RX_DESC_RING_SIZE,
-					    sizeof(struct t3e3_rx_desc), GFP_KERNEL);
-	if (sc->ether.rx_ring == NULL)
-		return -ENOMEM;
-
-	if (sc->ether.tx_ring == NULL)
-		sc->ether.tx_ring = kcalloc(SBE_2T3E3_TX_DESC_RING_SIZE,
-					    sizeof(struct t3e3_tx_desc), GFP_KERNEL);
-	if (sc->ether.tx_ring == NULL) {
-		kfree(sc->ether.rx_ring);
-		sc->ether.rx_ring = NULL;
-		return -ENOMEM;
-	}
-
-
-	/*
-	 * Receive ring
-	 */
-	for (i = 0; i < SBE_2T3E3_RX_DESC_RING_SIZE; i++) {
-		sc->ether.rx_ring[i].rdes0 = SBE_2T3E3_RX_DESC_21143_OWN;
-		sc->ether.rx_ring[i].rdes1 =
-			SBE_2T3E3_RX_DESC_SECOND_ADDRESS_CHAINED | SBE_2T3E3_MTU;
-
-		if (sc->ether.rx_data[i] == NULL) {
-			m = dev_alloc_skb(MCLBYTES);
-			if (!m) {
-				for (j = 0; j < i; j++) {
-					dev_kfree_skb_any(sc->ether.rx_data[j]);
-					sc->ether.rx_data[j] = NULL;
-				}
-				kfree(sc->ether.rx_ring);
-				sc->ether.rx_ring = NULL;
-				kfree(sc->ether.tx_ring);
-				sc->ether.tx_ring = NULL;
-				dev_err(&sc->pdev->dev, "SBE 2T3E3: token_alloc err:"
-					" no buffer space for RX ring\n");
-				return -ENOBUFS;
-			}
-			sc->ether.rx_data[i] = m;
-		}
-		sc->ether.rx_ring[i].rdes2 = virt_to_phys(sc->ether.rx_data[i]->data);
-
-		sc->ether.rx_ring[i].rdes3 = virt_to_phys(
-			&sc->ether.rx_ring[(i + 1) % SBE_2T3E3_RX_DESC_RING_SIZE]);
-	}
-	sc->ether.rx_ring[SBE_2T3E3_RX_DESC_RING_SIZE - 1].rdes1 |=
-		SBE_2T3E3_RX_DESC_END_OF_RING;
-	sc->ether.rx_ring_current_read = 0;
-
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_RECEIVE_LIST_BASE_ADDRESS,
-		 virt_to_phys(&sc->ether.rx_ring[0]));
-
-	/*
-	 * Transmit ring
-	 */
-	for (i = 0; i < SBE_2T3E3_TX_DESC_RING_SIZE; i++) {
-		sc->ether.tx_ring[i].tdes0 = 0;
-		sc->ether.tx_ring[i].tdes1 = SBE_2T3E3_TX_DESC_SECOND_ADDRESS_CHAINED |
-			SBE_2T3E3_TX_DESC_DISABLE_PADDING;
-
-		sc->ether.tx_ring[i].tdes2 = 0;
-		sc->ether.tx_data[i] = NULL;
-
-		sc->ether.tx_ring[i].tdes3 = virt_to_phys(
-			&sc->ether.tx_ring[(i + 1) % SBE_2T3E3_TX_DESC_RING_SIZE]);
-	}
-	sc->ether.tx_ring[SBE_2T3E3_TX_DESC_RING_SIZE - 1].tdes1 |=
-		SBE_2T3E3_TX_DESC_END_OF_RING;
-
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_TRANSMIT_LIST_BASE_ADDRESS,
-		 virt_to_phys(&sc->ether.tx_ring[0]));
-	sc->ether.tx_ring_current_read = 0;
-	sc->ether.tx_ring_current_write = 0;
-	sc->ether.tx_free_cnt = SBE_2T3E3_TX_DESC_RING_SIZE;
-	spin_lock_init(&sc->ether.tx_lock);
-
-	return 0;
-}
-
-void dc_clear_descriptor_list(struct channel *sc)
-{
-	u32 i;
-
-	/* clear CSR3 and CSR4 */
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_RECEIVE_LIST_BASE_ADDRESS, 0);
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_TRANSMIT_LIST_BASE_ADDRESS, 0);
-
-	/* free all data buffers on TX ring */
-	for (i = 0; i < SBE_2T3E3_TX_DESC_RING_SIZE; i++) {
-		if (sc->ether.tx_data[i] != NULL) {
-			dev_kfree_skb_any(sc->ether.tx_data[i]);
-			sc->ether.tx_data[i] = NULL;
-		}
-	}
-}
-
-void dc_drop_descriptor_list(struct channel *sc)
-{
-	u32 i;
-
-	dc_clear_descriptor_list(sc);
-
-	/* free all data buffers on RX ring */
-	for (i = 0; i < SBE_2T3E3_RX_DESC_RING_SIZE; i++) {
-		if (sc->ether.rx_data[i] != NULL) {
-			dev_kfree_skb_any(sc->ether.rx_data[i]);
-			sc->ether.rx_data[i] = NULL;
-		}
-	}
-
-	kfree(sc->ether.rx_ring);
-	sc->ether.rx_ring = NULL;
-	kfree(sc->ether.tx_ring);
-	sc->ether.tx_ring = NULL;
-}
-
-
-void dc_set_output_port(struct channel *sc)
-{
-	dc_clear_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-		      SBE_2T3E3_21143_VAL_PORT_SELECT);
-
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_SIA_STATUS, 0x00000301);
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_SIA_CONNECTIVITY, 0);
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_SIA_TRANSMIT_AND_RECEIVE, 0);
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_SIA_AND_GENERAL_PURPOSE_PORT, 0x08000011);
-
-	dc_set_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-		   SBE_2T3E3_21143_VAL_TRANSMIT_THRESHOLD_MODE_100Mbs |
-		   SBE_2T3E3_21143_VAL_HEARTBEAT_DISABLE |
-		   SBE_2T3E3_21143_VAL_PORT_SELECT |
-		   SBE_2T3E3_21143_VAL_FULL_DUPLEX_MODE);
-}
-
-void dc_restart(struct channel *sc)
-{
-	dev_warn(&sc->pdev->dev, "SBE 2T3E3: 21143 restart\n");
-
-	dc_stop(sc);
-	dc_reset(sc);
-	dc_init(sc);	/* stop + reset + init */
-	dc_start(sc);
-}
diff --git a/drivers/staging/sbe-2t3e3/exar7250.c b/drivers/staging/sbe-2t3e3/exar7250.c
deleted file mode 100644
index e3ddd140207..00000000000
--- a/drivers/staging/sbe-2t3e3/exar7250.c
+++ /dev/null
@@ -1,185 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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 code is based on a driver written by SBE Inc.
- */
-
-#include "2t3e3.h"
-#include "ctrl.h"
-
-void exar7250_init(struct channel *sc)
-{
-	exar7250_write(sc, SBE_2T3E3_FRAMER_REG_OPERATING_MODE,
-		       SBE_2T3E3_FRAMER_VAL_T3_CBIT |
-		       SBE_2T3E3_FRAMER_VAL_INTERRUPT_ENABLE_RESET |
-		       SBE_2T3E3_FRAMER_VAL_TIMING_ASYNCH_TXINCLK);
-
-	exar7250_write(sc, SBE_2T3E3_FRAMER_REG_IO_CONTROL,
-		       SBE_2T3E3_FRAMER_VAL_DISABLE_TX_LOSS_OF_CLOCK |
-		       SBE_2T3E3_FRAMER_VAL_DISABLE_RX_LOSS_OF_CLOCK |
-		       SBE_2T3E3_FRAMER_VAL_AMI_LINE_CODE |
-		       SBE_2T3E3_FRAMER_VAL_RX_LINE_CLOCK_INVERT);
-
-	exar7250_set_frame_type(sc, SBE_2T3E3_FRAME_TYPE_T3_CBIT);
-}
-
-void exar7250_set_frame_type(struct channel *sc, u32 type)
-{
-	u32 val;
-
-	switch (type) {
-	case SBE_2T3E3_FRAME_TYPE_E3_G751:
-	case SBE_2T3E3_FRAME_TYPE_E3_G832:
-	case SBE_2T3E3_FRAME_TYPE_T3_CBIT:
-	case SBE_2T3E3_FRAME_TYPE_T3_M13:
-		break;
-	default:
-		return;
-	}
-
-	exar7250_stop_intr(sc, type);
-
-	val = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_OPERATING_MODE);
-	val &= ~(SBE_2T3E3_FRAMER_VAL_LOCAL_LOOPBACK_MODE |
-		 SBE_2T3E3_FRAMER_VAL_T3_E3_SELECT |
-		 SBE_2T3E3_FRAMER_VAL_FRAME_FORMAT_SELECT);
-	switch (type) {
-	case SBE_2T3E3_FRAME_TYPE_E3_G751:
-		val |= SBE_2T3E3_FRAMER_VAL_E3_G751;
-		break;
-	case SBE_2T3E3_FRAME_TYPE_E3_G832:
-		val |= SBE_2T3E3_FRAMER_VAL_E3_G832;
-		break;
-	case SBE_2T3E3_FRAME_TYPE_T3_CBIT:
-		val |= SBE_2T3E3_FRAMER_VAL_T3_CBIT;
-		break;
-	case SBE_2T3E3_FRAME_TYPE_T3_M13:
-		val |= SBE_2T3E3_FRAMER_VAL_T3_M13;
-		break;
-	default:
-		return;
-	}
-	exar7250_write(sc, SBE_2T3E3_FRAMER_REG_OPERATING_MODE, val);
-	exar7250_start_intr(sc, type);
-}
-
-
-void exar7250_start_intr(struct channel *sc, u32 type)
-{
-	u32 val;
-
-	switch (type) {
-	case SBE_2T3E3_FRAME_TYPE_E3_G751:
-	case SBE_2T3E3_FRAME_TYPE_E3_G832:
-		val = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_CONFIGURATION_STATUS_2);
-
-		cpld_LOS_update(sc);
-
-		sc->s.OOF = val & SBE_2T3E3_FRAMER_VAL_E3_RX_OOF ? 1 : 0;
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_STATUS_1);
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_ENABLE_1,
-			       SBE_2T3E3_FRAMER_VAL_E3_RX_OOF_INTERRUPT_ENABLE |
-			       SBE_2T3E3_FRAMER_VAL_E3_RX_LOS_INTERRUPT_ENABLE);
-
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_STATUS_2);
-		break;
-
-	case SBE_2T3E3_FRAME_TYPE_T3_CBIT:
-	case SBE_2T3E3_FRAME_TYPE_T3_M13:
-		val = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_CONFIGURATION_STATUS);
-
-		cpld_LOS_update(sc);
-
-		sc->s.OOF = val & SBE_2T3E3_FRAMER_VAL_T3_RX_OOF ? 1 : 0;
-
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_INTERRUPT_STATUS);
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_T3_RX_INTERRUPT_ENABLE,
-			       SBE_2T3E3_FRAMER_VAL_T3_RX_LOS_INTERRUPT_ENABLE |
-			       SBE_2T3E3_FRAMER_VAL_T3_RX_OOF_INTERRUPT_ENABLE);
-
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_FEAC_INTERRUPT_ENABLE_STATUS);
-
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_T3_RX_LAPD_CONTROL, 0);
-		break;
-
-	default:
-		return;
-	}
-
-	exar7250_read(sc, SBE_2T3E3_FRAMER_REG_BLOCK_INTERRUPT_STATUS);
-	exar7250_write(sc, SBE_2T3E3_FRAMER_REG_BLOCK_INTERRUPT_ENABLE,
-		       SBE_2T3E3_FRAMER_VAL_RX_INTERRUPT_ENABLE |
-		       SBE_2T3E3_FRAMER_VAL_TX_INTERRUPT_ENABLE);
-}
-
-
-void exar7250_stop_intr(struct channel *sc, u32 type)
-{
-	exar7250_write(sc, SBE_2T3E3_FRAMER_REG_BLOCK_INTERRUPT_ENABLE, 0);
-	exar7250_read(sc, SBE_2T3E3_FRAMER_REG_BLOCK_INTERRUPT_STATUS);
-
-	switch (type) {
-	case SBE_2T3E3_FRAME_TYPE_E3_G751:
-	case SBE_2T3E3_FRAME_TYPE_E3_G832:
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_ENABLE_1, 0);
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_STATUS_1);
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_ENABLE_2, 0);
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_STATUS_2);
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_E3_RX_LAPD_CONTROL, 0);
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_LAPD_CONTROL);
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_E3_TX_LAPD_STATUS, 0);
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_TX_LAPD_STATUS);
-		break;
-
-	case SBE_2T3E3_FRAME_TYPE_T3_CBIT:
-	case SBE_2T3E3_FRAME_TYPE_T3_M13:
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_T3_RX_INTERRUPT_ENABLE, 0);
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_INTERRUPT_STATUS);
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_T3_RX_FEAC_INTERRUPT_ENABLE_STATUS, 0);
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_FEAC_INTERRUPT_ENABLE_STATUS);
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_T3_RX_LAPD_CONTROL, 0);
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_LAPD_CONTROL);
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_T3_TX_FEAC_CONFIGURATION_STATUS, 0);
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_TX_FEAC_CONFIGURATION_STATUS);
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_T3_TX_LAPD_STATUS, 0);
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_TX_LAPD_STATUS);
-		break;
-	}
-}
-
-
-
-
-void exar7250_unipolar_onoff(struct channel *sc, u32 mode)
-{
-	switch (mode) {
-	case SBE_2T3E3_OFF:
-		exar7300_clear_bit(sc, SBE_2T3E3_FRAMER_REG_IO_CONTROL,
-				   SBE_2T3E3_FRAMER_VAL_UNIPOLAR);
-		break;
-	case SBE_2T3E3_ON:
-		exar7300_set_bit(sc, SBE_2T3E3_FRAMER_REG_IO_CONTROL,
-				 SBE_2T3E3_FRAMER_VAL_UNIPOLAR);
-		break;
-	}
-}
-
-void exar7250_set_loopback(struct channel *sc, u32 mode)
-{
-	switch (mode) {
-	case SBE_2T3E3_FRAMER_VAL_LOOPBACK_OFF:
-		exar7300_clear_bit(sc, SBE_2T3E3_FRAMER_REG_OPERATING_MODE,
-				   SBE_2T3E3_FRAMER_VAL_LOCAL_LOOPBACK_MODE);
-		break;
-	case SBE_2T3E3_FRAMER_VAL_LOOPBACK_ON:
-		exar7300_set_bit(sc, SBE_2T3E3_FRAMER_REG_OPERATING_MODE,
-				 SBE_2T3E3_FRAMER_VAL_LOCAL_LOOPBACK_MODE);
-		break;
-	}
-}
diff --git a/drivers/staging/sbe-2t3e3/exar7300.c b/drivers/staging/sbe-2t3e3/exar7300.c
deleted file mode 100644
index cd229998a62..00000000000
--- a/drivers/staging/sbe-2t3e3/exar7300.c
+++ /dev/null
@@ -1,165 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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 code is based on a driver written by SBE Inc.
- */
-
-#include "2t3e3.h"
-#include "ctrl.h"
-
-void exar7300_init(struct channel *sc)
-{
-	exar7300_write(sc, SBE_2T3E3_LIU_REG_REG1, 0);
-
-	/* enable line decodeer and encoder */
-	exar7300_write(sc, SBE_2T3E3_LIU_REG_REG2, 0);
-	exar7300_write(sc, SBE_2T3E3_LIU_REG_REG3, 0);
-	exar7300_write(sc, SBE_2T3E3_LIU_REG_REG4,
-		       SBE_2T3E3_LIU_VAL_T3_MODE_SELECT |
-		       SBE_2T3E3_LIU_VAL_LOOPBACK_OFF);
-}
-
-void exar7300_set_loopback(struct channel *sc, u32 mode)
-{
-	u32 val;
-
-	switch (mode) {
-	case SBE_2T3E3_LIU_VAL_LOOPBACK_OFF:
-	case SBE_2T3E3_LIU_VAL_LOOPBACK_REMOTE:
-	case SBE_2T3E3_LIU_VAL_LOOPBACK_ANALOG:
-	case SBE_2T3E3_LIU_VAL_LOOPBACK_DIGITAL:
-		break;
-	default:
-		return;
-	}
-
-	val = exar7300_read(sc, SBE_2T3E3_LIU_REG_REG4);
-	val &= ~(SBE_2T3E3_LIU_VAL_LOCAL_LOOPBACK | SBE_2T3E3_LIU_VAL_REMOTE_LOOPBACK);
-	val |= mode;
-	exar7300_write(sc, SBE_2T3E3_LIU_REG_REG4, val);
-}
-
-void exar7300_set_frame_type(struct channel *sc, u32 type)
-{
-	u32 val;
-
-	switch (type) {
-	case SBE_2T3E3_FRAME_TYPE_T3_CBIT:
-	case SBE_2T3E3_FRAME_TYPE_T3_M13:
-	case SBE_2T3E3_FRAME_TYPE_E3_G751:
-	case SBE_2T3E3_FRAME_TYPE_E3_G832:
-		break;
-	default:
-		return;
-	}
-
-	val = exar7300_read(sc, SBE_2T3E3_LIU_REG_REG4);
-	val &= ~(SBE_2T3E3_LIU_VAL_T3_MODE_SELECT |
-		 SBE_2T3E3_LIU_VAL_E3_MODE_SELECT);
-
-	switch (type) {
-	case SBE_2T3E3_FRAME_TYPE_T3_CBIT:
-	case SBE_2T3E3_FRAME_TYPE_T3_M13:
-		val |= SBE_2T3E3_LIU_VAL_T3_MODE_SELECT;
-		break;
-	case SBE_2T3E3_FRAME_TYPE_E3_G751:
-	case SBE_2T3E3_FRAME_TYPE_E3_G832:
-		val |= SBE_2T3E3_LIU_VAL_E3_MODE_SELECT;
-		break;
-	default:
-		return;
-	}
-
-	exar7300_write(sc, SBE_2T3E3_LIU_REG_REG4, val);
-}
-
-
-void exar7300_transmit_all_ones_onoff(struct channel *sc, u32 mode)
-{
-	if (sc->p.transmit_all_ones == mode)
-		return;
-
-	switch (mode) {
-	case SBE_2T3E3_ON:
-		exar7300_set_bit(sc, SBE_2T3E3_LIU_REG_REG1,
-				 SBE_2T3E3_LIU_VAL_TRANSMIT_ALL_ONES);
-		break;
-	case SBE_2T3E3_OFF:
-		exar7300_clear_bit(sc, SBE_2T3E3_LIU_REG_REG1,
-				   SBE_2T3E3_LIU_VAL_TRANSMIT_ALL_ONES);
-		break;
-	default:
-		return;
-	}
-
-	sc->p.transmit_all_ones = mode;
-}
-
-void exar7300_receive_equalization_onoff(struct channel *sc, u32 mode)
-{
-	if (sc->p.receive_equalization == mode)
-		return;
-
-	switch (mode) {
-	case SBE_2T3E3_OFF:
-		exar7300_set_bit(sc, SBE_2T3E3_LIU_REG_REG2,
-				 SBE_2T3E3_LIU_VAL_RECEIVE_EQUALIZATION_DISABLE);
-		break;
-	case SBE_2T3E3_ON:
-		exar7300_clear_bit(sc, SBE_2T3E3_LIU_REG_REG2,
-				   SBE_2T3E3_LIU_VAL_RECEIVE_EQUALIZATION_DISABLE);
-		break;
-	default:
-		return;
-	}
-
-	sc->p.receive_equalization = mode;
-}
-
-void exar7300_line_build_out_onoff(struct channel *sc, u32 mode)
-{
-	if (sc->p.line_build_out == mode)
-		return;
-
-	switch (mode) {
-	case SBE_2T3E3_OFF:
-		exar7300_set_bit(sc, SBE_2T3E3_LIU_REG_REG1,
-				 SBE_2T3E3_LIU_VAL_TRANSMIT_LEVEL_SELECT);
-		exar7300_receive_equalization_onoff(sc, SBE_2T3E3_OFF);
-		break;
-	case SBE_2T3E3_ON:
-		exar7300_clear_bit(sc, SBE_2T3E3_LIU_REG_REG1,
-				   SBE_2T3E3_LIU_VAL_TRANSMIT_LEVEL_SELECT);
-		exar7300_receive_equalization_onoff(sc, SBE_2T3E3_ON);
-		break;
-	default:
-		return;
-	}
-
-	sc->p.line_build_out = mode;
-}
-
-/* TODO - what about encoder in raw mode??? disable it too? */
-void exar7300_unipolar_onoff(struct channel *sc, u32 mode)
-{
-	switch (mode) {
-	case SBE_2T3E3_OFF:
-		exar7300_clear_bit(sc, SBE_2T3E3_LIU_REG_REG3,
-				   SBE_2T3E3_LIU_VAL_DECODER_DISABLE);
-		exar7300_clear_bit(sc, SBE_2T3E3_LIU_REG_REG1,
-				   SBE_2T3E3_LIU_VAL_TRANSMIT_BINARY_DATA);
-		break;
-	case SBE_2T3E3_ON:
-		exar7300_set_bit(sc, SBE_2T3E3_LIU_REG_REG3,
-				 SBE_2T3E3_LIU_VAL_DECODER_DISABLE);
-		exar7300_set_bit(sc, SBE_2T3E3_LIU_REG_REG1,
-				 SBE_2T3E3_LIU_VAL_TRANSMIT_BINARY_DATA);
-		break;
-	}
-}
diff --git a/drivers/staging/sbe-2t3e3/intr.c b/drivers/staging/sbe-2t3e3/intr.c
deleted file mode 100644
index 1bf74b78800..00000000000
--- a/drivers/staging/sbe-2t3e3/intr.c
+++ /dev/null
@@ -1,579 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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 code is based on a driver written by SBE Inc.
- */
-
-#include <linux/hdlc.h>
-#include <linux/interrupt.h>
-#include <linux/netdevice.h>
-#include "2t3e3.h"
-
-irqreturn_t t3e3_intr(int irq, void *dev_instance)
-{
-	struct channel *sc = dev_to_priv(dev_instance);
-	u32 val;
-	irqreturn_t ret = IRQ_NONE;
-
-	sc->interrupt_active = 1;
-
-	val = cpld_read(sc, SBE_2T3E3_CPLD_REG_PICSR);
-
-	if (val & SBE_2T3E3_CPLD_VAL_RECEIVE_LOSS_OF_SIGNAL_CHANGE) {
-		dev_dbg(&sc->pdev->dev,
-			"Rx LOS Chng Int r=%02x (LOS|OOF=%02x)\n",
-			val, (sc->s.LOS << 4) | sc->s.OOF);
-		cpld_LOS_update(sc);
-		ret = IRQ_HANDLED;
-	}
-
-	if (val & SBE_2T3E3_CPLD_VAL_INTERRUPT_FROM_ETHERNET_ASSERTED) {
-		dc_intr(sc);
-		ret = IRQ_HANDLED;
-	}
-
-	if (val & SBE_2T3E3_CPLD_VAL_INTERRUPT_FROM_FRAMER_ASSERTED) {
-		exar7250_intr(sc);
-		ret = IRQ_HANDLED;
-	}
-
-	/*
-	  we don't care about other interrupt sources (DMO, LOS, LCV) because
-	  they are handled by Framer too
-	*/
-
-	sc->interrupt_active = 0;
-	return ret;
-}
-
-void dc_intr(struct channel *sc)
-{
-	u32 val;
-
-	/* disable ethernet interrupts */
-	/* grrr this clears interrupt summary bits !!! */
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_INTERRUPT_ENABLE, 0);
-
-	while ((val = dc_read(sc->addr, SBE_2T3E3_21143_REG_STATUS)) &
-	       (SBE_2T3E3_21143_VAL_RECEIVE_PROCESS_STOPPED |
-		SBE_2T3E3_21143_VAL_RECEIVE_BUFFER_UNAVAILABLE |
-		SBE_2T3E3_21143_VAL_RECEIVE_INTERRUPT |
-		SBE_2T3E3_21143_VAL_TRANSMIT_UNDERFLOW |
-		SBE_2T3E3_21143_VAL_TRANSMIT_BUFFER_UNAVAILABLE |
-		SBE_2T3E3_21143_VAL_TRANSMIT_PROCESS_STOPPED |
-		SBE_2T3E3_21143_VAL_TRANSMIT_INTERRUPT)) {
-		dc_write(sc->addr, SBE_2T3E3_21143_REG_STATUS, val);
-
-		dev_dbg(&sc->pdev->dev, "SBE 2T3E3: Ethernet controller interrupt! (CSR5 = %08X)\n",
-			val);
-
-		if (val & (SBE_2T3E3_21143_VAL_RECEIVE_INTERRUPT |
-			   SBE_2T3E3_21143_VAL_RECEIVE_BUFFER_UNAVAILABLE |
-			   SBE_2T3E3_21143_VAL_RECEIVE_PROCESS_STOPPED)) {
-			if (val & SBE_2T3E3_21143_VAL_RECEIVE_INTERRUPT)
-				dev_dbg(&sc->pdev->dev,
-					"Receive interrupt (LOS=%d, OOF=%d)\n",
-					sc->s.LOS, sc->s.OOF);
-			if (val & SBE_2T3E3_21143_VAL_RECEIVE_BUFFER_UNAVAILABLE)
-				dev_dbg(&sc->pdev->dev,
-					"Receive buffer unavailable\n");
-			if (val & SBE_2T3E3_21143_VAL_RECEIVE_PROCESS_STOPPED)
-				dev_dbg(&sc->pdev->dev,
-					"Receive process stopped\n");
-			dc_intr_rx(sc);
-		}
-
-		if (val & SBE_2T3E3_21143_VAL_TRANSMIT_UNDERFLOW) {
-			dev_dbg(&sc->pdev->dev, "Transmit underflow\n");
-			dc_intr_tx_underflow(sc);
-		}
-
-		if (val & (SBE_2T3E3_21143_VAL_TRANSMIT_BUFFER_UNAVAILABLE |
-			   SBE_2T3E3_21143_VAL_TRANSMIT_INTERRUPT |
-			   SBE_2T3E3_21143_VAL_TRANSMIT_PROCESS_STOPPED)) {
-			if (val & SBE_2T3E3_21143_VAL_TRANSMIT_INTERRUPT)
-				dev_dbg(&sc->pdev->dev, "Transmit interrupt\n");
-			if (val & SBE_2T3E3_21143_VAL_TRANSMIT_BUFFER_UNAVAILABLE)
-				dev_dbg(&sc->pdev->dev,
-					"Transmit buffer unavailable\n");
-			if (val & SBE_2T3E3_21143_VAL_TRANSMIT_PROCESS_STOPPED)
-				dev_dbg(&sc->pdev->dev,
-					"Transmit process stopped\n");
-			dc_intr_tx(sc);
-		}
-	}
-
-	/* enable ethernet interrupts */
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_INTERRUPT_ENABLE,
-		 sc->ether.interrupt_enable_mask);
-}
-
-void dc_intr_rx(struct channel *sc)
-{
-	u32 current_read;
-	u32 error_mask, error;
-	struct t3e3_rx_desc *current_desc;
-	struct sk_buff *m, *m2;
-	unsigned rcv_len;
-
-	sc->rcv_count++; /* for the activity LED */
-
-	current_read = sc->ether.rx_ring_current_read;
-	dev_dbg(&sc->pdev->dev, "intr_rx current_read = %d\n", current_read);
-
-	/* when ethernet loopback is set, ignore framer signals */
-	if ((sc->p.loopback != SBE_2T3E3_LOOPBACK_ETHERNET) && sc->s.OOF) {
-		while (!(sc->ether.rx_ring[current_read].rdes0 &
-			 SBE_2T3E3_RX_DESC_21143_OWN)) {
-			current_desc = &sc->ether.rx_ring[current_read];
-			current_desc->rdes1 &= SBE_2T3E3_RX_DESC_END_OF_RING |
-				SBE_2T3E3_RX_DESC_SECOND_ADDRESS_CHAINED;
-			current_desc->rdes1 |= SBE_2T3E3_MTU;
-			current_desc->rdes0 = SBE_2T3E3_RX_DESC_21143_OWN;
-			current_read = (current_read + 1) % SBE_2T3E3_RX_DESC_RING_SIZE;
-		}
-		sc->ether.rx_ring_current_read = current_read;
-		return;
-	}
-
-	while (!(sc->ether.rx_ring[current_read].rdes0 &
-		 SBE_2T3E3_RX_DESC_21143_OWN)) {
-		current_desc = &sc->ether.rx_ring[current_read];
-
-		dev_dbg(&sc->pdev->dev, "rdes0: %08X        rdes1: %08X\n",
-			current_desc->rdes0, current_desc->rdes1);
-
-		m = sc->ether.rx_data[current_read];
-		rcv_len = (current_desc->rdes0 & SBE_2T3E3_RX_DESC_FRAME_LENGTH) >>
-			SBE_2T3E3_RX_DESC_FRAME_LENGTH_SHIFT;
-
-		dev_dbg(&sc->pdev->dev, "mbuf was received (mbuf len = %d)\n",
-			rcv_len);
-
-		switch (sc->p.crc) {
-		case SBE_2T3E3_CRC_16:
-			rcv_len -= SBE_2T3E3_CRC16_LENGTH;
-			break;
-		case SBE_2T3E3_CRC_32:
-			rcv_len -= SBE_2T3E3_CRC32_LENGTH;
-			break;
-		default:
-			break;
-		}
-
-		if (current_desc->rdes0 & SBE_2T3E3_RX_DESC_LAST_DESC) {
-
-			/* TODO: is collision possible? */
-			error_mask = SBE_2T3E3_RX_DESC_DESC_ERROR |
-				SBE_2T3E3_RX_DESC_COLLISION_SEEN |
-				SBE_2T3E3_RX_DESC_DRIBBLING_BIT;
-
-			switch (sc->p.frame_mode) {
-			case SBE_2T3E3_FRAME_MODE_HDLC:
-				error_mask |= SBE_2T3E3_RX_DESC_MII_ERROR;
-				if (sc->p.crc == SBE_2T3E3_CRC_32)
-					error_mask |= SBE_2T3E3_RX_DESC_CRC_ERROR;
-				break;
-			case SBE_2T3E3_FRAME_MODE_TRANSPARENT:
-			case SBE_2T3E3_FRAME_MODE_RAW:
-				break;
-			default:
-				error_mask = 0;
-			}
-
-			if (sc->s.LOS) {
-				error_mask &= ~(SBE_2T3E3_RX_DESC_DRIBBLING_BIT |
-						SBE_2T3E3_RX_DESC_MII_ERROR);
-			}
-
-			error = current_desc->rdes0 & error_mask;
-			if (error) {
-				sc->s.in_errors++;
-				dev_dbg(&sc->pdev->dev,
-					"error interrupt: NO_ERROR_MESSAGE = %d\n",
-					sc->r.flags & SBE_2T3E3_FLAG_NO_ERROR_MESSAGES ? 1 : 0);
-
-				current_desc->rdes1 &= SBE_2T3E3_RX_DESC_END_OF_RING |
-					SBE_2T3E3_RX_DESC_SECOND_ADDRESS_CHAINED;
-				current_desc->rdes1 |= SBE_2T3E3_MTU;
-				current_desc->rdes0 = SBE_2T3E3_RX_DESC_21143_OWN;
-
-				if (error & SBE_2T3E3_RX_DESC_DESC_ERROR) {
-					if (!(sc->r.flags & SBE_2T3E3_FLAG_NO_ERROR_MESSAGES))
-						dev_err(&sc->pdev->dev,
-							"SBE 2T3E3: descriptor error\n");
-					sc->s.in_error_desc++;
-				}
-
-				if (error & SBE_2T3E3_RX_DESC_COLLISION_SEEN) {
-					if (!(sc->r.flags & SBE_2T3E3_FLAG_NO_ERROR_MESSAGES))
-						dev_err(&sc->pdev->dev,
-							"SBE 2T3E3: collision seen\n");
-					sc->s.in_error_coll++;
-				} else {
-					if (error & SBE_2T3E3_RX_DESC_DRIBBLING_BIT) {
-						if (!(sc->r.flags & SBE_2T3E3_FLAG_NO_ERROR_MESSAGES))
-							dev_err(&sc->pdev->dev,
-								"SBE 2T3E3: dribbling bits error\n");
-						sc->s.in_error_drib++;
-					}
-
-					if (error & SBE_2T3E3_RX_DESC_CRC_ERROR) {
-						if (!(sc->r.flags & SBE_2T3E3_FLAG_NO_ERROR_MESSAGES))
-							dev_err(&sc->pdev->dev,
-								"SBE 2T3E3: crc error\n");
-						sc->s.in_error_crc++;
-					}
-				}
-
-				if (error & SBE_2T3E3_RX_DESC_MII_ERROR) {
-					if (!(sc->r.flags & SBE_2T3E3_FLAG_NO_ERROR_MESSAGES))
-						dev_err(&sc->pdev->dev, "SBE 2T3E3: mii error\n");
-					sc->s.in_error_mii++;
-				}
-
-				current_read = (current_read + 1) % SBE_2T3E3_RX_DESC_RING_SIZE;
-				sc->r.flags |= SBE_2T3E3_FLAG_NO_ERROR_MESSAGES;
-				continue;
-			}
-		}
-
-		current_desc->rdes1 &= SBE_2T3E3_RX_DESC_END_OF_RING |
-			SBE_2T3E3_RX_DESC_SECOND_ADDRESS_CHAINED;
-		current_desc->rdes1 |= SBE_2T3E3_MTU;
-
-		if (rcv_len > 1600) {
-			sc->s.in_errors++;
-			sc->s.in_dropped++;
-			if (!(sc->r.flags & SBE_2T3E3_FLAG_NO_ERROR_MESSAGES))
-				dev_err(&sc->pdev->dev, "SBE 2T3E3: oversized rx: rdes0 = %08X\n",
-					current_desc->rdes0);
-		} else {
-			m2 = dev_alloc_skb(MCLBYTES);
-			if (m2 != NULL) {
-				current_desc->rdes2 = virt_to_phys(m2->data);
-				sc->ether.rx_data[current_read] = m2;
-				sc->s.in_packets++;
-				sc->s.in_bytes += rcv_len;
-				m->dev = sc->dev;
-				skb_put(m, rcv_len);
-				skb_reset_mac_header(m);
-				m->protocol = hdlc_type_trans(m, m->dev);
-				netif_rx(m);
-
-				/* good packet was received so we will show error messages again... */
-				if (sc->r.flags & SBE_2T3E3_FLAG_NO_ERROR_MESSAGES) {
-					dev_dbg(&sc->pdev->dev,
-						"setting ERROR_MESSAGES->0\n");
-					sc->r.flags &= ~SBE_2T3E3_FLAG_NO_ERROR_MESSAGES;
-				}
-
-			} else {
-				sc->s.in_errors++;
-				sc->s.in_dropped++;
-			}
-		}
-		current_desc->rdes0 = SBE_2T3E3_RX_DESC_21143_OWN;
-		current_read = (current_read + 1) % SBE_2T3E3_RX_DESC_RING_SIZE;
-	}
-
-	sc->ether.rx_ring_current_read = current_read;
-
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_RECEIVE_POLL_DEMAND, 0xFFFFFFFF);
-}
-
-void dc_intr_tx(struct channel *sc)
-{
-	u32 current_read, current_write;
-	u32 last_segment, error;
-	struct t3e3_tx_desc *current_desc;
-
-	spin_lock(&sc->ether.tx_lock);
-
-	current_read = sc->ether.tx_ring_current_read;
-	current_write = sc->ether.tx_ring_current_write;
-
-	while (current_read != current_write) {
-		current_desc = &sc->ether.tx_ring[current_read];
-
-		if (current_desc->tdes0 & SBE_2T3E3_RX_DESC_21143_OWN)
-			break;
-
-		dev_dbg(&sc->pdev->dev,
-			"txeof: tdes0 = %08X        tdes1 = %08X\n",
-			current_desc->tdes0, current_desc->tdes1);
-
-		error = current_desc->tdes0 & (SBE_2T3E3_TX_DESC_ERROR_SUMMARY |
-					       SBE_2T3E3_TX_DESC_TRANSMIT_JABBER_TIMEOUT |
-					       SBE_2T3E3_TX_DESC_LOSS_OF_CARRIER |
-					       SBE_2T3E3_TX_DESC_NO_CARRIER |
-					       SBE_2T3E3_TX_DESC_LINK_FAIL_REPORT |
-					       SBE_2T3E3_TX_DESC_UNDERFLOW_ERROR |
-					       SBE_2T3E3_TX_DESC_DEFFERED);
-
-		last_segment = current_desc->tdes1 & SBE_2T3E3_TX_DESC_LAST_SEGMENT;
-
-		current_desc->tdes0 = 0;
-		current_desc->tdes1 &= SBE_2T3E3_TX_DESC_END_OF_RING |
-			SBE_2T3E3_TX_DESC_SECOND_ADDRESS_CHAINED;
-		current_desc->tdes2 = 0;
-		sc->ether.tx_free_cnt++;
-
-		if (last_segment != SBE_2T3E3_TX_DESC_LAST_SEGMENT) {
-			current_read = (current_read + 1) % SBE_2T3E3_TX_DESC_RING_SIZE;
-			continue;
-		}
-
-
-		if (sc->ether.tx_data[current_read]) {
-			sc->s.out_packets++;
-			sc->s.out_bytes += sc->ether.tx_data[current_read]->len;
-			dev_kfree_skb_any(sc->ether.tx_data[current_read]);
-			sc->ether.tx_data[current_read] = NULL;
-		}
-
-		if (error > 0) {
-			sc->s.out_errors++;
-
-			if (error & SBE_2T3E3_TX_DESC_TRANSMIT_JABBER_TIMEOUT) {
-				dev_err(&sc->pdev->dev, "SBE 2T3E3: transmit jabber timeout\n");
-				sc->s.out_error_jab++;
-			}
-
-			if (sc->p.loopback != SBE_2T3E3_LOOPBACK_ETHERNET) {
-				if (error & SBE_2T3E3_TX_DESC_LOSS_OF_CARRIER) {
-					dev_err(&sc->pdev->dev, "SBE 2T3E3: loss of carrier\n");
-					sc->s.out_error_lost_carr++;
-				}
-
-				if (error & SBE_2T3E3_TX_DESC_NO_CARRIER) {
-					dev_err(&sc->pdev->dev, "SBE 2T3E3: no carrier\n");
-					sc->s.out_error_no_carr++;
-				}
-			}
-
-			if (error & SBE_2T3E3_TX_DESC_LINK_FAIL_REPORT) {
-				dev_err(&sc->pdev->dev, "SBE 2T3E3: link fail report\n");
-				sc->s.out_error_link_fail++;
-			}
-
-			if (error & SBE_2T3E3_TX_DESC_UNDERFLOW_ERROR) {
-				dev_err(&sc->pdev->dev, "SBE 2T3E3:"
-					" transmission underflow error\n");
-				sc->s.out_error_underflow++;
-				spin_unlock(&sc->ether.tx_lock);
-
-				dc_restart(sc);
-				return;
-			}
-
-			if (error & SBE_2T3E3_TX_DESC_DEFFERED) {
-				dev_err(&sc->pdev->dev, "SBE 2T3E3: transmission deferred\n");
-				sc->s.out_error_dereferred++;
-			}
-		}
-
-		current_read = (current_read + 1) % SBE_2T3E3_TX_DESC_RING_SIZE;
-	}
-
-	sc->ether.tx_ring_current_read = current_read;
-
-	/* Relieve flow control when the TX queue is drained at least half way */
-	if (sc->ether.tx_full &&
-	    (sc->ether.tx_free_cnt >= (SBE_2T3E3_TX_DESC_RING_SIZE / 2))) {
-		sc->ether.tx_full = 0;
-		netif_wake_queue(sc->dev);
-	}
-	spin_unlock(&sc->ether.tx_lock);
-}
-
-
-void dc_intr_tx_underflow(struct channel *sc)
-{
-	u32 val;
-
-	dc_transmitter_onoff(sc, SBE_2T3E3_OFF);
-
-	val = dc_read(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE);
-	dc_clear_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-		      SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS);
-
-	switch (val & SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS) {
-	case SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_1:
-		dc_set_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-			    SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_2);
-		break;
-	case SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_2:
-		dc_set_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-			    SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_3);
-		break;
-	case SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_3:
-		dc_set_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-			    SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_4);
-		break;
-	case SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_4:
-	default:
-		dc_set_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-			    SBE_2T3E3_21143_VAL_STORE_AND_FORWARD);
-		break;
-	}
-
-	dc_transmitter_onoff(sc, SBE_2T3E3_ON);
-}
-
-
-
-
-void exar7250_intr(struct channel *sc)
-{
-	u32 status, old_OOF;
-
-	old_OOF = sc->s.OOF;
-
-	status = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_BLOCK_INTERRUPT_STATUS);
-	dev_dbg(&sc->pdev->dev, "SBE 2T3E3: Framer interrupt! (REG[0x05] = %02X)\n", status);
-
-	switch (sc->p.frame_type) {
-	case SBE_2T3E3_FRAME_TYPE_E3_G751:
-	case SBE_2T3E3_FRAME_TYPE_E3_G832:
-		exar7250_E3_intr(sc, status);
-		break;
-
-	case SBE_2T3E3_FRAME_TYPE_T3_CBIT:
-	case SBE_2T3E3_FRAME_TYPE_T3_M13:
-		exar7250_T3_intr(sc, status);
-		break;
-
-	default:
-		break;
-	}
-
-	if (sc->s.OOF != old_OOF) {
-		if (sc->s.OOF) {
-			if (sc->p.loopback == SBE_2T3E3_LOOPBACK_NONE) {
-				dev_dbg(&sc->pdev->dev, "SBE 2T3E3: Disabling eth interrupts\n");
-				/* turn off ethernet interrupts */
-				dc_stop_intr(sc);
-			}
-		} else if (sc->r.flags & SBE_2T3E3_FLAG_NETWORK_UP) {
-			dev_dbg(&sc->pdev->dev, "SBE 2T3E3: Enabling eth interrupts\n");
-			/* start interrupts */
-			sc->s.OOF = 1;
-			dc_intr_rx(sc);
-			sc->s.OOF = 0;
-			if (sc->p.receiver_on) {
-				dc_receiver_onoff(sc, SBE_2T3E3_OFF);
-				dc_receiver_onoff(sc, SBE_2T3E3_ON);
-			}
-			dc_start_intr(sc);
-		}
-	}
-}
-
-
-void exar7250_T3_intr(struct channel *sc, u32 block_status)
-{
-	u32 status, result;
-
-	if (block_status & SBE_2T3E3_FRAMER_VAL_RX_INTERRUPT_STATUS) {
-		status = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_INTERRUPT_STATUS);
-
-		if (status) {
-			dev_dbg(&sc->pdev->dev,
-				"Framer interrupt T3 RX (REG[0x13] = %02X)\n",
-				status);
-
-			result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_CONFIGURATION_STATUS);
-
-			cpld_LOS_update(sc);
-
-			if (status & SBE_2T3E3_FRAMER_VAL_T3_RX_OOF_INTERRUPT_STATUS) {
-				sc->s.OOF = result & SBE_2T3E3_FRAMER_VAL_T3_RX_OOF ? 1 : 0;
-				dev_dbg(&sc->pdev->dev,
-					"Framer interrupt T3: OOF (%d)\n",
-					sc->s.OOF);
-			}
-
-			exar7250_write(sc, SBE_2T3E3_FRAMER_REG_T3_RX_INTERRUPT_ENABLE,
-				       SBE_2T3E3_FRAMER_VAL_T3_RX_LOS_INTERRUPT_ENABLE |
-				       SBE_2T3E3_FRAMER_VAL_T3_RX_OOF_INTERRUPT_ENABLE);
-				}
-
-		status = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_FEAC_INTERRUPT_ENABLE_STATUS);
-		if (status) {
-			dev_dbg(&sc->pdev->dev,
-				"Framer interrupt T3 RX (REG[0x17] = %02X)\n",
-				status);
-		}
-
-		status = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_LAPD_CONTROL);
-		if (status)
-			dev_dbg(&sc->pdev->dev,
-				"Framer interrupt T3 RX (REG[0x18] = %02X)\n",
-				status);
-	}
-
-
-	if (block_status & SBE_2T3E3_FRAMER_VAL_TX_INTERRUPT_STATUS) {
-		status = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_TX_FEAC_CONFIGURATION_STATUS);
-		dev_dbg(&sc->pdev->dev, "SBE 2T3E3: Framer interrupt T3 TX (REG[0x31] = %02X)\n",
-			status);
-
-		status = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_TX_LAPD_STATUS);
-		dev_dbg(&sc->pdev->dev, "SBE 2T3E3: Framer interrupt T3 TX (REG[0x34] = %02X)\n",
-			status);
-	}
-}
-
-
-void exar7250_E3_intr(struct channel *sc, u32 block_status)
-{
-	u32 status, result;
-
-	if (block_status & SBE_2T3E3_FRAMER_VAL_RX_INTERRUPT_STATUS) {
-		status = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_STATUS_1);
-
-		if (status) {
-			dev_dbg(&sc->pdev->dev,
-				"Framer interrupt E3 RX (REG[0x14] = %02X)\n",
-				status);
-
-			result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_CONFIGURATION_STATUS_2);
-
-			cpld_LOS_update(sc);
-
-			if (status & SBE_2T3E3_FRAMER_VAL_E3_RX_OOF_INTERRUPT_STATUS) {
-				sc->s.OOF = result & SBE_2T3E3_FRAMER_VAL_E3_RX_OOF ? 1 : 0;
-				dev_dbg(&sc->pdev->dev,
-					"Framer interrupt E3: OOF (%d)\n",
-					sc->s.OOF);
-			}
-
-			exar7250_write(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_ENABLE_1,
-				       SBE_2T3E3_FRAMER_VAL_E3_RX_OOF_INTERRUPT_ENABLE |
-				       SBE_2T3E3_FRAMER_VAL_E3_RX_LOS_INTERRUPT_ENABLE
-				);
-				}
-
-		status = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_STATUS_2);
-		if (status) {
-			dev_dbg(&sc->pdev->dev,
-				"Framer interrupt E3 RX (REG[0x15] = %02X)\n",
-				status);
-
-		}
-
-	}
-
-	if (block_status & SBE_2T3E3_FRAMER_VAL_TX_INTERRUPT_STATUS) {
-		status = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_TX_LAPD_STATUS);
-		dev_dbg(&sc->pdev->dev, "SBE 2T3E3: Framer interrupt E3 TX (REG[0x34] = %02X)\n",
-			status);
-	}
-}
diff --git a/drivers/staging/sbe-2t3e3/io.c b/drivers/staging/sbe-2t3e3/io.c
deleted file mode 100644
index c9947b165b3..00000000000
--- a/drivers/staging/sbe-2t3e3/io.c
+++ /dev/null
@@ -1,330 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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 code is based on a driver written by SBE Inc.
- */
-
-#include <linux/ip.h>
-#include "2t3e3.h"
-#include "ctrl.h"
-
-/* All access to registers done via the 21143 on port 0 must be
- * protected via the card->bootrom_lock. */
-
-/* private define to be used here only - must be protected by card->bootrom_lock */
-#define cpld_write_nolock(channel, reg, val)			\
-	bootrom_write((channel), CPLD_MAP_REG(reg, channel), val)
-
-u32 cpld_read(struct channel *channel, u32 reg)
-{
-	unsigned long flags;
-	u32 val;
-
-	spin_lock_irqsave(&channel->card->bootrom_lock, flags);
-	val = bootrom_read((channel), CPLD_MAP_REG(reg, channel));
-	spin_unlock_irqrestore(&channel->card->bootrom_lock, flags);
-	return val;
-}
-
-/****************************************
- * Access via BootROM port
- ****************************************/
-
-u32 bootrom_read(struct channel *channel, u32 reg)
-{
-	unsigned long addr = channel->card->bootrom_addr;
-	u32 result;
-
-	/* select BootROM address */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_PROGRAMMING_ADDRESS, reg & 0x3FFFF);
-
-	/* select reading from BootROM */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_READ_OPERATION |
-		 SBE_2T3E3_21143_VAL_BOOT_ROM_SELECT);
-
-	udelay(2); /* 20 PCI cycles */
-
-	/* read from BootROM */
-	result = dc_read(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT) & 0xff;
-
-	/* reset CSR9 */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT, 0);
-
-	return result;
-}
-
-void bootrom_write(struct channel *channel, u32 reg, u32 val)
-{
-	unsigned long addr = channel->card->bootrom_addr;
-
-	/* select BootROM address */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_PROGRAMMING_ADDRESS, reg & 0x3FFFF);
-
-	/* select writting to BootROM */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_WRITE_OPERATION |
-		 SBE_2T3E3_21143_VAL_BOOT_ROM_SELECT |
-		 (val & 0xff));
-
-	udelay(2); /* 20 PCI cycles */
-
-	/* reset CSR9 */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT, 0);
-}
-
-
-/****************************************
- * Access via Serial I/O port
- ****************************************/
-
-static u32 serialrom_read_bit(struct channel *channel)
-{
-	unsigned long addr = channel->card->bootrom_addr;
-	u32 bit;
-
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_READ_OPERATION |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CLOCK |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT);	/* clock high */
-
-	bit = (dc_read(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT) &
-	       SBE_2T3E3_21143_VAL_SERIAL_ROM_DATA_OUT) > 0 ? 1 : 0;
-
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_READ_OPERATION |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT);	/* clock low */
-
-	return bit;
-}
-
-static void serialrom_write_bit(struct channel *channel, u32 bit)
-{
-	unsigned long addr = channel->card->bootrom_addr;
-	u32 lastbit = -1;
-
-	bit &= 1;
-
-	if (bit != lastbit) {
-		dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-			 SBE_2T3E3_21143_VAL_WRITE_OPERATION |
-			 SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT |
-			 SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT |
-			 (bit << 2)); /* clock low */
-
-		lastbit = bit;
-	}
-
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_WRITE_OPERATION |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CLOCK |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT |
-		 (bit << 2)); /* clock high */
-
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_WRITE_OPERATION |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT |
-		 (bit << 2)); /* clock low */
-}
-
-/****************************************
- * Access to SerialROM (eeprom)
- ****************************************/
-
-u32 t3e3_eeprom_read_word(struct channel *channel, u32 address)
-{
-	unsigned long addr = channel->card->bootrom_addr;
-	u32 i, val;
-	unsigned long flags;
-
-	address &= 0x3f;
-
-	spin_lock_irqsave(&channel->card->bootrom_lock, flags);
-
-	/* select correct Serial Chip */
-	cpld_write_nolock(channel, SBE_2T3E3_CPLD_REG_SERIAL_CHIP_SELECT,
-			  SBE_2T3E3_CPLD_VAL_EEPROM_SELECT);
-
-	/* select reading from Serial I/O Bus */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_READ_OPERATION |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT);        /* clock low */
-
-	/* select read operation */
-	serialrom_write_bit(channel, 0);
-	serialrom_write_bit(channel, 1);
-	serialrom_write_bit(channel, 1);
-	serialrom_write_bit(channel, 0);
-
-	for (i = 0x20; i; i >>= 1)
-		serialrom_write_bit(channel, address & i ? 1 : 0);
-
-	val = 0;
-	for (i = 0x8000; i; i >>= 1)
-		val |= (serialrom_read_bit(channel) ? i : 0);
-
-	/* Reset 21143's CSR9 */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_READ_OPERATION |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT);        /* clock low */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT, 0);
-
-	/* Unselect Serial Chip */
-	cpld_write_nolock(channel, SBE_2T3E3_CPLD_REG_SERIAL_CHIP_SELECT, 0);
-
-	spin_unlock_irqrestore(&channel->card->bootrom_lock, flags);
-
-	return ntohs(val);
-}
-
-
-/****************************************
- * Access to Framer
- ****************************************/
-
-u32 exar7250_read(struct channel *channel, u32 reg)
-{
-	u32 result;
-	unsigned long flags;
-
-	spin_lock_irqsave(&channel->card->bootrom_lock, flags);
-
-	result = bootrom_read(channel, cpld_reg_map[SBE_2T3E3_CPLD_REG_FRAMER_BASE_ADDRESS]
-			      [channel->h.slot] + (t3e3_framer_reg_map[reg] << 2));
-
-	spin_unlock_irqrestore(&channel->card->bootrom_lock, flags);
-
-	return result;
-}
-
-void exar7250_write(struct channel *channel, u32 reg, u32 val)
-{
-	unsigned long flags;
-
-	val &= 0xff;
-	channel->framer_regs[reg] = val;
-
-	spin_lock_irqsave(&channel->card->bootrom_lock, flags);
-
-	bootrom_write(channel, cpld_reg_map[SBE_2T3E3_CPLD_REG_FRAMER_BASE_ADDRESS]
-		      [channel->h.slot] + (t3e3_framer_reg_map[reg] << 2), val);
-
-	spin_unlock_irqrestore(&channel->card->bootrom_lock, flags);
-}
-
-
-/****************************************
- * Access to LIU
- ****************************************/
-
-u32 exar7300_read(struct channel *channel, u32 reg)
-{
-	unsigned long addr = channel->card->bootrom_addr, flags;
-	u32 i, val;
-
-	/* select correct Serial Chip */
-
-	spin_lock_irqsave(&channel->card->bootrom_lock, flags);
-
-	cpld_write_nolock(channel, SBE_2T3E3_CPLD_REG_SERIAL_CHIP_SELECT,
-			  cpld_val_map[SBE_2T3E3_CPLD_VAL_LIU_SELECT][channel->h.slot]);
-
-	/* select reading from Serial I/O Bus */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_READ_OPERATION |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT);	/* clock low */
-
-	/* select read operation */
-	serialrom_write_bit(channel, 1);
-
-	/* Exar7300 register address is 4 bit long */
-	reg = t3e3_liu_reg_map[reg];
-	for (i = 0; i < 4; i++, reg >>= 1) /* 4 bits of SerialROM address */
-		serialrom_write_bit(channel, reg & 1);
-	for (i = 0; i < 3; i++)	/* remaining 3 bits of SerialROM address */
-		serialrom_write_bit(channel, 0);
-
-	val = 0; /* Exar7300 register value is 5 bit long */
-	for (i = 0; i < 8; i++)	/* 8 bits of SerialROM value */
-		val += (serialrom_read_bit(channel) << i);
-
-	/* Reset 21143's CSR9 */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_READ_OPERATION |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT);	/* clock low */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT, 0);
-
-	/* Unselect Serial Chip */
-	cpld_write_nolock(channel, SBE_2T3E3_CPLD_REG_SERIAL_CHIP_SELECT, 0);
-
-	spin_unlock_irqrestore(&channel->card->bootrom_lock, flags);
-
-	return val;
-}
-
-void exar7300_write(struct channel *channel, u32 reg, u32 val)
-{
-	unsigned long addr = channel->card->bootrom_addr, flags;
-	u32 i;
-
-	channel->liu_regs[reg] = val;
-
-	/* select correct Serial Chip */
-
-	spin_lock_irqsave(&channel->card->bootrom_lock, flags);
-
-	cpld_write_nolock(channel, SBE_2T3E3_CPLD_REG_SERIAL_CHIP_SELECT,
-			  cpld_val_map[SBE_2T3E3_CPLD_VAL_LIU_SELECT][channel->h.slot]);
-
-	/* select writting to Serial I/O Bus */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_WRITE_OPERATION |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT);	/* clock low */
-
-	/* select write operation */
-	serialrom_write_bit(channel, 0);
-
-	/* Exar7300 register address is 4 bit long */
-	reg = t3e3_liu_reg_map[reg];
-	for (i = 0; i < 4; i++) {	/* 4 bits */
-		serialrom_write_bit(channel, reg & 1);
-		reg >>= 1;
-	}
-	for (i = 0; i < 3; i++)	/* remaining 3 bits of SerialROM address */
-		serialrom_write_bit(channel, 0);
-
-	/* Exar7300 register value is 5 bit long */
-	for (i = 0; i < 5; i++) {
-		serialrom_write_bit(channel, val & 1);
-		val >>= 1;
-	}
-	for (i = 0; i < 3; i++)	/* remaining 3 bits of SerialROM value */
-		serialrom_write_bit(channel, 0);
-
-	/* Reset 21143_CSR9 */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_WRITE_OPERATION |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT);	/* clock low */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT, 0);
-
-	/* Unselect Serial Chip */
-	cpld_write_nolock(channel, SBE_2T3E3_CPLD_REG_SERIAL_CHIP_SELECT, 0);
-
-	spin_unlock_irqrestore(&channel->card->bootrom_lock, flags);
-}
diff --git a/drivers/staging/sbe-2t3e3/main.c b/drivers/staging/sbe-2t3e3/main.c
deleted file mode 100644
index c8e039860dc..00000000000
--- a/drivers/staging/sbe-2t3e3/main.c
+++ /dev/null
@@ -1,172 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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 code is based on a driver written by SBE Inc.
- */
-
-#include <linux/interrupt.h>
-#include <linux/netdevice.h>
-#include "2t3e3.h"
-
-void t3e3_init(struct channel *sc)
-{
-	cpld_init(sc);
-	dc_reset(sc);
-	dc_init(sc);
-	exar7250_init(sc);
-	exar7300_init(sc);
-}
-
-int t3e3_if_start_xmit(struct sk_buff *skb, struct net_device *dev)
-{
-	struct channel *sc = dev_to_priv(dev);
-	u32 current_write, last_write;
-	unsigned long flags;
-	struct sk_buff *skb2;
-
-	if (skb == NULL) {
-		sc->s.out_errors++;
-		return 0;
-	}
-
-	if (sc->p.transmitter_on != SBE_2T3E3_ON) {
-		sc->s.out_errors++;
-		sc->s.out_dropped++;
-		dev_kfree_skb_any(skb);
-		return 0;
-	}
-
-	if (sc->s.OOF && sc->p.loopback == SBE_2T3E3_LOOPBACK_NONE) {
-		sc->s.out_dropped++;
-		dev_kfree_skb_any(skb);
-		return 0;
-	}
-
-	spin_lock_irqsave(&sc->ether.tx_lock, flags);
-
-	current_write = sc->ether.tx_ring_current_write;
-	for (skb2 = skb; skb2 != NULL; skb2 = NULL) {
-		if (skb2->len) {
-			if ((sc->ether.tx_ring[current_write].tdes1 &
-			     SBE_2T3E3_TX_DESC_BUFFER_1_SIZE) > 0)
-				break;
-			current_write = (current_write + 1) % SBE_2T3E3_TX_DESC_RING_SIZE;
-			/*
-			 * Leave at least 1 tx desc free so that dc_intr_tx() can
-			 * identify empty list
-			 */
-			if (current_write == sc->ether.tx_ring_current_read)
-				break;
-		}
-	}
-	if (skb2 != NULL) {
-		netif_stop_queue(sc->dev);
-		sc->ether.tx_full = 1;
-		dev_dbg(&sc->pdev->dev, "SBE 2T3E3: out of descriptors\n");
-		spin_unlock_irqrestore(&sc->ether.tx_lock, flags);
-		return NETDEV_TX_BUSY;
-	}
-
-	current_write = last_write = sc->ether.tx_ring_current_write;
-	dev_dbg(&sc->pdev->dev, "sending mbuf (current_write = %d)\n",
-		current_write);
-
-	for (skb2 = skb; skb2 != NULL; skb2 = NULL) {
-		if (skb2->len) {
-			dev_dbg(&sc->pdev->dev,
-				"sending mbuf (len = %d, next = %p)\n",
-				skb2->len, NULL);
-
-			sc->ether.tx_free_cnt--;
-			sc->ether.tx_ring[current_write].tdes0 = 0;
-			sc->ether.tx_ring[current_write].tdes1 &=
-				SBE_2T3E3_TX_DESC_END_OF_RING |
-				SBE_2T3E3_TX_DESC_SECOND_ADDRESS_CHAINED;
-/* DISABLE_PADDING sometimes gets lost somehow, hands off... */
-			sc->ether.tx_ring[current_write].tdes1 |=
-				SBE_2T3E3_TX_DESC_DISABLE_PADDING | skb2->len;
-
-			if (current_write == sc->ether.tx_ring_current_write) {
-				sc->ether.tx_ring[current_write].tdes1 |=
-					SBE_2T3E3_TX_DESC_FIRST_SEGMENT;
-			} else {
-				sc->ether.tx_ring[current_write].tdes0 =
-					SBE_2T3E3_TX_DESC_21143_OWN;
-			}
-
-			sc->ether.tx_ring[current_write].tdes2 = virt_to_phys(skb2->data);
-			sc->ether.tx_data[current_write] = NULL;
-
-			last_write = current_write;
-			current_write = (current_write + 1) % SBE_2T3E3_TX_DESC_RING_SIZE;
-		}
-	}
-
-	sc->ether.tx_data[last_write] = skb;
-	sc->ether.tx_ring[last_write].tdes1 |=
-		SBE_2T3E3_TX_DESC_LAST_SEGMENT |
-		SBE_2T3E3_TX_DESC_INTERRUPT_ON_COMPLETION;
-	sc->ether.tx_ring[sc->ether.tx_ring_current_write].tdes0 |=
-		SBE_2T3E3_TX_DESC_21143_OWN;
-	sc->ether.tx_ring_current_write = current_write;
-
-	dev_dbg(&sc->pdev->dev, "txput: tdes0 = %08X        tdes1 = %08X\n",
-		sc->ether.tx_ring[last_write].tdes0,
-		sc->ether.tx_ring[last_write].tdes1);
-
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_TRANSMIT_POLL_DEMAND,
-		 0xffffffff);
-
-	spin_unlock_irqrestore(&sc->ether.tx_lock, flags);
-	return 0;
-}
-
-
-void t3e3_read_card_serial_number(struct channel *sc)
-{
-	u32 i;
-
-	for (i = 0; i < 3; i++)
-		sc->ether.card_serial_number[i] = t3e3_eeprom_read_word(sc, 10 + i);
-
-	netdev_info(sc->dev, "SBE wanPMC-2T3E3 serial number: %04X%04X%04X\n",
-		    sc->ether.card_serial_number[0],
-		    sc->ether.card_serial_number[1],
-		    sc->ether.card_serial_number[2]);
-}
-
-/*
-  bit 0 led1 (green)
-  bit 1 led1 (yellow)
-
-  bit 2 led2 (green)
-  bit 3 led2 (yellow)
-
-  bit 4 led3 (green)
-  bit 5 led3 (yellow)
-
-  bit 6 led4 (green)
-  bit 7 led4 (yellow)
-*/
-
-void update_led(struct channel *sc, int blinker)
-{
-	int leds;
-	if (sc->s.LOS)
-		leds = 0; /* led1 = off */
-	else if (sc->s.OOF)
-		leds = 2; /* led1 = yellow */
-	else if ((blinker & 1) && sc->rcv_count) {
-		leds = 0; /* led1 = off */
-		sc->rcv_count = 0;
-	} else
-		leds = 1; /* led1 = green */
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_LEDR, leds);
-	sc->leds = leds;
-}
diff --git a/drivers/staging/sbe-2t3e3/maps.c b/drivers/staging/sbe-2t3e3/maps.c
deleted file mode 100644
index e5494502cde..00000000000
--- a/drivers/staging/sbe-2t3e3/maps.c
+++ /dev/null
@@ -1,101 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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 code is based on a driver written by SBE Inc.
- */
-
-#include <linux/kernel.h>
-#include "2t3e3.h"
-
-const u32 cpld_reg_map[][2] = {
-	{ 0x0000, 0x0080 }, /* 0 - Port Control Register A (PCRA) */
-	{ 0x0004, 0x0084 }, /* 1 - Port Control Register B (PCRB) */
-	{ 0x0008, 0x0088 }, /* 2 - LCV Count Register (PLCR) */
-	{ 0x000c, 0x008c }, /* 3 - LCV Threshold register (PLTR) */
-	{ 0x0010, 0x0090 }, /* 4 - Payload Fill Register (PPFR) */
-	{ 0x0200, 0x0200 }, /* 5 - Board ID / FPGA Programming Status Register */
-	{ 0x0204, 0x0204 }, /* 6 - FPGA Version Register */
-	{ 0x0800, 0x1000 }, /* 7 - Framer Registers Base Address */
-	{ 0x2000, 0x2000 }, /* 8 - Serial Chip Select Register */
-	{ 0x2004, 0x2004 }, /* 9 - Static Reset Register */
-	{ 0x2008, 0x2008 }, /* 10 - Pulse Reset Register */
-	{ 0x200c, 0x200c }, /* 11 - FPGA Reconfiguration Register */
-	{ 0x2010, 0x2014 }, /* 12 - LED Register (LEDR) */
-	{ 0x2018, 0x201c }, /* 13 - LIU Control and Status Register (PISCR) */
-	{ 0x2020, 0x2024 }, /* 14 - Interrupt Enable Register (PIER) */
-	{ 0x0068, 0x00e8 }, /* 15 - Port Control Register C (PCRC) */
-	{ 0x006c, 0x00ec }, /* 16 - Port Bandwidth Start (PBWF) */
-	{ 0x0070, 0x00f0 }, /* 17 - Port Bandwidth Stop (PBWL) */
-};
-
-const u32 cpld_val_map[][2] = {
-	{ 0x01, 0x02 }, /* LIU1 / LIU2 select for Serial Chip Select */
-	{ 0x04, 0x08 }, /* DAC1 / DAC2 select for Serial Chip Select */
-	{ 0x00, 0x04 }, /* LOOP1 / LOOP2 - select of loop timing source */
-	{ 0x01, 0x02 }  /* PORT1 / PORT2 - select LIU and Framer for reset */
-};
-
-const u32 t3e3_framer_reg_map[] = {
-	0x00, /* 0 - OPERATING_MODE */
-	0x01, /* 1 - IO_CONTROL */
-	0x04, /* 2 - BLOCK_INTERRUPT_ENABLE */
-	0x05, /* 3 - BLOCK_INTERRUPT_STATUS */
-	0x10, /* 4 - T3_RX_CONFIGURATION_STATUS, E3_RX_CONFIGURATION_STATUS_1 */
-	0x11, /* 5 - T3_RX_STATUS, E3_RX_CONFIGURATION_STATUS_2 */
-	0x12, /* 6 - T3_RX_INTERRUPT_ENABLE, E3_RX_INTERRUPT_ENABLE_1 */
-	0x13, /* 7 - T3_RX_INTERRUPT_STATUS, E3_RX_INTERRUPT_ENABLE_2 */
-	0x14, /* 8 - T3_RX_SYNC_DETECT_ENABLE, E3_RX_INTERRUPT_STATUS_1 */
-	0x15, /* 9 - E3_RX_INTERRUPT_STATUS_2 */
-	0x16, /* 10 - T3_RX_FEAC */
-	0x17, /* 11 - T3_RX_FEAC_INTERRUPT_ENABLE_STATUS */
-	0x18, /* 12 - T3_RX_LAPD_CONTROL, E3_RX_LAPD_CONTROL */
-	0x19, /* 13 - T3_RX_LAPD_STATUS, E3_RX_LAPD_STATUS */
-	0x1a, /* 14 - E3_RX_NR_BYTE, E3_RX_SERVICE_BITS */
-	0x1b, /* 15 - E3_RX_GC_BYTE */
-	0x30, /* 16 - T3_TX_CONFIGURATION, E3_TX_CONFIGURATION */
-	0x31, /* 17 - T3_TX_FEAC_CONFIGURATION_STATUS */
-	0x32, /* 18 - T3_TX_FEAC */
-	0x33, /* 19 - T3_TX_LAPD_CONFIGURATION, E3_TX_LAPD_CONFIGURATION */
-	0x34, /* 20 - T3_TX_LAPD_STATUS, E3_TX_LAPD_STATUS_INTERRUPT */
-	0x35, /* 21 - T3_TX_MBIT_MASK, E3_TX_GC_BYTE, E3_TX_SERVICE_BITS */
-	0x36, /* 22 - T3_TX_FBIT_MASK, E3_TX_MA_BYTE */
-	0x37, /* 23 - T3_TX_FBIT_MASK_2, E3_TX_NR_BYTE */
-	0x38, /* 24 - T3_TX_FBIT_MASK_3 */
-	0x48, /* 25 - E3_TX_FA1_ERROR_MASK, E3_TX_FAS_ERROR_MASK_UPPER */
-	0x49, /* 26 - E3_TX_FA2_ERROR_MASK, E3_TX_FAS_ERROR_MASK_LOWER */
-	0x4a, /* 27 - E3_TX_BIP8_MASK, E3_TX_BIP4_MASK */
-	0x50, /* 28 - PMON_LCV_EVENT_COUNT_MSB */
-	0x51, /* 29 - PMON_LCV_EVENT_COUNT_LSB */
-	0x52, /* 30 - PMON_FRAMING_BIT_ERROR_EVENT_COUNT_MSB */
-	0x53, /* 31 - PMON_FRAMING_BIT_ERROR_EVENT_COUNT_LSB */
-	0x54, /* 32 - PMON_PARITY_ERROR_EVENT_COUNT_MSB */
-	0x55, /* 33 - PMON_PARITY_ERROR_EVENT_COUNT_LSB */
-	0x56, /* 34 - PMON_FEBE_EVENT_COUNT_MSB */
-	0x57, /* 35 - PMON_FEBE_EVENT_COUNT_LSB */
-	0x58, /* 36 - PMON_CP_BIT_ERROR_EVENT_COUNT_MSB */
-	0x59, /* 37 - PMON_CP_BIT_ERROR_EVENT_COUNT_LSB */
-	0x6c, /* 38 - PMON_HOLDING_REGISTER */
-	0x6d, /* 39 - ONE_SECOND_ERROR_STATUS */
-	0x6e, /* 40 - LCV_ONE_SECOND_ACCUMULATOR_MSB */
-	0x6f, /* 41 - LCV_ONE_SECOND_ACCUMULATOR_LSB */
-	0x70, /* 42 - FRAME_PARITY_ERROR_ONE_SECOND_ACCUMULATOR_MSB */
-	0x71, /* 43 - FRAME_PARITY_ERROR_ONE_SECOND_ACCUMULATOR_LSB */
-	0x72, /* 44 - FRAME_CP_BIT_ERROR_ONE_SECOND_ACCUMULATOR_MSB */
-	0x73, /* 45 - FRAME_CP_BIT_ERROR_ONE_SECOND_ACCUMULATOR_LSB */
-	0x80, /* 46 - LINE_INTERFACE_DRIVE */
-	0x81  /* 47 - LINE_INTERFACE_SCAN */
-};
-
-const u32 t3e3_liu_reg_map[] = {
-	0x00, /* REG0 */
-	0x01, /* REG1 */
-	0x02, /* REG2 */
-	0x03, /* REG3 */
-	0x04 /* REG4 */
-};
diff --git a/drivers/staging/sbe-2t3e3/module.c b/drivers/staging/sbe-2t3e3/module.c
deleted file mode 100644
index a6f93a43d21..00000000000
--- a/drivers/staging/sbe-2t3e3/module.c
+++ /dev/null
@@ -1,208 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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 code is based on a driver written by SBE Inc.
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/delay.h>
-#include <linux/netdevice.h>
-#include <linux/pci.h>
-#include <linux/hdlc.h>
-#include <linux/if_arp.h>
-#include <linux/interrupt.h>
-#include "2t3e3.h"
-
-static void check_leds(unsigned long arg)
-{
-	struct card *card = (struct card *)arg;
-	struct channel *channel0 = &card->channels[0];
-	static int blinker;
-
-	update_led(channel0, ++blinker);
-	if (has_two_ports(channel0->pdev))
-		update_led(&card->channels[1], blinker);
-
-	card->timer.expires = jiffies + HZ / 10;
-	add_timer(&card->timer);
-}
-
-static void t3e3_remove_channel(struct channel *channel)
-{
-	struct pci_dev *pdev = channel->pdev;
-	struct net_device *dev = channel->dev;
-
-	/* system hangs if board asserts irq while module is unloaded */
-	cpld_stop_intr(channel);
-	free_irq(dev->irq, dev);
-	dc_drop_descriptor_list(channel);
-	unregister_hdlc_device(dev);
-	free_netdev(dev);
-	pci_release_regions(pdev);
-	pci_disable_device(pdev);
-	pci_set_drvdata(pdev, NULL);
-}
-
-static int t3e3_init_channel(struct channel *channel, struct pci_dev *pdev, struct card *card)
-{
-	struct net_device *dev;
-	unsigned int val;
-	int err;
-
-	err = pci_enable_device(pdev);
-	if (err)
-		return err;
-
-	err = pci_request_regions(pdev, "SBE 2T3E3");
-	if (err)
-		goto disable;
-
-	dev = alloc_hdlcdev(channel);
-	if (!dev) {
-		pr_err("Out of memory\n");
-		err = -ENOMEM;
-		goto free_regions;
-	}
-
-	t3e3_sc_init(channel);
-	dev_to_priv(dev) = channel;
-
-	channel->pdev = pdev;
-	channel->dev = dev;
-	channel->card = card;
-	channel->addr = pci_resource_start(pdev, 0);
-	if (pdev->subsystem_device == PCI_SUBDEVICE_ID_SBE_2T3E3_P1)
-		channel->h.slot = 1;
-	else
-		channel->h.slot = 0;
-
-	err = setup_device(dev, channel);
-	if (err)
-		goto free_dev;
-
-	pci_read_config_dword(channel->pdev, 0x40, &val); /* mask sleep mode */
-	pci_write_config_dword(channel->pdev, 0x40, val & 0x3FFFFFFF);
-
-	pci_read_config_byte(channel->pdev, PCI_CACHE_LINE_SIZE, &channel->h.cache_size);
-	pci_read_config_dword(channel->pdev, PCI_COMMAND, &channel->h.command);
-	t3e3_init(channel);
-
-	err = request_irq(dev->irq, &t3e3_intr, IRQF_SHARED, dev->name, dev);
-	if (err) {
-		netdev_warn(channel->dev, "%s: could not get irq: %d\n",
-			    dev->name, dev->irq);
-		goto unregister_dev;
-	}
-
-	pci_set_drvdata(pdev, channel);
-	return 0;
-
-unregister_dev:
-	unregister_hdlc_device(dev);
-free_dev:
-	free_netdev(dev);
-free_regions:
-	pci_release_regions(pdev);
-disable:
-	pci_disable_device(pdev);
-	return err;
-}
-
-static void t3e3_remove_card(struct pci_dev *pdev)
-{
-	struct channel *channel0 = pci_get_drvdata(pdev);
-	struct card *card = channel0->card;
-
-	del_timer_sync(&card->timer);
-	if (has_two_ports(channel0->pdev)) {
-		t3e3_remove_channel(&card->channels[1]);
-		pci_dev_put(card->channels[1].pdev);
-	}
-	t3e3_remove_channel(channel0);
-	kfree(card);
-}
-
-static int t3e3_init_card(struct pci_dev *pdev, const struct pci_device_id *ent)
-{
-	/* pdev points to channel #0 */
-	struct pci_dev *pdev1 = NULL;
-	struct card *card;
-	int channels = 1, err;
-
-	if (has_two_ports(pdev)) {
-		while ((pdev1 = pci_get_subsys(PCI_VENDOR_ID_DEC, PCI_DEVICE_ID_DEC_21142,
-					       PCI_VENDOR_ID_SBE, PCI_SUBDEVICE_ID_SBE_2T3E3_P1,
-					       pdev1)))
-			if (pdev1->bus == pdev->bus &&
-			    pdev1->devfn == pdev->devfn + 8 /* next device on the same bus */)
-				break; /* found the second channel */
-
-		if (!pdev1) {
-			dev_err(&pdev->dev, "Can't find the second channel\n");
-			return -EFAULT;
-		}
-		channels = 2;
-		/* holds the reference for pdev1 */
-	}
-
-	card = kzalloc(sizeof(struct card) + channels * sizeof(struct channel),
-		       GFP_KERNEL);
-	if (!card)
-		return -ENOBUFS;
-
-	spin_lock_init(&card->bootrom_lock);
-	card->bootrom_addr = pci_resource_start(pdev, 0);
-
-	err = t3e3_init_channel(&card->channels[0], pdev, card);
-	if (err)
-		goto free_card;
-
-	if (channels == 2) {
-		err = t3e3_init_channel(&card->channels[1], pdev1, card);
-		if (err) {
-			t3e3_remove_channel(&card->channels[0]);
-			goto free_card;
-		}
-	}
-
-	/* start LED timer */
-	init_timer(&card->timer);
-	card->timer.function = check_leds;
-	card->timer.expires = jiffies + HZ / 10;
-	card->timer.data = (unsigned long)card;
-	add_timer(&card->timer);
-	return 0;
-
-free_card:
-	kfree(card);
-	return err;
-}
-
-static struct pci_device_id t3e3_pci_tbl[] = {
-	{ PCI_VENDOR_ID_DEC, PCI_DEVICE_ID_DEC_21142,
-	  PCI_VENDOR_ID_SBE, PCI_SUBDEVICE_ID_SBE_T3E3, 0, 0, 0 },
-	{ PCI_VENDOR_ID_DEC, PCI_DEVICE_ID_DEC_21142,
-	  PCI_VENDOR_ID_SBE, PCI_SUBDEVICE_ID_SBE_2T3E3_P0, 0, 0, 0 },
-	/* channel 1 will be initialized after channel 0 */
-	{ 0, }
-};
-
-static struct pci_driver t3e3_pci_driver = {
-	.name     = "SBE T3E3",
-	.id_table = t3e3_pci_tbl,
-	.probe    = t3e3_init_card,
-	.remove   = t3e3_remove_card,
-};
-
-module_pci_driver(t3e3_pci_driver);
-MODULE_LICENSE("GPL");
-MODULE_DEVICE_TABLE(pci, t3e3_pci_tbl);
diff --git a/drivers/staging/sbe-2t3e3/netdev.c b/drivers/staging/sbe-2t3e3/netdev.c
deleted file mode 100644
index fe6c9513c9c..00000000000
--- a/drivers/staging/sbe-2t3e3/netdev.c
+++ /dev/null
@@ -1,143 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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 code is based on a driver written by SBE Inc.
- */
-
-#include <linux/capability.h>
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/delay.h>
-#include <linux/netdevice.h>
-#include <linux/pci.h>
-#include <linux/hdlc.h>
-#include <linux/if_arp.h>
-#include <linux/interrupt.h>
-#include "2t3e3.h"
-
-static int t3e3_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
-{
-	struct channel *sc = dev_to_priv(dev);
-	int cmd_2t3e3, len, rlen;
-	struct t3e3_param param;
-	struct t3e3_resp resp;
-	void __user *data = ifr->ifr_data + sizeof(cmd_2t3e3) + sizeof(len);
-
-	if (cmd == SIOCWANDEV)
-		return hdlc_ioctl(dev, ifr, cmd);
-	if (!capable(CAP_SYS_ADMIN))
-		return -EPERM;
-	if (cmd != SIOCDEVPRIVATE + 15)
-		return -EINVAL;
-
-	if (copy_from_user(&cmd_2t3e3, ifr->ifr_data, sizeof(cmd_2t3e3)))
-		return -EFAULT;
-	if (copy_from_user(&len, ifr->ifr_data + sizeof(cmd_2t3e3), sizeof(len)))
-		return -EFAULT;
-
-	if (len > sizeof(param))
-		return -EFAULT;
-
-	if (len)
-		if (copy_from_user(&param, data, len))
-			return -EFAULT;
-
-	t3e3_if_config(sc, cmd_2t3e3, (char *)&param, &resp, &rlen);
-
-	if (rlen)
-		if (copy_to_user(data, &resp, rlen))
-			return -EFAULT;
-
-	return 0;
-}
-
-static struct net_device_stats *t3e3_get_stats(struct net_device *dev)
-{
-	struct net_device_stats *nstats = &dev->stats;
-	struct channel *sc = dev_to_priv(dev);
-	struct t3e3_stats *stats = &sc->s;
-
-	memset(nstats, 0, sizeof(struct net_device_stats));
-	nstats->rx_packets = stats->in_packets;
-	nstats->tx_packets = stats->out_packets;
-	nstats->rx_bytes = stats->in_bytes;
-	nstats->tx_bytes = stats->out_bytes;
-
-	nstats->rx_errors = stats->in_errors;
-	nstats->tx_errors = stats->out_errors;
-	nstats->rx_crc_errors = stats->in_error_crc;
-
-
-	nstats->rx_dropped = stats->in_dropped;
-	nstats->tx_dropped = stats->out_dropped;
-	nstats->tx_carrier_errors = stats->out_error_lost_carr +
-		stats->out_error_no_carr;
-
-	return nstats;
-}
-
-static int t3e3_open(struct net_device *dev)
-{
-	struct channel *sc = dev_to_priv(dev);
-	int ret = hdlc_open(dev);
-
-	if (ret)
-		return ret;
-
-	sc->r.flags |= SBE_2T3E3_FLAG_NETWORK_UP;
-	dc_start(dev_to_priv(dev));
-	netif_start_queue(dev);
-	try_module_get(THIS_MODULE);
-	return 0;
-}
-
-static int t3e3_close(struct net_device *dev)
-{
-	struct channel *sc = dev_to_priv(dev);
-	hdlc_close(dev);
-	netif_stop_queue(dev);
-	dc_stop(sc);
-	sc->r.flags &= ~SBE_2T3E3_FLAG_NETWORK_UP;
-	module_put(THIS_MODULE);
-	return 0;
-}
-
-static int t3e3_attach(struct net_device *dev, unsigned short foo1,
-		       unsigned short foo2)
-{
-	return 0;
-}
-
-static const struct net_device_ops t3e3_ops = {
-	.ndo_open       = t3e3_open,
-	.ndo_stop       = t3e3_close,
-	.ndo_change_mtu = hdlc_change_mtu,
-	.ndo_start_xmit = hdlc_start_xmit,
-	.ndo_do_ioctl   = t3e3_ioctl,
-	.ndo_get_stats  = t3e3_get_stats,
-};
-
-int setup_device(struct net_device *dev, struct channel *sc)
-{
-	hdlc_device *hdlc = dev_to_hdlc(dev);
-	int retval;
-
-	dev->base_addr = pci_resource_start(sc->pdev, 0);
-	dev->irq = sc->pdev->irq;
-	dev->netdev_ops = &t3e3_ops;
-	dev->tx_queue_len = 100;
-	hdlc->xmit = t3e3_if_start_xmit;
-	hdlc->attach = t3e3_attach;
-	retval = register_hdlc_device(dev);
-	if (retval) {
-		dev_err(&sc->pdev->dev, "error registering HDLC device\n");
-		return retval;
-	}
-	return 0;
-}
diff --git a/drivers/staging/sep/sep_crypto.c b/drivers/staging/sep/sep_crypto.c
index 965485f71fe..415f8ec5276 100644
--- a/drivers/staging/sep/sep_crypto.c
+++ b/drivers/staging/sep/sep_crypto.c
@@ -93,6 +93,7 @@ static void sep_do_callback(struct work_struct *work)
 {
 	struct sep_work_struct *sep_work = container_of(work,
 		struct sep_work_struct, work);
+
 	if (sep_work != NULL) {
 		(sep_work->callback)(sep_work->data);
 		kfree(sep_work);
@@ -649,6 +650,7 @@ weak:
 static u32 sep_sg_nents(struct scatterlist *sg)
 {
 	u32 ct1 = 0;
+
 	while (sg) {
 		ct1 += 1;
 		sg = sg_next(sg);
@@ -666,6 +668,7 @@ static u32 sep_sg_nents(struct scatterlist *sg)
 static u32 sep_start_msg(struct this_task_ctx *ta_ctx)
 {
 	u32 *word_ptr;
+
 	ta_ctx->msg_len_words = 2;
 	ta_ctx->msgptr = ta_ctx->msg;
 	memset(ta_ctx->msg, 0, SEP_DRIVER_MESSAGE_SHARED_AREA_SIZE_IN_BYTES);
@@ -740,6 +743,7 @@ static void sep_write_msg(struct this_task_ctx *ta_ctx, void *in_addr,
 {
 	u32 *word_ptr;
 	void *void_ptr;
+
 	void_ptr = ta_ctx->msgptr + *msg_offset;
 	word_ptr = (u32 *)void_ptr;
 	memcpy(void_ptr, in_addr, size);
@@ -748,6 +752,7 @@ static void sep_write_msg(struct this_task_ctx *ta_ctx, void *in_addr,
 	/* Do we need to manipulate endian? */
 	if (byte_array) {
 		u32 i;
+
 		for (i = 0; i < ((size + 3) / 4); i += 1)
 			*(word_ptr + i) = CHG_ENDIAN(*(word_ptr + i));
 	}
@@ -788,12 +793,14 @@ static void sep_read_msg(struct this_task_ctx *ta_ctx, void *in_addr,
 {
 	u32 *word_ptr;
 	void *void_ptr;
+
 	void_ptr = ta_ctx->msgptr + *msg_offset;
 	word_ptr = (u32 *)void_ptr;
 
 	/* Do we need to manipulate endian? */
 	if (byte_array) {
 		u32 i;
+
 		for (i = 0; i < ((size + 3) / 4); i += 1)
 			*(word_ptr + i) = CHG_ENDIAN(*(word_ptr + i));
 	}
@@ -865,6 +872,7 @@ static void sep_read_context(struct this_task_ctx *ta_ctx, u32 *msg_offset,
 	void *dst, u32 len)
 {
 	u32 max_length = ((len + 3) / sizeof(u32)) * sizeof(u32);
+
 	sep_read_msg(ta_ctx, dst, len, max_length, msg_offset, 0);
 }
 
@@ -884,6 +892,7 @@ static void sep_write_context(struct this_task_ctx *ta_ctx, u32 *msg_offset,
 	void *src, u32 len)
 {
 	u32 max_length = ((len + 3) / sizeof(u32)) * sizeof(u32);
+
 	sep_write_msg(ta_ctx, src, len, max_length, msg_offset, 0);
 }
 
@@ -3893,6 +3902,7 @@ static struct crypto_alg crypto_algs[] = {
 int sep_crypto_setup(void)
 {
 	int err, i, j, k;
+
 	tasklet_init(&sep_dev->finish_tasklet, sep_finish,
 		(unsigned long)sep_dev);
 
diff --git a/drivers/staging/sep/sep_dev.h b/drivers/staging/sep/sep_dev.h
index 5f6a07f59dd..bf56c06662f 100644
--- a/drivers/staging/sep/sep_dev.h
+++ b/drivers/staging/sep/sep_dev.h
@@ -152,6 +152,7 @@ static inline u32 sep_read_reg(struct sep_device *dev, int reg)
 static inline void sep_wait_sram_write(struct sep_device *dev)
 {
 	u32 reg_val;
+
 	do {
 		reg_val = sep_read_reg(dev, HW_SRAM_DATA_READY_REG_ADDR);
 	} while (!(reg_val & 1));
diff --git a/drivers/staging/sep/sep_main.c b/drivers/staging/sep/sep_main.c
index e301207eb0b..75ca15e86a2 100644
--- a/drivers/staging/sep/sep_main.c
+++ b/drivers/staging/sep/sep_main.c
@@ -1400,7 +1400,6 @@ static int sep_lli_table_secure_dma(struct sep_device *sep,
 	struct sep_dma_context *dma_ctx)
 
 {
-	int error = 0;
 	u32 count;
 	/* The the page of the end address of the user space buffer */
 	u32 end_page;
@@ -1491,7 +1490,7 @@ static int sep_lli_table_secure_dma(struct sep_device *sep,
 	dma_ctx->dma_res_arr[dma_ctx->nr_dcb_creat].out_map_array = NULL;
 	dma_ctx->dma_res_arr[dma_ctx->nr_dcb_creat].out_map_num_entries = 0;
 
-	return error;
+	return 0;
 }
 
 /**
diff --git a/drivers/staging/serqt_usb2/serqt_usb2.c b/drivers/staging/serqt_usb2/serqt_usb2.c
index 0267dd8b84b..998c3845421 100644
--- a/drivers/staging/serqt_usb2/serqt_usb2.c
+++ b/drivers/staging/serqt_usb2/serqt_usb2.c
@@ -990,18 +990,10 @@ static void qt_block_until_empty(struct tty_struct *tty,
 static void qt_close(struct usb_serial_port *port)
 {
 	struct usb_serial *serial = port->serial;
-	struct quatech_port *qt_port;
-	struct quatech_port *port0;
-	struct tty_struct *tty;
-	int status;
-	unsigned int index;
-	status = 0;
-
-	tty = tty_port_tty_get(&port->port);
-	index = port->port_number;
-
-	qt_port = qt_get_port_private(port);
-	port0 = qt_get_port_private(serial->port[0]);
+	struct tty_struct *tty = tty_port_tty_get(&port->port);
+	unsigned int index = port->port_number;
+	struct quatech_port *qt_port = qt_get_port_private(port);
+	struct quatech_port *port0 = qt_get_port_private(serial->port[0]);
 
 	/* shutdown any bulk reads that might be going on */
 	if (serial->num_bulk_out)
@@ -1015,8 +1007,7 @@ static void qt_close(struct usb_serial_port *port)
 	tty_kref_put(tty);
 
 	/* Close uart channel */
-	status = qt_close_channel(serial, index);
-	if (status < 0)
+	if (qt_close_channel(serial, index) < 0)
 		dev_dbg(&port->dev, "%s - qt_close_channel failed.\n",
 			__func__);
 
@@ -1268,9 +1259,9 @@ static void qt_set_termios(struct tty_struct *tty,
 	if (I_IXOFF(tty) || I_IXON(tty)) {
 		unsigned char stop_char = STOP_CHAR(tty);
 		unsigned char start_char = START_CHAR(tty);
-		status =
-		    box_set_sw_flow_ctrl(port->serial, index, stop_char,
-				      start_char);
+
+		status = box_set_sw_flow_ctrl(port->serial, index, stop_char,
+					      start_char);
 		if (status < 0)
 			dev_dbg(&port->dev,
 				"box_set_sw_flow_ctrl (enabled) failed\n");
diff --git a/drivers/staging/silicom/bpctl_mod.c b/drivers/staging/silicom/bpctl_mod.c
index 6b9365b28e8..765fce84d9d 100644
--- a/drivers/staging/silicom/bpctl_mod.c
+++ b/drivers/staging/silicom/bpctl_mod.c
@@ -41,7 +41,7 @@ MODULE_AUTHOR("Anna Lukin, annal@silicom.co.il");
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION(BP_MOD_DESCR);
 MODULE_VERSION(BP_MOD_VER);
-spinlock_t bpvm_lock;
+static spinlock_t bpvm_lock;
 
 #define unlock_bpctl()					\
 	up(&bpctl_sema);
@@ -119,7 +119,6 @@ static void if_scan_init(void);
 
 static int bypass_proc_create_dev_sd(struct bpctl_dev *pbp_device_block);
 static int bypass_proc_remove_dev_sd(struct bpctl_dev *pbp_device_block);
-static int bp_proc_create(void);
 
 static int is_bypass_fn(struct bpctl_dev *pbpctl_dev);
 static int get_dev_idx_bsf(int bus, int slot, int func);
@@ -220,8 +219,12 @@ static int bp_device_event(struct notifier_block *unused,
 			if (netif_carrier_ok(dev))
 				return NOTIFY_DONE;
 
-			if (((dev_num = get_dev_idx(dev->ifindex)) == -1) ||
-			    (!(pbpctl_dev = &bpctl_dev_arr[dev_num])))
+			dev_num = get_dev_idx(dev->ifindex);
+			if (dev_num == -1)
+				return NOTIFY_DONE;
+
+			pbpctl_dev = &bpctl_dev_arr[dev_num];
+			if (!pbpctl_dev)
 				return NOTIFY_DONE;
 
 			if ((is_bypass_fn(pbpctl_dev)) == 1)
@@ -749,6 +752,7 @@ static void write_reg(struct bpctl_dev *pbpctl_dev, unsigned char value,
 	uint32_t ctrl_ext = 0, ctrl = 0;
 	struct bpctl_dev *pbpctl_dev_c = NULL;
 	unsigned long flags;
+
 	if (pbpctl_dev->bp_10g9) {
 		pbpctl_dev_c = get_status_port_fn(pbpctl_dev);
 		if (!pbpctl_dev_c)
@@ -924,6 +928,7 @@ static int read_reg(struct bpctl_dev *pbpctl_dev, unsigned char addr)
 
 #ifdef BP_SYNC_FLAG
 	unsigned long flags;
+
 	spin_lock_irqsave(&pbpctl_dev->bypass_wr_lock, flags);
 #else
 	atomic_set(&pbpctl_dev->wdt_busy, 1);
@@ -1559,7 +1564,8 @@ int pulse_set_fn(struct bpctl_dev *pbpctl_dev, unsigned int counter)
 
 int zero_set_fn(struct bpctl_dev *pbpctl_dev)
 {
-	uint32_t ctrl_ext = 0, ctrl_value = 0;
+	uint32_t ctrl_ext = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -1579,12 +1585,13 @@ int zero_set_fn(struct bpctl_dev *pbpctl_dev)
 							   BPCTLI_CTRL_EXT_MDIO_DATA)));
 
 	}
-	return ctrl_value;
+	return 0;
 }
 
 int pulse_get2_fn(struct bpctl_dev *pbpctl_dev)
 {
 	uint32_t ctrl_ext = 0, ctrl_value = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -1600,6 +1607,7 @@ int pulse_get2_fn(struct bpctl_dev *pbpctl_dev)
 int pulse_get1_fn(struct bpctl_dev *pbpctl_dev)
 {
 	uint32_t ctrl_ext = 0, ctrl_value = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -1663,6 +1671,7 @@ static struct bpctl_dev *lookup_port(struct bpctl_dev *dev)
 {
 	struct bpctl_dev *p;
 	int n;
+
 	for (n = 0, p = bpctl_dev_arr; n < device_num && p->pdev; n++) {
 		if (p->bus == dev->bus
 		    && p->slot == dev->slot
@@ -1840,6 +1849,7 @@ static int bypass_off(struct bpctl_dev *pbpctl_dev)
 static int tap_off(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = BP_NOT_CAP;
+
 	if ((pbpctl_dev->bp_caps & TAP_CAP)
 	    && (pbpctl_dev->bp_ext_ver >= PXG2TBPI_VER)) {
 		write_data(pbpctl_dev, TAP_OFF);
@@ -1853,6 +1863,7 @@ static int tap_off(struct bpctl_dev *pbpctl_dev)
 static int tap_on(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = BP_NOT_CAP;
+
 	if ((pbpctl_dev->bp_caps & TAP_CAP)
 	    && (pbpctl_dev->bp_ext_ver >= PXG2TBPI_VER)) {
 		write_data(pbpctl_dev, TAP_ON);
@@ -1866,6 +1877,7 @@ static int tap_on(struct bpctl_dev *pbpctl_dev)
 static int disc_off(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = 0;
+
 	if ((pbpctl_dev->bp_caps & DISC_CAP) && (pbpctl_dev->bp_ext_ver >= 0x8)) {
 		write_data(pbpctl_dev, DISC_OFF);
 		msec_delay_bp(LATCH_DELAY);
@@ -1878,6 +1890,7 @@ static int disc_off(struct bpctl_dev *pbpctl_dev)
 static int disc_on(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = 0;
+
 	if ((pbpctl_dev->bp_caps & DISC_CAP) && (pbpctl_dev->bp_ext_ver >= 0x8)) {
 		write_data(pbpctl_dev, /*DISC_ON */ 0x85);
 		msec_delay_bp(LATCH_DELAY);
@@ -2267,6 +2280,7 @@ static int set_tx(struct bpctl_dev *pbpctl_dev, int tx_state)
 {
 	int ret = 0, ctrl = 0;
 	struct bpctl_dev *pbpctl_dev_m;
+
 	if ((is_bypass_fn(pbpctl_dev)) == 1)
 		pbpctl_dev_m = pbpctl_dev;
 	else
@@ -2799,6 +2813,7 @@ int wdt_time_left(struct bpctl_dev *pbpctl_dev)
 static int wdt_timer(struct bpctl_dev *pbpctl_dev, int *time_left)
 {
 	int ret = 0;
+
 	if (pbpctl_dev->bp_caps & WD_CTL_CAP) {
 		{
 			if (pbpctl_dev->wdt_status == WDT_STATUS_UNKNOWN)
@@ -3011,6 +3026,7 @@ static int tx_status(struct bpctl_dev *pbpctl_dev)
 {
 	uint32_t ctrl = 0;
 	struct bpctl_dev *pbpctl_dev_m;
+
 	if ((is_bypass_fn(pbpctl_dev)) == 1)
 		pbpctl_dev_m = pbpctl_dev;
 	else
@@ -3192,6 +3208,7 @@ static int bypass_change_status(struct bpctl_dev *pbpctl_dev)
 static int bypass_status(struct bpctl_dev *pbpctl_dev)
 {
 	u32 ctrl_ext = 0;
+
 	if (pbpctl_dev->bp_caps & BP_CAP) {
 
 		struct bpctl_dev *pbpctl_dev_b = NULL;
@@ -3320,6 +3337,7 @@ static int dis_bypass_cap_status(struct bpctl_dev *pbpctl_dev)
 static int wdt_programmed(struct bpctl_dev *pbpctl_dev, int *timeout)
 {
 	int ret = 0;
+
 	if (pbpctl_dev->bp_caps & WD_CTL_CAP) {
 		if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER) {
 			if ((read_reg(pbpctl_dev, STATUS_REG_ADDR)) &
@@ -3383,6 +3401,7 @@ static int tap_flag_status(struct bpctl_dev *pbpctl_dev)
 static int tap_flag_status_clear(struct bpctl_dev *pbpctl_dev)
 {
 	uint32_t status_reg = 0;
+
 	if (pbpctl_dev->bp_caps & TAP_STATUS_CAP) {
 		if (pbpctl_dev->bp_ext_ver >= PXG2TBPI_VER) {
 			status_reg = read_reg(pbpctl_dev, STATUS_TAP_REG_ADDR);
@@ -3397,6 +3416,7 @@ static int tap_flag_status_clear(struct bpctl_dev *pbpctl_dev)
 static int tap_change_status(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = BP_NOT_CAP;
+
 	if (pbpctl_dev->bp_ext_ver >= PXG2TBPI_VER) {
 		if (pbpctl_dev->bp_caps & TAP_CAP) {
 			if (pbpctl_dev->bp_caps & BP_CAP) {
@@ -3487,6 +3507,7 @@ static int disc_flag_status(struct bpctl_dev *pbpctl_dev)
 static int disc_flag_status_clear(struct bpctl_dev *pbpctl_dev)
 {
 	uint32_t status_reg = 0;
+
 	if (pbpctl_dev->bp_caps & DISC_CAP) {
 		if (pbpctl_dev->bp_ext_ver >= 0x8) {
 			status_reg = read_reg(pbpctl_dev, STATUS_DISC_REG_ADDR);
@@ -3501,6 +3522,7 @@ static int disc_flag_status_clear(struct bpctl_dev *pbpctl_dev)
 static int disc_change_status(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = BP_NOT_CAP;
+
 	if (pbpctl_dev->bp_caps & DISC_CAP) {
 		ret = disc_flag_status(pbpctl_dev);
 		disc_flag_status_clear(pbpctl_dev);
@@ -3596,6 +3618,7 @@ static int disc_off_status(struct bpctl_dev *pbpctl_dev)
 static int disc_status(struct bpctl_dev *pbpctl_dev)
 {
 	int ctrl = 0;
+
 	if (pbpctl_dev->bp_caps & DISC_CAP) {
 		ctrl = disc_off_status(pbpctl_dev);
 		if (ctrl < 0)
@@ -3741,6 +3764,7 @@ static void bypass_caps_init(struct bpctl_dev *pbpctl_dev)
 
 #ifdef BYPASS_DEBUG
 	int ret = 0;
+
 	if (!(INTEL_IF_SERIES(adapter->bp_device_block.subdevice))) {
 		ret = read_reg(pbpctl_dev, VER_REG_ADDR);
 		printk("VER_REG reg1=%x\n", ret);
@@ -4152,6 +4176,7 @@ static int get_bypass_change_fn(struct bpctl_dev *pbpctl_dev)
 static int set_dis_bypass_fn(struct bpctl_dev *pbpctl_dev, int dis_param)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4179,6 +4204,7 @@ static int get_dis_bypass_fn(struct bpctl_dev *pbpctl_dev)
 static int set_bypass_pwoff_fn(struct bpctl_dev *pbpctl_dev, int bypass_mode)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4206,6 +4232,7 @@ static int get_bypass_pwoff_fn(struct bpctl_dev *pbpctl_dev)
 static int set_bypass_pwup_fn(struct bpctl_dev *pbpctl_dev, int bypass_mode)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4233,6 +4260,7 @@ static int get_bypass_pwup_fn(struct bpctl_dev *pbpctl_dev)
 static int set_bypass_wd_fn(struct bpctl_dev *pbpctl_dev, int timeout)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4279,8 +4307,8 @@ static int reset_bypass_wd_timer_fn(struct bpctl_dev *pbpctl_dev)
 static int get_wd_set_caps_fn(struct bpctl_dev *pbpctl_dev)
 {
 	int bp_status = 0;
-
 	unsigned int step_value = TIMEOUT_MAX_STEP + 1, bit_cnt = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4303,6 +4331,7 @@ static int get_wd_set_caps_fn(struct bpctl_dev *pbpctl_dev)
 static int set_std_nic_fn(struct bpctl_dev *pbpctl_dev, int nic_mode)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4355,6 +4384,7 @@ static int get_tap_fn(struct bpctl_dev *pbpctl_dev)
 static int set_tap_pwup_fn(struct bpctl_dev *pbpctl_dev, int tap_mode)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4373,6 +4403,7 @@ static int set_tap_pwup_fn(struct bpctl_dev *pbpctl_dev, int tap_mode)
 static int get_tap_pwup_fn(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4393,6 +4424,7 @@ static int get_tap_change_fn(struct bpctl_dev *pbpctl_dev)
 static int set_dis_tap_fn(struct bpctl_dev *pbpctl_dev, int dis_param)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4435,6 +4467,7 @@ static int set_disc_fn(struct bpctl_dev *pbpctl_dev, int disc_mode)
 static int get_disc_fn(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4446,6 +4479,7 @@ static int get_disc_fn(struct bpctl_dev *pbpctl_dev)
 static int set_disc_pwup_fn(struct bpctl_dev *pbpctl_dev, int disc_mode)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4464,6 +4498,7 @@ static int set_disc_pwup_fn(struct bpctl_dev *pbpctl_dev, int disc_mode)
 static int get_disc_pwup_fn(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4474,6 +4509,7 @@ static int get_disc_pwup_fn(struct bpctl_dev *pbpctl_dev)
 static int get_disc_change_fn(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4484,6 +4520,7 @@ static int get_disc_change_fn(struct bpctl_dev *pbpctl_dev)
 static int set_dis_disc_fn(struct bpctl_dev *pbpctl_dev, int dis_param)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4502,6 +4539,7 @@ static int set_dis_disc_fn(struct bpctl_dev *pbpctl_dev, int dis_param)
 static int get_dis_disc_fn(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4528,8 +4566,8 @@ static int set_wd_exp_mode_fn(struct bpctl_dev *pbpctl_dev, int param)
 
 static int set_tx_fn(struct bpctl_dev *pbpctl_dev, int tx_state)
 {
-
 	struct bpctl_dev *pbpctl_dev_b = NULL;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4607,6 +4645,7 @@ static int get_bypass_slave_fn(struct bpctl_dev *pbpctl_dev,
 			       struct bpctl_dev **pbpctl_dev_out)
 {
 	int idx_dev = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4650,6 +4689,7 @@ static int is_bypass(struct bpctl_dev *pbpctl_dev)
 static int get_tx_fn(struct bpctl_dev *pbpctl_dev)
 {
 	struct bpctl_dev *pbpctl_dev_b = NULL;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4732,10 +4772,9 @@ static void bp_tpl_timer_fn(unsigned long param)
 
 static void remove_bypass_tpl_auto(struct bpctl_dev *pbpctl_dev)
 {
-	struct bpctl_dev *pbpctl_dev_b = NULL;
+	struct bpctl_dev *pbpctl_dev_b;
 	if (!pbpctl_dev)
 		return;
-	pbpctl_dev_b = get_status_port_fn(pbpctl_dev);
 
 	if (pbpctl_dev->bp_caps & TPL_CAP) {
 		del_timer_sync(&pbpctl_dev->bp_tpl_timer);
@@ -4782,12 +4821,10 @@ static int set_bypass_tpl_auto(struct bpctl_dev *pbpctl_dev, unsigned int param)
 static int set_tpl_fn(struct bpctl_dev *pbpctl_dev, int tpl_mode)
 {
 
-	struct bpctl_dev *pbpctl_dev_b = NULL;
+	struct bpctl_dev *pbpctl_dev_b;
 	if (!pbpctl_dev)
 		return -1;
 
-	pbpctl_dev_b = get_status_port_fn(pbpctl_dev);
-
 	if (pbpctl_dev->bp_caps & TPL_CAP) {
 		if (tpl_mode) {
 			pbpctl_dev_b = get_status_port_fn(pbpctl_dev);
@@ -4812,6 +4849,7 @@ static int set_tpl_fn(struct bpctl_dev *pbpctl_dev, int tpl_mode)
 static int get_tpl_fn(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = BP_NOT_CAP;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4846,6 +4884,7 @@ static int set_bp_wait_at_pwup_fn(struct bpctl_dev *pbpctl_dev, int tap_mode)
 static int get_bp_wait_at_pwup_fn(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4879,6 +4918,7 @@ static int set_bp_hw_reset_fn(struct bpctl_dev *pbpctl_dev, int tap_mode)
 static int get_bp_hw_reset_fn(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4906,6 +4946,7 @@ static int get_bypass_info_fn(struct bpctl_dev *pbpctl_dev, char *dev_name,
 static int get_dev_idx_bsf(int bus, int slot, int func)
 {
 	int idx_dev = 0;
+
 	for (idx_dev = 0;
 	     ((bpctl_dev_arr[idx_dev].pdev != NULL) && (idx_dev < device_num));
 	     idx_dev++) {
@@ -6368,56 +6409,30 @@ static int __init bypass_init_module(void)
 
 	sema_init(&bpctl_sema, 1);
 	spin_lock_init(&bpvm_lock);
-	{
-
-		struct bpctl_dev *pbpctl_dev_c = NULL;
-		for (idx_dev = 0, dev = bpctl_dev_arr;
-		     idx_dev < device_num && dev->pdev;
-		     idx_dev++, dev++) {
-			if (dev->bp_10g9) {
-				pbpctl_dev_c = get_status_port_fn(dev);
-				if (is_bypass_fn(dev)) {
-					printk(KERN_INFO "%s found, ",
-					       dev->name);
-					dev->bp_fw_ver = bypass_fw_ver(dev);
-					printk("firmware version: 0x%x\n",
-					       dev->bp_fw_ver);
-				}
-				dev->wdt_status = WDT_STATUS_UNKNOWN;
-				dev->reset_time = 0;
-				atomic_set(&dev->wdt_busy, 0);
-				dev->bp_status_un = 1;
-
-				bypass_caps_init(dev);
-
-				init_bypass_wd_auto(dev);
-				init_bypass_tpl_auto(dev);
 
+	for (idx_dev = 0, dev = bpctl_dev_arr;
+	     idx_dev < device_num && dev->pdev;
+	     idx_dev++, dev++) {
+		if (dev->bp_10g9) {
+			if (is_bypass_fn(dev)) {
+				printk(KERN_INFO "%s found, ", dev->name);
+				dev->bp_fw_ver = bypass_fw_ver(dev);
+				printk("firmware version: 0x%x\n",
+				       dev->bp_fw_ver);
 			}
+			dev->wdt_status = WDT_STATUS_UNKNOWN;
+			dev->reset_time = 0;
+			atomic_set(&dev->wdt_busy, 0);
+			dev->bp_status_un = 1;
 
-		}
-	}
-
-	register_netdevice_notifier(&bp_notifier_block);
-#ifdef BP_PROC_SUPPORT
-	{
-		int i = 0;
-		/* unsigned long flags; */
-		/* rcu_read_lock(); */
-		bp_proc_create();
-		for (i = 0; i < device_num; i++) {
-			if (bpctl_dev_arr[i].ifindex) {
-				/* spin_lock_irqsave(&bpvm_lock, flags); */
-				bypass_proc_remove_dev_sd(&bpctl_dev_arr[i]);
-				bypass_proc_create_dev_sd(&bpctl_dev_arr[i]);
-				/* spin_unlock_irqrestore(&bpvm_lock, flags); */
-			}
+			bypass_caps_init(dev);
 
+			init_bypass_wd_auto(dev);
+			init_bypass_tpl_auto(dev);
 		}
-		/* rcu_read_unlock(); */
 	}
-#endif
 
+	register_netdevice_notifier(&bp_notifier_block);
 	return 0;
 }
 
@@ -6427,17 +6442,11 @@ static int __init bypass_init_module(void)
 static void __exit bypass_cleanup_module(void)
 {
 	int i;
+
 	unregister_netdevice_notifier(&bp_notifier_block);
 
 	for (i = 0; i < device_num; i++) {
 		/* unsigned long flags; */
-#ifdef BP_PROC_SUPPORT
-/*	spin_lock_irqsave(&bpvm_lock, flags);
-	rcu_read_lock(); */
-		bypass_proc_remove_dev_sd(&bpctl_dev_arr[i]);
-/*	spin_unlock_irqrestore(&bpvm_lock, flags);
-	rcu_read_unlock(); */
-#endif
 		remove_bypass_wd_auto(&bpctl_dev_arr[i]);
 		bpctl_dev_arr[i].reset_time = 0;
 
@@ -6738,6 +6747,7 @@ int get_bypass_slave_sd(int ifindex)
 {
 	struct bpctl_dev *pbpctl_dev_out;
 	int ret = get_bypass_slave_fn(get_dev_idx_p(ifindex), &pbpctl_dev_out);
+
 	if (ret == 1)
 		return pbpctl_dev_out->ifindex;
 	return -1;
@@ -6783,22 +6793,11 @@ EXPORT_SYMBOL(bp_if_scan_sd);
 
 static struct proc_dir_entry *bp_procfs_dir;
 
-static int bp_proc_create(void)
-{
-	bp_procfs_dir = proc_mkdir(BP_PROC_DIR, init_net.proc_net);
-	if (bp_procfs_dir == (struct proc_dir_entry *)0) {
-		printk(KERN_DEBUG
-		       "Could not create procfs nicinfo directory %s\n",
-		       BP_PROC_DIR);
-		return -1;
-	}
-	return 0;
-}
-
 static int procfs_add(char *proc_name, const struct file_operations *fops,
 		      struct bpctl_dev *dev)
 {
 	struct bypass_pfs_sd *pfs = &dev->bypass_pfs_set;
+
 	if (!proc_create_data(proc_name, 0644, pfs->bypass_entry, fops, dev))
 		return -1;
 	return 0;
@@ -6843,6 +6842,7 @@ static int show_bypass_slave(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	struct bpctl_dev *slave = get_status_port_fn(dev);
+
 	if (!slave)
 		slave = dev;
 	if (!slave)
@@ -6857,6 +6857,7 @@ static int show_bypass_caps(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_bypass_caps_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "-1\n");
 	else
@@ -6869,6 +6870,7 @@ static int show_wd_set_caps(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_wd_set_caps_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "-1\n");
 	else
@@ -6905,6 +6907,7 @@ static ssize_t bypass_write(struct file *file, const char __user *buffer,
 				  size_t count, loff_t *pos)
 {
 	int bypass_param = user_on_off(buffer, count);
+
 	if (bypass_param < 0)
 		return -1;
 
@@ -6915,6 +6918,7 @@ static int show_bypass(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_bypass_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 1)
@@ -6929,6 +6933,7 @@ static ssize_t tap_write(struct file *file, const char __user *buffer,
 				  size_t count, loff_t *pos)
 {
 	int tap_param = user_on_off(buffer, count);
+
 	if (tap_param < 0)
 		return -1;
 
@@ -6939,6 +6944,7 @@ static int show_tap(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_tap_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 1)
@@ -6953,6 +6959,7 @@ static ssize_t disc_write(struct file *file, const char __user *buffer,
 				  size_t count, loff_t *pos)
 {
 	int tap_param = user_on_off(buffer, count);
+
 	if (tap_param < 0)
 		return -1;
 
@@ -6963,6 +6970,7 @@ static int show_disc(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_disc_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 1)
@@ -6977,6 +6985,7 @@ static int show_bypass_change(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_bypass_change_fn(dev);
+
 	if (ret == 1)
 		seq_puts(m, "on\n");
 	else if (ret == 0)
@@ -6991,6 +7000,7 @@ static int show_tap_change(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_tap_change_fn(dev);
+
 	if (ret == 1)
 		seq_puts(m, "on\n");
 	else if (ret == 0)
@@ -7005,6 +7015,7 @@ static int show_disc_change(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_disc_change_fn(dev);
+
 	if (ret == 1)
 		seq_puts(m, "on\n");
 	else if (ret == 0)
@@ -7021,6 +7032,7 @@ static ssize_t bypass_wd_write(struct file *file, const char __user *buffer,
 	struct bpctl_dev *dev = PDE_DATA(file_inode(file));
 	int timeout;
 	int ret = kstrtoint_from_user(buffer, count, 10, &timeout);
+
 	if (ret)
 		return ret;
 	set_bypass_wd_fn(dev, timeout);
@@ -7048,6 +7060,7 @@ static int show_wd_expire_time(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = 0, timeout = 0;
+
 	ret = get_wd_expire_time_fn(dev, &timeout);
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
@@ -7066,6 +7079,7 @@ static ssize_t tpl_write(struct file *file, const char __user *buffer,
 {
 	struct bpctl_dev *dev = PDE_DATA(file_inode(file));
 	int tpl_param = user_on_off(buffer, count);
+
 	if (tpl_param < 0)
 		return -1;
 
@@ -7076,6 +7090,7 @@ static int show_tpl(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_tpl_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 1)
@@ -7092,6 +7107,7 @@ static ssize_t wait_at_pwup_write(struct file *file, const char __user *buffer,
 {
 	struct bpctl_dev *dev = PDE_DATA(file_inode(file));
 	int tpl_param = user_on_off(buffer, count);
+
 	if (tpl_param < 0)
 		return -1;
 
@@ -7102,6 +7118,7 @@ static int show_wait_at_pwup(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_bp_wait_at_pwup_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 1)
@@ -7117,6 +7134,7 @@ static ssize_t hw_reset_write(struct file *file, const char __user *buffer,
 {
 	struct bpctl_dev *dev = PDE_DATA(file_inode(file));
 	int tpl_param = user_on_off(buffer, count);
+
 	if (tpl_param < 0)
 		return -1;
 
@@ -7127,6 +7145,7 @@ static int show_hw_reset(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_bp_hw_reset_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 1)
@@ -7143,6 +7162,7 @@ static int show_reset_bypass_wd(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = reset_bypass_wd_timer_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 0)
@@ -7157,6 +7177,7 @@ static ssize_t dis_bypass_write(struct file *file, const char __user *buffer,
 				  size_t count, loff_t *pos)
 {
 	int bypass_param = user_on_off(buffer, count);
+
 	if (bypass_param < 0)
 		return -EINVAL;
 
@@ -7167,6 +7188,7 @@ static int show_dis_bypass(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_dis_bypass_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 0)
@@ -7181,6 +7203,7 @@ static ssize_t dis_tap_write(struct file *file, const char __user *buffer,
 				  size_t count, loff_t *pos)
 {
 	int tap_param = user_on_off(buffer, count);
+
 	if (tap_param < 0)
 		return -EINVAL;
 
@@ -7191,6 +7214,7 @@ static int show_dis_tap(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_dis_tap_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 0)
@@ -7205,6 +7229,7 @@ static ssize_t dis_disc_write(struct file *file, const char __user *buffer,
 				  size_t count, loff_t *pos)
 {
 	int tap_param = user_on_off(buffer, count);
+
 	if (tap_param < 0)
 		return -EINVAL;
 
@@ -7215,6 +7240,7 @@ static int show_dis_disc(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_dis_disc_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 0)
@@ -7229,6 +7255,7 @@ static ssize_t bypass_pwup_write(struct file *file, const char __user *buffer,
 				  size_t count, loff_t *pos)
 {
 	int bypass_param = user_on_off(buffer, count);
+
 	if (bypass_param < 0)
 		return -EINVAL;
 
@@ -7239,6 +7266,7 @@ static int show_bypass_pwup(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_bypass_pwup_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 0)
@@ -7253,6 +7281,7 @@ static ssize_t bypass_pwoff_write(struct file *file, const char __user *buffer,
 				  size_t count, loff_t *pos)
 {
 	int bypass_param = user_on_off(buffer, count);
+
 	if (bypass_param < 0)
 		return -EINVAL;
 
@@ -7263,6 +7292,7 @@ static int show_bypass_pwoff(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_bypass_pwoff_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 0)
@@ -7277,6 +7307,7 @@ static ssize_t tap_pwup_write(struct file *file, const char __user *buffer,
 				  size_t count, loff_t *pos)
 {
 	int tap_param = user_on_off(buffer, count);
+
 	if (tap_param < 0)
 		return -EINVAL;
 
@@ -7287,6 +7318,7 @@ static int show_tap_pwup(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_tap_pwup_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 0)
@@ -7301,6 +7333,7 @@ static ssize_t disc_pwup_write(struct file *file, const char __user *buffer,
 				  size_t count, loff_t *pos)
 {
 	int tap_param = user_on_off(buffer, count);
+
 	if (tap_param < 0)
 		return -EINVAL;
 
@@ -7311,6 +7344,7 @@ static int show_disc_pwup(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_disc_pwup_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 0)
@@ -7325,6 +7359,7 @@ static ssize_t std_nic_write(struct file *file, const char __user *buffer,
 				  size_t count, loff_t *pos)
 {
 	int bypass_param = user_on_off(buffer, count);
+
 	if (bypass_param < 0)
 		return -EINVAL;
 
@@ -7335,6 +7370,7 @@ static int show_std_nic(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_std_nic_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 0)
@@ -7377,6 +7413,7 @@ static int show_wd_exp_mode(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_wd_exp_mode_fn(dev);
+
 	if (ret == 1)
 		seq_puts(m, "tap\n");
 	else if (ret == 0)
@@ -7394,6 +7431,7 @@ static ssize_t wd_autoreset_write(struct file *file, const char __user *buffer,
 {
 	int timeout;
 	int ret = kstrtoint_from_user(buffer, count, 10, &timeout);
+
 	if (ret)
 		return ret;
 	set_wd_autoreset_fn(PDE_DATA(file_inode(file)), timeout);
@@ -7403,6 +7441,7 @@ static int show_wd_autoreset(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_wd_autoreset_fn(dev);
+
 	if (ret >= 0)
 		seq_printf(m, "%d\n", ret);
 	else
@@ -7483,8 +7522,8 @@ static int bypass_proc_create_dev_sd(struct bpctl_dev *pbp_device_block)
 
 static int bypass_proc_remove_dev_sd(struct bpctl_dev *pbp_device_block)
 {
-
 	struct bypass_pfs_sd *current_pfs = &pbp_device_block->bypass_pfs_set;
+
 	remove_proc_subtree(current_pfs->dir_name, bp_procfs_dir);
 	current_pfs->bypass_entry = NULL;
 	return 0;
diff --git a/drivers/staging/silicom/bypasslib/bp_ioctl.h b/drivers/staging/silicom/bypasslib/bp_ioctl.h
index bf47f786866..a13932af733 100644
--- a/drivers/staging/silicom/bypasslib/bp_ioctl.h
+++ b/drivers/staging/silicom/bypasslib/bp_ioctl.h
@@ -101,7 +101,7 @@ enum {
 	SET_BP_WAIT_AT_PWUP,
 	GET_BP_HW_RESET,
 	SET_BP_HW_RESET,
-} CMND_TYPE;
+};
 
 enum {
 	IF_SCAN_SD,
@@ -154,7 +154,7 @@ enum {
 	GET_BP_HW_RESET_SD,
 	SET_BP_HW_RESET_SD,
 
-} CMND_TYPE_SD;
+};
 
 #define SIOCGIFBYPASS (SIOCDEVPRIVATE+10)
 
diff --git a/drivers/staging/silicom/bypasslib/bypass.c b/drivers/staging/silicom/bypasslib/bypass.c
index 09e00dac04f..8e714a8ea3a 100644
--- a/drivers/staging/silicom/bypasslib/bypass.c
+++ b/drivers/staging/silicom/bypasslib/bypass.c
@@ -135,6 +135,7 @@ static int doit(int cmd, int if_index, int *data)
 static int is_dev_sd(int if_index)
 {
 	int ret = 0;
+
 	SET_BPLIB_INT_FN(is_bypass, int, if_index, ret);
 	return ret >= 0 ? 1 : 0;
 }
@@ -149,38 +150,33 @@ static int is_bypass_dev(int if_index)
 
 	while ((pdev = pci_get_class(PCI_CLASS_NETWORK_ETHERNET << 8, pdev))) {
 		dev = pci_get_drvdata(pdev);
-		if (dev != NULL) {
-			dev = pci_get_drvdata(pdev);
-			if ((dev != NULL) && (dev->ifindex == if_index)) {
-				if ((pdev->vendor == SILICOM_VID) &&
-				    (pdev->device >= SILICOM_BP_PID_MIN) &&
-				    (pdev->device <= SILICOM_BP_PID_MAX)) {
-					goto send_cmd;
-				}
+		if ((dev != NULL) && (dev->ifindex == if_index)) {
+			if ((pdev->vendor == SILICOM_VID) &&
+			    (pdev->device >= SILICOM_BP_PID_MIN) &&
+			    (pdev->device <= SILICOM_BP_PID_MAX)) {
+				goto send_cmd;
+			}
 #if defined(BP_VENDOR_SUPPORT) && defined(ETHTOOL_GDRVINFO)
-				else {
-					struct ethtool_drvinfo info;
-					const struct ethtool_ops *ops =
-					    dev->ethtool_ops;
-					int k = 0;
-
-					if (ops->get_drvinfo) {
-						memset(&info, 0, sizeof(info));
-						info.cmd = ETHTOOL_GDRVINFO;
-						ops->get_drvinfo(dev, &info);
-						for (; bp_desc_array[k]; k++)
-							if (!
-							    (strcmp
-							     (bp_desc_array[k],
-							      info.driver)))
-								goto send_cmd;
-
-					}
+			else {
+				struct ethtool_drvinfo info;
+				const struct ethtool_ops *ops =
+					dev->ethtool_ops;
+				int k = 0;
+
+				if (ops->get_drvinfo) {
+					memset(&info, 0, sizeof(info));
+					info.cmd = ETHTOOL_GDRVINFO;
+					ops->get_drvinfo(dev, &info);
+					for (; bp_desc_array[k]; k++)
+						if (!(strcmp(bp_desc_array[k],
+							     info.driver)))
+							goto send_cmd;
 
 				}
-#endif
-				return -1;
+
 			}
+#endif
+			return -1;
 		}
 	}
  send_cmd:
@@ -191,6 +187,7 @@ static int is_bypass_dev(int if_index)
 static int is_bypass(int if_index)
 {
 	int ret = 0;
+
 	SET_BPLIB_INT_FN(is_bypass, int, if_index, ret);
 
 	if (ret < 0)
diff --git a/drivers/staging/skein/Kconfig b/drivers/staging/skein/Kconfig
new file mode 100644
index 00000000000..b9172bfcdc1
--- /dev/null
+++ b/drivers/staging/skein/Kconfig
@@ -0,0 +1,32 @@
+config CRYPTO_SKEIN
+	bool "Skein digest algorithm"
+	depends on (X86 || UML_X86) && 64BIT && CRYPTO
+	select CRYPTO_THREEFISH
+	select CRYPTO_HASH
+	help
+	  Skein secure hash algorithm is one of 5 finalists from the NIST SHA3
+	  competition.
+
+	  Skein is optimized for modern, 64bit processors and is highly
+	  customizable.  See:
+
+	  http://www.skein-hash.info/sites/default/files/skein1.3.pdf
+
+	  for more information.  This module depends on the threefish block
+	  cipher module.
+
+config CRYPTO_THREEFISH
+	bool "Threefish tweakable block cipher"
+	depends on (X86 || UML_X86) && 64BIT && CRYPTO
+	select CRYPTO_ALGAPI
+	help
+	  Threefish cipher algorithm is the tweakable block cipher underneath
+	  the Skein family of secure hash algorithms.  Skein is one of 5
+	  finalists from the NIST SHA3 competition.
+
+	  Skein is optimized for modern, 64bit processors and is highly
+	  customizable.  See:
+
+	  http://www.skein-hash.info/sites/default/files/skein1.3.pdf
+
+	  for more information.
diff --git a/drivers/staging/skein/Makefile b/drivers/staging/skein/Makefile
new file mode 100644
index 00000000000..a14aaddd829
--- /dev/null
+++ b/drivers/staging/skein/Makefile
@@ -0,0 +1,9 @@
+#
+# Makefile for the skein secure hash algorithm
+#
+obj-$(CONFIG_CRYPTO_SKEIN) +=   skein.o \
+				skein_api.o \
+				skein_block.o
+
+obj-$(CONFIG_CRYPTO_THREEFISH) += threefish_block.o \
+				  threefish_api.o
diff --git a/drivers/staging/skein/TODO b/drivers/staging/skein/TODO
new file mode 100644
index 00000000000..cd3508dd908
--- /dev/null
+++ b/drivers/staging/skein/TODO
@@ -0,0 +1,8 @@
+skein/threefish TODO
+
+ - move macros into appropriate header files
+ - add / pass test vectors
+ - module support
+
+Please send patches to Jason Cooper <jason@lakedaemon.net> in addition to the
+staging tree mailinglist.
diff --git a/drivers/staging/skein/skein.c b/drivers/staging/skein/skein.c
new file mode 100644
index 00000000000..8cc83587b1f
--- /dev/null
+++ b/drivers/staging/skein/skein.c
@@ -0,0 +1,883 @@
+/***********************************************************************
+**
+** Implementation of the Skein hash function.
+**
+** Source code author: Doug Whiting, 2008.
+**
+** This algorithm and source code is released to the public domain.
+**
+************************************************************************/
+
+#define  SKEIN_PORT_CODE /* instantiate any code in skein_port.h */
+
+#include <linux/string.h>       /* get the memcpy/memset functions */
+#include "skein.h" /* get the Skein API definitions   */
+#include "skein_iv.h"    /* get precomputed IVs */
+#include "skein_block.h"
+
+/*****************************************************************/
+/*     256-bit Skein                                             */
+/*****************************************************************/
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* init the context for a straight hashing operation  */
+int skein_256_init(struct skein_256_ctx *ctx, size_t hash_bit_len)
+{
+	union {
+		u8 b[SKEIN_256_STATE_BYTES];
+		u64 w[SKEIN_256_STATE_WORDS];
+	} cfg;                              /* config block */
+
+	skein_assert_ret(hash_bit_len > 0, SKEIN_BAD_HASHLEN);
+	ctx->h.hash_bit_len = hash_bit_len;         /* output hash bit count */
+
+	switch (hash_bit_len) { /* use pre-computed values, where available */
+	case  256:
+		memcpy(ctx->x, SKEIN_256_IV_256, sizeof(ctx->x));
+		break;
+	case  224:
+		memcpy(ctx->x, SKEIN_256_IV_224, sizeof(ctx->x));
+		break;
+	case  160:
+		memcpy(ctx->x, SKEIN_256_IV_160, sizeof(ctx->x));
+		break;
+	case  128:
+		memcpy(ctx->x, SKEIN_256_IV_128, sizeof(ctx->x));
+		break;
+	default:
+		/* here if there is no precomputed IV value available */
+		/*
+		 * build/process the config block, type == CONFIG (could be
+		 * precomputed)
+		 */
+		/* set tweaks: T0=0; T1=CFG | FINAL */
+		skein_start_new_type(ctx, CFG_FINAL);
+
+		/* set the schema, version */
+		cfg.w[0] = skein_swap64(SKEIN_SCHEMA_VER);
+		/* hash result length in bits */
+		cfg.w[1] = skein_swap64(hash_bit_len);
+		cfg.w[2] = skein_swap64(SKEIN_CFG_TREE_INFO_SEQUENTIAL);
+		/* zero pad config block */
+		memset(&cfg.w[3], 0, sizeof(cfg) - 3*sizeof(cfg.w[0]));
+
+		/* compute the initial chaining values from config block */
+		/* zero the chaining variables */
+		memset(ctx->x, 0, sizeof(ctx->x));
+		skein_256_process_block(ctx, cfg.b, 1, SKEIN_CFG_STR_LEN);
+		break;
+	}
+	/* The chaining vars ctx->x are now initialized for hash_bit_len. */
+	/* Set up to process the data message portion of the hash (default) */
+	skein_start_new_type(ctx, MSG);              /* T0=0, T1= MSG type */
+
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* init the context for a MAC and/or tree hash operation */
+/* [identical to skein_256_init() when key_bytes == 0 && \
+ *	tree_info == SKEIN_CFG_TREE_INFO_SEQUENTIAL] */
+int skein_256_init_ext(struct skein_256_ctx *ctx, size_t hash_bit_len,
+		       u64 tree_info, const u8 *key, size_t key_bytes)
+{
+	union {
+		u8  b[SKEIN_256_STATE_BYTES];
+		u64 w[SKEIN_256_STATE_WORDS];
+	} cfg; /* config block */
+
+	skein_assert_ret(hash_bit_len > 0, SKEIN_BAD_HASHLEN);
+	skein_assert_ret(key_bytes == 0 || key != NULL, SKEIN_FAIL);
+
+	/* compute the initial chaining values ctx->x[], based on key */
+	if (key_bytes == 0) { /* is there a key? */
+		/* no key: use all zeroes as key for config block */
+		memset(ctx->x, 0, sizeof(ctx->x));
+	} else { /* here to pre-process a key */
+		skein_assert(sizeof(cfg.b) >= sizeof(ctx->x));
+		/* do a mini-Init right here */
+		/* set output hash bit count = state size */
+		ctx->h.hash_bit_len = 8*sizeof(ctx->x);
+		/* set tweaks: T0 = 0; T1 = KEY type */
+		skein_start_new_type(ctx, KEY);
+		/* zero the initial chaining variables */
+		memset(ctx->x, 0, sizeof(ctx->x));
+		/* hash the key */
+		skein_256_update(ctx, key, key_bytes);
+		/* put result into cfg.b[] */
+		skein_256_final_pad(ctx, cfg.b);
+		/* copy over into ctx->x[] */
+		memcpy(ctx->x, cfg.b, sizeof(cfg.b));
+	}
+	/*
+	 * build/process the config block, type == CONFIG (could be
+	 * precomputed for each key)
+	 */
+	/* output hash bit count */
+	ctx->h.hash_bit_len = hash_bit_len;
+	skein_start_new_type(ctx, CFG_FINAL);
+
+	/* pre-pad cfg.w[] with zeroes */
+	memset(&cfg.w, 0, sizeof(cfg.w));
+	cfg.w[0] = skein_swap64(SKEIN_SCHEMA_VER);
+	/* hash result length in bits */
+	cfg.w[1] = skein_swap64(hash_bit_len);
+	/* tree hash config info (or SKEIN_CFG_TREE_INFO_SEQUENTIAL) */
+	cfg.w[2] = skein_swap64(tree_info);
+
+	skein_show_key(256, &ctx->h, key, key_bytes);
+
+	/* compute the initial chaining values from config block */
+	skein_256_process_block(ctx, cfg.b, 1, SKEIN_CFG_STR_LEN);
+
+	/* The chaining vars ctx->x are now initialized */
+	/* Set up to process the data message portion of the hash (default) */
+	skein_start_new_type(ctx, MSG);
+
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* process the input bytes */
+int skein_256_update(struct skein_256_ctx *ctx, const u8 *msg,
+		     size_t msg_byte_cnt)
+{
+	size_t n;
+
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_256_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* process full blocks, if any */
+	if (msg_byte_cnt + ctx->h.b_cnt > SKEIN_256_BLOCK_BYTES) {
+		/* finish up any buffered message data */
+		if (ctx->h.b_cnt) {
+			/* # bytes free in buffer b[] */
+			n = SKEIN_256_BLOCK_BYTES - ctx->h.b_cnt;
+			if (n) {
+				/* check on our logic here */
+				skein_assert(n < msg_byte_cnt);
+				memcpy(&ctx->b[ctx->h.b_cnt], msg, n);
+				msg_byte_cnt  -= n;
+				msg         += n;
+				ctx->h.b_cnt += n;
+			}
+			skein_assert(ctx->h.b_cnt == SKEIN_256_BLOCK_BYTES);
+			skein_256_process_block(ctx, ctx->b, 1,
+						SKEIN_256_BLOCK_BYTES);
+			ctx->h.b_cnt = 0;
+		}
+		/*
+		 * now process any remaining full blocks, directly from input
+		 * message data
+		 */
+		if (msg_byte_cnt > SKEIN_256_BLOCK_BYTES) {
+			/* number of full blocks to process */
+			n = (msg_byte_cnt-1) / SKEIN_256_BLOCK_BYTES;
+			skein_256_process_block(ctx, msg, n,
+						SKEIN_256_BLOCK_BYTES);
+			msg_byte_cnt -= n * SKEIN_256_BLOCK_BYTES;
+			msg        += n * SKEIN_256_BLOCK_BYTES;
+		}
+		skein_assert(ctx->h.b_cnt == 0);
+	}
+
+	/* copy any remaining source message data bytes into b[] */
+	if (msg_byte_cnt) {
+		skein_assert(msg_byte_cnt + ctx->h.b_cnt <=
+			     SKEIN_256_BLOCK_BYTES);
+		memcpy(&ctx->b[ctx->h.b_cnt], msg, msg_byte_cnt);
+		ctx->h.b_cnt += msg_byte_cnt;
+	}
+
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* finalize the hash computation and output the result */
+int skein_256_final(struct skein_256_ctx *ctx, u8 *hash_val)
+{
+	size_t i, n, byte_cnt;
+	u64 x[SKEIN_256_STATE_WORDS];
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_256_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* tag as the final block */
+	ctx->h.tweak[1] |= SKEIN_T1_FLAG_FINAL;
+	/* zero pad b[] if necessary */
+	if (ctx->h.b_cnt < SKEIN_256_BLOCK_BYTES)
+		memset(&ctx->b[ctx->h.b_cnt], 0,
+			SKEIN_256_BLOCK_BYTES - ctx->h.b_cnt);
+
+	/* process the final block */
+	skein_256_process_block(ctx, ctx->b, 1, ctx->h.b_cnt);
+
+	/* now output the result */
+	/* total number of output bytes */
+	byte_cnt = (ctx->h.hash_bit_len + 7) >> 3;
+
+	/* run Threefish in "counter mode" to generate output */
+	/* zero out b[], so it can hold the counter */
+	memset(ctx->b, 0, sizeof(ctx->b));
+	/* keep a local copy of counter mode "key" */
+	memcpy(x, ctx->x, sizeof(x));
+	for (i = 0; i*SKEIN_256_BLOCK_BYTES < byte_cnt; i++) {
+		/* build the counter block */
+		((u64 *)ctx->b)[0] = skein_swap64((u64) i);
+		skein_start_new_type(ctx, OUT_FINAL);
+		/* run "counter mode" */
+		skein_256_process_block(ctx, ctx->b, 1, sizeof(u64));
+		/* number of output bytes left to go */
+		n = byte_cnt - i*SKEIN_256_BLOCK_BYTES;
+		if (n >= SKEIN_256_BLOCK_BYTES)
+			n  = SKEIN_256_BLOCK_BYTES;
+		/* "output" the ctr mode bytes */
+		skein_put64_lsb_first(hash_val+i*SKEIN_256_BLOCK_BYTES, ctx->x,
+				      n);
+		skein_show_final(256, &ctx->h, n,
+				 hash_val+i*SKEIN_256_BLOCK_BYTES);
+		/* restore the counter mode key for next time */
+		memcpy(ctx->x, x, sizeof(x));
+	}
+	return SKEIN_SUCCESS;
+}
+
+/*****************************************************************/
+/*     512-bit Skein                                             */
+/*****************************************************************/
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* init the context for a straight hashing operation  */
+int skein_512_init(struct skein_512_ctx *ctx, size_t hash_bit_len)
+{
+	union {
+		u8 b[SKEIN_512_STATE_BYTES];
+		u64 w[SKEIN_512_STATE_WORDS];
+	} cfg;                              /* config block */
+
+	skein_assert_ret(hash_bit_len > 0, SKEIN_BAD_HASHLEN);
+	ctx->h.hash_bit_len = hash_bit_len;         /* output hash bit count */
+
+	switch (hash_bit_len) { /* use pre-computed values, where available */
+	case  512:
+		memcpy(ctx->x, SKEIN_512_IV_512, sizeof(ctx->x));
+		break;
+	case  384:
+		memcpy(ctx->x, SKEIN_512_IV_384, sizeof(ctx->x));
+		break;
+	case  256:
+		memcpy(ctx->x, SKEIN_512_IV_256, sizeof(ctx->x));
+		break;
+	case  224:
+		memcpy(ctx->x, SKEIN_512_IV_224, sizeof(ctx->x));
+		break;
+	default:
+		/* here if there is no precomputed IV value available */
+		/*
+		 * build/process the config block, type == CONFIG (could be
+		 * precomputed)
+		 */
+		/* set tweaks: T0=0; T1=CFG | FINAL */
+		skein_start_new_type(ctx, CFG_FINAL);
+
+		/* set the schema, version */
+		cfg.w[0] = skein_swap64(SKEIN_SCHEMA_VER);
+		/* hash result length in bits */
+		cfg.w[1] = skein_swap64(hash_bit_len);
+		cfg.w[2] = skein_swap64(SKEIN_CFG_TREE_INFO_SEQUENTIAL);
+		/* zero pad config block */
+		memset(&cfg.w[3], 0, sizeof(cfg) - 3*sizeof(cfg.w[0]));
+
+		/* compute the initial chaining values from config block */
+		/* zero the chaining variables */
+		memset(ctx->x, 0, sizeof(ctx->x));
+		skein_512_process_block(ctx, cfg.b, 1, SKEIN_CFG_STR_LEN);
+		break;
+	}
+
+	/*
+	 * The chaining vars ctx->x are now initialized for the given
+	 * hash_bit_len.
+	 */
+	/* Set up to process the data message portion of the hash (default) */
+	skein_start_new_type(ctx, MSG);              /* T0=0, T1= MSG type */
+
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* init the context for a MAC and/or tree hash operation */
+/* [identical to skein_512_init() when key_bytes == 0 && \
+ *	tree_info == SKEIN_CFG_TREE_INFO_SEQUENTIAL] */
+int skein_512_init_ext(struct skein_512_ctx *ctx, size_t hash_bit_len,
+		       u64 tree_info, const u8 *key, size_t key_bytes)
+{
+	union {
+		u8 b[SKEIN_512_STATE_BYTES];
+		u64 w[SKEIN_512_STATE_WORDS];
+	} cfg;                              /* config block */
+
+	skein_assert_ret(hash_bit_len > 0, SKEIN_BAD_HASHLEN);
+	skein_assert_ret(key_bytes == 0 || key != NULL, SKEIN_FAIL);
+
+	/* compute the initial chaining values ctx->x[], based on key */
+	if (key_bytes == 0) { /* is there a key? */
+		/* no key: use all zeroes as key for config block */
+		memset(ctx->x, 0, sizeof(ctx->x));
+	} else { /* here to pre-process a key */
+		skein_assert(sizeof(cfg.b) >= sizeof(ctx->x));
+		/* do a mini-Init right here */
+		/* set output hash bit count = state size */
+		ctx->h.hash_bit_len = 8*sizeof(ctx->x);
+		/* set tweaks: T0 = 0; T1 = KEY type */
+		skein_start_new_type(ctx, KEY);
+		/* zero the initial chaining variables */
+		memset(ctx->x, 0, sizeof(ctx->x));
+		/* hash the key */
+		skein_512_update(ctx, key, key_bytes);
+		/* put result into cfg.b[] */
+		skein_512_final_pad(ctx, cfg.b);
+		/* copy over into ctx->x[] */
+		memcpy(ctx->x, cfg.b, sizeof(cfg.b));
+	}
+	/*
+	 * build/process the config block, type == CONFIG (could be
+	 * precomputed for each key)
+	 */
+	ctx->h.hash_bit_len = hash_bit_len;          /* output hash bit count */
+	skein_start_new_type(ctx, CFG_FINAL);
+
+	/* pre-pad cfg.w[] with zeroes */
+	memset(&cfg.w, 0, sizeof(cfg.w));
+	cfg.w[0] = skein_swap64(SKEIN_SCHEMA_VER);
+	/* hash result length in bits */
+	cfg.w[1] = skein_swap64(hash_bit_len);
+	/* tree hash config info (or SKEIN_CFG_TREE_INFO_SEQUENTIAL) */
+	cfg.w[2] = skein_swap64(tree_info);
+
+	skein_show_key(512, &ctx->h, key, key_bytes);
+
+	/* compute the initial chaining values from config block */
+	skein_512_process_block(ctx, cfg.b, 1, SKEIN_CFG_STR_LEN);
+
+	/* The chaining vars ctx->x are now initialized */
+	/* Set up to process the data message portion of the hash (default) */
+	skein_start_new_type(ctx, MSG);
+
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* process the input bytes */
+int skein_512_update(struct skein_512_ctx *ctx, const u8 *msg,
+		     size_t msg_byte_cnt)
+{
+	size_t n;
+
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_512_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* process full blocks, if any */
+	if (msg_byte_cnt + ctx->h.b_cnt > SKEIN_512_BLOCK_BYTES) {
+		/* finish up any buffered message data */
+		if (ctx->h.b_cnt) {
+			/* # bytes free in buffer b[] */
+			n = SKEIN_512_BLOCK_BYTES - ctx->h.b_cnt;
+			if (n) {
+				/* check on our logic here */
+				skein_assert(n < msg_byte_cnt);
+				memcpy(&ctx->b[ctx->h.b_cnt], msg, n);
+				msg_byte_cnt  -= n;
+				msg         += n;
+				ctx->h.b_cnt += n;
+			}
+			skein_assert(ctx->h.b_cnt == SKEIN_512_BLOCK_BYTES);
+			skein_512_process_block(ctx, ctx->b, 1,
+						SKEIN_512_BLOCK_BYTES);
+			ctx->h.b_cnt = 0;
+		}
+		/*
+		 * now process any remaining full blocks, directly from input
+		 * message data
+		 */
+		if (msg_byte_cnt > SKEIN_512_BLOCK_BYTES) {
+			/* number of full blocks to process */
+			n = (msg_byte_cnt-1) / SKEIN_512_BLOCK_BYTES;
+			skein_512_process_block(ctx, msg, n,
+						SKEIN_512_BLOCK_BYTES);
+			msg_byte_cnt -= n * SKEIN_512_BLOCK_BYTES;
+			msg        += n * SKEIN_512_BLOCK_BYTES;
+		}
+		skein_assert(ctx->h.b_cnt == 0);
+	}
+
+	/* copy any remaining source message data bytes into b[] */
+	if (msg_byte_cnt) {
+		skein_assert(msg_byte_cnt + ctx->h.b_cnt <=
+			     SKEIN_512_BLOCK_BYTES);
+		memcpy(&ctx->b[ctx->h.b_cnt], msg, msg_byte_cnt);
+		ctx->h.b_cnt += msg_byte_cnt;
+	}
+
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* finalize the hash computation and output the result */
+int skein_512_final(struct skein_512_ctx *ctx, u8 *hash_val)
+{
+	size_t i, n, byte_cnt;
+	u64 x[SKEIN_512_STATE_WORDS];
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_512_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* tag as the final block */
+	ctx->h.tweak[1] |= SKEIN_T1_FLAG_FINAL;
+	/* zero pad b[] if necessary */
+	if (ctx->h.b_cnt < SKEIN_512_BLOCK_BYTES)
+		memset(&ctx->b[ctx->h.b_cnt], 0,
+			SKEIN_512_BLOCK_BYTES - ctx->h.b_cnt);
+
+	/* process the final block */
+	skein_512_process_block(ctx, ctx->b, 1, ctx->h.b_cnt);
+
+	/* now output the result */
+	/* total number of output bytes */
+	byte_cnt = (ctx->h.hash_bit_len + 7) >> 3;
+
+	/* run Threefish in "counter mode" to generate output */
+	/* zero out b[], so it can hold the counter */
+	memset(ctx->b, 0, sizeof(ctx->b));
+	/* keep a local copy of counter mode "key" */
+	memcpy(x, ctx->x, sizeof(x));
+	for (i = 0; i*SKEIN_512_BLOCK_BYTES < byte_cnt; i++) {
+		/* build the counter block */
+		((u64 *)ctx->b)[0] = skein_swap64((u64) i);
+		skein_start_new_type(ctx, OUT_FINAL);
+		/* run "counter mode" */
+		skein_512_process_block(ctx, ctx->b, 1, sizeof(u64));
+		/* number of output bytes left to go */
+		n = byte_cnt - i*SKEIN_512_BLOCK_BYTES;
+		if (n >= SKEIN_512_BLOCK_BYTES)
+			n  = SKEIN_512_BLOCK_BYTES;
+		/* "output" the ctr mode bytes */
+		skein_put64_lsb_first(hash_val+i*SKEIN_512_BLOCK_BYTES, ctx->x,
+				      n);
+		skein_show_final(512, &ctx->h, n,
+				 hash_val+i*SKEIN_512_BLOCK_BYTES);
+		/* restore the counter mode key for next time */
+		memcpy(ctx->x, x, sizeof(x));
+	}
+	return SKEIN_SUCCESS;
+}
+
+/*****************************************************************/
+/*    1024-bit Skein                                             */
+/*****************************************************************/
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* init the context for a straight hashing operation  */
+int skein_1024_init(struct skein_1024_ctx *ctx, size_t hash_bit_len)
+{
+	union {
+		u8 b[SKEIN_1024_STATE_BYTES];
+		u64 w[SKEIN_1024_STATE_WORDS];
+	} cfg;                              /* config block */
+
+	skein_assert_ret(hash_bit_len > 0, SKEIN_BAD_HASHLEN);
+	ctx->h.hash_bit_len = hash_bit_len;         /* output hash bit count */
+
+	switch (hash_bit_len) { /* use pre-computed values, where available */
+	case  512:
+		memcpy(ctx->x, SKEIN_1024_IV_512, sizeof(ctx->x));
+		break;
+	case  384:
+		memcpy(ctx->x, SKEIN_1024_IV_384, sizeof(ctx->x));
+		break;
+	case 1024:
+		memcpy(ctx->x, SKEIN_1024_IV_1024, sizeof(ctx->x));
+		break;
+	default:
+		/* here if there is no precomputed IV value available */
+		/*
+		 * build/process the config block, type == CONFIG
+		 * (could be precomputed)
+		 */
+		/* set tweaks: T0=0; T1=CFG | FINAL */
+		skein_start_new_type(ctx, CFG_FINAL);
+
+		/* set the schema, version */
+		cfg.w[0] = skein_swap64(SKEIN_SCHEMA_VER);
+		/* hash result length in bits */
+		cfg.w[1] = skein_swap64(hash_bit_len);
+		cfg.w[2] = skein_swap64(SKEIN_CFG_TREE_INFO_SEQUENTIAL);
+		/* zero pad config block */
+		memset(&cfg.w[3], 0, sizeof(cfg) - 3*sizeof(cfg.w[0]));
+
+		/* compute the initial chaining values from config block */
+		/* zero the chaining variables */
+		memset(ctx->x, 0, sizeof(ctx->x));
+		skein_1024_process_block(ctx, cfg.b, 1, SKEIN_CFG_STR_LEN);
+		break;
+	}
+
+	/* The chaining vars ctx->x are now initialized for the hash_bit_len. */
+	/* Set up to process the data message portion of the hash (default) */
+	skein_start_new_type(ctx, MSG);              /* T0=0, T1= MSG type */
+
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* init the context for a MAC and/or tree hash operation */
+/* [identical to skein_1024_init() when key_bytes == 0 && \
+ *	tree_info == SKEIN_CFG_TREE_INFO_SEQUENTIAL] */
+int skein_1024_init_ext(struct skein_1024_ctx *ctx, size_t hash_bit_len,
+			u64 tree_info, const u8 *key, size_t key_bytes)
+{
+	union {
+		u8 b[SKEIN_1024_STATE_BYTES];
+		u64 w[SKEIN_1024_STATE_WORDS];
+	} cfg;                              /* config block */
+
+	skein_assert_ret(hash_bit_len > 0, SKEIN_BAD_HASHLEN);
+	skein_assert_ret(key_bytes == 0 || key != NULL, SKEIN_FAIL);
+
+	/* compute the initial chaining values ctx->x[], based on key */
+	if (key_bytes == 0) { /* is there a key? */
+		/* no key: use all zeroes as key for config block */
+		memset(ctx->x, 0, sizeof(ctx->x));
+	} else { /* here to pre-process a key */
+		skein_assert(sizeof(cfg.b) >= sizeof(ctx->x));
+		/* do a mini-Init right here */
+		/* set output hash bit count = state size */
+		ctx->h.hash_bit_len = 8*sizeof(ctx->x);
+		/* set tweaks: T0 = 0; T1 = KEY type */
+		skein_start_new_type(ctx, KEY);
+		/* zero the initial chaining variables */
+		memset(ctx->x, 0, sizeof(ctx->x));
+		/* hash the key */
+		skein_1024_update(ctx, key, key_bytes);
+		/* put result into cfg.b[] */
+		skein_1024_final_pad(ctx, cfg.b);
+		/* copy over into ctx->x[] */
+		memcpy(ctx->x, cfg.b, sizeof(cfg.b));
+	}
+	/*
+	 * build/process the config block, type == CONFIG (could be
+	 * precomputed for each key)
+	 */
+	/* output hash bit count */
+	ctx->h.hash_bit_len = hash_bit_len;
+	skein_start_new_type(ctx, CFG_FINAL);
+
+	/* pre-pad cfg.w[] with zeroes */
+	memset(&cfg.w, 0, sizeof(cfg.w));
+	cfg.w[0] = skein_swap64(SKEIN_SCHEMA_VER);
+	/* hash result length in bits */
+	cfg.w[1] = skein_swap64(hash_bit_len);
+	/* tree hash config info (or SKEIN_CFG_TREE_INFO_SEQUENTIAL) */
+	cfg.w[2] = skein_swap64(tree_info);
+
+	skein_show_key(1024, &ctx->h, key, key_bytes);
+
+	/* compute the initial chaining values from config block */
+	skein_1024_process_block(ctx, cfg.b, 1, SKEIN_CFG_STR_LEN);
+
+	/* The chaining vars ctx->x are now initialized */
+	/* Set up to process the data message portion of the hash (default) */
+	skein_start_new_type(ctx, MSG);
+
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* process the input bytes */
+int skein_1024_update(struct skein_1024_ctx *ctx, const u8 *msg,
+		      size_t msg_byte_cnt)
+{
+	size_t n;
+
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_1024_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* process full blocks, if any */
+	if (msg_byte_cnt + ctx->h.b_cnt > SKEIN_1024_BLOCK_BYTES) {
+		/* finish up any buffered message data */
+		if (ctx->h.b_cnt) {
+			/* # bytes free in buffer b[] */
+			n = SKEIN_1024_BLOCK_BYTES - ctx->h.b_cnt;
+			if (n) {
+				/* check on our logic here */
+				skein_assert(n < msg_byte_cnt);
+				memcpy(&ctx->b[ctx->h.b_cnt], msg, n);
+				msg_byte_cnt  -= n;
+				msg         += n;
+				ctx->h.b_cnt += n;
+			}
+			skein_assert(ctx->h.b_cnt == SKEIN_1024_BLOCK_BYTES);
+			skein_1024_process_block(ctx, ctx->b, 1,
+						 SKEIN_1024_BLOCK_BYTES);
+			ctx->h.b_cnt = 0;
+		}
+		/*
+		 * now process any remaining full blocks, directly from input
+		 * message data
+		 */
+		if (msg_byte_cnt > SKEIN_1024_BLOCK_BYTES) {
+			/* number of full blocks to process */
+			n = (msg_byte_cnt-1) / SKEIN_1024_BLOCK_BYTES;
+			skein_1024_process_block(ctx, msg, n,
+						 SKEIN_1024_BLOCK_BYTES);
+			msg_byte_cnt -= n * SKEIN_1024_BLOCK_BYTES;
+			msg        += n * SKEIN_1024_BLOCK_BYTES;
+		}
+		skein_assert(ctx->h.b_cnt == 0);
+	}
+
+	/* copy any remaining source message data bytes into b[] */
+	if (msg_byte_cnt) {
+		skein_assert(msg_byte_cnt + ctx->h.b_cnt <=
+			     SKEIN_1024_BLOCK_BYTES);
+		memcpy(&ctx->b[ctx->h.b_cnt], msg, msg_byte_cnt);
+		ctx->h.b_cnt += msg_byte_cnt;
+	}
+
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* finalize the hash computation and output the result */
+int skein_1024_final(struct skein_1024_ctx *ctx, u8 *hash_val)
+{
+	size_t i, n, byte_cnt;
+	u64 x[SKEIN_1024_STATE_WORDS];
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_1024_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* tag as the final block */
+	ctx->h.tweak[1] |= SKEIN_T1_FLAG_FINAL;
+	/* zero pad b[] if necessary */
+	if (ctx->h.b_cnt < SKEIN_1024_BLOCK_BYTES)
+		memset(&ctx->b[ctx->h.b_cnt], 0,
+			SKEIN_1024_BLOCK_BYTES - ctx->h.b_cnt);
+
+	/* process the final block */
+	skein_1024_process_block(ctx, ctx->b, 1, ctx->h.b_cnt);
+
+	/* now output the result */
+	/* total number of output bytes */
+	byte_cnt = (ctx->h.hash_bit_len + 7) >> 3;
+
+	/* run Threefish in "counter mode" to generate output */
+	/* zero out b[], so it can hold the counter */
+	memset(ctx->b, 0, sizeof(ctx->b));
+	/* keep a local copy of counter mode "key" */
+	memcpy(x, ctx->x, sizeof(x));
+	for (i = 0; i*SKEIN_1024_BLOCK_BYTES < byte_cnt; i++) {
+		/* build the counter block */
+		((u64 *)ctx->b)[0] = skein_swap64((u64) i);
+		skein_start_new_type(ctx, OUT_FINAL);
+		/* run "counter mode" */
+		skein_1024_process_block(ctx, ctx->b, 1, sizeof(u64));
+		/* number of output bytes left to go */
+		n = byte_cnt - i*SKEIN_1024_BLOCK_BYTES;
+		if (n >= SKEIN_1024_BLOCK_BYTES)
+			n  = SKEIN_1024_BLOCK_BYTES;
+		/* "output" the ctr mode bytes */
+		skein_put64_lsb_first(hash_val+i*SKEIN_1024_BLOCK_BYTES, ctx->x,
+				      n);
+		skein_show_final(1024, &ctx->h, n,
+				 hash_val+i*SKEIN_1024_BLOCK_BYTES);
+		/* restore the counter mode key for next time */
+		memcpy(ctx->x, x, sizeof(x));
+	}
+	return SKEIN_SUCCESS;
+}
+
+/**************** Functions to support MAC/tree hashing ***************/
+/*   (this code is identical for Optimized and Reference versions)    */
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* finalize the hash computation and output the block, no OUTPUT stage */
+int skein_256_final_pad(struct skein_256_ctx *ctx, u8 *hash_val)
+{
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_256_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* tag as the final block */
+	ctx->h.tweak[1] |= SKEIN_T1_FLAG_FINAL;
+	/* zero pad b[] if necessary */
+	if (ctx->h.b_cnt < SKEIN_256_BLOCK_BYTES)
+		memset(&ctx->b[ctx->h.b_cnt], 0,
+			SKEIN_256_BLOCK_BYTES - ctx->h.b_cnt);
+	/* process the final block */
+	skein_256_process_block(ctx, ctx->b, 1, ctx->h.b_cnt);
+
+	/* "output" the state bytes */
+	skein_put64_lsb_first(hash_val, ctx->x, SKEIN_256_BLOCK_BYTES);
+
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* finalize the hash computation and output the block, no OUTPUT stage */
+int skein_512_final_pad(struct skein_512_ctx *ctx, u8 *hash_val)
+{
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_512_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* tag as the final block */
+	ctx->h.tweak[1] |= SKEIN_T1_FLAG_FINAL;
+	/* zero pad b[] if necessary */
+	if (ctx->h.b_cnt < SKEIN_512_BLOCK_BYTES)
+		memset(&ctx->b[ctx->h.b_cnt], 0,
+			SKEIN_512_BLOCK_BYTES - ctx->h.b_cnt);
+	/* process the final block */
+	skein_512_process_block(ctx, ctx->b, 1, ctx->h.b_cnt);
+
+	/* "output" the state bytes */
+	skein_put64_lsb_first(hash_val, ctx->x, SKEIN_512_BLOCK_BYTES);
+
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* finalize the hash computation and output the block, no OUTPUT stage */
+int skein_1024_final_pad(struct skein_1024_ctx *ctx, u8 *hash_val)
+{
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_1024_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* tag as the final block */
+	ctx->h.tweak[1] |= SKEIN_T1_FLAG_FINAL;
+	/* zero pad b[] if necessary */
+	if (ctx->h.b_cnt < SKEIN_1024_BLOCK_BYTES)
+		memset(&ctx->b[ctx->h.b_cnt], 0,
+			SKEIN_1024_BLOCK_BYTES - ctx->h.b_cnt);
+	/* process the final block */
+	skein_1024_process_block(ctx, ctx->b, 1, ctx->h.b_cnt);
+
+	/* "output" the state bytes */
+	skein_put64_lsb_first(hash_val, ctx->x, SKEIN_1024_BLOCK_BYTES);
+
+	return SKEIN_SUCCESS;
+}
+
+#if SKEIN_TREE_HASH
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* just do the OUTPUT stage                                       */
+int skein_256_output(struct skein_256_ctx *ctx, u8 *hash_val)
+{
+	size_t i, n, byte_cnt;
+	u64 x[SKEIN_256_STATE_WORDS];
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_256_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* now output the result */
+	/* total number of output bytes */
+	byte_cnt = (ctx->h.hash_bit_len + 7) >> 3;
+
+	/* run Threefish in "counter mode" to generate output */
+	/* zero out b[], so it can hold the counter */
+	memset(ctx->b, 0, sizeof(ctx->b));
+	/* keep a local copy of counter mode "key" */
+	memcpy(x, ctx->x, sizeof(x));
+	for (i = 0; i*SKEIN_256_BLOCK_BYTES < byte_cnt; i++) {
+		/* build the counter block */
+		((u64 *)ctx->b)[0] = skein_swap64((u64) i);
+		skein_start_new_type(ctx, OUT_FINAL);
+		/* run "counter mode" */
+		skein_256_process_block(ctx, ctx->b, 1, sizeof(u64));
+		/* number of output bytes left to go */
+		n = byte_cnt - i*SKEIN_256_BLOCK_BYTES;
+		if (n >= SKEIN_256_BLOCK_BYTES)
+			n  = SKEIN_256_BLOCK_BYTES;
+		/* "output" the ctr mode bytes */
+		skein_put64_lsb_first(hash_val+i*SKEIN_256_BLOCK_BYTES, ctx->x,
+				      n);
+		skein_show_final(256, &ctx->h, n,
+				 hash_val+i*SKEIN_256_BLOCK_BYTES);
+		/* restore the counter mode key for next time */
+		memcpy(ctx->x, x, sizeof(x));
+	}
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* just do the OUTPUT stage                                       */
+int skein_512_output(struct skein_512_ctx *ctx, u8 *hash_val)
+{
+	size_t i, n, byte_cnt;
+	u64 x[SKEIN_512_STATE_WORDS];
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_512_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* now output the result */
+	/* total number of output bytes */
+	byte_cnt = (ctx->h.hash_bit_len + 7) >> 3;
+
+	/* run Threefish in "counter mode" to generate output */
+	/* zero out b[], so it can hold the counter */
+	memset(ctx->b, 0, sizeof(ctx->b));
+	/* keep a local copy of counter mode "key" */
+	memcpy(x, ctx->x, sizeof(x));
+	for (i = 0; i*SKEIN_512_BLOCK_BYTES < byte_cnt; i++) {
+		/* build the counter block */
+		((u64 *)ctx->b)[0] = skein_swap64((u64) i);
+		skein_start_new_type(ctx, OUT_FINAL);
+		/* run "counter mode" */
+		skein_512_process_block(ctx, ctx->b, 1, sizeof(u64));
+		/* number of output bytes left to go */
+		n = byte_cnt - i*SKEIN_512_BLOCK_BYTES;
+		if (n >= SKEIN_512_BLOCK_BYTES)
+			n  = SKEIN_512_BLOCK_BYTES;
+		/* "output" the ctr mode bytes */
+		skein_put64_lsb_first(hash_val+i*SKEIN_512_BLOCK_BYTES, ctx->x,
+				      n);
+		skein_show_final(256, &ctx->h, n,
+				 hash_val+i*SKEIN_512_BLOCK_BYTES);
+		/* restore the counter mode key for next time */
+		memcpy(ctx->x, x, sizeof(x));
+	}
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* just do the OUTPUT stage                                       */
+int skein_1024_output(struct skein_1024_ctx *ctx, u8 *hash_val)
+{
+	size_t i, n, byte_cnt;
+	u64 x[SKEIN_1024_STATE_WORDS];
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_1024_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* now output the result */
+	/* total number of output bytes */
+	byte_cnt = (ctx->h.hash_bit_len + 7) >> 3;
+
+	/* run Threefish in "counter mode" to generate output */
+	/* zero out b[], so it can hold the counter */
+	memset(ctx->b, 0, sizeof(ctx->b));
+	/* keep a local copy of counter mode "key" */
+	memcpy(x, ctx->x, sizeof(x));
+	for (i = 0; i*SKEIN_1024_BLOCK_BYTES < byte_cnt; i++) {
+		/* build the counter block */
+		((u64 *)ctx->b)[0] = skein_swap64((u64) i);
+		skein_start_new_type(ctx, OUT_FINAL);
+		/* run "counter mode" */
+		skein_1024_process_block(ctx, ctx->b, 1, sizeof(u64));
+		/* number of output bytes left to go */
+		n = byte_cnt - i*SKEIN_1024_BLOCK_BYTES;
+		if (n >= SKEIN_1024_BLOCK_BYTES)
+			n  = SKEIN_1024_BLOCK_BYTES;
+		/* "output" the ctr mode bytes */
+		skein_put64_lsb_first(hash_val+i*SKEIN_1024_BLOCK_BYTES, ctx->x,
+				      n);
+		skein_show_final(256, &ctx->h, n,
+				 hash_val+i*SKEIN_1024_BLOCK_BYTES);
+		/* restore the counter mode key for next time */
+		memcpy(ctx->x, x, sizeof(x));
+	}
+	return SKEIN_SUCCESS;
+}
+#endif
diff --git a/drivers/staging/skein/skein.h b/drivers/staging/skein/skein.h
new file mode 100644
index 00000000000..e6669f196e5
--- /dev/null
+++ b/drivers/staging/skein/skein.h
@@ -0,0 +1,346 @@
+#ifndef _SKEIN_H_
+#define _SKEIN_H_     1
+/**************************************************************************
+**
+** Interface declarations and internal definitions for Skein hashing.
+**
+** Source code author: Doug Whiting, 2008.
+**
+** This algorithm and source code is released to the public domain.
+**
+***************************************************************************
+**
+** The following compile-time switches may be defined to control some
+** tradeoffs between speed, code size, error checking, and security.
+**
+** The "default" note explains what happens when the switch is not defined.
+**
+**  SKEIN_DEBUG            -- make callouts from inside Skein code
+**                            to examine/display intermediate values.
+**                            [default: no callouts (no overhead)]
+**
+**  SKEIN_ERR_CHECK        -- how error checking is handled inside Skein
+**                            code. If not defined, most error checking
+**                            is disabled (for performance). Otherwise,
+**                            the switch value is interpreted as:
+**                                0: use assert()      to flag errors
+**                                1: return SKEIN_FAIL to flag errors
+**
+***************************************************************************/
+
+#ifndef rotl_64
+#define rotl_64(x, N)    (((x) << (N)) | ((x) >> (64-(N))))
+#endif
+
+/* below two prototype assume we are handed aligned data */
+#define skein_put64_lsb_first(dst08, src64, b_cnt) memcpy(dst08, src64, b_cnt)
+#define skein_get64_lsb_first(dst64, src08, w_cnt) \
+		memcpy(dst64, src08, 8*(w_cnt))
+#define skein_swap64(w64)  (w64)
+
+enum {
+	SKEIN_SUCCESS         =      0, /* return codes from Skein calls */
+	SKEIN_FAIL            =      1,
+	SKEIN_BAD_HASHLEN     =      2
+};
+
+#define  SKEIN_MODIFIER_WORDS   (2) /* number of modifier (tweak) words */
+
+#define  SKEIN_256_STATE_WORDS  (4)
+#define  SKEIN_512_STATE_WORDS  (8)
+#define  SKEIN_1024_STATE_WORDS (16)
+#define  SKEIN_MAX_STATE_WORDS (16)
+
+#define  SKEIN_256_STATE_BYTES  (8*SKEIN_256_STATE_WORDS)
+#define  SKEIN_512_STATE_BYTES  (8*SKEIN_512_STATE_WORDS)
+#define  SKEIN_1024_STATE_BYTES  (8*SKEIN_1024_STATE_WORDS)
+
+#define  SKEIN_256_STATE_BITS  (64*SKEIN_256_STATE_WORDS)
+#define  SKEIN_512_STATE_BITS  (64*SKEIN_512_STATE_WORDS)
+#define  SKEIN_1024_STATE_BITS  (64*SKEIN_1024_STATE_WORDS)
+
+#define  SKEIN_256_BLOCK_BYTES  (8*SKEIN_256_STATE_WORDS)
+#define  SKEIN_512_BLOCK_BYTES  (8*SKEIN_512_STATE_WORDS)
+#define  SKEIN_1024_BLOCK_BYTES  (8*SKEIN_1024_STATE_WORDS)
+
+struct skein_ctx_hdr {
+	size_t hash_bit_len;		/* size of hash result, in bits */
+	size_t b_cnt;			/* current byte count in buffer b[] */
+	u64 tweak[SKEIN_MODIFIER_WORDS]; /* tweak[0]=byte cnt, tweak[1]=flags */
+};
+
+struct skein_256_ctx { /* 256-bit Skein hash context structure */
+	struct skein_ctx_hdr h;		/* common header context variables */
+	u64 x[SKEIN_256_STATE_WORDS];	/* chaining variables */
+	u8 b[SKEIN_256_BLOCK_BYTES];	/* partial block buf (8-byte aligned) */
+};
+
+struct skein_512_ctx { /* 512-bit Skein hash context structure */
+	struct skein_ctx_hdr h;		/* common header context variables */
+	u64 x[SKEIN_512_STATE_WORDS];	/* chaining variables */
+	u8 b[SKEIN_512_BLOCK_BYTES];	/* partial block buf (8-byte aligned) */
+};
+
+struct skein_1024_ctx { /* 1024-bit Skein hash context structure */
+	struct skein_ctx_hdr h;		/* common header context variables */
+	u64 x[SKEIN_1024_STATE_WORDS];	/* chaining variables */
+	u8 b[SKEIN_1024_BLOCK_BYTES];	/* partial block buf (8-byte aligned) */
+};
+
+/* Skein APIs for (incremental) "straight hashing" */
+int skein_256_init(struct skein_256_ctx *ctx, size_t hash_bit_len);
+int skein_512_init(struct skein_512_ctx *ctx, size_t hash_bit_len);
+int skein_1024_init(struct skein_1024_ctx *ctx, size_t hash_bit_len);
+
+int skein_256_update(struct skein_256_ctx *ctx, const u8 *msg,
+		     size_t msg_byte_cnt);
+int skein_512_update(struct skein_512_ctx *ctx, const u8 *msg,
+		     size_t msg_byte_cnt);
+int skein_1024_update(struct skein_1024_ctx *ctx, const u8 *msg,
+		      size_t msg_byte_cnt);
+
+int skein_256_final(struct skein_256_ctx *ctx, u8 *hash_val);
+int skein_512_final(struct skein_512_ctx *ctx, u8 *hash_val);
+int skein_1024_final(struct skein_1024_ctx *ctx, u8 *hash_val);
+
+/*
+**   Skein APIs for "extended" initialization: MAC keys, tree hashing.
+**   After an init_ext() call, just use update/final calls as with init().
+**
+**   Notes: Same parameters as _init() calls, plus tree_info/key/key_bytes.
+**          When key_bytes == 0 and tree_info == SKEIN_SEQUENTIAL,
+**              the results of init_ext() are identical to calling init().
+**          The function init() may be called once to "precompute" the IV for
+**              a given hash_bit_len value, then by saving a copy of the context
+**              the IV computation may be avoided in later calls.
+**          Similarly, the function init_ext() may be called once per MAC key
+**              to precompute the MAC IV, then a copy of the context saved and
+**              reused for each new MAC computation.
+**/
+int skein_256_init_ext(struct skein_256_ctx *ctx, size_t hash_bit_len,
+		       u64 tree_info, const u8 *key, size_t key_bytes);
+int skein_512_init_ext(struct skein_512_ctx *ctx, size_t hash_bit_len,
+		       u64 tree_info, const u8 *key, size_t key_bytes);
+int skein_1024_init_ext(struct skein_1024_ctx *ctx, size_t hash_bit_len,
+			u64 tree_info, const u8 *key, size_t key_bytes);
+
+/*
+**   Skein APIs for MAC and tree hash:
+**      final_pad:  pad, do final block, but no OUTPUT type
+**      output:     do just the output stage
+*/
+int skein_256_final_pad(struct skein_256_ctx *ctx, u8 *hash_val);
+int skein_512_final_pad(struct skein_512_ctx *ctx, u8 *hash_val);
+int skein_1024_final_pad(struct skein_1024_ctx *ctx, u8 *hash_val);
+
+#ifndef SKEIN_TREE_HASH
+#define SKEIN_TREE_HASH (1)
+#endif
+#if  SKEIN_TREE_HASH
+int skein_256_output(struct skein_256_ctx *ctx, u8 *hash_val);
+int skein_512_output(struct skein_512_ctx *ctx, u8 *hash_val);
+int skein_1024_output(struct skein_1024_ctx *ctx, u8 *hash_val);
+#endif
+
+/*****************************************************************
+** "Internal" Skein definitions
+**    -- not needed for sequential hashing API, but will be
+**           helpful for other uses of Skein (e.g., tree hash mode).
+**    -- included here so that they can be shared between
+**           reference and optimized code.
+******************************************************************/
+
+/* tweak word tweak[1]: bit field starting positions */
+#define SKEIN_T1_BIT(BIT)       ((BIT) - 64)      /* second word  */
+
+#define SKEIN_T1_POS_TREE_LVL   SKEIN_T1_BIT(112) /* 112..118 hash tree level */
+#define SKEIN_T1_POS_BIT_PAD    SKEIN_T1_BIT(119) /* 119 part. final in byte */
+#define SKEIN_T1_POS_BLK_TYPE   SKEIN_T1_BIT(120) /* 120..125 type field `*/
+#define SKEIN_T1_POS_FIRST      SKEIN_T1_BIT(126) /* 126      first blk flag */
+#define SKEIN_T1_POS_FINAL      SKEIN_T1_BIT(127) /* 127      final blk flag */
+
+/* tweak word tweak[1]: flag bit definition(s) */
+#define SKEIN_T1_FLAG_FIRST     (((u64)  1) << SKEIN_T1_POS_FIRST)
+#define SKEIN_T1_FLAG_FINAL     (((u64)  1) << SKEIN_T1_POS_FINAL)
+#define SKEIN_T1_FLAG_BIT_PAD   (((u64)  1) << SKEIN_T1_POS_BIT_PAD)
+
+/* tweak word tweak[1]: tree level bit field mask */
+#define SKEIN_T1_TREE_LVL_MASK  (((u64)0x7F) << SKEIN_T1_POS_TREE_LVL)
+#define SKEIN_T1_TREE_LEVEL(n)  (((u64) (n)) << SKEIN_T1_POS_TREE_LVL)
+
+/* tweak word tweak[1]: block type field */
+#define SKEIN_BLK_TYPE_KEY       (0) /* key, for MAC and KDF */
+#define SKEIN_BLK_TYPE_CFG       (4) /* configuration block */
+#define SKEIN_BLK_TYPE_PERS      (8) /* personalization string */
+#define SKEIN_BLK_TYPE_PK       (12) /* pubkey (for digital sigs) */
+#define SKEIN_BLK_TYPE_KDF      (16) /* key identifier for KDF */
+#define SKEIN_BLK_TYPE_NONCE    (20) /* nonce for PRNG */
+#define SKEIN_BLK_TYPE_MSG      (48) /* message processing */
+#define SKEIN_BLK_TYPE_OUT      (63) /* output stage */
+#define SKEIN_BLK_TYPE_MASK     (63) /* bit field mask */
+
+#define SKEIN_T1_BLK_TYPE(T)   (((u64) (SKEIN_BLK_TYPE_##T)) << \
+					SKEIN_T1_POS_BLK_TYPE)
+#define SKEIN_T1_BLK_TYPE_KEY   SKEIN_T1_BLK_TYPE(KEY)  /* for MAC and KDF */
+#define SKEIN_T1_BLK_TYPE_CFG   SKEIN_T1_BLK_TYPE(CFG)  /* config block */
+#define SKEIN_T1_BLK_TYPE_PERS  SKEIN_T1_BLK_TYPE(PERS) /* personalization */
+#define SKEIN_T1_BLK_TYPE_PK    SKEIN_T1_BLK_TYPE(PK)   /* pubkey (for sigs) */
+#define SKEIN_T1_BLK_TYPE_KDF   SKEIN_T1_BLK_TYPE(KDF)  /* key ident for KDF */
+#define SKEIN_T1_BLK_TYPE_NONCE SKEIN_T1_BLK_TYPE(NONCE)/* nonce for PRNG */
+#define SKEIN_T1_BLK_TYPE_MSG   SKEIN_T1_BLK_TYPE(MSG)  /* message processing */
+#define SKEIN_T1_BLK_TYPE_OUT   SKEIN_T1_BLK_TYPE(OUT)  /* output stage */
+#define SKEIN_T1_BLK_TYPE_MASK  SKEIN_T1_BLK_TYPE(MASK) /* field bit mask */
+
+#define SKEIN_T1_BLK_TYPE_CFG_FINAL    (SKEIN_T1_BLK_TYPE_CFG | \
+					SKEIN_T1_FLAG_FINAL)
+#define SKEIN_T1_BLK_TYPE_OUT_FINAL    (SKEIN_T1_BLK_TYPE_OUT | \
+					SKEIN_T1_FLAG_FINAL)
+
+#define SKEIN_VERSION           (1)
+
+#ifndef SKEIN_ID_STRING_LE      /* allow compile-time personalization */
+#define SKEIN_ID_STRING_LE      (0x33414853) /* "SHA3" (little-endian)*/
+#endif
+
+#define SKEIN_MK_64(hi32, lo32)  ((lo32) + (((u64) (hi32)) << 32))
+#define SKEIN_SCHEMA_VER        SKEIN_MK_64(SKEIN_VERSION, SKEIN_ID_STRING_LE)
+#define SKEIN_KS_PARITY         SKEIN_MK_64(0x1BD11BDA, 0xA9FC1A22)
+
+#define SKEIN_CFG_STR_LEN       (4*8)
+
+/* bit field definitions in config block tree_info word */
+#define SKEIN_CFG_TREE_LEAF_SIZE_POS  (0)
+#define SKEIN_CFG_TREE_NODE_SIZE_POS  (8)
+#define SKEIN_CFG_TREE_MAX_LEVEL_POS  (16)
+
+#define SKEIN_CFG_TREE_LEAF_SIZE_MSK (((u64)0xFF) << \
+					SKEIN_CFG_TREE_LEAF_SIZE_POS)
+#define SKEIN_CFG_TREE_NODE_SIZE_MSK (((u64)0xFF) << \
+					SKEIN_CFG_TREE_NODE_SIZE_POS)
+#define SKEIN_CFG_TREE_MAX_LEVEL_MSK (((u64)0xFF) << \
+					SKEIN_CFG_TREE_MAX_LEVEL_POS)
+
+#define SKEIN_CFG_TREE_INFO(leaf, node, max_lvl)                   \
+	((((u64)(leaf))   << SKEIN_CFG_TREE_LEAF_SIZE_POS) |    \
+	 (((u64)(node))   << SKEIN_CFG_TREE_NODE_SIZE_POS) |    \
+	 (((u64)(max_lvl)) << SKEIN_CFG_TREE_MAX_LEVEL_POS))
+
+/* use as tree_info in InitExt() call for sequential processing */
+#define SKEIN_CFG_TREE_INFO_SEQUENTIAL SKEIN_CFG_TREE_INFO(0, 0, 0)
+
+/*
+**   Skein macros for getting/setting tweak words, etc.
+**   These are useful for partial input bytes, hash tree init/update, etc.
+**/
+#define skein_get_tweak(ctx_ptr, TWK_NUM)          ((ctx_ptr)->h.tweak[TWK_NUM])
+#define skein_set_tweak(ctx_ptr, TWK_NUM, t_val) { \
+		(ctx_ptr)->h.tweak[TWK_NUM] = (t_val); \
+	}
+
+#define skein_get_T0(ctx_ptr)     skein_get_tweak(ctx_ptr, 0)
+#define skein_get_T1(ctx_ptr)     skein_get_tweak(ctx_ptr, 1)
+#define skein_set_T0(ctx_ptr, T0) skein_set_tweak(ctx_ptr, 0, T0)
+#define skein_set_T1(ctx_ptr, T1) skein_set_tweak(ctx_ptr, 1, T1)
+
+/* set both tweak words at once */
+#define skein_set_T0_T1(ctx_ptr, T0, T1)           \
+	{                                          \
+	skein_set_T0(ctx_ptr, (T0));               \
+	skein_set_T1(ctx_ptr, (T1));               \
+	}
+
+#define skein_set_type(ctx_ptr, BLK_TYPE)         \
+	skein_set_T1(ctx_ptr, SKEIN_T1_BLK_TYPE_##BLK_TYPE)
+
+/*
+ * setup for starting with a new type:
+ * h.tweak[0]=0; h.tweak[1] = NEW_TYPE; h.b_cnt=0;
+ */
+#define skein_start_new_type(ctx_ptr, BLK_TYPE) { \
+		skein_set_T0_T1(ctx_ptr, 0, SKEIN_T1_FLAG_FIRST | \
+				SKEIN_T1_BLK_TYPE_##BLK_TYPE); \
+		(ctx_ptr)->h.b_cnt = 0; \
+	}
+
+#define skein_clear_first_flag(hdr) { \
+		(hdr).tweak[1] &= ~SKEIN_T1_FLAG_FIRST; \
+	}
+#define skein_set_bit_pad_flag(hdr) { \
+		(hdr).tweak[1] |=  SKEIN_T1_FLAG_BIT_PAD; \
+	}
+
+#define skein_set_tree_level(hdr, height) { \
+		(hdr).tweak[1] |= SKEIN_T1_TREE_LEVEL(height); \
+	}
+
+/*****************************************************************
+** "Internal" Skein definitions for debugging and error checking
+******************************************************************/
+#ifdef SKEIN_DEBUG             /* examine/display intermediate values? */
+#include "skein_debug.h"
+#else                           /* default is no callouts */
+#define skein_show_block(bits, ctx, x, blk_ptr, w_ptr, ks_event_ptr, ks_odd_ptr)
+#define skein_show_round(bits, ctx, r, x)
+#define skein_show_r_ptr(bits, ctx, r, x_ptr)
+#define skein_show_final(bits, ctx, cnt, out_ptr)
+#define skein_show_key(bits, ctx, key, key_bytes)
+#endif
+
+/* ignore all asserts, for performance */
+#define skein_assert_ret(x, ret_code)
+#define skein_assert(x)
+
+/*****************************************************************
+** Skein block function constants (shared across Ref and Opt code)
+******************************************************************/
+enum {
+	    /* SKEIN_256 round rotation constants */
+	R_256_0_0 = 14, R_256_0_1 = 16,
+	R_256_1_0 = 52, R_256_1_1 = 57,
+	R_256_2_0 = 23, R_256_2_1 = 40,
+	R_256_3_0 =  5, R_256_3_1 = 37,
+	R_256_4_0 = 25, R_256_4_1 = 33,
+	R_256_5_0 = 46, R_256_5_1 = 12,
+	R_256_6_0 = 58, R_256_6_1 = 22,
+	R_256_7_0 = 32, R_256_7_1 = 32,
+
+	    /* SKEIN_512 round rotation constants */
+	R_512_0_0 = 46, R_512_0_1 = 36, R_512_0_2 = 19, R_512_0_3 = 37,
+	R_512_1_0 = 33, R_512_1_1 = 27, R_512_1_2 = 14, R_512_1_3 = 42,
+	R_512_2_0 = 17, R_512_2_1 = 49, R_512_2_2 = 36, R_512_2_3 = 39,
+	R_512_3_0 = 44, R_512_3_1 =  9, R_512_3_2 = 54, R_512_3_3 = 56,
+	R_512_4_0 = 39, R_512_4_1 = 30, R_512_4_2 = 34, R_512_4_3 = 24,
+	R_512_5_0 = 13, R_512_5_1 = 50, R_512_5_2 = 10, R_512_5_3 = 17,
+	R_512_6_0 = 25, R_512_6_1 = 29, R_512_6_2 = 39, R_512_6_3 = 43,
+	R_512_7_0 =  8, R_512_7_1 = 35, R_512_7_2 = 56, R_512_7_3 = 22,
+
+	    /* SKEIN_1024 round rotation constants */
+	R1024_0_0 = 24, R1024_0_1 = 13, R1024_0_2 =  8, R1024_0_3 = 47,
+	R1024_0_4 =  8, R1024_0_5 = 17, R1024_0_6 = 22, R1024_0_7 = 37,
+	R1024_1_0 = 38, R1024_1_1 = 19, R1024_1_2 = 10, R1024_1_3 = 55,
+	R1024_1_4 = 49, R1024_1_5 = 18, R1024_1_6 = 23, R1024_1_7 = 52,
+	R1024_2_0 = 33, R1024_2_1 =  4, R1024_2_2 = 51, R1024_2_3 = 13,
+	R1024_2_4 = 34, R1024_2_5 = 41, R1024_2_6 = 59, R1024_2_7 = 17,
+	R1024_3_0 =  5, R1024_3_1 = 20, R1024_3_2 = 48, R1024_3_3 = 41,
+	R1024_3_4 = 47, R1024_3_5 = 28, R1024_3_6 = 16, R1024_3_7 = 25,
+	R1024_4_0 = 41, R1024_4_1 =  9, R1024_4_2 = 37, R1024_4_3 = 31,
+	R1024_4_4 = 12, R1024_4_5 = 47, R1024_4_6 = 44, R1024_4_7 = 30,
+	R1024_5_0 = 16, R1024_5_1 = 34, R1024_5_2 = 56, R1024_5_3 = 51,
+	R1024_5_4 =  4, R1024_5_5 = 53, R1024_5_6 = 42, R1024_5_7 = 41,
+	R1024_6_0 = 31, R1024_6_1 = 44, R1024_6_2 = 47, R1024_6_3 = 46,
+	R1024_6_4 = 19, R1024_6_5 = 42, R1024_6_6 = 44, R1024_6_7 = 25,
+	R1024_7_0 =  9, R1024_7_1 = 48, R1024_7_2 = 35, R1024_7_3 = 52,
+	R1024_7_4 = 23, R1024_7_5 = 31, R1024_7_6 = 37, R1024_7_7 = 20
+};
+
+#ifndef SKEIN_ROUNDS
+#define SKEIN_256_ROUNDS_TOTAL (72)	/* # rounds for diff block sizes */
+#define SKEIN_512_ROUNDS_TOTAL (72)
+#define SKEIN_1024_ROUNDS_TOTAL (80)
+#else			/* allow command-line define in range 8*(5..14)   */
+#define SKEIN_256_ROUNDS_TOTAL (8*((((SKEIN_ROUNDS/100) + 5) % 10) + 5))
+#define SKEIN_512_ROUNDS_TOTAL (8*((((SKEIN_ROUNDS/10)  + 5) % 10) + 5))
+#define SKEIN_1024_ROUNDS_TOTAL (8*((((SKEIN_ROUNDS)     + 5) % 10) + 5))
+#endif
+
+#endif  /* ifndef _SKEIN_H_ */
diff --git a/drivers/staging/skein/skein_api.c b/drivers/staging/skein/skein_api.c
new file mode 100644
index 00000000000..6e700eefc00
--- /dev/null
+++ b/drivers/staging/skein/skein_api.c
@@ -0,0 +1,239 @@
+/*
+Copyright (c) 2010 Werner Dittmann
+
+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 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/string.h>
+#include "skein_api.h"
+
+int skein_ctx_prepare(struct skein_ctx *ctx, enum skein_size size)
+{
+	skein_assert_ret(ctx && size, SKEIN_FAIL);
+
+	memset(ctx , 0, sizeof(struct skein_ctx));
+	ctx->skein_size = size;
+
+	return SKEIN_SUCCESS;
+}
+
+int skein_init(struct skein_ctx *ctx, size_t hash_bit_len)
+{
+	int ret = SKEIN_FAIL;
+	size_t x_len = 0;
+	u64 *x = NULL;
+	u64 tree_info = SKEIN_CFG_TREE_INFO_SEQUENTIAL;
+
+	skein_assert_ret(ctx, SKEIN_FAIL);
+	/*
+	 * The following two lines rely of the fact that the real Skein
+	 * contexts are a union in out context and thus have tha maximum
+	 * memory available.  The beauty of C :-) .
+	 */
+	x = ctx->m.s256.x;
+	x_len = ctx->skein_size/8;
+	/*
+	 * If size is the same and hash bit length is zero then reuse
+	 * the save chaining variables.
+	 */
+	switch (ctx->skein_size) {
+	case SKEIN_256:
+		ret = skein_256_init_ext(&ctx->m.s256, hash_bit_len,
+					 tree_info, NULL, 0);
+		break;
+	case SKEIN_512:
+		ret = skein_512_init_ext(&ctx->m.s512, hash_bit_len,
+					 tree_info, NULL, 0);
+		break;
+	case SKEIN_1024:
+		ret = skein_1024_init_ext(&ctx->m.s1024, hash_bit_len,
+					  tree_info, NULL, 0);
+		break;
+	}
+
+	if (ret == SKEIN_SUCCESS) {
+		/*
+		 * Save chaining variables for this combination of size and
+		 * hash_bit_len
+		 */
+		memcpy(ctx->x_save, x, x_len);
+	}
+	return ret;
+}
+
+int skein_mac_init(struct skein_ctx *ctx, const u8 *key, size_t key_len,
+		   size_t hash_bit_len)
+{
+	int ret = SKEIN_FAIL;
+	u64 *x = NULL;
+	size_t x_len = 0;
+	u64 tree_info = SKEIN_CFG_TREE_INFO_SEQUENTIAL;
+
+	skein_assert_ret(ctx, SKEIN_FAIL);
+
+	x = ctx->m.s256.x;
+	x_len = ctx->skein_size/8;
+
+	skein_assert_ret(hash_bit_len, SKEIN_BAD_HASHLEN);
+
+	switch (ctx->skein_size) {
+	case SKEIN_256:
+		ret = skein_256_init_ext(&ctx->m.s256, hash_bit_len,
+					 tree_info,
+					 (const u8 *)key, key_len);
+
+		break;
+	case SKEIN_512:
+		ret = skein_512_init_ext(&ctx->m.s512, hash_bit_len,
+					 tree_info,
+					 (const u8 *)key, key_len);
+		break;
+	case SKEIN_1024:
+		ret = skein_1024_init_ext(&ctx->m.s1024, hash_bit_len,
+					  tree_info,
+					  (const u8 *)key, key_len);
+
+		break;
+	}
+	if (ret == SKEIN_SUCCESS) {
+		/*
+		 * Save chaining variables for this combination of key,
+		 * key_len, hash_bit_len
+		 */
+		memcpy(ctx->x_save, x, x_len);
+	}
+	return ret;
+}
+
+void skein_reset(struct skein_ctx *ctx)
+{
+	size_t x_len = 0;
+	u64 *x = NULL;
+
+	/*
+	 * The following two lines rely of the fact that the real Skein
+	 * contexts are a union in out context and thus have tha maximum
+	 * memory available.  The beautiy of C :-) .
+	 */
+	x = ctx->m.s256.x;
+	x_len = ctx->skein_size/8;
+	/* Restore the chaing variable, reset byte counter */
+	memcpy(x, ctx->x_save, x_len);
+
+	/* Setup context to process the message */
+	skein_start_new_type(&ctx->m, MSG);
+}
+
+int skein_update(struct skein_ctx *ctx, const u8 *msg,
+		 size_t msg_byte_cnt)
+{
+	int ret = SKEIN_FAIL;
+
+	skein_assert_ret(ctx, SKEIN_FAIL);
+
+	switch (ctx->skein_size) {
+	case SKEIN_256:
+		ret = skein_256_update(&ctx->m.s256, (const u8 *)msg,
+				       msg_byte_cnt);
+		break;
+	case SKEIN_512:
+		ret = skein_512_update(&ctx->m.s512, (const u8 *)msg,
+				       msg_byte_cnt);
+		break;
+	case SKEIN_1024:
+		ret = skein_1024_update(&ctx->m.s1024, (const u8 *)msg,
+					msg_byte_cnt);
+		break;
+	}
+	return ret;
+
+}
+
+int skein_update_bits(struct skein_ctx *ctx, const u8 *msg,
+		      size_t msg_bit_cnt)
+{
+	/*
+	 * I've used the bit pad implementation from skein_test.c (see NIST CD)
+	 * and modified it to use the convenience functions and added some
+	 * pointer arithmetic.
+	 */
+	size_t length;
+	u8 mask;
+	u8 *up;
+
+	/*
+	 * only the final Update() call is allowed do partial bytes, else
+	 * assert an error
+	 */
+	skein_assert_ret((ctx->m.h.T[1] & SKEIN_T1_FLAG_BIT_PAD) == 0 ||
+			 msg_bit_cnt == 0, SKEIN_FAIL);
+
+	/* if number of bits is a multiple of bytes - that's easy */
+	if ((msg_bit_cnt & 0x7) == 0)
+		return skein_update(ctx, msg, msg_bit_cnt >> 3);
+
+	skein_update(ctx, msg, (msg_bit_cnt >> 3) + 1);
+
+	/*
+	 * The next line rely on the fact that the real Skein contexts
+	 * are a union in our context. After the addition the pointer points to
+	 * Skein's real partial block buffer.
+	 * If this layout ever changes we have to adapt this as well.
+	 */
+	up = (u8 *)ctx->m.s256.x + ctx->skein_size / 8;
+
+	/* set tweak flag for the skein_final call */
+	skein_set_bit_pad_flag(ctx->m.h);
+
+	/* now "pad" the final partial byte the way NIST likes */
+	/* get the b_cnt value (same location for all block sizes) */
+	length = ctx->m.h.b_cnt;
+	/* internal sanity check: there IS a partial byte in the buffer! */
+	skein_assert(length != 0);
+	/* partial byte bit mask */
+	mask = (u8) (1u << (7 - (msg_bit_cnt & 7)));
+	/* apply bit padding on final byte (in the buffer) */
+	up[length-1]  = (u8)((up[length-1] & (0-mask))|mask);
+
+	return SKEIN_SUCCESS;
+}
+
+int skein_final(struct skein_ctx *ctx, u8 *hash)
+{
+	int ret = SKEIN_FAIL;
+
+	skein_assert_ret(ctx, SKEIN_FAIL);
+
+	switch (ctx->skein_size) {
+	case SKEIN_256:
+		ret = skein_256_final(&ctx->m.s256, (u8 *)hash);
+		break;
+	case SKEIN_512:
+		ret = skein_512_final(&ctx->m.s512, (u8 *)hash);
+		break;
+	case SKEIN_1024:
+		ret = skein_1024_final(&ctx->m.s1024, (u8 *)hash);
+		break;
+	}
+	return ret;
+}
diff --git a/drivers/staging/skein/skein_api.h b/drivers/staging/skein/skein_api.h
new file mode 100644
index 00000000000..e02fa19d945
--- /dev/null
+++ b/drivers/staging/skein/skein_api.h
@@ -0,0 +1,230 @@
+/*
+Copyright (c) 2010 Werner Dittmann
+
+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 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 SKEINAPI_H
+#define SKEINAPI_H
+
+/**
+ * @file skein_api.h
+ * @brief A Skein API and its functions.
+ * @{
+ *
+ * This API and the functions that implement this API simplify the usage
+ * of Skein. The design and the way to use the functions follow the openSSL
+ * design but at the same time take care of some Skein specific behaviour
+ * and possibilities.
+ *
+ * The functions enable applications to create a normal Skein hashes and
+ * message authentication codes (MAC).
+ *
+ * Using these functions is simple and straight forward:
+ *
+ * @code
+ *
+ * #include "skein_api.h"
+ *
+ * ...
+ * struct skein_ctx ctx;             // a Skein hash or MAC context
+ *
+ * // prepare context, here for a Skein with a state size of 512 bits.
+ * skein_ctx_prepare(&ctx, SKEIN_512);
+ *
+ * // Initialize the context to set the requested hash length in bits
+ * // here request a output hash size of 31 bits (Skein supports variable
+ * // output sizes even very strange sizes)
+ * skein_init(&ctx, 31);
+ *
+ * // Now update Skein with any number of message bits. A function that
+ * // takes a number of bytes is also available.
+ * skein_update_bits(&ctx, message, msg_length);
+ *
+ * // Now get the result of the Skein hash. The output buffer must be
+ * // large enough to hold the request number of output bits. The application
+ * // may now extract the bits.
+ * skein_final(&ctx, result);
+ * ...
+ * @endcode
+ *
+ * An application may use @c skein_reset to reset a Skein context and use
+ * it for creation of another hash with the same Skein state size and output
+ * bit length. In this case the API implementation restores some internal
+ * internal state data and saves a full Skein initialization round.
+ *
+ * To create a MAC the application just uses @c skein_mac_init instead of
+ * @c skein_init. All other functions calls remain the same.
+ *
+ */
+
+#include <linux/types.h>
+#include "skein.h"
+
+/**
+ * Which Skein size to use
+ */
+enum skein_size {
+	SKEIN_256 = 256,     /*!< Skein with 256 bit state */
+	SKEIN_512 = 512,     /*!< Skein with 512 bit state */
+	SKEIN_1024 = 1024    /*!< Skein with 1024 bit state */
+};
+
+/**
+ * Context for Skein.
+ *
+ * This structure was setup with some know-how of the internal
+ * Skein structures, in particular ordering of header and size dependent
+ * variables. If Skein implementation changes this, then adapt these
+ * structures as well.
+ */
+struct skein_ctx {
+	u64 skein_size;
+	u64 x_save[SKEIN_MAX_STATE_WORDS];   /* save area for state variables */
+	union {
+		struct skein_ctx_hdr h;
+		struct skein_256_ctx s256;
+		struct skein_512_ctx s512;
+		struct skein_1024_ctx s1024;
+	} m;
+};
+
+/**
+ * Prepare a Skein context.
+ *
+ * An application must call this function before it can use the Skein
+ * context. The functions clears memory and initializes size dependent
+ * variables.
+ *
+ * @param ctx
+ *     Pointer to a Skein context.
+ * @param size
+ *     Which Skein size to use.
+ * @return
+ *     SKEIN_SUCESS of SKEIN_FAIL
+ */
+int skein_ctx_prepare(struct skein_ctx *ctx, enum skein_size size);
+
+/**
+ * Initialize a Skein context.
+ *
+ * Initializes the context with this data and saves the resulting Skein
+ * state variables for further use.
+ *
+ * @param ctx
+ *     Pointer to a Skein context.
+ * @param hash_bit_len
+ *     Number of MAC hash bits to compute
+ * @return
+ *     SKEIN_SUCESS of SKEIN_FAIL
+ * @see skein_reset
+ */
+int skein_init(struct skein_ctx *ctx, size_t hash_bit_len);
+
+/**
+ * Resets a Skein context for further use.
+ *
+ * Restores the saved chaining variables to reset the Skein context.
+ * Thus applications can reuse the same setup to  process several
+ * messages. This saves a complete Skein initialization cycle.
+ *
+ * @param ctx
+ *     Pointer to a pre-initialized Skein MAC context
+ */
+void skein_reset(struct skein_ctx *ctx);
+
+/**
+ * Initializes a Skein context for MAC usage.
+ *
+ * Initializes the context with this data and saves the resulting Skein
+ * state variables for further use.
+ *
+ * Applications call the normal Skein functions to update the MAC and
+ * get the final result.
+ *
+ * @param ctx
+ *     Pointer to an empty or preinitialized Skein MAC context
+ * @param key
+ *     Pointer to key bytes or NULL
+ * @param key_len
+ *     Length of the key in bytes or zero
+ * @param hash_bit_len
+ *     Number of MAC hash bits to compute
+ * @return
+ *     SKEIN_SUCESS of SKEIN_FAIL
+ */
+int skein_mac_init(struct skein_ctx *ctx, const u8 *key, size_t key_len,
+		   size_t hash_bit_len);
+
+/**
+ * Update Skein with the next part of the message.
+ *
+ * @param ctx
+ *     Pointer to initialized Skein context
+ * @param msg
+ *     Pointer to the message.
+ * @param msg_byte_cnt
+ *     Length of the message in @b bytes
+ * @return
+ *     Success or error code.
+ */
+int skein_update(struct skein_ctx *ctx, const u8 *msg,
+		 size_t msg_byte_cnt);
+
+/**
+ * Update the hash with a message bit string.
+ *
+ * Skein can handle data not only as bytes but also as bit strings of
+ * arbitrary length (up to its maximum design size).
+ *
+ * @param ctx
+ *     Pointer to initialized Skein context
+ * @param msg
+ *     Pointer to the message.
+ * @param msg_bit_cnt
+ *     Length of the message in @b bits.
+ */
+int skein_update_bits(struct skein_ctx *ctx, const u8 *msg,
+		      size_t msg_bit_cnt);
+
+/**
+ * Finalize Skein and return the hash.
+ *
+ * Before an application can reuse a Skein setup the application must
+ * reset the Skein context.
+ *
+ * @param ctx
+ *     Pointer to initialized Skein context
+ * @param hash
+ *     Pointer to buffer that receives the hash. The buffer must be large
+ *     enough to store @c hash_bit_len bits.
+ * @return
+ *     Success or error code.
+ * @see skein_reset
+ */
+int skein_final(struct skein_ctx *ctx, u8 *hash);
+
+/**
+ * @}
+ */
+#endif
diff --git a/drivers/staging/skein/skein_block.c b/drivers/staging/skein/skein_block.c
new file mode 100644
index 00000000000..04ce1d00547
--- /dev/null
+++ b/drivers/staging/skein/skein_block.c
@@ -0,0 +1,777 @@
+/***********************************************************************
+**
+** Implementation of the Skein block functions.
+**
+** Source code author: Doug Whiting, 2008.
+**
+** This algorithm and source code is released to the public domain.
+**
+** Compile-time switches:
+**
+**  SKEIN_USE_ASM  -- set bits (256/512/1024) to select which
+**                    versions use ASM code for block processing
+**                    [default: use C for all block sizes]
+**
+************************************************************************/
+
+#include <linux/string.h>
+#include "skein.h"
+#include "skein_block.h"
+
+#ifndef SKEIN_USE_ASM
+#define SKEIN_USE_ASM   (0) /* default is all C code (no ASM) */
+#endif
+
+#ifndef SKEIN_LOOP
+#define SKEIN_LOOP 001 /* default: unroll 256 and 512, but not 1024 */
+#endif
+
+#define BLK_BITS        (WCNT*64) /* some useful definitions for code here */
+#define KW_TWK_BASE     (0)
+#define KW_KEY_BASE     (3)
+#define ks              (kw + KW_KEY_BASE)
+#define ts              (kw + KW_TWK_BASE)
+
+#ifdef SKEIN_DEBUG
+#define debug_save_tweak(ctx) { \
+                        ctx->h.tweak[0] = ts[0]; ctx->h.tweak[1] = ts[1]; }
+#else
+#define debug_save_tweak(ctx)
+#endif
+
+/*****************************  SKEIN_256 ******************************/
+#if !(SKEIN_USE_ASM & 256)
+void skein_256_process_block(struct skein_256_ctx *ctx, const u8 *blk_ptr,
+			     size_t blk_cnt, size_t byte_cnt_add)
+	{ /* do it in C */
+	enum {
+		WCNT = SKEIN_256_STATE_WORDS
+	};
+#undef  RCNT
+#define RCNT  (SKEIN_256_ROUNDS_TOTAL/8)
+
+#ifdef SKEIN_LOOP /* configure how much to unroll the loop */
+#define SKEIN_UNROLL_256 (((SKEIN_LOOP)/100)%10)
+#else
+#define SKEIN_UNROLL_256 (0)
+#endif
+
+#if SKEIN_UNROLL_256
+#if (RCNT % SKEIN_UNROLL_256)
+#error "Invalid SKEIN_UNROLL_256" /* sanity check on unroll count */
+#endif
+	size_t  r;
+	u64  kw[WCNT+4+RCNT*2]; /* key schedule: chaining vars + tweak + "rot"*/
+#else
+	u64  kw[WCNT+4]; /* key schedule words : chaining vars + tweak */
+#endif
+	u64  X0, X1, X2, X3; /* local copy of context vars, for speed */
+	u64  w[WCNT]; /* local copy of input block */
+#ifdef SKEIN_DEBUG
+	const u64 *X_ptr[4]; /* use for debugging (help cc put Xn in regs) */
+
+	X_ptr[0] = &X0;  X_ptr[1] = &X1;  X_ptr[2] = &X2;  X_ptr[3] = &X3;
+#endif
+	skein_assert(blk_cnt != 0); /* never call with blk_cnt == 0! */
+	ts[0] = ctx->h.tweak[0];
+	ts[1] = ctx->h.tweak[1];
+	do  {
+		/*
+		 * this implementation only supports 2**64 input bytes
+		 * (no carry out here)
+		 */
+		ts[0] += byte_cnt_add; /* update processed length */
+
+		/* precompute the key schedule for this block */
+		ks[0] = ctx->x[0];
+		ks[1] = ctx->x[1];
+		ks[2] = ctx->x[2];
+		ks[3] = ctx->x[3];
+		ks[4] = ks[0] ^ ks[1] ^ ks[2] ^ ks[3] ^ SKEIN_KS_PARITY;
+
+		ts[2] = ts[0] ^ ts[1];
+
+		/* get input block in little-endian format */
+		skein_get64_lsb_first(w, blk_ptr, WCNT);
+		debug_save_tweak(ctx);
+		skein_show_block(BLK_BITS, &ctx->h, ctx->x, blk_ptr, w, ks, ts);
+
+		X0 = w[0] + ks[0]; /* do the first full key injection */
+		X1 = w[1] + ks[1] + ts[0];
+		X2 = w[2] + ks[2] + ts[1];
+		X3 = w[3] + ks[3];
+
+		/* show starting state values */
+		skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INITIAL,
+				 x_ptr);
+
+		blk_ptr += SKEIN_256_BLOCK_BYTES;
+
+		/* run the rounds */
+
+#define ROUND256(p0, p1, p2, p3, ROT, r_num)                              \
+do { \
+	X##p0 += X##p1; X##p1 = rotl_64(X##p1, ROT##_0); X##p1 ^= X##p0; \
+	X##p2 += X##p3; X##p3 = rotl_64(X##p3, ROT##_1); X##p3 ^= X##p2; \
+} while (0)
+
+#if SKEIN_UNROLL_256 == 0
+#define R256(p0, p1, p2, p3, ROT, r_num) /* fully unrolled */ \
+do { \
+	ROUND256(p0, p1, p2, p3, ROT, r_num); \
+	skein_show_r_ptr(BLK_BITS, &ctx->h, r_num, X_ptr); \
+} while (0)
+
+#define I256(R) \
+do { \
+	/* inject the key schedule value */ \
+	X0   += ks[((R)+1) % 5]; \
+	X1   += ks[((R)+2) % 5] + ts[((R)+1) % 3]; \
+	X2   += ks[((R)+3) % 5] + ts[((R)+2) % 3]; \
+	X3   += ks[((R)+4) % 5] +     (R)+1;       \
+	skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INJECT, X_ptr); \
+} while (0)
+#else /* looping version */
+#define R256(p0, p1, p2, p3, ROT, r_num) \
+do { \
+	ROUND256(p0, p1, p2, p3, ROT, r_num); \
+	skein_show_r_ptr(BLK_BITS, &ctx->h, 4 * (r - 1) + r_num, X_ptr); \
+} while (0)
+
+#define I256(R) \
+do { \
+	/* inject the key schedule value */ \
+	X0   += ks[r+(R)+0]; \
+	X1   += ks[r+(R)+1] + ts[r+(R)+0]; \
+	X2   += ks[r+(R)+2] + ts[r+(R)+1]; \
+	X3   += ks[r+(R)+3] +    r+(R);    \
+	/* rotate key schedule */ \
+	ks[r + (R) + 4]   = ks[r + (R) - 1]; \
+	ts[r + (R) + 2]   = ts[r + (R) - 1]; \
+	skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INJECT, X_ptr); \
+} while (0)
+
+	for (r = 1; r < 2 * RCNT; r += 2 * SKEIN_UNROLL_256)
+#endif
+		{
+#define R256_8_ROUNDS(R)                  \
+do { \
+		R256(0, 1, 2, 3, R_256_0, 8 * (R) + 1);  \
+		R256(0, 3, 2, 1, R_256_1, 8 * (R) + 2);  \
+		R256(0, 1, 2, 3, R_256_2, 8 * (R) + 3);  \
+		R256(0, 3, 2, 1, R_256_3, 8 * (R) + 4);  \
+		I256(2 * (R));                      \
+		R256(0, 1, 2, 3, R_256_4, 8 * (R) + 5);  \
+		R256(0, 3, 2, 1, R_256_5, 8 * (R) + 6);  \
+		R256(0, 1, 2, 3, R_256_6, 8 * (R) + 7);  \
+		R256(0, 3, 2, 1, R_256_7, 8 * (R) + 8);  \
+		I256(2 * (R) + 1); \
+} while (0)
+
+		R256_8_ROUNDS(0);
+
+#define R256_UNROLL_R(NN) \
+	((SKEIN_UNROLL_256 == 0 && \
+	  SKEIN_256_ROUNDS_TOTAL/8 > (NN)) || \
+	 (SKEIN_UNROLL_256 > (NN)))
+
+	#if   R256_UNROLL_R(1)
+		R256_8_ROUNDS(1);
+	#endif
+	#if   R256_UNROLL_R(2)
+		R256_8_ROUNDS(2);
+	#endif
+	#if   R256_UNROLL_R(3)
+		R256_8_ROUNDS(3);
+	#endif
+	#if   R256_UNROLL_R(4)
+		R256_8_ROUNDS(4);
+	#endif
+	#if   R256_UNROLL_R(5)
+		R256_8_ROUNDS(5);
+	#endif
+	#if   R256_UNROLL_R(6)
+		R256_8_ROUNDS(6);
+	#endif
+	#if   R256_UNROLL_R(7)
+		R256_8_ROUNDS(7);
+	#endif
+	#if   R256_UNROLL_R(8)
+		R256_8_ROUNDS(8);
+	#endif
+	#if   R256_UNROLL_R(9)
+		R256_8_ROUNDS(9);
+	#endif
+	#if   R256_UNROLL_R(10)
+		R256_8_ROUNDS(10);
+	#endif
+	#if   R256_UNROLL_R(11)
+		R256_8_ROUNDS(11);
+	#endif
+	#if   R256_UNROLL_R(12)
+		R256_8_ROUNDS(12);
+	#endif
+	#if   R256_UNROLL_R(13)
+		R256_8_ROUNDS(13);
+	#endif
+	#if   R256_UNROLL_R(14)
+		R256_8_ROUNDS(14);
+	#endif
+	#if  (SKEIN_UNROLL_256 > 14)
+#error  "need more unrolling in skein_256_process_block"
+	#endif
+		}
+		/* do the final "feedforward" xor, update context chaining */
+		ctx->x[0] = X0 ^ w[0];
+		ctx->x[1] = X1 ^ w[1];
+		ctx->x[2] = X2 ^ w[2];
+		ctx->x[3] = X3 ^ w[3];
+
+		skein_show_round(BLK_BITS, &ctx->h, SKEIN_RND_FEED_FWD, ctx->x);
+
+		ts[1] &= ~SKEIN_T1_FLAG_FIRST;
+	} while (--blk_cnt);
+	ctx->h.tweak[0] = ts[0];
+	ctx->h.tweak[1] = ts[1];
+}
+
+#if defined(SKEIN_CODE_SIZE) || defined(SKEIN_PERF)
+size_t skein_256_process_block_code_size(void)
+{
+	return ((u8 *) skein_256_process_block_code_size) -
+		((u8 *) skein_256_process_block);
+}
+unsigned int skein_256_unroll_cnt(void)
+{
+	return SKEIN_UNROLL_256;
+}
+#endif
+#endif
+
+/*****************************  SKEIN_512 ******************************/
+#if !(SKEIN_USE_ASM & 512)
+void skein_512_process_block(struct skein_512_ctx *ctx, const u8 *blk_ptr,
+			     size_t blk_cnt, size_t byte_cnt_add)
+{ /* do it in C */
+	enum {
+		WCNT = SKEIN_512_STATE_WORDS
+	};
+#undef  RCNT
+#define RCNT  (SKEIN_512_ROUNDS_TOTAL/8)
+
+#ifdef SKEIN_LOOP /* configure how much to unroll the loop */
+#define SKEIN_UNROLL_512 (((SKEIN_LOOP)/10)%10)
+#else
+#define SKEIN_UNROLL_512 (0)
+#endif
+
+#if SKEIN_UNROLL_512
+#if (RCNT % SKEIN_UNROLL_512)
+#error "Invalid SKEIN_UNROLL_512" /* sanity check on unroll count */
+#endif
+	size_t  r;
+	u64  kw[WCNT+4+RCNT*2]; /* key sched: chaining vars + tweak + "rot"*/
+#else
+	u64  kw[WCNT+4]; /* key schedule words : chaining vars + tweak */
+#endif
+	u64  X0, X1, X2, X3, X4, X5, X6, X7; /* local copies, for speed */
+	u64  w[WCNT]; /* local copy of input block */
+#ifdef SKEIN_DEBUG
+	const u64 *X_ptr[8]; /* use for debugging (help cc put Xn in regs) */
+
+	X_ptr[0] = &X0;  X_ptr[1] = &X1;  X_ptr[2] = &X2;  X_ptr[3] = &X3;
+	X_ptr[4] = &X4;  X_ptr[5] = &X5;  X_ptr[6] = &X6;  X_ptr[7] = &X7;
+#endif
+
+	skein_assert(blk_cnt != 0); /* never call with blk_cnt == 0! */
+	ts[0] = ctx->h.tweak[0];
+	ts[1] = ctx->h.tweak[1];
+	do  {
+		/*
+		 * this implementation only supports 2**64 input bytes
+		 * (no carry out here)
+		 */
+		ts[0] += byte_cnt_add; /* update processed length */
+
+		/* precompute the key schedule for this block */
+		ks[0] = ctx->x[0];
+		ks[1] = ctx->x[1];
+		ks[2] = ctx->x[2];
+		ks[3] = ctx->x[3];
+		ks[4] = ctx->x[4];
+		ks[5] = ctx->x[5];
+		ks[6] = ctx->x[6];
+		ks[7] = ctx->x[7];
+		ks[8] = ks[0] ^ ks[1] ^ ks[2] ^ ks[3] ^
+			ks[4] ^ ks[5] ^ ks[6] ^ ks[7] ^ SKEIN_KS_PARITY;
+
+		ts[2] = ts[0] ^ ts[1];
+
+		/* get input block in little-endian format */
+		skein_get64_lsb_first(w, blk_ptr, WCNT);
+		debug_save_tweak(ctx);
+		skein_show_block(BLK_BITS, &ctx->h, ctx->x, blk_ptr, w, ks, ts);
+
+		X0   = w[0] + ks[0]; /* do the first full key injection */
+		X1   = w[1] + ks[1];
+		X2   = w[2] + ks[2];
+		X3   = w[3] + ks[3];
+		X4   = w[4] + ks[4];
+		X5   = w[5] + ks[5] + ts[0];
+		X6   = w[6] + ks[6] + ts[1];
+		X7   = w[7] + ks[7];
+
+		blk_ptr += SKEIN_512_BLOCK_BYTES;
+
+		skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INITIAL,
+				 X_ptr);
+		/* run the rounds */
+#define ROUND512(p0, p1, p2, p3, p4, p5, p6, p7, ROT, r_num) \
+do { \
+	X##p0 += X##p1; X##p1 = rotl_64(X##p1, ROT##_0); X##p1 ^= X##p0; \
+	X##p2 += X##p3; X##p3 = rotl_64(X##p3, ROT##_1); X##p3 ^= X##p2; \
+	X##p4 += X##p5; X##p5 = rotl_64(X##p5, ROT##_2); X##p5 ^= X##p4; \
+	X##p6 += X##p7; X##p7 = rotl_64(X##p7, ROT##_3); X##p7 ^= X##p6; \
+} while (0)
+
+#if SKEIN_UNROLL_512 == 0
+#define R512(p0, p1, p2, p3, p4, p5, p6, p7, ROT, r_num) /* unrolled */ \
+do { \
+	ROUND512(p0, p1, p2, p3, p4, p5, p6, p7, ROT, r_num) \
+	skein_show_r_ptr(BLK_BITS, &ctx->h, r_num, X_ptr); \
+} while (0)
+
+#define I512(R) \
+do { \
+	/* inject the key schedule value */ \
+	X0   += ks[((R) + 1) % 9]; \
+	X1   += ks[((R) + 2) % 9]; \
+	X2   += ks[((R) + 3) % 9]; \
+	X3   += ks[((R) + 4) % 9]; \
+	X4   += ks[((R) + 5) % 9]; \
+	X5   += ks[((R) + 6) % 9] + ts[((R) + 1) % 3]; \
+	X6   += ks[((R) + 7) % 9] + ts[((R) + 2) % 3]; \
+	X7   += ks[((R) + 8) % 9] +     (R) + 1;       \
+	skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INJECT, X_ptr); \
+} while (0)
+#else /* looping version */
+#define R512(p0, p1, p2, p3, p4, p5, p6, p7, ROT, r_num) \
+do { \
+	ROUND512(p0, p1, p2, p3, p4, p5, p6, p7, ROT, r_num); \
+	skein_show_r_ptr(BLK_BITS, &ctx->h, 4 * (r - 1) + r_num, X_ptr); \
+} while (0)
+
+#define I512(R) \
+do { \
+	/* inject the key schedule value */ \
+	X0   += ks[r + (R) + 0]; \
+	X1   += ks[r + (R) + 1]; \
+	X2   += ks[r + (R) + 2]; \
+	X3   += ks[r + (R) + 3]; \
+	X4   += ks[r + (R) + 4]; \
+	X5   += ks[r + (R) + 5] + ts[r + (R) + 0]; \
+	X6   += ks[r + (R) + 6] + ts[r + (R) + 1]; \
+	X7   += ks[r + (R) + 7] +         r + (R); \
+	/* rotate key schedule */ \
+	ks[r +         (R) + 8] = ks[r + (R) - 1]; \
+	ts[r +         (R) + 2] = ts[r + (R) - 1]; \
+	skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INJECT, X_ptr); \
+} while (0)
+
+		for (r = 1; r < 2 * RCNT; r += 2 * SKEIN_UNROLL_512)
+#endif /* end of looped code definitions */
+		{
+#define R512_8_ROUNDS(R)  /* do 8 full rounds */  \
+do { \
+		R512(0, 1, 2, 3, 4, 5, 6, 7, R_512_0, 8 * (R) + 1);   \
+		R512(2, 1, 4, 7, 6, 5, 0, 3, R_512_1, 8 * (R) + 2);   \
+		R512(4, 1, 6, 3, 0, 5, 2, 7, R_512_2, 8 * (R) + 3);   \
+		R512(6, 1, 0, 7, 2, 5, 4, 3, R_512_3, 8 * (R) + 4);   \
+		I512(2 * (R));                              \
+		R512(0, 1, 2, 3, 4, 5, 6, 7, R_512_4, 8 * (R) + 5);   \
+		R512(2, 1, 4, 7, 6, 5, 0, 3, R_512_5, 8 * (R) + 6);   \
+		R512(4, 1, 6, 3, 0, 5, 2, 7, R_512_6, 8 * (R) + 7);   \
+		R512(6, 1, 0, 7, 2, 5, 4, 3, R_512_7, 8 * (R) + 8);   \
+		I512(2 * (R) + 1);        /* and key injection */ \
+} while (0)
+
+			R512_8_ROUNDS(0);
+
+#define R512_UNROLL_R(NN) \
+		((SKEIN_UNROLL_512 == 0 && \
+		  SKEIN_512_ROUNDS_TOTAL/8 > (NN)) || \
+		 (SKEIN_UNROLL_512 > (NN)))
+
+	#if   R512_UNROLL_R(1)
+			R512_8_ROUNDS(1);
+	#endif
+	#if   R512_UNROLL_R(2)
+			R512_8_ROUNDS(2);
+	#endif
+	#if   R512_UNROLL_R(3)
+			R512_8_ROUNDS(3);
+	#endif
+	#if   R512_UNROLL_R(4)
+			R512_8_ROUNDS(4);
+	#endif
+	#if   R512_UNROLL_R(5)
+			R512_8_ROUNDS(5);
+	#endif
+	#if   R512_UNROLL_R(6)
+			R512_8_ROUNDS(6);
+	#endif
+	#if   R512_UNROLL_R(7)
+			R512_8_ROUNDS(7);
+	#endif
+	#if   R512_UNROLL_R(8)
+			R512_8_ROUNDS(8);
+	#endif
+	#if   R512_UNROLL_R(9)
+			R512_8_ROUNDS(9);
+	#endif
+	#if   R512_UNROLL_R(10)
+			R512_8_ROUNDS(10);
+	#endif
+	#if   R512_UNROLL_R(11)
+			R512_8_ROUNDS(11);
+	#endif
+	#if   R512_UNROLL_R(12)
+			R512_8_ROUNDS(12);
+	#endif
+	#if   R512_UNROLL_R(13)
+			R512_8_ROUNDS(13);
+	#endif
+	#if   R512_UNROLL_R(14)
+			R512_8_ROUNDS(14);
+	#endif
+	#if  (SKEIN_UNROLL_512 > 14)
+#error  "need more unrolling in skein_512_process_block"
+	#endif
+		}
+
+		/* do the final "feedforward" xor, update context chaining */
+		ctx->x[0] = X0 ^ w[0];
+		ctx->x[1] = X1 ^ w[1];
+		ctx->x[2] = X2 ^ w[2];
+		ctx->x[3] = X3 ^ w[3];
+		ctx->x[4] = X4 ^ w[4];
+		ctx->x[5] = X5 ^ w[5];
+		ctx->x[6] = X6 ^ w[6];
+		ctx->x[7] = X7 ^ w[7];
+		skein_show_round(BLK_BITS, &ctx->h, SKEIN_RND_FEED_FWD, ctx->x);
+
+		ts[1] &= ~SKEIN_T1_FLAG_FIRST;
+	} while (--blk_cnt);
+	ctx->h.tweak[0] = ts[0];
+	ctx->h.tweak[1] = ts[1];
+}
+
+#if defined(SKEIN_CODE_SIZE) || defined(SKEIN_PERF)
+size_t skein_512_process_block_code_size(void)
+{
+	return ((u8 *) skein_512_process_block_code_size) -
+		((u8 *) skein_512_process_block);
+}
+unsigned int skein_512_unroll_cnt(void)
+{
+	return SKEIN_UNROLL_512;
+}
+#endif
+#endif
+
+/*****************************  SKEIN_1024 ******************************/
+#if !(SKEIN_USE_ASM & 1024)
+void skein_1024_process_block(struct skein_1024_ctx *ctx, const u8 *blk_ptr,
+			      size_t blk_cnt, size_t byte_cnt_add)
+{ /* do it in C, always looping (unrolled is bigger AND slower!) */
+	enum {
+		WCNT = SKEIN_1024_STATE_WORDS
+	};
+#undef  RCNT
+#define RCNT  (SKEIN_1024_ROUNDS_TOTAL/8)
+
+#ifdef SKEIN_LOOP /* configure how much to unroll the loop */
+#define SKEIN_UNROLL_1024 ((SKEIN_LOOP)%10)
+#else
+#define SKEIN_UNROLL_1024 (0)
+#endif
+
+#if (SKEIN_UNROLL_1024 != 0)
+#if (RCNT % SKEIN_UNROLL_1024)
+#error "Invalid SKEIN_UNROLL_1024" /* sanity check on unroll count */
+#endif
+	size_t  r;
+	u64  kw[WCNT+4+RCNT*2]; /* key sched: chaining vars + tweak + "rot" */
+#else
+	u64  kw[WCNT+4]; /* key schedule words : chaining vars + tweak */
+#endif
+
+	/* local copy of vars, for speed */
+	u64  X00, X01, X02, X03, X04, X05, X06, X07,
+	     X08, X09, X10, X11, X12, X13, X14, X15;
+	u64  w[WCNT]; /* local copy of input block */
+#ifdef SKEIN_DEBUG
+	const u64 *X_ptr[16]; /* use for debugging (help cc put Xn in regs) */
+
+	X_ptr[0]  = &X00;  X_ptr[1]  = &X01;  X_ptr[2]  = &X02;
+	X_ptr[3]  = &X03;  X_ptr[4]  = &X04;  X_ptr[5]  = &X05;
+	X_ptr[6]  = &X06;  X_ptr[7]  = &X07;  X_ptr[8]  = &X08;
+	X_ptr[9]  = &X09;  X_ptr[10] = &X10;  X_ptr[11] = &X11;
+	X_ptr[12] = &X12;  X_ptr[13] = &X13;  X_ptr[14] = &X14;
+	X_ptr[15] = &X15;
+#endif
+
+	skein_assert(blk_cnt != 0); /* never call with blk_cnt == 0! */
+	ts[0] = ctx->h.tweak[0];
+	ts[1] = ctx->h.tweak[1];
+	do  {
+		/*
+		 * this implementation only supports 2**64 input bytes
+		 * (no carry out here)
+		 */
+		ts[0] += byte_cnt_add; /* update processed length */
+
+		/* precompute the key schedule for this block */
+		ks[0]  = ctx->x[0];
+		ks[1]  = ctx->x[1];
+		ks[2]  = ctx->x[2];
+		ks[3]  = ctx->x[3];
+		ks[4]  = ctx->x[4];
+		ks[5]  = ctx->x[5];
+		ks[6]  = ctx->x[6];
+		ks[7]  = ctx->x[7];
+		ks[8]  = ctx->x[8];
+		ks[9]  = ctx->x[9];
+		ks[10] = ctx->x[10];
+		ks[11] = ctx->x[11];
+		ks[12] = ctx->x[12];
+		ks[13] = ctx->x[13];
+		ks[14] = ctx->x[14];
+		ks[15] = ctx->x[15];
+		ks[16] =  ks[0] ^  ks[1] ^  ks[2] ^  ks[3] ^
+			  ks[4] ^  ks[5] ^  ks[6] ^  ks[7] ^
+			  ks[8] ^  ks[9] ^ ks[10] ^ ks[11] ^
+			  ks[12] ^ ks[13] ^ ks[14] ^ ks[15] ^ SKEIN_KS_PARITY;
+
+		ts[2]  = ts[0] ^ ts[1];
+
+		/* get input block in little-endian format */
+		skein_get64_lsb_first(w, blk_ptr, WCNT);
+		debug_save_tweak(ctx);
+		skein_show_block(BLK_BITS, &ctx->h, ctx->x, blk_ptr, w, ks, ts);
+
+		X00    =  w[0] +  ks[0]; /* do the first full key injection */
+		X01    =  w[1] +  ks[1];
+		X02    =  w[2] +  ks[2];
+		X03    =  w[3] +  ks[3];
+		X04    =  w[4] +  ks[4];
+		X05    =  w[5] +  ks[5];
+		X06    =  w[6] +  ks[6];
+		X07    =  w[7] +  ks[7];
+		X08    =  w[8] +  ks[8];
+		X09    =  w[9] +  ks[9];
+		X10    = w[10] + ks[10];
+		X11    = w[11] + ks[11];
+		X12    = w[12] + ks[12];
+		X13    = w[13] + ks[13] + ts[0];
+		X14    = w[14] + ks[14] + ts[1];
+		X15    = w[15] + ks[15];
+
+		skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INITIAL,
+				 X_ptr);
+
+#define ROUND1024(p0, p1, p2, p3, p4, p5, p6, p7, p8, p9, pA, pB, pC, pD, pE, \
+			pF, ROT, r_num) \
+do { \
+	X##p0 += X##p1; X##p1 = rotl_64(X##p1, ROT##_0); X##p1 ^= X##p0;   \
+	X##p2 += X##p3; X##p3 = rotl_64(X##p3, ROT##_1); X##p3 ^= X##p2;   \
+	X##p4 += X##p5; X##p5 = rotl_64(X##p5, ROT##_2); X##p5 ^= X##p4;   \
+	X##p6 += X##p7; X##p7 = rotl_64(X##p7, ROT##_3); X##p7 ^= X##p6;   \
+	X##p8 += X##p9; X##p9 = rotl_64(X##p9, ROT##_4); X##p9 ^= X##p8;   \
+	X##pA += X##pB; X##pB = rotl_64(X##pB, ROT##_5); X##pB ^= X##pA;   \
+	X##pC += X##pD; X##pD = rotl_64(X##pD, ROT##_6); X##pD ^= X##pC;   \
+	X##pE += X##pF; X##pF = rotl_64(X##pF, ROT##_7); X##pF ^= X##pE;   \
+} while (0)
+
+#if SKEIN_UNROLL_1024 == 0
+#define R1024(p0, p1, p2, p3, p4, p5, p6, p7, p8, p9, pA, pB, pC, pD, pE, pF, \
+		ROT, rn) \
+do { \
+	ROUND1024(p0, p1, p2, p3, p4, p5, p6, p7, p8, p9, pA, pB, pC, pD, pE, \
+			pF, ROT, rn); \
+	skein_show_r_ptr(BLK_BITS, &ctx->h, rn, X_ptr); \
+} while (0)
+
+#define I1024(R) \
+do { \
+	/* inject the key schedule value */ \
+	X00   += ks[((R) +  1) % 17]; \
+	X01   += ks[((R) +  2) % 17]; \
+	X02   += ks[((R) +  3) % 17]; \
+	X03   += ks[((R) +  4) % 17]; \
+	X04   += ks[((R) +  5) % 17]; \
+	X05   += ks[((R) +  6) % 17]; \
+	X06   += ks[((R) +  7) % 17]; \
+	X07   += ks[((R) +  8) % 17]; \
+	X08   += ks[((R) +  9) % 17]; \
+	X09   += ks[((R) + 10) % 17]; \
+	X10   += ks[((R) + 11) % 17]; \
+	X11   += ks[((R) + 12) % 17]; \
+	X12   += ks[((R) + 13) % 17]; \
+	X13   += ks[((R) + 14) % 17] + ts[((R) + 1) % 3]; \
+	X14   += ks[((R) + 15) % 17] + ts[((R) + 2) % 3]; \
+	X15   += ks[((R) + 16) % 17] +     (R) + 1;       \
+	skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INJECT, X_ptr); \
+} while (0)
+#else /* looping version */
+#define R1024(p0, p1, p2, p3, p4, p5, p6, p7, p8, p9, pA, pB, pC, pD, pE, pF, \
+		ROT, rn) \
+do { \
+	ROUND1024(p0, p1, p2, p3, p4, p5, p6, p7, p8, p9, pA, pB, pC, pD, pE, \
+			pF, ROT, rn); \
+	skein_show_r_ptr(BLK_BITS, &ctx->h, 4 * (r - 1) + rn, X_ptr); \
+} while (0)
+
+#define I1024(R) \
+do { \
+	/* inject the key schedule value */ \
+	X00   += ks[r + (R) +  0]; \
+	X01   += ks[r + (R) +  1]; \
+	X02   += ks[r + (R) +  2]; \
+	X03   += ks[r + (R) +  3]; \
+	X04   += ks[r + (R) +  4]; \
+	X05   += ks[r + (R) +  5]; \
+	X06   += ks[r + (R) +  6]; \
+	X07   += ks[r + (R) +  7]; \
+	X08   += ks[r + (R) +  8]; \
+	X09   += ks[r + (R) +  9]; \
+	X10   += ks[r + (R) + 10]; \
+	X11   += ks[r + (R) + 11]; \
+	X12   += ks[r + (R) + 12]; \
+	X13   += ks[r + (R) + 13] + ts[r + (R) + 0]; \
+	X14   += ks[r + (R) + 14] + ts[r + (R) + 1]; \
+	X15   += ks[r + (R) + 15] +         r + (R); \
+	/* rotate key schedule */ \
+	ks[r  +         (R) + 16] = ks[r + (R) - 1]; \
+	ts[r  +         (R) +  2] = ts[r + (R) - 1]; \
+	skein_show_r_ptr(BLK_BITSi, &ctx->h, SKEIN_RND_KEY_INJECT, X_ptr); \
+} while (0)
+
+		for (r = 1; r <= 2 * RCNT; r += 2 * SKEIN_UNROLL_1024)
+#endif
+		{
+#define R1024_8_ROUNDS(R) \
+do { \
+	R1024(00, 01, 02, 03, 04, 05, 06, 07, 08, 09, 10, 11, 12, 13, 14, 15, \
+		R1024_0, 8*(R) + 1); \
+	R1024(00, 09, 02, 13, 06, 11, 04, 15, 10, 07, 12, 03, 14, 05, 08, 01, \
+		R1024_1, 8*(R) + 2); \
+	R1024(00, 07, 02, 05, 04, 03, 06, 01, 12, 15, 14, 13, 08, 11, 10, 09, \
+		R1024_2, 8*(R) + 3); \
+	R1024(00, 15, 02, 11, 06, 13, 04, 09, 14, 01, 08, 05, 10, 03, 12, 07, \
+		R1024_3, 8*(R) + 4); \
+	I1024(2*(R)); \
+	R1024(00, 01, 02, 03, 04, 05, 06, 07, 08, 09, 10, 11, 12, 13, 14, 15, \
+		R1024_4, 8*(R) + 5); \
+	R1024(00, 09, 02, 13, 06, 11, 04, 15, 10, 07, 12, 03, 14, 05, 08, 01, \
+		R1024_5, 8*(R) + 6); \
+	R1024(00, 07, 02, 05, 04, 03, 06, 01, 12, 15, 14, 13, 08, 11, 10, 09, \
+		R1024_6, 8*(R) + 7); \
+	R1024(00, 15, 02, 11, 06, 13, 04, 09, 14, 01, 08, 05, 10, 03, 12, 07, \
+		R1024_7, 8*(R) + 8); \
+	I1024(2*(R)+1); \
+} while (0)
+
+			R1024_8_ROUNDS(0);
+
+#define R1024_UNROLL_R(NN) \
+		((SKEIN_UNROLL_1024 == 0 && \
+		  SKEIN_1024_ROUNDS_TOTAL/8 > (NN)) || \
+		 (SKEIN_UNROLL_1024 > (NN)))
+
+	#if   R1024_UNROLL_R(1)
+			R1024_8_ROUNDS(1);
+	#endif
+	#if   R1024_UNROLL_R(2)
+			R1024_8_ROUNDS(2);
+	#endif
+	#if   R1024_UNROLL_R(3)
+			R1024_8_ROUNDS(3);
+	#endif
+	#if   R1024_UNROLL_R(4)
+			R1024_8_ROUNDS(4);
+	#endif
+	#if   R1024_UNROLL_R(5)
+			R1024_8_ROUNDS(5);
+	#endif
+	#if   R1024_UNROLL_R(6)
+			R1024_8_ROUNDS(6);
+	#endif
+	#if   R1024_UNROLL_R(7)
+			R1024_8_ROUNDS(7);
+	#endif
+	#if   R1024_UNROLL_R(8)
+			R1024_8_ROUNDS(8);
+	#endif
+	#if   R1024_UNROLL_R(9)
+			R1024_8_ROUNDS(9);
+	#endif
+	#if   R1024_UNROLL_R(10)
+			R1024_8_ROUNDS(10);
+	#endif
+	#if   R1024_UNROLL_R(11)
+			R1024_8_ROUNDS(11);
+	#endif
+	#if   R1024_UNROLL_R(12)
+			R1024_8_ROUNDS(12);
+	#endif
+	#if   R1024_UNROLL_R(13)
+			R1024_8_ROUNDS(13);
+	#endif
+	#if   R1024_UNROLL_R(14)
+			R1024_8_ROUNDS(14);
+	#endif
+#if  (SKEIN_UNROLL_1024 > 14)
+#error  "need more unrolling in Skein_1024_Process_Block"
+  #endif
+		}
+		/* do the final "feedforward" xor, update context chaining */
+
+		ctx->x[0] = X00 ^ w[0];
+		ctx->x[1] = X01 ^ w[1];
+		ctx->x[2] = X02 ^ w[2];
+		ctx->x[3] = X03 ^ w[3];
+		ctx->x[4] = X04 ^ w[4];
+		ctx->x[5] = X05 ^ w[5];
+		ctx->x[6] = X06 ^ w[6];
+		ctx->x[7] = X07 ^ w[7];
+		ctx->x[8] = X08 ^ w[8];
+		ctx->x[9] = X09 ^ w[9];
+		ctx->x[10] = X10 ^ w[10];
+		ctx->x[11] = X11 ^ w[11];
+		ctx->x[12] = X12 ^ w[12];
+		ctx->x[13] = X13 ^ w[13];
+		ctx->x[14] = X14 ^ w[14];
+		ctx->x[15] = X15 ^ w[15];
+
+		skein_show_round(BLK_BITS, &ctx->h, SKEIN_RND_FEED_FWD, ctx->x);
+
+		ts[1] &= ~SKEIN_T1_FLAG_FIRST;
+		blk_ptr += SKEIN_1024_BLOCK_BYTES;
+	} while (--blk_cnt);
+	ctx->h.tweak[0] = ts[0];
+	ctx->h.tweak[1] = ts[1];
+}
+
+#if defined(SKEIN_CODE_SIZE) || defined(SKEIN_PERF)
+size_t skein_1024_process_block_code_size(void)
+{
+	return ((u8 *) skein_1024_process_block_code_size) -
+		((u8 *) skein_1024_process_block);
+}
+unsigned int skein_1024_unroll_cnt(void)
+{
+	return SKEIN_UNROLL_1024;
+}
+#endif
+#endif
diff --git a/drivers/staging/skein/skein_block.h b/drivers/staging/skein/skein_block.h
new file mode 100644
index 00000000000..bd7bdc35df2
--- /dev/null
+++ b/drivers/staging/skein/skein_block.h
@@ -0,0 +1,22 @@
+/***********************************************************************
+**
+** Implementation of the Skein hash function.
+**
+** Source code author: Doug Whiting, 2008.
+**
+** This algorithm and source code is released to the public domain.
+**
+************************************************************************/
+#ifndef _SKEIN_BLOCK_H_
+#define _SKEIN_BLOCK_H_
+
+#include "skein.h" /* get the Skein API definitions   */
+
+void skein_256_process_block(struct skein_256_ctx *ctx, const u8 *blk_ptr,
+			     size_t blk_cnt, size_t byte_cnt_add);
+void skein_512_process_block(struct skein_512_ctx *ctx, const u8 *blk_ptr,
+			     size_t blk_cnt, size_t byte_cnt_add);
+void skein_1024_process_block(struct skein_1024_ctx *ctx, const u8 *blk_ptr,
+			      size_t blk_cnt, size_t byte_cnt_add);
+
+#endif
diff --git a/drivers/staging/skein/skein_iv.h b/drivers/staging/skein/skein_iv.h
new file mode 100644
index 00000000000..a03703deeaf
--- /dev/null
+++ b/drivers/staging/skein/skein_iv.h
@@ -0,0 +1,186 @@
+#ifndef _SKEIN_IV_H_
+#define _SKEIN_IV_H_
+
+#include "skein.h"    /* get Skein macros and types */
+
+/*
+***************** Pre-computed Skein IVs *******************
+**
+** NOTE: these values are not "magic" constants, but
+** are generated using the Threefish block function.
+** They are pre-computed here only for speed; i.e., to
+** avoid the need for a Threefish call during Init().
+**
+** The IV for any fixed hash length may be pre-computed.
+** Only the most common values are included here.
+**
+************************************************************
+**/
+
+#define MK_64 SKEIN_MK_64
+
+/* blkSize =  256 bits. hashSize =  128 bits */
+const u64 SKEIN_256_IV_128[] = {
+	MK_64(0xE1111906, 0x964D7260),
+	MK_64(0x883DAAA7, 0x7C8D811C),
+	MK_64(0x10080DF4, 0x91960F7A),
+	MK_64(0xCCF7DDE5, 0xB45BC1C2)
+};
+
+/* blkSize =  256 bits. hashSize =  160 bits */
+const u64 SKEIN_256_IV_160[] = {
+	MK_64(0x14202314, 0x72825E98),
+	MK_64(0x2AC4E9A2, 0x5A77E590),
+	MK_64(0xD47A5856, 0x8838D63E),
+	MK_64(0x2DD2E496, 0x8586AB7D)
+};
+
+/* blkSize =  256 bits. hashSize =  224 bits */
+const u64 SKEIN_256_IV_224[] = {
+	MK_64(0xC6098A8C, 0x9AE5EA0B),
+	MK_64(0x876D5686, 0x08C5191C),
+	MK_64(0x99CB88D7, 0xD7F53884),
+	MK_64(0x384BDDB1, 0xAEDDB5DE)
+};
+
+/* blkSize =  256 bits. hashSize =  256 bits */
+const u64 SKEIN_256_IV_256[] = {
+	MK_64(0xFC9DA860, 0xD048B449),
+	MK_64(0x2FCA6647, 0x9FA7D833),
+	MK_64(0xB33BC389, 0x6656840F),
+	MK_64(0x6A54E920, 0xFDE8DA69)
+};
+
+/* blkSize =  512 bits. hashSize =  128 bits */
+const u64 SKEIN_512_IV_128[] = {
+	MK_64(0xA8BC7BF3, 0x6FBF9F52),
+	MK_64(0x1E9872CE, 0xBD1AF0AA),
+	MK_64(0x309B1790, 0xB32190D3),
+	MK_64(0xBCFBB854, 0x3F94805C),
+	MK_64(0x0DA61BCD, 0x6E31B11B),
+	MK_64(0x1A18EBEA, 0xD46A32E3),
+	MK_64(0xA2CC5B18, 0xCE84AA82),
+	MK_64(0x6982AB28, 0x9D46982D)
+};
+
+/* blkSize =  512 bits. hashSize =  160 bits */
+const u64 SKEIN_512_IV_160[] = {
+	MK_64(0x28B81A2A, 0xE013BD91),
+	MK_64(0xC2F11668, 0xB5BDF78F),
+	MK_64(0x1760D8F3, 0xF6A56F12),
+	MK_64(0x4FB74758, 0x8239904F),
+	MK_64(0x21EDE07F, 0x7EAF5056),
+	MK_64(0xD908922E, 0x63ED70B8),
+	MK_64(0xB8EC76FF, 0xECCB52FA),
+	MK_64(0x01A47BB8, 0xA3F27A6E)
+};
+
+/* blkSize =  512 bits. hashSize =  224 bits */
+const u64 SKEIN_512_IV_224[] = {
+	MK_64(0xCCD06162, 0x48677224),
+	MK_64(0xCBA65CF3, 0xA92339EF),
+	MK_64(0x8CCD69D6, 0x52FF4B64),
+	MK_64(0x398AED7B, 0x3AB890B4),
+	MK_64(0x0F59D1B1, 0x457D2BD0),
+	MK_64(0x6776FE65, 0x75D4EB3D),
+	MK_64(0x99FBC70E, 0x997413E9),
+	MK_64(0x9E2CFCCF, 0xE1C41EF7)
+};
+
+/* blkSize =  512 bits. hashSize =  256 bits */
+const u64 SKEIN_512_IV_256[] = {
+	MK_64(0xCCD044A1, 0x2FDB3E13),
+	MK_64(0xE8359030, 0x1A79A9EB),
+	MK_64(0x55AEA061, 0x4F816E6F),
+	MK_64(0x2A2767A4, 0xAE9B94DB),
+	MK_64(0xEC06025E, 0x74DD7683),
+	MK_64(0xE7A436CD, 0xC4746251),
+	MK_64(0xC36FBAF9, 0x393AD185),
+	MK_64(0x3EEDBA18, 0x33EDFC13)
+};
+
+/* blkSize =  512 bits. hashSize =  384 bits */
+const u64 SKEIN_512_IV_384[] = {
+	MK_64(0xA3F6C6BF, 0x3A75EF5F),
+	MK_64(0xB0FEF9CC, 0xFD84FAA4),
+	MK_64(0x9D77DD66, 0x3D770CFE),
+	MK_64(0xD798CBF3, 0xB468FDDA),
+	MK_64(0x1BC4A666, 0x8A0E4465),
+	MK_64(0x7ED7D434, 0xE5807407),
+	MK_64(0x548FC1AC, 0xD4EC44D6),
+	MK_64(0x266E1754, 0x6AA18FF8)
+};
+
+/* blkSize =  512 bits. hashSize =  512 bits */
+const u64 SKEIN_512_IV_512[] = {
+	MK_64(0x4903ADFF, 0x749C51CE),
+	MK_64(0x0D95DE39, 0x9746DF03),
+	MK_64(0x8FD19341, 0x27C79BCE),
+	MK_64(0x9A255629, 0xFF352CB1),
+	MK_64(0x5DB62599, 0xDF6CA7B0),
+	MK_64(0xEABE394C, 0xA9D5C3F4),
+	MK_64(0x991112C7, 0x1A75B523),
+	MK_64(0xAE18A40B, 0x660FCC33)
+};
+
+/* blkSize = 1024 bits. hashSize =  384 bits */
+const u64 SKEIN_1024_IV_384[] = {
+	MK_64(0x5102B6B8, 0xC1894A35),
+	MK_64(0xFEEBC9E3, 0xFE8AF11A),
+	MK_64(0x0C807F06, 0xE32BED71),
+	MK_64(0x60C13A52, 0xB41A91F6),
+	MK_64(0x9716D35D, 0xD4917C38),
+	MK_64(0xE780DF12, 0x6FD31D3A),
+	MK_64(0x797846B6, 0xC898303A),
+	MK_64(0xB172C2A8, 0xB3572A3B),
+	MK_64(0xC9BC8203, 0xA6104A6C),
+	MK_64(0x65909338, 0xD75624F4),
+	MK_64(0x94BCC568, 0x4B3F81A0),
+	MK_64(0x3EBBF51E, 0x10ECFD46),
+	MK_64(0x2DF50F0B, 0xEEB08542),
+	MK_64(0x3B5A6530, 0x0DBC6516),
+	MK_64(0x484B9CD2, 0x167BBCE1),
+	MK_64(0x2D136947, 0xD4CBAFEA)
+};
+
+/* blkSize = 1024 bits. hashSize =  512 bits */
+const u64 SKEIN_1024_IV_512[] = {
+	MK_64(0xCAEC0E5D, 0x7C1B1B18),
+	MK_64(0xA01B0E04, 0x5F03E802),
+	MK_64(0x33840451, 0xED912885),
+	MK_64(0x374AFB04, 0xEAEC2E1C),
+	MK_64(0xDF25A0E2, 0x813581F7),
+	MK_64(0xE4004093, 0x8B12F9D2),
+	MK_64(0xA662D539, 0xC2ED39B6),
+	MK_64(0xFA8B85CF, 0x45D8C75A),
+	MK_64(0x8316ED8E, 0x29EDE796),
+	MK_64(0x053289C0, 0x2E9F91B8),
+	MK_64(0xC3F8EF1D, 0x6D518B73),
+	MK_64(0xBDCEC3C4, 0xD5EF332E),
+	MK_64(0x549A7E52, 0x22974487),
+	MK_64(0x67070872, 0x5B749816),
+	MK_64(0xB9CD28FB, 0xF0581BD1),
+	MK_64(0x0E2940B8, 0x15804974)
+};
+
+/* blkSize = 1024 bits. hashSize = 1024 bits */
+const u64 SKEIN_1024_IV_1024[] = {
+	MK_64(0xD593DA07, 0x41E72355),
+	MK_64(0x15B5E511, 0xAC73E00C),
+	MK_64(0x5180E5AE, 0xBAF2C4F0),
+	MK_64(0x03BD41D3, 0xFCBCAFAF),
+	MK_64(0x1CAEC6FD, 0x1983A898),
+	MK_64(0x6E510B8B, 0xCDD0589F),
+	MK_64(0x77E2BDFD, 0xC6394ADA),
+	MK_64(0xC11E1DB5, 0x24DCB0A3),
+	MK_64(0xD6D14AF9, 0xC6329AB5),
+	MK_64(0x6A9B0BFC, 0x6EB67E0D),
+	MK_64(0x9243C60D, 0xCCFF1332),
+	MK_64(0x1A1F1DDE, 0x743F02D4),
+	MK_64(0x0996753C, 0x10ED0BB8),
+	MK_64(0x6572DD22, 0xF2B4969A),
+	MK_64(0x61FD3062, 0xD00A579A),
+	MK_64(0x1DE0536E, 0x8682E539)
+};
+
+#endif /* _SKEIN_IV_H_ */
diff --git a/drivers/staging/skein/threefish_api.c b/drivers/staging/skein/threefish_api.c
new file mode 100644
index 00000000000..2b649abb78c
--- /dev/null
+++ b/drivers/staging/skein/threefish_api.c
@@ -0,0 +1,77 @@
+#include <linux/string.h>
+#include "threefish_api.h"
+
+void threefish_set_key(struct threefish_key *key_ctx,
+		       enum threefish_size state_size,
+		       u64 *key_data, u64 *tweak)
+{
+	int key_words = state_size / 64;
+	int i;
+	u64 parity = KEY_SCHEDULE_CONST;
+
+	key_ctx->tweak[0] = tweak[0];
+	key_ctx->tweak[1] = tweak[1];
+	key_ctx->tweak[2] = tweak[0] ^ tweak[1];
+
+	for (i = 0; i < key_words; i++) {
+		key_ctx->key[i] = key_data[i];
+		parity ^= key_data[i];
+	}
+	key_ctx->key[i] = parity;
+	key_ctx->state_size = state_size;
+}
+
+void threefish_encrypt_block_bytes(struct threefish_key *key_ctx, u8 *in,
+				   u8 *out)
+{
+	u64 plain[SKEIN_MAX_STATE_WORDS];        /* max number of words*/
+	u64 cipher[SKEIN_MAX_STATE_WORDS];
+
+	skein_get64_lsb_first(plain, in, key_ctx->state_size / 64);
+	threefish_encrypt_block_words(key_ctx, plain, cipher);
+	skein_put64_lsb_first(out, cipher, key_ctx->state_size / 8);
+}
+
+void threefish_encrypt_block_words(struct threefish_key *key_ctx, u64 *in,
+				   u64 *out)
+{
+	switch (key_ctx->state_size) {
+	case THREEFISH_256:
+		threefish_encrypt_256(key_ctx, in, out);
+		break;
+	case THREEFISH_512:
+		threefish_encrypt_512(key_ctx, in, out);
+		break;
+	case THREEFISH_1024:
+		threefish_encrypt_1024(key_ctx, in, out);
+		break;
+	}
+}
+
+void threefish_decrypt_block_bytes(struct threefish_key *key_ctx, u8 *in,
+				   u8 *out)
+{
+	u64 plain[SKEIN_MAX_STATE_WORDS];        /* max number of words*/
+	u64 cipher[SKEIN_MAX_STATE_WORDS];
+
+	skein_get64_lsb_first(cipher, in, key_ctx->state_size / 64);
+	threefish_decrypt_block_words(key_ctx, cipher, plain);
+	skein_put64_lsb_first(out, plain, key_ctx->state_size / 8);
+}
+
+void threefish_decrypt_block_words(struct threefish_key *key_ctx, u64 *in,
+				   u64 *out)
+{
+	switch (key_ctx->state_size) {
+	case THREEFISH_256:
+		threefish_decrypt_256(key_ctx, in, out);
+		break;
+	case THREEFISH_512:
+		threefish_decrypt_512(key_ctx, in, out);
+		break;
+	case THREEFISH_1024:
+		threefish_decrypt_1024(key_ctx, in, out);
+		break;
+	}
+}
+
diff --git a/drivers/staging/skein/threefish_api.h b/drivers/staging/skein/threefish_api.h
new file mode 100644
index 00000000000..8d5ddf8b3a9
--- /dev/null
+++ b/drivers/staging/skein/threefish_api.h
@@ -0,0 +1,170 @@
+
+#ifndef THREEFISHAPI_H
+#define THREEFISHAPI_H
+
+/**
+ * @file threefish_api.h
+ * @brief A Threefish cipher API and its functions.
+ * @{
+ *
+ * This API and the functions that implement this API simplify the usage
+ * of the Threefish cipher. The design and the way to use the functions
+ * follow the openSSL design but at the same time take care of some Threefish
+ * specific behaviour and possibilities.
+ *
+ * These are the low level functions that deal with Threefish blocks only.
+ * Implementations for cipher modes such as ECB, CFB, or CBC may use these
+ * functions.
+ *
+@code
+	// Threefish cipher context data
+	struct threefish_key key_ctx;
+
+	// Initialize the context
+	threefish_set_key(&key_ctx, THREEFISH_512, key, tweak);
+
+	// Encrypt
+	threefish_encrypt_block_bytes(&key_ctx, input, cipher);
+@endcode
+ */
+
+#include <linux/types.h>
+#include "skein.h"
+
+#define KEY_SCHEDULE_CONST 0x1BD11BDAA9FC1A22L
+
+/**
+ * Which Threefish size to use
+ */
+enum threefish_size {
+	THREEFISH_256 = 256,     /*!< Skein with 256 bit state */
+	THREEFISH_512 = 512,     /*!< Skein with 512 bit state */
+	THREEFISH_1024 = 1024    /*!< Skein with 1024 bit state */
+};
+
+/**
+ * Context for Threefish key and tweak words.
+ *
+ * This structure was setup with some know-how of the internal
+ * Skein structures, in particular ordering of header and size dependent
+ * variables. If Skein implementation changes this, the adapt these
+ * structures as well.
+ */
+struct threefish_key {
+	u64 state_size;
+	u64 key[SKEIN_MAX_STATE_WORDS+1];   /* max number of key words*/
+	u64 tweak[3];
+};
+
+/**
+ * Set Threefish key and tweak data.
+ *
+ * This function sets the key and tweak data for the Threefish cipher of
+ * the given size. The key data must have the same length (number of bits)
+ * as the state size
+ *
+ * @param key_ctx
+ *     Pointer to a Threefish key structure.
+ * @param size
+ *     Which Skein size to use.
+ * @param key_data
+ *     Pointer to the key words (word has 64 bits).
+ * @param tweak
+ *     Pointer to the two tweak words (word has 64 bits).
+ */
+void threefish_set_key(struct threefish_key *key_ctx,
+		       enum threefish_size state_size,
+		       u64 *key_data, u64 *tweak);
+
+/**
+ * Encrypt Threefish block (bytes).
+ *
+ * The buffer must have at least the same length (number of bits) as the
+ * state size for this key. The function uses the first @c state_size bits
+ * of the input buffer, encrypts them and stores the result in the output
+ * buffer.
+ *
+ * @param key_ctx
+ *     Pointer to a Threefish key structure.
+ * @param in
+ *     Poionter to plaintext data buffer.
+ * @param out
+ *     Pointer to cipher buffer.
+ */
+void threefish_encrypt_block_bytes(struct threefish_key *key_ctx, u8 *in,
+				   u8 *out);
+
+/**
+ * Encrypt Threefish block (words).
+ *
+ * The buffer must have at least the same length (number of bits) as the
+ * state size for this key. The function uses the first @c state_size bits
+ * of the input buffer, encrypts them and stores the result in the output
+ * buffer.
+ *
+ * The wordsize ist set to 64 bits.
+ *
+ * @param key_ctx
+ *     Pointer to a Threefish key structure.
+ * @param in
+ *     Poionter to plaintext data buffer.
+ * @param out
+ *     Pointer to cipher buffer.
+ */
+void threefish_encrypt_block_words(struct threefish_key *key_ctx, u64 *in,
+				   u64 *out);
+
+/**
+ * Decrypt Threefish block (bytes).
+ *
+ * The buffer must have at least the same length (number of bits) as the
+ * state size for this key. The function uses the first @c state_size bits
+ * of the input buffer, decrypts them and stores the result in the output
+ * buffer
+ *
+ * @param key_ctx
+ *     Pointer to a Threefish key structure.
+ * @param in
+ *     Poionter to cipher data buffer.
+ * @param out
+ *     Pointer to plaintext buffer.
+ */
+void threefish_decrypt_block_bytes(struct threefish_key *key_ctx, u8 *in,
+				   u8 *out);
+
+/**
+ * Decrypt Threefish block (words).
+ *
+ * The buffer must have at least the same length (number of bits) as the
+ * state size for this key. The function uses the first @c state_size bits
+ * of the input buffer, encrypts them and stores the result in the output
+ * buffer.
+ *
+ * The wordsize ist set to 64 bits.
+ *
+ * @param key_ctx
+ *     Pointer to a Threefish key structure.
+ * @param in
+ *     Poionter to cipher data buffer.
+ * @param out
+ *     Pointer to plaintext buffer.
+ */
+void threefish_decrypt_block_words(struct threefish_key *key_ctx, u64 *in,
+				   u64 *out);
+
+void threefish_encrypt_256(struct threefish_key *key_ctx, u64 *input,
+			   u64 *output);
+void threefish_encrypt_512(struct threefish_key *key_ctx, u64 *input,
+			   u64 *output);
+void threefish_encrypt_1024(struct threefish_key *key_ctx, u64 *input,
+			    u64 *output);
+void threefish_decrypt_256(struct threefish_key *key_ctx, u64 *input,
+			   u64 *output);
+void threefish_decrypt_512(struct threefish_key *key_ctx, u64 *input,
+			   u64 *output);
+void threefish_decrypt_1024(struct threefish_key *key_ctx, u64 *input,
+			    u64 *output);
+/**
+ * @}
+ */
+#endif
diff --git a/drivers/staging/skein/threefish_block.c b/drivers/staging/skein/threefish_block.c
new file mode 100644
index 00000000000..bd1e15caae4
--- /dev/null
+++ b/drivers/staging/skein/threefish_block.c
@@ -0,0 +1,8258 @@
+#include "threefish_api.h"
+
+void threefish_encrypt_256(struct threefish_key *key_ctx, u64 *input,
+			   u64 *output)
+{
+	u64 b0 = input[0], b1 = input[1],
+	    b2 = input[2], b3 = input[3];
+	u64 k0 = key_ctx->key[0], k1 = key_ctx->key[1],
+	    k2 = key_ctx->key[2], k3 = key_ctx->key[3],
+	    k4 = key_ctx->key[4];
+	u64 t0 = key_ctx->tweak[0], t1 = key_ctx->tweak[1],
+	    t2 = key_ctx->tweak[2];
+
+	b1 += k1 + t0;
+	b0 += b1 + k0;
+	b1 = ((b1 << 14) | (b1 >> (64 - 14))) ^ b0;
+
+	b3 += k3;
+	b2 += b3 + k2 + t1;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 52) | (b3 >> (64 - 52))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 57) | (b1 >> (64 - 57))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 40) | (b3 >> (64 - 40))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 5) | (b3 >> (64 - 5))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 37) | (b1 >> (64 - 37))) ^ b2;
+
+	b1 += k2 + t1;
+	b0 += b1 + k1;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b0;
+
+	b3 += k4 + 1;
+	b2 += b3 + k3 + t2;
+	b3 = ((b3 << 33) | (b3 >> (64 - 33))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 46) | (b3 >> (64 - 46))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 12) | (b1 >> (64 - 12))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 58) | (b1 >> (64 - 58))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 32) | (b3 >> (64 - 32))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 32) | (b1 >> (64 - 32))) ^ b2;
+
+
+	b1 += k3 + t2;
+	b0 += b1 + k2;
+	b1 = ((b1 << 14) | (b1 >> (64 - 14))) ^ b0;
+
+	b3 += k0 + 2;
+	b2 += b3 + k4 + t0;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 52) | (b3 >> (64 - 52))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 57) | (b1 >> (64 - 57))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 40) | (b3 >> (64 - 40))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 5) | (b3 >> (64 - 5))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 37) | (b1 >> (64 - 37))) ^ b2;
+
+	b1 += k4 + t0;
+	b0 += b1 + k3;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b0;
+
+	b3 += k1 + 3;
+	b2 += b3 + k0 + t1;
+	b3 = ((b3 << 33) | (b3 >> (64 - 33))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 46) | (b3 >> (64 - 46))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 12) | (b1 >> (64 - 12))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 58) | (b1 >> (64 - 58))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 32) | (b3 >> (64 - 32))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 32) | (b1 >> (64 - 32))) ^ b2;
+
+
+	b1 += k0 + t1;
+	b0 += b1 + k4;
+	b1 = ((b1 << 14) | (b1 >> (64 - 14))) ^ b0;
+
+	b3 += k2 + 4;
+	b2 += b3 + k1 + t2;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 52) | (b3 >> (64 - 52))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 57) | (b1 >> (64 - 57))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 40) | (b3 >> (64 - 40))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 5) | (b3 >> (64 - 5))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 37) | (b1 >> (64 - 37))) ^ b2;
+
+	b1 += k1 + t2;
+	b0 += b1 + k0;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b0;
+
+	b3 += k3 + 5;
+	b2 += b3 + k2 + t0;
+	b3 = ((b3 << 33) | (b3 >> (64 - 33))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 46) | (b3 >> (64 - 46))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 12) | (b1 >> (64 - 12))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 58) | (b1 >> (64 - 58))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 32) | (b3 >> (64 - 32))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 32) | (b1 >> (64 - 32))) ^ b2;
+
+
+	b1 += k2 + t0;
+	b0 += b1 + k1;
+	b1 = ((b1 << 14) | (b1 >> (64 - 14))) ^ b0;
+
+	b3 += k4 + 6;
+	b2 += b3 + k3 + t1;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 52) | (b3 >> (64 - 52))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 57) | (b1 >> (64 - 57))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 40) | (b3 >> (64 - 40))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 5) | (b3 >> (64 - 5))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 37) | (b1 >> (64 - 37))) ^ b2;
+
+	b1 += k3 + t1;
+	b0 += b1 + k2;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b0;
+
+	b3 += k0 + 7;
+	b2 += b3 + k4 + t2;
+	b3 = ((b3 << 33) | (b3 >> (64 - 33))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 46) | (b3 >> (64 - 46))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 12) | (b1 >> (64 - 12))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 58) | (b1 >> (64 - 58))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 32) | (b3 >> (64 - 32))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 32) | (b1 >> (64 - 32))) ^ b2;
+
+
+	b1 += k4 + t2;
+	b0 += b1 + k3;
+	b1 = ((b1 << 14) | (b1 >> (64 - 14))) ^ b0;
+
+	b3 += k1 + 8;
+	b2 += b3 + k0 + t0;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 52) | (b3 >> (64 - 52))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 57) | (b1 >> (64 - 57))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 40) | (b3 >> (64 - 40))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 5) | (b3 >> (64 - 5))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 37) | (b1 >> (64 - 37))) ^ b2;
+
+	b1 += k0 + t0;
+	b0 += b1 + k4;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b0;
+
+	b3 += k2 + 9;
+	b2 += b3 + k1 + t1;
+	b3 = ((b3 << 33) | (b3 >> (64 - 33))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 46) | (b3 >> (64 - 46))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 12) | (b1 >> (64 - 12))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 58) | (b1 >> (64 - 58))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 32) | (b3 >> (64 - 32))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 32) | (b1 >> (64 - 32))) ^ b2;
+
+
+	b1 += k1 + t1;
+	b0 += b1 + k0;
+	b1 = ((b1 << 14) | (b1 >> (64 - 14))) ^ b0;
+
+	b3 += k3 + 10;
+	b2 += b3 + k2 + t2;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 52) | (b3 >> (64 - 52))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 57) | (b1 >> (64 - 57))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 40) | (b3 >> (64 - 40))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 5) | (b3 >> (64 - 5))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 37) | (b1 >> (64 - 37))) ^ b2;
+
+	b1 += k2 + t2;
+	b0 += b1 + k1;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b0;
+
+	b3 += k4 + 11;
+	b2 += b3 + k3 + t0;
+	b3 = ((b3 << 33) | (b3 >> (64 - 33))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 46) | (b3 >> (64 - 46))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 12) | (b1 >> (64 - 12))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 58) | (b1 >> (64 - 58))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 32) | (b3 >> (64 - 32))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 32) | (b1 >> (64 - 32))) ^ b2;
+
+
+	b1 += k3 + t0;
+	b0 += b1 + k2;
+	b1 = ((b1 << 14) | (b1 >> (64 - 14))) ^ b0;
+
+	b3 += k0 + 12;
+	b2 += b3 + k4 + t1;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 52) | (b3 >> (64 - 52))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 57) | (b1 >> (64 - 57))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 40) | (b3 >> (64 - 40))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 5) | (b3 >> (64 - 5))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 37) | (b1 >> (64 - 37))) ^ b2;
+
+	b1 += k4 + t1;
+	b0 += b1 + k3;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b0;
+
+	b3 += k1 + 13;
+	b2 += b3 + k0 + t2;
+	b3 = ((b3 << 33) | (b3 >> (64 - 33))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 46) | (b3 >> (64 - 46))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 12) | (b1 >> (64 - 12))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 58) | (b1 >> (64 - 58))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 32) | (b3 >> (64 - 32))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 32) | (b1 >> (64 - 32))) ^ b2;
+
+
+	b1 += k0 + t2;
+	b0 += b1 + k4;
+	b1 = ((b1 << 14) | (b1 >> (64 - 14))) ^ b0;
+
+	b3 += k2 + 14;
+	b2 += b3 + k1 + t0;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 52) | (b3 >> (64 - 52))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 57) | (b1 >> (64 - 57))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 40) | (b3 >> (64 - 40))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 5) | (b3 >> (64 - 5))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 37) | (b1 >> (64 - 37))) ^ b2;
+
+	b1 += k1 + t0;
+	b0 += b1 + k0;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b0;
+
+	b3 += k3 + 15;
+	b2 += b3 + k2 + t1;
+	b3 = ((b3 << 33) | (b3 >> (64 - 33))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 46) | (b3 >> (64 - 46))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 12) | (b1 >> (64 - 12))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 58) | (b1 >> (64 - 58))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 32) | (b3 >> (64 - 32))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 32) | (b1 >> (64 - 32))) ^ b2;
+
+
+	b1 += k2 + t1;
+	b0 += b1 + k1;
+	b1 = ((b1 << 14) | (b1 >> (64 - 14))) ^ b0;
+
+	b3 += k4 + 16;
+	b2 += b3 + k3 + t2;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 52) | (b3 >> (64 - 52))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 57) | (b1 >> (64 - 57))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 40) | (b3 >> (64 - 40))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 5) | (b3 >> (64 - 5))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 37) | (b1 >> (64 - 37))) ^ b2;
+
+	b1 += k3 + t2;
+	b0 += b1 + k2;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b0;
+
+	b3 += k0 + 17;
+	b2 += b3 + k4 + t0;
+	b3 = ((b3 << 33) | (b3 >> (64 - 33))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 46) | (b3 >> (64 - 46))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 12) | (b1 >> (64 - 12))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 58) | (b1 >> (64 - 58))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 32) | (b3 >> (64 - 32))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 32) | (b1 >> (64 - 32))) ^ b2;
+
+	output[0] = b0 + k3;
+	output[1] = b1 + k4 + t0;
+	output[2] = b2 + k0 + t1;
+	output[3] = b3 + k1 + 18;
+}
+
+void threefish_decrypt_256(struct threefish_key *key_ctx, u64 *input,
+			   u64 *output)
+{
+	u64 b0 = input[0], b1 = input[1],
+	    b2 = input[2], b3 = input[3];
+	u64 k0 = key_ctx->key[0], k1 = key_ctx->key[1],
+	    k2 = key_ctx->key[2], k3 = key_ctx->key[3],
+	    k4 = key_ctx->key[4];
+	u64 t0 = key_ctx->tweak[0], t1 = key_ctx->tweak[1],
+	    t2 = key_ctx->tweak[2];
+
+	u64 tmp;
+
+	b0 -= k3;
+	b1 -= k4 + t0;
+	b2 -= k0 + t1;
+	b3 -= k1 + 18;
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 32) | (tmp << (64 - 32));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 32) | (tmp << (64 - 32));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 58) | (tmp << (64 - 58));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 12) | (tmp << (64 - 12));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b0 -= b1 + k2;
+	b1 -= k3 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b3 + k4 + t0;
+	b3 -= k0 + 17;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 37) | (tmp << (64 - 37));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 40) | (tmp << (64 - 40));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 52) | (tmp << (64 - 52));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 57) | (tmp << (64 - 57));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 14) | (tmp << (64 - 14));
+	b0 -= b1 + k1;
+	b1 -= k2 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b2 -= b3 + k3 + t2;
+	b3 -= k4 + 16;
+
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 32) | (tmp << (64 - 32));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 32) | (tmp << (64 - 32));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 58) | (tmp << (64 - 58));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 12) | (tmp << (64 - 12));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b0 -= b1 + k0;
+	b1 -= k1 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b3 + k2 + t1;
+	b3 -= k3 + 15;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 37) | (tmp << (64 - 37));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 40) | (tmp << (64 - 40));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 52) | (tmp << (64 - 52));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 57) | (tmp << (64 - 57));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 14) | (tmp << (64 - 14));
+	b0 -= b1 + k4;
+	b1 -= k0 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b2 -= b3 + k1 + t0;
+	b3 -= k2 + 14;
+
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 32) | (tmp << (64 - 32));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 32) | (tmp << (64 - 32));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 58) | (tmp << (64 - 58));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 12) | (tmp << (64 - 12));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b0 -= b1 + k3;
+	b1 -= k4 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b3 + k0 + t2;
+	b3 -= k1 + 13;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 37) | (tmp << (64 - 37));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 40) | (tmp << (64 - 40));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 52) | (tmp << (64 - 52));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 57) | (tmp << (64 - 57));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 14) | (tmp << (64 - 14));
+	b0 -= b1 + k2;
+	b1 -= k3 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b2 -= b3 + k4 + t1;
+	b3 -= k0 + 12;
+
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 32) | (tmp << (64 - 32));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 32) | (tmp << (64 - 32));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 58) | (tmp << (64 - 58));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 12) | (tmp << (64 - 12));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b0 -= b1 + k1;
+	b1 -= k2 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b3 + k3 + t0;
+	b3 -= k4 + 11;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 37) | (tmp << (64 - 37));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 40) | (tmp << (64 - 40));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 52) | (tmp << (64 - 52));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 57) | (tmp << (64 - 57));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 14) | (tmp << (64 - 14));
+	b0 -= b1 + k0;
+	b1 -= k1 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b2 -= b3 + k2 + t2;
+	b3 -= k3 + 10;
+
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 32) | (tmp << (64 - 32));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 32) | (tmp << (64 - 32));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 58) | (tmp << (64 - 58));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 12) | (tmp << (64 - 12));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b0 -= b1 + k4;
+	b1 -= k0 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b3 + k1 + t1;
+	b3 -= k2 + 9;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 37) | (tmp << (64 - 37));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 40) | (tmp << (64 - 40));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 52) | (tmp << (64 - 52));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 57) | (tmp << (64 - 57));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 14) | (tmp << (64 - 14));
+	b0 -= b1 + k3;
+	b1 -= k4 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b2 -= b3 + k0 + t0;
+	b3 -= k1 + 8;
+
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 32) | (tmp << (64 - 32));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 32) | (tmp << (64 - 32));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 58) | (tmp << (64 - 58));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 12) | (tmp << (64 - 12));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b0 -= b1 + k2;
+	b1 -= k3 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b3 + k4 + t2;
+	b3 -= k0 + 7;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 37) | (tmp << (64 - 37));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 40) | (tmp << (64 - 40));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 52) | (tmp << (64 - 52));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 57) | (tmp << (64 - 57));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 14) | (tmp << (64 - 14));
+	b0 -= b1 + k1;
+	b1 -= k2 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b2 -= b3 + k3 + t1;
+	b3 -= k4 + 6;
+
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 32) | (tmp << (64 - 32));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 32) | (tmp << (64 - 32));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 58) | (tmp << (64 - 58));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 12) | (tmp << (64 - 12));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b0 -= b1 + k0;
+	b1 -= k1 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b3 + k2 + t0;
+	b3 -= k3 + 5;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 37) | (tmp << (64 - 37));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 40) | (tmp << (64 - 40));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 52) | (tmp << (64 - 52));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 57) | (tmp << (64 - 57));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 14) | (tmp << (64 - 14));
+	b0 -= b1 + k4;
+	b1 -= k0 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b2 -= b3 + k1 + t2;
+	b3 -= k2 + 4;
+
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 32) | (tmp << (64 - 32));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 32) | (tmp << (64 - 32));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 58) | (tmp << (64 - 58));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 12) | (tmp << (64 - 12));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b0 -= b1 + k3;
+	b1 -= k4 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b3 + k0 + t1;
+	b3 -= k1 + 3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 37) | (tmp << (64 - 37));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 40) | (tmp << (64 - 40));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 52) | (tmp << (64 - 52));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 57) | (tmp << (64 - 57));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 14) | (tmp << (64 - 14));
+	b0 -= b1 + k2;
+	b1 -= k3 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b2 -= b3 + k4 + t0;
+	b3 -= k0 + 2;
+
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 32) | (tmp << (64 - 32));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 32) | (tmp << (64 - 32));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 58) | (tmp << (64 - 58));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 12) | (tmp << (64 - 12));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b0 -= b1 + k1;
+	b1 -= k2 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b3 + k3 + t2;
+	b3 -= k4 + 1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 37) | (tmp << (64 - 37));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 40) | (tmp << (64 - 40));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 52) | (tmp << (64 - 52));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 57) | (tmp << (64 - 57));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 14) | (tmp << (64 - 14));
+	b0 -= b1 + k0;
+	b1 -= k1 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b2 -= b3 + k2 + t1;
+	b3 -= k3;
+
+	output[0] = b0;
+	output[1] = b1;
+	output[2] = b2;
+	output[3] = b3;
+}
+
+void threefish_encrypt_512(struct threefish_key *key_ctx, u64 *input,
+			   u64 *output)
+{
+	u64 b0 = input[0], b1 = input[1],
+	    b2 = input[2], b3 = input[3],
+	    b4 = input[4], b5 = input[5],
+	    b6 = input[6], b7 = input[7];
+	u64 k0 = key_ctx->key[0], k1 = key_ctx->key[1],
+	    k2 = key_ctx->key[2], k3 = key_ctx->key[3],
+	    k4 = key_ctx->key[4], k5 = key_ctx->key[5],
+	    k6 = key_ctx->key[6], k7 = key_ctx->key[7],
+	    k8 = key_ctx->key[8];
+	u64 t0 = key_ctx->tweak[0], t1 = key_ctx->tweak[1],
+	    t2 = key_ctx->tweak[2];
+
+	b1 += k1;
+	b0 += b1 + k0;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b0;
+
+	b3 += k3;
+	b2 += b3 + k2;
+	b3 = ((b3 << 36) | (b3 >> (64 - 36))) ^ b2;
+
+	b5 += k5 + t0;
+	b4 += b5 + k4;
+	b5 = ((b5 << 19) | (b5 >> (64 - 19))) ^ b4;
+
+	b7 += k7;
+	b6 += b7 + k6 + t1;
+	b7 = ((b7 << 37) | (b7 >> (64 - 37))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 33) | (b1 >> (64 - 33))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 27) | (b7 >> (64 - 27))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 14) | (b5 >> (64 - 14))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 42) | (b3 >> (64 - 42))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 17) | (b1 >> (64 - 17))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 49) | (b3 >> (64 - 49))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 36) | (b5 >> (64 - 36))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 39) | (b7 >> (64 - 39))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 44) | (b1 >> (64 - 44))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 9) | (b7 >> (64 - 9))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 54) | (b5 >> (64 - 54))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 56) | (b3 >> (64 - 56))) ^ b4;
+
+	b1 += k2;
+	b0 += b1 + k1;
+	b1 = ((b1 << 39) | (b1 >> (64 - 39))) ^ b0;
+
+	b3 += k4;
+	b2 += b3 + k3;
+	b3 = ((b3 << 30) | (b3 >> (64 - 30))) ^ b2;
+
+	b5 += k6 + t1;
+	b4 += b5 + k5;
+	b5 = ((b5 << 34) | (b5 >> (64 - 34))) ^ b4;
+
+	b7 += k8 + 1;
+	b6 += b7 + k7 + t2;
+	b7 = ((b7 << 24) | (b7 >> (64 - 24))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 50) | (b7 >> (64 - 50))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 10) | (b5 >> (64 - 10))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 17) | (b3 >> (64 - 17))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 29) | (b3 >> (64 - 29))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 39) | (b5 >> (64 - 39))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 43) | (b7 >> (64 - 43))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 8) | (b1 >> (64 - 8))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 35) | (b7 >> (64 - 35))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 56) | (b5 >> (64 - 56))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b4;
+
+	b1 += k3;
+	b0 += b1 + k2;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b0;
+
+	b3 += k5;
+	b2 += b3 + k4;
+	b3 = ((b3 << 36) | (b3 >> (64 - 36))) ^ b2;
+
+	b5 += k7 + t2;
+	b4 += b5 + k6;
+	b5 = ((b5 << 19) | (b5 >> (64 - 19))) ^ b4;
+
+	b7 += k0 + 2;
+	b6 += b7 + k8 + t0;
+	b7 = ((b7 << 37) | (b7 >> (64 - 37))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 33) | (b1 >> (64 - 33))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 27) | (b7 >> (64 - 27))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 14) | (b5 >> (64 - 14))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 42) | (b3 >> (64 - 42))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 17) | (b1 >> (64 - 17))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 49) | (b3 >> (64 - 49))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 36) | (b5 >> (64 - 36))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 39) | (b7 >> (64 - 39))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 44) | (b1 >> (64 - 44))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 9) | (b7 >> (64 - 9))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 54) | (b5 >> (64 - 54))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 56) | (b3 >> (64 - 56))) ^ b4;
+
+	b1 += k4;
+	b0 += b1 + k3;
+	b1 = ((b1 << 39) | (b1 >> (64 - 39))) ^ b0;
+
+	b3 += k6;
+	b2 += b3 + k5;
+	b3 = ((b3 << 30) | (b3 >> (64 - 30))) ^ b2;
+
+	b5 += k8 + t0;
+	b4 += b5 + k7;
+	b5 = ((b5 << 34) | (b5 >> (64 - 34))) ^ b4;
+
+	b7 += k1 + 3;
+	b6 += b7 + k0 + t1;
+	b7 = ((b7 << 24) | (b7 >> (64 - 24))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 50) | (b7 >> (64 - 50))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 10) | (b5 >> (64 - 10))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 17) | (b3 >> (64 - 17))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 29) | (b3 >> (64 - 29))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 39) | (b5 >> (64 - 39))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 43) | (b7 >> (64 - 43))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 8) | (b1 >> (64 - 8))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 35) | (b7 >> (64 - 35))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 56) | (b5 >> (64 - 56))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b4;
+
+	b1 += k5;
+	b0 += b1 + k4;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b0;
+
+	b3 += k7;
+	b2 += b3 + k6;
+	b3 = ((b3 << 36) | (b3 >> (64 - 36))) ^ b2;
+
+	b5 += k0 + t1;
+	b4 += b5 + k8;
+	b5 = ((b5 << 19) | (b5 >> (64 - 19))) ^ b4;
+
+	b7 += k2 + 4;
+	b6 += b7 + k1 + t2;
+	b7 = ((b7 << 37) | (b7 >> (64 - 37))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 33) | (b1 >> (64 - 33))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 27) | (b7 >> (64 - 27))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 14) | (b5 >> (64 - 14))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 42) | (b3 >> (64 - 42))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 17) | (b1 >> (64 - 17))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 49) | (b3 >> (64 - 49))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 36) | (b5 >> (64 - 36))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 39) | (b7 >> (64 - 39))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 44) | (b1 >> (64 - 44))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 9) | (b7 >> (64 - 9))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 54) | (b5 >> (64 - 54))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 56) | (b3 >> (64 - 56))) ^ b4;
+
+	b1 += k6;
+	b0 += b1 + k5;
+	b1 = ((b1 << 39) | (b1 >> (64 - 39))) ^ b0;
+
+	b3 += k8;
+	b2 += b3 + k7;
+	b3 = ((b3 << 30) | (b3 >> (64 - 30))) ^ b2;
+
+	b5 += k1 + t2;
+	b4 += b5 + k0;
+	b5 = ((b5 << 34) | (b5 >> (64 - 34))) ^ b4;
+
+	b7 += k3 + 5;
+	b6 += b7 + k2 + t0;
+	b7 = ((b7 << 24) | (b7 >> (64 - 24))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 50) | (b7 >> (64 - 50))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 10) | (b5 >> (64 - 10))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 17) | (b3 >> (64 - 17))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 29) | (b3 >> (64 - 29))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 39) | (b5 >> (64 - 39))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 43) | (b7 >> (64 - 43))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 8) | (b1 >> (64 - 8))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 35) | (b7 >> (64 - 35))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 56) | (b5 >> (64 - 56))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b4;
+
+	b1 += k7;
+	b0 += b1 + k6;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b0;
+
+	b3 += k0;
+	b2 += b3 + k8;
+	b3 = ((b3 << 36) | (b3 >> (64 - 36))) ^ b2;
+
+	b5 += k2 + t0;
+	b4 += b5 + k1;
+	b5 = ((b5 << 19) | (b5 >> (64 - 19))) ^ b4;
+
+	b7 += k4 + 6;
+	b6 += b7 + k3 + t1;
+	b7 = ((b7 << 37) | (b7 >> (64 - 37))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 33) | (b1 >> (64 - 33))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 27) | (b7 >> (64 - 27))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 14) | (b5 >> (64 - 14))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 42) | (b3 >> (64 - 42))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 17) | (b1 >> (64 - 17))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 49) | (b3 >> (64 - 49))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 36) | (b5 >> (64 - 36))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 39) | (b7 >> (64 - 39))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 44) | (b1 >> (64 - 44))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 9) | (b7 >> (64 - 9))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 54) | (b5 >> (64 - 54))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 56) | (b3 >> (64 - 56))) ^ b4;
+
+	b1 += k8;
+	b0 += b1 + k7;
+	b1 = ((b1 << 39) | (b1 >> (64 - 39))) ^ b0;
+
+	b3 += k1;
+	b2 += b3 + k0;
+	b3 = ((b3 << 30) | (b3 >> (64 - 30))) ^ b2;
+
+	b5 += k3 + t1;
+	b4 += b5 + k2;
+	b5 = ((b5 << 34) | (b5 >> (64 - 34))) ^ b4;
+
+	b7 += k5 + 7;
+	b6 += b7 + k4 + t2;
+	b7 = ((b7 << 24) | (b7 >> (64 - 24))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 50) | (b7 >> (64 - 50))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 10) | (b5 >> (64 - 10))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 17) | (b3 >> (64 - 17))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 29) | (b3 >> (64 - 29))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 39) | (b5 >> (64 - 39))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 43) | (b7 >> (64 - 43))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 8) | (b1 >> (64 - 8))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 35) | (b7 >> (64 - 35))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 56) | (b5 >> (64 - 56))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b4;
+
+	b1 += k0;
+	b0 += b1 + k8;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b0;
+
+	b3 += k2;
+	b2 += b3 + k1;
+	b3 = ((b3 << 36) | (b3 >> (64 - 36))) ^ b2;
+
+	b5 += k4 + t2;
+	b4 += b5 + k3;
+	b5 = ((b5 << 19) | (b5 >> (64 - 19))) ^ b4;
+
+	b7 += k6 + 8;
+	b6 += b7 + k5 + t0;
+	b7 = ((b7 << 37) | (b7 >> (64 - 37))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 33) | (b1 >> (64 - 33))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 27) | (b7 >> (64 - 27))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 14) | (b5 >> (64 - 14))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 42) | (b3 >> (64 - 42))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 17) | (b1 >> (64 - 17))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 49) | (b3 >> (64 - 49))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 36) | (b5 >> (64 - 36))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 39) | (b7 >> (64 - 39))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 44) | (b1 >> (64 - 44))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 9) | (b7 >> (64 - 9))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 54) | (b5 >> (64 - 54))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 56) | (b3 >> (64 - 56))) ^ b4;
+
+	b1 += k1;
+	b0 += b1 + k0;
+	b1 = ((b1 << 39) | (b1 >> (64 - 39))) ^ b0;
+
+	b3 += k3;
+	b2 += b3 + k2;
+	b3 = ((b3 << 30) | (b3 >> (64 - 30))) ^ b2;
+
+	b5 += k5 + t0;
+	b4 += b5 + k4;
+	b5 = ((b5 << 34) | (b5 >> (64 - 34))) ^ b4;
+
+	b7 += k7 + 9;
+	b6 += b7 + k6 + t1;
+	b7 = ((b7 << 24) | (b7 >> (64 - 24))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 50) | (b7 >> (64 - 50))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 10) | (b5 >> (64 - 10))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 17) | (b3 >> (64 - 17))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 29) | (b3 >> (64 - 29))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 39) | (b5 >> (64 - 39))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 43) | (b7 >> (64 - 43))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 8) | (b1 >> (64 - 8))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 35) | (b7 >> (64 - 35))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 56) | (b5 >> (64 - 56))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b4;
+
+	b1 += k2;
+	b0 += b1 + k1;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b0;
+
+	b3 += k4;
+	b2 += b3 + k3;
+	b3 = ((b3 << 36) | (b3 >> (64 - 36))) ^ b2;
+
+	b5 += k6 + t1;
+	b4 += b5 + k5;
+	b5 = ((b5 << 19) | (b5 >> (64 - 19))) ^ b4;
+
+	b7 += k8 + 10;
+	b6 += b7 + k7 + t2;
+	b7 = ((b7 << 37) | (b7 >> (64 - 37))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 33) | (b1 >> (64 - 33))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 27) | (b7 >> (64 - 27))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 14) | (b5 >> (64 - 14))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 42) | (b3 >> (64 - 42))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 17) | (b1 >> (64 - 17))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 49) | (b3 >> (64 - 49))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 36) | (b5 >> (64 - 36))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 39) | (b7 >> (64 - 39))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 44) | (b1 >> (64 - 44))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 9) | (b7 >> (64 - 9))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 54) | (b5 >> (64 - 54))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 56) | (b3 >> (64 - 56))) ^ b4;
+
+	b1 += k3;
+	b0 += b1 + k2;
+	b1 = ((b1 << 39) | (b1 >> (64 - 39))) ^ b0;
+
+	b3 += k5;
+	b2 += b3 + k4;
+	b3 = ((b3 << 30) | (b3 >> (64 - 30))) ^ b2;
+
+	b5 += k7 + t2;
+	b4 += b5 + k6;
+	b5 = ((b5 << 34) | (b5 >> (64 - 34))) ^ b4;
+
+	b7 += k0 + 11;
+	b6 += b7 + k8 + t0;
+	b7 = ((b7 << 24) | (b7 >> (64 - 24))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 50) | (b7 >> (64 - 50))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 10) | (b5 >> (64 - 10))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 17) | (b3 >> (64 - 17))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 29) | (b3 >> (64 - 29))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 39) | (b5 >> (64 - 39))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 43) | (b7 >> (64 - 43))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 8) | (b1 >> (64 - 8))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 35) | (b7 >> (64 - 35))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 56) | (b5 >> (64 - 56))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b4;
+
+	b1 += k4;
+	b0 += b1 + k3;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b0;
+
+	b3 += k6;
+	b2 += b3 + k5;
+	b3 = ((b3 << 36) | (b3 >> (64 - 36))) ^ b2;
+
+	b5 += k8 + t0;
+	b4 += b5 + k7;
+	b5 = ((b5 << 19) | (b5 >> (64 - 19))) ^ b4;
+
+	b7 += k1 + 12;
+	b6 += b7 + k0 + t1;
+	b7 = ((b7 << 37) | (b7 >> (64 - 37))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 33) | (b1 >> (64 - 33))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 27) | (b7 >> (64 - 27))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 14) | (b5 >> (64 - 14))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 42) | (b3 >> (64 - 42))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 17) | (b1 >> (64 - 17))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 49) | (b3 >> (64 - 49))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 36) | (b5 >> (64 - 36))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 39) | (b7 >> (64 - 39))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 44) | (b1 >> (64 - 44))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 9) | (b7 >> (64 - 9))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 54) | (b5 >> (64 - 54))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 56) | (b3 >> (64 - 56))) ^ b4;
+
+	b1 += k5;
+	b0 += b1 + k4;
+	b1 = ((b1 << 39) | (b1 >> (64 - 39))) ^ b0;
+
+	b3 += k7;
+	b2 += b3 + k6;
+	b3 = ((b3 << 30) | (b3 >> (64 - 30))) ^ b2;
+
+	b5 += k0 + t1;
+	b4 += b5 + k8;
+	b5 = ((b5 << 34) | (b5 >> (64 - 34))) ^ b4;
+
+	b7 += k2 + 13;
+	b6 += b7 + k1 + t2;
+	b7 = ((b7 << 24) | (b7 >> (64 - 24))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 50) | (b7 >> (64 - 50))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 10) | (b5 >> (64 - 10))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 17) | (b3 >> (64 - 17))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 29) | (b3 >> (64 - 29))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 39) | (b5 >> (64 - 39))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 43) | (b7 >> (64 - 43))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 8) | (b1 >> (64 - 8))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 35) | (b7 >> (64 - 35))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 56) | (b5 >> (64 - 56))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b4;
+
+	b1 += k6;
+	b0 += b1 + k5;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b0;
+
+	b3 += k8;
+	b2 += b3 + k7;
+	b3 = ((b3 << 36) | (b3 >> (64 - 36))) ^ b2;
+
+	b5 += k1 + t2;
+	b4 += b5 + k0;
+	b5 = ((b5 << 19) | (b5 >> (64 - 19))) ^ b4;
+
+	b7 += k3 + 14;
+	b6 += b7 + k2 + t0;
+	b7 = ((b7 << 37) | (b7 >> (64 - 37))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 33) | (b1 >> (64 - 33))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 27) | (b7 >> (64 - 27))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 14) | (b5 >> (64 - 14))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 42) | (b3 >> (64 - 42))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 17) | (b1 >> (64 - 17))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 49) | (b3 >> (64 - 49))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 36) | (b5 >> (64 - 36))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 39) | (b7 >> (64 - 39))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 44) | (b1 >> (64 - 44))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 9) | (b7 >> (64 - 9))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 54) | (b5 >> (64 - 54))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 56) | (b3 >> (64 - 56))) ^ b4;
+
+	b1 += k7;
+	b0 += b1 + k6;
+	b1 = ((b1 << 39) | (b1 >> (64 - 39))) ^ b0;
+
+	b3 += k0;
+	b2 += b3 + k8;
+	b3 = ((b3 << 30) | (b3 >> (64 - 30))) ^ b2;
+
+	b5 += k2 + t0;
+	b4 += b5 + k1;
+	b5 = ((b5 << 34) | (b5 >> (64 - 34))) ^ b4;
+
+	b7 += k4 + 15;
+	b6 += b7 + k3 + t1;
+	b7 = ((b7 << 24) | (b7 >> (64 - 24))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 50) | (b7 >> (64 - 50))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 10) | (b5 >> (64 - 10))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 17) | (b3 >> (64 - 17))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 29) | (b3 >> (64 - 29))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 39) | (b5 >> (64 - 39))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 43) | (b7 >> (64 - 43))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 8) | (b1 >> (64 - 8))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 35) | (b7 >> (64 - 35))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 56) | (b5 >> (64 - 56))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b4;
+
+	b1 += k8;
+	b0 += b1 + k7;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b0;
+
+	b3 += k1;
+	b2 += b3 + k0;
+	b3 = ((b3 << 36) | (b3 >> (64 - 36))) ^ b2;
+
+	b5 += k3 + t1;
+	b4 += b5 + k2;
+	b5 = ((b5 << 19) | (b5 >> (64 - 19))) ^ b4;
+
+	b7 += k5 + 16;
+	b6 += b7 + k4 + t2;
+	b7 = ((b7 << 37) | (b7 >> (64 - 37))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 33) | (b1 >> (64 - 33))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 27) | (b7 >> (64 - 27))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 14) | (b5 >> (64 - 14))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 42) | (b3 >> (64 - 42))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 17) | (b1 >> (64 - 17))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 49) | (b3 >> (64 - 49))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 36) | (b5 >> (64 - 36))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 39) | (b7 >> (64 - 39))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 44) | (b1 >> (64 - 44))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 9) | (b7 >> (64 - 9))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 54) | (b5 >> (64 - 54))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 56) | (b3 >> (64 - 56))) ^ b4;
+
+	b1 += k0;
+	b0 += b1 + k8;
+	b1 = ((b1 << 39) | (b1 >> (64 - 39))) ^ b0;
+
+	b3 += k2;
+	b2 += b3 + k1;
+	b3 = ((b3 << 30) | (b3 >> (64 - 30))) ^ b2;
+
+	b5 += k4 + t2;
+	b4 += b5 + k3;
+	b5 = ((b5 << 34) | (b5 >> (64 - 34))) ^ b4;
+
+	b7 += k6 + 17;
+	b6 += b7 + k5 + t0;
+	b7 = ((b7 << 24) | (b7 >> (64 - 24))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 50) | (b7 >> (64 - 50))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 10) | (b5 >> (64 - 10))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 17) | (b3 >> (64 - 17))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 29) | (b3 >> (64 - 29))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 39) | (b5 >> (64 - 39))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 43) | (b7 >> (64 - 43))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 8) | (b1 >> (64 - 8))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 35) | (b7 >> (64 - 35))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 56) | (b5 >> (64 - 56))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b4;
+
+	output[0] = b0 + k0;
+	output[1] = b1 + k1;
+	output[2] = b2 + k2;
+	output[3] = b3 + k3;
+	output[4] = b4 + k4;
+	output[5] = b5 + k5 + t0;
+	output[6] = b6 + k6 + t1;
+	output[7] = b7 + k7 + 18;
+}
+
+void threefish_decrypt_512(struct threefish_key *key_ctx, u64 *input,
+			   u64 *output)
+{
+	u64 b0 = input[0], b1 = input[1],
+	    b2 = input[2], b3 = input[3],
+	    b4 = input[4], b5 = input[5],
+	    b6 = input[6], b7 = input[7];
+	u64 k0 = key_ctx->key[0], k1 = key_ctx->key[1],
+	    k2 = key_ctx->key[2], k3 = key_ctx->key[3],
+	    k4 = key_ctx->key[4], k5 = key_ctx->key[5],
+	    k6 = key_ctx->key[6], k7 = key_ctx->key[7],
+	    k8 = key_ctx->key[8];
+	u64 t0 = key_ctx->tweak[0], t1 = key_ctx->tweak[1],
+	    t2 = key_ctx->tweak[2];
+
+	u64 tmp;
+
+	b0 -= k0;
+	b1 -= k1;
+	b2 -= k2;
+	b3 -= k3;
+	b4 -= k4;
+	b5 -= k5 + t0;
+	b6 -= k6 + t1;
+	b7 -= k7 + 18;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 56) | (tmp << (64 - 56));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 35) | (tmp << (64 - 35));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 8) | (tmp << (64 - 8));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 43) | (tmp << (64 - 43));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 29) | (tmp << (64 - 29));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 17) | (tmp << (64 - 17));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 50) | (tmp << (64 - 50));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 24) | (tmp << (64 - 24));
+	b6 -= b7 + k5 + t0;
+	b7 -= k6 + 17;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 34) | (tmp << (64 - 34));
+	b4 -= b5 + k3;
+	b5 -= k4 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 30) | (tmp << (64 - 30));
+	b2 -= b3 + k1;
+	b3 -= k2;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b1 + k8;
+	b1 -= k0;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 56) | (tmp << (64 - 56));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 54) | (tmp << (64 - 54));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 44) | (tmp << (64 - 44));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 39) | (tmp << (64 - 39));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 36) | (tmp << (64 - 36));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 49) | (tmp << (64 - 49));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 17) | (tmp << (64 - 17));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 42) | (tmp << (64 - 42));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 14) | (tmp << (64 - 14));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 27) | (tmp << (64 - 27));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 37) | (tmp << (64 - 37));
+	b6 -= b7 + k4 + t2;
+	b7 -= k5 + 16;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 19) | (tmp << (64 - 19));
+	b4 -= b5 + k2;
+	b5 -= k3 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 36) | (tmp << (64 - 36));
+	b2 -= b3 + k0;
+	b3 -= k1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b1 + k7;
+	b1 -= k8;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 56) | (tmp << (64 - 56));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 35) | (tmp << (64 - 35));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 8) | (tmp << (64 - 8));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 43) | (tmp << (64 - 43));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 29) | (tmp << (64 - 29));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 17) | (tmp << (64 - 17));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 50) | (tmp << (64 - 50));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 24) | (tmp << (64 - 24));
+	b6 -= b7 + k3 + t1;
+	b7 -= k4 + 15;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 34) | (tmp << (64 - 34));
+	b4 -= b5 + k1;
+	b5 -= k2 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 30) | (tmp << (64 - 30));
+	b2 -= b3 + k8;
+	b3 -= k0;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b1 + k6;
+	b1 -= k7;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 56) | (tmp << (64 - 56));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 54) | (tmp << (64 - 54));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 44) | (tmp << (64 - 44));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 39) | (tmp << (64 - 39));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 36) | (tmp << (64 - 36));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 49) | (tmp << (64 - 49));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 17) | (tmp << (64 - 17));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 42) | (tmp << (64 - 42));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 14) | (tmp << (64 - 14));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 27) | (tmp << (64 - 27));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 37) | (tmp << (64 - 37));
+	b6 -= b7 + k2 + t0;
+	b7 -= k3 + 14;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 19) | (tmp << (64 - 19));
+	b4 -= b5 + k0;
+	b5 -= k1 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 36) | (tmp << (64 - 36));
+	b2 -= b3 + k7;
+	b3 -= k8;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b1 + k5;
+	b1 -= k6;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 56) | (tmp << (64 - 56));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 35) | (tmp << (64 - 35));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 8) | (tmp << (64 - 8));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 43) | (tmp << (64 - 43));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 29) | (tmp << (64 - 29));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 17) | (tmp << (64 - 17));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 50) | (tmp << (64 - 50));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 24) | (tmp << (64 - 24));
+	b6 -= b7 + k1 + t2;
+	b7 -= k2 + 13;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 34) | (tmp << (64 - 34));
+	b4 -= b5 + k8;
+	b5 -= k0 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 30) | (tmp << (64 - 30));
+	b2 -= b3 + k6;
+	b3 -= k7;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b1 + k4;
+	b1 -= k5;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 56) | (tmp << (64 - 56));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 54) | (tmp << (64 - 54));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 44) | (tmp << (64 - 44));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 39) | (tmp << (64 - 39));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 36) | (tmp << (64 - 36));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 49) | (tmp << (64 - 49));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 17) | (tmp << (64 - 17));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 42) | (tmp << (64 - 42));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 14) | (tmp << (64 - 14));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 27) | (tmp << (64 - 27));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 37) | (tmp << (64 - 37));
+	b6 -= b7 + k0 + t1;
+	b7 -= k1 + 12;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 19) | (tmp << (64 - 19));
+	b4 -= b5 + k7;
+	b5 -= k8 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 36) | (tmp << (64 - 36));
+	b2 -= b3 + k5;
+	b3 -= k6;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b1 + k3;
+	b1 -= k4;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 56) | (tmp << (64 - 56));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 35) | (tmp << (64 - 35));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 8) | (tmp << (64 - 8));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 43) | (tmp << (64 - 43));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 29) | (tmp << (64 - 29));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 17) | (tmp << (64 - 17));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 50) | (tmp << (64 - 50));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 24) | (tmp << (64 - 24));
+	b6 -= b7 + k8 + t0;
+	b7 -= k0 + 11;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 34) | (tmp << (64 - 34));
+	b4 -= b5 + k6;
+	b5 -= k7 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 30) | (tmp << (64 - 30));
+	b2 -= b3 + k4;
+	b3 -= k5;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b1 + k2;
+	b1 -= k3;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 56) | (tmp << (64 - 56));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 54) | (tmp << (64 - 54));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 44) | (tmp << (64 - 44));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 39) | (tmp << (64 - 39));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 36) | (tmp << (64 - 36));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 49) | (tmp << (64 - 49));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 17) | (tmp << (64 - 17));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 42) | (tmp << (64 - 42));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 14) | (tmp << (64 - 14));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 27) | (tmp << (64 - 27));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 37) | (tmp << (64 - 37));
+	b6 -= b7 + k7 + t2;
+	b7 -= k8 + 10;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 19) | (tmp << (64 - 19));
+	b4 -= b5 + k5;
+	b5 -= k6 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 36) | (tmp << (64 - 36));
+	b2 -= b3 + k3;
+	b3 -= k4;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b1 + k1;
+	b1 -= k2;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 56) | (tmp << (64 - 56));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 35) | (tmp << (64 - 35));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 8) | (tmp << (64 - 8));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 43) | (tmp << (64 - 43));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 29) | (tmp << (64 - 29));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 17) | (tmp << (64 - 17));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 50) | (tmp << (64 - 50));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 24) | (tmp << (64 - 24));
+	b6 -= b7 + k6 + t1;
+	b7 -= k7 + 9;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 34) | (tmp << (64 - 34));
+	b4 -= b5 + k4;
+	b5 -= k5 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 30) | (tmp << (64 - 30));
+	b2 -= b3 + k2;
+	b3 -= k3;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b1 + k0;
+	b1 -= k1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 56) | (tmp << (64 - 56));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 54) | (tmp << (64 - 54));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 44) | (tmp << (64 - 44));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 39) | (tmp << (64 - 39));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 36) | (tmp << (64 - 36));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 49) | (tmp << (64 - 49));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 17) | (tmp << (64 - 17));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 42) | (tmp << (64 - 42));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 14) | (tmp << (64 - 14));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 27) | (tmp << (64 - 27));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 37) | (tmp << (64 - 37));
+	b6 -= b7 + k5 + t0;
+	b7 -= k6 + 8;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 19) | (tmp << (64 - 19));
+	b4 -= b5 + k3;
+	b5 -= k4 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 36) | (tmp << (64 - 36));
+	b2 -= b3 + k1;
+	b3 -= k2;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b1 + k8;
+	b1 -= k0;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 56) | (tmp << (64 - 56));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 35) | (tmp << (64 - 35));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 8) | (tmp << (64 - 8));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 43) | (tmp << (64 - 43));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 29) | (tmp << (64 - 29));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 17) | (tmp << (64 - 17));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 50) | (tmp << (64 - 50));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 24) | (tmp << (64 - 24));
+	b6 -= b7 + k4 + t2;
+	b7 -= k5 + 7;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 34) | (tmp << (64 - 34));
+	b4 -= b5 + k2;
+	b5 -= k3 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 30) | (tmp << (64 - 30));
+	b2 -= b3 + k0;
+	b3 -= k1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b1 + k7;
+	b1 -= k8;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 56) | (tmp << (64 - 56));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 54) | (tmp << (64 - 54));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 44) | (tmp << (64 - 44));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 39) | (tmp << (64 - 39));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 36) | (tmp << (64 - 36));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 49) | (tmp << (64 - 49));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 17) | (tmp << (64 - 17));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 42) | (tmp << (64 - 42));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 14) | (tmp << (64 - 14));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 27) | (tmp << (64 - 27));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 37) | (tmp << (64 - 37));
+	b6 -= b7 + k3 + t1;
+	b7 -= k4 + 6;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 19) | (tmp << (64 - 19));
+	b4 -= b5 + k1;
+	b5 -= k2 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 36) | (tmp << (64 - 36));
+	b2 -= b3 + k8;
+	b3 -= k0;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b1 + k6;
+	b1 -= k7;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 56) | (tmp << (64 - 56));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 35) | (tmp << (64 - 35));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 8) | (tmp << (64 - 8));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 43) | (tmp << (64 - 43));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 29) | (tmp << (64 - 29));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 17) | (tmp << (64 - 17));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 50) | (tmp << (64 - 50));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 24) | (tmp << (64 - 24));
+	b6 -= b7 + k2 + t0;
+	b7 -= k3 + 5;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 34) | (tmp << (64 - 34));
+	b4 -= b5 + k0;
+	b5 -= k1 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 30) | (tmp << (64 - 30));
+	b2 -= b3 + k7;
+	b3 -= k8;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b1 + k5;
+	b1 -= k6;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 56) | (tmp << (64 - 56));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 54) | (tmp << (64 - 54));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 44) | (tmp << (64 - 44));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 39) | (tmp << (64 - 39));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 36) | (tmp << (64 - 36));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 49) | (tmp << (64 - 49));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 17) | (tmp << (64 - 17));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 42) | (tmp << (64 - 42));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 14) | (tmp << (64 - 14));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 27) | (tmp << (64 - 27));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 37) | (tmp << (64 - 37));
+	b6 -= b7 + k1 + t2;
+	b7 -= k2 + 4;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 19) | (tmp << (64 - 19));
+	b4 -= b5 + k8;
+	b5 -= k0 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 36) | (tmp << (64 - 36));
+	b2 -= b3 + k6;
+	b3 -= k7;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b1 + k4;
+	b1 -= k5;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 56) | (tmp << (64 - 56));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 35) | (tmp << (64 - 35));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 8) | (tmp << (64 - 8));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 43) | (tmp << (64 - 43));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 29) | (tmp << (64 - 29));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 17) | (tmp << (64 - 17));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 50) | (tmp << (64 - 50));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 24) | (tmp << (64 - 24));
+	b6 -= b7 + k0 + t1;
+	b7 -= k1 + 3;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 34) | (tmp << (64 - 34));
+	b4 -= b5 + k7;
+	b5 -= k8 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 30) | (tmp << (64 - 30));
+	b2 -= b3 + k5;
+	b3 -= k6;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b1 + k3;
+	b1 -= k4;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 56) | (tmp << (64 - 56));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 54) | (tmp << (64 - 54));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 44) | (tmp << (64 - 44));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 39) | (tmp << (64 - 39));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 36) | (tmp << (64 - 36));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 49) | (tmp << (64 - 49));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 17) | (tmp << (64 - 17));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 42) | (tmp << (64 - 42));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 14) | (tmp << (64 - 14));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 27) | (tmp << (64 - 27));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 37) | (tmp << (64 - 37));
+	b6 -= b7 + k8 + t0;
+	b7 -= k0 + 2;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 19) | (tmp << (64 - 19));
+	b4 -= b5 + k6;
+	b5 -= k7 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 36) | (tmp << (64 - 36));
+	b2 -= b3 + k4;
+	b3 -= k5;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b1 + k2;
+	b1 -= k3;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 56) | (tmp << (64 - 56));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 35) | (tmp << (64 - 35));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 8) | (tmp << (64 - 8));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 43) | (tmp << (64 - 43));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 29) | (tmp << (64 - 29));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 17) | (tmp << (64 - 17));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 50) | (tmp << (64 - 50));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 24) | (tmp << (64 - 24));
+	b6 -= b7 + k7 + t2;
+	b7 -= k8 + 1;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 34) | (tmp << (64 - 34));
+	b4 -= b5 + k5;
+	b5 -= k6 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 30) | (tmp << (64 - 30));
+	b2 -= b3 + k3;
+	b3 -= k4;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b1 + k1;
+	b1 -= k2;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 56) | (tmp << (64 - 56));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 54) | (tmp << (64 - 54));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 44) | (tmp << (64 - 44));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 39) | (tmp << (64 - 39));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 36) | (tmp << (64 - 36));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 49) | (tmp << (64 - 49));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 17) | (tmp << (64 - 17));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 42) | (tmp << (64 - 42));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 14) | (tmp << (64 - 14));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 27) | (tmp << (64 - 27));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 37) | (tmp << (64 - 37));
+	b6 -= b7 + k6 + t1;
+	b7 -= k7;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 19) | (tmp << (64 - 19));
+	b4 -= b5 + k4;
+	b5 -= k5 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 36) | (tmp << (64 - 36));
+	b2 -= b3 + k2;
+	b3 -= k3;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b1 + k0;
+	b1 -= k1;
+
+	output[0] = b0;
+	output[1] = b1;
+	output[2] = b2;
+	output[3] = b3;
+
+	output[7] = b7;
+	output[6] = b6;
+	output[5] = b5;
+	output[4] = b4;
+}
+
+void threefish_encrypt_1024(struct threefish_key *key_ctx, u64 *input,
+			    u64 *output)
+{
+	u64 b0 = input[0], b1 = input[1],
+	    b2 = input[2], b3 = input[3],
+	    b4 = input[4], b5 = input[5],
+	    b6 = input[6], b7 = input[7],
+	    b8 = input[8], b9 = input[9],
+	    b10 = input[10], b11 = input[11],
+	    b12 = input[12], b13 = input[13],
+	    b14 = input[14], b15 = input[15];
+	u64 k0 = key_ctx->key[0], k1 = key_ctx->key[1],
+	    k2 = key_ctx->key[2], k3 = key_ctx->key[3],
+	    k4 = key_ctx->key[4], k5 = key_ctx->key[5],
+	    k6 = key_ctx->key[6], k7 = key_ctx->key[7],
+	    k8 = key_ctx->key[8], k9 = key_ctx->key[9],
+	    k10 = key_ctx->key[10], k11 = key_ctx->key[11],
+	    k12 = key_ctx->key[12], k13 = key_ctx->key[13],
+	    k14 = key_ctx->key[14], k15 = key_ctx->key[15],
+	    k16 = key_ctx->key[16];
+	u64 t0 = key_ctx->tweak[0], t1 = key_ctx->tweak[1],
+	    t2 = key_ctx->tweak[2];
+
+	b1 += k1;
+	b0 += b1 + k0;
+	b1 = ((b1 << 24) | (b1 >> (64 - 24))) ^ b0;
+
+	b3 += k3;
+	b2 += b3 + k2;
+	b3 = ((b3 << 13) | (b3 >> (64 - 13))) ^ b2;
+
+	b5 += k5;
+	b4 += b5 + k4;
+	b5 = ((b5 << 8) | (b5 >> (64 - 8))) ^ b4;
+
+	b7 += k7;
+	b6 += b7 + k6;
+	b7 = ((b7 << 47) | (b7 >> (64 - 47))) ^ b6;
+
+	b9 += k9;
+	b8 += b9 + k8;
+	b9 = ((b9 << 8) | (b9 >> (64 - 8))) ^ b8;
+
+	b11 += k11;
+	b10 += b11 + k10;
+	b11 = ((b11 << 17) | (b11 >> (64 - 17))) ^ b10;
+
+	b13 += k13 + t0;
+	b12 += b13 + k12;
+	b13 = ((b13 << 22) | (b13 >> (64 - 22))) ^ b12;
+
+	b15 += k15;
+	b14 += b15 + k14 + t1;
+	b15 = ((b15 << 37) | (b15 >> (64 - 37))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 38) | (b9 >> (64 - 38))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 19) | (b13 >> (64 - 19))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 10) | (b11 >> (64 - 10))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 55) | (b15 >> (64 - 55))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 49) | (b7 >> (64 - 49))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 18) | (b3 >> (64 - 18))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 23) | (b5 >> (64 - 23))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 52) | (b1 >> (64 - 52))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 33) | (b7 >> (64 - 33))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 4) | (b5 >> (64 - 4))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 51) | (b3 >> (64 - 51))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 34) | (b15 >> (64 - 34))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 41) | (b13 >> (64 - 41))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 59) | (b11 >> (64 - 59))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 17) | (b9 >> (64 - 17))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 5) | (b15 >> (64 - 5))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 20) | (b11 >> (64 - 20))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 48) | (b13 >> (64 - 48))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 41) | (b9 >> (64 - 41))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 47) | (b1 >> (64 - 47))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 28) | (b5 >> (64 - 28))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 25) | (b7 >> (64 - 25))) ^ b12;
+
+	b1 += k2;
+	b0 += b1 + k1;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b0;
+
+	b3 += k4;
+	b2 += b3 + k3;
+	b3 = ((b3 << 9) | (b3 >> (64 - 9))) ^ b2;
+
+	b5 += k6;
+	b4 += b5 + k5;
+	b5 = ((b5 << 37) | (b5 >> (64 - 37))) ^ b4;
+
+	b7 += k8;
+	b6 += b7 + k7;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b6;
+
+	b9 += k10;
+	b8 += b9 + k9;
+	b9 = ((b9 << 12) | (b9 >> (64 - 12))) ^ b8;
+
+	b11 += k12;
+	b10 += b11 + k11;
+	b11 = ((b11 << 47) | (b11 >> (64 - 47))) ^ b10;
+
+	b13 += k14 + t1;
+	b12 += b13 + k13;
+	b13 = ((b13 << 44) | (b13 >> (64 - 44))) ^ b12;
+
+	b15 += k16 + 1;
+	b14 += b15 + k15 + t2;
+	b15 = ((b15 << 30) | (b15 >> (64 - 30))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 16) | (b9 >> (64 - 16))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 34) | (b13 >> (64 - 34))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 56) | (b11 >> (64 - 56))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 51) | (b15 >> (64 - 51))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 4) | (b7 >> (64 - 4))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 53) | (b3 >> (64 - 53))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 42) | (b5 >> (64 - 42))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 44) | (b5 >> (64 - 44))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 47) | (b3 >> (64 - 47))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 19) | (b15 >> (64 - 19))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 42) | (b13 >> (64 - 42))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 44) | (b11 >> (64 - 44))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 25) | (b9 >> (64 - 25))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 9) | (b15 >> (64 - 9))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 48) | (b11 >> (64 - 48))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 35) | (b13 >> (64 - 35))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 52) | (b9 >> (64 - 52))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 31) | (b5 >> (64 - 31))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 37) | (b3 >> (64 - 37))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 20) | (b7 >> (64 - 20))) ^ b12;
+
+	b1 += k3;
+	b0 += b1 + k2;
+	b1 = ((b1 << 24) | (b1 >> (64 - 24))) ^ b0;
+
+	b3 += k5;
+	b2 += b3 + k4;
+	b3 = ((b3 << 13) | (b3 >> (64 - 13))) ^ b2;
+
+	b5 += k7;
+	b4 += b5 + k6;
+	b5 = ((b5 << 8) | (b5 >> (64 - 8))) ^ b4;
+
+	b7 += k9;
+	b6 += b7 + k8;
+	b7 = ((b7 << 47) | (b7 >> (64 - 47))) ^ b6;
+
+	b9 += k11;
+	b8 += b9 + k10;
+	b9 = ((b9 << 8) | (b9 >> (64 - 8))) ^ b8;
+
+	b11 += k13;
+	b10 += b11 + k12;
+	b11 = ((b11 << 17) | (b11 >> (64 - 17))) ^ b10;
+
+	b13 += k15 + t2;
+	b12 += b13 + k14;
+	b13 = ((b13 << 22) | (b13 >> (64 - 22))) ^ b12;
+
+	b15 += k0 + 2;
+	b14 += b15 + k16 + t0;
+	b15 = ((b15 << 37) | (b15 >> (64 - 37))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 38) | (b9 >> (64 - 38))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 19) | (b13 >> (64 - 19))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 10) | (b11 >> (64 - 10))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 55) | (b15 >> (64 - 55))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 49) | (b7 >> (64 - 49))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 18) | (b3 >> (64 - 18))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 23) | (b5 >> (64 - 23))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 52) | (b1 >> (64 - 52))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 33) | (b7 >> (64 - 33))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 4) | (b5 >> (64 - 4))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 51) | (b3 >> (64 - 51))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 34) | (b15 >> (64 - 34))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 41) | (b13 >> (64 - 41))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 59) | (b11 >> (64 - 59))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 17) | (b9 >> (64 - 17))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 5) | (b15 >> (64 - 5))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 20) | (b11 >> (64 - 20))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 48) | (b13 >> (64 - 48))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 41) | (b9 >> (64 - 41))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 47) | (b1 >> (64 - 47))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 28) | (b5 >> (64 - 28))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 25) | (b7 >> (64 - 25))) ^ b12;
+
+	b1 += k4;
+	b0 += b1 + k3;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b0;
+
+	b3 += k6;
+	b2 += b3 + k5;
+	b3 = ((b3 << 9) | (b3 >> (64 - 9))) ^ b2;
+
+	b5 += k8;
+	b4 += b5 + k7;
+	b5 = ((b5 << 37) | (b5 >> (64 - 37))) ^ b4;
+
+	b7 += k10;
+	b6 += b7 + k9;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b6;
+
+	b9 += k12;
+	b8 += b9 + k11;
+	b9 = ((b9 << 12) | (b9 >> (64 - 12))) ^ b8;
+
+	b11 += k14;
+	b10 += b11 + k13;
+	b11 = ((b11 << 47) | (b11 >> (64 - 47))) ^ b10;
+
+	b13 += k16 + t0;
+	b12 += b13 + k15;
+	b13 = ((b13 << 44) | (b13 >> (64 - 44))) ^ b12;
+
+	b15 += k1 + 3;
+	b14 += b15 + k0 + t1;
+	b15 = ((b15 << 30) | (b15 >> (64 - 30))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 16) | (b9 >> (64 - 16))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 34) | (b13 >> (64 - 34))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 56) | (b11 >> (64 - 56))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 51) | (b15 >> (64 - 51))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 4) | (b7 >> (64 - 4))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 53) | (b3 >> (64 - 53))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 42) | (b5 >> (64 - 42))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 44) | (b5 >> (64 - 44))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 47) | (b3 >> (64 - 47))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 19) | (b15 >> (64 - 19))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 42) | (b13 >> (64 - 42))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 44) | (b11 >> (64 - 44))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 25) | (b9 >> (64 - 25))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 9) | (b15 >> (64 - 9))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 48) | (b11 >> (64 - 48))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 35) | (b13 >> (64 - 35))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 52) | (b9 >> (64 - 52))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 31) | (b5 >> (64 - 31))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 37) | (b3 >> (64 - 37))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 20) | (b7 >> (64 - 20))) ^ b12;
+
+	b1 += k5;
+	b0 += b1 + k4;
+	b1 = ((b1 << 24) | (b1 >> (64 - 24))) ^ b0;
+
+	b3 += k7;
+	b2 += b3 + k6;
+	b3 = ((b3 << 13) | (b3 >> (64 - 13))) ^ b2;
+
+	b5 += k9;
+	b4 += b5 + k8;
+	b5 = ((b5 << 8) | (b5 >> (64 - 8))) ^ b4;
+
+	b7 += k11;
+	b6 += b7 + k10;
+	b7 = ((b7 << 47) | (b7 >> (64 - 47))) ^ b6;
+
+	b9 += k13;
+	b8 += b9 + k12;
+	b9 = ((b9 << 8) | (b9 >> (64 - 8))) ^ b8;
+
+	b11 += k15;
+	b10 += b11 + k14;
+	b11 = ((b11 << 17) | (b11 >> (64 - 17))) ^ b10;
+
+	b13 += k0 + t1;
+	b12 += b13 + k16;
+	b13 = ((b13 << 22) | (b13 >> (64 - 22))) ^ b12;
+
+	b15 += k2 + 4;
+	b14 += b15 + k1 + t2;
+	b15 = ((b15 << 37) | (b15 >> (64 - 37))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 38) | (b9 >> (64 - 38))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 19) | (b13 >> (64 - 19))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 10) | (b11 >> (64 - 10))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 55) | (b15 >> (64 - 55))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 49) | (b7 >> (64 - 49))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 18) | (b3 >> (64 - 18))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 23) | (b5 >> (64 - 23))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 52) | (b1 >> (64 - 52))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 33) | (b7 >> (64 - 33))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 4) | (b5 >> (64 - 4))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 51) | (b3 >> (64 - 51))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 34) | (b15 >> (64 - 34))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 41) | (b13 >> (64 - 41))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 59) | (b11 >> (64 - 59))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 17) | (b9 >> (64 - 17))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 5) | (b15 >> (64 - 5))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 20) | (b11 >> (64 - 20))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 48) | (b13 >> (64 - 48))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 41) | (b9 >> (64 - 41))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 47) | (b1 >> (64 - 47))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 28) | (b5 >> (64 - 28))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 25) | (b7 >> (64 - 25))) ^ b12;
+
+	b1 += k6;
+	b0 += b1 + k5;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b0;
+
+	b3 += k8;
+	b2 += b3 + k7;
+	b3 = ((b3 << 9) | (b3 >> (64 - 9))) ^ b2;
+
+	b5 += k10;
+	b4 += b5 + k9;
+	b5 = ((b5 << 37) | (b5 >> (64 - 37))) ^ b4;
+
+	b7 += k12;
+	b6 += b7 + k11;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b6;
+
+	b9 += k14;
+	b8 += b9 + k13;
+	b9 = ((b9 << 12) | (b9 >> (64 - 12))) ^ b8;
+
+	b11 += k16;
+	b10 += b11 + k15;
+	b11 = ((b11 << 47) | (b11 >> (64 - 47))) ^ b10;
+
+	b13 += k1 + t2;
+	b12 += b13 + k0;
+	b13 = ((b13 << 44) | (b13 >> (64 - 44))) ^ b12;
+
+	b15 += k3 + 5;
+	b14 += b15 + k2 + t0;
+	b15 = ((b15 << 30) | (b15 >> (64 - 30))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 16) | (b9 >> (64 - 16))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 34) | (b13 >> (64 - 34))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 56) | (b11 >> (64 - 56))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 51) | (b15 >> (64 - 51))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 4) | (b7 >> (64 - 4))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 53) | (b3 >> (64 - 53))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 42) | (b5 >> (64 - 42))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 44) | (b5 >> (64 - 44))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 47) | (b3 >> (64 - 47))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 19) | (b15 >> (64 - 19))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 42) | (b13 >> (64 - 42))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 44) | (b11 >> (64 - 44))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 25) | (b9 >> (64 - 25))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 9) | (b15 >> (64 - 9))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 48) | (b11 >> (64 - 48))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 35) | (b13 >> (64 - 35))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 52) | (b9 >> (64 - 52))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 31) | (b5 >> (64 - 31))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 37) | (b3 >> (64 - 37))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 20) | (b7 >> (64 - 20))) ^ b12;
+
+	b1 += k7;
+	b0 += b1 + k6;
+	b1 = ((b1 << 24) | (b1 >> (64 - 24))) ^ b0;
+
+	b3 += k9;
+	b2 += b3 + k8;
+	b3 = ((b3 << 13) | (b3 >> (64 - 13))) ^ b2;
+
+	b5 += k11;
+	b4 += b5 + k10;
+	b5 = ((b5 << 8) | (b5 >> (64 - 8))) ^ b4;
+
+	b7 += k13;
+	b6 += b7 + k12;
+	b7 = ((b7 << 47) | (b7 >> (64 - 47))) ^ b6;
+
+	b9 += k15;
+	b8 += b9 + k14;
+	b9 = ((b9 << 8) | (b9 >> (64 - 8))) ^ b8;
+
+	b11 += k0;
+	b10 += b11 + k16;
+	b11 = ((b11 << 17) | (b11 >> (64 - 17))) ^ b10;
+
+	b13 += k2 + t0;
+	b12 += b13 + k1;
+	b13 = ((b13 << 22) | (b13 >> (64 - 22))) ^ b12;
+
+	b15 += k4 + 6;
+	b14 += b15 + k3 + t1;
+	b15 = ((b15 << 37) | (b15 >> (64 - 37))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 38) | (b9 >> (64 - 38))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 19) | (b13 >> (64 - 19))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 10) | (b11 >> (64 - 10))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 55) | (b15 >> (64 - 55))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 49) | (b7 >> (64 - 49))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 18) | (b3 >> (64 - 18))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 23) | (b5 >> (64 - 23))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 52) | (b1 >> (64 - 52))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 33) | (b7 >> (64 - 33))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 4) | (b5 >> (64 - 4))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 51) | (b3 >> (64 - 51))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 34) | (b15 >> (64 - 34))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 41) | (b13 >> (64 - 41))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 59) | (b11 >> (64 - 59))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 17) | (b9 >> (64 - 17))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 5) | (b15 >> (64 - 5))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 20) | (b11 >> (64 - 20))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 48) | (b13 >> (64 - 48))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 41) | (b9 >> (64 - 41))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 47) | (b1 >> (64 - 47))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 28) | (b5 >> (64 - 28))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 25) | (b7 >> (64 - 25))) ^ b12;
+
+	b1 += k8;
+	b0 += b1 + k7;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b0;
+
+	b3 += k10;
+	b2 += b3 + k9;
+	b3 = ((b3 << 9) | (b3 >> (64 - 9))) ^ b2;
+
+	b5 += k12;
+	b4 += b5 + k11;
+	b5 = ((b5 << 37) | (b5 >> (64 - 37))) ^ b4;
+
+	b7 += k14;
+	b6 += b7 + k13;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b6;
+
+	b9 += k16;
+	b8 += b9 + k15;
+	b9 = ((b9 << 12) | (b9 >> (64 - 12))) ^ b8;
+
+	b11 += k1;
+	b10 += b11 + k0;
+	b11 = ((b11 << 47) | (b11 >> (64 - 47))) ^ b10;
+
+	b13 += k3 + t1;
+	b12 += b13 + k2;
+	b13 = ((b13 << 44) | (b13 >> (64 - 44))) ^ b12;
+
+	b15 += k5 + 7;
+	b14 += b15 + k4 + t2;
+	b15 = ((b15 << 30) | (b15 >> (64 - 30))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 16) | (b9 >> (64 - 16))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 34) | (b13 >> (64 - 34))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 56) | (b11 >> (64 - 56))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 51) | (b15 >> (64 - 51))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 4) | (b7 >> (64 - 4))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 53) | (b3 >> (64 - 53))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 42) | (b5 >> (64 - 42))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 44) | (b5 >> (64 - 44))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 47) | (b3 >> (64 - 47))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 19) | (b15 >> (64 - 19))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 42) | (b13 >> (64 - 42))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 44) | (b11 >> (64 - 44))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 25) | (b9 >> (64 - 25))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 9) | (b15 >> (64 - 9))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 48) | (b11 >> (64 - 48))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 35) | (b13 >> (64 - 35))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 52) | (b9 >> (64 - 52))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 31) | (b5 >> (64 - 31))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 37) | (b3 >> (64 - 37))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 20) | (b7 >> (64 - 20))) ^ b12;
+
+	b1 += k9;
+	b0 += b1 + k8;
+	b1 = ((b1 << 24) | (b1 >> (64 - 24))) ^ b0;
+
+	b3 += k11;
+	b2 += b3 + k10;
+	b3 = ((b3 << 13) | (b3 >> (64 - 13))) ^ b2;
+
+	b5 += k13;
+	b4 += b5 + k12;
+	b5 = ((b5 << 8) | (b5 >> (64 - 8))) ^ b4;
+
+	b7 += k15;
+	b6 += b7 + k14;
+	b7 = ((b7 << 47) | (b7 >> (64 - 47))) ^ b6;
+
+	b9 += k0;
+	b8 += b9 + k16;
+	b9 = ((b9 << 8) | (b9 >> (64 - 8))) ^ b8;
+
+	b11 += k2;
+	b10 += b11 + k1;
+	b11 = ((b11 << 17) | (b11 >> (64 - 17))) ^ b10;
+
+	b13 += k4 + t2;
+	b12 += b13 + k3;
+	b13 = ((b13 << 22) | (b13 >> (64 - 22))) ^ b12;
+
+	b15 += k6 + 8;
+	b14 += b15 + k5 + t0;
+	b15 = ((b15 << 37) | (b15 >> (64 - 37))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 38) | (b9 >> (64 - 38))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 19) | (b13 >> (64 - 19))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 10) | (b11 >> (64 - 10))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 55) | (b15 >> (64 - 55))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 49) | (b7 >> (64 - 49))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 18) | (b3 >> (64 - 18))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 23) | (b5 >> (64 - 23))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 52) | (b1 >> (64 - 52))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 33) | (b7 >> (64 - 33))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 4) | (b5 >> (64 - 4))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 51) | (b3 >> (64 - 51))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 34) | (b15 >> (64 - 34))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 41) | (b13 >> (64 - 41))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 59) | (b11 >> (64 - 59))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 17) | (b9 >> (64 - 17))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 5) | (b15 >> (64 - 5))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 20) | (b11 >> (64 - 20))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 48) | (b13 >> (64 - 48))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 41) | (b9 >> (64 - 41))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 47) | (b1 >> (64 - 47))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 28) | (b5 >> (64 - 28))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 25) | (b7 >> (64 - 25))) ^ b12;
+
+	b1 += k10;
+	b0 += b1 + k9;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b0;
+
+	b3 += k12;
+	b2 += b3 + k11;
+	b3 = ((b3 << 9) | (b3 >> (64 - 9))) ^ b2;
+
+	b5 += k14;
+	b4 += b5 + k13;
+	b5 = ((b5 << 37) | (b5 >> (64 - 37))) ^ b4;
+
+	b7 += k16;
+	b6 += b7 + k15;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b6;
+
+	b9 += k1;
+	b8 += b9 + k0;
+	b9 = ((b9 << 12) | (b9 >> (64 - 12))) ^ b8;
+
+	b11 += k3;
+	b10 += b11 + k2;
+	b11 = ((b11 << 47) | (b11 >> (64 - 47))) ^ b10;
+
+	b13 += k5 + t0;
+	b12 += b13 + k4;
+	b13 = ((b13 << 44) | (b13 >> (64 - 44))) ^ b12;
+
+	b15 += k7 + 9;
+	b14 += b15 + k6 + t1;
+	b15 = ((b15 << 30) | (b15 >> (64 - 30))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 16) | (b9 >> (64 - 16))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 34) | (b13 >> (64 - 34))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 56) | (b11 >> (64 - 56))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 51) | (b15 >> (64 - 51))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 4) | (b7 >> (64 - 4))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 53) | (b3 >> (64 - 53))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 42) | (b5 >> (64 - 42))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 44) | (b5 >> (64 - 44))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 47) | (b3 >> (64 - 47))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 19) | (b15 >> (64 - 19))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 42) | (b13 >> (64 - 42))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 44) | (b11 >> (64 - 44))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 25) | (b9 >> (64 - 25))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 9) | (b15 >> (64 - 9))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 48) | (b11 >> (64 - 48))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 35) | (b13 >> (64 - 35))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 52) | (b9 >> (64 - 52))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 31) | (b5 >> (64 - 31))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 37) | (b3 >> (64 - 37))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 20) | (b7 >> (64 - 20))) ^ b12;
+
+	b1 += k11;
+	b0 += b1 + k10;
+	b1 = ((b1 << 24) | (b1 >> (64 - 24))) ^ b0;
+
+	b3 += k13;
+	b2 += b3 + k12;
+	b3 = ((b3 << 13) | (b3 >> (64 - 13))) ^ b2;
+
+	b5 += k15;
+	b4 += b5 + k14;
+	b5 = ((b5 << 8) | (b5 >> (64 - 8))) ^ b4;
+
+	b7 += k0;
+	b6 += b7 + k16;
+	b7 = ((b7 << 47) | (b7 >> (64 - 47))) ^ b6;
+
+	b9 += k2;
+	b8 += b9 + k1;
+	b9 = ((b9 << 8) | (b9 >> (64 - 8))) ^ b8;
+
+	b11 += k4;
+	b10 += b11 + k3;
+	b11 = ((b11 << 17) | (b11 >> (64 - 17))) ^ b10;
+
+	b13 += k6 + t1;
+	b12 += b13 + k5;
+	b13 = ((b13 << 22) | (b13 >> (64 - 22))) ^ b12;
+
+	b15 += k8 + 10;
+	b14 += b15 + k7 + t2;
+	b15 = ((b15 << 37) | (b15 >> (64 - 37))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 38) | (b9 >> (64 - 38))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 19) | (b13 >> (64 - 19))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 10) | (b11 >> (64 - 10))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 55) | (b15 >> (64 - 55))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 49) | (b7 >> (64 - 49))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 18) | (b3 >> (64 - 18))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 23) | (b5 >> (64 - 23))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 52) | (b1 >> (64 - 52))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 33) | (b7 >> (64 - 33))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 4) | (b5 >> (64 - 4))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 51) | (b3 >> (64 - 51))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 34) | (b15 >> (64 - 34))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 41) | (b13 >> (64 - 41))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 59) | (b11 >> (64 - 59))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 17) | (b9 >> (64 - 17))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 5) | (b15 >> (64 - 5))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 20) | (b11 >> (64 - 20))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 48) | (b13 >> (64 - 48))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 41) | (b9 >> (64 - 41))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 47) | (b1 >> (64 - 47))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 28) | (b5 >> (64 - 28))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 25) | (b7 >> (64 - 25))) ^ b12;
+
+	b1 += k12;
+	b0 += b1 + k11;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b0;
+
+	b3 += k14;
+	b2 += b3 + k13;
+	b3 = ((b3 << 9) | (b3 >> (64 - 9))) ^ b2;
+
+	b5 += k16;
+	b4 += b5 + k15;
+	b5 = ((b5 << 37) | (b5 >> (64 - 37))) ^ b4;
+
+	b7 += k1;
+	b6 += b7 + k0;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b6;
+
+	b9 += k3;
+	b8 += b9 + k2;
+	b9 = ((b9 << 12) | (b9 >> (64 - 12))) ^ b8;
+
+	b11 += k5;
+	b10 += b11 + k4;
+	b11 = ((b11 << 47) | (b11 >> (64 - 47))) ^ b10;
+
+	b13 += k7 + t2;
+	b12 += b13 + k6;
+	b13 = ((b13 << 44) | (b13 >> (64 - 44))) ^ b12;
+
+	b15 += k9 + 11;
+	b14 += b15 + k8 + t0;
+	b15 = ((b15 << 30) | (b15 >> (64 - 30))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 16) | (b9 >> (64 - 16))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 34) | (b13 >> (64 - 34))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 56) | (b11 >> (64 - 56))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 51) | (b15 >> (64 - 51))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 4) | (b7 >> (64 - 4))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 53) | (b3 >> (64 - 53))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 42) | (b5 >> (64 - 42))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 44) | (b5 >> (64 - 44))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 47) | (b3 >> (64 - 47))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 19) | (b15 >> (64 - 19))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 42) | (b13 >> (64 - 42))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 44) | (b11 >> (64 - 44))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 25) | (b9 >> (64 - 25))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 9) | (b15 >> (64 - 9))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 48) | (b11 >> (64 - 48))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 35) | (b13 >> (64 - 35))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 52) | (b9 >> (64 - 52))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 31) | (b5 >> (64 - 31))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 37) | (b3 >> (64 - 37))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 20) | (b7 >> (64 - 20))) ^ b12;
+
+	b1 += k13;
+	b0 += b1 + k12;
+	b1 = ((b1 << 24) | (b1 >> (64 - 24))) ^ b0;
+
+	b3 += k15;
+	b2 += b3 + k14;
+	b3 = ((b3 << 13) | (b3 >> (64 - 13))) ^ b2;
+
+	b5 += k0;
+	b4 += b5 + k16;
+	b5 = ((b5 << 8) | (b5 >> (64 - 8))) ^ b4;
+
+	b7 += k2;
+	b6 += b7 + k1;
+	b7 = ((b7 << 47) | (b7 >> (64 - 47))) ^ b6;
+
+	b9 += k4;
+	b8 += b9 + k3;
+	b9 = ((b9 << 8) | (b9 >> (64 - 8))) ^ b8;
+
+	b11 += k6;
+	b10 += b11 + k5;
+	b11 = ((b11 << 17) | (b11 >> (64 - 17))) ^ b10;
+
+	b13 += k8 + t0;
+	b12 += b13 + k7;
+	b13 = ((b13 << 22) | (b13 >> (64 - 22))) ^ b12;
+
+	b15 += k10 + 12;
+	b14 += b15 + k9 + t1;
+	b15 = ((b15 << 37) | (b15 >> (64 - 37))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 38) | (b9 >> (64 - 38))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 19) | (b13 >> (64 - 19))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 10) | (b11 >> (64 - 10))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 55) | (b15 >> (64 - 55))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 49) | (b7 >> (64 - 49))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 18) | (b3 >> (64 - 18))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 23) | (b5 >> (64 - 23))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 52) | (b1 >> (64 - 52))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 33) | (b7 >> (64 - 33))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 4) | (b5 >> (64 - 4))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 51) | (b3 >> (64 - 51))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 34) | (b15 >> (64 - 34))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 41) | (b13 >> (64 - 41))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 59) | (b11 >> (64 - 59))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 17) | (b9 >> (64 - 17))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 5) | (b15 >> (64 - 5))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 20) | (b11 >> (64 - 20))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 48) | (b13 >> (64 - 48))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 41) | (b9 >> (64 - 41))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 47) | (b1 >> (64 - 47))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 28) | (b5 >> (64 - 28))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 25) | (b7 >> (64 - 25))) ^ b12;
+
+	b1 += k14;
+	b0 += b1 + k13;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b0;
+
+	b3 += k16;
+	b2 += b3 + k15;
+	b3 = ((b3 << 9) | (b3 >> (64 - 9))) ^ b2;
+
+	b5 += k1;
+	b4 += b5 + k0;
+	b5 = ((b5 << 37) | (b5 >> (64 - 37))) ^ b4;
+
+	b7 += k3;
+	b6 += b7 + k2;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b6;
+
+	b9 += k5;
+	b8 += b9 + k4;
+	b9 = ((b9 << 12) | (b9 >> (64 - 12))) ^ b8;
+
+	b11 += k7;
+	b10 += b11 + k6;
+	b11 = ((b11 << 47) | (b11 >> (64 - 47))) ^ b10;
+
+	b13 += k9 + t1;
+	b12 += b13 + k8;
+	b13 = ((b13 << 44) | (b13 >> (64 - 44))) ^ b12;
+
+	b15 += k11 + 13;
+	b14 += b15 + k10 + t2;
+	b15 = ((b15 << 30) | (b15 >> (64 - 30))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 16) | (b9 >> (64 - 16))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 34) | (b13 >> (64 - 34))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 56) | (b11 >> (64 - 56))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 51) | (b15 >> (64 - 51))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 4) | (b7 >> (64 - 4))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 53) | (b3 >> (64 - 53))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 42) | (b5 >> (64 - 42))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 44) | (b5 >> (64 - 44))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 47) | (b3 >> (64 - 47))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 19) | (b15 >> (64 - 19))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 42) | (b13 >> (64 - 42))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 44) | (b11 >> (64 - 44))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 25) | (b9 >> (64 - 25))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 9) | (b15 >> (64 - 9))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 48) | (b11 >> (64 - 48))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 35) | (b13 >> (64 - 35))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 52) | (b9 >> (64 - 52))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 31) | (b5 >> (64 - 31))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 37) | (b3 >> (64 - 37))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 20) | (b7 >> (64 - 20))) ^ b12;
+
+	b1 += k15;
+	b0 += b1 + k14;
+	b1 = ((b1 << 24) | (b1 >> (64 - 24))) ^ b0;
+
+	b3 += k0;
+	b2 += b3 + k16;
+	b3 = ((b3 << 13) | (b3 >> (64 - 13))) ^ b2;
+
+	b5 += k2;
+	b4 += b5 + k1;
+	b5 = ((b5 << 8) | (b5 >> (64 - 8))) ^ b4;
+
+	b7 += k4;
+	b6 += b7 + k3;
+	b7 = ((b7 << 47) | (b7 >> (64 - 47))) ^ b6;
+
+	b9 += k6;
+	b8 += b9 + k5;
+	b9 = ((b9 << 8) | (b9 >> (64 - 8))) ^ b8;
+
+	b11 += k8;
+	b10 += b11 + k7;
+	b11 = ((b11 << 17) | (b11 >> (64 - 17))) ^ b10;
+
+	b13 += k10 + t2;
+	b12 += b13 + k9;
+	b13 = ((b13 << 22) | (b13 >> (64 - 22))) ^ b12;
+
+	b15 += k12 + 14;
+	b14 += b15 + k11 + t0;
+	b15 = ((b15 << 37) | (b15 >> (64 - 37))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 38) | (b9 >> (64 - 38))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 19) | (b13 >> (64 - 19))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 10) | (b11 >> (64 - 10))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 55) | (b15 >> (64 - 55))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 49) | (b7 >> (64 - 49))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 18) | (b3 >> (64 - 18))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 23) | (b5 >> (64 - 23))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 52) | (b1 >> (64 - 52))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 33) | (b7 >> (64 - 33))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 4) | (b5 >> (64 - 4))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 51) | (b3 >> (64 - 51))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 34) | (b15 >> (64 - 34))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 41) | (b13 >> (64 - 41))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 59) | (b11 >> (64 - 59))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 17) | (b9 >> (64 - 17))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 5) | (b15 >> (64 - 5))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 20) | (b11 >> (64 - 20))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 48) | (b13 >> (64 - 48))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 41) | (b9 >> (64 - 41))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 47) | (b1 >> (64 - 47))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 28) | (b5 >> (64 - 28))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 25) | (b7 >> (64 - 25))) ^ b12;
+
+	b1 += k16;
+	b0 += b1 + k15;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b0;
+
+	b3 += k1;
+	b2 += b3 + k0;
+	b3 = ((b3 << 9) | (b3 >> (64 - 9))) ^ b2;
+
+	b5 += k3;
+	b4 += b5 + k2;
+	b5 = ((b5 << 37) | (b5 >> (64 - 37))) ^ b4;
+
+	b7 += k5;
+	b6 += b7 + k4;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b6;
+
+	b9 += k7;
+	b8 += b9 + k6;
+	b9 = ((b9 << 12) | (b9 >> (64 - 12))) ^ b8;
+
+	b11 += k9;
+	b10 += b11 + k8;
+	b11 = ((b11 << 47) | (b11 >> (64 - 47))) ^ b10;
+
+	b13 += k11 + t0;
+	b12 += b13 + k10;
+	b13 = ((b13 << 44) | (b13 >> (64 - 44))) ^ b12;
+
+	b15 += k13 + 15;
+	b14 += b15 + k12 + t1;
+	b15 = ((b15 << 30) | (b15 >> (64 - 30))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 16) | (b9 >> (64 - 16))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 34) | (b13 >> (64 - 34))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 56) | (b11 >> (64 - 56))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 51) | (b15 >> (64 - 51))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 4) | (b7 >> (64 - 4))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 53) | (b3 >> (64 - 53))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 42) | (b5 >> (64 - 42))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 44) | (b5 >> (64 - 44))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 47) | (b3 >> (64 - 47))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 19) | (b15 >> (64 - 19))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 42) | (b13 >> (64 - 42))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 44) | (b11 >> (64 - 44))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 25) | (b9 >> (64 - 25))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 9) | (b15 >> (64 - 9))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 48) | (b11 >> (64 - 48))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 35) | (b13 >> (64 - 35))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 52) | (b9 >> (64 - 52))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 31) | (b5 >> (64 - 31))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 37) | (b3 >> (64 - 37))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 20) | (b7 >> (64 - 20))) ^ b12;
+
+	b1 += k0;
+	b0 += b1 + k16;
+	b1 = ((b1 << 24) | (b1 >> (64 - 24))) ^ b0;
+
+	b3 += k2;
+	b2 += b3 + k1;
+	b3 = ((b3 << 13) | (b3 >> (64 - 13))) ^ b2;
+
+	b5 += k4;
+	b4 += b5 + k3;
+	b5 = ((b5 << 8) | (b5 >> (64 - 8))) ^ b4;
+
+	b7 += k6;
+	b6 += b7 + k5;
+	b7 = ((b7 << 47) | (b7 >> (64 - 47))) ^ b6;
+
+	b9 += k8;
+	b8 += b9 + k7;
+	b9 = ((b9 << 8) | (b9 >> (64 - 8))) ^ b8;
+
+	b11 += k10;
+	b10 += b11 + k9;
+	b11 = ((b11 << 17) | (b11 >> (64 - 17))) ^ b10;
+
+	b13 += k12 + t1;
+	b12 += b13 + k11;
+	b13 = ((b13 << 22) | (b13 >> (64 - 22))) ^ b12;
+
+	b15 += k14 + 16;
+	b14 += b15 + k13 + t2;
+	b15 = ((b15 << 37) | (b15 >> (64 - 37))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 38) | (b9 >> (64 - 38))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 19) | (b13 >> (64 - 19))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 10) | (b11 >> (64 - 10))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 55) | (b15 >> (64 - 55))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 49) | (b7 >> (64 - 49))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 18) | (b3 >> (64 - 18))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 23) | (b5 >> (64 - 23))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 52) | (b1 >> (64 - 52))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 33) | (b7 >> (64 - 33))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 4) | (b5 >> (64 - 4))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 51) | (b3 >> (64 - 51))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 34) | (b15 >> (64 - 34))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 41) | (b13 >> (64 - 41))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 59) | (b11 >> (64 - 59))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 17) | (b9 >> (64 - 17))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 5) | (b15 >> (64 - 5))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 20) | (b11 >> (64 - 20))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 48) | (b13 >> (64 - 48))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 41) | (b9 >> (64 - 41))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 47) | (b1 >> (64 - 47))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 28) | (b5 >> (64 - 28))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 25) | (b7 >> (64 - 25))) ^ b12;
+
+	b1 += k1;
+	b0 += b1 + k0;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b0;
+
+	b3 += k3;
+	b2 += b3 + k2;
+	b3 = ((b3 << 9) | (b3 >> (64 - 9))) ^ b2;
+
+	b5 += k5;
+	b4 += b5 + k4;
+	b5 = ((b5 << 37) | (b5 >> (64 - 37))) ^ b4;
+
+	b7 += k7;
+	b6 += b7 + k6;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b6;
+
+	b9 += k9;
+	b8 += b9 + k8;
+	b9 = ((b9 << 12) | (b9 >> (64 - 12))) ^ b8;
+
+	b11 += k11;
+	b10 += b11 + k10;
+	b11 = ((b11 << 47) | (b11 >> (64 - 47))) ^ b10;
+
+	b13 += k13 + t2;
+	b12 += b13 + k12;
+	b13 = ((b13 << 44) | (b13 >> (64 - 44))) ^ b12;
+
+	b15 += k15 + 17;
+	b14 += b15 + k14 + t0;
+	b15 = ((b15 << 30) | (b15 >> (64 - 30))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 16) | (b9 >> (64 - 16))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 34) | (b13 >> (64 - 34))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 56) | (b11 >> (64 - 56))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 51) | (b15 >> (64 - 51))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 4) | (b7 >> (64 - 4))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 53) | (b3 >> (64 - 53))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 42) | (b5 >> (64 - 42))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 44) | (b5 >> (64 - 44))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 47) | (b3 >> (64 - 47))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 19) | (b15 >> (64 - 19))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 42) | (b13 >> (64 - 42))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 44) | (b11 >> (64 - 44))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 25) | (b9 >> (64 - 25))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 9) | (b15 >> (64 - 9))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 48) | (b11 >> (64 - 48))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 35) | (b13 >> (64 - 35))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 52) | (b9 >> (64 - 52))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 31) | (b5 >> (64 - 31))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 37) | (b3 >> (64 - 37))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 20) | (b7 >> (64 - 20))) ^ b12;
+
+	b1 += k2;
+	b0 += b1 + k1;
+	b1 = ((b1 << 24) | (b1 >> (64 - 24))) ^ b0;
+
+	b3 += k4;
+	b2 += b3 + k3;
+	b3 = ((b3 << 13) | (b3 >> (64 - 13))) ^ b2;
+
+	b5 += k6;
+	b4 += b5 + k5;
+	b5 = ((b5 << 8) | (b5 >> (64 - 8))) ^ b4;
+
+	b7 += k8;
+	b6 += b7 + k7;
+	b7 = ((b7 << 47) | (b7 >> (64 - 47))) ^ b6;
+
+	b9 += k10;
+	b8 += b9 + k9;
+	b9 = ((b9 << 8) | (b9 >> (64 - 8))) ^ b8;
+
+	b11 += k12;
+	b10 += b11 + k11;
+	b11 = ((b11 << 17) | (b11 >> (64 - 17))) ^ b10;
+
+	b13 += k14 + t0;
+	b12 += b13 + k13;
+	b13 = ((b13 << 22) | (b13 >> (64 - 22))) ^ b12;
+
+	b15 += k16 + 18;
+	b14 += b15 + k15 + t1;
+	b15 = ((b15 << 37) | (b15 >> (64 - 37))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 38) | (b9 >> (64 - 38))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 19) | (b13 >> (64 - 19))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 10) | (b11 >> (64 - 10))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 55) | (b15 >> (64 - 55))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 49) | (b7 >> (64 - 49))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 18) | (b3 >> (64 - 18))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 23) | (b5 >> (64 - 23))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 52) | (b1 >> (64 - 52))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 33) | (b7 >> (64 - 33))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 4) | (b5 >> (64 - 4))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 51) | (b3 >> (64 - 51))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 34) | (b15 >> (64 - 34))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 41) | (b13 >> (64 - 41))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 59) | (b11 >> (64 - 59))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 17) | (b9 >> (64 - 17))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 5) | (b15 >> (64 - 5))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 20) | (b11 >> (64 - 20))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 48) | (b13 >> (64 - 48))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 41) | (b9 >> (64 - 41))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 47) | (b1 >> (64 - 47))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 28) | (b5 >> (64 - 28))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 25) | (b7 >> (64 - 25))) ^ b12;
+
+	b1 += k3;
+	b0 += b1 + k2;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b0;
+
+	b3 += k5;
+	b2 += b3 + k4;
+	b3 = ((b3 << 9) | (b3 >> (64 - 9))) ^ b2;
+
+	b5 += k7;
+	b4 += b5 + k6;
+	b5 = ((b5 << 37) | (b5 >> (64 - 37))) ^ b4;
+
+	b7 += k9;
+	b6 += b7 + k8;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b6;
+
+	b9 += k11;
+	b8 += b9 + k10;
+	b9 = ((b9 << 12) | (b9 >> (64 - 12))) ^ b8;
+
+	b11 += k13;
+	b10 += b11 + k12;
+	b11 = ((b11 << 47) | (b11 >> (64 - 47))) ^ b10;
+
+	b13 += k15 + t1;
+	b12 += b13 + k14;
+	b13 = ((b13 << 44) | (b13 >> (64 - 44))) ^ b12;
+
+	b15 += k0 + 19;
+	b14 += b15 + k16 + t2;
+	b15 = ((b15 << 30) | (b15 >> (64 - 30))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 16) | (b9 >> (64 - 16))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 34) | (b13 >> (64 - 34))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 56) | (b11 >> (64 - 56))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 51) | (b15 >> (64 - 51))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 4) | (b7 >> (64 - 4))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 53) | (b3 >> (64 - 53))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 42) | (b5 >> (64 - 42))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 44) | (b5 >> (64 - 44))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 47) | (b3 >> (64 - 47))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 19) | (b15 >> (64 - 19))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 42) | (b13 >> (64 - 42))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 44) | (b11 >> (64 - 44))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 25) | (b9 >> (64 - 25))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 9) | (b15 >> (64 - 9))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 48) | (b11 >> (64 - 48))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 35) | (b13 >> (64 - 35))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 52) | (b9 >> (64 - 52))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 31) | (b5 >> (64 - 31))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 37) | (b3 >> (64 - 37))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 20) | (b7 >> (64 - 20))) ^ b12;
+
+	output[0] = b0 + k3;
+	output[1] = b1 + k4;
+	output[2] = b2 + k5;
+	output[3] = b3 + k6;
+	output[4] = b4 + k7;
+	output[5] = b5 + k8;
+	output[6] = b6 + k9;
+	output[7] = b7 + k10;
+	output[8] = b8 + k11;
+	output[9] = b9 + k12;
+	output[10] = b10 + k13;
+	output[11] = b11 + k14;
+	output[12] = b12 + k15;
+	output[13] = b13 + k16 + t2;
+	output[14] = b14 + k0 + t0;
+	output[15] = b15 + k1 + 20;
+}
+
+void threefish_decrypt_1024(struct threefish_key *key_ctx, u64 *input,
+			    u64 *output)
+{
+	u64 b0 = input[0], b1 = input[1],
+	    b2 = input[2], b3 = input[3],
+	    b4 = input[4], b5 = input[5],
+	    b6 = input[6], b7 = input[7],
+	    b8 = input[8], b9 = input[9],
+	    b10 = input[10], b11 = input[11],
+	    b12 = input[12], b13 = input[13],
+	    b14 = input[14], b15 = input[15];
+	u64 k0 = key_ctx->key[0], k1 = key_ctx->key[1],
+	    k2 = key_ctx->key[2], k3 = key_ctx->key[3],
+	    k4 = key_ctx->key[4], k5 = key_ctx->key[5],
+	    k6 = key_ctx->key[6], k7 = key_ctx->key[7],
+	    k8 = key_ctx->key[8], k9 = key_ctx->key[9],
+	    k10 = key_ctx->key[10], k11 = key_ctx->key[11],
+	    k12 = key_ctx->key[12], k13 = key_ctx->key[13],
+	    k14 = key_ctx->key[14], k15 = key_ctx->key[15],
+	    k16 = key_ctx->key[16];
+	u64 t0 = key_ctx->tweak[0], t1 = key_ctx->tweak[1],
+	    t2 = key_ctx->tweak[2];
+	u64 tmp;
+
+	b0 -= k3;
+	b1 -= k4;
+	b2 -= k5;
+	b3 -= k6;
+	b4 -= k7;
+	b5 -= k8;
+	b6 -= k9;
+	b7 -= k10;
+	b8 -= k11;
+	b9 -= k12;
+	b10 -= k13;
+	b11 -= k14;
+	b12 -= k15;
+	b13 -= k16 + t2;
+	b14 -= k0 + t0;
+	b15 -= k1 + 20;
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 20) | (tmp << (64 - 20));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 37) | (tmp << (64 - 37));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 31) | (tmp << (64 - 31));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 52) | (tmp << (64 - 52));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 35) | (tmp << (64 - 35));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 48) | (tmp << (64 - 48));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 25) | (tmp << (64 - 25));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 44) | (tmp << (64 - 44));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 19) | (tmp << (64 - 19));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 47) | (tmp << (64 - 47));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 44) | (tmp << (64 - 44));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 53) | (tmp << (64 - 53));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 4) | (tmp << (64 - 4));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 56) | (tmp << (64 - 56));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 34) | (tmp << (64 - 34));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 16) | (tmp << (64 - 16));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 30) | (tmp << (64 - 30));
+	b14 -= b15 + k16 + t2;
+	b15 -= k0 + 19;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 44) | (tmp << (64 - 44));
+	b12 -= b13 + k14;
+	b13 -= k15 + t1;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 47) | (tmp << (64 - 47));
+	b10 -= b11 + k12;
+	b11 -= k13;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 12) | (tmp << (64 - 12));
+	b8 -= b9 + k10;
+	b9 -= k11;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b6 -= b7 + k8;
+	b7 -= k9;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 37) | (tmp << (64 - 37));
+	b4 -= b5 + k6;
+	b5 -= k7;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 9) | (tmp << (64 - 9));
+	b2 -= b3 + k4;
+	b3 -= k5;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b0 -= b1 + k2;
+	b1 -= k3;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 25) | (tmp << (64 - 25));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 28) | (tmp << (64 - 28));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 47) | (tmp << (64 - 47));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 41) | (tmp << (64 - 41));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 48) | (tmp << (64 - 48));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 20) | (tmp << (64 - 20));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 59) | (tmp << (64 - 59));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 41) | (tmp << (64 - 41));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 34) | (tmp << (64 - 34));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 4) | (tmp << (64 - 4));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 33) | (tmp << (64 - 33));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 52) | (tmp << (64 - 52));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 18) | (tmp << (64 - 18));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 49) | (tmp << (64 - 49));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 55) | (tmp << (64 - 55));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 19) | (tmp << (64 - 19));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 38) | (tmp << (64 - 38));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 37) | (tmp << (64 - 37));
+	b14 -= b15 + k15 + t1;
+	b15 -= k16 + 18;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 22) | (tmp << (64 - 22));
+	b12 -= b13 + k13;
+	b13 -= k14 + t0;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b11 + k11;
+	b11 -= k12;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 8) | (tmp << (64 - 8));
+	b8 -= b9 + k9;
+	b9 -= k10;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 47) | (tmp << (64 - 47));
+	b6 -= b7 + k7;
+	b7 -= k8;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 8) | (tmp << (64 - 8));
+	b4 -= b5 + k5;
+	b5 -= k6;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b3 + k3;
+	b3 -= k4;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 24) | (tmp << (64 - 24));
+	b0 -= b1 + k1;
+	b1 -= k2;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 20) | (tmp << (64 - 20));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 37) | (tmp << (64 - 37));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 31) | (tmp << (64 - 31));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 52) | (tmp << (64 - 52));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 35) | (tmp << (64 - 35));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 48) | (tmp << (64 - 48));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 25) | (tmp << (64 - 25));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 44) | (tmp << (64 - 44));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 19) | (tmp << (64 - 19));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 47) | (tmp << (64 - 47));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 44) | (tmp << (64 - 44));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 53) | (tmp << (64 - 53));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 4) | (tmp << (64 - 4));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 56) | (tmp << (64 - 56));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 34) | (tmp << (64 - 34));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 16) | (tmp << (64 - 16));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 30) | (tmp << (64 - 30));
+	b14 -= b15 + k14 + t0;
+	b15 -= k15 + 17;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 44) | (tmp << (64 - 44));
+	b12 -= b13 + k12;
+	b13 -= k13 + t2;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 47) | (tmp << (64 - 47));
+	b10 -= b11 + k10;
+	b11 -= k11;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 12) | (tmp << (64 - 12));
+	b8 -= b9 + k8;
+	b9 -= k9;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b6 -= b7 + k6;
+	b7 -= k7;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 37) | (tmp << (64 - 37));
+	b4 -= b5 + k4;
+	b5 -= k5;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 9) | (tmp << (64 - 9));
+	b2 -= b3 + k2;
+	b3 -= k3;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b0 -= b1 + k0;
+	b1 -= k1;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 25) | (tmp << (64 - 25));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 28) | (tmp << (64 - 28));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 47) | (tmp << (64 - 47));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 41) | (tmp << (64 - 41));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 48) | (tmp << (64 - 48));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 20) | (tmp << (64 - 20));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 59) | (tmp << (64 - 59));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 41) | (tmp << (64 - 41));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 34) | (tmp << (64 - 34));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 4) | (tmp << (64 - 4));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 33) | (tmp << (64 - 33));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 52) | (tmp << (64 - 52));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 18) | (tmp << (64 - 18));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 49) | (tmp << (64 - 49));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 55) | (tmp << (64 - 55));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 19) | (tmp << (64 - 19));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 38) | (tmp << (64 - 38));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 37) | (tmp << (64 - 37));
+	b14 -= b15 + k13 + t2;
+	b15 -= k14 + 16;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 22) | (tmp << (64 - 22));
+	b12 -= b13 + k11;
+	b13 -= k12 + t1;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b11 + k9;
+	b11 -= k10;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 8) | (tmp << (64 - 8));
+	b8 -= b9 + k7;
+	b9 -= k8;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 47) | (tmp << (64 - 47));
+	b6 -= b7 + k5;
+	b7 -= k6;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 8) | (tmp << (64 - 8));
+	b4 -= b5 + k3;
+	b5 -= k4;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b3 + k1;
+	b3 -= k2;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 24) | (tmp << (64 - 24));
+	b0 -= b1 + k16;
+	b1 -= k0;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 20) | (tmp << (64 - 20));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 37) | (tmp << (64 - 37));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 31) | (tmp << (64 - 31));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 52) | (tmp << (64 - 52));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 35) | (tmp << (64 - 35));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 48) | (tmp << (64 - 48));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 25) | (tmp << (64 - 25));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 44) | (tmp << (64 - 44));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 19) | (tmp << (64 - 19));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 47) | (tmp << (64 - 47));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 44) | (tmp << (64 - 44));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 53) | (tmp << (64 - 53));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 4) | (tmp << (64 - 4));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 56) | (tmp << (64 - 56));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 34) | (tmp << (64 - 34));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 16) | (tmp << (64 - 16));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 30) | (tmp << (64 - 30));
+	b14 -= b15 + k12 + t1;
+	b15 -= k13 + 15;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 44) | (tmp << (64 - 44));
+	b12 -= b13 + k10;
+	b13 -= k11 + t0;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 47) | (tmp << (64 - 47));
+	b10 -= b11 + k8;
+	b11 -= k9;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 12) | (tmp << (64 - 12));
+	b8 -= b9 + k6;
+	b9 -= k7;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b6 -= b7 + k4;
+	b7 -= k5;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 37) | (tmp << (64 - 37));
+	b4 -= b5 + k2;
+	b5 -= k3;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 9) | (tmp << (64 - 9));
+	b2 -= b3 + k0;
+	b3 -= k1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b0 -= b1 + k15;
+	b1 -= k16;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 25) | (tmp << (64 - 25));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 28) | (tmp << (64 - 28));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 47) | (tmp << (64 - 47));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 41) | (tmp << (64 - 41));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 48) | (tmp << (64 - 48));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 20) | (tmp << (64 - 20));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 59) | (tmp << (64 - 59));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 41) | (tmp << (64 - 41));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 34) | (tmp << (64 - 34));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 4) | (tmp << (64 - 4));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 33) | (tmp << (64 - 33));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 52) | (tmp << (64 - 52));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 18) | (tmp << (64 - 18));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 49) | (tmp << (64 - 49));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 55) | (tmp << (64 - 55));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 19) | (tmp << (64 - 19));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 38) | (tmp << (64 - 38));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 37) | (tmp << (64 - 37));
+	b14 -= b15 + k11 + t0;
+	b15 -= k12 + 14;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 22) | (tmp << (64 - 22));
+	b12 -= b13 + k9;
+	b13 -= k10 + t2;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b11 + k7;
+	b11 -= k8;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 8) | (tmp << (64 - 8));
+	b8 -= b9 + k5;
+	b9 -= k6;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 47) | (tmp << (64 - 47));
+	b6 -= b7 + k3;
+	b7 -= k4;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 8) | (tmp << (64 - 8));
+	b4 -= b5 + k1;
+	b5 -= k2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b3 + k16;
+	b3 -= k0;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 24) | (tmp << (64 - 24));
+	b0 -= b1 + k14;
+	b1 -= k15;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 20) | (tmp << (64 - 20));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 37) | (tmp << (64 - 37));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 31) | (tmp << (64 - 31));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 52) | (tmp << (64 - 52));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 35) | (tmp << (64 - 35));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 48) | (tmp << (64 - 48));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 25) | (tmp << (64 - 25));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 44) | (tmp << (64 - 44));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 19) | (tmp << (64 - 19));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 47) | (tmp << (64 - 47));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 44) | (tmp << (64 - 44));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 53) | (tmp << (64 - 53));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 4) | (tmp << (64 - 4));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 56) | (tmp << (64 - 56));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 34) | (tmp << (64 - 34));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 16) | (tmp << (64 - 16));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 30) | (tmp << (64 - 30));
+	b14 -= b15 + k10 + t2;
+	b15 -= k11 + 13;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 44) | (tmp << (64 - 44));
+	b12 -= b13 + k8;
+	b13 -= k9 + t1;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 47) | (tmp << (64 - 47));
+	b10 -= b11 + k6;
+	b11 -= k7;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 12) | (tmp << (64 - 12));
+	b8 -= b9 + k4;
+	b9 -= k5;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b6 -= b7 + k2;
+	b7 -= k3;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 37) | (tmp << (64 - 37));
+	b4 -= b5 + k0;
+	b5 -= k1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 9) | (tmp << (64 - 9));
+	b2 -= b3 + k15;
+	b3 -= k16;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b0 -= b1 + k13;
+	b1 -= k14;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 25) | (tmp << (64 - 25));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 28) | (tmp << (64 - 28));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 47) | (tmp << (64 - 47));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 41) | (tmp << (64 - 41));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 48) | (tmp << (64 - 48));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 20) | (tmp << (64 - 20));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 59) | (tmp << (64 - 59));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 41) | (tmp << (64 - 41));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 34) | (tmp << (64 - 34));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 4) | (tmp << (64 - 4));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 33) | (tmp << (64 - 33));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 52) | (tmp << (64 - 52));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 18) | (tmp << (64 - 18));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 49) | (tmp << (64 - 49));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 55) | (tmp << (64 - 55));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 19) | (tmp << (64 - 19));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 38) | (tmp << (64 - 38));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 37) | (tmp << (64 - 37));
+	b14 -= b15 + k9 + t1;
+	b15 -= k10 + 12;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 22) | (tmp << (64 - 22));
+	b12 -= b13 + k7;
+	b13 -= k8 + t0;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b11 + k5;
+	b11 -= k6;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 8) | (tmp << (64 - 8));
+	b8 -= b9 + k3;
+	b9 -= k4;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 47) | (tmp << (64 - 47));
+	b6 -= b7 + k1;
+	b7 -= k2;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 8) | (tmp << (64 - 8));
+	b4 -= b5 + k16;
+	b5 -= k0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b3 + k14;
+	b3 -= k15;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 24) | (tmp << (64 - 24));
+	b0 -= b1 + k12;
+	b1 -= k13;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 20) | (tmp << (64 - 20));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 37) | (tmp << (64 - 37));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 31) | (tmp << (64 - 31));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 52) | (tmp << (64 - 52));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 35) | (tmp << (64 - 35));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 48) | (tmp << (64 - 48));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 25) | (tmp << (64 - 25));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 44) | (tmp << (64 - 44));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 19) | (tmp << (64 - 19));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 47) | (tmp << (64 - 47));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 44) | (tmp << (64 - 44));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 53) | (tmp << (64 - 53));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 4) | (tmp << (64 - 4));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 56) | (tmp << (64 - 56));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 34) | (tmp << (64 - 34));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 16) | (tmp << (64 - 16));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 30) | (tmp << (64 - 30));
+	b14 -= b15 + k8 + t0;
+	b15 -= k9 + 11;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 44) | (tmp << (64 - 44));
+	b12 -= b13 + k6;
+	b13 -= k7 + t2;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 47) | (tmp << (64 - 47));
+	b10 -= b11 + k4;
+	b11 -= k5;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 12) | (tmp << (64 - 12));
+	b8 -= b9 + k2;
+	b9 -= k3;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b6 -= b7 + k0;
+	b7 -= k1;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 37) | (tmp << (64 - 37));
+	b4 -= b5 + k15;
+	b5 -= k16;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 9) | (tmp << (64 - 9));
+	b2 -= b3 + k13;
+	b3 -= k14;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b0 -= b1 + k11;
+	b1 -= k12;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 25) | (tmp << (64 - 25));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 28) | (tmp << (64 - 28));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 47) | (tmp << (64 - 47));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 41) | (tmp << (64 - 41));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 48) | (tmp << (64 - 48));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 20) | (tmp << (64 - 20));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 59) | (tmp << (64 - 59));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 41) | (tmp << (64 - 41));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 34) | (tmp << (64 - 34));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 4) | (tmp << (64 - 4));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 33) | (tmp << (64 - 33));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 52) | (tmp << (64 - 52));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 18) | (tmp << (64 - 18));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 49) | (tmp << (64 - 49));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 55) | (tmp << (64 - 55));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 19) | (tmp << (64 - 19));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 38) | (tmp << (64 - 38));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 37) | (tmp << (64 - 37));
+	b14 -= b15 + k7 + t2;
+	b15 -= k8 + 10;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 22) | (tmp << (64 - 22));
+	b12 -= b13 + k5;
+	b13 -= k6 + t1;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b11 + k3;
+	b11 -= k4;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 8) | (tmp << (64 - 8));
+	b8 -= b9 + k1;
+	b9 -= k2;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 47) | (tmp << (64 - 47));
+	b6 -= b7 + k16;
+	b7 -= k0;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 8) | (tmp << (64 - 8));
+	b4 -= b5 + k14;
+	b5 -= k15;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b3 + k12;
+	b3 -= k13;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 24) | (tmp << (64 - 24));
+	b0 -= b1 + k10;
+	b1 -= k11;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 20) | (tmp << (64 - 20));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 37) | (tmp << (64 - 37));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 31) | (tmp << (64 - 31));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 52) | (tmp << (64 - 52));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 35) | (tmp << (64 - 35));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 48) | (tmp << (64 - 48));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 25) | (tmp << (64 - 25));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 44) | (tmp << (64 - 44));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 19) | (tmp << (64 - 19));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 47) | (tmp << (64 - 47));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 44) | (tmp << (64 - 44));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 53) | (tmp << (64 - 53));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 4) | (tmp << (64 - 4));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 56) | (tmp << (64 - 56));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 34) | (tmp << (64 - 34));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 16) | (tmp << (64 - 16));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 30) | (tmp << (64 - 30));
+	b14 -= b15 + k6 + t1;
+	b15 -= k7 + 9;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 44) | (tmp << (64 - 44));
+	b12 -= b13 + k4;
+	b13 -= k5 + t0;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 47) | (tmp << (64 - 47));
+	b10 -= b11 + k2;
+	b11 -= k3;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 12) | (tmp << (64 - 12));
+	b8 -= b9 + k0;
+	b9 -= k1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b6 -= b7 + k15;
+	b7 -= k16;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 37) | (tmp << (64 - 37));
+	b4 -= b5 + k13;
+	b5 -= k14;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 9) | (tmp << (64 - 9));
+	b2 -= b3 + k11;
+	b3 -= k12;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b0 -= b1 + k9;
+	b1 -= k10;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 25) | (tmp << (64 - 25));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 28) | (tmp << (64 - 28));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 47) | (tmp << (64 - 47));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 41) | (tmp << (64 - 41));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 48) | (tmp << (64 - 48));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 20) | (tmp << (64 - 20));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 59) | (tmp << (64 - 59));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 41) | (tmp << (64 - 41));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 34) | (tmp << (64 - 34));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 4) | (tmp << (64 - 4));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 33) | (tmp << (64 - 33));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 52) | (tmp << (64 - 52));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 18) | (tmp << (64 - 18));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 49) | (tmp << (64 - 49));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 55) | (tmp << (64 - 55));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 19) | (tmp << (64 - 19));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 38) | (tmp << (64 - 38));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 37) | (tmp << (64 - 37));
+	b14 -= b15 + k5 + t0;
+	b15 -= k6 + 8;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 22) | (tmp << (64 - 22));
+	b12 -= b13 + k3;
+	b13 -= k4 + t2;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b11 + k1;
+	b11 -= k2;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 8) | (tmp << (64 - 8));
+	b8 -= b9 + k16;
+	b9 -= k0;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 47) | (tmp << (64 - 47));
+	b6 -= b7 + k14;
+	b7 -= k15;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 8) | (tmp << (64 - 8));
+	b4 -= b5 + k12;
+	b5 -= k13;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b3 + k10;
+	b3 -= k11;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 24) | (tmp << (64 - 24));
+	b0 -= b1 + k8;
+	b1 -= k9;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 20) | (tmp << (64 - 20));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 37) | (tmp << (64 - 37));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 31) | (tmp << (64 - 31));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 52) | (tmp << (64 - 52));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 35) | (tmp << (64 - 35));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 48) | (tmp << (64 - 48));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 25) | (tmp << (64 - 25));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 44) | (tmp << (64 - 44));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 19) | (tmp << (64 - 19));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 47) | (tmp << (64 - 47));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 44) | (tmp << (64 - 44));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 53) | (tmp << (64 - 53));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 4) | (tmp << (64 - 4));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 56) | (tmp << (64 - 56));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 34) | (tmp << (64 - 34));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 16) | (tmp << (64 - 16));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 30) | (tmp << (64 - 30));
+	b14 -= b15 + k4 + t2;
+	b15 -= k5 + 7;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 44) | (tmp << (64 - 44));
+	b12 -= b13 + k2;
+	b13 -= k3 + t1;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 47) | (tmp << (64 - 47));
+	b10 -= b11 + k0;
+	b11 -= k1;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 12) | (tmp << (64 - 12));
+	b8 -= b9 + k15;
+	b9 -= k16;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b6 -= b7 + k13;
+	b7 -= k14;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 37) | (tmp << (64 - 37));
+	b4 -= b5 + k11;
+	b5 -= k12;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 9) | (tmp << (64 - 9));
+	b2 -= b3 + k9;
+	b3 -= k10;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b0 -= b1 + k7;
+	b1 -= k8;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 25) | (tmp << (64 - 25));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 28) | (tmp << (64 - 28));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 47) | (tmp << (64 - 47));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 41) | (tmp << (64 - 41));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 48) | (tmp << (64 - 48));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 20) | (tmp << (64 - 20));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 59) | (tmp << (64 - 59));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 41) | (tmp << (64 - 41));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 34) | (tmp << (64 - 34));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 4) | (tmp << (64 - 4));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 33) | (tmp << (64 - 33));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 52) | (tmp << (64 - 52));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 18) | (tmp << (64 - 18));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 49) | (tmp << (64 - 49));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 55) | (tmp << (64 - 55));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 19) | (tmp << (64 - 19));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 38) | (tmp << (64 - 38));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 37) | (tmp << (64 - 37));
+	b14 -= b15 + k3 + t1;
+	b15 -= k4 + 6;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 22) | (tmp << (64 - 22));
+	b12 -= b13 + k1;
+	b13 -= k2 + t0;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b11 + k16;
+	b11 -= k0;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 8) | (tmp << (64 - 8));
+	b8 -= b9 + k14;
+	b9 -= k15;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 47) | (tmp << (64 - 47));
+	b6 -= b7 + k12;
+	b7 -= k13;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 8) | (tmp << (64 - 8));
+	b4 -= b5 + k10;
+	b5 -= k11;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b3 + k8;
+	b3 -= k9;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 24) | (tmp << (64 - 24));
+	b0 -= b1 + k6;
+	b1 -= k7;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 20) | (tmp << (64 - 20));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 37) | (tmp << (64 - 37));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 31) | (tmp << (64 - 31));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 52) | (tmp << (64 - 52));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 35) | (tmp << (64 - 35));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 48) | (tmp << (64 - 48));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 25) | (tmp << (64 - 25));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 44) | (tmp << (64 - 44));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 19) | (tmp << (64 - 19));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 47) | (tmp << (64 - 47));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 44) | (tmp << (64 - 44));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 53) | (tmp << (64 - 53));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 4) | (tmp << (64 - 4));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 56) | (tmp << (64 - 56));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 34) | (tmp << (64 - 34));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 16) | (tmp << (64 - 16));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 30) | (tmp << (64 - 30));
+	b14 -= b15 + k2 + t0;
+	b15 -= k3 + 5;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 44) | (tmp << (64 - 44));
+	b12 -= b13 + k0;
+	b13 -= k1 + t2;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 47) | (tmp << (64 - 47));
+	b10 -= b11 + k15;
+	b11 -= k16;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 12) | (tmp << (64 - 12));
+	b8 -= b9 + k13;
+	b9 -= k14;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b6 -= b7 + k11;
+	b7 -= k12;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 37) | (tmp << (64 - 37));
+	b4 -= b5 + k9;
+	b5 -= k10;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 9) | (tmp << (64 - 9));
+	b2 -= b3 + k7;
+	b3 -= k8;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b0 -= b1 + k5;
+	b1 -= k6;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 25) | (tmp << (64 - 25));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 28) | (tmp << (64 - 28));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 47) | (tmp << (64 - 47));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 41) | (tmp << (64 - 41));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 48) | (tmp << (64 - 48));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 20) | (tmp << (64 - 20));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 59) | (tmp << (64 - 59));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 41) | (tmp << (64 - 41));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 34) | (tmp << (64 - 34));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 4) | (tmp << (64 - 4));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 33) | (tmp << (64 - 33));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 52) | (tmp << (64 - 52));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 18) | (tmp << (64 - 18));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 49) | (tmp << (64 - 49));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 55) | (tmp << (64 - 55));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 19) | (tmp << (64 - 19));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 38) | (tmp << (64 - 38));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 37) | (tmp << (64 - 37));
+	b14 -= b15 + k1 + t2;
+	b15 -= k2 + 4;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 22) | (tmp << (64 - 22));
+	b12 -= b13 + k16;
+	b13 -= k0 + t1;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b11 + k14;
+	b11 -= k15;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 8) | (tmp << (64 - 8));
+	b8 -= b9 + k12;
+	b9 -= k13;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 47) | (tmp << (64 - 47));
+	b6 -= b7 + k10;
+	b7 -= k11;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 8) | (tmp << (64 - 8));
+	b4 -= b5 + k8;
+	b5 -= k9;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b3 + k6;
+	b3 -= k7;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 24) | (tmp << (64 - 24));
+	b0 -= b1 + k4;
+	b1 -= k5;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 20) | (tmp << (64 - 20));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 37) | (tmp << (64 - 37));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 31) | (tmp << (64 - 31));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 52) | (tmp << (64 - 52));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 35) | (tmp << (64 - 35));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 48) | (tmp << (64 - 48));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 25) | (tmp << (64 - 25));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 44) | (tmp << (64 - 44));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 19) | (tmp << (64 - 19));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 47) | (tmp << (64 - 47));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 44) | (tmp << (64 - 44));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 53) | (tmp << (64 - 53));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 4) | (tmp << (64 - 4));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 56) | (tmp << (64 - 56));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 34) | (tmp << (64 - 34));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 16) | (tmp << (64 - 16));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 30) | (tmp << (64 - 30));
+	b14 -= b15 + k0 + t1;
+	b15 -= k1 + 3;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 44) | (tmp << (64 - 44));
+	b12 -= b13 + k15;
+	b13 -= k16 + t0;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 47) | (tmp << (64 - 47));
+	b10 -= b11 + k13;
+	b11 -= k14;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 12) | (tmp << (64 - 12));
+	b8 -= b9 + k11;
+	b9 -= k12;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b6 -= b7 + k9;
+	b7 -= k10;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 37) | (tmp << (64 - 37));
+	b4 -= b5 + k7;
+	b5 -= k8;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 9) | (tmp << (64 - 9));
+	b2 -= b3 + k5;
+	b3 -= k6;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b0 -= b1 + k3;
+	b1 -= k4;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 25) | (tmp << (64 - 25));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 28) | (tmp << (64 - 28));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 47) | (tmp << (64 - 47));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 41) | (tmp << (64 - 41));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 48) | (tmp << (64 - 48));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 20) | (tmp << (64 - 20));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 59) | (tmp << (64 - 59));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 41) | (tmp << (64 - 41));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 34) | (tmp << (64 - 34));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 4) | (tmp << (64 - 4));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 33) | (tmp << (64 - 33));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 52) | (tmp << (64 - 52));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 18) | (tmp << (64 - 18));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 49) | (tmp << (64 - 49));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 55) | (tmp << (64 - 55));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 19) | (tmp << (64 - 19));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 38) | (tmp << (64 - 38));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 37) | (tmp << (64 - 37));
+	b14 -= b15 + k16 + t0;
+	b15 -= k0 + 2;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 22) | (tmp << (64 - 22));
+	b12 -= b13 + k14;
+	b13 -= k15 + t2;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b11 + k12;
+	b11 -= k13;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 8) | (tmp << (64 - 8));
+	b8 -= b9 + k10;
+	b9 -= k11;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 47) | (tmp << (64 - 47));
+	b6 -= b7 + k8;
+	b7 -= k9;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 8) | (tmp << (64 - 8));
+	b4 -= b5 + k6;
+	b5 -= k7;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b3 + k4;
+	b3 -= k5;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 24) | (tmp << (64 - 24));
+	b0 -= b1 + k2;
+	b1 -= k3;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 20) | (tmp << (64 - 20));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 37) | (tmp << (64 - 37));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 31) | (tmp << (64 - 31));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 52) | (tmp << (64 - 52));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 35) | (tmp << (64 - 35));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 48) | (tmp << (64 - 48));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 25) | (tmp << (64 - 25));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 44) | (tmp << (64 - 44));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 19) | (tmp << (64 - 19));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 47) | (tmp << (64 - 47));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 44) | (tmp << (64 - 44));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 53) | (tmp << (64 - 53));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 4) | (tmp << (64 - 4));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 56) | (tmp << (64 - 56));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 34) | (tmp << (64 - 34));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 16) | (tmp << (64 - 16));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 30) | (tmp << (64 - 30));
+	b14 -= b15 + k15 + t2;
+	b15 -= k16 + 1;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 44) | (tmp << (64 - 44));
+	b12 -= b13 + k13;
+	b13 -= k14 + t1;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 47) | (tmp << (64 - 47));
+	b10 -= b11 + k11;
+	b11 -= k12;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 12) | (tmp << (64 - 12));
+	b8 -= b9 + k9;
+	b9 -= k10;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b6 -= b7 + k7;
+	b7 -= k8;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 37) | (tmp << (64 - 37));
+	b4 -= b5 + k5;
+	b5 -= k6;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 9) | (tmp << (64 - 9));
+	b2 -= b3 + k3;
+	b3 -= k4;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b0 -= b1 + k1;
+	b1 -= k2;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 25) | (tmp << (64 - 25));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 28) | (tmp << (64 - 28));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 47) | (tmp << (64 - 47));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 41) | (tmp << (64 - 41));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 48) | (tmp << (64 - 48));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 20) | (tmp << (64 - 20));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 59) | (tmp << (64 - 59));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 41) | (tmp << (64 - 41));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 34) | (tmp << (64 - 34));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 4) | (tmp << (64 - 4));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 33) | (tmp << (64 - 33));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 52) | (tmp << (64 - 52));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 18) | (tmp << (64 - 18));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 49) | (tmp << (64 - 49));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 55) | (tmp << (64 - 55));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 19) | (tmp << (64 - 19));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 38) | (tmp << (64 - 38));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 37) | (tmp << (64 - 37));
+	b14 -= b15 + k14 + t1;
+	b15 -= k15;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 22) | (tmp << (64 - 22));
+	b12 -= b13 + k12;
+	b13 -= k13 + t0;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b11 + k10;
+	b11 -= k11;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 8) | (tmp << (64 - 8));
+	b8 -= b9 + k8;
+	b9 -= k9;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 47) | (tmp << (64 - 47));
+	b6 -= b7 + k6;
+	b7 -= k7;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 8) | (tmp << (64 - 8));
+	b4 -= b5 + k4;
+	b5 -= k5;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b3 + k2;
+	b3 -= k3;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 24) | (tmp << (64 - 24));
+	b0 -= b1 + k0;
+	b1 -= k1;
+
+	output[15] = b15;
+	output[14] = b14;
+	output[13] = b13;
+	output[12] = b12;
+	output[11] = b11;
+	output[10] = b10;
+	output[9] = b9;
+	output[8] = b8;
+	output[7] = b7;
+	output[6] = b6;
+	output[5] = b5;
+	output[4] = b4;
+	output[3] = b3;
+	output[2] = b2;
+	output[1] = b1;
+	output[0] = b0;
+}
diff --git a/drivers/staging/slicoss/TODO b/drivers/staging/slicoss/TODO
index 62ff1008b1e..20cc9abdc46 100644
--- a/drivers/staging/slicoss/TODO
+++ b/drivers/staging/slicoss/TODO
@@ -18,7 +18,6 @@ TODO:
 	  use ethtool instead
 	- reorder code to elminate use of forward declarations
 	- don't keep private linked list of drivers.
-	- remove all the gratiutous debug infrastructure
 	- use PCI_DEVICE()
 	- do ethtool correctly using ethtool_ops
 	- NAPI?
diff --git a/drivers/staging/slicoss/slic.h b/drivers/staging/slicoss/slic.h
index 702902cdb46..3a5aa882b95 100644
--- a/drivers/staging/slicoss/slic.h
+++ b/drivers/staging/slicoss/slic.h
@@ -310,8 +310,6 @@ struct sliccard {
 	u32           loadtimerset;
 	uint              config_set;
 	struct slic_config  config;
-	struct dentry      *debugfs_dir;
-	struct dentry      *debugfs_cardinfo;
 	struct adapter  *master;
 	struct adapter  *adapter[SLIC_MAX_PORTS];
 	struct sliccard *next;
@@ -364,12 +362,6 @@ struct slic_shmem {
 	volatile struct slic_stats     inicstats;
 };
 
-struct slic_reg_params {
-	u32       linkspeed;
-	u32       linkduplex;
-	u32       fail_on_bad_eeprom;
-};
-
 struct slic_upr {
 	uint               adapter;
 	u32            upr_request;
@@ -409,7 +401,6 @@ struct adapter {
 	uint                card_size;
 	uint                chipid;
 	struct net_device  *netdev;
-	struct net_device  *next_netdevice;
 	struct slic_spinlock     adapter_lock;
 	struct slic_spinlock     reset_lock;
 	struct pci_dev     *pcidev;
@@ -420,8 +411,6 @@ struct adapter {
 	ushort              devid;
 	ushort              subsysid;
 	u32             irq;
-	void __iomem *memorybase;
-	u32             memorylength;
 	u32             drambase;
 	u32             dramlength;
 	uint                queues_initialized;
@@ -452,7 +441,6 @@ struct adapter {
 	u32             pingtimerset;
 	struct timer_list   loadtimer;
 	u32             loadtimerset;
-	struct dentry      *debugfs_entry;
 	struct slic_spinlock     upr_lock;
 	struct slic_spinlock     bit64reglock;
 	struct slic_rspqueue     rspqueue;
@@ -498,7 +486,6 @@ struct adapter {
 	u32             intagg_period;
 	struct inicpm_state    *inicpm_info;
 	void *pinicpm_info;
-	struct slic_reg_params   reg_params;
 	struct slic_ifevents  if_events;
 	struct slic_stats        inicstats_prev;
 	struct slicnet_stats     slic_stats;
diff --git a/drivers/staging/slicoss/slicoss.c b/drivers/staging/slicoss/slicoss.c
index e27b88f02cc..48841e7c2f7 100644
--- a/drivers/staging/slicoss/slicoss.c
+++ b/drivers/staging/slicoss/slicoss.c
@@ -81,7 +81,6 @@
 #include <linux/etherdevice.h>
 #include <linux/skbuff.h>
 #include <linux/delay.h>
-#include <linux/debugfs.h>
 #include <linux/seq_file.h>
 #include <linux/kthread.h>
 #include <linux/module.h>
@@ -104,19 +103,11 @@ static char *slic_banner = "Alacritech SLIC Technology(tm) Server "
 		"and Storage Accelerator (Non-Accelerated)";
 
 static char *slic_proc_version = "2.0.351  2006/07/14 12:26:00";
-static char *slic_product_name = "SLIC Technology(tm) Server "
-		"and Storage Accelerator (Non-Accelerated)";
-static char *slic_vendor = "Alacritech, Inc.";
-
-static int slic_debug = 1;
-static int debug = -1;
-static struct net_device *head_netdevice;
 
 static struct base_driver slic_global = { {}, 0, 0, 0, 1, NULL, NULL };
 static int intagg_delay = 100;
 static u32 dynamic_intagg;
 static unsigned int rcv_count;
-static struct dentry *slic_debugfs;
 
 #define DRV_NAME          "slicoss"
 #define DRV_VERSION       "2.0.1"
@@ -404,7 +395,7 @@ static int slic_card_download_gbrcv(struct adapter *adapter)
 	ret = request_firmware(&fw, file, &adapter->pcidev->dev);
 	if (ret) {
 		dev_err(&adapter->pcidev->dev,
-			"SLICOSS: Failed to load firmware %s\n", file);
+			"Failed to load firmware %s\n", file);
 		return ret;
 	}
 
@@ -482,7 +473,7 @@ static int slic_card_download(struct adapter *adapter)
 	ret = request_firmware(&fw, file, &adapter->pcidev->dev);
 	if (ret) {
 		dev_err(&adapter->pcidev->dev,
-			"SLICOSS: Failed to load firmware %s\n", file);
+			"Failed to load firmware %s\n", file);
 		return ret;
 	}
 	numsects = *(u32 *)(fw->data + index);
@@ -1140,116 +1131,33 @@ static void slic_upr_request_complete(struct adapter *adapter, u32 isr)
 				adapter->upr_lock.flags);
 }
 
-static void slic_config_get(struct adapter *adapter, u32 config,
-							u32 config_h)
+static int slic_config_get(struct adapter *adapter, u32 config, u32 config_h)
 {
-	int status;
-
-	status = slic_upr_request(adapter,
-				  SLIC_UPR_RCONFIG,
-				  (u32) config, (u32) config_h, 0, 0);
+	return slic_upr_request(adapter, SLIC_UPR_RCONFIG, config, config_h,
+				0, 0);
 }
 
 /*
- *  this is here to checksum the eeprom, there is some ucode bug
- *  which prevens us from using the ucode result.
- *  remove this once ucode is fixed.
+ * Compute a checksum of the EEPROM according to RFC 1071.
  */
-static ushort slic_eeprom_cksum(char *m, int len)
+static u16 slic_eeprom_cksum(void *eeprom, unsigned len)
 {
-#define ADDCARRY(x)  (x > 65535 ? x -= 65535 : x)
-#define REDUCE {l_util.l = sum; sum = l_util.s[0] + l_util.s[1]; ADDCARRY(sum);\
-		}
-
-	u16 *w;
-	u32 sum = 0;
-	u32 byte_swapped = 0;
-	u32 w_int;
+	u16 *wp = eeprom;
+	u32 checksum = 0;
 
-	union {
-		char c[2];
-		ushort s;
-	} s_util;
-
-	union {
-		ushort s[2];
-		int l;
-	} l_util;
+	while (len > 1) {
+		checksum += *(wp++);
+		len -= 2;
+	}
 
-	l_util.l = 0;
-	s_util.s = 0;
+	if (len > 0)
+		checksum += *(u8 *) wp;
 
-	w = (u16 *)m;
-#if BITS_PER_LONG == 64
-	w_int = (u32) ((ulong) w & 0x00000000FFFFFFFF);
-#else
-	w_int = (u32) (w);
-#endif
-	if ((1 & w_int) && (len > 0)) {
-		REDUCE;
-		sum <<= 8;
-		s_util.c[0] = *(unsigned char *)w;
-		w = (u16 *)((char *)w + 1);
-		len--;
-		byte_swapped = 1;
-	}
-
-	/* Unroll the loop to make overhead from branches &c small. */
-	while ((len -= 32) >= 0) {
-		sum += w[0];
-		sum += w[1];
-		sum += w[2];
-		sum += w[3];
-		sum += w[4];
-		sum += w[5];
-		sum += w[6];
-		sum += w[7];
-		sum += w[8];
-		sum += w[9];
-		sum += w[10];
-		sum += w[11];
-		sum += w[12];
-		sum += w[13];
-		sum += w[14];
-		sum += w[15];
-		w = (u16 *)((ulong) w + 16);	/* verify */
-	}
-	len += 32;
-	while ((len -= 8) >= 0) {
-		sum += w[0];
-		sum += w[1];
-		sum += w[2];
-		sum += w[3];
-		w = (u16 *)((ulong) w + 4);	/* verify */
-	}
-	len += 8;
-	if (len != 0 || byte_swapped != 0) {
-		REDUCE;
-		while ((len -= 2) >= 0)
-			sum += *w++;	/* verify */
-		if (byte_swapped) {
-			REDUCE;
-			sum <<= 8;
-			byte_swapped = 0;
-			if (len == -1) {
-				s_util.c[1] = *(char *) w;
-				sum += s_util.s;
-				len = 0;
-			} else {
-				len = -1;
-			}
 
-		} else if (len == -1) {
-			s_util.c[0] = *(char *) w;
-		}
+	while (checksum >> 16)
+		checksum = (checksum & 0xFFFF) + ((checksum >> 16) & 0xFFFF);
 
-		if (len == -1) {
-			s_util.c[1] = 0;
-			sum += s_util.s;
-		}
-	}
-	REDUCE;
-	return (ushort) sum;
+	return ~checksum;
 }
 
 static void slic_rspqueue_free(struct adapter *adapter)
@@ -1422,8 +1330,7 @@ static void slic_cmdq_addcmdpage(struct adapter *adapter, u32 *page)
 		spin_lock_irqsave(&adapter->handle_lock.lock,
 				adapter->handle_lock.flags);
 		pslic_handle  =  adapter->pfree_slic_handles;
-		if (pslic_handle)
-			adapter->pfree_slic_handles = pslic_handle->next;
+		adapter->pfree_slic_handles = pslic_handle->next;
 		spin_unlock_irqrestore(&adapter->handle_lock.lock,
 				adapter->handle_lock.flags);
 		pslic_handle->type = SLIC_HANDLE_CMD;
@@ -1802,430 +1709,6 @@ static u32 slic_rcvqueue_reinsert(struct adapter *adapter, struct sk_buff *skb)
 	return rcvq->count;
 }
 
-static int slic_debug_card_show(struct seq_file *seq, void *v)
-{
-#ifdef MOOKTODO
-	int i;
-	struct sliccard *card = seq->private;
-	struct slic_config *config = &card->config;
-	unsigned char *fru = (unsigned char *)(&card->config.atk_fru);
-	unsigned char *oemfru = (unsigned char *)(&card->config.OemFru);
-#endif
-
-	seq_printf(seq, "driver_version           : %s\n", slic_proc_version);
-	seq_puts(seq, "Microcode versions:\n");
-	seq_printf(seq, "    Gigabit (gb)         : %s %s\n",
-		    MOJAVE_UCODE_VERS_STRING, MOJAVE_UCODE_VERS_DATE);
-	seq_printf(seq, "    Gigabit Receiver     : %s %s\n",
-		    GB_RCVUCODE_VERS_STRING, GB_RCVUCODE_VERS_DATE);
-	seq_printf(seq, "Vendor                   : %s\n", slic_vendor);
-	seq_printf(seq, "Product Name             : %s\n", slic_product_name);
-#ifdef MOOKTODO
-	seq_printf(seq, "VendorId                 : %4.4X\n",
-		    config->VendorId);
-	seq_printf(seq, "DeviceId                 : %4.4X\n",
-		    config->DeviceId);
-	seq_printf(seq, "RevisionId               : %2.2x\n",
-		    config->RevisionId);
-	seq_printf(seq, "Bus    #                 : %d\n", card->busnumber);
-	seq_printf(seq, "Device #                 : %d\n", card->slotnumber);
-	seq_printf(seq, "Interfaces               : %d\n", card->card_size);
-	seq_printf(seq, "     Initialized         : %d\n",
-		    card->adapters_activated);
-	seq_printf(seq, "     Allocated           : %d\n",
-		    card->adapters_allocated);
-	for (i = 0; i < card->card_size; i++) {
-		seq_printf(seq,
-			   "     MAC%d : %2.2X %2.2X %2.2X %2.2X %2.2X %2.2X\n",
-			   i, config->macinfo[i].macaddrA[0],
-			   config->macinfo[i].macaddrA[1],
-			   config->macinfo[i].macaddrA[2],
-			   config->macinfo[i].macaddrA[3],
-			   config->macinfo[i].macaddrA[4],
-			   config->macinfo[i].macaddrA[5]);
-	}
-	seq_puts(seq, "     IF  Init State Duplex/Speed irq\n");
-	seq_puts(seq, "     -------------------------------\n");
-	for (i = 0; i < card->adapters_allocated; i++) {
-		struct adapter *adapter;
-
-		adapter = card->adapter[i];
-		if (adapter) {
-			seq_printf(seq,
-				    "     %d   %d   %s  %s  %s    0x%X\n",
-				    adapter->physport, adapter->state,
-				    SLIC_LINKSTATE(adapter->linkstate),
-				    SLIC_DUPLEX(adapter->linkduplex),
-				    SLIC_SPEED(adapter->linkspeed),
-				    (uint) adapter->irq);
-		}
-	}
-	seq_printf(seq, "Generation #             : %4.4X\n", card->gennumber);
-	seq_printf(seq, "RcvQ max entries         : %4.4X\n",
-		    SLIC_RCVQ_ENTRIES);
-	seq_printf(seq, "Ping Status              : %8.8X\n",
-		    card->pingstatus);
-	seq_printf(seq, "Minimum grant            : %2.2x\n",
-		    config->MinGrant);
-	seq_printf(seq, "Maximum Latency          : %2.2x\n", config->MaxLat);
-	seq_printf(seq, "PciStatus                : %4.4x\n",
-		    config->Pcistatus);
-	seq_printf(seq, "Debug Device Id          : %4.4x\n",
-		    config->DbgDevId);
-	seq_printf(seq, "DRAM ROM Function        : %4.4x\n",
-		    config->DramRomFn);
-	seq_printf(seq, "Network interface Pin 1  : %2.2x\n",
-		    config->NetIntPin1);
-	seq_printf(seq, "Network interface Pin 2  : %2.2x\n",
-		    config->NetIntPin1);
-	seq_printf(seq, "Network interface Pin 3  : %2.2x\n",
-		    config->NetIntPin1);
-	seq_printf(seq, "PM capabilities          : %4.4X\n",
-		    config->PMECapab);
-	seq_printf(seq, "Network Clock Controls   : %4.4X\n",
-		    config->NwClkCtrls);
-
-	switch (config->FruFormat) {
-	case ATK_FRU_FORMAT:
-		{
-			seq_puts(seq,
-			    "Vendor                   : Alacritech, Inc.\n");
-			seq_printf(seq,
-			    "Assembly #               : %c%c%c%c%c%c\n",
-				    fru[0], fru[1], fru[2], fru[3], fru[4],
-				    fru[5]);
-			seq_printf(seq,
-				    "Revision #               : %c%c\n",
-				    fru[6], fru[7]);
-
-			if (config->OEMFruFormat == VENDOR4_FRU_FORMAT) {
-				seq_printf(seq,
-					    "Serial   #               : %c%c%c%c%c%c%c%c%c%c%c%c\n",
-					    fru[8], fru[9], fru[10],
-					    fru[11], fru[12], fru[13],
-					    fru[16], fru[17], fru[18],
-					    fru[19], fru[20], fru[21]);
-			} else {
-				seq_printf(seq,
-					    "Serial   #               : %c%c%c%c%c%c%c%c%c%c%c%c%c%c\n",
-					    fru[8], fru[9], fru[10],
-					    fru[11], fru[12], fru[13],
-					    fru[14], fru[15], fru[16],
-					    fru[17], fru[18], fru[19],
-					    fru[20], fru[21]);
-			}
-			break;
-		}
-
-	default:
-		{
-			seq_puts(seq,
-			    "Vendor                   : Alacritech, Inc.\n");
-			seq_puts(seq,
-			    "Serial   #               : Empty FRU\n");
-			break;
-		}
-	}
-
-	switch (config->OEMFruFormat) {
-	case VENDOR1_FRU_FORMAT:
-		{
-			seq_puts(seq, "FRU Information:\n");
-			seq_printf(seq, "    Commodity #          : %c\n",
-				    oemfru[0]);
-			seq_printf(seq,
-				    "    Assembly #           : %c%c%c%c\n",
-				    oemfru[1], oemfru[2], oemfru[3], oemfru[4]);
-			seq_printf(seq,
-				    "    Revision #           : %c%c\n",
-				    oemfru[5], oemfru[6]);
-			seq_printf(seq,
-				    "    Supplier #           : %c%c\n",
-				    oemfru[7], oemfru[8]);
-			seq_printf(seq,
-				    "    Date                 : %c%c\n",
-				    oemfru[9], oemfru[10]);
-			seq_sprintf(seq,
-				    "    Sequence #           : %c%c%c\n",
-				    oemfru[11], oemfru[12], oemfru[13]);
-			break;
-		}
-
-	case VENDOR2_FRU_FORMAT:
-		{
-			seq_puts(seq, "FRU Information:\n");
-			seq_printf(seq,
-				    "    Part     #           : %c%c%c%c%c%c%c%c\n",
-				    oemfru[0], oemfru[1], oemfru[2],
-				    oemfru[3], oemfru[4], oemfru[5],
-				    oemfru[6], oemfru[7]);
-			seq_printf(seq,
-				    "    Supplier #           : %c%c%c%c%c\n",
-				    oemfru[8], oemfru[9], oemfru[10],
-				    oemfru[11], oemfru[12]);
-			seq_printf(seq,
-				    "    Date                 : %c%c%c\n",
-				    oemfru[13], oemfru[14], oemfru[15]);
-			seq_sprintf(seq,
-				    "    Sequence #           : %c%c%c%c\n",
-				    oemfru[16], oemfru[17], oemfru[18],
-				    oemfru[19]);
-			break;
-		}
-
-	case VENDOR3_FRU_FORMAT:
-		{
-			seq_puts(seq, "FRU Information:\n");
-		}
-
-	case VENDOR4_FRU_FORMAT:
-		{
-			seq_puts(seq, "FRU Information:\n");
-			seq_printf(seq,
-				    "    FRU Number           : %c%c%c%c%c%c%c%c\n",
-				    oemfru[0], oemfru[1], oemfru[2],
-				    oemfru[3], oemfru[4], oemfru[5],
-				    oemfru[6], oemfru[7]);
-			seq_sprintf(seq,
-				    "    Part Number          : %c%c%c%c%c%c%c%c\n",
-				    oemfru[8], oemfru[9], oemfru[10],
-				    oemfru[11], oemfru[12], oemfru[13],
-				    oemfru[14], oemfru[15]);
-			seq_printf(seq,
-				    "    EC Level             : %c%c%c%c%c%c%c%c\n",
-				    oemfru[16], oemfru[17], oemfru[18],
-				    oemfru[19], oemfru[20], oemfru[21],
-				    oemfru[22], oemfru[23]);
-			break;
-		}
-
-	default:
-		break;
-	}
-#endif
-
-	return 0;
-}
-
-static int slic_debug_adapter_show(struct seq_file *seq, void *v)
-{
-	struct adapter *adapter = seq->private;
-	struct net_device *netdev = adapter->netdev;
-
-	seq_printf(seq, "info: interface          : %s\n",
-			    adapter->netdev->name);
-	seq_printf(seq, "info: status             : %s\n",
-		SLIC_LINKSTATE(adapter->linkstate));
-	seq_printf(seq, "info: port               : %d\n",
-		adapter->physport);
-	seq_printf(seq, "info: speed              : %s\n",
-		SLIC_SPEED(adapter->linkspeed));
-	seq_printf(seq, "info: duplex             : %s\n",
-		SLIC_DUPLEX(adapter->linkduplex));
-	seq_printf(seq, "info: irq                : 0x%X\n",
-		(uint) adapter->irq);
-	seq_printf(seq, "info: Interrupt Agg Delay: %d usec\n",
-		adapter->card->loadlevel_current);
-	seq_printf(seq, "info: RcvQ max entries   : %4.4X\n",
-		SLIC_RCVQ_ENTRIES);
-	seq_printf(seq, "info: RcvQ current       : %4.4X\n",
-		    adapter->rcvqueue.count);
-	seq_printf(seq, "rx stats: packets                  : %8.8lX\n",
-		    netdev->stats.rx_packets);
-	seq_printf(seq, "rx stats: bytes                    : %8.8lX\n",
-		    netdev->stats.rx_bytes);
-	seq_printf(seq, "rx stats: broadcasts               : %8.8X\n",
-		    adapter->rcv_broadcasts);
-	seq_printf(seq, "rx stats: multicasts               : %8.8X\n",
-		    adapter->rcv_multicasts);
-	seq_printf(seq, "rx stats: unicasts                 : %8.8X\n",
-		    adapter->rcv_unicasts);
-	seq_printf(seq, "rx stats: errors                   : %8.8X\n",
-		    (u32) adapter->slic_stats.iface.rcv_errors);
-	seq_printf(seq, "rx stats: Missed errors            : %8.8X\n",
-		    (u32) adapter->slic_stats.iface.rcv_discards);
-	seq_printf(seq, "rx stats: drops                    : %8.8X\n",
-			(u32) adapter->rcv_drops);
-	seq_printf(seq, "tx stats: packets                  : %8.8lX\n",
-			netdev->stats.tx_packets);
-	seq_printf(seq, "tx stats: bytes                    : %8.8lX\n",
-			netdev->stats.tx_bytes);
-	seq_printf(seq, "tx stats: errors                   : %8.8X\n",
-			(u32) adapter->slic_stats.iface.xmt_errors);
-	seq_printf(seq, "rx stats: multicasts               : %8.8lX\n",
-			netdev->stats.multicast);
-	seq_printf(seq, "tx stats: collision errors         : %8.8X\n",
-			(u32) adapter->slic_stats.iface.xmit_collisions);
-	seq_printf(seq, "perf: Max rcv frames/isr           : %8.8X\n",
-			adapter->max_isr_rcvs);
-	seq_printf(seq, "perf: Rcv interrupt yields         : %8.8X\n",
-			adapter->rcv_interrupt_yields);
-	seq_printf(seq, "perf: Max xmit complete/isr        : %8.8X\n",
-			adapter->max_isr_xmits);
-	seq_printf(seq, "perf: error interrupts             : %8.8X\n",
-			adapter->error_interrupts);
-	seq_printf(seq, "perf: error rmiss interrupts       : %8.8X\n",
-			adapter->error_rmiss_interrupts);
-	seq_printf(seq, "perf: rcv interrupts               : %8.8X\n",
-			adapter->rcv_interrupts);
-	seq_printf(seq, "perf: xmit interrupts              : %8.8X\n",
-			adapter->xmit_interrupts);
-	seq_printf(seq, "perf: link event interrupts        : %8.8X\n",
-			adapter->linkevent_interrupts);
-	seq_printf(seq, "perf: UPR interrupts               : %8.8X\n",
-			adapter->upr_interrupts);
-	seq_printf(seq, "perf: interrupt count              : %8.8X\n",
-			adapter->num_isrs);
-	seq_printf(seq, "perf: false interrupts             : %8.8X\n",
-			adapter->false_interrupts);
-	seq_printf(seq, "perf: All register writes          : %8.8X\n",
-			adapter->all_reg_writes);
-	seq_printf(seq, "perf: ICR register writes          : %8.8X\n",
-			adapter->icr_reg_writes);
-	seq_printf(seq, "perf: ISR register writes          : %8.8X\n",
-			adapter->isr_reg_writes);
-	seq_printf(seq, "ifevents: overflow 802 errors      : %8.8X\n",
-			adapter->if_events.oflow802);
-	seq_printf(seq, "ifevents: transport overflow errors: %8.8X\n",
-			adapter->if_events.Tprtoflow);
-	seq_printf(seq, "ifevents: underflow errors         : %8.8X\n",
-			adapter->if_events.uflow802);
-	seq_printf(seq, "ifevents: receive early            : %8.8X\n",
-			adapter->if_events.rcvearly);
-	seq_printf(seq, "ifevents: buffer overflows         : %8.8X\n",
-			adapter->if_events.Bufov);
-	seq_printf(seq, "ifevents: carrier errors           : %8.8X\n",
-			adapter->if_events.Carre);
-	seq_printf(seq, "ifevents: Long                     : %8.8X\n",
-			adapter->if_events.Longe);
-	seq_printf(seq, "ifevents: invalid preambles        : %8.8X\n",
-			adapter->if_events.Invp);
-	seq_printf(seq, "ifevents: CRC errors               : %8.8X\n",
-			adapter->if_events.Crc);
-	seq_printf(seq, "ifevents: dribble nibbles          : %8.8X\n",
-			adapter->if_events.Drbl);
-	seq_printf(seq, "ifevents: Code violations          : %8.8X\n",
-			adapter->if_events.Code);
-	seq_printf(seq, "ifevents: TCP checksum errors      : %8.8X\n",
-			adapter->if_events.TpCsum);
-	seq_printf(seq, "ifevents: TCP header short errors  : %8.8X\n",
-			adapter->if_events.TpHlen);
-	seq_printf(seq, "ifevents: IP checksum errors       : %8.8X\n",
-			adapter->if_events.IpCsum);
-	seq_printf(seq, "ifevents: IP frame incompletes     : %8.8X\n",
-			adapter->if_events.IpLen);
-	seq_printf(seq, "ifevents: IP headers shorts        : %8.8X\n",
-			adapter->if_events.IpHlen);
-
-	return 0;
-}
-static int slic_debug_adapter_open(struct inode *inode, struct file *file)
-{
-	return single_open(file, slic_debug_adapter_show, inode->i_private);
-}
-
-static int slic_debug_card_open(struct inode *inode, struct file *file)
-{
-	return single_open(file, slic_debug_card_show, inode->i_private);
-}
-
-static const struct file_operations slic_debug_adapter_fops = {
-	.owner		= THIS_MODULE,
-	.open		= slic_debug_adapter_open,
-	.read		= seq_read,
-	.llseek		= seq_lseek,
-	.release	= single_release,
-};
-
-static const struct file_operations slic_debug_card_fops = {
-	.owner		= THIS_MODULE,
-	.open		= slic_debug_card_open,
-	.read		= seq_read,
-	.llseek		= seq_lseek,
-	.release	= single_release,
-};
-
-static void slic_debug_adapter_create(struct adapter *adapter)
-{
-	struct dentry *d;
-	char    name[7];
-	struct sliccard *card = adapter->card;
-
-	if (!card->debugfs_dir)
-		return;
-
-	sprintf(name, "port%d", adapter->port);
-	d = debugfs_create_file(name, S_IRUGO,
-				card->debugfs_dir, adapter,
-				&slic_debug_adapter_fops);
-	if (!d || IS_ERR(d))
-		pr_info(PFX "%s: debugfs create failed\n", name);
-	else
-		adapter->debugfs_entry = d;
-}
-
-static void slic_debug_adapter_destroy(struct adapter *adapter)
-{
-	debugfs_remove(adapter->debugfs_entry);
-	adapter->debugfs_entry = NULL;
-}
-
-static void slic_debug_card_create(struct sliccard *card)
-{
-	struct dentry *d;
-	char    name[IFNAMSIZ];
-
-	snprintf(name, sizeof(name), "slic%d", card->cardnum);
-	d = debugfs_create_dir(name, slic_debugfs);
-	if (!d || IS_ERR(d))
-		pr_info(PFX "%s: debugfs create dir failed\n",
-				name);
-	else {
-		card->debugfs_dir = d;
-		d = debugfs_create_file("cardinfo", S_IRUGO,
-				slic_debugfs, card,
-				&slic_debug_card_fops);
-		if (!d || IS_ERR(d))
-			pr_info(PFX "%s: debugfs create failed\n",
-					name);
-		else
-			card->debugfs_cardinfo = d;
-	}
-}
-
-static void slic_debug_card_destroy(struct sliccard *card)
-{
-	int i;
-
-	for (i = 0; i < card->card_size; i++) {
-		struct adapter *adapter;
-
-		adapter = card->adapter[i];
-		if (adapter)
-			slic_debug_adapter_destroy(adapter);
-	}
-	debugfs_remove(card->debugfs_cardinfo);
-	debugfs_remove(card->debugfs_dir);
-}
-
-static void slic_debug_init(void)
-{
-	struct dentry *ent;
-
-	ent = debugfs_create_dir("slic", NULL);
-	if (!ent || IS_ERR(ent)) {
-		pr_info(PFX "debugfs create directory failed\n");
-		return;
-	}
-
-	slic_debugfs = ent;
-}
-
-static void slic_debug_cleanup(void)
-{
-	debugfs_remove(slic_debugfs);
-}
-
 /*
  * slic_link_event_handler -
  *
@@ -2947,30 +2430,21 @@ static void slic_card_cleanup(struct sliccard *card)
 		del_timer_sync(&card->loadtimer);
 	}
 
-	slic_debug_card_destroy(card);
-
 	kfree(card);
 }
 
 static void slic_entry_remove(struct pci_dev *pcidev)
 {
 	struct net_device *dev = pci_get_drvdata(pcidev);
-	u32 mmio_start = 0;
-	uint mmio_len = 0;
 	struct adapter *adapter = netdev_priv(dev);
 	struct sliccard *card;
 	struct mcast_address *mcaddr, *mlist;
 
-	slic_adapter_freeresources(adapter);
-	slic_unmap_mmio_space(adapter);
 	unregister_netdev(dev);
 
-	mmio_start = pci_resource_start(pcidev, 0);
-	mmio_len = pci_resource_len(pcidev, 0);
-
-	release_mem_region(mmio_start, mmio_len);
+	slic_adapter_freeresources(adapter);
+	slic_unmap_mmio_space(adapter);
 
-	iounmap((void __iomem *)dev->base_addr);
 	/* free multicast addresses */
 	mlist = adapter->mcastaddrs;
 	while (mlist) {
@@ -3225,13 +2699,8 @@ static int slic_card_init(struct sliccard *card, struct adapter *adapter)
 
 	/* Download the microcode */
 	status = slic_card_download(adapter);
-
-	if (status != 0) {
-		dev_err(&adapter->pcidev->dev,
-			"download failed bus %d slot %d\n",
-			adapter->busnumber, adapter->slotnumber);
+	if (status)
 		return status;
-	}
 
 	if (!card->config_set) {
 		peeprom = pci_alloc_consistent(adapter->pcidev,
@@ -3243,8 +2712,7 @@ static int slic_card_init(struct sliccard *card, struct adapter *adapter)
 
 		if (!peeprom) {
 			dev_err(&adapter->pcidev->dev,
-				"eeprom read failed to get memory bus %d slot %d\n", adapter->busnumber,
-				adapter->slotnumber);
+				"Failed to allocate DMA memory for EEPROM.\n");
 			return -ENOMEM;
 		} else {
 			memset(peeprom, 0, sizeof(struct slic_eeprom));
@@ -3256,13 +2724,19 @@ static int slic_card_init(struct sliccard *card, struct adapter *adapter)
 
 		spin_lock_irqsave(&adapter->bit64reglock.lock,
 					adapter->bit64reglock.flags);
-		slic_reg32_write(&slic_regs->slic_addr_upper, 0, DONT_FLUSH);
+		slic_reg32_write(&slic_regs->slic_addr_upper,
+				 SLIC_GET_ADDR_HIGH(&pshmem->isr), DONT_FLUSH);
 		slic_reg32_write(&slic_regs->slic_isp,
 				 SLIC_GET_ADDR_LOW(&pshmem->isr), FLUSH);
 		spin_unlock_irqrestore(&adapter->bit64reglock.lock,
 					adapter->bit64reglock.flags);
 
-		slic_config_get(adapter, phys_configl, phys_configh);
+		status = slic_config_get(adapter, phys_configl, phys_configh);
+		if (status) {
+			dev_err(&adapter->pcidev->dev,
+				"Failed to fetch config data from device.\n");
+			goto card_init_err;
+		}
 
 		for (;;) {
 			if (adapter->pshmem->isr) {
@@ -3287,13 +2761,13 @@ static int slic_card_init(struct sliccard *card, struct adapter *adapter)
 				i++;
 				if (i > 5000) {
 					dev_err(&adapter->pcidev->dev,
-						"%d config data fetch timed out!\n",
-						adapter->port);
+						"Fetch of config data timed out.\n");
 					slic_reg64_write(adapter,
 						&slic_regs->slic_isp, 0,
 						&slic_regs->slic_addr_upper,
 						0, FLUSH);
-					return -EINVAL;
+					status = -EINVAL;
+					goto card_init_err;
 				}
 			}
 		}
@@ -3339,9 +2813,8 @@ static int slic_card_init(struct sliccard *card, struct adapter *adapter)
 			/*
 			    calculate the EEPROM checksum
 			*/
-			calc_chksum =
-			    ~slic_eeprom_cksum((char *) peeprom,
-					       (eecodesize - 2));
+			calc_chksum = slic_eeprom_cksum(peeprom,
+							eecodesize - 2);
 			/*
 			    if the ucdoe chksum flag bit worked,
 			    we wouldn't need this
@@ -3367,24 +2840,20 @@ static int slic_card_init(struct sliccard *card, struct adapter *adapter)
 				    sizeof(struct slic_eeprom),
 				    peeprom, phys_config);
 
-		if ((!card->config.EepromValid) &&
-		    (adapter->reg_params.fail_on_bad_eeprom)) {
+		if (!card->config.EepromValid) {
 			slic_reg64_write(adapter, &slic_regs->slic_isp, 0,
 					 &slic_regs->slic_addr_upper,
 					 0, FLUSH);
-			dev_err(&adapter->pcidev->dev,
-				"unsupported CONFIGURATION EEPROM invalid\n");
+			dev_err(&adapter->pcidev->dev, "EEPROM invalid.\n");
 			return -EINVAL;
 		}
 
 		card->config_set = 1;
 	}
 
-	if (slic_card_download_gbrcv(adapter)) {
-		dev_err(&adapter->pcidev->dev,
-			"unable to download GB receive microcode\n");
-		return -EINVAL;
-	}
+	status = slic_card_download_gbrcv(adapter);
+	if (status)
+		return status;
 
 	if (slic_global.dynamic_intagg)
 		slic_intagg_set(adapter, 0);
@@ -3403,6 +2872,11 @@ static int slic_card_init(struct sliccard *card, struct adapter *adapter)
 	card->reset_in_progress = 0;
 
 	return 0;
+
+card_init_err:
+	pci_free_consistent(adapter->pcidev, sizeof(struct slic_eeprom),
+			    peeprom, phys_config);
+	return status;
 }
 
 static void slic_init_driver(void)
@@ -3410,7 +2884,6 @@ static void slic_init_driver(void)
 	if (slic_first_init) {
 		slic_first_init = 0;
 		spin_lock_init(&slic_global.driver_lock.lock);
-		slic_debug_init();
 	}
 }
 
@@ -3430,16 +2903,12 @@ static void slic_init_adapter(struct net_device *netdev,
 	adapter->busnumber = pcidev->bus->number;
 	adapter->slotnumber = ((pcidev->devfn >> 3) & 0x1F);
 	adapter->functionnumber = (pcidev->devfn & 0x7);
-	adapter->memorylength = pci_resource_len(pcidev, 0);
 	adapter->slic_regs = (__iomem struct slic_regs *)memaddr;
 	adapter->irq = pcidev->irq;
 /*	adapter->netdev = netdev;*/
-	adapter->next_netdevice = head_netdevice;
-	head_netdevice = netdev;
 	adapter->chipid = chip_idx;
 	adapter->port = 0;	/*adapter->functionnumber;*/
 	adapter->cardindex = adapter->port;
-	adapter->memorybase = memaddr;
 	spin_lock_init(&adapter->upr_lock.lock);
 	spin_lock_init(&adapter->bit64reglock.lock);
 	spin_lock_init(&adapter->adapter_lock.lock);
@@ -3529,8 +2998,6 @@ static u32 slic_card_locate(struct adapter *adapter)
 			}
 		}
 		slic_global.num_slic_cards++;
-
-		slic_debug_card_create(card);
 	} else {
 		/* Card exists, find the card this adapter belongs to */
 		while (card) {
@@ -3595,7 +3062,6 @@ static int slic_entry_probe(struct pci_dev *pcidev,
 	struct net_device *netdev;
 	struct adapter *adapter;
 	void __iomem *memmapped_ioaddr = NULL;
-	u32 status = 0;
 	ulong mmio_start = 0;
 	ulong mmio_len = 0;
 	struct sliccard *card = NULL;
@@ -3608,9 +3074,9 @@ static int slic_entry_probe(struct pci_dev *pcidev,
 	if (err)
 		return err;
 
-	if (slic_debug > 0 && did_version++ == 0) {
-		dev_dbg(&pcidev->dev, "%s\n", slic_banner);
-		dev_dbg(&pcidev->dev, "%s\n", slic_proc_version);
+	if (did_version++ == 0) {
+		dev_info(&pcidev->dev, "%s\n", slic_banner);
+		dev_info(&pcidev->dev, "%s\n", slic_proc_version);
 	}
 
 	if (!pci_set_dma_mask(pcidev, DMA_BIT_MASK(64))) {
@@ -3686,23 +3152,16 @@ static int slic_entry_probe(struct pci_dev *pcidev,
 		adapter->allocated = 1;
 	}
 
-	status = slic_card_init(card, adapter);
+	err = slic_card_init(card, adapter);
+	if (err)
+		goto err_out_unmap;
 
-	if (status != 0) {
-		card->state = CARD_FAIL;
-		adapter->state = ADAPT_FAIL;
-		adapter->linkstate = LINK_DOWN;
-		dev_err(&pcidev->dev, "FAILED status[%x]\n", status);
-	} else {
-		slic_adapter_set_hwaddr(adapter);
-	}
+	slic_adapter_set_hwaddr(adapter);
 
-	netdev->base_addr = (unsigned long)adapter->memorybase;
+	netdev->base_addr = (unsigned long) memmapped_ioaddr;
 	netdev->irq = adapter->irq;
 	netdev->netdev_ops = &slic_netdev_ops;
 
-	slic_debug_adapter_create(adapter);
-
 	strcpy(netdev->name, "eth%d");
 	err = register_netdev(netdev);
 	if (err) {
@@ -3712,7 +3171,7 @@ static int slic_entry_probe(struct pci_dev *pcidev,
 
 	cards_found++;
 
-	return status;
+	return 0;
 
 err_out_unmap:
 	iounmap(memmapped_ioaddr);
@@ -3736,18 +3195,12 @@ static int __init slic_module_init(void)
 {
 	slic_init_driver();
 
-	if (debug >= 0 && slic_debug != debug)
-		pr_debug("debug level is %d.\n", debug);
-	if (debug >= 0)
-		slic_debug = debug;
-
 	return pci_register_driver(&slic_driver);
 }
 
 static void __exit slic_module_cleanup(void)
 {
 	pci_unregister_driver(&slic_driver);
-	slic_debug_cleanup();
 }
 
 module_init(slic_module_init);
diff --git a/drivers/staging/speakup/kobjects.c b/drivers/staging/speakup/kobjects.c
index 1ca91f7092b..2ef7f6f201a 100644
--- a/drivers/staging/speakup/kobjects.c
+++ b/drivers/staging/speakup/kobjects.c
@@ -641,7 +641,7 @@ ssize_t spk_var_store(struct kobject *kobj, struct kobj_attribute *attr,
 		if (kstrtol(cp, 10, &value) == 0)
 			ret = spk_set_num_var(value, param, len);
 		else
-			pr_warn("overflow or parsing error has occured");
+			pr_warn("overflow or parsing error has occurred");
 		if (ret == -ERANGE) {
 			var_data = param->data;
 			pr_warn("value for %s out of range, expect %d to %d\n",
@@ -851,75 +851,75 @@ static ssize_t message_store(struct kobject *kobj, struct kobj_attribute *attr,
  * Declare the attributes.
  */
 static struct kobj_attribute keymap_attribute =
-	__ATTR(keymap, S_IWUSR|S_IRUGO, keymap_show, keymap_store);
+	__ATTR_RW(keymap);
 static struct kobj_attribute silent_attribute =
-	__ATTR(silent, S_IWUGO, NULL, silent_store);
+	__ATTR_WO(silent);
 static struct kobj_attribute synth_attribute =
-	__ATTR(synth, S_IWUGO|S_IRUGO, synth_show, synth_store);
+	__ATTR_RW(synth);
 static struct kobj_attribute synth_direct_attribute =
-	__ATTR(synth_direct, S_IWUGO, NULL, synth_direct_store);
+	__ATTR_WO(synth_direct);
 static struct kobj_attribute version_attribute =
 	__ATTR_RO(version);
 
 static struct kobj_attribute delimiters_attribute =
-	__ATTR(delimiters, S_IWUGO|S_IRUGO, punc_show, punc_store);
+	__ATTR(delimiters, S_IWUSR|S_IRUGO, punc_show, punc_store);
 static struct kobj_attribute ex_num_attribute =
-	__ATTR(ex_num, S_IWUGO|S_IRUGO, punc_show, punc_store);
+	__ATTR(ex_num, S_IWUSR|S_IRUGO, punc_show, punc_store);
 static struct kobj_attribute punc_all_attribute =
-	__ATTR(punc_all, S_IWUGO|S_IRUGO, punc_show, punc_store);
+	__ATTR(punc_all, S_IWUSR|S_IRUGO, punc_show, punc_store);
 static struct kobj_attribute punc_most_attribute =
-	__ATTR(punc_most, S_IWUGO|S_IRUGO, punc_show, punc_store);
+	__ATTR(punc_most, S_IWUSR|S_IRUGO, punc_show, punc_store);
 static struct kobj_attribute punc_some_attribute =
-	__ATTR(punc_some, S_IWUGO|S_IRUGO, punc_show, punc_store);
+	__ATTR(punc_some, S_IWUSR|S_IRUGO, punc_show, punc_store);
 static struct kobj_attribute repeats_attribute =
-	__ATTR(repeats, S_IWUGO|S_IRUGO, punc_show, punc_store);
+	__ATTR(repeats, S_IWUSR|S_IRUGO, punc_show, punc_store);
 
 static struct kobj_attribute attrib_bleep_attribute =
-	__ATTR(attrib_bleep, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(attrib_bleep, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute bell_pos_attribute =
-	__ATTR(bell_pos, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(bell_pos, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute bleep_time_attribute =
-	__ATTR(bleep_time, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(bleep_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute bleeps_attribute =
-	__ATTR(bleeps, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(bleeps, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute cursor_time_attribute =
-	__ATTR(cursor_time, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(cursor_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute key_echo_attribute =
-	__ATTR(key_echo, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(key_echo, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute no_interrupt_attribute =
-	__ATTR(no_interrupt, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(no_interrupt, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute punc_level_attribute =
-	__ATTR(punc_level, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(punc_level, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute reading_punc_attribute =
-	__ATTR(reading_punc, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(reading_punc, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute say_control_attribute =
-	__ATTR(say_control, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(say_control, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute say_word_ctl_attribute =
-	__ATTR(say_word_ctl, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(say_word_ctl, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute spell_delay_attribute =
-	__ATTR(spell_delay, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(spell_delay, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 /*
  * These attributes are i18n related.
  */
 static struct kobj_attribute announcements_attribute =
-	__ATTR(announcements, S_IWUGO|S_IRUGO, message_show, message_store);
+	__ATTR(announcements, S_IWUSR|S_IRUGO, message_show, message_store);
 static struct kobj_attribute characters_attribute =
-	__ATTR(characters, S_IWUGO|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_IWUGO|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_IWUGO|S_IRUGO, message_show, message_store);
+	__ATTR(ctl_keys, S_IWUSR|S_IRUGO, message_show, message_store);
 static struct kobj_attribute colors_attribute =
-	__ATTR(colors, S_IWUGO|S_IRUGO, message_show, message_store);
+	__ATTR(colors, S_IWUSR|S_IRUGO, message_show, message_store);
 static struct kobj_attribute formatted_attribute =
-	__ATTR(formatted, S_IWUGO|S_IRUGO, message_show, message_store);
+	__ATTR(formatted, S_IWUSR|S_IRUGO, message_show, message_store);
 static struct kobj_attribute function_names_attribute =
-	__ATTR(function_names, S_IWUGO|S_IRUGO, message_show, message_store);
+	__ATTR(function_names, S_IWUSR|S_IRUGO, message_show, message_store);
 static struct kobj_attribute key_names_attribute =
-	__ATTR(key_names, S_IWUGO|S_IRUGO, message_show, message_store);
+	__ATTR(key_names, S_IWUSR|S_IRUGO, message_show, message_store);
 static struct kobj_attribute states_attribute =
-	__ATTR(states, S_IWUGO|S_IRUGO, message_show, message_store);
+	__ATTR(states, S_IWUSR|S_IRUGO, message_show, message_store);
 
 /*
  * Create groups of attributes so that we can create and destroy them all
diff --git a/drivers/staging/speakup/main.c b/drivers/staging/speakup/main.c
index 3b6e5358c72..7de79d59a4c 100644
--- a/drivers/staging/speakup/main.c
+++ b/drivers/staging/speakup/main.c
@@ -2218,6 +2218,7 @@ static void __exit speakup_exit(void)
 	unregister_keyboard_notifier(&keyboard_notifier_block);
 	unregister_vt_notifier(&vt_notifier_block);
 	speakup_unregister_devsynth();
+	speakup_cancel_paste();
 	del_timer(&cursor_timer);
 	kthread_stop(speakup_task);
 	speakup_task = NULL;
diff --git a/drivers/staging/speakup/selection.c b/drivers/staging/speakup/selection.c
index f0fb00392d6..ca04d3669ac 100644
--- a/drivers/staging/speakup/selection.c
+++ b/drivers/staging/speakup/selection.c
@@ -4,6 +4,10 @@
 #include <linux/sched.h>
 #include <linux/device.h> /* for dev_warn */
 #include <linux/selection.h>
+#include <linux/workqueue.h>
+#include <linux/tty.h>
+#include <linux/tty_flip.h>
+#include <asm/cmpxchg.h>
 
 #include "speakup.h"
 
@@ -121,31 +125,61 @@ int speakup_set_selection(struct tty_struct *tty)
 	return 0;
 }
 
-/* TODO: move to some helper thread, probably.  That'd fix having to check for
- * in_atomic().  */
-int speakup_paste_selection(struct tty_struct *tty)
+struct speakup_paste_work {
+	struct work_struct work;
+	struct tty_struct *tty;
+};
+
+static void __speakup_paste_selection(struct work_struct *work)
 {
+	struct speakup_paste_work *spw =
+		container_of(work, struct speakup_paste_work, work);
+	struct tty_struct *tty = xchg(&spw->tty, NULL);
 	struct vc_data *vc = (struct vc_data *) tty->driver_data;
 	int pasted = 0, count;
+	struct tty_ldisc *ld;
 	DECLARE_WAITQUEUE(wait, current);
+
+	ld = tty_ldisc_ref_wait(tty);
+	tty_buffer_lock_exclusive(&vc->port);
+
 	add_wait_queue(&vc->paste_wait, &wait);
 	while (sel_buffer && sel_buffer_lth > pasted) {
 		set_current_state(TASK_INTERRUPTIBLE);
 		if (test_bit(TTY_THROTTLED, &tty->flags)) {
-			if (in_atomic())
-				/* if we are in an interrupt handler, abort */
-				break;
 			schedule();
 			continue;
 		}
 		count = sel_buffer_lth - pasted;
-		count = min_t(int, count, tty->receive_room);
-		tty->ldisc->ops->receive_buf(tty, sel_buffer + pasted,
-			NULL, count);
+		count = tty_ldisc_receive_buf(ld, sel_buffer + pasted, NULL,
+					      count);
 		pasted += count;
 	}
 	remove_wait_queue(&vc->paste_wait, &wait);
 	current->state = TASK_RUNNING;
+
+	tty_buffer_unlock_exclusive(&vc->port);
+	tty_ldisc_deref(ld);
+	tty_kref_put(tty);
+}
+
+static struct speakup_paste_work speakup_paste_work = {
+	.work = __WORK_INITIALIZER(speakup_paste_work.work,
+				   __speakup_paste_selection)
+};
+
+int speakup_paste_selection(struct tty_struct *tty)
+{
+	if (cmpxchg(&speakup_paste_work.tty, NULL, tty) != NULL)
+		return -EBUSY;
+
+	tty_kref_get(tty);
+	schedule_work_on(WORK_CPU_UNBOUND, &speakup_paste_work.work);
 	return 0;
 }
 
+void speakup_cancel_paste(void)
+{
+	cancel_work_sync(&speakup_paste_work.work);
+	tty_kref_put(speakup_paste_work.tty);
+}
diff --git a/drivers/staging/speakup/speakup.h b/drivers/staging/speakup/speakup.h
index a7bcceec436..898dce5e124 100644
--- a/drivers/staging/speakup/speakup.h
+++ b/drivers/staging/speakup/speakup.h
@@ -75,6 +75,7 @@ extern void synth_buffer_clear(void);
 extern void speakup_clear_selection(void);
 extern int speakup_set_selection(struct tty_struct *tty);
 extern int speakup_paste_selection(struct tty_struct *tty);
+extern void speakup_cancel_paste(void);
 extern void speakup_register_devsynth(void);
 extern void speakup_unregister_devsynth(void);
 extern void synth_write(const char *buf, size_t count);
diff --git a/drivers/staging/speakup/speakup_acntpc.c b/drivers/staging/speakup/speakup_acntpc.c
index e7dfa434bd9..31f952b9049 100644
--- a/drivers/staging/speakup/speakup_acntpc.c
+++ b/drivers/staging/speakup/speakup_acntpc.c
@@ -62,22 +62,22 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/acntpc.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute tone_attribute =
-	__ATTR(tone, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(tone, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_acntsa.c b/drivers/staging/speakup/speakup_acntsa.c
index c7f014ed962..3f2b5698a3d 100644
--- a/drivers/staging/speakup/speakup_acntsa.c
+++ b/drivers/staging/speakup/speakup_acntsa.c
@@ -47,28 +47,28 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/acntsa.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute tone_attribute =
-	__ATTR(tone, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(tone, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
-	__ATTR(delay_time, S_IRUSR|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
-	__ATTR(full_time, S_IRUSR|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
-	__ATTR(jiffy_delta, S_IRUSR|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(jiffy_delta, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute trigger_time_attribute =
-	__ATTR(trigger_time, S_IRUSR|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(trigger_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 /*
  * Create a group of attributes so that we can create and destroy them all
diff --git a/drivers/staging/speakup/speakup_apollo.c b/drivers/staging/speakup/speakup_apollo.c
index 38c8c2221e4..678b263e551 100644
--- a/drivers/staging/speakup/speakup_apollo.c
+++ b/drivers/staging/speakup/speakup_apollo.c
@@ -53,24 +53,24 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/apollo.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute lang_attribute =
-	__ATTR(lang, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(lang, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute voice_attribute =
-	__ATTR(voice, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(voice, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_audptr.c b/drivers/staging/speakup/speakup_audptr.c
index de5b4a5f43b..362f9747e48 100644
--- a/drivers/staging/speakup/speakup_audptr.c
+++ b/drivers/staging/speakup/speakup_audptr.c
@@ -49,24 +49,24 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/audptr.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute punct_attribute =
-	__ATTR(punct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(punct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute tone_attribute =
-	__ATTR(tone, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(tone, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_bns.c b/drivers/staging/speakup/speakup_bns.c
index 4939e8c7272..2f070282a85 100644
--- a/drivers/staging/speakup/speakup_bns.c
+++ b/drivers/staging/speakup/speakup_bns.c
@@ -44,22 +44,22 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/bns.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute tone_attribute =
-	__ATTR(tone, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(tone, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_decext.c b/drivers/staging/speakup/speakup_decext.c
index b17af980392..67b7de1d8c7 100644
--- a/drivers/staging/speakup/speakup_decext.c
+++ b/drivers/staging/speakup/speakup_decext.c
@@ -70,24 +70,24 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/decext.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute punct_attribute =
-	__ATTR(punct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(punct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute voice_attribute =
-	__ATTR(voice, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(voice, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_decpc.c b/drivers/staging/speakup/speakup_decpc.c
index cfa4bc03235..67678d8888c 100644
--- a/drivers/staging/speakup/speakup_decpc.c
+++ b/drivers/staging/speakup/speakup_decpc.c
@@ -164,24 +164,24 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/decpc.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute punct_attribute =
-	__ATTR(punct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(punct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute voice_attribute =
-	__ATTR(voice, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(voice, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_dectlk.c b/drivers/staging/speakup/speakup_dectlk.c
index 1fcae55dabb..af848686be7 100644
--- a/drivers/staging/speakup/speakup_dectlk.c
+++ b/drivers/staging/speakup/speakup_dectlk.c
@@ -70,24 +70,24 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/dectlk.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute punct_attribute =
-	__ATTR(punct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(punct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute voice_attribute =
-	__ATTR(voice, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(voice, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_dtlk.c b/drivers/staging/speakup/speakup_dtlk.c
index 5c6c34191e8..98d1f497e4e 100644
--- a/drivers/staging/speakup/speakup_dtlk.c
+++ b/drivers/staging/speakup/speakup_dtlk.c
@@ -67,28 +67,28 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/dtlk.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute freq_attribute =
-	__ATTR(freq, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(freq, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute punct_attribute =
-	__ATTR(punct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(punct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute tone_attribute =
-	__ATTR(tone, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(tone, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute voice_attribute =
-	__ATTR(voice, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(voice, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_dummy.c b/drivers/staging/speakup/speakup_dummy.c
index e19e9994bbb..362342a194a 100644
--- a/drivers/staging/speakup/speakup_dummy.c
+++ b/drivers/staging/speakup/speakup_dummy.c
@@ -46,22 +46,22 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/dummy.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute tone_attribute =
-	__ATTR(tone, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(tone, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_keypc.c b/drivers/staging/speakup/speakup_keypc.c
index 9c246d701a9..9d30c1945f9 100644
--- a/drivers/staging/speakup/speakup_keypc.c
+++ b/drivers/staging/speakup/speakup_keypc.c
@@ -59,18 +59,18 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/keypc.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_ltlk.c b/drivers/staging/speakup/speakup_ltlk.c
index c9be6f52c25..d6de72295d3 100644
--- a/drivers/staging/speakup/speakup_ltlk.c
+++ b/drivers/staging/speakup/speakup_ltlk.c
@@ -50,28 +50,28 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/ltlk.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute freq_attribute =
-	__ATTR(freq, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(freq, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute punct_attribute =
-	__ATTR(punct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(punct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute tone_attribute =
-	__ATTR(tone, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(tone, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute voice_attribute =
-	__ATTR(voice, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(voice, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_soft.c b/drivers/staging/speakup/speakup_soft.c
index ee6089502a9..9ed72650926 100644
--- a/drivers/staging/speakup/speakup_soft.c
+++ b/drivers/staging/speakup/speakup_soft.c
@@ -61,35 +61,35 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/soft.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute freq_attribute =
-	__ATTR(freq, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(freq, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute punct_attribute =
-	__ATTR(punct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(punct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute tone_attribute =
-	__ATTR(tone, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(tone, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute voice_attribute =
-	__ATTR(voice, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(voice, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 /*
  * We should uncomment the following definition, when we agree on a
  * method of passing a language designation to the software synthesizer.
  * static struct kobj_attribute lang_attribute =
- *	__ATTR(lang, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+ *	__ATTR(lang, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
  */
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_spkout.c b/drivers/staging/speakup/speakup_spkout.c
index 711cf114df8..77f2dc2c3d9 100644
--- a/drivers/staging/speakup/speakup_spkout.c
+++ b/drivers/staging/speakup/speakup_spkout.c
@@ -48,24 +48,24 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/spkout.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute punct_attribute =
-	__ATTR(punct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(punct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute tone_attribute =
-	__ATTR(tone, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(tone, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_txprt.c b/drivers/staging/speakup/speakup_txprt.c
index 3f0be04df07..dbe84b13772 100644
--- a/drivers/staging/speakup/speakup_txprt.c
+++ b/drivers/staging/speakup/speakup_txprt.c
@@ -44,22 +44,22 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/txprt.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute tone_attribute =
-	__ATTR(tone, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(tone, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/synth.c b/drivers/staging/speakup/synth.c
index 0b3549bd909..172cf62b1aa 100644
--- a/drivers/staging/speakup/synth.c
+++ b/drivers/staging/speakup/synth.c
@@ -212,6 +212,9 @@ void synth_start(void)
 
 void spk_do_flush(void)
 {
+	if (!synth)
+		return;
+
 	speakup_info.flushing = 1;
 	synth_buffer_clear();
 	if (synth->alive) {
diff --git a/drivers/staging/tidspbridge/core/dsp-clock.c b/drivers/staging/tidspbridge/core/dsp-clock.c
index 2f084e181d3..a1aca4416ca 100644
--- a/drivers/staging/tidspbridge/core/dsp-clock.c
+++ b/drivers/staging/tidspbridge/core/dsp-clock.c
@@ -226,7 +226,7 @@ int dsp_clk_enable(enum dsp_clk_id clk_id)
 	case GPT_CLK:
 		status = omap_dm_timer_start(timer[clk_id - 1]);
 		break;
-#ifdef CONFIG_OMAP_MCBSP
+#ifdef CONFIG_SND_OMAP_SOC_MCBSP
 	case MCBSP_CLK:
 		omap_mcbsp_request(MCBSP_ID(clk_id));
 		omap2_mcbsp_set_clks_src(MCBSP_ID(clk_id), MCBSP_CLKS_PAD_SRC);
@@ -302,7 +302,7 @@ int dsp_clk_disable(enum dsp_clk_id clk_id)
 	case GPT_CLK:
 		status = omap_dm_timer_stop(timer[clk_id - 1]);
 		break;
-#ifdef CONFIG_OMAP_MCBSP
+#ifdef CONFIG_SND_OMAP_SOC_MCBSP
 	case MCBSP_CLK:
 		omap2_mcbsp_set_clks_src(MCBSP_ID(clk_id), MCBSP_CLKS_PRCM_SRC);
 		omap_mcbsp_free(MCBSP_ID(clk_id));
diff --git a/drivers/staging/unisys/channels/channel.c b/drivers/staging/unisys/channels/channel.c
index f6452595b74..7223a14082b 100644
--- a/drivers/staging/unisys/channels/channel.c
+++ b/drivers/staging/unisys/channels/channel.c
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/channels/chanstub.c b/drivers/staging/unisys/channels/chanstub.c
index f504f49a436..1e7d6a78602 100644
--- a/drivers/staging/unisys/channels/chanstub.c
+++ b/drivers/staging/unisys/channels/chanstub.c
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -25,11 +25,14 @@
 
 #include "channel.h"
 #include "chanstub.h"
+#include "timskmodutils.h"
 #include "version.h"
 
 static __init int
 channel_mod_init(void)
 {
+	if (!unisys_spar_platform)
+		return -ENODEV;
 	return 0;
 }
 
diff --git a/drivers/staging/unisys/channels/chanstub.h b/drivers/staging/unisys/channels/chanstub.h
index 8d727debca6..bdee5d529f6 100644
--- a/drivers/staging/unisys/channels/chanstub.h
+++ b/drivers/staging/unisys/channels/chanstub.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/common-spar/include/channels/channel.h b/drivers/staging/unisys/common-spar/include/channels/channel.h
index aee204172b2..d19711de114 100644
--- a/drivers/staging/unisys/common-spar/include/channels/channel.h
+++ b/drivers/staging/unisys/common-spar/include/channels/channel.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -16,6 +16,8 @@
 #ifndef __CHANNEL_H__
 #define __CHANNEL_H__
 
+#include <linux/uuid.h>
+
 /*
 * Whenever this file is changed a corresponding change must be made in
 * the Console/ServicePart/visordiag_early/supervisor_channel.h file
@@ -24,7 +26,7 @@
 */
 
 /* define the following to prevent include nesting in kernel header
- * files of similar abreviated content
+ * files of similar abbreviated content
  */
 #define __SUPERVISOR_CHANNEL_H__
 
@@ -46,19 +48,6 @@
 #define COVER(v, d)   ((d)*COVERQ(v, d))
 #endif
 
-#ifndef GUID0
-#define GUID0 {0, 0, 0, {0, 0, 0, 0, 0, 0, 0, 0} }
-#endif
-
-/*  The C language is inconsistent with respect to where it allows literal
- *  constants, especially literal constant structs.  Literal constant structs
- *  are allowed for initialization only, whereas other types of literal
- *  constants are allowed anywhere.  We get around this inconsistency by
- *  declaring a "static const" variable for each GUID.  This variable can be
- *  used in expressions where the literal constant would not be allowed.
- */
-static const GUID Guid0 = GUID0;
-
 #define ULTRA_CHANNEL_PROTOCOL_SIGNATURE  SIGNATURE_32('E', 'C', 'N', 'L')
 
 typedef enum {
@@ -226,13 +215,13 @@ typedef struct _CHANNEL_HEADER {
 	U32 HeaderSize;		/* sizeof(CHANNEL_HEADER) */
 	U64 Size;		/* Total size of this channel in bytes */
 	U64 Features;		/* Flags to modify behavior */
-	GUID Type;		/* Channel type: data, bus, control, etc. */
+	uuid_le Type;		/* Channel type: data, bus, control, etc. */
 	U64 PartitionHandle;	/* ID of guest partition */
 	U64 Handle;		/* Device number of this channel in client */
 	U64 oChannelSpace;	/* Offset in bytes to channel specific area */
 	U32 VersionId;		/* CHANNEL_HEADER Version ID */
 	U32 PartitionIndex;	/* Index of guest partition */
-	GUID ZoneGuid;		/* Guid of Channel's zone */
+	uuid_le ZoneGuid;		/* Guid of Channel's zone */
 	U32 oClientString;	/* offset from channel header to
 				 * nul-terminated ClientString (0 if
 				 * ClientString not present) */
@@ -320,17 +309,17 @@ typedef struct _SIGNAL_QUEUE_HEADER {
  */
 static inline int
 ULTRA_check_channel_client(void __iomem *pChannel,
-			   GUID expectedTypeGuid,
+			   uuid_le expectedTypeGuid,
 			   char *channelName,
 			   U64 expectedMinBytes,
 			   U32 expectedVersionId,
 			   U64 expectedSignature,
 			   char *fileName, int lineNumber, void *logCtx)
 {
-	if (MEMCMP(&expectedTypeGuid, &Guid0, sizeof(GUID)) != 0)
+	if (uuid_le_cmp(expectedTypeGuid, NULL_UUID_LE) != 0)
 		/* caller wants us to verify type GUID */
 		if (MEMCMP_IO(&(((CHANNEL_HEADER __iomem *) (pChannel))->Type),
-			   &expectedTypeGuid, sizeof(GUID)) != 0) {
+			   &expectedTypeGuid, sizeof(uuid_le)) != 0) {
 			CHANNEL_GUID_MISMATCH(expectedTypeGuid, channelName,
 					      "type", expectedTypeGuid,
 					      ((CHANNEL_HEADER __iomem *)
@@ -344,8 +333,9 @@ ULTRA_check_channel_client(void __iomem *pChannel,
 			   (pChannel))->Size) < expectedMinBytes) {
 			CHANNEL_U64_MISMATCH(expectedTypeGuid, channelName,
 					     "size", expectedMinBytes,
-					     ((CHANNEL_HEADER __iomem *)
-					      (pChannel))->Size, fileName,
+					     readq(&((CHANNEL_HEADER __iomem *)
+						     (pChannel))->Size),
+					     fileName,
 					     lineNumber, logCtx);
 			return 0;
 		}
@@ -355,9 +345,9 @@ ULTRA_check_channel_client(void __iomem *pChannel,
 		    != expectedVersionId) {
 			CHANNEL_U32_MISMATCH(expectedTypeGuid, channelName,
 					     "version", expectedVersionId,
-					     ((CHANNEL_HEADER __iomem *)
-					      (pChannel))->VersionId, fileName,
-					     lineNumber, logCtx);
+					     readl(&((CHANNEL_HEADER __iomem *)
+						     (pChannel))->VersionId),
+					     fileName, lineNumber, logCtx);
 			return 0;
 		}
 	if (expectedSignature > 0)	/* caller wants us to verify
@@ -366,8 +356,9 @@ ULTRA_check_channel_client(void __iomem *pChannel,
 		    != expectedSignature) {
 			CHANNEL_U64_MISMATCH(expectedTypeGuid, channelName,
 					     "signature", expectedSignature,
-					     ((CHANNEL_HEADER __iomem *)
-					      (pChannel))->Signature, fileName,
+					     readq(&((CHANNEL_HEADER __iomem *)
+						     (pChannel))->Signature),
+					     fileName,
 					     lineNumber, logCtx);
 			return 0;
 		}
@@ -380,7 +371,7 @@ ULTRA_check_channel_client(void __iomem *pChannel,
  * is used to pass the EFI_DIAG_CAPTURE_PROTOCOL needed to log messages.
  */
 static inline int
-ULTRA_check_channel_server(GUID typeGuid,
+ULTRA_check_channel_server(uuid_le typeGuid,
 			   char *channelName,
 			   U64 expectedMinBytes,
 			   U64 actualBytes,
diff --git a/drivers/staging/unisys/common-spar/include/channels/channel_guid.h b/drivers/staging/unisys/common-spar/include/channels/channel_guid.h
index ae0dc2b2ad1..63c67ca4c9e 100644
--- a/drivers/staging/unisys/common-spar/include/channels/channel_guid.h
+++ b/drivers/staging/unisys/common-spar/include/channels/channel_guid.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -21,44 +21,44 @@
  * {414815ed-c58c-11da-95a9-00e08161165f}
  */
 #define ULTRA_VHBA_CHANNEL_PROTOCOL_GUID \
-	{ 0x414815ed, 0xc58c, 0x11da, \
-		{ 0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f } }
-static const GUID UltraVhbaChannelProtocolGuid =
+		UUID_LE(0x414815ed, 0xc58c, 0x11da, \
+				0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f)
+static const uuid_le UltraVhbaChannelProtocolGuid =
 	ULTRA_VHBA_CHANNEL_PROTOCOL_GUID;
 
 /* Used in IOChannel
  * {8cd5994d-c58e-11da-95a9-00e08161165f}
  */
 #define ULTRA_VNIC_CHANNEL_PROTOCOL_GUID \
-	{ 0x8cd5994d, 0xc58e, 0x11da, \
-		{ 0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f } }
-static const GUID UltraVnicChannelProtocolGuid =
+		UUID_LE(0x8cd5994d, 0xc58e, 0x11da, \
+				0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f)
+static const uuid_le UltraVnicChannelProtocolGuid =
 	ULTRA_VNIC_CHANNEL_PROTOCOL_GUID;
 
 /* Used in IOChannel
  * {72120008-4AAB-11DC-8530-444553544200}
  */
 #define ULTRA_SIOVM_GUID \
-	{ 0x72120008, 0x4AAB, 0x11DC,					\
-		{ 0x85, 0x30, 0x44, 0x45, 0x53, 0x54, 0x42, 0x00 } }
-static const GUID UltraSIOVMGuid = ULTRA_SIOVM_GUID;
+		UUID_LE(0x72120008, 0x4AAB, 0x11DC, \
+				0x85, 0x30, 0x44, 0x45, 0x53, 0x54, 0x42, 0x00)
+static const uuid_le UltraSIOVMGuid = ULTRA_SIOVM_GUID;
 
 
 /* Used in visornoop/visornoop_main.c
  * {5b52c5ac-e5f5-4d42-8dff-429eaecd221f}
  */
 #define ULTRA_CONTROLDIRECTOR_CHANNEL_PROTOCOL_GUID  \
-	{ 0x5b52c5ac, 0xe5f5, 0x4d42, \
-		{ 0x8d, 0xff, 0x42, 0x9e, 0xae, 0xcd, 0x22, 0x1f } }
+		UUID_LE(0x5b52c5ac, 0xe5f5, 0x4d42, \
+				0x8d, 0xff, 0x42, 0x9e, 0xae, 0xcd, 0x22, 0x1f)
 
-static const GUID UltraControlDirectorChannelProtocolGuid =
+static const uuid_le UltraControlDirectorChannelProtocolGuid =
 	ULTRA_CONTROLDIRECTOR_CHANNEL_PROTOCOL_GUID;
 
 /* Used in visorchipset/visorchipset_main.c
  * {B4E79625-AEDE-4EAA-9E11-D3EDDCD4504C}
  */
 #define ULTRA_DIAG_POOL_CHANNEL_PROTOCOL_GUID				\
-	{0xb4e79625, 0xaede, 0x4eaa,					\
-		{ 0x9e, 0x11, 0xd3, 0xed, 0xdc, 0xd4, 0x50, 0x4c } }
+		UUID_LE(0xb4e79625, 0xaede, 0x4eaa, \
+				0x9e, 0x11, 0xd3, 0xed, 0xdc, 0xd4, 0x50, 0x4c)
 
 
diff --git a/drivers/staging/unisys/common-spar/include/channels/controlframework.h b/drivers/staging/unisys/common-spar/include/channels/controlframework.h
index 51264334834..1a1c5053fcf 100644
--- a/drivers/staging/unisys/common-spar/include/channels/controlframework.h
+++ b/drivers/staging/unisys/common-spar/include/channels/controlframework.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/common-spar/include/channels/controlvmchannel.h b/drivers/staging/unisys/common-spar/include/channels/controlvmchannel.h
index 47f1c4fa1e7..d8b12a73348 100644
--- a/drivers/staging/unisys/common-spar/include/channels/controlvmchannel.h
+++ b/drivers/staging/unisys/common-spar/include/channels/controlvmchannel.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -16,6 +16,7 @@
 #ifndef __CONTROLVMCHANNEL_H__
 #define __CONTROLVMCHANNEL_H__
 
+#include <linux/uuid.h>
 #include "commontypes.h"
 #include "channel.h"
 #include "controlframework.h"
@@ -25,10 +26,10 @@ enum { INVALID_GUEST_FIRMWARE, SAMPLE_GUEST_FIRMWARE,
 
 /* {2B3C2D10-7EF5-4ad8-B966-3448B7386B3D} */
 #define ULTRA_CONTROLVM_CHANNEL_PROTOCOL_GUID	\
-	{0x2b3c2d10, 0x7ef5, 0x4ad8, \
-		{0xb9, 0x66, 0x34, 0x48, 0xb7, 0x38, 0x6b, 0x3d} }
+		UUID_LE(0x2b3c2d10, 0x7ef5, 0x4ad8, \
+				0xb9, 0x66, 0x34, 0x48, 0xb7, 0x38, 0x6b, 0x3d)
 
-static const GUID UltraControlvmChannelProtocolGuid =
+static const uuid_le UltraControlvmChannelProtocolGuid =
 	ULTRA_CONTROLVM_CHANNEL_PROTOCOL_GUID;
 
 #define ULTRA_CONTROLVM_CHANNEL_PROTOCOL_SIGNATURE \
@@ -293,8 +294,8 @@ typedef struct _CONTROLVM_PACKET_DEVICE_CREATE  {
 			*   can be dereferenced by the receiver
 			*   of this ControlVm command */
 	U64 channelBytes; /**< specifies size of the channel in bytes */
-	GUID dataTypeGuid;/**< specifies format of data in channel */
-	GUID devInstGuid; /**< instance guid for the device */
+	uuid_le dataTypeGuid;/**< specifies format of data in channel */
+	uuid_le devInstGuid; /**< instance guid for the device */
 	struct InterruptInfo intr; /**< specifies interrupt information */
 } CONTROLVM_PACKET_DEVICE_CREATE;	/* for CONTROLVM_DEVICE_CREATE */
 
@@ -333,9 +334,9 @@ typedef struct _CONTROLVM_MESSAGE_PACKET  {
 					      *   dereferenced by the receiver
 					      *   of this ControlVm command */
 			U64 channelBytes;    /*< size of the channel in bytes */
-			GUID busDataTypeGuid;/*< indicates format of data in bus
-					      * channel */
-			GUID busInstGuid;    /*< instance guid for the bus */
+			uuid_le busDataTypeGuid;/*< indicates format of data in
+						    bus channel */
+			uuid_le busInstGuid;    /*< instance guid for the bus */
 		} createBus;	/* for CONTROLVM_BUS_CREATE */
 		struct  {
 			U32 busNo;	      /*< bus # (0..n-1) from the msg
@@ -611,7 +612,7 @@ typedef struct _ULTRA_CONTROLVM_PARAMETERS_HEADER  {
 	U32 ClientLength;
 	U32 NameOffset;
 	U32 NameLength;
-	GUID Id;
+	uuid_le Id;
 	U32 Revision;
 	U32 Reserved;		/* Natural alignment */
 } ULTRA_CONTROLVM_PARAMETERS_HEADER;
diff --git a/drivers/staging/unisys/common-spar/include/channels/diagchannel.h b/drivers/staging/unisys/common-spar/include/channels/diagchannel.h
index c93515eb211..1bea2f720e4 100644
--- a/drivers/staging/unisys/common-spar/include/channels/diagchannel.h
+++ b/drivers/staging/unisys/common-spar/include/channels/diagchannel.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -38,16 +38,16 @@
 
 /* {EEA7A573-DB82-447c-8716-EFBEAAAE4858} */
 #define ULTRA_DIAG_CHANNEL_PROTOCOL_GUID \
-	{0xeea7a573, 0xdb82, 0x447c, \
-		{0x87, 0x16, 0xef, 0xbe, 0xaa, 0xae, 0x48, 0x58} }
+		UUID_LE(0xeea7a573, 0xdb82, 0x447c, \
+				0x87, 0x16, 0xef, 0xbe, 0xaa, 0xae, 0x48, 0x58)
 
-static const GUID UltraDiagChannelProtocolGuid =
+static const uuid_le UltraDiagChannelProtocolGuid =
 	ULTRA_DIAG_CHANNEL_PROTOCOL_GUID;
 
 /* {E850F968-3263-4484-8CA5-2A35D087A5A8} */
 #define ULTRA_DIAG_ROOT_CHANNEL_PROTOCOL_GUID \
-	{0xe850f968, 0x3263, 0x4484, \
-		{0x8c, 0xa5, 0x2a, 0x35, 0xd0, 0x87, 0xa5, 0xa8} }
+		UUID_LE(0xe850f968, 0x3263, 0x4484, \
+				0x8c, 0xa5, 0x2a, 0x35, 0xd0, 0x87, 0xa5, 0xa8)
 
 #define ULTRA_DIAG_CHANNEL_PROTOCOL_SIGNATURE  ULTRA_CHANNEL_PROTOCOL_SIGNATURE
 
@@ -356,7 +356,7 @@ typedef enum  {
  *IsChannelInitialized: 1 iff SignalInit was called for this channel; otherwise
  *			0, and assume the channel is not ready for use yet.
  *
- * Reserved: Padding to allign the fields in this structure.
+ * Reserved: Padding to align the fields in this structure.
  *
  *SubsystemSeverityFilter: Level of severity on a subsystem basis that controls
  *			whether events are logged.  Any event's severity for a
diff --git a/drivers/staging/unisys/common-spar/include/channels/iochannel.h b/drivers/staging/unisys/common-spar/include/channels/iochannel.h
index 8de1d249d55..6dcfa6e7837 100644
--- a/drivers/staging/unisys/common-spar/include/channels/iochannel.h
+++ b/drivers/staging/unisys/common-spar/include/channels/iochannel.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION */
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION */
 /* All rights reserved. */
 #ifndef __IOCHANNEL_H__
 #define __IOCHANNEL_H__
@@ -29,6 +29,8 @@
 *        CHANNEL_ATTACHED -> CHANNEL_OPENED (performed only by GuestPart)
 */
 
+#include <linux/uuid.h>
+
 #include "commontypes.h"
 #include "vmcallinterface.h"
 
@@ -192,7 +194,7 @@ typedef enum { NET_RCV_POST = 0,	/* submit buffer to hold receiving
 	/* uisnic -> virtnic */
 	NET_MACADDR,		/* indicates the client has requested to update
 				 * its MAC addr */
-	NET_MACADDR_ACK,	/* Mac addres  */
+	NET_MACADDR_ACK,	/* MAC address  */
 
 } NET_TYPES;
 
@@ -696,7 +698,7 @@ typedef struct _ULTRA_IO_CHANNEL_PROTOCOL {
 			U8 macaddr[MAX_MACADDR_LEN];	/* 6 bytes */
 			U32 num_rcv_bufs;	/* 4 */
 			U32 mtu;	/* 4 */
-			GUID zoneGuid;	/* 16 */
+			uuid_le zoneGuid;	/* 16 */
 		} vnic;		/* total     30 */
 	};
 
@@ -807,7 +809,7 @@ static inline int ULTRA_VHBA_init_channel(ULTRA_IO_CHANNEL_PROTOCOL *x,
 	x->ChannelHeader.HeaderSize = sizeof(x->ChannelHeader);
 	x->ChannelHeader.Size = COVER(bytes, 4096);
 	x->ChannelHeader.Type = UltraVhbaChannelProtocolGuid;
-	x->ChannelHeader.ZoneGuid = Guid0;
+	x->ChannelHeader.ZoneGuid = NULL_UUID_LE;
 	x->vhba.wwnn = *wwnn;
 	x->vhba.max = *max;
 	INIT_CLIENTSTRING(x, ULTRA_IO_CHANNEL_PROTOCOL, clientStr,
@@ -832,7 +834,7 @@ static inline void ULTRA_VHBA_set_max(ULTRA_IO_CHANNEL_PROTOCOL *x,
 static inline int ULTRA_VNIC_init_channel(ULTRA_IO_CHANNEL_PROTOCOL *x,
 						 unsigned char *macaddr,
 						 U32 num_rcv_bufs, U32 mtu,
-						 GUID zoneGuid,
+						 uuid_le zoneGuid,
 						 unsigned char *clientStr,
 						 U32 clientStrLen,
 						 U64 bytes)  {
@@ -843,7 +845,7 @@ static inline int ULTRA_VNIC_init_channel(ULTRA_IO_CHANNEL_PROTOCOL *x,
 	x->ChannelHeader.HeaderSize = sizeof(x->ChannelHeader);
 	x->ChannelHeader.Size = COVER(bytes, 4096);
 	x->ChannelHeader.Type = UltraVnicChannelProtocolGuid;
-	x->ChannelHeader.ZoneGuid = Guid0;
+	x->ChannelHeader.ZoneGuid = NULL_UUID_LE;
 	MEMCPY(x->vnic.macaddr, macaddr, MAX_MACADDR_LEN);
 	x->vnic.num_rcv_bufs = num_rcv_bufs;
 	x->vnic.mtu = mtu;
diff --git a/drivers/staging/unisys/common-spar/include/channels/vbuschannel.h b/drivers/staging/unisys/common-spar/include/channels/vbuschannel.h
index 99dbbcf3d11..0dd3e2dd0e1 100644
--- a/drivers/staging/unisys/common-spar/include/channels/vbuschannel.h
+++ b/drivers/staging/unisys/common-spar/include/channels/vbuschannel.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -23,15 +23,16 @@
  *  We currently use this for the client to provide various information about
  *  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"
 
 /* {193b331b-c58f-11da-95a9-00e08161165f} */
 #define ULTRA_VBUS_CHANNEL_PROTOCOL_GUID \
-	{0x193b331b, 0xc58f, 0x11da, \
-		{0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f} }
-static const GUID UltraVbusChannelProtocolGuid =
+		UUID_LE(0x193b331b, 0xc58f, 0x11da, \
+				0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f)
+static const uuid_le UltraVbusChannelProtocolGuid =
 	ULTRA_VBUS_CHANNEL_PROTOCOL_GUID;
 
 #define ULTRA_VBUS_CHANNEL_PROTOCOL_SIGNATURE ULTRA_CHANNEL_PROTOCOL_SIGNATURE
@@ -112,8 +113,7 @@ ULTRA_VBUS_init_channel(ULTRA_VBUS_CHANNEL_PROTOCOL __iomem *x,
 	writeq(bytesAllocated, &x->ChannelHeader.Size);
 	memcpy_toio(&x->ChannelHeader.Type, &UltraVbusChannelProtocolGuid,
 		    sizeof(x->ChannelHeader.Type));
-	memcpy_toio(&x->ChannelHeader.ZoneGuid, &Guid0,
-		    sizeof(x->ChannelHeader.ZoneGuid));
+	memcpy_toio(&x->ChannelHeader.ZoneGuid, &NULL_UUID_LE, sizeof(uuid_le));
 	writel(sizeof(ULTRA_VBUS_HEADERINFO), &x->HdrInfo.structBytes);
 	writel(sizeof(ULTRA_VBUS_HEADERINFO), &x->HdrInfo.chpInfoByteOffset);
 	writel(readl(&x->HdrInfo.chpInfoByteOffset) +
diff --git a/drivers/staging/unisys/common-spar/include/controlvmcompletionstatus.h b/drivers/staging/unisys/common-spar/include/controlvmcompletionstatus.h
index de30d321d98..db77d6f626a 100644
--- a/drivers/staging/unisys/common-spar/include/controlvmcompletionstatus.h
+++ b/drivers/staging/unisys/common-spar/include/controlvmcompletionstatus.h
@@ -1,6 +1,6 @@
 /* controlvmcompletionstatus.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All Rights Reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/common-spar/include/diagnostics/appos_subsystems.h b/drivers/staging/unisys/common-spar/include/diagnostics/appos_subsystems.h
index 4c6294d2060..18cc9ed2748 100644
--- a/drivers/staging/unisys/common-spar/include/diagnostics/appos_subsystems.h
+++ b/drivers/staging/unisys/common-spar/include/diagnostics/appos_subsystems.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/common-spar/include/iovmcall_gnuc.h b/drivers/staging/unisys/common-spar/include/iovmcall_gnuc.h
index 7304e9a0648..fe9598c941a 100644
--- a/drivers/staging/unisys/common-spar/include/iovmcall_gnuc.h
+++ b/drivers/staging/unisys/common-spar/include/iovmcall_gnuc.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/common-spar/include/vbusdeviceinfo.h b/drivers/staging/unisys/common-spar/include/vbusdeviceinfo.h
index ae708faaa94..8c0259a2cbc 100644
--- a/drivers/staging/unisys/common-spar/include/vbusdeviceinfo.h
+++ b/drivers/staging/unisys/common-spar/include/vbusdeviceinfo.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/common-spar/include/version.h b/drivers/staging/unisys/common-spar/include/version.h
index 00b0ebb09ea..f25208fc3ed 100644
--- a/drivers/staging/unisys/common-spar/include/version.h
+++ b/drivers/staging/unisys/common-spar/include/version.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/common-spar/include/vmcallinterface.h b/drivers/staging/unisys/common-spar/include/vmcallinterface.h
index 14c404367fa..c5c10f3f1eb 100644
--- a/drivers/staging/unisys/common-spar/include/vmcallinterface.h
+++ b/drivers/staging/unisys/common-spar/include/vmcallinterface.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/include/commontypes.h b/drivers/staging/unisys/include/commontypes.h
index ef12af4a72d..9de6f9dc5fb 100644
--- a/drivers/staging/unisys/include/commontypes.h
+++ b/drivers/staging/unisys/include/commontypes.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -17,13 +17,14 @@
 #define _COMMONTYPES_H_
 
 /* define the following to prevent include nesting in kernel header files of
- * similar abreviated content */
+ * similar abbreviated content */
 #define _SUPERVISOR_COMMONTYPES_H_
 
 #ifdef __KERNEL__
 #include <linux/types.h>
 #include <linux/version.h>
 #include <linux/io.h>
+#include <linux/uuid.h>
 #else
 #include <stdint.h>
 #include <syslog.h>
@@ -59,16 +60,6 @@
 
 #endif
 
-typedef struct {
-	U32 data1;
-	U16 data2;
-	U16 data3;
-	U8 data4[8];
-} __attribute__ ((__packed__)) GUID;
-
-#ifndef GUID0
-#define GUID0 {0, 0, 0, {0, 0, 0, 0, 0, 0, 0, 0} }
-#endif
 typedef U64 GUEST_PHYSICAL_ADDRESS;
 
 #define MEMSET(ptr, val, len) memset(ptr, val, len)
@@ -89,18 +80,16 @@ typedef U64 GUEST_PHYSICAL_ADDRESS;
 #define CHANNEL_GUID_MISMATCH(chType, chName, field, expected, actual, fil, \
 			      lin, logCtx)				\
 	do {								\
-		char s1[50], s2[50], s3[50];				\
-		pr_err("Channel mismatch on channel=%s(%s) field=%s expected=%s actual=%s @%s:%d\n", \
-		       chName, GUID_format2(&chType, s1), field,	\
-		       GUID_format2(&expected, s2), GUID_format2(&actual, s3), \
+		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 {								\
-		char s1[50];						\
-		pr_err("Channel mismatch on channel=%s(%s) field=%s expected=0x%-8.8lx actual=0x%-8.8lx @%s:%d\n", \
-		       chName, GUID_format2(&chType, s1), field,	\
+		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)
@@ -108,9 +97,8 @@ typedef U64 GUEST_PHYSICAL_ADDRESS;
 #define CHANNEL_U64_MISMATCH(chType, chName, field, expected, actual, fil, \
 			     lin, logCtx)				\
 	do {								\
-		char s1[50];						\
-		pr_err("Channel mismatch on channel=%s(%s) field=%s expected=0x%-8.8Lx actual=0x%-8.8Lx @%s:%d\n", \
-		       chName, GUID_format2(&chType, s1), field,	\
+		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);					\
@@ -128,21 +116,19 @@ typedef U64 GUEST_PHYSICAL_ADDRESS;
 #define CHANNEL_GUID_MISMATCH(chType, chName, field, expected, actual, fil, \
 			      lin, logCtx)				\
 	do {								\
-		char s1[50], s2[50], s3[50];				\
 		syslog(LOG_USER | LOG_ERR,				\
-		       "Channel mismatch on channel=%s(%s) field=%s expected=%s actual=%s @%s:%d", \
-		       chName, GUID_format2(&chType, s1), field,	\
-		       GUID_format2(&expected, s2), GUID_format2(&actual, s3), \
+		       "Channel mismatch on channel=%s(%pUL) field=%s expected=%pUL actual=%pUL @%s:%d", \
+		       chName, &chType, field,	\
+		       &expected, &actual, \
 		       fil, lin);					\
 	} while (0)
 
 #define CHANNEL_U32_MISMATCH(chType, chName, field, expected, actual, fil, \
 			     lin, logCtx)				\
 	do {								\
-		char s1[50];						\
 		syslog(LOG_USER | LOG_ERR,				\
-		       "Channel mismatch on channel=%s(%s) field=%s expected=0x%-8.8lx actual=0x%-8.8lx @%s:%d", \
-		       chName, GUID_format2(&chType, s1), field,	\
+		       "Channel mismatch on channel=%s(%pUL) field=%s expected=0x%-8.8lx actual=0x%-8.8lx @%s:%d", \
+		       chName, chType, field,	\
 		       (unsigned long)expected, (unsigned long)actual,	\
 		       fil, lin);					\
 	} while (0)
@@ -150,10 +136,9 @@ typedef U64 GUEST_PHYSICAL_ADDRESS;
 #define CHANNEL_U64_MISMATCH(chType, chName, field, expected, actual, fil, \
 			     lin, logCtx)				\
 	do {								\
-		char s1[50];						\
 		syslog(LOG_USER | LOG_ERR,				\
-		       "Channel mismatch on channel=%s(%s) field=%s expected=0x%-8.8Lx actual=0x%-8.8Lx @%s:%d", \
-		       chName, GUID_format2(&chType, s1), field,	\
+		       "Channel mismatch on channel=%s(%pUL) field=%s expected=0x%-8.8Lx actual=0x%-8.8Lx @%s:%d", \
+		       chName, chType, field,	\
 		       (unsigned long long)expected,			\
 		       (unsigned long long)actual,			\
 		       fil, lin);					\
@@ -167,4 +152,4 @@ typedef U64 GUEST_PHYSICAL_ADDRESS;
 #define VolatileBarrier() MEMORYBARRIER
 
 #endif
-#include "guidutils.h"
+
diff --git a/drivers/staging/unisys/include/guestlinuxdebug.h b/drivers/staging/unisys/include/guestlinuxdebug.h
index c3de8496e5d..efc4005368b 100644
--- a/drivers/staging/unisys/include/guestlinuxdebug.h
+++ b/drivers/staging/unisys/include/guestlinuxdebug.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -17,7 +17,7 @@
 #define __GUESTLINUXDEBUG_H__
 
 /*
-* This file contains supporting interface for "vmcallinterface.h", particuarly
+* This file contains supporting interface for "vmcallinterface.h", particularly
 * regarding adding additional structure and functionality to linux
 * ISSUE_IO_VMCALL_POSTCODE_SEVERITY */
 
@@ -142,7 +142,7 @@ typedef enum {			/* POSTCODE event identifier tuples */
  * Please also note that the resulting postcode is in hex, so if you are
  * searching for the __LINE__ number, convert it first to decimal.  The line
  * number combined with driver and type of call, will allow you to track down
- * exactly what line an error occured on, or where the last driver
+ * exactly what line an error occurred on, or where the last driver
  * entered/exited from.
  */
 
diff --git a/drivers/staging/unisys/include/guidutils.h b/drivers/staging/unisys/include/guidutils.h
deleted file mode 100644
index 75caf929cd6..00000000000
--- a/drivers/staging/unisys/include/guidutils.h
+++ /dev/null
@@ -1,203 +0,0 @@
-/* Copyright © 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.
- */
-
-/* guidutils.h
- *
- * These are GUID manipulation inlines that can be used from either
- * kernel-mode or user-mode.
- *
- */
-#ifndef __GUIDUTILS_H__
-#define __GUIDUTILS_H__
-
-#ifdef __KERNEL__
-#include <linux/kernel.h>
-#include <linux/string.h>
-#include <linux/ctype.h>
-#define GUID_STRTOUL kstrtoul
-#else
-#include <stdio.h>
-#include <ctype.h>
-#include <string.h>
-#include <stdlib.h>
-
-#define GUID_STRTOUL strtoul
-#endif
-
-static inline char *
-GUID_format1(const GUID *guid, char *s)
-{
-	sprintf(s, "{%-8.8lx-%-4.4x-%-4.4x-%-2.2x%-2.2x%-2.2x%-2.2x%-2.2x%-2.2x%-2.2x%-2.2x}",
-		(ulong) guid->data1,
-		guid->data2,
-		guid->data3,
-		guid->data4[0],
-		guid->data4[1],
-		guid->data4[2],
-		guid->data4[3],
-		guid->data4[4], guid->data4[5], guid->data4[6], guid->data4[7]);
-	return s;
-}
-
-/** Format a GUID in Microsoft's 'what in the world were they thinking'
- *  format.
- */
-static inline char *
-GUID_format2(const GUID *guid, char *s)
-{
-	sprintf(s, "{%-8.8lx-%-4.4x-%-4.4x-%-2.2x%-2.2x-%-2.2x%-2.2x%-2.2x%-2.2x%-2.2x%-2.2x}",
-		(ulong) guid->data1,
-		guid->data2,
-		guid->data3,
-		guid->data4[0],
-		guid->data4[1],
-		guid->data4[2],
-		guid->data4[3],
-		guid->data4[4], guid->data4[5], guid->data4[6], guid->data4[7]);
-	return s;
-}
-
-/**
- * Like GUID_format2 but without the curly braces and the
- * hex digits in upper case
- */
-static inline char *
-GUID_format3(const GUID *guid, char *s)
-{
-	sprintf(s, "%-8.8lX-%-4.4X-%-4.4X-%-2.2X%-2.2X-%-2.2X%-2.2X%-2.2X%-2.2X%-2.2X%-2.2X",
-		(ulong) guid->data1,
-		guid->data2,
-		guid->data3,
-		guid->data4[0],
-		guid->data4[1],
-		guid->data4[2],
-		guid->data4[3],
-		guid->data4[4], guid->data4[5], guid->data4[6], guid->data4[7]);
-	return s;
-}
-
-/** Parse a guid string in any of these forms:
- *      {11111111-2222-3333-4455-66778899aabb}
- *      {11111111-2222-3333-445566778899aabb}
- *      11111111-2222-3333-4455-66778899aabb
- *      11111111-2222-3333-445566778899aabb
- */
-static inline GUID
-GUID_scan(U8 *p)
-{
-	GUID guid = GUID0;
-	U8 x[33];
-	int count = 0;
-	int c, i = 0;
-	U8 cdata1[9];
-	U8 cdata2[5];
-	U8 cdata3[5];
-	U8 cdata4[3];
-	int dashcount = 0;
-	int brace = 0;
-	unsigned long uldata;
-
-	if (!p)
-		return guid;
-	if (*p == '{') {
-		p++;
-		brace = 1;
-	}
-	while (count < 32) {
-		if (*p == '}')
-			return guid;
-		if (*p == '\0')
-			return guid;
-		c = toupper(*p);
-		p++;
-		if (c == '-') {
-			switch (dashcount) {
-			case 0:
-				if (i != 8)
-					return guid;
-				break;
-			case 1:
-				if (i != 4)
-					return guid;
-				break;
-			case 2:
-				if (i != 4)
-					return guid;
-				break;
-			case 3:
-				if (i != 4)
-					return guid;
-				break;
-			default:
-				return guid;
-			}
-			dashcount++;
-			i = 0;
-			continue;
-		}
-		if ((c >= '0' && c <= '9') || (c >= 'A' && c <= 'F'))
-			i++;
-		else
-			return guid;
-		x[count++] = c;
-	}
-	x[count] = '\0';
-	if (brace) {
-		if (*p == '}')
-			p++;
-		else
-			return guid;
-	}
-	if (dashcount == 3 || dashcount == 4)
-		;
-	else
-		return guid;
-	memset(cdata1, 0, sizeof(cdata1));
-	memset(cdata2, 0, sizeof(cdata2));
-	memset(cdata3, 0, sizeof(cdata3));
-	memset(cdata4, 0, sizeof(cdata4));
-	memcpy(cdata1, x + 0, 8);
-	memcpy(cdata2, x + 8, 4);
-	memcpy(cdata3, x + 12, 4);
-
-	if (GUID_STRTOUL((char *) cdata1, 16, &uldata) == 0)
-		guid.data1 = (U32)uldata;
-	if (GUID_STRTOUL((char *) cdata2, 16, &uldata) == 0)
-		guid.data2 = (U16)uldata;
-	if (GUID_STRTOUL((char *) cdata3, 16, &uldata) == 0)
-		guid.data3 = (U16)uldata;
-
-	for (i = 0; i < 8; i++) {
-		memcpy(cdata4, x + 16 + (i * 2), 2);
-		if (GUID_STRTOUL((char *) cdata4, 16, &uldata) == 0)
-			guid.data4[i] = (U8) uldata;
-	}
-
-	return guid;
-}
-
-static inline char *
-GUID_sanitize(char *inputGuidStr, char *outputGuidStr)
-{
-	GUID g;
-	GUID guid0 = GUID0;
-	*outputGuidStr = '\0';
-	g = GUID_scan((U8 *) inputGuidStr);
-	if (memcmp(&g, &guid0, sizeof(GUID)) == 0)
-		return outputGuidStr;	/* bad GUID format */
-	return GUID_format1(&g, outputGuidStr);
-}
-
-#endif
diff --git a/drivers/staging/unisys/include/periodic_work.h b/drivers/staging/unisys/include/periodic_work.h
index 6c7190bdcd6..418ba634e1a 100644
--- a/drivers/staging/unisys/include/periodic_work.h
+++ b/drivers/staging/unisys/include/periodic_work.h
@@ -1,6 +1,6 @@
 /* periodic_work.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/include/procobjecttree.h b/drivers/staging/unisys/include/procobjecttree.h
index c81d11287e6..1174056ec3d 100644
--- a/drivers/staging/unisys/include/procobjecttree.h
+++ b/drivers/staging/unisys/include/procobjecttree.h
@@ -1,6 +1,6 @@
 /* procobjecttree.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/include/sparstop.h b/drivers/staging/unisys/include/sparstop.h
index 3603ac60764..05837399a74 100644
--- a/drivers/staging/unisys/include/sparstop.h
+++ b/drivers/staging/unisys/include/sparstop.h
@@ -1,6 +1,6 @@
 /* sparstop.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/include/timskmod.h b/drivers/staging/unisys/include/timskmod.h
index 5fd5ad51446..ecf1a6fac7a 100644
--- a/drivers/staging/unisys/include/timskmod.h
+++ b/drivers/staging/unisys/include/timskmod.h
@@ -1,6 +1,6 @@
 /* timskmod.h
  *
- * Copyright � 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/include/timskmodutils.h b/drivers/staging/unisys/include/timskmodutils.h
index 2d81d46bf11..c316c94ea19 100644
--- a/drivers/staging/unisys/include/timskmodutils.h
+++ b/drivers/staging/unisys/include/timskmodutils.h
@@ -1,6 +1,6 @@
 /* timskmodutils.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -72,4 +72,6 @@ char *cyclesToSomethingsPerSecond(u64 cycles, u64 cyclesPerSecond,
 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 6dab3900994..2a5bea3b332 100644
--- a/drivers/staging/unisys/include/uisqueue.h
+++ b/drivers/staging/unisys/include/uisqueue.h
@@ -1,6 +1,6 @@
 /* uisqueue.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -28,6 +28,7 @@
 #include "uniklog.h"
 #include <linux/atomic.h>
 #include <linux/semaphore.h>
+#include <linux/uuid.h>
 
 #include "controlvmchannel.h"
 #include "controlvmcompletionstatus.h"
@@ -136,8 +137,8 @@ struct device_info {
 	void __iomem *chanptr;
 	U64 channelAddr;
 	U64 channelBytes;
-	GUID channelTypeGuid;
-	GUID devInstGuid;
+	uuid_le channelTypeGuid;
+	uuid_le devInstGuid;
 	struct InterruptInfo intr;
 	struct switch_info *swtch;
 	char devid[30];		/* "vbus<busno>:dev<devno>" */
@@ -163,7 +164,7 @@ struct bus_info {
 	U32 busNo, deviceCount;
 	struct device_info **device;
 	U64 guestHandle, recvBusInterruptHandle;
-	GUID busInstGuid;
+	uuid_le busInstGuid;
 	ULTRA_VBUS_CHANNEL_PROTOCOL __iomem *pBusChannel;
 	int busChannelBytes;
 	struct proc_dir_entry *proc_dir;	/* proc/uislib/vbus/<x> */
@@ -356,8 +357,8 @@ struct add_vbus_guestpart {
 					 * NOT YET USED */
 	U32 busNo;		/* bus number to be created/deleted */
 	U32 deviceCount;	/* max num of devices on bus */
-	GUID busTypeGuid;	/* indicates type of bus */
-	GUID busInstGuid;	/* instance guid for device */
+	uuid_le busTypeGuid;	/* indicates type of bus */
+	uuid_le busInstGuid;	/* instance guid for device */
 };
 
 struct del_vbus_guestpart {
@@ -371,7 +372,7 @@ 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 */
-	GUID devInstGuid;	/* instance guid for device */
+	uuid_le devInstGuid;	/* 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.
diff --git a/drivers/staging/unisys/include/uisthread.h b/drivers/staging/unisys/include/uisthread.h
index 2b1fba75909..26837771b43 100644
--- a/drivers/staging/unisys/include/uisthread.h
+++ b/drivers/staging/unisys/include/uisthread.h
@@ -1,6 +1,6 @@
 /* uisthread.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/include/uisutils.h b/drivers/staging/unisys/include/uisutils.h
index 5fdab3a3464..70776c93cc5 100644
--- a/drivers/staging/unisys/include/uisutils.h
+++ b/drivers/staging/unisys/include/uisutils.h
@@ -1,6 +1,6 @@
 /* uisutils.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -25,6 +25,7 @@
 #include <linux/io.h>
 #include <linux/sched.h>
 #include <linux/gfp.h>
+#include <linux/uuid.h>
 
 #include "vmcallinterface.h"
 #include "channel.h"
@@ -54,7 +55,7 @@ extern atomic_t UisUtils_Registered_Services;
 
 typedef unsigned int MACARRAY[MAX_MACADDR_LEN];
 typedef struct ReqHandlerInfo_struct {
-	GUID switchTypeGuid;
+	uuid_le switchTypeGuid;
 	int (*controlfunc)(struct io_msgs *);
 	unsigned long min_channel_bytes;
 	int (*Server_Channel_Ok)(unsigned long channelBytes);
@@ -64,7 +65,7 @@ typedef struct ReqHandlerInfo_struct {
 	struct list_head list_link;	/* links into ReqHandlerInfo_list */
 } ReqHandlerInfo_t;
 
-ReqHandlerInfo_t *ReqHandlerAdd(GUID switchTypeGuid,
+ReqHandlerInfo_t *ReqHandlerAdd(uuid_le switchTypeGuid,
 				const char *switch_type_name,
 				int (*controlfunc)(struct io_msgs *),
 				unsigned long min_channel_bytes,
@@ -73,8 +74,8 @@ ReqHandlerInfo_t *ReqHandlerAdd(GUID switchTypeGuid,
 				int (*Server_Channel_Init)
 				 (void *x, unsigned char *clientStr,
 				  U32 clientStrLen, U64 bytes));
-ReqHandlerInfo_t *ReqHandlerFind(GUID switchTypeGuid);
-int ReqHandlerDel(GUID switchTypeGuid);
+ReqHandlerInfo_t *ReqHandlerFind(uuid_le switchTypeGuid);
+int ReqHandlerDel(uuid_le switchTypeGuid);
 
 #define uislib_ioremap_cache(addr, size) \
 	dbg_ioremap_cache(addr, size, __FILE__, __LINE__)
@@ -112,7 +113,7 @@ int uisutil_add_proc_line_ex(int *total, char **buffer, int *buffer_remaining,
 
 int uisctrl_register_req_handler(int type, void *fptr,
 				 ULTRA_VBUS_DEVICEINFO *chipset_DriverInfo);
-int uisctrl_register_req_handler_ex(GUID switchTypeGuid,
+int uisctrl_register_req_handler_ex(uuid_le switchTypeGuid,
 				    const char *switch_type_name,
 				    int (*fptr)(struct io_msgs *),
 				    unsigned long min_channel_bytes,
@@ -123,7 +124,7 @@ int uisctrl_register_req_handler_ex(GUID switchTypeGuid,
 				     U32 clientStrLen, U64 bytes),
 				    ULTRA_VBUS_DEVICEINFO *chipset_DriverInfo);
 
-int uisctrl_unregister_req_handler_ex(GUID switchTypeGuid);
+int uisctrl_unregister_req_handler_ex(uuid_le switchTypeGuid);
 unsigned char *util_map_virt(struct phys_info *sg);
 void util_unmap_virt(struct phys_info *sg);
 unsigned char *util_map_virt_atomic(struct phys_info *sg);
@@ -133,20 +134,20 @@ int uislib_server_inject_add_vnic(U32 switchNo, U32 BusNo, U32 numIntPorts,
 				  pCHANNEL_HEADER **chan);
 void uislib_server_inject_del_vnic(U32 switchNo, U32 busNo, U32 numIntPorts,
 				   U32 numExtPorts);
-int uislib_client_inject_add_bus(U32 busNo, GUID instGuid,
+int uislib_client_inject_add_bus(U32 busNo, uuid_le instGuid,
 				 U64 channelAddr, ulong nChannelBytes);
 int  uislib_client_inject_del_bus(U32 busNo);
 
 int uislib_client_inject_add_vhba(U32 busNo, U32 devNo,
 				  U64 phys_chan_addr, U32 chan_bytes,
-				  int is_test_addr, GUID instGuid,
+				  int is_test_addr, uuid_le instGuid,
 				  struct InterruptInfo *intr);
 int  uislib_client_inject_pause_vhba(U32 busNo, U32 devNo);
 int  uislib_client_inject_resume_vhba(U32 busNo, U32 devNo);
 int uislib_client_inject_del_vhba(U32 busNo, U32 devNo);
 int uislib_client_inject_add_vnic(U32 busNo, U32 devNo,
 				  U64 phys_chan_addr, U32 chan_bytes,
-				  int is_test_addr, GUID instGuid,
+				  int is_test_addr, uuid_le instGuid,
 				  struct InterruptInfo *intr);
 int uislib_client_inject_pause_vnic(U32 busNo, U32 devNo);
 int uislib_client_inject_resume_vnic(U32 busNo, U32 devNo);
@@ -193,14 +194,21 @@ struct chaninfo {
  * correctly at DEVICE_CREATE time, INSTEAD OF waiting until
  * DEVICE_CONFIGURE time.
  */
-#define WAIT_FOR_VALID_GUID(guid) \
-	do {						   \
-		while (MEMCMP_IO(&guid, &Guid0, sizeof(Guid0)) == 0) {	\
-			LOGERR("Waiting for non-0 GUID (why???)...\n"); \
-			UIS_THREAD_WAIT_SEC(5);				\
-		}							\
-		LOGERR("OK... GUID is non-0 now\n");			\
-	} while (0)
+static inline void
+wait_for_valid_guid(uuid_le __iomem *guid)
+{
+	uuid_le tmpguid;
+
+	while (1) {
+		memcpy_fromio((void *)&tmpguid,
+			      (void __iomem *)guid, sizeof(uuid_le));
+		if (uuid_le_cmp(tmpguid, NULL_UUID_LE) != 0)
+			break;
+		LOGERR("Waiting for non-0 GUID (why???)...\n");
+		UIS_THREAD_WAIT_SEC(5);
+	}
+	LOGERR("OK... GUID is non-0 now\n");
+}
 
 /* CopyFragsInfoFromSkb returns the number of entries added to frags array
  * Returns -1 on failure.
diff --git a/drivers/staging/unisys/include/uniklog.h b/drivers/staging/unisys/include/uniklog.h
index 4d7b87cefa6..6178cc46f7c 100644
--- a/drivers/staging/unisys/include/uniklog.h
+++ b/drivers/staging/unisys/include/uniklog.h
@@ -1,6 +1,6 @@
 /* uniklog.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/uislib/uislib.c b/drivers/staging/unisys/uislib/uislib.c
index 3152a2180c4..d4a7ef821ba 100644
--- a/drivers/staging/unisys/uislib/uislib.c
+++ b/drivers/staging/unisys/uislib/uislib.c
@@ -1,6 +1,6 @@
 /* uislib.c
  *
- * Copyright � 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -23,6 +23,7 @@
 #include <config/modversions.h>
 #endif
 #include <linux/module.h>
+#include <linux/debugfs.h>
 
 #include "commontypes.h"
 
@@ -45,7 +46,6 @@
 
 #define SET_PROC_OWNER(x, y)
 
-#define UISLIB_TEST_PROC
 #define POLLJIFFIES_NORMAL 1
 /* Choose whether or not you want to wakeup the request-polling thread
  * after an IO termination:
@@ -89,146 +89,34 @@ static DEFINE_SEMAPHORE(Lock_Polling_Device_Channels);	/* unlocked */
 static DECLARE_WAIT_QUEUE_HEAD(Wakeup_Polling_Device_Channels);
 static int Go_Polling_Device_Channels;
 
-static struct proc_dir_entry *uislib_proc_dir;
-static struct proc_dir_entry *uislib_proc_vbus_dir;
-static struct proc_dir_entry *vnic_proc_entry;	/* Used to be "datachan" */
-static struct proc_dir_entry *ctrlchan_proc_entry;
-static struct proc_dir_entry *pmem_proc_entry;
-static struct proc_dir_entry *info_proc_entry;
-static struct proc_dir_entry *switch_proc_entry;
-static struct proc_dir_entry *extport_proc_entry;
-static struct proc_dir_entry *platformnumber_proc_entry;
-static struct proc_dir_entry *bus_proc_entry;
-static struct proc_dir_entry *dev_proc_entry;
-static struct proc_dir_entry *chipset_proc_entry;
-static struct proc_dir_entry *cycles_before_wait_proc_entry;
-static struct proc_dir_entry *reset_counts_proc_entry;
-static struct proc_dir_entry *smart_wakeup_proc_entry;
-static struct proc_dir_entry *disable_proc_entry;
-
-#define DIR_PROC_ENTRY "uislib"
-#define DIR_VBUS_PROC_ENTRY "vbus"
-#define VNIC_PROC_ENTRY_FN "vnic"	/* Used to be "datachan" */
-#define CTRLCHAN_PROC_ENTRY_FN "ctrlchan"
-#define PMEM_PROC_ENTRY_FN "phys_to_virt"
-#define INFO_PROC_ENTRY_FN "info"
-#define SWITCH_PROC_ENTRY_FN "switch"
-#define SWITCH_COUNT_PROC_ENTRY_FN "switch_count"
-#define EXTPORT_PROC_ENTRY_FN "extport"
-#define PLATFORMNUMBER_PROC_ENTRY_FN "platform"
-#define BUS_PROC_ENTRY_FN "bus"
-#define DEV_PROC_ENTRY_FN "device"
-#define CHIPSET_PROC_ENTRY_FN "chipset"
-#define CYCLES_BEFORE_WAIT_PROC_ENTRY_FN "cycles_before_wait"
-#define RESET_COUNTS_PROC_ENTRY_FN "reset_counts"
-#define SMART_WAKEUP_PROC_ENTRY_FN "smart_wakeup"
 #define CALLHOME_PROC_ENTRY_FN "callhome"
 #define CALLHOME_THROTTLED_PROC_ENTRY_FN "callhome_throttled"
-#define DISABLE_PROC_ENTRY_FN "switch_state"
-#ifdef UISLIB_TEST_PROC
-static struct proc_dir_entry *test_proc_entry;
-#define TEST_PROC_ENTRY_FN "test"
-#endif
-static unsigned long long cycles_before_wait, wait_cycles;
 
-/*****************************************************/
-/* local functions                                   */
-/*****************************************************/
+#define DIR_DEBUGFS_ENTRY "uislib"
+static struct dentry *dir_debugfs;
 
-static int proc_info_vbus_show(struct seq_file *m, void *v);
-static int
-proc_info_vbus_open(struct inode *inode, struct file *filp)
-{
-	/* proc_info_vbus_show will grab this from seq_file.private: */
-	struct bus_info *bus = PDE_DATA(inode);
-	return single_open(filp, proc_info_vbus_show, bus);
-}
+#define PLATFORMNUMBER_DEBUGFS_ENTRY_FN "platform"
+static struct dentry *platformnumber_debugfs_read;
 
-static const struct file_operations proc_info_vbus_fops = {
-	.open = proc_info_vbus_open,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-static ssize_t uislib_proc_read_writeonly(struct file *file,
-					  char __user *buffer,
-					  size_t count, loff_t *ppos);
+#define CYCLES_BEFORE_WAIT_DEBUGFS_ENTRY_FN "cycles_before_wait"
+static struct dentry *cycles_before_wait_debugfs_read;
 
-static ssize_t vnic_proc_write(struct file *file, const char __user *buffer,
-			       size_t count, loff_t *ppos);
+#define SMART_WAKEUP_DEBUGFS_ENTRY_FN "smart_wakeup"
+static struct dentry *smart_wakeup_debugfs_entry;
 
-static const struct file_operations proc_vnic_fops = {
-	.read = uislib_proc_read_writeonly,
-	.write = vnic_proc_write,
-};
+#define INFO_DEBUGFS_ENTRY_FN "info"
+static struct dentry *info_debugfs_entry;
 
-static ssize_t chipset_proc_write(struct file *file, const char __user *buffer,
-				  size_t count, loff_t *ppos);
+static unsigned long long cycles_before_wait, wait_cycles;
 
-static const struct file_operations proc_chipset_fops = {
-	.read = uislib_proc_read_writeonly,
-	.write = chipset_proc_write,
-};
+/*****************************************************/
+/* local functions                                   */
+/*****************************************************/
 
-static ssize_t info_proc_read(struct file *file, char __user *buf,
+static ssize_t info_debugfs_read(struct file *file, char __user *buf,
 			      size_t len, loff_t *offset);
-static const struct file_operations proc_info_fops = {
-	.read = info_proc_read,
-};
-
-static ssize_t platformnumber_proc_read(struct file *file, char __user *buf,
-					size_t len, loff_t *offset);
-static const struct file_operations proc_platformnumber_fops = {
-	.read = platformnumber_proc_read,
-};
-
-static ssize_t cycles_before_wait_proc_write(struct file *file,
-					     const char __user *buffer,
-					     size_t count, loff_t *ppos);
-static const struct file_operations proc_cycles_before_wait_fops = {
-	.read = uislib_proc_read_writeonly,
-	.write = cycles_before_wait_proc_write,
-};
-
-static ssize_t reset_counts_proc_write(struct file *file,
-				       const char __user *buffer,
-				       size_t count, loff_t *ppos);
-static const struct file_operations proc_reset_counts_fops = {
-	.read = uislib_proc_read_writeonly,
-	.write = reset_counts_proc_write,
-};
-
-static ssize_t smart_wakeup_proc_write(struct file *file,
-				       const char __user *buffer,
-				       size_t count, loff_t *ppos);
-static const struct file_operations proc_smart_wakeup_fops = {
-	.read = uislib_proc_read_writeonly,
-	.write = smart_wakeup_proc_write,
-};
-
-static ssize_t test_proc_write(struct file *file,
-			       const char __user *buffer,
-			       size_t count, loff_t *ppos);
-static const struct file_operations proc_test_fops = {
-	.read = uislib_proc_read_writeonly,
-	.write = test_proc_write,
-};
-
-static ssize_t bus_proc_write(struct file *file,
-			      const char __user *buffer,
-			      size_t count, loff_t *ppos);
-static const struct file_operations proc_bus_fops = {
-	.read = uislib_proc_read_writeonly,
-	.write = bus_proc_write,
-};
-
-static ssize_t dev_proc_write(struct file *file,
-			      const char __user *buffer,
-			      size_t count, loff_t *ppos);
-static const struct file_operations proc_dev_fops = {
-	.read = uislib_proc_read_writeonly,
-	.write = dev_proc_write,
+static const struct file_operations debugfs_info_fops = {
+	.read = info_debugfs_read,
 };
 
 static void
@@ -240,31 +128,10 @@ init_msg_header(CONTROLVM_MESSAGE *msg, U32 id, uint rsp, uint svr)
 	msg->hdr.Flags.server = svr;
 }
 
-static void
-create_bus_proc_entries(struct bus_info *bus)
-{
-	bus->proc_dir = proc_mkdir(bus->name, uislib_proc_vbus_dir);
-	if (!bus->proc_dir) {
-		LOGERR("failed to create /proc/uislib/vbus/%s directory",
-		       bus->name);
-		return;
-	}
-	bus->proc_info = proc_create_data("info", 0, bus->proc_dir,
-					  &proc_info_vbus_fops, bus);
-	if (!bus->proc_info) {
-		LOGERR("failed to create /proc/uislib/vbus/%s/info", bus->name);
-		remove_proc_entry(bus->name, uislib_proc_vbus_dir);
-		bus->proc_dir = NULL;
-		return;
-	}
-	SET_PROC_OWNER(bus->proc_info, THIS_MODULE);
-
-}
-
 static __iomem void *
 init_vbus_channel(U64 channelAddr, U32 channelBytes, int isServer)
 {
-	void *rc = NULL;
+	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",
@@ -396,7 +263,6 @@ create_bus(CONTROLVM_MESSAGE *msg, char *buf)
 			    CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
 		}
 	}
-	create_bus_proc_entries(bus);
 
 	/* add bus at the head of our list */
 	write_lock(&BusListLock);
@@ -473,14 +339,6 @@ destroy_bus(CONTROLVM_MESSAGE *msg, char *buf)
 		read_unlock(&BusListLock);
 		return CONTROLVM_RESP_ERROR_ALREADY_DONE;
 	}
-	if (bus->proc_info) {
-		remove_proc_entry("info", bus->proc_dir);
-		bus->proc_info = NULL;
-	}
-	if (bus->proc_dir) {
-		remove_proc_entry(bus->name, uislib_proc_vbus_dir);
-		bus->proc_dir = NULL;
-	}
 	if (bus->pBusChannel) {
 		uislib_iounmap(bus->pBusChannel);
 		bus->pBusChannel = NULL;
@@ -587,11 +445,9 @@ create_device(CONTROLVM_MESSAGE *msg, char *buf)
 			 */
 			if (!msg->hdr.Flags.server) {
 				struct guest_msgs cmd;
-				if (!memcmp
-				    (&dev->channelTypeGuid,
-				     &UltraVhbaChannelProtocolGuid,
-				     sizeof(GUID))) {
-					WAIT_FOR_VALID_GUID(((CHANNEL_HEADER
+				if (!uuid_le_cmp(dev->channelTypeGuid,
+				     UltraVhbaChannelProtocolGuid)) {
+					wait_for_valid_guid(&((CHANNEL_HEADER
 							      __iomem *) (dev->
 								  chanptr))->
 							    Type);
@@ -614,11 +470,9 @@ create_device(CONTROLVM_MESSAGE *msg, char *buf)
 					    dev->devInstGuid;
 					cmd.add_vhba.intr = dev->intr;
 				} else
-				    if (!memcmp
-					(&dev->channelTypeGuid,
-					 &UltraVnicChannelProtocolGuid,
-					 sizeof(GUID))) {
-					WAIT_FOR_VALID_GUID(((CHANNEL_HEADER
+				    if (!uuid_le_cmp(dev->channelTypeGuid,
+					 UltraVnicChannelProtocolGuid)) {
+					wait_for_valid_guid(&((CHANNEL_HEADER
 							      __iomem *) (dev->
 								  chanptr))->
 							    Type);
@@ -723,16 +577,13 @@ pause_device(CONTROLVM_MESSAGE *msg)
 			/* the msg is bound for virtpci; send
 			 * guest_msgs struct to callback
 			 */
-			if (!memcmp
-			    (&dev->channelTypeGuid,
-			     &UltraVhbaChannelProtocolGuid, sizeof(GUID))) {
+			if (!uuid_le_cmp(dev->channelTypeGuid,
+					UltraVhbaChannelProtocolGuid)) {
 				cmd.msgtype = GUEST_PAUSE_VHBA;
 				cmd.pause_vhba.chanptr = dev->chanptr;
 			} else
-			    if (!memcmp
-				(&dev->channelTypeGuid,
-				 &UltraVnicChannelProtocolGuid,
-				 sizeof(GUID))) {
+			    if (!uuid_le_cmp(dev->channelTypeGuid,
+					    UltraVnicChannelProtocolGuid)) {
 				cmd.msgtype = GUEST_PAUSE_VNIC;
 				cmd.pause_vnic.chanptr = dev->chanptr;
 			} else {
@@ -798,15 +649,13 @@ resume_device(CONTROLVM_MESSAGE *msg)
 			/* the msg is bound for virtpci; send
 			 * guest_msgs struct to callback
 			 */
-			if (!memcmp(&dev->channelTypeGuid,
-				    &UltraVhbaChannelProtocolGuid,
-				    sizeof(GUID))) {
+			if (!uuid_le_cmp(dev->channelTypeGuid,
+					UltraVhbaChannelProtocolGuid)) {
 				cmd.msgtype = GUEST_RESUME_VHBA;
 				cmd.resume_vhba.chanptr = dev->chanptr;
 			} else
-			    if (!memcmp(&dev->channelTypeGuid,
-					&UltraVnicChannelProtocolGuid,
-					sizeof(GUID))) {
+			    if (!uuid_le_cmp(dev->channelTypeGuid,
+					    UltraVnicChannelProtocolGuid)) {
 				cmd.msgtype = GUEST_RESUME_VNIC;
 				cmd.resume_vnic.chanptr = dev->chanptr;
 			} else {
@@ -873,16 +722,13 @@ destroy_device(CONTROLVM_MESSAGE *msg, char *buf)
 			/* the msg is bound for virtpci; send
 			 * guest_msgs struct to callback
 			 */
-			if (!memcmp
-			    (&dev->channelTypeGuid,
-			     &UltraVhbaChannelProtocolGuid, sizeof(GUID))) {
+			if (!uuid_le_cmp(dev->channelTypeGuid,
+					UltraVhbaChannelProtocolGuid)) {
 				cmd.msgtype = GUEST_DEL_VHBA;
 				cmd.del_vhba.chanptr = dev->chanptr;
 			} else
-			    if (!memcmp
-				(&dev->channelTypeGuid,
-				 &UltraVnicChannelProtocolGuid,
-				 sizeof(GUID))) {
+			    if (!uuid_le_cmp(dev->channelTypeGuid,
+					    UltraVnicChannelProtocolGuid)) {
 				cmd.msgtype = GUEST_DEL_VNIC;
 				cmd.del_vnic.chanptr = dev->chanptr;
 			} else {
@@ -957,27 +803,6 @@ init_chipset(CONTROLVM_MESSAGE *msg, char *buf)
 }
 
 static int
-stop_chipset(CONTROLVM_MESSAGE *msg, char *buf)
-{
-	/* Check that all buses and switches have been torn down and
-	 * destroyed.
-	 */
-	if (BusListHead) {
-		/* Buses still exist. */
-		LOGERR("CONTROLVM_CHIPSET_STOP: BusListHead is not NULL");
-		return CONTROLVM_RESP_ERROR_CHIPSET_STOP_FAILED_BUS;
-	}
-	if (BusListCount) {
-		/* BusListHead is NULL, but BusListCount != 0 */
-		LOGERR("CONTROLVM_CHIPSET_STOP: BusListCount != 0");
-		return CONTROLVM_RESP_ERROR_CHIPSET_STOP_FAILED_BUS;
-	}
-
-	/* Buses are shut down. */
-	return visorchipset_chipset_notready();
-}
-
-static int
 delete_bus_glue(U32 busNo)
 {
 	CONTROLVM_MESSAGE msg;
@@ -1008,7 +833,7 @@ delete_device_glue(U32 busNo, U32 devNo)
 }
 
 int
-uislib_client_inject_add_bus(U32 busNo, GUID instGuid,
+uislib_client_inject_add_bus(U32 busNo, uuid_le instGuid,
 			     U64 channelAddr, ulong nChannelBytes)
 {
 	CONTROLVM_MESSAGE msg;
@@ -1109,7 +934,7 @@ EXPORT_SYMBOL_GPL(uislib_client_inject_resume_vhba);
 int
 uislib_client_inject_add_vhba(U32 busNo, U32 devNo,
 			      U64 phys_chan_addr, U32 chan_bytes,
-			      int is_test_addr, GUID instGuid,
+			      int is_test_addr, uuid_le instGuid,
 			      struct InterruptInfo *intr)
 {
 	CONTROLVM_MESSAGE msg;
@@ -1168,7 +993,7 @@ EXPORT_SYMBOL_GPL(uislib_client_inject_del_vhba);
 int
 uislib_client_inject_add_vnic(U32 busNo, U32 devNo,
 			      U64 phys_chan_addr, U32 chan_bytes,
-			      int is_test_addr, GUID instGuid,
+			      int is_test_addr, uuid_le instGuid,
 			      struct InterruptInfo *intr)
 {
 	CONTROLVM_MESSAGE msg;
@@ -1272,7 +1097,6 @@ uislib_client_add_vnic(U32 busNo)
 	BOOL busCreated = FALSE;
 	int devNo = 0;		/* Default to 0, since only one device
 				 * will be created for this bus... */
-	GUID dummyGuid = GUID0;
 	CONTROLVM_MESSAGE msg;
 
 	init_msg_header(&msg, CONTROLVM_BUS_CREATE, 0, 0);
@@ -1291,7 +1115,7 @@ uislib_client_add_vnic(U32 busNo)
 	msg.hdr.Flags.testMessage = 1;
 	msg.cmd.createDevice.busNo = busNo;
 	msg.cmd.createDevice.devNo = devNo;
-	msg.cmd.createDevice.devInstGuid = dummyGuid;
+	msg.cmd.createDevice.devInstGuid = NULL_UUID_LE;
 	memset(&msg.cmd.createDevice.intr, 0, sizeof(struct InterruptInfo));
 	msg.cmd.createDevice.channelAddr = PhysicalDataChan;
 	msg.cmd.createDevice.channelBytes = MIN_IO_CHANNEL_SIZE;
@@ -1376,135 +1200,11 @@ EXPORT_SYMBOL_GPL(uislib_cache_free);
 /* proc filesystem callback functions                */
 /*****************************************************/
 
-static ssize_t
-vnic_proc_write(struct file *file, const char __user *buffer,
-		size_t count, loff_t *ppos)
-{
-	int action = 0xffff, busNo = 0, i, result = 0;
-	char buf[4];
-	char direction;
-/* GUID guid; */
-	if (count >= ARRAY_SIZE(buf))
-		return -EINVAL;
-
-	if (copy_from_user(buf, buffer, count)) {
-		LOGERR("echo > /proc/uislib/vnic copy_from_user ****FAILED.\n");
-		return -EFAULT;
-	}
-
-	i = sscanf(buf, "%d%c", &action, &direction);
-	if (i != 2) {
-		LOGERR("unable to parse vnic proc parameters.\n");
-		return -EFAULT;
-	}
-
-	if ((direction != '-') && (direction != '+')) {
-		LOGERR("unable to determine whether to add or delete vnic\n");
-		return -EFAULT;
-	}
-
-	/* if (i < 1), i.e., if we didn't even read the action field,
-	* then action will default to 0xffff and the code below will
-	* fall through the switch and print usage.
-	*/
-	switch (action) {
-	case 0:
-		/* call client method... */
-		busNo = 0;	/* All client drivers use bus value of 0... */
-		if (direction == '+')
-			result = uislib_client_add_vnic(busNo);
-		else
-			result = uislib_client_delete_vnic(busNo);
-		if (!result) {
-			LOGERR("echo 0%c > /proc/uislib/vnic failed (client end)",
-			     direction);
-			return -EFAULT;
-		}
-		return count;
-
-	default:
-		break;
-	}
-
-	LOGERR("USAGE: echo <action><direction (up/down)> > /proc/uislib/vnic");
-	LOGERR(" ");
-	LOGERR("Client Syntax");
-	LOGERR("-------------");
-	LOGERR("0+    ==> add vnic");
-	LOGERR("0-    ==> delete vnic");
-	LOGERR(" ");
-	return count;
-}				/* end vnic_proc_write */
-
-static ssize_t
-chipset_proc_write(struct file *file, const char __user *buffer,
-		   size_t count, loff_t *ppos)
-{
-	int i, action = 0xffff;
-	char buf[4];
-	CONTROLVM_MESSAGE msg;
-
-	if (count >= ARRAY_SIZE(buf))
-		return -EINVAL;
-
-	memset(&msg, 0, sizeof(CONTROLVM_MESSAGE));
-
-	if (copy_from_user(buf, buffer, count)) {
-		LOGERR("copy_from_user ****FAILED.\n");
-		return -EFAULT;
-	}
-
-	if (chipset_inited) {
-		LOGINF("Chipset already initialized\n");
-		return -EFAULT;
-	}
-	i = sscanf(buf, "%x", &action);
-
-	/* if (i < 1), i.e., if we didn't even read the action field,
-	* then action will default to 0xffff and the code below will
-	* fall through the switch and print usage.
-	*/
-	switch (action) {
-	case 1:
-		/* GUEST */
-		/* step: initialize the chipset */
-		init_msg_header(&msg, CONTROLVM_CHIPSET_INIT, 0, 0);
-		msg.hdr.Flags.testMessage = 0;
-		msg.cmd.initChipset.busCount = 23;
-		msg.cmd.initChipset.switchCount = 23;
-
-		if (init_chipset(&msg, NULL) != CONTROLVM_RESP_SUCCESS) {
-			LOGERR("init_chipset failed.\n");
-			return 0;
-		}
-		return 1;
-	case 2:
-		/* BOTH */
-		init_msg_header(&msg, CONTROLVM_CHIPSET_INIT, 0, 0);
-		msg.hdr.Flags.testMessage = 1;
-		msg.cmd.initChipset.busCount = 23;
-		msg.cmd.initChipset.switchCount = 23;
-
-		if (init_chipset(&msg, NULL) != CONTROLVM_RESP_SUCCESS) {
-			LOGERR("init_chipset failed.\n");
-			return 0;
-		}
-		return 1;
-
-	default:
-		break;
-	}
-
-	LOGERR("usage: 1 ==> init_chipset client\n");
-	LOGERR("usage: 2 ==> init_chipset test\n");
-	return -EFAULT;
-}
-
 #define PLINE(...) uisutil_add_proc_line_ex(&tot, buff, \
 					       buff_len, __VA_ARGS__)
 
 static int
-info_proc_read_helper(char **buff, int *buff_len)
+info_debugfs_read_helper(char **buff, int *buff_len)
 {
 	int i, tot = 0;
 	struct bus_info *bus;
@@ -1564,7 +1264,8 @@ err_done:
 }
 
 static ssize_t
-info_proc_read(struct file *file, char __user *buf, size_t len, loff_t *offset)
+info_debugfs_read(struct file *file, char __user *buf,
+		size_t len, loff_t *offset)
 {
 	char *temp;
 	int totalBytes = 0;
@@ -1584,9 +1285,9 @@ info_proc_read(struct file *file, char __user *buf, size_t len, loff_t *offset)
 	temp = ProcReadBuffer;
 
 	if ((*offset == 0) || (!ProcReadBufferValid)) {
-		DBGINF("calling info_proc_read_helper.\n");
+		DBGINF("calling info_debugfs_read_helper.\n");
 		/* if the read fails, then -1 will be returned */
-		totalBytes = info_proc_read_helper(&temp, &remaining_bytes);
+		totalBytes = info_debugfs_read_helper(&temp, &remaining_bytes);
 		ProcReadBufferValid = 1;
 	} else
 		totalBytes = strlen(ProcReadBuffer);
@@ -1595,432 +1296,6 @@ info_proc_read(struct file *file, char __user *buf, size_t len, loff_t *offset)
 				       ProcReadBuffer, totalBytes);
 }
 
-static ssize_t
-platformnumber_proc_read(struct file *file, char __user *buf,
-			 size_t len, loff_t *offset)
-{
-	int length = 0;
-	char *vbuf;
-	loff_t pos = *offset;
-
-	if (pos < 0)
-		return -EINVAL;
-
-	if (pos > 0 || !len)
-		return 0;
-
-	vbuf = kzalloc(len, GFP_KERNEL);
-	if (!vbuf)
-		return -ENOMEM;
-
-	length = sprintf(vbuf, "%d\n", PlatformNumber);
-
-	if (copy_to_user(buf, vbuf, length)) {
-		kfree(vbuf);
-		return -EFAULT;
-	}
-
-	kfree(vbuf);
-	*offset += length;
-	return length;
-}
-
-#ifdef UISLIB_TEST_PROC
-
-/* proc/uislib/vbus/<x>/info */
-static int
-proc_info_vbus_show(struct seq_file *m, void *v)
-{
-	struct bus_info *bus = m->private;
-	int i, devInfoCount, x;
-	char buf[999];
-
-	if (bus == NULL)
-		return 0;
-	seq_printf(m, "Client device / client driver info for %s partition (vbus #%d):\n",
-		   bus->partitionName, bus->busNo);
-	if ((bus->busChannelBytes == 0) || (bus->pBusChannel == NULL))
-		return 0;
-	devInfoCount =
-	    (bus->busChannelBytes -
-	     sizeof(ULTRA_VBUS_CHANNEL_PROTOCOL)) /
-	    sizeof(ULTRA_VBUS_DEVICEINFO);
-	x = VBUSCHANNEL_devInfoToStringBuffer(&bus->pBusChannel->ChpInfo, buf,
-					      sizeof(buf) - 1, -1);
-	buf[x] = '\0';
-	seq_printf(m, "%s", buf);
-	x = VBUSCHANNEL_devInfoToStringBuffer(&bus->pBusChannel->BusInfo,
-					      buf, sizeof(buf) - 1, -1);
-	buf[x] = '\0';
-	seq_printf(m, "%s", buf);
-	for (i = 0; i < devInfoCount; i++) {
-		x = VBUSCHANNEL_devInfoToStringBuffer(&bus->pBusChannel->
-						      DevInfo[i], buf,
-						      sizeof(buf) - 1, i);
-		if (x > 0) {
-			buf[x] = '\0';
-			seq_printf(m, "%s", buf);
-		}
-	}
-	return 0;
-}
-
-static ssize_t
-bus_proc_write(struct file *file, const char __user *buffer,
-	       size_t count, loff_t *ppos)
-{
-	int server_flag = 0;
-	int i, action = 0xffff, result;
-	char buf[16];
-	CONTROLVM_MESSAGE msg;
-	U32 busNo, deviceCount;
-
-	if (count >= ARRAY_SIZE(buf))
-		return -EINVAL;
-
-	memset(&msg, 0, sizeof(CONTROLVM_MESSAGE));
-
-	if (copy_from_user(buf, buffer, count)) {
-		LOGERR("echo > /proc/uislib/bus: copy_from_user ****FAILED.");
-		return -EFAULT;
-	}
-
-	i = sscanf(buf, "%x-%d-%d", &action, &busNo, &deviceCount);
-
-	/* if (i < 1), i.e., if we didn't even read the action field,
-	* then action will default to 0xffff and the code below will
-	* fall through the switch and print usage.
-	*/
-	switch (action) {
-	case 0:
-		/* destroy a bus */
-		if (i != 2)
-			break;
-		init_msg_header(&msg, CONTROLVM_BUS_DESTROY, 0, server_flag);
-		msg.cmd.destroyBus.busNo = busNo;
-
-		result = destroy_bus(&msg, NULL);
-
-		if (result != CONTROLVM_RESP_SUCCESS) {
-			LOGERR("echo 0-%d > /proc/uislib/bus {CONTROLVM_BUS_DESTROY Failed} Result(%d)",
-			     busNo, result);
-			return -EFAULT;
-		}
-		return count;
-	case 1:
-		/* create a bus */
-		if (i != 3)
-			break;
-		init_msg_header(&msg, CONTROLVM_BUS_CREATE, 0, server_flag);
-		msg.cmd.createBus.busNo = busNo;
-		msg.cmd.createBus.deviceCount = deviceCount;
-
-		result = create_bus(&msg, NULL);
-
-		if (result != CONTROLVM_RESP_SUCCESS) {
-			LOGERR("echo 1-%d-%d > /proc/uislib/bus {CONTROLVM_BUS_CREATE Failed} Result(%d)",
-			     busNo, deviceCount, result);
-			return -EFAULT;
-		}
-
-		return count;
-	default:
-		break;
-	}
-
-	LOGERR("USAGE: echo <action>-<busNo>... > /proc/uislib/bus");
-	LOGERR(" ");
-	LOGERR("Destruct Syntax     ControlVM Message Id");
-	LOGERR("---------------     ---------------------");
-	LOGERR("0-<busNo>       ==> CONTROLVM_BUS_DESTROY");
-	LOGERR(" ");
-	LOGERR("Construct Syntax            ControlVM Message Id");
-	LOGERR("-----------------------     -------------------- ");
-	LOGERR("1-<busNo>-<deviceCount> ==> CONTROLVM_BUS_CREATE");
-
-	return -EFAULT;
-}
-
-static ssize_t
-uislib_proc_read_writeonly(struct file *file, char __user *buffer,
-	       size_t count, loff_t *ppos)
-{
-	return 0;
-}
-
-static ssize_t
-dev_proc_write(struct file *file, const char __user *buffer,
-	       size_t count, loff_t *ppos)
-{
-	int server_flag = 0;
-	CONTROLVM_MESSAGE msg;
-	U32 busNo, devNo;
-	char buf[32];
-	unsigned int chanptr;
-	int type, i, action = 0xffff, result;
-
-	if (count >= ARRAY_SIZE(buf))
-		return -EINVAL;
-
-	if (copy_from_user(buf, buffer, count)) {
-		LOGERR("echo > /proc/uislib/device: copy_from_user ****FAILED.");
-		return -EFAULT;
-	}
-
-	i = sscanf(buf, "%x-%d-%d-%x-%d",
-		   &action, &busNo, &devNo, &chanptr, &type);
-
-	switch (action) {
-	case 0:
-		if (i != 3)
-			break;
-
-		/* destroy a device */
-		init_msg_header(&msg, CONTROLVM_DEVICE_DESTROY, 0, server_flag);
-		msg.cmd.destroyDevice.busNo = busNo;
-		msg.cmd.destroyDevice.devNo = devNo;
-
-		result = destroy_device(&msg, NULL);
-
-		if (result != CONTROLVM_RESP_SUCCESS) {
-			LOGERR("echo 0-%d-%d > /proc/uislib/device {CONTROLVM_DEVICE_DESTROY Failed} Result(%d)",
-			     busNo, devNo, result);
-			return -EFAULT;
-		}
-
-		return count;
-
-	case 1:
-		if (i != 5)
-			break;
-
-		/* create a device */
-		init_msg_header(&msg, CONTROLVM_DEVICE_CREATE, 0, server_flag);
-		msg.cmd.createDevice.busNo = busNo;
-		msg.cmd.createDevice.devNo = devNo;
-		msg.cmd.createDevice.channelAddr = __pa(chanptr);
-		msg.cmd.createDevice.channelBytes = MIN_IO_CHANNEL_SIZE;
-
-		if (type == 0)
-			msg.cmd.createDevice.dataTypeGuid =
-			    UltraVhbaChannelProtocolGuid;
-		else if (type == 1)
-			msg.cmd.createDevice.dataTypeGuid =
-			    UltraVnicChannelProtocolGuid;
-		else {
-			LOGERR("echo 1-%d-%d-%x-<type> > /proc/uislib/devce failed: invalid device type %d.",
-			     busNo, devNo, chanptr, type);
-			return -EFAULT;
-		}
-
-		result = create_device(&msg, NULL);
-
-		if (result != CONTROLVM_RESP_SUCCESS) {
-			if (type == 0)
-				LOGERR("echo 1-%d-%d-%x-0 > /proc/uislib/device {CONTROLVM_DEVICE_CREATE[vHBA] Failed} Result(%d)",
-				     busNo, devNo, chanptr, result);
-			else
-				LOGERR("echo 1-%d-%d-%x-1 > /proc/uislib/device {CONTROLVM_DEVICE_CREATE[vNIC] Failed} Result(%d)",
-				     busNo, devNo, chanptr, result);
-			return -EFAULT;
-		}
-
-	default:
-		break;
-	}
-
-	LOGERR("USAGE: echo <action>-<busNo>-<devNo>... > /proc/uislib/device");
-	LOGERR(" ");
-	LOGERR("Destruct Syntax       ControlVM Message Id");
-	LOGERR("-----------------     ------------------------");
-	LOGERR("0-<busNo>-<devNo> ==> CONTROLVM_DEVICE_DESTROY");
-	LOGERR(" ");
-	LOGERR("Construct Syntax                       ControlVM Message Id");
-	LOGERR
-	    ("----------------------------------     ----------------------- ");
-	LOGERR
-	    ("1-<busNo>-<devNo>-<chanptr>-<type> ==> CONTROLVM_DEVICE_CREATE");
-	LOGERR("      <type = 0>: vHBA");
-	LOGERR("      <type = 1>: vNIC");
-	LOGERR(" ");
-
-	return -EFAULT;
-}
-
-static ssize_t
-cycles_before_wait_proc_write(struct file *file, const char __user *buffer,
-			      size_t count, loff_t *ppos)
-{
-	char buf[16];
-
-#define CYCLES_BEFORE_WAIT_USE_ERROR  { \
-	LOGERR("Incorrect Call Home Input.\n"); \
-	pr_info("Please pass Call Home Event Parameters in the form:\n"); \
-	pr_info("EventID Category Type[parameter1][parameter2][parameter3][parameter4][parameter5][parameter6]\n"); \
-	return -EFAULT; \
-}
-	if (count >= ARRAY_SIZE(buf))
-		return -EINVAL;
-
-	if (count == 0)
-		CYCLES_BEFORE_WAIT_USE_ERROR;
-
-	if (copy_from_user(buf, buffer, count)) {
-		LOGERR("copy_from_user failed.\n");
-		return -EFAULT;
-	}
-	buf[count - 1] = '\0';	/* Replace the LF at the end of the
-				 * input with a NULL */
-	/* Pull out the cycles_before_wait must be decimal integer */
-	if (sscanf(buf, "%lld", &cycles_before_wait) != 1)
-		CYCLES_BEFORE_WAIT_USE_ERROR;
-
-	return count;
-}
-
-static ssize_t
-reset_counts_proc_write(struct file *file, const char __user *buffer,
-			size_t count, loff_t *ppos)
-{
-	char buf[16];
-	unsigned long long new_value;
-	struct bus_info *bus;
-	int i;
-
-#define RESET_COUNTS_USE_ERROR  { \
-	LOGERR("Incorrect reset_counts Input.\n"); \
-	pr_info("Please pass the new value for the counters:\n"); \
-	pr_info("e.g. echo 0 > reset_counts\n"); \
-	return -EFAULT; \
-	}
-
-	if (count >= ARRAY_SIZE(buf))
-		return -EINVAL;
-
-	if (count == 0)
-		RESET_COUNTS_USE_ERROR;
-
-	if (copy_from_user(buf, buffer, count)) {
-		LOGERR("copy_from_user failed.\n");
-		return -EFAULT;
-	}
-	buf[count - 1] = '\0';	/* Replace the LF at the end of the
-				 * input with a NULL */
-	/* Pull out the reset_counts must be decimal integer */
-	if (sscanf(buf, "%llu", &new_value) != 1)
-		RESET_COUNTS_USE_ERROR;
-	read_lock(&BusListLock);
-	for (bus = BusListHead; bus; bus = bus->next) {
-
-		for (i = 0; i < bus->deviceCount; i++) {
-			if (bus->device[i]) {
-				bus->device[i]->first_busy_cnt = new_value;
-				bus->device[i]->moved_to_tail_cnt = new_value;
-				bus->device[i]->last_on_list_cnt = new_value;
-			}
-		}
-	}
-	read_unlock(&BusListLock);
-	tot_moved_to_tail_cnt = new_value;
-	tot_wait_cnt = new_value;
-	tot_wakeup_cnt = new_value;
-	tot_schedule_cnt = new_value;
-	return count;
-}
-
-static ssize_t
-smart_wakeup_proc_write(struct file *file, const char __user *buffer,
-			size_t count, loff_t *ppos)
-{
-	char buf[16];
-	int new_value;
-
-#define SMART_WAKEUP_USE_ERROR  { \
-	LOGERR("Incorrect smart_wakeup Input 0 disables smart_wakeup, and 1 enables smart_wakeup.\n"); \
-	pr_info("echo 0 > smart_wakeup\n"); \
-	pr_info("echo 1 > smart_wakeup\n"); \
-	return -EFAULT; \
-	}
-
-	if (count >= ARRAY_SIZE(buf))
-		return -EINVAL;
-
-	if (count == 0)
-		SMART_WAKEUP_USE_ERROR;
-
-	if (copy_from_user(buf, buffer, count)) {
-		LOGERR("copy_from_user failed.\n");
-		return -EFAULT;
-	}
-	buf[count - 1] = '\0';	/* Replace the LF at the end of the
-				 * input with a NULL */
-	/* Pull out the smart_wakeup must be decimal integer */
-	if (sscanf(buf, "%d", &new_value) != 1)
-		SMART_WAKEUP_USE_ERROR;
-	en_smart_wakeup = new_value;
-	return count;
-}
-
-static ssize_t
-test_proc_write(struct file *file, const char __user *buffer,
-		size_t count, loff_t *ppos)
-{
-	int i, action = 0xffff;
-	char buf[16];
-	CONTROLVM_MESSAGE msg;
-	S64 vrtc_offset;
-
-	if (count >= ARRAY_SIZE(buf))
-		return -EINVAL;
-
-	memset(&msg, 0, sizeof(CONTROLVM_MESSAGE));
-
-	if (copy_from_user(buf, buffer, count)) {
-		LOGERR("copy_from_user ****FAILED.\n");
-		return -EFAULT;
-	}
-
-	i = sscanf(buf, "%x", &action);
-
-	/* if (i < 1), i.e., if we didn't even read the action field,
-	* then action will default to 0xffff and the code below will
-	* fall through the switch and print usage. */
-	switch (action) {
-	case 6:
-		msg.hdr.Id = CONTROLVM_CHIPSET_STOP;
-		msg.hdr.Flags.responseExpected = 1;
-		stop_chipset(&msg, NULL);
-		break;
-	case 7:
-		vrtc_offset = 0;
-		LOGERR("about to issue QUERY vrtc_offset=%LX", vrtc_offset);
-		vrtc_offset = Issue_VMCALL_QUERY_GUEST_VIRTUAL_TIME_OFFSET();
-		LOGERR("result is vrtc_offset=%LX", vrtc_offset);
-		break;
-	case 8:
-		vrtc_offset = 60;
-		LOGERR("about to increase physical time by 0x%LX seconds",
-		       vrtc_offset);
-		vrtc_offset = Issue_VMCALL_UPDATE_PHYSICAL_TIME(vrtc_offset);
-		break;
-	case 9:
-		vrtc_offset = -60;
-		LOGERR("about to decrease physical time by 0x%LX seconds",
-		       vrtc_offset);
-		vrtc_offset = Issue_VMCALL_UPDATE_PHYSICAL_TIME(vrtc_offset);
-		break;
-	default:
-		LOGERR("usage: 6 for CHIPSET_STOP\n");
-		LOGERR("       7 for VMCALL_QUERY_GUEST_VIRTUAL_TIME_OFFSET()\n");
-		LOGERR("       8 for VMCALL_UPDATE_PHYSICAL_TIME(60)\n");
-		LOGERR("       9 for VMCALL_UPDATE_PHYSICAL_TIME(-60)\n");
-		return -EFAULT;
-		break;
-	}
-	return count;
-}
-
-#endif				/* UISLIB_TEST_PROC */
 static struct device_info *
 find_dev(U32 busNo, U32 devNo)
 {
@@ -2277,6 +1552,9 @@ static int __init
 uislib_mod_init(void)
 {
 
+	if (!unisys_spar_platform)
+		return -ENODEV;
+
 	LOGINF("MONITORAPIS");
 
 	LOGINF("sizeof(struct uiscmdrsp):%lu bytes\n",
@@ -2308,59 +1586,25 @@ uislib_mod_init(void)
 	 * then map this physical address to a virtual address. */
 	POSTCODE_LINUX_2(DRIVER_ENTRY_PC, POSTCODE_SEVERITY_INFO);
 
-	/* create the proc entries for the channels */
-	uislib_proc_dir = proc_mkdir(DIR_PROC_ENTRY, NULL);
-	/* (e.g., for /proc/uislib/vbus/<x>/info) */
-	uislib_proc_vbus_dir = proc_mkdir(DIR_VBUS_PROC_ENTRY, uislib_proc_dir);
-
-	vnic_proc_entry = proc_create(VNIC_PROC_ENTRY_FN, 0, uislib_proc_dir,
-				      &proc_vnic_fops);
-	SET_PROC_OWNER(vnic_proc_entry, THIS_MODULE);
-
-	/* for testing purposes only, create the proc entries for
-	 * enqueuing Control Channel messages */
-	chipset_proc_entry =
-	    proc_create(CHIPSET_PROC_ENTRY_FN, 0, uislib_proc_dir,
-			&proc_chipset_fops);
-	SET_PROC_OWNER(chipset_proc_entry, THIS_MODULE);
-
-	info_proc_entry = proc_create(INFO_PROC_ENTRY_FN, 0, uislib_proc_dir,
-				      &proc_info_fops);
-	SET_PROC_OWNER(info_proc_entry, THIS_MODULE);
-
-	platformnumber_proc_entry =
-	    proc_create(PLATFORMNUMBER_PROC_ENTRY_FN, 0, uislib_proc_dir,
-			&proc_platformnumber_fops);
-	SET_PROC_OWNER(platformnumberinfo_proc_entry, THIS_MODULE);
-
-	cycles_before_wait_proc_entry =
-	    proc_create(CYCLES_BEFORE_WAIT_PROC_ENTRY_FN, 0, uislib_proc_dir,
-			&proc_cycles_before_wait_fops);
-	SET_PROC_OWNER(cycles_before_wait_proc_entry, THIS_MODULE);
-
-	reset_counts_proc_entry =
-	    proc_create(RESET_COUNTS_PROC_ENTRY_FN, 0, uislib_proc_dir,
-			&proc_reset_counts_fops);
-	SET_PROC_OWNER(reset_counts_proc_entry, THIS_MODULE);
-
-	smart_wakeup_proc_entry =
-	    proc_create(SMART_WAKEUP_PROC_ENTRY_FN, 0, uislib_proc_dir,
-			&proc_smart_wakeup_fops);
-	SET_PROC_OWNER(smart_wakeup_proc_entry, THIS_MODULE);
-
-#ifdef UISLIB_TEST_PROC
-	test_proc_entry = proc_create(TEST_PROC_ENTRY_FN, 0, uislib_proc_dir,
-				      &proc_test_fops);
-	SET_PROC_OWNER(test_proc_entry, THIS_MODULE);
-
-	bus_proc_entry = proc_create(BUS_PROC_ENTRY_FN, 0, uislib_proc_dir,
-				     &proc_bus_fops);
-	SET_PROC_OWNER(bus_proc_entry, THIS_MODULE);
-
-	dev_proc_entry = proc_create(DEV_PROC_ENTRY_FN, 0, uislib_proc_dir,
-				     &proc_dev_fops);
-	SET_PROC_OWNER(dev_proc_entry, THIS_MODULE);
-#endif				/* UISLIB_TEST_PROC */
+	dir_debugfs = debugfs_create_dir(DIR_DEBUGFS_ENTRY, NULL);
+	if (dir_debugfs) {
+		info_debugfs_entry = debugfs_create_file(
+			INFO_DEBUGFS_ENTRY_FN, 0444, dir_debugfs, NULL,
+			&debugfs_info_fops);
+
+		platformnumber_debugfs_read = debugfs_create_u32(
+			PLATFORMNUMBER_DEBUGFS_ENTRY_FN, 0444, dir_debugfs,
+			&PlatformNumber);
+
+		cycles_before_wait_debugfs_read = debugfs_create_u64(
+			CYCLES_BEFORE_WAIT_DEBUGFS_ENTRY_FN, 0666, dir_debugfs,
+			&cycles_before_wait);
+
+		smart_wakeup_debugfs_entry = debugfs_create_bool(
+			SMART_WAKEUP_DEBUGFS_ENTRY_FN, 0666, dir_debugfs,
+			&en_smart_wakeup);
+	}
+
 	POSTCODE_LINUX_3(DRIVER_EXIT_PC, 0, POSTCODE_SEVERITY_INFO);
 	return 0;
 }
@@ -2368,46 +1612,17 @@ uislib_mod_init(void)
 static void __exit
 uislib_mod_exit(void)
 {
-	if (disable_proc_entry)
-		remove_proc_entry(DISABLE_PROC_ENTRY_FN, uislib_proc_dir);
-	if (cycles_before_wait_proc_entry)
-		remove_proc_entry(CYCLES_BEFORE_WAIT_PROC_ENTRY_FN,
-				  uislib_proc_dir);
-	if (reset_counts_proc_entry)
-		remove_proc_entry(RESET_COUNTS_PROC_ENTRY_FN, uislib_proc_dir);
-	if (smart_wakeup_proc_entry)
-		remove_proc_entry(SMART_WAKEUP_PROC_ENTRY_FN, uislib_proc_dir);
-	if (ctrlchan_proc_entry)
-		remove_proc_entry(CTRLCHAN_PROC_ENTRY_FN, uislib_proc_dir);
-	if (pmem_proc_entry)
-		remove_proc_entry(PMEM_PROC_ENTRY_FN, uislib_proc_dir);
-	if (info_proc_entry)
-		remove_proc_entry(INFO_PROC_ENTRY_FN, uislib_proc_dir);
-	if (switch_proc_entry)
-		remove_proc_entry(SWITCH_PROC_ENTRY_FN, uislib_proc_dir);
-	if (extport_proc_entry)
-		remove_proc_entry(EXTPORT_PROC_ENTRY_FN, uislib_proc_dir);
-	if (platformnumber_proc_entry)
-		remove_proc_entry(PLATFORMNUMBER_PROC_ENTRY_FN,
-				  uislib_proc_dir);
-	if (bus_proc_entry)
-		remove_proc_entry(BUS_PROC_ENTRY_FN, uislib_proc_dir);
-	if (dev_proc_entry)
-		remove_proc_entry(DEV_PROC_ENTRY_FN, uislib_proc_dir);
-	if (vnic_proc_entry)
-		remove_proc_entry(VNIC_PROC_ENTRY_FN, uislib_proc_dir);
-	if (chipset_proc_entry)
-		remove_proc_entry(CHIPSET_PROC_ENTRY_FN, uislib_proc_dir);
-	if (uislib_proc_vbus_dir)
-		remove_proc_entry(DIR_VBUS_PROC_ENTRY, uislib_proc_dir);
-	if (uislib_proc_dir)
-		remove_proc_entry(DIR_PROC_ENTRY, NULL);
-
 	if (ProcReadBuffer) {
 		vfree(ProcReadBuffer);
 		ProcReadBuffer = NULL;
 	}
 
+	debugfs_remove(info_debugfs_entry);
+	debugfs_remove(smart_wakeup_debugfs_entry);
+	debugfs_remove(cycles_before_wait_debugfs_read);
+	debugfs_remove(platformnumber_debugfs_read);
+	debugfs_remove(dir_debugfs);
+
 	DBGINF("goodbye.\n");
 	return;
 }
diff --git a/drivers/staging/unisys/uislib/uisqueue.c b/drivers/staging/unisys/uislib/uisqueue.c
index 40598ff1f4f..d4a6074cfaf 100644
--- a/drivers/staging/unisys/uislib/uisqueue.c
+++ b/drivers/staging/unisys/uislib/uisqueue.c
@@ -1,6 +1,6 @@
 /* uisqueue.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/uislib/uisthread.c b/drivers/staging/unisys/uislib/uisthread.c
index 782b06aad56..c93ab04c3cf 100644
--- a/drivers/staging/unisys/uislib/uisthread.c
+++ b/drivers/staging/unisys/uislib/uisthread.c
@@ -1,6 +1,6 @@
 /* uisthread.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/uislib/uisutils.c b/drivers/staging/unisys/uislib/uisutils.c
index 3178f75e1eb..0f1bb739975 100644
--- a/drivers/staging/unisys/uislib/uisutils.c
+++ b/drivers/staging/unisys/uislib/uisutils.c
@@ -1,6 +1,6 @@
 /* uisutils.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -24,8 +24,9 @@
 #include "uisutils.h"
 #include "version.h"
 #include "vbushelper.h"
-#include "guidutils.h"
+#include <linux/uuid.h>
 #include <linux/skbuff.h>
+#include <linux/uuid.h>
 #ifdef CONFIG_HIGHMEM
 #include <linux/highmem.h>
 #endif
@@ -104,7 +105,7 @@ uisctrl_register_req_handler(int type, void *fptr,
 EXPORT_SYMBOL_GPL(uisctrl_register_req_handler);
 
 int
-uisctrl_register_req_handler_ex(GUID switchTypeGuid,
+uisctrl_register_req_handler_ex(uuid_le switchTypeGuid,
 				const char *switch_type_name,
 				int (*controlfunc)(struct io_msgs *),
 				unsigned long min_channel_bytes,
@@ -115,24 +116,22 @@ uisctrl_register_req_handler_ex(GUID switchTypeGuid,
 				  U32 clientStrLen, U64 bytes),
 				ULTRA_VBUS_DEVICEINFO *chipset_DriverInfo)
 {
-	char s[99];
 	ReqHandlerInfo_t *pReqHandlerInfo;
 	int rc = 0;		/* assume failure */
-	LOGINF("type=%s, controlfunc=0x%p.\n",
-	       GUID_format1(&switchTypeGuid, s), controlfunc);
+	LOGINF("type=%pUL, controlfunc=0x%p.\n",
+	       &switchTypeGuid, controlfunc);
 	if (!controlfunc) {
-		LOGERR("%s: controlfunc must be supplied\n",
-		       GUID_format1(&switchTypeGuid, s));
+		LOGERR("%pUL: controlfunc must be supplied\n", &switchTypeGuid);
 		goto Away;
 	}
 	if (!Server_Channel_Ok) {
-		LOGERR("%s: Server_Channel_Ok must be supplied\n",
-		       GUID_format1(&switchTypeGuid, s));
+		LOGERR("%pUL: Server_Channel_Ok must be supplied\n",
+				&switchTypeGuid);
 		goto Away;
 	}
 	if (!Server_Channel_Init) {
-		LOGERR("%s: Server_Channel_Init must be supplied\n",
-		       GUID_format1(&switchTypeGuid, s));
+		LOGERR("%pUL: Server_Channel_Init must be supplied\n",
+				&switchTypeGuid);
 		goto Away;
 	}
 	pReqHandlerInfo = ReqHandlerAdd(switchTypeGuid,
@@ -141,8 +140,7 @@ uisctrl_register_req_handler_ex(GUID switchTypeGuid,
 					min_channel_bytes,
 					Server_Channel_Ok, Server_Channel_Init);
 	if (!pReqHandlerInfo) {
-		LOGERR("failed to add %s to server list\n",
-		       GUID_format1(&switchTypeGuid, s));
+		LOGERR("failed to add %pUL to server list\n", &switchTypeGuid);
 		goto Away;
 	}
 
@@ -156,30 +154,27 @@ Away:
 					   VERSION, NULL,
 					   __DATE__, __TIME__);
 	} else
-		LOGERR("failed to register type %s.\n",
-		       GUID_format1(&switchTypeGuid, s));
+		LOGERR("failed to register type %pUL.\n", &switchTypeGuid);
 
 	return rc;
 }
 EXPORT_SYMBOL_GPL(uisctrl_register_req_handler_ex);
 
 int
-uisctrl_unregister_req_handler_ex(GUID switchTypeGuid)
+uisctrl_unregister_req_handler_ex(uuid_le switchTypeGuid)
 {
-	char s[99];
 	int rc = 0;		/* assume failure */
-	LOGINF("type=%s.\n", GUID_format1(&switchTypeGuid, s));
+	LOGINF("type=%pUL.\n", &switchTypeGuid);
 	if (ReqHandlerDel(switchTypeGuid) < 0) {
-		LOGERR("failed to remove %s from server list\n",
-		       GUID_format1(&switchTypeGuid, s));
+		LOGERR("failed to remove %pUL from server list\n",
+				&switchTypeGuid);
 		goto Away;
 	}
 	atomic_dec(&UisUtils_Registered_Services);
 	rc = 1;			/* success */
 Away:
 	if (!rc)
-		LOGERR("failed to unregister type %s.\n",
-		       GUID_format1(&switchTypeGuid, s));
+		LOGERR("failed to unregister type %pUL.\n", &switchTypeGuid);
 	return rc;
 }
 EXPORT_SYMBOL_GPL(uisctrl_unregister_req_handler_ex);
@@ -281,7 +276,7 @@ static LIST_HEAD(ReqHandlerInfo_list);	/* list of ReqHandlerInfo_t */
 static DEFINE_SPINLOCK(ReqHandlerInfo_list_lock);
 
 ReqHandlerInfo_t *
-ReqHandlerAdd(GUID switchTypeGuid,
+ReqHandlerAdd(uuid_le switchTypeGuid,
 	      const char *switch_type_name,
 	      int (*controlfunc)(struct io_msgs *),
 	      unsigned long min_channel_bytes,
@@ -310,16 +305,14 @@ ReqHandlerAdd(GUID switchTypeGuid,
 }
 
 ReqHandlerInfo_t *
-ReqHandlerFind(GUID switchTypeGuid)
+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);
-		if (memcmp
-		    (&entry->switchTypeGuid, &switchTypeGuid,
-		     sizeof(GUID)) == 0) {
+		if (uuid_le_cmp(entry->switchTypeGuid, switchTypeGuid) == 0) {
 			spin_unlock(&ReqHandlerInfo_list_lock);
 			return entry;
 		}
@@ -329,7 +322,7 @@ ReqHandlerFind(GUID switchTypeGuid)
 }
 
 int
-ReqHandlerDel(GUID switchTypeGuid)
+ReqHandlerDel(uuid_le switchTypeGuid)
 {
 	struct list_head *lelt, *tmp;
 	ReqHandlerInfo_t *entry = NULL;
@@ -337,9 +330,7 @@ ReqHandlerDel(GUID switchTypeGuid)
 	spin_lock(&ReqHandlerInfo_list_lock);
 	list_for_each_safe(lelt, tmp, &ReqHandlerInfo_list) {
 		entry = list_entry(lelt, ReqHandlerInfo_t, list_link);
-		if (memcmp
-		    (&entry->switchTypeGuid, &switchTypeGuid,
-		     sizeof(GUID)) == 0) {
+		if (uuid_le_cmp(entry->switchTypeGuid, switchTypeGuid) == 0) {
 			list_del(lelt);
 			kfree(entry);
 			rc++;
diff --git a/drivers/staging/unisys/virthba/virthba.c b/drivers/staging/unisys/virthba/virthba.c
index 817b11dfa19..5c5aa700176 100644
--- a/drivers/staging/unisys/virthba/virthba.c
+++ b/drivers/staging/unisys/virthba/virthba.c
@@ -1,6 +1,6 @@
 /* virthba.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -82,8 +82,16 @@ static int virthba_ioctl(struct scsi_device *dev, int cmd, void __user *arg);
 static int virthba_queue_command_lck(struct scsi_cmnd *scsicmd,
 				     void (*virthba_cmnd_done)(struct scsi_cmnd *));
 
+static const struct x86_cpu_id unisys_spar_ids[] = {
+	{ X86_VENDOR_INTEL, 6, 62, X86_FEATURE_ANY },
+	{}
+};
+
+/* Autoload */
+MODULE_DEVICE_TABLE(x86cpu, unisys_spar_ids);
+
 #ifdef DEF_SCSI_QCMD
-DEF_SCSI_QCMD(virthba_queue_command)
+static DEF_SCSI_QCMD(virthba_queue_command)
 #else
 #define virthba_queue_command virthba_queue_command_lck
 #endif
@@ -1046,7 +1054,7 @@ static int
 virthba_slave_alloc(struct scsi_device *scsidev)
 {
 	/* this called by the midlayer before scan for new devices -
-	 * LLD can alloc any struc & do init if needed.
+	 * LLD can alloc any struct & do init if needed.
 	 */
 	struct virtdisk_info *vdisk;
 	struct virtdisk_info *tmpvdisk;
@@ -1400,7 +1408,7 @@ info_proc_read(struct file *file, char __user *buf, size_t len, loff_t *offset)
 		virthbainfo = VirtHbasOpen[i].virthbainfo;
 		length += sprintf(vbuf + length, "CHANSOCK is not defined.\n");
 
-		length += sprintf(vbuf + length, "MaxBuffLen:%d\n", MaxBuffLen);
+		length += sprintf(vbuf + length, "MaxBuffLen:%u\n", MaxBuffLen);
 
 		length += sprintf(vbuf + length, "\nvirthba result queue poll wait:%d usecs.\n",
 				  rsltq_wait_usecs);
@@ -1534,7 +1542,7 @@ virthba_serverup(struct virtpci_dev *virtpcidev)
 	       virtpcidev->deviceNo);
 
 	if (!virthbainfo->serverdown) {
-		DBGINF("Server up message recieved while server is already up.\n");
+		DBGINF("Server up message received while server is already up.\n");
 		return 1;
 	}
 	if (virthbainfo->serverchangingstate) {
@@ -1691,6 +1699,9 @@ virthba_mod_init(void)
 	int error;
 	int i;
 
+	if (!unisys_spar_platform)
+		return -ENODEV;
+
 	LOGINF("Entering virthba_mod_init...\n");
 
 	POSTCODE_LINUX_2(VHBA_CREATE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
diff --git a/drivers/staging/unisys/virthba/virthba.h b/drivers/staging/unisys/virthba/virthba.h
index 88b797439a1..d4b809b0c7b 100644
--- a/drivers/staging/unisys/virthba/virthba.h
+++ b/drivers/staging/unisys/virthba/virthba.h
@@ -1,6 +1,6 @@
 /* virthba.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/virtpci/virtpci.c b/drivers/staging/unisys/virtpci/virtpci.c
index 8e34650b00b..71246feb154 100644
--- a/drivers/staging/unisys/virtpci/virtpci.c
+++ b/drivers/staging/unisys/virtpci/virtpci.c
@@ -1,6 +1,6 @@
 /* virtpci.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -39,6 +39,7 @@
 #include <linux/version.h>
 #include "version.h"
 #include "guestlinuxdebug.h"
+#include "timskmodutils.h"
 
 struct driver_private {
 	struct kobject kobj;
@@ -364,7 +365,7 @@ static int add_vhba(struct add_virt_guestpart *addparams)
 		memcpy_fromio(&net.zoneGuid, \
 			      &((ULTRA_IO_CHANNEL_PROTOCOL __iomem *)	\
 				chanptr)->vnic.zoneGuid,		\
-			      sizeof(GUID));				\
+			      sizeof(uuid_le));				\
 }
 
 /* adds a vnic
@@ -390,14 +391,10 @@ add_vnic(struct add_virt_guestpart *addparams)
 
 	GET_BUS_DEV(addparams->busNo);
 
-	LOGINF("Adding vnic macaddr:%02x:%02x:%02x:%02x:%02x:%02x rcvbufs:%d mtu:%d chanptr:%p{%-8.8lx-%-4.4x-%-4.4x-%-2.2x%-2.2x%-2.2x%-2.2x%-2.2x%-2.2x%-2.2x%-2.2x}\n",
+	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],
 	     net.mac_addr[4], net.mac_addr[5], net.num_rcv_bufs, net.mtu,
-	     addparams->chanptr, (ulong) net.zoneGuid.data1, net.zoneGuid.data2,
-	     net.zoneGuid.data3, net.zoneGuid.data4[0], net.zoneGuid.data4[1],
-	     net.zoneGuid.data4[2], net.zoneGuid.data4[3],
-	     net.zoneGuid.data4[4], net.zoneGuid.data4[5],
-	     net.zoneGuid.data4[6], net.zoneGuid.data4[7]);
+	     addparams->chanptr, &net.zoneGuid);
 	i = virtpci_device_add(vbus, VIRTNIC_TYPE, addparams, NULL, &net);
 	if (i) {
 		LOGINF("Added vnic macaddr:%02x:%02x:%02x:%02x:%02x:%02x\n",
@@ -1509,7 +1506,6 @@ static ssize_t virt_proc_write(struct file *file, const char __user *buffer,
 	struct add_virt_guestpart addparams;
 	struct del_vbus_guestpart busdelparams;
 	struct del_virt_guestpart delparams;
-	GUID dummyGuid = GUID0;
 #ifdef STORAGE_CHANNEL
 	U64 storagechannel;
 #endif
@@ -1560,7 +1556,7 @@ static ssize_t virt_proc_write(struct file *file, const char __user *buffer,
 						   __pa(chanptr),
 						   MIN_IO_CHANNEL_SIZE,
 						   1, /* test msg */
-						   dummyGuid, /* inst guid */
+						   NULL_UUID_LE, /* inst guid */
 						   NULL)) { /*interrupt info */
 			LOGERR("FAILED to inject add vnic\n");
 			return -EFAULT;
@@ -1687,6 +1683,9 @@ static int __init virtpci_mod_init(void)
 	int ret;
 
 
+	if (!unisys_spar_platform)
+		return -ENODEV;
+
 	LOGINF("Module build: Date:%s Time:%s...\n", __DATE__, __TIME__);
 
 	POSTCODE_LINUX_2(VPCI_CREATE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
diff --git a/drivers/staging/unisys/virtpci/virtpci.h b/drivers/staging/unisys/virtpci/virtpci.h
index b8fd07bc543..f7be17b669c 100644
--- a/drivers/staging/unisys/virtpci/virtpci.h
+++ b/drivers/staging/unisys/virtpci/virtpci.h
@@ -1,6 +1,6 @@
 /* virtpci.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -24,6 +24,7 @@
 
 #include "uisqueue.h"
 #include <linux/version.h>
+#include <linux/uuid.h>
 
 #define PCI_DEVICE_ID_VIRTHBA 0xAA00
 #define PCI_DEVICE_ID_VIRTNIC 0xAB00
@@ -41,7 +42,7 @@ struct net_adap_info {
 	u8 mac_addr[MAX_MACADDR_LEN];
 	int num_rcv_bufs;
 	unsigned mtu;
-	GUID zoneGuid;
+	uuid_le zoneGuid;
 };
 
 typedef enum {
diff --git a/drivers/staging/unisys/visorchannel/globals.h b/drivers/staging/unisys/visorchannel/globals.h
index 668f832ca56..07653b8dea7 100644
--- a/drivers/staging/unisys/visorchannel/globals.h
+++ b/drivers/staging/unisys/visorchannel/globals.h
@@ -1,6 +1,6 @@
 /* globals.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/visorchannel/visorchannel.h b/drivers/staging/unisys/visorchannel/visorchannel.h
index 62d29a233fd..ecf0d11117e 100644
--- a/drivers/staging/unisys/visorchannel/visorchannel.h
+++ b/drivers/staging/unisys/visorchannel/visorchannel.h
@@ -1,6 +1,6 @@
 /* visorchannel.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -18,6 +18,8 @@
 #ifndef __VISORCHANNEL_H__
 #define __VISORCHANNEL_H__
 
+#include <linux/uuid.h>
+
 #include "commontypes.h"
 #include "memregion.h"
 #include "channel.h"
@@ -38,15 +40,15 @@ typedef struct VISORCHANNEL_Tag VISORCHANNEL;
  * In this case, the values can simply be read from the channel header.
  */
 VISORCHANNEL *visorchannel_create(HOSTADDRESS physaddr,
-				  ulong channelBytes, GUID guid);
+				  ulong channelBytes, uuid_le guid);
 VISORCHANNEL *visorchannel_create_overlapped(ulong channelBytes,
 					     VISORCHANNEL *parent, ulong off,
-					     GUID guid);
+					     uuid_le guid);
 VISORCHANNEL *visorchannel_create_with_lock(HOSTADDRESS physaddr,
-					    ulong channelBytes, GUID guid);
+					    ulong channelBytes, uuid_le guid);
 VISORCHANNEL *visorchannel_create_overlapped_with_lock(ulong channelBytes,
 						       VISORCHANNEL *parent,
-						       ulong off, GUID guid);
+						       ulong off, uuid_le guid);
 void visorchannel_destroy(VISORCHANNEL *channel);
 int visorchannel_read(VISORCHANNEL *channel, ulong offset,
 		      void *local, ulong nbytes);
@@ -64,43 +66,13 @@ ulong visorchannel_get_nbytes(VISORCHANNEL *channel);
 char *visorchannel_id(VISORCHANNEL *channel, char *s);
 char *visorchannel_zoneid(VISORCHANNEL *channel, char *s);
 U64 visorchannel_get_clientpartition(VISORCHANNEL *channel);
-GUID visorchannel_get_GUID(VISORCHANNEL *channel);
+uuid_le visorchannel_get_uuid(VISORCHANNEL *channel);
 MEMREGION *visorchannel_get_memregion(VISORCHANNEL *channel);
-char *visorchannel_GUID_id(GUID *guid, char *s);
+char *visorchannel_uuid_id(uuid_le *guid, char *s);
 void visorchannel_debug(VISORCHANNEL *channel, int nQueues,
 			struct seq_file *seq, U32 off);
 void visorchannel_dump_section(VISORCHANNEL *chan, char *s,
 			       int off, int len, struct seq_file *seq);
-void *visorchannel_get_header(VISORCHANNEL *channel);
-
-#define	VISORCHANNEL_CHANGE_SERVER_STATE(chan, chanId, newstate)	\
-	do {								\
-		U8 *p = (U8 *)visorchannel_get_header(chan);		\
-		if (p) {						\
-			ULTRA_CHANNEL_SERVER_TRANSITION(p, chanId, SrvState, \
-							newstate, logCtx); \
-			visorchannel_write				\
-				(chan,					\
-				 offsetof(ULTRA_CHANNEL_PROTOCOL, SrvState), \
-				 p +					\
-				 offsetof(ULTRA_CHANNEL_PROTOCOL, SrvState), \
-				 sizeof(U32));				\
-		}							\
-	} while (0)
-
-#define	VISORCHANNEL_CHANGE_CLIENT_STATE(chan, chanId, newstate)	\
-	do {								\
-		U8 *p = (U8 *)visorchannel_get_header(chan);		\
-		if (p) {						\
-			ULTRA_CHANNEL_CLIENT_TRANSITION(p, chanId,	\
-							newstate, logCtx); \
-			visorchannel_write				\
-				(chan,					\
-				 offsetof(ULTRA_CHANNEL_PROTOCOL, CliStateOS), \
-				 p +					\
-				 offsetof(ULTRA_CHANNEL_PROTOCOL, CliStateOS), \
-				 sizeof(U32));				\
-		}							\
-	} while (0)
+void __iomem *visorchannel_get_header(VISORCHANNEL *channel);
 
 #endif
diff --git a/drivers/staging/unisys/visorchannel/visorchannel_funcs.c b/drivers/staging/unisys/visorchannel/visorchannel_funcs.c
index 053681616ba..a44da7c84ae 100644
--- a/drivers/staging/unisys/visorchannel/visorchannel_funcs.c
+++ b/drivers/staging/unisys/visorchannel/visorchannel_funcs.c
@@ -1,6 +1,6 @@
 /* visorchannel_funcs.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -24,14 +24,14 @@
 
 #include "globals.h"
 #include "visorchannel.h"
-#include "guidutils.h"
+#include <linux/uuid.h>
 
 #define MYDRVNAME "visorchannel"
 
 struct VISORCHANNEL_Tag {
 	MEMREGION *memregion;	/* from visor_memregion_create() */
 	CHANNEL_HEADER chan_hdr;
-	GUID guid;
+	uuid_le guid;
 	ulong size;
 	BOOL needs_lock;
 	spinlock_t insert_lock;
@@ -50,7 +50,7 @@ struct VISORCHANNEL_Tag {
  */
 static VISORCHANNEL *
 visorchannel_create_guts(HOSTADDRESS physaddr, ulong channelBytes,
-			 VISORCHANNEL *parent, ulong off, GUID guid,
+			 VISORCHANNEL *parent, ulong off, uuid_le guid,
 			 BOOL needs_lock)
 {
 	VISORCHANNEL *p = NULL;
@@ -90,7 +90,7 @@ visorchannel_create_guts(HOSTADDRESS physaddr, ulong channelBytes,
 	if (channelBytes == 0)
 		/* we had better be a CLIENT of this channel */
 		channelBytes = (ulong) p->chan_hdr.Size;
-	if (STRUCTSEQUAL(guid, Guid0))
+	if (uuid_le_cmp(guid, NULL_UUID_LE) == 0)
 		/* we had better be a CLIENT of this channel */
 		guid = p->chan_hdr.Type;
 	if (visor_memregion_resize(p->memregion, channelBytes) < 0) {
@@ -114,7 +114,7 @@ Away:
 }
 
 VISORCHANNEL *
-visorchannel_create(HOSTADDRESS physaddr, ulong channelBytes, GUID guid)
+visorchannel_create(HOSTADDRESS physaddr, ulong channelBytes, uuid_le guid)
 {
 	return visorchannel_create_guts(physaddr, channelBytes, NULL, 0, guid,
 					FALSE);
@@ -123,7 +123,7 @@ EXPORT_SYMBOL_GPL(visorchannel_create);
 
 VISORCHANNEL *
 visorchannel_create_with_lock(HOSTADDRESS physaddr, ulong channelBytes,
-			      GUID guid)
+			      uuid_le guid)
 {
 	return visorchannel_create_guts(physaddr, channelBytes, NULL, 0, guid,
 					TRUE);
@@ -132,7 +132,7 @@ EXPORT_SYMBOL_GPL(visorchannel_create_with_lock);
 
 VISORCHANNEL *
 visorchannel_create_overlapped(ulong channelBytes,
-			       VISORCHANNEL *parent, ulong off, GUID guid)
+			       VISORCHANNEL *parent, ulong off, uuid_le guid)
 {
 	return visorchannel_create_guts(0, channelBytes, parent, off, guid,
 					FALSE);
@@ -142,7 +142,7 @@ EXPORT_SYMBOL_GPL(visorchannel_create_overlapped);
 VISORCHANNEL *
 visorchannel_create_overlapped_with_lock(ulong channelBytes,
 					 VISORCHANNEL *parent, ulong off,
-					 GUID guid)
+					 uuid_le guid)
 {
 	return visorchannel_create_guts(0, channelBytes, parent, off, guid,
 					TRUE);
@@ -177,23 +177,24 @@ visorchannel_get_nbytes(VISORCHANNEL *channel)
 EXPORT_SYMBOL_GPL(visorchannel_get_nbytes);
 
 char *
-visorchannel_GUID_id(GUID *guid, char *s)
+visorchannel_uuid_id(uuid_le *guid, char *s)
 {
-	return GUID_format1(guid, s);
+	sprintf(s, "%pUL", guid);
+	return s;
 }
-EXPORT_SYMBOL_GPL(visorchannel_GUID_id);
+EXPORT_SYMBOL_GPL(visorchannel_uuid_id);
 
 char *
 visorchannel_id(VISORCHANNEL *channel, char *s)
 {
-	return visorchannel_GUID_id(&channel->guid, s);
+	return visorchannel_uuid_id(&channel->guid, s);
 }
 EXPORT_SYMBOL_GPL(visorchannel_id);
 
 char *
 visorchannel_zoneid(VISORCHANNEL *channel, char *s)
 {
-	return visorchannel_GUID_id(&channel->chan_hdr.ZoneGuid, s);
+	return visorchannel_uuid_id(&channel->chan_hdr.ZoneGuid, s);
 }
 EXPORT_SYMBOL_GPL(visorchannel_zoneid);
 
@@ -204,12 +205,12 @@ visorchannel_get_clientpartition(VISORCHANNEL *channel)
 }
 EXPORT_SYMBOL_GPL(visorchannel_get_clientpartition);
 
-GUID
-visorchannel_get_GUID(VISORCHANNEL *channel)
+uuid_le
+visorchannel_get_uuid(VISORCHANNEL *channel)
 {
 	return channel->guid;
 }
-EXPORT_SYMBOL_GPL(visorchannel_get_GUID);
+EXPORT_SYMBOL_GPL(visorchannel_get_uuid);
 
 MEMREGION *
 visorchannel_get_memregion(VISORCHANNEL *channel)
@@ -278,10 +279,10 @@ Away:
 }
 EXPORT_SYMBOL_GPL(visorchannel_clear);
 
-void *
+void __iomem  *
 visorchannel_get_header(VISORCHANNEL *channel)
 {
-	return (void *) &(channel->chan_hdr);
+	return (void __iomem *) &(channel->chan_hdr);
 }
 EXPORT_SYMBOL_GPL(visorchannel_get_header);
 
@@ -558,7 +559,6 @@ visorchannel_debug(VISORCHANNEL *channel, int nQueues,
 	MEMREGION *memregion = NULL;
 	CHANNEL_HEADER hdr;
 	CHANNEL_HEADER *phdr = &hdr;
-	char s[99];
 	int i = 0;
 	int errcode = 0;
 
@@ -588,9 +588,8 @@ visorchannel_debug(VISORCHANNEL *channel, int nQueues,
 	nbytes = (ulong) (phdr->Size);
 	seq_printf(seq, "--- Begin channel @0x%-16.16Lx for 0x%lx bytes (region=0x%lx bytes) ---\n",
 		   addr + off, nbytes, nbytes_region);
-	seq_printf(seq, "Type            = %s\n", GUID_format2(&phdr->Type, s));
-	seq_printf(seq, "ZoneGuid        = %s\n",
-		   GUID_format2(&phdr->ZoneGuid, s));
+	seq_printf(seq, "Type            = %pUL\n", &phdr->Type);
+	seq_printf(seq, "ZoneGuid        = %pUL\n", &phdr->ZoneGuid);
 	seq_printf(seq, "Signature       = 0x%-16.16Lx\n",
 		   (long long) phdr->Signature);
 	seq_printf(seq, "LegacyState     = %lu\n", (ulong) phdr->LegacyState);
diff --git a/drivers/staging/unisys/visorchannel/visorchannel_main.c b/drivers/staging/unisys/visorchannel/visorchannel_main.c
index 482ee0ac1c1..f4be2e62c97 100644
--- a/drivers/staging/unisys/visorchannel/visorchannel_main.c
+++ b/drivers/staging/unisys/visorchannel/visorchannel_main.c
@@ -1,6 +1,6 @@
 /* visorchannel_main.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -22,13 +22,16 @@
 #include "globals.h"
 #include "channel.h"
 #include "visorchannel.h"
-#include "guidutils.h"
+#include <linux/uuid.h>
 
 #define MYDRVNAME "visorchannel"
 
 static int __init
 visorchannel_init(void)
 {
+	if (!unisys_spar_platform)
+		return -ENODEV;
+
 	INFODRV("driver version %s loaded", VERSION);
 	return 0;
 }
diff --git a/drivers/staging/unisys/visorchipset/Makefile b/drivers/staging/unisys/visorchipset/Makefile
index f5e8650e1b0..ead4b9c0271 100644
--- a/drivers/staging/unisys/visorchipset/Makefile
+++ b/drivers/staging/unisys/visorchipset/Makefile
@@ -4,7 +4,7 @@
 
 obj-$(CONFIG_UNISYS_VISORCHIPSET)	+= visorchipset.o
 
-visorchipset-y := visorchipset_main.o controlvm_direct.o file.o filexfer.o \
+visorchipset-y := visorchipset_main.o controlvm_direct.o file.o \
 			parser.o
 
 ccflags-y += -Idrivers/staging/unisys/include
diff --git a/drivers/staging/unisys/visorchipset/controlvm.h b/drivers/staging/unisys/visorchipset/controlvm.h
index 873fa12dfe6..012891c3f21 100644
--- a/drivers/staging/unisys/visorchipset/controlvm.h
+++ b/drivers/staging/unisys/visorchipset/controlvm.h
@@ -1,6 +1,6 @@
 /* controlvm.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/visorchipset/controlvm_direct.c b/drivers/staging/unisys/visorchipset/controlvm_direct.c
index b911ea85c09..cd10e3a2a07 100644
--- a/drivers/staging/unisys/visorchipset/controlvm_direct.c
+++ b/drivers/staging/unisys/visorchipset/controlvm_direct.c
@@ -1,6 +1,6 @@
 /* controlvm_direct.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/visorchipset/file.c b/drivers/staging/unisys/visorchipset/file.c
index e214a115328..fccc4f0c3a4 100644
--- a/drivers/staging/unisys/visorchipset/file.c
+++ b/drivers/staging/unisys/visorchipset/file.c
@@ -1,6 +1,6 @@
 /* file.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/visorchipset/file.h b/drivers/staging/unisys/visorchipset/file.h
index 597282aa9a0..21bb906242e 100644
--- a/drivers/staging/unisys/visorchipset/file.h
+++ b/drivers/staging/unisys/visorchipset/file.h
@@ -1,6 +1,6 @@
 /* file.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/visorchipset/filexfer.c b/drivers/staging/unisys/visorchipset/filexfer.c
deleted file mode 100644
index f950d6e85b5..00000000000
--- a/drivers/staging/unisys/visorchipset/filexfer.c
+++ /dev/null
@@ -1,506 +0,0 @@
-/* filexfer.c
- *
- * Copyright © 2013 - 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.
- */
-
-/* Code here-in is the "glue" that connects controlvm messages with the
- * sparfilexfer driver, which is used to transfer file contents as payload
- * across the controlvm channel.
- */
-
-#include "globals.h"
-#include "controlvm.h"
-#include "visorchipset.h"
-#include "filexfer.h"
-
-#ifdef ENABLE_SPARFILEXFER /* sparfilexfer kernel module enabled in build */
-#include "sparfilexfer.h"
-
-/* Driver-global memory */
-static LIST_HEAD(Request_list);	/* list of struct any_request *, via
-				 * req_list memb */
-
-/* lock for above pool for allocation of any_request structs, and pool
-* name; note that kmem_cache_create requires that we keep the storage
-* for the pool name for the life of the pool
- */
-static DEFINE_SPINLOCK(Request_list_lock);
-
-static struct kmem_cache *Request_memory_pool;
-static const char Request_memory_pool_name[] = "filexfer_request_pool";
-size_t Caller_req_context_bytes = 0;	/* passed to filexfer_constructor() */
-
-/* This structure defines a single controlvm GETFILE conversation, which
- * consists of a single controlvm request message and 1 or more controlvm
- * response messages.
- */
-struct getfile_request {
-	CONTROLVM_MESSAGE_HEADER controlvm_header;
-	atomic_t buffers_in_use;
-	GET_CONTIGUOUS_CONTROLVM_PAYLOAD_FUNC get_contiguous_controlvm_payload;
-	CONTROLVM_RESPOND_WITH_PAYLOAD_FUNC controlvm_respond_with_payload;
-};
-
-/* This structure defines a single controlvm PUTFILE conversation, which
- * consists of a single controlvm request with a filename, and additional
- * controlvm messages with file data.
- */
-struct putfile_request {
-	GET_CONTROLVM_FILEDATA_FUNC get_controlvm_filedata;
-	CONTROLVM_RESPOND_FUNC controlvm_end_putFile;
-};
-
-/* This structure defines a single file transfer operation, which can either
- * be a GETFILE or PUTFILE.
- */
-struct any_request {
-	struct list_head req_list;
-	ulong2 file_request_number;
-	ulong2 data_sequence_number;
-	TRANSMITFILE_DUMP_FUNC dump_func;
-	BOOL is_get;
-	union {
-		struct getfile_request get;
-		struct putfile_request put;
-	};
-	/* Size of caller_context_data will be
-	 * <Caller_req_context_bytes> bytes.  I aligned this because I
-	 * am paranoid about what happens when an arbitrary data
-	 * structure with unknown alignment requirements gets copied
-	 * here.  I want caller_context_data to be aligned to the
-	 * coarsest possible alignment boundary that could be required
-	 * for any user data structure.
-	 */
-	u8 caller_context_data[1] __aligned(sizeof(ulong2));
-};
-
-/*
- * Links the any_request into the global list of allocated requests
- * (<Request_list>).
- */
-static void
-unit_tracking_create(struct list_head *dev_list_link)
-{
-	unsigned long flags;
-	spin_lock_irqsave(&Request_list_lock, flags);
-	list_add(dev_list_link, &Request_list);
-	spin_unlock_irqrestore(&Request_list_lock, flags);
-}
-
-/* Unlinks a any_request from the global list (<Request_list>).
- */
-static void
-unit_tracking_destroy(struct list_head *dev_list_link)
-{
-	unsigned long flags;
-	spin_lock_irqsave(&Request_list_lock, flags);
-	list_del(dev_list_link);
-	spin_unlock_irqrestore(&Request_list_lock, flags);
-}
-
-/* Allocate memory for and return a new any_request struct, and
- * link it to the global list of outstanding requests.
- */
-static struct any_request *
-alloc_request(char *fn, int ln)
-{
-	struct any_request *req = (struct any_request *)
-	    (visorchipset_cache_alloc(Request_memory_pool,
-				      FALSE,
-				      fn, ln));
-	if (!req)
-		return NULL;
-	memset(req, 0, sizeof(struct any_request) + Caller_req_context_bytes);
-	unit_tracking_create(&req->req_list);
-	return req;
-}
-
-/* Book-end for alloc_request().
- */
-static void
-free_request(struct any_request *req, char *fn, int ln)
-{
-	unit_tracking_destroy(&req->req_list);
-	visorchipset_cache_free(Request_memory_pool, req, fn, ln);
-}
-
-/* Constructor for filexfer.o.
- */
-int
-filexfer_constructor(size_t req_context_bytes)
-{
-	int rc = -1;
-
-	Caller_req_context_bytes = req_context_bytes;
-	Request_memory_pool =
-	    kmem_cache_create(Request_memory_pool_name,
-			      sizeof(struct any_request) +
-			      Caller_req_context_bytes,
-			      0, SLAB_HWCACHE_ALIGN, NULL);
-	if (!Request_memory_pool) {
-		LOGERR("failed to alloc Request_memory_pool");
-		rc = -ENOMEM;
-		goto Away;
-	}
-	rc = 0;
-Away:
-	if (rc < 0) {
-		if (Request_memory_pool) {
-			kmem_cache_destroy(Request_memory_pool);
-			Request_memory_pool = NULL;
-		}
-	}
-	return rc;
-}
-
-/* Destructor for filexfer.o.
- */
-void
-filexfer_destructor(void)
-{
-	if (Request_memory_pool) {
-		kmem_cache_destroy(Request_memory_pool);
-		Request_memory_pool = NULL;
-	}
-}
-
-/* This function will obtain an available chunk from the controlvm payload area,
- * store the size in bytes of the chunk in <actual_size>, and return a pointer
- * to the chunk.  The function is passed to the sparfilexfer driver, which calls
- * it whenever payload space is required to copy file data into.
- */
-static void *
-get_empty_bucket_for_getfile_data(void *context,
-				  ulong min_size, ulong max_size,
-				  ulong *actual_size)
-{
-	void *bucket;
-	struct any_request *req = (struct any_request *) context;
-
-	if (!req->is_get) {
-		LOGERR("%s - unexpected call", __func__);
-		return NULL;
-	}
-	bucket = (*req->get.get_contiguous_controlvm_payload)
-	    (min_size, max_size, actual_size);
-	if (bucket != NULL) {
-		atomic_inc(&req->get.buffers_in_use);
-		DBGINF("%s - sent %lu-byte buffer", __func__, *actual_size);
-	}
-	return bucket;
-}
-
-/* This function will send a controlvm response with data in the payload
- * (whose space was obtained with get_empty_bucket_for_getfile_data).  The
- * function is passed to the sparfilexfer driver, which calls it whenever it
- * wants to send file data back across the controlvm channel.
- */
-static int
-send_full_getfile_data_bucket(void *context, void *bucket,
-			      ulong bucket_actual_size, ulong bucket_used_size)
-{
-	struct any_request *req = (struct any_request *) context;
-
-	if (!req->is_get) {
-		LOGERR("%s - unexpected call", __func__);
-		return 0;
-	}
-	DBGINF("sending buffer for %lu/%lu",
-	       bucket_used_size, bucket_actual_size);
-	if (!(*req->get.controlvm_respond_with_payload)
-	    (&req->get.controlvm_header,
-	     req->file_request_number,
-	     req->data_sequence_number++,
-	     0, bucket, bucket_actual_size, bucket_used_size, TRUE))
-		atomic_dec(&req->get.buffers_in_use);
-	return 0;
-}
-
-/* This function will send a controlvm response indicating the end of a
- * GETFILE transfer.  The function is passed to the sparfilexfer driver.
- */
-static void
-send_end_of_getfile_data(void *context, int status)
-{
-	struct any_request *req = (struct any_request *) context;
-	if (!req->is_get) {
-		LOGERR("%s - unexpected call", __func__);
-		return;
-	}
-	LOGINF("status=%d", status);
-	(*req->get.controlvm_respond_with_payload)
-	    (&req->get.controlvm_header,
-	     req->file_request_number,
-	     req->data_sequence_number++, status, NULL, 0, 0, FALSE);
-	free_request(req, __FILE__, __LINE__);
-	module_put(THIS_MODULE);
-}
-
-/* This function supplies data for a PUTFILE transfer.
- * The function is passed to the sparfilexfer driver.
- */
-static int
-get_putfile_data(void *context, void *pbuf, size_t bufsize,
-		 BOOL buf_is_userspace, size_t *bytes_transferred)
-{
-	struct any_request *req = (struct any_request *) context;
-	if (req->is_get) {
-		LOGERR("%s - unexpected call", __func__);
-		return -1;
-	}
-	return (*req->put.get_controlvm_filedata) (&req->caller_context_data[0],
-						   pbuf, bufsize,
-						   buf_is_userspace,
-						   bytes_transferred);
-}
-
-/* This function is called to indicate the end of a PUTFILE transfer.
- * The function is passed to the sparfilexfer driver.
- */
-static void
-end_putfile(void *context, int status)
-{
-	struct any_request *req = (struct any_request *) context;
-	if (req->is_get) {
-		LOGERR("%s - unexpected call", __func__);
-		return;
-	}
-	(*req->put.controlvm_end_putFile) (&req->caller_context_data[0],
-					   status);
-	free_request(req, __FILE__, __LINE__);
-	module_put(THIS_MODULE);
-}
-
-/* Refer to filexfer.h for description. */
-BOOL
-filexfer_getFile(CONTROLVM_MESSAGE_HEADER *msgHdr,
-		 ulong2 file_request_number,
-		 uint uplink_index,
-		 uint disk_index,
-		 char *file_name,
-		 GET_CONTIGUOUS_CONTROLVM_PAYLOAD_FUNC
-		 get_contiguous_controlvm_payload,
-		 CONTROLVM_RESPOND_WITH_PAYLOAD_FUNC
-		 controlvm_respond_with_payload,
-		 TRANSMITFILE_DUMP_FUNC dump_func)
-{
-	BOOL use_count_up = FALSE;
-	BOOL failed = TRUE;
-	struct any_request *req = alloc_request(__FILE__, __LINE__);
-
-	if (!req) {
-		LOGERR("allocation of any_request failed");
-		goto Away;
-	}
-	/* We need to increment this module's use count because we're handing
-	 * off pointers to functions within this module to be used by
-	 * another module.
-	 */
-	__module_get(THIS_MODULE);
-	use_count_up = TRUE;
-	req->is_get = TRUE;
-	req->file_request_number = file_request_number;
-	req->data_sequence_number = 0;
-	req->dump_func = dump_func;
-	req->get.controlvm_header = *msgHdr;
-	atomic_set(&req->get.buffers_in_use, 0);
-	req->get.get_contiguous_controlvm_payload =
-	    get_contiguous_controlvm_payload;
-	req->get.controlvm_respond_with_payload =
-	    controlvm_respond_with_payload;
-	if (sparfilexfer_local2remote(req,	/* context, passed to
-						 * callback funcs */
-				      file_name,
-				      file_request_number,
-				      uplink_index,
-				      disk_index,
-				      get_empty_bucket_for_getfile_data,
-				      send_full_getfile_data_bucket,
-				      send_end_of_getfile_data) < 0) {
-		LOGERR("sparfilexfer_local2remote failed");
-		goto Away;
-	}
-	failed = FALSE;
-Away:
-	if (failed) {
-		if (use_count_up) {
-			module_put(THIS_MODULE);
-			use_count_up = FALSE;
-		}
-		if (req) {
-			free_request(req, __FILE__, __LINE__);
-			req = NULL;
-		}
-		return FALSE;
-	} else {
-		return TRUE;
-		/* success; send callbacks will be called for responses */
-	}
-}
-
-/* Refer to filexfer.h for description. */
-void *
-filexfer_putFile(CONTROLVM_MESSAGE_HEADER *msgHdr,
-		 ulong2 file_request_number,
-		 uint uplink_index,
-		 uint disk_index,
-		 char *file_name,
-		 TRANSMITFILE_INIT_CONTEXT_FUNC init_context,
-		 GET_CONTROLVM_FILEDATA_FUNC get_controlvm_filedata,
-		 CONTROLVM_RESPOND_FUNC controlvm_end_putFile,
-		 TRANSMITFILE_DUMP_FUNC dump_func)
-{
-	BOOL use_count_up = FALSE;
-	BOOL failed = TRUE;
-	struct any_request *req = alloc_request(__FILE__, __LINE__);
-	void *caller_ctx = NULL;
-
-	if (!req) {
-		LOGERR("allocation of any_request failed");
-		goto Away;
-	}
-	caller_ctx = (void *) (&(req->caller_context_data[0]));
-	/* We need to increment this module's use count because we're handing
-	 * off pointers to functions within this module to be used by
-	 * another module.
-	 */
-	__module_get(THIS_MODULE);
-	use_count_up = TRUE;
-	req->is_get = FALSE;
-	req->file_request_number = file_request_number;
-	req->data_sequence_number = 0;
-	req->dump_func = dump_func;
-	req->put.get_controlvm_filedata = get_controlvm_filedata;
-	req->put.controlvm_end_putFile = controlvm_end_putFile;
-	(*init_context) (caller_ctx, msgHdr, file_request_number);
-	if (sparfilexfer_remote2local(req,	/* context, passed to
-						 * callback funcs */
-				      file_name,
-				      file_request_number,
-				      uplink_index,
-				      disk_index,
-				      get_putfile_data, end_putfile) < 0) {
-		LOGERR("sparfilexfer_remote2local failed");
-		goto Away;
-	}
-	failed = FALSE;
-Away:
-	if (failed) {
-		if (use_count_up) {
-			module_put(THIS_MODULE);
-			use_count_up = FALSE;
-		}
-		if (req) {
-			free_request(req, __FILE__, __LINE__);
-			req = NULL;
-		}
-		return NULL;
-	} else {
-		return caller_ctx;
-		/* success; callbacks will be called for responses */
-	}
-}
-
-static void
-dump_get_request(struct seq_file *f, struct getfile_request *getreq)
-{
-	seq_printf(f, "  buffers_in_use=%d\n",
-		   atomic_read(&getreq->buffers_in_use));
-}
-
-static void
-dump_put_request(struct seq_file *f, struct putfile_request *putreq)
-{
-}
-
-static void
-dump_request(struct seq_file *f, struct any_request *req)
-{
-	seq_printf(f, "* %s id=%llu seq=%llu\n",
-		   ((req->is_get) ? "Get" : "Put"),
-		   req->file_request_number, req->data_sequence_number);
-	if (req->is_get)
-		dump_get_request(f, &req->get);
-	else
-		dump_put_request(f, &req->put);
-	if (req->dump_func)
-		(*req->dump_func) (f, &(req->caller_context_data[0]), "  ");
-}
-
-void
-filexfer_dump(struct seq_file *f)
-{
-	ulong flags;
-	struct list_head *entry;
-
-	seq_puts(f, "Outstanding TRANSMIT_FILE requests:\n");
-	spin_lock_irqsave(&Request_list_lock, flags);
-	list_for_each(entry, &Request_list) {
-		struct any_request *req;
-		req = list_entry(entry, struct any_request, req_list);
-		dump_request(f, req);
-	}
-	spin_unlock_irqrestore(&Request_list_lock, flags);
-}
-
-#else				/* ifdef ENABLE_SPARFILEXFER */
-int
-filexfer_constructor(size_t req_context_bytes)
-{
-	return 0;		/* success */
-}
-
-void
-filexfer_destructor(void)
-{
-}
-
-BOOL
-filexfer_getFile(CONTROLVM_MESSAGE_HEADER *msgHdr,
-		 u64 file_request_number,
-		 uint uplink_index,
-		 uint disk_index,
-		 char *file_name,
-		 GET_CONTIGUOUS_CONTROLVM_PAYLOAD_FUNC
-		 get_contiguous_controlvm_payload,
-		 CONTROLVM_RESPOND_WITH_PAYLOAD_FUNC
-		 controlvm_respond_with_payload,
-		 TRANSMITFILE_DUMP_FUNC dump_func)
-{
-	/* since no sparfilexfer module exists to call, we just fail */
-	return FALSE;
-}
-
-void *
-filexfer_putFile(CONTROLVM_MESSAGE_HEADER *msgHdr,
-		 u64 file_request_number,
-		 uint uplink_index,
-		 uint disk_index,
-		 char *file_name,
-		 TRANSMITFILE_INIT_CONTEXT_FUNC init_context,
-		 GET_CONTROLVM_FILEDATA_FUNC get_controlvm_filedata,
-		 CONTROLVM_RESPOND_FUNC controlvm_end_putFile,
-		 TRANSMITFILE_DUMP_FUNC dump_func)
-{
-	/* since no sparfilexfer module exists to call, we just fail */
-	return NULL;
-}
-
-void
-filexfer_dump(struct seq_file *f)
-{
-}
-
-#endif				/* ifdef ENABLE_SPARFILEXFER */
diff --git a/drivers/staging/unisys/visorchipset/filexfer.h b/drivers/staging/unisys/visorchipset/filexfer.h
deleted file mode 100644
index a1bfca69cdc..00000000000
--- a/drivers/staging/unisys/visorchipset/filexfer.h
+++ /dev/null
@@ -1,147 +0,0 @@
-/* filexfer.h
- *
- * Copyright © 2013 - 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.
- */
-
-/* This header file defines the interface that filexfer.c provides to other
- * code in the visorchipset driver.
- */
-
-#ifndef __FILEXFER_H__
-#define __FILEXFER_H__
-
-#include "globals.h"
-#include "controlvmchannel.h"
-#include <linux/seq_file.h>
-
-typedef void *(*GET_CONTIGUOUS_CONTROLVM_PAYLOAD_FUNC) (ulong min_size,
-							ulong max_size,
-							ulong *actual_size);
-
-typedef BOOL
-(*CONTROLVM_RESPOND_WITH_PAYLOAD_FUNC) (CONTROLVM_MESSAGE_HEADER *msgHdr,
-					u64 fileRequestNumber,
-					u64 dataSequenceNumber,
-					int response,
-					void *bucket, ulong payloadChunkSize,
-					ulong payloadUsedBytes, BOOL partial);
-
-typedef void
-(*TRANSMITFILE_INIT_CONTEXT_FUNC)(void *ctx,
-				  const CONTROLVM_MESSAGE_HEADER *hdr,
-				  u64 file_request_number);
-typedef void (*TRANSMITFILE_DUMP_FUNC) (struct seq_file *f, void *ctx,
-					const char *pfx);
-typedef int (*GET_CONTROLVM_FILEDATA_FUNC) (void *ctx,
-					    void *buf, size_t bufsize,
-					    BOOL buf_is_userspace,
-					    size_t *bytes_transferred);
-typedef void (*CONTROLVM_RESPOND_FUNC) (void *ctx, int response);
-
-/* Call once to initialize filexfer.o.
- * req_context_bytes number of bytes the caller needs to keep track of each file
- * transfer conversation.  The <ctx_init_value> passed to filexfer_putFile() is
- * assumed to be this many bytes in size.  Code within filexfer.o will copy this
- * into a dynamically-allocated area, and pass back a pointer to that area in
- * callback functions.
- */
-int filexfer_constructor(size_t req_context_bytes);
-
-/* Call once to clean up filexfer.o */
-void filexfer_destructor(void);
-
-/* Call this to dump diagnostic info about all outstanding getFiles/putFiles */
-void filexfer_dump(struct seq_file *f);
-
-/* Call to transfer a file from the local filesystem (i.e., from the environment
- * where this driver is running) across the controlvm channel to a remote
- * environment.  1 or more controlvm responses will be sent as a result, each
- * of which whose payload contains file data.  Only the last controlvm message
- * will have Flags.partialCompletion==0.
- *
- *   msgHdr      the controlvm message header of the GETFILE request which
- *               we just received
- *   file_request_number  this is all data from the GETFILE request that
- *   uplink_index         define which file is to be transferred
- *   disk_index
- *   file_name
- *   get_contiguous_controlvm_payload  function to call when space is needed
- *                                     in the payload area
- *   controlvm_respond_with_payload    function to call to send each controlvm
- *                                     response containing file data as the
- *                                     payload; returns FALSE only if the
- *				       payload buffer was freed inline
- *   dump_func                         function to dump context data in
- *                                     human-readable format
- *
- *  Returns TRUE iff the file transfer request has been successfully initiated,
- *  or FALSE to indicate failure.
- */
-BOOL
-filexfer_getFile(CONTROLVM_MESSAGE_HEADER *msgHdr,
-		 u64 file_request_number,
-		 uint uplink_index,
-		 uint disk_index,
-		 char *file_name,
-		 GET_CONTIGUOUS_CONTROLVM_PAYLOAD_FUNC
-		 get_contiguous_controlvm_payload,
-		 CONTROLVM_RESPOND_WITH_PAYLOAD_FUNC
-		 controlvm_respond_with_payload,
-		 TRANSMITFILE_DUMP_FUNC dump_func);
-
-/* Call to create a file in the local filesystem (i.e., in the environment
- * where this driver is running) from data received as payload in
- * controlvm channel messages from a remote environment.  1 or more controlvm
- * messages will be received for this transfer, and only the last will have
- * Flags.partialCompletion==0.
- *
- *   msgHdr      the controlvm message header of the PUTFILE request which
- *               we just received
- *   file_request_number  this is all data from the PUTFILE request that
- *   uplink_index         define which file is to be created in the local
- *   disk_index           filesystem
- *   file_name
- *   init_context         function to call to initialize the
- *                        <req_context_bytes>-sized storage area returned by
- *                        this func; note that it would NOT be sufficient to
- *                        allow the caller to initialize this upon return, as
- *                        the the other user-supplied callbacks might have
- *                        already been called by then
- *   get_controlvm_filedata   function to call to obtain more data for the file
- *                            being written; refer to get_controlvm_filedata()
- *                            in visorchipset_main.c for a complete description
- *                            of parameters
- *   controlvm_end_putFile    function to call to indicate that creation of the
- *                            local file has completed;  set <response> to a
- *                            negative value to indicate an error
- *   dump_func                function to dump context data in human-readable
- *                            format
- *
- *  Returns a pointer to a dynamically-allocated storage area of size
- *  <req_context_bytes> which the caller can use, or NULL for error.  The
- *  caller should NEVER free the returned pointer, but should expect to receive
- *  it as the <ctx> argument when callback functions are called.
- */
-void *filexfer_putFile(CONTROLVM_MESSAGE_HEADER *msgHdr,
-		       u64 file_request_number,
-		       uint uplink_index,
-		       uint disk_index,
-		       char *file_name,
-		       TRANSMITFILE_INIT_CONTEXT_FUNC init_context,
-		       GET_CONTROLVM_FILEDATA_FUNC get_controlvm_filedata,
-		       CONTROLVM_RESPOND_FUNC controlvm_end_putFile,
-		       TRANSMITFILE_DUMP_FUNC dump_func);
-
-#endif
diff --git a/drivers/staging/unisys/visorchipset/globals.h b/drivers/staging/unisys/visorchipset/globals.h
index a0e6d4fc03a..0fe14599f18 100644
--- a/drivers/staging/unisys/visorchipset/globals.h
+++ b/drivers/staging/unisys/visorchipset/globals.h
@@ -1,6 +1,6 @@
 /* globals.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/visorchipset/parser.c b/drivers/staging/unisys/visorchipset/parser.c
index b408d415bd8..4274dd2dbbd 100644
--- a/drivers/staging/unisys/visorchipset/parser.c
+++ b/drivers/staging/unisys/visorchipset/parser.c
@@ -1,6 +1,6 @@
 /* parser.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -20,6 +20,7 @@
 #include "controlvmchannel.h"
 #include <linux/ctype.h>
 #include <linux/mm.h>
+#include <linux/uuid.h>
 
 #define MYDRVNAME "visorchipset_parser"
 #define CURRENT_FILE_PC VISOR_CHIPSET_PC_parser_c
@@ -191,7 +192,7 @@ parser_byteStream_get(PARSER_CONTEXT *ctx, ulong *nbytes)
 	return (void *) ctx->data;
 }
 
-GUID
+uuid_le
 parser_id_get(PARSER_CONTEXT *ctx)
 {
 	ULTRA_CONTROLVM_PARAMETERS_HEADER *phdr = NULL;
@@ -199,7 +200,7 @@ parser_id_get(PARSER_CONTEXT *ctx)
 	if (ctx == NULL) {
 		ERRDRV("%s (%s:%d) - no context",
 		       __func__, __FILE__, __LINE__);
-		return Guid0;
+		return NULL_UUID_LE;
 	}
 	phdr = (ULTRA_CONTROLVM_PARAMETERS_HEADER *) (ctx->data);
 	return phdr->Id;
diff --git a/drivers/staging/unisys/visorchipset/parser.h b/drivers/staging/unisys/visorchipset/parser.h
index a0cc50a533c..be85fd68c0c 100644
--- a/drivers/staging/unisys/visorchipset/parser.h
+++ b/drivers/staging/unisys/visorchipset/parser.h
@@ -1,6 +1,6 @@
 /* parser.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -18,6 +18,8 @@
 #ifndef __PARSER_H__
 #define __PARSER_H__
 
+#include <linux/uuid.h>
+
 #include "uniklog.h"
 #include "timskmod.h"
 #include "channel.h"
@@ -37,7 +39,7 @@ PARSER_CONTEXT *parser_init_byteStream(U64 addr, U32 bytes, BOOL isLocal,
 void parser_param_start(PARSER_CONTEXT *ctx, PARSER_WHICH_STRING which_string);
 void *parser_param_get(PARSER_CONTEXT *ctx, char *nam, int namesize);
 void *parser_string_get(PARSER_CONTEXT *ctx);
-GUID parser_id_get(PARSER_CONTEXT *ctx);
+uuid_le parser_id_get(PARSER_CONTEXT *ctx);
 char *parser_simpleString_get(PARSER_CONTEXT *ctx);
 void *parser_byteStream_get(PARSER_CONTEXT *ctx, ulong *nbytes);
 void parser_done(PARSER_CONTEXT *ctx);
diff --git a/drivers/staging/unisys/visorchipset/testing.h b/drivers/staging/unisys/visorchipset/testing.h
index a44f5556cb2..015d502cbb1 100644
--- a/drivers/staging/unisys/visorchipset/testing.h
+++ b/drivers/staging/unisys/visorchipset/testing.h
@@ -1,6 +1,6 @@
 /* testing.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -19,6 +19,7 @@
 #define __VISORCHIPSET_TESTING_H__
 
 #define VISORCHIPSET_TEST_PROC
+#include <linux/uuid.h>
 #include "globals.h"
 #include "controlvmchannel.h"
 
@@ -28,10 +29,10 @@ void test_manufacture_vnic_client_add(void *p);
 void test_manufacture_vnic_client_add_phys(HOSTADDRESS addr);
 void test_manufacture_preamble_messages(void);
 void test_manufacture_device_attach(ulong busNo, ulong devNo);
-void test_manufacture_device_add(ulong busNo, ulong devNo, GUID dataTypeGuid,
+void test_manufacture_device_add(ulong busNo, ulong devNo, uuid_le dataTypeGuid,
 				 void *pChannel);
 void test_manufacture_add_bus(ulong busNo, ulong maxDevices,
-			      GUID id, u8 *name, BOOL isServer);
+			      uuid_le id, u8 *name, BOOL isServer);
 void test_manufacture_device_destroy(ulong busNo, ulong devNo);
 void test_manufacture_bus_destroy(ulong busNo);
 void test_manufacture_detach_externalPort(ulong switchNo, ulong externalPortNo);
diff --git a/drivers/staging/unisys/visorchipset/visorchipset.h b/drivers/staging/unisys/visorchipset/visorchipset.h
index d95825dc541..e01cc7207ba 100644
--- a/drivers/staging/unisys/visorchipset/visorchipset.h
+++ b/drivers/staging/unisys/visorchipset/visorchipset.h
@@ -1,6 +1,6 @@
 /* visorchipset.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -18,6 +18,8 @@
 #ifndef __VISORCHIPSET_H__
 #define __VISORCHIPSET_H__
 
+#include <linux/uuid.h>
+
 #include "timskmod.h"
 #include "channel.h"
 #include "controlvmchannel.h"
@@ -63,8 +65,8 @@ typedef struct {
 	HOSTADDRESS channelAddr;
 	struct InterruptInfo intr;
 	U64 nChannelBytes;
-	GUID channelTypeGuid;
-	GUID channelInstGuid;
+	uuid_le channelTypeGuid;
+	uuid_le channelInstGuid;
 
 } VISORCHIPSET_CHANNEL_INFO;
 
@@ -77,7 +79,7 @@ typedef struct {
 	struct list_head entry;
 	U32 busNo;
 	U32 devNo;
-	GUID devInstGuid;
+	uuid_le devInstGuid;
 	VISORCHIPSET_STATE state;
 	VISORCHIPSET_CHANNEL_INFO chanInfo;
 	U32 Reserved1;		/* CONTROLVM_ID */
@@ -125,7 +127,7 @@ typedef struct {
 	U32 busNo;
 	VISORCHIPSET_STATE state;
 	VISORCHIPSET_CHANNEL_INFO chanInfo;
-	GUID partitionGuid;
+	uuid_le partitionGuid;
 	U64 partitionHandle;
 	U8 *name;		/* UTF8 */
 	U8 *description;	/* UTF8 */
@@ -161,7 +163,7 @@ findbus(struct list_head *list, U32 busNo)
 typedef struct {
 	U32 switchNo;
 	VISORCHIPSET_STATE state;
-	GUID switchTypeGuid;
+	uuid_le switchTypeGuid;
 	U8 *authService1;
 	U8 *authService2;
 	U8 *authService3;
@@ -181,7 +183,7 @@ typedef struct {
 	U32 switchNo;
 	U32 externalPortNo;
 	VISORCHIPSET_STATE state;
-	GUID networkZoneGuid;
+	uuid_le networkZoneGuid;
 	int pdPort;
 	U8 *ip;
 	U8 *ipNetmask;
@@ -224,7 +226,7 @@ typedef struct {
 	void (*device_destroy)(ulong busNo, ulong devNo);
 	void (*device_pause)(ulong busNo, ulong devNo);
 	void (*device_resume)(ulong busNo, ulong devNo);
-	int (*get_channel_info)(GUID typeGuid, ulong *minSize,
+	int (*get_channel_info)(uuid_le typeGuid, ulong *minSize,
 				 ulong *maxSize);
 } VISORCHIPSET_BUSDEV_NOTIFIERS;
 
diff --git a/drivers/staging/unisys/visorchipset/visorchipset_main.c b/drivers/staging/unisys/visorchipset/visorchipset_main.c
index c475e256e34..0a602b9c130 100644
--- a/drivers/staging/unisys/visorchipset/visorchipset_main.c
+++ b/drivers/staging/unisys/visorchipset/visorchipset_main.c
@@ -1,6 +1,6 @@
 /* visorchipset_main.c
  *
- * Copyright � 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -26,14 +26,13 @@
 #include "parser.h"
 #include "uniklog.h"
 #include "uisutils.h"
-#include "guidutils.h"
 #include "controlvmcompletionstatus.h"
 #include "guestlinuxdebug.h"
-#include "filexfer.h"
 
 #include <linux/nls.h>
 #include <linux/netdevice.h>
 #include <linux/platform_device.h>
+#include <linux/uuid.h>
 
 #define CURRENT_FILE_PC VISOR_CHIPSET_PC_visorchipset_main_c
 #define TEST_VNIC_PHYSITF "eth0"	/* physical network itf for
@@ -82,7 +81,7 @@ typedef struct {
 static CONTROLVM_MESSAGE_HEADER g_DiagMsgHdr;
 static CONTROLVM_MESSAGE_HEADER g_ChipSetMsgHdr;
 static CONTROLVM_MESSAGE_HEADER g_DelDumpMsgHdr;
-static const GUID UltraDiagPoolChannelProtocolGuid =
+static const uuid_le UltraDiagPoolChannelProtocolGuid =
 	ULTRA_DIAG_POOL_CHANNEL_PROTOCOL_GUID;
 /* 0xffffff is an invalid Bus/Device number */
 static ulong g_diagpoolBusNo = 0xffffff;
@@ -93,15 +92,12 @@ static CONTROLVM_MESSAGE_PACKET g_DeviceChangeStatePacket;
  * "visorhackbus")
  */
 #define FOR_VISORHACKBUS(channel_type_guid) \
-	((memcmp(&channel_type_guid, &UltraVnicChannelProtocolGuid, \
-		 sizeof(GUID)) == 0) ||				    \
-	 (memcmp(&channel_type_guid, &UltraVhbaChannelProtocolGuid, \
-		 sizeof(GUID)) == 0))
+	(((uuid_le_cmp(channel_type_guid, UltraVnicChannelProtocolGuid) == 0)\
+	|| (uuid_le_cmp(channel_type_guid, UltraVhbaChannelProtocolGuid) == 0)))
 #define FOR_VISORBUS(channel_type_guid) (!(FOR_VISORHACKBUS(channel_type_guid)))
 
 #define is_diagpool_channel(channel_type_guid) \
-	 (memcmp(&channel_type_guid, \
-		 &UltraDiagPoolChannelProtocolGuid, sizeof(GUID)) == 0)
+	 (uuid_le_cmp(channel_type_guid, UltraDiagPoolChannelProtocolGuid) == 0)
 
 typedef enum {
 	PARTPROP_invalid,
@@ -1189,7 +1185,7 @@ bus_configure(CONTROLVM_MESSAGE *inmsg, PARSER_CONTEXT *parser_ctx)
 	parser_param_start(parser_ctx, PARSERSTRING_NAME);
 	pBusInfo->name = parser_string_get(parser_ctx);
 
-	visorchannel_GUID_id(&pBusInfo->partitionGuid, s);
+	visorchannel_uuid_id(&pBusInfo->partitionGuid, s);
 	pBusInfo->procObject =
 	    visor_proc_CreateObject(PartitionType, s, (void *) (pBusInfo));
 	if (pBusInfo->procObject == NULL) {
@@ -2699,6 +2695,9 @@ visorchipset_init(void)
 	struct proc_dir_entry *toolaction_file;
 	struct proc_dir_entry *bootToTool_file;
 
+	if (!unisys_spar_platform)
+		return -ENODEV;
+
 	LOGINF("chipset driver version %s loaded", VERSION);
 	/* process module options */
 	POSTCODE_LINUX_2(DRIVER_ENTRY_PC, POSTCODE_SEVERITY_INFO);
@@ -2773,12 +2772,6 @@ visorchipset_init(void)
 			ProcDir, &parahotplug_proc_fops);
 	memset(&g_DelDumpMsgHdr, 0, sizeof(CONTROLVM_MESSAGE_HEADER));
 
-	if (filexfer_constructor(sizeof(struct putfile_request)) < 0) {
-		ERRDRV("filexfer_constructor failed: (status=-1)\n");
-		POSTCODE_LINUX_2(CHIPSET_INIT_FAILURE_PC, DIAG_SEVERITY_ERR);
-		rc = -1;
-		goto Away;
-	}
 	Putfile_buffer_list_pool =
 	    kmem_cache_create(Putfile_buffer_list_pool_name,
 			      sizeof(struct putfile_buffer_entry),
@@ -2862,7 +2855,6 @@ visorchipset_exit(void)
 		kmem_cache_destroy(Putfile_buffer_list_pool);
 		Putfile_buffer_list_pool = NULL;
 	}
-	filexfer_destructor();
 	if (ControlVmObject) {
 		visor_proc_DestroyObject(ControlVmObject);
 		ControlVmObject = NULL;
diff --git a/drivers/staging/unisys/visorchipset/visorchipset_umode.h b/drivers/staging/unisys/visorchipset/visorchipset_umode.h
index 259e840376a..06ba5b7e425 100644
--- a/drivers/staging/unisys/visorchipset/visorchipset_umode.h
+++ b/drivers/staging/unisys/visorchipset/visorchipset_umode.h
@@ -1,6 +1,6 @@
 /* visorchipset_umode.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/visorutil/charqueue.c b/drivers/staging/unisys/visorutil/charqueue.c
index 0ceede129e8..22241c7b4f7 100644
--- a/drivers/staging/unisys/visorutil/charqueue.c
+++ b/drivers/staging/unisys/visorutil/charqueue.c
@@ -1,6 +1,6 @@
 /* charqueue.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -41,6 +41,7 @@ CHARQUEUE *visor_charqueue_create(ulong nslots)
 {
 	int alloc_size = sizeof(CHARQUEUE) + nslots + 1;
 	CHARQUEUE *cq = kmalloc(alloc_size, GFP_KERNEL|__GFP_NORETRY);
+
 	if (cq == NULL) {
 		ERRDRV("visor_charqueue_create allocation failed (alloc_size=%d)",
 		       alloc_size);
@@ -75,6 +76,7 @@ EXPORT_SYMBOL_GPL(visor_charqueue_enqueue);
 BOOL visor_charqueue_is_empty(CHARQUEUE *charqueue)
 {
 	BOOL b;
+
 	spin_lock(&charqueue->lock);
 	b = IS_EMPTY(charqueue);
 	spin_unlock(&charqueue->lock);
diff --git a/drivers/staging/unisys/visorutil/charqueue.h b/drivers/staging/unisys/visorutil/charqueue.h
index e82ae0bc895..d6f16587a36 100644
--- a/drivers/staging/unisys/visorutil/charqueue.h
+++ b/drivers/staging/unisys/visorutil/charqueue.h
@@ -1,6 +1,6 @@
 /* charqueue.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/visorutil/easyproc.c b/drivers/staging/unisys/visorutil/easyproc.c
index 60b6b83d1b2..3b388494e2a 100644
--- a/drivers/staging/unisys/visorutil/easyproc.c
+++ b/drivers/staging/unisys/visorutil/easyproc.c
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -61,6 +61,7 @@ static struct proc_dir_entry *
 	createProcDir(char *name, struct proc_dir_entry *parent)
 {
 	struct proc_dir_entry *p = proc_mkdir_mode(name, S_IFDIR, parent);
+
 	if (p == NULL)
 		ERRDRV("failed to create /proc directory %s", name);
 	return p;
@@ -196,6 +197,7 @@ void visor_easyproc_InitDevice(struct easyproc_driver_info *pdriver,
 {
 	if ((pdriver->ProcDeviceDir != NULL) && (p->procDevicexDir == NULL)) {
 		char s[29];
+
 		sprintf(s, "%d", devno);
 		p->procDevicexDir = createProcDir(s, pdriver->ProcDeviceDir);
 		p->devno = devno;
@@ -267,6 +269,7 @@ void visor_easyproc_DeInitDevice(struct easyproc_driver_info *pdriver,
 				 struct easyproc_device_info *p, int devno)
 {
 	size_t i;
+
 	for (i = 0; i < ARRAY_SIZE(p->device_property_info); i++) {
 		if (p->device_property_info[i].procEntry != NULL) {
 			struct easyproc_device_property_info *px =
@@ -281,6 +284,7 @@ void visor_easyproc_DeInitDevice(struct easyproc_driver_info *pdriver,
 	}
 	if (p->procDevicexDir != NULL) {
 		char s[29];
+
 		sprintf(s, "%d", devno);
 		remove_proc_entry(s, pdriver->ProcDeviceDir);
 		p->procDevicexDir = NULL;
@@ -334,6 +338,7 @@ static ssize_t proc_write_driver(struct file *file, const char __user *buffer,
 	struct seq_file *seq = (struct seq_file *)file->private_data;
 	struct easyproc_driver_info *p = NULL;
 	char local_buf[256];
+
 	if (seq == NULL)
 		return 0;
 	p = (struct easyproc_driver_info *)(seq->private);
@@ -356,6 +361,7 @@ static ssize_t proc_write_device(struct file *file, const char __user *buffer,
 	struct seq_file *seq = (struct seq_file *)file->private_data;
 	struct easyproc_device_info *p = NULL;
 	char local_buf[256];
+
 	if (seq == NULL)
 		return 0;
 	p = (struct easyproc_device_info *)(seq->private);
diff --git a/drivers/staging/unisys/visorutil/easyproc.h b/drivers/staging/unisys/visorutil/easyproc.h
index 1cef1fd33d5..6ce7d5eb6ad 100644
--- a/drivers/staging/unisys/visorutil/easyproc.h
+++ b/drivers/staging/unisys/visorutil/easyproc.h
@@ -1,6 +1,6 @@
 /* easyproc.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/visorutil/memregion.h b/drivers/staging/unisys/visorutil/memregion.h
index bb122dbeafb..f4a65d2fcf0 100644
--- a/drivers/staging/unisys/visorutil/memregion.h
+++ b/drivers/staging/unisys/visorutil/memregion.h
@@ -1,6 +1,6 @@
 /* memregion.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -38,6 +38,6 @@ HOSTADDRESS visor_memregion_get_physaddr(MEMREGION *memregion);
 ulong visor_memregion_get_nbytes(MEMREGION *memregion);
 void memregion_dump(MEMREGION *memregion, char *s,
 		    ulong off, ulong len, struct seq_file *seq);
-void *visor_memregion_get_pointer(MEMREGION *memregion);
+void __iomem *visor_memregion_get_pointer(MEMREGION *memregion);
 
 #endif
diff --git a/drivers/staging/unisys/visorutil/memregion_direct.c b/drivers/staging/unisys/visorutil/memregion_direct.c
index 2c1061d55ee..28dfba0490f 100644
--- a/drivers/staging/unisys/visorutil/memregion_direct.c
+++ b/drivers/staging/unisys/visorutil/memregion_direct.c
@@ -1,6 +1,6 @@
 /* memregion_direct.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -29,7 +29,7 @@
 struct MEMREGION_Tag {
 	HOSTADDRESS physaddr;
 	ulong nbytes;
-	void *mapped;
+	void __iomem *mapped;
 	BOOL requested;
 	BOOL overlapped;
 };
@@ -93,7 +93,7 @@ visor_memregion_create_overlapped(MEMREGION *parent, ulong offset, ulong nbytes)
 
 	memregion->physaddr = parent->physaddr + offset;
 	memregion->nbytes = nbytes;
-	memregion->mapped = ((u8 *) (parent->mapped)) + offset;
+	memregion->mapped = ((u8 __iomem *) (parent->mapped)) + offset;
 	memregion->requested = FALSE;
 	memregion->overlapped = TRUE;
 	return memregion;
@@ -149,7 +149,7 @@ visor_memregion_get_nbytes(MEMREGION *memregion)
 }
 EXPORT_SYMBOL_GPL(visor_memregion_get_nbytes);
 
-void *
+void __iomem *
 visor_memregion_get_pointer(MEMREGION *memregion)
 {
 	return memregion->mapped;
diff --git a/drivers/staging/unisys/visorutil/periodic_work.c b/drivers/staging/unisys/visorutil/periodic_work.c
index 0670a317468..38a60ce8c27 100644
--- a/drivers/staging/unisys/visorutil/periodic_work.c
+++ b/drivers/staging/unisys/visorutil/periodic_work.c
@@ -1,6 +1,6 @@
 /* periodic_work.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -92,6 +92,7 @@ EXPORT_SYMBOL_GPL(visor_periodic_work_destroy);
 BOOL visor_periodic_work_nextperiod(PERIODIC_WORK *periodic_work)
 {
 	BOOL rc = FALSE;
+
 	write_lock(&periodic_work->lock);
 	if (periodic_work->want_to_stop) {
 		periodic_work->is_scheduled = FALSE;
diff --git a/drivers/staging/unisys/visorutil/procobjecttree.c b/drivers/staging/unisys/visorutil/procobjecttree.c
index 67a19e1c7b0..5c8c95c5151 100644
--- a/drivers/staging/unisys/visorutil/procobjecttree.c
+++ b/drivers/staging/unisys/visorutil/procobjecttree.c
@@ -1,6 +1,6 @@
 /* procobjecttree.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -95,6 +95,7 @@ static struct proc_dir_entry *
 createProcDir(const char *name, struct proc_dir_entry *parent)
 {
 	struct proc_dir_entry *p = proc_mkdir_mode(name, S_IFDIR, parent);
+
 	if (p == NULL)
 		ERRDRV("failed to create /proc directory %s", name);
 	return p;
@@ -197,9 +198,11 @@ void visor_proc_DestroyType(MYPROCTYPE *type)
 		return;
 	if (type->procDirs != NULL) {
 		int i = type->nNames-1;
+
 		while (i >= 0) {
 			if (type->procDirs[i] != NULL) {
 				struct proc_dir_entry *parent = NULL;
+
 				if (i == 0)
 					parent = type->procDirRoot;
 				else
@@ -299,6 +302,7 @@ EXPORT_SYMBOL_GPL(visor_proc_CreateObject);
 void visor_proc_DestroyObject(MYPROCOBJECT *obj)
 {
 	MYPROCTYPE *type = NULL;
+
 	if (obj == NULL)
 		return;
 	type = obj->type;
@@ -306,6 +310,7 @@ void visor_proc_DestroyObject(MYPROCOBJECT *obj)
 		return;
 	if (obj->procDirProperties != NULL) {
 		int i = 0;
+
 		for (i = 0; i < type->nProperties; i++) {
 			if (obj->procDirProperties[i] != NULL) {
 				remove_proc_entry(type->propertyNames[i],
@@ -338,6 +343,7 @@ EXPORT_SYMBOL_GPL(visor_proc_DestroyObject);
 static int seq_show(struct seq_file *seq, void *offset)
 {
 	PROCDIRENTRYCONTEXT *ctx = (PROCDIRENTRYCONTEXT *)(seq->private);
+
 	if (ctx == NULL) {
 		ERRDRV("I don't have a freakin' clue...");
 		return 0;
diff --git a/drivers/staging/unisys/visorutil/visorkmodutils.c b/drivers/staging/unisys/visorutil/visorkmodutils.c
index a7d1e94ca3c..10d77cb6ee9 100644
--- a/drivers/staging/unisys/visorutil/visorkmodutils.c
+++ b/drivers/staging/unisys/visorutil/visorkmodutils.c
@@ -1,6 +1,6 @@
 /* timskmodutils.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -20,6 +20,22 @@
 
 #define MYDRVNAME "timskmodutils"
 
+/* s-Par uses the Intel processor's VT-X features to separate groups of
+ * processors into partitions. The firmware sets the hypervisor bit and
+ * reports an ID in the HV capabilities leaf so that the partition's OS
+ * knows s-Par is present and managing the processors.
+ */
+
+#define UNISYS_SPAR_LEAF_ID 0x40000000
+
+/* The s-Par leaf ID returns "UnisysSpar64" encoded across ebx, ecx, edx */
+#define UNISYS_SPAR_ID_EBX 0x73696e55
+#define UNISYS_SPAR_ID_ECX 0x70537379
+#define UNISYS_SPAR_ID_EDX 0x34367261
+
+int unisys_spar_platform;
+EXPORT_SYMBOL_GPL(unisys_spar_platform);
+
 /** Callers to interfaces that set __GFP_NORETRY flag below
  *  must check for a NULL (error) result as we are telling the
  *  kernel interface that it is okay to fail.
@@ -69,3 +85,42 @@ void visor_seq_file_done_buffer(struct seq_file *m)
 	kfree(m);
 }
 EXPORT_SYMBOL_GPL(visor_seq_file_done_buffer);
+
+static __init uint32_t
+visorutil_spar_detect(void)
+{
+	unsigned int eax, ebx, ecx, edx;
+
+	if (cpu_has_hypervisor) {
+		/* check the ID */
+		cpuid(UNISYS_SPAR_LEAF_ID, &eax, &ebx, &ecx, &edx);
+		return  (ebx == UNISYS_SPAR_ID_EBX) &&
+			(ecx == UNISYS_SPAR_ID_ECX) &&
+			(edx == UNISYS_SPAR_ID_EDX);
+	} else
+		return 0;
+
+}
+
+
+
+
+static __init int
+visorutil_mod_init(void)
+{
+	if (visorutil_spar_detect()) {
+		unisys_spar_platform = TRUE;
+		return 0;
+	} else
+		return -ENODEV;
+}
+
+static __exit void
+visorutil_mod_exit(void)
+{
+}
+
+module_init(visorutil_mod_init);
+module_exit(visorutil_mod_exit);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/usbip/stub_dev.c b/drivers/staging/usbip/stub_dev.c
index de692d7011a..51d0c718873 100644
--- a/drivers/staging/usbip/stub_dev.c
+++ b/drivers/staging/usbip/stub_dev.c
@@ -99,6 +99,7 @@ static ssize_t store_sockfd(struct device *dev, struct device_attribute *attr,
 
 	if (sockfd != -1) {
 		int err;
+
 		dev_info(dev, "stub up\n");
 
 		spin_lock_irq(&sdev->ud.lock);
diff --git a/drivers/staging/usbip/stub_tx.c b/drivers/staging/usbip/stub_tx.c
index 1622563823a..dbcabc9dbe0 100644
--- a/drivers/staging/usbip/stub_tx.c
+++ b/drivers/staging/usbip/stub_tx.c
@@ -179,7 +179,7 @@ static int stub_send_ret_submit(struct stub_device *sdev)
 		else
 			iovnum = 2;
 
-		iov = kzalloc(iovnum * sizeof(struct kvec), GFP_KERNEL);
+		iov = kcalloc(iovnum, sizeof(struct kvec), GFP_KERNEL);
 
 		if (!iov) {
 			usbip_event_add(&sdev->ud, SDEV_EVENT_ERROR_MALLOC);
@@ -218,6 +218,7 @@ static int stub_send_ret_submit(struct stub_device *sdev)
 			 */
 
 			int i;
+
 			for (i = 0; i < urb->number_of_packets; i++) {
 				iov[iovnum].iov_base = urb->transfer_buffer +
 					urb->iso_frame_desc[i].offset;
diff --git a/drivers/staging/usbip/usbip_common.h b/drivers/staging/usbip/usbip_common.h
index f555d834f13..4da3866a037 100644
--- a/drivers/staging/usbip/usbip_common.h
+++ b/drivers/staging/usbip/usbip_common.h
@@ -321,12 +321,14 @@ int usbip_event_happened(struct usbip_device *ud);
 static inline int interface_to_busnum(struct usb_interface *interface)
 {
 	struct usb_device *udev = interface_to_usbdev(interface);
+
 	return udev->bus->busnum;
 }
 
 static inline int interface_to_devnum(struct usb_interface *interface)
 {
 	struct usb_device *udev = interface_to_usbdev(interface);
+
 	return udev->devnum;
 }
 
diff --git a/drivers/staging/usbip/userspace/README b/drivers/staging/usbip/userspace/README
index f528ba462b5..831f49fea3c 100644
--- a/drivers/staging/usbip/userspace/README
+++ b/drivers/staging/usbip/userspace/README
@@ -195,7 +195,6 @@ Detach the imported device:
     - Shutdown firewall.
 	- usbip now uses TCP port 3240.
     - Disable SELinux.
-    - If possible, compile your kernel with CONFIG_USB_DEBUG flag and try again.
     - Check the kernel and daemon messages.
 
 
diff --git a/drivers/staging/usbip/userspace/libsrc/usbip_common.c b/drivers/staging/usbip/userspace/libsrc/usbip_common.c
index 238bf5bf7f4..ac73710473d 100644
--- a/drivers/staging/usbip/userspace/libsrc/usbip_common.c
+++ b/drivers/staging/usbip/userspace/libsrc/usbip_common.c
@@ -75,6 +75,7 @@ const char *usbip_speed_string(int num)
 void dump_usb_interface(struct usbip_usb_interface *uinf)
 {
 	char buff[100];
+
 	usbip_names_get_class(buff, sizeof(buff),
 			uinf->bInterfaceClass,
 			uinf->bInterfaceSubClass,
@@ -86,7 +87,6 @@ void dump_usb_device(struct usbip_usb_device *udev)
 {
 	char buff[100];
 
-
 	dbg("%-20s = %s", "path",  udev->path);
 	dbg("%-20s = %s", "busid", udev->busid);
 
@@ -237,7 +237,7 @@ int usbip_names_init(char *f)
 	return names_init(f);
 }
 
-void usbip_names_free()
+void usbip_names_free(void)
 {
 	names_free();
 }
diff --git a/drivers/staging/usbip/userspace/libsrc/usbip_common.h b/drivers/staging/usbip/userspace/libsrc/usbip_common.h
index 23be848f24f..5a0e95edf4d 100644
--- a/drivers/staging/usbip/userspace/libsrc/usbip_common.h
+++ b/drivers/staging/usbip/userspace/libsrc/usbip_common.h
@@ -47,7 +47,7 @@ extern int usbip_use_debug ;
 
 #define pr_fmt(fmt)	"%s: %s: " fmt "\n", PROGNAME
 #define dbg_fmt(fmt)	pr_fmt("%s:%d:[%s] " fmt), "debug",	\
-		        __FILE__, __LINE__, __FUNCTION__
+		        __FILE__, __LINE__, __func__
 
 #define err(fmt, args...)						\
 	do {								\
diff --git a/drivers/staging/usbip/userspace/libsrc/vhci_driver.c b/drivers/staging/usbip/userspace/libsrc/vhci_driver.c
index 8901fcb7946..ad920477353 100644
--- a/drivers/staging/usbip/userspace/libsrc/vhci_driver.c
+++ b/drivers/staging/usbip/userspace/libsrc/vhci_driver.c
@@ -260,7 +260,7 @@ err:
 }
 
 
-void usbip_vhci_driver_close()
+void usbip_vhci_driver_close(void)
 {
 	if (!vhci_driver)
 		return;
diff --git a/drivers/staging/usbip/userspace/src/usbip_attach.c b/drivers/staging/usbip/userspace/src/usbip_attach.c
index 716a79e284c..d58a14dfc09 100644
--- a/drivers/staging/usbip/userspace/src/usbip_attach.c
+++ b/drivers/staging/usbip/userspace/src/usbip_attach.c
@@ -56,6 +56,7 @@ static int record_connection(char *host, char *port, char *busid, int rhport)
 		/* 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;
diff --git a/drivers/staging/usbip/userspace/src/usbip_port.c b/drivers/staging/usbip/userspace/src/usbip_port.c
index 52aa168b46b..a2e884fd922 100644
--- a/drivers/staging/usbip/userspace/src/usbip_port.c
+++ b/drivers/staging/usbip/userspace/src/usbip_port.c
@@ -16,7 +16,7 @@
 #include "vhci_driver.h"
 #include "usbip_common.h"
 
-static int list_imported_devices()
+static int list_imported_devices(void)
 {
 	int i;
 	struct usbip_imported_device *idev;
diff --git a/drivers/staging/usbip/userspace/src/usbipd.c b/drivers/staging/usbip/userspace/src/usbipd.c
index 2cae4ce48d1..2f87f2d348b 100644
--- a/drivers/staging/usbip/userspace/src/usbipd.c
+++ b/drivers/staging/usbip/userspace/src/usbipd.c
@@ -372,6 +372,7 @@ static int listen_all_addrinfo(struct addrinfo *ai_head, int sockfdlist[],
 
 	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);
@@ -457,11 +458,13 @@ static void set_signal(void)
 
 static const char *pid_file;
 
-static void write_pid_file()
+static void write_pid_file(void)
 {
 	if (pid_file) {
 		dbg("creating pid file %s", pid_file);
-		FILE *fp = fopen(pid_file, "w");
+		FILE *fp;
+
+		fp = fopen(pid_file, "w");
 		if (!fp) {
 			err("pid_file: %s: %d (%s)",
 			    pid_file, errno, strerror(errno));
@@ -472,7 +475,7 @@ static void write_pid_file()
 	}
 }
 
-static void remove_pid_file()
+static void remove_pid_file(void)
 {
 	if (pid_file) {
 		dbg("removing pid file %s", pid_file);
@@ -602,6 +605,7 @@ int main(int argc, char *argv[])
 	int daemonize = 0;
 	int ipv4 = 0, ipv6 = 0;
 	int opt, rc = -1;
+
 	pid_file = NULL;
 
 	usbip_use_stderr = 1;
diff --git a/drivers/staging/usbip/vhci_hcd.c b/drivers/staging/usbip/vhci_hcd.c
index 70e17551943..0007d30e45b 100644
--- a/drivers/staging/usbip/vhci_hcd.c
+++ b/drivers/staging/usbip/vhci_hcd.c
@@ -635,6 +635,7 @@ static int vhci_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
 
 	{
 		int ret = 0;
+
 		ret = usb_hcd_check_unlink_urb(hcd, urb, status);
 		if (ret) {
 			spin_unlock(&the_controller->lock);
@@ -885,6 +886,7 @@ static int vhci_start(struct usb_hcd *hcd)
 
 	for (rhport = 0; rhport < VHCI_NPORTS; rhport++) {
 		struct vhci_device *vdev = &vhci->vdev[rhport];
+
 		vhci_device_init(vdev);
 		vdev->rhport = rhport;
 	}
diff --git a/drivers/staging/vme/devices/vme_user.c b/drivers/staging/vme/devices/vme_user.c
index ffb4eeefddd..2d8497277aa 100644
--- a/drivers/staging/vme/devices/vme_user.c
+++ b/drivers/staging/vme/devices/vme_user.c
@@ -684,7 +684,7 @@ static int vme_user_match(struct vme_dev *vdev)
 static int vme_user_probe(struct vme_dev *vdev)
 {
 	int i, err;
-	char name[12];
+	char *name;
 
 	/* Save pointer to the bridge device */
 	if (vme_user_bridge != NULL) {
@@ -792,15 +792,16 @@ static int vme_user_probe(struct vme_dev *vdev)
 	/* Add sysfs Entries */
 	for (i = 0; i < VME_DEVS; i++) {
 		int num;
+
 		switch (type[i]) {
 		case MASTER_MINOR:
-			sprintf(name, "bus/vme/m%%d");
+			name = "bus/vme/m%d";
 			break;
 		case CONTROL_MINOR:
-			sprintf(name, "bus/vme/ctl");
+			name = "bus/vme/ctl";
 			break;
 		case SLAVE_MINOR:
-			sprintf(name, "bus/vme/s%%d");
+			name = "bus/vme/s%d";
 			break;
 		default:
 			err = -EINVAL;
diff --git a/drivers/staging/vt6655/IEEE11h.c b/drivers/staging/vt6655/IEEE11h.c
index dfda3c82906..6cfad1c0402 100644
--- a/drivers/staging/vt6655/IEEE11h.c
+++ b/drivers/staging/vt6655/IEEE11h.c
@@ -192,8 +192,6 @@ static bool s_bRxTPCReq(PSMgmtObject pMgmt,
 	if (csMgmt_xmit(pMgmt->pAdapter, pTxPacket) != CMD_STATUS_PENDING)
 		return false;
 	return true;
-/*    return CARDbSendPacket(pMgmt->pAdapter, pFrame, PKT_TYPE_802_11_MNG,
-      sizeof(WLAN_FRAME_TPCREP)); */
 }
 
 /*---------------------  Export Variables  --------------------------*/
@@ -274,8 +272,6 @@ IEEE11hbMgrRxAction(void *pMgmtHandle, void *pRxPacket)
 			pAction->byCategory);
 		pAction->byCategory |= 0x80;
 
-		/*return CARDbSendPacket(pMgmt->pAdapter, pAction, PKT_TYPE_802_11_MNG,
-		  uLength);*/
 		return true;
 	}
 	return true;
@@ -317,6 +313,4 @@ bool IEEE11hbMSRRepTx(void *pMgmtHandle)
 	if (csMgmt_xmit(pMgmt->pAdapter, pTxPacket) != CMD_STATUS_PENDING)
 		return false;
 	return true;
-/*    return CARDbSendPacket(pMgmt->pAdapter, pMSRRep, PKT_TYPE_802_11_MNG,
-      uLength); */
 }
diff --git a/drivers/staging/vt6655/baseband.c b/drivers/staging/vt6655/baseband.c
index fa14659ba43..6f95fb618dc 100644
--- a/drivers/staging/vt6655/baseband.c
+++ b/drivers/staging/vt6655/baseband.c
@@ -1095,7 +1095,6 @@ unsigned char byVT3253B0_AIROHA2230[CB_VT3253B0_INIT_FOR_AIROHA2230][2] = {
 	{0x69, 0x00},
 	{0x6a, 0x00},
 	{0x6b, 0x00},
-	//{0x6c, 0x80},
 	{0x6c, 0x00}, //RobertYu:20050125, request by JJSue
 	{0x6d, 0x03},
 	{0x6e, 0x01},
@@ -1357,7 +1356,6 @@ unsigned char byVT3253B0_UW2451[CB_VT3253B0_INIT_FOR_UW2451][2] = {
 	{0x69, 0x00},
 	{0x6a, 0x00},
 	{0x6b, 0x00},
-	//{0x6c, 0x80},
 	{0x6c, 0x00}, //RobertYu:20050125, request by JJSue
 	{0x6d, 0x03},
 	{0x6e, 0x01},
@@ -1786,29 +1784,27 @@ BBuGetFrameTime(
 	uRate = (unsigned int)awcFrameTime[uRateIdx];
 
 	if (uRateIdx <= 3) {          //CCK mode
-
-		if (byPreambleType == 1) {//Short
+		if (byPreambleType == 1) //Short
 			uPreamble = 96;
-		} else {
+		else
 			uPreamble = 192;
-		}
+
 		uFrameTime = (cbFrameLength * 80) / uRate;  //?????
 		uTmp = (uFrameTime * uRate) / 80;
-		if (cbFrameLength != uTmp) {
+		if (cbFrameLength != uTmp)
 			uFrameTime++;
-		}
 
 		return uPreamble + uFrameTime;
 	} else {
 		uFrameTime = (cbFrameLength * 8 + 22) / uRate;   //????????
 		uTmp = ((uFrameTime * uRate) - 22) / 8;
-		if (cbFrameLength != uTmp) {
+		if (cbFrameLength != uTmp)
 			uFrameTime++;
-		}
+
 		uFrameTime = uFrameTime * 4;    //???????
-		if (byPktType != PK_TYPE_11A) {
+		if (byPktType != PK_TYPE_11A)
 			uFrameTime += 6;     //??????
-		}
+
 		return 20 + uFrameTime; //??????
 	}
 }
@@ -2129,16 +2125,16 @@ bool BBbVT3253Init(PSDevice pDevice)
 
 	if (byRFType == RF_RFMD2959) {
 		if (byLocalID <= REV_ID_VT3253_A1) {
-			for (ii = 0; ii < CB_VT3253_INIT_FOR_RFMD; ii++) {
+			for (ii = 0; ii < CB_VT3253_INIT_FOR_RFMD; ii++)
 				bResult &= BBbWriteEmbedded(dwIoBase, byVT3253InitTab_RFMD[ii][0], byVT3253InitTab_RFMD[ii][1]);
-			}
+
 		} else {
-			for (ii = 0; ii < CB_VT3253B0_INIT_FOR_RFMD; ii++) {
+			for (ii = 0; ii < CB_VT3253B0_INIT_FOR_RFMD; ii++)
 				bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_RFMD[ii][0], byVT3253B0_RFMD[ii][1]);
-			}
-			for (ii = 0; ii < CB_VT3253B0_AGC_FOR_RFMD2959; ii++) {
+
+			for (ii = 0; ii < CB_VT3253B0_AGC_FOR_RFMD2959; ii++)
 				bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AGC4_RFMD2959[ii][0], byVT3253B0_AGC4_RFMD2959[ii][1]);
-			}
+
 			VNSvOutPortD(dwIoBase + MAC_REG_ITRTMSET, 0x23);
 			MACvRegBitsOn(dwIoBase, MAC_REG_PAPEDELAY, BIT0);
 		}
@@ -2151,12 +2147,12 @@ bool BBbVT3253Init(PSDevice pDevice)
 		pDevice->ldBmThreshold[2] = 0;
 		pDevice->ldBmThreshold[3] = 0;
 	} else if ((byRFType == RF_AIROHA) || (byRFType == RF_AL2230S)) {
-		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++) {
+		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++)
 			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AIROHA2230[ii][0], byVT3253B0_AIROHA2230[ii][1]);
-		}
-		for (ii = 0; ii < CB_VT3253B0_AGC; ii++) {
+
+		for (ii = 0; ii < CB_VT3253B0_AGC; ii++)
 			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
-		}
+
 		pDevice->abyBBVGA[0] = 0x1C;
 		pDevice->abyBBVGA[1] = 0x10;
 		pDevice->abyBBVGA[2] = 0x0;
@@ -2166,12 +2162,12 @@ bool BBbVT3253Init(PSDevice pDevice)
 		pDevice->ldBmThreshold[2] = 0;
 		pDevice->ldBmThreshold[3] = 0;
 	} else if (byRFType == RF_UW2451) {
-		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_UW2451; ii++) {
+		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_UW2451; ii++)
 			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_UW2451[ii][0], byVT3253B0_UW2451[ii][1]);
-		}
-		for (ii = 0; ii < CB_VT3253B0_AGC; ii++) {
+
+		for (ii = 0; ii < CB_VT3253B0_AGC; ii++)
 			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
-		}
+
 		VNSvOutPortB(dwIoBase + MAC_REG_ITRTMSET, 0x23);
 		MACvRegBitsOn(dwIoBase, MAC_REG_PAPEDELAY, BIT0);
 
@@ -2184,9 +2180,9 @@ bool BBbVT3253Init(PSDevice pDevice)
 		pDevice->ldBmThreshold[2] = 0;
 		pDevice->ldBmThreshold[3] = 0;
 	} else if (byRFType == RF_UW2452) {
-		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_UW2451; ii++) {
+		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_UW2451; ii++)
 			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_UW2451[ii][0], byVT3253B0_UW2451[ii][1]);
-		}
+
 		// Init ANT B select,TX Config CR09 = 0x61->0x45, 0x45->0x41(VC1/VC2 define, make the ANT_A, ANT_B inverted)
 		//bResult &= BBbWriteEmbedded(dwIoBase,0x09,0x41);
 		// Init ANT B select,RX Config CR10 = 0x28->0x2A, 0x2A->0x28(VC1/VC2 define, make the ANT_A, ANT_B inverted)
@@ -2205,11 +2201,8 @@ bool BBbVT3253Init(PSDevice pDevice)
 		//}}
 		bResult &= BBbWriteEmbedded(dwIoBase, 0xb0, 0x58);
 
-		for (ii = 0; ii < CB_VT3253B0_AGC; ii++) {
+		for (ii = 0; ii < CB_VT3253B0_AGC; ii++)
 			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
-		}
-		//VNSvOutPortB(dwIoBase + MAC_REG_ITRTMSET, 0x23); // RobertYu: 20050104, 20050131 disable PA_Delay
-		//MACvRegBitsOn(dwIoBase, MAC_REG_PAPEDELAY, BIT0); // RobertYu: 20050104, 20050131 disable PA_Delay
 
 		pDevice->abyBBVGA[0] = 0x14;
 		pDevice->abyBBVGA[1] = 0x0A;
@@ -2222,12 +2215,12 @@ bool BBbVT3253Init(PSDevice pDevice)
 		//}} RobertYu
 
 	} else if (byRFType == RF_VT3226) {
-		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++) {
+		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++)
 			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AIROHA2230[ii][0], byVT3253B0_AIROHA2230[ii][1]);
-		}
-		for (ii = 0; ii < CB_VT3253B0_AGC; ii++) {
+
+		for (ii = 0; ii < CB_VT3253B0_AGC; ii++)
 			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
-		}
+
 		pDevice->abyBBVGA[0] = 0x1C;
 		pDevice->abyBBVGA[1] = 0x10;
 		pDevice->abyBBVGA[2] = 0x0;
@@ -2240,9 +2233,9 @@ bool BBbVT3253Init(PSDevice pDevice)
 		MACvSetRFLE_LatchBase(dwIoBase);
 		//{{ RobertYu: 20050104
 	} else if (byRFType == RF_AIROHA7230) {
-		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++) {
+		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++)
 			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AIROHA2230[ii][0], byVT3253B0_AIROHA2230[ii][1]);
-		}
+
 
 		//{{ RobertYu:20050223, request by JerryChung
 		// Init ANT B select,TX Config CR09 = 0x61->0x45, 0x45->0x41(VC1/VC2 define, make the ANT_A, ANT_B inverted)
@@ -2253,9 +2246,9 @@ bool BBbVT3253Init(PSDevice pDevice)
 		bResult &= BBbWriteEmbedded(dwIoBase, 0xd7, 0x06);
 		//}}
 
-		for (ii = 0; ii < CB_VT3253B0_AGC; ii++) {
+		for (ii = 0; ii < CB_VT3253B0_AGC; ii++)
 			bResult &= BBbWriteEmbedded(dwIoBase, byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
-		}
+
 		pDevice->abyBBVGA[0] = 0x1C;
 		pDevice->abyBBVGA[1] = 0x10;
 		pDevice->abyBBVGA[2] = 0x0;
@@ -2411,17 +2404,15 @@ BBvSetShortSlotTime(PSDevice pDevice)
 
 	BBbReadEmbedded(pDevice->PortOffset, 0x0A, &byBBRxConf);//CR10
 
-	if (pDevice->bShortSlotTime) {
+	if (pDevice->bShortSlotTime)
 		byBBRxConf &= 0xDF;//1101 1111
-	} else {
+	else
 		byBBRxConf |= 0x20;//0010 0000
-	}
 
 	// patch for 3253B0 Baseband with Cardbus module
 	BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byBBVGA);
-	if (byBBVGA == pDevice->abyBBVGA[0]) {
+	if (byBBVGA == pDevice->abyBBVGA[0])
 		byBBRxConf |= 0x20;//0010 0000
-	}
 
 	BBbWriteEmbedded(pDevice->PortOffset, 0x0A, byBBRxConf);//CR10
 }
@@ -2613,13 +2604,11 @@ s_ulGetRatio(PSDevice pDevice)
 	if (pDevice->uNumSQ3[RATE_54M] != 0) {
 		ulPacketNum = pDevice->uNumSQ3[RATE_54M];
 		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-		//ulRatio = (pDevice->uNumSQ3[RATE_54M] * 1000 / pDevice->uDiversityCnt);
 		ulRatio += TOP_RATE_54M;
 	}
 	if (pDevice->uNumSQ3[RATE_48M] > ulMaxPacket) {
 		ulPacketNum = pDevice->uNumSQ3[RATE_54M] + pDevice->uNumSQ3[RATE_48M];
 		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-		//ulRatio = (pDevice->uNumSQ3[RATE_48M] * 1000 / pDevice->uDiversityCnt);
 		ulRatio += TOP_RATE_48M;
 		ulMaxPacket = pDevice->uNumSQ3[RATE_48M];
 	}
@@ -2627,7 +2616,6 @@ s_ulGetRatio(PSDevice pDevice)
 		ulPacketNum = pDevice->uNumSQ3[RATE_54M] + pDevice->uNumSQ3[RATE_48M] +
 			pDevice->uNumSQ3[RATE_36M];
 		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-		//ulRatio = (pDevice->uNumSQ3[RATE_36M] * 1000 / pDevice->uDiversityCnt);
 		ulRatio += TOP_RATE_36M;
 		ulMaxPacket = pDevice->uNumSQ3[RATE_36M];
 	}
@@ -2635,7 +2623,6 @@ s_ulGetRatio(PSDevice pDevice)
 		ulPacketNum = pDevice->uNumSQ3[RATE_54M] + pDevice->uNumSQ3[RATE_48M] +
 			pDevice->uNumSQ3[RATE_36M] + pDevice->uNumSQ3[RATE_24M];
 		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-		//ulRatio = (pDevice->uNumSQ3[RATE_24M] * 1000 / pDevice->uDiversityCnt);
 		ulRatio += TOP_RATE_24M;
 		ulMaxPacket = pDevice->uNumSQ3[RATE_24M];
 	}
@@ -2644,7 +2631,6 @@ s_ulGetRatio(PSDevice pDevice)
 			pDevice->uNumSQ3[RATE_36M] + pDevice->uNumSQ3[RATE_24M] +
 			pDevice->uNumSQ3[RATE_18M];
 		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-		//ulRatio = (pDevice->uNumSQ3[RATE_18M] * 1000 / pDevice->uDiversityCnt);
 		ulRatio += TOP_RATE_18M;
 		ulMaxPacket = pDevice->uNumSQ3[RATE_18M];
 	}
@@ -2653,7 +2639,6 @@ s_ulGetRatio(PSDevice pDevice)
 			pDevice->uNumSQ3[RATE_36M] + pDevice->uNumSQ3[RATE_24M] +
 			pDevice->uNumSQ3[RATE_18M] + pDevice->uNumSQ3[RATE_12M];
 		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-		//ulRatio = (pDevice->uNumSQ3[RATE_12M] * 1000 / pDevice->uDiversityCnt);
 		ulRatio += TOP_RATE_12M;
 		ulMaxPacket = pDevice->uNumSQ3[RATE_12M];
 	}
@@ -2662,7 +2647,6 @@ s_ulGetRatio(PSDevice pDevice)
 			pDevice->uNumSQ3[RATE_2M] - pDevice->uNumSQ3[RATE_5M] -
 			pDevice->uNumSQ3[RATE_6M] - pDevice->uNumSQ3[RATE_9M];
 		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-		//ulRatio = (pDevice->uNumSQ3[RATE_11M] * 1000 / pDevice->uDiversityCnt);
 		ulRatio += TOP_RATE_11M;
 		ulMaxPacket = pDevice->uNumSQ3[RATE_11M];
 	}
@@ -2671,7 +2655,6 @@ s_ulGetRatio(PSDevice pDevice)
 			pDevice->uNumSQ3[RATE_2M] - pDevice->uNumSQ3[RATE_5M] -
 			pDevice->uNumSQ3[RATE_6M];
 		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-		//ulRatio = (pDevice->uNumSQ3[RATE_9M] * 1000 / pDevice->uDiversityCnt);
 		ulRatio += TOP_RATE_9M;
 		ulMaxPacket = pDevice->uNumSQ3[RATE_9M];
 	}
@@ -2679,7 +2662,6 @@ s_ulGetRatio(PSDevice pDevice)
 		ulPacketNum = pDevice->uDiversityCnt - pDevice->uNumSQ3[RATE_1M] -
 			pDevice->uNumSQ3[RATE_2M] - pDevice->uNumSQ3[RATE_5M];
 		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-		//ulRatio = (pDevice->uNumSQ3[RATE_6M] * 1000 / pDevice->uDiversityCnt);
 		ulRatio += TOP_RATE_6M;
 		ulMaxPacket = pDevice->uNumSQ3[RATE_6M];
 	}
@@ -2687,21 +2669,18 @@ s_ulGetRatio(PSDevice pDevice)
 		ulPacketNum = pDevice->uDiversityCnt - pDevice->uNumSQ3[RATE_1M] -
 			pDevice->uNumSQ3[RATE_2M];
 		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-		//ulRatio = (pDevice->uNumSQ3[RATE_5M] * 1000 / pDevice->uDiversityCnt);
 		ulRatio += TOP_RATE_55M;
 		ulMaxPacket = pDevice->uNumSQ3[RATE_5M];
 	}
 	if (pDevice->uNumSQ3[RATE_2M] > ulMaxPacket) {
 		ulPacketNum = pDevice->uDiversityCnt - pDevice->uNumSQ3[RATE_1M];
 		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-		//ulRatio = (pDevice->uNumSQ3[RATE_2M]  * 1000 / pDevice->uDiversityCnt);
 		ulRatio += TOP_RATE_2M;
 		ulMaxPacket = pDevice->uNumSQ3[RATE_2M];
 	}
 	if (pDevice->uNumSQ3[RATE_1M] > ulMaxPacket) {
 		ulPacketNum = pDevice->uDiversityCnt;
 		ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-		//ulRatio = (pDevice->uNumSQ3[RATE_1M]  * 1000 / pDevice->uDiversityCnt);
 		ulRatio += TOP_RATE_1M;
 	}
 
@@ -2714,9 +2693,8 @@ BBvClearAntDivSQ3Value(PSDevice pDevice)
 	unsigned int ii;
 
 	pDevice->uDiversityCnt = 0;
-	for (ii = 0; ii < MAX_RATE; ii++) {
+	for (ii = 0; ii < MAX_RATE; ii++)
 		pDevice->uNumSQ3[ii] = 0;
-	}
 }
 
 /*
@@ -2737,11 +2715,10 @@ BBvClearAntDivSQ3Value(PSDevice pDevice)
 void
 BBvAntennaDiversity(PSDevice pDevice, unsigned char byRxRate, unsigned char bySQ3)
 {
-	if ((byRxRate >= MAX_RATE) || (pDevice->wAntDiversityMaxRate >= MAX_RATE)) {
+	if ((byRxRate >= MAX_RATE) || (pDevice->wAntDiversityMaxRate >= MAX_RATE))
 		return;
-	}
+
 	pDevice->uDiversityCnt++;
-	// DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->uDiversityCnt = %d\n", (int)pDevice->uDiversityCnt);
 
 	pDevice->uNumSQ3[byRxRate]++;
 
diff --git a/drivers/staging/vt6655/bssdb.c b/drivers/staging/vt6655/bssdb.c
index d7efd0173a9..59679cd4681 100644
--- a/drivers/staging/vt6655/bssdb.c
+++ b/drivers/staging/vt6655/bssdb.c
@@ -497,9 +497,8 @@ BSSbInsertToBSSList(
 		}
 	}
 
-	if (bParsingQuiet && (pQuiet != NULL)) {
+	if (bParsingQuiet && (pQuiet != NULL))
 		CARDbStartQuiet(pMgmt->pAdapter);
-	}
 
 	pBSSList->uIELength = uIELength;
 	if (pBSSList->uIELength > WLAN_BEACON_FR_MAXLEN)
@@ -662,9 +661,8 @@ BSSbUpdateToBSSList(
 		}
 	}
 
-	if (bParsingQuiet && (pQuiet != NULL)) {
+	if (bParsingQuiet && (pQuiet != NULL))
 		CARDbStartQuiet(pMgmt->pAdapter);
-	}
 
 	pBSSList->uIELength = uIELength;
 	if (pBSSList->uIELength > WLAN_BEACON_FR_MAXLEN)
diff --git a/drivers/staging/vt6655/bssdb.h b/drivers/staging/vt6655/bssdb.h
index 5c77677251e..a0938b7dd08 100644
--- a/drivers/staging/vt6655/bssdb.h
+++ b/drivers/staging/vt6655/bssdb.h
@@ -112,7 +112,6 @@ typedef struct tagKnownBSS {
 	unsigned char abySSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
 	unsigned char byRxRate;
 
-//    unsigned short wATIMWindow;
 	unsigned char byRSSIStatCnt;
 	long            ldBmMAX;
 	long            ldBmAverage[RSSI_STAT_COUNT];
@@ -147,7 +146,6 @@ typedef struct tagKnownBSS {
 
 	// Clear count
 	unsigned int	uClearCount;
-//    unsigned char abyIEs[WLAN_BEACON_FR_MAXLEN];
 	unsigned int	uIELength;
 	QWORD           qwBSSTimestamp;
 	QWORD           qwLocalTSF;     // local TSF timer
diff --git a/drivers/staging/vt6655/card.c b/drivers/staging/vt6655/card.c
index db38ca05113..05bf48a24f4 100644
--- a/drivers/staging/vt6655/card.c
+++ b/drivers/staging/vt6655/card.c
@@ -24,7 +24,6 @@
  *      vUpdateIFS - Update slotTime,SIFS,DIFS, and EIFS
  *      CARDvUpdateBasicTopRate - Update BasicTopRate
  *      CARDbAddBasicRate - Add to BasicRateSet
- *      CARDbSetBasicRate - Set Basic Tx Rate
  *      CARDbIsOFDMinBasicRate - Check if any OFDM rate is in BasicRateSet
  *      CARDvSetLoopbackMode - Set Loopback mode
  *      CARDbSoftwareReset - Sortware reset NIC
@@ -60,7 +59,6 @@
 
 /*---------------------  Static Definitions -------------------------*/
 
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 static int msglevel = MSG_LEVEL_INFO;
 
 #define C_SIFS_A        16      // micro sec.
@@ -339,36 +337,6 @@ s_vSetRSPINF(PSDevice pDevice, CARD_PHY_TYPE ePHYType, void *pvSupportRateIEs, v
 /*---------------------  Export Functions  --------------------------*/
 
 /*
- * Description: Card Send packet function
- *
- * Parameters:
- *  In:
- *      pDeviceHandler      - The adapter to be set
- *      pPacket             - Packet buffer pointer
- *      ePktType            - Packet type
- *      uLength             - Packet length
- *  Out:
- *      none
- *
- * Return Value: true if succeeded; false if failed.
- *
- */
-/*
-  bool CARDbSendPacket (void *pDeviceHandler, void *pPacket, CARD_PKT_TYPE ePktType, unsigned int uLength) {
-  PSDevice    pDevice = (PSDevice) pDeviceHandler;
-  if (ePktType == PKT_TYPE_802_11_MNG) {
-  return TXbTD0Send(pDevice, pPacket, uLength);
-  } else if (ePktType == PKT_TYPE_802_11_BCN) {
-  return TXbBeaconSend(pDevice, pPacket, uLength);
-  } if (ePktType == PKT_TYPE_802_11_DATA) {
-  return TXbTD1Send(pDevice, pPacket, uLength);
-  }
-
-  return true;
-  }
-*/
-
-/*
  * Description: Get Card short preamble option value
  *
  * Parameters:
@@ -383,9 +351,9 @@ s_vSetRSPINF(PSDevice pDevice, CARD_PHY_TYPE ePHYType, void *pvSupportRateIEs, v
 bool CARDbIsShortPreamble(void *pDeviceHandler)
 {
 	PSDevice    pDevice = (PSDevice) pDeviceHandler;
-	if (pDevice->byPreambleType == 0) {
+	if (pDevice->byPreambleType == 0)
 		return false;
-	}
+
 	return true;
 }
 
@@ -427,15 +395,14 @@ bool CARDbSetPhyParameter(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, unsigned
 	unsigned char bySIFS = 0;
 	unsigned char byDIFS = 0;
 	unsigned char byData;
-//    PWLAN_IE_SUPP_RATES pRates = NULL;
 	PWLAN_IE_SUPP_RATES pSupportRates = (PWLAN_IE_SUPP_RATES) pvSupportRateIEs;
 	PWLAN_IE_SUPP_RATES pExtSupportRates = (PWLAN_IE_SUPP_RATES) pvExtSupportRateIEs;
 
 	//Set SIFS, DIFS, EIFS, SlotTime, CwMin
 	if (ePHYType == PHY_TYPE_11A) {
-		if (pSupportRates == NULL) {
+		if (pSupportRates == NULL)
 			pSupportRates = (PWLAN_IE_SUPP_RATES) abyDefaultSuppRatesA;
-		}
+
 		if (pDevice->byRFType == RF_AIROHA7230) {
 			// AL7230 use single PAPE and connect to PAPE_2.4G
 			MACvSetBBType(pDevice->PortOffset, BB_TYPE_11G);
@@ -443,9 +410,9 @@ bool CARDbSetPhyParameter(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, unsigned
 			pDevice->abyBBVGA[2] = 0x10;
 			pDevice->abyBBVGA[3] = 0x10;
 			BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
-			if (byData == 0x1C) {
+			if (byData == 0x1C)
 				BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
-			}
+
 		} else if (pDevice->byRFType == RF_UW2452) {
 			MACvSetBBType(pDevice->PortOffset, BB_TYPE_11A);
 			pDevice->abyBBVGA[0] = 0x18;
@@ -463,18 +430,18 @@ bool CARDbSetPhyParameter(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, unsigned
 		byDIFS = C_SIFS_A + 2*C_SLOT_SHORT;
 		byCWMaxMin = 0xA4;
 	} else if (ePHYType == PHY_TYPE_11B) {
-		if (pSupportRates == NULL) {
+		if (pSupportRates == NULL)
 			pSupportRates = (PWLAN_IE_SUPP_RATES) abyDefaultSuppRatesB;
-		}
+
 		MACvSetBBType(pDevice->PortOffset, BB_TYPE_11B);
 		if (pDevice->byRFType == RF_AIROHA7230) {
 			pDevice->abyBBVGA[0] = 0x1C;
 			pDevice->abyBBVGA[2] = 0x00;
 			pDevice->abyBBVGA[3] = 0x00;
 			BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
-			if (byData == 0x20) {
+			if (byData == 0x20)
 				BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
-			}
+
 		} else if (pDevice->byRFType == RF_UW2452) {
 			pDevice->abyBBVGA[0] = 0x14;
 			BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
@@ -499,9 +466,9 @@ bool CARDbSetPhyParameter(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, unsigned
 			pDevice->abyBBVGA[2] = 0x00;
 			pDevice->abyBBVGA[3] = 0x00;
 			BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
-			if (byData == 0x20) {
+			if (byData == 0x20)
 				BBbWriteEmbedded(pDevice->PortOffset, 0xE7, pDevice->abyBBVGA[0]);
-			}
+
 		} else if (pDevice->byRFType == RF_UW2452) {
 			pDevice->abyBBVGA[0] = 0x14;
 			BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
@@ -519,26 +486,25 @@ bool CARDbSetPhyParameter(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, unsigned
 			bySlot = C_SLOT_LONG;
 			byDIFS = C_SIFS_BG + 2*C_SLOT_LONG;
 		}
-		if (VNTWIFIbyGetMaxSupportRate(pSupportRates, pExtSupportRates) > RATE_11M) {
+		if (VNTWIFIbyGetMaxSupportRate(pSupportRates, pExtSupportRates) > RATE_11M)
 			byCWMaxMin = 0xA4;
-		} else {
+		else
 			byCWMaxMin = 0xA5;
-		}
+
 		if (pDevice->bProtectMode != VNTWIFIbIsProtectMode(byERPField)) {
 			pDevice->bProtectMode = VNTWIFIbIsProtectMode(byERPField);
-			if (pDevice->bProtectMode) {
+			if (pDevice->bProtectMode)
 				MACvEnableProtectMD(pDevice->PortOffset);
-			} else {
+			else
 				MACvDisableProtectMD(pDevice->PortOffset);
-			}
+
 		}
 		if (pDevice->bBarkerPreambleMd != VNTWIFIbIsBarkerMode(byERPField)) {
 			pDevice->bBarkerPreambleMd = VNTWIFIbIsBarkerMode(byERPField);
-			if (pDevice->bBarkerPreambleMd) {
+			if (pDevice->bBarkerPreambleMd)
 				MACvEnableBarkerPreambleMd(pDevice->PortOffset);
-			} else {
+			else
 				MACvDisableBarkerPreambleMd(pDevice->PortOffset);
-			}
 		}
 	}
 
@@ -567,22 +533,22 @@ bool CARDbSetPhyParameter(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, unsigned
 	if (pDevice->bySlot != bySlot) {
 		pDevice->bySlot = bySlot;
 		VNSvOutPortB(pDevice->PortOffset + MAC_REG_SLOT, pDevice->bySlot);
-		if (pDevice->bySlot == C_SLOT_SHORT) {
+		if (pDevice->bySlot == C_SLOT_SHORT)
 			pDevice->bShortSlotTime = true;
-		} else {
+		else
 			pDevice->bShortSlotTime = false;
-		}
+
 		BBvSetShortSlotTime(pDevice);
 	}
 	if (pDevice->byCWMaxMin != byCWMaxMin) {
 		pDevice->byCWMaxMin = byCWMaxMin;
 		VNSvOutPortB(pDevice->PortOffset + MAC_REG_CWMAXMIN0, pDevice->byCWMaxMin);
 	}
-	if (VNTWIFIbIsShortPreamble(wCapInfo)) {
+	if (VNTWIFIbIsShortPreamble(wCapInfo))
 		pDevice->byPreambleType = pDevice->byShortPreamble;
-	} else {
+	else
 		pDevice->byPreambleType = 0;
-	}
+
 	s_vSetRSPINF(pDevice, ePHYType, pSupportRates, pExtSupportRates);
 	pDevice->eCurrentPHYType = ePHYType;
 	// set for NDIS OID_802_11SUPPORTED_RATES
@@ -662,9 +628,9 @@ bool CARDbSetBeaconPeriod(void *pDeviceHandler, unsigned short wBeaconInterval)
 	uLowRemain = uBeaconInterval - uLowRemain;
 
 	// check if carry when add one beacon interval
-	if ((~uLowNextTBTT) < uLowRemain) {
+	if ((~uLowNextTBTT) < uLowRemain)
 		HIDWORD(qwNextTBTT)++;
-	}
+
 	LODWORD(qwNextTBTT) = uLowNextTBTT + uLowRemain;
 
 	// set HW beacon interval
@@ -720,15 +686,13 @@ bool CARDbStopTxPacket(void *pDeviceHandler, CARD_PKT_TYPE ePktType)
 	}
 	// wait all TD0 complete
 	if (pDevice->bStopTx0Pkt == true) {
-		if (pDevice->iTDUsed[TYPE_TXDMA0] != 0) {
+		if (pDevice->iTDUsed[TYPE_TXDMA0] != 0)
 			return false;
-		}
 	}
 	// wait all Data TD complete
 	if (pDevice->bStopDataPkt == true) {
-		if (pDevice->iTDUsed[TYPE_AC0DMA] != 0) {
+		if (pDevice->iTDUsed[TYPE_AC0DMA] != 0)
 			return false;
-		}
 	}
 
 	return true;
@@ -792,16 +756,16 @@ bool CARDbSetBSSID(void *pDeviceHandler, unsigned char *pbyBSSID, CARD_OP_MODE e
 
 	MACvWriteBSSIDAddress(pDevice->PortOffset, pbyBSSID);
 	memcpy(pDevice->abyBSSID, pbyBSSID, WLAN_BSSID_LEN);
-	if (eOPMode == OP_MODE_ADHOC) {
+	if (eOPMode == OP_MODE_ADHOC)
 		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_ADHOC);
-	} else {
+	else
 		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_ADHOC);
-	}
-	if (eOPMode == OP_MODE_AP) {
+
+	if (eOPMode == OP_MODE_AP)
 		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_AP);
-	} else {
+	else
 		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_AP);
-	}
+
 	if (eOPMode == OP_MODE_UNKNOWN) {
 		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_RCR, RCR_BSSID);
 		pDevice->bBSSIDFilter = false;
@@ -1044,31 +1008,31 @@ CARDbAdd_PMKID_Candidate(
 		memset(&pDevice->gsPMKIDCandidate, 0, sizeof(SPMKIDCandidateEvent));
 	}
 
-	for (ii = 0; ii < 6; ii++) {
+	for (ii = 0; ii < 6; ii++)
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02X ", *(pbyBSSID + ii));
-	}
+
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
 
 	// Update Old Candidate
 	for (ii = 0; ii < pDevice->gsPMKIDCandidate.NumCandidates; ii++) {
 		pCandidateList = &pDevice->gsPMKIDCandidate.CandidateList[ii];
 		if (!memcmp(pCandidateList->BSSID, pbyBSSID, ETH_ALEN)) {
-			if (bRSNCapExist && (wRSNCap & BIT0)) {
+			if (bRSNCapExist && (wRSNCap & BIT0))
 				pCandidateList->Flags |= NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED;
-			} else {
+			else
 				pCandidateList->Flags &= ~(NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED);
-			}
+
 			return true;
 		}
 	}
 
 	// New Candidate
 	pCandidateList = &pDevice->gsPMKIDCandidate.CandidateList[pDevice->gsPMKIDCandidate.NumCandidates];
-	if (bRSNCapExist && (wRSNCap & BIT0)) {
+	if (bRSNCapExist && (wRSNCap & BIT0))
 		pCandidateList->Flags |= NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED;
-	} else {
+	else
 		pCandidateList->Flags &= ~(NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED);
-	}
+
 	memcpy(pCandidateList->BSSID, pbyBSSID, ETH_ALEN);
 	pDevice->gsPMKIDCandidate.NumCandidates++;
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "NumCandidates:%d\n", (int)pDevice->gsPMKIDCandidate.NumCandidates);
@@ -1149,16 +1113,16 @@ CARDbStartMeasure(
 				// start immediately by setting start TSF == current TSF + 2 TU
 				LODWORD(qwStartTSF) = LODWORD(qwCurrTSF) + 2048;
 				HIDWORD(qwStartTSF) = HIDWORD(qwCurrTSF);
-				if (LODWORD(qwCurrTSF) > LODWORD(qwStartTSF)) {
+				if (LODWORD(qwCurrTSF) > LODWORD(qwStartTSF))
 					HIDWORD(qwStartTSF)++;
-				}
+
 				bExpired = false;
 				break;
 			} else {
 				// start at setting start TSF - 1TU(for channel switching)
-				if (LODWORD(qwStartTSF) < 1024) {
+				if (LODWORD(qwStartTSF) < 1024)
 					HIDWORD(qwStartTSF)--;
-				}
+
 				LODWORD(qwStartTSF) -= 1024;
 			}
 
@@ -1247,9 +1211,9 @@ CARDbChannelSwitch(
 	pDevice->byChannelSwitchCount = byCount;
 	pDevice->byNewChannel = byNewChannel;
 	pDevice->bChannelSwitch = true;
-	if (byMode == 1) {
+	if (byMode == 1)
 		bResult = CARDbStopTxPacket(pDevice, PKT_TYPE_802_11_ALL);
-	}
+
 	return bResult;
 }
 
@@ -1282,9 +1246,9 @@ CARDbSetQuiet(
 
 	if (bResetQuiet) {
 		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_MSRCTL, (MSRCTL_QUIETTXCHK | MSRCTL_QUIETEN));
-		for (ii = 0; ii < MAX_QUIET_COUNT; ii++) {
+		for (ii = 0; ii < MAX_QUIET_COUNT; ii++)
 			pDevice->sQuiet[ii].bEnable = false;
-		}
+
 		pDevice->uQuietEnqueue = 0;
 		pDevice->bEnableFirstQuiet = false;
 		pDevice->bQuietEnable = false;
@@ -1299,11 +1263,8 @@ CARDbSetQuiet(
 		pDevice->sQuiet[pDevice->uQuietEnqueue].dwStartTime += wQuietOffset;
 		pDevice->uQuietEnqueue++;
 		pDevice->uQuietEnqueue %= MAX_QUIET_COUNT;
-		if (pDevice->byQuietStartCount < byQuietCount) {
+		if (pDevice->byQuietStartCount < byQuietCount)
 			pDevice->byQuietStartCount = byQuietCount;
-		}
-	} else {
-		// we can not handle Quiet EID more
 	}
 	return true;
 }
@@ -1399,9 +1360,9 @@ CARDbStartQuiet(
 		if (pDevice->dwCurrentQuietEndTime > 0x80010000) {
 			// decreament all time to avoid wrap around
 			for (ii = 0; ii < MAX_QUIET_COUNT; ii++) {
-				if (pDevice->sQuiet[ii].bEnable == true) {
+				if (pDevice->sQuiet[ii].bEnable == true)
 					pDevice->sQuiet[ii].dwStartTime -= 0x80000000;
-				}
+
 			}
 			pDevice->dwCurrentQuietEndTime -= 0x80000000;
 		}
@@ -1433,13 +1394,13 @@ CARDvSetPowerConstraint(
 	PSDevice    pDevice = (PSDevice) pDeviceHandler;
 
 	if (byChannel > CB_MAX_CHANNEL_24G) {
-		if (pDevice->bCountryInfo5G == true) {
+		if (pDevice->bCountryInfo5G == true)
 			pDevice->abyLocalPwr[byChannel] = pDevice->abyRegPwr[byChannel] - byPower;
-		}
+
 	} else {
-		if (pDevice->bCountryInfo24G == true) {
+		if (pDevice->bCountryInfo24G == true)
 			pDevice->abyLocalPwr[byChannel] = pDevice->abyRegPwr[byChannel] - byPower;
-		}
+
 	}
 }
 
@@ -1618,9 +1579,9 @@ unsigned short CARDwGetCCKControlRate(void *pDeviceHandler, unsigned short wRate
 	unsigned int ui = (unsigned int) wRateIdx;
 
 	while (ui > RATE_1M) {
-		if (pDevice->wBasicRate & ((unsigned short)1 << ui)) {
+		if (pDevice->wBasicRate & ((unsigned short)1 << ui))
 			return (unsigned short)ui;
-		}
+
 		ui--;
 	}
 	return (unsigned short)RATE_1M;
@@ -1820,11 +1781,11 @@ void vUpdateIFS(void *pDeviceHandler)
 		byMaxMin = 5;
 	} else { // PK_TYPE_11GA & PK_TYPE_11GB
 		pDevice->uSIFS = C_SIFS_BG;
-		if (pDevice->bShortSlotTime) {
+		if (pDevice->bShortSlotTime)
 			pDevice->uSlot = C_SLOT_SHORT;
-		} else {
+		else
 			pDevice->uSlot = C_SLOT_LONG;
-		}
+
 		pDevice->uDIFS = C_SIFS_BG + 2*pDevice->uSlot;
 		if (pDevice->wBasicRate & 0x0150) { //0000 0001 0101 0000,24M,12M,6M
 			pDevice->uCwMin = C_CWMIN_A;
@@ -1877,19 +1838,6 @@ void CARDvUpdateBasicTopRate(void *pDeviceHandler)
 	pDevice->byTopCCKBasicRate = byTopCCK;
 }
 
-/*
- * Description: Set NIC Tx Basic Rate
- *
- * Parameters:
- *  In:
- *      pDevice         - The adapter to be set
- *      wBasicRate      - Basic Rate to be set
- *  Out:
- *      none
- *
- * Return Value: true if succeeded; false if failed.
- *
- */
 bool CARDbAddBasicRate(void *pDeviceHandler, unsigned short wRateIdx)
 {
 	PSDevice pDevice = (PSDevice) pDeviceHandler;
@@ -1919,13 +1867,12 @@ unsigned char CARDbyGetPktType(void *pDeviceHandler)
 {
 	PSDevice pDevice = (PSDevice) pDeviceHandler;
 
-	if (pDevice->byBBType == BB_TYPE_11A || pDevice->byBBType == BB_TYPE_11B) {
+	if (pDevice->byBBType == BB_TYPE_11A || pDevice->byBBType == BB_TYPE_11B)
 		return (unsigned char)pDevice->byBBType;
-	} else if (CARDbIsOFDMinBasicRate((void *)pDevice)) {
+	else if (CARDbIsOFDMinBasicRate((void *)pDevice))
 		return PK_TYPE_11GA;
-	} else {
+	else
 		return PK_TYPE_11GB;
-	}
 }
 
 /*
@@ -2004,9 +1951,9 @@ QWORD CARDqGetTSFOffset(unsigned char byRxRate, QWORD qwTSF1, QWORD qwTSF2)
 	LODWORD(qwTSFOffset) = 0;
 	wRxBcnTSFOffst = cwRXBCNTSFOff[byRxRate%MAX_RATE];
 	(qwTSF2).u.dwLowDword += (unsigned long)(wRxBcnTSFOffst);
-	if ((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
@@ -2074,8 +2021,6 @@ QWORD CARDqGetNextTBTT(QWORD qwTSF, unsigned short wBeaconInterval)
 	uLowNextTBTT = (LODWORD(qwTSF) >> 10) << 10;
 	// low dword (mod) bcn
 	uLowRemain = (uLowNextTBTT) % uBeaconInterval;
-//    uHighRemain = ((0x80000000 % uBeaconInterval)* 2 * HIDWORD(qwTSF))
-//                  % uBeaconInterval;
 	// high dword (mod) bcn
 	uHighRemain = (((0xffffffff % uBeaconInterval) + 1) * HIDWORD(qwTSF))
 		% uBeaconInterval;
@@ -2117,7 +2062,7 @@ void CARDvSetFirstNextTBTT(unsigned long dwIoBase, unsigned short wBeaconInterva
 	VNSvOutPortD(dwIoBase + MAC_REG_NEXTTBTT, LODWORD(qwNextTBTT));
 	VNSvOutPortD(dwIoBase + MAC_REG_NEXTTBTT + 4, HIDWORD(qwNextTBTT));
 	MACvRegBitsOn(dwIoBase, MAC_REG_TFTCTL, TFTCTL_TBTTSYNCEN);
-	//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Card:First Next TBTT[%8xh:%8xh] \n", HIDWORD(qwNextTBTT), LODWORD(qwNextTBTT));
+
 	return;
 }
 
diff --git a/drivers/staging/vt6655/card.h b/drivers/staging/vt6655/card.h
index ac6e2b4d0fd..829be92838b 100644
--- a/drivers/staging/vt6655/card.h
+++ b/drivers/staging/vt6655/card.h
@@ -104,7 +104,6 @@ void CARDvSafeResetRx(void *pDeviceHandler);
 //xxx
 bool CARDbRadioPowerOff(void *pDeviceHandler);
 bool CARDbRadioPowerOn(void *pDeviceHandler);
-//bool CARDbSendPacket(void *pDeviceHandler, void *pPacket, CARD_PKT_TYPE ePktType, unsigned int uLength);
 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);
diff --git a/drivers/staging/vt6655/channel.c b/drivers/staging/vt6655/channel.c
index 3198a31e2ed..d5b89b7aaf2 100644
--- a/drivers/staging/vt6655/channel.c
+++ b/drivers/staging/vt6655/channel.c
@@ -30,7 +30,6 @@
 
 #define CARD_MAX_CHANNEL_TBL    56
 
-//static int msglevel = MSG_LEVEL_DEBUG;
 static int msglevel = MSG_LEVEL_INFO;
 
 /*---------------------  Static Variables  --------------------------*/
@@ -421,9 +420,8 @@ void init_channel_table(void *pDeviceHandler)
 	bool bMultiBand = false;
 	unsigned int ii;
 
-	for (ii = 1; ii <= CARD_MAX_CHANNEL_TBL; ii++) {
+	for (ii = 1; ii <= CARD_MAX_CHANNEL_TBL; ii++)
 		sChannelTbl[ii].bValid = false;
-	}
 
 	switch (pDevice->byRFType) {
 	case RF_RFMD2959:
@@ -509,7 +507,6 @@ unsigned char get_channel_mapping(void *pDeviceHandler, unsigned char byChannelN
 
 unsigned char get_channel_number(void *pDeviceHandler, unsigned char byChannelIndex)
 {
-	//PSDevice    pDevice = (PSDevice) pDeviceHandler;
 	return sChannelTbl[byChannelIndex].byChannelNumber;
 }
 
@@ -527,13 +524,11 @@ bool set_channel(void *pDeviceHandler, unsigned int uConnectionChannel)
 	PSDevice pDevice = (PSDevice) pDeviceHandler;
 	bool bResult = true;
 
-	if (pDevice->byCurrentCh == uConnectionChannel) {
+	if (pDevice->byCurrentCh == uConnectionChannel)
 		return bResult;
-	}
 
-	if (!sChannelTbl[uConnectionChannel].bValid) {
+	if (!sChannelTbl[uConnectionChannel].bValid)
 		return false;
-	}
 
 	if ((uConnectionChannel > CB_MAX_CHANNEL_24G) &&
 	    (pDevice->eCurrentPHYType != PHY_TYPE_11A)) {
@@ -548,9 +543,8 @@ bool set_channel(void *pDeviceHandler, unsigned int uConnectionChannel)
 	//{{ RobertYu: 20041202
 	//// TX_PE will reserve 3 us for MAX2829 A mode only, it is for better TX throughput
 
-	if (pDevice->byRFType == RF_AIROHA7230) {
+	if (pDevice->byRFType == RF_AIROHA7230)
 		RFbAL7230SelectChannelPostProcess(pDevice->PortOffset, pDevice->byCurrentCh, (unsigned char)uConnectionChannel);
-	}
 	//}} RobertYu
 
 	pDevice->byCurrentCh = (unsigned char)uConnectionChannel;
@@ -560,7 +554,6 @@ bool set_channel(void *pDeviceHandler, unsigned int uConnectionChannel)
 	if (pDevice->bEnablePSMode)
 		RFvWriteWakeProgSyn(pDevice->PortOffset, pDevice->byRFType, uConnectionChannel);
 
-	//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "CARDbSetMediaChannel: %d\n", (unsigned char)uConnectionChannel);
 	BBvSoftwareReset(pDevice->PortOffset);
 
 	if (pDevice->byLocalID > REV_ID_VT3253_B1) {
@@ -573,11 +566,10 @@ bool set_channel(void *pDeviceHandler, unsigned int uConnectionChannel)
 		MACvSelectPage0(pDevice->PortOffset);
 	}
 
-	if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
+	if (pDevice->eCurrentPHYType == PHY_TYPE_11B)
 		RFbSetPower(pDevice, RATE_1M, pDevice->byCurrentCh);
-	} else {
+	else
 		RFbSetPower(pDevice, RATE_6M, pDevice->byCurrentCh);
-	}
 
 	return bResult;
 }
@@ -604,15 +596,15 @@ void set_country_info(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, void *pIE)
 
 	if (ePHYType == PHY_TYPE_11A) {
 		pDevice->bCountryInfo5G = true;
-		for (ii = CB_MAX_CHANNEL_24G + 1; ii <= CARD_MAX_CHANNEL_TBL; ii++) {
+		for (ii = CB_MAX_CHANNEL_24G + 1; ii <= CARD_MAX_CHANNEL_TBL; ii++)
 			sChannelTbl[ii].bValid = false;
-		}
+
 		step = 4;
 	} else {
 		pDevice->bCountryInfo24G = true;
-		for (ii = 1; ii <= CB_MAX_CHANNEL_24G; ii++) {
+		for (ii = 1; ii <= CB_MAX_CHANNEL_24G; ii++)
 			sChannelTbl[ii].bValid = false;
-		}
+
 		step = 1;
 	}
 	pDevice->abyCountryCode[0] = pIE_Country->abyCountryString[0];
@@ -655,19 +647,19 @@ unsigned char set_support_channels(void *pDeviceHandler, unsigned char *pbyIEs)
 	byCount = 0;
 	if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[28] == true) {
 		for (ii = 28; ii < 36; ii += 2) {
-			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true) {
+			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true)
 				byCount++;
-			}
 		}
+
 		*pbyChTupple++ = 34;
 		*pbyChTupple++ = byCount;
 		byLen += 2;
 	} else if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[29] == true) {
 		for (ii = 29; ii < 36; ii += 2) {
-			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true) {
+			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true)
 				byCount++;
-			}
 		}
+
 		*pbyChTupple++ = 36;
 		*pbyChTupple++ = byCount;
 		byLen += 2;
@@ -676,10 +668,10 @@ unsigned char set_support_channels(void *pDeviceHandler, unsigned char *pbyIEs)
 	byCount = 0;
 	if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[36] == true) {
 		for (ii = 36; ii < 40; ii++) {
-			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true) {
+			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true)
 				byCount++;
-			}
 		}
+
 		*pbyChTupple++ = 52;
 		*pbyChTupple++ = byCount;
 		byLen += 2;
@@ -688,19 +680,19 @@ unsigned char set_support_channels(void *pDeviceHandler, unsigned char *pbyIEs)
 	byCount = 0;
 	if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[40] == true) {
 		for (ii = 40; ii < 51; ii++) {
-			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true) {
+			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true)
 				byCount++;
-			}
 		}
+
 		*pbyChTupple++ = 100;
 		*pbyChTupple++ = byCount;
 		byLen += 2;
 	} else if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[51] == true) {
 		for (ii = 51; ii < 56; ii++) {
-			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true) {
+			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true)
 				byCount++;
-			}
 		}
+
 		*pbyChTupple++ = 149;
 		*pbyChTupple++ = byCount;
 		byLen += 2;
@@ -767,14 +759,13 @@ unsigned char auto_channel_select(void *pDeviceHandler, CARD_PHY_TYPE ePHYType)
 	if (ePHYType == PHY_TYPE_11A) {
 		for (ii = CB_MAX_CHANNEL_24G + 1; ii <= CB_MAX_CHANNEL; ii++) {
 			if (sChannelTbl[ii].bValid) {
-				if (byOptionChannel == 0) {
+				if (byOptionChannel == 0)
 					byOptionChannel = (unsigned char) ii;
-				}
-				if (sChannelTbl[ii].byMAP == 0) {
+
+				if (sChannelTbl[ii].byMAP == 0)
 					return (unsigned char) ii;
-				} else if (!(sChannelTbl[ii].byMAP & 0x08)) {
+				else if (!(sChannelTbl[ii].byMAP & 0x08))
 					byOptionChannel = (unsigned char) ii;
-				}
 			}
 		}
 	} else {
@@ -784,25 +775,24 @@ unsigned char auto_channel_select(void *pDeviceHandler, CARD_PHY_TYPE ePHYType)
 				if (sChannelTbl[ii].byMAP == 0) {
 					aiWeight[ii] += 100;
 				} else if (sChannelTbl[ii].byMAP & 0x01) {
-					if (ii > 3) {
+					if (ii > 3)
 						aiWeight[ii - 3] -= 10;
-					}
-					if (ii > 2) {
+
+					if (ii > 2)
 						aiWeight[ii - 2] -= 20;
-					}
-					if (ii > 1) {
+
+					if (ii > 1)
 						aiWeight[ii - 1] -= 40;
-					}
+
 					aiWeight[ii] -= 80;
-					if (ii < CB_MAX_CHANNEL_24G) {
+					if (ii < CB_MAX_CHANNEL_24G)
 						aiWeight[ii + 1] -= 40;
-					}
-					if (ii < (CB_MAX_CHANNEL_24G - 1)) {
+
+					if (ii < (CB_MAX_CHANNEL_24G - 1))
 						aiWeight[ii+2] -= 20;
-					}
-					if (ii < (CB_MAX_CHANNEL_24G - 2)) {
+
+					if (ii < (CB_MAX_CHANNEL_24G - 2))
 						aiWeight[ii+3] -= 10;
-					}
 				}
 			}
 		}
diff --git a/drivers/staging/vt6655/datarate.c b/drivers/staging/vt6655/datarate.c
index c9a89cd7633..f8420d65cd9 100644
--- a/drivers/staging/vt6655/datarate.c
+++ b/drivers/staging/vt6655/datarate.c
@@ -49,9 +49,8 @@
 
 extern unsigned short TxRate_iwconfig; //2008-5-8 <add> by chester
 /*---------------------  Static Variables  --------------------------*/
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 static int msglevel = MSG_LEVEL_INFO;
-const unsigned char acbyIERate[MAX_RATE] =
+static const unsigned char acbyIERate[MAX_RATE] =
 {0x02, 0x04, 0x0B, 0x16, 0x0C, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C};
 
 #define AUTORATE_TXOK_CNT       0x0400
@@ -251,13 +250,11 @@ RATEvParseMaxRate(
 			if (byRate > byHighSuppRate)
 				byHighSuppRate = byRate;
 			*pwSuppRate |= (1<<wGetRateIdx(byRate));
-			//DBG_PRN_GRP09(("ParseMaxRate : HighSuppRate: %d, %X\n", wGetRateIdx(byRate), byRate));
 		}
-	} //if (pItemExtRates != NULL)
+	}
 
-	if ((pDevice->byPacketType == PK_TYPE_11GB) && CARDbIsOFDMinBasicRate((void *)pDevice)) {
+	if ((pDevice->byPacketType == PK_TYPE_11GB) && CARDbIsOFDMinBasicRate((void *)pDevice))
 		pDevice->byPacketType = PK_TYPE_11GA;
-	}
 
 	*pbyTopCCKRate = pDevice->byTopCCKBasicRate;
 	*pbyTopOFDMRate = pDevice->byTopOFDMBasicRate;
@@ -299,7 +296,6 @@ RATEvTxRateFallBack(
 	PSDevice        pDevice = (PSDevice) pDeviceHandler;
 	unsigned short wIdxDownRate = 0;
 	unsigned int ii;
-//unsigned long dwRateTable[MAX_RATE]  = {1,   2,   5,   11,  6,    9,    12,   18,  24,  36,  48,  54};
 	bool bAutoRate[MAX_RATE]    = {true, true, true, true, false, false, true, true, true, true, true, true};
 	unsigned long dwThroughputTbl[MAX_RATE] = {10, 20, 55, 110, 60, 90, 120, 180, 240, 360, 480, 540};
 	unsigned long dwThroughput = 0;
@@ -322,15 +318,14 @@ RATEvTxRateFallBack(
 		return;
 	}
 
-	if (psNodeDBTable->uTimeCount >= AUTORATE_TIMEOUT) {
+	if (psNodeDBTable->uTimeCount >= AUTORATE_TIMEOUT)
 		psNodeDBTable->uTimeCount = 0;
-	}
 
 	for (ii = 0; ii < MAX_RATE; ii++) {
 		if (psNodeDBTable->wSuppRate & (0x0001<<ii)) {
-			if (bAutoRate[ii]) {
+			if (bAutoRate[ii])
 				wIdxUpRate = (unsigned short) ii;
-			}
+
 		} else {
 			bAutoRate[ii] = false;
 		}
@@ -340,13 +335,11 @@ RATEvTxRateFallBack(
 		if ((psNodeDBTable->uTxOk[ii] != 0) ||
 		    (psNodeDBTable->uTxFail[ii] != 0)) {
 			dwThroughputTbl[ii] *= psNodeDBTable->uTxOk[ii];
-			if (ii < RATE_11M) {
+			if (ii < RATE_11M)
 				psNodeDBTable->uTxFail[ii] *= 4;
-			}
+
 			dwThroughputTbl[ii] /= (psNodeDBTable->uTxOk[ii] + psNodeDBTable->uTxFail[ii]);
 		}
-//        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Rate %d,Ok: %d, Fail:%d, Throughput:%d\n",
-//                       ii, psNodeDBTable->uTxOk[ii], psNodeDBTable->uTxFail[ii], dwThroughputTbl[ii]);
 	}
 	dwThroughput = dwThroughputTbl[psNodeDBTable->wTxDataRate];
 
@@ -371,7 +364,6 @@ RATEvTxRateFallBack(
 //2008-5-8 <add> by chester
 	TxRate_iwconfig = psNodeDBTable->wTxDataRate;
 	s_vResetCounter(psNodeDBTable);
-//    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Rate: %d, U:%d, D:%d\n", psNodeDBTable->wTxDataRate, wIdxUpRate, wIdxDownRate);
 
 	return;
 }
diff --git a/drivers/staging/vt6655/desc.h b/drivers/staging/vt6655/desc.h
index f8e41487f85..c620cbfbaad 100644
--- a/drivers/staging/vt6655/desc.h
+++ b/drivers/staging/vt6655/desc.h
@@ -213,29 +213,6 @@ typedef struct tagDEVICE_RD_INFO {
 	dma_addr_t  curr_desc;
 } DEVICE_RD_INFO,   *PDEVICE_RD_INFO;
 
-/*
-  static inline PDEVICE_RD_INFO alloc_rd_info(void) {
-  PDEVICE_RD_INFO  ptr;
-  ptr = kmalloc(sizeof(DEVICE_RD_INFO), GFP_ATOMIC);
-  if (ptr == NULL)
-  return NULL;
-  else {
-  memset(ptr,0,sizeof(DEVICE_RD_INFO));
-  return ptr;
-  }
-  }
-*/
-
-/*
-  typedef struct tagRDES0 {
-  unsigned short wResCount;
-  unsigned short wf1Owner;
-//    unsigned short f15Reserved : 15;
-//    unsigned short f1Owner : 1;
-} __attribute__ ((__packed__))
-SRDES0;
-*/
-
 #ifdef __BIG_ENDIAN
 
 typedef struct tagRDES0 {
@@ -284,17 +261,6 @@ typedef const SRxDesc *PCSRxDesc;
 
 #ifdef __BIG_ENDIAN
 
-/*
-  typedef struct tagTDES0 {
-  volatile    unsigned char byTSR0;
-  volatile    unsigned char byTSR1;
-  volatile    unsigned short wOwner_Txtime;
-//    volatile    unsigned short f15Txtime : 15;
-//    volatile    unsigned short f1Owner:1;
-} __attribute__ ((__packed__))
-STDES0;
-*/
-
 typedef struct tagTDES0 {
 	volatile    unsigned char byTSR0;
 	volatile    unsigned char byTSR1;
@@ -339,19 +305,6 @@ typedef struct tagDEVICE_TD_INFO {
 	unsigned char byFlags;
 } DEVICE_TD_INFO,    *PDEVICE_TD_INFO;
 
-/*
-  static inline PDEVICE_TD_INFO alloc_td_info(void) {
-  PDEVICE_TD_INFO  ptr;
-  ptr = kmalloc(sizeof(DEVICE_TD_INFO),GFP_ATOMIC);
-  if (ptr == NULL)
-  return NULL;
-  else {
-  memset(ptr,0,sizeof(DEVICE_TD_INFO));
-  return ptr;
-  }
-  }
-*/
-
 //
 // transmit descriptor
 //
diff --git a/drivers/staging/vt6655/device.h b/drivers/staging/vt6655/device.h
index 062c3a374b9..45fc8a0b9b5 100644
--- a/drivers/staging/vt6655/device.h
+++ b/drivers/staging/vt6655/device.h
@@ -148,8 +148,17 @@
 
 /*---------------------  Export Types  ------------------------------*/
 
-#define DBG_PRT(l, p, args...) { if (l <= msglevel) printk(p, ##args); }
-#define PRINT_K(p, args...) { if (PRIVATE_Message) printk(p, ##args); }
+#define DBG_PRT(l, p, args...)		\
+do {					\
+	if (l <= msglevel)		\
+		printk(p, ##args);	\
+} while (0)
+
+#define PRINT_K(p, args...)		\
+do {					\
+	if (PRIVATE_Message)		\
+		printk(p, ##args);	\
+} while (0)
 
 //0:11A 1:11B 2:11G
 typedef enum _VIA_BB_TYPE
@@ -741,12 +750,6 @@ typedef struct __device_info {
 	bool bWPADEVUp;
 	struct sk_buff          *skb;
 #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-/*
-  bool bwextstep0;
-  bool bwextstep1;
-  bool bwextstep2;
-  bool bwextstep3;
-*/
 	unsigned int	bwextcount;
 	bool bWPASuppWextEnabled;
 #endif
diff --git a/drivers/staging/vt6655/device_main.c b/drivers/staging/vt6655/device_main.c
index a952df1bf9d..1d3908d044d 100644
--- a/drivers/staging/vt6655/device_main.c
+++ b/drivers/staging/vt6655/device_main.c
@@ -89,7 +89,6 @@
 #include <linux/slab.h>
 
 /*---------------------  Static Definitions -------------------------*/
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 static int          msglevel                =   MSG_LEVEL_INFO;
 
 //
@@ -100,14 +99,8 @@ MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("VIA Networking Solomon-A/B/G Wireless LAN Adapter Driver");
 
 static int mlme_kill;
-//static  struct task_struct * mlme_task;
 
 #define DEVICE_PARAM(N, D)
-/*
-  static const int N[MAX_UINTS]=OPTION_DEFAULT;\
-  MODULE_PARM(N, "1-" __MODULE_STRING(MAX_UINTS) "i");\
-  MODULE_PARM_DESC(N, D);
-*/
 
 #define RX_DESC_MIN0     16
 #define RX_DESC_MAX0     128
@@ -346,38 +339,6 @@ static void vt6655_remove(struct pci_dev *pcid)
 	device_free_info(pDevice);
 }
 
-/*
-  static void
-  device_set_int_opt(int *opt, int val, int min, int max, int def,char* name,char* devname) {
-  if (val==-1)
-  *opt=def;
-  else if (val<min || val>max) {
-  DBG_PRT(MSG_LEVEL_INFO, KERN_NOTICE "%s: the value of parameter %s is invalid, the valid range is (%d-%d)\n" ,
-  devname,name, min,max);
-  *opt=def;
-  } else {
-  DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "%s: set value of parameter %s to %d\n",
-  devname, name, val);
-  *opt=val;
-  }
-  }
-
-  static void
-  device_set_bool_opt(unsigned int *opt, int val,bool def,u32 flag, char* name,char* devname) {
-  (*opt)&=(~flag);
-  if (val==-1)
-  *opt|=(def ? flag : 0);
-  else if (val<0 || val>1) {
-  DBG_PRT(MSG_LEVEL_INFO, KERN_NOTICE
-  "%s: the value of parameter %s is invalid, the valid range is (0-1)\n",devname,name);
-  *opt|=(def ? flag : 0);
-  } else {
-  DBG_PRT(MSG_LEVEL_INFO, KERN_NOTICE "%s: set parameter %s to %s\n",
-  devname,name , val ? "true" : "false");
-  *opt|=(val ? flag : 0);
-  }
-  }
-*/
 static void device_get_options(PSDevice pDevice, int index, char *devname)
 {
 	POPTIONS pOpts = &(pDevice->sOpts);
@@ -395,7 +356,6 @@ static void device_get_options(PSDevice pDevice, int index, char *devname)
 
 	pOpts->flags |= DEVICE_FLAGS_PREAMBLE_TYPE;
 	pOpts->flags |= DEVICE_FLAGS_OP_MODE;
-	//pOpts->flags|=DEVICE_FLAGS_PS_MODE;
 	pOpts->short_retry = SHORT_RETRY_DEF;
 	pOpts->long_retry = LONG_RETRY_DEF;
 	pOpts->bbp_type = BBP_TYPE_DEF;
@@ -431,7 +391,6 @@ device_set_options(PSDevice pDevice) {
 
 //PLICE_DEBUG->
 	pDevice->byAutoFBCtrl = AUTO_FB_0;
-	//pDevice->byAutoFBCtrl = AUTO_FB_1;
 //PLICE_DEBUG<-
 	pDevice->bUpdateBBVGA = true;
 	pDevice->byFOETuning = 0;
@@ -549,12 +508,12 @@ static void device_init_registers(PSDevice pDevice, DEVICE_INIT_TYPE InitType)
 		if (byValue == 0) // if not set default is All
 			byValue = (EEP_ANTENNA_AUX | EEP_ANTENNA_MAIN);
 
-		pDevice->ulDiversityNValue = 100*260;//100*SROMbyReadEmbedded(pDevice->PortOffset, 0x51);
-		pDevice->ulDiversityMValue = 100*16;//SROMbyReadEmbedded(pDevice->PortOffset, 0x52);
-		pDevice->byTMax = 1;//SROMbyReadEmbedded(pDevice->PortOffset, 0x53);
-		pDevice->byTMax2 = 4;//SROMbyReadEmbedded(pDevice->PortOffset, 0x54);
-		pDevice->ulSQ3TH = 0;//(unsigned long) SROMbyReadEmbedded(pDevice->PortOffset, 0x55);
-		pDevice->byTMax3 = 64;//SROMbyReadEmbedded(pDevice->PortOffset, 0x56);
+		pDevice->ulDiversityNValue = 100*260;
+		pDevice->ulDiversityMValue = 100*16;
+		pDevice->byTMax = 1;
+		pDevice->byTMax2 = 4;
+		pDevice->ulSQ3TH = 0;
+		pDevice->byTMax3 = 64;
 
 		if (byValue == (EEP_ANTENNA_AUX | EEP_ANTENNA_MAIN)) {
 			pDevice->byAntennaCount = 2;
@@ -568,7 +527,7 @@ static void device_init_registers(PSDevice pDevice, DEVICE_INIT_TYPE InitType)
 			// chester for antenna
 			byValue1 = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_ANTENNA);
 			if ((byValue1 & 0x08) == 0)
-				pDevice->bDiversityEnable = false;//SROMbyReadEmbedded(pDevice->PortOffset, 0x50);
+				pDevice->bDiversityEnable = false;
 			else
 				pDevice->bDiversityEnable = true;
 		} else  {
@@ -593,7 +552,6 @@ static void device_init_registers(PSDevice pDevice, DEVICE_INIT_TYPE InitType)
 		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);
 
-//#ifdef ZoneType_DefaultSetting
 //2008-8-4 <add> by chester
 //zonetype initial
 		pDevice->byOriginalZonetype = pDevice->abyEEPROM[EEP_OFS_ZONETYPE];
@@ -635,9 +593,9 @@ static void device_init_registers(PSDevice pDevice, DEVICE_INIT_TYPE InitType)
 		pDevice->byRFType &= RF_MASK;
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->byRFType = %x\n", pDevice->byRFType);
 
-		if (!pDevice->bZoneRegExist) {
+		if (!pDevice->bZoneRegExist)
 			pDevice->byZoneType = pDevice->abyEEPROM[EEP_OFS_ZONETYPE];
-		}
+
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->byZoneType = %x\n", pDevice->byZoneType);
 
 		//Init RF module
@@ -647,21 +605,18 @@ static void device_init_registers(PSDevice pDevice, DEVICE_INIT_TYPE InitType)
 		pDevice->byCurPwr = 0xFF;
 		pDevice->byCCKPwr = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_PWR_CCK);
 		pDevice->byOFDMPwrG = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_PWR_OFDMG);
-		//byCCKPwrdBm = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_CCK_PWR_dBm);
-
-		//byOFDMPwrdBm = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_OFDM_PWR_dBm);
 
 		// Load power Table
 
 		for (ii = 0; ii < CB_MAX_CHANNEL_24G; ii++) {
 			pDevice->abyCCKPwrTbl[ii + 1] = SROMbyReadEmbedded(pDevice->PortOffset, (unsigned char)(ii + EEP_OFS_CCK_PWR_TBL));
-			if (pDevice->abyCCKPwrTbl[ii + 1] == 0) {
+			if (pDevice->abyCCKPwrTbl[ii + 1] == 0)
 				pDevice->abyCCKPwrTbl[ii+1] = pDevice->byCCKPwr;
-			}
+
 			pDevice->abyOFDMPwrTbl[ii + 1] = SROMbyReadEmbedded(pDevice->PortOffset, (unsigned char)(ii + EEP_OFS_OFDM_PWR_TBL));
-			if (pDevice->abyOFDMPwrTbl[ii + 1] == 0) {
+			if (pDevice->abyOFDMPwrTbl[ii + 1] == 0)
 				pDevice->abyOFDMPwrTbl[ii + 1] = pDevice->byOFDMPwrG;
-			}
+
 			pDevice->abyCCKDefaultPwr[ii + 1] = byCCKPwrdBm;
 			pDevice->abyOFDMDefaultPwr[ii + 1] = byOFDMPwrdBm;
 		}
@@ -715,14 +670,12 @@ static void device_init_registers(PSDevice pDevice, DEVICE_INIT_TYPE InitType)
 
 		pDevice->byCurrentCh = 0;
 
-		//pDevice->NetworkType = Ndis802_11Automode;
 		// Set BB and packet type at the same time.
 		// Set Short Slot Time, xIFS, and RSPINF.
-		if (pDevice->uConnectionRate == RATE_AUTO) {
+		if (pDevice->uConnectionRate == RATE_AUTO)
 			pDevice->wCurrentRate = RATE_54M;
-		} else {
+		else
 			pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
-		}
 
 		// default G Mode
 		VNTWIFIbConfigPhyMode(pDevice->pMgmt, PHY_TYPE_11G);
@@ -738,22 +691,25 @@ static void device_init_registers(PSDevice pDevice, DEVICE_INIT_TYPE InitType)
 			MACvGPIOIn(pDevice->PortOffset, &pDevice->byGPIO);
 //2008-4-14 <add> by chester for led issue
 #ifdef FOR_LED_ON_NOTEBOOK
-			if (pDevice->byGPIO & GPIO0_DATA) { pDevice->bHWRadioOff = true; }
-			if (!(pDevice->byGPIO & GPIO0_DATA)) { pDevice->bHWRadioOff = false; }
+			if (pDevice->byGPIO & GPIO0_DATA)
+				pDevice->bHWRadioOff = true;
 
+			if (!(pDevice->byGPIO & GPIO0_DATA))
+				pDevice->bHWRadioOff = false;
 		}
-		if (pDevice->bRadioControlOff) {
+
+		if (pDevice->bRadioControlOff)
 			CARDbRadioPowerOff(pDevice);
-		} else  CARDbRadioPowerOn(pDevice);
+		else
+			CARDbRadioPowerOn(pDevice);
 #else
 		if (((pDevice->byGPIO & GPIO0_DATA) && !(pDevice->byRadioCtl & EEP_RADIOCTL_INV)) ||
 		    (!(pDevice->byGPIO & GPIO0_DATA) && (pDevice->byRadioCtl & EEP_RADIOCTL_INV))) {
 			pDevice->bHWRadioOff = true;
 		}
 	}
-	if (pDevice->bHWRadioOff || pDevice->bRadioControlOff) {
+	if (pDevice->bHWRadioOff || pDevice->bRadioControlOff)
 		CARDbRadioPowerOff(pDevice);
-	}
 
 #endif
 }
@@ -768,9 +724,8 @@ CARDvSafeResetRx(pDevice);
 // reset Rx pointer
 CARDvSafeResetTx(pDevice);
 
-if (pDevice->byLocalID <= REV_ID_VT3253_A1) {
+if (pDevice->byLocalID <= REV_ID_VT3253_A1)
 	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_RCR, RCR_WPAERR);
-}
 
 pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
 
@@ -807,7 +762,7 @@ static bool device_release_WPADEV(PSDevice pDevice)
 {
 	viawget_wpa_header *wpahdr;
 	int ii = 0;
-	// wait_queue_head_t	Set_wait;
+
 	//send device close to wpa_supplicnat layer
 	if (pDevice->bWPADEVUp) {
 		wpahdr = (viawget_wpa_header *)pDevice->skb->data;
@@ -823,9 +778,6 @@ static bool device_release_WPADEV(PSDevice pDevice)
 		netif_rx(pDevice->skb);
 		pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
 
-		//wait release WPADEV
-		//    init_waitqueue_head(&Set_wait);
-		//    wait_event_timeout(Set_wait, ((pDevice->wpadev==NULL)&&(pDevice->skb == NULL)),5*HZ);    //1s wait
 		while (pDevice->bWPADEVUp) {
 			set_current_state(TASK_UNINTERRUPTIBLE);
 			schedule_timeout(HZ / 20);          //wait 50ms
@@ -869,7 +821,6 @@ vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
 	}
 
 	// Chain it all together
-	// SET_MODULE_OWNER(dev);
 	SET_NETDEV_DEV(dev, &pcid->dev);
 
 	if (bFirst) {
@@ -902,7 +853,6 @@ vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
 
 #ifdef	DEBUG
 
-	//pci_read_config_byte(pcid, PCI_BASE_ADDRESS_0, &pDevice->byRevId);
 	printk("after get pci_info memaddr is %x, io addr is %x,io_size is %d\n", pDevice->memaddr, pDevice->ioaddr, pDevice->io_size);
 	{
 		int i;
@@ -916,7 +866,6 @@ vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
 			PCI_BASE_ADDRESS_5,
 			0};
 		for (i = 0; address[i]; i++) {
-			//pci_write_config_dword(pcid,address[i], 0xFFFFFFFF);
 			pci_read_config_dword(pcid, address[i], &bar);
 			printk("bar %d is %x\n", i, bar);
 			if (!bar) {
@@ -942,11 +891,7 @@ vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
 
 #endif
 
-#ifdef	DEBUG
-	//return  0;
-#endif
 	pDevice->PortOffset = (unsigned long)ioremap(pDevice->memaddr & PCI_BASE_ADDRESS_MEM_MASK, pDevice->io_size);
-	//pDevice->PortOffset = (unsigned long)ioremap(pDevice->ioaddr & PCI_BASE_ADDRESS_IO_MASK, pDevice->io_size);
 
 	if (pDevice->PortOffset == 0) {
 		printk(KERN_ERR DEVICE_NAME ": Failed to IO remapping ..\n");
@@ -967,7 +912,6 @@ vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
 
 	VNSvInPortB(pDevice->PortOffset+0x4F, &value);
 	printk("Before write: value is %x\n", value);
-	//VNSvInPortB(pDevice->PortOffset+0x3F, 0x00);
 	VNSvOutPortB(pDevice->PortOffset, value);
 	VNSvInPortB(pDevice->PortOffset+0x4F, &value);
 	printk("After write: value is %x\n", value);
@@ -1075,11 +1019,6 @@ static bool device_get_pci_info(PSDevice pDevice, struct pci_dev *pcid) {
 	pDevice->memaddr = pci_resource_start(pcid, 0);
 	pDevice->ioaddr = pci_resource_start(pcid, 1);
 
-#ifdef	DEBUG
-//	pDevice->ioaddr = pci_resource_start(pcid, 0);
-//	pDevice->memaddr = pci_resource_start(pcid,1);
-#endif
-
 	cis_addr = pci_resource_start(pcid, 2);
 
 	pDevice->pcid = pcid;
@@ -1088,13 +1027,6 @@ static bool device_get_pci_info(PSDevice pDevice, struct pci_dev *pcid) {
 	pci_write_config_byte(pcid, PCI_COMMAND, (b|PCI_COMMAND_MASTER));
 
 #ifdef	PLICE_DEBUG
-	//pci_read_config_word(pcid,PCI_MAX_LAT,&max_lat);
-	//for (ii=0;ii<0xFF;ii++)
-	//pci_read_config_word(pcid,PCI_MAX_LAT,&max_lat);
-	//max_lat  = 0x20;
-	//pci_write_config_word(pcid,PCI_MAX_LAT,max_lat);
-	//pci_read_config_word(pcid,PCI_MAX_LAT,&max_lat);
-
 	for (ii = 0; ii < 0xFF; ii++) {
 		pci_read_config_byte(pcid, ii, &value);
 		pci_config[ii] = value;
@@ -1468,7 +1400,6 @@ static int device_rx_srv(PSDevice pDevice, unsigned int uIdx) {
 	for (pRD = pDevice->pCurrRD[uIdx];
 	     pRD->m_rd0RD0.f1Owner == OWNED_BY_HOST;
 	     pRD = pRD->next) {
-//        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->pCurrRD = %x, works = %d\n", pRD, works);
 		if (works++ > 15)
 			break;
 		if (device_receive_frame(pDevice, pRD)) {
@@ -1564,9 +1495,9 @@ static int device_tx_srv(PSDevice pDevice, unsigned int uIdx) {
 						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Tx[%d] OK but has error. tsr1[%02X] tsr0[%02X].\n",
 							(int)uIdx, byTsr1, byTsr0);
 					}
-					if ((pTxBufHead->wFragCtl & FRAGCTL_ENDFRAG) != FRAGCTL_NONFRAG) {
+					if ((pTxBufHead->wFragCtl & FRAGCTL_ENDFRAG) != FRAGCTL_NONFRAG)
 						pDevice->s802_11Counter.TransmittedFragmentCount++;
-					}
+
 					pStats->tx_packets++;
 					pStats->tx_bytes += pTD->pTDInfo->skb->len;
 				} else {
@@ -1584,7 +1515,6 @@ static int device_tx_srv(PSDevice pDevice, unsigned int uIdx) {
 					skb->dev = pDevice->apdev;
 					skb_reset_mac_header(skb);
 					skb->pkt_type = PACKET_OTHERHOST;
-					//skb->protocol = htons(ETH_P_802_2);
 					memset(skb->cb, 0, sizeof(skb->cb));
 					netif_rx(skb);
 				}
@@ -1596,8 +1526,6 @@ static int device_tx_srv(PSDevice pDevice, unsigned int uIdx) {
 						(int)uIdx, byTsr1, byTsr0);
 				}
 
-//                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Tx[%d] fail has error. tsr1[%02X] tsr0[%02X].\n",
-//                          (int)uIdx, byTsr1, byTsr0);
 
 				if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) &&
 				    (pTD->pTDInfo->byFlags & TD_FLAGS_NETIF_SKB)) {
@@ -1633,9 +1561,9 @@ static int device_tx_srv(PSDevice pDevice, unsigned int uIdx) {
 			bFull = true;
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " AC0DMA is Full = %d\n", pDevice->iTDUsed[uIdx]);
 		}
-		if (netif_queue_stopped(pDevice->dev) && !bFull) {
+		if (netif_queue_stopped(pDevice->dev) && !bFull)
 			netif_wake_queue(pDevice->dev);
-		}
+
 	}
 
 	pDevice->apTailTD[uIdx] = pTD;
@@ -1689,33 +1617,22 @@ int MlmeThread(
 {
 	PSDevice	pDevice =  (PSDevice) Context;
 	PSRxMgmtPacket			pRxMgmtPacket;
-	// int i;
-	//complete(&pDevice->notify);
 
-	//i = 0;
-#if 1
 	while (1) {
-		//down(&pDevice->mlme_semaphore);
-		// pRxMgmtPacket =  DeQueue(pDevice);
-#if 1
 		spin_lock_irq(&pDevice->lock);
 		while (pDevice->rxManeQueue.packet_num != 0) {
 			pRxMgmtPacket = DeQueue(pDevice);
-			//pDevice;
-			//DequeueManageObject(pDevice->FirstRecvMngList, pDevice->LastRecvMngList);
 			vMgrRxManagePacket(pDevice, pDevice->pMgmt, pRxMgmtPacket);
 		}
 		spin_unlock_irq(&pDevice->lock);
 		if (mlme_kill == 0)
 			break;
-		//udelay(200);
-#endif
+
 		schedule();
 		if (mlme_kill == 0)
 			break;
 	}
 
-#endif
 	return 0;
 }
 
@@ -1727,9 +1644,9 @@ static int  device_open(struct net_device *dev) {
 #endif
 
 	pDevice->rx_buf_sz = PKT_BUF_SZ;
-	if (!device_init_rings(pDevice)) {
+	if (!device_init_rings(pDevice))
 		return -ENOMEM;
-	}
+
 //2008-5-13 <add> by chester
 	i = request_irq(pDevice->pcid->irq, &device_intr, IRQF_SHARED, dev->name, dev);
 	if (i)
@@ -1749,11 +1666,10 @@ static int  device_open(struct net_device *dev) {
 	device_init_defrag_cb(pDevice);
 	device_init_td0_ring(pDevice);
 	device_init_td1_ring(pDevice);
-//    VNTWIFIvSet11h(pDevice->pMgmt, pDevice->b11hEnable);
 
-	if (pDevice->bDiversityRegCtlON) {
+	if (pDevice->bDiversityRegCtlON)
 		device_init_diversity_timer(pDevice);
-	}
+
 	vMgrObjectInit(pDevice);
 	vMgrTimerInit(pDevice);
 
@@ -1773,8 +1689,6 @@ static int  device_open(struct net_device *dev) {
 	mlme_kill = 1;
 #endif
 
-	//wait_for_completion(&pDevice->notify);
-
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "call device_init_registers\n");
 	device_init_registers(pDevice, DEVICE_INIT_COLD);
 	MACvReadEtherAddress(pDevice->PortOffset, pDevice->abyCurrentNetAddr);
@@ -1786,12 +1700,6 @@ static int  device_open(struct net_device *dev) {
 	add_timer(&(pDevice->pMgmt->sTimerSecondCallback));
 
 #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-	/*
-	  pDevice->bwextstep0 = false;
-	  pDevice->bwextstep1 = false;
-	  pDevice->bwextstep2 = false;
-	  pDevice->bwextstep3 = false;
-	*/
 	pDevice->bwextcount = 0;
 	pDevice->bWPASuppWextEnabled = false;
 #endif
@@ -1873,9 +1781,7 @@ static int  device_close(struct net_device *dev) {
 	pDevice->flags &= (~DEVICE_FLAGS_OPENED);
 	//2008-0714-01<Add>by chester
 	device_release_WPADEV(pDevice);
-//PLICE_DEBUG->
-	//tasklet_kill(&pDevice->RxMngWorkItem);
-//PLICE_DEBUG<-
+
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "device_close.. \n");
 	return 0;
 }
@@ -1922,7 +1828,6 @@ bool device_dma0_xmit(PSDevice pDevice, struct sk_buff *skb, unsigned int uNodeI
 	unsigned int cbHeaderSize;
 	unsigned int ii;
 	SKeyItem        STempKey;
-//    unsigned char byKeyIndex = 0;
 
 	if (pDevice->bStopTx0Pkt) {
 		dev_kfree_skb_irq(skb);
@@ -1951,9 +1856,9 @@ bool device_dma0_xmit(PSDevice pDevice, struct sk_buff *skb, unsigned int uNodeI
 	cbFrameBodySize = skb->len - ETH_HLEN;
 
 	// 802.1H
-	if (ntohs(pDevice->sTxEthHeader.wType) > ETH_DATA_LEN) {
+	if (ntohs(pDevice->sTxEthHeader.wType) > ETH_DATA_LEN)
 		cbFrameBodySize += 8;
-	}
+
 	uMACfragNum = cbGetFragCount(pDevice, pTransmitKey, cbFrameBodySize, &pDevice->sTxEthHeader);
 
 	if (uMACfragNum > AVAIL_TD(pDevice, TYPE_TXDMA0)) {
@@ -1964,11 +1869,10 @@ bool device_dma0_xmit(PSDevice pDevice, struct sk_buff *skb, unsigned int uNodeI
 
 	if (pDevice->bFixRate) {
 		if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
-			if (pDevice->uConnectionRate >= RATE_11M) {
+			if (pDevice->uConnectionRate >= RATE_11M)
 				pDevice->wCurrentRate = RATE_11M;
-			} else {
+			else
 				pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
-			}
 		} else {
 			if (pDevice->uConnectionRate >= RATE_54M)
 				pDevice->wCurrentRate = RATE_54M;
@@ -1980,11 +1884,10 @@ bool device_dma0_xmit(PSDevice pDevice, struct sk_buff *skb, unsigned int uNodeI
 	}
 
 	//preamble type
-	if (pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble) {
+	if (pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble)
 		pDevice->byPreambleType = pDevice->byShortPreamble;
-	} else {
+	else
 		pDevice->byPreambleType = PREAMBLE_LONG;
-	}
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dma0: pDevice->wCurrentRate = %d \n", pDevice->wCurrentRate);
 
@@ -1993,11 +1896,10 @@ bool device_dma0_xmit(PSDevice pDevice, struct sk_buff *skb, unsigned int uNodeI
 	} else if (pDevice->eCurrentPHYType == PHY_TYPE_11A) {
 		byPktType = PK_TYPE_11A;
 	} else {
-		if (pDevice->bProtectMode) {
+		if (pDevice->bProtectMode)
 			byPktType = PK_TYPE_11GB;
-		} else {
+		else
 			byPktType = PK_TYPE_11GA;
-		}
 	}
 
 	if (pDevice->bEncryptionEnable)
@@ -2119,12 +2021,11 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev) {
 					return 0;
 				}
 
-				if (pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble) {
+				if (pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble)
 					pDevice->byPreambleType = pDevice->byShortPreamble;
-
-				} else {
+				else
 					pDevice->byPreambleType = PREAMBLE_LONG;
-				}
+
 				bNodeExist = true;
 
 			}
@@ -2145,9 +2046,8 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev) {
 	memcpy(pDevice->sTxEthHeader.abyDstAddr, (unsigned char *)(skb->data), ETH_HLEN);
 	cbFrameBodySize = skb->len - ETH_HLEN;
 	// 802.1H
-	if (ntohs(pDevice->sTxEthHeader.wType) > ETH_DATA_LEN) {
+	if (ntohs(pDevice->sTxEthHeader.wType) > ETH_DATA_LEN)
 		cbFrameBodySize += 8;
-	}
 
 	if (pDevice->bEncryptionEnable) {
 		bNeedEncryption = true;
@@ -2183,9 +2083,9 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev) {
 			pbyBSSID = pDevice->abyBroadcastAddr;
 			if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, GROUP_KEY, &pTransmitKey) == false) {
 				pTransmitKey = NULL;
-				if (pDevice->pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
+				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);
-				} else
+				else
 					DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "NOT IBSS and KEY is NULL. [%d]\n", pDevice->pMgmt->eCurrMode);
 			} else {
 				bTKIP_UseGTK = true;
@@ -2234,11 +2134,10 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev) {
 #endif
 
 		if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
-			if (pDevice->uConnectionRate >= RATE_11M) {
+			if (pDevice->uConnectionRate >= RATE_11M)
 				pDevice->wCurrentRate = RATE_11M;
-			} else {
+			else
 				pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
-			}
 		} else {
 			if ((pDevice->eCurrentPHYType == PHY_TYPE_11A) &&
 			    (pDevice->uConnectionRate <= RATE_6M)) {
@@ -2279,24 +2178,18 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev) {
 		}
 	}
 
-//    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "acdma0: pDevice->wCurrentRate = %d \n", pDevice->wCurrentRate);
 
 	if (pDevice->wCurrentRate <= RATE_11M) {
 		byPktType = PK_TYPE_11B;
 	} else if (pDevice->eCurrentPHYType == PHY_TYPE_11A) {
 		byPktType = PK_TYPE_11A;
 	} else {
-		if (pDevice->bProtectMode) {
+		if (pDevice->bProtectMode)
 			byPktType = PK_TYPE_11GB;
-		} else {
+		else
 			byPktType = PK_TYPE_11GA;
-		}
 	}
 
-//#ifdef	PLICE_DEBUG
-//	printk("FIX RATE:CurrentRate is %d");
-//#endif
-
 	if (bNeedEncryption) {
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ntohs Pkt Type=%04x\n", ntohs(pDevice->sTxEthHeader.wType));
 		if ((pDevice->sTxEthHeader.wType) == TYPE_PKT_802_1x) {
@@ -2369,17 +2262,13 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev) {
 #ifdef TxInSleep
 	pDevice->nTxDataTimeCout = 0; //2008-8-21 chester <add> for send null packet
 #endif
-	if (AVAIL_TD(pDevice, TYPE_AC0DMA) <= 1) {
+	if (AVAIL_TD(pDevice, TYPE_AC0DMA) <= 1)
 		netif_stop_queue(dev);
-	}
 
 	pDevice->apCurrTD[TYPE_AC0DMA] = pHeadTD;
-//#ifdef	PLICE_DEBUG
-	if (pDevice->bFixRate) {
+
+	if (pDevice->bFixRate)
 		printk("FixRate:Rate is %d,TxPower is %d\n", pDevice->wCurrentRate, pDevice->byCurPwr);
-	} else {
-	}
-//#endif
 
 	{
 		unsigned char Protocol_Version;    //802.1x Authentication
@@ -2411,7 +2300,6 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev) {
 	}
 
 	MACvTransmitAC0(pDevice->PortOffset);
-//    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "acdma0:pDevice->apCurrTD= %p\n", pHeadTD);
 
 	dev->trans_start = jiffies;
 
@@ -2430,7 +2318,6 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
 	int             handled = 0;
 	unsigned char byData = 0;
 	int             ii = 0;
-//    unsigned char byRSSI;
 
 	MACvReadISR(pDevice->PortOffset, &pDevice->dwIsr);
 
@@ -2441,17 +2328,6 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dwIsr = 0xffff\n");
 		return IRQ_RETVAL(handled);
 	}
-	/*
-	// 2008-05-21 <mark> by Richardtai, we can't read RSSI here, because no packet bound with RSSI
-
-	if ((pDevice->dwIsr & ISR_RXDMA0) &&
-	(pDevice->byLocalID != REV_ID_VT3253_B0) &&
-	(pDevice->bBSSIDFilter == true)) {
-	// update RSSI
-	//BBbReadEmbedded(pDevice->PortOffset, 0x3E, &byRSSI);
-	//pDevice->uCurrRSSI = byRSSI;
-	}
-	*/
 
 	handled = 1;
 	MACvIntDisable(pDevice->PortOffset);
@@ -2459,9 +2335,9 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
 
 	//Make sure current page is 0
 	VNSvInPortB(pDevice->PortOffset + MAC_REG_PAGE1SEL, &byOrgPageSel);
-	if (byOrgPageSel == 1) {
+	if (byOrgPageSel == 1)
 		MACvSelectPage0(pDevice->PortOffset);
-	} else
+	else
 		byOrgPageSel = 0;
 
 	MACvReadMIBCounter(pDevice->PortOffset, &dwMIBCounter);
@@ -2492,7 +2368,6 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
 				VNSvInPortD(pDevice->PortOffset + MAC_REG_MAR4, &(pDevice->dwOrgMAR4));
 				MACvSelectPage0(pDevice->PortOffset);
 				//xxxx
-				// WCMDbFlushCommandQueue(pDevice->pMgmt, true);
 				if (set_channel(pDevice, pDevice->pCurrMeasureEID->sReq.byChannel)) {
 					pDevice->bMeasureInProgress = true;
 					MACvSelectPage1(pDevice->PortOffset);
@@ -2500,12 +2375,11 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
 					MACvSelectPage0(pDevice->PortOffset);
 					pDevice->byBasicMap = 0;
 					pDevice->byCCAFraction = 0;
-					for (ii = 0; ii < 8; ii++) {
+					for (ii = 0; ii < 8; ii++)
 						pDevice->dwRPIs[ii] = 0;
-					}
+
 				} else {
 					// can not measure because set channel fail
-					// WCMDbResetCommandQueue(pDevice->pMgmt);
 					// clear measure control
 					MACvRegBitsOff(pDevice->PortOffset, MAC_REG_MSRCTL, MSRCTL_EN);
 					s_vCompleteCurrentMeasure(pDevice, MEASURE_MODE_INCAPABLE);
@@ -2529,7 +2403,6 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
 				MACvRegBitsOff(pDevice->PortOffset, MAC_REG_MSRCTL, MSRCTL_EN);
 				MACvSelectPage0(pDevice->PortOffset);
 				set_channel(pDevice, pDevice->byOrgChannel);
-				// WCMDbResetCommandQueue(pDevice->pMgmt);
 				MACvSelectPage1(pDevice->PortOffset);
 				MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL+1, MSRCTL1_TXPAUSE);
 				MACvSelectPage0(pDevice->PortOffset);
@@ -2572,9 +2445,7 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
 
 				}
 			}
-			if (pDevice->eOPMode == OP_MODE_ADHOC) {
-				//pDevice->bBeaconSent = false;
-			} else {
+			if (pDevice->eOPMode != OP_MODE_ADHOC) {
 				if ((pDevice->bUpdateBBVGA) && pDevice->bLinkPass && (pDevice->uCurrRSSI != 0)) {
 					long            ldBm;
 
@@ -2605,9 +2476,8 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
 			}
 
 			pDevice->bBeaconSent = false;
-			if (pDevice->bEnablePSMode) {
+			if (pDevice->bEnablePSMode)
 				PSbIsNextTBTTWakeUp((void *)pDevice);
-			}
 
 			if ((pDevice->eOPMode == OP_MODE_AP) ||
 			    (pDevice->eOPMode == OP_MODE_ADHOC)) {
@@ -2615,9 +2485,7 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
 							  (pMgmt->wIBSSBeaconPeriod - MAKE_BEACON_RESERVED) << 10);
 			}
 
-			if (pDevice->eOPMode == OP_MODE_ADHOC && pDevice->pMgmt->wCurrATIMWindow > 0) {
-				// todo adhoc PS mode
-			}
+			/* TODO: adhoc PS mode */
 
 		}
 
@@ -2651,27 +2519,24 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
 					MACvSelectPage1(pDevice->PortOffset);
 					MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL+1, MSRCTL1_TXPAUSE);
 					MACvSelectPage0(pDevice->PortOffset);
-					//VNTWIFIbSendBeacon(pDevice->pMgmt);
 					CARDbStartTxPacket(pDevice, PKT_TYPE_802_11_ALL);
 				}
 			}
 
 		}
 
-		if (pDevice->dwIsr & ISR_RXDMA0) {
+		if (pDevice->dwIsr & ISR_RXDMA0)
 			max_count += device_rx_srv(pDevice, TYPE_RXDMA0);
-		}
-		if (pDevice->dwIsr & ISR_RXDMA1) {
+
+		if (pDevice->dwIsr & ISR_RXDMA1)
 			max_count += device_rx_srv(pDevice, TYPE_RXDMA1);
-		}
-		if (pDevice->dwIsr & ISR_TXDMA0) {
+
+		if (pDevice->dwIsr & ISR_TXDMA0)
 			max_count += device_tx_srv(pDevice, TYPE_TXDMA0);
-		}
-		if (pDevice->dwIsr & ISR_AC0DMA) {
+
+		if (pDevice->dwIsr & ISR_AC0DMA)
 			max_count += device_tx_srv(pDevice, TYPE_AC0DMA);
-		}
-		if (pDevice->dwIsr & ISR_SOFTTIMER) {
-		}
+
 		if (pDevice->dwIsr & ISR_SOFTTIMER1) {
 			if (pDevice->eOPMode == OP_MODE_AP) {
 				if (pDevice->bShortSlotTime)
@@ -2692,9 +2557,8 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
 			break;
 	}
 
-	if (byOrgPageSel == 1) {
+	if (byOrgPageSel == 1)
 		MACvSelectPage1(pDevice->PortOffset);
-	}
 
 	spin_unlock_irq(&pDevice->lock);
 	MACvIntEnable(pDevice->PortOffset, IMR_MASK_VALUE);
@@ -3099,16 +2963,6 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd) {
 	case SIOCGIWPRIV:
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWPRIV \n");
 		rc = -EOPNOTSUPP;
-/*
-  if (wrq->u.data.pointer) {
-  wrq->u.data.length = sizeof(iwctl_private_args) / sizeof(iwctl_private_args[0]);
-
-  if (copy_to_user(wrq->u.data.pointer,
-  (u_char *) iwctl_private_args,
-  sizeof(iwctl_private_args)))
-  rc = -EFAULT;
-  }
-*/
 		break;
 
 //2008-0409-07, <Add> by Einsn Liu
@@ -3195,9 +3049,9 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd) {
 				rc = 0;
 			}
 
-		if (test_and_set_bit(0, (void *)&(pMgmt->uCmdBusy))) {
+		if (test_and_set_bit(0, (void *)&(pMgmt->uCmdBusy)))
 			return -EBUSY;
-		}
+
 		rc = private_ioctl(pDevice, rq);
 		clear_bit(0, (void *)&(pMgmt->uCmdBusy));
 		break;
@@ -3290,8 +3144,6 @@ static int __init vt6655_init_module(void)
 {
 	int ret;
 
-//    ret=pci_module_init(&device_driver);
-	//ret = pcie_port_service_register(&device_driver);
 	ret = pci_register_driver(&device_driver);
 #ifdef CONFIG_PM
 	if (ret >= 0)
diff --git a/drivers/staging/vt6655/dpc.c b/drivers/staging/vt6655/dpc.c
index 771bf35ae04..7ddaf2603ba 100644
--- a/drivers/staging/vt6655/dpc.c
+++ b/drivers/staging/vt6655/dpc.c
@@ -60,7 +60,6 @@
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 static int msglevel = MSG_LEVEL_INFO;
 
 const unsigned char acbyRxRate[MAX_RATE] =
@@ -182,11 +181,11 @@ s_vProcessRxMACHeader(PSDevice pDevice, unsigned char *pbyRxBufferAddr,
 			cbHeaderSize -= 8;
 			pwType = (unsigned short *)(pbyRxBufferAddr + cbHeaderSize);
 			if (bIsWEP) {
-				if (bExtIV) {
+				if (bExtIV)
 					*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 8);    // 8 is IV&ExtIV
-				} else {
+				else
 					*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 4);    // 4 is IV
-				}
+
 			} else {
 				*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN);
 			}
@@ -195,11 +194,11 @@ s_vProcessRxMACHeader(PSDevice pDevice, unsigned char *pbyRxBufferAddr,
 		cbHeaderSize -= 2;
 		pwType = (unsigned short *)(pbyRxBufferAddr + cbHeaderSize);
 		if (bIsWEP) {
-			if (bExtIV) {
+			if (bExtIV)
 				*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 8);    // 8 is IV&ExtIV
-			} else {
+			else
 				*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 4);    // 4 is IV
-			}
+
 		} else {
 			*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN);
 		}
@@ -223,6 +222,7 @@ static unsigned char s_byGetRateIdx(unsigned char byRate)
 		if (acbyRxRate[byRateIdx % MAX_RATE] == byRate)
 			return byRateIdx;
 	}
+
 	return 0;
 }
 
@@ -327,15 +327,11 @@ device_receive_frame(
 	PS802_11Header pMACHeader;
 	bool bRxeapol_key = false;
 
-//    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "---------- device_receive_frame---\n");
-
 	skb = pRDInfo->skb;
 
 //PLICE_DEBUG->
-#if 1
 	pci_unmap_single(pDevice->pcid, pRDInfo->skb_dma,
 			 pDevice->rx_buf_sz, PCI_DMA_FROMDEVICE);
-#endif
 //PLICE_DEBUG<-
 	pwFrameSize = (unsigned short *)(skb->data + 2);
 	FrameSize = cpu_to_le16(pCurrRD->m_rd1RD1.wReqCount) - cpu_to_le16(pCurrRD->m_rd0RD0.wResCount);
@@ -366,7 +362,6 @@ device_receive_frame(
 		return false;
 	}
 //PLICE_DEBUG->
-#if 1
 	// update receive statistic counter
 	STAvUpdateRDStatCounter(&pDevice->scStatistic,
 				*pbyRsr,
@@ -375,14 +370,12 @@ device_receive_frame(
 				pbyFrame,
 				FrameSize);
 
-#endif
-
 	pMACHeader = (PS802_11Header)((unsigned char *)(skb->data) + 8);
 //PLICE_DEBUG<-
 	if (pDevice->bMeasureInProgress) {
-		if ((*pbyRsr & RSR_CRCOK) != 0) {
+		if ((*pbyRsr & RSR_CRCOK) != 0)
 			pDevice->byBasicMap |= 0x01;
-		}
+
 		dwDuration = (FrameSize << 4);
 		dwDuration /= acbyRxRate[*pbyRxRate%MAX_RATE];
 		if (*pbyRxRate <= RATE_11M) {
@@ -399,9 +392,9 @@ device_receive_frame(
 		RFvRSSITodBm(pDevice, *pbyRSSI, &ldBm);
 		ldBmThreshold = -57;
 		for (ii = 7; ii > 0;) {
-			if (ldBm > ldBmThreshold) {
+			if (ldBm > ldBmThreshold)
 				break;
-			}
+
 			ldBmThreshold -= 5;
 			ii--;
 		}
@@ -436,9 +429,8 @@ device_receive_frame(
 	}
 
 	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-		if (s_bAPModeRxCtl(pDevice, pbyFrame, iSANodeIndex)) {
+		if (s_bAPModeRxCtl(pDevice, pbyFrame, iSANodeIndex))
 			return false;
-		}
 	}
 
 	if (IS_FC_WEP(pbyFrame)) {
@@ -488,13 +480,10 @@ device_receive_frame(
 				    (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) ||
 				    (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
 				    (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) {
-					if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_TKIP)) {
+					if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_TKIP))
 						pDevice->s802_11Counter.TKIPICVErrors++;
-					} else if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_CCMP)) {
+					else if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_CCMP))
 						pDevice->s802_11Counter.CCMPDecryptErrors++;
-					} else if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_WEP)) {
-//                      pDevice->s802_11Counter.WEPICVErrorCount.QuadPart++;
-					}
 				}
 				return false;
 			}
@@ -557,13 +546,9 @@ device_receive_frame(
 			pRxPacket->byRxRate = s_byGetRateIdx(*pbyRxRate);
 			pRxPacket->byRxChannel = (*pbyRxSts) >> 2;
 //PLICE_DEBUG->
-//EnQueue(pDevice,pRxPacket);
 
 #ifdef	THREAD
 			EnQueue(pDevice, pRxPacket);
-
-			//up(&pDevice->mlme_semaphore);
-			//Enque (pDevice->FirstRecvMngList,pDevice->LastRecvMngList,pMgmt);
 #else
 
 #ifdef	TASK_LET
@@ -571,12 +556,10 @@ device_receive_frame(
 			tasklet_schedule(&pDevice->RxMngWorkItem);
 #else
 			vMgrRxManagePacket((void *)pDevice, pDevice->pMgmt, pRxPacket);
-			//tasklet_schedule(&pDevice->RxMngWorkItem);
 #endif
 
 #endif
 //PLICE_DEBUG<-
-			//vMgrRxManagePacket((void *)pDevice, pDevice->pMgmt, pRxPacket);
 			// hostap Deamon handle 802.11 management
 			if (pDevice->bEnableHostapd) {
 				skb->dev = pDevice->apdev;
@@ -590,9 +573,8 @@ device_receive_frame(
 				netif_rx(skb);
 				return true;
 			}
-		} else {
-			// Control Frame
 		}
+
 		return false;
 	} else {
 		if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
@@ -643,14 +625,9 @@ device_receive_frame(
 // Data frame Handle
 
 	if (pDevice->bEnablePSMode) {
-		if (IS_FC_MOREDATA((skb->data+4))) {
-			if (*pbyRsr & RSR_ADDROK) {
-				//PSbSendPSPOLL((PSDevice)pDevice);
-			}
-		} else {
-			if (pDevice->pMgmt->bInTIMWake == true) {
+		if (!IS_FC_MOREDATA((skb->data+4))) {
+			if (pDevice->pMgmt->bInTIMWake == true)
 				pDevice->pMgmt->bInTIMWake = false;
-			}
 		}
 	}
 
@@ -661,9 +638,9 @@ device_receive_frame(
 		BBvAntennaDiversity(pDevice, s_byGetRateIdx(*pbyRxRate), 0);
 	}
 
-	if (pDevice->byLocalID != REV_ID_VT3253_B1) {
+	if (pDevice->byLocalID != REV_ID_VT3253_B1)
 		pDevice->uCurrRSSI = *pbyRSSI;
-	}
+
 	pDevice->byCurrSQ = *pbySQ;
 
 	if ((*pbyRSSI != 0) &&
@@ -673,11 +650,10 @@ device_receive_frame(
 		pMgmt->pCurrBSS->byRSSIStatCnt++;
 		pMgmt->pCurrBSS->byRSSIStatCnt %= RSSI_STAT_COUNT;
 		pMgmt->pCurrBSS->ldBmAverage[pMgmt->pCurrBSS->byRSSIStatCnt] = ldBm;
-		for (ii = 0; ii < RSSI_STAT_COUNT; ii++) {
-			if (pMgmt->pCurrBSS->ldBmAverage[ii] != 0) {
+		for (ii = 0; ii < RSSI_STAT_COUNT; ii++)
+			if (pMgmt->pCurrBSS->ldBmAverage[ii] != 0)
 				pMgmt->pCurrBSS->ldBmMAX = max(pMgmt->pCurrBSS->ldBmAverage[ii], ldBm);
-			}
-		}
+
 	}
 
 	// -----------------------------------------------
@@ -720,9 +696,8 @@ device_receive_frame(
 	}
 
 	if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_TKIP)) {
-		if (bIsWEP) {
+		if (bIsWEP)
 			FrameSize -= 8;  //MIC
-		}
 	}
 
 	//--------------------------------------------------------------------------------
@@ -765,16 +740,12 @@ device_receive_frame(
 
 			pdwMIC_L = (__le32 *)(skb->data + 4 + FrameSize);
 			pdwMIC_R = (__le32 *)(skb->data + 4 + FrameSize + 4);
-			//DBG_PRN_GRP12(("RxL: %lx, RxR: %lx\n", *pdwMIC_L, *pdwMIC_R));
-			//DBG_PRN_GRP12(("LocalL: %lx, LocalR: %lx\n", dwLocalMIC_L, dwLocalMIC_R));
-			//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dwMICKey0= %lx,dwMICKey1= %lx \n", dwMICKey0, dwMICKey1);
 
 			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");
 				pDevice->bRxMICFail = false;
-				//pDevice->s802_11Counter.TKIPLocalMICFailures.QuadPart++;
 				pDevice->s802_11Counter.TKIPLocalMICFailures++;
 				if (bDeFragRx) {
 					if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
@@ -813,10 +784,8 @@ device_receive_frame(
 					if ((pDevice->pMgmt->eCurrMode == WMAC_MODE_ESS_STA) &&
 					    (pDevice->pMgmt->eCurrState == WMAC_STATE_ASSOC) &&
 					    (*pbyRsr & (RSR_ADDRBROAD | RSR_ADDRMULTI)) == 0) {
-						//s802_11_Status.Flags = NDIS_802_11_AUTH_REQUEST_PAIRWISE_ERROR;
 						wpahdr->type = VIAWGET_PTK_MIC_MSG;
 					} else {
-						//s802_11_Status.Flags = NDIS_802_11_AUTH_REQUEST_GROUP_ERROR;
 						wpahdr->type = VIAWGET_GTK_MIC_MSG;
 					}
 					wpahdr->resp_ie_len = 0;
@@ -863,10 +832,8 @@ device_receive_frame(
 				    !((dwRxTSC47_16 == 0) && (dwLocalTSC47_16 == 0xFFFFFFFF))) {
 					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "TSC is illegal~~!\n ");
 					if (pKey->byCipherSuite == KEY_CTL_TKIP)
-						//pDevice->s802_11Counter.TKIPReplays.QuadPart++;
 						pDevice->s802_11Counter.TKIPReplays++;
 					else
-						//pDevice->s802_11Counter.CCMPReplays.QuadPart++;
 						pDevice->s802_11Counter.CCMPReplays++;
 
 					if (bDeFragRx) {
@@ -881,10 +848,6 @@ device_receive_frame(
 		}
 	} // ----- End of Reply Counter Check --------------------------
 
-	if ((pKey != NULL) && (bIsWEP)) {
-//      pDevice->s802_11Counter.DecryptSuccessCount.QuadPart++;
-	}
-
 	s_vProcessRxMACHeader(pDevice, (unsigned char *)(skb->data+4), FrameSize, bIsWEP, bExtIV, &cbHeaderOffset);
 	FrameSize -= cbHeaderOffset;
 	cbHeaderOffset += 4;        // 4 is Rcv buffer header
@@ -917,22 +880,6 @@ device_receive_frame(
 	skb->protocol = eth_type_trans(skb, skb->dev);
 
 	//drop frame not met IEEE 802.3
-/*
-  if (pDevice->flags & DEVICE_FLAGS_VAL_PKT_LEN) {
-  if ((skb->protocol==htons(ETH_P_802_3)) &&
-  (skb->len!=htons(skb->mac.ethernet->h_proto))) {
-  pStats->rx_length_errors++;
-  pStats->rx_dropped++;
-  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);
-  }
-  }
-  return false;
-  }
-  }
-*/
 
 	skb->ip_summed = CHECKSUM_NONE;
 	pStats->rx_bytes += skb->len;
@@ -1121,19 +1068,11 @@ static bool s_bHandleRxEncryption(
 
 	if (pKey == NULL) {
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey == NULL\n");
-		if (byDecMode == KEY_CTL_WEP) {
-//            pDevice->s802_11Counter.WEPUndecryptableCount.QuadPart++;
-		} else if (pDevice->bLinkPass) {
-//            pDevice->s802_11Counter.DecryptFailureCount.QuadPart++;
-		}
+
 		return false;
 	}
 	if (byDecMode != pKey->byCipherSuite) {
-		if (byDecMode == KEY_CTL_WEP) {
-//            pDevice->s802_11Counter.WEPUndecryptableCount.QuadPart++;
-		} else if (pDevice->bLinkPass) {
-//            pDevice->s802_11Counter.DecryptFailureCount.QuadPart++;
-		}
+
 		*pKeyOut = NULL;
 		return false;
 	}
@@ -1151,9 +1090,9 @@ static bool s_bHandleRxEncryption(
 			rc4_init(&pDevice->SBox, pDevice->abyPRNG, pKey->uKeyLength + 3);
 			rc4_encrypt(&pDevice->SBox, pbyIV+4, pbyIV+4, PayloadLen);
 
-			if (ETHbIsBufferCrc32Ok(pbyIV+4, PayloadLen)) {
+			if (ETHbIsBufferCrc32Ok(pbyIV+4, PayloadLen))
 				*pbyNewRsr |= NEWRSR_DECRYPTOK;
-			}
+
 		}
 	} else if ((byDecMode == KEY_CTL_TKIP) ||
 		   (byDecMode == KEY_CTL_CCMP)) {
@@ -1162,11 +1101,11 @@ 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);
-		if (byDecMode == KEY_CTL_TKIP) {
+		if (byDecMode == KEY_CTL_TKIP)
 			*pwRxTSC15_0 = cpu_to_le16(MAKEWORD(*(pbyIV + 2), *pbyIV));
-		} else {
+		else
 			*pwRxTSC15_0 = cpu_to_le16(*(unsigned short *)pbyIV);
-		}
+
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "TSC0_15: %x\n", *pwRxTSC15_0);
 
 		if ((byDecMode == KEY_CTL_TKIP) &&
@@ -1231,14 +1170,8 @@ static bool s_bHostWepRxEncryption(
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "AES:%d %d %d\n", pDevice->pMgmt->byCSSPK, pDevice->pMgmt->byCSSGK, byDecMode);
 
-	if (byDecMode != pKey->byCipherSuite) {
-		if (byDecMode == KEY_CTL_WEP) {
-//            pDevice->s802_11Counter.WEPUndecryptableCount.QuadPart++;
-		} else if (pDevice->bLinkPass) {
-//            pDevice->s802_11Counter.DecryptFailureCount.QuadPart++;
-		}
+	if (byDecMode != pKey->byCipherSuite)
 		return false;
-	}
 
 	if (byDecMode == KEY_CTL_WEP) {
 		// handle WEP
@@ -1257,9 +1190,9 @@ static bool s_bHostWepRxEncryption(
 			rc4_init(&pDevice->SBox, pDevice->abyPRNG, pKey->uKeyLength + 3);
 			rc4_encrypt(&pDevice->SBox, pbyIV+4, pbyIV+4, PayloadLen);
 
-			if (ETHbIsBufferCrc32Ok(pbyIV+4, PayloadLen)) {
+			if (ETHbIsBufferCrc32Ok(pbyIV+4, PayloadLen))
 				*pbyNewRsr |= NEWRSR_DECRYPTOK;
-			}
+
 		}
 	} else if ((byDecMode == KEY_CTL_TKIP) ||
 		   (byDecMode == KEY_CTL_CCMP)) {
@@ -1269,11 +1202,11 @@ static bool s_bHostWepRxEncryption(
 		*pdwRxTSC47_16 = cpu_to_le32(*(unsigned long *)(pbyIV + 4));
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ExtIV: %lx\n", *pdwRxTSC47_16);
 
-		if (byDecMode == KEY_CTL_TKIP) {
+		if (byDecMode == KEY_CTL_TKIP)
 			*pwRxTSC15_0 = cpu_to_le16(MAKEWORD(*(pbyIV+2), *pbyIV));
-		} else {
+		else
 			*pwRxTSC15_0 = cpu_to_le16(*(unsigned short *)pbyIV);
-		}
+
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "TSC0_15: %x\n", *pwRxTSC15_0);
 
 		if (byDecMode == KEY_CTL_TKIP) {
@@ -1386,9 +1319,8 @@ static bool s_bAPModeRxData(
 		if (bRelayAndForward)
 			iDANodeIndex = 0;
 
-		if ((pDevice->uAssocCount > 1) && (iDANodeIndex >= 0)) {
+		if ((pDevice->uAssocCount > 1) && (iDANodeIndex >= 0))
 			ROUTEbRelay(pDevice, (unsigned char *)(skb->data + cbHeaderOffset), FrameSize, (unsigned int)iDANodeIndex);
-		}
 
 		if (bRelayOnly)
 			return false;
diff --git a/drivers/staging/vt6655/hostap.c b/drivers/staging/vt6655/hostap.c
index 6eecd535891..317c2a8ee16 100644
--- a/drivers/staging/vt6655/hostap.c
+++ b/drivers/staging/vt6655/hostap.c
@@ -48,7 +48,6 @@
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 static int msglevel = MSG_LEVEL_INFO;
 
 /*---------------------  Static Functions  --------------------------*/
@@ -207,11 +206,11 @@ static int hostap_remove_sta(PSDevice pDevice,
 {
 	unsigned int uNodeIndex;
 
-	if (BSSDBbIsSTAInNodeDB(pDevice->pMgmt, param->sta_addr, &uNodeIndex)) {
+	if (BSSDBbIsSTAInNodeDB(pDevice->pMgmt, param->sta_addr, &uNodeIndex))
 		BSSvRemoveOneNode(pDevice, uNodeIndex);
-	} else {
+	else
 		return -ENOENT;
-	}
+
 	return 0;
 }
 
@@ -234,14 +233,13 @@ static int hostap_add_sta(PSDevice pDevice,
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	unsigned int uNodeIndex;
 
-	if (!BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &uNodeIndex)) {
+	if (!BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &uNodeIndex))
 		BSSvCreateOneNode((PSDevice)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
-//    pMgmt->sNodeDBTable[uNodeIndex].wListenInterval = 1;
 	pMgmt->sNodeDBTable[uNodeIndex].bPSEnable = false;
 	pMgmt->sNodeDBTable[uNodeIndex].bySuppRate = param->u.add_sta.tx_supp_rates;
 
@@ -296,8 +294,6 @@ static int hostap_get_info_sta(PSDevice pDevice,
 	if (BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &uNodeIndex)) {
 		param->u.get_info_sta.inactive_sec =
 			(jiffies - pMgmt->sNodeDBTable[uNodeIndex].ulLastRxJiffer) / HZ;
-
-		//param->u.get_info_sta.txexc = pMgmt->sNodeDBTable[uNodeIndex].uTxAttempts;
 	} else {
 		return -ENOENT;
 	}
@@ -307,37 +303,6 @@ static int hostap_get_info_sta(PSDevice pDevice,
 
 /*
  * Description:
- *      reset txexec
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *      true, false
- *
- * Return Value:
- *
- */
-/*
-  static int hostap_reset_txexc_sta(PSDevice pDevice,
-  struct viawget_hostapd_param *param)
-  {
-  PSMgmtObject    pMgmt = pDevice->pMgmt;
-  unsigned int uNodeIndex;
-
-  if (BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &uNodeIndex)) {
-  pMgmt->sNodeDBTable[uNodeIndex].uTxAttempts = 0;
-  } else {
-  return -ENOENT;
-  }
-
-  return 0;
-  }
-*/
-
-/*
- * Description:
  *      set station flag
  *
  * Parameters:
@@ -456,19 +421,12 @@ static int hostap_set_encryption(PSDevice pDevice,
 	unsigned char abySeq[MAX_KEY_LEN];
 	unsigned long long KeyRSC;
 	unsigned char byKeyDecMode = KEY_CTL_WEP;
-	int     ret = 0;
 	int     iNodeIndex = -1;
 	int     ii;
 	bool bKeyTableFull = false;
 	unsigned short wKeyCtl = 0;
 
 	param->u.crypt.err = 0;
-/*
-  if (param_len !=
-  (int) ((char *) param->u.crypt.key - (char *) param) +
-  param->u.crypt.key_len)
-  return -EINVAL;
-*/
 
 	if (param->u.crypt.alg > WPA_ALG_CCMP)
 		return -EINVAL;
@@ -516,7 +474,7 @@ static int hostap_set_encryption(PSDevice pDevice,
 		       MAX_KEY_LEN
 );
 
-		return ret;
+		return 0;
 	}
 
 	memcpy(abyKey, param->u.crypt.key, param->u.crypt.key_len);
@@ -572,7 +530,7 @@ static int hostap_set_encryption(PSDevice pDevice,
 		pMgmt->byCSSGK = KEY_CTL_WEP;
 		pMgmt->sNodeDBTable[iNodeIndex].byCipherSuite = KEY_CTL_WEP;
 		pMgmt->sNodeDBTable[iNodeIndex].dwKeyIndex = dwKeyIndex;
-		return ret;
+		return 0;
 	}
 
 	if (param->u.crypt.seq) {
@@ -663,7 +621,7 @@ static int hostap_set_encryption(PSDevice pDevice,
 	pMgmt->sNodeDBTable[iNodeIndex].dwTSC47_16 = 0;
 	pMgmt->sNodeDBTable[iNodeIndex].wTSC15_0 = 0;
 
-	return ret;
+	return 0;
 }
 
 /*
@@ -684,7 +642,6 @@ static int hostap_get_encryption(PSDevice pDevice,
 				 int param_len)
 {
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	int     ret = 0;
 	int     ii;
 	int     iNodeIndex = 0;
 
@@ -701,11 +658,10 @@ static int hostap_get_encryption(PSDevice pDevice,
 	}
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "hostap_get_encryption: %d\n", iNodeIndex);
 	memset(param->u.crypt.seq, 0, 8);
-	for (ii = 0; ii < 8; ii++) {
+	for (ii = 0; ii < 8; ii++)
 		param->u.crypt.seq[ii] = (unsigned char)pMgmt->sNodeDBTable[iNodeIndex].KeyRSC >> (ii * 8);
-	}
 
-	return ret;
+	return 0;
 }
 
 /*
@@ -780,12 +736,6 @@ int vt6655_hostap_ioctl(PSDevice pDevice, struct iw_point *p)
 		ret = hostap_get_info_sta(pDevice, param);
 		ap_ioctl = 1;
 		break;
-/*
-	case VIAWGET_HOSTAPD_RESET_TXEXC_STA:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_RESET_TXEXC_STA \n");
-		ret = hostap_reset_txexc_sta(pDevice, param);
-		break;
-*/
 	case VIAWGET_HOSTAPD_SET_FLAGS_STA:
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_SET_FLAGS_STA \n");
 		ret = hostap_set_flags_sta(pDevice, param);
@@ -814,9 +764,8 @@ int vt6655_hostap_ioctl(PSDevice pDevice, struct iw_point *p)
 	}
 
 	if ((ret == 0) && ap_ioctl) {
-		if (copy_to_user(p->pointer, param, p->length)) {
+		if (copy_to_user(p->pointer, param, p->length))
 			ret = -EFAULT;
-		}
 	}
 
 out:
diff --git a/drivers/staging/vt6655/ioctl.c b/drivers/staging/vt6655/ioctl.c
index b5cd2e44e53..18d11d1f4fa 100644
--- a/drivers/staging/vt6655/ioctl.c
+++ b/drivers/staging/vt6655/ioctl.c
@@ -305,7 +305,8 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 			result = -EINVAL;
 			break;
 		}
-		pList = (PSBSSIDList)kmalloc(sizeof(SBSSIDList) + (sList.uItem * sizeof(SBSSIDItem)), (int)GFP_ATOMIC);
+		pList = (PSBSSIDList)kmalloc(sizeof(SBSSIDList) + (sList.uItem * sizeof(SBSSIDItem)),
+					     GFP_ATOMIC);
 		if (pList == NULL) {
 			result = -ENOMEM;
 			break;
@@ -318,7 +319,6 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 				pList->sBSSIDList[ii].uChannel = pBSS->uChannel;
 				pList->sBSSIDList[ii].wBeaconInterval = pBSS->wBeaconInterval;
 				pList->sBSSIDList[ii].wCapInfo = pBSS->wCapInfo;
-				/* pList->sBSSIDList[ii].uRSSI = pBSS->uRSSI; */
 				RFvRSSITodBm(pDevice, (unsigned char)(pBSS->uRSSI), &ldBm);
 				pList->sBSSIDList[ii].uRSSI = (unsigned int)ldBm;
 				memcpy(pList->sBSSIDList[ii].abyBSSID, pBSS->abyBSSID, WLAN_BSSID_LEN);
@@ -576,7 +576,8 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 			result = -EINVAL;
 			break;
 		}
-		pNodeList = (PSNodeList)kmalloc(sizeof(SNodeList) + (sNodeList.uItem * sizeof(SNodeItem)), (int)GFP_ATOMIC);
+		pNodeList = (PSNodeList)kmalloc(sizeof(SNodeList) + (sNodeList.uItem * sizeof(SNodeItem)),
+						GFP_ATOMIC);
 		if (pNodeList == NULL) {
 			result = -ENOMEM;
 			break;
diff --git a/drivers/staging/vt6655/ioctl.h b/drivers/staging/vt6655/ioctl.h
index ae240fd71a1..2f0db920e2c 100644
--- a/drivers/staging/vt6655/ioctl.h
+++ b/drivers/staging/vt6655/ioctl.h
@@ -41,13 +41,4 @@
 
 int private_ioctl(PSDevice pDevice, struct ifreq *rq);
 
-/*
-  void vConfigWEPKey(
-  PSDevice pDevice,
-  unsigned long dwKeyIndex,
-  unsigned char *pbyKey,
-  unsigned long uKeyLength
-);
-*/
-
 #endif // __IOCTL_H__
diff --git a/drivers/staging/vt6655/iowpa.h b/drivers/staging/vt6655/iowpa.h
index bfea01f5e00..b7bd1909fa7 100644
--- a/drivers/staging/vt6655/iowpa.h
+++ b/drivers/staging/vt6655/iowpa.h
@@ -34,13 +34,6 @@
 #define WPA_IE_LEN 64
 
 //WPA related
-/*
-  typedef enum { WPA_ALG_NONE, WPA_ALG_WEP, WPA_ALG_TKIP, WPA_ALG_CCMP } wpa_alg;
-  typedef enum { CIPHER_NONE, CIPHER_WEP40, CIPHER_TKIP, CIPHER_CCMP,
-  CIPHER_WEP104 } wpa_cipher;
-  typedef enum { KEY_MGMT_802_1X, KEY_MGMT_PSK, KEY_MGMT_NONE,
-  KEY_MGMT_802_1X_NO_WPA, KEY_MGMT_WPA_NONE } wpa_key_mgmt;
-*/
 
 enum {
 	VIAWGET_SET_WPA = 1,
diff --git a/drivers/staging/vt6655/iwctl.c b/drivers/staging/vt6655/iwctl.c
index ac3fc16704c..ae2b87f177f 100644
--- a/drivers/staging/vt6655/iwctl.c
+++ b/drivers/staging/vt6655/iwctl.c
@@ -66,7 +66,6 @@ static const long frequency_list[] = {
 
 /*---------------------  Static Classes  ----------------------------*/
 
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 static int msglevel = MSG_LEVEL_INFO;
 
 /*---------------------  Static Variables  --------------------------*/
@@ -89,7 +88,6 @@ struct iw_statistics *iwctl_get_wireless_stats(struct net_device *dev)
 #endif
 	RFvRSSITodBm(pDevice, (unsigned char)(pDevice->uCurrRSSI), &ldBm);
 	pDevice->wstats.qual.level = ldBm;
-	//pDevice->wstats.qual.level = 0x100 - pDevice->uCurrRSSI;
 	pDevice->wstats.qual.noise = 0;
 	pDevice->wstats.qual.updated = 1;
 	pDevice->wstats.discard.nwid = 0;
@@ -237,11 +235,11 @@ int iwctl_giwscan(struct net_device *dev,
 			//ADD mode
 			memset(&iwe, 0, sizeof(iwe));
 			iwe.cmd = SIOCGIWMODE;
-			if (WLAN_GET_CAP_INFO_ESS(pBSS->wCapInfo)) {
+			if (WLAN_GET_CAP_INFO_ESS(pBSS->wCapInfo))
 				iwe.u.mode = IW_MODE_INFRA;
-			} else {
+			else
 				iwe.u.mode = IW_MODE_ADHOC;
-			}
+
 			iwe.len = IW_EV_UINT_LEN;
 			current_ev = iwe_stream_add_event(info, current_ev, end_buf, &iwe, IW_EV_UINT_LEN);
 			//ADD frequency
@@ -268,26 +266,25 @@ int iwctl_giwscan(struct net_device *dev,
 			iwe.u.qual.level = ldBm;
 			iwe.u.qual.noise = 0;
 //2008-0409-01, <Add> by Einsn Liu
-			if (-ldBm < 50) {
+			if (-ldBm < 50)
 				iwe.u.qual.qual = 100;
-			} else if (-ldBm > 90) {
+			else if (-ldBm > 90)
 				iwe.u.qual.qual = 0;
-			} else {
+			else
 				iwe.u.qual.qual = (40 - (-ldBm - 50)) * 100 / 40;
-			}
+
 			iwe.u.qual.updated = 7;
 
-			//  iwe.u.qual.qual = 0;
 			current_ev = iwe_stream_add_event(info, current_ev, end_buf, &iwe, IW_EV_QUAL_LEN);
 
 			memset(&iwe, 0, sizeof(iwe));
 			iwe.cmd = SIOCGIWENCODE;
 			iwe.u.data.length = 0;
-			if (WLAN_GET_CAP_INFO_PRIVACY(pBSS->wCapInfo)) {
+			if (WLAN_GET_CAP_INFO_PRIVACY(pBSS->wCapInfo))
 				iwe.u.data.flags = IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
-			} else {
+			else
 				iwe.u.data.flags = IW_ENCODE_DISABLED;
-			}
+
 			current_ev = iwe_stream_add_point(info, current_ev, end_buf, &iwe, pItemSSID->abySSID);
 
 			memset(&iwe, 0, sizeof(iwe));
@@ -440,9 +437,9 @@ int iwctl_siwmode(struct net_device *dev,
 	case IW_MODE_ADHOC:
 		if (pMgmt->eConfigMode != WMAC_CONFIG_IBSS_STA) {
 			pMgmt->eConfigMode = WMAC_CONFIG_IBSS_STA;
-			if (pDevice->flags & DEVICE_FLAGS_OPENED) {
+			if (pDevice->flags & DEVICE_FLAGS_OPENED)
 				pDevice->bCommit = true;
-			}
+
 		}
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to ad-hoc \n");
 		break;
@@ -450,9 +447,9 @@ int iwctl_siwmode(struct net_device *dev,
 	case IW_MODE_INFRA:
 		if (pMgmt->eConfigMode != WMAC_CONFIG_ESS_STA) {
 			pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
-			if (pDevice->flags & DEVICE_FLAGS_OPENED) {
+			if (pDevice->flags & DEVICE_FLAGS_OPENED)
 				pDevice->bCommit = true;
-			}
+
 		}
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to infrastructure \n");
 		break;
@@ -464,9 +461,9 @@ int iwctl_siwmode(struct net_device *dev,
 
 		if (pMgmt->eConfigMode != WMAC_CONFIG_AP) {
 			pMgmt->eConfigMode = WMAC_CONFIG_AP;
-			if (pDevice->flags & DEVICE_FLAGS_OPENED) {
+			if (pDevice->flags & DEVICE_FLAGS_OPENED)
 				pDevice->bCommit = true;
-			}
+
 		}
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to Access Point \n");
 		break;
@@ -640,7 +637,6 @@ int iwctl_siwap(struct net_device *dev,
 	if (pMgmt->eScanState ==  WMAC_IS_SCANNING) {
 		// In scanning..
 		printk("SIOCSIWAP(??)-->In scanning...\n");
-		//  return -EAGAIN;
 	}
 	if (wrq->sa_family != ARPHRD_ETHER)
 		rc = -EINVAL;
@@ -674,9 +670,9 @@ int iwctl_siwap(struct net_device *dev,
 			}
 		}
 
-		if (pDevice->flags & DEVICE_FLAGS_OPENED) {
+		if (pDevice->flags & DEVICE_FLAGS_OPENED)
 			pDevice->bCommit = true;
-		}
+
 	}
 	return rc;
 }
@@ -700,9 +696,8 @@ int iwctl_giwap(struct net_device *dev,
 	if ((pDevice->bLinkPass == false) && (pMgmt->eCurrMode != WMAC_MODE_ESS_AP))
 		memset(wrq->sa_data, 0, 6);
 
-	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
+	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP)
 		memcpy(wrq->sa_data, pMgmt->abyCurrBSSID, 6);
-	}
 
 	wrq->sa_family = ARPHRD_ETHER;
 
@@ -719,42 +714,66 @@ int iwctl_giwaplist(struct net_device *dev,
 		    char *extra)
 {
 	int ii, jj, rc = 0;
-	struct sockaddr sock[IW_MAX_AP];
-	struct iw_quality qual[IW_MAX_AP];
+	struct sockaddr *sock	= NULL;
+	struct sockaddr *s	= NULL;
+	struct iw_quality *qual	= NULL;
+	struct iw_quality *q	= NULL;
+	PKnownBSS pBSS		= NULL;
+
 	PSDevice pDevice = (PSDevice)netdev_priv(dev);
 	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWAPLIST \n");
-	// Only super-user can see AP list
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWAPLIST\n");
 
 	if (!capable(CAP_NET_ADMIN)) {
 		rc = -EPERM;
-		return rc;
+		goto exit;
 	}
 
-	if (wrq->pointer) {
-		PKnownBSS pBSS = &(pMgmt->sBSSList[0]);
+	if (!wrq->pointer)
+		goto exit;
 
-		for (ii = 0, jj = 0; ii < MAX_BSS_NUM; ii++) {
-			pBSS = &(pMgmt->sBSSList[ii]);
-			if (!pBSS->bActive)
-				continue;
-			if (jj >= IW_MAX_AP)
-				break;
-			memcpy(sock[jj].sa_data, pBSS->abyBSSID, 6);
-			sock[jj].sa_family = ARPHRD_ETHER;
-			qual[jj].level = pBSS->uRSSI;
-			qual[jj].qual = qual[jj].noise = 0;
-			qual[jj].updated = 2;
-			jj++;
-		}
+	sock = kmalloc_array(IW_MAX_AP, sizeof(struct sockaddr), GFP_KERNEL);
+	if (!sock) {
+		rc = -ENOMEM;
+		goto exit;
+	}
+
+	qual = kmalloc_array(IW_MAX_AP, sizeof(struct iw_quality), GFP_KERNEL);
+	if (!qual) {
+		rc = -ENOMEM;
+		goto exit;
+	}
+
+	for (ii = 0, jj = 0; ii < MAX_BSS_NUM; ii++) {
+		pBSS = &(pMgmt->sBSSList[ii]);
 
-		wrq->flags = 1; // Should be define'd
-		wrq->length = jj;
-		memcpy(extra, sock, sizeof(struct sockaddr)*jj);
-		memcpy(extra + sizeof(struct sockaddr)*jj, qual, sizeof(struct iw_quality)*jj);
+		if (!pBSS->bActive)
+			continue;
+		if (jj >= IW_MAX_AP)
+			break;
+
+		s = &sock[jj];
+		q = &qual[jj];
+
+		memcpy(s->sa_data, pBSS->abyBSSID, 6);
+		s->sa_family	= ARPHRD_ETHER;
+		q->level	= pBSS->uRSSI;
+		q->qual		= 0;
+		q->noise	= 0;
+		q->updated	= 2;
+		jj++;
 	}
 
+	wrq->flags = 1; /* Should be define'd */
+	wrq->length = jj;
+	memcpy(extra, sock, sizeof(struct sockaddr) * jj);
+	memcpy(extra + sizeof(struct sockaddr) * jj,
+		qual,
+		sizeof(struct iw_quality) * jj);
+exit:
+	kfree(sock);
+	kfree(qual);
 	return rc;
 }
 
@@ -778,7 +797,6 @@ int iwctl_siwessid(struct net_device *dev,
 	if (pMgmt->eScanState ==  WMAC_IS_SCANNING) {
 		// In scanning..
 		printk("SIOCSIWESSID(??)-->In scanning...\n");
-		//  return -EAGAIN;
 	}
 	// Check if we asked for `any'
 	if (wrq->flags == 0) {
@@ -862,9 +880,8 @@ int iwctl_siwessid(struct net_device *dev,
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set essid = %s \n", pItemSSID->abySSID);
 	}
 
-	if (pDevice->flags & DEVICE_FLAGS_OPENED) {
+	if (pDevice->flags & DEVICE_FLAGS_OPENED)
 		pDevice->bCommit = true;
-	}
 
 	return 0;
 }
@@ -889,7 +906,6 @@ int iwctl_giwessid(struct net_device *dev,
 
 	// Get the current SSID
 	pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
-	//pItemSSID = (PWLAN_IE_SSID)pMgmt->abyDesireSSID;
 	memcpy(extra, pItemSSID->abySSID , pItemSSID->len);
 	extra[pItemSSID->len] = '\0';
 	wrq->length = pItemSSID->len + 1;
@@ -1016,21 +1032,7 @@ int iwctl_giwrate(struct net_device *dev,
 				brate = abySupportedRates[TxRate_iwconfig];
 			}
 		} else brate = 0;
-//2007-0118-05,<Mark> by EinsnLiu
-//Mark the unnecessary sentences.
-/*
-  if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-  if (pDevice->byBBType == BB_TYPE_11B)
-  brate = 0x16;
-  if (pDevice->byBBType == BB_TYPE_11G)
-  brate = 0x6C;
-  if (pDevice->byBBType == BB_TYPE_11A)
-  brate = 0x6C;
-  }
-*/
-
-//		if (pDevice->uConnectionRate == 13)
-//                brate = abySupportedRates[pDevice->wCurrentRate];
+
 		wrq->value = brate * 500000;
 		// If more than one rate, set auto
 		if (pDevice->bFixRate == true)
@@ -1058,11 +1060,11 @@ int iwctl_siwrts(struct net_device *dev,
 		int rthr = wrq->value;
 		if (wrq->disabled)
 			rthr = 2312;
-		if ((rthr < 0) || (rthr > 2312)) {
+
+		if ((rthr < 0) || (rthr > 2312))
 			rc = -EINVAL;
-		} else {
+		else
 			pDevice->wRTSThreshold = rthr;
-		}
 	}
 
 	return 0;
@@ -1162,9 +1164,8 @@ int iwctl_siwretry(struct net_device *dev,
 			pDevice->byLongRetryLimit = wrq->value;
 		}
 	}
-	if (wrq->flags & IW_RETRY_LIFETIME) {
+	if (wrq->flags & IW_RETRY_LIFETIME)
 		pDevice->wMaxTransmitMSDULifetime = wrq->value;
-	}
 
 	return rc;
 }
@@ -1234,10 +1235,13 @@ int iwctl_siwencode(struct net_device *dev,
 		}
 
 		if (dwKeyIndex < 1 && ((wrq->flags & IW_ENCODE_NOKEY) == 0)) {//set default key
-			if (pDevice->byKeyIndex < WLAN_WEP_NKEYS) {
+			if (pDevice->byKeyIndex < WLAN_WEP_NKEYS)
 				dwKeyIndex = pDevice->byKeyIndex;
-			} else dwKeyIndex = 0;
-		} else dwKeyIndex--;
+			else
+				dwKeyIndex = 0;
+		} else {
+			dwKeyIndex--;
+		}
 
 		// Check the size of the key
 		if (wrq->length > WLAN_WEP232_KEYLEN) {
@@ -1261,9 +1265,8 @@ int iwctl_siwencode(struct net_device *dev,
 			memcpy(pDevice->abyKey, extra, wrq->length);
 
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "abyKey: ");
-			for (ii = 0; ii < wrq->length; ii++) {
+			for (ii = 0; ii < wrq->length; ii++)
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", pDevice->abyKey[ii]);
-			}
 
 			if (pDevice->flags & DEVICE_FLAGS_OPENED) {
 				spin_lock_irq(&pDevice->lock);
@@ -1319,82 +1322,6 @@ int iwctl_siwencode(struct net_device *dev,
 	}
 //End Modify,Einsn
 
-/*
-  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWENCODE \n");
-
-  // Check the size of the key
-  if (wrq->length > WLAN_WEP232_KEYLEN) {
-  rc = -EINVAL;
-  return rc;
-  }
-
-  if (dwKeyIndex > WLAN_WEP_NKEYS) {
-  rc = -EINVAL;
-  return rc;
-  }
-
-  if (dwKeyIndex > 0)
-  dwKeyIndex--;
-
-  // Send the key to the card
-  if (wrq->length > 0) {
-  if (wrq->length ==  WLAN_WEP232_KEYLEN) {
-  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "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");
-  } else if (wrq->length == WLAN_WEP40_KEYLEN) {
-  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set 40 bit wep key, index= %d\n", (int)dwKeyIndex);
-  }
-  memset(pDevice->abyKey, 0, WLAN_WEP232_KEYLEN);
-  memcpy(pDevice->abyKey, extra, wrq->length);
-
-  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "abyKey: ");
-  for (ii = 0; ii < wrq->length; ii++) {
-  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", pDevice->abyKey[ii]);
-  }
-
-  if (pDevice->flags & DEVICE_FLAGS_OPENED) {
-  spin_lock_irq(&pDevice->lock);
-  KeybSetDefaultKey(&(pDevice->sKey),
-  (unsigned long)(pDevice->byKeyIndex | (1 << 31)),
-  pDevice->uKeyLength,
-  NULL,
-  pDevice->abyKey,
-  KEY_CTL_WEP,
-  pDevice->PortOffset,
-  pDevice->byLocalID
-);
-  spin_unlock_irq(&pDevice->lock);
-  }
-  pDevice->byKeyIndex = (unsigned char)dwKeyIndex;
-  pDevice->uKeyLength = wrq->length;
-  pDevice->bTransmitKey = true;
-  pDevice->bEncryptionEnable = true;
-  pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-
-  // Do we want to just set the transmit key index ?
-  if (index < 4) {
-  pDevice->byKeyIndex = index;
-  } else if (!(wrq->flags & IW_ENCODE_MODE)) {
-  rc = -EINVAL;
-  return rc;
-  }
-  }
-  // Read the flags
-  if (wrq->flags & IW_ENCODE_DISABLED) {
-  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Disable WEP function\n");
-  pMgmt->bShareKeyAlgorithm = false;
-  pDevice->bEncryptionEnable = false;
-  pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
-  if (pDevice->flags & DEVICE_FLAGS_OPENED) {
-  spin_lock_irq(&pDevice->lock);
-  for (uu=0; uu<MAX_KEY_TABLE; uu++)
-  MACvDisableKeyEntry(pDevice->PortOffset, uu);
-  spin_unlock_irq(&pDevice->lock);
-  }
-  }
-*/
-
 	if (wrq->flags & IW_ENCODE_RESTRICTED) {
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enable WEP & ShareKey System\n");
 		pMgmt->bShareKeyAlgorithm = true;
@@ -1406,77 +1333,6 @@ int iwctl_siwencode(struct net_device *dev,
 	return rc;
 }
 
-/*
- * Wireless Handler : get encode mode
- */
-/*
-  int iwctl_giwencode(struct net_device *dev,
-  struct iw_request_info *info,
-  struct iw_point *wrq,
-  char *extra) {
-  PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-  PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-  int rc = 0;
-  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");
-//2007-0207-06,<Add> by EinsnLiu
-//the key index in iwconfig is 1-4 when our driver is 0-3
-//so it can't be used directly.
-//if the index is 0,we should used the index set by driver.
-if (index > WLAN_WEP_NKEYS) {
-rc = -EINVAL;
-return rc;
-}
-if (index<1) {//set default key
-if (pDevice->byKeyIndex<WLAN_WEP_NKEYS) {
-index=pDevice->byKeyIndex;
-}
-else index=0;
-} else index--;
-//End Add,Einsn
-
-memset(abyKey, 0, sizeof(abyKey));
-// Check encryption mode
-wrq->flags = IW_ENCODE_NOKEY;
-// Is WEP enabled ???
-if (pDevice->bEncryptionEnable)
-wrq->flags |=  IW_ENCODE_ENABLED;
-else
-wrq->flags |=  IW_ENCODE_DISABLED;
-
-if (pMgmt->bShareKeyAlgorithm)
-wrq->flags |=  IW_ENCODE_RESTRICTED;
-else
-wrq->flags |=  IW_ENCODE_OPEN;
-
-if (KeybGetKey(&(pDevice->sKey), pDevice->abyBroadcastAddr, (unsigned char)index , &pKey)) {
-wrq->length = pKey->uKeyLength;
-memcpy(abyKey, pKey->abyKey,  pKey->uKeyLength);
-//2007-0207-06,<Modify> by EinsnLiu
-//only get key success need to  copy data
-//index should +1.
-//there is not necessary to return -EINVAL when get key failed
-//if return -EINVAL,the encryption item can't be display by the command "iwconfig".
-wrq->flags |= index+1;
-memcpy(extra,  abyKey, WLAN_WEP232_KEYLEN);
-}
-
-//else {
-//    rc = -EINVAL;
-//     return rc;
-//  }
-
-//End Modify,Einsn
-
-return 0;
-}
-*/
-
-//2008-0409-06, <Add> by Einsn Liu
-
 int iwctl_giwencode(struct net_device *dev,
 		    struct iw_request_info *info,
 		    struct iw_point *wrq,
@@ -1491,16 +1347,17 @@ int iwctl_giwencode(struct net_device *dev,
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWENCODE\n");
 
-	if (index > WLAN_WEP_NKEYS) {
+	if (index > WLAN_WEP_NKEYS)
 		return	-EINVAL;
-	}
+
 	if (index < 1) {//get default key
-		if (pDevice->byKeyIndex < WLAN_WEP_NKEYS) {
+		if (pDevice->byKeyIndex < WLAN_WEP_NKEYS)
 			index = pDevice->byKeyIndex;
-		} else
+		else
 			index = 0;
-	} else
+	} else {
 		index--;
+	}
 
 	memset(abyKey, 0, WLAN_WEP232_KEYLEN);
 	// Check encryption mode
@@ -1657,38 +1514,36 @@ int iwctl_siwauth(struct net_device *dev,
 	switch (wrq->flags & IW_AUTH_INDEX) {
 	case IW_AUTH_WPA_VERSION:
 		wpa_version = wrq->value;
-		if (wrq->value == IW_AUTH_WPA_VERSION_DISABLED) {
+		if (wrq->value == IW_AUTH_WPA_VERSION_DISABLED)
 			PRINT_K("iwctl_siwauth:set WPADEV to disable at 1??????\n");
-			//pDevice->bWPADevEnable = false;
-		} else if (wrq->value == IW_AUTH_WPA_VERSION_WPA) {
+		else if (wrq->value == IW_AUTH_WPA_VERSION_WPA)
 			PRINT_K("iwctl_siwauth:set WPADEV to WPA1******\n");
-		} else {
+		else
 			PRINT_K("iwctl_siwauth:set WPADEV to WPA2******\n");
-		}
-		//pDevice->bWPASuppWextEnabled =true;
+
 		break;
 	case IW_AUTH_CIPHER_PAIRWISE:
 		pairwise = wrq->value;
-		if (pairwise == IW_AUTH_CIPHER_CCMP) {
+		if (pairwise == IW_AUTH_CIPHER_CCMP)
 			pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;
-		} else if (pairwise == IW_AUTH_CIPHER_TKIP) {
+		else if (pairwise == IW_AUTH_CIPHER_TKIP)
 			pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;
-		} else if (pairwise == IW_AUTH_CIPHER_WEP40 || pairwise == IW_AUTH_CIPHER_WEP104) {
+		else if (pairwise == IW_AUTH_CIPHER_WEP40 || pairwise == IW_AUTH_CIPHER_WEP104)
 			pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-		} else if (pairwise == IW_AUTH_CIPHER_NONE) {
-			//do nothing,einsn liu
-		} else pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
+		else if (pairwise == IW_AUTH_CIPHER_NONE)
+			; /* do nothing,einsn liu */
+		else
+			pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
 
 		break;
 	case IW_AUTH_CIPHER_GROUP:
 		if (wpa_version == IW_AUTH_WPA_VERSION_DISABLED)
 			break;
 		if (pairwise == IW_AUTH_CIPHER_NONE) {
-			if (wrq->value == IW_AUTH_CIPHER_CCMP) {
+			if (wrq->value == IW_AUTH_CIPHER_CCMP)
 				pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;
-			} else {
+			else
 				pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;
-			}
 		}
 		break;
 	case IW_AUTH_KEY_MGMT:
@@ -1696,13 +1551,15 @@ int iwctl_siwauth(struct net_device *dev,
 		if (wpa_version == IW_AUTH_WPA_VERSION_WPA2) {
 			if (wrq->value == IW_AUTH_KEY_MGMT_PSK)
 				pMgmt->eAuthenMode = WMAC_AUTH_WPA2PSK;
-			else pMgmt->eAuthenMode = WMAC_AUTH_WPA2;
+			else
+				pMgmt->eAuthenMode = WMAC_AUTH_WPA2;
 		} else if (wpa_version == IW_AUTH_WPA_VERSION_WPA) {
-			if (wrq->value == 0) {
+			if (wrq->value == 0)
 				pMgmt->eAuthenMode = WMAC_AUTH_WPANONE;
-			} else if (wrq->value == IW_AUTH_KEY_MGMT_PSK)
+			else if (wrq->value == IW_AUTH_KEY_MGMT_PSK)
 				pMgmt->eAuthenMode = WMAC_AUTH_WPAPSK;
-			else pMgmt->eAuthenMode = WMAC_AUTH_WPA;
+			else
+				pMgmt->eAuthenMode = WMAC_AUTH_WPA;
 		}
 
 		break;
@@ -1711,14 +1568,13 @@ int iwctl_siwauth(struct net_device *dev,
 	case IW_AUTH_DROP_UNENCRYPTED:
 		break;
 	case IW_AUTH_80211_AUTH_ALG:
-		if (wrq->value == IW_AUTH_ALG_OPEN_SYSTEM) {
+		if (wrq->value == IW_AUTH_ALG_OPEN_SYSTEM)
 			pMgmt->bShareKeyAlgorithm = false;
-		} else if (wrq->value == IW_AUTH_ALG_SHARED_KEY) {
+		else if (wrq->value == IW_AUTH_ALG_SHARED_KEY)
 			pMgmt->bShareKeyAlgorithm = true;
-		}
+
 		break;
 	case IW_AUTH_WPA_ENABLED:
-		//pDevice->bWPADevEnable = !! wrq->value;
 		break;
 	case IW_AUTH_RX_UNENCRYPTED_EAPOL:
 		break;
@@ -1733,7 +1589,6 @@ int iwctl_siwauth(struct net_device *dev,
 			pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
 			pMgmt->bShareKeyAlgorithm = false;
 			pMgmt->eAuthenMode = false;
-			//pDevice->bWPADevEnable = false;
 		}
 
 		break;
@@ -1741,15 +1596,7 @@ int iwctl_siwauth(struct net_device *dev,
 		ret = -EOPNOTSUPP;
 		break;
 	}
-/*
-  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wpa_version = %d\n",wpa_version);
-  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pairwise = %d\n",pairwise);
-  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->eEncryptionStatus = %d\n",pDevice->eEncryptionStatus);
-  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pMgmt->eAuthenMode  = %d\n",pMgmt->eAuthenMode);
-  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pMgmt->bShareKeyAlgorithm = %s\n",pMgmt->bShareKeyAlgorithm?"true":"false");
-  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->bEncryptionEnable = %s\n",pDevice->bEncryptionEnable?"true":"false");
-  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->bWPADevEnable = %s\n",pDevice->bWPADevEnable?"true":"false");
-*/
+
 	return ret;
 }
 
@@ -1808,11 +1655,12 @@ int iwctl_giwgenie(struct net_device *dev,
 	if (pMgmt->wWPAIELen > 0) {
 		wrq->length = pMgmt->wWPAIELen;
 		if (pMgmt->wWPAIELen <= space) {
-			if (copy_to_user(extra, pMgmt->abyWPAIE, pMgmt->wWPAIELen)) {
+			if (copy_to_user(extra, pMgmt->abyWPAIE, pMgmt->wWPAIELen))
 				ret = -EFAULT;
-			}
-		} else
+
+		} else {
 			ret = -E2BIG;
+		}
 	}
 
 	return ret;
@@ -1833,21 +1681,15 @@ int iwctl_siwencodeext(struct net_device *dev,
 	u8  seq[IW_ENCODE_SEQ_MAX_SIZE];
 	u8 key[64];
 	size_t seq_len = 0, key_len = 0;
-//
-	// int ii;
-	u8 *buf;
-	size_t blen;
+
 	u8 key_array[64];
 	int ret = 0;
 
 	PRINT_K("SIOCSIWENCODEEXT...... \n");
 
-	blen = sizeof(*param);
-	buf = kmalloc((int)blen, (int)GFP_KERNEL);
-	if (buf == NULL)
+	param = kzalloc(sizeof(*param), GFP_KERNEL);
+	if (param == NULL)
 		return -ENOMEM;
-	memset(buf, 0, blen);
-	param = (struct viawget_wpa_param *)buf;
 
 //recover alg_name
 	switch (ext->alg) {
@@ -1909,18 +1751,18 @@ int iwctl_siwencodeext(struct net_device *dev,
 //****set if current action is Network Manager count??
 //****this method is so foolish,but there is no other way???
 	if (param->u.wpa_key.alg_name == WPA_ALG_NONE) {
-		if (param->u.wpa_key.key_index == 0) {
+		if (param->u.wpa_key.key_index == 0)
 			pDevice->bwextcount++;
-		}
-		if ((pDevice->bwextcount == 1) && (param->u.wpa_key.key_index == 1)) {
+
+		if ((pDevice->bwextcount == 1) && (param->u.wpa_key.key_index == 1))
 			pDevice->bwextcount++;
-		}
-		if ((pDevice->bwextcount == 2) && (param->u.wpa_key.key_index == 2)) {
+
+		if ((pDevice->bwextcount == 2) && (param->u.wpa_key.key_index == 2))
 			pDevice->bwextcount++;
-		}
-		if ((pDevice->bwextcount == 3) && (param->u.wpa_key.key_index == 3)) {
+
+		if ((pDevice->bwextcount == 3) && (param->u.wpa_key.key_index == 3))
 			pDevice->bwextcount++;
-		}
+
 	}
 	if (pDevice->bwextcount == 4) {
 		printk("SIOCSIWENCODEEXT:Enable WPA WEXT SUPPORT!!!!!\n");
@@ -1954,7 +1796,6 @@ int iwctl_siwmlme(struct net_device *dev,
 	PSDevice			pDevice = (PSDevice)netdev_priv(dev);
 	PSMgmtObject	pMgmt = &(pDevice->sMgmtObj);
 	struct iw_mlme *mlme = (struct iw_mlme *)extra;
-	//u16 reason = cpu_to_le16(mlme->reason_code);
 	int ret = 0;
 
 	if (memcmp(pMgmt->abyCurrBSSID, mlme->addr.sa_data, ETH_ALEN)) {
@@ -1989,69 +1830,6 @@ int iwctl_siwmlme(struct net_device *dev,
  * Structures to export the Wireless Handlers
  */
 
-/*
-  static const iw_handler		iwctl_handler[] =
-  {
-  (iw_handler) iwctl_commit,      // SIOCSIWCOMMIT
-  (iw_handler) iwctl_giwname,     // SIOCGIWNAME
-  (iw_handler) NULL,				// SIOCSIWNWID
-  (iw_handler) NULL,				// SIOCGIWNWID
-  (iw_handler) iwctl_siwfreq,		// SIOCSIWFREQ
-  (iw_handler) iwctl_giwfreq,		// SIOCGIWFREQ
-  (iw_handler) iwctl_siwmode,		// SIOCSIWMODE
-  (iw_handler) iwctl_giwmode,		// SIOCGIWMODE
-  (iw_handler) NULL,		        // SIOCSIWSENS
-  (iw_handler) iwctl_giwsens,		        // SIOCGIWSENS
-  (iw_handler) NULL,		        // SIOCSIWRANGE
-  (iw_handler) iwctl_giwrange,		// SIOCGIWRANGE
-  (iw_handler) NULL,			// SIOCSIWPRIV
-  (iw_handler) NULL,			// SIOCGIWPRIV
-  (iw_handler) NULL,			// SIOCSIWSTATS
-  (iw_handler) NULL,                  // SIOCGIWSTATS
-  (iw_handler) NULL,                  // SIOCSIWSPY
-  (iw_handler) NULL,		            // SIOCGIWSPY
-  (iw_handler) NULL,				    // -- hole --
-  (iw_handler) NULL,				    // -- hole --
-  (iw_handler) iwctl_siwap,		    // SIOCSIWAP
-  (iw_handler) iwctl_giwap,		    // SIOCGIWAP
-  (iw_handler) NULL,				    // -- hole -- 0x16
-  (iw_handler) iwctl_giwaplist,       // SIOCGIWAPLIST
-  (iw_handler) iwctl_siwscan,         // SIOCSIWSCAN
-  (iw_handler) iwctl_giwscan,         // SIOCGIWSCAN
-  (iw_handler) iwctl_siwessid,		// SIOCSIWESSID
-  (iw_handler) iwctl_giwessid,		// SIOCGIWESSID
-  (iw_handler) NULL,		// SIOCSIWNICKN
-  (iw_handler) NULL,		// SIOCGIWNICKN
-  (iw_handler) NULL,				    // -- hole --
-  (iw_handler) NULL,				    // -- hole --
-  (iw_handler) iwctl_siwrate,		// SIOCSIWRATE 0x20
-  (iw_handler) iwctl_giwrate,		// SIOCGIWRATE
-  (iw_handler) iwctl_siwrts,		// SIOCSIWRTS
-  (iw_handler) iwctl_giwrts,		// SIOCGIWRTS
-  (iw_handler) iwctl_siwfrag,		// SIOCSIWFRAG
-  (iw_handler) iwctl_giwfrag,		// SIOCGIWFRAG
-  (iw_handler) NULL,		// SIOCSIWTXPOW
-  (iw_handler) NULL,		// SIOCGIWTXPOW
-  (iw_handler) iwctl_siwretry,		// SIOCSIWRETRY
-  (iw_handler) iwctl_giwretry,		// SIOCGIWRETRY
-  (iw_handler) iwctl_siwencode,		// SIOCSIWENCODE
-  (iw_handler) iwctl_giwencode,		// SIOCGIWENCODE
-  (iw_handler) iwctl_siwpower,		// SIOCSIWPOWER
-  (iw_handler) iwctl_giwpower,		// SIOCGIWPOWER
-  (iw_handler) NULL,			// -- hole --
-  (iw_handler) NULL,			// -- hole --
-  (iw_handler) iwctl_siwgenie,    // SIOCSIWGENIE
-  (iw_handler) iwctl_giwgenie,    // SIOCGIWGENIE
-  (iw_handler) iwctl_siwauth,		// SIOCSIWAUTH
-  (iw_handler) iwctl_giwauth,		// SIOCGIWAUTH
-  (iw_handler) iwctl_siwencodeext,		// SIOCSIWENCODEEXT
-  (iw_handler) iwctl_giwencodeext,		// SIOCGIWENCODEEXT
-  (iw_handler) NULL,				// SIOCSIWPMKSA
-  (iw_handler) NULL,				// -- hole --
-
-  };
-*/
-
 static const iw_handler		iwctl_handler[] =
 {
 	(iw_handler) iwctl_commit,      // SIOCSIWCOMMIT
@@ -2129,13 +1907,9 @@ const struct iw_handler_def	iwctl_handler_def =
 {
 	.get_wireless_stats = &iwctl_get_wireless_stats,
 	.num_standard	= sizeof(iwctl_handler)/sizeof(iw_handler),
-//	.num_private	= sizeof(iwctl_private_handler)/sizeof(iw_handler),
-//	.num_private_args = sizeof(iwctl_private_args)/sizeof(struct iw_priv_args),
 	.num_private	= 0,
 	.num_private_args = 0,
 	.standard	= (iw_handler *)iwctl_handler,
-//	.private	= (iw_handler *) iwctl_private_handler,
-//	.private_args	= (struct iw_priv_args *)iwctl_private_args,
 	.private	= NULL,
 	.private_args	= NULL,
 };
diff --git a/drivers/staging/vt6655/key.c b/drivers/staging/vt6655/key.c
index 78b5809b830..09a8bf50527 100644
--- a/drivers/staging/vt6655/key.c
+++ b/drivers/staging/vt6655/key.c
@@ -46,7 +46,6 @@
 
 /*---------------------  Static Variables  --------------------------*/
 static int msglevel = MSG_LEVEL_INFO;
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 /*---------------------  Static Functions  --------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
@@ -255,11 +254,10 @@ bool KeybSetKey(
 
 			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 ", pKey->uKeyLength);
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->abyKey: ");
-			for (ii = 0; ii < pKey->uKeyLength; ii++) {
+			for (ii = 0; ii < pKey->uKeyLength; ii++)
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", pKey->abyKey[ii]);
-			}
+
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
 
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->dwTSC47_16: %lx\n ", pKey->dwTSC47_16);
@@ -321,9 +319,9 @@ bool KeybSetKey(
 		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: ");
-		for (ii = 0; ii < pKey->uKeyLength; ii++) {
+		for (ii = 0; ii < pKey->uKeyLength; ii++)
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", pKey->abyKey[ii]);
-		}
+
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
 
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->dwTSC47_16: %lx\n ", pKey->dwTSC47_16);
@@ -361,9 +359,9 @@ bool KeybRemoveKey(
 	if (is_broadcast_ether_addr(pbyBSSID)) {
 		// delete all keys
 		if ((dwKeyIndex & PAIRWISE_KEY) != 0) {
-			for (i = 0; i < MAX_KEY_TABLE; i++) {
+			for (i = 0; i < MAX_KEY_TABLE; i++)
 				pTable->KeyTable[i].PairwiseKey.bKeyValid = false;
-			}
+
 			s_vCheckKeyTableValid(pTable, dwIoBase);
 			return true;
 		} else if ((dwKeyIndex & 0x000000FF) < MAX_GROUP_KEY) {
@@ -429,9 +427,9 @@ bool KeybRemoveAllKey(
 		if (pTable->KeyTable[i].bInUse &&
 		    ether_addr_equal(pTable->KeyTable[i].abyBSSID, pbyBSSID)) {
 			pTable->KeyTable[i].PairwiseKey.bKeyValid = false;
-			for (u = 0; u < MAX_GROUP_KEY; u++) {
+			for (u = 0; u < MAX_GROUP_KEY; u++)
 				pTable->KeyTable[i].GroupKey[u].bKeyValid = false;
-			}
+
 			pTable->KeyTable[i].dwGTKeyIndex = 0;
 			s_vCheckKeyTableValid(pTable, dwIoBase);
 			return true;
@@ -480,9 +478,8 @@ void KeyvRemoveAllWEPKey(
 {
 	int i;
 
-	for (i = 0; i < MAX_GROUP_KEY; i++) {
+	for (i = 0; i < MAX_GROUP_KEY; i++)
 		KeyvRemoveWEPKey(pTable, i, dwIoBase);
-	}
 }
 
 /*
@@ -517,9 +514,9 @@ bool KeybGetTransmitKey(
 
 					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "KeybGetTransmitKey:");
 					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "PAIRWISE_KEY: KeyTable.abyBSSID: ");
-					for (ii = 0; ii < 6; ii++) {
+					for (ii = 0; ii < 6; ii++)
 						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%x ", pTable->KeyTable[i].abyBSSID[ii]);
-					}
+
 					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
 
 					return true;
@@ -538,9 +535,9 @@ bool KeybGetTransmitKey(
 
 					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "KeybGetTransmitKey:");
 					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "GROUP_KEY: KeyTable.abyBSSID\n");
-					for (ii = 0; ii < 6; ii++) {
+					for (ii = 0; ii < 6; ii++)
 						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%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);
 
@@ -553,9 +550,9 @@ bool KeybGetTransmitKey(
 		} // BSSID match
 	}
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ERROR: NO Match BSSID !!! ");
-	for (ii = 0; ii < 6; ii++) {
+	for (ii = 0; ii < 6; ii++)
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", *(pbyBSSID+ii));
-	}
+
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
 	return false;
 }
@@ -623,11 +620,10 @@ bool KeybSetDefaultKey(
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enter KeybSetDefaultKey: %1x, %d \n", (int)dwKeyIndex, (int)uKeyLength);
 
-	if ((dwKeyIndex & PAIRWISE_KEY) != 0) {                  // Pairwise key
+	if ((dwKeyIndex & PAIRWISE_KEY) != 0) // Pairwise key
 		return false;
-	} else if ((dwKeyIndex & 0x000000FF) >= MAX_GROUP_KEY) {
+	else if ((dwKeyIndex & 0x000000FF) >= MAX_GROUP_KEY)
 		return false;
-	}
 
 	if (uKeyLength > MAX_KEY_LEN)
 		return false;
@@ -685,9 +681,9 @@ bool KeybSetDefaultKey(
 	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");
-	for (ii = 0; ii < pKey->uKeyLength; ii++) {
+	for (ii = 0; ii < pKey->uKeyLength; ii++)
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%x", pKey->abyKey[ii]);
-	}
+
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->dwTSC47_16: %lx\n", pKey->dwTSC47_16);
@@ -731,11 +727,10 @@ bool KeybSetAllGroupKey(
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enter KeybSetAllGroupKey: %lX\n", dwKeyIndex);
 
-	if ((dwKeyIndex & PAIRWISE_KEY) != 0) {                  // Pairwise key
+	if ((dwKeyIndex & PAIRWISE_KEY) != 0) // Pairwise key
 		return false;
-	} else if ((dwKeyIndex & 0x000000FF) >= MAX_GROUP_KEY) {
+	else if ((dwKeyIndex & 0x000000FF) >= MAX_GROUP_KEY)
 		return false;
-	}
 
 	for (i = 0; i < MAX_KEY_TABLE - 1; i++) {
 		if (pTable->KeyTable[i].bInUse) {
@@ -781,14 +776,10 @@ bool KeybSetAllGroupKey(
 			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: ");
-			for (ii = 0; ii < pKey->uKeyLength; ii++) {
+			for (ii = 0; ii < pKey->uKeyLength; ii++)
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", pKey->abyKey[ii]);
-			}
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
 
-			//DBG_PRN_GRP12(("pKey->dwTSC47_16: %lX\n ", pKey->dwTSC47_16));
-			//DBG_PRN_GRP12(("pKey->wTSC15_0: %X\n ", pKey->wTSC15_0));
-			//DBG_PRN_GRP12(("pKey->dwKeyIndex: %lX\n ", pKey->dwKeyIndex));
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
 
 		} // (pTable->KeyTable[i].bInUse == true)
 	}
diff --git a/drivers/staging/vt6655/key.h b/drivers/staging/vt6655/key.h
index 356e6de8f87..4b8b4b6bd6c 100644
--- a/drivers/staging/vt6655/key.h
+++ b/drivers/staging/vt6655/key.h
@@ -76,8 +76,6 @@ typedef struct tagSKeyTable
 	unsigned long dwGTKeyIndex;            // GroupTransmitKey Index
 	bool bInUse;
 	//2006-1116-01,<Modify> by NomadZhao
-	//unsigned short wKeyCtl;
-	//bool bSoftWEP;
 	bool bSoftWEP;
 	unsigned short wKeyCtl;      // for address of wKeyCtl at align 4
 
diff --git a/drivers/staging/vt6655/mac.c b/drivers/staging/vt6655/mac.c
index 0ec079fa039..af6876a01ee 100644
--- a/drivers/staging/vt6655/mac.c
+++ b/drivers/staging/vt6655/mac.c
@@ -74,7 +74,6 @@
 
 unsigned short TxRate_iwconfig;//2008-5-8 <add> by chester
 /*---------------------  Static Definitions -------------------------*/
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 static int msglevel = MSG_LEVEL_INFO;
 /*---------------------  Static Classes  ----------------------------*/
 
@@ -642,16 +641,14 @@ void MACvSaveContext(unsigned long dwIoBase, unsigned char *pbyCxtBuf)
 	int         ii;
 
 	// read page0 register
-	for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE0; ii++) {
+	for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE0; ii++)
 		VNSvInPortB((dwIoBase + ii), (pbyCxtBuf + ii));
-	}
 
 	MACvSelectPage1(dwIoBase);
 
 	// read page1 register
-	for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE1; ii++) {
+	for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE1; ii++)
 		VNSvInPortB((dwIoBase + ii), (pbyCxtBuf + MAC_MAX_CONTEXT_SIZE_PAGE0 + ii));
-	}
 
 	MACvSelectPage0(dwIoBase);
 }
@@ -676,25 +673,24 @@ void MACvRestoreContext(unsigned long dwIoBase, unsigned char *pbyCxtBuf)
 
 	MACvSelectPage1(dwIoBase);
 	// restore page1
-	for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE1; ii++) {
+	for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE1; ii++)
 		VNSvOutPortB((dwIoBase + ii), *(pbyCxtBuf + MAC_MAX_CONTEXT_SIZE_PAGE0 + ii));
-	}
+
 	MACvSelectPage0(dwIoBase);
 
 	// restore RCR,TCR,IMR...
-	for (ii = MAC_REG_RCR; ii < MAC_REG_ISR; ii++) {
+	for (ii = MAC_REG_RCR; ii < MAC_REG_ISR; ii++)
 		VNSvOutPortB(dwIoBase + ii, *(pbyCxtBuf + ii));
-	}
+
 	// restore MAC Config.
-	for (ii = MAC_REG_LRT; ii < MAC_REG_PAGE1SEL; ii++) {
+	for (ii = MAC_REG_LRT; ii < MAC_REG_PAGE1SEL; ii++)
 		VNSvOutPortB(dwIoBase + ii, *(pbyCxtBuf + ii));
-	}
+
 	VNSvOutPortB(dwIoBase + MAC_REG_CFG, *(pbyCxtBuf + MAC_REG_CFG));
 
 	// restore PS Config.
-	for (ii = MAC_REG_PSCFG; ii < MAC_REG_BBREGCTL; ii++) {
+	for (ii = MAC_REG_PSCFG; ii < MAC_REG_BBREGCTL; ii++)
 		VNSvOutPortB(dwIoBase + ii, *(pbyCxtBuf + ii));
-	}
 
 	// restore CURR_RX_DESC_ADDR, CURR_TX_DESC_ADDR
 	VNSvOutPortD(dwIoBase + MAC_REG_TXDMAPTR0, *(unsigned long *)(pbyCxtBuf + MAC_REG_TXDMAPTR0));
@@ -729,24 +725,20 @@ bool MACbCompareContext(unsigned long dwIoBase, unsigned char *pbyCxtBuf)
 
 	// compare CURR_RX_DESC_ADDR, CURR_TX_DESC_ADDR
 	VNSvInPortD(dwIoBase + MAC_REG_TXDMAPTR0, &dwData);
-	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_TXDMAPTR0)) {
+	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_TXDMAPTR0))
 		return false;
-	}
 
 	VNSvInPortD(dwIoBase + MAC_REG_AC0DMAPTR, &dwData);
-	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_AC0DMAPTR)) {
+	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_AC0DMAPTR))
 		return false;
-	}
 
 	VNSvInPortD(dwIoBase + MAC_REG_RXDMAPTR0, &dwData);
-	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_RXDMAPTR0)) {
+	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_RXDMAPTR0))
 		return false;
-	}
 
 	VNSvInPortD(dwIoBase + MAC_REG_RXDMAPTR1, &dwData);
-	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_RXDMAPTR1)) {
+	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_RXDMAPTR1))
 		return false;
-	}
 
 	return true;
 }
@@ -770,7 +762,6 @@ bool MACbSoftwareReset(unsigned long dwIoBase)
 	unsigned short ww;
 
 	// turn on HOSTCR_SOFTRST, just write 0x01 to reset
-	//MACvRegBitsOn(dwIoBase, MAC_REG_HOSTCR, HOSTCR_SOFTRST);
 	VNSvOutPortB(dwIoBase + MAC_REG_HOSTCR, 0x01);
 
 	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
@@ -809,7 +800,6 @@ bool MACbSafeSoftwareReset(unsigned long dwIoBase)
 	MACvSaveContext(dwIoBase, abyTmpRegData);
 	// do reset
 	bRetVal = MACbSoftwareReset(dwIoBase);
-	//BBvSoftwareReset(pDevice->PortOffset);
 	// restore MAC context, except CR0
 	MACvRestoreContext(dwIoBase, abyTmpRegData);
 
@@ -1022,11 +1012,6 @@ void MACvInitialize(unsigned long dwIoBase)
 	// disable force PME-enable
 	VNSvOutPortB(dwIoBase + MAC_REG_PMC1, PME_OVR);
 	// only 3253 A
-	/*
-	  MACvPwrEvntDisable(dwIoBase);
-	  // clear power status
-	  VNSvOutPortW(dwIoBase + MAC_REG_WAKEUPSR0, 0x0F0F);
-	*/
 
 	// do reset
 	MACbSoftwareReset(dwIoBase);
@@ -1063,21 +1048,21 @@ void MACvSetCurrRx0DescAddr(unsigned long dwIoBase, unsigned long dwCurrDescAddr
 	unsigned char byOrgDMACtl;
 
 	VNSvInPortB(dwIoBase + MAC_REG_RXDMACTL0, &byOrgDMACtl);
-	if (byOrgDMACtl & DMACTL_RUN) {
+	if (byOrgDMACtl & DMACTL_RUN)
 		VNSvOutPortB(dwIoBase + MAC_REG_RXDMACTL0+2, DMACTL_RUN);
-	}
+
 	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
 		VNSvInPortB(dwIoBase + MAC_REG_RXDMACTL0, &byData);
 		if (!(byData & DMACTL_RUN))
 			break;
 	}
-	if (ww == W_MAX_TIMEOUT) {
+
+	if (ww == W_MAX_TIMEOUT)
 		DBG_PORT80(0x13);
-	}
+
 	VNSvOutPortD(dwIoBase + MAC_REG_RXDMAPTR0, dwCurrDescAddr);
-	if (byOrgDMACtl & DMACTL_RUN) {
+	if (byOrgDMACtl & DMACTL_RUN)
 		VNSvOutPortB(dwIoBase + MAC_REG_RXDMACTL0, DMACTL_RUN);
-	}
 }
 
 /*
@@ -1101,21 +1086,21 @@ void MACvSetCurrRx1DescAddr(unsigned long dwIoBase, unsigned long dwCurrDescAddr
 	unsigned char byOrgDMACtl;
 
 	VNSvInPortB(dwIoBase + MAC_REG_RXDMACTL1, &byOrgDMACtl);
-	if (byOrgDMACtl & DMACTL_RUN) {
+	if (byOrgDMACtl & DMACTL_RUN)
 		VNSvOutPortB(dwIoBase + MAC_REG_RXDMACTL1+2, DMACTL_RUN);
-	}
+
 	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
 		VNSvInPortB(dwIoBase + MAC_REG_RXDMACTL1, &byData);
 		if (!(byData & DMACTL_RUN))
 			break;
 	}
-	if (ww == W_MAX_TIMEOUT) {
+	if (ww == W_MAX_TIMEOUT)
 		DBG_PORT80(0x14);
-	}
+
 	VNSvOutPortD(dwIoBase + MAC_REG_RXDMAPTR1, dwCurrDescAddr);
-	if (byOrgDMACtl & DMACTL_RUN) {
+	if (byOrgDMACtl & DMACTL_RUN)
 		VNSvOutPortB(dwIoBase + MAC_REG_RXDMACTL1, DMACTL_RUN);
-	}
+
 }
 
 /*
@@ -1139,21 +1124,20 @@ void MACvSetCurrTx0DescAddrEx(unsigned long dwIoBase, unsigned long dwCurrDescAd
 	unsigned char byOrgDMACtl;
 
 	VNSvInPortB(dwIoBase + MAC_REG_TXDMACTL0, &byOrgDMACtl);
-	if (byOrgDMACtl & DMACTL_RUN) {
+	if (byOrgDMACtl & DMACTL_RUN)
 		VNSvOutPortB(dwIoBase + MAC_REG_TXDMACTL0+2, DMACTL_RUN);
-	}
+
 	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
 		VNSvInPortB(dwIoBase + MAC_REG_TXDMACTL0, &byData);
 		if (!(byData & DMACTL_RUN))
 			break;
 	}
-	if (ww == W_MAX_TIMEOUT) {
+	if (ww == W_MAX_TIMEOUT)
 		DBG_PORT80(0x25);
-	}
+
 	VNSvOutPortD(dwIoBase + MAC_REG_TXDMAPTR0, dwCurrDescAddr);
-	if (byOrgDMACtl & DMACTL_RUN) {
+	if (byOrgDMACtl & DMACTL_RUN)
 		VNSvOutPortB(dwIoBase + MAC_REG_TXDMACTL0, DMACTL_RUN);
-	}
 }
 
 /*
@@ -1178,9 +1162,9 @@ void MACvSetCurrAC0DescAddrEx(unsigned long dwIoBase, unsigned long dwCurrDescAd
 	unsigned char byOrgDMACtl;
 
 	VNSvInPortB(dwIoBase + MAC_REG_AC0DMACTL, &byOrgDMACtl);
-	if (byOrgDMACtl & DMACTL_RUN) {
+	if (byOrgDMACtl & DMACTL_RUN)
 		VNSvOutPortB(dwIoBase + MAC_REG_AC0DMACTL+2, DMACTL_RUN);
-	}
+
 	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
 		VNSvInPortB(dwIoBase + MAC_REG_AC0DMACTL, &byData);
 		if (!(byData & DMACTL_RUN))
@@ -1191,18 +1175,16 @@ void MACvSetCurrAC0DescAddrEx(unsigned long dwIoBase, unsigned long dwCurrDescAd
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x26)\n");
 	}
 	VNSvOutPortD(dwIoBase + MAC_REG_AC0DMAPTR, dwCurrDescAddr);
-	if (byOrgDMACtl & DMACTL_RUN) {
+	if (byOrgDMACtl & DMACTL_RUN)
 		VNSvOutPortB(dwIoBase + MAC_REG_AC0DMACTL, DMACTL_RUN);
-	}
 }
 
 void MACvSetCurrTXDescAddr(int iTxType, unsigned long dwIoBase, unsigned long dwCurrDescAddr)
 {
-	if (iTxType == TYPE_AC0DMA) {
+	if (iTxType == TYPE_AC0DMA)
 		MACvSetCurrAC0DescAddrEx(dwIoBase, dwCurrDescAddr);
-	} else if (iTxType == TYPE_TXDMA0) {
+	else if (iTxType == TYPE_TXDMA0)
 		MACvSetCurrTx0DescAddrEx(dwIoBase, dwCurrDescAddr);
-	}
 }
 
 /*
@@ -1392,9 +1374,9 @@ bool MACbPSWakeup(unsigned long dwIoBase)
 	unsigned char byOrgValue;
 	unsigned int ww;
 	// Read PSCTL
-	if (MACbIsRegBitsOff(dwIoBase, MAC_REG_PSCTL, PSCTL_PS)) {
+	if (MACbIsRegBitsOff(dwIoBase, MAC_REG_PSCTL, PSCTL_PS))
 		return true;
-	}
+
 	// Disable PS
 	MACvRegBitsOff(dwIoBase, MAC_REG_PSCTL, PSCTL_PSEN);
 
@@ -1543,9 +1525,9 @@ void MACvSetDefaultKeyEntry(unsigned long dwIoBase, unsigned int uKeyLen,
 		VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
 	}
 	dwData = *pdwKey;
-	if (uKeyLen == WLAN_WEP104_KEYLEN) {
+	if (uKeyLen == WLAN_WEP104_KEYLEN)
 		dwData |= 0x80000000;
-	}
+
 	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset+3);
 	VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
 	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
diff --git a/drivers/staging/vt6655/mac.h b/drivers/staging/vt6655/mac.h
index 4615db05549..7333b8b526f 100644
--- a/drivers/staging/vt6655/mac.h
+++ b/drivers/staging/vt6655/mac.h
@@ -1042,7 +1042,6 @@ void MACvSetKeyEntry(unsigned long dwIoBase, unsigned short wKeyCtl, unsigned in
 void MACvDisableKeyEntry(unsigned long dwIoBase, unsigned int uEntryIdx);
 void MACvSetDefaultKeyEntry(unsigned long dwIoBase, unsigned int uKeyLen,
 			    unsigned int uKeyIdx, unsigned long *pdwKey, unsigned char byLocalID);
-//void MACvEnableDefaultKey(unsigned long dwIoBase, unsigned char byLocalID);
 void MACvDisableDefaultKey(unsigned long dwIoBase);
 void MACvSetDefaultTKIPKeyEntry(unsigned long dwIoBase, unsigned int uKeyLen,
 				unsigned int uKeyIdx, unsigned long *pdwKey, unsigned char byLocalID);
diff --git a/drivers/staging/vt6655/mib.c b/drivers/staging/vt6655/mib.c
index 6a59652052b..36895970a89 100644
--- a/drivers/staging/vt6655/mib.c
+++ b/drivers/staging/vt6655/mib.c
@@ -187,75 +187,75 @@ void STAvUpdateRDStatCounter(PSStatCounter pStatistic,
 
 	if (byRxRate == 22) {
 		pStatistic->CustomStat.ullRsr11M++;
-		if (byRSR & RSR_CRCOK) {
+		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);
 	} else if (byRxRate == 11) {
 		pStatistic->CustomStat.ullRsr5M++;
-		if (byRSR & RSR_CRCOK) {
+		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);
 	} else if (byRxRate == 4) {
 		pStatistic->CustomStat.ullRsr2M++;
-		if (byRSR & RSR_CRCOK) {
+		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);
 	} else if (byRxRate == 2) {
 		pStatistic->CustomStat.ullRsr1M++;
-		if (byRSR & RSR_CRCOK) {
+		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);
 	} else if (byRxRate == 12) {
 		pStatistic->CustomStat.ullRsr6M++;
-		if (byRSR & RSR_CRCOK) {
+		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);
 	} else if (byRxRate == 18) {
 		pStatistic->CustomStat.ullRsr9M++;
-		if (byRSR & RSR_CRCOK) {
+		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);
 	} else if (byRxRate == 24) {
 		pStatistic->CustomStat.ullRsr12M++;
-		if (byRSR & RSR_CRCOK) {
+		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);
 	} else if (byRxRate == 36) {
 		pStatistic->CustomStat.ullRsr18M++;
-		if (byRSR & RSR_CRCOK) {
+		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);
 	} else if (byRxRate == 48) {
 		pStatistic->CustomStat.ullRsr24M++;
-		if (byRSR & RSR_CRCOK) {
+		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);
 	} else if (byRxRate == 72) {
 		pStatistic->CustomStat.ullRsr36M++;
-		if (byRSR & RSR_CRCOK) {
+		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);
 	} else if (byRxRate == 96) {
 		pStatistic->CustomStat.ullRsr48M++;
-		if (byRSR & RSR_CRCOK) {
+		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);
 	} else if (byRxRate == 108) {
 		pStatistic->CustomStat.ullRsr54M++;
-		if (byRSR & RSR_CRCOK) {
+		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);
 	} else {
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Unknown: Total[%d], CRCOK[%d]\n", (int)pStatistic->dwRsrRxPacket+1, (int)pStatistic->dwRsrCRCOk);
@@ -288,13 +288,12 @@ void STAvUpdateRDStatCounter(PSStatCounter pStatistic,
 	pStatistic->dwRsrRxPacket++;
 	pStatistic->dwRsrRxOctet += cbFrameLength;
 
-	if (IS_TYPE_DATA(pbyBuffer)) {
+	if (IS_TYPE_DATA(pbyBuffer))
 		pStatistic->dwRsrRxData++;
-	} else if (IS_TYPE_MGMT(pbyBuffer)) {
+	else if (IS_TYPE_MGMT(pbyBuffer))
 		pStatistic->dwRsrRxManage++;
-	} else if (IS_TYPE_CONTROL(pbyBuffer)) {
+	else if (IS_TYPE_CONTROL(pbyBuffer))
 		pStatistic->dwRsrRxControl++;
-	}
 
 	if (byRSR & RSR_ADDRBROAD)
 		pStatistic->dwRsrBroadcast++;
@@ -306,23 +305,22 @@ void STAvUpdateRDStatCounter(PSStatCounter pStatistic,
 	if (WLAN_GET_FC_MOREFRAG(pHeader->wFrameCtl))
 		pStatistic->dwRsrRxFragment++;
 
-	if (cbFrameLength < ETH_ZLEN + 4) {
+	if (cbFrameLength < ETH_ZLEN + 4)
 		pStatistic->dwRsrRunt++;
-	} else if (cbFrameLength == ETH_ZLEN + 4) {
+	else if (cbFrameLength == ETH_ZLEN + 4)
 		pStatistic->dwRsrRxFrmLen64++;
-	} else if ((65 <= cbFrameLength) && (cbFrameLength <= 127)) {
+	else if ((65 <= cbFrameLength) && (cbFrameLength <= 127))
 		pStatistic->dwRsrRxFrmLen65_127++;
-	} else if ((128 <= cbFrameLength) && (cbFrameLength <= 255)) {
+	else if ((128 <= cbFrameLength) && (cbFrameLength <= 255))
 		pStatistic->dwRsrRxFrmLen128_255++;
-	} else if ((256 <= cbFrameLength) && (cbFrameLength <= 511)) {
+	else if ((256 <= cbFrameLength) && (cbFrameLength <= 511))
 		pStatistic->dwRsrRxFrmLen256_511++;
-	} else if ((512 <= cbFrameLength) && (cbFrameLength <= 1023)) {
+	else if ((512 <= cbFrameLength) && (cbFrameLength <= 1023))
 		pStatistic->dwRsrRxFrmLen512_1023++;
-	} else if ((1024 <= cbFrameLength) && (cbFrameLength <= ETH_FRAME_LEN + 4)) {
+	else if ((1024 <= cbFrameLength) && (cbFrameLength <= ETH_FRAME_LEN + 4))
 		pStatistic->dwRsrRxFrmLen1024_1518++;
-	} else if (cbFrameLength > ETH_FRAME_LEN + 4) {
+	else if (cbFrameLength > ETH_FRAME_LEN + 4)
 		pStatistic->dwRsrLong++;
-	}
 }
 
 /*
@@ -399,11 +397,11 @@ STAvUpdateTDStatCounter(
 	unsigned char byTSR0_NCR = byTSR0 & TSR0_NCR;
 
 	pHeader = (PWLAN_80211HDR_A4) pbyBuffer;
-	if (WLAN_GET_FC_TODS(pHeader->wFrameCtl) == 0) {
+	if (WLAN_GET_FC_TODS(pHeader->wFrameCtl) == 0)
 		pbyDestAddr = &(pHeader->abyAddr1[0]);
-	} else {
+	else
 		pbyDestAddr = &(pHeader->abyAddr3[0]);
-	}
+
 	// increase tx packet count
 	pStatistic->dwTsrTxPacket[uIdx]++;
 	pStatistic->dwTsrTxOctet[uIdx] += cbFrameLength;
@@ -504,7 +502,6 @@ STAvUpdate802_11Counter(
 	unsigned long dwCounter
 )
 {
-	//p802_11Counter->TransmittedFragmentCount
 	p802_11Counter->MulticastTransmittedFrameCount = (unsigned long long) (pStatistic->dwTsrBroadcast[TYPE_AC0DMA] +
 									       pStatistic->dwTsrBroadcast[TYPE_TXDMA0] +
 									       pStatistic->dwTsrMulticast[TYPE_AC0DMA] +
@@ -513,12 +510,10 @@ STAvUpdate802_11Counter(
 	p802_11Counter->RetryCount = (unsigned long long) (pStatistic->dwTsrRetry[TYPE_AC0DMA] + pStatistic->dwTsrRetry[TYPE_TXDMA0]);
 	p802_11Counter->MultipleRetryCount = (unsigned long long) (pStatistic->dwTsrMoreThanOnceRetry[TYPE_AC0DMA] +
 								   pStatistic->dwTsrMoreThanOnceRetry[TYPE_TXDMA0]);
-	//p802_11Counter->FrameDuplicateCount
 	p802_11Counter->RTSSuccessCount += (unsigned long long)  (dwCounter & 0x000000ff);
 	p802_11Counter->RTSFailureCount += (unsigned long long) ((dwCounter & 0x0000ff00) >> 8);
 	p802_11Counter->ACKFailureCount += (unsigned long long) ((dwCounter & 0x00ff0000) >> 16);
 	p802_11Counter->FCSErrorCount +=   (unsigned long long) ((dwCounter & 0xff000000) >> 24);
-	//p802_11Counter->ReceivedFragmentCount
 	p802_11Counter->MulticastReceivedFrameCount = (unsigned long long) (pStatistic->dwRsrBroadcast +
 									    pStatistic->dwRsrMulticast);
 }
diff --git a/drivers/staging/vt6655/mib.h b/drivers/staging/vt6655/mib.h
index 6b99c119b78..c0a59489184 100644
--- a/drivers/staging/vt6655/mib.h
+++ b/drivers/staging/vt6655/mib.h
@@ -61,10 +61,6 @@ typedef struct tagSDot11Counters {
 	unsigned long long   CCMPReplays;
 	unsigned long long   CCMPDecryptErrors;
 	unsigned long long   FourWayHandshakeFailures;
-//    unsigned long long   WEPUndecryptableCount;
-//    unsigned long long   WEPICVErrorCount;
-//    unsigned long long   DecryptSuccessCount;
-//    unsigned long long   DecryptFailureCount;
 } SDot11Counters, *PSDot11Counters;
 
 //
@@ -304,10 +300,6 @@ typedef struct tagSStatCounter {
 	unsigned long long   ullTxMulticastBytes[TYPE_MAXTD];
 	unsigned long long   ullTxDirectedBytes[TYPE_MAXTD];
 
-//    unsigned long dwTxRetryCount[8];
-	//
-	// ISR status count
-	//
 	SISRCounters ISRStat;
 
 	SCustomCounters CustomStat;
diff --git a/drivers/staging/vt6655/michael.c b/drivers/staging/vt6655/michael.c
index ade4c855f20..edee48777aa 100644
--- a/drivers/staging/vt6655/michael.c
+++ b/drivers/staging/vt6655/michael.c
@@ -47,10 +47,7 @@
 /*---------------------  Static Variables  --------------------------*/
 
 /*---------------------  Static Functions  --------------------------*/
-/*
-  static unsigned long s_dwGetUINT32(unsigned char *p);         // Get unsigned long from 4 bytes LSByte first
-  static void s_vPutUINT32(unsigned char *p, unsigned long val); // Put unsigned long into 4 bytes LSByte first
-*/
+
 static void s_vClear(void);                       // Clear the internal message,
 // resets the object to the state just after construction.
 static void s_vSetKey(u32  dwK0, u32  dwK1);
@@ -65,30 +62,6 @@ static unsigned int nBytesInM;      // # bytes in M
 
 /*---------------------  Export Functions  --------------------------*/
 
-/*
-  static unsigned long s_dwGetUINT32 (unsigned char *p)
-// Convert from unsigned char [] to unsigned long in a portable way
-{
-unsigned long res = 0;
-unsigned int i;
-for (i=0; i<4; i++)
-{
-	res |= (*p++) << (8 * i);
-}
-return res;
-}
-
-static void s_vPutUINT32 (unsigned char *p, unsigned long val)
-// Convert from unsigned long to unsigned char [] in a portable way
-{
-	unsigned int i;
-	for (i=0; i<4; i++) {
-		*p++ = (unsigned char) (val & 0xff);
-		val >>= 8;
-	}
-}
-*/
-
 static void s_vClear(void)
 {
 	// Reset the state to the empty message.
@@ -164,9 +137,9 @@ void MIC_vGetMIC(u32 *pdwL, u32 *pdwR)
 	s_vAppendByte(0);
 	s_vAppendByte(0);
 	// and then zeroes until the length is a multiple of 4
-	while (nBytesInM != 0) {
+	while (nBytesInM != 0)
 		s_vAppendByte(0);
-	}
+
 	// The s_vAppendByte function has already computed the result.
 	*pdwL = L;
 	*pdwR = R;
diff --git a/drivers/staging/vt6655/power.c b/drivers/staging/vt6655/power.c
index 4bd1ccb7951..5dfa911c6f4 100644
--- a/drivers/staging/vt6655/power.c
+++ b/drivers/staging/vt6655/power.c
@@ -95,14 +95,12 @@ PSvEnablePowerSaving(
 	if (wListenInterval >= 2) {
 		// clear always listen beacon
 		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_ALBCN);
-		//pDevice->wCFG &= ~CFG_ALB;
 		// first time set listen next beacon
 		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_LNBCN);
 		pMgmt->wCountToWakeUp = wListenInterval;
 	} else {
 		// always listen beacon
 		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_ALBCN);
-		//pDevice->wCFG |= CFG_ALB;
 		pMgmt->wCountToWakeUp = 0;
 	}
 
@@ -110,13 +108,10 @@ PSvEnablePowerSaving(
 	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_PSEN);
 	pDevice->bEnablePSMode = true;
 
-	if (pDevice->eOPMode == OP_MODE_ADHOC) {
-//        bMgrPrepareBeaconToSend((void *)pDevice, pMgmt);
-	}
-	// We don't send null pkt in ad hoc mode since beacon will handle this.
-	else if (pDevice->eOPMode == OP_MODE_INFRASTRUCTURE) {
+	/* 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)
 		PSbSendNullPacket(pDevice);
-	}
+
 	pDevice->bPWBitOn = true;
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "PS:Power Saving Mode Enable... \n");
 	return;
@@ -138,7 +133,6 @@ PSvDisablePowerSaving(
 )
 {
 	PSDevice        pDevice = (PSDevice)hDeviceContext;
-//    PSMgmtObject    pMgmt = pDevice->pMgmt;
 
 	// disable power saving hw function
 	MACbPSWakeup(pDevice->PortOffset);
@@ -151,9 +145,9 @@ PSvDisablePowerSaving(
 
 	pDevice->bEnablePSMode = false;
 
-	if (pDevice->eOPMode == OP_MODE_INFRASTRUCTURE) {
+	if (pDevice->eOPMode == OP_MODE_INFRASTRUCTURE)
 		PSbSendNullPacket(pDevice);
-	}
+
 	pDevice->bPWBitOn = false;
 	return;
 }
@@ -258,8 +252,6 @@ PSvSendPSPOLL(
 	// send the frame
 	if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Send PS-Poll packet failed..\n");
-	} else {
-//        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Send PS-Poll packet success..\n");
 	}
 
 	return;
@@ -284,17 +276,15 @@ PSbSendNullPacket(
 	PSMgmtObject        pMgmt = pDevice->pMgmt;
 	unsigned int uIdx;
 
-	if (!pDevice->bLinkPass) {
+	if (!pDevice->bLinkPass)
 		return false;
-	}
+
 #ifdef TxInSleep
-	if (!pDevice->bEnablePSMode && !pDevice->fTxDataInSleep) {
+	if (!pDevice->bEnablePSMode && !pDevice->fTxDataInSleep)
 		return false;
-	}
 #else
-	if (!pDevice->bEnablePSMode) {
+	if (!pDevice->bEnablePSMode)
 		return false;
-	}
 #endif
 	if (pDevice->bEnablePSMode) {
 		for (uIdx = 0; uIdx < TYPE_MAXTD; uIdx++) {
@@ -323,9 +313,8 @@ PSbSendNullPacket(
 ));
 	}
 
-	if (pMgmt->eCurrMode != WMAC_MODE_IBSS_STA) {
+	if (pMgmt->eCurrMode != WMAC_MODE_IBSS_STA)
 		pTxPacket->p80211Header->sA3.wFrameCtl |= cpu_to_le16((unsigned short)WLAN_SET_FC_TODS(1));
-	}
 
 	memcpy(pTxPacket->p80211Header->sA3.abyAddr1, pMgmt->abyCurrBSSID, WLAN_ADDR_LEN);
 	memcpy(pTxPacket->p80211Header->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
@@ -336,8 +325,6 @@ PSbSendNullPacket(
 	if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Send Null Packet failed !\n");
 		return false;
-	} else {
-//            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Send Null Packet success....\n");
 	}
 
 	return true;
@@ -363,9 +350,8 @@ PSbIsNextTBTTWakeUp(
 	bool bWakeUp = false;
 
 	if (pMgmt->wListenInterval >= 2) {
-		if (pMgmt->wCountToWakeUp == 0) {
+		if (pMgmt->wCountToWakeUp == 0)
 			pMgmt->wCountToWakeUp = pMgmt->wListenInterval;
-		}
 
 		pMgmt->wCountToWakeUp--;
 
diff --git a/drivers/staging/vt6655/power.h b/drivers/staging/vt6655/power.h
index 337dd659f12..ce56124acf1 100644
--- a/drivers/staging/vt6655/power.h
+++ b/drivers/staging/vt6655/power.h
@@ -42,9 +42,6 @@
 
 /*---------------------  Export Functions  --------------------------*/
 
-// PSDevice pDevice
-// PSDevice hDeviceContext
-
 bool
 PSbConsiderPowerDown(
 	void *hDeviceContext,
diff --git a/drivers/staging/vt6655/rf.c b/drivers/staging/vt6655/rf.c
index edb1b2768b1..99c89a14d89 100644
--- a/drivers/staging/vt6655/rf.c
+++ b/drivers/staging/vt6655/rf.c
@@ -39,8 +39,6 @@
 
 /*---------------------  Static Definitions -------------------------*/
 
-//static int          msglevel                =MSG_LEVEL_INFO;
-
 #define BY_AL2230_REG_LEN     23 //24bit
 #define CB_AL2230_INIT_SEQ    15
 #define SWITCH_CHANNEL_DELAY_AL2230 200 //us
@@ -183,7 +181,6 @@ static const unsigned long dwAL7230InitTable[CB_AL7230_INIT_SEQ] = {
 	0x841FF200+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11a: 451FE2
 	0x3FDFA300+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11a: 5FDFA3
 	0x7FD78400+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // 11b/g    // Need modify for 11a
-	//0x802B4500+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11a: 8D1B45
 	// RoberYu:20050113, Rev0.47 Regsiter Setting Guide
 	0x802B5500+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11a: 8D1B55
 	0x56AF3600+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW,
@@ -192,7 +189,6 @@ static const unsigned long dwAL7230InitTable[CB_AL7230_INIT_SEQ] = {
 	0x221BB900+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW,
 	0xE0000A00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11a: E0600A
 	0x08031B00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // init 0x080B1B00 => 0x080F1B00 for 3 wire control TxGain(D10)
-	//0x00093C00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11a: 00143C
 	// RoberYu:20050113, Rev0.47 Regsiter Setting Guide
 	0x000A3C00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW, // Need modify for 11a: 00143C
 	0xFFFFFD00+(BY_AL7230_REG_LEN<<3)+IFREGCTL_REGW,
@@ -585,10 +581,9 @@ bool IFRFbWriteEmbedded(unsigned long dwIoBase, unsigned long dwData)
 			break;
 	}
 
-	if (ww == W_MAX_TIMEOUT) {
-//        DBG_PORT80_ALWAYS(0x32);
+	if (ww == W_MAX_TIMEOUT)
 		return false;
-	}
+
 	return true;
 }
 
@@ -839,13 +834,12 @@ bool RFvWriteWakeProgSyn(unsigned long dwIoBase, unsigned char byRFType, unsigne
 
 		byInitCount = CB_AL2230_INIT_SEQ + 2; // Init Reg + Channel Reg (2)
 		bySleepCount = 0;
-		if (byInitCount > (MISCFIFO_SYNDATASIZE - bySleepCount)) {
+		if (byInitCount > (MISCFIFO_SYNDATASIZE - bySleepCount))
 			return false;
-		}
 
-		for (ii = 0; ii < CB_AL2230_INIT_SEQ; ii++) {
+		for (ii = 0; ii < CB_AL2230_INIT_SEQ; ii++)
 			MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL2230InitTable[ii]);
-		}
+
 		MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL2230ChannelTable0[uChannel-1]);
 		ii++;
 		MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL2230ChannelTable1[uChannel-1]);
@@ -856,18 +850,15 @@ bool RFvWriteWakeProgSyn(unsigned long dwIoBase, unsigned char byRFType, unsigne
 	case RF_AIROHA7230:
 		byInitCount = CB_AL7230_INIT_SEQ + 3; // Init Reg + Channel Reg (3)
 		bySleepCount = 0;
-		if (byInitCount > (MISCFIFO_SYNDATASIZE - bySleepCount)) {
+		if (byInitCount > (MISCFIFO_SYNDATASIZE - bySleepCount))
 			return false;
-		}
 
 		if (uChannel <= CB_MAX_CHANNEL_24G) {
-			for (ii = 0; ii < CB_AL7230_INIT_SEQ; ii++) {
+			for (ii = 0; ii < CB_AL7230_INIT_SEQ; ii++)
 				MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL7230InitTable[ii]);
-			}
 		} else {
-			for (ii = 0; ii < CB_AL7230_INIT_SEQ; ii++) {
+			for (ii = 0; ii < CB_AL7230_INIT_SEQ; ii++)
 				MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL7230InitTableAMode[ii]);
-			}
 		}
 
 		MACvSetMISCFifo(dwIoBase, (unsigned short)(MISCFIFO_SYNDATA_IDX + ii), dwAL7230ChannelTable0[uChannel-1]);
@@ -916,12 +907,11 @@ bool RFbSetPower(
 	unsigned char byDec = 0;
 	unsigned char byPwrdBm = 0;
 
-	if (pDevice->dwDiagRefCount != 0) {
+	if (pDevice->dwDiagRefCount != 0)
 		return true;
-	}
-	if ((uCH < 1) || (uCH > CB_MAX_CHANNEL)) {
+
+	if ((uCH < 1) || (uCH > CB_MAX_CHANNEL))
 		return false;
-	}
 
 	switch (uRATE) {
 	case RATE_1M:
@@ -930,22 +920,19 @@ bool RFbSetPower(
 	case RATE_11M:
 		byPwr = pDevice->abyCCKPwrTbl[uCH];
 		byPwrdBm = pDevice->abyCCKDefaultPwr[uCH];
-//PLICE_DEBUG->
-		//byPwr+=5;
-//PLICE_DEBUG <-
 		break;
 	case RATE_6M:
 	case RATE_9M:
 	case RATE_18M:
 		byPwr = pDevice->abyOFDMPwrTbl[uCH];
-		if (pDevice->byRFType == RF_UW2452) {
+		if (pDevice->byRFType == RF_UW2452)
 			byDec = byPwr + 14;
-		} else {
+		else
 			byDec = byPwr + 10;
-		}
-		if (byDec >= pDevice->byMaxPwrLevel) {
+
+		if (byDec >= pDevice->byMaxPwrLevel)
 			byDec = pDevice->byMaxPwrLevel-1;
-		}
+
 		if (pDevice->byRFType == RF_UW2452) {
 			byPwrdBm = byDec - byPwr;
 			byPwrdBm /= 3;
@@ -953,11 +940,9 @@ bool RFbSetPower(
 			byPwrdBm = byDec - byPwr;
 			byPwrdBm >>= 1;
 		}
+
 		byPwrdBm += pDevice->abyOFDMDefaultPwr[uCH];
 		byPwr = byDec;
-//PLICE_DEBUG->
-		//byPwr+=5;
-//PLICE_DEBUG<-
 		break;
 	case RATE_24M:
 	case RATE_36M:
@@ -965,20 +950,16 @@ bool RFbSetPower(
 	case RATE_54M:
 		byPwr = pDevice->abyOFDMPwrTbl[uCH];
 		byPwrdBm = pDevice->abyOFDMDefaultPwr[uCH];
-//PLICE_DEBUG->
-		//byPwr+=5;
-//PLICE_DEBUG<-
 		break;
 	}
 
-	if (pDevice->byCurPwr == byPwr) {
+	if (pDevice->byCurPwr == byPwr)
 		return true;
-	}
 
 	bResult = RFbRawSetPower(pDevice, byPwr, uRATE);
-	if (bResult) {
+	if (bResult)
 		pDevice->byCurPwr = byPwr;
-	}
+
 	return bResult;
 }
 
@@ -1005,17 +986,17 @@ bool RFbRawSetPower(
 	bool bResult = true;
 	unsigned long dwMax7230Pwr = 0;
 
-	if (byPwr >=  pDevice->byMaxPwrLevel) {
+	if (byPwr >=  pDevice->byMaxPwrLevel)
 		return false;
-	}
+
 	switch (pDevice->byRFType) {
 	case RF_AIROHA:
 		bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, dwAL2230PowerTable[byPwr]);
-		if (uRATE <= RATE_11M) {
+		if (uRATE <= RATE_11M)
 			bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x0001B400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
-		} else {
+		else
 			bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x0005A400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
-		}
+
 		break;
 
 	case RF_AL2230S:
diff --git a/drivers/staging/vt6655/rxtx.c b/drivers/staging/vt6655/rxtx.c
index c2653eb4f76..2219d71885a 100644
--- a/drivers/staging/vt6655/rxtx.c
+++ b/drivers/staging/vt6655/rxtx.c
@@ -68,7 +68,6 @@
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 static int msglevel = MSG_LEVEL_INFO;
 
 #define	PLICE_DEBUG
@@ -230,14 +229,14 @@ s_vFillTxKey(
 		*pdwIV |= (unsigned long)byKeyIndex << 30;
 		*pdwIV = cpu_to_le32(*pdwIV);
 		pDevice->dwIVCounter++;
-		if (pDevice->dwIVCounter > WEP_IV_MASK) {
+		if (pDevice->dwIVCounter > WEP_IV_MASK)
 			pDevice->dwIVCounter = 0;
-		}
+
 	} else if (pTransmitKey->byCipherSuite == KEY_CTL_TKIP) {
 		pTransmitKey->wTSC15_0++;
-		if (pTransmitKey->wTSC15_0 == 0) {
+		if (pTransmitKey->wTSC15_0 == 0)
 			pTransmitKey->dwTSC47_16++;
-		}
+
 		TKIPvMixKey(pTransmitKey->abyKey, pDevice->abyCurrentNetAddr,
 			    pTransmitKey->wTSC15_0, pTransmitKey->dwTSC47_16, pDevice->abyPRNG);
 		memcpy(pbyBuf, pDevice->abyPRNG, 16);
@@ -251,9 +250,9 @@ s_vFillTxKey(
 
 	} else if (pTransmitKey->byCipherSuite == KEY_CTL_CCMP) {
 		pTransmitKey->wTSC15_0++;
-		if (pTransmitKey->wTSC15_0 == 0) {
+		if (pTransmitKey->wTSC15_0 == 0)
 			pTransmitKey->dwTSC47_16++;
-		}
+
 		memcpy(pbyBuf, pTransmitKey->abyKey, 16);
 
 		// Make IV
@@ -278,11 +277,11 @@ s_vFillTxKey(
 
 		//Fill MICHDR1
 		*((unsigned char *)(pMICHDR+16)) = 0; // HLEN[15:8]
-		if (pDevice->bLongHeader) {
+		if (pDevice->bLongHeader)
 			*((unsigned char *)(pMICHDR+17)) = 28; // HLEN[7:0]
-		} else {
+		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);
@@ -294,9 +293,9 @@ s_vFillTxKey(
 		wValue &= 0x000F;
 		wValue = cpu_to_le16(wValue);
 		memcpy(pMICHDR+38, (unsigned char *)&wValue, 2); // MSKSEQCTL
-		if (pDevice->bLongHeader) {
+		if (pDevice->bLongHeader)
 			memcpy(pMICHDR+40, &(pMACHeader->abyAddr4[0]), 6);
-		}
+
 	}
 }
 
@@ -359,17 +358,15 @@ s_uGetTxRsvTime(
 	unsigned int uDataTime, uAckTime;
 
 	uDataTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, cbFrameLength, wRate);
-	if (byPktType == PK_TYPE_11B) {//llb,CCK mode
+	if (byPktType == PK_TYPE_11B) //llb,CCK mode
 		uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, (unsigned short)pDevice->byTopCCKBasicRate);
-	} else {//11g 2.4G OFDM mode & 11a 5G OFDM mode
+	else //11g 2.4G OFDM mode & 11a 5G OFDM mode
 		uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, (unsigned short)pDevice->byTopOFDMBasicRate);
-	}
 
-	if (bNeedAck) {
+	if (bNeedAck)
 		return uDataTime + pDevice->uSIFS + uAckTime;
-	} else {
+	else
 		return uDataTime;
-	}
 }
 
 //byFreqType: 0=>5GHZ 1=>2.4GHZ
@@ -429,9 +426,8 @@ s_uGetDataDuration(
 	bool bLastFrag = 0;
 	unsigned int uAckTime = 0, uNextPktTime = 0;
 
-	if (uFragIdx == (uMACfragNum-1)) {
+	if (uFragIdx == (uMACfragNum-1))
 		bLastFrag = 1;
-	}
 
 	switch (byDurType) {
 	case DATADUR_B:    //DATADUR_B
@@ -443,11 +439,11 @@ s_uGetDataDuration(
 				return 0;
 			}
 		} else {//First Frag or Mid Frag
-			if (uFragIdx == (uMACfragNum-2)) {
+			if (uFragIdx == (uMACfragNum-2))
 				uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wRate, bNeedAck);
-			} else {
+			else
 				uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
-			}
+
 			if (bNeedAck) {
 				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
 				return pDevice->uSIFS + uAckTime + uNextPktTime;
@@ -466,11 +462,11 @@ s_uGetDataDuration(
 				return 0;
 			}
 		} else {//First Frag or Mid Frag
-			if (uFragIdx == (uMACfragNum-2)) {
+			if (uFragIdx == (uMACfragNum-2))
 				uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wRate, bNeedAck);
-			} else {
+			else
 				uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
-			}
+
 			if (bNeedAck) {
 				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
 				return pDevice->uSIFS + uAckTime + uNextPktTime;
@@ -495,22 +491,22 @@ s_uGetDataDuration(
 				else if (wRate > RATE_54M)
 					wRate = RATE_54M;
 
-				if (uFragIdx == (uMACfragNum-2)) {
+				if (uFragIdx == (uMACfragNum-2))
 					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
-				} else {
+				else
 					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
-				}
+
 			} else { // (byFBOption == AUTO_FB_1)
 				if (wRate < RATE_18M)
 					wRate = RATE_18M;
 				else if (wRate > RATE_54M)
 					wRate = RATE_54M;
 
-				if (uFragIdx == (uMACfragNum-2)) {
+				if (uFragIdx == (uMACfragNum-2))
 					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wFB_Opt1[FB_RATE0][wRate-RATE_18M], bNeedAck);
-				} else {
+				else
 					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE0][wRate-RATE_18M], bNeedAck);
-				}
+
 			}
 
 			if (bNeedAck) {
@@ -537,11 +533,10 @@ s_uGetDataDuration(
 				else if (wRate > RATE_54M)
 					wRate = RATE_54M;
 
-				if (uFragIdx == (uMACfragNum-2)) {
+				if (uFragIdx == (uMACfragNum-2))
 					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
-				} else {
+				else
 					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
-				}
 
 			} else { // (byFBOption == AUTO_FB_1)
 				if (wRate < RATE_18M)
@@ -549,11 +544,10 @@ s_uGetDataDuration(
 				else if (wRate > RATE_54M)
 					wRate = RATE_54M;
 
-				if (uFragIdx == (uMACfragNum-2)) {
+				if (uFragIdx == (uMACfragNum-2))
 					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wFB_Opt1[FB_RATE1][wRate-RATE_18M], bNeedAck);
-				} else {
+				else
 					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE1][wRate-RATE_18M], bNeedAck);
-				}
 			}
 			if (bNeedAck) {
 				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
@@ -609,54 +603,54 @@ s_uGetRTSCTSDuration(
 
 	case RTSDUR_BA_F0: //RTSDuration_ba_f0
 		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
-		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
 			uDurTime = uCTSTime + 2 * pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
-		} else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+		else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
 			uDurTime = uCTSTime + 2 * pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE0][wRate-RATE_18M], bNeedAck);
-		}
+
 		break;
 
 	case RTSDUR_AA_F0: //RTSDuration_aa_f0
 		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
-		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
 			uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
-		} else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+		else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
 			uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE0][wRate-RATE_18M], bNeedAck);
-		}
+
 		break;
 
 	case RTSDUR_BA_F1: //RTSDuration_ba_f1
 		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
-		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
 			uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
-		} else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+		else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
 			uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE1][wRate-RATE_18M], bNeedAck);
-		}
+
 		break;
 
 	case RTSDUR_AA_F1: //RTSDuration_aa_f1
 		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
-		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
 			uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
-		} else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+		else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
 			uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE1][wRate-RATE_18M], bNeedAck);
-		}
+
 		break;
 
 	case CTSDUR_BA_F0: //CTSDuration_ba_f0
-		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
 			uDurTime = pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
-		} else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+		else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
 			uDurTime = pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE0][wRate-RATE_18M], bNeedAck);
-		}
+
 		break;
 
 	case CTSDUR_BA_F1: //CTSDuration_ba_f1
-		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
 			uDurTime = pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
-		} else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M)) {
+		else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
 			uDurTime = pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE1][wRate-RATE_18M], bNeedAck);
-		}
+
 		break;
 
 	default:
@@ -684,9 +678,9 @@ s_uFillDataHead(
 {
 	unsigned short wLen = 0x0000;
 
-	if (pTxDataHead == NULL) {
+	if (pTxDataHead == NULL)
 		return 0;
-	}
+
 
 	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
 		if (byFBOption == AUTO_FB_NONE) {
@@ -849,11 +843,11 @@ s_vFillRTSHead(
 			} else {
 				memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
 			}
-			if (pDevice->eOPMode == OP_MODE_AP) {
+			if (pDevice->eOPMode == OP_MODE_AP)
 				memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-			} else {
+			else
 				memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
-			}
+
 		} else {
 			PSRTS_g_FB pBuf = (PSRTS_g_FB)pvRTS;
 			//Get SignalField,ServiceField,Length
@@ -885,11 +879,10 @@ s_vFillRTSHead(
 				memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
 			}
 
-			if (pDevice->eOPMode == OP_MODE_AP) {
+			if (pDevice->eOPMode == OP_MODE_AP)
 				memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-			} else {
+			else
 				memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
-			}
 
 		} // if (byFBOption == AUTO_FB_NONE)
 	} else if (byPktType == PK_TYPE_11A) {
@@ -913,11 +906,10 @@ s_vFillRTSHead(
 				memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
 			}
 
-			if (pDevice->eOPMode == OP_MODE_AP) {
+			if (pDevice->eOPMode == OP_MODE_AP)
 				memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-			} else {
+			else
 				memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
-			}
 
 		} else {
 			PSRTS_a_FB pBuf = (PSRTS_a_FB)pvRTS;
@@ -940,11 +932,10 @@ s_vFillRTSHead(
 			} else {
 				memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
 			}
-			if (pDevice->eOPMode == OP_MODE_AP) {
+			if (pDevice->eOPMode == OP_MODE_AP)
 				memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-			} else {
+			else
 				memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
-			}
 		}
 	} else if (byPktType == PK_TYPE_11B) {
 		PSRTS_ab pBuf = (PSRTS_ab)pvRTS;
@@ -966,11 +957,10 @@ s_vFillRTSHead(
 			memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
 		}
 
-		if (pDevice->eOPMode == OP_MODE_AP) {
+		if (pDevice->eOPMode == OP_MODE_AP)
 			memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-		} else {
+		else
 			memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
-		}
 	}
 }
 
@@ -991,9 +981,8 @@ s_vFillCTSHead(
 	unsigned int uCTSFrameLen = 14;
 	unsigned short wLen = 0x0000;
 
-	if (pvCTS == NULL) {
+	if (pvCTS == NULL)
 		return;
-	}
 
 	if (bDisCRC) {
 		// When CRCDIS bit is on, H/W forgot to generate FCS for CTS frame,
@@ -1093,22 +1082,18 @@ s_vGenerateTxParameter(
 	unsigned short wFifoCtl;
 	bool bDisCRC = false;
 	unsigned char byFBOption = AUTO_FB_NONE;
-//    unsigned short wCurrentRate = pDevice->wCurrentRate;
 
-	//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "s_vGenerateTxParameter...\n");
 	PSTxBufHead pFifoHead = (PSTxBufHead)pTxBufHead;
 	pFifoHead->wReserved = wCurrentRate;
 	wFifoCtl = pFifoHead->wFIFOCtl;
 
-	if (wFifoCtl & FIFOCTL_CRCDIS) {
+	if (wFifoCtl & FIFOCTL_CRCDIS)
 		bDisCRC = true;
-	}
 
-	if (wFifoCtl & FIFOCTL_AUTO_FB_0) {
+	if (wFifoCtl & FIFOCTL_AUTO_FB_0)
 		byFBOption = AUTO_FB_0;
-	} else if (wFifoCtl & FIFOCTL_AUTO_FB_1) {
+	else if (wFifoCtl & FIFOCTL_AUTO_FB_1)
 		byFBOption = AUTO_FB_1;
-	}
 
 	if (pDevice->bLongHeader)
 		cbMACHdLen = WLAN_HDR_ADDR3_LEN + 6;
@@ -1174,13 +1159,8 @@ s_vGenerateTxParameter(
 			}
 		}
 	}
-	//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "s_vGenerateTxParameter END.\n");
 }
-/*
-  unsigned char *pbyBuffer,//point to pTxBufHead
-  unsigned short wFragType,//00:Non-Frag, 01:Start, 02:Mid, 03:Last
-  unsigned int cbFragmentSize,//Hdr+payoad+FCS
-*/
+
 static
 void
 s_vFillFragParameter(
@@ -1193,10 +1173,8 @@ s_vFillFragParameter(
 )
 {
 	PSTxBufHead pTxBufHead = (PSTxBufHead) pbyBuffer;
-	//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "s_vFillFragParameter...\n");
 
 	if (uTxType == TYPE_SYNCDMA) {
-		//PSTxSyncDesc ptdCurr = (PSTxSyncDesc)s_pvGetTxDescHead(pDevice, uTxType, uCurIdx);
 		PSTxSyncDesc ptdCurr = (PSTxSyncDesc)pvtdCurr;
 
 		//Set FIFOCtl & TimeStamp in TxSyncDesc
@@ -1204,26 +1182,21 @@ s_vFillFragParameter(
 		ptdCurr->m_wTimeStamp = pTxBufHead->wTimeStamp;
 		//Set TSR1 & ReqCount in TxDescHead
 		ptdCurr->m_td1TD1.wReqCount = cpu_to_le16((unsigned short)(cbReqCount));
-		if (wFragType == FRAGCTL_ENDFRAG) { //Last Fragmentation
+		if (wFragType == FRAGCTL_ENDFRAG) //Last Fragmentation
 			ptdCurr->m_td1TD1.byTCR |= (TCR_STP | TCR_EDP | EDMSDU);
-		} else {
+		else
 			ptdCurr->m_td1TD1.byTCR |= (TCR_STP | TCR_EDP);
-		}
 	} else {
-		//PSTxDesc ptdCurr = (PSTxDesc)s_pvGetTxDescHead(pDevice, uTxType, uCurIdx);
 		PSTxDesc ptdCurr = (PSTxDesc)pvtdCurr;
 		//Set TSR1 & ReqCount in TxDescHead
 		ptdCurr->m_td1TD1.wReqCount = cpu_to_le16((unsigned short)(cbReqCount));
-		if (wFragType == FRAGCTL_ENDFRAG) { //Last Fragmentation
+		if (wFragType == FRAGCTL_ENDFRAG) //Last Fragmentation
 			ptdCurr->m_td1TD1.byTCR |= (TCR_STP | TCR_EDP | EDMSDU);
-		} else {
+		else
 			ptdCurr->m_td1TD1.byTCR |= (TCR_STP | TCR_EDP);
-		}
 	}
 
 	pTxBufHead->wFragCtl |= (unsigned short)wFragType;//0x0001; //0000 0000 0000 0001
-
-	//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "s_vFillFragParameter END\n");
 }
 
 static unsigned int
@@ -1245,8 +1218,6 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 	unsigned short wFragType; //00:Non-Frag, 01:Start, 10:Mid, 11:Last
 	unsigned int uDuration;
 	unsigned char *pbyBuffer;
-//    unsigned int uKeyEntryIdx = NUM_KEY_ENTRY+1;
-//    unsigned char byKeySel = 0xFF;
 	unsigned int cbIVlen = 0;
 	unsigned int cbICVlen = 0;
 	unsigned int cbMIClen = 0;
@@ -1254,8 +1225,6 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 	unsigned int cb802_1_H_len = 0;
 	unsigned int uLength = 0;
 	unsigned int uTmpLen = 0;
-//    unsigned char abyTmp[8];
-//    unsigned long dwCRC;
 	unsigned int cbMICHDR = 0;
 	u32 dwMICKey0, dwMICKey1;
 	u32 dwMIC_Priority;
@@ -1274,7 +1243,6 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 	unsigned char *pbyType;
 	PSTxDesc       ptdCurr;
 	PSTxBufHead    psTxBufHd = (PSTxBufHead) pbyTxBufferAddr;
-//    unsigned int tmpDescIdx;
 	unsigned int cbHeaderLength = 0;
 	void *pvRrvTime;
 	PSMICHDRHead   pMICHDR;
@@ -1289,7 +1257,6 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 
 	pvRrvTime = pMICHDR = pvRTS = pvCTS = pvTxDataHd = NULL;
 
-	//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "s_cbFillTxBufHead...\n");
 	if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
 	    (pDevice->eOPMode == OP_MODE_AP)) {
 		if (is_multicast_ether_addr(&(psEthHeader->abyDstAddr[0])))
@@ -1312,9 +1279,8 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 		if (pTransmitKey->byCipherSuite == KEY_CTL_WEP) {
 			cbIVlen = 4;
 			cbICVlen = 4;
-			if (pTransmitKey->uKeyLength == WLAN_WEP232_KEYLEN) {
+			if (pTransmitKey->uKeyLength == WLAN_WEP232_KEYLEN)
 				bIsWEP256 = true;
-			}
 		}
 		if (pTransmitKey->byCipherSuite == KEY_CTL_TKIP) {
 			cbIVlen = 8;//IV+ExtIV
@@ -1347,11 +1313,10 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 	//
 	// Use for AUTO FALL BACK
 	//
-	if (psTxBufHd->wFIFOCtl & FIFOCTL_AUTO_FB_0) {
+	if (psTxBufHd->wFIFOCtl & FIFOCTL_AUTO_FB_0)
 		byFBOption = AUTO_FB_0;
-	} else if (psTxBufHd->wFIFOCtl & FIFOCTL_AUTO_FB_1) {
+	else if (psTxBufHd->wFIFOCtl & FIFOCTL_AUTO_FB_1)
 		byFBOption = AUTO_FB_1;
-	}
 
 	//////////////////////////////////////////////////////
 	//Set RrvTime/RTS/CTS Buffer
@@ -1465,11 +1430,11 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 		//FragNum = (FrameSize-(Hdr+FCS))/(Fragment Size -(Hrd+FCS)))
 		uMACfragNum = (unsigned short) ((cbFrameBodySize + cbMIClen) / cbFragPayloadSize);
 		cbLastFragPayloadSize = (cbFrameBodySize + cbMIClen) % cbFragPayloadSize;
-		if (cbLastFragPayloadSize == 0) {
+		if (cbLastFragPayloadSize == 0)
 			cbLastFragPayloadSize = cbFragPayloadSize;
-		} else {
+		else
 			uMACfragNum++;
-		}
+
 		//[Hdr+(IV)+last fragment payload+(MIC)+(ICV)+FCS]
 		cbLastFragmentSize = cbMACHdLen + cbLastFragPayloadSize + cbIVlen + cbICVlen + cbFCSlen;
 
@@ -1567,7 +1532,6 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 				//    Last Fragmentation
 				//=========================
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Last Fragmentation...\n");
-				//tmpDescIdx = (uDescIdx + uFragIdx) % pDevice->cbTD[uDMAIdx];
 
 				wFragType = FRAGCTL_ENDFRAG;
 
@@ -1600,7 +1564,6 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 				//---------------------------
 
 				pbyBuffer = (unsigned char *)pHeadTD->pTDInfo->buf;
-				//pbyBuffer = (unsigned char *)pDevice->aamTxBuf[uDMAIdx][tmpDescIdx].pbyVAddr;
 
 				uLength = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen;
 
@@ -1644,12 +1607,6 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 								*(unsigned char *)((unsigned char *)&dwSafeMIC_R + uMICFragLen - 4),
 								(cbMIClen - uMICFragLen));
 						}
-						/*
-						  for (ii = 0; ii < cbLastFragPayloadSize + 8 + 24; ii++) {
-						  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", *((unsigned char *)((pbyBuffer + uLength) + ii - 8 - 24)));
-						  }
-						  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n\n");
-						*/
 					}
 					MIC_vUnInit();
 				} else {
@@ -1690,7 +1647,6 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 				//    Middle Fragmentation
 				//=========================
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Middle Fragmentation...\n");
-				//tmpDescIdx = (uDescIdx + uFragIdx) % pDevice->cbTD[uDMAIdx];
 
 				wFragType = FRAGCTL_MIDFRAG;
 
@@ -1753,21 +1709,9 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 						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);
-						/*
-						  for (ii = 0; ii < uMICFragLen; ii++) {
-						  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", *((unsigned char *)((pbyBuffer + uLength + uTmpLen) + ii)));
-						  }
-						  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
-						*/
 						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Get MIC:%X, %X\n", *pdwMIC_L, *pdwMIC_R);
 					}
 					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Middle frag len: %d\n", uTmpLen);
-					/*
-					  for (ii = 0; ii < uTmpLen; ii++) {
-					  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", *((unsigned char *)((pbyBuffer + uLength) + ii)));
-					  }
-					  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n\n");
-					*/
 
 				} else {
 					ASSERT(uTmpLen == (cbFragPayloadSize));
@@ -1804,8 +1748,6 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 		//=========================
 		//    No Fragmentation
 		//=========================
-		//DBG_PRTGRP03(("No Fragmentation...\n"));
-		//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "No Fragmentation...\n");
 		wFragType = FRAGCTL_NONFRAG;
 
 		//Set FragCtl in TxBufferHead
@@ -1864,12 +1806,6 @@ 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);
-			/*
-			  for (ii = 0; ii < (cbFrameBodySize - cb802_1_H_len); ii++) {
-			  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", *((unsigned char *)((pbyBuffer + uLength) + ii)));
-			  }
-			  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
-			*/
 
 			MIC_vAppend((pbyBuffer + uLength - cb802_1_H_len), cbFrameBodySize);
 
@@ -1888,12 +1824,6 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 			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);
-/*
-  for (ii = 0; ii < 8; ii++) {
-  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", *(((unsigned char *)(pdwMIC_L) + ii)));
-  }
-  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
-*/
 
 		}
 
@@ -1917,12 +1847,9 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 
 		pDevice->iTDUsed[uDMAIdx]++;
 
-//   DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " ptdCurr->m_dwReserved0[%d] ptdCurr->m_dwReserved1[%d].\n", ptdCurr->pTDInfo->dwReqCount, ptdCurr->pTDInfo->dwHeaderLength);
-//   DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " cbHeaderLength[%d]\n", cbHeaderLength);
-
 	}
 	*puMACfragNum = uMACfragNum;
-	//DBG_PRTGRP03(("s_cbFillTxBufHead END\n"));
+
 	return cbHeaderLength;
 }
 
@@ -1973,40 +1900,37 @@ vGenerateFIFOHeader(PSDevice pDevice, unsigned char byPktType, unsigned char *pb
 	pTxBufHead->wFIFOCtl |= FIFOCTL_GENINT;
 
 	//Set FIFOCTL_ISDMA0
-	if (TYPE_TXDMA0 == uDMAIdx) {
+	if (TYPE_TXDMA0 == uDMAIdx)
 		pTxBufHead->wFIFOCtl |= FIFOCTL_ISDMA0;
-	}
 
 	//Set FRAGCTL_MACHDCNT
-	if (pDevice->bLongHeader) {
+	if (pDevice->bLongHeader)
 		cbMacHdLen = WLAN_HDR_ADDR3_LEN + 6;
-	} else {
+	else
 		cbMacHdLen = WLAN_HDR_ADDR3_LEN;
-	}
+
 	pTxBufHead->wFragCtl |= cpu_to_le16((unsigned short)(cbMacHdLen << 10));
 
 	//Set packet type
-	if (byPktType == PK_TYPE_11A) {//0000 0000 0000 0000
+	if (byPktType == PK_TYPE_11A) //0000 0000 0000 0000
 		;
-	} else if (byPktType == PK_TYPE_11B) {//0000 0001 0000 0000
+	else if (byPktType == PK_TYPE_11B) //0000 0001 0000 0000
 		pTxBufHead->wFIFOCtl |= FIFOCTL_11B;
-	} else if (byPktType == PK_TYPE_11GB) {//0000 0010 0000 0000
+	else if (byPktType == PK_TYPE_11GB) //0000 0010 0000 0000
 		pTxBufHead->wFIFOCtl |= FIFOCTL_11GB;
-	} else if (byPktType == PK_TYPE_11GA) {//0000 0011 0000 0000
+	else if (byPktType == PK_TYPE_11GA) //0000 0011 0000 0000
 		pTxBufHead->wFIFOCtl |= FIFOCTL_11GA;
-	}
+
 	//Set FIFOCTL_GrpAckPolicy
-	if (pDevice->bGrpAckPolicy == true) {//0000 0100 0000 0000
+	if (pDevice->bGrpAckPolicy == true) //0000 0100 0000 0000
 		pTxBufHead->wFIFOCtl |=	FIFOCTL_GRPACK;
-	}
 
 	//Set Auto Fallback Ctl
 	if (pDevice->wCurrentRate >= RATE_18M) {
-		if (pDevice->byAutoFBCtrl == AUTO_FB_0) {
+		if (pDevice->byAutoFBCtrl == AUTO_FB_0)
 			pTxBufHead->wFIFOCtl |= FIFOCTL_AUTO_FB_0;
-		} else if (pDevice->byAutoFBCtrl == AUTO_FB_1) {
+		else if (pDevice->byAutoFBCtrl == AUTO_FB_1)
 			pTxBufHead->wFIFOCtl |= FIFOCTL_AUTO_FB_1;
-		}
 	}
 
 	//Set FRAGCTL_WEPTYP
@@ -2031,10 +1955,6 @@ vGenerateFIFOHeader(PSDevice pDevice, unsigned char byPktType, unsigned char *pb
 #endif
 	pTxBufHead->byTxPower = pDevice->byCurPwr;
 
-/*
-  if (pDevice->bEnableHostWEP)
-  pTxBufHead->wFragCtl &=  ~(FRAGCTL_TKIP | FRAGCTL_LEGACY |FRAGCTL_AES);
-*/
 	*pcbHeaderSize = s_cbFillTxBufHead(pDevice, byPktType, pbyTxBufferAddr, cbPayloadSize,
 					   uDMAIdx, pHeadTD, psEthHeader, pPacket, bNeedEncrypt,
 					   pTransmitKey, uNodeIndex, puMACfragNum);
@@ -2075,13 +1995,12 @@ vGenerateMACHeader(
 {
 	PS802_11Header  pMACHeader = (PS802_11Header)pbyBufferAddr;
 
-	memset(pMACHeader, 0, (sizeof(S802_11Header)));  //- sizeof(pMACHeader->dwIV)));
+	memset(pMACHeader, 0, (sizeof(S802_11Header)));
 
-	if (uDMAIdx == TYPE_ATIMDMA) {
+	if (uDMAIdx == TYPE_ATIMDMA)
 		pMACHeader->wFrameCtl = TYPE_802_11_ATIM;
-	} else {
+	else
 		pMACHeader->wFrameCtl = TYPE_802_11_DATA;
-	}
 
 	if (pDevice->eOPMode == OP_MODE_AP) {
 		memcpy(&(pMACHeader->abyAddr1[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
@@ -2122,9 +2041,8 @@ vGenerateMACHeader(
 			pDevice->wSeqCounter = 0;
 	}
 
-	if ((wFragType == FRAGCTL_STAFRAG) || (wFragType == FRAGCTL_MIDFRAG)) { //StartFrag or MidFrag
+	if ((wFragType == FRAGCTL_STAFRAG) || (wFragType == FRAGCTL_MIDFRAG)) //StartFrag or MidFrag
 		pMACHeader->wFrameCtl |= FC_MOREFRAG;
-	}
 }
 
 CMD_STATUS csMgmt_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket) {
@@ -2156,9 +2074,8 @@ CMD_STATUS csMgmt_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket) {
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	unsigned short wCurrentRate = RATE_1M;
 
-	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 0) {
+	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 0)
 		return CMD_STATUS_RESOURCES;
-	}
 
 	pFrstTD = pDevice->apCurrTD[TYPE_TXDMA0];
 	pbyTxBufferAddr = (unsigned char *)pFrstTD->pTDInfo->buf;
@@ -2179,11 +2096,11 @@ CMD_STATUS csMgmt_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket) {
 	// 2004.11.11 Kyle -- Using OFDM power to tx MngPkt will decrease the connection capability.
 	//                    And cmd timer will wait data pkt TX finish before scanning so it's OK
 	//                    to set power here.
-	if (pDevice->pMgmt->eScanState != WMAC_NO_SCANNING) {
+	if (pDevice->pMgmt->eScanState != WMAC_NO_SCANNING)
 		RFbSetPower(pDevice, wCurrentRate, pDevice->byCurrentCh);
-	} else {
+	else
 		RFbSetPower(pDevice, wCurrentRate, pMgmt->uCurrChannel);
-	}
+
 	pTxBufHead->byTxPower = pDevice->byCurPwr;
 	//+++++++++++++++++++++ Patch VT3253 A1 performance +++++++++++++++++++++++++++
 	if (pDevice->byFOETuning) {
@@ -2247,8 +2164,6 @@ CMD_STATUS csMgmt_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket) {
 			cbICVlen = 4;
 			pTxBufHead->wFragCtl |= FRAGCTL_TKIP;
 			//We need to get seed here for filling TxKey entry.
-			//TKIPvMixKey(pTransmitKey->abyKey, pDevice->abyCurrentNetAddr,
-			//            pTransmitKey->wTSC15_0, pTransmitKey->dwTSC47_16, pDevice->abyPRNG);
 		} else if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
 			cbIVlen = 8;//RSN Header
 			cbICVlen = 8;//MIC
@@ -2263,9 +2178,9 @@ CMD_STATUS csMgmt_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket) {
 	cbFrameSize = cbMacHdLen + cbFrameBodySize + cbIVlen + cbMIClen + cbICVlen + cbFCSlen;
 
 	//Set FIFOCTL_GrpAckPolicy
-	if (pDevice->bGrpAckPolicy == true) {//0000 0100 0000 0000
+	if (pDevice->bGrpAckPolicy == true) //0000 0100 0000 0000
 		pTxBufHead->wFIFOCtl |=	FIFOCTL_GRPACK;
-	}
+
 	//the rest of pTxBufHead->wFragCtl:FragTyp will be set later in s_vFillFragParameter()
 
 	//Set RrvTime/RTS/CTS Buffer
@@ -2401,9 +2316,8 @@ CMD_STATUS csMgmt_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket) {
 
 	pDevice->iTDUsed[TYPE_TXDMA0]++;
 
-	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 1) {
+	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 1)
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " available td0 <= 1\n");
-	}
 
 	pDevice->apCurrTD[TYPE_TXDMA0] = pFrstTD->next;
 
@@ -2559,11 +2473,10 @@ cbGetFragCount(
 		cbFragPayloadSize = cbFragmentSize - cbMACHdLen - cbIVlen - cbICVlen - cbFCSlen;
 		uMACfragNum = (unsigned short) ((cbFrameBodySize + cbMIClen) / cbFragPayloadSize);
 		cbLastFragPayloadSize = (cbFrameBodySize + cbMIClen) % cbFragPayloadSize;
-		if (cbLastFragPayloadSize == 0) {
+		if (cbLastFragPayloadSize == 0)
 			cbLastFragPayloadSize = cbFragPayloadSize;
-		} else {
+		else
 			uMACfragNum++;
-		}
 	}
 	return uMACfragNum;
 }
@@ -2613,15 +2526,14 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, un
 	unsigned char *pbyMacHdr;
 
 	unsigned int cbExtSuppRate = 0;
-//    PWLAN_IE        pItem;
 
 	pvRrvTime = pMICHDR = pvRTS = pvCTS = pvTxDataHd = NULL;
 
-	if (cbMPDULen <= WLAN_HDR_ADDR3_LEN) {
+	if (cbMPDULen <= WLAN_HDR_ADDR3_LEN)
 		cbFrameBodySize = 0;
-	} else {
+	else
 		cbFrameBodySize = cbMPDULen - WLAN_HDR_ADDR3_LEN;
-	}
+
 	p80211Header = (PUWLAN_80211HDR)pbMPDU;
 
 	pFrstTD = pDevice->apCurrTD[TYPE_TXDMA0];
@@ -2642,11 +2554,11 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, un
 	// 2004.11.11 Kyle -- Using OFDM power to tx MngPkt will decrease the connection capability.
 	//                    And cmd timer will wait data pkt TX to finish before scanning so it's OK
 	//                    to set power here.
-	if (pDevice->pMgmt->eScanState != WMAC_NO_SCANNING) {
+	if (pDevice->pMgmt->eScanState != WMAC_NO_SCANNING)
 		RFbSetPower(pDevice, wCurrentRate, pDevice->byCurrentCh);
-	} else {
+	else
 		RFbSetPower(pDevice, wCurrentRate, pMgmt->uCurrChannel);
-	}
+
 	pTxBufHead->byTxPower = pDevice->byCurPwr;
 
 	//+++++++++++++++++++++ Patch VT3253 A1 performance +++++++++++++++++++++++++++
@@ -2704,17 +2616,14 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, un
 
 	// hostapd deamon ext support rate patch
 	if (WLAN_GET_FC_FSTYPE(p80211Header->sA4.wFrameCtl) == WLAN_FSTYPE_ASSOCRESP) {
-		if (((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len != 0) {
+		if (((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len != 0)
 			cbExtSuppRate += ((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len + WLAN_IEHDR_LEN;
-		}
 
-		if (((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates)->len != 0) {
+		if (((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates)->len != 0)
 			cbExtSuppRate += ((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates)->len + WLAN_IEHDR_LEN;
-		}
 
-		if (cbExtSuppRate > 0) {
+		if (cbExtSuppRate > 0)
 			cbFrameBodySize = WLAN_ASSOCRESP_OFF_SUPP_RATES;
-		}
 	}
 
 	//Set FRAGCTL_MACHDCNT
@@ -2736,8 +2645,6 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, un
 			cbICVlen = 4;
 			pTxBufHead->wFragCtl |= FRAGCTL_TKIP;
 			//We need to get seed here for filling TxKey entry.
-			//TKIPvMixKey(pTransmitKey->abyKey, pDevice->abyCurrentNetAddr,
-			//            pTransmitKey->wTSC15_0, pTransmitKey->dwTSC47_16, pDevice->abyPRNG);
 		} else if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
 			cbIVlen = 8;//RSN Header
 			cbICVlen = 8;//MIC
@@ -2753,9 +2660,9 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, un
 	cbFrameSize = cbMacHdLen + cbFrameBodySize + cbIVlen + cbMIClen + cbICVlen + cbFCSlen + cbExtSuppRate;
 
 	//Set FIFOCTL_GrpAckPolicy
-	if (pDevice->bGrpAckPolicy == true) {//0000 0100 0000 0000
+	if (pDevice->bGrpAckPolicy == true) //0000 0100 0000 0000
 		pTxBufHead->wFIFOCtl |=	FIFOCTL_GRPACK;
-	}
+
 	//the rest of pTxBufHead->wFragCtl:FragTyp will be set later in s_vFillFragParameter()
 
 	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {//802.11g packet
@@ -2881,9 +2788,8 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, un
 			pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0 = pTransmitKey->wTSC15_0;
 		}
 
-		if ((pDevice->byLocalID <= REV_ID_VT3253_A1)) {
+		if ((pDevice->byLocalID <= REV_ID_VT3253_A1))
 			s_vSWencryption(pDevice, pTransmitKey, pbyPayloadHead, (unsigned short)(cbFrameBodySize + cbMIClen));
-		}
 	}
 
 	pMACHeader->wSeqCtl = cpu_to_le16(pDevice->wSeqCounter << 4);
@@ -2927,9 +2833,8 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, un
 
 	pDevice->iTDUsed[TYPE_TXDMA0]++;
 
-	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 1) {
+	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 1)
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " 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 4f2cf34dc39..601bedb211d 100644
--- a/drivers/staging/vt6655/rxtx.h
+++ b/drivers/staging/vt6655/rxtx.h
@@ -39,17 +39,6 @@
 
 /*---------------------  Export Functions  --------------------------*/
 
-/*
-  void
-  vGenerateMACHeader(PSDevice pDevice, unsigned long dwTxBufferAddr, unsigned char *pbySkbData,
-  unsigned int cbPacketSize, bool bDMA0Used, unsigned int *pcbHeadSize,
-  unsigned int *pcbAppendPayload);
-
-  void
-  vProcessRxMACHeader(PSDevice pDevice, unsigned long dwRxBufferAddr, unsigned int cbPacketSize,
-  bool bIsWEP, unsigned int *pcbHeadSize);
-*/
-
 void
 vGenerateMACHeader(
 	PSDevice         pDevice,
diff --git a/drivers/staging/vt6655/tether.c b/drivers/staging/vt6655/tether.c
index a5dc3c09cfd..1e7d3e2115a 100644
--- a/drivers/staging/vt6655/tether.c
+++ b/drivers/staging/vt6655/tether.c
@@ -98,8 +98,8 @@ bool ETHbIsBufferCrc32Ok(unsigned char *pbyBuffer, unsigned int cbFrameLength)
 	unsigned long dwCRC;
 
 	dwCRC = CRCdwGetCrc32(pbyBuffer, cbFrameLength - 4);
-	if (cpu_to_le32(*((unsigned long *)(pbyBuffer + cbFrameLength - 4))) != dwCRC) {
+	if (cpu_to_le32(*((unsigned long *)(pbyBuffer + cbFrameLength - 4))) != dwCRC)
 		return false;
-	}
+
 	return true;
 }
diff --git a/drivers/staging/vt6655/tkip.c b/drivers/staging/vt6655/tkip.c
index e7c17c6f7ea..f758d021c60 100644
--- a/drivers/staging/vt6655/tkip.c
+++ b/drivers/staging/vt6655/tkip.c
@@ -157,11 +157,11 @@ unsigned int rotr1(unsigned int a)
 {
 	unsigned int b;
 
-	if ((a & 0x01) == 0x01) {
+	if ((a & 0x01) == 0x01)
 		b = (a >> 1) | 0x8000;
-	} else {
+	else
 		b = (a >> 1) & 0x7fff;
-	}
+
 	b = b % 65536;
 	return b;
 }
@@ -189,7 +189,6 @@ void TKIPvMixKey(
 )
 {
 	unsigned int p1k[5];
-//    unsigned int ttak0, ttak1, ttak2, ttak3, ttak4;
 	unsigned int tsc0, tsc1, tsc2;
 	unsigned int ppk0, ppk1, ppk2, ppk3, ppk4, ppk5;
 	unsigned long int pnl, pnh;
diff --git a/drivers/staging/vt6655/vntwifi.c b/drivers/staging/vt6655/vntwifi.c
index e78aedf9907..7d61598563c 100644
--- a/drivers/staging/vt6655/vntwifi.c
+++ b/drivers/staging/vt6655/vntwifi.c
@@ -39,8 +39,6 @@
 #include "datarate.h"
 
 /*---------------------  Static Definitions -------------------------*/
-//static int          msglevel                =MSG_LEVEL_DEBUG;
-//static int          msglevel                =MSG_LEVEL_INFO;
 
 /*---------------------  Static Classes  ----------------------------*/
 
@@ -153,9 +151,9 @@ VNTWIFIpGetCurrentChannel(
 )
 {
 	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
-	if (pMgmtHandle != NULL) {
+	if (pMgmtHandle != NULL)
 		return pMgmt->uCurrChannel;
-	}
+
 	return 0;
 }
 
@@ -210,17 +208,17 @@ VNTWIFIbyGetMaxSupportRate(
 	if (pSupportRateIEs) {
 		for (ii = 0; ii < pSupportRateIEs->len; ii++) {
 			bySupportRate = DATARATEbyGetRateIdx(pSupportRateIEs->abyRates[ii]);
-			if (bySupportRate > byMaxSupportRate) {
+			if (bySupportRate > byMaxSupportRate)
 				byMaxSupportRate = bySupportRate;
-			}
+
 		}
 	}
 	if (pExtSupportRateIEs) {
 		for (ii = 0; ii < pExtSupportRateIEs->len; ii++) {
 			bySupportRate = DATARATEbyGetRateIdx(pExtSupportRateIEs->abyRates[ii]);
-			if (bySupportRate > byMaxSupportRate) {
+			if (bySupportRate > byMaxSupportRate)
 				byMaxSupportRate = bySupportRate;
-			}
+
 		}
 	}
 
@@ -361,11 +359,10 @@ VNTWIFIbConfigPhyMode(
 
 	if ((ePhyType != PHY_TYPE_AUTO) &&
 	    (ePhyType != pMgmt->eCurrentPHYMode)) {
-		if (CARDbSetPhyParameter(pMgmt->pAdapter, ePhyType, 0, 0, NULL, NULL) == true) {
+		if (CARDbSetPhyParameter(pMgmt->pAdapter, ePhyType, 0, 0, NULL, NULL) == true)
 			pMgmt->eCurrentPHYMode = ePhyType;
-		} else {
+		else
 			return false;
-		}
 	}
 	pMgmt->eConfigPHYMode = ePhyType;
 	return true;
@@ -379,9 +376,8 @@ VNTWIFIbGetConfigPhyMode(
 {
 	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
 
-	if ((pMgmt != NULL) && (pePhyType != NULL)) {
+	if ((pMgmt != NULL) && (pePhyType != NULL))
 		*(PCARD_PHY_TYPE)pePhyType = pMgmt->eConfigPHYMode;
-	}
 }
 
 /*+
@@ -427,12 +423,12 @@ VNTWIFIvQueryBSSList(void *pMgmtHandle, unsigned int *puBSSCount, void **pvFirst
 
 	for (ii = 0; ii < MAX_BSS_NUM; ii++) {
 		pBSS = &(pMgmt->sBSSList[ii]);
-		if (!pBSS->bActive) {
+		if (!pBSS->bActive)
 			continue;
-		}
-		if (*pvFirstBSS == NULL) {
+
+		if (*pvFirstBSS == NULL)
 			*pvFirstBSS = &(pMgmt->sBSSList[ii]);
-		}
+
 		uCount++;
 	}
 	*puBSSCount = uCount;
@@ -452,9 +448,9 @@ VNTWIFIvGetNextBSS(
 
 	while (*pvNextBSS == NULL) {
 		pBSS++;
-		if (pBSS > &(pMgmt->sBSSList[MAX_BSS_NUM])) {
+		if (pBSS > &(pMgmt->sBSSList[MAX_BSS_NUM]))
 			return;
-		}
+
 		if (pBSS->bActive == true) {
 			*pvNextBSS = pBSS;
 			return;
@@ -489,10 +485,10 @@ VNTWIFIvUpdateNodeTxCounter(
 
 	if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) ||
 	    (pMgmt->eCurrMode == WMAC_MODE_ESS_AP)) {
-		if (BSSDBbIsSTAInNodeDB(pMgmt, pbyDestAddress, &uNodeIndex) == false) {
+		if (BSSDBbIsSTAInNodeDB(pMgmt, pbyDestAddress, &uNodeIndex) == false)
 			return;
-		}
 	}
+
 	pMgmt->sNodeDBTable[uNodeIndex].uTxAttempts++;
 	if (bTxOk) {
 		// transmit success, TxAttempts at least plus one
@@ -502,9 +498,9 @@ VNTWIFIvUpdateNodeTxCounter(
 		pMgmt->sNodeDBTable[uNodeIndex].uTxFailures++;
 	}
 	pMgmt->sNodeDBTable[uNodeIndex].uTxRetry += pbyTxFailCount[MAX_RATE];
-	for (ii = 0; ii < MAX_RATE; ii++) {
+	for (ii = 0; ii < MAX_RATE; ii++)
 		pMgmt->sNodeDBTable[uNodeIndex].uTxFail[ii] += pbyTxFailCount[ii];
-	}
+
 	return;
 }
 
@@ -535,11 +531,11 @@ VNTWIFIvGetTxRate(
 			pSupportRateIEs = (PWLAN_IE_SUPP_RATES) (pMgmt->sNodeDBTable[uNodeIndex].abyCurrSuppRates);
 			pExtSupportRateIEs = (PWLAN_IE_SUPP_RATES) (pMgmt->sNodeDBTable[uNodeIndex].abyCurrExtSuppRates);
 		} else {
-			if (pMgmt->eCurrentPHYMode != PHY_TYPE_11A) {
+			if (pMgmt->eCurrentPHYMode != PHY_TYPE_11A)
 				wTxDataRate = RATE_2M;
-			} else {
+			else
 				wTxDataRate = RATE_24M;
-			}
+
 			pSupportRateIEs = (PWLAN_IE_SUPP_RATES) pMgmt->abyCurrSuppRates;
 			pExtSupportRateIEs = (PWLAN_IE_SUPP_RATES) pMgmt->abyCurrExtSuppRates;
 		}
@@ -558,9 +554,9 @@ VNTWIFIvGetTxRate(
 					    pSupportRateIEs,
 					    pExtSupportRateIEs
 );
-	if (byACKRate > (unsigned char) wTxDataRate) {
+	if (byACKRate > (unsigned char) wTxDataRate)
 		byACKRate = (unsigned char) wTxDataRate;
-	}
+
 	byCCKBasicRate = VNTWIFIbyGetACKTxRate(RATE_11M,
 						pSupportRateIEs,
 						pExtSupportRateIEs
@@ -584,53 +580,12 @@ VNTWIFIbyGetKeyCypher(
 {
 	PSMgmtObject    pMgmt = (PSMgmtObject)pMgmtHandle;
 
-	if (bGroupKey) {
+	if (bGroupKey)
 		return pMgmt->byCSSGK;
-	} else {
+	else
 		return pMgmt->byCSSPK;
-	}
 }
 
-/*
-  bool
-  VNTWIFIbInit(
-  void *pAdapterHandler,
-  void **pMgmtHandler
-) {
-  PSMgmtObject        pMgmt = NULL;
-  unsigned int ii;
-
-  pMgmt = (PSMgmtObject)kmalloc(sizeof(SMgmtObject), (int)GFP_ATOMIC);
-  if (pMgmt == NULL) {
-  *pMgmtHandler = NULL;
-  return false;
-  }
-
-  memset(pMgmt, 0, sizeof(SMgmtObject));
-  pMgmt->pAdapter = (void *) pAdapterHandler;
-
-  // should initial MAC address abyMACAddr
-  for (ii=0; ii<WLAN_BSSID_LEN; ii++) {
-  pMgmt->abyDesireBSSID[ii] = 0xFF;
-  }
-  pMgmt->pbyPSPacketPool = &pMgmt->byPSPacketPool[0];
-  pMgmt->pbyMgmtPacketPool = &pMgmt->byMgmtPacketPool[0];
-  pMgmt->byCSSPK = KEY_CTL_NONE;
-  pMgmt->byCSSGK = KEY_CTL_NONE;
-  pMgmt->wIBSSBeaconPeriod = DEFAULT_IBSS_BI;
-
-  pMgmt->cbFreeCmdQueue = CMD_Q_SIZE;
-  pMgmt->uCmdDequeueIdx = 0;
-  pMgmt->uCmdEnqueueIdx = 0;
-  pMgmt->eCommandState = WLAN_CMD_STATE_IDLE;
-  pMgmt->bCmdStop = false;
-  pMgmt->bCmdRunning = false;
-
-  *pMgmtHandler = pMgmt;
-  return true;
-  }
-*/
-
 bool
 VNTWIFIbSetPMKIDCache(
 	void *pMgmtObject,
@@ -640,9 +595,9 @@ VNTWIFIbSetPMKIDCache(
 {
 	PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
 
-	if (ulCount > MAX_PMKID_CACHE) {
+	if (ulCount > MAX_PMKID_CACHE)
 		return false;
-	}
+
 	pMgmt->gsPMKIDCache.BSSIDInfoCount = ulCount;
 	memcpy(pMgmt->gsPMKIDCache.BSSIDInfo, pPMKIDInfo, (ulCount*sizeof(PMKIDInfo)));
 	return true;
@@ -657,15 +612,14 @@ VNTWIFIwGetMaxSupportRate(
 	PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
 
 	for (wRate = RATE_54M; wRate > RATE_1M; wRate--) {
-		if (pMgmt->sNodeDBTable[0].wSuppRate & (1<<wRate)) {
+		if (pMgmt->sNodeDBTable[0].wSuppRate & (1<<wRate))
 			return wRate;
-		}
 	}
-	if (pMgmt->eCurrentPHYMode == PHY_TYPE_11A) {
+
+	if (pMgmt->eCurrentPHYMode == PHY_TYPE_11A)
 		return RATE_6M;
-	} else {
+	else
 		return RATE_1M;
-	}
 }
 
 void
@@ -693,7 +647,6 @@ VNTWIFIbMeasureReport(
 	PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
 	unsigned char *pbyCurrentEID = (unsigned char *)(pMgmt->pCurrMeasureEIDRep);
 
-	//spin_lock_irq(&pDevice->lock);
 	if ((pvMeasureEID != NULL) &&
 	    (pMgmt->uLengthOfRepEIDs < (WLAN_A3FR_MAXLEN - sizeof(MEASEURE_REP) - sizeof(WLAN_80211HDR_A3) - 3))
 ) {
@@ -734,7 +687,7 @@ VNTWIFIbMeasureReport(
 	if (bEndOfReport) {
 		IEEE11hbMSRRepTx(pMgmt);
 	}
-	//spin_unlock_irq(&pDevice->lock);
+
 	return true;
 }
 
@@ -746,9 +699,7 @@ VNTWIFIbChannelSwitch(
 {
 	PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
 
-	//spin_lock_irq(&pDevice->lock);
 	pMgmt->uCurrChannel = byNewChannel;
 	pMgmt->bSwitchChannel = false;
-	//spin_unlock_irq(&pDevice->lock);
 	return true;
 }
diff --git a/drivers/staging/vt6655/vntwifi.h b/drivers/staging/vt6655/vntwifi.h
index 4b01ebd7299..880b8ab109b 100644
--- a/drivers/staging/vt6655/vntwifi.h
+++ b/drivers/staging/vt6655/vntwifi.h
@@ -222,13 +222,6 @@ VNTWIFIvGetTxRate(
 	unsigned char *pbyCCKBasicRate,
 	unsigned char *pbyOFDMBasicRate
 );
-/*
-  bool
-  VNTWIFIbInit(
-  void *pAdapterHandler,
-  void **pMgmtHandler
-);
-*/
 
 unsigned char
 VNTWIFIbyGetKeyCypher(
@@ -276,12 +269,5 @@ VNTWIFIbChannelSwitch(
 	void *pMgmtObject,
 	unsigned char byNewChannel
 );
-/*
-  bool
-  VNTWIFIbRadarPresent(
-  void *pMgmtObject,
-  unsigned char byChannel
-);
-*/
 
 #endif //__VNTWIFI_H__
diff --git a/drivers/staging/vt6655/wcmd.c b/drivers/staging/vt6655/wcmd.c
index 72caaa203dd..a689645fa01 100644
--- a/drivers/staging/vt6655/wcmd.c
+++ b/drivers/staging/vt6655/wcmd.c
@@ -60,7 +60,6 @@
 
 /*---------------------  Static Variables  --------------------------*/
 static int msglevel = MSG_LEVEL_INFO;
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 /*---------------------  Static Functions  --------------------------*/
 
 static
@@ -128,14 +127,13 @@ vAdHocBeaconStop(PSDevice  pDevice)
 		    (pMgmt->uScanChannel > CB_MAX_CHANNEL_24G)) {
 			bStop = true;
 		}
-		if (pMgmt->uIBSSChannel >  CB_MAX_CHANNEL_24G) {
+		if (pMgmt->uIBSSChannel >  CB_MAX_CHANNEL_24G)
 			bStop = true;
-		}
+
 	}
 
-	if (bStop) {
+	if (bStop)
 		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
-	}
 } /* vAdHocBeaconStop */
 
 /*
@@ -197,13 +195,13 @@ s_vProbeChannel(
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	unsigned int ii;
 
-	if (pDevice->eCurrentPHYType == PHY_TYPE_11A) {
+	if (pDevice->eCurrentPHYType == PHY_TYPE_11A)
 		pbyRate = &abyCurrSuppRatesA[0];
-	} else if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
+	else if (pDevice->eCurrentPHYType == PHY_TYPE_11B)
 		pbyRate = &abyCurrSuppRatesB[0];
-	} else {
+	else
 		pbyRate = &abyCurrSuppRatesG[0];
-	}
+
 	// build an assocreq frame and send it
 	pTxPacket = s_MgrMakeProbeRequest
 		(
@@ -217,11 +215,10 @@ s_vProbeChannel(
 
 	if (pTxPacket != NULL) {
 		for (ii = 0; ii < 2; ii++) {
-			if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
+			if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING)
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Probe request sending fail.. \n");
-			} else {
+			else
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Probe request is sending.. \n");
-			}
 		}
 	}
 }
@@ -360,11 +357,11 @@ vCommandTimer(
 			// 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);
-			if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
+			if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)
 				CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_ADHOC);
-			} else {
+			else
 				CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_INFRASTRUCTURE);
-			}
+
 			vAdHocBeaconRestart(pDevice);
 			s_bCommandComplete(pDevice);
 
@@ -377,7 +374,6 @@ vCommandTimer(
 				return;
 			}
 			if (pMgmt->uScanChannel == pDevice->byMinChannel) {
-				//pMgmt->eScanType = WMAC_SCAN_ACTIVE;
 				pMgmt->abyScanBSSID[0] = 0xFF;
 				pMgmt->abyScanBSSID[1] = 0xFF;
 				pMgmt->abyScanBSSID[2] = 0xFF;
@@ -385,19 +381,17 @@ vCommandTimer(
 				pMgmt->abyScanBSSID[4] = 0xFF;
 				pMgmt->abyScanBSSID[5] = 0xFF;
 				pItemSSID->byElementID = WLAN_EID_SSID;
-				// clear bssid list
-				// BSSvClearBSSList((void *)pDevice, pDevice->bLinkPass);
 				pMgmt->eScanState = WMAC_IS_SCANNING;
 
 			}
 
 			vAdHocBeaconStop(pDevice);
 
-			if (set_channel(pMgmt->pAdapter, pMgmt->uScanChannel)) {
+			if (set_channel(pMgmt->pAdapter, pMgmt->uScanChannel))
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "SCAN Channel: %d\n", pMgmt->uScanChannel);
-			} else {
+			else
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "SET SCAN Channel Fail: %d\n", pMgmt->uScanChannel);
-			}
+
 			CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_UNKNOWN);
 			pMgmt->uScanChannel++;
 //2008-8-4 <modify> by chester
@@ -430,11 +424,10 @@ vCommandTimer(
 		// 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);
-		if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
+		if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)
 			CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_ADHOC);
-		} else {
+		else
 			CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_INFRASTRUCTURE);
-		}
 
 		pMgmt->eScanState = WMAC_NO_SCANNING;
 		vAdHocBeaconRestart(pDevice);
@@ -468,7 +461,6 @@ vCommandTimer(
 			memset(pItemSSID->abySSID, 0, WLAN_SSID_MAXLEN);
 			pMgmt->eCurrState = WMAC_STATE_IDLE;
 			pMgmt->sNodeDBTable[0].bActive = false;
-//                pDevice->bBeaconBufReady = false;
 		}
 		netif_stop_queue(pDevice->dev);
 		pDevice->eCommandState = WLAN_DISASSOCIATE_WAIT;
@@ -480,7 +472,6 @@ vCommandTimer(
 		}
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " CARDbRadioPowerOff\n");
 		//2008-09-02  <mark>	by chester
-		// CARDbRadioPowerOff(pDevice);
 		s_bCommandComplete(pDevice);
 		break;
 
@@ -492,7 +483,6 @@ vCommandTimer(
 			return;
 		}
 //2008-09-02  <mark> by chester
-		// CARDbRadioPowerOff(pDevice);
 		s_bCommandComplete(pDevice);
 		break;
 
@@ -504,8 +494,6 @@ vCommandTimer(
 			return;
 		}
 		printk("chester-abyDesireSSID=%s\n", ((PWLAN_IE_SSID)pMgmt->abyDesireSSID)->abySSID);
-		//memcpy(pMgmt->abyAdHocSSID,pMgmt->abyDesireSSID,
-		//((PWLAN_IE_SSID)pMgmt->abyDesireSSID)->len + WLAN_IEHDR_LEN);
 		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);
@@ -543,9 +531,9 @@ vCommandTimer(
 		if ((pMgmt->eCurrMode == WMAC_MODE_ESS_STA) && (pMgmt->eCurrState == WMAC_STATE_JOINTED)) {
 			// Call mgr to begin the deauthentication
 			// reason = (3) because sta has left ESS
-			if (pMgmt->eCurrState >= WMAC_STATE_AUTH) {
+			if (pMgmt->eCurrState >= WMAC_STATE_AUTH)
 				vMgrDeAuthenBeginSta((void *)pDevice, pMgmt, pMgmt->abyCurrBSSID, (3), &Status);
-			}
+
 			// Call mgr to begin the authentication
 			vMgrAuthenBeginSta((void *)pDevice, pMgmt, &Status);
 			if (Status == CMD_STATUS_SUCCESS) {
@@ -560,9 +548,9 @@ vCommandTimer(
 		// if Adhoc mode
 		else if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
 			if (pMgmt->eCurrState == WMAC_STATE_JOINTED) {
-				if (netif_queue_stopped(pDevice->dev)) {
+				if (netif_queue_stopped(pDevice->dev))
 					netif_wake_queue(pDevice->dev);
-				}
+
 				pDevice->bLinkPass = true;
 
 				pMgmt->sNodeDBTable[0].bActive = true;
@@ -571,9 +559,9 @@ vCommandTimer(
 			} else {
 				// start own IBSS
 				vMgrCreateOwnIBSS((void *)pDevice, &Status);
-				if (Status != CMD_STATUS_SUCCESS) {
+				if (Status != CMD_STATUS_SUCCESS)
 					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " WLAN_CMD_IBSS_CREATE fail ! \n");
-				}
+
 				BSSvAddMulticastNode(pDevice);
 			}
 		}
@@ -583,13 +571,13 @@ vCommandTimer(
 			    pMgmt->eConfigMode == WMAC_CONFIG_AUTO) {
 				// start own IBSS
 				vMgrCreateOwnIBSS((void *)pDevice, &Status);
-				if (Status != CMD_STATUS_SUCCESS) {
+				if (Status != CMD_STATUS_SUCCESS)
 					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " WLAN_CMD_IBSS_CREATE fail ! \n");
-				}
+
 				BSSvAddMulticastNode(pDevice);
-				if (netif_queue_stopped(pDevice->dev)) {
+				if (netif_queue_stopped(pDevice->dev))
 					netif_wake_queue(pDevice->dev);
-				}
+
 				pDevice->bLinkPass = true;
 			} else {
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Disconnect SSID none\n");
@@ -641,12 +629,12 @@ vCommandTimer(
 	case WLAN_ASSOCIATE_WAIT:
 		if (pMgmt->eCurrState == WMAC_STATE_ASSOC) {
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "eCurrState == WMAC_STATE_ASSOC\n");
-			if (pDevice->ePSMode != WMAC_POWER_CAM) {
+			if (pDevice->ePSMode != WMAC_POWER_CAM)
 				PSvEnablePowerSaving((void *)pDevice, pMgmt->wListenInterval);
-			}
-			if (pMgmt->eAuthenMode >= WMAC_AUTH_WPA) {
+
+			if (pMgmt->eAuthenMode >= WMAC_AUTH_WPA)
 				KeybRemoveAllKey(&(pDevice->sKey), pDevice->abyBSSID, pDevice->PortOffset);
-			}
+
 			pDevice->bLinkPass = true;
 			pDevice->byLinkWaitCount = 0;
 			pDevice->byReAssocCount = 0;
@@ -655,9 +643,9 @@ vCommandTimer(
 				BBvSetFOE(pDevice->PortOffset);
 				PSbSendNullPacket(pDevice);
 			}
-			if (netif_queue_stopped(pDevice->dev)) {
+			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);
@@ -667,8 +655,8 @@ vCommandTimer(
 				pDevice->sTimerTxData.expires = RUN_AT(10*HZ);      //10s callback
 				pDevice->fTxDataInSleep = false;
 				pDevice->nTxDataTimeCout = 0;
-			} else {
 			}
+
 			pDevice->IsTxDataTrigger = true;
 			add_timer(&pDevice->sTimerTxData);
 #endif
@@ -703,17 +691,17 @@ vCommandTimer(
 			pDevice->bFixRate = false;
 
 			vMgrCreateOwnIBSS((void *)pDevice, &Status);
-			if (Status != CMD_STATUS_SUCCESS) {
+			if (Status != CMD_STATUS_SUCCESS)
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " 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);
 			BSSvAddMulticastNode(pDevice);
-			if (netif_queue_stopped(pDevice->dev)) {
+			if (netif_queue_stopped(pDevice->dev))
 				netif_wake_queue(pDevice->dev);
-			}
+
 			pDevice->bLinkPass = true;
 			add_timer(&pMgmt->sTimerSecondCallback);
 		}
@@ -730,9 +718,9 @@ vCommandTimer(
 				} else {
 					pDevice->bMoreData = true;
 				}
-				if (!device_dma0_xmit(pDevice, skb, 0)) {
+				if (!device_dma0_xmit(pDevice, skb, 0))
 					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Multicast ps tx fail \n");
-				}
+
 				pMgmt->sNodeDBTable[0].wEnQueueCnt--;
 			}
 		}
@@ -752,9 +740,9 @@ vCommandTimer(
 					} else {
 						pDevice->bMoreData = true;
 					}
-					if (!device_dma0_xmit(pDevice, skb, ii)) {
+					if (!device_dma0_xmit(pDevice, skb, ii))
 						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "sta ps tx fail \n");
-					}
+
 					pMgmt->sNodeDBTable[ii].wEnQueueCnt--;
 					// check if sta ps enabled, and wait next pspoll.
 					// if sta ps disable, then send all pending buffers.
@@ -785,7 +773,6 @@ vCommandTimer(
 		break;
 
 	case WLAN_CMD_CHECK_BBSENSITIVITY_CHANGE:
-		//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "eCommandState == WLAN_CMD_CHECK_BBSENSITIVITY_START\n");
 		// wait all TD complete
 		if (pDevice->iTDUsed[TYPE_AC0DMA] != 0) {
 			vCommandTimerWait((void *)pDevice, 10);
@@ -820,7 +807,6 @@ s_bCommandComplete(
 {
 	PWLAN_IE_SSID pSSID;
 	bool bRadioCmd = false;
-	//unsigned short wDeAuthenReason = 0;
 	bool bForceSCAN = true;
 	PSMgmtObject  pMgmt = pDevice->pMgmt;
 
@@ -842,19 +828,11 @@ s_bCommandComplete(
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "eCommandState= WLAN_CMD_BSSID_SCAN\n");
 			pDevice->eCommandState = WLAN_CMD_SCAN_START;
 			pMgmt->uScanChannel = 0;
-			if (pSSID->len != 0) {
+			if (pSSID->len != 0)
 				memcpy(pMgmt->abyScanSSID, pSSID, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-			} else {
+			else
 				memset(pMgmt->abyScanSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-			}
-/*
-  if ((bForceSCAN == false) && (pDevice->bLinkPass == true)) {
-  if ((pSSID->len == ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len) &&
-  (!memcmp(pSSID->abySSID, ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->abySSID, pSSID->len))) {
-  pDevice->eCommandState = WLAN_CMD_IDLE;
-  }
-  }
-*/
+
 			break;
 		case WLAN_CMD_SSID:
 			pDevice->eCommandState = WLAN_CMD_SSID_START;
@@ -900,9 +878,9 @@ bool bScheduleCommand(
 {
 	PSDevice        pDevice = (PSDevice)hDeviceContext;
 
-	if (pDevice->cbFreeCmdQueue == 0) {
+	if (pDevice->cbFreeCmdQueue == 0)
 		return false;
-	}
+
 	pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].eCmd = eCommand;
 	pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].bForceSCAN = true;
 	memset(pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].abyCmdDesireSSID, 0 , WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
@@ -923,11 +901,6 @@ bool bScheduleCommand(
 		case WLAN_CMD_DISASSOCIATE:
 			pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].bNeedRadioOFF = *((int *)pbyItem0);
 			break;
-/*
-  case WLAN_CMD_DEAUTH:
-  pDevice->eCmdQueue[pDevice->uCmdEnqueueIdx].wDeAuthenReason = *((unsigned short *)pbyItem0);
-  break;
-*/
 
 		case WLAN_CMD_RX_PSPOLL:
 			break;
@@ -948,10 +921,9 @@ bool bScheduleCommand(
 	ADD_ONE_WITH_WRAP_AROUND(pDevice->uCmdEnqueueIdx, CMD_Q_SIZE);
 	pDevice->cbFreeCmdQueue--;
 
-	if (!pDevice->bCmdRunning) {
+	if (!pDevice->bCmdRunning)
 		s_bCommandComplete(pDevice);
-	} else {
-	}
+
 	return true;
 }
 
diff --git a/drivers/staging/vt6655/wctl.c b/drivers/staging/vt6655/wctl.c
index 950039f6d12..fddea9fc9cf 100644
--- a/drivers/staging/vt6655/wctl.c
+++ b/drivers/staging/vt6655/wctl.c
@@ -43,7 +43,7 @@
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
-// static int          msglevel                =MSG_LEVEL_INFO;
+
 /*---------------------  Static Functions  --------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
@@ -113,7 +113,6 @@ unsigned int WCTLuSearchDFCB(PSDevice pDevice, PS802_11Header pMACHeader)
 		if (pDevice->sRxDFCB[ii].bInUse &&
 		    ether_addr_equal(pDevice->sRxDFCB[ii].abyAddr2,
 				     pMACHeader->abyAddr2)) {
-			//
 			return ii;
 		}
 	}
@@ -192,9 +191,8 @@ bool WCTLbHandleFragment(PSDevice pDevice, PS802_11Header pMACHeader, unsigned i
 			pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wFragNum = (pMACHeader->wSeqCtl & 0x000F);
 		} else {
 			pDevice->uCurrentDFCBIdx = WCTLuInsertDFCB(pDevice, pMACHeader);
-			if (pDevice->uCurrentDFCBIdx == pDevice->cbDFCB) {
+			if (pDevice->uCurrentDFCBIdx == pDevice->cbDFCB)
 				return false;
-			}
 		}
 		// reserve 4 byte to match MAC RX Buffer
 		pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer = (unsigned char *)(pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].skb->data + 4);
@@ -202,7 +200,6 @@ bool WCTLbHandleFragment(PSDevice pDevice, PS802_11Header pMACHeader, unsigned i
 		pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].cbFrameLength = cbFrameLength;
 		pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer += cbFrameLength;
 		pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wFragNum++;
-		//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "First pDevice->uCurrentDFCBIdx= %d\n", pDevice->uCurrentDFCBIdx);
 		return false;
 	} else {
 		pDevice->uCurrentDFCBIdx = WCTLuSearchDFCB(pDevice, pMACHeader);
@@ -214,7 +211,6 @@ bool WCTLbHandleFragment(PSDevice pDevice, PS802_11Header pMACHeader, unsigned i
 				pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].cbFrameLength += (cbFrameLength - uHeaderSize);
 				pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].pbyRxBuffer += (cbFrameLength - uHeaderSize);
 				pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wFragNum++;
-				//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Second pDevice->uCurrentDFCBIdx= %d\n", pDevice->uCurrentDFCBIdx);
 			} else {
 				// seq error or frag # error flush DFCB
 				pDevice->cbFreeDFCB++;
@@ -228,7 +224,6 @@ bool WCTLbHandleFragment(PSDevice pDevice, PS802_11Header pMACHeader, unsigned i
 			//enq defragcontrolblock
 			pDevice->cbFreeDFCB++;
 			pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].bInUse = false;
-			//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Last pDevice->uCurrentDFCBIdx= %d\n", pDevice->uCurrentDFCBIdx);
 			return true;
 		}
 		return false;
diff --git a/drivers/staging/vt6655/wmgr.c b/drivers/staging/vt6655/wmgr.c
index b673bc9b213..67384782b70 100644
--- a/drivers/staging/vt6655/wmgr.c
+++ b/drivers/staging/vt6655/wmgr.c
@@ -88,7 +88,6 @@
 
 /*---------------------  Static Variables  --------------------------*/
 static int msglevel = MSG_LEVEL_INFO;
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 
 /*---------------------  Static Functions  --------------------------*/
 //2008-8-4 <add> by chester
@@ -351,11 +350,10 @@ vMgrObjectInit(
 	pMgmt->pbyPSPacketPool = &pMgmt->byPSPacketPool[0];
 	pMgmt->pbyMgmtPacketPool = &pMgmt->byMgmtPacketPool[0];
 	pMgmt->uCurrChannel = pDevice->uChannel;
-	for (ii = 0; ii < WLAN_BSSID_LEN; ii++) {
+	for (ii = 0; ii < WLAN_BSSID_LEN; ii++)
 		pMgmt->abyDesireBSSID[ii] = 0xFF;
-	}
+
 	pMgmt->sAssocInfo.AssocInfo.Length = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
-	//memset(pMgmt->abyDesireSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN +1);
 	pMgmt->byCSSPK = KEY_CTL_NONE;
 	pMgmt->byCSSGK = KEY_CTL_NONE;
 	pMgmt->wIBSSBeaconPeriod = DEFAULT_IBSS_BI;
@@ -458,9 +456,9 @@ vMgrAssocBeginSta(
 
 	pMgmt->wCurrCapInfo = 0;
 	pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_ESS(1);
-	if (pDevice->bEncryptionEnable) {
+	if (pDevice->bEncryptionEnable)
 		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_PRIVACY(1);
-	}
+
 	pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
 	if (pMgmt->wListenInterval == 0)
 		pMgmt->wListenInterval = 1;    // at least one.
@@ -468,13 +466,11 @@ vMgrAssocBeginSta(
 	// ERP Phy (802.11g) should support short preamble.
 	if (pMgmt->eCurrentPHYMode == PHY_TYPE_11G) {
 		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-		if (CARDbIsShorSlotTime(pMgmt->pAdapter)) {
+		if (CARDbIsShorSlotTime(pMgmt->pAdapter))
 			pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTSLOTTIME(1);
-		}
 	} else if (pMgmt->eCurrentPHYMode == PHY_TYPE_11B) {
-		if (CARDbIsShortPreamble(pMgmt->pAdapter)) {
+		if (CARDbIsShortPreamble(pMgmt->pAdapter))
 			pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-		}
 	}
 	if (pMgmt->b11hEnable)
 		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SPECTRUMMNG(1);
@@ -499,8 +495,9 @@ vMgrAssocBeginSta(
 			pMgmt->eCurrState = WMAC_STATE_ASSOCPENDING;
 			*pStatus = CMD_STATUS_SUCCESS;
 		}
-	} else
+	} else {
 		*pStatus = CMD_STATUS_RESOURCES;
+	}
 
 	return;
 }
@@ -527,9 +524,8 @@ vMgrReAssocBeginSta(
 
 	pMgmt->wCurrCapInfo = 0;
 	pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_ESS(1);
-	if (pDevice->bEncryptionEnable) {
+	if (pDevice->bEncryptionEnable)
 		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_PRIVACY(1);
-	}
 
 	pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
 
@@ -539,14 +535,13 @@ vMgrReAssocBeginSta(
 	// ERP Phy (802.11g) should support short preamble.
 	if (pMgmt->eCurrentPHYMode == PHY_TYPE_11G) {
 		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-		if (CARDbIsShorSlotTime(pMgmt->pAdapter)) {
+		if (CARDbIsShorSlotTime(pMgmt->pAdapter))
 			pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTSLOTTIME(1);
-		}
 	} else if (pMgmt->eCurrentPHYMode == PHY_TYPE_11B) {
-		if (CARDbIsShortPreamble(pMgmt->pAdapter)) {
+		if (CARDbIsShortPreamble(pMgmt->pAdapter))
 			pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-		}
 	}
+
 	if (pMgmt->b11hEnable)
 		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SPECTRUMMNG(1);
 
@@ -565,11 +560,10 @@ vMgrReAssocBeginSta(
 	if (pTxPacket != NULL) {
 		/* send the frame */
 		*pStatus = csMgmt_xmit(pDevice, pTxPacket);
-		if (*pStatus != CMD_STATUS_PENDING) {
+		if (*pStatus != CMD_STATUS_PENDING)
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Reassociation tx failed.\n");
-		} else {
+		else
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Reassociation tx sending.\n");
-		}
 	}
 
 	return;
@@ -686,21 +680,20 @@ s_vMgrRxAssocRequest(
 		pMgmt->sNodeDBTable[uNodeIndex].bPSEnable =
 			WLAN_GET_FC_PWRMGT(sFrame.pHdr->sA3.wFrameCtl) ? true : false;
 		// Todo: check sta basic rate, if ap can't support, set status code
-		if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
+		if (pDevice->eCurrentPHYType == PHY_TYPE_11B)
 			uRateLen = WLAN_RATES_MAXLEN_11B;
-		}
+
 		abyCurrSuppRates[0] = WLAN_EID_SUPP_RATES;
 		abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pSuppRates,
 						 (PWLAN_IE_SUPP_RATES)abyCurrSuppRates,
 						 uRateLen);
 		abyCurrExtSuppRates[0] = WLAN_EID_EXTSUPP_RATES;
-		if (pDevice->eCurrentPHYType == PHY_TYPE_11G) {
+		if (pDevice->eCurrentPHYType == PHY_TYPE_11G)
 			abyCurrExtSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pExtSuppRates,
 							    (PWLAN_IE_SUPP_RATES)abyCurrExtSuppRates,
 							    uRateLen);
-		} else {
+		else
 			abyCurrExtSuppRates[1] = 0;
-		}
 
 		RATEvParseMaxRate((void *)pDevice,
 				  (PWLAN_IE_SUPP_RATES)abyCurrSuppRates,
@@ -736,9 +729,8 @@ s_vMgrRxAssocRequest(
 			pDevice->bProtectMode = true;
 			pDevice->bNonERPPresent = true;
 		}
-		if (!pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble) {
+		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",
@@ -751,8 +743,8 @@ s_vMgrRxAssocRequest(
 			);
 		DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Max Support rate = %d \n",
 			pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate);
-	}//else { TODO: received STA under state1 handle }
-	else {
+	} else {
+		/* TODO: received STA under state1 handle */
 		return;
 	}
 
@@ -769,17 +761,15 @@ s_vMgrRxAssocRequest(
 			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates
 );
 	if (pTxPacket != NULL) {
-		if (pDevice->bEnableHostapd) {
+		if (pDevice->bEnableHostapd)
 			return;
-		}
+
 		/* send the frame */
 		Status = csMgmt_xmit(pDevice, pTxPacket);
-		if (Status != CMD_STATUS_PENDING) {
+		if (Status != CMD_STATUS_PENDING)
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Assoc response tx failed\n");
-		} else {
+		else
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Assoc response tx sending..\n");
-		}
-
 	}
 
 	return;
@@ -839,9 +829,8 @@ s_vMgrRxReAssocRequest(
 			WLAN_GET_FC_PWRMGT(sFrame.pHdr->sA3.wFrameCtl) ? true : false;
 		// Todo: check sta basic rate, if ap can't support, set status code
 
-		if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
+		if (pDevice->eCurrentPHYType == PHY_TYPE_11B)
 			uRateLen = WLAN_RATES_MAXLEN_11B;
-		}
 
 		abyCurrSuppRates[0] = WLAN_EID_SUPP_RATES;
 		abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pSuppRates,
@@ -891,9 +880,8 @@ s_vMgrRxReAssocRequest(
 			pDevice->bProtectMode = true;
 			pDevice->bNonERPPresent = true;
 		}
-		if (!pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble) {
+		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",
@@ -924,15 +912,14 @@ s_vMgrRxReAssocRequest(
 
 	if (pTxPacket != NULL) {
 		/* send the frame */
-		if (pDevice->bEnableHostapd) {
+		if (pDevice->bEnableHostapd)
 			return;
-		}
+
 		Status = csMgmt_xmit(pDevice, pTxPacket);
-		if (Status != CMD_STATUS_PENDING) {
+		if (Status != CMD_STATUS_PENDING)
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:ReAssoc response tx failed\n");
-		} else {
+		else
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:ReAssoc response tx sending..\n");
-		}
 	}
 	return;
 }
@@ -990,9 +977,9 @@ s_vMgrRxAssocResponse(
 		if (cpu_to_le16((*(sFrame.pwStatus))) == WLAN_MGMT_STATUS_SUCCESS) {
 			// set AID
 			pMgmt->wCurrAID = cpu_to_le16((*(sFrame.pwAid)));
-			if ((pMgmt->wCurrAID >> 14) != (BIT0 | BIT1)) {
+			if ((pMgmt->wCurrAID >> 14) != (BIT0 | BIT1))
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "AID from AP, has two msb clear.\n");
-			}
+
 			DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Association Successful, AID=%d.\n", pMgmt->wCurrAID & ~(BIT14 | BIT15));
 			pMgmt->eCurrState = WMAC_STATE_ASSOC;
 			BSSvUpdateAPNode((void *)pDevice, sFrame.pwCapInfo, sFrame.pSuppRates, sFrame.pExtSuppRates);
@@ -1189,9 +1176,8 @@ vMgrDeAuthenBeginSta(
 	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
 
 	*pStatus = csMgmt_xmit(pDevice, pTxPacket);
-	if (*pStatus == CMD_STATUS_PENDING) {
+	if (*pStatus == CMD_STATUS_PENDING)
 		*pStatus = CMD_STATUS_SUCCESS;
-	}
 
 	return;
 }
@@ -1340,13 +1326,13 @@ s_vMgrRxAuthenSequence_1(
 	pTxPacket->cbMPDULen = sFrame.len;
 	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
 	// send the frame
-	if (pDevice->bEnableHostapd) {
+	if (pDevice->bEnableHostapd)
 		return;
-	}
+
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Authreq_reply sequence_1 tx.. \n");
-	if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
+	if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING)
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Authreq_reply sequence_1 tx failed.\n");
-	}
+
 	return;
 }
 
@@ -1384,11 +1370,6 @@ s_vMgrRxAuthenSequence_2(
 			s_vMgrLogStatus(pMgmt, cpu_to_le16((*(pFrame->pwStatus))));
 			pMgmt->eCurrState = WMAC_STATE_IDLE;
 		}
-		if (pDevice->eCommandState == WLAN_AUTHENTICATE_WAIT) {
-//                spin_unlock_irq(&pDevice->lock);
-//                vCommandTimerWait((void *)pDevice, 0);
-//                spin_lock_irq(&pDevice->lock);
-		}
 
 		break;
 
@@ -1424,17 +1405,12 @@ s_vMgrRxAuthenSequence_2(
 			pTxPacket->cbMPDULen = sFrame.len;
 			pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
 			// send the frame
-			if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
+			if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING)
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Auth_reply sequence_2 tx failed.\n");
-			}
+
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Auth_reply sequence_2 tx ...\n");
 		} else {
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:rx Auth_reply sequence_2 status error ...\n");
-			if (pDevice->eCommandState == WLAN_AUTHENTICATE_WAIT) {
-//                    spin_unlock_irq(&pDevice->lock);
-//                    vCommandTimerWait((void *)pDevice, 0);
-//                    spin_lock_irq(&pDevice->lock);
-			}
 			s_vMgrLogStatus(pMgmt, cpu_to_le16((*(pFrame->pwStatus))));
 		}
 		break;
@@ -1523,12 +1499,12 @@ reply:
 	pTxPacket->cbMPDULen = sFrame.len;
 	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
 	// send the frame
-	if (pDevice->bEnableHostapd) {
+	if (pDevice->bEnableHostapd)
 		return;
-	}
-	if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
+
+	if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING)
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Authreq_reply sequence_4 tx failed.\n");
-	}
+
 	return;
 }
 
@@ -1559,12 +1535,6 @@ s_vMgrRxAuthenSequence_4(
 		s_vMgrLogStatus(pMgmt, cpu_to_le16((*(pFrame->pwStatus))));
 		pMgmt->eCurrState = WMAC_STATE_IDLE;
 	}
-
-	if (pDevice->eCommandState == WLAN_AUTHENTICATE_WAIT) {
-//        spin_unlock_irq(&pDevice->lock);
-//        vCommandTimerWait((void *)pDevice, 0);
-//        spin_lock_irq(&pDevice->lock);
-	}
 }
 
 /*+
@@ -1588,7 +1558,6 @@ s_vMgrRxDisassociation(
 {
 	WLAN_FR_DISASSOC    sFrame;
 	unsigned int uNodeIndex = 0;
-//    CMD_STATUS          CmdStatus;
 	viawget_wpa_header *wpahdr;
 
 	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
@@ -1596,11 +1565,11 @@ s_vMgrRxDisassociation(
 		// a STA is leaving this BSS..
 		sFrame.len = pRxPacket->cbMPDULen;
 		sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-		if (BSSDBbIsSTAInNodeDB(pMgmt, pRxPacket->p80211Header->sA3.abyAddr2, &uNodeIndex)) {
+		if (BSSDBbIsSTAInNodeDB(pMgmt, pRxPacket->p80211Header->sA3.abyAddr2, &uNodeIndex))
 			BSSvRemoveOneNode(pDevice, uNodeIndex);
-		} else {
+		else
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Rx disassoc, sta not found\n");
-		}
+
 	} else if (pMgmt->eCurrMode == WMAC_MODE_ESS_STA) {
 		sFrame.len = pRxPacket->cbMPDULen;
 		sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
@@ -1668,11 +1637,10 @@ s_vMgrRxDeauthentication(
 		// a STA is leaving this BSS..
 		sFrame.len = pRxPacket->cbMPDULen;
 		sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-		if (BSSDBbIsSTAInNodeDB(pMgmt, pRxPacket->p80211Header->sA3.abyAddr2, &uNodeIndex)) {
+		if (BSSDBbIsSTAInNodeDB(pMgmt, pRxPacket->p80211Header->sA3.abyAddr2, &uNodeIndex))
 			BSSvRemoveOneNode(pDevice, uNodeIndex);
-		} else {
+		else
 			DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Rx deauth, sta not found\n");
-		}
 	} else {
 		if (pMgmt->eCurrMode == WMAC_MODE_ESS_STA) {
 			sFrame.len = pRxPacket->cbMPDULen;
@@ -1871,7 +1839,6 @@ s_vMgrRxBeacon(
 				    (void *)pRxPacket
 );
 	} else {
-//        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "update bcn: RxChannel = : %d\n", byCurrChannel);
 		BSSbUpdateToBSSList((void *)pDevice,
 				    *sFrame.pqwTimestamp,
 				    *sFrame.pwBeaconInterval,
@@ -1894,9 +1861,8 @@ s_vMgrRxBeacon(
 
 	}
 
-	if (bInScan) {
+	if (bInScan)
 		return;
-	}
 
 	if (byCurrChannel == (unsigned char)pMgmt->uCurrChannel)
 		bIsChannelEqual = true;
@@ -1942,10 +1908,8 @@ s_vMgrRxBeacon(
 		pDevice->uCurrRSSI = pRxPacket->uRSSI;
 		pDevice->byCurrSQ = pRxPacket->bySQ;
 
-		if (pMgmt->sNodeDBTable[0].uInActiveCount != 0) {
+		if (pMgmt->sNodeDBTable[0].uInActiveCount != 0)
 			pMgmt->sNodeDBTable[0].uInActiveCount = 0;
-			//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BCN:Wake Count= [%d]\n", pMgmt->wCountToWakeUp);
-		}
 	}
 	// check if SSID the same
 	if (sFrame.pSSID->len == ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len) {
@@ -1982,9 +1946,9 @@ s_vMgrRxBeacon(
 			//
 			// Basic Rate Set may change dynamically
 			//
-			if (pBSSList->eNetworkTypeInUse == PHY_TYPE_11B) {
+			if (pBSSList->eNetworkTypeInUse == PHY_TYPE_11B)
 				uRateLen = WLAN_RATES_MAXLEN_11B;
-			}
+
 			pMgmt->abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)pBSSList->abySuppRates,
 								(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
 								uRateLen);
@@ -2044,22 +2008,20 @@ s_vMgrRxBeacon(
 
 	// check if beacon TSF larger or small than our local TSF
 	if (HIDWORD(qwTimestamp) == HIDWORD(qwLocalTSF)) {
-		if (LODWORD(qwTimestamp) >= LODWORD(qwLocalTSF)) {
+		if (LODWORD(qwTimestamp) >= LODWORD(qwLocalTSF))
 			bTSFOffsetPostive = true;
-		} else {
+		else
 			bTSFOffsetPostive = false;
-		}
 	} else if (HIDWORD(qwTimestamp) > HIDWORD(qwLocalTSF)) {
 		bTSFOffsetPostive = true;
 	} else if (HIDWORD(qwTimestamp) < HIDWORD(qwLocalTSF)) {
 		bTSFOffsetPostive = false;
 	}
 
-	if (bTSFOffsetPostive) {
+	if (bTSFOffsetPostive)
 		qwTSFOffset = CARDqGetTSFOffset(pRxPacket->byRxRate, (qwTimestamp), (qwLocalTSF));
-	} else {
+	else
 		qwTSFOffset = CARDqGetTSFOffset(pRxPacket->byRxRate, (qwLocalTSF), (qwTimestamp));
-	}
 
 	if (HIDWORD(qwTSFOffset) != 0 ||
 	    (LODWORD(qwTSFOffset) > TRIVIAL_SYNC_DIFFERENCE)) {
@@ -2101,11 +2063,9 @@ s_vMgrRxBeacon(
 			    (pMgmt->bMulticastTIM && (pMgmt->byDTIMCount == 0))) {
 				pMgmt->bInTIMWake = true;
 				// send out ps-poll packet
-//                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BCN:In TIM\n");
-				if (pMgmt->bInTIM) {
+
+				if (pMgmt->bInTIM)
 					PSvSendPSPOLL((PSDevice)pDevice);
-//                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BCN:PS-POLL sent..\n");
-				}
 
 			} else {
 				pMgmt->bInTIMWake = false;
@@ -2115,9 +2075,8 @@ s_vMgrRxBeacon(
 					if (PSbSendNullPacket(pDevice))
 						pDevice->bPWBitOn = true;
 				}
-				if (PSbConsiderPowerDown(pDevice, false, false)) {
+				if (PSbConsiderPowerDown(pDevice, false, false))
 					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BCN: Power down now...\n");
-				}
 			}
 
 		}
@@ -2181,11 +2140,6 @@ s_vMgrRxBeacon(
 					printk("s_vMgrRxBeacon:TxDataRate is %d,Index is %d\n", pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate, uNodeIndex);
 				}
 #endif
-/*
-  pMgmt->sNodeDBTable[uNodeIndex].bShortSlotTime = WLAN_GET_CAP_INFO_SHORTSLOTTIME(*sFrame.pwCapInfo);
-  if (pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate > RATE_11M)
-  pMgmt->sNodeDBTable[uNodeIndex].bERPExist = true;
-*/
 			}
 
 			// if other stations joined, indicate connection to upper layer..
@@ -2193,9 +2147,9 @@ s_vMgrRxBeacon(
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Current IBSS State: [Started]........to: [Jointed] \n");
 				pMgmt->eCurrState = WMAC_STATE_JOINTED;
 				pDevice->bLinkPass = true;
-				if (netif_queue_stopped(pDevice->dev)) {
+				if (netif_queue_stopped(pDevice->dev))
 					netif_wake_queue(pDevice->dev);
-				}
+
 				pMgmt->sNodeDBTable[0].bActive = true;
 				pMgmt->sNodeDBTable[0].uInActiveCount = 0;
 
@@ -2230,12 +2184,8 @@ s_vMgrRxBeacon(
 						     pMgmt->abyCurrSuppRates,
 						     pMgmt->abyCurrExtSuppRates);
 
-				// MACvRegBitsOff(pDevice->PortOffset, MAC_REG_RCR, RCR_BSSID);
-				// set highest basic rate
-				// s_vSetHighestBasicRate(pDevice, (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates);
 				// Prepare beacon frame
 				bMgrPrepareBeaconToSend((void *)pDevice, pMgmt);
-				//  }
 			}
 		}
 	}
@@ -2365,9 +2315,8 @@ vMgrCreateOwnIBSS(
 			  &wMaxBasicRate, &wMaxSuppRate, &wSuppRate,
 			  &byTopCCKBasicRate, &byTopOFDMBasicRate);
 
-	if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
+	if (pMgmt->eConfigMode == WMAC_CONFIG_AP)
 		pMgmt->eCurrMode = WMAC_MODE_ESS_AP;
-	}
 
 	if (pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA) {
 		memcpy(pMgmt->abyIBSSDFSOwner, pDevice->abyCurrentNetAddr, 6);
@@ -2383,8 +2332,6 @@ vMgrCreateOwnIBSS(
 	MACvWriteATIMW(pDevice->PortOffset, pMgmt->wCurrATIMWindow);
 	pDevice->uCurrRSSI = 0;
 	pDevice->byCurrSQ = 0;
-	//memcpy(pMgmt->abyDesireSSID,pMgmt->abyAdHocSSID,
-	// ((PWLAN_IE_SSID)pMgmt->abyAdHocSSID)->len + WLAN_IEHDR_LEN);
 	memset(pMgmt->abyCurrSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
 	memcpy(pMgmt->abyCurrSSID,
 	       pMgmt->abyDesireSSID,
@@ -2428,9 +2375,8 @@ vMgrCreateOwnIBSS(
 		pMgmt->byDTIMCount = pMgmt->byDTIMPeriod - 1;
 	}
 
-	if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
+	if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)
 		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_IBSS(1);
-	}
 
 	if (pDevice->bEncryptionEnable) {
 		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_PRIVACY(1);
@@ -2453,8 +2399,6 @@ vMgrCreateOwnIBSS(
 
 	pMgmt->byERPContext = 0;
 
-//    memcpy(pDevice->abyBSSID, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
-
 	if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
 		CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_AP);
 	} else {
@@ -2474,11 +2418,10 @@ vMgrCreateOwnIBSS(
 	set_channel(pMgmt->pAdapter, pMgmt->uIBSSChannel);
 	pMgmt->uCurrChannel = pMgmt->uIBSSChannel;
 
-	if (CARDbIsShortPreamble(pMgmt->pAdapter)) {
+	if (CARDbIsShortPreamble(pMgmt->pAdapter))
 		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-	} else {
+	else
 		pMgmt->wCurrCapInfo &= (~WLAN_SET_CAP_INFO_SHORTPREAMBLE(1));
-	}
 
 	if (pMgmt->b11hEnable &&
 	    (pMgmt->eCurrentPHYMode == PHY_TYPE_11A)) {
@@ -2489,9 +2432,8 @@ vMgrCreateOwnIBSS(
 
 	pMgmt->eCurrState = WMAC_STATE_STARTED;
 	// Prepare beacon to send
-	if (bMgrPrepareBeaconToSend((void *)pDevice, pMgmt)) {
+	if (bMgrPrepareBeaconToSend((void *)pDevice, pMgmt))
 		*pStatus = CMD_STATUS_SUCCESS;
-	}
 
 	return;
 }
@@ -2540,7 +2482,6 @@ vMgrJoinBSSBegin(
 		return;
 	}
 
-	// memset(pMgmt->abyDesireBSSID, 0,  WLAN_BSSID_LEN);
 	// Search known BSS list for prefer BSSID or SSID
 
 	pCurr = BSSpSearchBSSList(pDevice,
@@ -2560,23 +2501,6 @@ vMgrJoinBSSBegin(
 	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
-/*
-  if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled) {
-  if (WPA_SearchRSN(0, WPA_TKIP, pCurr) == false) {
-  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "No match RSN info. ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
-  // encryption mode error
-  pMgmt->eCurrState = WMAC_STATE_IDLE;
-  return;
-  }
-  } else if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
-  if (WPA_SearchRSN(0, WPA_AESCCMP, pCurr) == false) {
-  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "No match RSN info. ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
-  // encryption mode error
-  pMgmt->eCurrState = WMAC_STATE_IDLE;
-  return;
-  }
-  }
-*/
 		}
 
 #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
@@ -2596,9 +2520,8 @@ vMgrJoinBSSBegin(
 			memset(pMgmt->abyCurrSuppRates, 0 , WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1);
 			memset(pMgmt->abyCurrExtSuppRates, 0 , WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1);
 
-			if (pCurr->eNetworkTypeInUse == PHY_TYPE_11B) {
+			if (pCurr->eNetworkTypeInUse == PHY_TYPE_11B)
 				uRateLen = WLAN_RATES_MAXLEN_11B;
-			}
 
 			pItemRates = (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates;
 			pItemExtRates = (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates;
@@ -2625,9 +2548,8 @@ vMgrJoinBSSBegin(
 				pItemRates->len += (unsigned char)ii;
 				if (pItemExtRates->len - ii > 0) {
 					pItemExtRates->len -= (unsigned char)ii;
-					for (uu = 0; uu < pItemExtRates->len; uu++) {
+					for (uu = 0; uu < pItemExtRates->len; uu++)
 						pItemExtRates->abyRates[uu] = pItemExtRates->abyRates[uu + ii];
-					}
 				} else {
 					pItemExtRates->len = 0;
 				}
@@ -2647,9 +2569,6 @@ vMgrJoinBSSBegin(
 			pMgmt->eCurrMode = WMAC_MODE_ESS_STA;
 
 			pMgmt->eCurrState = WMAC_STATE_JOINTED;
-			// Adopt BSS state in Adapter Device Object
-			//pDevice->byOpMode = OP_MODE_INFRASTRUCTURE;
-//            memcpy(pDevice->abyBSSID, pCurr->abyBSSID, WLAN_BSSID_LEN);
 
 			// Add current BSS to Candidate list
 			// This should only works for WPA2 BSS, and WPA2 BSS check must be done before.
@@ -2721,15 +2640,10 @@ vMgrJoinBSSBegin(
 			memset(pMgmt->abyCurrSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN);
 			memcpy(pMgmt->abyCurrBSSID, pCurr->abyBSSID, WLAN_BSSID_LEN);
 			memcpy(pMgmt->abyCurrSSID, pCurr->abySSID, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN);
-//          pMgmt->wCurrATIMWindow = pCurr->wATIMWindow;
 			MACvWriteATIMW(pDevice->PortOffset, pMgmt->wCurrATIMWindow);
 			pMgmt->eCurrMode = WMAC_MODE_IBSS_STA;
 
 			pMgmt->eCurrState = WMAC_STATE_STARTED;
-			// Adopt BSS state in Adapter Device Object
-			//pDevice->byOpMode = OP_MODE_ADHOC;
-//            pDevice->bLinkPass = true;
-//            memcpy(pDevice->abyBSSID, pCurr->abyBSSID, WLAN_BSSID_LEN);
 
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Join IBSS ok:%pM\n",
 				pMgmt->abyCurrBSSID);
@@ -2766,7 +2680,7 @@ s_vMgrSynchBSS(
 {
 	CARD_PHY_TYPE   ePhyType = PHY_TYPE_11B;
 	PSMgmtObject  pMgmt = pDevice->pMgmt;
-//    int     ii;
+
 	//1M,   2M,   5M,   11M,  18M,  24M,  36M,  54M
 	unsigned char abyCurrSuppRatesG[] = {WLAN_EID_SUPP_RATES, 8, 0x02, 0x04, 0x0B, 0x16, 0x24, 0x30, 0x48, 0x6C};
 	unsigned char abyCurrExtSuppRatesG[] = {WLAN_EID_EXTSUPP_RATES, 4, 0x0C, 0x12, 0x18, 0x60};
@@ -2862,9 +2776,8 @@ s_vMgrSynchBSS(
 		CARDbSetBSSID(pMgmt->pAdapter, pCurr->abyBSSID, OP_MODE_INFRASTRUCTURE);
 		// 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) {
+		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);
 	}
@@ -2884,24 +2797,6 @@ s_vMgrSynchBSS(
 		return;
 	}
 
-/*
-  for (ii=0; ii<BB_VGA_LEVEL; ii++) {
-  if (pCurr->ldBmMAX< pDevice->ldBmThreshold[ii]) {
-  pDevice->byBBVGANew = pDevice->abyBBVGA[ii];
-  break;
-  }
-  }
-
-  if (pDevice->byBBVGANew != pDevice->byBBVGACurrent) {
-  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "RSSI[%d] NewGain[%d] OldGain[%d] \n",
-  (int)pCurr->ldBmMAX, pDevice->byBBVGANew, pDevice->byBBVGACurrent);
-  printk("RSSI[%d] NewGain[%d] OldGain[%d] \n",
-  (int)pCurr->ldBmMAX, pDevice->byBBVGANew, pDevice->byBBVGACurrent);
-  BBvSetVGAGainOffset(pDevice, pDevice->byBBVGANew);
-  }
-  printk("ldBmMAX[%d] NewGain[%d] OldGain[%d] \n",
-  (int)pCurr->ldBmMAX, pDevice->byBBVGANew, pDevice->byBBVGACurrent);
-*/
 	pMgmt->uCurrChannel = pCurr->uChannel;
 	pMgmt->eCurrentPHYMode = ePhyType;
 	pMgmt->byERPContext = pCurr->sERP.byERP;
@@ -2943,7 +2838,7 @@ static void  Encyption_Rebuild(
 			}
 		}
 	}
-	//  }
+
 	return;
 }
 
@@ -2979,9 +2874,9 @@ s_vMgrFormatTIM(
 		if (!ii) {
 			// Mask out the broadcast bit which is indicated separately.
 			bMulticast = (byMap & byMask[0]) != 0;
-			if (bMulticast) {
+			if (bMulticast)
 				pMgmt->sNodeDBTable[0].bRxPSPoll = true;
-			}
+
 			byMap = 0;
 		}
 		if (byMap) {
@@ -3011,9 +2906,8 @@ s_vMgrFormatTIM(
 
 	// Append variable part of TIM
 
-	for (ii = wStartIndex, jj = 0; ii <= wEndIndex; ii++, jj++) {
+	for (ii = wStartIndex, jj = 0; ii <= wEndIndex; ii++, jj++)
 		pTIM->byVirtBitMap[jj] = pMgmt->abyPSTxMap[ii];
-	}
 
 	// Aid = 0 don't used.
 	pTIM->byVirtBitMap[0]  &= ~BIT0;
@@ -3068,9 +2962,8 @@ s_MgrMakeBeacon(
 			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_BEACON)
 ));
 
-	if (pDevice->bEnablePSMode) {
+	if (pDevice->bEnablePSMode)
 		sFrame.pHdr->sA3.wFrameCtl |= cpu_to_le16((unsigned short)WLAN_SET_FC_PWRMGT(1));
-	}
 
 	memcpy(sFrame.pHdr->sA3.abyAddr1, abyBroadcastAddr, WLAN_ADDR_LEN);
 	memcpy(sFrame.pHdr->sA3.abyAddr2, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
@@ -3293,9 +3186,8 @@ s_MgrMakeProbeResponse(
 	*sFrame.pwBeaconInterval = cpu_to_le16(wCurrBeaconPeriod);
 	*sFrame.pwCapInfo = cpu_to_le16(wCurrCapInfo);
 
-	if (byPHYType == BB_TYPE_11B) {
+	if (byPHYType == BB_TYPE_11B)
 		*sFrame.pwCapInfo &= cpu_to_le16((unsigned short)~(WLAN_SET_CAP_INFO_SHORTSLOTTIME(1)));
-	}
 
 	// Copy SSID
 	sFrame.pSSID = (PWLAN_IE_SSID)(sFrame.pBuf + sFrame.len);
@@ -3538,27 +3430,27 @@ s_MgrMakeAssocRequest(
 		sFrame.pRSNWPA->abyMulticast[0] = 0x00;
 		sFrame.pRSNWPA->abyMulticast[1] = 0x50;
 		sFrame.pRSNWPA->abyMulticast[2] = 0xf2;
-		if (pMgmt->byCSSGK == KEY_CTL_WEP) {
+		if (pMgmt->byCSSGK == KEY_CTL_WEP)
 			sFrame.pRSNWPA->abyMulticast[3] = pMgmt->pCurrBSS->byGKType;
-		} else if (pMgmt->byCSSGK == KEY_CTL_TKIP) {
+		else if (pMgmt->byCSSGK == KEY_CTL_TKIP)
 			sFrame.pRSNWPA->abyMulticast[3] = WPA_TKIP;
-		} else if (pMgmt->byCSSGK == KEY_CTL_CCMP) {
+		else if (pMgmt->byCSSGK == KEY_CTL_CCMP)
 			sFrame.pRSNWPA->abyMulticast[3] = WPA_AESCCMP;
-		} else {
+		else
 			sFrame.pRSNWPA->abyMulticast[3] = WPA_NONE;
-		}
+
 		// Pairwise Key Cipher Suite
 		sFrame.pRSNWPA->wPKCount = 1;
 		sFrame.pRSNWPA->PKSList[0].abyOUI[0] = 0x00;
 		sFrame.pRSNWPA->PKSList[0].abyOUI[1] = 0x50;
 		sFrame.pRSNWPA->PKSList[0].abyOUI[2] = 0xf2;
-		if (pMgmt->byCSSPK == KEY_CTL_TKIP) {
+		if (pMgmt->byCSSPK == KEY_CTL_TKIP)
 			sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_TKIP;
-		} else if (pMgmt->byCSSPK == KEY_CTL_CCMP) {
+		else if (pMgmt->byCSSPK == KEY_CTL_CCMP)
 			sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_AESCCMP;
-		} else {
+		else
 			sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_NONE;
-		}
+
 		// Auth Key Management Suite
 		pbyRSN = (unsigned char *)(sFrame.pBuf + sFrame.len + 2 + sFrame.pRSNWPA->len);
 		*pbyRSN++ = 0x01;
@@ -3567,13 +3459,12 @@ s_MgrMakeAssocRequest(
 
 		*pbyRSN++ = 0x50;
 		*pbyRSN++ = 0xf2;
-		if (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) {
+		if (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK)
 			*pbyRSN++ = WPA_AUTH_PSK;
-		} else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA) {
+		else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA)
 			*pbyRSN++ = WPA_AUTH_IEEE802_1X;
-		} else {
+		else
 			*pbyRSN++ = WPA_NONE;
-		}
 
 		sFrame.pRSNWPA->len += 6;
 
@@ -3604,15 +3495,14 @@ s_MgrMakeAssocRequest(
 		sFrame.pRSN->abyRSN[0] = 0x00;
 		sFrame.pRSN->abyRSN[1] = 0x0F;
 		sFrame.pRSN->abyRSN[2] = 0xAC;
-		if (pMgmt->byCSSGK == KEY_CTL_WEP) {
+		if (pMgmt->byCSSGK == KEY_CTL_WEP)
 			sFrame.pRSN->abyRSN[3] = pMgmt->pCurrBSS->byCSSGK;
-		} else if (pMgmt->byCSSGK == KEY_CTL_TKIP) {
+		else if (pMgmt->byCSSGK == KEY_CTL_TKIP)
 			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_TKIP;
-		} else if (pMgmt->byCSSGK == KEY_CTL_CCMP) {
+		else if (pMgmt->byCSSGK == KEY_CTL_CCMP)
 			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_CCMP;
-		} else {
+		else
 			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_UNKNOWN;
-		}
 
 		// Pairwise Key Cipher Suite
 		sFrame.pRSN->abyRSN[4] = 1;
@@ -3620,15 +3510,15 @@ s_MgrMakeAssocRequest(
 		sFrame.pRSN->abyRSN[6] = 0x00;
 		sFrame.pRSN->abyRSN[7] = 0x0F;
 		sFrame.pRSN->abyRSN[8] = 0xAC;
-		if (pMgmt->byCSSPK == KEY_CTL_TKIP) {
+		if (pMgmt->byCSSPK == KEY_CTL_TKIP)
 			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_TKIP;
-		} else if (pMgmt->byCSSPK == KEY_CTL_CCMP) {
+		else if (pMgmt->byCSSPK == KEY_CTL_CCMP)
 			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_CCMP;
-		} else if (pMgmt->byCSSPK == KEY_CTL_NONE) {
+		else if (pMgmt->byCSSPK == KEY_CTL_NONE)
 			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_USE_GROUP;
-		} else {
+		else
 			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_UNKNOWN;
-		}
+
 		sFrame.pRSN->len += 6;
 
 		// Auth Key Management Suite
@@ -3637,13 +3527,13 @@ s_MgrMakeAssocRequest(
 		sFrame.pRSN->abyRSN[12] = 0x00;
 		sFrame.pRSN->abyRSN[13] = 0x0F;
 		sFrame.pRSN->abyRSN[14] = 0xAC;
-		if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK) {
+		if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)
 			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_PSK;
-		} else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) {
+		else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2)
 			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_802_1X;
-		} else {
+		else
 			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_UNKNOWN;
-		}
+
 		sFrame.pRSN->len += 6;
 
 		// RSN Capabilities
@@ -3668,9 +3558,8 @@ s_MgrMakeAssocRequest(
 					pbyRSN += 16;
 				}
 			}
-			if (*pwPMKID != 0) {
+			if (*pwPMKID != 0)
 				sFrame.pRSN->len += (2 + (*pwPMKID)*16);
-			}
 		}
 
 		sFrame.len += sFrame.pRSN->len + WLAN_IEHDR_LEN;
@@ -3785,27 +3674,27 @@ s_MgrMakeReAssocRequest(
 		sFrame.pRSNWPA->abyMulticast[0] = 0x00;
 		sFrame.pRSNWPA->abyMulticast[1] = 0x50;
 		sFrame.pRSNWPA->abyMulticast[2] = 0xf2;
-		if (pMgmt->byCSSGK == KEY_CTL_WEP) {
+		if (pMgmt->byCSSGK == KEY_CTL_WEP)
 			sFrame.pRSNWPA->abyMulticast[3] = pMgmt->pCurrBSS->byGKType;
-		} else if (pMgmt->byCSSGK == KEY_CTL_TKIP) {
+		else if (pMgmt->byCSSGK == KEY_CTL_TKIP)
 			sFrame.pRSNWPA->abyMulticast[3] = WPA_TKIP;
-		} else if (pMgmt->byCSSGK == KEY_CTL_CCMP) {
+		else if (pMgmt->byCSSGK == KEY_CTL_CCMP)
 			sFrame.pRSNWPA->abyMulticast[3] = WPA_AESCCMP;
-		} else {
+		else
 			sFrame.pRSNWPA->abyMulticast[3] = WPA_NONE;
-		}
+
 		// Pairwise Key Cipher Suite
 		sFrame.pRSNWPA->wPKCount = 1;
 		sFrame.pRSNWPA->PKSList[0].abyOUI[0] = 0x00;
 		sFrame.pRSNWPA->PKSList[0].abyOUI[1] = 0x50;
 		sFrame.pRSNWPA->PKSList[0].abyOUI[2] = 0xf2;
-		if (pMgmt->byCSSPK == KEY_CTL_TKIP) {
+		if (pMgmt->byCSSPK == KEY_CTL_TKIP)
 			sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_TKIP;
-		} else if (pMgmt->byCSSPK == KEY_CTL_CCMP) {
+		else if (pMgmt->byCSSPK == KEY_CTL_CCMP)
 			sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_AESCCMP;
-		} else {
+		else
 			sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_NONE;
-		}
+
 		// Auth Key Management Suite
 		pbyRSN = (unsigned char *)(sFrame.pBuf + sFrame.len + 2 + sFrame.pRSNWPA->len);
 		*pbyRSN++ = 0x01;
@@ -3814,13 +3703,12 @@ s_MgrMakeReAssocRequest(
 
 		*pbyRSN++ = 0x50;
 		*pbyRSN++ = 0xf2;
-		if (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) {
+		if (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK)
 			*pbyRSN++ = WPA_AUTH_PSK;
-		} else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA) {
+		else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA)
 			*pbyRSN++ = WPA_AUTH_IEEE802_1X;
-		} else {
+		else
 			*pbyRSN++ = WPA_NONE;
-		}
 
 		sFrame.pRSNWPA->len += 6;
 
@@ -3850,15 +3738,14 @@ s_MgrMakeReAssocRequest(
 		sFrame.pRSN->abyRSN[0] = 0x00;
 		sFrame.pRSN->abyRSN[1] = 0x0F;
 		sFrame.pRSN->abyRSN[2] = 0xAC;
-		if (pMgmt->byCSSGK == KEY_CTL_WEP) {
+		if (pMgmt->byCSSGK == KEY_CTL_WEP)
 			sFrame.pRSN->abyRSN[3] = pMgmt->pCurrBSS->byCSSGK;
-		} else if (pMgmt->byCSSGK == KEY_CTL_TKIP) {
+		else if (pMgmt->byCSSGK == KEY_CTL_TKIP)
 			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_TKIP;
-		} else if (pMgmt->byCSSGK == KEY_CTL_CCMP) {
+		else if (pMgmt->byCSSGK == KEY_CTL_CCMP)
 			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_CCMP;
-		} else {
+		else
 			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_UNKNOWN;
-		}
 
 		// Pairwise Key Cipher Suite
 		sFrame.pRSN->abyRSN[4] = 1;
@@ -3866,15 +3753,15 @@ s_MgrMakeReAssocRequest(
 		sFrame.pRSN->abyRSN[6] = 0x00;
 		sFrame.pRSN->abyRSN[7] = 0x0F;
 		sFrame.pRSN->abyRSN[8] = 0xAC;
-		if (pMgmt->byCSSPK == KEY_CTL_TKIP) {
+		if (pMgmt->byCSSPK == KEY_CTL_TKIP)
 			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_TKIP;
-		} else if (pMgmt->byCSSPK == KEY_CTL_CCMP) {
+		else if (pMgmt->byCSSPK == KEY_CTL_CCMP)
 			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_CCMP;
-		} else if (pMgmt->byCSSPK == KEY_CTL_NONE) {
+		else if (pMgmt->byCSSPK == KEY_CTL_NONE)
 			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_USE_GROUP;
-		} else {
+		else
 			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_UNKNOWN;
-		}
+
 		sFrame.pRSN->len += 6;
 
 		// Auth Key Management Suite
@@ -3883,13 +3770,13 @@ s_MgrMakeReAssocRequest(
 		sFrame.pRSN->abyRSN[12] = 0x00;
 		sFrame.pRSN->abyRSN[13] = 0x0F;
 		sFrame.pRSN->abyRSN[14] = 0xAC;
-		if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK) {
+		if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)
 			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_PSK;
-		} else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) {
+		else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2)
 			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_802_1X;
-		} else {
+		else
 			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_UNKNOWN;
-		}
+
 		sFrame.pRSN->len += 6;
 
 		// RSN Capabilities
@@ -3914,9 +3801,9 @@ s_MgrMakeReAssocRequest(
 					pbyRSN += 16;
 				}
 			}
-			if (*pwPMKID != 0) {
+
+			if (*pwPMKID != 0)
 				sFrame.pRSN->len += (2 + (*pwPMKID) * 16);
-			}
 		}
 
 		sFrame.len += sFrame.pRSN->len + WLAN_IEHDR_LEN;
@@ -4228,10 +4115,7 @@ s_vMgrRxProbeRequest(
 		sFrame.len = pRxPacket->cbMPDULen;
 		sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
 		vMgrDecodeProbeRequest(&sFrame);
-/*
-  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Probe request rx:MAC addr:%pM\n",
-  sFrame.pHdr->sA3.abyAddr2);
-*/
+
 		if (sFrame.pSSID->len != 0) {
 			if (sFrame.pSSID->len != ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len)
 				return;
@@ -4242,9 +4126,8 @@ s_vMgrRxProbeRequest(
 			}
 		}
 
-		if ((sFrame.pSuppRates->len > 4) || (sFrame.pExtSuppRates != NULL)) {
+		if ((sFrame.pSuppRates->len > 4) || (sFrame.pExtSuppRates != NULL))
 			byPHYType = BB_TYPE_11G;
-		}
 
 		// Probe response reply..
 		pTxPacket = s_MgrMakeProbeResponse
@@ -4267,8 +4150,6 @@ s_vMgrRxProbeRequest(
 			Status = csMgmt_xmit(pDevice, pTxPacket);
 			if (Status != CMD_STATUS_PENDING) {
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Probe response tx failed\n");
-			} else {
-//                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Probe response tx sending..\n");
 			}
 		}
 	}
@@ -4361,7 +4242,6 @@ vMgrRxManagePacket(
 
 	case WLAN_FSTYPE_PROBEREQ:
 		// Frame Clase = 0
-		//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx probereq\n");
 		s_vMgrRxProbeRequest(pDevice, pMgmt, pRxPacket);
 		break;
 
@@ -4374,10 +4254,9 @@ vMgrRxManagePacket(
 
 	case WLAN_FSTYPE_BEACON:
 		// Frame Clase = 0
-		//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx beacon\n");
-		if (pMgmt->eScanState != WMAC_NO_SCANNING) {
+		if (pMgmt->eScanState != WMAC_NO_SCANNING)
 			bInScan = true;
-		}
+
 		s_vMgrRxBeacon(pDevice, pMgmt, pRxPacket, bInScan);
 		break;
 
@@ -4442,12 +4321,11 @@ bMgrPrepareBeaconToSend(
 	PSDevice            pDevice = (PSDevice)hDeviceContext;
 	PSTxMgmtPacket      pTxPacket;
 
-//    pDevice->bBeaconBufReady = false;
-	if (pDevice->bEncryptionEnable || pDevice->bEnable8021x) {
+	if (pDevice->bEncryptionEnable || pDevice->bEnable8021x)
 		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_PRIVACY(1);
-	} else {
+	else
 		pMgmt->wCurrCapInfo &= ~WLAN_SET_CAP_INFO_PRIVACY(1);
-	}
+
 	pTxPacket = s_MgrMakeBeacon
 		(
 			pDevice,
@@ -4455,7 +4333,7 @@ bMgrPrepareBeaconToSend(
 			pMgmt->wCurrCapInfo,
 			pMgmt->wCurrBeaconPeriod,
 			pMgmt->uCurrChannel,
-			pMgmt->wCurrATIMWindow, //0,
+			pMgmt->wCurrATIMWindow,
 			(PWLAN_IE_SSID)pMgmt->abyCurrSSID,
 			(unsigned char *)pMgmt->abyCurrBSSID,
 			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
@@ -4575,22 +4453,22 @@ bAdd_PMKID_Candidate(
 	for (ii = 0; ii < pDevice->gsPMKIDCandidate.NumCandidates; ii++) {
 		pCandidateList = &pDevice->gsPMKIDCandidate.CandidateList[ii];
 		if (!memcmp(pCandidateList->BSSID, pbyBSSID, ETH_ALEN)) {
-			if (psRSNCapObj->bRSNCapExist && (psRSNCapObj->wRSNCap & BIT0)) {
+			if (psRSNCapObj->bRSNCapExist && (psRSNCapObj->wRSNCap & BIT0))
 				pCandidateList->Flags |= NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED;
-			} else {
+			else
 				pCandidateList->Flags &= ~(NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED);
-			}
+
 			return true;
 		}
 	}
 
 	// New Candidate
 	pCandidateList = &pDevice->gsPMKIDCandidate.CandidateList[pDevice->gsPMKIDCandidate.NumCandidates];
-	if (psRSNCapObj->bRSNCapExist && (psRSNCapObj->wRSNCap & BIT0)) {
+	if (psRSNCapObj->bRSNCapExist && (psRSNCapObj->wRSNCap & BIT0))
 		pCandidateList->Flags |= NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED;
-	} else {
+	else
 		pCandidateList->Flags &= ~(NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED);
-	}
+
 	memcpy(pCandidateList->BSSID, pbyBSSID, ETH_ALEN);
 	pDevice->gsPMKIDCandidate.NumCandidates++;
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "NumCandidates:%d\n", (int)pDevice->gsPMKIDCandidate.NumCandidates);
diff --git a/drivers/staging/vt6655/wmgr.h b/drivers/staging/vt6655/wmgr.h
index b91f1f89ed0..2312d71bac4 100644
--- a/drivers/staging/vt6655/wmgr.h
+++ b/drivers/staging/vt6655/wmgr.h
@@ -119,32 +119,6 @@ typedef struct tagSAssocInfo {
 } SAssocInfo, *PSAssocInfo;
 //---
 
-/*
-  typedef enum tagWMAC_AUTHENTICATION_MODE {
-  WMAC_AUTH_OPEN,
-  WMAC_AUTH_SHAREKEY,
-  WMAC_AUTH_AUTO,
-  WMAC_AUTH_WPA,
-  WMAC_AUTH_WPAPSK,
-  WMAC_AUTH_WPANONE,
-  WMAC_AUTH_WPA2,
-  WMAC_AUTH_WPA2PSK,
-  WMAC_AUTH_MAX       // Not a real mode, defined as upper bound
-
-  } WMAC_AUTHENTICATION_MODE, *PWMAC_AUTHENTICATION_MODE;
-*/
-
-// Pre-configured Mode (from XP)
-/*
-  typedef enum tagWMAC_CONFIG_MODE {
-  WMAC_CONFIG_ESS_STA,
-  WMAC_CONFIG_IBSS_STA,
-  WMAC_CONFIG_AUTO,
-  WMAC_CONFIG_AP
-
-  } WMAC_CONFIG_MODE, *PWMAC_CONFIG_MODE;
-*/
-
 typedef enum tagWMAC_SCAN_TYPE {
 	WMAC_SCAN_ACTIVE,
 	WMAC_SCAN_PASSIVE,
@@ -232,9 +206,6 @@ typedef struct tagSMgmtObject
 	unsigned char byCSSGK;
 	unsigned char byCSSPK;
 
-//    unsigned char abyNewSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN];
-//    unsigned char abyNewExtSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN];
-
 	// Current state vars
 	unsigned int	uCurrChannel;
 	unsigned char abyCurrSuppRates[WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1];
@@ -256,7 +227,6 @@ typedef struct tagSMgmtObject
 	unsigned char abyDesireBSSID[WLAN_BSSID_LEN];
 
 	// Adhoc or AP configuration vars
-	//unsigned char abyAdHocSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
 	unsigned short wIBSSBeaconPeriod;
 	unsigned short wIBSSATIMWindow;
 	unsigned int	uIBSSChannel;
diff --git a/drivers/staging/vt6655/wpa.c b/drivers/staging/vt6655/wpa.c
index 990ea0f9e9f..9be59c23d7e 100644
--- a/drivers/staging/vt6655/wpa.c
+++ b/drivers/staging/vt6655/wpa.c
@@ -160,8 +160,7 @@ WPA_ParseRSN(
 						;
 				} else
 					break;
-				//DBG_PRN_GRP14(("abyPKType[%d]: %X\n", j-1, pBSSList->abyPKType[j-1]));
-			} //for
+			}
 			pBSSList->wPKCount = (unsigned short)j;
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wPKCount: %d\n", pBSSList->wPKCount);
 		}
@@ -187,7 +186,7 @@ WPA_ParseRSN(
 						;
 				} else
 					break;
-				//DBG_PRN_GRP14(("abyAuthType[%d]: %X\n", j-1, pBSSList->abyAuthType[j-1]));
+
 			}
 			if (j > 0)
 				pBSSList->wAuthCount = (unsigned short)j;
@@ -206,9 +205,6 @@ WPA_ParseRSN(
 				pBSSList->byReplayIdx = 2 << ((*pbyCaps >> WPA_REPLAYBITSSHIFT) & WPA_REPLAYBITS);
 				pBSSList->sRSNCapObj.bRSNCapExist = true;
 				pBSSList->sRSNCapObj.wRSNCap = *(unsigned short *)pbyCaps;
-				//DBG_PRN_GRP14(("pbyCaps: %X\n", *pbyCaps));
-				//DBG_PRN_GRP14(("byDefaultK_as_PK: %X\n", pBSSList->byDefaultK_as_PK));
-				//DBG_PRN_GRP14(("byReplayIdx: %X\n", pBSSList->byReplayIdx));
 			}
 		}
 		pBSSList->bWPAValid = true;
diff --git a/drivers/staging/vt6655/wpa2.c b/drivers/staging/vt6655/wpa2.c
index 2013122e92b..4e1b63be380 100644
--- a/drivers/staging/vt6655/wpa2.c
+++ b/drivers/staging/vt6655/wpa2.c
@@ -37,7 +37,6 @@
 
 /*---------------------  Static Definitions -------------------------*/
 static int msglevel = MSG_LEVEL_INFO;
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
@@ -122,9 +121,9 @@ WPA2vParseRSN(
 	WPA2_ClearRSN(pBSSNode);
 
 	if (pRSN->len == 2) { // ver(2)
-		if ((pRSN->byElementID == WLAN_EID_RSN) && (pRSN->wVersion == 1)) {
+		if ((pRSN->byElementID == WLAN_EID_RSN) && (pRSN->wVersion == 1))
 			pBSSNode->bWPA2Valid = true;
-		}
+
 		return;
 	}
 
@@ -268,9 +267,9 @@ WPA2uSetIEs(
 	unsigned int ii = 0;
 	unsigned short *pwPMKID = NULL;
 
-	if (pRSNIEs == NULL) {
+	if (pRSNIEs == NULL)
 		return 0;
-	}
+
 	if (((pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
 	     (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) &&
 	    (pMgmt->pCurrBSS != NULL)) {
@@ -283,15 +282,14 @@ WPA2uSetIEs(
 		pRSNIEs->abyRSN[0] = 0x00;
 		pRSNIEs->abyRSN[1] = 0x0F;
 		pRSNIEs->abyRSN[2] = 0xAC;
-		if (pMgmt->byCSSGK == KEY_CTL_WEP) {
+		if (pMgmt->byCSSGK == KEY_CTL_WEP)
 			pRSNIEs->abyRSN[3] = pMgmt->pCurrBSS->byCSSGK;
-		} else if (pMgmt->byCSSGK == KEY_CTL_TKIP) {
+		else if (pMgmt->byCSSGK == KEY_CTL_TKIP)
 			pRSNIEs->abyRSN[3] = WLAN_11i_CSS_TKIP;
-		} else if (pMgmt->byCSSGK == KEY_CTL_CCMP) {
+		else if (pMgmt->byCSSGK == KEY_CTL_CCMP)
 			pRSNIEs->abyRSN[3] = WLAN_11i_CSS_CCMP;
-		} else {
+		else
 			pRSNIEs->abyRSN[3] = WLAN_11i_CSS_UNKNOWN;
-		}
 
 		// Pairwise Key Cipher Suite
 		pRSNIEs->abyRSN[4] = 1;
@@ -299,15 +297,15 @@ WPA2uSetIEs(
 		pRSNIEs->abyRSN[6] = 0x00;
 		pRSNIEs->abyRSN[7] = 0x0F;
 		pRSNIEs->abyRSN[8] = 0xAC;
-		if (pMgmt->byCSSPK == KEY_CTL_TKIP) {
+		if (pMgmt->byCSSPK == KEY_CTL_TKIP)
 			pRSNIEs->abyRSN[9] = WLAN_11i_CSS_TKIP;
-		} else if (pMgmt->byCSSPK == KEY_CTL_CCMP) {
+		else if (pMgmt->byCSSPK == KEY_CTL_CCMP)
 			pRSNIEs->abyRSN[9] = WLAN_11i_CSS_CCMP;
-		} else if (pMgmt->byCSSPK == KEY_CTL_NONE) {
+		else if (pMgmt->byCSSPK == KEY_CTL_NONE)
 			pRSNIEs->abyRSN[9] = WLAN_11i_CSS_USE_GROUP;
-		} else {
+		else
 			pRSNIEs->abyRSN[9] = WLAN_11i_CSS_UNKNOWN;
-		}
+
 		pRSNIEs->len += 6;
 
 		// Auth Key Management Suite
@@ -316,13 +314,13 @@ WPA2uSetIEs(
 		pRSNIEs->abyRSN[12] = 0x00;
 		pRSNIEs->abyRSN[13] = 0x0F;
 		pRSNIEs->abyRSN[14] = 0xAC;
-		if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK) {
+		if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)
 			pRSNIEs->abyRSN[15] = WLAN_11i_AKMSS_PSK;
-		} else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) {
+		else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2)
 			pRSNIEs->abyRSN[15] = WLAN_11i_AKMSS_802_1X;
-		} else {
+		else
 			pRSNIEs->abyRSN[15] = WLAN_11i_AKMSS_UNKNOWN;
-		}
+
 		pRSNIEs->len += 6;
 
 		// RSN Capabilities
@@ -348,11 +346,10 @@ WPA2uSetIEs(
 					pbyBuffer += 16;
 				}
 			}
-			if (*pwPMKID != 0) {
+			if (*pwPMKID != 0)
 				pRSNIEs->len += (2 + (*pwPMKID)*16);
-			} else {
+			else
 				pbyBuffer = &pRSNIEs->abyRSN[18];
-			}
 		}
 		return pRSNIEs->len + WLAN_IEHDR_LEN;
 	}
diff --git a/drivers/staging/vt6655/wpactl.c b/drivers/staging/vt6655/wpactl.c
index d17224f39b4..8392d4d1d5e 100644
--- a/drivers/staging/vt6655/wpactl.c
+++ b/drivers/staging/vt6655/wpactl.c
@@ -51,7 +51,6 @@ static const int frequency_list[] = {
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 static int msglevel = MSG_LEVEL_INFO;
 
 /*---------------------  Static Functions  --------------------------*/
@@ -188,7 +187,8 @@ int wpa_set_wpadev(PSDevice pDevice, int val)
  *
  */
 
-int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
+int wpa_set_keys(PSDevice pDevice, void *ctx,
+		 bool fcpfkernel) __must_hold(&pDevice->lock)
 {
 	struct viawget_wpa_param *param = ctx;
 	PSMgmtObject pMgmt = pDevice->pMgmt;
@@ -196,7 +196,6 @@ int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
 	unsigned char abyKey[MAX_KEY_LEN];
 	unsigned char abySeq[MAX_KEY_LEN];
 	QWORD   KeyRSC;
-//    NDIS_802_11_KEY_RSC KeyRSC;
 	unsigned char byKeyDecMode = KEY_CTL_WEP;
 	int ret = 0;
 	int uu, ii;
@@ -213,13 +212,12 @@ int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
 		pDevice->byKeyIndex = 0;
 		pDevice->bTransmitKey = false;
 		KeyvRemoveAllWEPKey(&(pDevice->sKey), pDevice->PortOffset);
-		for (uu = 0; uu < MAX_KEY_TABLE; uu++) {
+		for (uu = 0; uu < MAX_KEY_TABLE; uu++)
 			MACvDisableKeyEntry(pDevice->PortOffset, uu);
-		}
+
 		return ret;
 	}
 
-	//spin_unlock_irq(&pDevice->lock);
 	if (param->u.wpa_key.key && fcpfkernel) {
 		memcpy(&abyKey[0], param->u.wpa_key.key, param->u.wpa_key.key_len);
 	} else {
@@ -258,7 +256,6 @@ int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
 		return ret;
 	}
 
-	//spin_unlock_irq(&pDevice->lock);
 	if (param->u.wpa_key.seq && fcpfkernel) {
 		memcpy(&abySeq[0], param->u.wpa_key.seq, param->u.wpa_key.seq_len);
 	} else {
@@ -277,7 +274,6 @@ int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
 				LODWORD(KeyRSC) |= (abySeq[ii] << (ii * 8));
 			else
 				HIDWORD(KeyRSC) |= (abySeq[ii] << ((ii-4) * 8));
-			//KeyRSC |= (abySeq[ii] << (ii * 8));
 		}
 		dwKeyIndex |= 1 << 29;
 	}
@@ -287,13 +283,11 @@ int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
 		return -EINVAL;
 	}
 
-	if (param->u.wpa_key.alg_name == WPA_ALG_TKIP) {
+	if (param->u.wpa_key.alg_name == WPA_ALG_TKIP)
 		pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;
-	}
 
-	if (param->u.wpa_key.alg_name == WPA_ALG_CCMP) {
+	if (param->u.wpa_key.alg_name == WPA_ALG_CCMP)
 		pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;
-	}
 
 	if (param->u.wpa_key.set_tx)
 		dwKeyIndex |= (1 << 31);
@@ -324,7 +318,6 @@ int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
 	if ((byKeyDecMode == KEY_CTL_TKIP) &&
 	    (param->u.wpa_key.key_len != MAX_KEY_LEN)) {
 		// TKIP Key must be 256 bits
-		//DBG_PRN_WLAN03(("return NDIS_STATUS_INVALID_DATA - TKIP Key must be 256 bits\n"));
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "return- TKIP Key must be 256 bits!\n");
 		return -EINVAL;
 	}
@@ -332,7 +325,6 @@ int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
 	if ((byKeyDecMode == KEY_CTL_CCMP) &&
 	    (param->u.wpa_key.key_len != AES_KEY_LEN)) {
 		// AES Key must be 128 bits
-		//DBG_PRN_WLAN03(("return NDIS_STATUS_INVALID_DATA - AES Key must be 128 bits\n"));
 		return -EINVAL;
 	}
 
@@ -360,8 +352,6 @@ int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "GROUP Key Assign.\n");
 
 		} else {
-			//DBG_PRN_WLAN03(("return NDIS_STATUS_INVALID_DATA -KeybSetDefaultKey Fail.0\n"));
-			// spin_unlock_irq(&pDevice->lock);
 			return -EINVAL;
 		}
 
@@ -371,16 +361,13 @@ int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
 		// 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");
-			//spin_unlock_irq(&pDevice->lock);
 			return -EINVAL;
 		}
 
 		dwKeyIndex |= (1 << 30); // set pairwise key
-		if (pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA) {
-			//DBG_PRN_WLAN03(("return NDIS_STATUS_INVALID_DATA - WMAC_CONFIG_IBSS_STA\n"));
-			//spin_unlock_irq(&pDevice->lock);
+		if (pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA)
 			return -EINVAL;
-		}
+
 		if (KeybSetKey(&(pDevice->sKey),
 			       &param->addr[0],
 			       dwKeyIndex,
@@ -394,17 +381,7 @@ int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
 
 		} else {
 			// Key Table Full
-			if (ether_addr_equal(param->addr, pDevice->abyBSSID)) {
-				//DBG_PRN_WLAN03(("return NDIS_STATUS_INVALID_DATA -Key Table Full.2\n"));
-				//spin_unlock_irq(&pDevice->lock);
-				return -EINVAL;
-
-			} else {
-				// Save Key and configure just before associate/reassociate to BSSID
-				// we do not implement now
-				//spin_unlock_irq(&pDevice->lock);
-				return -EINVAL;
-			}
+			return -EINVAL;
 		}
 	} // BSSID not 0xffffffffffff
 	if ((ret == 0) && ((param->u.wpa_key.set_tx) != 0)) {
@@ -412,17 +389,6 @@ int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
 		pDevice->bTransmitKey = true;
 	}
 	pDevice->bEncryptionEnable = true;
-	//spin_unlock_irq(&pDevice->lock);
-
-/*
-  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " key=%x-%x-%x-%x-%x-xxxxx\n",
-  pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[byKeyIndex][0],
-  pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[byKeyIndex][1],
-  pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[byKeyIndex][2],
-  pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[byKeyIndex][3],
-  pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[byKeyIndex][4]
-);
-*/
 
 	return ret;
 }
@@ -445,12 +411,11 @@ static int wpa_set_wpa(PSDevice pDevice,
 		       struct viawget_wpa_param *param)
 {
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	int ret = 0;
 
 	pMgmt->eAuthenMode = WMAC_AUTH_OPEN;
 	pMgmt->bShareKeyAlgorithm = false;
 
-	return ret;
+	return 0;
 }
 
 /*
@@ -471,7 +436,6 @@ static int wpa_set_disassociate(PSDevice pDevice,
 				struct viawget_wpa_param *param)
 {
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	int ret = 0;
 
 	spin_lock_irq(&pDevice->lock);
 	if (pDevice->bLinkPass) {
@@ -480,7 +444,7 @@ static int wpa_set_disassociate(PSDevice pDevice,
 	}
 	spin_unlock_irq(&pDevice->lock);
 
-	return ret;
+	return 0;
 }
 
 /*
@@ -500,14 +464,12 @@ static int wpa_set_disassociate(PSDevice pDevice,
 static int wpa_set_scan(PSDevice pDevice,
 			struct viawget_wpa_param *param)
 {
-	int ret = 0;
-
 	spin_lock_irq(&pDevice->lock);
 	BSSvClearBSSList((void *)pDevice, pDevice->bLinkPass);
 	bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, NULL);
 	spin_unlock_irq(&pDevice->lock);
 
-	return ret;
+	return 0;
 }
 
 /*
@@ -528,11 +490,10 @@ static int wpa_get_bssid(PSDevice pDevice,
 			 struct viawget_wpa_param *param)
 {
 	PSMgmtObject        pMgmt = pDevice->pMgmt;
-	int ret = 0;
 
 	memcpy(param->u.wpa_associate.bssid, pMgmt->abyCurrBSSID , 6);
 
-	return ret;
+	return 0;
 }
 
 /*
@@ -554,14 +515,13 @@ static int wpa_get_ssid(PSDevice pDevice,
 {
 	PSMgmtObject        pMgmt = pDevice->pMgmt;
 	PWLAN_IE_SSID       pItemSSID;
-	int ret = 0;
 
 	pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
 
 	memcpy(param->u.wpa_associate.ssid, pItemSSID->abySSID , pItemSSID->len);
 	param->u.wpa_associate.ssid_len = pItemSSID->len;
 
-	return ret;
+	return 0;
 }
 
 /*
@@ -593,7 +553,7 @@ static int wpa_get_scan(PSDevice pDevice,
 
 	unsigned char *ptempBSS;
 
-	ptempBSS = kmalloc(sizeof(KnownBSS), (int)GFP_ATOMIC);
+	ptempBSS = kmalloc(sizeof(KnownBSS), GFP_ATOMIC);
 
 	if (ptempBSS == NULL) {
 		printk(KERN_ERR "bubble sort kmalloc memory fail@@@\n");
@@ -635,7 +595,7 @@ static int wpa_get_scan(PSDevice pDevice,
 		count++;
 	}
 
-	pBuf = kcalloc(count, sizeof(struct viawget_scan_result), (int)GFP_ATOMIC);
+	pBuf = kcalloc(count, sizeof(struct viawget_scan_result), GFP_ATOMIC);
 
 	if (pBuf == NULL) {
 		ret = -ENOMEM;
@@ -654,11 +614,7 @@ static int wpa_get_scan(PSDevice pDevice,
 			scan_buf->ssid_len = pItemSSID->len;
 			scan_buf->freq = frequency_list[pBSS->uChannel-1];
 			scan_buf->caps = pBSS->wCapInfo;
-			//scan_buf->caps = pBSS->wCapInfo;
-			//scan_buf->qual =
-			//scan_buf->noise =
-			//scan_buf->level =
-			//scan_buf->maxrate =
+
 			if (pBSS->wWPALen != 0) {
 				scan_buf->wpa_ie_len = pBSS->wWPALen;
 				memcpy(scan_buf->wpa_ie, pBSS->byWPAIE, pBSS->wWPALen);
@@ -675,11 +631,11 @@ static int wpa_get_scan(PSDevice pDevice,
 	if (jj < count)
 		count = jj;
 
-	if (copy_to_user(param->u.scan_results.buf, pBuf, sizeof(struct viawget_scan_result) * count)) {
+	if (copy_to_user(param->u.scan_results.buf, pBuf, sizeof(struct viawget_scan_result) * count))
 		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)
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " param->u.scan_results.scan_count = %d\n", count);
 
 		kfree(pBuf);
 	return ret;
@@ -706,7 +662,6 @@ static int wpa_set_associate(PSDevice pDevice,
 	PWLAN_IE_SSID   pItemSSID;
 	unsigned char abyNullAddr[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
 	unsigned char abyWPAIE[64];
-	int ret = 0;
 	bool bWepEnabled = false;
 
 	// set key type & algorithm
@@ -739,9 +694,8 @@ static int wpa_set_associate(PSDevice pDevice,
 	// set bssid
 	if (memcmp(param->u.wpa_associate.bssid, &abyNullAddr[0], 6) != 0)
 		memcpy(pMgmt->abyDesireBSSID, param->u.wpa_associate.bssid, 6);
-	else {
+	else
 		bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, pItemSSID->abySSID);
-	}
 
 	if (param->u.wpa_associate.wpa_ie_len == 0) {
 		if (param->u.wpa_associate.auth_alg & AUTH_ALG_SHARED_KEY)
@@ -788,13 +742,10 @@ static int wpa_set_associate(PSDevice pDevice,
 
 	if (pMgmt->eAuthenMode == WMAC_AUTH_SHAREKEY) {
 		pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-		//pMgmt->eAuthenMode = WMAC_AUTH_SHAREKEY;
 		pMgmt->bShareKeyAlgorithm = true;
 	} else if (pMgmt->eAuthenMode == WMAC_AUTH_OPEN) {
 		if (!bWepEnabled)  pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
 		else pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-		//pMgmt->eAuthenMode = WMAC_AUTH_OPEN;
-		//pMgmt->bShareKeyAlgorithm = false; //20080717-06,<Modify> by chester//Fix Open mode, WEP encryption
 	}
 //mike save old encryption status
 	pDevice->eOldEncryptionStatus = pDevice->eEncryptionStatus;
@@ -830,7 +781,7 @@ static int wpa_set_associate(PSDevice pDevice,
 	bScheduleCommand((void *)pDevice, WLAN_CMD_SSID, NULL);
 	spin_unlock_irq(&pDevice->lock);
 
-	return ret;
+	return 0;
 }
 
 /*
@@ -857,7 +808,7 @@ int wpa_ioctl(PSDevice pDevice, struct iw_point *p)
 	    p->length > VIAWGET_WPA_MAX_BUF_SIZE || !p->pointer)
 		return -EINVAL;
 
-	param = kmalloc((int)p->length, (int)GFP_KERNEL);
+	param = kmalloc((int)p->length, GFP_KERNEL);
 	if (param == NULL)
 		return -ENOMEM;
 
diff --git a/drivers/staging/vt6655/wroute.c b/drivers/staging/vt6655/wroute.c
index c39d5ed59de..4da3fef139d 100644
--- a/drivers/staging/vt6655/wroute.c
+++ b/drivers/staging/vt6655/wroute.c
@@ -44,7 +44,6 @@
 
 /*---------------------  Static Variables  --------------------------*/
 static int msglevel = MSG_LEVEL_INFO;
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 /*---------------------  Static Functions  --------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
diff --git a/drivers/staging/vt6656/Makefile b/drivers/staging/vt6656/Makefile
index 1d829b46c2e..b5ec483f3eb 100644
--- a/drivers/staging/vt6656/Makefile
+++ b/drivers/staging/vt6656/Makefile
@@ -18,8 +18,6 @@ vt6656_stage-y +=	main_usb.o \
 			datarate.o \
 			rc4.o \
 			tether.o \
-			tcrc.o \
-			hostap.o \
 			wpa.o \
 			key.o \
 			tkip.o \
@@ -27,10 +25,8 @@ vt6656_stage-y +=	main_usb.o \
 			rf.o \
 			iwctl.o \
 			wpactl.o \
-			aes_ccmp.o \
 			usbpipe.o \
 			channel.o \
-			control.o \
 			firmware.o \
 			int.o
 
diff --git a/drivers/staging/vt6656/aes_ccmp.c b/drivers/staging/vt6656/aes_ccmp.c
deleted file mode 100644
index e2bfa8d266c..00000000000
--- a/drivers/staging/vt6656/aes_ccmp.c
+++ /dev/null
@@ -1,373 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: aes_ccmp.c
- *
- * Purpose: AES_CCMP decryption
- *
- * Author: Warren Hsu
- *
- * Date: Feb 15, 2005
- *
- * Functions:
- *      AESbGenCCMP - Parsing RX-packet
- *
- * Revision History:
- */
-
-#include "device.h"
-#include "80211hdr.h"
-
-/*
- * SBOX Table
- */
-
-static u8 sbox_table[256] = {
-	0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5, 0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
-	0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0, 0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
-	0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc, 0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
-	0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a, 0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
-	0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0, 0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
-	0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b, 0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
-	0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85, 0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8,
-	0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5, 0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
-	0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17, 0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
-	0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88, 0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
-	0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c, 0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79,
-	0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9, 0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
-	0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6, 0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a,
-	0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e, 0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
-	0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94, 0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
-	0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68, 0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
-};
-
-static u8 dot2_table[256] = {
-	0x00, 0x02, 0x04, 0x06, 0x08, 0x0a, 0x0c, 0x0e, 0x10, 0x12, 0x14, 0x16, 0x18, 0x1a, 0x1c, 0x1e,
-	0x20, 0x22, 0x24, 0x26, 0x28, 0x2a, 0x2c, 0x2e, 0x30, 0x32, 0x34, 0x36, 0x38, 0x3a, 0x3c, 0x3e,
-	0x40, 0x42, 0x44, 0x46, 0x48, 0x4a, 0x4c, 0x4e, 0x50, 0x52, 0x54, 0x56, 0x58, 0x5a, 0x5c, 0x5e,
-	0x60, 0x62, 0x64, 0x66, 0x68, 0x6a, 0x6c, 0x6e, 0x70, 0x72, 0x74, 0x76, 0x78, 0x7a, 0x7c, 0x7e,
-	0x80, 0x82, 0x84, 0x86, 0x88, 0x8a, 0x8c, 0x8e, 0x90, 0x92, 0x94, 0x96, 0x98, 0x9a, 0x9c, 0x9e,
-	0xa0, 0xa2, 0xa4, 0xa6, 0xa8, 0xaa, 0xac, 0xae, 0xb0, 0xb2, 0xb4, 0xb6, 0xb8, 0xba, 0xbc, 0xbe,
-	0xc0, 0xc2, 0xc4, 0xc6, 0xc8, 0xca, 0xcc, 0xce, 0xd0, 0xd2, 0xd4, 0xd6, 0xd8, 0xda, 0xdc, 0xde,
-	0xe0, 0xe2, 0xe4, 0xe6, 0xe8, 0xea, 0xec, 0xee, 0xf0, 0xf2, 0xf4, 0xf6, 0xf8, 0xfa, 0xfc, 0xfe,
-	0x1b, 0x19, 0x1f, 0x1d, 0x13, 0x11, 0x17, 0x15, 0x0b, 0x09, 0x0f, 0x0d, 0x03, 0x01, 0x07, 0x05,
-	0x3b, 0x39, 0x3f, 0x3d, 0x33, 0x31, 0x37, 0x35, 0x2b, 0x29, 0x2f, 0x2d, 0x23, 0x21, 0x27, 0x25,
-	0x5b, 0x59, 0x5f, 0x5d, 0x53, 0x51, 0x57, 0x55, 0x4b, 0x49, 0x4f, 0x4d, 0x43, 0x41, 0x47, 0x45,
-	0x7b, 0x79, 0x7f, 0x7d, 0x73, 0x71, 0x77, 0x75, 0x6b, 0x69, 0x6f, 0x6d, 0x63, 0x61, 0x67, 0x65,
-	0x9b, 0x99, 0x9f, 0x9d, 0x93, 0x91, 0x97, 0x95, 0x8b, 0x89, 0x8f, 0x8d, 0x83, 0x81, 0x87, 0x85,
-	0xbb, 0xb9, 0xbf, 0xbd, 0xb3, 0xb1, 0xb7, 0xb5, 0xab, 0xa9, 0xaf, 0xad, 0xa3, 0xa1, 0xa7, 0xa5,
-	0xdb, 0xd9, 0xdf, 0xdd, 0xd3, 0xd1, 0xd7, 0xd5, 0xcb, 0xc9, 0xcf, 0xcd, 0xc3, 0xc1, 0xc7, 0xc5,
-	0xfb, 0xf9, 0xff, 0xfd, 0xf3, 0xf1, 0xf7, 0xf5, 0xeb, 0xe9, 0xef, 0xed, 0xe3, 0xe1, 0xe7, 0xe5
-};
-
-static u8 dot3_table[256] = {
-	0x00, 0x03, 0x06, 0x05, 0x0c, 0x0f, 0x0a, 0x09, 0x18, 0x1b, 0x1e, 0x1d, 0x14, 0x17, 0x12, 0x11,
-	0x30, 0x33, 0x36, 0x35, 0x3c, 0x3f, 0x3a, 0x39, 0x28, 0x2b, 0x2e, 0x2d, 0x24, 0x27, 0x22, 0x21,
-	0x60, 0x63, 0x66, 0x65, 0x6c, 0x6f, 0x6a, 0x69, 0x78, 0x7b, 0x7e, 0x7d, 0x74, 0x77, 0x72, 0x71,
-	0x50, 0x53, 0x56, 0x55, 0x5c, 0x5f, 0x5a, 0x59, 0x48, 0x4b, 0x4e, 0x4d, 0x44, 0x47, 0x42, 0x41,
-	0xc0, 0xc3, 0xc6, 0xc5, 0xcc, 0xcf, 0xca, 0xc9, 0xd8, 0xdb, 0xde, 0xdd, 0xd4, 0xd7, 0xd2, 0xd1,
-	0xf0, 0xf3, 0xf6, 0xf5, 0xfc, 0xff, 0xfa, 0xf9, 0xe8, 0xeb, 0xee, 0xed, 0xe4, 0xe7, 0xe2, 0xe1,
-	0xa0, 0xa3, 0xa6, 0xa5, 0xac, 0xaf, 0xaa, 0xa9, 0xb8, 0xbb, 0xbe, 0xbd, 0xb4, 0xb7, 0xb2, 0xb1,
-	0x90, 0x93, 0x96, 0x95, 0x9c, 0x9f, 0x9a, 0x99, 0x88, 0x8b, 0x8e, 0x8d, 0x84, 0x87, 0x82, 0x81,
-	0x9b, 0x98, 0x9d, 0x9e, 0x97, 0x94, 0x91, 0x92, 0x83, 0x80, 0x85, 0x86, 0x8f, 0x8c, 0x89, 0x8a,
-	0xab, 0xa8, 0xad, 0xae, 0xa7, 0xa4, 0xa1, 0xa2, 0xb3, 0xb0, 0xb5, 0xb6, 0xbf, 0xbc, 0xb9, 0xba,
-	0xfb, 0xf8, 0xfd, 0xfe, 0xf7, 0xf4, 0xf1, 0xf2, 0xe3, 0xe0, 0xe5, 0xe6, 0xef, 0xec, 0xe9, 0xea,
-	0xcb, 0xc8, 0xcd, 0xce, 0xc7, 0xc4, 0xc1, 0xc2, 0xd3, 0xd0, 0xd5, 0xd6, 0xdf, 0xdc, 0xd9, 0xda,
-	0x5b, 0x58, 0x5d, 0x5e, 0x57, 0x54, 0x51, 0x52, 0x43, 0x40, 0x45, 0x46, 0x4f, 0x4c, 0x49, 0x4a,
-	0x6b, 0x68, 0x6d, 0x6e, 0x67, 0x64, 0x61, 0x62, 0x73, 0x70, 0x75, 0x76, 0x7f, 0x7c, 0x79, 0x7a,
-	0x3b, 0x38, 0x3d, 0x3e, 0x37, 0x34, 0x31, 0x32, 0x23, 0x20, 0x25, 0x26, 0x2f, 0x2c, 0x29, 0x2a,
-	0x0b, 0x08, 0x0d, 0x0e, 0x07, 0x04, 0x01, 0x02, 0x13, 0x10, 0x15, 0x16, 0x1f, 0x1c, 0x19, 0x1a
-};
-
-static void xor_128(u8 *a, u8 *b, u8 *out)
-{
-	u32 *dwPtrA = (u32 *) a;
-	u32 *dwPtrB = (u32 *) b;
-	u32 *dwPtrOut = (u32 *) out;
-
-	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
-	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
-	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
-	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
-}
-
-static void xor_32(u8 *a, u8 *b, u8 *out)
-{
-	u32 *dwPtrA = (u32 *) a;
-	u32 *dwPtrB = (u32 *) b;
-	u32 *dwPtrOut = (u32 *) out;
-
-	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
-}
-
-static void AddRoundKey(u8 *key, int round)
-{
-	u8 sbox_key[4];
-	u8 rcon_table[10] = { 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x1b, 0x36};
-
-	sbox_key[0] = sbox_table[key[13]];
-	sbox_key[1] = sbox_table[key[14]];
-	sbox_key[2] = sbox_table[key[15]];
-	sbox_key[3] = sbox_table[key[12]];
-
-	key[0] = key[0] ^ rcon_table[round];
-	xor_32(&key[0], sbox_key, &key[0]);
-
-	xor_32(&key[4], &key[0], &key[4]);
-	xor_32(&key[8], &key[4], &key[8]);
-	xor_32(&key[12], &key[8], &key[12]);
-}
-
-static void SubBytes(u8 *in, u8 *out)
-{
-	int i;
-
-	for (i = 0; i < 16; i++)
-		out[i] = sbox_table[in[i]];
-}
-
-static void ShiftRows(u8 *in, u8 *out)
-{
-	out[0]  = in[0];
-	out[1]  = in[5];
-	out[2]  = in[10];
-	out[3]  = in[15];
-	out[4]  = in[4];
-	out[5]  = in[9];
-	out[6]  = in[14];
-	out[7]  = in[3];
-	out[8]  = in[8];
-	out[9]  = in[13];
-	out[10] = in[2];
-	out[11] = in[7];
-	out[12] = in[12];
-	out[13] = in[1];
-	out[14] = in[6];
-	out[15] = in[11];
-}
-
-static void MixColumns(u8 *in, u8 *out)
-{
-
-	out[0] = dot2_table[in[0]] ^ dot3_table[in[1]] ^ in[2] ^ in[3];
-	out[1] = in[0] ^ dot2_table[in[1]] ^ dot3_table[in[2]] ^ in[3];
-	out[2] = in[0] ^ in[1] ^ dot2_table[in[2]] ^ dot3_table[in[3]];
-	out[3] = dot3_table[in[0]] ^ in[1] ^ in[2] ^ dot2_table[in[3]];
-}
-
-static void AESv128(u8 *key, u8 *data, u8 *ciphertext)
-{
-	int  i;
-	int  round;
-	u8 TmpdataA[16];
-	u8 TmpdataB[16];
-	u8 abyRoundKey[16];
-
-	for (i = 0; i < 16; i++)
-		abyRoundKey[i] = key[i];
-
-	for (round = 0; round < 11; round++) {
-		if (round == 0) {
-			xor_128(abyRoundKey, data, ciphertext);
-			AddRoundKey(abyRoundKey, round);
-		} else if (round == 10) {
-			SubBytes(ciphertext, TmpdataA);
-			ShiftRows(TmpdataA, TmpdataB);
-			xor_128(TmpdataB, abyRoundKey, ciphertext);
-		} else { /* round 1 ~ 9 */
-			SubBytes(ciphertext, TmpdataA);
-			ShiftRows(TmpdataA, TmpdataB);
-			MixColumns(&TmpdataB[0], &TmpdataA[0]);
-			MixColumns(&TmpdataB[4], &TmpdataA[4]);
-			MixColumns(&TmpdataB[8], &TmpdataA[8]);
-			MixColumns(&TmpdataB[12], &TmpdataA[12]);
-			xor_128(TmpdataA, abyRoundKey, ciphertext);
-			AddRoundKey(abyRoundKey, round);
-		}
-	}
-
-}
-
-/*
- * Description: AES decryption
- *
- * Parameters:
- *  In:
- *      pbyRxKey            - The key used to decrypt
- *      pbyFrame            - Starting address of packet header
- *      wFrameSize          - Total packet size including CRC
- *  Out:
- *      none
- *
- * Return Value: MIC compare result
- *
- */
-
-bool AESbGenCCMP(u8 *pbyRxKey, u8 *pbyFrame, u16 wFrameSize)
-{
-	u8            abyNonce[13];
-	u8            MIC_IV[16];
-	u8            MIC_HDR1[16];
-	u8            MIC_HDR2[16];
-	u8            abyMIC[16];
-	u8            abyCTRPLD[16];
-	u8            abyTmp[16];
-	u8            abyPlainText[16];
-	u8            abyLastCipher[16];
-
-	struct ieee80211_hdr *pMACHeader = (struct ieee80211_hdr *) pbyFrame;
-	u8 *pbyIV;
-	u8 *pbyPayload;
-	u16            wHLen = 22;
-	/* 8 is IV, 8 is MIC, 4 is CRC */
-	u16            wPayloadSize = wFrameSize - 8 - 8 - 4 - WLAN_HDR_ADDR3_LEN;
-	bool            bA4 = false;
-	u8            byTmp;
-	u16            wCnt;
-	int             ii, jj, kk;
-
-	pbyIV = pbyFrame + WLAN_HDR_ADDR3_LEN;
-	if (WLAN_GET_FC_TODS(*(u16 *) pbyFrame) &&
-	    WLAN_GET_FC_FROMDS(*(u16 *) pbyFrame)) {
-		bA4 = true;
-		pbyIV += 6;             /* 6 is 802.11 address4 */
-		wHLen += 6;
-		wPayloadSize -= 6;
-	}
-	pbyPayload = pbyIV + 8; /* IV-length */
-
-	abyNonce[0]  = 0x00; /* now is 0, if Qos here will be priority */
-	memcpy(&(abyNonce[1]), pMACHeader->addr2, ETH_ALEN);
-	abyNonce[7]  = pbyIV[7];
-	abyNonce[8]  = pbyIV[6];
-	abyNonce[9]  = pbyIV[5];
-	abyNonce[10] = pbyIV[4];
-	abyNonce[11] = pbyIV[1];
-	abyNonce[12] = pbyIV[0];
-
-	/* MIC_IV */
-	MIC_IV[0] = 0x59;
-	memcpy(&(MIC_IV[1]), &(abyNonce[0]), 13);
-	MIC_IV[14] = (u8)(wPayloadSize >> 8);
-	MIC_IV[15] = (u8)(wPayloadSize & 0xff);
-
-	/* MIC_HDR1 */
-	MIC_HDR1[0] = (u8)(wHLen >> 8);
-	MIC_HDR1[1] = (u8)(wHLen & 0xff);
-	byTmp = (u8)(le16_to_cpu(pMACHeader->frame_control) >> 8);
-	MIC_HDR1[2] = byTmp & 0x8f;
-	byTmp = (u8)(le16_to_cpu(pMACHeader->frame_control) & 0xff);
-	byTmp &= 0x87;
-	MIC_HDR1[3] = byTmp | 0x40;
-	memcpy(&(MIC_HDR1[4]), pMACHeader->addr1, ETH_ALEN);
-	memcpy(&(MIC_HDR1[10]), pMACHeader->addr2, ETH_ALEN);
-
-	/* MIC_HDR2 */
-	memcpy(&(MIC_HDR2[0]), pMACHeader->addr3, ETH_ALEN);
-	byTmp = (u8)(le16_to_cpu(pMACHeader->seq_ctrl) >> 8);
-	MIC_HDR2[6] = byTmp & 0x0f;
-	MIC_HDR2[7] = 0;
-
-	if (bA4) {
-		memcpy(&(MIC_HDR2[8]), pMACHeader->addr4, ETH_ALEN);
-	} else {
-		MIC_HDR2[8]  = 0x00;
-		MIC_HDR2[9]  = 0x00;
-		MIC_HDR2[10] = 0x00;
-		MIC_HDR2[11] = 0x00;
-		MIC_HDR2[12] = 0x00;
-		MIC_HDR2[13] = 0x00;
-	}
-	MIC_HDR2[14] = 0x00;
-	MIC_HDR2[15] = 0x00;
-
-	/* CCMP */
-	AESv128(pbyRxKey, MIC_IV, abyMIC);
-	for (kk = 0; kk < 16; kk++)
-		abyTmp[kk] = MIC_HDR1[kk] ^ abyMIC[kk];
-
-	AESv128(pbyRxKey, abyTmp, abyMIC);
-	for (kk = 0; kk < 16; kk++)
-		abyTmp[kk] = MIC_HDR2[kk] ^ abyMIC[kk];
-
-	AESv128(pbyRxKey, abyTmp, abyMIC);
-
-	wCnt = 1;
-	abyCTRPLD[0] = 0x01;
-	memcpy(&(abyCTRPLD[1]), &(abyNonce[0]), 13);
-
-	for (jj = wPayloadSize; jj > 16; jj = jj-16) {
-
-		abyCTRPLD[14] = (u8) (wCnt >> 8);
-		abyCTRPLD[15] = (u8) (wCnt & 0xff);
-
-		AESv128(pbyRxKey, abyCTRPLD, abyTmp);
-
-		for (kk = 0; kk < 16; kk++)
-			abyPlainText[kk] = abyTmp[kk] ^ pbyPayload[kk];
-
-		for (kk = 0; kk < 16; kk++)
-			abyTmp[kk] = abyMIC[kk] ^ abyPlainText[kk];
-
-		AESv128(pbyRxKey, abyTmp, abyMIC);
-
-		memcpy(pbyPayload, abyPlainText, 16);
-		wCnt++;
-		pbyPayload += 16;
-	} /* for wPayloadSize */
-
-	/* last payload */
-	memcpy(&(abyLastCipher[0]), pbyPayload, jj);
-	for (ii = jj; ii < 16; ii++)
-		abyLastCipher[ii] = 0x00;
-
-	abyCTRPLD[14] = (u8) (wCnt >> 8);
-	abyCTRPLD[15] = (u8) (wCnt & 0xff);
-
-	AESv128(pbyRxKey, abyCTRPLD, abyTmp);
-	for (kk = 0; kk < 16; kk++)
-		abyPlainText[kk] = abyTmp[kk] ^ abyLastCipher[kk];
-
-	memcpy(pbyPayload, abyPlainText, jj);
-	pbyPayload += jj;
-
-	/* for MIC calculation */
-	for (ii = jj; ii < 16; ii++)
-		abyPlainText[ii] = 0x00;
-	for (kk = 0; kk < 16; kk++)
-		abyTmp[kk] = abyMIC[kk] ^ abyPlainText[kk];
-
-	AESv128(pbyRxKey, abyTmp, abyMIC);
-
-	/* => above is the calculated MIC */
-
-	wCnt = 0;
-	abyCTRPLD[14] = (u8) (wCnt >> 8);
-	abyCTRPLD[15] = (u8) (wCnt & 0xff);
-	AESv128(pbyRxKey, abyCTRPLD, abyTmp);
-
-	for (kk = 0; kk < 8; kk++)
-		abyTmp[kk] = abyTmp[kk] ^ pbyPayload[kk];
-
-	/* => above is the packet dec-MIC */
-
-	if (!memcmp(abyMIC, abyTmp, 8))
-		return true;
-	else
-		return false;
-}
diff --git a/drivers/staging/vt6656/aes_ccmp.h b/drivers/staging/vt6656/aes_ccmp.h
deleted file mode 100644
index ed6a9ae315d..00000000000
--- a/drivers/staging/vt6656/aes_ccmp.h
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: aes_ccmp.h
- *
- * Purpose: AES_CCMP Decryption
- *
- * Author: Warren Hsu
- *
- * Date: Feb 15, 2005
- *
- */
-
-#ifndef __AES_H__
-#define __AES_H__
-
-bool AESbGenCCMP(u8 * pbyRxKey, u8 * pbyFrame, u16 wFrameSize);
-
-#endif /* __AES_CCMP_H__ */
diff --git a/drivers/staging/vt6656/baseband.c b/drivers/staging/vt6656/baseband.c
index 3d4610e25fc..694e34a5ff9 100644
--- a/drivers/staging/vt6656/baseband.c
+++ b/drivers/staging/vt6656/baseband.c
@@ -40,13 +40,8 @@
 #include "mac.h"
 #include "baseband.h"
 #include "rf.h"
-#include "srom.h"
-#include "control.h"
+#include "usbpipe.h"
 #include "datarate.h"
-#include "rndis.h"
-
-static int          msglevel                =MSG_LEVEL_INFO;
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 
 static u8 abyVT3184_AGC[] = {
     0x00,   //0
@@ -638,80 +633,59 @@ static const u16 awcFrameTime[MAX_RATE] =
 {10, 20, 55, 110, 24, 36, 48, 72, 96, 144, 192, 216};
 
 /*
-static
-unsigned long
-s_ulGetLowSQ3(PSDevice pDevice);
-
-static
-unsigned long
-s_ulGetRatio(PSDevice pDevice);
-
-static
-void
-s_vClearSQ3Value(PSDevice pDevice);
-*/
-
-/*
  * Description: Calculate data frame transmitting time
  *
  * Parameters:
  *  In:
- *      byPreambleType  - Preamble Type
- *      byPktType        - PK_TYPE_11A, PK_TYPE_11B, PK_TYPE_11GB, PK_TYPE_11GA
- *      cbFrameLength   - Baseband Type
- *      wRate           - Tx Rate
+ *	preamble_type	- Preamble Type
+ *	pkt_type	- PK_TYPE_11A, PK_TYPE_11B, PK_TYPE_11GB, PK_TYPE_11GA
+ *	frame_length	- Baseband Type
+ *	tx_rate		- Tx Rate
  *  Out:
  *
  * Return Value: FrameTime
  *
  */
-unsigned int
-BBuGetFrameTime(
-     u8 byPreambleType,
-     u8 byPktType,
-     unsigned int cbFrameLength,
-     u16 wRate
-    )
+unsigned int BBuGetFrameTime(u8 preamble_type, u8 pkt_type,
+	unsigned int frame_length, u16 tx_rate)
 {
-    unsigned int uFrameTime;
-    unsigned int uPreamble;
-    unsigned int uTmp;
-    unsigned int uRateIdx = (unsigned int)wRate;
-    unsigned int uRate = 0;
-
-    if (uRateIdx > RATE_54M) {
-        return 0;
-    }
-
-    uRate = (unsigned int)awcFrameTime[uRateIdx];
-
-    if (uRateIdx <= 3) {          //CCK mode
-
-        if (byPreambleType == 1) {//Short
-            uPreamble = 96;
-        } else {
-            uPreamble = 192;
-        }
-        uFrameTime = (cbFrameLength * 80) / uRate;  //?????
-        uTmp = (uFrameTime * uRate) / 80;
-        if (cbFrameLength != uTmp) {
-            uFrameTime ++;
-        }
-
-        return (uPreamble + uFrameTime);
-    }
-    else {
-        uFrameTime = (cbFrameLength * 8 + 22) / uRate;   //????????
-        uTmp = ((uFrameTime * uRate) - 22) / 8;
-        if(cbFrameLength != uTmp) {
-            uFrameTime ++;
-        }
-        uFrameTime = uFrameTime * 4;    //???????
-        if(byPktType != PK_TYPE_11A) {
-            uFrameTime += 6;
-        }
-        return (20 + uFrameTime); //??????
-    }
+	unsigned int frame_time;
+	unsigned int preamble;
+	unsigned int tmp;
+	unsigned int rate = 0;
+
+	if (tx_rate > RATE_54M)
+		return 0;
+
+	rate = (unsigned int)awcFrameTime[tx_rate];
+
+	if (tx_rate <= 3) {
+		if (preamble_type == 1)
+			preamble = 96;
+		else
+			preamble = 192;
+
+		frame_time = (frame_length * 80) / rate;
+		tmp = (frame_time * rate) / 80;
+
+		if (frame_length != tmp)
+			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_time * 4;
+
+		if (pkt_type != PK_TYPE_11A)
+			frame_time += 6;
+
+		return 20 + frame_time;
+	}
 }
 
 /*
@@ -719,9 +693,9 @@ 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:
  *	struct vnt_phy_field *phy
  * 			- pointer to Phy Length field
@@ -731,153 +705,135 @@ BBuGetFrameTime(
  * Return Value: none
  *
  */
-void BBvCalculateParameter(struct vnt_private *pDevice, u32 cbFrameLength,
-	u16 wRate, u8 byPacketType, struct vnt_phy_field *phy)
+void BBvCalculateParameter(struct vnt_private *priv, u32 frame_length,
+	u16 tx_rate, u8 pkt_type, struct vnt_phy_field *phy)
 {
-	u32 cbBitCount;
-	u32 cbUsCount = 0;
-	u32 cbTmp;
-	int bExtBit;
-	u8 byPreambleType = pDevice->byPreambleType;
-	int bCCK = pDevice->bCCK;
-
-    cbBitCount = cbFrameLength * 8;
-    bExtBit = false;
-
-    switch (wRate) {
-    case RATE_1M :
-        cbUsCount = cbBitCount;
-	phy->signal = 0x00;
-        break;
-
-    case RATE_2M :
-        cbUsCount = cbBitCount / 2;
-        if (byPreambleType == 1)
-		phy->signal = 0x09;
-        else // long preamble
-		phy->signal = 0x01;
-        break;
-
-    case RATE_5M :
-        if (bCCK == false)
-            cbBitCount ++;
-        cbUsCount = (cbBitCount * 10) / 55;
-        cbTmp = (cbUsCount * 55) / 10;
-        if (cbTmp != cbBitCount)
-            cbUsCount ++;
-        if (byPreambleType == 1)
-		phy->signal = 0x0a;
-        else // long preamble
-		phy->signal = 0x02;
-        break;
-
-    case RATE_11M :
-
-        if (bCCK == false)
-            cbBitCount ++;
-        cbUsCount = cbBitCount / 11;
-        cbTmp = cbUsCount * 11;
-        if (cbTmp != cbBitCount) {
-            cbUsCount ++;
-            if ((cbBitCount - cbTmp) <= 3)
-                bExtBit = true;
-        }
-        if (byPreambleType == 1)
-		phy->signal = 0x0b;
-        else // long preamble
-		phy->signal = 0x03;
-        break;
-
-    case RATE_6M :
-        if(byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-		phy->signal = 0x9b;
-        }
-        else {//11g, 2.4GHZ
-		phy->signal = 0x8b;
-        }
-        break;
-
-    case RATE_9M :
-        if(byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-		phy->signal = 0x9f;
-        }
-        else {//11g, 2.4GHZ
-		phy->signal = 0x8f;
-        }
-        break;
-
-    case RATE_12M :
-        if(byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-		phy->signal = 0x9a;
-        }
-        else {//11g, 2.4GHZ
-		phy->signal = 0x8a;
-        }
-        break;
-
-    case RATE_18M :
-        if(byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-		phy->signal = 0x9e;
-        }
-        else {//11g, 2.4GHZ
-		phy->signal = 0x8e;
-        }
-        break;
-
-    case RATE_24M :
-        if(byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-		phy->signal = 0x99;
-        }
-        else {//11g, 2.4GHZ
-		phy->signal = 0x89;
-        }
-        break;
-
-    case RATE_36M :
-        if(byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-		phy->signal = 0x9d;
-        }
-        else {//11g, 2.4GHZ
-		phy->signal = 0x8d;
-        }
-        break;
-
-    case RATE_48M :
-        if(byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-		phy->signal = 0x98;
-        }
-        else {//11g, 2.4GHZ
-		phy->signal = 0x88;
-        }
-        break;
-
-    case RATE_54M :
-        if (byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-		phy->signal = 0x9c;
-        }
-        else {//11g, 2.4GHZ
-		phy->signal = 0x8c;
-        }
-        break;
-
-    default :
-        if (byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-		phy->signal = 0x9c;
-        }
-        else {//11g, 2.4GHZ
-		phy->signal = 0x8c;
-        }
-        break;
-    }
-
-	if (byPacketType == PK_TYPE_11B) {
+	u32 bit_count;
+	u32 count = 0;
+	u32 tmp;
+	int ext_bit;
+	u8 preamble_type = priv->byPreambleType;
+
+	bit_count = frame_length * 8;
+	ext_bit = false;
+
+	switch (tx_rate) {
+	case RATE_1M:
+		count = bit_count;
+
+		phy->signal = 0x00;
+
+		break;
+	case RATE_2M:
+		count = bit_count / 2;
+
+		if (preamble_type == 1)
+			phy->signal = 0x09;
+		else
+			phy->signal = 0x01;
+
+		break;
+	case RATE_5M:
+		count = (bit_count * 10) / 55;
+		tmp = (count * 55) / 10;
+
+		if (tmp != bit_count)
+			count++;
+
+		if (preamble_type == 1)
+			phy->signal = 0x0a;
+		else
+			phy->signal = 0x02;
+
+		break;
+	case RATE_11M:
+		count = bit_count / 11;
+		tmp = count * 11;
+
+		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;
+
+		break;
+	case RATE_9M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9f;
+		else
+			phy->signal = 0x8f;
+
+		break;
+	case RATE_12M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9a;
+		else
+			phy->signal = 0x8a;
+
+		break;
+	case RATE_18M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9e;
+		else
+			phy->signal = 0x8e;
+
+		break;
+	case RATE_24M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x99;
+		else
+			phy->signal = 0x89;
+
+		break;
+	case RATE_36M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9d;
+		else
+			phy->signal = 0x8d;
+
+		break;
+	case RATE_48M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x98;
+		else
+			phy->signal = 0x88;
+
+		break;
+	case RATE_54M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9c;
+		else
+			phy->signal = 0x8c;
+		break;
+	default:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9c;
+		else
+			phy->signal = 0x8c;
+		break;
+	}
+
+	if (pkt_type == PK_TYPE_11B) {
 		phy->service = 0x00;
-		if (bExtBit)
+		if (ext_bit)
 			phy->service |= 0x80;
-		phy->len = cpu_to_le16((u16)cbUsCount);
+		phy->len = cpu_to_le16((u16)count);
 	} else {
 		phy->service = 0x00;
-		phy->len = cpu_to_le16((u16)cbFrameLength);
+		phy->len = cpu_to_le16((u16)frame_length);
 	}
 }
 
@@ -886,35 +842,31 @@ void BBvCalculateParameter(struct vnt_private *pDevice, u32 cbFrameLength,
  *
  * Parameters:
  *  In:
- *      pDevice          - Device Structure
- *      byAntennaMode    - Antenna Mode
+ *	priv		- Device Structure
+ *	antenna_mode	- Antenna Mode
  *  Out:
  *      none
  *
  * Return Value: none
  *
  */
-void BBvSetAntennaMode(struct vnt_private *pDevice, u8 byAntennaMode)
+void BBvSetAntennaMode(struct vnt_private *priv, u8 antenna_mode)
 {
-    switch (byAntennaMode) {
-        case ANT_TXA:
-        case ANT_TXB:
-            break;
-        case ANT_RXA:
-            pDevice->byBBRxConf &= 0xFC;
-            break;
-        case ANT_RXB:
-            pDevice->byBBRxConf &= 0xFE;
-            pDevice->byBBRxConf |= 0x02;
-            break;
-    }
-
-    CONTROLnsRequestOut(pDevice,
-                    MESSAGE_TYPE_SET_ANTMD,
-                    (u16) byAntennaMode,
-                    0,
-                    0,
-                    NULL);
+	switch (antenna_mode) {
+	case ANT_TXA:
+	case ANT_TXB:
+		break;
+	case ANT_RXA:
+		priv->byBBRxConf &= 0xFC;
+		break;
+	case ANT_RXB:
+		priv->byBBRxConf &= 0xFE;
+		priv->byBBRxConf |= 0x02;
+		break;
+	}
+
+	vnt_control_out(priv, MESSAGE_TYPE_SET_ANTMD,
+		(u16)antenna_mode, 0, 0, NULL);
 }
 
 /*
@@ -934,14 +886,14 @@ void BBvSetAntennaMode(struct vnt_private *pDevice, u8 byAntennaMode)
 int BBbVT3184Init(struct vnt_private *priv)
 {
 	int status;
-	u16 lenght;
+	u16 length;
 	u8 *addr;
 	u8 *agc;
-	u16 lenght_agc;
+	u16 length_agc;
 	u8 array[256];
 	u8 data;
 
-	status = CONTROLnsRequestIn(priv, MESSAGE_TYPE_READ, 0,
+	status = vnt_control_in(priv, MESSAGE_TYPE_READ, 0,
 		MESSAGE_REQUEST_EEPROM, EEP_MAX_CONTEXT_SIZE,
 						priv->abyEEPROM);
 	if (status != STATUS_SUCCESS)
@@ -955,20 +907,20 @@ int BBbVT3184Init(struct vnt_private *priv)
 			(priv->abyEEPROM[EEP_OFS_ZONETYPE] != 0x00)) {
 			priv->abyEEPROM[EEP_OFS_ZONETYPE] = 0;
 			priv->abyEEPROM[EEP_OFS_MAXCHANNEL] = 0x0B;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-						"Init Zone Type :USA\n");
+
+			dev_dbg(&priv->usb->dev, "Init Zone Type :USA\n");
 		} else if ((priv->config_file.ZoneType == 1) &&
 			(priv->abyEEPROM[EEP_OFS_ZONETYPE] != 0x01)) {
 			priv->abyEEPROM[EEP_OFS_ZONETYPE] = 0x01;
 			priv->abyEEPROM[EEP_OFS_MAXCHANNEL] = 0x0D;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-						"Init Zone Type :Japan\n");
+
+			dev_dbg(&priv->usb->dev, "Init Zone Type :Japan\n");
 		} else if ((priv->config_file.ZoneType == 2) &&
 			(priv->abyEEPROM[EEP_OFS_ZONETYPE] != 0x02)) {
 			priv->abyEEPROM[EEP_OFS_ZONETYPE] = 0x02;
 			priv->abyEEPROM[EEP_OFS_MAXCHANNEL] = 0x0D;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-						"Init Zone Type :Europe\n");
+
+			dev_dbg(&priv->usb->dev, "Init Zone Type :Europe\n");
 		} else {
 			if (priv->config_file.ZoneType !=
 					priv->abyEEPROM[EEP_OFS_ZONETYPE])
@@ -988,18 +940,17 @@ int BBbVT3184Init(struct vnt_private *priv)
 
 	priv->byRFType = priv->abyEEPROM[EEP_OFS_RFTYPE];
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Zone Type %x\n",
-							priv->byZoneType);
+	dev_dbg(&priv->usb->dev, "Zone Type %x\n", priv->byZoneType);
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"RF Type %d\n", priv->byRFType);
+	dev_dbg(&priv->usb->dev, "RF Type %d\n", priv->byRFType);
 
 	if ((priv->byRFType == RF_AL2230) ||
 				(priv->byRFType == RF_AL2230S)) {
 		priv->byBBRxConf = abyVT3184_AL2230[10];
-		lenght = sizeof(abyVT3184_AL2230);
+		length = sizeof(abyVT3184_AL2230);
 		addr = abyVT3184_AL2230;
 		agc = abyVT3184_AGC;
-		lenght_agc = sizeof(abyVT3184_AGC);
+		length_agc = sizeof(abyVT3184_AGC);
 
 		priv->abyBBVGA[0] = 0x1C;
 		priv->abyBBVGA[1] = 0x10;
@@ -1011,10 +962,10 @@ int BBbVT3184Init(struct vnt_private *priv)
 		priv->ldBmThreshold[3] = 0;
 	} else if (priv->byRFType == RF_AIROHA7230) {
 		priv->byBBRxConf = abyVT3184_AL2230[10];
-		lenght = sizeof(abyVT3184_AL2230);
+		length = sizeof(abyVT3184_AL2230);
 		addr = abyVT3184_AL2230;
 		agc = abyVT3184_AGC;
-		lenght_agc = sizeof(abyVT3184_AGC);
+		length_agc = sizeof(abyVT3184_AGC);
 
 		addr[0xd7] = 0x06;
 
@@ -1029,10 +980,10 @@ int BBbVT3184Init(struct vnt_private *priv)
 	} else if ((priv->byRFType == RF_VT3226) ||
 			(priv->byRFType == RF_VT3226D0)) {
 		priv->byBBRxConf = abyVT3184_VT3226D0[10];
-		lenght = sizeof(abyVT3184_VT3226D0);
+		length = sizeof(abyVT3184_VT3226D0);
 		addr = abyVT3184_VT3226D0;
 		agc = abyVT3184_AGC;
-		lenght_agc = sizeof(abyVT3184_AGC);
+		length_agc = sizeof(abyVT3184_AGC);
 
 		priv->abyBBVGA[0] = 0x20;
 		priv->abyBBVGA[1] = 0x10;
@@ -1046,10 +997,10 @@ int BBbVT3184Init(struct vnt_private *priv)
 		MACvRegBitsOn(priv, MAC_REG_SOFTPWRCTL2, SOFTPWRCTL_RFLEOPT);
 	} else if ((priv->byRFType == RF_VT3342A0)) {
 		priv->byBBRxConf = abyVT3184_VT3226D0[10];
-		lenght = sizeof(abyVT3184_VT3226D0);
+		length = sizeof(abyVT3184_VT3226D0);
 		addr = abyVT3184_VT3226D0;
 		agc = abyVT3184_AGC;
-		lenght_agc = sizeof(abyVT3184_AGC);
+		length_agc = sizeof(abyVT3184_AGC);
 
 		priv->abyBBVGA[0] = 0x20;
 		priv->abyBBVGA[1] = 0x10;
@@ -1065,40 +1016,39 @@ int BBbVT3184Init(struct vnt_private *priv)
 		return true;
 	}
 
-	memcpy(array, addr, lenght);
+	memcpy(array, addr, length);
 
-	CONTROLnsRequestOut(priv, MESSAGE_TYPE_WRITE, 0,
-		MESSAGE_REQUEST_BBREG, lenght, array);
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE, 0,
+		MESSAGE_REQUEST_BBREG, length, array);
 
-	memcpy(array, agc, lenght_agc);
+	memcpy(array, agc, length_agc);
 
-	CONTROLnsRequestOut(priv, MESSAGE_TYPE_WRITE, 0,
-		MESSAGE_REQUEST_BBAGC, lenght_agc, array);
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE, 0,
+		MESSAGE_REQUEST_BBAGC, length_agc, array);
 
 	if ((priv->byRFType == RF_VT3226) ||
 		(priv->byRFType == RF_VT3342A0)) {
-		ControlvWriteByte(priv, MESSAGE_REQUEST_MACREG,
+		vnt_control_out_u8(priv, MESSAGE_REQUEST_MACREG,
 						MAC_REG_ITRTMSET, 0x23);
 		MACvRegBitsOn(priv, MAC_REG_PAPEDELAY, 0x01);
 	} else if (priv->byRFType == RF_VT3226D0) {
-		ControlvWriteByte(priv, MESSAGE_REQUEST_MACREG,
+		vnt_control_out_u8(priv, MESSAGE_REQUEST_MACREG,
 						MAC_REG_ITRTMSET, 0x11);
 		MACvRegBitsOn(priv, MAC_REG_PAPEDELAY, 0x01);
 	}
 
-	ControlvWriteByte(priv, MESSAGE_REQUEST_BBREG, 0x04, 0x7f);
-	ControlvWriteByte(priv, MESSAGE_REQUEST_BBREG, 0x0d, 0x01);
-
-	RFbRFTableDownload(priv);
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x04, 0x7f);
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0d, 0x01);
 
+	vnt_rf_table_download(priv);
 
 	/* Fix for TX USB resets from vendors driver */
-	CONTROLnsRequestIn(priv, MESSAGE_TYPE_READ, USB_REG4,
+	vnt_control_in(priv, MESSAGE_TYPE_READ, USB_REG4,
 		MESSAGE_REQUEST_MEM, sizeof(data), &data);
 
 	data |= 0x2;
 
-	CONTROLnsRequestOut(priv, MESSAGE_TYPE_WRITE, USB_REG4,
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE, USB_REG4,
 		MESSAGE_REQUEST_MEM, sizeof(data), &data);
 
 	return true;
@@ -1109,41 +1059,42 @@ int BBbVT3184Init(struct vnt_private *priv)
  *
  * Parameters:
  *  In:
- *      pDevice     - Device Structure
+ *	priv	- Device Structure
  *  Out:
  *      none
  *
  * Return Value: none
  *
  */
-void BBvSetShortSlotTime(struct vnt_private *pDevice)
+void BBvSetShortSlotTime(struct vnt_private *priv)
 {
-    u8 byBBVGA=0;
+	u8 bb_vga = 0;
 
-	if (pDevice->bShortSlotTime)
-        pDevice->byBBRxConf &= 0xDF;//1101 1111
+	if (priv->bShortSlotTime)
+		priv->byBBRxConf &= 0xdf;
 	else
-        pDevice->byBBRxConf |= 0x20;//0010 0000
+		priv->byBBRxConf |= 0x20;
+
+	vnt_control_in_u8(priv, MESSAGE_REQUEST_BBREG, 0xe7, &bb_vga);
 
-    ControlvReadByte (pDevice, MESSAGE_REQUEST_BBREG, 0xE7, &byBBVGA);
-	if (byBBVGA == pDevice->abyBBVGA[0])
-        pDevice->byBBRxConf |= 0x20;//0010 0000
+	if (bb_vga == priv->abyBBVGA[0])
+		priv->byBBRxConf |= 0x20;
 
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x0A, pDevice->byBBRxConf);
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0a, priv->byBBRxConf);
 }
 
-void BBvSetVGAGainOffset(struct vnt_private *pDevice, u8 byData)
+void BBvSetVGAGainOffset(struct vnt_private *priv, u8 data)
 {
 
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xE7, byData);
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0xE7, data);
 
-    // patch for 3253B0 Baseband with Cardbus module
-	if (pDevice->bShortSlotTime)
-		pDevice->byBBRxConf &= 0xDF; /* 1101 1111 */
+	/* patch for 3253B0 Baseband with Cardbus module */
+	if (priv->bShortSlotTime)
+		priv->byBBRxConf &= 0xdf; /* 1101 1111 */
 	else
-		pDevice->byBBRxConf |= 0x20; /* 0010 0000 */
+		priv->byBBRxConf |= 0x20; /* 0010 0000 */
 
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x0A, pDevice->byBBRxConf);//CR10
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0a, priv->byBBRxConf);
 }
 
 /*
@@ -1151,735 +1102,288 @@ void BBvSetVGAGainOffset(struct vnt_private *pDevice, u8 byData)
  *
  * Parameters:
  *  In:
- *      pDevice          - Device Structure
+ *	priv	- Device Structure
  *  Out:
  *      none
  *
  * Return Value: none
  *
  */
-void BBvSetDeepSleep(struct vnt_private *pDevice)
-{
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x0c, 0x17);//CR12
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x0D, 0xB9);//CR13
-}
-
-void BBvExitDeepSleep(struct vnt_private *pDevice)
+void BBvSetDeepSleep(struct vnt_private *priv)
 {
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x0C, 0x00);//CR12
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x0D, 0x01);//CR13
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0c, 0x17);/* CR12 */
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0d, 0xB9);/* CR13 */
 }
 
-static unsigned long s_ulGetLowSQ3(struct vnt_private *pDevice)
+void BBvExitDeepSleep(struct vnt_private *priv)
 {
-	int ii;
-	unsigned long ulSQ3 = 0;
-	unsigned long ulMaxPacket;
-
-    ulMaxPacket = pDevice->aulPktNum[RATE_54M];
-	if (pDevice->aulPktNum[RATE_54M] != 0)
-        ulSQ3 = pDevice->aulSQ3Val[RATE_54M] / pDevice->aulPktNum[RATE_54M];
-
-	for (ii = RATE_48M; ii >= RATE_6M; ii--)
-		if (pDevice->aulPktNum[ii] > ulMaxPacket) {
-            ulMaxPacket = pDevice->aulPktNum[ii];
-            ulSQ3 = pDevice->aulSQ3Val[ii] / pDevice->aulPktNum[ii];
-        }
-
-    return ulSQ3;
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0c, 0x00);/* CR12 */
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0d, 0x01);/* CR13 */
 }
 
-static unsigned long s_ulGetRatio(struct vnt_private *pDevice)
+void BBvUpdatePreEDThreshold(struct vnt_private *priv, int scanning)
 {
-	int ii, jj;
-	unsigned long ulRatio = 0;
-	unsigned long ulMaxPacket;
-	unsigned long ulPacketNum;
-
-    //This is a thousand-ratio
-    ulMaxPacket = pDevice->aulPktNum[RATE_54M];
-    if ( pDevice->aulPktNum[RATE_54M] != 0 ) {
-        ulPacketNum = pDevice->aulPktNum[RATE_54M];
-        ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-        ulRatio += TOP_RATE_54M;
-    }
-	for (ii = RATE_48M; ii >= RATE_1M; ii--)
-        if ( pDevice->aulPktNum[ii] > ulMaxPacket ) {
-            ulPacketNum = 0;
-            for ( jj=RATE_54M;jj>=ii;jj--)
-                ulPacketNum += pDevice->aulPktNum[jj];
-            ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-            ulRatio += TOP_RATE_48M;
-            ulMaxPacket = pDevice->aulPktNum[ii];
-        }
-
-    return ulRatio;
-}
-
-static void s_vClearSQ3Value(struct vnt_private *pDevice)
-{
-    int ii;
-    pDevice->uDiversityCnt = 0;
-
-    for ( ii=RATE_1M;ii<MAX_RATE;ii++) {
-        pDevice->aulPktNum[ii] = 0;
-        pDevice->aulSQ3Val[ii] = 0;
-    }
-}
-
-/*
- * Description: Antenna Diversity
- *
- * Parameters:
- *  In:
- *      pDevice          - Device Structure
- *      byRSR            - RSR from received packet
- *      bySQ3            - SQ3 value from received packet
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-
-void BBvAntennaDiversity(struct vnt_private *pDevice,
-	u8 byRxRate, u8 bySQ3)
-{
-
-    pDevice->uDiversityCnt++;
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pDevice->uDiversityCnt = %d\n", (int)pDevice->uDiversityCnt);
-
-    if (byRxRate == 2) {
-        pDevice->aulPktNum[RATE_1M]++;
-    }
-    else if (byRxRate==4) {
-        pDevice->aulPktNum[RATE_2M]++;
-    }
-    else if (byRxRate==11) {
-        pDevice->aulPktNum[RATE_5M]++;
-    }
-    else if (byRxRate==22) {
-        pDevice->aulPktNum[RATE_11M]++;
-    }
-    else if(byRxRate==12){
-        pDevice->aulPktNum[RATE_6M]++;
-        pDevice->aulSQ3Val[RATE_6M] += bySQ3;
-    }
-    else if(byRxRate==18){
-        pDevice->aulPktNum[RATE_9M]++;
-        pDevice->aulSQ3Val[RATE_9M] += bySQ3;
-    }
-    else if(byRxRate==24){
-        pDevice->aulPktNum[RATE_12M]++;
-        pDevice->aulSQ3Val[RATE_12M] += bySQ3;
-    }
-    else if(byRxRate==36){
-        pDevice->aulPktNum[RATE_18M]++;
-        pDevice->aulSQ3Val[RATE_18M] += bySQ3;
-    }
-    else if(byRxRate==48){
-        pDevice->aulPktNum[RATE_24M]++;
-        pDevice->aulSQ3Val[RATE_24M] += bySQ3;
-    }
-    else if(byRxRate==72){
-        pDevice->aulPktNum[RATE_36M]++;
-        pDevice->aulSQ3Val[RATE_36M] += bySQ3;
-    }
-    else if(byRxRate==96){
-        pDevice->aulPktNum[RATE_48M]++;
-        pDevice->aulSQ3Val[RATE_48M] += bySQ3;
-    }
-    else if(byRxRate==108){
-        pDevice->aulPktNum[RATE_54M]++;
-        pDevice->aulSQ3Val[RATE_54M] += bySQ3;
-    }
-
-    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->aulPktNum[RATE_54M]);
-
-            pDevice->ulSQ3_State0 = s_ulGetLowSQ3(pDevice);
-            pDevice->ulRatio_State0 = s_ulGetRatio(pDevice);
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"SQ3_State0, SQ3= [%08x] rate = [%08x]\n",(int)pDevice->ulSQ3_State0,(int)pDevice->ulRatio_State0);
-
-            if ( ((pDevice->aulPktNum[RATE_54M] < pDevice->ulDiversityNValue/2) &&
-                  (pDevice->ulSQ3_State0 > pDevice->ulSQ3TH) ) ||
-                 (pDevice->ulSQ3_State0 == 0 ) )  {
-
-                if ( pDevice->byTMax == 0 )
-                    return;
-
-		bScheduleCommand((void *) pDevice,
-				 WLAN_CMD_CHANGE_ANTENNA,
-				 NULL);
-
-                pDevice->byAntennaState = 1;
-
-                del_timer(&pDevice->TimerSQ3Tmax3);
-                del_timer(&pDevice->TimerSQ3Tmax2);
-                pDevice->TimerSQ3Tmax1.expires =  RUN_AT(pDevice->byTMax * HZ);
-                add_timer(&pDevice->TimerSQ3Tmax1);
-
-            } else {
-                pDevice->TimerSQ3Tmax3.expires =  RUN_AT(pDevice->byTMax3 * HZ);
-                add_timer(&pDevice->TimerSQ3Tmax3);
-            }
-            s_vClearSQ3Value(pDevice);
-
-        }
-    } else { //byAntennaState == 1
-
-        if (pDevice->uDiversityCnt > pDevice->ulDiversityMValue) {
-
-            del_timer(&pDevice->TimerSQ3Tmax1);
-            pDevice->ulSQ3_State1 = s_ulGetLowSQ3(pDevice);
-            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);
-
-            if ( ((pDevice->ulSQ3_State1 == 0) && (pDevice->ulSQ3_State0 != 0)) ||
-                 ((pDevice->ulSQ3_State1 == 0) && (pDevice->ulSQ3_State0 == 0) && (pDevice->ulRatio_State1 < pDevice->ulRatio_State0)) ||
-                 ((pDevice->ulSQ3_State1 != 0) && (pDevice->ulSQ3_State0 != 0) && (pDevice->ulSQ3_State0 < pDevice->ulSQ3_State1))
-               ) {
-
-		bScheduleCommand((void *) pDevice,
-				 WLAN_CMD_CHANGE_ANTENNA,
-				 NULL);
-
-                pDevice->TimerSQ3Tmax3.expires =  RUN_AT(pDevice->byTMax3 * HZ);
-                pDevice->TimerSQ3Tmax2.expires =  RUN_AT(pDevice->byTMax2 * HZ);
-                add_timer(&pDevice->TimerSQ3Tmax3);
-                add_timer(&pDevice->TimerSQ3Tmax2);
-
-            }
-            pDevice->byAntennaState = 0;
-            s_vClearSQ3Value(pDevice);
-        }
-    } //byAntennaState
-}
-
-/*+
- *
- * Description:
- *  Timer for SQ3 antenna diversity
- *
- * Parameters:
- *  In:
- *      pvSysSpec1
- *      hDeviceContext - Pointer to the adapter
- *      pvSysSpec2
- *      pvSysSpec3
- *  Out:
- *      none
- *
- * Return Value: none
- *
--*/
-
-void TimerSQ3CallBack(struct vnt_private *pDevice)
-{
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"TimerSQ3CallBack...");
-    spin_lock_irq(&pDevice->lock);
-
-    bScheduleCommand((void *) pDevice, WLAN_CMD_CHANGE_ANTENNA, NULL);
-    pDevice->byAntennaState = 0;
-    s_vClearSQ3Value(pDevice);
-    pDevice->TimerSQ3Tmax3.expires =  RUN_AT(pDevice->byTMax3 * HZ);
-    pDevice->TimerSQ3Tmax2.expires =  RUN_AT(pDevice->byTMax2 * HZ);
-    add_timer(&pDevice->TimerSQ3Tmax3);
-    add_timer(&pDevice->TimerSQ3Tmax2);
-
-    spin_unlock_irq(&pDevice->lock);
-}
+	u8 cr_201 = 0x0, cr_206 = 0x0;
+	u8 ed_inx = priv->byBBPreEDIndex;
+
+	switch (priv->byRFType) {
+	case RF_AL2230:
+	case RF_AL2230S:
+	case RF_AIROHA7230:
+		if (scanning) { /* Max sensitivity */
+			ed_inx = 0;
+			cr_206 = 0x30;
+			break;
+		}
 
-/*+
- *
- * Description:
- *  Timer for SQ3 antenna diversity
- *
- * Parameters:
- *  In:
- *      pvSysSpec1
- *      hDeviceContext - Pointer to the adapter
- *      pvSysSpec2
- *      pvSysSpec3
- *  Out:
- *      none
- *
- * Return Value: none
- *
--*/
+		if (priv->byBBPreEDRSSI <= 45) {
+			ed_inx = 20;
+			cr_201 = 0xff;
+		} else if (priv->byBBPreEDRSSI <= 46) {
+			ed_inx = 19;
+			cr_201 = 0x1a;
+		} else if (priv->byBBPreEDRSSI <= 47) {
+			ed_inx = 18;
+			cr_201 = 0x15;
+		} else if (priv->byBBPreEDRSSI <= 49) {
+			ed_inx = 17;
+			cr_201 = 0xe;
+		} else if (priv->byBBPreEDRSSI <= 51) {
+			ed_inx = 16;
+			cr_201 = 0x9;
+		} else if (priv->byBBPreEDRSSI <= 53) {
+			ed_inx = 15;
+			cr_201 = 0x6;
+		} else if (priv->byBBPreEDRSSI <= 55) {
+			ed_inx = 14;
+			cr_201 = 0x3;
+		} else if (priv->byBBPreEDRSSI <= 56) {
+			ed_inx = 13;
+			cr_201 = 0x2;
+			cr_206 = 0xa0;
+		} else if (priv->byBBPreEDRSSI <= 57) {
+			ed_inx = 12;
+			cr_201 = 0x2;
+			cr_206 = 0x20;
+		} else if (priv->byBBPreEDRSSI <= 58) {
+			ed_inx = 11;
+			cr_201 = 0x1;
+			cr_206 = 0xa0;
+		} else if (priv->byBBPreEDRSSI <= 59) {
+			ed_inx = 10;
+			cr_201 = 0x1;
+			cr_206 = 0x54;
+		} else if (priv->byBBPreEDRSSI <= 60) {
+			ed_inx = 9;
+			cr_201 = 0x1;
+			cr_206 = 0x18;
+		} else if (priv->byBBPreEDRSSI <= 61) {
+			ed_inx = 8;
+			cr_206 = 0xe3;
+		} else if (priv->byBBPreEDRSSI <= 62) {
+			ed_inx = 7;
+			cr_206 = 0xb9;
+		} else if (priv->byBBPreEDRSSI <= 63) {
+			ed_inx = 6;
+			cr_206 = 0x93;
+		} else if (priv->byBBPreEDRSSI <= 64) {
+			ed_inx = 5;
+			cr_206 = 0x79;
+		} else if (priv->byBBPreEDRSSI <= 65) {
+			ed_inx = 4;
+			cr_206 = 0x62;
+		} else if (priv->byBBPreEDRSSI <= 66) {
+			ed_inx = 3;
+			cr_206 = 0x51;
+		} else if (priv->byBBPreEDRSSI <= 67) {
+			ed_inx = 2;
+			cr_206 = 0x43;
+		} else if (priv->byBBPreEDRSSI <= 68) {
+			ed_inx = 1;
+			cr_206 = 0x36;
+		} else {
+			ed_inx = 0;
+			cr_206 = 0x30;
+		}
+		break;
+
+	case RF_VT3226:
+	case RF_VT3226D0:
+		if (scanning)	{ /* Max sensitivity */
+			ed_inx = 0;
+			cr_206 = 0x24;
+			break;
+		}
 
-void TimerSQ3Tmax3CallBack(struct vnt_private *pDevice)
-{
+		if (priv->byBBPreEDRSSI <= 41) {
+			ed_inx = 22;
+			cr_201 = 0xff;
+		} else if (priv->byBBPreEDRSSI <= 42) {
+			ed_inx = 21;
+			cr_201 = 0x36;
+		} else if (priv->byBBPreEDRSSI <= 43) {
+			ed_inx = 20;
+			cr_201 = 0x26;
+		} else if (priv->byBBPreEDRSSI <= 45) {
+			ed_inx = 19;
+			cr_201 = 0x18;
+		} else if (priv->byBBPreEDRSSI <= 47) {
+			ed_inx = 18;
+			cr_201 = 0x11;
+		} else if (priv->byBBPreEDRSSI <= 49) {
+			ed_inx = 17;
+			cr_201 = 0xa;
+		} else if (priv->byBBPreEDRSSI <= 51) {
+			ed_inx = 16;
+			cr_201 = 0x7;
+		} else if (priv->byBBPreEDRSSI <= 53) {
+			ed_inx = 15;
+			cr_201 = 0x4;
+		} else if (priv->byBBPreEDRSSI <= 55) {
+			ed_inx = 14;
+			cr_201 = 0x2;
+			cr_206 = 0xc0;
+		} else if (priv->byBBPreEDRSSI <= 56) {
+			ed_inx = 13;
+			cr_201 = 0x2;
+			cr_206 = 0x30;
+		} else if (priv->byBBPreEDRSSI <= 57) {
+			ed_inx = 12;
+			cr_201 = 0x1;
+			cr_206 = 0xb0;
+		} else if (priv->byBBPreEDRSSI <= 58) {
+			ed_inx = 11;
+			cr_201 = 0x1;
+			cr_206 = 0x70;
+		} else if (priv->byBBPreEDRSSI <= 59) {
+			ed_inx = 10;
+			cr_201 = 0x1;
+			cr_206 = 0x30;
+		} else if (priv->byBBPreEDRSSI <= 60) {
+			ed_inx = 9;
+			cr_206 = 0xea;
+		} else if (priv->byBBPreEDRSSI <= 61) {
+			ed_inx = 8;
+			cr_206 = 0xc0;
+		} else if (priv->byBBPreEDRSSI <= 62) {
+			ed_inx = 7;
+			cr_206 = 0x9c;
+		} else if (priv->byBBPreEDRSSI <= 63) {
+			ed_inx = 6;
+			cr_206 = 0x80;
+		} else if (priv->byBBPreEDRSSI <= 64) {
+			ed_inx = 5;
+			cr_206 = 0x68;
+		} else if (priv->byBBPreEDRSSI <= 65) {
+			ed_inx = 4;
+			cr_206 = 0x52;
+		} else if (priv->byBBPreEDRSSI <= 66) {
+			ed_inx = 3;
+			cr_206 = 0x43;
+		} else if (priv->byBBPreEDRSSI <= 67) {
+			ed_inx = 2;
+			cr_206 = 0x36;
+		} else if (priv->byBBPreEDRSSI <= 68) {
+			ed_inx = 1;
+			cr_206 = 0x2d;
+		} else {
+			ed_inx = 0;
+			cr_206 = 0x24;
+		}
+		break;
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"TimerSQ3Tmax3CallBack...");
-    spin_lock_irq(&pDevice->lock);
+	case RF_VT3342A0:
+		if (scanning) { /* need Max sensitivity */
+			ed_inx = 0;
+			cr_206 = 0x38;
+			break;
+		}
 
-    pDevice->ulRatio_State0 = s_ulGetRatio(pDevice);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"SQ3_State0 = [%08x]\n",(int)pDevice->ulRatio_State0);
+		if (priv->byBBPreEDRSSI <= 41) {
+			ed_inx = 20;
+			cr_201 = 0xff;
+		} else if (priv->byBBPreEDRSSI <= 42) {
+			ed_inx = 19;
+			cr_201 = 0x36;
+		} else if (priv->byBBPreEDRSSI <= 43) {
+			ed_inx = 18;
+			cr_201 = 0x26;
+		} else if (priv->byBBPreEDRSSI <= 45) {
+			ed_inx = 17;
+			cr_201 = 0x18;
+		} else if (priv->byBBPreEDRSSI <= 47) {
+			ed_inx = 16;
+			cr_201 = 0x11;
+		} else if (priv->byBBPreEDRSSI <= 49) {
+			ed_inx = 15;
+			cr_201 = 0xa;
+		} else if (priv->byBBPreEDRSSI <= 51) {
+			ed_inx = 14;
+			cr_201 = 0x7;
+		} else if (priv->byBBPreEDRSSI <= 53) {
+			ed_inx = 13;
+			cr_201 = 0x4;
+		} else if (priv->byBBPreEDRSSI <= 55) {
+			ed_inx = 12;
+			cr_201 = 0x2;
+			cr_206 = 0xc0;
+		} else if (priv->byBBPreEDRSSI <= 56) {
+			ed_inx = 11;
+			cr_201 = 0x2;
+			cr_206 = 0x30;
+		} else if (priv->byBBPreEDRSSI <= 57) {
+			ed_inx = 10;
+			cr_201 = 0x1;
+			cr_206 = 0xb0;
+		} else if (priv->byBBPreEDRSSI <= 58) {
+			ed_inx = 9;
+			cr_201 = 0x1;
+			cr_206 = 0x70;
+		} else if (priv->byBBPreEDRSSI <= 59) {
+			ed_inx = 8;
+			cr_201 = 0x1;
+			cr_206 = 0x30;
+		} else if (priv->byBBPreEDRSSI <= 60) {
+			ed_inx = 7;
+			cr_206 = 0xea;
+		} else if (priv->byBBPreEDRSSI <= 61) {
+			ed_inx = 6;
+			cr_206 = 0xc0;
+		} else if (priv->byBBPreEDRSSI <= 62) {
+			ed_inx = 5;
+			cr_206 = 0x9c;
+		} else if (priv->byBBPreEDRSSI <= 63) {
+			ed_inx = 4;
+			cr_206 = 0x80;
+		} else if (priv->byBBPreEDRSSI <= 64) {
+			ed_inx = 3;
+			cr_206 = 0x68;
+		} else if (priv->byBBPreEDRSSI <= 65) {
+			ed_inx = 2;
+			cr_206 = 0x52;
+		} else if (priv->byBBPreEDRSSI <= 66) {
+			ed_inx = 1;
+			cr_206 = 0x43;
+		} else {
+			ed_inx = 0;
+			cr_206 = 0x38;
+		}
+		break;
 
-    s_vClearSQ3Value(pDevice);
-    if ( pDevice->byTMax == 0 ) {
-        pDevice->TimerSQ3Tmax3.expires =  RUN_AT(pDevice->byTMax3 * HZ);
-        add_timer(&pDevice->TimerSQ3Tmax3);
-        spin_unlock_irq(&pDevice->lock);
-        return;
-    }
+	}
 
-    bScheduleCommand((void *) pDevice, WLAN_CMD_CHANGE_ANTENNA, NULL);
-    pDevice->byAntennaState = 1;
-    del_timer(&pDevice->TimerSQ3Tmax3);
-    del_timer(&pDevice->TimerSQ3Tmax2);
-    pDevice->TimerSQ3Tmax1.expires =  RUN_AT(pDevice->byTMax * HZ);
-    add_timer(&pDevice->TimerSQ3Tmax1);
+	if (ed_inx == priv->byBBPreEDIndex && !scanning)
+		return;
 
-    spin_unlock_irq(&pDevice->lock);
-}
+	priv->byBBPreEDIndex = ed_inx;
 
-void BBvUpdatePreEDThreshold(struct vnt_private *pDevice, int bScanning)
-{
+	dev_dbg(&priv->usb->dev, "%s byBBPreEDRSSI %d\n",
+					__func__, priv->byBBPreEDRSSI);
 
-    switch(pDevice->byRFType)
-    {
-        case RF_AL2230:
-        case RF_AL2230S:
-        case RF_AIROHA7230:
-            //RobertYu:20060627, update new table
-
-            if( bScanning )
-            {   // need Max sensitivity //RSSI -69, -70,....
-                pDevice->byBBPreEDIndex = 0;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x30); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -69, -70, -71,...\n");
-                break;
-            }
-
-            if(pDevice->byBBPreEDRSSI <= 45) { // RSSI 0, -1,-2,....-45
-                if(pDevice->byBBPreEDIndex == 20) break;
-                pDevice->byBBPreEDIndex = 20;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0xFF); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI 0, -1,-2,..-45\n");
-            } else if(pDevice->byBBPreEDRSSI <= 46)  { //RSSI -46
-                if(pDevice->byBBPreEDIndex == 19) break;
-                pDevice->byBBPreEDIndex = 19;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x1A); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -46\n");
-            } else if(pDevice->byBBPreEDRSSI <= 47)  { //RSSI -47
-                if(pDevice->byBBPreEDIndex == 18) break;
-                pDevice->byBBPreEDIndex = 18;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x15); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -47\n");
-            } else if(pDevice->byBBPreEDRSSI <= 49)  { //RSSI -48, -49
-                if(pDevice->byBBPreEDIndex == 17) break;
-                pDevice->byBBPreEDIndex = 17;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x0E); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -48,-49\n");
-            } else if(pDevice->byBBPreEDRSSI <= 51)  { //RSSI -50, -51
-                if(pDevice->byBBPreEDIndex == 16) break;
-                pDevice->byBBPreEDIndex = 16;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x09); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -50,-51\n");
-            } else if(pDevice->byBBPreEDRSSI <= 53)  { //RSSI -52, -53
-                if(pDevice->byBBPreEDIndex == 15) break;
-                pDevice->byBBPreEDIndex = 15;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x06); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -52,-53\n");
-            } else if(pDevice->byBBPreEDRSSI <= 55)  { //RSSI -54, -55
-                if(pDevice->byBBPreEDIndex == 14) break;
-                pDevice->byBBPreEDIndex = 14;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x03); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -54,-55\n");
-            } else if(pDevice->byBBPreEDRSSI <= 56)  { //RSSI -56
-                if(pDevice->byBBPreEDIndex == 13) break;
-                pDevice->byBBPreEDIndex = 13;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x02); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xA0); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -56\n");
-            } else if(pDevice->byBBPreEDRSSI <= 57)  { //RSSI -57
-                if(pDevice->byBBPreEDIndex == 12) break;
-                pDevice->byBBPreEDIndex = 12;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x02); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x20); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -57\n");
-            } else if(pDevice->byBBPreEDRSSI <= 58)  { //RSSI -58
-                if(pDevice->byBBPreEDIndex == 11) break;
-                pDevice->byBBPreEDIndex = 11;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x01); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xA0); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -58\n");
-            } else if(pDevice->byBBPreEDRSSI <= 59)  { //RSSI -59
-                if(pDevice->byBBPreEDIndex == 10) break;
-                pDevice->byBBPreEDIndex = 10;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x01); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x54); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -59\n");
-            } else if(pDevice->byBBPreEDRSSI <= 60)  { //RSSI -60
-                if(pDevice->byBBPreEDIndex == 9) break;
-                pDevice->byBBPreEDIndex = 9;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x01); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x18); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -60\n");
-            } else if(pDevice->byBBPreEDRSSI <= 61)  { //RSSI -61
-                if(pDevice->byBBPreEDIndex == 8) break;
-                pDevice->byBBPreEDIndex = 8;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xE3); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -61\n");
-            } else if(pDevice->byBBPreEDRSSI <= 62)  { //RSSI -62
-                if(pDevice->byBBPreEDIndex == 7) break;
-                pDevice->byBBPreEDIndex = 7;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xB9); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -62\n");
-            } else if(pDevice->byBBPreEDRSSI <= 63)  { //RSSI -63
-                if(pDevice->byBBPreEDIndex == 6) break;
-                pDevice->byBBPreEDIndex = 6;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x93); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -63\n");
-            } else if(pDevice->byBBPreEDRSSI <= 64)  { //RSSI -64
-                if(pDevice->byBBPreEDIndex == 5) break;
-                pDevice->byBBPreEDIndex = 5;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x79); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -64\n");
-            } else if(pDevice->byBBPreEDRSSI <= 65)  { //RSSI -65
-                if(pDevice->byBBPreEDIndex == 4) break;
-                pDevice->byBBPreEDIndex = 4;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x62); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -65\n");
-            } else if(pDevice->byBBPreEDRSSI <= 66)  { //RSSI -66
-                if(pDevice->byBBPreEDIndex == 3) break;
-                pDevice->byBBPreEDIndex = 3;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x51); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -66\n");
-            } else if(pDevice->byBBPreEDRSSI <= 67)  { //RSSI -67
-                if(pDevice->byBBPreEDIndex == 2) break;
-                pDevice->byBBPreEDIndex = 2;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x43); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -67\n");
-            } else if(pDevice->byBBPreEDRSSI <= 68)  { //RSSI -68
-                if(pDevice->byBBPreEDIndex == 1) break;
-                pDevice->byBBPreEDIndex = 1;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x36); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -68\n");
-            } else { //RSSI -69, -70,....
-                if(pDevice->byBBPreEDIndex == 0) break;
-                pDevice->byBBPreEDIndex = 0;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x30); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -69, -70,...\n");
-            }
-            break;
-
-        case RF_VT3226:
-        case RF_VT3226D0:
-            //RobertYu:20060627, update new table
-
-            if( bScanning )
-            {   // need Max sensitivity  //RSSI -69, -70, ...
-                pDevice->byBBPreEDIndex = 0;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x24); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -69, -70,..\n");
-                break;
-            }
-
-            if(pDevice->byBBPreEDRSSI <= 41) { // RSSI 0, -1,-2,....-41
-                if(pDevice->byBBPreEDIndex == 22) break;
-                pDevice->byBBPreEDIndex = 22;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0xFF); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI 0, -1,-2,..-41\n");
-            } else if(pDevice->byBBPreEDRSSI <= 42)  { //RSSI -42
-                if(pDevice->byBBPreEDIndex == 21) break;
-                pDevice->byBBPreEDIndex = 21;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x36); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -42\n");
-            } else if(pDevice->byBBPreEDRSSI <= 43)  { //RSSI -43
-                if(pDevice->byBBPreEDIndex == 20) break;
-                pDevice->byBBPreEDIndex = 20;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x26); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -43\n");
-            } else if(pDevice->byBBPreEDRSSI <= 45)  { //RSSI -44, -45
-                if(pDevice->byBBPreEDIndex == 19) break;
-                pDevice->byBBPreEDIndex = 19;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x18); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -44,-45\n");
-            } else if(pDevice->byBBPreEDRSSI <= 47)  { //RSSI -46, -47
-                if(pDevice->byBBPreEDIndex == 18) break;
-                pDevice->byBBPreEDIndex = 18;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x11); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -46,-47\n");
-            } else if(pDevice->byBBPreEDRSSI <= 49)  { //RSSI -48, -49
-                if(pDevice->byBBPreEDIndex == 17) break;
-                pDevice->byBBPreEDIndex = 17;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x0a); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -48,-49\n");
-            } else if(pDevice->byBBPreEDRSSI <= 51)  { //RSSI -50, -51
-                if(pDevice->byBBPreEDIndex == 16) break;
-                pDevice->byBBPreEDIndex = 16;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x07); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -50,-51\n");
-            } else if(pDevice->byBBPreEDRSSI <= 53)  { //RSSI -52, -53
-                if(pDevice->byBBPreEDIndex == 15) break;
-                pDevice->byBBPreEDIndex = 15;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x04); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -52,-53\n");
-            } else if(pDevice->byBBPreEDRSSI <= 55)  { //RSSI -54, -55
-                if(pDevice->byBBPreEDIndex == 14) break;
-                pDevice->byBBPreEDIndex = 14;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x02); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xC0); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -54,-55\n");
-            } else if(pDevice->byBBPreEDRSSI <= 56)  { //RSSI -56
-                if(pDevice->byBBPreEDIndex == 13) break;
-                pDevice->byBBPreEDIndex = 13;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x02); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x30); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -56\n");
-            } else if(pDevice->byBBPreEDRSSI <= 57)  { //RSSI -57
-                if(pDevice->byBBPreEDIndex == 12) break;
-                pDevice->byBBPreEDIndex = 12;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x01); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xB0); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -57\n");
-            } else if(pDevice->byBBPreEDRSSI <= 58)  { //RSSI -58
-                if(pDevice->byBBPreEDIndex == 11) break;
-                pDevice->byBBPreEDIndex = 11;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x01); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x70); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -58\n");
-            } else if(pDevice->byBBPreEDRSSI <= 59)  { //RSSI -59
-                if(pDevice->byBBPreEDIndex == 10) break;
-                pDevice->byBBPreEDIndex = 10;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x01); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x30); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -59\n");
-            } else if(pDevice->byBBPreEDRSSI <= 60)  { //RSSI -60
-                if(pDevice->byBBPreEDIndex == 9) break;
-                pDevice->byBBPreEDIndex = 9;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xEA); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -60\n");
-            } else if(pDevice->byBBPreEDRSSI <= 61)  { //RSSI -61
-                if(pDevice->byBBPreEDIndex == 8) break;
-                pDevice->byBBPreEDIndex = 8;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xC0); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -61\n");
-            } else if(pDevice->byBBPreEDRSSI <= 62)  { //RSSI -62
-                if(pDevice->byBBPreEDIndex == 7) break;
-                pDevice->byBBPreEDIndex = 7;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x9C); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -62\n");
-            } else if(pDevice->byBBPreEDRSSI <= 63)  { //RSSI -63
-                if(pDevice->byBBPreEDIndex == 6) break;
-                pDevice->byBBPreEDIndex = 6;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x80); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -63\n");
-            } else if(pDevice->byBBPreEDRSSI <= 64)  { //RSSI -64
-                if(pDevice->byBBPreEDIndex == 5) break;
-                pDevice->byBBPreEDIndex = 5;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x68); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -64\n");
-            } else if(pDevice->byBBPreEDRSSI <= 65)  { //RSSI -65
-                if(pDevice->byBBPreEDIndex == 4) break;
-                pDevice->byBBPreEDIndex = 4;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x52); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -65\n");
-            } else if(pDevice->byBBPreEDRSSI <= 66)  { //RSSI -66
-                if(pDevice->byBBPreEDIndex == 3) break;
-                pDevice->byBBPreEDIndex = 3;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x43); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -66\n");
-            } else if(pDevice->byBBPreEDRSSI <= 67)  { //RSSI -67
-                if(pDevice->byBBPreEDIndex == 2) break;
-                pDevice->byBBPreEDIndex = 2;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x36); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -67\n");
-            } else if(pDevice->byBBPreEDRSSI <= 68)  { //RSSI -68
-                if(pDevice->byBBPreEDIndex == 1) break;
-                pDevice->byBBPreEDIndex = 1;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x2D); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -68\n");
-            } else { //RSSI -69, -70, ...
-                if(pDevice->byBBPreEDIndex == 0) break;
-                pDevice->byBBPreEDIndex = 0;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x24); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -69, -70,..\n");
-            }
-            break;
-
-        case RF_VT3342A0: //RobertYu:20060627, testing table
-            if( bScanning )
-            {   // need Max sensitivity  //RSSI -67, -68, ...
-                pDevice->byBBPreEDIndex = 0;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x38); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -67, -68,..\n");
-                break;
-            }
-
-            if(pDevice->byBBPreEDRSSI <= 41) { // RSSI 0, -1,-2,....-41
-                if(pDevice->byBBPreEDIndex == 20) break;
-                pDevice->byBBPreEDIndex = 20;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0xFF); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI 0, -1,-2,..-41\n");
-            } else if(pDevice->byBBPreEDRSSI <= 42)  { //RSSI -42
-                if(pDevice->byBBPreEDIndex == 19) break;
-                pDevice->byBBPreEDIndex = 19;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x36); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -42\n");
-            } else if(pDevice->byBBPreEDRSSI <= 43)  { //RSSI -43
-                if(pDevice->byBBPreEDIndex == 18) break;
-                pDevice->byBBPreEDIndex = 18;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x26); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -43\n");
-            } else if(pDevice->byBBPreEDRSSI <= 45)  { //RSSI -44, -45
-                if(pDevice->byBBPreEDIndex == 17) break;
-                pDevice->byBBPreEDIndex = 17;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x18); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -44,-45\n");
-            } else if(pDevice->byBBPreEDRSSI <= 47)  { //RSSI -46, -47
-                if(pDevice->byBBPreEDIndex == 16) break;
-                pDevice->byBBPreEDIndex = 16;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x11); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -46,-47\n");
-            } else if(pDevice->byBBPreEDRSSI <= 49)  { //RSSI -48, -49
-                if(pDevice->byBBPreEDIndex == 15) break;
-                pDevice->byBBPreEDIndex = 15;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x0a); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -48,-49\n");
-            } else if(pDevice->byBBPreEDRSSI <= 51)  { //RSSI -50, -51
-                if(pDevice->byBBPreEDIndex == 14) break;
-                pDevice->byBBPreEDIndex = 14;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x07); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -50,-51\n");
-            } else if(pDevice->byBBPreEDRSSI <= 53)  { //RSSI -52, -53
-                if(pDevice->byBBPreEDIndex == 13) break;
-                pDevice->byBBPreEDIndex = 13;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x04); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -52,-53\n");
-            } else if(pDevice->byBBPreEDRSSI <= 55)  { //RSSI -54, -55
-                if(pDevice->byBBPreEDIndex == 12) break;
-                pDevice->byBBPreEDIndex = 12;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x02); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xC0); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -54,-55\n");
-            } else if(pDevice->byBBPreEDRSSI <= 56)  { //RSSI -56
-                if(pDevice->byBBPreEDIndex == 11) break;
-                pDevice->byBBPreEDIndex = 11;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x02); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x30); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -56\n");
-            } else if(pDevice->byBBPreEDRSSI <= 57)  { //RSSI -57
-                if(pDevice->byBBPreEDIndex == 10) break;
-                pDevice->byBBPreEDIndex = 10;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x01); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xB0); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -57\n");
-            } else if(pDevice->byBBPreEDRSSI <= 58)  { //RSSI -58
-                if(pDevice->byBBPreEDIndex == 9) break;
-                pDevice->byBBPreEDIndex = 9;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x01); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x70); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -58\n");
-            } else if(pDevice->byBBPreEDRSSI <= 59)  { //RSSI -59
-                if(pDevice->byBBPreEDIndex == 8) break;
-                pDevice->byBBPreEDIndex = 8;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x01); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x30); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -59\n");
-            } else if(pDevice->byBBPreEDRSSI <= 60)  { //RSSI -60
-                if(pDevice->byBBPreEDIndex == 7) break;
-                pDevice->byBBPreEDIndex = 7;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xEA); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -60\n");
-            } else if(pDevice->byBBPreEDRSSI <= 61)  { //RSSI -61
-                if(pDevice->byBBPreEDIndex == 6) break;
-                pDevice->byBBPreEDIndex = 6;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xC0); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -61\n");
-            } else if(pDevice->byBBPreEDRSSI <= 62)  { //RSSI -62
-                if(pDevice->byBBPreEDIndex == 5) break;
-                pDevice->byBBPreEDIndex = 5;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x9C); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -62\n");
-            } else if(pDevice->byBBPreEDRSSI <= 63)  { //RSSI -63
-                if(pDevice->byBBPreEDIndex == 4) break;
-                pDevice->byBBPreEDIndex = 4;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x80); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -63\n");
-            } else if(pDevice->byBBPreEDRSSI <= 64)  { //RSSI -64
-                if(pDevice->byBBPreEDIndex == 3) break;
-                pDevice->byBBPreEDIndex = 3;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x68); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -64\n");
-            } else if(pDevice->byBBPreEDRSSI <= 65)  { //RSSI -65
-                if(pDevice->byBBPreEDIndex == 2) break;
-                pDevice->byBBPreEDIndex = 2;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x52); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -65\n");
-            } else if(pDevice->byBBPreEDRSSI <= 66)  { //RSSI -66
-                if(pDevice->byBBPreEDIndex == 1) break;
-                pDevice->byBBPreEDIndex = 1;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x43); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -66\n");
-            } else { //RSSI -67, -68, ...
-                if(pDevice->byBBPreEDIndex == 0) break;
-                pDevice->byBBPreEDIndex = 0;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x38); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -67, -68,..\n");
-            }
-            break;
-
-    }
+	if (!cr_201 && !cr_206)
+		return;
 
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0xc9, cr_201);
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0xce, cr_206);
 }
 
diff --git a/drivers/staging/vt6656/baseband.h b/drivers/staging/vt6656/baseband.h
index 79faedf4a5e..3044d6c4205 100644
--- a/drivers/staging/vt6656/baseband.h
+++ b/drivers/staging/vt6656/baseband.h
@@ -88,23 +88,11 @@ struct vnt_phy_field {
 	__le16 len;
 } __packed;
 
-unsigned int
-BBuGetFrameTime(
-     u8 byPreambleType,
-     u8 byFreqType,
-     unsigned int cbFrameLength,
-     u16 wRate
-    );
+unsigned int BBuGetFrameTime(u8 preamble_type, u8 pkt_type,
+	unsigned int frame_length, u16 tx_rate);
 
-void BBvCalculateParameter(struct vnt_private *, u32 cbFrameLength,
-	u16 wRate, u8 byPacketType, struct vnt_phy_field *);
-
-/* timer for antenna diversity */
-
-void TimerSQ3CallBack(struct vnt_private *);
-void TimerSQ3Tmax3CallBack(struct vnt_private *);
-
-void BBvAntennaDiversity(struct vnt_private *, u8 byRxRate, u8 bySQ3);
+void BBvCalculateParameter(struct vnt_private *, u32 frame_length,
+	u16 tx_rate, u8 pkt_type, struct vnt_phy_field *);
 
 void BBvSetShortSlotTime(struct vnt_private *);
 void BBvSetVGAGainOffset(struct vnt_private *, u8 byData);
@@ -112,6 +100,6 @@ void BBvSetAntennaMode(struct vnt_private *, u8 byAntennaMode);
 int BBbVT3184Init(struct vnt_private *);
 void BBvSetDeepSleep(struct vnt_private *);
 void BBvExitDeepSleep(struct vnt_private *);
-void BBvUpdatePreEDThreshold(struct vnt_private *, int bScanning);
+void BBvUpdatePreEDThreshold(struct vnt_private *, int scanning);
 
 #endif /* __BASEBAND_H__ */
diff --git a/drivers/staging/vt6656/bssdb.c b/drivers/staging/vt6656/bssdb.c
index 9c78dab95d3..8e9ce96442a 100644
--- a/drivers/staging/vt6656/bssdb.c
+++ b/drivers/staging/vt6656/bssdb.c
@@ -53,8 +53,7 @@
 #include "card.h"
 #include "mac.h"
 #include "wpa2.h"
-#include "control.h"
-#include "rndis.h"
+#include "usbpipe.h"
 #include "iowpa.h"
 #include "power.h"
 
@@ -456,16 +455,15 @@ int BSSbInsertToBSSList(struct vnt_private *pDevice,
 		}
 	}
 
-	if (pDevice->bUpdateBBVGA) {
-		/* Monitor if RSSI is too strong. */
-		pBSSList->byRSSIStatCnt = 0;
-		RFvRSSITodBm(pDevice, (u8) (pRxPacket->uRSSI),
-			     &pBSSList->ldBmMAX);
-		pBSSList->ldBmAverage[0] = pBSSList->ldBmMAX;
-		pBSSList->ldBmAverRange = pBSSList->ldBmMAX;
-		for (ii = 1; ii < RSSI_STAT_COUNT; ii++)
-			pBSSList->ldBmAverage[ii] = 0;
-	}
+	/* Monitor if RSSI is too strong. */
+	pBSSList->byRSSIStatCnt = 0;
+
+	vnt_rf_rssi_to_dbm(pDevice, (u8)pRxPacket->uRSSI, &pBSSList->ldBmMAX);
+
+	pBSSList->ldBmAverage[0] = pBSSList->ldBmMAX;
+	pBSSList->ldBmAverRange = pBSSList->ldBmMAX;
+	for (ii = 1; ii < RSSI_STAT_COUNT; ii++)
+		pBSSList->ldBmAverage[ii] = 0;
 
 	pBSSList->uIELength = uIELength;
 	if (pBSSList->uIELength > WLAN_BEACON_FR_MAXLEN)
@@ -581,7 +579,7 @@ int BSSbUpdateToBSSList(struct vnt_private *pDevice,
 	}
 
 	if (pRxPacket->uRSSI != 0) {
-		RFvRSSITodBm(pDevice, (u8) (pRxPacket->uRSSI), &ldBm);
+		vnt_rf_rssi_to_dbm(pDevice, (u8)pRxPacket->uRSSI, &ldBm);
 		/* Monitor if RSSI is too strong. */
 		pBSSList->byRSSIStatCnt++;
 		pBSSList->byRSSIStatCnt %= RSSI_STAT_COUNT;
@@ -774,8 +772,8 @@ void BSSvAddMulticastNode(struct vnt_private *pDevice)
 {
 	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
 
-	if (!pDevice->bEnableHostWEP)
-		memset(&pMgmt->sNodeDBTable[0], 0, sizeof(KnownNodeDB));
+	memset(&pMgmt->sNodeDBTable[0], 0, sizeof(KnownNodeDB));
+
 	memset(pMgmt->sNodeDBTable[0].abyMACAddr, 0xff, WLAN_ADDR_LEN);
 	pMgmt->sNodeDBTable[0].bActive = true;
 	pMgmt->sNodeDBTable[0].bPSEnable = false;
@@ -818,8 +816,6 @@ void BSSvSecondCallBack(struct work_struct *work)
 	if (pDevice->Flags & fMP_DISCONNECTED)
 		return;
 
-	spin_lock_irq(&pDevice->lock);
-
 	pDevice->uAssocCount = 0;
 
 	/* Power Saving Mode Tx Burst */
@@ -1001,10 +997,8 @@ void BSSvSecondCallBack(struct work_struct *work)
 
 		if (pMgmt->sNodeDBTable[0].bActive) { /* Assoc with BSS */
 
-			if (pDevice->bUpdateBBVGA) {
-				s_vCheckSensitivity(pDevice);
-				s_vCheckPreEDThreshold(pDevice);
-			}
+			s_vCheckSensitivity(pDevice);
+			s_vCheckPreEDThreshold(pDevice);
 
 			if (pMgmt->sNodeDBTable[0].uInActiveCount >=
 							(LOST_BEACON_COUNT/2) &&
@@ -1022,10 +1016,10 @@ void BSSvSecondCallBack(struct work_struct *work)
 				pMgmt->eCurrState = WMAC_STATE_IDLE;
 				netif_stop_queue(pDevice->dev);
 				pDevice->bLinkPass = false;
-				ControlvMaskByte(pDevice,
-						 MESSAGE_REQUEST_MACREG,
-						 MAC_REG_PAPEDELAY, LEDSTS_STS,
-						 LEDSTS_SLOW);
+
+				vnt_mac_set_led(pDevice, LEDSTS_STS,
+								LEDSTS_SLOW);
+
 				pDevice->bRoaming = true;
 				pDevice->bIsRoaming = false;
 
@@ -1121,10 +1115,9 @@ void BSSvSecondCallBack(struct work_struct *work)
 		}
 		if (pMgmt->eCurrState == WMAC_STATE_JOINTED) {
 
-			if (pDevice->bUpdateBBVGA) {
-				s_vCheckSensitivity(pDevice);
-				s_vCheckPreEDThreshold(pDevice);
-			}
+			s_vCheckSensitivity(pDevice);
+			s_vCheckPreEDThreshold(pDevice);
+
 			if (pMgmt->sNodeDBTable[0].uInActiveCount >=
 						ADHOC_LOST_BEACON_COUNT) {
 				DBG_PRT(MSG_LEVEL_NOTICE,
@@ -1134,10 +1127,8 @@ void BSSvSecondCallBack(struct work_struct *work)
 				pMgmt->eCurrState = WMAC_STATE_STARTED;
 				netif_stop_queue(pDevice->dev);
 				pDevice->bLinkPass = false;
-				ControlvMaskByte(pDevice,
-						 MESSAGE_REQUEST_MACREG,
-						 MAC_REG_PAPEDELAY, LEDSTS_STS,
-						 LEDSTS_SLOW);
+				vnt_mac_set_led(pDevice, LEDSTS_STS,
+								LEDSTS_SLOW);
 			}
 		}
 	}
@@ -1158,8 +1149,6 @@ void BSSvSecondCallBack(struct work_struct *work)
 			netif_wake_queue(pDevice->dev);
 	}
 
-	spin_unlock_irq(&pDevice->lock);
-
 	schedule_delayed_work(&pDevice->second_callback_work, HZ);
 }
 
@@ -1429,7 +1418,7 @@ static void s_uCalculateLinkQual(struct vnt_private *pDevice)
 	if (pDevice->bLinkPass != true) {
 		pDevice->wstats.qual.qual = 0;
 	} else {
-		RFvRSSITodBm(pDevice, (u8) (pDevice->uCurrRSSI), &ldBm);
+		vnt_rf_rssi_to_dbm(pDevice, (u8) (pDevice->uCurrRSSI), &ldBm);
 		if (-ldBm < 50)
 			RssiRatio = 4000;
 		else if (-ldBm > 90)
diff --git a/drivers/staging/vt6656/card.c b/drivers/staging/vt6656/card.c
index 0d8772858f0..d662e5431da 100644
--- a/drivers/staging/vt6656/card.c
+++ b/drivers/staging/vt6656/card.c
@@ -57,11 +57,7 @@
 #include "rc4.h"
 #include "country.h"
 #include "datarate.h"
-#include "rndis.h"
-#include "control.h"
-
-//static int          msglevel                =MSG_LEVEL_DEBUG;
-static int          msglevel                =MSG_LEVEL_INFO;
+#include "usbpipe.h"
 
 //const u16 cwRXBCNTSFOff[MAX_RATE] =
 //{17, 34, 96, 192, 34, 23, 17, 11, 8, 5, 4, 3};
@@ -98,24 +94,24 @@ void CARDbSetMediaChannel(struct vnt_private *priv, u32 connection_channel)
 	/* Set Channel[7] = 0 to tell H/W channel is changing now. */
 	MACvRegBitsOff(priv, MAC_REG_CHANNEL, 0xb0);
 
-	CONTROLnsRequestOut(priv, MESSAGE_TYPE_SELECT_CHANNLE,
+	vnt_control_out(priv, MESSAGE_TYPE_SELECT_CHANNLE,
 					connection_channel, 0, 0, NULL);
 
 	if (priv->byBBType == BB_TYPE_11A) {
 		priv->byCurPwr = 0xff;
-		RFbRawSetPower(priv,
+		vnt_rf_set_txpower(priv,
 			priv->abyOFDMAPwrTbl[connection_channel-15], RATE_54M);
 	} else if (priv->byBBType == BB_TYPE_11G) {
 		priv->byCurPwr = 0xff;
-		RFbRawSetPower(priv,
+		vnt_rf_set_txpower(priv,
 			priv->abyOFDMPwrTbl[connection_channel-1], RATE_54M);
 	} else {
 		priv->byCurPwr = 0xff;
-		RFbRawSetPower(priv,
+		vnt_rf_set_txpower(priv,
 			priv->abyCCKPwrTbl[connection_channel-1], RATE_1M);
 	}
 
-	ControlvWriteByte(priv, MESSAGE_REQUEST_MACREG, MAC_REG_CHANNEL,
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_MACREG, MAC_REG_CHANNEL,
 		(u8)(connection_channel|0x80));
 }
 
@@ -124,20 +120,20 @@ void CARDbSetMediaChannel(struct vnt_private *priv, u32 connection_channel)
  *
  * Parameters:
  *  In:
- *      pDevice             - The adapter to be set
- *      wRateIdx            - Receiving data rate
+ *      priv		- The adapter to be set
+ *      rate_idx	- Receiving data rate
  *  Out:
  *      none
  *
  * Return Value: response Control frame rate
  *
  */
-static u16 swGetCCKControlRate(struct vnt_private *pDevice, u16 wRateIdx)
+static u16 swGetCCKControlRate(struct vnt_private *priv, u16 rate_idx)
 {
-	u16 ui = wRateIdx;
+	u16 ui = rate_idx;
 
 	while (ui > RATE_1M) {
-		if (pDevice->wBasicRate & (1 << ui))
+		if (priv->wBasicRate & (1 << ui))
 			return ui;
 		ui--;
 	}
@@ -150,39 +146,39 @@ static u16 swGetCCKControlRate(struct vnt_private *pDevice, u16 wRateIdx)
  *
  * Parameters:
  *  In:
- *      pDevice             - The adapter to be set
- *      wRateIdx            - Receiving data rate
+ *      priv		- The adapter to be set
+ *      rate_idx	- Receiving data rate
  *  Out:
  *      none
  *
  * Return Value: response Control frame rate
  *
  */
-static u16 swGetOFDMControlRate(struct vnt_private *pDevice, u16 wRateIdx)
+static u16 swGetOFDMControlRate(struct vnt_private *priv, u16 rate_idx)
 {
-	u16 ui = wRateIdx;
+	u16 ui = rate_idx;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"BASIC RATE: %X\n",
-		pDevice->wBasicRate);
+	dev_dbg(&priv->usb->dev, "%s basic rate: %d\n",
+					__func__,  priv->wBasicRate);
 
-	if (!CARDbIsOFDMinBasicRate(pDevice)) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-			"swGetOFDMControlRate:(NO OFDM) %d\n", wRateIdx);
-		if (wRateIdx > RATE_24M)
-			wRateIdx = RATE_24M;
-		return wRateIdx;
+	if (!CARDbIsOFDMinBasicRate(priv)) {
+		dev_dbg(&priv->usb->dev, "%s (NO OFDM) %d\n",
+						__func__, rate_idx);
+		if (rate_idx > RATE_24M)
+			rate_idx = RATE_24M;
+		return rate_idx;
 	}
 
 	while (ui > RATE_11M) {
-		if (pDevice->wBasicRate & (1 << ui)) {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-				"swGetOFDMControlRate: %d\n", ui);
+		if (priv->wBasicRate & (1 << ui)) {
+			dev_dbg(&priv->usb->dev, "%s rate: %d\n",
+							__func__, ui);
 			return ui;
 		}
 		ui--;
 	}
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"swGetOFDMControlRate: 6M\n");
+	dev_dbg(&priv->usb->dev, "%s basic rate: 24M\n", __func__);
 
 	return RATE_24M;
 }
@@ -191,114 +187,95 @@ static u16 swGetOFDMControlRate(struct vnt_private *pDevice, u16 wRateIdx)
  * Description: Calculate TxRate and RsvTime fields for RSPINF in OFDM mode.
  *
  * Parameters:
- *  In:
- *      wRate           - Tx Rate
- *      byPktType       - Tx Packet type
- *  Out:
- *      pbyTxRate       - pointer to RSPINF TxRate field
- *      pbyRsvTime      - pointer to RSPINF RsvTime field
+ * In:
+ *	rate	- Tx Rate
+ *	bb_type	- Tx Packet type
+ * Out:
+ *	tx_rate	- pointer to RSPINF TxRate field
+ *	rsv_time- pointer to RSPINF RsvTime field
  *
  * Return Value: none
  *
  */
-static void
-CARDvCalculateOFDMRParameter (
-      u16 wRate,
-      u8 byBBType,
-     u8 * pbyTxRate,
-     u8 * pbyRsvTime
-    )
+static void CARDvCalculateOFDMRParameter(u16 rate, u8 bb_type,
+					u8 *tx_rate, u8 *rsv_time)
 {
-    switch (wRate) {
-    case RATE_6M :
-        if (byBBType == BB_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x9B;
-            *pbyRsvTime = 24;
-        }
-        else {
-            *pbyTxRate = 0x8B;
-            *pbyRsvTime = 30;
-        }
-        break;
-
-    case RATE_9M :
-        if (byBBType == BB_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x9F;
-            *pbyRsvTime = 16;
-        }
-        else {
-            *pbyTxRate = 0x8F;
-            *pbyRsvTime = 22;
-        }
-        break;
-
-   case RATE_12M :
-        if (byBBType == BB_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x9A;
-            *pbyRsvTime = 12;
-        }
-        else {
-            *pbyTxRate = 0x8A;
-            *pbyRsvTime = 18;
-        }
-        break;
-
-   case RATE_18M :
-        if (byBBType == BB_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x9E;
-            *pbyRsvTime = 8;
-        }
-        else {
-            *pbyTxRate = 0x8E;
-            *pbyRsvTime = 14;
-        }
-        break;
-
-    case RATE_36M :
-        if (byBBType == BB_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x9D;
-            *pbyRsvTime = 4;
-        }
-        else {
-            *pbyTxRate = 0x8D;
-            *pbyRsvTime = 10;
-        }
-        break;
-
-    case RATE_48M :
-        if (byBBType == BB_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x98;
-            *pbyRsvTime = 4;
-        }
-        else {
-            *pbyTxRate = 0x88;
-            *pbyRsvTime = 10;
-        }
-        break;
-
-    case RATE_54M :
-        if (byBBType == BB_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x9C;
-            *pbyRsvTime = 4;
-        }
-        else {
-            *pbyTxRate = 0x8C;
-            *pbyRsvTime = 10;
-        }
-        break;
-
-    case RATE_24M :
-    default :
-        if (byBBType == BB_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x99;
-            *pbyRsvTime = 8;
-        }
-        else {
-            *pbyTxRate = 0x89;
-            *pbyRsvTime = 14;
-        }
-        break;
-    }
+
+	switch (rate) {
+	case RATE_6M:
+		if (bb_type == BB_TYPE_11A) {
+			*tx_rate = 0x9b;
+			*rsv_time = 24;
+		} else {
+			*tx_rate = 0x8b;
+			*rsv_time = 30;
+		}
+			break;
+	case RATE_9M:
+		if (bb_type == BB_TYPE_11A) {
+			*tx_rate = 0x9f;
+			*rsv_time = 16;
+		} else {
+			*tx_rate = 0x8f;
+			*rsv_time = 22;
+		}
+		break;
+	case RATE_12M:
+		if (bb_type == BB_TYPE_11A) {
+			*tx_rate = 0x9a;
+			*rsv_time = 12;
+		} else {
+			*tx_rate = 0x8a;
+			*rsv_time = 18;
+		}
+		break;
+	case RATE_18M:
+		if (bb_type == BB_TYPE_11A) {
+			*tx_rate = 0x9e;
+			*rsv_time = 8;
+		} else {
+			*tx_rate = 0x8e;
+			*rsv_time = 14;
+		}
+		break;
+	case RATE_36M:
+		if (bb_type == BB_TYPE_11A) {
+			*tx_rate = 0x9d;
+			*rsv_time = 4;
+		} else {
+			*tx_rate = 0x8d;
+			*rsv_time = 10;
+		}
+		break;
+	case RATE_48M:
+		if (bb_type == BB_TYPE_11A) {
+			*tx_rate = 0x98;
+			*rsv_time = 4;
+		} else {
+			*tx_rate = 0x88;
+			*rsv_time = 10;
+		}
+		break;
+	case RATE_54M:
+		if (bb_type == BB_TYPE_11A) {
+			*tx_rate = 0x9c;
+			*rsv_time = 4;
+		} else {
+			*tx_rate = 0x8c;
+			*rsv_time = 10;
+		}
+		break;
+	case RATE_24M:
+	default:
+		if (bb_type == BB_TYPE_11A) {
+			*tx_rate = 0x99;
+			*rsv_time = 8;
+		} else {
+			*tx_rate = 0x89;
+			*rsv_time = 14;
+		}
+		break;
+	}
 }
 
 /*
@@ -313,112 +290,91 @@ CARDvCalculateOFDMRParameter (
  * Return Value: None.
  *
  */
-void CARDvSetRSPINF(struct vnt_private *pDevice, u8 byBBType)
+
+void CARDvSetRSPINF(struct vnt_private *priv, u8 bb_type)
 {
 	struct vnt_phy_field phy[4];
-	u8 abyTxRate[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; /* For OFDM */
-	u8 abyRsvTime[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
-	u8 abyData[34];
+	u8 tx_rate[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; /* For OFDM */
+	u8 rsv_time[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
+	u8 data[34];
 	int i;
 
-    //RSPINF_b_1
-	BBvCalculateParameter(pDevice, 14,
-		swGetCCKControlRate(pDevice, RATE_1M), PK_TYPE_11B, &phy[0]);
-
-    ///RSPINF_b_2
-	BBvCalculateParameter(pDevice, 14,
-		swGetCCKControlRate(pDevice, RATE_2M), PK_TYPE_11B, &phy[1]);
-
-    //RSPINF_b_5
-	BBvCalculateParameter(pDevice, 14,
-		swGetCCKControlRate(pDevice, RATE_5M), PK_TYPE_11B, &phy[2]);
-
-    //RSPINF_b_11
-	BBvCalculateParameter(pDevice, 14,
-		swGetCCKControlRate(pDevice, RATE_11M), PK_TYPE_11B, &phy[3]);
-
-    //RSPINF_a_6
-    CARDvCalculateOFDMRParameter (RATE_6M,
-                                 byBBType,
-                                 &abyTxRate[0],
-                                 &abyRsvTime[0]);
-
-    //RSPINF_a_9
-    CARDvCalculateOFDMRParameter (RATE_9M,
-                                 byBBType,
-                                 &abyTxRate[1],
-                                 &abyRsvTime[1]);
-
-    //RSPINF_a_12
-    CARDvCalculateOFDMRParameter (RATE_12M,
-                                 byBBType,
-                                 &abyTxRate[2],
-                                 &abyRsvTime[2]);
-
-    //RSPINF_a_18
-    CARDvCalculateOFDMRParameter (RATE_18M,
-                                 byBBType,
-                                 &abyTxRate[3],
-                                 &abyRsvTime[3]);
-
-    //RSPINF_a_24
-    CARDvCalculateOFDMRParameter (RATE_24M,
-                                 byBBType,
-                                 &abyTxRate[4],
-                                 &abyRsvTime[4]);
-
-    //RSPINF_a_36
-    CARDvCalculateOFDMRParameter (swGetOFDMControlRate(pDevice, RATE_36M),
-                                 byBBType,
-                                 &abyTxRate[5],
-                                 &abyRsvTime[5]);
-
-    //RSPINF_a_48
-    CARDvCalculateOFDMRParameter (swGetOFDMControlRate(pDevice, RATE_48M),
-                                 byBBType,
-                                 &abyTxRate[6],
-                                 &abyRsvTime[6]);
-
-    //RSPINF_a_54
-    CARDvCalculateOFDMRParameter (swGetOFDMControlRate(pDevice, RATE_54M),
-                                 byBBType,
-                                 &abyTxRate[7],
-                                 &abyRsvTime[7]);
-
-    //RSPINF_a_72
-    CARDvCalculateOFDMRParameter (swGetOFDMControlRate(pDevice, RATE_54M),
-                                 byBBType,
-                                 &abyTxRate[8],
-                                 &abyRsvTime[8]);
-
-	put_unaligned(phy[0].len, (u16 *)&abyData[0]);
-	abyData[2] = phy[0].signal;
-	abyData[3] = phy[0].service;
-
-	put_unaligned(phy[1].len, (u16 *)&abyData[4]);
-	abyData[6] = phy[1].signal;
-	abyData[7] = phy[1].service;
-
-	put_unaligned(phy[2].len, (u16 *)&abyData[8]);
-	abyData[10] = phy[2].signal;
-	abyData[11] = phy[2].service;
-
-	put_unaligned(phy[3].len, (u16 *)&abyData[12]);
-	abyData[14] = phy[3].signal;
-	abyData[15] = phy[3].service;
-
-    for (i = 0; i < 9; i++) {
-	abyData[16+i*2] = abyTxRate[i];
-	abyData[16+i*2+1] = abyRsvTime[i];
-    }
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE,
-                        MAC_REG_RSPINF_B_1,
-                        MESSAGE_REQUEST_MACREG,
-                        34,
-                        &abyData[0]);
+	/*RSPINF_b_1*/
+	BBvCalculateParameter(priv, 14,
+		swGetCCKControlRate(priv, RATE_1M), PK_TYPE_11B, &phy[0]);
+
+	/*RSPINF_b_2*/
+	BBvCalculateParameter(priv, 14,
+		swGetCCKControlRate(priv, RATE_2M), PK_TYPE_11B, &phy[1]);
+
+	/*RSPINF_b_5*/
+	BBvCalculateParameter(priv, 14,
+		swGetCCKControlRate(priv, RATE_5M), PK_TYPE_11B, &phy[2]);
+
+	/*RSPINF_b_11*/
+	BBvCalculateParameter(priv, 14,
+		swGetCCKControlRate(priv, RATE_11M), PK_TYPE_11B, &phy[3]);
+
+
+	/*RSPINF_a_6*/
+	CARDvCalculateOFDMRParameter(RATE_6M, bb_type,
+						&tx_rate[0], &rsv_time[0]);
 
+	/*RSPINF_a_9*/
+	CARDvCalculateOFDMRParameter(RATE_9M, bb_type,
+						&tx_rate[1], &rsv_time[1]);
+
+	/*RSPINF_a_12*/
+	CARDvCalculateOFDMRParameter(RATE_12M, bb_type,
+						&tx_rate[2], &rsv_time[2]);
+
+	/*RSPINF_a_18*/
+	CARDvCalculateOFDMRParameter(RATE_18M, bb_type,
+						&tx_rate[3], &rsv_time[3]);
+
+	/*RSPINF_a_24*/
+	CARDvCalculateOFDMRParameter(RATE_24M, bb_type,
+						&tx_rate[4], &rsv_time[4]);
+
+	/*RSPINF_a_36*/
+	CARDvCalculateOFDMRParameter(swGetOFDMControlRate(priv, RATE_36M),
+					bb_type, &tx_rate[5], &rsv_time[5]);
+
+	/*RSPINF_a_48*/
+	CARDvCalculateOFDMRParameter(swGetOFDMControlRate(priv, RATE_48M),
+					bb_type, &tx_rate[6], &rsv_time[6]);
+
+	/*RSPINF_a_54*/
+	CARDvCalculateOFDMRParameter(swGetOFDMControlRate(priv, RATE_54M),
+					bb_type, &tx_rate[7], &rsv_time[7]);
+
+	/*RSPINF_a_72*/
+	CARDvCalculateOFDMRParameter(swGetOFDMControlRate(priv, RATE_54M),
+					bb_type, &tx_rate[8], &rsv_time[8]);
+
+	put_unaligned(phy[0].len, (u16 *)&data[0]);
+	data[2] = phy[0].signal;
+	data[3] = phy[0].service;
+
+	put_unaligned(phy[1].len, (u16 *)&data[4]);
+	data[6] = phy[1].signal;
+	data[7] = phy[1].service;
+
+	put_unaligned(phy[2].len, (u16 *)&data[8]);
+	data[10] = phy[2].signal;
+	data[11] = phy[2].service;
+
+	put_unaligned(phy[3].len, (u16 *)&data[12]);
+	data[14] = phy[3].signal;
+	data[15] = phy[3].service;
+
+	for (i = 0; i < 9; i++) {
+		data[16 + i * 2] = tx_rate[i];
+		data[16 + i * 2 + 1] = rsv_time[i];
+	}
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE,
+		MAC_REG_RSPINF_B_1, MESSAGE_REQUEST_MACREG, 34, &data[0]);
 }
 
 /*
@@ -426,120 +382,119 @@ void CARDvSetRSPINF(struct vnt_private *pDevice, u8 byBBType)
  *
  * Parameters:
  *  In:
- *      pDevice             - The adapter to be set
- *  Out:
- *      none
+ *	priv - The adapter to be set
+ * Out:
+ *	none
  *
  * Return Value: None.
  *
  */
-void vUpdateIFS(struct vnt_private *pDevice)
+void vUpdateIFS(struct vnt_private *priv)
 {
-	u8 byMaxMin = 0;
-	u8 byData[4];
-
-    if (pDevice->byPacketType==PK_TYPE_11A) {//0000 0000 0000 0000,11a
-        pDevice->uSlot = C_SLOT_SHORT;
-        pDevice->uSIFS = C_SIFS_A;
-        pDevice->uDIFS = C_SIFS_A + 2*C_SLOT_SHORT;
-        pDevice->uCwMin = C_CWMIN_A;
-        byMaxMin = 4;
-    }
-    else if (pDevice->byPacketType==PK_TYPE_11B) {//0000 0001 0000 0000,11b
-        pDevice->uSlot = C_SLOT_LONG;
-        pDevice->uSIFS = C_SIFS_BG;
-        pDevice->uDIFS = C_SIFS_BG + 2*C_SLOT_LONG;
-          pDevice->uCwMin = C_CWMIN_B;
-        byMaxMin = 5;
-    }
-    else {// PK_TYPE_11GA & PK_TYPE_11GB
-        u8 byRate = 0;
-        bool bOFDMRate = false;
-	unsigned int ii = 0;
-        PWLAN_IE_SUPP_RATES pItemRates = NULL;
-
-        pDevice->uSIFS = C_SIFS_BG;
-        if (pDevice->bShortSlotTime) {
-            pDevice->uSlot = C_SLOT_SHORT;
-        } else {
-            pDevice->uSlot = C_SLOT_LONG;
-        }
-        pDevice->uDIFS = C_SIFS_BG + 2*pDevice->uSlot;
-
-	pItemRates = (PWLAN_IE_SUPP_RATES)pDevice->vnt_mgmt.abyCurrSuppRates;
-        for (ii = 0; ii < pItemRates->len; ii++) {
-            byRate = (u8)(pItemRates->abyRates[ii]&0x7F);
-            if (RATEwGetRateIdx(byRate) > RATE_11M) {
-                bOFDMRate = true;
-                break;
-            }
-        }
-        if (bOFDMRate == false) {
-		pItemRates = (PWLAN_IE_SUPP_RATES)pDevice->vnt_mgmt
-			.abyCurrExtSuppRates;
-            for (ii = 0; ii < pItemRates->len; ii++) {
-                byRate = (u8)(pItemRates->abyRates[ii]&0x7F);
-                if (RATEwGetRateIdx(byRate) > RATE_11M) {
-                    bOFDMRate = true;
-                    break;
-                }
-            }
-        }
-        if (bOFDMRate == true) {
-            pDevice->uCwMin = C_CWMIN_A;
-            byMaxMin = 4;
-        } else {
-            pDevice->uCwMin = C_CWMIN_B;
-            byMaxMin = 5;
-        }
-    }
-
-    pDevice->uCwMax = C_CWMAX;
-    pDevice->uEIFS = C_EIFS;
-
-    byData[0] = (u8)pDevice->uSIFS;
-    byData[1] = (u8)pDevice->uDIFS;
-    byData[2] = (u8)pDevice->uEIFS;
-    byData[3] = (u8)pDevice->uSlot;
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE,
-                        MAC_REG_SIFS,
-                        MESSAGE_REQUEST_MACREG,
-                        4,
-                        &byData[0]);
-
-    byMaxMin |= 0xA0;//1010 1111,C_CWMAX = 1023
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE,
-                        MAC_REG_CWMAXMIN0,
-                        MESSAGE_REQUEST_MACREG,
-                        1,
-                        &byMaxMin);
+	u8 max_min = 0;
+	u8 data[4];
+
+	if (priv->byPacketType == PK_TYPE_11A) {
+		priv->uSlot = C_SLOT_SHORT;
+		priv->uSIFS = C_SIFS_A;
+		priv->uDIFS = C_SIFS_A + 2 * C_SLOT_SHORT;
+		priv->uCwMin = C_CWMIN_A;
+		max_min = 4;
+	} else if (priv->byPacketType == PK_TYPE_11B) {
+		priv->uSlot = C_SLOT_LONG;
+		priv->uSIFS = C_SIFS_BG;
+		priv->uDIFS = C_SIFS_BG + 2 * C_SLOT_LONG;
+		priv->uCwMin = C_CWMIN_B;
+		max_min = 5;
+	} else {/* PK_TYPE_11GA & PK_TYPE_11GB */
+		u8 rate = 0;
+		bool ofdm_rate = false;
+		unsigned int ii = 0;
+		PWLAN_IE_SUPP_RATES item_rates = NULL;
+
+		priv->uSIFS = C_SIFS_BG;
+
+		if (priv->bShortSlotTime)
+			priv->uSlot = C_SLOT_SHORT;
+		else
+			priv->uSlot = C_SLOT_LONG;
+
+		priv->uDIFS = C_SIFS_BG + 2 * priv->uSlot;
+
+		item_rates =
+			(PWLAN_IE_SUPP_RATES)priv->vnt_mgmt.abyCurrSuppRates;
+
+		for (ii = 0; ii < item_rates->len; ii++) {
+			rate = (u8)(item_rates->abyRates[ii] & 0x7f);
+			if (RATEwGetRateIdx(rate) > RATE_11M) {
+				ofdm_rate = true;
+				break;
+			}
+		}
+
+		if (ofdm_rate == false) {
+			item_rates = (PWLAN_IE_SUPP_RATES)priv->vnt_mgmt
+				.abyCurrExtSuppRates;
+			for (ii = 0; ii < item_rates->len; ii++) {
+				rate = (u8)(item_rates->abyRates[ii] & 0x7f);
+				if (RATEwGetRateIdx(rate) > RATE_11M) {
+					ofdm_rate = true;
+					break;
+				}
+			}
+		}
+
+		if (ofdm_rate == true) {
+			priv->uCwMin = C_CWMIN_A;
+			max_min = 4;
+		} else {
+			priv->uCwMin = C_CWMIN_B;
+			max_min = 5;
+			}
+	}
+
+	priv->uCwMax = C_CWMAX;
+	priv->uEIFS = C_EIFS;
+
+	data[0] = (u8)priv->uSIFS;
+	data[1] = (u8)priv->uDIFS;
+	data[2] = (u8)priv->uEIFS;
+	data[3] = (u8)priv->uSlot;
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_SIFS,
+		MESSAGE_REQUEST_MACREG, 4, &data[0]);
+
+	max_min |= 0xa0;
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_CWMAXMIN0,
+		MESSAGE_REQUEST_MACREG, 1, &max_min);
 }
 
-void CARDvUpdateBasicTopRate(struct vnt_private *pDevice)
+void CARDvUpdateBasicTopRate(struct vnt_private *priv)
 {
-	u8 byTopOFDM = RATE_24M, byTopCCK = RATE_1M;
-	u8 ii;
-
-     //Determines the highest basic rate.
-     for (ii = RATE_54M; ii >= RATE_6M; ii --) {
-         if ( (pDevice->wBasicRate) & ((u16)(1<<ii)) ) {
-             byTopOFDM = ii;
-             break;
-         }
-     }
-     pDevice->byTopOFDMBasicRate = byTopOFDM;
-
-     for (ii = RATE_11M;; ii --) {
-         if ( (pDevice->wBasicRate) & ((u16)(1<<ii)) ) {
-             byTopCCK = ii;
-             break;
-         }
-         if (ii == RATE_1M)
-            break;
-     }
-     pDevice->byTopCCKBasicRate = byTopCCK;
+	u8 top_ofdm = RATE_24M, top_cck = RATE_1M;
+	u8 i;
+
+	/*Determines the highest basic rate.*/
+	for (i = RATE_54M; i >= RATE_6M; i--) {
+		if (priv->wBasicRate & (u16)(1 << i)) {
+			top_ofdm = i;
+			break;
+		}
+	}
+
+	priv->byTopOFDMBasicRate = top_ofdm;
+
+	for (i = RATE_11M;; i--) {
+		if (priv->wBasicRate & (u16)(1 << i)) {
+			top_cck = i;
+			break;
+		}
+		if (i == RATE_1M)
+			break;
+	}
+
+	priv->byTopCCKBasicRate = top_cck;
  }
 
 /*
@@ -555,39 +510,36 @@ void CARDvUpdateBasicTopRate(struct vnt_private *pDevice)
  * Return Value: true if succeeded; false if failed.
  *
  */
-void CARDbAddBasicRate(struct vnt_private *pDevice, u16 wRateIdx)
+void CARDbAddBasicRate(struct vnt_private *priv, u16 rate_idx)
 {
-	u16 wRate = (1 << wRateIdx);
 
-    pDevice->wBasicRate |= wRate;
+	priv->wBasicRate |= (1 << rate_idx);
 
-    //Determines the highest basic rate.
-    CARDvUpdateBasicTopRate(pDevice);
+	/*Determines the highest basic rate.*/
+	CARDvUpdateBasicTopRate(priv);
 }
 
-int CARDbIsOFDMinBasicRate(struct vnt_private *pDevice)
+int CARDbIsOFDMinBasicRate(struct vnt_private *priv)
 {
 	int ii;
 
-    for (ii = RATE_54M; ii >= RATE_6M; ii --) {
-        if ((pDevice->wBasicRate) & ((u16)(1<<ii)))
-            return true;
-    }
-    return false;
+	for (ii = RATE_54M; ii >= RATE_6M; ii--) {
+		if ((priv->wBasicRate) & ((u16)(1 << ii)))
+			return true;
+	}
+
+	return false;
 }
 
-u8 CARDbyGetPktType(struct vnt_private *pDevice)
+u8 CARDbyGetPktType(struct vnt_private *priv)
 {
 
-    if (pDevice->byBBType == BB_TYPE_11A || pDevice->byBBType == BB_TYPE_11B) {
-        return (u8)pDevice->byBBType;
-    }
-    else if (CARDbIsOFDMinBasicRate(pDevice)) {
-        return PK_TYPE_11GA;
-    }
-    else {
-        return PK_TYPE_11GB;
-    }
+	if (priv->byBBType == BB_TYPE_11A || priv->byBBType == BB_TYPE_11B)
+		return (u8)priv->byBBType;
+	else if (CARDbIsOFDMinBasicRate(priv))
+		return PK_TYPE_11GA;
+	else
+		return PK_TYPE_11GB;
 }
 
 /*
@@ -596,27 +548,27 @@ u8 CARDbyGetPktType(struct vnt_private *pDevice)
  *
  * Parameters:
  *  In:
- *      pDevice         - The adapter to be sync.
- *      qwTSF1          - Rx BCN's TSF
- *      qwTSF2          - Local TSF
+ *      rx_rate	- rx rate.
+ *      tsf1	- Rx BCN's TSF
+ *      tsf2	- Local TSF
  *  Out:
  *      none
  *
  * Return Value: TSF Offset value
  *
  */
-u64 CARDqGetTSFOffset(u8 byRxRate, u64 qwTSF1, u64 qwTSF2)
+u64 CARDqGetTSFOffset(u8 rx_rate, u64 tsf1, u64 tsf2)
 {
-	u64 qwTSFOffset = 0;
-	u16 wRxBcnTSFOffst = 0;
+	u64 tsf_offset = 0;
+	u16 rx_bcn_offset = 0;
 
-	wRxBcnTSFOffst = cwRXBCNTSFOff[byRxRate % MAX_RATE];
+	rx_bcn_offset = cwRXBCNTSFOff[rx_rate % MAX_RATE];
 
-	qwTSF2 += (u64)wRxBcnTSFOffst;
+	tsf2 += (u64)rx_bcn_offset;
 
-	qwTSFOffset = qwTSF1 - qwTSF2;
+	tsf_offset = tsf1 - tsf2;
 
-	return qwTSFOffset;
+	return tsf_offset;
 }
 
 /*
@@ -625,42 +577,34 @@ u64 CARDqGetTSFOffset(u8 byRxRate, u64 qwTSF1, u64 qwTSF2)
  *
  * Parameters:
  *  In:
- *      pDevice         - The adapter to be sync.
- *      qwBSSTimestamp  - Rx BCN's TSF
- *      qwLocalTSF      - Local TSF
+ *      priv		- The adapter to be sync.
+ *      time_stamp	- Rx BCN's TSF
+ *      local_tsf	- Local TSF
  *  Out:
  *      none
  *
  * Return Value: none
  *
  */
-void CARDvAdjustTSF(struct vnt_private *pDevice, u8 byRxRate,
-		u64 qwBSSTimestamp, u64 qwLocalTSF)
+void CARDvAdjustTSF(struct vnt_private *priv, u8 rx_rate,
+		u64 time_stamp, u64 local_tsf)
 {
-	u64 qwTSFOffset = 0;
-	u8 pbyData[8];
-
-    qwTSFOffset = CARDqGetTSFOffset(byRxRate, qwBSSTimestamp, qwLocalTSF);
-    // adjust TSF
-    // HW's TSF add TSF Offset reg
-
-	pbyData[0] = (u8)qwTSFOffset;
-	pbyData[1] = (u8)(qwTSFOffset >> 8);
-	pbyData[2] = (u8)(qwTSFOffset >> 16);
-	pbyData[3] = (u8)(qwTSFOffset >> 24);
-	pbyData[4] = (u8)(qwTSFOffset >> 32);
-	pbyData[5] = (u8)(qwTSFOffset >> 40);
-	pbyData[6] = (u8)(qwTSFOffset >> 48);
-	pbyData[7] = (u8)(qwTSFOffset >> 56);
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_SET_TSFTBTT,
-                        MESSAGE_REQUEST_TSF,
-                        0,
-                        8,
-                        pbyData
-                        );
-
+	u64 tsf_offset = 0;
+	u8 data[8];
+
+	tsf_offset = CARDqGetTSFOffset(rx_rate, time_stamp, local_tsf);
+
+	data[0] = (u8)tsf_offset;
+	data[1] = (u8)(tsf_offset >> 8);
+	data[2] = (u8)(tsf_offset >> 16);
+	data[3] = (u8)(tsf_offset >> 24);
+	data[4] = (u8)(tsf_offset >> 32);
+	data[5] = (u8)(tsf_offset >> 40);
+	data[6] = (u8)(tsf_offset >> 48);
+	data[7] = (u8)(tsf_offset >> 56);
+
+	vnt_control_out(priv, MESSAGE_TYPE_SET_TSFTBTT,
+		MESSAGE_REQUEST_TSF, 0, 8, data);
 }
 /*
  * Description: Read NIC TSF counter
@@ -668,17 +612,17 @@ void CARDvAdjustTSF(struct vnt_private *pDevice, u8 byRxRate,
  *
  * Parameters:
  *  In:
- *      pDevice         - The adapter to be read
+ *	priv		- The adapter to be read
  *  Out:
- *      qwCurrTSF       - Current TSF counter
+ *	current_tsf	- Current TSF counter
  *
  * Return Value: true if success; otherwise false
  *
  */
-bool CARDbGetCurrentTSF(struct vnt_private *pDevice, u64 *pqwCurrTSF)
+bool CARDbGetCurrentTSF(struct vnt_private *priv, u64 *current_tsf)
 {
 
-	*pqwCurrTSF = pDevice->qwCurrTSF;
+	*current_tsf = priv->qwCurrTSF;
 
 	return true;
 }
@@ -689,17 +633,17 @@ bool CARDbGetCurrentTSF(struct vnt_private *pDevice, u64 *pqwCurrTSF)
  *
  * Parameters:
  *  In:
- *      pDevice         - The adapter to be read
+ *      priv	- The adapter to be read
  *
  * Return Value: true if success; otherwise false
  *
  */
-bool CARDbClearCurrentTSF(struct vnt_private *pDevice)
+bool CARDbClearCurrentTSF(struct vnt_private *priv)
 {
 
-	MACvRegBitsOn(pDevice, MAC_REG_TFTCTL, TFTCTL_TSFCNTRST);
+	MACvRegBitsOn(priv, MAC_REG_TFTCTL, TFTCTL_TSFCNTRST);
 
-	pDevice->qwCurrTSF = 0;
+	priv->qwCurrTSF = 0;
 
 	return true;
 }
@@ -710,30 +654,30 @@ bool CARDbClearCurrentTSF(struct vnt_private *pDevice)
  *
  * Parameters:
  *  In:
- *      qwTSF           - Current TSF counter
- *      wbeaconInterval - Beacon Interval
+ *      tsf		- Current TSF counter
+ *      beacon_interval - Beacon Interval
  *  Out:
- *      qwCurrTSF       - Current TSF counter
+ *      tsf		- Current TSF counter
  *
  * Return Value: TSF value of next Beacon
  *
  */
-u64 CARDqGetNextTBTT(u64 qwTSF, u16 wBeaconInterval)
+u64 CARDqGetNextTBTT(u64 tsf, u16 beacon_interval)
 {
-	u32 uBeaconInterval;
+	u32 beacon_int;
 
-	uBeaconInterval = wBeaconInterval * 1024;
+	beacon_int = beacon_interval * 1024;
 
 	/* Next TBTT =
 	*	((local_current_TSF / beacon_interval) + 1) * beacon_interval
 	*/
-	if (uBeaconInterval) {
-		do_div(qwTSF, uBeaconInterval);
-		qwTSF += 1;
-		qwTSF *= uBeaconInterval;
+	if (beacon_int) {
+		do_div(tsf, beacon_int);
+		tsf += 1;
+		tsf *= beacon_int;
 	}
 
-	return qwTSF;
+	return tsf;
 }
 
 /*
@@ -743,41 +687,35 @@ u64 CARDqGetNextTBTT(u64 qwTSF, u16 wBeaconInterval)
  * Parameters:
  *  In:
  *      dwIoBase        - IO Base
- *      wBeaconInterval - Beacon Interval
+ *	beacon_interval - Beacon Interval
  *  Out:
  *      none
  *
  * Return Value: none
  *
  */
-void CARDvSetFirstNextTBTT(struct vnt_private *pDevice, u16 wBeaconInterval)
+void CARDvSetFirstNextTBTT(struct vnt_private *priv, u16 beacon_interval)
 {
-	u64 qwNextTBTT = 0;
-	u8 pbyData[8];
-
-	CARDbClearCurrentTSF(pDevice);
-    //CARDbGetCurrentTSF(pDevice, &qwNextTBTT); //Get Local TSF counter
-	qwNextTBTT = CARDqGetNextTBTT(qwNextTBTT, wBeaconInterval);
-    // Set NextTBTT
-
-	pbyData[0] = (u8)qwNextTBTT;
-	pbyData[1] = (u8)(qwNextTBTT >> 8);
-	pbyData[2] = (u8)(qwNextTBTT >> 16);
-	pbyData[3] = (u8)(qwNextTBTT >> 24);
-	pbyData[4] = (u8)(qwNextTBTT >> 32);
-	pbyData[5] = (u8)(qwNextTBTT >> 40);
-	pbyData[6] = (u8)(qwNextTBTT >> 48);
-	pbyData[7] = (u8)(qwNextTBTT >> 56);
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_SET_TSFTBTT,
-                        MESSAGE_REQUEST_TBTT,
-                        0,
-                        8,
-                        pbyData
-                        );
-
-    return;
+	u64 next_tbtt = 0;
+	u8 data[8];
+
+	CARDbClearCurrentTSF(priv);
+
+	next_tbtt = CARDqGetNextTBTT(next_tbtt, beacon_interval);
+
+	data[0] = (u8)next_tbtt;
+	data[1] = (u8)(next_tbtt >> 8);
+	data[2] = (u8)(next_tbtt >> 16);
+	data[3] = (u8)(next_tbtt >> 24);
+	data[4] = (u8)(next_tbtt >> 32);
+	data[5] = (u8)(next_tbtt >> 40);
+	data[6] = (u8)(next_tbtt >> 48);
+	data[7] = (u8)(next_tbtt >> 56);
+
+	vnt_control_out(priv, MESSAGE_TYPE_SET_TSFTBTT,
+		MESSAGE_REQUEST_TBTT, 0, 8, data);
+
+	return;
 }
 
 /*
@@ -786,45 +724,37 @@ void CARDvSetFirstNextTBTT(struct vnt_private *pDevice, u16 wBeaconInterval)
  *
  * Parameters:
  *  In:
- *      pDevice         - The adapter to be set
- *      qwTSF           - Current TSF counter
- *      wBeaconInterval - Beacon Interval
+ *	priv		- The adapter to be set
+ *      tsf		- Current TSF counter
+ *      beacon_interval - Beacon Interval
  *  Out:
  *      none
  *
  * Return Value: none
  *
  */
-void CARDvUpdateNextTBTT(struct vnt_private *pDevice, u64 qwTSF,
-			u16 wBeaconInterval)
+void CARDvUpdateNextTBTT(struct vnt_private *priv, u64 tsf,
+			u16 beacon_interval)
 {
-	u8 pbyData[8];
-
-    qwTSF = CARDqGetNextTBTT(qwTSF, wBeaconInterval);
+	u8 data[8];
 
-    // Set NextTBTT
+	tsf = CARDqGetNextTBTT(tsf, beacon_interval);
 
-	pbyData[0] = (u8)qwTSF;
-	pbyData[1] = (u8)(qwTSF >> 8);
-	pbyData[2] = (u8)(qwTSF >> 16);
-	pbyData[3] = (u8)(qwTSF >> 24);
-	pbyData[4] = (u8)(qwTSF >> 32);
-	pbyData[5] = (u8)(qwTSF >> 40);
-	pbyData[6] = (u8)(qwTSF >> 48);
-	pbyData[7] = (u8)(qwTSF >> 56);
+	data[0] = (u8)tsf;
+	data[1] = (u8)(tsf >> 8);
+	data[2] = (u8)(tsf >> 16);
+	data[3] = (u8)(tsf >> 24);
+	data[4] = (u8)(tsf >> 32);
+	data[5] = (u8)(tsf >> 40);
+	data[6] = (u8)(tsf >> 48);
+	data[7] = (u8)(tsf >> 56);
 
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_SET_TSFTBTT,
-                        MESSAGE_REQUEST_TBTT,
-                        0,
-                        8,
-                        pbyData
-                        );
+	vnt_control_out(priv, MESSAGE_TYPE_SET_TSFTBTT,
+		MESSAGE_REQUEST_TBTT, 0, 8, data);
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-		"Card:Update Next TBTT[%8lx]\n", (unsigned long)qwTSF);
+	dev_dbg(&priv->usb->dev, "%s TBTT: %8llx\n", __func__, tsf);
 
-    return;
+	return;
 }
 
 /*
@@ -832,38 +762,36 @@ void CARDvUpdateNextTBTT(struct vnt_private *pDevice, u64 qwTSF,
  *
  * Parameters:
  *  In:
- *      pDevice         - The adapter to be turned off
+ *      priv         - The adapter to be turned off
  *  Out:
  *      none
  *
  * Return Value: true if success; otherwise false
  *
  */
-int CARDbRadioPowerOff(struct vnt_private *pDevice)
+int CARDbRadioPowerOff(struct vnt_private *priv)
 {
-	int bResult = true;
-
-    //if (pDevice->bRadioOff == true)
-    //    return true;
-
-    pDevice->bRadioOff = true;
-
-    switch (pDevice->byRFType) {
-        case RF_AL2230:
-        case RF_AL2230S:
-        case RF_AIROHA7230:
-        case RF_VT3226:     //RobertYu:20051111
-        case RF_VT3226D0:
-        case RF_VT3342A0:   //RobertYu:20060609
-            MACvRegBitsOff(pDevice, MAC_REG_SOFTPWRCTL, (SOFTPWRCTL_SWPE2 | SOFTPWRCTL_SWPE3));
-            break;
-    }
+	int ret = true;
+
+	priv->bRadioOff = true;
+
+	switch (priv->byRFType) {
+	case RF_AL2230:
+	case RF_AL2230S:
+	case RF_AIROHA7230:
+	case RF_VT3226:
+	case RF_VT3226D0:
+	case RF_VT3342A0:
+		MACvRegBitsOff(priv, MAC_REG_SOFTPWRCTL,
+				(SOFTPWRCTL_SWPE2 | SOFTPWRCTL_SWPE3));
+		break;
+	}
 
-    MACvRegBitsOff(pDevice, MAC_REG_HOSTCR, HOSTCR_RXON);
+	MACvRegBitsOff(priv, MAC_REG_HOSTCR, HOSTCR_RXON);
 
-    BBvSetDeepSleep(pDevice);
+	BBvSetDeepSleep(priv);
 
-    return bResult;
+	return ret;
 }
 
 /*
@@ -871,84 +799,79 @@ int CARDbRadioPowerOff(struct vnt_private *pDevice)
  *
  * Parameters:
  *  In:
- *      pDevice         - The adapter to be turned on
+ *      priv         - The adapter to be turned on
  *  Out:
  *      none
  *
  * Return Value: true if success; otherwise false
  *
  */
-int CARDbRadioPowerOn(struct vnt_private *pDevice)
+int CARDbRadioPowerOn(struct vnt_private *priv)
 {
-	int bResult = true;
+	int ret = true;
 
-    if ((pDevice->bHWRadioOff == true) || (pDevice->bRadioControlOff == true)) {
-        return false;
-    }
+	if (priv->bHWRadioOff == true || priv->bRadioControlOff == true)
+		return false;
 
-    //if (pDevice->bRadioOff == false)
-    //    return true;
+	priv->bRadioOff = false;
 
-    pDevice->bRadioOff = false;
+	BBvExitDeepSleep(priv);
 
-    BBvExitDeepSleep(pDevice);
+	MACvRegBitsOn(priv, MAC_REG_HOSTCR, HOSTCR_RXON);
 
-    MACvRegBitsOn(pDevice, MAC_REG_HOSTCR, HOSTCR_RXON);
-
-    switch (pDevice->byRFType) {
-        case RF_AL2230:
-        case RF_AL2230S:
-        case RF_AIROHA7230:
-        case RF_VT3226:     //RobertYu:20051111
-        case RF_VT3226D0:
-        case RF_VT3342A0:   //RobertYu:20060609
-            MACvRegBitsOn(pDevice, MAC_REG_SOFTPWRCTL, (SOFTPWRCTL_SWPE2 | SOFTPWRCTL_SWPE3));
-            break;
-    }
+	switch (priv->byRFType) {
+	case RF_AL2230:
+	case RF_AL2230S:
+	case RF_AIROHA7230:
+	case RF_VT3226:
+	case RF_VT3226D0:
+	case RF_VT3342A0:
+		MACvRegBitsOn(priv, MAC_REG_SOFTPWRCTL,
+			(SOFTPWRCTL_SWPE2 | SOFTPWRCTL_SWPE3));
+		break;
+	}
 
-    return bResult;
+	return ret;
 }
 
-void CARDvSetBSSMode(struct vnt_private *pDevice)
+void CARDvSetBSSMode(struct vnt_private *priv)
 {
-    // Set BB and packet type at the same time.//{{RobertYu:20050222, AL7230 have two TX PA output, only connet to b/g now
-    // so in 11a mode need to set the MAC Reg0x4C to 11b/g mode to turn on PA
-    if( (pDevice->byRFType == RF_AIROHA7230 ) && (pDevice->byBBType == BB_TYPE_11A) )
-    {
-        MACvSetBBType(pDevice, BB_TYPE_11G);
-    }
-    else
-    {
-        MACvSetBBType(pDevice, pDevice->byBBType);
-    }
-    pDevice->byPacketType = CARDbyGetPktType(pDevice);
-
-    if (pDevice->byBBType == BB_TYPE_11A) {
-        ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x88, 0x03);
-    } else if (pDevice->byBBType == BB_TYPE_11B) {
-        ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x88, 0x02);
-    } else if (pDevice->byBBType == BB_TYPE_11G) {
-        ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x88, 0x08);
-    }
-
-    vUpdateIFS(pDevice);
-    CARDvSetRSPINF(pDevice, (u8)pDevice->byBBType);
-
-    if ( pDevice->byBBType == BB_TYPE_11A ) {
-        //request by Jack 2005-04-26
-        if (pDevice->byRFType == RF_AIROHA7230) {
-            pDevice->abyBBVGA[0] = 0x20;
-            ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xE7, pDevice->abyBBVGA[0]);
-        }
-        pDevice->abyBBVGA[2] = 0x10;
-        pDevice->abyBBVGA[3] = 0x10;
-    } else {
-        //request by Jack 2005-04-26
-        if (pDevice->byRFType == RF_AIROHA7230) {
-            pDevice->abyBBVGA[0] = 0x1C;
-            ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xE7, pDevice->abyBBVGA[0]);
-        }
-        pDevice->abyBBVGA[2] = 0x0;
-        pDevice->abyBBVGA[3] = 0x0;
-    }
+	if (priv->byRFType == RF_AIROHA7230 && priv->byBBType == BB_TYPE_11A)
+		MACvSetBBType(priv, BB_TYPE_11G);
+	else
+		MACvSetBBType(priv, priv->byBBType);
+
+	priv->byPacketType = CARDbyGetPktType(priv);
+
+	if (priv->byBBType == BB_TYPE_11A)
+		vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x88, 0x03);
+	else if (priv->byBBType == BB_TYPE_11B)
+		vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x88, 0x02);
+	else if (priv->byBBType == BB_TYPE_11G)
+		vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x88, 0x08);
+
+	vUpdateIFS(priv);
+	CARDvSetRSPINF(priv, (u8)priv->byBBType);
+
+	if (priv->byBBType == BB_TYPE_11A) {
+		if (priv->byRFType == RF_AIROHA7230) {
+			priv->abyBBVGA[0] = 0x20;
+
+			vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG,
+						0xe7, priv->abyBBVGA[0]);
+		}
+
+		priv->abyBBVGA[2] = 0x10;
+		priv->abyBBVGA[3] = 0x10;
+	} else {
+		if (priv->byRFType == RF_AIROHA7230) {
+			priv->abyBBVGA[0] = 0x1c;
+
+			vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG,
+						0xe7, priv->abyBBVGA[0]);
+		}
+
+		priv->abyBBVGA[2] = 0x0;
+		priv->abyBBVGA[3] = 0x0;
+	}
 }
diff --git a/drivers/staging/vt6656/card.h b/drivers/staging/vt6656/card.h
index f843e50875f..ac734714c7d 100644
--- a/drivers/staging/vt6656/card.h
+++ b/drivers/staging/vt6656/card.h
@@ -46,10 +46,10 @@ typedef enum _CARD_PHY_TYPE {
 struct vnt_private;
 
 void CARDbSetMediaChannel(struct vnt_private *pDevice, u32 uConnectionChannel);
-void CARDvSetRSPINF(struct vnt_private *pDevice, u8 byBBType);
-void vUpdateIFS(struct vnt_private *pDevice);
-void CARDvUpdateBasicTopRate(struct vnt_private *pDevice);
-void CARDbAddBasicRate(struct vnt_private *pDevice, u16 wRateIdx);
+void CARDvSetRSPINF(struct vnt_private *, u8);
+void vUpdateIFS(struct vnt_private *);
+void CARDvUpdateBasicTopRate(struct vnt_private *);
+void CARDbAddBasicRate(struct vnt_private *, u16);
 int CARDbIsOFDMinBasicRate(struct vnt_private *pDevice);
 void CARDvAdjustTSF(struct vnt_private *pDevice, u8 byRxRate,
 		u64 qwBSSTimestamp, u64 qwLocalTSF);
diff --git a/drivers/staging/vt6656/control.c b/drivers/staging/vt6656/control.c
deleted file mode 100644
index 026784fcde8..00000000000
--- a/drivers/staging/vt6656/control.c
+++ /dev/null
@@ -1,79 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: control.c
- *
- * Purpose: Handle USB control endpoint
- *
- * Author: Jerry Chen
- *
- * Date: Apr. 5, 2004
- *
- * Functions:
- *      CONTROLnsRequestOut - Write variable length bytes to MEM/BB/MAC/EEPROM
- *      CONTROLnsRequestIn - Read variable length bytes from MEM/BB/MAC/EEPROM
- *      ControlvWriteByte - Write one byte to MEM/BB/MAC/EEPROM
- *      ControlvReadByte - Read one byte from MEM/BB/MAC/EEPROM
- *      ControlvMaskByte - Read one byte from MEM/BB/MAC/EEPROM and clear/set
- *				some bits in the same address
- *
- * Revision History:
- *      04-05-2004 Jerry Chen:  Initial release
- *      11-24-2004 Warren Hsu: Add ControlvWriteByte, ControlvReadByte,
- *					ControlvMaskByte
- *
- */
-
-#include "control.h"
-#include "rndis.h"
-
-/* static int          msglevel                =MSG_LEVEL_INFO;  */
-/* static int          msglevel                =MSG_LEVEL_DEBUG; */
-
-void ControlvWriteByte(struct vnt_private *pDevice, u8 reg, u8 reg_off,
-			u8 data)
-{
-
-	CONTROLnsRequestOut(pDevice, MESSAGE_TYPE_WRITE, reg_off, reg,
-		sizeof(u8), &data);
-
-	return;
-}
-
-void ControlvReadByte(struct vnt_private *pDevice, u8 reg, u8 reg_off,
-			u8 *data)
-{
-	CONTROLnsRequestIn(pDevice, MESSAGE_TYPE_READ,
-			reg_off, reg, sizeof(u8), data);
-	return;
-}
-
-void ControlvMaskByte(struct vnt_private *pDevice, u8 reg_type, u8 reg_off,
-			u8 reg_mask, u8 data)
-{
-	u8 reg_data[2];
-
-	reg_data[0] = data;
-	reg_data[1] = reg_mask;
-
-	CONTROLnsRequestOut(pDevice, MESSAGE_TYPE_WRITE_MASK, reg_off,
-			reg_type, ARRAY_SIZE(reg_data), reg_data);
-
-	return;
-}
diff --git a/drivers/staging/vt6656/control.h b/drivers/staging/vt6656/control.h
deleted file mode 100644
index 9da9b96add7..00000000000
--- a/drivers/staging/vt6656/control.h
+++ /dev/null
@@ -1,54 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: control.h
- *
- * Purpose:
- *
- * Author: Jerry Chen
- *
- * Date: Apr. 5, 2004
- *
- */
-
-#ifndef __CONTROL_H__
-#define __CONTROL_H__
-
-#include "device.h"
-#include "usbpipe.h"
-
-#define CONTROLnsRequestOut(Device, Request, Value, Index, Length, Buffer) \
-	PIPEnsControlOut(Device, Request, Value, Index, Length, Buffer)
-
-#define CONTROLnsRequestOutAsyn(Device, Request, Value, Index, Length, Buffer) \
-	PIPEnsControlOutAsyn(Device, Request, Value, Index, Length, Buffer)
-
-#define CONTROLnsRequestIn(Device, Request, Value, Index, Length, Buffer) \
-	PIPEnsControlIn(Device, Request, Value, Index, Length, Buffer)
-
-void ControlvWriteByte(struct vnt_private *pDevice, u8 reg, u8 reg_off,
-			u8 data);
-
-void ControlvReadByte(struct vnt_private *pDevice, u8 reg, u8 reg_off,
-			u8 *data);
-
-void ControlvMaskByte(struct vnt_private *pDevice, u8 reg_type, u8 reg_off,
-			u8 reg_mask, u8 data);
-
-#endif /* __CONTROL_H__ */
diff --git a/drivers/staging/vt6656/datarate.c b/drivers/staging/vt6656/datarate.c
index 547db6f0c53..8032d6b5b38 100644
--- a/drivers/staging/vt6656/datarate.c
+++ b/drivers/staging/vt6656/datarate.c
@@ -40,7 +40,6 @@
 #include "datarate.h"
 #include "card.h"
 #include "baseband.h"
-#include "srom.h"
 #include "rf.h"
 
 /* static int msglevel = MSG_LEVEL_DEBUG; */
diff --git a/drivers/staging/vt6656/desc.h b/drivers/staging/vt6656/desc.h
index 7c6dd5f5229..617d479b85c 100644
--- a/drivers/staging/vt6656/desc.h
+++ b/drivers/staging/vt6656/desc.h
@@ -39,28 +39,15 @@
 /* max transmit or receive buffer size */
 #define CB_MAX_BUF_SIZE     2900U       /* NOTE: must be multiple of 4 */
 
-/* max TX buffer size */
-#define CB_MAX_TX_BUF_SIZE        CB_MAX_BUF_SIZE
-/* max RX buffer size when not use Multi-RD */
-#define CB_MAX_RX_BUF_SIZE_NORMAL CB_MAX_BUF_SIZE
-
-#define CB_BEACON_BUF_SIZE  512U        /* default beacon buffer size */
-
 #define MAX_TOTAL_SIZE_WITH_ALL_HEADERS CB_MAX_BUF_SIZE
 
 #define MAX_INTERRUPT_SIZE              32
 
-#define RX_BLOCKS           64          /* from 0x60 to 0xA0 */
-#define TX_BLOCKS           32          /* from 0xA0 to 0xC0 */
-
 #define CB_MAX_RX_DESC      128         /* max # of descriptors */
 #define CB_MIN_RX_DESC      16          /* min # of RX descriptors */
 #define CB_MAX_TX_DESC      128         /* max # of descriptors */
 #define CB_MIN_TX_DESC      16          /* min # of TX descriptors */
 
-#define CB_RD_NUM           64          /* default # of RD */
-#define CB_TD_NUM           64          /* default # of TD */
-
 /*
  * bits in the RSR register
  */
@@ -91,15 +78,6 @@
 #define TSR_ACKDATA         0x02
 #define TSR_VALID           0x01
 
-#define CB_PROTOCOL_RESERVED_SECTION    16
-
-/*
- * if retries exceed 15 times, TX will abort, and
- * if TX fifo underflow, TX will fail
- * we should try to resend it
- */
-#define CB_MAX_TX_ABORT_RETRY   3
-
 #define FIFOCTL_AUTO_FB_1   0x1000
 #define FIFOCTL_AUTO_FB_0   0x0800
 #define FIFOCTL_GRPACK      0x0400
@@ -126,50 +104,4 @@
 #define FRAGCTL_STAFRAG     0x0001
 #define FRAGCTL_NONFRAG     0x0000
 
-#define TYPE_TXDMA0     0
-#define TYPE_AC0DMA     1
-#define TYPE_ATIMDMA    2
-#define TYPE_SYNCDMA    3
-#define TYPE_MAXTD      2
-
-#define TYPE_BEACONDMA  4
-
-#define TYPE_RXDMA0     0
-#define TYPE_RXDMA1     1
-#define TYPE_MAXRD      2
-
-/* TD_INFO flags control bit */
-#define TD_FLAGS_NETIF_SKB 0x01 /* check if need release skb */
-#define TD_FLAGS_PRIV_SKB  0x02 /* check if called from private skb(hostap) */
-#define TD_FLAGS_PS_RETRY  0x04 /* check if PS STA frame re-transmit */
-
-/*
- * TX FIFO header
- */
-typedef struct tagSBEACONCtl {
-	u32 BufReady:1;
-	u32 TSF:15;
-	u32 BufLen:11;
-	u32 Reserved:5;
-} __attribute__ ((__packed__))
-SBEACONCtl;
-
-typedef struct tagSSecretKey {
-	u32 dwLowDword;
-    u8    byHighByte;
-} __attribute__ ((__packed__))
-SSecretKey;
-
-typedef struct tagSKeyEntry {
-    u8  abyAddrHi[2];
-    u16  wKCTL;
-    u8  abyAddrLo[4];
-	u32 dwKey0[4];
-	u32 dwKey1[4];
-	u32 dwKey2[4];
-	u32 dwKey3[4];
-	u32 dwKey4[4];
-} __attribute__ ((__packed__))
-SKeyEntry;
-
 #endif /* __DESC_H__ */
diff --git a/drivers/staging/vt6656/device.h b/drivers/staging/vt6656/device.h
index e2abe3ddd24..5b64ca7b62f 100644
--- a/drivers/staging/vt6656/device.h
+++ b/drivers/staging/vt6656/device.h
@@ -52,53 +52,35 @@
 #undef DEVICE_ETHTOOL_IOCTL_SUPPORT
 #endif
 
-/* please copy below macro to driver_event.c for API */
-#define RT_INSMOD_EVENT_FLAG                             0x0101
-#define RT_UPDEV_EVENT_FLAG                               0x0102
-#define RT_DISCONNECTED_EVENT_FLAG               0x0103
-#define RT_WPACONNECTED_EVENT_FLAG             0x0104
-#define RT_DOWNDEV_EVENT_FLAG                        0x0105
-#define RT_RMMOD_EVENT_FLAG                              0x0106
+#define MAX_RATE			12
 
 /*
  * device specific
  */
 
-#include "device_cfg.h"
 #include "80211hdr.h"
 #include "tether.h"
 #include "wmgr.h"
 #include "wcmd.h"
-#include "srom.h"
 #include "rc4.h"
 #include "desc.h"
 #include "key.h"
 #include "card.h"
-#include "rndis.h"
 
 #define VNT_USB_VENDOR_ID                     0x160a
 #define VNT_USB_PRODUCT_ID                    0x3184
 
-#define MAC_MAX_CONTEXT_REG     (256+128)
+#define DEVICE_NAME			"vt6656"
+#define DEVICE_FULL_DRV_NAM		"VIA Networking Wireless LAN USB Driver"
 
-#define MAX_MULTICAST_ADDRESS_NUM       32
-#define MULTICAST_ADDRESS_LIST_SIZE     (MAX_MULTICAST_ADDRESS_NUM * ETH_ALEN)
+#define DEVICE_VERSION			"1.19_12"
 
-#define DUPLICATE_RX_CACHE_LENGTH       5
-
-#define NUM_KEY_ENTRY                   11
+#define CONFIG_PATH			"/etc/vntconfiguration.dat"
 
-#define TX_WEP_NONE                     0
-#define TX_WEP_OTF                      1
-#define TX_WEP_SW                       2
-#define TX_WEP_SWOTP                    3
-#define TX_WEP_OTPSW                    4
-#define TX_WEP_SW232                    5
+#define MAX_UINTS			8
+#define OPTION_DEFAULT			{ [0 ... MAX_UINTS-1] = -1}
 
-#define KEYSEL_WEP40                    0
-#define KEYSEL_WEP104                   1
-#define KEYSEL_TKIP                     2
-#define KEYSEL_CCMP                     3
+#define DUPLICATE_RX_CACHE_LENGTH       5
 
 #define AUTO_FB_NONE            0
 #define AUTO_FB_0               1
@@ -119,23 +101,86 @@
 #define ANT_RXA                 2
 #define ANT_RXB                 3
 
-#define MAXCHECKHANGCNT         4
-
-/* Packet type */
-#define TX_PKT_UNI              0x00
-#define TX_PKT_MULTI            0x01
-#define TX_PKT_BROAD            0x02
-
 #define BB_VGA_LEVEL            4
 #define BB_VGA_CHANGE_THRESHOLD 3
 
+#define EEP_MAX_CONTEXT_SIZE    256
+
+/* Contents in the EEPROM */
+#define EEP_OFS_PAR		0x0
+#define EEP_OFS_ANTENNA		0x17
+#define EEP_OFS_RADIOCTL	0x18
+#define EEP_OFS_RFTYPE		0x1b
+#define EEP_OFS_MINCHANNEL	0x1c
+#define EEP_OFS_MAXCHANNEL	0x1d
+#define EEP_OFS_SIGNATURE	0x1e
+#define EEP_OFS_ZONETYPE	0x1f
+#define EEP_OFS_RFTABLE		0x20
+#define EEP_OFS_PWR_CCK		0x20
+#define EEP_OFS_SETPT_CCK	0x21
+#define EEP_OFS_PWR_OFDMG	0x23
+
+#define EEP_OFS_CALIB_TX_IQ	0x24
+#define EEP_OFS_CALIB_TX_DC	0x25
+#define EEP_OFS_CALIB_RX_IQ	0x26
+
+#define EEP_OFS_MAJOR_VER	0x2e
+#define EEP_OFS_MINOR_VER	0x2f
+
+#define EEP_OFS_CCK_PWR_TBL	0x30
+#define EEP_OFS_OFDM_PWR_TBL	0x40
+#define EEP_OFS_OFDMA_PWR_TBL	0x50
+
+/* Bits in EEP_OFS_ANTENNA */
+#define EEP_ANTENNA_MAIN	0x1
+#define EEP_ANTENNA_AUX		0x2
+#define EEP_ANTINV		0x4
+
+/* Bits in EEP_OFS_RADIOCTL */
+#define EEP_RADIOCTL_ENABLE	0x80
+
+/* control commands */
+#define MESSAGE_TYPE_READ		0x1
+#define MESSAGE_TYPE_WRITE		0x0
+#define MESSAGE_TYPE_LOCK_OR		0x2
+#define MESSAGE_TYPE_LOCK_AND		0x3
+#define MESSAGE_TYPE_WRITE_MASK		0x4
+#define MESSAGE_TYPE_CARDINIT		0x5
+#define MESSAGE_TYPE_INIT_RSP		0x6
+#define MESSAGE_TYPE_MACSHUTDOWN	0x7
+#define MESSAGE_TYPE_SETKEY		0x8
+#define MESSAGE_TYPE_CLRKEYENTRY	0x9
+#define MESSAGE_TYPE_WRITE_MISCFF	0xa
+#define MESSAGE_TYPE_SET_ANTMD		0xb
+#define MESSAGE_TYPE_SELECT_CHANNLE	0xc
+#define MESSAGE_TYPE_SET_TSFTBTT	0xd
+#define MESSAGE_TYPE_SET_SSTIFS		0xe
+#define MESSAGE_TYPE_CHANGE_BBTYPE	0xf
+#define MESSAGE_TYPE_DISABLE_PS		0x10
+#define MESSAGE_TYPE_WRITE_IFRF		0x11
+
+/* command read/write(index) */
+#define MESSAGE_REQUEST_MEM		0x1
+#define MESSAGE_REQUEST_BBREG		0x2
+#define MESSAGE_REQUEST_MACREG		0x3
+#define MESSAGE_REQUEST_EEPROM		0x4
+#define MESSAGE_REQUEST_TSF		0x5
+#define MESSAGE_REQUEST_TBTT		0x6
+#define MESSAGE_REQUEST_BBAGC		0x7
+#define MESSAGE_REQUEST_VERSION		0x8
+#define MESSAGE_REQUEST_RF_INIT		0x9
+#define MESSAGE_REQUEST_RF_INIT2	0xa
+#define MESSAGE_REQUEST_RF_CH0		0xb
+#define MESSAGE_REQUEST_RF_CH1		0xc
+#define MESSAGE_REQUEST_RF_CH2		0xd
+
+/* USB registers */
+#define USB_REG4			0x604
+
 #ifndef RUN_AT
 #define RUN_AT(x)                       (jiffies+(x))
 #endif
 
-/* DMA related */
-#define RESERV_AC0DMA                   4
-
 #define PRIVATE_Message                 0
 
 #define DBG_PRT(l, p, args...) { if (l <= msglevel) printk(p, ##args); }
@@ -153,6 +198,23 @@ typedef enum __device_msg_level {
 #define DEVICE_INIT_RESET	0x1 /* reset init or Dx to D0 power remain */
 #define DEVICE_INIT_DXPL	0x2 /* Dx to D0 power lost init */
 
+/* Device init */
+struct vnt_cmd_card_init {
+	u8 init_class;
+	u8 exist_sw_net_addr;
+	u8 sw_net_addr[6];
+	u8 short_retry_limit;
+	u8 long_retry_limit;
+};
+
+struct vnt_rsp_card_init {
+	u8 status;
+	u8 net_addr[6];
+	u8 rf_type;
+	u8 min_channel;
+	u8 max_channel;
+};
+
 /* USB */
 
 /*
@@ -176,13 +238,13 @@ struct vnt_rcb {
 
 /* used to track bulk out irps */
 struct vnt_usb_send_context {
-	void *pDevice;
-	struct sk_buff *pPacket;
-	struct urb *pUrb;
-	unsigned int uBufLen;
+	void *priv;
+	struct sk_buff *skb;
+	struct urb *urb;
+	unsigned int buf_len;
 	u8 type;
-	bool bBoolInUse;
-	unsigned char Data[MAX_TOTAL_SIZE_WITH_ALL_HEADERS];
+	bool in_use;
+	unsigned char data[MAX_TOTAL_SIZE_WITH_ALL_HEADERS];
 };
 
 /* tx packet info for rxtx */
@@ -278,9 +340,9 @@ typedef struct tagSPMKIDCandidateEvent {
 
 /* The receive duplicate detection cache entry */
 typedef struct tagSCacheEntry{
-    u16        wFmSequence;
-    u8        abyAddr2[ETH_ALEN];
-    u16        wFrameCtl;
+	__le16 wFmSequence;
+	u8 abyAddr2[ETH_ALEN];
+	__le16 wFrameCtl;
 } SCacheEntry, *PSCacheEntry;
 
 typedef struct tagSCache{
@@ -309,29 +371,9 @@ typedef struct tagSDeFragControlBlock
 
 /* flags for options */
 #define     DEVICE_FLAGS_UNPLUG          0x00000001UL
-#define     DEVICE_FLAGS_PREAMBLE_TYPE   0x00000002UL
-#define     DEVICE_FLAGS_OP_MODE         0x00000004UL
-#define     DEVICE_FLAGS_PS_MODE         0x00000008UL
-#define		DEVICE_FLAGS_80211h_MODE	 0x00000010UL
 
 /* flags for driver status */
 #define     DEVICE_FLAGS_OPENED          0x00010000UL
-#define     DEVICE_FLAGS_WOL_ENABLED     0x00080000UL
-/* flags for capabilities */
-#define     DEVICE_FLAGS_TX_ALIGN        0x01000000UL
-#define     DEVICE_FLAGS_HAVE_CAM        0x02000000UL
-#define     DEVICE_FLAGS_FLOW_CTRL       0x04000000UL
-
-/* flags for MII status */
-#define     DEVICE_LINK_FAIL             0x00000001UL
-#define     DEVICE_SPEED_10              0x00000002UL
-#define     DEVICE_SPEED_100             0x00000004UL
-#define     DEVICE_SPEED_1000            0x00000008UL
-#define     DEVICE_DUPLEX_FULL           0x00000010UL
-#define     DEVICE_AUTONEG_ENABLE        0x00000020UL
-#define     DEVICE_FORCED_BY_EEPROM      0x00000040UL
-/* for device_set_media_duplex */
-#define     DEVICE_LINK_CHANGE           0x00000001UL
 
 typedef struct __device_opt {
 	int nRxDescs0;  /* number of RX descriptors 0 */
@@ -363,11 +405,10 @@ struct vnt_private {
 	u8 byRxMode;
 
 	spinlock_t lock;
+	struct mutex usb_lock;
 
 	u32 rx_bytes;
 
-	u8 byRevId;
-
 	u32 flags;
 	unsigned long Flags;
 
@@ -379,9 +420,7 @@ struct vnt_private {
 	u32 uCurrentDFCBIdx;
 
 	/* USB */
-	struct urb *pControlURB;
 	struct urb *pInterruptURB;
-	struct usb_ctrlrequest sUsbCtlRequest;
 	u32 int_interval;
 
 	/* Variables to track resources for the BULK In Pipe */
@@ -453,30 +492,6 @@ struct vnt_private {
 	u8 byRadioCtl;
 	u8 bHWRadioOff;
 
-	/* SQ3 functions for antenna diversity */
-	struct timer_list TimerSQ3Tmax1;
-	struct timer_list TimerSQ3Tmax2;
-	struct timer_list TimerSQ3Tmax3;
-
-	int bDiversityRegCtlON;
-	int bDiversityEnable;
-	unsigned long ulDiversityNValue;
-	unsigned long ulDiversityMValue;
-	u8 byTMax;
-	u8 byTMax2;
-	u8 byTMax3;
-	unsigned long ulSQ3TH;
-
-	unsigned long uDiversityCnt;
-	u8 byAntennaState;
-	unsigned long ulRatio_State0;
-	unsigned long ulRatio_State1;
-	unsigned long ulSQ3_State0;
-	unsigned long ulSQ3_State1;
-
-	unsigned long aulSQ3Val[MAX_RATE];
-	unsigned long aulPktNum[MAX_RATE];
-
 	/* IFS & Cw */
 	u32 uSIFS;  /* Current SIFS */
 	u32 uDIFS;  /* Current DIFS */
@@ -496,14 +511,9 @@ struct vnt_private {
 	u8 byBBType; /* 0: 11A, 1:11B, 2:11G */
 	u8 byPacketType; /* 0:11a 1:11b 2:11gb 3:11ga */
 	u16 wBasicRate;
-	u8 byACKRate;
 	u8 byTopOFDMBasicRate;
 	u8 byTopCCKBasicRate;
 
-	u32 dwAotoRateTxOkCnt;
-	u32 dwAotoRateTxFailCnt;
-	u32 dwErrorRateThreshold[13];
-	u32 dwTPTable[MAX_RATE];
 	u8 abyEEPROM[EEP_MAX_CONTEXT_SIZE];  /*u32 alignment */
 
 	u8 byMinChannel;
@@ -542,7 +552,6 @@ struct vnt_private {
 
 	u32 dwMaxReceiveLifetime;  /* dot11MaxReceiveLifetime */
 
-	int bCCK;
 	int bEncryptionEnable;
 	int bShortSlotTime;
 	int bProtectMode;
@@ -569,7 +578,6 @@ struct vnt_private {
 	int bBeaconSent;
 	int bFixRate;
 	u8 byCurrentCh;
-	u32 uScanTime;
 
 	CMD_STATE eCommandState;
 
@@ -600,7 +608,6 @@ struct vnt_private {
 	u8 bSameBSSCurNum;
 	int bRoaming;
 	int b11hEable;
-	unsigned long ulTxPower;
 
 	/* Encryption */
 	NDIS_802_11_WEP_STATUS eEncryptionStatus;
@@ -629,7 +636,6 @@ struct vnt_private {
 	int bRxMICFail;
 
 	/* For Update BaseBand VGA Gain Offset */
-	int bUpdateBBVGA;
 	u32 uBBVGADiffCount;
 	u8 byBBVGANew;
 	u8 byBBVGACurrent;
@@ -641,18 +647,6 @@ struct vnt_private {
 
 	int bRadioCmd;
 
-	/* For FOE Tuning */
-	u8  byFOETuning;
-
-	/* For Auto Power Tunning */
-	u8  byAutoPwrTunning;
-
-	/* BaseBand Loopback Use */
-	u8 byBBCR4d;
-	u8 byBBCRc9;
-	u8 byBBCR88;
-	u8 byBBCR09;
-
 	/* command timer */
 	struct delayed_work run_command_work;
 	/* One second callback */
@@ -689,13 +683,6 @@ struct vnt_private {
 	int bwextstep3;
 	int bWPASuppWextEnabled;
 
-	/* user space daemon: hostapd, is used for HOSTAP */
-	int bEnableHostapd;
-	int bEnable8021x;
-	int bEnableHostWEP;
-	struct net_device *apdev;
-	int (*tx_80211)(struct sk_buff *skb, struct net_device *dev);
-
 	u32 uChannel;
 
 	struct iw_statistics wstats; /* wireless stats */
@@ -732,25 +719,15 @@ struct vnt_private {
         (uVar)++;                                   \
 }
 
-#define fMP_RESET_IN_PROGRESS               0x00000001
 #define fMP_DISCONNECTED                    0x00000002
-#define fMP_HALT_IN_PROGRESS                0x00000004
-#define fMP_SURPRISE_REMOVED                0x00000008
-#define fMP_RECV_LOOKASIDE                  0x00000010
-#define fMP_INIT_IN_PROGRESS                0x00000020
-#define fMP_SEND_SIDE_RESOURCE_ALLOCATED    0x00000040
-#define fMP_RECV_SIDE_RESOURCE_ALLOCATED    0x00000080
 #define fMP_POST_READS                      0x00000100
 #define fMP_POST_WRITES                     0x00000200
-#define fMP_CONTROL_READS                   0x00000400
-#define fMP_CONTROL_WRITES                  0x00000800
 
 #define MP_SET_FLAG(_M, _F)             ((_M)->Flags |= (_F))
 #define MP_CLEAR_FLAG(_M, _F)            ((_M)->Flags &= ~(_F))
 #define MP_TEST_FLAGS(_M, _F)            (((_M)->Flags & (_F)) == (_F))
 
-#define MP_IS_READY(_M)        (((_M)->Flags & \
-                                 (fMP_DISCONNECTED | fMP_RESET_IN_PROGRESS | fMP_HALT_IN_PROGRESS | fMP_INIT_IN_PROGRESS | fMP_SURPRISE_REMOVED)) == 0)
+#define MP_IS_READY(_M)        (((_M)->Flags & fMP_DISCONNECTED) == 0)
 
 int device_alloc_frag_buf(struct vnt_private *, PSDeFragControlBlock pDeF);
 void vnt_configure_filter(struct vnt_private *);
diff --git a/drivers/staging/vt6656/device_cfg.h b/drivers/staging/vt6656/device_cfg.h
deleted file mode 100644
index 0b9d8349c2e..00000000000
--- a/drivers/staging/vt6656/device_cfg.h
+++ /dev/null
@@ -1,87 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: device_cfg.h
- *
- * Purpose: Driver configuration header
- * Author: Lyndon Chen
- *
- * Date: Dec 9, 2005
- *
- */
-#ifndef __DEVICE_CONFIG_H
-#define __DEVICE_CONFIG_H
-
-#include <linux/types.h>
-
-typedef
-struct _version {
-    unsigned char   major;
-    unsigned char   minor;
-    unsigned char   build;
-} version_t, *pversion_t;
-
-#ifndef false
-#define false   (0)
-#endif
-
-#ifndef true
-#define true    (!(false))
-#endif
-
-#define VID_TABLE_SIZE      64
-#define MCAST_TABLE_SIZE    64
-#define MCAM_SIZE           32
-#define VCAM_SIZE           32
-#define TX_QUEUE_NO         8
-
-#define DEVICE_NAME         "vt6656"
-#define DEVICE_FULL_DRV_NAM "VIA Networking Wireless LAN USB Driver"
-
-#ifndef MAJOR_VERSION
-#define MAJOR_VERSION       1
-#endif
-
-#ifndef MINOR_VERSION
-#define MINOR_VERSION       13
-#endif
-
-#ifndef DEVICE_VERSION
-#define DEVICE_VERSION       "1.19_12"
-#endif
-
-#define MAX_RATE	12
-
-/* config file */
-#include <linux/fs.h>
-#include <linux/fcntl.h>
-#ifndef CONFIG_PATH
-#define CONFIG_PATH            "/etc/vntconfiguration.dat"
-#endif
-
-/* Max: 2378 = 2312 Payload + 30HD + 4CRC + 2Padding + 4Len + 8TSF + 4RSR */
-#define PKT_BUF_SZ          2390
-
-#define MAX_UINTS           8
-#define OPTION_DEFAULT      { [0 ... MAX_UINTS-1] = -1}
-
-typedef enum  _chip_type {
-    VT3184 = 1
-} CHIP_TYPE, *PCHIP_TYPE;
-
-#endif
diff --git a/drivers/staging/vt6656/dpc.c b/drivers/staging/vt6656/dpc.c
index 4ccaa7e29a1..c0ec5b37aa7 100644
--- a/drivers/staging/vt6656/dpc.c
+++ b/drivers/staging/vt6656/dpc.c
@@ -26,10 +26,7 @@
  *
  * Functions:
  *      device_receive_frame - Rcv 802.11 frame function
- *      s_bAPModeRxCtl- AP Rcv frame filer Ctl.
- *      s_bAPModeRxData- AP Rcv data frame handle
  *      s_bHandleRxEncryption- Rcv decrypted data via on-fly
- *      s_bHostWepRxEncryption- Rcv encrypted data via host
  *      s_byGetRateIdx- get rate index
  *      s_vGetDASA- get data offset
  *      s_vProcessRxMACHeader- Rcv 802.11 and translate to 802.3
@@ -48,12 +45,9 @@
 #include "baseband.h"
 #include "michael.h"
 #include "tkip.h"
-#include "tcrc.h"
 #include "wctl.h"
-#include "hostap.h"
 #include "rf.h"
 #include "iowpa.h"
-#include "aes_ccmp.h"
 #include "datarate.h"
 #include "usbpipe.h"
 
@@ -77,20 +71,10 @@ static void s_vProcessRxMACHeader(struct vnt_private *pDevice,
 	u8 *pbyRxBufferAddr, u32 cbPacketSize, int bIsWEP, int bExtIV,
 	u32 *pcbHeadSize);
 
-static int s_bAPModeRxCtl(struct vnt_private *pDevice, u8 *pbyFrame,
-	s32 iSANodeIndex);
-
-static int s_bAPModeRxData(struct vnt_private *pDevice, struct sk_buff *skb,
-	u32 FrameSize, u32 cbHeaderOffset, s32 iSANodeIndex, s32 iDANodeIndex);
-
 static int s_bHandleRxEncryption(struct vnt_private *pDevice, u8 *pbyFrame,
 	u32 FrameSize, u8 *pbyRsr, u8 *pbyNewRsr, PSKeyItem *pKeyOut,
 	s32 *pbExtIV, u16 *pwRxTSC15_0, u32 *pdwRxTSC47_16);
 
-static int s_bHostWepRxEncryption(struct vnt_private *pDevice, u8 *pbyFrame,
-	u32 FrameSize, u8 *pbyRsr, int bOnFly, PSKeyItem pKey, u8 *pbyNewRsr,
-	s32 *pbExtIV, u16 *pwRxTSC15_0, u32 *pdwRxTSC47_16);
-
 /*+
  *
  * Description:
@@ -261,14 +245,13 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, struct vnt_rcb *pRCB,
 	u32 cbHeaderOffset, cbIVOffset;
 	u32 FrameSize;
 	u16 wEtherType = 0;
-	s32 iSANodeIndex = -1, iDANodeIndex = -1;
+	s32 iSANodeIndex = -1;
 	int ii;
 	u8 *pbyRxSts, *pbyRxRate, *pbySQ, *pby3SQ;
 	u32 cbHeaderSize;
 	PSKeyItem pKey = NULL;
 	u16 wRxTSC15_0 = 0;
 	u32 dwRxTSC47_16 = 0;
-	SKeyItem STempKey;
 	/* signed long ldBm = 0; */
 	int bIsWEP = false; int bExtIV = false;
 	u32 dwWbkStatus;
@@ -390,50 +373,14 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, struct vnt_rcb *pRCB,
         }
     }
 
-    if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-        if (s_bAPModeRxCtl(pDevice, pbyFrame, iSANodeIndex) == true) {
-            return false;
-        }
-    }
-
     if (IS_FC_WEP(pbyFrame)) {
         bool     bRxDecryOK = false;
 
         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"rx WEP pkt\n");
         bIsWEP = true;
-        if ((pDevice->bEnableHostWEP) && (iSANodeIndex >= 0)) {
-            pKey = &STempKey;
-            pKey->byCipherSuite = pMgmt->sNodeDBTable[iSANodeIndex].byCipherSuite;
-            pKey->dwKeyIndex = pMgmt->sNodeDBTable[iSANodeIndex].dwKeyIndex;
-            pKey->uKeyLength = pMgmt->sNodeDBTable[iSANodeIndex].uWepKeyLength;
-            pKey->dwTSC47_16 = pMgmt->sNodeDBTable[iSANodeIndex].dwTSC47_16;
-            pKey->wTSC15_0 = pMgmt->sNodeDBTable[iSANodeIndex].wTSC15_0;
-            memcpy(pKey->abyKey,
-                &pMgmt->sNodeDBTable[iSANodeIndex].abyWepKey[0],
-                pKey->uKeyLength
-                );
-
-            bRxDecryOK = s_bHostWepRxEncryption(pDevice,
-                                                pbyFrame,
-                                                FrameSize,
-                                                pbyRsr,
-                                                pMgmt->sNodeDBTable[iSANodeIndex].bOnFly,
-                                                pKey,
-                                                pbyNewRsr,
-                                                &bExtIV,
-                                                &wRxTSC15_0,
-                                                &dwRxTSC47_16);
-        } else {
-            bRxDecryOK = s_bHandleRxEncryption(pDevice,
-                                                pbyFrame,
-                                                FrameSize,
-                                                pbyRsr,
-                                                pbyNewRsr,
-                                                &pKey,
-                                                &bExtIV,
-                                                &wRxTSC15_0,
-                                                &dwRxTSC47_16);
-        }
+
+	bRxDecryOK = s_bHandleRxEncryption(pDevice, pbyFrame, FrameSize,
+		pbyRsr, pbyNewRsr, &pKey, &bExtIV, &wRxTSC15_0, &dwRxTSC47_16);
 
         if (bRxDecryOK) {
             if ((*pbyNewRsr & NEWRSR_DECRYPTOK) == 0) {
@@ -517,22 +464,6 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, struct vnt_rcb *pRCB,
             }
             pRxPacket->byRxChannel = (*pbyRxSts) >> 2;
 
-            // hostap Deamon handle 802.11 management
-            if (pDevice->bEnableHostapd) {
-	            skb->dev = pDevice->apdev;
-	            //skb->data += 4;
-	            //skb->tail += 4;
-	            skb->data += 8;
-	            skb->tail += 8;
-                skb_put(skb, FrameSize);
-		skb_reset_mac_header(skb);
-	            skb->pkt_type = PACKET_OTHERHOST;
-    	        skb->protocol = htons(ETH_P_802_2);
-	            memset(skb->cb, 0, sizeof(skb->cb));
-	            netif_rx(skb);
-                return true;
-	        }
-
             //
             // Insert the RCB in the Recv Mng list
             //
@@ -625,13 +556,6 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, struct vnt_rcb *pRCB,
         }
     }
 
-    // Now it only supports 802.11g Infrastructure Mode, and support rate must up to 54 Mbps
-    if (pDevice->bDiversityEnable && (FrameSize>50) &&
-	pDevice->op_mode == NL80211_IFTYPE_STATION &&
-       (pDevice->bLinkPass == true)) {
-        BBvAntennaDiversity(pDevice, s_byGetRateIdx(*pbyRxRate), 0);
-    }
-
     // ++++++++ For BaseBand Algorithm +++++++++++++++
     pDevice->uCurrRSSI = *pbyRSSI;
     pDevice->byCurrSQ = *pbySQ;
@@ -654,45 +578,6 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, struct vnt_rcb *pRCB,
     }
 */
 
-    // -----------------------------------------------
-
-    if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) && (pDevice->bEnable8021x == true)){
-        u8    abyMacHdr[24];
-
-        // Only 802.1x packet incoming allowed
-        if (bIsWEP)
-            cbIVOffset = 8;
-        else
-            cbIVOffset = 0;
-        wEtherType = (skb->data[cbIVOffset + 8 + 24 + 6] << 8) |
-                    skb->data[cbIVOffset + 8 + 24 + 6 + 1];
-
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"wEtherType = %04x \n", wEtherType);
-        if (wEtherType == ETH_P_PAE) {
-            skb->dev = pDevice->apdev;
-
-            if (bIsWEP == true) {
-                // strip IV header(8)
-                memcpy(&abyMacHdr[0], (skb->data + 8), 24);
-                memcpy((skb->data + 8 + cbIVOffset), &abyMacHdr[0], 24);
-            }
-
-            skb->data +=  (cbIVOffset + 8);
-            skb->tail +=  (cbIVOffset + 8);
-            skb_put(skb, FrameSize);
-	    skb_reset_mac_header(skb);
-            skb->pkt_type = PACKET_OTHERHOST;
-            skb->protocol = htons(ETH_P_802_2);
-            memset(skb->cb, 0, sizeof(skb->cb));
-            netif_rx(skb);
-            return true;
-
-        }
-        // check if 802.1x authorized
-        if (!(pMgmt->sNodeDBTable[iSANodeIndex].dwFlags & WLAN_STA_AUTHORIZED))
-            return false;
-    }
-
     if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_TKIP)) {
         if (bIsWEP) {
             FrameSize -= 8;  //MIC
@@ -827,26 +712,6 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, struct vnt_rcb *pRCB,
     if (FrameSize < 12)
         return false;
 
-    if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-        if (s_bAPModeRxData(pDevice,
-                            skb,
-                            FrameSize,
-                            cbHeaderOffset,
-                            iSANodeIndex,
-                            iDANodeIndex
-                            ) == false) {
-
-            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);
-                }
-            }
-            return false;
-        }
-
-    }
-
 	skb->data += cbHeaderOffset;
 	skb->tail += cbHeaderOffset;
     skb_put(skb, FrameSize);
@@ -866,112 +731,6 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, struct vnt_rcb *pRCB,
     return true;
 }
 
-static int s_bAPModeRxCtl(struct vnt_private *pDevice, u8 *pbyFrame,
-	s32 iSANodeIndex)
-{
-	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	struct ieee80211_hdr *p802_11Header;
-	CMD_STATUS Status;
-
-    if (IS_CTL_PSPOLL(pbyFrame) || !IS_TYPE_CONTROL(pbyFrame)) {
-
-        p802_11Header = (struct ieee80211_hdr *) (pbyFrame);
-        if (!IS_TYPE_MGMT(pbyFrame)) {
-
-            // Data & PS-Poll packet
-            // check frame class
-            if (iSANodeIndex > 0) {
-                // frame class 3 fliter & checking
-                if (pMgmt->sNodeDBTable[iSANodeIndex].eNodeState < NODE_AUTH) {
-                    // send deauth notification
-                    // reason = (6) class 2 received from nonauth sta
-                    vMgrDeAuthenBeginSta(pDevice,
-                                         pMgmt,
-                                         (u8 *)(p802_11Header->addr2),
-                                         (WLAN_MGMT_REASON_CLASS2_NONAUTH),
-                                         &Status
-                                         );
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: send vMgrDeAuthenBeginSta 1\n");
-                    return true;
-                }
-                if (pMgmt->sNodeDBTable[iSANodeIndex].eNodeState < NODE_ASSOC) {
-                    // send deassoc notification
-                    // reason = (7) class 3 received from nonassoc sta
-                    vMgrDisassocBeginSta(pDevice,
-                                         pMgmt,
-                                         (u8 *)(p802_11Header->addr2),
-                                         (WLAN_MGMT_REASON_CLASS3_NONASSOC),
-                                         &Status
-                                         );
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: send vMgrDisassocBeginSta 2\n");
-                    return true;
-                }
-
-                if (pMgmt->sNodeDBTable[iSANodeIndex].bPSEnable) {
-                    // delcare received ps-poll event
-                    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");
-                    }
-                    else {
-                        // check Data PS state
-                        // if PW bit off, send out all PS bufferring packets.
-                        if (!IS_FC_POWERMGT(pbyFrame)) {
-                            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");
-                        }
-                    }
-                }
-                else {
-                   if (IS_FC_POWERMGT(pbyFrame)) {
-                       pMgmt->sNodeDBTable[iSANodeIndex].bPSEnable = true;
-                       // Once if STA in PS state, enable multicast bufferring
-                       pMgmt->sNodeDBTable[0].bPSEnable = true;
-                   }
-                   else {
-                      // clear all pending PS frame.
-                      if (pMgmt->sNodeDBTable[iSANodeIndex].wEnQueueCnt > 0) {
-                          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");
-
-                      }
-                   }
-                }
-            }
-            else {
-                  vMgrDeAuthenBeginSta(pDevice,
-                                       pMgmt,
-                                       (u8 *)(p802_11Header->addr2),
-                                       (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->addr3);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ADDR2:%pM\n",
-				p802_11Header->addr2);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ADDR1:%pM\n",
-				p802_11Header->addr1);
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: frame_control= %x\n", p802_11Header->frame_control);
-                    return true;
-            }
-        }
-    }
-    return false;
-
-}
-
 static int s_bHandleRxEncryption(struct vnt_private *pDevice, u8 *pbyFrame,
 	u32 FrameSize, u8 *pbyRsr, u8 *pbyNewRsr, PSKeyItem *pKeyOut,
 	s32 *pbExtIV, u16 *pwRxTSC15_0, u32 *pdwRxTSC47_16)
@@ -1098,213 +857,20 @@ static int s_bHandleRxEncryption(struct vnt_private *pDevice, u8 *pbyFrame,
     return true;
 }
 
-static int s_bHostWepRxEncryption(struct vnt_private *pDevice, u8 *pbyFrame,
-	u32 FrameSize, u8 *pbyRsr, int bOnFly, PSKeyItem pKey, u8 *pbyNewRsr,
-	s32 *pbExtIV, u16 *pwRxTSC15_0, u32 *pdwRxTSC47_16)
-{
-	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	struct ieee80211_hdr *pMACHeader;
-	u32 PayloadLen = FrameSize;
-	u8 *pbyIV;
-	u8 byKeyIdx;
-	u8 byDecMode = KEY_CTL_WEP;
-
-	*pwRxTSC15_0 = 0;
-	*pdwRxTSC47_16 = 0;
-
-    pbyIV = pbyFrame + WLAN_HDR_ADDR3_LEN;
-    if ( WLAN_GET_FC_TODS(*(u16 *)pbyFrame) &&
-         WLAN_GET_FC_FROMDS(*(u16 *)pbyFrame) ) {
-         pbyIV += 6;             // 6 is 802.11 address4
-         PayloadLen -= 6;
-    }
-    byKeyIdx = (*(pbyIV+3) & 0xc0);
-    byKeyIdx >>= 6;
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"\nKeyIdx: %d\n", byKeyIdx);
-
-    if (pMgmt->byCSSGK == KEY_CTL_TKIP)
-        byDecMode = KEY_CTL_TKIP;
-    else if (pMgmt->byCSSGK == KEY_CTL_CCMP)
-        byDecMode = KEY_CTL_CCMP;
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"AES:%d %d %d\n", pMgmt->byCSSPK, 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");
-        if ((pDevice->byLocalID <= REV_ID_VT3253_A1) ||
-		(((PSKeyTable)(pKey->pvKeyTable))->bSoftWEP == true) ||
-            (bOnFly == false)) {
-            // Software WEP
-            // 1. 3253A
-            // 2. WEP 256
-            // 3. NotOnFly
-
-            PayloadLen -= (WLAN_HDR_ADDR3_LEN + 4 + 4); // 24 is 802.11 header,4 is IV, 4 is crc
-            memcpy(pDevice->abyPRNG, pbyIV, 3);
-            memcpy(pDevice->abyPRNG + 3, pKey->abyKey, pKey->uKeyLength);
-            rc4_init(&pDevice->SBox, pDevice->abyPRNG, pKey->uKeyLength + 3);
-            rc4_encrypt(&pDevice->SBox, pbyIV+4, pbyIV+4, PayloadLen);
-
-            if (ETHbIsBufferCrc32Ok(pbyIV+4, PayloadLen)) {
-                *pbyNewRsr |= NEWRSR_DECRYPTOK;
-            }
-        }
-    } else if ((byDecMode == KEY_CTL_TKIP) ||
-               (byDecMode == KEY_CTL_CCMP)) {
-        // TKIP/AES
-
-        PayloadLen -= (WLAN_HDR_ADDR3_LEN + 8 + 4); // 24 is 802.11 header, 8 is IV&ExtIV, 4 is crc
-        *pdwRxTSC47_16 = cpu_to_le32(*(u32 *)(pbyIV + 4));
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ExtIV: %x\n", *pdwRxTSC47_16);
-
-        if (byDecMode == KEY_CTL_TKIP) {
-            *pwRxTSC15_0 = cpu_to_le16(MAKEWORD(*(pbyIV+2), *pbyIV));
-        } else {
-            *pwRxTSC15_0 = cpu_to_le16(*(u16 *)pbyIV);
-        }
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"TSC0_15: %x\n", *pwRxTSC15_0);
-
-        if (byDecMode == KEY_CTL_TKIP) {
-
-            if ((pDevice->byLocalID <= REV_ID_VT3253_A1) || (bOnFly == false)) {
-                // Software TKIP
-                // 1. 3253 A
-                // 2. NotOnFly
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"soft KEY_CTL_TKIP \n");
-                pMACHeader = (struct ieee80211_hdr *) (pbyFrame);
-                TKIPvMixKey(pKey->abyKey, pMACHeader->addr2, *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");
-                } else {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ICV FAIL!!!\n");
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"PayloadLen = %d\n", PayloadLen);
-                }
-            }
-        }
-
-        if (byDecMode == KEY_CTL_CCMP) {
-            if (bOnFly == false) {
-                // Software CCMP
-                // NotOnFly
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"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");
-                } else {
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"CCMP MIC fail!\n");
-                }
-            }
-        }
-
-    }// end of TKIP/AES
-
-    if ((*(pbyIV+3) & 0x20) != 0)
-        *pbExtIV = true;
-    return true;
-}
-
-static int s_bAPModeRxData(struct vnt_private *pDevice, struct sk_buff *skb,
-	u32 FrameSize, u32 cbHeaderOffset, s32 iSANodeIndex, s32 iDANodeIndex)
-{
-	struct sk_buff *skbcpy;
-	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	int  bRelayAndForward = false;
-	int bRelayOnly = false;
-	u8 byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
-	u16 wAID;
-
-    if (FrameSize > CB_MAX_BUF_SIZE)
-        return false;
-    // check DA
-    if (is_multicast_ether_addr((u8 *)(skb->data+cbHeaderOffset))) {
-       if (pMgmt->sNodeDBTable[0].bPSEnable) {
-
-	    skbcpy = netdev_alloc_skb(pDevice->dev, pDevice->rx_buf_sz);
-
-        // 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");
-           }
-           else {
-               skbcpy->len = FrameSize;
-               memcpy(skbcpy->data, skb->data+cbHeaderOffset, FrameSize);
-               skb_queue_tail(&(pMgmt->sNodeDBTable[0].sTxPSQueue), skbcpy);
-               pMgmt->sNodeDBTable[0].wEnQueueCnt++;
-               // set tx map
-               pMgmt->abyPSTxMap[0] |= byMask[0];
-           }
-       }
-       else {
-           bRelayAndForward = true;
-       }
-    }
-    else {
-        // check if relay
-        if (BSSbIsSTAInNodeDB(pDevice, (u8 *)(skb->data+cbHeaderOffset), &iDANodeIndex)) {
-            if (pMgmt->sNodeDBTable[iDANodeIndex].eNodeState >= NODE_ASSOC) {
-                if (pMgmt->sNodeDBTable[iDANodeIndex].bPSEnable) {
-                    // queue this skb until next PS tx, and then release.
-
-	                skb->data += cbHeaderOffset;
-	                skb->tail += cbHeaderOffset;
-                    skb_put(skb, FrameSize);
-                    skb_queue_tail(&pMgmt->sNodeDBTable[iDANodeIndex].sTxPSQueue, skb);
-
-                    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]);
-                    return true;
-                }
-                else {
-                    bRelayOnly = true;
-                }
-            }
-        }
-    }
-
-    if (bRelayOnly || bRelayAndForward) {
-        // relay this packet right now
-        if (bRelayAndForward)
-            iDANodeIndex = 0;
-
-        if ((pDevice->uAssocCount > 1) && (iDANodeIndex >= 0)) {
-		bRelayPacketSend(pDevice, (u8 *) (skb->data + cbHeaderOffset),
-				 FrameSize, (unsigned int) iDANodeIndex);
-        }
-
-        if (bRelayOnly)
-            return false;
-    }
-    // none associate, don't forward
-    if (pDevice->uAssocCount == 0)
-        return false;
-
-    return true;
-}
-
 void RXvWorkItem(struct work_struct *work)
 {
 	struct vnt_private *priv =
 		container_of(work, struct vnt_private, read_work_item);
 	int status;
 	struct vnt_rcb *rcb = NULL;
+	unsigned long flags;
 
 	if (priv->Flags & fMP_DISCONNECTED)
 		return;
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->Rx Polling Thread\n");
 
-	spin_lock_irq(&priv->lock);
+	spin_lock_irqsave(&priv->lock, flags);
 
 	while ((priv->Flags & fMP_POST_READS) && MP_IS_READY(priv) &&
 			(priv->NumRecvFreeList != 0)) {
@@ -1319,7 +885,7 @@ void RXvWorkItem(struct work_struct *work)
 
 	priv->bIsRxWorkItemQueued = false;
 
-	spin_unlock_irq(&priv->lock);
+	spin_unlock_irqrestore(&priv->lock, flags);
 }
 
 void RXvFreeRCB(struct vnt_rcb *rcb, int re_alloc_skb)
@@ -1363,34 +929,43 @@ void RXvMngWorkItem(struct work_struct *work)
 	struct vnt_rcb *pRCB = NULL;
 	struct vnt_rx_mgmt *pRxPacket;
 	int bReAllocSkb = false;
+	unsigned long flags;
 
 	if (pDevice->Flags & fMP_DISCONNECTED)
 		return;
 
     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->Rx Mng Thread\n");
 
-    spin_lock_irq(&pDevice->lock);
     while (pDevice->NumRecvMngList!=0)
     {
+	spin_lock_irqsave(&pDevice->lock, flags);
+
         pRCB = pDevice->FirstRecvMngList;
         pDevice->NumRecvMngList--;
         DequeueRCB(pDevice->FirstRecvMngList, pDevice->LastRecvMngList);
+
+	spin_unlock_irqrestore(&pDevice->lock, flags);
+
         if(!pRCB){
             break;
         }
         pRxPacket = &(pRCB->sMngPacket);
 	vMgrRxManagePacket(pDevice, &pDevice->vnt_mgmt, pRxPacket);
         pRCB->Ref--;
-        if(pRCB->Ref == 0) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"RxvFreeMng %d %d\n",pDevice->NumRecvFreeList, pDevice->NumRecvMngList);
-            RXvFreeRCB(pRCB, bReAllocSkb);
-        } else {
+	if (pRCB->Ref == 0) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"RxvFreeMng %d %d\n",
+			pDevice->NumRecvFreeList, pDevice->NumRecvMngList);
+
+		spin_lock_irqsave(&pDevice->lock, flags);
+
+		RXvFreeRCB(pRCB, bReAllocSkb);
+
+		spin_unlock_irqrestore(&pDevice->lock, flags);
+	} else {
             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Rx Mng Only we have the right to free RCB\n");
         }
     }
 
 	pDevice->bIsRxMngWorkItemQueued = false;
-	spin_unlock_irq(&pDevice->lock);
-
 }
 
diff --git a/drivers/staging/vt6656/firmware.c b/drivers/staging/vt6656/firmware.c
index cd2ea76c8b1..1159f0b3457 100644
--- a/drivers/staging/vt6656/firmware.c
+++ b/drivers/staging/vt6656/firmware.c
@@ -31,9 +31,9 @@
  *
  */
 
+#include <linux/compiler.h>
 #include "firmware.h"
-#include "control.h"
-#include "rndis.h"
+#include "usbpipe.h"
 
 static int msglevel = MSG_LEVEL_INFO;
 /* static int msglevel = MSG_LEVEL_DEBUG; */
@@ -54,7 +54,6 @@ int FIRMWAREbDownload(struct vnt_private *pDevice)
 	int ii, rc;
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->Download firmware\n");
-	spin_unlock_irq(&pDevice->lock);
 
 	rc = request_firmware(&fw, FIRMWARE_NAME, dev);
 	if (rc) {
@@ -71,7 +70,7 @@ int FIRMWAREbDownload(struct vnt_private *pDevice)
 		wLength = min_t(int, fw->size - ii, FIRMWARE_CHUNK_SIZE);
 		memcpy(pBuffer, fw->data + ii, wLength);
 
-		NdisStatus = CONTROLnsRequestOutAsyn(pDevice,
+		NdisStatus = vnt_control_out(pDevice,
 						0,
 						0x1200+ii,
 						0x0000,
@@ -91,7 +90,6 @@ free_fw:
 out:
 	kfree(pBuffer);
 
-	spin_lock_irq(&pDevice->lock);
 	return result;
 }
 MODULE_FIRMWARE(FIRMWARE_NAME);
@@ -102,7 +100,7 @@ int FIRMWAREbBrach2Sram(struct vnt_private *pDevice)
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->Branch to Sram\n");
 
-	NdisStatus = CONTROLnsRequestOut(pDevice,
+	NdisStatus = vnt_control_out(pDevice,
 					1,
 					0x1200,
 					0x0000,
@@ -118,7 +116,7 @@ int FIRMWAREbCheckVersion(struct vnt_private *pDevice)
 {
 	int ntStatus;
 
-	ntStatus = CONTROLnsRequestIn(pDevice,
+	ntStatus = vnt_control_in(pDevice,
 					MESSAGE_TYPE_READ,
 					0,
 					MESSAGE_REQUEST_VERSION,
diff --git a/drivers/staging/vt6656/hostap.c b/drivers/staging/vt6656/hostap.c
deleted file mode 100644
index 67ba48b9a8d..00000000000
--- a/drivers/staging/vt6656/hostap.c
+++ /dev/null
@@ -1,779 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: hostap.c
- *
- * Purpose: handle hostap daemon ioctl input/out functions
- *
- * Author: Lyndon Chen
- *
- * Date: Oct. 20, 2003
- *
- * Functions:
- *
- * Revision History:
- *
- */
-
-#include "hostap.h"
-#include "iocmd.h"
-#include "mac.h"
-#include "card.h"
-#include "baseband.h"
-#include "wpactl.h"
-#include "key.h"
-#include "datarate.h"
-
-#define VIAWGET_HOSTAPD_MAX_BUF_SIZE 1024
-#define HOSTAP_CRYPT_FLAG_SET_TX_KEY BIT0
-#define HOSTAP_CRYPT_ERR_UNKNOWN_ADDR 3
-#define HOSTAP_CRYPT_ERR_KEY_SET_FAILED 5
-
-static int          msglevel                =MSG_LEVEL_INFO;
-
-/*
- * Description:
- *      register net_device (AP) for hostap daemon
- *
- * Parameters:
- *  In:
- *      pDevice             -
- *      rtnl_locked         -
- *  Out:
- *
- * Return Value:
- *
- */
-
-static int hostap_enable_hostapd(struct vnt_private *pDevice, int rtnl_locked)
-{
-	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);
-
-	pDevice->apdev = kzalloc(sizeof(struct net_device), GFP_KERNEL);
-	if (pDevice->apdev == NULL)
-		return -ENOMEM;
-
-    apdev_priv = netdev_priv(pDevice->apdev);
-    *apdev_priv = *pDevice;
-	memcpy(pDevice->apdev->dev_addr, dev->dev_addr, ETH_ALEN);
-
-	pDevice->apdev->netdev_ops = &apdev_netdev_ops;
-
-	pDevice->apdev->type = ARPHRD_IEEE80211;
-
-	pDevice->apdev->base_addr = dev->base_addr;
-	pDevice->apdev->irq = dev->irq;
-	pDevice->apdev->mem_start = dev->mem_start;
-	pDevice->apdev->mem_end = dev->mem_end;
-	sprintf(pDevice->apdev->name, "%sap", dev->name);
-	if (rtnl_locked)
-		ret = register_netdevice(pDevice->apdev);
-	else
-		ret = register_netdev(pDevice->apdev);
-	if (ret) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: register_netdevice(AP) failed!\n",
-		       dev->name);
-		return -1;
-	}
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: Registered netdevice %s for AP management\n",
-	       dev->name, pDevice->apdev->name);
-
-    KeyvInitTable(pDevice,&pDevice->sKey);
-
-	return 0;
-}
-
-/*
- * Description:
- *      unregister net_device(AP)
- *
- * Parameters:
- *  In:
- *      pDevice             -
- *      rtnl_locked         -
- *  Out:
- *
- * Return Value:
- *
- */
-
-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);
-
-    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);
-	}
-	if (pDevice->apdev)
-		free_netdev(pDevice->apdev);
-	pDevice->apdev = NULL;
-    pDevice->bEnable8021x = false;
-    pDevice->bEnableHostWEP = false;
-    pDevice->bEncryptionEnable = false;
-
-	return 0;
-}
-
-/*
- * Description:
- *      Set enable/disable hostapd mode
- *
- * Parameters:
- *  In:
- *      pDevice             -
- *      rtnl_locked         -
- *  Out:
- *
- * Return Value:
- *
- */
-
-int vt6656_hostap_set_hostapd(struct vnt_private *pDevice,
-	int val, int rtnl_locked)
-{
-	if (val < 0 || val > 1)
-		return -EINVAL;
-
-	if (pDevice->bEnableHostapd == val)
-		return 0;
-
-	pDevice->bEnableHostapd = val;
-
-	if (val)
-		return hostap_enable_hostapd(pDevice, rtnl_locked);
-	else
-		return hostap_disable_hostapd(pDevice, rtnl_locked);
-}
-
-/*
- * Description:
- *      remove station function supported for hostap daemon
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-static int hostap_remove_sta(struct vnt_private *pDevice,
-	struct viawget_hostapd_param *param)
-{
-	unsigned int uNodeIndex;
-
-    if (BSSbIsSTAInNodeDB(pDevice, param->sta_addr, &uNodeIndex)) {
-        BSSvRemoveOneNode(pDevice, uNodeIndex);
-    }
-    else {
-        return -ENOENT;
-    }
-	return 0;
-}
-
-/*
- * Description:
- *      add a station from hostap daemon
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-static int hostap_add_sta(struct vnt_private *pDevice,
-	struct viawget_hostapd_param *param)
-{
-	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	unsigned int uNodeIndex;
-
-	if (!BSSbIsSTAInNodeDB(pDevice, param->sta_addr, &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
-//    pMgmt->sNodeDBTable[uNodeIndex].wListenInterval = 1;
-    pMgmt->sNodeDBTable[uNodeIndex].bPSEnable = false;
-    pMgmt->sNodeDBTable[uNodeIndex].bySuppRate = param->u.add_sta.tx_supp_rates;
-
-    // set max tx rate
-    pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate =
-           pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate;
-    // set max basic rate
-    pMgmt->sNodeDBTable[uNodeIndex].wMaxBasicRate = RATE_2M;
-    // 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);
-
-    pMgmt->sNodeDBTable[uNodeIndex].wAID = (u16)param->u.add_sta.aid;
-
-    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);
-
-	return 0;
-}
-
-/*
- * Description:
- *      get station info
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-
-static int hostap_get_info_sta(struct vnt_private *pDevice,
-	struct viawget_hostapd_param *param)
-{
-	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	unsigned int uNodeIndex;
-
-    if (BSSbIsSTAInNodeDB(pDevice, param->sta_addr, &uNodeIndex)) {
-	    param->u.get_info_sta.inactive_sec =
-	        (jiffies - pMgmt->sNodeDBTable[uNodeIndex].ulLastRxJiffer) / HZ;
-
-	    //param->u.get_info_sta.txexc = pMgmt->sNodeDBTable[uNodeIndex].uTxAttempts;
-	}
-	else {
-	    return -ENOENT;
-	}
-
-	return 0;
-}
-
-/*
- * Description:
- *      set station flag
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-static int hostap_set_flags_sta(struct vnt_private *pDevice,
-		struct viawget_hostapd_param *param)
-{
-	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	unsigned int uNodeIndex;
-
-    if (BSSbIsSTAInNodeDB(pDevice, 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);
-	}
-	else {
-	    return -ENOENT;
-	}
-
-	return 0;
-}
-
-/*
- * Description:
- *      set generic element (wpa ie)
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-static int hostap_set_generic_element(struct vnt_private *pDevice,
-					struct viawget_hostapd_param *param)
-{
-	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-
-    memcpy( pMgmt->abyWPAIE,
-            param->u.generic_elem.data,
-            param->u.generic_elem.len
-           );
-
-    pMgmt->wWPAIELen = 	param->u.generic_elem.len;
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pMgmt->wWPAIELen = %d\n",  pMgmt->wWPAIELen);
-
-    // disable wpa
-    if (pMgmt->wWPAIELen == 0) {
-        pMgmt->eAuthenMode = WMAC_AUTH_OPEN;
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " No WPAIE, Disable WPA \n");
-    } else  {
-        // 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");
-        } else
-            return -EINVAL;
-    }
-
-	return 0;
-}
-
-/*
- * Description:
- *      flush station nodes table.
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *  Out:
- *
- * Return Value:
- *
- */
-
-static void hostap_flush_sta(struct vnt_private *pDevice)
-{
-    // reserved node index =0 for multicast node.
-    BSSvClearNodeDBTable(pDevice, 1);
-    pDevice->uAssocCount = 0;
-
-    return;
-}
-
-/*
- * Description:
- *      set each stations encryption key
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-static int hostap_set_encryption(struct vnt_private *pDevice,
-	struct viawget_hostapd_param *param, int param_len)
-{
-	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	u32 dwKeyIndex = 0;
-	u8 abyKey[MAX_KEY_LEN];
-	u8 abySeq[MAX_KEY_LEN];
-	NDIS_802_11_KEY_RSC   KeyRSC;
-	u8 byKeyDecMode = KEY_CTL_WEP;
-	int ret = 0;
-	s32 iNodeIndex = -1;
-	int ii;
-	bool bKeyTableFull = false;
-	u16 wKeyCtl = 0;
-
-	param->u.crypt.err = 0;
-
-	if (param->u.crypt.alg > WPA_ALG_CCMP)
-		return -EINVAL;
-
-	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");
-		return -EINVAL;
-	}
-
-	if (is_broadcast_ether_addr(param->sta_addr)) {
-		if (param->u.crypt.idx >= MAX_GROUP_KEY)
-			return -EINVAL;
-        iNodeIndex = 0;
-
-	} else {
-	    if (BSSbIsSTAInNodeDB(pDevice, 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");
-	        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);
-
-	if (param->u.crypt.alg == WPA_ALG_NONE) {
-
-        if (pMgmt->sNodeDBTable[iNodeIndex].bOnFly == true) {
-            if (KeybRemoveKey( pDevice,
-                               &(pDevice->sKey),
-                               param->sta_addr,
-                               pMgmt->sNodeDBTable[iNodeIndex].dwKeyIndex
-                              ) == false) {
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "KeybRemoveKey fail \n");
-            }
-            pMgmt->sNodeDBTable[iNodeIndex].bOnFly = false;
-        }
-        pMgmt->sNodeDBTable[iNodeIndex].byKeyIndex = 0;
-        pMgmt->sNodeDBTable[iNodeIndex].dwKeyIndex = 0;
-        pMgmt->sNodeDBTable[iNodeIndex].uWepKeyLength = 0;
-        pMgmt->sNodeDBTable[iNodeIndex].KeyRSC = 0;
-        pMgmt->sNodeDBTable[iNodeIndex].dwTSC47_16 = 0;
-        pMgmt->sNodeDBTable[iNodeIndex].wTSC15_0 = 0;
-        pMgmt->sNodeDBTable[iNodeIndex].byCipherSuite = 0;
-        memset(&pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[0],
-                0,
-                MAX_KEY_LEN
-               );
-
-        return ret;
-	}
-
-    memcpy(abyKey, param->u.crypt.key, param->u.crypt.key_len);
-    // 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],
-            param->u.crypt.key,
-            param->u.crypt.key_len
-           );
-
-    dwKeyIndex = (u32)(param->u.crypt.idx);
-    if (param->u.crypt.flags & HOSTAP_CRYPT_FLAG_SET_TX_KEY) {
-        pDevice->byKeyIndex = (u8)dwKeyIndex;
-        pDevice->bTransmitKey = true;
-        dwKeyIndex |= (1 << 31);
-    }
-
-	if (param->u.crypt.alg == WPA_ALG_WEP) {
-
-        if ((pDevice->bEnable8021x == false) || (iNodeIndex == 0)) {
-            KeybSetDefaultKey(  pDevice,
-                                &(pDevice->sKey),
-                                dwKeyIndex & ~(BIT30 | USE_KEYRSC),
-                                param->u.crypt.key_len,
-                                NULL,
-                                abyKey,
-                                KEY_CTL_WEP
-                             );
-
-        } else {
-            // 8021x enable, individual key
-            dwKeyIndex |= (1 << 30); // set pairwise key
-		if (KeybSetKey(pDevice, &(pDevice->sKey),
-			&param->sta_addr[0],
-			dwKeyIndex & ~(USE_KEYRSC),
-			param->u.crypt.key_len,
-			&KeyRSC, (u8 *)abyKey,
-			KEY_CTL_WEP
-                           ) == true) {
-
-                pMgmt->sNodeDBTable[iNodeIndex].bOnFly = true;
-
-            } else {
-                // Key Table Full
-                pMgmt->sNodeDBTable[iNodeIndex].bOnFly = false;
-                bKeyTableFull = true;
-            }
-        }
-        pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-        pDevice->bEncryptionEnable = true;
-        pMgmt->byCSSPK = KEY_CTL_WEP;
-        pMgmt->byCSSGK = KEY_CTL_WEP;
-        pMgmt->sNodeDBTable[iNodeIndex].byCipherSuite = KEY_CTL_WEP;
-        pMgmt->sNodeDBTable[iNodeIndex].dwKeyIndex = dwKeyIndex;
-        return ret;
-	}
-
-	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);
-
-		dwKeyIndex |= 1 << 29;
-		pMgmt->sNodeDBTable[iNodeIndex].KeyRSC = KeyRSC;
-	}
-
-	if (param->u.crypt.alg == WPA_ALG_TKIP) {
-	    if (param->u.crypt.key_len != MAX_KEY_LEN)
-	        return -EINVAL;
-	    pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;
-        byKeyDecMode = KEY_CTL_TKIP;
-        pMgmt->byCSSPK = KEY_CTL_TKIP;
-        pMgmt->byCSSGK = KEY_CTL_TKIP;
-	}
-
-	if (param->u.crypt.alg == WPA_ALG_CCMP) {
-	    if ((param->u.crypt.key_len != AES_KEY_LEN) ||
-	        (pDevice->byLocalID <= REV_ID_VT3253_A1))
-	        return -EINVAL;
-        pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;
-        byKeyDecMode = KEY_CTL_CCMP;
-        pMgmt->byCSSPK = KEY_CTL_CCMP;
-        pMgmt->byCSSGK = KEY_CTL_CCMP;
-    }
-
-    if (iNodeIndex == 0) {
-       KeybSetDefaultKey(  pDevice,
-                           &(pDevice->sKey),
-                           dwKeyIndex,
-                           param->u.crypt.key_len,
-			&KeyRSC,
-                           abyKey,
-                           byKeyDecMode
-                          );
-       pMgmt->sNodeDBTable[iNodeIndex].bOnFly = true;
-
-    } else {
-        dwKeyIndex |= (1 << 30); // set pairwise key
-        if (KeybSetKey(pDevice,
-                       &(pDevice->sKey),
-                       &param->sta_addr[0],
-                       dwKeyIndex,
-                       param->u.crypt.key_len,
-			&KeyRSC,
-                       (u8 *)abyKey,
-                        byKeyDecMode
-                       ) == true) {
-
-            pMgmt->sNodeDBTable[iNodeIndex].bOnFly = true;
-
-        } else {
-            // Key Table Full
-            pMgmt->sNodeDBTable[iNodeIndex].bOnFly = false;
-            bKeyTableFull = true;
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Key Table Full\n");
-        }
-
-    }
-
-    if (bKeyTableFull == true) {
-        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
-// Todo.. xxxxxx
-        //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
-    pDevice->bEncryptionEnable = true;
-    pMgmt->sNodeDBTable[iNodeIndex].byCipherSuite = byKeyDecMode;
-    pMgmt->sNodeDBTable[iNodeIndex].dwKeyIndex = dwKeyIndex;
-    pMgmt->sNodeDBTable[iNodeIndex].dwTSC47_16 = 0;
-    pMgmt->sNodeDBTable[iNodeIndex].wTSC15_0 = 0;
-
-	return ret;
-}
-
-/*
- * Description:
- *      get each stations encryption key
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-static int hostap_get_encryption(struct vnt_private *pDevice,
-				       struct viawget_hostapd_param *param,
-				       int param_len)
-{
-	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	int ret = 0;
-	int ii;
-	s32 iNodeIndex = 0;
-
-	param->u.crypt.err = 0;
-
-	if (is_broadcast_ether_addr(param->sta_addr)) {
-        iNodeIndex = 0;
-	} else {
-	    if (BSSbIsSTAInNodeDB(pDevice, 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");
-	        return -EINVAL;
-	    }
-	}
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "hostap_get_encryption: %d\n", iNodeIndex);
-    memset(param->u.crypt.seq, 0, 8);
-    for (ii = 0 ; ii < 8 ; ii++) {
-        param->u.crypt.seq[ii] = (u8)pMgmt->sNodeDBTable[iNodeIndex].KeyRSC >> (ii * 8);
-    }
-
-	return ret;
-}
-
-/*
- * Description:
- *      vt6656_hostap_ioctl main function supported for hostap daemon.
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      iw_point  -
- *  Out:
- *
- * Return Value:
- *
- */
-
-int vt6656_hostap_ioctl(struct vnt_private *pDevice, struct iw_point *p)
-{
-	struct viawget_hostapd_param *param;
-	int ret = 0;
-	int ap_ioctl = 0;
-
-	if (p->length < sizeof(struct viawget_hostapd_param) ||
-	    p->length > VIAWGET_HOSTAPD_MAX_BUF_SIZE || !p->pointer)
-		return -EINVAL;
-
-	param = kmalloc((int)p->length, GFP_KERNEL);
-	if (param == NULL)
-		return -ENOMEM;
-
-	if (copy_from_user(param, p->pointer, p->length)) {
-		ret = -EFAULT;
-		goto out;
-	}
-
-	switch (param->cmd) {
-	case VIAWGET_HOSTAPD_SET_ENCRYPTION:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "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");
-        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");
-		ret = -EOPNOTSUPP;
-		goto out;
-	case VIAWGET_HOSTAPD_FLUSH:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "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");
-         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");
-         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");
-		 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");
-		 ret = hostap_set_flags_sta(pDevice, param);
-		break;
-
-	case VIAWGET_HOSTAPD_MLME:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_MLME \n");
-	    return -EOPNOTSUPP;
-
-	case VIAWGET_HOSTAPD_SET_GENERIC_ELEMENT:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "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");
-	    return -EOPNOTSUPP;
-
-	case VIAWGET_HOSTAPD_STA_CLEAR_STATS:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_STA_CLEAR_STATS \n");
-	    ret = -EOPNOTSUPP;
-	    goto out;
-	default:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "vt6656_hostap_ioctl: unknown cmd=%d\n",
-		       (int)param->cmd);
-		ret = -EOPNOTSUPP;
-		goto out;
-	}
-
-	if ((ret == 0) && ap_ioctl) {
-		if (copy_to_user(p->pointer, param, p->length)) {
-			ret = -EFAULT;
-			goto out;
-		}
-	}
-
- out:
-	kfree(param);
-
-	return ret;
-}
-
diff --git a/drivers/staging/vt6656/hostap.h b/drivers/staging/vt6656/hostap.h
deleted file mode 100644
index 6a68f7e69cf..00000000000
--- a/drivers/staging/vt6656/hostap.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- * File: hostap.h
- *
- * Purpose:
- *
- * Author: Lyndon Chen
- *
- * Date: May 21, 2003
- *
- */
-
-#ifndef __HOSTAP_H__
-#define __HOSTAP_H__
-
-#include "device.h"
-
-#define WLAN_RATE_1M    BIT0
-#define WLAN_RATE_2M    BIT1
-#define WLAN_RATE_5M5   BIT2
-#define WLAN_RATE_11M   BIT3
-#define WLAN_RATE_6M    BIT4
-#define WLAN_RATE_9M    BIT5
-#define WLAN_RATE_12M   BIT6
-#define WLAN_RATE_18M   BIT7
-#define WLAN_RATE_24M   BIT8
-#define WLAN_RATE_36M   BIT9
-#define WLAN_RATE_48M   BIT10
-#define WLAN_RATE_54M   BIT11
-
-#ifndef ETH_P_PAE
-#define ETH_P_PAE 0x888E /* Port Access Entity (IEEE 802.1X) */
-#endif /* ETH_P_PAE */
-
-#ifndef ARPHRD_IEEE80211
-#define ARPHRD_IEEE80211 801
-#endif
-
-int vt6656_hostap_set_hostapd(struct vnt_private *, int val, int rtnl_locked);
-int vt6656_hostap_ioctl(struct vnt_private *, struct iw_point *p);
-
-#endif /* __HOSTAP_H__ */
diff --git a/drivers/staging/vt6656/int.c b/drivers/staging/vt6656/int.c
index cca56b2f243..f2a5225b50f 100644
--- a/drivers/staging/vt6656/int.c
+++ b/drivers/staging/vt6656/int.c
@@ -65,15 +65,16 @@ static int msglevel = MSG_LEVEL_INFO; /* MSG_LEVEL_DEBUG */
 -*/
 void INTvWorkItem(struct vnt_private *pDevice)
 {
+	unsigned long flags;
 	int ntStatus;
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->Interrupt Polling Thread\n");
 
-	spin_lock_irq(&pDevice->lock);
+	spin_lock_irqsave(&pDevice->lock, flags);
 
 	ntStatus = PIPEnsInterruptRead(pDevice);
 
-	spin_unlock_irq(&pDevice->lock);
+	spin_unlock_irqrestore(&pDevice->lock, flags);
 }
 
 void INTnsProcessData(struct vnt_private *priv)
diff --git a/drivers/staging/vt6656/iocmd.h b/drivers/staging/vt6656/iocmd.h
index f3406da7336..b957e6d475a 100644
--- a/drivers/staging/vt6656/iocmd.h
+++ b/drivers/staging/vt6656/iocmd.h
@@ -29,397 +29,10 @@
 #ifndef __IOCMD_H__
 #define __IOCMD_H__
 
-// ioctl Command code
-#define MAGIC_CODE	                 0x3142
-#define IOCTL_CMD_TEST	            (SIOCDEVPRIVATE + 0)
-#define IOCTL_CMD_SET			    (SIOCDEVPRIVATE + 1)
-#define IOCTL_CMD_HOSTAPD           (SIOCDEVPRIVATE + 2)
-#define IOCTL_CMD_WPA               (SIOCDEVPRIVATE + 3)
-
-typedef enum tagWMAC_CMD {
-
-    WLAN_CMD_BSS_SCAN,
-    WLAN_CMD_BSS_JOIN,
-    WLAN_CMD_DISASSOC,
-    WLAN_CMD_SET_WEP,
-    WLAN_CMD_GET_LINK,
-    WLAN_CMD_GET_LISTLEN,
-    WLAN_CMD_GET_LIST,
-    WLAN_CMD_GET_MIB,
-    WLAN_CMD_GET_STAT,
-    WLAN_CMD_STOP_MAC,
-    WLAN_CMD_START_MAC,
-    WLAN_CMD_AP_START,
-    WLAN_CMD_SET_HOSTAPD,
-    WLAN_CMD_SET_HOSTAPD_STA,
-    WLAN_CMD_SET_802_1X,
-    WLAN_CMD_SET_HOST_WEP,
-    WLAN_CMD_SET_WPA,
-    WLAN_CMD_GET_NODE_CNT,
-    WLAN_CMD_ZONETYPE_SET,
-    WLAN_CMD_GET_NODE_LIST
-
-} WMAC_CMD, *PWMAC_CMD;
-
 typedef enum tagWZONETYPE {
   ZoneType_USA = 0,
   ZoneType_Japan = 1,
   ZoneType_Europe = 2
 } WZONETYPE;
 
-#define ADHOC	0
-#define INFRA	1
-#define BOTH	2
-#define AP	    3
-
-#define ADHOC_STARTED	   1
-#define ADHOC_JOINTED	   2
-
-#define PHY80211a 0
-#define PHY80211b 1
-#define PHY80211g 2
-
-#define SSID_ID                0
-#define SSID_MAXLEN            32
-#define BSSID_LEN              6
-#define WEP_NKEYS              4
-#define WEP_KEYMAXLEN          29
-#define WEP_40BIT_LEN          5
-#define WEP_104BIT_LEN         13
-#define WEP_232BIT_LEN         16
-
-// Ioctl interface structure
-// Command structure
-//
-typedef struct tagSCmdRequest {
-	u8 name[16];
-	void	*data;
-	u16	    wResult;
-	u16     wCmdCode;
-} __packed SCmdRequest, *PSCmdRequest;
-
-//
-// Scan
-//
-
-typedef struct tagSCmdScan {
-
-    u8	    ssid[SSID_MAXLEN + 2];
-
-} __packed SCmdScan, *PSCmdScan;
-
-//
-// BSS Join
-//
-
-typedef struct tagSCmdBSSJoin {
-
-    u16	    wBSSType;
-    u16     wBBPType;
-    u8	    ssid[SSID_MAXLEN + 2];
-    u32	    uChannel;
-    bool    bPSEnable;
-    bool    bShareKeyAuth;
-
-} __packed SCmdBSSJoin, *PSCmdBSSJoin;
-
-//
-// Zonetype Setting
-//
-
-typedef struct tagSCmdZoneTypeSet {
-
- bool       bWrite;
- WZONETYPE  ZoneType;
-
-} __packed SCmdZoneTypeSet, *PSCmdZoneTypeSet;
-
-typedef struct tagSWPAResult {
-         char	ifname[100];
-	u8 proto;
-	u8 key_mgmt;
-	u8 eap_type;
-         bool authenticated;
-} __packed SWPAResult, *PSWPAResult;
-
-typedef struct tagSCmdStartAP {
-
-    u16	    wBSSType;
-    u16     wBBPType;
-    u8	    ssid[SSID_MAXLEN + 2];
-	u32 uChannel;
-	u32 uBeaconInt;
-    bool    bShareKeyAuth;
-    u8      byBasicRate;
-
-} __packed SCmdStartAP, *PSCmdStartAP;
-
-typedef struct tagSCmdSetWEP {
-
-    bool    bEnableWep;
-    u8      byKeyIndex;
-    u8      abyWepKey[WEP_NKEYS][WEP_KEYMAXLEN];
-    bool    bWepKeyAvailable[WEP_NKEYS];
-    u32     auWepKeyLength[WEP_NKEYS];
-
-} __packed SCmdSetWEP, *PSCmdSetWEP;
-
-typedef struct tagSBSSIDItem {
-
-	u32	    uChannel;
-    u8      abyBSSID[BSSID_LEN];
-    u8      abySSID[SSID_MAXLEN + 1];
-    u16	    wBeaconInterval;
-    u16	    wCapInfo;
-    u8      byNetType;
-    bool    bWEPOn;
-    u32     uRSSI;
-
-} __packed SBSSIDItem;
-
-typedef struct tagSBSSIDList {
-
-	u32		    uItem;
-	SBSSIDItem	sBSSIDList[0];
-} __packed SBSSIDList, *PSBSSIDList;
-
-typedef struct tagSNodeItem {
-    // STA info
-    u16            wAID;
-    u8             abyMACAddr[6];
-    u16            wTxDataRate;
-    u16            wInActiveCount;
-    u16            wEnQueueCnt;
-    u16            wFlags;
-    bool           bPWBitOn;
-    u8             byKeyIndex;
-    u16            wWepKeyLength;
-    u8            abyWepKey[WEP_KEYMAXLEN];
-    // Auto rate fallback vars
-    bool           bIsInFallback;
-    u32            uTxFailures;
-    u32            uTxAttempts;
-    u16            wFailureRatio;
-
-} __packed SNodeItem;
-
-typedef struct tagSNodeList {
-
-	u32		    uItem;
-	SNodeItem	sNodeList[0];
-
-} __packed SNodeList, *PSNodeList;
-
-typedef struct tagSCmdLinkStatus {
-
-    bool    bLink;
-	u16	    wBSSType;
-	u8      byState;
-    u8      abyBSSID[BSSID_LEN];
-    u8      abySSID[SSID_MAXLEN + 2];
-    u32     uChannel;
-    u32     uLinkRate;
-
-} __packed SCmdLinkStatus, *PSCmdLinkStatus;
-
-//
-// 802.11 counter
-//
-typedef struct tagSDot11MIBCount {
-    u32 TransmittedFragmentCount;
-    u32 MulticastTransmittedFrameCount;
-    u32 FailedCount;
-    u32 RetryCount;
-    u32 MultipleRetryCount;
-    u32 RTSSuccessCount;
-    u32 RTSFailureCount;
-    u32 ACKFailureCount;
-    u32 FrameDuplicateCount;
-    u32 ReceivedFragmentCount;
-    u32 MulticastReceivedFrameCount;
-    u32 FCSErrorCount;
-} __packed SDot11MIBCount, *PSDot11MIBCount;
-
-//
-// statistic counter
-//
-typedef struct tagSStatMIBCount {
-    //
-    // ISR status count
-    //
-    u32   dwIsrTx0OK;
-    u32   dwIsrTx1OK;
-    u32   dwIsrBeaconTxOK;
-    u32   dwIsrRxOK;
-    u32   dwIsrTBTTInt;
-    u32   dwIsrSTIMERInt;
-    u32   dwIsrUnrecoverableError;
-    u32   dwIsrSoftInterrupt;
-    u32   dwIsrRxNoBuf;
-    /////////////////////////////////////
-
-	u32 dwIsrUnknown; /* unknown interrupt count */
-
-    // RSR status count
-    //
-    u32   dwRsrFrmAlgnErr;
-    u32   dwRsrErr;
-    u32   dwRsrCRCErr;
-    u32   dwRsrCRCOk;
-    u32   dwRsrBSSIDOk;
-    u32   dwRsrADDROk;
-    u32   dwRsrICVOk;
-    u32   dwNewRsrShortPreamble;
-    u32   dwRsrLong;
-    u32   dwRsrRunt;
-
-    u32   dwRsrRxControl;
-    u32   dwRsrRxData;
-    u32   dwRsrRxManage;
-
-    u32   dwRsrRxPacket;
-    u32   dwRsrRxOctet;
-    u32   dwRsrBroadcast;
-    u32   dwRsrMulticast;
-    u32   dwRsrDirected;
-    // 64-bit OID
-    u32   ullRsrOK;
-
-    // for some optional OIDs (64 bits) and DMI support
-    u32   ullRxBroadcastBytes;
-    u32   ullRxMulticastBytes;
-    u32   ullRxDirectedBytes;
-    u32   ullRxBroadcastFrames;
-    u32   ullRxMulticastFrames;
-    u32   ullRxDirectedFrames;
-
-    u32   dwRsrRxFragment;
-    u32   dwRsrRxFrmLen64;
-    u32   dwRsrRxFrmLen65_127;
-    u32   dwRsrRxFrmLen128_255;
-    u32   dwRsrRxFrmLen256_511;
-    u32   dwRsrRxFrmLen512_1023;
-    u32   dwRsrRxFrmLen1024_1518;
-
-    // TSR0,1 status count
-    //
-	u32 dwTsrTotalRetry[2];        /* total collision retry count */
-	u32 dwTsrOnceRetry[2];         /* this packet had one collision */
-	u32 dwTsrMoreThanOnceRetry[2]; /* this packet had many collisions */
-	u32 dwTsrRetry[2];             /* this packet has ever occur collision,
-					* that is (dwTsrOnceCollision0 plus
-					* dwTsrMoreThanOnceCollision0) */
-    u32   dwTsrACKData[2];
-    u32   dwTsrErr[2];
-    u32   dwAllTsrOK[2];
-    u32   dwTsrRetryTimeout[2];
-    u32   dwTsrTransmitTimeout[2];
-
-    u32   dwTsrTxPacket[2];
-    u32   dwTsrTxOctet[2];
-    u32   dwTsrBroadcast[2];
-    u32   dwTsrMulticast[2];
-    u32   dwTsrDirected[2];
-
-    // RD/TD count
-    u32   dwCntRxFrmLength;
-    u32   dwCntTxBufLength;
-
-    u8    abyCntRxPattern[16];
-    u8    abyCntTxPattern[16];
-
-	/* Software check.... */
-	u32 dwCntRxDataErr;  /* rx buffer data CRC err count */
-	u32 dwCntDecryptErr; /* rx buffer data CRC err count */
-	u32 dwCntRxICVErr;   /* rx buffer data CRC err count */
-	u32 idxRxErrorDesc;  /* index for rx data error RD */
-
-	/* 64-bit OID */
-	u32   ullTsrOK[2];
-
-    // for some optional OIDs (64 bits) and DMI support
-    u32   ullTxBroadcastFrames[2];
-    u32   ullTxMulticastFrames[2];
-    u32   ullTxDirectedFrames[2];
-    u32   ullTxBroadcastBytes[2];
-    u32   ullTxMulticastBytes[2];
-    u32   ullTxDirectedBytes[2];
-} __packed SStatMIBCount, *PSStatMIBCount;
-
-typedef struct tagSCmdValue {
-
-    u32     dwValue;
-
-} __packed SCmdValue,  *PSCmdValue;
-
-//
-// hostapd & viawget ioctl related
-//
-
-// VIAGWET_IOCTL_HOSTAPD ioctl() cmd:
-enum {
-	VIAWGET_HOSTAPD_FLUSH = 1,
-	VIAWGET_HOSTAPD_ADD_STA = 2,
-	VIAWGET_HOSTAPD_REMOVE_STA = 3,
-	VIAWGET_HOSTAPD_GET_INFO_STA = 4,
-	VIAWGET_HOSTAPD_SET_ENCRYPTION = 5,
-	VIAWGET_HOSTAPD_GET_ENCRYPTION = 6,
-	VIAWGET_HOSTAPD_SET_FLAGS_STA = 7,
-	VIAWGET_HOSTAPD_SET_ASSOC_AP_ADDR = 8,
-	VIAWGET_HOSTAPD_SET_GENERIC_ELEMENT = 9,
-	VIAWGET_HOSTAPD_MLME = 10,
-	VIAWGET_HOSTAPD_SCAN_REQ = 11,
-	VIAWGET_HOSTAPD_STA_CLEAR_STATS = 12,
-};
-
-#define VIAWGET_HOSTAPD_GENERIC_ELEMENT_HDR_LEN \
-((int) (&((struct viawget_hostapd_param *) 0)->u.generic_elem.data))
-
-// Maximum length for algorithm names (-1 for nul termination) used in ioctl()
-
-struct viawget_hostapd_param {
-	u32 cmd;
-	u8 sta_addr[6];
-	union {
-		struct {
-			u16 aid;
-			u16 capability;
-			u8 tx_supp_rates;
-		} add_sta;
-		struct {
-			u32 inactive_sec;
-		} get_info_sta;
-		struct {
-			u8 alg;
-			u32 flags;
-			u32 err;
-			u8 idx;
-			u8 seq[8];
-			u16 key_len;
-			u8 key[0];
-		} crypt;
-		struct {
-			u32 flags_and;
-			u32 flags_or;
-		} set_flags_sta;
-		struct {
-			u16 rid;
-			u16 len;
-			u8 data[0];
-		} rid;
-		struct {
-			u8 len;
-			u8 data[0];
-		} generic_elem;
-		struct {
-			u16 cmd;
-			u16 reason_code;
-		} mlme;
-		struct {
-			u8 ssid_len;
-			u8 ssid[32];
-		} scan_req;
-	} u;
-} __packed;
-
 #endif /* __IOCMD_H__ */
diff --git a/drivers/staging/vt6656/iowpa.h b/drivers/staging/vt6656/iowpa.h
index 97af32e1fa4..98f2b2195af 100644
--- a/drivers/staging/vt6656/iowpa.h
+++ b/drivers/staging/vt6656/iowpa.h
@@ -31,43 +31,6 @@
 
 #define WPA_IE_LEN 64
 
-//WPA related
-/*
-typedef enum { WPA_ALG_NONE, WPA_ALG_WEP, WPA_ALG_TKIP, WPA_ALG_CCMP } wpa_alg;
-typedef enum { CIPHER_NONE, CIPHER_WEP40, CIPHER_TKIP, CIPHER_CCMP,
-	       CIPHER_WEP104 } wpa_cipher;
-typedef enum { KEY_MGMT_802_1X, KEY_MGMT_PSK, KEY_MGMT_NONE,
-	       KEY_MGMT_802_1X_NO_WPA, KEY_MGMT_WPA_NONE } wpa_key_mgmt;
-*/
-
-enum {
-	VIAWGET_SET_WPA = 1,
-	VIAWGET_SET_KEY = 2,
-	VIAWGET_SET_SCAN = 3,
-	VIAWGET_GET_SCAN = 4,
-	VIAWGET_GET_SSID = 5,
-	VIAWGET_GET_BSSID = 6,
-	VIAWGET_SET_DROP_UNENCRYPT = 7,
-	VIAWGET_SET_DEAUTHENTICATE = 8,
-	VIAWGET_SET_ASSOCIATE = 9,
-	VIAWGET_SET_DISASSOCIATE = 10
-};
-
-enum {
-	VIAWGET_ASSOC_MSG = 1,
-	VIAWGET_DISASSOC_MSG = 2,
-	VIAWGET_PTK_MIC_MSG = 3,
-	VIAWGET_GTK_MIC_MSG = 4,
-	VIAWGET_CCKM_ROAM_MSG = 5,
-	VIAWGET_DEVICECLOSE_MSG = 6
-};
-
-typedef struct viawget_wpa_header {
-	u8 type;
-	u16 req_ie_len;
-	u16 resp_ie_len;
-} __packed viawget_wpa_header;
-
 struct viawget_wpa_param {
 	u32 cmd;
 	u8 addr[6];
@@ -109,20 +72,4 @@ struct viawget_wpa_param {
 	} u;
 } __packed;
 
-struct viawget_scan_result {
-	u8 bssid[6];
-	u8 ssid[32];
-	u16 ssid_len;
-	u8 wpa_ie[WPA_IE_LEN];
-	u16 wpa_ie_len;
-	u8 rsn_ie[WPA_IE_LEN];
-	u16 rsn_ie_len;
-	int freq; // MHz
-	int caps; // e.g. privacy
-	int qual; // signal quality
-	int noise;
-	int level;
-	int maxrate;
-} __packed;
-
 #endif /* __IOWPA_H__ */
diff --git a/drivers/staging/vt6656/iwctl.c b/drivers/staging/vt6656/iwctl.c
index cf4c06a4288..c43718d788c 100644
--- a/drivers/staging/vt6656/iwctl.c
+++ b/drivers/staging/vt6656/iwctl.c
@@ -34,13 +34,11 @@
 #include "iwctl.h"
 #include "mac.h"
 #include "card.h"
-#include "hostap.h"
 #include "power.h"
 #include "rf.h"
 #include "iowpa.h"
 #include "wpactl.h"
-#include "control.h"
-#include "rndis.h"
+#include "usbpipe.h"
 #include "baseband.h"
 
 static const long frequency_list[] = {
@@ -59,7 +57,7 @@ struct iw_statistics *iwctl_get_wireless_stats(struct net_device *dev)
 	long ldBm;
 
 	pDevice->wstats.status = pDevice->op_mode;
-	RFvRSSITodBm(pDevice, (u8)(pDevice->uCurrRSSI), &ldBm);
+	vnt_rf_rssi_to_dbm(pDevice, (u8)(pDevice->uCurrRSSI), &ldBm);
 	pDevice->wstats.qual.level = ldBm;
 	pDevice->wstats.qual.noise = 0;
 	pDevice->wstats.qual.updated = 1;
@@ -93,6 +91,7 @@ int iwctl_siwscan(struct net_device *dev, struct iw_request_info *info,
 	struct iw_scan_req *req = (struct iw_scan_req *)extra;
 	u8 abyScanSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
 	PWLAN_IE_SSID pItemSSID = NULL;
+	unsigned long flags;
 
 	if (!(pDevice->flags & DEVICE_FLAGS_OPENED))
 		return -EINVAL;
@@ -117,7 +116,7 @@ int iwctl_siwscan(struct net_device *dev, struct iw_request_info *info,
 		return 0;
 	}
 
-	spin_lock_irq(&pDevice->lock);
+	spin_lock_irqsave(&pDevice->lock, flags);
 
 	BSSvClearBSSList((void *)pDevice, pDevice->bLinkPass);
 
@@ -138,7 +137,8 @@ int iwctl_siwscan(struct net_device *dev, struct iw_request_info *info,
 			PRINT_K("SIOCSIWSCAN:[desired_ssid=%s,len=%d]\n", ((PWLAN_IE_SSID)abyScanSSID)->abySSID,
 				((PWLAN_IE_SSID)abyScanSSID)->len);
 			bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, abyScanSSID);
-			spin_unlock_irq(&pDevice->lock);
+
+			spin_unlock_irqrestore(&pDevice->lock, flags);
 
 			return 0;
 		} else if (req->scan_type == IW_SCAN_TYPE_PASSIVE) { // passive scan
@@ -150,7 +150,8 @@ int iwctl_siwscan(struct net_device *dev, struct iw_request_info *info,
 
 	pMgmt->eScanType = WMAC_SCAN_PASSIVE;
 	bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, NULL);
-	spin_unlock_irq(&pDevice->lock);
+
+	spin_unlock_irqrestore(&pDevice->lock, flags);
 
 	return 0;
 }
@@ -234,7 +235,7 @@ int iwctl_giwscan(struct net_device *dev, struct iw_request_info *info,
 			// ADD quality
 			memset(&iwe, 0, sizeof(iwe));
 			iwe.cmd = IWEVQUAL;
-			RFvRSSITodBm(pDevice, (u8)(pBSS->uRSSI), &ldBm);
+			vnt_rf_rssi_to_dbm(pDevice, (u8)(pBSS->uRSSI), &ldBm);
 			iwe.u.qual.level = ldBm;
 			iwe.u.qual.noise = 0;
 
@@ -377,6 +378,7 @@ int iwctl_siwmode(struct net_device *dev, struct iw_request_info *info,
 	struct vnt_private *pDevice = netdev_priv(dev);
 	__u32 *wmode = &wrqu->mode;
 	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
+	unsigned long flags;
 	int rc = 0;
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWMODE\n");
@@ -384,12 +386,6 @@ int iwctl_siwmode(struct net_device *dev, struct iw_request_info *info,
 	if (pMgmt == NULL)
 		return -EFAULT;
 
-	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");
-		return rc;
-	}
-
 	switch (*wmode) {
 	case IW_MODE_ADHOC:
 		if (pMgmt->eConfigMode != WMAC_CONFIG_IBSS_STA) {
@@ -409,19 +405,9 @@ int iwctl_siwmode(struct net_device *dev, struct iw_request_info *info,
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to infrastructure\n");
 		break;
 	case IW_MODE_MASTER:
-
-		pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
 		rc = -EOPNOTSUPP;
 		break;
 
-		if (pMgmt->eConfigMode != WMAC_CONFIG_AP) {
-			pMgmt->eConfigMode = WMAC_CONFIG_AP;
-			if (pDevice->flags & DEVICE_FLAGS_OPENED)
-				pDevice->bCommit = true;
-		}
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to Access Point\n");
-		break;
-
 	case IW_MODE_REPEAT:
 		pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
 		rc = -EOPNOTSUPP;
@@ -433,16 +419,17 @@ int iwctl_siwmode(struct net_device *dev, struct iw_request_info *info,
 	if (pDevice->bCommit) {
 		if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
 			netif_stop_queue(pDevice->dev);
-			spin_lock_irq(&pDevice->lock);
+
+			spin_lock_irqsave(&pDevice->lock, flags);
+
 			bScheduleCommand((void *) pDevice,
 				WLAN_CMD_RUN_AP, NULL);
-			spin_unlock_irq(&pDevice->lock);
+
+			spin_unlock_irqrestore(&pDevice->lock, flags);
 		} else {
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
 				"Commit the settings\n");
 
-			spin_lock_irq(&pDevice->lock);
-
 			if (pDevice->bLinkPass &&
 				memcmp(pMgmt->abyCurrSSID,
 					pMgmt->abyDesireSSID,
@@ -455,9 +442,7 @@ int iwctl_siwmode(struct net_device *dev, struct iw_request_info *info,
 				memset(pMgmt->abyCurrBSSID, 0, 6);
 			}
 
-			ControlvMaskByte(pDevice,
-				MESSAGE_REQUEST_MACREG,	MAC_REG_PAPEDELAY,
-					LEDSTS_STS, LEDSTS_SLOW);
+			vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_SLOW);
 
 			netif_stop_queue(pDevice->dev);
 
@@ -471,8 +456,6 @@ int iwctl_siwmode(struct net_device *dev, struct iw_request_info *info,
 			bScheduleCommand((void *) pDevice,
 				 WLAN_CMD_SSID,
 				 NULL);
-
-			spin_unlock_irq(&pDevice->lock);
 		}
 		pDevice->bCommit = false;
 	}
@@ -1172,7 +1155,7 @@ int iwctl_siwencode(struct net_device *dev, struct iw_request_info *info,
 	struct iw_point *wrq = &wrqu->encoding;
 	u32 dwKeyIndex = (u32)(wrq->flags & IW_ENCODE_INDEX);
 	int ii;
-	int uu;
+	u8 uu;
 	int rc = 0;
 	int index = (wrq->flags & IW_ENCODE_INDEX);
 
@@ -1212,14 +1195,12 @@ int iwctl_siwencode(struct net_device *dev, struct iw_request_info *info,
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", pDevice->abyKey[ii]);
 
 		if (pDevice->flags & DEVICE_FLAGS_OPENED) {
-			spin_lock_irq(&pDevice->lock);
 			KeybSetDefaultKey(pDevice,
 					&(pDevice->sKey),
 					dwKeyIndex | (1 << 31),
 					wrq->length, NULL,
 					pDevice->abyKey,
 					KEY_CTL_WEP);
-			spin_unlock_irq(&pDevice->lock);
 		}
 		pDevice->byKeyIndex = (u8)dwKeyIndex;
 		pDevice->uKeyLength = wrq->length;
@@ -1242,10 +1223,8 @@ int iwctl_siwencode(struct net_device *dev, struct iw_request_info *info,
 		pDevice->bEncryptionEnable = false;
 		pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
 		if (pDevice->flags & DEVICE_FLAGS_OPENED) {
-			spin_lock_irq(&pDevice->lock);
 			for (uu = 0; uu < MAX_KEY_TABLE; uu++)
 				MACvDisableKeyEntry(pDevice, uu);
-			spin_unlock_irq(&pDevice->lock);
 		}
 	}
 	if (wrq->flags & IW_ENCODE_RESTRICTED) {
@@ -1342,12 +1321,9 @@ int iwctl_siwpower(struct net_device *dev, struct iw_request_info *info,
 		return rc;
 	}
 
-	spin_lock_irq(&pDevice->lock);
-
 	if (wrq->disabled) {
 		pDevice->ePSMode = WMAC_POWER_CAM;
 		PSvDisablePowerSaving(pDevice);
-		spin_unlock_irq(&pDevice->lock);
 		return rc;
 	}
 	if ((wrq->flags & IW_POWER_TYPE) == IW_POWER_TIMEOUT) {
@@ -1359,8 +1335,6 @@ int iwctl_siwpower(struct net_device *dev, struct iw_request_info *info,
 		PSvEnablePowerSaving((void *)pDevice, pMgmt->wListenInterval);
 	}
 
-	spin_unlock_irq(&pDevice->lock);
-
 	switch (wrq->flags & IW_POWER_MODE) {
 	case IW_POWER_UNICAST_R:
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWPOWER: IW_POWER_UNICAST_R\n");
@@ -1425,7 +1399,7 @@ int iwctl_giwsens(struct net_device *dev, struct iw_request_info *info,
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWSENS\n");
 	if (pDevice->bLinkPass == true) {
-		RFvRSSITodBm(pDevice, (u8)(pDevice->uCurrRSSI), &ldBm);
+		vnt_rf_rssi_to_dbm(pDevice, (u8)(pDevice->uCurrRSSI), &ldBm);
 		wrq->value = ldBm;
 	} else {
 		wrq->value = 0;
@@ -1723,9 +1697,7 @@ int iwctl_siwencodeext(struct net_device *dev, struct iw_request_info *info,
 		KeyvInitTable(pDevice, &pDevice->sKey);
 	}
 /*******/
-	spin_lock_irq(&pDevice->lock);
 	ret = wpa_set_keys(pDevice, param);
-	spin_unlock_irq(&pDevice->lock);
 
 error:
 	kfree(buf);
diff --git a/drivers/staging/vt6656/key.c b/drivers/staging/vt6656/key.c
index be92c048a12..38ea67531d9 100644
--- a/drivers/staging/vt6656/key.c
+++ b/drivers/staging/vt6656/key.c
@@ -39,8 +39,7 @@
 #include "mac.h"
 #include "tmacro.h"
 #include "key.h"
-#include "rndis.h"
-#include "control.h"
+#include "usbpipe.h"
 
 static int          msglevel                =MSG_LEVEL_INFO;
 //static int          msglevel                =MSG_LEVEL_DEBUG;
@@ -68,15 +67,10 @@ static void s_vCheckKeyTableValid(struct vnt_private *pDevice,
             //MACvDisableKeyEntry(pDevice, i);
         }
     }
-    if ( wLength != 0 ) {
-        CONTROLnsRequestOut(pDevice,
-                            MESSAGE_TYPE_CLRKEYENTRY,
-                            0,
-                            0,
-                            wLength,
-                            pbyData
-                            );
-    }
+
+	if (wLength != 0)
+		vnt_control_out(pDevice, MESSAGE_TYPE_CLRKEYENTRY,
+			0, 0, wLength, pbyData);
 
 }
 
@@ -97,7 +91,6 @@ void KeyvInitTable(struct vnt_private *pDevice, PSKeyManagement pTable)
 	int i, jj;
 	u8 pbyData[MAX_KEY_TABLE+1];
 
-    spin_lock_irq(&pDevice->lock);
     for (i=0;i<MAX_KEY_TABLE;i++) {
         pTable->KeyTable[i].bInUse = false;
         pTable->KeyTable[i].PairwiseKey.bKeyValid = false;
@@ -114,15 +107,9 @@ void KeyvInitTable(struct vnt_private *pDevice, PSKeyManagement pTable)
         pbyData[i] = (u8) i;
     }
     pbyData[i] = (u8) i;
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_CLRKEYENTRY,
-                        0,
-                        0,
-                        11,
-                        pbyData
-                        );
 
-    spin_unlock_irq(&pDevice->lock);
+	vnt_control_out(pDevice, MESSAGE_TYPE_CLRKEYENTRY,
+			0, 0, 11, pbyData);
 
     return;
 }
@@ -251,7 +238,9 @@ int KeybSetKey(struct vnt_private *pDevice, PSKeyManagement pTable,
                 if (uKeyLength == WLAN_WEP104_KEYLEN)
                     pKey->abyKey[15] |= 0x80;
             }
-            MACvSetKeyEntry(pDevice, pTable->KeyTable[i].wKeyCtl, i, uKeyIdx, pbyBSSID, (u32 *)pKey->abyKey);
+
+	    MACvSetKeyEntry(pDevice, pTable->KeyTable[i].wKeyCtl, i, uKeyIdx,
+			pbyBSSID, pKey->abyKey);
 
 		if ((dwKeyIndex & USE_KEYRSC) == 0)
 			pKey->KeyRSC = 0; /* RSC set by NIC */
@@ -319,7 +308,9 @@ int KeybSetKey(struct vnt_private *pDevice, PSKeyManagement pTable,
             if (uKeyLength == WLAN_WEP104_KEYLEN)
                 pKey->abyKey[15] |= 0x80;
         }
-        MACvSetKeyEntry(pDevice, pTable->KeyTable[j].wKeyCtl, j, uKeyIdx, pbyBSSID, (u32 *)pKey->abyKey);
+
+	MACvSetKeyEntry(pDevice, pTable->KeyTable[j].wKeyCtl, j, uKeyIdx,
+					pbyBSSID, pKey->abyKey);
 
 		if ((dwKeyIndex & USE_KEYRSC) == 0)
 			pKey->KeyRSC = 0; /* RSC set by NIC */
@@ -614,7 +605,9 @@ int KeybSetDefaultKey(struct vnt_private *pDevice, PSKeyManagement pTable,
             pKey->abyKey[15] |= 0x80;
     }
 
-    MACvSetKeyEntry(pDevice, pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl, MAX_KEY_TABLE-1, uKeyIdx, pTable->KeyTable[MAX_KEY_TABLE-1].abyBSSID, (u32 *) pKey->abyKey);
+	MACvSetKeyEntry(pDevice, pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl,
+		MAX_KEY_TABLE-1, uKeyIdx,
+		pTable->KeyTable[MAX_KEY_TABLE-1].abyBSSID, pKey->abyKey);
 
 		if ((dwKeyIndex & USE_KEYRSC) == 0)
 			pKey->KeyRSC = 0; /* RSC set by NIC */
@@ -708,7 +701,8 @@ int KeybSetAllGroupKey(struct vnt_private *pDevice, PSKeyManagement pTable,
                     pKey->abyKey[15] |= 0x80;
             }
 
-            MACvSetKeyEntry(pDevice, pTable->KeyTable[i].wKeyCtl, i, uKeyIdx, pTable->KeyTable[i].abyBSSID, (u32 *) pKey->abyKey);
+	    MACvSetKeyEntry(pDevice, pTable->KeyTable[i].wKeyCtl, i, uKeyIdx,
+			pTable->KeyTable[i].abyBSSID, pKey->abyKey);
 
 		if ((dwKeyIndex & USE_KEYRSC) == 0)
 			pKey->KeyRSC = 0; /* RSC set by NIC */
diff --git a/drivers/staging/vt6656/mac.c b/drivers/staging/vt6656/mac.c
index 3ce19ddbc56..cadf7cd280c 100644
--- a/drivers/staging/vt6656/mac.c
+++ b/drivers/staging/vt6656/mac.c
@@ -35,11 +35,7 @@
 #include "desc.h"
 #include "mac.h"
 #include "80211hdr.h"
-#include "rndis.h"
-#include "control.h"
-
-//static int          msglevel                =MSG_LEVEL_DEBUG;
-static int          msglevel                =MSG_LEVEL_INFO;
+#include "usbpipe.h"
 
 /*
  * Description:
@@ -54,11 +50,11 @@ static int          msglevel                =MSG_LEVEL_INFO;
  * Return Value: none
  *
  */
-void MACvWriteMultiAddr(struct vnt_private *pDevice, u64 mc_filter)
+void MACvWriteMultiAddr(struct vnt_private *priv, u64 mc_filter)
 {
 	__le64 le_mc = cpu_to_le64(mc_filter);
 
-	CONTROLnsRequestOut(pDevice, MESSAGE_TYPE_WRITE, MAC_REG_MAR0,
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_MAR0,
 		MESSAGE_REQUEST_MACREG, sizeof(le_mc), (u8 *)&le_mc);
 }
 
@@ -73,31 +69,20 @@ void MACvWriteMultiAddr(struct vnt_private *pDevice, u64 mc_filter)
  *
  *
  */
-void MACbShutdown(struct vnt_private *pDevice)
+void MACbShutdown(struct vnt_private *priv)
 {
-    CONTROLnsRequestOutAsyn(pDevice,
-                        MESSAGE_TYPE_MACSHUTDOWN,
-                        0,
-                        0,
-                        0,
-                        NULL
-                        );
+	vnt_control_out(priv, MESSAGE_TYPE_MACSHUTDOWN, 0, 0, 0, NULL);
 }
 
-void MACvSetBBType(struct vnt_private *pDevice, u8 byType)
+void MACvSetBBType(struct vnt_private *priv, u8 type)
 {
-	u8 pbyData[2];
-
-    pbyData[0] = byType;
-    pbyData[1] = EnCFG_BBType_MASK;
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE_MASK,
-                        MAC_REG_ENCFG0,
-                        MESSAGE_REQUEST_MACREG,
-			ARRAY_SIZE(pbyData),
-                        pbyData
-                        );
+	u8 data[2];
+
+	data[0] = type;
+	data[1] = EnCFG_BBType_MASK;
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK, MAC_REG_ENCFG0,
+		MESSAGE_REQUEST_MACREG,	ARRAY_SIZE(data), data);
 }
 
 /*
@@ -114,20 +99,10 @@ void MACvSetBBType(struct vnt_private *pDevice, u8 byType)
  * Return Value: none
  *
  */
-void MACvDisableKeyEntry(struct vnt_private *pDevice, u32 uEntryIdx)
+void MACvDisableKeyEntry(struct vnt_private *priv, u8 entry_idx)
 {
-	u8 byData;
-
-    byData = (u8) uEntryIdx;
-
-    //issue write misc fifo command to device
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_CLRKEYENTRY,
-                        0,
-                        0,
-                        1,
-                        &byData
-                        );
+	vnt_control_out(priv, MESSAGE_TYPE_CLRKEYENTRY, 0, 0,
+		sizeof(entry_idx), &entry_idx);
 }
 
 /*
@@ -144,210 +119,137 @@ void MACvDisableKeyEntry(struct vnt_private *pDevice, u32 uEntryIdx)
  * Return Value: none
  *
  */
-void MACvSetKeyEntry(struct vnt_private *pDevice, u16 wKeyCtl, u32 uEntryIdx,
-	u32 uKeyIdx, u8 *pbyAddr, u32 *pdwKey)
+void MACvSetKeyEntry(struct vnt_private *priv, u16 key_ctl, u32 entry_idx,
+	u32 key_idx, u8 *addr, u8 *key)
 {
-	u8 *pbyKey;
-	u16 wOffset;
-	u32 dwData1, dwData2;
-	int ii;
-	u8 pbyData[24];
-
-	if (pDevice->byLocalID <= MAC_REVISION_A1)
-		if (pDevice->vnt_mgmt.byCSSPK == KEY_CTL_CCMP)
+	struct vnt_mac_set_key set_key;
+	u16 offset;
+
+	if (priv->byLocalID <= MAC_REVISION_A1)
+		if (priv->vnt_mgmt.byCSSPK == KEY_CTL_CCMP)
 			return;
 
-    wOffset = MISCFIFO_KEYETRY0;
-    wOffset += (uEntryIdx * MISCFIFO_KEYENTRYSIZE);
-
-    dwData1 = 0;
-    dwData1 |= wKeyCtl;
-    dwData1 <<= 16;
-    dwData1 |= MAKEWORD(*(pbyAddr+4), *(pbyAddr+5));
-
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"1. wOffset: %d, Data: %X,"\
-		" KeyCtl:%X\n", wOffset, dwData1, wKeyCtl);
-
-    dwData2 = 0;
-    dwData2 |= *(pbyAddr+3);
-    dwData2 <<= 8;
-    dwData2 |= *(pbyAddr+2);
-    dwData2 <<= 8;
-    dwData2 |= *(pbyAddr+1);
-    dwData2 <<= 8;
-    dwData2 |= *(pbyAddr+0);
-
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"2. wOffset: %d, Data: %X\n",
-		wOffset, dwData2);
-
-    pbyKey = (u8 *)pdwKey;
-
-    pbyData[0] = (u8)dwData1;
-    pbyData[1] = (u8)(dwData1>>8);
-    pbyData[2] = (u8)(dwData1>>16);
-    pbyData[3] = (u8)(dwData1>>24);
-    pbyData[4] = (u8)dwData2;
-    pbyData[5] = (u8)(dwData2>>8);
-    pbyData[6] = (u8)(dwData2>>16);
-    pbyData[7] = (u8)(dwData2>>24);
-    for (ii = 8; ii < 24; ii++)
-	pbyData[ii] = *pbyKey++;
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_SETKEY,
-                        wOffset,
-                        (u16)uKeyIdx,
-			ARRAY_SIZE(pbyData),
-                        pbyData
-                        );
+	offset = MISCFIFO_KEYETRY0;
+	offset += (entry_idx * MISCFIFO_KEYENTRYSIZE);
+
+	set_key.u.write.key_ctl = cpu_to_le16(key_ctl);
+	memcpy(set_key.u.write.addr, addr, ETH_ALEN);
+
+	/* swap over swap[0] and swap[1] to get correct write order */
+	swap(set_key.u.swap[0], set_key.u.swap[1]);
+
+	memcpy(set_key.key, key, WLAN_KEY_LEN_CCMP);
 
+	dev_dbg(&priv->usb->dev, "offset %d key ctl %d set key %24ph\n",
+				offset, key_ctl, (u8 *)&set_key);
+
+	vnt_control_out(priv, MESSAGE_TYPE_SETKEY, offset,
+		(u16)key_idx, sizeof(struct vnt_mac_set_key), (u8 *)&set_key);
 }
 
-void MACvRegBitsOff(struct vnt_private *pDevice, u8 byRegOfs, u8 byBits)
+void MACvRegBitsOff(struct vnt_private *priv, u8 reg_ofs, u8 bits)
 {
-	u8 pbyData[2];
-
-    pbyData[0] = 0;
-    pbyData[1] = byBits;
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE_MASK,
-                        byRegOfs,
-                        MESSAGE_REQUEST_MACREG,
-			ARRAY_SIZE(pbyData),
-                        pbyData
-                        );
+	u8 data[2];
+
+	data[0] = 0;
+	data[1] = bits;
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK,
+		reg_ofs, MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
 }
 
-void MACvRegBitsOn(struct vnt_private *pDevice, u8 byRegOfs, u8 byBits)
+void MACvRegBitsOn(struct vnt_private *priv, u8 reg_ofs, u8 bits)
 {
-	u8 pbyData[2];
-
-    pbyData[0] = byBits;
-    pbyData[1] = byBits;
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE_MASK,
-                        byRegOfs,
-                        MESSAGE_REQUEST_MACREG,
-			ARRAY_SIZE(pbyData),
-                        pbyData
-                        );
+	u8 data[2];
+
+	data[0] = bits;
+	data[1] = bits;
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK,
+		reg_ofs, MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
 }
 
-void MACvWriteWord(struct vnt_private *pDevice, u8 byRegOfs, u16 wData)
+void MACvWriteWord(struct vnt_private *priv, u8 reg_ofs, u16 word)
 {
-	u8 pbyData[2];
+	u8 data[2];
 
-    pbyData[0] = (u8)(wData & 0xff);
-    pbyData[1] = (u8)(wData >> 8);
+	data[0] = (u8)(word & 0xff);
+	data[1] = (u8)(word >> 8);
 
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE,
-                        byRegOfs,
-                        MESSAGE_REQUEST_MACREG,
-			ARRAY_SIZE(pbyData),
-                        pbyData
-                        );
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE,
+		reg_ofs, MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
+}
 
+void MACvWriteBSSIDAddress(struct vnt_private *priv, u8 *addr)
+{
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_BSSID0,
+		MESSAGE_REQUEST_MACREG, ETH_ALEN, addr);
 }
 
-void MACvWriteBSSIDAddress(struct vnt_private *pDevice, u8 *pbyEtherAddr)
+void MACvEnableProtectMD(struct vnt_private *priv)
 {
-	u8 pbyData[6];
-
-    pbyData[0] = *((u8 *)pbyEtherAddr);
-    pbyData[1] = *((u8 *)pbyEtherAddr+1);
-    pbyData[2] = *((u8 *)pbyEtherAddr+2);
-    pbyData[3] = *((u8 *)pbyEtherAddr+3);
-    pbyData[4] = *((u8 *)pbyEtherAddr+4);
-    pbyData[5] = *((u8 *)pbyEtherAddr+5);
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE,
-                        MAC_REG_BSSID0,
-                        MESSAGE_REQUEST_MACREG,
-			ARRAY_SIZE(pbyData),
-                        pbyData
-                        );
+	u8 data[2];
+
+	data[0] = EnCFG_ProtectMd;
+	data[1] = EnCFG_ProtectMd;
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK,
+		MAC_REG_ENCFG0, MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
 }
 
-void MACvEnableProtectMD(struct vnt_private *pDevice)
+void MACvDisableProtectMD(struct vnt_private *priv)
 {
-	u8 pbyData[2];
-
-    pbyData[0] = EnCFG_ProtectMd;
-    pbyData[1] = EnCFG_ProtectMd;
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE_MASK,
-                        MAC_REG_ENCFG0,
-                        MESSAGE_REQUEST_MACREG,
-			ARRAY_SIZE(pbyData),
-                        pbyData
-                        );
+	u8 data[2];
+
+	data[0] = 0;
+	data[1] = EnCFG_ProtectMd;
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK,
+		MAC_REG_ENCFG0, MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
 }
 
-void MACvDisableProtectMD(struct vnt_private *pDevice)
+void MACvEnableBarkerPreambleMd(struct vnt_private *priv)
 {
-	u8 pbyData[2];
-
-    pbyData[0] = 0;
-    pbyData[1] = EnCFG_ProtectMd;
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE_MASK,
-                        MAC_REG_ENCFG0,
-                        MESSAGE_REQUEST_MACREG,
-			ARRAY_SIZE(pbyData),
-                        pbyData
-                        );
+	u8 data[2];
+
+	data[0] = EnCFG_BarkerPream;
+	data[1] = EnCFG_BarkerPream;
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK,
+		MAC_REG_ENCFG2, MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
 }
 
-void MACvEnableBarkerPreambleMd(struct vnt_private *pDevice)
+void MACvDisableBarkerPreambleMd(struct vnt_private *priv)
 {
-	u8 pbyData[2];
-
-    pbyData[0] = EnCFG_BarkerPream;
-    pbyData[1] = EnCFG_BarkerPream;
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE_MASK,
-                        MAC_REG_ENCFG2,
-                        MESSAGE_REQUEST_MACREG,
-			ARRAY_SIZE(pbyData),
-                        pbyData
-                        );
+	u8 data[2];
+
+	data[0] = 0;
+	data[1] = EnCFG_BarkerPream;
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK,
+		MAC_REG_ENCFG2, MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
 }
 
-void MACvDisableBarkerPreambleMd(struct vnt_private *pDevice)
+void MACvWriteBeaconInterval(struct vnt_private *priv, u16 interval)
 {
-	u8 pbyData[2];
-
-    pbyData[0] = 0;
-    pbyData[1] = EnCFG_BarkerPream;
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE_MASK,
-                        MAC_REG_ENCFG2,
-                        MESSAGE_REQUEST_MACREG,
-			ARRAY_SIZE(pbyData),
-                        pbyData
-                        );
+	u8 data[2];
+
+	data[0] = (u8)(interval & 0xff);
+	data[1] = (u8)(interval >> 8);
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE,
+		MAC_REG_BI, MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
 }
 
-void MACvWriteBeaconInterval(struct vnt_private *pDevice, u16 wInterval)
+void vnt_mac_set_led(struct vnt_private *priv, u8 state, u8 led)
 {
-	u8 pbyData[2];
-
-	pbyData[0] = (u8)(wInterval & 0xff);
-	pbyData[1] = (u8)(wInterval >> 8);
-
-    CONTROLnsRequestOut(pDevice,
-			MESSAGE_TYPE_WRITE,
-			MAC_REG_BI,
-			MESSAGE_REQUEST_MACREG,
-			ARRAY_SIZE(pbyData),
-			pbyData
-			);
+	u8 data[2];
+
+	data[0] = led;
+	data[1] = state;
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK, MAC_REG_PAPEDELAY,
+			MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
+
+	return;
 }
diff --git a/drivers/staging/vt6656/mac.h b/drivers/staging/vt6656/mac.h
index 4053e431ef9..986ca95f720 100644
--- a/drivers/staging/vt6656/mac.h
+++ b/drivers/staging/vt6656/mac.h
@@ -403,11 +403,22 @@
 #define MAC_REVISION_A0     0x00
 #define MAC_REVISION_A1     0x01
 
+struct vnt_mac_set_key {
+	union {
+		struct {
+			u8 addr[ETH_ALEN];
+			__le16 key_ctl;
+		} write __packed;
+		u32 swap[2];
+	} u;
+	u8 key[WLAN_KEY_LEN_CCMP];
+} __packed;
+
 void MACvWriteMultiAddr(struct vnt_private *, u64);
 void MACbShutdown(struct vnt_private *);
 void MACvSetBBType(struct vnt_private *, u8);
-void MACvDisableKeyEntry(struct vnt_private *, u32);
-void MACvSetKeyEntry(struct vnt_private *, u16, u32, u32, u8 *, u32 *);
+void MACvDisableKeyEntry(struct vnt_private *, u8);
+void MACvSetKeyEntry(struct vnt_private *, u16, u32, u32, u8 *, u8 *);
 void MACvRegBitsOff(struct vnt_private *, u8, u8);
 void MACvRegBitsOn(struct vnt_private *, u8, u8);
 void MACvWriteWord(struct vnt_private *, u8, u16);
@@ -417,5 +428,6 @@ void MACvDisableProtectMD(struct vnt_private *);
 void MACvEnableBarkerPreambleMd(struct vnt_private *);
 void MACvDisableBarkerPreambleMd(struct vnt_private *);
 void MACvWriteBeaconInterval(struct vnt_private *, u16);
+void vnt_mac_set_led(struct vnt_private *priv, u8, u8);
 
 #endif /* __MAC_H__ */
diff --git a/drivers/staging/vt6656/main_usb.c b/drivers/staging/vt6656/main_usb.c
index 3c9323069e0..e18071f121c 100644
--- a/drivers/staging/vt6656/main_usb.c
+++ b/drivers/staging/vt6656/main_usb.c
@@ -57,17 +57,15 @@
 #include "power.h"
 #include "wcmd.h"
 #include "iocmd.h"
-#include "tcrc.h"
 #include "rxtx.h"
 #include "bssdb.h"
-#include "hostap.h"
 #include "wpactl.h"
 #include "iwctl.h"
 #include "dpc.h"
 #include "datarate.h"
 #include "rf.h"
 #include "firmware.h"
-#include "control.h"
+#include "usbpipe.h"
 #include "channel.h"
 #include "int.h"
 #include "iowpa.h"
@@ -216,8 +214,6 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd);
 
 static int device_init_registers(struct vnt_private *pDevice);
 static bool device_init_defrag_cb(struct vnt_private *pDevice);
-static void device_init_diversity_timer(struct vnt_private *pDevice);
-static int  device_dma0_tx_80211(struct sk_buff *skb, struct net_device *dev);
 
 static int  ethtool_ioctl(struct net_device *dev, struct ifreq *);
 static void device_free_tx_bufs(struct vnt_private *pDevice);
@@ -262,33 +258,8 @@ device_set_options(struct vnt_private *pDevice) {
     pDevice->byBBType = BBP_TYPE_DEF;
     pDevice->byPacketType = pDevice->byBBType;
     pDevice->byAutoFBCtrl = AUTO_FB_0;
-    pDevice->bUpdateBBVGA = true;
-    pDevice->byFOETuning = 0;
-    pDevice->byAutoPwrTunning = 0;
     pDevice->byPreambleType = 0;
     pDevice->bExistSWNetAddr = false;
-    /* pDevice->bDiversityRegCtlON = true; */
-    pDevice->bDiversityRegCtlON = false;
-}
-
-static void device_init_diversity_timer(struct vnt_private *pDevice)
-{
-    init_timer(&pDevice->TimerSQ3Tmax1);
-    pDevice->TimerSQ3Tmax1.data = (unsigned long)pDevice;
-    pDevice->TimerSQ3Tmax1.function = (TimerFunction)TimerSQ3CallBack;
-    pDevice->TimerSQ3Tmax1.expires = RUN_AT(HZ);
-
-    init_timer(&pDevice->TimerSQ3Tmax2);
-    pDevice->TimerSQ3Tmax2.data = (unsigned long)pDevice;
-    pDevice->TimerSQ3Tmax2.function = (TimerFunction)TimerSQ3CallBack;
-    pDevice->TimerSQ3Tmax2.expires = RUN_AT(HZ);
-
-    init_timer(&pDevice->TimerSQ3Tmax3);
-    pDevice->TimerSQ3Tmax3.data = (unsigned long)pDevice;
-    pDevice->TimerSQ3Tmax3.function = (TimerFunction)TimerSQ3Tmax3CallBack;
-    pDevice->TimerSQ3Tmax3.expires = RUN_AT(HZ);
-
-    return;
 }
 
 /*
@@ -312,8 +283,6 @@ static int device_init_registers(struct vnt_private *pDevice)
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "---->INIbInitAdapter. [%d][%d]\n",
 				DEVICE_INIT_COLD, pDevice->byPacketType);
 
-	spin_lock_irq(&pDevice->lock);
-
 	memcpy(pDevice->abyBroadcastAddr, abyBroadcastAddr, ETH_ALEN);
 	memcpy(pDevice->abySNAP_RFC1042, abySNAP_RFC1042, ETH_ALEN);
 	memcpy(pDevice->abySNAP_Bridgetunnel, abySNAP_Bridgetunnel, ETH_ALEN);
@@ -323,20 +292,17 @@ static int device_init_registers(struct vnt_private *pDevice)
 			if (FIRMWAREbBrach2Sram(pDevice) == false) {
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
 					" FIRMWAREbBrach2Sram fail\n");
-				spin_unlock_irq(&pDevice->lock);
 				return false;
 			}
 		} else {
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
 				" FIRMWAREbDownload fail\n");
-			spin_unlock_irq(&pDevice->lock);
 			return false;
 		}
 	}
 
 	if (!BBbVT3184Init(pDevice)) {
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" BBbVT3184Init fail\n");
-		spin_unlock_irq(&pDevice->lock);
 		return false;
 	}
 
@@ -348,37 +314,31 @@ static int device_init_registers(struct vnt_private *pDevice)
 	init_cmd->long_retry_limit = pDevice->byLongRetryLimit;
 
 	/* issue card_init command to device */
-	ntStatus = CONTROLnsRequestOut(pDevice,
+	ntStatus = vnt_control_out(pDevice,
 		MESSAGE_TYPE_CARDINIT, 0, 0,
 		sizeof(struct vnt_cmd_card_init), (u8 *)init_cmd);
 	if (ntStatus != STATUS_SUCCESS) {
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" Issue Card init fail\n");
-		spin_unlock_irq(&pDevice->lock);
 		return false;
 	}
 
-	ntStatus = CONTROLnsRequestIn(pDevice, MESSAGE_TYPE_INIT_RSP, 0, 0,
+	ntStatus = vnt_control_in(pDevice, MESSAGE_TYPE_INIT_RSP, 0, 0,
 		sizeof(struct vnt_rsp_card_init), (u8 *)init_rsp);
 	if (ntStatus != STATUS_SUCCESS) {
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
 			"Cardinit request in status fail!\n");
-		spin_unlock_irq(&pDevice->lock);
 		return false;
 	}
 
 	/* local ID for AES functions */
-	ntStatus = CONTROLnsRequestIn(pDevice, MESSAGE_TYPE_READ,
+	ntStatus = vnt_control_in(pDevice, MESSAGE_TYPE_READ,
 		MAC_REG_LOCALID, MESSAGE_REQUEST_MACREG, 1,
 			&pDevice->byLocalID);
-	if (ntStatus != STATUS_SUCCESS) {
-		spin_unlock_irq(&pDevice->lock);
+	if (ntStatus != STATUS_SUCCESS)
 		return false;
-	}
 
 	/* do MACbSoftwareReset in MACvInitialize */
 
-	/* force CCK */
-	pDevice->bCCK = true;
 	pDevice->bProtectMode = false;
 	/* only used in 11g type, sync with ERP IE */
 	pDevice->bNonERPPresent = false;
@@ -396,7 +356,7 @@ static int device_init_registers(struct vnt_private *pDevice)
 
 	pDevice->byTopOFDMBasicRate = RATE_24M;
 	pDevice->byTopCCKBasicRate = RATE_1M;
-	pDevice->byRevId = 0;
+
 	/* target to IF pin while programming to RF chip */
 	pDevice->byCurPwr = 0xFF;
 
@@ -461,13 +421,7 @@ static int device_init_registers(struct vnt_private *pDevice)
 			pDevice->byRxAntennaMode = ANT_A;
 		else
 			pDevice->byRxAntennaMode = ANT_B;
-
-		if (pDevice->bDiversityRegCtlON)
-			pDevice->bDiversityEnable = true;
-		else
-			pDevice->bDiversityEnable = false;
 	} else  {
-		pDevice->bDiversityEnable = false;
 		pDevice->byAntennaCount = 1;
 		pDevice->dwTxAntennaSel = 0;
 		pDevice->dwRxAntennaSel = 0;
@@ -489,27 +443,14 @@ static int device_init_registers(struct vnt_private *pDevice)
 		}
 	}
 
-	pDevice->ulDiversityNValue = 100 * 255;
-	pDevice->ulDiversityMValue = 100 * 16;
-	pDevice->byTMax = 1;
-	pDevice->byTMax2 = 4;
-	pDevice->ulSQ3TH = 0;
-	pDevice->byTMax3 = 64;
-
 	/* get Auto Fall Back type */
 	pDevice->byAutoFBCtrl = AUTO_FB_0;
 
-	/* set SCAN Time */
-	pDevice->uScanTime = WLAN_SCAN_MINITIME;
-
 	/* default Auto Mode */
 	/* pDevice->NetworkType = Ndis802_11Automode; */
 	pDevice->eConfigPHYMode = PHY_TYPE_AUTO;
 	pDevice->byBBType = BB_TYPE_11G;
 
-	/* initialize BBP registers */
-	pDevice->ulTxPower = 25;
-
 	/* get channel range */
 	pDevice->byMinChannel = 1;
 	pDevice->byMaxChannel = CB_MAX_CHANNEL;
@@ -517,11 +458,6 @@ static int device_init_registers(struct vnt_private *pDevice)
 	/* get RFType */
 	pDevice->byRFType = init_rsp->rf_type;
 
-	if ((pDevice->byRFType & RF_EMU) != 0) {
-		/* force change RevID for VT3253 emu */
-		pDevice->byRevId = 0x80;
-	}
-
 	/* load vt3266 calibration parameters in EEPROM */
 	if (pDevice->byRFType == RF_VT3226D0) {
 		if ((pDevice->abyEEPROM[EEP_OFS_MAJOR_VER] == 0x1) &&
@@ -532,28 +468,28 @@ static int device_init_registers(struct vnt_private *pDevice)
 			byCalibRXIQ = pDevice->abyEEPROM[EEP_OFS_CALIB_RX_IQ];
 			if (byCalibTXIQ || byCalibTXDC || byCalibRXIQ) {
 			/* CR255, enable TX/RX IQ and DC compensation mode */
-				ControlvWriteByte(pDevice,
+				vnt_control_out_u8(pDevice,
 					MESSAGE_REQUEST_BBREG,
 					0xff,
 					0x03);
 			/* CR251, TX I/Q Imbalance Calibration */
-				ControlvWriteByte(pDevice,
+				vnt_control_out_u8(pDevice,
 					MESSAGE_REQUEST_BBREG,
 					0xfb,
 					byCalibTXIQ);
 			/* CR252, TX DC-Offset Calibration */
-				ControlvWriteByte(pDevice,
+				vnt_control_out_u8(pDevice,
 					MESSAGE_REQUEST_BBREG,
 					0xfC,
 					byCalibTXDC);
 			/* CR253, RX I/Q Imbalance Calibration */
-				ControlvWriteByte(pDevice,
+				vnt_control_out_u8(pDevice,
 					MESSAGE_REQUEST_BBREG,
 					0xfd,
 					byCalibRXIQ);
 			} else {
 			/* CR255, turn off BB Calibration compensation */
-				ControlvWriteByte(pDevice,
+				vnt_control_out_u8(pDevice,
 					MESSAGE_REQUEST_BBREG,
 					0xff,
 					0x0);
@@ -590,24 +526,20 @@ static int device_init_registers(struct vnt_private *pDevice)
 	BBvSetShortSlotTime(pDevice);
 	CARDvSetBSSMode(pDevice);
 
-	if (pDevice->bUpdateBBVGA) {
-		pDevice->byBBVGACurrent = pDevice->abyBBVGA[0];
-		pDevice->byBBVGANew = pDevice->byBBVGACurrent;
+	pDevice->byBBVGACurrent = pDevice->abyBBVGA[0];
+	pDevice->byBBVGANew = pDevice->byBBVGACurrent;
 
-		BBvSetVGAGainOffset(pDevice, pDevice->abyBBVGA[0]);
-	}
+	BBvSetVGAGainOffset(pDevice, pDevice->abyBBVGA[0]);
 
 	pDevice->byRadioCtl = pDevice->abyEEPROM[EEP_OFS_RADIOCTL];
 	pDevice->bHWRadioOff = false;
 
 	if ((pDevice->byRadioCtl & EEP_RADIOCTL_ENABLE) != 0) {
-		ntStatus = CONTROLnsRequestIn(pDevice, MESSAGE_TYPE_READ,
+		ntStatus = vnt_control_in(pDevice, MESSAGE_TYPE_READ,
 			MAC_REG_GPIOCTL1, MESSAGE_REQUEST_MACREG, 1, &byTmp);
 
-		if (ntStatus != STATUS_SUCCESS) {
-			spin_unlock_irq(&pDevice->lock);
+		if (ntStatus != STATUS_SUCCESS)
 			return false;
-		}
 
 		if ((byTmp & GPIO3_DATA) == 0) {
 			pDevice->bHWRadioOff = true;
@@ -619,11 +551,9 @@ static int device_init_registers(struct vnt_private *pDevice)
 
 	}
 
-	ControlvMaskByte(pDevice, MESSAGE_REQUEST_MACREG,
-				MAC_REG_PAPEDELAY, LEDSTS_TMLEN, 0x38);
+	vnt_mac_set_led(pDevice, LEDSTS_TMLEN, 0x38);
 
-	ControlvMaskByte(pDevice, MESSAGE_REQUEST_MACREG,
-				MAC_REG_PAPEDELAY, LEDSTS_STS, LEDSTS_SLOW);
+	vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_SLOW);
 
 	MACvRegBitsOn(pDevice, MAC_REG_GPIOCTL0, 0x01);
 
@@ -634,9 +564,6 @@ static int device_init_registers(struct vnt_private *pDevice)
 		CARDbRadioPowerOn(pDevice);
 	}
 
-
-	spin_unlock_irq(&pDevice->lock);
-
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"<----INIbInitAdapter Exit\n");
 
 	return true;
@@ -709,18 +636,13 @@ vt6656_probe(struct usb_interface *intf, const struct usb_device_id *id)
 
 	device_set_options(pDevice);
 	spin_lock_init(&pDevice->lock);
+	mutex_init(&pDevice->usb_lock);
+
 	INIT_DELAYED_WORK(&pDevice->run_command_work, vRunCommand);
 	INIT_DELAYED_WORK(&pDevice->second_callback_work, BSSvSecondCallBack);
 	INIT_WORK(&pDevice->read_work_item, RXvWorkItem);
 	INIT_WORK(&pDevice->rx_mng_work_item, RXvMngWorkItem);
 
-	pDevice->pControlURB = usb_alloc_urb(0, GFP_ATOMIC);
-	if (!pDevice->pControlURB) {
-		DBG_PRT(MSG_LEVEL_ERR, KERN_ERR"Failed to alloc control urb\n");
-		goto err_netdev;
-	}
-
-	pDevice->tx_80211 = device_dma0_tx_80211;
 	pDevice->vnt_mgmt.pAdapter = (void *) pDevice;
 
 	netdev->netdev_ops = &device_netdev_ops;
@@ -757,9 +679,9 @@ static void device_free_tx_bufs(struct vnt_private *priv)
 	for (ii = 0; ii < priv->cbTD; ii++) {
 		tx_context = priv->apTD[ii];
 		/* deallocate URBs */
-		if (tx_context->pUrb) {
-			usb_kill_urb(tx_context->pUrb);
-			usb_free_urb(tx_context->pUrb);
+		if (tx_context->urb) {
+			usb_kill_urb(tx_context->urb);
+			usb_free_urb(tx_context->urb);
 		}
 
 		kfree(tx_context);
@@ -825,17 +747,17 @@ static bool device_alloc_bufs(struct vnt_private *priv)
 		}
 
 		priv->apTD[ii] = tx_context;
-		tx_context->pDevice = priv;
+		tx_context->priv = priv;
 
 		/* allocate URBs */
-		tx_context->pUrb = usb_alloc_urb(0, GFP_ATOMIC);
-		if (tx_context->pUrb == NULL) {
+		tx_context->urb = usb_alloc_urb(0, GFP_ATOMIC);
+		if (!tx_context->urb) {
 			DBG_PRT(MSG_LEVEL_ERR,
 				KERN_ERR "alloc tx urb failed\n");
 			goto free_tx;
 		}
 
-		tx_context->bBoolInUse = false;
+		tx_context->in_use = false;
 	}
 
 	/* allocate RCB mem */
@@ -976,8 +898,6 @@ static int  device_open(struct net_device *dev)
     }
 
     MP_CLEAR_FLAG(pDevice, fMP_DISCONNECTED);
-    MP_CLEAR_FLAG(pDevice, fMP_CONTROL_READS);
-    MP_CLEAR_FLAG(pDevice, fMP_CONTROL_WRITES);
     MP_SET_FLAG(pDevice, fMP_POST_READS);
     MP_SET_FLAG(pDevice, fMP_POST_WRITES);
 
@@ -999,9 +919,6 @@ static int  device_open(struct net_device *dev)
     pDevice->bRoaming = false;
     pDevice->bIsRoaming = false;
     pDevice->bEnableRoaming = false;
-    if (pDevice->bDiversityRegCtlON) {
-        device_init_diversity_timer(pDevice);
-    }
 
     vMgrObjectInit(pDevice);
 
@@ -1025,7 +942,6 @@ static int  device_open(struct net_device *dev)
 
     /* if WEP key already set by iwconfig but device not yet open */
     if ((pDevice->bEncryptionEnable == true) && (pDevice->bTransmitKey == true)) {
-         spin_lock_irq(&pDevice->lock);
          KeybSetDefaultKey( pDevice,
                             &(pDevice->sKey),
                             pDevice->byKeyIndex | (1 << 31),
@@ -1034,7 +950,7 @@ static int  device_open(struct net_device *dev)
                             pDevice->abyKey,
                             KEY_CTL_WEP
                           );
-         spin_unlock_irq(&pDevice->lock);
+
          pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
     }
 
@@ -1066,7 +982,7 @@ static int device_close(struct net_device *dev)
 {
 	struct vnt_private *pDevice = netdev_priv(dev);
 	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	int uu;
+	u8 uu;
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "device_close1\n");
     if (pDevice == NULL)
@@ -1081,10 +997,9 @@ static int device_close(struct net_device *dev)
         pMgmt->bShareKeyAlgorithm = false;
         pDevice->bEncryptionEnable = false;
         pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
-	spin_lock_irq(&pDevice->lock);
+
 	for (uu = 0; uu < MAX_KEY_TABLE; uu++)
                 MACvDisableKeyEntry(pDevice,uu);
-	spin_unlock_irq(&pDevice->lock);
 
     if ((pDevice->flags & DEVICE_FLAGS_UNPLUG) == false) {
         MACbShutdown(pDevice);
@@ -1097,12 +1012,6 @@ static int device_close(struct net_device *dev)
 	cancel_delayed_work_sync(&pDevice->run_command_work);
 	cancel_delayed_work_sync(&pDevice->second_callback_work);
 
-    if (pDevice->bDiversityRegCtlON) {
-        del_timer(&pDevice->TimerSQ3Tmax1);
-        del_timer(&pDevice->TimerSQ3Tmax2);
-        del_timer(&pDevice->TimerSQ3Tmax3);
-    }
-
 	cancel_work_sync(&pDevice->rx_mng_work_item);
 	cancel_work_sync(&pDevice->read_work_item);
 
@@ -1145,36 +1054,17 @@ static void vt6656_disconnect(struct usb_interface *intf)
 
 	if (device->dev) {
 		unregister_netdev(device->dev);
-
-		usb_kill_urb(device->pControlURB);
-		usb_free_urb(device->pControlURB);
-
 		free_netdev(device->dev);
 	}
 }
 
-static int device_dma0_tx_80211(struct sk_buff *skb, struct net_device *dev)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-
-	spin_lock_irq(&pDevice->lock);
-
-	if (unlikely(pDevice->bStopTx0Pkt))
-		dev_kfree_skb_irq(skb);
-	else
-		vDMA0_tx_80211(pDevice, skb);
-
-	spin_unlock_irq(&pDevice->lock);
-
-	return NETDEV_TX_OK;
-}
-
 static int device_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct vnt_private *pDevice = netdev_priv(dev);
 	struct net_device_stats *stats = &pDevice->stats;
+	unsigned long flags;
 
-	spin_lock_irq(&pDevice->lock);
+	spin_lock_irqsave(&pDevice->lock, flags);
 
 	netif_stop_queue(dev);
 
@@ -1189,13 +1079,13 @@ static int device_xmit(struct sk_buff *skb, struct net_device *dev)
 		goto out;
 	}
 
-	if (nsDMA_tx_packet(pDevice, TYPE_AC0DMA, skb)) {
+	if (nsDMA_tx_packet(pDevice, skb)) {
 		if (netif_queue_stopped(dev))
 			netif_wake_queue(dev);
 	}
 
 out:
-	spin_unlock_irq(&pDevice->lock);
+	spin_unlock_irqrestore(&pDevice->lock, flags);
 
 	return NETDEV_TX_OK;
 }
@@ -1377,7 +1267,7 @@ void vnt_configure_filter(struct vnt_private *priv)
 	u8 tmp = 0;
 	int rc;
 
-	rc = CONTROLnsRequestIn(priv, MESSAGE_TYPE_READ,
+	rc = vnt_control_in(priv, MESSAGE_TYPE_READ,
 		MAC_REG_RCR, MESSAGE_REQUEST_MACREG, 1, &tmp);
 	if (rc == 0)
 		priv->byRxMode = tmp;
@@ -1418,7 +1308,7 @@ void vnt_configure_filter(struct vnt_private *priv)
 		priv->byRxMode &= ~(RCR_UNICAST);
 	}
 
-	ControlvWriteByte(priv, MESSAGE_REQUEST_MACREG,
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_MACREG,
 					MAC_REG_RCR, priv->byRxMode);
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
@@ -1434,20 +1324,9 @@ static struct net_device_stats *device_get_stats(struct net_device *dev)
 
 static int device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 {
-	struct vnt_private *pDevice = netdev_priv(dev);
-	struct iwreq *wrq = (struct iwreq *) rq;
 	int rc = 0;
 
 	switch (cmd) {
-
-	case IOCTL_CMD_HOSTAPD:
-
-		if (!(pDevice->flags & DEVICE_FLAGS_OPENED))
-			rc = -EFAULT;
-
-		rc = vt6656_hostap_ioctl(pDevice, &wrq->u.data);
-		break;
-
 	case SIOCETHTOOL:
 		return ethtool_ioctl(dev, rq);
 
diff --git a/drivers/staging/vt6656/power.c b/drivers/staging/vt6656/power.c
index 43da58927cd..ddbd04695c9 100644
--- a/drivers/staging/vt6656/power.c
+++ b/drivers/staging/vt6656/power.c
@@ -44,8 +44,7 @@
 #include "wcmd.h"
 #include "rxtx.h"
 #include "card.h"
-#include "control.h"
-#include "rndis.h"
+#include "usbpipe.h"
 
 static int msglevel = MSG_LEVEL_INFO;
 
@@ -130,7 +129,7 @@ void PSvDisablePowerSaving(struct vnt_private *pDevice)
 {
 
 	/* disable power saving hw function */
-	CONTROLnsRequestOut(pDevice, MESSAGE_TYPE_DISABLE_PS, 0,
+	vnt_control_out(pDevice, MESSAGE_TYPE_DISABLE_PS, 0,
 						0, 0, NULL);
 
 	/* clear AutoSleep */
@@ -163,7 +162,7 @@ int PSbConsiderPowerDown(struct vnt_private *pDevice, int bCheckRxDMA,
 	u8 byData;
 
 	/* check if already in Doze mode */
-	ControlvReadByte(pDevice, MESSAGE_REQUEST_MACREG,
+	vnt_control_in_u8(pDevice, MESSAGE_REQUEST_MACREG,
 					MAC_REG_PSCTL, &byData);
 
 	if ((byData & PSCTL_PS) != 0)
diff --git a/drivers/staging/vt6656/rf.c b/drivers/staging/vt6656/rf.c
index 1e8f64bff03..3f54ae3cfb4 100644
--- a/drivers/staging/vt6656/rf.c
+++ b/drivers/staging/vt6656/rf.c
@@ -26,7 +26,7 @@
  * Date: Feb. 19, 2004
  *
  * Functions:
- *      IFRFbWriteEmbedded      - Embedded write RF register via MAC
+ *	vnt_rf_write_embedded	- Embedded write RF register via MAC
  *
  * Revision History:
  *	RF_VT3226: RobertYu:20051111, VT3226C0 and before
@@ -38,12 +38,9 @@
 #include "mac.h"
 #include "rf.h"
 #include "baseband.h"
-#include "control.h"
-#include "rndis.h"
+#include "usbpipe.h"
 #include "datarate.h"
 
-static int          msglevel                =MSG_LEVEL_INFO;
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 #define BY_AL2230_REG_LEN     23 //24bit
 #define CB_AL2230_INIT_SEQ    15
 #define AL2230_PWR_IDX_LEN    64
@@ -355,7 +352,7 @@ static u8 al7230_channel_table2[CB_MAX_CHANNEL][3] = {
     };
 
 ///{{RobertYu:20051111
-static u8 at3226_init_table[CB_VT3226_INIT_SEQ][3] = {
+static u8 vt3226_init_table[CB_VT3226_INIT_SEQ][3] = {
     {0x03, 0xFF, 0x80},
     {0x02, 0x82, 0xA1},
     {0x03, 0xC6, 0xA2},
@@ -369,7 +366,7 @@ static u8 at3226_init_table[CB_VT3226_INIT_SEQ][3] = {
     {0x02, 0x00, 0x2A}
     };
 
-static u8 at3226d0_init_table[CB_VT3226_INIT_SEQ][3] = {
+static u8 vt3226d0_init_table[CB_VT3226_INIT_SEQ][3] = {
     {0x03, 0xFF, 0x80},
     {0x03, 0x02, 0x21}, //RobertYu:20060327
     {0x03, 0xC6, 0xA2},
@@ -707,17 +704,17 @@ const u8 RFaby11aChannelIndex[200] = {
  * Return Value: true if succeeded; false if failed.
  *
  */
-int IFRFbWriteEmbedded(struct vnt_private *pDevice, u32 dwData)
+int vnt_rf_write_embedded(struct vnt_private *priv, u32 data)
 {
-	u8 pbyData[4];
+	u8 reg_data[4];
 
-	pbyData[0] = (u8)dwData;
-	pbyData[1] = (u8)(dwData >> 8);
-	pbyData[2] = (u8)(dwData >> 16);
-	pbyData[3] = (u8)(dwData >> 24);
+	reg_data[0] = (u8)data;
+	reg_data[1] = (u8)(data >> 8);
+	reg_data[2] = (u8)(data >> 16);
+	reg_data[3] = (u8)(data >> 24);
 
-	CONTROLnsRequestOut(pDevice,
-		MESSAGE_TYPE_WRITE_IFRF, 0, 0, 4, pbyData);
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE_IFRF,
+				0, 0, ARRAY_SIZE(reg_data), reg_data);
 
 	return true;
 }
@@ -735,7 +732,7 @@ int IFRFbWriteEmbedded(struct vnt_private *pDevice, u32 dwData)
  * Return Value: true if succeeded; false if failed.
  *
  */
-int RFbSetPower(struct vnt_private *priv, u32 rate, u32 channel)
+int vnt_rf_setpower(struct vnt_private *priv, u32 rate, u32 channel)
 {
 	int ret = true;
 	u8 power = priv->byCCKPwr;
@@ -764,11 +761,37 @@ int RFbSetPower(struct vnt_private *priv, u32 rate, u32 channel)
 		break;
 	}
 
-	ret = RFbRawSetPower(priv, power, rate);
+	ret = vnt_rf_set_txpower(priv, power, rate);
 
 	return ret;
 }
 
+static u8 vnt_rf_addpower(struct vnt_private *priv)
+{
+	s32 rssi = -priv->uCurrRSSI;
+
+	if (!rssi)
+		return 7;
+
+	if (priv->byRFType == RF_VT3226D0) {
+		if (rssi < -70)
+			return 9;
+		else if (rssi < -65)
+			return 7;
+		else if (rssi < -60)
+			return 5;
+	} else {
+		if (rssi < -80)
+			return 9;
+		else if (rssi < -75)
+			return 7;
+		else if (rssi < -70)
+			return 5;
+	}
+
+	return 0;
+}
+
 /*
  * Description: Set Tx power
  *
@@ -783,11 +806,15 @@ int RFbSetPower(struct vnt_private *priv, u32 rate, u32 channel)
  *
  */
 
-int RFbRawSetPower(struct vnt_private *priv, u8 power, u32 rate)
+int vnt_rf_set_txpower(struct vnt_private *priv, u8 power, u32 rate)
 {
 	u32 power_setting = 0;
 	int ret = true;
 
+	power += vnt_rf_addpower(priv);
+	if (power > VNT_RF_MAX_POWER)
+		power = VNT_RF_MAX_POWER;
+
 	if (priv->byCurPwr == power)
 		return true;
 
@@ -798,42 +825,42 @@ int RFbRawSetPower(struct vnt_private *priv, u8 power, u32 rate)
 		if (priv->byCurPwr >= AL2230_PWR_IDX_LEN)
 			return false;
 
-		ret &= IFRFbWriteEmbedded(priv,
+		ret &= vnt_rf_write_embedded(priv,
 			al2230_power_table[priv->byCurPwr]);
 
 		if (rate <= RATE_11M)
-			ret &= IFRFbWriteEmbedded(priv, 0x0001b400 +
+			ret &= vnt_rf_write_embedded(priv, 0x0001b400 +
 				(BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW);
 		else
-			ret &= IFRFbWriteEmbedded(priv, 0x0005a400 +
+			ret &= vnt_rf_write_embedded(priv, 0x0005a400 +
 				(BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW);
 		break;
 	case RF_AL2230S:
 		if (priv->byCurPwr >= AL2230_PWR_IDX_LEN)
 			return false;
 
-		ret &= IFRFbWriteEmbedded(priv,
+		ret &= vnt_rf_write_embedded(priv,
 			al2230_power_table[priv->byCurPwr]);
 
 		if (rate <= RATE_11M) {
-			ret &= IFRFbWriteEmbedded(priv, 0x040c1400 +
+			ret &= vnt_rf_write_embedded(priv, 0x040c1400 +
 				(BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW);
-			ret &= IFRFbWriteEmbedded(priv, 0x00299b00 +
+			ret &= vnt_rf_write_embedded(priv, 0x00299b00 +
 				(BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW);
 		} else {
-			ret &= IFRFbWriteEmbedded(priv, 0x0005a400 +
+			ret &= vnt_rf_write_embedded(priv, 0x0005a400 +
 				(BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW);
-			ret &= IFRFbWriteEmbedded(priv, 0x00099b00 +
+			ret &= vnt_rf_write_embedded(priv, 0x00099b00 +
 				(BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW);
 		}
 		break;
 
 	case RF_AIROHA7230:
 		if (rate <= RATE_11M)
-			ret &= IFRFbWriteEmbedded(priv, 0x111bb900 +
+			ret &= vnt_rf_write_embedded(priv, 0x111bb900 +
 				(BY_AL7230_REG_LEN << 3)+IFREGCTL_REGW);
 		else
-			ret &= IFRFbWriteEmbedded(priv, 0x221bb900 +
+			ret &= vnt_rf_write_embedded(priv, 0x221bb900 +
 				(BY_AL7230_REG_LEN << 3)+IFREGCTL_REGW);
 
 		if (priv->byCurPwr > AL7230_PWR_IDX_LEN)
@@ -846,7 +873,7 @@ int RFbRawSetPower(struct vnt_private *priv, u8 power, u32 rate)
 		power_setting = 0x080c0b00 | ((priv->byCurPwr) << 12) |
 				(BY_AL7230_REG_LEN << 3) | IFREGCTL_REGW;
 
-		ret &= IFRFbWriteEmbedded(priv, power_setting);
+		ret &= vnt_rf_write_embedded(priv, power_setting);
 
 		break;
 
@@ -856,7 +883,7 @@ int RFbRawSetPower(struct vnt_private *priv, u8 power, u32 rate)
 		power_setting = ((0x3f - priv->byCurPwr) << 20) | (0x17 << 8) |
 				(BY_VT3226_REG_LEN << 3) | IFREGCTL_REGW;
 
-		ret &= IFRFbWriteEmbedded(priv, power_setting);
+		ret &= vnt_rf_write_embedded(priv, power_setting);
 
 		break;
 	case RF_VT3226D0:
@@ -868,42 +895,42 @@ int RFbRawSetPower(struct vnt_private *priv, u8 power, u32 rate)
 				(0xe07 << 8) | (BY_VT3226_REG_LEN << 3) |
 						IFREGCTL_REGW;
 
-			ret &= IFRFbWriteEmbedded(priv, power_setting);
-			ret &= IFRFbWriteEmbedded(priv, 0x03c6a200 +
+			ret &= vnt_rf_write_embedded(priv, power_setting);
+			ret &= vnt_rf_write_embedded(priv, 0x03c6a200 +
 					(BY_VT3226_REG_LEN<<3)+IFREGCTL_REGW);
 
 			if (priv->vnt_mgmt.eScanState != WMAC_NO_SCANNING) {
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-				"RFbRawSetPower> 11B mode uCurrChannel[%d]\n",
+				dev_dbg(&priv->usb->dev,
+				"vnt_rf_set_txpower> 11B mode uCurrChannel[%d]\n",
 						priv->vnt_mgmt.uScanChannel);
-				ret &= IFRFbWriteEmbedded(priv,
+				ret &= vnt_rf_write_embedded(priv,
 					vt3226d0_lo_current_table[priv->
 						vnt_mgmt.uScanChannel - 1]);
 			} else {
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-				"RFbRawSetPower> 11B mode uCurrChannel[%d]\n",
+				dev_dbg(&priv->usb->dev,
+				"vnt_rf_set_txpower> 11B mode uCurrChannel[%d]\n",
 						priv->vnt_mgmt.uCurrChannel);
-				ret &= IFRFbWriteEmbedded(priv,
+				ret &= vnt_rf_write_embedded(priv,
 					vt3226d0_lo_current_table[priv->
 						vnt_mgmt.uCurrChannel - 1]);
 			}
 
-			ret &= IFRFbWriteEmbedded(priv, 0x015C0800 +
+			ret &= vnt_rf_write_embedded(priv, 0x015C0800 +
 				(BY_VT3226_REG_LEN<<3)+IFREGCTL_REGW);
 		} else {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-					"@@@@ RFbRawSetPower> 11G mode\n");
+			dev_dbg(&priv->usb->dev,
+					"@@@@ vnt_rf_set_txpower> 11G mode\n");
 
 			power_setting = ((0x3f-priv->byCurPwr) << 20) |
 				(0x7 << 8) | (BY_VT3226_REG_LEN << 3) |
 					IFREGCTL_REGW;
 
-			ret &= IFRFbWriteEmbedded(priv, power_setting);
-			ret &= IFRFbWriteEmbedded(priv, 0x00C6A200 +
+			ret &= vnt_rf_write_embedded(priv, power_setting);
+			ret &= vnt_rf_write_embedded(priv, 0x00C6A200 +
 				(BY_VT3226_REG_LEN << 3) + IFREGCTL_REGW);
-			ret &= IFRFbWriteEmbedded(priv, 0x016BC600 +
+			ret &= vnt_rf_write_embedded(priv, 0x016BC600 +
 					(BY_VT3226_REG_LEN<<3)+IFREGCTL_REGW);
-			ret &= IFRFbWriteEmbedded(priv, 0x00900800 +
+			ret &= vnt_rf_write_embedded(priv, 0x00900800 +
 					(BY_VT3226_REG_LEN<<3)+IFREGCTL_REGW);
 		}
 		break;
@@ -916,7 +943,7 @@ int RFbRawSetPower(struct vnt_private *priv, u8 power, u32 rate)
 			(0x27 << 8) | (BY_VT3342_REG_LEN << 3) |
 					IFREGCTL_REGW;
 
-		ret &= IFRFbWriteEmbedded(priv, power_setting);
+		ret &= vnt_rf_write_embedded(priv, power_setting);
 
 		break;
 	default:
@@ -940,7 +967,7 @@ int RFbRawSetPower(struct vnt_private *priv, u8 power, u32 rate)
  * Return Value: none
  *
 -*/
-void RFvRSSITodBm(struct vnt_private *priv, u8 rssi, long *dbm)
+void vnt_rf_rssi_to_dbm(struct vnt_private *priv, u8 rssi, long *dbm)
 {
 	u8 idx = (((rssi & 0xc0) >> 6) & 0x03);
 	long b = (rssi & 0x3f);
@@ -963,7 +990,7 @@ void RFvRSSITodBm(struct vnt_private *priv, u8 rssi, long *dbm)
 	*dbm = -1 * (a + b * 2);
 }
 
-void RFbRFTableDownload(struct vnt_private *priv)
+void vnt_rf_table_download(struct vnt_private *priv)
 {
 	u16 length1 = 0, length2 = 0, length3 = 0;
 	u8 *addr1 = NULL, *addr2 = NULL, *addr3 = NULL;
@@ -992,7 +1019,7 @@ void RFbRFTableDownload(struct vnt_private *priv)
 		length1 = CB_VT3226_INIT_SEQ * 3;
 		length2 = CB_MAX_CHANNEL_24G * 3;
 		length3 = CB_MAX_CHANNEL_24G * 3;
-		addr1 = &at3226_init_table[0][0];
+		addr1 = &vt3226_init_table[0][0];
 		addr2 = &vt3226_channel_table0[0][0];
 		addr3 = &vt3226_channel_table1[0][0];
 		break;
@@ -1000,7 +1027,7 @@ void RFbRFTableDownload(struct vnt_private *priv)
 		length1 = CB_VT3226_INIT_SEQ * 3;
 		length2 = CB_MAX_CHANNEL_24G * 3;
 		length3 = CB_MAX_CHANNEL_24G * 3;
-		addr1 = &at3226d0_init_table[0][0];
+		addr1 = &vt3226d0_init_table[0][0];
 		addr2 = &vt3226_channel_table0[0][0];
 		addr3 = &vt3226_channel_table1[0][0];
 		break;
@@ -1017,7 +1044,7 @@ void RFbRFTableDownload(struct vnt_private *priv)
 	/* Init Table */
 	memcpy(array, addr1, length1);
 
-	CONTROLnsRequestOut(priv, MESSAGE_TYPE_WRITE, 0,
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE, 0,
 		MESSAGE_REQUEST_RF_INIT, length1, array);
 
 	/* Channel Table 0 */
@@ -1030,7 +1057,7 @@ void RFbRFTableDownload(struct vnt_private *priv)
 
 		memcpy(array, addr2, length);
 
-		CONTROLnsRequestOut(priv, MESSAGE_TYPE_WRITE,
+		vnt_control_out(priv, MESSAGE_TYPE_WRITE,
 			value, MESSAGE_REQUEST_RF_CH0, length, array);
 
 		length2 -= length;
@@ -1048,7 +1075,7 @@ void RFbRFTableDownload(struct vnt_private *priv)
 
 		memcpy(array, addr3, length);
 
-		CONTROLnsRequestOut(priv, MESSAGE_TYPE_WRITE,
+		vnt_control_out(priv, MESSAGE_TYPE_WRITE,
 			value, MESSAGE_REQUEST_RF_CH1, length, array);
 
 		length3 -= length;
@@ -1065,7 +1092,7 @@ void RFbRFTableDownload(struct vnt_private *priv)
 		memcpy(array, addr1, length1);
 
 		/* Init Table 2 */
-		CONTROLnsRequestOut(priv, MESSAGE_TYPE_WRITE,
+		vnt_control_out(priv, MESSAGE_TYPE_WRITE,
 			0, MESSAGE_REQUEST_RF_INIT2, length1, array);
 
 		/* Channel Table 0 */
@@ -1078,7 +1105,7 @@ void RFbRFTableDownload(struct vnt_private *priv)
 
 			memcpy(array, addr2, length);
 
-			CONTROLnsRequestOut(priv, MESSAGE_TYPE_WRITE,
+			vnt_control_out(priv, MESSAGE_TYPE_WRITE,
 				value, MESSAGE_REQUEST_RF_CH2, length, array);
 
 			length2 -= length;
diff --git a/drivers/staging/vt6656/rf.h b/drivers/staging/vt6656/rf.h
index de5c613b4ef..cb331518568 100644
--- a/drivers/staging/vt6656/rf.h
+++ b/drivers/staging/vt6656/rf.h
@@ -53,12 +53,14 @@
 #define RF_EMU              0x80
 #define RF_MASK             0x7F
 
+#define VNT_RF_MAX_POWER    0x3f
+
 extern const u8 RFaby11aChannelIndex[200];
 
-int IFRFbWriteEmbedded(struct vnt_private *, u32 dwData);
-int RFbSetPower(struct vnt_private *, u32 uRATE, u32 uCH);
-int RFbRawSetPower(struct vnt_private *, u8 byPwr, u32 uRATE);
-void RFvRSSITodBm(struct vnt_private *, u8 byCurrRSSI, long *pldBm);
-void RFbRFTableDownload(struct vnt_private *pDevice);
+int vnt_rf_write_embedded(struct vnt_private *, u32);
+int vnt_rf_setpower(struct vnt_private *, u32, u32);
+int vnt_rf_set_txpower(struct vnt_private *, u8, u32);
+void vnt_rf_rssi_to_dbm(struct vnt_private *, u8, long *);
+void vnt_rf_table_download(struct vnt_private *);
 
 #endif /* __RF_H__ */
diff --git a/drivers/staging/vt6656/rndis.h b/drivers/staging/vt6656/rndis.h
deleted file mode 100644
index 3661f82766e..00000000000
--- a/drivers/staging/vt6656/rndis.h
+++ /dev/null
@@ -1,151 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: rndis.h
- *
- * Purpose: Interface between firmware and driver
- *
- * Author: Warren Hsu
- *
- * Date: Nov 24, 2004
- *
- */
-
-#ifndef __RNDIS_H__
-#define __RNDIS_H__
-
-#define MESSAGE_TYPE_READ               0x01
-#define MESSAGE_TYPE_WRITE              0x00
-#define MESSAGE_TYPE_LOCK_OR            0x02
-#define MESSAGE_TYPE_LOCK_AND           0x03
-#define MESSAGE_TYPE_WRITE_MASK         0x04
-#define MESSAGE_TYPE_CARDINIT           0x05
-#define MESSAGE_TYPE_INIT_RSP           0x06
-#define MESSAGE_TYPE_MACSHUTDOWN        0x07
-#define MESSAGE_TYPE_SETKEY             0x08
-#define MESSAGE_TYPE_CLRKEYENTRY        0x09
-#define MESSAGE_TYPE_WRITE_MISCFF       0x0A
-#define MESSAGE_TYPE_SET_ANTMD          0x0B
-#define MESSAGE_TYPE_SELECT_CHANNLE     0x0C
-#define MESSAGE_TYPE_SET_TSFTBTT        0x0D
-#define MESSAGE_TYPE_SET_SSTIFS         0x0E
-#define MESSAGE_TYPE_CHANGE_BBTYPE      0x0F
-#define MESSAGE_TYPE_DISABLE_PS         0x10
-#define MESSAGE_TYPE_WRITE_IFRF         0x11
-
-//used for read/write(index)
-#define MESSAGE_REQUEST_MEM             0x01
-#define MESSAGE_REQUEST_BBREG           0x02
-#define MESSAGE_REQUEST_MACREG          0x03
-#define MESSAGE_REQUEST_EEPROM          0x04
-#define MESSAGE_REQUEST_TSF             0x05
-#define MESSAGE_REQUEST_TBTT            0x06
-#define MESSAGE_REQUEST_BBAGC           0x07
-#define MESSAGE_REQUEST_VERSION         0x08
-#define MESSAGE_REQUEST_RF_INIT         0x09
-#define MESSAGE_REQUEST_RF_INIT2        0x0A
-#define MESSAGE_REQUEST_RF_CH0          0x0B
-#define MESSAGE_REQUEST_RF_CH1          0x0C
-#define MESSAGE_REQUEST_RF_CH2          0x0D
-
-#define VIAUSB20_PACKET_HEADER          0x04
-
-#define USB_REG4	0x604
-
-typedef struct _CMD_MESSAGE
-{
-    u8        byData[256];
-} CMD_MESSAGE, *PCMD_MESSAGE;
-
-typedef struct _CMD_WRITE_MASK
-{
-    u8        byData;
-    u8        byMask;
-} CMD_WRITE_MASK, *PCMD_WRITE_MASK;
-
-struct vnt_cmd_card_init
-{
-	u8 init_class;
-	u8 exist_sw_net_addr;
-	u8 sw_net_addr[6];
-	u8 short_retry_limit;
-	u8 long_retry_limit;
-};
-
-struct vnt_rsp_card_init
-{
-	u8 status;
-	u8 net_addr[6];
-	u8 rf_type;
-	u8 min_channel;
-	u8 max_channel;
-};
-
-typedef struct _CMD_SET_KEY
-{
-    u16        wKCTL;
-    u8        abyMacAddr[6];
-    u8        abyKey[16];
-} CMD_SET_KEY, *PCMD_SET_KEY;
-
-typedef struct _CMD_CLRKEY_ENTRY
-{
-    u8        abyKeyEntry[11];
-} CMD_CLRKEY_ENTRY, *PCMD_CLRKEY_ENTRY;
-
-typedef struct _CMD_WRITE_MISCFF
-{
-    u32       adwMiscFFData[22][4];  //a key entry has only 22 dwords
-} CMD_WRITE_MISCFF, *PCMD_WRITE_MISCFF;
-
-typedef struct _CMD_SET_TSFTBTT
-{
-    u8        abyTSF_TBTT[8];
-} CMD_SET_TSFTBTT, *PCMD_SET_TSFTBTT;
-
-typedef struct _CMD_SET_SSTIFS
-{
-    u8        bySIFS;
-    u8        byDIFS;
-    u8        byEIFS;
-    u8        bySlotTime;
-    u8        byCwMax_Min;
-    u8        byBBCR10;
-} CMD_SET_SSTIFS, *PCMD_SET_SSTIFS;
-
-typedef struct _CMD_CHANGE_BBTYPE
-{
-    u8        bySIFS;
-    u8        byDIFS;
-    u8        byEIFS;
-    u8        bySlotTime;
-    u8        byCwMax_Min;
-    u8        byBBCR10;
-    u8        byBB_BBType;    //CR88
-    u8        byMAC_BBType;
-    u32       dwRSPINF_b_1;
-    u32       dwRSPINF_b_2;
-    u32       dwRSPINF_b_55;
-    u32       dwRSPINF_b_11;
-    u16        wRSPINF_a[9];
-} CMD_CHANGE_BBTYPE, *PCMD_CHANGE_BBTYPE;
-
-#define EXCH_WORD(w) ((u16)((u16)(w)<<8) | (u16)((u16)(w)>>8))
-
-#endif /* _RNDIS_H_ */
diff --git a/drivers/staging/vt6656/rxtx.c b/drivers/staging/vt6656/rxtx.c
index 3840323858f..704f4d3639b 100644
--- a/drivers/staging/vt6656/rxtx.c
+++ b/drivers/staging/vt6656/rxtx.c
@@ -54,9 +54,7 @@
 #include "mac.h"
 #include "michael.h"
 #include "tkip.h"
-#include "tcrc.h"
 #include "wctl.h"
-#include "hostap.h"
 #include "rf.h"
 #include "datarate.h"
 #include "usbpipe.h"
@@ -101,11 +99,11 @@ static struct vnt_usb_send_context *s_vGetFreeContext(struct vnt_private *);
 static u16 s_vGenerateTxParameter(struct vnt_private *pDevice,
 	u8 byPktType, u16 wCurrentRate,	struct vnt_tx_buffer *tx_buffer,
 	struct vnt_mic_hdr **mic_hdr, u32 need_mic, u32 cbFrameSize,
-	int bNeedACK, u32 uDMAIdx, struct ethhdr *psEthHeader, bool need_rts);
+	int bNeedACK, struct ethhdr *psEthHeader, bool need_rts);
 
 static void s_vGenerateMACHeader(struct vnt_private *pDevice,
 	u8 *pbyBufferAddr, u16 wDuration, struct ethhdr *psEthHeader,
-	int bNeedEncrypt, u16 wFragType, u32 uDMAIdx, u32 uFragIdx);
+	int bNeedEncrypt, u16 wFragType, u32 uFragIdx);
 
 static void s_vFillTxKey(struct vnt_private *pDevice,
 	struct vnt_tx_fifo_head *fifo_head, u8 *pbyIVHead,
@@ -119,9 +117,9 @@ static unsigned int s_uGetTxRsvTime(struct vnt_private *pDevice, u8 byPktType,
 	u32 cbFrameLength, u16 wRate, int bNeedAck);
 
 static __le16 s_uGetRTSCTSRsvTime(struct vnt_private *priv,
-	u8 rsv_type, u8 pkt_type, u32 frame_lenght, u16 current_rate);
+	u8 rsv_type, u8 pkt_type, u32 frame_length, u16 current_rate);
 
-static u16 s_vFillCTSHead(struct vnt_private *pDevice, u32 uDMAIdx,
+static u16 s_vFillCTSHead(struct vnt_private *pDevice,
 	u8 byPktType, union vnt_tx_data_head *head, u32 cbFrameLength,
 	int bNeedAck, u16 wCurrentRate, u8 byFBOption);
 
@@ -149,9 +147,9 @@ static struct vnt_usb_send_context
 			return NULL;
 
 		context = priv->apTD[ii];
-		if (context->bBoolInUse == false) {
-			context->bBoolInUse = true;
-			memset(context->Data, 0,
+		if (context->in_use == false) {
+			context->in_use = true;
+			memset(context->data, 0,
 					MAX_TOTAL_SIZE_WITH_ALL_HEADERS);
 			return context;
 		}
@@ -181,41 +179,40 @@ static void s_vFillTxKey(struct vnt_private *pDevice,
 	struct vnt_mic_hdr *mic_hdr)
 {
 	u8 *pbyBuf = (u8 *)&fifo_head->adwTxKey[0];
-	u32 *pdwIV = (u32 *)pbyIVHead;
-	u32 *pdwExtIV = (u32 *)((u8 *)pbyIVHead + 4);
+	__le32 *pdwIV = (__le32 *)pbyIVHead;
+	__le32 *pdwExtIV = (__le32 *)((u8 *)pbyIVHead + 4);
 	struct ieee80211_hdr *pMACHeader = (struct ieee80211_hdr *)pbyHdrBuf;
-	u32 dwRevIVCounter;
+	__le32 rev_iv_counter;
 
 	/* Fill TXKEY */
 	if (pTransmitKey == NULL)
 		return;
 
-	dwRevIVCounter = cpu_to_le32(pDevice->dwIVCounter);
-	*pdwIV = pDevice->dwIVCounter;
+	rev_iv_counter = cpu_to_le32(pDevice->dwIVCounter);
+	*pdwIV = cpu_to_le32(pDevice->dwIVCounter);
 	pDevice->byKeyIndex = pTransmitKey->dwKeyIndex & 0xf;
 
 	switch (pTransmitKey->byCipherSuite) {
 	case KEY_CTL_WEP:
 		if (pTransmitKey->uKeyLength == WLAN_WEP232_KEYLEN) {
-			memcpy(pDevice->abyPRNG, (u8 *)&dwRevIVCounter, 3);
+			memcpy(pDevice->abyPRNG, (u8 *)&rev_iv_counter, 3);
 			memcpy(pDevice->abyPRNG + 3, pTransmitKey->abyKey,
 						pTransmitKey->uKeyLength);
 		} else {
-			memcpy(pbyBuf, (u8 *)&dwRevIVCounter, 3);
+			memcpy(pbyBuf, (u8 *)&rev_iv_counter, 3);
 			memcpy(pbyBuf + 3, pTransmitKey->abyKey,
 						pTransmitKey->uKeyLength);
 			if (pTransmitKey->uKeyLength == WLAN_WEP40_KEYLEN) {
-				memcpy(pbyBuf+8, (u8 *)&dwRevIVCounter, 3);
-			memcpy(pbyBuf+11, pTransmitKey->abyKey,
+				memcpy(pbyBuf+8, (u8 *)&rev_iv_counter, 3);
+				memcpy(pbyBuf+11, pTransmitKey->abyKey,
 						pTransmitKey->uKeyLength);
 			}
 
 			memcpy(pDevice->abyPRNG, pbyBuf, 16);
 		}
 		/* Append IV after Mac Header */
-		*pdwIV &= WEP_IV_MASK;
-		*pdwIV |= (u32)pDevice->byKeyIndex << 30;
-		*pdwIV = cpu_to_le32(*pdwIV);
+		*pdwIV &= cpu_to_le32(WEP_IV_MASK);
+		*pdwIV |= cpu_to_le32((u32)pDevice->byKeyIndex << 30);
 
 		pDevice->dwIVCounter++;
 		if (pDevice->dwIVCounter > WEP_IV_MASK)
@@ -256,7 +253,7 @@ static void s_vFillTxKey(struct vnt_private *pDevice,
 		*(pbyIVHead+3) = (u8)(((pDevice->byKeyIndex << 6) &
 							0xc0) | 0x20);
 
-		*pdwIV |= cpu_to_le16((u16)(pTransmitKey->wTSC15_0));
+		*pdwIV |= cpu_to_le32((u32)(pTransmitKey->wTSC15_0));
 
 		/* Append IV&ExtIV after Mac Header */
 		*pdwExtIV = cpu_to_le32(pTransmitKey->dwTSC47_16);
@@ -283,9 +280,10 @@ static void s_vFillTxKey(struct vnt_private *pDevice,
 
 		/* MICHDR2 */
 		memcpy(mic_hdr->addr3, pMACHeader->addr3, ETH_ALEN);
-		mic_hdr->frame_control = cpu_to_le16(pMACHeader->frame_control
-								& 0xc78f);
-		mic_hdr->seq_ctrl = cpu_to_le16(pMACHeader->seq_ctrl & 0xf);
+		mic_hdr->frame_control = cpu_to_le16(
+			le16_to_cpu(pMACHeader->frame_control) & 0xc78f);
+		mic_hdr->seq_ctrl = cpu_to_le16(
+				le16_to_cpu(pMACHeader->seq_ctrl) & 0xf);
 
 		if (ieee80211_has_a4(pMACHeader->frame_control))
 			memcpy(mic_hdr->addr4, pMACHeader->addr4, ETH_ALEN);
@@ -305,7 +303,7 @@ static void s_vSWencryption(struct vnt_private *pDevice,
     if (pTransmitKey->byCipherSuite == KEY_CTL_WEP) {
         //=======================================================================
         // Append ICV after payload
-        dwICV = CRCdwGetCrc32Ex(pbyPayloadHead, wPayloadSize, dwICV);//ICV(Payload)
+	dwICV = ether_crc_le(wPayloadSize, pbyPayloadHead);
         pdwICV = (u32 *)(pbyPayloadHead + wPayloadSize);
         // finally, we must invert dwCRC to get the correct answer
         *pdwICV = cpu_to_le32(~dwICV);
@@ -316,7 +314,7 @@ static void s_vSWencryption(struct vnt_private *pDevice,
     } else if (pTransmitKey->byCipherSuite == KEY_CTL_TKIP) {
         //=======================================================================
         //Append ICV after payload
-        dwICV = CRCdwGetCrc32Ex(pbyPayloadHead, wPayloadSize, dwICV);//ICV(Payload)
+	dwICV = ether_crc_le(wPayloadSize, pbyPayloadHead);
         pdwICV = (u32 *)(pbyPayloadHead + wPayloadSize);
         // finally, we must invert dwCRC to get the correct answer
         *pdwICV = cpu_to_le32(~dwICV);
@@ -368,14 +366,14 @@ static __le16 vnt_rxtx_rsvtime_le16(struct vnt_private *priv, u8 pkt_type,
 
 //byFreqType: 0=>5GHZ 1=>2.4GHZ
 static __le16 s_uGetRTSCTSRsvTime(struct vnt_private *priv,
-	u8 rsv_type, u8 pkt_type, u32 frame_lenght, u16 current_rate)
+	u8 rsv_type, u8 pkt_type, u32 frame_length, u16 current_rate)
 {
 	u32 rrv_time, rts_time, cts_time, ack_time, data_time;
 
 	rrv_time = rts_time = cts_time = ack_time = data_time = 0;
 
 	data_time = BBuGetFrameTime(priv->byPreambleType, pkt_type,
-						frame_lenght, current_rate);
+						frame_length, current_rate);
 
 	if (rsv_type == 0) {
 		rts_time = BBuGetFrameTime(priv->byPreambleType,
@@ -551,7 +549,8 @@ static int vnt_fill_ieee80211_rts(struct vnt_private *priv,
 		__le16 duration)
 {
 	rts->duration = duration;
-	rts->frame_control = TYPE_CTL_RTS;
+	rts->frame_control =
+		cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_RTS);
 
 	if (priv->op_mode == NL80211_IFTYPE_ADHOC ||
 				priv->op_mode == NL80211_IFTYPE_AP)
@@ -713,7 +712,7 @@ static u16 s_vFillRTSHead(struct vnt_private *pDevice, u8 byPktType,
 	return 0;
 }
 
-static u16 s_vFillCTSHead(struct vnt_private *pDevice, u32 uDMAIdx,
+static u16 s_vFillCTSHead(struct vnt_private *pDevice,
 	u8 byPktType, union vnt_tx_data_head *head, u32 cbFrameLength,
 	int bNeedAck, u16 wCurrentRate, u8 byFBOption)
 {
@@ -741,7 +740,9 @@ static u16 s_vFillCTSHead(struct vnt_private *pDevice, u32 uDMAIdx,
 			pDevice->tx_rate_fb1, bNeedAck, byFBOption);
 		/* Get CTS Frame body */
 		pBuf->data.duration = pBuf->duration_ba;
-		pBuf->data.frame_control = TYPE_CTL_CTS;
+		pBuf->data.frame_control =
+			cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_CTS);
+
 		memcpy(pBuf->data.ra, pDevice->abyCurrentNetAddr, ETH_ALEN);
 
 		return vnt_rxtx_datahead_g_fb(pDevice, byPktType, wCurrentRate,
@@ -757,7 +758,9 @@ static u16 s_vFillCTSHead(struct vnt_private *pDevice, u32 uDMAIdx,
 			wCurrentRate, bNeedAck, byFBOption);
 		/*Get CTS Frame body*/
 		pBuf->data.duration = pBuf->duration_ba;
-		pBuf->data.frame_control = TYPE_CTL_CTS;
+		pBuf->data.frame_control =
+			cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_CTS);
+
 		memcpy(pBuf->data.ra, pDevice->abyCurrentNetAddr, ETH_ALEN);
 
 		return vnt_rxtx_datahead_g(pDevice, byPktType, wCurrentRate,
@@ -782,7 +785,6 @@ static u16 s_vFillCTSHead(struct vnt_private *pDevice, u32 uDMAIdx,
  *      pCTS            - CTS Buffer
  *      cbFrameSize     - Transmit Data Length (Hdr+Payload+FCS)
  *      bNeedACK        - If need ACK
- *      uDMAIdx         - DMA Index
  *  Out:
  *      none
  *
@@ -793,14 +795,14 @@ static u16 s_vFillCTSHead(struct vnt_private *pDevice, u32 uDMAIdx,
 static u16 s_vGenerateTxParameter(struct vnt_private *pDevice,
 	u8 byPktType, u16 wCurrentRate,	struct vnt_tx_buffer *tx_buffer,
 	struct vnt_mic_hdr **mic_hdr, u32 need_mic, u32 cbFrameSize,
-	int bNeedACK, u32 uDMAIdx, struct ethhdr *psEthHeader, bool need_rts)
+	int bNeedACK, struct ethhdr *psEthHeader, bool need_rts)
 {
 	struct vnt_tx_fifo_head *pFifoHead = &tx_buffer->fifo_head;
 	union vnt_tx_data_head *head = NULL;
 	u16 wFifoCtl;
 	u8 byFBOption = AUTO_FB_NONE;
 
-	pFifoHead->wReserved = wCurrentRate;
+	pFifoHead->current_rate = cpu_to_le16(wCurrentRate);
 	wFifoCtl = pFifoHead->wFIFOCtl;
 
 	if (wFifoCtl & FIFOCTL_AUTO_FB_0)
@@ -808,9 +810,6 @@ static u16 s_vGenerateTxParameter(struct vnt_private *pDevice,
 	else if (wFifoCtl & FIFOCTL_AUTO_FB_1)
 		byFBOption = AUTO_FB_1;
 
-	if (!pFifoHead)
-		return 0;
-
 	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
 		if (need_rts) {
 			struct vnt_rrv_time_rts *pBuf =
@@ -864,7 +863,7 @@ static u16 s_vGenerateTxParameter(struct vnt_private *pDevice,
 			}
 
 			/* Fill CTS */
-			return s_vFillCTSHead(pDevice, uDMAIdx, byPktType,
+			return s_vFillCTSHead(pDevice, byPktType,
 				head, cbFrameSize, bNeedACK, wCurrentRate,
 					byFBOption);
 		}
@@ -949,12 +948,11 @@ static u16 s_vGenerateTxParameter(struct vnt_private *pDevice,
 
 static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
 	struct vnt_tx_buffer *tx_buffer, int bNeedEncryption,
-	u32 uSkbPacketLen, u32 uDMAIdx,	struct ethhdr *psEthHeader,
+	u32 uSkbPacketLen, struct ethhdr *psEthHeader,
 	u8 *pPacket, PSKeyItem pTransmitKey, u32 uNodeIndex, u16 wCurrentRate,
 	u32 *pcbHeaderLen, u32 *pcbTotalLen)
 {
 	struct vnt_tx_fifo_head *pTxBufHead = &tx_buffer->fifo_head;
-	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
 	u32 cbFrameSize, cbFrameBodySize;
 	u32 cb802_1_H_len;
 	u32 cbIVlen = 0, cbICVlen = 0, cbMIClen = 0, cbMACHdLen = 0;
@@ -1008,7 +1006,7 @@ static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
 		pTxBufHead->wFIFOCtl |= FIFOCTL_NEEDACK;
 	}
 
-    pTxBufHead->wTimeStamp = DEFAULT_MSDU_LIFETIME_RES_64us;
+    pTxBufHead->time_stamp = cpu_to_le16(DEFAULT_MSDU_LIFETIME_RES_64us);
 
     //Set FRAGCTL_MACHDCNT
 	cbMACHdLen = WLAN_HDR_ADDR3_LEN;
@@ -1145,27 +1143,21 @@ static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
     //=========================
     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"No Fragmentation...\n");
     byFragType = FRAGCTL_NONFRAG;
-    //uDMAIdx = TYPE_AC0DMA;
     //pTxBufHead = (PSTxBufHead) &(pTxBufHead->adwTxKey[0]);
 
 	/* Fill FIFO, RrvTime, RTS and CTS */
 	uDuration = s_vGenerateTxParameter(pDevice, byPktType, wCurrentRate,
 			tx_buffer, &pMICHDR, cbMICHDR,
-			cbFrameSize, bNeedACK, uDMAIdx, psEthHeader, bRTS);
+			cbFrameSize, bNeedACK, psEthHeader, bRTS);
 
     // Generate TX MAC Header
     s_vGenerateMACHeader(pDevice, pbyMacHdr, (u16)uDuration, psEthHeader, bNeedEncryption,
-                           byFragType, uDMAIdx, 0);
+		byFragType, 0);
 
     if (bNeedEncryption == true) {
         //Fill TXKEY
 	s_vFillTxKey(pDevice, pTxBufHead, pbyIVHead, pTransmitKey,
 		pbyMacHdr, (u16)cbFrameBodySize, pMICHDR);
-
-        if (pDevice->bEnableHostWEP) {
-            pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16 = pTransmitKey->dwTSC47_16;
-            pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0 = pTransmitKey->wTSC15_0;
-        }
     }
 
 	/* 802.1H */
@@ -1287,7 +1279,7 @@ static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
 
 static void s_vGenerateMACHeader(struct vnt_private *pDevice,
 	u8 *pbyBufferAddr, u16 wDuration, struct ethhdr *psEthHeader,
-	int bNeedEncrypt, u16 wFragType, u32 uDMAIdx, u32 uFragIdx)
+	int bNeedEncrypt, u16 wFragType, u32 uFragIdx)
 {
 	struct ieee80211_hdr *pMACHeader = (struct ieee80211_hdr *)pbyBufferAddr;
 
@@ -1383,38 +1375,39 @@ CMD_STATUS csMgmt_xmit(struct vnt_private *pDevice,
 	u16 wTxBufSize;
 	u32 cbMacHdLen;
 	u16 wCurrentRate = RATE_1M;
+	unsigned long flags;
 
-	pContext = s_vGetFreeContext(pDevice);
+	if (pDevice->byBBType == BB_TYPE_11A) {
+		wCurrentRate = RATE_6M;
+		byPktType = PK_TYPE_11A;
+	} else {
+		wCurrentRate = RATE_1M;
+		byPktType = PK_TYPE_11B;
+	}
 
-    if (NULL == pContext) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ManagementSend TX...NO CONTEXT!\n");
-        return CMD_STATUS_RESOURCES;
-    }
+	if (pMgmt->eScanState != WMAC_NO_SCANNING)
+		vnt_rf_setpower(pDevice, wCurrentRate, pDevice->byCurrentCh);
+	else
+		vnt_rf_setpower(pDevice, wCurrentRate, pMgmt->uCurrChannel);
+
+	pDevice->wCurrentRate = wCurrentRate;
+
+	spin_lock_irqsave(&pDevice->lock, flags);
+
+	pContext = s_vGetFreeContext(pDevice);
+	if (!pContext) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
+			"ManagementSend TX...NO CONTEXT!\n");
+		spin_unlock_irqrestore(&pDevice->lock, flags);
+		return CMD_STATUS_RESOURCES;
+	}
 
-	pTX_Buffer = (struct vnt_tx_buffer *)&pContext->Data[0];
+	pTX_Buffer = (struct vnt_tx_buffer *)&pContext->data[0];
     cbFrameBodySize = pPacket->cbPayloadLen;
 	pTxBufHead = &pTX_Buffer->fifo_head;
 	pbyTxBufferAddr = (u8 *)&pTxBufHead->adwTxKey[0];
 	wTxBufSize = sizeof(struct vnt_tx_fifo_head);
 
-    if (pDevice->byBBType == BB_TYPE_11A) {
-        wCurrentRate = RATE_6M;
-        byPktType = PK_TYPE_11A;
-    } else {
-        wCurrentRate = RATE_1M;
-        byPktType = PK_TYPE_11B;
-    }
-
-    // SetPower will cause error power TX state for OFDM Date packet in TX buffer.
-    // 2004.11.11 Kyle -- Using OFDM power to tx MngPkt will decrease the connection capability.
-    //                    And cmd timer will wait data pkt TX finish before scanning so it's OK
-    //                    to set power here.
-    if (pMgmt->eScanState != WMAC_NO_SCANNING) {
-        RFbSetPower(pDevice, wCurrentRate, pDevice->byCurrentCh);
-    } else {
-        RFbSetPower(pDevice, wCurrentRate, pMgmt->uCurrChannel);
-    }
-    pDevice->wCurrentRate = wCurrentRate;
 
     //Set packet type
     if (byPktType == PK_TYPE_11A) {//0000 0000 0000 0000
@@ -1431,7 +1424,7 @@ CMD_STATUS csMgmt_xmit(struct vnt_private *pDevice,
     }
 
     pTxBufHead->wFIFOCtl |= FIFOCTL_TMOEN;
-    pTxBufHead->wTimeStamp = cpu_to_le16(DEFAULT_MGN_LIFETIME_RES_64us);
+    pTxBufHead->time_stamp = cpu_to_le16(DEFAULT_MGN_LIFETIME_RES_64us);
 
     if (is_multicast_ether_addr(pPacket->p80211Header->sA3.abyAddr1)) {
         bNeedACK = false;
@@ -1527,7 +1520,7 @@ CMD_STATUS csMgmt_xmit(struct vnt_private *pDevice,
 	/* Fill FIFO,RrvTime,RTS,and CTS */
 	uDuration = s_vGenerateTxParameter(pDevice, byPktType, wCurrentRate,
 		pTX_Buffer, &pMICHDR, 0,
-		cbFrameSize, bNeedACK, TYPE_TXDMA0, &sEthHeader, false);
+		cbFrameSize, bNeedACK, &sEthHeader, false);
 
     pMACHeader = (struct ieee80211_hdr *) (pbyTxBufferAddr + cbHeaderSize);
 
@@ -1605,13 +1598,13 @@ CMD_STATUS csMgmt_xmit(struct vnt_private *pDevice,
 	}
     }
 
-    pTX_Buffer->wTxByteCount = cpu_to_le16((u16)(cbReqCount));
+    pTX_Buffer->tx_byte_count = cpu_to_le16((u16)(cbReqCount));
     pTX_Buffer->byPKTNO = (u8) (((wCurrentRate<<4) &0x00F0) | ((pDevice->wSeqCounter - 1) & 0x000F));
     pTX_Buffer->byType = 0x00;
 
-    pContext->pPacket = NULL;
-    pContext->type = CONTEXT_MGMT_PACKET;
-    pContext->uBufLen = (u16)cbReqCount + 4;  //USB header
+	pContext->skb = NULL;
+	pContext->type = CONTEXT_MGMT_PACKET;
+	pContext->buf_len = (u16)cbReqCount + 4; /* USB header */
 
     if (WLAN_GET_FC_TODS(pMACHeader->frame_control) == 0) {
 	s_vSaveTxPktInfo(pDevice, (u8)(pTX_Buffer->byPKTNO & 0x0F),
@@ -1625,6 +1618,9 @@ CMD_STATUS csMgmt_xmit(struct vnt_private *pDevice,
     }
 
     PIPEnsSendBulkOut(pDevice,pContext);
+
+	spin_unlock_irqrestore(&pDevice->lock, flags);
+
     return CMD_STATUS_PENDING;
 }
 
@@ -1649,7 +1645,7 @@ CMD_STATUS csBeacon_xmit(struct vnt_private *pDevice,
         return status ;
     }
 
-	pTX_Buffer = (struct vnt_beacon_buffer *)&pContext->Data[0];
+	pTX_Buffer = (struct vnt_beacon_buffer *)&pContext->data[0];
 	short_head = &pTX_Buffer->short_head;
 
     cbFrameBodySize = pPacket->cbPayloadLen;
@@ -1697,377 +1693,19 @@ CMD_STATUS csBeacon_xmit(struct vnt_private *pDevice,
 
     cbReqCount = cbHeaderSize + WLAN_HDR_ADDR3_LEN + cbFrameBodySize;
 
-    pTX_Buffer->wTxByteCount = (u16)cbReqCount;
+    pTX_Buffer->tx_byte_count = cpu_to_le16((u16)cbReqCount);
     pTX_Buffer->byPKTNO = (u8) (((wCurrentRate<<4) &0x00F0) | ((pDevice->wSeqCounter - 1) & 0x000F));
     pTX_Buffer->byType = 0x01;
 
-    pContext->pPacket = NULL;
-    pContext->type = CONTEXT_MGMT_PACKET;
-    pContext->uBufLen = (u16)cbReqCount + 4;  //USB header
+	pContext->skb = NULL;
+	pContext->type = CONTEXT_MGMT_PACKET;
+	pContext->buf_len = (u16)cbReqCount + 4; /* USB header */
 
     PIPEnsSendBulkOut(pDevice,pContext);
     return CMD_STATUS_PENDING;
 
 }
 
-void vDMA0_tx_80211(struct vnt_private *pDevice, struct sk_buff *skb)
-{
-	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	struct vnt_tx_buffer *pTX_Buffer;
-	struct vnt_tx_fifo_head *pTxBufHead;
-	u8 byPktType;
-	u8 *pbyTxBufferAddr;
-	u32 uDuration, cbReqCount;
-	struct ieee80211_hdr *pMACHeader;
-	u32 cbHeaderSize, cbFrameBodySize;
-	int bNeedACK, bIsPSPOLL = false;
-	u32 cbFrameSize;
-	u32 cbIVlen = 0, cbICVlen = 0, cbMIClen = 0, cbFCSlen = 4;
-	u32 uPadding = 0;
-	u32 cbMICHDR = 0, uLength = 0;
-	u32 dwMICKey0, dwMICKey1;
-	u32 dwMIC_Priority;
-	u32 *pdwMIC_L, *pdwMIC_R;
-	u16 wTxBufSize;
-	u32 cbMacHdLen;
-	struct ethhdr sEthHeader;
-	struct vnt_mic_hdr *pMICHDR;
-	u32 wCurrentRate = RATE_1M;
-	PUWLAN_80211HDR  p80211Header;
-	u32 uNodeIndex = 0;
-	int bNodeExist = false;
-	SKeyItem STempKey;
-	PSKeyItem pTransmitKey = NULL;
-	u8 *pbyIVHead, *pbyPayloadHead, *pbyMacHdr;
-	u32 cbExtSuppRate = 0;
-	struct vnt_usb_send_context *pContext;
-
-	pMICHDR = NULL;
-
-    if(skb->len <= WLAN_HDR_ADDR3_LEN) {
-       cbFrameBodySize = 0;
-    }
-    else {
-       cbFrameBodySize = skb->len - WLAN_HDR_ADDR3_LEN;
-    }
-    p80211Header = (PUWLAN_80211HDR)skb->data;
-
-	pContext = s_vGetFreeContext(pDevice);
-
-    if (NULL == pContext) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"DMA0 TX...NO CONTEXT!\n");
-        dev_kfree_skb_irq(skb);
-        return ;
-    }
-
-	pTX_Buffer = (struct vnt_tx_buffer *)&pContext->Data[0];
-	pTxBufHead = &pTX_Buffer->fifo_head;
-	pbyTxBufferAddr = (u8 *)&pTxBufHead->adwTxKey[0];
-	wTxBufSize = sizeof(struct vnt_tx_fifo_head);
-
-    if (pDevice->byBBType == BB_TYPE_11A) {
-        wCurrentRate = RATE_6M;
-        byPktType = PK_TYPE_11A;
-    } else {
-        wCurrentRate = RATE_1M;
-        byPktType = PK_TYPE_11B;
-    }
-
-    // SetPower will cause error power TX state for OFDM Date packet in TX buffer.
-    // 2004.11.11 Kyle -- Using OFDM power to tx MngPkt will decrease the connection capability.
-    //                    And cmd timer will wait data pkt TX finish before scanning so it's OK
-    //                    to set power here.
-    if (pMgmt->eScanState != WMAC_NO_SCANNING) {
-        RFbSetPower(pDevice, wCurrentRate, pDevice->byCurrentCh);
-    } else {
-        RFbSetPower(pDevice, wCurrentRate, pMgmt->uCurrChannel);
-    }
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"vDMA0_tx_80211: p80211Header->sA3.wFrameCtl = %x \n", p80211Header->sA3.wFrameCtl);
-
-    //Set packet type
-    if (byPktType == PK_TYPE_11A) {//0000 0000 0000 0000
-        pTxBufHead->wFIFOCtl = 0;
-    }
-    else if (byPktType == PK_TYPE_11B) {//0000 0001 0000 0000
-        pTxBufHead->wFIFOCtl |= FIFOCTL_11B;
-    }
-    else if (byPktType == PK_TYPE_11GB) {//0000 0010 0000 0000
-        pTxBufHead->wFIFOCtl |= FIFOCTL_11GB;
-    }
-    else if (byPktType == PK_TYPE_11GA) {//0000 0011 0000 0000
-        pTxBufHead->wFIFOCtl |= FIFOCTL_11GA;
-    }
-
-    pTxBufHead->wFIFOCtl |= FIFOCTL_TMOEN;
-    pTxBufHead->wTimeStamp = cpu_to_le16(DEFAULT_MGN_LIFETIME_RES_64us);
-
-    if (is_multicast_ether_addr(p80211Header->sA3.abyAddr1)) {
-        bNeedACK = false;
-        if (pDevice->bEnableHostWEP) {
-            uNodeIndex = 0;
-            bNodeExist = true;
-        }
-    }
-    else {
-        if (pDevice->bEnableHostWEP) {
-            if (BSSbIsSTAInNodeDB(pDevice, (u8 *)(p80211Header->sA3.abyAddr1), &uNodeIndex))
-                bNodeExist = true;
-        }
-        bNeedACK = true;
-        pTxBufHead->wFIFOCtl |= FIFOCTL_NEEDACK;
-    };
-
-    if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) ||
-        (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) ) {
-
-        pTxBufHead->wFIFOCtl |= FIFOCTL_LRETRY;
-        //Set Preamble type always long
-        //pDevice->byPreambleType = PREAMBLE_LONG;
-
-        // probe-response don't retry
-        //if ((p80211Header->sA4.wFrameCtl & TYPE_SUBTYPE_MASK) == TYPE_MGMT_PROBE_RSP) {
-        //     bNeedACK = false;
-        //     pTxBufHead->wFIFOCtl  &= (~FIFOCTL_NEEDACK);
-        //}
-    }
-
-    pTxBufHead->wFIFOCtl |= (FIFOCTL_GENINT | FIFOCTL_ISDMA0);
-
-    if ((p80211Header->sA4.wFrameCtl & TYPE_SUBTYPE_MASK) == TYPE_CTL_PSPOLL) {
-        bIsPSPOLL = true;
-        cbMacHdLen = WLAN_HDR_ADDR2_LEN;
-    } else {
-        cbMacHdLen = WLAN_HDR_ADDR3_LEN;
-    }
-
-    // hostapd daemon ext support rate patch
-    if (WLAN_GET_FC_FSTYPE(p80211Header->sA4.wFrameCtl) == WLAN_FSTYPE_ASSOCRESP) {
-
-        if (((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len != 0) {
-            cbExtSuppRate += ((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len + WLAN_IEHDR_LEN;
-         }
-
-        if (((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates)->len != 0) {
-            cbExtSuppRate += ((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates)->len + WLAN_IEHDR_LEN;
-         }
-
-         if (cbExtSuppRate >0) {
-            cbFrameBodySize = WLAN_ASSOCRESP_OFF_SUPP_RATES;
-         }
-    }
-
-    //Set FRAGCTL_MACHDCNT
-    pTxBufHead->wFragCtl |= cpu_to_le16((u16)cbMacHdLen << 10);
-
-    // Notes:
-    // Although spec says MMPDU can be fragmented; In most case,
-    // no one will send a MMPDU under fragmentation. With RTS may occur.
-
-    if (WLAN_GET_FC_ISWEP(p80211Header->sA4.wFrameCtl) != 0) {
-        if (pDevice->eEncryptionStatus == Ndis802_11Encryption1Enabled) {
-            cbIVlen = 4;
-            cbICVlen = 4;
-    	    pTxBufHead->wFragCtl |= FRAGCTL_LEGACY;
-        }
-        else if (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled) {
-            cbIVlen = 8;//IV+ExtIV
-            cbMIClen = 8;
-            cbICVlen = 4;
-    	    pTxBufHead->wFragCtl |= FRAGCTL_TKIP;
-    	    //We need to get seed here for filling TxKey entry.
-            //TKIPvMixKey(pTransmitKey->abyKey, pDevice->abyCurrentNetAddr,
-            //            pTransmitKey->wTSC15_0, pTransmitKey->dwTSC47_16, pDevice->abyPRNG);
-        }
-        else if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
-            cbIVlen = 8;//RSN Header
-            cbICVlen = 8;//MIC
-	    cbMICHDR = sizeof(struct vnt_mic_hdr);
-            pTxBufHead->wFragCtl |= FRAGCTL_AES;
-        }
-        //MAC Header should be padding 0 to DW alignment.
-        uPadding = 4 - (cbMacHdLen%4);
-        uPadding %= 4;
-    }
-
-    cbFrameSize = cbMacHdLen + cbFrameBodySize + cbIVlen + cbMIClen + cbICVlen + cbFCSlen + cbExtSuppRate;
-
-    //Set FIFOCTL_GrpAckPolicy
-    if (pDevice->bGrpAckPolicy == true) {//0000 0100 0000 0000
-        pTxBufHead->wFIFOCtl |=	FIFOCTL_GRPACK;
-    }
-    //the rest of pTxBufHead->wFragCtl:FragTyp will be set later in s_vFillFragParameter()
-
-    if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {//802.11g packet
-	cbHeaderSize = wTxBufSize + sizeof(struct vnt_rrv_time_cts) + cbMICHDR +
-		sizeof(struct vnt_cts);
-
-    }
-    else {//802.11a/b packet
-	cbHeaderSize = wTxBufSize + sizeof(struct vnt_rrv_time_ab) + cbMICHDR +
-					sizeof(struct vnt_tx_datahead_ab);
-    }
-    memcpy(&(sEthHeader.h_dest[0]),
-	   &(p80211Header->sA3.abyAddr1[0]),
-	   ETH_ALEN);
-    memcpy(&(sEthHeader.h_source[0]),
-	   &(p80211Header->sA3.abyAddr2[0]),
-	   ETH_ALEN);
-    //=========================
-    //    No Fragmentation
-    //=========================
-    pTxBufHead->wFragCtl |= (u16)FRAGCTL_NONFRAG;
-
-	/* Fill FIFO,RrvTime,RTS,and CTS */
-	uDuration = s_vGenerateTxParameter(pDevice, byPktType, wCurrentRate,
-		pTX_Buffer, &pMICHDR, cbMICHDR,
-		cbFrameSize, bNeedACK, TYPE_TXDMA0, &sEthHeader, false);
-
-	pMACHeader = (struct ieee80211_hdr *) (pbyTxBufferAddr + cbHeaderSize);
-
-    cbReqCount = cbHeaderSize + cbMacHdLen + uPadding + cbIVlen + (cbFrameBodySize + cbMIClen) + cbExtSuppRate;
-
-    pbyMacHdr = (u8 *)(pbyTxBufferAddr + cbHeaderSize);
-    pbyPayloadHead = (u8 *)(pbyMacHdr + cbMacHdLen + uPadding + cbIVlen);
-    pbyIVHead = (u8 *)(pbyMacHdr + cbMacHdLen + uPadding);
-
-    // Copy the Packet into a tx Buffer
-    memcpy(pbyMacHdr, skb->data, cbMacHdLen);
-
-    // version set to 0, patch for hostapd deamon
-    pMACHeader->frame_control &= cpu_to_le16(0xfffc);
-    memcpy(pbyPayloadHead, (skb->data + cbMacHdLen), cbFrameBodySize);
-
-    // replace support rate, patch for hostapd daemon( only support 11M)
-    if (WLAN_GET_FC_FSTYPE(p80211Header->sA4.wFrameCtl) == WLAN_FSTYPE_ASSOCRESP) {
-        if (cbExtSuppRate != 0) {
-            if (((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len != 0)
-                memcpy((pbyPayloadHead + cbFrameBodySize),
-                        pMgmt->abyCurrSuppRates,
-                        ((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len + WLAN_IEHDR_LEN
-                       );
-             if (((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates)->len != 0)
-                memcpy((pbyPayloadHead + cbFrameBodySize) + ((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates)->len + WLAN_IEHDR_LEN,
-                        pMgmt->abyCurrExtSuppRates,
-                        ((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates)->len + WLAN_IEHDR_LEN
-                       );
-         }
-    }
-
-    // Set wep
-    if (WLAN_GET_FC_ISWEP(p80211Header->sA4.wFrameCtl) != 0) {
-
-        if (pDevice->bEnableHostWEP) {
-            pTransmitKey = &STempKey;
-            pTransmitKey->byCipherSuite = pMgmt->sNodeDBTable[uNodeIndex].byCipherSuite;
-            pTransmitKey->dwKeyIndex = pMgmt->sNodeDBTable[uNodeIndex].dwKeyIndex;
-            pTransmitKey->uKeyLength = pMgmt->sNodeDBTable[uNodeIndex].uWepKeyLength;
-            pTransmitKey->dwTSC47_16 = pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16;
-            pTransmitKey->wTSC15_0 = pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0;
-            memcpy(pTransmitKey->abyKey,
-                &pMgmt->sNodeDBTable[uNodeIndex].abyWepKey[0],
-                pTransmitKey->uKeyLength
-                );
-        }
-
-        if ((pTransmitKey != NULL) && (pTransmitKey->byCipherSuite == KEY_CTL_TKIP)) {
-
-            dwMICKey0 = *(u32 *)(&pTransmitKey->abyKey[16]);
-            dwMICKey1 = *(u32 *)(&pTransmitKey->abyKey[20]);
-
-            // DO Software Michael
-            MIC_vInit(dwMICKey0, dwMICKey1);
-            MIC_vAppend((u8 *)&(sEthHeader.h_dest[0]), 12);
-            dwMIC_Priority = 0;
-            MIC_vAppend((u8 *)&dwMIC_Priority, 4);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"DMA0_tx_8021:MIC KEY:"\
-			" %X, %X\n", dwMICKey0, dwMICKey1);
-
-            uLength = cbHeaderSize + cbMacHdLen + uPadding + cbIVlen;
-
-            MIC_vAppend((pbyTxBufferAddr + uLength), cbFrameBodySize);
-
-            pdwMIC_L = (u32 *)(pbyTxBufferAddr + uLength + cbFrameBodySize);
-            pdwMIC_R = (u32 *)(pbyTxBufferAddr + uLength + cbFrameBodySize + 4);
-
-            MIC_vGetMIC(pdwMIC_L, pdwMIC_R);
-            MIC_vUnInit();
-
-            if (pDevice->bTxMICFail == true) {
-                *pdwMIC_L = 0;
-                *pdwMIC_R = 0;
-                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);
-
-        }
-
-	s_vFillTxKey(pDevice, pTxBufHead, pbyIVHead, pTransmitKey,
-		pbyMacHdr, (u16)cbFrameBodySize, pMICHDR);
-
-        if (pDevice->bEnableHostWEP) {
-            pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16 = pTransmitKey->dwTSC47_16;
-            pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0 = pTransmitKey->wTSC15_0;
-        }
-
-        if ((pDevice->byLocalID <= REV_ID_VT3253_A1)) {
-            s_vSWencryption(pDevice, pTransmitKey, pbyPayloadHead, (u16)(cbFrameBodySize + cbMIClen));
-        }
-    }
-
-    pMACHeader->seq_ctrl = cpu_to_le16(pDevice->wSeqCounter << 4);
-    pDevice->wSeqCounter++ ;
-    if (pDevice->wSeqCounter > 0x0fff)
-        pDevice->wSeqCounter = 0;
-
-    if (bIsPSPOLL) {
-        // The MAC will automatically replace the Duration-field of MAC header by Duration-field
-        // of  FIFO control header.
-        // This will cause AID-field of PS-POLL packet be incorrect (Because PS-POLL's AID field is
-        // 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) {
-		struct vnt_tx_datahead_g *data_head = &pTX_Buffer->tx_head.
-						tx_cts.tx.head.cts_g.data_head;
-		data_head->duration_a =
-			cpu_to_le16(p80211Header->sA2.wDurationID);
-		data_head->duration_b =
-			cpu_to_le16(p80211Header->sA2.wDurationID);
-	} else {
-		struct vnt_tx_datahead_ab *data_head = &pTX_Buffer->tx_head.
-					tx_ab.tx.head.data_head_ab;
-		data_head->duration =
-			cpu_to_le16(p80211Header->sA2.wDurationID);
-	}
-    }
-
-    pTX_Buffer->wTxByteCount = cpu_to_le16((u16)(cbReqCount));
-    pTX_Buffer->byPKTNO = (u8) (((wCurrentRate<<4) &0x00F0) | ((pDevice->wSeqCounter - 1) & 0x000F));
-    pTX_Buffer->byType = 0x00;
-
-    pContext->pPacket = skb;
-    pContext->type = CONTEXT_MGMT_PACKET;
-    pContext->uBufLen = (u16)cbReqCount + 4;  //USB header
-
-    if (WLAN_GET_FC_TODS(pMACHeader->frame_control) == 0) {
-	s_vSaveTxPktInfo(pDevice, (u8)(pTX_Buffer->byPKTNO & 0x0F),
-			&pMACHeader->addr1[0], (u16)cbFrameSize,
-			pTxBufHead->wFIFOCtl);
-    }
-    else {
-	s_vSaveTxPktInfo(pDevice, (u8)(pTX_Buffer->byPKTNO & 0x0F),
-			&pMACHeader->addr3[0], (u16)cbFrameSize,
-			pTxBufHead->wFIFOCtl);
-    }
-    PIPEnsSendBulkOut(pDevice,pContext);
-    return ;
-
-}
-
 //TYPE_AC0DMA data tx
 /*
  * Description:
@@ -2083,8 +1721,7 @@ void vDMA0_tx_80211(struct vnt_private *pDevice, struct sk_buff *skb)
  * Return Value: NULL
  */
 
-int nsDMA_tx_packet(struct vnt_private *pDevice,
-	u32 uDMAIdx, struct sk_buff *skb)
+int nsDMA_tx_packet(struct vnt_private *pDevice, struct sk_buff *skb)
 {
 	struct net_device_stats *pStats = &pDevice->stats;
 	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
@@ -2096,7 +1733,6 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
 	u8 byPktType;
 	int bNeedEncryption = false;
 	PSKeyItem pTransmitKey = NULL;
-	SKeyItem STempKey;
 	int ii;
 	int bTKIP_UseGTK = false;
 	int bNeedDeAuth = false;
@@ -2173,15 +1809,7 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
         }
     }
 
-	pContext = s_vGetFreeContext(pDevice);
-
-    if (pContext == NULL) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG" pContext == NULL\n");
-        dev_kfree_skb_irq(skb);
-        return STATUS_RESOURCES;
-    }
-
-    memcpy(pDevice->sTxEthHeader.h_dest, (u8 *)(skb->data), ETH_HLEN);
+	memcpy(&pDevice->sTxEthHeader, skb->data, ETH_HLEN);
 
 //mike add:station mode check eapol-key challenge--->
 {
@@ -2268,22 +1896,6 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
         } while(false);
     }
 
-    if (pDevice->bEnableHostWEP) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG"acdma0: STA index %d\n", uNodeIndex);
-        if (pDevice->bEncryptionEnable == true) {
-            pTransmitKey = &STempKey;
-            pTransmitKey->byCipherSuite = pMgmt->sNodeDBTable[uNodeIndex].byCipherSuite;
-            pTransmitKey->dwKeyIndex = pMgmt->sNodeDBTable[uNodeIndex].dwKeyIndex;
-            pTransmitKey->uKeyLength = pMgmt->sNodeDBTable[uNodeIndex].uWepKeyLength;
-            pTransmitKey->dwTSC47_16 = pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16;
-            pTransmitKey->wTSC15_0 = pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0;
-            memcpy(pTransmitKey->abyKey,
-                &pMgmt->sNodeDBTable[uNodeIndex].abyWepKey[0],
-                pTransmitKey->uKeyLength
-                );
-         }
-    }
-
     byPktType = (u8)pDevice->byPacketType;
 
     if (pDevice->bFixRate) {
@@ -2346,12 +1958,10 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
 	if (pDevice->sTxEthHeader.h_proto == cpu_to_be16(ETH_P_PAE)) {
 		if (pDevice->byBBType != BB_TYPE_11A) {
 			pDevice->wCurrentRate = RATE_1M;
-			pDevice->byACKRate = RATE_1M;
 			pDevice->byTopCCKBasicRate = RATE_1M;
 			pDevice->byTopOFDMBasicRate = RATE_6M;
 		} else {
 			pDevice->wCurrentRate = RATE_6M;
-			pDevice->byACKRate = RATE_6M;
 			pDevice->byTopCCKBasicRate = RATE_1M;
 			pDevice->byTopOFDMBasicRate = RATE_6M;
 		}
@@ -2389,21 +1999,11 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
                     }
                 }
             }
-
-            if (pDevice->bEnableHostWEP) {
-                if ((uNodeIndex != 0) &&
-                    (pMgmt->sNodeDBTable[uNodeIndex].dwKeyIndex & PAIRWISE_KEY)) {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Find PTK [%X]\n",
-				pTransmitKey->dwKeyIndex);
-                    bNeedEncryption = true;
-                 }
-             }
         }
         else {
 
             if (pTransmitKey == NULL) {
                 DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"return no tx key\n");
-		pContext->bBoolInUse = false;
                 dev_kfree_skb_irq(skb);
                 pStats->tx_dropped++;
                 return STATUS_FAILURE;
@@ -2411,21 +2011,28 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
         }
     }
 
-	pTX_Buffer = (struct vnt_tx_buffer *)&pContext->Data[0];
+	pContext = s_vGetFreeContext(pDevice);
+	if (!pContext) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG" pContext == NULL\n");
+		dev_kfree_skb_irq(skb);
+		return STATUS_RESOURCES;
+	}
+
+	pTX_Buffer = (struct vnt_tx_buffer *)&pContext->data[0];
 
     fConvertedPacket = s_bPacketToWirelessUsb(pDevice, byPktType,
 			pTX_Buffer, bNeedEncryption,
-                        skb->len, uDMAIdx, &pDevice->sTxEthHeader,
+			skb->len, &pDevice->sTxEthHeader,
                         (u8 *)skb->data, pTransmitKey, uNodeIndex,
                         pDevice->wCurrentRate,
                         &uHeaderLen, &BytesToWrite
                        );
 
-    if (fConvertedPacket == false) {
-        pContext->bBoolInUse = false;
-        dev_kfree_skb_irq(skb);
-        return STATUS_FAILURE;
-    }
+	if (fConvertedPacket == false) {
+		pContext->in_use = false;
+		dev_kfree_skb_irq(skb);
+		return STATUS_FAILURE;
+	}
 
     if ( pDevice->bEnablePSMode == true ) {
         if ( !pDevice->bPSModeTxBurst ) {
@@ -2437,11 +2044,11 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
     }
 
     pTX_Buffer->byPKTNO = (u8) (((pDevice->wCurrentRate<<4) &0x00F0) | ((pDevice->wSeqCounter - 1) & 0x000F));
-    pTX_Buffer->wTxByteCount = (u16)BytesToWrite;
+    pTX_Buffer->tx_byte_count = cpu_to_le16((u16)BytesToWrite);
 
-    pContext->pPacket = skb;
-    pContext->type = CONTEXT_DATA_PACKET;
-    pContext->uBufLen = (u16)BytesToWrite + 4 ; //USB header
+	pContext->skb = skb;
+	pContext->type = CONTEXT_DATA_PACKET;
+	pContext->buf_len = (u16)BytesToWrite + 4 ; /* USB header */
 
     s_vSaveTxPktInfo(pDevice, (u8)(pTX_Buffer->byPKTNO & 0x0F),
 			&pDevice->sTxEthHeader.h_dest[0],
@@ -2456,154 +2063,12 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
 	bScheduleCommand((void *) pDevice, WLAN_CMD_DEAUTH, (u8 *) &wReason);
     }
 
-  if(status!=STATUS_PENDING) {
-     pContext->bBoolInUse = false;
-    dev_kfree_skb_irq(skb);
-    return STATUS_FAILURE;
-  }
-  else
-    return 0;
-
-}
-
-/*
- * Description:
- *      Relay packet send (AC1DMA) from rx dpc.
- *
- * Parameters:
- *  In:
- *      pDevice         - Pointer to the adapter
- *      pPacket         - Pointer to rx packet
- *      cbPacketSize    - rx ethernet frame size
- *  Out:
- *      TURE, false
- *
- * Return Value: Return true if packet is copy to dma1; otherwise false
- */
-
-int bRelayPacketSend(struct vnt_private *pDevice, u8 *pbySkbData, u32 uDataLen,
-	u32 uNodeIndex)
-{
-	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	struct vnt_tx_buffer *pTX_Buffer;
-	u32 BytesToWrite = 0, uHeaderLen = 0;
-	u8 byPktType = PK_TYPE_11B;
-	int bNeedEncryption = false;
-	SKeyItem STempKey;
-	PSKeyItem pTransmitKey = NULL;
-	u8 *pbyBSSID;
-	struct vnt_usb_send_context *pContext;
-	u8 byPktTyp;
-	int fConvertedPacket;
-	u32 status;
-	u16 wKeepRate = pDevice->wCurrentRate;
-
-	pContext = s_vGetFreeContext(pDevice);
-
-    if (NULL == pContext) {
-        return false;
-    }
-
-    memcpy(pDevice->sTxEthHeader.h_dest, (u8 *)pbySkbData, ETH_HLEN);
-
-    if (pDevice->bEncryptionEnable == true) {
-        bNeedEncryption = true;
-        // get group key
-        pbyBSSID = pDevice->abyBroadcastAddr;
-        if(KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, GROUP_KEY, &pTransmitKey) == false) {
-            pTransmitKey = NULL;
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG"KEY is NULL. [%d]\n", pMgmt->eCurrMode);
-        } else {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG"Get GTK.\n");
-        }
-    }
-
-    if (pDevice->bEnableHostWEP) {
-        if (uNodeIndex < MAX_NODE_NUM + 1) {
-            pTransmitKey = &STempKey;
-            pTransmitKey->byCipherSuite = pMgmt->sNodeDBTable[uNodeIndex].byCipherSuite;
-            pTransmitKey->dwKeyIndex = pMgmt->sNodeDBTable[uNodeIndex].dwKeyIndex;
-            pTransmitKey->uKeyLength = pMgmt->sNodeDBTable[uNodeIndex].uWepKeyLength;
-            pTransmitKey->dwTSC47_16 = pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16;
-            pTransmitKey->wTSC15_0 = pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0;
-            memcpy(pTransmitKey->abyKey,
-                    &pMgmt->sNodeDBTable[uNodeIndex].abyWepKey[0],
-                    pTransmitKey->uKeyLength
-                  );
-        }
-    }
-
-    if ( bNeedEncryption && (pTransmitKey == NULL) ) {
-        pContext->bBoolInUse = false;
-        return false;
-    }
-
-    byPktTyp = (u8)pDevice->byPacketType;
-
-    if (pDevice->bFixRate) {
-        if (pDevice->byBBType == BB_TYPE_11B) {
-            if (pDevice->uConnectionRate >= RATE_11M) {
-                pDevice->wCurrentRate = RATE_11M;
-            } else {
-                pDevice->wCurrentRate = (u16)pDevice->uConnectionRate;
-            }
-        } else {
-            if ((pDevice->byBBType == BB_TYPE_11A) &&
-                (pDevice->uConnectionRate <= RATE_6M)) {
-                pDevice->wCurrentRate = RATE_6M;
-            } else {
-                if (pDevice->uConnectionRate >= RATE_54M)
-                    pDevice->wCurrentRate = RATE_54M;
-                else
-                    pDevice->wCurrentRate = (u16)pDevice->uConnectionRate;
-            }
-        }
-    }
-    else {
-        pDevice->wCurrentRate = pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate;
-    }
-
-    if (wKeepRate != pDevice->wCurrentRate) {
-	bScheduleCommand((void *) pDevice, WLAN_CMD_SETPOWER, NULL);
-    }
-
-    if (pDevice->wCurrentRate <= RATE_11M)
-        byPktType = PK_TYPE_11B;
-
-    BytesToWrite = uDataLen + ETH_FCS_LEN;
-
-    // Convert the packet to an usb frame and copy into our buffer
-    // and send the irp.
-
-	pTX_Buffer = (struct vnt_tx_buffer *)&pContext->Data[0];
-
-    fConvertedPacket = s_bPacketToWirelessUsb(pDevice, byPktType,
-			pTX_Buffer, bNeedEncryption,
-                         uDataLen, TYPE_AC0DMA, &pDevice->sTxEthHeader,
-                         pbySkbData, pTransmitKey, uNodeIndex,
-                         pDevice->wCurrentRate,
-                         &uHeaderLen, &BytesToWrite
-                        );
-
-    if (fConvertedPacket == false) {
-        pContext->bBoolInUse = false;
-        return false;
-    }
-
-    pTX_Buffer->byPKTNO = (u8) (((pDevice->wCurrentRate<<4) &0x00F0) | ((pDevice->wSeqCounter - 1) & 0x000F));
-    pTX_Buffer->wTxByteCount = (u16)BytesToWrite;
-
-    pContext->pPacket = NULL;
-    pContext->type = CONTEXT_DATA_PACKET;
-    pContext->uBufLen = (u16)BytesToWrite + 4 ; //USB header
-
-    s_vSaveTxPktInfo(pDevice, (u8)(pTX_Buffer->byPKTNO & 0x0F),
-		&pDevice->sTxEthHeader.h_dest[0],
-		(u16)(BytesToWrite - uHeaderLen),
-		pTX_Buffer->fifo_head.wFIFOCtl);
+	if (status != STATUS_PENDING) {
+		pContext->in_use = false;
+		dev_kfree_skb_irq(skb);
+		return STATUS_FAILURE;
+	}
 
-    status = PIPEnsSendBulkOut(pDevice,pContext);
 
-    return true;
+	return 0;
 }
-
diff --git a/drivers/staging/vt6656/rxtx.h b/drivers/staging/vt6656/rxtx.h
index 6d6539d29d0..6db3337f1d1 100644
--- a/drivers/staging/vt6656/rxtx.h
+++ b/drivers/staging/vt6656/rxtx.h
@@ -217,15 +217,15 @@ union vnt_tx_head {
 struct vnt_tx_fifo_head {
 	u32 adwTxKey[4];
 	u16 wFIFOCtl;
-	u16 wTimeStamp;
+	__le16 time_stamp;
 	u16 wFragCtl;
-	u16 wReserved;
+	__le16 current_rate;
 } __packed;
 
 struct vnt_tx_buffer {
 	u8 byType;
 	u8 byPKTNO;
-	u16 wTxByteCount;
+	__le16 tx_byte_count;
 	struct vnt_tx_fifo_head fifo_head;
 	union vnt_tx_head tx_head;
 } __packed;
@@ -241,16 +241,14 @@ struct vnt_tx_short_buf_head {
 struct vnt_beacon_buffer {
 	u8 byType;
 	u8 byPKTNO;
-	u16 wTxByteCount;
+	__le16 tx_byte_count;
 	struct vnt_tx_short_buf_head short_head;
 	struct ieee80211_hdr hdr;
 } __packed;
 
 void vDMA0_tx_80211(struct vnt_private *, struct sk_buff *skb);
-int nsDMA_tx_packet(struct vnt_private *, u32 uDMAIdx, struct sk_buff *skb);
+int nsDMA_tx_packet(struct vnt_private *, struct sk_buff *skb);
 CMD_STATUS csMgmt_xmit(struct vnt_private *, struct vnt_tx_mgmt *);
 CMD_STATUS csBeacon_xmit(struct vnt_private *, struct vnt_tx_mgmt *);
-int bRelayPacketSend(struct vnt_private *, u8 *pbySkbData, u32 uDataLen,
-	u32 uNodeIndex);
 
 #endif /* __RXTX_H__ */
diff --git a/drivers/staging/vt6656/srom.h b/drivers/staging/vt6656/srom.h
deleted file mode 100644
index 488192d79ac..00000000000
--- a/drivers/staging/vt6656/srom.h
+++ /dev/null
@@ -1,112 +0,0 @@
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: srom.h
- *
- * Purpose: Implement functions to access eeprom
- *
- * Author: Jerry Chen
- *
- * Date: Jan 29, 2003
- *
- */
-
-#ifndef __SROM_H__
-#define __SROM_H__
-
-#define EEP_MAX_CONTEXT_SIZE    256
-
-#define CB_EEPROM_READBYTE_WAIT 900     //us
-
-#define W_MAX_I2CRETRY          0x0fff
-
-//
-// Contents in the EEPROM
-//
-#define EEP_OFS_PAR         0x00        // physical address
-#define EEP_OFS_ANTENNA     0x17
-#define EEP_OFS_RADIOCTL    0x18
-#define EEP_OFS_RFTYPE      0x1B        // for select RF
-#define EEP_OFS_MINCHANNEL  0x1C        // Min Channel #
-#define EEP_OFS_MAXCHANNEL  0x1D        // Max Channel #
-#define EEP_OFS_SIGNATURE   0x1E        //
-#define EEP_OFS_ZONETYPE    0x1F        //
-#define EEP_OFS_RFTABLE     0x20        // RF POWER TABLE
-#define EEP_OFS_PWR_CCK     0x20
-#define EEP_OFS_SETPT_CCK   0x21
-#define EEP_OFS_PWR_OFDMG   0x23
-
-#define EEP_OFS_CALIB_TX_IQ 0x24
-#define EEP_OFS_CALIB_TX_DC 0x25
-#define EEP_OFS_CALIB_RX_IQ 0x26
-
-#define EEP_OFS_MAJOR_VER 0x2E
-#define EEP_OFS_MINOR_VER 0x2F
-
-#define EEP_OFS_CCK_PWR_TBL     0x30
-#define EEP_OFS_OFDM_PWR_TBL    0x40
-#define EEP_OFS_OFDMA_PWR_TBL   0x50
-
-//
-// Bits in EEP_OFS_ANTENNA
-//
-#define EEP_ANTENNA_MAIN    0x01
-#define EEP_ANTENNA_AUX     0x02
-#define EEP_ANTINV          0x04
-
-//
-// Bits in EEP_OFS_RADIOCTL
-//
-#define EEP_RADIOCTL_ENABLE 0x80
-
-// AT24C02 eeprom contents
-//      2048 bits = 256 bytes = 128 words
-//
-typedef struct tagSSromReg {
-    u8    abyPAR[6];                  // 0x00 (u16)
-
-    u16    wSUB_VID;                   // 0x03 (u16)
-    u16    wSUB_SID;
-
-    u8    byBCFG0;                    // 0x05 (u16)
-    u8    byBCFG1;
-
-    u8    byFCR0;                     // 0x06 (u16)
-    u8    byFCR1;
-    u8    byPMC0;                     // 0x07 (u16)
-    u8    byPMC1;
-    u8    byMAXLAT;                   // 0x08 (u16)
-    u8    byMINGNT;
-    u8    byCFG0;                     // 0x09 (u16)
-    u8    byCFG1;
-    u16    wCISPTR;                    // 0x0A (u16)
-    u16    wRsv0;                      // 0x0B (u16)
-    u16    wRsv1;                      // 0x0C (u16)
-    u8    byBBPAIR;                   // 0x0D (u16)
-    u8    byRFTYPE;
-    u8    byMinChannel;               // 0x0E (u16)
-    u8    byMaxChannel;
-    u8    bySignature;                // 0x0F (u16)
-    u8    byCheckSum;
-
-    u8    abyReserved0[96];           // 0x10 (u16)
-    u8    abyCIS[128];                // 0x80 (u16)
-} SSromReg, *PSSromReg;
-
-#endif /* __EEPROM_H__ */
diff --git a/drivers/staging/vt6656/tcrc.c b/drivers/staging/vt6656/tcrc.c
deleted file mode 100644
index 7229f26c052..00000000000
--- a/drivers/staging/vt6656/tcrc.c
+++ /dev/null
@@ -1,183 +0,0 @@
-/*
- * Copyright (c) 2003 VIA Networking, Inc. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: tcrc.c
- *
- * Purpose: Implement functions to calculate CRC
- *
- * Author: Tevin Chen
- *
- * Date: May 21, 1996
- *
- * Functions:
- *      CRCdwCrc32 -
- *      CRCdwGetCrc32 -
- *      CRCdwGetCrc32Ex -
- *
- * Revision History:
- *
- */
-
-#include "tcrc.h"
-
-/* 32-bit CRC table */
-static const u32 s_adwCrc32Table[256] = {
-    0x00000000L, 0x77073096L, 0xEE0E612CL, 0x990951BAL,
-    0x076DC419L, 0x706AF48FL, 0xE963A535L, 0x9E6495A3L,
-    0x0EDB8832L, 0x79DCB8A4L, 0xE0D5E91EL, 0x97D2D988L,
-    0x09B64C2BL, 0x7EB17CBDL, 0xE7B82D07L, 0x90BF1D91L,
-    0x1DB71064L, 0x6AB020F2L, 0xF3B97148L, 0x84BE41DEL,
-    0x1ADAD47DL, 0x6DDDE4EBL, 0xF4D4B551L, 0x83D385C7L,
-    0x136C9856L, 0x646BA8C0L, 0xFD62F97AL, 0x8A65C9ECL,
-    0x14015C4FL, 0x63066CD9L, 0xFA0F3D63L, 0x8D080DF5L,
-    0x3B6E20C8L, 0x4C69105EL, 0xD56041E4L, 0xA2677172L,
-    0x3C03E4D1L, 0x4B04D447L, 0xD20D85FDL, 0xA50AB56BL,
-    0x35B5A8FAL, 0x42B2986CL, 0xDBBBC9D6L, 0xACBCF940L,
-    0x32D86CE3L, 0x45DF5C75L, 0xDCD60DCFL, 0xABD13D59L,
-    0x26D930ACL, 0x51DE003AL, 0xC8D75180L, 0xBFD06116L,
-    0x21B4F4B5L, 0x56B3C423L, 0xCFBA9599L, 0xB8BDA50FL,
-    0x2802B89EL, 0x5F058808L, 0xC60CD9B2L, 0xB10BE924L,
-    0x2F6F7C87L, 0x58684C11L, 0xC1611DABL, 0xB6662D3DL,
-    0x76DC4190L, 0x01DB7106L, 0x98D220BCL, 0xEFD5102AL,
-    0x71B18589L, 0x06B6B51FL, 0x9FBFE4A5L, 0xE8B8D433L,
-    0x7807C9A2L, 0x0F00F934L, 0x9609A88EL, 0xE10E9818L,
-    0x7F6A0DBBL, 0x086D3D2DL, 0x91646C97L, 0xE6635C01L,
-    0x6B6B51F4L, 0x1C6C6162L, 0x856530D8L, 0xF262004EL,
-    0x6C0695EDL, 0x1B01A57BL, 0x8208F4C1L, 0xF50FC457L,
-    0x65B0D9C6L, 0x12B7E950L, 0x8BBEB8EAL, 0xFCB9887CL,
-    0x62DD1DDFL, 0x15DA2D49L, 0x8CD37CF3L, 0xFBD44C65L,
-    0x4DB26158L, 0x3AB551CEL, 0xA3BC0074L, 0xD4BB30E2L,
-    0x4ADFA541L, 0x3DD895D7L, 0xA4D1C46DL, 0xD3D6F4FBL,
-    0x4369E96AL, 0x346ED9FCL, 0xAD678846L, 0xDA60B8D0L,
-    0x44042D73L, 0x33031DE5L, 0xAA0A4C5FL, 0xDD0D7CC9L,
-    0x5005713CL, 0x270241AAL, 0xBE0B1010L, 0xC90C2086L,
-    0x5768B525L, 0x206F85B3L, 0xB966D409L, 0xCE61E49FL,
-    0x5EDEF90EL, 0x29D9C998L, 0xB0D09822L, 0xC7D7A8B4L,
-    0x59B33D17L, 0x2EB40D81L, 0xB7BD5C3BL, 0xC0BA6CADL,
-    0xEDB88320L, 0x9ABFB3B6L, 0x03B6E20CL, 0x74B1D29AL,
-    0xEAD54739L, 0x9DD277AFL, 0x04DB2615L, 0x73DC1683L,
-    0xE3630B12L, 0x94643B84L, 0x0D6D6A3EL, 0x7A6A5AA8L,
-    0xE40ECF0BL, 0x9309FF9DL, 0x0A00AE27L, 0x7D079EB1L,
-    0xF00F9344L, 0x8708A3D2L, 0x1E01F268L, 0x6906C2FEL,
-    0xF762575DL, 0x806567CBL, 0x196C3671L, 0x6E6B06E7L,
-    0xFED41B76L, 0x89D32BE0L, 0x10DA7A5AL, 0x67DD4ACCL,
-    0xF9B9DF6FL, 0x8EBEEFF9L, 0x17B7BE43L, 0x60B08ED5L,
-    0xD6D6A3E8L, 0xA1D1937EL, 0x38D8C2C4L, 0x4FDFF252L,
-    0xD1BB67F1L, 0xA6BC5767L, 0x3FB506DDL, 0x48B2364BL,
-    0xD80D2BDAL, 0xAF0A1B4CL, 0x36034AF6L, 0x41047A60L,
-    0xDF60EFC3L, 0xA867DF55L, 0x316E8EEFL, 0x4669BE79L,
-    0xCB61B38CL, 0xBC66831AL, 0x256FD2A0L, 0x5268E236L,
-    0xCC0C7795L, 0xBB0B4703L, 0x220216B9L, 0x5505262FL,
-    0xC5BA3BBEL, 0xB2BD0B28L, 0x2BB45A92L, 0x5CB36A04L,
-    0xC2D7FFA7L, 0xB5D0CF31L, 0x2CD99E8BL, 0x5BDEAE1DL,
-    0x9B64C2B0L, 0xEC63F226L, 0x756AA39CL, 0x026D930AL,
-    0x9C0906A9L, 0xEB0E363FL, 0x72076785L, 0x05005713L,
-    0x95BF4A82L, 0xE2B87A14L, 0x7BB12BAEL, 0x0CB61B38L,
-    0x92D28E9BL, 0xE5D5BE0DL, 0x7CDCEFB7L, 0x0BDBDF21L,
-    0x86D3D2D4L, 0xF1D4E242L, 0x68DDB3F8L, 0x1FDA836EL,
-    0x81BE16CDL, 0xF6B9265BL, 0x6FB077E1L, 0x18B74777L,
-    0x88085AE6L, 0xFF0F6A70L, 0x66063BCAL, 0x11010B5CL,
-    0x8F659EFFL, 0xF862AE69L, 0x616BFFD3L, 0x166CCF45L,
-    0xA00AE278L, 0xD70DD2EEL, 0x4E048354L, 0x3903B3C2L,
-    0xA7672661L, 0xD06016F7L, 0x4969474DL, 0x3E6E77DBL,
-    0xAED16A4AL, 0xD9D65ADCL, 0x40DF0B66L, 0x37D83BF0L,
-    0xA9BCAE53L, 0xDEBB9EC5L, 0x47B2CF7FL, 0x30B5FFE9L,
-    0xBDBDF21CL, 0xCABAC28AL, 0x53B39330L, 0x24B4A3A6L,
-    0xBAD03605L, 0xCDD70693L, 0x54DE5729L, 0x23D967BFL,
-    0xB3667A2EL, 0xC4614AB8L, 0x5D681B02L, 0x2A6F2B94L,
-    0xB40BBE37L, 0xC30C8EA1L, 0x5A05DF1BL, 0x2D02EF8DL
-};
-
-/*+
- *
- * Description:
- *    Generate a CRC-32 from the data stream
- *
- * Parameters:
- *  In:
- *      pbyData     - the data stream
- *      cbByte      - the length of the stream
- *      dwCrcSeed   - Seed for CRC32
- *  Out:
- *      none
- *
- * Return Value: CRC-32
- *
--*/
-u32 CRCdwCrc32(u8 * pbyData, unsigned int cbByte, u32 dwCrcSeed)
-{
-	u32 dwCrc;
-
-	dwCrc = dwCrcSeed;
-	while (cbByte--) {
-		dwCrc = s_adwCrc32Table[(u8)((dwCrc ^ (*pbyData)) & 0xFF)] ^
-			(dwCrc >> 8);
-		pbyData++;
-	}
-
-	return dwCrc;
-}
-
-/*+
- *
- * Description:
- * To test CRC generator, input 8 bytes packet
- *      -- 0xff 0xff 0xff 0xff 0x00 0x00 0x00 0x00
- * the generated CRC should be
- *      -- 0xff 0xff 0xff 0xff
- *
- * Parameters:
- *  In:
- *      pbyData     - the data stream
- *      cbByte      - the length of the stream
- *  Out:
- *      none
- *
- * Return Value: CRC-32
- *
--*/
-u32 CRCdwGetCrc32(u8 * pbyData, unsigned int cbByte)
-{
-    return ~CRCdwCrc32(pbyData, cbByte, 0xFFFFFFFFL);
-}
-
-/*+
- *
- * Description:
- *
- * NOTE.... Because CRCdwGetCrc32Ex() is an iteration function,
- *          this means we will use the output of CRCdwGetCrc32Ex()
- *          to be a new argument to do next CRCdwGetCrc32Ex() calculation.
- *          Thus, the final result must be inverted to be the
- *          correct answer.
- *
- * Parameters:
- *  In:
- *      pbyData     - the data stream
- *      cbByte      - the length of the stream
- *  Out:
- *      none
- *
- * Return Value: CRC-32
- *
--*/
-u32 CRCdwGetCrc32Ex(u8 * pbyData, unsigned int cbByte, u32 dwPreCRC)
-{
-    return CRCdwCrc32(pbyData, cbByte, dwPreCRC);
-}
-
diff --git a/drivers/staging/vt6656/tcrc.h b/drivers/staging/vt6656/tcrc.h
deleted file mode 100644
index 5b1f368964e..00000000000
--- a/drivers/staging/vt6656/tcrc.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/*
- * Copyright (c) 2003 VIA Networking, Inc. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
- *
- * File: tcrc.h
- *
- * Purpose: Implement functions to calculate CRC
- *
- * Author: Tevin Chen
- *
- * Date: Jan. 28, 1997
- *
- */
-
-#ifndef __TCRC_H__
-#define __TCRC_H__
-
-#include <linux/types.h>
-
-u32 CRCdwCrc32(u8 * pbyData, unsigned int cbByte, u32 dwCrcSeed);
-u32 CRCdwGetCrc32(u8 * pbyData, unsigned int cbByte);
-u32 CRCdwGetCrc32Ex(u8 * pbyData, unsigned int cbByte, u32 dwPreCRC);
-
-#endif /* __TCRC_H__ */
diff --git a/drivers/staging/vt6656/tether.c b/drivers/staging/vt6656/tether.c
index 1db1e8419a9..2ef54f608cb 100644
--- a/drivers/staging/vt6656/tether.c
+++ b/drivers/staging/vt6656/tether.c
@@ -33,7 +33,6 @@
 
 #include "device.h"
 #include "tmacro.h"
-#include "tcrc.h"
 #include "tether.h"
 
 /*
@@ -51,11 +50,11 @@
  */
 bool ETHbIsBufferCrc32Ok(u8 * pbyBuffer, unsigned int cbFrameLength)
 {
-	u32 dwCRC;
+	u32 n_crc = ~ether_crc_le(cbFrameLength - 4, pbyBuffer);
 
-	dwCRC = CRCdwGetCrc32(pbyBuffer, cbFrameLength - 4);
-	if (cpu_to_le32(*((u32 *)(pbyBuffer + cbFrameLength - 4))) != dwCRC)
+	if (le32_to_cpu(*((__le32 *)(pbyBuffer + cbFrameLength - 4))) != n_crc)
 		return false;
+
 	return true;
 }
 
diff --git a/drivers/staging/vt6656/tether.h b/drivers/staging/vt6656/tether.h
index aec6b568a4a..f57fcfdc24d 100644
--- a/drivers/staging/vt6656/tether.h
+++ b/drivers/staging/vt6656/tether.h
@@ -60,8 +60,6 @@
 #define TYPE_DATE_NULL      0x4800
 
 #define TYPE_CTL_PSPOLL     0xa400
-#define TYPE_CTL_RTS        0xb400
-#define TYPE_CTL_CTS        0xc400
 #define TYPE_CTL_ACK        0xd400
 
 #else //if LITTLE_ENDIAN
@@ -91,8 +89,6 @@
 #define TYPE_DATE_NULL      0x0048
 
 #define TYPE_CTL_PSPOLL     0x00a4
-#define TYPE_CTL_RTS        0x00b4
-#define TYPE_CTL_CTS        0x00c4
 #define TYPE_CTL_ACK        0x00d4
 
 #endif //#ifdef __BIG_ENDIAN
diff --git a/drivers/staging/vt6656/usbpipe.c b/drivers/staging/vt6656/usbpipe.c
index c5838d99f89..e4751b71e4d 100644
--- a/drivers/staging/vt6656/usbpipe.c
+++ b/drivers/staging/vt6656/usbpipe.c
@@ -26,10 +26,10 @@
  * Date: Mar. 29, 2005
  *
  * Functions:
- *      CONTROLnsRequestOut - Write variable length bytes to MEM/BB/MAC/EEPROM
- *      CONTROLnsRequestIn - Read variable length bytes from MEM/BB/MAC/EEPROM
- *      ControlvWriteByte - Write one byte to MEM/BB/MAC/EEPROM
- *      ControlvReadByte - Read one byte from MEM/BB/MAC/EEPROM
+ *	vnt_control_out - Write variable length bytes to MEM/BB/MAC/EEPROM
+ *	vnt_control_in - Read variable length bytes from MEM/BB/MAC/EEPROM
+ *	vnt_control_out_u8 - Write one byte to MEM/BB/MAC/EEPROM
+ *	vnt_control_in_u8 - Read one byte from MEM/BB/MAC/EEPROM
  *      ControlvMaskByte - Read one byte from MEM/BB/MAC/EEPROM and clear/set some bits in the same address
  *
  * Revision History:
@@ -41,9 +41,9 @@
 #include "int.h"
 #include "rxtx.h"
 #include "dpc.h"
-#include "control.h"
 #include "desc.h"
 #include "device.h"
+#include "usbpipe.h"
 
 //endpoint def
 //endpoint 0: control
@@ -51,9 +51,6 @@
 //endpoint 2: read bulk
 //endpoint 3: write bulk
 
-//static int          msglevel                =MSG_LEVEL_DEBUG;
-static int          msglevel                =MSG_LEVEL_INFO;
-
 #define USB_CTL_WAIT   500 //ms
 
 #ifndef URB_ASYNC_UNLINK
@@ -63,226 +60,61 @@ static int          msglevel                =MSG_LEVEL_INFO;
 static void s_nsInterruptUsbIoCompleteRead(struct urb *urb);
 static void s_nsBulkInUsbIoCompleteRead(struct urb *urb);
 static void s_nsBulkOutIoCompleteWrite(struct urb *urb);
-static void s_nsControlInUsbIoCompleteRead(struct urb *urb);
-static void s_nsControlInUsbIoCompleteWrite(struct urb *urb);
 
-int PIPEnsControlOutAsyn(struct vnt_private *pDevice, u8 byRequest,
-	u16 wValue, u16 wIndex, u16 wLength, u8 *pbyBuffer)
+int vnt_control_out(struct vnt_private *priv, u8 request, u16 value,
+		u16 index, u16 length, u8 *buffer)
 {
-	int ntStatus;
-
-    if (pDevice->Flags & fMP_DISCONNECTED)
-        return STATUS_FAILURE;
-
-    if (pDevice->Flags & fMP_CONTROL_WRITES)
-        return STATUS_FAILURE;
-
-    if (in_interrupt()) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"in_interrupt return ..byRequest %x\n", byRequest);
-        return STATUS_FAILURE;
-    }
-
-    ntStatus = usb_control_msg(
-                            pDevice->usb,
-                            usb_sndctrlpipe(pDevice->usb , 0),
-                            byRequest,
-                            0x40, // RequestType
-                            wValue,
-                            wIndex,
-			    (void *) pbyBuffer,
-                            wLength,
-                            HZ
-                          );
-    if (ntStatus >= 0) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"usb_sndctrlpipe ntStatus= %d\n", ntStatus);
-        ntStatus = 0;
-    } else {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"usb_sndctrlpipe fail, ntStatus= %d\n", ntStatus);
-    }
-
-    return ntStatus;
-}
-
-int PIPEnsControlOut(struct vnt_private *pDevice, u8 byRequest, u16 wValue,
-		u16 wIndex, u16 wLength, u8 *pbyBuffer)
-		__releases(&pDevice->lock)
-		__acquires(&pDevice->lock)
-{
-	int ntStatus = 0;
-	int ii;
-
-    if (pDevice->Flags & fMP_DISCONNECTED)
-        return STATUS_FAILURE;
-
-    if (pDevice->Flags & fMP_CONTROL_WRITES)
-        return STATUS_FAILURE;
+	int status = 0;
 
-	if (pDevice->Flags & fMP_CONTROL_READS)
+	if (priv->Flags & fMP_DISCONNECTED)
 		return STATUS_FAILURE;
 
-	if (pDevice->pControlURB->hcpriv)
-		return STATUS_FAILURE;
+	mutex_lock(&priv->usb_lock);
 
-	MP_SET_FLAG(pDevice, fMP_CONTROL_WRITES);
-
-	pDevice->sUsbCtlRequest.bRequestType = 0x40;
-	pDevice->sUsbCtlRequest.bRequest = byRequest;
-	pDevice->sUsbCtlRequest.wValue = cpu_to_le16p(&wValue);
-	pDevice->sUsbCtlRequest.wIndex = cpu_to_le16p(&wIndex);
-	pDevice->sUsbCtlRequest.wLength = cpu_to_le16p(&wLength);
-	pDevice->pControlURB->transfer_flags |= URB_ASYNC_UNLINK;
-    pDevice->pControlURB->actual_length = 0;
-    // Notice, pbyBuffer limited point to variable buffer, can't be constant.
-  	usb_fill_control_urb(pDevice->pControlURB, pDevice->usb,
-			 usb_sndctrlpipe(pDevice->usb , 0), (char *) &pDevice->sUsbCtlRequest,
-			 pbyBuffer, wLength, s_nsControlInUsbIoCompleteWrite, pDevice);
-
-	ntStatus = usb_submit_urb(pDevice->pControlURB, GFP_ATOMIC);
-	if (ntStatus != 0) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-			"control send request submission failed: %d\n",
-				ntStatus);
-		MP_CLEAR_FLAG(pDevice, fMP_CONTROL_WRITES);
-		return STATUS_FAILURE;
-	}
+	status = usb_control_msg(priv->usb,
+		usb_sndctrlpipe(priv->usb, 0), request, 0x40, value,
+			index, buffer, length, USB_CTL_WAIT);
 
-	spin_unlock_irq(&pDevice->lock);
-    for (ii = 0; ii <= USB_CTL_WAIT; ii ++) {
+	mutex_unlock(&priv->usb_lock);
 
-	if (pDevice->Flags & fMP_CONTROL_WRITES)
-		mdelay(1);
-        else
-		break;
+	if (status < (int)length)
+		return STATUS_FAILURE;
 
-        if (ii >= USB_CTL_WAIT) {
-		DBG_PRT(MSG_LEVEL_DEBUG,
-			KERN_INFO "control send request submission timeout\n");
-            spin_lock_irq(&pDevice->lock);
-            MP_CLEAR_FLAG(pDevice, fMP_CONTROL_WRITES);
-            return STATUS_FAILURE;
-        }
-    }
-	spin_lock_irq(&pDevice->lock);
-
-    return STATUS_SUCCESS;
+	return STATUS_SUCCESS;
 }
 
-int PIPEnsControlIn(struct vnt_private *pDevice, u8 byRequest, u16 wValue,
-	u16 wIndex, u16 wLength,  u8 *pbyBuffer)
-	__releases(&pDevice->lock)
-	__acquires(&pDevice->lock)
+void vnt_control_out_u8(struct vnt_private *priv, u8 reg, u8 reg_off, u8 data)
 {
-	int ntStatus = 0;
-	int ii;
-
-    if (pDevice->Flags & fMP_DISCONNECTED)
-        return STATUS_FAILURE;
-
-    if (pDevice->Flags & fMP_CONTROL_READS)
-	return STATUS_FAILURE;
-
-	if (pDevice->Flags & fMP_CONTROL_WRITES)
-		return STATUS_FAILURE;
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE,
+					reg_off, reg, sizeof(u8), &data);
+}
 
-	if (pDevice->pControlURB->hcpriv)
-		return STATUS_FAILURE;
+int vnt_control_in(struct vnt_private *priv, u8 request, u16 value,
+		u16 index, u16 length, u8 *buffer)
+{
+	int status;
 
-	MP_SET_FLAG(pDevice, fMP_CONTROL_READS);
-
-	pDevice->sUsbCtlRequest.bRequestType = 0xC0;
-	pDevice->sUsbCtlRequest.bRequest = byRequest;
-	pDevice->sUsbCtlRequest.wValue = cpu_to_le16p(&wValue);
-	pDevice->sUsbCtlRequest.wIndex = cpu_to_le16p(&wIndex);
-	pDevice->sUsbCtlRequest.wLength = cpu_to_le16p(&wLength);
-	pDevice->pControlURB->transfer_flags |= URB_ASYNC_UNLINK;
-    pDevice->pControlURB->actual_length = 0;
-	usb_fill_control_urb(pDevice->pControlURB, pDevice->usb,
-			 usb_rcvctrlpipe(pDevice->usb , 0), (char *) &pDevice->sUsbCtlRequest,
-			 pbyBuffer, wLength, s_nsControlInUsbIoCompleteRead, pDevice);
-
-	ntStatus = usb_submit_urb(pDevice->pControlURB, GFP_ATOMIC);
-	if (ntStatus != 0) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-			"control request submission failed: %d\n", ntStatus);
-		MP_CLEAR_FLAG(pDevice, fMP_CONTROL_READS);
+	if (priv->Flags & fMP_DISCONNECTED)
 		return STATUS_FAILURE;
-	}
 
-	spin_unlock_irq(&pDevice->lock);
-    for (ii = 0; ii <= USB_CTL_WAIT; ii ++) {
+	mutex_lock(&priv->usb_lock);
 
-	if (pDevice->Flags & fMP_CONTROL_READS)
-		mdelay(1);
-	else
-		break;
-
-	if (ii >= USB_CTL_WAIT) {
-		DBG_PRT(MSG_LEVEL_DEBUG,
-			KERN_INFO "control rcv request submission timeout\n");
-            spin_lock_irq(&pDevice->lock);
-            MP_CLEAR_FLAG(pDevice, fMP_CONTROL_READS);
-            return STATUS_FAILURE;
-        }
-    }
-	spin_lock_irq(&pDevice->lock);
-
-    return ntStatus;
-}
+	status = usb_control_msg(priv->usb,
+		usb_rcvctrlpipe(priv->usb, 0), request, 0xc0, value,
+			index, buffer, length, USB_CTL_WAIT);
 
-static void s_nsControlInUsbIoCompleteWrite(struct urb *urb)
-{
-	struct vnt_private *pDevice = (struct vnt_private *)urb->context;
+	mutex_unlock(&priv->usb_lock);
 
-	pDevice = urb->context;
-	switch (urb->status) {
-	case 0:
-		break;
-	case -EINPROGRESS:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ctrl write urb status EINPROGRESS%d\n", urb->status);
-		break;
-	case -ENOENT:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ctrl write urb status ENOENT %d\n", urb->status);
-		break;
-	default:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ctrl write urb status %d\n", urb->status);
-	}
+	if (status < (int)length)
+		return STATUS_FAILURE;
 
-    MP_CLEAR_FLAG(pDevice, fMP_CONTROL_WRITES);
+	return STATUS_SUCCESS;
 }
 
-/*
- * Description:
- *      Complete function of usb Control callback
- *
- * Parameters:
- *  In:
- *      pDevice     - Pointer to the adapter
- *
- *  Out:
- *      none
- *
- * Return Value: STATUS_INSUFFICIENT_RESOURCES or result of IoCallDriver
- *
- */
-
-static void s_nsControlInUsbIoCompleteRead(struct urb *urb)
+void vnt_control_in_u8(struct vnt_private *priv, u8 reg, u8 reg_off, u8 *data)
 {
-	struct vnt_private *pDevice = (struct vnt_private *)urb->context;
-
-	switch (urb->status) {
-	case 0:
-		break;
-	case -EINPROGRESS:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ctrl read urb status EINPROGRESS%d\n", urb->status);
-		break;
-	case -ENOENT:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ctrl read urb status = ENOENT %d\n", urb->status);
-		break;
-	default:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ctrl read urb status %d\n", urb->status);
-	}
-
-    MP_CLEAR_FLAG(pDevice, fMP_CONTROL_READS);
+	vnt_control_in(priv, MESSAGE_TYPE_READ,
+			reg_off, reg, sizeof(u8), data);
 }
 
 /*
@@ -303,9 +135,6 @@ int PIPEnsInterruptRead(struct vnt_private *priv)
 {
 	int status = STATUS_FAILURE;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-			"---->s_nsStartInterruptUsbRead()\n");
-
 	if (priv->int_buf.in_use == true)
 		return STATUS_FAILURE;
 
@@ -322,14 +151,10 @@ int PIPEnsInterruptRead(struct vnt_private *priv)
 
 	status = usb_submit_urb(priv->pInterruptURB, GFP_ATOMIC);
 	if (status) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-			"Submit int URB failed %d\n", status);
+		dev_dbg(&priv->usb->dev, "Submit int URB failed %d\n", status);
 		priv->int_buf.in_use = false;
 	}
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-		"<----s_nsStartInterruptUsbRead Return(%x)\n", status);
-
 	return status;
 }
 
@@ -353,9 +178,6 @@ static void s_nsInterruptUsbIoCompleteRead(struct urb *urb)
 	struct vnt_private *priv = urb->context;
 	int status;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-			"---->s_nsInterruptUsbIoCompleteRead\n");
-
 	switch (urb->status) {
 	case 0:
 	case -ETIMEDOUT:
@@ -371,22 +193,17 @@ static void s_nsInterruptUsbIoCompleteRead(struct urb *urb)
 
 	status = urb->status;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-		"s_nsInterruptUsbIoCompleteRead Status %d\n", status);
-
 	if (status != STATUS_SUCCESS) {
 		priv->int_buf.in_use = false;
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-			"IntUSBIoCompleteControl STATUS = %d\n", status);
+		dev_dbg(&priv->usb->dev, "%s status = %d\n", __func__, status);
 	} else {
 		INTnsProcessData(priv);
 	}
 
 	status = usb_submit_urb(priv->pInterruptURB, GFP_ATOMIC);
 	if (status) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-			"Submit int URB failed %d\n", status);
+		dev_dbg(&priv->usb->dev, "Submit int URB failed %d\n", status);
 	} else {
 		priv->int_buf.in_use = true;
 	}
@@ -413,14 +230,12 @@ int PIPEnsBulkInUsbRead(struct vnt_private *priv, struct vnt_rcb *rcb)
 	int status = 0;
 	struct urb *urb;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->s_nsStartBulkInUsbRead\n");
-
 	if (priv->Flags & fMP_DISCONNECTED)
 		return STATUS_FAILURE;
 
 	urb = rcb->pUrb;
 	if (rcb->skb == NULL) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"rcb->skb is null\n");
+		dev_dbg(&priv->usb->dev, "rcb->skb is null\n");
 		return status;
 	}
 
@@ -434,8 +249,7 @@ int PIPEnsBulkInUsbRead(struct vnt_private *priv, struct vnt_rcb *rcb)
 
 	status = usb_submit_urb(urb, GFP_ATOMIC);
 	if (status != 0) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-				"Submit Rx URB failed %d\n", status);
+		dev_dbg(&priv->usb->dev, "Submit Rx URB failed %d\n", status);
 		return STATUS_FAILURE ;
 	}
 
@@ -464,10 +278,9 @@ static void s_nsBulkInUsbIoCompleteRead(struct urb *urb)
 {
 	struct vnt_rcb *rcb = urb->context;
 	struct vnt_private *priv = rcb->pDevice;
+	unsigned long flags;
 	int re_alloc_skb = false;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->s_nsBulkInUsbIoCompleteRead\n");
-
 	switch (urb->status) {
 	case 0:
 		break;
@@ -477,29 +290,29 @@ static void s_nsBulkInUsbIoCompleteRead(struct urb *urb)
 		return;
 	case -ETIMEDOUT:
 	default:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-				"BULK In failed %d\n", urb->status);
+		dev_dbg(&priv->usb->dev, "BULK In failed %d\n", urb->status);
 		break;
 	}
 
 	if (urb->actual_length) {
-		spin_lock(&priv->lock);
+		spin_lock_irqsave(&priv->lock, flags);
 
 		if (RXbBulkInProcessData(priv, rcb, urb->actual_length) == true)
 			re_alloc_skb = true;
 
-		spin_unlock(&priv->lock);
+		spin_unlock_irqrestore(&priv->lock, flags);
 	}
 
 	rcb->Ref--;
 	if (rcb->Ref == 0) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"RxvFreeNormal %d\n",
-							priv->NumRecvFreeList);
-		spin_lock(&priv->lock);
+		dev_dbg(&priv->usb->dev,
+				"RxvFreeNormal %d\n", priv->NumRecvFreeList);
+
+		spin_lock_irqsave(&priv->lock, flags);
 
 		RXvFreeRCB(rcb, re_alloc_skb);
 
-		spin_unlock(&priv->lock);
+		spin_unlock_irqrestore(&priv->lock, flags);
 	}
 
 	return;
@@ -527,28 +340,26 @@ int PIPEnsSendBulkOut(struct vnt_private *priv,
 
 	priv->bPWBitOn = false;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"s_nsSendBulkOut\n");
-
 	if (!(MP_IS_READY(priv) && priv->Flags & fMP_POST_WRITES)) {
-		context->bBoolInUse = false;
+		context->in_use = false;
 		return STATUS_RESOURCES;
 	}
 
-	urb = context->pUrb;
+	urb = context->urb;
 
 	usb_fill_bulk_urb(urb,
 			priv->usb,
 			usb_sndbulkpipe(priv->usb, 3),
-			context->Data,
-			context->uBufLen,
+			context->data,
+			context->buf_len,
 			s_nsBulkOutIoCompleteWrite,
 			context);
 
 	status = usb_submit_urb(urb, GFP_ATOMIC);
 	if (status != 0) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-				"Submit Tx URB failed %d\n", status);
-		context->bBoolInUse = false;
+		dev_dbg(&priv->usb->dev, "Submit Tx URB failed %d\n", status);
+
+		context->in_use = false;
 		return STATUS_FAILURE;
 	}
 
@@ -586,25 +397,21 @@ int PIPEnsSendBulkOut(struct vnt_private *priv,
 static void s_nsBulkOutIoCompleteWrite(struct urb *urb)
 {
 	struct vnt_usb_send_context *context = urb->context;
-	struct vnt_private *priv = context->pDevice;
+	struct vnt_private *priv = context->priv;
 	u8 context_type = context->type;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->s_nsBulkOutIoCompleteWrite\n");
-
 	switch (urb->status) {
 	case 0:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-			"Write %d bytes\n", context->uBufLen);
+		dev_dbg(&priv->usb->dev, "Write %d bytes\n", context->buf_len);
 		break;
 	case -ECONNRESET:
 	case -ENOENT:
 	case -ESHUTDOWN:
-		context->bBoolInUse = false;
+		context->in_use = false;
 		return;
 	case -ETIMEDOUT:
 	default:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-				"BULK Out failed %d\n", urb->status);
+		dev_dbg(&priv->usb->dev, "BULK Out failed %d\n", urb->status);
 		break;
 	}
 
@@ -612,11 +419,11 @@ static void s_nsBulkOutIoCompleteWrite(struct urb *urb)
 		return;
 
 	if (CONTEXT_DATA_PACKET == context_type) {
-		if (context->pPacket != NULL) {
-			dev_kfree_skb_irq(context->pPacket);
-			context->pPacket = NULL;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-				"tx  %d bytes\n", context->uBufLen);
+		if (context->skb != NULL) {
+			dev_kfree_skb_irq(context->skb);
+			context->skb = NULL;
+			dev_dbg(&priv->usb->dev,
+					"tx  %d bytes\n", context->buf_len);
 		}
 
 		priv->dev->trans_start = jiffies;
@@ -627,7 +434,7 @@ static void s_nsBulkOutIoCompleteWrite(struct urb *urb)
 			netif_wake_queue(priv->dev);
 	}
 
-	context->bBoolInUse = false;
+	context->in_use = false;
 
 	return;
 }
diff --git a/drivers/staging/vt6656/usbpipe.h b/drivers/staging/vt6656/usbpipe.h
index f53770329e7..ea71782d890 100644
--- a/drivers/staging/vt6656/usbpipe.h
+++ b/drivers/staging/vt6656/usbpipe.h
@@ -32,12 +32,11 @@
 
 #include "device.h"
 
-int PIPEnsControlOut(struct vnt_private *, u8 byRequest, u16 wValue,
-		u16 wIndex, u16 wLength, u8 *pbyBuffer);
-int PIPEnsControlOutAsyn(struct vnt_private *, u8 byRequest,
-	u16 wValue, u16 wIndex, u16 wLength, u8 *pbyBuffer);
-int PIPEnsControlIn(struct vnt_private *, u8 byRequest, u16 wValue,
-	u16 wIndex, u16 wLength,  u8 *pbyBuffer);
+int vnt_control_out(struct vnt_private *, u8, u16, u16, u16, u8 *);
+int vnt_control_in(struct vnt_private *, u8, u16, u16, u16,  u8 *);
+
+void vnt_control_out_u8(struct vnt_private *, u8, u8, u8);
+void vnt_control_in_u8(struct vnt_private *, u8, u8, u8 *);
 
 int PIPEnsInterruptRead(struct vnt_private *);
 int PIPEnsBulkInUsbRead(struct vnt_private *, struct vnt_rcb *pRCB);
diff --git a/drivers/staging/vt6656/wcmd.c b/drivers/staging/vt6656/wcmd.c
index 3cf3f24247a..da72d4df6fc 100644
--- a/drivers/staging/vt6656/wcmd.c
+++ b/drivers/staging/vt6656/wcmd.c
@@ -48,10 +48,9 @@
 #include "power.h"
 #include "wctl.h"
 #include "baseband.h"
-#include "control.h"
+#include "usbpipe.h"
 #include "rxtx.h"
 #include "rf.h"
-#include "rndis.h"
 #include "channel.h"
 #include "iowpa.h"
 
@@ -279,6 +278,7 @@ void vRunCommand(struct work_struct *work)
 	int ii;
 	u8 byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
 	u8 byData;
+	unsigned long flags;
 
 	if (pDevice->Flags & fMP_DISCONNECTED)
 		return;
@@ -286,8 +286,6 @@ void vRunCommand(struct work_struct *work)
 	if (pDevice->bCmdRunning != true)
 		return;
 
-	spin_lock_irq(&pDevice->lock);
-
 	switch (pDevice->eCommandState) {
 
 	case WLAN_CMD_SCAN_START:
@@ -346,11 +344,10 @@ void vRunCommand(struct work_struct *work)
 			CARDbSetMediaChannel(pDevice, pMgmt->uScanChannel);
 			// Set Baseband to be more sensitive.
 
-			if (pDevice->bUpdateBBVGA) {
-				BBvSetShortSlotTime(pDevice);
-				BBvSetVGAGainOffset(pDevice, pDevice->abyBBVGA[0]);
-				BBvUpdatePreEDThreshold(pDevice, true);
-			}
+			BBvSetShortSlotTime(pDevice);
+			BBvSetVGAGainOffset(pDevice, pDevice->abyBBVGA[0]);
+			BBvUpdatePreEDThreshold(pDevice, true);
+
 			pMgmt->uScanChannel++;
 
 			while (!ChannelValid(pDevice->byZoneType, pMgmt->uScanChannel) &&
@@ -365,11 +362,9 @@ void vRunCommand(struct work_struct *work)
 			if ((pMgmt->b11hEnable == false) ||
 			    (pMgmt->uScanChannel < CB_MAX_CHANNEL_24G)) {
 				s_vProbeChannel(pDevice);
-				spin_unlock_irq(&pDevice->lock);
 				vCommandTimerWait((void *) pDevice, 100);
 				return;
 			} else {
-				spin_unlock_irq(&pDevice->lock);
 				vCommandTimerWait((void *) pDevice, WCMD_PASSIVE_SCAN_TIME);
 				return;
 			}
@@ -385,11 +380,9 @@ void vRunCommand(struct work_struct *work)
 			CARDvSetBSSMode(pDevice);
 		}
 
-		if (pDevice->bUpdateBBVGA) {
-			BBvSetShortSlotTime(pDevice);
-			BBvSetVGAGainOffset(pDevice, pDevice->byBBVGACurrent);
-			BBvUpdatePreEDThreshold(pDevice, false);
-		}
+		BBvSetShortSlotTime(pDevice);
+		BBvSetVGAGainOffset(pDevice, pDevice->byBBVGACurrent);
+		BBvUpdatePreEDThreshold(pDevice, false);
 
 		// Set channel back
 		vAdHocBeaconRestart(pDevice);
@@ -433,7 +426,9 @@ void vRunCommand(struct work_struct *work)
 					     (8),
 					     &Status);
 			pDevice->bLinkPass = false;
-			ControlvMaskByte(pDevice, MESSAGE_REQUEST_MACREG, MAC_REG_PAPEDELAY, LEDSTS_STS, LEDSTS_SLOW);
+
+			vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_SLOW);
+
 			// unlock command busy
 			pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
 			pItemSSID->len = 0;
@@ -479,7 +474,8 @@ void vRunCommand(struct work_struct *work)
 			}
 			netif_stop_queue(pDevice->dev);
 			pDevice->bLinkPass = false;
-			ControlvMaskByte(pDevice, MESSAGE_REQUEST_MACREG, MAC_REG_PAPEDELAY, LEDSTS_STS, LEDSTS_SLOW);
+
+			vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_SLOW);
 		}
 		// set initial state
 		pMgmt->eCurrState = WMAC_STATE_IDLE;
@@ -504,7 +500,6 @@ void vRunCommand(struct work_struct *work)
 				pDevice->byLinkWaitCount = 0;
 				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");
 				return;
 			}
@@ -515,7 +510,9 @@ void vRunCommand(struct work_struct *work)
 				if (netif_queue_stopped(pDevice->dev))
 					netif_wake_queue(pDevice->dev);
 				pDevice->bLinkPass = true;
-				ControlvMaskByte(pDevice, MESSAGE_REQUEST_MACREG, MAC_REG_PAPEDELAY, LEDSTS_STS, LEDSTS_INTER);
+
+				vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_INTER);
+
 				pMgmt->sNodeDBTable[0].bActive = true;
 				pMgmt->sNodeDBTable[0].uInActiveCount = 0;
 			} else {
@@ -546,7 +543,6 @@ void vRunCommand(struct work_struct *work)
 				s_bClearBSSID_SCAN(pDevice);
 /*
 				pDevice->bLinkPass = true;
-				ControlvMaskByte(pDevice,MESSAGE_REQUEST_MACREG,MAC_REG_PAPEDELAY,LEDSTS_STS,LEDSTS_INTER);
 				if (netif_queue_stopped(pDevice->dev)){
 					netif_wake_queue(pDevice->dev);
 				}
@@ -578,7 +574,6 @@ void vRunCommand(struct work_struct *work)
 				pDevice->byLinkWaitCount = 0;
 				pDevice->eCommandState = WLAN_ASSOCIATE_WAIT;
 				vCommandTimerWait((void *) pDevice, ASSOCIATE_TIMEOUT);
-				spin_unlock_irq(&pDevice->lock);
 				return;
 			}
 		} else if (pMgmt->eCurrState < WMAC_STATE_AUTHPENDING) {
@@ -587,7 +582,6 @@ void vRunCommand(struct work_struct *work)
 			//mike add:wait another 2 sec if authenticated_frame delay!
 			pDevice->byLinkWaitCount++;
 			printk("WLAN_AUTHENTICATE_WAIT:wait %d times!!\n", pDevice->byLinkWaitCount);
-			spin_unlock_irq(&pDevice->lock);
 			vCommandTimerWait((void *) pDevice, AUTHENTICATE_TIMEOUT/2);
 			return;
 		}
@@ -610,7 +604,9 @@ void vRunCommand(struct work_struct *work)
 			pDevice->byLinkWaitCount = 0;
 			pDevice->byReAssocCount = 0;
 			pDevice->bLinkPass = true;
-			ControlvMaskByte(pDevice, MESSAGE_REQUEST_MACREG, MAC_REG_PAPEDELAY, LEDSTS_STS, LEDSTS_INTER);
+
+			vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_INTER);
+
 			s_bClearBSSID_SCAN(pDevice);
 
 			if (netif_queue_stopped(pDevice->dev))
@@ -622,7 +618,6 @@ void vRunCommand(struct work_struct *work)
 			//mike add:wait another 2 sec if associated_frame delay!
 			pDevice->byLinkWaitCount++;
 			printk("WLAN_ASSOCIATE_WAIT:wait %d times!!\n", pDevice->byLinkWaitCount);
-			spin_unlock_irq(&pDevice->lock);
 			vCommandTimerWait((void *) pDevice, ASSOCIATE_TIMEOUT/2);
 			return;
 		}
@@ -637,11 +632,11 @@ void vRunCommand(struct work_struct *work)
 			pMgmt->eCurrState = WMAC_STATE_IDLE;
 			pMgmt->eCurrMode = WMAC_MODE_STANDBY;
 			pDevice->bLinkPass = false;
-			ControlvMaskByte(pDevice, MESSAGE_REQUEST_MACREG, MAC_REG_PAPEDELAY, LEDSTS_STS, LEDSTS_SLOW);
-			if (pDevice->bEnableHostWEP == true)
-				BSSvClearNodeDBTable(pDevice, 1);
-			else
-				BSSvClearNodeDBTable(pDevice, 0);
+
+			vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_SLOW);
+
+			BSSvClearNodeDBTable(pDevice, 0);
+
 			pDevice->uAssocCount = 0;
 			pMgmt->eCurrState = WMAC_STATE_IDLE;
 			pDevice->bFixRate = false;
@@ -659,7 +654,9 @@ void vRunCommand(struct work_struct *work)
 			if (netif_queue_stopped(pDevice->dev))
 				netif_wake_queue(pDevice->dev);
 			pDevice->bLinkPass = true;
-			ControlvMaskByte(pDevice, MESSAGE_REQUEST_MACREG, MAC_REG_PAPEDELAY, LEDSTS_STS, LEDSTS_INTER);
+
+			vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_INTER);
+
 			schedule_delayed_work(&pDevice->second_callback_work, HZ);
 		}
 		break;
@@ -675,9 +672,13 @@ void vRunCommand(struct work_struct *work)
 					pDevice->bMoreData = true;
 				}
 
-				if (nsDMA_tx_packet(pDevice, TYPE_AC0DMA, skb) != 0)
+				spin_lock_irqsave(&pDevice->lock, flags);
+
+				if (nsDMA_tx_packet(pDevice, skb) != 0)
 					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Multicast ps tx fail\n");
 
+				spin_unlock_irqrestore(&pDevice->lock, flags);
+
 				pMgmt->sNodeDBTable[0].wEnQueueCnt--;
 			}
 		}
@@ -698,9 +699,13 @@ void vRunCommand(struct work_struct *work)
 						pDevice->bMoreData = true;
 					}
 
-					if (nsDMA_tx_packet(pDevice, TYPE_AC0DMA, skb) != 0)
+					spin_lock_irqsave(&pDevice->lock, flags);
+
+					if (nsDMA_tx_packet(pDevice, skb) != 0)
 						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "sta ps tx fail\n");
 
+					spin_unlock_irqrestore(&pDevice->lock, flags);
+
 					pMgmt->sNodeDBTable[ii].wEnQueueCnt--;
 					// check if sta ps enable, wait next pspoll
 					// if sta ps disable, send all pending buffers.
@@ -729,7 +734,7 @@ void vRunCommand(struct work_struct *work)
 			int ntStatus = STATUS_SUCCESS;
 			u8            byTmp;
 
-			ntStatus = CONTROLnsRequestIn(pDevice,
+			ntStatus = vnt_control_in(pDevice,
 					MESSAGE_TYPE_READ,
 					MAC_REG_GPIOCTL1,
 					MESSAGE_REQUEST_MACREG,
@@ -752,9 +757,9 @@ void vRunCommand(struct work_struct *work)
 
 				pDevice->byKeyIndex = 0;
 				pDevice->bTransmitKey = false;
-				spin_unlock_irq(&pDevice->lock);
+
 				KeyvInitTable(pDevice, &pDevice->sKey);
-				spin_lock_irq(&pDevice->lock);
+
 				pMgmt->byCSSPK = KEY_CTL_NONE;
 				pMgmt->byCSSGK = KEY_CTL_NONE;
 
@@ -795,14 +800,17 @@ void vRunCommand(struct work_struct *work)
 				netif_stop_queue(pDevice->dev);
 				CARDbRadioPowerOff(pDevice);
 				MACvRegBitsOn(pDevice, MAC_REG_GPIOCTL1, GPIO3_INTMD);
-				ControlvMaskByte(pDevice, MESSAGE_REQUEST_MACREG, MAC_REG_PAPEDELAY, LEDSTS_STS, LEDSTS_OFF);
+
+				vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_OFF);
+
 				pDevice->bHWRadioOff = true;
 			} else {
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" WLAN_CMD_RADIO_START_ON........................\n");
 				pDevice->bHWRadioOff = false;
 				CARDbRadioPowerOn(pDevice);
 				MACvRegBitsOff(pDevice, MAC_REG_GPIOCTL1, GPIO3_INTMD);
-				ControlvMaskByte(pDevice, MESSAGE_REQUEST_MACREG, MAC_REG_PAPEDELAY, LEDSTS_STS, LEDSTS_ON);
+
+				vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_ON);
 			}
 		}
 
@@ -827,7 +835,8 @@ void vRunCommand(struct work_struct *work)
 
 	case WLAN_CMD_SETPOWER_START:
 
-		RFbSetPower(pDevice, pDevice->wCurrentRate, pMgmt->uCurrChannel);
+		vnt_rf_setpower(pDevice, pDevice->wCurrentRate,
+							pMgmt->uCurrChannel);
 
 		break;
 
@@ -853,10 +862,10 @@ void vRunCommand(struct work_struct *work)
 		break;
 
 	case WLAN_CMD_MAC_DISPOWERSAVING_START:
-		ControlvReadByte(pDevice, MESSAGE_REQUEST_MACREG, MAC_REG_PSCTL, &byData);
+		vnt_control_in_u8(pDevice, MESSAGE_REQUEST_MACREG, MAC_REG_PSCTL, &byData);
 		if ((byData & PSCTL_PS) != 0) {
 			// disable power saving hw function
-			CONTROLnsRequestOut(pDevice,
+			vnt_control_out(pDevice,
 					MESSAGE_TYPE_DISABLE_PS,
 					0,
 					0,
@@ -882,7 +891,6 @@ void vRunCommand(struct work_struct *work)
 
 	s_bCommandComplete(pDevice);
 
-	spin_unlock_irq(&pDevice->lock);
 	return;
 }
 
diff --git a/drivers/staging/vt6656/wctl.c b/drivers/staging/vt6656/wctl.c
index 814342cd948..efdc5d5d38e 100644
--- a/drivers/staging/vt6656/wctl.c
+++ b/drivers/staging/vt6656/wctl.c
@@ -70,7 +70,7 @@ bool WCTLbIsDuplicate (PSCache pCache, struct ieee80211_hdr *pMACHeader)
             pCacheEntry = &(pCache->asCacheEntry[uIndex]);
             if ((pCacheEntry->wFmSequence == pMACHeader->seq_ctrl) &&
 		ether_addr_equal(pCacheEntry->abyAddr2, pMACHeader->addr2) &&
-                (LOBYTE(pCacheEntry->wFrameCtl) == LOBYTE(pMACHeader->frame_control))
+		(pCacheEntry->wFrameCtl == pMACHeader->frame_control)
                 ) {
                 /* Duplicate match */
                 return true;
diff --git a/drivers/staging/vt6656/wmgr.c b/drivers/staging/vt6656/wmgr.c
index 0d69719a742..18723eab93d 100644
--- a/drivers/staging/vt6656/wmgr.c
+++ b/drivers/staging/vt6656/wmgr.c
@@ -78,8 +78,7 @@
 #include "wpa.h"
 #include "rf.h"
 #include "iowpa.h"
-#include "control.h"
-#include "rndis.h"
+#include "usbpipe.h"
 
 static int msglevel = MSG_LEVEL_INFO;
 //static int          msglevel                =MSG_LEVEL_DEBUG;
@@ -541,10 +540,6 @@ static void s_vMgrRxAssocRequest(struct vnt_private *pDevice,
                   (PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates
                 );
     if (pTxPacket != NULL ){
-
-        if (pDevice->bEnableHostapd) {
-            return;
-        }
         /* send the frame */
         Status = csMgmt_xmit(pDevice, pTxPacket);
         if (Status != CMD_STATUS_PENDING) {
@@ -690,9 +685,6 @@ static void s_vMgrRxReAssocRequest(struct vnt_private *pDevice,
 
     if (pTxPacket != NULL ){
         /* send the frame */
-        if (pDevice->bEnableHostapd) {
-            return;
-        }
         Status = csMgmt_xmit(pDevice, pTxPacket);
         if (Status != CMD_STATUS_PENDING) {
             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:ReAssoc response tx failed\n");
@@ -764,7 +756,8 @@ static void s_vMgrRxAssocResponse(struct vnt_private *pDevice,
             pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
             DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Link with AP(SSID): %s\n", pItemSSID->abySSID);
             pDevice->bLinkPass = true;
-            ControlvMaskByte(pDevice,MESSAGE_REQUEST_MACREG,MAC_REG_PAPEDELAY,LEDSTS_STS,LEDSTS_INTER);
+
+	    vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_INTER);
 
 	//if(pDevice->bWPASuppWextEnabled == true)
 	   {
@@ -1075,9 +1068,6 @@ static void s_vMgrRxAuthenSequence_1(struct vnt_private *pDevice,
     pTxPacket->cbMPDULen = sFrame.len;
     pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
     // send the frame
-    if (pDevice->bEnableHostapd) {
-        return;
-    }
     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "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");
@@ -1257,9 +1247,6 @@ reply:
     pTxPacket->cbMPDULen = sFrame.len;
     pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
     // send the frame
-    if (pDevice->bEnableHostapd) {
-        return;
-    }
     if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Authreq_reply sequence_4 tx failed.\n");
     }
@@ -1412,7 +1399,8 @@ static void s_vMgrRxDeauthentication(struct vnt_private *pDevice,
                     pMgmt->eCurrState = WMAC_STATE_IDLE;
                     netif_stop_queue(pDevice->dev);
                     pDevice->bLinkPass = false;
-                    ControlvMaskByte(pDevice,MESSAGE_REQUEST_MACREG,MAC_REG_PAPEDELAY,LEDSTS_STS,LEDSTS_SLOW);
+
+		    vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_SLOW);
                 }
             }
 
@@ -1897,7 +1885,9 @@ if(ChannelExceedZoneType(pDevice,byCurrChannel)==true)
                 DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Current IBSS State: [Started]........to: [Jointed] \n");
                 pMgmt->eCurrState = WMAC_STATE_JOINTED;
                 pDevice->bLinkPass = true;
-                ControlvMaskByte(pDevice,MESSAGE_REQUEST_MACREG,MAC_REG_PAPEDELAY,LEDSTS_STS,LEDSTS_INTER);
+
+		vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_INTER);
+
                 if (netif_queue_stopped(pDevice->dev)){
                     netif_wake_queue(pDevice->dev);
                 }
@@ -2502,7 +2492,9 @@ void vMgrJoinBSSBegin(struct vnt_private *pDevice, PCMD_STATUS pStatus)
             // Adopt BSS state in Adapter Device Object
 	    pDevice->op_mode = NL80211_IFTYPE_ADHOC;
             pDevice->bLinkPass = true;
-            ControlvMaskByte(pDevice,MESSAGE_REQUEST_MACREG,MAC_REG_PAPEDELAY,LEDSTS_STS,LEDSTS_INTER);
+
+	    vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_INTER);
+
             memcpy(pDevice->abyBSSID, pCurr->abyBSSID, WLAN_BSSID_LEN);
 
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Join IBSS ok:%pM\n",
@@ -2572,7 +2564,6 @@ static void s_vMgrSynchBSS(struct vnt_private *pDevice, u32 uBSSMode,
     }
 
     // Init the BSS informations
-    pDevice->bCCK = true;
     pDevice->bProtectMode = false;
     MACvDisableProtectMD(pDevice);
     pDevice->bBarkerPreambleMd = false;
@@ -2655,8 +2646,7 @@ static void s_vMgrSynchBSS(struct vnt_private *pDevice, u32 uBSSMode,
     pMgmt->uCurrChannel = pCurr->uChannel;
     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "<----s_bSynchBSS Set Channel [%d]\n", pCurr->uChannel);
 
-    if ((pDevice->bUpdateBBVGA) &&
-        (pDevice->byBBVGACurrent != pDevice->abyBBVGA[0])) {
+    if (pDevice->byBBVGACurrent != pDevice->abyBBVGA[0]) {
         pDevice->byBBVGACurrent = pDevice->abyBBVGA[0];
         BBvSetVGAGainOffset(pDevice, pDevice->byBBVGACurrent);
         BBvSetShortSlotTime(pDevice);
@@ -2932,16 +2922,6 @@ static struct vnt_tx_mgmt *s_MgrMakeBeacon(struct vnt_private *pDevice,
              ((PWLAN_IE_SUPP_RATES)pCurrExtSuppRates)->len + WLAN_IEHDR_LEN
              );
     }
-    // hostapd wpa/wpa2 IE
-    if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) && (pDevice->bEnableHostapd == true)) {
-         if (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
-             if (pMgmt->wWPAIELen != 0) {
-                 sFrame.pRSN = (PWLAN_IE_RSN)(sFrame.pBuf + sFrame.len);
-                 memcpy(sFrame.pRSN, pMgmt->abyWPAIE, pMgmt->wWPAIELen);
-                 sFrame.len += pMgmt->wWPAIELen;
-             }
-         }
-    }
 
     /* Adjust the length fields */
     pTxPacket->cbMPDULen = sFrame.len;
@@ -3052,17 +3032,6 @@ static struct vnt_tx_mgmt *s_MgrMakeProbeResponse(struct vnt_private *pDevice,
              );
     }
 
-    // hostapd wpa/wpa2 IE
-    if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) && (pDevice->bEnableHostapd == true)) {
-         if (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
-             if (pMgmt->wWPAIELen != 0) {
-                 sFrame.pRSN = (PWLAN_IE_RSN)(sFrame.pBuf + sFrame.len);
-                 memcpy(sFrame.pRSN, pMgmt->abyWPAIE, pMgmt->wWPAIELen);
-                 sFrame.len += pMgmt->wWPAIELen;
-             }
-         }
-    }
-
     // Adjust the length fields
     pTxPacket->cbMPDULen = sFrame.len;
     pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
@@ -4058,14 +4027,14 @@ int bMgrPrepareBeaconToSend(struct vnt_private *pDevice,
 	struct vnt_manager *pMgmt)
 {
 	struct vnt_tx_mgmt *pTxPacket;
+	unsigned long flags;
 
 //    pDevice->bBeaconBufReady = false;
-    if (pDevice->bEncryptionEnable || pDevice->bEnable8021x){
-        pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_PRIVACY(1);
-    }
-    else {
-        pMgmt->wCurrCapInfo &= ~WLAN_SET_CAP_INFO_PRIVACY(1);
-    }
+	if (pDevice->bEncryptionEnable)
+		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_PRIVACY(1);
+	else
+		pMgmt->wCurrCapInfo &= ~WLAN_SET_CAP_INFO_PRIVACY(1);
+
     pTxPacket = s_MgrMakeBeacon
                 (
                   pDevice,
@@ -4084,8 +4053,13 @@ int bMgrPrepareBeaconToSend(struct vnt_private *pDevice,
         (pMgmt->abyCurrBSSID[0] == 0))
         return false;
 
-    csBeacon_xmit(pDevice, pTxPacket);
-    MACvRegBitsOn(pDevice, MAC_REG_TCR, TCR_AUTOBCNTX);
+	spin_lock_irqsave(&pDevice->lock, flags);
+
+	csBeacon_xmit(pDevice, pTxPacket);
+
+	spin_unlock_irqrestore(&pDevice->lock, flags);
+
+	MACvRegBitsOn(pDevice, MAC_REG_TCR, TCR_AUTOBCNTX);
 
     return true;
 }
diff --git a/drivers/staging/vt6656/wpactl.c b/drivers/staging/vt6656/wpactl.c
index f4a8a5cb9fe..0a067151eca 100644
--- a/drivers/staging/vt6656/wpactl.c
+++ b/drivers/staging/vt6656/wpactl.c
@@ -38,8 +38,7 @@
 #include "wmgr.h"
 #include "iocmd.h"
 #include "iowpa.h"
-#include "control.h"
-#include "rndis.h"
+#include "usbpipe.h"
 #include "rf.h"
 
 static int msglevel = MSG_LEVEL_INFO;
@@ -67,7 +66,7 @@ int wpa_set_keys(struct vnt_private *pDevice, void *ctx)
 	u64 KeyRSC;
 	u8 byKeyDecMode = KEY_CTL_WEP;
 	int ret = 0;
-	int uu;
+	u8 uu;
 	int ii;
 
 	if (param->u.wpa_key.alg_name > WPA_ALG_CCMP)
diff --git a/drivers/staging/winbond/wb35tx.c b/drivers/staging/winbond/wb35tx.c
index 708c5b05f86..870cff39a22 100644
--- a/drivers/staging/winbond/wb35tx.c
+++ b/drivers/staging/winbond/wb35tx.c
@@ -49,7 +49,7 @@ static void Wb35Tx_complete(struct urb *pUrb)
 
 	/* The URB is completed, check the result */
 	if (pWb35Tx->EP4VM_status != 0) {
-		printk("URB submission failed\n");
+		dev_err(&pUrb->dev->dev, "URB submission failed\n");
 		pWb35Tx->EP4vm_state = VM_STOP;
 		goto error;
 	}
@@ -96,7 +96,7 @@ static void Wb35Tx(struct wbsoft_priv *adapter)
 	pWb35Tx->EP4vm_state = VM_RUNNING;
 	retv = usb_submit_urb(pUrb, GFP_ATOMIC);
 	if (retv < 0) {
-		printk("EP4 Tx Irp sending error\n");
+		dev_err(&pUrb->dev->dev, "EP4 Tx Irp sending error\n");
 		goto cleanup;
 	}
 
@@ -218,7 +218,7 @@ static void Wb35Tx_EP2VM_complete(struct urb *pUrb)
 
 	/* The Urb is completed, check the result */
 	if (pWb35Tx->EP2VM_status != 0) {
-		printk("EP2 IoCompleteRoutine return error\n");
+		dev_err(&pUrb->dev->dev, "EP2 IoCompleteRoutine return error\n");
 		pWb35Tx->EP2vm_state = VM_STOP;
 		goto error;
 	}
diff --git a/drivers/staging/winbond/wb35tx_s.h b/drivers/staging/winbond/wb35tx_s.h
index 715f87d6ac5..dc120085d52 100644
--- a/drivers/staging/winbond/wb35tx_s.h
+++ b/drivers/staging/winbond/wb35tx_s.h
@@ -12,7 +12,7 @@
 /* Internal variable for module */
 struct wb35_tx {
 	/* For Tx buffer */
-	u8	TxBuffer[ MAX_USB_TX_BUFFER_NUMBER ][ MAX_USB_TX_BUFFER ];
+	u8	TxBuffer[MAX_USB_TX_BUFFER_NUMBER][MAX_USB_TX_BUFFER];
 
 	/* For Interrupt pipe */
 	u8	EP2_buf[MAX_INTERRUPT_LENGTH];
diff --git a/drivers/staging/wlags49_h2/wl_priv.c b/drivers/staging/wlags49_h2/wl_priv.c
index 41f332499d4..aff92735022 100644
--- a/drivers/staging/wlags49_h2/wl_priv.c
+++ b/drivers/staging/wlags49_h2/wl_priv.c
@@ -179,8 +179,6 @@ int wvlan_uil(struct uilreq *urq, struct wl_private *lp)
  ******************************************************************************/
 int wvlan_uil_connect(struct uilreq *urq, struct wl_private *lp)
 {
-	int result = 0;
-
 	if (!(lp->flags & WVLAN2_UIL_CONNECTED)) {
 		lp->flags |= WVLAN2_UIL_CONNECTED;
 		urq->hcfCtx = &(lp->hcfCtx);
@@ -190,7 +188,7 @@ int wvlan_uil_connect(struct uilreq *urq, struct wl_private *lp)
 		urq->result = UIL_ERR_IN_USE;
 	}
 
-	return result;
+	return 0;
 } /* wvlan_uil_connect */
 /*============================================================================*/
 
@@ -218,8 +216,6 @@ int wvlan_uil_connect(struct uilreq *urq, struct wl_private *lp)
  ******************************************************************************/
 int wvlan_uil_disconnect(struct uilreq *urq, struct wl_private *lp)
 {
-	int result = 0;
-
 	if (urq->hcfCtx == &(lp->hcfCtx)) {
 		if (lp->flags & WVLAN2_UIL_CONNECTED) {
 			lp->flags &= ~WVLAN2_UIL_CONNECTED;
@@ -238,7 +234,7 @@ int wvlan_uil_disconnect(struct uilreq *urq, struct wl_private *lp)
 		urq->result = UIL_ERR_WRONG_IFB;
 	}
 
-	return result;
+	return 0;
 } /* wvlan_uil_disconnect */
 /*============================================================================*/
 
@@ -1580,7 +1576,6 @@ int wvlan_set_netname(struct net_device *dev,
 {
 	struct wl_private *lp = wl_priv(dev);
 	unsigned long flags;
-	int ret = 0;
 
 	wl_lock(lp, &flags);
 
@@ -1591,7 +1586,7 @@ int wvlan_set_netname(struct net_device *dev,
 	wl_apply(lp);
 	wl_unlock(lp, &flags);
 
-	return ret;
+	return 0;
 } /* wvlan_set_netname */
 /*============================================================================*/
 
@@ -1683,7 +1678,6 @@ int wvlan_set_station_nickname(struct net_device *dev,
 	struct wl_private *lp = wl_priv(dev);
 	unsigned long flags;
 	size_t len;
-	int         ret = 0;
 
 	wl_lock(lp, &flags);
 
@@ -1695,7 +1689,7 @@ int wvlan_set_station_nickname(struct net_device *dev,
 	wl_apply(lp);
 	wl_unlock(lp, &flags);
 
-	return ret;
+	return 0;
 } /* wvlan_set_station_nickname */
 /*============================================================================*/
 
diff --git a/drivers/staging/wlags49_h2/wl_wext.c b/drivers/staging/wlags49_h2/wl_wext.c
index 49eeeaee664..3aeff818afc 100644
--- a/drivers/staging/wlags49_h2/wl_wext.c
+++ b/drivers/staging/wlags49_h2/wl_wext.c
@@ -159,15 +159,12 @@ static int hermes_set_tkip_keys(ltv_t *ltv, u16 key_idx, u8 *addr,
 /* Set up the LTV to clear the appropriate key */
 static int hermes_clear_tkip_keys(ltv_t *ltv, u16 key_idx, u8 *addr)
 {
-	int ret;
-
 	switch (key_idx) {
 	case 0:
 		if (!is_broadcast_ether_addr(addr)) {
 			ltv->len = 7;
 			ltv->typ = CFG_REMOVE_TKIP_MAPPED_KEY;
 			memcpy(&ltv->u.u8[0], addr, ETH_ALEN);
-			ret = 0;
 		}
 		break;
 	case 1:
@@ -178,13 +175,12 @@ static int hermes_clear_tkip_keys(ltv_t *ltv, u16 key_idx, u8 *addr)
 		ltv->typ = CFG_REMOVE_TKIP_DEFAULT_KEY;
 		ltv->u.u16[0] = cpu_to_le16(key_idx);
 
-		ret = 0;
 		break;
 	default:
 		break;
 	}
 
-	return ret;
+	return 0;
 }
 
 /* Set the WEP keys in the wl_private structure */
@@ -3027,13 +3023,10 @@ static int wireless_set_genie(struct net_device *dev,
 			      struct iw_point *data, char *extra)
 
 {
-	int   ret = 0;
-
 	/* We can't write this to the card, but apparently this
 	 * operation needs to succeed */
-	ret = 0;
 
-	return ret;
+	return 0;
 }
 /*============================================================================*/
 
diff --git a/drivers/staging/wlan-ng/hfa384x_usb.c b/drivers/staging/wlan-ng/hfa384x_usb.c
index 5b8b094c872..98343ff7061 100644
--- a/drivers/staging/wlan-ng/hfa384x_usb.c
+++ b/drivers/staging/wlan-ng/hfa384x_usb.c
@@ -203,7 +203,7 @@ static int hfa384x_usbctlx_submit(hfa384x_t *hw, hfa384x_usbctlx_t *ctlx);
 static void unlocked_usbctlx_complete(hfa384x_t *hw, hfa384x_usbctlx_t *ctlx);
 
 struct usbctlx_completor {
-	int (*complete) (struct usbctlx_completor *);
+	int (*complete)(struct usbctlx_completor *);
 };
 
 static int
@@ -272,7 +272,7 @@ static int hfa384x_isgood_pdrcode(u16 pdrcode);
 
 static inline const char *ctlxstr(CTLX_STATE s)
 {
-	static const char *ctlx_str[] = {
+	static const char * const ctlx_str[] = {
 		"Initial state",
 		"Complete",
 		"Request failed",
@@ -350,14 +350,14 @@ static int submit_rx_urb(hfa384x_t *hw, gfp_t memflags)
 
 	result = -ENOLINK;
 	if (!hw->wlandev->hwremoved &&
-			!test_bit(WORK_RX_HALT, &hw->usb_flags)) {
+	    !test_bit(WORK_RX_HALT, &hw->usb_flags)) {
 		result = SUBMIT_URB(&hw->rx_urb, memflags);
 
 		/* Check whether we need to reset the RX pipe */
 		if (result == -EPIPE) {
 			netdev_warn(hw->wlandev->netdev,
-			       "%s rx pipe stalled: requesting reset\n",
-			       hw->wlandev->netdev->name);
+				    "%s rx pipe stalled: requesting reset\n",
+				    hw->wlandev->netdev->name);
 			if (!test_and_set_bit(WORK_RX_HALT, &hw->usb_flags))
 				schedule_work(&hw->usb_work);
 		}
@@ -398,16 +398,15 @@ static int submit_tx_urb(hfa384x_t *hw, struct urb *tx_urb, gfp_t memflags)
 
 	result = -ENOLINK;
 	if (netif_running(netdev)) {
-
-		if (!hw->wlandev->hwremoved
-		    && !test_bit(WORK_TX_HALT, &hw->usb_flags)) {
+		if (!hw->wlandev->hwremoved &&
+		    !test_bit(WORK_TX_HALT, &hw->usb_flags)) {
 			result = SUBMIT_URB(tx_urb, memflags);
 
 			/* Test whether we need to reset the TX pipe */
 			if (result == -EPIPE) {
 				netdev_warn(hw->wlandev->netdev,
-				       "%s tx pipe stalled: requesting reset\n",
-				       netdev->name);
+					    "%s tx pipe stalled: requesting reset\n",
+					    netdev->name);
 				set_bit(WORK_TX_HALT, &hw->usb_flags);
 				schedule_work(&hw->usb_work);
 			} else if (result == 0) {
@@ -455,11 +454,11 @@ static void hfa384x_usb_defer(struct work_struct *data)
 		ret = usb_clear_halt(hw->usb, hw->endp_in);
 		if (ret != 0) {
 			netdev_err(hw->wlandev->netdev,
-			       "Failed to clear rx pipe for %s: err=%d\n",
-			       netdev->name, ret);
+				   "Failed to clear rx pipe for %s: err=%d\n",
+				   netdev->name, ret);
 		} else {
 			netdev_info(hw->wlandev->netdev, "%s rx pipe reset complete.\n",
-			       netdev->name);
+				    netdev->name);
 			clear_bit(WORK_RX_HALT, &hw->usb_flags);
 			set_bit(WORK_RX_RESUME, &hw->usb_flags);
 		}
@@ -472,7 +471,8 @@ static void hfa384x_usb_defer(struct work_struct *data)
 		ret = submit_rx_urb(hw, GFP_KERNEL);
 		if (ret != 0) {
 			netdev_err(hw->wlandev->netdev,
-			       "Failed to resume %s rx pipe.\n", netdev->name);
+				   "Failed to resume %s rx pipe.\n",
+				   netdev->name);
 		} else {
 			clear_bit(WORK_RX_RESUME, &hw->usb_flags);
 		}
@@ -486,11 +486,11 @@ static void hfa384x_usb_defer(struct work_struct *data)
 		ret = usb_clear_halt(hw->usb, hw->endp_out);
 		if (ret != 0) {
 			netdev_err(hw->wlandev->netdev,
-			       "Failed to clear tx pipe for %s: err=%d\n",
-			       netdev->name, ret);
+				   "Failed to clear tx pipe for %s: err=%d\n",
+				   netdev->name, ret);
 		} else {
 			netdev_info(hw->wlandev->netdev, "%s tx pipe reset complete.\n",
-			       netdev->name);
+				    netdev->name);
 			clear_bit(WORK_TX_HALT, &hw->usb_flags);
 			set_bit(WORK_TX_RESUME, &hw->usb_flags);
 
@@ -642,8 +642,7 @@ usbctlx_get_status(const hfa384x_usb_cmdresp_t *cmdresp,
 	result->resp1 = le16_to_cpu(cmdresp->resp1);
 	result->resp2 = le16_to_cpu(cmdresp->resp2);
 
-	pr_debug("cmdresult:status=0x%04x "
-		 "resp0=0x%04x resp1=0x%04x resp2=0x%04x\n",
+	pr_debug("cmdresult:status=0x%04x resp0=0x%04x resp1=0x%04x resp2=0x%04x\n",
 		 result->status, result->resp0, result->resp1, result->resp2);
 
 	return result->status & HFA384x_STATUS_RESULT;
@@ -656,7 +655,6 @@ usbctlx_get_rridresult(const hfa384x_usb_rridresp_t *rridresp,
 	result->rid = le16_to_cpu(rridresp->rid);
 	result->riddata = rridresp->data;
 	result->riddata_len = ((le16_to_cpu(rridresp->frmlen) - 1) * 2);
-
 }
 
 /*----------------------------------------------------------------
@@ -675,7 +673,7 @@ static inline int usbctlx_cmd_completor_fn(struct usbctlx_completor *head)
 {
 	struct usbctlx_cmd_completor *complete;
 
-	complete = (struct usbctlx_cmd_completor *) head;
+	complete = (struct usbctlx_cmd_completor *)head;
 	return usbctlx_get_status(complete->cmdresp, complete->result);
 }
 
@@ -710,15 +708,14 @@ static int usbctlx_rrid_completor_fn(struct usbctlx_completor *head)
 	struct usbctlx_rrid_completor *complete;
 	hfa384x_rridresult_t rridresult;
 
-	complete = (struct usbctlx_rrid_completor *) head;
+	complete = (struct usbctlx_rrid_completor *)head;
 	usbctlx_get_rridresult(complete->rridresp, &rridresult);
 
 	/* Validate the length, note body len calculation in bytes */
 	if (rridresult.riddata_len != complete->riddatalen) {
-		printk(KERN_WARNING
-		       "RID len mismatch, rid=0x%04x hlen=%d fwlen=%d\n",
-		       rridresult.rid,
-		       complete->riddatalen, rridresult.riddata_len);
+		pr_warn("RID len mismatch, rid=0x%04x hlen=%d fwlen=%d\n",
+			rridresult.rid,
+			complete->riddatalen, rridresult.riddata_len);
 		return -ENODATA;
 	}
 
@@ -745,14 +742,12 @@ static inline struct usbctlx_completor *init_rrid_completor(
 * Completor object:
 * Interprets the results of a synchronous RID-write
 ----------------------------------------------------------------*/
-typedef struct usbctlx_cmd_completor usbctlx_wrid_completor_t;
 #define init_wrid_completor  init_cmd_completor
 
 /*----------------------------------------------------------------
 * Completor object:
 * Interprets the results of a synchronous memory-write
 ----------------------------------------------------------------*/
-typedef struct usbctlx_cmd_completor usbctlx_wmem_completor_t;
 #define init_wmem_completor  init_cmd_completor
 
 /*----------------------------------------------------------------
@@ -766,11 +761,11 @@ struct usbctlx_rmem_completor {
 	void *data;
 	unsigned int len;
 };
-typedef struct usbctlx_rmem_completor usbctlx_rmem_completor_t;
 
 static int usbctlx_rmem_completor_fn(struct usbctlx_completor *head)
 {
-	usbctlx_rmem_completor_t *complete = (usbctlx_rmem_completor_t *) head;
+	struct usbctlx_rmem_completor *complete =
+		(struct usbctlx_rmem_completor *)head;
 
 	pr_debug("rmemresp:len=%d\n", complete->rmemresp->frmlen);
 	memcpy(complete->data, complete->rmemresp->data, complete->len);
@@ -778,7 +773,7 @@ static int usbctlx_rmem_completor_fn(struct usbctlx_completor *head)
 }
 
 static inline struct usbctlx_completor *init_rmem_completor(
-						usbctlx_rmem_completor_t
+						struct usbctlx_rmem_completor
 							*completor,
 						hfa384x_usb_rmemresp_t
 							*rmemresp,
@@ -991,9 +986,7 @@ int hfa384x_cmd_initialize(hfa384x_t *hw)
 
 	result = hfa384x_docmd_wait(hw, &cmd);
 
-	pr_debug("cmdresp.init: "
-		 "status=0x%04x, resp0=0x%04x, "
-		 "resp1=0x%04x, resp2=0x%04x\n",
+	pr_debug("cmdresp.init: status=0x%04x, resp0=0x%04x, resp1=0x%04x, resp2=0x%04x\n",
 		 cmd.result.status,
 		 cmd.result.resp0, cmd.result.resp1, cmd.result.resp2);
 	if (result == 0) {
@@ -1212,7 +1205,7 @@ int hfa384x_corereset(hfa384x_t *hw, int holdtime, int settletime, int genesis)
 	result = usb_reset_device(hw->usb);
 	if (result < 0) {
 		netdev_err(hw->wlandev->netdev, "usb_reset_device() failed, result=%d.\n",
-		       result);
+			   result);
 	}
 
 	return result;
@@ -1312,8 +1305,8 @@ cleanup:
 			result = completor->complete(completor);
 		} else {
 			netdev_warn(hw->wlandev->netdev, "CTLX[%d] error: state(%s)\n",
-			       le16_to_cpu(ctlx->outbuf.type),
-			       ctlxstr(ctlx->state));
+				    le16_to_cpu(ctlx->outbuf.type),
+				    ctlxstr(ctlx->state));
 			result = -EIO;
 		}
 
@@ -1381,8 +1374,7 @@ hfa384x_docmd(hfa384x_t *hw,
 
 	ctlx->outbufsize = sizeof(ctlx->outbuf.cmdreq);
 
-	pr_debug("cmdreq: cmd=0x%04x "
-		 "parm0=0x%04x parm1=0x%04x parm2=0x%04x\n",
+	pr_debug("cmdreq: cmd=0x%04x parm0=0x%04x parm1=0x%04x parm2=0x%04x\n",
 		 cmd->cmd, cmd->parm0, cmd->parm1, cmd->parm2);
 
 	ctlx->reapable = mode;
@@ -1566,7 +1558,7 @@ hfa384x_dowrid(hfa384x_t *hw,
 	if (result != 0) {
 		kfree(ctlx);
 	} else if (mode == DOWAIT) {
-		usbctlx_wrid_completor_t completor;
+		struct usbctlx_cmd_completor completor;
 		hfa384x_cmdresult_t wridresult;
 
 		result = hfa384x_usbctlx_complete_sync(hw,
@@ -1658,7 +1650,7 @@ hfa384x_dormem(hfa384x_t *hw,
 	if (result != 0) {
 		kfree(ctlx);
 	} else if (mode == DOWAIT) {
-		usbctlx_rmem_completor_t completor;
+		struct usbctlx_rmem_completor completor;
 
 		result =
 		    hfa384x_usbctlx_complete_sync(hw, ctlx,
@@ -1748,7 +1740,7 @@ hfa384x_dowmem(hfa384x_t *hw,
 	if (result != 0) {
 		kfree(ctlx);
 	} else if (mode == DOWAIT) {
-		usbctlx_wmem_completor_t completor;
+		struct usbctlx_cmd_completor completor;
 		hfa384x_cmdresult_t wmemresult;
 
 		result = hfa384x_usbctlx_complete_sync(hw,
@@ -2018,7 +2010,8 @@ int hfa384x_drvr_flashdl_write(hfa384x_t *hw, u32 daddr, void *buf, u32 len)
 	if (hw->dlstate != HFA384x_DLSTATE_FLASHENABLED)
 		return -EINVAL;
 
-	netdev_info(hw->wlandev->netdev, "Download %d bytes to flash @0x%06x\n", len, daddr);
+	netdev_info(hw->wlandev->netdev,
+		    "Download %d bytes to flash @0x%06x\n", len, daddr);
 
 	/* Convert to flat address for arithmetic */
 	/* NOTE: dlbuffer RID stores the address in AUX format */
@@ -2026,11 +2019,6 @@ int hfa384x_drvr_flashdl_write(hfa384x_t *hw, u32 daddr, void *buf, u32 len)
 	    HFA384x_ADDR_AUX_MKFLAT(hw->bufinfo.page, hw->bufinfo.offset);
 	pr_debug("dlbuf.page=0x%04x dlbuf.offset=0x%04x dlbufaddr=0x%08x\n",
 		 hw->bufinfo.page, hw->bufinfo.offset, dlbufaddr);
-
-#if 0
-	netdev_warn(hw->wlandev->netdev, "dlbuf@0x%06lx len=%d to=%d\n", dlbufaddr,
-	       hw->bufinfo.len, hw->dltimeout);
-#endif
 	/* Calculations to determine how many fills of the dlbuffer to do
 	 * and how many USB wmemreq's to do for each fill.  At this point
 	 * in time, the dlbuffer size and the wmemreq size are the same.
@@ -2056,15 +2044,15 @@ int hfa384x_drvr_flashdl_write(hfa384x_t *hw, u32 daddr, void *buf, u32 len)
 		burnhi = HFA384x_ADDR_CMD_MKPAGE(burndaddr);
 
 		netdev_info(hw->wlandev->netdev, "Writing %d bytes to flash @0x%06x\n",
-		       burnlen, burndaddr);
+			    burnlen, burndaddr);
 
 		/* Set the download mode */
 		result = hfa384x_cmd_download(hw, HFA384x_PROGMODE_NV,
 					      burnlo, burnhi, burnlen);
 		if (result) {
-			netdev_err(hw->wlandev->netdev, "download(NV,lo=%x,hi=%x,len=%x) "
-			       "cmd failed, result=%d. Aborting d/l\n",
-			       burnlo, burnhi, burnlen, result);
+			netdev_err(hw->wlandev->netdev,
+				   "download(NV,lo=%x,hi=%x,len=%x) cmd failed, result=%d. Aborting d/l\n",
+				   burnlo, burnhi, burnlen, result);
 			goto exit_proc;
 		}
 
@@ -2095,9 +2083,8 @@ int hfa384x_drvr_flashdl_write(hfa384x_t *hw, u32 daddr, void *buf, u32 len)
 					      0, 0, 0);
 		if (result) {
 			netdev_err(hw->wlandev->netdev,
-			       "download(NVWRITE,lo=%x,hi=%x,len=%x) "
-			       "cmd failed, result=%d. Aborting d/l\n",
-			       burnlo, burnhi, burnlen, result);
+				   "download(NVWRITE,lo=%x,hi=%x,len=%x) cmd failed, result=%d. Aborting d/l\n",
+				   burnlo, burnhi, burnlen, result);
 			goto exit_proc;
 		}
 
@@ -2280,7 +2267,7 @@ int hfa384x_drvr_ramdl_enable(hfa384x_t *hw, u32 exeaddr)
 	for (i = 0; i < HFA384x_PORTID_MAX; i++) {
 		if (hw->port_enabled[i]) {
 			netdev_err(hw->wlandev->netdev,
-			       "Can't download with a macport enabled.\n");
+				   "Can't download with a macport enabled.\n");
 			return -EINVAL;
 		}
 	}
@@ -2352,7 +2339,8 @@ int hfa384x_drvr_ramdl_write(hfa384x_t *hw, u32 daddr, void *buf, u32 len)
 	if (hw->dlstate != HFA384x_DLSTATE_RAMENABLED)
 		return -EINVAL;
 
-	netdev_info(hw->wlandev->netdev, "Writing %d bytes to ram @0x%06x\n", len, daddr);
+	netdev_info(hw->wlandev->netdev, "Writing %d bytes to ram @0x%06x\n",
+		    len, daddr);
 
 	/* How many dowmem calls?  */
 	nwrites = len / HFA384x_USB_RWMEM_MAXLEN;
@@ -2450,7 +2438,8 @@ int hfa384x_drvr_readpda(hfa384x_t *hw, void *buf, unsigned int len)
 
 		if (result) {
 			netdev_warn(hw->wlandev->netdev,
-			       "Read from index %zd failed, continuing\n", i);
+				    "Read from index %zd failed, continuing\n",
+				    i);
 			continue;
 		}
 
@@ -2462,14 +2451,15 @@ int hfa384x_drvr_readpda(hfa384x_t *hw, void *buf, unsigned int len)
 			pdrcode = le16_to_cpu(pda[currpdr + 1]);
 			/* Test the record length */
 			if (pdrlen > HFA384x_PDR_LEN_MAX || pdrlen == 0) {
-				netdev_err(hw->wlandev->netdev, "pdrlen invalid=%d\n", pdrlen);
+				netdev_err(hw->wlandev->netdev,
+					   "pdrlen invalid=%d\n", pdrlen);
 				pdaok = 0;
 				break;
 			}
 			/* Test the code */
 			if (!hfa384x_isgood_pdrcode(pdrcode)) {
 				netdev_err(hw->wlandev->netdev, "pdrcode invalid=%d\n",
-				       pdrcode);
+					   pdrcode);
 				pdaok = 0;
 				break;
 			}
@@ -2485,13 +2475,13 @@ int hfa384x_drvr_readpda(hfa384x_t *hw, void *buf, unsigned int len)
 		}
 		if (pdaok) {
 			netdev_info(hw->wlandev->netdev,
-			       "PDA Read from 0x%08x in %s space.\n",
-			       pdaloc[i].cardaddr,
-			       pdaloc[i].auxctl == 0 ? "EXTDS" :
-			       pdaloc[i].auxctl == 1 ? "NV" :
-			       pdaloc[i].auxctl == 2 ? "PHY" :
-			       pdaloc[i].auxctl == 3 ? "ICSRAM" :
-			       "<bogus auxctl>");
+				    "PDA Read from 0x%08x in %s space.\n",
+				    pdaloc[i].cardaddr,
+				    pdaloc[i].auxctl == 0 ? "EXTDS" :
+				    pdaloc[i].auxctl == 1 ? "NV" :
+				    pdaloc[i].auxctl == 2 ? "PHY" :
+				    pdaloc[i].auxctl == 3 ? "ICSRAM" :
+				    "<bogus auxctl>");
 			break;
 		}
 	}
@@ -2586,7 +2576,8 @@ int hfa384x_drvr_start(hfa384x_t *hw)
 	result = submit_rx_urb(hw, GFP_KERNEL);
 	if (result != 0) {
 		netdev_err(hw->wlandev->netdev,
-		       "Fatal, failed to submit RX URB, result=%d\n", result);
+			   "Fatal, failed to submit RX URB, result=%d\n",
+			   result);
 		goto done;
 	}
 
@@ -2602,12 +2593,13 @@ int hfa384x_drvr_start(hfa384x_t *hw)
 	 */
 	result1 = hfa384x_cmd_initialize(hw);
 	msleep(1000);
-	result = result2 = hfa384x_cmd_initialize(hw);
+	result = hfa384x_cmd_initialize(hw);
+	result2 = result;
 	if (result1 != 0) {
 		if (result2 != 0) {
 			netdev_err(hw->wlandev->netdev,
-				"cmd_initialize() failed on two attempts, results %d and %d\n",
-				result1, result2);
+				   "cmd_initialize() failed on two attempts, results %d and %d\n",
+				   result1, result2);
 			usb_kill_urb(&hw->rx_urb);
 			goto done;
 		} else {
@@ -2617,9 +2609,9 @@ int hfa384x_drvr_start(hfa384x_t *hw)
 		}
 	} else if (result2 != 0) {
 		netdev_warn(hw->wlandev->netdev, "First cmd_initialize() succeeded, but second attempt failed (result=%d)\n",
-			result2);
+			    result2);
 		netdev_warn(hw->wlandev->netdev,
-		       "Most likely the card will be functional\n");
+			    "Most likely the card will be functional\n");
 		goto done;
 	}
 
@@ -2650,7 +2642,6 @@ done:
 ----------------------------------------------------------------*/
 int hfa384x_drvr_stop(hfa384x_t *hw)
 {
-	int result = 0;
 	int i;
 
 	might_sleep();
@@ -2675,7 +2666,7 @@ int hfa384x_drvr_stop(hfa384x_t *hw)
 	for (i = 0; i < HFA384x_NUMPORTS_MAX; i++)
 		hw->port_enabled[i] = 0;
 
-	return result;
+	return 0;
 }
 
 /*----------------------------------------------------------------
@@ -2784,7 +2775,8 @@ int hfa384x_drvr_txframe(hfa384x_t *hw, struct sk_buff *skb,
 	result = 1;
 	ret = submit_tx_urb(hw, &hw->tx_urb, GFP_ATOMIC);
 	if (ret != 0) {
-		netdev_err(hw->wlandev->netdev, "submit_tx_urb() failed, error=%d\n", ret);
+		netdev_err(hw->wlandev->netdev,
+			   "submit_tx_urb() failed, error=%d\n", ret);
 		result = 3;
 	}
 
@@ -2826,7 +2818,7 @@ void hfa384x_tx_timeout(wlandevice_t *wlandev)
 ----------------------------------------------------------------*/
 static void hfa384x_usbctlx_reaper_task(unsigned long data)
 {
-	hfa384x_t *hw = (hfa384x_t *) data;
+	hfa384x_t *hw = (hfa384x_t *)data;
 	struct list_head *entry;
 	struct list_head *temp;
 	unsigned long flags;
@@ -2845,7 +2837,6 @@ static void hfa384x_usbctlx_reaper_task(unsigned long data)
 	}
 
 	spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
-
 }
 
 /*----------------------------------------------------------------
@@ -2864,7 +2855,7 @@ static void hfa384x_usbctlx_reaper_task(unsigned long data)
 ----------------------------------------------------------------*/
 static void hfa384x_usbctlx_completion_task(unsigned long data)
 {
-	hfa384x_t *hw = (hfa384x_t *) data;
+	hfa384x_t *hw = (hfa384x_t *)data;
 	struct list_head *entry;
 	struct list_head *temp;
 	unsigned long flags;
@@ -3010,7 +3001,8 @@ static void unlocked_usbctlx_complete(hfa384x_t *hw, hfa384x_usbctlx_t *ctlx)
 
 	default:
 		netdev_err(hw->wlandev->netdev, "CTLX[%d] not in a terminating state(%s)\n",
-		       le16_to_cpu(ctlx->outbuf.type), ctlxstr(ctlx->state));
+			   le16_to_cpu(ctlx->outbuf.type),
+			   ctlxstr(ctlx->state));
 		break;
 	}			/* switch */
 }
@@ -3092,8 +3084,8 @@ static void hfa384x_usbctlxq_run(hfa384x_t *hw)
 			 * and schedule a reset ...
 			 */
 			netdev_warn(hw->wlandev->netdev,
-			       "%s tx pipe stalled: requesting reset\n",
-			       hw->wlandev->netdev->name);
+				    "%s tx pipe stalled: requesting reset\n",
+				    hw->wlandev->netdev->name);
 			list_move(&head->list, &hw->ctlxq.pending);
 			set_bit(WORK_TX_HALT, &hw->usb_flags);
 			schedule_work(&hw->usb_work);
@@ -3102,12 +3094,12 @@ static void hfa384x_usbctlxq_run(hfa384x_t *hw)
 
 		if (result == -ESHUTDOWN) {
 			netdev_warn(hw->wlandev->netdev, "%s urb shutdown!\n",
-			       hw->wlandev->netdev->name);
+				    hw->wlandev->netdev->name);
 			break;
 		}
 
 		netdev_err(hw->wlandev->netdev, "Failed to submit CTLX[%d]: error=%d\n",
-		       le16_to_cpu(head->outbuf.type), result);
+			   le16_to_cpu(head->outbuf.type), result);
 		unlocked_usbctlx_complete(hw, head);
 	}			/* while */
 
@@ -3135,7 +3127,7 @@ static void hfa384x_usbin_callback(struct urb *urb)
 {
 	wlandevice_t *wlandev = urb->context;
 	hfa384x_t *hw;
-	hfa384x_usbin_t *usbin = (hfa384x_usbin_t *) urb->transfer_buffer;
+	hfa384x_usbin_t *usbin = (hfa384x_usbin_t *)urb->transfer_buffer;
 	struct sk_buff *skb = NULL;
 	int result;
 	int urb_status;
@@ -3174,7 +3166,7 @@ static void hfa384x_usbin_callback(struct urb *urb)
 
 	case -EPIPE:
 		netdev_warn(hw->wlandev->netdev, "%s rx pipe stalled: requesting reset\n",
-		       wlandev->netdev->name);
+			    wlandev->netdev->name);
 		if (!test_and_set_bit(WORK_RX_HALT, &hw->usb_flags))
 			schedule_work(&hw->usb_work);
 		++(wlandev->linux_stats.rx_errors);
@@ -3225,8 +3217,8 @@ static void hfa384x_usbin_callback(struct urb *urb)
 
 		if (result != 0) {
 			netdev_err(hw->wlandev->netdev,
-			       "Fatal, failed to resubmit rx_urb. error=%d\n",
-			       result);
+				   "Fatal, failed to resubmit rx_urb. error=%d\n",
+				   result);
 		}
 	}
 
@@ -3361,9 +3353,9 @@ retry:
 		 */
 		if (ctlx->outbuf.type != intype) {
 			netdev_warn(hw->wlandev->netdev,
-			       "Expected IN[%d], received IN[%d] - ignored.\n",
-			       le16_to_cpu(ctlx->outbuf.type),
-			       le16_to_cpu(intype));
+				    "Expected IN[%d], received IN[%d] - ignored.\n",
+				    le16_to_cpu(ctlx->outbuf.type),
+				    le16_to_cpu(intype));
 			goto unlock;
 		}
 
@@ -3397,10 +3389,9 @@ retry:
 			 * Throw this CTLX away ...
 			 */
 			netdev_err(hw->wlandev->netdev,
-			       "Matched IN URB, CTLX[%d] in invalid state(%s)."
-			       " Discarded.\n",
-			       le16_to_cpu(ctlx->outbuf.type),
-			       ctlxstr(ctlx->state));
+				   "Matched IN URB, CTLX[%d] in invalid state(%s). Discarded.\n",
+				   le16_to_cpu(ctlx->outbuf.type),
+				   ctlxstr(ctlx->state));
 			if (unlocked_usbctlx_cancel_async(hw, ctlx) == 0)
 				run_queue = 1;
 			break;
@@ -3464,7 +3455,7 @@ static void hfa384x_usbin_txcompl(wlandevice_t *wlandev,
 ----------------------------------------------------------------*/
 static void hfa384x_usbin_rx(wlandevice_t *wlandev, struct sk_buff *skb)
 {
-	hfa384x_usbin_t *usbin = (hfa384x_usbin_t *) skb->data;
+	hfa384x_usbin_t *usbin = (hfa384x_usbin_t *)skb->data;
 	hfa384x_t *hw = wlandev->priv;
 	int hdrlen;
 	struct p80211_rxmeta *rxmeta;
@@ -3535,7 +3526,8 @@ static void hfa384x_usbin_rx(wlandevice_t *wlandev, struct sk_buff *skb)
 
 	default:
 		netdev_warn(hw->wlandev->netdev, "Received frame on unsupported port=%d\n",
-		       HFA384x_RXSTATUS_MACPORT_GET(usbin->rxfrm.desc.status));
+			    HFA384x_RXSTATUS_MACPORT_GET(
+				    usbin->rxfrm.desc.status));
 		goto done;
 		break;
 	}
@@ -3597,8 +3589,8 @@ static void hfa384x_int_rxmonitor(wlandevice_t *wlandev,
 	skb = dev_alloc_skb(skblen);
 	if (skb == NULL) {
 		netdev_err(hw->wlandev->netdev,
-		       "alloc_skb failed trying to allocate %d bytes\n",
-		       skblen);
+			   "alloc_skb failed trying to allocate %d bytes\n",
+			   skblen);
 		return;
 	}
 
@@ -3608,7 +3600,7 @@ static void hfa384x_int_rxmonitor(wlandevice_t *wlandev,
 		struct p80211_caphdr *caphdr;
 		/* The NEW header format! */
 		datap = skb_put(skb, sizeof(struct p80211_caphdr));
-		caphdr = (struct p80211_caphdr *) datap;
+		caphdr = (struct p80211_caphdr *)datap;
 
 		caphdr->version = htonl(P80211CAPTURE_VERSION);
 		caphdr->length = htonl(sizeof(struct p80211_caphdr));
@@ -3705,7 +3697,6 @@ static void hfa384x_usbout_callback(struct urb *urb)
 #endif
 
 	if (wlandev && wlandev->netdev) {
-
 		switch (urb->status) {
 		case 0:
 			hfa384x_usbout_tx(wlandev, usbout);
@@ -3715,8 +3706,8 @@ static void hfa384x_usbout_callback(struct urb *urb)
 			{
 				hfa384x_t *hw = wlandev->priv;
 				netdev_warn(hw->wlandev->netdev,
-				       "%s tx pipe stalled: requesting reset\n",
-				       wlandev->netdev->name);
+					    "%s tx pipe stalled: requesting reset\n",
+					    wlandev->netdev->name);
 				if (!test_and_set_bit
 				    (WORK_TX_HALT, &hw->usb_flags))
 					schedule_work(&hw->usb_work);
@@ -3731,8 +3722,8 @@ static void hfa384x_usbout_callback(struct urb *urb)
 				hfa384x_t *hw = wlandev->priv;
 
 				if (!test_and_set_bit
-				    (THROTTLE_TX, &hw->usb_flags)
-				    && !timer_pending(&hw->throttle)) {
+				    (THROTTLE_TX, &hw->usb_flags) &&
+				    !timer_pending(&hw->throttle)) {
 					mod_timer(&hw->throttle,
 						  jiffies + THROTTLE_JIFFIES);
 				}
@@ -3748,7 +3739,7 @@ static void hfa384x_usbout_callback(struct urb *urb)
 
 		default:
 			netdev_info(wlandev->netdev, "unknown urb->status=%d\n",
-			       urb->status);
+				    urb->status);
 			++(wlandev->linux_stats.tx_errors);
 			break;
 		}		/* switch */
@@ -3842,9 +3833,9 @@ retry:
 		default:
 			/* This is NOT a valid CTLX "success" state! */
 			netdev_err(hw->wlandev->netdev,
-				"Illegal CTLX[%d] success state(%s, %d) in OUT URB\n",
-				le16_to_cpu(ctlx->outbuf.type),
-				ctlxstr(ctlx->state), urb->status);
+				   "Illegal CTLX[%d] success state(%s, %d) in OUT URB\n",
+				   le16_to_cpu(ctlx->outbuf.type),
+				   ctlxstr(ctlx->state), urb->status);
 			break;
 		}		/* switch */
 	} else {
@@ -3852,8 +3843,8 @@ retry:
 		if ((urb->status == -EPIPE) &&
 		    !test_and_set_bit(WORK_TX_HALT, &hw->usb_flags)) {
 			netdev_warn(hw->wlandev->netdev,
-			       "%s tx pipe stalled: requesting reset\n",
-			       hw->wlandev->netdev->name);
+				    "%s tx pipe stalled: requesting reset\n",
+				    hw->wlandev->netdev->name);
 			schedule_work(&hw->usb_work);
 		}
 
@@ -3904,7 +3895,7 @@ delresp:
 ----------------------------------------------------------------*/
 static void hfa384x_usbctlx_reqtimerfn(unsigned long data)
 {
-	hfa384x_t *hw = (hfa384x_t *) data;
+	hfa384x_t *hw = (hfa384x_t *)data;
 	unsigned long flags;
 
 	spin_lock_irqsave(&hw->ctlxq.lock, flags);
@@ -3962,7 +3953,7 @@ static void hfa384x_usbctlx_reqtimerfn(unsigned long data)
 ----------------------------------------------------------------*/
 static void hfa384x_usbctlx_resptimerfn(unsigned long data)
 {
-	hfa384x_t *hw = (hfa384x_t *) data;
+	hfa384x_t *hw = (hfa384x_t *)data;
 	unsigned long flags;
 
 	spin_lock_irqsave(&hw->ctlxq.lock, flags);
@@ -4001,7 +3992,7 @@ static void hfa384x_usbctlx_resptimerfn(unsigned long data)
 ----------------------------------------------------------------*/
 static void hfa384x_usb_throttlefn(unsigned long data)
 {
-	hfa384x_t *hw = (hfa384x_t *) data;
+	hfa384x_t *hw = (hfa384x_t *)data;
 	unsigned long flags;
 
 	spin_lock_irqsave(&hw->ctlxq.lock, flags);
@@ -4139,13 +4130,13 @@ static int hfa384x_isgood_pdrcode(u16 pdrcode)
 	default:
 		if (pdrcode < 0x1000) {
 			/* code is OK, but we don't know exactly what it is */
-			pr_debug("Encountered unknown PDR#=0x%04x, "
-				 "assuming it's ok.\n", pdrcode);
+			pr_debug("Encountered unknown PDR#=0x%04x, assuming it's ok.\n",
+				 pdrcode);
 			return 1;
 		} else {
 			/* bad code */
-			pr_debug("Encountered unknown PDR#=0x%04x, "
-				 "(>=0x1000), assuming it's bad.\n", pdrcode);
+			pr_debug("Encountered unknown PDR#=0x%04x, (>=0x1000), assuming it's bad.\n",
+				 pdrcode);
 			return 0;
 		}
 		break;
diff --git a/drivers/staging/wlan-ng/p80211conv.c b/drivers/staging/wlan-ng/p80211conv.c
index 3df753b51e8..913676e1797 100644
--- a/drivers/staging/wlan-ng/p80211conv.c
+++ b/drivers/staging/wlan-ng/p80211conv.c
@@ -195,8 +195,8 @@ int skb_ether_to_p80211(wlandevice_t *wlandev, u32 ethconv,
 		memcpy(p80211_hdr->a3.a3, &e_hdr.saddr, ETH_ALEN);
 		break;
 	default:
-		printk(KERN_ERR
-		       "Error: Converting eth to wlan in unknown mode.\n");
+		netdev_err(wlandev->netdev,
+			   "Error: Converting eth to wlan in unknown mode.\n");
 		return 1;
 		break;
 	}
@@ -213,7 +213,7 @@ int skb_ether_to_p80211(wlandevice_t *wlandev, u32 ethconv,
 				  (wlandev->hostwep & HOSTWEP_DEFAULTKEY_MASK),
 				  p80211_wep->iv, p80211_wep->icv);
 		if (foo) {
-			printk(KERN_WARNING
+			netdev_warn(wlandev->netdev,
 			       "Host en-WEP failed, dropping frame (%d).\n",
 			       foo);
 			return 2;
@@ -310,7 +310,7 @@ int skb_p80211_to_ether(wlandevice_t *wlandev, u32 ethconv,
 	} else {
 		payload_offset = WLAN_HDR_A4_LEN;
 		if (payload_length < WLAN_HDR_A4_LEN - WLAN_HDR_A3_LEN) {
-			printk(KERN_ERR "A4 frame too short!\n");
+			netdev_err(netdev, "A4 frame too short!\n");
 			return 1;
 		}
 		payload_length -= (WLAN_HDR_A4_LEN - WLAN_HDR_A3_LEN);
@@ -322,8 +322,8 @@ int skb_p80211_to_ether(wlandevice_t *wlandev, u32 ethconv,
 	if ((wlandev->hostwep & HOSTWEP_PRIVACYINVOKED) && WLAN_GET_FC_ISWEP(fc)
 	    && (wlandev->hostwep & HOSTWEP_DECRYPT)) {
 		if (payload_length <= 8) {
-			printk(KERN_ERR "WEP frame too short (%u).\n",
-			       skb->len);
+			netdev_err(netdev,
+				   "WEP frame too short (%u).\n", skb->len);
 			return 1;
 		}
 		foo = wep_decrypt(wlandev, skb->data + payload_offset + 4,
@@ -367,7 +367,7 @@ int skb_p80211_to_ether(wlandevice_t *wlandev, u32 ethconv,
 		if (payload_length > (netdev->mtu + WLAN_ETHHDR_LEN)) {
 			/* A bogus length ethfrm has been encap'd. */
 			/* Is someone trying an oflow attack? */
-			printk(KERN_ERR "ENCAP frame too large (%d > %d)\n",
+			netdev_err(netdev, "ENCAP frame too large (%d > %d)\n",
 			       payload_length, netdev->mtu + WLAN_ETHHDR_LEN);
 			return 1;
 		}
@@ -396,7 +396,7 @@ int skb_p80211_to_ether(wlandevice_t *wlandev, u32 ethconv,
 		if (payload_length > netdev->mtu) {
 			/* A bogus length ethfrm has been sent. */
 			/* Is someone trying an oflow attack? */
-			printk(KERN_ERR "SNAP frame too large (%d > %d)\n",
+			netdev_err(netdev, "SNAP frame too large (%d > %d)\n",
 			       payload_length, netdev->mtu);
 			return 1;
 		}
@@ -428,7 +428,7 @@ int skb_p80211_to_ether(wlandevice_t *wlandev, u32 ethconv,
 			> netdev->mtu) {
 			/* A bogus length ethfrm has been sent. */
 			/* Is someone trying an oflow attack? */
-			printk(KERN_ERR "DIXII frame too large (%ld > %d)\n",
+			netdev_err(netdev, "DIXII frame too large (%ld > %d)\n",
 			       (long int)(payload_length -
 					sizeof(struct wlan_llc) -
 					sizeof(struct wlan_snap)), netdev->mtu);
@@ -463,7 +463,7 @@ int skb_p80211_to_ether(wlandevice_t *wlandev, u32 ethconv,
 		if (payload_length > netdev->mtu) {
 			/* A bogus length ethfrm has been sent. */
 			/* Is someone trying an oflow attack? */
-			printk(KERN_ERR "OTHER frame too large (%d > %d)\n",
+			netdev_err(netdev, "OTHER frame too large (%d > %d)\n",
 			       payload_length, netdev->mtu);
 			return 1;
 		}
@@ -603,8 +603,8 @@ int p80211skb_rxmeta_attach(struct wlandevice *wlandev, struct sk_buff *skb)
 
 	/* If these already have metadata, we error out! */
 	if (P80211SKB_RXMETA(skb) != NULL) {
-		printk(KERN_ERR "%s: RXmeta already attached!\n",
-		       wlandev->name);
+		netdev_err(wlandev->netdev,
+			   "%s: RXmeta already attached!\n", wlandev->name);
 		result = 0;
 		goto exit;
 	}
@@ -613,8 +613,8 @@ int p80211skb_rxmeta_attach(struct wlandevice *wlandev, struct sk_buff *skb)
 	rxmeta = kzalloc(sizeof(struct p80211_rxmeta), GFP_ATOMIC);
 
 	if (rxmeta == NULL) {
-		printk(KERN_ERR "%s: Failed to allocate rxmeta.\n",
-		       wlandev->name);
+		netdev_err(wlandev->netdev,
+			   "%s: Failed to allocate rxmeta.\n", wlandev->name);
 		result = 1;
 		goto exit;
 	}
@@ -656,6 +656,7 @@ void p80211skb_free(struct wlandevice *wlandev, struct sk_buff *skb)
 	if (meta && meta->rx)
 		p80211skb_rxmeta_detach(skb);
 	else
-		printk(KERN_ERR "Freeing an skb (%p) w/ no frmmeta.\n", skb);
+		netdev_err(wlandev->netdev,
+			   "Freeing an skb (%p) w/ no frmmeta.\n", skb);
 	dev_kfree_skb(skb);
 }
diff --git a/drivers/staging/wlan-ng/p80211netdev.c b/drivers/staging/wlan-ng/p80211netdev.c
index e3ae8024d3a..00b186c5972 100644
--- a/drivers/staging/wlan-ng/p80211netdev.c
+++ b/drivers/staging/wlan-ng/p80211netdev.c
@@ -90,9 +90,6 @@
 
 #include "cfg80211.c"
 
-/* Support functions */
-static void p80211netdev_rx_bh(unsigned long arg);
-
 /* netdevice method functions */
 static int p80211knetdev_init(netdevice_t *netdev);
 static struct net_device_stats *p80211knetdev_get_stats(netdevice_t *netdev);
@@ -243,25 +240,59 @@ void p80211netdev_rx(wlandevice_t *wlandev, struct sk_buff *skb)
 	tasklet_schedule(&wlandev->rx_bh);
 }
 
-/*----------------------------------------------------------------
-* p80211netdev_rx_bh
-*
-* Deferred processing of all received frames.
-*
-* Arguments:
-*	wlandev		WLAN network device structure
-*	skb		skbuff containing a full 802.11 frame.
-* Returns:
-*	nothing
-* Side effects:
-*
-----------------------------------------------------------------*/
+#define CONV_TO_ETHER_SKIPPED	0x01
+#define CONV_TO_ETHER_FAILED	0x02
+
+/**
+ * p80211_convert_to_ether - conversion from 802.11 frame to ethernet frame
+ * @wlandev: pointer to WLAN device
+ * @skb: pointer to socket buffer
+ *
+ * Returns: 0 if conversion succeeded
+ *	    CONV_TO_ETHER_FAILED if conversion failed
+ *	    CONV_TO_ETHER_SKIPPED if frame is ignored
+ */
+static int p80211_convert_to_ether(wlandevice_t *wlandev, struct sk_buff *skb)
+{
+	struct p80211_hdr_a3 *hdr;
+
+	hdr = (struct p80211_hdr_a3 *) skb->data;
+	if (p80211_rx_typedrop(wlandev, hdr->fc))
+		return CONV_TO_ETHER_SKIPPED;
+
+	/* perform mcast filtering: allow my local address through but reject
+	 * anything else that isn't multicast
+	 */
+	if (wlandev->netdev->flags & IFF_ALLMULTI) {
+		if (!ether_addr_equal_unaligned(wlandev->netdev->dev_addr,
+						hdr->a1)) {
+			if (!is_multicast_ether_addr(hdr->a1))
+				return CONV_TO_ETHER_SKIPPED;
+		}
+	}
+
+	if (skb_p80211_to_ether(wlandev, wlandev->ethconv, skb) == 0) {
+		skb->dev->last_rx = jiffies;
+		wlandev->linux_stats.rx_packets++;
+		wlandev->linux_stats.rx_bytes += skb->len;
+		netif_rx_ni(skb);
+		return 0;
+	}
+
+	netdev_dbg(wlandev->netdev, "p80211_convert_to_ether failed.\n");
+	return CONV_TO_ETHER_FAILED;
+}
+
+/**
+ * p80211netdev_rx_bh - deferred processing of all received frames
+ *
+ * @arg: pointer to WLAN network device structure (cast to unsigned long)
+ */
 static void p80211netdev_rx_bh(unsigned long arg)
 {
 	wlandevice_t *wlandev = (wlandevice_t *) arg;
 	struct sk_buff *skb = NULL;
 	netdevice_t *dev = wlandev->netdev;
-	struct p80211_hdr_a3 *hdr;
 
 	/* Let's empty our our queue */
 	while ((skb = skb_dequeue(&wlandev->nsd_rxq))) {
@@ -284,37 +315,8 @@ static void p80211netdev_rx_bh(unsigned long arg)
 				netif_rx_ni(skb);
 				continue;
 			} else {
-				hdr = (struct p80211_hdr_a3 *) skb->data;
-				if (p80211_rx_typedrop(wlandev, hdr->fc)) {
-					dev_kfree_skb(skb);
-					continue;
-				}
-
-				/* perform mcast filtering */
-				if (wlandev->netdev->flags & IFF_ALLMULTI) {
-					/* allow my local address through */
-					if (memcmp
-					    (hdr->a1, wlandev->netdev->dev_addr,
-					     ETH_ALEN) != 0) {
-						/* but reject anything else that
-						   isn't multicast */
-						if (!(hdr->a1[0] & 0x01)) {
-							dev_kfree_skb(skb);
-							continue;
-						}
-					}
-				}
-
-				if (skb_p80211_to_ether
-				    (wlandev, wlandev->ethconv, skb) == 0) {
-					skb->dev->last_rx = jiffies;
-					wlandev->linux_stats.rx_packets++;
-					wlandev->linux_stats.rx_bytes +=
-					    skb->len;
-					netif_rx_ni(skb);
+				if (!p80211_convert_to_ether(wlandev, skb))
 					continue;
-				}
-				pr_debug("p80211_to_ether failed.\n");
 			}
 		}
 		dev_kfree_skb(skb);
@@ -363,7 +365,7 @@ static int p80211knetdev_hard_start_xmit(struct sk_buff *skb,
 	memset(&p80211_wep, 0, sizeof(struct p80211_metawep));
 
 	if (netif_queue_stopped(netdev)) {
-		pr_debug("called when queue stopped.\n");
+		netdev_dbg(netdev, "called when queue stopped.\n");
 		result = 1;
 		goto failed;
 	}
@@ -383,8 +385,7 @@ static int p80211knetdev_hard_start_xmit(struct sk_buff *skb,
 		 */
 		if (skb->protocol != ETH_P_80211_RAW) {
 			netif_start_queue(wlandev->netdev);
-			printk(KERN_NOTICE
-			       "Tx attempt prior to association, frame dropped.\n");
+			netdev_notice(netdev, "Tx attempt prior to association, frame dropped.\n");
 			wlandev->linux_stats.tx_dropped++;
 			result = 0;
 			goto failed;
@@ -406,8 +407,8 @@ static int p80211knetdev_hard_start_xmit(struct sk_buff *skb,
 		    (wlandev, wlandev->ethconv, skb, &p80211_hdr,
 		     &p80211_wep) != 0) {
 			/* convert failed */
-			pr_debug("ether_to_80211(%d) failed.\n",
-				 wlandev->ethconv);
+			netdev_dbg(netdev, "ether_to_80211(%d) failed.\n",
+				   wlandev->ethconv);
 			result = 1;
 			goto failed;
 		}
@@ -432,17 +433,17 @@ static int p80211knetdev_hard_start_xmit(struct sk_buff *skb,
 		result = NETDEV_TX_OK;
 	} else if (txresult == 1) {
 		/* success, no more avail */
-		pr_debug("txframe success, no more bufs\n");
+		netdev_dbg(netdev, "txframe success, no more bufs\n");
 		/* netdev->tbusy = 1;  don't set here, irqhdlr */
 		/*   may have already cleared it */
 		result = NETDEV_TX_OK;
 	} else if (txresult == 2) {
 		/* alloc failure, drop frame */
-		pr_debug("txframe returned alloc_fail\n");
+		netdev_dbg(netdev, "txframe returned alloc_fail\n");
 		result = NETDEV_TX_BUSY;
 	} else {
 		/* buffer full or queue busy, drop frame. */
-		pr_debug("txframe returned full or busy\n");
+		netdev_dbg(netdev, "txframe returned full or busy\n");
 		result = NETDEV_TX_BUSY;
 	}
 
@@ -562,7 +563,7 @@ static int p80211knetdev_do_ioctl(netdevice_t *dev, struct ifreq *ifr, int cmd)
 	wlandevice_t *wlandev = dev->ml_priv;
 	u8 *msgbuf;
 
-	pr_debug("rx'd ioctl, cmd=%d, len=%d\n", cmd, req->len);
+	netdev_dbg(dev, "rx'd ioctl, cmd=%d, len=%d\n", cmd, req->len);
 
 #ifdef SIOCETHTOOL
 	if (cmd == SIOCETHTOOL) {
@@ -683,8 +684,7 @@ static int p80211knetdev_set_mac_address(netdevice_t *dev, void *addr)
 	 * change the netdev address
 	 */
 	if (result != 0 || resultcode->data != P80211ENUM_resultcode_success) {
-		printk(KERN_ERR
-		       "Low-level driver failed dot11req_mibset(dot11MACAddress).\n");
+		netdev_err(dev, "Low-level driver failed dot11req_mibset(dot11MACAddress).\n");
 		result = -EADDRNOTAVAIL;
 	} else {
 		/* everything's ok, change the addr in netdev */
@@ -763,7 +763,7 @@ int wlan_setup(wlandevice_t *wlandev, struct device *physdev)
 	/* Allocate and initialize the wiphy struct */
 	wiphy = wlan_create_wiphy(physdev, wlandev);
 	if (wiphy == NULL) {
-		printk(KERN_ERR "Failed to alloc wiphy.\n");
+		dev_err(physdev, "Failed to alloc wiphy.\n");
 		return 1;
 	}
 
@@ -771,7 +771,7 @@ int wlan_setup(wlandevice_t *wlandev, struct device *physdev)
 	netdev = alloc_netdev(sizeof(struct wireless_dev), "wlan%d",
 				ether_setup);
 	if (netdev == NULL) {
-		printk(KERN_ERR "Failed to alloc netdev.\n");
+		dev_err(physdev, "Failed to alloc netdev.\n");
 		wlan_free_wiphy(wiphy);
 		result = 1;
 	} else {
@@ -947,7 +947,8 @@ static int p80211_rx_typedrop(wlandevice_t *wlandev, u16 fc)
 	ftype = WLAN_GET_FC_FTYPE(fc);
 	fstype = WLAN_GET_FC_FSTYPE(fc);
 #if 0
-	pr_debug("rx_typedrop : ftype=%d fstype=%d.\n", ftype, fstype);
+	netdev_dbg(wlandev->netdev, "rx_typedrop : ftype=%d fstype=%d.\n",
+		   ftype, fstype);
 #endif
 	switch (ftype) {
 	case WLAN_FTYPE_MGMT:
@@ -956,7 +957,7 @@ static int p80211_rx_typedrop(wlandevice_t *wlandev, u16 fc)
 			drop = 1;
 			break;
 		}
-		pr_debug("rx'd mgmt:\n");
+		netdev_dbg(wlandev->netdev, "rx'd mgmt:\n");
 		wlandev->rx.mgmt++;
 		switch (fstype) {
 		case WLAN_FSTYPE_ASSOCREQ:
@@ -1018,7 +1019,7 @@ static int p80211_rx_typedrop(wlandevice_t *wlandev, u16 fc)
 			drop = 1;
 			break;
 		}
-		pr_debug("rx'd ctl:\n");
+		netdev_dbg(wlandev->netdev, "rx'd ctl:\n");
 		wlandev->rx.ctl++;
 		switch (fstype) {
 		case WLAN_FSTYPE_PSPOLL:
@@ -1070,19 +1071,19 @@ static int p80211_rx_typedrop(wlandevice_t *wlandev, u16 fc)
 			wlandev->rx.data__cfack_cfpoll++;
 			break;
 		case WLAN_FSTYPE_NULL:
-			pr_debug("rx'd data:null\n");
+			netdev_dbg(wlandev->netdev, "rx'd data:null\n");
 			wlandev->rx.null++;
 			break;
 		case WLAN_FSTYPE_CFACK:
-			pr_debug("rx'd data:cfack\n");
+			netdev_dbg(wlandev->netdev, "rx'd data:cfack\n");
 			wlandev->rx.cfack++;
 			break;
 		case WLAN_FSTYPE_CFPOLL:
-			pr_debug("rx'd data:cfpoll\n");
+			netdev_dbg(wlandev->netdev, "rx'd data:cfpoll\n");
 			wlandev->rx.cfpoll++;
 			break;
 		case WLAN_FSTYPE_CFACK_CFPOLL:
-			pr_debug("rx'd data:cfack_cfpoll\n");
+			netdev_dbg(wlandev->netdev, "rx'd data:cfack_cfpoll\n");
 			wlandev->rx.cfack_cfpoll++;
 			break;
 		default:
@@ -1103,8 +1104,8 @@ static void p80211knetdev_tx_timeout(netdevice_t *netdev)
 	if (wlandev->tx_timeout) {
 		wlandev->tx_timeout(wlandev);
 	} else {
-		printk(KERN_WARNING "Implement tx_timeout for %s\n",
-		       wlandev->nsdname);
+		netdev_warn(netdev, "Implement tx_timeout for %s\n",
+			    wlandev->nsdname);
 		netif_wake_queue(wlandev->netdev);
 	}
 }
diff --git a/drivers/staging/wlan-ng/p80211req.c b/drivers/staging/wlan-ng/p80211req.c
index cdfd808d685..7221379c974 100644
--- a/drivers/staging/wlan-ng/p80211req.c
+++ b/drivers/staging/wlan-ng/p80211req.c
@@ -95,7 +95,6 @@ static void p80211req_mibset_mibget(wlandevice_t *wlandev,
 ----------------------------------------------------------------*/
 int p80211req_dorequest(wlandevice_t *wlandev, u8 *msgbuf)
 {
-	int result = 0;
 	struct p80211msg *msg = (struct p80211msg *) msgbuf;
 
 	/* Check to make sure the MSD is running */
@@ -109,9 +108,9 @@ int p80211req_dorequest(wlandevice_t *wlandev, u8 *msgbuf)
 	/* Check Permissions */
 	if (!capable(CAP_NET_ADMIN) &&
 	(msg->msgcode != DIDmsg_dot11req_mibget)) {
-		printk(KERN_ERR
-		       "%s: only dot11req_mibget allowed for non-root.\n",
-		       wlandev->name);
+		netdev_err(wlandev->netdev,
+			   "%s: only dot11req_mibget allowed for non-root.\n",
+			   wlandev->name);
 		return -EPERM;
 	}
 
@@ -129,7 +128,7 @@ int p80211req_dorequest(wlandevice_t *wlandev, u8 *msgbuf)
 		wlandev->mlmerequest(wlandev, msg);
 
 	clear_bit(1, &(wlandev->request_pending));
-	return result;	/* if result==0, msg->status still may contain an err */
+	return 0;	/* if result==0, msg->status still may contain an err */
 }
 
 /*----------------------------------------------------------------
diff --git a/drivers/staging/wlan-ng/prism2fw.c b/drivers/staging/wlan-ng/prism2fw.c
index 2b0c23587df..f7870355c69 100644
--- a/drivers/staging/wlan-ng/prism2fw.c
+++ b/drivers/staging/wlan-ng/prism2fw.c
@@ -205,19 +205,20 @@ static int prism2_fwtry(struct usb_device *udev, wlandevice_t *wlandev)
 {
 	const struct firmware *fw_entry = NULL;
 
-	printk(KERN_INFO "prism2_usb: Checking for firmware %s\n",
+	netdev_info(wlandev->netdev, "prism2_usb: Checking for firmware %s\n",
 	       PRISM2_USB_FWFILE);
 	if (request_ihex_firmware(&fw_entry,
 				  PRISM2_USB_FWFILE, &udev->dev) != 0) {
-		printk(KERN_INFO
+		netdev_info(wlandev->netdev,
 		       "prism2_usb: Firmware not available, but not essential\n");
-		printk(KERN_INFO
+		netdev_info(wlandev->netdev,
 		       "prism2_usb: can continue to use card anyway.\n");
 		return 1;
 	}
 
-	printk(KERN_INFO "prism2_usb: %s will be processed, size %zu\n",
-	       PRISM2_USB_FWFILE, fw_entry->size);
+	netdev_info(wlandev->netdev,
+		    "prism2_usb: %s will be processed, size %zu\n",
+		    PRISM2_USB_FWFILE, fw_entry->size);
 	prism2_fwapply((const struct ihex_binrec *)fw_entry->data, wlandev);
 
 	release_firmware(fw_entry);
@@ -275,7 +276,7 @@ static int prism2_fwapply(const struct ihex_binrec *rfptr, wlandevice_t *wlandev
 
 	/* Build the PDA we're going to use. */
 	if (read_cardpda(&pda, wlandev)) {
-		printk(KERN_ERR "load_cardpda failed, exiting.\n");
+		netdev_err(wlandev->netdev, "load_cardpda failed, exiting.\n");
 		return 1;
 	}
 
@@ -299,7 +300,7 @@ static int prism2_fwapply(const struct ihex_binrec *rfptr, wlandevice_t *wlandev
 	/* DIDmsg_dot11req_mibget */
 	prism2mgmt_mibset_mibget(wlandev, &getmsg);
 	if (getmsg.resultcode.data != P80211ENUM_resultcode_success)
-		printk(KERN_ERR "Couldn't fetch PRI-SUP info\n");
+		netdev_err(wlandev->netdev, "Couldn't fetch PRI-SUP info\n");
 
 	/* Already in host order */
 	priid.role = *data++;
@@ -311,19 +312,21 @@ static int prism2_fwapply(const struct ihex_binrec *rfptr, wlandevice_t *wlandev
 	/* Read the S3 file */
 	result = read_fwfile(rfptr);
 	if (result) {
-		printk(KERN_ERR "Failed to read the data exiting.\n");
+		netdev_err(wlandev->netdev,
+			   "Failed to read the data exiting.\n");
 		return 1;
 	}
 
 	result = validate_identity();
 
 	if (result) {
-		printk(KERN_ERR "Incompatible firmware image.\n");
+		netdev_err(wlandev->netdev, "Incompatible firmware image.\n");
 		return 1;
 	}
 
 	if (startaddr == 0x00000000) {
-		printk(KERN_ERR "Can't RAM download a Flash image!\n");
+		netdev_err(wlandev->netdev,
+			   "Can't RAM download a Flash image!\n");
 		return 1;
 	}
 
@@ -333,20 +336,20 @@ static int prism2_fwapply(const struct ihex_binrec *rfptr, wlandevice_t *wlandev
 	/* Do any plugging */
 	result = plugimage(fchunk, nfchunks, s3plug, ns3plug, &pda);
 	if (result) {
-		printk(KERN_ERR "Failed to plug data.\n");
+		netdev_err(wlandev->netdev, "Failed to plug data.\n");
 		return 1;
 	}
 
 	/* Insert any CRCs */
 	if (crcimage(fchunk, nfchunks, s3crc, ns3crc)) {
-		printk(KERN_ERR "Failed to insert all CRCs\n");
+		netdev_err(wlandev->netdev, "Failed to insert all CRCs\n");
 		return 1;
 	}
 
 	/* Write the image */
 	result = writeimage(wlandev, fchunk, nfchunks);
 	if (result) {
-		printk(KERN_ERR "Failed to ramwrite image data.\n");
+		netdev_err(wlandev->netdev, "Failed to ramwrite image data.\n");
 		return 1;
 	}
 
@@ -354,7 +357,7 @@ static int prism2_fwapply(const struct ihex_binrec *rfptr, wlandevice_t *wlandev
 	free_chunks(fchunk, &nfchunks);
 	free_srecs();
 
-	printk(KERN_INFO "prism2_usb: firmware loading finished.\n");
+	netdev_info(wlandev->netdev, "prism2_usb: firmware loading finished.\n");
 
 	return result;
 }
@@ -410,8 +413,7 @@ static int crcimage(struct imgchunk *fchunk, unsigned int nfchunks,
 				break;
 		}
 		if (c >= nfchunks) {
-			printk(KERN_ERR
-			       "Failed to find chunk for "
+			pr_err("Failed to find chunk for "
 			       "crcrec[%d], addr=0x%06x len=%d , "
 			       "aborting crc.\n",
 			       i, s3crc[i].addr, s3crc[i].len);
@@ -537,8 +539,7 @@ static int mkimage(struct imgchunk *clist, unsigned int *ccnt)
 	for (i = 0; i < *ccnt; i++) {
 		clist[i].data = kzalloc(clist[i].len, GFP_KERNEL);
 		if (clist[i].data == NULL) {
-			printk(KERN_ERR
-			       "failed to allocate image space, exitting.\n");
+			pr_err("failed to allocate image space, exitting.\n");
 			return 1;
 		}
 		pr_debug("chunk[%d]: addr=0x%06x len=%d\n",
@@ -556,8 +557,7 @@ static int mkimage(struct imgchunk *clist, unsigned int *ccnt)
 				break;
 		}
 		if (((unsigned int)j) >= (*ccnt)) {
-			printk(KERN_ERR
-			       "s3rec(a=0x%06x,l=%d), no chunk match, exiting.\n",
+			pr_err("s3rec(a=0x%06x,l=%d), no chunk match, exiting.\n",
 			       s3start, s3data[i].len);
 			return 1;
 		}
@@ -627,8 +627,7 @@ static int mkpdrlist(struct pda *pda)
 
 	}
 	if (curroff >= (HFA384x_PDA_LEN_MAX / 2)) {
-		printk(KERN_ERR
-		       "no end record found or invalid lengths in "
+		pr_err("no end record found or invalid lengths in "
 		       "PDR data, exiting. %x %d\n", curroff, pda->nrec);
 		return 1;
 	}
@@ -685,8 +684,7 @@ static int plugimage(struct imgchunk *fchunk, unsigned int nfchunks,
 			j = -1;
 		}
 		if (j >= pda->nrec && j != -1) { /*  if no matching PDR, fail */
-			printk(KERN_WARNING
-			       "warning: Failed to find PDR for "
+			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
@@ -704,8 +702,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)) {
-			printk(KERN_ERR
-			       "error: Plug vs. PDR len mismatch for "
+			pr_err("error: Plug vs. PDR len mismatch for "
 			       "plugrec 0x%04x, abort plugging.\n",
 			       s3plug[i].itemcode);
 			result = 1;
@@ -720,8 +717,7 @@ static int plugimage(struct imgchunk *fchunk, unsigned int nfchunks,
 				break;
 		}
 		if (c >= nfchunks) {
-			printk(KERN_ERR
-			       "error: Failed to find image chunk for "
+			pr_err("error: Failed to find image chunk for "
 			       "plugrec 0x%04x.\n", s3plug[i].itemcode);
 			result = 1;
 			continue;
@@ -898,7 +894,7 @@ static int read_fwfile(const struct ihex_binrec *record)
 
 			ns3plug++;
 			if (ns3plug == S3PLUG_MAX) {
-				printk(KERN_ERR "S3 plugrec limit reached - aborting\n");
+				pr_err("S3 plugrec limit reached - aborting\n");
 				return 1;
 			}
 			break;
@@ -915,7 +911,7 @@ static int read_fwfile(const struct ihex_binrec *record)
 				      s3crc[ns3crc].dowrite);
 			ns3crc++;
 			if (ns3crc == S3CRC_MAX) {
-				printk(KERN_ERR "S3 crcrec limit reached - aborting\n");
+				pr_err("S3 crcrec limit reached - aborting\n");
 				return 1;
 			}
 			break;
@@ -929,7 +925,7 @@ static int read_fwfile(const struct ihex_binrec *record)
 				      s3info[ns3info].len,
 				      s3info[ns3info].type);
 			if (((s3info[ns3info].len - 1) * sizeof(u16)) > sizeof(s3info[ns3info].info)) {
-				printk(KERN_ERR " S3 inforec length too long - aborting\n");
+				pr_err("S3 inforec length too long - aborting\n");
 				return 1;
 			}
 
@@ -943,7 +939,7 @@ static int read_fwfile(const struct ihex_binrec *record)
 
 			ns3info++;
 			if (ns3info == S3INFO_MAX) {
-				printk(KERN_ERR "S3 inforec limit reached - aborting\n");
+				pr_err("S3 inforec limit reached - aborting\n");
 				return 1;
 			}
 			break;
@@ -953,7 +949,7 @@ static int read_fwfile(const struct ihex_binrec *record)
 			s3data[ns3data].data = (uint8_t *) record->data;
 			ns3data++;
 			if (ns3data == S3DATA_MAX) {
-				printk(KERN_ERR "S3 datarec limit reached - aborting\n");
+				pr_err("S3 datarec limit reached - aborting\n");
 				return 1;
 			}
 			break;
@@ -997,9 +993,9 @@ static int writeimage(wlandevice_t *wlandev, struct imgchunk *fchunk,
 	if (!rstmsg || !rwrmsg) {
 		kfree(rstmsg);
 		kfree(rwrmsg);
-		printk(KERN_ERR
-		       "writeimage: no memory for firmware download, "
-		       "aborting download\n");
+		netdev_err(wlandev->netdev,
+			   "writeimage: no memory for firmware download, "
+			   "aborting download\n");
 		return -ENOMEM;
 	}
 
@@ -1042,16 +1038,16 @@ static int writeimage(wlandevice_t *wlandev, struct imgchunk *fchunk,
 
 	result = prism2mgmt_ramdl_state(wlandev, rstmsg);
 	if (result) {
-		printk(KERN_ERR
-		       "writeimage state enable failed w/ result=%d, "
-		       "aborting download\n", result);
+		netdev_err(wlandev->netdev,
+			   "writeimage state enable failed w/ result=%d, "
+			   "aborting download\n", result);
 		goto free_result;
 	}
 	resultcode = rstmsg->resultcode.data;
 	if (resultcode != P80211ENUM_resultcode_success) {
-		printk(KERN_ERR
-		       "writeimage()->xxxdl_state msg indicates failure, "
-		       "w/ resultcode=%d, aborting download.\n", resultcode);
+		netdev_err(wlandev->netdev,
+			   "writeimage()->xxxdl_state msg indicates failure, "
+			   "w/ resultcode=%d, aborting download.\n", resultcode);
 		result = 1;
 		goto free_result;
 	}
@@ -1085,15 +1081,14 @@ static int writeimage(wlandevice_t *wlandev, struct imgchunk *fchunk,
 
 			/* Check the results */
 			if (result) {
-				printk(KERN_ERR
-				       "writeimage chunk write failed w/ result=%d, "
-				       "aborting download\n", result);
+				netdev_err(wlandev->netdev,
+					   "writeimage chunk write failed w/ "
+					   "result=%d, aborting download\n", result);
 				goto free_result;
 			}
 			resultcode = rstmsg->resultcode.data;
 			if (resultcode != P80211ENUM_resultcode_success) {
-				printk(KERN_ERR
-				       "writeimage()->xxxdl_write msg indicates failure, "
+				pr_err("writeimage()->xxxdl_write msg indicates failure, "
 				       "w/ resultcode=%d, aborting download.\n",
 				       resultcode);
 				result = 1;
@@ -1110,16 +1105,16 @@ static int writeimage(wlandevice_t *wlandev, struct imgchunk *fchunk,
 
 	result = prism2mgmt_ramdl_state(wlandev, rstmsg);
 	if (result) {
-		printk(KERN_ERR
-		       "writeimage state disable failed w/ result=%d, "
-		       "aborting download\n", result);
+		netdev_err(wlandev->netdev,
+			   "writeimage state disable failed w/ result=%d, "
+			   "aborting download\n", result);
 		goto free_result;
 	}
 	resultcode = rstmsg->resultcode.data;
 	if (resultcode != P80211ENUM_resultcode_success) {
-		printk(KERN_ERR
-		       "writeimage()->xxxdl_state msg indicates failure, "
-		       "w/ resultcode=%d, aborting download.\n", resultcode);
+		netdev_err(wlandev->netdev,
+			   "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 a9909f6b000..d110b362c3b 100644
--- a/drivers/staging/wlan-ng/prism2mgmt.c
+++ b/drivers/staging/wlan-ng/prism2mgmt.c
@@ -129,8 +129,8 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 				     hw->ident_sta_fw.minor,
 				     hw->ident_sta_fw.variant) <
 	    HFA384x_FIRMWARE_VERSION(1, 3, 2)) {
-		printk(KERN_ERR
-		       "HostScan not supported with current firmware (<1.3.2).\n");
+		netdev_err(wlandev->netdev,
+			   "HostScan not supported with current firmware (<1.3.2).\n");
 		result = 1;
 		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
 		goto exit;
@@ -143,8 +143,8 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 					  HFA384x_RID_CNFROAMINGMODE,
 					  &roamingmode);
 	if (result) {
-		printk(KERN_ERR "getconfig(ROAMMODE) failed. result=%d\n",
-		       result);
+		netdev_err(wlandev->netdev,
+			   "getconfig(ROAMMODE) failed. result=%d\n", result);
 		msg->resultcode.data =
 		    P80211ENUM_resultcode_implementation_failure;
 		goto exit;
@@ -155,8 +155,8 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 					  HFA384x_RID_CNFROAMINGMODE,
 					  HFA384x_ROAMMODE_HOSTSCAN_HOSTROAM);
 	if (result) {
-		printk(KERN_ERR "setconfig(ROAMINGMODE) failed. result=%d\n",
-		       result);
+		netdev_err(wlandev->netdev,
+			   "setconfig(ROAMINGMODE) failed. result=%d\n", result);
 		msg->resultcode.data =
 		    P80211ENUM_resultcode_implementation_failure;
 		goto exit;
@@ -176,8 +176,9 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 		    hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFPASSIVESCANCTRL,
 					     word);
 		if (result) {
-			printk(KERN_WARNING "Passive scan not supported with "
-			       "current firmware.  (<1.5.1)\n");
+			netdev_warn(wlandev->netdev,
+				    "Passive scan not supported with "
+				    "current firmware.  (<1.5.1)\n");
 		}
 	}
 
@@ -203,8 +204,8 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 	/* Enable the MAC port if it's not already enabled  */
 	result = hfa384x_drvr_getconfig16(hw, HFA384x_RID_PORTSTATUS, &word);
 	if (result) {
-		printk(KERN_ERR "getconfig(PORTSTATUS) failed. "
-		       "result=%d\n", result);
+		netdev_err(wlandev->netdev,
+			   "getconfig(PORTSTATUS) failed. result=%d\n", result);
 		msg->resultcode.data =
 		    P80211ENUM_resultcode_implementation_failure;
 		goto exit;
@@ -216,9 +217,9 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 					HFA384x_RID_CNFROAMINGMODE,
 					HFA384x_ROAMMODE_HOSTSCAN_HOSTROAM);
 		if (result) {
-			printk(KERN_ERR
-			       "setconfig(ROAMINGMODE) failed. result=%d\n",
-			       result);
+			netdev_err(wlandev->netdev,
+				   "setconfig(ROAMINGMODE) failed. result=%d\n",
+				   result);
 			msg->resultcode.data =
 			    P80211ENUM_resultcode_implementation_failure;
 			goto exit;
@@ -232,7 +233,7 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 						wordbuf,
 						HFA384x_RID_CNFOWNSSID_LEN);
 		if (result) {
-			printk(KERN_ERR "Failed to set OwnSSID.\n");
+			netdev_err(wlandev->netdev, "Failed to set OwnSSID.\n");
 			msg->resultcode.data =
 			    P80211ENUM_resultcode_implementation_failure;
 			goto exit;
@@ -241,7 +242,8 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 						wordbuf,
 						HFA384x_RID_CNFDESIREDSSID_LEN);
 		if (result) {
-			printk(KERN_ERR "Failed to set DesiredSSID.\n");
+			netdev_err(wlandev->netdev,
+				   "Failed to set DesiredSSID.\n");
 			msg->resultcode.data =
 			    P80211ENUM_resultcode_implementation_failure;
 			goto exit;
@@ -251,7 +253,8 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 						  HFA384x_RID_CNFPORTTYPE,
 						  HFA384x_PORTTYPE_IBSS);
 		if (result) {
-			printk(KERN_ERR "Failed to set CNFPORTTYPE.\n");
+			netdev_err(wlandev->netdev,
+				   "Failed to set CNFPORTTYPE.\n");
 			msg->resultcode.data =
 			    P80211ENUM_resultcode_implementation_failure;
 			goto exit;
@@ -261,15 +264,16 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 					HFA384x_RID_CREATEIBSS,
 					HFA384x_CREATEIBSS_JOINCREATEIBSS);
 		if (result) {
-			printk(KERN_ERR "Failed to set CREATEIBSS.\n");
+			netdev_err(wlandev->netdev,
+				   "Failed to set CREATEIBSS.\n");
 			msg->resultcode.data =
 			    P80211ENUM_resultcode_implementation_failure;
 			goto exit;
 		}
 		result = hfa384x_drvr_enable(hw, 0);
 		if (result) {
-			printk(KERN_ERR "drvr_enable(0) failed. "
-			       "result=%d\n", result);
+			netdev_err(wlandev->netdev,
+				   "drvr_enable(0) failed. result=%d\n", result);
 			msg->resultcode.data =
 			    P80211ENUM_resultcode_implementation_failure;
 			goto exit;
@@ -288,8 +292,8 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 					HFA384x_RID_HOSTSCAN, &scanreq,
 					sizeof(hfa384x_HostScanRequest_data_t));
 	if (result) {
-		printk(KERN_ERR "setconfig(SCANREQUEST) failed. result=%d\n",
-		       result);
+		netdev_err(wlandev->netdev,
+			   "setconfig(SCANREQUEST) failed. result=%d\n", result);
 		msg->resultcode.data =
 		    P80211ENUM_resultcode_implementation_failure;
 		goto exit;
@@ -310,8 +314,8 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 	if (istmpenable) {
 		result = hfa384x_drvr_disable(hw, 0);
 		if (result) {
-			printk(KERN_ERR "drvr_disable(0) failed. "
-			       "result=%d\n", result);
+			netdev_err(wlandev->netdev,
+				   "drvr_disable(0) failed. result=%d\n", result);
 			msg->resultcode.data =
 			    P80211ENUM_resultcode_implementation_failure;
 			goto exit;
@@ -322,8 +326,8 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFROAMINGMODE,
 					  roamingmode);
 	if (result) {
-		printk(KERN_ERR "setconfig(ROAMMODE) failed. result=%d\n",
-		       result);
+		netdev_err(wlandev->netdev,
+			   "setconfig(ROAMMODE) failed. result=%d\n", result);
 		msg->resultcode.data =
 		    P80211ENUM_resultcode_implementation_failure;
 		goto exit;
@@ -372,8 +376,9 @@ int prism2mgmt_scan_results(wlandevice_t *wlandev, void *msgp)
 	req->resultcode.status = P80211ENUM_msgitem_status_data_ok;
 
 	if (!hw->scanresults) {
-		printk(KERN_ERR
-		       "dot11req_scan_results can only be used after a successful dot11req_scan.\n");
+		netdev_err(wlandev->netdev,
+			   "dot11req_scan_results can only be used after "
+			   "a successful dot11req_scan.\n");
 		result = 2;
 		req->resultcode.data = P80211ENUM_resultcode_invalid_parameters;
 		goto exit;
@@ -555,14 +560,14 @@ int prism2mgmt_start(wlandevice_t *wlandev, void *msgp)
 	result = hfa384x_drvr_setconfig(hw, HFA384x_RID_CNFOWNSSID,
 					bytebuf, HFA384x_RID_CNFOWNSSID_LEN);
 	if (result) {
-		printk(KERN_ERR "Failed to set CnfOwnSSID\n");
+		netdev_err(wlandev->netdev, "Failed to set CnfOwnSSID\n");
 		goto failed;
 	}
 	result = hfa384x_drvr_setconfig(hw, HFA384x_RID_CNFDESIREDSSID,
 					bytebuf,
 					HFA384x_RID_CNFDESIREDSSID_LEN);
 	if (result) {
-		printk(KERN_ERR "Failed to set CnfDesiredSSID\n");
+		netdev_err(wlandev->netdev, "Failed to set CnfDesiredSSID\n");
 		goto failed;
 	}
 
@@ -574,7 +579,8 @@ int prism2mgmt_start(wlandevice_t *wlandev, void *msgp)
 	word = msg->beaconperiod.data;
 	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFAPBCNint, word);
 	if (result) {
-		printk(KERN_ERR "Failed to set beacon period=%d.\n", word);
+		netdev_err(wlandev->netdev,
+			   "Failed to set beacon period=%d.\n", word);
 		goto failed;
 	}
 
@@ -582,7 +588,8 @@ int prism2mgmt_start(wlandevice_t *wlandev, void *msgp)
 	word = msg->dschannel.data;
 	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFOWNCHANNEL, word);
 	if (result) {
-		printk(KERN_ERR "Failed to set channel=%d.\n", word);
+		netdev_err(wlandev->netdev,
+			   "Failed to set channel=%d.\n", word);
 		goto failed;
 	}
 	/* Basic rates */
@@ -610,7 +617,8 @@ int prism2mgmt_start(wlandevice_t *wlandev, void *msgp)
 
 	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFBASICRATES, word);
 	if (result) {
-		printk(KERN_ERR "Failed to set basicrates=%d.\n", word);
+		netdev_err(wlandev->netdev,
+			   "Failed to set basicrates=%d.\n", word);
 		goto failed;
 	}
 
@@ -639,13 +647,14 @@ int prism2mgmt_start(wlandevice_t *wlandev, void *msgp)
 
 	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFSUPPRATES, word);
 	if (result) {
-		printk(KERN_ERR "Failed to set supprates=%d.\n", word);
+		netdev_err(wlandev->netdev,
+			   "Failed to set supprates=%d.\n", word);
 		goto failed;
 	}
 
 	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_TXRATECNTL, word);
 	if (result) {
-		printk(KERN_ERR "Failed to set txrates=%d.\n", word);
+		netdev_err(wlandev->netdev, "Failed to set txrates=%d.\n", word);
 		goto failed;
 	}
 
@@ -659,7 +668,8 @@ int prism2mgmt_start(wlandevice_t *wlandev, void *msgp)
 	/* Enable the Port */
 	result = hfa384x_drvr_enable(hw, 0);
 	if (result) {
-		printk(KERN_ERR "Enable macport failed, result=%d.\n", result);
+		netdev_err(wlandev->netdev,
+			   "Enable macport failed, result=%d.\n", result);
 		goto failed;
 	}
 
@@ -704,8 +714,8 @@ int prism2mgmt_readpda(wlandevice_t *wlandev, void *msgp)
 	 * state.
 	 */
 	if (wlandev->msdstate != WLAN_MSD_FWLOAD) {
-		printk(KERN_ERR
-		       "PDA may only be read " "in the fwload state.\n");
+		netdev_err(wlandev->netdev,
+			   "PDA may only be read in the fwload state.\n");
 		msg->resultcode.data =
 		    P80211ENUM_resultcode_implementation_failure;
 		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
@@ -717,9 +727,9 @@ int prism2mgmt_readpda(wlandevice_t *wlandev, void *msgp)
 					      msg->pda.data,
 					      HFA384x_PDA_LEN_MAX);
 		if (result) {
-			printk(KERN_ERR
-			       "hfa384x_drvr_readpda() failed, "
-			       "result=%d\n", result);
+			netdev_err(wlandev->netdev,
+				   "hfa384x_drvr_readpda() failed, "
+				   "result=%d\n", result);
 
 			msg->resultcode.data =
 			    P80211ENUM_resultcode_implementation_failure;
@@ -766,9 +776,9 @@ int prism2mgmt_ramdl_state(wlandevice_t *wlandev, void *msgp)
 	struct p80211msg_p2req_ramdl_state *msg = msgp;
 
 	if (wlandev->msdstate != WLAN_MSD_FWLOAD) {
-		printk(KERN_ERR
-		       "ramdl_state(): may only be called "
-		       "in the fwload state.\n");
+		netdev_err(wlandev->netdev,
+			   "ramdl_state(): may only be called "
+			   "in the fwload state.\n");
 		msg->resultcode.data =
 		    P80211ENUM_resultcode_implementation_failure;
 		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
@@ -825,9 +835,9 @@ int prism2mgmt_ramdl_write(wlandevice_t *wlandev, void *msgp)
 	u8 *buf;
 
 	if (wlandev->msdstate != WLAN_MSD_FWLOAD) {
-		printk(KERN_ERR
-		       "ramdl_write(): may only be called "
-		       "in the fwload state.\n");
+		netdev_err(wlandev->netdev,
+			   "ramdl_write(): may only be called "
+			   "in the fwload state.\n");
 		msg->resultcode.data =
 		    P80211ENUM_resultcode_implementation_failure;
 		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
@@ -885,9 +895,9 @@ int prism2mgmt_flashdl_state(wlandevice_t *wlandev, void *msgp)
 	struct p80211msg_p2req_flashdl_state *msg = msgp;
 
 	if (wlandev->msdstate != WLAN_MSD_FWLOAD) {
-		printk(KERN_ERR
-		       "flashdl_state(): may only be called "
-		       "in the fwload state.\n");
+		netdev_err(wlandev->netdev,
+			   "flashdl_state(): may only be called "
+			   "in the fwload state.\n");
 		msg->resultcode.data =
 		    P80211ENUM_resultcode_implementation_failure;
 		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
@@ -920,8 +930,9 @@ int prism2mgmt_flashdl_state(wlandevice_t *wlandev, void *msgp)
 		wlandev->msdstate = WLAN_MSD_HWPRESENT;
 		result = prism2sta_ifstate(wlandev, P80211ENUM_ifstate_fwload);
 		if (result != P80211ENUM_resultcode_success) {
-			printk(KERN_ERR "prism2sta_ifstate(fwload) failed,"
-			       "P80211ENUM_resultcode=%d\n", result);
+			netdev_err(wlandev->netdev,
+				   "prism2sta_ifstate(fwload) failed,"
+				   "P80211ENUM_resultcode=%d\n", result);
 			msg->resultcode.data =
 			    P80211ENUM_resultcode_implementation_failure;
 			result = -1;
@@ -958,9 +969,9 @@ int prism2mgmt_flashdl_write(wlandevice_t *wlandev, void *msgp)
 	u8 *buf;
 
 	if (wlandev->msdstate != WLAN_MSD_FWLOAD) {
-		printk(KERN_ERR
-		       "flashdl_write(): may only be called "
-		       "in the fwload state.\n");
+		netdev_err(wlandev->netdev,
+			   "flashdl_write(): may only be called "
+			   "in the fwload state.\n");
 		msg->resultcode.data =
 		    P80211ENUM_resultcode_implementation_failure;
 		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
@@ -1153,7 +1164,7 @@ int prism2mgmt_wlansniff(wlandevice_t *wlandev, void *msgp)
 
 		}
 
-		printk(KERN_INFO "monitor mode disabled\n");
+		netdev_info(wlandev->netdev, "monitor mode disabled\n");
 		msg->resultcode.data = P80211ENUM_resultcode_success;
 		result = 0;
 		goto exit;
@@ -1282,7 +1293,7 @@ int prism2mgmt_wlansniff(wlandevice_t *wlandev, void *msgp)
 		}
 
 		if (wlandev->netdev->type == ARPHRD_ETHER)
-			printk(KERN_INFO "monitor mode enabled\n");
+			netdev_info(wlandev->netdev, "monitor mode enabled\n");
 
 		/* Set the driver state */
 		/* Do we want the prism2 header? */
diff --git a/drivers/staging/wlan-ng/prism2mgmt.h b/drivers/staging/wlan-ng/prism2mgmt.h
index 190d390c849..b62fdcba94e 100644
--- a/drivers/staging/wlan-ng/prism2mgmt.h
+++ b/drivers/staging/wlan-ng/prism2mgmt.h
@@ -109,4 +109,9 @@ void prism2sta_processing_defer(struct work_struct *data);
 void prism2sta_commsqual_defer(struct work_struct *data);
 void prism2sta_commsqual_timer(unsigned long data);
 
+/* Interface callback functions, passing data back up to the cfg80211 layer */
+void prism2_connect_result(wlandevice_t *wlandev, u8 failed);
+void prism2_disconnected(wlandevice_t *wlandev);
+void prism2_roamed(wlandevice_t *wlandev);
+
 #endif
diff --git a/drivers/staging/wlan-ng/prism2mib.c b/drivers/staging/wlan-ng/prism2mib.c
index 9b5f3b72d3c..0fb42dfca2a 100644
--- a/drivers/staging/wlan-ng/prism2mib.c
+++ b/drivers/staging/wlan-ng/prism2mib.c
@@ -672,7 +672,7 @@ static int prism2mib_fragmentationthreshold(struct mibrec *mib,
 
 	if (!isget)
 		if ((*uint32) % 2) {
-			printk(KERN_WARNING "Attempt to set odd number "
+			netdev_warn(wlandev->netdev, "Attempt to set odd number "
 			       "FragmentationThreshold\n");
 			msg->resultcode.data =
 			    P80211ENUM_resultcode_not_supported;
@@ -742,7 +742,7 @@ static int prism2mib_priv(struct mibrec *mib,
 			break;
 		}
 	default:
-		printk(KERN_ERR "Unhandled DID 0x%08x\n", mib->did);
+		netdev_err(wlandev->netdev, "Unhandled DID 0x%08x\n", mib->did);
 	}
 
 	return 0;
diff --git a/drivers/staging/wlan-ng/prism2sta.c b/drivers/staging/wlan-ng/prism2sta.c
index f9ccf2371db..278b6a1ef31 100644
--- a/drivers/staging/wlan-ng/prism2sta.c
+++ b/drivers/staging/wlan-ng/prism2sta.c
@@ -120,10 +120,6 @@ MODULE_PARM_DESC(prism2_reset_settletime, "reset settle time in ms");
 
 MODULE_LICENSE("Dual MPL/GPL");
 
-void prism2_connect_result(wlandevice_t *wlandev, u8 failed);
-void prism2_disconnected(wlandevice_t *wlandev);
-void prism2_roamed(wlandevice_t *wlandev);
-
 static int prism2sta_open(wlandevice_t *wlandev);
 static int prism2sta_close(wlandevice_t *wlandev);
 static void prism2sta_reset(wlandevice_t *wlandev);
@@ -405,8 +401,9 @@ static int prism2sta_mlmerequest(wlandevice_t *wlandev, struct p80211msg *msg)
 			break;
 		}
 	default:
-		printk(KERN_WARNING "Unknown mgmt request message 0x%08x",
-		       msg->msgcode);
+		netdev_warn(wlandev->netdev,
+			    "Unknown mgmt request message 0x%08x",
+			    msg->msgcode);
 		break;
 	}
 
@@ -469,7 +466,7 @@ u32 prism2sta_ifstate(wlandevice_t *wlandev, u32 ifstate)
 			result = P80211ENUM_resultcode_success;
 			break;
 		case WLAN_MSD_RUNNING:
-			printk(KERN_WARNING
+			netdev_warn(wlandev->netdev,
 			       "Cannot enter fwload state from enable state,"
 			       "you must disable first.\n");
 			result = P80211ENUM_resultcode_invalid_parameters;
@@ -1431,7 +1428,7 @@ void prism2sta_processing_defer(struct work_struct *data)
 
 	default:
 		/* This is bad, IO port problems? */
-		printk(KERN_WARNING
+		netdev_warn(wlandev->netdev,
 		       "unknown linkstatus=0x%02x\n", hw->link_status);
 		return;
 	}
@@ -1513,7 +1510,7 @@ static void prism2sta_inf_assocstatus(wlandevice_t *wlandev,
 
 	if (i >= hw->authlist.cnt) {
 		if (rec.assocstatus != HFA384x_ASSOCSTATUS_AUTHFAIL)
-			printk(KERN_WARNING
+			netdev_warn(wlandev->netdev,
 	"assocstatus info frame received for non-authenticated station.\n");
 	} else {
 		hw->authlist.assoc[i] =
@@ -1521,7 +1518,7 @@ static void prism2sta_inf_assocstatus(wlandevice_t *wlandev,
 		     rec.assocstatus == HFA384x_ASSOCSTATUS_REASSOC);
 
 		if (rec.assocstatus == HFA384x_ASSOCSTATUS_AUTHFAIL)
-			printk(KERN_WARNING
+			netdev_warn(wlandev->netdev,
 "authfail assocstatus info frame received for authenticated station.\n");
 	}
 }
@@ -1791,16 +1788,16 @@ void prism2sta_ev_info(wlandevice_t *wlandev, hfa384x_InfFrame_t *inf)
 		prism2sta_inf_psusercnt(wlandev, inf);
 		break;
 	case HFA384x_IT_KEYIDCHANGED:
-		printk(KERN_WARNING "Unhandled IT_KEYIDCHANGED\n");
+		netdev_warn(wlandev->netdev, "Unhandled IT_KEYIDCHANGED\n");
 		break;
 	case HFA384x_IT_ASSOCREQ:
-		printk(KERN_WARNING "Unhandled IT_ASSOCREQ\n");
+		netdev_warn(wlandev->netdev, "Unhandled IT_ASSOCREQ\n");
 		break;
 	case HFA384x_IT_MICFAILURE:
-		printk(KERN_WARNING "Unhandled IT_MICFAILURE\n");
+		netdev_warn(wlandev->netdev, "Unhandled IT_MICFAILURE\n");
 		break;
 	default:
-		printk(KERN_WARNING
+		netdev_warn(wlandev->netdev,
 		       "Unknown info type=0x%02x\n", inf->infotype);
 		break;
 	}
diff --git a/drivers/staging/wlan-ng/prism2usb.c b/drivers/staging/wlan-ng/prism2usb.c
index 4739c14d835..e92bbc12bb0 100644
--- a/drivers/staging/wlan-ng/prism2usb.c
+++ b/drivers/staging/wlan-ng/prism2usb.c
@@ -4,103 +4,58 @@
 #include "prism2sta.c"
 #include "prism2fw.c"
 
-#define PRISM_USB_DEVICE(vid, pid, name)	\
-	USB_DEVICE(vid, pid),			\
-	.driver_info = (unsigned long) name
+#define PRISM_DEV(vid, pid, name)		\
+	{ USB_DEVICE(vid, pid),			\
+	.driver_info = (unsigned long) name }
 
 static struct usb_device_id usb_prism_tbl[] = {
-	{PRISM_USB_DEVICE(0x04bb, 0x0922, "IOData AirPort WN-B11/USBS")},
-	{PRISM_USB_DEVICE(0x07aa, 0x0012, "Corega Wireless LAN USB Stick-11")},
-	{PRISM_USB_DEVICE(0x09aa, 0x3642, "Prism2.x 11Mbps WLAN USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x1668, 0x0408, "Actiontec Prism2.5 11Mbps WLAN USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x1668, 0x0421, "Actiontec Prism2.5 11Mbps WLAN USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x1915, 0x2236, "Linksys WUSB11v3.0 11Mbps WLAN USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x066b, 0x2212, "Linksys WUSB11v2.5 11Mbps WLAN USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x066b, 0x2213, "Linksys WUSB12v1.1 11Mbps WLAN USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x067c, 0x1022, "Siemens SpeedStream 1022 11Mbps WLAN USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x049f, 0x0033,
-	 "Compaq/Intel W100 PRO/Wireless 11Mbps multiport WLAN Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x0411, 0x0016, "Melco WLI-USB-S11 11Mbps WLAN Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x08de, 0x7a01, "PRISM25 IEEE 802.11 Mini USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x8086, 0x1111, "Intel PRO/Wireless 2011B LAN USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x0d8e, 0x7a01, "PRISM25 IEEE 802.11 Mini USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x045e, 0x006e, "Microsoft MN510 Wireless USB Adapter")},
-	{PRISM_USB_DEVICE(0x0967, 0x0204, "Acer Warplink USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x0cde, 0x0002, "Z-Com 725/726 Prism2.5 USB/USB Integrated")},
-	{PRISM_USB_DEVICE
-	 (0x0cde, 0x0005, "Z-Com Xl735 Wireless 802.11b USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x413c, 0x8100, "Dell TrueMobile 1180 Wireless USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x0b3b, 0x1601, "ALLNET 0193 11Mbps WLAN USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x0b3b, 0x1602, "ZyXEL ZyAIR B200 Wireless USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x0baf, 0x00eb, "USRobotics USR1120 Wireless USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x0411, 0x0027, "Melco WLI-USB-KS11G 11Mbps WLAN Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x04f1, 0x3009, "JVC MP-XP7250 Builtin USB WLAN Adapter")},
-	{PRISM_USB_DEVICE(0x0846, 0x4110, "NetGear MA111")},
-	{PRISM_USB_DEVICE(0x03f3, 0x0020, "Adaptec AWN-8020 USB WLAN Adapter")},
-	{PRISM_USB_DEVICE(0x2821, 0x3300, "ASUS-WL140 Wireless USB Adapter")},
-	{PRISM_USB_DEVICE(0x2001, 0x3700, "DWL-122 Wireless USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x2001, 0x3702, "DWL-120 Rev F Wireless USB Adapter")},
-	{PRISM_USB_DEVICE(0x50c2, 0x4013, "Averatec USB WLAN Adapter")},
-	{PRISM_USB_DEVICE(0x2c02, 0x14ea, "Planex GW-US11H WLAN USB Adapter")},
-	{PRISM_USB_DEVICE(0x124a, 0x168b, "Airvast PRISM3 WLAN USB Adapter")},
-	{PRISM_USB_DEVICE(0x083a, 0x3503, "T-Sinus 111 USB WLAN Adapter")},
-	{PRISM_USB_DEVICE(0x2821, 0x3300, "Hawking HighDB USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x0411, 0x0044, "Melco WLI-USB-KB11 11Mbps WLAN Adapter")},
-	{PRISM_USB_DEVICE(0x1668, 0x6106, "ROPEX FreeLan 802.11b USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x124a, 0x4017, "Pheenet WL-503IA 802.11b USB Adapter")},
-	{PRISM_USB_DEVICE(0x0bb2, 0x0302, "Ambit Microsystems Corp.")},
-	{PRISM_USB_DEVICE
-	 (0x9016, 0x182d, "Sitecom WL-022 802.11b USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x0543, 0x0f01, "ViewSonic Airsync USB Adapter 11Mbps (Prism2.5)")},
-	{ /* terminator */ }
+	PRISM_DEV(0x04bb, 0x0922, "IOData AirPort WN-B11/USBS"),
+	PRISM_DEV(0x07aa, 0x0012, "Corega Wireless LAN USB Stick-11"),
+	PRISM_DEV(0x09aa, 0x3642, "Prism2.x 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x1668, 0x0408, "Actiontec Prism2.5 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x1668, 0x0421, "Actiontec Prism2.5 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x1915, 0x2236, "Linksys WUSB11v3.0 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x066b, 0x2212, "Linksys WUSB11v2.5 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x066b, 0x2213, "Linksys WUSB12v1.1 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x0411, 0x0016, "Melco WLI-USB-S11 11Mbps WLAN Adapter"),
+	PRISM_DEV(0x08de, 0x7a01, "PRISM25 IEEE 802.11 Mini USB Adapter"),
+	PRISM_DEV(0x8086, 0x1111, "Intel PRO/Wireless 2011B LAN USB Adapter"),
+	PRISM_DEV(0x0d8e, 0x7a01, "PRISM25 IEEE 802.11 Mini USB Adapter"),
+	PRISM_DEV(0x045e, 0x006e, "Microsoft MN510 Wireless USB Adapter"),
+	PRISM_DEV(0x0967, 0x0204, "Acer Warplink USB Adapter"),
+	PRISM_DEV(0x0cde, 0x0002, "Z-Com 725/726 Prism2.5 USB/USB Integrated"),
+	PRISM_DEV(0x0cde, 0x0005, "Z-Com Xl735 Wireless 802.11b USB Adapter"),
+	PRISM_DEV(0x413c, 0x8100, "Dell TrueMobile 1180 Wireless USB Adapter"),
+	PRISM_DEV(0x0b3b, 0x1601, "ALLNET 0193 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x0b3b, 0x1602, "ZyXEL ZyAIR B200 Wireless USB Adapter"),
+	PRISM_DEV(0x0baf, 0x00eb, "USRobotics USR1120 Wireless USB Adapter"),
+	PRISM_DEV(0x0411, 0x0027, "Melco WLI-USB-KS11G 11Mbps WLAN Adapter"),
+	PRISM_DEV(0x04f1, 0x3009, "JVC MP-XP7250 Builtin USB WLAN Adapter"),
+	PRISM_DEV(0x0846, 0x4110, "NetGear MA111"),
+	PRISM_DEV(0x03f3, 0x0020, "Adaptec AWN-8020 USB WLAN Adapter"),
+	PRISM_DEV(0x2821, 0x3300, "ASUS-WL140 Wireless USB Adapter"),
+	PRISM_DEV(0x2001, 0x3700, "DWL-122 Wireless USB Adapter"),
+	PRISM_DEV(0x2001, 0x3702, "DWL-120 Rev F Wireless USB Adapter"),
+	PRISM_DEV(0x50c2, 0x4013, "Averatec USB WLAN Adapter"),
+	PRISM_DEV(0x2c02, 0x14ea, "Planex GW-US11H WLAN USB Adapter"),
+	PRISM_DEV(0x124a, 0x168b, "Airvast PRISM3 WLAN USB Adapter"),
+	PRISM_DEV(0x083a, 0x3503, "T-Sinus 111 USB WLAN Adapter"),
+	PRISM_DEV(0x2821, 0x3300, "Hawking HighDB USB Adapter"),
+	PRISM_DEV(0x0411, 0x0044, "Melco WLI-USB-KB11 11Mbps WLAN Adapter"),
+	PRISM_DEV(0x1668, 0x6106, "ROPEX FreeLan 802.11b USB Adapter"),
+	PRISM_DEV(0x124a, 0x4017, "Pheenet WL-503IA 802.11b USB Adapter"),
+	PRISM_DEV(0x0bb2, 0x0302, "Ambit Microsystems Corp."),
+	PRISM_DEV(0x9016, 0x182d, "Sitecom WL-022 802.11b USB Adapter"),
+	PRISM_DEV(0x0543, 0x0f01,
+		"ViewSonic Airsync USB Adapter 11Mbps (Prism2.5)"),
+	PRISM_DEV(0x067c, 0x1022,
+		"Siemens SpeedStream 1022 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x049f, 0x0033,
+		"Compaq/Intel W100 PRO/Wireless 11Mbps multiport WLAN Adapter"),
+	{ } /* terminator */
 };
-
 MODULE_DEVICE_TABLE(usb, usb_prism_tbl);
 
-/*----------------------------------------------------------------
-* prism2sta_probe_usb
-*
-* Probe routine called by the USB subsystem.
-*
-* Arguments:
-*	dev		ptr to the usb_device struct
-*	ifnum		interface number being offered
-*
-* Returns:
-*	NULL		- we're not claiming the device+interface
-*	non-NULL	- we are claiming the device+interface and
-*			  this is a ptr to the data we want back
-*			  when disconnect is called.
-*
-* Side effects:
-*
-* Call context:
-*	I'm not sure, assume it's interrupt.
-*
-----------------------------------------------------------------*/
 static int prism2sta_probe_usb(struct usb_interface *interface,
 			       const struct usb_device_id *id)
 {
@@ -141,7 +96,8 @@ static int prism2sta_probe_usb(struct usb_interface *interface,
 					   prism2_reset_settletime, 0);
 		if (result != 0) {
 			result = -EIO;
-			dev_err(&interface->dev, "hfa384x_corereset() failed.\n");
+			dev_err(&interface->dev,
+				"hfa384x_corereset() failed.\n");
 			goto failed_reset;
 		}
 	}
@@ -176,31 +132,11 @@ done:
 	return result;
 }
 
-/*----------------------------------------------------------------
-* prism2sta_disconnect_usb
-*
-* Called when a device previously claimed by probe is removed
-* from the USB.
-*
-* Arguments:
-*	dev		ptr to the usb_device struct
-*	ptr		ptr returned by probe() when the device
-*                       was claimed.
-*
-* Returns:
-*	Nothing
-*
-* Side effects:
-*
-* Call context:
-*	process
-----------------------------------------------------------------*/
 static void prism2sta_disconnect_usb(struct usb_interface *interface)
 {
 	wlandevice_t *wlandev;
 
 	wlandev = (wlandevice_t *) usb_get_intfdata(interface);
-
 	if (wlandev != NULL) {
 		LIST_HEAD(cleanlist);
 		struct list_head *entry;
@@ -292,6 +228,7 @@ static int prism2sta_suspend(struct usb_interface *interface,
 {
 	hfa384x_t *hw = NULL;
 	wlandevice_t *wlandev;
+
 	wlandev = (wlandevice_t *) usb_get_intfdata(interface);
 	if (!wlandev)
 		return -ENODEV;
@@ -314,6 +251,7 @@ static int prism2sta_resume(struct usb_interface *interface)
 	int result = 0;
 	hfa384x_t *hw = NULL;
 	wlandevice_t *wlandev;
+
 	wlandev = (wlandevice_t *) usb_get_intfdata(interface);
 	if (!wlandev)
 		return -ENODEV;
diff --git a/drivers/staging/xgifb/TODO b/drivers/staging/xgifb/TODO
index 392b29d8f13..7eb99140a39 100644
--- a/drivers/staging/xgifb/TODO
+++ b/drivers/staging/xgifb/TODO
@@ -6,8 +6,7 @@ Arnaud
 TODO:
 - clean ups
 - sort out dup ids with SiS driver
-- remove useless/wrong/unused #ifdef/code/...
-- fix printk usages
+- remove useless/wrong/unused code...
 - get rid of non-linux related stuff
 
 Please send patches to:
diff --git a/drivers/staging/xgifb/XGI_main.h b/drivers/staging/xgifb/XGI_main.h
index 95ce9708cec..85079fea715 100644
--- a/drivers/staging/xgifb/XGI_main.h
+++ b/drivers/staging/xgifb/XGI_main.h
@@ -4,12 +4,8 @@
 #include "XGIfb.h"
 #include "vb_def.h"
 
-#ifndef PCI_DEVICE_ID_XGI_42
 #define PCI_DEVICE_ID_XGI_42      0x042
-#endif
-#ifndef PCI_DEVICE_ID_XGI_27
 #define PCI_DEVICE_ID_XGI_27      0x027
-#endif
 
 static const struct pci_device_id xgifb_pci_table[] = {
 	{PCI_DEVICE(PCI_VENDOR_ID_XGI, PCI_DEVICE_ID_XGI_20)},
diff --git a/drivers/staging/xgifb/XGI_main_26.c b/drivers/staging/xgifb/XGI_main_26.c
index 466804687fc..f82f0574bbd 100644
--- a/drivers/staging/xgifb/XGI_main_26.c
+++ b/drivers/staging/xgifb/XGI_main_26.c
@@ -66,6 +66,7 @@ static int XGIfb_mode_rate_to_dclock(struct vb_device_info *XGI_Pr,
 	unsigned short ModeIdIndex = 0, ClockIndex = 0;
 	unsigned short RefreshRateTableIndex = 0;
 	int Clock;
+
 	InitTo330Pointer(HwDeviceExtension->jChipType, XGI_Pr);
 
 	XGI_SearchModeID(ModeNo, &ModeIdIndex);
@@ -95,6 +96,7 @@ static int XGIfb_mode_rate_to_ddata(struct vb_device_info *XGI_Pr,
 	unsigned short HRE, HBE, HRS, HDE;
 	unsigned char sr_data, cr_data, cr_data2;
 	int B, C, D, F, temp, j;
+
 	InitTo330Pointer(HwDeviceExtension->jChipType, XGI_Pr);
 	if (!XGI_SearchModeID(ModeNo, &ModeIdIndex))
 		return 0;
@@ -216,6 +218,7 @@ void XGIRegInit(struct vb_device_info *XGI_Pr, unsigned long BaseAddr)
 	XGI_Pr->P3c8 = BaseAddr + 0x18;
 	XGI_Pr->P3c9 = BaseAddr + 0x19;
 	XGI_Pr->P3da = BaseAddr + 0x2A;
+	XGI_Pr->Part0Port = BaseAddr + XGI_CRT2_PORT_00;
 	/* Digital video interface registers (LCD) */
 	XGI_Pr->Part1Port = BaseAddr + SIS_CRT2_PORT_04;
 	/* 301 TV Encoder registers */
diff --git a/drivers/staging/xgifb/vb_def.h b/drivers/staging/xgifb/vb_def.h
index 949f0e5eed8..481eb174fdf 100644
--- a/drivers/staging/xgifb/vb_def.h
+++ b/drivers/staging/xgifb/vb_def.h
@@ -44,12 +44,10 @@
 #define SetLCDtoNonExpanding 0x0010
 #define SetLCDDualLink       0x0100
 #define SetLCDLowResolution  0x0200
-#define SetLCDStdMode        0x0400
 
 /* LCD Capability shampoo */
 #define DefaultLCDCap        0x80ea
 #define EnableLCD24bpp       0x0004 /* default */
-#define DisableLCD24bpp      0x0000
 #define LCDPolarity          0x00c0 /* default: SyncNN */
 #define XGI_LCDDualLink      0x0100
 #define EnableSpectrum       0x0200
diff --git a/drivers/staging/xgifb/vb_init.c b/drivers/staging/xgifb/vb_init.c
index 21541720e05..ff210ddcbf9 100644
--- a/drivers/staging/xgifb/vb_init.c
+++ b/drivers/staging/xgifb/vb_init.c
@@ -130,6 +130,7 @@ static void XGINew_DDRII_Bootup_XG27(
 			unsigned long P3c4, struct vb_device_info *pVBInfo)
 {
 	unsigned long P3d4 = P3c4 + 0x10;
+
 	pVBInfo->ram_type = XGINew_GetXG20DRAMType(HwDeviceExtension, pVBInfo);
 	XGINew_SetMemoryClock(pVBInfo);
 
@@ -389,6 +390,7 @@ static void XGI_SetDRAM_Helper(unsigned long P3d4, u8 seed, u8 temp2, u8 reg,
 	u8 shift_factor, u8 mask1, u8 mask2)
 {
 	u8 j;
+
 	for (j = 0; j < 4; j++) {
 		temp2 |= (((seed >> (2 * j)) & 0x03) << shift_factor);
 		xgifb_reg_set(P3d4, reg, temp2);
diff --git a/drivers/staging/xgifb/vb_setmode.c b/drivers/staging/xgifb/vb_setmode.c
index 400c726753f..8795e173cb5 100644
--- a/drivers/staging/xgifb/vb_setmode.c
+++ b/drivers/staging/xgifb/vb_setmode.c
@@ -49,6 +49,7 @@ void InitTo330Pointer(unsigned char ChipType, struct vb_device_info *pVBInfo)
 
 	if (ChipType == XG27) {
 		unsigned char temp;
+
 		pVBInfo->MCLKData = XGI27New_MCLKData;
 		pVBInfo->CR40 = XGI27_cr41;
 		pVBInfo->XGINew_CR97 = 0xc1;
@@ -5222,6 +5223,7 @@ void XGI_SenseCRT1(struct vb_device_info *pVBInfo)
 	unsigned short temp;
 
 	int i;
+
 	xgifb_reg_set(pVBInfo->P3c4, 0x05, 0x86);
 
 	/* to fix XG42 single LCD sense to CRT+LCD */
@@ -5460,6 +5462,7 @@ unsigned char XGISetModeNew(struct xgifb_video_info *xgifb_info,
 	unsigned short ModeIdIndex;
 	struct vb_device_info VBINF;
 	struct vb_device_info *pVBInfo = &VBINF;
+
 	pVBInfo->IF_DEF_LVDS = 0;
 
 	if (HwDeviceExtension->jChipType >= XG20)
diff --git a/drivers/staging/xgifb/vgatypes.h b/drivers/staging/xgifb/vgatypes.h
index 264351441f9..61fa10fd470 100644
--- a/drivers/staging/xgifb/vgatypes.h
+++ b/drivers/staging/xgifb/vgatypes.h
@@ -5,7 +5,6 @@
 #include "../../video/fbdev/sis/vgatypes.h"
 #include "../../video/fbdev/sis/sis.h"		/* for LCD_TYPE */
 
-#ifndef XGI_VB_CHIP_TYPE
 enum XGI_VB_CHIP_TYPE {
 	VB_CHIP_Legacy = 0,
 	VB_CHIP_301,
@@ -19,7 +18,6 @@ enum XGI_VB_CHIP_TYPE {
 	VB_CHIP_UNKNOWN, /* other video bridge or no video bridge */
 	MAX_VB_CHIP
 };
-#endif
 
 struct xgi_hw_device_info {
 	unsigned long ulExternalChip; /* NO VB or other video bridge*/
diff --git a/drivers/staging/xillybus/xillybus.h b/drivers/staging/xillybus/xillybus.h
index e5e91d61288..78a749a7a1c 100644
--- a/drivers/staging/xillybus/xillybus.h
+++ b/drivers/staging/xillybus/xillybus.h
@@ -116,7 +116,6 @@ struct xilly_endpoint {
 	 */
 	struct pci_dev *pdev;
 	struct device *dev;
-	struct resource res; /* OF devices only */
 	struct xilly_endpoint_hardware *ephw;
 
 	struct list_head ep_list;
diff --git a/drivers/staging/xillybus/xillybus_core.c b/drivers/staging/xillybus/xillybus_core.c
index b0a6696f2da..fe8f9d28b03 100644
--- a/drivers/staging/xillybus/xillybus_core.c
+++ b/drivers/staging/xillybus/xillybus_core.c
@@ -2094,7 +2094,7 @@ struct xilly_endpoint *xillybus_init_endpoint(struct pci_dev *pdev,
 {
 	struct xilly_endpoint *endpoint;
 
-	endpoint = kzalloc(sizeof(*endpoint), GFP_KERNEL);
+	endpoint = devm_kzalloc(dev, sizeof(*endpoint), GFP_KERNEL);
 	if (!endpoint) {
 		dev_err(dev, "Failed to allocate memory. Aborting.\n");
 		return NULL;
diff --git a/drivers/staging/xillybus/xillybus_of.c b/drivers/staging/xillybus/xillybus_of.c
index 23a609b0ab1..46ea010b4ac 100644
--- a/drivers/staging/xillybus/xillybus_of.c
+++ b/drivers/staging/xillybus/xillybus_of.c
@@ -19,6 +19,7 @@
 #include <linux/of_address.h>
 #include <linux/of_device.h>
 #include <linux/of_platform.h>
+#include <linux/err.h>
 #include "xillybus.h"
 
 MODULE_DESCRIPTION("Xillybus driver for Open Firmware");
@@ -123,6 +124,7 @@ static int xilly_drv_probe(struct platform_device *op)
 	struct xilly_endpoint *endpoint;
 	int rc = 0;
 	int irq;
+	struct resource res;
 	struct xilly_endpoint_hardware *ephw = &of_hw;
 
 	if (of_property_read_bool(dev->of_node, "dma-coherent"))
@@ -135,38 +137,26 @@ static int xilly_drv_probe(struct platform_device *op)
 
 	dev_set_drvdata(dev, endpoint);
 
-	rc = of_address_to_resource(dev->of_node, 0, &endpoint->res);
+	rc = of_address_to_resource(dev->of_node, 0, &res);
 	if (rc) {
 		dev_warn(endpoint->dev,
 			 "Failed to obtain device tree resource\n");
-		goto failed_request_regions;
+		return rc;
 	}
 
-	if  (!request_mem_region(endpoint->res.start,
-				 resource_size(&endpoint->res), xillyname)) {
-		dev_err(endpoint->dev,
-			"request_mem_region failed. Aborting.\n");
-		rc = -EBUSY;
-		goto failed_request_regions;
-	}
+	endpoint->registers = devm_ioremap_resource(dev, &res);
 
-	endpoint->registers = of_iomap(dev->of_node, 0);
-	if (!endpoint->registers) {
-		dev_err(endpoint->dev,
-			"Failed to map I/O memory. Aborting.\n");
-		rc = -EIO;
-		goto failed_iomap0;
-	}
+	if (IS_ERR(endpoint->registers))
+		return PTR_ERR(endpoint->registers);
 
 	irq = irq_of_parse_and_map(dev->of_node, 0);
 
-	rc = request_irq(irq, xillybus_isr, 0, xillyname, endpoint);
+	rc = devm_request_irq(dev, irq, xillybus_isr, 0, xillyname, endpoint);
 
 	if (rc) {
 		dev_err(endpoint->dev,
 			"Failed to register IRQ handler. Aborting.\n");
-		rc = -ENODEV;
-		goto failed_register_irq;
+		return -ENODEV;
 	}
 
 	rc = xillybus_endpoint_discovery(endpoint);
@@ -174,18 +164,8 @@ static int xilly_drv_probe(struct platform_device *op)
 	if (!rc)
 		return 0;
 
-	free_irq(irq, endpoint);
-
-failed_register_irq:
-	iounmap(endpoint->registers);
-failed_iomap0:
-	release_mem_region(endpoint->res.start,
-			   resource_size(&endpoint->res));
-
-failed_request_regions:
 	xillybus_do_cleanup(&endpoint->cleanup, endpoint);
 
-	kfree(endpoint);
 	return rc;
 }
 
@@ -193,20 +173,11 @@ static int xilly_drv_remove(struct platform_device *op)
 {
 	struct device *dev = &op->dev;
 	struct xilly_endpoint *endpoint = dev_get_drvdata(dev);
-	int irq = irq_of_parse_and_map(dev->of_node, 0);
 
 	xillybus_endpoint_remove(endpoint);
 
-	free_irq(irq, endpoint);
-
-	iounmap(endpoint->registers);
-	release_mem_region(endpoint->res.start,
-			   resource_size(&endpoint->res));
-
 	xillybus_do_cleanup(&endpoint->cleanup, endpoint);
 
-	kfree(endpoint);
-
 	return 0;
 }
 
diff --git a/drivers/staging/xillybus/xillybus_pcie.c b/drivers/staging/xillybus/xillybus_pcie.c
index 51426d80ca7..a4fe51c90e9 100644
--- a/drivers/staging/xillybus/xillybus_pcie.c
+++ b/drivers/staging/xillybus/xillybus_pcie.c
@@ -141,38 +141,32 @@ static int xilly_probe(struct pci_dev *pdev,
 
 	pci_set_drvdata(pdev, endpoint);
 
-	rc = pci_enable_device(pdev);
-
-	/* L0s has caused packet drops. No power saving, thank you. */
-
-	pci_disable_link_state(pdev, PCIE_LINK_STATE_L0S);
+	rc = pcim_enable_device(pdev);
 
 	if (rc) {
 		dev_err(endpoint->dev,
-			"pci_enable_device() failed. Aborting.\n");
-		goto no_enable;
+			"pcim_enable_device() failed. Aborting.\n");
+		return rc;
 	}
 
+	/* L0s has caused packet drops. No power saving, thank you. */
+
+	pci_disable_link_state(pdev, PCIE_LINK_STATE_L0S);
+
 	if (!(pci_resource_flags(pdev, 0) & IORESOURCE_MEM)) {
 		dev_err(endpoint->dev,
 			"Incorrect BAR configuration. Aborting.\n");
-		rc = -ENODEV;
-		goto bad_bar;
+		return -ENODEV;
 	}
 
-	rc = pci_request_regions(pdev, xillyname);
+	rc = pcim_iomap_regions(pdev, 0x01, xillyname);
 	if (rc) {
 		dev_err(endpoint->dev,
-			"pci_request_regions() failed. Aborting.\n");
-		goto failed_request_regions;
+			"pcim_iomap_regions() failed. Aborting.\n");
+		return rc;
 	}
 
-	endpoint->registers = pci_iomap(pdev, 0, 128);
-	if (!endpoint->registers) {
-		dev_err(endpoint->dev, "Failed to map BAR 0. Aborting.\n");
-		rc = -EIO;
-		goto failed_iomap0;
-	}
+	endpoint->registers = pcim_iomap_table(pdev)[0];
 
 	pci_set_master(pdev);
 
@@ -180,16 +174,15 @@ static int xilly_probe(struct pci_dev *pdev,
 	if (pci_enable_msi(pdev)) {
 		dev_err(endpoint->dev,
 			"Failed to enable MSI interrupts. Aborting.\n");
-		rc = -ENODEV;
-		goto failed_enable_msi;
+		return -ENODEV;
 	}
-	rc = request_irq(pdev->irq, xillybus_isr, 0, xillyname, endpoint);
+	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");
-		rc = -ENODEV;
-		goto failed_register_msi;
+		return -ENODEV;
 	}
 
 	/*
@@ -203,8 +196,7 @@ static int xilly_probe(struct pci_dev *pdev,
 		endpoint->dma_using_dac = 0;
 	else {
 		dev_err(endpoint->dev, "Failed to set DMA mask. Aborting.\n");
-		rc = -ENODEV;
-		goto failed_dmamask;
+		return -ENODEV;
 	}
 
 	rc = xillybus_endpoint_discovery(endpoint);
@@ -212,22 +204,8 @@ static int xilly_probe(struct pci_dev *pdev,
 	if (!rc)
 		return 0;
 
-failed_dmamask:
-	free_irq(pdev->irq, endpoint);
-failed_register_msi:
-	pci_disable_msi(pdev);
-failed_enable_msi:
-	/* pci_clear_master(pdev); Nobody else seems to do this */
-	pci_iounmap(pdev, endpoint->registers);
-failed_iomap0:
-	pci_release_regions(pdev);
-failed_request_regions:
-bad_bar:
-	pci_disable_device(pdev);
-no_enable:
 	xillybus_do_cleanup(&endpoint->cleanup, endpoint);
 
-	kfree(endpoint);
 	return rc;
 }
 
@@ -237,16 +215,7 @@ static void xilly_remove(struct pci_dev *pdev)
 
 	xillybus_endpoint_remove(endpoint);
 
-	free_irq(pdev->irq, endpoint);
-
-	pci_disable_msi(pdev);
-	pci_iounmap(pdev, endpoint->registers);
-	pci_release_regions(pdev);
-	pci_disable_device(pdev);
-
 	xillybus_do_cleanup(&endpoint->cleanup, endpoint);
-
-	kfree(endpoint);
 }
 
 MODULE_DEVICE_TABLE(pci, xillyids);
diff --git a/drivers/target/iscsi/iscsi_target.c b/drivers/target/iscsi/iscsi_target.c
index 78cab13bbb1..9189bc0a87a 100644
--- a/drivers/target/iscsi/iscsi_target.c
+++ b/drivers/target/iscsi/iscsi_target.c
@@ -460,6 +460,7 @@ int iscsit_del_np(struct iscsi_np *np)
 	spin_lock_bh(&np->np_thread_lock);
 	np->np_exports--;
 	if (np->np_exports) {
+		np->enabled = true;
 		spin_unlock_bh(&np->np_thread_lock);
 		return 0;
 	}
@@ -1593,7 +1594,9 @@ int iscsit_process_nop_out(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
 	 * Initiator is expecting a NopIN ping reply..
 	 */
 	if (hdr->itt != RESERVED_ITT) {
-		BUG_ON(!cmd);
+		if (!cmd)
+			return iscsit_add_reject(conn, ISCSI_REASON_PROTOCOL_ERROR,
+						(unsigned char *)hdr);
 
 		spin_lock_bh(&conn->cmd_lock);
 		list_add_tail(&cmd->i_conn_node, &conn->conn_cmd_list);
diff --git a/drivers/target/iscsi/iscsi_target_core.h b/drivers/target/iscsi/iscsi_target_core.h
index 6960f22909a..302eb3b7871 100644
--- a/drivers/target/iscsi/iscsi_target_core.h
+++ b/drivers/target/iscsi/iscsi_target_core.h
@@ -775,6 +775,7 @@ struct iscsi_np {
 	int			np_ip_proto;
 	int			np_sock_type;
 	enum np_thread_state_table np_thread_state;
+	bool                    enabled;
 	enum iscsi_timer_flags_table np_login_timer_flags;
 	u32			np_exports;
 	enum np_flags_table	np_flags;
diff --git a/drivers/target/iscsi/iscsi_target_login.c b/drivers/target/iscsi/iscsi_target_login.c
index 8739b98f6f9..d9b1d88e1ad 100644
--- a/drivers/target/iscsi/iscsi_target_login.c
+++ b/drivers/target/iscsi/iscsi_target_login.c
@@ -249,6 +249,28 @@ static void iscsi_login_set_conn_values(
 	mutex_unlock(&auth_id_lock);
 }
 
+static __printf(2, 3) int iscsi_change_param_sprintf(
+	struct iscsi_conn *conn,
+	const char *fmt, ...)
+{
+	va_list args;
+	unsigned char buf[64];
+
+	memset(buf, 0, sizeof buf);
+
+	va_start(args, fmt);
+	vsnprintf(buf, sizeof buf, fmt, args);
+	va_end(args);
+
+	if (iscsi_change_param_value(buf, conn->param_list, 0) < 0) {
+		iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
+				ISCSI_LOGIN_STATUS_NO_RESOURCES);
+		return -1;
+	}
+
+	return 0;
+}
+
 /*
  *	This is the leading connection of a new session,
  *	or session reinstatement.
@@ -339,7 +361,6 @@ static int iscsi_login_zero_tsih_s2(
 {
 	struct iscsi_node_attrib *na;
 	struct iscsi_session *sess = conn->sess;
-	unsigned char buf[32];
 	bool iser = false;
 
 	sess->tpg = conn->tpg;
@@ -380,26 +401,16 @@ static int iscsi_login_zero_tsih_s2(
 	 *
 	 * In our case, we have already located the struct iscsi_tiqn at this point.
 	 */
-	memset(buf, 0, 32);
-	sprintf(buf, "TargetPortalGroupTag=%hu", sess->tpg->tpgt);
-	if (iscsi_change_param_value(buf, conn->param_list, 0) < 0) {
-		iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-				ISCSI_LOGIN_STATUS_NO_RESOURCES);
+	if (iscsi_change_param_sprintf(conn, "TargetPortalGroupTag=%hu", sess->tpg->tpgt))
 		return -1;
-	}
 
 	/*
 	 * Workaround for Initiators that have broken connection recovery logic.
 	 *
 	 * "We would really like to get rid of this." Linux-iSCSI.org team
 	 */
-	memset(buf, 0, 32);
-	sprintf(buf, "ErrorRecoveryLevel=%d", na->default_erl);
-	if (iscsi_change_param_value(buf, conn->param_list, 0) < 0) {
-		iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-				ISCSI_LOGIN_STATUS_NO_RESOURCES);
+	if (iscsi_change_param_sprintf(conn, "ErrorRecoveryLevel=%d", na->default_erl))
 		return -1;
-	}
 
 	if (iscsi_login_disable_FIM_keys(conn->param_list, conn) < 0)
 		return -1;
@@ -411,12 +422,9 @@ static int iscsi_login_zero_tsih_s2(
 		unsigned long mrdsl, off;
 		int rc;
 
-		sprintf(buf, "RDMAExtensions=Yes");
-		if (iscsi_change_param_value(buf, conn->param_list, 0) < 0) {
-			iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-				ISCSI_LOGIN_STATUS_NO_RESOURCES);
+		if (iscsi_change_param_sprintf(conn, "RDMAExtensions=Yes"))
 			return -1;
-		}
+
 		/*
 		 * Make MaxRecvDataSegmentLength PAGE_SIZE aligned for
 		 * Immediate Data + Unsolicitied Data-OUT if necessary..
@@ -436,7 +444,7 @@ static int iscsi_login_zero_tsih_s2(
 		}
 		off = mrdsl % PAGE_SIZE;
 		if (!off)
-			return 0;
+			goto check_prot;
 
 		if (mrdsl < PAGE_SIZE)
 			mrdsl = PAGE_SIZE;
@@ -446,11 +454,25 @@ static int iscsi_login_zero_tsih_s2(
 		pr_warn("Aligning ISER MaxRecvDataSegmentLength: %lu down"
 			" to PAGE_SIZE\n", mrdsl);
 
-		sprintf(buf, "MaxRecvDataSegmentLength=%lu\n", mrdsl);
-		if (iscsi_change_param_value(buf, conn->param_list, 0) < 0) {
-			iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-				ISCSI_LOGIN_STATUS_NO_RESOURCES);
+		if (iscsi_change_param_sprintf(conn, "MaxRecvDataSegmentLength=%lu\n", mrdsl))
 			return -1;
+		/*
+		 * ISER currently requires that ImmediateData + Unsolicited
+		 * Data be disabled when protection / signature MRs are enabled.
+		 */
+check_prot:
+		if (sess->se_sess->sup_prot_ops &
+		   (TARGET_PROT_DOUT_STRIP | TARGET_PROT_DOUT_PASS |
+		    TARGET_PROT_DOUT_INSERT)) {
+
+			if (iscsi_change_param_sprintf(conn, "ImmediateData=No"))
+				return -1;
+
+			if (iscsi_change_param_sprintf(conn, "InitialR2T=Yes"))
+				return -1;
+
+			pr_debug("Forcing ImmediateData=No + InitialR2T=Yes for"
+				 " T10-PI enabled ISER session\n");
 		}
 	}
 
@@ -593,13 +615,8 @@ static int iscsi_login_non_zero_tsih_s2(
 	 *
 	 * In our case, we have already located the struct iscsi_tiqn at this point.
 	 */
-	memset(buf, 0, 32);
-	sprintf(buf, "TargetPortalGroupTag=%hu", sess->tpg->tpgt);
-	if (iscsi_change_param_value(buf, conn->param_list, 0) < 0) {
-		iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-				ISCSI_LOGIN_STATUS_NO_RESOURCES);
+	if (iscsi_change_param_sprintf(conn, "TargetPortalGroupTag=%hu", sess->tpg->tpgt))
 		return -1;
-	}
 
 	return iscsi_login_disable_FIM_keys(conn->param_list, conn);
 }
@@ -984,6 +1001,7 @@ int iscsi_target_setup_login_socket(
 	}
 
 	np->np_transport = t;
+	np->enabled = true;
 	return 0;
 }
 
diff --git a/drivers/target/iscsi/iscsi_target_tpg.c b/drivers/target/iscsi/iscsi_target_tpg.c
index eb96b20dc09..1431e8400d2 100644
--- a/drivers/target/iscsi/iscsi_target_tpg.c
+++ b/drivers/target/iscsi/iscsi_target_tpg.c
@@ -184,6 +184,8 @@ static void iscsit_clear_tpg_np_login_thread(
 		return;
 	}
 
+	if (shutdown)
+		tpg_np->tpg_np->enabled = false;
 	iscsit_reset_np_thread(tpg_np->tpg_np, tpg_np, tpg, shutdown);
 }
 
diff --git a/drivers/target/loopback/tcm_loop.c b/drivers/target/loopback/tcm_loop.c
index c886ad1c39f..73ab75ddaf4 100644
--- a/drivers/target/loopback/tcm_loop.c
+++ b/drivers/target/loopback/tcm_loop.c
@@ -951,7 +951,7 @@ static int tcm_loop_port_link(
 	struct tcm_loop_hba *tl_hba = tl_tpg->tl_hba;
 
 	atomic_inc(&tl_tpg->tl_tpg_port_count);
-	smp_mb__after_atomic_inc();
+	smp_mb__after_atomic();
 	/*
 	 * Add Linux/SCSI struct scsi_device by HCTL
 	 */
@@ -986,7 +986,7 @@ static void tcm_loop_port_unlink(
 	scsi_device_put(sd);
 
 	atomic_dec(&tl_tpg->tl_tpg_port_count);
-	smp_mb__after_atomic_dec();
+	smp_mb__after_atomic();
 
 	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 fcbe6125b73..fbc5ebb5f76 100644
--- a/drivers/target/target_core_alua.c
+++ b/drivers/target/target_core_alua.c
@@ -393,7 +393,7 @@ target_emulate_set_target_port_groups(struct se_cmd *cmd)
 					continue;
 
 				atomic_inc(&tg_pt_gp->tg_pt_gp_ref_cnt);
-				smp_mb__after_atomic_inc();
+				smp_mb__after_atomic();
 
 				spin_unlock(&dev->t10_alua.tg_pt_gps_lock);
 
@@ -404,7 +404,7 @@ target_emulate_set_target_port_groups(struct se_cmd *cmd)
 
 				spin_lock(&dev->t10_alua.tg_pt_gps_lock);
 				atomic_dec(&tg_pt_gp->tg_pt_gp_ref_cnt);
-				smp_mb__after_atomic_dec();
+				smp_mb__after_atomic();
 				break;
 			}
 			spin_unlock(&dev->t10_alua.tg_pt_gps_lock);
@@ -576,7 +576,16 @@ static inline int core_alua_state_standby(
 	case REPORT_LUNS:
 	case RECEIVE_DIAGNOSTIC:
 	case SEND_DIAGNOSTIC:
+	case READ_CAPACITY:
 		return 0;
+	case SERVICE_ACTION_IN:
+		switch (cdb[1] & 0x1f) {
+		case SAI_READ_CAPACITY_16:
+			return 0;
+		default:
+			set_ascq(cmd, ASCQ_04H_ALUA_TG_PT_STANDBY);
+			return 1;
+		}
 	case MAINTENANCE_IN:
 		switch (cdb[1] & 0x1f) {
 		case MI_REPORT_TARGET_PGS:
@@ -990,7 +999,7 @@ static void core_alua_do_transition_tg_pt_work(struct work_struct *work)
 		 * TARGET PORT GROUPS command
 		 */
 		atomic_inc(&mem->tg_pt_gp_mem_ref_cnt);
-		smp_mb__after_atomic_inc();
+		smp_mb__after_atomic();
 		spin_unlock(&tg_pt_gp->tg_pt_gp_lock);
 
 		spin_lock_bh(&port->sep_alua_lock);
@@ -1020,7 +1029,7 @@ static void core_alua_do_transition_tg_pt_work(struct work_struct *work)
 
 		spin_lock(&tg_pt_gp->tg_pt_gp_lock);
 		atomic_dec(&mem->tg_pt_gp_mem_ref_cnt);
-		smp_mb__after_atomic_dec();
+		smp_mb__after_atomic();
 	}
 	spin_unlock(&tg_pt_gp->tg_pt_gp_lock);
 	/*
@@ -1054,7 +1063,7 @@ 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_dec();
+	smp_mb__after_atomic();
 	spin_unlock(&dev->t10_alua.tg_pt_gps_lock);
 
 	if (tg_pt_gp->tg_pt_gp_transition_complete)
@@ -1116,7 +1125,7 @@ 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_inc();
+	smp_mb__after_atomic();
 	spin_unlock(&dev->t10_alua.tg_pt_gps_lock);
 
 	if (!explicit && tg_pt_gp->tg_pt_gp_implicit_trans_secs) {
@@ -1159,7 +1168,7 @@ 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_inc();
+	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',
@@ -1176,7 +1185,7 @@ int core_alua_do_port_transition(
 		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_dec();
+		smp_mb__after_atomic();
 		return rc;
 	}
 	/*
@@ -1190,7 +1199,7 @@ int core_alua_do_port_transition(
 
 		dev = lu_gp_mem->lu_gp_mem_dev;
 		atomic_inc(&lu_gp_mem->lu_gp_mem_ref_cnt);
-		smp_mb__after_atomic_inc();
+		smp_mb__after_atomic();
 		spin_unlock(&lu_gp->lu_gp_lock);
 
 		spin_lock(&dev->t10_alua.tg_pt_gps_lock);
@@ -1219,7 +1228,7 @@ int core_alua_do_port_transition(
 				tg_pt_gp->tg_pt_gp_alua_nacl = NULL;
 			}
 			atomic_inc(&tg_pt_gp->tg_pt_gp_ref_cnt);
-			smp_mb__after_atomic_inc();
+			smp_mb__after_atomic();
 			spin_unlock(&dev->t10_alua.tg_pt_gps_lock);
 			/*
 			 * core_alua_do_transition_tg_pt() will always return
@@ -1230,7 +1239,7 @@ int core_alua_do_port_transition(
 
 			spin_lock(&dev->t10_alua.tg_pt_gps_lock);
 			atomic_dec(&tg_pt_gp->tg_pt_gp_ref_cnt);
-			smp_mb__after_atomic_dec();
+			smp_mb__after_atomic();
 			if (rc)
 				break;
 		}
@@ -1238,7 +1247,7 @@ int core_alua_do_port_transition(
 
 		spin_lock(&lu_gp->lu_gp_lock);
 		atomic_dec(&lu_gp_mem->lu_gp_mem_ref_cnt);
-		smp_mb__after_atomic_dec();
+		smp_mb__after_atomic();
 	}
 	spin_unlock(&lu_gp->lu_gp_lock);
 
@@ -1252,7 +1261,7 @@ int core_alua_do_port_transition(
 	}
 
 	atomic_dec(&lu_gp->lu_gp_ref_cnt);
-	smp_mb__after_atomic_dec();
+	smp_mb__after_atomic();
 	return rc;
 }
 
diff --git a/drivers/target/target_core_configfs.c b/drivers/target/target_core_configfs.c
index 60a9ae6df76..bf55c5a04cf 100644
--- a/drivers/target/target_core_configfs.c
+++ b/drivers/target/target_core_configfs.c
@@ -2227,6 +2227,11 @@ static ssize_t target_core_alua_tg_pt_gp_store_attr_alua_access_state(
 			" tg_pt_gp ID: %hu\n", tg_pt_gp->tg_pt_gp_valid_id);
 		return -EINVAL;
 	}
+	if (!(dev->dev_flags & DF_CONFIGURED)) {
+		pr_err("Unable to set alua_access_state while device is"
+		       " not configured\n");
+		return -ENODEV;
+	}
 
 	ret = kstrtoul(page, 0, &tmp);
 	if (ret < 0) {
diff --git a/drivers/target/target_core_device.c b/drivers/target/target_core_device.c
index 65001e13367..11d26fe65bf 100644
--- a/drivers/target/target_core_device.c
+++ b/drivers/target/target_core_device.c
@@ -225,7 +225,7 @@ struct se_dev_entry *core_get_se_deve_from_rtpi(
 			continue;
 
 		atomic_inc(&deve->pr_ref_count);
-		smp_mb__after_atomic_inc();
+		smp_mb__after_atomic();
 		spin_unlock_irq(&nacl->device_list_lock);
 
 		return deve;
@@ -798,10 +798,10 @@ int se_dev_set_emulate_write_cache(struct se_device *dev, int flag)
 		pr_err("emulate_write_cache not supported for pSCSI\n");
 		return -EINVAL;
 	}
-	if (dev->transport->get_write_cache) {
-		pr_warn("emulate_write_cache cannot be changed when underlying"
-			" HW reports WriteCacheEnabled, ignoring request\n");
-		return 0;
+	if (flag &&
+	    dev->transport->get_write_cache) {
+		pr_err("emulate_write_cache not supported for this device\n");
+		return -EINVAL;
 	}
 
 	dev->dev_attrib.emulate_write_cache = flag;
@@ -936,6 +936,10 @@ int se_dev_set_pi_prot_type(struct se_device *dev, int flag)
 		return 0;
 	}
 	if (!dev->transport->init_prot || !dev->transport->free_prot) {
+		/* 0 is only allowed value for non-supporting backends */
+		if (flag == 0)
+			return 0;
+
 		pr_err("DIF protection not supported by backend: %s\n",
 		       dev->transport->name);
 		return -ENOSYS;
@@ -1392,7 +1396,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_inc();
+	smp_mb__after_atomic();
 	spin_unlock(&lun->lun_acl_lock);
 
 	pr_debug("%s_TPG[%hu]_LUN[%u->%u] - Added %s ACL for "
@@ -1426,7 +1430,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_dec();
+	smp_mb__after_atomic();
 	spin_unlock(&lun->lun_acl_lock);
 
 	core_disable_device_list_for_node(lun, NULL, lacl->mapped_lun,
diff --git a/drivers/target/target_core_iblock.c b/drivers/target/target_core_iblock.c
index 9e0232cca92..7e6b857c6b3 100644
--- a/drivers/target/target_core_iblock.c
+++ b/drivers/target/target_core_iblock.c
@@ -323,7 +323,7 @@ static void iblock_bio_done(struct bio *bio, int err)
 		 * Bump the ib_bio_err_cnt and release bio.
 		 */
 		atomic_inc(&ibr->ib_bio_err_cnt);
-		smp_mb__after_atomic_inc();
+		smp_mb__after_atomic();
 	}
 
 	bio_put(bio);
diff --git a/drivers/target/target_core_pr.c b/drivers/target/target_core_pr.c
index 3013287a2aa..df357862286 100644
--- a/drivers/target/target_core_pr.c
+++ b/drivers/target/target_core_pr.c
@@ -675,7 +675,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_inc();
+		smp_mb__after_atomic();
 		spin_unlock(&dev->se_port_lock);
 
 		spin_lock_bh(&port->sep_alua_lock);
@@ -710,7 +710,7 @@ static struct t10_pr_registration *__core_scsi3_alloc_registration(
 				continue;
 
 			atomic_inc(&deve_tmp->pr_ref_count);
-			smp_mb__after_atomic_inc();
+			smp_mb__after_atomic();
 			spin_unlock_bh(&port->sep_alua_lock);
 			/*
 			 * Grab a configfs group dependency that is released
@@ -723,9 +723,9 @@ static struct t10_pr_registration *__core_scsi3_alloc_registration(
 				pr_err("core_scsi3_lunacl_depend"
 						"_item() failed\n");
 				atomic_dec(&port->sep_tg_pt_ref_cnt);
-				smp_mb__after_atomic_dec();
+				smp_mb__after_atomic();
 				atomic_dec(&deve_tmp->pr_ref_count);
-				smp_mb__after_atomic_dec();
+				smp_mb__after_atomic();
 				goto out;
 			}
 			/*
@@ -740,9 +740,9 @@ static struct t10_pr_registration *__core_scsi3_alloc_registration(
 						sa_res_key, all_tg_pt, aptpl);
 			if (!pr_reg_atp) {
 				atomic_dec(&port->sep_tg_pt_ref_cnt);
-				smp_mb__after_atomic_dec();
+				smp_mb__after_atomic();
 				atomic_dec(&deve_tmp->pr_ref_count);
-				smp_mb__after_atomic_dec();
+				smp_mb__after_atomic();
 				core_scsi3_lunacl_undepend_item(deve_tmp);
 				goto out;
 			}
@@ -755,7 +755,7 @@ static struct t10_pr_registration *__core_scsi3_alloc_registration(
 
 		spin_lock(&dev->se_port_lock);
 		atomic_dec(&port->sep_tg_pt_ref_cnt);
-		smp_mb__after_atomic_dec();
+		smp_mb__after_atomic();
 	}
 	spin_unlock(&dev->se_port_lock);
 
@@ -1110,7 +1110,7 @@ static struct t10_pr_registration *__core_scsi3_locate_pr_reg(
 					continue;
 			}
 			atomic_inc(&pr_reg->pr_res_holders);
-			smp_mb__after_atomic_inc();
+			smp_mb__after_atomic();
 			spin_unlock(&pr_tmpl->registration_lock);
 			return pr_reg;
 		}
@@ -1125,7 +1125,7 @@ static struct t10_pr_registration *__core_scsi3_locate_pr_reg(
 			continue;
 
 		atomic_inc(&pr_reg->pr_res_holders);
-		smp_mb__after_atomic_inc();
+		smp_mb__after_atomic();
 		spin_unlock(&pr_tmpl->registration_lock);
 		return pr_reg;
 	}
@@ -1155,7 +1155,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_dec();
+	smp_mb__after_atomic();
 }
 
 static int core_scsi3_check_implicit_release(
@@ -1349,7 +1349,7 @@ static void core_scsi3_tpg_undepend_item(struct se_portal_group *tpg)
 			&tpg->tpg_group.cg_item);
 
 	atomic_dec(&tpg->tpg_pr_ref_count);
-	smp_mb__after_atomic_dec();
+	smp_mb__after_atomic();
 }
 
 static int core_scsi3_nodeacl_depend_item(struct se_node_acl *nacl)
@@ -1369,7 +1369,7 @@ static void core_scsi3_nodeacl_undepend_item(struct se_node_acl *nacl)
 
 	if (nacl->dynamic_node_acl) {
 		atomic_dec(&nacl->acl_pr_ref_count);
-		smp_mb__after_atomic_dec();
+		smp_mb__after_atomic();
 		return;
 	}
 
@@ -1377,7 +1377,7 @@ static void core_scsi3_nodeacl_undepend_item(struct se_node_acl *nacl)
 			&nacl->acl_group.cg_item);
 
 	atomic_dec(&nacl->acl_pr_ref_count);
-	smp_mb__after_atomic_dec();
+	smp_mb__after_atomic();
 }
 
 static int core_scsi3_lunacl_depend_item(struct se_dev_entry *se_deve)
@@ -1408,7 +1408,7 @@ static void core_scsi3_lunacl_undepend_item(struct se_dev_entry *se_deve)
 	 */
 	if (!lun_acl) {
 		atomic_dec(&se_deve->pr_ref_count);
-		smp_mb__after_atomic_dec();
+		smp_mb__after_atomic();
 		return;
 	}
 	nacl = lun_acl->se_lun_nacl;
@@ -1418,7 +1418,7 @@ static void core_scsi3_lunacl_undepend_item(struct se_dev_entry *se_deve)
 			&lun_acl->se_lun_group.cg_item);
 
 	atomic_dec(&se_deve->pr_ref_count);
-	smp_mb__after_atomic_dec();
+	smp_mb__after_atomic();
 }
 
 static sense_reason_t
@@ -1552,14 +1552,14 @@ core_scsi3_decode_spec_i_port(
 				continue;
 
 			atomic_inc(&tmp_tpg->tpg_pr_ref_count);
-			smp_mb__after_atomic_inc();
+			smp_mb__after_atomic();
 			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_dec();
+				smp_mb__after_atomic();
 				ret = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
 				goto out_unmap;
 			}
@@ -1573,7 +1573,7 @@ core_scsi3_decode_spec_i_port(
 						tmp_tpg, i_str);
 			if (dest_node_acl) {
 				atomic_inc(&dest_node_acl->acl_pr_ref_count);
-				smp_mb__after_atomic_inc();
+				smp_mb__after_atomic();
 			}
 			spin_unlock_irq(&tmp_tpg->acl_node_lock);
 
@@ -1587,7 +1587,7 @@ core_scsi3_decode_spec_i_port(
 				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_dec();
+				smp_mb__after_atomic();
 				core_scsi3_tpg_undepend_item(tmp_tpg);
 				ret = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
 				goto out_unmap;
@@ -1647,7 +1647,7 @@ core_scsi3_decode_spec_i_port(
 			pr_err("core_scsi3_lunacl_depend_item()"
 					" failed\n");
 			atomic_dec(&dest_se_deve->pr_ref_count);
-			smp_mb__after_atomic_dec();
+			smp_mb__after_atomic();
 			core_scsi3_nodeacl_undepend_item(dest_node_acl);
 			core_scsi3_tpg_undepend_item(dest_tpg);
 			ret = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
@@ -3168,14 +3168,14 @@ core_scsi3_emulate_pro_register_and_move(struct se_cmd *cmd, u64 res_key,
 			continue;
 
 		atomic_inc(&dest_se_tpg->tpg_pr_ref_count);
-		smp_mb__after_atomic_inc();
+		smp_mb__after_atomic();
 		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_dec();
+			smp_mb__after_atomic();
 			ret = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
 			goto out_put_pr_reg;
 		}
@@ -3273,7 +3273,7 @@ after_iport_check:
 				initiator_str);
 	if (dest_node_acl) {
 		atomic_inc(&dest_node_acl->acl_pr_ref_count);
-		smp_mb__after_atomic_inc();
+		smp_mb__after_atomic();
 	}
 	spin_unlock_irq(&dest_se_tpg->acl_node_lock);
 
@@ -3289,7 +3289,7 @@ after_iport_check:
 		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_dec();
+		smp_mb__after_atomic();
 		dest_node_acl = NULL;
 		ret = TCM_INVALID_PARAMETER_LIST;
 		goto out;
@@ -3314,7 +3314,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_dec();
+		smp_mb__after_atomic();
 		dest_se_deve = NULL;
 		ret = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
 		goto out;
@@ -3880,7 +3880,7 @@ core_scsi3_pri_read_full_status(struct se_cmd *cmd)
 		add_desc_len = 0;
 
 		atomic_inc(&pr_reg->pr_res_holders);
-		smp_mb__after_atomic_inc();
+		smp_mb__after_atomic();
 		spin_unlock(&pr_tmpl->registration_lock);
 		/*
 		 * Determine expected length of $FABRIC_MOD specific
@@ -3894,7 +3894,7 @@ core_scsi3_pri_read_full_status(struct se_cmd *cmd)
 				" 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_dec();
+			smp_mb__after_atomic();
 			break;
 		}
 		/*
@@ -3956,7 +3956,7 @@ core_scsi3_pri_read_full_status(struct se_cmd *cmd)
 
 		spin_lock(&pr_tmpl->registration_lock);
 		atomic_dec(&pr_reg->pr_res_holders);
-		smp_mb__after_atomic_dec();
+		smp_mb__after_atomic();
 		/*
 		 * Set the ADDITIONAL DESCRIPTOR LENGTH
 		 */
diff --git a/drivers/target/target_core_pscsi.c b/drivers/target/target_core_pscsi.c
index 0f199f6a073..94d00df28f3 100644
--- a/drivers/target/target_core_pscsi.c
+++ b/drivers/target/target_core_pscsi.c
@@ -1055,6 +1055,8 @@ pscsi_execute_cmd(struct se_cmd *cmd)
 			ret = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
 			goto fail;
 		}
+
+		blk_rq_set_block_pc(req);
 	} else {
 		BUG_ON(!cmd->data_length);
 
@@ -1071,7 +1073,6 @@ pscsi_execute_cmd(struct se_cmd *cmd)
 		}
 	}
 
-	req->cmd_type = REQ_TYPE_BLOCK_PC;
 	req->end_io = pscsi_req_done;
 	req->end_io_data = cmd;
 	req->cmd_len = scsi_command_size(pt->pscsi_cdb);
diff --git a/drivers/target/target_core_transport.c b/drivers/target/target_core_transport.c
index d4b98690a73..2179feed0d6 100644
--- a/drivers/target/target_core_transport.c
+++ b/drivers/target/target_core_transport.c
@@ -736,7 +736,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_dec();
+		smp_mb__after_atomic();
 
 		pr_debug("Processing %s cmd: %p QUEUE_FULL in work queue"
 			" context: %s\n", cmd->se_tfo->get_fabric_name(), cmd,
@@ -1113,6 +1113,7 @@ void transport_init_se_cmd(
 	init_completion(&cmd->cmd_wait_comp);
 	init_completion(&cmd->task_stop_comp);
 	spin_lock_init(&cmd->t_state_lock);
+	kref_init(&cmd->cmd_kref);
 	cmd->transport_state = CMD_T_DEV_ACTIVE;
 
 	cmd->se_tfo = tfo;
@@ -1148,7 +1149,7 @@ 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_inc();
+	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);
@@ -1705,7 +1706,7 @@ static bool target_handle_task_attr(struct se_cmd *cmd)
 		return false;
 	case MSG_ORDERED_TAG:
 		atomic_inc(&dev->dev_ordered_sync);
-		smp_mb__after_atomic_inc();
+		smp_mb__after_atomic();
 
 		pr_debug("Added ORDERED for CDB: 0x%02x to ordered list, "
 			 " se_ordered_id: %u\n",
@@ -1723,7 +1724,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_inc();
+		smp_mb__after_atomic();
 		break;
 	}
 
@@ -1828,7 +1829,7 @@ static void transport_complete_task_attr(struct se_cmd *cmd)
 
 	if (cmd->sam_task_attr == MSG_SIMPLE_TAG) {
 		atomic_dec(&dev->simple_cmds);
-		smp_mb__after_atomic_dec();
+		smp_mb__after_atomic();
 		dev->dev_cur_ordered_id++;
 		pr_debug("Incremented dev->dev_cur_ordered_id: %u for"
 			" SIMPLE: %u\n", dev->dev_cur_ordered_id,
@@ -1840,7 +1841,7 @@ static void transport_complete_task_attr(struct se_cmd *cmd)
 			cmd->se_ordered_id);
 	} else if (cmd->sam_task_attr == MSG_ORDERED_TAG) {
 		atomic_dec(&dev->dev_ordered_sync);
-		smp_mb__after_atomic_dec();
+		smp_mb__after_atomic();
 
 		dev->dev_cur_ordered_id++;
 		pr_debug("Incremented dev_cur_ordered_id: %u for ORDERED:"
@@ -1899,7 +1900,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_inc();
+	smp_mb__after_atomic();
 	spin_unlock_irq(&cmd->se_dev->qf_cmd_lock);
 
 	schedule_work(&cmd->se_dev->qf_work_queue);
@@ -2357,7 +2358,6 @@ int target_get_sess_cmd(struct se_session *se_sess, struct se_cmd *se_cmd,
 	unsigned long flags;
 	int ret = 0;
 
-	kref_init(&se_cmd->cmd_kref);
 	/*
 	 * Add a second kref if the fabric caller is expecting to handle
 	 * fabric acknowledgement that requires two target_put_sess_cmd()
@@ -2875,7 +2875,7 @@ 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_inc();
+			smp_mb__after_atomic();
 			return;
 		}
 	}
diff --git a/drivers/target/target_core_ua.c b/drivers/target/target_core_ua.c
index 505519b10cb..101858e245b 100644
--- a/drivers/target/target_core_ua.c
+++ b/drivers/target/target_core_ua.c
@@ -162,7 +162,7 @@ int core_scsi3_ua_allocate(
 		spin_unlock_irq(&nacl->device_list_lock);
 
 		atomic_inc(&deve->ua_count);
-		smp_mb__after_atomic_inc();
+		smp_mb__after_atomic();
 		return 0;
 	}
 	list_add_tail(&ua->ua_nacl_list, &deve->ua_list);
@@ -175,7 +175,7 @@ int core_scsi3_ua_allocate(
 		asc, ascq);
 
 	atomic_inc(&deve->ua_count);
-	smp_mb__after_atomic_inc();
+	smp_mb__after_atomic();
 	return 0;
 }
 
@@ -190,7 +190,7 @@ void core_scsi3_ua_release_all(
 		kmem_cache_free(se_ua_cache, ua);
 
 		atomic_dec(&deve->ua_count);
-		smp_mb__after_atomic_dec();
+		smp_mb__after_atomic();
 	}
 	spin_unlock(&deve->ua_lock);
 }
@@ -251,7 +251,7 @@ void core_scsi3_ua_for_check_condition(
 		kmem_cache_free(se_ua_cache, ua);
 
 		atomic_dec(&deve->ua_count);
-		smp_mb__after_atomic_dec();
+		smp_mb__after_atomic();
 	}
 	spin_unlock(&deve->ua_lock);
 	spin_unlock_irq(&nacl->device_list_lock);
@@ -310,7 +310,7 @@ int core_scsi3_ua_clear_for_request_sense(
 		kmem_cache_free(se_ua_cache, ua);
 
 		atomic_dec(&deve->ua_count);
-		smp_mb__after_atomic_dec();
+		smp_mb__after_atomic();
 	}
 	spin_unlock(&deve->ua_lock);
 	spin_unlock_irq(&nacl->device_list_lock);
diff --git a/drivers/target/tcm_fc/tfc_cmd.c b/drivers/target/tcm_fc/tfc_cmd.c
index 01cf37f212c..f5fd515b2be 100644
--- a/drivers/target/tcm_fc/tfc_cmd.c
+++ b/drivers/target/tcm_fc/tfc_cmd.c
@@ -90,18 +90,18 @@ static void ft_free_cmd(struct ft_cmd *cmd)
 {
 	struct fc_frame *fp;
 	struct fc_lport *lport;
-	struct se_session *se_sess;
+	struct ft_sess *sess;
 
 	if (!cmd)
 		return;
-	se_sess = cmd->sess->se_sess;
+	sess = cmd->sess;
 	fp = cmd->req_frame;
 	lport = fr_dev(fp);
 	if (fr_seq(fp))
 		lport->tt.seq_release(fr_seq(fp));
 	fc_frame_free(fp);
-	percpu_ida_free(&se_sess->sess_tag_pool, cmd->se_cmd.map_tag);
-	ft_sess_put(cmd->sess);	/* undo get from lookup at recv */
+	percpu_ida_free(&sess->se_sess->sess_tag_pool, cmd->se_cmd.map_tag);
+	ft_sess_put(sess);	/* undo get from lookup at recv */
 }
 
 void ft_release_cmd(struct se_cmd *se_cmd)
diff --git a/drivers/thermal/cpu_cooling.c b/drivers/thermal/cpu_cooling.c
index 4246262c4bd..84a75f89bf7 100644
--- a/drivers/thermal/cpu_cooling.c
+++ b/drivers/thermal/cpu_cooling.c
@@ -144,11 +144,11 @@ static int get_property(unsigned int cpu, unsigned long input,
 			unsigned int *output,
 			enum cpufreq_cooling_property property)
 {
-	int i, j;
+	int i;
 	unsigned long max_level = 0, level = 0;
 	unsigned int freq = CPUFREQ_ENTRY_INVALID;
 	int descend = -1;
-	struct cpufreq_frequency_table *table =
+	struct cpufreq_frequency_table *pos, *table =
 					cpufreq_frequency_get_table(cpu);
 
 	if (!output)
@@ -157,20 +157,16 @@ static int get_property(unsigned int cpu, unsigned long input,
 	if (!table)
 		return -EINVAL;
 
-	for (i = 0; table[i].frequency != CPUFREQ_TABLE_END; i++) {
-		/* ignore invalid entries */
-		if (table[i].frequency == CPUFREQ_ENTRY_INVALID)
-			continue;
-
+	cpufreq_for_each_valid_entry(pos, table) {
 		/* ignore duplicate entry */
-		if (freq == table[i].frequency)
+		if (freq == pos->frequency)
 			continue;
 
 		/* get the frequency order */
 		if (freq != CPUFREQ_ENTRY_INVALID && descend == -1)
-			descend = !!(freq > table[i].frequency);
+			descend = freq > pos->frequency;
 
-		freq = table[i].frequency;
+		freq = pos->frequency;
 		max_level++;
 	}
 
@@ -190,29 +186,26 @@ static int get_property(unsigned int cpu, unsigned long input,
 	if (property == GET_FREQ)
 		level = descend ? input : (max_level - input);
 
-	for (i = 0, j = 0; table[i].frequency != CPUFREQ_TABLE_END; i++) {
-		/* ignore invalid entry */
-		if (table[i].frequency == CPUFREQ_ENTRY_INVALID)
-			continue;
-
+	i = 0;
+	cpufreq_for_each_valid_entry(pos, table) {
 		/* ignore duplicate entry */
-		if (freq == table[i].frequency)
+		if (freq == pos->frequency)
 			continue;
 
 		/* now we have a valid frequency entry */
-		freq = table[i].frequency;
+		freq = pos->frequency;
 
 		if (property == GET_LEVEL && (unsigned int)input == freq) {
 			/* get level by frequency */
-			*output = descend ? j : (max_level - j);
+			*output = descend ? i : (max_level - i);
 			return 0;
 		}
-		if (property == GET_FREQ && level == j) {
+		if (property == GET_FREQ && level == i) {
 			/* get frequency by level */
 			*output = freq;
 			return 0;
 		}
-		j++;
+		i++;
 	}
 
 	return -EINVAL;
diff --git a/drivers/tty/goldfish.c b/drivers/tty/goldfish.c
index 75dc9d25f32..09495f515fa 100644
--- a/drivers/tty/goldfish.c
+++ b/drivers/tty/goldfish.c
@@ -21,6 +21,7 @@
 #include <linux/slab.h>
 #include <linux/io.h>
 #include <linux/module.h>
+#include <linux/goldfish.h>
 
 enum {
 	GOLDFISH_TTY_PUT_CHAR       = 0x00,
@@ -29,6 +30,7 @@ enum {
 
 	GOLDFISH_TTY_DATA_PTR       = 0x10,
 	GOLDFISH_TTY_DATA_LEN       = 0x14,
+	GOLDFISH_TTY_DATA_PTR_HIGH  = 0x18,
 
 	GOLDFISH_TTY_CMD_INT_DISABLE    = 0,
 	GOLDFISH_TTY_CMD_INT_ENABLE     = 1,
@@ -57,7 +59,8 @@ static void goldfish_tty_do_write(int line, const char *buf, unsigned count)
 	struct goldfish_tty *qtty = &goldfish_ttys[line];
 	void __iomem *base = qtty->base;
 	spin_lock_irqsave(&qtty->lock, irq_flags);
-	writel((u32)buf, base + GOLDFISH_TTY_DATA_PTR);
+	gf_write64((u64)buf, base + GOLDFISH_TTY_DATA_PTR,
+				base + GOLDFISH_TTY_DATA_PTR_HIGH);
 	writel(count, base + GOLDFISH_TTY_DATA_LEN);
 	writel(GOLDFISH_TTY_CMD_WRITE_BUFFER, base + GOLDFISH_TTY_CMD);
 	spin_unlock_irqrestore(&qtty->lock, irq_flags);
@@ -73,12 +76,13 @@ static irqreturn_t goldfish_tty_interrupt(int irq, void *dev_id)
 	u32 count;
 
 	count = readl(base + GOLDFISH_TTY_BYTES_READY);
-	if(count == 0)
+	if (count == 0)
 		return IRQ_NONE;
 
 	count = tty_prepare_flip_string(&qtty->port, &buf, count);
 	spin_lock_irqsave(&qtty->lock, irq_flags);
-	writel((u32)buf, base + GOLDFISH_TTY_DATA_PTR);
+	gf_write64((u64)buf, base + GOLDFISH_TTY_DATA_PTR,
+				base + GOLDFISH_TTY_DATA_PTR_HIGH);
 	writel(count, base + GOLDFISH_TTY_DATA_LEN);
 	writel(GOLDFISH_TTY_CMD_READ_BUFFER, base + GOLDFISH_TTY_CMD);
 	spin_unlock_irqrestore(&qtty->lock, irq_flags);
@@ -88,24 +92,26 @@ static irqreturn_t goldfish_tty_interrupt(int irq, void *dev_id)
 
 static int goldfish_tty_activate(struct tty_port *port, struct tty_struct *tty)
 {
-	struct goldfish_tty *qtty = container_of(port, struct goldfish_tty, port);
+	struct goldfish_tty *qtty = container_of(port, struct goldfish_tty,
+									port);
 	writel(GOLDFISH_TTY_CMD_INT_ENABLE, qtty->base + GOLDFISH_TTY_CMD);
 	return 0;
 }
 
 static void goldfish_tty_shutdown(struct tty_port *port)
 {
-	struct goldfish_tty *qtty = container_of(port, struct goldfish_tty, port);
+	struct goldfish_tty *qtty = container_of(port, struct goldfish_tty,
+									port);
 	writel(GOLDFISH_TTY_CMD_INT_DISABLE, qtty->base + GOLDFISH_TTY_CMD);
 }
 
-static int goldfish_tty_open(struct tty_struct * tty, struct file * filp)
+static int goldfish_tty_open(struct tty_struct *tty, struct file *filp)
 {
 	struct goldfish_tty *qtty = &goldfish_ttys[tty->index];
 	return tty_port_open(&qtty->port, tty, filp);
 }
 
-static void goldfish_tty_close(struct tty_struct * tty, struct file * filp)
+static void goldfish_tty_close(struct tty_struct *tty, struct file *filp)
 {
 	tty_port_close(tty->port, tty, filp);
 }
@@ -115,7 +121,8 @@ static void goldfish_tty_hangup(struct tty_struct *tty)
 	tty_port_hangup(tty->port);
 }
 
-static int goldfish_tty_write(struct tty_struct * tty, const unsigned char *buf, int count)
+static int goldfish_tty_write(struct tty_struct *tty, const unsigned char *buf,
+								int count)
 {
 	goldfish_tty_do_write(tty->index, buf, count);
 	return count;
@@ -133,12 +140,14 @@ static int goldfish_tty_chars_in_buffer(struct tty_struct *tty)
 	return readl(base + GOLDFISH_TTY_BYTES_READY);
 }
 
-static void goldfish_tty_console_write(struct console *co, const char *b, unsigned count)
+static void goldfish_tty_console_write(struct console *co, const char *b,
+								unsigned count)
 {
 	goldfish_tty_do_write(co->index, b, count);
 }
 
-static struct tty_driver *goldfish_tty_console_device(struct console *c, int *index)
+static struct tty_driver *goldfish_tty_console_device(struct console *c,
+								int *index)
 {
 	*index = c->index;
 	return goldfish_tty_driver;
@@ -146,9 +155,9 @@ static struct tty_driver *goldfish_tty_console_device(struct console *c, int *in
 
 static int goldfish_tty_console_setup(struct console *co, char *options)
 {
-	if((unsigned)co->index > goldfish_tty_line_count)
+	if ((unsigned)co->index > goldfish_tty_line_count)
 		return -ENODEV;
-	if(goldfish_ttys[co->index].base == 0)
+	if (goldfish_ttys[co->index].base == 0)
 		return -ENODEV;
 	return 0;
 }
@@ -158,7 +167,7 @@ static struct tty_port_operations goldfish_port_ops = {
 	.shutdown = goldfish_tty_shutdown
 };
 
-static struct tty_operations goldfish_tty_ops = {
+static const struct tty_operations goldfish_tty_ops = {
 	.open = goldfish_tty_open,
 	.close = goldfish_tty_close,
 	.hangup = goldfish_tty_hangup,
@@ -172,13 +181,14 @@ static int goldfish_tty_create_driver(void)
 	int ret;
 	struct tty_driver *tty;
 
-	goldfish_ttys = kzalloc(sizeof(*goldfish_ttys) * goldfish_tty_line_count, GFP_KERNEL);
-	if(goldfish_ttys == NULL) {
+	goldfish_ttys = kzalloc(sizeof(*goldfish_ttys) *
+				goldfish_tty_line_count, GFP_KERNEL);
+	if (goldfish_ttys == NULL) {
 		ret = -ENOMEM;
 		goto err_alloc_goldfish_ttys_failed;
 	}
 	tty = alloc_tty_driver(goldfish_tty_line_count);
-	if(tty == NULL) {
+	if (tty == NULL) {
 		ret = -ENOMEM;
 		goto err_alloc_tty_driver_failed;
 	}
@@ -187,10 +197,11 @@ static int goldfish_tty_create_driver(void)
 	tty->type = TTY_DRIVER_TYPE_SERIAL;
 	tty->subtype = SERIAL_TYPE_NORMAL;
 	tty->init_termios = tty_std_termios;
-	tty->flags = TTY_DRIVER_RESET_TERMIOS | TTY_DRIVER_REAL_RAW | TTY_DRIVER_DYNAMIC_DEV;
+	tty->flags = TTY_DRIVER_RESET_TERMIOS | TTY_DRIVER_REAL_RAW |
+						TTY_DRIVER_DYNAMIC_DEV;
 	tty_set_operations(tty, &goldfish_tty_ops);
 	ret = tty_register_driver(tty);
-	if(ret)
+	if (ret)
 		goto err_tty_register_driver_failed;
 
 	goldfish_tty_driver = tty;
@@ -225,7 +236,7 @@ static int goldfish_tty_probe(struct platform_device *pdev)
 	u32 irq;
 
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if(r == NULL)
+	if (r == NULL)
 		return -EINVAL;
 
 	base = ioremap(r->start, 0x1000);
@@ -233,18 +244,18 @@ static int goldfish_tty_probe(struct platform_device *pdev)
 		pr_err("goldfish_tty: unable to remap base\n");
 
 	r = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if(r == NULL)
+	if (r == NULL)
 		goto err_unmap;
 
 	irq = r->start;
 
-	if(pdev->id >= goldfish_tty_line_count)
+	if (pdev->id >= goldfish_tty_line_count)
 		goto err_unmap;
 
 	mutex_lock(&goldfish_tty_lock);
-	if(goldfish_tty_current_line_count == 0) {
+	if (goldfish_tty_current_line_count == 0) {
 		ret = goldfish_tty_create_driver();
-		if(ret)
+		if (ret)
 			goto err_create_driver_failed;
 	}
 	goldfish_tty_current_line_count++;
@@ -258,14 +269,15 @@ static int goldfish_tty_probe(struct platform_device *pdev)
 
 	writel(GOLDFISH_TTY_CMD_INT_DISABLE, base + GOLDFISH_TTY_CMD);
 
-	ret = request_irq(irq, goldfish_tty_interrupt, IRQF_SHARED, "goldfish_tty", pdev);
-	if(ret)
+	ret = request_irq(irq, goldfish_tty_interrupt, IRQF_SHARED,
+						"goldfish_tty", pdev);
+	if (ret)
 		goto err_request_irq_failed;
 
 
 	ttydev = tty_port_register_device(&qtty->port, goldfish_tty_driver,
 							pdev->id, &pdev->dev);
-	if(IS_ERR(ttydev)) {
+	if (IS_ERR(ttydev)) {
 		ret = PTR_ERR(ttydev);
 		goto err_tty_register_device_failed;
 	}
@@ -286,7 +298,7 @@ err_tty_register_device_failed:
 	free_irq(irq, pdev);
 err_request_irq_failed:
 	goldfish_tty_current_line_count--;
-	if(goldfish_tty_current_line_count == 0)
+	if (goldfish_tty_current_line_count == 0)
 		goldfish_tty_delete_driver();
 err_create_driver_failed:
 	mutex_unlock(&goldfish_tty_lock);
@@ -308,7 +320,7 @@ static int goldfish_tty_remove(struct platform_device *pdev)
 	qtty->base = 0;
 	free_irq(qtty->irq, pdev);
 	goldfish_tty_current_line_count--;
-	if(goldfish_tty_current_line_count == 0)
+	if (goldfish_tty_current_line_count == 0)
 		goldfish_tty_delete_driver();
 	mutex_unlock(&goldfish_tty_lock);
 	return 0;
diff --git a/drivers/tty/hvc/hvc_console.c b/drivers/tty/hvc/hvc_console.c
index 0ff7fda0742..4fcec1d793a 100644
--- a/drivers/tty/hvc/hvc_console.c
+++ b/drivers/tty/hvc/hvc_console.c
@@ -760,10 +760,17 @@ static int khvcd(void *unused)
 			if (poll_mask == 0)
 				schedule();
 			else {
+				unsigned long j_timeout;
+
 				if (timeout < MAX_TIMEOUT)
 					timeout += (timeout >> 6) + 1;
 
-				msleep_interruptible(timeout);
+				/*
+				 * We don't use msleep_interruptible otherwise
+				 * "kick" will fail to wake us up
+				 */
+				j_timeout = msecs_to_jiffies(timeout) + 1;
+				schedule_timeout_interruptible(j_timeout);
 			}
 		}
 		__set_current_state(TASK_RUNNING);
diff --git a/drivers/tty/hvc/hvc_dcc.c b/drivers/tty/hvc/hvc_dcc.c
index 3502a7bbb69..809920d80a6 100644
--- a/drivers/tty/hvc/hvc_dcc.c
+++ b/drivers/tty/hvc/hvc_dcc.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2010, Code Aurora Forum. All rights reserved.
+/* Copyright (c) 2010, 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
@@ -8,20 +8,11 @@
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have 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/console.h>
-#include <linux/delay.h>
-#include <linux/err.h>
 #include <linux/init.h>
-#include <linux/moduleparam.h>
-#include <linux/types.h>
 
+#include <asm/dcc.h>
 #include <asm/processor.h>
 
 #include "hvc_console.h"
@@ -30,35 +21,6 @@
 #define DCC_STATUS_RX		(1 << 30)
 #define DCC_STATUS_TX		(1 << 29)
 
-static inline u32 __dcc_getstatus(void)
-{
-	u32 __ret;
-	asm volatile("mrc p14, 0, %0, c0, c1, 0	@ read comms ctrl reg"
-		: "=r" (__ret) : : "cc");
-
-	return __ret;
-}
-
-
-static inline char __dcc_getchar(void)
-{
-	char __c;
-
-	asm volatile("mrc p14, 0, %0, c0, c5, 0	@ read comms data reg"
-		: "=r" (__c));
-	isb();
-
-	return __c;
-}
-
-static inline void __dcc_putchar(char c)
-{
-	asm volatile("mcr p14, 0, %0, c0, c5, 0	@ write a char"
-		: /* no output register */
-		: "r" (c));
-	isb();
-}
-
 static int hvc_dcc_put_chars(uint32_t vt, const char *buf, int count)
 {
 	int i;
diff --git a/drivers/tty/hvc/hvc_tile.c b/drivers/tty/hvc/hvc_tile.c
index af8cdaa1dcb..147d49e95db 100644
--- a/drivers/tty/hvc/hvc_tile.c
+++ b/drivers/tty/hvc/hvc_tile.c
@@ -133,14 +133,14 @@ static int hvc_tile_probe(struct platform_device *pdev)
 	int tile_hvc_irq;
 
 	/* Create our IRQ and register it. */
-	tile_hvc_irq = create_irq();
-	if (tile_hvc_irq < 0)
+	tile_hvc_irq = irq_alloc_hwirq(-1);
+	if (!tile_hvc_irq)
 		return -ENXIO;
 
 	tile_irq_activate(tile_hvc_irq, TILE_IRQ_PERCPU);
 	hp = hvc_alloc(0, tile_hvc_irq, &hvc_tile_get_put_ops, 128);
 	if (IS_ERR(hp)) {
-		destroy_irq(tile_hvc_irq);
+		irq_free_hwirq(tile_hvc_irq);
 		return PTR_ERR(hp);
 	}
 	dev_set_drvdata(&pdev->dev, hp);
@@ -155,7 +155,7 @@ static int hvc_tile_remove(struct platform_device *pdev)
 
 	rc = hvc_remove(hp);
 	if (rc == 0)
-		destroy_irq(hp->data);
+		irq_free_hwirq(hp->data);
 
 	return rc;
 }
diff --git a/drivers/tty/n_hdlc.c b/drivers/tty/n_hdlc.c
index 1b2db9a3038..644ddb841d9 100644
--- a/drivers/tty/n_hdlc.c
+++ b/drivers/tty/n_hdlc.c
@@ -848,13 +848,11 @@ static struct n_hdlc *n_hdlc_alloc(void)
 {
 	struct n_hdlc_buf *buf;
 	int i;
-	struct n_hdlc *n_hdlc = kmalloc(sizeof(*n_hdlc), GFP_KERNEL);
+	struct n_hdlc *n_hdlc = kzalloc(sizeof(*n_hdlc), GFP_KERNEL);
 
 	if (!n_hdlc)
 		return NULL;
 
-	memset(n_hdlc, 0, sizeof(*n_hdlc));
-
 	n_hdlc_buf_list_init(&n_hdlc->rx_free_buf_list);
 	n_hdlc_buf_list_init(&n_hdlc->tx_free_buf_list);
 	n_hdlc_buf_list_init(&n_hdlc->rx_buf_list);
@@ -952,8 +950,6 @@ static char hdlc_register_ok[] __initdata =
 	KERN_INFO "N_HDLC line discipline registered.\n";
 static char hdlc_register_fail[] __initdata =
 	KERN_ERR "error registering line discipline: %d\n";
-static char hdlc_init_fail[] __initdata =
-	KERN_INFO "N_HDLC: init failure %d\n";
 
 static int __init n_hdlc_init(void)
 {
@@ -973,8 +969,6 @@ static int __init n_hdlc_init(void)
 	else
 		printk(hdlc_register_fail, status);
 
-	if (status)
-		printk(hdlc_init_fail, status);
 	return status;
 	
 }	/* end of init_module() */
diff --git a/drivers/tty/n_tty.c b/drivers/tty/n_tty.c
index fe9d129c873..f95569dedc8 100644
--- a/drivers/tty/n_tty.c
+++ b/drivers/tty/n_tty.c
@@ -2041,7 +2041,7 @@ static int canon_copy_from_read_buf(struct tty_struct *tty,
 
 	if (found)
 		clear_bit(eol, ldata->read_flags);
-	smp_mb__after_clear_bit();
+	smp_mb__after_atomic();
 	ldata->read_tail += c;
 
 	if (found) {
diff --git a/drivers/tty/serial/8250/8250_core.c b/drivers/tty/serial/8250/8250_core.c
index 2d4bd3929e5..27f7ad6b74c 100644
--- a/drivers/tty/serial/8250/8250_core.c
+++ b/drivers/tty/serial/8250/8250_core.c
@@ -1926,13 +1926,8 @@ static void serial8250_put_poll_char(struct uart_port *port,
 	wait_for_xmitr(up, BOTH_EMPTY);
 	/*
 	 *	Send the character out.
-	 *	If a LF, also do CR...
 	 */
 	serial_port_out(port, UART_TX, c);
-	if (c == 10) {
-		wait_for_xmitr(up, BOTH_EMPTY);
-		serial_port_out(port, UART_TX, 13);
-	}
 
 	/*
 	 *	Finally, wait for transmitter to become empty
@@ -2338,9 +2333,11 @@ 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.
+	 * UART to respond.  IOW, at least 32 bytes of FIFO. Also enable
+	 * AFE if hw flow control is supported
 	 */
-	if (up->capabilities & UART_CAP_AFE && port->fifosize >= 32) {
+	if ((up->capabilities & UART_CAP_AFE && (port->fifosize >= 32)) ||
+	    (port->flags & UPF_HARD_FLOW)) {
 		up->mcr &= ~UART_MCR_AFE;
 		if (termios->c_cflag & CRTSCTS)
 			up->mcr |= UART_MCR_AFE;
diff --git a/drivers/tty/serial/8250/8250_dma.c b/drivers/tty/serial/8250/8250_dma.c
index ab9096dc384..148ffe4c232 100644
--- a/drivers/tty/serial/8250/8250_dma.c
+++ b/drivers/tty/serial/8250/8250_dma.c
@@ -192,21 +192,28 @@ int serial8250_request_dma(struct uart_8250_port *p)
 
 	dma->rx_buf = dma_alloc_coherent(dma->rxchan->device->dev, dma->rx_size,
 					&dma->rx_addr, GFP_KERNEL);
-	if (!dma->rx_buf) {
-		dma_release_channel(dma->rxchan);
-		dma_release_channel(dma->txchan);
-		return -ENOMEM;
-	}
+	if (!dma->rx_buf)
+		goto err;
 
 	/* TX buffer */
 	dma->tx_addr = dma_map_single(dma->txchan->device->dev,
 					p->port.state->xmit.buf,
 					UART_XMIT_SIZE,
 					DMA_TO_DEVICE);
+	if (dma_mapping_error(dma->txchan->device->dev, dma->tx_addr)) {
+		dma_free_coherent(dma->rxchan->device->dev, dma->rx_size,
+				  dma->rx_buf, dma->rx_addr);
+		goto err;
+	}
 
 	dev_dbg_ratelimited(p->port.dev, "got both dma channels\n");
 
 	return 0;
+err:
+	dma_release_channel(dma->rxchan);
+	dma_release_channel(dma->txchan);
+
+	return -ENOMEM;
 }
 EXPORT_SYMBOL_GPL(serial8250_request_dma);
 
diff --git a/drivers/tty/serial/8250/8250_dw.c b/drivers/tty/serial/8250/8250_dw.c
index ed311357674..51b307aab75 100644
--- a/drivers/tty/serial/8250/8250_dw.c
+++ b/drivers/tty/serial/8250/8250_dw.c
@@ -62,6 +62,70 @@ struct dw8250_data {
 	struct uart_8250_dma	dma;
 };
 
+struct dw8250_acpi_desc {
+	void (*set_termios)(struct uart_port *p, struct ktermios *termios,
+			    struct ktermios *old);
+};
+
+#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)
+
+static void byt_set_termios(struct uart_port *p, struct ktermios *termios,
+			    struct ktermios *old)
+{
+	unsigned int baud = tty_termios_baud_rate(termios);
+	unsigned int m, n;
+	u32 reg;
+
+	/*
+	* For baud rates 0.5M, 1M, 1.5M, 2M, 2.5M, 3M, 3.5M and 4M the
+	* dividers must be adjusted.
+	*
+	* uartclk = (m / n) * 100 MHz, where m <= n
+	*/
+	switch (baud) {
+	case 500000:
+	case 1000000:
+	case 2000000:
+	case 4000000:
+		m = 64;
+		n = 100;
+		p->uartclk = 64000000;
+		break;
+	case 3500000:
+		m = 56;
+		n = 100;
+		p->uartclk = 56000000;
+		break;
+	case 1500000:
+	case 3000000:
+		m = 48;
+		n = 100;
+		p->uartclk = 48000000;
+		break;
+	case 2500000:
+		m = 40;
+		n = 100;
+		p->uartclk = 40000000;
+		break;
+	default:
+		m = 2304;
+		n = 3125;
+		p->uartclk = 73728000;
+	}
+
+	/* Reset the clock */
+	reg = (m << BYT_PRV_CLK_M_VAL_SHIFT) | (n << BYT_PRV_CLK_N_VAL_SHIFT);
+	writel(reg, p->membase + BYT_PRV_CLK);
+	reg |= BYT_PRV_CLK_EN | BYT_PRV_CLK_UPDATE;
+	writel(reg, p->membase + BYT_PRV_CLK);
+
+	serial8250_do_set_termios(p, termios, old);
+}
+
 static inline int dw8250_modify_msr(struct uart_port *p, int offset, int value)
 {
 	struct dw8250_data *d = p->private_data;
@@ -278,6 +342,7 @@ static int dw8250_probe_acpi(struct uart_8250_port *up,
 {
 	const struct acpi_device_id *id;
 	struct uart_port *p = &up->port;
+	struct dw8250_acpi_desc *acpi_desc;
 
 	dw8250_setup_port(up);
 
@@ -290,14 +355,18 @@ static int dw8250_probe_acpi(struct uart_8250_port *up,
 	p->serial_out = dw8250_serial_out32;
 	p->regshift = 2;
 
-	if (!p->uartclk)
-		p->uartclk = (unsigned int)id->driver_data;
-
 	up->dma = &data->dma;
 
 	up->dma->rxconf.src_maxburst = p->fifosize / 4;
 	up->dma->txconf.dst_maxburst = p->fifosize / 4;
 
+	acpi_desc = (struct dw8250_acpi_desc *)id->driver_data;
+	if (!acpi_desc)
+		return 0;
+
+	if (acpi_desc->set_termios)
+		p->set_termios = acpi_desc->set_termios;
+
 	return 0;
 }
 
@@ -445,12 +514,16 @@ static const struct of_device_id dw8250_of_match[] = {
 };
 MODULE_DEVICE_TABLE(of, dw8250_of_match);
 
+static struct dw8250_acpi_desc byt_8250_desc = {
+	.set_termios = byt_set_termios,
+};
+
 static const struct acpi_device_id dw8250_acpi_match[] = {
 	{ "INT33C4", 0 },
 	{ "INT33C5", 0 },
 	{ "INT3434", 0 },
 	{ "INT3435", 0 },
-	{ "80860F0A", 0 },
+	{ "80860F0A", (kernel_ulong_t)&byt_8250_desc},
 	{ },
 };
 MODULE_DEVICE_TABLE(acpi, dw8250_acpi_match);
diff --git a/drivers/tty/serial/8250/8250_early.c b/drivers/tty/serial/8250/8250_early.c
index c100d6343d5..cfef801a49d 100644
--- a/drivers/tty/serial/8250/8250_early.c
+++ b/drivers/tty/serial/8250/8250_early.c
@@ -35,18 +35,8 @@
 #include <linux/serial_8250.h>
 #include <asm/io.h>
 #include <asm/serial.h>
-#ifdef CONFIG_FIX_EARLYCON_MEM
-#include <asm/pgtable.h>
-#include <asm/fixmap.h>
-#endif
 
-struct early_serial8250_device {
-	struct uart_port port;
-	char options[16];		/* e.g., 115200n8 */
-	unsigned int baud;
-};
-
-static struct early_serial8250_device early_device;
+static struct earlycon_device *early_device;
 
 unsigned int __weak __init serial8250_early_in(struct uart_port *port, int offset)
 {
@@ -100,7 +90,7 @@ static void __init serial_putc(struct uart_port *port, int c)
 static void __init early_serial8250_write(struct console *console,
 					const char *s, unsigned int count)
 {
-	struct uart_port *port = &early_device.port;
+	struct uart_port *port = &early_device->port;
 	unsigned int ier;
 
 	/* Save the IER and disable interrupts */
@@ -129,7 +119,7 @@ static unsigned int __init probe_baud(struct uart_port *port)
 	return (port->uartclk / 16) / quot;
 }
 
-static void __init init_port(struct early_serial8250_device *device)
+static void __init init_port(struct earlycon_device *device)
 {
 	struct uart_port *port = &device->port;
 	unsigned int divisor;
@@ -148,128 +138,42 @@ static void __init init_port(struct early_serial8250_device *device)
 	serial8250_early_out(port, UART_LCR, c & ~UART_LCR_DLAB);
 }
 
-static int __init parse_options(struct early_serial8250_device *device,
-								char *options)
+static int __init early_serial8250_setup(struct earlycon_device *device,
+					 const char *options)
 {
-	struct uart_port *port = &device->port;
-	int mmio, mmio32, length;
-
-	if (!options)
-		return -ENODEV;
-
-	port->uartclk = BASE_BAUD * 16;
-
-	mmio = !strncmp(options, "mmio,", 5);
-	mmio32 = !strncmp(options, "mmio32,", 7);
-	if (mmio || mmio32) {
-		port->iotype = (mmio ? UPIO_MEM : UPIO_MEM32);
-		port->mapbase = simple_strtoul(options + (mmio ? 5 : 7),
-					       &options, 0);
-		if (mmio32)
-			port->regshift = 2;
-#ifdef CONFIG_FIX_EARLYCON_MEM
-		set_fixmap_nocache(FIX_EARLYCON_MEM_BASE,
-					port->mapbase & PAGE_MASK);
-		port->membase =
-			(void __iomem *)__fix_to_virt(FIX_EARLYCON_MEM_BASE);
-		port->membase += port->mapbase & ~PAGE_MASK;
-#else
-		port->membase = ioremap_nocache(port->mapbase, 64);
-		if (!port->membase) {
-			printk(KERN_ERR "%s: Couldn't ioremap 0x%llx\n",
-				__func__,
-			       (unsigned long long) port->mapbase);
-			return -ENOMEM;
-		}
-#endif
-	} else if (!strncmp(options, "io,", 3)) {
-		port->iotype = UPIO_PORT;
-		port->iobase = simple_strtoul(options + 3, &options, 0);
-		mmio = 0;
-	} else
-		return -EINVAL;
-
-	options = strchr(options, ',');
-	if (options) {
-		options++;
-		device->baud = simple_strtoul(options, NULL, 0);
-		length = min(strcspn(options, " ") + 1,
-			     (size_t)(sizeof(device->options)));
-		strlcpy(device->options, options, length);
-	} else {
-		device->baud = probe_baud(port);
-		snprintf(device->options, sizeof(device->options), "%u",
-			device->baud);
-	}
-
-	if (mmio || mmio32)
-		printk(KERN_INFO
-		       "Early serial console at MMIO%s 0x%llx (options '%s')\n",
-			mmio32 ? "32" : "",
-			(unsigned long long)port->mapbase,
-			device->options);
-	else
-		printk(KERN_INFO
-		      "Early serial console at I/O port 0x%lx (options '%s')\n",
-			port->iobase,
-			device->options);
-
-	return 0;
-}
-
-static struct console early_serial8250_console __initdata = {
-	.name	= "uart",
-	.write	= early_serial8250_write,
-	.flags	= CON_PRINTBUFFER | CON_BOOT,
-	.index	= -1,
-};
-
-static int __init early_serial8250_setup(char *options)
-{
-	struct early_serial8250_device *device = &early_device;
-	int err;
-
-	if (device->port.membase || device->port.iobase)
+	if (!(device->port.membase || device->port.iobase))
 		return 0;
 
-	err = parse_options(device, options);
-	if (err < 0)
-		return err;
+	if (!device->baud)
+		device->baud = probe_baud(&device->port);
 
 	init_port(device);
+
+	early_device = device;
+	device->con->write = early_serial8250_write;
 	return 0;
 }
+EARLYCON_DECLARE(uart8250, early_serial8250_setup);
+EARLYCON_DECLARE(uart, early_serial8250_setup);
 
 int __init setup_early_serial8250_console(char *cmdline)
 {
-	char *options;
-	int err;
+	char match[] = "uart8250";
 
-	options = strstr(cmdline, "uart8250,");
-	if (!options) {
-		options = strstr(cmdline, "uart,");
-		if (!options)
-			return 0;
-	}
+	if (cmdline && cmdline[4] == ',')
+		match[4] = '\0';
 
-	options = strchr(cmdline, ',') + 1;
-	err = early_serial8250_setup(options);
-	if (err < 0)
-		return err;
-
-	register_console(&early_serial8250_console);
-
-	return 0;
+	return setup_earlycon(cmdline, match, early_serial8250_setup);
 }
 
 int serial8250_find_port_for_earlycon(void)
 {
-	struct early_serial8250_device *device = &early_device;
-	struct uart_port *port = &device->port;
+	struct earlycon_device *device = early_device;
+	struct uart_port *port = device ? &device->port : NULL;
 	int line;
 	int ret;
 
-	if (!device->port.membase && !device->port.iobase)
+	if (!port || (!port->membase && !port->iobase))
 		return -ENODEV;
 
 	line = serial8250_find_port(port);
@@ -284,5 +188,3 @@ int serial8250_find_port_for_earlycon(void)
 
 	return ret;
 }
-
-early_param("earlycon", setup_early_serial8250_console);
diff --git a/drivers/tty/serial/8250/8250_pci.c b/drivers/tty/serial/8250/8250_pci.c
index b14bcba96c2..33137b3ba94 100644
--- a/drivers/tty/serial/8250/8250_pci.c
+++ b/drivers/tty/serial/8250/8250_pci.c
@@ -1753,6 +1753,8 @@ pci_wch_ch353_setup(struct serial_private *priv,
 #define PCI_VENDOR_ID_ADVANTECH		0x13fe
 #define PCI_DEVICE_ID_INTEL_CE4100_UART 0x2e66
 #define PCI_DEVICE_ID_ADVANTECH_PCI3620	0x3620
+#define PCI_DEVICE_ID_ADVANTECH_PCI3618	0x3618
+#define PCI_DEVICE_ID_ADVANTECH_PCIf618	0xf618
 #define PCI_DEVICE_ID_TITAN_200I	0x8028
 #define PCI_DEVICE_ID_TITAN_400I	0x8048
 #define PCI_DEVICE_ID_TITAN_800I	0x8088
@@ -1778,6 +1780,7 @@ pci_wch_ch353_setup(struct serial_private *priv,
 #define PCI_DEVICE_ID_WCH_CH352_2S	0x3253
 #define PCI_DEVICE_ID_WCH_CH353_4S	0x3453
 #define PCI_DEVICE_ID_WCH_CH353_2S1PF	0x5046
+#define PCI_DEVICE_ID_WCH_CH353_1S1P	0x5053
 #define PCI_DEVICE_ID_WCH_CH353_2S1P	0x7053
 #define PCI_VENDOR_ID_AGESTAR		0x5372
 #define PCI_DEVICE_ID_AGESTAR_9375	0x6872
@@ -2410,6 +2413,14 @@ static struct pci_serial_quirk pci_serial_quirks[] __refdata = {
 		.subdevice	= PCI_ANY_ID,
 		.setup		= pci_omegapci_setup,
 	},
+	/* WCH CH353 1S1P card (16550 clone) */
+	{
+		.vendor         = PCI_VENDOR_ID_WCH,
+		.device         = PCI_DEVICE_ID_WCH_CH353_1S1P,
+		.subvendor      = PCI_ANY_ID,
+		.subdevice      = PCI_ANY_ID,
+		.setup          = pci_wch_ch353_setup,
+	},
 	/* WCH CH353 2S1P card (16550 clone) */
 	{
 		.vendor         = PCI_VENDOR_ID_WCH,
@@ -3526,6 +3537,7 @@ static const struct pci_device_id blacklist[] = {
 
 	/* multi-io cards handled by parport_serial */
 	{ PCI_DEVICE(0x4348, 0x7053), }, /* WCH CH353 2S1P */
+	{ PCI_DEVICE(0x4348, 0x5053), }, /* WCH CH353 1S1P */
 };
 
 /*
@@ -3880,6 +3892,13 @@ static struct pci_device_id serial_pci_tbl[] = {
 	{	PCI_VENDOR_ID_ADVANTECH, PCI_DEVICE_ID_ADVANTECH_PCI3620,
 		PCI_DEVICE_ID_ADVANTECH_PCI3620, 0x0001, 0, 0,
 		pbn_b2_8_921600 },
+	/* Advantech also use 0x3618 and 0xf618 */
+	{	PCI_VENDOR_ID_ADVANTECH, PCI_DEVICE_ID_ADVANTECH_PCI3618,
+		PCI_DEVICE_ID_ADVANTECH_PCI3618, PCI_ANY_ID, 0, 0,
+		pbn_b0_4_921600 },
+	{	PCI_VENDOR_ID_ADVANTECH, PCI_DEVICE_ID_ADVANTECH_PCIf618,
+		PCI_DEVICE_ID_ADVANTECH_PCI3618, PCI_ANY_ID, 0, 0,
+		pbn_b0_4_921600 },
 	{	PCI_VENDOR_ID_V3, PCI_DEVICE_ID_V3_V960,
 		PCI_SUBVENDOR_ID_CONNECT_TECH,
 		PCI_SUBDEVICE_ID_CONNECT_TECH_BH8_232, 0, 0,
diff --git a/drivers/tty/serial/8250/Kconfig b/drivers/tty/serial/8250/Kconfig
index 23329918f22..349ee598b34 100644
--- a/drivers/tty/serial/8250/Kconfig
+++ b/drivers/tty/serial/8250/Kconfig
@@ -61,6 +61,7 @@ config SERIAL_8250_CONSOLE
 	bool "Console on 8250/16550 and compatible serial port"
 	depends on SERIAL_8250=y
 	select SERIAL_CORE_CONSOLE
+	select SERIAL_EARLYCON
 	---help---
 	  If you say Y here, it will be possible to use a serial port as the
 	  system console (the system console is the device which receives all
@@ -90,11 +91,6 @@ config SERIAL_8250_CONSOLE
 
 	  If unsure, say N.
 
-config FIX_EARLYCON_MEM
-	bool
-	depends on X86
-	default y
-
 config SERIAL_8250_GSC
 	tristate
 	depends on SERIAL_8250 && GSC
diff --git a/drivers/tty/serial/Kconfig b/drivers/tty/serial/Kconfig
index 5d9b01aa54f..fb57159bad3 100644
--- a/drivers/tty/serial/Kconfig
+++ b/drivers/tty/serial/Kconfig
@@ -7,6 +7,13 @@ if TTY
 menu "Serial drivers"
 	depends on HAS_IOMEM
 
+config SERIAL_EARLYCON
+	bool
+	help
+	  Support for early consoles with the earlycon parameter. This enables
+	  the console before standard serial driver is probed. The console is
+	  enabled when early_param is processed.
+
 source "drivers/tty/serial/8250/Kconfig"
 
 comment "Non-8250 serial port support"
@@ -53,6 +60,7 @@ config SERIAL_AMBA_PL011_CONSOLE
 	bool "Support for console on AMBA serial port"
 	depends on SERIAL_AMBA_PL011=y
 	select SERIAL_CORE_CONSOLE
+	select SERIAL_EARLYCON
 	---help---
 	  Say Y here if you wish to use an AMBA PrimeCell UART as the system
 	  console (the system console is the device which receives all kernel
@@ -65,6 +73,18 @@ config SERIAL_AMBA_PL011_CONSOLE
 	  your boot loader (lilo or loadlin) about how to pass options to the
 	  kernel at boot time.)
 
+config SERIAL_EARLYCON_ARM_SEMIHOST
+	bool "Early console using ARM semihosting"
+	depends on ARM64 || ARM
+	select SERIAL_CORE
+	select SERIAL_CORE_CONSOLE
+	select SERIAL_EARLYCON
+	help
+	  Support for early debug console using ARM semihosting. This enables
+	  the console before standard serial driver is probed. This is enabled
+	  with "earlycon=smh" on the kernel command line. The console is
+	  enabled when early_param is processed.
+
 config SERIAL_SB1250_DUART
 	tristate "BCM1xxx on-chip DUART serial support"
 	depends on SIBYTE_SB1xxx_SOC=y
@@ -97,6 +117,7 @@ config SERIAL_ATMEL
 	bool "AT91 / AT32 on-chip serial port support"
 	depends on ARCH_AT91 || AVR32
 	select SERIAL_CORE
+	select SERIAL_MCTRL_GPIO
 	help
 	  This enables the driver for the on-chip UARTs of the Atmel
 	  AT91 and AT32 processors.
@@ -1160,6 +1181,16 @@ config SERIAL_SCCNXP_CONSOLE
 	help
 	  Support for console on SCCNXP serial ports.
 
+config SERIAL_SC16IS7XX
+	tristate "SC16IS7xx serial support"
+	depends on I2C
+	select SERIAL_CORE
+	select REGMAP_I2C if I2C
+	help
+	  This selects support for SC16IS7xx serial ports.
+	  Supported ICs are SC16IS740, SC16IS741, SC16IS750, SC16IS752,
+	  SC16IS760 and SC16IS762.
+
 config SERIAL_BFIN_SPORT
 	tristate "Blackfin SPORT emulate UART"
 	depends on BLACKFIN
@@ -1323,7 +1354,7 @@ config SERIAL_IFX6X60
 
 config SERIAL_PCH_UART
 	tristate "Intel EG20T PCH/LAPIS Semicon IOH(ML7213/ML7223/ML7831) UART"
-	depends on PCI
+	depends on PCI && (X86_32 || COMPILE_TEST)
 	select SERIAL_CORE
 	help
 	  This driver is for PCH(Platform controller Hub) UART of Intel EG20T
@@ -1369,18 +1400,19 @@ config SERIAL_MXS_AUART_CONSOLE
 	  Enable a MXS AUART port to be the system console.
 
 config SERIAL_XILINX_PS_UART
-	tristate "Xilinx PS UART support"
+	tristate "Cadence (Xilinx Zynq) UART support"
 	depends on OF
 	select SERIAL_CORE
 	help
-	  This driver supports the Xilinx PS UART port.
+	  This driver supports the Cadence UART. It is found e.g. in Xilinx
+	  Zynq.
 
 config SERIAL_XILINX_PS_UART_CONSOLE
-	bool "Xilinx PS UART console support"
+	bool "Cadence UART console support"
 	depends on SERIAL_XILINX_PS_UART=y
 	select SERIAL_CORE_CONSOLE
 	help
-	  Enable a Xilinx PS UART port to be the system console.
+	  Enable a Cadence UART port to be the system console.
 
 config SERIAL_AR933X
 	tristate "AR933X serial port support"
@@ -1479,6 +1511,7 @@ config SERIAL_RP2_NR_UARTS
 
 config SERIAL_FSL_LPUART
 	tristate "Freescale lpuart serial port support"
+	depends on HAS_DMA
 	select SERIAL_CORE
 	help
 	  Support for the on-chip lpuart on some Freescale SOCs.
@@ -1508,6 +1541,20 @@ config SERIAL_ST_ASC_CONSOLE
 	depends on SERIAL_ST_ASC=y
 	select SERIAL_CORE_CONSOLE
 
+config SERIAL_MEN_Z135
+	tristate "MEN 16z135 Support"
+	select SERIAL_CORE
+	depends on MCB
+	help
+	  Say yes here to enable support for the MEN 16z135 High Speed UART IP-Core
+	  on a MCB carrier.
+
+	  This driver can also be build as a module. If so, the module will be called
+	  men_z135_uart.ko
+
 endmenu
 
+config SERIAL_MCTRL_GPIO
+	tristate
+
 endif # TTY
diff --git a/drivers/tty/serial/Makefile b/drivers/tty/serial/Makefile
index 3680854fef4..0080cc362e0 100644
--- a/drivers/tty/serial/Makefile
+++ b/drivers/tty/serial/Makefile
@@ -5,6 +5,9 @@
 obj-$(CONFIG_SERIAL_CORE) += serial_core.o
 obj-$(CONFIG_SERIAL_21285) += 21285.o
 
+obj-$(CONFIG_SERIAL_EARLYCON) += earlycon.o
+obj-$(CONFIG_SERIAL_EARLYCON_ARM_SEMIHOST) += earlycon-arm-semihost.o
+
 # These Sparc drivers have to appear before others such as 8250
 # which share ttySx minor node space.  Otherwise console device
 # names change and other unplesantries.
@@ -48,6 +51,7 @@ obj-$(CONFIG_SERIAL_MPSC) += mpsc.o
 obj-$(CONFIG_SERIAL_SB1250_DUART) += sb1250-duart.o
 obj-$(CONFIG_ETRAX_SERIAL) += crisv10.o
 obj-$(CONFIG_SERIAL_SCCNXP) += sccnxp.o
+obj-$(CONFIG_SERIAL_SC16IS7XX) += sc16is7xx.o
 obj-$(CONFIG_SERIAL_JSM) += jsm/
 obj-$(CONFIG_SERIAL_TXX9) += serial_txx9.o
 obj-$(CONFIG_SERIAL_VR41XX) += vr41xx_siu.o
@@ -87,3 +91,7 @@ obj-$(CONFIG_SERIAL_EFM32_UART) += efm32-uart.o
 obj-$(CONFIG_SERIAL_ARC)	+= arc_uart.o
 obj-$(CONFIG_SERIAL_RP2)	+= rp2.o
 obj-$(CONFIG_SERIAL_FSL_LPUART)	+= fsl_lpuart.o
+obj-$(CONFIG_SERIAL_MEN_Z135)	+= men_z135_uart.o
+
+# GPIOLIB helpers for modem control lines
+obj-$(CONFIG_SERIAL_MCTRL_GPIO)	+= serial_mctrl_gpio.o
diff --git a/drivers/tty/serial/amba-pl011.c b/drivers/tty/serial/amba-pl011.c
index dacf0a09ab2..908a6e3142a 100644
--- a/drivers/tty/serial/amba-pl011.c
+++ b/drivers/tty/serial/amba-pl011.c
@@ -303,7 +303,7 @@ static void pl011_dma_probe_initcall(struct device *dev, struct uart_amba_port *
 
 	/* Optionally make use of an RX channel as well */
 	chan = dma_request_slave_channel(dev, "rx");
-	
+
 	if (!chan && plat->dma_rx_param) {
 		chan = dma_request_channel(mask, plat->dma_filter, plat->dma_rx_param);
 
@@ -2045,6 +2045,35 @@ static struct console amba_console = {
 };
 
 #define AMBA_CONSOLE	(&amba_console)
+
+static void pl011_putc(struct uart_port *port, int c)
+{
+	while (readl(port->membase + UART01x_FR) & UART01x_FR_TXFF)
+		;
+	writeb(c, port->membase + UART01x_DR);
+	while (readl(port->membase + UART01x_FR) & UART01x_FR_BUSY)
+		;
+}
+
+static void pl011_early_write(struct console *con, const char *s, unsigned n)
+{
+	struct earlycon_device *dev = con->data;
+
+	uart_console_write(&dev->port, s, n, pl011_putc);
+}
+
+static int __init pl011_early_console_setup(struct earlycon_device *device,
+					    const char *opt)
+{
+	if (!device->port.membase)
+		return -ENODEV;
+
+	device->con->write = pl011_early_write;
+	return 0;
+}
+EARLYCON_DECLARE(pl011, pl011_early_console_setup);
+OF_EARLYCON_DECLARE(pl011, "arm,pl011", pl011_early_console_setup);
+
 #else
 #define AMBA_CONSOLE	NULL
 #endif
diff --git a/drivers/tty/serial/atmel_serial.c b/drivers/tty/serial/atmel_serial.c
index 53eeea13ff1..3fceae099c4 100644
--- a/drivers/tty/serial/atmel_serial.c
+++ b/drivers/tty/serial/atmel_serial.c
@@ -43,6 +43,9 @@
 #include <linux/platform_data/atmel.h>
 #include <linux/timer.h>
 #include <linux/gpio.h>
+#include <linux/gpio/consumer.h>
+#include <linux/err.h>
+#include <linux/irq.h>
 
 #include <asm/io.h>
 #include <asm/ioctls.h>
@@ -57,6 +60,8 @@
 
 #include <linux/serial_core.h>
 
+#include "serial_mctrl_gpio.h"
+
 static void atmel_start_rx(struct uart_port *port);
 static void atmel_stop_rx(struct uart_port *port);
 
@@ -162,8 +167,10 @@ struct atmel_uart_port {
 	struct circ_buf		rx_ring;
 
 	struct serial_rs485	rs485;		/* rs485 settings */
-	int			rts_gpio;	/* optional RTS GPIO */
+	struct mctrl_gpios	*gpios;
+	int			gpio_irq[UART_GPIO_MAX];
 	unsigned int		tx_done_mask;
+	bool			ms_irq_enabled;
 	bool			is_usart;	/* usart or uart */
 	struct timer_list	uart_timer;	/* uart timer */
 	int (*prepare_rx)(struct uart_port *port);
@@ -237,6 +244,50 @@ static bool atmel_use_dma_rx(struct uart_port *port)
 	return atmel_port->use_dma_rx;
 }
 
+static unsigned int atmel_get_lines_status(struct uart_port *port)
+{
+	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
+	unsigned int status, ret = 0;
+
+	status = UART_GET_CSR(port);
+
+	mctrl_gpio_get(atmel_port->gpios, &ret);
+
+	if (!IS_ERR_OR_NULL(mctrl_gpio_to_gpiod(atmel_port->gpios,
+						UART_GPIO_CTS))) {
+		if (ret & TIOCM_CTS)
+			status &= ~ATMEL_US_CTS;
+		else
+			status |= ATMEL_US_CTS;
+	}
+
+	if (!IS_ERR_OR_NULL(mctrl_gpio_to_gpiod(atmel_port->gpios,
+						UART_GPIO_DSR))) {
+		if (ret & TIOCM_DSR)
+			status &= ~ATMEL_US_DSR;
+		else
+			status |= ATMEL_US_DSR;
+	}
+
+	if (!IS_ERR_OR_NULL(mctrl_gpio_to_gpiod(atmel_port->gpios,
+						UART_GPIO_RI))) {
+		if (ret & TIOCM_RI)
+			status &= ~ATMEL_US_RI;
+		else
+			status |= ATMEL_US_RI;
+	}
+
+	if (!IS_ERR_OR_NULL(mctrl_gpio_to_gpiod(atmel_port->gpios,
+						UART_GPIO_DCD))) {
+		if (ret & TIOCM_CD)
+			status &= ~ATMEL_US_DCD;
+		else
+			status |= ATMEL_US_DCD;
+	}
+
+	return status;
+}
+
 /* Enable or disable the rs485 support */
 void atmel_config_rs485(struct uart_port *port, struct serial_rs485 *rs485conf)
 {
@@ -296,17 +347,6 @@ static void atmel_set_mctrl(struct uart_port *port, u_int mctrl)
 	unsigned int mode;
 	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
 
-	/*
-	 * AT91RM9200 Errata #39: RTS0 is not internally connected
-	 * to PA21. We need to drive the pin as a GPIO.
-	 */
-	if (gpio_is_valid(atmel_port->rts_gpio)) {
-		if (mctrl & TIOCM_RTS)
-			gpio_set_value(atmel_port->rts_gpio, 0);
-		else
-			gpio_set_value(atmel_port->rts_gpio, 1);
-	}
-
 	if (mctrl & TIOCM_RTS)
 		control |= ATMEL_US_RTSEN;
 	else
@@ -319,6 +359,8 @@ static void atmel_set_mctrl(struct uart_port *port, u_int mctrl)
 
 	UART_PUT_CR(port, control);
 
+	mctrl_gpio_set(atmel_port->gpios, mctrl);
+
 	/* Local loopback mode? */
 	mode = UART_GET_MR(port) & ~ATMEL_US_CHMODE;
 	if (mctrl & TIOCM_LOOP)
@@ -346,7 +388,8 @@ static void atmel_set_mctrl(struct uart_port *port, u_int mctrl)
  */
 static u_int atmel_get_mctrl(struct uart_port *port)
 {
-	unsigned int status, ret = 0;
+	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
+	unsigned int ret = 0, status;
 
 	status = UART_GET_CSR(port);
 
@@ -362,7 +405,7 @@ static u_int atmel_get_mctrl(struct uart_port *port)
 	if (!(status & ATMEL_US_RI))
 		ret |= TIOCM_RI;
 
-	return ret;
+	return mctrl_gpio_get(atmel_port->gpios, &ret);
 }
 
 /*
@@ -449,8 +492,38 @@ static void atmel_stop_rx(struct uart_port *port)
  */
 static void atmel_enable_ms(struct uart_port *port)
 {
-	UART_PUT_IER(port, ATMEL_US_RIIC | ATMEL_US_DSRIC
-			| ATMEL_US_DCDIC | ATMEL_US_CTSIC);
+	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
+	uint32_t ier = 0;
+
+	/*
+	 * Interrupt should not be enabled twice
+	 */
+	if (atmel_port->ms_irq_enabled)
+		return;
+
+	atmel_port->ms_irq_enabled = true;
+
+	if (atmel_port->gpio_irq[UART_GPIO_CTS] >= 0)
+		enable_irq(atmel_port->gpio_irq[UART_GPIO_CTS]);
+	else
+		ier |= ATMEL_US_CTSIC;
+
+	if (atmel_port->gpio_irq[UART_GPIO_DSR] >= 0)
+		enable_irq(atmel_port->gpio_irq[UART_GPIO_DSR]);
+	else
+		ier |= ATMEL_US_DSRIC;
+
+	if (atmel_port->gpio_irq[UART_GPIO_RI] >= 0)
+		enable_irq(atmel_port->gpio_irq[UART_GPIO_RI]);
+	else
+		ier |= ATMEL_US_RIIC;
+
+	if (atmel_port->gpio_irq[UART_GPIO_DCD] >= 0)
+		enable_irq(atmel_port->gpio_irq[UART_GPIO_DCD]);
+	else
+		ier |= ATMEL_US_DCDIC;
+
+	UART_PUT_IER(port, ier);
 }
 
 /*
@@ -1039,11 +1112,31 @@ atmel_handle_status(struct uart_port *port, unsigned int pending,
 static irqreturn_t atmel_interrupt(int irq, void *dev_id)
 {
 	struct uart_port *port = dev_id;
+	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
 	unsigned int status, pending, pass_counter = 0;
+	bool gpio_handled = false;
 
 	do {
-		status = UART_GET_CSR(port);
+		status = atmel_get_lines_status(port);
 		pending = status & UART_GET_IMR(port);
+		if (!gpio_handled) {
+			/*
+			 * Dealing with GPIO interrupt
+			 */
+			if (irq == atmel_port->gpio_irq[UART_GPIO_CTS])
+				pending |= ATMEL_US_CTSIC;
+
+			if (irq == atmel_port->gpio_irq[UART_GPIO_DSR])
+				pending |= ATMEL_US_DSRIC;
+
+			if (irq == atmel_port->gpio_irq[UART_GPIO_RI])
+				pending |= ATMEL_US_RIIC;
+
+			if (irq == atmel_port->gpio_irq[UART_GPIO_DCD])
+				pending |= ATMEL_US_DCDIC;
+
+			gpio_handled = true;
+		}
 		if (!pending)
 			break;
 
@@ -1523,6 +1616,45 @@ static void atmel_get_ip_name(struct uart_port *port)
 	}
 }
 
+static void atmel_free_gpio_irq(struct uart_port *port)
+{
+	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
+	enum mctrl_gpio_idx i;
+
+	for (i = 0; i < UART_GPIO_MAX; i++)
+		if (atmel_port->gpio_irq[i] >= 0)
+			free_irq(atmel_port->gpio_irq[i], port);
+}
+
+static int atmel_request_gpio_irq(struct uart_port *port)
+{
+	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
+	int *irq = atmel_port->gpio_irq;
+	enum mctrl_gpio_idx i;
+	int err = 0;
+
+	for (i = 0; (i < UART_GPIO_MAX) && !err; i++) {
+		if (irq[i] < 0)
+			continue;
+
+		irq_set_status_flags(irq[i], IRQ_NOAUTOEN);
+		err = request_irq(irq[i], atmel_interrupt, IRQ_TYPE_EDGE_BOTH,
+				  "atmel_serial", port);
+		if (err)
+			dev_err(port->dev, "atmel_startup - Can't get %d irq\n",
+				irq[i]);
+	}
+
+	/*
+	 * If something went wrong, rollback.
+	 */
+	while (err && (--i >= 0))
+		if (irq[i] >= 0)
+			free_irq(irq[i], port);
+
+	return err;
+}
+
 /*
  * Perform initialization and enable port for reception
  */
@@ -1539,6 +1671,7 @@ static int atmel_startup(struct uart_port *port)
 	 * handle an unexpected interrupt
 	 */
 	UART_PUT_IDR(port, -1);
+	atmel_port->ms_irq_enabled = false;
 
 	/*
 	 * Allocate the IRQ
@@ -1551,6 +1684,13 @@ static int atmel_startup(struct uart_port *port)
 	}
 
 	/*
+	 * Get the GPIO lines IRQ
+	 */
+	retval = atmel_request_gpio_irq(port);
+	if (retval)
+		goto free_irq;
+
+	/*
 	 * Initialize DMA (if necessary)
 	 */
 	atmel_init_property(atmel_port, pdev);
@@ -1568,7 +1708,7 @@ static int atmel_startup(struct uart_port *port)
 	}
 
 	/* Save current CSR for comparison in atmel_tasklet_func() */
-	atmel_port->irq_status_prev = UART_GET_CSR(port);
+	atmel_port->irq_status_prev = atmel_get_lines_status(port);
 	atmel_port->irq_status = atmel_port->irq_status_prev;
 
 	/*
@@ -1614,6 +1754,11 @@ static int atmel_startup(struct uart_port *port)
 	}
 
 	return 0;
+
+free_irq:
+	free_irq(port->irq, port);
+
+	return retval;
 }
 
 /*
@@ -1661,9 +1806,12 @@ static void atmel_shutdown(struct uart_port *port)
 	atmel_port->rx_ring.tail = 0;
 
 	/*
-	 * Free the interrupt
+	 * Free the interrupts
 	 */
 	free_irq(port->irq, port);
+	atmel_free_gpio_irq(port);
+
+	atmel_port->ms_irq_enabled = false;
 }
 
 /*
@@ -2324,6 +2472,26 @@ static int atmel_serial_resume(struct platform_device *pdev)
 #define atmel_serial_resume NULL
 #endif
 
+static int atmel_init_gpios(struct atmel_uart_port *p, struct device *dev)
+{
+	enum mctrl_gpio_idx i;
+	struct gpio_desc *gpiod;
+
+	p->gpios = mctrl_gpio_init(dev, 0);
+	if (IS_ERR_OR_NULL(p->gpios))
+		return -1;
+
+	for (i = 0; i < UART_GPIO_MAX; i++) {
+		gpiod = mctrl_gpio_to_gpiod(p->gpios, i);
+		if (gpiod && (gpiod_get_direction(gpiod) == GPIOF_DIR_IN))
+			p->gpio_irq[i] = gpiod_to_irq(gpiod);
+		else
+			p->gpio_irq[i] = -EINVAL;
+	}
+
+	return 0;
+}
+
 static int atmel_serial_probe(struct platform_device *pdev)
 {
 	struct atmel_uart_port *port;
@@ -2359,25 +2527,11 @@ static int atmel_serial_probe(struct platform_device *pdev)
 	port = &atmel_ports[ret];
 	port->backup_imr = 0;
 	port->uart.line = ret;
-	port->rts_gpio = -EINVAL; /* Invalid, zero could be valid */
-	if (pdata)
-		port->rts_gpio = pdata->rts_gpio;
-	else if (np)
-		port->rts_gpio = of_get_named_gpio(np, "rts-gpios", 0);
-
-	if (gpio_is_valid(port->rts_gpio)) {
-		ret = devm_gpio_request(&pdev->dev, port->rts_gpio, "RTS");
-		if (ret) {
-			dev_err(&pdev->dev, "error requesting RTS GPIO\n");
-			goto err;
-		}
-		/* Default to 1 as RTS is active low */
-		ret = gpio_direction_output(port->rts_gpio, 1);
-		if (ret) {
-			dev_err(&pdev->dev, "error setting up RTS GPIO\n");
-			goto err;
-		}
-	}
+
+	ret = atmel_init_gpios(port, &pdev->dev);
+	if (ret < 0)
+		dev_err(&pdev->dev, "%s",
+			"Failed to initialize GPIOs. The serial port may not work as expected");
 
 	ret = atmel_init_port(port, pdev);
 	if (ret)
diff --git a/drivers/tty/serial/cpm_uart/cpm_uart_core.c b/drivers/tty/serial/cpm_uart/cpm_uart_core.c
index 7d76214612c..aa60e6d13ec 100644
--- a/drivers/tty/serial/cpm_uart/cpm_uart_core.c
+++ b/drivers/tty/serial/cpm_uart/cpm_uart_core.c
@@ -971,7 +971,7 @@ static void cpm_uart_config_port(struct uart_port *port, int flags)
  * Note that this is called with interrupts already disabled
  */
 static void cpm_uart_early_write(struct uart_cpm_port *pinfo,
-		const char *string, u_int count)
+		const char *string, u_int count, bool handle_linefeed)
 {
 	unsigned int i;
 	cbd_t __iomem *bdp, *bdbase;
@@ -1013,7 +1013,7 @@ static void cpm_uart_early_write(struct uart_cpm_port *pinfo,
 			bdp++;
 
 		/* if a LF, also do CR... */
-		if (*string == 10) {
+		if (handle_linefeed && *string == 10) {
 			while ((in_be16(&bdp->cbd_sc) & BD_SC_READY) != 0)
 				;
 
@@ -1111,7 +1111,7 @@ static void cpm_put_poll_char(struct uart_port *port,
 	static char ch[2];
 
 	ch[0] = (char)c;
-	cpm_uart_early_write(pinfo, ch, 1);
+	cpm_uart_early_write(pinfo, ch, 1, false);
 }
 #endif /* CONFIG_CONSOLE_POLL */
 
@@ -1275,7 +1275,7 @@ static void cpm_uart_console_write(struct console *co, const char *s,
 		spin_lock_irqsave(&pinfo->port.lock, flags);
 	}
 
-	cpm_uart_early_write(pinfo, s, count);
+	cpm_uart_early_write(pinfo, s, count, true);
 
 	if (unlikely(nolock)) {
 		local_irq_restore(flags);
diff --git a/drivers/tty/serial/earlycon-arm-semihost.c b/drivers/tty/serial/earlycon-arm-semihost.c
new file mode 100644
index 00000000000..383db10fbb4
--- /dev/null
+++ b/drivers/tty/serial/earlycon-arm-semihost.c
@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) 2012 ARM Ltd.
+ * Author: Marc Zyngier <marc.zyngier@arm.com>
+ *
+ * Adapted for ARM and earlycon:
+ * Copyright (C) 2014 Linaro Ltd.
+ * Author: Rob Herring <robh@kernel.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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <linux/kernel.h>
+#include <linux/console.h>
+#include <linux/init.h>
+#include <linux/serial_core.h>
+
+#ifdef CONFIG_THUMB2_KERNEL
+#define SEMIHOST_SWI	"0xab"
+#else
+#define SEMIHOST_SWI	"0x123456"
+#endif
+
+/*
+ * Semihosting-based debug console
+ */
+static void smh_putc(struct uart_port *port, int c)
+{
+#ifdef CONFIG_ARM64
+	asm volatile("mov  x1, %0\n"
+		     "mov  x0, #3\n"
+		     "hlt  0xf000\n"
+		     : : "r" (&c) : "x0", "x1", "memory");
+#else
+	asm volatile("mov  r1, %0\n"
+		     "mov  r0, #3\n"
+		     "svc  " SEMIHOST_SWI "\n"
+		     : : "r" (&c) : "r0", "r1", "memory");
+#endif
+}
+
+static void smh_write(struct console *con, const char *s, unsigned n)
+{
+	struct earlycon_device *dev = con->data;
+	uart_console_write(&dev->port, s, n, smh_putc);
+}
+
+int __init early_smh_setup(struct earlycon_device *device, const char *opt)
+{
+	device->con->write = smh_write;
+	return 0;
+}
+EARLYCON_DECLARE(smh, early_smh_setup);
diff --git a/drivers/tty/serial/earlycon.c b/drivers/tty/serial/earlycon.c
new file mode 100644
index 00000000000..5131b5ee616
--- /dev/null
+++ b/drivers/tty/serial/earlycon.c
@@ -0,0 +1,172 @@
+/*
+ * Copyright (C) 2014 Linaro Ltd.
+ * Author: Rob Herring <robh@kernel.org>
+ *
+ * Based on 8250 earlycon:
+ * (c) Copyright 2004 Hewlett-Packard Development Company, L.P.
+ *	Bjorn Helgaas <bjorn.helgaas@hp.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/console.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/serial_core.h>
+#include <linux/sizes.h>
+#include <linux/mod_devicetable.h>
+
+#ifdef CONFIG_FIX_EARLYCON_MEM
+#include <asm/fixmap.h>
+#endif
+
+#include <asm/serial.h>
+
+static struct console early_con = {
+	.name =		"earlycon",
+	.flags =	CON_PRINTBUFFER | CON_BOOT,
+	.index =	-1,
+};
+
+static struct earlycon_device early_console_dev = {
+	.con = &early_con,
+};
+
+static const struct of_device_id __earlycon_of_table_sentinel
+	__used __section(__earlycon_of_table_end);
+
+static void __iomem * __init earlycon_map(unsigned long paddr, size_t size)
+{
+	void __iomem *base;
+#ifdef CONFIG_FIX_EARLYCON_MEM
+	set_fixmap_io(FIX_EARLYCON_MEM_BASE, paddr & PAGE_MASK);
+	base = (void __iomem *)__fix_to_virt(FIX_EARLYCON_MEM_BASE);
+	base += paddr & ~PAGE_MASK;
+#else
+	base = ioremap(paddr, size);
+#endif
+	if (!base)
+		pr_err("%s: Couldn't map 0x%llx\n", __func__,
+		       (unsigned long long)paddr);
+
+	return base;
+}
+
+static int __init parse_options(struct earlycon_device *device,
+				char *options)
+{
+	struct uart_port *port = &device->port;
+	int mmio, mmio32, length;
+	unsigned long addr;
+
+	if (!options)
+		return -ENODEV;
+
+	mmio = !strncmp(options, "mmio,", 5);
+	mmio32 = !strncmp(options, "mmio32,", 7);
+	if (mmio || mmio32) {
+		port->iotype = (mmio ? UPIO_MEM : UPIO_MEM32);
+		options += mmio ? 5 : 7;
+		addr = simple_strtoul(options, NULL, 0);
+		port->mapbase = addr;
+		if (mmio32)
+			port->regshift = 2;
+	} else if (!strncmp(options, "io,", 3)) {
+		port->iotype = UPIO_PORT;
+		options += 3;
+		addr = simple_strtoul(options, NULL, 0);
+		port->iobase = addr;
+		mmio = 0;
+	} else if (!strncmp(options, "0x", 2)) {
+		port->iotype = UPIO_MEM;
+		addr = simple_strtoul(options, NULL, 0);
+		port->mapbase = addr;
+	} else {
+		return -EINVAL;
+	}
+
+	port->uartclk = BASE_BAUD * 16;
+
+	options = strchr(options, ',');
+	if (options) {
+		options++;
+		device->baud = simple_strtoul(options, NULL, 0);
+		length = min(strcspn(options, " ") + 1,
+			     (size_t)(sizeof(device->options)));
+		strlcpy(device->options, options, length);
+	}
+
+	if (mmio || mmio32)
+		pr_info("Early serial console at MMIO%s 0x%llx (options '%s')\n",
+			mmio32 ? "32" : "",
+			(unsigned long long)port->mapbase,
+			device->options);
+	else
+		pr_info("Early serial console at I/O port 0x%lx (options '%s')\n",
+			port->iobase,
+			device->options);
+
+	return 0;
+}
+
+int __init setup_earlycon(char *buf, const char *match,
+			  int (*setup)(struct earlycon_device *, const char *))
+{
+	int err;
+	size_t len;
+	struct uart_port *port = &early_console_dev.port;
+
+	if (!buf || !match || !setup)
+		return 0;
+
+	len = strlen(match);
+	if (strncmp(buf, match, len))
+		return 0;
+	if (buf[len] && (buf[len] != ','))
+		return 0;
+
+	buf += len + 1;
+
+	err = parse_options(&early_console_dev, buf);
+	/* On parsing error, pass the options buf to the setup function */
+	if (!err)
+		buf = NULL;
+
+	if (port->mapbase)
+		port->membase = earlycon_map(port->mapbase, 64);
+
+	early_console_dev.con->data = &early_console_dev;
+	err = setup(&early_console_dev, buf);
+	if (err < 0)
+		return err;
+	if (!early_console_dev.con->write)
+		return -ENODEV;
+
+	register_console(early_console_dev.con);
+	return 0;
+}
+
+int __init of_setup_earlycon(unsigned long addr,
+			     int (*setup)(struct earlycon_device *, const char *))
+{
+	int err;
+	struct uart_port *port = &early_console_dev.port;
+
+	port->iotype = UPIO_MEM;
+	port->mapbase = addr;
+	port->uartclk = BASE_BAUD * 16;
+	port->membase = earlycon_map(addr, SZ_4K);
+
+	early_console_dev.con->data = &early_console_dev;
+	err = setup(&early_console_dev, NULL);
+	if (err < 0)
+		return err;
+	if (!early_console_dev.con->write)
+		return -ENODEV;
+
+
+	register_console(early_console_dev.con);
+	return 0;
+}
diff --git a/drivers/tty/serial/efm32-uart.c b/drivers/tty/serial/efm32-uart.c
index c167a710dc3..b373f6416e8 100644
--- a/drivers/tty/serial/efm32-uart.c
+++ b/drivers/tty/serial/efm32-uart.c
@@ -842,6 +842,7 @@ static void __exit efm32_uart_exit(void)
 	platform_driver_unregister(&efm32_uart_driver);
 	uart_unregister_driver(&efm32_uart_reg);
 }
+module_exit(efm32_uart_exit);
 
 MODULE_AUTHOR("Uwe Kleine-Koenig <u.kleine-koenig@pengutronix.de>");
 MODULE_DESCRIPTION("EFM32 UART/USART driver");
diff --git a/drivers/tty/serial/imx.c b/drivers/tty/serial/imx.c
index 3b6c1a2e25d..e2f93874989 100644
--- a/drivers/tty/serial/imx.c
+++ b/drivers/tty/serial/imx.c
@@ -116,7 +116,7 @@
 #define UCR3_DSR	(1<<10) /* Data set ready */
 #define UCR3_DCD	(1<<9)	/* Data carrier detect */
 #define UCR3_RI		(1<<8)	/* Ring indicator */
-#define UCR3_TIMEOUTEN	(1<<7)	/* Timeout interrupt enable */
+#define UCR3_ADNIMP	(1<<7)	/* Autobaud Detection Not Improved */
 #define UCR3_RXDSEN	(1<<6)	/* Receive status interrupt enable */
 #define UCR3_AIRINTEN	(1<<5)	/* Async IR wake interrupt enable */
 #define UCR3_AWAKEN	(1<<4)	/* Async wake interrupt enable */
@@ -444,6 +444,10 @@ static void imx_stop_rx(struct uart_port *port)
 
 	temp = readl(sport->port.membase + UCR2);
 	writel(temp & ~UCR2_RXEN, sport->port.membase + UCR2);
+
+	/* disable the `Receiver Ready Interrrupt` */
+	temp = readl(sport->port.membase + UCR1);
+	writel(temp & ~UCR1_RRDYEN, sport->port.membase + UCR1);
 }
 
 /*
@@ -1070,7 +1074,7 @@ static void imx_disable_dma(struct imx_port *sport)
 static int imx_startup(struct uart_port *port)
 {
 	struct imx_port *sport = (struct imx_port *)port;
-	int retval;
+	int retval, i;
 	unsigned long flags, temp;
 
 	retval = clk_prepare_enable(sport->clk_per);
@@ -1098,17 +1102,15 @@ static int imx_startup(struct uart_port *port)
 
 	writel(temp & ~UCR4_DREN, sport->port.membase + UCR4);
 
-	if (USE_IRDA(sport)) {
-		/* reset fifo's and state machines */
-		int i = 100;
-		temp = readl(sport->port.membase + UCR2);
-		temp &= ~UCR2_SRST;
-		writel(temp, sport->port.membase + UCR2);
-		while (!(readl(sport->port.membase + UCR2) & UCR2_SRST) &&
-		    (--i > 0)) {
-			udelay(1);
-		}
-	}
+	/* Reset fifo's and state machines */
+	i = 100;
+
+	temp = readl(sport->port.membase + UCR2);
+	temp &= ~UCR2_SRST;
+	writel(temp, sport->port.membase + UCR2);
+
+	while (!(readl(sport->port.membase + UCR2) & UCR2_SRST) && (--i > 0))
+		udelay(1);
 
 	/*
 	 * Allocate the IRQ(s) i.MX1 has three interrupts whereas later
@@ -1163,18 +1165,9 @@ static int imx_startup(struct uart_port *port)
 		temp |= UCR2_IRTS;
 	writel(temp, sport->port.membase + UCR2);
 
-	if (USE_IRDA(sport)) {
-		/* clear RX-FIFO */
-		int i = 64;
-		while ((--i > 0) &&
-			(readl(sport->port.membase + URXD0) & URXD_CHARRDY)) {
-			barrier();
-		}
-	}
-
 	if (!is_imx1_uart(sport)) {
 		temp = readl(sport->port.membase + UCR3);
-		temp |= IMX21_UCR3_RXDMUXSEL;
+		temp |= IMX21_UCR3_RXDMUXSEL | UCR3_ADNIMP;
 		writel(temp, sport->port.membase + UCR3);
 	}
 
diff --git a/drivers/tty/serial/kgdb_nmi.c b/drivers/tty/serial/kgdb_nmi.c
index 5f673b7ca50..cfadf2971b1 100644
--- a/drivers/tty/serial/kgdb_nmi.c
+++ b/drivers/tty/serial/kgdb_nmi.c
@@ -44,13 +44,22 @@ MODULE_PARM_DESC(magic, "magic sequence to enter NMI debugger (default $3#33)");
 
 static bool kgdb_nmi_tty_enabled;
 
+static int kgdb_nmi_console_setup(struct console *co, char *options)
+{
+	/* 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
+	 * be displayed on the dbg_io.
+	 */
+	dbg_io_ops->is_console = true;
+
+	return 0;
+}
+
 static void kgdb_nmi_console_write(struct console *co, const char *s, uint c)
 {
 	int i;
 
-	if (!kgdb_nmi_tty_enabled || atomic_read(&kgdb_active) >= 0)
-		return;
-
 	for (i = 0; i < c; i++)
 		dbg_io_ops->write_char(s[i]);
 }
@@ -65,6 +74,7 @@ static struct tty_driver *kgdb_nmi_console_device(struct console *co, int *idx)
 
 static struct console kgdb_nmi_console = {
 	.name	= "ttyNMI",
+	.setup  = kgdb_nmi_console_setup,
 	.write	= kgdb_nmi_console_write,
 	.device	= kgdb_nmi_console_device,
 	.flags	= CON_PRINTBUFFER | CON_ANYTIME | CON_ENABLED,
@@ -80,29 +90,10 @@ static struct console kgdb_nmi_console = {
 
 struct kgdb_nmi_tty_priv {
 	struct tty_port port;
-	struct tasklet_struct tlet;
+	struct timer_list timer;
 	STRUCT_KFIFO(char, KGDB_NMI_FIFO_SIZE) fifo;
 };
 
-static struct kgdb_nmi_tty_priv *kgdb_nmi_port_to_priv(struct tty_port *port)
-{
-	return container_of(port, struct kgdb_nmi_tty_priv, port);
-}
-
-/*
- * Our debugging console is polled in a tasklet, so we'll check for input
- * every tick. In HZ-less mode, we should program the next tick.  We have
- * to use the lowlevel stuff as no locks should be grabbed.
- */
-#ifdef CONFIG_HIGH_RES_TIMERS
-static void kgdb_tty_poke(void)
-{
-	tick_program_event(ktime_get(), 0);
-}
-#else
-static inline void kgdb_tty_poke(void) {}
-#endif
-
 static struct tty_port *kgdb_nmi_port;
 
 static void kgdb_tty_recv(int ch)
@@ -113,14 +104,13 @@ static void kgdb_tty_recv(int ch)
 	if (!kgdb_nmi_port || ch < 0)
 		return;
 	/*
-	 * Can't use port->tty->driver_data as tty might be not there. Tasklet
+	 * Can't use port->tty->driver_data as tty might be not there. Timer
 	 * will check for tty and will get the ref, but here we don't have to
 	 * do that, and actually, we can't: we're in NMI context, no locks are
 	 * possible.
 	 */
-	priv = kgdb_nmi_port_to_priv(kgdb_nmi_port);
+	priv = container_of(kgdb_nmi_port, struct kgdb_nmi_tty_priv, port);
 	kfifo_in(&priv->fifo, &c, 1);
-	kgdb_tty_poke();
 }
 
 static int kgdb_nmi_poll_one_knock(void)
@@ -204,7 +194,8 @@ static void kgdb_nmi_tty_receiver(unsigned long data)
 	struct kgdb_nmi_tty_priv *priv = (void *)data;
 	char ch;
 
-	tasklet_schedule(&priv->tlet);
+	priv->timer.expires = jiffies + (HZ/100);
+	add_timer(&priv->timer);
 
 	if (likely(!kgdb_nmi_tty_enabled || !kfifo_len(&priv->fifo)))
 		return;
@@ -216,18 +207,22 @@ static void kgdb_nmi_tty_receiver(unsigned long data)
 
 static int kgdb_nmi_tty_activate(struct tty_port *port, struct tty_struct *tty)
 {
-	struct kgdb_nmi_tty_priv *priv = tty->driver_data;
+	struct kgdb_nmi_tty_priv *priv =
+	    container_of(port, struct kgdb_nmi_tty_priv, port);
 
 	kgdb_nmi_port = port;
-	tasklet_schedule(&priv->tlet);
+	priv->timer.expires = jiffies + (HZ/100);
+	add_timer(&priv->timer);
+
 	return 0;
 }
 
 static void kgdb_nmi_tty_shutdown(struct tty_port *port)
 {
-	struct kgdb_nmi_tty_priv *priv = port->tty->driver_data;
+	struct kgdb_nmi_tty_priv *priv =
+	    container_of(port, struct kgdb_nmi_tty_priv, port);
 
-	tasklet_kill(&priv->tlet);
+	del_timer(&priv->timer);
 	kgdb_nmi_port = NULL;
 }
 
@@ -246,7 +241,7 @@ static int kgdb_nmi_tty_install(struct tty_driver *drv, struct tty_struct *tty)
 		return -ENOMEM;
 
 	INIT_KFIFO(priv->fifo);
-	tasklet_init(&priv->tlet, kgdb_nmi_tty_receiver, (unsigned long)priv);
+	setup_timer(&priv->timer, kgdb_nmi_tty_receiver, (unsigned long)priv);
 	tty_port_init(&priv->port);
 	priv->port.ops = &kgdb_nmi_tty_port_ops;
 	tty->driver_data = priv;
diff --git a/drivers/tty/serial/men_z135_uart.c b/drivers/tty/serial/men_z135_uart.c
new file mode 100644
index 00000000000..c9d18548783
--- /dev/null
+++ b/drivers/tty/serial/men_z135_uart.c
@@ -0,0 +1,867 @@
+/*
+ * MEN 16z135 High Speed UART
+ *
+ * Copyright (C) 2014 MEN Mikroelektronik GmbH (www.men.de)
+ * Author: Johannes Thumshirn <johannes.thumshirn@men.de>
+ *
+ * This program is free software; 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.
+ */
+#define pr_fmt(fmt) KBUILD_MODNAME ":" fmt
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/serial_core.h>
+#include <linux/ioport.h>
+#include <linux/io.h>
+#include <linux/tty_flip.h>
+#include <linux/bitops.h>
+#include <linux/mcb.h>
+
+#define MEN_Z135_MAX_PORTS		12
+#define MEN_Z135_BASECLK		29491200
+#define MEN_Z135_FIFO_SIZE		1024
+#define MEN_Z135_NUM_MSI_VECTORS	2
+#define MEN_Z135_FIFO_WATERMARK		1020
+
+#define MEN_Z135_STAT_REG		0x0
+#define MEN_Z135_RX_RAM			0x4
+#define MEN_Z135_TX_RAM			0x400
+#define MEN_Z135_RX_CTRL		0x800
+#define MEN_Z135_TX_CTRL		0x804
+#define MEN_Z135_CONF_REG		0x808
+#define MEN_Z135_UART_FREQ		0x80c
+#define MEN_Z135_BAUD_REG		0x810
+#define MENZ135_TIMEOUT			0x814
+
+#define MEN_Z135_MEM_SIZE		0x818
+
+#define IS_IRQ(x) ((x) & 1)
+#define IRQ_ID(x) (((x) >> 1) & 7)
+
+#define MEN_Z135_IER_RXCIEN BIT(0)		/* RX Space IRQ */
+#define MEN_Z135_IER_TXCIEN BIT(1)		/* TX Space IRQ */
+#define MEN_Z135_IER_RLSIEN BIT(2)		/* Receiver Line Status IRQ */
+#define MEN_Z135_IER_MSIEN  BIT(3)		/* Modem Status IRQ */
+#define MEN_Z135_ALL_IRQS (MEN_Z135_IER_RXCIEN		\
+				| MEN_Z135_IER_RLSIEN	\
+				| MEN_Z135_IER_MSIEN	\
+				| MEN_Z135_IER_TXCIEN)
+
+#define MEN_Z135_MCR_DTR	BIT(24)
+#define MEN_Z135_MCR_RTS	BIT(25)
+#define MEN_Z135_MCR_OUT1	BIT(26)
+#define MEN_Z135_MCR_OUT2	BIT(27)
+#define MEN_Z135_MCR_LOOP	BIT(28)
+#define MEN_Z135_MCR_RCFC	BIT(29)
+
+#define MEN_Z135_MSR_DCTS	BIT(0)
+#define MEN_Z135_MSR_DDSR	BIT(1)
+#define MEN_Z135_MSR_DRI	BIT(2)
+#define MEN_Z135_MSR_DDCD	BIT(3)
+#define MEN_Z135_MSR_CTS	BIT(4)
+#define MEN_Z135_MSR_DSR	BIT(5)
+#define MEN_Z135_MSR_RI		BIT(6)
+#define MEN_Z135_MSR_DCD	BIT(7)
+
+#define MEN_Z135_LCR_SHIFT 8	/* LCR shift mask */
+
+#define MEN_Z135_WL5 0		/* CS5 */
+#define MEN_Z135_WL6 1		/* CS6 */
+#define MEN_Z135_WL7 2		/* CS7 */
+#define MEN_Z135_WL8 3		/* CS8 */
+
+#define MEN_Z135_STB_SHIFT 2	/* Stopbits */
+#define MEN_Z135_NSTB1 0
+#define MEN_Z135_NSTB2 1
+
+#define MEN_Z135_PEN_SHIFT 3	/* Parity enable */
+#define MEN_Z135_PAR_DIS 0
+#define MEN_Z135_PAR_ENA 1
+
+#define MEN_Z135_PTY_SHIFT 4	/* Parity type */
+#define MEN_Z135_PTY_ODD 0
+#define MEN_Z135_PTY_EVN 1
+
+#define MEN_Z135_LSR_DR BIT(0)
+#define MEN_Z135_LSR_OE BIT(1)
+#define MEN_Z135_LSR_PE BIT(2)
+#define MEN_Z135_LSR_FE BIT(3)
+#define MEN_Z135_LSR_BI BIT(4)
+#define MEN_Z135_LSR_THEP BIT(5)
+#define MEN_Z135_LSR_TEXP BIT(6)
+#define MEN_Z135_LSR_RXFIFOERR BIT(7)
+
+#define MEN_Z135_IRQ_ID_MST 0
+#define MEN_Z135_IRQ_ID_TSA 1
+#define MEN_Z135_IRQ_ID_RDA 2
+#define MEN_Z135_IRQ_ID_RLS 3
+#define MEN_Z135_IRQ_ID_CTI 6
+
+#define LCR(x) (((x) >> MEN_Z135_LCR_SHIFT) & 0xff)
+
+#define BYTES_TO_ALIGN(x) ((x) & 0x3)
+
+static int line;
+
+static int txlvl = 5;
+module_param(txlvl, int, S_IRUGO);
+MODULE_PARM_DESC(txlvl, "TX IRQ trigger level 0-7, default 5 (128 byte)");
+
+static int rxlvl = 6;
+module_param(rxlvl, int, S_IRUGO);
+MODULE_PARM_DESC(rxlvl, "RX IRQ trigger level 0-7, default 6 (256 byte)");
+
+static int align;
+module_param(align, int, S_IRUGO);
+MODULE_PARM_DESC(align, "Keep hardware FIFO write pointer aligned, default 0");
+
+struct men_z135_port {
+	struct uart_port port;
+	struct mcb_device *mdev;
+	unsigned char *rxbuf;
+	u32 stat_reg;
+	spinlock_t lock;
+};
+#define to_men_z135(port) container_of((port), struct men_z135_port, port)
+
+/**
+ * men_z135_reg_set() - Set value in register
+ * @uart: The UART port
+ * @addr: Register address
+ * @val: value to set
+ */
+static inline void men_z135_reg_set(struct men_z135_port *uart,
+				u32 addr, u32 val)
+{
+	struct uart_port *port = &uart->port;
+	unsigned long flags;
+	u32 reg;
+
+	spin_lock_irqsave(&uart->lock, flags);
+
+	reg = ioread32(port->membase + addr);
+	reg |= val;
+	iowrite32(reg, port->membase + addr);
+
+	spin_unlock_irqrestore(&uart->lock, flags);
+}
+
+/**
+ * men_z135_reg_clr() - Unset value in register
+ * @uart: The UART port
+ * @addr: Register address
+ * @val: value to clear
+ */
+static inline void men_z135_reg_clr(struct men_z135_port *uart,
+				u32 addr, u32 val)
+{
+	struct uart_port *port = &uart->port;
+	unsigned long flags;
+	u32 reg;
+
+	spin_lock_irqsave(&uart->lock, flags);
+
+	reg = ioread32(port->membase + addr);
+	reg &= ~val;
+	iowrite32(reg, port->membase + addr);
+
+	spin_unlock_irqrestore(&uart->lock, flags);
+}
+
+/**
+ * men_z135_handle_modem_status() - Handle change of modem status
+ * @port: The UART port
+ *
+ * Handle change of modem status register. This is done by reading the "delta"
+ * versions of DCD (Data Carrier Detect) and CTS (Clear To Send).
+ */
+static void men_z135_handle_modem_status(struct men_z135_port *uart)
+{
+	if (uart->stat_reg & MEN_Z135_MSR_DDCD)
+		uart_handle_dcd_change(&uart->port,
+				uart->stat_reg & ~MEN_Z135_MSR_DCD);
+	if (uart->stat_reg & MEN_Z135_MSR_DCTS)
+		uart_handle_cts_change(&uart->port,
+				uart->stat_reg & ~MEN_Z135_MSR_CTS);
+}
+
+static void men_z135_handle_lsr(struct men_z135_port *uart)
+{
+	struct uart_port *port = &uart->port;
+	u8 lsr;
+
+	lsr = (uart->stat_reg >> 16) & 0xff;
+
+	if (lsr & MEN_Z135_LSR_OE)
+		port->icount.overrun++;
+	if (lsr & MEN_Z135_LSR_PE)
+		port->icount.parity++;
+	if (lsr & MEN_Z135_LSR_FE)
+		port->icount.frame++;
+	if (lsr & MEN_Z135_LSR_BI) {
+		port->icount.brk++;
+		uart_handle_break(port);
+	}
+}
+
+/**
+ * get_rx_fifo_content() - Get the number of bytes in RX FIFO
+ * @uart: The UART port
+ *
+ * Read RXC register from hardware and return current FIFO fill size.
+ */
+static u16 get_rx_fifo_content(struct men_z135_port *uart)
+{
+	struct uart_port *port = &uart->port;
+	u32 stat_reg;
+	u16 rxc;
+	u8 rxc_lo;
+	u8 rxc_hi;
+
+	stat_reg = ioread32(port->membase + MEN_Z135_STAT_REG);
+	rxc_lo = stat_reg >> 24;
+	rxc_hi = (stat_reg & 0xC0) >> 6;
+
+	rxc = rxc_lo | (rxc_hi << 8);
+
+	return rxc;
+}
+
+/**
+ * men_z135_handle_rx() - RX tasklet routine
+ * @arg: Pointer to struct men_z135_port
+ *
+ * Copy from RX FIFO and acknowledge number of bytes copied.
+ */
+static void men_z135_handle_rx(struct men_z135_port *uart)
+{
+	struct uart_port *port = &uart->port;
+	struct tty_port *tport = &port->state->port;
+	int copied;
+	u16 size;
+	int room;
+
+	size = get_rx_fifo_content(uart);
+
+	if (size == 0)
+		return;
+
+	/* Avoid accidently accessing TX FIFO instead of RX FIFO. Last
+	 * longword in RX FIFO cannot be read.(0x004-0x3FF)
+	 */
+	if (size > MEN_Z135_FIFO_WATERMARK)
+		size = MEN_Z135_FIFO_WATERMARK;
+
+	room = tty_buffer_request_room(tport, size);
+	if (room != size)
+		dev_warn(&uart->mdev->dev,
+			"Not enough room in flip buffer, truncating to %d\n",
+			room);
+
+	if (room == 0)
+		return;
+
+	memcpy_fromio(uart->rxbuf, port->membase + MEN_Z135_RX_RAM, room);
+	/* Be sure to first copy all data and then acknowledge it */
+	mb();
+	iowrite32(room, port->membase +  MEN_Z135_RX_CTRL);
+
+	copied = tty_insert_flip_string(tport, uart->rxbuf, room);
+	if (copied != room)
+		dev_warn(&uart->mdev->dev,
+			"Only copied %d instead of %d bytes\n",
+			copied, room);
+
+	port->icount.rx += copied;
+
+	tty_flip_buffer_push(tport);
+
+}
+
+/**
+ * men_z135_handle_tx() - TX tasklet routine
+ * @arg: Pointer to struct men_z135_port
+ *
+ */
+static void men_z135_handle_tx(struct men_z135_port *uart)
+{
+	struct uart_port *port = &uart->port;
+	struct circ_buf *xmit = &port->state->xmit;
+	u32 txc;
+	u32 wptr;
+	int qlen;
+	int n;
+	int txfree;
+	int head;
+	int tail;
+	int s;
+
+	if (uart_circ_empty(xmit))
+		goto out;
+
+	if (uart_tx_stopped(port))
+		goto out;
+
+	if (port->x_char)
+		goto out;
+
+	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+		uart_write_wakeup(port);
+
+	/* calculate bytes to copy */
+	qlen = uart_circ_chars_pending(xmit);
+	if (qlen <= 0)
+		goto out;
+
+	wptr = ioread32(port->membase + MEN_Z135_TX_CTRL);
+	txc = (wptr >> 16) & 0x3ff;
+	wptr &= 0x3ff;
+
+	if (txc > MEN_Z135_FIFO_WATERMARK)
+		txc = MEN_Z135_FIFO_WATERMARK;
+
+	txfree = MEN_Z135_FIFO_WATERMARK - txc;
+	if (txfree <= 0) {
+		pr_err("Not enough room in TX FIFO have %d, need %d\n",
+			txfree, qlen);
+		goto irq_en;
+	}
+
+	/* if we're not aligned, it's better to copy only 1 or 2 bytes and
+	 * then the rest.
+	 */
+	if (align && qlen >= 3 && BYTES_TO_ALIGN(wptr))
+		n = 4 - BYTES_TO_ALIGN(wptr);
+	else if (qlen > txfree)
+		n = txfree;
+	else
+		n = qlen;
+
+	if (n <= 0)
+		goto irq_en;
+
+	head = xmit->head & (UART_XMIT_SIZE - 1);
+	tail = xmit->tail & (UART_XMIT_SIZE - 1);
+
+	s = ((head >= tail) ? head : UART_XMIT_SIZE) - tail;
+	n = min(n, s);
+
+	memcpy_toio(port->membase + MEN_Z135_TX_RAM, &xmit->buf[xmit->tail], n);
+	xmit->tail = (xmit->tail + n) & (UART_XMIT_SIZE - 1);
+	mmiowb();
+
+	iowrite32(n & 0x3ff, port->membase + MEN_Z135_TX_CTRL);
+
+	port->icount.tx += n;
+
+irq_en:
+	if (!uart_circ_empty(xmit))
+		men_z135_reg_set(uart, MEN_Z135_CONF_REG, MEN_Z135_IER_TXCIEN);
+	else
+		men_z135_reg_clr(uart, MEN_Z135_CONF_REG, MEN_Z135_IER_TXCIEN);
+
+out:
+	return;
+
+}
+
+/**
+ * men_z135_intr() - Handle legacy IRQs
+ * @irq: The IRQ number
+ * @data: Pointer to UART port
+ *
+ * Check IIR register to see which tasklet to start.
+ */
+static irqreturn_t men_z135_intr(int irq, void *data)
+{
+	struct men_z135_port *uart = (struct men_z135_port *)data;
+	struct uart_port *port = &uart->port;
+	int irq_id;
+
+	uart->stat_reg = ioread32(port->membase + MEN_Z135_STAT_REG);
+	/* IRQ pending is low active */
+	if (IS_IRQ(uart->stat_reg))
+		return IRQ_NONE;
+
+	irq_id = IRQ_ID(uart->stat_reg);
+	switch (irq_id) {
+	case MEN_Z135_IRQ_ID_MST:
+		men_z135_handle_modem_status(uart);
+		break;
+	case MEN_Z135_IRQ_ID_TSA:
+		men_z135_handle_tx(uart);
+		break;
+	case MEN_Z135_IRQ_ID_CTI:
+		dev_dbg(&uart->mdev->dev, "Character Timeout Indication\n");
+		/* Fallthrough */
+	case MEN_Z135_IRQ_ID_RDA:
+		/* Reading data clears RX IRQ */
+		men_z135_handle_rx(uart);
+		break;
+	case MEN_Z135_IRQ_ID_RLS:
+		men_z135_handle_lsr(uart);
+		break;
+	default:
+		dev_warn(&uart->mdev->dev, "Unknown IRQ id %d\n", irq_id);
+		return IRQ_NONE;
+	}
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * men_z135_request_irq() - Request IRQ for 16z135 core
+ * @uart: z135 private uart port structure
+ *
+ * Request an IRQ for 16z135 to use. First try using MSI, if it fails
+ * fall back to using legacy interrupts.
+ */
+static int men_z135_request_irq(struct men_z135_port *uart)
+{
+	struct device *dev = &uart->mdev->dev;
+	struct uart_port *port = &uart->port;
+	int err = 0;
+
+	err = request_irq(port->irq, men_z135_intr, IRQF_SHARED,
+			"men_z135_intr", uart);
+	if (err)
+		dev_err(dev, "Error %d getting interrupt\n", err);
+
+	return err;
+}
+
+/**
+ * men_z135_tx_empty() - Handle tx_empty call
+ * @port: The UART port
+ *
+ * This function tests whether the TX FIFO and shifter for the port
+ * described by @port is empty.
+ */
+static unsigned int men_z135_tx_empty(struct uart_port *port)
+{
+	u32 wptr;
+	u16 txc;
+
+	wptr = ioread32(port->membase + MEN_Z135_TX_CTRL);
+	txc = (wptr >> 16) & 0x3ff;
+
+	if (txc == 0)
+		return TIOCSER_TEMT;
+	else
+		return 0;
+}
+
+/**
+ * men_z135_set_mctrl() - Set modem control lines
+ * @port: The UART port
+ * @mctrl: The modem control lines
+ *
+ * This function sets the modem control lines for a port described by @port
+ * to the state described by @mctrl
+ */
+static void men_z135_set_mctrl(struct uart_port *port, unsigned int mctrl)
+{
+	struct men_z135_port *uart = to_men_z135(port);
+	u32 conf_reg = 0;
+
+	if (mctrl & TIOCM_RTS)
+		conf_reg |= MEN_Z135_MCR_RTS;
+	if (mctrl & TIOCM_DTR)
+		conf_reg |= MEN_Z135_MCR_DTR;
+	if (mctrl & TIOCM_OUT1)
+		conf_reg |= MEN_Z135_MCR_OUT1;
+	if (mctrl & TIOCM_OUT2)
+		conf_reg |= MEN_Z135_MCR_OUT2;
+	if (mctrl & TIOCM_LOOP)
+		conf_reg |= MEN_Z135_MCR_LOOP;
+
+	men_z135_reg_set(uart, MEN_Z135_CONF_REG, conf_reg);
+}
+
+/**
+ * men_z135_get_mctrl() - Get modem control lines
+ * @port: The UART port
+ *
+ * Retruns the current state of modem control inputs.
+ */
+static unsigned int men_z135_get_mctrl(struct uart_port *port)
+{
+	unsigned int mctrl = 0;
+	u32 stat_reg;
+	u8 msr;
+
+	stat_reg = ioread32(port->membase + MEN_Z135_STAT_REG);
+
+	msr = ~((stat_reg >> 8) & 0xff);
+
+	if (msr & MEN_Z135_MSR_CTS)
+		mctrl |= TIOCM_CTS;
+	if (msr & MEN_Z135_MSR_DSR)
+		mctrl |= TIOCM_DSR;
+	if (msr & MEN_Z135_MSR_RI)
+		mctrl |= TIOCM_RI;
+	if (msr & MEN_Z135_MSR_DCD)
+		mctrl |= TIOCM_CAR;
+
+	return mctrl;
+}
+
+/**
+ * men_z135_stop_tx() - Stop transmitting characters
+ * @port: The UART port
+ *
+ * Stop transmitting characters. This might be due to CTS line becomming
+ * inactive or the tty layer indicating we want to stop transmission due to
+ * an XOFF character.
+ */
+static void men_z135_stop_tx(struct uart_port *port)
+{
+	struct men_z135_port *uart = to_men_z135(port);
+
+	men_z135_reg_clr(uart, MEN_Z135_CONF_REG, MEN_Z135_IER_TXCIEN);
+}
+
+/**
+ * men_z135_start_tx() - Start transmitting characters
+ * @port: The UART port
+ *
+ * Start transmitting character. This actually doesn't transmit anything, but
+ * fires off the TX tasklet.
+ */
+static void men_z135_start_tx(struct uart_port *port)
+{
+	struct men_z135_port *uart = to_men_z135(port);
+
+	men_z135_handle_tx(uart);
+}
+
+/**
+ * men_z135_stop_rx() - Stop receiving characters
+ * @port: The UART port
+ *
+ * Stop receiving characters; the port is in the process of being closed.
+ */
+static void men_z135_stop_rx(struct uart_port *port)
+{
+	struct men_z135_port *uart = to_men_z135(port);
+
+	men_z135_reg_clr(uart, MEN_Z135_CONF_REG, MEN_Z135_IER_RXCIEN);
+}
+
+/**
+ * men_z135_enable_ms() - Enable Modem Status
+ * port:
+ *
+ * Enable Modem Status IRQ.
+ */
+static void men_z135_enable_ms(struct uart_port *port)
+{
+	struct men_z135_port *uart = to_men_z135(port);
+
+	men_z135_reg_set(uart, MEN_Z135_CONF_REG, MEN_Z135_IER_MSIEN);
+}
+
+static int men_z135_startup(struct uart_port *port)
+{
+	struct men_z135_port *uart = to_men_z135(port);
+	int err;
+	u32 conf_reg = 0;
+
+	err = men_z135_request_irq(uart);
+	if (err)
+		return -ENODEV;
+
+	conf_reg = ioread32(port->membase + MEN_Z135_CONF_REG);
+
+	/* Activate all but TX space available IRQ */
+	conf_reg |= MEN_Z135_ALL_IRQS & ~MEN_Z135_IER_TXCIEN;
+	conf_reg &= ~(0xff << 16);
+	conf_reg |= (txlvl << 16);
+	conf_reg |= (rxlvl << 20);
+
+	iowrite32(conf_reg, port->membase + MEN_Z135_CONF_REG);
+
+	return 0;
+}
+
+static void men_z135_shutdown(struct uart_port *port)
+{
+	struct men_z135_port *uart = to_men_z135(port);
+	u32 conf_reg = 0;
+
+	conf_reg |= MEN_Z135_ALL_IRQS;
+
+	men_z135_reg_clr(uart, MEN_Z135_CONF_REG, conf_reg);
+
+	free_irq(uart->port.irq, uart);
+}
+
+static void men_z135_set_termios(struct uart_port *port,
+				struct ktermios *termios,
+				struct ktermios *old)
+{
+	unsigned int baud;
+	u32 conf_reg;
+	u32 bd_reg;
+	u32 uart_freq;
+	u8 lcr;
+
+	conf_reg = ioread32(port->membase + MEN_Z135_CONF_REG);
+	lcr = LCR(conf_reg);
+
+	/* byte size */
+	switch (termios->c_cflag & CSIZE) {
+	case CS5:
+		lcr |= MEN_Z135_WL5;
+		break;
+	case CS6:
+		lcr |= MEN_Z135_WL6;
+		break;
+	case CS7:
+		lcr |= MEN_Z135_WL7;
+		break;
+	case CS8:
+		lcr |= MEN_Z135_WL8;
+		break;
+	}
+
+	/* stop bits */
+	if (termios->c_cflag & CSTOPB)
+		lcr |= MEN_Z135_NSTB2 << MEN_Z135_STB_SHIFT;
+
+	/* parity */
+	if (termios->c_cflag & PARENB) {
+		lcr |= MEN_Z135_PAR_ENA << MEN_Z135_PEN_SHIFT;
+
+		if (termios->c_cflag & PARODD)
+			lcr |= MEN_Z135_PTY_ODD << MEN_Z135_PTY_SHIFT;
+		else
+			lcr |= MEN_Z135_PTY_EVN << MEN_Z135_PTY_SHIFT;
+	} else
+		lcr |= MEN_Z135_PAR_DIS << MEN_Z135_PEN_SHIFT;
+
+	termios->c_cflag &= ~CMSPAR; /* Mark/Space parity is not supported */
+
+	conf_reg |= lcr << MEN_Z135_LCR_SHIFT;
+	iowrite32(conf_reg, port->membase + MEN_Z135_CONF_REG);
+
+	uart_freq = ioread32(port->membase + MEN_Z135_UART_FREQ);
+	if (uart_freq == 0)
+		uart_freq = MEN_Z135_BASECLK;
+
+	baud = uart_get_baud_rate(port, termios, old, 0, uart_freq / 16);
+
+	spin_lock(&port->lock);
+	if (tty_termios_baud_rate(termios))
+		tty_termios_encode_baud_rate(termios, baud, baud);
+
+	bd_reg = uart_freq / (4 * baud);
+	iowrite32(bd_reg, port->membase + MEN_Z135_BAUD_REG);
+
+	uart_update_timeout(port, termios->c_cflag, baud);
+	spin_unlock(&port->lock);
+}
+
+static const char *men_z135_type(struct uart_port *port)
+{
+	return KBUILD_MODNAME;
+}
+
+static void men_z135_release_port(struct uart_port *port)
+{
+	iounmap(port->membase);
+	port->membase = NULL;
+
+	release_mem_region(port->mapbase, MEN_Z135_MEM_SIZE);
+}
+
+static int men_z135_request_port(struct uart_port *port)
+{
+	int size = MEN_Z135_MEM_SIZE;
+
+	if (!request_mem_region(port->mapbase, size, "men_z135_port"))
+		return -EBUSY;
+
+	port->membase = ioremap(port->mapbase, MEN_Z135_MEM_SIZE);
+	if (port->membase == NULL) {
+		release_mem_region(port->mapbase, MEN_Z135_MEM_SIZE);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void men_z135_config_port(struct uart_port *port, int type)
+{
+	port->type = PORT_MEN_Z135;
+	men_z135_request_port(port);
+}
+
+static int men_z135_verify_port(struct uart_port *port,
+				struct serial_struct *serinfo)
+{
+	return -EINVAL;
+}
+
+static struct uart_ops men_z135_ops = {
+	.tx_empty = men_z135_tx_empty,
+	.set_mctrl = men_z135_set_mctrl,
+	.get_mctrl = men_z135_get_mctrl,
+	.stop_tx = men_z135_stop_tx,
+	.start_tx = men_z135_start_tx,
+	.stop_rx = men_z135_stop_rx,
+	.enable_ms = men_z135_enable_ms,
+	.startup = men_z135_startup,
+	.shutdown = men_z135_shutdown,
+	.set_termios = men_z135_set_termios,
+	.type = men_z135_type,
+	.release_port = men_z135_release_port,
+	.request_port = men_z135_request_port,
+	.config_port = men_z135_config_port,
+	.verify_port = men_z135_verify_port,
+};
+
+static struct uart_driver men_z135_driver = {
+	.owner = THIS_MODULE,
+	.driver_name = KBUILD_MODNAME,
+	.dev_name = "ttyHSU",
+	.major = 0,
+	.minor = 0,
+	.nr = MEN_Z135_MAX_PORTS,
+};
+
+/**
+ * men_z135_probe() - Probe a z135 instance
+ * @mdev: The MCB device
+ * @id: The MCB device ID
+ *
+ * men_z135_probe does the basic setup of hardware resources and registers the
+ * new uart port to the tty layer.
+ */
+static int men_z135_probe(struct mcb_device *mdev,
+			const struct mcb_device_id *id)
+{
+	struct men_z135_port *uart;
+	struct resource *mem;
+	struct device *dev;
+	int err;
+
+	dev = &mdev->dev;
+
+	uart = devm_kzalloc(dev, sizeof(struct men_z135_port), GFP_KERNEL);
+	if (!uart)
+		return -ENOMEM;
+
+	uart->rxbuf = (unsigned char *)__get_free_page(GFP_KERNEL);
+	if (!uart->rxbuf)
+		return -ENOMEM;
+
+	mem = &mdev->mem;
+
+	mcb_set_drvdata(mdev, uart);
+
+	uart->port.uartclk = MEN_Z135_BASECLK * 16;
+	uart->port.fifosize = MEN_Z135_FIFO_SIZE;
+	uart->port.iotype = UPIO_MEM;
+	uart->port.ops = &men_z135_ops;
+	uart->port.irq = mcb_get_irq(mdev);
+	uart->port.iotype = UPIO_MEM;
+	uart->port.flags = UPF_BOOT_AUTOCONF | UPF_IOREMAP;
+	uart->port.line = line++;
+	uart->port.dev = dev;
+	uart->port.type = PORT_MEN_Z135;
+	uart->port.mapbase = mem->start;
+	uart->port.membase = NULL;
+	uart->mdev = mdev;
+
+	spin_lock_init(&uart->port.lock);
+	spin_lock_init(&uart->lock);
+
+	err = uart_add_one_port(&men_z135_driver, &uart->port);
+	if (err)
+		goto err;
+
+	return 0;
+
+err:
+	free_page((unsigned long) uart->rxbuf);
+	dev_err(dev, "Failed to add UART: %d\n", err);
+
+	return err;
+}
+
+/**
+ * men_z135_remove() - Remove a z135 instance from the system
+ *
+ * @mdev: The MCB device
+ */
+static void men_z135_remove(struct mcb_device *mdev)
+{
+	struct men_z135_port *uart = mcb_get_drvdata(mdev);
+
+	line--;
+	uart_remove_one_port(&men_z135_driver, &uart->port);
+	free_page((unsigned long) uart->rxbuf);
+}
+
+static const struct mcb_device_id men_z135_ids[] = {
+	{ .device = 0x87 },
+};
+MODULE_DEVICE_TABLE(mcb, men_z135_ids);
+
+static struct mcb_driver mcb_driver = {
+	.driver = {
+		.name = "z135-uart",
+		.owner = THIS_MODULE,
+	},
+	.probe = men_z135_probe,
+	.remove = men_z135_remove,
+	.id_table = men_z135_ids,
+};
+
+/**
+ * men_z135_init() - Driver Registration Routine
+ *
+ * men_z135_init is the first routine called when the driver is loaded. All it
+ * does is register with the legacy MEN Chameleon subsystem.
+ */
+static int __init men_z135_init(void)
+{
+	int err;
+
+	err = uart_register_driver(&men_z135_driver);
+	if (err) {
+		pr_err("Failed to register UART: %d\n", err);
+		return err;
+	}
+
+	err = mcb_register_driver(&mcb_driver);
+	if  (err) {
+		pr_err("Failed to register MCB driver: %d\n", err);
+		uart_unregister_driver(&men_z135_driver);
+		return err;
+	}
+
+	return 0;
+}
+module_init(men_z135_init);
+
+/**
+ * men_z135_exit() - Driver Exit Routine
+ *
+ * men_z135_exit is called just before the driver is removed from memory.
+ */
+static void __exit men_z135_exit(void)
+{
+	mcb_unregister_driver(&mcb_driver);
+	uart_unregister_driver(&men_z135_driver);
+}
+module_exit(men_z135_exit);
+
+MODULE_AUTHOR("Johannes Thumshirn <johannes.thumshirn@men.de>");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("MEN 16z135 High Speed UART");
+MODULE_ALIAS("mcb:16z135");
diff --git a/drivers/tty/serial/msm_serial.c b/drivers/tty/serial/msm_serial.c
index 053b98eb46c..778e376f197 100644
--- a/drivers/tty/serial/msm_serial.c
+++ b/drivers/tty/serial/msm_serial.c
@@ -52,7 +52,6 @@ struct msm_port {
 	struct clk		*clk;
 	struct clk		*pclk;
 	unsigned int		imr;
-	void __iomem		*gsbi_base;
 	int			is_uartdm;
 	unsigned int		old_snap_state;
 };
@@ -599,9 +598,7 @@ static const char *msm_type(struct uart_port *port)
 static void msm_release_port(struct uart_port *port)
 {
 	struct platform_device *pdev = to_platform_device(port->dev);
-	struct msm_port *msm_port = UART_TO_MSM(port);
 	struct resource *uart_resource;
-	struct resource *gsbi_resource;
 	resource_size_t size;
 
 	uart_resource = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -612,28 +609,12 @@ static void msm_release_port(struct uart_port *port)
 	release_mem_region(port->mapbase, size);
 	iounmap(port->membase);
 	port->membase = NULL;
-
-	if (msm_port->gsbi_base) {
-		writel_relaxed(GSBI_PROTOCOL_IDLE,
-				msm_port->gsbi_base + GSBI_CONTROL);
-
-		gsbi_resource = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-		if (unlikely(!gsbi_resource))
-			return;
-
-		size = resource_size(gsbi_resource);
-		release_mem_region(gsbi_resource->start, size);
-		iounmap(msm_port->gsbi_base);
-		msm_port->gsbi_base = NULL;
-	}
 }
 
 static int msm_request_port(struct uart_port *port)
 {
-	struct msm_port *msm_port = UART_TO_MSM(port);
 	struct platform_device *pdev = to_platform_device(port->dev);
 	struct resource *uart_resource;
-	struct resource *gsbi_resource;
 	resource_size_t size;
 	int ret;
 
@@ -652,30 +633,8 @@ static int msm_request_port(struct uart_port *port)
 		goto fail_release_port;
 	}
 
-	gsbi_resource = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-	/* Is this a GSBI-based port? */
-	if (gsbi_resource) {
-		size = resource_size(gsbi_resource);
-
-		if (!request_mem_region(gsbi_resource->start, size,
-						 "msm_serial")) {
-			ret = -EBUSY;
-			goto fail_release_port_membase;
-		}
-
-		msm_port->gsbi_base = ioremap(gsbi_resource->start, size);
-		if (!msm_port->gsbi_base) {
-			ret = -EBUSY;
-			goto fail_release_gsbi;
-		}
-	}
-
 	return 0;
 
-fail_release_gsbi:
-	release_mem_region(gsbi_resource->start, size);
-fail_release_port_membase:
-	iounmap(port->membase);
 fail_release_port:
 	release_mem_region(port->mapbase, size);
 	return ret;
@@ -683,7 +642,6 @@ fail_release_port:
 
 static void msm_config_port(struct uart_port *port, int flags)
 {
-	struct msm_port *msm_port = UART_TO_MSM(port);
 	int ret;
 	if (flags & UART_CONFIG_TYPE) {
 		port->type = PORT_MSM;
@@ -691,9 +649,6 @@ static void msm_config_port(struct uart_port *port, int flags)
 		if (ret)
 			return;
 	}
-	if (msm_port->gsbi_base)
-		writel_relaxed(GSBI_PROTOCOL_UART,
-				msm_port->gsbi_base + GSBI_CONTROL);
 }
 
 static int msm_verify_port(struct uart_port *port, struct serial_struct *ser)
@@ -1110,6 +1065,7 @@ static struct of_device_id msm_match_table[] = {
 
 static struct platform_driver msm_platform_driver = {
 	.remove = msm_serial_remove,
+	.probe = msm_serial_probe,
 	.driver = {
 		.name = "msm_serial",
 		.owner = THIS_MODULE,
@@ -1125,7 +1081,7 @@ static int __init msm_serial_init(void)
 	if (unlikely(ret))
 		return ret;
 
-	ret = platform_driver_probe(&msm_platform_driver, msm_serial_probe);
+	ret = platform_driver_register(&msm_platform_driver);
 	if (unlikely(ret))
 		uart_unregister_driver(&msm_uart_driver);
 
diff --git a/drivers/tty/serial/msm_serial.h b/drivers/tty/serial/msm_serial.h
index 1e9b68b6f9e..d98d45efdf8 100644
--- a/drivers/tty/serial/msm_serial.h
+++ b/drivers/tty/serial/msm_serial.h
@@ -109,11 +109,6 @@
 #define UART_ISR		0x0014
 #define UART_ISR_TX_READY	(1 << 7)
 
-#define GSBI_CONTROL		0x0
-#define GSBI_PROTOCOL_CODE	0x30
-#define GSBI_PROTOCOL_UART	0x40
-#define GSBI_PROTOCOL_IDLE	0x0
-
 #define UARTDM_RXFS		0x50
 #define UARTDM_RXFS_BUF_SHIFT	0x7
 #define UARTDM_RXFS_BUF_MASK	0x7
diff --git a/drivers/tty/serial/mux.c b/drivers/tty/serial/mux.c
index 7fd6aaaacd8..be127d0da32 100644
--- a/drivers/tty/serial/mux.c
+++ b/drivers/tty/serial/mux.c
@@ -29,7 +29,7 @@
 #include <asm/irq.h>
 #include <asm/parisc-device.h>
 
-#ifdef CONFIG_MAGIC_SYSRQ
+#if defined(CONFIG_SERIAL_MUX_CONSOLE) && defined(CONFIG_MAGIC_SYSRQ)
 #include <linux/sysrq.h>
 #define SUPPORT_SYSRQ
 #endif
@@ -613,7 +613,7 @@ static void __exit mux_exit(void)
 {
 	/* Delete the Mux timer. */
 	if(port_cnt > 0) {
-		del_timer(&mux_timer);
+		del_timer_sync(&mux_timer);
 #ifdef CONFIG_SERIAL_MUX_CONSOLE
 		unregister_console(&mux_console);
 #endif
diff --git a/drivers/tty/serial/mxs-auart.c b/drivers/tty/serial/mxs-auart.c
index aa97fd845b4..4b5b3c2fe32 100644
--- a/drivers/tty/serial/mxs-auart.c
+++ b/drivers/tty/serial/mxs-auart.c
@@ -200,7 +200,7 @@ static void dma_tx_callback(void *param)
 
 	/* clear the bit used to serialize the DMA tx. */
 	clear_bit(MXS_AUART_DMA_TX_SYNC, &s->flags);
-	smp_mb__after_clear_bit();
+	smp_mb__after_atomic();
 
 	/* wake up the possible processes. */
 	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
@@ -275,7 +275,7 @@ static void mxs_auart_tx_chars(struct mxs_auart_port *s)
 			mxs_auart_dma_tx(s, i);
 		} else {
 			clear_bit(MXS_AUART_DMA_TX_SYNC, &s->flags);
-			smp_mb__after_clear_bit();
+			smp_mb__after_atomic();
 		}
 		return;
 	}
diff --git a/drivers/tty/serial/of_serial.c b/drivers/tty/serial/of_serial.c
index 99246606a25..68d4455f3cf 100644
--- a/drivers/tty/serial/of_serial.c
+++ b/drivers/tty/serial/of_serial.c
@@ -173,6 +173,7 @@ static int of_platform_serial_probe(struct platform_device *ofdev)
 	{
 		struct uart_8250_port port8250;
 		memset(&port8250, 0, sizeof(port8250));
+		port.type = port_type;
 		port8250.port = port;
 
 		if (port.fifosize)
@@ -182,6 +183,10 @@ 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;
 	}
diff --git a/drivers/tty/serial/omap-serial.c b/drivers/tty/serial/omap-serial.c
index 08b6b9419f0..d017cec8a34 100644
--- a/drivers/tty/serial/omap-serial.c
+++ b/drivers/tty/serial/omap-serial.c
@@ -163,10 +163,6 @@ struct uart_omap_port {
 	u8			wakeups_enabled;
 	u32			features;
 
-	int			DTR_gpio;
-	int			DTR_inverted;
-	int			DTR_active;
-
 	struct serial_rs485	rs485;
 	int			rts_gpio;
 
@@ -184,8 +180,6 @@ static struct uart_omap_port *ui[OMAP_MAX_HSUART_PORTS];
 /* Forward declaration of functions */
 static void serial_omap_mdr1_errataset(struct uart_omap_port *up, u8 mdr1);
 
-static struct workqueue_struct *serial_omap_uart_wq;
-
 static inline unsigned int serial_in(struct uart_omap_port *up, int offset)
 {
 	offset <<= up->port.regshift;
@@ -398,11 +392,8 @@ static void transmit_chars(struct uart_omap_port *up, unsigned int lsr)
 			break;
 	} while (--count > 0);
 
-	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS) {
-		spin_unlock(&up->port.lock);
+	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
 		uart_write_wakeup(&up->port);
-		spin_lock(&up->port.lock);
-	}
 
 	if (uart_circ_empty(xmit))
 		serial_omap_stop_tx(&up->port);
@@ -697,16 +688,6 @@ static void serial_omap_set_mctrl(struct uart_port *port, unsigned int mctrl)
 	serial_out(up, UART_MCR, up->mcr);
 	pm_runtime_mark_last_busy(up->dev);
 	pm_runtime_put_autosuspend(up->dev);
-
-	if (gpio_is_valid(up->DTR_gpio) &&
-	    !!(mctrl & TIOCM_DTR) != up->DTR_active) {
-		up->DTR_active = !up->DTR_active;
-		if (gpio_cansleep(up->DTR_gpio))
-			schedule_work(&up->qos_work);
-		else
-			gpio_set_value(up->DTR_gpio,
-				       up->DTR_active != up->DTR_inverted);
-	}
 }
 
 static void serial_omap_break_ctl(struct uart_port *port, int break_state)
@@ -850,9 +831,6 @@ static void serial_omap_uart_qos_work(struct work_struct *work)
 						qos_work);
 
 	pm_qos_update_request(&up->pm_qos_request, up->latency);
-	if (gpio_is_valid(up->DTR_gpio))
-		gpio_set_value_cansleep(up->DTR_gpio,
-					up->DTR_active != up->DTR_inverted);
 }
 
 static void
@@ -1420,7 +1398,7 @@ serial_omap_ioctl(struct uart_port *port, unsigned int cmd, unsigned long arg)
 
 	switch (cmd) {
 	case TIOCSRS485:
-		if (copy_from_user(&rs485conf, (struct serial_rs485 *) arg,
+		if (copy_from_user(&rs485conf, (void __user *) arg,
 					sizeof(rs485conf)))
 			return -EFAULT;
 
@@ -1428,7 +1406,7 @@ serial_omap_ioctl(struct uart_port *port, unsigned int cmd, unsigned long arg)
 		break;
 
 	case TIOCGRS485:
-		if (copy_to_user((struct serial_rs485 *) arg,
+		if (copy_to_user((void __user *) arg,
 					&(to_uart_omap_port(port)->rs485),
 					sizeof(rs485conf)))
 			return -EFAULT;
@@ -1614,7 +1592,7 @@ static int serial_omap_probe_rs485(struct uart_omap_port *up,
 	/* check for tx enable gpio */
 	up->rts_gpio = of_get_named_gpio_flags(np, "rts-gpio", 0, &flags);
 	if (gpio_is_valid(up->rts_gpio)) {
-		ret = gpio_request(up->rts_gpio, "omap-serial");
+		ret = devm_gpio_request(up->dev, up->rts_gpio, "omap-serial");
 		if (ret < 0)
 			return ret;
 		ret = gpio_direction_output(up->rts_gpio,
@@ -1644,10 +1622,13 @@ static int serial_omap_probe_rs485(struct uart_omap_port *up,
 
 static int serial_omap_probe(struct platform_device *pdev)
 {
-	struct uart_omap_port	*up;
-	struct resource		*mem, *irq;
 	struct omap_uart_port_info *omap_up_info = dev_get_platdata(&pdev->dev);
-	int ret, uartirq = 0, wakeirq = 0;
+	struct uart_omap_port *up;
+	struct resource *mem;
+	void __iomem *base;
+	int uartirq = 0;
+	int wakeirq = 0;
+	int ret;
 
 	/* The optional wakeirq may be specified in the board dts file */
 	if (pdev->dev.of_node) {
@@ -1658,48 +1639,19 @@ static int serial_omap_probe(struct platform_device *pdev)
 		omap_up_info = of_get_uart_port_info(&pdev->dev);
 		pdev->dev.platform_data = omap_up_info;
 	} else {
-		irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-		if (!irq) {
-			dev_err(&pdev->dev, "no irq resource?\n");
-			return -ENODEV;
-		}
-		uartirq = irq->start;
-	}
-
-	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!mem) {
-		dev_err(&pdev->dev, "no mem resource?\n");
-		return -ENODEV;
-	}
-
-	if (!devm_request_mem_region(&pdev->dev, mem->start, resource_size(mem),
-				pdev->dev.driver->name)) {
-		dev_err(&pdev->dev, "memory region already claimed\n");
-		return -EBUSY;
-	}
-
-	if (gpio_is_valid(omap_up_info->DTR_gpio) &&
-	    omap_up_info->DTR_present) {
-		ret = gpio_request(omap_up_info->DTR_gpio, "omap-serial");
-		if (ret < 0)
-			return ret;
-		ret = gpio_direction_output(omap_up_info->DTR_gpio,
-					    omap_up_info->DTR_inverted);
-		if (ret < 0)
-			return ret;
+		uartirq = platform_get_irq(pdev, 0);
+		if (uartirq < 0)
+			return -EPROBE_DEFER;
 	}
 
 	up = devm_kzalloc(&pdev->dev, sizeof(*up), GFP_KERNEL);
 	if (!up)
 		return -ENOMEM;
 
-	if (gpio_is_valid(omap_up_info->DTR_gpio) &&
-	    omap_up_info->DTR_present) {
-		up->DTR_gpio = omap_up_info->DTR_gpio;
-		up->DTR_inverted = omap_up_info->DTR_inverted;
-	} else
-		up->DTR_gpio = -EINVAL;
-	up->DTR_active = 0;
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	base = devm_ioremap_resource(&pdev->dev, mem);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
 
 	up->dev = &pdev->dev;
 	up->port.dev = &pdev->dev;
@@ -1733,14 +1685,7 @@ static int serial_omap_probe(struct platform_device *pdev)
 
 	sprintf(up->name, "OMAP UART%d", up->port.line);
 	up->port.mapbase = mem->start;
-	up->port.membase = devm_ioremap(&pdev->dev, mem->start,
-						resource_size(mem));
-	if (!up->port.membase) {
-		dev_err(&pdev->dev, "can't ioremap UART\n");
-		ret = -ENOMEM;
-		goto err_ioremap;
-	}
-
+	up->port.membase = base;
 	up->port.flags = omap_up_info->flags;
 	up->port.uartclk = omap_up_info->uartclk;
 	if (!up->port.uartclk) {
@@ -1754,12 +1699,12 @@ static int serial_omap_probe(struct platform_device *pdev)
 	up->calc_latency = PM_QOS_CPU_DMA_LAT_DEFAULT_VALUE;
 	pm_qos_add_request(&up->pm_qos_request,
 		PM_QOS_CPU_DMA_LATENCY, up->latency);
-	serial_omap_uart_wq = create_singlethread_workqueue(up->name);
 	INIT_WORK(&up->qos_work, serial_omap_uart_qos_work);
 
 	platform_set_drvdata(pdev, up);
 	if (omap_up_info->autosuspend_timeout == 0)
 		omap_up_info->autosuspend_timeout = -1;
+
 	device_init_wakeup(up->dev, true);
 	pm_runtime_use_autosuspend(&pdev->dev);
 	pm_runtime_set_autosuspend_delay(&pdev->dev,
@@ -1786,7 +1731,6 @@ static int serial_omap_probe(struct platform_device *pdev)
 err_add_port:
 	pm_runtime_put(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
-err_ioremap:
 err_rs485:
 err_port_line:
 	dev_err(&pdev->dev, "[UART%d]: failure [%s]: %d\n",
diff --git a/drivers/tty/serial/pch_uart.c b/drivers/tty/serial/pch_uart.c
index 0931b3fe9ed..0cb6a8e52bd 100644
--- a/drivers/tty/serial/pch_uart.c
+++ b/drivers/tty/serial/pch_uart.c
@@ -257,6 +257,8 @@ struct eg20t_port {
 	dma_addr_t			rx_buf_dma;
 
 	struct dentry	*debugfs;
+#define IRQ_NAME_SIZE 17
+	char				irq_name[IRQ_NAME_SIZE];
 
 	/* protect the eg20t_port private structure and io access to membase */
 	spinlock_t lock;
@@ -1343,7 +1345,7 @@ static int pch_uart_startup(struct uart_port *port)
 		return ret;
 
 	ret = request_irq(priv->port.irq, pch_uart_interrupt, IRQF_SHARED,
-			KBUILD_MODNAME, priv);
+			priv->irq_name, priv);
 	if (ret < 0)
 		return ret;
 
@@ -1588,13 +1590,8 @@ static void pch_uart_put_poll_char(struct uart_port *port,
 	wait_for_xmitr(priv, UART_LSR_THRE);
 	/*
 	 * Send the character out.
-	 * If a LF, also do CR...
 	 */
 	iowrite8(c, priv->membase + PCH_UART_THR);
-	if (c == 10) {
-		wait_for_xmitr(priv, UART_LSR_THRE);
-		iowrite8(13, priv->membase + PCH_UART_THR);
-	}
 
 	/*
 	 * Finally, wait for transmitter to become empty
@@ -1818,6 +1815,10 @@ static struct eg20t_port *pch_uart_init_port(struct pci_dev *pdev,
 	priv->port.line = board->line_no;
 	priv->trigger = PCH_UART_HAL_TRIGGER_M;
 
+	snprintf(priv->irq_name, IRQ_NAME_SIZE,
+		 KBUILD_MODNAME ":" PCH_UART_DRIVER_DEVICE "%d",
+		 priv->port.line);
+
 	spin_lock_init(&priv->port.lock);
 
 	pci_set_drvdata(pdev, priv);
diff --git a/drivers/tty/serial/pxa.c b/drivers/tty/serial/pxa.c
index f9f20f38376..9e7ee39f8b2 100644
--- a/drivers/tty/serial/pxa.c
+++ b/drivers/tty/serial/pxa.c
@@ -711,13 +711,8 @@ static void serial_pxa_put_poll_char(struct uart_port *port,
 	wait_for_xmitr(up);
 	/*
 	 *	Send the character out.
-	 *	If a LF, also do CR...
 	 */
 	serial_out(up, UART_TX, c);
-	if (c == 10) {
-		wait_for_xmitr(up);
-		serial_out(up, UART_TX, 13);
-	}
 
 	/*
 	 *	Finally, wait for transmitter to become empty
diff --git a/drivers/tty/serial/samsung.c b/drivers/tty/serial/samsung.c
index 1f5505e7f90..329337711bb 100644
--- a/drivers/tty/serial/samsung.c
+++ b/drivers/tty/serial/samsung.c
@@ -53,6 +53,29 @@
 
 #include "samsung.h"
 
+#if	defined(CONFIG_SERIAL_SAMSUNG_DEBUG) &&	\
+	defined(CONFIG_DEBUG_LL) &&		\
+	!defined(MODULE)
+
+extern void printascii(const char *);
+
+__printf(1, 2)
+static void dbg(const char *fmt, ...)
+{
+	va_list va;
+	char buff[256];
+
+	va_start(va, fmt);
+	vscnprintf(buff, sizeof(buf), fmt, va);
+	va_end(va);
+
+	printascii(buff);
+}
+
+#else
+#define dbg(fmt, ...) do { if (0) no_printk(fmt, ##__VA_ARGS__); } while (0)
+#endif
+
 /* UART name and device definitions */
 
 #define S3C24XX_SERIAL_NAME	"ttySAC"
@@ -468,8 +491,8 @@ static int s3c24xx_serial_startup(struct uart_port *port)
 	struct s3c24xx_uart_port *ourport = to_ourport(port);
 	int ret;
 
-	dbg("s3c24xx_serial_startup: port=%p (%08lx,%p)\n",
-	    port->mapbase, port->membase);
+	dbg("s3c24xx_serial_startup: port=%p (%08llx,%p)\n",
+	    port, (unsigned long long)port->mapbase, port->membase);
 
 	rx_enabled(port) = 1;
 
@@ -514,8 +537,8 @@ static int s3c64xx_serial_startup(struct uart_port *port)
 	struct s3c24xx_uart_port *ourport = to_ourport(port);
 	int ret;
 
-	dbg("s3c64xx_serial_startup: port=%p (%08lx,%p)\n",
-	    port->mapbase, port->membase);
+	dbg("s3c64xx_serial_startup: port=%p (%08llx,%p)\n",
+	    port, (unsigned long long)port->mapbase, port->membase);
 
 	wr_regl(port, S3C64XX_UINTM, 0xf);
 
@@ -1160,7 +1183,7 @@ static int s3c24xx_serial_init_port(struct s3c24xx_uart_port *ourport,
 		return -EINVAL;
 	}
 
-	dbg("resource %p (%lx..%lx)\n", res, res->start, res->end);
+	dbg("resource %pR)\n", res);
 
 	port->membase = devm_ioremap(port->dev, res->start, resource_size(res));
 	if (!port->membase) {
@@ -1203,7 +1226,7 @@ static int s3c24xx_serial_init_port(struct s3c24xx_uart_port *ourport,
 		wr_regl(port, S3C64XX_UINTSP, 0xf);
 	}
 
-	dbg("port: map=%08x, mem=%08x, irq=%d (%d,%d), clock=%ld\n",
+	dbg("port: map=%08x, mem=%p, irq=%d (%d,%d), clock=%u\n",
 	    port->mapbase, port->membase, port->irq,
 	    ourport->rx_irq, ourport->tx_irq, port->uartclk);
 
diff --git a/drivers/tty/serial/samsung.h b/drivers/tty/serial/samsung.h
index 8827e5424ce..eb071dd19b2 100644
--- a/drivers/tty/serial/samsung.h
+++ b/drivers/tty/serial/samsung.h
@@ -1,3 +1,6 @@
+#ifndef __SAMSUNG_H
+#define __SAMSUNG_H
+
 /*
  * Driver for Samsung SoC onboard UARTs.
  *
@@ -77,24 +80,4 @@ struct s3c24xx_uart_port {
 #define wr_regb(port, reg, val) __raw_writeb(val, portaddr(port, reg))
 #define wr_regl(port, reg, val) __raw_writel(val, portaddr(port, reg))
 
-#if defined(CONFIG_SERIAL_SAMSUNG_DEBUG) && \
-    defined(CONFIG_DEBUG_LL) && \
-    !defined(MODULE)
-
-extern void printascii(const char *);
-
-static void dbg(const char *fmt, ...)
-{
-	va_list va;
-	char buff[256];
-
-	va_start(va, fmt);
-	vsprintf(buff, fmt, va);
-	va_end(va);
-
-	printascii(buff);
-}
-
-#else
-#define dbg(x...) do { } while (0)
 #endif
diff --git a/drivers/tty/serial/sc16is7xx.c b/drivers/tty/serial/sc16is7xx.c
new file mode 100644
index 00000000000..1b6a77c4b2c
--- /dev/null
+++ b/drivers/tty/serial/sc16is7xx.c
@@ -0,0 +1,1277 @@
+/*
+ * SC16IS7xx tty serial driver - Copyright (C) 2014 GridPoint
+ * Author: Jon Ringle <jringle@gridpoint.com>
+ *
+ *  Based on max310x.c, by 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/bitops.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/regmap.h>
+#include <linux/serial_core.h>
+#include <linux/serial.h>
+#include <linux/tty.h>
+#include <linux/tty_flip.h>
+#include <linux/uaccess.h>
+
+#define SC16IS7XX_NAME			"sc16is7xx"
+
+/* SC16IS7XX register definitions */
+#define SC16IS7XX_RHR_REG		(0x00) /* RX FIFO */
+#define SC16IS7XX_THR_REG		(0x00) /* TX FIFO */
+#define SC16IS7XX_IER_REG		(0x01) /* Interrupt enable */
+#define SC16IS7XX_IIR_REG		(0x02) /* Interrupt Identification */
+#define SC16IS7XX_FCR_REG		(0x02) /* FIFO control */
+#define SC16IS7XX_LCR_REG		(0x03) /* Line Control */
+#define SC16IS7XX_MCR_REG		(0x04) /* Modem Control */
+#define SC16IS7XX_LSR_REG		(0x05) /* Line Status */
+#define SC16IS7XX_MSR_REG		(0x06) /* Modem Status */
+#define SC16IS7XX_SPR_REG		(0x07) /* Scratch Pad */
+#define SC16IS7XX_TXLVL_REG		(0x08) /* TX FIFO level */
+#define SC16IS7XX_RXLVL_REG		(0x09) /* RX FIFO level */
+#define SC16IS7XX_IODIR_REG		(0x0a) /* I/O Direction
+						* - only on 75x/76x
+						*/
+#define SC16IS7XX_IOSTATE_REG		(0x0b) /* I/O State
+						* - only on 75x/76x
+						*/
+#define SC16IS7XX_IOINTENA_REG		(0x0c) /* I/O Interrupt Enable
+						* - only on 75x/76x
+						*/
+#define SC16IS7XX_IOCONTROL_REG		(0x0e) /* I/O Control
+						* - only on 75x/76x
+						*/
+#define SC16IS7XX_EFCR_REG		(0x0f) /* Extra Features Control */
+
+/* TCR/TLR Register set: Only if ((MCR[2] == 1) && (EFR[4] == 1)) */
+#define SC16IS7XX_TCR_REG		(0x06) /* Transmit control */
+#define SC16IS7XX_TLR_REG		(0x07) /* Trigger level */
+
+/* Special Register set: Only if ((LCR[7] == 1) && (LCR != 0xBF)) */
+#define SC16IS7XX_DLL_REG		(0x00) /* Divisor Latch Low */
+#define SC16IS7XX_DLH_REG		(0x01) /* Divisor Latch High */
+
+/* Enhanced Register set: Only if (LCR == 0xBF) */
+#define SC16IS7XX_EFR_REG		(0x02) /* Enhanced Features */
+#define SC16IS7XX_XON1_REG		(0x04) /* Xon1 word */
+#define SC16IS7XX_XON2_REG		(0x05) /* Xon2 word */
+#define SC16IS7XX_XOFF1_REG		(0x06) /* Xoff1 word */
+#define SC16IS7XX_XOFF2_REG		(0x07) /* Xoff2 word */
+
+/* IER register bits */
+#define SC16IS7XX_IER_RDI_BIT		(1 << 0) /* Enable RX data interrupt */
+#define SC16IS7XX_IER_THRI_BIT		(1 << 1) /* Enable TX holding register
+						  * interrupt */
+#define SC16IS7XX_IER_RLSI_BIT		(1 << 2) /* Enable RX line status
+						  * interrupt */
+#define SC16IS7XX_IER_MSI_BIT		(1 << 3) /* Enable Modem status
+						  * interrupt */
+
+/* IER register bits - write only if (EFR[4] == 1) */
+#define SC16IS7XX_IER_SLEEP_BIT		(1 << 4) /* Enable Sleep mode */
+#define SC16IS7XX_IER_XOFFI_BIT		(1 << 5) /* Enable Xoff interrupt */
+#define SC16IS7XX_IER_RTSI_BIT		(1 << 6) /* Enable nRTS interrupt */
+#define SC16IS7XX_IER_CTSI_BIT		(1 << 7) /* Enable nCTS interrupt */
+
+/* FCR register bits */
+#define SC16IS7XX_FCR_FIFO_BIT		(1 << 0) /* Enable FIFO */
+#define SC16IS7XX_FCR_RXRESET_BIT	(1 << 1) /* Reset RX FIFO */
+#define SC16IS7XX_FCR_TXRESET_BIT	(1 << 2) /* Reset TX FIFO */
+#define SC16IS7XX_FCR_RXLVLL_BIT	(1 << 6) /* RX Trigger level LSB */
+#define SC16IS7XX_FCR_RXLVLH_BIT	(1 << 7) /* RX Trigger level MSB */
+
+/* FCR register bits - write only if (EFR[4] == 1) */
+#define SC16IS7XX_FCR_TXLVLL_BIT	(1 << 4) /* TX Trigger level LSB */
+#define SC16IS7XX_FCR_TXLVLH_BIT	(1 << 5) /* TX Trigger level MSB */
+
+/* IIR register bits */
+#define SC16IS7XX_IIR_NO_INT_BIT	(1 << 0) /* No interrupts pending */
+#define SC16IS7XX_IIR_ID_MASK		0x3e     /* Mask for the interrupt ID */
+#define SC16IS7XX_IIR_THRI_SRC		0x02     /* TX holding register empty */
+#define SC16IS7XX_IIR_RDI_SRC		0x04     /* RX data interrupt */
+#define SC16IS7XX_IIR_RLSE_SRC		0x06     /* RX line status error */
+#define SC16IS7XX_IIR_RTOI_SRC		0x0c     /* RX time-out interrupt */
+#define SC16IS7XX_IIR_MSI_SRC		0x00     /* Modem status interrupt
+						  * - only on 75x/76x
+						  */
+#define SC16IS7XX_IIR_INPIN_SRC		0x30     /* Input pin change of state
+						  * - only on 75x/76x
+						  */
+#define SC16IS7XX_IIR_XOFFI_SRC		0x10     /* Received Xoff */
+#define SC16IS7XX_IIR_CTSRTS_SRC	0x20     /* nCTS,nRTS change of state
+						  * from active (LOW)
+						  * to inactive (HIGH)
+						  */
+/* LCR register bits */
+#define SC16IS7XX_LCR_LENGTH0_BIT	(1 << 0) /* Word length bit 0 */
+#define SC16IS7XX_LCR_LENGTH1_BIT	(1 << 1) /* Word length bit 1
+						  *
+						  * Word length bits table:
+						  * 00 -> 5 bit words
+						  * 01 -> 6 bit words
+						  * 10 -> 7 bit words
+						  * 11 -> 8 bit words
+						  */
+#define SC16IS7XX_LCR_STOPLEN_BIT	(1 << 2) /* STOP length bit
+						  *
+						  * STOP length bit table:
+						  * 0 -> 1 stop bit
+						  * 1 -> 1-1.5 stop bits if
+						  *      word length is 5,
+						  *      2 stop bits otherwise
+						  */
+#define SC16IS7XX_LCR_PARITY_BIT	(1 << 3) /* Parity bit enable */
+#define SC16IS7XX_LCR_EVENPARITY_BIT	(1 << 4) /* Even parity bit enable */
+#define SC16IS7XX_LCR_FORCEPARITY_BIT	(1 << 5) /* 9-bit multidrop parity */
+#define SC16IS7XX_LCR_TXBREAK_BIT	(1 << 6) /* TX break enable */
+#define SC16IS7XX_LCR_DLAB_BIT		(1 << 7) /* Divisor Latch enable */
+#define SC16IS7XX_LCR_WORD_LEN_5	(0x00)
+#define SC16IS7XX_LCR_WORD_LEN_6	(0x01)
+#define SC16IS7XX_LCR_WORD_LEN_7	(0x02)
+#define SC16IS7XX_LCR_WORD_LEN_8	(0x03)
+#define SC16IS7XX_LCR_CONF_MODE_A	SC16IS7XX_LCR_DLAB_BIT /* Special
+								* reg set */
+#define SC16IS7XX_LCR_CONF_MODE_B	0xBF                   /* Enhanced
+								* reg set */
+
+/* MCR register bits */
+#define SC16IS7XX_MCR_DTR_BIT		(1 << 0) /* DTR complement
+						  * - only on 75x/76x
+						  */
+#define SC16IS7XX_MCR_RTS_BIT		(1 << 1) /* RTS complement */
+#define SC16IS7XX_MCR_TCRTLR_BIT	(1 << 2) /* TCR/TLR register enable */
+#define SC16IS7XX_MCR_LOOP_BIT		(1 << 4) /* Enable loopback test mode */
+#define SC16IS7XX_MCR_XONANY_BIT	(1 << 5) /* Enable Xon Any
+						  * - write enabled
+						  * if (EFR[4] == 1)
+						  */
+#define SC16IS7XX_MCR_IRDA_BIT		(1 << 6) /* Enable IrDA mode
+						  * - write enabled
+						  * if (EFR[4] == 1)
+						  */
+#define SC16IS7XX_MCR_CLKSEL_BIT	(1 << 7) /* Divide clock by 4
+						  * - write enabled
+						  * if (EFR[4] == 1)
+						  */
+
+/* LSR register bits */
+#define SC16IS7XX_LSR_DR_BIT		(1 << 0) /* Receiver data ready */
+#define SC16IS7XX_LSR_OE_BIT		(1 << 1) /* Overrun Error */
+#define SC16IS7XX_LSR_PE_BIT		(1 << 2) /* Parity Error */
+#define SC16IS7XX_LSR_FE_BIT		(1 << 3) /* Frame Error */
+#define SC16IS7XX_LSR_BI_BIT		(1 << 4) /* Break Interrupt */
+#define SC16IS7XX_LSR_BRK_ERROR_MASK	0x1E     /* BI, FE, PE, OE bits */
+#define SC16IS7XX_LSR_THRE_BIT		(1 << 5) /* TX holding register empty */
+#define SC16IS7XX_LSR_TEMT_BIT		(1 << 6) /* Transmitter empty */
+#define SC16IS7XX_LSR_FIFOE_BIT		(1 << 7) /* Fifo Error */
+
+/* MSR register bits */
+#define SC16IS7XX_MSR_DCTS_BIT		(1 << 0) /* Delta CTS Clear To Send */
+#define SC16IS7XX_MSR_DDSR_BIT		(1 << 1) /* Delta DSR Data Set Ready
+						  * or (IO4)
+						  * - only on 75x/76x
+						  */
+#define SC16IS7XX_MSR_DRI_BIT		(1 << 2) /* Delta RI Ring Indicator
+						  * or (IO7)
+						  * - only on 75x/76x
+						  */
+#define SC16IS7XX_MSR_DCD_BIT		(1 << 3) /* Delta CD Carrier Detect
+						  * or (IO6)
+						  * - only on 75x/76x
+						  */
+#define SC16IS7XX_MSR_CTS_BIT		(1 << 0) /* CTS */
+#define SC16IS7XX_MSR_DSR_BIT		(1 << 1) /* DSR (IO4)
+						  * - only on 75x/76x
+						  */
+#define SC16IS7XX_MSR_RI_BIT		(1 << 2) /* RI (IO7)
+						  * - only on 75x/76x
+						  */
+#define SC16IS7XX_MSR_CD_BIT		(1 << 3) /* CD (IO6)
+						  * - only on 75x/76x
+						  */
+#define SC16IS7XX_MSR_DELTA_MASK	0x0F     /* Any of the delta bits! */
+
+/*
+ * TCR register bits
+ * TCR trigger levels are available from 0 to 60 characters with a granularity
+ * of four.
+ * The programmer must program the TCR such that TCR[3:0] > TCR[7:4]. There is
+ * no built-in hardware check to make sure this condition is met. Also, the TCR
+ * must be programmed with this condition before auto RTS or software flow
+ * control is enabled to avoid spurious operation of the device.
+ */
+#define SC16IS7XX_TCR_RX_HALT(words)	((((words) / 4) & 0x0f) << 0)
+#define SC16IS7XX_TCR_RX_RESUME(words)	((((words) / 4) & 0x0f) << 4)
+
+/*
+ * TLR register bits
+ * If TLR[3:0] or TLR[7:4] are logical 0, the selectable trigger levels via the
+ * FIFO Control Register (FCR) are used for the transmit and receive FIFO
+ * trigger levels. Trigger levels from 4 characters to 60 characters are
+ * available with a granularity of four.
+ *
+ * When the trigger level setting in TLR is zero, the SC16IS740/750/760 uses the
+ * trigger level setting defined in FCR. If TLR has non-zero trigger level value
+ * the trigger level defined in FCR is discarded. This applies to both transmit
+ * FIFO and receive FIFO trigger level setting.
+ *
+ * When TLR is used for RX trigger level control, FCR[7:6] should be left at the
+ * default state, that is, '00'.
+ */
+#define SC16IS7XX_TLR_TX_TRIGGER(words)	((((words) / 4) & 0x0f) << 0)
+#define SC16IS7XX_TLR_RX_TRIGGER(words)	((((words) / 4) & 0x0f) << 4)
+
+/* IOControl register bits (Only 750/760) */
+#define SC16IS7XX_IOCONTROL_LATCH_BIT	(1 << 0) /* Enable input latching */
+#define SC16IS7XX_IOCONTROL_GPIO_BIT	(1 << 1) /* Enable GPIO[7:4] */
+#define SC16IS7XX_IOCONTROL_SRESET_BIT	(1 << 3) /* Software Reset */
+
+/* EFCR register bits */
+#define SC16IS7XX_EFCR_9BIT_MODE_BIT	(1 << 0) /* Enable 9-bit or Multidrop
+						  * mode (RS485) */
+#define SC16IS7XX_EFCR_RXDISABLE_BIT	(1 << 1) /* Disable receiver */
+#define SC16IS7XX_EFCR_TXDISABLE_BIT	(1 << 2) /* Disable transmitter */
+#define SC16IS7XX_EFCR_AUTO_RS485_BIT	(1 << 4) /* Auto RS485 RTS direction */
+#define SC16IS7XX_EFCR_RTS_INVERT_BIT	(1 << 5) /* RTS output inversion */
+#define SC16IS7XX_EFCR_IRDA_MODE_BIT	(1 << 7) /* IrDA mode
+						  * 0 = rate upto 115.2 kbit/s
+						  *   - Only 750/760
+						  * 1 = rate upto 1.152 Mbit/s
+						  *   - Only 760
+						  */
+
+/* EFR register bits */
+#define SC16IS7XX_EFR_AUTORTS_BIT	(1 << 6) /* Auto RTS flow ctrl enable */
+#define SC16IS7XX_EFR_AUTOCTS_BIT	(1 << 7) /* Auto CTS flow ctrl enable */
+#define SC16IS7XX_EFR_XOFF2_DETECT_BIT	(1 << 5) /* Enable Xoff2 detection */
+#define SC16IS7XX_EFR_ENABLE_BIT	(1 << 4) /* Enable enhanced functions
+						  * and writing to IER[7:4],
+						  * FCR[5:4], MCR[7:5]
+						  */
+#define SC16IS7XX_EFR_SWFLOW3_BIT	(1 << 3) /* SWFLOW bit 3 */
+#define SC16IS7XX_EFR_SWFLOW2_BIT	(1 << 2) /* SWFLOW bit 2
+						  *
+						  * SWFLOW bits 3 & 2 table:
+						  * 00 -> no transmitter flow
+						  *       control
+						  * 01 -> transmitter generates
+						  *       XON2 and XOFF2
+						  * 10 -> transmitter generates
+						  *       XON1 and XOFF1
+						  * 11 -> transmitter generates
+						  *       XON1, XON2, XOFF1 and
+						  *       XOFF2
+						  */
+#define SC16IS7XX_EFR_SWFLOW1_BIT	(1 << 1) /* SWFLOW bit 2 */
+#define SC16IS7XX_EFR_SWFLOW0_BIT	(1 << 0) /* SWFLOW bit 3
+						  *
+						  * SWFLOW bits 3 & 2 table:
+						  * 00 -> no received flow
+						  *       control
+						  * 01 -> receiver compares
+						  *       XON2 and XOFF2
+						  * 10 -> receiver compares
+						  *       XON1 and XOFF1
+						  * 11 -> receiver compares
+						  *       XON1, XON2, XOFF1 and
+						  *       XOFF2
+						  */
+
+/* Misc definitions */
+#define SC16IS7XX_FIFO_SIZE		(64)
+#define SC16IS7XX_REG_SHIFT		2
+
+struct sc16is7xx_devtype {
+	char	name[10];
+	int	nr_gpio;
+	int	nr_uart;
+};
+
+struct sc16is7xx_one {
+	struct uart_port		port;
+	struct work_struct		tx_work;
+	struct work_struct		md_work;
+
+	struct serial_rs485		rs485;
+};
+
+struct sc16is7xx_port {
+	struct uart_driver		uart;
+	struct sc16is7xx_devtype	*devtype;
+	struct regmap			*regmap;
+	struct mutex			mutex;
+	struct clk			*clk;
+#ifdef CONFIG_GPIOLIB
+	struct gpio_chip		gpio;
+#endif
+	unsigned char			buf[SC16IS7XX_FIFO_SIZE];
+	struct sc16is7xx_one		p[0];
+};
+
+#define to_sc16is7xx_one(p,e)	((container_of((p), struct sc16is7xx_one, e)))
+
+static u8 sc16is7xx_port_read(struct uart_port *port, u8 reg)
+{
+	struct sc16is7xx_port *s = dev_get_drvdata(port->dev);
+	unsigned int val = 0;
+
+	regmap_read(s->regmap,
+		    (reg << SC16IS7XX_REG_SHIFT) | port->line, &val);
+
+	return val;
+}
+
+static void sc16is7xx_port_write(struct uart_port *port, u8 reg, u8 val)
+{
+	struct sc16is7xx_port *s = dev_get_drvdata(port->dev);
+
+	regmap_write(s->regmap,
+		     (reg << SC16IS7XX_REG_SHIFT) | port->line, val);
+}
+
+static void sc16is7xx_port_update(struct uart_port *port, u8 reg,
+				  u8 mask, u8 val)
+{
+	struct sc16is7xx_port *s = dev_get_drvdata(port->dev);
+
+	regmap_update_bits(s->regmap,
+			   (reg << SC16IS7XX_REG_SHIFT) | port->line,
+			   mask, val);
+}
+
+
+static void sc16is7xx_power(struct uart_port *port, int on)
+{
+	sc16is7xx_port_update(port, SC16IS7XX_IER_REG,
+			      SC16IS7XX_IER_SLEEP_BIT,
+			      on ? 0 : SC16IS7XX_IER_SLEEP_BIT);
+}
+
+static const struct sc16is7xx_devtype sc16is74x_devtype = {
+	.name		= "SC16IS74X",
+	.nr_gpio	= 0,
+	.nr_uart	= 1,
+};
+
+static const struct sc16is7xx_devtype sc16is750_devtype = {
+	.name		= "SC16IS750",
+	.nr_gpio	= 8,
+	.nr_uart	= 1,
+};
+
+static const struct sc16is7xx_devtype sc16is752_devtype = {
+	.name		= "SC16IS752",
+	.nr_gpio	= 8,
+	.nr_uart	= 2,
+};
+
+static const struct sc16is7xx_devtype sc16is760_devtype = {
+	.name		= "SC16IS760",
+	.nr_gpio	= 8,
+	.nr_uart	= 1,
+};
+
+static const struct sc16is7xx_devtype sc16is762_devtype = {
+	.name		= "SC16IS762",
+	.nr_gpio	= 8,
+	.nr_uart	= 2,
+};
+
+static bool sc16is7xx_regmap_volatile(struct device *dev, unsigned int reg)
+{
+	switch (reg >> SC16IS7XX_REG_SHIFT) {
+	case SC16IS7XX_RHR_REG:
+	case SC16IS7XX_IIR_REG:
+	case SC16IS7XX_LSR_REG:
+	case SC16IS7XX_MSR_REG:
+	case SC16IS7XX_TXLVL_REG:
+	case SC16IS7XX_RXLVL_REG:
+	case SC16IS7XX_IOSTATE_REG:
+		return true;
+	default:
+		break;
+	}
+
+	return false;
+}
+
+static bool sc16is7xx_regmap_precious(struct device *dev, unsigned int reg)
+{
+	switch (reg >> SC16IS7XX_REG_SHIFT) {
+	case SC16IS7XX_RHR_REG:
+		return true;
+	default:
+		break;
+	}
+
+	return false;
+}
+
+static int sc16is7xx_set_baud(struct uart_port *port, int baud)
+{
+	struct sc16is7xx_port *s = dev_get_drvdata(port->dev);
+	u8 lcr;
+	u8 prescaler = 0;
+	unsigned long clk = port->uartclk, div = clk / 16 / baud;
+
+	if (div > 0xffff) {
+		prescaler = SC16IS7XX_MCR_CLKSEL_BIT;
+		div /= 4;
+	}
+
+	lcr = sc16is7xx_port_read(port, SC16IS7XX_LCR_REG);
+
+	/* Open the LCR divisors for configuration */
+	sc16is7xx_port_write(port, SC16IS7XX_LCR_REG,
+			     SC16IS7XX_LCR_CONF_MODE_B);
+
+	/* Enable enhanced features */
+	regcache_cache_bypass(s->regmap, true);
+	sc16is7xx_port_write(port, SC16IS7XX_EFR_REG,
+			     SC16IS7XX_EFR_ENABLE_BIT);
+	regcache_cache_bypass(s->regmap, false);
+
+	/* Put LCR back to the normal mode */
+	sc16is7xx_port_write(port, SC16IS7XX_LCR_REG, lcr);
+
+	sc16is7xx_port_update(port, SC16IS7XX_MCR_REG,
+			      SC16IS7XX_MCR_CLKSEL_BIT,
+			      prescaler);
+
+	/* Open the LCR divisors for configuration */
+	sc16is7xx_port_write(port, SC16IS7XX_LCR_REG,
+			     SC16IS7XX_LCR_CONF_MODE_A);
+
+	/* Write the new divisor */
+	regcache_cache_bypass(s->regmap, true);
+	sc16is7xx_port_write(port, SC16IS7XX_DLH_REG, div / 256);
+	sc16is7xx_port_write(port, SC16IS7XX_DLL_REG, div % 256);
+	regcache_cache_bypass(s->regmap, false);
+
+	/* Put LCR back to the normal mode */
+	sc16is7xx_port_write(port, SC16IS7XX_LCR_REG, lcr);
+
+	return DIV_ROUND_CLOSEST(clk / 16, div);
+}
+
+static void sc16is7xx_handle_rx(struct uart_port *port, unsigned int rxlen,
+				unsigned int iir)
+{
+	struct sc16is7xx_port *s = dev_get_drvdata(port->dev);
+	unsigned int lsr = 0, ch, flag, bytes_read, i;
+	bool read_lsr = (iir == SC16IS7XX_IIR_RLSE_SRC) ? true : false;
+
+	if (unlikely(rxlen >= sizeof(s->buf))) {
+		dev_warn_ratelimited(port->dev,
+				     "Port %i: Possible RX FIFO overrun: %d\n",
+				     port->line, rxlen);
+		port->icount.buf_overrun++;
+		/* Ensure sanity of RX level */
+		rxlen = sizeof(s->buf);
+	}
+
+	while (rxlen) {
+		/* Only read lsr if there are possible errors in FIFO */
+		if (read_lsr) {
+			lsr = sc16is7xx_port_read(port, SC16IS7XX_LSR_REG);
+			if (!(lsr & SC16IS7XX_LSR_FIFOE_BIT))
+				read_lsr = false; /* No errors left in FIFO */
+		} else
+			lsr = 0;
+
+		if (read_lsr) {
+			s->buf[0] = sc16is7xx_port_read(port, SC16IS7XX_RHR_REG);
+			bytes_read = 1;
+		} else {
+			regcache_cache_bypass(s->regmap, true);
+			regmap_raw_read(s->regmap, SC16IS7XX_RHR_REG,
+					s->buf, rxlen);
+			regcache_cache_bypass(s->regmap, false);
+			bytes_read = rxlen;
+		}
+
+		lsr &= SC16IS7XX_LSR_BRK_ERROR_MASK;
+
+		port->icount.rx++;
+		flag = TTY_NORMAL;
+
+		if (unlikely(lsr)) {
+			if (lsr & SC16IS7XX_LSR_BI_BIT) {
+				port->icount.brk++;
+				if (uart_handle_break(port))
+					continue;
+			} else if (lsr & SC16IS7XX_LSR_PE_BIT)
+				port->icount.parity++;
+			else if (lsr & SC16IS7XX_LSR_FE_BIT)
+				port->icount.frame++;
+			else if (lsr & SC16IS7XX_LSR_OE_BIT)
+				port->icount.overrun++;
+
+			lsr &= port->read_status_mask;
+			if (lsr & SC16IS7XX_LSR_BI_BIT)
+				flag = TTY_BREAK;
+			else if (lsr & SC16IS7XX_LSR_PE_BIT)
+				flag = TTY_PARITY;
+			else if (lsr & SC16IS7XX_LSR_FE_BIT)
+				flag = TTY_FRAME;
+			else if (lsr & SC16IS7XX_LSR_OE_BIT)
+				flag = TTY_OVERRUN;
+		}
+
+		for (i = 0; i < bytes_read; ++i) {
+			ch = s->buf[i];
+			if (uart_handle_sysrq_char(port, ch))
+				continue;
+
+			if (lsr & port->ignore_status_mask)
+				continue;
+
+			uart_insert_char(port, lsr, SC16IS7XX_LSR_OE_BIT, ch,
+					 flag);
+		}
+		rxlen -= bytes_read;
+	}
+
+	tty_flip_buffer_push(&port->state->port);
+}
+
+static void sc16is7xx_handle_tx(struct uart_port *port)
+{
+	struct sc16is7xx_port *s = dev_get_drvdata(port->dev);
+	struct circ_buf *xmit = &port->state->xmit;
+	unsigned int txlen, to_send, i;
+
+	if (unlikely(port->x_char)) {
+		sc16is7xx_port_write(port, SC16IS7XX_THR_REG, port->x_char);
+		port->icount.tx++;
+		port->x_char = 0;
+		return;
+	}
+
+	if (uart_circ_empty(xmit) || uart_tx_stopped(port))
+		return;
+
+	/* Get length of data pending in circular buffer */
+	to_send = uart_circ_chars_pending(xmit);
+	if (likely(to_send)) {
+		/* Limit to size of TX FIFO */
+		txlen = sc16is7xx_port_read(port, SC16IS7XX_TXLVL_REG);
+		to_send = (to_send > txlen) ? txlen : to_send;
+
+		/* Add data to send */
+		port->icount.tx += to_send;
+
+		/* Convert to linear buffer */
+		for (i = 0; i < to_send; ++i) {
+			s->buf[i] = xmit->buf[xmit->tail];
+			xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
+		}
+		regcache_cache_bypass(s->regmap, true);
+		regmap_raw_write(s->regmap, SC16IS7XX_THR_REG, s->buf, to_send);
+		regcache_cache_bypass(s->regmap, false);
+	}
+
+	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+		uart_write_wakeup(port);
+}
+
+static void sc16is7xx_port_irq(struct sc16is7xx_port *s, int portno)
+{
+	struct uart_port *port = &s->p[portno].port;
+
+	do {
+		unsigned int iir, msr, rxlen;
+
+		iir = sc16is7xx_port_read(port, SC16IS7XX_IIR_REG);
+		if (iir & SC16IS7XX_IIR_NO_INT_BIT)
+			break;
+
+		iir &= SC16IS7XX_IIR_ID_MASK;
+
+		switch (iir) {
+		case SC16IS7XX_IIR_RDI_SRC:
+		case SC16IS7XX_IIR_RLSE_SRC:
+		case SC16IS7XX_IIR_RTOI_SRC:
+		case SC16IS7XX_IIR_XOFFI_SRC:
+			rxlen = sc16is7xx_port_read(port, SC16IS7XX_RXLVL_REG);
+			if (rxlen)
+				sc16is7xx_handle_rx(port, rxlen, iir);
+			break;
+
+		case SC16IS7XX_IIR_CTSRTS_SRC:
+			msr = sc16is7xx_port_read(port, SC16IS7XX_MSR_REG);
+			uart_handle_cts_change(port,
+					       !!(msr & SC16IS7XX_MSR_CTS_BIT));
+			break;
+		case SC16IS7XX_IIR_THRI_SRC:
+			mutex_lock(&s->mutex);
+			sc16is7xx_handle_tx(port);
+			mutex_unlock(&s->mutex);
+			break;
+		default:
+			dev_err_ratelimited(port->dev,
+					    "Port %i: Unexpected interrupt: %x",
+					    port->line, iir);
+			break;
+		}
+	} while (1);
+}
+
+static irqreturn_t sc16is7xx_ist(int irq, void *dev_id)
+{
+	struct sc16is7xx_port *s = (struct sc16is7xx_port *)dev_id;
+	int i;
+
+	for (i = 0; i < s->uart.nr; ++i)
+		sc16is7xx_port_irq(s, i);
+
+	return IRQ_HANDLED;
+}
+
+static void sc16is7xx_wq_proc(struct work_struct *ws)
+{
+	struct sc16is7xx_one *one = to_sc16is7xx_one(ws, tx_work);
+	struct sc16is7xx_port *s = dev_get_drvdata(one->port.dev);
+
+	mutex_lock(&s->mutex);
+	sc16is7xx_handle_tx(&one->port);
+	mutex_unlock(&s->mutex);
+}
+
+static void sc16is7xx_stop_tx(struct uart_port* port)
+{
+	struct sc16is7xx_one *one = to_sc16is7xx_one(port, port);
+	struct circ_buf *xmit = &one->port.state->xmit;
+
+	/* handle rs485 */
+	if (one->rs485.flags & SER_RS485_ENABLED) {
+		/* do nothing if current tx not yet completed */
+		int lsr = sc16is7xx_port_read(port, SC16IS7XX_LSR_REG);
+		if (!(lsr & SC16IS7XX_LSR_TEMT_BIT))
+			return;
+
+		if (uart_circ_empty(xmit) &&
+		    (one->rs485.delay_rts_after_send > 0))
+			mdelay(one->rs485.delay_rts_after_send);
+	}
+
+	sc16is7xx_port_update(port, SC16IS7XX_IER_REG,
+			      SC16IS7XX_IER_THRI_BIT,
+			      0);
+}
+
+static void sc16is7xx_stop_rx(struct uart_port* port)
+{
+	struct sc16is7xx_one *one = to_sc16is7xx_one(port, port);
+
+	one->port.read_status_mask &= ~SC16IS7XX_LSR_DR_BIT;
+	sc16is7xx_port_update(port, SC16IS7XX_IER_REG,
+			      SC16IS7XX_LSR_DR_BIT,
+			      0);
+}
+
+static void sc16is7xx_start_tx(struct uart_port *port)
+{
+	struct sc16is7xx_one *one = to_sc16is7xx_one(port, port);
+
+	/* handle rs485 */
+	if ((one->rs485.flags & SER_RS485_ENABLED) &&
+	    (one->rs485.delay_rts_before_send > 0)) {
+		mdelay(one->rs485.delay_rts_before_send);
+	}
+
+	if (!work_pending(&one->tx_work))
+		schedule_work(&one->tx_work);
+}
+
+static unsigned int sc16is7xx_tx_empty(struct uart_port *port)
+{
+	unsigned int lvl, lsr;
+
+	lvl = sc16is7xx_port_read(port, SC16IS7XX_TXLVL_REG);
+	lsr = sc16is7xx_port_read(port, SC16IS7XX_LSR_REG);
+
+	return ((lsr & SC16IS7XX_LSR_THRE_BIT) && !lvl) ? TIOCSER_TEMT : 0;
+}
+
+static unsigned int sc16is7xx_get_mctrl(struct uart_port *port)
+{
+	/* DCD and DSR are not wired and CTS/RTS is handled automatically
+	 * so just indicate DSR and CAR asserted
+	 */
+	return TIOCM_DSR | TIOCM_CAR;
+}
+
+static void sc16is7xx_md_proc(struct work_struct *ws)
+{
+	struct sc16is7xx_one *one = to_sc16is7xx_one(ws, md_work);
+
+	sc16is7xx_port_update(&one->port, SC16IS7XX_MCR_REG,
+			      SC16IS7XX_MCR_LOOP_BIT,
+			      (one->port.mctrl & TIOCM_LOOP) ?
+				      SC16IS7XX_MCR_LOOP_BIT : 0);
+}
+
+static void sc16is7xx_set_mctrl(struct uart_port *port, unsigned int mctrl)
+{
+	struct sc16is7xx_one *one = to_sc16is7xx_one(port, port);
+
+	schedule_work(&one->md_work);
+}
+
+static void sc16is7xx_break_ctl(struct uart_port *port, int break_state)
+{
+	sc16is7xx_port_update(port, SC16IS7XX_LCR_REG,
+			      SC16IS7XX_LCR_TXBREAK_BIT,
+			      break_state ? SC16IS7XX_LCR_TXBREAK_BIT : 0);
+}
+
+static void sc16is7xx_set_termios(struct uart_port *port,
+				  struct ktermios *termios,
+				  struct ktermios *old)
+{
+	struct sc16is7xx_port *s = dev_get_drvdata(port->dev);
+	unsigned int lcr, flow = 0;
+	int baud;
+
+	/* Mask termios capabilities we don't support */
+	termios->c_cflag &= ~CMSPAR;
+
+	/* Word size */
+	switch (termios->c_cflag & CSIZE) {
+	case CS5:
+		lcr = SC16IS7XX_LCR_WORD_LEN_5;
+		break;
+	case CS6:
+		lcr = SC16IS7XX_LCR_WORD_LEN_6;
+		break;
+	case CS7:
+		lcr = SC16IS7XX_LCR_WORD_LEN_7;
+		break;
+	case CS8:
+		lcr = SC16IS7XX_LCR_WORD_LEN_8;
+		break;
+	default:
+		lcr = SC16IS7XX_LCR_WORD_LEN_8;
+		termios->c_cflag &= ~CSIZE;
+		termios->c_cflag |= CS8;
+		break;
+	}
+
+	/* Parity */
+	if (termios->c_cflag & PARENB) {
+		lcr |= SC16IS7XX_LCR_PARITY_BIT;
+		if (!(termios->c_cflag & PARODD))
+			lcr |= SC16IS7XX_LCR_EVENPARITY_BIT;
+	}
+
+	/* Stop bits */
+	if (termios->c_cflag & CSTOPB)
+		lcr |= SC16IS7XX_LCR_STOPLEN_BIT; /* 2 stops */
+
+	/* Set read status mask */
+	port->read_status_mask = SC16IS7XX_LSR_OE_BIT;
+	if (termios->c_iflag & INPCK)
+		port->read_status_mask |= SC16IS7XX_LSR_PE_BIT |
+					  SC16IS7XX_LSR_FE_BIT;
+	if (termios->c_iflag & (BRKINT | PARMRK))
+		port->read_status_mask |= SC16IS7XX_LSR_BI_BIT;
+
+	/* Set status ignore mask */
+	port->ignore_status_mask = 0;
+	if (termios->c_iflag & IGNBRK)
+		port->ignore_status_mask |= SC16IS7XX_LSR_BI_BIT;
+	if (!(termios->c_cflag & CREAD))
+		port->ignore_status_mask |= SC16IS7XX_LSR_BRK_ERROR_MASK;
+
+	sc16is7xx_port_write(port, SC16IS7XX_LCR_REG,
+			     SC16IS7XX_LCR_CONF_MODE_B);
+
+	/* Configure flow control */
+	regcache_cache_bypass(s->regmap, true);
+	sc16is7xx_port_write(port, SC16IS7XX_XON1_REG, termios->c_cc[VSTART]);
+	sc16is7xx_port_write(port, SC16IS7XX_XOFF1_REG, termios->c_cc[VSTOP]);
+	if (termios->c_cflag & CRTSCTS)
+		flow |= SC16IS7XX_EFR_AUTOCTS_BIT |
+			SC16IS7XX_EFR_AUTORTS_BIT;
+	if (termios->c_iflag & IXON)
+		flow |= SC16IS7XX_EFR_SWFLOW3_BIT;
+	if (termios->c_iflag & IXOFF)
+		flow |= SC16IS7XX_EFR_SWFLOW1_BIT;
+
+	sc16is7xx_port_write(port, SC16IS7XX_EFR_REG, flow);
+	regcache_cache_bypass(s->regmap, false);
+
+	/* Update LCR register */
+	sc16is7xx_port_write(port, SC16IS7XX_LCR_REG, lcr);
+
+	/* Get baud rate generator configuration */
+	baud = uart_get_baud_rate(port, termios, old,
+				  port->uartclk / 16 / 4 / 0xffff,
+				  port->uartclk / 16);
+
+	/* Setup baudrate generator */
+	baud = sc16is7xx_set_baud(port, baud);
+
+	/* Update timeout according to new baud rate */
+	uart_update_timeout(port, termios->c_cflag, baud);
+}
+
+#if defined(TIOCSRS485) && defined(TIOCGRS485)
+static void sc16is7xx_config_rs485(struct uart_port *port,
+				   struct serial_rs485 *rs485)
+{
+	struct sc16is7xx_one *one = to_sc16is7xx_one(port, port);
+
+	one->rs485 = *rs485;
+
+	if (one->rs485.flags & SER_RS485_ENABLED) {
+		sc16is7xx_port_update(port, SC16IS7XX_EFCR_REG,
+				      SC16IS7XX_EFCR_AUTO_RS485_BIT,
+				      SC16IS7XX_EFCR_AUTO_RS485_BIT);
+	} else {
+		sc16is7xx_port_update(port, SC16IS7XX_EFCR_REG,
+				      SC16IS7XX_EFCR_AUTO_RS485_BIT,
+				      0);
+	}
+}
+#endif
+
+static int sc16is7xx_ioctl(struct uart_port *port, unsigned int cmd,
+			   unsigned long arg)
+{
+#if defined(TIOCSRS485) && defined(TIOCGRS485)
+	struct serial_rs485 rs485;
+
+	switch (cmd) {
+	case TIOCSRS485:
+		if (copy_from_user(&rs485, (void __user *)arg, sizeof(rs485)))
+			return -EFAULT;
+
+		sc16is7xx_config_rs485(port, &rs485);
+		return 0;
+	case TIOCGRS485:
+		if (copy_to_user((void __user *)arg,
+				 &(to_sc16is7xx_one(port, port)->rs485),
+				 sizeof(rs485)))
+			return -EFAULT;
+		return 0;
+	default:
+		break;
+	}
+#endif
+
+	return -ENOIOCTLCMD;
+}
+
+static int sc16is7xx_startup(struct uart_port *port)
+{
+	struct sc16is7xx_port *s = dev_get_drvdata(port->dev);
+	unsigned int val;
+
+	sc16is7xx_power(port, 1);
+
+	/* Reset FIFOs*/
+	val = SC16IS7XX_FCR_RXRESET_BIT | SC16IS7XX_FCR_TXRESET_BIT;
+	sc16is7xx_port_write(port, SC16IS7XX_FCR_REG, val);
+	udelay(5);
+	sc16is7xx_port_write(port, SC16IS7XX_FCR_REG,
+			     SC16IS7XX_FCR_FIFO_BIT);
+
+	/* Enable EFR */
+	sc16is7xx_port_write(port, SC16IS7XX_LCR_REG,
+			     SC16IS7XX_LCR_CONF_MODE_B);
+
+	regcache_cache_bypass(s->regmap, true);
+
+	/* Enable write access to enhanced features and internal clock div */
+	sc16is7xx_port_write(port, SC16IS7XX_EFR_REG,
+			     SC16IS7XX_EFR_ENABLE_BIT);
+
+	/* Enable TCR/TLR */
+	sc16is7xx_port_update(port, SC16IS7XX_MCR_REG,
+			      SC16IS7XX_MCR_TCRTLR_BIT,
+			      SC16IS7XX_MCR_TCRTLR_BIT);
+
+	/* Configure flow control levels */
+	/* Flow control halt level 48, resume level 24 */
+	sc16is7xx_port_write(port, SC16IS7XX_TCR_REG,
+			     SC16IS7XX_TCR_RX_RESUME(24) |
+			     SC16IS7XX_TCR_RX_HALT(48));
+
+	regcache_cache_bypass(s->regmap, false);
+
+	/* Now, initialize the UART */
+	sc16is7xx_port_write(port, SC16IS7XX_LCR_REG, SC16IS7XX_LCR_WORD_LEN_8);
+
+	/* Enable the Rx and Tx FIFO */
+	sc16is7xx_port_update(port, SC16IS7XX_EFCR_REG,
+			      SC16IS7XX_EFCR_RXDISABLE_BIT |
+			      SC16IS7XX_EFCR_TXDISABLE_BIT,
+			      0);
+
+	/* Enable RX, TX, CTS change interrupts */
+	val = SC16IS7XX_IER_RDI_BIT | SC16IS7XX_IER_THRI_BIT |
+	      SC16IS7XX_IER_CTSI_BIT;
+	sc16is7xx_port_write(port, SC16IS7XX_IER_REG, val);
+
+	return 0;
+}
+
+static void sc16is7xx_shutdown(struct uart_port *port)
+{
+	/* Disable all interrupts */
+	sc16is7xx_port_write(port, SC16IS7XX_IER_REG, 0);
+	/* Disable TX/RX */
+	sc16is7xx_port_write(port, SC16IS7XX_EFCR_REG,
+			     SC16IS7XX_EFCR_RXDISABLE_BIT |
+			     SC16IS7XX_EFCR_TXDISABLE_BIT);
+
+	sc16is7xx_power(port, 0);
+}
+
+static const char *sc16is7xx_type(struct uart_port *port)
+{
+	struct sc16is7xx_port *s = dev_get_drvdata(port->dev);
+
+	return (port->type == PORT_SC16IS7XX) ? s->devtype->name : NULL;
+}
+
+static int sc16is7xx_request_port(struct uart_port *port)
+{
+	/* Do nothing */
+	return 0;
+}
+
+static void sc16is7xx_config_port(struct uart_port *port, int flags)
+{
+	if (flags & UART_CONFIG_TYPE)
+		port->type = PORT_SC16IS7XX;
+}
+
+static int sc16is7xx_verify_port(struct uart_port *port,
+				 struct serial_struct *s)
+{
+	if ((s->type != PORT_UNKNOWN) && (s->type != PORT_SC16IS7XX))
+		return -EINVAL;
+	if (s->irq != port->irq)
+		return -EINVAL;
+
+	return 0;
+}
+
+static void sc16is7xx_pm(struct uart_port *port, unsigned int state,
+			 unsigned int oldstate)
+{
+	sc16is7xx_power(port, (state == UART_PM_STATE_ON) ? 1 : 0);
+}
+
+static void sc16is7xx_null_void(struct uart_port *port)
+{
+	/* Do nothing */
+}
+
+static const struct uart_ops sc16is7xx_ops = {
+	.tx_empty	= sc16is7xx_tx_empty,
+	.set_mctrl	= sc16is7xx_set_mctrl,
+	.get_mctrl	= sc16is7xx_get_mctrl,
+	.stop_tx	= sc16is7xx_stop_tx,
+	.start_tx	= sc16is7xx_start_tx,
+	.stop_rx	= sc16is7xx_stop_rx,
+	.enable_ms	= sc16is7xx_null_void,
+	.break_ctl	= sc16is7xx_break_ctl,
+	.startup	= sc16is7xx_startup,
+	.shutdown	= sc16is7xx_shutdown,
+	.set_termios	= sc16is7xx_set_termios,
+	.type		= sc16is7xx_type,
+	.request_port	= sc16is7xx_request_port,
+	.release_port	= sc16is7xx_null_void,
+	.config_port	= sc16is7xx_config_port,
+	.verify_port	= sc16is7xx_verify_port,
+	.ioctl		= sc16is7xx_ioctl,
+	.pm		= sc16is7xx_pm,
+};
+
+#ifdef CONFIG_GPIOLIB
+static int sc16is7xx_gpio_get(struct gpio_chip *chip, unsigned offset)
+{
+	unsigned int val;
+	struct sc16is7xx_port *s = container_of(chip, struct sc16is7xx_port,
+						gpio);
+	struct uart_port *port = &s->p[0].port;
+
+	val = sc16is7xx_port_read(port, SC16IS7XX_IOSTATE_REG);
+
+	return !!(val & BIT(offset));
+}
+
+static void sc16is7xx_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
+{
+	struct sc16is7xx_port *s = container_of(chip, struct sc16is7xx_port,
+						gpio);
+	struct uart_port *port = &s->p[0].port;
+
+	sc16is7xx_port_update(port, SC16IS7XX_IOSTATE_REG, BIT(offset),
+			      val ? BIT(offset) : 0);
+}
+
+static int sc16is7xx_gpio_direction_input(struct gpio_chip *chip,
+					  unsigned offset)
+{
+	struct sc16is7xx_port *s = container_of(chip, struct sc16is7xx_port,
+						gpio);
+	struct uart_port *port = &s->p[0].port;
+
+	sc16is7xx_port_update(port, SC16IS7XX_IODIR_REG, BIT(offset), 0);
+
+	return 0;
+}
+
+static int sc16is7xx_gpio_direction_output(struct gpio_chip *chip,
+					   unsigned offset, int val)
+{
+	struct sc16is7xx_port *s = container_of(chip, struct sc16is7xx_port,
+						gpio);
+	struct uart_port *port = &s->p[0].port;
+
+	sc16is7xx_port_update(port, SC16IS7XX_IOSTATE_REG, BIT(offset),
+			      val ? BIT(offset) : 0);
+	sc16is7xx_port_update(port, SC16IS7XX_IODIR_REG, BIT(offset),
+			      BIT(offset));
+
+	return 0;
+}
+#endif
+
+static int sc16is7xx_probe(struct device *dev,
+			   struct sc16is7xx_devtype *devtype,
+			   struct regmap *regmap, int irq, unsigned long flags)
+{
+	unsigned long freq, *pfreq = dev_get_platdata(dev);
+	struct clk *clk;
+	int i, ret;
+	struct sc16is7xx_port *s;
+
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	/* Alloc port structure */
+	s = devm_kzalloc(dev, sizeof(*s) +
+			 sizeof(struct sc16is7xx_one) * devtype->nr_uart,
+			 GFP_KERNEL);
+	if (!s) {
+		dev_err(dev, "Error allocating port structure\n");
+		return -ENOMEM;
+	}
+
+	clk = devm_clk_get(dev, NULL);
+	if (IS_ERR(clk)) {
+		if (pfreq)
+			freq = *pfreq;
+		else
+			return PTR_ERR(clk);
+	} else {
+		freq = clk_get_rate(clk);
+	}
+
+	s->regmap = regmap;
+	s->devtype = devtype;
+	dev_set_drvdata(dev, s);
+
+	/* Register UART driver */
+	s->uart.owner		= THIS_MODULE;
+	s->uart.dev_name	= "ttySC";
+	s->uart.nr		= devtype->nr_uart;
+	ret = uart_register_driver(&s->uart);
+	if (ret) {
+		dev_err(dev, "Registering UART driver failed\n");
+		goto out_clk;
+	}
+
+#ifdef CONFIG_GPIOLIB
+	if (devtype->nr_gpio) {
+		/* Setup GPIO cotroller */
+		s->gpio.owner		 = THIS_MODULE;
+		s->gpio.dev		 = dev;
+		s->gpio.label		 = dev_name(dev);
+		s->gpio.direction_input	 = sc16is7xx_gpio_direction_input;
+		s->gpio.get		 = sc16is7xx_gpio_get;
+		s->gpio.direction_output = sc16is7xx_gpio_direction_output;
+		s->gpio.set		 = sc16is7xx_gpio_set;
+		s->gpio.base		 = -1;
+		s->gpio.ngpio		 = devtype->nr_gpio;
+		s->gpio.can_sleep	 = 1;
+		ret = gpiochip_add(&s->gpio);
+		if (ret)
+			goto out_uart;
+	}
+#endif
+
+	mutex_init(&s->mutex);
+
+	for (i = 0; i < devtype->nr_uart; ++i) {
+		/* Initialize port data */
+		s->p[i].port.line	= i;
+		s->p[i].port.dev	= dev;
+		s->p[i].port.irq	= irq;
+		s->p[i].port.type	= PORT_SC16IS7XX;
+		s->p[i].port.fifosize	= SC16IS7XX_FIFO_SIZE;
+		s->p[i].port.flags	= UPF_FIXED_TYPE | UPF_LOW_LATENCY;
+		s->p[i].port.iotype	= UPIO_PORT;
+		s->p[i].port.uartclk	= freq;
+		s->p[i].port.ops	= &sc16is7xx_ops;
+		/* Disable all interrupts */
+		sc16is7xx_port_write(&s->p[i].port, SC16IS7XX_IER_REG, 0);
+		/* Disable TX/RX */
+		sc16is7xx_port_write(&s->p[i].port, SC16IS7XX_EFCR_REG,
+				     SC16IS7XX_EFCR_RXDISABLE_BIT |
+				     SC16IS7XX_EFCR_TXDISABLE_BIT);
+		/* Initialize queue for start TX */
+		INIT_WORK(&s->p[i].tx_work, sc16is7xx_wq_proc);
+		/* Initialize queue for changing mode */
+		INIT_WORK(&s->p[i].md_work, sc16is7xx_md_proc);
+		/* Register port */
+		uart_add_one_port(&s->uart, &s->p[i].port);
+		/* Go to suspend mode */
+		sc16is7xx_power(&s->p[i].port, 0);
+	}
+
+	/* Setup interrupt */
+	ret = devm_request_threaded_irq(dev, irq, NULL, sc16is7xx_ist,
+					IRQF_ONESHOT | flags, dev_name(dev), s);
+	if (!ret)
+		return 0;
+
+	mutex_destroy(&s->mutex);
+
+#ifdef CONFIG_GPIOLIB
+	if (devtype->nr_gpio)
+		WARN_ON(gpiochip_remove(&s->gpio));
+
+out_uart:
+#endif
+	uart_unregister_driver(&s->uart);
+
+out_clk:
+	if (!IS_ERR(s->clk))
+		clk_disable_unprepare(s->clk);
+
+	return ret;
+}
+
+static int sc16is7xx_remove(struct device *dev)
+{
+	struct sc16is7xx_port *s = dev_get_drvdata(dev);
+	int i, ret = 0;
+
+#ifdef CONFIG_GPIOLIB
+	if (s->devtype->nr_gpio) {
+		ret = gpiochip_remove(&s->gpio);
+		if (ret)
+			return ret;
+	}
+#endif
+
+	for (i = 0; i < s->uart.nr; i++) {
+		cancel_work_sync(&s->p[i].tx_work);
+		cancel_work_sync(&s->p[i].md_work);
+		uart_remove_one_port(&s->uart, &s->p[i].port);
+		sc16is7xx_power(&s->p[i].port, 0);
+	}
+
+	mutex_destroy(&s->mutex);
+	uart_unregister_driver(&s->uart);
+	if (!IS_ERR(s->clk))
+		clk_disable_unprepare(s->clk);
+
+	return ret;
+}
+
+static const struct of_device_id __maybe_unused sc16is7xx_dt_ids[] = {
+	{ .compatible = "nxp,sc16is740",	.data = &sc16is74x_devtype, },
+	{ .compatible = "nxp,sc16is741",	.data = &sc16is74x_devtype, },
+	{ .compatible = "nxp,sc16is750",	.data = &sc16is750_devtype, },
+	{ .compatible = "nxp,sc16is752",	.data = &sc16is752_devtype, },
+	{ .compatible = "nxp,sc16is760",	.data = &sc16is760_devtype, },
+	{ .compatible = "nxp,sc16is762",	.data = &sc16is762_devtype, },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, sc16is7xx_dt_ids);
+
+static struct regmap_config regcfg = {
+	.reg_bits = 7,
+	.pad_bits = 1,
+	.val_bits = 8,
+	.cache_type = REGCACHE_RBTREE,
+	.volatile_reg = sc16is7xx_regmap_volatile,
+	.precious_reg = sc16is7xx_regmap_precious,
+};
+
+static int sc16is7xx_i2c_probe(struct i2c_client *i2c,
+			       const struct i2c_device_id *id)
+{
+	struct sc16is7xx_devtype *devtype;
+	unsigned long flags = 0;
+	struct regmap *regmap;
+
+	if (i2c->dev.of_node) {
+		const struct of_device_id *of_id =
+				of_match_device(sc16is7xx_dt_ids, &i2c->dev);
+
+		devtype = (struct sc16is7xx_devtype *)of_id->data;
+	} else {
+		devtype = (struct sc16is7xx_devtype *)id->driver_data;
+		flags = IRQF_TRIGGER_FALLING;
+	}
+
+	regcfg.max_register = (0xf << SC16IS7XX_REG_SHIFT) |
+			      (devtype->nr_uart - 1);
+	regmap = devm_regmap_init_i2c(i2c, &regcfg);
+
+	return sc16is7xx_probe(&i2c->dev, devtype, regmap, i2c->irq, flags);
+}
+
+static int sc16is7xx_i2c_remove(struct i2c_client *client)
+{
+	return sc16is7xx_remove(&client->dev);
+}
+
+static const struct i2c_device_id sc16is7xx_i2c_id_table[] = {
+	{ "sc16is74x",	(kernel_ulong_t)&sc16is74x_devtype, },
+	{ "sc16is750",	(kernel_ulong_t)&sc16is750_devtype, },
+	{ "sc16is752",	(kernel_ulong_t)&sc16is752_devtype, },
+	{ "sc16is760",	(kernel_ulong_t)&sc16is760_devtype, },
+	{ "sc16is762",	(kernel_ulong_t)&sc16is762_devtype, },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, sc16is7xx_i2c_id_table);
+
+static struct i2c_driver sc16is7xx_i2c_uart_driver = {
+	.driver = {
+		.name		= SC16IS7XX_NAME,
+		.owner		= THIS_MODULE,
+		.of_match_table	= of_match_ptr(sc16is7xx_dt_ids),
+	},
+	.probe		= sc16is7xx_i2c_probe,
+	.remove		= sc16is7xx_i2c_remove,
+	.id_table	= sc16is7xx_i2c_id_table,
+};
+module_i2c_driver(sc16is7xx_i2c_uart_driver);
+MODULE_ALIAS("i2c:sc16is7xx");
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Jon Ringle <jringle@gridpoint.com>");
+MODULE_DESCRIPTION("SC16IS7XX serial driver");
diff --git a/drivers/tty/serial/sccnxp.c b/drivers/tty/serial/sccnxp.c
index a447f71538e..5443b46345e 100644
--- a/drivers/tty/serial/sccnxp.c
+++ b/drivers/tty/serial/sccnxp.c
@@ -474,9 +474,7 @@ static void sccnxp_timer(unsigned long data)
 	sccnxp_handle_events(s);
 	spin_unlock_irqrestore(&s->lock, flags);
 
-	if (!timer_pending(&s->timer))
-		mod_timer(&s->timer, jiffies +
-			  usecs_to_jiffies(s->pdata.poll_time_us));
+	mod_timer(&s->timer, jiffies + usecs_to_jiffies(s->pdata.poll_time_us));
 }
 
 static irqreturn_t sccnxp_ist(int irq, void *dev_id)
@@ -674,6 +672,8 @@ static void sccnxp_set_termios(struct uart_port *port,
 	port->ignore_status_mask = 0;
 	if (termios->c_iflag & IGNBRK)
 		port->ignore_status_mask |= SR_BRK;
+	if (termios->c_iflag & IGNPAR)
+		port->ignore_status_mask |= SR_PE;
 	if (!(termios->c_cflag & CREAD))
 		port->ignore_status_mask |= SR_PE | SR_OVR | SR_FE | SR_BRK;
 
diff --git a/drivers/tty/serial/serial_core.c b/drivers/tty/serial/serial_core.c
index b68550d95a4..fbf6c5ad222 100644
--- a/drivers/tty/serial/serial_core.c
+++ b/drivers/tty/serial/serial_core.c
@@ -174,8 +174,12 @@ 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 (tty_port_cts_enabled(port)) {
+		/*
+		 * 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;
@@ -2239,6 +2243,9 @@ static void uart_poll_put_char(struct tty_driver *driver, int line, char ch)
 		return;
 
 	port = state->uart_port;
+
+	if (ch == '\n')
+		port->ops->poll_put_char(port, '\r');
 	port->ops->poll_put_char(port, ch);
 }
 #endif
@@ -2772,7 +2779,9 @@ void uart_handle_cts_change(struct uart_port *uport, unsigned int status)
 
 	uport->icount.cts++;
 
-	if (tty_port_cts_enabled(port)) {
+	/* 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 (status) {
 				tty->hw_stopped = 0;
diff --git a/drivers/tty/serial/serial_mctrl_gpio.c b/drivers/tty/serial/serial_mctrl_gpio.c
new file mode 100644
index 00000000000..bf9560ffe3f
--- /dev/null
+++ b/drivers/tty/serial/serial_mctrl_gpio.c
@@ -0,0 +1,143 @@
+/*
+ * Helpers for controlling modem lines via GPIO
+ *
+ * Copyright (C) 2014 Paratronic S.A.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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/device.h>
+#include <linux/gpio/consumer.h>
+#include <uapi/asm-generic/termios.h>
+
+#include "serial_mctrl_gpio.h"
+
+struct mctrl_gpios {
+	struct gpio_desc *gpio[UART_GPIO_MAX];
+};
+
+static const struct {
+	const char *name;
+	unsigned int mctrl;
+	bool dir_out;
+} mctrl_gpios_desc[UART_GPIO_MAX] = {
+	{ "cts", TIOCM_CTS, false, },
+	{ "dsr", TIOCM_DSR, false, },
+	{ "dcd", TIOCM_CD, false, },
+	{ "rng", TIOCM_RNG, false, },
+	{ "rts", TIOCM_RTS, true, },
+	{ "dtr", TIOCM_DTR, true, },
+	{ "out1", TIOCM_OUT1, true, },
+	{ "out2", TIOCM_OUT2, true, },
+};
+
+void mctrl_gpio_set(struct mctrl_gpios *gpios, unsigned int mctrl)
+{
+	enum mctrl_gpio_idx i;
+
+	if (IS_ERR_OR_NULL(gpios))
+		return;
+
+	for (i = 0; i < UART_GPIO_MAX; i++)
+		if (!IS_ERR_OR_NULL(gpios->gpio[i]) &&
+		    mctrl_gpios_desc[i].dir_out)
+			gpiod_set_value(gpios->gpio[i],
+					!!(mctrl & mctrl_gpios_desc[i].mctrl));
+}
+EXPORT_SYMBOL_GPL(mctrl_gpio_set);
+
+struct gpio_desc *mctrl_gpio_to_gpiod(struct mctrl_gpios *gpios,
+				      enum mctrl_gpio_idx gidx)
+{
+	if (!IS_ERR_OR_NULL(gpios) && !IS_ERR_OR_NULL(gpios->gpio[gidx]))
+		return gpios->gpio[gidx];
+	else
+		return NULL;
+}
+EXPORT_SYMBOL_GPL(mctrl_gpio_to_gpiod);
+
+unsigned int mctrl_gpio_get(struct mctrl_gpios *gpios, unsigned int *mctrl)
+{
+	enum mctrl_gpio_idx i;
+
+	/*
+	 * return it unchanged if the structure is not allocated
+	 */
+	if (IS_ERR_OR_NULL(gpios))
+		return *mctrl;
+
+	for (i = 0; i < UART_GPIO_MAX; i++) {
+		if (!IS_ERR_OR_NULL(gpios->gpio[i]) &&
+		    !mctrl_gpios_desc[i].dir_out) {
+			if (gpiod_get_value(gpios->gpio[i]))
+				*mctrl |= mctrl_gpios_desc[i].mctrl;
+			else
+				*mctrl &= ~mctrl_gpios_desc[i].mctrl;
+		}
+	}
+
+	return *mctrl;
+}
+EXPORT_SYMBOL_GPL(mctrl_gpio_get);
+
+struct mctrl_gpios *mctrl_gpio_init(struct device *dev, unsigned int idx)
+{
+	struct mctrl_gpios *gpios;
+	enum mctrl_gpio_idx i;
+	int err;
+
+	gpios = devm_kzalloc(dev, sizeof(*gpios), GFP_KERNEL);
+	if (!gpios)
+		return ERR_PTR(-ENOMEM);
+
+	for (i = 0; i < UART_GPIO_MAX; i++) {
+		gpios->gpio[i] = devm_gpiod_get_index(dev,
+						      mctrl_gpios_desc[i].name,
+						      idx);
+
+		/*
+		 * The GPIOs are maybe not all filled,
+		 * this is not an error.
+		 */
+		if (IS_ERR_OR_NULL(gpios->gpio[i]))
+			continue;
+
+		if (mctrl_gpios_desc[i].dir_out)
+			err = gpiod_direction_output(gpios->gpio[i], 0);
+		else
+			err = gpiod_direction_input(gpios->gpio[i]);
+		if (err) {
+			dev_dbg(dev, "Unable to set direction for %s GPIO",
+				mctrl_gpios_desc[i].name);
+			devm_gpiod_put(dev, gpios->gpio[i]);
+			gpios->gpio[i] = NULL;
+		}
+	}
+
+	return gpios;
+}
+EXPORT_SYMBOL_GPL(mctrl_gpio_init);
+
+void mctrl_gpio_free(struct device *dev, struct mctrl_gpios *gpios)
+{
+	enum mctrl_gpio_idx i;
+
+	if (IS_ERR_OR_NULL(gpios))
+		return;
+
+	for (i = 0; i < UART_GPIO_MAX; i++)
+		if (!IS_ERR_OR_NULL(gpios->gpio[i]))
+			devm_gpiod_put(dev, gpios->gpio[i]);
+	devm_kfree(dev, gpios);
+}
+EXPORT_SYMBOL_GPL(mctrl_gpio_free);
diff --git a/drivers/tty/serial/serial_mctrl_gpio.h b/drivers/tty/serial/serial_mctrl_gpio.h
new file mode 100644
index 00000000000..400ba0494a1
--- /dev/null
+++ b/drivers/tty/serial/serial_mctrl_gpio.h
@@ -0,0 +1,110 @@
+/*
+ * Helpers for controlling modem lines via GPIO
+ *
+ * Copyright (C) 2014 Paratronic S.A.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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 __SERIAL_MCTRL_GPIO__
+#define __SERIAL_MCTRL_GPIO__
+
+#include <linux/err.h>
+#include <linux/device.h>
+#include <linux/gpio/consumer.h>
+
+enum mctrl_gpio_idx {
+	UART_GPIO_CTS,
+	UART_GPIO_DSR,
+	UART_GPIO_DCD,
+	UART_GPIO_RNG,
+	UART_GPIO_RI = UART_GPIO_RNG,
+	UART_GPIO_RTS,
+	UART_GPIO_DTR,
+	UART_GPIO_OUT1,
+	UART_GPIO_OUT2,
+	UART_GPIO_MAX,
+};
+
+/*
+ * Opaque descriptor for modem lines controlled by GPIOs
+ */
+struct mctrl_gpios;
+
+#ifdef CONFIG_GPIOLIB
+
+/*
+ * Set state of the modem control output lines via GPIOs.
+ */
+void mctrl_gpio_set(struct mctrl_gpios *gpios, unsigned int mctrl);
+
+/*
+ * Get state of the modem control output lines from GPIOs.
+ * The mctrl flags are updated and returned.
+ */
+unsigned int mctrl_gpio_get(struct mctrl_gpios *gpios, unsigned int *mctrl);
+
+/*
+ * Returns the associated struct gpio_desc to the modem line gidx
+ */
+struct gpio_desc *mctrl_gpio_to_gpiod(struct mctrl_gpios *gpios,
+				      enum mctrl_gpio_idx gidx);
+
+/*
+ * Request and set direction of modem control lines GPIOs.
+ * devm_* functions are used, so there's no need to call mctrl_gpio_free().
+ * Returns a pointer to the allocated mctrl structure if ok, -ENOMEM on
+ * allocation error.
+ */
+struct mctrl_gpios *mctrl_gpio_init(struct device *dev, unsigned int idx);
+
+/*
+ * Free the mctrl_gpios structure.
+ * Normally, this function will not be called, as the GPIOs will
+ * be disposed of by the resource management code.
+ */
+void mctrl_gpio_free(struct device *dev, struct mctrl_gpios *gpios);
+
+#else /* GPIOLIB */
+
+static inline
+void mctrl_gpio_set(struct mctrl_gpios *gpios, unsigned int mctrl)
+{
+}
+
+static inline
+unsigned int mctrl_gpio_get(struct mctrl_gpios *gpios, unsigned int *mctrl)
+{
+	return *mctrl;
+}
+
+static inline
+struct gpio_desc *mctrl_gpio_to_gpiod(struct mctrl_gpios *gpios,
+				      enum mctrl_gpio_idx gidx)
+{
+	return ERR_PTR(-ENOSYS);
+}
+
+static inline
+struct mctrl_gpios *mctrl_gpio_init(struct device *dev, unsigned int idx)
+{
+	return ERR_PTR(-ENOSYS);
+}
+
+static inline
+void mctrl_gpio_free(struct device *dev, struct mctrl_gpios *gpios)
+{
+}
+
+#endif /* GPIOLIB */
+
+#endif
diff --git a/drivers/tty/serial/serial_txx9.c b/drivers/tty/serial/serial_txx9.c
index 90a080b1f9e..60f49b9d7e3 100644
--- a/drivers/tty/serial/serial_txx9.c
+++ b/drivers/tty/serial/serial_txx9.c
@@ -535,13 +535,8 @@ static void serial_txx9_put_poll_char(struct uart_port *port, unsigned char c)
 	wait_for_xmitr(up);
 	/*
 	 *	Send the character out.
-	 *	If a LF, also do CR...
 	 */
 	sio_out(up, TXX9_SITFIFO, c);
-	if (c == 10) {
-		wait_for_xmitr(up);
-		sio_out(up, TXX9_SITFIFO, 13);
-	}
 
 	/*
 	 *	Finally, wait for transmitter to become empty
diff --git a/drivers/tty/serial/sirfsoc_uart.c b/drivers/tty/serial/sirfsoc_uart.c
index 68b0fd4b9a6..1f2be48c92c 100644
--- a/drivers/tty/serial/sirfsoc_uart.c
+++ b/drivers/tty/serial/sirfsoc_uart.c
@@ -358,9 +358,11 @@ static irqreturn_t sirfsoc_uart_usp_cts_handler(int irq, void *dev_id)
 {
 	struct sirfsoc_uart_port *sirfport = (struct sirfsoc_uart_port *)dev_id;
 	struct uart_port *port = &sirfport->port;
+	spin_lock(&port->lock);
 	if (gpio_is_valid(sirfport->cts_gpio) && sirfport->ms_enabled)
 		uart_handle_cts_change(port,
 				!gpio_get_value(sirfport->cts_gpio));
+	spin_unlock(&port->lock);
 	return IRQ_HANDLED;
 }
 
@@ -428,10 +430,6 @@ sirfsoc_uart_pio_rx_chars(struct uart_port *port, unsigned int max_rx_count)
 	sirfport->rx_io_count += rx_count;
 	port->icount.rx += rx_count;
 
-	spin_unlock(&port->lock);
-	tty_flip_buffer_push(&port->state->port);
-	spin_lock(&port->lock);
-
 	return rx_count;
 }
 
@@ -465,6 +463,7 @@ static void sirfsoc_uart_tx_dma_complete_callback(void *param)
 	struct circ_buf *xmit = &port->state->xmit;
 	unsigned long flags;
 
+	spin_lock_irqsave(&port->lock, flags);
 	xmit->tail = (xmit->tail + sirfport->transfer_size) &
 				(UART_XMIT_SIZE - 1);
 	port->icount.tx += sirfport->transfer_size;
@@ -473,10 +472,9 @@ static void sirfsoc_uart_tx_dma_complete_callback(void *param)
 	if (sirfport->tx_dma_addr)
 		dma_unmap_single(port->dev, sirfport->tx_dma_addr,
 				sirfport->transfer_size, DMA_TO_DEVICE);
-	spin_lock_irqsave(&sirfport->tx_lock, flags);
 	sirfport->tx_dma_state = TX_DMA_IDLE;
 	sirfsoc_uart_tx_with_dma(sirfport);
-	spin_unlock_irqrestore(&sirfport->tx_lock, flags);
+	spin_unlock_irqrestore(&port->lock, flags);
 }
 
 static void sirfsoc_uart_insert_rx_buf_to_tty(
@@ -489,7 +487,6 @@ static void sirfsoc_uart_insert_rx_buf_to_tty(
 	inserted = tty_insert_flip_string(tport,
 		sirfport->rx_dma_items[sirfport->rx_completed].xmit.buf, count);
 	port->icount.rx += inserted;
-	tty_flip_buffer_push(tport);
 }
 
 static void sirfsoc_rx_submit_one_dma_desc(struct uart_port *port, int index)
@@ -525,7 +522,7 @@ static void sirfsoc_rx_tmo_process_tl(unsigned long param)
 	unsigned long flags;
 	struct dma_tx_state tx_state;
 
-	spin_lock_irqsave(&sirfport->rx_lock, flags);
+	spin_lock_irqsave(&port->lock, flags);
 	while (DMA_COMPLETE == dmaengine_tx_status(sirfport->rx_dma_chan,
 		sirfport->rx_dma_items[sirfport->rx_completed].cookie, &tx_state)) {
 		sirfsoc_uart_insert_rx_buf_to_tty(sirfport,
@@ -541,12 +538,8 @@ static void sirfsoc_rx_tmo_process_tl(unsigned long param)
 	wr_regl(port, ureg->sirfsoc_rx_dma_io_ctrl,
 			rd_regl(port, ureg->sirfsoc_rx_dma_io_ctrl) |
 			SIRFUART_IO_MODE);
-	spin_unlock_irqrestore(&sirfport->rx_lock, flags);
-	spin_lock(&port->lock);
 	sirfsoc_uart_pio_rx_chars(port, 4 - sirfport->rx_io_count);
-	spin_unlock(&port->lock);
 	if (sirfport->rx_io_count == 4) {
-		spin_lock_irqsave(&sirfport->rx_lock, flags);
 		sirfport->rx_io_count = 0;
 		wr_regl(port, ureg->sirfsoc_int_st_reg,
 				uint_st->sirfsoc_rx_done);
@@ -557,11 +550,8 @@ static void sirfsoc_rx_tmo_process_tl(unsigned long param)
 		else
 			wr_regl(port, SIRFUART_INT_EN_CLR,
 					uint_en->sirfsoc_rx_done_en);
-		spin_unlock_irqrestore(&sirfport->rx_lock, flags);
-
 		sirfsoc_uart_start_next_rx_dma(port);
 	} else {
-		spin_lock_irqsave(&sirfport->rx_lock, flags);
 		wr_regl(port, ureg->sirfsoc_int_st_reg,
 				uint_st->sirfsoc_rx_done);
 		if (!sirfport->is_marco)
@@ -571,8 +561,9 @@ static void sirfsoc_rx_tmo_process_tl(unsigned long param)
 		else
 			wr_regl(port, ureg->sirfsoc_int_en_reg,
 					uint_en->sirfsoc_rx_done_en);
-		spin_unlock_irqrestore(&sirfport->rx_lock, flags);
 	}
+	spin_unlock_irqrestore(&port->lock, flags);
+	tty_flip_buffer_push(&port->state->port);
 }
 
 static void sirfsoc_uart_handle_rx_tmo(struct sirfsoc_uart_port *sirfport)
@@ -581,8 +572,6 @@ static void sirfsoc_uart_handle_rx_tmo(struct sirfsoc_uart_port *sirfport)
 	struct sirfsoc_register *ureg = &sirfport->uart_reg->uart_reg;
 	struct sirfsoc_int_en *uint_en = &sirfport->uart_reg->uart_int_en;
 	struct dma_tx_state tx_state;
-	spin_lock(&sirfport->rx_lock);
-
 	dmaengine_tx_status(sirfport->rx_dma_chan,
 		sirfport->rx_dma_items[sirfport->rx_issued].cookie, &tx_state);
 	dmaengine_terminate_all(sirfport->rx_dma_chan);
@@ -595,7 +584,6 @@ static void sirfsoc_uart_handle_rx_tmo(struct sirfsoc_uart_port *sirfport)
 	else
 		wr_regl(port, SIRFUART_INT_EN_CLR,
 				uint_en->sirfsoc_rx_timeout_en);
-	spin_unlock(&sirfport->rx_lock);
 	tasklet_schedule(&sirfport->rx_tmo_process_tasklet);
 }
 
@@ -659,7 +647,6 @@ static irqreturn_t sirfsoc_uart_isr(int irq, void *dev_id)
 		intr_status &= port->read_status_mask;
 		uart_insert_char(port, intr_status,
 					uint_en->sirfsoc_rx_oflow_en, 0, flag);
-		tty_flip_buffer_push(&state->port);
 	}
 recv_char:
 	if ((sirfport->uart_reg->uart_type == SIRF_REAL_UART) &&
@@ -684,6 +671,9 @@ recv_char:
 			sirfsoc_uart_pio_rx_chars(port,
 					SIRFSOC_UART_IO_RX_MAX_CNT);
 	}
+	spin_unlock(&port->lock);
+	tty_flip_buffer_push(&state->port);
+	spin_lock(&port->lock);
 	if (intr_status & uint_st->sirfsoc_txfifo_empty) {
 		if (sirfport->tx_dma_chan)
 			sirfsoc_uart_tx_with_dma(sirfport);
@@ -702,6 +692,7 @@ recv_char:
 		}
 	}
 	spin_unlock(&port->lock);
+
 	return IRQ_HANDLED;
 }
 
@@ -713,7 +704,7 @@ static void sirfsoc_uart_rx_dma_complete_tl(unsigned long param)
 	struct sirfsoc_int_en *uint_en = &sirfport->uart_reg->uart_int_en;
 	unsigned long flags;
 	struct dma_tx_state tx_state;
-	spin_lock_irqsave(&sirfport->rx_lock, flags);
+	spin_lock_irqsave(&port->lock, flags);
 	while (DMA_COMPLETE == dmaengine_tx_status(sirfport->rx_dma_chan,
 			sirfport->rx_dma_items[sirfport->rx_completed].cookie, &tx_state)) {
 		sirfsoc_uart_insert_rx_buf_to_tty(sirfport,
@@ -726,17 +717,20 @@ static void sirfsoc_uart_rx_dma_complete_tl(unsigned long param)
 			sirfport->rx_completed++;
 		sirfport->rx_completed %= SIRFSOC_RX_LOOP_BUF_CNT;
 	}
-	spin_unlock_irqrestore(&sirfport->rx_lock, flags);
+	spin_unlock_irqrestore(&port->lock, flags);
+	tty_flip_buffer_push(&port->state->port);
 }
 
 static void sirfsoc_uart_rx_dma_complete_callback(void *param)
 {
 	struct sirfsoc_uart_port *sirfport = (struct sirfsoc_uart_port *)param;
-	spin_lock(&sirfport->rx_lock);
+	unsigned long flags;
+
+	spin_lock_irqsave(&sirfport->port.lock, flags);
 	sirfport->rx_issued++;
 	sirfport->rx_issued %= SIRFSOC_RX_LOOP_BUF_CNT;
-	spin_unlock(&sirfport->rx_lock);
 	tasklet_schedule(&sirfport->rx_dma_complete_tasklet);
+	spin_unlock_irqrestore(&sirfport->port.lock, flags);
 }
 
 /* submit rx dma task into dmaengine */
@@ -745,18 +739,14 @@ static void sirfsoc_uart_start_next_rx_dma(struct uart_port *port)
 	struct sirfsoc_uart_port *sirfport = to_sirfport(port);
 	struct sirfsoc_register *ureg = &sirfport->uart_reg->uart_reg;
 	struct sirfsoc_int_en *uint_en = &sirfport->uart_reg->uart_int_en;
-	unsigned long flags;
 	int i;
-	spin_lock_irqsave(&sirfport->rx_lock, flags);
 	sirfport->rx_io_count = 0;
 	wr_regl(port, ureg->sirfsoc_rx_dma_io_ctrl,
 		rd_regl(port, ureg->sirfsoc_rx_dma_io_ctrl) &
 		~SIRFUART_IO_MODE);
-	spin_unlock_irqrestore(&sirfport->rx_lock, flags);
 	for (i = 0; i < SIRFSOC_RX_LOOP_BUF_CNT; i++)
 		sirfsoc_rx_submit_one_dma_desc(port, i);
 	sirfport->rx_completed = sirfport->rx_issued = 0;
-	spin_lock_irqsave(&sirfport->rx_lock, flags);
 	if (!sirfport->is_marco)
 		wr_regl(port, ureg->sirfsoc_int_en_reg,
 				rd_regl(port, ureg->sirfsoc_int_en_reg) |
@@ -764,7 +754,6 @@ static void sirfsoc_uart_start_next_rx_dma(struct uart_port *port)
 	else
 		wr_regl(port, ureg->sirfsoc_int_en_reg,
 			SIRFUART_RX_DMA_INT_EN(port, uint_en));
-	spin_unlock_irqrestore(&sirfport->rx_lock, flags);
 }
 
 static void sirfsoc_uart_start_rx(struct uart_port *port)
@@ -1228,7 +1217,7 @@ static void sirfsoc_uart_console_putchar(struct uart_port *port, int ch)
 	while (rd_regl(port,
 		ureg->sirfsoc_tx_fifo_status) & ufifo_st->ff_full(port->line))
 		cpu_relax();
-	wr_regb(port, ureg->sirfsoc_tx_fifo_data, ch);
+	wr_regl(port, ureg->sirfsoc_tx_fifo_data, ch);
 }
 
 static void sirfsoc_uart_console_write(struct console *co, const char *s,
@@ -1369,8 +1358,6 @@ usp_no_flow_control:
 		ret = -EFAULT;
 		goto err;
 	}
-	spin_lock_init(&sirfport->rx_lock);
-	spin_lock_init(&sirfport->tx_lock);
 	tasklet_init(&sirfport->rx_dma_complete_tasklet,
 			sirfsoc_uart_rx_dma_complete_tl, (unsigned long)sirfport);
 	tasklet_init(&sirfport->rx_tmo_process_tasklet,
diff --git a/drivers/tty/serial/sirfsoc_uart.h b/drivers/tty/serial/sirfsoc_uart.h
index 8a6eddad2f3..69a62ebd3af 100644
--- a/drivers/tty/serial/sirfsoc_uart.h
+++ b/drivers/tty/serial/sirfsoc_uart.h
@@ -424,8 +424,6 @@ struct sirfsoc_uart_port {
 	struct dma_chan			*tx_dma_chan;
 	dma_addr_t			tx_dma_addr;
 	struct dma_async_tx_descriptor	*tx_dma_desc;
-	spinlock_t			rx_lock;
-	spinlock_t			tx_lock;
 	struct tasklet_struct		rx_dma_complete_tasklet;
 	struct tasklet_struct		rx_tmo_process_tasklet;
 	unsigned int			rx_io_count;
@@ -441,9 +439,7 @@ struct sirfsoc_uart_port {
 
 /* Register Access Control */
 #define portaddr(port, reg)		((port)->membase + (reg))
-#define rd_regb(port, reg)		(__raw_readb(portaddr(port, reg)))
 #define rd_regl(port, reg)		(__raw_readl(portaddr(port, reg)))
-#define wr_regb(port, reg, val)		__raw_writeb(val, portaddr(port, reg))
 #define wr_regl(port, reg, val)		__raw_writel(val, portaddr(port, reg))
 
 /* UART Port Mask */
diff --git a/drivers/tty/serial/st-asc.c b/drivers/tty/serial/st-asc.c
index dd3a96e0702..c7f61ac2713 100644
--- a/drivers/tty/serial/st-asc.c
+++ b/drivers/tty/serial/st-asc.c
@@ -194,9 +194,9 @@ static inline u32 asc_txfifo_is_empty(struct uart_port *port)
 	return asc_in(port, ASC_STA) & ASC_STA_TE;
 }
 
-static inline int asc_txfifo_is_full(struct uart_port *port)
+static inline u32 asc_txfifo_is_half_empty(struct uart_port *port)
 {
-	return asc_in(port, ASC_STA) & ASC_STA_TF;
+	return asc_in(port, ASC_STA) & ASC_STA_THE;
 }
 
 static inline const char *asc_port_name(struct uart_port *port)
@@ -628,7 +628,7 @@ static int asc_get_poll_char(struct uart_port *port)
 
 static void asc_put_poll_char(struct uart_port *port, unsigned char c)
 {
-	while (asc_txfifo_is_full(port))
+	while (!asc_txfifo_is_half_empty(port))
 		cpu_relax();
 	asc_out(port, ASC_TXBUF, c);
 }
@@ -783,7 +783,7 @@ static void asc_console_putchar(struct uart_port *port, int ch)
 	unsigned int timeout = 1000000;
 
 	/* Wait for upto 1 second in case flow control is stopping us. */
-	while (--timeout && asc_txfifo_is_full(port))
+	while (--timeout && !asc_txfifo_is_half_empty(port))
 		udelay(1);
 
 	asc_out(port, ASC_TXBUF, ch);
diff --git a/drivers/tty/serial/tilegx.c b/drivers/tty/serial/tilegx.c
index f92d7e6bd87..613ccf09dc2 100644
--- a/drivers/tty/serial/tilegx.c
+++ b/drivers/tty/serial/tilegx.c
@@ -359,8 +359,8 @@ static int tilegx_startup(struct uart_port *port)
 		}
 
 		/* Create our IRQs. */
-		port->irq = create_irq();
-		if (port->irq < 0)
+		port->irq = irq_alloc_hwirq(-1);
+		if (!port->irq)
 			goto err_uart_dest;
 		tile_irq_activate(port->irq, TILE_IRQ_PERCPU);
 
@@ -395,7 +395,7 @@ static int tilegx_startup(struct uart_port *port)
 err_free_irq:
 	free_irq(port->irq, port);
 err_dest_irq:
-	destroy_irq(port->irq);
+	irq_free_hwirq(port->irq);
 err_uart_dest:
 	gxio_uart_destroy(context);
 	ret = -ENXIO;
@@ -435,7 +435,7 @@ static void tilegx_shutdown(struct uart_port *port)
 
 	if (port->irq > 0) {
 		free_irq(port->irq, port);
-		destroy_irq(port->irq);
+		irq_free_hwirq(port->irq);
 		port->irq = 0;
 	}
 
diff --git a/drivers/tty/serial/uartlite.c b/drivers/tty/serial/uartlite.c
index 5f90ef24d47..dce27f34937 100644
--- a/drivers/tty/serial/uartlite.c
+++ b/drivers/tty/serial/uartlite.c
@@ -418,14 +418,23 @@ static struct uart_ops ulite_ops = {
 #ifdef CONFIG_SERIAL_UARTLITE_CONSOLE
 static void ulite_console_wait_tx(struct uart_port *port)
 {
-	int i;
 	u8 val;
-
-	/* Spin waiting for TX fifo to have space available */
-	for (i = 0; i < 100000; i++) {
+	unsigned long timeout;
+
+	/*
+	 * Spin waiting for TX fifo to have space available.
+	 * When using the Microblaze Debug Module this can take up to 1s
+	 */
+	timeout = jiffies + msecs_to_jiffies(1000);
+	while (1) {
 		val = uart_in32(ULITE_STATUS, port);
 		if ((val & ULITE_STATUS_TXFULL) == 0)
 			break;
+		if (time_after(jiffies, timeout)) {
+			dev_warn(port->dev,
+				 "timeout waiting for TX buffer empty\n");
+			break;
+		}
 		cpu_relax();
 	}
 }
diff --git a/drivers/tty/serial/xilinx_uartps.c b/drivers/tty/serial/xilinx_uartps.c
index f619ad5b5ea..8809775e2ba 100644
--- a/drivers/tty/serial/xilinx_uartps.c
+++ b/drivers/tty/serial/xilinx_uartps.c
@@ -1,7 +1,7 @@
 /*
- * Xilinx PS UART driver
+ * Cadence UART driver (found in Xilinx Zynq)
  *
- * 2011 - 2013 (C) Xilinx Inc.
+ * 2011 - 2014 (C) Xilinx Inc.
  *
  * This program is free software; you can redistribute it
  * and/or modify it under the terms of the GNU General Public
@@ -9,6 +9,9 @@
  * either version 2 of the License, or (at your option) any
  * later version.
  *
+ * This driver has originally been pushed by Xilinx using a Zynq-branding. This
+ * still shows in the naming of this file, the kconfig symbols and some symbols
+ * in the code.
  */
 
 #if defined(CONFIG_SERIAL_XILINX_PS_UART_CONSOLE) && defined(CONFIG_MAGIC_SYSRQ)
@@ -28,16 +31,16 @@
 #include <linux/of.h>
 #include <linux/module.h>
 
-#define XUARTPS_TTY_NAME	"ttyPS"
-#define XUARTPS_NAME		"xuartps"
-#define XUARTPS_MAJOR		0	/* use dynamic node allocation */
-#define XUARTPS_MINOR		0	/* works best with devtmpfs */
-#define XUARTPS_NR_PORTS	2
-#define XUARTPS_FIFO_SIZE	64	/* FIFO size */
-#define XUARTPS_REGISTER_SPACE	0xFFF
+#define CDNS_UART_TTY_NAME	"ttyPS"
+#define CDNS_UART_NAME		"xuartps"
+#define CDNS_UART_MAJOR		0	/* use dynamic node allocation */
+#define CDNS_UART_MINOR		0	/* works best with devtmpfs */
+#define CDNS_UART_NR_PORTS	2
+#define CDNS_UART_FIFO_SIZE	64	/* FIFO size */
+#define CDNS_UART_REGISTER_SPACE	0xFFF
 
-#define xuartps_readl(offset)		ioread32(port->membase + offset)
-#define xuartps_writel(val, offset)	iowrite32(val, port->membase + offset)
+#define cdns_uart_readl(offset)		ioread32(port->membase + offset)
+#define cdns_uart_writel(val, offset)	iowrite32(val, port->membase + offset)
 
 /* Rx Trigger level */
 static int rx_trigger_level = 56;
@@ -49,77 +52,62 @@ static int rx_timeout = 10;
 module_param(rx_timeout, uint, S_IRUGO);
 MODULE_PARM_DESC(rx_timeout, "Rx timeout, 1-255");
 
-/********************************Register Map********************************/
-/** UART
- *
- * Register offsets for the UART.
- *
- */
-#define XUARTPS_CR_OFFSET	0x00  /* Control Register [8:0] */
-#define XUARTPS_MR_OFFSET	0x04  /* Mode Register [10:0] */
-#define XUARTPS_IER_OFFSET	0x08  /* Interrupt Enable [10:0] */
-#define XUARTPS_IDR_OFFSET	0x0C  /* Interrupt Disable [10:0] */
-#define XUARTPS_IMR_OFFSET	0x10  /* Interrupt Mask [10:0] */
-#define XUARTPS_ISR_OFFSET	0x14  /* Interrupt Status [10:0]*/
-#define XUARTPS_BAUDGEN_OFFSET	0x18  /* Baud Rate Generator [15:0] */
-#define XUARTPS_RXTOUT_OFFSET	0x1C  /* RX Timeout [7:0] */
-#define XUARTPS_RXWM_OFFSET	0x20  /* RX FIFO Trigger Level [5:0] */
-#define XUARTPS_MODEMCR_OFFSET	0x24  /* Modem Control [5:0] */
-#define XUARTPS_MODEMSR_OFFSET	0x28  /* Modem Status [8:0] */
-#define XUARTPS_SR_OFFSET	0x2C  /* Channel Status [11:0] */
-#define XUARTPS_FIFO_OFFSET	0x30  /* FIFO [15:0] or [7:0] */
-#define XUARTPS_BAUDDIV_OFFSET	0x34  /* Baud Rate Divider [7:0] */
-#define XUARTPS_FLOWDEL_OFFSET	0x38  /* Flow Delay [15:0] */
-#define XUARTPS_IRRX_PWIDTH_OFFSET 0x3C /* IR Minimum Received Pulse
-						Width [15:0] */
-#define XUARTPS_IRTX_PWIDTH_OFFSET 0x40 /* IR Transmitted pulse
-						Width [7:0] */
-#define XUARTPS_TXWM_OFFSET	0x44  /* TX FIFO Trigger Level [5:0] */
-
-/** Control Register
- *
- * The Control register (CR) controls the major functions of the device.
- *
- * Control Register Bit Definitions
- */
-#define XUARTPS_CR_STOPBRK	0x00000100  /* Stop TX break */
-#define XUARTPS_CR_STARTBRK	0x00000080  /* Set TX break */
-#define XUARTPS_CR_TX_DIS	0x00000020  /* TX disabled. */
-#define XUARTPS_CR_TX_EN	0x00000010  /* TX enabled */
-#define XUARTPS_CR_RX_DIS	0x00000008  /* RX disabled. */
-#define XUARTPS_CR_RX_EN	0x00000004  /* RX enabled */
-#define XUARTPS_CR_TXRST	0x00000002  /* TX logic reset */
-#define XUARTPS_CR_RXRST	0x00000001  /* RX logic reset */
-#define XUARTPS_CR_RST_TO	0x00000040  /* Restart Timeout Counter */
-
-/** Mode Register
- *
+/* Register offsets for the UART. */
+#define CDNS_UART_CR_OFFSET		0x00  /* Control Register */
+#define CDNS_UART_MR_OFFSET		0x04  /* Mode Register */
+#define CDNS_UART_IER_OFFSET		0x08  /* Interrupt Enable */
+#define CDNS_UART_IDR_OFFSET		0x0C  /* Interrupt Disable */
+#define CDNS_UART_IMR_OFFSET		0x10  /* Interrupt Mask */
+#define CDNS_UART_ISR_OFFSET		0x14  /* Interrupt Status */
+#define CDNS_UART_BAUDGEN_OFFSET	0x18  /* Baud Rate Generator */
+#define CDNS_UART_RXTOUT_OFFSET		0x1C  /* RX Timeout */
+#define CDNS_UART_RXWM_OFFSET		0x20  /* RX FIFO Trigger Level */
+#define CDNS_UART_MODEMCR_OFFSET	0x24  /* Modem Control */
+#define CDNS_UART_MODEMSR_OFFSET	0x28  /* Modem Status */
+#define CDNS_UART_SR_OFFSET		0x2C  /* Channel Status */
+#define CDNS_UART_FIFO_OFFSET		0x30  /* FIFO */
+#define CDNS_UART_BAUDDIV_OFFSET	0x34  /* Baud Rate Divider */
+#define CDNS_UART_FLOWDEL_OFFSET	0x38  /* Flow Delay */
+#define CDNS_UART_IRRX_PWIDTH_OFFSET	0x3C  /* IR Min Received Pulse Width */
+#define CDNS_UART_IRTX_PWIDTH_OFFSET	0x40  /* IR Transmitted pulse Width */
+#define CDNS_UART_TXWM_OFFSET		0x44  /* TX FIFO Trigger Level */
+
+/* Control Register Bit Definitions */
+#define CDNS_UART_CR_STOPBRK	0x00000100  /* Stop TX break */
+#define CDNS_UART_CR_STARTBRK	0x00000080  /* Set TX break */
+#define CDNS_UART_CR_TX_DIS	0x00000020  /* TX disabled. */
+#define CDNS_UART_CR_TX_EN	0x00000010  /* TX enabled */
+#define CDNS_UART_CR_RX_DIS	0x00000008  /* RX disabled. */
+#define CDNS_UART_CR_RX_EN	0x00000004  /* RX enabled */
+#define CDNS_UART_CR_TXRST	0x00000002  /* TX logic reset */
+#define CDNS_UART_CR_RXRST	0x00000001  /* RX logic reset */
+#define CDNS_UART_CR_RST_TO	0x00000040  /* Restart Timeout Counter */
+
+/*
+ * Mode Register:
  * The mode register (MR) defines the mode of transfer as well as the data
  * format. If this register is modified during transmission or reception,
  * data validity cannot be guaranteed.
- *
- * Mode Register Bit Definitions
- *
  */
-#define XUARTPS_MR_CLKSEL		0x00000001  /* Pre-scalar selection */
-#define XUARTPS_MR_CHMODE_L_LOOP	0x00000200  /* Local loop back mode */
-#define XUARTPS_MR_CHMODE_NORM		0x00000000  /* Normal mode */
+#define CDNS_UART_MR_CLKSEL		0x00000001  /* Pre-scalar selection */
+#define CDNS_UART_MR_CHMODE_L_LOOP	0x00000200  /* Local loop back mode */
+#define CDNS_UART_MR_CHMODE_NORM	0x00000000  /* Normal mode */
 
-#define XUARTPS_MR_STOPMODE_2_BIT	0x00000080  /* 2 stop bits */
-#define XUARTPS_MR_STOPMODE_1_BIT	0x00000000  /* 1 stop bit */
+#define CDNS_UART_MR_STOPMODE_2_BIT	0x00000080  /* 2 stop bits */
+#define CDNS_UART_MR_STOPMODE_1_BIT	0x00000000  /* 1 stop bit */
 
-#define XUARTPS_MR_PARITY_NONE		0x00000020  /* No parity mode */
-#define XUARTPS_MR_PARITY_MARK		0x00000018  /* Mark parity mode */
-#define XUARTPS_MR_PARITY_SPACE		0x00000010  /* Space parity mode */
-#define XUARTPS_MR_PARITY_ODD		0x00000008  /* Odd parity mode */
-#define XUARTPS_MR_PARITY_EVEN		0x00000000  /* Even parity mode */
+#define CDNS_UART_MR_PARITY_NONE	0x00000020  /* No parity mode */
+#define CDNS_UART_MR_PARITY_MARK	0x00000018  /* Mark parity mode */
+#define CDNS_UART_MR_PARITY_SPACE	0x00000010  /* Space parity mode */
+#define CDNS_UART_MR_PARITY_ODD		0x00000008  /* Odd parity mode */
+#define CDNS_UART_MR_PARITY_EVEN	0x00000000  /* Even parity mode */
 
-#define XUARTPS_MR_CHARLEN_6_BIT	0x00000006  /* 6 bits data */
-#define XUARTPS_MR_CHARLEN_7_BIT	0x00000004  /* 7 bits data */
-#define XUARTPS_MR_CHARLEN_8_BIT	0x00000000  /* 8 bits data */
+#define CDNS_UART_MR_CHARLEN_6_BIT	0x00000006  /* 6 bits data */
+#define CDNS_UART_MR_CHARLEN_7_BIT	0x00000004  /* 7 bits data */
+#define CDNS_UART_MR_CHARLEN_8_BIT	0x00000000  /* 8 bits data */
 
-/** Interrupt Registers
- *
+/*
+ * Interrupt Registers:
  * Interrupt control logic uses the interrupt enable register (IER) and the
  * interrupt disable register (IDR) to set the value of the bits in the
  * interrupt mask register (IMR). The IMR determines whether to pass an
@@ -127,65 +115,65 @@ MODULE_PARM_DESC(rx_timeout, "Rx timeout, 1-255");
  * Writing a 1 to IER Enables an interrupt, writing a 1 to IDR disables an
  * interrupt. IMR and ISR are read only, and IER and IDR are write only.
  * Reading either IER or IDR returns 0x00.
- *
  * All four registers have the same bit definitions.
  */
-#define XUARTPS_IXR_TOUT	0x00000100 /* RX Timeout error interrupt */
-#define XUARTPS_IXR_PARITY	0x00000080 /* Parity error interrupt */
-#define XUARTPS_IXR_FRAMING	0x00000040 /* Framing error interrupt */
-#define XUARTPS_IXR_OVERRUN	0x00000020 /* Overrun error interrupt */
-#define XUARTPS_IXR_TXFULL	0x00000010 /* TX FIFO Full interrupt */
-#define XUARTPS_IXR_TXEMPTY	0x00000008 /* TX FIFO empty interrupt */
-#define XUARTPS_ISR_RXEMPTY	0x00000002 /* RX FIFO empty interrupt */
-#define XUARTPS_IXR_RXTRIG	0x00000001 /* RX FIFO trigger interrupt */
-#define XUARTPS_IXR_RXFULL	0x00000004 /* RX FIFO full interrupt. */
-#define XUARTPS_IXR_RXEMPTY	0x00000002 /* RX FIFO empty interrupt. */
-#define XUARTPS_IXR_MASK	0x00001FFF /* Valid bit mask */
+#define CDNS_UART_IXR_TOUT	0x00000100 /* RX Timeout error interrupt */
+#define CDNS_UART_IXR_PARITY	0x00000080 /* Parity error interrupt */
+#define CDNS_UART_IXR_FRAMING	0x00000040 /* Framing error interrupt */
+#define CDNS_UART_IXR_OVERRUN	0x00000020 /* Overrun error interrupt */
+#define CDNS_UART_IXR_TXFULL	0x00000010 /* TX FIFO Full interrupt */
+#define CDNS_UART_IXR_TXEMPTY	0x00000008 /* TX FIFO empty interrupt */
+#define CDNS_UART_ISR_RXEMPTY	0x00000002 /* RX FIFO empty interrupt */
+#define CDNS_UART_IXR_RXTRIG	0x00000001 /* RX FIFO trigger interrupt */
+#define CDNS_UART_IXR_RXFULL	0x00000004 /* RX FIFO full interrupt. */
+#define CDNS_UART_IXR_RXEMPTY	0x00000002 /* RX FIFO empty interrupt. */
+#define CDNS_UART_IXR_MASK	0x00001FFF /* Valid bit mask */
 
 /* Goes in read_status_mask for break detection as the HW doesn't do it*/
-#define XUARTPS_IXR_BRK		0x80000000
+#define CDNS_UART_IXR_BRK	0x80000000
 
-/** Channel Status Register
- *
+/*
+ * Channel Status Register:
  * The channel status register (CSR) is provided to enable the control logic
  * to monitor the status of bits in the channel interrupt status register,
  * even if these are masked out by the interrupt mask register.
  */
-#define XUARTPS_SR_RXEMPTY	0x00000002 /* RX FIFO empty */
-#define XUARTPS_SR_TXEMPTY	0x00000008 /* TX FIFO empty */
-#define XUARTPS_SR_TXFULL	0x00000010 /* TX FIFO full */
-#define XUARTPS_SR_RXTRIG	0x00000001 /* Rx Trigger */
+#define CDNS_UART_SR_RXEMPTY	0x00000002 /* RX FIFO empty */
+#define CDNS_UART_SR_TXEMPTY	0x00000008 /* TX FIFO empty */
+#define CDNS_UART_SR_TXFULL	0x00000010 /* TX FIFO full */
+#define CDNS_UART_SR_RXTRIG	0x00000001 /* Rx Trigger */
 
 /* baud dividers min/max values */
-#define XUARTPS_BDIV_MIN	4
-#define XUARTPS_BDIV_MAX	255
-#define XUARTPS_CD_MAX		65535
+#define CDNS_UART_BDIV_MIN	4
+#define CDNS_UART_BDIV_MAX	255
+#define CDNS_UART_CD_MAX	65535
 
 /**
- * struct xuartps - device data
- * @port		Pointer to the UART port
- * @refclk		Reference clock
- * @aperclk		APB clock
- * @baud		Current baud rate
- * @clk_rate_change_nb	Notifier block for clock changes
+ * struct cdns_uart - device data
+ * @port:		Pointer to the UART port
+ * @uartclk:		Reference clock
+ * @pclk:		APB clock
+ * @baud:		Current baud rate
+ * @clk_rate_change_nb:	Notifier block for clock changes
  */
-struct xuartps {
+struct cdns_uart {
 	struct uart_port	*port;
-	struct clk		*refclk;
-	struct clk		*aperclk;
+	struct clk		*uartclk;
+	struct clk		*pclk;
 	unsigned int		baud;
 	struct notifier_block	clk_rate_change_nb;
 };
-#define to_xuartps(_nb) container_of(_nb, struct xuartps, clk_rate_change_nb);
+#define to_cdns_uart(_nb) container_of(_nb, struct cdns_uart, \
+		clk_rate_change_nb);
 
 /**
- * xuartps_isr - Interrupt handler
+ * cdns_uart_isr - Interrupt handler
  * @irq: Irq number
  * @dev_id: Id of the port
  *
- * Returns IRQHANDLED
- **/
-static irqreturn_t xuartps_isr(int irq, void *dev_id)
+ * Return: IRQHANDLED
+ */
+static irqreturn_t cdns_uart_isr(int irq, void *dev_id)
 {
 	struct uart_port *port = (struct uart_port *)dev_id;
 	unsigned long flags;
@@ -198,43 +186,42 @@ static irqreturn_t xuartps_isr(int irq, void *dev_id)
 	/* Read the interrupt status register to determine which
 	 * interrupt(s) is/are active.
 	 */
-	isrstatus = xuartps_readl(XUARTPS_ISR_OFFSET);
+	isrstatus = cdns_uart_readl(CDNS_UART_ISR_OFFSET);
 
 	/*
 	 * There is no hardware break detection, so we interpret framing
 	 * error with all-zeros data as a break sequence. Most of the time,
 	 * there's another non-zero byte at the end of the sequence.
 	 */
-
-	if (isrstatus & XUARTPS_IXR_FRAMING) {
-		while (!(xuartps_readl(XUARTPS_SR_OFFSET) &
-					XUARTPS_SR_RXEMPTY)) {
-			if (!xuartps_readl(XUARTPS_FIFO_OFFSET)) {
-				port->read_status_mask |= XUARTPS_IXR_BRK;
-				isrstatus &= ~XUARTPS_IXR_FRAMING;
+	if (isrstatus & CDNS_UART_IXR_FRAMING) {
+		while (!(cdns_uart_readl(CDNS_UART_SR_OFFSET) &
+					CDNS_UART_SR_RXEMPTY)) {
+			if (!cdns_uart_readl(CDNS_UART_FIFO_OFFSET)) {
+				port->read_status_mask |= CDNS_UART_IXR_BRK;
+				isrstatus &= ~CDNS_UART_IXR_FRAMING;
 			}
 		}
-		xuartps_writel(XUARTPS_IXR_FRAMING, XUARTPS_ISR_OFFSET);
+		cdns_uart_writel(CDNS_UART_IXR_FRAMING, CDNS_UART_ISR_OFFSET);
 	}
 
 	/* drop byte with parity error if IGNPAR specified */
-	if (isrstatus & port->ignore_status_mask & XUARTPS_IXR_PARITY)
-		isrstatus &= ~(XUARTPS_IXR_RXTRIG | XUARTPS_IXR_TOUT);
+	if (isrstatus & port->ignore_status_mask & CDNS_UART_IXR_PARITY)
+		isrstatus &= ~(CDNS_UART_IXR_RXTRIG | CDNS_UART_IXR_TOUT);
 
 	isrstatus &= port->read_status_mask;
 	isrstatus &= ~port->ignore_status_mask;
 
-	if ((isrstatus & XUARTPS_IXR_TOUT) ||
-		(isrstatus & XUARTPS_IXR_RXTRIG)) {
+	if ((isrstatus & CDNS_UART_IXR_TOUT) ||
+		(isrstatus & CDNS_UART_IXR_RXTRIG)) {
 		/* Receive Timeout Interrupt */
-		while ((xuartps_readl(XUARTPS_SR_OFFSET) &
-			XUARTPS_SR_RXEMPTY) != XUARTPS_SR_RXEMPTY) {
-			data = xuartps_readl(XUARTPS_FIFO_OFFSET);
+		while ((cdns_uart_readl(CDNS_UART_SR_OFFSET) &
+			CDNS_UART_SR_RXEMPTY) != CDNS_UART_SR_RXEMPTY) {
+			data = cdns_uart_readl(CDNS_UART_FIFO_OFFSET);
 
 			/* Non-NULL byte after BREAK is garbage (99%) */
 			if (data && (port->read_status_mask &
-						XUARTPS_IXR_BRK)) {
-				port->read_status_mask &= ~XUARTPS_IXR_BRK;
+						CDNS_UART_IXR_BRK)) {
+				port->read_status_mask &= ~CDNS_UART_IXR_BRK;
 				port->icount.brk++;
 				if (uart_handle_break(port))
 					continue;
@@ -258,16 +245,17 @@ static irqreturn_t xuartps_isr(int irq, void *dev_id)
 
 			port->icount.rx++;
 
-			if (isrstatus & XUARTPS_IXR_PARITY) {
+			if (isrstatus & CDNS_UART_IXR_PARITY) {
 				port->icount.parity++;
 				status = TTY_PARITY;
-			} else if (isrstatus & XUARTPS_IXR_FRAMING) {
+			} else if (isrstatus & CDNS_UART_IXR_FRAMING) {
 				port->icount.frame++;
 				status = TTY_FRAME;
-			} else if (isrstatus & XUARTPS_IXR_OVERRUN)
+			} else if (isrstatus & CDNS_UART_IXR_OVERRUN) {
 				port->icount.overrun++;
+			}
 
-			uart_insert_char(port, isrstatus, XUARTPS_IXR_OVERRUN,
+			uart_insert_char(port, isrstatus, CDNS_UART_IXR_OVERRUN,
 					data, status);
 		}
 		spin_unlock(&port->lock);
@@ -276,10 +264,10 @@ static irqreturn_t xuartps_isr(int irq, void *dev_id)
 	}
 
 	/* Dispatch an appropriate handler */
-	if ((isrstatus & XUARTPS_IXR_TXEMPTY) == XUARTPS_IXR_TXEMPTY) {
+	if ((isrstatus & CDNS_UART_IXR_TXEMPTY) == CDNS_UART_IXR_TXEMPTY) {
 		if (uart_circ_empty(&port->state->xmit)) {
-			xuartps_writel(XUARTPS_IXR_TXEMPTY,
-						XUARTPS_IDR_OFFSET);
+			cdns_uart_writel(CDNS_UART_IXR_TXEMPTY,
+						CDNS_UART_IDR_OFFSET);
 		} else {
 			numbytes = port->fifosize;
 			/* Break if no more data available in the UART buffer */
@@ -287,12 +275,12 @@ static irqreturn_t xuartps_isr(int irq, void *dev_id)
 				if (uart_circ_empty(&port->state->xmit))
 					break;
 				/* Get the data from the UART circular buffer
-				 * and write it to the xuartps's TX_FIFO
+				 * and write it to the cdns_uart's TX_FIFO
 				 * register.
 				 */
-				xuartps_writel(
+				cdns_uart_writel(
 					port->state->xmit.buf[port->state->xmit.
-					tail], XUARTPS_FIFO_OFFSET);
+					tail], CDNS_UART_FIFO_OFFSET);
 
 				port->icount.tx++;
 
@@ -300,7 +288,7 @@ static irqreturn_t xuartps_isr(int irq, void *dev_id)
 				 * the buffer if it reaches limit.
 				 */
 				port->state->xmit.tail =
-					(port->state->xmit.tail + 1) & \
+					(port->state->xmit.tail + 1) &
 						(UART_XMIT_SIZE - 1);
 			}
 
@@ -310,7 +298,7 @@ static irqreturn_t xuartps_isr(int irq, void *dev_id)
 		}
 	}
 
-	xuartps_writel(isrstatus, XUARTPS_ISR_OFFSET);
+	cdns_uart_writel(isrstatus, CDNS_UART_ISR_OFFSET);
 
 	/* be sure to release the lock and tty before leaving */
 	spin_unlock_irqrestore(&port->lock, flags);
@@ -319,13 +307,13 @@ static irqreturn_t xuartps_isr(int irq, void *dev_id)
 }
 
 /**
- * xuartps_calc_baud_divs - Calculate baud rate divisors
+ * cdns_uart_calc_baud_divs - Calculate baud rate divisors
  * @clk: UART module input clock
  * @baud: Desired baud rate
  * @rbdiv: BDIV value (return value)
  * @rcd: CD value (return value)
  * @div8: Value for clk_sel bit in mod (return value)
- * Returns baud rate, requested baud when possible, or actual baud when there
+ * Return: baud rate, requested baud when possible, or actual baud when there
  *	was too much error, zero if no valid divisors are found.
  *
  * Formula to obtain baud rate is
@@ -338,8 +326,8 @@ static irqreturn_t xuartps_isr(int irq, void *dev_id)
  *			baud rate generate register
  *			baud rate clock divisor register
  */
-static unsigned int xuartps_calc_baud_divs(unsigned int clk, unsigned int baud,
-		u32 *rbdiv, u32 *rcd, int *div8)
+static unsigned int cdns_uart_calc_baud_divs(unsigned int clk,
+		unsigned int baud, u32 *rbdiv, u32 *rcd, int *div8)
 {
 	u32 cd, bdiv;
 	unsigned int calc_baud;
@@ -347,16 +335,16 @@ static unsigned int xuartps_calc_baud_divs(unsigned int clk, unsigned int baud,
 	unsigned int bauderror;
 	unsigned int besterror = ~0;
 
-	if (baud < clk / ((XUARTPS_BDIV_MAX + 1) * XUARTPS_CD_MAX)) {
+	if (baud < clk / ((CDNS_UART_BDIV_MAX + 1) * CDNS_UART_CD_MAX)) {
 		*div8 = 1;
 		clk /= 8;
 	} else {
 		*div8 = 0;
 	}
 
-	for (bdiv = XUARTPS_BDIV_MIN; bdiv <= XUARTPS_BDIV_MAX; bdiv++) {
+	for (bdiv = CDNS_UART_BDIV_MIN; bdiv <= CDNS_UART_BDIV_MAX; bdiv++) {
 		cd = DIV_ROUND_CLOSEST(clk, baud * (bdiv + 1));
-		if (cd < 1 || cd > XUARTPS_CD_MAX)
+		if (cd < 1 || cd > CDNS_UART_CD_MAX)
 			continue;
 
 		calc_baud = clk / (cd * (bdiv + 1));
@@ -381,47 +369,47 @@ static unsigned int xuartps_calc_baud_divs(unsigned int clk, unsigned int baud,
 }
 
 /**
- * xuartps_set_baud_rate - Calculate and set the baud rate
+ * cdns_uart_set_baud_rate - Calculate and set the baud rate
  * @port: Handle to the uart port structure
  * @baud: Baud rate to set
- * Returns baud rate, requested baud when possible, or actual baud when there
+ * Return: baud rate, requested baud when possible, or actual baud when there
  *	   was too much error, zero if no valid divisors are found.
  */
-static unsigned int xuartps_set_baud_rate(struct uart_port *port,
+static unsigned int cdns_uart_set_baud_rate(struct uart_port *port,
 		unsigned int baud)
 {
 	unsigned int calc_baud;
 	u32 cd = 0, bdiv = 0;
 	u32 mreg;
 	int div8;
-	struct xuartps *xuartps = port->private_data;
+	struct cdns_uart *cdns_uart = port->private_data;
 
-	calc_baud = xuartps_calc_baud_divs(port->uartclk, baud, &bdiv, &cd,
+	calc_baud = cdns_uart_calc_baud_divs(port->uartclk, baud, &bdiv, &cd,
 			&div8);
 
 	/* Write new divisors to hardware */
-	mreg = xuartps_readl(XUARTPS_MR_OFFSET);
+	mreg = cdns_uart_readl(CDNS_UART_MR_OFFSET);
 	if (div8)
-		mreg |= XUARTPS_MR_CLKSEL;
+		mreg |= CDNS_UART_MR_CLKSEL;
 	else
-		mreg &= ~XUARTPS_MR_CLKSEL;
-	xuartps_writel(mreg, XUARTPS_MR_OFFSET);
-	xuartps_writel(cd, XUARTPS_BAUDGEN_OFFSET);
-	xuartps_writel(bdiv, XUARTPS_BAUDDIV_OFFSET);
-	xuartps->baud = baud;
+		mreg &= ~CDNS_UART_MR_CLKSEL;
+	cdns_uart_writel(mreg, CDNS_UART_MR_OFFSET);
+	cdns_uart_writel(cd, CDNS_UART_BAUDGEN_OFFSET);
+	cdns_uart_writel(bdiv, CDNS_UART_BAUDDIV_OFFSET);
+	cdns_uart->baud = baud;
 
 	return calc_baud;
 }
 
 #ifdef CONFIG_COMMON_CLK
 /**
- * xuartps_clk_notitifer_cb - Clock notifier callback
+ * cdns_uart_clk_notitifer_cb - Clock notifier callback
  * @nb:		Notifier block
  * @event:	Notify event
  * @data:	Notifier data
- * Returns NOTIFY_OK on success, NOTIFY_BAD on error.
+ * Return:	NOTIFY_OK or NOTIFY_DONE on success, NOTIFY_BAD on error.
  */
-static int xuartps_clk_notifier_cb(struct notifier_block *nb,
+static int cdns_uart_clk_notifier_cb(struct notifier_block *nb,
 		unsigned long event, void *data)
 {
 	u32 ctrl_reg;
@@ -429,35 +417,36 @@ static int xuartps_clk_notifier_cb(struct notifier_block *nb,
 	int locked = 0;
 	struct clk_notifier_data *ndata = data;
 	unsigned long flags = 0;
-	struct xuartps *xuartps = to_xuartps(nb);
+	struct cdns_uart *cdns_uart = to_cdns_uart(nb);
 
-	port = xuartps->port;
+	port = cdns_uart->port;
 	if (port->suspended)
 		return NOTIFY_OK;
 
 	switch (event) {
 	case PRE_RATE_CHANGE:
 	{
-		u32 bdiv;
-		u32 cd;
+		u32 bdiv, cd;
 		int div8;
 
 		/*
 		 * Find out if current baud-rate can be achieved with new clock
 		 * frequency.
 		 */
-		if (!xuartps_calc_baud_divs(ndata->new_rate, xuartps->baud,
-					&bdiv, &cd, &div8))
+		if (!cdns_uart_calc_baud_divs(ndata->new_rate, cdns_uart->baud,
+					&bdiv, &cd, &div8)) {
+			dev_warn(port->dev, "clock rate change rejected\n");
 			return NOTIFY_BAD;
+		}
 
-		spin_lock_irqsave(&xuartps->port->lock, flags);
+		spin_lock_irqsave(&cdns_uart->port->lock, flags);
 
 		/* Disable the TX and RX to set baud rate */
-		xuartps_writel(xuartps_readl(XUARTPS_CR_OFFSET) |
-				(XUARTPS_CR_TX_DIS | XUARTPS_CR_RX_DIS),
-				XUARTPS_CR_OFFSET);
+		ctrl_reg = cdns_uart_readl(CDNS_UART_CR_OFFSET);
+		ctrl_reg |= CDNS_UART_CR_TX_DIS | CDNS_UART_CR_RX_DIS;
+		cdns_uart_writel(ctrl_reg, CDNS_UART_CR_OFFSET);
 
-		spin_unlock_irqrestore(&xuartps->port->lock, flags);
+		spin_unlock_irqrestore(&cdns_uart->port->lock, flags);
 
 		return NOTIFY_OK;
 	}
@@ -467,25 +456,25 @@ static int xuartps_clk_notifier_cb(struct notifier_block *nb,
 		 * frequency.
 		 */
 
-		spin_lock_irqsave(&xuartps->port->lock, flags);
+		spin_lock_irqsave(&cdns_uart->port->lock, flags);
 
 		locked = 1;
 		port->uartclk = ndata->new_rate;
 
-		xuartps->baud = xuartps_set_baud_rate(xuartps->port,
-				xuartps->baud);
+		cdns_uart->baud = cdns_uart_set_baud_rate(cdns_uart->port,
+				cdns_uart->baud);
 		/* fall through */
 	case ABORT_RATE_CHANGE:
 		if (!locked)
-			spin_lock_irqsave(&xuartps->port->lock, flags);
+			spin_lock_irqsave(&cdns_uart->port->lock, flags);
 
 		/* Set TX/RX Reset */
-		xuartps_writel(xuartps_readl(XUARTPS_CR_OFFSET) |
-				(XUARTPS_CR_TXRST | XUARTPS_CR_RXRST),
-				XUARTPS_CR_OFFSET);
+		ctrl_reg = cdns_uart_readl(CDNS_UART_CR_OFFSET);
+		ctrl_reg |= CDNS_UART_CR_TXRST | CDNS_UART_CR_RXRST;
+		cdns_uart_writel(ctrl_reg, CDNS_UART_CR_OFFSET);
 
-		while (xuartps_readl(XUARTPS_CR_OFFSET) &
-				(XUARTPS_CR_TXRST | XUARTPS_CR_RXRST))
+		while (cdns_uart_readl(CDNS_UART_CR_OFFSET) &
+				(CDNS_UART_CR_TXRST | CDNS_UART_CR_RXRST))
 			cpu_relax();
 
 		/*
@@ -493,14 +482,13 @@ static int xuartps_clk_notifier_cb(struct notifier_block *nb,
 		 * enable bit and RX enable bit to enable the transmitter and
 		 * receiver.
 		 */
-		xuartps_writel(rx_timeout, XUARTPS_RXTOUT_OFFSET);
-		ctrl_reg = xuartps_readl(XUARTPS_CR_OFFSET);
-		xuartps_writel(
-			(ctrl_reg & ~(XUARTPS_CR_TX_DIS | XUARTPS_CR_RX_DIS)) |
-			(XUARTPS_CR_TX_EN | XUARTPS_CR_RX_EN),
-			XUARTPS_CR_OFFSET);
+		cdns_uart_writel(rx_timeout, CDNS_UART_RXTOUT_OFFSET);
+		ctrl_reg = cdns_uart_readl(CDNS_UART_CR_OFFSET);
+		ctrl_reg &= ~(CDNS_UART_CR_TX_DIS | CDNS_UART_CR_RX_DIS);
+		ctrl_reg |= CDNS_UART_CR_TX_EN | CDNS_UART_CR_RX_EN;
+		cdns_uart_writel(ctrl_reg, CDNS_UART_CR_OFFSET);
 
-		spin_unlock_irqrestore(&xuartps->port->lock, flags);
+		spin_unlock_irqrestore(&cdns_uart->port->lock, flags);
 
 		return NOTIFY_OK;
 	default:
@@ -509,40 +497,36 @@ static int xuartps_clk_notifier_cb(struct notifier_block *nb,
 }
 #endif
 
-/*----------------------Uart Operations---------------------------*/
-
 /**
- * xuartps_start_tx -  Start transmitting bytes
+ * cdns_uart_start_tx -  Start transmitting bytes
  * @port: Handle to the uart port structure
- *
- **/
-static void xuartps_start_tx(struct uart_port *port)
+ */
+static void cdns_uart_start_tx(struct uart_port *port)
 {
 	unsigned int status, numbytes = port->fifosize;
 
 	if (uart_circ_empty(&port->state->xmit) || uart_tx_stopped(port))
 		return;
 
-	status = xuartps_readl(XUARTPS_CR_OFFSET);
+	status = cdns_uart_readl(CDNS_UART_CR_OFFSET);
 	/* Set the TX enable bit and clear the TX disable bit to enable the
 	 * transmitter.
 	 */
-	xuartps_writel((status & ~XUARTPS_CR_TX_DIS) | XUARTPS_CR_TX_EN,
-		XUARTPS_CR_OFFSET);
-
-	while (numbytes-- && ((xuartps_readl(XUARTPS_SR_OFFSET)
-		& XUARTPS_SR_TXFULL)) != XUARTPS_SR_TXFULL) {
+	cdns_uart_writel((status & ~CDNS_UART_CR_TX_DIS) | CDNS_UART_CR_TX_EN,
+		CDNS_UART_CR_OFFSET);
 
+	while (numbytes-- && ((cdns_uart_readl(CDNS_UART_SR_OFFSET) &
+				CDNS_UART_SR_TXFULL)) != CDNS_UART_SR_TXFULL) {
 		/* Break if no more data available in the UART buffer */
 		if (uart_circ_empty(&port->state->xmit))
 			break;
 
 		/* Get the data from the UART circular buffer and
-		 * write it to the xuartps's TX_FIFO register.
+		 * write it to the cdns_uart's TX_FIFO register.
 		 */
-		xuartps_writel(
+		cdns_uart_writel(
 			port->state->xmit.buf[port->state->xmit.tail],
-			XUARTPS_FIFO_OFFSET);
+			CDNS_UART_FIFO_OFFSET);
 		port->icount.tx++;
 
 		/* Adjust the tail of the UART buffer and wrap
@@ -551,94 +535,90 @@ static void xuartps_start_tx(struct uart_port *port)
 		port->state->xmit.tail = (port->state->xmit.tail + 1) &
 					(UART_XMIT_SIZE - 1);
 	}
-	xuartps_writel(XUARTPS_IXR_TXEMPTY, XUARTPS_ISR_OFFSET);
+	cdns_uart_writel(CDNS_UART_IXR_TXEMPTY, CDNS_UART_ISR_OFFSET);
 	/* Enable the TX Empty interrupt */
-	xuartps_writel(XUARTPS_IXR_TXEMPTY, XUARTPS_IER_OFFSET);
+	cdns_uart_writel(CDNS_UART_IXR_TXEMPTY, CDNS_UART_IER_OFFSET);
 
 	if (uart_circ_chars_pending(&port->state->xmit) < WAKEUP_CHARS)
 		uart_write_wakeup(port);
 }
 
 /**
- * xuartps_stop_tx - Stop TX
+ * cdns_uart_stop_tx - Stop TX
  * @port: Handle to the uart port structure
- *
- **/
-static void xuartps_stop_tx(struct uart_port *port)
+ */
+static void cdns_uart_stop_tx(struct uart_port *port)
 {
 	unsigned int regval;
 
-	regval = xuartps_readl(XUARTPS_CR_OFFSET);
-	regval |= XUARTPS_CR_TX_DIS;
+	regval = cdns_uart_readl(CDNS_UART_CR_OFFSET);
+	regval |= CDNS_UART_CR_TX_DIS;
 	/* Disable the transmitter */
-	xuartps_writel(regval, XUARTPS_CR_OFFSET);
+	cdns_uart_writel(regval, CDNS_UART_CR_OFFSET);
 }
 
 /**
- * xuartps_stop_rx - Stop RX
+ * cdns_uart_stop_rx - Stop RX
  * @port: Handle to the uart port structure
- *
- **/
-static void xuartps_stop_rx(struct uart_port *port)
+ */
+static void cdns_uart_stop_rx(struct uart_port *port)
 {
 	unsigned int regval;
 
-	regval = xuartps_readl(XUARTPS_CR_OFFSET);
-	regval |= XUARTPS_CR_RX_DIS;
+	regval = cdns_uart_readl(CDNS_UART_CR_OFFSET);
+	regval |= CDNS_UART_CR_RX_DIS;
 	/* Disable the receiver */
-	xuartps_writel(regval, XUARTPS_CR_OFFSET);
+	cdns_uart_writel(regval, CDNS_UART_CR_OFFSET);
 }
 
 /**
- * xuartps_tx_empty -  Check whether TX is empty
+ * cdns_uart_tx_empty -  Check whether TX is empty
  * @port: Handle to the uart port structure
  *
- * Returns TIOCSER_TEMT on success, 0 otherwise
- **/
-static unsigned int xuartps_tx_empty(struct uart_port *port)
+ * Return: TIOCSER_TEMT on success, 0 otherwise
+ */
+static unsigned int cdns_uart_tx_empty(struct uart_port *port)
 {
 	unsigned int status;
 
-	status = xuartps_readl(XUARTPS_ISR_OFFSET) & XUARTPS_IXR_TXEMPTY;
+	status = cdns_uart_readl(CDNS_UART_ISR_OFFSET) & CDNS_UART_IXR_TXEMPTY;
 	return status ? TIOCSER_TEMT : 0;
 }
 
 /**
- * xuartps_break_ctl - Based on the input ctl we have to start or stop
+ * cdns_uart_break_ctl - Based on the input ctl we have to start or stop
  *			transmitting char breaks
  * @port: Handle to the uart port structure
  * @ctl: Value based on which start or stop decision is taken
- *
- **/
-static void xuartps_break_ctl(struct uart_port *port, int ctl)
+ */
+static void cdns_uart_break_ctl(struct uart_port *port, int ctl)
 {
 	unsigned int status;
 	unsigned long flags;
 
 	spin_lock_irqsave(&port->lock, flags);
 
-	status = xuartps_readl(XUARTPS_CR_OFFSET);
+	status = cdns_uart_readl(CDNS_UART_CR_OFFSET);
 
 	if (ctl == -1)
-		xuartps_writel(XUARTPS_CR_STARTBRK | status,
-					XUARTPS_CR_OFFSET);
+		cdns_uart_writel(CDNS_UART_CR_STARTBRK | status,
+					CDNS_UART_CR_OFFSET);
 	else {
-		if ((status & XUARTPS_CR_STOPBRK) == 0)
-			xuartps_writel(XUARTPS_CR_STOPBRK | status,
-					 XUARTPS_CR_OFFSET);
+		if ((status & CDNS_UART_CR_STOPBRK) == 0)
+			cdns_uart_writel(CDNS_UART_CR_STOPBRK | status,
+					 CDNS_UART_CR_OFFSET);
 	}
 	spin_unlock_irqrestore(&port->lock, flags);
 }
 
 /**
- * xuartps_set_termios - termios operations, handling data length, parity,
+ * cdns_uart_set_termios - termios operations, handling data length, parity,
  *				stop bits, flow control, baud rate
  * @port: Handle to the uart port structure
  * @termios: Handle to the input termios structure
  * @old: Values of the previously saved termios structure
- *
- **/
-static void xuartps_set_termios(struct uart_port *port,
+ */
+static void cdns_uart_set_termios(struct uart_port *port,
 				struct ktermios *termios, struct ktermios *old)
 {
 	unsigned int cval = 0;
@@ -649,81 +629,79 @@ static void xuartps_set_termios(struct uart_port *port,
 	spin_lock_irqsave(&port->lock, flags);
 
 	/* Empty the receive FIFO 1st before making changes */
-	while ((xuartps_readl(XUARTPS_SR_OFFSET) &
-		 XUARTPS_SR_RXEMPTY) != XUARTPS_SR_RXEMPTY) {
-		xuartps_readl(XUARTPS_FIFO_OFFSET);
+	while ((cdns_uart_readl(CDNS_UART_SR_OFFSET) &
+		 CDNS_UART_SR_RXEMPTY) != CDNS_UART_SR_RXEMPTY) {
+		cdns_uart_readl(CDNS_UART_FIFO_OFFSET);
 	}
 
 	/* Disable the TX and RX to set baud rate */
-	xuartps_writel(xuartps_readl(XUARTPS_CR_OFFSET) |
-			(XUARTPS_CR_TX_DIS | XUARTPS_CR_RX_DIS),
-			XUARTPS_CR_OFFSET);
+	ctrl_reg = cdns_uart_readl(CDNS_UART_CR_OFFSET);
+	ctrl_reg |= CDNS_UART_CR_TX_DIS | CDNS_UART_CR_RX_DIS;
+	cdns_uart_writel(ctrl_reg, CDNS_UART_CR_OFFSET);
 
 	/*
 	 * Min baud rate = 6bps and Max Baud Rate is 10Mbps for 100Mhz clk
 	 * min and max baud should be calculated here based on port->uartclk.
 	 * this way we get a valid baud and can safely call set_baud()
 	 */
-	minbaud = port->uartclk / ((XUARTPS_BDIV_MAX + 1) * XUARTPS_CD_MAX * 8);
-	maxbaud = port->uartclk / (XUARTPS_BDIV_MIN + 1);
+	minbaud = port->uartclk /
+			((CDNS_UART_BDIV_MAX + 1) * CDNS_UART_CD_MAX * 8);
+	maxbaud = port->uartclk / (CDNS_UART_BDIV_MIN + 1);
 	baud = uart_get_baud_rate(port, termios, old, minbaud, maxbaud);
-	baud = xuartps_set_baud_rate(port, baud);
+	baud = cdns_uart_set_baud_rate(port, baud);
 	if (tty_termios_baud_rate(termios))
 		tty_termios_encode_baud_rate(termios, baud, baud);
 
-	/*
-	 * Update the per-port timeout.
-	 */
+	/* Update the per-port timeout. */
 	uart_update_timeout(port, termios->c_cflag, baud);
 
 	/* Set TX/RX Reset */
-	xuartps_writel(xuartps_readl(XUARTPS_CR_OFFSET) |
-			(XUARTPS_CR_TXRST | XUARTPS_CR_RXRST),
-			XUARTPS_CR_OFFSET);
+	ctrl_reg = cdns_uart_readl(CDNS_UART_CR_OFFSET);
+	ctrl_reg |= CDNS_UART_CR_TXRST | CDNS_UART_CR_RXRST;
+	cdns_uart_writel(ctrl_reg, CDNS_UART_CR_OFFSET);
 
-	ctrl_reg = xuartps_readl(XUARTPS_CR_OFFSET);
-
-	/* Clear the RX disable and TX disable bits and then set the TX enable
+	/*
+	 * Clear the RX disable and TX disable bits and then set the TX enable
 	 * bit and RX enable bit to enable the transmitter and receiver.
 	 */
-	xuartps_writel(
-		(ctrl_reg & ~(XUARTPS_CR_TX_DIS | XUARTPS_CR_RX_DIS))
-			| (XUARTPS_CR_TX_EN | XUARTPS_CR_RX_EN),
-			XUARTPS_CR_OFFSET);
+	ctrl_reg = cdns_uart_readl(CDNS_UART_CR_OFFSET);
+	ctrl_reg &= ~(CDNS_UART_CR_TX_DIS | CDNS_UART_CR_RX_DIS);
+	ctrl_reg |= CDNS_UART_CR_TX_EN | CDNS_UART_CR_RX_EN;
+	cdns_uart_writel(ctrl_reg, CDNS_UART_CR_OFFSET);
 
-	xuartps_writel(rx_timeout, XUARTPS_RXTOUT_OFFSET);
+	cdns_uart_writel(rx_timeout, CDNS_UART_RXTOUT_OFFSET);
 
-	port->read_status_mask = XUARTPS_IXR_TXEMPTY | XUARTPS_IXR_RXTRIG |
-			XUARTPS_IXR_OVERRUN | XUARTPS_IXR_TOUT;
+	port->read_status_mask = CDNS_UART_IXR_TXEMPTY | CDNS_UART_IXR_RXTRIG |
+			CDNS_UART_IXR_OVERRUN | CDNS_UART_IXR_TOUT;
 	port->ignore_status_mask = 0;
 
 	if (termios->c_iflag & INPCK)
-		port->read_status_mask |= XUARTPS_IXR_PARITY |
-		XUARTPS_IXR_FRAMING;
+		port->read_status_mask |= CDNS_UART_IXR_PARITY |
+		CDNS_UART_IXR_FRAMING;
 
 	if (termios->c_iflag & IGNPAR)
-		port->ignore_status_mask |= XUARTPS_IXR_PARITY |
-			XUARTPS_IXR_FRAMING | XUARTPS_IXR_OVERRUN;
+		port->ignore_status_mask |= CDNS_UART_IXR_PARITY |
+			CDNS_UART_IXR_FRAMING | CDNS_UART_IXR_OVERRUN;
 
 	/* ignore all characters if CREAD is not set */
 	if ((termios->c_cflag & CREAD) == 0)
-		port->ignore_status_mask |= XUARTPS_IXR_RXTRIG |
-			XUARTPS_IXR_TOUT | XUARTPS_IXR_PARITY |
-			XUARTPS_IXR_FRAMING | XUARTPS_IXR_OVERRUN;
+		port->ignore_status_mask |= CDNS_UART_IXR_RXTRIG |
+			CDNS_UART_IXR_TOUT | CDNS_UART_IXR_PARITY |
+			CDNS_UART_IXR_FRAMING | CDNS_UART_IXR_OVERRUN;
 
-	mode_reg = xuartps_readl(XUARTPS_MR_OFFSET);
+	mode_reg = cdns_uart_readl(CDNS_UART_MR_OFFSET);
 
 	/* Handling Data Size */
 	switch (termios->c_cflag & CSIZE) {
 	case CS6:
-		cval |= XUARTPS_MR_CHARLEN_6_BIT;
+		cval |= CDNS_UART_MR_CHARLEN_6_BIT;
 		break;
 	case CS7:
-		cval |= XUARTPS_MR_CHARLEN_7_BIT;
+		cval |= CDNS_UART_MR_CHARLEN_7_BIT;
 		break;
 	default:
 	case CS8:
-		cval |= XUARTPS_MR_CHARLEN_8_BIT;
+		cval |= CDNS_UART_MR_CHARLEN_8_BIT;
 		termios->c_cflag &= ~CSIZE;
 		termios->c_cflag |= CS8;
 		break;
@@ -731,134 +709,135 @@ static void xuartps_set_termios(struct uart_port *port,
 
 	/* Handling Parity and Stop Bits length */
 	if (termios->c_cflag & CSTOPB)
-		cval |= XUARTPS_MR_STOPMODE_2_BIT; /* 2 STOP bits */
+		cval |= CDNS_UART_MR_STOPMODE_2_BIT; /* 2 STOP bits */
 	else
-		cval |= XUARTPS_MR_STOPMODE_1_BIT; /* 1 STOP bit */
+		cval |= CDNS_UART_MR_STOPMODE_1_BIT; /* 1 STOP bit */
 
 	if (termios->c_cflag & PARENB) {
 		/* Mark or Space parity */
 		if (termios->c_cflag & CMSPAR) {
 			if (termios->c_cflag & PARODD)
-				cval |= XUARTPS_MR_PARITY_MARK;
+				cval |= CDNS_UART_MR_PARITY_MARK;
 			else
-				cval |= XUARTPS_MR_PARITY_SPACE;
+				cval |= CDNS_UART_MR_PARITY_SPACE;
 		} else {
 			if (termios->c_cflag & PARODD)
-				cval |= XUARTPS_MR_PARITY_ODD;
+				cval |= CDNS_UART_MR_PARITY_ODD;
 			else
-				cval |= XUARTPS_MR_PARITY_EVEN;
+				cval |= CDNS_UART_MR_PARITY_EVEN;
 		}
 	} else {
-		cval |= XUARTPS_MR_PARITY_NONE;
+		cval |= CDNS_UART_MR_PARITY_NONE;
 	}
 	cval |= mode_reg & 1;
-	xuartps_writel(cval, XUARTPS_MR_OFFSET);
+	cdns_uart_writel(cval, CDNS_UART_MR_OFFSET);
 
 	spin_unlock_irqrestore(&port->lock, flags);
 }
 
 /**
- * xuartps_startup - Called when an application opens a xuartps port
+ * cdns_uart_startup - Called when an application opens a cdns_uart port
  * @port: Handle to the uart port structure
  *
- * Returns 0 on success, negative error otherwise
- **/
-static int xuartps_startup(struct uart_port *port)
+ * Return: 0 on success, negative errno otherwise
+ */
+static int cdns_uart_startup(struct uart_port *port)
 {
 	unsigned int retval = 0, status = 0;
 
-	retval = request_irq(port->irq, xuartps_isr, 0, XUARTPS_NAME,
+	retval = request_irq(port->irq, cdns_uart_isr, 0, CDNS_UART_NAME,
 								(void *)port);
 	if (retval)
 		return retval;
 
 	/* Disable the TX and RX */
-	xuartps_writel(XUARTPS_CR_TX_DIS | XUARTPS_CR_RX_DIS,
-						XUARTPS_CR_OFFSET);
+	cdns_uart_writel(CDNS_UART_CR_TX_DIS | CDNS_UART_CR_RX_DIS,
+						CDNS_UART_CR_OFFSET);
 
 	/* Set the Control Register with TX/RX Enable, TX/RX Reset,
 	 * no break chars.
 	 */
-	xuartps_writel(XUARTPS_CR_TXRST | XUARTPS_CR_RXRST,
-				XUARTPS_CR_OFFSET);
+	cdns_uart_writel(CDNS_UART_CR_TXRST | CDNS_UART_CR_RXRST,
+				CDNS_UART_CR_OFFSET);
 
-	status = xuartps_readl(XUARTPS_CR_OFFSET);
+	status = cdns_uart_readl(CDNS_UART_CR_OFFSET);
 
 	/* Clear the RX disable and TX disable bits and then set the TX enable
 	 * bit and RX enable bit to enable the transmitter and receiver.
 	 */
-	xuartps_writel((status & ~(XUARTPS_CR_TX_DIS | XUARTPS_CR_RX_DIS))
-			| (XUARTPS_CR_TX_EN | XUARTPS_CR_RX_EN |
-			XUARTPS_CR_STOPBRK), XUARTPS_CR_OFFSET);
+	cdns_uart_writel((status & ~(CDNS_UART_CR_TX_DIS | CDNS_UART_CR_RX_DIS))
+			| (CDNS_UART_CR_TX_EN | CDNS_UART_CR_RX_EN |
+			CDNS_UART_CR_STOPBRK), CDNS_UART_CR_OFFSET);
 
 	/* Set the Mode Register with normal mode,8 data bits,1 stop bit,
 	 * no parity.
 	 */
-	xuartps_writel(XUARTPS_MR_CHMODE_NORM | XUARTPS_MR_STOPMODE_1_BIT
-		| XUARTPS_MR_PARITY_NONE | XUARTPS_MR_CHARLEN_8_BIT,
-		 XUARTPS_MR_OFFSET);
+	cdns_uart_writel(CDNS_UART_MR_CHMODE_NORM | CDNS_UART_MR_STOPMODE_1_BIT
+		| CDNS_UART_MR_PARITY_NONE | CDNS_UART_MR_CHARLEN_8_BIT,
+		 CDNS_UART_MR_OFFSET);
 
 	/*
 	 * Set the RX FIFO Trigger level to use most of the FIFO, but it
 	 * can be tuned with a module parameter
 	 */
-	xuartps_writel(rx_trigger_level, XUARTPS_RXWM_OFFSET);
+	cdns_uart_writel(rx_trigger_level, CDNS_UART_RXWM_OFFSET);
 
 	/*
 	 * Receive Timeout register is enabled but it
 	 * can be tuned with a module parameter
 	 */
-	xuartps_writel(rx_timeout, XUARTPS_RXTOUT_OFFSET);
+	cdns_uart_writel(rx_timeout, CDNS_UART_RXTOUT_OFFSET);
 
 	/* Clear out any pending interrupts before enabling them */
-	xuartps_writel(xuartps_readl(XUARTPS_ISR_OFFSET), XUARTPS_ISR_OFFSET);
+	cdns_uart_writel(cdns_uart_readl(CDNS_UART_ISR_OFFSET),
+			CDNS_UART_ISR_OFFSET);
 
 	/* Set the Interrupt Registers with desired interrupts */
-	xuartps_writel(XUARTPS_IXR_TXEMPTY | XUARTPS_IXR_PARITY |
-		XUARTPS_IXR_FRAMING | XUARTPS_IXR_OVERRUN |
-		XUARTPS_IXR_RXTRIG | XUARTPS_IXR_TOUT, XUARTPS_IER_OFFSET);
+	cdns_uart_writel(CDNS_UART_IXR_TXEMPTY | CDNS_UART_IXR_PARITY |
+		CDNS_UART_IXR_FRAMING | CDNS_UART_IXR_OVERRUN |
+		CDNS_UART_IXR_RXTRIG | CDNS_UART_IXR_TOUT,
+		CDNS_UART_IER_OFFSET);
 
 	return retval;
 }
 
 /**
- * xuartps_shutdown - Called when an application closes a xuartps port
+ * cdns_uart_shutdown - Called when an application closes a cdns_uart port
  * @port: Handle to the uart port structure
- *
- **/
-static void xuartps_shutdown(struct uart_port *port)
+ */
+static void cdns_uart_shutdown(struct uart_port *port)
 {
 	int status;
 
 	/* Disable interrupts */
-	status = xuartps_readl(XUARTPS_IMR_OFFSET);
-	xuartps_writel(status, XUARTPS_IDR_OFFSET);
+	status = cdns_uart_readl(CDNS_UART_IMR_OFFSET);
+	cdns_uart_writel(status, CDNS_UART_IDR_OFFSET);
 
 	/* Disable the TX and RX */
-	xuartps_writel(XUARTPS_CR_TX_DIS | XUARTPS_CR_RX_DIS,
-				 XUARTPS_CR_OFFSET);
+	cdns_uart_writel(CDNS_UART_CR_TX_DIS | CDNS_UART_CR_RX_DIS,
+				 CDNS_UART_CR_OFFSET);
 	free_irq(port->irq, port);
 }
 
 /**
- * xuartps_type - Set UART type to xuartps port
+ * cdns_uart_type - Set UART type to cdns_uart port
  * @port: Handle to the uart port structure
  *
- * Returns string on success, NULL otherwise
- **/
-static const char *xuartps_type(struct uart_port *port)
+ * Return: string on success, NULL otherwise
+ */
+static const char *cdns_uart_type(struct uart_port *port)
 {
-	return port->type == PORT_XUARTPS ? XUARTPS_NAME : NULL;
+	return port->type == PORT_XUARTPS ? CDNS_UART_NAME : NULL;
 }
 
 /**
- * xuartps_verify_port - Verify the port params
+ * cdns_uart_verify_port - Verify the port params
  * @port: Handle to the uart port structure
  * @ser: Handle to the structure whose members are compared
  *
- * Returns 0 if success otherwise -EINVAL
- **/
-static int xuartps_verify_port(struct uart_port *port,
+ * Return: 0 on success, negative errno otherwise.
+ */
+static int cdns_uart_verify_port(struct uart_port *port,
 					struct serial_struct *ser)
 {
 	if (ser->type != PORT_UNKNOWN && ser->type != PORT_XUARTPS)
@@ -875,187 +854,170 @@ static int xuartps_verify_port(struct uart_port *port,
 }
 
 /**
- * xuartps_request_port - Claim the memory region attached to xuartps port,
- *				called when the driver adds a xuartps port via
+ * cdns_uart_request_port - Claim the memory region attached to cdns_uart port,
+ *				called when the driver adds a cdns_uart port via
  *				uart_add_one_port()
  * @port: Handle to the uart port structure
  *
- * Returns 0, -ENOMEM if request fails
- **/
-static int xuartps_request_port(struct uart_port *port)
+ * Return: 0 on success, negative errno otherwise.
+ */
+static int cdns_uart_request_port(struct uart_port *port)
 {
-	if (!request_mem_region(port->mapbase, XUARTPS_REGISTER_SPACE,
-					 XUARTPS_NAME)) {
+	if (!request_mem_region(port->mapbase, CDNS_UART_REGISTER_SPACE,
+					 CDNS_UART_NAME)) {
 		return -ENOMEM;
 	}
 
-	port->membase = ioremap(port->mapbase, XUARTPS_REGISTER_SPACE);
+	port->membase = ioremap(port->mapbase, CDNS_UART_REGISTER_SPACE);
 	if (!port->membase) {
 		dev_err(port->dev, "Unable to map registers\n");
-		release_mem_region(port->mapbase, XUARTPS_REGISTER_SPACE);
+		release_mem_region(port->mapbase, CDNS_UART_REGISTER_SPACE);
 		return -ENOMEM;
 	}
 	return 0;
 }
 
 /**
- * xuartps_release_port - Release the memory region attached to a xuartps
- *				port, called when the driver removes a xuartps
- *				port via uart_remove_one_port().
+ * cdns_uart_release_port - Release UART port
  * @port: Handle to the uart port structure
  *
- **/
-static void xuartps_release_port(struct uart_port *port)
+ * Release the memory region attached to a cdns_uart port. Called when the
+ * driver removes a cdns_uart port via uart_remove_one_port().
+ */
+static void cdns_uart_release_port(struct uart_port *port)
 {
-	release_mem_region(port->mapbase, XUARTPS_REGISTER_SPACE);
+	release_mem_region(port->mapbase, CDNS_UART_REGISTER_SPACE);
 	iounmap(port->membase);
 	port->membase = NULL;
 }
 
 /**
- * xuartps_config_port - Configure xuartps, called when the driver adds a
- *				xuartps port
+ * cdns_uart_config_port - Configure UART port
  * @port: Handle to the uart port structure
  * @flags: If any
- *
- **/
-static void xuartps_config_port(struct uart_port *port, int flags)
+ */
+static void cdns_uart_config_port(struct uart_port *port, int flags)
 {
-	if (flags & UART_CONFIG_TYPE && xuartps_request_port(port) == 0)
+	if (flags & UART_CONFIG_TYPE && cdns_uart_request_port(port) == 0)
 		port->type = PORT_XUARTPS;
 }
 
 /**
- * xuartps_get_mctrl - Get the modem control state
- *
+ * cdns_uart_get_mctrl - Get the modem control state
  * @port: Handle to the uart port structure
  *
- * Returns the modem control state
- *
- **/
-static unsigned int xuartps_get_mctrl(struct uart_port *port)
+ * Return: the modem control state
+ */
+static unsigned int cdns_uart_get_mctrl(struct uart_port *port)
 {
 	return TIOCM_CTS | TIOCM_DSR | TIOCM_CAR;
 }
 
-static void xuartps_set_mctrl(struct uart_port *port, unsigned int mctrl)
+static void cdns_uart_set_mctrl(struct uart_port *port, unsigned int mctrl)
 {
 	/* N/A */
 }
 
-static void xuartps_enable_ms(struct uart_port *port)
+static void cdns_uart_enable_ms(struct uart_port *port)
 {
 	/* N/A */
 }
 
 #ifdef CONFIG_CONSOLE_POLL
-static int xuartps_poll_get_char(struct uart_port *port)
+static int cdns_uart_poll_get_char(struct uart_port *port)
 {
 	u32 imr;
 	int c;
 
 	/* Disable all interrupts */
-	imr = xuartps_readl(XUARTPS_IMR_OFFSET);
-	xuartps_writel(imr, XUARTPS_IDR_OFFSET);
+	imr = cdns_uart_readl(CDNS_UART_IMR_OFFSET);
+	cdns_uart_writel(imr, CDNS_UART_IDR_OFFSET);
 
 	/* Check if FIFO is empty */
-	if (xuartps_readl(XUARTPS_SR_OFFSET) & XUARTPS_SR_RXEMPTY)
+	if (cdns_uart_readl(CDNS_UART_SR_OFFSET) & CDNS_UART_SR_RXEMPTY)
 		c = NO_POLL_CHAR;
 	else /* Read a character */
-		c = (unsigned char) xuartps_readl(XUARTPS_FIFO_OFFSET);
+		c = (unsigned char) cdns_uart_readl(CDNS_UART_FIFO_OFFSET);
 
 	/* Enable interrupts */
-	xuartps_writel(imr, XUARTPS_IER_OFFSET);
+	cdns_uart_writel(imr, CDNS_UART_IER_OFFSET);
 
 	return c;
 }
 
-static void xuartps_poll_put_char(struct uart_port *port, unsigned char c)
+static void cdns_uart_poll_put_char(struct uart_port *port, unsigned char c)
 {
 	u32 imr;
 
 	/* Disable all interrupts */
-	imr = xuartps_readl(XUARTPS_IMR_OFFSET);
-	xuartps_writel(imr, XUARTPS_IDR_OFFSET);
+	imr = cdns_uart_readl(CDNS_UART_IMR_OFFSET);
+	cdns_uart_writel(imr, CDNS_UART_IDR_OFFSET);
 
 	/* Wait until FIFO is empty */
-	while (!(xuartps_readl(XUARTPS_SR_OFFSET) & XUARTPS_SR_TXEMPTY))
+	while (!(cdns_uart_readl(CDNS_UART_SR_OFFSET) & CDNS_UART_SR_TXEMPTY))
 		cpu_relax();
 
 	/* Write a character */
-	xuartps_writel(c, XUARTPS_FIFO_OFFSET);
+	cdns_uart_writel(c, CDNS_UART_FIFO_OFFSET);
 
 	/* Wait until FIFO is empty */
-	while (!(xuartps_readl(XUARTPS_SR_OFFSET) & XUARTPS_SR_TXEMPTY))
+	while (!(cdns_uart_readl(CDNS_UART_SR_OFFSET) & CDNS_UART_SR_TXEMPTY))
 		cpu_relax();
 
 	/* Enable interrupts */
-	xuartps_writel(imr, XUARTPS_IER_OFFSET);
+	cdns_uart_writel(imr, CDNS_UART_IER_OFFSET);
 
 	return;
 }
 #endif
 
-/** The UART operations structure
- */
-static struct uart_ops xuartps_ops = {
-	.set_mctrl	= xuartps_set_mctrl,
-	.get_mctrl	= xuartps_get_mctrl,
-	.enable_ms	= xuartps_enable_ms,
-
-	.start_tx	= xuartps_start_tx,	/* Start transmitting */
-	.stop_tx	= xuartps_stop_tx,	/* Stop transmission */
-	.stop_rx	= xuartps_stop_rx,	/* Stop reception */
-	.tx_empty	= xuartps_tx_empty,	/* Transmitter busy? */
-	.break_ctl	= xuartps_break_ctl,	/* Start/stop
-						 * transmitting break
-						 */
-	.set_termios	= xuartps_set_termios,	/* Set termios */
-	.startup	= xuartps_startup,	/* App opens xuartps */
-	.shutdown	= xuartps_shutdown,	/* App closes xuartps */
-	.type		= xuartps_type,		/* Set UART type */
-	.verify_port	= xuartps_verify_port,	/* Verification of port
-						 * params
-						 */
-	.request_port	= xuartps_request_port,	/* Claim resources
-						 * associated with a
-						 * xuartps port
-						 */
-	.release_port	= xuartps_release_port,	/* Release resources
-						 * associated with a
-						 * xuartps port
-						 */
-	.config_port	= xuartps_config_port,	/* Configure when driver
-						 * adds a xuartps port
-						 */
+static struct uart_ops cdns_uart_ops = {
+	.set_mctrl	= cdns_uart_set_mctrl,
+	.get_mctrl	= cdns_uart_get_mctrl,
+	.enable_ms	= cdns_uart_enable_ms,
+	.start_tx	= cdns_uart_start_tx,
+	.stop_tx	= cdns_uart_stop_tx,
+	.stop_rx	= cdns_uart_stop_rx,
+	.tx_empty	= cdns_uart_tx_empty,
+	.break_ctl	= cdns_uart_break_ctl,
+	.set_termios	= cdns_uart_set_termios,
+	.startup	= cdns_uart_startup,
+	.shutdown	= cdns_uart_shutdown,
+	.type		= cdns_uart_type,
+	.verify_port	= cdns_uart_verify_port,
+	.request_port	= cdns_uart_request_port,
+	.release_port	= cdns_uart_release_port,
+	.config_port	= cdns_uart_config_port,
 #ifdef CONFIG_CONSOLE_POLL
-	.poll_get_char	= xuartps_poll_get_char,
-	.poll_put_char	= xuartps_poll_put_char,
+	.poll_get_char	= cdns_uart_poll_get_char,
+	.poll_put_char	= cdns_uart_poll_put_char,
 #endif
 };
 
-static struct uart_port xuartps_port[2];
+static struct uart_port cdns_uart_port[2];
 
 /**
- * xuartps_get_port - Configure the port from the platform device resource
- *			info
+ * cdns_uart_get_port - Configure the port from platform device resource info
+ * @id: Port id
  *
- * Returns a pointer to a uart_port or NULL for failure
- **/
-static struct uart_port *xuartps_get_port(void)
+ * Return: a pointer to a uart_port or NULL for failure
+ */
+static struct uart_port *cdns_uart_get_port(int id)
 {
 	struct uart_port *port;
-	int id;
 
-	/* Find the next unused port */
-	for (id = 0; id < XUARTPS_NR_PORTS; id++)
-		if (xuartps_port[id].mapbase == 0)
-			break;
+	/* Try the given port id if failed use default method */
+	if (cdns_uart_port[id].mapbase != 0) {
+		/* Find the next unused port */
+		for (id = 0; id < CDNS_UART_NR_PORTS; id++)
+			if (cdns_uart_port[id].mapbase == 0)
+				break;
+	}
 
-	if (id >= XUARTPS_NR_PORTS)
+	if (id >= CDNS_UART_NR_PORTS)
 		return NULL;
 
-	port = &xuartps_port[id];
+	port = &cdns_uart_port[id];
 
 	/* At this point, we've got an empty uart_port struct, initialize it */
 	spin_lock_init(&port->lock);
@@ -1065,50 +1027,46 @@ static struct uart_port *xuartps_get_port(void)
 	port->type	= PORT_UNKNOWN;
 	port->iotype	= UPIO_MEM32;
 	port->flags	= UPF_BOOT_AUTOCONF;
-	port->ops	= &xuartps_ops;
-	port->fifosize	= XUARTPS_FIFO_SIZE;
+	port->ops	= &cdns_uart_ops;
+	port->fifosize	= CDNS_UART_FIFO_SIZE;
 	port->line	= id;
 	port->dev	= NULL;
 	return port;
 }
 
-/*-----------------------Console driver operations--------------------------*/
-
 #ifdef CONFIG_SERIAL_XILINX_PS_UART_CONSOLE
 /**
- * xuartps_console_wait_tx - Wait for the TX to be full
+ * cdns_uart_console_wait_tx - Wait for the TX to be full
  * @port: Handle to the uart port structure
- *
- **/
-static void xuartps_console_wait_tx(struct uart_port *port)
+ */
+static void cdns_uart_console_wait_tx(struct uart_port *port)
 {
-	while ((xuartps_readl(XUARTPS_SR_OFFSET) & XUARTPS_SR_TXEMPTY)
-				!= XUARTPS_SR_TXEMPTY)
+	while ((cdns_uart_readl(CDNS_UART_SR_OFFSET) & CDNS_UART_SR_TXEMPTY)
+				!= CDNS_UART_SR_TXEMPTY)
 		barrier();
 }
 
 /**
- * xuartps_console_putchar - write the character to the FIFO buffer
+ * cdns_uart_console_putchar - write the character to the FIFO buffer
  * @port: Handle to the uart port structure
  * @ch: Character to be written
- *
- **/
-static void xuartps_console_putchar(struct uart_port *port, int ch)
+ */
+static void cdns_uart_console_putchar(struct uart_port *port, int ch)
 {
-	xuartps_console_wait_tx(port);
-	xuartps_writel(ch, XUARTPS_FIFO_OFFSET);
+	cdns_uart_console_wait_tx(port);
+	cdns_uart_writel(ch, CDNS_UART_FIFO_OFFSET);
 }
 
 /**
- * xuartps_console_write - perform write operation
- * @port: Handle to the uart port structure
+ * cdns_uart_console_write - perform write operation
+ * @co: Console handle
  * @s: Pointer to character array
  * @count: No of characters
- **/
-static void xuartps_console_write(struct console *co, const char *s,
+ */
+static void cdns_uart_console_write(struct console *co, const char *s,
 				unsigned int count)
 {
-	struct uart_port *port = &xuartps_port[co->index];
+	struct uart_port *port = &cdns_uart_port[co->index];
 	unsigned long flags;
 	unsigned int imr, ctrl;
 	int locked = 1;
@@ -1119,49 +1077,45 @@ static void xuartps_console_write(struct console *co, const char *s,
 		spin_lock_irqsave(&port->lock, flags);
 
 	/* save and disable interrupt */
-	imr = xuartps_readl(XUARTPS_IMR_OFFSET);
-	xuartps_writel(imr, XUARTPS_IDR_OFFSET);
+	imr = cdns_uart_readl(CDNS_UART_IMR_OFFSET);
+	cdns_uart_writel(imr, CDNS_UART_IDR_OFFSET);
 
 	/*
 	 * Make sure that the tx part is enabled. Set the TX enable bit and
 	 * clear the TX disable bit to enable the transmitter.
 	 */
-	ctrl = xuartps_readl(XUARTPS_CR_OFFSET);
-	xuartps_writel((ctrl & ~XUARTPS_CR_TX_DIS) | XUARTPS_CR_TX_EN,
-		XUARTPS_CR_OFFSET);
+	ctrl = cdns_uart_readl(CDNS_UART_CR_OFFSET);
+	cdns_uart_writel((ctrl & ~CDNS_UART_CR_TX_DIS) | CDNS_UART_CR_TX_EN,
+		CDNS_UART_CR_OFFSET);
 
-	uart_console_write(port, s, count, xuartps_console_putchar);
-	xuartps_console_wait_tx(port);
+	uart_console_write(port, s, count, cdns_uart_console_putchar);
+	cdns_uart_console_wait_tx(port);
 
-	xuartps_writel(ctrl, XUARTPS_CR_OFFSET);
+	cdns_uart_writel(ctrl, CDNS_UART_CR_OFFSET);
 
-	/* restore interrupt state, it seems like there may be a h/w bug
-	 * in that the interrupt enable register should not need to be
-	 * written based on the data sheet
-	 */
-	xuartps_writel(~imr, XUARTPS_IDR_OFFSET);
-	xuartps_writel(imr, XUARTPS_IER_OFFSET);
+	/* restore interrupt state */
+	cdns_uart_writel(imr, CDNS_UART_IER_OFFSET);
 
 	if (locked)
 		spin_unlock_irqrestore(&port->lock, flags);
 }
 
 /**
- * xuartps_console_setup - Initialize the uart to default config
+ * cdns_uart_console_setup - Initialize the uart to default config
  * @co: Console handle
  * @options: Initial settings of uart
  *
- * Returns 0, -ENODEV if no device
- **/
-static int __init xuartps_console_setup(struct console *co, char *options)
+ * Return: 0 on success, negative errno otherwise.
+ */
+static int __init cdns_uart_console_setup(struct console *co, char *options)
 {
-	struct uart_port *port = &xuartps_port[co->index];
+	struct uart_port *port = &cdns_uart_port[co->index];
 	int baud = 9600;
 	int bits = 8;
 	int parity = 'n';
 	int flow = 'n';
 
-	if (co->index < 0 || co->index >= XUARTPS_NR_PORTS)
+	if (co->index < 0 || co->index >= CDNS_UART_NR_PORTS)
 		return -EINVAL;
 
 	if (!port->mapbase) {
@@ -1175,55 +1129,53 @@ static int __init xuartps_console_setup(struct console *co, char *options)
 	return uart_set_options(port, co, baud, parity, bits, flow);
 }
 
-static struct uart_driver xuartps_uart_driver;
+static struct uart_driver cdns_uart_uart_driver;
 
-static struct console xuartps_console = {
-	.name	= XUARTPS_TTY_NAME,
-	.write	= xuartps_console_write,
+static struct console cdns_uart_console = {
+	.name	= CDNS_UART_TTY_NAME,
+	.write	= cdns_uart_console_write,
 	.device	= uart_console_device,
-	.setup	= xuartps_console_setup,
+	.setup	= cdns_uart_console_setup,
 	.flags	= CON_PRINTBUFFER,
 	.index	= -1, /* Specified on the cmdline (e.g. console=ttyPS ) */
-	.data	= &xuartps_uart_driver,
+	.data	= &cdns_uart_uart_driver,
 };
 
 /**
- * xuartps_console_init - Initialization call
+ * cdns_uart_console_init - Initialization call
  *
- * Returns 0 on success, negative error otherwise
- **/
-static int __init xuartps_console_init(void)
+ * Return: 0 on success, negative errno otherwise
+ */
+static int __init cdns_uart_console_init(void)
 {
-	register_console(&xuartps_console);
+	register_console(&cdns_uart_console);
 	return 0;
 }
 
-console_initcall(xuartps_console_init);
+console_initcall(cdns_uart_console_init);
 
 #endif /* CONFIG_SERIAL_XILINX_PS_UART_CONSOLE */
 
-/** Structure Definitions
- */
-static struct uart_driver xuartps_uart_driver = {
-	.owner		= THIS_MODULE,		/* Owner */
-	.driver_name	= XUARTPS_NAME,		/* Driver name */
-	.dev_name	= XUARTPS_TTY_NAME,	/* Node name */
-	.major		= XUARTPS_MAJOR,	/* Major number */
-	.minor		= XUARTPS_MINOR,	/* Minor number */
-	.nr		= XUARTPS_NR_PORTS,	/* Number of UART ports */
+static struct uart_driver cdns_uart_uart_driver = {
+	.owner		= THIS_MODULE,
+	.driver_name	= CDNS_UART_NAME,
+	.dev_name	= CDNS_UART_TTY_NAME,
+	.major		= CDNS_UART_MAJOR,
+	.minor		= CDNS_UART_MINOR,
+	.nr		= CDNS_UART_NR_PORTS,
 #ifdef CONFIG_SERIAL_XILINX_PS_UART_CONSOLE
-	.cons		= &xuartps_console,	/* Console */
+	.cons		= &cdns_uart_console,
 #endif
 };
 
 #ifdef CONFIG_PM_SLEEP
 /**
- * xuartps_suspend - suspend event
+ * cdns_uart_suspend - suspend event
  * @device: Pointer to the device structure
  *
- * Returns 0
+ * Return: 0
  */
-static int xuartps_suspend(struct device *device)
+static int cdns_uart_suspend(struct device *device)
 {
 	struct uart_port *port = dev_get_drvdata(device);
 	struct tty_struct *tty;
@@ -1242,23 +1194,24 @@ static int xuartps_suspend(struct device *device)
 	 * Call the API provided in serial_core.c file which handles
 	 * the suspend.
 	 */
-	uart_suspend_port(&xuartps_uart_driver, port);
+	uart_suspend_port(&cdns_uart_uart_driver, port);
 	if (console_suspend_enabled && !may_wake) {
-		struct xuartps *xuartps = port->private_data;
+		struct cdns_uart *cdns_uart = port->private_data;
 
-		clk_disable(xuartps->refclk);
-		clk_disable(xuartps->aperclk);
+		clk_disable(cdns_uart->uartclk);
+		clk_disable(cdns_uart->pclk);
 	} else {
 		unsigned long flags = 0;
 
 		spin_lock_irqsave(&port->lock, flags);
 		/* Empty the receive FIFO 1st before making changes */
-		while (!(xuartps_readl(XUARTPS_SR_OFFSET) & XUARTPS_SR_RXEMPTY))
-			xuartps_readl(XUARTPS_FIFO_OFFSET);
+		while (!(cdns_uart_readl(CDNS_UART_SR_OFFSET) &
+					CDNS_UART_SR_RXEMPTY))
+			cdns_uart_readl(CDNS_UART_FIFO_OFFSET);
 		/* set RX trigger level to 1 */
-		xuartps_writel(1, XUARTPS_RXWM_OFFSET);
+		cdns_uart_writel(1, CDNS_UART_RXWM_OFFSET);
 		/* disable RX timeout interrups */
-		xuartps_writel(XUARTPS_IXR_TOUT, XUARTPS_IDR_OFFSET);
+		cdns_uart_writel(CDNS_UART_IXR_TOUT, CDNS_UART_IDR_OFFSET);
 		spin_unlock_irqrestore(&port->lock, flags);
 	}
 
@@ -1266,12 +1219,12 @@ static int xuartps_suspend(struct device *device)
 }
 
 /**
- * xuartps_resume - Resume after a previous suspend
+ * cdns_uart_resume - Resume after a previous suspend
  * @device: Pointer to the device structure
  *
- * Returns 0
+ * Return: 0
  */
-static int xuartps_resume(struct device *device)
+static int cdns_uart_resume(struct device *device)
 {
 	struct uart_port *port = dev_get_drvdata(device);
 	unsigned long flags = 0;
@@ -1289,87 +1242,95 @@ static int xuartps_resume(struct device *device)
 	}
 
 	if (console_suspend_enabled && !may_wake) {
-		struct xuartps *xuartps = port->private_data;
+		struct cdns_uart *cdns_uart = port->private_data;
 
-		clk_enable(xuartps->aperclk);
-		clk_enable(xuartps->refclk);
+		clk_enable(cdns_uart->pclk);
+		clk_enable(cdns_uart->uartclk);
 
 		spin_lock_irqsave(&port->lock, flags);
 
 		/* Set TX/RX Reset */
-		xuartps_writel(xuartps_readl(XUARTPS_CR_OFFSET) |
-				(XUARTPS_CR_TXRST | XUARTPS_CR_RXRST),
-				XUARTPS_CR_OFFSET);
-		while (xuartps_readl(XUARTPS_CR_OFFSET) &
-				(XUARTPS_CR_TXRST | XUARTPS_CR_RXRST))
+		ctrl_reg = cdns_uart_readl(CDNS_UART_CR_OFFSET);
+		ctrl_reg |= CDNS_UART_CR_TXRST | CDNS_UART_CR_RXRST;
+		cdns_uart_writel(ctrl_reg, CDNS_UART_CR_OFFSET);
+		while (cdns_uart_readl(CDNS_UART_CR_OFFSET) &
+				(CDNS_UART_CR_TXRST | CDNS_UART_CR_RXRST))
 			cpu_relax();
 
 		/* restore rx timeout value */
-		xuartps_writel(rx_timeout, XUARTPS_RXTOUT_OFFSET);
+		cdns_uart_writel(rx_timeout, CDNS_UART_RXTOUT_OFFSET);
 		/* Enable Tx/Rx */
-		ctrl_reg = xuartps_readl(XUARTPS_CR_OFFSET);
-		xuartps_writel(
-			(ctrl_reg & ~(XUARTPS_CR_TX_DIS | XUARTPS_CR_RX_DIS)) |
-			(XUARTPS_CR_TX_EN | XUARTPS_CR_RX_EN),
-			XUARTPS_CR_OFFSET);
+		ctrl_reg = cdns_uart_readl(CDNS_UART_CR_OFFSET);
+		ctrl_reg &= ~(CDNS_UART_CR_TX_DIS | CDNS_UART_CR_RX_DIS);
+		ctrl_reg |= CDNS_UART_CR_TX_EN | CDNS_UART_CR_RX_EN;
+		cdns_uart_writel(ctrl_reg, CDNS_UART_CR_OFFSET);
 
 		spin_unlock_irqrestore(&port->lock, flags);
 	} else {
 		spin_lock_irqsave(&port->lock, flags);
 		/* restore original rx trigger level */
-		xuartps_writel(rx_trigger_level, XUARTPS_RXWM_OFFSET);
+		cdns_uart_writel(rx_trigger_level, CDNS_UART_RXWM_OFFSET);
 		/* enable RX timeout interrupt */
-		xuartps_writel(XUARTPS_IXR_TOUT, XUARTPS_IER_OFFSET);
+		cdns_uart_writel(CDNS_UART_IXR_TOUT, CDNS_UART_IER_OFFSET);
 		spin_unlock_irqrestore(&port->lock, flags);
 	}
 
-	return uart_resume_port(&xuartps_uart_driver, port);
+	return uart_resume_port(&cdns_uart_uart_driver, port);
 }
 #endif /* ! CONFIG_PM_SLEEP */
 
-static SIMPLE_DEV_PM_OPS(xuartps_dev_pm_ops, xuartps_suspend, xuartps_resume);
+static SIMPLE_DEV_PM_OPS(cdns_uart_dev_pm_ops, cdns_uart_suspend,
+		cdns_uart_resume);
 
-/* ---------------------------------------------------------------------
- * Platform bus binding
- */
 /**
- * xuartps_probe - Platform driver probe
+ * cdns_uart_probe - Platform driver probe
  * @pdev: Pointer to the platform device structure
  *
- * Returns 0 on success, negative error otherwise
- **/
-static int xuartps_probe(struct platform_device *pdev)
+ * Return: 0 on success, negative errno otherwise
+ */
+static int cdns_uart_probe(struct platform_device *pdev)
 {
-	int rc;
+	int rc, id;
 	struct uart_port *port;
 	struct resource *res, *res2;
-	struct xuartps *xuartps_data;
+	struct cdns_uart *cdns_uart_data;
 
-	xuartps_data = devm_kzalloc(&pdev->dev, sizeof(*xuartps_data),
+	cdns_uart_data = devm_kzalloc(&pdev->dev, sizeof(*cdns_uart_data),
 			GFP_KERNEL);
-	if (!xuartps_data)
+	if (!cdns_uart_data)
 		return -ENOMEM;
 
-	xuartps_data->aperclk = devm_clk_get(&pdev->dev, "aper_clk");
-	if (IS_ERR(xuartps_data->aperclk)) {
-		dev_err(&pdev->dev, "aper_clk clock not found.\n");
-		return PTR_ERR(xuartps_data->aperclk);
+	cdns_uart_data->pclk = devm_clk_get(&pdev->dev, "pclk");
+	if (IS_ERR(cdns_uart_data->pclk)) {
+		cdns_uart_data->pclk = devm_clk_get(&pdev->dev, "aper_clk");
+		if (!IS_ERR(cdns_uart_data->pclk))
+			dev_err(&pdev->dev, "clock name 'aper_clk' is deprecated.\n");
 	}
-	xuartps_data->refclk = devm_clk_get(&pdev->dev, "ref_clk");
-	if (IS_ERR(xuartps_data->refclk)) {
-		dev_err(&pdev->dev, "ref_clk clock not found.\n");
-		return PTR_ERR(xuartps_data->refclk);
+	if (IS_ERR(cdns_uart_data->pclk)) {
+		dev_err(&pdev->dev, "pclk clock not found.\n");
+		return PTR_ERR(cdns_uart_data->pclk);
 	}
 
-	rc = clk_prepare_enable(xuartps_data->aperclk);
+	cdns_uart_data->uartclk = devm_clk_get(&pdev->dev, "uart_clk");
+	if (IS_ERR(cdns_uart_data->uartclk)) {
+		cdns_uart_data->uartclk = devm_clk_get(&pdev->dev, "ref_clk");
+		if (!IS_ERR(cdns_uart_data->uartclk))
+			dev_err(&pdev->dev, "clock name 'ref_clk' is deprecated.\n");
+	}
+	if (IS_ERR(cdns_uart_data->uartclk)) {
+		dev_err(&pdev->dev, "uart_clk clock not found.\n");
+		return PTR_ERR(cdns_uart_data->uartclk);
+	}
+
+	rc = clk_prepare_enable(cdns_uart_data->pclk);
 	if (rc) {
-		dev_err(&pdev->dev, "Unable to enable APER clock.\n");
+		dev_err(&pdev->dev, "Unable to enable pclk clock.\n");
 		return rc;
 	}
-	rc = clk_prepare_enable(xuartps_data->refclk);
+	rc = clk_prepare_enable(cdns_uart_data->uartclk);
 	if (rc) {
 		dev_err(&pdev->dev, "Unable to enable device clock.\n");
-		goto err_out_clk_dis_aper;
+		goto err_out_clk_dis_pclk;
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -1385,15 +1346,19 @@ static int xuartps_probe(struct platform_device *pdev)
 	}
 
 #ifdef CONFIG_COMMON_CLK
-	xuartps_data->clk_rate_change_nb.notifier_call =
-			xuartps_clk_notifier_cb;
-	if (clk_notifier_register(xuartps_data->refclk,
-				&xuartps_data->clk_rate_change_nb))
+	cdns_uart_data->clk_rate_change_nb.notifier_call =
+			cdns_uart_clk_notifier_cb;
+	if (clk_notifier_register(cdns_uart_data->uartclk,
+				&cdns_uart_data->clk_rate_change_nb))
 		dev_warn(&pdev->dev, "Unable to register clock notifier.\n");
 #endif
+	/* Look for a serialN alias */
+	id = of_alias_get_id(pdev->dev.of_node, "serial");
+	if (id < 0)
+		id = 0;
 
 	/* Initialize the port structure */
-	port = xuartps_get_port();
+	port = cdns_uart_get_port(id);
 
 	if (!port) {
 		dev_err(&pdev->dev, "Cannot get uart_port structure\n");
@@ -1407,11 +1372,11 @@ static int xuartps_probe(struct platform_device *pdev)
 		port->mapbase = res->start;
 		port->irq = res2->start;
 		port->dev = &pdev->dev;
-		port->uartclk = clk_get_rate(xuartps_data->refclk);
-		port->private_data = xuartps_data;
-		xuartps_data->port = port;
+		port->uartclk = clk_get_rate(cdns_uart_data->uartclk);
+		port->private_data = cdns_uart_data;
+		cdns_uart_data->port = port;
 		platform_set_drvdata(pdev, port);
-		rc = uart_add_one_port(&xuartps_uart_driver, port);
+		rc = uart_add_one_port(&cdns_uart_uart_driver, port);
 		if (rc) {
 			dev_err(&pdev->dev,
 				"uart_add_one_port() failed; err=%i\n", rc);
@@ -1422,103 +1387,89 @@ static int xuartps_probe(struct platform_device *pdev)
 
 err_out_notif_unreg:
 #ifdef CONFIG_COMMON_CLK
-	clk_notifier_unregister(xuartps_data->refclk,
-			&xuartps_data->clk_rate_change_nb);
+	clk_notifier_unregister(cdns_uart_data->uartclk,
+			&cdns_uart_data->clk_rate_change_nb);
 #endif
 err_out_clk_disable:
-	clk_disable_unprepare(xuartps_data->refclk);
-err_out_clk_dis_aper:
-	clk_disable_unprepare(xuartps_data->aperclk);
+	clk_disable_unprepare(cdns_uart_data->uartclk);
+err_out_clk_dis_pclk:
+	clk_disable_unprepare(cdns_uart_data->pclk);
 
 	return rc;
 }
 
 /**
- * xuartps_remove - called when the platform driver is unregistered
+ * cdns_uart_remove - called when the platform driver is unregistered
  * @pdev: Pointer to the platform device structure
  *
- * Returns 0 on success, negative error otherwise
- **/
-static int xuartps_remove(struct platform_device *pdev)
+ * Return: 0 on success, negative errno otherwise
+ */
+static int cdns_uart_remove(struct platform_device *pdev)
 {
 	struct uart_port *port = platform_get_drvdata(pdev);
-	struct xuartps *xuartps_data = port->private_data;
+	struct cdns_uart *cdns_uart_data = port->private_data;
 	int rc;
 
-	/* Remove the xuartps port from the serial core */
+	/* Remove the cdns_uart port from the serial core */
 #ifdef CONFIG_COMMON_CLK
-	clk_notifier_unregister(xuartps_data->refclk,
-			&xuartps_data->clk_rate_change_nb);
+	clk_notifier_unregister(cdns_uart_data->uartclk,
+			&cdns_uart_data->clk_rate_change_nb);
 #endif
-	rc = uart_remove_one_port(&xuartps_uart_driver, port);
+	rc = uart_remove_one_port(&cdns_uart_uart_driver, port);
 	port->mapbase = 0;
-	clk_disable_unprepare(xuartps_data->refclk);
-	clk_disable_unprepare(xuartps_data->aperclk);
+	clk_disable_unprepare(cdns_uart_data->uartclk);
+	clk_disable_unprepare(cdns_uart_data->pclk);
 	return rc;
 }
 
 /* Match table for of_platform binding */
-static struct of_device_id xuartps_of_match[] = {
+static struct of_device_id cdns_uart_of_match[] = {
 	{ .compatible = "xlnx,xuartps", },
+	{ .compatible = "cdns,uart-r1p8", },
 	{}
 };
-MODULE_DEVICE_TABLE(of, xuartps_of_match);
+MODULE_DEVICE_TABLE(of, cdns_uart_of_match);
 
-static struct platform_driver xuartps_platform_driver = {
-	.probe   = xuartps_probe,		/* Probe method */
-	.remove  = xuartps_remove,		/* Detach method */
+static struct platform_driver cdns_uart_platform_driver = {
+	.probe   = cdns_uart_probe,
+	.remove  = cdns_uart_remove,
 	.driver  = {
 		.owner = THIS_MODULE,
-		.name = XUARTPS_NAME,		/* Driver name */
-		.of_match_table = xuartps_of_match,
-		.pm = &xuartps_dev_pm_ops,
+		.name = CDNS_UART_NAME,
+		.of_match_table = cdns_uart_of_match,
+		.pm = &cdns_uart_dev_pm_ops,
 		},
 };
 
-/* ---------------------------------------------------------------------
- * Module Init and Exit
- */
-/**
- * xuartps_init - Initial driver registration call
- *
- * Returns whether the registration was successful or not
- **/
-static int __init xuartps_init(void)
+static int __init cdns_uart_init(void)
 {
 	int retval = 0;
 
-	/* Register the xuartps driver with the serial core */
-	retval = uart_register_driver(&xuartps_uart_driver);
+	/* Register the cdns_uart driver with the serial core */
+	retval = uart_register_driver(&cdns_uart_uart_driver);
 	if (retval)
 		return retval;
 
 	/* Register the platform driver */
-	retval = platform_driver_register(&xuartps_platform_driver);
+	retval = platform_driver_register(&cdns_uart_platform_driver);
 	if (retval)
-		uart_unregister_driver(&xuartps_uart_driver);
+		uart_unregister_driver(&cdns_uart_uart_driver);
 
 	return retval;
 }
 
-/**
- * xuartps_exit - Driver unregistration call
- **/
-static void __exit xuartps_exit(void)
+static void __exit cdns_uart_exit(void)
 {
-	/* The order of unregistration is important. Unregister the
-	 * UART driver before the platform driver crashes the system.
-	 */
-
 	/* Unregister the platform driver */
-	platform_driver_unregister(&xuartps_platform_driver);
+	platform_driver_unregister(&cdns_uart_platform_driver);
 
-	/* Unregister the xuartps driver */
-	uart_unregister_driver(&xuartps_uart_driver);
+	/* Unregister the cdns_uart driver */
+	uart_unregister_driver(&cdns_uart_uart_driver);
 }
 
-module_init(xuartps_init);
-module_exit(xuartps_exit);
+module_init(cdns_uart_init);
+module_exit(cdns_uart_exit);
 
-MODULE_DESCRIPTION("Driver for PS UART");
+MODULE_DESCRIPTION("Driver for Cadence UART");
 MODULE_AUTHOR("Xilinx Inc.");
 MODULE_LICENSE("GPL");
diff --git a/drivers/tty/sysrq.c b/drivers/tty/sysrq.c
index ce396ecdf41..454b65898e2 100644
--- a/drivers/tty/sysrq.c
+++ b/drivers/tty/sysrq.c
@@ -46,6 +46,7 @@
 #include <linux/jiffies.h>
 #include <linux/syscalls.h>
 #include <linux/of.h>
+#include <linux/rcupdate.h>
 
 #include <asm/ptrace.h>
 #include <asm/irq_regs.h>
@@ -88,7 +89,7 @@ static void sysrq_handle_loglevel(int key)
 	int i;
 
 	i = key - '0';
-	console_loglevel = 7;
+	console_loglevel = CONSOLE_LOGLEVEL_DEFAULT;
 	printk("Loglevel set to %d\n", i);
 	console_loglevel = i;
 }
@@ -343,7 +344,7 @@ static void send_sig_all(int sig)
 static void sysrq_handle_term(int key)
 {
 	send_sig_all(SIGTERM);
-	console_loglevel = 8;
+	console_loglevel = CONSOLE_LOGLEVEL_DEBUG;
 }
 static struct sysrq_key_op sysrq_term_op = {
 	.handler	= sysrq_handle_term,
@@ -387,7 +388,7 @@ static struct sysrq_key_op sysrq_thaw_op = {
 static void sysrq_handle_kill(int key)
 {
 	send_sig_all(SIGKILL);
-	console_loglevel = 8;
+	console_loglevel = CONSOLE_LOGLEVEL_DEBUG;
 }
 static struct sysrq_key_op sysrq_kill_op = {
 	.handler	= sysrq_handle_kill,
@@ -510,9 +511,9 @@ void __handle_sysrq(int key, bool check_mask)
 	struct sysrq_key_op *op_p;
 	int orig_log_level;
 	int i;
-	unsigned long flags;
 
-	spin_lock_irqsave(&sysrq_key_table_lock, flags);
+	rcu_sysrq_start();
+	rcu_read_lock();
 	/*
 	 * Raise the apparent loglevel to maximum so that the sysrq header
 	 * is shown to provide the user with positive feedback.  We do not
@@ -520,7 +521,7 @@ void __handle_sysrq(int key, bool check_mask)
 	 * routing in the consumers of /proc/kmsg.
 	 */
 	orig_log_level = console_loglevel;
-	console_loglevel = 7;
+	console_loglevel = CONSOLE_LOGLEVEL_DEFAULT;
 	printk(KERN_INFO "SysRq : ");
 
         op_p = __sysrq_get_key_op(key);
@@ -554,7 +555,8 @@ void __handle_sysrq(int key, bool check_mask)
 		printk("\n");
 		console_loglevel = orig_log_level;
 	}
-	spin_unlock_irqrestore(&sysrq_key_table_lock, flags);
+	rcu_read_unlock();
+	rcu_sysrq_end();
 }
 
 void handle_sysrq(int key)
@@ -1043,16 +1045,23 @@ static int __sysrq_swap_key_ops(int key, struct sysrq_key_op *insert_op_p,
                                 struct sysrq_key_op *remove_op_p)
 {
 	int retval;
-	unsigned long flags;
 
-	spin_lock_irqsave(&sysrq_key_table_lock, flags);
+	spin_lock(&sysrq_key_table_lock);
 	if (__sysrq_get_key_op(key) == remove_op_p) {
 		__sysrq_put_key_op(key, insert_op_p);
 		retval = 0;
 	} else {
 		retval = -1;
 	}
-	spin_unlock_irqrestore(&sysrq_key_table_lock, flags);
+	spin_unlock(&sysrq_key_table_lock);
+
+	/*
+	 * A concurrent __handle_sysrq either got the old op or the new op.
+	 * Wait for it to go away before returning, so the code for an old
+	 * op is not freed (eg. on module unload) while it is in use.
+	 */
+	synchronize_rcu();
+
 	return retval;
 }
 
diff --git a/drivers/tty/tty_buffer.c b/drivers/tty/tty_buffer.c
index cf78d1985cd..143deb62467 100644
--- a/drivers/tty/tty_buffer.c
+++ b/drivers/tty/tty_buffer.c
@@ -60,6 +60,7 @@ void tty_buffer_lock_exclusive(struct tty_port *port)
 	atomic_inc(&buf->priority);
 	mutex_lock(&buf->lock);
 }
+EXPORT_SYMBOL_GPL(tty_buffer_lock_exclusive);
 
 void tty_buffer_unlock_exclusive(struct tty_port *port)
 {
@@ -73,6 +74,7 @@ void tty_buffer_unlock_exclusive(struct tty_port *port)
 	if (restart)
 		queue_work(system_unbound_wq, &buf->work);
 }
+EXPORT_SYMBOL_GPL(tty_buffer_unlock_exclusive);
 
 /**
  *	tty_buffer_space_avail	-	return unused buffer space
diff --git a/drivers/tty/vt/consolemap.c b/drivers/tty/vt/consolemap.c
index 2978ca596a7..610b720d3b9 100644
--- a/drivers/tty/vt/consolemap.c
+++ b/drivers/tty/vt/consolemap.c
@@ -179,7 +179,6 @@ struct uni_pagedir {
 	unsigned long	sum;
 	unsigned char	*inverse_translations[4];
 	u16		*inverse_trans_unicode;
-	int		readonly;
 };
 
 static struct uni_pagedir *dflt;
@@ -262,7 +261,7 @@ u16 inverse_translate(struct vc_data *conp, int glyph, int use_unicode)
 	int m;
 	if (glyph < 0 || glyph >= MAX_GLYPH)
 		return 0;
-	else if (!(p = (struct uni_pagedir *)*conp->vc_uni_pagedir_loc))
+	else if (!(p = *conp->vc_uni_pagedir_loc))
 		return glyph;
 	else if (use_unicode) {
 		if (!p->inverse_trans_unicode)
@@ -287,7 +286,7 @@ static void update_user_maps(void)
 	for (i = 0; i < MAX_NR_CONSOLES; i++) {
 		if (!vc_cons_allocated(i))
 			continue;
-		p = (struct uni_pagedir *)*vc_cons[i].d->vc_uni_pagedir_loc;
+		p = *vc_cons[i].d->vc_uni_pagedir_loc;
 		if (p && p != q) {
 			set_inverse_transl(vc_cons[i].d, p, USER_MAP);
 			set_inverse_trans_unicode(vc_cons[i].d, p);
@@ -418,10 +417,10 @@ void con_free_unimap(struct vc_data *vc)
 {
 	struct uni_pagedir *p;
 
-	p = (struct uni_pagedir *)*vc->vc_uni_pagedir_loc;
+	p = *vc->vc_uni_pagedir_loc;
 	if (!p)
 		return;
-	*vc->vc_uni_pagedir_loc = 0;
+	*vc->vc_uni_pagedir_loc = NULL;
 	if (--p->refcount)
 		return;
 	con_release_unimap(p);
@@ -436,7 +435,7 @@ static int con_unify_unimap(struct vc_data *conp, struct uni_pagedir *p)
 	for (i = 0; i < MAX_NR_CONSOLES; i++) {
 		if (!vc_cons_allocated(i))
 			continue;
-		q = (struct uni_pagedir *)*vc_cons[i].d->vc_uni_pagedir_loc;
+		q = *vc_cons[i].d->vc_uni_pagedir_loc;
 		if (!q || q == p || q->sum != p->sum)
 			continue;
 		for (j = 0; j < 32; j++) {
@@ -459,7 +458,7 @@ static int con_unify_unimap(struct vc_data *conp, struct uni_pagedir *p)
 		}
 		if (j == 32) {
 			q->refcount++;
-			*conp->vc_uni_pagedir_loc = (unsigned long)q;
+			*conp->vc_uni_pagedir_loc = q;
 			con_release_unimap(p);
 			kfree(p);
 			return 1;
@@ -500,10 +499,7 @@ static int con_do_clear_unimap(struct vc_data *vc, struct unimapinit *ui)
 {
 	struct uni_pagedir *p, *q;
 
-	p = (struct uni_pagedir *)*vc->vc_uni_pagedir_loc;
-	if (p && p->readonly)
-		return -EIO;
-
+	p = *vc->vc_uni_pagedir_loc;
 	if (!p || --p->refcount) {
 		q = kzalloc(sizeof(*p), GFP_KERNEL);
 		if (!q) {
@@ -512,7 +508,7 @@ static int con_do_clear_unimap(struct vc_data *vc, struct unimapinit *ui)
 			return -ENOMEM;
 		}
 		q->refcount=1;
-		*vc->vc_uni_pagedir_loc = (unsigned long)q;
+		*vc->vc_uni_pagedir_loc = q;
 	} else {
 		if (p == dflt) dflt = NULL;
 		p->refcount++;
@@ -536,19 +532,13 @@ int con_set_unimap(struct vc_data *vc, ushort ct, struct unipair __user *list)
 	int err = 0, err1, i;
 	struct uni_pagedir *p, *q;
 
+	if (!ct)
+		return 0;
+
 	console_lock();
 
 	/* Save original vc_unipagdir_loc in case we allocate a new one */
-	p = (struct uni_pagedir *)*vc->vc_uni_pagedir_loc;
-	if (p->readonly) {
-		console_unlock();
-		return -EIO;
-	}
-	
-	if (!ct) {
-		console_unlock();
-		return 0;
-	}
+	p = *vc->vc_uni_pagedir_loc;
 	
 	if (p->refcount > 1) {
 		int j, k;
@@ -564,7 +554,7 @@ int con_set_unimap(struct vc_data *vc, ushort ct, struct unipair __user *list)
 		 * Since refcount was > 1, con_clear_unimap() allocated a
 		 * a new uni_pagedir for this vc.  Re: p != q
 		 */
-		q = (struct uni_pagedir *)*vc->vc_uni_pagedir_loc;
+		q = *vc->vc_uni_pagedir_loc;
 
 		/*
 		 * uni_pgdir is a 32*32*64 table with rows allocated
@@ -586,7 +576,7 @@ int con_set_unimap(struct vc_data *vc, ushort ct, struct unipair __user *list)
 					err1 = con_insert_unipair(q, l, p2[k]);
 					if (err1) {
 						p->refcount++;
-						*vc->vc_uni_pagedir_loc = (unsigned long)p;
+						*vc->vc_uni_pagedir_loc = p;
 						con_release_unimap(q);
 						kfree(q);
 						console_unlock();
@@ -655,12 +645,12 @@ int con_set_default_unimap(struct vc_data *vc)
 	struct uni_pagedir *p;
 
 	if (dflt) {
-		p = (struct uni_pagedir *)*vc->vc_uni_pagedir_loc;
+		p = *vc->vc_uni_pagedir_loc;
 		if (p == dflt)
 			return 0;
 
 		dflt->refcount++;
-		*vc->vc_uni_pagedir_loc = (unsigned long)dflt;
+		*vc->vc_uni_pagedir_loc = dflt;
 		if (p && !--p->refcount) {
 			con_release_unimap(p);
 			kfree(p);
@@ -674,7 +664,7 @@ int con_set_default_unimap(struct vc_data *vc)
 	if (err)
 		return err;
     
-	p = (struct uni_pagedir *)*vc->vc_uni_pagedir_loc;
+	p = *vc->vc_uni_pagedir_loc;
 	q = dfont_unitable;
 	
 	for (i = 0; i < 256; i++)
@@ -685,7 +675,7 @@ int con_set_default_unimap(struct vc_data *vc)
 		}
 			
 	if (con_unify_unimap(vc, p)) {
-		dflt = (struct uni_pagedir *)*vc->vc_uni_pagedir_loc;
+		dflt = *vc->vc_uni_pagedir_loc;
 		return err;
 	}
 
@@ -713,9 +703,9 @@ int con_copy_unimap(struct vc_data *dst_vc, struct vc_data *src_vc)
 	if (*dst_vc->vc_uni_pagedir_loc == *src_vc->vc_uni_pagedir_loc)
 		return 0;
 	con_free_unimap(dst_vc);
-	q = (struct uni_pagedir *)*src_vc->vc_uni_pagedir_loc;
+	q = *src_vc->vc_uni_pagedir_loc;
 	q->refcount++;
-	*dst_vc->vc_uni_pagedir_loc = (long)q;
+	*dst_vc->vc_uni_pagedir_loc = q;
 	return 0;
 }
 EXPORT_SYMBOL(con_copy_unimap);
@@ -737,7 +727,7 @@ int con_get_unimap(struct vc_data *vc, ushort ct, ushort __user *uct, struct uni
 
 	ect = 0;
 	if (*vc->vc_uni_pagedir_loc) {
-		p = (struct uni_pagedir *)*vc->vc_uni_pagedir_loc;
+		p = *vc->vc_uni_pagedir_loc;
 		for (i = 0; i < 32; i++)
 		if ((p1 = p->uni_pgdir[i]))
 			for (j = 0; j < 32; j++)
@@ -810,7 +800,7 @@ conv_uni_to_pc(struct vc_data *conp, long ucs)
 	if (!*conp->vc_uni_pagedir_loc)
 		return -3;
 
-	p = (struct uni_pagedir *)*conp->vc_uni_pagedir_loc;  
+	p = *conp->vc_uni_pagedir_loc;
 	if ((p1 = p->uni_pgdir[ucs >> 11]) &&
 	    (p2 = p1[(ucs >> 6) & 0x1f]) &&
 	    (h = p2[ucs & 0x3f]) < MAX_GLYPH)
diff --git a/drivers/tty/vt/vt.c b/drivers/tty/vt/vt.c
index 3ad0b61e35b..5e0f6ff2e2f 100644
--- a/drivers/tty/vt/vt.c
+++ b/drivers/tty/vt/vt.c
@@ -735,7 +735,7 @@ static void visual_init(struct vc_data *vc, int num, int init)
 	vc->vc_num = num;
 	vc->vc_display_fg = &master_display_fg;
 	vc->vc_uni_pagedir_loc = &vc->vc_uni_pagedir;
-	vc->vc_uni_pagedir = 0;
+	vc->vc_uni_pagedir = NULL;
 	vc->vc_hi_font_mask = 0;
 	vc->vc_complement_mask = 0;
 	vc->vc_can_do_color = 0;
@@ -1231,6 +1231,52 @@ static void default_attr(struct vc_data *vc)
 	vc->vc_color = vc->vc_def_color;
 }
 
+struct rgb { u8 r; u8 g; u8 b; };
+
+struct rgb rgb_from_256(int i)
+{
+	struct rgb c;
+	if (i < 8) {            /* Standard colours. */
+		c.r = i&1 ? 0xaa : 0x00;
+		c.g = i&2 ? 0xaa : 0x00;
+		c.b = i&4 ? 0xaa : 0x00;
+	} else if (i < 16) {
+		c.r = i&1 ? 0xff : 0x55;
+		c.g = i&2 ? 0xff : 0x55;
+		c.b = i&4 ? 0xff : 0x55;
+	} else if (i < 232) {   /* 6x6x6 colour cube. */
+		c.r = (i - 16) / 36 * 85 / 2;
+		c.g = (i - 16) / 6 % 6 * 85 / 2;
+		c.b = (i - 16) % 6 * 85 / 2;
+	} else                  /* Grayscale ramp. */
+		c.r = c.g = c.b = i * 10 - 2312;
+	return c;
+}
+
+static void rgb_foreground(struct vc_data *vc, struct rgb c)
+{
+	u8 hue, max = c.r;
+	if (c.g > max)
+		max = c.g;
+	if (c.b > max)
+		max = c.b;
+	hue = (c.r > max/2 ? 4 : 0)
+	    | (c.g > max/2 ? 2 : 0)
+	    | (c.b > max/2 ? 1 : 0);
+	if (hue == 7 && max <= 0x55)
+		hue = 0, vc->vc_intensity = 2;
+	else
+		vc->vc_intensity = (max > 0xaa) + 1;
+	vc->vc_color = (vc->vc_color & 0xf0) | hue;
+}
+
+static void rgb_background(struct vc_data *vc, struct rgb c)
+{
+	/* For backgrounds, err on the dark side. */
+	vc->vc_color = (vc->vc_color & 0x0f)
+		| (c.r&0x80) >> 1 | (c.g&0x80) >> 2 | (c.b&0x80) >> 3;
+}
+
 /* console_lock is held */
 static void csi_m(struct vc_data *vc)
 {
@@ -1302,8 +1348,7 @@ static void csi_m(struct vc_data *vc)
 			case 27:
 				vc->vc_reverse = 0;
 				break;
-			case 38:
-			case 48: /* ITU T.416
+			case 38: /* ITU T.416
 				  * Higher colour modes.
 				  * They break the usual properties of SGR codes
 				  * and thus need to be detected and ignored by
@@ -1315,15 +1360,41 @@ static void csi_m(struct vc_data *vc)
 				i++;
 				if (i > vc->vc_npar)
 					break;
-				if (vc->vc_par[i] == 5)      /* 256 colours */
-					i++;                 /* ubiquitous */
-				else if (vc->vc_par[i] == 2) /* 24 bit colours */
-					i += 3;              /* extremely rare */
+				if (vc->vc_par[i] == 5 &&  /* 256 colours */
+				    i < vc->vc_npar) {     /* ubiquitous */
+					i++;
+					rgb_foreground(vc,
+						rgb_from_256(vc->vc_par[i]));
+				} else if (vc->vc_par[i] == 2 &&  /* 24 bit */
+				           i <= vc->vc_npar + 3) {/* extremely rare */
+					struct rgb c = {r:vc->vc_par[i+1],
+							g:vc->vc_par[i+2],
+							b:vc->vc_par[i+3]};
+					rgb_foreground(vc, c);
+					i += 3;
+				}
 				/* Subcommands 3 (CMY) and 4 (CMYK) are so insane
-				 * that detecting them is not worth the few extra
-				 * bytes of kernel's size.
+				 * there's no point in supporting them.
 				 */
 				break;
+			case 48:
+				i++;
+				if (i > vc->vc_npar)
+					break;
+				if (vc->vc_par[i] == 5 &&  /* 256 colours */
+				    i < vc->vc_npar) {
+					i++;
+					rgb_background(vc,
+						rgb_from_256(vc->vc_par[i]));
+				} else if (vc->vc_par[i] == 2 && /* 24 bit */
+				           i <= vc->vc_npar + 3) {
+					struct rgb c = {r:vc->vc_par[i+1],
+							g:vc->vc_par[i+2],
+							b:vc->vc_par[i+3]};
+					rgb_background(vc, c);
+					i += 3;
+				}
+				break;
 			case 39:
 				vc->vc_color = (vc->vc_def_color & 0x0f) | (vc->vc_color & 0xf0);
 				break;
diff --git a/drivers/uio/uio.c b/drivers/uio/uio.c
index a673e5b6a2e..e371f5af11f 100644
--- a/drivers/uio/uio.c
+++ b/drivers/uio/uio.c
@@ -655,7 +655,7 @@ static int uio_mmap_physical(struct vm_area_struct *vma)
 
 	if (mem->addr & ~PAGE_MASK)
 		return -ENODEV;
-	if (vma->vm_end - vma->vm_start > mem->size)
+	if (vma->vm_end - vma->vm_start > PAGE_ALIGN(mem->size))
 		return -EINVAL;
 
 	vma->vm_ops = &uio_physical_vm_ops;
diff --git a/drivers/uio/uio_dmem_genirq.c b/drivers/uio/uio_dmem_genirq.c
index 1270f3b2613..8d0bba46956 100644
--- a/drivers/uio/uio_dmem_genirq.c
+++ b/drivers/uio/uio_dmem_genirq.c
@@ -204,7 +204,7 @@ static int uio_dmem_genirq_probe(struct platform_device *pdev)
 		ret = platform_get_irq(pdev, 0);
 		if (ret < 0) {
 			dev_err(&pdev->dev, "failed to get IRQ\n");
-			goto bad0;
+			goto bad1;
 		}
 		uioinfo->irq = ret;
 	}
@@ -275,6 +275,7 @@ static int uio_dmem_genirq_probe(struct platform_device *pdev)
 	ret = uio_register_device(&pdev->dev, priv->uioinfo);
 	if (ret) {
 		dev_err(&pdev->dev, "unable to register uio device\n");
+		pm_runtime_disable(&pdev->dev);
 		goto bad1;
 	}
 
@@ -282,7 +283,6 @@ static int uio_dmem_genirq_probe(struct platform_device *pdev)
 	return 0;
  bad1:
 	kfree(priv);
-	pm_runtime_disable(&pdev->dev);
  bad0:
 	/* kfree uioinfo for OF */
 	if (pdev->dev.of_node)
diff --git a/drivers/usb/Makefile b/drivers/usb/Makefile
index 1ae2bf39d84..3cba892b83a 100644
--- a/drivers/usb/Makefile
+++ b/drivers/usb/Makefile
@@ -28,6 +28,7 @@ obj-$(CONFIG_USB_IMX21_HCD)	+= host/
 obj-$(CONFIG_USB_FSL_MPH_DR_OF)	+= host/
 obj-$(CONFIG_USB_FUSBH200_HCD)	+= host/
 obj-$(CONFIG_USB_FOTG210_HCD)	+= host/
+obj-$(CONFIG_USB_MAX3421_HCD)	+= host/
 
 obj-$(CONFIG_USB_C67X00_HCD)	+= c67x00/
 
@@ -58,4 +59,4 @@ obj-$(CONFIG_USB_CHIPIDEA)	+= chipidea/
 obj-$(CONFIG_USB_RENESAS_USBHS)	+= renesas_usbhs/
 obj-$(CONFIG_USB_GADGET)	+= gadget/
 
-obj-$(CONFIG_USB_COMMON)	+= usb-common.o
+obj-$(CONFIG_USB_COMMON)	+= common/
diff --git a/drivers/usb/chipidea/Makefile b/drivers/usb/chipidea/Makefile
index 480bd4d5710..2f099c7df7b 100644
--- a/drivers/usb/chipidea/Makefile
+++ b/drivers/usb/chipidea/Makefile
@@ -6,6 +6,7 @@ ci_hdrc-y				:= core.o otg.o
 ci_hdrc-$(CONFIG_USB_CHIPIDEA_UDC)	+= udc.o
 ci_hdrc-$(CONFIG_USB_CHIPIDEA_HOST)	+= host.o
 ci_hdrc-$(CONFIG_USB_CHIPIDEA_DEBUG)	+= debug.o
+ci_hdrc-$(CONFIG_USB_OTG_FSM)		+= otg_fsm.o
 
 # Glue/Bridge layers go here
 
diff --git a/drivers/usb/chipidea/bits.h b/drivers/usb/chipidea/bits.h
index 83d06c1455b..ca57e3dcd3d 100644
--- a/drivers/usb/chipidea/bits.h
+++ b/drivers/usb/chipidea/bits.h
@@ -44,9 +44,14 @@
 #define DEVICEADDR_USBADR     (0x7FUL << 25)
 
 /* PORTSC */
+#define PORTSC_CCS            BIT(0)
+#define PORTSC_CSC            BIT(1)
+#define PORTSC_PEC            BIT(3)
+#define PORTSC_OCC            BIT(5)
 #define PORTSC_FPR            BIT(6)
 #define PORTSC_SUSP           BIT(7)
 #define PORTSC_HSP            BIT(9)
+#define PORTSC_PP             BIT(12)
 #define PORTSC_PTC            (0x0FUL << 16)
 #define PORTSC_PHCD(d)	      ((d) ? BIT(22) : BIT(23))
 /* PTS and PTW for non lpm version only */
@@ -56,6 +61,9 @@
 #define PORTSC_PTW            BIT(28)
 #define PORTSC_STS            BIT(29)
 
+#define PORTSC_W1C_BITS						\
+	(PORTSC_CSC | PORTSC_PEC | PORTSC_OCC)
+
 /* DEVLC */
 #define DEVLC_PFSC            BIT(23)
 #define DEVLC_PSPD            (0x03UL << 25)
@@ -72,6 +80,8 @@
 
 /* OTGSC */
 #define OTGSC_IDPU	      BIT(5)
+#define OTGSC_HADP	      BIT(6)
+#define OTGSC_HABA	      BIT(7)
 #define OTGSC_ID	      BIT(8)
 #define OTGSC_AVV	      BIT(9)
 #define OTGSC_ASV	      BIT(10)
diff --git a/drivers/usb/chipidea/ci.h b/drivers/usb/chipidea/ci.h
index e206406ae1d..9563cb56d56 100644
--- a/drivers/usb/chipidea/ci.h
+++ b/drivers/usb/chipidea/ci.h
@@ -17,6 +17,7 @@
 #include <linux/irqreturn.h>
 #include <linux/usb.h>
 #include <linux/usb/gadget.h>
+#include <linux/usb/otg-fsm.h>
 
 /******************************************************************************
  * DEFINE
@@ -139,6 +140,8 @@ struct hw_bank {
  * @roles: array of supported roles for this controller
  * @role: current role
  * @is_otg: if the device is otg-capable
+ * @fsm: otg finite state machine
+ * @fsm_timer: pointer to timer list of otg fsm
  * @work: work for role changing
  * @wq: workqueue thread
  * @qh_pool: allocation pool for queue heads
@@ -174,6 +177,8 @@ struct ci_hdrc {
 	struct ci_role_driver		*roles[CI_ROLE_END];
 	enum ci_role			role;
 	bool				is_otg;
+	struct otg_fsm			fsm;
+	struct ci_otg_fsm_timer_list	*fsm_timer;
 	struct work_struct		work;
 	struct workqueue_struct		*wq;
 
@@ -319,6 +324,24 @@ static inline u32 hw_test_and_write(struct ci_hdrc *ci, enum ci_hw_regs reg,
 	return (val & mask) >> __ffs(mask);
 }
 
+/**
+ * ci_otg_is_fsm_mode: runtime check if otg controller
+ * is in otg fsm mode.
+ */
+static inline bool ci_otg_is_fsm_mode(struct ci_hdrc *ci)
+{
+#ifdef CONFIG_USB_OTG_FSM
+	return ci->is_otg && ci->roles[CI_ROLE_HOST] &&
+					ci->roles[CI_ROLE_GADGET];
+#else
+	return false;
+#endif
+}
+
+u32 hw_read_intr_enable(struct ci_hdrc *ci);
+
+u32 hw_read_intr_status(struct ci_hdrc *ci);
+
 int hw_device_reset(struct ci_hdrc *ci, u32 mode);
 
 int hw_port_test_set(struct ci_hdrc *ci, u8 mode);
diff --git a/drivers/usb/chipidea/ci_hdrc_msm.c b/drivers/usb/chipidea/ci_hdrc_msm.c
index 2d51d852b47..d72b9d2de2c 100644
--- a/drivers/usb/chipidea/ci_hdrc_msm.c
+++ b/drivers/usb/chipidea/ci_hdrc_msm.c
@@ -47,6 +47,7 @@ static void ci_hdrc_msm_notify_event(struct ci_hdrc *ci, unsigned event)
 
 static struct ci_hdrc_platform_data ci_hdrc_msm_platdata = {
 	.name			= "ci_hdrc_msm",
+	.capoffset		= DEF_CAPOFFSET,
 	.flags			= CI_HDRC_REGS_SHARED |
 				  CI_HDRC_REQUIRE_TRANSCEIVER |
 				  CI_HDRC_DISABLE_STREAMING,
@@ -57,9 +58,21 @@ static struct ci_hdrc_platform_data ci_hdrc_msm_platdata = {
 static int ci_hdrc_msm_probe(struct platform_device *pdev)
 {
 	struct platform_device *plat_ci;
+	struct usb_phy *phy;
 
 	dev_dbg(&pdev->dev, "ci_hdrc_msm_probe\n");
 
+	/*
+	 * OTG(PHY) driver takes care of PHY initialization, clock management,
+	 * powering up VBUS, mapping of registers address space and power
+	 * management.
+	 */
+	phy = devm_usb_get_phy_by_phandle(&pdev->dev, "usb-phy", 0);
+	if (IS_ERR(phy))
+		return PTR_ERR(phy);
+
+	ci_hdrc_msm_platdata.phy = phy;
+
 	plat_ci = ci_hdrc_add_device(&pdev->dev,
 				pdev->resource, pdev->num_resources,
 				&ci_hdrc_msm_platdata);
@@ -86,10 +99,19 @@ static int ci_hdrc_msm_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static const struct of_device_id msm_ci_dt_match[] = {
+	{ .compatible = "qcom,ci-hdrc", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, msm_ci_dt_match);
+
 static struct platform_driver ci_hdrc_msm_driver = {
 	.probe = ci_hdrc_msm_probe,
 	.remove = ci_hdrc_msm_remove,
-	.driver = { .name = "msm_hsusb", },
+	.driver = {
+		.name = "msm_hsusb",
+		.of_match_table = msm_ci_dt_match,
+	},
 };
 
 module_platform_driver(ci_hdrc_msm_driver);
diff --git a/drivers/usb/chipidea/core.c b/drivers/usb/chipidea/core.c
index 1cd5d0ba587..619d13e2999 100644
--- a/drivers/usb/chipidea/core.c
+++ b/drivers/usb/chipidea/core.c
@@ -23,7 +23,7 @@
  * - BUS:    bus glue code, bus abstraction layer
  *
  * Compile Options
- * - CONFIG_USB_GADGET_DEBUG_FILES: enable debug facilities
+ * - CONFIG_USB_CHIPIDEA_DEBUG: enable debug facilities
  * - STALL_IN:  non-empty bulk-in pipes cannot be halted
  *              if defined mass storage compliance succeeds but with warnings
  *              => case 4: Hi >  Dn
@@ -42,10 +42,6 @@
  * - Not Supported: 15 & 16 (ISO)
  *
  * TODO List
- * - OTG
- * - Interrupt Traffic
- * - GET_STATUS(device) - always reports 0
- * - Gadget API (majority of optional features)
  * - Suspend & Remote Wakeup
  */
 #include <linux/delay.h>
@@ -74,6 +70,7 @@
 #include "host.h"
 #include "debug.h"
 #include "otg.h"
+#include "otg_fsm.h"
 
 /* Controller register map */
 static const u8 ci_regs_nolpm[] = {
@@ -140,6 +137,26 @@ static int hw_alloc_regmap(struct ci_hdrc *ci, bool is_lpm)
 }
 
 /**
+ * hw_read_intr_enable: returns interrupt enable register
+ *
+ * This function returns register data
+ */
+u32 hw_read_intr_enable(struct ci_hdrc *ci)
+{
+	return hw_read(ci, OP_USBINTR, ~0);
+}
+
+/**
+ * hw_read_intr_status: returns interrupt status register
+ *
+ * This function returns register data
+ */
+u32 hw_read_intr_status(struct ci_hdrc *ci)
+{
+	return hw_read(ci, OP_USBSTS, ~0);
+}
+
+/**
  * hw_port_test_set: writes port test mode (execute without interruption)
  * @mode: new value
  *
@@ -179,11 +196,10 @@ static void ci_hdrc_enter_lpm(struct ci_hdrc *ci, bool enable)
 		hw_write(ci, reg, PORTSC_PHCD(ci->hw_bank.lpm),
 				0);
 		/* 
-		 * The controller needs at least 1ms to reflect
-		 * PHY's status, the PHY also needs some time (less
+		 * the PHY needs some time (less
 		 * than 1ms) to leave low power mode.
 		 */
-		usleep_range(1500, 2000);
+		usleep_range(1000, 1100);
 	}
 }
 
@@ -392,8 +408,14 @@ static irqreturn_t ci_irq(int irq, void *data)
 	irqreturn_t ret = IRQ_NONE;
 	u32 otgsc = 0;
 
-	if (ci->is_otg)
-		otgsc = hw_read(ci, OP_OTGSC, ~0);
+	if (ci->is_otg) {
+		otgsc = hw_read_otgsc(ci, ~0);
+		if (ci_otg_is_fsm_mode(ci)) {
+			ret = ci_otg_fsm_irq(ci);
+			if (ret == IRQ_HANDLED)
+				return ret;
+		}
+	}
 
 	/*
 	 * Handle id change interrupt, it indicates device/host function
@@ -401,9 +423,9 @@ static irqreturn_t ci_irq(int irq, void *data)
 	 */
 	if (ci->is_otg && (otgsc & OTGSC_IDIE) && (otgsc & OTGSC_IDIS)) {
 		ci->id_event = true;
-		ci_clear_otg_interrupt(ci, OTGSC_IDIS);
-		disable_irq_nosync(ci->irq);
-		queue_work(ci->wq, &ci->work);
+		/* Clear ID change irq status */
+		hw_write_otgsc(ci, OTGSC_IDIS, OTGSC_IDIS);
+		ci_otg_queue_work(ci);
 		return IRQ_HANDLED;
 	}
 
@@ -413,9 +435,9 @@ static irqreturn_t ci_irq(int irq, void *data)
 	 */
 	if (ci->is_otg && (otgsc & OTGSC_BSVIE) && (otgsc & OTGSC_BSVIS)) {
 		ci->b_sess_valid_event = true;
-		ci_clear_otg_interrupt(ci, OTGSC_BSVIS);
-		disable_irq_nosync(ci->irq);
-		queue_work(ci->wq, &ci->work);
+		/* Clear BSV irq */
+		hw_write_otgsc(ci, OTGSC_BSVIS, OTGSC_BSVIS);
+		ci_otg_queue_work(ci);
 		return IRQ_HANDLED;
 	}
 
@@ -533,11 +555,8 @@ static void ci_get_otg_capable(struct ci_hdrc *ci)
 		ci->is_otg = (hw_read(ci, CAP_DCCPARAMS,
 				DCCPARAMS_DC | DCCPARAMS_HC)
 					== (DCCPARAMS_DC | DCCPARAMS_HC));
-	if (ci->is_otg) {
+	if (ci->is_otg)
 		dev_dbg(ci->dev, "It is OTG capable controller\n");
-		ci_disable_otg_interrupt(ci, OTGSC_INT_EN_BITS);
-		ci_clear_otg_interrupt(ci, OTGSC_INT_STATUS_BITS);
-	}
 }
 
 static int ci_hdrc_probe(struct platform_device *pdev)
@@ -599,6 +618,13 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 	if (ret) {
 		dev_err(dev, "unable to init phy: %d\n", ret);
 		return ret;
+	} else {
+		/* 
+		 * The delay to sync PHY's status, the maximum delay is
+		 * 2ms since the otgsc uses 1ms timer to debounce the
+		 * PHY's input
+		 */
+		usleep_range(2000, 2500);
 	}
 
 	ci->hw_bank.phys = res->start;
@@ -633,6 +659,9 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 	}
 
 	if (ci->is_otg) {
+		/* Disable and clear all OTG irq */
+		hw_write_otgsc(ci, OTGSC_INT_EN_BITS | OTGSC_INT_STATUS_BITS,
+							OTGSC_INT_STATUS_BITS);
 		ret = ci_hdrc_otg_init(ci);
 		if (ret) {
 			dev_err(dev, "init otg fails, ret = %d\n", ret);
@@ -642,13 +671,9 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 
 	if (ci->roles[CI_ROLE_HOST] && ci->roles[CI_ROLE_GADGET]) {
 		if (ci->is_otg) {
-			/*
-			 * ID pin needs 1ms debouce time,
-			 * we delay 2ms for safe.
-			 */
-			mdelay(2);
 			ci->role = ci_otg_role(ci);
-			ci_enable_otg_interrupt(ci, OTGSC_IDIE);
+			/* Enable ID change irq */
+			hw_write_otgsc(ci, OTGSC_IDIE, OTGSC_IDIE);
 		} else {
 			/*
 			 * If the controller is not OTG capable, but support
@@ -667,10 +692,13 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 	if (ci->role == CI_ROLE_GADGET)
 		ci_handle_vbus_change(ci);
 
-	ret = ci_role_start(ci, ci->role);
-	if (ret) {
-		dev_err(dev, "can't start %s role\n", ci_role(ci)->name);
-		goto stop;
+	if (!ci_otg_is_fsm_mode(ci)) {
+		ret = ci_role_start(ci, ci->role);
+		if (ret) {
+			dev_err(dev, "can't start %s role\n",
+						ci_role(ci)->name);
+			goto stop;
+		}
 	}
 
 	platform_set_drvdata(pdev, ci);
@@ -679,6 +707,9 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 	if (ret)
 		goto stop;
 
+	if (ci_otg_is_fsm_mode(ci))
+		ci_hdrc_otg_fsm_start(ci);
+
 	ret = dbg_create_files(ci);
 	if (!ret)
 		return 0;
@@ -711,6 +742,7 @@ static struct platform_driver ci_hdrc_driver = {
 	.remove	= ci_hdrc_remove,
 	.driver	= {
 		.name	= "ci_hdrc",
+		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/usb/chipidea/debug.c b/drivers/usb/chipidea/debug.c
index 96d899aee47..7cccab6ff30 100644
--- a/drivers/usb/chipidea/debug.c
+++ b/drivers/usb/chipidea/debug.c
@@ -7,11 +7,15 @@
 #include <linux/uaccess.h>
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
+#include <linux/usb/phy.h>
+#include <linux/usb/otg.h>
+#include <linux/usb/otg-fsm.h>
 
 #include "ci.h"
 #include "udc.h"
 #include "bits.h"
 #include "debug.h"
+#include "otg.h"
 
 /**
  * ci_device_show: prints information about device capabilities and status
@@ -204,6 +208,80 @@ static const struct file_operations ci_requests_fops = {
 	.release	= single_release,
 };
 
+int ci_otg_show(struct seq_file *s, void *unused)
+{
+	struct ci_hdrc *ci = s->private;
+	struct otg_fsm *fsm;
+
+	if (!ci || !ci_otg_is_fsm_mode(ci))
+		return 0;
+
+	fsm = &ci->fsm;
+
+	/* ------ State ----- */
+	seq_printf(s, "OTG state: %s\n\n",
+		usb_otg_state_string(ci->transceiver->state));
+
+	/* ------ State Machine Variables ----- */
+	seq_printf(s, "a_bus_drop: %d\n", fsm->a_bus_drop);
+
+	seq_printf(s, "a_bus_req: %d\n", fsm->a_bus_req);
+
+	seq_printf(s, "a_srp_det: %d\n", fsm->a_srp_det);
+
+	seq_printf(s, "a_vbus_vld: %d\n", fsm->a_vbus_vld);
+
+	seq_printf(s, "b_conn: %d\n", fsm->b_conn);
+
+	seq_printf(s, "adp_change: %d\n", fsm->adp_change);
+
+	seq_printf(s, "power_up: %d\n", fsm->power_up);
+
+	seq_printf(s, "a_bus_resume: %d\n", fsm->a_bus_resume);
+
+	seq_printf(s, "a_bus_suspend: %d\n", fsm->a_bus_suspend);
+
+	seq_printf(s, "a_conn: %d\n", fsm->a_conn);
+
+	seq_printf(s, "b_bus_req: %d\n", fsm->b_bus_req);
+
+	seq_printf(s, "b_bus_suspend: %d\n", fsm->b_bus_suspend);
+
+	seq_printf(s, "b_se0_srp: %d\n", fsm->b_se0_srp);
+
+	seq_printf(s, "b_ssend_srp: %d\n", fsm->b_ssend_srp);
+
+	seq_printf(s, "b_sess_vld: %d\n", fsm->b_sess_vld);
+
+	seq_printf(s, "b_srp_done: %d\n", fsm->b_srp_done);
+
+	seq_printf(s, "drv_vbus: %d\n", fsm->drv_vbus);
+
+	seq_printf(s, "loc_conn: %d\n", fsm->loc_conn);
+
+	seq_printf(s, "loc_sof: %d\n", fsm->loc_sof);
+
+	seq_printf(s, "adp_prb: %d\n", fsm->adp_prb);
+
+	seq_printf(s, "id: %d\n", fsm->id);
+
+	seq_printf(s, "protocol: %d\n", fsm->protocol);
+
+	return 0;
+}
+
+static int ci_otg_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ci_otg_show, inode->i_private);
+}
+
+static const struct file_operations ci_otg_fops = {
+	.open			= ci_otg_open,
+	.read			= seq_read,
+	.llseek			= seq_lseek,
+	.release		= single_release,
+};
+
 static int ci_role_show(struct seq_file *s, void *data)
 {
 	struct ci_hdrc *ci = s->private;
@@ -253,6 +331,50 @@ static const struct file_operations ci_role_fops = {
 	.release	= single_release,
 };
 
+int ci_registers_show(struct seq_file *s, void *unused)
+{
+	struct ci_hdrc *ci = s->private;
+	u32 tmp_reg;
+
+	if (!ci)
+		return 0;
+
+	/* ------ Registers ----- */
+	tmp_reg = hw_read_intr_enable(ci);
+	seq_printf(s, "USBINTR reg: %08x\n", tmp_reg);
+
+	tmp_reg = hw_read_intr_status(ci);
+	seq_printf(s, "USBSTS reg: %08x\n", tmp_reg);
+
+	tmp_reg = hw_read(ci, OP_USBMODE, ~0);
+	seq_printf(s, "USBMODE reg: %08x\n", tmp_reg);
+
+	tmp_reg = hw_read(ci, OP_USBCMD, ~0);
+	seq_printf(s, "USBCMD reg: %08x\n", tmp_reg);
+
+	tmp_reg = hw_read(ci, OP_PORTSC, ~0);
+	seq_printf(s, "PORTSC reg: %08x\n", tmp_reg);
+
+	if (ci->is_otg) {
+		tmp_reg = hw_read_otgsc(ci, ~0);
+		seq_printf(s, "OTGSC reg: %08x\n", tmp_reg);
+	}
+
+	return 0;
+}
+
+static int ci_registers_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ci_registers_show, inode->i_private);
+}
+
+static const struct file_operations ci_registers_fops = {
+	.open			= ci_registers_open,
+	.read			= seq_read,
+	.llseek			= seq_lseek,
+	.release		= single_release,
+};
+
 /**
  * dbg_create_files: initializes the attribute interface
  * @ci: device
@@ -287,8 +409,21 @@ int dbg_create_files(struct ci_hdrc *ci)
 	if (!dent)
 		goto err;
 
+	if (ci_otg_is_fsm_mode(ci)) {
+		dent = debugfs_create_file("otg", S_IRUGO, ci->debugfs, ci,
+					&ci_otg_fops);
+		if (!dent)
+			goto err;
+	}
+
 	dent = debugfs_create_file("role", S_IRUGO | S_IWUSR, ci->debugfs, ci,
 				   &ci_role_fops);
+	if (!dent)
+		goto err;
+
+	dent = debugfs_create_file("registers", S_IRUGO, ci->debugfs, ci,
+				&ci_registers_fops);
+
 	if (dent)
 		return 0;
 err:
diff --git a/drivers/usb/chipidea/host.c b/drivers/usb/chipidea/host.c
index a8ac6c16dac..a93d950e946 100644
--- a/drivers/usb/chipidea/host.c
+++ b/drivers/usb/chipidea/host.c
@@ -67,7 +67,11 @@ static int host_start(struct ci_hdrc *ci)
 	ehci->has_tdi_phy_lpm = ci->hw_bank.lpm;
 	ehci->imx28_write_fix = ci->imx28_write_fix;
 
-	if (ci->platdata->reg_vbus) {
+	/*
+	 * vbus is always on if host is not in OTG FSM mode,
+	 * otherwise should be controlled by OTG FSM
+	 */
+	if (ci->platdata->reg_vbus && !ci_otg_is_fsm_mode(ci)) {
 		ret = regulator_enable(ci->platdata->reg_vbus);
 		if (ret) {
 			dev_err(ci->dev,
@@ -78,10 +82,17 @@ static int host_start(struct ci_hdrc *ci)
 	}
 
 	ret = usb_add_hcd(hcd, 0, 0);
-	if (ret)
+	if (ret) {
 		goto disable_reg;
-	else
+	} else {
+		struct usb_otg *otg = ci->transceiver->otg;
+
 		ci->hcd = hcd;
+		if (otg) {
+			otg->host = &hcd->self;
+			hcd->self.otg_port = 1;
+		}
+	}
 
 	if (ci->platdata->flags & CI_HDRC_DISABLE_STREAMING)
 		hw_write(ci, OP_USBMODE, USBMODE_CI_SDIS, USBMODE_CI_SDIS);
@@ -89,7 +100,7 @@ static int host_start(struct ci_hdrc *ci)
 	return ret;
 
 disable_reg:
-	if (ci->platdata->reg_vbus)
+	if (ci->platdata->reg_vbus && !ci_otg_is_fsm_mode(ci))
 		regulator_disable(ci->platdata->reg_vbus);
 
 put_hcd:
@@ -105,7 +116,7 @@ static void host_stop(struct ci_hdrc *ci)
 	if (hcd) {
 		usb_remove_hcd(hcd);
 		usb_put_hcd(hcd);
-		if (ci->platdata->reg_vbus)
+		if (ci->platdata->reg_vbus && !ci_otg_is_fsm_mode(ci))
 			regulator_disable(ci->platdata->reg_vbus);
 	}
 }
diff --git a/drivers/usb/chipidea/otg.c b/drivers/usb/chipidea/otg.c
index 39bd7ec8bf7..a048b08b9d4 100644
--- a/drivers/usb/chipidea/otg.c
+++ b/drivers/usb/chipidea/otg.c
@@ -11,8 +11,8 @@
  */
 
 /*
- * This file mainly handles otgsc register, it may include OTG operation
- * in the future.
+ * This file mainly handles otgsc register, OTG fsm operations for HNP and SRP
+ * are also included.
  */
 
 #include <linux/usb/otg.h>
@@ -22,6 +22,26 @@
 #include "ci.h"
 #include "bits.h"
 #include "otg.h"
+#include "otg_fsm.h"
+
+/**
+ * hw_read_otgsc returns otgsc register bits value.
+ * @mask: bitfield mask
+ */
+u32 hw_read_otgsc(struct ci_hdrc *ci, u32 mask)
+{
+	return hw_read(ci, OP_OTGSC, mask);
+}
+
+/**
+ * hw_write_otgsc updates target bits of OTGSC register.
+ * @mask: bitfield mask
+ * @data: to be written
+ */
+void hw_write_otgsc(struct ci_hdrc *ci, u32 mask, u32 data)
+{
+	hw_write(ci, OP_OTGSC, mask | OTGSC_INT_STATUS_BITS, data);
+}
 
 /**
  * ci_otg_role - pick role based on ID pin state
@@ -29,8 +49,7 @@
  */
 enum ci_role ci_otg_role(struct ci_hdrc *ci)
 {
-	u32 sts = hw_read(ci, OP_OTGSC, ~0);
-	enum ci_role role = sts & OTGSC_ID
+	enum ci_role role = hw_read_otgsc(ci, OTGSC_ID)
 		? CI_ROLE_GADGET
 		: CI_ROLE_HOST;
 
@@ -39,14 +58,10 @@ enum ci_role ci_otg_role(struct ci_hdrc *ci)
 
 void ci_handle_vbus_change(struct ci_hdrc *ci)
 {
-	u32 otgsc;
-
 	if (!ci->is_otg)
 		return;
 
-	otgsc = hw_read(ci, OP_OTGSC, ~0);
-
-	if (otgsc & OTGSC_BSV)
+	if (hw_read_otgsc(ci, OTGSC_BSV))
 		usb_gadget_vbus_connect(&ci->gadget);
 	else
 		usb_gadget_vbus_disconnect(&ci->gadget);
@@ -76,6 +91,11 @@ static void ci_otg_work(struct work_struct *work)
 {
 	struct ci_hdrc *ci = container_of(work, struct ci_hdrc, work);
 
+	if (ci_otg_is_fsm_mode(ci) && !ci_otg_fsm_work(ci)) {
+		enable_irq(ci->irq);
+		return;
+	}
+
 	if (ci->id_event) {
 		ci->id_event = false;
 		ci_handle_id_switch(ci);
@@ -102,6 +122,9 @@ int ci_hdrc_otg_init(struct ci_hdrc *ci)
 		return -ENODEV;
 	}
 
+	if (ci_otg_is_fsm_mode(ci))
+		return ci_hdrc_otg_fsm_init(ci);
+
 	return 0;
 }
 
@@ -115,6 +138,9 @@ void ci_hdrc_otg_destroy(struct ci_hdrc *ci)
 		flush_workqueue(ci->wq);
 		destroy_workqueue(ci->wq);
 	}
-	ci_disable_otg_interrupt(ci, OTGSC_INT_EN_BITS);
-	ci_clear_otg_interrupt(ci, OTGSC_INT_STATUS_BITS);
+	/* Disable all OTG irq and clear status */
+	hw_write_otgsc(ci, OTGSC_INT_EN_BITS | OTGSC_INT_STATUS_BITS,
+						OTGSC_INT_STATUS_BITS);
+	if (ci_otg_is_fsm_mode(ci))
+		ci_hdrc_otg_fsm_remove(ci);
 }
diff --git a/drivers/usb/chipidea/otg.h b/drivers/usb/chipidea/otg.h
index 449bee07f4f..9ecb598e48f 100644
--- a/drivers/usb/chipidea/otg.h
+++ b/drivers/usb/chipidea/otg.h
@@ -11,25 +11,16 @@
 #ifndef __DRIVERS_USB_CHIPIDEA_OTG_H
 #define __DRIVERS_USB_CHIPIDEA_OTG_H
 
-static inline void ci_clear_otg_interrupt(struct ci_hdrc *ci, u32 bits)
-{
-	/* Only clear request bits */
-	hw_write(ci, OP_OTGSC, OTGSC_INT_STATUS_BITS, bits);
-}
-
-static inline void ci_enable_otg_interrupt(struct ci_hdrc *ci, u32 bits)
-{
-	hw_write(ci, OP_OTGSC, bits | OTGSC_INT_STATUS_BITS, bits);
-}
-
-static inline void ci_disable_otg_interrupt(struct ci_hdrc *ci, u32 bits)
-{
-	hw_write(ci, OP_OTGSC, bits | OTGSC_INT_STATUS_BITS, 0);
-}
-
+u32 hw_read_otgsc(struct ci_hdrc *ci, u32 mask);
+void hw_write_otgsc(struct ci_hdrc *ci, u32 mask, u32 data);
 int ci_hdrc_otg_init(struct ci_hdrc *ci);
 void ci_hdrc_otg_destroy(struct ci_hdrc *ci);
 enum ci_role ci_otg_role(struct ci_hdrc *ci);
 void ci_handle_vbus_change(struct ci_hdrc *ci);
+static inline void ci_otg_queue_work(struct ci_hdrc *ci)
+{
+	disable_irq_nosync(ci->irq);
+	queue_work(ci->wq, &ci->work);
+}
 
 #endif /* __DRIVERS_USB_CHIPIDEA_OTG_H */
diff --git a/drivers/usb/chipidea/otg_fsm.c b/drivers/usb/chipidea/otg_fsm.c
new file mode 100644
index 00000000000..caaabc58021
--- /dev/null
+++ b/drivers/usb/chipidea/otg_fsm.c
@@ -0,0 +1,842 @@
+/*
+ * otg_fsm.c - ChipIdea USB IP core OTG FSM driver
+ *
+ * Copyright (C) 2014 Freescale Semiconductor, Inc.
+ *
+ * Author: Jun Li
+ *
+ * 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 file mainly handles OTG fsm, it includes OTG fsm operations
+ * for HNP and SRP.
+ *
+ * TODO List
+ * - ADP
+ * - OTG test device
+ */
+
+#include <linux/usb/otg.h>
+#include <linux/usb/gadget.h>
+#include <linux/usb/hcd.h>
+#include <linux/usb/chipidea.h>
+#include <linux/regulator/consumer.h>
+
+#include "ci.h"
+#include "bits.h"
+#include "otg.h"
+#include "otg_fsm.h"
+
+static struct ci_otg_fsm_timer *otg_timer_initializer
+(struct ci_hdrc *ci, void (*function)(void *, unsigned long),
+			unsigned long expires, unsigned long data)
+{
+	struct ci_otg_fsm_timer *timer;
+
+	timer = devm_kzalloc(ci->dev, sizeof(struct ci_otg_fsm_timer),
+								GFP_KERNEL);
+	if (!timer)
+		return NULL;
+	timer->function = function;
+	timer->expires = expires;
+	timer->data = data;
+	return timer;
+}
+
+/* Add for otg: interact with user space app */
+static ssize_t
+get_a_bus_req(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	char		*next;
+	unsigned	size, t;
+	struct ci_hdrc	*ci = dev_get_drvdata(dev);
+
+	next = buf;
+	size = PAGE_SIZE;
+	t = scnprintf(next, size, "%d\n", ci->fsm.a_bus_req);
+	size -= t;
+	next += t;
+
+	return PAGE_SIZE - size;
+}
+
+static ssize_t
+set_a_bus_req(struct device *dev, struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct ci_hdrc *ci = dev_get_drvdata(dev);
+
+	if (count > 2)
+		return -1;
+
+	mutex_lock(&ci->fsm.lock);
+	if (buf[0] == '0') {
+		ci->fsm.a_bus_req = 0;
+	} else if (buf[0] == '1') {
+		/* If a_bus_drop is TRUE, a_bus_req can't be set */
+		if (ci->fsm.a_bus_drop) {
+			mutex_unlock(&ci->fsm.lock);
+			return count;
+		}
+		ci->fsm.a_bus_req = 1;
+	}
+
+	ci_otg_queue_work(ci);
+	mutex_unlock(&ci->fsm.lock);
+
+	return count;
+}
+static DEVICE_ATTR(a_bus_req, S_IRUGO | S_IWUSR, get_a_bus_req, set_a_bus_req);
+
+static ssize_t
+get_a_bus_drop(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	char		*next;
+	unsigned	size, t;
+	struct ci_hdrc	*ci = dev_get_drvdata(dev);
+
+	next = buf;
+	size = PAGE_SIZE;
+	t = scnprintf(next, size, "%d\n", ci->fsm.a_bus_drop);
+	size -= t;
+	next += t;
+
+	return PAGE_SIZE - size;
+}
+
+static ssize_t
+set_a_bus_drop(struct device *dev, struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct ci_hdrc	*ci = dev_get_drvdata(dev);
+
+	if (count > 2)
+		return -1;
+
+	mutex_lock(&ci->fsm.lock);
+	if (buf[0] == '0') {
+		ci->fsm.a_bus_drop = 0;
+	} else if (buf[0] == '1') {
+		ci->fsm.a_bus_drop = 1;
+		ci->fsm.a_bus_req = 0;
+	}
+
+	ci_otg_queue_work(ci);
+	mutex_unlock(&ci->fsm.lock);
+
+	return count;
+}
+static DEVICE_ATTR(a_bus_drop, S_IRUGO | S_IWUSR, get_a_bus_drop,
+						set_a_bus_drop);
+
+static ssize_t
+get_b_bus_req(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	char		*next;
+	unsigned	size, t;
+	struct ci_hdrc	*ci = dev_get_drvdata(dev);
+
+	next = buf;
+	size = PAGE_SIZE;
+	t = scnprintf(next, size, "%d\n", ci->fsm.b_bus_req);
+	size -= t;
+	next += t;
+
+	return PAGE_SIZE - size;
+}
+
+static ssize_t
+set_b_bus_req(struct device *dev, struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct ci_hdrc	*ci = dev_get_drvdata(dev);
+
+	if (count > 2)
+		return -1;
+
+	mutex_lock(&ci->fsm.lock);
+	if (buf[0] == '0')
+		ci->fsm.b_bus_req = 0;
+	else if (buf[0] == '1')
+		ci->fsm.b_bus_req = 1;
+
+	ci_otg_queue_work(ci);
+	mutex_unlock(&ci->fsm.lock);
+
+	return count;
+}
+static DEVICE_ATTR(b_bus_req, S_IRUGO | S_IWUSR, get_b_bus_req, set_b_bus_req);
+
+static ssize_t
+set_a_clr_err(struct device *dev, struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct ci_hdrc	*ci = dev_get_drvdata(dev);
+
+	if (count > 2)
+		return -1;
+
+	mutex_lock(&ci->fsm.lock);
+	if (buf[0] == '1')
+		ci->fsm.a_clr_err = 1;
+
+	ci_otg_queue_work(ci);
+	mutex_unlock(&ci->fsm.lock);
+
+	return count;
+}
+static DEVICE_ATTR(a_clr_err, S_IWUSR, NULL, set_a_clr_err);
+
+static struct attribute *inputs_attrs[] = {
+	&dev_attr_a_bus_req.attr,
+	&dev_attr_a_bus_drop.attr,
+	&dev_attr_b_bus_req.attr,
+	&dev_attr_a_clr_err.attr,
+	NULL,
+};
+
+static struct attribute_group inputs_attr_group = {
+	.name = "inputs",
+	.attrs = inputs_attrs,
+};
+
+/*
+ * Add timer to active timer list
+ */
+static void ci_otg_add_timer(struct ci_hdrc *ci, enum ci_otg_fsm_timer_index t)
+{
+	struct ci_otg_fsm_timer *tmp_timer;
+	struct ci_otg_fsm_timer *timer = ci->fsm_timer->timer_list[t];
+	struct list_head *active_timers = &ci->fsm_timer->active_timers;
+
+	if (t >= NUM_CI_OTG_FSM_TIMERS)
+		return;
+
+	/*
+	 * Check if the timer is already in the active list,
+	 * if so update timer count
+	 */
+	list_for_each_entry(tmp_timer, active_timers, list)
+		if (tmp_timer == timer) {
+			timer->count = timer->expires;
+			return;
+		}
+
+	timer->count = timer->expires;
+	list_add_tail(&timer->list, active_timers);
+
+	/* Enable 1ms irq */
+	if (!(hw_read_otgsc(ci, OTGSC_1MSIE)))
+		hw_write_otgsc(ci, OTGSC_1MSIE, OTGSC_1MSIE);
+}
+
+/*
+ * Remove timer from active timer list
+ */
+static void ci_otg_del_timer(struct ci_hdrc *ci, enum ci_otg_fsm_timer_index t)
+{
+	struct ci_otg_fsm_timer *tmp_timer, *del_tmp;
+	struct ci_otg_fsm_timer *timer = ci->fsm_timer->timer_list[t];
+	struct list_head *active_timers = &ci->fsm_timer->active_timers;
+
+	if (t >= NUM_CI_OTG_FSM_TIMERS)
+		return;
+
+	list_for_each_entry_safe(tmp_timer, del_tmp, active_timers, list)
+		if (tmp_timer == timer)
+			list_del(&timer->list);
+
+	/* Disable 1ms irq if there is no any active timer */
+	if (list_empty(active_timers))
+		hw_write_otgsc(ci, OTGSC_1MSIE, 0);
+}
+
+/*
+ * Reduce timer count by 1, and find timeout conditions.
+ * Called by otg 1ms timer interrupt
+ */
+static inline int ci_otg_tick_timer(struct ci_hdrc *ci)
+{
+	struct ci_otg_fsm_timer *tmp_timer, *del_tmp;
+	struct list_head *active_timers = &ci->fsm_timer->active_timers;
+	int expired = 0;
+
+	list_for_each_entry_safe(tmp_timer, del_tmp, active_timers, list) {
+		tmp_timer->count--;
+		/* check if timer expires */
+		if (!tmp_timer->count) {
+			list_del(&tmp_timer->list);
+			tmp_timer->function(ci, tmp_timer->data);
+			expired = 1;
+		}
+	}
+
+	/* disable 1ms irq if there is no any timer active */
+	if ((expired == 1) && list_empty(active_timers))
+		hw_write_otgsc(ci, OTGSC_1MSIE, 0);
+
+	return expired;
+}
+
+/* The timeout callback function to set time out bit */
+static void set_tmout(void *ptr, unsigned long indicator)
+{
+	*(int *)indicator = 1;
+}
+
+static void set_tmout_and_fsm(void *ptr, unsigned long indicator)
+{
+	struct ci_hdrc *ci = (struct ci_hdrc *)ptr;
+
+	set_tmout(ci, indicator);
+
+	ci_otg_queue_work(ci);
+}
+
+static void a_wait_vfall_tmout_func(void *ptr, unsigned long indicator)
+{
+	struct ci_hdrc *ci = (struct ci_hdrc *)ptr;
+
+	set_tmout(ci, indicator);
+	/* Disable port power */
+	hw_write(ci, OP_PORTSC, PORTSC_W1C_BITS | PORTSC_PP, 0);
+	/* Clear exsiting DP irq */
+	hw_write_otgsc(ci, OTGSC_DPIS, OTGSC_DPIS);
+	/* Enable data pulse irq */
+	hw_write_otgsc(ci, OTGSC_DPIE, OTGSC_DPIE);
+	ci_otg_queue_work(ci);
+}
+
+static void b_ase0_brst_tmout_func(void *ptr, unsigned long indicator)
+{
+	struct ci_hdrc *ci = (struct ci_hdrc *)ptr;
+
+	set_tmout(ci, indicator);
+	if (!hw_read_otgsc(ci, OTGSC_BSV))
+		ci->fsm.b_sess_vld = 0;
+
+	ci_otg_queue_work(ci);
+}
+
+static void b_ssend_srp_tmout_func(void *ptr, unsigned long indicator)
+{
+	struct ci_hdrc *ci = (struct ci_hdrc *)ptr;
+
+	set_tmout(ci, indicator);
+
+	/* only vbus fall below B_sess_vld in b_idle state */
+	if (ci->transceiver->state == OTG_STATE_B_IDLE)
+		ci_otg_queue_work(ci);
+}
+
+static void b_sess_vld_tmout_func(void *ptr, unsigned long indicator)
+{
+	struct ci_hdrc *ci = (struct ci_hdrc *)ptr;
+
+	/* Check if A detached */
+	if (!(hw_read_otgsc(ci, OTGSC_BSV))) {
+		ci->fsm.b_sess_vld = 0;
+		ci_otg_add_timer(ci, B_SSEND_SRP);
+		ci_otg_queue_work(ci);
+	}
+}
+
+static void b_data_pulse_end(void *ptr, unsigned long indicator)
+{
+	struct ci_hdrc *ci = (struct ci_hdrc *)ptr;
+
+	ci->fsm.b_srp_done = 1;
+	ci->fsm.b_bus_req = 0;
+	if (ci->fsm.power_up)
+		ci->fsm.power_up = 0;
+
+	hw_write_otgsc(ci, OTGSC_HABA, 0);
+
+	ci_otg_queue_work(ci);
+}
+
+/* Initialize timers */
+static int ci_otg_init_timers(struct ci_hdrc *ci)
+{
+	struct otg_fsm *fsm = &ci->fsm;
+
+	/* FSM used timers */
+	ci->fsm_timer->timer_list[A_WAIT_VRISE] =
+		otg_timer_initializer(ci, &set_tmout_and_fsm, TA_WAIT_VRISE,
+			(unsigned long)&fsm->a_wait_vrise_tmout);
+	if (ci->fsm_timer->timer_list[A_WAIT_VRISE] == NULL)
+		return -ENOMEM;
+
+	ci->fsm_timer->timer_list[A_WAIT_VFALL] =
+		otg_timer_initializer(ci, &a_wait_vfall_tmout_func,
+		TA_WAIT_VFALL, (unsigned long)&fsm->a_wait_vfall_tmout);
+	if (ci->fsm_timer->timer_list[A_WAIT_VFALL] == NULL)
+		return -ENOMEM;
+
+	ci->fsm_timer->timer_list[A_WAIT_BCON] =
+		otg_timer_initializer(ci, &set_tmout_and_fsm, TA_WAIT_BCON,
+				(unsigned long)&fsm->a_wait_bcon_tmout);
+	if (ci->fsm_timer->timer_list[A_WAIT_BCON] == NULL)
+		return -ENOMEM;
+
+	ci->fsm_timer->timer_list[A_AIDL_BDIS] =
+		otg_timer_initializer(ci, &set_tmout_and_fsm, TA_AIDL_BDIS,
+				(unsigned long)&fsm->a_aidl_bdis_tmout);
+	if (ci->fsm_timer->timer_list[A_AIDL_BDIS] == NULL)
+		return -ENOMEM;
+
+	ci->fsm_timer->timer_list[A_BIDL_ADIS] =
+		otg_timer_initializer(ci, &set_tmout_and_fsm, TA_BIDL_ADIS,
+				(unsigned long)&fsm->a_bidl_adis_tmout);
+	if (ci->fsm_timer->timer_list[A_BIDL_ADIS] == NULL)
+		return -ENOMEM;
+
+	ci->fsm_timer->timer_list[B_ASE0_BRST] =
+		otg_timer_initializer(ci, &b_ase0_brst_tmout_func, TB_ASE0_BRST,
+					(unsigned long)&fsm->b_ase0_brst_tmout);
+	if (ci->fsm_timer->timer_list[B_ASE0_BRST] == NULL)
+		return -ENOMEM;
+
+	ci->fsm_timer->timer_list[B_SE0_SRP] =
+		otg_timer_initializer(ci, &set_tmout_and_fsm, TB_SE0_SRP,
+					(unsigned long)&fsm->b_se0_srp);
+	if (ci->fsm_timer->timer_list[B_SE0_SRP] == NULL)
+		return -ENOMEM;
+
+	ci->fsm_timer->timer_list[B_SSEND_SRP] =
+		otg_timer_initializer(ci, &b_ssend_srp_tmout_func, TB_SSEND_SRP,
+					(unsigned long)&fsm->b_ssend_srp);
+	if (ci->fsm_timer->timer_list[B_SSEND_SRP] == NULL)
+		return -ENOMEM;
+
+	ci->fsm_timer->timer_list[B_SRP_FAIL] =
+		otg_timer_initializer(ci, &set_tmout, TB_SRP_FAIL,
+				(unsigned long)&fsm->b_srp_done);
+	if (ci->fsm_timer->timer_list[B_SRP_FAIL] == NULL)
+		return -ENOMEM;
+
+	ci->fsm_timer->timer_list[B_DATA_PLS] =
+		otg_timer_initializer(ci, &b_data_pulse_end, TB_DATA_PLS, 0);
+	if (ci->fsm_timer->timer_list[B_DATA_PLS] == NULL)
+		return -ENOMEM;
+
+	ci->fsm_timer->timer_list[B_SESS_VLD] =	otg_timer_initializer(ci,
+					&b_sess_vld_tmout_func, TB_SESS_VLD, 0);
+	if (ci->fsm_timer->timer_list[B_SESS_VLD] == NULL)
+		return -ENOMEM;
+
+	return 0;
+}
+
+/* -------------------------------------------------------------*/
+/* Operations that will be called from OTG Finite State Machine */
+/* -------------------------------------------------------------*/
+static void ci_otg_fsm_add_timer(struct otg_fsm *fsm, enum otg_fsm_timer t)
+{
+	struct ci_hdrc	*ci = container_of(fsm, struct ci_hdrc, fsm);
+
+	if (t < NUM_OTG_FSM_TIMERS)
+		ci_otg_add_timer(ci, t);
+	return;
+}
+
+static void ci_otg_fsm_del_timer(struct otg_fsm *fsm, enum otg_fsm_timer t)
+{
+	struct ci_hdrc	*ci = container_of(fsm, struct ci_hdrc, fsm);
+
+	if (t < NUM_OTG_FSM_TIMERS)
+		ci_otg_del_timer(ci, t);
+	return;
+}
+
+/*
+ * A-device drive vbus: turn on vbus regulator and enable port power
+ * Data pulse irq should be disabled while vbus is on.
+ */
+static void ci_otg_drv_vbus(struct otg_fsm *fsm, int on)
+{
+	int ret;
+	struct ci_hdrc	*ci = container_of(fsm, struct ci_hdrc, fsm);
+
+	if (on) {
+		/* Enable power power */
+		hw_write(ci, OP_PORTSC, PORTSC_W1C_BITS | PORTSC_PP,
+							PORTSC_PP);
+		if (ci->platdata->reg_vbus) {
+			ret = regulator_enable(ci->platdata->reg_vbus);
+			if (ret) {
+				dev_err(ci->dev,
+				"Failed to enable vbus regulator, ret=%d\n",
+				ret);
+				return;
+			}
+		}
+		/* Disable data pulse irq */
+		hw_write_otgsc(ci, OTGSC_DPIE, 0);
+
+		fsm->a_srp_det = 0;
+		fsm->power_up = 0;
+	} else {
+		if (ci->platdata->reg_vbus)
+			regulator_disable(ci->platdata->reg_vbus);
+
+		fsm->a_bus_drop = 1;
+		fsm->a_bus_req = 0;
+	}
+}
+
+/*
+ * Control data line by Run Stop bit.
+ */
+static void ci_otg_loc_conn(struct otg_fsm *fsm, int on)
+{
+	struct ci_hdrc	*ci = container_of(fsm, struct ci_hdrc, fsm);
+
+	if (on)
+		hw_write(ci, OP_USBCMD, USBCMD_RS, USBCMD_RS);
+	else
+		hw_write(ci, OP_USBCMD, USBCMD_RS, 0);
+}
+
+/*
+ * Generate SOF by host.
+ * This is controlled through suspend/resume the port.
+ * In host mode, controller will automatically send SOF.
+ * Suspend will block the data on the port.
+ */
+static void ci_otg_loc_sof(struct otg_fsm *fsm, int on)
+{
+	struct ci_hdrc	*ci = container_of(fsm, struct ci_hdrc, fsm);
+
+	if (on)
+		hw_write(ci, OP_PORTSC, PORTSC_W1C_BITS | PORTSC_FPR,
+							PORTSC_FPR);
+	else
+		hw_write(ci, OP_PORTSC, PORTSC_W1C_BITS | PORTSC_SUSP,
+							PORTSC_SUSP);
+}
+
+/*
+ * Start SRP pulsing by data-line pulsing,
+ * no v-bus pulsing followed
+ */
+static void ci_otg_start_pulse(struct otg_fsm *fsm)
+{
+	struct ci_hdrc	*ci = container_of(fsm, struct ci_hdrc, fsm);
+
+	/* Hardware Assistant Data pulse */
+	hw_write_otgsc(ci, OTGSC_HADP, OTGSC_HADP);
+
+	ci_otg_add_timer(ci, B_DATA_PLS);
+}
+
+static int ci_otg_start_host(struct otg_fsm *fsm, int on)
+{
+	struct ci_hdrc	*ci = container_of(fsm, struct ci_hdrc, fsm);
+
+	mutex_unlock(&fsm->lock);
+	if (on) {
+		ci_role_stop(ci);
+		ci_role_start(ci, CI_ROLE_HOST);
+	} else {
+		ci_role_stop(ci);
+		hw_device_reset(ci, USBMODE_CM_DC);
+		ci_role_start(ci, CI_ROLE_GADGET);
+	}
+	mutex_lock(&fsm->lock);
+	return 0;
+}
+
+static int ci_otg_start_gadget(struct otg_fsm *fsm, int on)
+{
+	struct ci_hdrc	*ci = container_of(fsm, struct ci_hdrc, fsm);
+
+	mutex_unlock(&fsm->lock);
+	if (on)
+		usb_gadget_vbus_connect(&ci->gadget);
+	else
+		usb_gadget_vbus_disconnect(&ci->gadget);
+	mutex_lock(&fsm->lock);
+
+	return 0;
+}
+
+static struct otg_fsm_ops ci_otg_ops = {
+	.drv_vbus = ci_otg_drv_vbus,
+	.loc_conn = ci_otg_loc_conn,
+	.loc_sof = ci_otg_loc_sof,
+	.start_pulse = ci_otg_start_pulse,
+	.add_timer = ci_otg_fsm_add_timer,
+	.del_timer = ci_otg_fsm_del_timer,
+	.start_host = ci_otg_start_host,
+	.start_gadget = ci_otg_start_gadget,
+};
+
+int ci_otg_fsm_work(struct ci_hdrc *ci)
+{
+	/*
+	 * Don't do fsm transition for B device
+	 * when there is no gadget class driver
+	 */
+	if (ci->fsm.id && !(ci->driver) &&
+		ci->transceiver->state < OTG_STATE_A_IDLE)
+		return 0;
+
+	if (otg_statemachine(&ci->fsm)) {
+		if (ci->transceiver->state == OTG_STATE_A_IDLE) {
+			/*
+			 * Further state change for cases:
+			 * a_idle to b_idle; or
+			 * a_idle to a_wait_vrise due to ID change(1->0), so
+			 * B-dev becomes A-dev can try to start new session
+			 * consequently; or
+			 * a_idle to a_wait_vrise when power up
+			 */
+			if ((ci->fsm.id) || (ci->id_event) ||
+						(ci->fsm.power_up))
+				ci_otg_queue_work(ci);
+			if (ci->id_event)
+				ci->id_event = false;
+		} else if (ci->transceiver->state == OTG_STATE_B_IDLE) {
+			if (ci->fsm.b_sess_vld) {
+				ci->fsm.power_up = 0;
+				/*
+				 * Further transite to b_periphearl state
+				 * when register gadget driver with vbus on
+				 */
+				ci_otg_queue_work(ci);
+			}
+		}
+	}
+	return 0;
+}
+
+/*
+ * Update fsm variables in each state if catching expected interrupts,
+ * called by otg fsm isr.
+ */
+static void ci_otg_fsm_event(struct ci_hdrc *ci)
+{
+	u32 intr_sts, otg_bsess_vld, port_conn;
+	struct otg_fsm *fsm = &ci->fsm;
+
+	intr_sts = hw_read_intr_status(ci);
+	otg_bsess_vld = hw_read_otgsc(ci, OTGSC_BSV);
+	port_conn = hw_read(ci, OP_PORTSC, PORTSC_CCS);
+
+	switch (ci->transceiver->state) {
+	case OTG_STATE_A_WAIT_BCON:
+		if (port_conn) {
+			fsm->b_conn = 1;
+			fsm->a_bus_req = 1;
+			ci_otg_queue_work(ci);
+		}
+		break;
+	case OTG_STATE_B_IDLE:
+		if (otg_bsess_vld && (intr_sts & USBi_PCI) && port_conn) {
+			fsm->b_sess_vld = 1;
+			ci_otg_queue_work(ci);
+		}
+		break;
+	case OTG_STATE_B_PERIPHERAL:
+		if ((intr_sts & USBi_SLI) && port_conn && otg_bsess_vld) {
+			fsm->a_bus_suspend = 1;
+			ci_otg_queue_work(ci);
+		} else if (intr_sts & USBi_PCI) {
+			if (fsm->a_bus_suspend == 1)
+				fsm->a_bus_suspend = 0;
+		}
+		break;
+	case OTG_STATE_B_HOST:
+		if ((intr_sts & USBi_PCI) && !port_conn) {
+			fsm->a_conn = 0;
+			fsm->b_bus_req = 0;
+			ci_otg_queue_work(ci);
+			ci_otg_add_timer(ci, B_SESS_VLD);
+		}
+		break;
+	case OTG_STATE_A_PERIPHERAL:
+		if (intr_sts & USBi_SLI) {
+			 fsm->b_bus_suspend = 1;
+			/*
+			 * Init a timer to know how long this suspend
+			 * will contine, if time out, indicates B no longer
+			 * wants to be host role
+			 */
+			 ci_otg_add_timer(ci, A_BIDL_ADIS);
+		}
+
+		if (intr_sts & USBi_URI)
+			ci_otg_del_timer(ci, A_BIDL_ADIS);
+
+		if (intr_sts & USBi_PCI) {
+			if (fsm->b_bus_suspend == 1) {
+				ci_otg_del_timer(ci, A_BIDL_ADIS);
+				fsm->b_bus_suspend = 0;
+			}
+		}
+		break;
+	case OTG_STATE_A_SUSPEND:
+		if ((intr_sts & USBi_PCI) && !port_conn) {
+			fsm->b_conn = 0;
+
+			/* if gadget driver is binded */
+			if (ci->driver) {
+				/* A device to be peripheral mode */
+				ci->gadget.is_a_peripheral = 1;
+			}
+			ci_otg_queue_work(ci);
+		}
+		break;
+	case OTG_STATE_A_HOST:
+		if ((intr_sts & USBi_PCI) && !port_conn) {
+			fsm->b_conn = 0;
+			ci_otg_queue_work(ci);
+		}
+		break;
+	case OTG_STATE_B_WAIT_ACON:
+		if ((intr_sts & USBi_PCI) && port_conn) {
+			fsm->a_conn = 1;
+			ci_otg_queue_work(ci);
+		}
+		break;
+	default:
+		break;
+	}
+}
+
+/*
+ * ci_otg_irq - otg fsm related irq handling
+ * and also update otg fsm variable by monitoring usb host and udc
+ * state change interrupts.
+ * @ci: ci_hdrc
+ */
+irqreturn_t ci_otg_fsm_irq(struct ci_hdrc *ci)
+{
+	irqreturn_t retval =  IRQ_NONE;
+	u32 otgsc, otg_int_src = 0;
+	struct otg_fsm *fsm = &ci->fsm;
+
+	otgsc = hw_read_otgsc(ci, ~0);
+	otg_int_src = otgsc & OTGSC_INT_STATUS_BITS & (otgsc >> 8);
+	fsm->id = (otgsc & OTGSC_ID) ? 1 : 0;
+
+	if (otg_int_src) {
+		if (otg_int_src & OTGSC_1MSIS) {
+			hw_write_otgsc(ci, OTGSC_1MSIS, OTGSC_1MSIS);
+			retval = ci_otg_tick_timer(ci);
+			return IRQ_HANDLED;
+		} else if (otg_int_src & OTGSC_DPIS) {
+			hw_write_otgsc(ci, OTGSC_DPIS, OTGSC_DPIS);
+			fsm->a_srp_det = 1;
+			fsm->a_bus_drop = 0;
+		} else if (otg_int_src & OTGSC_IDIS) {
+			hw_write_otgsc(ci, OTGSC_IDIS, OTGSC_IDIS);
+			if (fsm->id == 0) {
+				fsm->a_bus_drop = 0;
+				fsm->a_bus_req = 1;
+				ci->id_event = true;
+			}
+		} else if (otg_int_src & OTGSC_BSVIS) {
+			hw_write_otgsc(ci, OTGSC_BSVIS, OTGSC_BSVIS);
+			if (otgsc & OTGSC_BSV) {
+				fsm->b_sess_vld = 1;
+				ci_otg_del_timer(ci, B_SSEND_SRP);
+				ci_otg_del_timer(ci, B_SRP_FAIL);
+				fsm->b_ssend_srp = 0;
+			} else {
+				fsm->b_sess_vld = 0;
+				if (fsm->id)
+					ci_otg_add_timer(ci, B_SSEND_SRP);
+			}
+		} else if (otg_int_src & OTGSC_AVVIS) {
+			hw_write_otgsc(ci, OTGSC_AVVIS, OTGSC_AVVIS);
+			if (otgsc & OTGSC_AVV) {
+				fsm->a_vbus_vld = 1;
+			} else {
+				fsm->a_vbus_vld = 0;
+				fsm->b_conn = 0;
+			}
+		}
+		ci_otg_queue_work(ci);
+		return IRQ_HANDLED;
+	}
+
+	ci_otg_fsm_event(ci);
+
+	return retval;
+}
+
+void ci_hdrc_otg_fsm_start(struct ci_hdrc *ci)
+{
+	ci_otg_queue_work(ci);
+}
+
+int ci_hdrc_otg_fsm_init(struct ci_hdrc *ci)
+{
+	int retval = 0;
+	struct usb_otg *otg;
+
+	otg = devm_kzalloc(ci->dev,
+			sizeof(struct usb_otg), GFP_KERNEL);
+	if (!otg) {
+		dev_err(ci->dev,
+		"Failed to allocate usb_otg structure for ci hdrc otg!\n");
+		return -ENOMEM;
+	}
+
+	otg->phy = ci->transceiver;
+	otg->gadget = &ci->gadget;
+	ci->fsm.otg = otg;
+	ci->transceiver->otg = ci->fsm.otg;
+	ci->fsm.power_up = 1;
+	ci->fsm.id = hw_read_otgsc(ci, OTGSC_ID) ? 1 : 0;
+	ci->transceiver->state = OTG_STATE_UNDEFINED;
+	ci->fsm.ops = &ci_otg_ops;
+
+	mutex_init(&ci->fsm.lock);
+
+	ci->fsm_timer = devm_kzalloc(ci->dev,
+			sizeof(struct ci_otg_fsm_timer_list), GFP_KERNEL);
+	if (!ci->fsm_timer) {
+		dev_err(ci->dev,
+		"Failed to allocate timer structure for ci hdrc otg!\n");
+		return -ENOMEM;
+	}
+
+	INIT_LIST_HEAD(&ci->fsm_timer->active_timers);
+	retval = ci_otg_init_timers(ci);
+	if (retval) {
+		dev_err(ci->dev, "Couldn't init OTG timers\n");
+		return retval;
+	}
+
+	retval = sysfs_create_group(&ci->dev->kobj, &inputs_attr_group);
+	if (retval < 0) {
+		dev_dbg(ci->dev,
+			"Can't register sysfs attr group: %d\n", retval);
+		return retval;
+	}
+
+	/* Enable A vbus valid irq */
+	hw_write_otgsc(ci, OTGSC_AVVIE, OTGSC_AVVIE);
+
+	if (ci->fsm.id) {
+		ci->fsm.b_ssend_srp =
+			hw_read_otgsc(ci, OTGSC_BSV) ? 0 : 1;
+		ci->fsm.b_sess_vld =
+			hw_read_otgsc(ci, OTGSC_BSV) ? 1 : 0;
+		/* Enable BSV irq */
+		hw_write_otgsc(ci, OTGSC_BSVIE, OTGSC_BSVIE);
+	}
+
+	return 0;
+}
+
+void ci_hdrc_otg_fsm_remove(struct ci_hdrc *ci)
+{
+	sysfs_remove_group(&ci->dev->kobj, &inputs_attr_group);
+}
diff --git a/drivers/usb/chipidea/otg_fsm.h b/drivers/usb/chipidea/otg_fsm.h
new file mode 100644
index 00000000000..94c085f456a
--- /dev/null
+++ b/drivers/usb/chipidea/otg_fsm.h
@@ -0,0 +1,129 @@
+/*
+ * Copyright (C) 2014 Freescale Semiconductor, Inc.
+ *
+ * Author: Jun Li
+ *
+ * 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 __DRIVERS_USB_CHIPIDEA_OTG_FSM_H
+#define __DRIVERS_USB_CHIPIDEA_OTG_FSM_H
+
+#include <linux/usb/otg-fsm.h>
+
+/*
+ *  A-DEVICE timing  constants
+ */
+
+/* Wait for VBUS Rise  */
+#define TA_WAIT_VRISE        (100)	/* a_wait_vrise: section 7.1.2
+					 * a_wait_vrise_tmr: section 7.4.5.1
+					 * TA_VBUS_RISE <= 100ms, section 4.4
+					 * Table 4-1: Electrical Characteristics
+					 * ->DC Electrical Timing
+					 */
+/* Wait for VBUS Fall  */
+#define TA_WAIT_VFALL        (1000)	/* a_wait_vfall: section 7.1.7
+					 * a_wait_vfall_tmr: section: 7.4.5.2
+					 */
+/* Wait for B-Connect */
+#define TA_WAIT_BCON         (10000)	/* a_wait_bcon: section 7.1.3
+					 * TA_WAIT_BCON: should be between 1100
+					 * and 30000 ms, section 5.5, Table 5-1
+					 */
+/* A-Idle to B-Disconnect */
+#define TA_AIDL_BDIS         (5000)	/* a_suspend min 200 ms, section 5.2.1
+					 * TA_AIDL_BDIS: section 5.5, Table 5-1
+					 */
+/* B-Idle to A-Disconnect */
+#define TA_BIDL_ADIS         (500)	/* TA_BIDL_ADIS: section 5.2.1
+					 * 500ms is used for B switch to host
+					 * for safe
+					 */
+
+/*
+ * B-device timing constants
+ */
+
+/* Data-Line Pulse Time*/
+#define TB_DATA_PLS          (10)	/* b_srp_init,continue 5~10ms
+					 * section:5.1.3
+					 */
+/* SRP Fail Time  */
+#define TB_SRP_FAIL          (6000)	/* b_srp_init,fail time 5~6s
+					 * section:5.1.6
+					 */
+/* A-SE0 to B-Reset  */
+#define TB_ASE0_BRST         (155)	/* minimum 155 ms, section:5.3.1 */
+/* SE0 Time Before SRP */
+#define TB_SE0_SRP           (1000)	/* b_idle,minimum 1s, section:5.1.2 */
+/* SSEND time before SRP */
+#define TB_SSEND_SRP         (1500)	/* minimum 1.5 sec, section:5.1.2 */
+
+#define TB_SESS_VLD          (1000)
+
+enum ci_otg_fsm_timer_index {
+	/*
+	 * CI specific timers, start from the end
+	 * of standard and auxiliary OTG timers
+	 */
+	B_DATA_PLS = NUM_OTG_FSM_TIMERS,
+	B_SSEND_SRP,
+	B_SESS_VLD,
+
+	NUM_CI_OTG_FSM_TIMERS,
+};
+
+struct ci_otg_fsm_timer {
+	unsigned long expires;  /* Number of count increase to timeout */
+	unsigned long count;    /* Tick counter */
+	void (*function)(void *, unsigned long);        /* Timeout function */
+	unsigned long data;     /* Data passed to function */
+	struct list_head list;
+};
+
+struct ci_otg_fsm_timer_list {
+	struct ci_otg_fsm_timer *timer_list[NUM_CI_OTG_FSM_TIMERS];
+	struct list_head active_timers;
+};
+
+#ifdef CONFIG_USB_OTG_FSM
+
+int ci_hdrc_otg_fsm_init(struct ci_hdrc *ci);
+int ci_otg_fsm_work(struct ci_hdrc *ci);
+irqreturn_t ci_otg_fsm_irq(struct ci_hdrc *ci);
+void ci_hdrc_otg_fsm_start(struct ci_hdrc *ci);
+void ci_hdrc_otg_fsm_remove(struct ci_hdrc *ci);
+
+#else
+
+static inline int ci_hdrc_otg_fsm_init(struct ci_hdrc *ci)
+{
+	return 0;
+}
+
+static inline int ci_otg_fsm_work(struct ci_hdrc *ci)
+{
+	return -ENXIO;
+}
+
+static inline irqreturn_t ci_otg_fsm_irq(struct ci_hdrc *ci)
+{
+	return IRQ_NONE;
+}
+
+static inline void ci_hdrc_otg_fsm_start(struct ci_hdrc *ci)
+{
+
+}
+
+static inline void ci_hdrc_otg_fsm_remove(struct ci_hdrc *ci)
+{
+
+}
+
+#endif
+
+#endif /* __DRIVERS_USB_CHIPIDEA_OTG_FSM_H */
diff --git a/drivers/usb/chipidea/udc.c b/drivers/usb/chipidea/udc.c
index 7739c64ef25..69425b3cb6b 100644
--- a/drivers/usb/chipidea/udc.c
+++ b/drivers/usb/chipidea/udc.c
@@ -20,6 +20,7 @@
 #include <linux/pm_runtime.h>
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
+#include <linux/usb/otg-fsm.h>
 #include <linux/usb/chipidea.h>
 
 #include "ci.h"
@@ -27,6 +28,7 @@
 #include "bits.h"
 #include "debug.h"
 #include "otg.h"
+#include "otg_fsm.h"
 
 /* control endpoint description */
 static const struct usb_endpoint_descriptor
@@ -242,26 +244,6 @@ static int hw_port_is_high_speed(struct ci_hdrc *ci)
 }
 
 /**
- * hw_read_intr_enable: returns interrupt enable register
- *
- * This function returns register data
- */
-static u32 hw_read_intr_enable(struct ci_hdrc *ci)
-{
-	return hw_read(ci, OP_USBINTR, ~0);
-}
-
-/**
- * hw_read_intr_status: returns interrupt status register
- *
- * This function returns register data
- */
-static u32 hw_read_intr_status(struct ci_hdrc *ci)
-{
-	return hw_read(ci, OP_USBSTS, ~0);
-}
-
-/**
  * hw_test_and_clear_complete: test & clear complete status (execute without
  *                             interruption)
  * @n: endpoint number
@@ -727,6 +709,8 @@ __acquires(ci->lock)
 	if (ci->status == NULL)
 		retval = -ENOMEM;
 
+	usb_gadget_set_state(&ci->gadget, USB_STATE_DEFAULT);
+
 done:
 	spin_lock(&ci->lock);
 
@@ -841,7 +825,6 @@ __acquires(hwep->lock)
 	if ((setup->bRequestType & USB_RECIP_MASK) == USB_RECIP_DEVICE) {
 		/* Assume that device is bus powered for now. */
 		*(u16 *)req->buf = ci->remote_wakeup << 1;
-		retval = 0;
 	} else if ((setup->bRequestType & USB_RECIP_MASK) \
 		   == USB_RECIP_ENDPOINT) {
 		dir = (le16_to_cpu(setup->wIndex) & USB_ENDPOINT_DIR_MASK) ?
@@ -883,6 +866,8 @@ isr_setup_status_complete(struct usb_ep *ep, struct usb_request *req)
 	if (ci->setaddr) {
 		hw_usb_set_address(ci, ci->address);
 		ci->setaddr = false;
+		if (ci->address)
+			usb_gadget_set_state(&ci->gadget, USB_STATE_ADDRESS);
 	}
 
 	spin_lock_irqsave(&ci->lock, flags);
@@ -1072,6 +1057,14 @@ __acquires(ci->lock)
 				default:
 					break;
 				}
+				break;
+			case USB_DEVICE_B_HNP_ENABLE:
+				if (ci_otg_is_fsm_mode(ci)) {
+					ci->gadget.b_hnp_enable = 1;
+					err = isr_setup_status_phase(
+							ci);
+				}
+				break;
 			default:
 				goto delegate;
 			}
@@ -1477,7 +1470,7 @@ static int ci_udc_vbus_session(struct usb_gadget *_gadget, int is_active)
 			pm_runtime_get_sync(&_gadget->dev);
 			hw_device_reset(ci, USBMODE_CM_DC);
 			hw_device_state(ci, ci->ep0out->qh.dma);
-			dev_dbg(ci->dev, "Connected to host\n");
+			usb_gadget_set_state(_gadget, USB_STATE_POWERED);
 		} else {
 			if (ci->driver)
 				ci->driver->disconnect(&ci->gadget);
@@ -1487,7 +1480,7 @@ static int ci_udc_vbus_session(struct usb_gadget *_gadget, int is_active)
 				CI_HDRC_CONTROLLER_STOPPED_EVENT);
 			_gadget_stop_activity(&ci->gadget);
 			pm_runtime_put_sync(&_gadget->dev);
-			dev_dbg(ci->dev, "Disconnected from host\n");
+			usb_gadget_set_state(_gadget, USB_STATE_NOTATTACHED);
 		}
 	}
 
@@ -1655,6 +1648,13 @@ static int ci_udc_start(struct usb_gadget *gadget,
 		return retval;
 
 	ci->driver = driver;
+
+	/* Start otg fsm for B-device */
+	if (ci_otg_is_fsm_mode(ci) && ci->fsm.id) {
+		ci_hdrc_otg_fsm_start(ci);
+		return retval;
+	}
+
 	pm_runtime_get_sync(&ci->gadget.dev);
 	if (ci->vbus_active) {
 		spin_lock_irqsave(&ci->lock, flags);
@@ -1753,6 +1753,8 @@ static irqreturn_t udc_irq(struct ci_hdrc *ci)
 				ci->suspended = 1;
 				spin_unlock(&ci->lock);
 				ci->driver->suspend(&ci->gadget);
+				usb_gadget_set_state(&ci->gadget,
+						USB_STATE_SUSPENDED);
 				spin_lock(&ci->lock);
 			}
 		}
@@ -1779,7 +1781,7 @@ static int udc_start(struct ci_hdrc *ci)
 	ci->gadget.ops          = &usb_gadget_ops;
 	ci->gadget.speed        = USB_SPEED_UNKNOWN;
 	ci->gadget.max_speed    = USB_SPEED_HIGH;
-	ci->gadget.is_otg       = 0;
+	ci->gadget.is_otg       = ci->is_otg ? 1 : 0;
 	ci->gadget.name         = ci->platdata->name;
 
 	INIT_LIST_HEAD(&ci->gadget.ep_list);
@@ -1843,21 +1845,22 @@ void ci_hdrc_gadget_destroy(struct ci_hdrc *ci)
 
 static int udc_id_switch_for_device(struct ci_hdrc *ci)
 {
-	if (ci->is_otg) {
-		ci_clear_otg_interrupt(ci, OTGSC_BSVIS);
-		ci_enable_otg_interrupt(ci, OTGSC_BSVIE);
-	}
+	if (ci->is_otg)
+		/* Clear and enable BSV irq */
+		hw_write_otgsc(ci, OTGSC_BSVIS | OTGSC_BSVIE,
+					OTGSC_BSVIS | OTGSC_BSVIE);
 
 	return 0;
 }
 
 static void udc_id_switch_for_host(struct ci_hdrc *ci)
 {
-	if (ci->is_otg) {
-		/* host doesn't care B_SESSION_VALID event */
-		ci_clear_otg_interrupt(ci, OTGSC_BSVIS);
-		ci_disable_otg_interrupt(ci, OTGSC_BSVIE);
-	}
+	/*
+	 * host doesn't care B_SESSION_VALID event
+	 * so clear and disbale BSV irq
+	 */
+	if (ci->is_otg)
+		hw_write_otgsc(ci, OTGSC_BSVIE | OTGSC_BSVIS, OTGSC_BSVIS);
 }
 
 /**
diff --git a/drivers/usb/chipidea/usbmisc_imx.c b/drivers/usb/chipidea/usbmisc_imx.c
index cd061abe350..85293b8b1df 100644
--- a/drivers/usb/chipidea/usbmisc_imx.c
+++ b/drivers/usb/chipidea/usbmisc_imx.c
@@ -21,16 +21,39 @@
 #define MX25_USB_PHY_CTRL_OFFSET	0x08
 #define MX25_BM_EXTERNAL_VBUS_DIVIDER	BIT(23)
 
+#define MX25_EHCI_INTERFACE_SINGLE_UNI	(2 << 0)
+#define MX25_EHCI_INTERFACE_DIFF_UNI	(0 << 0)
+#define MX25_EHCI_INTERFACE_MASK	(0xf)
+
+#define MX25_OTG_SIC_SHIFT		29
+#define MX25_OTG_SIC_MASK		(0x3 << MX25_OTG_SIC_SHIFT)
+#define MX25_OTG_PM_BIT			BIT(24)
+#define MX25_OTG_PP_BIT			BIT(11)
+#define MX25_OTG_OCPOL_BIT		BIT(3)
+
+#define MX25_H1_SIC_SHIFT		21
+#define MX25_H1_SIC_MASK		(0x3 << MX25_H1_SIC_SHIFT)
+#define MX25_H1_PP_BIT			BIT(18)
+#define MX25_H1_PM_BIT			BIT(16)
+#define MX25_H1_IPPUE_UP_BIT		BIT(7)
+#define MX25_H1_IPPUE_DOWN_BIT		BIT(6)
+#define MX25_H1_TLL_BIT			BIT(5)
+#define MX25_H1_USBTE_BIT		BIT(4)
+#define MX25_H1_OCPOL_BIT		BIT(2)
+
 #define MX27_H1_PM_BIT			BIT(8)
 #define MX27_H2_PM_BIT			BIT(16)
 #define MX27_OTG_PM_BIT			BIT(24)
 
 #define MX53_USB_OTG_PHY_CTRL_0_OFFSET	0x08
+#define MX53_USB_OTG_PHY_CTRL_1_OFFSET	0x0c
 #define MX53_USB_UH2_CTRL_OFFSET	0x14
 #define MX53_USB_UH3_CTRL_OFFSET	0x18
 #define MX53_BM_OVER_CUR_DIS_H1		BIT(5)
 #define MX53_BM_OVER_CUR_DIS_OTG	BIT(8)
 #define MX53_BM_OVER_CUR_DIS_UHx	BIT(30)
+#define MX53_USB_PHYCTRL1_PLLDIV_MASK	0x3
+#define MX53_USB_PLL_DIV_24_MHZ		0x01
 
 #define MX6_BM_OVER_CUR_DIS		BIT(7)
 
@@ -50,6 +73,39 @@ struct imx_usbmisc {
 
 static struct imx_usbmisc *usbmisc;
 
+static int usbmisc_imx25_init(struct imx_usbmisc_data *data)
+{
+	unsigned long flags;
+	u32 val = 0;
+
+	if (data->index > 1)
+		return -EINVAL;
+
+	spin_lock_irqsave(&usbmisc->lock, flags);
+	switch (data->index) {
+	case 0:
+		val = readl(usbmisc->base);
+		val &= ~(MX25_OTG_SIC_MASK | MX25_OTG_PP_BIT);
+		val |= (MX25_EHCI_INTERFACE_DIFF_UNI & MX25_EHCI_INTERFACE_MASK) << MX25_OTG_SIC_SHIFT;
+		val |= (MX25_OTG_PM_BIT | MX25_OTG_OCPOL_BIT);
+		writel(val, usbmisc->base);
+		break;
+	case 1:
+		val = readl(usbmisc->base);
+		val &= ~(MX25_H1_SIC_MASK | MX25_H1_PP_BIT |  MX25_H1_IPPUE_UP_BIT);
+		val |= (MX25_EHCI_INTERFACE_SINGLE_UNI & MX25_EHCI_INTERFACE_MASK) << MX25_H1_SIC_SHIFT;
+		val |= (MX25_H1_PM_BIT | MX25_H1_OCPOL_BIT | MX25_H1_TLL_BIT |
+			MX25_H1_USBTE_BIT | MX25_H1_IPPUE_DOWN_BIT);
+
+		writel(val, usbmisc->base);
+
+		break;
+	}
+	spin_unlock_irqrestore(&usbmisc->lock, flags);
+
+	return 0;
+}
+
 static int usbmisc_imx25_post(struct imx_usbmisc_data *data)
 {
 	void __iomem *reg;
@@ -111,6 +167,13 @@ static int usbmisc_imx53_init(struct imx_usbmisc_data *data)
 	if (data->index > 3)
 		return -EINVAL;
 
+	/* Select a 24 MHz reference clock for the PHY  */
+	reg = usbmisc->base + MX53_USB_OTG_PHY_CTRL_1_OFFSET;
+	val = readl(reg);
+	val &= ~MX53_USB_PHYCTRL1_PLLDIV_MASK;
+	val |= MX53_USB_PLL_DIV_24_MHZ;
+	writel(val, usbmisc->base + MX53_USB_OTG_PHY_CTRL_1_OFFSET);
+
 	if (data->disable_oc) {
 		spin_lock_irqsave(&usbmisc->lock, flags);
 		switch (data->index) {
@@ -159,6 +222,7 @@ static int usbmisc_imx6q_init(struct imx_usbmisc_data *data)
 }
 
 static const struct usbmisc_ops imx25_usbmisc_ops = {
+	.init = usbmisc_imx25_init,
 	.post = usbmisc_imx25_post,
 };
 
@@ -200,6 +264,10 @@ static const struct of_device_id usbmisc_imx_dt_ids[] = {
 		.data = &imx25_usbmisc_ops,
 	},
 	{
+		.compatible = "fsl,imx35-usbmisc",
+		.data = &imx25_usbmisc_ops,
+	},
+	{
 		.compatible = "fsl,imx27-usbmisc",
 		.data = &imx27_usbmisc_ops,
 	},
diff --git a/drivers/usb/class/cdc-acm.c b/drivers/usb/class/cdc-acm.c
index 904efb6035b..e934e19f49f 100644
--- a/drivers/usb/class/cdc-acm.c
+++ b/drivers/usb/class/cdc-acm.c
@@ -122,13 +122,23 @@ static void acm_release_minor(struct acm *acm)
 static int acm_ctrl_msg(struct acm *acm, int request, int value,
 							void *buf, int len)
 {
-	int retval = usb_control_msg(acm->dev, usb_sndctrlpipe(acm->dev, 0),
+	int retval;
+
+	retval = usb_autopm_get_interface(acm->control);
+	if (retval)
+		return retval;
+
+	retval = usb_control_msg(acm->dev, usb_sndctrlpipe(acm->dev, 0),
 		request, USB_RT_ACM, value,
 		acm->control->altsetting[0].desc.bInterfaceNumber,
 		buf, len, 5000);
+
 	dev_dbg(&acm->control->dev,
 			"%s - rq 0x%02x, val %#x, len %#x, result %d\n",
 			__func__, request, value, len, retval);
+
+	usb_autopm_put_interface(acm->control);
+
 	return retval < 0 ? retval : 0;
 }
 
@@ -406,19 +416,21 @@ static void acm_read_bulk_callback(struct urb *urb)
 		dev_dbg(&acm->data->dev, "%s - disconnected\n", __func__);
 		return;
 	}
-	usb_mark_last_busy(acm->dev);
 
 	if (urb->status) {
 		dev_dbg(&acm->data->dev, "%s - non-zero urb status: %d\n",
 							__func__, urb->status);
 		return;
 	}
+
+	usb_mark_last_busy(acm->dev);
+
 	acm_process_read_urb(acm, urb);
 
 	/* throttle device if requested by tty */
 	spin_lock_irqsave(&acm->read_lock, flags);
 	acm->throttled = acm->throttle_req;
-	if (!acm->throttled && !acm->susp_count) {
+	if (!acm->throttled) {
 		spin_unlock_irqrestore(&acm->read_lock, flags);
 		acm_submit_read_urb(acm, rb->index, GFP_ATOMIC);
 	} else {
@@ -492,10 +504,30 @@ static int acm_tty_open(struct tty_struct *tty, struct file *filp)
 	return tty_port_open(&acm->port, tty, filp);
 }
 
+static void acm_port_dtr_rts(struct tty_port *port, int raise)
+{
+	struct acm *acm = container_of(port, struct acm, port);
+	int val;
+	int res;
+
+	if (raise)
+		val = ACM_CTRL_DTR | ACM_CTRL_RTS;
+	else
+		val = 0;
+
+	/* FIXME: add missing ctrlout locking throughout driver */
+	acm->ctrlout = val;
+
+	res = acm_set_control(acm, val);
+	if (res && (acm->ctrl_caps & USB_CDC_CAP_LINE))
+		dev_err(&acm->control->dev, "failed to set dtr/rts\n");
+}
+
 static int acm_port_activate(struct tty_port *port, struct tty_struct *tty)
 {
 	struct acm *acm = container_of(port, struct acm, port);
 	int retval = -ENODEV;
+	int i;
 
 	dev_dbg(&acm->control->dev, "%s\n", __func__);
 
@@ -515,22 +547,13 @@ static int acm_port_activate(struct tty_port *port, struct tty_struct *tty)
 	acm->control->needs_remote_wakeup = 1;
 
 	acm->ctrlurb->dev = acm->dev;
-	if (usb_submit_urb(acm->ctrlurb, GFP_KERNEL)) {
+	retval = usb_submit_urb(acm->ctrlurb, GFP_KERNEL);
+	if (retval) {
 		dev_err(&acm->control->dev,
 			"%s - usb_submit_urb(ctrl irq) failed\n", __func__);
-		usb_autopm_put_interface(acm->control);
 		goto error_submit_urb;
 	}
 
-	acm->ctrlout = ACM_CTRL_DTR | ACM_CTRL_RTS;
-	if (acm_set_control(acm, acm->ctrlout) < 0 &&
-	    (acm->ctrl_caps & USB_CDC_CAP_LINE)) {
-		usb_autopm_put_interface(acm->control);
-		goto error_set_control;
-	}
-
-	usb_autopm_put_interface(acm->control);
-
 	/*
 	 * Unthrottle device in case the TTY was closed while throttled.
 	 */
@@ -539,23 +562,27 @@ static int acm_port_activate(struct tty_port *port, struct tty_struct *tty)
 	acm->throttle_req = 0;
 	spin_unlock_irq(&acm->read_lock);
 
-	if (acm_submit_read_urbs(acm, GFP_KERNEL))
+	retval = acm_submit_read_urbs(acm, GFP_KERNEL);
+	if (retval)
 		goto error_submit_read_urbs;
 
+	usb_autopm_put_interface(acm->control);
+
 	mutex_unlock(&acm->mutex);
 
 	return 0;
 
 error_submit_read_urbs:
-	acm->ctrlout = 0;
-	acm_set_control(acm, acm->ctrlout);
-error_set_control:
+	for (i = 0; i < acm->rx_buflimit; i++)
+		usb_kill_urb(acm->read_urbs[i]);
 	usb_kill_urb(acm->ctrlurb);
 error_submit_urb:
+	usb_autopm_put_interface(acm->control);
 error_get_interface:
 disconnected:
 	mutex_unlock(&acm->mutex);
-	return retval;
+
+	return usb_translate_errors(retval);
 }
 
 static void acm_port_destruct(struct tty_port *port)
@@ -573,23 +600,37 @@ static void acm_port_destruct(struct tty_port *port)
 static void acm_port_shutdown(struct tty_port *port)
 {
 	struct acm *acm = container_of(port, struct acm, port);
+	struct urb *urb;
+	struct acm_wb *wb;
 	int i;
 
 	dev_dbg(&acm->control->dev, "%s\n", __func__);
 
-	mutex_lock(&acm->mutex);
-	if (!acm->disconnected) {
-		usb_autopm_get_interface(acm->control);
-		acm_set_control(acm, acm->ctrlout = 0);
-		usb_kill_urb(acm->ctrlurb);
-		for (i = 0; i < ACM_NW; i++)
-			usb_kill_urb(acm->wb[i].urb);
-		for (i = 0; i < acm->rx_buflimit; i++)
-			usb_kill_urb(acm->read_urbs[i]);
-		acm->control->needs_remote_wakeup = 0;
-		usb_autopm_put_interface(acm->control);
+	/*
+	 * Need to grab write_lock to prevent race with resume, but no need to
+	 * hold it due to the tty-port initialised flag.
+	 */
+	spin_lock_irq(&acm->write_lock);
+	spin_unlock_irq(&acm->write_lock);
+
+	usb_autopm_get_interface_no_resume(acm->control);
+	acm->control->needs_remote_wakeup = 0;
+	usb_autopm_put_interface(acm->control);
+
+	for (;;) {
+		urb = usb_get_from_anchor(&acm->delayed);
+		if (!urb)
+			break;
+		wb = urb->context;
+		wb->use = 0;
+		usb_autopm_put_interface_async(acm->control);
 	}
-	mutex_unlock(&acm->mutex);
+
+	usb_kill_urb(acm->ctrlurb);
+	for (i = 0; i < ACM_NW; i++)
+		usb_kill_urb(acm->wb[i].urb);
+	for (i = 0; i < acm->rx_buflimit; i++)
+		usb_kill_urb(acm->read_urbs[i]);
 }
 
 static void acm_tty_cleanup(struct tty_struct *tty)
@@ -646,16 +687,18 @@ static int acm_tty_write(struct tty_struct *tty,
 	memcpy(wb->buf, buf, count);
 	wb->len = count;
 
-	usb_autopm_get_interface_async(acm->control);
+	stat = usb_autopm_get_interface_async(acm->control);
+	if (stat) {
+		wb->use = 0;
+		spin_unlock_irqrestore(&acm->write_lock, flags);
+		return stat;
+	}
+
 	if (acm->susp_count) {
-		if (!acm->delayed_wb)
-			acm->delayed_wb = wb;
-		else
-			usb_autopm_put_interface_async(acm->control);
+		usb_anchor_urb(wb->urb, &acm->delayed);
 		spin_unlock_irqrestore(&acm->write_lock, flags);
-		return count;	/* A white lie */
+		return count;
 	}
-	usb_mark_last_busy(acm->dev);
 
 	stat = acm_start_wb(acm, wb);
 	spin_unlock_irqrestore(&acm->write_lock, flags);
@@ -958,6 +1001,7 @@ static void acm_tty_set_termios(struct tty_struct *tty,
 }
 
 static const struct tty_port_operations acm_port_ops = {
+	.dtr_rts = acm_port_dtr_rts,
 	.shutdown = acm_port_shutdown,
 	.activate = acm_port_activate,
 	.destruct = acm_port_destruct,
@@ -1269,6 +1313,7 @@ made_compressed_probe:
 		acm->bInterval = epread->bInterval;
 	tty_port_init(&acm->port);
 	acm->port.ops = &acm_port_ops;
+	init_usb_anchor(&acm->delayed);
 
 	buf = usb_alloc_coherent(usb_dev, ctrlsize, GFP_KERNEL, &acm->ctrl_dma);
 	if (!buf) {
@@ -1394,8 +1439,6 @@ skip_countries:
 
 	dev_info(&intf->dev, "ttyACM%d: USB ACM device\n", minor);
 
-	acm_set_control(acm, acm->ctrlout);
-
 	acm->line.dwDTERate = cpu_to_le32(9600);
 	acm->line.bDataBits = 8;
 	acm_set_line(acm, &acm->line);
@@ -1514,27 +1557,20 @@ static int acm_suspend(struct usb_interface *intf, pm_message_t message)
 	struct acm *acm = usb_get_intfdata(intf);
 	int cnt;
 
+	spin_lock_irq(&acm->write_lock);
 	if (PMSG_IS_AUTO(message)) {
-		int b;
-
-		spin_lock_irq(&acm->write_lock);
-		b = acm->transmitting;
-		spin_unlock_irq(&acm->write_lock);
-		if (b)
+		if (acm->transmitting) {
+			spin_unlock_irq(&acm->write_lock);
 			return -EBUSY;
+		}
 	}
-
-	spin_lock_irq(&acm->read_lock);
-	spin_lock(&acm->write_lock);
 	cnt = acm->susp_count++;
-	spin_unlock(&acm->write_lock);
-	spin_unlock_irq(&acm->read_lock);
+	spin_unlock_irq(&acm->write_lock);
 
 	if (cnt)
 		return 0;
 
-	if (test_bit(ASYNCB_INITIALIZED, &acm->port.flags))
-		stop_data_traffic(acm);
+	stop_data_traffic(acm);
 
 	return 0;
 }
@@ -1542,29 +1578,23 @@ static int acm_suspend(struct usb_interface *intf, pm_message_t message)
 static int acm_resume(struct usb_interface *intf)
 {
 	struct acm *acm = usb_get_intfdata(intf);
-	struct acm_wb *wb;
+	struct urb *urb;
 	int rv = 0;
-	int cnt;
 
-	spin_lock_irq(&acm->read_lock);
-	acm->susp_count -= 1;
-	cnt = acm->susp_count;
-	spin_unlock_irq(&acm->read_lock);
+	spin_lock_irq(&acm->write_lock);
 
-	if (cnt)
-		return 0;
+	if (--acm->susp_count)
+		goto out;
 
 	if (test_bit(ASYNCB_INITIALIZED, &acm->port.flags)) {
-		rv = usb_submit_urb(acm->ctrlurb, GFP_NOIO);
+		rv = usb_submit_urb(acm->ctrlurb, GFP_ATOMIC);
 
-		spin_lock_irq(&acm->write_lock);
-		if (acm->delayed_wb) {
-			wb = acm->delayed_wb;
-			acm->delayed_wb = NULL;
-			spin_unlock_irq(&acm->write_lock);
-			acm_start_wb(acm, wb);
-		} else {
-			spin_unlock_irq(&acm->write_lock);
+		for (;;) {
+			urb = usb_get_from_anchor(&acm->delayed);
+			if (!urb)
+				break;
+
+			acm_start_wb(acm, urb->context);
 		}
 
 		/*
@@ -1572,12 +1602,13 @@ static int acm_resume(struct usb_interface *intf)
 		 * do the write path at all cost
 		 */
 		if (rv < 0)
-			goto err_out;
+			goto out;
 
-		rv = acm_submit_read_urbs(acm, GFP_NOIO);
+		rv = acm_submit_read_urbs(acm, GFP_ATOMIC);
 	}
+out:
+	spin_unlock_irq(&acm->write_lock);
 
-err_out:
 	return rv;
 }
 
diff --git a/drivers/usb/class/cdc-acm.h b/drivers/usb/class/cdc-acm.h
index e38dc785808..fc75651afe1 100644
--- a/drivers/usb/class/cdc-acm.h
+++ b/drivers/usb/class/cdc-acm.h
@@ -120,15 +120,15 @@ struct acm {
 	unsigned int throttled:1;			/* actually throttled */
 	unsigned int throttle_req:1;			/* throttle requested */
 	u8 bInterval;
-	struct acm_wb *delayed_wb;			/* write queued for a device about to be woken */
+	struct usb_anchor delayed;			/* writes queued for a device about to be woken */
 };
 
 #define CDC_DATA_INTERFACE_TYPE	0x0a
 
 /* constants describing various quirks and errors */
-#define NO_UNION_NORMAL			1
-#define SINGLE_RX_URB			2
-#define NO_CAP_LINE			4
-#define NOT_A_MODEM			8
-#define NO_DATA_INTERFACE		16
-#define IGNORE_DEVICE			32
+#define NO_UNION_NORMAL			BIT(0)
+#define SINGLE_RX_URB			BIT(1)
+#define NO_CAP_LINE			BIT(2)
+#define NOT_A_MODEM			BIT(3)
+#define NO_DATA_INTERFACE		BIT(4)
+#define IGNORE_DEVICE			BIT(5)
diff --git a/drivers/usb/class/usbtmc.c b/drivers/usb/class/usbtmc.c
index cfbec9c7e09..103a6e9ee49 100644
--- a/drivers/usb/class/usbtmc.c
+++ b/drivers/usb/class/usbtmc.c
@@ -383,9 +383,12 @@ exit:
 static int send_request_dev_dep_msg_in(struct usbtmc_device_data *data, size_t transfer_size)
 {
 	int retval;
-	u8 buffer[USBTMC_HEADER_SIZE];
+	u8 *buffer;
 	int actual;
 
+	buffer = kmalloc(USBTMC_HEADER_SIZE, GFP_KERNEL);
+	if (!buffer)
+		return -ENOMEM;
 	/* Setup IO buffer for REQUEST_DEV_DEP_MSG_IN message
 	 * Refer to class specs for details
 	 */
@@ -417,6 +420,7 @@ static int send_request_dev_dep_msg_in(struct usbtmc_device_data *data, size_t t
 	if (!data->bTag)
 		data->bTag++;
 
+	kfree(buffer);
 	if (retval < 0) {
 		dev_err(&data->intf->dev, "usb_bulk_msg in send_request_dev_dep_msg_in() returned %d\n", retval);
 		return retval;
diff --git a/drivers/usb/common/Makefile b/drivers/usb/common/Makefile
new file mode 100644
index 00000000000..752646167e1
--- /dev/null
+++ b/drivers/usb/common/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for the usb common parts.
+#
+
+obj-$(CONFIG_USB_COMMON) += usb-common.o
+obj-$(CONFIG_USB_OTG_FSM) += usb-otg-fsm.o
diff --git a/drivers/usb/usb-common.c b/drivers/usb/common/usb-common.c
index 6dfd30a863c..6dfd30a863c 100644
--- a/drivers/usb/usb-common.c
+++ b/drivers/usb/common/usb-common.c
diff --git a/drivers/usb/phy/phy-fsm-usb.c b/drivers/usb/common/usb-otg-fsm.c
index d03fadd2629..98e8340a5bb 100644
--- a/drivers/usb/phy/phy-fsm-usb.c
+++ b/drivers/usb/common/usb-otg-fsm.c
@@ -364,3 +364,4 @@ int otg_statemachine(struct otg_fsm *fsm)
 	VDBG("quit statemachine, changed = %d\n", state_changed);
 	return state_changed;
 }
+EXPORT_SYMBOL_GPL(otg_statemachine);
diff --git a/drivers/usb/core/Kconfig b/drivers/usb/core/Kconfig
index cb8e99156f5..1060657ca1b 100644
--- a/drivers/usb/core/Kconfig
+++ b/drivers/usb/core/Kconfig
@@ -1,13 +1,6 @@
 #
 # USB Core configuration
 #
-config USB_DEBUG
-	bool "USB verbose debug messages"
-	help
-	  Say Y here if you want the USB core & hub drivers to produce a bunch
-	  of debug messages to the system log. Select this if you are having a
-	  problem with USB support and want to see more of what is going on.
-
 config USB_ANNOUNCE_NEW_DEVICES
 	bool "USB announce new devices"
 	help
@@ -88,3 +81,12 @@ config USB_OTG_BLACKLIST_HUB
 	  and software costs by not supporting external hubs.  So
 	  are "Embedded Hosts" that don't offer OTG support.
 
+config USB_OTG_FSM
+	tristate "USB 2.0 OTG FSM implementation"
+	depends on USB
+	select USB_OTG
+	select USB_PHY
+	help
+	  Implements OTG Finite State Machine as specified in On-The-Go
+	  and Embedded Host Supplement to the USB Revision 2.0 Specification.
+
diff --git a/drivers/usb/core/driver.c b/drivers/usb/core/driver.c
index 888881e5f29..4aeb10034de 100644
--- a/drivers/usb/core/driver.c
+++ b/drivers/usb/core/driver.c
@@ -1822,10 +1822,13 @@ int usb_runtime_suspend(struct device *dev)
 	if (status == -EAGAIN || status == -EBUSY)
 		usb_mark_last_busy(udev);
 
-	/* The PM core reacts badly unless the return code is 0,
-	 * -EAGAIN, or -EBUSY, so always return -EBUSY on an error.
+	/*
+	 * The PM core reacts badly unless the return code is 0,
+	 * -EAGAIN, or -EBUSY, so always return -EBUSY on an error
+	 * (except for root hubs, because they don't suspend through
+	 * an upstream port like other USB devices).
 	 */
-	if (status != 0)
+	if (status != 0 && udev->parent)
 		return -EBUSY;
 	return status;
 }
diff --git a/drivers/usb/core/hcd-pci.c b/drivers/usb/core/hcd-pci.c
index 1f02e65fe30..82044b5d611 100644
--- a/drivers/usb/core/hcd-pci.c
+++ b/drivers/usb/core/hcd-pci.c
@@ -192,7 +192,6 @@ int usb_hcd_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 
 	if (pci_enable_device(dev) < 0)
 		return -ENODEV;
-	dev->current_state = PCI_D0;
 
 	/*
 	 * The xHCI driver has its own irq management
diff --git a/drivers/usb/core/hcd.c b/drivers/usb/core/hcd.c
index 9c4e2922b04..bec31e2efb8 100644
--- a/drivers/usb/core/hcd.c
+++ b/drivers/usb/core/hcd.c
@@ -918,6 +918,7 @@ static void usb_bus_init (struct usb_bus *bus)
 	bus->bandwidth_allocated = 0;
 	bus->bandwidth_int_reqs  = 0;
 	bus->bandwidth_isoc_reqs = 0;
+	mutex_init(&bus->usb_address0_mutex);
 
 	INIT_LIST_HEAD (&bus->bus_list);
 }
@@ -1502,6 +1503,9 @@ int usb_hcd_map_urb_for_dma(struct usb_hcd *hcd, struct urb *urb,
 					ret = -EAGAIN;
 				else
 					urb->transfer_flags |= URB_DMA_MAP_PAGE;
+			} else if (is_vmalloc_addr(urb->transfer_buffer)) {
+				WARN_ONCE(1, "transfer buffer not dma capable\n");
+				ret = -EAGAIN;
 			} else {
 				urb->transfer_dma = dma_map_single(
 						hcd->self.controller,
@@ -2263,9 +2267,7 @@ static void hcd_resume_work(struct work_struct *work)
 	struct usb_hcd *hcd = container_of(work, struct usb_hcd, wakeup_work);
 	struct usb_device *udev = hcd->self.root_hub;
 
-	usb_lock_device(udev);
 	usb_remote_wakeup(udev);
-	usb_unlock_device(udev);
 }
 
 /**
@@ -2454,11 +2456,13 @@ struct usb_hcd *usb_create_shared_hcd(const struct hc_driver *driver,
 		mutex_init(hcd->bandwidth_mutex);
 		dev_set_drvdata(dev, hcd);
 	} else {
+		mutex_lock(&usb_port_peer_mutex);
 		hcd->bandwidth_mutex = primary_hcd->bandwidth_mutex;
 		hcd->primary_hcd = primary_hcd;
 		primary_hcd->primary_hcd = primary_hcd;
 		hcd->shared_hcd = primary_hcd;
 		primary_hcd->shared_hcd = hcd;
+		mutex_unlock(&usb_port_peer_mutex);
 	}
 
 	kref_init(&hcd->kref);
@@ -2510,18 +2514,25 @@ EXPORT_SYMBOL_GPL(usb_create_hcd);
  * deallocated.
  *
  * Make sure to only deallocate the bandwidth_mutex when the primary HCD is
- * freed.  When hcd_release() is called for the non-primary HCD, set the
- * primary_hcd's shared_hcd pointer to null (since the non-primary HCD will be
- * freed shortly).
+ * freed.  When hcd_release() is called for either hcd in a peer set
+ * invalidate the peer's ->shared_hcd and ->primary_hcd pointers to
+ * block new peering attempts
  */
-static void hcd_release (struct kref *kref)
+static void hcd_release(struct kref *kref)
 {
 	struct usb_hcd *hcd = container_of (kref, struct usb_hcd, kref);
 
+	mutex_lock(&usb_port_peer_mutex);
 	if (usb_hcd_is_primary_hcd(hcd))
 		kfree(hcd->bandwidth_mutex);
-	else
-		hcd->shared_hcd->shared_hcd = NULL;
+	if (hcd->shared_hcd) {
+		struct usb_hcd *peer = hcd->shared_hcd;
+
+		peer->shared_hcd = NULL;
+		if (peer->primary_hcd == hcd)
+			peer->primary_hcd = NULL;
+	}
+	mutex_unlock(&usb_port_peer_mutex);
 	kfree(hcd);
 }
 
@@ -2589,6 +2600,21 @@ static int usb_hcd_request_irqs(struct usb_hcd *hcd,
 	return 0;
 }
 
+/*
+ * Before we free this root hub, flush in-flight peering attempts
+ * and disable peer lookups
+ */
+static void usb_put_invalidate_rhdev(struct usb_hcd *hcd)
+{
+	struct usb_device *rhdev;
+
+	mutex_lock(&usb_port_peer_mutex);
+	rhdev = hcd->self.root_hub;
+	hcd->self.root_hub = NULL;
+	mutex_unlock(&usb_port_peer_mutex);
+	usb_put_dev(rhdev);
+}
+
 /**
  * usb_add_hcd - finish generic HCD structure initialization and register
  * @hcd: the usb_hcd structure to initialize
@@ -2649,7 +2675,9 @@ int usb_add_hcd(struct usb_hcd *hcd,
 		retval = -ENOMEM;
 		goto err_allocate_root_hub;
 	}
+	mutex_lock(&usb_port_peer_mutex);
 	hcd->self.root_hub = rhdev;
+	mutex_unlock(&usb_port_peer_mutex);
 
 	switch (hcd->speed) {
 	case HCD_USB11:
@@ -2758,7 +2786,7 @@ err_hcd_driver_start:
 err_request_irq:
 err_hcd_driver_setup:
 err_set_rh_speed:
-	usb_put_dev(hcd->self.root_hub);
+	usb_put_invalidate_rhdev(hcd);
 err_allocate_root_hub:
 	usb_deregister_bus(&hcd->self);
 err_register_bus:
@@ -2838,7 +2866,6 @@ void usb_remove_hcd(struct usb_hcd *hcd)
 			free_irq(hcd->irq, hcd);
 	}
 
-	usb_put_dev(hcd->self.root_hub);
 	usb_deregister_bus(&hcd->self);
 	hcd_buffer_destroy(hcd);
 	if (hcd->remove_phy && hcd->phy) {
@@ -2846,6 +2873,8 @@ void usb_remove_hcd(struct usb_hcd *hcd)
 		usb_put_phy(hcd->phy);
 		hcd->phy = NULL;
 	}
+
+	usb_put_invalidate_rhdev(hcd);
 }
 EXPORT_SYMBOL_GPL(usb_remove_hcd);
 
diff --git a/drivers/usb/core/hub.c b/drivers/usb/core/hub.c
index 090469ebfcf..879b66e1337 100644
--- a/drivers/usb/core/hub.c
+++ b/drivers/usb/core/hub.c
@@ -36,11 +36,6 @@
 #define USB_VENDOR_GENESYS_LOGIC		0x05e3
 #define HUB_QUIRK_CHECK_PORT_AUTOSUSPEND	0x01
 
-static inline int hub_is_superspeed(struct usb_device *hdev)
-{
-	return (hdev->descriptor.bDeviceProtocol == USB_HUB_PR_SS);
-}
-
 /* Protect struct usb_device->state and ->children members
  * Note: Both are also protected by ->dev.sem, except that ->state can
  * change to USB_STATE_NOTATTACHED even when the semaphore isn't held. */
@@ -55,6 +50,9 @@ static DECLARE_WAIT_QUEUE_HEAD(khubd_wait);
 
 static struct task_struct *khubd_task;
 
+/* synchronize hub-port add/remove and peering operations */
+DEFINE_MUTEX(usb_port_peer_mutex);
+
 /* cycle leds on hubs that aren't blinking for attention */
 static bool blinkenlights = 0;
 module_param (blinkenlights, bool, S_IRUGO);
@@ -412,30 +410,35 @@ static int set_port_feature(struct usb_device *hdev, int port1, int feature)
 		NULL, 0, 1000);
 }
 
+static char *to_led_name(int selector)
+{
+	switch (selector) {
+	case HUB_LED_AMBER:
+		return "amber";
+	case HUB_LED_GREEN:
+		return "green";
+	case HUB_LED_OFF:
+		return "off";
+	case HUB_LED_AUTO:
+		return "auto";
+	default:
+		return "??";
+	}
+}
+
 /*
  * USB 2.0 spec Section 11.24.2.7.1.10 and table 11-7
  * for info about using port indicators
  */
-static void set_port_led(
-	struct usb_hub *hub,
-	int port1,
-	int selector
-)
+static void set_port_led(struct usb_hub *hub, int port1, int selector)
 {
-	int status = set_port_feature(hub->hdev, (selector << 8) | port1,
+	struct usb_port *port_dev = hub->ports[port1 - 1];
+	int status;
+
+	status = set_port_feature(hub->hdev, (selector << 8) | port1,
 			USB_PORT_FEAT_INDICATOR);
-	if (status < 0)
-		dev_dbg (hub->intfdev,
-			"port %d indicator %s status %d\n",
-			port1,
-			({ char *s; switch (selector) {
-			case HUB_LED_AMBER: s = "amber"; break;
-			case HUB_LED_GREEN: s = "green"; break;
-			case HUB_LED_OFF: s = "off"; break;
-			case HUB_LED_AUTO: s = "auto"; break;
-			default: s = "??"; break;
-			} s; }),
-			status);
+	dev_dbg(&port_dev->dev, "indicator %s status %d\n",
+		to_led_name(selector), status);
 }
 
 #define	LED_CYCLE_PERIOD	((2*HZ)/3)
@@ -743,16 +746,20 @@ int usb_hub_set_port_power(struct usb_device *hdev, struct usb_hub *hub,
 			   int port1, bool set)
 {
 	int ret;
-	struct usb_port *port_dev = hub->ports[port1 - 1];
 
 	if (set)
 		ret = set_port_feature(hdev, port1, USB_PORT_FEAT_POWER);
 	else
 		ret = usb_clear_port_feature(hdev, port1, USB_PORT_FEAT_POWER);
 
-	if (!ret)
-		port_dev->power_is_on = set;
-	return ret;
+	if (ret)
+		return ret;
+
+	if (set)
+		set_bit(port1, hub->power_bits);
+	else
+		clear_bit(port1, hub->power_bits);
+	return 0;
 }
 
 /**
@@ -810,16 +817,9 @@ int usb_hub_clear_tt_buffer(struct urb *urb)
 }
 EXPORT_SYMBOL_GPL(usb_hub_clear_tt_buffer);
 
-/* If do_delay is false, return the number of milliseconds the caller
- * needs to delay.
- */
-static unsigned hub_power_on(struct usb_hub *hub, bool do_delay)
+static void hub_power_on(struct usb_hub *hub, bool do_delay)
 {
 	int port1;
-	unsigned pgood_delay = hub->descriptor->bPwrOn2PwrGood * 2;
-	unsigned delay;
-	u16 wHubCharacteristics =
-			le16_to_cpu(hub->descriptor->wHubCharacteristics);
 
 	/* Enable power on each port.  Some hubs have reserved values
 	 * of LPSM (> 2) in their descriptors, even though they are
@@ -827,23 +827,19 @@ static unsigned hub_power_on(struct usb_hub *hub, bool do_delay)
 	 * but only emulate it.  In all cases, the ports won't work
 	 * unless we send these messages to the hub.
 	 */
-	if ((wHubCharacteristics & HUB_CHAR_LPSM) < 2)
+	if (hub_is_port_power_switchable(hub))
 		dev_dbg(hub->intfdev, "enabling power on all ports\n");
 	else
 		dev_dbg(hub->intfdev, "trying to enable port power on "
 				"non-switchable hub\n");
 	for (port1 = 1; port1 <= hub->hdev->maxchild; port1++)
-		if (hub->ports[port1 - 1]->power_is_on)
+		if (test_bit(port1, hub->power_bits))
 			set_port_feature(hub->hdev, port1, USB_PORT_FEAT_POWER);
 		else
 			usb_clear_port_feature(hub->hdev, port1,
 						USB_PORT_FEAT_POWER);
-
-	/* Wait at least 100 msec for power to become stable */
-	delay = max(pgood_delay, (unsigned) 100);
 	if (do_delay)
-		msleep(delay);
-	return delay;
+		msleep(hub_power_on_good_delay(hub));
 }
 
 static int hub_hub_status(struct usb_hub *hub,
@@ -911,20 +907,20 @@ static int hub_usb3_port_disable(struct usb_hub *hub, int port1)
 		msleep(HUB_DEBOUNCE_STEP);
 	}
 	if (total_time >= HUB_DEBOUNCE_TIMEOUT)
-		dev_warn(hub->intfdev, "Could not disable port %d after %d ms\n",
-				port1, total_time);
+		dev_warn(&hub->ports[port1 - 1]->dev,
+				"Could not disable after %d ms\n", total_time);
 
 	return hub_set_port_link_state(hub, port1, USB_SS_PORT_LS_RX_DETECT);
 }
 
 static int hub_port_disable(struct usb_hub *hub, int port1, int set_state)
 {
+	struct usb_port *port_dev = hub->ports[port1 - 1];
 	struct usb_device *hdev = hub->hdev;
 	int ret = 0;
 
-	if (hub->ports[port1 - 1]->child && set_state)
-		usb_set_device_state(hub->ports[port1 - 1]->child,
-				USB_STATE_NOTATTACHED);
+	if (port_dev->child && set_state)
+		usb_set_device_state(port_dev->child, USB_STATE_NOTATTACHED);
 	if (!hub->error) {
 		if (hub_is_superspeed(hub->hdev))
 			ret = hub_usb3_port_disable(hub, port1);
@@ -933,8 +929,7 @@ static int hub_port_disable(struct usb_hub *hub, int port1, int set_state)
 					USB_PORT_FEAT_ENABLE);
 	}
 	if (ret && ret != -ENODEV)
-		dev_err(hub->intfdev, "cannot disable port %d (err = %d)\n",
-				port1, ret);
+		dev_err(&port_dev->dev, "cannot disable (err = %d)\n", ret);
 	return ret;
 }
 
@@ -945,7 +940,7 @@ static int hub_port_disable(struct usb_hub *hub, int port1, int set_state)
  */
 static void hub_port_logical_disconnect(struct usb_hub *hub, int port1)
 {
-	dev_dbg(hub->intfdev, "logical disconnect on port %d\n", port1);
+	dev_dbg(&hub->ports[port1 - 1]->dev, "logical disconnect\n");
 	hub_port_disable(hub, port1, 1);
 
 	/* FIXME let caller ask to power down the port:
@@ -1048,7 +1043,9 @@ static void hub_activate(struct usb_hub *hub, enum hub_activation_type type)
 		 * for HUB_POST_RESET, but it's easier not to.
 		 */
 		if (type == HUB_INIT) {
-			delay = hub_power_on(hub, false);
+			unsigned delay = hub_power_on_good_delay(hub);
+
+			hub_power_on(hub, false);
 			INIT_DELAYED_WORK(&hub->init_work, hub_init_func2);
 			queue_delayed_work(system_power_efficient_wq,
 					&hub->init_work,
@@ -1083,21 +1080,23 @@ 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 khubd know
 	 * which ports need attention.
 	 */
 	for (port1 = 1; port1 <= hdev->maxchild; ++port1) {
-		struct usb_device *udev = hub->ports[port1 - 1]->child;
+		struct usb_port *port_dev = hub->ports[port1 - 1];
+		struct usb_device *udev = port_dev->child;
 		u16 portstatus, portchange;
 
 		portstatus = portchange = 0;
 		status = hub_port_status(hub, port1, &portstatus, &portchange);
 		if (udev || (portstatus & USB_PORT_STAT_CONNECTION))
-			dev_dbg(hub->intfdev,
-					"port %d: status %04x change %04x\n",
-					port1, portstatus, portchange);
+			dev_dbg(&port_dev->dev, "status %04x change %04x\n",
+					portstatus, portchange);
 
-		/* After anything other than HUB_RESUME (i.e., initialization
+		/*
+		 * After anything other than HUB_RESUME (i.e., initialization
 		 * or any sort of reset), every port should be disabled.
 		 * Unconnected ports should likewise be disabled (paranoia),
 		 * and so should ports for which we have no usb_device.
@@ -1173,15 +1172,13 @@ static void hub_activate(struct usb_hub *hub, enum hub_activation_type type)
 				set_bit(port1, hub->change_bits);
 
 		} else if (udev->persist_enabled) {
-			struct usb_port *port_dev = hub->ports[port1 - 1];
-
 #ifdef CONFIG_PM
 			udev->reset_resume = 1;
 #endif
 			/* Don't set the change_bits when the device
 			 * was powered off.
 			 */
-			if (port_dev->power_is_on)
+			if (test_bit(port1, hub->power_bits))
 				set_bit(port1, hub->change_bits);
 
 		} else {
@@ -1276,12 +1273,22 @@ static void hub_quiesce(struct usb_hub *hub, enum hub_quiescing_type type)
 		flush_work(&hub->tt.clear_work);
 }
 
+static void hub_pm_barrier_for_all_ports(struct usb_hub *hub)
+{
+	int i;
+
+	for (i = 0; i < hub->hdev->maxchild; ++i)
+		pm_runtime_barrier(&hub->ports[i]->dev);
+}
+
 /* caller has locked the hub device */
 static int hub_pre_reset(struct usb_interface *intf)
 {
 	struct usb_hub *hub = usb_get_intfdata(intf);
 
 	hub_quiesce(hub, HUB_PRE_RESET);
+	hub->in_reset = 1;
+	hub_pm_barrier_for_all_ports(hub);
 	return 0;
 }
 
@@ -1290,6 +1297,8 @@ static int hub_post_reset(struct usb_interface *intf)
 {
 	struct usb_hub *hub = usb_get_intfdata(intf);
 
+	hub->in_reset = 0;
+	hub_pm_barrier_for_all_ports(hub);
 	hub_activate(hub, HUB_POST_RESET);
 	return 0;
 }
@@ -1307,6 +1316,7 @@ static int hub_configure(struct usb_hub *hub,
 	char *message = "out of memory";
 	unsigned unit_load;
 	unsigned full_load;
+	unsigned maxchild;
 
 	hub->buffer = kmalloc(sizeof(*hub->buffer), GFP_KERNEL);
 	if (!hub->buffer) {
@@ -1345,12 +1355,11 @@ static int hub_configure(struct usb_hub *hub,
 		goto fail;
 	}
 
-	hdev->maxchild = hub->descriptor->bNbrPorts;
-	dev_info (hub_dev, "%d port%s detected\n", hdev->maxchild,
-		(hdev->maxchild == 1) ? "" : "s");
+	maxchild = hub->descriptor->bNbrPorts;
+	dev_info(hub_dev, "%d port%s detected\n", maxchild,
+			(maxchild == 1) ? "" : "s");
 
-	hub->ports = kzalloc(hdev->maxchild * sizeof(struct usb_port *),
-			     GFP_KERNEL);
+	hub->ports = kzalloc(maxchild * sizeof(struct usb_port *), GFP_KERNEL);
 	if (!hub->ports) {
 		ret = -ENOMEM;
 		goto fail;
@@ -1371,11 +1380,11 @@ static int hub_configure(struct usb_hub *hub,
 		int	i;
 		char	portstr[USB_MAXCHILDREN + 1];
 
-		for (i = 0; i < hdev->maxchild; i++)
+		for (i = 0; i < maxchild; i++)
 			portstr[i] = hub->descriptor->u.hs.DeviceRemovable
 				    [((i + 1) / 8)] & (1 << ((i + 1) % 8))
 				? 'F' : 'R';
-		portstr[hdev->maxchild] = 0;
+		portstr[maxchild] = 0;
 		dev_dbg(hub_dev, "compound device; port removable status: %s\n", portstr);
 	} else
 		dev_dbg(hub_dev, "standalone hub\n");
@@ -1487,7 +1496,7 @@ static int hub_configure(struct usb_hub *hub,
 		if (hcd->power_budget > 0)
 			hdev->bus_mA = hcd->power_budget;
 		else
-			hdev->bus_mA = full_load * hdev->maxchild;
+			hdev->bus_mA = full_load * maxchild;
 		if (hdev->bus_mA >= full_load)
 			hub->mA_per_port = full_load;
 		else {
@@ -1502,7 +1511,7 @@ static int hub_configure(struct usb_hub *hub,
 			hub->descriptor->bHubContrCurrent);
 		hub->limited_power = 1;
 
-		if (remaining < hdev->maxchild * unit_load)
+		if (remaining < maxchild * unit_load)
 			dev_warn(hub_dev,
 					"insufficient power available "
 					"to use all downstream ports\n");
@@ -1570,15 +1579,19 @@ static int hub_configure(struct usb_hub *hub,
 	if (hub->has_indicators && blinkenlights)
 		hub->indicator[0] = INDICATOR_CYCLE;
 
-	for (i = 0; i < hdev->maxchild; i++) {
+	mutex_lock(&usb_port_peer_mutex);
+	for (i = 0; i < maxchild; i++) {
 		ret = usb_hub_create_port_device(hub, i + 1);
 		if (ret < 0) {
 			dev_err(hub->intfdev,
 				"couldn't create port%d device.\n", i + 1);
-			hdev->maxchild = i;
-			goto fail_keep_maxchild;
+			break;
 		}
 	}
+	hdev->maxchild = i;
+	mutex_unlock(&usb_port_peer_mutex);
+	if (ret < 0)
+		goto fail;
 
 	usb_hub_adjust_deviceremovable(hdev, hub->descriptor);
 
@@ -1586,8 +1599,6 @@ static int hub_configure(struct usb_hub *hub,
 	return 0;
 
 fail:
-	hdev->maxchild = 0;
-fail_keep_maxchild:
 	dev_err (hub_dev, "config failed, %s (err %d)\n",
 			message, ret);
 	/* hub_disconnect() frees urb and descriptor */
@@ -1623,6 +1634,8 @@ static void hub_disconnect(struct usb_interface *intf)
 	hub->error = 0;
 	hub_quiesce(hub, HUB_DISCONNECT);
 
+	mutex_lock(&usb_port_peer_mutex);
+
 	/* Avoid races with recursively_mark_NOTATTACHED() */
 	spin_lock_irq(&device_state_lock);
 	port1 = hdev->maxchild;
@@ -1633,6 +1646,8 @@ static void hub_disconnect(struct usb_interface *intf)
 	for (; port1 > 0; --port1)
 		usb_hub_remove_port_device(hub, port1);
 
+	mutex_unlock(&usb_port_peer_mutex);
+
 	if (hub->hdev->speed == USB_SPEED_HIGH)
 		highspeed_hubs--;
 
@@ -1691,8 +1706,19 @@ static int hub_probe(struct usb_interface *intf, const struct usb_device_id *id)
 	 */
 	pm_runtime_set_autosuspend_delay(&hdev->dev, 0);
 
-	/* Hubs have proper suspend/resume support. */
-	usb_enable_autosuspend(hdev);
+	/*
+	 * Hubs have proper suspend/resume support, except for root hubs
+	 * where the controller driver doesn't have bus_suspend and
+	 * bus_resume methods.
+	 */
+	if (hdev->parent) {		/* normal device */
+		usb_enable_autosuspend(hdev);
+	} else {			/* root hub */
+		const struct hc_driver *drv = bus_to_hcd(hdev->bus)->driver;
+
+		if (drv->bus_suspend && drv->bus_resume)
+			usb_enable_autosuspend(hdev);
+	}
 
 	if (hdev->level == MAX_TOPO_LEVEL) {
 		dev_err(&intf->dev,
@@ -2024,6 +2050,18 @@ static void hub_free_dev(struct usb_device *udev)
 		hcd->driver->free_dev(hcd, udev);
 }
 
+static void hub_disconnect_children(struct usb_device *udev)
+{
+	struct usb_hub *hub = usb_hub_to_struct_hub(udev);
+	int i;
+
+	/* Free up all the children before we remove this device */
+	for (i = 0; i < udev->maxchild; i++) {
+		if (hub->ports[i]->child)
+			usb_disconnect(&hub->ports[i]->child);
+	}
+}
+
 /**
  * usb_disconnect - disconnect a device (usbcore-internal)
  * @pdev: pointer to device being disconnected
@@ -2042,9 +2080,10 @@ static void hub_free_dev(struct usb_device *udev)
  */
 void usb_disconnect(struct usb_device **pdev)
 {
-	struct usb_device	*udev = *pdev;
-	struct usb_hub		*hub = usb_hub_to_struct_hub(udev);
-	int			i;
+	struct usb_port *port_dev = NULL;
+	struct usb_device *udev = *pdev;
+	struct usb_hub *hub;
+	int port1;
 
 	/* mark the device as inactive, so any further urb submissions for
 	 * this device (and any of its children) will fail immediately.
@@ -2056,11 +2095,7 @@ void usb_disconnect(struct usb_device **pdev)
 
 	usb_lock_device(udev);
 
-	/* Free up all the children before we remove this device */
-	for (i = 0; i < udev->maxchild; i++) {
-		if (hub->ports[i]->child)
-			usb_disconnect(&hub->ports[i]->child);
-	}
+	hub_disconnect_children(udev);
 
 	/* deallocate hcd/hardware state ... nuking all pending urbs and
 	 * cleaning up all state associated with the current configuration
@@ -2071,16 +2106,19 @@ void usb_disconnect(struct usb_device **pdev)
 	usb_hcd_synchronize_unlinks(udev);
 
 	if (udev->parent) {
-		struct usb_hub *hub = usb_hub_to_struct_hub(udev->parent);
-		struct usb_port	*port_dev = hub->ports[udev->portnum - 1];
+		port1 = udev->portnum;
+		hub = usb_hub_to_struct_hub(udev->parent);
+		port_dev = hub->ports[port1 - 1];
 
 		sysfs_remove_link(&udev->dev.kobj, "port");
 		sysfs_remove_link(&port_dev->dev.kobj, "device");
 
-		if (!port_dev->did_runtime_put)
-			pm_runtime_put(&port_dev->dev);
-		else
-			port_dev->did_runtime_put = false;
+		/*
+		 * As usb_port_runtime_resume() de-references udev, make
+		 * sure no resumes occur during removal
+		 */
+		if (!test_and_set_bit(port1, hub->child_usage_bits))
+			pm_runtime_get_sync(&port_dev->dev);
 	}
 
 	usb_remove_ep_devs(&udev->ep0);
@@ -2102,6 +2140,9 @@ void usb_disconnect(struct usb_device **pdev)
 	*pdev = NULL;
 	spin_unlock_irq(&device_state_lock);
 
+	if (port_dev && test_and_clear_bit(port1, hub->child_usage_bits))
+		pm_runtime_put(&port_dev->dev);
+
 	hub_free_dev(udev);
 
 	put_device(&udev->dev);
@@ -2289,6 +2330,22 @@ static void set_usb_port_removable(struct usb_device *udev)
 		udev->removable = USB_DEVICE_REMOVABLE;
 	else
 		udev->removable = USB_DEVICE_FIXED;
+
+	/*
+	 * Platform firmware may have populated an alternative value for
+	 * removable.  If the parent port has a known connect_type use
+	 * that instead.
+	 */
+	switch (hub->ports[udev->portnum - 1]->connect_type) {
+	case USB_PORT_CONNECT_TYPE_HOT_PLUG:
+		udev->removable = USB_DEVICE_REMOVABLE;
+		break;
+	case USB_PORT_CONNECT_TYPE_HARD_WIRED:
+		udev->removable = USB_DEVICE_FIXED;
+		break;
+	default: /* use what was set above */
+		break;
+	}
 }
 
 /**
@@ -2358,11 +2415,7 @@ int usb_new_device(struct usb_device *udev)
 
 	device_enable_async_suspend(&udev->dev);
 
-	/*
-	 * check whether the hub marks this port as non-removable. Do it
-	 * now so that platform-specific data can override it in
-	 * device_add()
-	 */
+	/* check whether the hub or firmware marks this port as non-removable */
 	if (udev->parent)
 		set_usb_port_removable(udev);
 
@@ -2379,7 +2432,8 @@ int usb_new_device(struct usb_device *udev)
 	/* Create link files between child device and usb port device. */
 	if (udev->parent) {
 		struct usb_hub *hub = usb_hub_to_struct_hub(udev->parent);
-		struct usb_port	*port_dev = hub->ports[udev->portnum - 1];
+		int port1 = udev->portnum;
+		struct usb_port	*port_dev = hub->ports[port1 - 1];
 
 		err = sysfs_create_link(&udev->dev.kobj,
 				&port_dev->dev.kobj, "port");
@@ -2393,7 +2447,8 @@ int usb_new_device(struct usb_device *udev)
 			goto fail;
 		}
 
-		pm_runtime_get_sync(&port_dev->dev);
+		if (!test_and_set_bit(port1, hub->child_usage_bits))
+			pm_runtime_get_sync(&port_dev->dev);
 	}
 
 	(void) usb_create_ep_devs(&udev->dev, &udev->ep0, udev);
@@ -2561,9 +2616,9 @@ static int hub_port_wait_reset(struct usb_hub *hub, int port1,
 		if (delay_time >= 2 * HUB_SHORT_RESET_TIME)
 			delay = HUB_LONG_RESET_TIME;
 
-		dev_dbg (hub->intfdev,
-			"port %d not %sreset yet, waiting %dms\n",
-			port1, warm ? "warm " : "", delay);
+		dev_dbg(&hub->ports[port1 - 1]->dev,
+				"not %sreset yet, waiting %dms\n",
+				warm ? "warm " : "", delay);
 	}
 
 	if ((portstatus & USB_PORT_STAT_RESET))
@@ -2647,6 +2702,7 @@ static int hub_port_reset(struct usb_hub *hub, int port1,
 {
 	int i, status;
 	u16 portchange, portstatus;
+	struct usb_port *port_dev = hub->ports[port1 - 1];
 
 	if (!hub_is_superspeed(hub->hdev)) {
 		if (warm) {
@@ -2680,9 +2736,9 @@ static int hub_port_reset(struct usb_hub *hub, int port1,
 		if (status == -ENODEV) {
 			;	/* The hub is gone */
 		} else if (status) {
-			dev_err(hub->intfdev,
-					"cannot %sreset port %d (err = %d)\n",
-					warm ? "warm " : "", port1, status);
+			dev_err(&port_dev->dev,
+					"cannot %sreset (err = %d)\n",
+					warm ? "warm " : "", status);
 		} else {
 			status = hub_port_wait_reset(hub, port1, udev, delay,
 								warm);
@@ -2715,21 +2771,19 @@ static int hub_port_reset(struct usb_hub *hub, int port1,
 			 * hot or warm reset failed.  Try another warm reset.
 			 */
 			if (!warm) {
-				dev_dbg(hub->intfdev, "hot reset failed, warm reset port %d\n",
-						port1);
+				dev_dbg(&port_dev->dev,
+						"hot reset failed, warm reset\n");
 				warm = true;
 			}
 		}
 
-		dev_dbg (hub->intfdev,
-			"port %d not enabled, trying %sreset again...\n",
-			port1, warm ? "warm " : "");
+		dev_dbg(&port_dev->dev,
+				"not enabled, trying %sreset again...\n",
+				warm ? "warm " : "");
 		delay = HUB_LONG_RESET_TIME;
 	}
 
-	dev_err (hub->intfdev,
-		"Cannot enable port %i.  Maybe the USB cable is bad?\n",
-		port1);
+	dev_err(&port_dev->dev, "Cannot enable. Maybe the USB cable is bad?\n");
 
 done:
 	if (!hub_is_superspeed(hub->hdev))
@@ -2754,6 +2808,20 @@ static int port_is_power_on(struct usb_hub *hub, unsigned portstatus)
 	return ret;
 }
 
+static void usb_lock_port(struct usb_port *port_dev)
+		__acquires(&port_dev->status_lock)
+{
+	mutex_lock(&port_dev->status_lock);
+	__acquire(&port_dev->status_lock);
+}
+
+static void usb_unlock_port(struct usb_port *port_dev)
+		__releases(&port_dev->status_lock)
+{
+	mutex_unlock(&port_dev->status_lock);
+	__release(&port_dev->status_lock);
+}
+
 #ifdef	CONFIG_PM
 
 /* Check if a port is suspended(USB2.0 port) or in U3 state(USB3.0 port) */
@@ -2780,6 +2848,8 @@ static int check_port_resume_type(struct usb_device *udev,
 		struct usb_hub *hub, int port1,
 		int status, unsigned portchange, unsigned portstatus)
 {
+	struct usb_port *port_dev = hub->ports[port1 - 1];
+
 	/* Is the device still present? */
 	if (status || port_is_suspended(hub, portstatus) ||
 			!port_is_power_on(hub, portstatus) ||
@@ -2799,9 +2869,8 @@ static int check_port_resume_type(struct usb_device *udev,
 	}
 
 	if (status) {
-		dev_dbg(hub->intfdev,
-				"port %d status %04x.%04x after resume, %d\n",
-				port1, portchange, portstatus, status);
+		dev_dbg(&port_dev->dev, "status %04x.%04x after resume, %d\n",
+				portchange, portstatus, status);
 	} else if (udev->reset_resume) {
 
 		/* Late port handoff can set status-change bits */
@@ -2975,6 +3044,8 @@ int usb_port_suspend(struct usb_device *udev, pm_message_t msg)
 	int		status;
 	bool		really_suspend = true;
 
+	usb_lock_port(port_dev);
+
 	/* enable remote wakeup when appropriate; this lets the device
 	 * wake up the upstream hub (including maybe the root hub).
 	 *
@@ -3032,8 +3103,7 @@ int usb_port_suspend(struct usb_device *udev, pm_message_t msg)
 		status = 0;
 	}
 	if (status) {
-		dev_dbg(hub->intfdev, "can't suspend port %d, status %d\n",
-				port1, status);
+		dev_dbg(&port_dev->dev, "can't suspend, status %d\n", status);
 
 		/* Try to enable USB3 LPM and LTM again */
 		usb_unlocked_enable_lpm(udev);
@@ -3064,12 +3134,13 @@ int usb_port_suspend(struct usb_device *udev, pm_message_t msg)
 		usb_set_device_state(udev, USB_STATE_SUSPENDED);
 	}
 
-	if (status == 0 && !udev->do_remote_wakeup && udev->persist_enabled) {
+	if (status == 0 && !udev->do_remote_wakeup && udev->persist_enabled
+			&& test_and_clear_bit(port1, hub->child_usage_bits))
 		pm_runtime_put_sync(&port_dev->dev);
-		port_dev->did_runtime_put = true;
-	}
 
 	usb_mark_last_busy(hub->hdev);
+
+	usb_unlock_port(port_dev);
 	return status;
 }
 
@@ -3209,9 +3280,8 @@ int usb_port_resume(struct usb_device *udev, pm_message_t msg)
 	int		status;
 	u16		portchange, portstatus;
 
-	if (port_dev->did_runtime_put) {
+	if (!test_and_set_bit(port1, hub->child_usage_bits)) {
 		status = pm_runtime_get_sync(&port_dev->dev);
-		port_dev->did_runtime_put = false;
 		if (status < 0) {
 			dev_dbg(&udev->dev, "can't resume usb port, status %d\n",
 					status);
@@ -3219,15 +3289,13 @@ int usb_port_resume(struct usb_device *udev, pm_message_t msg)
 		}
 	}
 
+	usb_lock_port(port_dev);
+
 	/* Skip the initial Clear-Suspend step for a remote wakeup */
 	status = hub_port_status(hub, port1, &portstatus, &portchange);
 	if (status == 0 && !port_is_suspended(hub, portstatus))
 		goto SuspendCleared;
 
-	/* dev_dbg(hub->intfdev, "resume port %d\n", port1); */
-
-	set_bit(port1, hub->busy_bits);
-
 	/* see 7.1.7.7; affects power usage, but not budgeting */
 	if (hub_is_superspeed(hub->hdev))
 		status = hub_set_port_link_state(hub, port1, USB_SS_PORT_LS_U0);
@@ -3235,8 +3303,7 @@ int usb_port_resume(struct usb_device *udev, pm_message_t msg)
 		status = usb_clear_port_feature(hub->hdev,
 				port1, USB_PORT_FEAT_SUSPEND);
 	if (status) {
-		dev_dbg(hub->intfdev, "can't resume port %d, status %d\n",
-				port1, status);
+		dev_dbg(&port_dev->dev, "can't resume, status %d\n", status);
 	} else {
 		/* drive resume for at least 20 msec */
 		dev_dbg(&udev->dev, "usb %sresume\n",
@@ -3267,8 +3334,6 @@ int usb_port_resume(struct usb_device *udev, pm_message_t msg)
 		}
 	}
 
-	clear_bit(port1, hub->busy_bits);
-
 	status = check_port_resume_type(udev,
 			hub, port1, status, portchange, portstatus);
 	if (status == 0)
@@ -3286,16 +3351,18 @@ int usb_port_resume(struct usb_device *udev, pm_message_t msg)
 		usb_unlocked_enable_lpm(udev);
 	}
 
+	usb_unlock_port(port_dev);
+
 	return status;
 }
 
 #ifdef	CONFIG_PM_RUNTIME
 
-/* caller has locked udev */
 int usb_remote_wakeup(struct usb_device *udev)
 {
 	int	status = 0;
 
+	usb_lock_device(udev);
 	if (udev->state == USB_STATE_SUSPENDED) {
 		dev_dbg(&udev->dev, "usb %sresume\n", "wakeup-");
 		status = usb_autoresume_device(udev);
@@ -3304,9 +3371,59 @@ int usb_remote_wakeup(struct usb_device *udev)
 			usb_autosuspend_device(udev);
 		}
 	}
+	usb_unlock_device(udev);
 	return status;
 }
 
+/* Returns 1 if there was a remote wakeup and a connect status change. */
+static int hub_handle_remote_wakeup(struct usb_hub *hub, unsigned int port,
+		u16 portstatus, u16 portchange)
+		__must_hold(&port_dev->status_lock)
+{
+	struct usb_port *port_dev = hub->ports[port - 1];
+	struct usb_device *hdev;
+	struct usb_device *udev;
+	int connect_change = 0;
+	int ret;
+
+	hdev = hub->hdev;
+	udev = port_dev->child;
+	if (!hub_is_superspeed(hdev)) {
+		if (!(portchange & USB_PORT_STAT_C_SUSPEND))
+			return 0;
+		usb_clear_port_feature(hdev, port, USB_PORT_FEAT_C_SUSPEND);
+	} else {
+		if (!udev || udev->state != USB_STATE_SUSPENDED ||
+				 (portstatus & USB_PORT_STAT_LINK_STATE) !=
+				 USB_SS_PORT_LS_U0)
+			return 0;
+	}
+
+	if (udev) {
+		/* TRSMRCY = 10 msec */
+		msleep(10);
+
+		usb_unlock_port(port_dev);
+		ret = usb_remote_wakeup(udev);
+		usb_lock_port(port_dev);
+		if (ret < 0)
+			connect_change = 1;
+	} else {
+		ret = -ENODEV;
+		hub_port_disable(hub, port, 1);
+	}
+	dev_dbg(&port_dev->dev, "resume, status %d\n", ret);
+	return connect_change;
+}
+
+#else
+
+static int hub_handle_remote_wakeup(struct usb_hub *hub, unsigned int port,
+		u16 portstatus, u16 portchange)
+{
+	return 0;
+}
+
 #endif
 
 static int check_ports_changed(struct usb_hub *hub)
@@ -3337,12 +3454,11 @@ static int hub_suspend(struct usb_interface *intf, pm_message_t msg)
 	 */
 	hub->wakeup_enabled_descendants = 0;
 	for (port1 = 1; port1 <= hdev->maxchild; port1++) {
-		struct usb_device	*udev;
+		struct usb_port *port_dev = hub->ports[port1 - 1];
+		struct usb_device *udev = port_dev->child;
 
-		udev = hub->ports[port1 - 1]->child;
 		if (udev && udev->can_submit) {
-			dev_warn(&intf->dev, "port %d not suspended yet\n",
-					port1);
+			dev_warn(&port_dev->dev, "not suspended yet\n");
 			if (PMSG_IS_AUTO(msg))
 				return -EBUSY;
 		}
@@ -3861,6 +3977,12 @@ EXPORT_SYMBOL_GPL(usb_disable_ltm);
 void usb_enable_ltm(struct usb_device *udev) { }
 EXPORT_SYMBOL_GPL(usb_enable_ltm);
 
+static int hub_handle_remote_wakeup(struct usb_hub *hub, unsigned int port,
+		u16 portstatus, u16 portchange)
+{
+	return 0;
+}
+
 #endif	/* CONFIG_PM */
 
 
@@ -3882,9 +4004,10 @@ EXPORT_SYMBOL_GPL(usb_enable_ltm);
 int hub_port_debounce(struct usb_hub *hub, int port1, bool must_be_connected)
 {
 	int ret;
-	int total_time, stable_time = 0;
 	u16 portchange, portstatus;
 	unsigned connection = 0xffff;
+	int total_time, stable_time = 0;
+	struct usb_port *port_dev = hub->ports[port1 - 1];
 
 	for (total_time = 0; ; total_time += HUB_DEBOUNCE_STEP) {
 		ret = hub_port_status(hub, port1, &portstatus, &portchange);
@@ -3913,9 +4036,8 @@ int hub_port_debounce(struct usb_hub *hub, int port1, bool must_be_connected)
 		msleep(HUB_DEBOUNCE_STEP);
 	}
 
-	dev_dbg (hub->intfdev,
-		"debounce: port %d: total %dms stable %dms status 0x%x\n",
-		port1, total_time, stable_time, portstatus);
+	dev_dbg(&port_dev->dev, "debounce total %dms stable %dms status 0x%x\n",
+			total_time, stable_time, portstatus);
 
 	if (stable_time < HUB_DEBOUNCE_STABLE)
 		return -ETIMEDOUT;
@@ -3974,13 +4096,14 @@ static int hub_set_address(struct usb_device *udev, int devnum)
  */
 static void hub_set_initial_usb2_lpm_policy(struct usb_device *udev)
 {
-	int connect_type;
+	struct usb_hub *hub = usb_hub_to_struct_hub(udev->parent);
+	int connect_type = USB_PORT_CONNECT_TYPE_UNKNOWN;
 
 	if (!udev->usb2_hw_lpm_capable)
 		return;
 
-	connect_type = usb_get_hub_port_connect_type(udev->parent,
-			udev->portnum);
+	if (hub)
+		connect_type = hub->ports[udev->portnum - 1]->connect_type;
 
 	if ((udev->bos->ext_cap->bmAttributes & cpu_to_le32(USB_BESL_SUPPORT)) ||
 			connect_type == USB_PORT_CONNECT_TYPE_HARD_WIRED) {
@@ -4008,16 +4131,15 @@ static int hub_enable_device(struct usb_device *udev)
  * Returns device in USB_STATE_ADDRESS, except on error.
  *
  * If this is called for an already-existing device (as part of
- * usb_reset_and_verify_device), the caller must own the device lock.  For a
- * newly detected device that is not accessible through any global
- * pointers, it's not necessary to lock the device.
+ * usb_reset_and_verify_device), the caller must own the device lock and
+ * the port lock.  For a newly detected device that is not accessible
+ * through any global pointers, it's not necessary to lock the device,
+ * but it is still necessary to lock the port.
  */
 static int
 hub_port_init (struct usb_hub *hub, struct usb_device *udev, int port1,
 		int retry_counter)
 {
-	static DEFINE_MUTEX(usb_address0_mutex);
-
 	struct usb_device	*hdev = hub->hdev;
 	struct usb_hcd		*hcd = bus_to_hcd(hdev->bus);
 	int			i, j, retval;
@@ -4040,7 +4162,7 @@ hub_port_init (struct usb_hub *hub, struct usb_device *udev, int port1,
 	if (oldspeed == USB_SPEED_LOW)
 		delay = HUB_LONG_RESET_TIME;
 
-	mutex_lock(&usb_address0_mutex);
+	mutex_lock(&hdev->bus->usb_address0_mutex);
 
 	/* Reset the device; full speed may morph to high speed */
 	/* FIXME a USB 2.0 device may morph into SuperSpeed on reset. */
@@ -4317,7 +4439,7 @@ fail:
 		hub_port_disable(hub, port1, 0);
 		update_devnum(udev, devnum);	/* for disconnect processing */
 	}
-	mutex_unlock(&usb_address0_mutex);
+	mutex_unlock(&hdev->bus->usb_address0_mutex);
 	return retval;
 }
 
@@ -4358,9 +4480,10 @@ hub_power_remaining (struct usb_hub *hub)
 
 	remaining = hdev->bus_mA - hub->descriptor->bHubContrCurrent;
 	for (port1 = 1; port1 <= hdev->maxchild; ++port1) {
-		struct usb_device	*udev = hub->ports[port1 - 1]->child;
-		int			delta;
-		unsigned		unit_load;
+		struct usb_port *port_dev = hub->ports[port1 - 1];
+		struct usb_device *udev = port_dev->child;
+		unsigned unit_load;
+		int delta;
 
 		if (!udev)
 			continue;
@@ -4380,9 +4503,8 @@ hub_power_remaining (struct usb_hub *hub)
 		else
 			delta = 8;
 		if (delta > hub->mA_per_port)
-			dev_warn(&udev->dev,
-				 "%dmA is over %umA budget for port %d!\n",
-				 delta, hub->mA_per_port, port1);
+			dev_warn(&port_dev->dev, "%dmA is over %umA budget!\n",
+					delta, hub->mA_per_port);
 		remaining -= delta;
 	}
 	if (remaining < 0) {
@@ -4393,78 +4515,23 @@ hub_power_remaining (struct usb_hub *hub)
 	return remaining;
 }
 
-/* Handle physical or logical connection change events.
- * This routine is called when:
- * 	a port connection-change occurs;
- *	a port enable-change occurs (often caused by EMI);
- *	usb_reset_and_verify_device() encounters changed descriptors (as from
- *		a firmware download)
- * caller already locked the hub
- */
-static void hub_port_connect_change(struct usb_hub *hub, int port1,
-					u16 portstatus, u16 portchange)
+static void hub_port_connect(struct usb_hub *hub, int port1, u16 portstatus,
+		u16 portchange)
 {
-	struct usb_device *hdev = hub->hdev;
-	struct device *hub_dev = hub->intfdev;
-	struct usb_hcd *hcd = bus_to_hcd(hdev->bus);
-	unsigned wHubCharacteristics =
-			le16_to_cpu(hub->descriptor->wHubCharacteristics);
-	struct usb_device *udev;
 	int status, i;
 	unsigned unit_load;
-
-	dev_dbg (hub_dev,
-		"port %d, status %04x, change %04x, %s\n",
-		port1, portstatus, portchange, portspeed(hub, portstatus));
-
-	if (hub->has_indicators) {
-		set_port_led(hub, port1, HUB_LED_AUTO);
-		hub->indicator[port1-1] = INDICATOR_AUTO;
-	}
-
-#ifdef	CONFIG_USB_OTG
-	/* during HNP, don't repeat the debounce */
-	if (hdev->bus->is_b_host)
-		portchange &= ~(USB_PORT_STAT_C_CONNECTION |
-				USB_PORT_STAT_C_ENABLE);
-#endif
-
-	/* Try to resuscitate an existing device */
-	udev = hub->ports[port1 - 1]->child;
-	if ((portstatus & USB_PORT_STAT_CONNECTION) && udev &&
-			udev->state != USB_STATE_NOTATTACHED) {
-		usb_lock_device(udev);
-		if (portstatus & USB_PORT_STAT_ENABLE) {
-			status = 0;		/* Nothing to do */
-
-#ifdef CONFIG_PM_RUNTIME
-		} else if (udev->state == USB_STATE_SUSPENDED &&
-				udev->persist_enabled) {
-			/* For a suspended device, treat this as a
-			 * remote wakeup event.
-			 */
-			status = usb_remote_wakeup(udev);
-#endif
-
-		} else {
-			status = -ENODEV;	/* Don't resuscitate */
-		}
-		usb_unlock_device(udev);
-
-		if (status == 0) {
-			clear_bit(port1, hub->change_bits);
-			return;
-		}
-	}
+	struct usb_device *hdev = hub->hdev;
+	struct usb_hcd *hcd = bus_to_hcd(hdev->bus);
+	struct usb_port *port_dev = hub->ports[port1 - 1];
+	struct usb_device *udev = port_dev->child;
 
 	/* Disconnect any existing devices under this port */
 	if (udev) {
 		if (hcd->phy && !hdev->parent &&
 				!(portstatus & USB_PORT_STAT_CONNECTION))
 			usb_phy_notify_disconnect(hcd->phy, udev->speed);
-		usb_disconnect(&hub->ports[port1 - 1]->child);
+		usb_disconnect(&port_dev->child);
 	}
-	clear_bit(port1, hub->change_bits);
 
 	/* We can forget about a "removed" device when there's a physical
 	 * disconnect or the connect status changes.
@@ -4478,8 +4545,8 @@ static void hub_port_connect_change(struct usb_hub *hub, int port1,
 		status = hub_port_debounce_be_stable(hub, port1);
 		if (status < 0) {
 			if (status != -ENODEV && printk_ratelimit())
-				dev_err(hub_dev, "connect-debounce failed, "
-						"port %d disabled\n", port1);
+				dev_err(&port_dev->dev,
+						"connect-debounce failed\n");
 			portstatus &= ~USB_PORT_STAT_CONNECTION;
 		} else {
 			portstatus = status;
@@ -4493,7 +4560,7 @@ static void hub_port_connect_change(struct usb_hub *hub, int port1,
 			test_bit(port1, hub->removed_bits)) {
 
 		/* maybe switch power back on (e.g. root hub was reset) */
-		if ((wHubCharacteristics & HUB_CHAR_LPSM) < 2
+		if (hub_is_port_power_switchable(hub)
 				&& !port_is_power_on(hub, portstatus))
 			set_port_feature(hdev, port1, USB_PORT_FEAT_POWER);
 
@@ -4514,9 +4581,8 @@ static void hub_port_connect_change(struct usb_hub *hub, int port1,
 		 */
 		udev = usb_alloc_dev(hdev, hdev->bus, port1);
 		if (!udev) {
-			dev_err (hub_dev,
-				"couldn't allocate port %d usb_device\n",
-				port1);
+			dev_err(&port_dev->dev,
+					"couldn't allocate usb_device\n");
 			goto done;
 		}
 
@@ -4538,7 +4604,9 @@ static void hub_port_connect_change(struct usb_hub *hub, int port1,
 		}
 
 		/* reset (non-USB 3.0 devices) and get descriptor */
+		usb_lock_port(port_dev);
 		status = hub_port_init(hub, udev, port1, i);
+		usb_unlock_port(port_dev);
 		if (status < 0)
 			goto loop;
 
@@ -4590,6 +4658,8 @@ static void hub_port_connect_change(struct usb_hub *hub, int port1,
 		 */
 		status = 0;
 
+		mutex_lock(&usb_port_peer_mutex);
+
 		/* We mustn't add new devices if the parent hub has
 		 * been disconnected; we would race with the
 		 * recursively_mark_NOTATTACHED() routine.
@@ -4598,16 +4668,19 @@ static void hub_port_connect_change(struct usb_hub *hub, int port1,
 		if (hdev->state == USB_STATE_NOTATTACHED)
 			status = -ENOTCONN;
 		else
-			hub->ports[port1 - 1]->child = udev;
+			port_dev->child = udev;
 		spin_unlock_irq(&device_state_lock);
+		mutex_unlock(&usb_port_peer_mutex);
 
 		/* Run it through the hoops (find a driver, etc) */
 		if (!status) {
 			status = usb_new_device(udev);
 			if (status) {
+				mutex_lock(&usb_port_peer_mutex);
 				spin_lock_irq(&device_state_lock);
-				hub->ports[port1 - 1]->child = NULL;
+				port_dev->child = NULL;
 				spin_unlock_irq(&device_state_lock);
+				mutex_unlock(&usb_port_peer_mutex);
 			}
 		}
 
@@ -4616,7 +4689,7 @@ static void hub_port_connect_change(struct usb_hub *hub, int port1,
 
 		status = hub_power_remaining(hub);
 		if (status)
-			dev_dbg(hub_dev, "%dmA power budget left\n", status);
+			dev_dbg(hub->intfdev, "%dmA power budget left\n", status);
 
 		return;
 
@@ -4634,56 +4707,200 @@ loop:
 			!hcd->driver->port_handed_over ||
 			!(hcd->driver->port_handed_over)(hcd, port1)) {
 		if (status != -ENOTCONN && status != -ENODEV)
-			dev_err(hub_dev, "unable to enumerate USB device on port %d\n",
-					port1);
+			dev_err(&port_dev->dev,
+					"unable to enumerate USB device\n");
 	}
 
 done:
 	hub_port_disable(hub, port1, 1);
 	if (hcd->driver->relinquish_port && !hub->hdev->parent)
 		hcd->driver->relinquish_port(hcd, port1);
+
 }
 
-/* Returns 1 if there was a remote wakeup and a connect status change. */
-static int hub_handle_remote_wakeup(struct usb_hub *hub, unsigned int port,
-		u16 portstatus, u16 portchange)
+/* Handle physical or logical connection change events.
+ * This routine is called when:
+ *	a port connection-change occurs;
+ *	a port enable-change occurs (often caused by EMI);
+ *	usb_reset_and_verify_device() encounters changed descriptors (as from
+ *		a firmware download)
+ * caller already locked the hub
+ */
+static void hub_port_connect_change(struct usb_hub *hub, int port1,
+					u16 portstatus, u16 portchange)
+		__must_hold(&port_dev->status_lock)
 {
-	struct usb_device *hdev;
-	struct usb_device *udev;
-	int connect_change = 0;
-	int ret;
+	struct usb_port *port_dev = hub->ports[port1 - 1];
+	struct usb_device *udev = port_dev->child;
+	int status = -ENODEV;
 
-	hdev = hub->hdev;
-	udev = hub->ports[port - 1]->child;
-	if (!hub_is_superspeed(hdev)) {
-		if (!(portchange & USB_PORT_STAT_C_SUSPEND))
-			return 0;
-		usb_clear_port_feature(hdev, port, USB_PORT_FEAT_C_SUSPEND);
-	} else {
-		if (!udev || udev->state != USB_STATE_SUSPENDED ||
-				 (portstatus & USB_PORT_STAT_LINK_STATE) !=
-				 USB_SS_PORT_LS_U0)
-			return 0;
+	dev_dbg(&port_dev->dev, "status %04x, change %04x, %s\n", portstatus,
+			portchange, portspeed(hub, portstatus));
+
+	if (hub->has_indicators) {
+		set_port_led(hub, port1, HUB_LED_AUTO);
+		hub->indicator[port1-1] = INDICATOR_AUTO;
 	}
 
-	if (udev) {
-		/* TRSMRCY = 10 msec */
-		msleep(10);
+#ifdef	CONFIG_USB_OTG
+	/* during HNP, don't repeat the debounce */
+	if (hub->hdev->bus->is_b_host)
+		portchange &= ~(USB_PORT_STAT_C_CONNECTION |
+				USB_PORT_STAT_C_ENABLE);
+#endif
 
+	/* Try to resuscitate an existing device */
+	if ((portstatus & USB_PORT_STAT_CONNECTION) && udev &&
+			udev->state != USB_STATE_NOTATTACHED) {
+		if (portstatus & USB_PORT_STAT_ENABLE) {
+			status = 0;		/* Nothing to do */
+#ifdef CONFIG_PM_RUNTIME
+		} else if (udev->state == USB_STATE_SUSPENDED &&
+				udev->persist_enabled) {
+			/* For a suspended device, treat this as a
+			 * remote wakeup event.
+			 */
+			usb_unlock_port(port_dev);
+			status = usb_remote_wakeup(udev);
+			usb_lock_port(port_dev);
+#endif
+		} else {
+			/* Don't resuscitate */;
+		}
+	}
+	clear_bit(port1, hub->change_bits);
+
+	/* successfully revalidated the connection */
+	if (status == 0)
+		return;
+
+	usb_unlock_port(port_dev);
+	hub_port_connect(hub, port1, portstatus, portchange);
+	usb_lock_port(port_dev);
+}
+
+static void port_event(struct usb_hub *hub, int port1)
+		__must_hold(&port_dev->status_lock)
+{
+	int connect_change, reset_device = 0;
+	struct usb_port *port_dev = hub->ports[port1 - 1];
+	struct usb_device *udev = port_dev->child;
+	struct usb_device *hdev = hub->hdev;
+	u16 portstatus, portchange;
+
+	connect_change = test_bit(port1, hub->change_bits);
+	clear_bit(port1, hub->event_bits);
+	clear_bit(port1, hub->wakeup_bits);
+
+	if (hub_port_status(hub, port1, &portstatus, &portchange) < 0)
+		return;
+
+	if (portchange & USB_PORT_STAT_C_CONNECTION) {
+		usb_clear_port_feature(hdev, port1, USB_PORT_FEAT_C_CONNECTION);
+		connect_change = 1;
+	}
+
+	if (portchange & USB_PORT_STAT_C_ENABLE) {
+		if (!connect_change)
+			dev_dbg(&port_dev->dev, "enable change, status %08x\n",
+					portstatus);
+		usb_clear_port_feature(hdev, port1, USB_PORT_FEAT_C_ENABLE);
+
+		/*
+		 * EM interference sometimes causes badly shielded USB devices
+		 * to be shutdown by the hub, this hack enables them again.
+		 * Works at least with mouse driver.
+		 */
+		if (!(portstatus & USB_PORT_STAT_ENABLE)
+		    && !connect_change && udev) {
+			dev_err(&port_dev->dev, "disabled by hub (EMI?), re-enabling...\n");
+			connect_change = 1;
+		}
+	}
+
+	if (portchange & USB_PORT_STAT_C_OVERCURRENT) {
+		u16 status = 0, unused;
+
+		dev_dbg(&port_dev->dev, "over-current change\n");
+		usb_clear_port_feature(hdev, port1,
+				USB_PORT_FEAT_C_OVER_CURRENT);
+		msleep(100);	/* Cool down */
+		hub_power_on(hub, true);
+		hub_port_status(hub, port1, &status, &unused);
+		if (status & USB_PORT_STAT_OVERCURRENT)
+			dev_err(&port_dev->dev, "over-current condition\n");
+	}
+
+	if (portchange & USB_PORT_STAT_C_RESET) {
+		dev_dbg(&port_dev->dev, "reset change\n");
+		usb_clear_port_feature(hdev, port1, USB_PORT_FEAT_C_RESET);
+	}
+	if ((portchange & USB_PORT_STAT_C_BH_RESET)
+	    && hub_is_superspeed(hdev)) {
+		dev_dbg(&port_dev->dev, "warm reset change\n");
+		usb_clear_port_feature(hdev, port1,
+				USB_PORT_FEAT_C_BH_PORT_RESET);
+	}
+	if (portchange & USB_PORT_STAT_C_LINK_STATE) {
+		dev_dbg(&port_dev->dev, "link state change\n");
+		usb_clear_port_feature(hdev, port1,
+				USB_PORT_FEAT_C_PORT_LINK_STATE);
+	}
+	if (portchange & USB_PORT_STAT_C_CONFIG_ERROR) {
+		dev_warn(&port_dev->dev, "config error\n");
+		usb_clear_port_feature(hdev, port1,
+				USB_PORT_FEAT_C_PORT_CONFIG_ERROR);
+	}
+
+	/* skip port actions that require the port to be powered on */
+	if (!pm_runtime_active(&port_dev->dev))
+		return;
+
+	if (hub_handle_remote_wakeup(hub, port1, portstatus, portchange))
+		connect_change = 1;
+
+	/*
+	 * Warm reset a USB3 protocol port if it's in
+	 * SS.Inactive state.
+	 */
+	if (hub_port_warm_reset_required(hub, portstatus)) {
+		dev_dbg(&port_dev->dev, "do warm reset\n");
+		if (!udev || !(portstatus & USB_PORT_STAT_CONNECTION)
+				|| udev->state == USB_STATE_NOTATTACHED) {
+			if (hub_port_reset(hub, port1, NULL,
+					HUB_BH_RESET_TIME, true) < 0)
+				hub_port_disable(hub, port1, 1);
+		} else
+			reset_device = 1;
+	}
+
+	/*
+	 * 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
+	 * device may be an unknown state.
+	 *
+	 * If the port went through SS.Inactive without khubd
+	 * seeing it the C_LINK_STATE change flag will be set,
+	 * and we reset the dev to put it in a known state.
+	 */
+	if (reset_device || (udev && hub_is_superspeed(hub->hdev)
+				&& (portchange & USB_PORT_STAT_C_LINK_STATE)
+				&& (portstatus & USB_PORT_STAT_CONNECTION))) {
+		usb_unlock_port(port_dev);
 		usb_lock_device(udev);
-		ret = usb_remote_wakeup(udev);
+		usb_reset_device(udev);
 		usb_unlock_device(udev);
-		if (ret < 0)
-			connect_change = 1;
-	} else {
-		ret = -ENODEV;
-		hub_port_disable(hub, port, 1);
+		usb_lock_port(port_dev);
+		connect_change = 0;
 	}
-	dev_dbg(hub->intfdev, "resume on port %d, status %d\n",
-			port, ret);
-	return connect_change;
+
+	if (connect_change)
+		hub_port_connect_change(hub, port1, portstatus, portchange);
 }
 
+
 static void hub_events(void)
 {
 	struct list_head *tmp;
@@ -4693,10 +4910,7 @@ static void hub_events(void)
 	struct device *hub_dev;
 	u16 hubstatus;
 	u16 hubchange;
-	u16 portstatus;
-	u16 portchange;
 	int i, ret;
-	int connect_change, wakeup_change;
 
 	/*
 	 *  We restart the list every time to avoid a deadlock with
@@ -4770,146 +4984,28 @@ static void hub_events(void)
 
 		/* deal with port status changes */
 		for (i = 1; i <= hdev->maxchild; i++) {
-			struct usb_device *udev = hub->ports[i - 1]->child;
-
-			if (test_bit(i, hub->busy_bits))
-				continue;
-			connect_change = test_bit(i, hub->change_bits);
-			wakeup_change = test_and_clear_bit(i, hub->wakeup_bits);
-			if (!test_and_clear_bit(i, hub->event_bits) &&
-					!connect_change && !wakeup_change)
-				continue;
-
-			ret = hub_port_status(hub, i,
-					&portstatus, &portchange);
-			if (ret < 0)
-				continue;
-
-			if (portchange & USB_PORT_STAT_C_CONNECTION) {
-				usb_clear_port_feature(hdev, i,
-					USB_PORT_FEAT_C_CONNECTION);
-				connect_change = 1;
-			}
-
-			if (portchange & USB_PORT_STAT_C_ENABLE) {
-				if (!connect_change)
-					dev_dbg (hub_dev,
-						"port %d enable change, "
-						"status %08x\n",
-						i, portstatus);
-				usb_clear_port_feature(hdev, i,
-					USB_PORT_FEAT_C_ENABLE);
+			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)) {
 				/*
-				 * EM interference sometimes causes badly
-				 * shielded USB devices to be shutdown by
-				 * the hub, this hack enables them again.
-				 * Works at least with mouse driver.
+				 * 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.
 				 */
-				if (!(portstatus & USB_PORT_STAT_ENABLE)
-				    && !connect_change
-				    && hub->ports[i - 1]->child) {
-					dev_err (hub_dev,
-					    "port %i "
-					    "disabled by hub (EMI?), "
-					    "re-enabling...\n",
-						i);
-					connect_change = 1;
-				}
-			}
-
-			if (hub_handle_remote_wakeup(hub, i,
-						portstatus, portchange))
-				connect_change = 1;
-
-			if (portchange & USB_PORT_STAT_C_OVERCURRENT) {
-				u16 status = 0;
-				u16 unused;
-
-				dev_dbg(hub_dev, "over-current change on port "
-					"%d\n", i);
-				usb_clear_port_feature(hdev, i,
-					USB_PORT_FEAT_C_OVER_CURRENT);
-				msleep(100);	/* Cool down */
-				hub_power_on(hub, true);
-				hub_port_status(hub, i, &status, &unused);
-				if (status & USB_PORT_STAT_OVERCURRENT)
-					dev_err(hub_dev, "over-current "
-						"condition on port %d\n", i);
-			}
-
-			if (portchange & USB_PORT_STAT_C_RESET) {
-				dev_dbg (hub_dev,
-					"reset change on port %d\n",
-					i);
-				usb_clear_port_feature(hdev, i,
-					USB_PORT_FEAT_C_RESET);
-			}
-			if ((portchange & USB_PORT_STAT_C_BH_RESET) &&
-					hub_is_superspeed(hub->hdev)) {
-				dev_dbg(hub_dev,
-					"warm reset change on port %d\n",
-					i);
-				usb_clear_port_feature(hdev, i,
-					USB_PORT_FEAT_C_BH_PORT_RESET);
-			}
-			if (portchange & USB_PORT_STAT_C_LINK_STATE) {
-				usb_clear_port_feature(hub->hdev, i,
-						USB_PORT_FEAT_C_PORT_LINK_STATE);
+				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);
 			}
-			if (portchange & USB_PORT_STAT_C_CONFIG_ERROR) {
-				dev_warn(hub_dev,
-					"config error on port %d\n",
-					i);
-				usb_clear_port_feature(hub->hdev, i,
-						USB_PORT_FEAT_C_PORT_CONFIG_ERROR);
-			}
-
-			/* Warm reset a USB3 protocol port if it's in
-			 * SS.Inactive state.
-			 */
-			if (hub_port_warm_reset_required(hub, portstatus)) {
-				int status;
-
-				dev_dbg(hub_dev, "warm reset port %d\n", i);
-				if (!udev ||
-				    !(portstatus & USB_PORT_STAT_CONNECTION) ||
-				    udev->state == USB_STATE_NOTATTACHED) {
-					status = hub_port_reset(hub, i,
-							NULL, HUB_BH_RESET_TIME,
-							true);
-					if (status < 0)
-						hub_port_disable(hub, i, 1);
-				} else {
-					usb_lock_device(udev);
-					status = usb_reset_device(udev);
-					usb_unlock_device(udev);
-					connect_change = 0;
-				}
-			/*
-			 * 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
-			 * device may be an unknown state.
-			 *
-			 * If the port went through SS.Inactive without khubd
-			 * seeing it the C_LINK_STATE change flag will be set,
-			 * and we reset the dev to put it in a known state.
-			 */
-			} else if (udev && hub_is_superspeed(hub->hdev) &&
-				   (portchange & USB_PORT_STAT_C_LINK_STATE) &&
-				   (portstatus & USB_PORT_STAT_CONNECTION)) {
-				usb_lock_device(udev);
-				usb_reset_device(udev);
-				usb_unlock_device(udev);
-				connect_change = 0;
-			}
-
-			if (connect_change)
-				hub_port_connect_change(hub, i,
-						portstatus, portchange);
-		} /* end for i */
+		}
 
 		/* deal with hub status changes */
 		if (test_and_clear_bit(0, hub->event_bits) == 0)
@@ -5144,15 +5240,18 @@ static int descriptors_changed(struct usb_device *udev,
  * if the reset wasn't even attempted.
  *
  * Note:
- * The caller must own the device lock.  For example, it's safe to use
- * this from a driver probe() routine after downloading new firmware.
- * For calls that might not occur during probe(), drivers should lock
- * the device using usb_lock_device_for_reset().
+ * The caller must own the device lock and the port lock, the latter is
+ * taken by usb_reset_device().  For example, it's safe to use
+ * usb_reset_device() from a driver probe() routine after downloading
+ * new firmware.  For calls that might not occur during probe(), drivers
+ * should lock the device using usb_lock_device_for_reset().
  *
  * Locking exception: This routine may also be called from within an
  * autoresume handler.  Such usage won't conflict with other tasks
  * holding the device lock because these tasks should always call
- * usb_autopm_resume_device(), thereby preventing any unwanted autoresume.
+ * usb_autopm_resume_device(), thereby preventing any unwanted
+ * autoresume.  The autoresume handler is expected to have already
+ * acquired the port lock before calling this routine.
  */
 static int usb_reset_and_verify_device(struct usb_device *udev)
 {
@@ -5171,11 +5270,9 @@ static int usb_reset_and_verify_device(struct usb_device *udev)
 		return -EINVAL;
 	}
 
-	if (!parent_hdev) {
-		/* this requires hcd-specific logic; see ohci_restart() */
-		dev_dbg(&udev->dev, "%s for root hub!\n", __func__);
+	if (!parent_hdev)
 		return -EISDIR;
-	}
+
 	parent_hub = usb_hub_to_struct_hub(parent_hdev);
 
 	/* Disable USB2 hardware LPM.
@@ -5204,7 +5301,6 @@ static int usb_reset_and_verify_device(struct usb_device *udev)
 		goto re_enumerate;
 	}
 
-	set_bit(port1, parent_hub->busy_bits);
 	for (i = 0; i < SET_CONFIG_TRIES; ++i) {
 
 		/* ep0 maxpacket size may change; let the HCD know about it.
@@ -5214,7 +5310,6 @@ static int usb_reset_and_verify_device(struct usb_device *udev)
 		if (ret >= 0 || ret == -ENOTCONN || ret == -ENODEV)
 			break;
 	}
-	clear_bit(port1, parent_hub->busy_bits);
 
 	if (ret < 0)
 		goto re_enumerate;
@@ -5335,7 +5430,9 @@ int usb_reset_device(struct usb_device *udev)
 	int ret;
 	int i;
 	unsigned int noio_flag;
+	struct usb_port *port_dev;
 	struct usb_host_config *config = udev->actconfig;
+	struct usb_hub *hub = usb_hub_to_struct_hub(udev->parent);
 
 	if (udev->state == USB_STATE_NOTATTACHED ||
 			udev->state == USB_STATE_SUSPENDED) {
@@ -5344,6 +5441,14 @@ int usb_reset_device(struct usb_device *udev)
 		return -EINVAL;
 	}
 
+	if (!udev->parent) {
+		/* this requires hcd-specific logic; see ohci_restart() */
+		dev_dbg(&udev->dev, "%s for root hub!\n", __func__);
+		return -EISDIR;
+	}
+
+	port_dev = hub->ports[udev->portnum - 1];
+
 	/*
 	 * Don't allocate memory with GFP_KERNEL in current
 	 * context to avoid possible deadlock if usb mass
@@ -5377,7 +5482,9 @@ int usb_reset_device(struct usb_device *udev)
 		}
 	}
 
+	usb_lock_port(port_dev);
 	ret = usb_reset_and_verify_device(udev);
+	usb_unlock_port(port_dev);
 
 	if (config) {
 		for (i = config->desc.bNumInterfaces - 1; i >= 0; --i) {
@@ -5472,56 +5579,26 @@ struct usb_device *usb_hub_find_child(struct usb_device *hdev,
 }
 EXPORT_SYMBOL_GPL(usb_hub_find_child);
 
-/**
- * usb_set_hub_port_connect_type - set hub port connect type.
- * @hdev: USB device belonging to the usb hub
- * @port1: port num of the port
- * @type: connect type of the port
- */
-void usb_set_hub_port_connect_type(struct usb_device *hdev, int port1,
-	enum usb_port_connect_type type)
-{
-	struct usb_hub *hub = usb_hub_to_struct_hub(hdev);
-
-	if (hub)
-		hub->ports[port1 - 1]->connect_type = type;
-}
-
-/**
- * usb_get_hub_port_connect_type - Get the port's connect type
- * @hdev: USB device belonging to the usb hub
- * @port1: port num of the port
- *
- * Return: The connect type of the port if successful. Or
- * USB_PORT_CONNECT_TYPE_UNKNOWN if input params are invalid.
- */
-enum usb_port_connect_type
-usb_get_hub_port_connect_type(struct usb_device *hdev, int port1)
-{
-	struct usb_hub *hub = usb_hub_to_struct_hub(hdev);
-
-	if (!hub)
-		return USB_PORT_CONNECT_TYPE_UNKNOWN;
-
-	return hub->ports[port1 - 1]->connect_type;
-}
-
 void usb_hub_adjust_deviceremovable(struct usb_device *hdev,
 		struct usb_hub_descriptor *desc)
 {
+	struct usb_hub *hub = usb_hub_to_struct_hub(hdev);
 	enum usb_port_connect_type connect_type;
 	int i;
 
+	if (!hub)
+		return;
+
 	if (!hub_is_superspeed(hdev)) {
 		for (i = 1; i <= hdev->maxchild; i++) {
-			connect_type = usb_get_hub_port_connect_type(hdev, i);
+			struct usb_port *port_dev = hub->ports[i - 1];
 
+			connect_type = port_dev->connect_type;
 			if (connect_type == USB_PORT_CONNECT_TYPE_HARD_WIRED) {
 				u8 mask = 1 << (i%8);
 
 				if (!(desc->u.hs.DeviceRemovable[i/8] & mask)) {
-					dev_dbg(&hdev->dev, "usb port%d's DeviceRemovable is changed to 1 according to platform information.\n",
-						i);
+					dev_dbg(&port_dev->dev, "DeviceRemovable is changed to 1 according to platform information.\n");
 					desc->u.hs.DeviceRemovable[i/8]	|= mask;
 				}
 			}
@@ -5530,14 +5607,14 @@ void usb_hub_adjust_deviceremovable(struct usb_device *hdev,
 		u16 port_removable = le16_to_cpu(desc->u.ss.DeviceRemovable);
 
 		for (i = 1; i <= hdev->maxchild; i++) {
-			connect_type = usb_get_hub_port_connect_type(hdev, i);
+			struct usb_port *port_dev = hub->ports[i - 1];
 
+			connect_type = port_dev->connect_type;
 			if (connect_type == USB_PORT_CONNECT_TYPE_HARD_WIRED) {
 				u16 mask = 1 << i;
 
 				if (!(port_removable & mask)) {
-					dev_dbg(&hdev->dev, "usb port%d's DeviceRemovable is changed to 1 according to platform information.\n",
-						i);
+					dev_dbg(&port_dev->dev, "DeviceRemovable is changed to 1 according to platform information.\n");
 					port_removable |= mask;
 				}
 			}
diff --git a/drivers/usb/core/hub.h b/drivers/usb/core/hub.h
index 33bcb2c6f90..0a7cdc0ef0a 100644
--- a/drivers/usb/core/hub.h
+++ b/drivers/usb/core/hub.h
@@ -45,12 +45,13 @@ struct usb_hub {
 	unsigned long		event_bits[1];	/* status change bitmask */
 	unsigned long		change_bits[1];	/* ports with logical connect
 							status change */
-	unsigned long		busy_bits[1];	/* ports being reset or
-							resumed */
 	unsigned long		removed_bits[1]; /* ports with a "removed"
 							device present */
 	unsigned long		wakeup_bits[1];	/* ports that have signaled
 							remote wakeup */
+	unsigned long		power_bits[1]; /* ports that are powered */
+	unsigned long		child_usage_bits[1]; /* ports powered on for
+							children */
 #if USB_MAXCHILDREN > 31 /* 8*sizeof(unsigned long) - 1 */
 #error event_bits[] is too short!
 #endif
@@ -66,6 +67,7 @@ struct usb_hub {
 	unsigned		limited_power:1;
 	unsigned		quiescing:1;
 	unsigned		disconnected:1;
+	unsigned		in_reset:1;
 
 	unsigned		quirk_check_port_auto_suspend:1;
 
@@ -81,19 +83,23 @@ struct usb_hub {
  * @child: usb device attached to the port
  * @dev: generic device interface
  * @port_owner: port's owner
+ * @peer: related usb2 and usb3 ports (share the same connector)
  * @connect_type: port's connect type
+ * @location: opaque representation of platform connector location
+ * @status_lock: synchronize port_event() vs usb_port_{suspend|resume}
  * @portnum: port index num based one
- * @power_is_on: port's power state
- * @did_runtime_put: port has done pm_runtime_put().
+ * @is_superspeed cache super-speed status
  */
 struct usb_port {
 	struct usb_device *child;
 	struct device dev;
 	struct usb_dev_state *port_owner;
+	struct usb_port *peer;
 	enum usb_port_connect_type connect_type;
+	usb_port_location_t location;
+	struct mutex status_lock;
 	u8 portnum;
-	unsigned power_is_on:1;
-	unsigned did_runtime_put:1;
+	unsigned int is_superspeed:1;
 };
 
 #define to_usb_port(_dev) \
@@ -111,6 +117,29 @@ extern int hub_port_debounce(struct usb_hub *hub, int port1,
 extern int usb_clear_port_feature(struct usb_device *hdev,
 		int port1, int feature);
 
+static inline bool hub_is_port_power_switchable(struct usb_hub *hub)
+{
+	__le16 hcs;
+
+	if (!hub)
+		return false;
+	hcs = hub->descriptor->wHubCharacteristics;
+	return (le16_to_cpu(hcs) & HUB_CHAR_LPSM) < HUB_CHAR_NO_LPSM;
+}
+
+static inline int hub_is_superspeed(struct usb_device *hdev)
+{
+	return hdev->descriptor.bDeviceProtocol == USB_HUB_PR_SS;
+}
+
+static inline unsigned hub_power_on_good_delay(struct usb_hub *hub)
+{
+	unsigned delay = hub->descriptor->bPwrOn2PwrGood * 2;
+
+	/* Wait at least 100 msec for power to become stable */
+	return max(delay, 100U);
+}
+
 static inline int hub_port_debounce_be_connected(struct usb_hub *hub,
 		int port1)
 {
diff --git a/drivers/usb/core/port.c b/drivers/usb/core/port.c
index 51542f85239..62036faf56c 100644
--- a/drivers/usb/core/port.c
+++ b/drivers/usb/core/port.c
@@ -76,17 +76,29 @@ static int usb_port_runtime_resume(struct device *dev)
 	struct usb_device *hdev = to_usb_device(dev->parent->parent);
 	struct usb_interface *intf = to_usb_interface(dev->parent);
 	struct usb_hub *hub = usb_hub_to_struct_hub(hdev);
+	struct usb_device *udev = port_dev->child;
+	struct usb_port *peer = port_dev->peer;
 	int port1 = port_dev->portnum;
 	int retval;
 
 	if (!hub)
 		return -EINVAL;
+	if (hub->in_reset) {
+		set_bit(port1, hub->power_bits);
+		return 0;
+	}
 
-	usb_autopm_get_interface(intf);
-	set_bit(port1, hub->busy_bits);
+	/*
+	 * Power on our usb3 peer before this usb2 port to prevent a usb3
+	 * device from degrading to its usb2 connection
+	 */
+	if (!port_dev->is_superspeed && peer)
+		pm_runtime_get_sync(&peer->dev);
 
+	usb_autopm_get_interface(intf);
 	retval = usb_hub_set_port_power(hdev, hub, port1, true);
-	if (port_dev->child && !retval) {
+	msleep(hub_power_on_good_delay(hub));
+	if (udev && !retval) {
 		/*
 		 * Attempt to wait for usb hub port to be reconnected in order
 		 * to make the resume procedure successful.  The device may have
@@ -97,12 +109,17 @@ static int usb_port_runtime_resume(struct device *dev)
 		if (retval < 0)
 			dev_dbg(&port_dev->dev, "can't get reconnection after setting port  power on, status %d\n",
 					retval);
-		usb_clear_port_feature(hdev, port1, USB_PORT_FEAT_C_ENABLE);
 		retval = 0;
+
+		/* Force the child awake to revalidate after the power loss. */
+		if (!test_and_set_bit(port1, hub->child_usage_bits)) {
+			pm_runtime_get_noresume(&port_dev->dev);
+			pm_request_resume(&udev->dev);
+		}
 	}
 
-	clear_bit(port1, hub->busy_bits);
 	usb_autopm_put_interface(intf);
+
 	return retval;
 }
 
@@ -112,23 +129,34 @@ static int usb_port_runtime_suspend(struct device *dev)
 	struct usb_device *hdev = to_usb_device(dev->parent->parent);
 	struct usb_interface *intf = to_usb_interface(dev->parent);
 	struct usb_hub *hub = usb_hub_to_struct_hub(hdev);
+	struct usb_port *peer = port_dev->peer;
 	int port1 = port_dev->portnum;
 	int retval;
 
 	if (!hub)
 		return -EINVAL;
+	if (hub->in_reset)
+		return -EBUSY;
 
 	if (dev_pm_qos_flags(&port_dev->dev, PM_QOS_FLAG_NO_POWER_OFF)
 			== PM_QOS_FLAGS_ALL)
 		return -EAGAIN;
 
 	usb_autopm_get_interface(intf);
-	set_bit(port1, hub->busy_bits);
 	retval = usb_hub_set_port_power(hdev, hub, port1, false);
 	usb_clear_port_feature(hdev, port1, USB_PORT_FEAT_C_CONNECTION);
-	usb_clear_port_feature(hdev, port1,	USB_PORT_FEAT_C_ENABLE);
-	clear_bit(port1, hub->busy_bits);
+	if (!port_dev->is_superspeed)
+		usb_clear_port_feature(hdev, port1, USB_PORT_FEAT_C_ENABLE);
 	usb_autopm_put_interface(intf);
+
+	/*
+	 * Our peer usb3 port may now be able to suspend, so
+	 * asynchronously queue a suspend request to observe that this
+	 * usb2 port is now off.
+	 */
+	if (!port_dev->is_superspeed && peer)
+		pm_runtime_put(&peer->dev);
+
 	return retval;
 }
 #endif
@@ -146,9 +174,215 @@ struct device_type usb_port_device_type = {
 	.pm =		&usb_port_pm_ops,
 };
 
+static struct device_driver usb_port_driver = {
+	.name = "usb",
+	.owner = THIS_MODULE,
+};
+
+static int link_peers(struct usb_port *left, struct usb_port *right)
+{
+	struct usb_port *ss_port, *hs_port;
+	int rc;
+
+	if (left->peer == right && right->peer == left)
+		return 0;
+
+	if (left->peer || right->peer) {
+		struct usb_port *lpeer = left->peer;
+		struct usb_port *rpeer = right->peer;
+
+		WARN(1, "failed to peer %s and %s (%s -> %p) (%s -> %p)\n",
+			dev_name(&left->dev), dev_name(&right->dev),
+			dev_name(&left->dev), lpeer,
+			dev_name(&right->dev), rpeer);
+		return -EBUSY;
+	}
+
+	rc = sysfs_create_link(&left->dev.kobj, &right->dev.kobj, "peer");
+	if (rc)
+		return rc;
+	rc = sysfs_create_link(&right->dev.kobj, &left->dev.kobj, "peer");
+	if (rc) {
+		sysfs_remove_link(&left->dev.kobj, "peer");
+		return rc;
+	}
+
+	/*
+	 * We need to wake the HiSpeed port to make sure we don't race
+	 * setting ->peer with usb_port_runtime_suspend().  Otherwise we
+	 * may miss a suspend event for the SuperSpeed port.
+	 */
+	if (left->is_superspeed) {
+		ss_port = left;
+		WARN_ON(right->is_superspeed);
+		hs_port = right;
+	} else {
+		ss_port = right;
+		WARN_ON(!right->is_superspeed);
+		hs_port = left;
+	}
+	pm_runtime_get_sync(&hs_port->dev);
+
+	left->peer = right;
+	right->peer = left;
+
+	/*
+	 * The SuperSpeed reference is dropped when the HiSpeed port in
+	 * this relationship suspends, i.e. when it is safe to allow a
+	 * SuperSpeed connection to drop since there is no risk of a
+	 * device degrading to its powered-off HiSpeed connection.
+	 *
+	 * Also, drop the HiSpeed ref taken above.
+	 */
+	pm_runtime_get_sync(&ss_port->dev);
+	pm_runtime_put(&hs_port->dev);
+
+	return 0;
+}
+
+static void link_peers_report(struct usb_port *left, struct usb_port *right)
+{
+	int rc;
+
+	rc = link_peers(left, right);
+	if (rc == 0) {
+		dev_dbg(&left->dev, "peered to %s\n", dev_name(&right->dev));
+	} else {
+		dev_warn(&left->dev, "failed to peer to %s (%d)\n",
+				dev_name(&right->dev), rc);
+		pr_warn_once("usb: port power management may be unreliable\n");
+	}
+}
+
+static void unlink_peers(struct usb_port *left, struct usb_port *right)
+{
+	struct usb_port *ss_port, *hs_port;
+
+	WARN(right->peer != left || left->peer != right,
+			"%s and %s are not peers?\n",
+			dev_name(&left->dev), dev_name(&right->dev));
+
+	/*
+	 * We wake the HiSpeed port to make sure we don't race its
+	 * usb_port_runtime_resume() event which takes a SuperSpeed ref
+	 * when ->peer is !NULL.
+	 */
+	if (left->is_superspeed) {
+		ss_port = left;
+		hs_port = right;
+	} else {
+		ss_port = right;
+		hs_port = left;
+	}
+
+	pm_runtime_get_sync(&hs_port->dev);
+
+	sysfs_remove_link(&left->dev.kobj, "peer");
+	right->peer = NULL;
+	sysfs_remove_link(&right->dev.kobj, "peer");
+	left->peer = NULL;
+
+	/* Drop the SuperSpeed ref held on behalf of the active HiSpeed port */
+	pm_runtime_put(&ss_port->dev);
+
+	/* Drop the ref taken above */
+	pm_runtime_put(&hs_port->dev);
+}
+
+/*
+ * For each usb hub device in the system check to see if it is in the
+ * peer domain of the given port_dev, and if it is check to see if it
+ * has a port that matches the given port by location
+ */
+static int match_location(struct usb_device *peer_hdev, void *p)
+{
+	int port1;
+	struct usb_hcd *hcd, *peer_hcd;
+	struct usb_port *port_dev = p, *peer;
+	struct usb_hub *peer_hub = usb_hub_to_struct_hub(peer_hdev);
+	struct usb_device *hdev = to_usb_device(port_dev->dev.parent->parent);
+
+	if (!peer_hub)
+		return 0;
+
+	hcd = bus_to_hcd(hdev->bus);
+	peer_hcd = bus_to_hcd(peer_hdev->bus);
+	/* peer_hcd is provisional until we verify it against the known peer */
+	if (peer_hcd != hcd->shared_hcd)
+		return 0;
+
+	for (port1 = 1; port1 <= peer_hdev->maxchild; port1++) {
+		peer = peer_hub->ports[port1 - 1];
+		if (peer && peer->location == port_dev->location) {
+			link_peers_report(port_dev, peer);
+			return 1; /* done */
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * Find the peer port either via explicit platform firmware "location"
+ * data, the peer hcd for root hubs, or the upstream peer relationship
+ * for all other hubs.
+ */
+static void find_and_link_peer(struct usb_hub *hub, int port1)
+{
+	struct usb_port *port_dev = hub->ports[port1 - 1], *peer;
+	struct usb_device *hdev = hub->hdev;
+	struct usb_device *peer_hdev;
+	struct usb_hub *peer_hub;
+
+	/*
+	 * If location data is available then we can only peer this port
+	 * by a location match, not the default peer (lest we create a
+	 * situation where we need to go back and undo a default peering
+	 * when the port is later peered by location data)
+	 */
+	if (port_dev->location) {
+		/* we link the peer in match_location() if found */
+		usb_for_each_dev(port_dev, match_location);
+		return;
+	} else if (!hdev->parent) {
+		struct usb_hcd *hcd = bus_to_hcd(hdev->bus);
+		struct usb_hcd *peer_hcd = hcd->shared_hcd;
+
+		if (!peer_hcd)
+			return;
+
+		peer_hdev = peer_hcd->self.root_hub;
+	} else {
+		struct usb_port *upstream;
+		struct usb_device *parent = hdev->parent;
+		struct usb_hub *parent_hub = usb_hub_to_struct_hub(parent);
+
+		if (!parent_hub)
+			return;
+
+		upstream = parent_hub->ports[hdev->portnum - 1];
+		if (!upstream || !upstream->peer)
+			return;
+
+		peer_hdev = upstream->peer->child;
+	}
+
+	peer_hub = usb_hub_to_struct_hub(peer_hdev);
+	if (!peer_hub || port1 > peer_hdev->maxchild)
+		return;
+
+	/*
+	 * we found a valid default peer, last check is to make sure it
+	 * does not have location data
+	 */
+	peer = peer_hub->ports[port1 - 1];
+	if (peer && peer->location == 0)
+		link_peers_report(port_dev, peer);
+}
+
 int usb_hub_create_port_device(struct usb_hub *hub, int port1)
 {
-	struct usb_port *port_dev = NULL;
+	struct usb_port *port_dev;
 	int retval;
 
 	port_dev = kzalloc(sizeof(*port_dev), GFP_KERNEL);
@@ -159,24 +393,33 @@ int usb_hub_create_port_device(struct usb_hub *hub, int port1)
 
 	hub->ports[port1 - 1] = port_dev;
 	port_dev->portnum = port1;
-	port_dev->power_is_on = true;
+	set_bit(port1, hub->power_bits);
 	port_dev->dev.parent = hub->intfdev;
 	port_dev->dev.groups = port_dev_group;
 	port_dev->dev.type = &usb_port_device_type;
-	dev_set_name(&port_dev->dev, "port%d", port1);
-
+	port_dev->dev.driver = &usb_port_driver;
+	if (hub_is_superspeed(hub->hdev))
+		port_dev->is_superspeed = 1;
+	dev_set_name(&port_dev->dev, "%s-port%d", dev_name(&hub->hdev->dev),
+			port1);
+	mutex_init(&port_dev->status_lock);
 	retval = device_register(&port_dev->dev);
 	if (retval)
 		goto error_register;
 
+	find_and_link_peer(hub, port1);
+
 	pm_runtime_set_active(&port_dev->dev);
 
-	/* It would be dangerous if user space couldn't
-	 * prevent usb device from being powered off. So don't
-	 * enable port runtime pm if failed to expose port's pm qos.
+	/*
+	 * Do not enable port runtime pm if the hub does not support
+	 * power switching.  Also, userspace must have final say of
+	 * whether a port is permitted to power-off.  Do not enable
+	 * runtime pm if we fail to expose pm_qos_no_power_off.
 	 */
-	if (!dev_pm_qos_expose_flags(&port_dev->dev,
-			PM_QOS_FLAG_NO_POWER_OFF))
+	if (hub_is_port_power_switchable(hub)
+			&& dev_pm_qos_expose_flags(&port_dev->dev,
+			PM_QOS_FLAG_NO_POWER_OFF) == 0)
 		pm_runtime_enable(&port_dev->dev);
 
 	device_enable_async_suspend(&port_dev->dev);
@@ -188,9 +431,13 @@ exit:
 	return retval;
 }
 
-void usb_hub_remove_port_device(struct usb_hub *hub,
-				       int port1)
+void usb_hub_remove_port_device(struct usb_hub *hub, int port1)
 {
-	device_unregister(&hub->ports[port1 - 1]->dev);
-}
+	struct usb_port *port_dev = hub->ports[port1 - 1];
+	struct usb_port *peer;
 
+	peer = port_dev->peer;
+	if (peer)
+		unlink_peers(port_dev, peer);
+	device_unregister(&port_dev->dev);
+}
diff --git a/drivers/usb/core/usb-acpi.c b/drivers/usb/core/usb-acpi.c
index 5ca4070b1f3..2776cfe64c0 100644
--- a/drivers/usb/core/usb-acpi.c
+++ b/drivers/usb/core/usb-acpi.c
@@ -17,7 +17,7 @@
 #include <linux/pci.h>
 #include <linux/usb/hcd.h>
 
-#include "usb.h"
+#include "hub.h"
 
 /**
  * usb_acpi_power_manageable - check whether usb port has
@@ -55,13 +55,18 @@ EXPORT_SYMBOL_GPL(usb_acpi_power_manageable);
  */
 int usb_acpi_set_power_state(struct usb_device *hdev, int index, bool enable)
 {
+	struct usb_hub *hub = usb_hub_to_struct_hub(hdev);
+	struct usb_port *port_dev;
 	acpi_handle port_handle;
 	unsigned char state;
 	int port1 = index + 1;
 	int error = -EINVAL;
 
-	port_handle = (acpi_handle)usb_get_hub_port_acpi_handle(hdev,
-		port1);
+	if (!hub)
+		return -ENODEV;
+	port_dev = hub->ports[port1 - 1];
+
+	port_handle = (acpi_handle) usb_get_hub_port_acpi_handle(hdev, port1);
 	if (!port_handle)
 		return error;
 
@@ -72,23 +77,21 @@ int usb_acpi_set_power_state(struct usb_device *hdev, int index, bool enable)
 
 	error = acpi_bus_set_power(port_handle, state);
 	if (!error)
-		dev_dbg(&hdev->dev, "The power of hub port %d was set to %d\n",
-			port1, enable);
+		dev_dbg(&port_dev->dev, "acpi: power was set to %d\n", enable);
 	else
-		dev_dbg(&hdev->dev, "The power of hub port failed to be set\n");
+		dev_dbg(&port_dev->dev, "acpi: power failed to be set\n");
 
 	return error;
 }
 EXPORT_SYMBOL_GPL(usb_acpi_set_power_state);
 
-static int usb_acpi_check_port_connect_type(struct usb_device *hdev,
-	acpi_handle handle, int port1)
+static enum usb_port_connect_type usb_acpi_get_connect_type(acpi_handle handle,
+		struct acpi_pld_info *pld)
 {
-	acpi_status status;
+	enum usb_port_connect_type connect_type = USB_PORT_CONNECT_TYPE_UNKNOWN;
 	struct acpi_buffer buffer = { ACPI_ALLOCATE_BUFFER, NULL };
 	union acpi_object *upc;
-	struct acpi_pld_info *pld;
-	int ret = 0;
+	acpi_status status;
 
 	/*
 	 * According to ACPI Spec 9.13. PLD indicates whether usb port is
@@ -98,39 +101,37 @@ static int usb_acpi_check_port_connect_type(struct usb_device *hdev,
 	 * a usb device is directly hard-wired to the port. If no visible and
 	 * no connectable, the port would be not used.
 	 */
-	status = acpi_get_physical_device_location(handle, &pld);
-	if (ACPI_FAILURE(status))
-		return -ENODEV;
-
 	status = acpi_evaluate_object(handle, "_UPC", NULL, &buffer);
 	upc = buffer.pointer;
 	if (!upc || (upc->type != ACPI_TYPE_PACKAGE)
 		|| upc->package.count != 4) {
-		ret = -EINVAL;
 		goto out;
 	}
 
 	if (upc->package.elements[0].integer.value)
 		if (pld->user_visible)
-			usb_set_hub_port_connect_type(hdev, port1,
-				USB_PORT_CONNECT_TYPE_HOT_PLUG);
+			connect_type = USB_PORT_CONNECT_TYPE_HOT_PLUG;
 		else
-			usb_set_hub_port_connect_type(hdev, port1,
-				USB_PORT_CONNECT_TYPE_HARD_WIRED);
+			connect_type = USB_PORT_CONNECT_TYPE_HARD_WIRED;
 	else if (!pld->user_visible)
-		usb_set_hub_port_connect_type(hdev, port1, USB_PORT_NOT_USED);
-
+		connect_type = USB_PORT_NOT_USED;
 out:
-	ACPI_FREE(pld);
 	kfree(upc);
-	return ret;
+	return connect_type;
 }
 
+
+/*
+ * Private to usb-acpi, all the core needs to know is that
+ * port_dev->location is non-zero when it has been set by the firmware.
+ */
+#define USB_ACPI_LOCATION_VALID (1 << 31)
+
 static struct acpi_device *usb_acpi_find_companion(struct device *dev)
 {
 	struct usb_device *udev;
+	struct acpi_device *adev;
 	acpi_handle *parent_handle;
-	int port_num;
 
 	/*
 	 * In the ACPI DSDT table, only usb root hub and usb ports are
@@ -147,37 +148,19 @@ static struct acpi_device *usb_acpi_find_companion(struct device *dev)
 	 */
 	if (is_usb_device(dev)) {
 		udev = to_usb_device(dev);
-		if (udev->parent) {
-			enum usb_port_connect_type type;
-
-			/*
-			 * According usb port's connect type to set usb device's
-			 * removability.
-			 */
-			type = usb_get_hub_port_connect_type(udev->parent,
-				udev->portnum);
-			switch (type) {
-			case USB_PORT_CONNECT_TYPE_HOT_PLUG:
-				udev->removable = USB_DEVICE_REMOVABLE;
-				break;
-			case USB_PORT_CONNECT_TYPE_HARD_WIRED:
-				udev->removable = USB_DEVICE_FIXED;
-				break;
-			default:
-				udev->removable = USB_DEVICE_REMOVABLE_UNKNOWN;
-				break;
-			}
-
+		if (udev->parent)
 			return NULL;
-		}
 
-		/* root hub's parent is the usb hcd. */
-		return acpi_find_child_device(ACPI_COMPANION(dev->parent),
-					      udev->portnum, false);
+		/* root hub is only child (_ADR=0) under its parent, the HC */
+		adev = ACPI_COMPANION(dev->parent);
+		return acpi_find_child_device(adev, 0, false);
 	} else if (is_usb_port(dev)) {
-		struct acpi_device *adev = NULL;
+		struct usb_port *port_dev = to_usb_port(dev);
+		int port1 = port_dev->portnum;
+		struct acpi_pld_info *pld;
+		acpi_handle *handle;
+		acpi_status status;
 
-		sscanf(dev_name(dev), "port%d", &port_num);
 		/* Get the struct usb_device point of port's hub */
 		udev = to_usb_device(dev->parent->parent);
 
@@ -188,12 +171,11 @@ static struct acpi_device *usb_acpi_find_companion(struct device *dev)
 		 */
 		if (!udev->parent) {
 			struct usb_hcd *hcd = bus_to_hcd(udev->bus);
-			int raw_port_num;
+			int raw;
 
-			raw_port_num = usb_hcd_find_raw_port_number(hcd,
-				port_num);
+			raw = usb_hcd_find_raw_port_number(hcd, port1);
 			adev = acpi_find_child_device(ACPI_COMPANION(&udev->dev),
-						      raw_port_num, false);
+					raw, false);
 			if (!adev)
 				return NULL;
 		} else {
@@ -204,11 +186,20 @@ static struct acpi_device *usb_acpi_find_companion(struct device *dev)
 				return NULL;
 
 			acpi_bus_get_device(parent_handle, &adev);
-			adev = acpi_find_child_device(adev, port_num, false);
+			adev = acpi_find_child_device(adev, port1, false);
 			if (!adev)
 				return NULL;
 		}
-		usb_acpi_check_port_connect_type(udev, adev->handle, port_num);
+		handle = adev->handle;
+		status = acpi_get_physical_device_location(handle, &pld);
+		if (ACPI_FAILURE(status) || !pld)
+			return adev;
+
+		port_dev->location = USB_ACPI_LOCATION_VALID
+			| pld->group_token << 8 | pld->group_position;
+		port_dev->connect_type = usb_acpi_get_connect_type(handle, pld);
+		ACPI_FREE(pld);
+
 		return adev;
 	}
 
diff --git a/drivers/usb/core/usb.h b/drivers/usb/core/usb.h
index 75bf649da82..d9d08720c38 100644
--- a/drivers/usb/core/usb.h
+++ b/drivers/usb/core/usb.h
@@ -107,11 +107,6 @@ static inline int usb_autoresume_device(struct usb_device *udev)
 	return 0;
 }
 
-static inline int usb_remote_wakeup(struct usb_device *udev)
-{
-	return 0;
-}
-
 static inline int usb_set_usb2_hardware_lpm(struct usb_device *udev, int enable)
 {
 	return 0;
@@ -119,6 +114,7 @@ static inline int usb_set_usb2_hardware_lpm(struct usb_device *udev, int enable)
 #endif
 
 extern struct bus_type usb_bus_type;
+extern struct mutex usb_port_peer_mutex;
 extern struct device_type usb_device_type;
 extern struct device_type usb_if_device_type;
 extern struct device_type usb_ep_device_type;
@@ -170,15 +166,17 @@ extern void usbfs_conn_disc_event(void);
 extern int usb_devio_init(void);
 extern void usb_devio_cleanup(void);
 
+/*
+ * Firmware specific cookie identifying a port's location. '0' == no location
+ * data available
+ */
+typedef u32 usb_port_location_t;
+
 /* internal notify stuff */
 extern void usb_notify_add_device(struct usb_device *udev);
 extern void usb_notify_remove_device(struct usb_device *udev);
 extern void usb_notify_add_bus(struct usb_bus *ubus);
 extern void usb_notify_remove_bus(struct usb_bus *ubus);
-extern enum usb_port_connect_type
-	usb_get_hub_port_connect_type(struct usb_device *hdev, int port1);
-extern void usb_set_hub_port_connect_type(struct usb_device *hdev, int port1,
-	enum usb_port_connect_type type);
 extern void usb_hub_adjust_deviceremovable(struct usb_device *hdev,
 		struct usb_hub_descriptor *desc);
 
diff --git a/drivers/usb/dwc2/Kconfig b/drivers/usb/dwc2/Kconfig
index be947d67384..f93807b3631 100644
--- a/drivers/usb/dwc2/Kconfig
+++ b/drivers/usb/dwc2/Kconfig
@@ -1,25 +1,58 @@
 config USB_DWC2
-	tristate "DesignWare USB2 DRD Core Support"
+	bool "DesignWare USB2 DRD Core Support"
 	depends on USB
 	help
-	  Say Y or M here if your system has a Dual Role HighSpeed
-	  USB controller based on the DesignWare HSOTG IP Core.
+	  Say Y here if your system has a Dual Role Hi-Speed USB
+	  controller based on the DesignWare HSOTG IP Core.
 
-	  If you choose to build this driver as dynamically linked
-	  modules, the core module will be called dwc2.ko, the
-	  PCI bus interface module (if you have a PCI bus system)
-	  will be called dwc2_pci.ko and the platform interface module
-	  (for controllers directly connected to the CPU) will be called
-	  dwc2_platform.ko.
+	  For host mode, if you choose to build the driver as dynamically
+	  linked modules, the core module will be called dwc2.ko, the PCI
+	  bus interface module (if you have a PCI bus system) will be
+	  called dwc2_pci.ko, and the platform interface module (for
+	  controllers directly connected to the CPU) will be called
+	  dwc2_platform.ko. For gadget mode, there will be a single
+	  module called dwc2_gadget.ko.
 
-	  NOTE: This driver at present only implements the Host mode
-	  of the controller. The existing s3c-hsotg driver supports
-	  Peripheral mode, but only for the Samsung S3C platforms.
-	  There are plans to merge the s3c-hsotg driver with this
-	  driver in the near future to create a dual-role driver.
+	  NOTE: The s3c-hsotg driver is now renamed to dwc2_gadget. The
+	  host and gadget drivers are still currently separate drivers.
+	  There are plans to merge the dwc2_gadget driver with the dwc2
+	  host driver in the near future to create a dual-role driver.
 
 if USB_DWC2
 
+config USB_DWC2_HOST
+	tristate "Host only mode"
+	depends on USB
+	help
+	  The Designware USB2.0 high-speed host controller
+	  integrated into many SoCs.
+
+config USB_DWC2_PLATFORM
+	bool "DWC2 Platform"
+	depends on USB_DWC2_HOST
+	default USB_DWC2_HOST
+	help
+	  The Designware USB2.0 platform interface module for
+	  controllers directly connected to the CPU. This is only
+	  used for host mode.
+
+config USB_DWC2_PCI
+	bool "DWC2 PCI"
+	depends on USB_DWC2_HOST && PCI
+	default USB_DWC2_HOST
+	help
+	  The Designware USB2.0 PCI interface module for controllers
+	  connected to a PCI bus. This is only used for host mode.
+
+comment "Gadget mode requires USB Gadget support to be enabled"
+
+config USB_DWC2_PERIPHERAL
+	tristate "Gadget only mode"
+	depends on USB_GADGET
+	help
+	  The Designware USB2.0 high-speed gadget controller
+	  integrated into many SoCs.
+
 config USB_DWC2_DEBUG
 	bool "Enable Debugging Messages"
 	help
diff --git a/drivers/usb/dwc2/Makefile b/drivers/usb/dwc2/Makefile
index 11529d3439b..b73d2a52797 100644
--- a/drivers/usb/dwc2/Makefile
+++ b/drivers/usb/dwc2/Makefile
@@ -1,25 +1,28 @@
 ccflags-$(CONFIG_USB_DWC2_DEBUG)	+= -DDEBUG
 ccflags-$(CONFIG_USB_DWC2_VERBOSE)	+= -DVERBOSE_DEBUG
 
-obj-$(CONFIG_USB_DWC2)			+= dwc2.o
-
-dwc2-y					+= core.o core_intr.o
-
-# NOTE: This driver at present only implements the Host mode
-# of the controller. The existing s3c-hsotg driver supports
-# Peripheral mode, but only for the Samsung S3C platforms.
-# There are plans to merge the s3c-hsotg driver with this
-# driver in the near future to create a dual-role driver. Once
-# that is done, Host mode will become an optional feature that
-# is selected with a config option.
-
+obj-$(CONFIG_USB_DWC2_HOST)		+= dwc2.o
+dwc2-y					:= core.o core_intr.o
 dwc2-y					+= hcd.o hcd_intr.o
 dwc2-y					+= hcd_queue.o hcd_ddma.o
 
-ifneq ($(CONFIG_PCI),)
-	obj-$(CONFIG_USB_DWC2)		+= dwc2_pci.o
+# NOTE: The previous s3c-hsotg peripheral mode only driver has been moved to
+# this location and renamed gadget.c. When building for dynamically linked
+# modules, dwc2_gadget.ko will get built for peripheral mode. For host mode,
+# the core module will be dwc2.ko, the PCI bus interface module will called
+# dwc2_pci.ko and the platform interface module will be called dwc2_platform.ko.
+# At present the host and gadget driver will be separate drivers, but there
+# are plans in the near future to create a dual-role driver.
+
+ifneq ($(CONFIG_USB_DWC2_PCI),)
+	obj-$(CONFIG_USB_DWC2_HOST)	+= dwc2_pci.o
+	dwc2_pci-y			:= pci.o
+endif
+
+ifneq ($(CONFIG_USB_DWC2_PLATFORM),)
+	obj-$(CONFIG_USB_DWC2_HOST)	+= dwc2_platform.o
+	dwc2_platform-y			:= platform.o
 endif
-obj-$(CONFIG_USB_DWC2)			+= dwc2_platform.o
 
-dwc2_pci-y				+= pci.o
-dwc2_platform-y				+= platform.o
+obj-$(CONFIG_USB_DWC2_PERIPHERAL)	+= dwc2_gadget.o
+dwc2_gadget-y				:= gadget.o
diff --git a/drivers/usb/dwc2/core.c b/drivers/usb/dwc2/core.c
index 1d129884cc3..27d2c9b8a03 100644
--- a/drivers/usb/dwc2/core.c
+++ b/drivers/usb/dwc2/core.c
@@ -507,6 +507,72 @@ void dwc2_disable_host_interrupts(struct dwc2_hsotg *hsotg)
 	writel(intmsk, hsotg->regs + GINTMSK);
 }
 
+/*
+ * dwc2_calculate_dynamic_fifo() - Calculates the default fifo size
+ * For system that have a total fifo depth that is smaller than the default
+ * RX + TX fifo size.
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ */
+static void dwc2_calculate_dynamic_fifo(struct dwc2_hsotg *hsotg)
+{
+	struct dwc2_core_params *params = hsotg->core_params;
+	struct dwc2_hw_params *hw = &hsotg->hw_params;
+	u32 rxfsiz, nptxfsiz, ptxfsiz, total_fifo_size;
+
+	total_fifo_size = hw->total_fifo_size;
+	rxfsiz = params->host_rx_fifo_size;
+	nptxfsiz = params->host_nperio_tx_fifo_size;
+	ptxfsiz = params->host_perio_tx_fifo_size;
+
+	/*
+	 * Will use Method 2 defined in the DWC2 spec: minimum FIFO depth
+	 * allocation with support for high bandwidth endpoints. Synopsys
+	 * defines MPS(Max Packet size) for a periodic EP=1024, and for
+	 * non-periodic as 512.
+	 */
+	if (total_fifo_size < (rxfsiz + nptxfsiz + ptxfsiz)) {
+		/*
+		 * For Buffer DMA mode/Scatter Gather DMA mode
+		 * 2 * ((Largest Packet size / 4) + 1 + 1) + n
+		 * with n = number of host channel.
+		 * 2 * ((1024/4) + 2) = 516
+		 */
+		rxfsiz = 516 + hw->host_channels;
+
+		/*
+		 * min non-periodic tx fifo depth
+		 * 2 * (largest non-periodic USB packet used / 4)
+		 * 2 * (512/4) = 256
+		 */
+		nptxfsiz = 256;
+
+		/*
+		 * min periodic tx fifo depth
+		 * (largest packet size*MC)/4
+		 * (1024 * 3)/4 = 768
+		 */
+		ptxfsiz = 768;
+
+		params->host_rx_fifo_size = rxfsiz;
+		params->host_nperio_tx_fifo_size = nptxfsiz;
+		params->host_perio_tx_fifo_size = ptxfsiz;
+	}
+
+	/*
+	 * If the summation of RX, NPTX and PTX fifo sizes is still
+	 * bigger than the total_fifo_size, then we have a problem.
+	 *
+	 * We won't be able to allocate as many endpoints. Right now,
+	 * we're just printing an error message, but ideally this FIFO
+	 * allocation algorithm would be improved in the future.
+	 *
+	 * FIXME improve this FIFO allocation algorithm.
+	 */
+	if (unlikely(total_fifo_size < (rxfsiz + nptxfsiz + ptxfsiz)))
+		dev_err(hsotg->dev, "invalid fifo sizes\n");
+}
+
 static void dwc2_config_fifos(struct dwc2_hsotg *hsotg)
 {
 	struct dwc2_core_params *params = hsotg->core_params;
@@ -515,6 +581,8 @@ static void dwc2_config_fifos(struct dwc2_hsotg *hsotg)
 	if (!params->enable_dynamic_fifo)
 		return;
 
+	dwc2_calculate_dynamic_fifo(hsotg);
+
 	/* Rx FIFO */
 	grxfsiz = readl(hsotg->regs + GRXFSIZ);
 	dev_dbg(hsotg->dev, "initial grxfsiz=%08x\n", grxfsiz);
diff --git a/drivers/usb/dwc2/core.h b/drivers/usb/dwc2/core.h
index 648519c024b..1efd10cc962 100644
--- a/drivers/usb/dwc2/core.h
+++ b/drivers/usb/dwc2/core.h
@@ -37,6 +37,10 @@
 #ifndef __DWC2_CORE_H__
 #define __DWC2_CORE_H__
 
+#include <linux/phy/phy.h>
+#include <linux/regulator/consumer.h>
+#include <linux/usb/gadget.h>
+#include <linux/usb/otg.h>
 #include <linux/usb/phy.h>
 #include "hw.h"
 
@@ -54,6 +58,184 @@ static inline void do_write(u32 value, void *addr)
 /* Maximum number of Endpoints/HostChannels */
 #define MAX_EPS_CHANNELS	16
 
+/* s3c-hsotg declarations */
+static const char * const s3c_hsotg_supply_names[] = {
+	"vusb_d",               /* digital USB supply, 1.2V */
+	"vusb_a",               /* analog USB supply, 1.1V */
+};
+
+/*
+ * EP0_MPS_LIMIT
+ *
+ * Unfortunately there seems to be a limit of the amount of data that can
+ * be transferred by IN transactions on EP0. This is either 127 bytes or 3
+ * packets (which practically means 1 packet and 63 bytes of data) when the
+ * MPS is set to 64.
+ *
+ * This means if we are wanting to move >127 bytes of data, we need to
+ * split the transactions up, but just doing one packet at a time does
+ * not work (this may be an implicit DATA0 PID on first packet of the
+ * transaction) and doing 2 packets is outside the controller's limits.
+ *
+ * If we try to lower the MPS size for EP0, then no transfers work properly
+ * for EP0, and the system will fail basic enumeration. As no cause for this
+ * has currently been found, we cannot support any large IN transfers for
+ * EP0.
+ */
+#define EP0_MPS_LIMIT   64
+
+struct s3c_hsotg;
+struct s3c_hsotg_req;
+
+/**
+ * struct s3c_hsotg_ep - driver endpoint definition.
+ * @ep: The gadget layer representation of the endpoint.
+ * @name: The driver generated name for the endpoint.
+ * @queue: Queue of requests for this endpoint.
+ * @parent: Reference back to the parent device structure.
+ * @req: The current request that the endpoint is processing. This is
+ *       used to indicate an request has been loaded onto the endpoint
+ *       and has yet to be completed (maybe due to data move, or simply
+ *       awaiting an ack from the core all the data has been completed).
+ * @debugfs: File entry for debugfs file for this endpoint.
+ * @lock: State lock to protect contents of endpoint.
+ * @dir_in: Set to true if this endpoint is of the IN direction, which
+ *          means that it is sending data to the Host.
+ * @index: The index for the endpoint registers.
+ * @mc: Multi Count - number of transactions per microframe
+ * @interval - Interval for periodic endpoints
+ * @name: The name array passed to the USB core.
+ * @halted: Set if the endpoint has been halted.
+ * @periodic: Set if this is a periodic ep, such as Interrupt
+ * @isochronous: Set if this is a isochronous ep
+ * @sent_zlp: Set if we've sent a zero-length packet.
+ * @total_data: The total number of data bytes done.
+ * @fifo_size: The size of the FIFO (for periodic IN endpoints)
+ * @fifo_load: The amount of data loaded into the FIFO (periodic IN)
+ * @last_load: The offset of data for the last start of request.
+ * @size_loaded: The last loaded size for DxEPTSIZE for periodic IN
+ *
+ * This is the driver's state for each registered enpoint, allowing it
+ * to keep track of transactions that need doing. Each endpoint has a
+ * lock to protect the state, to try and avoid using an overall lock
+ * for the host controller as much as possible.
+ *
+ * For periodic IN endpoints, we have fifo_size and fifo_load to try
+ * and keep track of the amount of data in the periodic FIFO for each
+ * of these as we don't have a status register that tells us how much
+ * is in each of them. (note, this may actually be useless information
+ * as in shared-fifo mode periodic in acts like a single-frame packet
+ * buffer than a fifo)
+ */
+struct s3c_hsotg_ep {
+	struct usb_ep           ep;
+	struct list_head        queue;
+	struct s3c_hsotg        *parent;
+	struct s3c_hsotg_req    *req;
+	struct dentry           *debugfs;
+
+	unsigned long           total_data;
+	unsigned int            size_loaded;
+	unsigned int            last_load;
+	unsigned int            fifo_load;
+	unsigned short          fifo_size;
+
+	unsigned char           dir_in;
+	unsigned char           index;
+	unsigned char           mc;
+	unsigned char           interval;
+
+	unsigned int            halted:1;
+	unsigned int            periodic:1;
+	unsigned int            isochronous:1;
+	unsigned int            sent_zlp:1;
+
+	char                    name[10];
+};
+
+/**
+ * struct s3c_hsotg - driver state.
+ * @dev: The parent device supplied to the probe function
+ * @driver: USB gadget driver
+ * @phy: The otg phy transceiver structure for phy control.
+ * @uphy: The otg phy transceiver structure for old USB phy control.
+ * @plat: The platform specific configuration data. This can be removed once
+ * all SoCs support usb transceiver.
+ * @regs: The memory area mapped for accessing registers.
+ * @irq: The IRQ number we are using
+ * @supplies: Definition of USB power supplies
+ * @phyif: PHY interface width
+ * @dedicated_fifos: Set if the hardware has dedicated IN-EP fifos.
+ * @num_of_eps: Number of available EPs (excluding EP0)
+ * @debug_root: root directrory for debugfs.
+ * @debug_file: main status file for debugfs.
+ * @debug_fifo: FIFO status file for debugfs.
+ * @ep0_reply: Request used for ep0 reply.
+ * @ep0_buff: Buffer for EP0 reply data, if needed.
+ * @ctrl_buff: Buffer for EP0 control requests.
+ * @ctrl_req: Request for EP0 control packets.
+ * @setup: NAK management for EP0 SETUP
+ * @last_rst: Time of last reset
+ * @eps: The endpoints being supplied to the gadget framework
+ */
+struct s3c_hsotg {
+	struct device            *dev;
+	struct usb_gadget_driver *driver;
+	struct phy               *phy;
+	struct usb_phy           *uphy;
+	struct s3c_hsotg_plat    *plat;
+
+	spinlock_t              lock;
+
+	void __iomem            *regs;
+	int                     irq;
+	struct clk              *clk;
+
+	struct regulator_bulk_data supplies[ARRAY_SIZE(s3c_hsotg_supply_names)];
+
+	u32                     phyif;
+	unsigned int            dedicated_fifos:1;
+	unsigned char           num_of_eps;
+
+	struct dentry           *debug_root;
+	struct dentry           *debug_file;
+	struct dentry           *debug_fifo;
+
+	struct usb_request      *ep0_reply;
+	struct usb_request      *ctrl_req;
+	u8                      ep0_buff[8];
+	u8                      ctrl_buff[8];
+
+	struct usb_gadget       gadget;
+	unsigned int            setup;
+	unsigned long           last_rst;
+	struct s3c_hsotg_ep     *eps;
+};
+
+/**
+ * struct s3c_hsotg_req - data transfer request
+ * @req: The USB gadget request
+ * @queue: The list of requests for the endpoint this is queued for.
+ * @in_progress: Has already had size/packets written to core
+ * @mapped: DMA buffer for this request has been mapped via dma_map_single().
+ */
+struct s3c_hsotg_req {
+	struct usb_request      req;
+	struct list_head        queue;
+	unsigned char           in_progress;
+	unsigned char           mapped;
+};
+
+#define call_gadget(_hs, _entry) \
+do { \
+	if ((_hs)->gadget.speed != USB_SPEED_UNKNOWN && \
+		(_hs)->driver && (_hs)->driver->_entry) { \
+		spin_unlock(&_hs->lock); \
+		(_hs)->driver->_entry(&(_hs)->gadget); \
+		spin_lock(&_hs->lock); \
+	} \
+} while (0)
+
 struct dwc2_hsotg;
 struct dwc2_host_chan;
 
diff --git a/drivers/usb/gadget/s3c-hsotg.c b/drivers/usb/dwc2/gadget.c
index 2a9cb674926..f3c56a2fed5 100644
--- a/drivers/usb/gadget/s3c-hsotg.c
+++ b/drivers/usb/dwc2/gadget.c
@@ -37,175 +37,7 @@
 #include <linux/usb/phy.h>
 #include <linux/platform_data/s3c-hsotg.h>
 
-#include "s3c-hsotg.h"
-
-static const char * const s3c_hsotg_supply_names[] = {
-	"vusb_d",		/* digital USB supply, 1.2V */
-	"vusb_a",		/* analog USB supply, 1.1V */
-};
-
-/*
- * EP0_MPS_LIMIT
- *
- * Unfortunately there seems to be a limit of the amount of data that can
- * be transferred by IN transactions on EP0. This is either 127 bytes or 3
- * packets (which practically means 1 packet and 63 bytes of data) when the
- * MPS is set to 64.
- *
- * This means if we are wanting to move >127 bytes of data, we need to
- * split the transactions up, but just doing one packet at a time does
- * not work (this may be an implicit DATA0 PID on first packet of the
- * transaction) and doing 2 packets is outside the controller's limits.
- *
- * If we try to lower the MPS size for EP0, then no transfers work properly
- * for EP0, and the system will fail basic enumeration. As no cause for this
- * has currently been found, we cannot support any large IN transfers for
- * EP0.
- */
-#define EP0_MPS_LIMIT	64
-
-struct s3c_hsotg;
-struct s3c_hsotg_req;
-
-/**
- * struct s3c_hsotg_ep - driver endpoint definition.
- * @ep: The gadget layer representation of the endpoint.
- * @name: The driver generated name for the endpoint.
- * @queue: Queue of requests for this endpoint.
- * @parent: Reference back to the parent device structure.
- * @req: The current request that the endpoint is processing. This is
- *       used to indicate an request has been loaded onto the endpoint
- *       and has yet to be completed (maybe due to data move, or simply
- *	 awaiting an ack from the core all the data has been completed).
- * @debugfs: File entry for debugfs file for this endpoint.
- * @lock: State lock to protect contents of endpoint.
- * @dir_in: Set to true if this endpoint is of the IN direction, which
- *	    means that it is sending data to the Host.
- * @index: The index for the endpoint registers.
- * @mc: Multi Count - number of transactions per microframe
- * @interval - Interval for periodic endpoints
- * @name: The name array passed to the USB core.
- * @halted: Set if the endpoint has been halted.
- * @periodic: Set if this is a periodic ep, such as Interrupt
- * @isochronous: Set if this is a isochronous ep
- * @sent_zlp: Set if we've sent a zero-length packet.
- * @total_data: The total number of data bytes done.
- * @fifo_size: The size of the FIFO (for periodic IN endpoints)
- * @fifo_load: The amount of data loaded into the FIFO (periodic IN)
- * @last_load: The offset of data for the last start of request.
- * @size_loaded: The last loaded size for DxEPTSIZE for periodic IN
- *
- * This is the driver's state for each registered enpoint, allowing it
- * to keep track of transactions that need doing. Each endpoint has a
- * lock to protect the state, to try and avoid using an overall lock
- * for the host controller as much as possible.
- *
- * For periodic IN endpoints, we have fifo_size and fifo_load to try
- * and keep track of the amount of data in the periodic FIFO for each
- * of these as we don't have a status register that tells us how much
- * is in each of them. (note, this may actually be useless information
- * as in shared-fifo mode periodic in acts like a single-frame packet
- * buffer than a fifo)
- */
-struct s3c_hsotg_ep {
-	struct usb_ep		ep;
-	struct list_head	queue;
-	struct s3c_hsotg	*parent;
-	struct s3c_hsotg_req	*req;
-	struct dentry		*debugfs;
-
-
-	unsigned long		total_data;
-	unsigned int		size_loaded;
-	unsigned int		last_load;
-	unsigned int		fifo_load;
-	unsigned short		fifo_size;
-
-	unsigned char		dir_in;
-	unsigned char		index;
-	unsigned char		mc;
-	unsigned char		interval;
-
-	unsigned int		halted:1;
-	unsigned int		periodic:1;
-	unsigned int		isochronous:1;
-	unsigned int		sent_zlp:1;
-
-	char			name[10];
-};
-
-/**
- * struct s3c_hsotg - driver state.
- * @dev: The parent device supplied to the probe function
- * @driver: USB gadget driver
- * @phy: The otg phy transceiver structure for phy control.
- * @uphy: The otg phy transceiver structure for old USB phy control.
- * @plat: The platform specific configuration data. This can be removed once
- * all SoCs support usb transceiver.
- * @regs: The memory area mapped for accessing registers.
- * @irq: The IRQ number we are using
- * @supplies: Definition of USB power supplies
- * @phyif: PHY interface width
- * @dedicated_fifos: Set if the hardware has dedicated IN-EP fifos.
- * @num_of_eps: Number of available EPs (excluding EP0)
- * @debug_root: root directrory for debugfs.
- * @debug_file: main status file for debugfs.
- * @debug_fifo: FIFO status file for debugfs.
- * @ep0_reply: Request used for ep0 reply.
- * @ep0_buff: Buffer for EP0 reply data, if needed.
- * @ctrl_buff: Buffer for EP0 control requests.
- * @ctrl_req: Request for EP0 control packets.
- * @setup: NAK management for EP0 SETUP
- * @last_rst: Time of last reset
- * @eps: The endpoints being supplied to the gadget framework
- */
-struct s3c_hsotg {
-	struct device		 *dev;
-	struct usb_gadget_driver *driver;
-	struct phy		 *phy;
-	struct usb_phy		 *uphy;
-	struct s3c_hsotg_plat	 *plat;
-
-	spinlock_t              lock;
-
-	void __iomem		*regs;
-	int			irq;
-	struct clk		*clk;
-
-	struct regulator_bulk_data supplies[ARRAY_SIZE(s3c_hsotg_supply_names)];
-
-	u32			phyif;
-	unsigned int		dedicated_fifos:1;
-	unsigned char           num_of_eps;
-
-	struct dentry		*debug_root;
-	struct dentry		*debug_file;
-	struct dentry		*debug_fifo;
-
-	struct usb_request	*ep0_reply;
-	struct usb_request	*ctrl_req;
-	u8			ep0_buff[8];
-	u8			ctrl_buff[8];
-
-	struct usb_gadget	gadget;
-	unsigned int		setup;
-	unsigned long           last_rst;
-	struct s3c_hsotg_ep	*eps;
-};
-
-/**
- * struct s3c_hsotg_req - data transfer request
- * @req: The USB gadget request
- * @queue: The list of requests for the endpoint this is queued for.
- * @in_progress: Has already had size/packets written to core
- * @mapped: DMA buffer for this request has been mapped via dma_map_single().
- */
-struct s3c_hsotg_req {
-	struct usb_request	req;
-	struct list_head	queue;
-	unsigned char		in_progress;
-	unsigned char		mapped;
-};
+#include "core.h"
 
 /* conversion functions */
 static inline struct s3c_hsotg_req *our_req(struct usb_request *req)
@@ -340,9 +172,8 @@ static void s3c_hsotg_init_fifo(struct s3c_hsotg *hsotg)
 	/* set FIFO sizes to 2048/1024 */
 
 	writel(2048, hsotg->regs + GRXFSIZ);
-	writel(GNPTXFSIZ_NPTxFStAddr(2048) |
-	       GNPTXFSIZ_NPTxFDep(1024),
-	       hsotg->regs + GNPTXFSIZ);
+	writel((2048 << FIFOSIZE_STARTADDR_SHIFT) |
+		(1024 << FIFOSIZE_DEPTH_SHIFT), hsotg->regs + GNPTXFSIZ);
 
 	/*
 	 * arange all the rest of the TX FIFOs, as some versions of this
@@ -362,10 +193,10 @@ static void s3c_hsotg_init_fifo(struct s3c_hsotg *hsotg)
 
 	for (ep = 1; ep <= 15; ep++) {
 		val = addr;
-		val |= size << DPTXFSIZn_DPTxFSize_SHIFT;
+		val |= size << FIFOSIZE_DEPTH_SHIFT;
 		addr += size;
 
-		writel(val, hsotg->regs + DPTXFSIZn(ep));
+		writel(val, hsotg->regs + DPTXFSIZN(ep));
 	}
 
 	/*
@@ -373,15 +204,15 @@ static void s3c_hsotg_init_fifo(struct s3c_hsotg *hsotg)
 	 * all fifos are flushed before continuing
 	 */
 
-	writel(GRSTCTL_TxFNum(0x10) | GRSTCTL_TxFFlsh |
-	       GRSTCTL_RxFFlsh, hsotg->regs + GRSTCTL);
+	writel(GRSTCTL_TXFNUM(0x10) | GRSTCTL_TXFFLSH |
+	       GRSTCTL_RXFFLSH, hsotg->regs + GRSTCTL);
 
 	/* wait until the fifos are both flushed */
 	timeout = 100;
 	while (1) {
 		val = readl(hsotg->regs + GRSTCTL);
 
-		if ((val & (GRSTCTL_TxFFlsh | GRSTCTL_RxFFlsh)) == 0)
+		if ((val & (GRSTCTL_TXFFLSH | GRSTCTL_RXFFLSH)) == 0)
 			break;
 
 		if (--timeout == 0) {
@@ -495,14 +326,14 @@ static int s3c_hsotg_write_fifo(struct s3c_hsotg *hsotg,
 		 * how much data is left in the fifo.
 		 */
 
-		size_left = DxEPTSIZ_XferSize_GET(epsize);
+		size_left = DXEPTSIZ_XFERSIZE_GET(epsize);
 
 		/*
 		 * if shared fifo, we cannot write anything until the
 		 * previous data has been completely sent.
 		 */
 		if (hs_ep->fifo_load != 0) {
-			s3c_hsotg_en_gsint(hsotg, GINTSTS_PTxFEmp);
+			s3c_hsotg_en_gsint(hsotg, GINTSTS_PTXFEMP);
 			return -ENOSPC;
 		}
 
@@ -523,7 +354,7 @@ static int s3c_hsotg_write_fifo(struct s3c_hsotg *hsotg,
 			__func__, can_write);
 
 		if (can_write <= 0) {
-			s3c_hsotg_en_gsint(hsotg, GINTSTS_PTxFEmp);
+			s3c_hsotg_en_gsint(hsotg, GINTSTS_PTXFEMP);
 			return -ENOSPC;
 		}
 	} else if (hsotg->dedicated_fifos && hs_ep->index != 0) {
@@ -532,16 +363,16 @@ static int s3c_hsotg_write_fifo(struct s3c_hsotg *hsotg,
 		can_write &= 0xffff;
 		can_write *= 4;
 	} else {
-		if (GNPTXSTS_NPTxQSpcAvail_GET(gnptxsts) == 0) {
+		if (GNPTXSTS_NP_TXQ_SPC_AVAIL_GET(gnptxsts) == 0) {
 			dev_dbg(hsotg->dev,
 				"%s: no queue slots available (0x%08x)\n",
 				__func__, gnptxsts);
 
-			s3c_hsotg_en_gsint(hsotg, GINTSTS_NPTxFEmp);
+			s3c_hsotg_en_gsint(hsotg, GINTSTS_NPTXFEMP);
 			return -ENOSPC;
 		}
 
-		can_write = GNPTXSTS_NPTxFSpcAvail_GET(gnptxsts);
+		can_write = GNPTXSTS_NP_TXF_SPC_AVAIL_GET(gnptxsts);
 		can_write *= 4;	/* fifo size is in 32bit quantities. */
 	}
 
@@ -569,8 +400,8 @@ static int s3c_hsotg_write_fifo(struct s3c_hsotg *hsotg,
 		/* it's needed only when we do not use dedicated fifos */
 		if (!hsotg->dedicated_fifos)
 			s3c_hsotg_en_gsint(hsotg,
-					   periodic ? GINTSTS_PTxFEmp :
-					   GINTSTS_NPTxFEmp);
+					   periodic ? GINTSTS_PTXFEMP :
+					   GINTSTS_NPTXFEMP);
 	}
 
 	/* see if we can write data */
@@ -598,8 +429,8 @@ static int s3c_hsotg_write_fifo(struct s3c_hsotg *hsotg,
 		/* it's needed only when we do not use dedicated fifos */
 		if (!hsotg->dedicated_fifos)
 			s3c_hsotg_en_gsint(hsotg,
-					   periodic ? GINTSTS_PTxFEmp :
-					   GINTSTS_NPTxFEmp);
+					   periodic ? GINTSTS_PTXFEMP :
+					   GINTSTS_NPTXFEMP);
 	}
 
 	dev_dbg(hsotg->dev, "write %d/%d, can_write %d, done %d\n",
@@ -636,12 +467,12 @@ static unsigned get_ep_limit(struct s3c_hsotg_ep *hs_ep)
 	unsigned maxpkt;
 
 	if (index != 0) {
-		maxsize = DxEPTSIZ_XferSize_LIMIT + 1;
-		maxpkt = DxEPTSIZ_PktCnt_LIMIT + 1;
+		maxsize = DXEPTSIZ_XFERSIZE_LIMIT + 1;
+		maxpkt = DXEPTSIZ_PKTCNT_LIMIT + 1;
 	} else {
 		maxsize = 64+64;
 		if (hs_ep->dir_in)
-			maxpkt = DIEPTSIZ0_PktCnt_LIMIT + 1;
+			maxpkt = DIEPTSIZ0_PKTCNT_LIMIT + 1;
 		else
 			maxpkt = 2;
 	}
@@ -710,7 +541,7 @@ static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
 	/* If endpoint is stalled, we will restart request later */
 	ctrl = readl(hsotg->regs + epctrl_reg);
 
-	if (ctrl & DxEPCTL_Stall) {
+	if (ctrl & DXEPCTL_STALL) {
 		dev_warn(hsotg->dev, "%s: ep%d is stalled\n", __func__, index);
 		return;
 	}
@@ -720,7 +551,7 @@ static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
 		ureq->length, ureq->actual);
 	if (0)
 		dev_dbg(hsotg->dev,
-			"REQ buf %p len %d dma 0x%pad noi=%d zp=%d snok=%d\n",
+			"REQ buf %p len %d dma %pad noi=%d zp=%d snok=%d\n",
 			ureq->buf, length, &ureq->dma,
 			ureq->no_interrupt, ureq->zero, ureq->short_not_ok);
 
@@ -750,9 +581,9 @@ static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
 
 	if (dir_in && index != 0)
 		if (hs_ep->isochronous)
-			epsize = DxEPTSIZ_MC(packets);
+			epsize = DXEPTSIZ_MC(packets);
 		else
-			epsize = DxEPTSIZ_MC(1);
+			epsize = DXEPTSIZ_MC(1);
 	else
 		epsize = 0;
 
@@ -766,8 +597,8 @@ static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
 			packets++;
 	}
 
-	epsize |= DxEPTSIZ_PktCnt(packets);
-	epsize |= DxEPTSIZ_XferSize(length);
+	epsize |= DXEPTSIZ_PKTCNT(packets);
+	epsize |= DXEPTSIZ_XFERSIZE(length);
 
 	dev_dbg(hsotg->dev, "%s: %d@%d/%d, 0x%08x => 0x%08x\n",
 		__func__, packets, length, ureq->length, epsize, epsize_reg);
@@ -789,12 +620,12 @@ static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
 		dma_reg = dir_in ? DIEPDMA(index) : DOEPDMA(index);
 		writel(ureq->dma, hsotg->regs + dma_reg);
 
-		dev_dbg(hsotg->dev, "%s: 0x%pad => 0x%08x\n",
+		dev_dbg(hsotg->dev, "%s: %pad => 0x%08x\n",
 			__func__, &ureq->dma, dma_reg);
 	}
 
-	ctrl |= DxEPCTL_EPEna;	/* ensure ep enabled */
-	ctrl |= DxEPCTL_USBActEp;
+	ctrl |= DXEPCTL_EPENA;	/* ensure ep enabled */
+	ctrl |= DXEPCTL_USBACTEP;
 
 	dev_dbg(hsotg->dev, "setup req:%d\n", hsotg->setup);
 
@@ -802,7 +633,7 @@ static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
 	if (hsotg->setup && index == 0)
 		hsotg->setup = 0;
 	else
-		ctrl |= DxEPCTL_CNAK;	/* clear NAK set by core */
+		ctrl |= DXEPCTL_CNAK;	/* clear NAK set by core */
 
 
 	dev_dbg(hsotg->dev, "%s: DxEPCTL=0x%08x\n", __func__, ctrl);
@@ -828,7 +659,7 @@ static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
 	 * to debugging to see what is going on.
 	 */
 	if (dir_in)
-		writel(DIEPMSK_INTknTXFEmpMsk,
+		writel(DIEPMSK_INTKNTXFEMPMSK,
 		       hsotg->regs + DIEPINT(index));
 
 	/*
@@ -837,12 +668,12 @@ static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
 	 */
 
 	/* check ep is enabled */
-	if (!(readl(hsotg->regs + epctrl_reg) & DxEPCTL_EPEna))
+	if (!(readl(hsotg->regs + epctrl_reg) & DXEPCTL_EPENA))
 		dev_warn(hsotg->dev,
-			 "ep%d: failed to become enabled (DxEPCTL=0x%08x)?\n",
+			 "ep%d: failed to become enabled (DXEPCTL=0x%08x)?\n",
 			 index, readl(hsotg->regs + epctrl_reg));
 
-	dev_dbg(hsotg->dev, "%s: DxEPCTL=0x%08x\n",
+	dev_dbg(hsotg->dev, "%s: DXEPCTL=0x%08x\n",
 		__func__, readl(hsotg->regs + epctrl_reg));
 
 	/* enable ep interrupts */
@@ -1205,12 +1036,12 @@ static void s3c_hsotg_stall_ep0(struct s3c_hsotg *hsotg) {
 	 */
 
 	ctrl = readl(hsotg->regs + reg);
-	ctrl |= DxEPCTL_Stall;
-	ctrl |= DxEPCTL_CNAK;
+	ctrl |= DXEPCTL_STALL;
+	ctrl |= DXEPCTL_CNAK;
 	writel(ctrl, hsotg->regs + reg);
 
 	dev_dbg(hsotg->dev,
-		"written DxEPCTL=0x%08x to %08x (DxEPCTL=0x%08x)\n",
+		"written DXEPCTL=0x%08x to %08x (DXEPCTL=0x%08x)\n",
 		ctrl, reg, readl(hsotg->regs + reg));
 
 	 /*
@@ -1262,8 +1093,9 @@ static void s3c_hsotg_process_control(struct s3c_hsotg *hsotg,
 		case USB_REQ_SET_ADDRESS:
 			s3c_hsotg_disconnect(hsotg);
 			dcfg = readl(hsotg->regs + DCFG);
-			dcfg &= ~DCFG_DevAddr_MASK;
-			dcfg |= ctrl->wValue << DCFG_DevAddr_SHIFT;
+			dcfg &= ~DCFG_DEVADDR_MASK;
+			dcfg |= (le16_to_cpu(ctrl->wValue) <<
+				 DCFG_DEVADDR_SHIFT) & DCFG_DEVADDR_MASK;
 			writel(dcfg, hsotg->regs + DCFG);
 
 			dev_info(hsotg->dev, "new address %d\n", ctrl->wValue);
@@ -1458,7 +1290,7 @@ static void s3c_hsotg_rx_data(struct s3c_hsotg *hsotg, int ep_idx, int size)
 		int ptr;
 
 		dev_warn(hsotg->dev,
-			 "%s: FIFO %d bytes on ep%d but no req (DxEPCTl=0x%08x)\n",
+			 "%s: FIFO %d bytes on ep%d but no req (DXEPCTl=0x%08x)\n",
 			 __func__, size, ep_idx, epctl);
 
 		/* dump the data from the FIFO, we've nothing we can do */
@@ -1530,13 +1362,13 @@ static void s3c_hsotg_send_zlp(struct s3c_hsotg *hsotg,
 	dev_dbg(hsotg->dev, "sending zero-length packet\n");
 
 	/* issue a zero-sized packet to terminate this */
-	writel(DxEPTSIZ_MC(1) | DxEPTSIZ_PktCnt(1) |
-	       DxEPTSIZ_XferSize(0), hsotg->regs + DIEPTSIZ(0));
+	writel(DXEPTSIZ_MC(1) | DXEPTSIZ_PKTCNT(1) |
+	       DXEPTSIZ_XFERSIZE(0), hsotg->regs + DIEPTSIZ(0));
 
 	ctrl = readl(hsotg->regs + DIEPCTL0);
-	ctrl |= DxEPCTL_CNAK;  /* clear NAK set by core */
-	ctrl |= DxEPCTL_EPEna; /* ensure ep enabled */
-	ctrl |= DxEPCTL_USBActEp;
+	ctrl |= DXEPCTL_CNAK;  /* clear NAK set by core */
+	ctrl |= DXEPCTL_EPENA; /* ensure ep enabled */
+	ctrl |= DXEPCTL_USBACTEP;
 	writel(ctrl, hsotg->regs + DIEPCTL0);
 }
 
@@ -1557,7 +1389,7 @@ static void s3c_hsotg_handle_outdone(struct s3c_hsotg *hsotg,
 	struct s3c_hsotg_ep *hs_ep = &hsotg->eps[epnum];
 	struct s3c_hsotg_req *hs_req = hs_ep->req;
 	struct usb_request *req = &hs_req->req;
-	unsigned size_left = DxEPTSIZ_XferSize_GET(epsize);
+	unsigned size_left = DXEPTSIZ_XFERSIZE_GET(epsize);
 	int result = 0;
 
 	if (!hs_req) {
@@ -1657,24 +1489,22 @@ static void s3c_hsotg_handle_rx(struct s3c_hsotg *hsotg)
 
 	WARN_ON(using_dma(hsotg));
 
-	epnum = grxstsr & GRXSTS_EPNum_MASK;
-	status = grxstsr & GRXSTS_PktSts_MASK;
+	epnum = grxstsr & GRXSTS_EPNUM_MASK;
+	status = grxstsr & GRXSTS_PKTSTS_MASK;
 
-	size = grxstsr & GRXSTS_ByteCnt_MASK;
-	size >>= GRXSTS_ByteCnt_SHIFT;
+	size = grxstsr & GRXSTS_BYTECNT_MASK;
+	size >>= GRXSTS_BYTECNT_SHIFT;
 
 	if (1)
 		dev_dbg(hsotg->dev, "%s: GRXSTSP=0x%08x (%d@%d)\n",
 			__func__, grxstsr, size, epnum);
 
-#define __status(x) ((x) >> GRXSTS_PktSts_SHIFT)
-
-	switch (status >> GRXSTS_PktSts_SHIFT) {
-	case __status(GRXSTS_PktSts_GlobalOutNAK):
-		dev_dbg(hsotg->dev, "GlobalOutNAK\n");
+	switch ((status & GRXSTS_PKTSTS_MASK) >> GRXSTS_PKTSTS_SHIFT) {
+	case GRXSTS_PKTSTS_GLOBALOUTNAK:
+		dev_dbg(hsotg->dev, "GLOBALOUTNAK\n");
 		break;
 
-	case __status(GRXSTS_PktSts_OutDone):
+	case GRXSTS_PKTSTS_OUTDONE:
 		dev_dbg(hsotg->dev, "OutDone (Frame=0x%08x)\n",
 			s3c_hsotg_read_frameno(hsotg));
 
@@ -1682,7 +1512,7 @@ static void s3c_hsotg_handle_rx(struct s3c_hsotg *hsotg)
 			s3c_hsotg_handle_outdone(hsotg, epnum, false);
 		break;
 
-	case __status(GRXSTS_PktSts_SetupDone):
+	case GRXSTS_PKTSTS_SETUPDONE:
 		dev_dbg(hsotg->dev,
 			"SetupDone (Frame=0x%08x, DOPEPCTL=0x%08x)\n",
 			s3c_hsotg_read_frameno(hsotg),
@@ -1691,11 +1521,11 @@ static void s3c_hsotg_handle_rx(struct s3c_hsotg *hsotg)
 		s3c_hsotg_handle_outdone(hsotg, epnum, true);
 		break;
 
-	case __status(GRXSTS_PktSts_OutRX):
+	case GRXSTS_PKTSTS_OUTRX:
 		s3c_hsotg_rx_data(hsotg, epnum, size);
 		break;
 
-	case __status(GRXSTS_PktSts_SetupRX):
+	case GRXSTS_PKTSTS_SETUPRX:
 		dev_dbg(hsotg->dev,
 			"SetupRX (Frame=0x%08x, DOPEPCTL=0x%08x)\n",
 			s3c_hsotg_read_frameno(hsotg),
@@ -1761,7 +1591,7 @@ static void s3c_hsotg_set_ep_maxpacket(struct s3c_hsotg *hsotg,
 		hs_ep->ep.maxpacket = mps;
 		hs_ep->mc = 1;
 	} else {
-		mpsval = mps & DxEPCTL_MPS_MASK;
+		mpsval = mps & DXEPCTL_MPS_MASK;
 		if (mpsval > 1024)
 			goto bad_mps;
 		mcval = ((mps >> 11) & 0x3) + 1;
@@ -1777,13 +1607,13 @@ static void s3c_hsotg_set_ep_maxpacket(struct s3c_hsotg *hsotg,
 	 */
 
 	reg = readl(regs + DIEPCTL(ep));
-	reg &= ~DxEPCTL_MPS_MASK;
+	reg &= ~DXEPCTL_MPS_MASK;
 	reg |= mpsval;
 	writel(reg, regs + DIEPCTL(ep));
 
 	if (ep) {
 		reg = readl(regs + DOEPCTL(ep));
-		reg &= ~DxEPCTL_MPS_MASK;
+		reg &= ~DXEPCTL_MPS_MASK;
 		reg |= mpsval;
 		writel(reg, regs + DOEPCTL(ep));
 	}
@@ -1804,7 +1634,7 @@ static void s3c_hsotg_txfifo_flush(struct s3c_hsotg *hsotg, unsigned int idx)
 	int timeout;
 	int val;
 
-	writel(GRSTCTL_TxFNum(idx) | GRSTCTL_TxFFlsh,
+	writel(GRSTCTL_TXFNUM(idx) | GRSTCTL_TXFFLSH,
 		hsotg->regs + GRSTCTL);
 
 	/* wait until the fifo is flushed */
@@ -1813,7 +1643,7 @@ static void s3c_hsotg_txfifo_flush(struct s3c_hsotg *hsotg, unsigned int idx)
 	while (1) {
 		val = readl(hsotg->regs + GRSTCTL);
 
-		if ((val & (GRSTCTL_TxFFlsh)) == 0)
+		if ((val & (GRSTCTL_TXFFLSH)) == 0)
 			break;
 
 		if (--timeout == 0) {
@@ -1896,7 +1726,7 @@ static void s3c_hsotg_complete_in(struct s3c_hsotg *hsotg,
 	 * aligned).
 	 */
 
-	size_left = DxEPTSIZ_XferSize_GET(epsize);
+	size_left = DXEPTSIZ_XFERSIZE_GET(epsize);
 
 	size_done = hs_ep->size_loaded - size_left;
 	size_done += hs_ep->last_load;
@@ -1963,17 +1793,17 @@ static void s3c_hsotg_epint(struct s3c_hsotg *hsotg, unsigned int idx,
 	dev_dbg(hsotg->dev, "%s: ep%d(%s) DxEPINT=0x%08x\n",
 		__func__, idx, dir_in ? "in" : "out", ints);
 
-	if (ints & DxEPINT_XferCompl) {
+	if (ints & DXEPINT_XFERCOMPL) {
 		if (hs_ep->isochronous && hs_ep->interval == 1) {
-			if (ctrl & DxEPCTL_EOFrNum)
-				ctrl |= DxEPCTL_SetEvenFr;
+			if (ctrl & DXEPCTL_EOFRNUM)
+				ctrl |= DXEPCTL_SETEVENFR;
 			else
-				ctrl |= DxEPCTL_SetOddFr;
+				ctrl |= DXEPCTL_SETODDFR;
 			writel(ctrl, hsotg->regs + epctl_reg);
 		}
 
 		dev_dbg(hsotg->dev,
-			"%s: XferCompl: DxEPCTL=0x%08x, DxEPTSIZ=%08x\n",
+			"%s: XferCompl: DxEPCTL=0x%08x, DXEPTSIZ=%08x\n",
 			__func__, readl(hsotg->regs + epctl_reg),
 			readl(hsotg->regs + epsiz_reg));
 
@@ -1996,7 +1826,7 @@ static void s3c_hsotg_epint(struct s3c_hsotg *hsotg, unsigned int idx,
 		}
 	}
 
-	if (ints & DxEPINT_EPDisbld) {
+	if (ints & DXEPINT_EPDISBLD) {
 		dev_dbg(hsotg->dev, "%s: EPDisbld\n", __func__);
 
 		if (dir_in) {
@@ -2004,20 +1834,20 @@ static void s3c_hsotg_epint(struct s3c_hsotg *hsotg, unsigned int idx,
 
 			s3c_hsotg_txfifo_flush(hsotg, idx);
 
-			if ((epctl & DxEPCTL_Stall) &&
-				(epctl & DxEPCTL_EPType_Bulk)) {
+			if ((epctl & DXEPCTL_STALL) &&
+				(epctl & DXEPCTL_EPTYPE_BULK)) {
 				int dctl = readl(hsotg->regs + DCTL);
 
-				dctl |= DCTL_CGNPInNAK;
+				dctl |= DCTL_CGNPINNAK;
 				writel(dctl, hsotg->regs + DCTL);
 			}
 		}
 	}
 
-	if (ints & DxEPINT_AHBErr)
+	if (ints & DXEPINT_AHBERR)
 		dev_dbg(hsotg->dev, "%s: AHBErr\n", __func__);
 
-	if (ints & DxEPINT_Setup) {  /* Setup or Timeout */
+	if (ints & DXEPINT_SETUP) {  /* Setup or Timeout */
 		dev_dbg(hsotg->dev, "%s: Setup/Timeout\n",  __func__);
 
 		if (using_dma(hsotg) && idx == 0) {
@@ -2035,25 +1865,25 @@ static void s3c_hsotg_epint(struct s3c_hsotg *hsotg, unsigned int idx,
 		}
 	}
 
-	if (ints & DxEPINT_Back2BackSetup)
+	if (ints & DXEPINT_BACK2BACKSETUP)
 		dev_dbg(hsotg->dev, "%s: B2BSetup/INEPNakEff\n", __func__);
 
 	if (dir_in && !hs_ep->isochronous) {
 		/* not sure if this is important, but we'll clear it anyway */
-		if (ints & DIEPMSK_INTknTXFEmpMsk) {
+		if (ints & DIEPMSK_INTKNTXFEMPMSK) {
 			dev_dbg(hsotg->dev, "%s: ep%d: INTknTXFEmpMsk\n",
 				__func__, idx);
 		}
 
 		/* this probably means something bad is happening */
-		if (ints & DIEPMSK_INTknEPMisMsk) {
+		if (ints & DIEPMSK_INTKNEPMISMSK) {
 			dev_warn(hsotg->dev, "%s: ep%d: INTknEP\n",
 				 __func__, idx);
 		}
 
 		/* FIFO has space or is empty (see GAHBCFG) */
 		if (hsotg->dedicated_fifos &&
-		    ints & DIEPMSK_TxFIFOEmpty) {
+		    ints & DIEPMSK_TXFIFOEMPTY) {
 			dev_dbg(hsotg->dev, "%s: ep%d: TxFIFOEmpty\n",
 				__func__, idx);
 			if (!using_dma(hsotg))
@@ -2089,21 +1919,21 @@ static void s3c_hsotg_irq_enumdone(struct s3c_hsotg *hsotg)
 	 */
 
 	/* catch both EnumSpd_FS and EnumSpd_FS48 */
-	switch (dsts & DSTS_EnumSpd_MASK) {
-	case DSTS_EnumSpd_FS:
-	case DSTS_EnumSpd_FS48:
+	switch (dsts & DSTS_ENUMSPD_MASK) {
+	case DSTS_ENUMSPD_FS:
+	case DSTS_ENUMSPD_FS48:
 		hsotg->gadget.speed = USB_SPEED_FULL;
 		ep0_mps = EP0_MPS_LIMIT;
 		ep_mps = 1023;
 		break;
 
-	case DSTS_EnumSpd_HS:
+	case DSTS_ENUMSPD_HS:
 		hsotg->gadget.speed = USB_SPEED_HIGH;
 		ep0_mps = EP0_MPS_LIMIT;
 		ep_mps = 1024;
 		break;
 
-	case DSTS_EnumSpd_LS:
+	case DSTS_ENUMSPD_LS:
 		hsotg->gadget.speed = USB_SPEED_LOW;
 		/*
 		 * note, we don't actually support LS in this driver at the
@@ -2169,16 +1999,6 @@ static void kill_all_requests(struct s3c_hsotg *hsotg,
 			s3c_hsotg_txfifo_flush(hsotg, ep->index);
 }
 
-#define call_gadget(_hs, _entry) \
-do { \
-	if ((_hs)->gadget.speed != USB_SPEED_UNKNOWN &&	\
-	    (_hs)->driver && (_hs)->driver->_entry) { \
-		spin_unlock(&_hs->lock); \
-		(_hs)->driver->_entry(&(_hs)->gadget); \
-		spin_lock(&_hs->lock); \
-	} \
-} while (0)
-
 /**
  * s3c_hsotg_disconnect - disconnect service
  * @hsotg: The device state.
@@ -2226,9 +2046,9 @@ static void s3c_hsotg_irq_fifoempty(struct s3c_hsotg *hsotg, bool periodic)
 }
 
 /* IRQ flags which will trigger a retry around the IRQ loop */
-#define IRQ_RETRY_MASK (GINTSTS_NPTxFEmp | \
-			GINTSTS_PTxFEmp |  \
-			GINTSTS_RxFLvl)
+#define IRQ_RETRY_MASK (GINTSTS_NPTXFEMP | \
+			GINTSTS_PTXFEMP |  \
+			GINTSTS_RXFLVL)
 
 /**
  * s3c_hsotg_corereset - issue softreset to the core
@@ -2244,14 +2064,14 @@ static int s3c_hsotg_corereset(struct s3c_hsotg *hsotg)
 	dev_dbg(hsotg->dev, "resetting core\n");
 
 	/* issue soft reset */
-	writel(GRSTCTL_CSftRst, hsotg->regs + GRSTCTL);
+	writel(GRSTCTL_CSFTRST, hsotg->regs + GRSTCTL);
 
 	timeout = 10000;
 	do {
 		grstctl = readl(hsotg->regs + GRSTCTL);
-	} while ((grstctl & GRSTCTL_CSftRst) && timeout-- > 0);
+	} while ((grstctl & GRSTCTL_CSFTRST) && timeout-- > 0);
 
-	if (grstctl & GRSTCTL_CSftRst) {
+	if (grstctl & GRSTCTL_CSFTRST) {
 		dev_err(hsotg->dev, "Failed to get CSftRst asserted\n");
 		return -EINVAL;
 	}
@@ -2268,7 +2088,7 @@ static int s3c_hsotg_corereset(struct s3c_hsotg *hsotg)
 			return -ETIMEDOUT;
 		}
 
-		if (!(grstctl & GRSTCTL_AHBIdle))
+		if (!(grstctl & GRSTCTL_AHBIDLE))
 			continue;
 
 		break;		/* reset done */
@@ -2294,14 +2114,14 @@ static void s3c_hsotg_core_init(struct s3c_hsotg *hsotg)
 	 */
 
 	/* set the PLL on, remove the HNP/SRP and set the PHY */
-	writel(hsotg->phyif | GUSBCFG_TOutCal(7) |
+	writel(hsotg->phyif | GUSBCFG_TOUTCAL(7) |
 	       (0x5 << 10), hsotg->regs + GUSBCFG);
 
 	s3c_hsotg_init_fifo(hsotg);
 
-	__orr32(hsotg->regs + DCTL, DCTL_SftDiscon);
+	__orr32(hsotg->regs + DCTL, DCTL_SFTDISCON);
 
-	writel(1 << 18 | DCFG_DevSpd_HS,  hsotg->regs + DCFG);
+	writel(1 << 18 | DCFG_DEVSPD_HS,  hsotg->regs + DCFG);
 
 	/* Clear any pending OTG interrupts */
 	writel(0xffffffff, hsotg->regs + GOTGINT);
@@ -2309,21 +2129,21 @@ static void s3c_hsotg_core_init(struct s3c_hsotg *hsotg)
 	/* Clear any pending interrupts */
 	writel(0xffffffff, hsotg->regs + GINTSTS);
 
-	writel(GINTSTS_ErlySusp | GINTSTS_SessReqInt |
-	       GINTSTS_GOUTNakEff | GINTSTS_GINNakEff |
-	       GINTSTS_ConIDStsChng | GINTSTS_USBRst |
-	       GINTSTS_EnumDone | GINTSTS_OTGInt |
-	       GINTSTS_USBSusp | GINTSTS_WkUpInt,
-	       hsotg->regs + GINTMSK);
+	writel(GINTSTS_ERLYSUSP | GINTSTS_SESSREQINT |
+		GINTSTS_GOUTNAKEFF | GINTSTS_GINNAKEFF |
+		GINTSTS_CONIDSTSCHNG | GINTSTS_USBRST |
+		GINTSTS_ENUMDONE | GINTSTS_OTGINT |
+		GINTSTS_USBSUSP | GINTSTS_WKUPINT,
+		hsotg->regs + GINTMSK);
 
 	if (using_dma(hsotg))
-		writel(GAHBCFG_GlblIntrEn | GAHBCFG_DMAEn |
-		       GAHBCFG_HBstLen_Incr4,
+		writel(GAHBCFG_GLBL_INTR_EN | GAHBCFG_DMA_EN |
+		       GAHBCFG_HBSTLEN_INCR4,
 		       hsotg->regs + GAHBCFG);
 	else
-		writel(((hsotg->dedicated_fifos) ? (GAHBCFG_NPTxFEmpLvl |
-						    GAHBCFG_PTxFEmpLvl) : 0) |
-		       GAHBCFG_GlblIntrEn,
+		writel(((hsotg->dedicated_fifos) ? (GAHBCFG_NP_TXF_EMP_LVL |
+						    GAHBCFG_P_TXF_EMP_LVL) : 0) |
+		       GAHBCFG_GLBL_INTR_EN,
 		       hsotg->regs + GAHBCFG);
 
 	/*
@@ -2332,22 +2152,22 @@ static void s3c_hsotg_core_init(struct s3c_hsotg *hsotg)
 	 * interrupts.
 	 */
 
-	writel(((hsotg->dedicated_fifos) ? DIEPMSK_TxFIFOEmpty |
-	       DIEPMSK_INTknTXFEmpMsk : 0) |
-	       DIEPMSK_EPDisbldMsk | DIEPMSK_XferComplMsk |
-	       DIEPMSK_TimeOUTMsk | DIEPMSK_AHBErrMsk |
-	       DIEPMSK_INTknEPMisMsk,
-	       hsotg->regs + DIEPMSK);
+	writel(((hsotg->dedicated_fifos) ? DIEPMSK_TXFIFOEMPTY |
+		DIEPMSK_INTKNTXFEMPMSK : 0) |
+		DIEPMSK_EPDISBLDMSK | DIEPMSK_XFERCOMPLMSK |
+		DIEPMSK_TIMEOUTMSK | DIEPMSK_AHBERRMSK |
+		DIEPMSK_INTKNEPMISMSK,
+		hsotg->regs + DIEPMSK);
 
 	/*
 	 * don't need XferCompl, we get that from RXFIFO in slave mode. In
 	 * DMA mode we may need this.
 	 */
-	writel((using_dma(hsotg) ? (DIEPMSK_XferComplMsk |
-				    DIEPMSK_TimeOUTMsk) : 0) |
-	       DOEPMSK_EPDisbldMsk | DOEPMSK_AHBErrMsk |
-	       DOEPMSK_SetupMsk,
-	       hsotg->regs + DOEPMSK);
+	writel((using_dma(hsotg) ? (DIEPMSK_XFERCOMPLMSK |
+				    DIEPMSK_TIMEOUTMSK) : 0) |
+		DOEPMSK_EPDISBLDMSK | DOEPMSK_AHBERRMSK |
+		DOEPMSK_SETUPMSK,
+		hsotg->regs + DOEPMSK);
 
 	writel(0, hsotg->regs + DAINTMSK);
 
@@ -2356,7 +2176,7 @@ static void s3c_hsotg_core_init(struct s3c_hsotg *hsotg)
 		readl(hsotg->regs + DOEPCTL0));
 
 	/* enable in and out endpoint interrupts */
-	s3c_hsotg_en_gsint(hsotg, GINTSTS_OEPInt | GINTSTS_IEPInt);
+	s3c_hsotg_en_gsint(hsotg, GINTSTS_OEPINT | GINTSTS_IEPINT);
 
 	/*
 	 * Enable the RXFIFO when in slave mode, as this is how we collect
@@ -2364,15 +2184,15 @@ static void s3c_hsotg_core_init(struct s3c_hsotg *hsotg)
 	 * things we cannot process, so do not use it.
 	 */
 	if (!using_dma(hsotg))
-		s3c_hsotg_en_gsint(hsotg, GINTSTS_RxFLvl);
+		s3c_hsotg_en_gsint(hsotg, GINTSTS_RXFLVL);
 
 	/* Enable interrupts for EP0 in and out */
 	s3c_hsotg_ctrl_epint(hsotg, 0, 0, 1);
 	s3c_hsotg_ctrl_epint(hsotg, 0, 1, 1);
 
-	__orr32(hsotg->regs + DCTL, DCTL_PWROnPrgDone);
+	__orr32(hsotg->regs + DCTL, DCTL_PWRONPRGDONE);
 	udelay(10);  /* see openiboot */
-	__bic32(hsotg->regs + DCTL, DCTL_PWROnPrgDone);
+	__bic32(hsotg->regs + DCTL, DCTL_PWRONPRGDONE);
 
 	dev_dbg(hsotg->dev, "DCTL=0x%08x\n", readl(hsotg->regs + DCTL));
 
@@ -2382,17 +2202,17 @@ static void s3c_hsotg_core_init(struct s3c_hsotg *hsotg)
 	 */
 
 	/* set to read 1 8byte packet */
-	writel(DxEPTSIZ_MC(1) | DxEPTSIZ_PktCnt(1) |
-	       DxEPTSIZ_XferSize(8), hsotg->regs + DOEPTSIZ0);
+	writel(DXEPTSIZ_MC(1) | DXEPTSIZ_PKTCNT(1) |
+	       DXEPTSIZ_XFERSIZE(8), hsotg->regs + DOEPTSIZ0);
 
 	writel(s3c_hsotg_ep0_mps(hsotg->eps[0].ep.maxpacket) |
-	       DxEPCTL_CNAK | DxEPCTL_EPEna |
-	       DxEPCTL_USBActEp,
+	       DXEPCTL_CNAK | DXEPCTL_EPENA |
+	       DXEPCTL_USBACTEP,
 	       hsotg->regs + DOEPCTL0);
 
 	/* enable, but don't activate EP0in */
 	writel(s3c_hsotg_ep0_mps(hsotg->eps[0].ep.maxpacket) |
-	       DxEPCTL_USBActEp, hsotg->regs + DIEPCTL0);
+	       DXEPCTL_USBACTEP, hsotg->regs + DIEPCTL0);
 
 	s3c_hsotg_enqueue_setup(hsotg);
 
@@ -2401,14 +2221,14 @@ static void s3c_hsotg_core_init(struct s3c_hsotg *hsotg)
 		readl(hsotg->regs + DOEPCTL0));
 
 	/* clear global NAKs */
-	writel(DCTL_CGOUTNak | DCTL_CGNPInNAK,
+	writel(DCTL_CGOUTNAK | DCTL_CGNPINNAK,
 	       hsotg->regs + DCTL);
 
 	/* must be at-least 3ms to allow bus to see disconnect */
 	mdelay(3);
 
 	/* remove the soft-disconnect and let's go */
-	__bic32(hsotg->regs + DCTL, DCTL_SftDiscon);
+	__bic32(hsotg->regs + DCTL, DCTL_SFTDISCON);
 }
 
 /**
@@ -2433,7 +2253,7 @@ irq_retry:
 
 	gintsts &= gintmsk;
 
-	if (gintsts & GINTSTS_OTGInt) {
+	if (gintsts & GINTSTS_OTGINT) {
 		u32 otgint = readl(hsotg->regs + GOTGINT);
 
 		dev_info(hsotg->dev, "OTGInt: %08x\n", otgint);
@@ -2441,34 +2261,34 @@ irq_retry:
 		writel(otgint, hsotg->regs + GOTGINT);
 	}
 
-	if (gintsts & GINTSTS_SessReqInt) {
+	if (gintsts & GINTSTS_SESSREQINT) {
 		dev_dbg(hsotg->dev, "%s: SessReqInt\n", __func__);
-		writel(GINTSTS_SessReqInt, hsotg->regs + GINTSTS);
+		writel(GINTSTS_SESSREQINT, hsotg->regs + GINTSTS);
 	}
 
-	if (gintsts & GINTSTS_EnumDone) {
-		writel(GINTSTS_EnumDone, hsotg->regs + GINTSTS);
+	if (gintsts & GINTSTS_ENUMDONE) {
+		writel(GINTSTS_ENUMDONE, hsotg->regs + GINTSTS);
 
 		s3c_hsotg_irq_enumdone(hsotg);
 	}
 
-	if (gintsts & GINTSTS_ConIDStsChng) {
+	if (gintsts & GINTSTS_CONIDSTSCHNG) {
 		dev_dbg(hsotg->dev, "ConIDStsChg (DSTS=0x%08x, GOTCTL=%08x)\n",
 			readl(hsotg->regs + DSTS),
 			readl(hsotg->regs + GOTGCTL));
 
-		writel(GINTSTS_ConIDStsChng, hsotg->regs + GINTSTS);
+		writel(GINTSTS_CONIDSTSCHNG, hsotg->regs + GINTSTS);
 	}
 
-	if (gintsts & (GINTSTS_OEPInt | GINTSTS_IEPInt)) {
+	if (gintsts & (GINTSTS_OEPINT | GINTSTS_IEPINT)) {
 		u32 daint = readl(hsotg->regs + DAINT);
 		u32 daintmsk = readl(hsotg->regs + DAINTMSK);
 		u32 daint_out, daint_in;
 		int ep;
 
 		daint &= daintmsk;
-		daint_out = daint >> DAINT_OutEP_SHIFT;
-		daint_in = daint & ~(daint_out << DAINT_OutEP_SHIFT);
+		daint_out = daint >> DAINT_OUTEP_SHIFT;
+		daint_in = daint & ~(daint_out << DAINT_OUTEP_SHIFT);
 
 		dev_dbg(hsotg->dev, "%s: daint=%08x\n", __func__, daint);
 
@@ -2483,7 +2303,7 @@ irq_retry:
 		}
 	}
 
-	if (gintsts & GINTSTS_USBRst) {
+	if (gintsts & GINTSTS_USBRST) {
 
 		u32 usb_status = readl(hsotg->regs + GOTGCTL);
 
@@ -2491,7 +2311,7 @@ irq_retry:
 		dev_dbg(hsotg->dev, "GNPTXSTS=%08x\n",
 			readl(hsotg->regs + GNPTXSTS));
 
-		writel(GINTSTS_USBRst, hsotg->regs + GINTSTS);
+		writel(GINTSTS_USBRST, hsotg->regs + GINTSTS);
 
 		if (usb_status & GOTGCTL_BSESVLD) {
 			if (time_after(jiffies, hsotg->last_rst +
@@ -2508,7 +2328,7 @@ irq_retry:
 
 	/* check both FIFOs */
 
-	if (gintsts & GINTSTS_NPTxFEmp) {
+	if (gintsts & GINTSTS_NPTXFEMP) {
 		dev_dbg(hsotg->dev, "NPTxFEmp\n");
 
 		/*
@@ -2517,20 +2337,20 @@ irq_retry:
 		 * it needs re-enabling
 		 */
 
-		s3c_hsotg_disable_gsint(hsotg, GINTSTS_NPTxFEmp);
+		s3c_hsotg_disable_gsint(hsotg, GINTSTS_NPTXFEMP);
 		s3c_hsotg_irq_fifoempty(hsotg, false);
 	}
 
-	if (gintsts & GINTSTS_PTxFEmp) {
+	if (gintsts & GINTSTS_PTXFEMP) {
 		dev_dbg(hsotg->dev, "PTxFEmp\n");
 
 		/* See note in GINTSTS_NPTxFEmp */
 
-		s3c_hsotg_disable_gsint(hsotg, GINTSTS_PTxFEmp);
+		s3c_hsotg_disable_gsint(hsotg, GINTSTS_PTXFEMP);
 		s3c_hsotg_irq_fifoempty(hsotg, true);
 	}
 
-	if (gintsts & GINTSTS_RxFLvl) {
+	if (gintsts & GINTSTS_RXFLVL) {
 		/*
 		 * note, since GINTSTS_RxFLvl doubles as FIFO-not-empty,
 		 * we need to retry s3c_hsotg_handle_rx if this is still
@@ -2540,28 +2360,28 @@ irq_retry:
 		s3c_hsotg_handle_rx(hsotg);
 	}
 
-	if (gintsts & GINTSTS_ModeMis) {
+	if (gintsts & GINTSTS_MODEMIS) {
 		dev_warn(hsotg->dev, "warning, mode mismatch triggered\n");
-		writel(GINTSTS_ModeMis, hsotg->regs + GINTSTS);
+		writel(GINTSTS_MODEMIS, hsotg->regs + GINTSTS);
 	}
 
-	if (gintsts & GINTSTS_USBSusp) {
+	if (gintsts & GINTSTS_USBSUSP) {
 		dev_info(hsotg->dev, "GINTSTS_USBSusp\n");
-		writel(GINTSTS_USBSusp, hsotg->regs + GINTSTS);
+		writel(GINTSTS_USBSUSP, hsotg->regs + GINTSTS);
 
 		call_gadget(hsotg, suspend);
 	}
 
-	if (gintsts & GINTSTS_WkUpInt) {
+	if (gintsts & GINTSTS_WKUPINT) {
 		dev_info(hsotg->dev, "GINTSTS_WkUpIn\n");
-		writel(GINTSTS_WkUpInt, hsotg->regs + GINTSTS);
+		writel(GINTSTS_WKUPINT, hsotg->regs + GINTSTS);
 
 		call_gadget(hsotg, resume);
 	}
 
-	if (gintsts & GINTSTS_ErlySusp) {
+	if (gintsts & GINTSTS_ERLYSUSP) {
 		dev_dbg(hsotg->dev, "GINTSTS_ErlySusp\n");
-		writel(GINTSTS_ErlySusp, hsotg->regs + GINTSTS);
+		writel(GINTSTS_ERLYSUSP, hsotg->regs + GINTSTS);
 	}
 
 	/*
@@ -2570,18 +2390,18 @@ irq_retry:
 	 * the occurrence.
 	 */
 
-	if (gintsts & GINTSTS_GOUTNakEff) {
+	if (gintsts & GINTSTS_GOUTNAKEFF) {
 		dev_info(hsotg->dev, "GOUTNakEff triggered\n");
 
-		writel(DCTL_CGOUTNak, hsotg->regs + DCTL);
+		writel(DCTL_CGOUTNAK, hsotg->regs + DCTL);
 
 		s3c_hsotg_dump(hsotg);
 	}
 
-	if (gintsts & GINTSTS_GINNakEff) {
+	if (gintsts & GINTSTS_GINNAKEFF) {
 		dev_info(hsotg->dev, "GINNakEff triggered\n");
 
-		writel(DCTL_CGNPInNAK, hsotg->regs + DCTL);
+		writel(DCTL_CGNPINNAK, hsotg->regs + DCTL);
 
 		s3c_hsotg_dump(hsotg);
 	}
@@ -2645,14 +2465,14 @@ static int s3c_hsotg_ep_enable(struct usb_ep *ep,
 
 	spin_lock_irqsave(&hsotg->lock, flags);
 
-	epctrl &= ~(DxEPCTL_EPType_MASK | DxEPCTL_MPS_MASK);
-	epctrl |= DxEPCTL_MPS(mps);
+	epctrl &= ~(DXEPCTL_EPTYPE_MASK | DXEPCTL_MPS_MASK);
+	epctrl |= DXEPCTL_MPS(mps);
 
 	/*
 	 * mark the endpoint as active, otherwise the core may ignore
 	 * transactions entirely for this endpoint
 	 */
-	epctrl |= DxEPCTL_USBActEp;
+	epctrl |= DXEPCTL_USBACTEP;
 
 	/*
 	 * set the NAK status on the endpoint, otherwise we might try and
@@ -2661,7 +2481,7 @@ static int s3c_hsotg_ep_enable(struct usb_ep *ep,
 	 * size register hasn't been set.
 	 */
 
-	epctrl |= DxEPCTL_SNAK;
+	epctrl |= DXEPCTL_SNAK;
 
 	/* update the endpoint state */
 	s3c_hsotg_set_ep_maxpacket(hsotg, hs_ep->index, mps);
@@ -2677,15 +2497,15 @@ static int s3c_hsotg_ep_enable(struct usb_ep *ep,
 
 	switch (desc->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK) {
 	case USB_ENDPOINT_XFER_ISOC:
-		epctrl |= DxEPCTL_EPType_Iso;
-		epctrl |= DxEPCTL_SetEvenFr;
+		epctrl |= DXEPCTL_EPTYPE_ISO;
+		epctrl |= DXEPCTL_SETEVENFR;
 		hs_ep->isochronous = 1;
 		if (dir_in)
 			hs_ep->periodic = 1;
 		break;
 
 	case USB_ENDPOINT_XFER_BULK:
-		epctrl |= DxEPCTL_EPType_Bulk;
+		epctrl |= DXEPCTL_EPTYPE_BULK;
 		break;
 
 	case USB_ENDPOINT_XFER_INT:
@@ -2698,14 +2518,14 @@ static int s3c_hsotg_ep_enable(struct usb_ep *ep,
 			 */
 
 			hs_ep->periodic = 1;
-			epctrl |= DxEPCTL_TxFNum(index);
+			epctrl |= DXEPCTL_TXFNUM(index);
 		}
 
-		epctrl |= DxEPCTL_EPType_Intterupt;
+		epctrl |= DXEPCTL_EPTYPE_INTERRUPT;
 		break;
 
 	case USB_ENDPOINT_XFER_CONTROL:
-		epctrl |= DxEPCTL_EPType_Control;
+		epctrl |= DXEPCTL_EPTYPE_CONTROL;
 		break;
 	}
 
@@ -2714,11 +2534,11 @@ static int s3c_hsotg_ep_enable(struct usb_ep *ep,
 	 * a unique tx-fifo even if it is non-periodic.
 	 */
 	if (dir_in && hsotg->dedicated_fifos)
-		epctrl |= DxEPCTL_TxFNum(index);
+		epctrl |= DXEPCTL_TXFNUM(index);
 
 	/* for non control endpoints, set PID to D0 */
 	if (index)
-		epctrl |= DxEPCTL_SetD0PID;
+		epctrl |= DXEPCTL_SETD0PID;
 
 	dev_dbg(hsotg->dev, "%s: write DxEPCTL=0x%08x\n",
 		__func__, epctrl);
@@ -2763,9 +2583,9 @@ static int s3c_hsotg_ep_disable(struct usb_ep *ep)
 
 
 	ctrl = readl(hsotg->regs + epctrl_reg);
-	ctrl &= ~DxEPCTL_EPEna;
-	ctrl &= ~DxEPCTL_USBActEp;
-	ctrl |= DxEPCTL_SNAK;
+	ctrl &= ~DXEPCTL_EPENA;
+	ctrl &= ~DXEPCTL_USBACTEP;
+	ctrl |= DXEPCTL_SNAK;
 
 	dev_dbg(hsotg->dev, "%s: DxEPCTL=0x%08x\n", __func__, ctrl);
 	writel(ctrl, hsotg->regs + epctrl_reg);
@@ -2852,15 +2672,15 @@ static int s3c_hsotg_ep_sethalt(struct usb_ep *ep, int value)
 	epctl = readl(hs->regs + epreg);
 
 	if (value) {
-		epctl |= DxEPCTL_Stall + DxEPCTL_SNAK;
-		if (epctl & DxEPCTL_EPEna)
-			epctl |= DxEPCTL_EPDis;
+		epctl |= DXEPCTL_STALL + DXEPCTL_SNAK;
+		if (epctl & DXEPCTL_EPENA)
+			epctl |= DXEPCTL_EPDIS;
 	} else {
-		epctl &= ~DxEPCTL_Stall;
-		xfertype = epctl & DxEPCTL_EPType_MASK;
-		if (xfertype == DxEPCTL_EPType_Bulk ||
-			xfertype == DxEPCTL_EPType_Intterupt)
-				epctl |= DxEPCTL_SetD0PID;
+		epctl &= ~DXEPCTL_STALL;
+		xfertype = epctl & DXEPCTL_EPTYPE_MASK;
+		if (xfertype == DXEPCTL_EPTYPE_BULK ||
+			xfertype == DXEPCTL_EPTYPE_INTERRUPT)
+				epctl |= DXEPCTL_SETD0PID;
 	}
 
 	writel(epctl, hs->regs + epreg);
@@ -2869,13 +2689,13 @@ static int s3c_hsotg_ep_sethalt(struct usb_ep *ep, int value)
 	epctl = readl(hs->regs + epreg);
 
 	if (value)
-		epctl |= DxEPCTL_Stall;
+		epctl |= DXEPCTL_STALL;
 	else {
-		epctl &= ~DxEPCTL_Stall;
-		xfertype = epctl & DxEPCTL_EPType_MASK;
-		if (xfertype == DxEPCTL_EPType_Bulk ||
-			xfertype == DxEPCTL_EPType_Intterupt)
-				epctl |= DxEPCTL_SetD0PID;
+		epctl &= ~DXEPCTL_STALL;
+		xfertype = epctl & DXEPCTL_EPTYPE_MASK;
+		if (xfertype == DXEPCTL_EPTYPE_BULK ||
+			xfertype == DXEPCTL_EPTYPE_INTERRUPT)
+				epctl |= DXEPCTL_SETD0PID;
 	}
 
 	writel(epctl, hs->regs + epreg);
@@ -2965,22 +2785,22 @@ static void s3c_hsotg_init(struct s3c_hsotg *hsotg)
 {
 	/* unmask subset of endpoint interrupts */
 
-	writel(DIEPMSK_TimeOUTMsk | DIEPMSK_AHBErrMsk |
-	       DIEPMSK_EPDisbldMsk | DIEPMSK_XferComplMsk,
-	       hsotg->regs + DIEPMSK);
+	writel(DIEPMSK_TIMEOUTMSK | DIEPMSK_AHBERRMSK |
+		DIEPMSK_EPDISBLDMSK | DIEPMSK_XFERCOMPLMSK,
+		hsotg->regs + DIEPMSK);
 
-	writel(DOEPMSK_SetupMsk | DOEPMSK_AHBErrMsk |
-	       DOEPMSK_EPDisbldMsk | DOEPMSK_XferComplMsk,
-	       hsotg->regs + DOEPMSK);
+	writel(DOEPMSK_SETUPMSK | DOEPMSK_AHBERRMSK |
+		DOEPMSK_EPDISBLDMSK | DOEPMSK_XFERCOMPLMSK,
+		hsotg->regs + DOEPMSK);
 
 	writel(0, hsotg->regs + DAINTMSK);
 
 	/* Be in disconnected state until gadget is registered */
-	__orr32(hsotg->regs + DCTL, DCTL_SftDiscon);
+	__orr32(hsotg->regs + DCTL, DCTL_SFTDISCON);
 
 	if (0) {
 		/* post global nak until we're ready */
-		writel(DCTL_SGNPInNAK | DCTL_SGOUTNak,
+		writel(DCTL_SGNPINNAK | DCTL_SGOUTNAK,
 		       hsotg->regs + DCTL);
 	}
 
@@ -2993,10 +2813,10 @@ static void s3c_hsotg_init(struct s3c_hsotg *hsotg)
 	s3c_hsotg_init_fifo(hsotg);
 
 	/* set the PLL on, remove the HNP/SRP and set the PHY */
-	writel(GUSBCFG_PHYIf16 | GUSBCFG_TOutCal(7) | (0x5 << 10),
+	writel(GUSBCFG_PHYIF16 | GUSBCFG_TOUTCAL(7) | (0x5 << 10),
 	       hsotg->regs + GUSBCFG);
 
-	writel(using_dma(hsotg) ? GAHBCFG_DMAEn : 0x0,
+	writel(using_dma(hsotg) ? GAHBCFG_DMA_EN : 0x0,
 	       hsotg->regs + GAHBCFG);
 }
 
@@ -3187,8 +3007,8 @@ static void s3c_hsotg_initep(struct s3c_hsotg *hsotg,
 	 * code is changed to make each endpoint's direction changeable.
 	 */
 
-	ptxfifo = readl(hsotg->regs + DPTXFSIZn(epnum));
-	hs_ep->fifo_size = DPTXFSIZn_DPTxFSize_GET(ptxfifo) * 4;
+	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
@@ -3196,7 +3016,7 @@ static void s3c_hsotg_initep(struct s3c_hsotg *hsotg,
 	 */
 
 	if (using_dma(hsotg)) {
-		u32 next = DxEPCTL_NextEp((epnum + 1) % 15);
+		u32 next = DXEPCTL_NEXTEP((epnum + 1) % 15);
 		writel(next, hsotg->regs + DIEPCTL(epnum));
 		writel(next, hsotg->regs + DOEPCTL(epnum));
 	}
@@ -3250,10 +3070,10 @@ static void s3c_hsotg_dump(struct s3c_hsotg *hsotg)
 	/* show periodic fifo settings */
 
 	for (idx = 1; idx <= 15; idx++) {
-		val = readl(regs + DPTXFSIZn(idx));
+		val = readl(regs + DPTXFSIZN(idx));
 		dev_info(dev, "DPTx[%d] FSize=%d, StAddr=0x%08x\n", idx,
-			 val >> DPTXFSIZn_DPTxFSize_SHIFT,
-			 val & DPTXFSIZn_DPTxFStAddr_MASK);
+			 val >> FIFOSIZE_DEPTH_SHIFT,
+			 val & FIFOSIZE_STARTADDR_MASK);
 	}
 
 	for (idx = 0; idx < 15; idx++) {
@@ -3368,17 +3188,17 @@ static int fifo_show(struct seq_file *seq, void *v)
 
 	val = readl(regs + GNPTXFSIZ);
 	seq_printf(seq, "NPTXFIFO: Size %d, Start 0x%08x\n",
-		   val >> GNPTXFSIZ_NPTxFDep_SHIFT,
-		   val & GNPTXFSIZ_NPTxFStAddr_MASK);
+		   val >> FIFOSIZE_DEPTH_SHIFT,
+		   val & FIFOSIZE_DEPTH_MASK);
 
 	seq_puts(seq, "\nPeriodic TXFIFOs:\n");
 
 	for (idx = 1; idx <= 15; idx++) {
-		val = readl(regs + DPTXFSIZn(idx));
+		val = readl(regs + DPTXFSIZN(idx));
 
 		seq_printf(seq, "\tDPTXFIFO%2d: Size %d, Start 0x%08x\n", idx,
-			   val >> DPTXFSIZn_DPTxFSize_SHIFT,
-			   val & DPTXFSIZn_DPTxFStAddr_MASK);
+			   val >> FIFOSIZE_DEPTH_SHIFT,
+			   val & FIFOSIZE_STARTADDR_MASK);
 	}
 
 	return 0;
@@ -3662,14 +3482,14 @@ static int s3c_hsotg_probe(struct platform_device *pdev)
 	}
 
 	/* Set default UTMI width */
-	hsotg->phyif = GUSBCFG_PHYIf16;
+	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;
+		hsotg->phyif = GUSBCFG_PHYIF8;
 
 	if (hsotg->phy)
 		phy_init(hsotg->phy);
diff --git a/drivers/usb/dwc2/hw.h b/drivers/usb/dwc2/hw.h
index 9c92a3c7588..51248b93549 100644
--- a/drivers/usb/dwc2/hw.h
+++ b/drivers/usb/dwc2/hw.h
@@ -109,6 +109,7 @@
 #define GUSBCFG_FSINTF			(1 << 5)
 #define GUSBCFG_ULPI_UTMI_SEL		(1 << 4)
 #define GUSBCFG_PHYIF16			(1 << 3)
+#define GUSBCFG_PHYIF8			(0 << 3)
 #define GUSBCFG_TOUTCAL_MASK		(0x7 << 0)
 #define GUSBCFG_TOUTCAL_SHIFT		0
 #define GUSBCFG_TOUTCAL_LIMIT		0x7
@@ -403,6 +404,7 @@
 #define FIFOSIZE_DEPTH_SHIFT		16
 #define FIFOSIZE_STARTADDR_MASK		(0xffff << 0)
 #define FIFOSIZE_STARTADDR_SHIFT	0
+#define FIFOSIZE_DEPTH_GET(_x)		(((_x) >> 16) & 0xffff)
 
 /* Device mode registers */
 
@@ -519,11 +521,11 @@
 #define DXEPCTL_STALL			(1 << 21)
 #define DXEPCTL_SNP			(1 << 20)
 #define DXEPCTL_EPTYPE_MASK		(0x3 << 18)
-#define DXEPCTL_EPTYPE_SHIFT		18
-#define DXEPCTL_EPTYPE_CONTROL		0
-#define DXEPCTL_EPTYPE_ISO		1
-#define DXEPCTL_EPTYPE_BULK		2
-#define DXEPCTL_EPTYPE_INTTERUPT	3
+#define DXEPCTL_EPTYPE_CONTROL		(0x0 << 18)
+#define DXEPCTL_EPTYPE_ISO		(0x1 << 18)
+#define DXEPCTL_EPTYPE_BULK		(0x2 << 18)
+#define DXEPCTL_EPTYPE_INTERRUPT	(0x3 << 18)
+
 #define DXEPCTL_NAKSTS			(1 << 17)
 #define DXEPCTL_DPID			(1 << 16)
 #define DXEPCTL_EOFRNUM			(1 << 16)
diff --git a/drivers/usb/dwc2/platform.c b/drivers/usb/dwc2/platform.c
index eaba547ce26..a10e7a35357 100644
--- a/drivers/usb/dwc2/platform.c
+++ b/drivers/usb/dwc2/platform.c
@@ -134,6 +134,12 @@ static int dwc2_driver_probe(struct platform_device *dev)
 		/* Default all params to autodetect */
 		dwc2_set_all_params(&defparams, -1);
 		params = &defparams;
+
+		/*
+		 * Disable descriptor dma mode by default as the HW can support
+		 * it, but does not support it for SPLIT transactions.
+		 */
+		defparams.dma_desc_enable = 0;
 	}
 
 	hsotg = devm_kzalloc(&dev->dev, sizeof(*hsotg), GFP_KERNEL);
diff --git a/drivers/usb/dwc3/Kconfig b/drivers/usb/dwc3/Kconfig
index e2c730fc9a9..8eb996e4f05 100644
--- a/drivers/usb/dwc3/Kconfig
+++ b/drivers/usb/dwc3/Kconfig
@@ -44,7 +44,7 @@ comment "Platform Glue Driver Support"
 
 config USB_DWC3_OMAP
 	tristate "Texas Instruments OMAP5 and similar Platforms"
-	depends on EXTCON
+	depends on EXTCON && (ARCH_OMAP2PLUS || COMPILE_TEST)
 	default USB_DWC3
 	help
 	  Some platforms from Texas Instruments like OMAP5, DRA7xxx and
@@ -54,6 +54,7 @@ config USB_DWC3_OMAP
 
 config USB_DWC3_EXYNOS
 	tristate "Samsung Exynos Platform"
+	depends on ARCH_EXYNOS || COMPILE_TEST
 	default USB_DWC3
 	help
 	  Recent Exynos5 SoCs ship with one DesignWare Core USB3 IP inside,
@@ -72,6 +73,7 @@ config USB_DWC3_PCI
 
 config USB_DWC3_KEYSTONE
 	tristate "Texas Instruments Keystone2 Platforms"
+	depends on ARCH_KEYSTONE || COMPILE_TEST
 	default USB_DWC3
 	help
 	  Support of USB2/3 functionality in TI Keystone2 platforms.
diff --git a/drivers/usb/dwc3/core.c b/drivers/usb/dwc3/core.c
index 10aaaae9af2..eb69eb9f06c 100644
--- a/drivers/usb/dwc3/core.c
+++ b/drivers/usb/dwc3/core.c
@@ -486,70 +486,20 @@ static void dwc3_core_exit(struct dwc3 *dwc)
 	phy_exit(dwc->usb3_generic_phy);
 }
 
-#define DWC3_ALIGN_MASK		(16 - 1)
-
-static int dwc3_probe(struct platform_device *pdev)
+static int dwc3_core_get_phy(struct dwc3 *dwc)
 {
-	struct device		*dev = &pdev->dev;
-	struct dwc3_platform_data *pdata = dev_get_platdata(dev);
+	struct device		*dev = dwc->dev;
 	struct device_node	*node = dev->of_node;
-	struct resource		*res;
-	struct dwc3		*dwc;
-
-	int			ret = -ENOMEM;
-
-	void __iomem		*regs;
-	void			*mem;
-
-	mem = devm_kzalloc(dev, sizeof(*dwc) + DWC3_ALIGN_MASK, GFP_KERNEL);
-	if (!mem) {
-		dev_err(dev, "not enough memory\n");
-		return -ENOMEM;
-	}
-	dwc = PTR_ALIGN(mem, DWC3_ALIGN_MASK + 1);
-	dwc->mem = mem;
-
-	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if (!res) {
-		dev_err(dev, "missing IRQ\n");
-		return -ENODEV;
-	}
-	dwc->xhci_resources[1].start = res->start;
-	dwc->xhci_resources[1].end = res->end;
-	dwc->xhci_resources[1].flags = res->flags;
-	dwc->xhci_resources[1].name = res->name;
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(dev, "missing memory resource\n");
-		return -ENODEV;
-	}
+	int ret;
 
 	if (node) {
-		dwc->maximum_speed = of_usb_get_maximum_speed(node);
-
 		dwc->usb2_phy = devm_usb_get_phy_by_phandle(dev, "usb-phy", 0);
 		dwc->usb3_phy = devm_usb_get_phy_by_phandle(dev, "usb-phy", 1);
-
-		dwc->needs_fifo_resize = of_property_read_bool(node, "tx-fifo-resize");
-		dwc->dr_mode = of_usb_get_dr_mode(node);
-	} else if (pdata) {
-		dwc->maximum_speed = pdata->maximum_speed;
-
-		dwc->usb2_phy = devm_usb_get_phy(dev, USB_PHY_TYPE_USB2);
-		dwc->usb3_phy = devm_usb_get_phy(dev, USB_PHY_TYPE_USB3);
-
-		dwc->needs_fifo_resize = pdata->tx_fifo_resize;
-		dwc->dr_mode = pdata->dr_mode;
 	} else {
 		dwc->usb2_phy = devm_usb_get_phy(dev, USB_PHY_TYPE_USB2);
 		dwc->usb3_phy = devm_usb_get_phy(dev, USB_PHY_TYPE_USB3);
 	}
 
-	/* default to superspeed if no maximum_speed passed */
-	if (dwc->maximum_speed == USB_SPEED_UNKNOWN)
-		dwc->maximum_speed = USB_SPEED_SUPER;
-
 	if (IS_ERR(dwc->usb2_phy)) {
 		ret = PTR_ERR(dwc->usb2_phy);
 		if (ret == -ENXIO || ret == -ENODEV) {
@@ -600,6 +550,132 @@ static int dwc3_probe(struct platform_device *pdev)
 		}
 	}
 
+	return 0;
+}
+
+static int dwc3_core_init_mode(struct dwc3 *dwc)
+{
+	struct device *dev = dwc->dev;
+	int ret;
+
+	switch (dwc->dr_mode) {
+	case USB_DR_MODE_PERIPHERAL:
+		dwc3_set_mode(dwc, DWC3_GCTL_PRTCAP_DEVICE);
+		ret = dwc3_gadget_init(dwc);
+		if (ret) {
+			dev_err(dev, "failed to initialize gadget\n");
+			return ret;
+		}
+		break;
+	case USB_DR_MODE_HOST:
+		dwc3_set_mode(dwc, DWC3_GCTL_PRTCAP_HOST);
+		ret = dwc3_host_init(dwc);
+		if (ret) {
+			dev_err(dev, "failed to initialize host\n");
+			return ret;
+		}
+		break;
+	case USB_DR_MODE_OTG:
+		dwc3_set_mode(dwc, DWC3_GCTL_PRTCAP_OTG);
+		ret = dwc3_host_init(dwc);
+		if (ret) {
+			dev_err(dev, "failed to initialize host\n");
+			return ret;
+		}
+
+		ret = dwc3_gadget_init(dwc);
+		if (ret) {
+			dev_err(dev, "failed to initialize gadget\n");
+			return ret;
+		}
+		break;
+	default:
+		dev_err(dev, "Unsupported mode of operation %d\n", dwc->dr_mode);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void dwc3_core_exit_mode(struct dwc3 *dwc)
+{
+	switch (dwc->dr_mode) {
+	case USB_DR_MODE_PERIPHERAL:
+		dwc3_gadget_exit(dwc);
+		break;
+	case USB_DR_MODE_HOST:
+		dwc3_host_exit(dwc);
+		break;
+	case USB_DR_MODE_OTG:
+		dwc3_host_exit(dwc);
+		dwc3_gadget_exit(dwc);
+		break;
+	default:
+		/* do nothing */
+		break;
+	}
+}
+
+#define DWC3_ALIGN_MASK		(16 - 1)
+
+static int dwc3_probe(struct platform_device *pdev)
+{
+	struct device		*dev = &pdev->dev;
+	struct dwc3_platform_data *pdata = dev_get_platdata(dev);
+	struct device_node	*node = dev->of_node;
+	struct resource		*res;
+	struct dwc3		*dwc;
+
+	int			ret;
+
+	void __iomem		*regs;
+	void			*mem;
+
+	mem = devm_kzalloc(dev, sizeof(*dwc) + DWC3_ALIGN_MASK, GFP_KERNEL);
+	if (!mem) {
+		dev_err(dev, "not enough memory\n");
+		return -ENOMEM;
+	}
+	dwc = PTR_ALIGN(mem, DWC3_ALIGN_MASK + 1);
+	dwc->mem = mem;
+	dwc->dev = dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res) {
+		dev_err(dev, "missing IRQ\n");
+		return -ENODEV;
+	}
+	dwc->xhci_resources[1].start = res->start;
+	dwc->xhci_resources[1].end = res->end;
+	dwc->xhci_resources[1].flags = res->flags;
+	dwc->xhci_resources[1].name = res->name;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(dev, "missing memory resource\n");
+		return -ENODEV;
+	}
+
+	if (node) {
+		dwc->maximum_speed = of_usb_get_maximum_speed(node);
+
+		dwc->needs_fifo_resize = of_property_read_bool(node, "tx-fifo-resize");
+		dwc->dr_mode = of_usb_get_dr_mode(node);
+	} else if (pdata) {
+		dwc->maximum_speed = pdata->maximum_speed;
+
+		dwc->needs_fifo_resize = pdata->tx_fifo_resize;
+		dwc->dr_mode = pdata->dr_mode;
+	}
+
+	/* default to superspeed if no maximum_speed passed */
+	if (dwc->maximum_speed == USB_SPEED_UNKNOWN)
+		dwc->maximum_speed = USB_SPEED_SUPER;
+
+	ret = dwc3_core_get_phy(dwc);
+	if (ret)
+		return ret;
+
 	dwc->xhci_resources[0].start = res->start;
 	dwc->xhci_resources[0].end = dwc->xhci_resources[0].start +
 					DWC3_XHCI_REGS_END;
@@ -621,7 +697,6 @@ static int dwc3_probe(struct platform_device *pdev)
 
 	dwc->regs	= regs;
 	dwc->regs_size	= resource_size(res);
-	dwc->dev	= dev;
 
 	dev->dma_mask	= dev->parent->dma_mask;
 	dev->dma_parms	= dev->parent->dma_parms;
@@ -670,41 +745,9 @@ static int dwc3_probe(struct platform_device *pdev)
 		goto err_usb3phy_power;
 	}
 
-	switch (dwc->dr_mode) {
-	case USB_DR_MODE_PERIPHERAL:
-		dwc3_set_mode(dwc, DWC3_GCTL_PRTCAP_DEVICE);
-		ret = dwc3_gadget_init(dwc);
-		if (ret) {
-			dev_err(dev, "failed to initialize gadget\n");
-			goto err2;
-		}
-		break;
-	case USB_DR_MODE_HOST:
-		dwc3_set_mode(dwc, DWC3_GCTL_PRTCAP_HOST);
-		ret = dwc3_host_init(dwc);
-		if (ret) {
-			dev_err(dev, "failed to initialize host\n");
-			goto err2;
-		}
-		break;
-	case USB_DR_MODE_OTG:
-		dwc3_set_mode(dwc, DWC3_GCTL_PRTCAP_OTG);
-		ret = dwc3_host_init(dwc);
-		if (ret) {
-			dev_err(dev, "failed to initialize host\n");
-			goto err2;
-		}
-
-		ret = dwc3_gadget_init(dwc);
-		if (ret) {
-			dev_err(dev, "failed to initialize gadget\n");
-			goto err2;
-		}
-		break;
-	default:
-		dev_err(dev, "Unsupported mode of operation %d\n", dwc->dr_mode);
+	ret = dwc3_core_init_mode(dwc);
+	if (ret)
 		goto err2;
-	}
 
 	ret = dwc3_debugfs_init(dwc);
 	if (ret) {
@@ -717,21 +760,7 @@ static int dwc3_probe(struct platform_device *pdev)
 	return 0;
 
 err3:
-	switch (dwc->dr_mode) {
-	case USB_DR_MODE_PERIPHERAL:
-		dwc3_gadget_exit(dwc);
-		break;
-	case USB_DR_MODE_HOST:
-		dwc3_host_exit(dwc);
-		break;
-	case USB_DR_MODE_OTG:
-		dwc3_host_exit(dwc);
-		dwc3_gadget_exit(dwc);
-		break;
-	default:
-		/* do nothing */
-		break;
-	}
+	dwc3_core_exit_mode(dwc);
 
 err2:
 	dwc3_event_buffers_cleanup(dwc);
@@ -766,23 +795,7 @@ static int dwc3_remove(struct platform_device *pdev)
 	pm_runtime_disable(&pdev->dev);
 
 	dwc3_debugfs_exit(dwc);
-
-	switch (dwc->dr_mode) {
-	case USB_DR_MODE_PERIPHERAL:
-		dwc3_gadget_exit(dwc);
-		break;
-	case USB_DR_MODE_HOST:
-		dwc3_host_exit(dwc);
-		break;
-	case USB_DR_MODE_OTG:
-		dwc3_host_exit(dwc);
-		dwc3_gadget_exit(dwc);
-		break;
-	default:
-		/* do nothing */
-		break;
-	}
-
+	dwc3_core_exit_mode(dwc);
 	dwc3_event_buffers_cleanup(dwc);
 	dwc3_free_event_buffers(dwc);
 	dwc3_core_exit(dwc);
diff --git a/drivers/usb/dwc3/dwc3-exynos.c b/drivers/usb/dwc3/dwc3-exynos.c
index 28c8ad79f5e..f9fb8adb785 100644
--- a/drivers/usb/dwc3/dwc3-exynos.c
+++ b/drivers/usb/dwc3/dwc3-exynos.c
@@ -24,9 +24,10 @@
 #include <linux/dma-mapping.h>
 #include <linux/clk.h>
 #include <linux/usb/otg.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 #include <linux/of.h>
 #include <linux/of_platform.h>
+#include <linux/regulator/consumer.h>
 
 struct dwc3_exynos {
 	struct platform_device	*usb2_phy;
@@ -34,17 +35,19 @@ struct dwc3_exynos {
 	struct device		*dev;
 
 	struct clk		*clk;
+	struct regulator	*vdd33;
+	struct regulator	*vdd10;
 };
 
 static int dwc3_exynos_register_phys(struct dwc3_exynos *exynos)
 {
-	struct usb_phy_gen_xceiv_platform_data pdata;
+	struct usb_phy_generic_platform_data pdata;
 	struct platform_device	*pdev;
 	int			ret;
 
 	memset(&pdata, 0x00, sizeof(pdata));
 
-	pdev = platform_device_alloc("usb_phy_gen_xceiv", PLATFORM_DEVID_AUTO);
+	pdev = platform_device_alloc("usb_phy_generic", PLATFORM_DEVID_AUTO);
 	if (!pdev)
 		return -ENOMEM;
 
@@ -56,7 +59,7 @@ static int dwc3_exynos_register_phys(struct dwc3_exynos *exynos)
 	if (ret)
 		goto err1;
 
-	pdev = platform_device_alloc("usb_phy_gen_xceiv", PLATFORM_DEVID_AUTO);
+	pdev = platform_device_alloc("usb_phy_generic", PLATFORM_DEVID_AUTO);
 	if (!pdev) {
 		ret = -ENOMEM;
 		goto err1;
@@ -107,12 +110,12 @@ static int dwc3_exynos_probe(struct platform_device *pdev)
 	struct device		*dev = &pdev->dev;
 	struct device_node	*node = dev->of_node;
 
-	int			ret = -ENOMEM;
+	int			ret;
 
 	exynos = devm_kzalloc(dev, sizeof(*exynos), GFP_KERNEL);
 	if (!exynos) {
 		dev_err(dev, "not enough memory\n");
-		goto err1;
+		return -ENOMEM;
 	}
 
 	/*
@@ -122,21 +125,20 @@ static int dwc3_exynos_probe(struct platform_device *pdev)
 	 */
 	ret = dma_coerce_mask_and_coherent(dev, DMA_BIT_MASK(32));
 	if (ret)
-		goto err1;
+		return ret;
 
 	platform_set_drvdata(pdev, exynos);
 
 	ret = dwc3_exynos_register_phys(exynos);
 	if (ret) {
 		dev_err(dev, "couldn't register PHYs\n");
-		goto err1;
+		return ret;
 	}
 
 	clk = devm_clk_get(dev, "usbdrd30");
 	if (IS_ERR(clk)) {
 		dev_err(dev, "couldn't get clock\n");
-		ret = -EINVAL;
-		goto err1;
+		return -EINVAL;
 	}
 
 	exynos->dev	= dev;
@@ -144,23 +146,48 @@ static int dwc3_exynos_probe(struct platform_device *pdev)
 
 	clk_prepare_enable(exynos->clk);
 
+	exynos->vdd33 = devm_regulator_get(dev, "vdd33");
+	if (IS_ERR(exynos->vdd33)) {
+		ret = PTR_ERR(exynos->vdd33);
+		goto err2;
+	}
+	ret = regulator_enable(exynos->vdd33);
+	if (ret) {
+		dev_err(dev, "Failed to enable VDD33 supply\n");
+		goto err2;
+	}
+
+	exynos->vdd10 = devm_regulator_get(dev, "vdd10");
+	if (IS_ERR(exynos->vdd10)) {
+		ret = PTR_ERR(exynos->vdd10);
+		goto err3;
+	}
+	ret = regulator_enable(exynos->vdd10);
+	if (ret) {
+		dev_err(dev, "Failed to enable VDD10 supply\n");
+		goto err3;
+	}
+
 	if (node) {
 		ret = of_platform_populate(node, NULL, NULL, dev);
 		if (ret) {
 			dev_err(dev, "failed to add dwc3 core\n");
-			goto err2;
+			goto err4;
 		}
 	} else {
 		dev_err(dev, "no device node, failed to add dwc3 core\n");
 		ret = -ENODEV;
-		goto err2;
+		goto err4;
 	}
 
 	return 0;
 
+err4:
+	regulator_disable(exynos->vdd10);
+err3:
+	regulator_disable(exynos->vdd33);
 err2:
 	clk_disable_unprepare(clk);
-err1:
 	return ret;
 }
 
@@ -174,6 +201,9 @@ static int dwc3_exynos_remove(struct platform_device *pdev)
 
 	clk_disable_unprepare(exynos->clk);
 
+	regulator_disable(exynos->vdd33);
+	regulator_disable(exynos->vdd10);
+
 	return 0;
 }
 
@@ -192,12 +222,27 @@ static int dwc3_exynos_suspend(struct device *dev)
 
 	clk_disable(exynos->clk);
 
+	regulator_disable(exynos->vdd33);
+	regulator_disable(exynos->vdd10);
+
 	return 0;
 }
 
 static int dwc3_exynos_resume(struct device *dev)
 {
 	struct dwc3_exynos *exynos = dev_get_drvdata(dev);
+	int ret;
+
+	ret = regulator_enable(exynos->vdd33);
+	if (ret) {
+		dev_err(dev, "Failed to enable VDD33 supply\n");
+		return ret;
+	}
+	ret = regulator_enable(exynos->vdd10);
+	if (ret) {
+		dev_err(dev, "Failed to enable VDD10 supply\n");
+		return ret;
+	}
 
 	clk_enable(exynos->clk);
 
diff --git a/drivers/usb/dwc3/dwc3-omap.c b/drivers/usb/dwc3/dwc3-omap.c
index 1160ff41bed..4af4c356765 100644
--- a/drivers/usb/dwc3/dwc3-omap.c
+++ b/drivers/usb/dwc3/dwc3-omap.c
@@ -393,7 +393,7 @@ static int dwc3_omap_probe(struct platform_device *pdev)
 	struct extcon_dev	*edev;
 	struct regulator	*vbus_reg = NULL;
 
-	int			ret = -ENOMEM;
+	int			ret;
 	int			irq;
 
 	int			utmi_mode = 0;
diff --git a/drivers/usb/dwc3/dwc3-pci.c b/drivers/usb/dwc3/dwc3-pci.c
index f393c183cc6..a60bab7dfa0 100644
--- a/drivers/usb/dwc3/dwc3-pci.c
+++ b/drivers/usb/dwc3/dwc3-pci.c
@@ -23,7 +23,7 @@
 #include <linux/platform_device.h>
 
 #include <linux/usb/otg.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 
 /* FIXME define these in <linux/pci_ids.h> */
 #define PCI_VENDOR_ID_SYNOPSYS		0x16c3
@@ -40,13 +40,13 @@ struct dwc3_pci {
 
 static int dwc3_pci_register_phys(struct dwc3_pci *glue)
 {
-	struct usb_phy_gen_xceiv_platform_data pdata;
+	struct usb_phy_generic_platform_data pdata;
 	struct platform_device	*pdev;
 	int			ret;
 
 	memset(&pdata, 0x00, sizeof(pdata));
 
-	pdev = platform_device_alloc("usb_phy_gen_xceiv", 0);
+	pdev = platform_device_alloc("usb_phy_generic", 0);
 	if (!pdev)
 		return -ENOMEM;
 
@@ -58,7 +58,7 @@ static int dwc3_pci_register_phys(struct dwc3_pci *glue)
 	if (ret)
 		goto err1;
 
-	pdev = platform_device_alloc("usb_phy_gen_xceiv", 1);
+	pdev = platform_device_alloc("usb_phy_generic", 1);
 	if (!pdev) {
 		ret = -ENOMEM;
 		goto err1;
@@ -99,7 +99,7 @@ static int dwc3_pci_probe(struct pci_dev *pci,
 	struct resource		res[2];
 	struct platform_device	*dwc3;
 	struct dwc3_pci		*glue;
-	int			ret = -ENOMEM;
+	int			ret;
 	struct device		*dev = &pci->dev;
 
 	glue = devm_kzalloc(dev, sizeof(*glue), GFP_KERNEL);
@@ -110,7 +110,7 @@ static int dwc3_pci_probe(struct pci_dev *pci,
 
 	glue->dev = dev;
 
-	ret = pci_enable_device(pci);
+	ret = pcim_enable_device(pci);
 	if (ret) {
 		dev_err(dev, "failed to enable pci device\n");
 		return -ENODEV;
@@ -127,8 +127,7 @@ static int dwc3_pci_probe(struct pci_dev *pci,
 	dwc3 = platform_device_alloc("dwc3", PLATFORM_DEVID_AUTO);
 	if (!dwc3) {
 		dev_err(dev, "couldn't allocate dwc3 device\n");
-		ret = -ENOMEM;
-		goto err1;
+		return -ENOMEM;
 	}
 
 	memset(res, 0x00, sizeof(struct resource) * ARRAY_SIZE(res));
@@ -145,7 +144,7 @@ static int dwc3_pci_probe(struct pci_dev *pci,
 	ret = platform_device_add_resources(dwc3, res, ARRAY_SIZE(res));
 	if (ret) {
 		dev_err(dev, "couldn't add resources to dwc3 device\n");
-		goto err1;
+		return ret;
 	}
 
 	pci_set_drvdata(pci, glue);
@@ -167,9 +166,6 @@ static int dwc3_pci_probe(struct pci_dev *pci,
 
 err3:
 	platform_device_put(dwc3);
-err1:
-	pci_disable_device(pci);
-
 	return ret;
 }
 
@@ -180,7 +176,6 @@ static void dwc3_pci_remove(struct pci_dev *pci)
 	platform_device_unregister(glue->dwc3);
 	platform_device_unregister(glue->usb2_phy);
 	platform_device_unregister(glue->usb3_phy);
-	pci_disable_device(pci);
 }
 
 static const struct pci_device_id dwc3_pci_id_table[] = {
diff --git a/drivers/usb/dwc3/gadget.c b/drivers/usb/dwc3/gadget.c
index 70715eeeded..9d64dd02c57 100644
--- a/drivers/usb/dwc3/gadget.c
+++ b/drivers/usb/dwc3/gadget.c
@@ -298,11 +298,76 @@ static const char *dwc3_gadget_ep_cmd_string(u8 cmd)
 	}
 }
 
+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)
 {
 	u32		timeout = 500;
 	u32		reg;
 
+	dev_vdbg(dwc->dev, "generic cmd '%s' [%d] param %08x\n",
+			dwc3_gadget_generic_cmd_string(cmd), cmd, param);
+
 	dwc3_writel(dwc->regs, DWC3_DGCMDPAR, param);
 	dwc3_writel(dwc->regs, DWC3_DGCMD, cmd | DWC3_DGCMD_CMDACT);
 
@@ -332,9 +397,9 @@ int dwc3_send_gadget_ep_cmd(struct dwc3 *dwc, unsigned ep,
 	u32			timeout = 500;
 	u32			reg;
 
-	dev_vdbg(dwc->dev, "%s: cmd '%s' params %08x %08x %08x\n",
+	dev_vdbg(dwc->dev, "%s: cmd '%s' [%d] params %08x %08x %08x\n",
 			dep->name,
-			dwc3_gadget_ep_cmd_string(cmd), params->param0,
+			dwc3_gadget_ep_cmd_string(cmd), cmd, params->param0,
 			params->param1, params->param2);
 
 	dwc3_writel(dwc->regs, DWC3_DEPCMDPAR0(ep), params->param0);
@@ -515,7 +580,7 @@ static int __dwc3_gadget_ep_enable(struct dwc3_ep *dep,
 {
 	struct dwc3		*dwc = dep->dwc;
 	u32			reg;
-	int			ret = -ENOMEM;
+	int			ret;
 
 	dev_vdbg(dwc->dev, "Enabling %s\n", dep->name);
 
@@ -604,6 +669,10 @@ static int __dwc3_gadget_ep_disable(struct dwc3_ep *dep)
 
 	dwc3_remove_requests(dwc, dep);
 
+	/* make sure HW endpoint isn't stalled */
+	if (dep->flags & DWC3_EP_STALL)
+		__dwc3_gadget_ep_set_halt(dep, 0);
+
 	reg = dwc3_readl(dwc->regs, DWC3_DALEPENA);
 	reg &= ~DWC3_DALEPENA_EP(dep->number);
 	dwc3_writel(dwc->regs, DWC3_DALEPENA, reg);
@@ -2441,8 +2510,6 @@ static void dwc3_gadget_linksts_change_interrupt(struct dwc3 *dwc,
 		}
 	}
 
-	dwc->link_state = next;
-
 	switch (next) {
 	case DWC3_LINK_STATE_U1:
 		if (dwc->speed == USB_SPEED_SUPER)
@@ -2460,7 +2527,11 @@ static void dwc3_gadget_linksts_change_interrupt(struct dwc3 *dwc,
 		break;
 	}
 
-	dev_vdbg(dwc->dev, "%s link %d\n", __func__, dwc->link_state);
+	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;
 }
 
 static void dwc3_gadget_hibernation_interrupt(struct dwc3 *dwc,
diff --git a/drivers/usb/gadget/Kconfig b/drivers/usb/gadget/Kconfig
index 3557c7e5040..ba18e9c110c 100644
--- a/drivers/usb/gadget/Kconfig
+++ b/drivers/usb/gadget/Kconfig
@@ -157,7 +157,7 @@ config USB_AT91
 
 config USB_LPC32XX
 	tristate "LPC32XX USB Peripheral Controller"
-	depends on ARCH_LPC32XX
+	depends on ARCH_LPC32XX && I2C
 	select USB_ISP1301
 	help
 	   This option selects the USB device controller in the LPC32xx SoC.
@@ -226,7 +226,7 @@ config USB_GR_UDC
 config USB_OMAP
 	tristate "OMAP USB Device Controller"
 	depends on ARCH_OMAP1
-	select ISP1301_OMAP if MACH_OMAP_H2 || MACH_OMAP_H3
+	depends on ISP1301_OMAP || !(MACH_OMAP_H2 || MACH_OMAP_H3)
 	help
 	   Many Texas Instruments OMAP processors have flexible full
 	   speed USB device controllers, with support for up to 30
@@ -300,12 +300,6 @@ config USB_PXA27X
 	   dynamically linked module called "pxa27x_udc" and force all
 	   gadget drivers to also be dynamically linked.
 
-config USB_S3C_HSOTG
-	tristate "Designware/S3C HS/OtG USB Device controller"
-	help
-	  The Designware USB2.0 high-speed gadget controller
-	  integrated into many SoCs.
-
 config USB_S3C2410
 	tristate "S3C2410 USB Device Controller"
 	depends on ARCH_S3C24XX
diff --git a/drivers/usb/gadget/Makefile b/drivers/usb/gadget/Makefile
index 5f150bc1b4b..49514ea60a9 100644
--- a/drivers/usb/gadget/Makefile
+++ b/drivers/usb/gadget/Makefile
@@ -26,7 +26,6 @@ fsl_usb2_udc-$(CONFIG_ARCH_MXC)	+= fsl_mxc_udc.o
 obj-$(CONFIG_USB_M66592)	+= m66592-udc.o
 obj-$(CONFIG_USB_R8A66597)	+= r8a66597-udc.o
 obj-$(CONFIG_USB_FSL_QE)	+= fsl_qe_udc.o
-obj-$(CONFIG_USB_S3C_HSOTG)	+= s3c-hsotg.o
 obj-$(CONFIG_USB_S3C_HSUDC)	+= s3c-hsudc.o
 obj-$(CONFIG_USB_LPC32XX)	+= lpc32xx_udc.o
 obj-$(CONFIG_USB_EG20T)		+= pch_udc.o
diff --git a/drivers/usb/gadget/atmel_usba_udc.c b/drivers/usb/gadget/atmel_usba_udc.c
index 9f65324f9ae..76023ce449a 100644
--- a/drivers/usb/gadget/atmel_usba_udc.c
+++ b/drivers/usb/gadget/atmel_usba_udc.c
@@ -1686,7 +1686,7 @@ static irqreturn_t usba_udc_irq(int irq, void *devid)
 		reset_all_endpoints(udc);
 
 		if (udc->gadget.speed != USB_SPEED_UNKNOWN
-				&& udc->driver->disconnect) {
+				&& udc->driver && udc->driver->disconnect) {
 			udc->gadget.speed = USB_SPEED_UNKNOWN;
 			spin_unlock(&udc->lock);
 			udc->driver->disconnect(&udc->gadget);
diff --git a/drivers/usb/gadget/composite.c b/drivers/usb/gadget/composite.c
index fab906429b8..f8015193205 100644
--- a/drivers/usb/gadget/composite.c
+++ b/drivers/usb/gadget/composite.c
@@ -21,6 +21,24 @@
 #include <linux/usb/composite.h>
 #include <asm/unaligned.h>
 
+#include "u_os_desc.h"
+
+/**
+ * struct usb_os_string - represents OS String to be reported by a gadget
+ * @bLength: total length of the entire descritor, always 0x12
+ * @bDescriptorType: USB_DT_STRING
+ * @qwSignature: the OS String proper
+ * @bMS_VendorCode: code used by the host for subsequent requests
+ * @bPad: not used, must be zero
+ */
+struct usb_os_string {
+	__u8	bLength;
+	__u8	bDescriptorType;
+	__u8	qwSignature[OS_STRING_QW_SIGN_LEN];
+	__u8	bMS_VendorCode;
+	__u8	bPad;
+} __packed;
+
 /*
  * The code in this file is utility code, used to build a gadget driver
  * from one or more "function" drivers, one or more "configuration"
@@ -422,6 +440,7 @@ static int config_desc(struct usb_composite_dev *cdev, unsigned w_value)
 {
 	struct usb_gadget		*gadget = cdev->gadget;
 	struct usb_configuration	*c;
+	struct list_head		*pos;
 	u8				type = w_value >> 8;
 	enum usb_device_speed		speed = USB_SPEED_UNKNOWN;
 
@@ -440,7 +459,20 @@ static int config_desc(struct usb_composite_dev *cdev, unsigned w_value)
 
 	/* This is a lookup by config *INDEX* */
 	w_value &= 0xff;
-	list_for_each_entry(c, &cdev->configs, list) {
+
+	pos = &cdev->configs;
+	c = cdev->os_desc_config;
+	if (c)
+		goto check_config;
+
+	while ((pos = pos->next) !=  &cdev->configs) {
+		c = list_entry(pos, typeof(*c), list);
+
+		/* skip OS Descriptors config which is handled separately */
+		if (c == cdev->os_desc_config)
+			continue;
+
+check_config:
 		/* ignore configs that won't work at this speed */
 		switch (speed) {
 		case USB_SPEED_SUPER:
@@ -634,6 +666,7 @@ static int set_config(struct usb_composite_dev *cdev,
 	if (!c)
 		goto done;
 
+	usb_gadget_set_state(gadget, USB_STATE_CONFIGURED);
 	cdev->config = c;
 
 	/* Initialize all interfaces by setting them to altsetting zero. */
@@ -960,6 +993,19 @@ static int get_string(struct usb_composite_dev *cdev,
 		return s->bLength;
 	}
 
+	if (cdev->use_os_string && language == 0 && id == OS_STRING_IDX) {
+		struct usb_os_string *b = buf;
+		b->bLength = sizeof(*b);
+		b->bDescriptorType = USB_DT_STRING;
+		compiletime_assert(
+			sizeof(b->qwSignature) == sizeof(cdev->qw_sign),
+			"qwSignature size must be equal to qw_sign");
+		memcpy(&b->qwSignature, cdev->qw_sign, sizeof(b->qwSignature));
+		b->bMS_VendorCode = cdev->b_vendor_code;
+		b->bPad = 0;
+		return sizeof(*b);
+	}
+
 	list_for_each_entry(uc, &cdev->gstrings, list) {
 		struct usb_gadget_strings **sp;
 
@@ -1206,6 +1252,156 @@ static void composite_setup_complete(struct usb_ep *ep, struct usb_request *req)
 				req->status, req->actual, req->length);
 }
 
+static int count_ext_compat(struct usb_configuration *c)
+{
+	int i, res;
+
+	res = 0;
+	for (i = 0; i < c->next_interface_id; ++i) {
+		struct usb_function *f;
+		int j;
+
+		f = c->interface[i];
+		for (j = 0; j < f->os_desc_n; ++j) {
+			struct usb_os_desc *d;
+
+			if (i != f->os_desc_table[j].if_id)
+				continue;
+			d = f->os_desc_table[j].os_desc;
+			if (d && d->ext_compat_id)
+				++res;
+		}
+	}
+	BUG_ON(res > 255);
+	return res;
+}
+
+static void fill_ext_compat(struct usb_configuration *c, u8 *buf)
+{
+	int i, count;
+
+	count = 16;
+	for (i = 0; i < c->next_interface_id; ++i) {
+		struct usb_function *f;
+		int j;
+
+		f = c->interface[i];
+		for (j = 0; j < f->os_desc_n; ++j) {
+			struct usb_os_desc *d;
+
+			if (i != f->os_desc_table[j].if_id)
+				continue;
+			d = f->os_desc_table[j].os_desc;
+			if (d && d->ext_compat_id) {
+				*buf++ = i;
+				*buf++ = 0x01;
+				memcpy(buf, d->ext_compat_id, 16);
+				buf += 22;
+			} else {
+				++buf;
+				*buf = 0x01;
+				buf += 23;
+			}
+			count += 24;
+			if (count >= 4096)
+				return;
+		}
+	}
+}
+
+static int count_ext_prop(struct usb_configuration *c, int interface)
+{
+	struct usb_function *f;
+	int j;
+
+	f = c->interface[interface];
+	for (j = 0; j < f->os_desc_n; ++j) {
+		struct usb_os_desc *d;
+
+		if (interface != f->os_desc_table[j].if_id)
+			continue;
+		d = f->os_desc_table[j].os_desc;
+		if (d && d->ext_compat_id)
+			return d->ext_prop_count;
+	}
+	return 0;
+}
+
+static int len_ext_prop(struct usb_configuration *c, int interface)
+{
+	struct usb_function *f;
+	struct usb_os_desc *d;
+	int j, res;
+
+	res = 10; /* header length */
+	f = c->interface[interface];
+	for (j = 0; j < f->os_desc_n; ++j) {
+		if (interface != f->os_desc_table[j].if_id)
+			continue;
+		d = f->os_desc_table[j].os_desc;
+		if (d)
+			return min(res + d->ext_prop_len, 4096);
+	}
+	return res;
+}
+
+static int fill_ext_prop(struct usb_configuration *c, int interface, u8 *buf)
+{
+	struct usb_function *f;
+	struct usb_os_desc *d;
+	struct usb_os_desc_ext_prop *ext_prop;
+	int j, count, n, ret;
+	u8 *start = buf;
+
+	f = c->interface[interface];
+	for (j = 0; j < f->os_desc_n; ++j) {
+		if (interface != f->os_desc_table[j].if_id)
+			continue;
+		d = f->os_desc_table[j].os_desc;
+		if (d)
+			list_for_each_entry(ext_prop, &d->ext_prop, entry) {
+				/* 4kB minus header length */
+				n = buf - start;
+				if (n >= 4086)
+					return 0;
+
+				count = ext_prop->data_len +
+					ext_prop->name_len + 14;
+				if (count > 4086 - n)
+					return -EINVAL;
+				usb_ext_prop_put_size(buf, count);
+				usb_ext_prop_put_type(buf, ext_prop->type);
+				ret = usb_ext_prop_put_name(buf, ext_prop->name,
+							    ext_prop->name_len);
+				if (ret < 0)
+					return ret;
+				switch (ext_prop->type) {
+				case USB_EXT_PROP_UNICODE:
+				case USB_EXT_PROP_UNICODE_ENV:
+				case USB_EXT_PROP_UNICODE_LINK:
+					usb_ext_prop_put_unicode(buf, ret,
+							 ext_prop->data,
+							 ext_prop->data_len);
+					break;
+				case USB_EXT_PROP_BINARY:
+					usb_ext_prop_put_binary(buf, ret,
+							ext_prop->data,
+							ext_prop->data_len);
+					break;
+				case USB_EXT_PROP_LE32:
+					/* not implemented */
+				case USB_EXT_PROP_BE32:
+					/* not implemented */
+				default:
+					return -EINVAL;
+				}
+				buf += count;
+			}
+	}
+
+	return 0;
+}
+
 /*
  * The setup() callback implements all the ep0 functionality that's
  * not handled lower down, in hardware or the hardware driver(like
@@ -1415,6 +1611,91 @@ composite_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
 		break;
 	default:
 unknown:
+		/*
+		 * OS descriptors handling
+		 */
+		if (cdev->use_os_string && cdev->os_desc_config &&
+		    (ctrl->bRequest & USB_TYPE_VENDOR) &&
+		    ctrl->bRequest == cdev->b_vendor_code) {
+			struct usb_request		*req;
+			struct usb_configuration	*os_desc_cfg;
+			u8				*buf;
+			int				interface;
+			int				count = 0;
+
+			req = cdev->os_desc_req;
+			req->complete = composite_setup_complete;
+			buf = req->buf;
+			os_desc_cfg = cdev->os_desc_config;
+			memset(buf, 0, w_length);
+			buf[5] = 0x01;
+			switch (ctrl->bRequestType & USB_RECIP_MASK) {
+			case USB_RECIP_DEVICE:
+				if (w_index != 0x4 || (w_value >> 8))
+					break;
+				buf[6] = w_index;
+				if (w_length == 0x10) {
+					/* Number of ext compat interfaces */
+					count = count_ext_compat(os_desc_cfg);
+					buf[8] = count;
+					count *= 24; /* 24 B/ext compat desc */
+					count += 16; /* header */
+					put_unaligned_le32(count, buf);
+					value = w_length;
+				} else {
+					/* "extended compatibility ID"s */
+					count = count_ext_compat(os_desc_cfg);
+					buf[8] = count;
+					count *= 24; /* 24 B/ext compat desc */
+					count += 16; /* header */
+					put_unaligned_le32(count, buf);
+					buf += 16;
+					fill_ext_compat(os_desc_cfg, buf);
+					value = w_length;
+				}
+				break;
+			case USB_RECIP_INTERFACE:
+				if (w_index != 0x5 || (w_value >> 8))
+					break;
+				interface = w_value & 0xFF;
+				buf[6] = w_index;
+				if (w_length == 0x0A) {
+					count = count_ext_prop(os_desc_cfg,
+						interface);
+					put_unaligned_le16(count, buf + 8);
+					count = len_ext_prop(os_desc_cfg,
+						interface);
+					put_unaligned_le32(count, buf);
+
+					value = w_length;
+				} else {
+					count = count_ext_prop(os_desc_cfg,
+						interface);
+					put_unaligned_le16(count, buf + 8);
+					count = len_ext_prop(os_desc_cfg,
+						interface);
+					put_unaligned_le32(count, buf);
+					buf += 10;
+					value = fill_ext_prop(os_desc_cfg,
+							      interface, buf);
+					if (value < 0)
+						return value;
+
+					value = w_length;
+				}
+				break;
+			}
+			req->length = value;
+			req->zero = value < w_length;
+			value = usb_ep_queue(gadget->ep0, req, GFP_ATOMIC);
+			if (value < 0) {
+				DBG(cdev, "ep_queue --> %d\n", value);
+				req->status = 0;
+				composite_setup_complete(gadget->ep0, req);
+			}
+			return value;
+		}
+
 		VDBG(cdev,
 			"non-core control req%02x.%02x v%04x i%04x l%d\n",
 			ctrl->bRequestType, ctrl->bRequest,
@@ -1638,6 +1919,29 @@ fail:
 	return ret;
 }
 
+int composite_os_desc_req_prepare(struct usb_composite_dev *cdev,
+				  struct usb_ep *ep0)
+{
+	int ret = 0;
+
+	cdev->os_desc_req = usb_ep_alloc_request(ep0, GFP_KERNEL);
+	if (!cdev->os_desc_req) {
+		ret = PTR_ERR(cdev->os_desc_req);
+		goto end;
+	}
+
+	/* OS feature descriptor length <= 4kB */
+	cdev->os_desc_req->buf = kmalloc(4096, GFP_KERNEL);
+	if (!cdev->os_desc_req->buf) {
+		ret = PTR_ERR(cdev->os_desc_req->buf);
+		kfree(cdev->os_desc_req);
+		goto end;
+	}
+	cdev->os_desc_req->complete = composite_setup_complete;
+end:
+	return ret;
+}
+
 void composite_dev_cleanup(struct usb_composite_dev *cdev)
 {
 	struct usb_gadget_string_container *uc, *tmp;
@@ -1646,6 +1950,10 @@ void composite_dev_cleanup(struct usb_composite_dev *cdev)
 		list_del(&uc->list);
 		kfree(uc);
 	}
+	if (cdev->os_desc_req) {
+		kfree(cdev->os_desc_req->buf);
+		usb_ep_free_request(cdev->gadget->ep0, cdev->os_desc_req);
+	}
 	if (cdev->req) {
 		kfree(cdev->req->buf);
 		usb_ep_free_request(cdev->gadget->ep0, cdev->req);
@@ -1683,6 +1991,12 @@ static int composite_bind(struct usb_gadget *gadget,
 	if (status < 0)
 		goto fail;
 
+	if (cdev->use_os_string) {
+		status = composite_os_desc_req_prepare(cdev, gadget->ep0);
+		if (status)
+			goto fail;
+	}
+
 	update_unchanged_dev_desc(&cdev->desc, composite->dev);
 
 	/* has userspace failed to provide a serial number? */
diff --git a/drivers/usb/gadget/configfs.c b/drivers/usb/gadget/configfs.c
index 7d1cc01796b..2ddcd635ca2 100644
--- a/drivers/usb/gadget/configfs.c
+++ b/drivers/usb/gadget/configfs.c
@@ -2,9 +2,12 @@
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/device.h>
+#include <linux/nls.h>
 #include <linux/usb/composite.h>
 #include <linux/usb/gadget_configfs.h>
 #include "configfs.h"
+#include "u_f.h"
+#include "u_os_desc.h"
 
 int check_user_usb_string(const char *name,
 		struct usb_gadget_strings *stringtab_dev)
@@ -43,7 +46,8 @@ struct gadget_info {
 	struct config_group functions_group;
 	struct config_group configs_group;
 	struct config_group strings_group;
-	struct config_group *default_groups[4];
+	struct config_group os_desc_group;
+	struct config_group *default_groups[5];
 
 	struct mutex lock;
 	struct usb_gadget_strings *gstrings[MAX_USB_STRING_LANGS + 1];
@@ -56,6 +60,9 @@ struct gadget_info {
 #endif
 	struct usb_composite_driver composite;
 	struct usb_composite_dev cdev;
+	bool use_os_desc;
+	char b_vendor_code;
+	char qw_sign[OS_STRING_QW_SIGN_LEN];
 };
 
 struct config_usb_cfg {
@@ -79,6 +86,10 @@ struct gadget_strings {
 	struct list_head list;
 };
 
+struct os_desc {
+	struct config_group group;
+};
+
 struct gadget_config_name {
 	struct usb_gadget_strings stringtab_dev;
 	struct usb_string strings;
@@ -736,6 +747,525 @@ static void gadget_strings_attr_release(struct config_item *item)
 USB_CONFIG_STRING_RW_OPS(gadget_strings);
 USB_CONFIG_STRINGS_LANG(gadget_strings, gadget_info);
 
+static inline struct os_desc *to_os_desc(struct config_item *item)
+{
+	return container_of(to_config_group(item), struct os_desc, group);
+}
+
+CONFIGFS_ATTR_STRUCT(os_desc);
+CONFIGFS_ATTR_OPS(os_desc);
+
+static ssize_t os_desc_use_show(struct os_desc *os_desc, char *page)
+{
+	struct gadget_info *gi;
+
+	gi = to_gadget_info(os_desc->group.cg_item.ci_parent);
+
+	return sprintf(page, "%d", gi->use_os_desc);
+}
+
+static ssize_t os_desc_use_store(struct os_desc *os_desc, const char *page,
+				 size_t len)
+{
+	struct gadget_info *gi;
+	int ret;
+	bool use;
+
+	gi = to_gadget_info(os_desc->group.cg_item.ci_parent);
+
+	mutex_lock(&gi->lock);
+	ret = strtobool(page, &use);
+	if (!ret) {
+		gi->use_os_desc = use;
+		ret = len;
+	}
+	mutex_unlock(&gi->lock);
+
+	return ret;
+}
+
+static struct os_desc_attribute os_desc_use =
+	__CONFIGFS_ATTR(use, S_IRUGO | S_IWUSR,
+			os_desc_use_show,
+			os_desc_use_store);
+
+static ssize_t os_desc_b_vendor_code_show(struct os_desc *os_desc, char *page)
+{
+	struct gadget_info *gi;
+
+	gi = to_gadget_info(os_desc->group.cg_item.ci_parent);
+
+	return sprintf(page, "%d", gi->b_vendor_code);
+}
+
+static ssize_t os_desc_b_vendor_code_store(struct os_desc *os_desc,
+					   const char *page, size_t len)
+{
+	struct gadget_info *gi;
+	int ret;
+	u8 b_vendor_code;
+
+	gi = to_gadget_info(os_desc->group.cg_item.ci_parent);
+
+	mutex_lock(&gi->lock);
+	ret = kstrtou8(page, 0, &b_vendor_code);
+	if (!ret) {
+		gi->b_vendor_code = b_vendor_code;
+		ret = len;
+	}
+	mutex_unlock(&gi->lock);
+
+	return ret;
+}
+
+static struct os_desc_attribute os_desc_b_vendor_code =
+	__CONFIGFS_ATTR(b_vendor_code, S_IRUGO | S_IWUSR,
+			os_desc_b_vendor_code_show,
+			os_desc_b_vendor_code_store);
+
+static ssize_t os_desc_qw_sign_show(struct os_desc *os_desc, char *page)
+{
+	struct gadget_info *gi;
+
+	gi = to_gadget_info(os_desc->group.cg_item.ci_parent);
+
+	memcpy(page, gi->qw_sign, OS_STRING_QW_SIGN_LEN);
+
+	return OS_STRING_QW_SIGN_LEN;
+}
+
+static ssize_t os_desc_qw_sign_store(struct os_desc *os_desc, const char *page,
+				     size_t len)
+{
+	struct gadget_info *gi;
+	int res, l;
+
+	gi = to_gadget_info(os_desc->group.cg_item.ci_parent);
+	l = min((int)len, OS_STRING_QW_SIGN_LEN >> 1);
+	if (page[l - 1] == '\n')
+		--l;
+
+	mutex_lock(&gi->lock);
+	res = utf8s_to_utf16s(page, l,
+			      UTF16_LITTLE_ENDIAN, (wchar_t *) gi->qw_sign,
+			      OS_STRING_QW_SIGN_LEN);
+	if (res > 0)
+		res = len;
+	mutex_unlock(&gi->lock);
+
+	return res;
+}
+
+static struct os_desc_attribute os_desc_qw_sign =
+	__CONFIGFS_ATTR(qw_sign, S_IRUGO | S_IWUSR,
+			os_desc_qw_sign_show,
+			os_desc_qw_sign_store);
+
+static struct configfs_attribute *os_desc_attrs[] = {
+	&os_desc_use.attr,
+	&os_desc_b_vendor_code.attr,
+	&os_desc_qw_sign.attr,
+	NULL,
+};
+
+static void os_desc_attr_release(struct config_item *item)
+{
+	struct os_desc *os_desc = to_os_desc(item);
+	kfree(os_desc);
+}
+
+static int os_desc_link(struct config_item *os_desc_ci,
+			struct config_item *usb_cfg_ci)
+{
+	struct gadget_info *gi = container_of(to_config_group(os_desc_ci),
+					struct gadget_info, os_desc_group);
+	struct usb_composite_dev *cdev = &gi->cdev;
+	struct config_usb_cfg *c_target =
+		container_of(to_config_group(usb_cfg_ci),
+			     struct config_usb_cfg, group);
+	struct usb_configuration *c;
+	int ret;
+
+	mutex_lock(&gi->lock);
+	list_for_each_entry(c, &cdev->configs, list) {
+		if (c == &c_target->c)
+			break;
+	}
+	if (c != &c_target->c) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (cdev->os_desc_config) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	cdev->os_desc_config = &c_target->c;
+	ret = 0;
+
+out:
+	mutex_unlock(&gi->lock);
+	return ret;
+}
+
+static int os_desc_unlink(struct config_item *os_desc_ci,
+			  struct config_item *usb_cfg_ci)
+{
+	struct gadget_info *gi = container_of(to_config_group(os_desc_ci),
+					struct gadget_info, os_desc_group);
+	struct usb_composite_dev *cdev = &gi->cdev;
+
+	mutex_lock(&gi->lock);
+	if (gi->udc_name)
+		unregister_gadget(gi);
+	cdev->os_desc_config = NULL;
+	WARN_ON(gi->udc_name);
+	mutex_unlock(&gi->lock);
+	return 0;
+}
+
+static struct configfs_item_operations os_desc_ops = {
+	.release                = os_desc_attr_release,
+	.show_attribute         = os_desc_attr_show,
+	.store_attribute        = os_desc_attr_store,
+	.allow_link		= os_desc_link,
+	.drop_link		= os_desc_unlink,
+};
+
+static struct config_item_type os_desc_type = {
+	.ct_item_ops	= &os_desc_ops,
+	.ct_attrs	= os_desc_attrs,
+	.ct_owner	= THIS_MODULE,
+};
+
+CONFIGFS_ATTR_STRUCT(usb_os_desc);
+CONFIGFS_ATTR_OPS(usb_os_desc);
+
+
+static inline struct usb_os_desc_ext_prop
+*to_usb_os_desc_ext_prop(struct config_item *item)
+{
+	return container_of(item, struct usb_os_desc_ext_prop, item);
+}
+
+CONFIGFS_ATTR_STRUCT(usb_os_desc_ext_prop);
+CONFIGFS_ATTR_OPS(usb_os_desc_ext_prop);
+
+static ssize_t ext_prop_type_show(struct usb_os_desc_ext_prop *ext_prop,
+				  char *page)
+{
+	return sprintf(page, "%d", ext_prop->type);
+}
+
+static ssize_t ext_prop_type_store(struct usb_os_desc_ext_prop *ext_prop,
+				   const char *page, size_t len)
+{
+	struct usb_os_desc *desc = to_usb_os_desc(ext_prop->item.ci_parent);
+	u8 type;
+	int ret;
+
+	if (desc->opts_mutex)
+		mutex_lock(desc->opts_mutex);
+	ret = kstrtou8(page, 0, &type);
+	if (ret)
+		goto end;
+	if (type < USB_EXT_PROP_UNICODE || type > USB_EXT_PROP_UNICODE_MULTI) {
+		ret = -EINVAL;
+		goto end;
+	}
+
+	if ((ext_prop->type == USB_EXT_PROP_BINARY ||
+	    ext_prop->type == USB_EXT_PROP_LE32 ||
+	    ext_prop->type == USB_EXT_PROP_BE32) &&
+	    (type == USB_EXT_PROP_UNICODE ||
+	    type == USB_EXT_PROP_UNICODE_ENV ||
+	    type == USB_EXT_PROP_UNICODE_LINK))
+		ext_prop->data_len <<= 1;
+	else if ((ext_prop->type == USB_EXT_PROP_UNICODE ||
+		   ext_prop->type == USB_EXT_PROP_UNICODE_ENV ||
+		   ext_prop->type == USB_EXT_PROP_UNICODE_LINK) &&
+		   (type == USB_EXT_PROP_BINARY ||
+		   type == USB_EXT_PROP_LE32 ||
+		   type == USB_EXT_PROP_BE32))
+		ext_prop->data_len >>= 1;
+	ext_prop->type = type;
+	ret = len;
+
+end:
+	if (desc->opts_mutex)
+		mutex_unlock(desc->opts_mutex);
+	return ret;
+}
+
+static ssize_t ext_prop_data_show(struct usb_os_desc_ext_prop *ext_prop,
+				  char *page)
+{
+	int len = ext_prop->data_len;
+
+	if (ext_prop->type == USB_EXT_PROP_UNICODE ||
+	    ext_prop->type == USB_EXT_PROP_UNICODE_ENV ||
+	    ext_prop->type == USB_EXT_PROP_UNICODE_LINK)
+		len >>= 1;
+	memcpy(page, ext_prop->data, len);
+
+	return len;
+}
+
+static ssize_t ext_prop_data_store(struct usb_os_desc_ext_prop *ext_prop,
+				   const char *page, size_t len)
+{
+	struct usb_os_desc *desc = to_usb_os_desc(ext_prop->item.ci_parent);
+	char *new_data;
+	size_t ret_len = len;
+
+	if (page[len - 1] == '\n' || page[len - 1] == '\0')
+		--len;
+	new_data = kzalloc(len, GFP_KERNEL);
+	if (!new_data)
+		return -ENOMEM;
+
+	memcpy(new_data, page, len);
+
+	if (desc->opts_mutex)
+		mutex_lock(desc->opts_mutex);
+	kfree(ext_prop->data);
+	ext_prop->data = new_data;
+	desc->ext_prop_len -= ext_prop->data_len;
+	ext_prop->data_len = len;
+	desc->ext_prop_len += ext_prop->data_len;
+	if (ext_prop->type == USB_EXT_PROP_UNICODE ||
+	    ext_prop->type == USB_EXT_PROP_UNICODE_ENV ||
+	    ext_prop->type == USB_EXT_PROP_UNICODE_LINK) {
+		desc->ext_prop_len -= ext_prop->data_len;
+		ext_prop->data_len <<= 1;
+		ext_prop->data_len += 2;
+		desc->ext_prop_len += ext_prop->data_len;
+	}
+	if (desc->opts_mutex)
+		mutex_unlock(desc->opts_mutex);
+	return ret_len;
+}
+
+static struct usb_os_desc_ext_prop_attribute ext_prop_type =
+	__CONFIGFS_ATTR(type, S_IRUGO | S_IWUSR,
+			ext_prop_type_show, ext_prop_type_store);
+
+static struct usb_os_desc_ext_prop_attribute ext_prop_data =
+	__CONFIGFS_ATTR(data, S_IRUGO | S_IWUSR,
+			ext_prop_data_show, ext_prop_data_store);
+
+static struct configfs_attribute *ext_prop_attrs[] = {
+	&ext_prop_type.attr,
+	&ext_prop_data.attr,
+	NULL,
+};
+
+static void usb_os_desc_ext_prop_release(struct config_item *item)
+{
+	struct usb_os_desc_ext_prop *ext_prop = to_usb_os_desc_ext_prop(item);
+
+	kfree(ext_prop); /* frees a whole chunk */
+}
+
+static struct configfs_item_operations ext_prop_ops = {
+	.release		= usb_os_desc_ext_prop_release,
+	.show_attribute		= usb_os_desc_ext_prop_attr_show,
+	.store_attribute	= usb_os_desc_ext_prop_attr_store,
+};
+
+static struct config_item *ext_prop_make(
+		struct config_group *group,
+		const char *name)
+{
+	struct usb_os_desc_ext_prop *ext_prop;
+	struct config_item_type *ext_prop_type;
+	struct usb_os_desc *desc;
+	char *vlabuf;
+
+	vla_group(data_chunk);
+	vla_item(data_chunk, struct usb_os_desc_ext_prop, ext_prop, 1);
+	vla_item(data_chunk, struct config_item_type, ext_prop_type, 1);
+
+	vlabuf = kzalloc(vla_group_size(data_chunk), GFP_KERNEL);
+	if (!vlabuf)
+		return ERR_PTR(-ENOMEM);
+
+	ext_prop = vla_ptr(vlabuf, data_chunk, ext_prop);
+	ext_prop_type = vla_ptr(vlabuf, data_chunk, ext_prop_type);
+
+	desc = container_of(group, struct usb_os_desc, group);
+	ext_prop_type->ct_item_ops = &ext_prop_ops;
+	ext_prop_type->ct_attrs = ext_prop_attrs;
+	ext_prop_type->ct_owner = desc->owner;
+
+	config_item_init_type_name(&ext_prop->item, name, ext_prop_type);
+
+	ext_prop->name = kstrdup(name, GFP_KERNEL);
+	if (!ext_prop->name) {
+		kfree(vlabuf);
+		return ERR_PTR(-ENOMEM);
+	}
+	desc->ext_prop_len += 14;
+	ext_prop->name_len = 2 * strlen(ext_prop->name) + 2;
+	if (desc->opts_mutex)
+		mutex_lock(desc->opts_mutex);
+	desc->ext_prop_len += ext_prop->name_len;
+	list_add_tail(&ext_prop->entry, &desc->ext_prop);
+	++desc->ext_prop_count;
+	if (desc->opts_mutex)
+		mutex_unlock(desc->opts_mutex);
+
+	return &ext_prop->item;
+}
+
+static void ext_prop_drop(struct config_group *group, struct config_item *item)
+{
+	struct usb_os_desc_ext_prop *ext_prop = to_usb_os_desc_ext_prop(item);
+	struct usb_os_desc *desc = to_usb_os_desc(&group->cg_item);
+
+	if (desc->opts_mutex)
+		mutex_lock(desc->opts_mutex);
+	list_del(&ext_prop->entry);
+	--desc->ext_prop_count;
+	kfree(ext_prop->name);
+	desc->ext_prop_len -= (ext_prop->name_len + ext_prop->data_len + 14);
+	if (desc->opts_mutex)
+		mutex_unlock(desc->opts_mutex);
+	config_item_put(item);
+}
+
+static struct configfs_group_operations interf_grp_ops = {
+	.make_item	= &ext_prop_make,
+	.drop_item	= &ext_prop_drop,
+};
+
+static struct configfs_item_operations interf_item_ops = {
+	.show_attribute		= usb_os_desc_attr_show,
+	.store_attribute	= usb_os_desc_attr_store,
+};
+
+static ssize_t rndis_grp_compatible_id_show(struct usb_os_desc *desc,
+					    char *page)
+{
+	memcpy(page, desc->ext_compat_id, 8);
+	return 8;
+}
+
+static ssize_t rndis_grp_compatible_id_store(struct usb_os_desc *desc,
+					     const char *page, size_t len)
+{
+	int l;
+
+	l = min_t(int, 8, len);
+	if (page[l - 1] == '\n')
+		--l;
+	if (desc->opts_mutex)
+		mutex_lock(desc->opts_mutex);
+	memcpy(desc->ext_compat_id, page, l);
+	desc->ext_compat_id[l] = '\0';
+
+	if (desc->opts_mutex)
+		mutex_unlock(desc->opts_mutex);
+
+	return len;
+}
+
+static struct usb_os_desc_attribute rndis_grp_attr_compatible_id =
+	__CONFIGFS_ATTR(compatible_id, S_IRUGO | S_IWUSR,
+			rndis_grp_compatible_id_show,
+			rndis_grp_compatible_id_store);
+
+static ssize_t rndis_grp_sub_compatible_id_show(struct usb_os_desc *desc,
+						char *page)
+{
+	memcpy(page, desc->ext_compat_id + 8, 8);
+	return 8;
+}
+
+static ssize_t rndis_grp_sub_compatible_id_store(struct usb_os_desc *desc,
+						 const char *page, size_t len)
+{
+	int l;
+
+	l = min_t(int, 8, len);
+	if (page[l - 1] == '\n')
+		--l;
+	if (desc->opts_mutex)
+		mutex_lock(desc->opts_mutex);
+	memcpy(desc->ext_compat_id + 8, page, l);
+	desc->ext_compat_id[l + 8] = '\0';
+
+	if (desc->opts_mutex)
+		mutex_unlock(desc->opts_mutex);
+
+	return len;
+}
+
+static struct usb_os_desc_attribute rndis_grp_attr_sub_compatible_id =
+	__CONFIGFS_ATTR(sub_compatible_id, S_IRUGO | S_IWUSR,
+			rndis_grp_sub_compatible_id_show,
+			rndis_grp_sub_compatible_id_store);
+
+static struct configfs_attribute *interf_grp_attrs[] = {
+	&rndis_grp_attr_compatible_id.attr,
+	&rndis_grp_attr_sub_compatible_id.attr,
+	NULL
+};
+
+int usb_os_desc_prepare_interf_dir(struct config_group *parent,
+				   int n_interf,
+				   struct usb_os_desc **desc,
+				   struct module *owner)
+{
+	struct config_group **f_default_groups, *os_desc_group,
+				**interface_groups;
+	struct config_item_type *os_desc_type, *interface_type;
+
+	vla_group(data_chunk);
+	vla_item(data_chunk, struct config_group *, f_default_groups, 2);
+	vla_item(data_chunk, struct config_group, os_desc_group, 1);
+	vla_item(data_chunk, struct config_group *, interface_groups,
+		 n_interf + 1);
+	vla_item(data_chunk, struct config_item_type, os_desc_type, 1);
+	vla_item(data_chunk, struct config_item_type, interface_type, 1);
+
+	char *vlabuf = kzalloc(vla_group_size(data_chunk), GFP_KERNEL);
+	if (!vlabuf)
+		return -ENOMEM;
+
+	f_default_groups = vla_ptr(vlabuf, data_chunk, f_default_groups);
+	os_desc_group = vla_ptr(vlabuf, data_chunk, os_desc_group);
+	os_desc_type = vla_ptr(vlabuf, data_chunk, os_desc_type);
+	interface_groups = vla_ptr(vlabuf, data_chunk, interface_groups);
+	interface_type = vla_ptr(vlabuf, data_chunk, interface_type);
+
+	parent->default_groups = f_default_groups;
+	os_desc_type->ct_owner = owner;
+	config_group_init_type_name(os_desc_group, "os_desc", os_desc_type);
+	f_default_groups[0] = os_desc_group;
+
+	os_desc_group->default_groups = interface_groups;
+	interface_type->ct_item_ops = &interf_item_ops;
+	interface_type->ct_group_ops = &interf_grp_ops;
+	interface_type->ct_attrs = interf_grp_attrs;
+	interface_type->ct_owner = owner;
+
+	while (n_interf--) {
+		struct usb_os_desc *d;
+
+		d = desc[n_interf];
+		d->owner = owner;
+		config_group_init_type_name(&d->group, "", interface_type);
+		config_item_set_name(&d->group.cg_item, "interface.%d",
+				     n_interf);
+		interface_groups[n_interf] = &d->group;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(usb_os_desc_prepare_interf_dir);
+
 static int configfs_do_nothing(struct usb_composite_dev *cdev)
 {
 	WARN_ON(1);
@@ -745,6 +1275,9 @@ static int configfs_do_nothing(struct usb_composite_dev *cdev)
 int composite_dev_prepare(struct usb_composite_driver *composite,
 		struct usb_composite_dev *dev);
 
+int composite_os_desc_req_prepare(struct usb_composite_dev *cdev,
+				  struct usb_ep *ep0);
+
 static void purge_configs_funcs(struct gadget_info *gi)
 {
 	struct usb_configuration	*c;
@@ -793,7 +1326,7 @@ static int configfs_composite_bind(struct usb_gadget *gadget,
 	ret = -EINVAL;
 
 	if (list_empty(&gi->cdev.configs)) {
-		pr_err("Need atleast one configuration in %s.\n",
+		pr_err("Need at least one configuration in %s.\n",
 				gi->composite.name);
 		goto err_comp_cleanup;
 	}
@@ -804,7 +1337,7 @@ static int configfs_composite_bind(struct usb_gadget *gadget,
 
 		cfg = container_of(c, struct config_usb_cfg, c);
 		if (list_empty(&cfg->func_list)) {
-			pr_err("Config %s/%d of %s needs atleast one function.\n",
+			pr_err("Config %s/%d of %s needs at least one function.\n",
 			      c->label, c->bConfigurationValue,
 			      gi->composite.name);
 			goto err_comp_cleanup;
@@ -839,6 +1372,12 @@ static int configfs_composite_bind(struct usb_gadget *gadget,
 		gi->cdev.desc.iSerialNumber = s[USB_GADGET_SERIAL_IDX].id;
 	}
 
+	if (gi->use_os_desc) {
+		cdev->use_os_string = true;
+		cdev->b_vendor_code = gi->b_vendor_code;
+		memcpy(cdev->qw_sign, gi->qw_sign, OS_STRING_QW_SIGN_LEN);
+	}
+
 	/* Go through all configs, attach all functions */
 	list_for_each_entry(c, &gi->cdev.configs, list) {
 		struct config_usb_cfg *cfg;
@@ -874,6 +1413,12 @@ static int configfs_composite_bind(struct usb_gadget *gadget,
 		}
 		usb_ep_autoconfig_reset(cdev->gadget);
 	}
+	if (cdev->use_os_string) {
+		ret = composite_os_desc_req_prepare(cdev, gadget->ep0);
+		if (ret)
+			goto err_purge_funcs;
+	}
+
 	usb_ep_autoconfig_reset(cdev->gadget);
 	return 0;
 
@@ -929,6 +1474,7 @@ static struct config_group *gadgets_make(
 	gi->group.default_groups[0] = &gi->functions_group;
 	gi->group.default_groups[1] = &gi->configs_group;
 	gi->group.default_groups[2] = &gi->strings_group;
+	gi->group.default_groups[3] = &gi->os_desc_group;
 
 	config_group_init_type_name(&gi->functions_group, "functions",
 			&functions_type);
@@ -936,6 +1482,8 @@ static struct config_group *gadgets_make(
 			&config_desc_type);
 	config_group_init_type_name(&gi->strings_group, "strings",
 			&gadget_strings_strings_type);
+	config_group_init_type_name(&gi->os_desc_group, "os_desc",
+			&os_desc_type);
 
 	gi->composite.bind = configfs_do_nothing;
 	gi->composite.unbind = configfs_do_nothing;
@@ -1005,7 +1553,7 @@ void unregister_gadget_item(struct config_item *item)
 
 	unregister_gadget(gi);
 }
-EXPORT_SYMBOL(unregister_gadget_item);
+EXPORT_SYMBOL_GPL(unregister_gadget_item);
 
 static int __init gadget_cfs_init(void)
 {
diff --git a/drivers/usb/gadget/configfs.h b/drivers/usb/gadget/configfs.h
index a7b564a913d..a14ac792c69 100644
--- a/drivers/usb/gadget/configfs.h
+++ b/drivers/usb/gadget/configfs.h
@@ -1,6 +1,18 @@
 #ifndef USB__GADGET__CONFIGFS__H
 #define USB__GADGET__CONFIGFS__H
 
+#include <linux/configfs.h>
+
 void unregister_gadget_item(struct config_item *item);
 
+int usb_os_desc_prepare_interf_dir(struct config_group *parent,
+				   int n_interf,
+				   struct usb_os_desc **desc,
+				   struct module *owner);
+
+static inline struct usb_os_desc *to_usb_os_desc(struct config_item *item)
+{
+	return container_of(to_config_group(item), struct usb_os_desc, group);
+}
+
 #endif /*  USB__GADGET__CONFIGFS__H */
diff --git a/drivers/usb/gadget/dummy_hcd.c b/drivers/usb/gadget/dummy_hcd.c
index 8c06430dcc4..2b54955d316 100644
--- a/drivers/usb/gadget/dummy_hcd.c
+++ b/drivers/usb/gadget/dummy_hcd.c
@@ -561,7 +561,6 @@ static int dummy_disable(struct usb_ep *_ep)
 	struct dummy_ep		*ep;
 	struct dummy		*dum;
 	unsigned long		flags;
-	int			retval;
 
 	ep = usb_ep_to_dummy_ep(_ep);
 	if (!_ep || !ep->desc || _ep->name == ep0name)
@@ -571,12 +570,11 @@ static int dummy_disable(struct usb_ep *_ep)
 	spin_lock_irqsave(&dum->lock, flags);
 	ep->desc = NULL;
 	ep->stream_en = 0;
-	retval = 0;
 	nuke(dum, ep);
 	spin_unlock_irqrestore(&dum->lock, flags);
 
 	dev_dbg(udc_dev(dum), "disabled %s\n", _ep->name);
-	return retval;
+	return 0;
 }
 
 static struct usb_request *dummy_alloc_request(struct usb_ep *_ep,
diff --git a/drivers/usb/gadget/f_fs.c b/drivers/usb/gadget/f_fs.c
index 1e12b3ee56f..74202d67f91 100644
--- a/drivers/usb/gadget/f_fs.c
+++ b/drivers/usb/gadget/f_fs.c
@@ -33,36 +33,11 @@
 #include <linux/poll.h>
 
 #include "u_fs.h"
+#include "u_f.h"
 #include "configfs.h"
 
 #define FUNCTIONFS_MAGIC	0xa647361 /* Chosen by a honest dice roll ;) */
 
-/* Variable Length Array Macros **********************************************/
-#define vla_group(groupname) size_t groupname##__next = 0
-#define vla_group_size(groupname) groupname##__next
-
-#define vla_item(groupname, type, name, n) \
-	size_t groupname##_##name##__offset = ({			       \
-		size_t align_mask = __alignof__(type) - 1;		       \
-		size_t offset = (groupname##__next + align_mask) & ~align_mask;\
-		size_t size = (n) * sizeof(type);			       \
-		groupname##__next = offset + size;			       \
-		offset;							       \
-	})
-
-#define vla_item_with_sz(groupname, type, name, n) \
-	size_t groupname##_##name##__sz = (n) * sizeof(type);		       \
-	size_t groupname##_##name##__offset = ({			       \
-		size_t align_mask = __alignof__(type) - 1;		       \
-		size_t offset = (groupname##__next + align_mask) & ~align_mask;\
-		size_t size = groupname##_##name##__sz;			       \
-		groupname##__next = offset + size;			       \
-		offset;							       \
-	})
-
-#define vla_ptr(ptr, groupname, name) \
-	((void *) ((char *)ptr + groupname##_##name##__offset))
-
 /* Reference counter handling */
 static void ffs_data_get(struct ffs_data *ffs);
 static void ffs_data_put(struct ffs_data *ffs);
@@ -190,7 +165,7 @@ ffs_sb_create_file(struct super_block *sb, const char *name, void *data,
 /* Devices management *******************************************************/
 
 DEFINE_MUTEX(ffs_lock);
-EXPORT_SYMBOL(ffs_lock);
+EXPORT_SYMBOL_GPL(ffs_lock);
 
 static struct ffs_dev *_ffs_find_dev(const char *name);
 static struct ffs_dev *_ffs_alloc_dev(void);
@@ -2883,7 +2858,7 @@ int ffs_name_dev(struct ffs_dev *dev, const char *name)
 
 	return ret;
 }
-EXPORT_SYMBOL(ffs_name_dev);
+EXPORT_SYMBOL_GPL(ffs_name_dev);
 
 int ffs_single_dev(struct ffs_dev *dev)
 {
@@ -2900,7 +2875,7 @@ int ffs_single_dev(struct ffs_dev *dev)
 	ffs_dev_unlock();
 	return ret;
 }
-EXPORT_SYMBOL(ffs_single_dev);
+EXPORT_SYMBOL_GPL(ffs_single_dev);
 
 /*
  * ffs_lock must be taken by the caller of this function
diff --git a/drivers/usb/gadget/f_rndis.c b/drivers/usb/gadget/f_rndis.c
index 9a4f49dc6ac..eed3ad87804 100644
--- a/drivers/usb/gadget/f_rndis.c
+++ b/drivers/usb/gadget/f_rndis.c
@@ -27,6 +27,7 @@
 #include "u_ether_configfs.h"
 #include "u_rndis.h"
 #include "rndis.h"
+#include "configfs.h"
 
 /*
  * This function is an RNDIS Ethernet port -- a Microsoft protocol that's
@@ -682,6 +683,15 @@ rndis_bind(struct usb_configuration *c, struct usb_function *f)
 
 	rndis_opts = container_of(f->fi, struct f_rndis_opts, func_inst);
 
+	if (cdev->use_os_string) {
+		f->os_desc_table = kzalloc(sizeof(*f->os_desc_table),
+					   GFP_KERNEL);
+		if (!f->os_desc_table)
+			return PTR_ERR(f->os_desc_table);
+		f->os_desc_n = 1;
+		f->os_desc_table[0].os_desc = &rndis_opts->rndis_os_desc;
+	}
+
 	/*
 	 * in drivers/usb/gadget/configfs.c:configfs_composite_bind()
 	 * configurations are bound in sequence with list_for_each_entry,
@@ -693,14 +703,16 @@ rndis_bind(struct usb_configuration *c, struct usb_function *f)
 		gether_set_gadget(rndis_opts->net, cdev->gadget);
 		status = gether_register_netdev(rndis_opts->net);
 		if (status)
-			return status;
+			goto fail;
 		rndis_opts->bound = true;
 	}
 
 	us = usb_gstrings_attach(cdev, rndis_strings,
 				 ARRAY_SIZE(rndis_string_defs));
-	if (IS_ERR(us))
-		return PTR_ERR(us);
+	if (IS_ERR(us)) {
+		status = PTR_ERR(us);
+		goto fail;
+	}
 	rndis_control_intf.iInterface = us[0].id;
 	rndis_data_intf.iInterface = us[1].id;
 	rndis_iad_descriptor.iFunction = us[2].id;
@@ -802,6 +814,8 @@ rndis_bind(struct usb_configuration *c, struct usb_function *f)
 	return 0;
 
 fail:
+	kfree(f->os_desc_table);
+	f->os_desc_n = 0;
 	usb_free_all_descriptors(f);
 
 	if (rndis->notify_req) {
@@ -834,7 +848,7 @@ void rndis_borrow_net(struct usb_function_instance *f, struct net_device *net)
 	opts->borrowed_net = opts->bound = true;
 	opts->net = net;
 }
-EXPORT_SYMBOL(rndis_borrow_net);
+EXPORT_SYMBOL_GPL(rndis_borrow_net);
 
 static inline struct f_rndis_opts *to_f_rndis_opts(struct config_item *item)
 {
@@ -882,16 +896,21 @@ static void rndis_free_inst(struct usb_function_instance *f)
 		else
 			free_netdev(opts->net);
 	}
+
+	kfree(opts->rndis_os_desc.group.default_groups); /* single VLA chunk */
 	kfree(opts);
 }
 
 static struct usb_function_instance *rndis_alloc_inst(void)
 {
 	struct f_rndis_opts *opts;
+	struct usb_os_desc *descs[1];
 
 	opts = kzalloc(sizeof(*opts), GFP_KERNEL);
 	if (!opts)
 		return ERR_PTR(-ENOMEM);
+	opts->rndis_os_desc.ext_compat_id = opts->rndis_ext_compat_id;
+
 	mutex_init(&opts->lock);
 	opts->func_inst.free_func_inst = rndis_free_inst;
 	opts->net = gether_setup_default();
@@ -900,7 +919,11 @@ static struct usb_function_instance *rndis_alloc_inst(void)
 		kfree(opts);
 		return ERR_CAST(net);
 	}
+	INIT_LIST_HEAD(&opts->rndis_os_desc.ext_prop);
 
+	descs[0] = &opts->rndis_os_desc;
+	usb_os_desc_prepare_interf_dir(&opts->func_inst.group, 1, descs,
+				       THIS_MODULE);
 	config_group_init_type_name(&opts->func_inst.group, "",
 				    &rndis_func_type);
 
@@ -925,6 +948,8 @@ static void rndis_unbind(struct usb_configuration *c, struct usb_function *f)
 {
 	struct f_rndis		*rndis = func_to_rndis(f);
 
+	kfree(f->os_desc_table);
+	f->os_desc_n = 0;
 	usb_free_all_descriptors(f);
 
 	kfree(rndis->notify_req->buf);
diff --git a/drivers/usb/gadget/f_subset.c b/drivers/usb/gadget/f_subset.c
index df4a0dcbc99..1ea8baf3333 100644
--- a/drivers/usb/gadget/f_subset.c
+++ b/drivers/usb/gadget/f_subset.c
@@ -276,7 +276,7 @@ static int geth_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
 	}
 
 	net = gether_connect(&geth->port);
-	return PTR_RET(net);
+	return PTR_ERR_OR_ZERO(net);
 }
 
 static void geth_disable(struct usb_function *f)
diff --git a/drivers/usb/gadget/f_uac2.c b/drivers/usb/gadget/f_uac2.c
index bc23040c779..6261db4a991 100644
--- a/drivers/usb/gadget/f_uac2.c
+++ b/drivers/usb/gadget/f_uac2.c
@@ -196,7 +196,7 @@ agdev_iso_complete(struct usb_ep *ep, struct usb_request *req)
 	struct snd_uac2_chip *uac2 = prm->uac2;
 
 	/* i/f shutting down */
-	if (!prm->ep_enabled)
+	if (!prm->ep_enabled || req->status == -ESHUTDOWN)
 		return;
 
 	/*
@@ -974,8 +974,6 @@ afunc_bind(struct usb_configuration *cfg, struct usb_function *fn)
 	prm->rbuf = kzalloc(prm->max_psize * USB_XFERS, GFP_KERNEL);
 	if (!prm->rbuf) {
 		prm->max_psize = 0;
-		dev_err(&uac2->pdev.dev,
-			"%s:%d Error!\n", __func__, __LINE__);
 		goto err;
 	}
 
@@ -984,8 +982,6 @@ afunc_bind(struct usb_configuration *cfg, struct usb_function *fn)
 	prm->rbuf = kzalloc(prm->max_psize * USB_XFERS, GFP_KERNEL);
 	if (!prm->rbuf) {
 		prm->max_psize = 0;
-		dev_err(&uac2->pdev.dev,
-			"%s:%d Error!\n", __func__, __LINE__);
 		goto err;
 	}
 
@@ -1298,10 +1294,8 @@ static int audio_bind_config(struct usb_configuration *cfg)
 	int res;
 
 	agdev_g = kzalloc(sizeof *agdev_g, GFP_KERNEL);
-	if (agdev_g == NULL) {
-		printk(KERN_ERR "Unable to allocate audio gadget\n");
+	if (agdev_g == NULL)
 		return -ENOMEM;
-	}
 
 	res = usb_string_ids_tab(cfg->cdev, strings_fn);
 	if (res)
diff --git a/drivers/usb/gadget/fotg210-udc.c b/drivers/usb/gadget/fotg210-udc.c
index 2d0305280e8..e143d69f601 100644
--- a/drivers/usb/gadget/fotg210-udc.c
+++ b/drivers/usb/gadget/fotg210-udc.c
@@ -1112,17 +1112,13 @@ static int fotg210_udc_probe(struct platform_device *pdev)
 
 	/* initialize udc */
 	fotg210 = kzalloc(sizeof(struct fotg210_udc), GFP_KERNEL);
-	if (fotg210 == NULL) {
-		pr_err("kzalloc error\n");
+	if (fotg210 == NULL)
 		goto err_alloc;
-	}
 
 	for (i = 0; i < FOTG210_MAX_NUM_EP; i++) {
 		_ep[i] = kzalloc(sizeof(struct fotg210_ep), GFP_KERNEL);
-		if (_ep[i] == NULL) {
-			pr_err("_ep kzalloc error\n");
+		if (_ep[i] == NULL)
 			goto err_alloc;
-		}
 		fotg210->ep[i] = _ep[i];
 	}
 
diff --git a/drivers/usb/gadget/fsl_udc_core.c b/drivers/usb/gadget/fsl_udc_core.c
index a2f26cdb56f..28e4fc95702 100644
--- a/drivers/usb/gadget/fsl_udc_core.c
+++ b/drivers/usb/gadget/fsl_udc_core.c
@@ -2256,10 +2256,8 @@ static int __init struct_udc_setup(struct fsl_udc *udc,
 	udc->phy_mode = pdata->phy_mode;
 
 	udc->eps = kzalloc(sizeof(struct fsl_ep) * udc->max_ep, GFP_KERNEL);
-	if (!udc->eps) {
-		ERR("malloc fsl_ep failed\n");
+	if (!udc->eps)
 		return -1;
-	}
 
 	/* initialized QHs, take care of alignment */
 	size = udc->max_ep * sizeof(struct ep_queue_head);
@@ -2342,10 +2340,8 @@ static int __init fsl_udc_probe(struct platform_device *pdev)
 	u32 dccparams;
 
 	udc_controller = kzalloc(sizeof(struct fsl_udc), GFP_KERNEL);
-	if (udc_controller == NULL) {
-		ERR("malloc udc failed\n");
+	if (udc_controller == NULL)
 		return -ENOMEM;
-	}
 
 	pdata = dev_get_platdata(&pdev->dev);
 	udc_controller->pdata = pdata;
diff --git a/drivers/usb/gadget/fusb300_udc.c b/drivers/usb/gadget/fusb300_udc.c
index 6423f1840ed..3deb4e93807 100644
--- a/drivers/usb/gadget/fusb300_udc.c
+++ b/drivers/usb/gadget/fusb300_udc.c
@@ -1400,17 +1400,13 @@ static int __init fusb300_probe(struct platform_device *pdev)
 
 	/* initialize udc */
 	fusb300 = kzalloc(sizeof(struct fusb300), GFP_KERNEL);
-	if (fusb300 == NULL) {
-		pr_err("kzalloc error\n");
+	if (fusb300 == NULL)
 		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) {
-			pr_err("_ep kzalloc error\n");
+		if (_ep[i] == NULL)
 			goto clean_up;
-		}
 		fusb300->ep[i] = _ep[i];
 	}
 
diff --git a/drivers/usb/gadget/gr_udc.c b/drivers/usb/gadget/gr_udc.c
index f984ee75324..99a37ed03e2 100644
--- a/drivers/usb/gadget/gr_udc.c
+++ b/drivers/usb/gadget/gr_udc.c
@@ -143,6 +143,7 @@ static void gr_seq_ep_show(struct seq_file *seq, struct gr_ep *ep)
 	seq_printf(seq, "  wedged = %d\n", ep->wedged);
 	seq_printf(seq, "  callback = %d\n", ep->callback);
 	seq_printf(seq, "  maxpacket = %d\n", ep->ep.maxpacket);
+	seq_printf(seq, "  maxpacket_limit = %d\n", ep->ep.maxpacket_limit);
 	seq_printf(seq, "  bytes_per_buffer = %d\n", ep->bytes_per_buffer);
 	if (mode == 1 || mode == 3)
 		seq_printf(seq, "  nt = %d\n",
@@ -1541,6 +1542,10 @@ static int gr_ep_enable(struct usb_ep *_ep,
 	} else if (max == 0) {
 		dev_err(dev->dev, "Max payload cannot be set to 0\n");
 		return -EINVAL;
+	} else if (max > ep->ep.maxpacket_limit) {
+		dev_err(dev->dev, "Requested max payload %d > limit %d\n",
+			max, ep->ep.maxpacket_limit);
+		return -EINVAL;
 	}
 
 	spin_lock(&ep->dev->lock);
@@ -1679,7 +1684,7 @@ static int gr_queue_ext(struct usb_ep *_ep, struct usb_request *_req,
 	if (ep->is_in)
 		gr_dbgprint_request("EXTERN", ep, req);
 
-	ret = gr_queue(ep, req, gfp_flags);
+	ret = gr_queue(ep, req, GFP_ATOMIC);
 
 	spin_unlock(&ep->dev->lock);
 
@@ -1985,8 +1990,8 @@ static int gr_ep_init(struct gr_udc *dev, int num, int is_in, u32 maxplimit)
 	INIT_LIST_HEAD(&ep->queue);
 
 	if (num == 0) {
-		_req = gr_alloc_request(&ep->ep, GFP_KERNEL);
-		buf = devm_kzalloc(dev->dev, PAGE_SIZE, GFP_DMA | GFP_KERNEL);
+		_req = gr_alloc_request(&ep->ep, GFP_ATOMIC);
+		buf = devm_kzalloc(dev->dev, PAGE_SIZE, GFP_DMA | GFP_ATOMIC);
 		if (!_req || !buf) {
 			/* possible _req freed by gr_probe via gr_remove */
 			return -ENOMEM;
@@ -2020,9 +2025,7 @@ static int gr_udc_init(struct gr_udc *dev)
 	u32 dmactrl_val;
 	int i;
 	int ret = 0;
-	u32 *bufsizes;
 	u32 bufsize;
-	int len;
 
 	gr_set_address(dev, 0);
 
@@ -2033,19 +2036,17 @@ static int gr_udc_init(struct gr_udc *dev)
 	INIT_LIST_HEAD(&dev->ep_list);
 	gr_set_ep0state(dev, GR_EP0_DISCONNECT);
 
-	bufsizes = (u32 *)of_get_property(np, "epobufsizes", &len);
-	len /= sizeof(u32);
 	for (i = 0; i < dev->nepo; i++) {
-		bufsize = (bufsizes && i < len) ? bufsizes[i] : 1024;
+		if (of_property_read_u32_index(np, "epobufsizes", i, &bufsize))
+			bufsize = 1024;
 		ret = gr_ep_init(dev, i, 0, bufsize);
 		if (ret)
 			return ret;
 	}
 
-	bufsizes = (u32 *)of_get_property(np, "epibufsizes", &len);
-	len /= sizeof(u32);
 	for (i = 0; i < dev->nepi; i++) {
-		bufsize = (bufsizes && i < len) ? bufsizes[i] : 1024;
+		if (of_property_read_u32_index(np, "epibufsizes", i, &bufsize))
+			bufsize = 1024;
 		ret = gr_ep_init(dev, i, 1, bufsize);
 		if (ret)
 			return ret;
@@ -2065,9 +2066,9 @@ static int gr_udc_init(struct gr_udc *dev)
 	return 0;
 }
 
-static int gr_remove(struct platform_device *ofdev)
+static int gr_remove(struct platform_device *pdev)
 {
-	struct gr_udc *dev = dev_get_drvdata(&ofdev->dev);
+	struct gr_udc *dev = platform_get_drvdata(pdev);
 
 	if (dev->added)
 		usb_del_gadget_udc(&dev->gadget); /* Shuts everything down */
@@ -2077,7 +2078,7 @@ static int gr_remove(struct platform_device *ofdev)
 	gr_dfs_delete(dev);
 	if (dev->desc_pool)
 		dma_pool_destroy(dev->desc_pool);
-	dev_set_drvdata(&ofdev->dev, NULL);
+	platform_set_drvdata(pdev, NULL);
 
 	gr_free_request(&dev->epi[0].ep, &dev->ep0reqi->req);
 	gr_free_request(&dev->epo[0].ep, &dev->ep0reqo->req);
@@ -2090,7 +2091,7 @@ static int gr_request_irq(struct gr_udc *dev, int irq)
 					 IRQF_SHARED, driver_name, dev);
 }
 
-static int gr_probe(struct platform_device *ofdev)
+static int gr_probe(struct platform_device *pdev)
 {
 	struct gr_udc *dev;
 	struct resource *res;
@@ -2098,30 +2099,32 @@ static int gr_probe(struct platform_device *ofdev)
 	int retval;
 	u32 status;
 
-	dev = devm_kzalloc(&ofdev->dev, sizeof(*dev), GFP_KERNEL);
+	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
 	if (!dev)
 		return -ENOMEM;
-	dev->dev = &ofdev->dev;
+	dev->dev = &pdev->dev;
 
-	res = platform_get_resource(ofdev, IORESOURCE_MEM, 0);
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	regs = devm_ioremap_resource(dev->dev, res);
 	if (IS_ERR(regs))
 		return PTR_ERR(regs);
 
-	dev->irq = irq_of_parse_and_map(dev->dev->of_node, 0);
-	if (!dev->irq) {
+	dev->irq = platform_get_irq(pdev, 0);
+	if (dev->irq <= 0) {
 		dev_err(dev->dev, "No irq found\n");
 		return -ENODEV;
 	}
 
 	/* Some core configurations has separate irqs for IN and OUT events */
-	dev->irqi = irq_of_parse_and_map(dev->dev->of_node, 1);
-	if (dev->irqi) {
-		dev->irqo = irq_of_parse_and_map(dev->dev->of_node, 2);
-		if (!dev->irqo) {
+	dev->irqi = platform_get_irq(pdev, 1);
+	if (dev->irqi > 0) {
+		dev->irqo = platform_get_irq(pdev, 2);
+		if (dev->irqo <= 0) {
 			dev_err(dev->dev, "Found irqi but not irqo\n");
 			return -ENODEV;
 		}
+	} else {
+		dev->irqi = 0;
 	}
 
 	dev->gadget.name = driver_name;
@@ -2132,7 +2135,7 @@ static int gr_probe(struct platform_device *ofdev)
 	spin_lock_init(&dev->lock);
 	dev->regs = regs;
 
-	dev_set_drvdata(&ofdev->dev, dev);
+	platform_set_drvdata(pdev, dev);
 
 	/* Determine number of endpoints and data interface mode */
 	status = gr_read32(&dev->regs->status);
@@ -2204,7 +2207,7 @@ out:
 	spin_unlock(&dev->lock);
 
 	if (retval)
-		gr_remove(ofdev);
+		gr_remove(pdev);
 
 	return retval;
 }
diff --git a/drivers/usb/gadget/inode.c b/drivers/usb/gadget/inode.c
index a925d0cbcd4..ee6c16416c3 100644
--- a/drivers/usb/gadget/inode.c
+++ b/drivers/usb/gadget/inode.c
@@ -1494,6 +1494,7 @@ gadgetfs_setup (struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
 		 */
 		if (value == 0) {
 			INFO (dev, "configuration #%d\n", dev->current_config);
+			usb_gadget_set_state(gadget, USB_STATE_CONFIGURED);
 			if (dev->usermode_setup) {
 				dev->setup_can_stall = 0;
 				goto delegate;
@@ -1501,7 +1502,7 @@ gadgetfs_setup (struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
 		}
 		break;
 
-#ifndef	CONFIG_USB_GADGET_PXA25X
+#ifndef	CONFIG_USB_PXA25X
 	/* PXA automagically handles this request too */
 	case USB_REQ_GET_CONFIGURATION:
 		if (ctrl->bRequestType != 0x80)
diff --git a/drivers/usb/gadget/m66592-udc.c b/drivers/usb/gadget/m66592-udc.c
index 8cae01d8859..0d17174b86f 100644
--- a/drivers/usb/gadget/m66592-udc.c
+++ b/drivers/usb/gadget/m66592-udc.c
@@ -1594,7 +1594,6 @@ static int __init m66592_probe(struct platform_device *pdev)
 	m66592 = kzalloc(sizeof(struct m66592), GFP_KERNEL);
 	if (m66592 == NULL) {
 		ret = -ENOMEM;
-		pr_err("kzalloc error\n");
 		goto clean_up;
 	}
 
diff --git a/drivers/usb/gadget/mv_u3d_core.c b/drivers/usb/gadget/mv_u3d_core.c
index d2ca59e7b47..16248711c15 100644
--- a/drivers/usb/gadget/mv_u3d_core.c
+++ b/drivers/usb/gadget/mv_u3d_core.c
@@ -297,10 +297,8 @@ static struct mv_u3d_trb *mv_u3d_build_trb_one(struct mv_u3d_req *req,
 	u3d = req->ep->u3d;
 
 	trb = kzalloc(sizeof(*trb), GFP_ATOMIC);
-	if (!trb) {
-		dev_err(u3d->dev, "%s, trb alloc fail\n", __func__);
+	if (!trb)
 		return NULL;
-	}
 
 	/*
 	 * Be careful that no _GFP_HIGHMEM is set,
@@ -446,17 +444,12 @@ static int mv_u3d_req_to_trb(struct mv_u3d_req *req)
 			trb_num++;
 
 		trb = kcalloc(trb_num, sizeof(*trb), GFP_ATOMIC);
-		if (!trb) {
-			dev_err(u3d->dev,
-					"%s, trb alloc fail\n", __func__);
+		if (!trb)
 			return -ENOMEM;
-		}
 
 		trb_hw = kcalloc(trb_num, sizeof(*trb_hw), GFP_ATOMIC);
 		if (!trb_hw) {
 			kfree(trb);
-			dev_err(u3d->dev,
-					"%s, trb_hw alloc fail\n", __func__);
 			return -ENOMEM;
 		}
 
@@ -1811,7 +1804,6 @@ static int mv_u3d_probe(struct platform_device *dev)
 
 	u3d = kzalloc(sizeof(*u3d), GFP_KERNEL);
 	if (!u3d) {
-		dev_err(&dev->dev, "failed to allocate memory for u3d\n");
 		retval = -ENOMEM;
 		goto err_alloc_private;
 	}
@@ -1905,7 +1897,6 @@ static int mv_u3d_probe(struct platform_device *dev)
 	size = u3d->max_eps * sizeof(struct mv_u3d_ep) * 2;
 	u3d->eps = kzalloc(size, GFP_KERNEL);
 	if (!u3d->eps) {
-		dev_err(&dev->dev, "allocate ep memory failed\n");
 		retval = -ENOMEM;
 		goto err_alloc_eps;
 	}
@@ -1913,7 +1904,6 @@ static int mv_u3d_probe(struct platform_device *dev)
 	/* initialize ep0 status request structure */
 	u3d->status_req = kzalloc(sizeof(struct mv_u3d_req) + 8, GFP_KERNEL);
 	if (!u3d->status_req) {
-		dev_err(&dev->dev, "allocate status_req memory failed\n");
 		retval = -ENOMEM;
 		goto err_alloc_status_req;
 	}
diff --git a/drivers/usb/gadget/net2280.c b/drivers/usb/gadget/net2280.c
index 43e5e2f9888..300b3a71383 100644
--- a/drivers/usb/gadget/net2280.c
+++ b/drivers/usb/gadget/net2280.c
@@ -1972,7 +1972,9 @@ static int net2280_stop(struct usb_gadget *_gadget,
 	device_remove_file (&dev->pdev->dev, &dev_attr_function);
 	device_remove_file (&dev->pdev->dev, &dev_attr_queues);
 
-	DEBUG (dev, "unregistered driver '%s'\n", driver->driver.name);
+	DEBUG(dev, "unregistered driver '%s'\n",
+			driver ? driver->driver.name : "");
+
 	return 0;
 }
 
diff --git a/drivers/usb/gadget/r8a66597-udc.c b/drivers/usb/gadget/r8a66597-udc.c
index aff0a6718bc..b698a490cc7 100644
--- a/drivers/usb/gadget/r8a66597-udc.c
+++ b/drivers/usb/gadget/r8a66597-udc.c
@@ -1904,7 +1904,6 @@ static int __init r8a66597_probe(struct platform_device *pdev)
 	r8a66597 = kzalloc(sizeof(struct r8a66597), GFP_KERNEL);
 	if (r8a66597 == NULL) {
 		ret = -ENOMEM;
-		dev_err(&pdev->dev, "kzalloc error\n");
 		goto clean_up;
 	}
 
diff --git a/drivers/usb/gadget/rndis.c b/drivers/usb/gadget/rndis.c
index 7ed452d90f4..95d2324f697 100644
--- a/drivers/usb/gadget/rndis.c
+++ b/drivers/usb/gadget/rndis.c
@@ -761,7 +761,7 @@ int rndis_signal_connect(int configNr)
 	return rndis_indicate_status_msg(configNr,
 					  RNDIS_STATUS_MEDIA_CONNECT);
 }
-EXPORT_SYMBOL(rndis_signal_connect);
+EXPORT_SYMBOL_GPL(rndis_signal_connect);
 
 int rndis_signal_disconnect(int configNr)
 {
@@ -770,7 +770,7 @@ int rndis_signal_disconnect(int configNr)
 	return rndis_indicate_status_msg(configNr,
 					  RNDIS_STATUS_MEDIA_DISCONNECT);
 }
-EXPORT_SYMBOL(rndis_signal_disconnect);
+EXPORT_SYMBOL_GPL(rndis_signal_disconnect);
 
 void rndis_uninit(int configNr)
 {
@@ -785,13 +785,13 @@ void rndis_uninit(int configNr)
 	while ((buf = rndis_get_next_response(configNr, &length)))
 		rndis_free_response(configNr, buf);
 }
-EXPORT_SYMBOL(rndis_uninit);
+EXPORT_SYMBOL_GPL(rndis_uninit);
 
 void rndis_set_host_mac(int configNr, const u8 *addr)
 {
 	rndis_per_dev_params[configNr].host_mac = addr;
 }
-EXPORT_SYMBOL(rndis_set_host_mac);
+EXPORT_SYMBOL_GPL(rndis_set_host_mac);
 
 /*
  * Message Parser
@@ -874,7 +874,7 @@ int rndis_msg_parser(u8 configNr, u8 *buf)
 
 	return -ENOTSUPP;
 }
-EXPORT_SYMBOL(rndis_msg_parser);
+EXPORT_SYMBOL_GPL(rndis_msg_parser);
 
 int rndis_register(void (*resp_avail)(void *v), void *v)
 {
@@ -896,7 +896,7 @@ int rndis_register(void (*resp_avail)(void *v), void *v)
 
 	return -ENODEV;
 }
-EXPORT_SYMBOL(rndis_register);
+EXPORT_SYMBOL_GPL(rndis_register);
 
 void rndis_deregister(int configNr)
 {
@@ -905,7 +905,7 @@ void rndis_deregister(int configNr)
 	if (configNr >= RNDIS_MAX_CONFIGS) return;
 	rndis_per_dev_params[configNr].used = 0;
 }
-EXPORT_SYMBOL(rndis_deregister);
+EXPORT_SYMBOL_GPL(rndis_deregister);
 
 int rndis_set_param_dev(u8 configNr, struct net_device *dev, u16 *cdc_filter)
 {
@@ -919,7 +919,7 @@ int rndis_set_param_dev(u8 configNr, struct net_device *dev, u16 *cdc_filter)
 
 	return 0;
 }
-EXPORT_SYMBOL(rndis_set_param_dev);
+EXPORT_SYMBOL_GPL(rndis_set_param_dev);
 
 int rndis_set_param_vendor(u8 configNr, u32 vendorID, const char *vendorDescr)
 {
@@ -932,7 +932,7 @@ int rndis_set_param_vendor(u8 configNr, u32 vendorID, const char *vendorDescr)
 
 	return 0;
 }
-EXPORT_SYMBOL(rndis_set_param_vendor);
+EXPORT_SYMBOL_GPL(rndis_set_param_vendor);
 
 int rndis_set_param_medium(u8 configNr, u32 medium, u32 speed)
 {
@@ -944,7 +944,7 @@ int rndis_set_param_medium(u8 configNr, u32 medium, u32 speed)
 
 	return 0;
 }
-EXPORT_SYMBOL(rndis_set_param_medium);
+EXPORT_SYMBOL_GPL(rndis_set_param_medium);
 
 void rndis_add_hdr(struct sk_buff *skb)
 {
@@ -959,7 +959,7 @@ void rndis_add_hdr(struct sk_buff *skb)
 	header->DataOffset = cpu_to_le32(36);
 	header->DataLength = cpu_to_le32(skb->len - sizeof(*header));
 }
-EXPORT_SYMBOL(rndis_add_hdr);
+EXPORT_SYMBOL_GPL(rndis_add_hdr);
 
 void rndis_free_response(int configNr, u8 *buf)
 {
@@ -976,7 +976,7 @@ void rndis_free_response(int configNr, u8 *buf)
 		}
 	}
 }
-EXPORT_SYMBOL(rndis_free_response);
+EXPORT_SYMBOL_GPL(rndis_free_response);
 
 u8 *rndis_get_next_response(int configNr, u32 *length)
 {
@@ -998,7 +998,7 @@ u8 *rndis_get_next_response(int configNr, u32 *length)
 
 	return NULL;
 }
-EXPORT_SYMBOL(rndis_get_next_response);
+EXPORT_SYMBOL_GPL(rndis_get_next_response);
 
 static rndis_resp_t *rndis_add_response(int configNr, u32 length)
 {
@@ -1042,7 +1042,7 @@ int rndis_rm_hdr(struct gether *port,
 	skb_queue_tail(list, skb);
 	return 0;
 }
-EXPORT_SYMBOL(rndis_rm_hdr);
+EXPORT_SYMBOL_GPL(rndis_rm_hdr);
 
 #ifdef CONFIG_USB_GADGET_DEBUG_FILES
 
diff --git a/drivers/usb/gadget/s3c-hsotg.h b/drivers/usb/gadget/s3c-hsotg.h
deleted file mode 100644
index 85f549ff8c1..00000000000
--- a/drivers/usb/gadget/s3c-hsotg.h
+++ /dev/null
@@ -1,378 +0,0 @@
-/* drivers/usb/gadget/s3c-hsotg.h
- *
- * Copyright 2008 Openmoko, Inc.
- * Copyright 2008 Simtec Electronics
- *      http://armlinux.simtec.co.uk/
- *      Ben Dooks <ben@simtec.co.uk>
- *
- * USB2.0 Highspeed/OtG Synopsis DWC2 device block registers
- *
- * 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 __REGS_USB_HSOTG_H
-#define __REGS_USB_HSOTG_H __FILE__
-
-#define HSOTG_REG(x) (x)
-
-#define GOTGCTL				HSOTG_REG(0x000)
-#define GOTGCTL_BSESVLD			(1 << 19)
-#define GOTGCTL_ASESVLD			(1 << 18)
-#define GOTGCTL_DBNC_SHORT			(1 << 17)
-#define GOTGCTL_CONID_B			(1 << 16)
-#define GOTGCTL_DEVHNPEN			(1 << 11)
-#define GOTGCTL_HSSETHNPEN			(1 << 10)
-#define GOTGCTL_HNPREQ				(1 << 9)
-#define GOTGCTL_HSTNEGSCS			(1 << 8)
-#define GOTGCTL_SESREQ				(1 << 1)
-#define GOTGCTL_SESREQSCS			(1 << 0)
-
-#define GOTGINT				HSOTG_REG(0x004)
-#define GOTGINT_DbnceDone			(1 << 19)
-#define GOTGINT_ADevTOUTChg			(1 << 18)
-#define GOTGINT_HstNegDet			(1 << 17)
-#define GOTGINT_HstnegSucStsChng		(1 << 9)
-#define GOTGINT_SesReqSucStsChng		(1 << 8)
-#define GOTGINT_SesEndDet			(1 << 2)
-
-#define GAHBCFG				HSOTG_REG(0x008)
-#define GAHBCFG_PTxFEmpLvl			(1 << 8)
-#define GAHBCFG_NPTxFEmpLvl			(1 << 7)
-#define GAHBCFG_DMAEn				(1 << 5)
-#define GAHBCFG_HBstLen_MASK			(0xf << 1)
-#define GAHBCFG_HBstLen_SHIFT			(1)
-#define GAHBCFG_HBstLen_Single			(0x0 << 1)
-#define GAHBCFG_HBstLen_Incr			(0x1 << 1)
-#define GAHBCFG_HBstLen_Incr4			(0x3 << 1)
-#define GAHBCFG_HBstLen_Incr8			(0x5 << 1)
-#define GAHBCFG_HBstLen_Incr16			(0x7 << 1)
-#define GAHBCFG_GlblIntrEn			(1 << 0)
-
-#define GUSBCFG				HSOTG_REG(0x00C)
-#define GUSBCFG_PHYLPClkSel			(1 << 15)
-#define GUSBCFG_HNPCap				(1 << 9)
-#define GUSBCFG_SRPCap				(1 << 8)
-#define GUSBCFG_PHYIf16			(1 << 3)
-#define GUSBCFG_PHYIf8				(0 << 3)
-#define GUSBCFG_TOutCal_MASK			(0x7 << 0)
-#define GUSBCFG_TOutCal_SHIFT			(0)
-#define GUSBCFG_TOutCal_LIMIT			(0x7)
-#define GUSBCFG_TOutCal(_x)			((_x) << 0)
-
-#define GRSTCTL				HSOTG_REG(0x010)
-
-#define GRSTCTL_AHBIdle			(1 << 31)
-#define GRSTCTL_DMAReq				(1 << 30)
-#define GRSTCTL_TxFNum_MASK			(0x1f << 6)
-#define GRSTCTL_TxFNum_SHIFT			(6)
-#define GRSTCTL_TxFNum_LIMIT			(0x1f)
-#define GRSTCTL_TxFNum(_x)			((_x) << 6)
-#define GRSTCTL_TxFFlsh			(1 << 5)
-#define GRSTCTL_RxFFlsh			(1 << 4)
-#define GRSTCTL_INTknQFlsh			(1 << 3)
-#define GRSTCTL_FrmCntrRst			(1 << 2)
-#define GRSTCTL_HSftRst			(1 << 1)
-#define GRSTCTL_CSftRst			(1 << 0)
-
-#define GINTSTS				HSOTG_REG(0x014)
-#define GINTMSK				HSOTG_REG(0x018)
-
-#define GINTSTS_WkUpInt			(1 << 31)
-#define GINTSTS_SessReqInt			(1 << 30)
-#define GINTSTS_DisconnInt			(1 << 29)
-#define GINTSTS_ConIDStsChng			(1 << 28)
-#define GINTSTS_PTxFEmp			(1 << 26)
-#define GINTSTS_HChInt				(1 << 25)
-#define GINTSTS_PrtInt				(1 << 24)
-#define GINTSTS_FetSusp			(1 << 22)
-#define GINTSTS_incompIP			(1 << 21)
-#define GINTSTS_IncomplSOIN			(1 << 20)
-#define GINTSTS_OEPInt				(1 << 19)
-#define GINTSTS_IEPInt				(1 << 18)
-#define GINTSTS_EPMis				(1 << 17)
-#define GINTSTS_EOPF				(1 << 15)
-#define GINTSTS_ISOutDrop			(1 << 14)
-#define GINTSTS_EnumDone			(1 << 13)
-#define GINTSTS_USBRst				(1 << 12)
-#define GINTSTS_USBSusp			(1 << 11)
-#define GINTSTS_ErlySusp			(1 << 10)
-#define GINTSTS_GOUTNakEff			(1 << 7)
-#define GINTSTS_GINNakEff			(1 << 6)
-#define GINTSTS_NPTxFEmp			(1 << 5)
-#define GINTSTS_RxFLvl				(1 << 4)
-#define GINTSTS_SOF				(1 << 3)
-#define GINTSTS_OTGInt				(1 << 2)
-#define GINTSTS_ModeMis			(1 << 1)
-#define GINTSTS_CurMod_Host			(1 << 0)
-
-#define GRXSTSR				HSOTG_REG(0x01C)
-#define GRXSTSP				HSOTG_REG(0x020)
-
-#define GRXSTS_FN_MASK				(0x7f << 25)
-#define GRXSTS_FN_SHIFT			(25)
-
-#define GRXSTS_PktSts_MASK			(0xf << 17)
-#define GRXSTS_PktSts_SHIFT			(17)
-#define GRXSTS_PktSts_GlobalOutNAK		(0x1 << 17)
-#define GRXSTS_PktSts_OutRX			(0x2 << 17)
-#define GRXSTS_PktSts_OutDone			(0x3 << 17)
-#define GRXSTS_PktSts_SetupDone		(0x4 << 17)
-#define GRXSTS_PktSts_SetupRX			(0x6 << 17)
-
-#define GRXSTS_DPID_MASK			(0x3 << 15)
-#define GRXSTS_DPID_SHIFT			(15)
-#define GRXSTS_ByteCnt_MASK			(0x7ff << 4)
-#define GRXSTS_ByteCnt_SHIFT			(4)
-#define GRXSTS_EPNum_MASK			(0xf << 0)
-#define GRXSTS_EPNum_SHIFT			(0)
-
-#define GRXFSIZ				HSOTG_REG(0x024)
-
-#define GNPTXFSIZ				HSOTG_REG(0x028)
-
-#define GNPTXFSIZ_NPTxFDep_MASK		(0xffff << 16)
-#define GNPTXFSIZ_NPTxFDep_SHIFT		(16)
-#define GNPTXFSIZ_NPTxFDep_LIMIT		(0xffff)
-#define GNPTXFSIZ_NPTxFDep(_x)			((_x) << 16)
-#define GNPTXFSIZ_NPTxFStAddr_MASK		(0xffff << 0)
-#define GNPTXFSIZ_NPTxFStAddr_SHIFT		(0)
-#define GNPTXFSIZ_NPTxFStAddr_LIMIT		(0xffff)
-#define GNPTXFSIZ_NPTxFStAddr(_x)		((_x) << 0)
-
-#define GNPTXSTS				HSOTG_REG(0x02C)
-
-#define GNPTXSTS_NPtxQTop_MASK			(0x7f << 24)
-#define GNPTXSTS_NPtxQTop_SHIFT		(24)
-
-#define GNPTXSTS_NPTxQSpcAvail_MASK		(0xff << 16)
-#define GNPTXSTS_NPTxQSpcAvail_SHIFT		(16)
-#define GNPTXSTS_NPTxQSpcAvail_GET(_v)		(((_v) >> 16) & 0xff)
-
-#define GNPTXSTS_NPTxFSpcAvail_MASK		(0xffff << 0)
-#define GNPTXSTS_NPTxFSpcAvail_SHIFT		(0)
-#define GNPTXSTS_NPTxFSpcAvail_GET(_v)		(((_v) >> 0) & 0xffff)
-
-
-#define HPTXFSIZ				HSOTG_REG(0x100)
-
-#define DPTXFSIZn(_a)		HSOTG_REG(0x104 + (((_a) - 1) * 4))
-
-#define DPTXFSIZn_DPTxFSize_MASK		(0xffff << 16)
-#define DPTXFSIZn_DPTxFSize_SHIFT		(16)
-#define DPTXFSIZn_DPTxFSize_GET(_v)		(((_v) >> 16) & 0xffff)
-#define DPTXFSIZn_DPTxFSize_LIMIT		(0xffff)
-#define DPTXFSIZn_DPTxFSize(_x)		((_x) << 16)
-
-#define DPTXFSIZn_DPTxFStAddr_MASK		(0xffff << 0)
-#define DPTXFSIZn_DPTxFStAddr_SHIFT		(0)
-
-/* Device mode registers */
-#define DCFG					HSOTG_REG(0x800)
-
-#define DCFG_EPMisCnt_MASK			(0x1f << 18)
-#define DCFG_EPMisCnt_SHIFT			(18)
-#define DCFG_EPMisCnt_LIMIT			(0x1f)
-#define DCFG_EPMisCnt(_x)			((_x) << 18)
-
-#define DCFG_PerFrInt_MASK			(0x3 << 11)
-#define DCFG_PerFrInt_SHIFT			(11)
-#define DCFG_PerFrInt_LIMIT			(0x3)
-#define DCFG_PerFrInt(_x)			((_x) << 11)
-
-#define DCFG_DevAddr_MASK			(0x7f << 4)
-#define DCFG_DevAddr_SHIFT			(4)
-#define DCFG_DevAddr_LIMIT			(0x7f)
-#define DCFG_DevAddr(_x)			((_x) << 4)
-
-#define DCFG_NZStsOUTHShk			(1 << 2)
-
-#define DCFG_DevSpd_MASK			(0x3 << 0)
-#define DCFG_DevSpd_SHIFT			(0)
-#define DCFG_DevSpd_HS				(0x0 << 0)
-#define DCFG_DevSpd_FS				(0x1 << 0)
-#define DCFG_DevSpd_LS				(0x2 << 0)
-#define DCFG_DevSpd_FS48			(0x3 << 0)
-
-#define DCTL					HSOTG_REG(0x804)
-
-#define DCTL_PWROnPrgDone			(1 << 11)
-#define DCTL_CGOUTNak				(1 << 10)
-#define DCTL_SGOUTNak				(1 << 9)
-#define DCTL_CGNPInNAK				(1 << 8)
-#define DCTL_SGNPInNAK				(1 << 7)
-#define DCTL_TstCtl_MASK			(0x7 << 4)
-#define DCTL_TstCtl_SHIFT			(4)
-#define DCTL_GOUTNakSts			(1 << 3)
-#define DCTL_GNPINNakSts			(1 << 2)
-#define DCTL_SftDiscon				(1 << 1)
-#define DCTL_RmtWkUpSig			(1 << 0)
-
-#define DSTS					HSOTG_REG(0x808)
-
-#define DSTS_SOFFN_MASK			(0x3fff << 8)
-#define DSTS_SOFFN_SHIFT			(8)
-#define DSTS_SOFFN_LIMIT			(0x3fff)
-#define DSTS_SOFFN(_x)				((_x) << 8)
-#define DSTS_ErraticErr			(1 << 3)
-#define DSTS_EnumSpd_MASK			(0x3 << 1)
-#define DSTS_EnumSpd_SHIFT			(1)
-#define DSTS_EnumSpd_HS			(0x0 << 1)
-#define DSTS_EnumSpd_FS			(0x1 << 1)
-#define DSTS_EnumSpd_LS			(0x2 << 1)
-#define DSTS_EnumSpd_FS48			(0x3 << 1)
-
-#define DSTS_SuspSts				(1 << 0)
-
-#define DIEPMSK				HSOTG_REG(0x810)
-
-#define DIEPMSK_TxFIFOEmpty			(1 << 7)
-#define DIEPMSK_INEPNakEffMsk			(1 << 6)
-#define DIEPMSK_INTknEPMisMsk			(1 << 5)
-#define DIEPMSK_INTknTXFEmpMsk			(1 << 4)
-#define DIEPMSK_TimeOUTMsk			(1 << 3)
-#define DIEPMSK_AHBErrMsk			(1 << 2)
-#define DIEPMSK_EPDisbldMsk			(1 << 1)
-#define DIEPMSK_XferComplMsk			(1 << 0)
-
-#define DOEPMSK				HSOTG_REG(0x814)
-
-#define DOEPMSK_Back2BackSetup			(1 << 6)
-#define DOEPMSK_OUTTknEPdisMsk			(1 << 4)
-#define DOEPMSK_SetupMsk			(1 << 3)
-#define DOEPMSK_AHBErrMsk			(1 << 2)
-#define DOEPMSK_EPDisbldMsk			(1 << 1)
-#define DOEPMSK_XferComplMsk			(1 << 0)
-
-#define DAINT					HSOTG_REG(0x818)
-#define DAINTMSK				HSOTG_REG(0x81C)
-
-#define DAINT_OutEP_SHIFT			(16)
-#define DAINT_OutEP(x)				(1 << ((x) + 16))
-#define DAINT_InEP(x)				(1 << (x))
-
-#define DTKNQR1				HSOTG_REG(0x820)
-#define DTKNQR2				HSOTG_REG(0x824)
-#define DTKNQR3				HSOTG_REG(0x830)
-#define DTKNQR4				HSOTG_REG(0x834)
-
-#define DVBUSDIS				HSOTG_REG(0x828)
-#define DVBUSPULSE				HSOTG_REG(0x82C)
-
-#define DIEPCTL0				HSOTG_REG(0x900)
-#define DOEPCTL0				HSOTG_REG(0xB00)
-#define DIEPCTL(_a)			HSOTG_REG(0x900 + ((_a) * 0x20))
-#define DOEPCTL(_a)			HSOTG_REG(0xB00 + ((_a) * 0x20))
-
-/* EP0 specialness:
- * bits[29..28] - reserved (no SetD0PID, SetD1PID)
- * bits[25..22] - should always be zero, this isn't a periodic endpoint
- * bits[10..0] - MPS setting differenct for EP0
- */
-#define D0EPCTL_MPS_MASK			(0x3 << 0)
-#define D0EPCTL_MPS_SHIFT			(0)
-#define D0EPCTL_MPS_64				(0x0 << 0)
-#define D0EPCTL_MPS_32				(0x1 << 0)
-#define D0EPCTL_MPS_16				(0x2 << 0)
-#define D0EPCTL_MPS_8				(0x3 << 0)
-
-#define DxEPCTL_EPEna				(1 << 31)
-#define DxEPCTL_EPDis				(1 << 30)
-#define DxEPCTL_SetD1PID			(1 << 29)
-#define DxEPCTL_SetOddFr			(1 << 29)
-#define DxEPCTL_SetD0PID			(1 << 28)
-#define DxEPCTL_SetEvenFr			(1 << 28)
-#define DxEPCTL_SNAK				(1 << 27)
-#define DxEPCTL_CNAK				(1 << 26)
-#define DxEPCTL_TxFNum_MASK			(0xf << 22)
-#define DxEPCTL_TxFNum_SHIFT			(22)
-#define DxEPCTL_TxFNum_LIMIT			(0xf)
-#define DxEPCTL_TxFNum(_x)			((_x) << 22)
-
-#define DxEPCTL_Stall				(1 << 21)
-#define DxEPCTL_Snp				(1 << 20)
-#define DxEPCTL_EPType_MASK			(0x3 << 18)
-#define DxEPCTL_EPType_SHIFT			(18)
-#define DxEPCTL_EPType_Control			(0x0 << 18)
-#define DxEPCTL_EPType_Iso			(0x1 << 18)
-#define DxEPCTL_EPType_Bulk			(0x2 << 18)
-#define DxEPCTL_EPType_Intterupt		(0x3 << 18)
-
-#define DxEPCTL_NAKsts				(1 << 17)
-#define DxEPCTL_DPID				(1 << 16)
-#define DxEPCTL_EOFrNum			(1 << 16)
-#define DxEPCTL_USBActEp			(1 << 15)
-#define DxEPCTL_NextEp_MASK			(0xf << 11)
-#define DxEPCTL_NextEp_SHIFT			(11)
-#define DxEPCTL_NextEp_LIMIT			(0xf)
-#define DxEPCTL_NextEp(_x)			((_x) << 11)
-
-#define DxEPCTL_MPS_MASK			(0x7ff << 0)
-#define DxEPCTL_MPS_SHIFT			(0)
-#define DxEPCTL_MPS_LIMIT			(0x7ff)
-#define DxEPCTL_MPS(_x)			((_x) << 0)
-
-#define DIEPINT(_a)			HSOTG_REG(0x908 + ((_a) * 0x20))
-#define DOEPINT(_a)			HSOTG_REG(0xB08 + ((_a) * 0x20))
-
-#define DxEPINT_INEPNakEff			(1 << 6)
-#define DxEPINT_Back2BackSetup			(1 << 6)
-#define DxEPINT_INTknEPMis			(1 << 5)
-#define DxEPINT_INTknTXFEmp			(1 << 4)
-#define DxEPINT_OUTTknEPdis			(1 << 4)
-#define DxEPINT_Timeout			(1 << 3)
-#define DxEPINT_Setup				(1 << 3)
-#define DxEPINT_AHBErr				(1 << 2)
-#define DxEPINT_EPDisbld			(1 << 1)
-#define DxEPINT_XferCompl			(1 << 0)
-
-#define DIEPTSIZ0				HSOTG_REG(0x910)
-
-#define DIEPTSIZ0_PktCnt_MASK			(0x3 << 19)
-#define DIEPTSIZ0_PktCnt_SHIFT			(19)
-#define DIEPTSIZ0_PktCnt_LIMIT			(0x3)
-#define DIEPTSIZ0_PktCnt(_x)			((_x) << 19)
-
-#define DIEPTSIZ0_XferSize_MASK		(0x7f << 0)
-#define DIEPTSIZ0_XferSize_SHIFT		(0)
-#define DIEPTSIZ0_XferSize_LIMIT		(0x7f)
-#define DIEPTSIZ0_XferSize(_x)			((_x) << 0)
-
-#define DOEPTSIZ0				HSOTG_REG(0xB10)
-#define DOEPTSIZ0_SUPCnt_MASK			(0x3 << 29)
-#define DOEPTSIZ0_SUPCnt_SHIFT			(29)
-#define DOEPTSIZ0_SUPCnt_LIMIT			(0x3)
-#define DOEPTSIZ0_SUPCnt(_x)			((_x) << 29)
-
-#define DOEPTSIZ0_PktCnt			(1 << 19)
-#define DOEPTSIZ0_XferSize_MASK		(0x7f << 0)
-#define DOEPTSIZ0_XferSize_SHIFT		(0)
-
-#define DIEPTSIZ(_a)			HSOTG_REG(0x910 + ((_a) * 0x20))
-#define DOEPTSIZ(_a)			HSOTG_REG(0xB10 + ((_a) * 0x20))
-
-#define DxEPTSIZ_MC_MASK			(0x3 << 29)
-#define DxEPTSIZ_MC_SHIFT			(29)
-#define DxEPTSIZ_MC_LIMIT			(0x3)
-#define DxEPTSIZ_MC(_x)			((_x) << 29)
-
-#define DxEPTSIZ_PktCnt_MASK			(0x3ff << 19)
-#define DxEPTSIZ_PktCnt_SHIFT			(19)
-#define DxEPTSIZ_PktCnt_GET(_v)		(((_v) >> 19) & 0x3ff)
-#define DxEPTSIZ_PktCnt_LIMIT			(0x3ff)
-#define DxEPTSIZ_PktCnt(_x)			((_x) << 19)
-
-#define DxEPTSIZ_XferSize_MASK			(0x7ffff << 0)
-#define DxEPTSIZ_XferSize_SHIFT		(0)
-#define DxEPTSIZ_XferSize_GET(_v)		(((_v) >> 0) & 0x7ffff)
-#define DxEPTSIZ_XferSize_LIMIT		(0x7ffff)
-#define DxEPTSIZ_XferSize(_x)			((_x) << 0)
-
-#define DIEPDMA(_a)			HSOTG_REG(0x914 + ((_a) * 0x20))
-#define DOEPDMA(_a)			HSOTG_REG(0xB14 + ((_a) * 0x20))
-#define DTXFSTS(_a)			HSOTG_REG(0x918 + ((_a) * 0x20))
-
-#define EPFIFO(_a)			HSOTG_REG(0x1000 + ((_a) * 0x1000))
-
-#endif /* __REGS_USB_HSOTG_H */
diff --git a/drivers/usb/gadget/s3c2410_udc.c b/drivers/usb/gadget/s3c2410_udc.c
index dd9678f85c5..7987aa049fa 100644
--- a/drivers/usb/gadget/s3c2410_udc.c
+++ b/drivers/usb/gadget/s3c2410_udc.c
@@ -117,7 +117,8 @@ static int dprintk(int level, const char *fmt, ...)
 			sizeof(printk_buf)-len, fmt, args);
 	va_end(args);
 
-	return pr_debug("%s", printk_buf);
+	pr_debug("%s", printk_buf);
+	return len;
 }
 #else
 static int dprintk(int level, const char *fmt, ...)
diff --git a/drivers/usb/gadget/storage_common.c b/drivers/usb/gadget/storage_common.c
index ec20a1f50c2..ff205a7bc55 100644
--- a/drivers/usb/gadget/storage_common.c
+++ b/drivers/usb/gadget/storage_common.c
@@ -43,7 +43,7 @@ struct usb_interface_descriptor fsg_intf_desc = {
 	.bInterfaceProtocol =	USB_PR_BULK,	/* Adjusted during fsg_bind() */
 	.iInterface =		FSG_STRING_INTERFACE,
 };
-EXPORT_SYMBOL(fsg_intf_desc);
+EXPORT_SYMBOL_GPL(fsg_intf_desc);
 
 /*
  * Three full-speed endpoint descriptors: bulk-in, bulk-out, and
@@ -58,7 +58,7 @@ struct usb_endpoint_descriptor fsg_fs_bulk_in_desc = {
 	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
 	/* wMaxPacketSize set by autoconfiguration */
 };
-EXPORT_SYMBOL(fsg_fs_bulk_in_desc);
+EXPORT_SYMBOL_GPL(fsg_fs_bulk_in_desc);
 
 struct usb_endpoint_descriptor fsg_fs_bulk_out_desc = {
 	.bLength =		USB_DT_ENDPOINT_SIZE,
@@ -68,7 +68,7 @@ struct usb_endpoint_descriptor fsg_fs_bulk_out_desc = {
 	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
 	/* wMaxPacketSize set by autoconfiguration */
 };
-EXPORT_SYMBOL(fsg_fs_bulk_out_desc);
+EXPORT_SYMBOL_GPL(fsg_fs_bulk_out_desc);
 
 struct usb_descriptor_header *fsg_fs_function[] = {
 	(struct usb_descriptor_header *) &fsg_intf_desc,
@@ -76,7 +76,7 @@ struct usb_descriptor_header *fsg_fs_function[] = {
 	(struct usb_descriptor_header *) &fsg_fs_bulk_out_desc,
 	NULL,
 };
-EXPORT_SYMBOL(fsg_fs_function);
+EXPORT_SYMBOL_GPL(fsg_fs_function);
 
 
 /*
@@ -95,7 +95,7 @@ struct usb_endpoint_descriptor fsg_hs_bulk_in_desc = {
 	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
 	.wMaxPacketSize =	cpu_to_le16(512),
 };
-EXPORT_SYMBOL(fsg_hs_bulk_in_desc);
+EXPORT_SYMBOL_GPL(fsg_hs_bulk_in_desc);
 
 struct usb_endpoint_descriptor fsg_hs_bulk_out_desc = {
 	.bLength =		USB_DT_ENDPOINT_SIZE,
@@ -106,7 +106,7 @@ struct usb_endpoint_descriptor fsg_hs_bulk_out_desc = {
 	.wMaxPacketSize =	cpu_to_le16(512),
 	.bInterval =		1,	/* NAK every 1 uframe */
 };
-EXPORT_SYMBOL(fsg_hs_bulk_out_desc);
+EXPORT_SYMBOL_GPL(fsg_hs_bulk_out_desc);
 
 
 struct usb_descriptor_header *fsg_hs_function[] = {
@@ -115,7 +115,7 @@ struct usb_descriptor_header *fsg_hs_function[] = {
 	(struct usb_descriptor_header *) &fsg_hs_bulk_out_desc,
 	NULL,
 };
-EXPORT_SYMBOL(fsg_hs_function);
+EXPORT_SYMBOL_GPL(fsg_hs_function);
 
 struct usb_endpoint_descriptor fsg_ss_bulk_in_desc = {
 	.bLength =		USB_DT_ENDPOINT_SIZE,
@@ -125,7 +125,7 @@ struct usb_endpoint_descriptor fsg_ss_bulk_in_desc = {
 	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
 	.wMaxPacketSize =	cpu_to_le16(1024),
 };
-EXPORT_SYMBOL(fsg_ss_bulk_in_desc);
+EXPORT_SYMBOL_GPL(fsg_ss_bulk_in_desc);
 
 struct usb_ss_ep_comp_descriptor fsg_ss_bulk_in_comp_desc = {
 	.bLength =		sizeof(fsg_ss_bulk_in_comp_desc),
@@ -133,7 +133,7 @@ struct usb_ss_ep_comp_descriptor fsg_ss_bulk_in_comp_desc = {
 
 	/*.bMaxBurst =		DYNAMIC, */
 };
-EXPORT_SYMBOL(fsg_ss_bulk_in_comp_desc);
+EXPORT_SYMBOL_GPL(fsg_ss_bulk_in_comp_desc);
 
 struct usb_endpoint_descriptor fsg_ss_bulk_out_desc = {
 	.bLength =		USB_DT_ENDPOINT_SIZE,
@@ -143,7 +143,7 @@ struct usb_endpoint_descriptor fsg_ss_bulk_out_desc = {
 	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
 	.wMaxPacketSize =	cpu_to_le16(1024),
 };
-EXPORT_SYMBOL(fsg_ss_bulk_out_desc);
+EXPORT_SYMBOL_GPL(fsg_ss_bulk_out_desc);
 
 struct usb_ss_ep_comp_descriptor fsg_ss_bulk_out_comp_desc = {
 	.bLength =		sizeof(fsg_ss_bulk_in_comp_desc),
@@ -151,7 +151,7 @@ struct usb_ss_ep_comp_descriptor fsg_ss_bulk_out_comp_desc = {
 
 	/*.bMaxBurst =		DYNAMIC, */
 };
-EXPORT_SYMBOL(fsg_ss_bulk_out_comp_desc);
+EXPORT_SYMBOL_GPL(fsg_ss_bulk_out_comp_desc);
 
 struct usb_descriptor_header *fsg_ss_function[] = {
 	(struct usb_descriptor_header *) &fsg_intf_desc,
@@ -161,7 +161,7 @@ struct usb_descriptor_header *fsg_ss_function[] = {
 	(struct usb_descriptor_header *) &fsg_ss_bulk_out_comp_desc,
 	NULL,
 };
-EXPORT_SYMBOL(fsg_ss_function);
+EXPORT_SYMBOL_GPL(fsg_ss_function);
 
 
  /*-------------------------------------------------------------------------*/
@@ -179,7 +179,7 @@ void fsg_lun_close(struct fsg_lun *curlun)
 		curlun->filp = NULL;
 	}
 }
-EXPORT_SYMBOL(fsg_lun_close);
+EXPORT_SYMBOL_GPL(fsg_lun_close);
 
 int fsg_lun_open(struct fsg_lun *curlun, const char *filename)
 {
@@ -278,7 +278,7 @@ out:
 	fput(filp);
 	return rc;
 }
-EXPORT_SYMBOL(fsg_lun_open);
+EXPORT_SYMBOL_GPL(fsg_lun_open);
 
 
 /*-------------------------------------------------------------------------*/
@@ -295,7 +295,7 @@ int fsg_lun_fsync_sub(struct fsg_lun *curlun)
 		return 0;
 	return vfs_fsync(filp, 1);
 }
-EXPORT_SYMBOL(fsg_lun_fsync_sub);
+EXPORT_SYMBOL_GPL(fsg_lun_fsync_sub);
 
 void store_cdrom_address(u8 *dest, int msf, u32 addr)
 {
@@ -314,7 +314,7 @@ void store_cdrom_address(u8 *dest, int msf, u32 addr)
 		put_unaligned_be32(addr, dest);
 	}
 }
-EXPORT_SYMBOL(store_cdrom_address);
+EXPORT_SYMBOL_GPL(store_cdrom_address);
 
 /*-------------------------------------------------------------------------*/
 
@@ -325,13 +325,13 @@ ssize_t fsg_show_ro(struct fsg_lun *curlun, char *buf)
 				  ? curlun->ro
 				  : curlun->initially_ro);
 }
-EXPORT_SYMBOL(fsg_show_ro);
+EXPORT_SYMBOL_GPL(fsg_show_ro);
 
 ssize_t fsg_show_nofua(struct fsg_lun *curlun, char *buf)
 {
 	return sprintf(buf, "%u\n", curlun->nofua);
 }
-EXPORT_SYMBOL(fsg_show_nofua);
+EXPORT_SYMBOL_GPL(fsg_show_nofua);
 
 ssize_t fsg_show_file(struct fsg_lun *curlun, struct rw_semaphore *filesem,
 		      char *buf)
@@ -357,19 +357,19 @@ ssize_t fsg_show_file(struct fsg_lun *curlun, struct rw_semaphore *filesem,
 	up_read(filesem);
 	return rc;
 }
-EXPORT_SYMBOL(fsg_show_file);
+EXPORT_SYMBOL_GPL(fsg_show_file);
 
 ssize_t fsg_show_cdrom(struct fsg_lun *curlun, char *buf)
 {
 	return sprintf(buf, "%u\n", curlun->cdrom);
 }
-EXPORT_SYMBOL(fsg_show_cdrom);
+EXPORT_SYMBOL_GPL(fsg_show_cdrom);
 
 ssize_t fsg_show_removable(struct fsg_lun *curlun, char *buf)
 {
 	return sprintf(buf, "%u\n", curlun->removable);
 }
-EXPORT_SYMBOL(fsg_show_removable);
+EXPORT_SYMBOL_GPL(fsg_show_removable);
 
 /*
  * The caller must hold fsg->filesem for reading when calling this function.
@@ -410,7 +410,7 @@ ssize_t fsg_store_ro(struct fsg_lun *curlun, struct rw_semaphore *filesem,
 
 	return rc;
 }
-EXPORT_SYMBOL(fsg_store_ro);
+EXPORT_SYMBOL_GPL(fsg_store_ro);
 
 ssize_t fsg_store_nofua(struct fsg_lun *curlun, const char *buf, size_t count)
 {
@@ -429,7 +429,7 @@ ssize_t fsg_store_nofua(struct fsg_lun *curlun, const char *buf, size_t count)
 
 	return count;
 }
-EXPORT_SYMBOL(fsg_store_nofua);
+EXPORT_SYMBOL_GPL(fsg_store_nofua);
 
 ssize_t fsg_store_file(struct fsg_lun *curlun, struct rw_semaphore *filesem,
 		       const char *buf, size_t count)
@@ -460,7 +460,7 @@ ssize_t fsg_store_file(struct fsg_lun *curlun, struct rw_semaphore *filesem,
 	up_write(filesem);
 	return (rc < 0 ? rc : count);
 }
-EXPORT_SYMBOL(fsg_store_file);
+EXPORT_SYMBOL_GPL(fsg_store_file);
 
 ssize_t fsg_store_cdrom(struct fsg_lun *curlun, struct rw_semaphore *filesem,
 			const char *buf, size_t count)
@@ -483,7 +483,7 @@ ssize_t fsg_store_cdrom(struct fsg_lun *curlun, struct rw_semaphore *filesem,
 
 	return ret;
 }
-EXPORT_SYMBOL(fsg_store_cdrom);
+EXPORT_SYMBOL_GPL(fsg_store_cdrom);
 
 ssize_t fsg_store_removable(struct fsg_lun *curlun, const char *buf,
 			    size_t count)
@@ -499,6 +499,6 @@ ssize_t fsg_store_removable(struct fsg_lun *curlun, const char *buf,
 
 	return count;
 }
-EXPORT_SYMBOL(fsg_store_removable);
+EXPORT_SYMBOL_GPL(fsg_store_removable);
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/usb/gadget/tcm_usb_gadget.c b/drivers/usb/gadget/tcm_usb_gadget.c
index f058c0368d6..6cdb7a534f2 100644
--- a/drivers/usb/gadget/tcm_usb_gadget.c
+++ b/drivers/usb/gadget/tcm_usb_gadget.c
@@ -1383,10 +1383,8 @@ static struct se_node_acl *usbg_alloc_fabric_acl(struct se_portal_group *se_tpg)
 	struct usbg_nacl *nacl;
 
 	nacl = kzalloc(sizeof(struct usbg_nacl), GFP_KERNEL);
-	if (!nacl) {
-		printk(KERN_ERR "Unable to allocate struct usbg_nacl\n");
+	if (!nacl)
 		return NULL;
-	}
 
 	return &nacl->se_node_acl;
 }
@@ -1561,10 +1559,8 @@ static struct se_portal_group *usbg_make_tpg(
 	}
 
 	tpg = kzalloc(sizeof(struct usbg_tpg), GFP_KERNEL);
-	if (!tpg) {
-		printk(KERN_ERR "Unable to allocate struct usbg_tpg");
+	if (!tpg)
 		return ERR_PTR(-ENOMEM);
-	}
 	mutex_init(&tpg->tpg_mutex);
 	atomic_set(&tpg->tpg_port_count, 0);
 	tpg->workqueue = alloc_workqueue("tcm_usb_gadget", 0, 1);
@@ -1613,10 +1609,8 @@ static struct se_wwn *usbg_make_tport(
 		return ERR_PTR(-EINVAL);
 
 	tport = kzalloc(sizeof(struct usbg_tport), GFP_KERNEL);
-	if (!(tport)) {
-		printk(KERN_ERR "Unable to allocate struct usbg_tport");
+	if (!(tport))
 		return ERR_PTR(-ENOMEM);
-	}
 	tport->tport_wwpn = wwpn;
 	snprintf(tport->tport_name, sizeof(tport->tport_name), "%s", wnn_name);
 	return &tport->tport_wwn;
@@ -1727,10 +1721,8 @@ static int tcm_usbg_make_nexus(struct usbg_tpg *tpg, char *name)
 
 	ret = -ENOMEM;
 	tv_nexus = kzalloc(sizeof(*tv_nexus), GFP_KERNEL);
-	if (!tv_nexus) {
-		pr_err("Unable to allocate struct tcm_vhost_nexus\n");
+	if (!tv_nexus)
 		goto err_unlock;
-	}
 	tv_nexus->tvn_se_sess = transport_init_session(TARGET_PROT_NORMAL);
 	if (IS_ERR(tv_nexus->tvn_se_sess))
 		goto err_free;
@@ -1851,7 +1843,7 @@ static int usbg_port_link(struct se_portal_group *se_tpg, struct se_lun *lun)
 	struct usbg_tpg *tpg = container_of(se_tpg, struct usbg_tpg, se_tpg);
 
 	atomic_inc(&tpg->tpg_port_count);
-	smp_mb__after_atomic_inc();
+	smp_mb__after_atomic();
 	return 0;
 }
 
@@ -1861,7 +1853,7 @@ static void usbg_port_unlink(struct se_portal_group *se_tpg,
 	struct usbg_tpg *tpg = container_of(se_tpg, struct usbg_tpg, se_tpg);
 
 	atomic_dec(&tpg->tpg_port_count);
-	smp_mb__after_atomic_dec();
+	smp_mb__after_atomic();
 }
 
 static int usbg_check_stop_free(struct se_cmd *se_cmd)
diff --git a/drivers/usb/gadget/u_ether.c b/drivers/usb/gadget/u_ether.c
index b7d4f82872b..fe0880d0873 100644
--- a/drivers/usb/gadget/u_ether.c
+++ b/drivers/usb/gadget/u_ether.c
@@ -818,7 +818,7 @@ struct eth_dev *gether_setup_name(struct usb_gadget *g,
 
 	return dev;
 }
-EXPORT_SYMBOL(gether_setup_name);
+EXPORT_SYMBOL_GPL(gether_setup_name);
 
 struct net_device *gether_setup_name_default(const char *netname)
 {
@@ -855,7 +855,7 @@ struct net_device *gether_setup_name_default(const char *netname)
 
 	return net;
 }
-EXPORT_SYMBOL(gether_setup_name_default);
+EXPORT_SYMBOL_GPL(gether_setup_name_default);
 
 int gether_register_netdev(struct net_device *net)
 {
@@ -893,7 +893,7 @@ int gether_register_netdev(struct net_device *net)
 
 	return status;
 }
-EXPORT_SYMBOL(gether_register_netdev);
+EXPORT_SYMBOL_GPL(gether_register_netdev);
 
 void gether_set_gadget(struct net_device *net, struct usb_gadget *g)
 {
@@ -903,7 +903,7 @@ void gether_set_gadget(struct net_device *net, struct usb_gadget *g)
 	dev->gadget = g;
 	SET_NETDEV_DEV(net, &g->dev);
 }
-EXPORT_SYMBOL(gether_set_gadget);
+EXPORT_SYMBOL_GPL(gether_set_gadget);
 
 int gether_set_dev_addr(struct net_device *net, const char *dev_addr)
 {
@@ -916,7 +916,7 @@ int gether_set_dev_addr(struct net_device *net, const char *dev_addr)
 	memcpy(dev->dev_mac, new_addr, ETH_ALEN);
 	return 0;
 }
-EXPORT_SYMBOL(gether_set_dev_addr);
+EXPORT_SYMBOL_GPL(gether_set_dev_addr);
 
 int gether_get_dev_addr(struct net_device *net, char *dev_addr, int len)
 {
@@ -925,7 +925,7 @@ int gether_get_dev_addr(struct net_device *net, char *dev_addr, int len)
 	dev = netdev_priv(net);
 	return get_ether_addr_str(dev->dev_mac, dev_addr, len);
 }
-EXPORT_SYMBOL(gether_get_dev_addr);
+EXPORT_SYMBOL_GPL(gether_get_dev_addr);
 
 int gether_set_host_addr(struct net_device *net, const char *host_addr)
 {
@@ -938,7 +938,7 @@ int gether_set_host_addr(struct net_device *net, const char *host_addr)
 	memcpy(dev->host_mac, new_addr, ETH_ALEN);
 	return 0;
 }
-EXPORT_SYMBOL(gether_set_host_addr);
+EXPORT_SYMBOL_GPL(gether_set_host_addr);
 
 int gether_get_host_addr(struct net_device *net, char *host_addr, int len)
 {
@@ -947,7 +947,7 @@ int gether_get_host_addr(struct net_device *net, char *host_addr, int len)
 	dev = netdev_priv(net);
 	return get_ether_addr_str(dev->host_mac, host_addr, len);
 }
-EXPORT_SYMBOL(gether_get_host_addr);
+EXPORT_SYMBOL_GPL(gether_get_host_addr);
 
 int gether_get_host_addr_cdc(struct net_device *net, char *host_addr, int len)
 {
@@ -961,7 +961,7 @@ int gether_get_host_addr_cdc(struct net_device *net, char *host_addr, int len)
 
 	return strlen(host_addr);
 }
-EXPORT_SYMBOL(gether_get_host_addr_cdc);
+EXPORT_SYMBOL_GPL(gether_get_host_addr_cdc);
 
 void gether_get_host_addr_u8(struct net_device *net, u8 host_mac[ETH_ALEN])
 {
@@ -970,7 +970,7 @@ void gether_get_host_addr_u8(struct net_device *net, u8 host_mac[ETH_ALEN])
 	dev = netdev_priv(net);
 	memcpy(host_mac, dev->host_mac, ETH_ALEN);
 }
-EXPORT_SYMBOL(gether_get_host_addr_u8);
+EXPORT_SYMBOL_GPL(gether_get_host_addr_u8);
 
 void gether_set_qmult(struct net_device *net, unsigned qmult)
 {
@@ -979,7 +979,7 @@ void gether_set_qmult(struct net_device *net, unsigned qmult)
 	dev = netdev_priv(net);
 	dev->qmult = qmult;
 }
-EXPORT_SYMBOL(gether_set_qmult);
+EXPORT_SYMBOL_GPL(gether_set_qmult);
 
 unsigned gether_get_qmult(struct net_device *net)
 {
@@ -988,7 +988,7 @@ unsigned gether_get_qmult(struct net_device *net)
 	dev = netdev_priv(net);
 	return dev->qmult;
 }
-EXPORT_SYMBOL(gether_get_qmult);
+EXPORT_SYMBOL_GPL(gether_get_qmult);
 
 int gether_get_ifname(struct net_device *net, char *name, int len)
 {
@@ -997,7 +997,7 @@ int gether_get_ifname(struct net_device *net, char *name, int len)
 	rtnl_unlock();
 	return strlen(name);
 }
-EXPORT_SYMBOL(gether_get_ifname);
+EXPORT_SYMBOL_GPL(gether_get_ifname);
 
 /**
  * gether_cleanup - remove Ethernet-over-USB device
@@ -1014,7 +1014,7 @@ void gether_cleanup(struct eth_dev *dev)
 	flush_work(&dev->work);
 	free_netdev(dev->net);
 }
-EXPORT_SYMBOL(gether_cleanup);
+EXPORT_SYMBOL_GPL(gether_cleanup);
 
 /**
  * gether_connect - notify network layer that USB link is active
@@ -1095,7 +1095,7 @@ fail0:
 		return ERR_PTR(result);
 	return dev->net;
 }
-EXPORT_SYMBOL(gether_connect);
+EXPORT_SYMBOL_GPL(gether_connect);
 
 /**
  * gether_disconnect - notify network layer that USB link is inactive
@@ -1166,7 +1166,7 @@ void gether_disconnect(struct gether *link)
 	dev->port_usb = NULL;
 	spin_unlock(&dev->lock);
 }
-EXPORT_SYMBOL(gether_disconnect);
+EXPORT_SYMBOL_GPL(gether_disconnect);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("David Brownell");
diff --git a/drivers/usb/gadget/u_f.c b/drivers/usb/gadget/u_f.c
index 63b6642c162..c6276f0268a 100644
--- a/drivers/usb/gadget/u_f.c
+++ b/drivers/usb/gadget/u_f.c
@@ -29,4 +29,4 @@ struct usb_request *alloc_ep_req(struct usb_ep *ep, int len, int default_len)
 	}
 	return req;
 }
-EXPORT_SYMBOL(alloc_ep_req);
+EXPORT_SYMBOL_GPL(alloc_ep_req);
diff --git a/drivers/usb/gadget/u_f.h b/drivers/usb/gadget/u_f.h
index 71034c061fc..1d5f0eb6855 100644
--- a/drivers/usb/gadget/u_f.h
+++ b/drivers/usb/gadget/u_f.h
@@ -16,6 +16,32 @@
 #ifndef __U_F_H__
 #define __U_F_H__
 
+/* Variable Length Array Macros **********************************************/
+#define vla_group(groupname) size_t groupname##__next = 0
+#define vla_group_size(groupname) groupname##__next
+
+#define vla_item(groupname, type, name, n) \
+	size_t groupname##_##name##__offset = ({			       \
+		size_t align_mask = __alignof__(type) - 1;		       \
+		size_t offset = (groupname##__next + align_mask) & ~align_mask;\
+		size_t size = (n) * sizeof(type);			       \
+		groupname##__next = offset + size;			       \
+		offset;							       \
+	})
+
+#define vla_item_with_sz(groupname, type, name, n) \
+	size_t groupname##_##name##__sz = (n) * sizeof(type);		       \
+	size_t groupname##_##name##__offset = ({			       \
+		size_t align_mask = __alignof__(type) - 1;		       \
+		size_t offset = (groupname##__next + align_mask) & ~align_mask;\
+		size_t size = groupname##_##name##__sz;			       \
+		groupname##__next = offset + size;			       \
+		offset;							       \
+	})
+
+#define vla_ptr(ptr, groupname, name) \
+	((void *) ((char *)ptr + groupname##_##name##__offset))
+
 struct usb_ep;
 struct usb_request;
 
diff --git a/drivers/usb/gadget/u_os_desc.h b/drivers/usb/gadget/u_os_desc.h
new file mode 100644
index 00000000000..ea5cf8c2da2
--- /dev/null
+++ b/drivers/usb/gadget/u_os_desc.h
@@ -0,0 +1,90 @@
+/*
+ * u_os_desc.h
+ *
+ * Utility definitions for "OS Descriptors" support
+ *
+ * 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_OS_DESC_H__
+#define __U_OS_DESC_H__
+
+#include <asm/unaligned.h>
+#include <linux/nls.h>
+
+#define USB_EXT_PROP_DW_SIZE			0
+#define USB_EXT_PROP_DW_PROPERTY_DATA_TYPE	4
+#define USB_EXT_PROP_W_PROPERTY_NAME_LENGTH	8
+#define USB_EXT_PROP_B_PROPERTY_NAME		10
+#define USB_EXT_PROP_DW_PROPERTY_DATA_LENGTH	10
+#define USB_EXT_PROP_B_PROPERTY_DATA		14
+
+#define USB_EXT_PROP_RESERVED			0
+#define USB_EXT_PROP_UNICODE			1
+#define USB_EXT_PROP_UNICODE_ENV		2
+#define USB_EXT_PROP_BINARY			3
+#define USB_EXT_PROP_LE32			4
+#define USB_EXT_PROP_BE32			5
+#define USB_EXT_PROP_UNICODE_LINK		6
+#define USB_EXT_PROP_UNICODE_MULTI		7
+
+static inline void usb_ext_prop_put_size(u8 *buf, int dw_size)
+{
+	put_unaligned_le32(dw_size, &buf[USB_EXT_PROP_DW_SIZE]);
+}
+
+static inline void usb_ext_prop_put_type(u8 *buf, int type)
+{
+	put_unaligned_le32(type, &buf[USB_EXT_PROP_DW_PROPERTY_DATA_TYPE]);
+}
+
+static inline int usb_ext_prop_put_name(u8 *buf, const char *name, int pnl)
+{
+	int result;
+
+	put_unaligned_le16(pnl, &buf[USB_EXT_PROP_W_PROPERTY_NAME_LENGTH]);
+	result = utf8s_to_utf16s(name, strlen(name), UTF16_LITTLE_ENDIAN,
+		(wchar_t *) &buf[USB_EXT_PROP_B_PROPERTY_NAME], pnl - 2);
+	if (result < 0)
+		return result;
+
+	put_unaligned_le16(0, &buf[USB_EXT_PROP_B_PROPERTY_NAME + pnl]);
+
+	return pnl;
+}
+
+static inline void usb_ext_prop_put_binary(u8 *buf, int pnl, const u8 *data,
+					   int data_len)
+{
+	put_unaligned_le32(data_len,
+			   &buf[USB_EXT_PROP_DW_PROPERTY_DATA_LENGTH + pnl]);
+	memcpy(&buf[USB_EXT_PROP_B_PROPERTY_DATA + pnl], data, data_len);
+}
+
+static inline int usb_ext_prop_put_unicode(u8 *buf, int pnl, const char *string,
+					   int data_len)
+{
+	int result;
+	put_unaligned_le32(data_len,
+			&buf[USB_EXT_PROP_DW_PROPERTY_DATA_LENGTH + pnl]);
+
+	result = utf8s_to_utf16s(string, data_len >> 1, UTF16_LITTLE_ENDIAN,
+			(wchar_t *) &buf[USB_EXT_PROP_B_PROPERTY_DATA + pnl],
+			data_len - 2);
+	if (result < 0)
+		return result;
+
+	put_unaligned_le16(0,
+			&buf[USB_EXT_PROP_B_PROPERTY_DATA + pnl + data_len]);
+
+	return data_len;
+}
+
+#endif /* __U_OS_DESC_H__ */
diff --git a/drivers/usb/gadget/u_rndis.h b/drivers/usb/gadget/u_rndis.h
index 7291b15c9dc..e902aa42a29 100644
--- a/drivers/usb/gadget/u_rndis.h
+++ b/drivers/usb/gadget/u_rndis.h
@@ -26,6 +26,9 @@ struct f_rndis_opts {
 	bool				bound;
 	bool				borrowed_net;
 
+	struct usb_os_desc		rndis_os_desc;
+	char				rndis_ext_compat_id[16];
+
 	/*
 	 * Read/write access to configfs attributes is handled by configfs.
 	 *
diff --git a/drivers/usb/gadget/udc-core.c b/drivers/usb/gadget/udc-core.c
index 27768a7d986..b0d98172bc0 100644
--- a/drivers/usb/gadget/udc-core.c
+++ b/drivers/usb/gadget/udc-core.c
@@ -428,6 +428,8 @@ int usb_gadget_unregister_driver(struct usb_gadget_driver *driver)
 	list_for_each_entry(udc, &udc_list, list)
 		if (udc->driver == driver) {
 			usb_gadget_remove_driver(udc);
+			usb_gadget_set_state(udc->gadget,
+					USB_STATE_NOTATTACHED);
 			ret = 0;
 			break;
 		}
diff --git a/drivers/usb/gadget/uvc_queue.c b/drivers/usb/gadget/uvc_queue.c
index 0bb5d50075d..1c29bc954db 100644
--- a/drivers/usb/gadget/uvc_queue.c
+++ b/drivers/usb/gadget/uvc_queue.c
@@ -20,6 +20,7 @@
 #include <linux/vmalloc.h>
 #include <linux/wait.h>
 
+#include <media/v4l2-common.h>
 #include <media/videobuf2-vmalloc.h>
 
 #include "uvc.h"
@@ -136,6 +137,8 @@ static int uvc_queue_init(struct uvc_video_queue *queue,
 	queue->queue.buf_struct_size = sizeof(struct uvc_buffer);
 	queue->queue.ops = &uvc_queue_qops;
 	queue->queue.mem_ops = &vb2_vmalloc_memops;
+	queue->queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC
+				     | V4L2_BUF_FLAG_TSTAMP_SRC_EOF;
 	ret = vb2_queue_init(&queue->queue);
 	if (ret)
 		return ret;
@@ -379,14 +382,9 @@ static struct uvc_buffer *uvc_queue_next_buffer(struct uvc_video_queue *queue,
 	else
 		nextbuf = NULL;
 
-	/*
-	 * FIXME: with videobuf2, the sequence number or timestamp fields
-	 * are valid only for video capture devices and the UVC gadget usually
-	 * is a video output device. Keeping these until the specs are clear on
-	 * this aspect.
-	 */
+	buf->buf.v4l2_buf.field = V4L2_FIELD_NONE;
 	buf->buf.v4l2_buf.sequence = queue->sequence++;
-	do_gettimeofday(&buf->buf.v4l2_buf.timestamp);
+	v4l2_get_timestamp(&buf->buf.v4l2_buf.timestamp);
 
 	vb2_set_plane_payload(&buf->buf, 0, buf->bytesused);
 	vb2_buffer_done(&buf->buf, VB2_BUF_STATE_DONE);
diff --git a/drivers/usb/host/Kconfig b/drivers/usb/host/Kconfig
index 3d9e54062d6..61b7817bd66 100644
--- a/drivers/usb/host/Kconfig
+++ b/drivers/usb/host/Kconfig
@@ -29,6 +29,14 @@ if USB_XHCI_HCD
 config USB_XHCI_PLATFORM
 	tristate
 
+config USB_XHCI_MVEBU
+	tristate "xHCI support for Marvell Armada 375/38x"
+	select USB_XHCI_PLATFORM
+	depends on ARCH_MVEBU || COMPILE_TEST
+	---help---
+	  Say 'Y' to enable the support for the xHCI host controller
+	  found in Marvell Armada 375/38x ARM SOCs.
+
 endif # USB_XHCI_HCD
 
 config USB_EHCI_HCD
@@ -170,7 +178,6 @@ config USB_EHCI_MSM
 	tristate "Support for Qualcomm QSD/MSM on-chip EHCI USB controller"
 	depends on ARCH_MSM
 	select USB_EHCI_ROOT_HUB_TT
-	select USB_MSM_OTG
 	---help---
 	  Enables support for the USB Host controller present on the
 	  Qualcomm chipsets. Root Hub has inbuilt TT.
@@ -343,10 +350,19 @@ config USB_FOTG210_HCD
 	  To compile this driver as a module, choose M here: the
 	  module will be called fotg210-hcd.
 
+config USB_MAX3421_HCD
+	tristate "MAX3421 HCD (USB-over-SPI) support"
+	depends on USB && SPI
+	---help---
+	  The Maxim MAX3421E chip supports standard USB 2.0-compliant
+	  full-speed devices either in host or peripheral mode.  This
+	  driver supports the host-mode of the MAX3421E only.
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called max3421-hcd.
+
 config USB_OHCI_HCD
 	tristate "OHCI HCD (USB 1.1) support"
-	select ISP1301_OMAP if MACH_OMAP_H2 || MACH_OMAP_H3
-	depends on USB_ISP1301 || !ARCH_LPC32XX
 	---help---
 	  The Open Host Controller Interface (OHCI) is a standard for accessing
 	  USB 1.1 host controller hardware.  It does more in hardware than Intel's
@@ -365,6 +381,7 @@ if USB_OHCI_HCD
 config USB_OHCI_HCD_OMAP1
 	tristate "OHCI support for OMAP1/2 chips"
 	depends on ARCH_OMAP1
+	depends on ISP1301_OMAP || !(MACH_OMAP_H2 || MACH_OMAP_H3)
 	default y
 	---help---
 	  Enables support for the OHCI controller on OMAP1/2 chips.
@@ -388,6 +405,7 @@ config USB_OHCI_HCD_S3C2410
 config USB_OHCI_HCD_LPC32XX
 	tristate "Support for LPC on-chip OHCI USB controller"
 	depends on USB_OHCI_HCD && ARCH_LPC32XX
+	depends on USB_ISP1301
 	default y
 	---help---
           Enables support for the on-chip OHCI controller on
@@ -417,6 +435,16 @@ config USB_OHCI_HCD_OMAP3
 	  Enables support for the on-chip OHCI controller on
 	  OMAP3 and later chips.
 
+config USB_OHCI_HCD_DAVINCI
+	bool "OHCI support for TI DaVinci DA8xx"
+	depends on ARCH_DAVINCI_DA8XX
+	depends on USB_OHCI_HCD=y
+	default y
+	help
+	  Enables support for the DaVinci DA8xx integrated OHCI
+	  controller. This driver cannot currently be a loadable
+	  module because it lacks a proper PHY abstraction.
+
 config USB_OHCI_ATH79
 	bool "USB OHCI support for the Atheros AR71XX/AR7240 SoCs (DEPRECATED)"
 	depends on (SOC_AR71XX || SOC_AR724X)
diff --git a/drivers/usb/host/Makefile b/drivers/usb/host/Makefile
index 7530468c9a4..af89a903d97 100644
--- a/drivers/usb/host/Makefile
+++ b/drivers/usb/host/Makefile
@@ -19,6 +19,9 @@ xhci-hcd-$(CONFIG_PCI)	+= xhci-pci.o
 
 ifneq ($(CONFIG_USB_XHCI_PLATFORM), )
 	xhci-hcd-y		+= xhci-plat.o
+ifneq ($(CONFIG_USB_XHCI_MVEBU), )
+	xhci-hcd-y		+= xhci-mvebu.o
+endif
 endif
 
 obj-$(CONFIG_USB_WHCI_HCD)	+= whci/
@@ -70,3 +73,4 @@ obj-$(CONFIG_USB_HCD_BCMA)	+= bcma-hcd.o
 obj-$(CONFIG_USB_HCD_SSB)	+= ssb-hcd.o
 obj-$(CONFIG_USB_FUSBH200_HCD)	+= fusbh200-hcd.o
 obj-$(CONFIG_USB_FOTG210_HCD)	+= fotg210-hcd.o
+obj-$(CONFIG_USB_MAX3421_HCD)	+= max3421-hcd.o
diff --git a/drivers/usb/host/ehci-exynos.c b/drivers/usb/host/ehci-exynos.c
index 7f425acd9be..d1c76216350 100644
--- a/drivers/usb/host/ehci-exynos.c
+++ b/drivers/usb/host/ehci-exynos.c
@@ -19,6 +19,7 @@
 #include <linux/module.h>
 #include <linux/of.h>
 #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>
@@ -42,17 +43,106 @@
 static const char hcd_name[] = "ehci-exynos";
 static struct hc_driver __read_mostly exynos_ehci_hc_driver;
 
+#define PHY_NUMBER 3
+
 struct exynos_ehci_hcd {
 	struct clk *clk;
 	struct usb_phy *phy;
 	struct usb_otg *otg;
+	struct phy *phy_g[PHY_NUMBER];
 };
 
 #define to_exynos_ehci(hcd) (struct exynos_ehci_hcd *)(hcd_to_ehci(hcd)->priv)
 
-static void exynos_setup_vbus_gpio(struct platform_device *pdev)
+static int exynos_ehci_get_phy(struct device *dev,
+				struct exynos_ehci_hcd *exynos_ehci)
+{
+	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;
+	}
+
+	for_each_available_child_of_node(dev->of_node, child) {
+		ret = of_property_read_u32(child, "reg", &phy_number);
+		if (ret) {
+			dev_err(dev, "Failed to parse device tree\n");
+			of_node_put(child);
+			return ret;
+		}
+
+		if (phy_number >= PHY_NUMBER) {
+			dev_err(dev, "Invalid number of PHYs\n");
+			of_node_put(child);
+			return -EINVAL;
+		}
+
+		phy = devm_of_phy_get(dev, child, 0);
+		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");
+				return ret;
+			}
+			dev_dbg(dev, "Failed to get usb2 phy\n");
+		}
+		exynos_ehci->phy_g[phy_number] = phy;
+	}
+
+	return ret;
+}
+
+static int exynos_ehci_phy_enable(struct device *dev)
+{
+	struct usb_hcd *hcd = dev_get_drvdata(dev);
+	struct exynos_ehci_hcd *exynos_ehci = to_exynos_ehci(hcd);
+	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 (ret)
+		for (i--; i >= 0; i--)
+			if (!IS_ERR(exynos_ehci->phy_g[i]))
+				phy_power_off(exynos_ehci->phy_g[i]);
+
+	return ret;
+}
+
+static void exynos_ehci_phy_disable(struct device *dev)
+{
+	struct usb_hcd *hcd = dev_get_drvdata(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]);
+}
+
+static void exynos_setup_vbus_gpio(struct device *dev)
 {
-	struct device *dev = &pdev->dev;
 	int err;
 	int gpio;
 
@@ -75,7 +165,6 @@ static int exynos_ehci_probe(struct platform_device *pdev)
 	struct usb_hcd *hcd;
 	struct ehci_hcd *ehci;
 	struct resource *res;
-	struct usb_phy *phy;
 	int irq;
 	int err;
 
@@ -88,7 +177,7 @@ static int exynos_ehci_probe(struct platform_device *pdev)
 	if (err)
 		return err;
 
-	exynos_setup_vbus_gpio(pdev);
+	exynos_setup_vbus_gpio(&pdev->dev);
 
 	hcd = usb_create_hcd(&exynos_ehci_hc_driver,
 			     &pdev->dev, dev_name(&pdev->dev));
@@ -102,15 +191,9 @@ static int exynos_ehci_probe(struct platform_device *pdev)
 					"samsung,exynos5440-ehci"))
 		goto skip_phy;
 
-	phy = devm_usb_get_phy(&pdev->dev, USB_PHY_TYPE_USB2);
-	if (IS_ERR(phy)) {
-		usb_put_hcd(hcd);
-		dev_warn(&pdev->dev, "no platform data or transceiver defined\n");
-		return -EPROBE_DEFER;
-	} else {
-		exynos_ehci->phy = phy;
-		exynos_ehci->otg = phy->otg;
-	}
+	err = exynos_ehci_get_phy(&pdev->dev, exynos_ehci);
+	if (err)
+		goto fail_clk;
 
 skip_phy:
 
@@ -135,10 +218,9 @@ skip_phy:
 
 	hcd->rsrc_start = res->start;
 	hcd->rsrc_len = resource_size(res);
-	hcd->regs = devm_ioremap(&pdev->dev, res->start, hcd->rsrc_len);
-	if (!hcd->regs) {
-		dev_err(&pdev->dev, "Failed to remap I/O memory\n");
-		err = -ENOMEM;
+	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(hcd->regs)) {
+		err = PTR_ERR(hcd->regs);
 		goto fail_io;
 	}
 
@@ -152,8 +234,11 @@ skip_phy:
 	if (exynos_ehci->otg)
 		exynos_ehci->otg->set_host(exynos_ehci->otg, &hcd->self);
 
-	if (exynos_ehci->phy)
-		usb_phy_init(exynos_ehci->phy);
+	err = exynos_ehci_phy_enable(&pdev->dev);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to enable USB phy\n");
+		goto fail_io;
+	}
 
 	ehci = hcd_to_ehci(hcd);
 	ehci->caps = hcd->regs;
@@ -173,8 +258,7 @@ skip_phy:
 	return 0;
 
 fail_add_hcd:
-	if (exynos_ehci->phy)
-		usb_phy_shutdown(exynos_ehci->phy);
+	exynos_ehci_phy_disable(&pdev->dev);
 fail_io:
 	clk_disable_unprepare(exynos_ehci->clk);
 fail_clk:
@@ -192,8 +276,7 @@ static int exynos_ehci_remove(struct platform_device *pdev)
 	if (exynos_ehci->otg)
 		exynos_ehci->otg->set_host(exynos_ehci->otg, &hcd->self);
 
-	if (exynos_ehci->phy)
-		usb_phy_shutdown(exynos_ehci->phy);
+	exynos_ehci_phy_disable(&pdev->dev);
 
 	clk_disable_unprepare(exynos_ehci->clk);
 
@@ -218,8 +301,7 @@ static int exynos_ehci_suspend(struct device *dev)
 	if (exynos_ehci->otg)
 		exynos_ehci->otg->set_host(exynos_ehci->otg, &hcd->self);
 
-	if (exynos_ehci->phy)
-		usb_phy_shutdown(exynos_ehci->phy);
+	exynos_ehci_phy_disable(dev);
 
 	clk_disable_unprepare(exynos_ehci->clk);
 
@@ -230,14 +312,19 @@ static int exynos_ehci_resume(struct device *dev)
 {
 	struct usb_hcd *hcd = dev_get_drvdata(dev);
 	struct exynos_ehci_hcd *exynos_ehci = to_exynos_ehci(hcd);
+	int ret;
 
 	clk_prepare_enable(exynos_ehci->clk);
 
 	if (exynos_ehci->otg)
 		exynos_ehci->otg->set_host(exynos_ehci->otg, &hcd->self);
 
-	if (exynos_ehci->phy)
-		usb_phy_init(exynos_ehci->phy);
+	ret = exynos_ehci_phy_enable(dev);
+	if (ret) {
+		dev_err(dev, "Failed to enable USB phy\n");
+		clk_disable_unprepare(exynos_ehci->clk);
+		return ret;
+	}
 
 	/* DMA burst Enable */
 	writel(EHCI_INSNREG00_ENABLE_DMA_BURST, EHCI_INSNREG00(hcd->regs));
diff --git a/drivers/usb/host/ehci-hub.c b/drivers/usb/host/ehci-hub.c
index 7ae0c4d5174..cc305c71ac3 100644
--- a/drivers/usb/host/ehci-hub.c
+++ b/drivers/usb/host/ehci-hub.c
@@ -33,15 +33,6 @@
 
 #ifdef	CONFIG_PM
 
-static int ehci_hub_control(
-	struct usb_hcd	*hcd,
-	u16		typeReq,
-	u16		wValue,
-	u16		wIndex,
-	char		*buf,
-	u16		wLength
-);
-
 static int persist_enabled_on_companion(struct usb_device *udev, void *unused)
 {
 	return !udev->maxchild && udev->persist_enabled &&
@@ -865,7 +856,7 @@ cleanup:
 #endif /* CONFIG_USB_HCD_TEST_MODE */
 /*-------------------------------------------------------------------------*/
 
-static int ehci_hub_control (
+int ehci_hub_control(
 	struct usb_hcd	*hcd,
 	u16		typeReq,
 	u16		wValue,
@@ -1285,6 +1276,7 @@ error_exit:
 	spin_unlock_irqrestore (&ehci->lock, flags);
 	return retval;
 }
+EXPORT_SYMBOL_GPL(ehci_hub_control);
 
 static void ehci_relinquish_port(struct usb_hcd *hcd, int portnum)
 {
diff --git a/drivers/usb/host/ehci-msm.c b/drivers/usb/host/ehci-msm.c
index f341651d6f6..982c09bebe0 100644
--- a/drivers/usb/host/ehci-msm.c
+++ b/drivers/usb/host/ehci-msm.c
@@ -96,10 +96,9 @@ static int ehci_msm_probe(struct platform_device *pdev)
 
 	hcd->rsrc_start = res->start;
 	hcd->rsrc_len = resource_size(res);
-	hcd->regs = devm_ioremap(&pdev->dev, hcd->rsrc_start, hcd->rsrc_len);
-	if (!hcd->regs) {
-		dev_err(&pdev->dev, "ioremap failed\n");
-		ret = -ENOMEM;
+	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(hcd->regs)) {
+		ret = PTR_ERR(hcd->regs);
 		goto put_hcd;
 	}
 
diff --git a/drivers/usb/host/ehci-mv.c b/drivers/usb/host/ehci-mv.c
index bd61612a725..08147c35f83 100644
--- a/drivers/usb/host/ehci-mv.c
+++ b/drivers/usb/host/ehci-mv.c
@@ -176,11 +176,9 @@ static int mv_ehci_probe(struct platform_device *pdev)
 		goto err_put_hcd;
 	}
 
-	ehci_mv->phy_regs = devm_ioremap(&pdev->dev, r->start,
-					 resource_size(r));
-	if (!ehci_mv->phy_regs) {
-		dev_err(&pdev->dev, "failed to map phy I/O memory\n");
-		retval = -EFAULT;
+	ehci_mv->phy_regs = devm_ioremap_resource(&pdev->dev, r);
+	if (IS_ERR(ehci_mv->phy_regs)) {
+		retval = PTR_ERR(ehci_mv->phy_regs);
 		goto err_put_hcd;
 	}
 
@@ -191,11 +189,9 @@ static int mv_ehci_probe(struct platform_device *pdev)
 		goto err_put_hcd;
 	}
 
-	ehci_mv->cap_regs = devm_ioremap(&pdev->dev, r->start,
-					 resource_size(r));
-	if (ehci_mv->cap_regs == NULL) {
-		dev_err(&pdev->dev, "failed to map I/O memory\n");
-		retval = -EFAULT;
+	ehci_mv->cap_regs = devm_ioremap_resource(&pdev->dev, r);
+	if (IS_ERR(ehci_mv->cap_regs)) {
+		retval = PTR_ERR(ehci_mv->cap_regs);
 		goto err_put_hcd;
 	}
 
diff --git a/drivers/usb/host/ehci-orion.c b/drivers/usb/host/ehci-orion.c
index 30d35e5e503..22e15cab8ea 100644
--- a/drivers/usb/host/ehci-orion.c
+++ b/drivers/usb/host/ehci-orion.c
@@ -15,6 +15,7 @@
 #include <linux/clk.h>
 #include <linux/platform_data/usb-ehci-orion.h>
 #include <linux/of.h>
+#include <linux/phy/phy.h>
 #include <linux/of_device.h>
 #include <linux/of_irq.h>
 #include <linux/usb.h>
@@ -42,6 +43,13 @@
 
 #define DRIVER_DESC "EHCI orion driver"
 
+#define hcd_to_orion_priv(h) ((struct orion_ehci_hcd *)hcd_to_ehci(h)->priv)
+
+struct orion_ehci_hcd {
+	struct clk *clk;
+	struct phy *phy;
+};
+
 static const char hcd_name[] = "ehci-orion";
 
 static struct hc_driver __read_mostly ehci_orion_hc_driver;
@@ -137,6 +145,10 @@ ehci_orion_conf_mbus_windows(struct usb_hcd *hcd,
 	}
 }
 
+static const struct ehci_driver_overrides orion_overrides __initconst = {
+	.extra_priv_size =	sizeof(struct orion_ehci_hcd),
+};
+
 static int ehci_orion_drv_probe(struct platform_device *pdev)
 {
 	struct orion_ehci_data *pd = dev_get_platdata(&pdev->dev);
@@ -144,26 +156,23 @@ static int ehci_orion_drv_probe(struct platform_device *pdev)
 	struct resource *res;
 	struct usb_hcd *hcd;
 	struct ehci_hcd *ehci;
-	struct clk *clk;
 	void __iomem *regs;
 	int irq, err;
 	enum orion_ehci_phy_ver phy_version;
+	struct orion_ehci_hcd *priv;
 
 	if (usb_disabled())
 		return -ENODEV;
 
 	pr_debug("Initializing Orion-SoC USB Host Controller\n");
 
-	if (pdev->dev.of_node)
-		irq = irq_of_parse_and_map(pdev->dev.of_node, 0);
-	else
-		irq = platform_get_irq(pdev, 0);
+	irq = platform_get_irq(pdev, 0);
 	if (irq <= 0) {
 		dev_err(&pdev->dev,
 			"Found HC with no IRQ. Check %s setup!\n",
 			dev_name(&pdev->dev));
 		err = -ENODEV;
-		goto err1;
+		goto err;
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -172,7 +181,7 @@ static int ehci_orion_drv_probe(struct platform_device *pdev)
 			"Found HC with no register addr. Check %s setup!\n",
 			dev_name(&pdev->dev));
 		err = -ENODEV;
-		goto err1;
+		goto err;
 	}
 
 	/*
@@ -182,25 +191,19 @@ static int ehci_orion_drv_probe(struct platform_device *pdev)
 	 */
 	err = dma_coerce_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(32));
 	if (err)
-		goto err1;
+		goto err;
 
 	regs = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(regs)) {
 		err = PTR_ERR(regs);
-		goto err1;
+		goto err;
 	}
 
-	/* Not all platforms can gate the clock, so it is not
-	   an error if the clock does not exists. */
-	clk = devm_clk_get(&pdev->dev, NULL);
-	if (!IS_ERR(clk))
-		clk_prepare_enable(clk);
-
 	hcd = usb_create_hcd(&ehci_orion_hc_driver,
 			&pdev->dev, dev_name(&pdev->dev));
 	if (!hcd) {
 		err = -ENOMEM;
-		goto err2;
+		goto err;
 	}
 
 	hcd->rsrc_start = res->start;
@@ -211,6 +214,29 @@ static int ehci_orion_drv_probe(struct platform_device *pdev)
 	ehci->caps = hcd->regs + 0x100;
 	hcd->has_tt = 1;
 
+	priv = hcd_to_orion_priv(hcd);
+	/*
+	 * Not all platforms can gate the clock, so it is not an error if
+	 * the clock does not exists.
+	 */
+	priv->clk = devm_clk_get(&pdev->dev, NULL);
+	if (!IS_ERR(priv->clk))
+		clk_prepare_enable(priv->clk);
+
+	priv->phy = devm_phy_optional_get(&pdev->dev, "usb");
+	if (IS_ERR(priv->phy)) {
+		err = PTR_ERR(priv->phy);
+		goto err_phy_get;
+	} else {
+		err = phy_init(priv->phy);
+		if (err)
+			goto err_phy_init;
+
+		err = phy_power_on(priv->phy);
+		if (err)
+			goto err_phy_power_on;
+	}
+
 	/*
 	 * (Re-)program MBUS remapping windows if we are asked to.
 	 */
@@ -240,17 +266,23 @@ static int ehci_orion_drv_probe(struct platform_device *pdev)
 
 	err = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (err)
-		goto err3;
+		goto err_add_hcd;
 
 	device_wakeup_enable(hcd->self.controller);
 	return 0;
 
-err3:
+err_add_hcd:
+	if (!IS_ERR(priv->phy))
+		phy_power_off(priv->phy);
+err_phy_power_on:
+	if (!IS_ERR(priv->phy))
+		phy_exit(priv->phy);
+err_phy_init:
+err_phy_get:
+	if (!IS_ERR(priv->clk))
+		clk_disable_unprepare(priv->clk);
 	usb_put_hcd(hcd);
-err2:
-	if (!IS_ERR(clk))
-		clk_disable_unprepare(clk);
-err1:
+err:
 	dev_err(&pdev->dev, "init %s fail, %d\n",
 		dev_name(&pdev->dev), err);
 
@@ -260,14 +292,20 @@ err1:
 static int ehci_orion_drv_remove(struct platform_device *pdev)
 {
 	struct usb_hcd *hcd = platform_get_drvdata(pdev);
-	struct clk *clk;
+	struct orion_ehci_hcd *priv = hcd_to_orion_priv(hcd);
 
 	usb_remove_hcd(hcd);
+
+	if (!IS_ERR(priv->phy)) {
+		phy_power_off(priv->phy);
+		phy_exit(priv->phy);
+	}
+
+	if (!IS_ERR(priv->clk))
+		clk_disable_unprepare(priv->clk);
+
 	usb_put_hcd(hcd);
 
-	clk = devm_clk_get(&pdev->dev, NULL);
-	if (!IS_ERR(clk))
-		clk_disable_unprepare(clk);
 	return 0;
 }
 
@@ -295,7 +333,7 @@ static int __init ehci_orion_init(void)
 
 	pr_info("%s: " DRIVER_DESC "\n", hcd_name);
 
-	ehci_init_driver(&ehci_orion_hc_driver, NULL);
+	ehci_init_driver(&ehci_orion_hc_driver, &orion_overrides);
 	return platform_driver_register(&ehci_orion_driver);
 }
 module_init(ehci_orion_init);
diff --git a/drivers/usb/host/ehci-platform.c b/drivers/usb/host/ehci-platform.c
index c7dd93aad20..2f5b9ce3e04 100644
--- a/drivers/usb/host/ehci-platform.c
+++ b/drivers/usb/host/ehci-platform.c
@@ -29,6 +29,7 @@
 #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>
@@ -41,6 +42,7 @@
 
 struct ehci_platform_priv {
 	struct clk *clks[EHCI_MAX_CLKS];
+	struct reset_control *rst;
 	struct phy *phy;
 };
 
@@ -208,6 +210,18 @@ static int ehci_platform_probe(struct platform_device *dev)
 		}
 	}
 
+	priv->rst = devm_reset_control_get_optional(&dev->dev, NULL);
+	if (IS_ERR(priv->rst)) {
+		err = PTR_ERR(priv->rst);
+		if (err == -EPROBE_DEFER)
+			goto err_put_clks;
+		priv->rst = NULL;
+	} else {
+		err = reset_control_deassert(priv->rst);
+		if (err)
+			goto err_put_clks;
+	}
+
 	if (pdata->big_endian_desc)
 		ehci->big_endian_desc = 1;
 	if (pdata->big_endian_mmio)
@@ -218,7 +232,7 @@ static int ehci_platform_probe(struct platform_device *dev)
 		dev_err(&dev->dev,
 			"Error: CONFIG_USB_EHCI_BIG_ENDIAN_MMIO not set\n");
 		err = -EINVAL;
-		goto err_put_clks;
+		goto err_reset;
 	}
 #endif
 #ifndef CONFIG_USB_EHCI_BIG_ENDIAN_DESC
@@ -226,14 +240,14 @@ static int ehci_platform_probe(struct platform_device *dev)
 		dev_err(&dev->dev,
 			"Error: CONFIG_USB_EHCI_BIG_ENDIAN_DESC not set\n");
 		err = -EINVAL;
-		goto err_put_clks;
+		goto err_reset;
 	}
 #endif
 
 	if (pdata->power_on) {
 		err = pdata->power_on(dev);
 		if (err < 0)
-			goto err_put_clks;
+			goto err_reset;
 	}
 
 	hcd->rsrc_start = res_mem->start;
@@ -256,6 +270,9 @@ static int ehci_platform_probe(struct platform_device *dev)
 err_power:
 	if (pdata->power_off)
 		pdata->power_off(dev);
+err_reset:
+	if (priv->rst)
+		reset_control_assert(priv->rst);
 err_put_clks:
 	while (--clk >= 0)
 		clk_put(priv->clks[clk]);
@@ -280,6 +297,9 @@ static int ehci_platform_remove(struct platform_device *dev)
 	if (pdata->power_off)
 		pdata->power_off(dev);
 
+	if (priv->rst)
+		reset_control_assert(priv->rst);
+
 	for (clk = 0; clk < EHCI_MAX_CLKS && priv->clks[clk]; clk++)
 		clk_put(priv->clks[clk]);
 
diff --git a/drivers/usb/host/ehci-pmcmsp.c b/drivers/usb/host/ehci-pmcmsp.c
index af3974a5e7c..7d75465d97c 100644
--- a/drivers/usb/host/ehci-pmcmsp.c
+++ b/drivers/usb/host/ehci-pmcmsp.c
@@ -68,9 +68,6 @@ static void usb_hcd_tdi_set_mode(struct ehci_hcd *ehci)
 
 	/* set TWI GPIO USB_HOST_DEV pin high */
 	gpio_direction_output(MSP_PIN_USB0_HOST_DEV, 1);
-#ifdef CONFIG_MSP_HAS_DUAL_USB
-	gpio_direction_output(MSP_PIN_USB1_HOST_DEV, 1);
-#endif
 }
 
 /* called during probe() after chip reset completes */
@@ -248,33 +245,6 @@ void usb_hcd_msp_remove(struct usb_hcd *hcd, struct platform_device *dev)
 	usb_put_hcd(hcd);
 }
 
-#ifdef CONFIG_MSP_HAS_DUAL_USB
-/*
- * Wrapper around the main ehci_irq.  Since both USB host controllers are
- * sharing the same IRQ, need to first determine whether we're the intended
- * recipient of this interrupt.
- */
-static irqreturn_t ehci_msp_irq(struct usb_hcd *hcd)
-{
-	u32 int_src;
-	struct device *dev = hcd->self.controller;
-	struct platform_device *pdev;
-	struct mspusb_device *mdev;
-	struct ehci_hcd	*ehci = hcd_to_ehci(hcd);
-	/* need to reverse-map a couple of containers to get our device */
-	pdev = to_platform_device(dev);
-	mdev = to_mspusb_device(pdev);
-
-	/* Check to see if this interrupt is for this host controller */
-	int_src = ehci_readl(ehci, &mdev->mab_regs->int_stat);
-	if (int_src & (1 << pdev->id))
-		return ehci_irq(hcd);
-
-	/* Not for this device */
-	return IRQ_NONE;
-}
-#endif /* DUAL_USB */
-
 static const struct hc_driver ehci_msp_hc_driver = {
 	.description =		hcd_name,
 	.product_desc =		"PMC MSP EHCI",
@@ -283,11 +253,7 @@ static const struct hc_driver ehci_msp_hc_driver = {
 	/*
 	 * generic hardware linkage
 	 */
-#ifdef CONFIG_MSP_HAS_DUAL_USB
-	.irq =			ehci_msp_irq,
-#else
 	.irq =			ehci_irq,
-#endif
 	.flags =		HCD_MEMORY | HCD_USB2 | HCD_BH,
 
 	/*
@@ -334,9 +300,6 @@ static int ehci_hcd_msp_drv_probe(struct platform_device *pdev)
 		return -ENODEV;
 
 	gpio_request(MSP_PIN_USB0_HOST_DEV, "USB0_HOST_DEV_GPIO");
-#ifdef CONFIG_MSP_HAS_DUAL_USB
-	gpio_request(MSP_PIN_USB1_HOST_DEV, "USB1_HOST_DEV_GPIO");
-#endif
 
 	ret = usb_hcd_msp_probe(&ehci_msp_hc_driver, pdev);
 
@@ -351,9 +314,6 @@ static int ehci_hcd_msp_drv_remove(struct platform_device *pdev)
 
 	/* free TWI GPIO USB_HOST_DEV pin */
 	gpio_free(MSP_PIN_USB0_HOST_DEV);
-#ifdef CONFIG_MSP_HAS_DUAL_USB
-	gpio_free(MSP_PIN_USB1_HOST_DEV);
-#endif
 
 	return 0;
 }
diff --git a/drivers/usb/host/ehci-spear.c b/drivers/usb/host/ehci-spear.c
index 8bd915b2ae8..1d59958ad0c 100644
--- a/drivers/usb/host/ehci-spear.c
+++ b/drivers/usb/host/ehci-spear.c
@@ -106,16 +106,9 @@ static int spear_ehci_hcd_drv_probe(struct platform_device *pdev)
 
 	hcd->rsrc_start = res->start;
 	hcd->rsrc_len = resource_size(res);
-	if (!devm_request_mem_region(&pdev->dev, hcd->rsrc_start, hcd->rsrc_len,
-				driver->description)) {
-		retval = -EBUSY;
-		goto err_put_hcd;
-	}
-
-	hcd->regs = devm_ioremap(&pdev->dev, hcd->rsrc_start, hcd->rsrc_len);
-	if (hcd->regs == NULL) {
-		dev_dbg(&pdev->dev, "error mapping memory\n");
-		retval = -ENOMEM;
+	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(hcd->regs)) {
+		retval = PTR_ERR(hcd->regs);
 		goto err_put_hcd;
 	}
 
diff --git a/drivers/usb/host/ehci-tegra.c b/drivers/usb/host/ehci-tegra.c
index 7ef00ecb0da..6fdcb8ad229 100644
--- a/drivers/usb/host/ehci-tegra.c
+++ b/drivers/usb/host/ehci-tegra.c
@@ -51,10 +51,6 @@ struct tegra_ehci_soc_config {
 	bool has_hostpc;
 };
 
-static int (*orig_hub_control)(struct usb_hcd *hcd,
-				u16 typeReq, u16 wValue, u16 wIndex,
-				char *buf, u16 wLength);
-
 struct tegra_ehci_hcd {
 	struct tegra_usb_phy *phy;
 	struct clk *clk;
@@ -236,7 +232,7 @@ static int tegra_ehci_hub_control(
 	spin_unlock_irqrestore(&ehci->lock, flags);
 
 	/* Handle the hub control events here */
-	return orig_hub_control(hcd, typeReq, wValue, wIndex, buf, wLength);
+	return ehci_hub_control(hcd, typeReq, wValue, wIndex, buf, wLength);
 
 done:
 	spin_unlock_irqrestore(&ehci->lock, flags);
@@ -415,10 +411,9 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 	}
 	hcd->rsrc_start = res->start;
 	hcd->rsrc_len = resource_size(res);
-	hcd->regs = devm_ioremap(&pdev->dev, res->start, resource_size(res));
-	if (!hcd->regs) {
-		dev_err(&pdev->dev, "Failed to remap I/O memory\n");
-		err = -ENOMEM;
+	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(hcd->regs)) {
+		err = PTR_ERR(hcd->regs);
 		goto cleanup_clk_en;
 	}
 	ehci->caps = hcd->regs + 0x100;
@@ -554,8 +549,6 @@ static int __init ehci_tegra_init(void)
 	 * too easy.
 	 */
 
-	orig_hub_control = tegra_ehci_hc_driver.hub_control;
-
 	tegra_ehci_hc_driver.map_urb_for_dma = tegra_ehci_map_urb_for_dma;
 	tegra_ehci_hc_driver.unmap_urb_for_dma = tegra_ehci_unmap_urb_for_dma;
 	tegra_ehci_hc_driver.hub_control = tegra_ehci_hub_control;
diff --git a/drivers/usb/host/ehci-tilegx.c b/drivers/usb/host/ehci-tilegx.c
index f3713d32c9a..0d247673c3c 100644
--- a/drivers/usb/host/ehci-tilegx.c
+++ b/drivers/usb/host/ehci-tilegx.c
@@ -142,8 +142,8 @@ static int ehci_hcd_tilegx_drv_probe(struct platform_device *pdev)
 	ehci->hcs_params = readl(&ehci->caps->hcs_params);
 
 	/* Create our IRQs and register them. */
-	pdata->irq = create_irq();
-	if (pdata->irq < 0) {
+	pdata->irq = irq_alloc_hwirq(-1);
+	if (!pdata->irq) {
 		ret = -ENXIO;
 		goto err_no_irq;
 	}
@@ -175,7 +175,7 @@ static int ehci_hcd_tilegx_drv_probe(struct platform_device *pdev)
 	}
 
 err_have_irq:
-	destroy_irq(pdata->irq);
+	irq_free_hwirq(pdata->irq);
 err_no_irq:
 	tilegx_stop_ehc();
 	usb_put_hcd(hcd);
@@ -193,7 +193,7 @@ static int ehci_hcd_tilegx_drv_remove(struct platform_device *pdev)
 	usb_put_hcd(hcd);
 	tilegx_stop_ehc();
 	gxio_usb_host_destroy(&pdata->usb_ctx);
-	destroy_irq(pdata->irq);
+	irq_free_hwirq(pdata->irq);
 
 	return 0;
 }
diff --git a/drivers/usb/host/ehci.h b/drivers/usb/host/ehci.h
index 9dfc6c1394d..eee228a26a0 100644
--- a/drivers/usb/host/ehci.h
+++ b/drivers/usb/host/ehci.h
@@ -872,4 +872,7 @@ extern int	ehci_suspend(struct usb_hcd *hcd, bool do_wakeup);
 extern int	ehci_resume(struct usb_hcd *hcd, bool hibernated);
 #endif	/* CONFIG_PM */
 
+extern int	ehci_hub_control(struct usb_hcd	*hcd, u16 typeReq, u16 wValue,
+				 u16 wIndex, char *buf, u16 wLength);
+
 #endif /* __LINUX_EHCI_HCD_H */
diff --git a/drivers/usb/host/max3421-hcd.c b/drivers/usb/host/max3421-hcd.c
new file mode 100644
index 00000000000..858efcfda50
--- /dev/null
+++ b/drivers/usb/host/max3421-hcd.c
@@ -0,0 +1,1957 @@
+/*
+ * MAX3421 Host Controller driver for USB.
+ *
+ * Author: David Mosberger-Tang <davidm@egauge.net>
+ *
+ * (C) Copyright 2014 David Mosberger-Tang <davidm@egauge.net>
+ *
+ * MAX3421 is a chip implementing a USB 2.0 Full-/Low-Speed host
+ * controller on a SPI bus.
+ *
+ * Based on:
+ *	o MAX3421E datasheet
+ *		http://datasheets.maximintegrated.com/en/ds/MAX3421E.pdf
+ *	o MAX3421E Programming Guide
+ *		http://www.hdl.co.jp/ftpdata/utl-001/AN3785.pdf
+ *	o gadget/dummy_hcd.c
+ *		For USB HCD implementation.
+ *	o Arduino MAX3421 driver
+ *	     https://github.com/felis/USB_Host_Shield_2.0/blob/master/Usb.cpp
+ *
+ * This file is licenced under the GPL v2.
+ *
+ * Important note on worst-case (full-speed) packet size constraints
+ * (See USB 2.0 Section 5.6.3 and following):
+ *
+ *	- control:	  64 bytes
+ *	- isochronous:	1023 bytes
+ *	- interrupt:	  64 bytes
+ *	- bulk:		  64 bytes
+ *
+ * Since the MAX3421 FIFO size is 64 bytes, we do not have to work about
+ * multi-FIFO writes/reads for a single USB packet *except* for isochronous
+ * transfers.  We don't support isochronous transfers at this time, so we
+ * just assume that a USB packet always fits into a single FIFO buffer.
+ *
+ * NOTE: The June 2006 version of "MAX3421E Programming Guide"
+ * (AN3785) has conflicting info for the RCVDAVIRQ bit:
+ *
+ *	The description of RCVDAVIRQ says "The CPU *must* clear
+ *	this IRQ bit (by writing a 1 to it) before reading the
+ *	RCVFIFO data.
+ *
+ * However, the earlier section on "Programming BULK-IN
+ * Transfers" says * that:
+ *
+ *	After the CPU retrieves the data, it clears the
+ *	RCVDAVIRQ bit.
+ *
+ * The December 2006 version has been corrected and it consistently
+ * states the second behavior is the correct one.
+ *
+ * Synchronous SPI transactions sleep so we can't perform any such
+ * transactions while holding a spin-lock (and/or while interrupts are
+ * masked).  To achieve this, all SPI transactions are issued from a
+ * single thread (max3421_spi_thread).
+ */
+
+#include <linux/module.h>
+#include <linux/spi/spi.h>
+#include <linux/usb.h>
+#include <linux/usb/hcd.h>
+
+#include <linux/platform_data/max3421-hcd.h>
+
+#define DRIVER_DESC	"MAX3421 USB Host-Controller Driver"
+#define DRIVER_VERSION	"1.0"
+
+/* 11-bit counter that wraps around (USB 2.0 Section 8.3.3): */
+#define USB_MAX_FRAME_NUMBER	0x7ff
+#define USB_MAX_RETRIES		3 /* # of retries before error is reported */
+
+/*
+ * Max. # of times we're willing to retransmit a request immediately in
+ * resposne to a NAK.  Afterwards, we fall back on trying once a frame.
+ */
+#define NAK_MAX_FAST_RETRANSMITS	2
+
+#define POWER_BUDGET	500	/* in mA; use 8 for low-power port testing */
+
+/* Port-change mask: */
+#define PORT_C_MASK	((USB_PORT_STAT_C_CONNECTION |	\
+			  USB_PORT_STAT_C_ENABLE |	\
+			  USB_PORT_STAT_C_SUSPEND |	\
+			  USB_PORT_STAT_C_OVERCURRENT | \
+			  USB_PORT_STAT_C_RESET) << 16)
+
+enum max3421_rh_state {
+	MAX3421_RH_RESET,
+	MAX3421_RH_SUSPENDED,
+	MAX3421_RH_RUNNING
+};
+
+enum pkt_state {
+	PKT_STATE_SETUP,	/* waiting to send setup packet to ctrl pipe */
+	PKT_STATE_TRANSFER,	/* waiting to xfer transfer_buffer */
+	PKT_STATE_TERMINATE	/* waiting to terminate control transfer */
+};
+
+enum scheduling_pass {
+	SCHED_PASS_PERIODIC,
+	SCHED_PASS_NON_PERIODIC,
+	SCHED_PASS_DONE
+};
+
+struct max3421_dma_buf {
+	u8 data[2];
+};
+
+struct max3421_hcd {
+	spinlock_t lock;
+
+	struct task_struct *spi_thread;
+
+	struct max3421_hcd *next;
+
+	enum max3421_rh_state rh_state;
+	/* lower 16 bits contain port status, upper 16 bits the change mask: */
+	u32 port_status;
+
+	unsigned active:1;
+
+	struct list_head ep_list;	/* list of EP's with work */
+
+	/*
+	 * The following are owned by spi_thread (may be accessed by
+	 * SPI-thread without acquiring the HCD lock:
+	 */
+	u8 rev;				/* chip revision */
+	u16 frame_number;
+	/*
+	 * kmalloc'd buffers guaranteed to be in separate (DMA)
+	 * cache-lines:
+	 */
+	struct max3421_dma_buf *tx;
+	struct max3421_dma_buf *rx;
+	/*
+	 * URB we're currently processing.  Must not be reset to NULL
+	 * unless MAX3421E chip is idle:
+	 */
+	struct urb *curr_urb;
+	enum scheduling_pass sched_pass;
+	struct usb_device *loaded_dev;	/* dev that's loaded into the chip */
+	int loaded_epnum;		/* epnum whose toggles are loaded */
+	int urb_done;			/* > 0 -> no errors, < 0: errno */
+	size_t curr_len;
+	u8 hien;
+	u8 mode;
+	u8 iopins[2];
+	unsigned int do_enable_irq:1;
+	unsigned int do_reset_hcd:1;
+	unsigned int do_reset_port:1;
+	unsigned int do_check_unlink:1;
+	unsigned int do_iopin_update:1;
+#ifdef DEBUG
+	unsigned long err_stat[16];
+#endif
+};
+
+struct max3421_ep {
+	struct usb_host_endpoint *ep;
+	struct list_head ep_list;
+	u32 naks;
+	u16 last_active;		/* frame # this ep was last active */
+	enum pkt_state pkt_state;
+	u8 retries;
+	u8 retransmit;			/* packet needs retransmission */
+};
+
+static struct max3421_hcd *max3421_hcd_list;
+
+#define MAX3421_FIFO_SIZE	64
+
+#define MAX3421_SPI_DIR_RD	0	/* read register from MAX3421 */
+#define MAX3421_SPI_DIR_WR	1	/* write register to MAX3421 */
+
+/* SPI commands: */
+#define MAX3421_SPI_DIR_SHIFT	1
+#define MAX3421_SPI_REG_SHIFT	3
+
+#define MAX3421_REG_RCVFIFO	1
+#define MAX3421_REG_SNDFIFO	2
+#define MAX3421_REG_SUDFIFO	4
+#define MAX3421_REG_RCVBC	6
+#define MAX3421_REG_SNDBC	7
+#define MAX3421_REG_USBIRQ	13
+#define MAX3421_REG_USBIEN	14
+#define MAX3421_REG_USBCTL	15
+#define MAX3421_REG_CPUCTL	16
+#define MAX3421_REG_PINCTL	17
+#define MAX3421_REG_REVISION	18
+#define MAX3421_REG_IOPINS1	20
+#define MAX3421_REG_IOPINS2	21
+#define MAX3421_REG_GPINIRQ	22
+#define MAX3421_REG_GPINIEN	23
+#define MAX3421_REG_GPINPOL	24
+#define MAX3421_REG_HIRQ	25
+#define MAX3421_REG_HIEN	26
+#define MAX3421_REG_MODE	27
+#define MAX3421_REG_PERADDR	28
+#define MAX3421_REG_HCTL	29
+#define MAX3421_REG_HXFR	30
+#define MAX3421_REG_HRSL	31
+
+enum {
+	MAX3421_USBIRQ_OSCOKIRQ_BIT = 0,
+	MAX3421_USBIRQ_NOVBUSIRQ_BIT = 5,
+	MAX3421_USBIRQ_VBUSIRQ_BIT
+};
+
+enum {
+	MAX3421_CPUCTL_IE_BIT = 0,
+	MAX3421_CPUCTL_PULSEWID0_BIT = 6,
+	MAX3421_CPUCTL_PULSEWID1_BIT
+};
+
+enum {
+	MAX3421_USBCTL_PWRDOWN_BIT = 4,
+	MAX3421_USBCTL_CHIPRES_BIT
+};
+
+enum {
+	MAX3421_PINCTL_GPXA_BIT	= 0,
+	MAX3421_PINCTL_GPXB_BIT,
+	MAX3421_PINCTL_POSINT_BIT,
+	MAX3421_PINCTL_INTLEVEL_BIT,
+	MAX3421_PINCTL_FDUPSPI_BIT,
+	MAX3421_PINCTL_EP0INAK_BIT,
+	MAX3421_PINCTL_EP2INAK_BIT,
+	MAX3421_PINCTL_EP3INAK_BIT,
+};
+
+enum {
+	MAX3421_HI_BUSEVENT_BIT = 0,	/* bus-reset/-resume */
+	MAX3421_HI_RWU_BIT,		/* remote wakeup */
+	MAX3421_HI_RCVDAV_BIT,		/* receive FIFO data available */
+	MAX3421_HI_SNDBAV_BIT,		/* send buffer available */
+	MAX3421_HI_SUSDN_BIT,		/* suspend operation done */
+	MAX3421_HI_CONDET_BIT,		/* peripheral connect/disconnect */
+	MAX3421_HI_FRAME_BIT,		/* frame generator */
+	MAX3421_HI_HXFRDN_BIT,		/* host transfer done */
+};
+
+enum {
+	MAX3421_HCTL_BUSRST_BIT = 0,
+	MAX3421_HCTL_FRMRST_BIT,
+	MAX3421_HCTL_SAMPLEBUS_BIT,
+	MAX3421_HCTL_SIGRSM_BIT,
+	MAX3421_HCTL_RCVTOG0_BIT,
+	MAX3421_HCTL_RCVTOG1_BIT,
+	MAX3421_HCTL_SNDTOG0_BIT,
+	MAX3421_HCTL_SNDTOG1_BIT
+};
+
+enum {
+	MAX3421_MODE_HOST_BIT = 0,
+	MAX3421_MODE_LOWSPEED_BIT,
+	MAX3421_MODE_HUBPRE_BIT,
+	MAX3421_MODE_SOFKAENAB_BIT,
+	MAX3421_MODE_SEPIRQ_BIT,
+	MAX3421_MODE_DELAYISO_BIT,
+	MAX3421_MODE_DMPULLDN_BIT,
+	MAX3421_MODE_DPPULLDN_BIT
+};
+
+enum {
+	MAX3421_HRSL_OK = 0,
+	MAX3421_HRSL_BUSY,
+	MAX3421_HRSL_BADREQ,
+	MAX3421_HRSL_UNDEF,
+	MAX3421_HRSL_NAK,
+	MAX3421_HRSL_STALL,
+	MAX3421_HRSL_TOGERR,
+	MAX3421_HRSL_WRONGPID,
+	MAX3421_HRSL_BADBC,
+	MAX3421_HRSL_PIDERR,
+	MAX3421_HRSL_PKTERR,
+	MAX3421_HRSL_CRCERR,
+	MAX3421_HRSL_KERR,
+	MAX3421_HRSL_JERR,
+	MAX3421_HRSL_TIMEOUT,
+	MAX3421_HRSL_BABBLE,
+	MAX3421_HRSL_RESULT_MASK = 0xf,
+	MAX3421_HRSL_RCVTOGRD_BIT = 4,
+	MAX3421_HRSL_SNDTOGRD_BIT,
+	MAX3421_HRSL_KSTATUS_BIT,
+	MAX3421_HRSL_JSTATUS_BIT
+};
+
+/* Return same error-codes as ohci.h:cc_to_error: */
+static const int hrsl_to_error[] = {
+	[MAX3421_HRSL_OK] =		0,
+	[MAX3421_HRSL_BUSY] =		-EINVAL,
+	[MAX3421_HRSL_BADREQ] =		-EINVAL,
+	[MAX3421_HRSL_UNDEF] =		-EINVAL,
+	[MAX3421_HRSL_NAK] =		-EAGAIN,
+	[MAX3421_HRSL_STALL] =		-EPIPE,
+	[MAX3421_HRSL_TOGERR] =		-EILSEQ,
+	[MAX3421_HRSL_WRONGPID] =	-EPROTO,
+	[MAX3421_HRSL_BADBC] =		-EREMOTEIO,
+	[MAX3421_HRSL_PIDERR] =		-EPROTO,
+	[MAX3421_HRSL_PKTERR] =		-EPROTO,
+	[MAX3421_HRSL_CRCERR] =		-EILSEQ,
+	[MAX3421_HRSL_KERR] =		-EIO,
+	[MAX3421_HRSL_JERR] =		-EIO,
+	[MAX3421_HRSL_TIMEOUT] =	-ETIME,
+	[MAX3421_HRSL_BABBLE] =		-EOVERFLOW
+};
+
+/*
+ * See http://www.beyondlogic.org/usbnutshell/usb4.shtml#Control for a
+ * reasonable overview of how control transfers use the the IN/OUT
+ * tokens.
+ */
+#define MAX3421_HXFR_BULK_IN(ep)	(0x00 | (ep))	/* bulk or interrupt */
+#define MAX3421_HXFR_SETUP		 0x10
+#define MAX3421_HXFR_BULK_OUT(ep)	(0x20 | (ep))	/* bulk or interrupt */
+#define MAX3421_HXFR_ISO_IN(ep)		(0x40 | (ep))
+#define MAX3421_HXFR_ISO_OUT(ep)	(0x60 | (ep))
+#define MAX3421_HXFR_HS_IN		 0x80		/* handshake in */
+#define MAX3421_HXFR_HS_OUT		 0xa0		/* handshake out */
+
+#define field(val, bit)	((val) << (bit))
+
+static inline s16
+frame_diff(u16 left, u16 right)
+{
+	return ((unsigned) (left - right)) % (USB_MAX_FRAME_NUMBER + 1);
+}
+
+static inline struct max3421_hcd *
+hcd_to_max3421(struct usb_hcd *hcd)
+{
+	return (struct max3421_hcd *) hcd->hcd_priv;
+}
+
+static inline struct usb_hcd *
+max3421_to_hcd(struct max3421_hcd *max3421_hcd)
+{
+	return container_of((void *) max3421_hcd, struct usb_hcd, hcd_priv);
+}
+
+static u8
+spi_rd8(struct usb_hcd *hcd, unsigned int reg)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct spi_transfer transfer;
+	struct spi_message msg;
+
+	memset(&transfer, 0, sizeof(transfer));
+
+	spi_message_init(&msg);
+
+	max3421_hcd->tx->data[0] =
+		(field(reg, MAX3421_SPI_REG_SHIFT) |
+		 field(MAX3421_SPI_DIR_RD, MAX3421_SPI_DIR_SHIFT));
+
+	transfer.tx_buf = max3421_hcd->tx->data;
+	transfer.rx_buf = max3421_hcd->rx->data;
+	transfer.len = 2;
+
+	spi_message_add_tail(&transfer, &msg);
+	spi_sync(spi, &msg);
+
+	return max3421_hcd->rx->data[1];
+}
+
+static void
+spi_wr8(struct usb_hcd *hcd, unsigned int reg, u8 val)
+{
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct spi_transfer transfer;
+	struct spi_message msg;
+
+	memset(&transfer, 0, sizeof(transfer));
+
+	spi_message_init(&msg);
+
+	max3421_hcd->tx->data[0] =
+		(field(reg, MAX3421_SPI_REG_SHIFT) |
+		 field(MAX3421_SPI_DIR_WR, MAX3421_SPI_DIR_SHIFT));
+	max3421_hcd->tx->data[1] = val;
+
+	transfer.tx_buf = max3421_hcd->tx->data;
+	transfer.len = 2;
+
+	spi_message_add_tail(&transfer, &msg);
+	spi_sync(spi, &msg);
+}
+
+static void
+spi_rd_buf(struct usb_hcd *hcd, unsigned int reg, void *buf, size_t len)
+{
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct spi_transfer transfer[2];
+	struct spi_message msg;
+
+	memset(transfer, 0, sizeof(transfer));
+
+	spi_message_init(&msg);
+
+	max3421_hcd->tx->data[0] =
+		(field(reg, MAX3421_SPI_REG_SHIFT) |
+		 field(MAX3421_SPI_DIR_RD, MAX3421_SPI_DIR_SHIFT));
+	transfer[0].tx_buf = max3421_hcd->tx->data;
+	transfer[0].len = 1;
+
+	transfer[1].rx_buf = buf;
+	transfer[1].len = len;
+
+	spi_message_add_tail(&transfer[0], &msg);
+	spi_message_add_tail(&transfer[1], &msg);
+	spi_sync(spi, &msg);
+}
+
+static void
+spi_wr_buf(struct usb_hcd *hcd, unsigned int reg, void *buf, size_t len)
+{
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct spi_transfer transfer[2];
+	struct spi_message msg;
+
+	memset(transfer, 0, sizeof(transfer));
+
+	spi_message_init(&msg);
+
+	max3421_hcd->tx->data[0] =
+		(field(reg, MAX3421_SPI_REG_SHIFT) |
+		 field(MAX3421_SPI_DIR_WR, MAX3421_SPI_DIR_SHIFT));
+
+	transfer[0].tx_buf = max3421_hcd->tx->data;
+	transfer[0].len = 1;
+
+	transfer[1].tx_buf = buf;
+	transfer[1].len = len;
+
+	spi_message_add_tail(&transfer[0], &msg);
+	spi_message_add_tail(&transfer[1], &msg);
+	spi_sync(spi, &msg);
+}
+
+/*
+ * Figure out the correct setting for the LOWSPEED and HUBPRE mode
+ * bits.  The HUBPRE bit needs to be set when MAX3421E operates at
+ * full speed, but it's talking to a low-speed device (i.e., through a
+ * hub).  Setting that bit ensures that every low-speed packet is
+ * preceded by a full-speed PRE PID.  Possible configurations:
+ *
+ * Hub speed:	Device speed:	=>	LOWSPEED bit:	HUBPRE bit:
+ *	FULL	FULL		=>	0		0
+ *	FULL	LOW		=>	1		1
+ *	LOW	LOW		=>	1		0
+ *	LOW	FULL		=>	1		0
+ */
+static void
+max3421_set_speed(struct usb_hcd *hcd, struct usb_device *dev)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	u8 mode_lowspeed, mode_hubpre, mode = max3421_hcd->mode;
+
+	mode_lowspeed = BIT(MAX3421_MODE_LOWSPEED_BIT);
+	mode_hubpre   = BIT(MAX3421_MODE_HUBPRE_BIT);
+	if (max3421_hcd->port_status & USB_PORT_STAT_LOW_SPEED) {
+		mode |=  mode_lowspeed;
+		mode &= ~mode_hubpre;
+	} else if (dev->speed == USB_SPEED_LOW) {
+		mode |= mode_lowspeed | mode_hubpre;
+	} else {
+		mode &= ~(mode_lowspeed | mode_hubpre);
+	}
+	if (mode != max3421_hcd->mode) {
+		max3421_hcd->mode = mode;
+		spi_wr8(hcd, MAX3421_REG_MODE, max3421_hcd->mode);
+	}
+
+}
+
+/*
+ * Caller must NOT hold HCD spinlock.
+ */
+static void
+max3421_set_address(struct usb_hcd *hcd, struct usb_device *dev, int epnum,
+		    int force_toggles)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	int old_epnum, same_ep, rcvtog, sndtog;
+	struct usb_device *old_dev;
+	u8 hctl;
+
+	old_dev = max3421_hcd->loaded_dev;
+	old_epnum = max3421_hcd->loaded_epnum;
+
+	same_ep = (dev == old_dev && epnum == old_epnum);
+	if (same_ep && !force_toggles)
+		return;
+
+	if (old_dev && !same_ep) {
+		/* save the old end-points toggles: */
+		u8 hrsl = spi_rd8(hcd, MAX3421_REG_HRSL);
+
+		rcvtog = (hrsl >> MAX3421_HRSL_RCVTOGRD_BIT) & 1;
+		sndtog = (hrsl >> MAX3421_HRSL_SNDTOGRD_BIT) & 1;
+
+		/* no locking: HCD (i.e., we) own toggles, don't we? */
+		usb_settoggle(old_dev, old_epnum, 0, rcvtog);
+		usb_settoggle(old_dev, old_epnum, 1, sndtog);
+	}
+	/* setup new endpoint's toggle bits: */
+	rcvtog = usb_gettoggle(dev, epnum, 0);
+	sndtog = usb_gettoggle(dev, epnum, 1);
+	hctl = (BIT(rcvtog + MAX3421_HCTL_RCVTOG0_BIT) |
+		BIT(sndtog + MAX3421_HCTL_SNDTOG0_BIT));
+
+	max3421_hcd->loaded_epnum = epnum;
+	spi_wr8(hcd, MAX3421_REG_HCTL, hctl);
+
+	/*
+	 * Note: devnum for one and the same device can change during
+	 * address-assignment so it's best to just always load the
+	 * address whenever the end-point changed/was forced.
+	 */
+	max3421_hcd->loaded_dev = dev;
+	spi_wr8(hcd, MAX3421_REG_PERADDR, dev->devnum);
+}
+
+static int
+max3421_ctrl_setup(struct usb_hcd *hcd, struct urb *urb)
+{
+	spi_wr_buf(hcd, MAX3421_REG_SUDFIFO, urb->setup_packet, 8);
+	return MAX3421_HXFR_SETUP;
+}
+
+static int
+max3421_transfer_in(struct usb_hcd *hcd, struct urb *urb)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	int epnum = usb_pipeendpoint(urb->pipe);
+
+	max3421_hcd->curr_len = 0;
+	max3421_hcd->hien |= BIT(MAX3421_HI_RCVDAV_BIT);
+	return MAX3421_HXFR_BULK_IN(epnum);
+}
+
+static int
+max3421_transfer_out(struct usb_hcd *hcd, struct urb *urb, int fast_retransmit)
+{
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	int epnum = usb_pipeendpoint(urb->pipe);
+	u32 max_packet;
+	void *src;
+
+	src = urb->transfer_buffer + urb->actual_length;
+
+	if (fast_retransmit) {
+		if (max3421_hcd->rev == 0x12) {
+			/* work around rev 0x12 bug: */
+			spi_wr8(hcd, MAX3421_REG_SNDBC, 0);
+			spi_wr8(hcd, MAX3421_REG_SNDFIFO, ((u8 *) src)[0]);
+			spi_wr8(hcd, MAX3421_REG_SNDBC, max3421_hcd->curr_len);
+		}
+		return MAX3421_HXFR_BULK_OUT(epnum);
+	}
+
+	max_packet = usb_maxpacket(urb->dev, urb->pipe, 1);
+
+	if (max_packet > MAX3421_FIFO_SIZE) {
+		/*
+		 * We do not support isochronous transfers at this
+		 * time.
+		 */
+		dev_err(&spi->dev,
+			"%s: packet-size of %u too big (limit is %u bytes)",
+			__func__, max_packet, MAX3421_FIFO_SIZE);
+		max3421_hcd->urb_done = -EMSGSIZE;
+		return -EMSGSIZE;
+	}
+	max3421_hcd->curr_len = min((urb->transfer_buffer_length -
+				     urb->actual_length), max_packet);
+
+	spi_wr_buf(hcd, MAX3421_REG_SNDFIFO, src, max3421_hcd->curr_len);
+	spi_wr8(hcd, MAX3421_REG_SNDBC, max3421_hcd->curr_len);
+	return MAX3421_HXFR_BULK_OUT(epnum);
+}
+
+/*
+ * Issue the next host-transfer command.
+ * Caller must NOT hold HCD spinlock.
+ */
+static void
+max3421_next_transfer(struct usb_hcd *hcd, int fast_retransmit)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct urb *urb = max3421_hcd->curr_urb;
+	struct max3421_ep *max3421_ep;
+	int cmd = -EINVAL;
+
+	if (!urb)
+		return;	/* nothing to do */
+
+	max3421_ep = urb->ep->hcpriv;
+
+	switch (max3421_ep->pkt_state) {
+	case PKT_STATE_SETUP:
+		cmd = max3421_ctrl_setup(hcd, urb);
+		break;
+
+	case PKT_STATE_TRANSFER:
+		if (usb_urb_dir_in(urb))
+			cmd = max3421_transfer_in(hcd, urb);
+		else
+			cmd = max3421_transfer_out(hcd, urb, fast_retransmit);
+		break;
+
+	case PKT_STATE_TERMINATE:
+		/*
+		 * IN transfers are terminated with HS_OUT token,
+		 * OUT transfers with HS_IN:
+		 */
+		if (usb_urb_dir_in(urb))
+			cmd = MAX3421_HXFR_HS_OUT;
+		else
+			cmd = MAX3421_HXFR_HS_IN;
+		break;
+	}
+
+	if (cmd < 0)
+		return;
+
+	/* issue the command and wait for host-xfer-done interrupt: */
+
+	spi_wr8(hcd, MAX3421_REG_HXFR, cmd);
+	max3421_hcd->hien |= BIT(MAX3421_HI_HXFRDN_BIT);
+}
+
+/*
+ * Find the next URB to process and start its execution.
+ *
+ * At this time, we do not anticipate ever connecting a USB hub to the
+ * MAX3421 chip, so at most USB device can be connected and we can use
+ * a simplistic scheduler: at the start of a frame, schedule all
+ * periodic transfers.  Once that is done, use the remainder of the
+ * frame to process non-periodic (bulk & control) transfers.
+ *
+ * Preconditions:
+ * o Caller must NOT hold HCD spinlock.
+ * o max3421_hcd->curr_urb MUST BE NULL.
+ * o MAX3421E chip must be idle.
+ */
+static int
+max3421_select_and_start_urb(struct usb_hcd *hcd)
+{
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct urb *urb, *curr_urb = NULL;
+	struct max3421_ep *max3421_ep;
+	int epnum, force_toggles = 0;
+	struct usb_host_endpoint *ep;
+	struct list_head *pos;
+	unsigned long flags;
+
+	spin_lock_irqsave(&max3421_hcd->lock, flags);
+
+	for (;
+	     max3421_hcd->sched_pass < SCHED_PASS_DONE;
+	     ++max3421_hcd->sched_pass)
+		list_for_each(pos, &max3421_hcd->ep_list) {
+			urb = NULL;
+			max3421_ep = container_of(pos, struct max3421_ep,
+						  ep_list);
+			ep = max3421_ep->ep;
+
+			switch (usb_endpoint_type(&ep->desc)) {
+			case USB_ENDPOINT_XFER_ISOC:
+			case USB_ENDPOINT_XFER_INT:
+				if (max3421_hcd->sched_pass !=
+				    SCHED_PASS_PERIODIC)
+					continue;
+				break;
+
+			case USB_ENDPOINT_XFER_CONTROL:
+			case USB_ENDPOINT_XFER_BULK:
+				if (max3421_hcd->sched_pass !=
+				    SCHED_PASS_NON_PERIODIC)
+					continue;
+				break;
+			}
+
+			if (list_empty(&ep->urb_list))
+				continue;	/* nothing to do */
+			urb = list_first_entry(&ep->urb_list, struct urb,
+					       urb_list);
+			if (urb->unlinked) {
+				dev_dbg(&spi->dev, "%s: URB %p unlinked=%d",
+					__func__, urb, urb->unlinked);
+				max3421_hcd->curr_urb = urb;
+				max3421_hcd->urb_done = 1;
+				spin_unlock_irqrestore(&max3421_hcd->lock,
+						       flags);
+				return 1;
+			}
+
+			switch (usb_endpoint_type(&ep->desc)) {
+			case USB_ENDPOINT_XFER_CONTROL:
+				/*
+				 * Allow one control transaction per
+				 * frame per endpoint:
+				 */
+				if (frame_diff(max3421_ep->last_active,
+					       max3421_hcd->frame_number) == 0)
+					continue;
+				break;
+
+			case USB_ENDPOINT_XFER_BULK:
+				if (max3421_ep->retransmit
+				    && (frame_diff(max3421_ep->last_active,
+						   max3421_hcd->frame_number)
+					== 0))
+					/*
+					 * We already tried this EP
+					 * during this frame and got a
+					 * NAK or error; wait for next frame
+					 */
+					continue;
+				break;
+
+			case USB_ENDPOINT_XFER_ISOC:
+			case USB_ENDPOINT_XFER_INT:
+				if (frame_diff(max3421_hcd->frame_number,
+					       max3421_ep->last_active)
+				    < urb->interval)
+					/*
+					 * We already processed this
+					 * end-point in the current
+					 * frame
+					 */
+					continue;
+				break;
+			}
+
+			/* move current ep to tail: */
+			list_move_tail(pos, &max3421_hcd->ep_list);
+			curr_urb = urb;
+			goto done;
+		}
+done:
+	if (!curr_urb) {
+		spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+		return 0;
+	}
+
+	urb = max3421_hcd->curr_urb = curr_urb;
+	epnum = usb_endpoint_num(&urb->ep->desc);
+	if (max3421_ep->retransmit)
+		/* restart (part of) a USB transaction: */
+		max3421_ep->retransmit = 0;
+	else {
+		/* start USB transaction: */
+		if (usb_endpoint_xfer_control(&ep->desc)) {
+			/*
+			 * See USB 2.0 spec section 8.6.1
+			 * Initialization via SETUP Token:
+			 */
+			usb_settoggle(urb->dev, epnum, 0, 1);
+			usb_settoggle(urb->dev, epnum, 1, 1);
+			max3421_ep->pkt_state = PKT_STATE_SETUP;
+			force_toggles = 1;
+		} else
+			max3421_ep->pkt_state = PKT_STATE_TRANSFER;
+	}
+
+	spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+
+	max3421_ep->last_active = max3421_hcd->frame_number;
+	max3421_set_address(hcd, urb->dev, epnum, force_toggles);
+	max3421_set_speed(hcd, urb->dev);
+	max3421_next_transfer(hcd, 0);
+	return 1;
+}
+
+/*
+ * Check all endpoints for URBs that got unlinked.
+ *
+ * Caller must NOT hold HCD spinlock.
+ */
+static int
+max3421_check_unlink(struct usb_hcd *hcd)
+{
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct list_head *pos, *upos, *next_upos;
+	struct max3421_ep *max3421_ep;
+	struct usb_host_endpoint *ep;
+	struct urb *urb;
+	unsigned long flags;
+	int retval = 0;
+
+	spin_lock_irqsave(&max3421_hcd->lock, flags);
+	list_for_each(pos, &max3421_hcd->ep_list) {
+		max3421_ep = container_of(pos, struct max3421_ep, ep_list);
+		ep = max3421_ep->ep;
+		list_for_each_safe(upos, next_upos, &ep->urb_list) {
+			urb = container_of(upos, struct urb, urb_list);
+			if (urb->unlinked) {
+				retval = 1;
+				dev_dbg(&spi->dev, "%s: URB %p unlinked=%d",
+					__func__, urb, urb->unlinked);
+				usb_hcd_unlink_urb_from_ep(hcd, urb);
+				spin_unlock_irqrestore(&max3421_hcd->lock,
+						       flags);
+				usb_hcd_giveback_urb(hcd, urb, 0);
+				spin_lock_irqsave(&max3421_hcd->lock, flags);
+			}
+		}
+	}
+	spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+	return retval;
+}
+
+/*
+ * Caller must NOT hold HCD spinlock.
+ */
+static void
+max3421_slow_retransmit(struct usb_hcd *hcd)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct urb *urb = max3421_hcd->curr_urb;
+	struct max3421_ep *max3421_ep;
+
+	max3421_ep = urb->ep->hcpriv;
+	max3421_ep->retransmit = 1;
+	max3421_hcd->curr_urb = NULL;
+}
+
+/*
+ * Caller must NOT hold HCD spinlock.
+ */
+static void
+max3421_recv_data_available(struct usb_hcd *hcd)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct urb *urb = max3421_hcd->curr_urb;
+	size_t remaining, transfer_size;
+	u8 rcvbc;
+
+	rcvbc = spi_rd8(hcd, MAX3421_REG_RCVBC);
+
+	if (rcvbc > MAX3421_FIFO_SIZE)
+		rcvbc = MAX3421_FIFO_SIZE;
+	if (urb->actual_length >= urb->transfer_buffer_length)
+		remaining = 0;
+	else
+		remaining = urb->transfer_buffer_length - urb->actual_length;
+	transfer_size = rcvbc;
+	if (transfer_size > remaining)
+		transfer_size = remaining;
+	if (transfer_size > 0) {
+		void *dst = urb->transfer_buffer + urb->actual_length;
+
+		spi_rd_buf(hcd, MAX3421_REG_RCVFIFO, dst, transfer_size);
+		urb->actual_length += transfer_size;
+		max3421_hcd->curr_len = transfer_size;
+	}
+
+	/* ack the RCVDAV irq now that the FIFO has been read: */
+	spi_wr8(hcd, MAX3421_REG_HIRQ, BIT(MAX3421_HI_RCVDAV_BIT));
+}
+
+static void
+max3421_handle_error(struct usb_hcd *hcd, u8 hrsl)
+{
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	u8 result_code = hrsl & MAX3421_HRSL_RESULT_MASK;
+	struct urb *urb = max3421_hcd->curr_urb;
+	struct max3421_ep *max3421_ep = urb->ep->hcpriv;
+	int switch_sndfifo;
+
+	/*
+	 * If an OUT command results in any response other than OK
+	 * (i.e., error or NAK), we have to perform a dummy-write to
+	 * SNDBC so the FIFO gets switched back to us.  Otherwise, we
+	 * get out of sync with the SNDFIFO double buffer.
+	 */
+	switch_sndfifo = (max3421_ep->pkt_state == PKT_STATE_TRANSFER &&
+			  usb_urb_dir_out(urb));
+
+	switch (result_code) {
+	case MAX3421_HRSL_OK:
+		return;			/* this shouldn't happen */
+
+	case MAX3421_HRSL_WRONGPID:	/* received wrong PID */
+	case MAX3421_HRSL_BUSY:		/* SIE busy */
+	case MAX3421_HRSL_BADREQ:	/* bad val in HXFR */
+	case MAX3421_HRSL_UNDEF:	/* reserved */
+	case MAX3421_HRSL_KERR:		/* K-state instead of response */
+	case MAX3421_HRSL_JERR:		/* J-state instead of response */
+		/*
+		 * packet experienced an error that we cannot recover
+		 * from; report error
+		 */
+		max3421_hcd->urb_done = hrsl_to_error[result_code];
+		dev_dbg(&spi->dev, "%s: unexpected error HRSL=0x%02x",
+			__func__, hrsl);
+		break;
+
+	case MAX3421_HRSL_TOGERR:
+		if (usb_urb_dir_in(urb))
+			; /* don't do anything (device will switch toggle) */
+		else {
+			/* flip the send toggle bit: */
+			int sndtog = (hrsl >> MAX3421_HRSL_SNDTOGRD_BIT) & 1;
+
+			sndtog ^= 1;
+			spi_wr8(hcd, MAX3421_REG_HCTL,
+				BIT(sndtog + MAX3421_HCTL_SNDTOG0_BIT));
+		}
+		/* FALL THROUGH */
+	case MAX3421_HRSL_BADBC:	/* bad byte count */
+	case MAX3421_HRSL_PIDERR:	/* received PID is corrupted */
+	case MAX3421_HRSL_PKTERR:	/* packet error (stuff, EOP) */
+	case MAX3421_HRSL_CRCERR:	/* CRC error */
+	case MAX3421_HRSL_BABBLE:	/* device talked too long */
+	case MAX3421_HRSL_TIMEOUT:
+		if (max3421_ep->retries++ < USB_MAX_RETRIES)
+			/* retry the packet again in the next frame */
+			max3421_slow_retransmit(hcd);
+		else {
+			/* Based on ohci.h cc_to_err[]: */
+			max3421_hcd->urb_done = hrsl_to_error[result_code];
+			dev_dbg(&spi->dev, "%s: unexpected error HRSL=0x%02x",
+				__func__, hrsl);
+		}
+		break;
+
+	case MAX3421_HRSL_STALL:
+		dev_dbg(&spi->dev, "%s: unexpected error HRSL=0x%02x",
+			__func__, hrsl);
+		max3421_hcd->urb_done = hrsl_to_error[result_code];
+		break;
+
+	case MAX3421_HRSL_NAK:
+		/*
+		 * Device wasn't ready for data or has no data
+		 * available: retry the packet again.
+		 */
+		if (max3421_ep->naks++ < NAK_MAX_FAST_RETRANSMITS) {
+			max3421_next_transfer(hcd, 1);
+			switch_sndfifo = 0;
+		} else
+			max3421_slow_retransmit(hcd);
+		break;
+	}
+	if (switch_sndfifo)
+		spi_wr8(hcd, MAX3421_REG_SNDBC, 0);
+}
+
+/*
+ * Caller must NOT hold HCD spinlock.
+ */
+static int
+max3421_transfer_in_done(struct usb_hcd *hcd, struct urb *urb)
+{
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	u32 max_packet;
+
+	if (urb->actual_length >= urb->transfer_buffer_length)
+		return 1;	/* read is complete, so we're done */
+
+	/*
+	 * USB 2.0 Section 5.3.2 Pipes: packets must be full size
+	 * except for last one.
+	 */
+	max_packet = usb_maxpacket(urb->dev, urb->pipe, 0);
+	if (max_packet > MAX3421_FIFO_SIZE) {
+		/*
+		 * We do not support isochronous transfers at this
+		 * time...
+		 */
+		dev_err(&spi->dev,
+			"%s: packet-size of %u too big (limit is %u bytes)",
+			__func__, max_packet, MAX3421_FIFO_SIZE);
+		return -EINVAL;
+	}
+
+	if (max3421_hcd->curr_len < max_packet) {
+		if (urb->transfer_flags & URB_SHORT_NOT_OK) {
+			/*
+			 * remaining > 0 and received an
+			 * unexpected partial packet ->
+			 * error
+			 */
+			return -EREMOTEIO;
+		} else
+			/* short read, but it's OK */
+			return 1;
+	}
+	return 0;	/* not done */
+}
+
+/*
+ * Caller must NOT hold HCD spinlock.
+ */
+static int
+max3421_transfer_out_done(struct usb_hcd *hcd, struct urb *urb)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+
+	urb->actual_length += max3421_hcd->curr_len;
+	if (urb->actual_length < urb->transfer_buffer_length)
+		return 0;
+	if (urb->transfer_flags & URB_ZERO_PACKET) {
+		/*
+		 * Some hardware needs a zero-size packet at the end
+		 * of a bulk-out transfer if the last transfer was a
+		 * full-sized packet (i.e., such hardware use <
+		 * max_packet as an indicator that the end of the
+		 * packet has been reached).
+		 */
+		u32 max_packet = usb_maxpacket(urb->dev, urb->pipe, 1);
+
+		if (max3421_hcd->curr_len == max_packet)
+			return 0;
+	}
+	return 1;
+}
+
+/*
+ * Caller must NOT hold HCD spinlock.
+ */
+static void
+max3421_host_transfer_done(struct usb_hcd *hcd)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct urb *urb = max3421_hcd->curr_urb;
+	struct max3421_ep *max3421_ep;
+	u8 result_code, hrsl;
+	int urb_done = 0;
+
+	max3421_hcd->hien &= ~(BIT(MAX3421_HI_HXFRDN_BIT) |
+			       BIT(MAX3421_HI_RCVDAV_BIT));
+
+	hrsl = spi_rd8(hcd, MAX3421_REG_HRSL);
+	result_code = hrsl & MAX3421_HRSL_RESULT_MASK;
+
+#ifdef DEBUG
+	++max3421_hcd->err_stat[result_code];
+#endif
+
+	max3421_ep = urb->ep->hcpriv;
+
+	if (unlikely(result_code != MAX3421_HRSL_OK)) {
+		max3421_handle_error(hcd, hrsl);
+		return;
+	}
+
+	max3421_ep->naks = 0;
+	max3421_ep->retries = 0;
+	switch (max3421_ep->pkt_state) {
+
+	case PKT_STATE_SETUP:
+		if (urb->transfer_buffer_length > 0)
+			max3421_ep->pkt_state = PKT_STATE_TRANSFER;
+		else
+			max3421_ep->pkt_state = PKT_STATE_TERMINATE;
+		break;
+
+	case PKT_STATE_TRANSFER:
+		if (usb_urb_dir_in(urb))
+			urb_done = max3421_transfer_in_done(hcd, urb);
+		else
+			urb_done = max3421_transfer_out_done(hcd, urb);
+		if (urb_done > 0 && usb_pipetype(urb->pipe) == PIPE_CONTROL) {
+			/*
+			 * We aren't really done - we still need to
+			 * terminate the control transfer:
+			 */
+			max3421_hcd->urb_done = urb_done = 0;
+			max3421_ep->pkt_state = PKT_STATE_TERMINATE;
+		}
+		break;
+
+	case PKT_STATE_TERMINATE:
+		urb_done = 1;
+		break;
+	}
+
+	if (urb_done)
+		max3421_hcd->urb_done = urb_done;
+	else
+		max3421_next_transfer(hcd, 0);
+}
+
+/*
+ * Caller must NOT hold HCD spinlock.
+ */
+static void
+max3421_detect_conn(struct usb_hcd *hcd)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	unsigned int jk, have_conn = 0;
+	u32 old_port_status, chg;
+	unsigned long flags;
+	u8 hrsl, mode;
+
+	hrsl = spi_rd8(hcd, MAX3421_REG_HRSL);
+
+	jk = ((((hrsl >> MAX3421_HRSL_JSTATUS_BIT) & 1) << 0) |
+	      (((hrsl >> MAX3421_HRSL_KSTATUS_BIT) & 1) << 1));
+
+	mode = max3421_hcd->mode;
+
+	switch (jk) {
+	case 0x0: /* SE0: disconnect */
+		/*
+		 * Turn off SOFKAENAB bit to avoid getting interrupt
+		 * every milli-second:
+		 */
+		mode &= ~BIT(MAX3421_MODE_SOFKAENAB_BIT);
+		break;
+
+	case 0x1: /* J=0,K=1: low-speed (in full-speed or vice versa) */
+	case 0x2: /* J=1,K=0: full-speed (in full-speed or vice versa) */
+		if (jk == 0x2)
+			/* need to switch to the other speed: */
+			mode ^= BIT(MAX3421_MODE_LOWSPEED_BIT);
+		/* turn on SOFKAENAB bit: */
+		mode |= BIT(MAX3421_MODE_SOFKAENAB_BIT);
+		have_conn = 1;
+		break;
+
+	case 0x3: /* illegal */
+		break;
+	}
+
+	max3421_hcd->mode = mode;
+	spi_wr8(hcd, MAX3421_REG_MODE, max3421_hcd->mode);
+
+	spin_lock_irqsave(&max3421_hcd->lock, flags);
+	old_port_status = max3421_hcd->port_status;
+	if (have_conn)
+		max3421_hcd->port_status |=  USB_PORT_STAT_CONNECTION;
+	else
+		max3421_hcd->port_status &= ~USB_PORT_STAT_CONNECTION;
+	if (mode & BIT(MAX3421_MODE_LOWSPEED_BIT))
+		max3421_hcd->port_status |=  USB_PORT_STAT_LOW_SPEED;
+	else
+		max3421_hcd->port_status &= ~USB_PORT_STAT_LOW_SPEED;
+	chg = (old_port_status ^ max3421_hcd->port_status);
+	max3421_hcd->port_status |= chg << 16;
+	spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+}
+
+static irqreturn_t
+max3421_irq_handler(int irq, void *dev_id)
+{
+	struct usb_hcd *hcd = dev_id;
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+
+	if (max3421_hcd->spi_thread &&
+	    max3421_hcd->spi_thread->state != TASK_RUNNING)
+		wake_up_process(max3421_hcd->spi_thread);
+	if (!max3421_hcd->do_enable_irq) {
+		max3421_hcd->do_enable_irq = 1;
+		disable_irq_nosync(spi->irq);
+	}
+	return IRQ_HANDLED;
+}
+
+#ifdef DEBUG
+
+static void
+dump_eps(struct usb_hcd *hcd)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct max3421_ep *max3421_ep;
+	struct usb_host_endpoint *ep;
+	struct list_head *pos, *upos;
+	char ubuf[512], *dp, *end;
+	unsigned long flags;
+	struct urb *urb;
+	int epnum, ret;
+
+	spin_lock_irqsave(&max3421_hcd->lock, flags);
+	list_for_each(pos, &max3421_hcd->ep_list) {
+		max3421_ep = container_of(pos, struct max3421_ep, ep_list);
+		ep = max3421_ep->ep;
+
+		dp = ubuf;
+		end = dp + sizeof(ubuf);
+		*dp = '\0';
+		list_for_each(upos, &ep->urb_list) {
+			urb = container_of(upos, struct urb, urb_list);
+			ret = snprintf(dp, end - dp, " %p(%d.%s %d/%d)", urb,
+				       usb_pipetype(urb->pipe),
+				       usb_urb_dir_in(urb) ? "IN" : "OUT",
+				       urb->actual_length,
+				       urb->transfer_buffer_length);
+			if (ret < 0 || ret >= end - dp)
+				break;	/* error or buffer full */
+			dp += ret;
+		}
+
+		epnum = usb_endpoint_num(&ep->desc);
+		pr_info("EP%0u %u lst %04u rtr %u nak %6u rxmt %u: %s\n",
+			epnum, max3421_ep->pkt_state, max3421_ep->last_active,
+			max3421_ep->retries, max3421_ep->naks,
+			max3421_ep->retransmit, ubuf);
+	}
+	spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+}
+
+#endif /* DEBUG */
+
+/* Return zero if no work was performed, 1 otherwise.  */
+static int
+max3421_handle_irqs(struct usb_hcd *hcd)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	u32 chg, old_port_status;
+	unsigned long flags;
+	u8 hirq;
+
+	/*
+	 * Read and ack pending interrupts (CPU must never
+	 * clear SNDBAV directly and RCVDAV must be cleared by
+	 * max3421_recv_data_available()!):
+	 */
+	hirq = spi_rd8(hcd, MAX3421_REG_HIRQ);
+	hirq &= max3421_hcd->hien;
+	if (!hirq)
+		return 0;
+
+	spi_wr8(hcd, MAX3421_REG_HIRQ,
+		hirq & ~(BIT(MAX3421_HI_SNDBAV_BIT) |
+			 BIT(MAX3421_HI_RCVDAV_BIT)));
+
+	if (hirq & BIT(MAX3421_HI_FRAME_BIT)) {
+		max3421_hcd->frame_number = ((max3421_hcd->frame_number + 1)
+					     & USB_MAX_FRAME_NUMBER);
+		max3421_hcd->sched_pass = SCHED_PASS_PERIODIC;
+	}
+
+	if (hirq & BIT(MAX3421_HI_RCVDAV_BIT))
+		max3421_recv_data_available(hcd);
+
+	if (hirq & BIT(MAX3421_HI_HXFRDN_BIT))
+		max3421_host_transfer_done(hcd);
+
+	if (hirq & BIT(MAX3421_HI_CONDET_BIT))
+		max3421_detect_conn(hcd);
+
+	/*
+	 * Now process interrupts that may affect HCD state
+	 * other than the end-points:
+	 */
+	spin_lock_irqsave(&max3421_hcd->lock, flags);
+
+	old_port_status = max3421_hcd->port_status;
+	if (hirq & BIT(MAX3421_HI_BUSEVENT_BIT)) {
+		if (max3421_hcd->port_status & USB_PORT_STAT_RESET) {
+			/* BUSEVENT due to completion of Bus Reset */
+			max3421_hcd->port_status &= ~USB_PORT_STAT_RESET;
+			max3421_hcd->port_status |=  USB_PORT_STAT_ENABLE;
+		} else {
+			/* BUSEVENT due to completion of Bus Resume */
+			pr_info("%s: BUSEVENT Bus Resume Done\n", __func__);
+		}
+	}
+	if (hirq & BIT(MAX3421_HI_RWU_BIT))
+		pr_info("%s: RWU\n", __func__);
+	if (hirq & BIT(MAX3421_HI_SUSDN_BIT))
+		pr_info("%s: SUSDN\n", __func__);
+
+	chg = (old_port_status ^ max3421_hcd->port_status);
+	max3421_hcd->port_status |= chg << 16;
+
+	spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+
+#ifdef DEBUG
+	{
+		static unsigned long last_time;
+		char sbuf[16 * 16], *dp, *end;
+		int i;
+
+		if (jiffies - last_time > 5*HZ) {
+			dp = sbuf;
+			end = sbuf + sizeof(sbuf);
+			*dp = '\0';
+			for (i = 0; i < 16; ++i) {
+				int ret = snprintf(dp, end - dp, " %lu",
+						   max3421_hcd->err_stat[i]);
+				if (ret < 0 || ret >= end - dp)
+					break;	/* error or buffer full */
+				dp += ret;
+			}
+			pr_info("%s: hrsl_stats %s\n", __func__, sbuf);
+			memset(max3421_hcd->err_stat, 0,
+			       sizeof(max3421_hcd->err_stat));
+			last_time = jiffies;
+
+			dump_eps(hcd);
+		}
+	}
+#endif
+	return 1;
+}
+
+static int
+max3421_reset_hcd(struct usb_hcd *hcd)
+{
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	int timeout;
+
+	/* perform a chip reset and wait for OSCIRQ signal to appear: */
+	spi_wr8(hcd, MAX3421_REG_USBCTL, BIT(MAX3421_USBCTL_CHIPRES_BIT));
+	/* clear reset: */
+	spi_wr8(hcd, MAX3421_REG_USBCTL, 0);
+	timeout = 1000;
+	while (1) {
+		if (spi_rd8(hcd, MAX3421_REG_USBIRQ)
+		    & BIT(MAX3421_USBIRQ_OSCOKIRQ_BIT))
+			break;
+		if (--timeout < 0) {
+			dev_err(&spi->dev,
+				"timed out waiting for oscillator OK signal");
+			return 1;
+		}
+		cond_resched();
+	}
+
+	/*
+	 * Turn on host mode, automatic generation of SOF packets, and
+	 * enable pull-down registers on DM/DP:
+	 */
+	max3421_hcd->mode = (BIT(MAX3421_MODE_HOST_BIT) |
+			     BIT(MAX3421_MODE_SOFKAENAB_BIT) |
+			     BIT(MAX3421_MODE_DMPULLDN_BIT) |
+			     BIT(MAX3421_MODE_DPPULLDN_BIT));
+	spi_wr8(hcd, MAX3421_REG_MODE, max3421_hcd->mode);
+
+	/* reset frame-number: */
+	max3421_hcd->frame_number = USB_MAX_FRAME_NUMBER;
+	spi_wr8(hcd, MAX3421_REG_HCTL, BIT(MAX3421_HCTL_FRMRST_BIT));
+
+	/* sample the state of the D+ and D- lines */
+	spi_wr8(hcd, MAX3421_REG_HCTL, BIT(MAX3421_HCTL_SAMPLEBUS_BIT));
+	max3421_detect_conn(hcd);
+
+	/* enable frame, connection-detected, and bus-event interrupts: */
+	max3421_hcd->hien = (BIT(MAX3421_HI_FRAME_BIT) |
+			     BIT(MAX3421_HI_CONDET_BIT) |
+			     BIT(MAX3421_HI_BUSEVENT_BIT));
+	spi_wr8(hcd, MAX3421_REG_HIEN, max3421_hcd->hien);
+
+	/* enable interrupts: */
+	spi_wr8(hcd, MAX3421_REG_CPUCTL, BIT(MAX3421_CPUCTL_IE_BIT));
+	return 1;
+}
+
+static int
+max3421_urb_done(struct usb_hcd *hcd)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	unsigned long flags;
+	struct urb *urb;
+	int status;
+
+	status = max3421_hcd->urb_done;
+	max3421_hcd->urb_done = 0;
+	if (status > 0)
+		status = 0;
+	urb = max3421_hcd->curr_urb;
+	if (urb) {
+		max3421_hcd->curr_urb = NULL;
+		spin_lock_irqsave(&max3421_hcd->lock, flags);
+		usb_hcd_unlink_urb_from_ep(hcd, urb);
+		spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+
+		/* must be called without the HCD spinlock: */
+		usb_hcd_giveback_urb(hcd, urb, status);
+	}
+	return 1;
+}
+
+static int
+max3421_spi_thread(void *dev_id)
+{
+	struct usb_hcd *hcd = dev_id;
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	int i, i_worked = 1;
+
+	/* set full-duplex SPI mode, low-active interrupt pin: */
+	spi_wr8(hcd, MAX3421_REG_PINCTL,
+		(BIT(MAX3421_PINCTL_FDUPSPI_BIT) |	/* full-duplex */
+		 BIT(MAX3421_PINCTL_INTLEVEL_BIT)));	/* low-active irq */
+
+	while (!kthread_should_stop()) {
+		max3421_hcd->rev = spi_rd8(hcd, MAX3421_REG_REVISION);
+		if (max3421_hcd->rev == 0x12 || max3421_hcd->rev == 0x13)
+			break;
+		dev_err(&spi->dev, "bad rev 0x%02x", max3421_hcd->rev);
+		msleep(10000);
+	}
+	dev_info(&spi->dev, "rev 0x%x, SPI clk %dHz, bpw %u, irq %d\n",
+		 max3421_hcd->rev, spi->max_speed_hz, spi->bits_per_word,
+		 spi->irq);
+
+	while (!kthread_should_stop()) {
+		if (!i_worked) {
+			/*
+			 * We'll be waiting for wakeups from the hard
+			 * interrupt handler, so now is a good time to
+			 * sync our hien with the chip:
+			 */
+			spi_wr8(hcd, MAX3421_REG_HIEN, max3421_hcd->hien);
+
+			set_current_state(TASK_INTERRUPTIBLE);
+			if (max3421_hcd->do_enable_irq) {
+				max3421_hcd->do_enable_irq = 0;
+				enable_irq(spi->irq);
+			}
+			schedule();
+			__set_current_state(TASK_RUNNING);
+		}
+
+		i_worked = 0;
+
+		if (max3421_hcd->urb_done)
+			i_worked |= max3421_urb_done(hcd);
+		else if (max3421_handle_irqs(hcd))
+			i_worked = 1;
+		else if (!max3421_hcd->curr_urb)
+			i_worked |= max3421_select_and_start_urb(hcd);
+
+		if (max3421_hcd->do_reset_hcd) {
+			/* reset the HCD: */
+			max3421_hcd->do_reset_hcd = 0;
+			i_worked |= max3421_reset_hcd(hcd);
+		}
+		if (max3421_hcd->do_reset_port) {
+			/* perform a USB bus reset: */
+			max3421_hcd->do_reset_port = 0;
+			spi_wr8(hcd, MAX3421_REG_HCTL,
+				BIT(MAX3421_HCTL_BUSRST_BIT));
+			i_worked = 1;
+		}
+		if (max3421_hcd->do_check_unlink) {
+			max3421_hcd->do_check_unlink = 0;
+			i_worked |= max3421_check_unlink(hcd);
+		}
+		if (max3421_hcd->do_iopin_update) {
+			/*
+			 * IOPINS1/IOPINS2 do not auto-increment, so we can't
+			 * use spi_wr_buf().
+			 */
+			for (i = 0; i < ARRAY_SIZE(max3421_hcd->iopins); ++i) {
+				u8 val = spi_rd8(hcd, MAX3421_REG_IOPINS1);
+
+				val = ((val & 0xf0) |
+				       (max3421_hcd->iopins[i] & 0x0f));
+				spi_wr8(hcd, MAX3421_REG_IOPINS1 + i, val);
+				max3421_hcd->iopins[i] = val;
+			}
+			max3421_hcd->do_iopin_update = 0;
+			i_worked = 1;
+		}
+	}
+	set_current_state(TASK_RUNNING);
+	dev_info(&spi->dev, "SPI thread exiting");
+	return 0;
+}
+
+static int
+max3421_reset_port(struct usb_hcd *hcd)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+
+	max3421_hcd->port_status &= ~(USB_PORT_STAT_ENABLE |
+				      USB_PORT_STAT_LOW_SPEED);
+	max3421_hcd->do_reset_port = 1;
+	wake_up_process(max3421_hcd->spi_thread);
+	return 0;
+}
+
+static int
+max3421_reset(struct usb_hcd *hcd)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+
+	hcd->self.sg_tablesize = 0;
+	hcd->speed = HCD_USB2;
+	hcd->self.root_hub->speed = USB_SPEED_FULL;
+	max3421_hcd->do_reset_hcd = 1;
+	wake_up_process(max3421_hcd->spi_thread);
+	return 0;
+}
+
+static int
+max3421_start(struct usb_hcd *hcd)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+
+	spin_lock_init(&max3421_hcd->lock);
+	max3421_hcd->rh_state = MAX3421_RH_RUNNING;
+
+	INIT_LIST_HEAD(&max3421_hcd->ep_list);
+
+	hcd->power_budget = POWER_BUDGET;
+	hcd->state = HC_STATE_RUNNING;
+	hcd->uses_new_polling = 1;
+	return 0;
+}
+
+static void
+max3421_stop(struct usb_hcd *hcd)
+{
+}
+
+static int
+max3421_urb_enqueue(struct usb_hcd *hcd, struct urb *urb, gfp_t mem_flags)
+{
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct max3421_ep *max3421_ep;
+	unsigned long flags;
+	int retval;
+
+	switch (usb_pipetype(urb->pipe)) {
+	case PIPE_INTERRUPT:
+	case PIPE_ISOCHRONOUS:
+		if (urb->interval < 0) {
+			dev_err(&spi->dev,
+			  "%s: interval=%d for intr-/iso-pipe; expected > 0\n",
+				__func__, urb->interval);
+			return -EINVAL;
+		}
+	default:
+		break;
+	}
+
+	spin_lock_irqsave(&max3421_hcd->lock, flags);
+
+	max3421_ep = urb->ep->hcpriv;
+	if (!max3421_ep) {
+		/* gets freed in max3421_endpoint_disable: */
+		max3421_ep = kzalloc(sizeof(struct max3421_ep), mem_flags);
+		if (!max3421_ep) {
+			retval = -ENOMEM;
+			goto out;
+		}
+		max3421_ep->ep = urb->ep;
+		max3421_ep->last_active = max3421_hcd->frame_number;
+		urb->ep->hcpriv = max3421_ep;
+
+		list_add_tail(&max3421_ep->ep_list, &max3421_hcd->ep_list);
+	}
+
+	retval = usb_hcd_link_urb_to_ep(hcd, urb);
+	if (retval == 0) {
+		/* Since we added to the queue, restart scheduling: */
+		max3421_hcd->sched_pass = SCHED_PASS_PERIODIC;
+		wake_up_process(max3421_hcd->spi_thread);
+	}
+
+out:
+	spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+	return retval;
+}
+
+static int
+max3421_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	unsigned long flags;
+	int retval;
+
+	spin_lock_irqsave(&max3421_hcd->lock, flags);
+
+	/*
+	 * This will set urb->unlinked which in turn causes the entry
+	 * to be dropped at the next opportunity.
+	 */
+	retval = usb_hcd_check_unlink_urb(hcd, urb, status);
+	if (retval == 0) {
+		max3421_hcd->do_check_unlink = 1;
+		wake_up_process(max3421_hcd->spi_thread);
+	}
+	spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+	return retval;
+}
+
+static void
+max3421_endpoint_disable(struct usb_hcd *hcd, struct usb_host_endpoint *ep)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	unsigned long flags;
+
+	spin_lock_irqsave(&max3421_hcd->lock, flags);
+
+	if (ep->hcpriv) {
+		struct max3421_ep *max3421_ep = ep->hcpriv;
+
+		/* remove myself from the ep_list: */
+		if (!list_empty(&max3421_ep->ep_list))
+			list_del(&max3421_ep->ep_list);
+		kfree(max3421_ep);
+		ep->hcpriv = NULL;
+	}
+
+	spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+}
+
+static int
+max3421_get_frame_number(struct usb_hcd *hcd)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	return max3421_hcd->frame_number;
+}
+
+/*
+ * Should return a non-zero value when any port is undergoing a resume
+ * transition while the root hub is suspended.
+ */
+static int
+max3421_hub_status_data(struct usb_hcd *hcd, char *buf)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	unsigned long flags;
+	int retval = 0;
+
+	spin_lock_irqsave(&max3421_hcd->lock, flags);
+	if (!HCD_HW_ACCESSIBLE(hcd))
+		goto done;
+
+	*buf = 0;
+	if ((max3421_hcd->port_status & PORT_C_MASK) != 0) {
+		*buf = (1 << 1); /* a hub over-current condition exists */
+		dev_dbg(hcd->self.controller,
+			"port status 0x%08x has changes\n",
+			max3421_hcd->port_status);
+		retval = 1;
+		if (max3421_hcd->rh_state == MAX3421_RH_SUSPENDED)
+			usb_hcd_resume_root_hub(hcd);
+	}
+done:
+	spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+	return retval;
+}
+
+static inline void
+hub_descriptor(struct usb_hub_descriptor *desc)
+{
+	memset(desc, 0, sizeof(*desc));
+	/*
+	 * See Table 11-13: Hub Descriptor in USB 2.0 spec.
+	 */
+	desc->bDescriptorType = 0x29;	/* hub descriptor */
+	desc->bDescLength = 9;
+	desc->wHubCharacteristics = cpu_to_le16(0x0001);
+	desc->bNbrPorts = 1;
+}
+
+/*
+ * Set the MAX3421E general-purpose output with number PIN_NUMBER to
+ * VALUE (0 or 1).  PIN_NUMBER may be in the range from 1-8.  For
+ * any other value, this function acts as a no-op.
+ */
+static void
+max3421_gpout_set_value(struct usb_hcd *hcd, u8 pin_number, u8 value)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	u8 mask, idx;
+
+	--pin_number;
+	if (pin_number > 7)
+		return;
+
+	mask = 1u << pin_number;
+	idx = pin_number / 4;
+
+	if (value)
+		max3421_hcd->iopins[idx] |=  mask;
+	else
+		max3421_hcd->iopins[idx] &= ~mask;
+	max3421_hcd->do_iopin_update = 1;
+	wake_up_process(max3421_hcd->spi_thread);
+}
+
+static int
+max3421_hub_control(struct usb_hcd *hcd, u16 type_req, u16 value, u16 index,
+		    char *buf, u16 length)
+{
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct max3421_hcd_platform_data *pdata;
+	unsigned long flags;
+	int retval = 0;
+
+	spin_lock_irqsave(&max3421_hcd->lock, flags);
+
+	pdata = spi->dev.platform_data;
+
+	switch (type_req) {
+	case ClearHubFeature:
+		break;
+	case ClearPortFeature:
+		switch (value) {
+		case USB_PORT_FEAT_SUSPEND:
+			break;
+		case USB_PORT_FEAT_POWER:
+			dev_dbg(hcd->self.controller, "power-off\n");
+			max3421_gpout_set_value(hcd, pdata->vbus_gpout,
+						!pdata->vbus_active_level);
+			/* FALLS THROUGH */
+		default:
+			max3421_hcd->port_status &= ~(1 << value);
+		}
+		break;
+	case GetHubDescriptor:
+		hub_descriptor((struct usb_hub_descriptor *) buf);
+		break;
+
+	case DeviceRequest | USB_REQ_GET_DESCRIPTOR:
+	case GetPortErrorCount:
+	case SetHubDepth:
+		/* USB3 only */
+		goto error;
+
+	case GetHubStatus:
+		*(__le32 *) buf = cpu_to_le32(0);
+		break;
+
+	case GetPortStatus:
+		if (index != 1) {
+			retval = -EPIPE;
+			goto error;
+		}
+		((__le16 *) buf)[0] = cpu_to_le16(max3421_hcd->port_status);
+		((__le16 *) buf)[1] =
+			cpu_to_le16(max3421_hcd->port_status >> 16);
+		break;
+
+	case SetHubFeature:
+		retval = -EPIPE;
+		break;
+
+	case SetPortFeature:
+		switch (value) {
+		case USB_PORT_FEAT_LINK_STATE:
+		case USB_PORT_FEAT_U1_TIMEOUT:
+		case USB_PORT_FEAT_U2_TIMEOUT:
+		case USB_PORT_FEAT_BH_PORT_RESET:
+			goto error;
+		case USB_PORT_FEAT_SUSPEND:
+			if (max3421_hcd->active)
+				max3421_hcd->port_status |=
+					USB_PORT_STAT_SUSPEND;
+			break;
+		case USB_PORT_FEAT_POWER:
+			dev_dbg(hcd->self.controller, "power-on\n");
+			max3421_hcd->port_status |= USB_PORT_STAT_POWER;
+			max3421_gpout_set_value(hcd, pdata->vbus_gpout,
+						pdata->vbus_active_level);
+			break;
+		case USB_PORT_FEAT_RESET:
+			max3421_reset_port(hcd);
+			/* FALLS THROUGH */
+		default:
+			if ((max3421_hcd->port_status & USB_PORT_STAT_POWER)
+			    != 0)
+				max3421_hcd->port_status |= (1 << value);
+		}
+		break;
+
+	default:
+		dev_dbg(hcd->self.controller,
+			"hub control req%04x v%04x i%04x l%d\n",
+			type_req, value, index, length);
+error:		/* "protocol stall" on error */
+		retval = -EPIPE;
+	}
+
+	spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+	return retval;
+}
+
+static int
+max3421_bus_suspend(struct usb_hcd *hcd)
+{
+	return -1;
+}
+
+static int
+max3421_bus_resume(struct usb_hcd *hcd)
+{
+	return -1;
+}
+
+/*
+ * The SPI driver already takes care of DMA-mapping/unmapping, so no
+ * reason to do it twice.
+ */
+static int
+max3421_map_urb_for_dma(struct usb_hcd *hcd, struct urb *urb, gfp_t mem_flags)
+{
+	return 0;
+}
+
+static void
+max3421_unmap_urb_for_dma(struct usb_hcd *hcd, struct urb *urb)
+{
+}
+
+static struct hc_driver max3421_hcd_desc = {
+	.description =		"max3421",
+	.product_desc =		DRIVER_DESC,
+	.hcd_priv_size =	sizeof(struct max3421_hcd),
+	.flags =		HCD_USB11,
+	.reset =		max3421_reset,
+	.start =		max3421_start,
+	.stop =			max3421_stop,
+	.get_frame_number =	max3421_get_frame_number,
+	.urb_enqueue =		max3421_urb_enqueue,
+	.urb_dequeue =		max3421_urb_dequeue,
+	.map_urb_for_dma =	max3421_map_urb_for_dma,
+	.unmap_urb_for_dma =	max3421_unmap_urb_for_dma,
+	.endpoint_disable =	max3421_endpoint_disable,
+	.hub_status_data =	max3421_hub_status_data,
+	.hub_control =		max3421_hub_control,
+	.bus_suspend =		max3421_bus_suspend,
+	.bus_resume =		max3421_bus_resume,
+};
+
+static int
+max3421_probe(struct spi_device *spi)
+{
+	struct max3421_hcd *max3421_hcd;
+	struct usb_hcd *hcd = NULL;
+	int retval = -ENOMEM;
+
+	if (spi_setup(spi) < 0) {
+		dev_err(&spi->dev, "Unable to setup SPI bus");
+		return -EFAULT;
+	}
+
+	hcd = usb_create_hcd(&max3421_hcd_desc, &spi->dev,
+			     dev_name(&spi->dev));
+	if (!hcd) {
+		dev_err(&spi->dev, "failed to create HCD structure\n");
+		goto error;
+	}
+	set_bit(HCD_FLAG_POLL_RH, &hcd->flags);
+	max3421_hcd = hcd_to_max3421(hcd);
+	max3421_hcd->next = max3421_hcd_list;
+	max3421_hcd_list = max3421_hcd;
+	INIT_LIST_HEAD(&max3421_hcd->ep_list);
+
+	max3421_hcd->tx = kmalloc(sizeof(*max3421_hcd->tx), GFP_KERNEL);
+	if (!max3421_hcd->tx) {
+		dev_err(&spi->dev, "failed to kmalloc tx buffer\n");
+		goto error;
+	}
+	max3421_hcd->rx = kmalloc(sizeof(*max3421_hcd->rx), GFP_KERNEL);
+	if (!max3421_hcd->rx) {
+		dev_err(&spi->dev, "failed to kmalloc rx buffer\n");
+		goto error;
+	}
+
+	max3421_hcd->spi_thread = kthread_run(max3421_spi_thread, hcd,
+					      "max3421_spi_thread");
+	if (max3421_hcd->spi_thread == ERR_PTR(-ENOMEM)) {
+		dev_err(&spi->dev,
+			"failed to create SPI thread (out of memory)\n");
+		goto error;
+	}
+
+	retval = usb_add_hcd(hcd, 0, 0);
+	if (retval) {
+		dev_err(&spi->dev, "failed to add HCD\n");
+		goto error;
+	}
+
+	retval = request_irq(spi->irq, max3421_irq_handler,
+			     IRQF_TRIGGER_LOW, "max3421", hcd);
+	if (retval < 0) {
+		dev_err(&spi->dev, "failed to request irq %d\n", spi->irq);
+		goto error;
+	}
+	return 0;
+
+error:
+	if (hcd) {
+		kfree(max3421_hcd->tx);
+		kfree(max3421_hcd->rx);
+		if (max3421_hcd->spi_thread)
+			kthread_stop(max3421_hcd->spi_thread);
+		usb_put_hcd(hcd);
+	}
+	return retval;
+}
+
+static int
+max3421_remove(struct spi_device *spi)
+{
+	struct max3421_hcd *max3421_hcd = NULL, **prev;
+	struct usb_hcd *hcd = NULL;
+	unsigned long flags;
+
+	for (prev = &max3421_hcd_list; *prev; prev = &(*prev)->next) {
+		max3421_hcd = *prev;
+		hcd = max3421_to_hcd(max3421_hcd);
+		if (hcd->self.controller == &spi->dev)
+			break;
+	}
+	if (!max3421_hcd) {
+		dev_err(&spi->dev, "no MAX3421 HCD found for SPI device %p\n",
+			spi);
+		return -ENODEV;
+	}
+
+	usb_remove_hcd(hcd);
+
+	spin_lock_irqsave(&max3421_hcd->lock, flags);
+
+	kthread_stop(max3421_hcd->spi_thread);
+	*prev = max3421_hcd->next;
+
+	spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+
+	free_irq(spi->irq, hcd);
+
+	usb_put_hcd(hcd);
+	return 0;
+}
+
+static struct spi_driver max3421_driver = {
+	.probe		= max3421_probe,
+	.remove		= max3421_remove,
+	.driver		= {
+		.name	= "max3421-hcd",
+		.owner	= THIS_MODULE,
+	},
+};
+
+module_spi_driver(max3421_driver);
+
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_AUTHOR("David Mosberger <davidm@egauge.net>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/host/ohci-at91.c b/drivers/usb/host/ohci-at91.c
index 091ae4905cf..e49eb4f90f5 100644
--- a/drivers/usb/host/ohci-at91.c
+++ b/drivers/usb/host/ohci-at91.c
@@ -46,9 +46,6 @@ static const char hcd_name[] = "ohci-atmel";
 
 static struct hc_driver __read_mostly ohci_at91_hc_driver;
 static int clocked;
-static int (*orig_ohci_hub_control)(struct usb_hcd  *hcd, u16 typeReq,
-			u16 wValue, u16 wIndex, char *buf, u16 wLength);
-static int (*orig_ohci_hub_status_data)(struct usb_hcd *hcd, char *buf);
 
 extern int usb_disabled(void);
 
@@ -262,7 +259,7 @@ static int ohci_at91_usb_get_power(struct at91_usbh_data *pdata, int port)
 static int ohci_at91_hub_status_data(struct usb_hcd *hcd, char *buf)
 {
 	struct at91_usbh_data *pdata = hcd->self.controller->platform_data;
-	int length = orig_ohci_hub_status_data(hcd, buf);
+	int length = ohci_hub_status_data(hcd, buf);
 	int port;
 
 	at91_for_each_port(port) {
@@ -340,8 +337,7 @@ static int ohci_at91_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 		break;
 	}
 
-	ret = orig_ohci_hub_control(hcd, typeReq, wValue, wIndex + 1,
-				buf, wLength);
+	ret = ohci_hub_control(hcd, typeReq, wValue, wIndex + 1, buf, wLength);
 	if (ret)
 		goto out;
 
@@ -690,9 +686,6 @@ static int __init ohci_at91_init(void)
 	 * too easy.
 	 */
 
-	orig_ohci_hub_control = ohci_at91_hc_driver.hub_control;
-	orig_ohci_hub_status_data = ohci_at91_hc_driver.hub_status_data;
-
 	ohci_at91_hc_driver.hub_status_data	= ohci_at91_hub_status_data;
 	ohci_at91_hc_driver.hub_control		= ohci_at91_hub_control;
 
diff --git a/drivers/usb/host/ohci-exynos.c b/drivers/usb/host/ohci-exynos.c
index 68588d8a09b..060a6a41475 100644
--- a/drivers/usb/host/ohci-exynos.c
+++ b/drivers/usb/host/ohci-exynos.c
@@ -18,6 +18,7 @@
 #include <linux/module.h>
 #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>
@@ -33,28 +34,110 @@ static struct hc_driver __read_mostly exynos_ohci_hc_driver;
 
 #define to_exynos_ohci(hcd) (struct exynos_ohci_hcd *)(hcd_to_ohci(hcd)->priv)
 
+#define PHY_NUMBER 3
+
 struct exynos_ohci_hcd {
 	struct clk *clk;
 	struct usb_phy *phy;
 	struct usb_otg *otg;
+	struct phy *phy_g[PHY_NUMBER];
 };
 
-static void exynos_ohci_phy_enable(struct platform_device *pdev)
+static int exynos_ohci_get_phy(struct device *dev,
+				struct exynos_ohci_hcd *exynos_ohci)
 {
-	struct usb_hcd *hcd = platform_get_drvdata(pdev);
+	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;
+	}
+
+	/*
+	 * 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.
+	 */
+	for_each_available_child_of_node(dev->of_node, child) {
+		ret = of_property_read_u32(child, "reg", &phy_number);
+		if (ret) {
+			dev_err(dev, "Failed to parse device tree\n");
+			of_node_put(child);
+			return ret;
+		}
+
+		if (phy_number >= PHY_NUMBER) {
+			dev_err(dev, "Invalid number of PHYs\n");
+			of_node_put(child);
+			return -EINVAL;
+		}
+
+		phy = devm_of_phy_get(dev, child, 0);
+		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");
+				return ret;
+			}
+			dev_dbg(dev, "Failed to get usb2 phy\n");
+		}
+		exynos_ohci->phy_g[phy_number] = phy;
+	}
+
+	return ret;
+}
+
+static int exynos_ohci_phy_enable(struct device *dev)
+{
+	struct usb_hcd *hcd = dev_get_drvdata(dev);
 	struct exynos_ohci_hcd *exynos_ohci = to_exynos_ohci(hcd);
+	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 (ret)
+		for (i--; i >= 0; i--)
+			if (!IS_ERR(exynos_ohci->phy_g[i]))
+				phy_power_off(exynos_ohci->phy_g[i]);
 
-	if (exynos_ohci->phy)
-		usb_phy_init(exynos_ohci->phy);
+	return ret;
 }
 
-static void exynos_ohci_phy_disable(struct platform_device *pdev)
+static void exynos_ohci_phy_disable(struct device *dev)
 {
-	struct usb_hcd *hcd = platform_get_drvdata(pdev);
+	struct usb_hcd *hcd = dev_get_drvdata(dev);
 	struct exynos_ohci_hcd *exynos_ohci = to_exynos_ohci(hcd);
+	int i;
 
-	if (exynos_ohci->phy)
+	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]);
 }
 
 static int exynos_ohci_probe(struct platform_device *pdev)
@@ -62,7 +145,6 @@ static int exynos_ohci_probe(struct platform_device *pdev)
 	struct exynos_ohci_hcd *exynos_ohci;
 	struct usb_hcd *hcd;
 	struct resource *res;
-	struct usb_phy *phy;
 	int irq;
 	int err;
 
@@ -88,15 +170,9 @@ static int exynos_ohci_probe(struct platform_device *pdev)
 					"samsung,exynos5440-ohci"))
 		goto skip_phy;
 
-	phy = devm_usb_get_phy(&pdev->dev, USB_PHY_TYPE_USB2);
-	if (IS_ERR(phy)) {
-		usb_put_hcd(hcd);
-		dev_warn(&pdev->dev, "no platform data or transceiver defined\n");
-		return -EPROBE_DEFER;
-	} else {
-		exynos_ohci->phy = phy;
-		exynos_ohci->otg = phy->otg;
-	}
+	err = exynos_ohci_get_phy(&pdev->dev, exynos_ohci);
+	if (err)
+		goto fail_clk;
 
 skip_phy:
 	exynos_ohci->clk = devm_clk_get(&pdev->dev, "usbhost");
@@ -120,10 +196,9 @@ skip_phy:
 
 	hcd->rsrc_start = res->start;
 	hcd->rsrc_len = resource_size(res);
-	hcd->regs = devm_ioremap(&pdev->dev, res->start, hcd->rsrc_len);
-	if (!hcd->regs) {
-		dev_err(&pdev->dev, "Failed to remap I/O memory\n");
-		err = -ENOMEM;
+	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(hcd->regs)) {
+		err = PTR_ERR(hcd->regs);
 		goto fail_io;
 	}
 
@@ -139,7 +214,11 @@ skip_phy:
 
 	platform_set_drvdata(pdev, hcd);
 
-	exynos_ohci_phy_enable(pdev);
+	err = exynos_ohci_phy_enable(&pdev->dev);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to enable USB phy\n");
+		goto fail_io;
+	}
 
 	err = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (err) {
@@ -150,7 +229,7 @@ skip_phy:
 	return 0;
 
 fail_add_hcd:
-	exynos_ohci_phy_disable(pdev);
+	exynos_ohci_phy_disable(&pdev->dev);
 fail_io:
 	clk_disable_unprepare(exynos_ohci->clk);
 fail_clk:
@@ -168,7 +247,7 @@ static int exynos_ohci_remove(struct platform_device *pdev)
 	if (exynos_ohci->otg)
 		exynos_ohci->otg->set_host(exynos_ohci->otg, &hcd->self);
 
-	exynos_ohci_phy_disable(pdev);
+	exynos_ohci_phy_disable(&pdev->dev);
 
 	clk_disable_unprepare(exynos_ohci->clk);
 
@@ -190,26 +269,19 @@ static int exynos_ohci_suspend(struct device *dev)
 {
 	struct usb_hcd *hcd = dev_get_drvdata(dev);
 	struct exynos_ohci_hcd *exynos_ohci = to_exynos_ohci(hcd);
-	struct ohci_hcd *ohci = hcd_to_ohci(hcd);
-	struct platform_device *pdev = to_platform_device(dev);
 	bool do_wakeup = device_may_wakeup(dev);
-	unsigned long flags;
 	int rc = ohci_suspend(hcd, do_wakeup);
 
 	if (rc)
 		return rc;
 
-	spin_lock_irqsave(&ohci->lock, flags);
-
 	if (exynos_ohci->otg)
 		exynos_ohci->otg->set_host(exynos_ohci->otg, &hcd->self);
 
-	exynos_ohci_phy_disable(pdev);
+	exynos_ohci_phy_disable(dev);
 
 	clk_disable_unprepare(exynos_ohci->clk);
 
-	spin_unlock_irqrestore(&ohci->lock, flags);
-
 	return 0;
 }
 
@@ -217,14 +289,19 @@ static int exynos_ohci_resume(struct device *dev)
 {
 	struct usb_hcd *hcd			= dev_get_drvdata(dev);
 	struct exynos_ohci_hcd *exynos_ohci	= to_exynos_ohci(hcd);
-	struct platform_device *pdev		= to_platform_device(dev);
+	int ret;
 
 	clk_prepare_enable(exynos_ohci->clk);
 
 	if (exynos_ohci->otg)
 		exynos_ohci->otg->set_host(exynos_ohci->otg, &hcd->self);
 
-	exynos_ohci_phy_enable(pdev);
+	ret = exynos_ohci_phy_enable(dev);
+	if (ret) {
+		dev_err(dev, "Failed to enable USB phy\n");
+		clk_disable_unprepare(exynos_ohci->clk);
+		return ret;
+	}
 
 	ohci_resume(hcd, false);
 
diff --git a/drivers/usb/host/ohci-hcd.c b/drivers/usb/host/ohci-hcd.c
index 3586460fb2a..f98d03f3144 100644
--- a/drivers/usb/host/ohci-hcd.c
+++ b/drivers/usb/host/ohci-hcd.c
@@ -1178,7 +1178,7 @@ MODULE_LICENSE ("GPL");
 #define SA1111_DRIVER		ohci_hcd_sa1111_driver
 #endif
 
-#ifdef CONFIG_ARCH_DAVINCI_DA8XX
+#ifdef CONFIG_USB_OHCI_HCD_DAVINCI
 #include "ohci-da8xx.c"
 #define DAVINCI_PLATFORM_DRIVER	ohci_hcd_da8xx_driver
 #endif
diff --git a/drivers/usb/host/ohci-hub.c b/drivers/usb/host/ohci-hub.c
index cd871b89501..b4940de1eba 100644
--- a/drivers/usb/host/ohci-hub.c
+++ b/drivers/usb/host/ohci-hub.c
@@ -456,8 +456,7 @@ static int ohci_root_hub_state_changes(struct ohci_hcd *ohci, int changed,
 
 /* build "status change" packet (one or two bytes) from HC registers */
 
-static int
-ohci_hub_status_data (struct usb_hcd *hcd, char *buf)
+int ohci_hub_status_data(struct usb_hcd *hcd, char *buf)
 {
 	struct ohci_hcd	*ohci = hcd_to_ohci (hcd);
 	int		i, changed = 0, length = 1;
@@ -522,6 +521,7 @@ done:
 
 	return changed ? length : 0;
 }
+EXPORT_SYMBOL_GPL(ohci_hub_status_data);
 
 /*-------------------------------------------------------------------------*/
 
@@ -664,7 +664,7 @@ static inline int root_port_reset (struct ohci_hcd *ohci, unsigned port)
 	return 0;
 }
 
-static int ohci_hub_control (
+int ohci_hub_control(
 	struct usb_hcd	*hcd,
 	u16		typeReq,
 	u16		wValue,
@@ -790,4 +790,4 @@ error:
 	}
 	return retval;
 }
-
+EXPORT_SYMBOL_GPL(ohci_hub_control);
diff --git a/drivers/usb/host/ohci-platform.c b/drivers/usb/host/ohci-platform.c
index b6002c951c5..4369299064c 100644
--- a/drivers/usb/host/ohci-platform.c
+++ b/drivers/usb/host/ohci-platform.c
@@ -24,6 +24,7 @@
 #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>
@@ -36,6 +37,7 @@
 
 struct ohci_platform_priv {
 	struct clk *clks[OHCI_MAX_CLKS];
+	struct reset_control *rst;
 	struct phy *phy;
 };
 
@@ -191,6 +193,19 @@ static int ohci_platform_probe(struct platform_device *dev)
 				break;
 			}
 		}
+
+	}
+
+	priv->rst = devm_reset_control_get_optional(&dev->dev, NULL);
+	if (IS_ERR(priv->rst)) {
+		err = PTR_ERR(priv->rst);
+		if (err == -EPROBE_DEFER)
+			goto err_put_clks;
+		priv->rst = NULL;
+	} else {
+		err = reset_control_deassert(priv->rst);
+		if (err)
+			goto err_put_clks;
 	}
 
 	if (pdata->big_endian_desc)
@@ -203,7 +218,7 @@ static int ohci_platform_probe(struct platform_device *dev)
 		dev_err(&dev->dev,
 			"Error: CONFIG_USB_OHCI_BIG_ENDIAN_MMIO not set\n");
 		err = -EINVAL;
-		goto err_put_clks;
+		goto err_reset;
 	}
 #endif
 #ifndef CONFIG_USB_OHCI_BIG_ENDIAN_DESC
@@ -211,14 +226,14 @@ static int ohci_platform_probe(struct platform_device *dev)
 		dev_err(&dev->dev,
 			"Error: CONFIG_USB_OHCI_BIG_ENDIAN_DESC not set\n");
 		err = -EINVAL;
-		goto err_put_clks;
+		goto err_reset;
 	}
 #endif
 
 	if (pdata->power_on) {
 		err = pdata->power_on(dev);
 		if (err < 0)
-			goto err_put_clks;
+			goto err_reset;
 	}
 
 	hcd->rsrc_start = res_mem->start;
@@ -242,6 +257,9 @@ static int ohci_platform_probe(struct platform_device *dev)
 err_power:
 	if (pdata->power_off)
 		pdata->power_off(dev);
+err_reset:
+	if (priv->rst)
+		reset_control_assert(priv->rst);
 err_put_clks:
 	while (--clk >= 0)
 		clk_put(priv->clks[clk]);
@@ -266,6 +284,9 @@ static int ohci_platform_remove(struct platform_device *dev)
 	if (pdata->power_off)
 		pdata->power_off(dev);
 
+	if (priv->rst)
+		reset_control_assert(priv->rst);
+
 	for (clk = 0; clk < OHCI_MAX_CLKS && priv->clks[clk]; clk++)
 		clk_put(priv->clks[clk]);
 
diff --git a/drivers/usb/host/ohci-pxa27x.c b/drivers/usb/host/ohci-pxa27x.c
index d21d5fefa76..e68f3d02cd1 100644
--- a/drivers/usb/host/ohci-pxa27x.c
+++ b/drivers/usb/host/ohci-pxa27x.c
@@ -30,6 +30,7 @@
 #include <linux/platform_data/usb-ohci-pxa27x.h>
 #include <linux/platform_data/usb-pxa3xx-ulpi.h>
 #include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
 #include <linux/signal.h>
 #include <linux/usb.h>
 #include <linux/usb/hcd.h>
@@ -120,6 +121,8 @@ static struct hc_driver __read_mostly ohci_pxa27x_hc_driver;
 struct pxa27x_ohci {
 	struct clk	*clk;
 	void __iomem	*mmio_base;
+	struct regulator *vbus[3];
+	bool		vbus_enabled[3];
 };
 
 #define to_pxa27x_ohci(hcd)	(struct pxa27x_ohci *)(hcd_to_ohci(hcd)->priv)
@@ -166,6 +169,52 @@ static int pxa27x_ohci_select_pmm(struct pxa27x_ohci *pxa_ohci, int mode)
 	return 0;
 }
 
+static int pxa27x_ohci_set_vbus_power(struct pxa27x_ohci *pxa_ohci,
+				      unsigned int port, bool enable)
+{
+	struct regulator *vbus = pxa_ohci->vbus[port];
+	int ret = 0;
+
+	if (IS_ERR_OR_NULL(vbus))
+		return 0;
+
+	if (enable && !pxa_ohci->vbus_enabled[port])
+		ret = regulator_enable(vbus);
+	else if (!enable && pxa_ohci->vbus_enabled[port])
+		ret = regulator_disable(vbus);
+
+	if (ret < 0)
+		return ret;
+
+	pxa_ohci->vbus_enabled[port] = enable;
+
+	return 0;
+}
+
+static int pxa27x_ohci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
+				   u16 wIndex, char *buf, u16 wLength)
+{
+	struct pxa27x_ohci *pxa_ohci = to_pxa27x_ohci(hcd);
+	int ret;
+
+	switch (typeReq) {
+	case SetPortFeature:
+	case ClearPortFeature:
+		if (!wIndex || wIndex > 3)
+			return -EPIPE;
+
+		if (wValue != USB_PORT_FEAT_POWER)
+			break;
+
+		ret = pxa27x_ohci_set_vbus_power(pxa_ohci, wIndex - 1,
+						 typeReq == SetPortFeature);
+		if (ret)
+			return ret;
+		break;
+	}
+
+	return ohci_hub_control(hcd, typeReq, wValue, wIndex, buf, wLength);
+}
 /*-------------------------------------------------------------------------*/
 
 static inline void pxa27x_setup_hc(struct pxa27x_ohci *pxa_ohci,
@@ -372,6 +421,7 @@ int usb_hcd_pxa27x_probe (const struct hc_driver *driver, struct platform_device
 	struct ohci_hcd *ohci;
 	struct resource *r;
 	struct clk *usb_clk;
+	unsigned int i;
 
 	retval = ohci_pxa_of_init(pdev);
 	if (retval)
@@ -417,6 +467,16 @@ int usb_hcd_pxa27x_probe (const struct hc_driver *driver, struct platform_device
 	pxa_ohci->clk = usb_clk;
 	pxa_ohci->mmio_base = (void __iomem *)hcd->regs;
 
+	for (i = 0; i < 3; ++i) {
+		char name[6];
+
+		if (!(inf->flags & (ENABLE_PORT1 << i)))
+			continue;
+
+		sprintf(name, "vbus%u", i + 1);
+		pxa_ohci->vbus[i] = devm_regulator_get(&pdev->dev, name);
+	}
+
 	retval = pxa27x_start_hc(pxa_ohci, &pdev->dev);
 	if (retval < 0) {
 		pr_debug("pxa27x_start_hc failed");
@@ -462,9 +522,14 @@ int usb_hcd_pxa27x_probe (const struct hc_driver *driver, struct platform_device
 void usb_hcd_pxa27x_remove (struct usb_hcd *hcd, struct platform_device *pdev)
 {
 	struct pxa27x_ohci *pxa_ohci = to_pxa27x_ohci(hcd);
+	unsigned int i;
 
 	usb_remove_hcd(hcd);
 	pxa27x_stop_hc(pxa_ohci, &pdev->dev);
+
+	for (i = 0; i < 3; ++i)
+		pxa27x_ohci_set_vbus_power(pxa_ohci, i, false);
+
 	usb_put_hcd(hcd);
 }
 
@@ -563,7 +628,10 @@ static int __init ohci_pxa27x_init(void)
 		return -ENODEV;
 
 	pr_info("%s: " DRIVER_DESC "\n", hcd_name);
+
 	ohci_init_driver(&ohci_pxa27x_hc_driver, &pxa27x_overrides);
+	ohci_pxa27x_hc_driver.hub_control = pxa27x_ohci_hub_control;
+
 	return platform_driver_register(&ohci_hcd_pxa27x_driver);
 }
 module_init(ohci_pxa27x_init);
diff --git a/drivers/usb/host/ohci-s3c2410.c b/drivers/usb/host/ohci-s3c2410.c
index ff7c8f1c48f..3d753a9d314 100644
--- a/drivers/usb/host/ohci-s3c2410.c
+++ b/drivers/usb/host/ohci-s3c2410.c
@@ -45,10 +45,6 @@ static struct clk *usb_clk;
 
 /* forward definitions */
 
-static int (*orig_ohci_hub_control)(struct usb_hcd  *hcd, u16 typeReq,
-			u16 wValue, u16 wIndex, char *buf, u16 wLength);
-static int (*orig_ohci_hub_status_data)(struct usb_hcd *hcd, char *buf);
-
 static void s3c2410_hcd_oc(struct s3c2410_hcd_info *info, int port_oc);
 
 /* conversion functions */
@@ -110,7 +106,7 @@ ohci_s3c2410_hub_status_data(struct usb_hcd *hcd, char *buf)
 	int orig;
 	int portno;
 
-	orig = orig_ohci_hub_status_data(hcd, buf);
+	orig = ohci_hub_status_data(hcd, buf);
 
 	if (info == NULL)
 		return orig;
@@ -181,7 +177,7 @@ static int ohci_s3c2410_hub_control(
 	 * process the request straight away and exit */
 
 	if (info == NULL) {
-		ret = orig_ohci_hub_control(hcd, typeReq, wValue,
+		ret = ohci_hub_control(hcd, typeReq, wValue,
 				       wIndex, buf, wLength);
 		goto out;
 	}
@@ -231,7 +227,7 @@ static int ohci_s3c2410_hub_control(
 		break;
 	}
 
-	ret = orig_ohci_hub_control(hcd, typeReq, wValue, wIndex, buf, wLength);
+	ret = ohci_hub_control(hcd, typeReq, wValue, wIndex, buf, wLength);
 	if (ret)
 		goto out;
 
@@ -489,9 +485,6 @@ static int __init ohci_s3c2410_init(void)
 	 * override these functions by making it too easy.
 	 */
 
-	orig_ohci_hub_control = ohci_s3c2410_hc_driver.hub_control;
-	orig_ohci_hub_status_data = ohci_s3c2410_hc_driver.hub_status_data;
-
 	ohci_s3c2410_hc_driver.hub_status_data	= ohci_s3c2410_hub_status_data;
 	ohci_s3c2410_hc_driver.hub_control	= ohci_s3c2410_hub_control;
 
diff --git a/drivers/usb/host/ohci-tilegx.c b/drivers/usb/host/ohci-tilegx.c
index 0b183e0b0a8..bef6dfb0405 100644
--- a/drivers/usb/host/ohci-tilegx.c
+++ b/drivers/usb/host/ohci-tilegx.c
@@ -129,8 +129,8 @@ static int ohci_hcd_tilegx_drv_probe(struct platform_device *pdev)
 	tilegx_start_ohc();
 
 	/* Create our IRQs and register them. */
-	pdata->irq = create_irq();
-	if (pdata->irq < 0) {
+	pdata->irq = irq_alloc_hwirq(-1);
+	if (!pdata->irq) {
 		ret = -ENXIO;
 		goto err_no_irq;
 	}
@@ -164,7 +164,7 @@ static int ohci_hcd_tilegx_drv_probe(struct platform_device *pdev)
 	}
 
 err_have_irq:
-	destroy_irq(pdata->irq);
+	irq_free_hwirq(pdata->irq);
 err_no_irq:
 	tilegx_stop_ohc();
 	usb_put_hcd(hcd);
@@ -182,7 +182,7 @@ static int ohci_hcd_tilegx_drv_remove(struct platform_device *pdev)
 	usb_put_hcd(hcd);
 	tilegx_stop_ohc();
 	gxio_usb_host_destroy(&pdata->usb_ctx);
-	destroy_irq(pdata->irq);
+	irq_free_hwirq(pdata->irq);
 
 	return 0;
 }
diff --git a/drivers/usb/host/ohci.h b/drivers/usb/host/ohci.h
index 4550ce05af7..05e02a709d4 100644
--- a/drivers/usb/host/ohci.h
+++ b/drivers/usb/host/ohci.h
@@ -729,3 +729,6 @@ extern int	ohci_setup(struct usb_hcd *hcd);
 extern int	ohci_suspend(struct usb_hcd *hcd, bool do_wakeup);
 extern int	ohci_resume(struct usb_hcd *hcd, bool hibernated);
 #endif
+extern int	ohci_hub_control(struct usb_hcd	*hcd, u16 typeReq, u16 wValue,
+				 u16 wIndex, char *buf, u16 wLength);
+extern int	ohci_hub_status_data(struct usb_hcd *hcd, char *buf);
diff --git a/drivers/usb/host/pci-quirks.c b/drivers/usb/host/pci-quirks.c
index 00661d30514..4a6d3dd6857 100644
--- a/drivers/usb/host/pci-quirks.c
+++ b/drivers/usb/host/pci-quirks.c
@@ -847,6 +847,13 @@ void usb_enable_intel_xhci_ports(struct pci_dev *xhci_pdev)
 	bool		ehci_found = false;
 	struct pci_dev	*companion = NULL;
 
+	/* Sony VAIO t-series with subsystem device ID 90a8 is not capable of
+	 * switching ports from EHCI to xHCI
+	 */
+	if (xhci_pdev->subsystem_vendor == PCI_VENDOR_ID_SONY &&
+	    xhci_pdev->subsystem_device == 0x90a8)
+		return;
+
 	/* make sure an intel EHCI controller exists */
 	for_each_pci_dev(companion) {
 		if (companion->class == PCI_CLASS_SERIAL_USB_EHCI &&
diff --git a/drivers/usb/host/pci-quirks.h b/drivers/usb/host/pci-quirks.h
index 638e88f7a28..c622ddf21c9 100644
--- a/drivers/usb/host/pci-quirks.h
+++ b/drivers/usb/host/pci-quirks.h
@@ -5,6 +5,7 @@
 void uhci_reset_hc(struct pci_dev *pdev, unsigned long base);
 int uhci_check_and_reset_hc(struct pci_dev *pdev, unsigned long base);
 int usb_amd_find_chipset_info(void);
+int usb_hcd_amd_remote_wakeup_quirk(struct pci_dev *pdev);
 bool usb_amd_hang_symptom_quirk(void);
 bool usb_amd_prefetch_quirk(void);
 void usb_amd_dev_put(void);
diff --git a/drivers/usb/host/xhci-hub.c b/drivers/usb/host/xhci-hub.c
index 1ad6bc1951c..6231ce6aa0c 100644
--- a/drivers/usb/host/xhci-hub.c
+++ b/drivers/usb/host/xhci-hub.c
@@ -20,7 +20,8 @@
  * Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
-#include <linux/gfp.h>
+
+#include <linux/slab.h>
 #include <asm/unaligned.h>
 
 #include "xhci.h"
@@ -270,7 +271,6 @@ static int xhci_stop_device(struct xhci_hcd *xhci, int slot_id, int suspend)
 	struct xhci_virt_device *virt_dev;
 	struct xhci_command *cmd;
 	unsigned long flags;
-	int timeleft;
 	int ret;
 	int i;
 
@@ -284,34 +284,31 @@ static int xhci_stop_device(struct xhci_hcd *xhci, int slot_id, int suspend)
 
 	spin_lock_irqsave(&xhci->lock, flags);
 	for (i = LAST_EP_INDEX; i > 0; i--) {
-		if (virt_dev->eps[i].ring && virt_dev->eps[i].ring->dequeue)
-			xhci_queue_stop_endpoint(xhci, slot_id, i, suspend);
+		if (virt_dev->eps[i].ring && virt_dev->eps[i].ring->dequeue) {
+			struct xhci_command *command;
+			command = xhci_alloc_command(xhci, false, false,
+						     GFP_NOIO);
+			if (!command) {
+				spin_unlock_irqrestore(&xhci->lock, flags);
+				xhci_free_command(xhci, cmd);
+				return -ENOMEM;
+
+			}
+			xhci_queue_stop_endpoint(xhci, command, slot_id, i,
+						 suspend);
+		}
 	}
-	cmd->command_trb = xhci_find_next_enqueue(xhci->cmd_ring);
-	list_add_tail(&cmd->cmd_list, &virt_dev->cmd_list);
-	xhci_queue_stop_endpoint(xhci, slot_id, 0, suspend);
+	xhci_queue_stop_endpoint(xhci, cmd, slot_id, 0, suspend);
 	xhci_ring_cmd_db(xhci);
 	spin_unlock_irqrestore(&xhci->lock, flags);
 
 	/* Wait for last stop endpoint command to finish */
-	timeleft = wait_for_completion_interruptible_timeout(
-			cmd->completion,
-			XHCI_CMD_DEFAULT_TIMEOUT);
-	if (timeleft <= 0) {
-		xhci_warn(xhci, "%s while waiting for stop endpoint command\n",
-				timeleft == 0 ? "Timeout" : "Signal");
-		spin_lock_irqsave(&xhci->lock, flags);
-		/* The timeout might have raced with the event ring handler, so
-		 * only delete from the list if the item isn't poisoned.
-		 */
-		if (cmd->cmd_list.next != LIST_POISON1)
-			list_del(&cmd->cmd_list);
-		spin_unlock_irqrestore(&xhci->lock, flags);
+	wait_for_completion(cmd->completion);
+
+	if (cmd->status == COMP_CMD_ABORT || cmd->status == COMP_CMD_STOP) {
+		xhci_warn(xhci, "Timeout while waiting for stop endpoint command\n");
 		ret = -ETIME;
-		goto command_cleanup;
 	}
-
-command_cleanup:
 	xhci_free_command(xhci, cmd);
 	return ret;
 }
diff --git a/drivers/usb/host/xhci-mem.c b/drivers/usb/host/xhci-mem.c
index c089668308a..8056d90690e 100644
--- a/drivers/usb/host/xhci-mem.c
+++ b/drivers/usb/host/xhci-mem.c
@@ -1020,7 +1020,6 @@ int xhci_alloc_virt_device(struct xhci_hcd *xhci, int slot_id,
 	dev->num_rings_cached = 0;
 
 	init_completion(&dev->cmd_completion);
-	INIT_LIST_HEAD(&dev->cmd_list);
 	dev->udev = udev;
 
 	/* Point to output device context in dcbaa. */
@@ -1794,10 +1793,11 @@ void xhci_free_command(struct xhci_hcd *xhci,
 void xhci_mem_cleanup(struct xhci_hcd *xhci)
 {
 	struct device	*dev = xhci_to_hcd(xhci)->self.controller;
-	struct xhci_cd  *cur_cd, *next_cd;
 	int size;
 	int i, j, num_ports;
 
+	del_timer_sync(&xhci->cmd_timer);
+
 	/* Free the Event Ring Segment Table and the actual Event Ring */
 	size = sizeof(struct xhci_erst_entry)*(xhci->erst.num_entries);
 	if (xhci->erst.entries)
@@ -1816,10 +1816,16 @@ void xhci_mem_cleanup(struct xhci_hcd *xhci)
 		xhci_ring_free(xhci, xhci->cmd_ring);
 	xhci->cmd_ring = NULL;
 	xhci_dbg_trace(xhci, trace_xhci_dbg_init, "Freed command ring");
-	list_for_each_entry_safe(cur_cd, next_cd,
-			&xhci->cancel_cmd_list, cancel_cmd_list) {
-		list_del(&cur_cd->cancel_cmd_list);
-		kfree(cur_cd);
+	xhci_cleanup_command_queue(xhci);
+
+	num_ports = HCS_MAX_PORTS(xhci->hcs_params1);
+	for (i = 0; i < num_ports; 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;
+			while (!list_empty(ep))
+				list_del_init(ep->next);
+		}
 	}
 
 	for (i = 1; i < MAX_HC_SLOTS; ++i)
@@ -1857,16 +1863,6 @@ void xhci_mem_cleanup(struct xhci_hcd *xhci)
 	if (!xhci->rh_bw)
 		goto no_bw;
 
-	num_ports = HCS_MAX_PORTS(xhci->hcs_params1);
-	for (i = 0; i < num_ports; 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;
-			while (!list_empty(ep))
-				list_del_init(ep->next);
-		}
-	}
-
 	for (i = 0; i < num_ports; i++) {
 		struct xhci_tt_bw_info *tt, *n;
 		list_for_each_entry_safe(tt, n, &xhci->rh_bw[i].tts, tt_list) {
@@ -2323,7 +2319,7 @@ int xhci_mem_init(struct xhci_hcd *xhci, gfp_t flags)
 	u32 page_size, temp;
 	int i;
 
-	INIT_LIST_HEAD(&xhci->cancel_cmd_list);
+	INIT_LIST_HEAD(&xhci->cmd_list);
 
 	page_size = readl(&xhci->op_regs->page_size);
 	xhci_dbg_trace(xhci, trace_xhci_dbg_init,
@@ -2509,6 +2505,11 @@ int xhci_mem_init(struct xhci_hcd *xhci, gfp_t flags)
 			"Wrote ERST address to ir_set 0.");
 	xhci_print_ir_set(xhci, 0);
 
+	/* init command timeout timer */
+	init_timer(&xhci->cmd_timer);
+	xhci->cmd_timer.data = (unsigned long) xhci;
+	xhci->cmd_timer.function = xhci_handle_command_timeout;
+
 	/*
 	 * XXX: Might need to set the Interrupter Moderation Register to
 	 * something other than the default (~1ms minimum between interrupts).
diff --git a/drivers/usb/host/xhci-mvebu.c b/drivers/usb/host/xhci-mvebu.c
new file mode 100644
index 00000000000..1eefc988192
--- /dev/null
+++ b/drivers/usb/host/xhci-mvebu.c
@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) 2014 Marvell
+ * Author: Gregory CLEMENT <gregory.clement@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
+ * version 2 as published by the Free Software Foundation.
+ */
+
+#include <linux/io.h>
+#include <linux/mbus.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+
+#include "xhci-mvebu.h"
+
+#define USB3_MAX_WINDOWS	4
+#define USB3_WIN_CTRL(w)	(0x0 + ((w) * 8))
+#define USB3_WIN_BASE(w)	(0x4 + ((w) * 8))
+
+static void xhci_mvebu_mbus_config(void __iomem *base,
+			const struct mbus_dram_target_info *dram)
+{
+	int win;
+
+	/* Clear all existing windows */
+	for (win = 0; win < USB3_MAX_WINDOWS; win++) {
+		writel(0, base + USB3_WIN_CTRL(win));
+		writel(0, base + USB3_WIN_BASE(win));
+	}
+
+	/* Program each DRAM CS in a seperate window */
+	for (win = 0; win < dram->num_cs; win++) {
+		const struct mbus_dram_window *cs = dram->cs + win;
+
+		writel(((cs->size - 1) & 0xffff0000) | (cs->mbus_attr << 8) |
+		       (dram->mbus_dram_target_id << 4) | 1,
+		       base + USB3_WIN_CTRL(win));
+
+		writel((cs->base & 0xffff0000), base + USB3_WIN_BASE(win));
+	}
+}
+
+int xhci_mvebu_mbus_init_quirk(struct platform_device *pdev)
+{
+	struct resource	*res;
+	void __iomem *base;
+	const struct mbus_dram_target_info *dram;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (!res)
+		return -ENODEV;
+
+	/*
+	 * We don't use devm_ioremap() because this mapping should
+	 * only exists for the duration of this probe function.
+	 */
+	base = ioremap(res->start, resource_size(res));
+	if (!base)
+		return -ENODEV;
+
+	dram = mv_mbus_dram_info();
+	xhci_mvebu_mbus_config(base, dram);
+
+	/*
+	 * This memory area was only needed to configure the MBus
+	 * windows, and is therefore no longer useful.
+	 */
+	iounmap(base);
+
+	return 0;
+}
diff --git a/drivers/usb/host/xhci-mvebu.h b/drivers/usb/host/xhci-mvebu.h
new file mode 100644
index 00000000000..7ede92aa41f
--- /dev/null
+++ b/drivers/usb/host/xhci-mvebu.h
@@ -0,0 +1,21 @@
+/*
+ * Copyright (C) 2014 Marvell
+ *
+ * Gregory Clement <gregory.clement@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#ifndef __LINUX_XHCI_MVEBU_H
+#define __LINUX_XHCI_MVEBU_H
+#if IS_ENABLED(CONFIG_USB_XHCI_MVEBU)
+int xhci_mvebu_mbus_init_quirk(struct platform_device *pdev);
+#else
+static inline int xhci_mvebu_mbus_init_quirk(struct platform_device *pdev)
+{
+	return 0;
+}
+#endif
+#endif /* __LINUX_XHCI_MVEBU_H */
diff --git a/drivers/usb/host/xhci-pci.c b/drivers/usb/host/xhci-pci.c
index 35d44778070..e20520f4275 100644
--- a/drivers/usb/host/xhci-pci.c
+++ b/drivers/usb/host/xhci-pci.c
@@ -134,14 +134,14 @@ static void xhci_pci_quirks(struct device *dev, struct xhci_hcd *xhci)
 		 */
 		if (pdev->subsystem_vendor == PCI_VENDOR_ID_HP)
 			xhci->quirks |= XHCI_SPURIOUS_WAKEUP;
-
+	}
+	if (pdev->vendor == PCI_VENDOR_ID_INTEL &&
+		pdev->device == PCI_DEVICE_ID_INTEL_LYNXPOINT_LP_XHCI) {
 		xhci->quirks |= XHCI_SPURIOUS_REBOOT;
 	}
 	if (pdev->vendor == PCI_VENDOR_ID_ETRON &&
 			pdev->device == PCI_DEVICE_ID_ASROCK_P67) {
 		xhci->quirks |= XHCI_RESET_ON_RESUME;
-		xhci_dbg_trace(xhci, trace_xhci_dbg_quirks,
-				"QUIRK: Resetting on resume");
 		xhci->quirks |= XHCI_TRUST_TX_LENGTH;
 	}
 	if (pdev->vendor == PCI_VENDOR_ID_RENESAS &&
@@ -149,6 +149,10 @@ static void xhci_pci_quirks(struct device *dev, struct xhci_hcd *xhci)
 		xhci->quirks |= XHCI_RESET_ON_RESUME;
 	if (pdev->vendor == PCI_VENDOR_ID_VIA)
 		xhci->quirks |= XHCI_RESET_ON_RESUME;
+
+	if (xhci->quirks & XHCI_RESET_ON_RESUME)
+		xhci_dbg_trace(xhci, trace_xhci_dbg_quirks,
+				"QUIRK: Resetting on resume");
 }
 
 /* called during probe() after chip reset completes */
diff --git a/drivers/usb/host/xhci-plat.c b/drivers/usb/host/xhci-plat.c
index 151901ce1ba..29d8adb5c8d 100644
--- a/drivers/usb/host/xhci-plat.c
+++ b/drivers/usb/host/xhci-plat.c
@@ -11,13 +11,15 @@
  * version 2 as published by the Free Software Foundation.
  */
 
-#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/dma-mapping.h>
 #include <linux/module.h>
-#include <linux/slab.h>
 #include <linux/of.h>
-#include <linux/dma-mapping.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
 
 #include "xhci.h"
+#include "xhci-mvebu.h"
 
 static void xhci_plat_quirks(struct device *dev, struct xhci_hcd *xhci)
 {
@@ -35,6 +37,11 @@ static int xhci_plat_setup(struct usb_hcd *hcd)
 	return xhci_gen_setup(hcd, xhci_plat_quirks);
 }
 
+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",
@@ -50,7 +57,7 @@ static const struct hc_driver xhci_plat_xhci_driver = {
 	 * basic lifecycle operations
 	 */
 	.reset =		xhci_plat_setup,
-	.start =		xhci_run,
+	.start =		xhci_plat_start,
 	.stop =			xhci_stop,
 	.shutdown =		xhci_shutdown,
 
@@ -91,6 +98,7 @@ static int xhci_plat_probe(struct platform_device *pdev)
 	struct xhci_hcd		*xhci;
 	struct resource         *res;
 	struct usb_hcd		*hcd;
+	struct clk              *clk;
 	int			ret;
 	int			irq;
 
@@ -107,6 +115,15 @@ static int xhci_plat_probe(struct platform_device *pdev)
 	if (!res)
 		return -ENODEV;
 
+	if (of_device_is_compatible(pdev->dev.of_node,
+				    "marvell,armada-375-xhci") ||
+	    of_device_is_compatible(pdev->dev.of_node,
+				    "marvell,armada-380-xhci")) {
+		ret = xhci_mvebu_mbus_init_quirk(pdev);
+		if (ret)
+			return ret;
+	}
+
 	/* Initialize dma_mask and coherent_dma_mask to 32-bits */
 	ret = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
 	if (ret)
@@ -137,14 +154,27 @@ static int xhci_plat_probe(struct platform_device *pdev)
 		goto release_mem_region;
 	}
 
+	/*
+	 * Not all platforms have a clk so it is not an error if the
+	 * clock does not exists.
+	 */
+	clk = devm_clk_get(&pdev->dev, NULL);
+	if (!IS_ERR(clk)) {
+		ret = clk_prepare_enable(clk);
+		if (ret)
+			goto unmap_registers;
+	}
+
 	ret = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (ret)
-		goto unmap_registers;
+		goto disable_clk;
+
 	device_wakeup_enable(hcd->self.controller);
 
 	/* USB 2.0 roothub is stored in the platform_device now. */
 	hcd = platform_get_drvdata(pdev);
 	xhci = hcd_to_xhci(hcd);
+	xhci->clk = clk;
 	xhci->shared_hcd = usb_create_shared_hcd(driver, &pdev->dev,
 			dev_name(&pdev->dev), hcd);
 	if (!xhci->shared_hcd) {
@@ -173,6 +203,10 @@ put_usb3_hcd:
 dealloc_usb2_hcd:
 	usb_remove_hcd(hcd);
 
+disable_clk:
+	if (!IS_ERR(clk))
+		clk_disable_unprepare(clk);
+
 unmap_registers:
 	iounmap(hcd->regs);
 
@@ -189,11 +223,14 @@ static int xhci_plat_remove(struct platform_device *dev)
 {
 	struct usb_hcd	*hcd = platform_get_drvdata(dev);
 	struct xhci_hcd	*xhci = hcd_to_xhci(hcd);
+	struct clk *clk = xhci->clk;
 
 	usb_remove_hcd(xhci->shared_hcd);
 	usb_put_hcd(xhci->shared_hcd);
 
 	usb_remove_hcd(hcd);
+	if (!IS_ERR(clk))
+		clk_disable_unprepare(clk);
 	iounmap(hcd->regs);
 	release_mem_region(hcd->rsrc_start, hcd->rsrc_len);
 	usb_put_hcd(hcd);
@@ -202,7 +239,7 @@ static int xhci_plat_remove(struct platform_device *dev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int xhci_plat_suspend(struct device *dev)
 {
 	struct usb_hcd	*hcd = dev_get_drvdata(dev);
@@ -231,6 +268,8 @@ static const struct dev_pm_ops xhci_plat_pm_ops = {
 static const struct of_device_id usb_xhci_of_match[] = {
 	{ .compatible = "generic-xhci" },
 	{ .compatible = "xhci-platform" },
+	{ .compatible = "marvell,armada-375-xhci"},
+	{ .compatible = "marvell,armada-380-xhci"},
 	{ },
 };
 MODULE_DEVICE_TABLE(of, usb_xhci_of_match);
diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index 7a0e3c720c0..d67ff71209f 100644
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -69,10 +69,6 @@
 #include "xhci.h"
 #include "xhci-trace.h"
 
-static int handle_cmd_in_cmd_wait_list(struct xhci_hcd *xhci,
-		struct xhci_virt_device *virt_dev,
-		struct xhci_event_cmd *event);
-
 /*
  * Returns zero if the TRB isn't in this segment, otherwise it returns the DMA
  * address of the TRB.
@@ -123,16 +119,6 @@ static int enqueue_is_link_trb(struct xhci_ring *ring)
 	return TRB_TYPE_LINK_LE32(link->control);
 }
 
-union xhci_trb *xhci_find_next_enqueue(struct xhci_ring *ring)
-{
-	/* Enqueue pointer can be left pointing to the link TRB,
-	 * we must handle that
-	 */
-	if (TRB_TYPE_LINK_LE32(ring->enqueue->link.control))
-		return ring->enq_seg->next->trbs;
-	return ring->enqueue;
-}
-
 /* Updates trb to point to the next TRB in the ring, and updates seg if the next
  * TRB is in a new segment.  This does not skip over link TRBs, and it does not
  * effect the ring dequeue or enqueue pointers.
@@ -301,17 +287,7 @@ static int xhci_abort_cmd_ring(struct xhci_hcd *xhci)
 
 	xhci_dbg(xhci, "Abort command ring\n");
 
-	if (!(xhci->cmd_ring_state & CMD_RING_STATE_RUNNING)) {
-		xhci_dbg(xhci, "The command ring isn't running, "
-				"Have the command ring been stopped?\n");
-		return 0;
-	}
-
 	temp_64 = xhci_read_64(xhci, &xhci->op_regs->cmd_ring);
-	if (!(temp_64 & CMD_RING_RUNNING)) {
-		xhci_dbg(xhci, "Command ring had been stopped\n");
-		return 0;
-	}
 	xhci->cmd_ring_state = CMD_RING_STATE_ABORTED;
 	xhci_write_64(xhci, temp_64 | CMD_RING_ABORT,
 			&xhci->op_regs->cmd_ring);
@@ -337,71 +313,6 @@ static int xhci_abort_cmd_ring(struct xhci_hcd *xhci)
 	return 0;
 }
 
-static int xhci_queue_cd(struct xhci_hcd *xhci,
-		struct xhci_command *command,
-		union xhci_trb *cmd_trb)
-{
-	struct xhci_cd *cd;
-	cd = kzalloc(sizeof(struct xhci_cd), GFP_ATOMIC);
-	if (!cd)
-		return -ENOMEM;
-	INIT_LIST_HEAD(&cd->cancel_cmd_list);
-
-	cd->command = command;
-	cd->cmd_trb = cmd_trb;
-	list_add_tail(&cd->cancel_cmd_list, &xhci->cancel_cmd_list);
-
-	return 0;
-}
-
-/*
- * Cancel the command which has issue.
- *
- * Some commands may hang due to waiting for acknowledgement from
- * usb device. It is outside of the xHC's ability to control and
- * will cause the command ring is blocked. When it occurs software
- * should intervene to recover the command ring.
- * See Section 4.6.1.1 and 4.6.1.2
- */
-int xhci_cancel_cmd(struct xhci_hcd *xhci, struct xhci_command *command,
-		union xhci_trb *cmd_trb)
-{
-	int retval = 0;
-	unsigned long flags;
-
-	spin_lock_irqsave(&xhci->lock, flags);
-
-	if (xhci->xhc_state & XHCI_STATE_DYING) {
-		xhci_warn(xhci, "Abort the command ring,"
-				" but the xHCI is dead.\n");
-		retval = -ESHUTDOWN;
-		goto fail;
-	}
-
-	/* queue the cmd desriptor to cancel_cmd_list */
-	retval = xhci_queue_cd(xhci, command, cmd_trb);
-	if (retval) {
-		xhci_warn(xhci, "Queuing command descriptor failed.\n");
-		goto fail;
-	}
-
-	/* abort command ring */
-	retval = xhci_abort_cmd_ring(xhci);
-	if (retval) {
-		xhci_err(xhci, "Abort command ring failed\n");
-		if (unlikely(retval == -ESHUTDOWN)) {
-			spin_unlock_irqrestore(&xhci->lock, flags);
-			usb_hc_died(xhci_to_hcd(xhci)->primary_hcd);
-			xhci_dbg(xhci, "xHCI host controller is dead.\n");
-			return retval;
-		}
-	}
-
-fail:
-	spin_unlock_irqrestore(&xhci->lock, flags);
-	return retval;
-}
-
 void xhci_ring_ep_doorbell(struct xhci_hcd *xhci,
 		unsigned int slot_id,
 		unsigned int ep_index,
@@ -684,12 +595,14 @@ static void td_to_noop(struct xhci_hcd *xhci, struct xhci_ring *ep_ring,
 	}
 }
 
-static int queue_set_tr_deq(struct xhci_hcd *xhci, int slot_id,
+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)
@@ -704,7 +617,7 @@ void xhci_queue_new_dequeue_state(struct xhci_hcd *xhci,
 			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, slot_id, ep_index, stream_id,
+	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);
@@ -773,7 +686,6 @@ static void xhci_handle_cmd_stop_ep(struct xhci_hcd *xhci, int slot_id,
 		union xhci_trb *trb, struct xhci_event_cmd *event)
 {
 	unsigned int ep_index;
-	struct xhci_virt_device *virt_dev;
 	struct xhci_ring *ep_ring;
 	struct xhci_virt_ep *ep;
 	struct list_head *entry;
@@ -783,11 +695,7 @@ static void xhci_handle_cmd_stop_ep(struct xhci_hcd *xhci, int slot_id,
 	struct xhci_dequeue_state deq_state;
 
 	if (unlikely(TRB_TO_SUSPEND_PORT(le32_to_cpu(trb->generic.field[3])))) {
-		virt_dev = xhci->devs[slot_id];
-		if (virt_dev)
-			handle_cmd_in_cmd_wait_list(xhci, virt_dev,
-				event);
-		else
+		if (!xhci->devs[slot_id])
 			xhci_warn(xhci, "Stop endpoint command "
 				"completion for disabled slot %u\n",
 				slot_id);
@@ -858,7 +766,9 @@ remove_finished_td:
 
 	/* If necessary, queue a Set Transfer Ring Dequeue Pointer command */
 	if (deq_state.new_deq_ptr && deq_state.new_deq_seg) {
-		xhci_queue_new_dequeue_state(xhci,
+		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);
@@ -1206,9 +1116,11 @@ static void xhci_handle_cmd_reset_ep(struct xhci_hcd *xhci, int slot_id,
 	 * because the HW can't handle two commands being queued in a row.
 	 */
 	if (xhci->quirks & XHCI_RESET_EP_QUIRK) {
+		struct xhci_command *command;
+		command = xhci_alloc_command(xhci, false, false, GFP_ATOMIC);
 		xhci_dbg_trace(xhci, trace_xhci_dbg_quirks,
 				"Queueing configure endpoint command");
-		xhci_queue_configure_endpoint(xhci,
+		xhci_queue_configure_endpoint(xhci, command,
 				xhci->devs[slot_id]->in_ctx->dma, slot_id,
 				false);
 		xhci_ring_cmd_db(xhci);
@@ -1219,187 +1131,6 @@ static void xhci_handle_cmd_reset_ep(struct xhci_hcd *xhci, int slot_id,
 	}
 }
 
-/* Complete the command and detele it from the devcie's command queue.
- */
-static void xhci_complete_cmd_in_cmd_wait_list(struct xhci_hcd *xhci,
-		struct xhci_command *command, u32 status)
-{
-	command->status = status;
-	list_del(&command->cmd_list);
-	if (command->completion)
-		complete(command->completion);
-	else
-		xhci_free_command(xhci, command);
-}
-
-
-/* Check to see if a command in the device's command queue matches this one.
- * Signal the completion or free the command, and return 1.  Return 0 if the
- * completed command isn't at the head of the command list.
- */
-static int handle_cmd_in_cmd_wait_list(struct xhci_hcd *xhci,
-		struct xhci_virt_device *virt_dev,
-		struct xhci_event_cmd *event)
-{
-	struct xhci_command *command;
-
-	if (list_empty(&virt_dev->cmd_list))
-		return 0;
-
-	command = list_entry(virt_dev->cmd_list.next,
-			struct xhci_command, cmd_list);
-	if (xhci->cmd_ring->dequeue != command->command_trb)
-		return 0;
-
-	xhci_complete_cmd_in_cmd_wait_list(xhci, command,
-			GET_COMP_CODE(le32_to_cpu(event->status)));
-	return 1;
-}
-
-/*
- * Finding the command trb need to be cancelled and modifying it to
- * NO OP command. And if the command is in device's command wait
- * list, finishing and freeing it.
- *
- * If we can't find the command trb, we think it had already been
- * executed.
- */
-static void xhci_cmd_to_noop(struct xhci_hcd *xhci, struct xhci_cd *cur_cd)
-{
-	struct xhci_segment *cur_seg;
-	union xhci_trb *cmd_trb;
-	u32 cycle_state;
-
-	if (xhci->cmd_ring->dequeue == xhci->cmd_ring->enqueue)
-		return;
-
-	/* find the current segment of command ring */
-	cur_seg = find_trb_seg(xhci->cmd_ring->first_seg,
-			xhci->cmd_ring->dequeue, &cycle_state);
-
-	if (!cur_seg) {
-		xhci_warn(xhci, "Command ring mismatch, dequeue = %p %llx (dma)\n",
-				xhci->cmd_ring->dequeue,
-				(unsigned long long)
-				xhci_trb_virt_to_dma(xhci->cmd_ring->deq_seg,
-					xhci->cmd_ring->dequeue));
-		xhci_debug_ring(xhci, xhci->cmd_ring);
-		xhci_dbg_ring_ptrs(xhci, xhci->cmd_ring);
-		return;
-	}
-
-	/* find the command trb matched by cd from command ring */
-	for (cmd_trb = xhci->cmd_ring->dequeue;
-			cmd_trb != xhci->cmd_ring->enqueue;
-			next_trb(xhci, xhci->cmd_ring, &cur_seg, &cmd_trb)) {
-		/* If the trb is link trb, continue */
-		if (TRB_TYPE_LINK_LE32(cmd_trb->generic.field[3]))
-			continue;
-
-		if (cur_cd->cmd_trb == cmd_trb) {
-
-			/* If the command in device's command list, we should
-			 * finish it and free the command structure.
-			 */
-			if (cur_cd->command)
-				xhci_complete_cmd_in_cmd_wait_list(xhci,
-					cur_cd->command, COMP_CMD_STOP);
-
-			/* get cycle state from the origin command trb */
-			cycle_state = le32_to_cpu(cmd_trb->generic.field[3])
-				& TRB_CYCLE;
-
-			/* modify the command trb to NO OP command */
-			cmd_trb->generic.field[0] = 0;
-			cmd_trb->generic.field[1] = 0;
-			cmd_trb->generic.field[2] = 0;
-			cmd_trb->generic.field[3] = cpu_to_le32(
-					TRB_TYPE(TRB_CMD_NOOP) | cycle_state);
-			break;
-		}
-	}
-}
-
-static void xhci_cancel_cmd_in_cd_list(struct xhci_hcd *xhci)
-{
-	struct xhci_cd *cur_cd, *next_cd;
-
-	if (list_empty(&xhci->cancel_cmd_list))
-		return;
-
-	list_for_each_entry_safe(cur_cd, next_cd,
-			&xhci->cancel_cmd_list, cancel_cmd_list) {
-		xhci_cmd_to_noop(xhci, cur_cd);
-		list_del(&cur_cd->cancel_cmd_list);
-		kfree(cur_cd);
-	}
-}
-
-/*
- * traversing the cancel_cmd_list. If the command descriptor according
- * to cmd_trb is found, the function free it and return 1, otherwise
- * return 0.
- */
-static int xhci_search_cmd_trb_in_cd_list(struct xhci_hcd *xhci,
-		union xhci_trb *cmd_trb)
-{
-	struct xhci_cd *cur_cd, *next_cd;
-
-	if (list_empty(&xhci->cancel_cmd_list))
-		return 0;
-
-	list_for_each_entry_safe(cur_cd, next_cd,
-			&xhci->cancel_cmd_list, cancel_cmd_list) {
-		if (cur_cd->cmd_trb == cmd_trb) {
-			if (cur_cd->command)
-				xhci_complete_cmd_in_cmd_wait_list(xhci,
-					cur_cd->command, COMP_CMD_STOP);
-			list_del(&cur_cd->cancel_cmd_list);
-			kfree(cur_cd);
-			return 1;
-		}
-	}
-
-	return 0;
-}
-
-/*
- * If the cmd_trb_comp_code is COMP_CMD_ABORT, we just check whether the
- * trb pointed by the command ring dequeue pointer is the trb we want to
- * cancel or not. And if the cmd_trb_comp_code is COMP_CMD_STOP, we will
- * traverse the cancel_cmd_list to trun the all of the commands according
- * to command descriptor to NO-OP trb.
- */
-static int handle_stopped_cmd_ring(struct xhci_hcd *xhci,
-		int cmd_trb_comp_code)
-{
-	int cur_trb_is_good = 0;
-
-	/* Searching the cmd trb pointed by the command ring dequeue
-	 * pointer in command descriptor list. If it is found, free it.
-	 */
-	cur_trb_is_good = xhci_search_cmd_trb_in_cd_list(xhci,
-			xhci->cmd_ring->dequeue);
-
-	if (cmd_trb_comp_code == COMP_CMD_ABORT)
-		xhci->cmd_ring_state = CMD_RING_STATE_STOPPED;
-	else if (cmd_trb_comp_code == COMP_CMD_STOP) {
-		/* traversing the cancel_cmd_list and canceling
-		 * the command according to command descriptor
-		 */
-		xhci_cancel_cmd_in_cd_list(xhci);
-
-		xhci->cmd_ring_state = CMD_RING_STATE_RUNNING;
-		/*
-		 * ring command ring doorbell again to restart the
-		 * command ring
-		 */
-		if (xhci->cmd_ring->dequeue != xhci->cmd_ring->enqueue)
-			xhci_ring_cmd_db(xhci);
-	}
-	return cur_trb_is_good;
-}
-
 static void xhci_handle_cmd_enable_slot(struct xhci_hcd *xhci, int slot_id,
 		u32 cmd_comp_code)
 {
@@ -1407,7 +1138,6 @@ static void xhci_handle_cmd_enable_slot(struct xhci_hcd *xhci, int slot_id,
 		xhci->slot_id = slot_id;
 	else
 		xhci->slot_id = 0;
-	complete(&xhci->addr_dev);
 }
 
 static void xhci_handle_cmd_disable_slot(struct xhci_hcd *xhci, int slot_id)
@@ -1432,9 +1162,6 @@ static void xhci_handle_cmd_config_ep(struct xhci_hcd *xhci, int slot_id,
 	unsigned int ep_state;
 	u32 add_flags, drop_flags;
 
-	virt_dev = xhci->devs[slot_id];
-	if (handle_cmd_in_cmd_wait_list(xhci, virt_dev, event))
-		return;
 	/*
 	 * Configure endpoint commands can come from the USB core
 	 * configuration or alt setting changes, or because the HW
@@ -1443,6 +1170,7 @@ static void xhci_handle_cmd_config_ep(struct xhci_hcd *xhci, int slot_id,
 	 * If the command was for a halted endpoint, the xHCI driver
 	 * is not waiting on the configure endpoint command.
 	 */
+	virt_dev = xhci->devs[slot_id];
 	ctrl_ctx = xhci_get_input_control_ctx(xhci, virt_dev->in_ctx);
 	if (!ctrl_ctx) {
 		xhci_warn(xhci, "Could not get input context, bad type.\n");
@@ -1465,7 +1193,7 @@ static void xhci_handle_cmd_config_ep(struct xhci_hcd *xhci, int slot_id,
 			add_flags - SLOT_FLAG == drop_flags) {
 		ep_state = virt_dev->eps[ep_index].ep_state;
 		if (!(ep_state & EP_HALTED))
-			goto bandwidth_change;
+			return;
 		xhci_dbg_trace(xhci, trace_xhci_dbg_quirks,
 				"Completed config ep cmd - "
 				"last ep index = %d, state = %d",
@@ -1475,43 +1203,14 @@ static void xhci_handle_cmd_config_ep(struct xhci_hcd *xhci, int slot_id,
 		ring_doorbell_for_active_rings(xhci, slot_id, ep_index);
 		return;
 	}
-bandwidth_change:
-	xhci_dbg_trace(xhci,  trace_xhci_dbg_context_change,
-			"Completed config ep cmd");
-	virt_dev->cmd_status = cmd_comp_code;
-	complete(&virt_dev->cmd_completion);
 	return;
 }
 
-static void xhci_handle_cmd_eval_ctx(struct xhci_hcd *xhci, int slot_id,
-		struct xhci_event_cmd *event, u32 cmd_comp_code)
-{
-	struct xhci_virt_device *virt_dev;
-
-	virt_dev = xhci->devs[slot_id];
-	if (handle_cmd_in_cmd_wait_list(xhci, virt_dev, event))
-		return;
-	virt_dev->cmd_status = cmd_comp_code;
-	complete(&virt_dev->cmd_completion);
-}
-
-static void xhci_handle_cmd_addr_dev(struct xhci_hcd *xhci, int slot_id,
-		u32 cmd_comp_code)
-{
-	xhci->devs[slot_id]->cmd_status = cmd_comp_code;
-	complete(&xhci->addr_dev);
-}
-
 static void xhci_handle_cmd_reset_dev(struct xhci_hcd *xhci, int slot_id,
 		struct xhci_event_cmd *event)
 {
-	struct xhci_virt_device *virt_dev;
-
 	xhci_dbg(xhci, "Completed reset device command.\n");
-	virt_dev = xhci->devs[slot_id];
-	if (virt_dev)
-		handle_cmd_in_cmd_wait_list(xhci, virt_dev, event);
-	else
+	if (!xhci->devs[slot_id])
 		xhci_warn(xhci, "Reset device command completion "
 				"for disabled slot %u\n", slot_id);
 }
@@ -1529,6 +1228,116 @@ static void xhci_handle_cmd_nec_get_fw(struct xhci_hcd *xhci,
 			NEC_FW_MINOR(le32_to_cpu(event->status)));
 }
 
+static void xhci_complete_del_and_free_cmd(struct xhci_command *cmd, u32 status)
+{
+	list_del(&cmd->cmd_list);
+
+	if (cmd->completion) {
+		cmd->status = status;
+		complete(cmd->completion);
+	} else {
+		kfree(cmd);
+	}
+}
+
+void xhci_cleanup_command_queue(struct xhci_hcd *xhci)
+{
+	struct xhci_command *cur_cmd, *tmp_cmd;
+	list_for_each_entry_safe(cur_cmd, tmp_cmd, &xhci->cmd_list, cmd_list)
+		xhci_complete_del_and_free_cmd(cur_cmd, COMP_CMD_ABORT);
+}
+
+/*
+ * Turn all commands on command ring with status set to "aborted" to no-op trbs.
+ * If there are other commands waiting then restart the ring and kick the timer.
+ * This must be called with command ring stopped and xhci->lock held.
+ */
+static void xhci_handle_stopped_cmd_ring(struct xhci_hcd *xhci,
+					 struct xhci_command *cur_cmd)
+{
+	struct xhci_command *i_cmd, *tmp_cmd;
+	u32 cycle_state;
+
+	/* Turn all aborted commands in list to no-ops, then restart */
+	list_for_each_entry_safe(i_cmd, tmp_cmd, &xhci->cmd_list,
+				 cmd_list) {
+
+		if (i_cmd->status != COMP_CMD_ABORT)
+			continue;
+
+		i_cmd->status = COMP_CMD_STOP;
+
+		xhci_dbg(xhci, "Turn aborted command %p to no-op\n",
+			 i_cmd->command_trb);
+		/* get cycle state from the original cmd trb */
+		cycle_state = le32_to_cpu(
+			i_cmd->command_trb->generic.field[3]) &	TRB_CYCLE;
+		/* modify the command trb to no-op command */
+		i_cmd->command_trb->generic.field[0] = 0;
+		i_cmd->command_trb->generic.field[1] = 0;
+		i_cmd->command_trb->generic.field[2] = 0;
+		i_cmd->command_trb->generic.field[3] = cpu_to_le32(
+			TRB_TYPE(TRB_CMD_NOOP) | cycle_state);
+
+		/*
+		 * caller waiting for completion is called when command
+		 *  completion event is received for these no-op commands
+		 */
+	}
+
+	xhci->cmd_ring_state = CMD_RING_STATE_RUNNING;
+
+	/* ring command ring doorbell to restart the command ring */
+	if ((xhci->cmd_ring->dequeue != xhci->cmd_ring->enqueue) &&
+	    !(xhci->xhc_state & XHCI_STATE_DYING)) {
+		xhci->current_cmd = cur_cmd;
+		mod_timer(&xhci->cmd_timer, jiffies + XHCI_CMD_DEFAULT_TIMEOUT);
+		xhci_ring_cmd_db(xhci);
+	}
+	return;
+}
+
+
+void xhci_handle_command_timeout(unsigned long data)
+{
+	struct xhci_hcd *xhci;
+	int ret;
+	unsigned long flags;
+	u64 hw_ring_state;
+	struct xhci_command *cur_cmd = NULL;
+	xhci = (struct xhci_hcd *) data;
+
+	/* mark this command to be cancelled */
+	spin_lock_irqsave(&xhci->lock, flags);
+	if (xhci->current_cmd) {
+		cur_cmd = xhci->current_cmd;
+		cur_cmd->status = COMP_CMD_ABORT;
+	}
+
+
+	/* Make sure command ring is running before aborting it */
+	hw_ring_state = xhci_read_64(xhci, &xhci->op_regs->cmd_ring);
+	if ((xhci->cmd_ring_state & CMD_RING_STATE_RUNNING) &&
+	    (hw_ring_state & CMD_RING_RUNNING))  {
+
+		spin_unlock_irqrestore(&xhci->lock, flags);
+		xhci_dbg(xhci, "Command timeout\n");
+		ret = xhci_abort_cmd_ring(xhci);
+		if (unlikely(ret == -ESHUTDOWN)) {
+			xhci_err(xhci, "Abort command ring failed\n");
+			xhci_cleanup_command_queue(xhci);
+			usb_hc_died(xhci_to_hcd(xhci)->primary_hcd);
+			xhci_dbg(xhci, "xHCI host controller is dead.\n");
+		}
+		return;
+	}
+	/* command timeout on stopped ring, ring can't be aborted */
+	xhci_dbg(xhci, "Command timeout on stopped ring\n");
+	xhci_handle_stopped_cmd_ring(xhci, xhci->current_cmd);
+	spin_unlock_irqrestore(&xhci->lock, flags);
+	return;
+}
+
 static void handle_cmd_completion(struct xhci_hcd *xhci,
 		struct xhci_event_cmd *event)
 {
@@ -1537,6 +1346,7 @@ static void handle_cmd_completion(struct xhci_hcd *xhci,
 	dma_addr_t cmd_dequeue_dma;
 	u32 cmd_comp_code;
 	union xhci_trb *cmd_trb;
+	struct xhci_command *cmd;
 	u32 cmd_type;
 
 	cmd_dma = le64_to_cpu(event->cmd_trb);
@@ -1554,26 +1364,35 @@ static void handle_cmd_completion(struct xhci_hcd *xhci,
 		return;
 	}
 
+	cmd = list_entry(xhci->cmd_list.next, struct xhci_command, cmd_list);
+
+	if (cmd->command_trb != xhci->cmd_ring->dequeue) {
+		xhci_err(xhci,
+			 "Command completion event does not match command\n");
+		return;
+	}
+
+	del_timer(&xhci->cmd_timer);
+
 	trace_xhci_cmd_completion(cmd_trb, (struct xhci_generic_trb *) event);
 
 	cmd_comp_code = GET_COMP_CODE(le32_to_cpu(event->status));
-	if (cmd_comp_code == COMP_CMD_ABORT || cmd_comp_code == COMP_CMD_STOP) {
-		/* If the return value is 0, we think the trb pointed by
-		 * command ring dequeue pointer is a good trb. The good
-		 * trb means we don't want to cancel the trb, but it have
-		 * been stopped by host. So we should handle it normally.
-		 * Otherwise, driver should invoke inc_deq() and return.
-		 */
-		if (handle_stopped_cmd_ring(xhci, cmd_comp_code)) {
-			inc_deq(xhci, xhci->cmd_ring);
-			return;
-		}
-		/* There is no command to handle if we get a stop event when the
-		 * command ring is empty, event->cmd_trb points to the next
-		 * unset command
-		 */
-		if (xhci->cmd_ring->dequeue == xhci->cmd_ring->enqueue)
-			return;
+
+	/* If CMD ring stopped we own the trbs between enqueue and dequeue */
+	if (cmd_comp_code == COMP_CMD_STOP) {
+		xhci_handle_stopped_cmd_ring(xhci, cmd);
+		return;
+	}
+	/*
+	 * Host aborted the command ring, check if the current command was
+	 * supposed to be aborted, otherwise continue normally.
+	 * The command ring is stopped now, but the xHC will issue a Command
+	 * Ring Stopped event which will cause us to restart it.
+	 */
+	if (cmd_comp_code == COMP_CMD_ABORT) {
+		xhci->cmd_ring_state = CMD_RING_STATE_STOPPED;
+		if (cmd->status == COMP_CMD_ABORT)
+			goto event_handled;
 	}
 
 	cmd_type = TRB_FIELD_TO_TYPE(le32_to_cpu(cmd_trb->generic.field[3]));
@@ -1585,13 +1404,13 @@ static void handle_cmd_completion(struct xhci_hcd *xhci,
 		xhci_handle_cmd_disable_slot(xhci, slot_id);
 		break;
 	case TRB_CONFIG_EP:
-		xhci_handle_cmd_config_ep(xhci, slot_id, event, cmd_comp_code);
+		if (!cmd->completion)
+			xhci_handle_cmd_config_ep(xhci, slot_id, event,
+						  cmd_comp_code);
 		break;
 	case TRB_EVAL_CONTEXT:
-		xhci_handle_cmd_eval_ctx(xhci, slot_id, event, cmd_comp_code);
 		break;
 	case TRB_ADDR_DEV:
-		xhci_handle_cmd_addr_dev(xhci, slot_id, cmd_comp_code);
 		break;
 	case TRB_STOP_RING:
 		WARN_ON(slot_id != TRB_TO_SLOT_ID(
@@ -1604,6 +1423,9 @@ static void handle_cmd_completion(struct xhci_hcd *xhci,
 		xhci_handle_cmd_set_deq(xhci, slot_id, cmd_trb, cmd_comp_code);
 		break;
 	case TRB_CMD_NOOP:
+		/* Is this an aborted command turned to NO-OP? */
+		if (cmd->status == COMP_CMD_STOP)
+			cmd_comp_code = COMP_CMD_STOP;
 		break;
 	case TRB_RESET_EP:
 		WARN_ON(slot_id != TRB_TO_SLOT_ID(
@@ -1623,6 +1445,17 @@ static void handle_cmd_completion(struct xhci_hcd *xhci,
 		xhci->error_bitmask |= 1 << 6;
 		break;
 	}
+
+	/* restart timer if this wasn't the last command */
+	if (cmd->cmd_list.next != &xhci->cmd_list) {
+		xhci->current_cmd = list_entry(cmd->cmd_list.next,
+					       struct xhci_command, cmd_list);
+		mod_timer(&xhci->cmd_timer, jiffies + XHCI_CMD_DEFAULT_TIMEOUT);
+	}
+
+event_handled:
+	xhci_complete_del_and_free_cmd(cmd, cmd_comp_code);
+
 	inc_deq(xhci, xhci->cmd_ring);
 }
 
@@ -1938,11 +1771,16 @@ static void xhci_cleanup_halted_endpoint(struct xhci_hcd *xhci,
 		struct xhci_td *td, union xhci_trb *event_trb)
 {
 	struct xhci_virt_ep *ep = &xhci->devs[slot_id]->eps[ep_index];
+	struct xhci_command *command;
+	command = xhci_alloc_command(xhci, false, false, GFP_ATOMIC);
+	if (!command)
+		return;
+
 	ep->ep_state |= EP_HALTED;
 	ep->stopped_td = td;
 	ep->stopped_stream = stream_id;
 
-	xhci_queue_reset_ep(xhci, slot_id, ep_index);
+	xhci_queue_reset_ep(xhci, command, slot_id, ep_index);
 	xhci_cleanup_stalled_ring(xhci, td->urb->dev, ep_index);
 
 	ep->stopped_td = NULL;
@@ -2654,7 +2492,7 @@ static int handle_tx_event(struct xhci_hcd *xhci,
 				 * successful event after a short transfer.
 				 * Ignore it.
 				 */
-				if ((xhci->quirks & XHCI_SPURIOUS_SUCCESS) && 
+				if ((xhci->quirks & XHCI_SPURIOUS_SUCCESS) &&
 						ep_ring->last_td_was_short) {
 					ep_ring->last_td_was_short = false;
 					ret = 0;
@@ -3996,11 +3834,14 @@ int xhci_queue_isoc_tx_prepare(struct xhci_hcd *xhci, gfp_t mem_flags,
  * Don't decrement xhci->cmd_ring_reserved_trbs after we've queued the TRB
  * because the command event handler may want to resubmit a failed command.
  */
-static int queue_command(struct xhci_hcd *xhci, u32 field1, u32 field2,
-		u32 field3, u32 field4, bool command_must_succeed)
+static int queue_command(struct xhci_hcd *xhci, struct xhci_command *cmd,
+			 u32 field1, u32 field2,
+			 u32 field3, u32 field4, bool command_must_succeed)
 {
 	int reserved_trbs = xhci->cmd_ring_reserved_trbs;
 	int ret;
+	if (xhci->xhc_state & XHCI_STATE_DYING)
+		return -ESHUTDOWN;
 
 	if (!command_must_succeed)
 		reserved_trbs++;
@@ -4014,57 +3855,71 @@ static int queue_command(struct xhci_hcd *xhci, u32 field1, u32 field2,
 					"unfailable commands failed.\n");
 		return ret;
 	}
+
+	cmd->command_trb = xhci->cmd_ring->enqueue;
+	list_add_tail(&cmd->cmd_list, &xhci->cmd_list);
+
+	/* if there are no other commands queued we start the timeout timer */
+	if (xhci->cmd_list.next == &cmd->cmd_list &&
+	    !timer_pending(&xhci->cmd_timer)) {
+		xhci->current_cmd = cmd;
+		mod_timer(&xhci->cmd_timer, jiffies + XHCI_CMD_DEFAULT_TIMEOUT);
+	}
+
 	queue_trb(xhci, xhci->cmd_ring, false, field1, field2, field3,
 			field4 | xhci->cmd_ring->cycle_state);
 	return 0;
 }
 
 /* Queue a slot enable or disable request on the command ring */
-int xhci_queue_slot_control(struct xhci_hcd *xhci, u32 trb_type, u32 slot_id)
+int xhci_queue_slot_control(struct xhci_hcd *xhci, struct xhci_command *cmd,
+		u32 trb_type, u32 slot_id)
 {
-	return queue_command(xhci, 0, 0, 0,
+	return queue_command(xhci, cmd, 0, 0, 0,
 			TRB_TYPE(trb_type) | SLOT_ID_FOR_TRB(slot_id), false);
 }
 
 /* Queue an address device command TRB */
-int xhci_queue_address_device(struct xhci_hcd *xhci, dma_addr_t in_ctx_ptr,
-			      u32 slot_id, enum xhci_setup_dev setup)
+int xhci_queue_address_device(struct xhci_hcd *xhci, struct xhci_command *cmd,
+		dma_addr_t in_ctx_ptr, u32 slot_id, enum xhci_setup_dev setup)
 {
-	return queue_command(xhci, lower_32_bits(in_ctx_ptr),
+	return queue_command(xhci, cmd, lower_32_bits(in_ctx_ptr),
 			upper_32_bits(in_ctx_ptr), 0,
 			TRB_TYPE(TRB_ADDR_DEV) | SLOT_ID_FOR_TRB(slot_id)
 			| (setup == SETUP_CONTEXT_ONLY ? TRB_BSR : 0), false);
 }
 
-int xhci_queue_vendor_command(struct xhci_hcd *xhci,
+int xhci_queue_vendor_command(struct xhci_hcd *xhci, struct xhci_command *cmd,
 		u32 field1, u32 field2, u32 field3, u32 field4)
 {
-	return queue_command(xhci, field1, field2, field3, field4, false);
+	return queue_command(xhci, cmd, field1, field2, field3, field4, false);
 }
 
 /* Queue a reset device command TRB */
-int xhci_queue_reset_device(struct xhci_hcd *xhci, u32 slot_id)
+int xhci_queue_reset_device(struct xhci_hcd *xhci, struct xhci_command *cmd,
+		u32 slot_id)
 {
-	return queue_command(xhci, 0, 0, 0,
+	return queue_command(xhci, cmd, 0, 0, 0,
 			TRB_TYPE(TRB_RESET_DEV) | SLOT_ID_FOR_TRB(slot_id),
 			false);
 }
 
 /* Queue a configure endpoint command TRB */
-int xhci_queue_configure_endpoint(struct xhci_hcd *xhci, dma_addr_t in_ctx_ptr,
+int xhci_queue_configure_endpoint(struct xhci_hcd *xhci,
+		struct xhci_command *cmd, dma_addr_t in_ctx_ptr,
 		u32 slot_id, bool command_must_succeed)
 {
-	return queue_command(xhci, lower_32_bits(in_ctx_ptr),
+	return queue_command(xhci, cmd, lower_32_bits(in_ctx_ptr),
 			upper_32_bits(in_ctx_ptr), 0,
 			TRB_TYPE(TRB_CONFIG_EP) | SLOT_ID_FOR_TRB(slot_id),
 			command_must_succeed);
 }
 
 /* Queue an evaluate context command TRB */
-int xhci_queue_evaluate_context(struct xhci_hcd *xhci, dma_addr_t in_ctx_ptr,
-		u32 slot_id, bool command_must_succeed)
+int xhci_queue_evaluate_context(struct xhci_hcd *xhci, struct xhci_command *cmd,
+		dma_addr_t in_ctx_ptr, u32 slot_id, bool command_must_succeed)
 {
-	return queue_command(xhci, lower_32_bits(in_ctx_ptr),
+	return queue_command(xhci, cmd, lower_32_bits(in_ctx_ptr),
 			upper_32_bits(in_ctx_ptr), 0,
 			TRB_TYPE(TRB_EVAL_CONTEXT) | SLOT_ID_FOR_TRB(slot_id),
 			command_must_succeed);
@@ -4074,25 +3929,26 @@ int xhci_queue_evaluate_context(struct xhci_hcd *xhci, dma_addr_t in_ctx_ptr,
  * Suspend is set to indicate "Stop Endpoint Command" is being issued to stop
  * activity on an endpoint that is about to be suspended.
  */
-int xhci_queue_stop_endpoint(struct xhci_hcd *xhci, int slot_id,
-		unsigned int ep_index, int suspend)
+int xhci_queue_stop_endpoint(struct xhci_hcd *xhci, struct xhci_command *cmd,
+			     int slot_id, unsigned int ep_index, int suspend)
 {
 	u32 trb_slot_id = SLOT_ID_FOR_TRB(slot_id);
 	u32 trb_ep_index = EP_ID_FOR_TRB(ep_index);
 	u32 type = TRB_TYPE(TRB_STOP_RING);
 	u32 trb_suspend = SUSPEND_PORT_FOR_TRB(suspend);
 
-	return queue_command(xhci, 0, 0, 0,
+	return queue_command(xhci, cmd, 0, 0, 0,
 			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, int slot_id,
-		unsigned int ep_index, unsigned int stream_id,
-		struct xhci_segment *deq_seg,
-		union xhci_trb *deq_ptr, u32 cycle_state)
+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)
 {
 	dma_addr_t addr;
 	u32 trb_slot_id = SLOT_ID_FOR_TRB(slot_id);
@@ -4119,18 +3975,19 @@ static int queue_set_tr_deq(struct xhci_hcd *xhci, int slot_id,
 	ep->queued_deq_ptr = deq_ptr;
 	if (stream_id)
 		trb_sct = SCT_FOR_TRB(SCT_PRI_TR);
-	return queue_command(xhci, lower_32_bits(addr) | trb_sct | cycle_state,
+	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);
 }
 
-int xhci_queue_reset_ep(struct xhci_hcd *xhci, int slot_id,
-		unsigned int ep_index)
+int xhci_queue_reset_ep(struct xhci_hcd *xhci, struct xhci_command *cmd,
+			int slot_id, unsigned int ep_index)
 {
 	u32 trb_slot_id = SLOT_ID_FOR_TRB(slot_id);
 	u32 trb_ep_index = EP_ID_FOR_TRB(ep_index);
 	u32 type = TRB_TYPE(TRB_RESET_EP);
 
-	return queue_command(xhci, 0, 0, 0, trb_slot_id | trb_ep_index | type,
-			false);
+	return queue_command(xhci, cmd, 0, 0, 0,
+			trb_slot_id | trb_ep_index | type, false);
 }
diff --git a/drivers/usb/host/xhci.c b/drivers/usb/host/xhci.c
index 300836972fa..2b8d9a24af0 100644
--- a/drivers/usb/host/xhci.c
+++ b/drivers/usb/host/xhci.c
@@ -291,7 +291,7 @@ static int xhci_setup_msix(struct xhci_hcd *xhci)
 		xhci->msix_entries[i].vector = 0;
 	}
 
-	ret = pci_enable_msix(pdev, xhci->msix_entries, xhci->msix_count);
+	ret = pci_enable_msix_exact(pdev, xhci->msix_entries, xhci->msix_count);
 	if (ret) {
 		xhci_dbg_trace(xhci, trace_xhci_dbg_init,
 				"Failed to enable MSI-X");
@@ -641,10 +641,14 @@ int xhci_run(struct usb_hcd *hcd)
 	writel(ER_IRQ_ENABLE(temp), &xhci->ir_set->irq_pending);
 	xhci_print_ir_set(xhci, 0);
 
-	if (xhci->quirks & XHCI_NEC_HOST)
-		xhci_queue_vendor_command(xhci, 0, 0, 0,
+	if (xhci->quirks & XHCI_NEC_HOST) {
+		struct xhci_command *command;
+		command = xhci_alloc_command(xhci, false, false, GFP_KERNEL);
+		if (!command)
+			return -ENOMEM;
+		xhci_queue_vendor_command(xhci, command, 0, 0, 0,
 				TRB_TYPE(TRB_NEC_GET_FW));
-
+	}
 	xhci_dbg_trace(xhci, trace_xhci_dbg_init,
 			"Finished xhci_run for USB2 roothub");
 	return 0;
@@ -1187,10 +1191,10 @@ static int xhci_configure_endpoint(struct xhci_hcd *xhci,
 static int xhci_check_maxpacket(struct xhci_hcd *xhci, unsigned int slot_id,
 		unsigned int ep_index, struct urb *urb)
 {
-	struct xhci_container_ctx *in_ctx;
 	struct xhci_container_ctx *out_ctx;
 	struct xhci_input_control_ctx *ctrl_ctx;
 	struct xhci_ep_ctx *ep_ctx;
+	struct xhci_command *command;
 	int max_packet_size;
 	int hw_max_packet_size;
 	int ret = 0;
@@ -1215,18 +1219,24 @@ static int xhci_check_maxpacket(struct xhci_hcd *xhci, unsigned int slot_id,
 		/* FIXME: This won't work if a non-default control endpoint
 		 * changes max packet sizes.
 		 */
-		in_ctx = xhci->devs[slot_id]->in_ctx;
-		ctrl_ctx = xhci_get_input_control_ctx(xhci, in_ctx);
+
+		command = xhci_alloc_command(xhci, false, true, GFP_KERNEL);
+		if (!command)
+			return -ENOMEM;
+
+		command->in_ctx = xhci->devs[slot_id]->in_ctx;
+		ctrl_ctx = xhci_get_input_control_ctx(xhci, command->in_ctx);
 		if (!ctrl_ctx) {
 			xhci_warn(xhci, "%s: Could not get input context, bad type.\n",
 					__func__);
-			return -ENOMEM;
+			ret = -ENOMEM;
+			goto command_cleanup;
 		}
 		/* Set up the modified control endpoint 0 */
 		xhci_endpoint_copy(xhci, xhci->devs[slot_id]->in_ctx,
 				xhci->devs[slot_id]->out_ctx, ep_index);
 
-		ep_ctx = xhci_get_ep_ctx(xhci, in_ctx, ep_index);
+		ep_ctx = xhci_get_ep_ctx(xhci, command->in_ctx, ep_index);
 		ep_ctx->ep_info2 &= cpu_to_le32(~MAX_PACKET_MASK);
 		ep_ctx->ep_info2 |= cpu_to_le32(MAX_PACKET(max_packet_size));
 
@@ -1234,17 +1244,20 @@ static int xhci_check_maxpacket(struct xhci_hcd *xhci, unsigned int slot_id,
 		ctrl_ctx->drop_flags = 0;
 
 		xhci_dbg(xhci, "Slot %d input context\n", slot_id);
-		xhci_dbg_ctx(xhci, in_ctx, ep_index);
+		xhci_dbg_ctx(xhci, command->in_ctx, ep_index);
 		xhci_dbg(xhci, "Slot %d output context\n", slot_id);
 		xhci_dbg_ctx(xhci, out_ctx, ep_index);
 
-		ret = xhci_configure_endpoint(xhci, urb->dev, NULL,
+		ret = xhci_configure_endpoint(xhci, urb->dev, command,
 				true, false);
 
 		/* Clean up the input context for later use by bandwidth
 		 * functions.
 		 */
 		ctrl_ctx->add_flags = cpu_to_le32(SLOT_FLAG);
+command_cleanup:
+		kfree(command->completion);
+		kfree(command);
 	}
 	return ret;
 }
@@ -1465,6 +1478,7 @@ int xhci_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
 	unsigned int ep_index;
 	struct xhci_ring *ep_ring;
 	struct xhci_virt_ep *ep;
+	struct xhci_command *command;
 
 	xhci = hcd_to_xhci(hcd);
 	spin_lock_irqsave(&xhci->lock, flags);
@@ -1534,12 +1548,14 @@ int xhci_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
 	 * the first cancellation to be handled.
 	 */
 	if (!(ep->ep_state & EP_HALT_PENDING)) {
+		command = xhci_alloc_command(xhci, false, false, GFP_ATOMIC);
 		ep->ep_state |= EP_HALT_PENDING;
 		ep->stop_cmds_pending++;
 		ep->stop_cmd_timer.expires = jiffies +
 			XHCI_STOP_EP_CMD_TIMEOUT * HZ;
 		add_timer(&ep->stop_cmd_timer);
-		xhci_queue_stop_endpoint(xhci, urb->dev->slot_id, ep_index, 0);
+		xhci_queue_stop_endpoint(xhci, command, urb->dev->slot_id,
+					 ep_index, 0);
 		xhci_ring_cmd_db(xhci);
 	}
 done:
@@ -1804,6 +1820,11 @@ static int xhci_configure_endpoint_result(struct xhci_hcd *xhci,
 	int ret;
 
 	switch (*cmd_status) {
+	case COMP_CMD_ABORT:
+	case COMP_CMD_STOP:
+		xhci_warn(xhci, "Timeout while waiting for configure endpoint command\n");
+		ret = -ETIME;
+		break;
 	case COMP_ENOMEM:
 		dev_warn(&udev->dev, "Not enough host controller resources "
 				"for new device state.\n");
@@ -1850,6 +1871,11 @@ static int xhci_evaluate_context_result(struct xhci_hcd *xhci,
 	struct xhci_virt_device *virt_dev = xhci->devs[udev->slot_id];
 
 	switch (*cmd_status) {
+	case COMP_CMD_ABORT:
+	case COMP_CMD_STOP:
+		xhci_warn(xhci, "Timeout while waiting for evaluate context command\n");
+		ret = -ETIME;
+		break;
 	case COMP_EINVAL:
 		dev_warn(&udev->dev, "WARN: xHCI driver setup invalid evaluate "
 				"context command.\n");
@@ -2574,23 +2600,17 @@ static int xhci_configure_endpoint(struct xhci_hcd *xhci,
 		bool ctx_change, bool must_succeed)
 {
 	int ret;
-	int timeleft;
 	unsigned long flags;
-	struct xhci_container_ctx *in_ctx;
 	struct xhci_input_control_ctx *ctrl_ctx;
-	struct completion *cmd_completion;
-	u32 *cmd_status;
 	struct xhci_virt_device *virt_dev;
-	union xhci_trb *cmd_trb;
+
+	if (!command)
+		return -EINVAL;
 
 	spin_lock_irqsave(&xhci->lock, flags);
 	virt_dev = xhci->devs[udev->slot_id];
 
-	if (command)
-		in_ctx = command->in_ctx;
-	else
-		in_ctx = virt_dev->in_ctx;
-	ctrl_ctx = xhci_get_input_control_ctx(xhci, in_ctx);
+	ctrl_ctx = xhci_get_input_control_ctx(xhci, command->in_ctx);
 	if (!ctrl_ctx) {
 		spin_unlock_irqrestore(&xhci->lock, flags);
 		xhci_warn(xhci, "%s: Could not get input context, bad type.\n",
@@ -2607,7 +2627,7 @@ static int xhci_configure_endpoint(struct xhci_hcd *xhci,
 		return -ENOMEM;
 	}
 	if ((xhci->quirks & XHCI_SW_BW_CHECKING) &&
-			xhci_reserve_bandwidth(xhci, virt_dev, in_ctx)) {
+	    xhci_reserve_bandwidth(xhci, virt_dev, command->in_ctx)) {
 		if ((xhci->quirks & XHCI_EP_LIMIT_QUIRK))
 			xhci_free_host_resources(xhci, ctrl_ctx);
 		spin_unlock_irqrestore(&xhci->lock, flags);
@@ -2615,27 +2635,15 @@ static int xhci_configure_endpoint(struct xhci_hcd *xhci,
 		return -ENOMEM;
 	}
 
-	if (command) {
-		cmd_completion = command->completion;
-		cmd_status = &command->status;
-		command->command_trb = xhci_find_next_enqueue(xhci->cmd_ring);
-		list_add_tail(&command->cmd_list, &virt_dev->cmd_list);
-	} else {
-		cmd_completion = &virt_dev->cmd_completion;
-		cmd_status = &virt_dev->cmd_status;
-	}
-	init_completion(cmd_completion);
-
-	cmd_trb = xhci_find_next_enqueue(xhci->cmd_ring);
 	if (!ctx_change)
-		ret = xhci_queue_configure_endpoint(xhci, in_ctx->dma,
+		ret = xhci_queue_configure_endpoint(xhci, command,
+				command->in_ctx->dma,
 				udev->slot_id, must_succeed);
 	else
-		ret = xhci_queue_evaluate_context(xhci, in_ctx->dma,
+		ret = xhci_queue_evaluate_context(xhci, command,
+				command->in_ctx->dma,
 				udev->slot_id, must_succeed);
 	if (ret < 0) {
-		if (command)
-			list_del(&command->cmd_list);
 		if ((xhci->quirks & XHCI_EP_LIMIT_QUIRK))
 			xhci_free_host_resources(xhci, ctrl_ctx);
 		spin_unlock_irqrestore(&xhci->lock, flags);
@@ -2647,26 +2655,14 @@ static int xhci_configure_endpoint(struct xhci_hcd *xhci,
 	spin_unlock_irqrestore(&xhci->lock, flags);
 
 	/* Wait for the configure endpoint command to complete */
-	timeleft = wait_for_completion_interruptible_timeout(
-			cmd_completion,
-			XHCI_CMD_DEFAULT_TIMEOUT);
-	if (timeleft <= 0) {
-		xhci_warn(xhci, "%s while waiting for %s command\n",
-				timeleft == 0 ? "Timeout" : "Signal",
-				ctx_change == 0 ?
-					"configure endpoint" :
-					"evaluate context");
-		/* cancel the configure endpoint command */
-		ret = xhci_cancel_cmd(xhci, command, cmd_trb);
-		if (ret < 0)
-			return ret;
-		return -ETIME;
-	}
+	wait_for_completion(command->completion);
 
 	if (!ctx_change)
-		ret = xhci_configure_endpoint_result(xhci, udev, cmd_status);
+		ret = xhci_configure_endpoint_result(xhci, udev,
+						     &command->status);
 	else
-		ret = xhci_evaluate_context_result(xhci, udev, cmd_status);
+		ret = xhci_evaluate_context_result(xhci, udev,
+						   &command->status);
 
 	if ((xhci->quirks & XHCI_EP_LIMIT_QUIRK)) {
 		spin_lock_irqsave(&xhci->lock, flags);
@@ -2714,6 +2710,7 @@ int xhci_check_bandwidth(struct usb_hcd *hcd, struct usb_device *udev)
 	struct xhci_virt_device	*virt_dev;
 	struct xhci_input_control_ctx *ctrl_ctx;
 	struct xhci_slot_ctx *slot_ctx;
+	struct xhci_command *command;
 
 	ret = xhci_check_args(hcd, udev, NULL, 0, true, __func__);
 	if (ret <= 0)
@@ -2725,12 +2722,19 @@ int xhci_check_bandwidth(struct usb_hcd *hcd, struct usb_device *udev)
 	xhci_dbg(xhci, "%s called for udev %p\n", __func__, udev);
 	virt_dev = xhci->devs[udev->slot_id];
 
+	command = xhci_alloc_command(xhci, false, true, GFP_KERNEL);
+	if (!command)
+		return -ENOMEM;
+
+	command->in_ctx = virt_dev->in_ctx;
+
 	/* See section 4.6.6 - A0 = 1; A1 = D0 = D1 = 0 */
-	ctrl_ctx = xhci_get_input_control_ctx(xhci, virt_dev->in_ctx);
+	ctrl_ctx = xhci_get_input_control_ctx(xhci, command->in_ctx);
 	if (!ctrl_ctx) {
 		xhci_warn(xhci, "%s: Could not get input context, bad type.\n",
 				__func__);
-		return -ENOMEM;
+		ret = -ENOMEM;
+		goto command_cleanup;
 	}
 	ctrl_ctx->add_flags |= cpu_to_le32(SLOT_FLAG);
 	ctrl_ctx->add_flags &= cpu_to_le32(~EP0_FLAG);
@@ -2738,20 +2742,20 @@ int xhci_check_bandwidth(struct usb_hcd *hcd, struct usb_device *udev)
 
 	/* Don't issue the command if there's no endpoints to update. */
 	if (ctrl_ctx->add_flags == cpu_to_le32(SLOT_FLAG) &&
-			ctrl_ctx->drop_flags == 0)
-		return 0;
-
+	    ctrl_ctx->drop_flags == 0) {
+		ret = 0;
+		goto command_cleanup;
+	}
 	xhci_dbg(xhci, "New Input Control Context:\n");
 	slot_ctx = xhci_get_slot_ctx(xhci, virt_dev->in_ctx);
 	xhci_dbg_ctx(xhci, virt_dev->in_ctx,
 		     LAST_CTX_TO_EP_NUM(le32_to_cpu(slot_ctx->dev_info)));
 
-	ret = xhci_configure_endpoint(xhci, udev, NULL,
+	ret = xhci_configure_endpoint(xhci, udev, command,
 			false, false);
-	if (ret) {
+	if (ret)
 		/* Callee should call reset_bandwidth() */
-		return ret;
-	}
+		goto command_cleanup;
 
 	xhci_dbg(xhci, "Output context after successful config ep cmd:\n");
 	xhci_dbg_ctx(xhci, virt_dev->out_ctx,
@@ -2783,6 +2787,9 @@ int xhci_check_bandwidth(struct usb_hcd *hcd, struct usb_device *udev)
 		virt_dev->eps[i].ring = virt_dev->eps[i].new_ring;
 		virt_dev->eps[i].new_ring = NULL;
 	}
+command_cleanup:
+	kfree(command->completion);
+	kfree(command);
 
 	return ret;
 }
@@ -2884,9 +2891,14 @@ void xhci_cleanup_stalled_ring(struct xhci_hcd *xhci,
 	 * 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, udev->slot_id,
+		xhci_queue_new_dequeue_state(xhci, command, udev->slot_id,
 				ep_index, ep->stopped_stream, &deq_state);
 	} else {
 		/* Better hope no one uses the input context between now and the
@@ -2917,6 +2929,7 @@ void xhci_endpoint_reset(struct usb_hcd *hcd,
 	unsigned long flags;
 	int ret;
 	struct xhci_virt_ep *virt_ep;
+	struct xhci_command *command;
 
 	xhci = hcd_to_xhci(hcd);
 	udev = (struct usb_device *) ep->hcpriv;
@@ -2939,10 +2952,14 @@ void xhci_endpoint_reset(struct usb_hcd *hcd,
 		return;
 	}
 
+	command = xhci_alloc_command(xhci, false, false, GFP_ATOMIC);
+	if (!command)
+		return;
+
 	xhci_dbg_trace(xhci, trace_xhci_dbg_reset_ep,
 			"Queueing reset endpoint command");
 	spin_lock_irqsave(&xhci->lock, flags);
-	ret = xhci_queue_reset_ep(xhci, udev->slot_id, ep_index);
+	ret = xhci_queue_reset_ep(xhci, command, udev->slot_id, ep_index);
 	/*
 	 * Can't change the ring dequeue pointer until it's transitioned to the
 	 * stopped state, which is only upon a successful reset endpoint
@@ -3416,7 +3433,6 @@ int xhci_discover_or_reset_device(struct usb_hcd *hcd, struct usb_device *udev)
 	unsigned int slot_id;
 	struct xhci_virt_device *virt_dev;
 	struct xhci_command *reset_device_cmd;
-	int timeleft;
 	int last_freed_endpoint;
 	struct xhci_slot_ctx *slot_ctx;
 	int old_active_eps = 0;
@@ -3473,13 +3489,10 @@ int xhci_discover_or_reset_device(struct usb_hcd *hcd, struct usb_device *udev)
 
 	/* Attempt to submit the Reset Device command to the command ring */
 	spin_lock_irqsave(&xhci->lock, flags);
-	reset_device_cmd->command_trb = xhci_find_next_enqueue(xhci->cmd_ring);
 
-	list_add_tail(&reset_device_cmd->cmd_list, &virt_dev->cmd_list);
-	ret = xhci_queue_reset_device(xhci, slot_id);
+	ret = xhci_queue_reset_device(xhci, reset_device_cmd, slot_id);
 	if (ret) {
 		xhci_dbg(xhci, "FIXME: allocate a command ring segment\n");
-		list_del(&reset_device_cmd->cmd_list);
 		spin_unlock_irqrestore(&xhci->lock, flags);
 		goto command_cleanup;
 	}
@@ -3487,22 +3500,7 @@ int xhci_discover_or_reset_device(struct usb_hcd *hcd, struct usb_device *udev)
 	spin_unlock_irqrestore(&xhci->lock, flags);
 
 	/* Wait for the Reset Device command to finish */
-	timeleft = wait_for_completion_interruptible_timeout(
-			reset_device_cmd->completion,
-			XHCI_CMD_DEFAULT_TIMEOUT);
-	if (timeleft <= 0) {
-		xhci_warn(xhci, "%s while waiting for reset device command\n",
-				timeleft == 0 ? "Timeout" : "Signal");
-		spin_lock_irqsave(&xhci->lock, flags);
-		/* The timeout might have raced with the event ring handler, so
-		 * only delete from the list if the item isn't poisoned.
-		 */
-		if (reset_device_cmd->cmd_list.next != LIST_POISON1)
-			list_del(&reset_device_cmd->cmd_list);
-		spin_unlock_irqrestore(&xhci->lock, flags);
-		ret = -ETIME;
-		goto command_cleanup;
-	}
+	wait_for_completion(reset_device_cmd->completion);
 
 	/* The Reset Device command can't fail, according to the 0.95/0.96 spec,
 	 * unless we tried to reset a slot ID that wasn't enabled,
@@ -3510,6 +3508,11 @@ int xhci_discover_or_reset_device(struct usb_hcd *hcd, struct usb_device *udev)
 	 */
 	ret = reset_device_cmd->status;
 	switch (ret) {
+	case COMP_CMD_ABORT:
+	case COMP_CMD_STOP:
+		xhci_warn(xhci, "Timeout waiting for reset device command\n");
+		ret = -ETIME;
+		goto command_cleanup;
 	case COMP_EBADSLT: /* 0.95 completion code for bad slot ID */
 	case COMP_CTX_STATE: /* 0.96 completion code for same thing */
 		xhci_dbg(xhci, "Can't reset device (slot ID %u) in %s state\n",
@@ -3589,6 +3592,11 @@ void xhci_free_dev(struct usb_hcd *hcd, struct usb_device *udev)
 	unsigned long flags;
 	u32 state;
 	int i, ret;
+	struct xhci_command *command;
+
+	command = xhci_alloc_command(xhci, false, false, GFP_KERNEL);
+	if (!command)
+		return;
 
 #ifndef CONFIG_USB_DEFAULT_PERSIST
 	/*
@@ -3604,8 +3612,10 @@ void xhci_free_dev(struct usb_hcd *hcd, struct usb_device *udev)
 	/* If the host is halted due to driver unload, we still need to free the
 	 * device.
 	 */
-	if (ret <= 0 && ret != -ENODEV)
+	if (ret <= 0 && ret != -ENODEV) {
+		kfree(command);
 		return;
+	}
 
 	virt_dev = xhci->devs[udev->slot_id];
 
@@ -3622,16 +3632,19 @@ void xhci_free_dev(struct usb_hcd *hcd, struct usb_device *udev)
 			(xhci->xhc_state & XHCI_STATE_HALTED)) {
 		xhci_free_virt_device(xhci, udev->slot_id);
 		spin_unlock_irqrestore(&xhci->lock, flags);
+		kfree(command);
 		return;
 	}
 
-	if (xhci_queue_slot_control(xhci, TRB_DISABLE_SLOT, udev->slot_id)) {
+	if (xhci_queue_slot_control(xhci, command, TRB_DISABLE_SLOT,
+				    udev->slot_id)) {
 		spin_unlock_irqrestore(&xhci->lock, flags);
 		xhci_dbg(xhci, "FIXME: allocate a command ring segment\n");
 		return;
 	}
 	xhci_ring_cmd_db(xhci);
 	spin_unlock_irqrestore(&xhci->lock, flags);
+
 	/*
 	 * Event command completion handler will free any data structures
 	 * associated with the slot.  XXX Can free sleep?
@@ -3669,33 +3682,33 @@ int xhci_alloc_dev(struct usb_hcd *hcd, struct usb_device *udev)
 {
 	struct xhci_hcd *xhci = hcd_to_xhci(hcd);
 	unsigned long flags;
-	int timeleft;
 	int ret;
-	union xhci_trb *cmd_trb;
+	struct xhci_command *command;
+
+	command = xhci_alloc_command(xhci, false, false, GFP_KERNEL);
+	if (!command)
+		return 0;
 
 	spin_lock_irqsave(&xhci->lock, flags);
-	cmd_trb = xhci_find_next_enqueue(xhci->cmd_ring);
-	ret = xhci_queue_slot_control(xhci, TRB_ENABLE_SLOT, 0);
+	command->completion = &xhci->addr_dev;
+	ret = xhci_queue_slot_control(xhci, command, TRB_ENABLE_SLOT, 0);
 	if (ret) {
 		spin_unlock_irqrestore(&xhci->lock, flags);
 		xhci_dbg(xhci, "FIXME: allocate a command ring segment\n");
+		kfree(command);
 		return 0;
 	}
 	xhci_ring_cmd_db(xhci);
 	spin_unlock_irqrestore(&xhci->lock, flags);
 
-	/* XXX: how much time for xHC slot assignment? */
-	timeleft = wait_for_completion_interruptible_timeout(&xhci->addr_dev,
-			XHCI_CMD_DEFAULT_TIMEOUT);
-	if (timeleft <= 0) {
-		xhci_warn(xhci, "%s while waiting for a slot\n",
-				timeleft == 0 ? "Timeout" : "Signal");
-		/* cancel the enable slot request */
-		return xhci_cancel_cmd(xhci, NULL, cmd_trb);
-	}
+	wait_for_completion(command->completion);
 
-	if (!xhci->slot_id) {
+	if (!xhci->slot_id || command->status != COMP_SUCCESS) {
 		xhci_err(xhci, "Error while assigning device slot ID\n");
+		xhci_err(xhci, "Max number of devices this xHCI host supports is %u.\n",
+				HCS_MAX_SLOTS(
+					readl(&xhci->cap_regs->hcs_params1)));
+		kfree(command);
 		return 0;
 	}
 
@@ -3730,6 +3743,8 @@ int xhci_alloc_dev(struct usb_hcd *hcd, struct usb_device *udev)
 		pm_runtime_get_noresume(hcd->self.controller);
 #endif
 
+
+	kfree(command);
 	/* Is this a LS or FS device under a HS hub? */
 	/* Hub or peripherial? */
 	return 1;
@@ -3737,7 +3752,10 @@ int xhci_alloc_dev(struct usb_hcd *hcd, struct usb_device *udev)
 disable_slot:
 	/* Disable slot, if we can do it without mem alloc */
 	spin_lock_irqsave(&xhci->lock, flags);
-	if (!xhci_queue_slot_control(xhci, TRB_DISABLE_SLOT, udev->slot_id))
+	command->completion = NULL;
+	command->status = 0;
+	if (!xhci_queue_slot_control(xhci, command, TRB_DISABLE_SLOT,
+				     udev->slot_id))
 		xhci_ring_cmd_db(xhci);
 	spin_unlock_irqrestore(&xhci->lock, flags);
 	return 0;
@@ -3754,14 +3772,13 @@ static int xhci_setup_device(struct usb_hcd *hcd, struct usb_device *udev,
 {
 	const char *act = setup == SETUP_CONTEXT_ONLY ? "context" : "address";
 	unsigned long flags;
-	int timeleft;
 	struct xhci_virt_device *virt_dev;
 	int ret = 0;
 	struct xhci_hcd *xhci = hcd_to_xhci(hcd);
 	struct xhci_slot_ctx *slot_ctx;
 	struct xhci_input_control_ctx *ctrl_ctx;
 	u64 temp_64;
-	union xhci_trb *cmd_trb;
+	struct xhci_command *command;
 
 	if (!udev->slot_id) {
 		xhci_dbg_trace(xhci, trace_xhci_dbg_address,
@@ -3782,11 +3799,19 @@ static int xhci_setup_device(struct usb_hcd *hcd, struct usb_device *udev,
 		return -EINVAL;
 	}
 
+	command = xhci_alloc_command(xhci, false, false, GFP_KERNEL);
+	if (!command)
+		return -ENOMEM;
+
+	command->in_ctx = virt_dev->in_ctx;
+	command->completion = &xhci->addr_dev;
+
 	slot_ctx = xhci_get_slot_ctx(xhci, virt_dev->in_ctx);
 	ctrl_ctx = xhci_get_input_control_ctx(xhci, virt_dev->in_ctx);
 	if (!ctrl_ctx) {
 		xhci_warn(xhci, "%s: Could not get input context, bad type.\n",
 				__func__);
+		kfree(command);
 		return -EINVAL;
 	}
 	/*
@@ -3808,36 +3833,31 @@ static int xhci_setup_device(struct usb_hcd *hcd, struct usb_device *udev,
 				le32_to_cpu(slot_ctx->dev_info) >> 27);
 
 	spin_lock_irqsave(&xhci->lock, flags);
-	cmd_trb = xhci_find_next_enqueue(xhci->cmd_ring);
-	ret = xhci_queue_address_device(xhci, virt_dev->in_ctx->dma,
+	ret = xhci_queue_address_device(xhci, command, virt_dev->in_ctx->dma,
 					udev->slot_id, setup);
 	if (ret) {
 		spin_unlock_irqrestore(&xhci->lock, flags);
 		xhci_dbg_trace(xhci, trace_xhci_dbg_address,
 				"FIXME: allocate a command ring segment");
+		kfree(command);
 		return ret;
 	}
 	xhci_ring_cmd_db(xhci);
 	spin_unlock_irqrestore(&xhci->lock, flags);
 
 	/* ctrl tx can take up to 5 sec; XXX: need more time for xHC? */
-	timeleft = wait_for_completion_interruptible_timeout(&xhci->addr_dev,
-			XHCI_CMD_DEFAULT_TIMEOUT);
+	wait_for_completion(command->completion);
+
 	/* FIXME: From section 4.3.4: "Software shall be responsible for timing
 	 * the SetAddress() "recovery interval" required by USB and aborting the
 	 * command on a timeout.
 	 */
-	if (timeleft <= 0) {
-		xhci_warn(xhci, "%s while waiting for setup %s command\n",
-			  timeleft == 0 ? "Timeout" : "Signal", act);
-		/* cancel the address device command */
-		ret = xhci_cancel_cmd(xhci, NULL, cmd_trb);
-		if (ret < 0)
-			return ret;
-		return -ETIME;
-	}
-
-	switch (virt_dev->cmd_status) {
+	switch (command->status) {
+	case COMP_CMD_ABORT:
+	case COMP_CMD_STOP:
+		xhci_warn(xhci, "Timeout while waiting for setup device command\n");
+		ret = -ETIME;
+		break;
 	case COMP_CTX_STATE:
 	case COMP_EBADSLT:
 		xhci_err(xhci, "Setup ERROR: setup %s command for slot %d.\n",
@@ -3860,7 +3880,7 @@ static int xhci_setup_device(struct usb_hcd *hcd, struct usb_device *udev,
 	default:
 		xhci_err(xhci,
 			 "ERROR: unexpected setup %s command completion code 0x%x.\n",
-			 act, virt_dev->cmd_status);
+			 act, command->status);
 		xhci_dbg(xhci, "Slot ID %d Output Context:\n", udev->slot_id);
 		xhci_dbg_ctx(xhci, virt_dev->out_ctx, 2);
 		trace_xhci_address_ctx(xhci, virt_dev->out_ctx, 1);
@@ -3868,6 +3888,7 @@ static int xhci_setup_device(struct usb_hcd *hcd, struct usb_device *udev,
 		break;
 	}
 	if (ret) {
+		kfree(command);
 		return ret;
 	}
 	temp_64 = xhci_read_64(xhci, &xhci->op_regs->dcbaa_ptr);
@@ -3902,7 +3923,7 @@ static int xhci_setup_device(struct usb_hcd *hcd, struct usb_device *udev,
 	xhci_dbg_trace(xhci, trace_xhci_dbg_address,
 		       "Internal device address = %d",
 		       le32_to_cpu(slot_ctx->dev_state) & DEV_ADDR_MASK);
-
+	kfree(command);
 	return 0;
 }
 
@@ -4092,7 +4113,7 @@ int xhci_set_usb2_hardware_lpm(struct usb_hcd *hcd,
 	field = le32_to_cpu(udev->bos->ext_cap->bmAttributes);
 
 	xhci_dbg(xhci, "%s port %d USB2 hardware LPM\n",
-			enable ? "enable" : "disable", port_num);
+			enable ? "enable" : "disable", port_num + 1);
 
 	if (enable) {
 		/* Host supports BESL timeout instead of HIRD */
diff --git a/drivers/usb/host/xhci.h b/drivers/usb/host/xhci.h
index 4746816aed3..9ffecd56600 100644
--- a/drivers/usb/host/xhci.h
+++ b/drivers/usb/host/xhci.h
@@ -937,9 +937,6 @@ struct xhci_virt_device {
 #define	XHCI_MAX_RINGS_CACHED	31
 	struct xhci_virt_ep		eps[31];
 	struct completion		cmd_completion;
-	/* Status of the last command issued for this device */
-	u32				cmd_status;
-	struct list_head		cmd_list;
 	u8				fake_port;
 	u8				real_port;
 	struct xhci_interval_bw_table	*bw_table;
@@ -1298,7 +1295,6 @@ struct xhci_td {
 
 /* command descriptor */
 struct xhci_cd {
-	struct list_head	cancel_cmd_list;
 	struct xhci_command	*command;
 	union xhci_trb		*cmd_trb;
 };
@@ -1476,6 +1472,8 @@ struct xhci_hcd {
 	/* msi-x vectors */
 	int		msix_count;
 	struct msix_entry	*msix_entries;
+	/* optional clock */
+	struct clk		*clk;
 	/* data structures */
 	struct xhci_device_context_array *dcbaa;
 	struct xhci_ring	*cmd_ring;
@@ -1483,8 +1481,10 @@ struct xhci_hcd {
 #define CMD_RING_STATE_RUNNING         (1 << 0)
 #define CMD_RING_STATE_ABORTED         (1 << 1)
 #define CMD_RING_STATE_STOPPED         (1 << 2)
-	struct list_head        cancel_cmd_list;
+	struct list_head        cmd_list;
 	unsigned int		cmd_ring_reserved_trbs;
+	struct timer_list	cmd_timer;
+	struct xhci_command	*current_cmd;
 	struct xhci_ring	*event_ring;
 	struct xhci_erst	erst;
 	/* Scratchpad */
@@ -1738,8 +1738,7 @@ static inline int xhci_register_pci(void) { return 0; }
 static inline void xhci_unregister_pci(void) {}
 #endif
 
-#if defined(CONFIG_USB_XHCI_PLATFORM) \
-	|| defined(CONFIG_USB_XHCI_PLATFORM_MODULE)
+#if IS_ENABLED(CONFIG_USB_XHCI_PLATFORM)
 int xhci_register_plat(void);
 void xhci_unregister_plat(void);
 #else
@@ -1808,13 +1807,14 @@ struct xhci_segment *trb_in_td(struct xhci_segment *start_seg,
 		dma_addr_t suspect_dma);
 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, u32 trb_type, u32 slot_id);
-int xhci_queue_address_device(struct xhci_hcd *xhci, dma_addr_t in_ctx_ptr,
-		u32 slot_id, enum xhci_setup_dev);
-int xhci_queue_vendor_command(struct xhci_hcd *xhci,
+int xhci_queue_slot_control(struct xhci_hcd *xhci, struct xhci_command *cmd,
+		u32 trb_type, u32 slot_id);
+int xhci_queue_address_device(struct xhci_hcd *xhci, struct xhci_command *cmd,
+		dma_addr_t in_ctx_ptr, u32 slot_id, enum xhci_setup_dev);
+int xhci_queue_vendor_command(struct xhci_hcd *xhci, struct xhci_command *cmd,
 		u32 field1, u32 field2, u32 field3, u32 field4);
-int xhci_queue_stop_endpoint(struct xhci_hcd *xhci, int slot_id,
-		unsigned int ep_index, int suspend);
+int xhci_queue_stop_endpoint(struct xhci_hcd *xhci, struct xhci_command *cmd,
+		int slot_id, unsigned int ep_index, int suspend);
 int xhci_queue_ctrl_tx(struct xhci_hcd *xhci, gfp_t mem_flags, struct urb *urb,
 		int slot_id, unsigned int ep_index);
 int xhci_queue_bulk_tx(struct xhci_hcd *xhci, gfp_t mem_flags, struct urb *urb,
@@ -1823,18 +1823,21 @@ int xhci_queue_intr_tx(struct xhci_hcd *xhci, gfp_t mem_flags, struct urb *urb,
 		int slot_id, unsigned int ep_index);
 int xhci_queue_isoc_tx_prepare(struct xhci_hcd *xhci, gfp_t mem_flags,
 		struct urb *urb, int slot_id, unsigned int ep_index);
-int xhci_queue_configure_endpoint(struct xhci_hcd *xhci, dma_addr_t in_ctx_ptr,
-		u32 slot_id, bool command_must_succeed);
-int xhci_queue_evaluate_context(struct xhci_hcd *xhci, dma_addr_t in_ctx_ptr,
-		u32 slot_id, bool command_must_succeed);
-int xhci_queue_reset_ep(struct xhci_hcd *xhci, int slot_id,
-		unsigned int ep_index);
-int xhci_queue_reset_device(struct xhci_hcd *xhci, u32 slot_id);
+int xhci_queue_configure_endpoint(struct xhci_hcd *xhci,
+		struct xhci_command *cmd, dma_addr_t in_ctx_ptr, u32 slot_id,
+		bool command_must_succeed);
+int xhci_queue_evaluate_context(struct xhci_hcd *xhci, struct xhci_command *cmd,
+		dma_addr_t in_ctx_ptr, u32 slot_id, bool command_must_succeed);
+int xhci_queue_reset_ep(struct xhci_hcd *xhci, struct xhci_command *cmd,
+		int slot_id, unsigned int ep_index);
+int xhci_queue_reset_device(struct xhci_hcd *xhci, struct xhci_command *cmd,
+		u32 slot_id);
 void xhci_find_new_dequeue_state(struct xhci_hcd *xhci,
 		unsigned int slot_id, unsigned int ep_index,
 		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);
@@ -1844,11 +1847,11 @@ void xhci_queue_config_ep_quirk(struct xhci_hcd *xhci,
 		unsigned int slot_id, unsigned int ep_index,
 		struct xhci_dequeue_state *deq_state);
 void xhci_stop_endpoint_command_watchdog(unsigned long arg);
-int xhci_cancel_cmd(struct xhci_hcd *xhci, struct xhci_command *command,
-		union xhci_trb *cmd_trb);
+void xhci_handle_command_timeout(unsigned long data);
+
 void xhci_ring_ep_doorbell(struct xhci_hcd *xhci, unsigned int slot_id,
 		unsigned int ep_index, unsigned int stream_id);
-union xhci_trb *xhci_find_next_enqueue(struct xhci_ring *ring);
+void xhci_cleanup_command_queue(struct xhci_hcd *xhci);
 
 /* xHCI roothub code */
 void xhci_set_link_state(struct xhci_hcd *xhci, __le32 __iomem **port_array,
diff --git a/drivers/usb/misc/appledisplay.c b/drivers/usb/misc/appledisplay.c
index ba6a5d6e618..b3d245ef46e 100644
--- a/drivers/usb/misc/appledisplay.c
+++ b/drivers/usb/misc/appledisplay.c
@@ -81,6 +81,7 @@ struct appledisplay {
 	struct delayed_work work;
 	int button_pressed;
 	spinlock_t lock;
+	struct mutex sysfslock;		/* concurrent read and write */
 };
 
 static atomic_t count_displays = ATOMIC_INIT(0);
@@ -110,7 +111,7 @@ static void appledisplay_complete(struct urb *urb)
 			__func__, status);
 		return;
 	default:
-		dev_dbg(dev, "%s - nonzero urb status received: %d/n",
+		dev_dbg(dev, "%s - nonzero urb status received: %d\n",
 			__func__, status);
 		goto exit;
 	}
@@ -144,6 +145,7 @@ static int appledisplay_bl_update_status(struct backlight_device *bd)
 	struct appledisplay *pdata = bl_get_data(bd);
 	int retval;
 
+	mutex_lock(&pdata->sysfslock);
 	pdata->msgdata[0] = 0x10;
 	pdata->msgdata[1] = bd->props.brightness;
 
@@ -156,15 +158,17 @@ static int appledisplay_bl_update_status(struct backlight_device *bd)
 		0,
 		pdata->msgdata, 2,
 		ACD_USB_TIMEOUT);
-
+	mutex_unlock(&pdata->sysfslock);
+	
 	return retval;
 }
 
 static int appledisplay_bl_get_brightness(struct backlight_device *bd)
 {
 	struct appledisplay *pdata = bl_get_data(bd);
-	int retval;
+	int retval, brightness;
 
+	mutex_lock(&pdata->sysfslock);
 	retval = usb_control_msg(
 		pdata->udev,
 		usb_rcvctrlpipe(pdata->udev, 0),
@@ -174,11 +178,13 @@ static int appledisplay_bl_get_brightness(struct backlight_device *bd)
 		0,
 		pdata->msgdata, 2,
 		ACD_USB_TIMEOUT);
+	brightness = pdata->msgdata[1];
+	mutex_unlock(&pdata->sysfslock);
 
 	if (retval < 0)
 		return retval;
 	else
-		return pdata->msgdata[1];
+		return brightness;
 }
 
 static const struct backlight_ops appledisplay_bl_data = {
@@ -241,6 +247,7 @@ static int appledisplay_probe(struct usb_interface *iface,
 
 	spin_lock_init(&pdata->lock);
 	INIT_DELAYED_WORK(&pdata->work, appledisplay_work);
+	mutex_init(&pdata->sysfslock);
 
 	/* Allocate buffer for control messages */
 	pdata->msgdata = kmalloc(ACD_MSG_BUFFER_LEN, GFP_KERNEL);
diff --git a/drivers/usb/misc/ftdi-elan.c b/drivers/usb/misc/ftdi-elan.c
index a4a3c7cd4a1..8ab1f8f3c26 100644
--- a/drivers/usb/misc/ftdi-elan.c
+++ b/drivers/usb/misc/ftdi-elan.c
@@ -1,40 +1,43 @@
 /*
-* USB FTDI client driver for Elan Digital Systems's Uxxx adapters
-*
-* Copyright(C) 2006 Elan Digital Systems Limited
-* http://www.elandigitalsystems.com
-*
-* Author and Maintainer - Tony Olech - Elan Digital Systems
-* tony.olech@elandigitalsystems.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 driver was written by Tony Olech(tony.olech@elandigitalsystems.com)
-* based on various USB client drivers in the 2.6.15 linux kernel
-* with constant reference to the 3rd Edition of Linux Device Drivers
-* published by O'Reilly
-*
-* The U132 adapter is a USB to CardBus adapter specifically designed
-* for PC cards that contain an OHCI host controller. Typical PC cards
-* are the Orange Mobile 3G Option GlobeTrotter Fusion card.
-*
-* The U132 adapter will *NOT *work with PC cards that do not contain
-* an OHCI controller. A simple way to test whether a PC card has an
-* OHCI controller as an interface is to insert the PC card directly
-* into a laptop(or desktop) with a CardBus slot and if "lspci" shows
-* a new USB controller and "lsusb -v" shows a new OHCI Host Controller
-* then there is a good chance that the U132 adapter will support the
-* PC card.(you also need the specific client driver for the PC card)
-*
-* Please inform the Author and Maintainer about any PC cards that
-* contain OHCI Host Controller and work when directly connected to
-* an embedded CardBus slot but do not work when they are connected
-* via an ELAN U132 adapter.
-*
-*/
+ * USB FTDI client driver for Elan Digital Systems's Uxxx adapters
+ *
+ * Copyright(C) 2006 Elan Digital Systems Limited
+ * http://www.elandigitalsystems.com
+ *
+ * Author and Maintainer - Tony Olech - Elan Digital Systems
+ * tony.olech@elandigitalsystems.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 driver was written by Tony Olech(tony.olech@elandigitalsystems.com)
+ * based on various USB client drivers in the 2.6.15 linux kernel
+ * with constant reference to the 3rd Edition of Linux Device Drivers
+ * published by O'Reilly
+ *
+ * The U132 adapter is a USB to CardBus adapter specifically designed
+ * for PC cards that contain an OHCI host controller. Typical PC cards
+ * are the Orange Mobile 3G Option GlobeTrotter Fusion card.
+ *
+ * The U132 adapter will *NOT *work with PC cards that do not contain
+ * an OHCI controller. A simple way to test whether a PC card has an
+ * OHCI controller as an interface is to insert the PC card directly
+ * into a laptop(or desktop) with a CardBus slot and if "lspci" shows
+ * a new USB controller and "lsusb -v" shows a new OHCI Host Controller
+ * then there is a good chance that the U132 adapter will support the
+ * PC card.(you also need the specific client driver for the PC card)
+ *
+ * Please inform the Author and Maintainer about any PC cards that
+ * contain OHCI Host Controller and work when directly connected to
+ * an embedded CardBus slot but do not work when they are connected
+ * via an ELAN U132 adapter.
+ *
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/kernel.h>
 #include <linux/errno.h>
 #include <linux/init.h>
@@ -55,31 +58,31 @@ MODULE_LICENSE("GPL");
 #define INT_MODULE_PARM(n, v) static int n = v;module_param(n, int, 0444)
 static bool distrust_firmware = 1;
 module_param(distrust_firmware, bool, 0);
-MODULE_PARM_DESC(distrust_firmware, "true to distrust firmware power/overcurren"
-        "t setup");
+MODULE_PARM_DESC(distrust_firmware,
+		 "true to distrust firmware power/overcurrent setup");
 extern struct platform_driver u132_platform_driver;
 static struct workqueue_struct *status_queue;
 static struct workqueue_struct *command_queue;
 static struct workqueue_struct *respond_queue;
 /*
-* ftdi_module_lock exists to protect access to global variables
-*
-*/
+ * ftdi_module_lock exists to protect access to global variables
+ *
+ */
 static struct mutex ftdi_module_lock;
 static int ftdi_instances = 0;
 static struct list_head ftdi_static_list;
 /*
-* end of the global variables protected by ftdi_module_lock
-*/
+ * end of the global variables protected by ftdi_module_lock
+ */
 #include "usb_u132.h"
 #include <asm/io.h>
 #include <linux/usb/hcd.h>
 
-	/* FIXME ohci.h is ONLY for internal use by the OHCI driver.
-	 * If you're going to try stuff like this, you need to split
-	 * out shareable stuff (register declarations?) into its own
-	 * file, maybe name <linux/usb/ohci.h>
-	 */
+/* FIXME ohci.h is ONLY for internal use by the OHCI driver.
+ * If you're going to try stuff like this, you need to split
+ * out shareable stuff (register declarations?) into its own
+ * file, maybe name <linux/usb/ohci.h>
+ */
 
 #include "../host/ohci.h"
 /* Define these values to match your devices*/
@@ -87,140 +90,140 @@ static struct list_head ftdi_static_list;
 #define USB_FTDI_ELAN_PRODUCT_ID 0xd6ea
 /* table of devices that work with this driver*/
 static const struct usb_device_id ftdi_elan_table[] = {
-        {USB_DEVICE(USB_FTDI_ELAN_VENDOR_ID, USB_FTDI_ELAN_PRODUCT_ID)},
-        { /* Terminating entry */ }
+	{USB_DEVICE(USB_FTDI_ELAN_VENDOR_ID, USB_FTDI_ELAN_PRODUCT_ID)},
+	{ /* Terminating entry */ }
 };
 
 MODULE_DEVICE_TABLE(usb, ftdi_elan_table);
 /* only the jtag(firmware upgrade device) interface requires
-* a device file and corresponding minor number, but the
-* interface is created unconditionally - I suppose it could
-* be configured or not according to a module parameter.
-* But since we(now) require one interface per device,
-* and since it unlikely that a normal installation would
-* require more than a couple of elan-ftdi devices, 8 seems
-* like a reasonable limit to have here, and if someone
-* really requires more than 8 devices, then they can frig the
-* code and recompile
-*/
+ * a device file and corresponding minor number, but the
+ * interface is created unconditionally - I suppose it could
+ * be configured or not according to a module parameter.
+ * But since we(now) require one interface per device,
+ * and since it unlikely that a normal installation would
+ * require more than a couple of elan-ftdi devices, 8 seems
+ * like a reasonable limit to have here, and if someone
+ * really requires more than 8 devices, then they can frig the
+ * code and recompile
+ */
 #define USB_FTDI_ELAN_MINOR_BASE 192
 #define COMMAND_BITS 5
 #define COMMAND_SIZE (1<<COMMAND_BITS)
 #define COMMAND_MASK (COMMAND_SIZE-1)
 struct u132_command {
-        u8 header;
-        u16 length;
-        u8 address;
-        u8 width;
-        u32 value;
-        int follows;
-        void *buffer;
+	u8 header;
+	u16 length;
+	u8 address;
+	u8 width;
+	u32 value;
+	int follows;
+	void *buffer;
 };
 #define RESPOND_BITS 5
 #define RESPOND_SIZE (1<<RESPOND_BITS)
 #define RESPOND_MASK (RESPOND_SIZE-1)
 struct u132_respond {
-        u8 header;
-        u8 address;
-        u32 *value;
-        int *result;
-        struct completion wait_completion;
+	u8 header;
+	u8 address;
+	u32 *value;
+	int *result;
+	struct completion wait_completion;
 };
 struct u132_target {
-        void *endp;
-        struct urb *urb;
-        int toggle_bits;
-        int error_count;
-        int condition_code;
-        int repeat_number;
-        int halted;
-        int skipped;
-        int actual;
-        int non_null;
-        int active;
-        int abandoning;
-        void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
-                int toggle_bits, int error_count, int condition_code,
-                int repeat_number, int halted, int skipped, int actual,
-                int non_null);
+	void *endp;
+	struct urb *urb;
+	int toggle_bits;
+	int error_count;
+	int condition_code;
+	int repeat_number;
+	int halted;
+	int skipped;
+	int actual;
+	int non_null;
+	int active;
+	int abandoning;
+	void (*callback)(void *endp, struct urb *urb, u8 *buf, int len,
+			 int toggle_bits, int error_count, int condition_code,
+			 int repeat_number, int halted, int skipped, int actual,
+			 int non_null);
 };
 /* Structure to hold all of our device specific stuff*/
 struct usb_ftdi {
-        struct list_head ftdi_list;
-        struct mutex u132_lock;
-        int command_next;
-        int command_head;
-        struct u132_command command[COMMAND_SIZE];
-        int respond_next;
-        int respond_head;
-        struct u132_respond respond[RESPOND_SIZE];
-        struct u132_target target[4];
-        char device_name[16];
-        unsigned synchronized:1;
-        unsigned enumerated:1;
-        unsigned registered:1;
-        unsigned initialized:1;
-        unsigned card_ejected:1;
-        int function;
-        int sequence_num;
-        int disconnected;
-        int gone_away;
-        int stuck_status;
-        int status_queue_delay;
-        struct semaphore sw_lock;
-        struct usb_device *udev;
-        struct usb_interface *interface;
-        struct usb_class_driver *class;
-        struct delayed_work status_work;
-        struct delayed_work command_work;
-        struct delayed_work respond_work;
-        struct u132_platform_data platform_data;
-        struct resource resources[0];
-        struct platform_device platform_dev;
-        unsigned char *bulk_in_buffer;
-        size_t bulk_in_size;
-        size_t bulk_in_last;
-        size_t bulk_in_left;
-        __u8 bulk_in_endpointAddr;
-        __u8 bulk_out_endpointAddr;
-        struct kref kref;
-        u32 controlreg;
-        u8 response[4 + 1024];
-        int expected;
-        int received;
-        int ed_found;
+	struct list_head ftdi_list;
+	struct mutex u132_lock;
+	int command_next;
+	int command_head;
+	struct u132_command command[COMMAND_SIZE];
+	int respond_next;
+	int respond_head;
+	struct u132_respond respond[RESPOND_SIZE];
+	struct u132_target target[4];
+	char device_name[16];
+	unsigned synchronized:1;
+	unsigned enumerated:1;
+	unsigned registered:1;
+	unsigned initialized:1;
+	unsigned card_ejected:1;
+	int function;
+	int sequence_num;
+	int disconnected;
+	int gone_away;
+	int stuck_status;
+	int status_queue_delay;
+	struct semaphore sw_lock;
+	struct usb_device *udev;
+	struct usb_interface *interface;
+	struct usb_class_driver *class;
+	struct delayed_work status_work;
+	struct delayed_work command_work;
+	struct delayed_work respond_work;
+	struct u132_platform_data platform_data;
+	struct resource resources[0];
+	struct platform_device platform_dev;
+	unsigned char *bulk_in_buffer;
+	size_t bulk_in_size;
+	size_t bulk_in_last;
+	size_t bulk_in_left;
+	__u8 bulk_in_endpointAddr;
+	__u8 bulk_out_endpointAddr;
+	struct kref kref;
+	u32 controlreg;
+	u8 response[4 + 1024];
+	int expected;
+	int received;
+	int ed_found;
 };
 #define kref_to_usb_ftdi(d) container_of(d, struct usb_ftdi, kref)
 #define platform_device_to_usb_ftdi(d) container_of(d, struct usb_ftdi, \
-        platform_dev)
+						    platform_dev)
 static struct usb_driver ftdi_elan_driver;
 static void ftdi_elan_delete(struct kref *kref)
 {
-        struct usb_ftdi *ftdi = kref_to_usb_ftdi(kref);
-        dev_warn(&ftdi->udev->dev, "FREEING ftdi=%p\n", ftdi);
-        usb_put_dev(ftdi->udev);
-        ftdi->disconnected += 1;
-        mutex_lock(&ftdi_module_lock);
-        list_del_init(&ftdi->ftdi_list);
-        ftdi_instances -= 1;
-        mutex_unlock(&ftdi_module_lock);
-        kfree(ftdi->bulk_in_buffer);
-        ftdi->bulk_in_buffer = NULL;
+	struct usb_ftdi *ftdi = kref_to_usb_ftdi(kref);
+	dev_warn(&ftdi->udev->dev, "FREEING ftdi=%p\n", ftdi);
+	usb_put_dev(ftdi->udev);
+	ftdi->disconnected += 1;
+	mutex_lock(&ftdi_module_lock);
+	list_del_init(&ftdi->ftdi_list);
+	ftdi_instances -= 1;
+	mutex_unlock(&ftdi_module_lock);
+	kfree(ftdi->bulk_in_buffer);
+	ftdi->bulk_in_buffer = NULL;
 }
 
 static void ftdi_elan_put_kref(struct usb_ftdi *ftdi)
 {
-        kref_put(&ftdi->kref, ftdi_elan_delete);
+	kref_put(&ftdi->kref, ftdi_elan_delete);
 }
 
 static void ftdi_elan_get_kref(struct usb_ftdi *ftdi)
 {
-        kref_get(&ftdi->kref);
+	kref_get(&ftdi->kref);
 }
 
 static void ftdi_elan_init_kref(struct usb_ftdi *ftdi)
 {
-        kref_init(&ftdi->kref);
+	kref_init(&ftdi->kref);
 }
 
 static void ftdi_status_requeue_work(struct usb_ftdi *ftdi, unsigned int delta)
@@ -237,8 +240,8 @@ static void ftdi_status_queue_work(struct usb_ftdi *ftdi, unsigned int delta)
 
 static void ftdi_status_cancel_work(struct usb_ftdi *ftdi)
 {
-        if (cancel_delayed_work(&ftdi->status_work))
-                kref_put(&ftdi->kref, ftdi_elan_delete);
+	if (cancel_delayed_work(&ftdi->status_work))
+		kref_put(&ftdi->kref, ftdi_elan_delete);
 }
 
 static void ftdi_command_requeue_work(struct usb_ftdi *ftdi, unsigned int delta)
@@ -255,12 +258,12 @@ static void ftdi_command_queue_work(struct usb_ftdi *ftdi, unsigned int delta)
 
 static void ftdi_command_cancel_work(struct usb_ftdi *ftdi)
 {
-        if (cancel_delayed_work(&ftdi->command_work))
-                kref_put(&ftdi->kref, ftdi_elan_delete);
+	if (cancel_delayed_work(&ftdi->command_work))
+		kref_put(&ftdi->kref, ftdi_elan_delete);
 }
 
 static void ftdi_response_requeue_work(struct usb_ftdi *ftdi,
-        unsigned int delta)
+				       unsigned int delta)
 {
 	if (!queue_delayed_work(respond_queue, &ftdi->respond_work, delta))
 		kref_put(&ftdi->kref, ftdi_elan_delete);
@@ -274,26 +277,26 @@ static void ftdi_respond_queue_work(struct usb_ftdi *ftdi, unsigned int delta)
 
 static void ftdi_response_cancel_work(struct usb_ftdi *ftdi)
 {
-        if (cancel_delayed_work(&ftdi->respond_work))
-                kref_put(&ftdi->kref, ftdi_elan_delete);
+	if (cancel_delayed_work(&ftdi->respond_work))
+		kref_put(&ftdi->kref, ftdi_elan_delete);
 }
 
 void ftdi_elan_gone_away(struct platform_device *pdev)
 {
-        struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
-        ftdi->gone_away += 1;
-        ftdi_elan_put_kref(ftdi);
+	struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
+	ftdi->gone_away += 1;
+	ftdi_elan_put_kref(ftdi);
 }
 
 
 EXPORT_SYMBOL_GPL(ftdi_elan_gone_away);
 static void ftdi_release_platform_dev(struct device *dev)
 {
-        dev->parent = NULL;
+	dev->parent = NULL;
 }
 
 static void ftdi_elan_do_callback(struct usb_ftdi *ftdi,
-        struct u132_target *target, u8 *buffer, int length);
+				  struct u132_target *target, u8 *buffer, int length);
 static void ftdi_elan_kick_command_queue(struct usb_ftdi *ftdi);
 static void ftdi_elan_kick_respond_queue(struct usb_ftdi *ftdi);
 static int ftdi_elan_setupOHCI(struct usb_ftdi *ftdi);
@@ -305,421 +308,416 @@ static int ftdi_elan_command_engine(struct usb_ftdi *ftdi);
 static int ftdi_elan_respond_engine(struct usb_ftdi *ftdi);
 static int ftdi_elan_hcd_init(struct usb_ftdi *ftdi)
 {
-        int result;
-        if (ftdi->platform_dev.dev.parent)
-                return -EBUSY;
-        ftdi_elan_get_kref(ftdi);
-        ftdi->platform_data.potpg = 100;
-        ftdi->platform_data.reset = NULL;
-        ftdi->platform_dev.id = ftdi->sequence_num;
-        ftdi->platform_dev.resource = ftdi->resources;
-        ftdi->platform_dev.num_resources = ARRAY_SIZE(ftdi->resources);
-        ftdi->platform_dev.dev.platform_data = &ftdi->platform_data;
-        ftdi->platform_dev.dev.parent = NULL;
-        ftdi->platform_dev.dev.release = ftdi_release_platform_dev;
-        ftdi->platform_dev.dev.dma_mask = NULL;
-        snprintf(ftdi->device_name, sizeof(ftdi->device_name), "u132_hcd");
-        ftdi->platform_dev.name = ftdi->device_name;
-        dev_info(&ftdi->udev->dev, "requesting module '%s'\n", "u132_hcd");
-        request_module("u132_hcd");
-        dev_info(&ftdi->udev->dev, "registering '%s'\n",
-                ftdi->platform_dev.name);
-        result = platform_device_register(&ftdi->platform_dev);
-        return result;
+	int result;
+	if (ftdi->platform_dev.dev.parent)
+		return -EBUSY;
+	ftdi_elan_get_kref(ftdi);
+	ftdi->platform_data.potpg = 100;
+	ftdi->platform_data.reset = NULL;
+	ftdi->platform_dev.id = ftdi->sequence_num;
+	ftdi->platform_dev.resource = ftdi->resources;
+	ftdi->platform_dev.num_resources = ARRAY_SIZE(ftdi->resources);
+	ftdi->platform_dev.dev.platform_data = &ftdi->platform_data;
+	ftdi->platform_dev.dev.parent = NULL;
+	ftdi->platform_dev.dev.release = ftdi_release_platform_dev;
+	ftdi->platform_dev.dev.dma_mask = NULL;
+	snprintf(ftdi->device_name, sizeof(ftdi->device_name), "u132_hcd");
+	ftdi->platform_dev.name = ftdi->device_name;
+	dev_info(&ftdi->udev->dev, "requesting module '%s'\n", "u132_hcd");
+	request_module("u132_hcd");
+	dev_info(&ftdi->udev->dev, "registering '%s'\n",
+		 ftdi->platform_dev.name);
+	result = platform_device_register(&ftdi->platform_dev);
+	return result;
 }
 
 static void ftdi_elan_abandon_completions(struct usb_ftdi *ftdi)
 {
-        mutex_lock(&ftdi->u132_lock);
-        while (ftdi->respond_next > ftdi->respond_head) {
-                struct u132_respond *respond = &ftdi->respond[RESPOND_MASK &
-                        ftdi->respond_head++];
-                *respond->result = -ESHUTDOWN;
-                *respond->value = 0;
-                complete(&respond->wait_completion);
-        } mutex_unlock(&ftdi->u132_lock);
+	mutex_lock(&ftdi->u132_lock);
+	while (ftdi->respond_next > ftdi->respond_head) {
+		struct u132_respond *respond = &ftdi->respond[RESPOND_MASK &
+							      ftdi->respond_head++];
+		*respond->result = -ESHUTDOWN;
+		*respond->value = 0;
+		complete(&respond->wait_completion);
+	} mutex_unlock(&ftdi->u132_lock);
 }
 
 static void ftdi_elan_abandon_targets(struct usb_ftdi *ftdi)
 {
-        int ed_number = 4;
-        mutex_lock(&ftdi->u132_lock);
-        while (ed_number-- > 0) {
-                struct u132_target *target = &ftdi->target[ed_number];
-                if (target->active == 1) {
-                        target->condition_code = TD_DEVNOTRESP;
-                        mutex_unlock(&ftdi->u132_lock);
-                        ftdi_elan_do_callback(ftdi, target, NULL, 0);
-                        mutex_lock(&ftdi->u132_lock);
-                }
-        }
-        ftdi->received = 0;
-        ftdi->expected = 4;
-        ftdi->ed_found = 0;
-        mutex_unlock(&ftdi->u132_lock);
+	int ed_number = 4;
+	mutex_lock(&ftdi->u132_lock);
+	while (ed_number-- > 0) {
+		struct u132_target *target = &ftdi->target[ed_number];
+		if (target->active == 1) {
+			target->condition_code = TD_DEVNOTRESP;
+			mutex_unlock(&ftdi->u132_lock);
+			ftdi_elan_do_callback(ftdi, target, NULL, 0);
+			mutex_lock(&ftdi->u132_lock);
+		}
+	}
+	ftdi->received = 0;
+	ftdi->expected = 4;
+	ftdi->ed_found = 0;
+	mutex_unlock(&ftdi->u132_lock);
 }
 
 static void ftdi_elan_flush_targets(struct usb_ftdi *ftdi)
 {
-        int ed_number = 4;
-        mutex_lock(&ftdi->u132_lock);
-        while (ed_number-- > 0) {
-                struct u132_target *target = &ftdi->target[ed_number];
-                target->abandoning = 1;
-              wait_1:if (target->active == 1) {
-                        int command_size = ftdi->command_next -
-                                ftdi->command_head;
-                        if (command_size < COMMAND_SIZE) {
-                                struct u132_command *command = &ftdi->command[
-                                        COMMAND_MASK & ftdi->command_next];
-                                command->header = 0x80 | (ed_number << 5) | 0x4;
-                                command->length = 0x00;
-                                command->address = 0x00;
-                                command->width = 0x00;
-                                command->follows = 0;
-                                command->value = 0;
-                                command->buffer = &command->value;
-                                ftdi->command_next += 1;
-                                ftdi_elan_kick_command_queue(ftdi);
-                        } else {
-                                mutex_unlock(&ftdi->u132_lock);
-                                msleep(100);
-                                mutex_lock(&ftdi->u132_lock);
-                                goto wait_1;
-                        }
-                }
-              wait_2:if (target->active == 1) {
-                        int command_size = ftdi->command_next -
-                                ftdi->command_head;
-                        if (command_size < COMMAND_SIZE) {
-                                struct u132_command *command = &ftdi->command[
-                                        COMMAND_MASK & ftdi->command_next];
-                                command->header = 0x90 | (ed_number << 5);
-                                command->length = 0x00;
-                                command->address = 0x00;
-                                command->width = 0x00;
-                                command->follows = 0;
-                                command->value = 0;
-                                command->buffer = &command->value;
-                                ftdi->command_next += 1;
-                                ftdi_elan_kick_command_queue(ftdi);
-                        } else {
-                                mutex_unlock(&ftdi->u132_lock);
-                                msleep(100);
-                                mutex_lock(&ftdi->u132_lock);
-                                goto wait_2;
-                        }
-                }
-        }
-        ftdi->received = 0;
-        ftdi->expected = 4;
-        ftdi->ed_found = 0;
-        mutex_unlock(&ftdi->u132_lock);
+	int ed_number = 4;
+	mutex_lock(&ftdi->u132_lock);
+	while (ed_number-- > 0) {
+		struct u132_target *target = &ftdi->target[ed_number];
+		target->abandoning = 1;
+	wait_1:if (target->active == 1) {
+			int command_size = ftdi->command_next -
+				ftdi->command_head;
+			if (command_size < COMMAND_SIZE) {
+				struct u132_command *command = &ftdi->command[
+					COMMAND_MASK & ftdi->command_next];
+				command->header = 0x80 | (ed_number << 5) | 0x4;
+				command->length = 0x00;
+				command->address = 0x00;
+				command->width = 0x00;
+				command->follows = 0;
+				command->value = 0;
+				command->buffer = &command->value;
+				ftdi->command_next += 1;
+				ftdi_elan_kick_command_queue(ftdi);
+			} else {
+				mutex_unlock(&ftdi->u132_lock);
+				msleep(100);
+				mutex_lock(&ftdi->u132_lock);
+				goto wait_1;
+			}
+		}
+	wait_2:if (target->active == 1) {
+			int command_size = ftdi->command_next -
+				ftdi->command_head;
+			if (command_size < COMMAND_SIZE) {
+				struct u132_command *command = &ftdi->command[
+					COMMAND_MASK & ftdi->command_next];
+				command->header = 0x90 | (ed_number << 5);
+				command->length = 0x00;
+				command->address = 0x00;
+				command->width = 0x00;
+				command->follows = 0;
+				command->value = 0;
+				command->buffer = &command->value;
+				ftdi->command_next += 1;
+				ftdi_elan_kick_command_queue(ftdi);
+			} else {
+				mutex_unlock(&ftdi->u132_lock);
+				msleep(100);
+				mutex_lock(&ftdi->u132_lock);
+				goto wait_2;
+			}
+		}
+	}
+	ftdi->received = 0;
+	ftdi->expected = 4;
+	ftdi->ed_found = 0;
+	mutex_unlock(&ftdi->u132_lock);
 }
 
 static void ftdi_elan_cancel_targets(struct usb_ftdi *ftdi)
 {
-        int ed_number = 4;
-        mutex_lock(&ftdi->u132_lock);
-        while (ed_number-- > 0) {
-                struct u132_target *target = &ftdi->target[ed_number];
-                target->abandoning = 1;
-              wait:if (target->active == 1) {
-                        int command_size = ftdi->command_next -
-                                ftdi->command_head;
-                        if (command_size < COMMAND_SIZE) {
-                                struct u132_command *command = &ftdi->command[
-                                        COMMAND_MASK & ftdi->command_next];
-                                command->header = 0x80 | (ed_number << 5) | 0x4;
-                                command->length = 0x00;
-                                command->address = 0x00;
-                                command->width = 0x00;
-                                command->follows = 0;
-                                command->value = 0;
-                                command->buffer = &command->value;
-                                ftdi->command_next += 1;
-                                ftdi_elan_kick_command_queue(ftdi);
-                        } else {
-                                mutex_unlock(&ftdi->u132_lock);
-                                msleep(100);
-                                mutex_lock(&ftdi->u132_lock);
-                                goto wait;
-                        }
-                }
-        }
-        ftdi->received = 0;
-        ftdi->expected = 4;
-        ftdi->ed_found = 0;
-        mutex_unlock(&ftdi->u132_lock);
+	int ed_number = 4;
+	mutex_lock(&ftdi->u132_lock);
+	while (ed_number-- > 0) {
+		struct u132_target *target = &ftdi->target[ed_number];
+		target->abandoning = 1;
+	wait:if (target->active == 1) {
+			int command_size = ftdi->command_next -
+				ftdi->command_head;
+			if (command_size < COMMAND_SIZE) {
+				struct u132_command *command = &ftdi->command[
+					COMMAND_MASK & ftdi->command_next];
+				command->header = 0x80 | (ed_number << 5) | 0x4;
+				command->length = 0x00;
+				command->address = 0x00;
+				command->width = 0x00;
+				command->follows = 0;
+				command->value = 0;
+				command->buffer = &command->value;
+				ftdi->command_next += 1;
+				ftdi_elan_kick_command_queue(ftdi);
+			} else {
+				mutex_unlock(&ftdi->u132_lock);
+				msleep(100);
+				mutex_lock(&ftdi->u132_lock);
+				goto wait;
+			}
+		}
+	}
+	ftdi->received = 0;
+	ftdi->expected = 4;
+	ftdi->ed_found = 0;
+	mutex_unlock(&ftdi->u132_lock);
 }
 
 static void ftdi_elan_kick_command_queue(struct usb_ftdi *ftdi)
 {
-        ftdi_command_queue_work(ftdi, 0);
+	ftdi_command_queue_work(ftdi, 0);
 }
 
 static void ftdi_elan_command_work(struct work_struct *work)
 {
-        struct usb_ftdi *ftdi =
+	struct usb_ftdi *ftdi =
 		container_of(work, struct usb_ftdi, command_work.work);
 
-        if (ftdi->disconnected > 0) {
-                ftdi_elan_put_kref(ftdi);
-                return;
-        } else {
-                int retval = ftdi_elan_command_engine(ftdi);
-                if (retval == -ESHUTDOWN) {
-                        ftdi->disconnected += 1;
-                } else if (retval == -ENODEV) {
-                        ftdi->disconnected += 1;
-                } else if (retval)
-                        dev_err(&ftdi->udev->dev, "command error %d\n", retval);
-                ftdi_command_requeue_work(ftdi, msecs_to_jiffies(10));
-                return;
-        }
+	if (ftdi->disconnected > 0) {
+		ftdi_elan_put_kref(ftdi);
+		return;
+	} else {
+		int retval = ftdi_elan_command_engine(ftdi);
+		if (retval == -ESHUTDOWN) {
+			ftdi->disconnected += 1;
+		} else if (retval == -ENODEV) {
+			ftdi->disconnected += 1;
+		} else if (retval)
+			dev_err(&ftdi->udev->dev, "command error %d\n", retval);
+		ftdi_command_requeue_work(ftdi, msecs_to_jiffies(10));
+		return;
+	}
 }
 
 static void ftdi_elan_kick_respond_queue(struct usb_ftdi *ftdi)
 {
-        ftdi_respond_queue_work(ftdi, 0);
+	ftdi_respond_queue_work(ftdi, 0);
 }
 
 static void ftdi_elan_respond_work(struct work_struct *work)
 {
-        struct usb_ftdi *ftdi =
+	struct usb_ftdi *ftdi =
 		container_of(work, struct usb_ftdi, respond_work.work);
-        if (ftdi->disconnected > 0) {
-                ftdi_elan_put_kref(ftdi);
-                return;
-        } else {
-                int retval = ftdi_elan_respond_engine(ftdi);
-                if (retval == 0) {
-                } else if (retval == -ESHUTDOWN) {
-                        ftdi->disconnected += 1;
-                } else if (retval == -ENODEV) {
-                        ftdi->disconnected += 1;
-                } else if (retval == -EILSEQ) {
-                        ftdi->disconnected += 1;
-                } else {
-                        ftdi->disconnected += 1;
-                        dev_err(&ftdi->udev->dev, "respond error %d\n", retval);
-                }
-                if (ftdi->disconnected > 0) {
-                        ftdi_elan_abandon_completions(ftdi);
-                        ftdi_elan_abandon_targets(ftdi);
-                }
-                ftdi_response_requeue_work(ftdi, msecs_to_jiffies(10));
-                return;
-        }
+	if (ftdi->disconnected > 0) {
+		ftdi_elan_put_kref(ftdi);
+		return;
+	} else {
+		int retval = ftdi_elan_respond_engine(ftdi);
+		if (retval == 0) {
+		} else if (retval == -ESHUTDOWN) {
+			ftdi->disconnected += 1;
+		} else if (retval == -ENODEV) {
+			ftdi->disconnected += 1;
+		} else if (retval == -EILSEQ) {
+			ftdi->disconnected += 1;
+		} else {
+			ftdi->disconnected += 1;
+			dev_err(&ftdi->udev->dev, "respond error %d\n", retval);
+		}
+		if (ftdi->disconnected > 0) {
+			ftdi_elan_abandon_completions(ftdi);
+			ftdi_elan_abandon_targets(ftdi);
+		}
+		ftdi_response_requeue_work(ftdi, msecs_to_jiffies(10));
+		return;
+	}
 }
 
 
 /*
-* the sw_lock is initially held and will be freed
-* after the FTDI has been synchronized
-*
-*/
+ * the sw_lock is initially held and will be freed
+ * after the FTDI has been synchronized
+ *
+ */
 static void ftdi_elan_status_work(struct work_struct *work)
 {
-        struct usb_ftdi *ftdi =
+	struct usb_ftdi *ftdi =
 		container_of(work, struct usb_ftdi, status_work.work);
-        int work_delay_in_msec = 0;
-        if (ftdi->disconnected > 0) {
-                ftdi_elan_put_kref(ftdi);
-                return;
-        } else if (ftdi->synchronized == 0) {
-                down(&ftdi->sw_lock);
-                if (ftdi_elan_synchronize(ftdi) == 0) {
-                        ftdi->synchronized = 1;
-                        ftdi_command_queue_work(ftdi, 1);
-                        ftdi_respond_queue_work(ftdi, 1);
-                        up(&ftdi->sw_lock);
-                        work_delay_in_msec = 100;
-                } else {
-                        dev_err(&ftdi->udev->dev, "synchronize failed\n");
-                        up(&ftdi->sw_lock);
-                        work_delay_in_msec = 10 *1000;
-                }
-        } else if (ftdi->stuck_status > 0) {
-                if (ftdi_elan_stuck_waiting(ftdi) == 0) {
-                        ftdi->stuck_status = 0;
-                        ftdi->synchronized = 0;
-                } else if ((ftdi->stuck_status++ % 60) == 1) {
-                        dev_err(&ftdi->udev->dev, "WRONG type of card inserted "
-                                "- please remove\n");
-                } else
-                        dev_err(&ftdi->udev->dev, "WRONG type of card inserted "
-                                "- checked %d times\n", ftdi->stuck_status);
-                work_delay_in_msec = 100;
-        } else if (ftdi->enumerated == 0) {
-                if (ftdi_elan_enumeratePCI(ftdi) == 0) {
-                        ftdi->enumerated = 1;
-                        work_delay_in_msec = 250;
-                } else
-                        work_delay_in_msec = 1000;
-        } else if (ftdi->initialized == 0) {
-                if (ftdi_elan_setupOHCI(ftdi) == 0) {
-                        ftdi->initialized = 1;
-                        work_delay_in_msec = 500;
-                } else {
-                        dev_err(&ftdi->udev->dev, "initialized failed - trying "
-                                "again in 10 seconds\n");
-                        work_delay_in_msec = 1 *1000;
-                }
-        } else if (ftdi->registered == 0) {
-                work_delay_in_msec = 10;
-                if (ftdi_elan_hcd_init(ftdi) == 0) {
-                        ftdi->registered = 1;
-                } else
-                        dev_err(&ftdi->udev->dev, "register failed\n");
-                work_delay_in_msec = 250;
-        } else {
-                if (ftdi_elan_checkingPCI(ftdi) == 0) {
-                        work_delay_in_msec = 250;
-                } else if (ftdi->controlreg & 0x00400000) {
-                        if (ftdi->gone_away > 0) {
-                                dev_err(&ftdi->udev->dev, "PCI device eject con"
-                                        "firmed platform_dev.dev.parent=%p plat"
-                                        "form_dev.dev=%p\n",
-                                        ftdi->platform_dev.dev.parent,
-                                        &ftdi->platform_dev.dev);
-                                platform_device_unregister(&ftdi->platform_dev);
-                                ftdi->platform_dev.dev.parent = NULL;
-                                ftdi->registered = 0;
-                                ftdi->enumerated = 0;
-                                ftdi->card_ejected = 0;
-                                ftdi->initialized = 0;
-                                ftdi->gone_away = 0;
-                        } else
-                                ftdi_elan_flush_targets(ftdi);
-                        work_delay_in_msec = 250;
-                } else {
-                        dev_err(&ftdi->udev->dev, "PCI device has disappeared\n"
-                                );
-                        ftdi_elan_cancel_targets(ftdi);
-                        work_delay_in_msec = 500;
-                        ftdi->enumerated = 0;
-                        ftdi->initialized = 0;
-                }
-        }
-        if (ftdi->disconnected > 0) {
-                ftdi_elan_put_kref(ftdi);
-                return;
-        } else {
-                ftdi_status_requeue_work(ftdi,
-                        msecs_to_jiffies(work_delay_in_msec));
-                return;
-        }
+	int work_delay_in_msec = 0;
+	if (ftdi->disconnected > 0) {
+		ftdi_elan_put_kref(ftdi);
+		return;
+	} else if (ftdi->synchronized == 0) {
+		down(&ftdi->sw_lock);
+		if (ftdi_elan_synchronize(ftdi) == 0) {
+			ftdi->synchronized = 1;
+			ftdi_command_queue_work(ftdi, 1);
+			ftdi_respond_queue_work(ftdi, 1);
+			up(&ftdi->sw_lock);
+			work_delay_in_msec = 100;
+		} else {
+			dev_err(&ftdi->udev->dev, "synchronize failed\n");
+			up(&ftdi->sw_lock);
+			work_delay_in_msec = 10 *1000;
+		}
+	} else if (ftdi->stuck_status > 0) {
+		if (ftdi_elan_stuck_waiting(ftdi) == 0) {
+			ftdi->stuck_status = 0;
+			ftdi->synchronized = 0;
+		} else if ((ftdi->stuck_status++ % 60) == 1) {
+			dev_err(&ftdi->udev->dev, "WRONG type of card inserted - please remove\n");
+		} else
+			dev_err(&ftdi->udev->dev, "WRONG type of card inserted - checked %d times\n",
+				ftdi->stuck_status);
+		work_delay_in_msec = 100;
+	} else if (ftdi->enumerated == 0) {
+		if (ftdi_elan_enumeratePCI(ftdi) == 0) {
+			ftdi->enumerated = 1;
+			work_delay_in_msec = 250;
+		} else
+			work_delay_in_msec = 1000;
+	} else if (ftdi->initialized == 0) {
+		if (ftdi_elan_setupOHCI(ftdi) == 0) {
+			ftdi->initialized = 1;
+			work_delay_in_msec = 500;
+		} else {
+			dev_err(&ftdi->udev->dev, "initialized failed - trying again in 10 seconds\n");
+			work_delay_in_msec = 1 *1000;
+		}
+	} else if (ftdi->registered == 0) {
+		work_delay_in_msec = 10;
+		if (ftdi_elan_hcd_init(ftdi) == 0) {
+			ftdi->registered = 1;
+		} else
+			dev_err(&ftdi->udev->dev, "register failed\n");
+		work_delay_in_msec = 250;
+	} else {
+		if (ftdi_elan_checkingPCI(ftdi) == 0) {
+			work_delay_in_msec = 250;
+		} else if (ftdi->controlreg & 0x00400000) {
+			if (ftdi->gone_away > 0) {
+				dev_err(&ftdi->udev->dev, "PCI device eject confirmed platform_dev.dev.parent=%p platform_dev.dev=%p\n",
+					ftdi->platform_dev.dev.parent,
+					&ftdi->platform_dev.dev);
+				platform_device_unregister(&ftdi->platform_dev);
+				ftdi->platform_dev.dev.parent = NULL;
+				ftdi->registered = 0;
+				ftdi->enumerated = 0;
+				ftdi->card_ejected = 0;
+				ftdi->initialized = 0;
+				ftdi->gone_away = 0;
+			} else
+				ftdi_elan_flush_targets(ftdi);
+			work_delay_in_msec = 250;
+		} else {
+			dev_err(&ftdi->udev->dev, "PCI device has disappeared\n");
+			ftdi_elan_cancel_targets(ftdi);
+			work_delay_in_msec = 500;
+			ftdi->enumerated = 0;
+			ftdi->initialized = 0;
+		}
+	}
+	if (ftdi->disconnected > 0) {
+		ftdi_elan_put_kref(ftdi);
+		return;
+	} else {
+		ftdi_status_requeue_work(ftdi,
+					 msecs_to_jiffies(work_delay_in_msec));
+		return;
+	}
 }
 
 
 /*
-* file_operations for the jtag interface
-*
-* the usage count for the device is incremented on open()
-* and decremented on release()
-*/
+ * file_operations for the jtag interface
+ *
+ * the usage count for the device is incremented on open()
+ * and decremented on release()
+ */
 static int ftdi_elan_open(struct inode *inode, struct file *file)
 {
 	int subminor;
 	struct usb_interface *interface;
 
-        subminor = iminor(inode);
-        interface = usb_find_interface(&ftdi_elan_driver, subminor);
-
-        if (!interface) {
-                printk(KERN_ERR "can't find device for minor %d\n", subminor);
-                return -ENODEV;
-        } else {
-                struct usb_ftdi *ftdi = usb_get_intfdata(interface);
-                if (!ftdi) {
-                        return -ENODEV;
-                } else {
-                        if (down_interruptible(&ftdi->sw_lock)) {
-                                return -EINTR;
-                        } else {
-                                ftdi_elan_get_kref(ftdi);
-                                file->private_data = ftdi;
-                                return 0;
-                        }
-                }
-        }
+	subminor = iminor(inode);
+	interface = usb_find_interface(&ftdi_elan_driver, subminor);
+
+	if (!interface) {
+		pr_err("can't find device for minor %d\n", subminor);
+		return -ENODEV;
+	} else {
+		struct usb_ftdi *ftdi = usb_get_intfdata(interface);
+		if (!ftdi) {
+			return -ENODEV;
+		} else {
+			if (down_interruptible(&ftdi->sw_lock)) {
+				return -EINTR;
+			} else {
+				ftdi_elan_get_kref(ftdi);
+				file->private_data = ftdi;
+				return 0;
+			}
+		}
+	}
 }
 
 static int ftdi_elan_release(struct inode *inode, struct file *file)
 {
-        struct usb_ftdi *ftdi = file->private_data;
-        if (ftdi == NULL)
-                return -ENODEV;
-        up(&ftdi->sw_lock);        /* decrement the count on our device */
-        ftdi_elan_put_kref(ftdi);
-        return 0;
+	struct usb_ftdi *ftdi = file->private_data;
+	if (ftdi == NULL)
+		return -ENODEV;
+	up(&ftdi->sw_lock);        /* decrement the count on our device */
+	ftdi_elan_put_kref(ftdi);
+	return 0;
 }
 
 
 /*
-*
-* blocking bulk reads are used to get data from the device
-*
-*/
+ *
+ * blocking bulk reads are used to get data from the device
+ *
+ */
 static ssize_t ftdi_elan_read(struct file *file, char __user *buffer,
 			      size_t count, loff_t *ppos)
 {
-        char data[30 *3 + 4];
-        char *d = data;
-        int m = (sizeof(data) - 1) / 3;
-        int bytes_read = 0;
-        int retry_on_empty = 10;
-        int retry_on_timeout = 5;
-        struct usb_ftdi *ftdi = file->private_data;
-        if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        }
-        data[0] = 0;
-      have:if (ftdi->bulk_in_left > 0) {
-                if (count-- > 0) {
-                        char *p = ++ftdi->bulk_in_last + ftdi->bulk_in_buffer;
-                        ftdi->bulk_in_left -= 1;
-                        if (bytes_read < m) {
-                                d += sprintf(d, " %02X", 0x000000FF & *p);
-                        } else if (bytes_read > m) {
-                        } else
-                                d += sprintf(d, " ..");
-                        if (copy_to_user(buffer++, p, 1)) {
-                                return -EFAULT;
-                        } else {
-                                bytes_read += 1;
-                                goto have;
-                        }
-                } else
-                        return bytes_read;
-        }
-      more:if (count > 0) {
-                int packet_bytes = 0;
-                int retval = usb_bulk_msg(ftdi->udev,
-                        usb_rcvbulkpipe(ftdi->udev, ftdi->bulk_in_endpointAddr),
-                         ftdi->bulk_in_buffer, ftdi->bulk_in_size,
-                        &packet_bytes, 50);
-                if (packet_bytes > 2) {
-                        ftdi->bulk_in_left = packet_bytes - 2;
-                        ftdi->bulk_in_last = 1;
-                        goto have;
-                } else if (retval == -ETIMEDOUT) {
-                        if (retry_on_timeout-- > 0) {
-                                goto more;
-                        } else if (bytes_read > 0) {
-                                return bytes_read;
-                        } else
-                                return retval;
-                } else if (retval == 0) {
-                        if (retry_on_empty-- > 0) {
-                                goto more;
-                        } else
-                                return bytes_read;
-                } else
-                        return retval;
-        } else
-                return bytes_read;
+	char data[30 *3 + 4];
+	char *d = data;
+	int m = (sizeof(data) - 1) / 3;
+	int bytes_read = 0;
+	int retry_on_empty = 10;
+	int retry_on_timeout = 5;
+	struct usb_ftdi *ftdi = file->private_data;
+	if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	}
+	data[0] = 0;
+have:if (ftdi->bulk_in_left > 0) {
+		if (count-- > 0) {
+			char *p = ++ftdi->bulk_in_last + ftdi->bulk_in_buffer;
+			ftdi->bulk_in_left -= 1;
+			if (bytes_read < m) {
+				d += sprintf(d, " %02X", 0x000000FF & *p);
+			} else if (bytes_read > m) {
+			} else
+				d += sprintf(d, " ..");
+			if (copy_to_user(buffer++, p, 1)) {
+				return -EFAULT;
+			} else {
+				bytes_read += 1;
+				goto have;
+			}
+		} else
+			return bytes_read;
+	}
+more:if (count > 0) {
+		int packet_bytes = 0;
+		int retval = usb_bulk_msg(ftdi->udev,
+					  usb_rcvbulkpipe(ftdi->udev, ftdi->bulk_in_endpointAddr),
+					  ftdi->bulk_in_buffer, ftdi->bulk_in_size,
+					  &packet_bytes, 50);
+		if (packet_bytes > 2) {
+			ftdi->bulk_in_left = packet_bytes - 2;
+			ftdi->bulk_in_last = 1;
+			goto have;
+		} else if (retval == -ETIMEDOUT) {
+			if (retry_on_timeout-- > 0) {
+				goto more;
+			} else if (bytes_read > 0) {
+				return bytes_read;
+			} else
+				return retval;
+		} else if (retval == 0) {
+			if (retry_on_empty-- > 0) {
+				goto more;
+			} else
+				return bytes_read;
+		} else
+			return retval;
+	} else
+		return bytes_read;
 }
 
 static void ftdi_elan_write_bulk_callback(struct urb *urb)
@@ -728,467 +726,460 @@ static void ftdi_elan_write_bulk_callback(struct urb *urb)
 	int status = urb->status;
 
 	if (status && !(status == -ENOENT || status == -ECONNRESET ||
-	    status == -ESHUTDOWN)) {
-                dev_err(&ftdi->udev->dev, "urb=%p write bulk status received: %"
-                        "d\n", urb, status);
-        }
-        usb_free_coherent(urb->dev, urb->transfer_buffer_length,
-                urb->transfer_buffer, urb->transfer_dma);
+			status == -ESHUTDOWN)) {
+		dev_err(&ftdi->udev->dev,
+			"urb=%p write bulk status received: %d\n", urb, status);
+	}
+	usb_free_coherent(urb->dev, urb->transfer_buffer_length,
+			  urb->transfer_buffer, urb->transfer_dma);
 }
 
 static int fill_buffer_with_all_queued_commands(struct usb_ftdi *ftdi,
-        char *buf, int command_size, int total_size)
-{
-        int ed_commands = 0;
-        int b = 0;
-        int I = command_size;
-        int i = ftdi->command_head;
-        while (I-- > 0) {
-                struct u132_command *command = &ftdi->command[COMMAND_MASK &
-                        i++];
-                int F = command->follows;
-                u8 *f = command->buffer;
-                if (command->header & 0x80) {
-                        ed_commands |= 1 << (0x3 & (command->header >> 5));
-                }
-                buf[b++] = command->header;
-                buf[b++] = (command->length >> 0) & 0x00FF;
-                buf[b++] = (command->length >> 8) & 0x00FF;
-                buf[b++] = command->address;
-                buf[b++] = command->width;
-                while (F-- > 0) {
-                        buf[b++] = *f++;
-                }
-        }
-        return ed_commands;
+						char *buf, int command_size, int total_size)
+{
+	int ed_commands = 0;
+	int b = 0;
+	int I = command_size;
+	int i = ftdi->command_head;
+	while (I-- > 0) {
+		struct u132_command *command = &ftdi->command[COMMAND_MASK &
+							      i++];
+		int F = command->follows;
+		u8 *f = command->buffer;
+		if (command->header & 0x80) {
+			ed_commands |= 1 << (0x3 & (command->header >> 5));
+		}
+		buf[b++] = command->header;
+		buf[b++] = (command->length >> 0) & 0x00FF;
+		buf[b++] = (command->length >> 8) & 0x00FF;
+		buf[b++] = command->address;
+		buf[b++] = command->width;
+		while (F-- > 0) {
+			buf[b++] = *f++;
+		}
+	}
+	return ed_commands;
 }
 
 static int ftdi_elan_total_command_size(struct usb_ftdi *ftdi, int command_size)
 {
-        int total_size = 0;
-        int I = command_size;
-        int i = ftdi->command_head;
-        while (I-- > 0) {
-                struct u132_command *command = &ftdi->command[COMMAND_MASK &
-                        i++];
-                total_size += 5 + command->follows;
-        } return total_size;
+	int total_size = 0;
+	int I = command_size;
+	int i = ftdi->command_head;
+	while (I-- > 0) {
+		struct u132_command *command = &ftdi->command[COMMAND_MASK &
+							      i++];
+		total_size += 5 + command->follows;
+	} return total_size;
 }
 
 static int ftdi_elan_command_engine(struct usb_ftdi *ftdi)
 {
-        int retval;
-        char *buf;
-        int ed_commands;
-        int total_size;
-        struct urb *urb;
-        int command_size = ftdi->command_next - ftdi->command_head;
-        if (command_size == 0)
-                return 0;
-        total_size = ftdi_elan_total_command_size(ftdi, command_size);
-        urb = usb_alloc_urb(0, GFP_KERNEL);
-        if (!urb) {
-                dev_err(&ftdi->udev->dev, "could not get a urb to write %d comm"
-                        "ands totaling %d bytes to the Uxxx\n", command_size,
-                        total_size);
-                return -ENOMEM;
-        }
-        buf = usb_alloc_coherent(ftdi->udev, total_size, GFP_KERNEL,
-                &urb->transfer_dma);
-        if (!buf) {
-                dev_err(&ftdi->udev->dev, "could not get a buffer to write %d c"
-                        "ommands totaling %d bytes to the Uxxx\n", command_size,
-                         total_size);
-                usb_free_urb(urb);
-                return -ENOMEM;
-        }
-        ed_commands = fill_buffer_with_all_queued_commands(ftdi, buf,
-                command_size, total_size);
-        usb_fill_bulk_urb(urb, ftdi->udev, usb_sndbulkpipe(ftdi->udev,
-                ftdi->bulk_out_endpointAddr), buf, total_size,
-                ftdi_elan_write_bulk_callback, ftdi);
-        urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
-        if (ed_commands) {
-                char diag[40 *3 + 4];
-                char *d = diag;
-                int m = total_size;
-                u8 *c = buf;
-                int s = (sizeof(diag) - 1) / 3;
-                diag[0] = 0;
-                while (s-- > 0 && m-- > 0) {
-                        if (s > 0 || m == 0) {
-                                d += sprintf(d, " %02X", *c++);
-                        } else
-                                d += sprintf(d, " ..");
-                }
-        }
-        retval = usb_submit_urb(urb, GFP_KERNEL);
-        if (retval) {
-                dev_err(&ftdi->udev->dev, "failed %d to submit urb %p to write "
-                        "%d commands totaling %d bytes to the Uxxx\n", retval,
-                        urb, command_size, total_size);
-                usb_free_coherent(ftdi->udev, total_size, buf, urb->transfer_dma);
-                usb_free_urb(urb);
-                return retval;
-        }
-        usb_free_urb(urb);        /* release our reference to this urb,
-                the USB core will eventually free it entirely */
-        ftdi->command_head += command_size;
-        ftdi_elan_kick_respond_queue(ftdi);
-        return 0;
+	int retval;
+	char *buf;
+	int ed_commands;
+	int total_size;
+	struct urb *urb;
+	int command_size = ftdi->command_next - ftdi->command_head;
+	if (command_size == 0)
+		return 0;
+	total_size = ftdi_elan_total_command_size(ftdi, command_size);
+	urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!urb) {
+		dev_err(&ftdi->udev->dev, "could not get a urb to write %d commands totaling %d bytes to the Uxxx\n",
+			command_size, total_size);
+		return -ENOMEM;
+	}
+	buf = usb_alloc_coherent(ftdi->udev, total_size, GFP_KERNEL,
+				 &urb->transfer_dma);
+	if (!buf) {
+		dev_err(&ftdi->udev->dev, "could not get a buffer to write %d commands totaling %d bytes to the Uxxx\n",
+			command_size, total_size);
+		usb_free_urb(urb);
+		return -ENOMEM;
+	}
+	ed_commands = fill_buffer_with_all_queued_commands(ftdi, buf,
+							   command_size, total_size);
+	usb_fill_bulk_urb(urb, ftdi->udev, usb_sndbulkpipe(ftdi->udev,
+							   ftdi->bulk_out_endpointAddr), buf, total_size,
+			  ftdi_elan_write_bulk_callback, ftdi);
+	urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+	if (ed_commands) {
+		char diag[40 *3 + 4];
+		char *d = diag;
+		int m = total_size;
+		u8 *c = buf;
+		int s = (sizeof(diag) - 1) / 3;
+		diag[0] = 0;
+		while (s-- > 0 && m-- > 0) {
+			if (s > 0 || m == 0) {
+				d += sprintf(d, " %02X", *c++);
+			} else
+				d += sprintf(d, " ..");
+		}
+	}
+	retval = usb_submit_urb(urb, GFP_KERNEL);
+	if (retval) {
+		dev_err(&ftdi->udev->dev, "failed %d to submit urb %p to write %d commands totaling %d bytes to the Uxxx\n",
+			retval, urb, command_size, total_size);
+		usb_free_coherent(ftdi->udev, total_size, buf, urb->transfer_dma);
+		usb_free_urb(urb);
+		return retval;
+	}
+	usb_free_urb(urb);        /* release our reference to this urb,
+				     the USB core will eventually free it entirely */
+	ftdi->command_head += command_size;
+	ftdi_elan_kick_respond_queue(ftdi);
+	return 0;
 }
 
 static void ftdi_elan_do_callback(struct usb_ftdi *ftdi,
-        struct u132_target *target, u8 *buffer, int length)
-{
-        struct urb *urb = target->urb;
-        int halted = target->halted;
-        int skipped = target->skipped;
-        int actual = target->actual;
-        int non_null = target->non_null;
-        int toggle_bits = target->toggle_bits;
-        int error_count = target->error_count;
-        int condition_code = target->condition_code;
-        int repeat_number = target->repeat_number;
-        void (*callback) (void *, struct urb *, u8 *, int, int, int, int, int,
-                int, int, int, int) = target->callback;
-        target->active -= 1;
-        target->callback = NULL;
-        (*callback) (target->endp, urb, buffer, length, toggle_bits,
-                error_count, condition_code, repeat_number, halted, skipped,
-                actual, non_null);
+				  struct u132_target *target, u8 *buffer, int length)
+{
+	struct urb *urb = target->urb;
+	int halted = target->halted;
+	int skipped = target->skipped;
+	int actual = target->actual;
+	int non_null = target->non_null;
+	int toggle_bits = target->toggle_bits;
+	int error_count = target->error_count;
+	int condition_code = target->condition_code;
+	int repeat_number = target->repeat_number;
+	void (*callback) (void *, struct urb *, u8 *, int, int, int, int, int,
+			  int, int, int, int) = target->callback;
+	target->active -= 1;
+	target->callback = NULL;
+	(*callback) (target->endp, urb, buffer, length, toggle_bits,
+		     error_count, condition_code, repeat_number, halted, skipped,
+		     actual, non_null);
 }
 
 static char *have_ed_set_response(struct usb_ftdi *ftdi,
-        struct u132_target *target, u16 ed_length, int ed_number, int ed_type,
-        char *b)
-{
-        int payload = (ed_length >> 0) & 0x07FF;
-        mutex_lock(&ftdi->u132_lock);
-        target->actual = 0;
-        target->non_null = (ed_length >> 15) & 0x0001;
-        target->repeat_number = (ed_length >> 11) & 0x000F;
-        if (ed_type == 0x02) {
-                if (payload == 0 || target->abandoning > 0) {
-                        target->abandoning = 0;
-                        mutex_unlock(&ftdi->u132_lock);
-                        ftdi_elan_do_callback(ftdi, target, 4 + ftdi->response,
-                                payload);
-                        ftdi->received = 0;
-                        ftdi->expected = 4;
-                        ftdi->ed_found = 0;
-                        return ftdi->response;
-                } else {
-                        ftdi->expected = 4 + payload;
-                        ftdi->ed_found = 1;
-                        mutex_unlock(&ftdi->u132_lock);
-                        return b;
-                }
-        } else if (ed_type == 0x03) {
-                if (payload == 0 || target->abandoning > 0) {
-                        target->abandoning = 0;
-                        mutex_unlock(&ftdi->u132_lock);
-                        ftdi_elan_do_callback(ftdi, target, 4 + ftdi->response,
-                                payload);
-                        ftdi->received = 0;
-                        ftdi->expected = 4;
-                        ftdi->ed_found = 0;
-                        return ftdi->response;
-                } else {
-                        ftdi->expected = 4 + payload;
-                        ftdi->ed_found = 1;
-                        mutex_unlock(&ftdi->u132_lock);
-                        return b;
-                }
-        } else if (ed_type == 0x01) {
-                target->abandoning = 0;
-                mutex_unlock(&ftdi->u132_lock);
-                ftdi_elan_do_callback(ftdi, target, 4 + ftdi->response,
-                        payload);
-                ftdi->received = 0;
-                ftdi->expected = 4;
-                ftdi->ed_found = 0;
-                return ftdi->response;
-        } else {
-                target->abandoning = 0;
-                mutex_unlock(&ftdi->u132_lock);
-                ftdi_elan_do_callback(ftdi, target, 4 + ftdi->response,
-                        payload);
-                ftdi->received = 0;
-                ftdi->expected = 4;
-                ftdi->ed_found = 0;
-                return ftdi->response;
-        }
+				  struct u132_target *target, u16 ed_length, int ed_number, int ed_type,
+				  char *b)
+{
+	int payload = (ed_length >> 0) & 0x07FF;
+	mutex_lock(&ftdi->u132_lock);
+	target->actual = 0;
+	target->non_null = (ed_length >> 15) & 0x0001;
+	target->repeat_number = (ed_length >> 11) & 0x000F;
+	if (ed_type == 0x02) {
+		if (payload == 0 || target->abandoning > 0) {
+			target->abandoning = 0;
+			mutex_unlock(&ftdi->u132_lock);
+			ftdi_elan_do_callback(ftdi, target, 4 + ftdi->response,
+					      payload);
+			ftdi->received = 0;
+			ftdi->expected = 4;
+			ftdi->ed_found = 0;
+			return ftdi->response;
+		} else {
+			ftdi->expected = 4 + payload;
+			ftdi->ed_found = 1;
+			mutex_unlock(&ftdi->u132_lock);
+			return b;
+		}
+	} else if (ed_type == 0x03) {
+		if (payload == 0 || target->abandoning > 0) {
+			target->abandoning = 0;
+			mutex_unlock(&ftdi->u132_lock);
+			ftdi_elan_do_callback(ftdi, target, 4 + ftdi->response,
+					      payload);
+			ftdi->received = 0;
+			ftdi->expected = 4;
+			ftdi->ed_found = 0;
+			return ftdi->response;
+		} else {
+			ftdi->expected = 4 + payload;
+			ftdi->ed_found = 1;
+			mutex_unlock(&ftdi->u132_lock);
+			return b;
+		}
+	} else if (ed_type == 0x01) {
+		target->abandoning = 0;
+		mutex_unlock(&ftdi->u132_lock);
+		ftdi_elan_do_callback(ftdi, target, 4 + ftdi->response,
+				      payload);
+		ftdi->received = 0;
+		ftdi->expected = 4;
+		ftdi->ed_found = 0;
+		return ftdi->response;
+	} else {
+		target->abandoning = 0;
+		mutex_unlock(&ftdi->u132_lock);
+		ftdi_elan_do_callback(ftdi, target, 4 + ftdi->response,
+				      payload);
+		ftdi->received = 0;
+		ftdi->expected = 4;
+		ftdi->ed_found = 0;
+		return ftdi->response;
+	}
 }
 
 static char *have_ed_get_response(struct usb_ftdi *ftdi,
-        struct u132_target *target, u16 ed_length, int ed_number, int ed_type,
-        char *b)
+				  struct u132_target *target, u16 ed_length, int ed_number, int ed_type,
+				  char *b)
 {
-        mutex_lock(&ftdi->u132_lock);
-        target->condition_code = TD_DEVNOTRESP;
-        target->actual = (ed_length >> 0) & 0x01FF;
-        target->non_null = (ed_length >> 15) & 0x0001;
-        target->repeat_number = (ed_length >> 11) & 0x000F;
-        mutex_unlock(&ftdi->u132_lock);
-        if (target->active)
-                ftdi_elan_do_callback(ftdi, target, NULL, 0);
-        target->abandoning = 0;
-        ftdi->received = 0;
-        ftdi->expected = 4;
-        ftdi->ed_found = 0;
-        return ftdi->response;
+	mutex_lock(&ftdi->u132_lock);
+	target->condition_code = TD_DEVNOTRESP;
+	target->actual = (ed_length >> 0) & 0x01FF;
+	target->non_null = (ed_length >> 15) & 0x0001;
+	target->repeat_number = (ed_length >> 11) & 0x000F;
+	mutex_unlock(&ftdi->u132_lock);
+	if (target->active)
+		ftdi_elan_do_callback(ftdi, target, NULL, 0);
+	target->abandoning = 0;
+	ftdi->received = 0;
+	ftdi->expected = 4;
+	ftdi->ed_found = 0;
+	return ftdi->response;
 }
 
 
 /*
-* The engine tries to empty the FTDI fifo
-*
-* all responses found in the fifo data are dispatched thus
-* the response buffer can only ever hold a maximum sized
-* response from the Uxxx.
-*
-*/
+ * The engine tries to empty the FTDI fifo
+ *
+ * all responses found in the fifo data are dispatched thus
+ * the response buffer can only ever hold a maximum sized
+ * response from the Uxxx.
+ *
+ */
 static int ftdi_elan_respond_engine(struct usb_ftdi *ftdi)
 {
-        u8 *b = ftdi->response + ftdi->received;
-        int bytes_read = 0;
-        int retry_on_empty = 1;
-        int retry_on_timeout = 3;
-        int empty_packets = 0;
-      read:{
-                int packet_bytes = 0;
-                int retval = usb_bulk_msg(ftdi->udev,
-                        usb_rcvbulkpipe(ftdi->udev, ftdi->bulk_in_endpointAddr),
-                         ftdi->bulk_in_buffer, ftdi->bulk_in_size,
-                        &packet_bytes, 500);
-                char diag[30 *3 + 4];
-                char *d = diag;
-                int m = packet_bytes;
-                u8 *c = ftdi->bulk_in_buffer;
-                int s = (sizeof(diag) - 1) / 3;
-                diag[0] = 0;
-                while (s-- > 0 && m-- > 0) {
-                        if (s > 0 || m == 0) {
-                                d += sprintf(d, " %02X", *c++);
-                        } else
-                                d += sprintf(d, " ..");
-                }
-                if (packet_bytes > 2) {
-                        ftdi->bulk_in_left = packet_bytes - 2;
-                        ftdi->bulk_in_last = 1;
-                        goto have;
-                } else if (retval == -ETIMEDOUT) {
-                        if (retry_on_timeout-- > 0) {
-                                dev_err(&ftdi->udev->dev, "TIMED OUT with packe"
-                                        "t_bytes = %d with total %d bytes%s\n",
-                                        packet_bytes, bytes_read, diag);
-                                goto more;
-                        } else if (bytes_read > 0) {
-                                dev_err(&ftdi->udev->dev, "ONLY %d bytes%s\n",
-                                        bytes_read, diag);
-                                return -ENOMEM;
-                        } else {
-                                dev_err(&ftdi->udev->dev, "TIMED OUT with packe"
-                                        "t_bytes = %d with total %d bytes%s\n",
-                                        packet_bytes, bytes_read, diag);
-                                return -ENOMEM;
-                        }
-                } else if (retval == -EILSEQ) {
-                        dev_err(&ftdi->udev->dev, "error = %d with packet_bytes"
-                                " = %d with total %d bytes%s\n", retval,
-                                packet_bytes, bytes_read, diag);
-                        return retval;
-                } else if (retval) {
-                        dev_err(&ftdi->udev->dev, "error = %d with packet_bytes"
-                                " = %d with total %d bytes%s\n", retval,
-                                packet_bytes, bytes_read, diag);
-                        return retval;
-                } else if (packet_bytes == 2) {
-                        unsigned char s0 = ftdi->bulk_in_buffer[0];
-                        unsigned char s1 = ftdi->bulk_in_buffer[1];
-                        empty_packets += 1;
-                        if (s0 == 0x31 && s1 == 0x60) {
-                                if (retry_on_empty-- > 0) {
-                                        goto more;
-                                } else
-                                        return 0;
-                        } else if (s0 == 0x31 && s1 == 0x00) {
-                                if (retry_on_empty-- > 0) {
-                                        goto more;
-                                } else
-                                        return 0;
-                        } else {
-                                if (retry_on_empty-- > 0) {
-                                        goto more;
-                                } else
-                                        return 0;
-                        }
-                } else if (packet_bytes == 1) {
-                        if (retry_on_empty-- > 0) {
-                                goto more;
-                        } else
-                                return 0;
-                } else {
-                        if (retry_on_empty-- > 0) {
-                                goto more;
-                        } else
-                                return 0;
-                }
-        }
-      more:{
-                goto read;
-        }
-      have:if (ftdi->bulk_in_left > 0) {
-                u8 c = ftdi->bulk_in_buffer[++ftdi->bulk_in_last];
-                bytes_read += 1;
-                ftdi->bulk_in_left -= 1;
-                if (ftdi->received == 0 && c == 0xFF) {
-                        goto have;
-                } else
-                        *b++ = c;
-                if (++ftdi->received < ftdi->expected) {
-                        goto have;
-                } else if (ftdi->ed_found) {
-                        int ed_number = (ftdi->response[0] >> 5) & 0x03;
-                        u16 ed_length = (ftdi->response[2] << 8) |
-                                ftdi->response[1];
-                        struct u132_target *target = &ftdi->target[ed_number];
-                        int payload = (ed_length >> 0) & 0x07FF;
-                        char diag[30 *3 + 4];
-                        char *d = diag;
-                        int m = payload;
-                        u8 *c = 4 + ftdi->response;
-                        int s = (sizeof(diag) - 1) / 3;
-                        diag[0] = 0;
-                        while (s-- > 0 && m-- > 0) {
-                                if (s > 0 || m == 0) {
-                                        d += sprintf(d, " %02X", *c++);
-                                } else
-                                        d += sprintf(d, " ..");
-                        }
-                        ftdi_elan_do_callback(ftdi, target, 4 + ftdi->response,
-                                payload);
-                        ftdi->received = 0;
-                        ftdi->expected = 4;
-                        ftdi->ed_found = 0;
-                        b = ftdi->response;
-                        goto have;
-                } else if (ftdi->expected == 8) {
-                        u8 buscmd;
-                        int respond_head = ftdi->respond_head++;
-                        struct u132_respond *respond = &ftdi->respond[
-                                RESPOND_MASK & respond_head];
-                        u32 data = ftdi->response[7];
-                        data <<= 8;
-                        data |= ftdi->response[6];
-                        data <<= 8;
-                        data |= ftdi->response[5];
-                        data <<= 8;
-                        data |= ftdi->response[4];
-                        *respond->value = data;
-                        *respond->result = 0;
-                        complete(&respond->wait_completion);
-                        ftdi->received = 0;
-                        ftdi->expected = 4;
-                        ftdi->ed_found = 0;
-                        b = ftdi->response;
-                        buscmd = (ftdi->response[0] >> 0) & 0x0F;
-                        if (buscmd == 0x00) {
-                        } else if (buscmd == 0x02) {
-                        } else if (buscmd == 0x06) {
-                        } else if (buscmd == 0x0A) {
-                        } else
-                                dev_err(&ftdi->udev->dev, "Uxxx unknown(%0X) va"
-                                        "lue = %08X\n", buscmd, data);
-                        goto have;
-                } else {
-                        if ((ftdi->response[0] & 0x80) == 0x00) {
-                                ftdi->expected = 8;
-                                goto have;
-                        } else {
-                                int ed_number = (ftdi->response[0] >> 5) & 0x03;
-                                int ed_type = (ftdi->response[0] >> 0) & 0x03;
-                                u16 ed_length = (ftdi->response[2] << 8) |
-                                        ftdi->response[1];
-                                struct u132_target *target = &ftdi->target[
-                                        ed_number];
-                                target->halted = (ftdi->response[0] >> 3) &
-                                        0x01;
-                                target->skipped = (ftdi->response[0] >> 2) &
-                                        0x01;
-                                target->toggle_bits = (ftdi->response[3] >> 6)
-                                        & 0x03;
-                                target->error_count = (ftdi->response[3] >> 4)
-                                        & 0x03;
-                                target->condition_code = (ftdi->response[
-                                        3] >> 0) & 0x0F;
-                                if ((ftdi->response[0] & 0x10) == 0x00) {
-                                        b = have_ed_set_response(ftdi, target,
-                                                ed_length, ed_number, ed_type,
-                                                b);
-                                        goto have;
-                                } else {
-                                        b = have_ed_get_response(ftdi, target,
-                                                ed_length, ed_number, ed_type,
-                                                b);
-                                        goto have;
-                                }
-                        }
-                }
-        } else
-                goto more;
+	u8 *b = ftdi->response + ftdi->received;
+	int bytes_read = 0;
+	int retry_on_empty = 1;
+	int retry_on_timeout = 3;
+	int empty_packets = 0;
+read:{
+		int packet_bytes = 0;
+		int retval = usb_bulk_msg(ftdi->udev,
+					  usb_rcvbulkpipe(ftdi->udev, ftdi->bulk_in_endpointAddr),
+					  ftdi->bulk_in_buffer, ftdi->bulk_in_size,
+					  &packet_bytes, 500);
+		char diag[30 *3 + 4];
+		char *d = diag;
+		int m = packet_bytes;
+		u8 *c = ftdi->bulk_in_buffer;
+		int s = (sizeof(diag) - 1) / 3;
+		diag[0] = 0;
+		while (s-- > 0 && m-- > 0) {
+			if (s > 0 || m == 0) {
+				d += sprintf(d, " %02X", *c++);
+			} else
+				d += sprintf(d, " ..");
+		}
+		if (packet_bytes > 2) {
+			ftdi->bulk_in_left = packet_bytes - 2;
+			ftdi->bulk_in_last = 1;
+			goto have;
+		} else if (retval == -ETIMEDOUT) {
+			if (retry_on_timeout-- > 0) {
+				dev_err(&ftdi->udev->dev, "TIMED OUT with packet_bytes = %d with total %d bytes%s\n",
+					packet_bytes, bytes_read, diag);
+				goto more;
+			} else if (bytes_read > 0) {
+				dev_err(&ftdi->udev->dev, "ONLY %d bytes%s\n",
+					bytes_read, diag);
+				return -ENOMEM;
+			} else {
+				dev_err(&ftdi->udev->dev, "TIMED OUT with packet_bytes = %d with total %d bytes%s\n",
+					packet_bytes, bytes_read, diag);
+				return -ENOMEM;
+			}
+		} else if (retval == -EILSEQ) {
+			dev_err(&ftdi->udev->dev, "error = %d with packet_bytes = %d with total %d bytes%s\n",
+				retval, packet_bytes, bytes_read, diag);
+			return retval;
+		} else if (retval) {
+			dev_err(&ftdi->udev->dev, "error = %d with packet_bytes = %d with total %d bytes%s\n",
+				retval, packet_bytes, bytes_read, diag);
+			return retval;
+		} else if (packet_bytes == 2) {
+			unsigned char s0 = ftdi->bulk_in_buffer[0];
+			unsigned char s1 = ftdi->bulk_in_buffer[1];
+			empty_packets += 1;
+			if (s0 == 0x31 && s1 == 0x60) {
+				if (retry_on_empty-- > 0) {
+					goto more;
+				} else
+					return 0;
+			} else if (s0 == 0x31 && s1 == 0x00) {
+				if (retry_on_empty-- > 0) {
+					goto more;
+				} else
+					return 0;
+			} else {
+				if (retry_on_empty-- > 0) {
+					goto more;
+				} else
+					return 0;
+			}
+		} else if (packet_bytes == 1) {
+			if (retry_on_empty-- > 0) {
+				goto more;
+			} else
+				return 0;
+		} else {
+			if (retry_on_empty-- > 0) {
+				goto more;
+			} else
+				return 0;
+		}
+	}
+more:{
+		goto read;
+	}
+have:if (ftdi->bulk_in_left > 0) {
+		u8 c = ftdi->bulk_in_buffer[++ftdi->bulk_in_last];
+		bytes_read += 1;
+		ftdi->bulk_in_left -= 1;
+		if (ftdi->received == 0 && c == 0xFF) {
+			goto have;
+		} else
+			*b++ = c;
+		if (++ftdi->received < ftdi->expected) {
+			goto have;
+		} else if (ftdi->ed_found) {
+			int ed_number = (ftdi->response[0] >> 5) & 0x03;
+			u16 ed_length = (ftdi->response[2] << 8) |
+				ftdi->response[1];
+			struct u132_target *target = &ftdi->target[ed_number];
+			int payload = (ed_length >> 0) & 0x07FF;
+			char diag[30 *3 + 4];
+			char *d = diag;
+			int m = payload;
+			u8 *c = 4 + ftdi->response;
+			int s = (sizeof(diag) - 1) / 3;
+			diag[0] = 0;
+			while (s-- > 0 && m-- > 0) {
+				if (s > 0 || m == 0) {
+					d += sprintf(d, " %02X", *c++);
+				} else
+					d += sprintf(d, " ..");
+			}
+			ftdi_elan_do_callback(ftdi, target, 4 + ftdi->response,
+					      payload);
+			ftdi->received = 0;
+			ftdi->expected = 4;
+			ftdi->ed_found = 0;
+			b = ftdi->response;
+			goto have;
+		} else if (ftdi->expected == 8) {
+			u8 buscmd;
+			int respond_head = ftdi->respond_head++;
+			struct u132_respond *respond = &ftdi->respond[
+				RESPOND_MASK & respond_head];
+			u32 data = ftdi->response[7];
+			data <<= 8;
+			data |= ftdi->response[6];
+			data <<= 8;
+			data |= ftdi->response[5];
+			data <<= 8;
+			data |= ftdi->response[4];
+			*respond->value = data;
+			*respond->result = 0;
+			complete(&respond->wait_completion);
+			ftdi->received = 0;
+			ftdi->expected = 4;
+			ftdi->ed_found = 0;
+			b = ftdi->response;
+			buscmd = (ftdi->response[0] >> 0) & 0x0F;
+			if (buscmd == 0x00) {
+			} else if (buscmd == 0x02) {
+			} else if (buscmd == 0x06) {
+			} else if (buscmd == 0x0A) {
+			} else
+				dev_err(&ftdi->udev->dev, "Uxxx unknown(%0X) value = %08X\n",
+					buscmd, data);
+			goto have;
+		} else {
+			if ((ftdi->response[0] & 0x80) == 0x00) {
+				ftdi->expected = 8;
+				goto have;
+			} else {
+				int ed_number = (ftdi->response[0] >> 5) & 0x03;
+				int ed_type = (ftdi->response[0] >> 0) & 0x03;
+				u16 ed_length = (ftdi->response[2] << 8) |
+					ftdi->response[1];
+				struct u132_target *target = &ftdi->target[
+					ed_number];
+				target->halted = (ftdi->response[0] >> 3) &
+					0x01;
+				target->skipped = (ftdi->response[0] >> 2) &
+					0x01;
+				target->toggle_bits = (ftdi->response[3] >> 6)
+					& 0x03;
+				target->error_count = (ftdi->response[3] >> 4)
+					& 0x03;
+				target->condition_code = (ftdi->response[
+								  3] >> 0) & 0x0F;
+				if ((ftdi->response[0] & 0x10) == 0x00) {
+					b = have_ed_set_response(ftdi, target,
+								 ed_length, ed_number, ed_type,
+								 b);
+					goto have;
+				} else {
+					b = have_ed_get_response(ftdi, target,
+								 ed_length, ed_number, ed_type,
+								 b);
+					goto have;
+				}
+			}
+		}
+	} else
+		goto more;
 }
 
 
 /*
-* create a urb, and a buffer for it, and copy the data to the urb
-*
-*/
+ * create a urb, and a buffer for it, and copy the data to the urb
+ *
+ */
 static ssize_t ftdi_elan_write(struct file *file,
 			       const char __user *user_buffer, size_t count,
 			       loff_t *ppos)
 {
-        int retval = 0;
-        struct urb *urb;
-        char *buf;
-        struct usb_ftdi *ftdi = file->private_data;
-
-        if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        }
-        if (count == 0) {
-                goto exit;
-        }
-        urb = usb_alloc_urb(0, GFP_KERNEL);
-        if (!urb) {
-                retval = -ENOMEM;
-                goto error_1;
-        }
-        buf = usb_alloc_coherent(ftdi->udev, count, GFP_KERNEL,
-                &urb->transfer_dma);
-        if (!buf) {
-                retval = -ENOMEM;
-                goto error_2;
-        }
-        if (copy_from_user(buf, user_buffer, count)) {
-                retval = -EFAULT;
-                goto error_3;
-        }
-        usb_fill_bulk_urb(urb, ftdi->udev, usb_sndbulkpipe(ftdi->udev,
-                ftdi->bulk_out_endpointAddr), buf, count,
-                ftdi_elan_write_bulk_callback, ftdi);
-        urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
-        retval = usb_submit_urb(urb, GFP_KERNEL);
-        if (retval) {
-                dev_err(&ftdi->udev->dev, "failed submitting write urb, error %"
-                        "d\n", retval);
-                goto error_3;
-        }
-        usb_free_urb(urb);
+	int retval = 0;
+	struct urb *urb;
+	char *buf;
+	struct usb_ftdi *ftdi = file->private_data;
+
+	if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	}
+	if (count == 0) {
+		goto exit;
+	}
+	urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!urb) {
+		retval = -ENOMEM;
+		goto error_1;
+	}
+	buf = usb_alloc_coherent(ftdi->udev, count, GFP_KERNEL,
+				 &urb->transfer_dma);
+	if (!buf) {
+		retval = -ENOMEM;
+		goto error_2;
+	}
+	if (copy_from_user(buf, user_buffer, count)) {
+		retval = -EFAULT;
+		goto error_3;
+	}
+	usb_fill_bulk_urb(urb, ftdi->udev, usb_sndbulkpipe(ftdi->udev,
+							   ftdi->bulk_out_endpointAddr), buf, count,
+			  ftdi_elan_write_bulk_callback, ftdi);
+	urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+	retval = usb_submit_urb(urb, GFP_KERNEL);
+	if (retval) {
+		dev_err(&ftdi->udev->dev,
+			"failed submitting write urb, error %d\n", retval);
+		goto error_3;
+	}
+	usb_free_urb(urb);
 
 exit:
-        return count;
+	return count;
 error_3:
 	usb_free_coherent(ftdi->udev, count, buf, urb->transfer_dma);
 error_2:
@@ -1198,29 +1189,29 @@ error_1:
 }
 
 static const struct file_operations ftdi_elan_fops = {
-        .owner = THIS_MODULE,
-        .llseek = no_llseek,
-        .read = ftdi_elan_read,
-        .write = ftdi_elan_write,
-        .open = ftdi_elan_open,
-        .release = ftdi_elan_release,
+	.owner = THIS_MODULE,
+	.llseek = no_llseek,
+	.read = ftdi_elan_read,
+	.write = ftdi_elan_write,
+	.open = ftdi_elan_open,
+	.release = ftdi_elan_release,
 };
 
 /*
-* usb class driver info in order to get a minor number from the usb core,
-* and to have the device registered with the driver core
-*/
+ * usb class driver info in order to get a minor number from the usb core,
+ * and to have the device registered with the driver core
+ */
 static struct usb_class_driver ftdi_elan_jtag_class = {
-        .name = "ftdi-%d-jtag",
-        .fops = &ftdi_elan_fops,
-        .minor_base = USB_FTDI_ELAN_MINOR_BASE,
+	.name = "ftdi-%d-jtag",
+	.fops = &ftdi_elan_fops,
+	.minor_base = USB_FTDI_ELAN_MINOR_BASE,
 };
 
 /*
-* the following definitions are for the
-* ELAN FPGA state machgine processor that
-* lies on the other side of the FTDI chip
-*/
+ * the following definitions are for the
+ * ELAN FPGA state machgine processor that
+ * lies on the other side of the FTDI chip
+ */
 #define cPCIu132rd 0x0
 #define cPCIu132wr 0x1
 #define cPCIiord 0x2
@@ -1251,1694 +1242,1663 @@ static struct usb_class_driver ftdi_elan_jtag_class = {
 #define cCCnotaccessed 0xF
 static int ftdi_elan_write_reg(struct usb_ftdi *ftdi, u32 data)
 {
-      wait:if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else {
-                int command_size;
-                mutex_lock(&ftdi->u132_lock);
-                command_size = ftdi->command_next - ftdi->command_head;
-                if (command_size < COMMAND_SIZE) {
-                        struct u132_command *command = &ftdi->command[
-                                COMMAND_MASK & ftdi->command_next];
-                        command->header = 0x00 | cPCIu132wr;
-                        command->length = 0x04;
-                        command->address = 0x00;
-                        command->width = 0x00;
-                        command->follows = 4;
-                        command->value = data;
-                        command->buffer = &command->value;
-                        ftdi->command_next += 1;
-                        ftdi_elan_kick_command_queue(ftdi);
-                        mutex_unlock(&ftdi->u132_lock);
-                        return 0;
-                } else {
-                        mutex_unlock(&ftdi->u132_lock);
-                        msleep(100);
-                        goto wait;
-                }
-        }
+wait:if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else {
+		int command_size;
+		mutex_lock(&ftdi->u132_lock);
+		command_size = ftdi->command_next - ftdi->command_head;
+		if (command_size < COMMAND_SIZE) {
+			struct u132_command *command = &ftdi->command[
+				COMMAND_MASK & ftdi->command_next];
+			command->header = 0x00 | cPCIu132wr;
+			command->length = 0x04;
+			command->address = 0x00;
+			command->width = 0x00;
+			command->follows = 4;
+			command->value = data;
+			command->buffer = &command->value;
+			ftdi->command_next += 1;
+			ftdi_elan_kick_command_queue(ftdi);
+			mutex_unlock(&ftdi->u132_lock);
+			return 0;
+		} else {
+			mutex_unlock(&ftdi->u132_lock);
+			msleep(100);
+			goto wait;
+		}
+	}
 }
 
 static int ftdi_elan_write_config(struct usb_ftdi *ftdi, int config_offset,
-        u8 width, u32 data)
-{
-        u8 addressofs = config_offset / 4;
-      wait:if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else {
-                int command_size;
-                mutex_lock(&ftdi->u132_lock);
-                command_size = ftdi->command_next - ftdi->command_head;
-                if (command_size < COMMAND_SIZE) {
-                        struct u132_command *command = &ftdi->command[
-                                COMMAND_MASK & ftdi->command_next];
-                        command->header = 0x00 | (cPCIcfgwr & 0x0F);
-                        command->length = 0x04;
-                        command->address = addressofs;
-                        command->width = 0x00 | (width & 0x0F);
-                        command->follows = 4;
-                        command->value = data;
-                        command->buffer = &command->value;
-                        ftdi->command_next += 1;
-                        ftdi_elan_kick_command_queue(ftdi);
-                        mutex_unlock(&ftdi->u132_lock);
-                        return 0;
-                } else {
-                        mutex_unlock(&ftdi->u132_lock);
-                        msleep(100);
-                        goto wait;
-                }
-        }
+				  u8 width, u32 data)
+{
+	u8 addressofs = config_offset / 4;
+wait:if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else {
+		int command_size;
+		mutex_lock(&ftdi->u132_lock);
+		command_size = ftdi->command_next - ftdi->command_head;
+		if (command_size < COMMAND_SIZE) {
+			struct u132_command *command = &ftdi->command[
+				COMMAND_MASK & ftdi->command_next];
+			command->header = 0x00 | (cPCIcfgwr & 0x0F);
+			command->length = 0x04;
+			command->address = addressofs;
+			command->width = 0x00 | (width & 0x0F);
+			command->follows = 4;
+			command->value = data;
+			command->buffer = &command->value;
+			ftdi->command_next += 1;
+			ftdi_elan_kick_command_queue(ftdi);
+			mutex_unlock(&ftdi->u132_lock);
+			return 0;
+		} else {
+			mutex_unlock(&ftdi->u132_lock);
+			msleep(100);
+			goto wait;
+		}
+	}
 }
 
 static int ftdi_elan_write_pcimem(struct usb_ftdi *ftdi, int mem_offset,
-        u8 width, u32 data)
-{
-        u8 addressofs = mem_offset / 4;
-      wait:if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else {
-                int command_size;
-                mutex_lock(&ftdi->u132_lock);
-                command_size = ftdi->command_next - ftdi->command_head;
-                if (command_size < COMMAND_SIZE) {
-                        struct u132_command *command = &ftdi->command[
-                                COMMAND_MASK & ftdi->command_next];
-                        command->header = 0x00 | (cPCImemwr & 0x0F);
-                        command->length = 0x04;
-                        command->address = addressofs;
-                        command->width = 0x00 | (width & 0x0F);
-                        command->follows = 4;
-                        command->value = data;
-                        command->buffer = &command->value;
-                        ftdi->command_next += 1;
-                        ftdi_elan_kick_command_queue(ftdi);
-                        mutex_unlock(&ftdi->u132_lock);
-                        return 0;
-                } else {
-                        mutex_unlock(&ftdi->u132_lock);
-                        msleep(100);
-                        goto wait;
-                }
-        }
+				  u8 width, u32 data)
+{
+	u8 addressofs = mem_offset / 4;
+wait:if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else {
+		int command_size;
+		mutex_lock(&ftdi->u132_lock);
+		command_size = ftdi->command_next - ftdi->command_head;
+		if (command_size < COMMAND_SIZE) {
+			struct u132_command *command = &ftdi->command[
+				COMMAND_MASK & ftdi->command_next];
+			command->header = 0x00 | (cPCImemwr & 0x0F);
+			command->length = 0x04;
+			command->address = addressofs;
+			command->width = 0x00 | (width & 0x0F);
+			command->follows = 4;
+			command->value = data;
+			command->buffer = &command->value;
+			ftdi->command_next += 1;
+			ftdi_elan_kick_command_queue(ftdi);
+			mutex_unlock(&ftdi->u132_lock);
+			return 0;
+		} else {
+			mutex_unlock(&ftdi->u132_lock);
+			msleep(100);
+			goto wait;
+		}
+	}
 }
 
 int usb_ftdi_elan_write_pcimem(struct platform_device *pdev, int mem_offset,
-        u8 width, u32 data)
+			       u8 width, u32 data)
 {
-        struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
-        return ftdi_elan_write_pcimem(ftdi, mem_offset, width, data);
+	struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
+	return ftdi_elan_write_pcimem(ftdi, mem_offset, width, data);
 }
 
 
 EXPORT_SYMBOL_GPL(usb_ftdi_elan_write_pcimem);
 static int ftdi_elan_read_reg(struct usb_ftdi *ftdi, u32 *data)
 {
-      wait:if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else {
-                int command_size;
-                int respond_size;
-                mutex_lock(&ftdi->u132_lock);
-                command_size = ftdi->command_next - ftdi->command_head;
-                respond_size = ftdi->respond_next - ftdi->respond_head;
-                if (command_size < COMMAND_SIZE && respond_size < RESPOND_SIZE)
-                        {
-                        struct u132_command *command = &ftdi->command[
-                                COMMAND_MASK & ftdi->command_next];
-                        struct u132_respond *respond = &ftdi->respond[
-                                RESPOND_MASK & ftdi->respond_next];
-                        int result = -ENODEV;
-                        respond->result = &result;
-                        respond->header = command->header = 0x00 | cPCIu132rd;
-                        command->length = 0x04;
-                        respond->address = command->address = cU132cmd_status;
-                        command->width = 0x00;
-                        command->follows = 0;
-                        command->value = 0;
-                        command->buffer = NULL;
-                        respond->value = data;
-                        init_completion(&respond->wait_completion);
-                        ftdi->command_next += 1;
-                        ftdi->respond_next += 1;
-                        ftdi_elan_kick_command_queue(ftdi);
-                        mutex_unlock(&ftdi->u132_lock);
-                        wait_for_completion(&respond->wait_completion);
-                        return result;
-                } else {
-                        mutex_unlock(&ftdi->u132_lock);
-                        msleep(100);
-                        goto wait;
-                }
-        }
+wait:if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else {
+		int command_size;
+		int respond_size;
+		mutex_lock(&ftdi->u132_lock);
+		command_size = ftdi->command_next - ftdi->command_head;
+		respond_size = ftdi->respond_next - ftdi->respond_head;
+		if (command_size < COMMAND_SIZE && respond_size < RESPOND_SIZE)
+		{
+			struct u132_command *command = &ftdi->command[
+				COMMAND_MASK & ftdi->command_next];
+			struct u132_respond *respond = &ftdi->respond[
+				RESPOND_MASK & ftdi->respond_next];
+			int result = -ENODEV;
+			respond->result = &result;
+			respond->header = command->header = 0x00 | cPCIu132rd;
+			command->length = 0x04;
+			respond->address = command->address = cU132cmd_status;
+			command->width = 0x00;
+			command->follows = 0;
+			command->value = 0;
+			command->buffer = NULL;
+			respond->value = data;
+			init_completion(&respond->wait_completion);
+			ftdi->command_next += 1;
+			ftdi->respond_next += 1;
+			ftdi_elan_kick_command_queue(ftdi);
+			mutex_unlock(&ftdi->u132_lock);
+			wait_for_completion(&respond->wait_completion);
+			return result;
+		} else {
+			mutex_unlock(&ftdi->u132_lock);
+			msleep(100);
+			goto wait;
+		}
+	}
 }
 
 static int ftdi_elan_read_config(struct usb_ftdi *ftdi, int config_offset,
-        u8 width, u32 *data)
-{
-        u8 addressofs = config_offset / 4;
-      wait:if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else {
-                int command_size;
-                int respond_size;
-                mutex_lock(&ftdi->u132_lock);
-                command_size = ftdi->command_next - ftdi->command_head;
-                respond_size = ftdi->respond_next - ftdi->respond_head;
-                if (command_size < COMMAND_SIZE && respond_size < RESPOND_SIZE)
-                        {
-                        struct u132_command *command = &ftdi->command[
-                                COMMAND_MASK & ftdi->command_next];
-                        struct u132_respond *respond = &ftdi->respond[
-                                RESPOND_MASK & ftdi->respond_next];
-                        int result = -ENODEV;
-                        respond->result = &result;
-                        respond->header = command->header = 0x00 | (cPCIcfgrd &
-                                0x0F);
-                        command->length = 0x04;
-                        respond->address = command->address = addressofs;
-                        command->width = 0x00 | (width & 0x0F);
-                        command->follows = 0;
-                        command->value = 0;
-                        command->buffer = NULL;
-                        respond->value = data;
-                        init_completion(&respond->wait_completion);
-                        ftdi->command_next += 1;
-                        ftdi->respond_next += 1;
-                        ftdi_elan_kick_command_queue(ftdi);
-                        mutex_unlock(&ftdi->u132_lock);
-                        wait_for_completion(&respond->wait_completion);
-                        return result;
-                } else {
-                        mutex_unlock(&ftdi->u132_lock);
-                        msleep(100);
-                        goto wait;
-                }
-        }
+				 u8 width, u32 *data)
+{
+	u8 addressofs = config_offset / 4;
+wait:if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else {
+		int command_size;
+		int respond_size;
+		mutex_lock(&ftdi->u132_lock);
+		command_size = ftdi->command_next - ftdi->command_head;
+		respond_size = ftdi->respond_next - ftdi->respond_head;
+		if (command_size < COMMAND_SIZE && respond_size < RESPOND_SIZE)
+		{
+			struct u132_command *command = &ftdi->command[
+				COMMAND_MASK & ftdi->command_next];
+			struct u132_respond *respond = &ftdi->respond[
+				RESPOND_MASK & ftdi->respond_next];
+			int result = -ENODEV;
+			respond->result = &result;
+			respond->header = command->header = 0x00 | (cPCIcfgrd &
+								    0x0F);
+			command->length = 0x04;
+			respond->address = command->address = addressofs;
+			command->width = 0x00 | (width & 0x0F);
+			command->follows = 0;
+			command->value = 0;
+			command->buffer = NULL;
+			respond->value = data;
+			init_completion(&respond->wait_completion);
+			ftdi->command_next += 1;
+			ftdi->respond_next += 1;
+			ftdi_elan_kick_command_queue(ftdi);
+			mutex_unlock(&ftdi->u132_lock);
+			wait_for_completion(&respond->wait_completion);
+			return result;
+		} else {
+			mutex_unlock(&ftdi->u132_lock);
+			msleep(100);
+			goto wait;
+		}
+	}
 }
 
 static int ftdi_elan_read_pcimem(struct usb_ftdi *ftdi, int mem_offset,
-        u8 width, u32 *data)
-{
-        u8 addressofs = mem_offset / 4;
-      wait:if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else {
-                int command_size;
-                int respond_size;
-                mutex_lock(&ftdi->u132_lock);
-                command_size = ftdi->command_next - ftdi->command_head;
-                respond_size = ftdi->respond_next - ftdi->respond_head;
-                if (command_size < COMMAND_SIZE && respond_size < RESPOND_SIZE)
-                        {
-                        struct u132_command *command = &ftdi->command[
-                                COMMAND_MASK & ftdi->command_next];
-                        struct u132_respond *respond = &ftdi->respond[
-                                RESPOND_MASK & ftdi->respond_next];
-                        int result = -ENODEV;
-                        respond->result = &result;
-                        respond->header = command->header = 0x00 | (cPCImemrd &
-                                0x0F);
-                        command->length = 0x04;
-                        respond->address = command->address = addressofs;
-                        command->width = 0x00 | (width & 0x0F);
-                        command->follows = 0;
-                        command->value = 0;
-                        command->buffer = NULL;
-                        respond->value = data;
-                        init_completion(&respond->wait_completion);
-                        ftdi->command_next += 1;
-                        ftdi->respond_next += 1;
-                        ftdi_elan_kick_command_queue(ftdi);
-                        mutex_unlock(&ftdi->u132_lock);
-                        wait_for_completion(&respond->wait_completion);
-                        return result;
-                } else {
-                        mutex_unlock(&ftdi->u132_lock);
-                        msleep(100);
-                        goto wait;
-                }
-        }
+				 u8 width, u32 *data)
+{
+	u8 addressofs = mem_offset / 4;
+wait:if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else {
+		int command_size;
+		int respond_size;
+		mutex_lock(&ftdi->u132_lock);
+		command_size = ftdi->command_next - ftdi->command_head;
+		respond_size = ftdi->respond_next - ftdi->respond_head;
+		if (command_size < COMMAND_SIZE && respond_size < RESPOND_SIZE)
+		{
+			struct u132_command *command = &ftdi->command[
+				COMMAND_MASK & ftdi->command_next];
+			struct u132_respond *respond = &ftdi->respond[
+				RESPOND_MASK & ftdi->respond_next];
+			int result = -ENODEV;
+			respond->result = &result;
+			respond->header = command->header = 0x00 | (cPCImemrd &
+								    0x0F);
+			command->length = 0x04;
+			respond->address = command->address = addressofs;
+			command->width = 0x00 | (width & 0x0F);
+			command->follows = 0;
+			command->value = 0;
+			command->buffer = NULL;
+			respond->value = data;
+			init_completion(&respond->wait_completion);
+			ftdi->command_next += 1;
+			ftdi->respond_next += 1;
+			ftdi_elan_kick_command_queue(ftdi);
+			mutex_unlock(&ftdi->u132_lock);
+			wait_for_completion(&respond->wait_completion);
+			return result;
+		} else {
+			mutex_unlock(&ftdi->u132_lock);
+			msleep(100);
+			goto wait;
+		}
+	}
 }
 
 int usb_ftdi_elan_read_pcimem(struct platform_device *pdev, int mem_offset,
-        u8 width, u32 *data)
+			      u8 width, u32 *data)
 {
-        struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
-        if (ftdi->initialized == 0) {
-                return -ENODEV;
-        } else
-                return ftdi_elan_read_pcimem(ftdi, mem_offset, width, data);
+	struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
+	if (ftdi->initialized == 0) {
+		return -ENODEV;
+	} else
+		return ftdi_elan_read_pcimem(ftdi, mem_offset, width, data);
 }
 
 
 EXPORT_SYMBOL_GPL(usb_ftdi_elan_read_pcimem);
 static int ftdi_elan_edset_setup(struct usb_ftdi *ftdi, u8 ed_number,
-        void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
-        void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
-        int toggle_bits, int error_count, int condition_code, int repeat_number,
-         int halted, int skipped, int actual, int non_null))
-{
-        u8 ed = ed_number - 1;
-      wait:if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else if (ftdi->initialized == 0) {
-                return -ENODEV;
-        } else {
-                int command_size;
-                mutex_lock(&ftdi->u132_lock);
-                command_size = ftdi->command_next - ftdi->command_head;
-                if (command_size < COMMAND_SIZE) {
-                        struct u132_target *target = &ftdi->target[ed];
-                        struct u132_command *command = &ftdi->command[
-                                COMMAND_MASK & ftdi->command_next];
-                        command->header = 0x80 | (ed << 5);
-                        command->length = 0x8007;
-                        command->address = (toggle_bits << 6) | (ep_number << 2)
-                                | (address << 0);
-                        command->width = usb_maxpacket(urb->dev, urb->pipe,
-                                usb_pipeout(urb->pipe));
-                        command->follows = 8;
-                        command->value = 0;
-                        command->buffer = urb->setup_packet;
-                        target->callback = callback;
-                        target->endp = endp;
-                        target->urb = urb;
-                        target->active = 1;
-                        ftdi->command_next += 1;
-                        ftdi_elan_kick_command_queue(ftdi);
-                        mutex_unlock(&ftdi->u132_lock);
-                        return 0;
-                } else {
-                        mutex_unlock(&ftdi->u132_lock);
-                        msleep(100);
-                        goto wait;
-                }
-        }
+				 void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
+				 void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
+						   int toggle_bits, int error_count, int condition_code, int repeat_number,
+						   int halted, int skipped, int actual, int non_null))
+{
+	u8 ed = ed_number - 1;
+wait:if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else if (ftdi->initialized == 0) {
+		return -ENODEV;
+	} else {
+		int command_size;
+		mutex_lock(&ftdi->u132_lock);
+		command_size = ftdi->command_next - ftdi->command_head;
+		if (command_size < COMMAND_SIZE) {
+			struct u132_target *target = &ftdi->target[ed];
+			struct u132_command *command = &ftdi->command[
+				COMMAND_MASK & ftdi->command_next];
+			command->header = 0x80 | (ed << 5);
+			command->length = 0x8007;
+			command->address = (toggle_bits << 6) | (ep_number << 2)
+				| (address << 0);
+			command->width = usb_maxpacket(urb->dev, urb->pipe,
+						       usb_pipeout(urb->pipe));
+			command->follows = 8;
+			command->value = 0;
+			command->buffer = urb->setup_packet;
+			target->callback = callback;
+			target->endp = endp;
+			target->urb = urb;
+			target->active = 1;
+			ftdi->command_next += 1;
+			ftdi_elan_kick_command_queue(ftdi);
+			mutex_unlock(&ftdi->u132_lock);
+			return 0;
+		} else {
+			mutex_unlock(&ftdi->u132_lock);
+			msleep(100);
+			goto wait;
+		}
+	}
 }
 
 int usb_ftdi_elan_edset_setup(struct platform_device *pdev, u8 ed_number,
-        void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
-        void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
-        int toggle_bits, int error_count, int condition_code, int repeat_number,
-         int halted, int skipped, int actual, int non_null))
+			      void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
+			      void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
+						int toggle_bits, int error_count, int condition_code, int repeat_number,
+						int halted, int skipped, int actual, int non_null))
 {
-        struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
-        return ftdi_elan_edset_setup(ftdi, ed_number, endp, urb, address,
-                ep_number, toggle_bits, callback);
+	struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
+	return ftdi_elan_edset_setup(ftdi, ed_number, endp, urb, address,
+				     ep_number, toggle_bits, callback);
 }
 
 
 EXPORT_SYMBOL_GPL(usb_ftdi_elan_edset_setup);
 static int ftdi_elan_edset_input(struct usb_ftdi *ftdi, u8 ed_number,
-        void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
-        void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
-        int toggle_bits, int error_count, int condition_code, int repeat_number,
-         int halted, int skipped, int actual, int non_null))
-{
-        u8 ed = ed_number - 1;
-      wait:if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else if (ftdi->initialized == 0) {
-                return -ENODEV;
-        } else {
-                int command_size;
-                mutex_lock(&ftdi->u132_lock);
-                command_size = ftdi->command_next - ftdi->command_head;
-                if (command_size < COMMAND_SIZE) {
-                        struct u132_target *target = &ftdi->target[ed];
-                        struct u132_command *command = &ftdi->command[
-                                COMMAND_MASK & ftdi->command_next];
-                        u32 remaining_length = urb->transfer_buffer_length -
-                                urb->actual_length;
-                        command->header = 0x82 | (ed << 5);
-                        if (remaining_length == 0) {
-                                command->length = 0x0000;
-                        } else if (remaining_length > 1024) {
-                                command->length = 0x8000 | 1023;
-                        } else
-                                command->length = 0x8000 | (remaining_length -
-                                        1);
-                        command->address = (toggle_bits << 6) | (ep_number << 2)
-                                | (address << 0);
-                        command->width = usb_maxpacket(urb->dev, urb->pipe,
-                                usb_pipeout(urb->pipe));
-                        command->follows = 0;
-                        command->value = 0;
-                        command->buffer = NULL;
-                        target->callback = callback;
-                        target->endp = endp;
-                        target->urb = urb;
-                        target->active = 1;
-                        ftdi->command_next += 1;
-                        ftdi_elan_kick_command_queue(ftdi);
-                        mutex_unlock(&ftdi->u132_lock);
-                        return 0;
-                } else {
-                        mutex_unlock(&ftdi->u132_lock);
-                        msleep(100);
-                        goto wait;
-                }
-        }
+				 void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
+				 void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
+						   int toggle_bits, int error_count, int condition_code, int repeat_number,
+						   int halted, int skipped, int actual, int non_null))
+{
+	u8 ed = ed_number - 1;
+wait:if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else if (ftdi->initialized == 0) {
+		return -ENODEV;
+	} else {
+		int command_size;
+		mutex_lock(&ftdi->u132_lock);
+		command_size = ftdi->command_next - ftdi->command_head;
+		if (command_size < COMMAND_SIZE) {
+			struct u132_target *target = &ftdi->target[ed];
+			struct u132_command *command = &ftdi->command[
+				COMMAND_MASK & ftdi->command_next];
+			u32 remaining_length = urb->transfer_buffer_length -
+				urb->actual_length;
+			command->header = 0x82 | (ed << 5);
+			if (remaining_length == 0) {
+				command->length = 0x0000;
+			} else if (remaining_length > 1024) {
+				command->length = 0x8000 | 1023;
+			} else
+				command->length = 0x8000 | (remaining_length -
+							    1);
+			command->address = (toggle_bits << 6) | (ep_number << 2)
+				| (address << 0);
+			command->width = usb_maxpacket(urb->dev, urb->pipe,
+						       usb_pipeout(urb->pipe));
+			command->follows = 0;
+			command->value = 0;
+			command->buffer = NULL;
+			target->callback = callback;
+			target->endp = endp;
+			target->urb = urb;
+			target->active = 1;
+			ftdi->command_next += 1;
+			ftdi_elan_kick_command_queue(ftdi);
+			mutex_unlock(&ftdi->u132_lock);
+			return 0;
+		} else {
+			mutex_unlock(&ftdi->u132_lock);
+			msleep(100);
+			goto wait;
+		}
+	}
 }
 
 int usb_ftdi_elan_edset_input(struct platform_device *pdev, u8 ed_number,
-        void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
-        void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
-        int toggle_bits, int error_count, int condition_code, int repeat_number,
-         int halted, int skipped, int actual, int non_null))
+			      void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
+			      void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
+						int toggle_bits, int error_count, int condition_code, int repeat_number,
+						int halted, int skipped, int actual, int non_null))
 {
-        struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
-        return ftdi_elan_edset_input(ftdi, ed_number, endp, urb, address,
-                ep_number, toggle_bits, callback);
+	struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
+	return ftdi_elan_edset_input(ftdi, ed_number, endp, urb, address,
+				     ep_number, toggle_bits, callback);
 }
 
 
 EXPORT_SYMBOL_GPL(usb_ftdi_elan_edset_input);
 static int ftdi_elan_edset_empty(struct usb_ftdi *ftdi, u8 ed_number,
-        void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
-        void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
-        int toggle_bits, int error_count, int condition_code, int repeat_number,
-         int halted, int skipped, int actual, int non_null))
-{
-        u8 ed = ed_number - 1;
-      wait:if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else if (ftdi->initialized == 0) {
-                return -ENODEV;
-        } else {
-                int command_size;
-                mutex_lock(&ftdi->u132_lock);
-                command_size = ftdi->command_next - ftdi->command_head;
-                if (command_size < COMMAND_SIZE) {
-                        struct u132_target *target = &ftdi->target[ed];
-                        struct u132_command *command = &ftdi->command[
-                                COMMAND_MASK & ftdi->command_next];
-                        command->header = 0x81 | (ed << 5);
-                        command->length = 0x0000;
-                        command->address = (toggle_bits << 6) | (ep_number << 2)
-                                | (address << 0);
-                        command->width = usb_maxpacket(urb->dev, urb->pipe,
-                                usb_pipeout(urb->pipe));
-                        command->follows = 0;
-                        command->value = 0;
-                        command->buffer = NULL;
-                        target->callback = callback;
-                        target->endp = endp;
-                        target->urb = urb;
-                        target->active = 1;
-                        ftdi->command_next += 1;
-                        ftdi_elan_kick_command_queue(ftdi);
-                        mutex_unlock(&ftdi->u132_lock);
-                        return 0;
-                } else {
-                        mutex_unlock(&ftdi->u132_lock);
-                        msleep(100);
-                        goto wait;
-                }
-        }
+				 void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
+				 void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
+						   int toggle_bits, int error_count, int condition_code, int repeat_number,
+						   int halted, int skipped, int actual, int non_null))
+{
+	u8 ed = ed_number - 1;
+wait:if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else if (ftdi->initialized == 0) {
+		return -ENODEV;
+	} else {
+		int command_size;
+		mutex_lock(&ftdi->u132_lock);
+		command_size = ftdi->command_next - ftdi->command_head;
+		if (command_size < COMMAND_SIZE) {
+			struct u132_target *target = &ftdi->target[ed];
+			struct u132_command *command = &ftdi->command[
+				COMMAND_MASK & ftdi->command_next];
+			command->header = 0x81 | (ed << 5);
+			command->length = 0x0000;
+			command->address = (toggle_bits << 6) | (ep_number << 2)
+				| (address << 0);
+			command->width = usb_maxpacket(urb->dev, urb->pipe,
+						       usb_pipeout(urb->pipe));
+			command->follows = 0;
+			command->value = 0;
+			command->buffer = NULL;
+			target->callback = callback;
+			target->endp = endp;
+			target->urb = urb;
+			target->active = 1;
+			ftdi->command_next += 1;
+			ftdi_elan_kick_command_queue(ftdi);
+			mutex_unlock(&ftdi->u132_lock);
+			return 0;
+		} else {
+			mutex_unlock(&ftdi->u132_lock);
+			msleep(100);
+			goto wait;
+		}
+	}
 }
 
 int usb_ftdi_elan_edset_empty(struct platform_device *pdev, u8 ed_number,
-        void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
-        void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
-        int toggle_bits, int error_count, int condition_code, int repeat_number,
-         int halted, int skipped, int actual, int non_null))
+			      void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
+			      void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
+						int toggle_bits, int error_count, int condition_code, int repeat_number,
+						int halted, int skipped, int actual, int non_null))
 {
-        struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
-        return ftdi_elan_edset_empty(ftdi, ed_number, endp, urb, address,
-                ep_number, toggle_bits, callback);
+	struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
+	return ftdi_elan_edset_empty(ftdi, ed_number, endp, urb, address,
+				     ep_number, toggle_bits, callback);
 }
 
 
 EXPORT_SYMBOL_GPL(usb_ftdi_elan_edset_empty);
 static int ftdi_elan_edset_output(struct usb_ftdi *ftdi, u8 ed_number,
-        void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
-        void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
-        int toggle_bits, int error_count, int condition_code, int repeat_number,
-         int halted, int skipped, int actual, int non_null))
-{
-        u8 ed = ed_number - 1;
-      wait:if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else if (ftdi->initialized == 0) {
-                return -ENODEV;
-        } else {
-                int command_size;
-                mutex_lock(&ftdi->u132_lock);
-                command_size = ftdi->command_next - ftdi->command_head;
-                if (command_size < COMMAND_SIZE) {
-                        u8 *b;
-                        u16 urb_size;
-                        int i = 0;
-                        char data[30 *3 + 4];
-                        char *d = data;
-                        int m = (sizeof(data) - 1) / 3;
-                        int l = 0;
-                        struct u132_target *target = &ftdi->target[ed];
-                        struct u132_command *command = &ftdi->command[
-                                COMMAND_MASK & ftdi->command_next];
-                        command->header = 0x81 | (ed << 5);
-                        command->address = (toggle_bits << 6) | (ep_number << 2)
-                                | (address << 0);
-                        command->width = usb_maxpacket(urb->dev, urb->pipe,
-                                usb_pipeout(urb->pipe));
-                        command->follows = min_t(u32, 1024,
-                                urb->transfer_buffer_length -
-                                urb->actual_length);
-                        command->value = 0;
-                        command->buffer = urb->transfer_buffer +
-                                urb->actual_length;
-                        command->length = 0x8000 | (command->follows - 1);
-                        b = command->buffer;
-                        urb_size = command->follows;
-                        data[0] = 0;
-                        while (urb_size-- > 0) {
-                                if (i > m) {
-                                } else if (i++ < m) {
-                                        int w = sprintf(d, " %02X", *b++);
-                                        d += w;
-                                        l += w;
-                                } else
-                                        d += sprintf(d, " ..");
-                        }
-                        target->callback = callback;
-                        target->endp = endp;
-                        target->urb = urb;
-                        target->active = 1;
-                        ftdi->command_next += 1;
-                        ftdi_elan_kick_command_queue(ftdi);
-                        mutex_unlock(&ftdi->u132_lock);
-                        return 0;
-                } else {
-                        mutex_unlock(&ftdi->u132_lock);
-                        msleep(100);
-                        goto wait;
-                }
-        }
+				  void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
+				  void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
+						    int toggle_bits, int error_count, int condition_code, int repeat_number,
+						    int halted, int skipped, int actual, int non_null))
+{
+	u8 ed = ed_number - 1;
+wait:if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else if (ftdi->initialized == 0) {
+		return -ENODEV;
+	} else {
+		int command_size;
+		mutex_lock(&ftdi->u132_lock);
+		command_size = ftdi->command_next - ftdi->command_head;
+		if (command_size < COMMAND_SIZE) {
+			u8 *b;
+			u16 urb_size;
+			int i = 0;
+			char data[30 *3 + 4];
+			char *d = data;
+			int m = (sizeof(data) - 1) / 3;
+			int l = 0;
+			struct u132_target *target = &ftdi->target[ed];
+			struct u132_command *command = &ftdi->command[
+				COMMAND_MASK & ftdi->command_next];
+			command->header = 0x81 | (ed << 5);
+			command->address = (toggle_bits << 6) | (ep_number << 2)
+				| (address << 0);
+			command->width = usb_maxpacket(urb->dev, urb->pipe,
+						       usb_pipeout(urb->pipe));
+			command->follows = min_t(u32, 1024,
+						 urb->transfer_buffer_length -
+						 urb->actual_length);
+			command->value = 0;
+			command->buffer = urb->transfer_buffer +
+				urb->actual_length;
+			command->length = 0x8000 | (command->follows - 1);
+			b = command->buffer;
+			urb_size = command->follows;
+			data[0] = 0;
+			while (urb_size-- > 0) {
+				if (i > m) {
+				} else if (i++ < m) {
+					int w = sprintf(d, " %02X", *b++);
+					d += w;
+					l += w;
+				} else
+					d += sprintf(d, " ..");
+			}
+			target->callback = callback;
+			target->endp = endp;
+			target->urb = urb;
+			target->active = 1;
+			ftdi->command_next += 1;
+			ftdi_elan_kick_command_queue(ftdi);
+			mutex_unlock(&ftdi->u132_lock);
+			return 0;
+		} else {
+			mutex_unlock(&ftdi->u132_lock);
+			msleep(100);
+			goto wait;
+		}
+	}
 }
 
 int usb_ftdi_elan_edset_output(struct platform_device *pdev, u8 ed_number,
-        void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
-        void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
-        int toggle_bits, int error_count, int condition_code, int repeat_number,
-         int halted, int skipped, int actual, int non_null))
+			       void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
+			       void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
+						 int toggle_bits, int error_count, int condition_code, int repeat_number,
+						 int halted, int skipped, int actual, int non_null))
 {
-        struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
-        return ftdi_elan_edset_output(ftdi, ed_number, endp, urb, address,
-                ep_number, toggle_bits, callback);
+	struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
+	return ftdi_elan_edset_output(ftdi, ed_number, endp, urb, address,
+				      ep_number, toggle_bits, callback);
 }
 
 
 EXPORT_SYMBOL_GPL(usb_ftdi_elan_edset_output);
 static int ftdi_elan_edset_single(struct usb_ftdi *ftdi, u8 ed_number,
-        void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
-        void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
-        int toggle_bits, int error_count, int condition_code, int repeat_number,
-         int halted, int skipped, int actual, int non_null))
-{
-        u8 ed = ed_number - 1;
-      wait:if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else if (ftdi->initialized == 0) {
-                return -ENODEV;
-        } else {
-                int command_size;
-                mutex_lock(&ftdi->u132_lock);
-                command_size = ftdi->command_next - ftdi->command_head;
-                if (command_size < COMMAND_SIZE) {
-                        u32 remaining_length = urb->transfer_buffer_length -
-                                urb->actual_length;
-                        struct u132_target *target = &ftdi->target[ed];
-                        struct u132_command *command = &ftdi->command[
-                                COMMAND_MASK & ftdi->command_next];
-                        command->header = 0x83 | (ed << 5);
-                        if (remaining_length == 0) {
-                                command->length = 0x0000;
-                        } else if (remaining_length > 1024) {
-                                command->length = 0x8000 | 1023;
-                        } else
-                                command->length = 0x8000 | (remaining_length -
-                                        1);
-                        command->address = (toggle_bits << 6) | (ep_number << 2)
-                                | (address << 0);
-                        command->width = usb_maxpacket(urb->dev, urb->pipe,
-                                usb_pipeout(urb->pipe));
-                        command->follows = 0;
-                        command->value = 0;
-                        command->buffer = NULL;
-                        target->callback = callback;
-                        target->endp = endp;
-                        target->urb = urb;
-                        target->active = 1;
-                        ftdi->command_next += 1;
-                        ftdi_elan_kick_command_queue(ftdi);
-                        mutex_unlock(&ftdi->u132_lock);
-                        return 0;
-                } else {
-                        mutex_unlock(&ftdi->u132_lock);
-                        msleep(100);
-                        goto wait;
-                }
-        }
+				  void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
+				  void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
+						    int toggle_bits, int error_count, int condition_code, int repeat_number,
+						    int halted, int skipped, int actual, int non_null))
+{
+	u8 ed = ed_number - 1;
+wait:if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else if (ftdi->initialized == 0) {
+		return -ENODEV;
+	} else {
+		int command_size;
+		mutex_lock(&ftdi->u132_lock);
+		command_size = ftdi->command_next - ftdi->command_head;
+		if (command_size < COMMAND_SIZE) {
+			u32 remaining_length = urb->transfer_buffer_length -
+				urb->actual_length;
+			struct u132_target *target = &ftdi->target[ed];
+			struct u132_command *command = &ftdi->command[
+				COMMAND_MASK & ftdi->command_next];
+			command->header = 0x83 | (ed << 5);
+			if (remaining_length == 0) {
+				command->length = 0x0000;
+			} else if (remaining_length > 1024) {
+				command->length = 0x8000 | 1023;
+			} else
+				command->length = 0x8000 | (remaining_length -
+							    1);
+			command->address = (toggle_bits << 6) | (ep_number << 2)
+				| (address << 0);
+			command->width = usb_maxpacket(urb->dev, urb->pipe,
+						       usb_pipeout(urb->pipe));
+			command->follows = 0;
+			command->value = 0;
+			command->buffer = NULL;
+			target->callback = callback;
+			target->endp = endp;
+			target->urb = urb;
+			target->active = 1;
+			ftdi->command_next += 1;
+			ftdi_elan_kick_command_queue(ftdi);
+			mutex_unlock(&ftdi->u132_lock);
+			return 0;
+		} else {
+			mutex_unlock(&ftdi->u132_lock);
+			msleep(100);
+			goto wait;
+		}
+	}
 }
 
 int usb_ftdi_elan_edset_single(struct platform_device *pdev, u8 ed_number,
-        void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
-        void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
-        int toggle_bits, int error_count, int condition_code, int repeat_number,
-         int halted, int skipped, int actual, int non_null))
+			       void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
+			       void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
+						 int toggle_bits, int error_count, int condition_code, int repeat_number,
+						 int halted, int skipped, int actual, int non_null))
 {
-        struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
-        return ftdi_elan_edset_single(ftdi, ed_number, endp, urb, address,
-                ep_number, toggle_bits, callback);
+	struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
+	return ftdi_elan_edset_single(ftdi, ed_number, endp, urb, address,
+				      ep_number, toggle_bits, callback);
 }
 
 
 EXPORT_SYMBOL_GPL(usb_ftdi_elan_edset_single);
 static int ftdi_elan_edset_flush(struct usb_ftdi *ftdi, u8 ed_number,
-        void *endp)
-{
-        u8 ed = ed_number - 1;
-        if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else if (ftdi->initialized == 0) {
-                return -ENODEV;
-        } else {
-                struct u132_target *target = &ftdi->target[ed];
-                mutex_lock(&ftdi->u132_lock);
-                if (target->abandoning > 0) {
-                        mutex_unlock(&ftdi->u132_lock);
-                        return 0;
-                } else {
-                        target->abandoning = 1;
-                      wait_1:if (target->active == 1) {
-                                int command_size = ftdi->command_next -
-                                        ftdi->command_head;
-                                if (command_size < COMMAND_SIZE) {
-                                        struct u132_command *command =
-                                                &ftdi->command[COMMAND_MASK &
-                                                ftdi->command_next];
-                                        command->header = 0x80 | (ed << 5) |
-                                                0x4;
-                                        command->length = 0x00;
-                                        command->address = 0x00;
-                                        command->width = 0x00;
-                                        command->follows = 0;
-                                        command->value = 0;
-                                        command->buffer = &command->value;
-                                        ftdi->command_next += 1;
-                                        ftdi_elan_kick_command_queue(ftdi);
-                                } else {
-                                        mutex_unlock(&ftdi->u132_lock);
-                                        msleep(100);
-                                        mutex_lock(&ftdi->u132_lock);
-                                        goto wait_1;
-                                }
-                        }
-                        mutex_unlock(&ftdi->u132_lock);
-                        return 0;
-                }
-        }
+				 void *endp)
+{
+	u8 ed = ed_number - 1;
+	if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else if (ftdi->initialized == 0) {
+		return -ENODEV;
+	} else {
+		struct u132_target *target = &ftdi->target[ed];
+		mutex_lock(&ftdi->u132_lock);
+		if (target->abandoning > 0) {
+			mutex_unlock(&ftdi->u132_lock);
+			return 0;
+		} else {
+			target->abandoning = 1;
+		wait_1:if (target->active == 1) {
+				int command_size = ftdi->command_next -
+					ftdi->command_head;
+				if (command_size < COMMAND_SIZE) {
+					struct u132_command *command =
+						&ftdi->command[COMMAND_MASK &
+							       ftdi->command_next];
+					command->header = 0x80 | (ed << 5) |
+						0x4;
+					command->length = 0x00;
+					command->address = 0x00;
+					command->width = 0x00;
+					command->follows = 0;
+					command->value = 0;
+					command->buffer = &command->value;
+					ftdi->command_next += 1;
+					ftdi_elan_kick_command_queue(ftdi);
+				} else {
+					mutex_unlock(&ftdi->u132_lock);
+					msleep(100);
+					mutex_lock(&ftdi->u132_lock);
+					goto wait_1;
+				}
+			}
+			mutex_unlock(&ftdi->u132_lock);
+			return 0;
+		}
+	}
 }
 
 int usb_ftdi_elan_edset_flush(struct platform_device *pdev, u8 ed_number,
-        void *endp)
+			      void *endp)
 {
-        struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
-        return ftdi_elan_edset_flush(ftdi, ed_number, endp);
+	struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
+	return ftdi_elan_edset_flush(ftdi, ed_number, endp);
 }
 
 
 EXPORT_SYMBOL_GPL(usb_ftdi_elan_edset_flush);
 static int ftdi_elan_flush_input_fifo(struct usb_ftdi *ftdi)
 {
-        int retry_on_empty = 10;
-        int retry_on_timeout = 5;
-        int retry_on_status = 20;
-      more:{
-                int packet_bytes = 0;
-                int retval = usb_bulk_msg(ftdi->udev,
-                        usb_rcvbulkpipe(ftdi->udev, ftdi->bulk_in_endpointAddr),
-                         ftdi->bulk_in_buffer, ftdi->bulk_in_size,
-                        &packet_bytes, 100);
-                if (packet_bytes > 2) {
-                        char diag[30 *3 + 4];
-                        char *d = diag;
-                        int m = (sizeof(diag) - 1) / 3;
-                        char *b = ftdi->bulk_in_buffer;
-                        int bytes_read = 0;
-                        diag[0] = 0;
-                        while (packet_bytes-- > 0) {
-                                char c = *b++;
-                                if (bytes_read < m) {
-                                        d += sprintf(d, " %02X",
-                                                0x000000FF & c);
-                                } else if (bytes_read > m) {
-                                } else
-                                        d += sprintf(d, " ..");
-                                bytes_read += 1;
-                                continue;
-                        }
-                        goto more;
-                } else if (packet_bytes > 1) {
-                        char s1 = ftdi->bulk_in_buffer[0];
-                        char s2 = ftdi->bulk_in_buffer[1];
-                        if (s1 == 0x31 && s2 == 0x60) {
-                                return 0;
-                        } else if (retry_on_status-- > 0) {
-                                goto more;
-                        } else {
-                                dev_err(&ftdi->udev->dev, "STATUS ERROR retry l"
-                                        "imit reached\n");
-                                return -EFAULT;
-                        }
-                } else if (packet_bytes > 0) {
-                        char b1 = ftdi->bulk_in_buffer[0];
-                        dev_err(&ftdi->udev->dev, "only one byte flushed from F"
-                                "TDI = %02X\n", b1);
-                        if (retry_on_status-- > 0) {
-                                goto more;
-                        } else {
-                                dev_err(&ftdi->udev->dev, "STATUS ERROR retry l"
-                                        "imit reached\n");
-                                return -EFAULT;
-                        }
-                } else if (retval == -ETIMEDOUT) {
-                        if (retry_on_timeout-- > 0) {
-                                goto more;
-                        } else {
-                                dev_err(&ftdi->udev->dev, "TIMED OUT retry limi"
-                                        "t reached\n");
-                                return -ENOMEM;
-                        }
-                } else if (retval == 0) {
-                        if (retry_on_empty-- > 0) {
-                                goto more;
-                        } else {
-                                dev_err(&ftdi->udev->dev, "empty packet retry l"
-                                        "imit reached\n");
-                                return -ENOMEM;
-                        }
-                } else {
-                        dev_err(&ftdi->udev->dev, "error = %d\n", retval);
-                        return retval;
-                }
-        }
-        return -1;
+	int retry_on_empty = 10;
+	int retry_on_timeout = 5;
+	int retry_on_status = 20;
+more:{
+		int packet_bytes = 0;
+		int retval = usb_bulk_msg(ftdi->udev,
+					  usb_rcvbulkpipe(ftdi->udev, ftdi->bulk_in_endpointAddr),
+					  ftdi->bulk_in_buffer, ftdi->bulk_in_size,
+					  &packet_bytes, 100);
+		if (packet_bytes > 2) {
+			char diag[30 *3 + 4];
+			char *d = diag;
+			int m = (sizeof(diag) - 1) / 3;
+			char *b = ftdi->bulk_in_buffer;
+			int bytes_read = 0;
+			diag[0] = 0;
+			while (packet_bytes-- > 0) {
+				char c = *b++;
+				if (bytes_read < m) {
+					d += sprintf(d, " %02X",
+						     0x000000FF & c);
+				} else if (bytes_read > m) {
+				} else
+					d += sprintf(d, " ..");
+				bytes_read += 1;
+				continue;
+			}
+			goto more;
+		} else if (packet_bytes > 1) {
+			char s1 = ftdi->bulk_in_buffer[0];
+			char s2 = ftdi->bulk_in_buffer[1];
+			if (s1 == 0x31 && s2 == 0x60) {
+				return 0;
+			} else if (retry_on_status-- > 0) {
+				goto more;
+			} else {
+				dev_err(&ftdi->udev->dev, "STATUS ERROR retry limit reached\n");
+				return -EFAULT;
+			}
+		} else if (packet_bytes > 0) {
+			char b1 = ftdi->bulk_in_buffer[0];
+			dev_err(&ftdi->udev->dev, "only one byte flushed from FTDI = %02X\n",
+				b1);
+			if (retry_on_status-- > 0) {
+				goto more;
+			} else {
+				dev_err(&ftdi->udev->dev, "STATUS ERROR retry limit reached\n");
+				return -EFAULT;
+			}
+		} else if (retval == -ETIMEDOUT) {
+			if (retry_on_timeout-- > 0) {
+				goto more;
+			} else {
+				dev_err(&ftdi->udev->dev, "TIMED OUT retry limit reached\n");
+				return -ENOMEM;
+			}
+		} else if (retval == 0) {
+			if (retry_on_empty-- > 0) {
+				goto more;
+			} else {
+				dev_err(&ftdi->udev->dev, "empty packet retry limit reached\n");
+				return -ENOMEM;
+			}
+		} else {
+			dev_err(&ftdi->udev->dev, "error = %d\n", retval);
+			return retval;
+		}
+	}
+	return -1;
 }
 
 
 /*
-* send the long flush sequence
-*
-*/
+ * send the long flush sequence
+ *
+ */
 static int ftdi_elan_synchronize_flush(struct usb_ftdi *ftdi)
 {
-        int retval;
-        struct urb *urb;
-        char *buf;
-        int I = 257;
-        int i = 0;
-        urb = usb_alloc_urb(0, GFP_KERNEL);
-        if (!urb) {
-                dev_err(&ftdi->udev->dev, "could not alloc a urb for flush sequ"
-                        "ence\n");
-                return -ENOMEM;
-        }
-        buf = usb_alloc_coherent(ftdi->udev, I, GFP_KERNEL, &urb->transfer_dma);
-        if (!buf) {
-                dev_err(&ftdi->udev->dev, "could not get a buffer for flush seq"
-                        "uence\n");
-                usb_free_urb(urb);
-                return -ENOMEM;
-        }
-        while (I-- > 0)
-                buf[i++] = 0x55;
-        usb_fill_bulk_urb(urb, ftdi->udev, usb_sndbulkpipe(ftdi->udev,
-                ftdi->bulk_out_endpointAddr), buf, i,
-                ftdi_elan_write_bulk_callback, ftdi);
-        urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
-        retval = usb_submit_urb(urb, GFP_KERNEL);
-        if (retval) {
-                dev_err(&ftdi->udev->dev, "failed to submit urb containing the "
-                        "flush sequence\n");
-                usb_free_coherent(ftdi->udev, i, buf, urb->transfer_dma);
-                usb_free_urb(urb);
-                return -ENOMEM;
-        }
-        usb_free_urb(urb);
-        return 0;
+	int retval;
+	struct urb *urb;
+	char *buf;
+	int I = 257;
+	int i = 0;
+	urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!urb) {
+		dev_err(&ftdi->udev->dev, "could not alloc a urb for flush sequence\n");
+		return -ENOMEM;
+	}
+	buf = usb_alloc_coherent(ftdi->udev, I, GFP_KERNEL, &urb->transfer_dma);
+	if (!buf) {
+		dev_err(&ftdi->udev->dev, "could not get a buffer for flush sequence\n");
+		usb_free_urb(urb);
+		return -ENOMEM;
+	}
+	while (I-- > 0)
+		buf[i++] = 0x55;
+	usb_fill_bulk_urb(urb, ftdi->udev, usb_sndbulkpipe(ftdi->udev,
+							   ftdi->bulk_out_endpointAddr), buf, i,
+			  ftdi_elan_write_bulk_callback, ftdi);
+	urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+	retval = usb_submit_urb(urb, GFP_KERNEL);
+	if (retval) {
+		dev_err(&ftdi->udev->dev, "failed to submit urb containing the flush sequence\n");
+		usb_free_coherent(ftdi->udev, i, buf, urb->transfer_dma);
+		usb_free_urb(urb);
+		return -ENOMEM;
+	}
+	usb_free_urb(urb);
+	return 0;
 }
 
 
 /*
-* send the reset sequence
-*
-*/
+ * send the reset sequence
+ *
+ */
 static int ftdi_elan_synchronize_reset(struct usb_ftdi *ftdi)
 {
-        int retval;
-        struct urb *urb;
-        char *buf;
-        int I = 4;
-        int i = 0;
-        urb = usb_alloc_urb(0, GFP_KERNEL);
-        if (!urb) {
-                dev_err(&ftdi->udev->dev, "could not get a urb for the reset se"
-                        "quence\n");
-                return -ENOMEM;
-        }
-        buf = usb_alloc_coherent(ftdi->udev, I, GFP_KERNEL, &urb->transfer_dma);
-        if (!buf) {
-                dev_err(&ftdi->udev->dev, "could not get a buffer for the reset"
-                        " sequence\n");
-                usb_free_urb(urb);
-                return -ENOMEM;
-        }
-        buf[i++] = 0x55;
-        buf[i++] = 0xAA;
-        buf[i++] = 0x5A;
-        buf[i++] = 0xA5;
-        usb_fill_bulk_urb(urb, ftdi->udev, usb_sndbulkpipe(ftdi->udev,
-                ftdi->bulk_out_endpointAddr), buf, i,
-                ftdi_elan_write_bulk_callback, ftdi);
-        urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
-        retval = usb_submit_urb(urb, GFP_KERNEL);
-        if (retval) {
-                dev_err(&ftdi->udev->dev, "failed to submit urb containing the "
-                        "reset sequence\n");
-                usb_free_coherent(ftdi->udev, i, buf, urb->transfer_dma);
-                usb_free_urb(urb);
-                return -ENOMEM;
-        }
-        usb_free_urb(urb);
-        return 0;
+	int retval;
+	struct urb *urb;
+	char *buf;
+	int I = 4;
+	int i = 0;
+	urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!urb) {
+		dev_err(&ftdi->udev->dev, "could not get a urb for the reset sequence\n");
+		return -ENOMEM;
+	}
+	buf = usb_alloc_coherent(ftdi->udev, I, GFP_KERNEL, &urb->transfer_dma);
+	if (!buf) {
+		dev_err(&ftdi->udev->dev, "could not get a buffer for the reset sequence\n");
+		usb_free_urb(urb);
+		return -ENOMEM;
+	}
+	buf[i++] = 0x55;
+	buf[i++] = 0xAA;
+	buf[i++] = 0x5A;
+	buf[i++] = 0xA5;
+	usb_fill_bulk_urb(urb, ftdi->udev, usb_sndbulkpipe(ftdi->udev,
+							   ftdi->bulk_out_endpointAddr), buf, i,
+			  ftdi_elan_write_bulk_callback, ftdi);
+	urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+	retval = usb_submit_urb(urb, GFP_KERNEL);
+	if (retval) {
+		dev_err(&ftdi->udev->dev, "failed to submit urb containing the reset sequence\n");
+		usb_free_coherent(ftdi->udev, i, buf, urb->transfer_dma);
+		usb_free_urb(urb);
+		return -ENOMEM;
+	}
+	usb_free_urb(urb);
+	return 0;
 }
 
 static int ftdi_elan_synchronize(struct usb_ftdi *ftdi)
 {
-        int retval;
-        int long_stop = 10;
-        int retry_on_timeout = 5;
-        int retry_on_empty = 10;
-        int err_count = 0;
-        retval = ftdi_elan_flush_input_fifo(ftdi);
-        if (retval)
-                return retval;
-        ftdi->bulk_in_left = 0;
-        ftdi->bulk_in_last = -1;
-        while (long_stop-- > 0) {
-                int read_stop;
-                int read_stuck;
-                retval = ftdi_elan_synchronize_flush(ftdi);
-                if (retval)
-                        return retval;
-                retval = ftdi_elan_flush_input_fifo(ftdi);
-                if (retval)
-                        return retval;
-              reset:retval = ftdi_elan_synchronize_reset(ftdi);
-                if (retval)
-                        return retval;
-                read_stop = 100;
-                read_stuck = 10;
-              read:{
-                        int packet_bytes = 0;
-                        retval = usb_bulk_msg(ftdi->udev,
-                                usb_rcvbulkpipe(ftdi->udev,
-                                ftdi->bulk_in_endpointAddr),
-                                ftdi->bulk_in_buffer, ftdi->bulk_in_size,
-                                &packet_bytes, 500);
-                        if (packet_bytes > 2) {
-                                char diag[30 *3 + 4];
-                                char *d = diag;
-                                int m = (sizeof(diag) - 1) / 3;
-                                char *b = ftdi->bulk_in_buffer;
-                                int bytes_read = 0;
-                                unsigned char c = 0;
-                                diag[0] = 0;
-                                while (packet_bytes-- > 0) {
-                                        c = *b++;
-                                        if (bytes_read < m) {
-                                                d += sprintf(d, " %02X", c);
-                                        } else if (bytes_read > m) {
-                                        } else
-                                                d += sprintf(d, " ..");
-                                        bytes_read += 1;
-                                        continue;
-                                }
-                                if (c == 0x7E) {
-                                        return 0;
-                                } else {
-                                        if (c == 0x55) {
-                                                goto read;
-                                        } else if (read_stop-- > 0) {
-                                                goto read;
-                                        } else {
-                                                dev_err(&ftdi->udev->dev, "retr"
-                                                        "y limit reached\n");
-                                                continue;
-                                        }
-                                }
-                        } else if (packet_bytes > 1) {
-                                unsigned char s1 = ftdi->bulk_in_buffer[0];
-                                unsigned char s2 = ftdi->bulk_in_buffer[1];
-                                if (s1 == 0x31 && s2 == 0x00) {
-                                        if (read_stuck-- > 0) {
-                                                goto read;
-                                        } else
-                                                goto reset;
-                                } else if (s1 == 0x31 && s2 == 0x60) {
-                                        if (read_stop-- > 0) {
-                                                goto read;
-                                        } else {
-                                                dev_err(&ftdi->udev->dev, "retr"
-                                                        "y limit reached\n");
-                                                continue;
-                                        }
-                                } else {
-                                        if (read_stop-- > 0) {
-                                                goto read;
-                                        } else {
-                                                dev_err(&ftdi->udev->dev, "retr"
-                                                        "y limit reached\n");
-                                                continue;
-                                        }
-                                }
-                        } else if (packet_bytes > 0) {
-                                if (read_stop-- > 0) {
-                                        goto read;
-                                } else {
-                                        dev_err(&ftdi->udev->dev, "retry limit "
-                                                "reached\n");
-                                        continue;
-                                }
-                        } else if (retval == -ETIMEDOUT) {
-                                if (retry_on_timeout-- > 0) {
-                                        goto read;
-                                } else {
-                                        dev_err(&ftdi->udev->dev, "TIMED OUT re"
-                                                "try limit reached\n");
-                                        continue;
-                                }
-                        } else if (retval == 0) {
-                                if (retry_on_empty-- > 0) {
-                                        goto read;
-                                } else {
-                                        dev_err(&ftdi->udev->dev, "empty packet"
-                                                " retry limit reached\n");
-                                        continue;
-                                }
-                        } else {
-                                err_count += 1;
-                                dev_err(&ftdi->udev->dev, "error = %d\n",
-                                        retval);
-                                if (read_stop-- > 0) {
-                                        goto read;
-                                } else {
-                                        dev_err(&ftdi->udev->dev, "retry limit "
-                                                "reached\n");
-                                        continue;
-                                }
-                        }
-                }
-        }
-        dev_err(&ftdi->udev->dev, "failed to synchronize\n");
-        return -EFAULT;
+	int retval;
+	int long_stop = 10;
+	int retry_on_timeout = 5;
+	int retry_on_empty = 10;
+	int err_count = 0;
+	retval = ftdi_elan_flush_input_fifo(ftdi);
+	if (retval)
+		return retval;
+	ftdi->bulk_in_left = 0;
+	ftdi->bulk_in_last = -1;
+	while (long_stop-- > 0) {
+		int read_stop;
+		int read_stuck;
+		retval = ftdi_elan_synchronize_flush(ftdi);
+		if (retval)
+			return retval;
+		retval = ftdi_elan_flush_input_fifo(ftdi);
+		if (retval)
+			return retval;
+	reset:retval = ftdi_elan_synchronize_reset(ftdi);
+		if (retval)
+			return retval;
+		read_stop = 100;
+		read_stuck = 10;
+	read:{
+			int packet_bytes = 0;
+			retval = usb_bulk_msg(ftdi->udev,
+					      usb_rcvbulkpipe(ftdi->udev,
+							      ftdi->bulk_in_endpointAddr),
+					      ftdi->bulk_in_buffer, ftdi->bulk_in_size,
+					      &packet_bytes, 500);
+			if (packet_bytes > 2) {
+				char diag[30 *3 + 4];
+				char *d = diag;
+				int m = (sizeof(diag) - 1) / 3;
+				char *b = ftdi->bulk_in_buffer;
+				int bytes_read = 0;
+				unsigned char c = 0;
+				diag[0] = 0;
+				while (packet_bytes-- > 0) {
+					c = *b++;
+					if (bytes_read < m) {
+						d += sprintf(d, " %02X", c);
+					} else if (bytes_read > m) {
+					} else
+						d += sprintf(d, " ..");
+					bytes_read += 1;
+					continue;
+				}
+				if (c == 0x7E) {
+					return 0;
+				} else {
+					if (c == 0x55) {
+						goto read;
+					} else if (read_stop-- > 0) {
+						goto read;
+					} else {
+						dev_err(&ftdi->udev->dev, "retry limit reached\n");
+						continue;
+					}
+				}
+			} else if (packet_bytes > 1) {
+				unsigned char s1 = ftdi->bulk_in_buffer[0];
+				unsigned char s2 = ftdi->bulk_in_buffer[1];
+				if (s1 == 0x31 && s2 == 0x00) {
+					if (read_stuck-- > 0) {
+						goto read;
+					} else
+						goto reset;
+				} else if (s1 == 0x31 && s2 == 0x60) {
+					if (read_stop-- > 0) {
+						goto read;
+					} else {
+						dev_err(&ftdi->udev->dev, "retry limit reached\n");
+						continue;
+					}
+				} else {
+					if (read_stop-- > 0) {
+						goto read;
+					} else {
+						dev_err(&ftdi->udev->dev, "retry limit reached\n");
+						continue;
+					}
+				}
+			} else if (packet_bytes > 0) {
+				if (read_stop-- > 0) {
+					goto read;
+				} else {
+					dev_err(&ftdi->udev->dev, "retry limit reached\n");
+					continue;
+				}
+			} else if (retval == -ETIMEDOUT) {
+				if (retry_on_timeout-- > 0) {
+					goto read;
+				} else {
+					dev_err(&ftdi->udev->dev, "TIMED OUT retry limit reached\n");
+					continue;
+				}
+			} else if (retval == 0) {
+				if (retry_on_empty-- > 0) {
+					goto read;
+				} else {
+					dev_err(&ftdi->udev->dev, "empty packet retry limit reached\n");
+					continue;
+				}
+			} else {
+				err_count += 1;
+				dev_err(&ftdi->udev->dev, "error = %d\n",
+					retval);
+				if (read_stop-- > 0) {
+					goto read;
+				} else {
+					dev_err(&ftdi->udev->dev, "retry limit reached\n");
+					continue;
+				}
+			}
+		}
+	}
+	dev_err(&ftdi->udev->dev, "failed to synchronize\n");
+	return -EFAULT;
 }
 
 static int ftdi_elan_stuck_waiting(struct usb_ftdi *ftdi)
 {
-        int retry_on_empty = 10;
-        int retry_on_timeout = 5;
-        int retry_on_status = 50;
-      more:{
-                int packet_bytes = 0;
-                int retval = usb_bulk_msg(ftdi->udev,
-                        usb_rcvbulkpipe(ftdi->udev, ftdi->bulk_in_endpointAddr),
-                         ftdi->bulk_in_buffer, ftdi->bulk_in_size,
-                        &packet_bytes, 1000);
-                if (packet_bytes > 2) {
-                        char diag[30 *3 + 4];
-                        char *d = diag;
-                        int m = (sizeof(diag) - 1) / 3;
-                        char *b = ftdi->bulk_in_buffer;
-                        int bytes_read = 0;
-                        diag[0] = 0;
-                        while (packet_bytes-- > 0) {
-                                char c = *b++;
-                                if (bytes_read < m) {
-                                        d += sprintf(d, " %02X",
-                                                0x000000FF & c);
-                                } else if (bytes_read > m) {
-                                } else
-                                        d += sprintf(d, " ..");
-                                bytes_read += 1;
-                                continue;
-                        }
-                        goto more;
-                } else if (packet_bytes > 1) {
-                        char s1 = ftdi->bulk_in_buffer[0];
-                        char s2 = ftdi->bulk_in_buffer[1];
-                        if (s1 == 0x31 && s2 == 0x60) {
-                                return 0;
-                        } else if (retry_on_status-- > 0) {
-                                msleep(5);
-                                goto more;
-                        } else
-                                return -EFAULT;
-                } else if (packet_bytes > 0) {
-                        char b1 = ftdi->bulk_in_buffer[0];
-                        dev_err(&ftdi->udev->dev, "only one byte flushed from F"
-                                "TDI = %02X\n", b1);
-                        if (retry_on_status-- > 0) {
-                                msleep(5);
-                                goto more;
-                        } else {
-                                dev_err(&ftdi->udev->dev, "STATUS ERROR retry l"
-                                        "imit reached\n");
-                                return -EFAULT;
-                        }
-                } else if (retval == -ETIMEDOUT) {
-                        if (retry_on_timeout-- > 0) {
-                                goto more;
-                        } else {
-                                dev_err(&ftdi->udev->dev, "TIMED OUT retry limi"
-                                        "t reached\n");
-                                return -ENOMEM;
-                        }
-                } else if (retval == 0) {
-                        if (retry_on_empty-- > 0) {
-                                goto more;
-                        } else {
-                                dev_err(&ftdi->udev->dev, "empty packet retry l"
-                                        "imit reached\n");
-                                return -ENOMEM;
-                        }
-                } else {
-                        dev_err(&ftdi->udev->dev, "error = %d\n", retval);
-                        return -ENOMEM;
-                }
-        }
-        return -1;
+	int retry_on_empty = 10;
+	int retry_on_timeout = 5;
+	int retry_on_status = 50;
+more:{
+		int packet_bytes = 0;
+		int retval = usb_bulk_msg(ftdi->udev,
+					  usb_rcvbulkpipe(ftdi->udev, ftdi->bulk_in_endpointAddr),
+					  ftdi->bulk_in_buffer, ftdi->bulk_in_size,
+					  &packet_bytes, 1000);
+		if (packet_bytes > 2) {
+			char diag[30 *3 + 4];
+			char *d = diag;
+			int m = (sizeof(diag) - 1) / 3;
+			char *b = ftdi->bulk_in_buffer;
+			int bytes_read = 0;
+			diag[0] = 0;
+			while (packet_bytes-- > 0) {
+				char c = *b++;
+				if (bytes_read < m) {
+					d += sprintf(d, " %02X",
+						     0x000000FF & c);
+				} else if (bytes_read > m) {
+				} else
+					d += sprintf(d, " ..");
+				bytes_read += 1;
+				continue;
+			}
+			goto more;
+		} else if (packet_bytes > 1) {
+			char s1 = ftdi->bulk_in_buffer[0];
+			char s2 = ftdi->bulk_in_buffer[1];
+			if (s1 == 0x31 && s2 == 0x60) {
+				return 0;
+			} else if (retry_on_status-- > 0) {
+				msleep(5);
+				goto more;
+			} else
+				return -EFAULT;
+		} else if (packet_bytes > 0) {
+			char b1 = ftdi->bulk_in_buffer[0];
+			dev_err(&ftdi->udev->dev, "only one byte flushed from FTDI = %02X\n", b1);
+			if (retry_on_status-- > 0) {
+				msleep(5);
+				goto more;
+			} else {
+				dev_err(&ftdi->udev->dev, "STATUS ERROR retry limit reached\n");
+				return -EFAULT;
+			}
+		} else if (retval == -ETIMEDOUT) {
+			if (retry_on_timeout-- > 0) {
+				goto more;
+			} else {
+				dev_err(&ftdi->udev->dev, "TIMED OUT retry limit reached\n");
+				return -ENOMEM;
+			}
+		} else if (retval == 0) {
+			if (retry_on_empty-- > 0) {
+				goto more;
+			} else {
+				dev_err(&ftdi->udev->dev, "empty packet retry limit reached\n");
+				return -ENOMEM;
+			}
+		} else {
+			dev_err(&ftdi->udev->dev, "error = %d\n", retval);
+			return -ENOMEM;
+		}
+	}
+	return -1;
 }
 
 static int ftdi_elan_checkingPCI(struct usb_ftdi *ftdi)
 {
-        int UxxxStatus = ftdi_elan_read_reg(ftdi, &ftdi->controlreg);
-        if (UxxxStatus)
-                return UxxxStatus;
-        if (ftdi->controlreg & 0x00400000) {
-                if (ftdi->card_ejected) {
-                } else {
-                        ftdi->card_ejected = 1;
-                        dev_err(&ftdi->udev->dev, "CARD EJECTED - controlreg = "
-                                "%08X\n", ftdi->controlreg);
-                }
-                return -ENODEV;
-        } else {
-                u8 fn = ftdi->function - 1;
-                int activePCIfn = fn << 8;
-                u32 pcidata;
-                u32 pciVID;
-                u32 pciPID;
-                int reg = 0;
-                UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                        &pcidata);
-                if (UxxxStatus)
-                        return UxxxStatus;
-                pciVID = pcidata & 0xFFFF;
-                pciPID = (pcidata >> 16) & 0xFFFF;
-                if (pciVID == ftdi->platform_data.vendor && pciPID ==
-                        ftdi->platform_data.device) {
-                        return 0;
-                } else {
-                        dev_err(&ftdi->udev->dev, "vendor=%04X pciVID=%04X devi"
-                                "ce=%04X pciPID=%04X\n",
-                                ftdi->platform_data.vendor, pciVID,
-                                ftdi->platform_data.device, pciPID);
-                        return -ENODEV;
-                }
-        }
+	int UxxxStatus = ftdi_elan_read_reg(ftdi, &ftdi->controlreg);
+	if (UxxxStatus)
+		return UxxxStatus;
+	if (ftdi->controlreg & 0x00400000) {
+		if (ftdi->card_ejected) {
+		} else {
+			ftdi->card_ejected = 1;
+			dev_err(&ftdi->udev->dev, "CARD EJECTED - controlreg = %08X\n",
+				ftdi->controlreg);
+		}
+		return -ENODEV;
+	} else {
+		u8 fn = ftdi->function - 1;
+		int activePCIfn = fn << 8;
+		u32 pcidata;
+		u32 pciVID;
+		u32 pciPID;
+		int reg = 0;
+		UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+						   &pcidata);
+		if (UxxxStatus)
+			return UxxxStatus;
+		pciVID = pcidata & 0xFFFF;
+		pciPID = (pcidata >> 16) & 0xFFFF;
+		if (pciVID == ftdi->platform_data.vendor && pciPID ==
+		    ftdi->platform_data.device) {
+			return 0;
+		} else {
+			dev_err(&ftdi->udev->dev, "vendor=%04X pciVID=%04X device=%04X pciPID=%04X\n",
+				ftdi->platform_data.vendor, pciVID,
+				ftdi->platform_data.device, pciPID);
+			return -ENODEV;
+		}
+	}
 }
 
 
 #define ftdi_read_pcimem(ftdi, member, data) ftdi_elan_read_pcimem(ftdi, \
-        offsetof(struct ohci_regs, member), 0, data);
+								   offsetof(struct ohci_regs, member), 0, data);
 #define ftdi_write_pcimem(ftdi, member, data) ftdi_elan_write_pcimem(ftdi, \
-        offsetof(struct ohci_regs, member), 0, data);
+								     offsetof(struct ohci_regs, member), 0, data);
 
 #define OHCI_CONTROL_INIT OHCI_CTRL_CBSR
-#define OHCI_INTR_INIT (OHCI_INTR_MIE | OHCI_INTR_UE | OHCI_INTR_RD | \
-        OHCI_INTR_WDH)
+#define OHCI_INTR_INIT (OHCI_INTR_MIE | OHCI_INTR_UE | OHCI_INTR_RD |	\
+			OHCI_INTR_WDH)
 static int ftdi_elan_check_controller(struct usb_ftdi *ftdi, int quirk)
 {
-        int devices = 0;
-        int retval;
-        u32 hc_control;
-        int num_ports;
-        u32 control;
-        u32 rh_a = -1;
-        u32 status;
-        u32 fminterval;
-        u32 hc_fminterval;
-        u32 periodicstart;
-        u32 cmdstatus;
-        u32 roothub_a;
-        int mask = OHCI_INTR_INIT;
-        int sleep_time = 0;
-        int reset_timeout = 30;        /* ... allow extra time */
-        int temp;
-        retval = ftdi_write_pcimem(ftdi, intrdisable, OHCI_INTR_MIE);
-        if (retval)
-                return retval;
-        retval = ftdi_read_pcimem(ftdi, control, &control);
-        if (retval)
-                return retval;
-        retval = ftdi_read_pcimem(ftdi, roothub.a, &rh_a);
-        if (retval)
-                return retval;
-        num_ports = rh_a & RH_A_NDP;
-        retval = ftdi_read_pcimem(ftdi, fminterval, &hc_fminterval);
-        if (retval)
-                return retval;
-        hc_fminterval &= 0x3fff;
-        if (hc_fminterval != FI) {
-        }
-        hc_fminterval |= FSMP(hc_fminterval) << 16;
-        retval = ftdi_read_pcimem(ftdi, control, &hc_control);
-        if (retval)
-                return retval;
-        switch (hc_control & OHCI_CTRL_HCFS) {
-        case OHCI_USB_OPER:
-                sleep_time = 0;
-                break;
-        case OHCI_USB_SUSPEND:
-        case OHCI_USB_RESUME:
-                hc_control &= OHCI_CTRL_RWC;
-                hc_control |= OHCI_USB_RESUME;
-                sleep_time = 10;
-                break;
-        default:
-                hc_control &= OHCI_CTRL_RWC;
-                hc_control |= OHCI_USB_RESET;
-                sleep_time = 50;
-                break;
-        }
-        retval = ftdi_write_pcimem(ftdi, control, hc_control);
-        if (retval)
-                return retval;
-        retval = ftdi_read_pcimem(ftdi, control, &control);
-        if (retval)
-                return retval;
-        msleep(sleep_time);
-        retval = ftdi_read_pcimem(ftdi, roothub.a, &roothub_a);
-        if (retval)
-                return retval;
-        if (!(roothub_a & RH_A_NPS)) {        /* power down each port */
-                for (temp = 0; temp < num_ports; temp++) {
-                        retval = ftdi_write_pcimem(ftdi,
-                                roothub.portstatus[temp], RH_PS_LSDA);
-                        if (retval)
-                                return retval;
-                }
-        }
-        retval = ftdi_read_pcimem(ftdi, control, &control);
-        if (retval)
-                return retval;
-      retry:retval = ftdi_read_pcimem(ftdi, cmdstatus, &status);
-        if (retval)
-                return retval;
-        retval = ftdi_write_pcimem(ftdi, cmdstatus, OHCI_HCR);
-        if (retval)
-                return retval;
-      extra:{
-                retval = ftdi_read_pcimem(ftdi, cmdstatus, &status);
-                if (retval)
-                        return retval;
-                if (0 != (status & OHCI_HCR)) {
-                        if (--reset_timeout == 0) {
-                                dev_err(&ftdi->udev->dev, "USB HC reset timed o"
-                                        "ut!\n");
-                                return -ENODEV;
-                        } else {
-                                msleep(5);
-                                goto extra;
-                        }
-                }
-        }
-        if (quirk & OHCI_QUIRK_INITRESET) {
-                retval = ftdi_write_pcimem(ftdi, control, hc_control);
-                if (retval)
-                        return retval;
-                retval = ftdi_read_pcimem(ftdi, control, &control);
-                if (retval)
-                        return retval;
-        }
-        retval = ftdi_write_pcimem(ftdi, ed_controlhead, 0x00000000);
-        if (retval)
-                return retval;
-        retval = ftdi_write_pcimem(ftdi, ed_bulkhead, 0x11000000);
-        if (retval)
-                return retval;
-        retval = ftdi_write_pcimem(ftdi, hcca, 0x00000000);
-        if (retval)
-                return retval;
-        retval = ftdi_read_pcimem(ftdi, fminterval, &fminterval);
-        if (retval)
-                return retval;
-        retval = ftdi_write_pcimem(ftdi, fminterval,
-                ((fminterval & FIT) ^ FIT) | hc_fminterval);
-        if (retval)
-                return retval;
-        retval = ftdi_write_pcimem(ftdi, periodicstart,
-                ((9 *hc_fminterval) / 10) & 0x3fff);
-        if (retval)
-                return retval;
-        retval = ftdi_read_pcimem(ftdi, fminterval, &fminterval);
-        if (retval)
-                return retval;
-        retval = ftdi_read_pcimem(ftdi, periodicstart, &periodicstart);
-        if (retval)
-                return retval;
-        if (0 == (fminterval & 0x3fff0000) || 0 == periodicstart) {
-                if (!(quirk & OHCI_QUIRK_INITRESET)) {
-                        quirk |= OHCI_QUIRK_INITRESET;
-                        goto retry;
-                } else
-                        dev_err(&ftdi->udev->dev, "init err(%08x %04x)\n",
-                                fminterval, periodicstart);
-        }                        /* start controller operations */
-        hc_control &= OHCI_CTRL_RWC;
-        hc_control |= OHCI_CONTROL_INIT | OHCI_CTRL_BLE | OHCI_USB_OPER;
-        retval = ftdi_write_pcimem(ftdi, control, hc_control);
-        if (retval)
-                return retval;
-        retval = ftdi_write_pcimem(ftdi, cmdstatus, OHCI_BLF);
-        if (retval)
-                return retval;
-        retval = ftdi_read_pcimem(ftdi, cmdstatus, &cmdstatus);
-        if (retval)
-                return retval;
-        retval = ftdi_read_pcimem(ftdi, control, &control);
-        if (retval)
-                return retval;
-        retval = ftdi_write_pcimem(ftdi, roothub.status, RH_HS_DRWE);
-        if (retval)
-                return retval;
-        retval = ftdi_write_pcimem(ftdi, intrstatus, mask);
-        if (retval)
-                return retval;
-        retval = ftdi_write_pcimem(ftdi, intrdisable,
-                OHCI_INTR_MIE | OHCI_INTR_OC | OHCI_INTR_RHSC | OHCI_INTR_FNO |
-                OHCI_INTR_UE | OHCI_INTR_RD | OHCI_INTR_SF | OHCI_INTR_WDH |
-                OHCI_INTR_SO);
-        if (retval)
-                return retval;        /* handle root hub init quirks ... */
-        retval = ftdi_read_pcimem(ftdi, roothub.a, &roothub_a);
-        if (retval)
-                return retval;
-        roothub_a &= ~(RH_A_PSM | RH_A_OCPM);
-        if (quirk & OHCI_QUIRK_SUPERIO) {
-                roothub_a |= RH_A_NOCP;
-                roothub_a &= ~(RH_A_POTPGT | RH_A_NPS);
-                retval = ftdi_write_pcimem(ftdi, roothub.a, roothub_a);
-                if (retval)
-                        return retval;
-        } else if ((quirk & OHCI_QUIRK_AMD756) || distrust_firmware) {
-                roothub_a |= RH_A_NPS;
-                retval = ftdi_write_pcimem(ftdi, roothub.a, roothub_a);
-                if (retval)
-                        return retval;
-        }
-        retval = ftdi_write_pcimem(ftdi, roothub.status, RH_HS_LPSC);
-        if (retval)
-                return retval;
-        retval = ftdi_write_pcimem(ftdi, roothub.b,
-                (roothub_a & RH_A_NPS) ? 0 : RH_B_PPCM);
-        if (retval)
-                return retval;
-        retval = ftdi_read_pcimem(ftdi, control, &control);
-        if (retval)
-                return retval;
-        mdelay((roothub_a >> 23) & 0x1fe);
-        for (temp = 0; temp < num_ports; temp++) {
-                u32 portstatus;
-                retval = ftdi_read_pcimem(ftdi, roothub.portstatus[temp],
-                        &portstatus);
-                if (retval)
-                        return retval;
-                if (1 & portstatus)
-                        devices += 1;
-        }
-        return devices;
+	int devices = 0;
+	int retval;
+	u32 hc_control;
+	int num_ports;
+	u32 control;
+	u32 rh_a = -1;
+	u32 status;
+	u32 fminterval;
+	u32 hc_fminterval;
+	u32 periodicstart;
+	u32 cmdstatus;
+	u32 roothub_a;
+	int mask = OHCI_INTR_INIT;
+	int sleep_time = 0;
+	int reset_timeout = 30;        /* ... allow extra time */
+	int temp;
+	retval = ftdi_write_pcimem(ftdi, intrdisable, OHCI_INTR_MIE);
+	if (retval)
+		return retval;
+	retval = ftdi_read_pcimem(ftdi, control, &control);
+	if (retval)
+		return retval;
+	retval = ftdi_read_pcimem(ftdi, roothub.a, &rh_a);
+	if (retval)
+		return retval;
+	num_ports = rh_a & RH_A_NDP;
+	retval = ftdi_read_pcimem(ftdi, fminterval, &hc_fminterval);
+	if (retval)
+		return retval;
+	hc_fminterval &= 0x3fff;
+	if (hc_fminterval != FI) {
+	}
+	hc_fminterval |= FSMP(hc_fminterval) << 16;
+	retval = ftdi_read_pcimem(ftdi, control, &hc_control);
+	if (retval)
+		return retval;
+	switch (hc_control & OHCI_CTRL_HCFS) {
+	case OHCI_USB_OPER:
+		sleep_time = 0;
+		break;
+	case OHCI_USB_SUSPEND:
+	case OHCI_USB_RESUME:
+		hc_control &= OHCI_CTRL_RWC;
+		hc_control |= OHCI_USB_RESUME;
+		sleep_time = 10;
+		break;
+	default:
+		hc_control &= OHCI_CTRL_RWC;
+		hc_control |= OHCI_USB_RESET;
+		sleep_time = 50;
+		break;
+	}
+	retval = ftdi_write_pcimem(ftdi, control, hc_control);
+	if (retval)
+		return retval;
+	retval = ftdi_read_pcimem(ftdi, control, &control);
+	if (retval)
+		return retval;
+	msleep(sleep_time);
+	retval = ftdi_read_pcimem(ftdi, roothub.a, &roothub_a);
+	if (retval)
+		return retval;
+	if (!(roothub_a & RH_A_NPS)) {        /* power down each port */
+		for (temp = 0; temp < num_ports; temp++) {
+			retval = ftdi_write_pcimem(ftdi,
+						   roothub.portstatus[temp], RH_PS_LSDA);
+			if (retval)
+				return retval;
+		}
+	}
+	retval = ftdi_read_pcimem(ftdi, control, &control);
+	if (retval)
+		return retval;
+retry:retval = ftdi_read_pcimem(ftdi, cmdstatus, &status);
+	if (retval)
+		return retval;
+	retval = ftdi_write_pcimem(ftdi, cmdstatus, OHCI_HCR);
+	if (retval)
+		return retval;
+extra:{
+		retval = ftdi_read_pcimem(ftdi, cmdstatus, &status);
+		if (retval)
+			return retval;
+		if (0 != (status & OHCI_HCR)) {
+			if (--reset_timeout == 0) {
+				dev_err(&ftdi->udev->dev, "USB HC reset timed out!\n");
+				return -ENODEV;
+			} else {
+				msleep(5);
+				goto extra;
+			}
+		}
+	}
+	if (quirk & OHCI_QUIRK_INITRESET) {
+		retval = ftdi_write_pcimem(ftdi, control, hc_control);
+		if (retval)
+			return retval;
+		retval = ftdi_read_pcimem(ftdi, control, &control);
+		if (retval)
+			return retval;
+	}
+	retval = ftdi_write_pcimem(ftdi, ed_controlhead, 0x00000000);
+	if (retval)
+		return retval;
+	retval = ftdi_write_pcimem(ftdi, ed_bulkhead, 0x11000000);
+	if (retval)
+		return retval;
+	retval = ftdi_write_pcimem(ftdi, hcca, 0x00000000);
+	if (retval)
+		return retval;
+	retval = ftdi_read_pcimem(ftdi, fminterval, &fminterval);
+	if (retval)
+		return retval;
+	retval = ftdi_write_pcimem(ftdi, fminterval,
+				   ((fminterval & FIT) ^ FIT) | hc_fminterval);
+	if (retval)
+		return retval;
+	retval = ftdi_write_pcimem(ftdi, periodicstart,
+				   ((9 *hc_fminterval) / 10) & 0x3fff);
+	if (retval)
+		return retval;
+	retval = ftdi_read_pcimem(ftdi, fminterval, &fminterval);
+	if (retval)
+		return retval;
+	retval = ftdi_read_pcimem(ftdi, periodicstart, &periodicstart);
+	if (retval)
+		return retval;
+	if (0 == (fminterval & 0x3fff0000) || 0 == periodicstart) {
+		if (!(quirk & OHCI_QUIRK_INITRESET)) {
+			quirk |= OHCI_QUIRK_INITRESET;
+			goto retry;
+		} else
+			dev_err(&ftdi->udev->dev, "init err(%08x %04x)\n",
+				fminterval, periodicstart);
+	}                        /* start controller operations */
+	hc_control &= OHCI_CTRL_RWC;
+	hc_control |= OHCI_CONTROL_INIT | OHCI_CTRL_BLE | OHCI_USB_OPER;
+	retval = ftdi_write_pcimem(ftdi, control, hc_control);
+	if (retval)
+		return retval;
+	retval = ftdi_write_pcimem(ftdi, cmdstatus, OHCI_BLF);
+	if (retval)
+		return retval;
+	retval = ftdi_read_pcimem(ftdi, cmdstatus, &cmdstatus);
+	if (retval)
+		return retval;
+	retval = ftdi_read_pcimem(ftdi, control, &control);
+	if (retval)
+		return retval;
+	retval = ftdi_write_pcimem(ftdi, roothub.status, RH_HS_DRWE);
+	if (retval)
+		return retval;
+	retval = ftdi_write_pcimem(ftdi, intrstatus, mask);
+	if (retval)
+		return retval;
+	retval = ftdi_write_pcimem(ftdi, intrdisable,
+				   OHCI_INTR_MIE | OHCI_INTR_OC | OHCI_INTR_RHSC | OHCI_INTR_FNO |
+				   OHCI_INTR_UE | OHCI_INTR_RD | OHCI_INTR_SF | OHCI_INTR_WDH |
+				   OHCI_INTR_SO);
+	if (retval)
+		return retval;        /* handle root hub init quirks ... */
+	retval = ftdi_read_pcimem(ftdi, roothub.a, &roothub_a);
+	if (retval)
+		return retval;
+	roothub_a &= ~(RH_A_PSM | RH_A_OCPM);
+	if (quirk & OHCI_QUIRK_SUPERIO) {
+		roothub_a |= RH_A_NOCP;
+		roothub_a &= ~(RH_A_POTPGT | RH_A_NPS);
+		retval = ftdi_write_pcimem(ftdi, roothub.a, roothub_a);
+		if (retval)
+			return retval;
+	} else if ((quirk & OHCI_QUIRK_AMD756) || distrust_firmware) {
+		roothub_a |= RH_A_NPS;
+		retval = ftdi_write_pcimem(ftdi, roothub.a, roothub_a);
+		if (retval)
+			return retval;
+	}
+	retval = ftdi_write_pcimem(ftdi, roothub.status, RH_HS_LPSC);
+	if (retval)
+		return retval;
+	retval = ftdi_write_pcimem(ftdi, roothub.b,
+				   (roothub_a & RH_A_NPS) ? 0 : RH_B_PPCM);
+	if (retval)
+		return retval;
+	retval = ftdi_read_pcimem(ftdi, control, &control);
+	if (retval)
+		return retval;
+	mdelay((roothub_a >> 23) & 0x1fe);
+	for (temp = 0; temp < num_ports; temp++) {
+		u32 portstatus;
+		retval = ftdi_read_pcimem(ftdi, roothub.portstatus[temp],
+					  &portstatus);
+		if (retval)
+			return retval;
+		if (1 & portstatus)
+			devices += 1;
+	}
+	return devices;
 }
 
 static int ftdi_elan_setup_controller(struct usb_ftdi *ftdi, int fn)
 {
-        u32 latence_timer;
-        int UxxxStatus;
-        u32 pcidata;
-        int reg = 0;
-        int activePCIfn = fn << 8;
-        UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000025FL | 0x2800);
-        if (UxxxStatus)
-                return UxxxStatus;
-        reg = 16;
-        UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0,
-                0xFFFFFFFF);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                &pcidata);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0,
-                0xF0000000);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                &pcidata);
-        if (UxxxStatus)
-                return UxxxStatus;
-        reg = 12;
-        UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                &latence_timer);
-        if (UxxxStatus)
-                return UxxxStatus;
-        latence_timer &= 0xFFFF00FF;
-        latence_timer |= 0x00001600;
-        UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0x00,
-                latence_timer);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                &pcidata);
-        if (UxxxStatus)
-                return UxxxStatus;
-        reg = 4;
-        UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0x00,
-                0x06);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                &pcidata);
-        if (UxxxStatus)
-                return UxxxStatus;
-        for (reg = 0; reg <= 0x54; reg += 4) {
-                UxxxStatus = ftdi_elan_read_pcimem(ftdi, reg, 0, &pcidata);
-                if (UxxxStatus)
-                        return UxxxStatus;
-        }
-        return 0;
+	u32 latence_timer;
+	int UxxxStatus;
+	u32 pcidata;
+	int reg = 0;
+	int activePCIfn = fn << 8;
+	UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000025FL | 0x2800);
+	if (UxxxStatus)
+		return UxxxStatus;
+	reg = 16;
+	UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0,
+					    0xFFFFFFFF);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+					   &pcidata);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0,
+					    0xF0000000);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+					   &pcidata);
+	if (UxxxStatus)
+		return UxxxStatus;
+	reg = 12;
+	UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+					   &latence_timer);
+	if (UxxxStatus)
+		return UxxxStatus;
+	latence_timer &= 0xFFFF00FF;
+	latence_timer |= 0x00001600;
+	UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0x00,
+					    latence_timer);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+					   &pcidata);
+	if (UxxxStatus)
+		return UxxxStatus;
+	reg = 4;
+	UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0x00,
+					    0x06);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+					   &pcidata);
+	if (UxxxStatus)
+		return UxxxStatus;
+	for (reg = 0; reg <= 0x54; reg += 4) {
+		UxxxStatus = ftdi_elan_read_pcimem(ftdi, reg, 0, &pcidata);
+		if (UxxxStatus)
+			return UxxxStatus;
+	}
+	return 0;
 }
 
 static int ftdi_elan_close_controller(struct usb_ftdi *ftdi, int fn)
 {
-        u32 latence_timer;
-        int UxxxStatus;
-        u32 pcidata;
-        int reg = 0;
-        int activePCIfn = fn << 8;
-        UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000025FL | 0x2800);
-        if (UxxxStatus)
-                return UxxxStatus;
-        reg = 16;
-        UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0,
-                0xFFFFFFFF);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                &pcidata);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0,
-                0x00000000);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                &pcidata);
-        if (UxxxStatus)
-                return UxxxStatus;
-        reg = 12;
-        UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                &latence_timer);
-        if (UxxxStatus)
-                return UxxxStatus;
-        latence_timer &= 0xFFFF00FF;
-        latence_timer |= 0x00001600;
-        UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0x00,
-                latence_timer);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                &pcidata);
-        if (UxxxStatus)
-                return UxxxStatus;
-        reg = 4;
-        UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0x00,
-                0x00);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                &pcidata);
-        if (UxxxStatus)
-                return UxxxStatus;
-        return 0;
+	u32 latence_timer;
+	int UxxxStatus;
+	u32 pcidata;
+	int reg = 0;
+	int activePCIfn = fn << 8;
+	UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000025FL | 0x2800);
+	if (UxxxStatus)
+		return UxxxStatus;
+	reg = 16;
+	UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0,
+					    0xFFFFFFFF);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+					   &pcidata);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0,
+					    0x00000000);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+					   &pcidata);
+	if (UxxxStatus)
+		return UxxxStatus;
+	reg = 12;
+	UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+					   &latence_timer);
+	if (UxxxStatus)
+		return UxxxStatus;
+	latence_timer &= 0xFFFF00FF;
+	latence_timer |= 0x00001600;
+	UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0x00,
+					    latence_timer);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+					   &pcidata);
+	if (UxxxStatus)
+		return UxxxStatus;
+	reg = 4;
+	UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0x00,
+					    0x00);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+					   &pcidata);
+	if (UxxxStatus)
+		return UxxxStatus;
+	return 0;
 }
 
 static int ftdi_elan_found_controller(struct usb_ftdi *ftdi, int fn, int quirk)
 {
-        int result;
-        int UxxxStatus;
-        UxxxStatus = ftdi_elan_setup_controller(ftdi, fn);
-        if (UxxxStatus)
-                return UxxxStatus;
-        result = ftdi_elan_check_controller(ftdi, quirk);
-        UxxxStatus = ftdi_elan_close_controller(ftdi, fn);
-        if (UxxxStatus)
-                return UxxxStatus;
-        return result;
+	int result;
+	int UxxxStatus;
+	UxxxStatus = ftdi_elan_setup_controller(ftdi, fn);
+	if (UxxxStatus)
+		return UxxxStatus;
+	result = ftdi_elan_check_controller(ftdi, quirk);
+	UxxxStatus = ftdi_elan_close_controller(ftdi, fn);
+	if (UxxxStatus)
+		return UxxxStatus;
+	return result;
 }
 
 static int ftdi_elan_enumeratePCI(struct usb_ftdi *ftdi)
 {
-        u32 controlreg;
-        u8 sensebits;
-        int UxxxStatus;
-        UxxxStatus = ftdi_elan_read_reg(ftdi, &controlreg);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_write_reg(ftdi, 0x00000000L);
-        if (UxxxStatus)
-                return UxxxStatus;
-        msleep(750);
-        UxxxStatus = ftdi_elan_write_reg(ftdi, 0x00000200L | 0x100);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_write_reg(ftdi, 0x00000200L | 0x500);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_reg(ftdi, &controlreg);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000020CL | 0x000);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000020DL | 0x000);
-        if (UxxxStatus)
-                return UxxxStatus;
-        msleep(250);
-        UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000020FL | 0x000);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_reg(ftdi, &controlreg);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000025FL | 0x800);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_reg(ftdi, &controlreg);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_reg(ftdi, &controlreg);
-        if (UxxxStatus)
-                return UxxxStatus;
-        msleep(1000);
-        sensebits = (controlreg >> 16) & 0x000F;
-        if (0x0D == sensebits)
-                return 0;
-        else
+	u32 controlreg;
+	u8 sensebits;
+	int UxxxStatus;
+	UxxxStatus = ftdi_elan_read_reg(ftdi, &controlreg);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_write_reg(ftdi, 0x00000000L);
+	if (UxxxStatus)
+		return UxxxStatus;
+	msleep(750);
+	UxxxStatus = ftdi_elan_write_reg(ftdi, 0x00000200L | 0x100);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_write_reg(ftdi, 0x00000200L | 0x500);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_reg(ftdi, &controlreg);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000020CL | 0x000);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000020DL | 0x000);
+	if (UxxxStatus)
+		return UxxxStatus;
+	msleep(250);
+	UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000020FL | 0x000);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_reg(ftdi, &controlreg);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000025FL | 0x800);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_reg(ftdi, &controlreg);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_reg(ftdi, &controlreg);
+	if (UxxxStatus)
+		return UxxxStatus;
+	msleep(1000);
+	sensebits = (controlreg >> 16) & 0x000F;
+	if (0x0D == sensebits)
+		return 0;
+	else
 		return - ENXIO;
 }
 
 static int ftdi_elan_setupOHCI(struct usb_ftdi *ftdi)
 {
-        int UxxxStatus;
-        u32 pcidata;
-        int reg = 0;
-        u8 fn;
-        int activePCIfn = 0;
-        int max_devices = 0;
-        int controllers = 0;
-        int unrecognized = 0;
-        ftdi->function = 0;
-        for (fn = 0; (fn < 4); fn++) {
-                u32 pciVID = 0;
-                u32 pciPID = 0;
-                int devices = 0;
-                activePCIfn = fn << 8;
-                UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                        &pcidata);
-                if (UxxxStatus)
-                        return UxxxStatus;
-                pciVID = pcidata & 0xFFFF;
-                pciPID = (pcidata >> 16) & 0xFFFF;
-                if ((pciVID == PCI_VENDOR_ID_OPTI) && (pciPID == 0xc861)) {
-                        devices = ftdi_elan_found_controller(ftdi, fn, 0);
-                        controllers += 1;
-                } else if ((pciVID == PCI_VENDOR_ID_NEC) && (pciPID == 0x0035))
-                        {
-                        devices = ftdi_elan_found_controller(ftdi, fn, 0);
-                        controllers += 1;
-                } else if ((pciVID == PCI_VENDOR_ID_AL) && (pciPID == 0x5237)) {
-                        devices = ftdi_elan_found_controller(ftdi, fn, 0);
-                        controllers += 1;
-                } else if ((pciVID == PCI_VENDOR_ID_ATT) && (pciPID == 0x5802))
-                        {
-                        devices = ftdi_elan_found_controller(ftdi, fn, 0);
-                        controllers += 1;
-                } else if (pciVID == PCI_VENDOR_ID_AMD && pciPID == 0x740c) {
-                        devices = ftdi_elan_found_controller(ftdi, fn,
-                                OHCI_QUIRK_AMD756);
-                        controllers += 1;
-                } else if (pciVID == PCI_VENDOR_ID_COMPAQ && pciPID == 0xa0f8) {
-                        devices = ftdi_elan_found_controller(ftdi, fn,
-                                OHCI_QUIRK_ZFMICRO);
-                        controllers += 1;
-                } else if (0 == pcidata) {
-                } else
-                        unrecognized += 1;
-                if (devices > max_devices) {
-                        max_devices = devices;
-                        ftdi->function = fn + 1;
-                        ftdi->platform_data.vendor = pciVID;
-                        ftdi->platform_data.device = pciPID;
-                }
-        }
-        if (ftdi->function > 0) {
-                UxxxStatus = ftdi_elan_setup_controller(ftdi,
-                        ftdi->function - 1);
-                if (UxxxStatus)
-                        return UxxxStatus;
-                return 0;
-        } else if (controllers > 0) {
-                return -ENXIO;
-        } else if (unrecognized > 0) {
-                return -ENXIO;
-        } else {
-                ftdi->enumerated = 0;
-                return -ENXIO;
-        }
+	int UxxxStatus;
+	u32 pcidata;
+	int reg = 0;
+	u8 fn;
+	int activePCIfn = 0;
+	int max_devices = 0;
+	int controllers = 0;
+	int unrecognized = 0;
+	ftdi->function = 0;
+	for (fn = 0; (fn < 4); fn++) {
+		u32 pciVID = 0;
+		u32 pciPID = 0;
+		int devices = 0;
+		activePCIfn = fn << 8;
+		UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+						   &pcidata);
+		if (UxxxStatus)
+			return UxxxStatus;
+		pciVID = pcidata & 0xFFFF;
+		pciPID = (pcidata >> 16) & 0xFFFF;
+		if ((pciVID == PCI_VENDOR_ID_OPTI) && (pciPID == 0xc861)) {
+			devices = ftdi_elan_found_controller(ftdi, fn, 0);
+			controllers += 1;
+		} else if ((pciVID == PCI_VENDOR_ID_NEC) && (pciPID == 0x0035))
+		{
+			devices = ftdi_elan_found_controller(ftdi, fn, 0);
+			controllers += 1;
+		} else if ((pciVID == PCI_VENDOR_ID_AL) && (pciPID == 0x5237)) {
+			devices = ftdi_elan_found_controller(ftdi, fn, 0);
+			controllers += 1;
+		} else if ((pciVID == PCI_VENDOR_ID_ATT) && (pciPID == 0x5802))
+		{
+			devices = ftdi_elan_found_controller(ftdi, fn, 0);
+			controllers += 1;
+		} else if (pciVID == PCI_VENDOR_ID_AMD && pciPID == 0x740c) {
+			devices = ftdi_elan_found_controller(ftdi, fn,
+							     OHCI_QUIRK_AMD756);
+			controllers += 1;
+		} else if (pciVID == PCI_VENDOR_ID_COMPAQ && pciPID == 0xa0f8) {
+			devices = ftdi_elan_found_controller(ftdi, fn,
+							     OHCI_QUIRK_ZFMICRO);
+			controllers += 1;
+		} else if (0 == pcidata) {
+		} else
+			unrecognized += 1;
+		if (devices > max_devices) {
+			max_devices = devices;
+			ftdi->function = fn + 1;
+			ftdi->platform_data.vendor = pciVID;
+			ftdi->platform_data.device = pciPID;
+		}
+	}
+	if (ftdi->function > 0) {
+		UxxxStatus = ftdi_elan_setup_controller(ftdi,
+							ftdi->function - 1);
+		if (UxxxStatus)
+			return UxxxStatus;
+		return 0;
+	} else if (controllers > 0) {
+		return -ENXIO;
+	} else if (unrecognized > 0) {
+		return -ENXIO;
+	} else {
+		ftdi->enumerated = 0;
+		return -ENXIO;
+	}
 }
 
 
 /*
-* we use only the first bulk-in and bulk-out endpoints
-*/
+ * we use only the first bulk-in and bulk-out endpoints
+ */
 static int ftdi_elan_probe(struct usb_interface *interface,
-        const struct usb_device_id *id)
+			   const struct usb_device_id *id)
 {
-        struct usb_host_interface *iface_desc;
-        struct usb_endpoint_descriptor *endpoint;
-        size_t buffer_size;
-        int i;
-        int retval = -ENOMEM;
-        struct usb_ftdi *ftdi;
+	struct usb_host_interface *iface_desc;
+	struct usb_endpoint_descriptor *endpoint;
+	size_t buffer_size;
+	int i;
+	int retval = -ENOMEM;
+	struct usb_ftdi *ftdi;
 
 	ftdi = kzalloc(sizeof(struct usb_ftdi), GFP_KERNEL);
-	if (!ftdi) {
-                printk(KERN_ERR "Out of memory\n");
-                return -ENOMEM;
-        }
-
-        mutex_lock(&ftdi_module_lock);
-        list_add_tail(&ftdi->ftdi_list, &ftdi_static_list);
-        ftdi->sequence_num = ++ftdi_instances;
-        mutex_unlock(&ftdi_module_lock);
-        ftdi_elan_init_kref(ftdi);
+	if (!ftdi)
+		return -ENOMEM;
+
+	mutex_lock(&ftdi_module_lock);
+	list_add_tail(&ftdi->ftdi_list, &ftdi_static_list);
+	ftdi->sequence_num = ++ftdi_instances;
+	mutex_unlock(&ftdi_module_lock);
+	ftdi_elan_init_kref(ftdi);
 	sema_init(&ftdi->sw_lock, 1);
-        ftdi->udev = usb_get_dev(interface_to_usbdev(interface));
-        ftdi->interface = interface;
-        mutex_init(&ftdi->u132_lock);
-        ftdi->expected = 4;
-        iface_desc = interface->cur_altsetting;
-        for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
-                endpoint = &iface_desc->endpoint[i].desc;
-                if (!ftdi->bulk_in_endpointAddr &&
+	ftdi->udev = usb_get_dev(interface_to_usbdev(interface));
+	ftdi->interface = interface;
+	mutex_init(&ftdi->u132_lock);
+	ftdi->expected = 4;
+	iface_desc = interface->cur_altsetting;
+	for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
+		endpoint = &iface_desc->endpoint[i].desc;
+		if (!ftdi->bulk_in_endpointAddr &&
 		    usb_endpoint_is_bulk_in(endpoint)) {
-                        buffer_size = usb_endpoint_maxp(endpoint);
-                        ftdi->bulk_in_size = buffer_size;
-                        ftdi->bulk_in_endpointAddr = endpoint->bEndpointAddress;
-                        ftdi->bulk_in_buffer = kmalloc(buffer_size, GFP_KERNEL);
-                        if (!ftdi->bulk_in_buffer) {
-                                dev_err(&ftdi->udev->dev, "Could not allocate b"
-                                        "ulk_in_buffer\n");
-                                retval = -ENOMEM;
-                                goto error;
-                        }
-                }
-                if (!ftdi->bulk_out_endpointAddr &&
+			buffer_size = usb_endpoint_maxp(endpoint);
+			ftdi->bulk_in_size = buffer_size;
+			ftdi->bulk_in_endpointAddr = endpoint->bEndpointAddress;
+			ftdi->bulk_in_buffer = kmalloc(buffer_size, GFP_KERNEL);
+			if (!ftdi->bulk_in_buffer) {
+				dev_err(&ftdi->udev->dev, "Could not allocate bulk_in_buffer\n");
+				retval = -ENOMEM;
+				goto error;
+			}
+		}
+		if (!ftdi->bulk_out_endpointAddr &&
 		    usb_endpoint_is_bulk_out(endpoint)) {
-                        ftdi->bulk_out_endpointAddr =
-                                endpoint->bEndpointAddress;
-                }
-        }
-        if (!(ftdi->bulk_in_endpointAddr && ftdi->bulk_out_endpointAddr)) {
-                dev_err(&ftdi->udev->dev, "Could not find both bulk-in and bulk"
-                        "-out endpoints\n");
-                retval = -ENODEV;
-                goto error;
-        }
-        dev_info(&ftdi->udev->dev, "interface %d has I=%02X O=%02X\n",
-                iface_desc->desc.bInterfaceNumber, ftdi->bulk_in_endpointAddr,
-                ftdi->bulk_out_endpointAddr);
-        usb_set_intfdata(interface, ftdi);
-        if (iface_desc->desc.bInterfaceNumber == 0 &&
-                ftdi->bulk_in_endpointAddr == 0x81 &&
-                ftdi->bulk_out_endpointAddr == 0x02) {
-                retval = usb_register_dev(interface, &ftdi_elan_jtag_class);
-                if (retval) {
-                        dev_err(&ftdi->udev->dev, "Not able to get a minor for "
-                                "this device.\n");
-                        usb_set_intfdata(interface, NULL);
-                        retval = -ENOMEM;
-                        goto error;
-                } else {
-                        ftdi->class = &ftdi_elan_jtag_class;
-                        dev_info(&ftdi->udev->dev, "USB FDTI=%p JTAG interface "
-                                "%d now attached to ftdi%d\n", ftdi,
-                                iface_desc->desc.bInterfaceNumber,
-                                interface->minor);
-                        return 0;
-                }
-        } else if (iface_desc->desc.bInterfaceNumber == 1 &&
-                ftdi->bulk_in_endpointAddr == 0x83 &&
-                ftdi->bulk_out_endpointAddr == 0x04) {
-                ftdi->class = NULL;
-                dev_info(&ftdi->udev->dev, "USB FDTI=%p ELAN interface %d now a"
-                        "ctivated\n", ftdi, iface_desc->desc.bInterfaceNumber);
-                INIT_DELAYED_WORK(&ftdi->status_work, ftdi_elan_status_work);
-                INIT_DELAYED_WORK(&ftdi->command_work, ftdi_elan_command_work);
-                INIT_DELAYED_WORK(&ftdi->respond_work, ftdi_elan_respond_work);
-                ftdi_status_queue_work(ftdi, msecs_to_jiffies(3 *1000));
-                return 0;
-        } else {
-                dev_err(&ftdi->udev->dev,
-                        "Could not find ELAN's U132 device\n");
-                retval = -ENODEV;
-                goto error;
-        }
-      error:if (ftdi) {
-                ftdi_elan_put_kref(ftdi);
-        }
-        return retval;
+			ftdi->bulk_out_endpointAddr =
+				endpoint->bEndpointAddress;
+		}
+	}
+	if (!(ftdi->bulk_in_endpointAddr && ftdi->bulk_out_endpointAddr)) {
+		dev_err(&ftdi->udev->dev, "Could not find both bulk-in and bulk-out endpoints\n");
+		retval = -ENODEV;
+		goto error;
+	}
+	dev_info(&ftdi->udev->dev, "interface %d has I=%02X O=%02X\n",
+		 iface_desc->desc.bInterfaceNumber, ftdi->bulk_in_endpointAddr,
+		 ftdi->bulk_out_endpointAddr);
+	usb_set_intfdata(interface, ftdi);
+	if (iface_desc->desc.bInterfaceNumber == 0 &&
+	    ftdi->bulk_in_endpointAddr == 0x81 &&
+	    ftdi->bulk_out_endpointAddr == 0x02) {
+		retval = usb_register_dev(interface, &ftdi_elan_jtag_class);
+		if (retval) {
+			dev_err(&ftdi->udev->dev, "Not able to get a minor for this device\n");
+			usb_set_intfdata(interface, NULL);
+			retval = -ENOMEM;
+			goto error;
+		} else {
+			ftdi->class = &ftdi_elan_jtag_class;
+			dev_info(&ftdi->udev->dev, "USB FDTI=%p JTAG interface %d now attached to ftdi%d\n",
+				 ftdi, iface_desc->desc.bInterfaceNumber,
+				 interface->minor);
+			return 0;
+		}
+	} else if (iface_desc->desc.bInterfaceNumber == 1 &&
+		   ftdi->bulk_in_endpointAddr == 0x83 &&
+		   ftdi->bulk_out_endpointAddr == 0x04) {
+		ftdi->class = NULL;
+		dev_info(&ftdi->udev->dev, "USB FDTI=%p ELAN interface %d now activated\n",
+			 ftdi, iface_desc->desc.bInterfaceNumber);
+		INIT_DELAYED_WORK(&ftdi->status_work, ftdi_elan_status_work);
+		INIT_DELAYED_WORK(&ftdi->command_work, ftdi_elan_command_work);
+		INIT_DELAYED_WORK(&ftdi->respond_work, ftdi_elan_respond_work);
+		ftdi_status_queue_work(ftdi, msecs_to_jiffies(3 *1000));
+		return 0;
+	} else {
+		dev_err(&ftdi->udev->dev,
+			"Could not find ELAN's U132 device\n");
+		retval = -ENODEV;
+		goto error;
+	}
+error:if (ftdi) {
+		ftdi_elan_put_kref(ftdi);
+	}
+	return retval;
 }
 
 static void ftdi_elan_disconnect(struct usb_interface *interface)
 {
-        struct usb_ftdi *ftdi = usb_get_intfdata(interface);
-        ftdi->disconnected += 1;
-        if (ftdi->class) {
-                int minor = interface->minor;
-                struct usb_class_driver *class = ftdi->class;
-                usb_set_intfdata(interface, NULL);
-                usb_deregister_dev(interface, class);
-                dev_info(&ftdi->udev->dev, "USB FTDI U132 jtag interface on min"
-                        "or %d now disconnected\n", minor);
-        } else {
-                ftdi_status_cancel_work(ftdi);
-                ftdi_command_cancel_work(ftdi);
-                ftdi_response_cancel_work(ftdi);
-                ftdi_elan_abandon_completions(ftdi);
-                ftdi_elan_abandon_targets(ftdi);
-                if (ftdi->registered) {
-                        platform_device_unregister(&ftdi->platform_dev);
-                        ftdi->synchronized = 0;
-                        ftdi->enumerated = 0;
-                        ftdi->initialized = 0;
-                        ftdi->registered = 0;
-                }
-                flush_workqueue(status_queue);
-                flush_workqueue(command_queue);
-                flush_workqueue(respond_queue);
-                ftdi->disconnected += 1;
-                usb_set_intfdata(interface, NULL);
-                dev_info(&ftdi->udev->dev, "USB FTDI U132 host controller inter"
-                        "face now disconnected\n");
-        }
-        ftdi_elan_put_kref(ftdi);
+	struct usb_ftdi *ftdi = usb_get_intfdata(interface);
+	ftdi->disconnected += 1;
+	if (ftdi->class) {
+		int minor = interface->minor;
+		struct usb_class_driver *class = ftdi->class;
+		usb_set_intfdata(interface, NULL);
+		usb_deregister_dev(interface, class);
+		dev_info(&ftdi->udev->dev, "USB FTDI U132 jtag interface on minor %d now disconnected\n",
+			 minor);
+	} else {
+		ftdi_status_cancel_work(ftdi);
+		ftdi_command_cancel_work(ftdi);
+		ftdi_response_cancel_work(ftdi);
+		ftdi_elan_abandon_completions(ftdi);
+		ftdi_elan_abandon_targets(ftdi);
+		if (ftdi->registered) {
+			platform_device_unregister(&ftdi->platform_dev);
+			ftdi->synchronized = 0;
+			ftdi->enumerated = 0;
+			ftdi->initialized = 0;
+			ftdi->registered = 0;
+		}
+		flush_workqueue(status_queue);
+		flush_workqueue(command_queue);
+		flush_workqueue(respond_queue);
+		ftdi->disconnected += 1;
+		usb_set_intfdata(interface, NULL);
+		dev_info(&ftdi->udev->dev, "USB FTDI U132 host controller interface now disconnected\n");
+	}
+	ftdi_elan_put_kref(ftdi);
 }
 
 static struct usb_driver ftdi_elan_driver = {
-        .name = "ftdi-elan",
-        .probe = ftdi_elan_probe,
-        .disconnect = ftdi_elan_disconnect,
-        .id_table = ftdi_elan_table,
+	.name = "ftdi-elan",
+	.probe = ftdi_elan_probe,
+	.disconnect = ftdi_elan_disconnect,
+	.id_table = ftdi_elan_table,
 };
 static int __init ftdi_elan_init(void)
 {
-        int result;
-        printk(KERN_INFO "driver %s\n", ftdi_elan_driver.name);
-        mutex_init(&ftdi_module_lock);
-        INIT_LIST_HEAD(&ftdi_static_list);
-        status_queue = create_singlethread_workqueue("ftdi-status-control");
+	int result;
+	pr_info("driver %s\n", ftdi_elan_driver.name);
+	mutex_init(&ftdi_module_lock);
+	INIT_LIST_HEAD(&ftdi_static_list);
+	status_queue = create_singlethread_workqueue("ftdi-status-control");
 	if (!status_queue)
 		goto err_status_queue;
-        command_queue = create_singlethread_workqueue("ftdi-command-engine");
+	command_queue = create_singlethread_workqueue("ftdi-command-engine");
 	if (!command_queue)
 		goto err_command_queue;
-        respond_queue = create_singlethread_workqueue("ftdi-respond-engine");
+	respond_queue = create_singlethread_workqueue("ftdi-respond-engine");
 	if (!respond_queue)
 		goto err_respond_queue;
-        result = usb_register(&ftdi_elan_driver);
-        if (result) {
+	result = usb_register(&ftdi_elan_driver);
+	if (result) {
 		destroy_workqueue(status_queue);
 		destroy_workqueue(command_queue);
 		destroy_workqueue(respond_queue);
-                printk(KERN_ERR "usb_register failed. Error number %d\n",
-		       result);
+		pr_err("usb_register failed. Error number %d\n", result);
 	}
-        return result;
+	return result;
 
- err_respond_queue:
+err_respond_queue:
 	destroy_workqueue(command_queue);
- err_command_queue:
+err_command_queue:
 	destroy_workqueue(status_queue);
- err_status_queue:
-	printk(KERN_ERR "%s couldn't create workqueue\n", ftdi_elan_driver.name);
+err_status_queue:
+	pr_err("%s couldn't create workqueue\n", ftdi_elan_driver.name);
 	return -ENOMEM;
 }
 
 static void __exit ftdi_elan_exit(void)
 {
-        struct usb_ftdi *ftdi;
-        struct usb_ftdi *temp;
-        usb_deregister(&ftdi_elan_driver);
-        printk(KERN_INFO "ftdi_u132 driver deregistered\n");
-        list_for_each_entry_safe(ftdi, temp, &ftdi_static_list, ftdi_list) {
-                ftdi_status_cancel_work(ftdi);
-                ftdi_command_cancel_work(ftdi);
-                ftdi_response_cancel_work(ftdi);
-        } flush_workqueue(status_queue);
-        destroy_workqueue(status_queue);
-        status_queue = NULL;
-        flush_workqueue(command_queue);
-        destroy_workqueue(command_queue);
-        command_queue = NULL;
-        flush_workqueue(respond_queue);
-        destroy_workqueue(respond_queue);
-        respond_queue = NULL;
+	struct usb_ftdi *ftdi;
+	struct usb_ftdi *temp;
+	usb_deregister(&ftdi_elan_driver);
+	pr_info("ftdi_u132 driver deregistered\n");
+	list_for_each_entry_safe(ftdi, temp, &ftdi_static_list, ftdi_list) {
+		ftdi_status_cancel_work(ftdi);
+		ftdi_command_cancel_work(ftdi);
+		ftdi_response_cancel_work(ftdi);
+	} flush_workqueue(status_queue);
+	destroy_workqueue(status_queue);
+	status_queue = NULL;
+	flush_workqueue(command_queue);
+	destroy_workqueue(command_queue);
+	command_queue = NULL;
+	flush_workqueue(respond_queue);
+	destroy_workqueue(respond_queue);
+	respond_queue = NULL;
 }
 
 
diff --git a/drivers/usb/misc/iowarrior.c b/drivers/usb/misc/iowarrior.c
index 20bcfdd7eac..c6bfd13f6c9 100644
--- a/drivers/usb/misc/iowarrior.c
+++ b/drivers/usb/misc/iowarrior.c
@@ -51,19 +51,12 @@
 */
 #define MAX_WRITES_IN_FLIGHT 4
 
-/* Use our own dbg macro */
-#undef dbg
-#define dbg( format, arg... ) do { if( debug ) printk( KERN_DEBUG __FILE__ ": " format "\n" , ## arg ); } while ( 0 )
-
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
 
 /* Module parameters */
 static DEFINE_MUTEX(iowarrior_mutex);
-static bool debug = 0;
-module_param(debug, bool, 0644);
-MODULE_PARM_DESC(debug, "debug=1 enables debugging messages");
 
 static struct usb_driver iowarrior_driver;
 static DEFINE_MUTEX(iowarrior_open_disc_lock);
@@ -235,8 +228,8 @@ static void iowarrior_write_callback(struct urb *urb)
 	if (status &&
 	    !(status == -ENOENT ||
 	      status == -ECONNRESET || status == -ESHUTDOWN)) {
-		dbg("%s - nonzero write bulk status received: %d",
-		    __func__, status);
+		dev_dbg(&dev->interface->dev,
+			"nonzero write bulk status received: %d\n", status);
 	}
 	/* free up our allocated buffer */
 	usb_free_coherent(urb->dev, urb->transfer_buffer_length,
@@ -251,7 +244,7 @@ static void iowarrior_write_callback(struct urb *urb)
  */
 static inline void iowarrior_delete(struct iowarrior *dev)
 {
-	dbg("%s - minor %d", __func__, dev->minor);
+	dev_dbg(&dev->interface->dev, "minor %d\n", dev->minor);
 	kfree(dev->int_in_buffer);
 	usb_free_urb(dev->int_in_urb);
 	kfree(dev->read_queue);
@@ -288,7 +281,8 @@ static ssize_t iowarrior_read(struct file *file, char __user *buffer,
 	if (dev == NULL || !dev->present)
 		return -ENODEV;
 
-	dbg("%s - minor %d, count = %zd", __func__, dev->minor, count);
+	dev_dbg(&dev->interface->dev, "minor %d, count = %zd\n",
+		dev->minor, count);
 
 	/* read count must be packet size (+ time stamp) */
 	if ((count != dev->report_size)
@@ -356,7 +350,8 @@ static ssize_t iowarrior_write(struct file *file,
 		retval = -ENODEV;
 		goto exit;
 	}
-	dbg("%s - minor %d, count = %zd", __func__, dev->minor, count);
+	dev_dbg(&dev->interface->dev, "minor %d, count = %zd\n",
+		dev->minor, count);
 	/* if count is 0 we're already done */
 	if (count == 0) {
 		retval = 0;
@@ -418,14 +413,16 @@ static ssize_t iowarrior_write(struct file *file,
 		int_out_urb = usb_alloc_urb(0, GFP_KERNEL);
 		if (!int_out_urb) {
 			retval = -ENOMEM;
-			dbg("%s Unable to allocate urb ", __func__);
+			dev_dbg(&dev->interface->dev,
+				"Unable to allocate urb\n");
 			goto error_no_urb;
 		}
 		buf = usb_alloc_coherent(dev->udev, dev->report_size,
 					 GFP_KERNEL, &int_out_urb->transfer_dma);
 		if (!buf) {
 			retval = -ENOMEM;
-			dbg("%s Unable to allocate buffer ", __func__);
+			dev_dbg(&dev->interface->dev,
+				"Unable to allocate buffer\n");
 			goto error_no_buffer;
 		}
 		usb_fill_int_urb(int_out_urb, dev->udev,
@@ -441,8 +438,9 @@ static ssize_t iowarrior_write(struct file *file,
 		}
 		retval = usb_submit_urb(int_out_urb, GFP_KERNEL);
 		if (retval) {
-			dbg("%s submit error %d for urb nr.%d", __func__,
-			    retval, atomic_read(&dev->write_busy));
+			dev_dbg(&dev->interface->dev,
+				"submit error %d for urb nr.%d\n",
+				retval, atomic_read(&dev->write_busy));
 			goto error;
 		}
 		/* submit was ok */
@@ -502,8 +500,8 @@ static long iowarrior_ioctl(struct file *file, unsigned int cmd,
 		goto error_out;
 	}
 
-	dbg("%s - minor %d, cmd 0x%.4x, arg %ld", __func__, dev->minor, cmd,
-	    arg);
+	dev_dbg(&dev->interface->dev, "minor %d, cmd 0x%.4x, arg %ld\n",
+		dev->minor, cmd, arg);
 
 	retval = 0;
 	io_res = 0;
@@ -601,8 +599,6 @@ static int iowarrior_open(struct inode *inode, struct file *file)
 	int subminor;
 	int retval = 0;
 
-	dbg("%s", __func__);
-
 	mutex_lock(&iowarrior_mutex);
 	subminor = iminor(inode);
 
@@ -662,7 +658,7 @@ static int iowarrior_release(struct inode *inode, struct file *file)
 		return -ENODEV;
 	}
 
-	dbg("%s - minor %d", __func__, dev->minor);
+	dev_dbg(&dev->interface->dev, "minor %d\n", dev->minor);
 
 	/* lock our device */
 	mutex_lock(&dev->mutex);
diff --git a/drivers/usb/misc/usb3503.c b/drivers/usb/misc/usb3503.c
index a31641e18d1..f43c61989ce 100644
--- a/drivers/usb/misc/usb3503.c
+++ b/drivers/usb/misc/usb3503.c
@@ -18,6 +18,7 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  */
 
+#include <linux/clk.h>
 #include <linux/i2c.h>
 #include <linux/gpio.h>
 #include <linux/delay.h>
@@ -57,10 +58,12 @@ struct usb3503 {
 	enum usb3503_mode	mode;
 	struct regmap		*regmap;
 	struct device		*dev;
+	struct clk		*clk;
 	u8	port_off_mask;
 	int	gpio_intn;
 	int	gpio_reset;
 	int	gpio_connect;
+	bool	secondary_ref_clk;
 };
 
 static int usb3503_reset(struct usb3503 *hub, int state)
@@ -184,8 +187,58 @@ static int usb3503_probe(struct usb3503 *hub)
 		hub->gpio_reset		= pdata->gpio_reset;
 		hub->mode		= pdata->initial_mode;
 	} else if (np) {
+		struct clk *clk;
 		hub->port_off_mask = 0;
 
+		clk = devm_clk_get(dev, "refclk");
+		if (IS_ERR(clk) && PTR_ERR(clk) != -ENOENT) {
+			dev_err(dev, "unable to request refclk (%d)\n", err);
+			return PTR_ERR(clk);
+		}
+
+		if (!IS_ERR(clk)) {
+			u32 rate = 0;
+			hub->clk = clk;
+
+			if (!of_property_read_u32(np, "refclk-frequency",
+						 &rate)) {
+
+				switch (rate) {
+				case 38400000:
+				case 26000000:
+				case 19200000:
+				case 12000000:
+					hub->secondary_ref_clk = 0;
+					break;
+				case 24000000:
+				case 27000000:
+				case 25000000:
+				case 50000000:
+					hub->secondary_ref_clk = 1;
+					break;
+				default:
+					dev_err(dev,
+						"unsupported reference clock rate (%d)\n",
+						(int) rate);
+					return -EINVAL;
+				}
+				err = clk_set_rate(hub->clk, rate);
+				if (err) {
+					dev_err(dev,
+						"unable to set reference clock rate to %d\n",
+						(int) rate);
+					return err;
+				}
+			}
+
+			err = clk_prepare_enable(hub->clk);
+			if (err) {
+				dev_err(dev,
+					"unable to enable reference clock\n");
+				return err;
+			}
+		}
+
 		property = of_get_property(np, "disabled-ports", &len);
 		if (property && (len / sizeof(u32)) > 0) {
 			int i;
@@ -213,8 +266,10 @@ static int usb3503_probe(struct usb3503 *hub)
 		dev_err(dev, "Ports disabled with no control interface\n");
 
 	if (gpio_is_valid(hub->gpio_intn)) {
-		err = devm_gpio_request_one(dev, hub->gpio_intn,
-				GPIOF_OUT_INIT_HIGH, "usb3503 intn");
+		int val = hub->secondary_ref_clk ? GPIOF_OUT_INIT_LOW :
+						   GPIOF_OUT_INIT_HIGH;
+		err = devm_gpio_request_one(dev, hub->gpio_intn, val,
+					    "usb3503 intn");
 		if (err) {
 			dev_err(dev,
 				"unable to request GPIO %d as connect pin (%d)\n",
diff --git a/drivers/usb/misc/usbtest.c b/drivers/usb/misc/usbtest.c
index f6568b5e9b0..51a6da25677 100644
--- a/drivers/usb/misc/usbtest.c
+++ b/drivers/usb/misc/usbtest.c
@@ -1320,6 +1320,11 @@ static int unlink1(struct usbtest_dev *dev, int pipe, int size, int async)
 	urb->context = &completion;
 	urb->complete = unlink1_callback;
 
+	if (usb_pipeout(urb->pipe)) {
+		simple_fill_buf(urb);
+		urb->transfer_flags |= URB_ZERO_PACKET;
+	}
+
 	/* keep the endpoint busy.  there are lots of hc/hcd-internal
 	 * states, and testing should get to all of them over time.
 	 *
@@ -1340,6 +1345,9 @@ static int unlink1(struct usbtest_dev *dev, int pipe, int size, int async)
 		while (!completion_done(&completion)) {
 			retval = usb_unlink_urb(urb);
 
+			if (retval == 0 && usb_pipein(urb->pipe))
+				retval = simple_check_buf(dev, urb);
+
 			switch (retval) {
 			case -EBUSY:
 			case -EIDRM:
@@ -1450,6 +1458,11 @@ static int unlink_queued(struct usbtest_dev *dev, int pipe, unsigned num,
 				unlink_queued_callback, &ctx);
 		ctx.urbs[i]->transfer_dma = buf_dma;
 		ctx.urbs[i]->transfer_flags = URB_NO_TRANSFER_DMA_MAP;
+
+		if (usb_pipeout(ctx.urbs[i]->pipe)) {
+			simple_fill_buf(ctx.urbs[i]);
+			ctx.urbs[i]->transfer_flags |= URB_ZERO_PACKET;
+		}
 	}
 
 	/* Submit all the URBs and then unlink URBs num - 4 and num - 2. */
diff --git a/drivers/usb/misc/yurex.c b/drivers/usb/misc/yurex.c
index 24278208bf7..1472805083d 100644
--- a/drivers/usb/misc/yurex.c
+++ b/drivers/usb/misc/yurex.c
@@ -296,6 +296,7 @@ static int yurex_probe(struct usb_interface *interface, const struct usb_device_
 
 	/* save our data pointer in this interface device */
 	usb_set_intfdata(interface, dev);
+	dev->bbu = -1;
 
 	/* we can register the device now, as it is ready */
 	retval = usb_register_dev(interface, &yurex_class);
@@ -306,8 +307,6 @@ static int yurex_probe(struct usb_interface *interface, const struct usb_device_
 		goto error;
 	}
 
-	dev->bbu = -1;
-
 	dev_info(&interface->dev,
 		 "USB YUREX device now attached to Yurex #%d\n",
 		 interface->minor);
diff --git a/drivers/usb/musb/Kconfig b/drivers/usb/musb/Kconfig
index 8b789792f6f..06cc5d6ea68 100644
--- a/drivers/usb/musb/Kconfig
+++ b/drivers/usb/musb/Kconfig
@@ -76,7 +76,7 @@ config USB_MUSB_TUSB6010
 
 config USB_MUSB_OMAP2PLUS
 	tristate "OMAP2430 and onwards"
-	depends on ARCH_OMAP2PLUS
+	depends on ARCH_OMAP2PLUS && USB
 	select GENERIC_PHY
 
 config USB_MUSB_AM35X
@@ -141,10 +141,11 @@ config USB_TI_CPPI_DMA
 config USB_TI_CPPI41_DMA
 	bool 'TI CPPI 4.1 (AM335x)'
 	depends on ARCH_OMAP
+	select TI_CPPI41
 
 config USB_TUSB_OMAP_DMA
 	bool 'TUSB 6010'
-	depends on USB_MUSB_TUSB6010
+	depends on USB_MUSB_TUSB6010 = USB_MUSB_HDRC # both built-in or both modules
 	depends on ARCH_OMAP
 	help
 	  Enable DMA transfers on TUSB 6010 when OMAP DMA is available.
diff --git a/drivers/usb/musb/am35x.c b/drivers/usb/musb/am35x.c
index b3aa0184af9..0a34dd85955 100644
--- a/drivers/usb/musb/am35x.c
+++ b/drivers/usb/musb/am35x.c
@@ -32,7 +32,7 @@
 #include <linux/io.h>
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 #include <linux/platform_data/usb-omap.h>
 
 #include "musb_core.h"
@@ -85,6 +85,7 @@
 struct am35x_glue {
 	struct device		*dev;
 	struct platform_device	*musb;
+	struct platform_device	*phy;
 	struct clk		*phy_clk;
 	struct clk		*clk;
 };
@@ -360,7 +361,6 @@ static int am35x_musb_init(struct musb *musb)
 	if (!rev)
 		return -ENODEV;
 
-	usb_nop_xceiv_register();
 	musb->xceiv = usb_get_phy(USB_PHY_TYPE_USB2);
 	if (IS_ERR_OR_NULL(musb->xceiv))
 		return -EPROBE_DEFER;
@@ -402,7 +402,6 @@ static int am35x_musb_exit(struct musb *musb)
 		data->set_phy_power(0);
 
 	usb_put_phy(musb->xceiv);
-	usb_nop_xceiv_unregister();
 
 	return 0;
 }
@@ -505,6 +504,9 @@ static int am35x_probe(struct platform_device *pdev)
 
 	pdata->platform_ops		= &am35x_ops;
 
+	glue->phy = usb_phy_generic_register();
+	if (IS_ERR(glue->phy))
+		goto err7;
 	platform_set_drvdata(pdev, glue);
 
 	pinfo = am35x_dev_info;
@@ -518,11 +520,14 @@ static int am35x_probe(struct platform_device *pdev)
 	if (IS_ERR(musb)) {
 		ret = PTR_ERR(musb);
 		dev_err(&pdev->dev, "failed to register musb device: %d\n", ret);
-		goto err7;
+		goto err8;
 	}
 
 	return 0;
 
+err8:
+	usb_phy_generic_unregister(glue->phy);
+
 err7:
 	clk_disable(clk);
 
@@ -547,6 +552,7 @@ static int am35x_remove(struct platform_device *pdev)
 	struct am35x_glue	*glue = platform_get_drvdata(pdev);
 
 	platform_device_unregister(glue->musb);
+	usb_phy_generic_unregister(glue->phy);
 	clk_disable(glue->clk);
 	clk_disable(glue->phy_clk);
 	clk_put(glue->clk);
diff --git a/drivers/usb/musb/blackfin.c b/drivers/usb/musb/blackfin.c
index 796677fa9a1..d40d5f0b552 100644
--- a/drivers/usb/musb/blackfin.c
+++ b/drivers/usb/musb/blackfin.c
@@ -18,7 +18,7 @@
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
 #include <linux/prefetch.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 
 #include <asm/cacheflush.h>
 
@@ -29,6 +29,7 @@
 struct bfin_glue {
 	struct device		*dev;
 	struct platform_device	*musb;
+	struct platform_device	*phy;
 };
 #define glue_to_musb(g)		platform_get_drvdata(g->musb)
 
@@ -401,7 +402,6 @@ static int bfin_musb_init(struct musb *musb)
 	}
 	gpio_direction_output(musb->config->gpio_vrsel, 0);
 
-	usb_nop_xceiv_register();
 	musb->xceiv = usb_get_phy(USB_PHY_TYPE_USB2);
 	if (IS_ERR_OR_NULL(musb->xceiv)) {
 		gpio_free(musb->config->gpio_vrsel);
@@ -424,9 +424,8 @@ static int bfin_musb_init(struct musb *musb)
 static int bfin_musb_exit(struct musb *musb)
 {
 	gpio_free(musb->config->gpio_vrsel);
-
 	usb_put_phy(musb->xceiv);
-	usb_nop_xceiv_unregister();
+
 	return 0;
 }
 
@@ -477,6 +476,9 @@ static int bfin_probe(struct platform_device *pdev)
 
 	pdata->platform_ops		= &bfin_ops;
 
+	glue->phy = usb_phy_generic_register();
+	if (IS_ERR(glue->phy))
+		goto err2;
 	platform_set_drvdata(pdev, glue);
 
 	memset(musb_resources, 0x00, sizeof(*musb_resources) *
@@ -514,6 +516,9 @@ static int bfin_probe(struct platform_device *pdev)
 	return 0;
 
 err3:
+	usb_phy_generic_unregister(glue->phy);
+
+err2:
 	platform_device_put(musb);
 
 err1:
@@ -528,6 +533,7 @@ static int bfin_remove(struct platform_device *pdev)
 	struct bfin_glue		*glue = platform_get_drvdata(pdev);
 
 	platform_device_unregister(glue->musb);
+	usb_phy_generic_unregister(glue->phy);
 	kfree(glue);
 
 	return 0;
diff --git a/drivers/usb/musb/da8xx.c b/drivers/usb/musb/da8xx.c
index e3486de7199..058775e647a 100644
--- a/drivers/usb/musb/da8xx.c
+++ b/drivers/usb/musb/da8xx.c
@@ -32,7 +32,7 @@
 #include <linux/io.h>
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 
 #include <mach/da8xx.h>
 #include <linux/platform_data/usb-davinci.h>
@@ -85,6 +85,7 @@
 struct da8xx_glue {
 	struct device		*dev;
 	struct platform_device	*musb;
+	struct platform_device	*phy;
 	struct clk		*clk;
 };
 
@@ -418,7 +419,6 @@ static int da8xx_musb_init(struct musb *musb)
 	if (!rev)
 		goto fail;
 
-	usb_nop_xceiv_register();
 	musb->xceiv = usb_get_phy(USB_PHY_TYPE_USB2);
 	if (IS_ERR_OR_NULL(musb->xceiv)) {
 		ret = -EPROBE_DEFER;
@@ -453,7 +453,6 @@ static int da8xx_musb_exit(struct musb *musb)
 	phy_off();
 
 	usb_put_phy(musb->xceiv);
-	usb_nop_xceiv_unregister();
 
 	return 0;
 }
@@ -512,6 +511,11 @@ static int da8xx_probe(struct platform_device *pdev)
 
 	pdata->platform_ops		= &da8xx_ops;
 
+	glue->phy = usb_phy_generic_register();
+	if (IS_ERR(glue->phy)) {
+		ret = PTR_ERR(glue->phy);
+		goto err5;
+	}
 	platform_set_drvdata(pdev, glue);
 
 	memset(musb_resources, 0x00, sizeof(*musb_resources) *
@@ -538,11 +542,14 @@ static int da8xx_probe(struct platform_device *pdev)
 	if (IS_ERR(musb)) {
 		ret = PTR_ERR(musb);
 		dev_err(&pdev->dev, "failed to register musb device: %d\n", ret);
-		goto err5;
+		goto err6;
 	}
 
 	return 0;
 
+err6:
+	usb_phy_generic_unregister(glue->phy);
+
 err5:
 	clk_disable(clk);
 
@@ -561,6 +568,7 @@ static int da8xx_remove(struct platform_device *pdev)
 	struct da8xx_glue		*glue = platform_get_drvdata(pdev);
 
 	platform_device_unregister(glue->musb);
+	usb_phy_generic_unregister(glue->phy);
 	clk_disable(glue->clk);
 	clk_put(glue->clk);
 	kfree(glue);
diff --git a/drivers/usb/musb/davinci.c b/drivers/usb/musb/davinci.c
index c259dac9d05..de8492b06e4 100644
--- a/drivers/usb/musb/davinci.c
+++ b/drivers/usb/musb/davinci.c
@@ -32,7 +32,7 @@
 #include <linux/gpio.h>
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 
 #include <mach/cputype.h>
 #include <mach/hardware.h>
@@ -381,7 +381,6 @@ static int davinci_musb_init(struct musb *musb)
 	u32		revision;
 	int 		ret = -ENODEV;
 
-	usb_nop_xceiv_register();
 	musb->xceiv = usb_get_phy(USB_PHY_TYPE_USB2);
 	if (IS_ERR_OR_NULL(musb->xceiv)) {
 		ret = -EPROBE_DEFER;
@@ -439,7 +438,7 @@ static int davinci_musb_init(struct musb *musb)
 fail:
 	usb_put_phy(musb->xceiv);
 unregister:
-	usb_nop_xceiv_unregister();
+	usb_phy_generic_unregister();
 	return ret;
 }
 
@@ -487,7 +486,6 @@ static int davinci_musb_exit(struct musb *musb)
 	phy_off();
 
 	usb_put_phy(musb->xceiv);
-	usb_nop_xceiv_unregister();
 
 	return 0;
 }
@@ -545,6 +543,7 @@ static int davinci_probe(struct platform_device *pdev)
 
 	pdata->platform_ops		= &davinci_ops;
 
+	usb_phy_generic_register();
 	platform_set_drvdata(pdev, glue);
 
 	memset(musb_resources, 0x00, sizeof(*musb_resources) *
@@ -603,6 +602,7 @@ static int davinci_remove(struct platform_device *pdev)
 	struct davinci_glue		*glue = platform_get_drvdata(pdev);
 
 	platform_device_unregister(glue->musb);
+	usb_phy_generic_unregister();
 	clk_disable(glue->clk);
 	clk_put(glue->clk);
 	kfree(glue);
diff --git a/drivers/usb/musb/musb_core.c b/drivers/usb/musb/musb_core.c
index 07576907e2c..61da471b7ae 100644
--- a/drivers/usb/musb/musb_core.c
+++ b/drivers/usb/musb/musb_core.c
@@ -848,6 +848,10 @@ b_host:
 		}
 	}
 
+	/* handle babble condition */
+	if (int_usb & MUSB_INTR_BABBLE)
+		schedule_work(&musb->recover_work);
+
 #if 0
 /* REVISIT ... this would be for multiplexing periodic endpoints, or
  * supporting transfer phasing to prevent exceeding ISO bandwidth
@@ -1746,6 +1750,34 @@ static void musb_irq_work(struct work_struct *data)
 	}
 }
 
+/* Recover from babble interrupt conditions */
+static void musb_recover_work(struct work_struct *data)
+{
+	struct musb *musb = container_of(data, struct musb, recover_work);
+	int status;
+
+	musb_platform_reset(musb);
+
+	usb_phy_vbus_off(musb->xceiv);
+	udelay(100);
+
+	usb_phy_vbus_on(musb->xceiv);
+	udelay(100);
+
+	/*
+	 * When a babble condition occurs, the musb controller removes the
+	 * session bit and the endpoint config is lost.
+	 */
+	if (musb->dyn_fifo)
+		status = ep_config_from_table(musb);
+	else
+		status = ep_config_from_hw(musb);
+
+	/* start the session again */
+	if (status == 0)
+		musb_start(musb);
+}
+
 /* --------------------------------------------------------------------------
  * Init support
  */
@@ -1913,6 +1945,7 @@ musb_init_controller(struct device *dev, int nIrq, void __iomem *ctrl)
 
 	/* Init IRQ workqueue before request_irq */
 	INIT_WORK(&musb->irq_work, musb_irq_work);
+	INIT_WORK(&musb->recover_work, musb_recover_work);
 	INIT_DELAYED_WORK(&musb->deassert_reset_work, musb_deassert_reset);
 	INIT_DELAYED_WORK(&musb->finish_resume_work, musb_host_finish_resume);
 
@@ -2008,6 +2041,7 @@ fail4:
 
 fail3:
 	cancel_work_sync(&musb->irq_work);
+	cancel_work_sync(&musb->recover_work);
 	cancel_delayed_work_sync(&musb->finish_resume_work);
 	cancel_delayed_work_sync(&musb->deassert_reset_work);
 	if (musb->dma_controller)
@@ -2073,6 +2107,7 @@ static int musb_remove(struct platform_device *pdev)
 		dma_controller_destroy(musb->dma_controller);
 
 	cancel_work_sync(&musb->irq_work);
+	cancel_work_sync(&musb->recover_work);
 	cancel_delayed_work_sync(&musb->finish_resume_work);
 	cancel_delayed_work_sync(&musb->deassert_reset_work);
 	musb_free(musb);
diff --git a/drivers/usb/musb/musb_core.h b/drivers/usb/musb/musb_core.h
index 7083e82776f..d155a156f24 100644
--- a/drivers/usb/musb/musb_core.h
+++ b/drivers/usb/musb/musb_core.h
@@ -192,6 +192,7 @@ struct musb_platform_ops {
 
 	int	(*set_mode)(struct musb *musb, u8 mode);
 	void	(*try_idle)(struct musb *musb, unsigned long timeout);
+	void	(*reset)(struct musb *musb);
 
 	int	(*vbus_status)(struct musb *musb);
 	void	(*set_vbus)(struct musb *musb, int on);
@@ -296,6 +297,7 @@ struct musb {
 
 	irqreturn_t		(*isr)(int, void *);
 	struct work_struct	irq_work;
+	struct work_struct	recover_work;
 	struct delayed_work	deassert_reset_work;
 	struct delayed_work	finish_resume_work;
 	u16			hwvers;
@@ -337,6 +339,7 @@ struct musb {
 	dma_addr_t		async;
 	dma_addr_t		sync;
 	void __iomem		*sync_va;
+	u8			tusb_revision;
 #endif
 
 	/* passed down from chip/board specific irq handlers */
@@ -552,6 +555,12 @@ static inline void musb_platform_try_idle(struct musb *musb,
 		musb->ops->try_idle(musb, timeout);
 }
 
+static inline void musb_platform_reset(struct musb *musb)
+{
+	if (musb->ops->reset)
+		musb->ops->reset(musb);
+}
+
 static inline int musb_platform_get_vbus_status(struct musb *musb)
 {
 	if (!musb->ops->vbus_status)
diff --git a/drivers/usb/musb/musb_dsps.c b/drivers/usb/musb/musb_dsps.c
index e2fd263585d..51beb13c7e1 100644
--- a/drivers/usb/musb/musb_dsps.c
+++ b/drivers/usb/musb/musb_dsps.c
@@ -35,7 +35,7 @@
 #include <linux/dma-mapping.h>
 #include <linux/pm_runtime.h>
 #include <linux/module.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 #include <linux/platform_data/usb-omap.h>
 #include <linux/sizes.h>
 
@@ -329,9 +329,21 @@ static irqreturn_t dsps_interrupt(int irq, void *hci)
 	 * value but DEVCTL.BDEVICE is invalid without DEVCTL.SESSION set.
 	 * Also, DRVVBUS pulses for SRP (but not at 5V) ...
 	 */
-	if (is_host_active(musb) && usbintr & MUSB_INTR_BABBLE)
+	if (is_host_active(musb) && usbintr & MUSB_INTR_BABBLE) {
 		pr_info("CAUTION: musb: Babble Interrupt Occurred\n");
 
+		/*
+		 * When a babble condition occurs, the musb controller removes
+		 * the session and is no longer in host mode. Hence, all
+		 * devices connected to its root hub get disconnected.
+		 *
+		 * Hand this error down to the musb core isr, so it can
+		 * recover.
+		 */
+		musb->int_usb = MUSB_INTR_BABBLE | MUSB_INTR_DISCONNECT;
+		musb->int_tx = musb->int_rx = 0;
+	}
+
 	if (usbintr & ((1 << wrp->drvvbus) << wrp->usb_shift)) {
 		int drvvbus = dsps_readl(reg_base, wrp->status);
 		void __iomem *mregs = musb->mregs;
@@ -524,6 +536,16 @@ static int dsps_musb_set_mode(struct musb *musb, u8 mode)
 	return 0;
 }
 
+static void dsps_musb_reset(struct musb *musb)
+{
+	struct device *dev = musb->controller;
+	struct dsps_glue *glue = dev_get_drvdata(dev->parent);
+	const struct dsps_musb_wrapper *wrp = glue->wrp;
+
+	dsps_writel(musb->ctrl_base, wrp->control, (1 << wrp->reset));
+	udelay(100);
+}
+
 static struct musb_platform_ops dsps_ops = {
 	.init		= dsps_musb_init,
 	.exit		= dsps_musb_exit,
@@ -533,6 +555,7 @@ static struct musb_platform_ops dsps_ops = {
 
 	.try_idle	= dsps_musb_try_idle,
 	.set_mode	= dsps_musb_set_mode,
+	.reset		= dsps_musb_reset,
 };
 
 static u64 musb_dmamask = DMA_BIT_MASK(32);
@@ -750,7 +773,7 @@ static const struct of_device_id musb_dsps_of_match[] = {
 };
 MODULE_DEVICE_TABLE(of, musb_dsps_of_match);
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int dsps_suspend(struct device *dev)
 {
 	struct dsps_glue *glue = dev_get_drvdata(dev);
diff --git a/drivers/usb/musb/tusb6010.c b/drivers/usb/musb/tusb6010.c
index 4e9fb1d0869..159ef4be1ef 100644
--- a/drivers/usb/musb/tusb6010.c
+++ b/drivers/usb/musb/tusb6010.c
@@ -24,13 +24,14 @@
 #include <linux/io.h>
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 
 #include "musb_core.h"
 
 struct tusb6010_glue {
 	struct device		*dev;
 	struct platform_device	*musb;
+	struct platform_device	*phy;
 };
 
 static void tusb_musb_set_vbus(struct musb *musb, int is_on);
@@ -42,7 +43,7 @@ static void tusb_musb_set_vbus(struct musb *musb, int is_on);
  * Checks the revision. We need to use the DMA register as 3.0 does not
  * have correct versions for TUSB_PRCM_REV or TUSB_INT_CTRL_REV.
  */
-u8 tusb_get_revision(struct musb *musb)
+static u8 tusb_get_revision(struct musb *musb)
 {
 	void __iomem	*tbase = musb->ctrl_base;
 	u32		die_id;
@@ -58,14 +59,13 @@ u8 tusb_get_revision(struct musb *musb)
 
 	return rev;
 }
-EXPORT_SYMBOL_GPL(tusb_get_revision);
 
-static int tusb_print_revision(struct musb *musb)
+static void tusb_print_revision(struct musb *musb)
 {
 	void __iomem	*tbase = musb->ctrl_base;
 	u8		rev;
 
-	rev = tusb_get_revision(musb);
+	rev = musb->tusb_revision;
 
 	pr_info("tusb: %s%i.%i %s%i.%i %s%i.%i %s%i.%i %s%i %s%i.%i\n",
 		"prcm",
@@ -84,8 +84,6 @@ static int tusb_print_revision(struct musb *musb)
 		TUSB_DIDR1_HI_CHIP_REV(musb_readl(tbase, TUSB_DIDR1_HI)),
 		"rev",
 		TUSB_REV_MAJOR(rev), TUSB_REV_MINOR(rev));
-
-	return tusb_get_revision(musb);
 }
 
 #define WBUS_QUIRK_MASK	(TUSB_PHY_OTG_CTRL_TESTM2 | TUSB_PHY_OTG_CTRL_TESTM1 \
@@ -349,7 +347,7 @@ static void tusb_allow_idle(struct musb *musb, u32 wakeup_enables)
 	u32		reg;
 
 	if ((wakeup_enables & TUSB_PRCM_WBUS)
-			&& (tusb_get_revision(musb) == TUSB_REV_30))
+			&& (musb->tusb_revision == TUSB_REV_30))
 		tusb_wbus_quirk(musb, 1);
 
 	tusb_set_clock_source(musb, 0);
@@ -797,7 +795,7 @@ static irqreturn_t tusb_musb_interrupt(int irq, void *__hci)
 		u32	reg;
 		u32	i;
 
-		if (tusb_get_revision(musb) == TUSB_REV_30)
+		if (musb->tusb_revision == TUSB_REV_30)
 			tusb_wbus_quirk(musb, 0);
 
 		/* there are issues re-locking the PLL on wakeup ... */
@@ -1011,10 +1009,11 @@ static int tusb_musb_start(struct musb *musb)
 		goto err;
 	}
 
-	ret = tusb_print_revision(musb);
-	if (ret < 2) {
+	musb->tusb_revision = tusb_get_revision(musb);
+	tusb_print_revision(musb);
+	if (musb->tusb_revision < 2) {
 		printk(KERN_ERR "tusb: Unsupported TUSB6010 revision %i\n",
-				ret);
+				musb->tusb_revision);
 		goto err;
 	}
 
@@ -1065,7 +1064,6 @@ static int tusb_musb_init(struct musb *musb)
 	void __iomem		*sync = NULL;
 	int			ret;
 
-	usb_nop_xceiv_register();
 	musb->xceiv = usb_get_phy(USB_PHY_TYPE_USB2);
 	if (IS_ERR_OR_NULL(musb->xceiv))
 		return -EPROBE_DEFER;
@@ -1117,7 +1115,6 @@ done:
 			iounmap(sync);
 
 		usb_put_phy(musb->xceiv);
-		usb_nop_xceiv_unregister();
 	}
 	return ret;
 }
@@ -1133,7 +1130,6 @@ static int tusb_musb_exit(struct musb *musb)
 	iounmap(musb->sync_va);
 
 	usb_put_phy(musb->xceiv);
-	usb_nop_xceiv_unregister();
 	return 0;
 }
 
@@ -1176,6 +1172,7 @@ static int tusb_probe(struct platform_device *pdev)
 
 	pdata->platform_ops		= &tusb_ops;
 
+	usb_phy_generic_register();
 	platform_set_drvdata(pdev, glue);
 
 	memset(musb_resources, 0x00, sizeof(*musb_resources) *
@@ -1224,6 +1221,7 @@ static int tusb_remove(struct platform_device *pdev)
 	struct tusb6010_glue		*glue = platform_get_drvdata(pdev);
 
 	platform_device_unregister(glue->musb);
+	usb_phy_generic_unregister(glue->phy);
 	kfree(glue);
 
 	return 0;
diff --git a/drivers/usb/musb/tusb6010.h b/drivers/usb/musb/tusb6010.h
index 35c933a5d99..aec86c86ce3 100644
--- a/drivers/usb/musb/tusb6010.h
+++ b/drivers/usb/musb/tusb6010.h
@@ -12,14 +12,6 @@
 #ifndef __TUSB6010_H__
 #define __TUSB6010_H__
 
-extern u8 tusb_get_revision(struct musb *musb);
-
-#ifdef CONFIG_USB_TUSB6010
-#define musb_in_tusb()			1
-#else
-#define musb_in_tusb()			0
-#endif
-
 #ifdef CONFIG_USB_TUSB_OMAP_DMA
 #define tusb_dma_omap()			1
 #else
diff --git a/drivers/usb/musb/tusb6010_omap.c b/drivers/usb/musb/tusb6010_omap.c
index e33b6b2c44c..3ce152c0408 100644
--- a/drivers/usb/musb/tusb6010_omap.c
+++ b/drivers/usb/musb/tusb6010_omap.c
@@ -677,7 +677,7 @@ struct dma_controller *dma_controller_create(struct musb *musb, void __iomem *ba
 	tusb_dma->controller.channel_program = tusb_omap_dma_program;
 	tusb_dma->controller.channel_abort = tusb_omap_dma_abort;
 
-	if (tusb_get_revision(musb) >= TUSB_REV_30)
+	if (musb->tusb_revision >= TUSB_REV_30)
 		tusb_dma->multichannel = 1;
 
 	for (i = 0; i < MAX_DMAREQ; i++) {
diff --git a/drivers/usb/phy/Kconfig b/drivers/usb/phy/Kconfig
index 416e0c8cf6f..e253fa05be6 100644
--- a/drivers/usb/phy/Kconfig
+++ b/drivers/usb/phy/Kconfig
@@ -6,15 +6,6 @@ menu "USB Physical Layer drivers"
 config USB_PHY
 	def_bool n
 
-config USB_OTG_FSM
-	tristate "USB 2.0 OTG FSM implementation"
-	depends on USB
-	select USB_OTG
-	select USB_PHY
-	help
-	  Implements OTG Final State Machine as specified in On-The-Go
-	  and Embedded Host Supplement to the USB Revision 2.0 Specification.
-
 #
 # USB Transceiver Drivers
 #
@@ -59,14 +50,6 @@ config KEYSTONE_USB_PHY
 	  interface to interact with USB 2.0 and USB 3.0 PHY that is part
 	  of the Keystone SOC.
 
-config MV_U3D_PHY
-	bool "Marvell USB 3.0 PHY controller Driver"
-	depends on CPU_MMP3
-	select USB_PHY
-	help
-	  Enable this to support Marvell USB 3.0 phy controller for Marvell
-	  SoC.
-
 config NOP_USB_XCEIV
 	tristate "NOP USB Transceiver Driver"
 	select USB_PHY
@@ -171,11 +154,12 @@ config USB_ISP1301
 	  module will be called phy-isp1301.
 
 config USB_MSM_OTG
-	tristate "OTG support for Qualcomm on-chip USB controller"
-	depends on (USB || USB_GADGET) && ARCH_MSM
+	tristate "Qualcomm on-chip USB OTG controller support"
+	depends on (USB || USB_GADGET) && (ARCH_MSM || ARCH_QCOM || COMPILE_TEST)
+	depends on RESET_CONTROLLER
 	select USB_PHY
 	help
-	  Enable this to support the USB OTG transceiver on MSM chips. It
+	  Enable this to support the USB OTG transceiver on Qualcomm chips. It
 	  handles PHY initialization, clock management, and workarounds
 	  required after resetting the hardware and power management.
 	  This driver is required even for peripheral only or host only
@@ -208,6 +192,7 @@ config USB_MXS_PHY
 config USB_RCAR_PHY
 	tristate "Renesas R-Car USB PHY support"
 	depends on USB || USB_GADGET
+	depends on ARCH_R8A7778 || ARCH_R8A7779 || COMPILE_TEST
 	select USB_PHY
 	help
 	  Say Y here to add support for the Renesas R-Car USB common PHY driver.
@@ -232,7 +217,7 @@ config USB_RCAR_GEN2_PHY
 
 config USB_ULPI
 	bool "Generic ULPI Transceiver Driver"
-	depends on ARM
+	depends on ARM || ARM64
 	help
 	  Enable this to support ULPI connected USB OTG transceivers which
 	  are likely found on embedded boards.
diff --git a/drivers/usb/phy/Makefile b/drivers/usb/phy/Makefile
index f8fa719a31b..24a91332d4a 100644
--- a/drivers/usb/phy/Makefile
+++ b/drivers/usb/phy/Makefile
@@ -3,14 +3,12 @@
 #
 obj-$(CONFIG_USB_PHY)			+= phy.o
 obj-$(CONFIG_OF)			+= of.o
-obj-$(CONFIG_USB_OTG_FSM)		+= phy-fsm-usb.o
 
 # transceiver drivers, keep the list sorted
 
 obj-$(CONFIG_AB8500_USB)		+= phy-ab8500-usb.o
 obj-$(CONFIG_FSL_USB2_OTG)		+= phy-fsl-usb.o
 obj-$(CONFIG_ISP1301_OMAP)		+= phy-isp1301-omap.o
-obj-$(CONFIG_MV_U3D_PHY)		+= phy-mv-u3d-usb.o
 obj-$(CONFIG_NOP_USB_XCEIV)		+= phy-generic.o
 obj-$(CONFIG_TAHVO_USB)			+= phy-tahvo.o
 obj-$(CONFIG_AM335X_CONTROL_USB)	+= phy-am335x-control.o
diff --git a/drivers/usb/phy/phy-am335x.c b/drivers/usb/phy/phy-am335x.c
index 12fc3468a01..585e50cb198 100644
--- a/drivers/usb/phy/phy-am335x.c
+++ b/drivers/usb/phy/phy-am335x.c
@@ -2,7 +2,7 @@
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
 #include <linux/usb/otg.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 #include <linux/slab.h>
 #include <linux/clk.h>
 #include <linux/regulator/consumer.h>
@@ -13,7 +13,7 @@
 #include "phy-generic.h"
 
 struct am335x_phy {
-	struct usb_phy_gen_xceiv usb_phy_gen;
+	struct usb_phy_generic usb_phy_gen;
 	struct phy_control *phy_ctrl;
 	int id;
 };
diff --git a/drivers/usb/phy/phy-generic.c b/drivers/usb/phy/phy-generic.c
index bb394980532..7594e5069ae 100644
--- a/drivers/usb/phy/phy-generic.c
+++ b/drivers/usb/phy/phy-generic.c
@@ -30,7 +30,7 @@
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
 #include <linux/usb/otg.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 #include <linux/slab.h>
 #include <linux/clk.h>
 #include <linux/regulator/consumer.h>
@@ -41,34 +41,25 @@
 
 #include "phy-generic.h"
 
-static struct platform_device *pd;
-
-void usb_nop_xceiv_register(void)
+struct platform_device *usb_phy_generic_register(void)
 {
-	if (pd)
-		return;
-	pd = platform_device_register_simple("usb_phy_gen_xceiv", -1, NULL, 0);
-	if (IS_ERR(pd)) {
-		pr_err("Unable to register generic usb transceiver\n");
-		pd = NULL;
-		return;
-	}
+	return platform_device_register_simple("usb_phy_generic",
+			PLATFORM_DEVID_AUTO, NULL, 0);
 }
-EXPORT_SYMBOL(usb_nop_xceiv_register);
+EXPORT_SYMBOL_GPL(usb_phy_generic_register);
 
-void usb_nop_xceiv_unregister(void)
+void usb_phy_generic_unregister(struct platform_device *pdev)
 {
-	platform_device_unregister(pd);
-	pd = NULL;
+	platform_device_unregister(pdev);
 }
-EXPORT_SYMBOL(usb_nop_xceiv_unregister);
+EXPORT_SYMBOL_GPL(usb_phy_generic_unregister);
 
 static int nop_set_suspend(struct usb_phy *x, int suspend)
 {
 	return 0;
 }
 
-static void nop_reset_set(struct usb_phy_gen_xceiv *nop, int asserted)
+static void nop_reset_set(struct usb_phy_generic *nop, int asserted)
 {
 	int value;
 
@@ -87,7 +78,7 @@ static void nop_reset_set(struct usb_phy_gen_xceiv *nop, int asserted)
 
 int usb_gen_phy_init(struct usb_phy *phy)
 {
-	struct usb_phy_gen_xceiv *nop = dev_get_drvdata(phy->dev);
+	struct usb_phy_generic *nop = dev_get_drvdata(phy->dev);
 
 	if (!IS_ERR(nop->vcc)) {
 		if (regulator_enable(nop->vcc))
@@ -106,7 +97,7 @@ EXPORT_SYMBOL_GPL(usb_gen_phy_init);
 
 void usb_gen_phy_shutdown(struct usb_phy *phy)
 {
-	struct usb_phy_gen_xceiv *nop = dev_get_drvdata(phy->dev);
+	struct usb_phy_generic *nop = dev_get_drvdata(phy->dev);
 
 	/* Assert RESET */
 	nop_reset_set(nop, 1);
@@ -150,8 +141,8 @@ static int nop_set_host(struct usb_otg *otg, struct usb_bus *host)
 	return 0;
 }
 
-int usb_phy_gen_create_phy(struct device *dev, struct usb_phy_gen_xceiv *nop,
-		struct usb_phy_gen_xceiv_platform_data *pdata)
+int usb_phy_gen_create_phy(struct device *dev, struct usb_phy_generic *nop,
+		struct usb_phy_generic_platform_data *pdata)
 {
 	enum usb_phy_type type = USB_PHY_TYPE_USB2;
 	int err;
@@ -245,10 +236,10 @@ int usb_phy_gen_create_phy(struct device *dev, struct usb_phy_gen_xceiv *nop,
 }
 EXPORT_SYMBOL_GPL(usb_phy_gen_create_phy);
 
-static int usb_phy_gen_xceiv_probe(struct platform_device *pdev)
+static int usb_phy_generic_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
-	struct usb_phy_gen_xceiv	*nop;
+	struct usb_phy_generic	*nop;
 	int err;
 
 	nop = devm_kzalloc(dev, sizeof(*nop), GFP_KERNEL);
@@ -274,9 +265,9 @@ static int usb_phy_gen_xceiv_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static int usb_phy_gen_xceiv_remove(struct platform_device *pdev)
+static int usb_phy_generic_remove(struct platform_device *pdev)
 {
-	struct usb_phy_gen_xceiv *nop = platform_get_drvdata(pdev);
+	struct usb_phy_generic *nop = platform_get_drvdata(pdev);
 
 	usb_remove_phy(&nop->phy);
 
@@ -290,29 +281,29 @@ static const struct of_device_id nop_xceiv_dt_ids[] = {
 
 MODULE_DEVICE_TABLE(of, nop_xceiv_dt_ids);
 
-static struct platform_driver usb_phy_gen_xceiv_driver = {
-	.probe		= usb_phy_gen_xceiv_probe,
-	.remove		= usb_phy_gen_xceiv_remove,
+static struct platform_driver usb_phy_generic_driver = {
+	.probe		= usb_phy_generic_probe,
+	.remove		= usb_phy_generic_remove,
 	.driver		= {
-		.name	= "usb_phy_gen_xceiv",
+		.name	= "usb_phy_generic",
 		.owner	= THIS_MODULE,
 		.of_match_table = nop_xceiv_dt_ids,
 	},
 };
 
-static int __init usb_phy_gen_xceiv_init(void)
+static int __init usb_phy_generic_init(void)
 {
-	return platform_driver_register(&usb_phy_gen_xceiv_driver);
+	return platform_driver_register(&usb_phy_generic_driver);
 }
-subsys_initcall(usb_phy_gen_xceiv_init);
+subsys_initcall(usb_phy_generic_init);
 
-static void __exit usb_phy_gen_xceiv_exit(void)
+static void __exit usb_phy_generic_exit(void)
 {
-	platform_driver_unregister(&usb_phy_gen_xceiv_driver);
+	platform_driver_unregister(&usb_phy_generic_driver);
 }
-module_exit(usb_phy_gen_xceiv_exit);
+module_exit(usb_phy_generic_exit);
 
-MODULE_ALIAS("platform:usb_phy_gen_xceiv");
+MODULE_ALIAS("platform:usb_phy_generic");
 MODULE_AUTHOR("Texas Instruments Inc");
 MODULE_DESCRIPTION("NOP USB Transceiver driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/usb/phy/phy-generic.h b/drivers/usb/phy/phy-generic.h
index 38a81f307b8..d8feacc0b7f 100644
--- a/drivers/usb/phy/phy-generic.h
+++ b/drivers/usb/phy/phy-generic.h
@@ -1,9 +1,9 @@
 #ifndef _PHY_GENERIC_H_
 #define _PHY_GENERIC_H_
 
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 
-struct usb_phy_gen_xceiv {
+struct usb_phy_generic {
 	struct usb_phy phy;
 	struct device *dev;
 	struct clk *clk;
@@ -15,7 +15,7 @@ struct usb_phy_gen_xceiv {
 int usb_gen_phy_init(struct usb_phy *phy);
 void usb_gen_phy_shutdown(struct usb_phy *phy);
 
-int usb_phy_gen_create_phy(struct device *dev, struct usb_phy_gen_xceiv *nop,
-		struct usb_phy_gen_xceiv_platform_data *pdata);
+int usb_phy_gen_create_phy(struct device *dev, struct usb_phy_generic *nop,
+		struct usb_phy_generic_platform_data *pdata);
 
 #endif
diff --git a/drivers/usb/phy/phy-isp1301-omap.c b/drivers/usb/phy/phy-isp1301-omap.c
index 6e146d723b3..69e49be8866 100644
--- a/drivers/usb/phy/phy-isp1301-omap.c
+++ b/drivers/usb/phy/phy-isp1301-omap.c
@@ -1295,7 +1295,7 @@ isp1301_set_host(struct usb_otg *otg, struct usb_bus *host)
 		return isp1301_otg_enable(isp);
 	return 0;
 
-#elif	!defined(CONFIG_USB_GADGET_OMAP)
+#elif !IS_ENABLED(CONFIG_USB_OMAP)
 	// FIXME update its refcount
 	otg->host = host;
 
diff --git a/drivers/usb/phy/phy-keystone.c b/drivers/usb/phy/phy-keystone.c
index d762003896c..f4d722de912 100644
--- a/drivers/usb/phy/phy-keystone.c
+++ b/drivers/usb/phy/phy-keystone.c
@@ -18,7 +18,7 @@
 
 #include <linux/module.h>
 #include <linux/platform_device.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 #include <linux/io.h>
 #include <linux/of.h>
 
@@ -35,7 +35,7 @@
 #define PHY_REF_SSP_EN			BIT(29)
 
 struct keystone_usbphy {
-	struct usb_phy_gen_xceiv	usb_phy_gen;
+	struct usb_phy_generic	usb_phy_gen;
 	void __iomem			*phy_ctrl;
 };
 
diff --git a/drivers/usb/phy/phy-msm-usb.c b/drivers/usb/phy/phy-msm-usb.c
index 5b37b81f2ef..ced34f39bdd 100644
--- a/drivers/usb/phy/phy-msm-usb.c
+++ b/drivers/usb/phy/phy-msm-usb.c
@@ -30,9 +30,13 @@
 #include <linux/debugfs.h>
 #include <linux/seq_file.h>
 #include <linux/pm_runtime.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/reset.h>
 
 #include <linux/usb.h>
 #include <linux/usb/otg.h>
+#include <linux/usb/of.h>
 #include <linux/usb/ulpi.h>
 #include <linux/usb/gadget.h>
 #include <linux/usb/hcd.h>
@@ -44,6 +48,7 @@
 #define DRIVER_NAME	"msm_otg"
 
 #define ULPI_IO_TIMEOUT_USEC	(10 * 1000)
+#define LINK_RESET_TIMEOUT_USEC	(250 * 1000)
 
 #define USB_PHY_3P3_VOL_MIN	3050000 /* uV */
 #define USB_PHY_3P3_VOL_MAX	3300000 /* uV */
@@ -57,48 +62,38 @@
 
 #define USB_PHY_VDD_DIG_VOL_MIN	1000000 /* uV */
 #define USB_PHY_VDD_DIG_VOL_MAX	1320000 /* uV */
+#define USB_PHY_SUSP_DIG_VOL	500000  /* uV */
 
-static struct regulator *hsusb_3p3;
-static struct regulator *hsusb_1p8;
-static struct regulator *hsusb_vddcx;
+enum vdd_levels {
+	VDD_LEVEL_NONE = 0,
+	VDD_LEVEL_MIN,
+	VDD_LEVEL_MAX,
+};
 
 static int msm_hsusb_init_vddcx(struct msm_otg *motg, int init)
 {
 	int ret = 0;
 
 	if (init) {
-		hsusb_vddcx = regulator_get(motg->phy.dev, "HSUSB_VDDCX");
-		if (IS_ERR(hsusb_vddcx)) {
-			dev_err(motg->phy.dev, "unable to get hsusb vddcx\n");
-			return PTR_ERR(hsusb_vddcx);
-		}
-
-		ret = regulator_set_voltage(hsusb_vddcx,
-				USB_PHY_VDD_DIG_VOL_MIN,
-				USB_PHY_VDD_DIG_VOL_MAX);
+		ret = regulator_set_voltage(motg->vddcx,
+				motg->vdd_levels[VDD_LEVEL_MIN],
+				motg->vdd_levels[VDD_LEVEL_MAX]);
 		if (ret) {
-			dev_err(motg->phy.dev, "unable to set the voltage "
-					"for hsusb vddcx\n");
-			regulator_put(hsusb_vddcx);
+			dev_err(motg->phy.dev, "Cannot set vddcx voltage\n");
 			return ret;
 		}
 
-		ret = regulator_enable(hsusb_vddcx);
-		if (ret) {
+		ret = regulator_enable(motg->vddcx);
+		if (ret)
 			dev_err(motg->phy.dev, "unable to enable hsusb vddcx\n");
-			regulator_put(hsusb_vddcx);
-		}
 	} else {
-		ret = regulator_set_voltage(hsusb_vddcx, 0,
-			USB_PHY_VDD_DIG_VOL_MAX);
+		ret = regulator_set_voltage(motg->vddcx, 0,
+				motg->vdd_levels[VDD_LEVEL_MAX]);
 		if (ret)
-			dev_err(motg->phy.dev, "unable to set the voltage "
-					"for hsusb vddcx\n");
-		ret = regulator_disable(hsusb_vddcx);
+			dev_err(motg->phy.dev, "Cannot set vddcx voltage\n");
+		ret = regulator_disable(motg->vddcx);
 		if (ret)
 			dev_err(motg->phy.dev, "unable to disable hsusb vddcx\n");
-
-		regulator_put(hsusb_vddcx);
 	}
 
 	return ret;
@@ -109,98 +104,67 @@ static int msm_hsusb_ldo_init(struct msm_otg *motg, int init)
 	int rc = 0;
 
 	if (init) {
-		hsusb_3p3 = regulator_get(motg->phy.dev, "HSUSB_3p3");
-		if (IS_ERR(hsusb_3p3)) {
-			dev_err(motg->phy.dev, "unable to get hsusb 3p3\n");
-			return PTR_ERR(hsusb_3p3);
-		}
-
-		rc = regulator_set_voltage(hsusb_3p3, USB_PHY_3P3_VOL_MIN,
+		rc = regulator_set_voltage(motg->v3p3, USB_PHY_3P3_VOL_MIN,
 				USB_PHY_3P3_VOL_MAX);
 		if (rc) {
-			dev_err(motg->phy.dev, "unable to set voltage level "
-					"for hsusb 3p3\n");
-			goto put_3p3;
+			dev_err(motg->phy.dev, "Cannot set v3p3 voltage\n");
+			goto exit;
 		}
-		rc = regulator_enable(hsusb_3p3);
+		rc = regulator_enable(motg->v3p3);
 		if (rc) {
 			dev_err(motg->phy.dev, "unable to enable the hsusb 3p3\n");
-			goto put_3p3;
+			goto exit;
 		}
-		hsusb_1p8 = regulator_get(motg->phy.dev, "HSUSB_1p8");
-		if (IS_ERR(hsusb_1p8)) {
-			dev_err(motg->phy.dev, "unable to get hsusb 1p8\n");
-			rc = PTR_ERR(hsusb_1p8);
-			goto disable_3p3;
-		}
-		rc = regulator_set_voltage(hsusb_1p8, USB_PHY_1P8_VOL_MIN,
+		rc = regulator_set_voltage(motg->v1p8, USB_PHY_1P8_VOL_MIN,
 				USB_PHY_1P8_VOL_MAX);
 		if (rc) {
-			dev_err(motg->phy.dev, "unable to set voltage level "
-					"for hsusb 1p8\n");
-			goto put_1p8;
+			dev_err(motg->phy.dev, "Cannot set v1p8 voltage\n");
+			goto disable_3p3;
 		}
-		rc = regulator_enable(hsusb_1p8);
+		rc = regulator_enable(motg->v1p8);
 		if (rc) {
 			dev_err(motg->phy.dev, "unable to enable the hsusb 1p8\n");
-			goto put_1p8;
+			goto disable_3p3;
 		}
 
 		return 0;
 	}
 
-	regulator_disable(hsusb_1p8);
-put_1p8:
-	regulator_put(hsusb_1p8);
+	regulator_disable(motg->v1p8);
 disable_3p3:
-	regulator_disable(hsusb_3p3);
-put_3p3:
-	regulator_put(hsusb_3p3);
+	regulator_disable(motg->v3p3);
+exit:
 	return rc;
 }
 
-static int msm_hsusb_ldo_set_mode(int on)
+static int msm_hsusb_ldo_set_mode(struct msm_otg *motg, int on)
 {
 	int ret = 0;
 
-	if (!hsusb_1p8 || IS_ERR(hsusb_1p8)) {
-		pr_err("%s: HSUSB_1p8 is not initialized\n", __func__);
-		return -ENODEV;
-	}
-
-	if (!hsusb_3p3 || IS_ERR(hsusb_3p3)) {
-		pr_err("%s: HSUSB_3p3 is not initialized\n", __func__);
-		return -ENODEV;
-	}
-
 	if (on) {
-		ret = regulator_set_optimum_mode(hsusb_1p8,
+		ret = regulator_set_optimum_mode(motg->v1p8,
 				USB_PHY_1P8_HPM_LOAD);
 		if (ret < 0) {
-			pr_err("%s: Unable to set HPM of the regulator "
-				"HSUSB_1p8\n", __func__);
+			pr_err("Could not set HPM for v1p8\n");
 			return ret;
 		}
-		ret = regulator_set_optimum_mode(hsusb_3p3,
+		ret = regulator_set_optimum_mode(motg->v3p3,
 				USB_PHY_3P3_HPM_LOAD);
 		if (ret < 0) {
-			pr_err("%s: Unable to set HPM of the regulator "
-				"HSUSB_3p3\n", __func__);
-			regulator_set_optimum_mode(hsusb_1p8,
+			pr_err("Could not set HPM for v3p3\n");
+			regulator_set_optimum_mode(motg->v1p8,
 				USB_PHY_1P8_LPM_LOAD);
 			return ret;
 		}
 	} else {
-		ret = regulator_set_optimum_mode(hsusb_1p8,
+		ret = regulator_set_optimum_mode(motg->v1p8,
 				USB_PHY_1P8_LPM_LOAD);
 		if (ret < 0)
-			pr_err("%s: Unable to set LPM of the regulator "
-				"HSUSB_1p8\n", __func__);
-		ret = regulator_set_optimum_mode(hsusb_3p3,
+			pr_err("Could not set LPM for v1p8\n");
+		ret = regulator_set_optimum_mode(motg->v3p3,
 				USB_PHY_3P3_LPM_LOAD);
 		if (ret < 0)
-			pr_err("%s: Unable to set LPM of the regulator "
-				"HSUSB_3p3\n", __func__);
+			pr_err("Could not set LPM for v3p3\n");
 	}
 
 	pr_debug("reg (%s)\n", on ? "HPM" : "LPM");
@@ -265,27 +229,47 @@ static struct usb_phy_io_ops msm_otg_io_ops = {
 static void ulpi_init(struct msm_otg *motg)
 {
 	struct msm_otg_platform_data *pdata = motg->pdata;
-	int *seq = pdata->phy_init_seq;
+	int *seq = pdata->phy_init_seq, idx;
+	u32 addr = ULPI_EXT_VENDOR_SPECIFIC;
 
-	if (!seq)
-		return;
+	for (idx = 0; idx < pdata->phy_init_sz; idx++) {
+		if (seq[idx] == -1)
+			continue;
 
-	while (seq[0] >= 0) {
 		dev_vdbg(motg->phy.dev, "ulpi: write 0x%02x to 0x%02x\n",
-				seq[0], seq[1]);
-		ulpi_write(&motg->phy, seq[0], seq[1]);
-		seq += 2;
+				seq[idx], addr + idx);
+		ulpi_write(&motg->phy, seq[idx], addr + idx);
 	}
 }
 
+static int msm_phy_notify_disconnect(struct usb_phy *phy,
+				   enum usb_device_speed speed)
+{
+	int val;
+
+	/*
+	 * Put the transceiver in non-driving mode. Otherwise host
+	 * may not detect soft-disconnection.
+	 */
+	val = ulpi_read(phy, ULPI_FUNC_CTRL);
+	val &= ~ULPI_FUNC_CTRL_OPMODE_MASK;
+	val |= ULPI_FUNC_CTRL_OPMODE_NONDRIVING;
+	ulpi_write(phy, val, ULPI_FUNC_CTRL);
+
+	return 0;
+}
+
 static int msm_otg_link_clk_reset(struct msm_otg *motg, bool assert)
 {
-	int ret = 0;
+	int ret;
 
-	if (!motg->pdata->link_clk_reset)
-		return ret;
+	if (motg->pdata->link_clk_reset)
+		ret = motg->pdata->link_clk_reset(motg->clk, assert);
+	else if (assert)
+		ret = reset_control_assert(motg->link_rst);
+	else
+		ret = reset_control_deassert(motg->link_rst);
 
-	ret = motg->pdata->link_clk_reset(motg->clk, assert);
 	if (ret)
 		dev_err(motg->phy.dev, "usb link clk reset %s failed\n",
 			assert ? "assert" : "deassert");
@@ -295,111 +279,150 @@ static int msm_otg_link_clk_reset(struct msm_otg *motg, bool assert)
 
 static int msm_otg_phy_clk_reset(struct msm_otg *motg)
 {
-	int ret = 0;
+	int ret;
 
-	if (!motg->pdata->phy_clk_reset)
-		return ret;
+	if (motg->pdata->phy_clk_reset)
+		ret = motg->pdata->phy_clk_reset(motg->phy_reset_clk);
+	else
+		ret = reset_control_reset(motg->phy_rst);
 
-	ret = motg->pdata->phy_clk_reset(motg->phy_reset_clk);
 	if (ret)
 		dev_err(motg->phy.dev, "usb phy clk reset failed\n");
 
 	return ret;
 }
 
-static int msm_otg_phy_reset(struct msm_otg *motg)
+static int msm_link_reset(struct msm_otg *motg)
 {
 	u32 val;
 	int ret;
-	int retries;
 
 	ret = msm_otg_link_clk_reset(motg, 1);
 	if (ret)
 		return ret;
-	ret = msm_otg_phy_clk_reset(motg);
-	if (ret)
-		return ret;
+
+	/* wait for 1ms delay as suggested in HPG. */
+	usleep_range(1000, 1200);
+
 	ret = msm_otg_link_clk_reset(motg, 0);
 	if (ret)
 		return ret;
 
+	if (motg->phy_number)
+		writel(readl(USB_PHY_CTRL2) | BIT(16), USB_PHY_CTRL2);
+
+	/* put transceiver in serial mode as part of reset */
 	val = readl(USB_PORTSC) & ~PORTSC_PTS_MASK;
-	writel(val | PORTSC_PTS_ULPI, USB_PORTSC);
+	writel(val | PORTSC_PTS_SERIAL, USB_PORTSC);
 
-	for (retries = 3; retries > 0; retries--) {
-		ret = ulpi_write(&motg->phy, ULPI_FUNC_CTRL_SUSPENDM,
-				ULPI_CLR(ULPI_FUNC_CTRL));
-		if (!ret)
-			break;
-		ret = msm_otg_phy_clk_reset(motg);
-		if (ret)
-			return ret;
-	}
-	if (!retries)
-		return -ETIMEDOUT;
+	return 0;
+}
 
-	/* This reset calibrates the phy, if the above write succeeded */
-	ret = msm_otg_phy_clk_reset(motg);
-	if (ret)
-		return ret;
+static int msm_otg_reset(struct usb_phy *phy)
+{
+	struct msm_otg *motg = container_of(phy, struct msm_otg, phy);
+	int cnt = 0;
 
-	for (retries = 3; retries > 0; retries--) {
-		ret = ulpi_read(&motg->phy, ULPI_DEBUG);
-		if (ret != -ETIMEDOUT)
+	writel(USBCMD_RESET, USB_USBCMD);
+	while (cnt < LINK_RESET_TIMEOUT_USEC) {
+		if (!(readl(USB_USBCMD) & USBCMD_RESET))
 			break;
-		ret = msm_otg_phy_clk_reset(motg);
-		if (ret)
-			return ret;
+		udelay(1);
+		cnt++;
 	}
-	if (!retries)
+	if (cnt >= LINK_RESET_TIMEOUT_USEC)
 		return -ETIMEDOUT;
 
-	dev_info(motg->phy.dev, "phy_reset: success\n");
+	/* select ULPI phy and clear other status/control bits in PORTSC */
+	writel(PORTSC_PTS_ULPI, USB_PORTSC);
+
+	writel(0x0, USB_AHBBURST);
+	writel(0x08, USB_AHBMODE);
+
+	if (motg->phy_number)
+		writel(readl(USB_PHY_CTRL2) | BIT(16), USB_PHY_CTRL2);
 	return 0;
 }
 
-#define LINK_RESET_TIMEOUT_USEC		(250 * 1000)
-static int msm_otg_reset(struct usb_phy *phy)
+static void msm_phy_reset(struct msm_otg *motg)
+{
+	void __iomem *addr;
+
+	if (motg->pdata->phy_type != SNPS_28NM_INTEGRATED_PHY) {
+		msm_otg_phy_clk_reset(motg);
+		return;
+	}
+
+	addr = USB_PHY_CTRL;
+	if (motg->phy_number)
+		addr = USB_PHY_CTRL2;
+
+	/* Assert USB PHY_POR */
+	writel(readl(addr) | PHY_POR_ASSERT, addr);
+
+	/*
+	 * wait for minimum 10 microseconds as suggested in HPG.
+	 * Use a slightly larger value since the exact value didn't
+	 * work 100% of the time.
+	 */
+	udelay(12);
+
+	/* Deassert USB PHY_POR */
+	writel(readl(addr) & ~PHY_POR_ASSERT, addr);
+}
+
+static int msm_usb_reset(struct usb_phy *phy)
 {
 	struct msm_otg *motg = container_of(phy, struct msm_otg, phy);
-	struct msm_otg_platform_data *pdata = motg->pdata;
-	int cnt = 0;
 	int ret;
-	u32 val = 0;
-	u32 ulpi_val = 0;
 
-	ret = msm_otg_phy_reset(motg);
+	if (!IS_ERR(motg->core_clk))
+		clk_prepare_enable(motg->core_clk);
+
+	ret = msm_link_reset(motg);
 	if (ret) {
 		dev_err(phy->dev, "phy_reset failed\n");
 		return ret;
 	}
 
-	ulpi_init(motg);
-
-	writel(USBCMD_RESET, USB_USBCMD);
-	while (cnt < LINK_RESET_TIMEOUT_USEC) {
-		if (!(readl(USB_USBCMD) & USBCMD_RESET))
-			break;
-		udelay(1);
-		cnt++;
+	ret = msm_otg_reset(&motg->phy);
+	if (ret) {
+		dev_err(phy->dev, "link reset failed\n");
+		return ret;
 	}
-	if (cnt >= LINK_RESET_TIMEOUT_USEC)
-		return -ETIMEDOUT;
-
-	/* select ULPI phy */
-	writel(0x80000000, USB_PORTSC);
 
 	msleep(100);
 
-	writel(0x0, USB_AHBBURST);
-	writel(0x00, USB_AHBMODE);
+	/* Reset USB PHY after performing USB Link RESET */
+	msm_phy_reset(motg);
+
+	if (!IS_ERR(motg->core_clk))
+		clk_disable_unprepare(motg->core_clk);
+
+	return 0;
+}
+
+static int msm_phy_init(struct usb_phy *phy)
+{
+	struct msm_otg *motg = container_of(phy, struct msm_otg, phy);
+	struct msm_otg_platform_data *pdata = motg->pdata;
+	u32 val, ulpi_val = 0;
+
+	/* Program USB PHY Override registers. */
+	ulpi_init(motg);
+
+	/*
+	 * It is recommended in HPG to reset USB PHY after programming
+	 * USB PHY Override registers.
+	 */
+	msm_phy_reset(motg);
 
 	if (pdata->otg_control == OTG_PHY_CONTROL) {
 		val = readl(USB_OTGSC);
-		if (pdata->mode == USB_OTG) {
+		if (pdata->mode == USB_DR_MODE_OTG) {
 			ulpi_val = ULPI_INT_IDGRD | ULPI_INT_SESS_VALID;
 			val |= OTGSC_IDIE | OTGSC_BSVIE;
-		} else if (pdata->mode == USB_PERIPHERAL) {
+		} else if (pdata->mode == USB_DR_MODE_PERIPHERAL) {
 			ulpi_val = ULPI_INT_SESS_VALID;
 			val |= OTGSC_BSVIE;
 		}
@@ -408,6 +431,9 @@ static int msm_otg_reset(struct usb_phy *phy)
 		ulpi_write(phy, ulpi_val, ULPI_USB_INT_EN_FALL);
 	}
 
+	if (motg->phy_number)
+		writel(readl(USB_PHY_CTRL2) | BIT(16), USB_PHY_CTRL2);
+
 	return 0;
 }
 
@@ -416,22 +442,20 @@ static int msm_otg_reset(struct usb_phy *phy)
 
 #ifdef CONFIG_PM
 
-#define USB_PHY_SUSP_DIG_VOL  500000
-static int msm_hsusb_config_vddcx(int high)
+static int msm_hsusb_config_vddcx(struct msm_otg *motg, int high)
 {
-	int max_vol = USB_PHY_VDD_DIG_VOL_MAX;
+	int max_vol = motg->vdd_levels[VDD_LEVEL_MAX];
 	int min_vol;
 	int ret;
 
 	if (high)
-		min_vol = USB_PHY_VDD_DIG_VOL_MIN;
+		min_vol = motg->vdd_levels[VDD_LEVEL_MIN];
 	else
-		min_vol = USB_PHY_SUSP_DIG_VOL;
+		min_vol = motg->vdd_levels[VDD_LEVEL_NONE];
 
-	ret = regulator_set_voltage(hsusb_vddcx, min_vol, max_vol);
+	ret = regulator_set_voltage(motg->vddcx, min_vol, max_vol);
 	if (ret) {
-		pr_err("%s: unable to set the voltage for regulator "
-			"HSUSB_VDDCX\n", __func__);
+		pr_err("Cannot set vddcx voltage\n");
 		return ret;
 	}
 
@@ -445,6 +469,7 @@ static int msm_otg_suspend(struct msm_otg *motg)
 	struct usb_phy *phy = &motg->phy;
 	struct usb_bus *bus = phy->otg->host;
 	struct msm_otg_platform_data *pdata = motg->pdata;
+	void __iomem *addr;
 	int cnt = 0;
 
 	if (atomic_read(&motg->in_lpm))
@@ -504,22 +529,23 @@ static int msm_otg_suspend(struct msm_otg *motg)
 	 */
 	writel(readl(USB_USBCMD) | ASYNC_INTR_CTRL | ULPI_STP_CTRL, USB_USBCMD);
 
+	addr = USB_PHY_CTRL;
+	if (motg->phy_number)
+		addr = USB_PHY_CTRL2;
+
 	if (motg->pdata->phy_type == SNPS_28NM_INTEGRATED_PHY &&
 			motg->pdata->otg_control == OTG_PMIC_CONTROL)
-		writel(readl(USB_PHY_CTRL) | PHY_RETEN, USB_PHY_CTRL);
+		writel(readl(addr) | PHY_RETEN, addr);
 
 	clk_disable_unprepare(motg->pclk);
 	clk_disable_unprepare(motg->clk);
-	if (motg->core_clk)
+	if (!IS_ERR(motg->core_clk))
 		clk_disable_unprepare(motg->core_clk);
 
-	if (!IS_ERR(motg->pclk_src))
-		clk_disable_unprepare(motg->pclk_src);
-
 	if (motg->pdata->phy_type == SNPS_28NM_INTEGRATED_PHY &&
 			motg->pdata->otg_control == OTG_PMIC_CONTROL) {
-		msm_hsusb_ldo_set_mode(0);
-		msm_hsusb_config_vddcx(0);
+		msm_hsusb_ldo_set_mode(motg, 0);
+		msm_hsusb_config_vddcx(motg, 0);
 	}
 
 	if (device_may_wakeup(phy->dev))
@@ -539,25 +565,28 @@ static int msm_otg_resume(struct msm_otg *motg)
 {
 	struct usb_phy *phy = &motg->phy;
 	struct usb_bus *bus = phy->otg->host;
+	void __iomem *addr;
 	int cnt = 0;
 	unsigned temp;
 
 	if (!atomic_read(&motg->in_lpm))
 		return 0;
 
-	if (!IS_ERR(motg->pclk_src))
-		clk_prepare_enable(motg->pclk_src);
-
 	clk_prepare_enable(motg->pclk);
 	clk_prepare_enable(motg->clk);
-	if (motg->core_clk)
+	if (!IS_ERR(motg->core_clk))
 		clk_prepare_enable(motg->core_clk);
 
 	if (motg->pdata->phy_type == SNPS_28NM_INTEGRATED_PHY &&
 			motg->pdata->otg_control == OTG_PMIC_CONTROL) {
-		msm_hsusb_ldo_set_mode(1);
-		msm_hsusb_config_vddcx(1);
-		writel(readl(USB_PHY_CTRL) & ~PHY_RETEN, USB_PHY_CTRL);
+
+		addr = USB_PHY_CTRL;
+		if (motg->phy_number)
+			addr = USB_PHY_CTRL2;
+
+		msm_hsusb_ldo_set_mode(motg, 1);
+		msm_hsusb_config_vddcx(motg, 1);
+		writel(readl(addr) & ~PHY_RETEN, addr);
 	}
 
 	temp = readl(USB_USBCMD);
@@ -586,8 +615,7 @@ static int msm_otg_resume(struct msm_otg *motg)
 		 * PHY. USB state can not be restored. Re-insertion
 		 * of USB cable is the only way to get USB working.
 		 */
-		dev_err(phy->dev, "Unable to resume USB."
-				"Re-plugin the cable\n");
+		dev_err(phy->dev, "Unable to resume USB. Re-plugin the cable\n");
 		msm_otg_reset(phy);
 	}
 
@@ -687,7 +715,7 @@ static int msm_otg_set_host(struct usb_otg *otg, struct usb_bus *host)
 	 * Fail host registration if this board can support
 	 * only peripheral configuration.
 	 */
-	if (motg->pdata->mode == USB_PERIPHERAL) {
+	if (motg->pdata->mode == USB_DR_MODE_PERIPHERAL) {
 		dev_info(otg->phy->dev, "Host mode is not supported\n");
 		return -ENODEV;
 	}
@@ -716,7 +744,7 @@ static int msm_otg_set_host(struct usb_otg *otg, struct usb_bus *host)
 	 * Kick the state machine work, if peripheral is not supported
 	 * or peripheral is already registered with us.
 	 */
-	if (motg->pdata->mode == USB_HOST || otg->gadget) {
+	if (motg->pdata->mode == USB_DR_MODE_HOST || otg->gadget) {
 		pm_runtime_get_sync(otg->phy->dev);
 		schedule_work(&motg->sm_work);
 	}
@@ -760,7 +788,7 @@ static int msm_otg_set_peripheral(struct usb_otg *otg,
 	 * Fail peripheral registration if this board can support
 	 * only host configuration.
 	 */
-	if (motg->pdata->mode == USB_HOST) {
+	if (motg->pdata->mode == USB_DR_MODE_HOST) {
 		dev_info(otg->phy->dev, "Peripheral mode is not supported\n");
 		return -ENODEV;
 	}
@@ -785,7 +813,7 @@ static int msm_otg_set_peripheral(struct usb_otg *otg,
 	 * Kick the state machine work, if host is not supported
 	 * or host is already registered with us.
 	 */
-	if (motg->pdata->mode == USB_PERIPHERAL || otg->host) {
+	if (motg->pdata->mode == USB_DR_MODE_PERIPHERAL || otg->host) {
 		pm_runtime_get_sync(otg->phy->dev);
 		schedule_work(&motg->sm_work);
 	}
@@ -1106,7 +1134,7 @@ static void msm_otg_init_sm(struct msm_otg *motg)
 	u32 otgsc = readl(USB_OTGSC);
 
 	switch (pdata->mode) {
-	case USB_OTG:
+	case USB_DR_MODE_OTG:
 		if (pdata->otg_control == OTG_PHY_CONTROL) {
 			if (otgsc & OTGSC_ID)
 				set_bit(ID, &motg->inputs);
@@ -1118,21 +1146,14 @@ static void msm_otg_init_sm(struct msm_otg *motg)
 			else
 				clear_bit(B_SESS_VLD, &motg->inputs);
 		} else if (pdata->otg_control == OTG_USER_CONTROL) {
-			if (pdata->default_mode == USB_HOST) {
-				clear_bit(ID, &motg->inputs);
-			} else if (pdata->default_mode == USB_PERIPHERAL) {
-				set_bit(ID, &motg->inputs);
-				set_bit(B_SESS_VLD, &motg->inputs);
-			} else {
 				set_bit(ID, &motg->inputs);
 				clear_bit(B_SESS_VLD, &motg->inputs);
-			}
 		}
 		break;
-	case USB_HOST:
+	case USB_DR_MODE_HOST:
 		clear_bit(ID, &motg->inputs);
 		break;
-	case USB_PERIPHERAL:
+	case USB_DR_MODE_PERIPHERAL:
 		set_bit(ID, &motg->inputs);
 		if (otgsc & OTGSC_BSV)
 			set_bit(B_SESS_VLD, &motg->inputs);
@@ -1282,13 +1303,13 @@ static int msm_otg_mode_show(struct seq_file *s, void *unused)
 
 	switch (otg->phy->state) {
 	case OTG_STATE_A_HOST:
-		seq_printf(s, "host\n");
+		seq_puts(s, "host\n");
 		break;
 	case OTG_STATE_B_PERIPHERAL:
-		seq_printf(s, "peripheral\n");
+		seq_puts(s, "peripheral\n");
 		break;
 	default:
-		seq_printf(s, "none\n");
+		seq_puts(s, "none\n");
 		break;
 	}
 
@@ -1308,7 +1329,7 @@ static ssize_t msm_otg_mode_write(struct file *file, const char __user *ubuf,
 	char buf[16];
 	struct usb_otg *otg = motg->phy.otg;
 	int status = count;
-	enum usb_mode_type req_mode;
+	enum usb_dr_mode req_mode;
 
 	memset(buf, 0x00, sizeof(buf));
 
@@ -1318,18 +1339,18 @@ static ssize_t msm_otg_mode_write(struct file *file, const char __user *ubuf,
 	}
 
 	if (!strncmp(buf, "host", 4)) {
-		req_mode = USB_HOST;
+		req_mode = USB_DR_MODE_HOST;
 	} else if (!strncmp(buf, "peripheral", 10)) {
-		req_mode = USB_PERIPHERAL;
+		req_mode = USB_DR_MODE_PERIPHERAL;
 	} else if (!strncmp(buf, "none", 4)) {
-		req_mode = USB_NONE;
+		req_mode = USB_DR_MODE_UNKNOWN;
 	} else {
 		status = -EINVAL;
 		goto out;
 	}
 
 	switch (req_mode) {
-	case USB_NONE:
+	case USB_DR_MODE_UNKNOWN:
 		switch (otg->phy->state) {
 		case OTG_STATE_A_HOST:
 		case OTG_STATE_B_PERIPHERAL:
@@ -1340,7 +1361,7 @@ static ssize_t msm_otg_mode_write(struct file *file, const char __user *ubuf,
 			goto out;
 		}
 		break;
-	case USB_PERIPHERAL:
+	case USB_DR_MODE_PERIPHERAL:
 		switch (otg->phy->state) {
 		case OTG_STATE_B_IDLE:
 		case OTG_STATE_A_HOST:
@@ -1351,7 +1372,7 @@ static ssize_t msm_otg_mode_write(struct file *file, const char __user *ubuf,
 			goto out;
 		}
 		break;
-	case USB_HOST:
+	case USB_DR_MODE_HOST:
 		switch (otg->phy->state) {
 		case OTG_STATE_B_IDLE:
 		case OTG_STATE_B_PERIPHERAL:
@@ -1406,74 +1427,154 @@ static void msm_otg_debugfs_cleanup(void)
 	debugfs_remove(msm_otg_dbg_root);
 }
 
-static int __init msm_otg_probe(struct platform_device *pdev)
+static struct of_device_id msm_otg_dt_match[] = {
+	{
+		.compatible = "qcom,usb-otg-ci",
+		.data = (void *) CI_45NM_INTEGRATED_PHY
+	},
+	{
+		.compatible = "qcom,usb-otg-snps",
+		.data = (void *) SNPS_28NM_INTEGRATED_PHY
+	},
+	{ }
+};
+MODULE_DEVICE_TABLE(of, msm_otg_dt_match);
+
+static int msm_otg_read_dt(struct platform_device *pdev, struct msm_otg *motg)
+{
+	struct msm_otg_platform_data *pdata;
+	const struct of_device_id *id;
+	struct device_node *node = pdev->dev.of_node;
+	struct property *prop;
+	int len, ret, words;
+	u32 val, tmp[3];
+
+	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return -ENOMEM;
+
+	motg->pdata = pdata;
+
+	id = of_match_device(msm_otg_dt_match, &pdev->dev);
+	pdata->phy_type = (enum msm_usb_phy_type) id->data;
+
+	motg->link_rst = devm_reset_control_get(&pdev->dev, "link");
+	if (IS_ERR(motg->link_rst))
+		return PTR_ERR(motg->link_rst);
+
+	motg->phy_rst = devm_reset_control_get(&pdev->dev, "phy");
+	if (IS_ERR(motg->phy_rst))
+		return PTR_ERR(motg->phy_rst);
+
+	pdata->mode = of_usb_get_dr_mode(node);
+	if (pdata->mode == USB_DR_MODE_UNKNOWN)
+		pdata->mode = USB_DR_MODE_OTG;
+
+	pdata->otg_control = OTG_PHY_CONTROL;
+	if (!of_property_read_u32(node, "qcom,otg-control", &val))
+		if (val == OTG_PMIC_CONTROL)
+			pdata->otg_control = val;
+
+	if (!of_property_read_u32(node, "qcom,phy-num", &val) && val < 2)
+		motg->phy_number = val;
+
+	motg->vdd_levels[VDD_LEVEL_NONE] = USB_PHY_SUSP_DIG_VOL;
+	motg->vdd_levels[VDD_LEVEL_MIN] = USB_PHY_VDD_DIG_VOL_MIN;
+	motg->vdd_levels[VDD_LEVEL_MAX] = USB_PHY_VDD_DIG_VOL_MAX;
+
+	if (of_get_property(node, "qcom,vdd-levels", &len) &&
+	    len == sizeof(tmp)) {
+		of_property_read_u32_array(node, "qcom,vdd-levels",
+					   tmp, len / sizeof(*tmp));
+		motg->vdd_levels[VDD_LEVEL_NONE] = tmp[VDD_LEVEL_NONE];
+		motg->vdd_levels[VDD_LEVEL_MIN] = tmp[VDD_LEVEL_MIN];
+		motg->vdd_levels[VDD_LEVEL_MAX] = tmp[VDD_LEVEL_MAX];
+	}
+
+	prop = of_find_property(node, "qcom,phy-init-sequence", &len);
+	if (!prop || !len)
+		return 0;
+
+	words = len / sizeof(u32);
+
+	if (words >= ULPI_EXT_VENDOR_SPECIFIC) {
+		dev_warn(&pdev->dev, "Too big PHY init sequence %d\n", words);
+		return 0;
+	}
+
+	pdata->phy_init_seq = devm_kzalloc(&pdev->dev, len, GFP_KERNEL);
+	if (!pdata->phy_init_seq) {
+		dev_warn(&pdev->dev, "No space for PHY init sequence\n");
+		return 0;
+	}
+
+	ret = of_property_read_u32_array(node, "qcom,phy-init-sequence",
+					 pdata->phy_init_seq, words);
+	if (!ret)
+		pdata->phy_init_sz = words;
+
+	return 0;
+}
+
+static int msm_otg_probe(struct platform_device *pdev)
 {
+	struct regulator_bulk_data regs[3];
 	int ret = 0;
+	struct device_node *np = pdev->dev.of_node;
+	struct msm_otg_platform_data *pdata;
 	struct resource *res;
 	struct msm_otg *motg;
 	struct usb_phy *phy;
+	void __iomem *phy_select;
 
-	dev_info(&pdev->dev, "msm_otg probe\n");
-	if (!dev_get_platdata(&pdev->dev)) {
-		dev_err(&pdev->dev, "No platform data given. Bailing out\n");
-		return -ENODEV;
-	}
-
-	motg = kzalloc(sizeof(struct msm_otg), GFP_KERNEL);
+	motg = devm_kzalloc(&pdev->dev, sizeof(struct msm_otg), GFP_KERNEL);
 	if (!motg) {
 		dev_err(&pdev->dev, "unable to allocate msm_otg\n");
 		return -ENOMEM;
 	}
 
-	motg->phy.otg = kzalloc(sizeof(struct usb_otg), GFP_KERNEL);
+	pdata = dev_get_platdata(&pdev->dev);
+	if (!pdata) {
+		if (!np)
+			return -ENXIO;
+		ret = msm_otg_read_dt(pdev, motg);
+		if (ret)
+			return ret;
+	}
+
+	motg->phy.otg = devm_kzalloc(&pdev->dev, sizeof(struct usb_otg),
+				     GFP_KERNEL);
 	if (!motg->phy.otg) {
 		dev_err(&pdev->dev, "unable to allocate msm_otg\n");
-		ret = -ENOMEM;
-		goto free_motg;
+		return -ENOMEM;
 	}
 
-	motg->pdata = dev_get_platdata(&pdev->dev);
 	phy = &motg->phy;
 	phy->dev = &pdev->dev;
 
-	motg->phy_reset_clk = clk_get(&pdev->dev, "usb_phy_clk");
+	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");
-		ret = PTR_ERR(motg->phy_reset_clk);
-		goto free_motg;
+		return PTR_ERR(motg->phy_reset_clk);
 	}
 
-	motg->clk = clk_get(&pdev->dev, "usb_hs_clk");
+	motg->clk = devm_clk_get(&pdev->dev, np ? "core" : "usb_hs_clk");
 	if (IS_ERR(motg->clk)) {
 		dev_err(&pdev->dev, "failed to get usb_hs_clk\n");
-		ret = PTR_ERR(motg->clk);
-		goto put_phy_reset_clk;
+		return PTR_ERR(motg->clk);
 	}
-	clk_set_rate(motg->clk, 60000000);
 
 	/*
 	 * If USB Core is running its protocol engine based on CORE CLK,
 	 * CORE CLK  must be running at >55Mhz for correct HSUSB
 	 * operation and USB core cannot tolerate frequency changes on
-	 * CORE CLK. For such USB cores, vote for maximum clk frequency
-	 * on pclk source
+	 * CORE CLK.
 	 */
-	 if (motg->pdata->pclk_src_name) {
-		motg->pclk_src = clk_get(&pdev->dev,
-			motg->pdata->pclk_src_name);
-		if (IS_ERR(motg->pclk_src))
-			goto put_clk;
-		clk_set_rate(motg->pclk_src, INT_MAX);
-		clk_prepare_enable(motg->pclk_src);
-	} else
-		motg->pclk_src = ERR_PTR(-ENOENT);
-
-
-	motg->pclk = clk_get(&pdev->dev, "usb_hs_pclk");
+	motg->pclk = devm_clk_get(&pdev->dev, np ? "iface" : "usb_hs_pclk");
 	if (IS_ERR(motg->pclk)) {
 		dev_err(&pdev->dev, "failed to get usb_hs_pclk\n");
-		ret = PTR_ERR(motg->pclk);
-		goto put_pclk_src;
+		return PTR_ERR(motg->pclk);
 	}
 
 	/*
@@ -1481,69 +1582,90 @@ static int __init msm_otg_probe(struct platform_device *pdev)
 	 * clock is introduced to remove the dependency on AXI
 	 * bus frequency.
 	 */
-	motg->core_clk = clk_get(&pdev->dev, "usb_hs_core_clk");
-	if (IS_ERR(motg->core_clk))
-		motg->core_clk = NULL;
+	motg->core_clk = devm_clk_get(&pdev->dev,
+				      np ? "alt_core" : "usb_hs_core_clk");
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "failed to get platform resource mem\n");
-		ret = -ENODEV;
-		goto put_core_clk;
-	}
+	if (!res)
+		return -EINVAL;
+	motg->regs = devm_ioremap(&pdev->dev, res->start, resource_size(res));
+	if (!motg->regs)
+		return -ENOMEM;
 
-	motg->regs = ioremap(res->start, resource_size(res));
-	if (!motg->regs) {
-		dev_err(&pdev->dev, "ioremap failed\n");
-		ret = -ENOMEM;
-		goto put_core_clk;
+	/*
+	 * NOTE: The PHYs can be multiplexed between the chipidea controller
+	 * and the dwc3 controller, using a single bit. It is important that
+	 * the dwc3 driver does not set this bit in an incompatible way.
+	 */
+	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);
+		/* Enable second PHY with the OTG port */
+		writel(0x1, phy_select);
 	}
+
 	dev_info(&pdev->dev, "OTG regs = %p\n", motg->regs);
 
 	motg->irq = platform_get_irq(pdev, 0);
-	if (!motg->irq) {
+	if (motg->irq < 0) {
 		dev_err(&pdev->dev, "platform_get_irq failed\n");
-		ret = -ENODEV;
-		goto free_regs;
+		return motg->irq;
 	}
 
+	regs[0].supply = "vddcx";
+	regs[1].supply = "v3p3";
+	regs[2].supply = "v1p8";
+
+	ret = devm_regulator_bulk_get(motg->phy.dev, ARRAY_SIZE(regs), regs);
+	if (ret)
+		return ret;
+
+	motg->vddcx = regs[0].consumer;
+	motg->v3p3  = regs[1].consumer;
+	motg->v1p8  = regs[2].consumer;
+
+	clk_set_rate(motg->clk, 60000000);
+
 	clk_prepare_enable(motg->clk);
 	clk_prepare_enable(motg->pclk);
 
+	if (!IS_ERR(motg->core_clk))
+		clk_prepare_enable(motg->core_clk);
+
 	ret = msm_hsusb_init_vddcx(motg, 1);
 	if (ret) {
 		dev_err(&pdev->dev, "hsusb vddcx configuration failed\n");
-		goto free_regs;
+		goto disable_clks;
 	}
 
 	ret = msm_hsusb_ldo_init(motg, 1);
 	if (ret) {
 		dev_err(&pdev->dev, "hsusb vreg configuration failed\n");
-		goto vddcx_exit;
+		goto disable_vddcx;
 	}
-	ret = msm_hsusb_ldo_set_mode(1);
+	ret = msm_hsusb_ldo_set_mode(motg, 1);
 	if (ret) {
 		dev_err(&pdev->dev, "hsusb vreg enable failed\n");
-		goto ldo_exit;
+		goto disable_ldo;
 	}
 
-	if (motg->core_clk)
-		clk_prepare_enable(motg->core_clk);
-
 	writel(0, USB_USBINTR);
 	writel(0, USB_OTGSC);
 
 	INIT_WORK(&motg->sm_work, msm_otg_sm_work);
 	INIT_DELAYED_WORK(&motg->chg_work, msm_chg_detect_work);
-	ret = request_irq(motg->irq, msm_otg_irq, IRQF_SHARED,
+	ret = devm_request_irq(&pdev->dev, motg->irq, msm_otg_irq, IRQF_SHARED,
 					"msm_otg", motg);
 	if (ret) {
 		dev_err(&pdev->dev, "request irq failed\n");
-		goto disable_clks;
+		goto disable_ldo;
 	}
 
-	phy->init = msm_otg_reset;
+	phy->init = msm_phy_init;
 	phy->set_power = msm_otg_set_power;
+	phy->notify_disconnect = msm_phy_notify_disconnect;
+	phy->type = USB_PHY_TYPE_USB2;
 
 	phy->io_ops = &msm_otg_io_ops;
 
@@ -1551,54 +1673,38 @@ static int __init msm_otg_probe(struct platform_device *pdev)
 	phy->otg->set_host = msm_otg_set_host;
 	phy->otg->set_peripheral = msm_otg_set_peripheral;
 
-	ret = usb_add_phy(&motg->phy, USB_PHY_TYPE_USB2);
+	msm_usb_reset(phy);
+
+	ret = usb_add_phy_dev(&motg->phy);
 	if (ret) {
 		dev_err(&pdev->dev, "usb_add_phy failed\n");
-		goto free_irq;
+		goto disable_ldo;
 	}
 
 	platform_set_drvdata(pdev, motg);
 	device_init_wakeup(&pdev->dev, 1);
 
-	if (motg->pdata->mode == USB_OTG &&
-			motg->pdata->otg_control == OTG_USER_CONTROL) {
+	if (motg->pdata->mode == USB_DR_MODE_OTG &&
+		motg->pdata->otg_control == OTG_USER_CONTROL) {
 		ret = msm_otg_debugfs_init(motg);
 		if (ret)
-			dev_dbg(&pdev->dev, "mode debugfs file is"
-					"not available\n");
+			dev_dbg(&pdev->dev, "Can not create mode change file\n");
 	}
 
 	pm_runtime_set_active(&pdev->dev);
 	pm_runtime_enable(&pdev->dev);
 
 	return 0;
-free_irq:
-	free_irq(motg->irq, motg);
+
+disable_ldo:
+	msm_hsusb_ldo_init(motg, 0);
+disable_vddcx:
+	msm_hsusb_init_vddcx(motg, 0);
 disable_clks:
 	clk_disable_unprepare(motg->pclk);
 	clk_disable_unprepare(motg->clk);
-ldo_exit:
-	msm_hsusb_ldo_init(motg, 0);
-vddcx_exit:
-	msm_hsusb_init_vddcx(motg, 0);
-free_regs:
-	iounmap(motg->regs);
-put_core_clk:
-	if (motg->core_clk)
-		clk_put(motg->core_clk);
-	clk_put(motg->pclk);
-put_pclk_src:
-	if (!IS_ERR(motg->pclk_src)) {
-		clk_disable_unprepare(motg->pclk_src);
-		clk_put(motg->pclk_src);
-	}
-put_clk:
-	clk_put(motg->clk);
-put_phy_reset_clk:
-	clk_put(motg->phy_reset_clk);
-free_motg:
-	kfree(motg->phy.otg);
-	kfree(motg);
+	if (!IS_ERR(motg->core_clk))
+		clk_disable_unprepare(motg->core_clk);
 	return ret;
 }
 
@@ -1621,7 +1727,7 @@ static int msm_otg_remove(struct platform_device *pdev)
 	pm_runtime_disable(&pdev->dev);
 
 	usb_remove_phy(phy);
-	free_irq(motg->irq, motg);
+	disable_irq(motg->irq);
 
 	/*
 	 * Put PHY in low power mode.
@@ -1641,26 +1747,12 @@ static int msm_otg_remove(struct platform_device *pdev)
 
 	clk_disable_unprepare(motg->pclk);
 	clk_disable_unprepare(motg->clk);
-	if (motg->core_clk)
+	if (!IS_ERR(motg->core_clk))
 		clk_disable_unprepare(motg->core_clk);
-	if (!IS_ERR(motg->pclk_src)) {
-		clk_disable_unprepare(motg->pclk_src);
-		clk_put(motg->pclk_src);
-	}
 	msm_hsusb_ldo_init(motg, 0);
 
-	iounmap(motg->regs);
 	pm_runtime_set_suspended(&pdev->dev);
 
-	clk_put(motg->phy_reset_clk);
-	clk_put(motg->pclk);
-	clk_put(motg->clk);
-	if (motg->core_clk)
-		clk_put(motg->core_clk);
-
-	kfree(motg->phy.otg);
-	kfree(motg);
-
 	return 0;
 }
 
@@ -1740,15 +1832,17 @@ static const struct dev_pm_ops msm_otg_dev_pm_ops = {
 };
 
 static struct platform_driver msm_otg_driver = {
+	.probe = msm_otg_probe,
 	.remove = msm_otg_remove,
 	.driver = {
 		.name = DRIVER_NAME,
 		.owner = THIS_MODULE,
 		.pm = &msm_otg_dev_pm_ops,
+		.of_match_table = msm_otg_dt_match,
 	},
 };
 
-module_platform_driver_probe(msm_otg_driver, msm_otg_probe);
+module_platform_driver(msm_otg_driver);
 
 MODULE_LICENSE("GPL v2");
 MODULE_DESCRIPTION("MSM USB transceiver driver");
diff --git a/drivers/usb/phy/phy-mv-u3d-usb.c b/drivers/usb/phy/phy-mv-u3d-usb.c
deleted file mode 100644
index d317903022b..00000000000
--- a/drivers/usb/phy/phy-mv-u3d-usb.c
+++ /dev/null
@@ -1,338 +0,0 @@
-/*
- * Copyright (C) 2011 Marvell International Ltd. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- */
-
-#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/usb/otg.h>
-#include <linux/platform_data/mv_usb.h>
-
-#include "phy-mv-u3d-usb.h"
-
-/*
- * struct mv_u3d_phy - transceiver driver state
- * @phy: transceiver structure
- * @dev: The parent device supplied to the probe function
- * @clk: usb phy clock
- * @base: usb phy register memory base
- */
-struct mv_u3d_phy {
-	struct usb_phy	phy;
-	struct mv_usb_platform_data *plat;
-	struct device	*dev;
-	struct clk	*clk;
-	void __iomem	*base;
-};
-
-static u32 mv_u3d_phy_read(void __iomem *base, u32 reg)
-{
-	void __iomem *addr, *data;
-
-	addr = base;
-	data = base + 0x4;
-
-	writel_relaxed(reg, addr);
-	return readl_relaxed(data);
-}
-
-static void mv_u3d_phy_set(void __iomem *base, u32 reg, u32 value)
-{
-	void __iomem *addr, *data;
-	u32 tmp;
-
-	addr = base;
-	data = base + 0x4;
-
-	writel_relaxed(reg, addr);
-	tmp = readl_relaxed(data);
-	tmp |= value;
-	writel_relaxed(tmp, data);
-}
-
-static void mv_u3d_phy_clear(void __iomem *base, u32 reg, u32 value)
-{
-	void __iomem *addr, *data;
-	u32 tmp;
-
-	addr = base;
-	data = base + 0x4;
-
-	writel_relaxed(reg, addr);
-	tmp = readl_relaxed(data);
-	tmp &= ~value;
-	writel_relaxed(tmp, data);
-}
-
-static void mv_u3d_phy_write(void __iomem *base, u32 reg, u32 value)
-{
-	void __iomem *addr, *data;
-
-	addr = base;
-	data = base + 0x4;
-
-	writel_relaxed(reg, addr);
-	writel_relaxed(value, data);
-}
-
-static void mv_u3d_phy_shutdown(struct usb_phy *phy)
-{
-	struct mv_u3d_phy *mv_u3d_phy;
-	void __iomem *base;
-	u32 val;
-
-	mv_u3d_phy = container_of(phy, struct mv_u3d_phy, phy);
-	base = mv_u3d_phy->base;
-
-	/* Power down Reference Analog current, bit 15
-	 * Power down PLL, bit 14
-	 * Power down Receiver, bit 13
-	 * Power down Transmitter, bit 12
-	 * of USB3_POWER_PLL_CONTROL register
-	 */
-	val = mv_u3d_phy_read(base, USB3_POWER_PLL_CONTROL);
-	val &= ~(USB3_POWER_PLL_CONTROL_PU);
-	mv_u3d_phy_write(base, USB3_POWER_PLL_CONTROL, val);
-
-	if (mv_u3d_phy->clk)
-		clk_disable(mv_u3d_phy->clk);
-}
-
-static int mv_u3d_phy_init(struct usb_phy *phy)
-{
-	struct mv_u3d_phy *mv_u3d_phy;
-	void __iomem *base;
-	u32 val, count;
-
-	/* enable usb3 phy */
-	mv_u3d_phy = container_of(phy, struct mv_u3d_phy, phy);
-
-	if (mv_u3d_phy->clk)
-		clk_enable(mv_u3d_phy->clk);
-
-	base = mv_u3d_phy->base;
-
-	val = mv_u3d_phy_read(base, USB3_POWER_PLL_CONTROL);
-	val &= ~(USB3_POWER_PLL_CONTROL_PU_MASK);
-	val |= 0xF << USB3_POWER_PLL_CONTROL_PU_SHIFT;
-	mv_u3d_phy_write(base, USB3_POWER_PLL_CONTROL, val);
-	udelay(100);
-
-	mv_u3d_phy_write(base, USB3_RESET_CONTROL,
-			USB3_RESET_CONTROL_RESET_PIPE);
-	udelay(100);
-
-	mv_u3d_phy_write(base, USB3_RESET_CONTROL,
-			USB3_RESET_CONTROL_RESET_PIPE
-			| USB3_RESET_CONTROL_RESET_PHY);
-	udelay(100);
-
-	val = mv_u3d_phy_read(base, USB3_POWER_PLL_CONTROL);
-	val &= ~(USB3_POWER_PLL_CONTROL_REF_FREF_SEL_MASK
-		| USB3_POWER_PLL_CONTROL_PHY_MODE_MASK);
-	val |=  (USB3_PLL_25MHZ << USB3_POWER_PLL_CONTROL_REF_FREF_SEL_SHIFT)
-		| (0x5 << USB3_POWER_PLL_CONTROL_PHY_MODE_SHIFT);
-	mv_u3d_phy_write(base, USB3_POWER_PLL_CONTROL, val);
-	udelay(100);
-
-	mv_u3d_phy_clear(base, USB3_KVCO_CALI_CONTROL,
-		USB3_KVCO_CALI_CONTROL_USE_MAX_PLL_RATE_MASK);
-	udelay(100);
-
-	val = mv_u3d_phy_read(base, USB3_SQUELCH_FFE);
-	val &= ~(USB3_SQUELCH_FFE_FFE_CAP_SEL_MASK
-		| USB3_SQUELCH_FFE_FFE_RES_SEL_MASK
-		| USB3_SQUELCH_FFE_SQ_THRESH_IN_MASK);
-	val |= ((0xD << USB3_SQUELCH_FFE_FFE_CAP_SEL_SHIFT)
-		| (0x7 << USB3_SQUELCH_FFE_FFE_RES_SEL_SHIFT)
-		| (0x8 << USB3_SQUELCH_FFE_SQ_THRESH_IN_SHIFT));
-	mv_u3d_phy_write(base, USB3_SQUELCH_FFE, val);
-	udelay(100);
-
-	val = mv_u3d_phy_read(base, USB3_GEN1_SET0);
-	val &= ~USB3_GEN1_SET0_G1_TX_SLEW_CTRL_EN_MASK;
-	val |= 1 << USB3_GEN1_SET0_G1_TX_EMPH_EN_SHIFT;
-	mv_u3d_phy_write(base, USB3_GEN1_SET0, val);
-	udelay(100);
-
-	val = mv_u3d_phy_read(base, USB3_GEN2_SET0);
-	val &= ~(USB3_GEN2_SET0_G2_TX_AMP_MASK
-		| USB3_GEN2_SET0_G2_TX_EMPH_AMP_MASK
-		| USB3_GEN2_SET0_G2_TX_SLEW_CTRL_EN_MASK);
-	val |= ((0x14 << USB3_GEN2_SET0_G2_TX_AMP_SHIFT)
-		| (1 << USB3_GEN2_SET0_G2_TX_AMP_ADJ_SHIFT)
-		| (0xA << USB3_GEN2_SET0_G2_TX_EMPH_AMP_SHIFT)
-		| (1 << USB3_GEN2_SET0_G2_TX_EMPH_EN_SHIFT));
-	mv_u3d_phy_write(base, USB3_GEN2_SET0, val);
-	udelay(100);
-
-	mv_u3d_phy_read(base, USB3_TX_EMPPH);
-	val &= ~(USB3_TX_EMPPH_AMP_MASK
-		| USB3_TX_EMPPH_EN_MASK
-		| USB3_TX_EMPPH_AMP_FORCE_MASK
-		| USB3_TX_EMPPH_PAR1_MASK
-		| USB3_TX_EMPPH_PAR2_MASK);
-	val |= ((0xB << USB3_TX_EMPPH_AMP_SHIFT)
-		| (1 << USB3_TX_EMPPH_EN_SHIFT)
-		| (1 << USB3_TX_EMPPH_AMP_FORCE_SHIFT)
-		| (0x1C << USB3_TX_EMPPH_PAR1_SHIFT)
-		| (1 << USB3_TX_EMPPH_PAR2_SHIFT));
-
-	mv_u3d_phy_write(base, USB3_TX_EMPPH, val);
-	udelay(100);
-
-	val = mv_u3d_phy_read(base, USB3_GEN2_SET1);
-	val &= ~(USB3_GEN2_SET1_G2_RX_SELMUPI_MASK
-		| USB3_GEN2_SET1_G2_RX_SELMUPF_MASK
-		| USB3_GEN2_SET1_G2_RX_SELMUFI_MASK
-		| USB3_GEN2_SET1_G2_RX_SELMUFF_MASK);
-	val |= ((1 << USB3_GEN2_SET1_G2_RX_SELMUPI_SHIFT)
-		| (1 << USB3_GEN2_SET1_G2_RX_SELMUPF_SHIFT)
-		| (1 << USB3_GEN2_SET1_G2_RX_SELMUFI_SHIFT)
-		| (1 << USB3_GEN2_SET1_G2_RX_SELMUFF_SHIFT));
-	mv_u3d_phy_write(base, USB3_GEN2_SET1, val);
-	udelay(100);
-
-	val = mv_u3d_phy_read(base, USB3_DIGITAL_LOOPBACK_EN);
-	val &= ~USB3_DIGITAL_LOOPBACK_EN_SEL_BITS_MASK;
-	val |= 1 << USB3_DIGITAL_LOOPBACK_EN_SEL_BITS_SHIFT;
-	mv_u3d_phy_write(base, USB3_DIGITAL_LOOPBACK_EN, val);
-	udelay(100);
-
-	val = mv_u3d_phy_read(base, USB3_IMPEDANCE_TX_SSC);
-	val &= ~USB3_IMPEDANCE_TX_SSC_SSC_AMP_MASK;
-	val |= 0xC << USB3_IMPEDANCE_TX_SSC_SSC_AMP_SHIFT;
-	mv_u3d_phy_write(base, USB3_IMPEDANCE_TX_SSC, val);
-	udelay(100);
-
-	val = mv_u3d_phy_read(base, USB3_IMPEDANCE_CALI_CTRL);
-	val &= ~USB3_IMPEDANCE_CALI_CTRL_IMP_CAL_THR_MASK;
-	val |= 0x4 << USB3_IMPEDANCE_CALI_CTRL_IMP_CAL_THR_SHIFT;
-	mv_u3d_phy_write(base, USB3_IMPEDANCE_CALI_CTRL, val);
-	udelay(100);
-
-	val = mv_u3d_phy_read(base, USB3_PHY_ISOLATION_MODE);
-	val &= ~(USB3_PHY_ISOLATION_MODE_PHY_GEN_RX_MASK
-		| USB3_PHY_ISOLATION_MODE_PHY_GEN_TX_MASK
-		| USB3_PHY_ISOLATION_MODE_TX_DRV_IDLE_MASK);
-	val |= ((1 << USB3_PHY_ISOLATION_MODE_PHY_GEN_RX_SHIFT)
-		| (1 << USB3_PHY_ISOLATION_MODE_PHY_GEN_TX_SHIFT));
-	mv_u3d_phy_write(base, USB3_PHY_ISOLATION_MODE, val);
-	udelay(100);
-
-	val = mv_u3d_phy_read(base, USB3_TXDETRX);
-	val &= ~(USB3_TXDETRX_VTHSEL_MASK);
-	val |= 0x1 << USB3_TXDETRX_VTHSEL_SHIFT;
-	mv_u3d_phy_write(base, USB3_TXDETRX, val);
-	udelay(100);
-
-	dev_dbg(mv_u3d_phy->dev, "start calibration\n");
-
-calstart:
-	/* Perform Manual Calibration */
-	mv_u3d_phy_set(base, USB3_KVCO_CALI_CONTROL,
-		1 << USB3_KVCO_CALI_CONTROL_CAL_START_SHIFT);
-
-	mdelay(1);
-
-	count = 0;
-	while (1) {
-		val = mv_u3d_phy_read(base, USB3_KVCO_CALI_CONTROL);
-		if (val & (1 << USB3_KVCO_CALI_CONTROL_CAL_DONE_SHIFT))
-			break;
-		else if (count > 50) {
-			dev_dbg(mv_u3d_phy->dev, "calibration failure, retry...\n");
-			goto calstart;
-		}
-		count++;
-		mdelay(1);
-	}
-
-	/* active PIPE interface */
-	mv_u3d_phy_write(base, USB3_PIPE_SM_CTRL,
-		1 << USB3_PIPE_SM_CTRL_PHY_INIT_DONE);
-
-	return 0;
-}
-
-static int mv_u3d_phy_probe(struct platform_device *pdev)
-{
-	struct mv_u3d_phy *mv_u3d_phy;
-	struct mv_usb_platform_data *pdata;
-	struct device *dev = &pdev->dev;
-	struct resource *res;
-	void __iomem	*phy_base;
-	int	ret;
-
-	pdata = dev_get_platdata(&pdev->dev);
-	if (!pdata) {
-		dev_err(&pdev->dev, "%s: no platform data defined\n", __func__);
-		return -EINVAL;
-	}
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	phy_base = devm_ioremap_resource(dev, res);
-	if (IS_ERR(phy_base))
-		return PTR_ERR(phy_base);
-
-	mv_u3d_phy = devm_kzalloc(dev, sizeof(*mv_u3d_phy), GFP_KERNEL);
-	if (!mv_u3d_phy)
-		return -ENOMEM;
-
-	mv_u3d_phy->dev			= &pdev->dev;
-	mv_u3d_phy->plat		= pdata;
-	mv_u3d_phy->base		= phy_base;
-	mv_u3d_phy->phy.dev		= mv_u3d_phy->dev;
-	mv_u3d_phy->phy.label		= "mv-u3d-phy";
-	mv_u3d_phy->phy.init		= mv_u3d_phy_init;
-	mv_u3d_phy->phy.shutdown	= mv_u3d_phy_shutdown;
-
-	ret = usb_add_phy(&mv_u3d_phy->phy, USB_PHY_TYPE_USB3);
-	if (ret)
-		goto err;
-
-	if (!mv_u3d_phy->clk)
-		mv_u3d_phy->clk = clk_get(mv_u3d_phy->dev, "u3dphy");
-
-	platform_set_drvdata(pdev, mv_u3d_phy);
-
-	dev_info(&pdev->dev, "Initialized Marvell USB 3.0 PHY\n");
-err:
-	return ret;
-}
-
-static int mv_u3d_phy_remove(struct platform_device *pdev)
-{
-	struct mv_u3d_phy *mv_u3d_phy = platform_get_drvdata(pdev);
-
-	usb_remove_phy(&mv_u3d_phy->phy);
-
-	if (mv_u3d_phy->clk) {
-		clk_put(mv_u3d_phy->clk);
-		mv_u3d_phy->clk = NULL;
-	}
-
-	return 0;
-}
-
-static struct platform_driver mv_u3d_phy_driver = {
-	.probe		= mv_u3d_phy_probe,
-	.remove		= mv_u3d_phy_remove,
-	.driver		= {
-		.name	= "mv-u3d-phy",
-		.owner	= THIS_MODULE,
-	},
-};
-
-module_platform_driver(mv_u3d_phy_driver);
-MODULE_DESCRIPTION("Marvell USB 3.0 PHY controller");
-MODULE_AUTHOR("Yu Xu <yuxu@marvell.com>");
-MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:mv-u3d-phy");
diff --git a/drivers/usb/phy/phy-mv-u3d-usb.h b/drivers/usb/phy/phy-mv-u3d-usb.h
deleted file mode 100644
index 2a658cb9a52..00000000000
--- a/drivers/usb/phy/phy-mv-u3d-usb.h
+++ /dev/null
@@ -1,105 +0,0 @@
-/*
- * Copyright (C) 2011 Marvell International Ltd. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- */
-
-#ifndef __MV_U3D_PHY_H
-#define __MV_U3D_PHY_H
-
-#define USB3_POWER_PLL_CONTROL		0x1
-#define USB3_KVCO_CALI_CONTROL		0x2
-#define USB3_IMPEDANCE_CALI_CTRL	0x3
-#define USB3_IMPEDANCE_TX_SSC		0x4
-#define USB3_SQUELCH_FFE		0x6
-#define USB3_GEN1_SET0			0xD
-#define USB3_GEN2_SET0			0xF
-#define USB3_GEN2_SET1			0x10
-#define USB3_DIGITAL_LOOPBACK_EN	0x23
-#define USB3_PHY_ISOLATION_MODE		0x26
-#define USB3_TXDETRX			0x48
-#define USB3_TX_EMPPH			0x5E
-#define USB3_RESET_CONTROL		0x90
-#define USB3_PIPE_SM_CTRL		0x91
-
-#define USB3_RESET_CONTROL_RESET_PIPE			0x1
-#define USB3_RESET_CONTROL_RESET_PHY			0x2
-
-#define USB3_POWER_PLL_CONTROL_REF_FREF_SEL_MASK	(0x1F << 0)
-#define USB3_POWER_PLL_CONTROL_REF_FREF_SEL_SHIFT	0
-#define USB3_PLL_25MHZ					0x2
-#define USB3_PLL_26MHZ					0x5
-#define USB3_POWER_PLL_CONTROL_PHY_MODE_MASK		(0x7 << 5)
-#define USB3_POWER_PLL_CONTROL_PHY_MODE_SHIFT		5
-#define USB3_POWER_PLL_CONTROL_PU_MASK			(0xF << 12)
-#define USB3_POWER_PLL_CONTROL_PU_SHIFT			12
-#define USB3_POWER_PLL_CONTROL_PU			(0xF << 12)
-
-#define USB3_KVCO_CALI_CONTROL_USE_MAX_PLL_RATE_MASK	(0x1 << 12)
-#define USB3_KVCO_CALI_CONTROL_USE_MAX_PLL_RATE_SHIFT	12
-#define USB3_KVCO_CALI_CONTROL_CAL_DONE_SHIFT		14
-#define USB3_KVCO_CALI_CONTROL_CAL_START_SHIFT		15
-
-#define USB3_SQUELCH_FFE_FFE_CAP_SEL_MASK		0xF
-#define USB3_SQUELCH_FFE_FFE_CAP_SEL_SHIFT		0
-#define USB3_SQUELCH_FFE_FFE_RES_SEL_MASK		(0x7 << 4)
-#define USB3_SQUELCH_FFE_FFE_RES_SEL_SHIFT		4
-#define USB3_SQUELCH_FFE_SQ_THRESH_IN_MASK		(0x1F << 8)
-#define USB3_SQUELCH_FFE_SQ_THRESH_IN_SHIFT		8
-
-#define USB3_GEN1_SET0_G1_TX_SLEW_CTRL_EN_MASK		(0x1 << 15)
-#define USB3_GEN1_SET0_G1_TX_EMPH_EN_SHIFT		11
-
-#define USB3_GEN2_SET0_G2_TX_AMP_MASK			(0x1F << 1)
-#define USB3_GEN2_SET0_G2_TX_AMP_SHIFT			1
-#define USB3_GEN2_SET0_G2_TX_AMP_ADJ_SHIFT		6
-#define USB3_GEN2_SET0_G2_TX_EMPH_AMP_MASK		(0xF << 7)
-#define USB3_GEN2_SET0_G2_TX_EMPH_AMP_SHIFT		7
-#define USB3_GEN2_SET0_G2_TX_EMPH_EN_MASK		(0x1 << 11)
-#define USB3_GEN2_SET0_G2_TX_EMPH_EN_SHIFT		11
-#define USB3_GEN2_SET0_G2_TX_SLEW_CTRL_EN_MASK		(0x1 << 15)
-#define USB3_GEN2_SET0_G2_TX_SLEW_CTRL_EN_SHIFT		15
-
-#define USB3_GEN2_SET1_G2_RX_SELMUPI_MASK		(0x7 << 0)
-#define USB3_GEN2_SET1_G2_RX_SELMUPI_SHIFT		0
-#define USB3_GEN2_SET1_G2_RX_SELMUPF_MASK		(0x7 << 3)
-#define USB3_GEN2_SET1_G2_RX_SELMUPF_SHIFT		3
-#define USB3_GEN2_SET1_G2_RX_SELMUFI_MASK		(0x3 << 6)
-#define USB3_GEN2_SET1_G2_RX_SELMUFI_SHIFT		6
-#define USB3_GEN2_SET1_G2_RX_SELMUFF_MASK		(0x3 << 8)
-#define USB3_GEN2_SET1_G2_RX_SELMUFF_SHIFT		8
-
-#define USB3_DIGITAL_LOOPBACK_EN_SEL_BITS_MASK		(0x3 << 10)
-#define USB3_DIGITAL_LOOPBACK_EN_SEL_BITS_SHIFT		10
-
-#define USB3_IMPEDANCE_CALI_CTRL_IMP_CAL_THR_MASK	(0x7 << 12)
-#define USB3_IMPEDANCE_CALI_CTRL_IMP_CAL_THR_SHIFT	12
-
-#define USB3_IMPEDANCE_TX_SSC_SSC_AMP_MASK		(0x3F << 0)
-#define USB3_IMPEDANCE_TX_SSC_SSC_AMP_SHIFT		0
-
-#define USB3_PHY_ISOLATION_MODE_PHY_GEN_RX_MASK		0xF
-#define USB3_PHY_ISOLATION_MODE_PHY_GEN_RX_SHIFT	0
-#define USB3_PHY_ISOLATION_MODE_PHY_GEN_TX_MASK		(0xF << 4)
-#define USB3_PHY_ISOLATION_MODE_PHY_GEN_TX_SHIFT	4
-#define USB3_PHY_ISOLATION_MODE_TX_DRV_IDLE_MASK	(0x1 << 8)
-
-#define USB3_TXDETRX_VTHSEL_MASK			(0x3 << 4)
-#define USB3_TXDETRX_VTHSEL_SHIFT			4
-
-#define USB3_TX_EMPPH_AMP_MASK				(0xF << 0)
-#define USB3_TX_EMPPH_AMP_SHIFT				0
-#define USB3_TX_EMPPH_EN_MASK				(0x1 << 6)
-#define USB3_TX_EMPPH_EN_SHIFT				6
-#define USB3_TX_EMPPH_AMP_FORCE_MASK			(0x1 << 7)
-#define USB3_TX_EMPPH_AMP_FORCE_SHIFT			7
-#define USB3_TX_EMPPH_PAR1_MASK				(0x1F << 8)
-#define USB3_TX_EMPPH_PAR1_SHIFT			8
-#define USB3_TX_EMPPH_PAR2_MASK				(0x1 << 13)
-#define USB3_TX_EMPPH_PAR2_SHIFT			13
-
-#define USB3_PIPE_SM_CTRL_PHY_INIT_DONE			15
-
-#endif /* __MV_U3D_PHY_H */
diff --git a/drivers/usb/phy/phy-ulpi.c b/drivers/usb/phy/phy-ulpi.c
index 17ea3f271bd..4e3877c329f 100644
--- a/drivers/usb/phy/phy-ulpi.c
+++ b/drivers/usb/phy/phy-ulpi.c
@@ -48,6 +48,7 @@ static struct ulpi_info ulpi_ids[] = {
 	ULPI_INFO(ULPI_ID(0x04cc, 0x1504), "NXP ISP1504"),
 	ULPI_INFO(ULPI_ID(0x0424, 0x0006), "SMSC USB331x"),
 	ULPI_INFO(ULPI_ID(0x0424, 0x0007), "SMSC USB3320"),
+	ULPI_INFO(ULPI_ID(0x0424, 0x0009), "SMSC USB334x"),
 	ULPI_INFO(ULPI_ID(0x0451, 0x1507), "TI TUSB1210"),
 };
 
diff --git a/drivers/usb/serial/bus.c b/drivers/usb/serial/bus.c
index 35a2373cde6..9374bd2aba2 100644
--- a/drivers/usb/serial/bus.c
+++ b/drivers/usb/serial/bus.c
@@ -97,13 +97,19 @@ static int usb_serial_device_remove(struct device *dev)
 	struct usb_serial_port *port;
 	int retval = 0;
 	int minor;
+	int autopm_err;
 
 	port = to_usb_serial_port(dev);
 	if (!port)
 		return -ENODEV;
 
-	/* make sure suspend/resume doesn't race against port_remove */
-	usb_autopm_get_interface(port->serial->interface);
+	/*
+	 * Make sure suspend/resume doesn't race against port_remove.
+	 *
+	 * Note that no further runtime PM callbacks will be made if
+	 * autopm_get fails.
+	 */
+	autopm_err = usb_autopm_get_interface(port->serial->interface);
 
 	minor = port->minor;
 	tty_unregister_device(usb_serial_tty_driver, minor);
@@ -117,7 +123,9 @@ static int usb_serial_device_remove(struct device *dev)
 	dev_info(dev, "%s converter now disconnected from ttyUSB%d\n",
 		 driver->description, minor);
 
-	usb_autopm_put_interface(port->serial->interface);
+	if (!autopm_err)
+		usb_autopm_put_interface(port->serial->interface);
+
 	return retval;
 }
 
diff --git a/drivers/usb/serial/ftdi_sio.c b/drivers/usb/serial/ftdi_sio.c
index 7c6e1dedeb0..edf3b124583 100644
--- a/drivers/usb/serial/ftdi_sio.c
+++ b/drivers/usb/serial/ftdi_sio.c
@@ -580,6 +580,8 @@ static const struct usb_device_id id_table_combined[] = {
 	{ USB_DEVICE(FTDI_VID, FTDI_TAVIR_STK500_PID) },
 	{ USB_DEVICE(FTDI_VID, FTDI_TIAO_UMPA_PID),
 		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
+	{ USB_DEVICE(FTDI_VID, FTDI_NT_ORIONLXM_PID),
+		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
 	/*
 	 * ELV devices:
 	 */
diff --git a/drivers/usb/serial/ftdi_sio_ids.h b/drivers/usb/serial/ftdi_sio_ids.h
index 993c93df687..500474c48f4 100644
--- a/drivers/usb/serial/ftdi_sio_ids.h
+++ b/drivers/usb/serial/ftdi_sio_ids.h
@@ -538,6 +538,11 @@
  */
 #define FTDI_TIAO_UMPA_PID	0x8a98	/* TIAO/DIYGADGET USB Multi-Protocol Adapter */
 
+/*
+ * NovaTech product ids (FTDI_VID)
+ */
+#define FTDI_NT_ORIONLXM_PID	0x7c90	/* OrionLXm Substation Automation Platform */
+
 
 /********************************/
 /** third-party VID/PID combos **/
diff --git a/drivers/usb/serial/io_ti.c b/drivers/usb/serial/io_ti.c
index df90dae53eb..c0a42e9e677 100644
--- a/drivers/usb/serial/io_ti.c
+++ b/drivers/usb/serial/io_ti.c
@@ -821,7 +821,7 @@ static int build_i2c_fw_hdr(__u8 *header, struct device *dev)
 	firmware_rec =  (struct ti_i2c_firmware_rec*)i2c_header->Data;
 
 	i2c_header->Type	= I2C_DESC_TYPE_FIRMWARE_BLANK;
-	i2c_header->Size	= (__u16)buffer_size;
+	i2c_header->Size	= cpu_to_le16(buffer_size);
 	i2c_header->CheckSum	= cs;
 	firmware_rec->Ver_Major	= OperationalMajorVersion;
 	firmware_rec->Ver_Minor	= OperationalMinorVersion;
diff --git a/drivers/usb/serial/io_usbvend.h b/drivers/usb/serial/io_usbvend.h
index 51f83fbb73b..6f6a856bc37 100644
--- a/drivers/usb/serial/io_usbvend.h
+++ b/drivers/usb/serial/io_usbvend.h
@@ -594,7 +594,7 @@ struct edge_boot_descriptor {
 
 struct ti_i2c_desc {
 	__u8	Type;			// Type of descriptor
-	__u16	Size;			// Size of data only not including header
+	__le16	Size;			// Size of data only not including header
 	__u8	CheckSum;		// Checksum (8 bit sum of data only)
 	__u8	Data[0];		// Data starts here
 } __attribute__((packed));
diff --git a/drivers/usb/serial/keyspan.c b/drivers/usb/serial/keyspan.c
index d3acaead5a8..93cb7cebda6 100644
--- a/drivers/usb/serial/keyspan.c
+++ b/drivers/usb/serial/keyspan.c
@@ -1535,14 +1535,14 @@ static int keyspan_usa26_send_setup(struct usb_serial *serial,
 
 	this_urb = p_priv->outcont_urb;
 
-	dev_dbg(&port->dev, "%s - endpoint %d\n", __func__, usb_pipeendpoint(this_urb->pipe));
-
 		/* Make sure we have an urb then send the message */
 	if (this_urb == NULL) {
 		dev_dbg(&port->dev, "%s - oops no urb.\n", __func__);
 		return -1;
 	}
 
+	dev_dbg(&port->dev, "%s - endpoint %d\n", __func__, usb_pipeendpoint(this_urb->pipe));
+
 	/* Save reset port val for resend.
 	   Don't overwrite resend for open/close condition. */
 	if ((reset_port + 1) > p_priv->resend_cont)
diff --git a/drivers/usb/serial/kobil_sct.c b/drivers/usb/serial/kobil_sct.c
index fee242387f5..078f9ed419c 100644
--- a/drivers/usb/serial/kobil_sct.c
+++ b/drivers/usb/serial/kobil_sct.c
@@ -215,13 +215,13 @@ static int kobil_open(struct tty_struct *tty, struct usb_serial_port *port)
 			priv->device_type == KOBIL_ADAPTER_K_PRODUCT_ID) {
 		/* Setting Baudrate, Parity and Stopbits */
 		result = usb_control_msg(port->serial->dev,
-			  usb_rcvctrlpipe(port->serial->dev, 0),
+			  usb_sndctrlpipe(port->serial->dev, 0),
 			  SUSBCRequest_SetBaudRateParityAndStopBits,
 			  USB_TYPE_VENDOR | USB_RECIP_ENDPOINT | USB_DIR_OUT,
 			  SUSBCR_SBR_9600 | SUSBCR_SPASB_EvenParity |
 							SUSBCR_SPASB_1StopBit,
 			  0,
-			  transfer_buffer,
+			  NULL,
 			  0,
 			  KOBIL_TIMEOUT
 		);
@@ -229,12 +229,12 @@ static int kobil_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 		/* reset all queues */
 		result = usb_control_msg(port->serial->dev,
-			  usb_rcvctrlpipe(port->serial->dev, 0),
+			  usb_sndctrlpipe(port->serial->dev, 0),
 			  SUSBCRequest_Misc,
 			  USB_TYPE_VENDOR | USB_RECIP_ENDPOINT | USB_DIR_OUT,
 			  SUSBCR_MSC_ResetAllQueues,
 			  0,
-			  transfer_buffer,
+			  NULL,
 			  0,
 			  KOBIL_TIMEOUT
 		);
@@ -445,12 +445,12 @@ static int kobil_tiocmset(struct tty_struct *tty,
 		else
 			dev_dbg(dev, "%s - Clearing DTR\n", __func__);
 		result = usb_control_msg(port->serial->dev,
-			  usb_rcvctrlpipe(port->serial->dev, 0),
+			  usb_sndctrlpipe(port->serial->dev, 0),
 			  SUSBCRequest_SetStatusLinesOrQueues,
 			  USB_TYPE_VENDOR | USB_RECIP_ENDPOINT | USB_DIR_OUT,
 			  ((dtr != 0) ? SUSBCR_SSL_SETDTR : SUSBCR_SSL_CLRDTR),
 			  0,
-			  transfer_buffer,
+			  NULL,
 			  0,
 			  KOBIL_TIMEOUT);
 	} else {
@@ -459,12 +459,12 @@ static int kobil_tiocmset(struct tty_struct *tty,
 		else
 			dev_dbg(dev, "%s - Clearing RTS\n", __func__);
 		result = usb_control_msg(port->serial->dev,
-			usb_rcvctrlpipe(port->serial->dev, 0),
+			usb_sndctrlpipe(port->serial->dev, 0),
 			SUSBCRequest_SetStatusLinesOrQueues,
 			USB_TYPE_VENDOR | USB_RECIP_ENDPOINT | USB_DIR_OUT,
 			((rts != 0) ? SUSBCR_SSL_SETRTS : SUSBCR_SSL_CLRRTS),
 			0,
-			transfer_buffer,
+			NULL,
 			0,
 			KOBIL_TIMEOUT);
 	}
@@ -514,7 +514,7 @@ static void kobil_set_termios(struct tty_struct *tty,
 	tty_encode_baud_rate(tty, speed, speed);
 
 	result = usb_control_msg(port->serial->dev,
-		  usb_rcvctrlpipe(port->serial->dev, 0),
+		  usb_sndctrlpipe(port->serial->dev, 0),
 		  SUSBCRequest_SetBaudRateParityAndStopBits,
 		  USB_TYPE_VENDOR | USB_RECIP_ENDPOINT | USB_DIR_OUT,
 		  urb_val,
@@ -546,12 +546,12 @@ static int kobil_ioctl(struct tty_struct *tty,
 			return -ENOBUFS;
 
 		result = usb_control_msg(port->serial->dev,
-			  usb_rcvctrlpipe(port->serial->dev, 0),
+			  usb_sndctrlpipe(port->serial->dev, 0),
 			  SUSBCRequest_Misc,
 			  USB_TYPE_VENDOR | USB_RECIP_ENDPOINT | USB_DIR_OUT,
 			  SUSBCR_MSC_ResetAllQueues,
 			  0,
-			  NULL, /* transfer_buffer, */
+			  NULL,
 			  0,
 			  KOBIL_TIMEOUT
 			);
diff --git a/drivers/usb/serial/option.c b/drivers/usb/serial/option.c
index f213ee97851..59c3108cc13 100644
--- a/drivers/usb/serial/option.c
+++ b/drivers/usb/serial/option.c
@@ -161,6 +161,7 @@ static void option_instat_callback(struct urb *urb);
 #define NOVATELWIRELESS_PRODUCT_HSPA_EMBEDDED_FULLSPEED	0x9000
 #define NOVATELWIRELESS_PRODUCT_HSPA_EMBEDDED_HIGHSPEED	0x9001
 #define NOVATELWIRELESS_PRODUCT_E362		0x9010
+#define NOVATELWIRELESS_PRODUCT_E371		0x9011
 #define NOVATELWIRELESS_PRODUCT_G2		0xA010
 #define NOVATELWIRELESS_PRODUCT_MC551		0xB001
 
@@ -1012,6 +1013,7 @@ static const struct usb_device_id option_ids[] = {
 	/* Novatel Ovation MC551 a.k.a. Verizon USB551L */
 	{ USB_DEVICE_AND_INTERFACE_INFO(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_MC551, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_E362, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_E371, 0xff, 0xff, 0xff) },
 
 	{ USB_DEVICE(AMOI_VENDOR_ID, AMOI_PRODUCT_H01) },
 	{ USB_DEVICE(AMOI_VENDOR_ID, AMOI_PRODUCT_H01A) },
@@ -1729,7 +1731,6 @@ static struct usb_serial_driver option_1port_device = {
 	.write             = usb_wwan_write,
 	.write_room        = usb_wwan_write_room,
 	.chars_in_buffer   = usb_wwan_chars_in_buffer,
-	.set_termios       = usb_wwan_set_termios,
 	.tiocmget          = usb_wwan_tiocmget,
 	.tiocmset          = usb_wwan_tiocmset,
 	.ioctl             = usb_wwan_ioctl,
@@ -1904,6 +1905,7 @@ static void option_instat_callback(struct urb *urb)
 
 	/* Resubmit urb so we continue receiving IRQ data */
 	if (status != -ESHUTDOWN && status != -ENOENT) {
+		usb_mark_last_busy(port->serial->dev);
 		err = usb_submit_urb(urb, GFP_ATOMIC);
 		if (err)
 			dev_dbg(dev, "%s: resubmit intr urb failed. (%d)\n",
@@ -1923,6 +1925,7 @@ static int option_send_setup(struct usb_serial_port *port)
 	struct option_private *priv = intfdata->private;
 	struct usb_wwan_port_private *portdata;
 	int val = 0;
+	int res;
 
 	portdata = usb_get_serial_port_data(port);
 
@@ -1931,9 +1934,17 @@ static int option_send_setup(struct usb_serial_port *port)
 	if (portdata->rts_state)
 		val |= 0x02;
 
-	return usb_control_msg(serial->dev, usb_rcvctrlpipe(serial->dev, 0),
+	res = usb_autopm_get_interface(serial->interface);
+	if (res)
+		return res;
+
+	res = usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
 				0x22, 0x21, val, priv->bInterfaceNumber, NULL,
 				0, USB_CTRL_SET_TIMEOUT);
+
+	usb_autopm_put_interface(serial->interface);
+
+	return res;
 }
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
diff --git a/drivers/usb/serial/qcserial.c b/drivers/usb/serial/qcserial.c
index 6c0a542e8ec..b2aa003bf41 100644
--- a/drivers/usb/serial/qcserial.c
+++ b/drivers/usb/serial/qcserial.c
@@ -22,8 +22,17 @@
 #define DRIVER_AUTHOR "Qualcomm Inc"
 #define DRIVER_DESC "Qualcomm USB Serial driver"
 
+/* standard device layouts supported by this driver */
+enum qcserial_layouts {
+	QCSERIAL_G2K = 0,	/* Gobi 2000 */
+	QCSERIAL_G1K = 1,	/* Gobi 1000 */
+	QCSERIAL_SWI = 2,	/* Sierra Wireless */
+};
+
 #define DEVICE_G1K(v, p) \
-	USB_DEVICE(v, p), .driver_info = 1
+	USB_DEVICE(v, p), .driver_info = QCSERIAL_G1K
+#define DEVICE_SWI(v, p) \
+	USB_DEVICE(v, p), .driver_info = QCSERIAL_SWI
 
 static const struct usb_device_id id_table[] = {
 	/* Gobi 1000 devices */
@@ -126,46 +135,27 @@ static const struct usb_device_id id_table[] = {
 	{USB_DEVICE(0x12D1, 0x14F1)},	/* Sony Gobi 3000 Composite */
 	{USB_DEVICE(0x0AF0, 0x8120)},	/* Option GTM681W */
 
-	/* non Gobi Qualcomm serial devices */
-	{USB_DEVICE_INTERFACE_NUMBER(0x0f3d, 0x68a2, 0)},	/* Sierra Wireless MC7700 Device Management */
-	{USB_DEVICE_INTERFACE_NUMBER(0x0f3d, 0x68a2, 2)},	/* Sierra Wireless MC7700 NMEA */
-	{USB_DEVICE_INTERFACE_NUMBER(0x0f3d, 0x68a2, 3)},	/* Sierra Wireless MC7700 Modem */
-	{USB_DEVICE_INTERFACE_NUMBER(0x114f, 0x68a2, 0)},	/* Sierra Wireless MC7750 Device Management */
-	{USB_DEVICE_INTERFACE_NUMBER(0x114f, 0x68a2, 2)},	/* Sierra Wireless MC7750 NMEA */
-	{USB_DEVICE_INTERFACE_NUMBER(0x114f, 0x68a2, 3)},	/* Sierra Wireless MC7750 Modem */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x68a2, 0)},	/* Sierra Wireless MC7710 Device Management */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x68a2, 2)},	/* Sierra Wireless MC7710 NMEA */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x68a2, 3)},	/* Sierra Wireless MC7710 Modem */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x68c0, 0)},	/* Sierra Wireless MC73xx Device Management */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x68c0, 2)},	/* Sierra Wireless MC73xx NMEA */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x68c0, 3)},	/* Sierra Wireless MC73xx Modem */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x901c, 0)},	/* Sierra Wireless EM7700 Device Management */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x901c, 2)},	/* Sierra Wireless EM7700 NMEA */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x901c, 3)},	/* Sierra Wireless EM7700 Modem */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x901f, 0)},	/* Sierra Wireless EM7355 Device Management */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x901f, 2)},	/* Sierra Wireless EM7355 NMEA */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x901f, 3)},	/* Sierra Wireless EM7355 Modem */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x9041, 0)},	/* Sierra Wireless MC7305/MC7355 Device Management */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x9041, 2)},	/* Sierra Wireless MC7305/MC7355 NMEA */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x9041, 3)},	/* Sierra Wireless MC7305/MC7355 Modem */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x9051, 0)},	/* Netgear AirCard 340U Device Management */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x9051, 2)},	/* Netgear AirCard 340U NMEA */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x9051, 3)},	/* Netgear AirCard 340U Modem */
-	{USB_DEVICE_INTERFACE_NUMBER(0x413c, 0x81a2, 0)},	/* Dell Wireless 5806 Gobi(TM) 4G LTE Mobile Broadband Card Device Management */
-	{USB_DEVICE_INTERFACE_NUMBER(0x413c, 0x81a2, 2)},	/* Dell Wireless 5806 Gobi(TM) 4G LTE Mobile Broadband Card NMEA */
-	{USB_DEVICE_INTERFACE_NUMBER(0x413c, 0x81a2, 3)},	/* Dell Wireless 5806 Gobi(TM) 4G LTE Mobile Broadband Card Modem */
-	{USB_DEVICE_INTERFACE_NUMBER(0x413c, 0x81a3, 0)},	/* Dell Wireless 5570 HSPA+ (42Mbps) Mobile Broadband Card Device Management */
-	{USB_DEVICE_INTERFACE_NUMBER(0x413c, 0x81a3, 2)},	/* Dell Wireless 5570 HSPA+ (42Mbps) Mobile Broadband Card NMEA */
-	{USB_DEVICE_INTERFACE_NUMBER(0x413c, 0x81a3, 3)},	/* Dell Wireless 5570 HSPA+ (42Mbps) Mobile Broadband Card Modem */
-	{USB_DEVICE_INTERFACE_NUMBER(0x413c, 0x81a4, 0)},	/* Dell Wireless 5570e HSPA+ (42Mbps) Mobile Broadband Card Device Management */
-	{USB_DEVICE_INTERFACE_NUMBER(0x413c, 0x81a4, 2)},	/* Dell Wireless 5570e HSPA+ (42Mbps) Mobile Broadband Card NMEA */
-	{USB_DEVICE_INTERFACE_NUMBER(0x413c, 0x81a4, 3)},	/* Dell Wireless 5570e HSPA+ (42Mbps) Mobile Broadband Card Modem */
-	{USB_DEVICE_INTERFACE_NUMBER(0x413c, 0x81a8, 0)},	/* Dell Wireless 5808 Gobi(TM) 4G LTE Mobile Broadband Card Device Management */
-	{USB_DEVICE_INTERFACE_NUMBER(0x413c, 0x81a8, 2)},	/* Dell Wireless 5808 Gobi(TM) 4G LTE Mobile Broadband Card NMEA */
-	{USB_DEVICE_INTERFACE_NUMBER(0x413c, 0x81a8, 3)},	/* Dell Wireless 5808 Gobi(TM) 4G LTE Mobile Broadband Card Modem */
-	{USB_DEVICE_INTERFACE_NUMBER(0x413c, 0x81a9, 0)},	/* Dell Wireless 5808e Gobi(TM) 4G LTE Mobile Broadband Card Device Management */
-	{USB_DEVICE_INTERFACE_NUMBER(0x413c, 0x81a9, 2)},	/* Dell Wireless 5808e Gobi(TM) 4G LTE Mobile Broadband Card NMEA */
-	{USB_DEVICE_INTERFACE_NUMBER(0x413c, 0x81a9, 3)},	/* Dell Wireless 5808e Gobi(TM) 4G LTE Mobile Broadband Card Modem */
+	/* non-Gobi Sierra Wireless devices */
+	{DEVICE_SWI(0x0f3d, 0x68a2)},	/* Sierra Wireless MC7700 */
+	{DEVICE_SWI(0x114f, 0x68a2)},	/* Sierra Wireless MC7750 */
+	{DEVICE_SWI(0x1199, 0x68a2)},	/* Sierra Wireless MC7710 */
+	{DEVICE_SWI(0x1199, 0x68c0)},	/* Sierra Wireless MC73xx */
+	{DEVICE_SWI(0x1199, 0x901c)},	/* Sierra Wireless EM7700 */
+	{DEVICE_SWI(0x1199, 0x901f)},	/* Sierra Wireless EM7355 */
+	{DEVICE_SWI(0x1199, 0x9040)},	/* Sierra Wireless Modem */
+	{DEVICE_SWI(0x1199, 0x9041)},	/* Sierra Wireless MC7305/MC7355 */
+	{DEVICE_SWI(0x1199, 0x9051)},	/* Netgear AirCard 340U */
+	{DEVICE_SWI(0x1199, 0x9053)},	/* Sierra Wireless Modem */
+	{DEVICE_SWI(0x1199, 0x9054)},	/* Sierra Wireless Modem */
+	{DEVICE_SWI(0x1199, 0x9055)},	/* Netgear AirCard 341U */
+	{DEVICE_SWI(0x1199, 0x9056)},	/* Sierra Wireless Modem */
+	{DEVICE_SWI(0x1199, 0x9060)},	/* Sierra Wireless Modem */
+	{DEVICE_SWI(0x1199, 0x9061)},	/* Sierra Wireless Modem */
+	{DEVICE_SWI(0x413c, 0x81a2)},	/* Dell Wireless 5806 Gobi(TM) 4G LTE Mobile Broadband Card */
+	{DEVICE_SWI(0x413c, 0x81a3)},	/* Dell Wireless 5570 HSPA+ (42Mbps) Mobile Broadband Card */
+	{DEVICE_SWI(0x413c, 0x81a4)},	/* Dell Wireless 5570e HSPA+ (42Mbps) Mobile Broadband Card */
+	{DEVICE_SWI(0x413c, 0x81a8)},	/* Dell Wireless 5808 Gobi(TM) 4G LTE Mobile Broadband Card */
+	{DEVICE_SWI(0x413c, 0x81a9)},	/* Dell Wireless 5808e Gobi(TM) 4G LTE Mobile Broadband Card */
 
 	{ }				/* Terminating entry */
 };
@@ -178,11 +168,8 @@ static int qcprobe(struct usb_serial *serial, const struct usb_device_id *id)
 	int retval = -ENODEV;
 	__u8 nintf;
 	__u8 ifnum;
-	bool is_gobi1k = id->driver_info ? true : false;
 	int altsetting = -1;
 
-	dev_dbg(dev, "Is Gobi 1000 = %d\n", is_gobi1k);
-
 	nintf = serial->dev->actconfig->desc.bNumInterfaces;
 	dev_dbg(dev, "Num Interfaces = %d\n", nintf);
 	ifnum = intf->desc.bInterfaceNumber;
@@ -210,32 +197,29 @@ static int qcprobe(struct usb_serial *serial, const struct usb_device_id *id)
 
 	}
 
-	/* allow any number of interfaces when doing direct interface match */
-	if (id->match_flags & USB_DEVICE_ID_MATCH_INT_NUMBER) {
-		dev_dbg(dev, "Generic Qualcomm serial interface found\n");
-		altsetting = 0;
-		goto done;
-	}
-
-	if (nintf < 3 || nintf > 4) {
-		dev_err(dev, "unknown number of interfaces: %d\n", nintf);
-		goto done;
-	}
-
 	/* default to enabling interface */
 	altsetting = 0;
 
-	/* Composite mode; don't bind to the QMI/net interface as that
+	/*
+	 * Composite mode; don't bind to the QMI/net interface as that
 	 * gets handled by other drivers.
 	 */
 
-	if (is_gobi1k) {
-		/* Gobi 1K USB layout:
+	switch (id->driver_info) {
+	case QCSERIAL_G1K:
+		/*
+		 * Gobi 1K USB layout:
 		 * 0: DM/DIAG (use libqcdm from ModemManager for communication)
 		 * 1: serial port (doesn't respond)
 		 * 2: AT-capable modem port
 		 * 3: QMI/net
 		 */
+		if (nintf < 3 || nintf > 4) {
+			dev_err(dev, "unknown number of interfaces: %d\n", nintf);
+			altsetting = -1;
+			goto done;
+		}
+
 		if (ifnum == 0) {
 			dev_dbg(dev, "Gobi 1K DM/DIAG interface found\n");
 			altsetting = 1;
@@ -243,13 +227,21 @@ static int qcprobe(struct usb_serial *serial, const struct usb_device_id *id)
 			dev_dbg(dev, "Modem port found\n");
 		else
 			altsetting = -1;
-	} else {
-		/* Gobi 2K+ USB layout:
+		break;
+	case QCSERIAL_G2K:
+		/*
+		 * Gobi 2K+ USB layout:
 		 * 0: QMI/net
 		 * 1: DM/DIAG (use libqcdm from ModemManager for communication)
 		 * 2: AT-capable modem port
 		 * 3: NMEA
 		 */
+		if (nintf < 3 || nintf > 4) {
+			dev_err(dev, "unknown number of interfaces: %d\n", nintf);
+			altsetting = -1;
+			goto done;
+		}
+
 		switch (ifnum) {
 		case 0:
 			/* Don't claim the QMI/net interface */
@@ -270,6 +262,35 @@ static int qcprobe(struct usb_serial *serial, const struct usb_device_id *id)
 			dev_dbg(dev, "Gobi 2K+ NMEA GPS interface found\n");
 			break;
 		}
+		break;
+	case QCSERIAL_SWI:
+		/*
+		 * Sierra Wireless layout:
+		 * 0: DM/DIAG (use libqcdm from ModemManager for communication)
+		 * 2: NMEA
+		 * 3: AT-capable modem port
+		 * 8: QMI/net
+		 */
+		switch (ifnum) {
+		case 0:
+			dev_dbg(dev, "DM/DIAG interface found\n");
+			break;
+		case 2:
+			dev_dbg(dev, "NMEA GPS interface found\n");
+			break;
+		case 3:
+			dev_dbg(dev, "Modem port found\n");
+			break;
+		default:
+			/* don't claim any unsupported interface */
+			altsetting = -1;
+			break;
+		}
+		break;
+	default:
+		dev_err(dev, "unsupported device layout type: %lu\n",
+			id->driver_info);
+		break;
 	}
 
 done:
diff --git a/drivers/usb/serial/sierra.c b/drivers/usb/serial/sierra.c
index 6b192e602ce..6f7f01eb556 100644
--- a/drivers/usb/serial/sierra.c
+++ b/drivers/usb/serial/sierra.c
@@ -58,6 +58,7 @@ struct sierra_intf_private {
 	spinlock_t susp_lock;
 	unsigned int suspended:1;
 	int in_flight;
+	unsigned int open_ports;
 };
 
 static int sierra_set_power_state(struct usb_device *udev, __u16 swiState)
@@ -315,7 +316,6 @@ struct sierra_port_private {
 	int dsr_state;
 	int dcd_state;
 	int ri_state;
-	unsigned int opened:1;
 };
 
 static int sierra_send_setup(struct usb_serial_port *port)
@@ -364,20 +364,13 @@ static int sierra_send_setup(struct usb_serial_port *port)
 	if (retval < 0)
 		return retval;
 
-	retval = usb_control_msg(serial->dev, usb_rcvctrlpipe(serial->dev, 0),
+	retval = usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
 		0x22, 0x21, val, interface, NULL, 0, USB_CTRL_SET_TIMEOUT);
 	usb_autopm_put_interface(serial->interface);
 
 	return retval;
 }
 
-static void sierra_set_termios(struct tty_struct *tty,
-		struct usb_serial_port *port, struct ktermios *old_termios)
-{
-	tty_termios_copy_hw(&tty->termios, old_termios);
-	sierra_send_setup(port);
-}
-
 static int sierra_tiocmget(struct tty_struct *tty)
 {
 	struct usb_serial_port *port = tty->driver_data;
@@ -418,9 +411,7 @@ static int sierra_tiocmset(struct tty_struct *tty,
 
 static void sierra_release_urb(struct urb *urb)
 {
-	struct usb_serial_port *port;
 	if (urb) {
-		port = urb->context;
 		kfree(urb->transfer_buffer);
 		usb_free_urb(urb);
 	}
@@ -433,7 +424,7 @@ static void sierra_outdat_callback(struct urb *urb)
 	struct sierra_intf_private *intfdata;
 	int status = urb->status;
 
-	intfdata = port->serial->private;
+	intfdata = usb_get_serial_data(port->serial);
 
 	/* free up the transfer buffer, as usb_free_urb() does not do this */
 	kfree(urb->transfer_buffer);
@@ -470,7 +461,7 @@ static int sierra_write(struct tty_struct *tty, struct usb_serial_port *port,
 		return 0;
 
 	portdata = usb_get_serial_port_data(port);
-	intfdata = serial->private;
+	intfdata = usb_get_serial_data(serial);
 
 	dev_dbg(&port->dev, "%s: write (%zd bytes)\n", __func__, writesize);
 	spin_lock_irqsave(&portdata->lock, flags);
@@ -674,6 +665,23 @@ static int sierra_write_room(struct tty_struct *tty)
 	return 2048;
 }
 
+static int sierra_chars_in_buffer(struct tty_struct *tty)
+{
+	struct usb_serial_port *port = tty->driver_data;
+	struct sierra_port_private *portdata = usb_get_serial_port_data(port);
+	unsigned long flags;
+	int chars;
+
+	/* NOTE: This overcounts somewhat. */
+	spin_lock_irqsave(&portdata->lock, flags);
+	chars = portdata->outstanding_urbs * MAX_TRANSFER;
+	spin_unlock_irqrestore(&portdata->lock, flags);
+
+	dev_dbg(&port->dev, "%s - %d\n", __func__, chars);
+
+	return chars;
+}
+
 static void sierra_stop_rx_urbs(struct usb_serial_port *port)
 {
 	int i;
@@ -729,9 +737,6 @@ static struct urb *sierra_setup_urb(struct usb_serial *serial, int endpoint,
 	struct urb	*urb;
 	u8		*buf;
 
-	if (endpoint == -1)
-		return NULL;
-
 	urb = usb_alloc_urb(0, mem_flags);
 	if (!urb)
 		return NULL;
@@ -758,40 +763,48 @@ static void sierra_close(struct usb_serial_port *port)
 	int i;
 	struct usb_serial *serial = port->serial;
 	struct sierra_port_private *portdata;
-	struct sierra_intf_private *intfdata = port->serial->private;
+	struct sierra_intf_private *intfdata = usb_get_serial_data(serial);
+	struct urb *urb;
 
 	portdata = usb_get_serial_port_data(port);
 
-	portdata->rts_state = 0;
-	portdata->dtr_state = 0;
-
-	mutex_lock(&serial->disc_mutex);
-	if (!serial->disconnected) {
+	/*
+	 * Need to take susp_lock to make sure port is not already being
+	 * resumed, but no need to hold it due to ASYNC_INITIALIZED.
+	 */
+	spin_lock_irq(&intfdata->susp_lock);
+	if (--intfdata->open_ports == 0)
 		serial->interface->needs_remote_wakeup = 0;
-		/* odd error handling due to pm counters */
-		if (!usb_autopm_get_interface(serial->interface))
-			sierra_send_setup(port);
-		else
-			usb_autopm_get_interface_no_resume(serial->interface);
+	spin_unlock_irq(&intfdata->susp_lock);
 
+	for (;;) {
+		urb = usb_get_from_anchor(&portdata->delayed);
+		if (!urb)
+			break;
+		kfree(urb->transfer_buffer);
+		usb_free_urb(urb);
+		usb_autopm_put_interface_async(serial->interface);
+		spin_lock(&portdata->lock);
+		portdata->outstanding_urbs--;
+		spin_unlock(&portdata->lock);
 	}
-	mutex_unlock(&serial->disc_mutex);
-	spin_lock_irq(&intfdata->susp_lock);
-	portdata->opened = 0;
-	spin_unlock_irq(&intfdata->susp_lock);
 
 	sierra_stop_rx_urbs(port);
+	usb_kill_anchored_urbs(&portdata->active);
+
 	for (i = 0; i < portdata->num_in_urbs; i++) {
 		sierra_release_urb(portdata->in_urbs[i]);
 		portdata->in_urbs[i] = NULL;
 	}
+
+	usb_autopm_get_interface_no_resume(serial->interface);
 }
 
 static int sierra_open(struct tty_struct *tty, struct usb_serial_port *port)
 {
 	struct sierra_port_private *portdata;
 	struct usb_serial *serial = port->serial;
-	struct sierra_intf_private *intfdata = serial->private;
+	struct sierra_intf_private *intfdata = usb_get_serial_data(serial);
 	int i;
 	int err;
 	int endpoint;
@@ -799,11 +812,6 @@ static int sierra_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 	portdata = usb_get_serial_port_data(port);
 
-	/* Set some sane defaults */
-	portdata->rts_state = 1;
-	portdata->dtr_state = 1;
-
-
 	endpoint = port->bulk_in_endpointAddress;
 	for (i = 0; i < portdata->num_in_urbs; i++) {
 		urb = sierra_setup_urb(serial, endpoint, USB_DIR_IN, port,
@@ -816,23 +824,26 @@ static int sierra_open(struct tty_struct *tty, struct usb_serial_port *port)
 			usb_sndbulkpipe(serial->dev, endpoint) | USB_DIR_IN);
 
 	err = sierra_submit_rx_urbs(port, GFP_KERNEL);
-	if (err) {
-		/* get rid of everything as in close */
-		sierra_close(port);
-		/* restore balance for autopm */
-		if (!serial->disconnected)
-			usb_autopm_put_interface(serial->interface);
-		return err;
-	}
-	sierra_send_setup(port);
+	if (err)
+		goto err_submit;
 
-	serial->interface->needs_remote_wakeup = 1;
 	spin_lock_irq(&intfdata->susp_lock);
-	portdata->opened = 1;
+	if (++intfdata->open_ports == 1)
+		serial->interface->needs_remote_wakeup = 1;
 	spin_unlock_irq(&intfdata->susp_lock);
 	usb_autopm_put_interface(serial->interface);
 
 	return 0;
+
+err_submit:
+	sierra_stop_rx_urbs(port);
+
+	for (i = 0; i < portdata->num_in_urbs; i++) {
+		sierra_release_urb(portdata->in_urbs[i]);
+		portdata->in_urbs[i] = NULL;
+	}
+
+	return err;
 }
 
 
@@ -928,6 +939,7 @@ static int sierra_port_remove(struct usb_serial_port *port)
 	struct sierra_port_private *portdata;
 
 	portdata = usb_get_serial_port_data(port);
+	usb_set_serial_port_data(port, NULL);
 	kfree(portdata);
 
 	return 0;
@@ -944,6 +956,8 @@ static void stop_read_write_urbs(struct usb_serial *serial)
 	for (i = 0; i < serial->num_ports; ++i) {
 		port = serial->port[i];
 		portdata = usb_get_serial_port_data(port);
+		if (!portdata)
+			continue;
 		sierra_stop_rx_urbs(port);
 		usb_kill_anchored_urbs(&portdata->active);
 	}
@@ -951,58 +965,84 @@ static void stop_read_write_urbs(struct usb_serial *serial)
 
 static int sierra_suspend(struct usb_serial *serial, pm_message_t message)
 {
-	struct sierra_intf_private *intfdata;
-	int b;
+	struct sierra_intf_private *intfdata = usb_get_serial_data(serial);
 
+	spin_lock_irq(&intfdata->susp_lock);
 	if (PMSG_IS_AUTO(message)) {
-		intfdata = serial->private;
-		spin_lock_irq(&intfdata->susp_lock);
-		b = intfdata->in_flight;
-
-		if (b) {
+		if (intfdata->in_flight) {
 			spin_unlock_irq(&intfdata->susp_lock);
 			return -EBUSY;
-		} else {
-			intfdata->suspended = 1;
-			spin_unlock_irq(&intfdata->susp_lock);
 		}
 	}
+	intfdata->suspended = 1;
+	spin_unlock_irq(&intfdata->susp_lock);
+
 	stop_read_write_urbs(serial);
 
 	return 0;
 }
 
+/* Caller must hold susp_lock. */
+static int sierra_submit_delayed_urbs(struct usb_serial_port *port)
+{
+	struct sierra_port_private *portdata = usb_get_serial_port_data(port);
+	struct sierra_intf_private *intfdata;
+	struct urb *urb;
+	int ec = 0;
+	int err;
+
+	intfdata = usb_get_serial_data(port->serial);
+
+	for (;;) {
+		urb = usb_get_from_anchor(&portdata->delayed);
+		if (!urb)
+			break;
+
+		usb_anchor_urb(urb, &portdata->active);
+		intfdata->in_flight++;
+		err = usb_submit_urb(urb, GFP_ATOMIC);
+		if (err) {
+			dev_err(&port->dev, "%s - submit urb failed: %d",
+					__func__, err);
+			ec++;
+			intfdata->in_flight--;
+			usb_unanchor_urb(urb);
+			kfree(urb->transfer_buffer);
+			usb_free_urb(urb);
+
+			spin_lock(&portdata->lock);
+			portdata->outstanding_urbs--;
+			spin_unlock(&portdata->lock);
+		}
+	}
+
+	if (ec)
+		return -EIO;
+
+	return 0;
+}
+
 static int sierra_resume(struct usb_serial *serial)
 {
 	struct usb_serial_port *port;
-	struct sierra_intf_private *intfdata = serial->private;
-	struct sierra_port_private *portdata;
-	struct urb *urb;
+	struct sierra_intf_private *intfdata = usb_get_serial_data(serial);
 	int ec = 0;
 	int i, err;
 
 	spin_lock_irq(&intfdata->susp_lock);
 	for (i = 0; i < serial->num_ports; i++) {
 		port = serial->port[i];
-		portdata = usb_get_serial_port_data(port);
 
-		while ((urb = usb_get_from_anchor(&portdata->delayed))) {
-			usb_anchor_urb(urb, &portdata->active);
-			intfdata->in_flight++;
-			err = usb_submit_urb(urb, GFP_ATOMIC);
-			if (err < 0) {
-				intfdata->in_flight--;
-				usb_unanchor_urb(urb);
-				usb_scuttle_anchored_urbs(&portdata->delayed);
-				break;
-			}
-		}
+		if (!test_bit(ASYNCB_INITIALIZED, &port->port.flags))
+			continue;
 
-		if (portdata->opened) {
-			err = sierra_submit_rx_urbs(port, GFP_ATOMIC);
-			if (err)
-				ec++;
-		}
+		err = sierra_submit_delayed_urbs(port);
+		if (err)
+			ec++;
+
+		err = sierra_submit_rx_urbs(port, GFP_ATOMIC);
+		if (err)
+			ec++;
 	}
 	intfdata->suspended = 0;
 	spin_unlock_irq(&intfdata->susp_lock);
@@ -1029,7 +1069,7 @@ static struct usb_serial_driver sierra_device = {
 	.dtr_rts	   = sierra_dtr_rts,
 	.write             = sierra_write,
 	.write_room        = sierra_write_room,
-	.set_termios       = sierra_set_termios,
+	.chars_in_buffer   = sierra_chars_in_buffer,
 	.tiocmget          = sierra_tiocmget,
 	.tiocmset          = sierra_tiocmset,
 	.attach            = sierra_startup,
diff --git a/drivers/usb/serial/usb-serial.c b/drivers/usb/serial/usb-serial.c
index 6d40d56378d..02de3110fe9 100644
--- a/drivers/usb/serial/usb-serial.c
+++ b/drivers/usb/serial/usb-serial.c
@@ -1060,6 +1060,7 @@ static void usb_serial_disconnect(struct usb_interface *interface)
 	struct usb_serial *serial = usb_get_intfdata(interface);
 	struct device *dev = &interface->dev;
 	struct usb_serial_port *port;
+	struct tty_struct *tty;
 
 	usb_serial_console_disconnect(serial);
 
@@ -1070,18 +1071,16 @@ static void usb_serial_disconnect(struct usb_interface *interface)
 
 	for (i = 0; i < serial->num_ports; ++i) {
 		port = serial->port[i];
-		if (port) {
-			struct tty_struct *tty = tty_port_tty_get(&port->port);
-			if (tty) {
-				tty_vhangup(tty);
-				tty_kref_put(tty);
-			}
-			usb_serial_port_poison_urbs(port);
-			wake_up_interruptible(&port->port.delta_msr_wait);
-			cancel_work_sync(&port->work);
-			if (device_is_registered(&port->dev))
-				device_del(&port->dev);
+		tty = tty_port_tty_get(&port->port);
+		if (tty) {
+			tty_vhangup(tty);
+			tty_kref_put(tty);
 		}
+		usb_serial_port_poison_urbs(port);
+		wake_up_interruptible(&port->port.delta_msr_wait);
+		cancel_work_sync(&port->work);
+		if (device_is_registered(&port->dev))
+			device_del(&port->dev);
 	}
 	if (serial->type->disconnect)
 		serial->type->disconnect(serial);
@@ -1094,7 +1093,6 @@ static void usb_serial_disconnect(struct usb_interface *interface)
 int usb_serial_suspend(struct usb_interface *intf, pm_message_t message)
 {
 	struct usb_serial *serial = usb_get_intfdata(intf);
-	struct usb_serial_port *port;
 	int i, r = 0;
 
 	serial->suspending = 1;
@@ -1112,12 +1110,8 @@ int usb_serial_suspend(struct usb_interface *intf, pm_message_t message)
 		}
 	}
 
-	for (i = 0; i < serial->num_ports; ++i) {
-		port = serial->port[i];
-		if (port)
-			usb_serial_port_poison_urbs(port);
-	}
-
+	for (i = 0; i < serial->num_ports; ++i)
+		usb_serial_port_poison_urbs(serial->port[i]);
 err_out:
 	return r;
 }
@@ -1125,14 +1119,10 @@ EXPORT_SYMBOL(usb_serial_suspend);
 
 static void usb_serial_unpoison_port_urbs(struct usb_serial *serial)
 {
-	struct usb_serial_port *port;
 	int i;
 
-	for (i = 0; i < serial->num_ports; ++i) {
-		port = serial->port[i];
-		if (port)
-			usb_serial_port_unpoison_urbs(port);
-	}
+	for (i = 0; i < serial->num_ports; ++i)
+		usb_serial_port_unpoison_urbs(serial->port[i]);
 }
 
 int usb_serial_resume(struct usb_interface *intf)
diff --git a/drivers/usb/serial/usb-wwan.h b/drivers/usb/serial/usb-wwan.h
index 684739b8efd..f22dff58b58 100644
--- a/drivers/usb/serial/usb-wwan.h
+++ b/drivers/usb/serial/usb-wwan.h
@@ -11,15 +11,11 @@ extern void usb_wwan_close(struct usb_serial_port *port);
 extern int usb_wwan_port_probe(struct usb_serial_port *port);
 extern int usb_wwan_port_remove(struct usb_serial_port *port);
 extern int usb_wwan_write_room(struct tty_struct *tty);
-extern void usb_wwan_set_termios(struct tty_struct *tty,
-				 struct usb_serial_port *port,
-				 struct ktermios *old);
 extern int usb_wwan_tiocmget(struct tty_struct *tty);
 extern int usb_wwan_tiocmset(struct tty_struct *tty,
 			     unsigned int set, unsigned int clear);
 extern int usb_wwan_ioctl(struct tty_struct *tty,
 			  unsigned int cmd, unsigned long arg);
-extern int usb_wwan_send_setup(struct usb_serial_port *port);
 extern int usb_wwan_write(struct tty_struct *tty, struct usb_serial_port *port,
 			  const unsigned char *buf, int count);
 extern int usb_wwan_chars_in_buffer(struct tty_struct *tty);
@@ -39,6 +35,7 @@ struct usb_wwan_intf_private {
 	spinlock_t susp_lock;
 	unsigned int suspended:1;
 	int in_flight;
+	unsigned int open_ports;
 	int (*send_setup) (struct usb_serial_port *port);
 	void *private;
 };
@@ -51,7 +48,6 @@ struct usb_wwan_port_private {
 	struct urb *out_urbs[N_OUT_URB];
 	u8 *out_buffer[N_OUT_URB];
 	unsigned long out_busy;	/* Bit vector of URBs in use */
-	int opened;
 	struct usb_anchor delayed;
 
 	/* Settings for the port */
diff --git a/drivers/usb/serial/usb_wwan.c b/drivers/usb/serial/usb_wwan.c
index b078440e822..2f805cb386a 100644
--- a/drivers/usb/serial/usb_wwan.c
+++ b/drivers/usb/serial/usb_wwan.c
@@ -41,7 +41,7 @@ void usb_wwan_dtr_rts(struct usb_serial_port *port, int on)
 	struct usb_wwan_port_private *portdata;
 	struct usb_wwan_intf_private *intfdata;
 
-	intfdata = port->serial->private;
+	intfdata = usb_get_serial_data(port->serial);
 
 	if (!intfdata->send_setup)
 		return;
@@ -55,20 +55,6 @@ void usb_wwan_dtr_rts(struct usb_serial_port *port, int on)
 }
 EXPORT_SYMBOL(usb_wwan_dtr_rts);
 
-void usb_wwan_set_termios(struct tty_struct *tty,
-			  struct usb_serial_port *port,
-			  struct ktermios *old_termios)
-{
-	struct usb_wwan_intf_private *intfdata = port->serial->private;
-
-	/* Doesn't support option setting */
-	tty_termios_copy_hw(&tty->termios, old_termios);
-
-	if (intfdata->send_setup)
-		intfdata->send_setup(port);
-}
-EXPORT_SYMBOL(usb_wwan_set_termios);
-
 int usb_wwan_tiocmget(struct tty_struct *tty)
 {
 	struct usb_serial_port *port = tty->driver_data;
@@ -96,7 +82,7 @@ int usb_wwan_tiocmset(struct tty_struct *tty,
 	struct usb_wwan_intf_private *intfdata;
 
 	portdata = usb_get_serial_port_data(port);
-	intfdata = port->serial->private;
+	intfdata = usb_get_serial_data(port->serial);
 
 	if (!intfdata->send_setup)
 		return -EINVAL;
@@ -192,7 +178,6 @@ int usb_wwan_ioctl(struct tty_struct *tty,
 }
 EXPORT_SYMBOL(usb_wwan_ioctl);
 
-/* Write */
 int usb_wwan_write(struct tty_struct *tty, struct usb_serial_port *port,
 		   const unsigned char *buf, int count)
 {
@@ -205,7 +190,7 @@ int usb_wwan_write(struct tty_struct *tty, struct usb_serial_port *port,
 	unsigned long flags;
 
 	portdata = usb_get_serial_port_data(port);
-	intfdata = port->serial->private;
+	intfdata = usb_get_serial_data(port->serial);
 
 	dev_dbg(&port->dev, "%s: write (%d chars)\n", __func__, count);
 
@@ -228,8 +213,10 @@ int usb_wwan_write(struct tty_struct *tty, struct usb_serial_port *port,
 			usb_pipeendpoint(this_urb->pipe), i);
 
 		err = usb_autopm_get_interface_async(port->serial->interface);
-		if (err < 0)
+		if (err < 0) {
+			clear_bit(i, &portdata->out_busy);
 			break;
+		}
 
 		/* send the data */
 		memcpy(this_urb->transfer_buffer, buf, todo);
@@ -244,9 +231,9 @@ int usb_wwan_write(struct tty_struct *tty, struct usb_serial_port *port,
 			spin_unlock_irqrestore(&intfdata->susp_lock, flags);
 			err = usb_submit_urb(this_urb, GFP_ATOMIC);
 			if (err) {
-				dev_dbg(&port->dev,
-					"usb_submit_urb %p (write bulk) failed (%d)\n",
-					this_urb, err);
+				dev_err(&port->dev,
+					"%s: submit urb %d failed: %d\n",
+					__func__, i, err);
 				clear_bit(i, &portdata->out_busy);
 				spin_lock_irqsave(&intfdata->susp_lock, flags);
 				intfdata->in_flight--;
@@ -314,7 +301,7 @@ static void usb_wwan_outdat_callback(struct urb *urb)
 	int i;
 
 	port = urb->context;
-	intfdata = port->serial->private;
+	intfdata = usb_get_serial_data(port->serial);
 
 	usb_serial_port_softint(port);
 	usb_autopm_put_interface_async(port->serial->interface);
@@ -325,7 +312,7 @@ static void usb_wwan_outdat_callback(struct urb *urb)
 
 	for (i = 0; i < N_OUT_URB; ++i) {
 		if (portdata->out_urbs[i] == urb) {
-			smp_mb__before_clear_bit();
+			smp_mb__before_atomic();
 			clear_bit(i, &portdata->out_busy);
 			break;
 		}
@@ -384,7 +371,15 @@ int usb_wwan_open(struct tty_struct *tty, struct usb_serial_port *port)
 	struct urb *urb;
 
 	portdata = usb_get_serial_port_data(port);
-	intfdata = serial->private;
+	intfdata = usb_get_serial_data(serial);
+
+	if (port->interrupt_in_urb) {
+		err = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
+		if (err) {
+			dev_err(&port->dev, "%s: submit int urb failed: %d\n",
+				__func__, err);
+		}
+	}
 
 	/* Start reading from the IN endpoint */
 	for (i = 0; i < N_IN_URB; i++) {
@@ -393,17 +388,15 @@ int usb_wwan_open(struct tty_struct *tty, struct usb_serial_port *port)
 			continue;
 		err = usb_submit_urb(urb, GFP_KERNEL);
 		if (err) {
-			dev_dbg(&port->dev, "%s: submit urb %d failed (%d) %d\n",
-				__func__, i, err, urb->transfer_buffer_length);
+			dev_err(&port->dev,
+				"%s: submit read urb %d failed: %d\n",
+				__func__, i, err);
 		}
 	}
 
-	if (intfdata->send_setup)
-		intfdata->send_setup(port);
-
-	serial->interface->needs_remote_wakeup = 1;
 	spin_lock_irq(&intfdata->susp_lock);
-	portdata->opened = 1;
+	if (++intfdata->open_ports == 1)
+		serial->interface->needs_remote_wakeup = 1;
 	spin_unlock_irq(&intfdata->susp_lock);
 	/* this balances a get in the generic USB serial code */
 	usb_autopm_put_interface(serial->interface);
@@ -412,32 +405,56 @@ int usb_wwan_open(struct tty_struct *tty, struct usb_serial_port *port)
 }
 EXPORT_SYMBOL(usb_wwan_open);
 
+static void unbusy_queued_urb(struct urb *urb,
+					struct usb_wwan_port_private *portdata)
+{
+	int i;
+
+	for (i = 0; i < N_OUT_URB; i++) {
+		if (urb == portdata->out_urbs[i]) {
+			clear_bit(i, &portdata->out_busy);
+			break;
+		}
+	}
+}
+
 void usb_wwan_close(struct usb_serial_port *port)
 {
 	int i;
 	struct usb_serial *serial = port->serial;
 	struct usb_wwan_port_private *portdata;
-	struct usb_wwan_intf_private *intfdata = port->serial->private;
+	struct usb_wwan_intf_private *intfdata = usb_get_serial_data(serial);
+	struct urb *urb;
 
 	portdata = usb_get_serial_port_data(port);
 
-	/* Stop reading/writing urbs */
+	/*
+	 * Need to take susp_lock to make sure port is not already being
+	 * resumed, but no need to hold it due to ASYNC_INITIALIZED.
+	 */
 	spin_lock_irq(&intfdata->susp_lock);
-	portdata->opened = 0;
+	if (--intfdata->open_ports == 0)
+		serial->interface->needs_remote_wakeup = 0;
 	spin_unlock_irq(&intfdata->susp_lock);
 
+	for (;;) {
+		urb = usb_get_from_anchor(&portdata->delayed);
+		if (!urb)
+			break;
+		unbusy_queued_urb(urb, portdata);
+		usb_autopm_put_interface_async(serial->interface);
+	}
+
 	for (i = 0; i < N_IN_URB; i++)
 		usb_kill_urb(portdata->in_urbs[i]);
 	for (i = 0; i < N_OUT_URB; i++)
 		usb_kill_urb(portdata->out_urbs[i]);
+	usb_kill_urb(port->interrupt_in_urb);
 
-	/* balancing - important as an error cannot be handled*/
 	usb_autopm_get_interface_no_resume(serial->interface);
-	serial->interface->needs_remote_wakeup = 0;
 }
 EXPORT_SYMBOL(usb_wwan_close);
 
-/* Helper functions used by usb_wwan_setup_urbs */
 static struct urb *usb_wwan_setup_urb(struct usb_serial_port *port,
 				      int endpoint,
 				      int dir, void *ctx, char *buf, int len,
@@ -450,7 +467,6 @@ static struct urb *usb_wwan_setup_urb(struct usb_serial_port *port,
 	if (!urb)
 		return NULL;
 
-	/* Fill URB using supplied data. */
 	usb_fill_bulk_urb(urb, serial->dev,
 			  usb_sndbulkpipe(serial->dev, endpoint) | dir,
 			  buf, len, callback, ctx);
@@ -463,7 +479,6 @@ int usb_wwan_port_probe(struct usb_serial_port *port)
 	struct usb_wwan_port_private *portdata;
 	struct urb *urb;
 	u8 *buffer;
-	int err;
 	int i;
 
 	if (!port->bulk_in_size || !port->bulk_out_size)
@@ -503,13 +518,6 @@ int usb_wwan_port_probe(struct usb_serial_port *port)
 
 	usb_set_serial_port_data(port, portdata);
 
-	if (port->interrupt_in_urb) {
-		err = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
-		if (err)
-			dev_dbg(&port->dev, "%s: submit irq_in urb failed %d\n",
-				__func__, err);
-	}
-
 	return 0;
 
 bail_out_error2:
@@ -536,32 +544,28 @@ int usb_wwan_port_remove(struct usb_serial_port *port)
 	portdata = usb_get_serial_port_data(port);
 	usb_set_serial_port_data(port, NULL);
 
-	/* Stop reading/writing urbs and free them */
 	for (i = 0; i < N_IN_URB; i++) {
-		usb_kill_urb(portdata->in_urbs[i]);
 		usb_free_urb(portdata->in_urbs[i]);
 		free_page((unsigned long)portdata->in_buffer[i]);
 	}
 	for (i = 0; i < N_OUT_URB; i++) {
-		usb_kill_urb(portdata->out_urbs[i]);
 		usb_free_urb(portdata->out_urbs[i]);
 		kfree(portdata->out_buffer[i]);
 	}
 
-	/* Now free port private data */
 	kfree(portdata);
+
 	return 0;
 }
 EXPORT_SYMBOL(usb_wwan_port_remove);
 
 #ifdef CONFIG_PM
-static void stop_read_write_urbs(struct usb_serial *serial)
+static void stop_urbs(struct usb_serial *serial)
 {
 	int i, j;
 	struct usb_serial_port *port;
 	struct usb_wwan_port_private *portdata;
 
-	/* Stop reading/writing urbs */
 	for (i = 0; i < serial->num_ports; ++i) {
 		port = serial->port[i];
 		portdata = usb_get_serial_port_data(port);
@@ -571,123 +575,117 @@ static void stop_read_write_urbs(struct usb_serial *serial)
 			usb_kill_urb(portdata->in_urbs[j]);
 		for (j = 0; j < N_OUT_URB; j++)
 			usb_kill_urb(portdata->out_urbs[j]);
+		usb_kill_urb(port->interrupt_in_urb);
 	}
 }
 
 int usb_wwan_suspend(struct usb_serial *serial, pm_message_t message)
 {
-	struct usb_wwan_intf_private *intfdata = serial->private;
-	int b;
+	struct usb_wwan_intf_private *intfdata = usb_get_serial_data(serial);
 
+	spin_lock_irq(&intfdata->susp_lock);
 	if (PMSG_IS_AUTO(message)) {
-		spin_lock_irq(&intfdata->susp_lock);
-		b = intfdata->in_flight;
-		spin_unlock_irq(&intfdata->susp_lock);
-
-		if (b)
+		if (intfdata->in_flight) {
+			spin_unlock_irq(&intfdata->susp_lock);
 			return -EBUSY;
+		}
 	}
-
-	spin_lock_irq(&intfdata->susp_lock);
 	intfdata->suspended = 1;
 	spin_unlock_irq(&intfdata->susp_lock);
-	stop_read_write_urbs(serial);
+
+	stop_urbs(serial);
 
 	return 0;
 }
 EXPORT_SYMBOL(usb_wwan_suspend);
 
-static void unbusy_queued_urb(struct urb *urb, struct usb_wwan_port_private *portdata)
+/* Caller must hold susp_lock. */
+static int usb_wwan_submit_delayed_urbs(struct usb_serial_port *port)
 {
-	int i;
-
-	for (i = 0; i < N_OUT_URB; i++) {
-		if (urb == portdata->out_urbs[i]) {
-			clear_bit(i, &portdata->out_busy);
-			break;
-		}
-	}
-}
-
-static void play_delayed(struct usb_serial_port *port)
-{
-	struct usb_wwan_intf_private *data;
+	struct usb_serial *serial = port->serial;
+	struct usb_wwan_intf_private *data = usb_get_serial_data(serial);
 	struct usb_wwan_port_private *portdata;
 	struct urb *urb;
+	int err_count = 0;
 	int err;
 
 	portdata = usb_get_serial_port_data(port);
-	data = port->serial->private;
-	while ((urb = usb_get_from_anchor(&portdata->delayed))) {
-		err = usb_submit_urb(urb, GFP_ATOMIC);
-		if (!err) {
-			data->in_flight++;
-		} else {
-			/* we have to throw away the rest */
-			do {
-				unbusy_queued_urb(urb, portdata);
-				usb_autopm_put_interface_no_suspend(port->serial->interface);
-			} while ((urb = usb_get_from_anchor(&portdata->delayed)));
+
+	for (;;) {
+		urb = usb_get_from_anchor(&portdata->delayed);
+		if (!urb)
 			break;
+
+		err = usb_submit_urb(urb, GFP_ATOMIC);
+		if (err) {
+			dev_err(&port->dev, "%s: submit urb failed: %d\n",
+					__func__, err);
+			err_count++;
+			unbusy_queued_urb(urb, portdata);
+			usb_autopm_put_interface_async(serial->interface);
+			continue;
 		}
+		data->in_flight++;
 	}
+
+	if (err_count)
+		return -EIO;
+
+	return 0;
 }
 
 int usb_wwan_resume(struct usb_serial *serial)
 {
 	int i, j;
 	struct usb_serial_port *port;
-	struct usb_wwan_intf_private *intfdata = serial->private;
+	struct usb_wwan_intf_private *intfdata = usb_get_serial_data(serial);
 	struct usb_wwan_port_private *portdata;
 	struct urb *urb;
-	int err = 0;
+	int err;
+	int err_count = 0;
 
-	/* get the interrupt URBs resubmitted unconditionally */
+	spin_lock_irq(&intfdata->susp_lock);
 	for (i = 0; i < serial->num_ports; i++) {
 		port = serial->port[i];
-		if (!port->interrupt_in_urb) {
-			dev_dbg(&port->dev, "%s: No interrupt URB for port\n", __func__);
+
+		if (!test_bit(ASYNCB_INITIALIZED, &port->port.flags))
 			continue;
-		}
-		err = usb_submit_urb(port->interrupt_in_urb, GFP_NOIO);
-		dev_dbg(&port->dev, "Submitted interrupt URB for port (result %d)\n", err);
-		if (err < 0) {
-			dev_err(&port->dev, "%s: Error %d for interrupt URB\n",
-				__func__, err);
-			goto err_out;
-		}
-	}
 
-	for (i = 0; i < serial->num_ports; i++) {
-		/* walk all ports */
-		port = serial->port[i];
 		portdata = usb_get_serial_port_data(port);
 
-		/* skip closed ports */
-		spin_lock_irq(&intfdata->susp_lock);
-		if (!portdata || !portdata->opened) {
-			spin_unlock_irq(&intfdata->susp_lock);
-			continue;
+		if (port->interrupt_in_urb) {
+			err = usb_submit_urb(port->interrupt_in_urb,
+					GFP_ATOMIC);
+			if (err) {
+				dev_err(&port->dev,
+					"%s: submit int urb failed: %d\n",
+					__func__, err);
+				err_count++;
+			}
 		}
 
+		err = usb_wwan_submit_delayed_urbs(port);
+		if (err)
+			err_count++;
+
 		for (j = 0; j < N_IN_URB; j++) {
 			urb = portdata->in_urbs[j];
 			err = usb_submit_urb(urb, GFP_ATOMIC);
 			if (err < 0) {
-				dev_err(&port->dev, "%s: Error %d for bulk URB %d\n",
-					__func__, err, i);
-				spin_unlock_irq(&intfdata->susp_lock);
-				goto err_out;
+				dev_err(&port->dev,
+					"%s: submit read urb %d failed: %d\n",
+					__func__, i, err);
+				err_count++;
 			}
 		}
-		play_delayed(port);
-		spin_unlock_irq(&intfdata->susp_lock);
 	}
-	spin_lock_irq(&intfdata->susp_lock);
 	intfdata->suspended = 0;
 	spin_unlock_irq(&intfdata->susp_lock);
-err_out:
-	return err;
+
+	if (err_count)
+		return -EIO;
+
+	return 0;
 }
 EXPORT_SYMBOL(usb_wwan_resume);
 #endif
diff --git a/drivers/usb/storage/ene_ub6250.c b/drivers/usb/storage/ene_ub6250.c
index 1bfc9a6cab5..ef6efb55dc3 100644
--- a/drivers/usb/storage/ene_ub6250.c
+++ b/drivers/usb/storage/ene_ub6250.c
@@ -1928,11 +1928,10 @@ static int ene_load_bincode(struct us_data *us, unsigned char flag)
 		usb_stor_dbg(us, "load firmware %s failed\n", fw_name);
 		goto nofw;
 	}
-	buf = kmalloc(sd_fw->size, GFP_KERNEL);
+	buf = kmemdup(sd_fw->data, sd_fw->size, GFP_KERNEL);
 	if (buf == NULL)
 		goto nofw;
 
-	memcpy(buf, sd_fw->data, sd_fw->size);
 	memset(bcb, 0, sizeof(struct bulk_cb_wrap));
 	bcb->Signature = cpu_to_le32(US_BULK_CB_SIGN);
 	bcb->DataTransferLength = sd_fw->size;
diff --git a/drivers/usb/wusbcore/wa-rpipe.c b/drivers/usb/wusbcore/wa-rpipe.c
index 6d6da127f6d..a80c5d284b5 100644
--- a/drivers/usb/wusbcore/wa-rpipe.c
+++ b/drivers/usb/wusbcore/wa-rpipe.c
@@ -524,7 +524,7 @@ void rpipe_ep_disable(struct wahc *wa, struct usb_host_endpoint *ep)
 		u16 index = le16_to_cpu(rpipe->descr.wRPipeIndex);
 
 		usb_control_msg(
-			wa->usb_dev, usb_rcvctrlpipe(wa->usb_dev, 0),
+			wa->usb_dev, usb_sndctrlpipe(wa->usb_dev, 0),
 			USB_REQ_RPIPE_ABORT,
 			USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_RPIPE,
 			0, index, NULL, 0, USB_CTRL_SET_TIMEOUT);
@@ -545,7 +545,7 @@ void rpipe_clear_feature_stalled(struct wahc *wa, struct usb_host_endpoint *ep)
 		u16 index = le16_to_cpu(rpipe->descr.wRPipeIndex);
 
 		usb_control_msg(
-			wa->usb_dev, usb_rcvctrlpipe(wa->usb_dev, 0),
+			wa->usb_dev, usb_sndctrlpipe(wa->usb_dev, 0),
 			USB_REQ_CLEAR_FEATURE,
 			USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_RPIPE,
 			RPIPE_STALL, index, NULL, 0, USB_CTRL_SET_TIMEOUT);
diff --git a/drivers/uwb/beacon.c b/drivers/uwb/beacon.c
index 57b5ff61020..f40745fcbd3 100644
--- a/drivers/uwb/beacon.c
+++ b/drivers/uwb/beacon.c
@@ -125,8 +125,10 @@ int uwb_rc_beacon(struct uwb_rc *rc, int channel, unsigned bpst_offset)
 	else {
 		/* channel >= 0...dah */
 		result = uwb_rc_start_beacon(rc, bpst_offset, channel);
-		if (result < 0)
+		if (result < 0) {
+			dev_err(dev, "Cannot start beaconing: %d\n", result);
 			return result;
+		}
 		if (le16_to_cpu(rc->ies->wIELength) > 0) {
 			result = uwb_rc_set_ie(rc, rc->ies);
 			if (result < 0) {
@@ -395,7 +397,6 @@ int uwbd_evt_handle_rc_beacon(struct uwb_event *evt)
 	struct uwb_rc_evt_beacon *be;
 	struct uwb_beacon_frame *bf;
 	struct uwb_beca_e *bce;
-	unsigned long last_ts;
 
 	rc = evt->rc;
 	be = container_of(evt->notif.rceb, struct uwb_rc_evt_beacon, rceb);
@@ -439,8 +440,6 @@ int uwbd_evt_handle_rc_beacon(struct uwb_event *evt)
 	/* purge old beacon data */
 	kfree(bce->be);
 
-	last_ts = bce->ts_jiffies;
-
 	/* Update commonly used fields */
 	bce->ts_jiffies = evt->ts_jiffies;
 	bce->be = be;
diff --git a/drivers/uwb/drp.c b/drivers/uwb/drp.c
index 468c89fb6a1..05b7bd76225 100644
--- a/drivers/uwb/drp.c
+++ b/drivers/uwb/drp.c
@@ -29,19 +29,19 @@
 enum uwb_drp_conflict_action {
 	/* Reservation is maintained, no action needed */
 	UWB_DRP_CONFLICT_MANTAIN = 0,
-	
+
 	/* the device shall not transmit frames in conflicting MASs in
 	 * the following superframe. If the device is the reservation
 	 * target, it shall also set the Reason Code in its DRP IE to
 	 * Conflict in its beacon in the following superframe.
 	 */
 	UWB_DRP_CONFLICT_ACT1,
-	
+
 	/* the device shall not set the Reservation Status bit to ONE
 	 * and shall not transmit frames in conflicting MASs. If the
 	 * device is the reservation target, it shall also set the
 	 * Reason Code in its DRP IE to Conflict.
-	 */	
+	 */
 	UWB_DRP_CONFLICT_ACT2,
 
 	/* the device shall not transmit frames in conflicting MASs in
@@ -115,7 +115,8 @@ int uwb_rc_send_all_drp_ie(struct uwb_rc *rc)
 			if (uwb_rsv_has_two_drp_ies(rsv) &&
 				(rsv->mv.companion_drp_ie != NULL)) {
 				mv = &rsv->mv;
-				num_bytes += mv->companion_drp_ie->hdr.length + 2;	
+				num_bytes +=
+					mv->companion_drp_ie->hdr.length + 2;
 			}
 		}
 	}
@@ -139,21 +140,23 @@ int uwb_rc_send_all_drp_ie(struct uwb_rc *rc)
 			memcpy(IEDataptr, rsv->drp_ie,
 			       rsv->drp_ie->hdr.length + 2);
 			IEDataptr += rsv->drp_ie->hdr.length + 2;
-			
+
 			if (uwb_rsv_has_two_drp_ies(rsv) &&
 				(rsv->mv.companion_drp_ie != NULL)) {
 				mv = &rsv->mv;
 				memcpy(IEDataptr, mv->companion_drp_ie,
 				       mv->companion_drp_ie->hdr.length + 2);
-				IEDataptr += mv->companion_drp_ie->hdr.length + 2;	
+				IEDataptr +=
+					mv->companion_drp_ie->hdr.length + 2;
 			}
 		}
 	}
 
-	result = uwb_rc_cmd_async(rc, "SET-DRP-IE", &cmd->rccb, sizeof(*cmd) + num_bytes,
-				  UWB_RC_CET_GENERAL, UWB_RC_CMD_SET_DRP_IE,
-				  uwb_rc_set_drp_cmd_done, NULL);
-	
+	result = uwb_rc_cmd_async(rc, "SET-DRP-IE",
+				&cmd->rccb, sizeof(*cmd) + num_bytes,
+				UWB_RC_CET_GENERAL, UWB_RC_CMD_SET_DRP_IE,
+				uwb_rc_set_drp_cmd_done, NULL);
+
 	rc->set_drp_ie_pending = 1;
 
 	kfree(cmd);
@@ -176,8 +179,8 @@ static int evaluate_conflict_action(struct uwb_ie_drp *ext_drp_ie, int ext_beaco
 	int ext_tie_breaker = uwb_ie_drp_tiebreaker(ext_drp_ie);
 	int ext_status      = uwb_ie_drp_status(ext_drp_ie);
 	int ext_type        = uwb_ie_drp_type(ext_drp_ie);
-	
-	
+
+
 	/* [ECMA-368 2nd Edition] 17.4.6 */
 	if (ext_type == UWB_DRP_TYPE_PCA && our_type == UWB_DRP_TYPE_PCA) {
 		return UWB_DRP_CONFLICT_MANTAIN;
@@ -187,7 +190,7 @@ static int evaluate_conflict_action(struct uwb_ie_drp *ext_drp_ie, int ext_beaco
 	if (our_type == UWB_DRP_TYPE_ALIEN_BP) {
 		return UWB_DRP_CONFLICT_MANTAIN;
 	}
-	
+
 	/* [ECMA-368 2nd Edition] 17.4.6-2 */
 	if (ext_type == UWB_DRP_TYPE_ALIEN_BP) {
 		/* here we know our_type != UWB_DRP_TYPE_ALIEN_BP */
@@ -215,7 +218,7 @@ static int evaluate_conflict_action(struct uwb_ie_drp *ext_drp_ie, int ext_beaco
 	    our_beacon_slot >  ext_beacon_slot) {
 		return UWB_DRP_CONFLICT_MANTAIN;
 	}
-	
+
 	if (our_status == 0) {
 		if (our_tie_breaker == ext_tie_breaker) {
 			/* [ECMA-368 2nd Edition] 17.4.6-6a */
@@ -244,9 +247,9 @@ static int evaluate_conflict_action(struct uwb_ie_drp *ext_drp_ie, int ext_beaco
 	return UWB_DRP_CONFLICT_MANTAIN;
 }
 
-static void handle_conflict_normal(struct uwb_ie_drp *drp_ie, 
-				   int ext_beacon_slot, 
-				   struct uwb_rsv *rsv, 
+static void handle_conflict_normal(struct uwb_ie_drp *drp_ie,
+				   int ext_beacon_slot,
+				   struct uwb_rsv *rsv,
 				   struct uwb_mas_bm *conflicting_mas)
 {
 	struct uwb_rc *rc = rsv->rc;
@@ -263,7 +266,7 @@ static void handle_conflict_normal(struct uwb_ie_drp *drp_ie,
 			uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_TO_BE_MOVED);
 			if (bow->can_reserve_extra_mases == false)
 				uwb_rsv_backoff_win_increment(rc);
-			
+
 			break;
 		case UWB_DRP_CONFLICT_ACT3:
 			uwb_rsv_backoff_win_increment(rc);
@@ -278,13 +281,13 @@ static void handle_conflict_normal(struct uwb_ie_drp *drp_ie,
 		switch(action) {
 		case UWB_DRP_CONFLICT_ACT2:
 		case UWB_DRP_CONFLICT_ACT3:
-			uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_CONFLICT);	
+			uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_CONFLICT);
 		default:
 			break;
 		}
 
 	}
-	
+
 }
 
 static void handle_conflict_expanding(struct uwb_ie_drp *drp_ie, int ext_beacon_slot,
@@ -295,7 +298,7 @@ static void handle_conflict_expanding(struct uwb_ie_drp *drp_ie, int ext_beacon_
 	struct uwb_drp_backoff_win *bow = &rc->bow;
 	struct uwb_rsv_move *mv = &rsv->mv;
 	int action;
-	
+
 	if (companion_only) {
 		/* status of companion is 0 at this point */
 		action = evaluate_conflict_action(drp_ie, ext_beacon_slot, rsv, 0);
@@ -303,21 +306,24 @@ static void handle_conflict_expanding(struct uwb_ie_drp *drp_ie, int ext_beacon_
 			switch(action) {
 			case UWB_DRP_CONFLICT_ACT2:
 			case UWB_DRP_CONFLICT_ACT3:
-				uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_ESTABLISHED);
+				uwb_rsv_set_state(rsv,
+						UWB_RSV_STATE_O_ESTABLISHED);
 				rsv->needs_release_companion_mas = false;
 				if (bow->can_reserve_extra_mases == false)
 					uwb_rsv_backoff_win_increment(rc);
-				uwb_drp_avail_release(rsv->rc, &rsv->mv.companion_mas);
+				uwb_drp_avail_release(rsv->rc,
+						&rsv->mv.companion_mas);
 			}
-		} else { /* rsv is target */			
+		} else { /* rsv is target */
 			switch(action) {
 			case UWB_DRP_CONFLICT_ACT2:
 			case UWB_DRP_CONFLICT_ACT3:
-				uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_EXPANDING_CONFLICT);
+				uwb_rsv_set_state(rsv,
+					UWB_RSV_STATE_T_EXPANDING_CONFLICT);
                                 /* send_drp_avail_ie = true; */
 			}
 		}
-	} else { /* also base part of the reservation is conflicting */		
+	} else { /* also base part of the reservation is conflicting */
 		if (uwb_rsv_is_owner(rsv)) {
 			uwb_rsv_backoff_win_increment(rc);
 			/* remove companion part */
@@ -326,7 +332,8 @@ static void handle_conflict_expanding(struct uwb_ie_drp *drp_ie, int ext_beacon_
 			/* drop some mases with reason modified */
 
 			/* put in the companion the mases to be dropped */
-			bitmap_andnot(mv->companion_mas.bm, rsv->mas.bm, conflicting_mas->bm, UWB_NUM_MAS);
+			bitmap_andnot(mv->companion_mas.bm, rsv->mas.bm,
+					conflicting_mas->bm, UWB_NUM_MAS);
 			uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MODIFIED);
 		} else { /* it is a target rsv */
 			uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_CONFLICT);
@@ -336,7 +343,7 @@ static void handle_conflict_expanding(struct uwb_ie_drp *drp_ie, int ext_beacon_
 }
 
 static void uwb_drp_handle_conflict_rsv(struct uwb_rc *rc, struct uwb_rsv *rsv,
-					struct uwb_rc_evt_drp *drp_evt, 
+					struct uwb_rc_evt_drp *drp_evt,
 					struct uwb_ie_drp *drp_ie,
 					struct uwb_mas_bm *conflicting_mas)
 {
@@ -345,83 +352,107 @@ static void uwb_drp_handle_conflict_rsv(struct uwb_rc *rc, struct uwb_rsv *rsv,
 	/* check if the conflicting reservation has two drp_ies */
 	if (uwb_rsv_has_two_drp_ies(rsv)) {
 		mv = &rsv->mv;
-		if (bitmap_intersects(rsv->mas.bm, conflicting_mas->bm, UWB_NUM_MAS)) {
-			handle_conflict_expanding(drp_ie, drp_evt->beacon_slot_number,
-						  rsv, false, conflicting_mas);
+		if (bitmap_intersects(rsv->mas.bm, conflicting_mas->bm,
+								UWB_NUM_MAS)) {
+			handle_conflict_expanding(drp_ie,
+						drp_evt->beacon_slot_number,
+						rsv, false, conflicting_mas);
 		} else {
-			if (bitmap_intersects(mv->companion_mas.bm, conflicting_mas->bm, UWB_NUM_MAS)) {
-				handle_conflict_expanding(drp_ie, drp_evt->beacon_slot_number,
-							  rsv, true, conflicting_mas);	
+			if (bitmap_intersects(mv->companion_mas.bm,
+					conflicting_mas->bm, UWB_NUM_MAS)) {
+				handle_conflict_expanding(
+					drp_ie, drp_evt->beacon_slot_number,
+					rsv, true, conflicting_mas);
 			}
 		}
-	} else if (bitmap_intersects(rsv->mas.bm, conflicting_mas->bm, UWB_NUM_MAS)) {
-		handle_conflict_normal(drp_ie, drp_evt->beacon_slot_number, rsv, conflicting_mas);
+	} else if (bitmap_intersects(rsv->mas.bm, conflicting_mas->bm,
+							UWB_NUM_MAS)) {
+		handle_conflict_normal(drp_ie, drp_evt->beacon_slot_number,
+					rsv, conflicting_mas);
 	}
 }
 
 static void uwb_drp_handle_all_conflict_rsv(struct uwb_rc *rc,
-					    struct uwb_rc_evt_drp *drp_evt, 
+					    struct uwb_rc_evt_drp *drp_evt,
 					    struct uwb_ie_drp *drp_ie,
 					    struct uwb_mas_bm *conflicting_mas)
 {
 	struct uwb_rsv *rsv;
-	
+
 	list_for_each_entry(rsv, &rc->reservations, rc_node) {
-		uwb_drp_handle_conflict_rsv(rc, rsv, drp_evt, drp_ie, conflicting_mas);	
+		uwb_drp_handle_conflict_rsv(rc, rsv, drp_evt, drp_ie,
+							conflicting_mas);
+	}
+}
+
+static void uwb_drp_process_target_accepted(struct uwb_rc *rc,
+	struct uwb_rsv *rsv, struct uwb_rc_evt_drp *drp_evt,
+	struct uwb_ie_drp *drp_ie, struct uwb_mas_bm *mas)
+{
+	struct uwb_rsv_move *mv = &rsv->mv;
+	int status;
+
+	status = uwb_ie_drp_status(drp_ie);
+
+	if (rsv->state == UWB_RSV_STATE_T_CONFLICT) {
+		uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_CONFLICT);
+		return;
+	}
+
+	if (rsv->state == UWB_RSV_STATE_T_EXPANDING_ACCEPTED) {
+		/* drp_ie is companion */
+		if (!bitmap_equal(rsv->mas.bm, mas->bm, UWB_NUM_MAS)) {
+			/* stroke companion */
+			uwb_rsv_set_state(rsv,
+				UWB_RSV_STATE_T_EXPANDING_ACCEPTED);
+		}
+	} else {
+		if (!bitmap_equal(rsv->mas.bm, mas->bm, UWB_NUM_MAS)) {
+			if (uwb_drp_avail_reserve_pending(rc, mas) == -EBUSY) {
+				/* FIXME: there is a conflict, find
+				 * the conflicting reservations and
+				 * take a sensible action. Consider
+				 * that in drp_ie there is the
+				 * "neighbour" */
+				uwb_drp_handle_all_conflict_rsv(rc, drp_evt,
+						drp_ie, mas);
+			} else {
+				/* accept the extra reservation */
+				bitmap_copy(mv->companion_mas.bm, mas->bm,
+								UWB_NUM_MAS);
+				uwb_rsv_set_state(rsv,
+					UWB_RSV_STATE_T_EXPANDING_ACCEPTED);
+			}
+		} else {
+			if (status) {
+				uwb_rsv_set_state(rsv,
+						UWB_RSV_STATE_T_ACCEPTED);
+			}
+		}
+
 	}
 }
-	
+
 /*
  * Based on the DRP IE, transition a target reservation to a new
  * state.
  */
 static void uwb_drp_process_target(struct uwb_rc *rc, struct uwb_rsv *rsv,
-				   struct uwb_ie_drp *drp_ie, struct uwb_rc_evt_drp *drp_evt)
+		   struct uwb_ie_drp *drp_ie, struct uwb_rc_evt_drp *drp_evt)
 {
 	struct device *dev = &rc->uwb_dev.dev;
 	struct uwb_rsv_move *mv = &rsv->mv;
 	int status;
 	enum uwb_drp_reason reason_code;
 	struct uwb_mas_bm mas;
-	
+
 	status = uwb_ie_drp_status(drp_ie);
 	reason_code = uwb_ie_drp_reason_code(drp_ie);
 	uwb_drp_ie_to_bm(&mas, drp_ie);
 
 	switch (reason_code) {
 	case UWB_DRP_REASON_ACCEPTED:
-
-		if (rsv->state == UWB_RSV_STATE_T_CONFLICT) {
-			uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_CONFLICT);
-			break;
-		}
-
-		if (rsv->state == UWB_RSV_STATE_T_EXPANDING_ACCEPTED) {
-			/* drp_ie is companion */
-			if (!bitmap_equal(rsv->mas.bm, mas.bm, UWB_NUM_MAS))
-				/* stroke companion */
-				uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_EXPANDING_ACCEPTED);	
-		} else {
-			if (!bitmap_equal(rsv->mas.bm, mas.bm, UWB_NUM_MAS)) {
-				if (uwb_drp_avail_reserve_pending(rc, &mas) == -EBUSY) {
-					/* FIXME: there is a conflict, find
-					 * the conflicting reservations and
-					 * take a sensible action. Consider
-					 * that in drp_ie there is the
-					 * "neighbour" */
-					uwb_drp_handle_all_conflict_rsv(rc, drp_evt, drp_ie, &mas);
-				} else {
-					/* accept the extra reservation */
-					bitmap_copy(mv->companion_mas.bm, mas.bm, UWB_NUM_MAS);
-					uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_EXPANDING_ACCEPTED);
-				}
-			} else {
-				if (status) {
-					uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_ACCEPTED);
-				}
-			}
-			
-		}
+		uwb_drp_process_target_accepted(rc, rsv, drp_evt, drp_ie, &mas);
 		break;
 
 	case UWB_DRP_REASON_MODIFIED:
@@ -434,7 +465,8 @@ static void uwb_drp_process_target(struct uwb_rc *rc, struct uwb_rsv *rsv,
 		/* find if the owner wants to expand or reduce */
 		if (bitmap_subset(mas.bm, rsv->mas.bm, UWB_NUM_MAS)) {
 			/* owner is reducing */
-			bitmap_andnot(mv->companion_mas.bm, rsv->mas.bm, mas.bm, UWB_NUM_MAS);
+			bitmap_andnot(mv->companion_mas.bm, rsv->mas.bm, mas.bm,
+				UWB_NUM_MAS);
 			uwb_drp_avail_release(rsv->rc, &mv->companion_mas);
 		}
 
@@ -447,6 +479,48 @@ static void uwb_drp_process_target(struct uwb_rc *rc, struct uwb_rsv *rsv,
 	}
 }
 
+static void uwb_drp_process_owner_accepted(struct uwb_rsv *rsv,
+						struct uwb_mas_bm *mas)
+{
+	struct uwb_rsv_move *mv = &rsv->mv;
+
+	switch (rsv->state) {
+	case UWB_RSV_STATE_O_PENDING:
+	case UWB_RSV_STATE_O_INITIATED:
+	case UWB_RSV_STATE_O_ESTABLISHED:
+		uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_ESTABLISHED);
+		break;
+	case UWB_RSV_STATE_O_MODIFIED:
+		if (bitmap_equal(mas->bm, rsv->mas.bm, UWB_NUM_MAS))
+			uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_ESTABLISHED);
+		else
+			uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MODIFIED);
+		break;
+
+	case UWB_RSV_STATE_O_MOVE_REDUCING: /* shouldn' t be a problem */
+		if (bitmap_equal(mas->bm, rsv->mas.bm, UWB_NUM_MAS))
+			uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_ESTABLISHED);
+		else
+			uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_REDUCING);
+		break;
+	case UWB_RSV_STATE_O_MOVE_EXPANDING:
+		if (bitmap_equal(mas->bm, mv->companion_mas.bm, UWB_NUM_MAS)) {
+			/* Companion reservation accepted */
+			uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_COMBINING);
+		} else {
+			uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_EXPANDING);
+		}
+		break;
+	case UWB_RSV_STATE_O_MOVE_COMBINING:
+		if (bitmap_equal(mas->bm, rsv->mas.bm, UWB_NUM_MAS))
+			uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_REDUCING);
+		else
+			uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_COMBINING);
+		break;
+	default:
+		break;
+	}
+}
 /*
  * Based on the DRP IE, transition an owner reservation to a new
  * state.
@@ -456,7 +530,6 @@ static void uwb_drp_process_owner(struct uwb_rc *rc, struct uwb_rsv *rsv,
 				  struct uwb_rc_evt_drp *drp_evt)
 {
 	struct device *dev = &rc->uwb_dev.dev;
-	struct uwb_rsv_move *mv = &rsv->mv;
 	int status;
 	enum uwb_drp_reason reason_code;
 	struct uwb_mas_bm mas;
@@ -468,44 +541,7 @@ static void uwb_drp_process_owner(struct uwb_rc *rc, struct uwb_rsv *rsv,
 	if (status) {
 		switch (reason_code) {
 		case UWB_DRP_REASON_ACCEPTED:
-			switch (rsv->state) {
-			case UWB_RSV_STATE_O_PENDING:
-			case UWB_RSV_STATE_O_INITIATED:
-			case UWB_RSV_STATE_O_ESTABLISHED:
-				uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_ESTABLISHED);
-				break;
-			case UWB_RSV_STATE_O_MODIFIED:
-				if (bitmap_equal(mas.bm, rsv->mas.bm, UWB_NUM_MAS)) {
-					uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_ESTABLISHED);
-				} else {
-					uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MODIFIED);	
-				}
-				break;
-				
-			case UWB_RSV_STATE_O_MOVE_REDUCING: /* shouldn' t be a problem */
-				if (bitmap_equal(mas.bm, rsv->mas.bm, UWB_NUM_MAS)) {
-					uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_ESTABLISHED);
-				} else {
-					uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_REDUCING);	
-				}
-				break;
-			case UWB_RSV_STATE_O_MOVE_EXPANDING:
-				if (bitmap_equal(mas.bm, mv->companion_mas.bm, UWB_NUM_MAS)) {
-					/* Companion reservation accepted */
-					uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_COMBINING);
-				} else {
-					uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_EXPANDING);
-				}
-				break;
-			case UWB_RSV_STATE_O_MOVE_COMBINING:
-				if (bitmap_equal(mas.bm, rsv->mas.bm, UWB_NUM_MAS))
-					uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_REDUCING);
-				else
-					uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_COMBINING);
-				break;
-			default:
-				break;	
-			}
+			uwb_drp_process_owner_accepted(rsv, &mas);
 			break;
 		default:
 			dev_warn(dev, "ignoring invalid DRP IE state (%d/%d)\n",
@@ -545,9 +581,9 @@ static void uwb_cnflt_update_work(struct work_struct *work)
 						     cnflt_update_work);
 	struct uwb_cnflt_alien *c;
 	struct uwb_rc *rc = cnflt->rc;
-	
+
 	unsigned long delay_us = UWB_MAS_LENGTH_US * UWB_MAS_PER_ZONE;
-	
+
 	mutex_lock(&rc->rsvs_mutex);
 
 	list_del(&cnflt->rc_node);
@@ -556,10 +592,12 @@ static void uwb_cnflt_update_work(struct work_struct *work)
 	bitmap_zero(rc->cnflt_alien_bitmap.bm, UWB_NUM_MAS);
 
 	list_for_each_entry(c, &rc->cnflt_alien_list, rc_node) {
-		bitmap_or(rc->cnflt_alien_bitmap.bm, rc->cnflt_alien_bitmap.bm, c->mas.bm, UWB_NUM_MAS);			
+		bitmap_or(rc->cnflt_alien_bitmap.bm, rc->cnflt_alien_bitmap.bm,
+						c->mas.bm, UWB_NUM_MAS);
 	}
-	
-	queue_delayed_work(rc->rsv_workq, &rc->rsv_alien_bp_work, usecs_to_jiffies(delay_us));
+
+	queue_delayed_work(rc->rsv_workq, &rc->rsv_alien_bp_work,
+					usecs_to_jiffies(delay_us));
 
 	kfree(cnflt);
 	mutex_unlock(&rc->rsvs_mutex);
@@ -583,10 +621,10 @@ static void uwb_drp_handle_alien_drp(struct uwb_rc *rc, struct uwb_ie_drp *drp_i
 	struct uwb_cnflt_alien *cnflt;
 	char buf[72];
 	unsigned long delay_us = UWB_MAS_LENGTH_US * UWB_MAS_PER_ZONE;
-	
+
 	uwb_drp_ie_to_bm(&mas, drp_ie);
 	bitmap_scnprintf(buf, sizeof(buf), mas.bm, UWB_NUM_MAS);
-	
+
 	list_for_each_entry(cnflt, &rc->cnflt_alien_list, rc_node) {
 		if (bitmap_equal(cnflt->mas.bm, mas.bm, UWB_NUM_MAS)) {
 			/* Existing alien BP reservation conflicting
@@ -612,7 +650,7 @@ static void uwb_drp_handle_alien_drp(struct uwb_rc *rc, struct uwb_ie_drp *drp_i
 
 	cnflt->rc = rc;
 	INIT_WORK(&cnflt->cnflt_update_work, uwb_cnflt_update_work);
-	
+
 	bitmap_copy(cnflt->mas.bm, mas.bm, UWB_NUM_MAS);
 
 	list_add_tail(&cnflt->rc_node, &rc->cnflt_alien_list);
@@ -621,17 +659,17 @@ static void uwb_drp_handle_alien_drp(struct uwb_rc *rc, struct uwb_ie_drp *drp_i
 	bitmap_or(rc->cnflt_alien_bitmap.bm, rc->cnflt_alien_bitmap.bm, mas.bm, UWB_NUM_MAS);
 
 	queue_delayed_work(rc->rsv_workq, &rc->rsv_alien_bp_work, usecs_to_jiffies(delay_us));
-	
+
 	/* start the timer */
 	uwb_cnflt_alien_stroke_timer(cnflt);
 }
 
 static void uwb_drp_process_not_involved(struct uwb_rc *rc,
-					 struct uwb_rc_evt_drp *drp_evt, 
+					 struct uwb_rc_evt_drp *drp_evt,
 					 struct uwb_ie_drp *drp_ie)
 {
 	struct uwb_mas_bm mas;
-	
+
 	uwb_drp_ie_to_bm(&mas, drp_ie);
 	uwb_drp_handle_all_conflict_rsv(rc, drp_evt, drp_ie, &mas);
 }
@@ -651,7 +689,7 @@ static void uwb_drp_process_involved(struct uwb_rc *rc, struct uwb_dev *src,
 		 */
 		return;
 	}
-	
+
 	/*
 	 * Do nothing with DRP IEs for reservations that have been
 	 * terminated.
@@ -660,12 +698,12 @@ static void uwb_drp_process_involved(struct uwb_rc *rc, struct uwb_dev *src,
 		uwb_rsv_set_state(rsv, UWB_RSV_STATE_NONE);
 		return;
 	}
-			
+
 	if (uwb_ie_drp_owner(drp_ie))
 		uwb_drp_process_target(rc, rsv, drp_ie, drp_evt);
 	else
 		uwb_drp_process_owner(rc, rsv, src, drp_ie, drp_evt);
-	
+
 }
 
 
diff --git a/drivers/uwb/est.c b/drivers/uwb/est.c
index 457f31d99bf..f3e23258428 100644
--- a/drivers/uwb/est.c
+++ b/drivers/uwb/est.c
@@ -258,7 +258,6 @@ int uwb_est_register(u8 type, u8 event_high, u16 vendor, u16 product,
 {
 	unsigned long flags;
 	unsigned itr;
-	u16 type_event_high;
 	int result = 0;
 
 	write_lock_irqsave(&uwb_est_lock, flags);
@@ -268,7 +267,6 @@ int uwb_est_register(u8 type, u8 event_high, u16 vendor, u16 product,
 			goto out;
 	}
 	/* Find the right spot to insert it in */
-	type_event_high = type << 8 | event_high;
 	for (itr = 0; itr < uwb_est_used; itr++)
 		if (uwb_est[itr].type_event_high < type
 		    && uwb_est[itr].vendor < vendor
diff --git a/drivers/uwb/ie-rcv.c b/drivers/uwb/ie-rcv.c
index 917e6d78a79..5fac5744a69 100644
--- a/drivers/uwb/ie-rcv.c
+++ b/drivers/uwb/ie-rcv.c
@@ -31,7 +31,6 @@ int uwbd_evt_handle_rc_ie_rcv(struct uwb_event *evt)
 	int result = -EINVAL;
 	struct device *dev = &evt->rc->uwb_dev.dev;
 	struct uwb_rc_evt_ie_rcv *iercv;
-	size_t iesize;
 
 	/* Is there enough data to decode it? */
 	if (evt->notif.size < sizeof(*iercv)) {
@@ -41,7 +40,6 @@ int uwbd_evt_handle_rc_ie_rcv(struct uwb_event *evt)
 		goto error;
 	}
 	iercv = container_of(evt->notif.rceb, struct uwb_rc_evt_ie_rcv, rceb);
-	iesize = le16_to_cpu(iercv->wIELength);
 
 	dev_dbg(dev, "IE received, element ID=%d\n", iercv->IEData[0]);
 
diff --git a/drivers/uwb/radio.c b/drivers/uwb/radio.c
index fd23d985ea7..2427e944883 100644
--- a/drivers/uwb/radio.c
+++ b/drivers/uwb/radio.c
@@ -115,7 +115,7 @@ int uwb_radio_start(struct uwb_pal *pal)
 EXPORT_SYMBOL_GPL(uwb_radio_start);
 
 /**
- * uwb_radio_stop - request tha the radio be stopped.
+ * uwb_radio_stop - request that the radio be stopped.
  * @pal: the PAL making the request.
  *
  * Stops the radio if no other PAL is making use of it.
diff --git a/drivers/uwb/rsv.c b/drivers/uwb/rsv.c
index 3fe61194104..0887ae98278 100644
--- a/drivers/uwb/rsv.c
+++ b/drivers/uwb/rsv.c
@@ -163,8 +163,10 @@ static int uwb_rsv_get_stream(struct uwb_rsv *rsv)
 	}
 
 	stream = find_first_zero_bit(streams_bm, UWB_NUM_STREAMS);
-	if (stream >= UWB_NUM_STREAMS)
+	if (stream >= UWB_NUM_STREAMS) {
+		dev_err(dev, "%s: no available stream found\n", __func__);
 		return -EBUSY;
+	}
 
 	rsv->stream = stream;
 	set_bit(stream, streams_bm);
@@ -249,7 +251,9 @@ static void uwb_rsv_stroke_timer(struct uwb_rsv *rsv)
 	 * super frame and should not be terminated if no response is
 	 * received.
 	 */
-	if (rsv->is_multicast) {
+	if (rsv->state == UWB_RSV_STATE_NONE) {
+		sframes = 0;
+	} else if (rsv->is_multicast) {
 		if (rsv->state == UWB_RSV_STATE_O_INITIATED
 		    || rsv->state == UWB_RSV_STATE_O_MOVE_EXPANDING
 		    || rsv->state == UWB_RSV_STATE_O_MOVE_COMBINING
@@ -322,6 +326,7 @@ void uwb_rsv_set_state(struct uwb_rsv *rsv, enum uwb_rsv_state new_state)
 	switch (new_state) {
 	case UWB_RSV_STATE_NONE:
 		uwb_rsv_state_update(rsv, UWB_RSV_STATE_NONE);
+		uwb_rsv_remove(rsv);
 		uwb_rsv_callback(rsv);
 		break;
 	case UWB_RSV_STATE_O_INITIATED:
@@ -442,6 +447,8 @@ static void uwb_rsv_handle_timeout_work(struct work_struct *work)
 		uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_ACCEPTED);
 		uwb_drp_avail_release(rsv->rc, &rsv->mv.companion_mas);
 		goto unlock;
+	case UWB_RSV_STATE_NONE:
+		goto unlock;
 	default:
 		break;
 	}
@@ -550,12 +557,16 @@ int uwb_rsv_establish(struct uwb_rsv *rsv)
 {
 	struct uwb_rc *rc = rsv->rc;
 	struct uwb_mas_bm available;
+	struct device *dev = &rc->uwb_dev.dev;
 	int ret;
 
 	mutex_lock(&rc->rsvs_mutex);
 	ret = uwb_rsv_get_stream(rsv);
-	if (ret)
+	if (ret) {
+		dev_err(dev, "%s: uwb_rsv_get_stream failed: %d\n",
+			__func__, ret);
 		goto out;
+	}
 
 	rsv->tiebreaker = prandom_u32() & 1;
 	/* get available mas bitmap */
@@ -565,12 +576,16 @@ int uwb_rsv_establish(struct uwb_rsv *rsv)
 	if (ret == UWB_RSV_ALLOC_NOT_FOUND) {
 		ret = -EBUSY;
 		uwb_rsv_put_stream(rsv);
+		dev_err(dev, "%s: uwb_rsv_find_best_allocation failed: %d\n",
+			__func__, ret);
 		goto out;
 	}
 
 	ret = uwb_drp_avail_reserve_pending(rc, &rsv->mas);
 	if (ret != 0) {
 		uwb_rsv_put_stream(rsv);
+		dev_err(dev, "%s: uwb_drp_avail_reserve_pending failed: %d\n",
+			__func__, ret);
 		goto out;
 	}
 
diff --git a/drivers/vfio/pci/vfio_pci.c b/drivers/vfio/pci/vfio_pci.c
index 7ba04249885..010e0f8b8e4 100644
--- a/drivers/vfio/pci/vfio_pci.c
+++ b/drivers/vfio/pci/vfio_pci.c
@@ -57,7 +57,8 @@ static int vfio_pci_enable(struct vfio_pci_device *vdev)
 
 	ret = vfio_config_init(vdev);
 	if (ret) {
-		pci_load_and_free_saved_state(pdev, &vdev->pci_saved_state);
+		kfree(vdev->pci_saved_state);
+		vdev->pci_saved_state = NULL;
 		pci_disable_device(pdev);
 		return ret;
 	}
@@ -196,8 +197,7 @@ static int vfio_pci_get_irq_count(struct vfio_pci_device *vdev, int irq_type)
 		if (pos) {
 			pci_read_config_word(vdev->pdev,
 					     pos + PCI_MSI_FLAGS, &flags);
-
-			return 1 << (flags & PCI_MSI_FLAGS_QMASK);
+			return 1 << ((flags & PCI_MSI_FLAGS_QMASK) >> 1);
 		}
 	} else if (irq_type == VFIO_PCI_MSIX_IRQ_INDEX) {
 		u8 pos;
diff --git a/drivers/vfio/pci/vfio_pci_config.c b/drivers/vfio/pci/vfio_pci_config.c
index 83cd1574c81..e50790e91f7 100644
--- a/drivers/vfio/pci/vfio_pci_config.c
+++ b/drivers/vfio/pci/vfio_pci_config.c
@@ -1126,8 +1126,7 @@ static int vfio_ext_cap_len(struct vfio_pci_device *vdev, u16 ecap, u16 epos)
 			return pcibios_err_to_errno(ret);
 
 		byte &= PCI_DPA_CAP_SUBSTATE_MASK;
-		byte = round_up(byte + 1, 4);
-		return PCI_DPA_BASE_SIZEOF + byte;
+		return PCI_DPA_BASE_SIZEOF + byte + 1;
 	case PCI_EXT_CAP_ID_TPH:
 		ret = pci_read_config_dword(pdev, epos + PCI_TPH_CAP, &dword);
 		if (ret)
@@ -1136,9 +1135,9 @@ static int vfio_ext_cap_len(struct vfio_pci_device *vdev, u16 ecap, u16 epos)
 		if ((dword & PCI_TPH_CAP_LOC_MASK) == PCI_TPH_LOC_CAP) {
 			int sts;
 
-			sts = byte & PCI_TPH_CAP_ST_MASK;
+			sts = dword & PCI_TPH_CAP_ST_MASK;
 			sts >>= PCI_TPH_CAP_ST_SHIFT;
-			return PCI_TPH_BASE_SIZEOF + round_up(sts * 2, 4);
+			return PCI_TPH_BASE_SIZEOF + (sts * 2) + 2;
 		}
 		return PCI_TPH_BASE_SIZEOF;
 	default:
diff --git a/drivers/vfio/vfio.c b/drivers/vfio/vfio.c
index 512f479d8a5..f018d8d0f97 100644
--- a/drivers/vfio/vfio.c
+++ b/drivers/vfio/vfio.c
@@ -349,7 +349,6 @@ struct vfio_device *vfio_group_create_device(struct vfio_group *group,
 					     void *device_data)
 {
 	struct vfio_device *device;
-	int ret;
 
 	device = kzalloc(sizeof(*device), GFP_KERNEL);
 	if (!device)
@@ -360,12 +359,7 @@ struct vfio_device *vfio_group_create_device(struct vfio_group *group,
 	device->group = group;
 	device->ops = ops;
 	device->device_data = device_data;
-
-	ret = dev_set_drvdata(dev, device);
-	if (ret) {
-		kfree(device);
-		return ERR_PTR(ret);
-	}
+	dev_set_drvdata(dev, device);
 
 	/* No need to get group_lock, caller has group reference */
 	vfio_group_get(group);
diff --git a/drivers/vfio/vfio_iommu_type1.c b/drivers/vfio/vfio_iommu_type1.c
index 6673e7be507..0734fbe5b65 100644
--- a/drivers/vfio/vfio_iommu_type1.c
+++ b/drivers/vfio/vfio_iommu_type1.c
@@ -524,7 +524,7 @@ unwind:
 static int vfio_dma_do_map(struct vfio_iommu *iommu,
 			   struct vfio_iommu_type1_dma_map *map)
 {
-	dma_addr_t end, iova;
+	dma_addr_t iova = map->iova;
 	unsigned long vaddr = map->vaddr;
 	size_t size = map->size;
 	long npage;
@@ -533,39 +533,30 @@ static int vfio_dma_do_map(struct vfio_iommu *iommu,
 	struct vfio_dma *dma;
 	unsigned long pfn;
 
-	end = map->iova + map->size;
+	/* Verify that none of our __u64 fields overflow */
+	if (map->size != size || map->vaddr != vaddr || map->iova != iova)
+		return -EINVAL;
 
 	mask = ((uint64_t)1 << __ffs(vfio_pgsize_bitmap(iommu))) - 1;
 
+	WARN_ON(mask & PAGE_MASK);
+
 	/* READ/WRITE from device perspective */
 	if (map->flags & VFIO_DMA_MAP_FLAG_WRITE)
 		prot |= IOMMU_WRITE;
 	if (map->flags & VFIO_DMA_MAP_FLAG_READ)
 		prot |= IOMMU_READ;
 
-	if (!prot)
-		return -EINVAL; /* No READ/WRITE? */
-
-	if (vaddr & mask)
-		return -EINVAL;
-	if (map->iova & mask)
-		return -EINVAL;
-	if (!map->size || map->size & mask)
-		return -EINVAL;
-
-	WARN_ON(mask & PAGE_MASK);
-
-	/* Don't allow IOVA wrap */
-	if (end && end < map->iova)
+	if (!prot || !size || (size | iova | vaddr) & mask)
 		return -EINVAL;
 
-	/* Don't allow virtual address wrap */
-	if (vaddr + map->size && vaddr + map->size < vaddr)
+	/* Don't allow IOVA or virtual address wrap */
+	if (iova + size - 1 < iova || vaddr + size - 1 < vaddr)
 		return -EINVAL;
 
 	mutex_lock(&iommu->lock);
 
-	if (vfio_find_dma(iommu, map->iova, map->size)) {
+	if (vfio_find_dma(iommu, iova, size)) {
 		mutex_unlock(&iommu->lock);
 		return -EEXIST;
 	}
@@ -576,17 +567,17 @@ static int vfio_dma_do_map(struct vfio_iommu *iommu,
 		return -ENOMEM;
 	}
 
-	dma->iova = map->iova;
-	dma->vaddr = map->vaddr;
+	dma->iova = iova;
+	dma->vaddr = vaddr;
 	dma->prot = prot;
 
 	/* Insert zero-sized and grow as we map chunks of it */
 	vfio_link_dma(iommu, dma);
 
-	for (iova = map->iova; iova < end; iova += size, vaddr += size) {
+	while (size) {
 		/* Pin a contiguous chunk of memory */
-		npage = vfio_pin_pages(vaddr, (end - iova) >> PAGE_SHIFT,
-				       prot, &pfn);
+		npage = vfio_pin_pages(vaddr + dma->size,
+				       size >> PAGE_SHIFT, prot, &pfn);
 		if (npage <= 0) {
 			WARN_ON(!npage);
 			ret = (int)npage;
@@ -594,14 +585,14 @@ static int vfio_dma_do_map(struct vfio_iommu *iommu,
 		}
 
 		/* Map it! */
-		ret = vfio_iommu_map(iommu, iova, pfn, npage, prot);
+		ret = vfio_iommu_map(iommu, iova + dma->size, pfn, npage, prot);
 		if (ret) {
 			vfio_unpin_pages(pfn, npage, prot, true);
 			break;
 		}
 
-		size = npage << PAGE_SHIFT;
-		dma->size += size;
+		size -= npage << PAGE_SHIFT;
+		dma->size += npage << PAGE_SHIFT;
 	}
 
 	if (ret)
diff --git a/drivers/vhost/scsi.c b/drivers/vhost/scsi.c
index cf50ce93975..aeb51310844 100644
--- a/drivers/vhost/scsi.c
+++ b/drivers/vhost/scsi.c
@@ -1255,7 +1255,7 @@ vhost_scsi_set_endpoint(struct vhost_scsi *vs,
 			tpg->tv_tpg_vhost_count++;
 			tpg->vhost_scsi = vs;
 			vs_tpg[tpg->tport_tpgt] = tpg;
-			smp_mb__after_atomic_inc();
+			smp_mb__after_atomic();
 			match = true;
 		}
 		mutex_unlock(&tpg->tv_tpg_mutex);
diff --git a/drivers/video/backlight/backlight.c b/drivers/video/backlight/backlight.c
index bd2172c2d65..428089009cd 100644
--- a/drivers/video/backlight/backlight.c
+++ b/drivers/video/backlight/backlight.c
@@ -23,6 +23,7 @@
 
 static struct list_head backlight_dev_list;
 static struct mutex backlight_dev_list_mutex;
+static struct blocking_notifier_head backlight_notifier;
 
 static const char *const backlight_types[] = {
 	[BACKLIGHT_RAW] = "raw",
@@ -370,6 +371,9 @@ struct backlight_device *backlight_device_register(const char *name,
 	list_add(&new_bd->entry, &backlight_dev_list);
 	mutex_unlock(&backlight_dev_list_mutex);
 
+	blocking_notifier_call_chain(&backlight_notifier,
+				     BACKLIGHT_REGISTERED, new_bd);
+
 	return new_bd;
 }
 EXPORT_SYMBOL(backlight_device_register);
@@ -413,6 +417,10 @@ void backlight_device_unregister(struct backlight_device *bd)
 		pmac_backlight = NULL;
 	mutex_unlock(&pmac_backlight_mutex);
 #endif
+
+	blocking_notifier_call_chain(&backlight_notifier,
+				     BACKLIGHT_UNREGISTERED, bd);
+
 	mutex_lock(&bd->ops_lock);
 	bd->ops = NULL;
 	mutex_unlock(&bd->ops_lock);
@@ -438,6 +446,36 @@ static int devm_backlight_device_match(struct device *dev, void *res,
 }
 
 /**
+ * backlight_register_notifier - get notified of backlight (un)registration
+ * @nb: notifier block with the notifier to call on backlight (un)registration
+ *
+ * @return 0 on success, otherwise a negative error code
+ *
+ * Register a notifier to get notified when backlight devices get registered
+ * or unregistered.
+ */
+int backlight_register_notifier(struct notifier_block *nb)
+{
+	return blocking_notifier_chain_register(&backlight_notifier, nb);
+}
+EXPORT_SYMBOL(backlight_register_notifier);
+
+/**
+ * backlight_unregister_notifier - unregister a backlight notifier
+ * @nb: notifier block to unregister
+ *
+ * @return 0 on success, otherwise a negative error code
+ *
+ * Register a notifier to get notified when backlight devices get registered
+ * or unregistered.
+ */
+int backlight_unregister_notifier(struct notifier_block *nb)
+{
+	return blocking_notifier_chain_unregister(&backlight_notifier, nb);
+}
+EXPORT_SYMBOL(backlight_unregister_notifier);
+
+/**
  * devm_backlight_device_register - resource managed backlight_device_register()
  * @dev: the device to register
  * @name: the name of the device
@@ -544,6 +582,8 @@ static int __init backlight_class_init(void)
 	backlight_class->pm = &backlight_class_dev_pm_ops;
 	INIT_LIST_HEAD(&backlight_dev_list);
 	mutex_init(&backlight_dev_list_mutex);
+	BLOCKING_INIT_NOTIFIER_HEAD(&backlight_notifier);
+
 	return 0;
 }
 
diff --git a/drivers/video/backlight/pwm_bl.c b/drivers/video/backlight/pwm_bl.c
index b75201ff46f..38ca88bc5c3 100644
--- a/drivers/video/backlight/pwm_bl.c
+++ b/drivers/video/backlight/pwm_bl.c
@@ -10,8 +10,8 @@
  * published by the Free Software Foundation.
  */
 
+#include <linux/gpio/consumer.h>
 #include <linux/gpio.h>
-#include <linux/of_gpio.h>
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
@@ -32,8 +32,7 @@ struct pwm_bl_data {
 	unsigned int		*levels;
 	bool			enabled;
 	struct regulator	*power_supply;
-	int			enable_gpio;
-	unsigned long		enable_gpio_flags;
+	struct gpio_desc	*enable_gpio;
 	unsigned int		scale;
 	int			(*notify)(struct device *,
 					  int brightness);
@@ -54,12 +53,8 @@ static void pwm_backlight_power_on(struct pwm_bl_data *pb, int brightness)
 	if (err < 0)
 		dev_err(pb->dev, "failed to enable power supply\n");
 
-	if (gpio_is_valid(pb->enable_gpio)) {
-		if (pb->enable_gpio_flags & PWM_BACKLIGHT_GPIO_ACTIVE_LOW)
-			gpio_set_value(pb->enable_gpio, 0);
-		else
-			gpio_set_value(pb->enable_gpio, 1);
-	}
+	if (pb->enable_gpio)
+		gpiod_set_value(pb->enable_gpio, 1);
 
 	pwm_enable(pb->pwm);
 	pb->enabled = true;
@@ -73,12 +68,8 @@ static void pwm_backlight_power_off(struct pwm_bl_data *pb)
 	pwm_config(pb->pwm, 0, pb->period);
 	pwm_disable(pb->pwm);
 
-	if (gpio_is_valid(pb->enable_gpio)) {
-		if (pb->enable_gpio_flags & PWM_BACKLIGHT_GPIO_ACTIVE_LOW)
-			gpio_set_value(pb->enable_gpio, 1);
-		else
-			gpio_set_value(pb->enable_gpio, 0);
-	}
+	if (pb->enable_gpio)
+		gpiod_set_value(pb->enable_gpio, 0);
 
 	regulator_disable(pb->power_supply);
 	pb->enabled = false;
@@ -148,7 +139,6 @@ static int pwm_backlight_parse_dt(struct device *dev,
 				  struct platform_pwm_backlight_data *data)
 {
 	struct device_node *node = dev->of_node;
-	enum of_gpio_flags flags;
 	struct property *prop;
 	int length;
 	u32 value;
@@ -189,14 +179,6 @@ static int pwm_backlight_parse_dt(struct device *dev,
 		data->max_brightness--;
 	}
 
-	data->enable_gpio = of_get_named_gpio_flags(node, "enable-gpios", 0,
-						    &flags);
-	if (data->enable_gpio == -EPROBE_DEFER)
-		return -EPROBE_DEFER;
-
-	if (gpio_is_valid(data->enable_gpio) && (flags & OF_GPIO_ACTIVE_LOW))
-		data->enable_gpio_flags |= PWM_BACKLIGHT_GPIO_ACTIVE_LOW;
-
 	return 0;
 }
 
@@ -256,8 +238,6 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 	} else
 		pb->scale = data->max_brightness;
 
-	pb->enable_gpio = data->enable_gpio;
-	pb->enable_gpio_flags = data->enable_gpio_flags;
 	pb->notify = data->notify;
 	pb->notify_after = data->notify_after;
 	pb->check_fb = data->check_fb;
@@ -265,26 +245,38 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 	pb->dev = &pdev->dev;
 	pb->enabled = false;
 
-	if (gpio_is_valid(pb->enable_gpio)) {
-		unsigned long flags;
-
-		if (pb->enable_gpio_flags & PWM_BACKLIGHT_GPIO_ACTIVE_LOW)
-			flags = GPIOF_OUT_INIT_HIGH;
+	pb->enable_gpio = devm_gpiod_get(&pdev->dev, "enable");
+	if (IS_ERR(pb->enable_gpio)) {
+		ret = PTR_ERR(pb->enable_gpio);
+		if (ret == -ENOENT)
+			pb->enable_gpio = NULL;
 		else
-			flags = GPIOF_OUT_INIT_LOW;
+			goto err_alloc;
+	}
 
-		ret = gpio_request_one(pb->enable_gpio, flags, "enable");
+	/*
+	 * Compatibility fallback for drivers still using the integer GPIO
+	 * platform data. Must go away soon.
+	 */
+	if (!pb->enable_gpio && gpio_is_valid(data->enable_gpio)) {
+		ret = devm_gpio_request_one(&pdev->dev, data->enable_gpio,
+					    GPIOF_OUT_INIT_HIGH, "enable");
 		if (ret < 0) {
 			dev_err(&pdev->dev, "failed to request GPIO#%d: %d\n",
-				pb->enable_gpio, ret);
+				data->enable_gpio, ret);
 			goto err_alloc;
 		}
+
+		pb->enable_gpio = gpio_to_desc(data->enable_gpio);
 	}
 
+	if (pb->enable_gpio)
+		gpiod_direction_output(pb->enable_gpio, 1);
+
 	pb->power_supply = devm_regulator_get(&pdev->dev, "power");
 	if (IS_ERR(pb->power_supply)) {
 		ret = PTR_ERR(pb->power_supply);
-		goto err_gpio;
+		goto err_alloc;
 	}
 
 	pb->pwm = devm_pwm_get(&pdev->dev, NULL);
@@ -295,7 +287,7 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 		if (IS_ERR(pb->pwm)) {
 			dev_err(&pdev->dev, "unable to request legacy PWM\n");
 			ret = PTR_ERR(pb->pwm);
-			goto err_gpio;
+			goto err_alloc;
 		}
 	}
 
@@ -304,12 +296,15 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 	/*
 	 * The DT case will set the pwm_period_ns field to 0 and store the
 	 * period, parsed from the DT, in the PWM device. For the non-DT case,
-	 * set the period from platform data.
+	 * set the period from platform data if it has not already been set
+	 * via the PWM lookup table.
 	 */
-	if (data->pwm_period_ns > 0)
+	pb->period = pwm_get_period(pb->pwm);
+	if (!pb->period && (data->pwm_period_ns > 0)) {
+		pb->period = data->pwm_period_ns;
 		pwm_set_period(pb->pwm, data->pwm_period_ns);
+	}
 
-	pb->period = pwm_get_period(pb->pwm);
 	pb->lth_brightness = data->lth_brightness * (pb->period / pb->scale);
 
 	memset(&props, 0, sizeof(struct backlight_properties));
@@ -320,7 +315,7 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 	if (IS_ERR(bl)) {
 		dev_err(&pdev->dev, "failed to register backlight\n");
 		ret = PTR_ERR(bl);
-		goto err_gpio;
+		goto err_alloc;
 	}
 
 	if (data->dft_brightness > data->max_brightness) {
@@ -336,9 +331,6 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, bl);
 	return 0;
 
-err_gpio:
-	if (gpio_is_valid(pb->enable_gpio))
-		gpio_free(pb->enable_gpio);
 err_alloc:
 	if (data->exit)
 		data->exit(&pdev->dev);
@@ -359,6 +351,14 @@ static int pwm_backlight_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static void pwm_backlight_shutdown(struct platform_device *pdev)
+{
+	struct backlight_device *bl = platform_get_drvdata(pdev);
+	struct pwm_bl_data *pb = bl_get_data(bl);
+
+	pwm_backlight_power_off(pb);
+}
+
 #ifdef CONFIG_PM_SLEEP
 static int pwm_backlight_suspend(struct device *dev)
 {
@@ -404,6 +404,7 @@ static struct platform_driver pwm_backlight_driver = {
 	},
 	.probe		= pwm_backlight_probe,
 	.remove		= pwm_backlight_remove,
+	.shutdown	= pwm_backlight_shutdown,
 };
 
 module_platform_driver(pwm_backlight_driver);
diff --git a/drivers/video/console/fbcon.c b/drivers/video/console/fbcon.c
index f447734b09b..57b1d44acbf 100644
--- a/drivers/video/console/fbcon.c
+++ b/drivers/video/console/fbcon.c
@@ -748,6 +748,7 @@ static int con2fb_release_oldinfo(struct vc_data *vc, struct fb_info *oldinfo,
 		fbcon_del_cursor_timer(oldinfo);
 		kfree(ops->cursor_state.mask);
 		kfree(ops->cursor_data);
+		kfree(ops->cursor_src);
 		kfree(ops->fontbuffer);
 		kfree(oldinfo->fbcon_par);
 		oldinfo->fbcon_par = NULL;
diff --git a/drivers/video/console/vgacon.c b/drivers/video/console/vgacon.c
index 9d8feac6763..f267284b423 100644
--- a/drivers/video/console/vgacon.c
+++ b/drivers/video/console/vgacon.c
@@ -87,7 +87,8 @@ static void vgacon_save_screen(struct vc_data *c);
 static int vgacon_scroll(struct vc_data *c, int t, int b, int dir,
 			 int lines);
 static void vgacon_invert_region(struct vc_data *c, u16 * p, int count);
-static unsigned long vgacon_uni_pagedir[2];
+static struct uni_pagedir *vgacon_uni_pagedir;
+static int vgacon_refcount;
 
 /* Description of the hardware situation */
 static int		vga_init_done		__read_mostly;
@@ -553,7 +554,7 @@ static const char *vgacon_startup(void)
 
 static void vgacon_init(struct vc_data *c, int init)
 {
-	unsigned long p;
+	struct uni_pagedir *p;
 
 	/*
 	 * We cannot be loaded as a module, therefore init is always 1,
@@ -575,12 +576,12 @@ static void vgacon_init(struct vc_data *c, int init)
 	if (vga_512_chars)
 		c->vc_hi_font_mask = 0x0800;
 	p = *c->vc_uni_pagedir_loc;
-	if (c->vc_uni_pagedir_loc == &c->vc_uni_pagedir ||
-	    !--c->vc_uni_pagedir_loc[1])
+	if (c->vc_uni_pagedir_loc != &vgacon_uni_pagedir) {
 		con_free_unimap(c);
-	c->vc_uni_pagedir_loc = vgacon_uni_pagedir;
-	vgacon_uni_pagedir[1]++;
-	if (!vgacon_uni_pagedir[0] && p)
+		c->vc_uni_pagedir_loc = &vgacon_uni_pagedir;
+		vgacon_refcount++;
+	}
+	if (!vgacon_uni_pagedir && p)
 		con_set_default_unimap(c);
 
 	/* Only set the default if the user didn't deliberately override it */
@@ -597,7 +598,7 @@ static void vgacon_deinit(struct vc_data *c)
 		vga_set_mem_top(c);
 	}
 
-	if (!--vgacon_uni_pagedir[1])
+	if (!--vgacon_refcount)
 		con_free_unimap(c);
 	c->vc_uni_pagedir_loc = &c->vc_uni_pagedir;
 	con_set_default_unimap(c);
diff --git a/drivers/video/fbdev/Kconfig b/drivers/video/fbdev/Kconfig
index e1f47272fde..59c98bfd5a8 100644
--- a/drivers/video/fbdev/Kconfig
+++ b/drivers/video/fbdev/Kconfig
@@ -360,6 +360,7 @@ config FB_CYBER2000_DDC
 config FB_CYBER2000_I2C
 	bool "CyberPro 2000/2010/5000 I2C support"
 	depends on FB_CYBER2000 && I2C && ARCH_NETWINDER
+	depends on I2C=y || FB_CYBER2000=m
 	select I2C_ALGOBIT
 	help
 	  Enable support for the I2C video decoder interface on the
@@ -966,6 +967,7 @@ config FB_S1D13XXX
 config FB_ATMEL
 	tristate "AT91/AT32 LCD Controller support"
 	depends on FB && HAVE_FB_ATMEL
+	select FB_BACKLIGHT
 	select FB_CFB_FILLRECT
 	select FB_CFB_COPYAREA
 	select FB_CFB_IMAGEBLIT
@@ -1971,6 +1973,7 @@ config FB_W100
 config FB_SH_MOBILE_LCDC
 	tristate "SuperH Mobile LCDC framebuffer support"
 	depends on FB && (SUPERH || ARCH_SHMOBILE) && HAVE_CLK
+	depends on FB_SH_MOBILE_MERAM || !FB_SH_MOBILE_MERAM
 	select FB_SYS_FILLRECT
 	select FB_SYS_COPYAREA
 	select FB_SYS_IMAGEBLIT
@@ -1993,7 +1996,7 @@ config FB_SH_MOBILE_HDMI
 
 config FB_TMIO
 	tristate "Toshiba Mobile IO FrameBuffer support"
-	depends on FB && MFD_CORE
+	depends on FB && (MFD_TMIO || COMPILE_TEST)
 	select FB_CFB_FILLRECT
 	select FB_CFB_COPYAREA
 	select FB_CFB_IMAGEBLIT
@@ -2062,7 +2065,7 @@ config FB_S3C2410_DEBUG
 	  through sysfs
 
 config FB_NUC900
-        bool "NUC900 LCD framebuffer support"
+        tristate "NUC900 LCD framebuffer support"
         depends on FB && ARCH_W90X900
         select FB_CFB_FILLRECT
         select FB_CFB_COPYAREA
@@ -2169,7 +2172,7 @@ config FB_XILINX
 
 config FB_GOLDFISH
 	tristate "Goldfish Framebuffer"
-	depends on FB && HAS_DMA
+	depends on FB && HAS_DMA && (GOLDFISH || COMPILE_TEST)
 	select FB_CFB_FILLRECT
 	select FB_CFB_COPYAREA
 	select FB_CFB_IMAGEBLIT
@@ -2295,6 +2298,7 @@ endchoice
 config FB_MB862XX_I2C
 	bool "Support I2C bus on MB862XX GDC"
 	depends on FB_MB862XX && I2C
+	depends on FB_MB862XX=m || I2C=y
 	default y
 	help
 	  Selecting this option adds Coral-P(A)/Lime GDC I2C bus adapter
@@ -2332,6 +2336,7 @@ config FB_MX3
 	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/atafb.c b/drivers/video/fbdev/atafb.c
index e21d1f58554..4953b657635 100644
--- a/drivers/video/fbdev/atafb.c
+++ b/drivers/video/fbdev/atafb.c
@@ -191,7 +191,7 @@ static struct fb_info fb_info = {
 };
 
 static void *screen_base;	/* base address of screen */
-static void *real_screen_base;	/* (only for Overscan) */
+static unsigned long phys_screen_base;	/* (only for Overscan) */
 
 static int screen_len;
 
@@ -213,7 +213,8 @@ static unsigned int external_yres;
  */
 static unsigned int external_depth;
 static int external_pmode;
-static void *external_addr;
+static void *external_screen_base;
+static unsigned long external_addr;
 static unsigned long external_len;
 static unsigned long external_vgaiobase;
 static unsigned int external_bitspercol = 6;
@@ -592,7 +593,7 @@ static int tt_encode_fix(struct fb_fix_screeninfo *fix, struct atafb_par *par)
 	int mode;
 
 	strcpy(fix->id, "Atari Builtin");
-	fix->smem_start = (unsigned long)real_screen_base;
+	fix->smem_start = phys_screen_base;
 	fix->smem_len = screen_len;
 	fix->type = FB_TYPE_INTERLEAVED_PLANES;
 	fix->type_aux = 2;
@@ -790,7 +791,7 @@ static void tt_get_par(struct atafb_par *par)
 	addr = ((shifter.bas_hi & 0xff) << 16) |
 	       ((shifter.bas_md & 0xff) << 8)  |
 	       ((shifter.bas_lo & 0xff));
-	par->screen_base = phys_to_virt(addr);
+	par->screen_base = atari_stram_to_virt(addr);
 }
 
 static void tt_set_par(struct atafb_par *par)
@@ -888,7 +889,7 @@ static int falcon_encode_fix(struct fb_fix_screeninfo *fix,
 			     struct atafb_par *par)
 {
 	strcpy(fix->id, "Atari Builtin");
-	fix->smem_start = (unsigned long)real_screen_base;
+	fix->smem_start = phys_screen_base;
 	fix->smem_len = screen_len;
 	fix->type = FB_TYPE_INTERLEAVED_PLANES;
 	fix->type_aux = 2;
@@ -1584,7 +1585,7 @@ static void falcon_get_par(struct atafb_par *par)
 	addr = (shifter.bas_hi & 0xff) << 16 |
 	       (shifter.bas_md & 0xff) << 8  |
 	       (shifter.bas_lo & 0xff);
-	par->screen_base = phys_to_virt(addr);
+	par->screen_base = atari_stram_to_virt(addr);
 
 	/* derived parameters */
 	hw->ste_mode = (hw->f_shift & 0x510) == 0 && hw->st_shift == 0x100;
@@ -1814,7 +1815,7 @@ static int stste_encode_fix(struct fb_fix_screeninfo *fix,
 	int mode;
 
 	strcpy(fix->id, "Atari Builtin");
-	fix->smem_start = (unsigned long)real_screen_base;
+	fix->smem_start = phys_screen_base;
 	fix->smem_len = screen_len;
 	fix->type = FB_TYPE_INTERLEAVED_PLANES;
 	fix->type_aux = 2;
@@ -1980,7 +1981,7 @@ static void stste_get_par(struct atafb_par *par)
 	       ((shifter.bas_md & 0xff) << 8);
 	if (ATARIHW_PRESENT(EXTD_SHIFTER))
 		addr |= (shifter.bas_lo & 0xff);
-	par->screen_base = phys_to_virt(addr);
+	par->screen_base = atari_stram_to_virt(addr);
 }
 
 static void stste_set_par(struct atafb_par *par)
@@ -2039,7 +2040,7 @@ static int stste_detect(void)
 static void stste_set_screen_base(void *s_base)
 {
 	unsigned long addr;
-	addr = virt_to_phys(s_base);
+	addr = atari_stram_to_phys(s_base);
 	/* Setup Screen Memory */
 	shifter.bas_hi = (unsigned char)((addr & 0xff0000) >> 16);
 	shifter.bas_md = (unsigned char)((addr & 0x00ff00) >> 8);
@@ -2113,7 +2114,7 @@ static void st_ovsc_switch(void)
 static int ext_encode_fix(struct fb_fix_screeninfo *fix, struct atafb_par *par)
 {
 	strcpy(fix->id, "Unknown Extern");
-	fix->smem_start = (unsigned long)external_addr;
+	fix->smem_start = external_addr;
 	fix->smem_len = PAGE_ALIGN(external_len);
 	if (external_depth == 1) {
 		fix->type = FB_TYPE_PACKED_PIXELS;
@@ -2213,7 +2214,7 @@ static int ext_encode_var(struct fb_var_screeninfo *var, struct atafb_par *par)
 
 static void ext_get_par(struct atafb_par *par)
 {
-	par->screen_base = external_addr;
+	par->screen_base = external_screen_base;
 }
 
 static void ext_set_par(struct atafb_par *par)
@@ -2286,7 +2287,7 @@ static void set_screen_base(void *s_base)
 {
 	unsigned long addr;
 
-	addr = virt_to_phys(s_base);
+	addr = atari_stram_to_phys(s_base);
 	/* Setup Screen Memory */
 	shifter.bas_hi = (unsigned char)((addr & 0xff0000) >> 16);
 	shifter.bas_md = (unsigned char)((addr & 0x00ff00) >> 8);
@@ -2433,7 +2434,9 @@ static void atafb_set_disp(struct fb_info *info)
 	atafb_get_var(&info->var, info);
 	atafb_get_fix(&info->fix, info);
 
-	info->screen_base = (void *)info->fix.smem_start;
+	/* Note: smem_start derives from phys_screen_base, not screen_base! */
+	info->screen_base = (external_addr ? external_screen_base :
+				atari_stram_to_virt(info->fix.smem_start));
 }
 
 static int atafb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
@@ -2904,7 +2907,7 @@ static void __init atafb_setup_ext(char *spec)
 	external_yres = yres;
 	external_depth = depth;
 	external_pmode = planes;
-	external_addr = (void *)addr;
+	external_addr = addr;
 	external_len = len;
 
 	if (external_card_type == IS_MV300) {
@@ -3166,30 +3169,30 @@ int __init atafb_init(void)
 		memset(screen_base, 0, mem_req);
 		pad = -(unsigned long)screen_base & (PAGE_SIZE - 1);
 		screen_base += pad;
-		real_screen_base = screen_base + ovsc_offset;
+		phys_screen_base = atari_stram_to_phys(screen_base + ovsc_offset);
 		screen_len = (mem_req - pad - ovsc_offset) & PAGE_MASK;
 		st_ovsc_switch();
 		if (CPU_IS_040_OR_060) {
 			/* On a '040+, the cache mode of video RAM must be set to
 			 * write-through also for internal video hardware! */
-			cache_push(virt_to_phys(screen_base), screen_len);
+			cache_push(atari_stram_to_phys(screen_base), screen_len);
 			kernel_set_cachemode(screen_base, screen_len,
 					     IOMAP_WRITETHROUGH);
 		}
-		printk("atafb: screen_base %p real_screen_base %p screen_len %d\n",
-			screen_base, real_screen_base, screen_len);
+		printk("atafb: screen_base %p phys_screen_base %lx screen_len %d\n",
+			screen_base, phys_screen_base, screen_len);
 #ifdef ATAFB_EXT
 	} else {
 		/* Map the video memory (physical address given) to somewhere
 		 * in the kernel address space.
 		 */
-		external_addr = ioremap_writethrough((unsigned long)external_addr,
+		external_screen_base = ioremap_writethrough(external_addr,
 						     external_len);
 		if (external_vgaiobase)
 			external_vgaiobase =
 			  (unsigned long)ioremap(external_vgaiobase, 0x10000);
-		screen_base =
-		real_screen_base = external_addr;
+		screen_base = external_screen_base;
+		phys_screen_base = external_addr;
 		screen_len = external_len & PAGE_MASK;
 		memset (screen_base, 0, external_len);
 	}
@@ -3235,8 +3238,8 @@ int __init atafb_init(void)
 	if (register_framebuffer(&fb_info) < 0) {
 #ifdef ATAFB_EXT
 		if (external_addr) {
-			iounmap(external_addr);
-			external_addr = NULL;
+			iounmap(external_screen_base);
+			external_addr = 0;
 		}
 		if (external_vgaiobase) {
 			iounmap((void*)external_vgaiobase);
diff --git a/drivers/video/fbdev/bf54x-lq043fb.c b/drivers/video/fbdev/bf54x-lq043fb.c
index e2c42ad8515..adbef542c99 100644
--- a/drivers/video/fbdev/bf54x-lq043fb.c
+++ b/drivers/video/fbdev/bf54x-lq043fb.c
@@ -717,8 +717,6 @@ static int bfin_bf54x_remove(struct platform_device *pdev)
 #ifdef CONFIG_PM
 static int bfin_bf54x_suspend(struct platform_device *pdev, pm_message_t state)
 {
-	struct fb_info *fbinfo = platform_get_drvdata(pdev);
-
 	bfin_write_EPPI0_CONTROL(bfin_read_EPPI0_CONTROL() & ~EPPI_EN);
 	disable_dma(CH_EPPI0);
 	bfin_write_EPPI0_STATUS(0xFFFF);
diff --git a/drivers/video/fbdev/core/fbmem.c b/drivers/video/fbdev/core/fbmem.c
index b6d5008f361..b5e85f6c1c2 100644
--- a/drivers/video/fbdev/core/fbmem.c
+++ b/drivers/video/fbdev/core/fbmem.c
@@ -433,7 +433,7 @@ static void fb_do_show_logo(struct fb_info *info, struct fb_image *image,
 			image->dx += image->width + 8;
 		}
 	} else if (rotate == FB_ROTATE_UD) {
-		for (x = 0; x < num && image->dx >= 0; x++) {
+		for (x = 0; x < num; x++) {
 			info->fbops->fb_imageblit(info, image);
 			image->dx -= image->width + 8;
 		}
@@ -445,7 +445,7 @@ static void fb_do_show_logo(struct fb_info *info, struct fb_image *image,
 			image->dy += image->height + 8;
 		}
 	} else if (rotate == FB_ROTATE_CCW) {
-		for (x = 0; x < num && image->dy >= 0; x++) {
+		for (x = 0; x < num; x++) {
 			info->fbops->fb_imageblit(info, image);
 			image->dy -= image->height + 8;
 		}
@@ -674,6 +674,7 @@ int fb_show_logo(struct fb_info *info, int rotate)
 int fb_prepare_logo(struct fb_info *info, int rotate) { return 0; }
 int fb_show_logo(struct fb_info *info, int rotate) { return 0; }
 #endif /* CONFIG_LOGO */
+EXPORT_SYMBOL(fb_prepare_logo);
 EXPORT_SYMBOL(fb_show_logo);
 
 static void *fb_seq_start(struct seq_file *m, loff_t *pos)
@@ -1179,7 +1180,7 @@ static long do_fb_ioctl(struct fb_info *info, unsigned int cmd,
 			return -EFAULT;
 		if (con2fb.console < 1 || con2fb.console > MAX_NR_CONSOLES)
 			return -EINVAL;
-		if (con2fb.framebuffer < 0 || con2fb.framebuffer >= FB_MAX)
+		if (con2fb.framebuffer >= FB_MAX)
 			return -EINVAL;
 		if (!registered_fb[con2fb.framebuffer])
 			request_module("fb%d", con2fb.framebuffer);
diff --git a/drivers/video/fbdev/core/fbmon.c b/drivers/video/fbdev/core/fbmon.c
index c204ebe6187..5b0e313849b 100644
--- a/drivers/video/fbdev/core/fbmon.c
+++ b/drivers/video/fbdev/core/fbmon.c
@@ -1012,13 +1012,20 @@ void fb_edid_add_monspecs(unsigned char *edid, struct fb_monspecs *specs)
 	while (pos < edid[2]) {
 		u8 len = edid[pos] & 0x1f, type = (edid[pos] >> 5) & 7;
 		pr_debug("Data block %u of %u bytes\n", type, len);
-		if (type == 2)
+		if (type == 2) {
 			for (i = pos; i < pos + len; i++) {
 				u8 idx = edid[pos + i] & 0x7f;
 				svd[svd_n++] = idx;
 				pr_debug("N%sative mode #%d\n",
 					 edid[pos + i] & 0x80 ? "" : "on-n", idx);
 			}
+		} else if (type == 3 && len >= 3) {
+			/* Check Vendor Specific Data Block.  For HDMI,
+			   it is always 00-0C-03 for HDMI Licensing, LLC. */
+			if (edid[pos + 1] == 3 && edid[pos + 2] == 0xc &&
+			    edid[pos + 3] == 0)
+				specs->misc |= FB_MISC_HDMI;
+		}
 		pos += len + 1;
 	}
 
diff --git a/drivers/video/fbdev/da8xx-fb.c b/drivers/video/fbdev/da8xx-fb.c
index 6b23508ff0a..a8484f768d0 100644
--- a/drivers/video/fbdev/da8xx-fb.c
+++ b/drivers/video/fbdev/da8xx-fb.c
@@ -242,6 +242,20 @@ static struct fb_videomode known_lcd_panels[] = {
 		.sync           = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
 		.flag           = 0,
 	},
+	[3] = {
+		/* Densitron 84-0023-001T */
+		.name           = "Densitron_84-0023-001T",
+		.xres           = 320,
+		.yres           = 240,
+		.pixclock       = KHZ2PICOS(6400),
+		.left_margin    = 0,
+		.right_margin   = 0,
+		.upper_margin   = 0,
+		.lower_margin   = 0,
+		.hsync_len      = 30,
+		.vsync_len      = 3,
+		.sync           = 0,
+	},
 };
 
 static bool da8xx_fb_is_raster_enabled(void)
diff --git a/drivers/video/fbdev/exynos/Kconfig b/drivers/video/fbdev/exynos/Kconfig
index fcf2d48ac6d..1f16b4678c7 100644
--- a/drivers/video/fbdev/exynos/Kconfig
+++ b/drivers/video/fbdev/exynos/Kconfig
@@ -4,6 +4,7 @@
 
 menuconfig EXYNOS_VIDEO
 	bool "Exynos Video driver support"
+	depends on ARCH_S5PV210 || ARCH_EXYNOS
 	help
 	  This enables support for EXYNOS Video device.
 
@@ -15,7 +16,6 @@ if EXYNOS_VIDEO
 
 config EXYNOS_MIPI_DSI
 	bool "EXYNOS MIPI DSI driver support."
-	depends on ARCH_S5PV210 || ARCH_EXYNOS
 	select GENERIC_PHY
 	help
 	  This enables support for MIPI-DSI device.
diff --git a/drivers/video/fbdev/fb-puv3.c b/drivers/video/fbdev/fb-puv3.c
index 6db9ebd042a..88fa2e70a0b 100644
--- a/drivers/video/fbdev/fb-puv3.c
+++ b/drivers/video/fbdev/fb-puv3.c
@@ -18,8 +18,10 @@
 #include <linux/fb.h>
 #include <linux/init.h>
 #include <linux/console.h>
+#include <linux/mm.h>
 
 #include <asm/sizes.h>
+#include <asm/pgtable.h>
 #include <mach/hardware.h>
 
 /* Platform_data reserved for unifb registers. */
diff --git a/drivers/video/fbdev/gbefb.c b/drivers/video/fbdev/gbefb.c
index 3ec65a878ac..4aa56ba78f3 100644
--- a/drivers/video/fbdev/gbefb.c
+++ b/drivers/video/fbdev/gbefb.c
@@ -1068,7 +1068,7 @@ static struct fb_ops gbefb_ops = {
 
 static ssize_t gbefb_show_memsize(struct device *dev, struct device_attribute *attr, char *buf)
 {
-	return snprintf(buf, PAGE_SIZE, "%d\n", gbe_mem_size);
+	return snprintf(buf, PAGE_SIZE, "%u\n", gbe_mem_size);
 }
 
 static DEVICE_ATTR(size, S_IRUGO, gbefb_show_memsize, NULL);
diff --git a/drivers/video/fbdev/grvga.c b/drivers/video/fbdev/grvga.c
index c078701f15f..2db5bb1a33e 100644
--- a/drivers/video/fbdev/grvga.c
+++ b/drivers/video/fbdev/grvga.c
@@ -514,9 +514,10 @@ free_fb:
 static int grvga_remove(struct platform_device *device)
 {
 	struct fb_info *info = dev_get_drvdata(&device->dev);
-	struct grvga_par *par = info->par;
+	struct grvga_par *par;
 
 	if (info) {
+		par = info->par;
 		unregister_framebuffer(info);
 		fb_dealloc_cmap(&info->cmap);
 
diff --git a/drivers/video/fbdev/matrox/matroxfb_base.h b/drivers/video/fbdev/matrox/matroxfb_base.h
index 556d96ce40b..89a8a89a5eb 100644
--- a/drivers/video/fbdev/matrox/matroxfb_base.h
+++ b/drivers/video/fbdev/matrox/matroxfb_base.h
@@ -698,7 +698,7 @@ void matroxfb_unregister_driver(struct matroxfb_driver* drv);
 
 #define mga_fifo(n)	do {} while ((mga_inl(M_FIFOSTATUS) & 0xFF) < (n))
 
-#define WaitTillIdle()	do {} while (mga_inl(M_STATUS) & 0x10000)
+#define WaitTillIdle()	do { mga_inl(M_STATUS); do {} while (mga_inl(M_STATUS) & 0x10000); } while (0)
 
 /* code speedup */
 #ifdef CONFIG_FB_MATROX_MILLENIUM
diff --git a/drivers/video/fbdev/mbx/Makefile b/drivers/video/fbdev/mbx/Makefile
index 16c1165cf9c..d7ae5a9bb37 100644
--- a/drivers/video/fbdev/mbx/Makefile
+++ b/drivers/video/fbdev/mbx/Makefile
@@ -1,4 +1,3 @@
 # Makefile for the 2700G controller driver.
 
-obj-$(CONFIG_FB_MBX)	   += mbxfb.o
-obj-$(CONFIG_FB_MBX_DEBUG) += mbxfbdebugfs.o
+obj-y 			+= mbxfb.o
diff --git a/drivers/video/fbdev/mbx/mbxdebugfs.c b/drivers/video/fbdev/mbx/mbxdebugfs.c
index 4449f249b0e..e3bc00a7529 100644
--- a/drivers/video/fbdev/mbx/mbxdebugfs.c
+++ b/drivers/video/fbdev/mbx/mbxdebugfs.c
@@ -17,7 +17,7 @@ struct mbxfb_debugfs_data {
 
 static int open_file_generic(struct inode *inode, struct file *file)
 {
-	file->private_data = inode->u.generic_ip;
+	file->private_data = inode->i_private;
 	return 0;
 }
 
diff --git a/drivers/video/fbdev/mbx/mbxfb.c b/drivers/video/fbdev/mbx/mbxfb.c
index f0a5392f5fd..2bd52ed8832 100644
--- a/drivers/video/fbdev/mbx/mbxfb.c
+++ b/drivers/video/fbdev/mbx/mbxfb.c
@@ -877,6 +877,8 @@ static int mbxfb_resume(struct platform_device *dev)
 #ifndef CONFIG_FB_MBX_DEBUG
 #define mbxfb_debugfs_init(x)	do {} while(0)
 #define mbxfb_debugfs_remove(x)	do {} while(0)
+#else
+#include "mbxdebugfs.c"
 #endif
 
 #define res_size(_r) (((_r)->end - (_r)->start) + 1)
diff --git a/drivers/video/fbdev/mmp/Kconfig b/drivers/video/fbdev/mmp/Kconfig
index d4a4ffc2474..f56a7e2e813 100644
--- a/drivers/video/fbdev/mmp/Kconfig
+++ b/drivers/video/fbdev/mmp/Kconfig
@@ -1,6 +1,6 @@
 menuconfig MMP_DISP
         tristate "Marvell MMP Display Subsystem support"
-        depends on CPU_PXA910 || CPU_MMP2 || CPU_MMP3 || CPU_PXA988
+        depends on CPU_PXA910 || CPU_MMP2
         help
 	  Marvell Display Subsystem support.
 
diff --git a/drivers/video/fbdev/mmp/fb/mmpfb.c b/drivers/video/fbdev/mmp/fb/mmpfb.c
index 7ab31eb76a8..910fcc6ecec 100644
--- a/drivers/video/fbdev/mmp/fb/mmpfb.c
+++ b/drivers/video/fbdev/mmp/fb/mmpfb.c
@@ -554,8 +554,8 @@ static void fb_info_clear(struct fb_info *info)
 static int mmpfb_probe(struct platform_device *pdev)
 {
 	struct mmp_buffer_driver_mach_info *mi;
-	struct fb_info *info = 0;
-	struct mmpfb_info *fbi = 0;
+	struct fb_info *info;
+	struct mmpfb_info *fbi;
 	int ret, modes_num;
 
 	mi = pdev->dev.platform_data;
@@ -569,10 +569,6 @@ static int mmpfb_probe(struct platform_device *pdev)
 	if (info == NULL)
 		return -ENOMEM;
 	fbi = info->par;
-	if (!fbi) {
-		ret = -EINVAL;
-		goto failed;
-	}
 
 	/* init fb */
 	fbi->fb_info = info;
@@ -667,7 +663,6 @@ failed_free_buff:
 		fbi->fb_start_dma);
 failed_destroy_mutex:
 	mutex_destroy(&fbi->access_ok);
-failed:
 	dev_err(fbi->dev, "mmp-fb: frame buffer device init failed\n");
 
 	framebuffer_release(info);
diff --git a/drivers/video/fbdev/mmp/hw/Kconfig b/drivers/video/fbdev/mmp/hw/Kconfig
index 02f109a20cd..c735d133895 100644
--- a/drivers/video/fbdev/mmp/hw/Kconfig
+++ b/drivers/video/fbdev/mmp/hw/Kconfig
@@ -2,12 +2,12 @@ if MMP_DISP
 
 config MMP_DISP_CONTROLLER
 	bool "mmp display controller hw support"
-	depends on CPU_PXA910 || CPU_MMP2 || CPU_MMP3 || CPU_PXA988
+	depends on CPU_PXA910 || CPU_MMP2
 	default n
 	help
 		Marvell MMP display hw controller support
-		this controller is used on Marvell PXA910,
-		MMP2, MMP3, PXA988 chips
+		this controller is used on Marvell PXA910 and
+		MMP2 chips
 
 config MMP_DISP_SPI
 	bool "mmp display controller spi port"
diff --git a/drivers/video/fbdev/mmp/hw/mmp_ctrl.h b/drivers/video/fbdev/mmp/hw/mmp_ctrl.h
index 53301cfdb1a..56fdeab3435 100644
--- a/drivers/video/fbdev/mmp/hw/mmp_ctrl.h
+++ b/drivers/video/fbdev/mmp/hw/mmp_ctrl.h
@@ -167,11 +167,7 @@ struct lcd_regs {
 				PN2_IOPAD_CONTROL) : LCD_TOP_CTRL)
 
 /* dither configure */
-#ifdef CONFIG_CPU_PXA988
-#define LCD_DITHER_CTRL				(0x01EC)
-#else
 #define LCD_DITHER_CTRL				(0x00A0)
-#endif
 
 #define DITHER_TBL_INDEX_SEL(s)		((s) << 16)
 #define DITHER_MODE2(m)				((m) << 12)
@@ -186,15 +182,6 @@ struct lcd_regs {
 #define DITHER_EN1					(1)
 
 /* dither table data was fixed by video bpp of input and output*/
-#ifdef CONFIG_CPU_PXA988
-#define DITHER_TB_4X4_INDEX0		(0x6e4ca280)
-#define DITHER_TB_4X4_INDEX1		(0x5d7f91b3)
-#define DITHER_TB_4X8_INDEX0		(0xb391a280)
-#define DITHER_TB_4X8_INDEX1		(0x7f5d6e4c)
-#define DITHER_TB_4X8_INDEX2		(0x80a291b3)
-#define DITHER_TB_4X8_INDEX3		(0x4c6e5d7f)
-#define LCD_DITHER_TBL_DATA		(0x01F0)
-#else
 #define DITHER_TB_4X4_INDEX0		(0x3b19f7d5)
 #define DITHER_TB_4X4_INDEX1		(0x082ac4e6)
 #define DITHER_TB_4X8_INDEX0		(0xf7d508e6)
@@ -202,7 +189,6 @@ struct lcd_regs {
 #define DITHER_TB_4X8_INDEX2		(0xc4e6d5f7)
 #define DITHER_TB_4X8_INDEX3		(0x082a193b)
 #define LCD_DITHER_TBL_DATA		(0x00A4)
-#endif
 
 /* Video Frame 0&1 start address registers */
 #define	LCD_SPU_DMA_START_ADDR_Y0	0x00C0
@@ -933,16 +919,9 @@ struct lcd_regs {
 #define LCD_PN2_SQULN2_CTRL			(0x02F0)
 #define ALL_LAYER_ALPHA_SEL			(0x02F4)
 
-/* pxa988 has different MASTER_CTRL from MMP3/MMP2 */
-#ifdef CONFIG_CPU_PXA988
-#define TIMING_MASTER_CONTROL			(0x01F4)
-#define MASTER_ENH(id)				(1 << ((id) + 5))
-#define MASTER_ENV(id)				(1 << ((id) + 6))
-#else
 #define TIMING_MASTER_CONTROL			(0x02F8)
 #define MASTER_ENH(id)				(1 << (id))
 #define MASTER_ENV(id)				(1 << ((id) + 4))
-#endif
 
 #define DSI_START_SEL_SHIFT(id)		(((id) << 1) + 8)
 #define timing_master_config(path, dsi_id, lcd_id) \
@@ -1312,19 +1291,8 @@ struct dsi_regs {
 #define	DSI_PHY_TIME_3_CFG_CSR_TIME_REQRDY_MASK		(0xff)
 #define	DSI_PHY_TIME_3_CFG_CSR_TIME_REQRDY_SHIFT	0
 
-/*
- * DSI timings
- * PXA988 has diffrent ESC CLK with MMP2/MMP3
- * it will be used in dsi_set_dphy() in pxa688_phy.c
- * as low power mode clock.
- */
-#ifdef CONFIG_CPU_PXA988
-#define DSI_ESC_CLK				52  /* Unit: Mhz */
-#define DSI_ESC_CLK_T				19  /* Unit: ns */
-#else
 #define DSI_ESC_CLK				66  /* Unit: Mhz */
 #define DSI_ESC_CLK_T				15  /* Unit: ns */
-#endif
 
 /* LVDS */
 /* LVDS_PHY_CTRL */
diff --git a/drivers/video/fbdev/mx3fb.c b/drivers/video/fbdev/mx3fb.c
index 142e860fb52..c645a0a0c34 100644
--- a/drivers/video/fbdev/mx3fb.c
+++ b/drivers/video/fbdev/mx3fb.c
@@ -27,6 +27,7 @@
 #include <linux/clk.h>
 #include <linux/mutex.h>
 #include <linux/dma/ipu-dma.h>
+#include <linux/backlight.h>
 
 #include <linux/platform_data/dma-imx.h>
 #include <linux/platform_data/video-mx3fb.h>
@@ -241,6 +242,7 @@ struct mx3fb_data {
 	void __iomem		*reg_base;
 	spinlock_t		lock;
 	struct device		*dev;
+	struct backlight_device	*bl;
 
 	uint32_t		h_start_width;
 	uint32_t		v_start_width;
@@ -271,6 +273,71 @@ struct mx3fb_info {
 	struct fb_var_screeninfo	cur_var; /* current var info */
 };
 
+static void sdc_set_brightness(struct mx3fb_data *mx3fb, uint8_t value);
+static u32 sdc_get_brightness(struct mx3fb_data *mx3fb);
+
+static int mx3fb_bl_get_brightness(struct backlight_device *bl)
+{
+	struct mx3fb_data *fbd = bl_get_data(bl);
+
+	return sdc_get_brightness(fbd);
+}
+
+static int mx3fb_bl_update_status(struct backlight_device *bl)
+{
+	struct mx3fb_data *fbd = bl_get_data(bl);
+	int brightness = bl->props.brightness;
+
+	if (bl->props.power != FB_BLANK_UNBLANK)
+		brightness = 0;
+	if (bl->props.fb_blank != FB_BLANK_UNBLANK)
+		brightness = 0;
+
+	fbd->backlight_level = (fbd->backlight_level & ~0xFF) | brightness;
+
+	sdc_set_brightness(fbd, fbd->backlight_level);
+
+	return 0;
+}
+
+static const struct backlight_ops mx3fb_lcdc_bl_ops = {
+	.update_status = mx3fb_bl_update_status,
+	.get_brightness = mx3fb_bl_get_brightness,
+};
+
+static void mx3fb_init_backlight(struct mx3fb_data *fbd)
+{
+	struct backlight_properties props;
+	struct backlight_device	*bl;
+
+	if (fbd->bl)
+		return;
+
+	memset(&props, 0, sizeof(struct backlight_properties));
+	props.max_brightness = 0xff;
+	props.type = BACKLIGHT_RAW;
+	sdc_set_brightness(fbd, fbd->backlight_level);
+
+	bl = backlight_device_register("mx3fb-bl", fbd->dev, fbd,
+				       &mx3fb_lcdc_bl_ops, &props);
+	if (IS_ERR(bl)) {
+		dev_err(fbd->dev, "error %ld on backlight register\n",
+				PTR_ERR(bl));
+		return;
+	}
+
+	fbd->bl = bl;
+	bl->props.power = FB_BLANK_UNBLANK;
+	bl->props.fb_blank = FB_BLANK_UNBLANK;
+	bl->props.brightness = mx3fb_bl_get_brightness(bl);
+}
+
+static void mx3fb_exit_backlight(struct mx3fb_data *fbd)
+{
+	if (fbd->bl)
+		backlight_device_unregister(fbd->bl);
+}
+
 static void mx3fb_dma_done(void *);
 
 /* Used fb-mode and bpp. Can be set on kernel command line, therefore file-static. */
@@ -628,6 +695,16 @@ static int sdc_set_global_alpha(struct mx3fb_data *mx3fb, bool enable, uint8_t a
 	return 0;
 }
 
+static u32 sdc_get_brightness(struct mx3fb_data *mx3fb)
+{
+	u32 brightness;
+
+	brightness = mx3fb_read_reg(mx3fb, SDC_PWM_CTRL);
+	brightness = (brightness >> 16) & 0xFF;
+
+	return brightness;
+}
+
 static void sdc_set_brightness(struct mx3fb_data *mx3fb, uint8_t value)
 {
 	dev_dbg(mx3fb->dev, "%s: value = %d\n", __func__, value);
@@ -1496,7 +1573,7 @@ static int mx3fb_probe(struct platform_device *pdev)
 	if (!sdc_reg)
 		return -EINVAL;
 
-	mx3fb = kzalloc(sizeof(*mx3fb), GFP_KERNEL);
+	mx3fb = devm_kzalloc(&pdev->dev, sizeof(*mx3fb), GFP_KERNEL);
 	if (!mx3fb)
 		return -ENOMEM;
 
@@ -1534,6 +1611,8 @@ static int mx3fb_probe(struct platform_device *pdev)
 	if (ret < 0)
 		goto eisdc0;
 
+	mx3fb_init_backlight(mx3fb);
+
 	return 0;
 
 eisdc0:
@@ -1542,7 +1621,6 @@ ersdc0:
 	dmaengine_put();
 	iounmap(mx3fb->reg_base);
 eremap:
-	kfree(mx3fb);
 	dev_err(dev, "mx3fb: failed to register fb\n");
 	return ret;
 }
@@ -1557,11 +1635,12 @@ static int mx3fb_remove(struct platform_device *dev)
 	chan = &mx3_fbi->idmac_channel->dma_chan;
 	release_fbi(fbi);
 
+	mx3fb_exit_backlight(mx3fb);
+
 	dma_release_channel(chan);
 	dmaengine_put();
 
 	iounmap(mx3fb->reg_base);
-	kfree(mx3fb);
 	return 0;
 }
 
diff --git a/drivers/video/fbdev/omap/Kconfig b/drivers/video/fbdev/omap/Kconfig
index 0bc3a936ce2..18c4cb0d569 100644
--- a/drivers/video/fbdev/omap/Kconfig
+++ b/drivers/video/fbdev/omap/Kconfig
@@ -39,6 +39,15 @@ config FB_OMAP_LCD_MIPID
 	  the Mobile Industry Processor Interface DBI-C/DCS
 	  specification. (Supported LCDs: Philips LPH8923, Sharp LS041Y3)
 
+config FB_OMAP_LCD_H3
+	bool "TPS65010 LCD controller on OMAP-H3"
+	depends on MACH_OMAP_H3
+	depends on TPS65010
+	default y
+	help
+	  Say Y here if you want to have support for the LCD on the
+	  H3 board.
+
 config FB_OMAP_DMA_TUNE
         bool "Set DMA SDRAM access priority high"
         depends on FB_OMAP
diff --git a/drivers/video/fbdev/omap/Makefile b/drivers/video/fbdev/omap/Makefile
index 1927faffb5b..732e0718be5 100644
--- a/drivers/video/fbdev/omap/Makefile
+++ b/drivers/video/fbdev/omap/Makefile
@@ -10,17 +10,18 @@ objs-y$(CONFIG_FB_OMAP_LCDC_EXTERNAL) += sossi.o
 
 objs-y$(CONFIG_FB_OMAP_LCDC_HWA742) += hwa742.o
 
-objs-y$(CONFIG_MACH_AMS_DELTA) += lcd_ams_delta.o
-objs-y$(CONFIG_MACH_OMAP_H3) += lcd_h3.o
-objs-y$(CONFIG_MACH_OMAP_PALMTE) += lcd_palmte.o
-objs-y$(CONFIG_MACH_OMAP_PALMTT) += lcd_palmtt.o
-objs-y$(CONFIG_MACH_OMAP_PALMZ71) += lcd_palmz71.o
-objs-$(CONFIG_ARCH_OMAP16XX)$(CONFIG_MACH_OMAP_INNOVATOR) += lcd_inn1610.o
-objs-$(CONFIG_ARCH_OMAP15XX)$(CONFIG_MACH_OMAP_INNOVATOR) += lcd_inn1510.o
-objs-y$(CONFIG_MACH_OMAP_OSK) += lcd_osk.o
-
-objs-y$(CONFIG_FB_OMAP_LCD_MIPID) += lcd_mipid.o
-objs-y$(CONFIG_MACH_HERALD) += lcd_htcherald.o
+lcds-y$(CONFIG_MACH_AMS_DELTA) += lcd_ams_delta.o
+lcds-y$(CONFIG_FB_OMAP_LCD_H3) += lcd_h3.o
+lcds-y$(CONFIG_MACH_OMAP_PALMTE) += lcd_palmte.o
+lcds-y$(CONFIG_MACH_OMAP_PALMTT) += lcd_palmtt.o
+lcds-y$(CONFIG_MACH_OMAP_PALMZ71) += lcd_palmz71.o
+lcds-$(CONFIG_ARCH_OMAP16XX)$(CONFIG_MACH_OMAP_INNOVATOR) += lcd_inn1610.o
+lcds-$(CONFIG_ARCH_OMAP15XX)$(CONFIG_MACH_OMAP_INNOVATOR) += lcd_inn1510.o
+lcds-y$(CONFIG_MACH_OMAP_OSK) += lcd_osk.o
+
+lcds-y$(CONFIG_FB_OMAP_LCD_MIPID) += lcd_mipid.o
+lcds-y$(CONFIG_MACH_HERALD) += lcd_htcherald.o
 
 omapfb-objs := $(objs-yy)
 
+obj-$(CONFIG_FB_OMAP) += $(lcds-yy)
diff --git a/drivers/video/fbdev/omap/lcdc.c b/drivers/video/fbdev/omap/lcdc.c
index b52f62595f6..6efa2591eaa 100644
--- a/drivers/video/fbdev/omap/lcdc.c
+++ b/drivers/video/fbdev/omap/lcdc.c
@@ -74,7 +74,6 @@ static struct omap_lcd_controller {
 	void			(*dma_callback)(void *data);
 	void			*dma_callback_data;
 
-	int			fbmem_allocated;
 	dma_addr_t		vram_phys;
 	void			*vram_virt;
 	unsigned long		vram_size;
@@ -611,42 +610,6 @@ static void lcdc_dma_handler(u16 status, void *data)
 		lcdc.dma_callback(lcdc.dma_callback_data);
 }
 
-static int mmap_kern(void)
-{
-	struct vm_struct	*kvma;
-	struct vm_area_struct	vma;
-	pgprot_t		pgprot;
-	unsigned long		vaddr;
-
-	kvma = get_vm_area(lcdc.vram_size, VM_IOREMAP);
-	if (kvma == NULL) {
-		dev_err(lcdc.fbdev->dev, "can't get kernel vm area\n");
-		return -ENOMEM;
-	}
-	vma.vm_mm = &init_mm;
-
-	vaddr = (unsigned long)kvma->addr;
-	vma.vm_start = vaddr;
-	vma.vm_end = vaddr + lcdc.vram_size;
-
-	pgprot = pgprot_writecombine(pgprot_kernel);
-	if (io_remap_pfn_range(&vma, vaddr,
-			   lcdc.vram_phys >> PAGE_SHIFT,
-			   lcdc.vram_size, pgprot) < 0) {
-		dev_err(lcdc.fbdev->dev, "kernel mmap for FB memory failed\n");
-		return -EAGAIN;
-	}
-
-	lcdc.vram_virt = (void *)vaddr;
-
-	return 0;
-}
-
-static void unmap_kern(void)
-{
-	vunmap(lcdc.vram_virt);
-}
-
 static int alloc_palette_ram(void)
 {
 	lcdc.palette_virt = dma_alloc_writecombine(lcdc.fbdev->dev,
@@ -703,8 +666,6 @@ static void free_fbmem(void)
 
 static int setup_fbmem(struct omapfb_mem_desc *req_md)
 {
-	int r;
-
 	if (!req_md->region_cnt) {
 		dev_err(lcdc.fbdev->dev, "no memory regions defined\n");
 		return -EINVAL;
@@ -715,31 +676,7 @@ static int setup_fbmem(struct omapfb_mem_desc *req_md)
 		req_md->region_cnt = 1;
 	}
 
-	if (req_md->region[0].paddr == 0) {
-		lcdc.fbmem_allocated = 1;
-		if ((r = alloc_fbmem(&req_md->region[0])) < 0)
-			return r;
-		return 0;
-	}
-
-	lcdc.vram_phys = req_md->region[0].paddr;
-	lcdc.vram_size = req_md->region[0].size;
-
-	if ((r = mmap_kern()) < 0)
-		return r;
-
-	dev_dbg(lcdc.fbdev->dev, "vram at %08x size %08lx mapped to 0x%p\n",
-		 lcdc.vram_phys, lcdc.vram_size, lcdc.vram_virt);
-
-	return 0;
-}
-
-static void cleanup_fbmem(void)
-{
-	if (lcdc.fbmem_allocated)
-		free_fbmem();
-	else
-		unmap_kern();
+	return alloc_fbmem(&req_md->region[0]);
 }
 
 static int omap_lcdc_init(struct omapfb_device *fbdev, int ext_mode,
@@ -833,7 +770,7 @@ static void omap_lcdc_cleanup(void)
 {
 	if (!lcdc.ext_mode)
 		free_palette_ram();
-	cleanup_fbmem();
+	free_fbmem();
 	omap_free_lcd_dma();
 	free_irq(OMAP_LCDC_IRQ, lcdc.fbdev);
 	clk_disable(lcdc.lcd_ck);
diff --git a/drivers/video/fbdev/omap/omapfb_main.c b/drivers/video/fbdev/omap/omapfb_main.c
index e4fc6d9b537..d8d028d9871 100644
--- a/drivers/video/fbdev/omap/omapfb_main.c
+++ b/drivers/video/fbdev/omap/omapfb_main.c
@@ -1823,6 +1823,7 @@ void omapfb_register_panel(struct lcd_panel *panel)
 	if (fbdev_pdev != NULL)
 		omapfb_do_probe(fbdev_pdev, fbdev_panel);
 }
+EXPORT_SYMBOL_GPL(omapfb_register_panel);
 
 /* Called when the device is being detached from the driver */
 static int omapfb_remove(struct platform_device *pdev)
diff --git a/drivers/video/fbdev/omap2/Makefile b/drivers/video/fbdev/omap2/Makefile
index bf8127df8c7..f8745ec369c 100644
--- a/drivers/video/fbdev/omap2/Makefile
+++ b/drivers/video/fbdev/omap2/Makefile
@@ -1,5 +1,5 @@
 obj-$(CONFIG_OMAP2_VRFB) += vrfb.o
 
-obj-$(CONFIG_OMAP2_DSS) += dss/
+obj-y += dss/
 obj-y += displays-new/
 obj-$(CONFIG_FB_OMAP2) += omapfb/
diff --git a/drivers/video/fbdev/omap2/displays-new/connector-hdmi.c b/drivers/video/fbdev/omap2/displays-new/connector-hdmi.c
index 29ed21b9dce..4420ccb69aa 100644
--- a/drivers/video/fbdev/omap2/displays-new/connector-hdmi.c
+++ b/drivers/video/fbdev/omap2/displays-new/connector-hdmi.c
@@ -13,6 +13,7 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/of.h>
+#include <linux/of_gpio.h>
 
 #include <drm/drm_edid.h>
 
@@ -43,6 +44,8 @@ struct panel_drv_data {
 	struct device *dev;
 
 	struct omap_video_timings timings;
+
+	int hpd_gpio;
 };
 
 #define to_panel_data(x) container_of(x, struct panel_drv_data, dssdev)
@@ -161,7 +164,10 @@ static bool hdmic_detect(struct omap_dss_device *dssdev)
 	struct panel_drv_data *ddata = to_panel_data(dssdev);
 	struct omap_dss_device *in = ddata->in;
 
-	return in->ops.hdmi->detect(in);
+	if (gpio_is_valid(ddata->hpd_gpio))
+		return gpio_get_value_cansleep(ddata->hpd_gpio);
+	else
+		return in->ops.hdmi->detect(in);
 }
 
 static int hdmic_audio_enable(struct omap_dss_device *dssdev)
@@ -288,6 +294,8 @@ static int hdmic_probe_pdata(struct platform_device *pdev)
 
 	pdata = dev_get_platdata(&pdev->dev);
 
+	ddata->hpd_gpio = -ENODEV;
+
 	in = omap_dss_find_output(pdata->source);
 	if (in == NULL) {
 		dev_err(&pdev->dev, "Failed to find video source\n");
@@ -307,6 +315,14 @@ static int hdmic_probe_of(struct platform_device *pdev)
 	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
 	struct device_node *node = pdev->dev.of_node;
 	struct omap_dss_device *in;
+	int gpio;
+
+	/* HPD GPIO */
+	gpio = of_get_named_gpio(node, "hpd-gpios", 0);
+	if (gpio_is_valid(gpio))
+		ddata->hpd_gpio = gpio;
+	else
+		ddata->hpd_gpio = -ENODEV;
 
 	in = omapdss_of_find_source_for_first_ep(node);
 	if (IS_ERR(in)) {
@@ -344,6 +360,13 @@ static int hdmic_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
+	if (gpio_is_valid(ddata->hpd_gpio)) {
+		r = devm_gpio_request_one(&pdev->dev, ddata->hpd_gpio,
+				GPIOF_DIR_IN, "hdmi_hpd");
+		if (r)
+			goto err_reg;
+	}
+
 	ddata->timings = hdmic_default_timings;
 
 	dssdev = &ddata->dssdev;
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-dpi.c b/drivers/video/fbdev/omap2/displays-new/panel-dpi.c
index 5f8f7e7c81e..3636b61dc9b 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-dpi.c
+++ b/drivers/video/fbdev/omap2/displays-new/panel-dpi.c
@@ -13,9 +13,12 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
 
 #include <video/omapdss.h>
 #include <video/omap-panel-data.h>
+#include <video/of_display_timing.h>
 
 struct panel_drv_data {
 	struct omap_dss_device dssdev;
@@ -25,8 +28,10 @@ struct panel_drv_data {
 
 	struct omap_video_timings videomode;
 
+	/* used for non-DT boot, to be removed */
 	int backlight_gpio;
-	int enable_gpio;
+
+	struct gpio_desc *enable_gpio;
 };
 
 #define to_panel_data(p) container_of(p, struct panel_drv_data, dssdev)
@@ -70,15 +75,16 @@ static int panel_dpi_enable(struct omap_dss_device *dssdev)
 	if (omapdss_device_is_enabled(dssdev))
 		return 0;
 
-	in->ops.dpi->set_data_lines(in, ddata->data_lines);
+	if (ddata->data_lines)
+		in->ops.dpi->set_data_lines(in, ddata->data_lines);
 	in->ops.dpi->set_timings(in, &ddata->videomode);
 
 	r = in->ops.dpi->enable(in);
 	if (r)
 		return r;
 
-	if (gpio_is_valid(ddata->enable_gpio))
-		gpio_set_value_cansleep(ddata->enable_gpio, 1);
+	if (ddata->enable_gpio)
+		gpiod_set_value_cansleep(ddata->enable_gpio, 1);
 
 	if (gpio_is_valid(ddata->backlight_gpio))
 		gpio_set_value_cansleep(ddata->backlight_gpio, 1);
@@ -96,8 +102,8 @@ static void panel_dpi_disable(struct omap_dss_device *dssdev)
 	if (!omapdss_device_is_enabled(dssdev))
 		return;
 
-	if (gpio_is_valid(ddata->enable_gpio))
-		gpio_set_value_cansleep(ddata->enable_gpio, 0);
+	if (ddata->enable_gpio)
+		gpiod_set_value_cansleep(ddata->enable_gpio, 0);
 
 	if (gpio_is_valid(ddata->backlight_gpio))
 		gpio_set_value_cansleep(ddata->backlight_gpio, 0);
@@ -156,6 +162,7 @@ static int panel_dpi_probe_pdata(struct platform_device *pdev)
 	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
 	struct omap_dss_device *dssdev, *in;
 	struct videomode vm;
+	int r;
 
 	pdata = dev_get_platdata(&pdev->dev);
 
@@ -176,10 +183,65 @@ static int panel_dpi_probe_pdata(struct platform_device *pdev)
 	dssdev = &ddata->dssdev;
 	dssdev->name = pdata->name;
 
-	ddata->enable_gpio = pdata->enable_gpio;
+	r = devm_gpio_request_one(&pdev->dev, pdata->enable_gpio,
+					GPIOF_OUT_INIT_LOW, "panel enable");
+	if (r)
+		goto err_gpio;
+
+	ddata->enable_gpio = gpio_to_desc(pdata->enable_gpio);
+
 	ddata->backlight_gpio = pdata->backlight_gpio;
 
 	return 0;
+
+err_gpio:
+	omap_dss_put_device(ddata->in);
+	return r;
+}
+
+static int panel_dpi_probe_of(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct device_node *node = pdev->dev.of_node;
+	struct omap_dss_device *in;
+	int r;
+	struct display_timing timing;
+	struct videomode vm;
+	struct gpio_desc *gpio;
+
+	gpio = devm_gpiod_get(&pdev->dev, "enable");
+
+	if (IS_ERR(gpio)) {
+		if (PTR_ERR(gpio) != -ENOENT)
+			return PTR_ERR(gpio);
+		else
+			gpio = NULL;
+	} else {
+		gpiod_direction_output(gpio, 0);
+	}
+
+	ddata->enable_gpio = gpio;
+
+	ddata->backlight_gpio = -ENOENT;
+
+	r = of_get_display_timing(node, "panel-timing", &timing);
+	if (r) {
+		dev_err(&pdev->dev, "failed to get video timing\n");
+		return r;
+	}
+
+	videomode_from_timing(&timing, &vm);
+	videomode_to_omap_video_timings(&vm, &ddata->videomode);
+
+	in = omapdss_of_find_source_for_first_ep(node);
+	if (IS_ERR(in)) {
+		dev_err(&pdev->dev, "failed to find video source\n");
+		return PTR_ERR(in);
+	}
+
+	ddata->in = in;
+
+	return 0;
 }
 
 static int panel_dpi_probe(struct platform_device *pdev)
@@ -198,17 +260,14 @@ static int panel_dpi_probe(struct platform_device *pdev)
 		r = panel_dpi_probe_pdata(pdev);
 		if (r)
 			return r;
+	} else if (pdev->dev.of_node) {
+		r = panel_dpi_probe_of(pdev);
+		if (r)
+			return r;
 	} else {
 		return -ENODEV;
 	}
 
-	if (gpio_is_valid(ddata->enable_gpio)) {
-		r = devm_gpio_request_one(&pdev->dev, ddata->enable_gpio,
-				GPIOF_OUT_INIT_LOW, "panel enable");
-		if (r)
-			goto err_gpio;
-	}
-
 	if (gpio_is_valid(ddata->backlight_gpio)) {
 		r = devm_gpio_request_one(&pdev->dev, ddata->backlight_gpio,
 				GPIOF_OUT_INIT_LOW, "panel backlight");
@@ -254,12 +313,20 @@ static int __exit panel_dpi_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static const struct of_device_id panel_dpi_of_match[] = {
+	{ .compatible = "omapdss,panel-dpi", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, panel_dpi_of_match);
+
 static struct platform_driver panel_dpi_driver = {
 	.probe = panel_dpi_probe,
 	.remove = __exit_p(panel_dpi_remove),
 	.driver = {
 		.name = "panel-dpi",
 		.owner = THIS_MODULE,
+		.of_match_table = panel_dpi_of_match,
 	},
 };
 
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-lgphilips-lb035q02.c b/drivers/video/fbdev/omap2/displays-new/panel-lgphilips-lb035q02.c
index 2e6b513222d..cc5b5124e0b 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-lgphilips-lb035q02.c
+++ b/drivers/video/fbdev/omap2/displays-new/panel-lgphilips-lb035q02.c
@@ -50,9 +50,10 @@ struct panel_drv_data {
 
 	struct omap_video_timings videomode;
 
-	int reset_gpio;
+	/* used for non-DT boot, to be removed */
 	int backlight_gpio;
-	int enable_gpio;
+
+	struct gpio_desc *enable_gpio;
 };
 
 #define to_panel_data(p) container_of(p, struct panel_drv_data, dssdev)
@@ -158,15 +159,16 @@ static int lb035q02_enable(struct omap_dss_device *dssdev)
 	if (omapdss_device_is_enabled(dssdev))
 		return 0;
 
-	in->ops.dpi->set_data_lines(in, ddata->data_lines);
+	if (ddata->data_lines)
+		in->ops.dpi->set_data_lines(in, ddata->data_lines);
 	in->ops.dpi->set_timings(in, &ddata->videomode);
 
 	r = in->ops.dpi->enable(in);
 	if (r)
 		return r;
 
-	if (gpio_is_valid(ddata->enable_gpio))
-		gpio_set_value_cansleep(ddata->enable_gpio, 1);
+	if (ddata->enable_gpio)
+		gpiod_set_value_cansleep(ddata->enable_gpio, 1);
 
 	if (gpio_is_valid(ddata->backlight_gpio))
 		gpio_set_value_cansleep(ddata->backlight_gpio, 1);
@@ -184,8 +186,8 @@ static void lb035q02_disable(struct omap_dss_device *dssdev)
 	if (!omapdss_device_is_enabled(dssdev))
 		return;
 
-	if (gpio_is_valid(ddata->enable_gpio))
-		gpio_set_value_cansleep(ddata->enable_gpio, 0);
+	if (ddata->enable_gpio)
+		gpiod_set_value_cansleep(ddata->enable_gpio, 0);
 
 	if (gpio_is_valid(ddata->backlight_gpio))
 		gpio_set_value_cansleep(ddata->backlight_gpio, 0);
@@ -243,6 +245,7 @@ static int lb035q02_probe_pdata(struct spi_device *spi)
 	const struct panel_lb035q02_platform_data *pdata;
 	struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
 	struct omap_dss_device *dssdev, *in;
+	int r;
 
 	pdata = dev_get_platdata(&spi->dev);
 
@@ -260,10 +263,48 @@ static int lb035q02_probe_pdata(struct spi_device *spi)
 	dssdev = &ddata->dssdev;
 	dssdev->name = pdata->name;
 
-	ddata->enable_gpio = pdata->enable_gpio;
+	r = devm_gpio_request_one(&spi->dev, pdata->enable_gpio,
+					GPIOF_OUT_INIT_LOW, "panel enable");
+	if (r)
+		goto err_gpio;
+
+	ddata->enable_gpio = gpio_to_desc(pdata->enable_gpio);
+
 	ddata->backlight_gpio = pdata->backlight_gpio;
 
 	return 0;
+err_gpio:
+	omap_dss_put_device(ddata->in);
+	return r;
+}
+
+static int lb035q02_probe_of(struct spi_device *spi)
+{
+	struct device_node *node = spi->dev.of_node;
+	struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+	struct omap_dss_device *in;
+	struct gpio_desc *gpio;
+
+	gpio = devm_gpiod_get(&spi->dev, "enable");
+	if (IS_ERR(gpio)) {
+		dev_err(&spi->dev, "failed to parse enable gpio\n");
+		return PTR_ERR(gpio);
+	} else {
+		gpiod_direction_output(gpio, 0);
+		ddata->enable_gpio = gpio;
+	}
+
+	ddata->backlight_gpio = -ENOENT;
+
+	in = omapdss_of_find_source_for_first_ep(node);
+	if (IS_ERR(in)) {
+		dev_err(&spi->dev, "failed to find video source\n");
+		return PTR_ERR(in);
+	}
+
+	ddata->in = in;
+
+	return 0;
 }
 
 static int lb035q02_panel_spi_probe(struct spi_device *spi)
@@ -284,17 +325,14 @@ static int lb035q02_panel_spi_probe(struct spi_device *spi)
 		r = lb035q02_probe_pdata(spi);
 		if (r)
 			return r;
+	} else if (spi->dev.of_node) {
+		r = lb035q02_probe_of(spi);
+		if (r)
+			return r;
 	} else {
 		return -ENODEV;
 	}
 
-	if (gpio_is_valid(ddata->enable_gpio)) {
-		r = devm_gpio_request_one(&spi->dev, ddata->enable_gpio,
-				GPIOF_OUT_INIT_LOW, "panel enable");
-		if (r)
-			goto err_gpio;
-	}
-
 	if (gpio_is_valid(ddata->backlight_gpio)) {
 		r = devm_gpio_request_one(&spi->dev, ddata->backlight_gpio,
 				GPIOF_OUT_INIT_LOW, "panel backlight");
@@ -342,17 +380,26 @@ static int lb035q02_panel_spi_remove(struct spi_device *spi)
 	return 0;
 }
 
+static const struct of_device_id lb035q02_of_match[] = {
+	{ .compatible = "omapdss,lgphilips,lb035q02", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, lb035q02_of_match);
+
 static struct spi_driver lb035q02_spi_driver = {
 	.probe		= lb035q02_panel_spi_probe,
 	.remove		= lb035q02_panel_spi_remove,
 	.driver		= {
 		.name	= "panel_lgphilips_lb035q02",
 		.owner	= THIS_MODULE,
+		.of_match_table = lb035q02_of_match,
 	},
 };
 
 module_spi_driver(lb035q02_spi_driver);
 
+MODULE_ALIAS("spi:lgphilips,lb035q02");
 MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ti.com>");
 MODULE_DESCRIPTION("LG.Philips LB035Q02 LCD Panel driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-nec-nl8048hl11.c b/drivers/video/fbdev/omap2/displays-new/panel-nec-nl8048hl11.c
index 996fa004b48..3595f111aa3 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-nec-nl8048hl11.c
+++ b/drivers/video/fbdev/omap2/displays-new/panel-nec-nl8048hl11.c
@@ -16,6 +16,7 @@
 #include <linux/spi/spi.h>
 #include <linux/fb.h>
 #include <linux/gpio.h>
+#include <linux/of_gpio.h>
 
 #include <video/omapdss.h>
 #include <video/omap-panel-data.h>
@@ -156,7 +157,8 @@ static int nec_8048_enable(struct omap_dss_device *dssdev)
 	if (omapdss_device_is_enabled(dssdev))
 		return 0;
 
-	in->ops.dpi->set_data_lines(in, ddata->data_lines);
+	if (ddata->data_lines)
+		in->ops.dpi->set_data_lines(in, ddata->data_lines);
 	in->ops.dpi->set_timings(in, &ddata->videomode);
 
 	r = in->ops.dpi->enable(in);
@@ -258,6 +260,34 @@ static int nec_8048_probe_pdata(struct spi_device *spi)
 	return 0;
 }
 
+static int nec_8048_probe_of(struct spi_device *spi)
+{
+	struct device_node *node = spi->dev.of_node;
+	struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+	struct omap_dss_device *in;
+	int gpio;
+
+	gpio = of_get_named_gpio(node, "reset-gpios", 0);
+	if (!gpio_is_valid(gpio)) {
+		dev_err(&spi->dev, "failed to parse enable gpio\n");
+		return gpio;
+	}
+	ddata->res_gpio = gpio;
+
+	/* XXX the panel spec doesn't mention any QVGA pin?? */
+	ddata->qvga_gpio = -ENOENT;
+
+	in = omapdss_of_find_source_for_first_ep(node);
+	if (IS_ERR(in)) {
+		dev_err(&spi->dev, "failed to find video source\n");
+		return PTR_ERR(in);
+	}
+
+	ddata->in = in;
+
+	return 0;
+}
+
 static int nec_8048_probe(struct spi_device *spi)
 {
 	struct panel_drv_data *ddata;
@@ -289,6 +319,10 @@ static int nec_8048_probe(struct spi_device *spi)
 		r = nec_8048_probe_pdata(spi);
 		if (r)
 			return r;
+	} else if (spi->dev.of_node) {
+		r = nec_8048_probe_of(spi);
+		if (r)
+			return r;
 	} else {
 		return -ENODEV;
 	}
@@ -377,11 +411,19 @@ static SIMPLE_DEV_PM_OPS(nec_8048_pm_ops, nec_8048_suspend,
 #define NEC_8048_PM_OPS NULL
 #endif
 
+static const struct of_device_id nec_8048_of_match[] = {
+	{ .compatible = "omapdss,nec,nl8048hl11", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, nec_8048_of_match);
+
 static struct spi_driver nec_8048_driver = {
 	.driver = {
 		.name	= "panel-nec-nl8048hl11",
 		.owner	= THIS_MODULE,
 		.pm	= NEC_8048_PM_OPS,
+		.of_match_table = nec_8048_of_match,
 	},
 	.probe	= nec_8048_probe,
 	.remove	= nec_8048_remove,
@@ -389,6 +431,7 @@ static struct spi_driver nec_8048_driver = {
 
 module_spi_driver(nec_8048_driver);
 
+MODULE_ALIAS("spi:nec,nl8048hl11");
 MODULE_AUTHOR("Erik Gilling <konkers@android.com>");
 MODULE_DESCRIPTION("NEC-NL8048HL11 Driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-sharp-ls037v7dw01.c b/drivers/video/fbdev/omap2/displays-new/panel-sharp-ls037v7dw01.c
index b2f710be565..f1f72ce50a1 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-sharp-ls037v7dw01.c
+++ b/drivers/video/fbdev/omap2/displays-new/panel-sharp-ls037v7dw01.c
@@ -12,25 +12,28 @@
 #include <linux/delay.h>
 #include <linux/gpio.h>
 #include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
-
+#include <linux/regulator/consumer.h>
 #include <video/omapdss.h>
 #include <video/omap-panel-data.h>
 
 struct panel_drv_data {
 	struct omap_dss_device dssdev;
 	struct omap_dss_device *in;
+	struct regulator *vcc;
 
 	int data_lines;
 
 	struct omap_video_timings videomode;
 
-	int resb_gpio;
-	int ini_gpio;
-	int mo_gpio;
-	int lr_gpio;
-	int ud_gpio;
+	struct gpio_desc *resb_gpio;	/* low = reset active min 20 us */
+	struct gpio_desc *ini_gpio;	/* high = power on */
+	struct gpio_desc *mo_gpio;	/* low = 480x640, high = 240x320 */
+	struct gpio_desc *lr_gpio;	/* high = conventional horizontal scanning */
+	struct gpio_desc *ud_gpio;	/* high = conventional vertical scanning */
 };
 
 static const struct omap_video_timings sharp_ls_timings = {
@@ -95,21 +98,30 @@ static int sharp_ls_enable(struct omap_dss_device *dssdev)
 	if (omapdss_device_is_enabled(dssdev))
 		return 0;
 
-	in->ops.dpi->set_data_lines(in, ddata->data_lines);
+	if (ddata->data_lines)
+		in->ops.dpi->set_data_lines(in, ddata->data_lines);
 	in->ops.dpi->set_timings(in, &ddata->videomode);
 
+	if (ddata->vcc) {
+		r = regulator_enable(ddata->vcc);
+		if (r != 0)
+			return r;
+	}
+
 	r = in->ops.dpi->enable(in);
-	if (r)
+	if (r) {
+		regulator_disable(ddata->vcc);
 		return r;
+	}
 
 	/* wait couple of vsyncs until enabling the LCD */
 	msleep(50);
 
-	if (gpio_is_valid(ddata->resb_gpio))
-		gpio_set_value_cansleep(ddata->resb_gpio, 1);
+	if (ddata->resb_gpio)
+		gpiod_set_value_cansleep(ddata->resb_gpio, 1);
 
-	if (gpio_is_valid(ddata->ini_gpio))
-		gpio_set_value_cansleep(ddata->ini_gpio, 1);
+	if (ddata->ini_gpio)
+		gpiod_set_value_cansleep(ddata->ini_gpio, 1);
 
 	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
 
@@ -124,11 +136,11 @@ static void sharp_ls_disable(struct omap_dss_device *dssdev)
 	if (!omapdss_device_is_enabled(dssdev))
 		return;
 
-	if (gpio_is_valid(ddata->ini_gpio))
-		gpio_set_value_cansleep(ddata->ini_gpio, 0);
+	if (ddata->ini_gpio)
+		gpiod_set_value_cansleep(ddata->ini_gpio, 0);
 
-	if (gpio_is_valid(ddata->resb_gpio))
-		gpio_set_value_cansleep(ddata->resb_gpio, 0);
+	if (ddata->resb_gpio)
+		gpiod_set_value_cansleep(ddata->resb_gpio, 0);
 
 	/* wait at least 5 vsyncs after disabling the LCD */
 
@@ -136,6 +148,9 @@ static void sharp_ls_disable(struct omap_dss_device *dssdev)
 
 	in->ops.dpi->disable(in);
 
+	if (ddata->vcc)
+		regulator_disable(ddata->vcc);
+
 	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
 }
 
@@ -182,11 +197,32 @@ static struct omap_dss_driver sharp_ls_ops = {
 	.get_resolution	= omapdss_default_get_resolution,
 };
 
+static int sharp_ls_get_gpio(struct device *dev, int gpio, unsigned long flags,
+		  char *desc, struct gpio_desc **gpiod)
+{
+	struct gpio_desc *gd;
+	int r;
+
+	*gpiod = NULL;
+
+	r = devm_gpio_request_one(dev, gpio, flags, desc);
+	if (r)
+		return r == -ENOENT ? 0 : r;
+
+	gd = gpio_to_desc(gpio);
+	if (IS_ERR(gd))
+		return PTR_ERR(gd) == -ENOENT ? 0 : PTR_ERR(gd);
+
+	*gpiod = gd;
+	return 0;
+}
+
 static int sharp_ls_probe_pdata(struct platform_device *pdev)
 {
 	const struct panel_sharp_ls037v7dw01_platform_data *pdata;
 	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
 	struct omap_dss_device *dssdev, *in;
+	int r;
 
 	pdata = dev_get_platdata(&pdev->dev);
 
@@ -204,11 +240,95 @@ static int sharp_ls_probe_pdata(struct platform_device *pdev)
 	dssdev = &ddata->dssdev;
 	dssdev->name = pdata->name;
 
-	ddata->resb_gpio = pdata->resb_gpio;
-	ddata->ini_gpio = pdata->ini_gpio;
-	ddata->mo_gpio = pdata->mo_gpio;
-	ddata->lr_gpio = pdata->lr_gpio;
-	ddata->ud_gpio = pdata->ud_gpio;
+	r = sharp_ls_get_gpio(&pdev->dev, pdata->mo_gpio, GPIOF_OUT_INIT_LOW,
+		"lcd MO", &ddata->mo_gpio);
+	if (r)
+		return r;
+	r = sharp_ls_get_gpio(&pdev->dev, pdata->lr_gpio, GPIOF_OUT_INIT_HIGH,
+		"lcd LR", &ddata->lr_gpio);
+	if (r)
+		return r;
+	r = sharp_ls_get_gpio(&pdev->dev, pdata->ud_gpio, GPIOF_OUT_INIT_HIGH,
+		"lcd UD", &ddata->ud_gpio);
+	if (r)
+		return r;
+	r = sharp_ls_get_gpio(&pdev->dev, pdata->resb_gpio, GPIOF_OUT_INIT_LOW,
+		"lcd RESB", &ddata->resb_gpio);
+	if (r)
+		return r;
+	r = sharp_ls_get_gpio(&pdev->dev, pdata->ini_gpio, GPIOF_OUT_INIT_LOW,
+		"lcd INI", &ddata->ini_gpio);
+	if (r)
+		return r;
+
+	return 0;
+}
+
+static  int sharp_ls_get_gpio_of(struct device *dev, int index, int val,
+	const char *desc, struct gpio_desc **gpiod)
+{
+	struct gpio_desc *gd;
+	int r;
+
+	*gpiod = NULL;
+
+	gd = devm_gpiod_get_index(dev, desc, index);
+	if (IS_ERR(gd))
+		return PTR_ERR(gd) == -ENOENT ? 0 : PTR_ERR(gd);
+
+	r = gpiod_direction_output(gd, val);
+	if (r)
+		return r;
+
+	*gpiod = gd;
+	return 0;
+}
+
+static int sharp_ls_probe_of(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct device_node *node = pdev->dev.of_node;
+	struct omap_dss_device *in;
+	int r;
+
+	ddata->vcc = devm_regulator_get(&pdev->dev, "envdd");
+	if (IS_ERR(ddata->vcc)) {
+		dev_err(&pdev->dev, "failed to get regulator\n");
+		return PTR_ERR(ddata->vcc);
+	}
+
+	/* lcd INI */
+	r = sharp_ls_get_gpio_of(&pdev->dev, 0, 0, "enable", &ddata->ini_gpio);
+	if (r)
+		return r;
+
+	/* lcd RESB */
+	r = sharp_ls_get_gpio_of(&pdev->dev, 0, 0, "reset", &ddata->resb_gpio);
+	if (r)
+		return r;
+
+	/* lcd MO */
+	r = sharp_ls_get_gpio_of(&pdev->dev, 0, 0, "mode", &ddata->mo_gpio);
+	if (r)
+		return r;
+
+	/* lcd LR */
+	r = sharp_ls_get_gpio_of(&pdev->dev, 1, 1, "mode", &ddata->lr_gpio);
+	if (r)
+		return r;
+
+	/* lcd UD */
+	r = sharp_ls_get_gpio_of(&pdev->dev, 2, 1, "mode", &ddata->ud_gpio);
+	if (r)
+		return r;
+
+	in = omapdss_of_find_source_for_first_ep(node);
+	if (IS_ERR(in)) {
+		dev_err(&pdev->dev, "failed to find video source\n");
+		return PTR_ERR(in);
+	}
+
+	ddata->in = in;
 
 	return 0;
 }
@@ -229,45 +349,14 @@ static int sharp_ls_probe(struct platform_device *pdev)
 		r = sharp_ls_probe_pdata(pdev);
 		if (r)
 			return r;
+	} else if (pdev->dev.of_node) {
+		r = sharp_ls_probe_of(pdev);
+		if (r)
+			return r;
 	} else {
 		return -ENODEV;
 	}
 
-	if (gpio_is_valid(ddata->mo_gpio)) {
-		r = devm_gpio_request_one(&pdev->dev, ddata->mo_gpio,
-				GPIOF_OUT_INIT_LOW, "lcd MO");
-		if (r)
-			goto err_gpio;
-	}
-
-	if (gpio_is_valid(ddata->lr_gpio)) {
-		r = devm_gpio_request_one(&pdev->dev, ddata->lr_gpio,
-				GPIOF_OUT_INIT_HIGH, "lcd LR");
-		if (r)
-			goto err_gpio;
-	}
-
-	if (gpio_is_valid(ddata->ud_gpio)) {
-		r = devm_gpio_request_one(&pdev->dev, ddata->ud_gpio,
-				GPIOF_OUT_INIT_HIGH, "lcd UD");
-		if (r)
-			goto err_gpio;
-	}
-
-	if (gpio_is_valid(ddata->resb_gpio)) {
-		r = devm_gpio_request_one(&pdev->dev, ddata->resb_gpio,
-				GPIOF_OUT_INIT_LOW, "lcd RESB");
-		if (r)
-			goto err_gpio;
-	}
-
-	if (gpio_is_valid(ddata->ini_gpio)) {
-		r = devm_gpio_request_one(&pdev->dev, ddata->ini_gpio,
-				GPIOF_OUT_INIT_LOW, "lcd INI");
-		if (r)
-			goto err_gpio;
-	}
-
 	ddata->videomode = sharp_ls_timings;
 
 	dssdev = &ddata->dssdev;
@@ -287,7 +376,6 @@ static int sharp_ls_probe(struct platform_device *pdev)
 	return 0;
 
 err_reg:
-err_gpio:
 	omap_dss_put_device(ddata->in);
 	return r;
 }
@@ -308,12 +396,20 @@ static int __exit sharp_ls_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static const struct of_device_id sharp_ls_of_match[] = {
+	{ .compatible = "omapdss,sharp,ls037v7dw01", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, sharp_ls_of_match);
+
 static struct platform_driver sharp_ls_driver = {
 	.probe = sharp_ls_probe,
 	.remove = __exit_p(sharp_ls_remove),
 	.driver = {
 		.name = "panel-sharp-ls037v7dw01",
 		.owner = THIS_MODULE,
+		.of_match_table = sharp_ls_of_match,
 	},
 };
 
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-tpo-td028ttec1.c b/drivers/video/fbdev/omap2/displays-new/panel-tpo-td028ttec1.c
index fae6adc005a..728808bccee 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-tpo-td028ttec1.c
+++ b/drivers/video/fbdev/omap2/displays-new/panel-tpo-td028ttec1.c
@@ -206,7 +206,8 @@ static int td028ttec1_panel_enable(struct omap_dss_device *dssdev)
 	if (omapdss_device_is_enabled(dssdev))
 		return 0;
 
-	in->ops.dpi->set_data_lines(in, ddata->data_lines);
+	if (ddata->data_lines)
+		in->ops.dpi->set_data_lines(in, ddata->data_lines);
 	in->ops.dpi->set_timings(in, &ddata->videomode);
 
 	r = in->ops.dpi->enable(in);
@@ -389,6 +390,23 @@ static int td028ttec1_panel_probe_pdata(struct spi_device *spi)
 	return 0;
 }
 
+static int td028ttec1_probe_of(struct spi_device *spi)
+{
+	struct device_node *node = spi->dev.of_node;
+	struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+	struct omap_dss_device *in;
+
+	in = omapdss_of_find_source_for_first_ep(node);
+	if (IS_ERR(in)) {
+		dev_err(&spi->dev, "failed to find video source\n");
+		return PTR_ERR(in);
+	}
+
+	ddata->in = in;
+
+	return 0;
+}
+
 static int td028ttec1_panel_probe(struct spi_device *spi)
 {
 	struct panel_drv_data *ddata;
@@ -418,6 +436,10 @@ static int td028ttec1_panel_probe(struct spi_device *spi)
 		r = td028ttec1_panel_probe_pdata(spi);
 		if (r)
 			return r;
+	} else if (spi->dev.of_node) {
+		r = td028ttec1_probe_of(spi);
+		if (r)
+			return r;
 	} else {
 		return -ENODEV;
 	}
@@ -463,6 +485,13 @@ static int td028ttec1_panel_remove(struct spi_device *spi)
 	return 0;
 }
 
+static const struct of_device_id td028ttec1_of_match[] = {
+	{ .compatible = "omapdss,toppoly,td028ttec1", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, td028ttec1_of_match);
+
 static struct spi_driver td028ttec1_spi_driver = {
 	.probe		= td028ttec1_panel_probe,
 	.remove		= td028ttec1_panel_remove,
@@ -470,11 +499,13 @@ static struct spi_driver td028ttec1_spi_driver = {
 	.driver         = {
 		.name   = "panel-tpo-td028ttec1",
 		.owner  = THIS_MODULE,
+		.of_match_table = td028ttec1_of_match,
 	},
 };
 
 module_spi_driver(td028ttec1_spi_driver);
 
+MODULE_ALIAS("spi:toppoly,td028ttec1");
 MODULE_AUTHOR("H. Nikolaus Schaller <hns@goldelico.com>");
 MODULE_DESCRIPTION("Toppoly TD028TTEC1 panel driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-tpo-td043mtea1.c b/drivers/video/fbdev/omap2/displays-new/panel-tpo-td043mtea1.c
index 875b40263b3..de78ab0caaa 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-tpo-td043mtea1.c
+++ b/drivers/video/fbdev/omap2/displays-new/panel-tpo-td043mtea1.c
@@ -17,6 +17,7 @@
 #include <linux/gpio.h>
 #include <linux/err.h>
 #include <linux/slab.h>
+#include <linux/of_gpio.h>
 
 #include <video/omapdss.h>
 #include <video/omap-panel-data.h>
@@ -376,7 +377,8 @@ static int tpo_td043_enable(struct omap_dss_device *dssdev)
 	if (omapdss_device_is_enabled(dssdev))
 		return 0;
 
-	in->ops.dpi->set_data_lines(in, ddata->data_lines);
+	if (ddata->data_lines)
+		in->ops.dpi->set_data_lines(in, ddata->data_lines);
 	in->ops.dpi->set_timings(in, &ddata->videomode);
 
 	r = in->ops.dpi->enable(in);
@@ -489,6 +491,31 @@ static int tpo_td043_probe_pdata(struct spi_device *spi)
 	return 0;
 }
 
+static int tpo_td043_probe_of(struct spi_device *spi)
+{
+	struct device_node *node = spi->dev.of_node;
+	struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+	struct omap_dss_device *in;
+	int gpio;
+
+	gpio = of_get_named_gpio(node, "reset-gpios", 0);
+	if (!gpio_is_valid(gpio)) {
+		dev_err(&spi->dev, "failed to parse enable gpio\n");
+		return gpio;
+	}
+	ddata->nreset_gpio = gpio;
+
+	in = omapdss_of_find_source_for_first_ep(node);
+	if (IS_ERR(in)) {
+		dev_err(&spi->dev, "failed to find video source\n");
+		return PTR_ERR(in);
+	}
+
+	ddata->in = in;
+
+	return 0;
+}
+
 static int tpo_td043_probe(struct spi_device *spi)
 {
 	struct panel_drv_data *ddata;
@@ -518,6 +545,10 @@ static int tpo_td043_probe(struct spi_device *spi)
 		r = tpo_td043_probe_pdata(spi);
 		if (r)
 			return r;
+	} else if (spi->dev.of_node) {
+		r = tpo_td043_probe_of(spi);
+		if (r)
+			return r;
 	} else {
 		return -ENODEV;
 	}
@@ -629,11 +660,19 @@ static int tpo_td043_spi_resume(struct device *dev)
 static SIMPLE_DEV_PM_OPS(tpo_td043_spi_pm,
 	tpo_td043_spi_suspend, tpo_td043_spi_resume);
 
+static const struct of_device_id tpo_td043_of_match[] = {
+	{ .compatible = "omapdss,tpo,td043mtea1", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, tpo_td043_of_match);
+
 static struct spi_driver tpo_td043_spi_driver = {
 	.driver = {
 		.name	= "panel-tpo-td043mtea1",
 		.owner	= THIS_MODULE,
 		.pm	= &tpo_td043_spi_pm,
+		.of_match_table = tpo_td043_of_match,
 	},
 	.probe	= tpo_td043_probe,
 	.remove	= tpo_td043_remove,
@@ -641,6 +680,7 @@ static struct spi_driver tpo_td043_spi_driver = {
 
 module_spi_driver(tpo_td043_spi_driver);
 
+MODULE_ALIAS("spi:tpo,td043mtea1");
 MODULE_AUTHOR("Gražvydas Ignotas <notasas@gmail.com>");
 MODULE_DESCRIPTION("TPO TD043MTEA1 LCD Driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/dss/Kconfig b/drivers/video/fbdev/omap2/dss/Kconfig
index dde4281663b..285bcd103dc 100644
--- a/drivers/video/fbdev/omap2/dss/Kconfig
+++ b/drivers/video/fbdev/omap2/dss/Kconfig
@@ -1,6 +1,10 @@
+config OMAP2_DSS_INIT
+	bool
+
 menuconfig OMAP2_DSS
         tristate "OMAP2+ Display Subsystem support"
 	select VIDEOMODE_HELPERS
+	select OMAP2_DSS_INIT
         help
 	  OMAP2+ Display Subsystem support.
 
@@ -59,16 +63,32 @@ config OMAP2_DSS_VENC
 	help
 	  OMAP Video Encoder support for S-Video and composite TV-out.
 
+config OMAP2_DSS_HDMI_COMMON
+	bool
+
 config OMAP4_DSS_HDMI
-	bool "HDMI support"
+	bool "HDMI support for OMAP4"
         default y
+	select OMAP2_DSS_HDMI_COMMON
 	help
-	  HDMI Interface. This adds the High Definition Multimedia Interface.
-	  See http://www.hdmi.org/ for HDMI specification.
+	  HDMI support for OMAP4 based SoCs.
 
 config OMAP4_DSS_HDMI_AUDIO
 	bool
 
+config OMAP5_DSS_HDMI
+	bool "HDMI support for OMAP5"
+	default n
+	select OMAP2_DSS_HDMI_COMMON
+	help
+	  HDMI Interface for OMAP5 and similar cores. This adds the High
+	  Definition Multimedia Interface. See http://www.hdmi.org/ for HDMI
+	  specification.
+
+config OMAP5_DSS_HDMI_AUDIO
+	depends on OMAP5_DSS_HDMI
+	bool
+
 config OMAP2_DSS_SDI
 	bool "SDI support"
         default n
diff --git a/drivers/video/fbdev/omap2/dss/Makefile b/drivers/video/fbdev/omap2/dss/Makefile
index 8aec8bda27c..245f933060e 100644
--- a/drivers/video/fbdev/omap2/dss/Makefile
+++ b/drivers/video/fbdev/omap2/dss/Makefile
@@ -1,3 +1,4 @@
+obj-$(CONFIG_OMAP2_DSS_INIT) += omapdss-boot-init.o
 obj-$(CONFIG_OMAP2_DSS) += omapdss.o
 # Core DSS files
 omapdss-y := core.o dss.o dss_features.o dispc.o dispc_coefs.o display.o \
@@ -10,6 +11,8 @@ omapdss-$(CONFIG_OMAP2_DSS_RFBI) += rfbi.o
 omapdss-$(CONFIG_OMAP2_DSS_VENC) += venc.o
 omapdss-$(CONFIG_OMAP2_DSS_SDI) += sdi.o
 omapdss-$(CONFIG_OMAP2_DSS_DSI) += dsi.o
-omapdss-$(CONFIG_OMAP4_DSS_HDMI) += hdmi4.o hdmi_common.o hdmi_wp.o hdmi_pll.o \
-	hdmi_phy.o hdmi4_core.o
+omapdss-$(CONFIG_OMAP2_DSS_HDMI_COMMON) += hdmi_common.o hdmi_wp.o hdmi_pll.o \
+	hdmi_phy.o
+omapdss-$(CONFIG_OMAP4_DSS_HDMI) += hdmi4.o hdmi4_core.o
+omapdss-$(CONFIG_OMAP5_DSS_HDMI) += hdmi5.o hdmi5_core.o
 ccflags-$(CONFIG_OMAP2_DSS_DEBUG) += -DDEBUG
diff --git a/drivers/video/fbdev/omap2/dss/core.c b/drivers/video/fbdev/omap2/dss/core.c
index ffa45c894cd..6b74f73fb52 100644
--- a/drivers/video/fbdev/omap2/dss/core.c
+++ b/drivers/video/fbdev/omap2/dss/core.c
@@ -268,6 +268,9 @@ static int (*dss_output_drv_reg_funcs[])(void) __initdata = {
 #ifdef CONFIG_OMAP4_DSS_HDMI
 	hdmi4_init_platform_driver,
 #endif
+#ifdef CONFIG_OMAP5_DSS_HDMI
+	hdmi5_init_platform_driver,
+#endif
 };
 
 static void (*dss_output_drv_unreg_funcs[])(void) __exitdata = {
@@ -289,6 +292,9 @@ static void (*dss_output_drv_unreg_funcs[])(void) __exitdata = {
 #ifdef CONFIG_OMAP4_DSS_HDMI
 	hdmi4_uninit_platform_driver,
 #endif
+#ifdef CONFIG_OMAP5_DSS_HDMI
+	hdmi5_uninit_platform_driver,
+#endif
 };
 
 static bool dss_output_drv_loaded[ARRAY_SIZE(dss_output_drv_reg_funcs)];
diff --git a/drivers/video/fbdev/omap2/dss/dispc.c b/drivers/video/fbdev/omap2/dss/dispc.c
index f18397c33e8..7aa33b0f4a1 100644
--- a/drivers/video/fbdev/omap2/dss/dispc.c
+++ b/drivers/video/fbdev/omap2/dss/dispc.c
@@ -2577,9 +2577,9 @@ int dispc_ovl_setup(enum omap_plane plane, const struct omap_overlay_info *oi,
 
 	channel = dispc_ovl_get_channel_out(plane);
 
-	DSSDBG("dispc_ovl_setup %d, pa %x, pa_uv %x, sw %d, %d,%d, %dx%d -> "
-		"%dx%d, cmode %x, rot %d, mir %d, chan %d repl %d\n",
-		plane, oi->paddr, oi->p_uv_addr, oi->screen_width, oi->pos_x,
+	DSSDBG("dispc_ovl_setup %d, pa %pad, pa_uv %pad, sw %d, %d,%d, %dx%d ->"
+		" %dx%d, cmode %x, rot %d, mir %d, chan %d repl %d\n",
+		plane, &oi->paddr, &oi->p_uv_addr, oi->screen_width, oi->pos_x,
 		oi->pos_y, oi->width, oi->height, oi->out_width, oi->out_height,
 		oi->color_mode, oi->rotation, oi->mirror, channel, replication);
 
@@ -2945,13 +2945,13 @@ static void _dispc_mgr_set_lcd_timings(enum omap_channel channel, int hsw,
 		BUG();
 	}
 
-	l = dispc_read_reg(DISPC_POL_FREQ(channel));
-	l |= FLD_VAL(onoff, 17, 17);
-	l |= FLD_VAL(rf, 16, 16);
-	l |= FLD_VAL(de_level, 15, 15);
-	l |= FLD_VAL(ipc, 14, 14);
-	l |= FLD_VAL(hsync_level, 13, 13);
-	l |= FLD_VAL(vsync_level, 12, 12);
+	l = FLD_VAL(onoff, 17, 17) |
+		FLD_VAL(rf, 16, 16) |
+		FLD_VAL(de_level, 15, 15) |
+		FLD_VAL(ipc, 14, 14) |
+		FLD_VAL(hsync_level, 13, 13) |
+		FLD_VAL(vsync_level, 12, 12);
+
 	dispc_write_reg(DISPC_POL_FREQ(channel), l);
 }
 
@@ -3656,6 +3656,7 @@ static int __init dispc_init_features(struct platform_device *pdev)
 	case OMAPDSS_VER_OMAP34xx_ES3:
 	case OMAPDSS_VER_OMAP3630:
 	case OMAPDSS_VER_AM35xx:
+	case OMAPDSS_VER_AM43xx:
 		src = &omap34xx_rev3_0_dispc_feats;
 		break;
 
@@ -3829,6 +3830,7 @@ static const struct of_device_id dispc_of_match[] = {
 	{ .compatible = "ti,omap2-dispc", },
 	{ .compatible = "ti,omap3-dispc", },
 	{ .compatible = "ti,omap4-dispc", },
+	{ .compatible = "ti,omap5-dispc", },
 	{},
 };
 
diff --git a/drivers/video/fbdev/omap2/dss/dpi.c b/drivers/video/fbdev/omap2/dss/dpi.c
index 157921db447..9368972d696 100644
--- a/drivers/video/fbdev/omap2/dss/dpi.c
+++ b/drivers/video/fbdev/omap2/dss/dpi.c
@@ -67,6 +67,7 @@ static struct platform_device *dpi_get_dsidev(enum omap_channel channel)
 	case OMAPDSS_VER_OMAP34xx_ES3:
 	case OMAPDSS_VER_OMAP3630:
 	case OMAPDSS_VER_AM35xx:
+	case OMAPDSS_VER_AM43xx:
 		return NULL;
 
 	case OMAPDSS_VER_OMAP4430_ES1:
@@ -103,6 +104,8 @@ static enum omap_dss_clk_source dpi_get_alt_clk_src(enum omap_channel channel)
 		return OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC;
 	case OMAP_DSS_CHANNEL_LCD2:
 		return OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC;
 	default:
 		/* this shouldn't happen */
 		WARN_ON(1);
@@ -595,6 +598,7 @@ static enum omap_channel dpi_get_channel(void)
 	case OMAPDSS_VER_OMAP34xx_ES3:
 	case OMAPDSS_VER_OMAP3630:
 	case OMAPDSS_VER_AM35xx:
+	case OMAPDSS_VER_AM43xx:
 		return OMAP_DSS_CHANNEL_LCD;
 
 	case OMAPDSS_VER_OMAP4430_ES1:
diff --git a/drivers/video/fbdev/omap2/dss/dsi.c b/drivers/video/fbdev/omap2/dss/dsi.c
index 8be9b04d884..4755a34a542 100644
--- a/drivers/video/fbdev/omap2/dss/dsi.c
+++ b/drivers/video/fbdev/omap2/dss/dsi.c
@@ -1161,6 +1161,7 @@ static int dsi_regulator_init(struct platform_device *dsidev)
 {
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	struct regulator *vdds_dsi;
+	int r;
 
 	if (dsi->vdds_dsi_reg != NULL)
 		return 0;
@@ -1173,6 +1174,15 @@ static int dsi_regulator_init(struct platform_device *dsidev)
 		return PTR_ERR(vdds_dsi);
 	}
 
+	if (regulator_can_change_voltage(vdds_dsi)) {
+		r = regulator_set_voltage(vdds_dsi, 1800000, 1800000);
+		if (r) {
+			devm_regulator_put(vdds_dsi);
+			DSSERR("can't set the DSI regulator voltage\n");
+			return r;
+		}
+	}
+
 	dsi->vdds_dsi_reg = vdds_dsi;
 
 	return 0;
@@ -5122,6 +5132,7 @@ static enum omap_channel dsi_get_channel(int module_id)
 {
 	switch (omapdss_get_version()) {
 	case OMAPDSS_VER_OMAP24xx:
+	case OMAPDSS_VER_AM43xx:
 		DSSWARN("DSI not supported\n");
 		return OMAP_DSS_CHANNEL_LCD;
 
@@ -5723,9 +5734,16 @@ static const struct dsi_module_id_data dsi_of_data_omap4[] = {
 	{ },
 };
 
+static const struct dsi_module_id_data dsi_of_data_omap5[] = {
+	{ .address = 0x58004000, .id = 0, },
+	{ .address = 0x58009000, .id = 1, },
+	{ },
+};
+
 static const struct of_device_id dsi_of_match[] = {
 	{ .compatible = "ti,omap3-dsi", .data = dsi_of_data_omap3, },
 	{ .compatible = "ti,omap4-dsi", .data = dsi_of_data_omap4, },
+	{ .compatible = "ti,omap5-dsi", .data = dsi_of_data_omap5, },
 	{},
 };
 
diff --git a/drivers/video/fbdev/omap2/dss/dss.c b/drivers/video/fbdev/omap2/dss/dss.c
index d55266c0e02..6daeb7ed44c 100644
--- a/drivers/video/fbdev/omap2/dss/dss.c
+++ b/drivers/video/fbdev/omap2/dss/dss.c
@@ -728,6 +728,13 @@ static const struct dss_features omap54xx_dss_feats __initconst = {
 	.dpi_select_source	=	&dss_dpi_select_source_omap5,
 };
 
+static const struct dss_features am43xx_dss_feats __initconst = {
+	.fck_div_max		=	0,
+	.dss_fck_multiplier	=	0,
+	.parent_clk_name	=	NULL,
+	.dpi_select_source	=	&dss_dpi_select_source_omap2_omap3,
+};
+
 static int __init dss_init_features(struct platform_device *pdev)
 {
 	const struct dss_features *src;
@@ -764,6 +771,10 @@ static int __init dss_init_features(struct platform_device *pdev)
 		src = &omap54xx_dss_feats;
 		break;
 
+	case OMAPDSS_VER_AM43xx:
+		src = &am43xx_dss_feats;
+		break;
+
 	default:
 		return -ENODEV;
 	}
@@ -784,12 +795,8 @@ static int __init dss_init_ports(struct platform_device *pdev)
 		return 0;
 
 	port = omapdss_of_get_next_port(parent, NULL);
-	if (!port) {
-#ifdef CONFIG_OMAP2_DSS_DPI
-		dpi_init_port(pdev, parent);
-#endif
+	if (!port)
 		return 0;
-	}
 
 	do {
 		u32 reg;
@@ -813,7 +820,7 @@ static int __init dss_init_ports(struct platform_device *pdev)
 	return 0;
 }
 
-static void dss_uninit_ports(void)
+static void __exit dss_uninit_ports(void)
 {
 #ifdef CONFIG_OMAP2_DSS_DPI
 	dpi_uninit_port();
@@ -946,6 +953,7 @@ static const struct of_device_id dss_of_match[] = {
 	{ .compatible = "ti,omap2-dss", },
 	{ .compatible = "ti,omap3-dss", },
 	{ .compatible = "ti,omap4-dss", },
+	{ .compatible = "ti,omap5-dss", },
 	{},
 };
 
diff --git a/drivers/video/fbdev/omap2/dss/dss.h b/drivers/video/fbdev/omap2/dss/dss.h
index 560078fcb19..8ff22c134c6 100644
--- a/drivers/video/fbdev/omap2/dss/dss.h
+++ b/drivers/video/fbdev/omap2/dss/dss.h
@@ -419,6 +419,9 @@ void venc_uninit_platform_driver(void) __exit;
 int hdmi4_init_platform_driver(void) __init;
 void hdmi4_uninit_platform_driver(void) __exit;
 
+int hdmi5_init_platform_driver(void) __init;
+void hdmi5_uninit_platform_driver(void) __exit;
+
 /* RFBI */
 int rfbi_init_platform_driver(void) __init;
 void rfbi_uninit_platform_driver(void) __exit;
diff --git a/drivers/video/fbdev/omap2/dss/dss_features.c b/drivers/video/fbdev/omap2/dss/dss_features.c
index 7f8969191dc..15088df7bd1 100644
--- a/drivers/video/fbdev/omap2/dss/dss_features.c
+++ b/drivers/video/fbdev/omap2/dss/dss_features.c
@@ -93,6 +93,17 @@ static const struct dss_reg_field omap3_dss_reg_fields[] = {
 	[FEAT_REG_DSIPLL_REGM_DSI]		= { 26, 23 },
 };
 
+static const struct dss_reg_field am43xx_dss_reg_fields[] = {
+	[FEAT_REG_FIRHINC]			= { 12, 0 },
+	[FEAT_REG_FIRVINC]			= { 28, 16 },
+	[FEAT_REG_FIFOLOWTHRESHOLD]	= { 11, 0 },
+	[FEAT_REG_FIFOHIGHTHRESHOLD]		= { 27, 16 },
+	[FEAT_REG_FIFOSIZE]		= { 10, 0 },
+	[FEAT_REG_HORIZONTALACCU]		= { 9, 0 },
+	[FEAT_REG_VERTICALACCU]			= { 25, 16 },
+	[FEAT_REG_DISPC_CLK_SWITCH]		= { 0, 0 },
+};
+
 static const struct dss_reg_field omap4_dss_reg_fields[] = {
 	[FEAT_REG_FIRHINC]			= { 12, 0 },
 	[FEAT_REG_FIRVINC]			= { 28, 16 },
@@ -149,6 +160,11 @@ static const enum omap_display_type omap3630_dss_supported_displays[] = {
 	OMAP_DISPLAY_TYPE_VENC,
 };
 
+static const enum omap_display_type am43xx_dss_supported_displays[] = {
+	/* OMAP_DSS_CHANNEL_LCD */
+	OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI,
+};
+
 static const enum omap_display_type omap4_dss_supported_displays[] = {
 	/* OMAP_DSS_CHANNEL_LCD */
 	OMAP_DISPLAY_TYPE_DBI | OMAP_DISPLAY_TYPE_DSI,
@@ -200,6 +216,11 @@ static const enum omap_dss_output_id omap3630_dss_supported_outputs[] = {
 	OMAP_DSS_OUTPUT_VENC,
 };
 
+static const enum omap_dss_output_id am43xx_dss_supported_outputs[] = {
+	/* OMAP_DSS_CHANNEL_LCD */
+	OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI,
+};
+
 static const enum omap_dss_output_id omap4_dss_supported_outputs[] = {
 	/* OMAP_DSS_CHANNEL_LCD */
 	OMAP_DSS_OUTPUT_DBI | OMAP_DSS_OUTPUT_DSI1,
@@ -444,6 +465,13 @@ static const struct dss_param_range omap3_dss_param_range[] = {
 	[FEAT_PARAM_LINEWIDTH]			= { 1, 1024 },
 };
 
+static const struct dss_param_range am43xx_dss_param_range[] = {
+	[FEAT_PARAM_DSS_FCK]			= { 0, 200000000 },
+	[FEAT_PARAM_DSS_PCD]			= { 2, 255 },
+	[FEAT_PARAM_DOWNSCALE]			= { 1, 4 },
+	[FEAT_PARAM_LINEWIDTH]			= { 1, 1024 },
+};
+
 static const struct dss_param_range omap4_dss_param_range[] = {
 	[FEAT_PARAM_DSS_FCK]			= { 0, 186000000 },
 	[FEAT_PARAM_DSS_PCD]			= { 1, 255 },
@@ -520,6 +548,21 @@ static const enum dss_feat_id am35xx_dss_feat_list[] = {
 	FEAT_OMAP3_DSI_FIFO_BUG,
 };
 
+static const enum dss_feat_id am43xx_dss_feat_list[] = {
+	FEAT_LCDENABLEPOL,
+	FEAT_LCDENABLESIGNAL,
+	FEAT_PCKFREEENABLE,
+	FEAT_FUNCGATED,
+	FEAT_LINEBUFFERSPLIT,
+	FEAT_ROWREPEATENABLE,
+	FEAT_RESIZECONF,
+	FEAT_CPR,
+	FEAT_PRELOAD,
+	FEAT_FIR_COEF_V,
+	FEAT_ALPHA_FIXED_ZORDER,
+	FEAT_FIFO_MERGE,
+};
+
 static const enum dss_feat_id omap3630_dss_feat_list[] = {
 	FEAT_LCDENABLEPOL,
 	FEAT_LCDENABLESIGNAL,
@@ -595,6 +638,7 @@ static const enum dss_feat_id omap4_dss_feat_list[] = {
 
 static const enum dss_feat_id omap5_dss_feat_list[] = {
 	FEAT_MGR_LCD2,
+	FEAT_MGR_LCD3,
 	FEAT_CORE_CLK_DIV,
 	FEAT_LCD_CLK_SRC,
 	FEAT_DSI_DCS_CMD_CONFIG_VC,
@@ -682,6 +726,26 @@ static const struct omap_dss_features am35xx_dss_features = {
 	.burst_size_unit = 8,
 };
 
+static const struct omap_dss_features am43xx_dss_features = {
+	.reg_fields = am43xx_dss_reg_fields,
+	.num_reg_fields = ARRAY_SIZE(am43xx_dss_reg_fields),
+
+	.features = am43xx_dss_feat_list,
+	.num_features = ARRAY_SIZE(am43xx_dss_feat_list),
+
+	.num_mgrs = 1,
+	.num_ovls = 3,
+	.supported_displays = am43xx_dss_supported_displays,
+	.supported_outputs = am43xx_dss_supported_outputs,
+	.supported_color_modes = omap3_dss_supported_color_modes,
+	.overlay_caps = omap3430_dss_overlay_caps,
+	.clksrc_names = omap2_dss_clk_source_names,
+	.dss_params = am43xx_dss_param_range,
+	.supported_rotation_types = OMAP_DSS_ROT_DMA,
+	.buffer_size_unit = 1,
+	.burst_size_unit = 8,
+};
+
 static const struct omap_dss_features omap3630_dss_features = {
 	.reg_fields = omap3_dss_reg_fields,
 	.num_reg_fields = ARRAY_SIZE(omap3_dss_reg_fields),
@@ -777,7 +841,7 @@ static const struct omap_dss_features omap5_dss_features = {
 	.features = omap5_dss_feat_list,
 	.num_features = ARRAY_SIZE(omap5_dss_feat_list),
 
-	.num_mgrs = 3,
+	.num_mgrs = 4,
 	.num_ovls = 4,
 	.supported_displays = omap5_dss_supported_displays,
 	.supported_outputs = omap5_dss_supported_outputs,
@@ -928,6 +992,10 @@ void dss_features_init(enum omapdss_version version)
 		omap_current_dss_features = &am35xx_dss_features;
 		break;
 
+	case OMAPDSS_VER_AM43xx:
+		omap_current_dss_features = &am43xx_dss_features;
+		break;
+
 	default:
 		DSSWARN("Unsupported OMAP version");
 		break;
diff --git a/drivers/video/fbdev/omap2/dss/hdmi.h b/drivers/video/fbdev/omap2/dss/hdmi.h
index e25681ff5a7..fbee0781633 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi.h
+++ b/drivers/video/fbdev/omap2/dss/hdmi.h
@@ -80,6 +80,7 @@
 #define HDMI_TXPHY_DIGITAL_CTRL			0x4
 #define HDMI_TXPHY_POWER_CTRL			0x8
 #define HDMI_TXPHY_PAD_CFG_CTRL			0xC
+#define HDMI_TXPHY_BIST_CONTROL			0x1C
 
 enum hdmi_pll_pwr {
 	HDMI_PLLPWRCMD_ALLOFF = 0,
@@ -351,7 +352,8 @@ struct hdmi_pll_data {
 struct hdmi_phy_data {
 	void __iomem *base;
 
-	int irq;
+	u8 lane_function[4];
+	u8 lane_polarity[4];
 };
 
 struct hdmi_core_data {
@@ -360,13 +362,13 @@ struct hdmi_core_data {
 	struct hdmi_core_infoframe_avi avi_cfg;
 };
 
-static inline void hdmi_write_reg(void __iomem *base_addr, const u16 idx,
+static inline void hdmi_write_reg(void __iomem *base_addr, const u32 idx,
 		u32 val)
 {
 	__raw_writel(val, base_addr + idx);
 }
 
-static inline u32 hdmi_read_reg(void __iomem *base_addr, const u16 idx)
+static inline u32 hdmi_read_reg(void __iomem *base_addr, const u32 idx)
 {
 	return __raw_readl(base_addr + idx);
 }
@@ -417,18 +419,19 @@ void hdmi_pll_compute(struct hdmi_pll_data *pll, unsigned long clkin, int phy);
 int hdmi_pll_init(struct platform_device *pdev, struct hdmi_pll_data *pll);
 
 /* HDMI PHY funcs */
-int hdmi_phy_enable(struct hdmi_phy_data *phy, struct hdmi_wp_data *wp,
-		struct hdmi_config *cfg);
-void hdmi_phy_disable(struct hdmi_phy_data *phy, struct hdmi_wp_data *wp);
+int hdmi_phy_configure(struct hdmi_phy_data *phy, struct hdmi_config *cfg);
 void hdmi_phy_dump(struct hdmi_phy_data *phy, struct seq_file *s);
 int hdmi_phy_init(struct platform_device *pdev, struct hdmi_phy_data *phy);
+int hdmi_phy_parse_lanes(struct hdmi_phy_data *phy, const u32 *lanes);
 
 /* HDMI common funcs */
 const struct hdmi_config *hdmi_default_timing(void);
 const struct hdmi_config *hdmi_get_timings(int mode, int code);
 struct hdmi_cm hdmi_get_code(struct omap_video_timings *timing);
+int hdmi_parse_lanes_of(struct platform_device *pdev, struct device_node *ep,
+	struct hdmi_phy_data *phy);
 
-#if defined(CONFIG_OMAP4_DSS_HDMI_AUDIO)
+#if defined(CONFIG_OMAP4_DSS_HDMI_AUDIO) || defined(CONFIG_OMAP5_DSS_HDMI_AUDIO)
 int hdmi_compute_acr(u32 pclk, u32 sample_freq, u32 *n, u32 *cts);
 int hdmi_wp_audio_enable(struct hdmi_wp_data *wp, bool enable);
 int hdmi_wp_audio_core_req_enable(struct hdmi_wp_data *wp, bool enable);
diff --git a/drivers/video/fbdev/omap2/dss/hdmi4.c b/drivers/video/fbdev/omap2/dss/hdmi4.c
index f5f7944a1fd..626aad2bef4 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi4.c
+++ b/drivers/video/fbdev/omap2/dss/hdmi4.c
@@ -81,8 +81,40 @@ static void hdmi_runtime_put(void)
 	WARN_ON(r < 0 && r != -ENOSYS);
 }
 
+static irqreturn_t hdmi_irq_handler(int irq, void *data)
+{
+	struct hdmi_wp_data *wp = data;
+	u32 irqstatus;
+
+	irqstatus = hdmi_wp_get_irqstatus(wp);
+	hdmi_wp_set_irqstatus(wp, irqstatus);
+
+	if ((irqstatus & HDMI_IRQ_LINK_CONNECT) &&
+			irqstatus & HDMI_IRQ_LINK_DISCONNECT) {
+		/*
+		 * If we get both connect and disconnect interrupts at the same
+		 * time, turn off the PHY, clear interrupts, and restart, which
+		 * raises connect interrupt if a cable is connected, or nothing
+		 * if cable is not connected.
+		 */
+		hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_OFF);
+
+		hdmi_wp_set_irqstatus(wp, HDMI_IRQ_LINK_CONNECT |
+				HDMI_IRQ_LINK_DISCONNECT);
+
+		hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_LDOON);
+	} else if (irqstatus & HDMI_IRQ_LINK_CONNECT) {
+		hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_TXON);
+	} else if (irqstatus & HDMI_IRQ_LINK_DISCONNECT) {
+		hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_LDOON);
+	}
+
+	return IRQ_HANDLED;
+}
+
 static int hdmi_init_regulator(void)
 {
+	int r;
 	struct regulator *reg;
 
 	if (hdmi.vdda_hdmi_dac_reg != NULL)
@@ -96,6 +128,15 @@ static int hdmi_init_regulator(void)
 		return PTR_ERR(reg);
 	}
 
+	if (regulator_can_change_voltage(reg)) {
+		r = regulator_set_voltage(reg, 1800000, 1800000);
+		if (r) {
+			devm_regulator_put(reg);
+			DSSWARN("can't set the regulator voltage\n");
+			return r;
+		}
+	}
+
 	hdmi.vdda_hdmi_dac_reg = reg;
 
 	return 0;
@@ -140,11 +181,16 @@ static int hdmi_power_on_full(struct omap_dss_device *dssdev)
 	struct omap_video_timings *p;
 	struct omap_overlay_manager *mgr = hdmi.output.manager;
 	unsigned long phy;
+	struct hdmi_wp_data *wp = &hdmi.wp;
 
 	r = hdmi_power_on_core(dssdev);
 	if (r)
 		return r;
 
+	/* disable and clear irqs */
+	hdmi_wp_clear_irqenable(wp, 0xffffffff);
+	hdmi_wp_set_irqstatus(wp, 0xffffffff);
+
 	p = &hdmi.cfg.timings;
 
 	DSSDBG("hdmi_power_on x_res= %d y_res = %d\n", p->x_res, p->y_res);
@@ -161,12 +207,16 @@ static int hdmi_power_on_full(struct omap_dss_device *dssdev)
 		goto err_pll_enable;
 	}
 
-	r = hdmi_phy_enable(&hdmi.phy, &hdmi.wp, &hdmi.cfg);
+	r = hdmi_phy_configure(&hdmi.phy, &hdmi.cfg);
 	if (r) {
-		DSSDBG("Failed to start PHY\n");
-		goto err_phy_enable;
+		DSSDBG("Failed to configure PHY\n");
+		goto err_phy_cfg;
 	}
 
+	r = hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_LDOON);
+	if (r)
+		goto err_phy_pwr;
+
 	hdmi4_configure(&hdmi.core, &hdmi.wp, &hdmi.cfg);
 
 	/* bypass TV gamma table */
@@ -183,13 +233,17 @@ static int hdmi_power_on_full(struct omap_dss_device *dssdev)
 	if (r)
 		goto err_mgr_enable;
 
+	hdmi_wp_set_irqenable(wp,
+		HDMI_IRQ_LINK_CONNECT | HDMI_IRQ_LINK_DISCONNECT);
+
 	return 0;
 
 err_mgr_enable:
 	hdmi_wp_video_stop(&hdmi.wp);
 err_vid_enable:
-	hdmi_phy_disable(&hdmi.phy, &hdmi.wp);
-err_phy_enable:
+err_phy_cfg:
+	hdmi_wp_set_phy_pwr(&hdmi.wp, HDMI_PHYPWRCMD_OFF);
+err_phy_pwr:
 	hdmi_pll_disable(&hdmi.pll, &hdmi.wp);
 err_pll_enable:
 	hdmi_power_off_core(dssdev);
@@ -200,10 +254,14 @@ static void hdmi_power_off_full(struct omap_dss_device *dssdev)
 {
 	struct omap_overlay_manager *mgr = hdmi.output.manager;
 
+	hdmi_wp_clear_irqenable(&hdmi.wp, 0xffffffff);
+
 	dss_mgr_disable(mgr);
 
 	hdmi_wp_video_stop(&hdmi.wp);
-	hdmi_phy_disable(&hdmi.phy, &hdmi.wp);
+
+	hdmi_wp_set_phy_pwr(&hdmi.wp, HDMI_PHYPWRCMD_OFF);
+
 	hdmi_pll_disable(&hdmi.pll, &hdmi.wp);
 
 	hdmi_power_off_core(dssdev);
@@ -600,15 +658,44 @@ static void __exit hdmi_uninit_output(struct platform_device *pdev)
 	omapdss_unregister_output(out);
 }
 
+static int hdmi_probe_of(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct device_node *ep;
+	int r;
+
+	ep = omapdss_of_get_first_endpoint(node);
+	if (!ep)
+		return 0;
+
+	r = hdmi_parse_lanes_of(pdev, ep, &hdmi.phy);
+	if (r)
+		goto err;
+
+	of_node_put(ep);
+	return 0;
+
+err:
+	of_node_put(ep);
+	return r;
+}
+
 /* HDMI HW IP initialisation */
 static int omapdss_hdmihw_probe(struct platform_device *pdev)
 {
 	int r;
+	int irq;
 
 	hdmi.pdev = pdev;
 
 	mutex_init(&hdmi.lock);
 
+	if (pdev->dev.of_node) {
+		r = hdmi_probe_of(pdev);
+		if (r)
+			return r;
+	}
+
 	r = hdmi_wp_init(pdev, &hdmi.wp);
 	if (r)
 		return r;
@@ -631,6 +718,20 @@ static int omapdss_hdmihw_probe(struct platform_device *pdev)
 		return r;
 	}
 
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		DSSERR("platform_get_irq failed\n");
+		return -ENODEV;
+	}
+
+	r = devm_request_threaded_irq(&pdev->dev, irq,
+			NULL, hdmi_irq_handler,
+			IRQF_ONESHOT, "OMAP HDMI", &hdmi.wp);
+	if (r) {
+		DSSERR("HDMI IRQ request failed\n");
+		return r;
+	}
+
 	pm_runtime_enable(&pdev->dev);
 
 	hdmi_init_output(pdev);
diff --git a/drivers/video/fbdev/omap2/dss/hdmi4_core.c b/drivers/video/fbdev/omap2/dss/hdmi4_core.c
index 2eb04dcf807..8bde7b7e95f 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi4_core.c
+++ b/drivers/video/fbdev/omap2/dss/hdmi4_core.c
@@ -998,38 +998,20 @@ int hdmi4_audio_get_dma_port(u32 *offset, u32 *size)
 
 #endif
 
-#define CORE_OFFSET		0x400
-#define CORE_SIZE		0xc00
-
 int hdmi4_core_init(struct platform_device *pdev, struct hdmi_core_data *core)
 {
 	struct resource *res;
-	struct resource temp_res;
 
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "core");
 	if (!res) {
-		DSSDBG("can't get CORE mem resource by name\n");
-		/*
-		 * if hwmod/DT doesn't have the memory resource information
-		 * split into HDMI sub blocks by name, we try again by getting
-		 * the platform's first resource. this code will be removed when
-		 * the driver can get the mem resources by name
-		 */
-		res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-		if (!res) {
-			DSSERR("can't get CORE mem resource\n");
-			return -EINVAL;
-		}
-
-		temp_res.start = res->start + CORE_OFFSET;
-		temp_res.end = temp_res.start + CORE_SIZE - 1;
-		res = &temp_res;
+		DSSERR("can't get CORE mem resource\n");
+		return -EINVAL;
 	}
 
-	core->base = devm_ioremap(&pdev->dev, res->start, resource_size(res));
-	if (!core->base) {
+	core->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(core->base)) {
 		DSSERR("can't ioremap CORE\n");
-		return -ENOMEM;
+		return PTR_ERR(core->base);
 	}
 
 	return 0;
diff --git a/drivers/video/fbdev/omap2/dss/hdmi5.c b/drivers/video/fbdev/omap2/dss/hdmi5.c
new file mode 100644
index 00000000000..c468b9e1f29
--- /dev/null
+++ b/drivers/video/fbdev/omap2/dss/hdmi5.c
@@ -0,0 +1,829 @@
+/*
+ * HDMI driver for OMAP5
+ *
+ * Copyright (C) 2014 Texas Instruments Incorporated
+ *
+ * Authors:
+ *	Yong Zhi
+ *	Mythri pk
+ *	Archit Taneja <archit@ti.com>
+ *	Tomi Valkeinen <tomi.valkeinen@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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define DSS_SUBSYS_NAME "HDMI"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/string.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/clk.h>
+#include <linux/gpio.h>
+#include <linux/regulator/consumer.h>
+#include <video/omapdss.h>
+
+#include "hdmi5_core.h"
+#include "dss.h"
+#include "dss_features.h"
+
+static struct {
+	struct mutex lock;
+	struct platform_device *pdev;
+
+	struct hdmi_wp_data	wp;
+	struct hdmi_pll_data	pll;
+	struct hdmi_phy_data	phy;
+	struct hdmi_core_data	core;
+
+	struct hdmi_config cfg;
+
+	struct clk *sys_clk;
+	struct regulator *vdda_reg;
+
+	bool core_enabled;
+
+	struct omap_dss_device output;
+} hdmi;
+
+static int hdmi_runtime_get(void)
+{
+	int r;
+
+	DSSDBG("hdmi_runtime_get\n");
+
+	r = pm_runtime_get_sync(&hdmi.pdev->dev);
+	WARN_ON(r < 0);
+	if (r < 0)
+		return r;
+
+	return 0;
+}
+
+static void hdmi_runtime_put(void)
+{
+	int r;
+
+	DSSDBG("hdmi_runtime_put\n");
+
+	r = pm_runtime_put_sync(&hdmi.pdev->dev);
+	WARN_ON(r < 0 && r != -ENOSYS);
+}
+
+static irqreturn_t hdmi_irq_handler(int irq, void *data)
+{
+	struct hdmi_wp_data *wp = data;
+	u32 irqstatus;
+
+	irqstatus = hdmi_wp_get_irqstatus(wp);
+	hdmi_wp_set_irqstatus(wp, irqstatus);
+
+	if ((irqstatus & HDMI_IRQ_LINK_CONNECT) &&
+			irqstatus & HDMI_IRQ_LINK_DISCONNECT) {
+		u32 v;
+		/*
+		 * If we get both connect and disconnect interrupts at the same
+		 * time, turn off the PHY, clear interrupts, and restart, which
+		 * raises connect interrupt if a cable is connected, or nothing
+		 * if cable is not connected.
+		 */
+
+		hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_OFF);
+
+		/*
+		 * We always get bogus CONNECT & DISCONNECT interrupts when
+		 * setting the PHY to LDOON. To ignore those, we force the RXDET
+		 * line to 0 until the PHY power state has been changed.
+		 */
+		v = hdmi_read_reg(hdmi.phy.base, HDMI_TXPHY_PAD_CFG_CTRL);
+		v = FLD_MOD(v, 1, 15, 15); /* FORCE_RXDET_HIGH */
+		v = FLD_MOD(v, 0, 14, 7); /* RXDET_LINE */
+		hdmi_write_reg(hdmi.phy.base, HDMI_TXPHY_PAD_CFG_CTRL, v);
+
+		hdmi_wp_set_irqstatus(wp, HDMI_IRQ_LINK_CONNECT |
+				HDMI_IRQ_LINK_DISCONNECT);
+
+		hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_LDOON);
+
+		REG_FLD_MOD(hdmi.phy.base, HDMI_TXPHY_PAD_CFG_CTRL, 0, 15, 15);
+
+	} else if (irqstatus & HDMI_IRQ_LINK_CONNECT) {
+		hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_TXON);
+	} else if (irqstatus & HDMI_IRQ_LINK_DISCONNECT) {
+		hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_LDOON);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int hdmi_init_regulator(void)
+{
+	int r;
+	struct regulator *reg;
+
+	if (hdmi.vdda_reg != NULL)
+		return 0;
+
+	reg = devm_regulator_get(&hdmi.pdev->dev, "vdda");
+	if (IS_ERR(reg)) {
+		DSSERR("can't get VDDA regulator\n");
+		return PTR_ERR(reg);
+	}
+
+	if (regulator_can_change_voltage(reg)) {
+		r = regulator_set_voltage(reg, 1800000, 1800000);
+		if (r) {
+			devm_regulator_put(reg);
+			DSSWARN("can't set the regulator voltage\n");
+			return r;
+		}
+	}
+
+	hdmi.vdda_reg = reg;
+
+	return 0;
+}
+
+static int hdmi_power_on_core(struct omap_dss_device *dssdev)
+{
+	int r;
+
+	r = regulator_enable(hdmi.vdda_reg);
+	if (r)
+		return r;
+
+	r = hdmi_runtime_get();
+	if (r)
+		goto err_runtime_get;
+
+	/* Make selection of HDMI in DSS */
+	dss_select_hdmi_venc_clk_source(DSS_HDMI_M_PCLK);
+
+	hdmi.core_enabled = true;
+
+	return 0;
+
+err_runtime_get:
+	regulator_disable(hdmi.vdda_reg);
+
+	return r;
+}
+
+static void hdmi_power_off_core(struct omap_dss_device *dssdev)
+{
+	hdmi.core_enabled = false;
+
+	hdmi_runtime_put();
+	regulator_disable(hdmi.vdda_reg);
+}
+
+static int hdmi_power_on_full(struct omap_dss_device *dssdev)
+{
+	int r;
+	struct omap_video_timings *p;
+	struct omap_overlay_manager *mgr = hdmi.output.manager;
+	unsigned long phy;
+
+	r = hdmi_power_on_core(dssdev);
+	if (r)
+		return r;
+
+	p = &hdmi.cfg.timings;
+
+	DSSDBG("hdmi_power_on x_res= %d y_res = %d\n", p->x_res, p->y_res);
+
+	/* the functions below use kHz pixel clock. TODO: change to Hz */
+	phy = p->pixelclock / 1000;
+
+	hdmi_pll_compute(&hdmi.pll, clk_get_rate(hdmi.sys_clk), phy);
+
+	/* disable and clear irqs */
+	hdmi_wp_clear_irqenable(&hdmi.wp, 0xffffffff);
+	hdmi_wp_set_irqstatus(&hdmi.wp,
+			hdmi_wp_get_irqstatus(&hdmi.wp));
+
+	/* config the PLL and PHY hdmi_set_pll_pwrfirst */
+	r = hdmi_pll_enable(&hdmi.pll, &hdmi.wp);
+	if (r) {
+		DSSDBG("Failed to lock PLL\n");
+		goto err_pll_enable;
+	}
+
+	r = hdmi_phy_configure(&hdmi.phy, &hdmi.cfg);
+	if (r) {
+		DSSDBG("Failed to start PHY\n");
+		goto err_phy_cfg;
+	}
+
+	r = hdmi_wp_set_phy_pwr(&hdmi.wp, HDMI_PHYPWRCMD_LDOON);
+	if (r)
+		goto err_phy_pwr;
+
+	hdmi5_configure(&hdmi.core, &hdmi.wp, &hdmi.cfg);
+
+	/* bypass TV gamma table */
+	dispc_enable_gamma_table(0);
+
+	/* tv size */
+	dss_mgr_set_timings(mgr, p);
+
+	r = hdmi_wp_video_start(&hdmi.wp);
+	if (r)
+		goto err_vid_enable;
+
+	r = dss_mgr_enable(mgr);
+	if (r)
+		goto err_mgr_enable;
+
+	hdmi_wp_set_irqenable(&hdmi.wp,
+			HDMI_IRQ_LINK_CONNECT | HDMI_IRQ_LINK_DISCONNECT);
+
+	return 0;
+
+err_mgr_enable:
+	hdmi_wp_video_stop(&hdmi.wp);
+err_vid_enable:
+	hdmi_wp_set_phy_pwr(&hdmi.wp, HDMI_PHYPWRCMD_OFF);
+err_phy_pwr:
+err_phy_cfg:
+	hdmi_pll_disable(&hdmi.pll, &hdmi.wp);
+err_pll_enable:
+	hdmi_power_off_core(dssdev);
+	return -EIO;
+}
+
+static void hdmi_power_off_full(struct omap_dss_device *dssdev)
+{
+	struct omap_overlay_manager *mgr = hdmi.output.manager;
+
+	hdmi_wp_clear_irqenable(&hdmi.wp, 0xffffffff);
+
+	dss_mgr_disable(mgr);
+
+	hdmi_wp_video_stop(&hdmi.wp);
+
+	hdmi_wp_set_phy_pwr(&hdmi.wp, HDMI_PHYPWRCMD_OFF);
+
+	hdmi_pll_disable(&hdmi.pll, &hdmi.wp);
+
+	hdmi_power_off_core(dssdev);
+}
+
+static int hdmi_display_check_timing(struct omap_dss_device *dssdev,
+					struct omap_video_timings *timings)
+{
+	struct omap_dss_device *out = &hdmi.output;
+
+	if (!dispc_mgr_timings_ok(out->dispc_channel, timings))
+		return -EINVAL;
+
+	return 0;
+}
+
+static void hdmi_display_set_timing(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct hdmi_cm cm;
+	const struct hdmi_config *t;
+
+	mutex_lock(&hdmi.lock);
+
+	cm = hdmi_get_code(timings);
+	hdmi.cfg.cm = cm;
+
+	t = hdmi_get_timings(cm.mode, cm.code);
+	if (t != NULL) {
+		hdmi.cfg = *t;
+
+		dispc_set_tv_pclk(t->timings.pixelclock);
+	} else {
+		hdmi.cfg.timings = *timings;
+		hdmi.cfg.cm.code = 0;
+		hdmi.cfg.cm.mode = HDMI_DVI;
+
+		dispc_set_tv_pclk(timings->pixelclock);
+	}
+
+	DSSDBG("using mode: %s, code %d\n", hdmi.cfg.cm.mode == HDMI_DVI ?
+			"DVI" : "HDMI", hdmi.cfg.cm.code);
+
+	mutex_unlock(&hdmi.lock);
+}
+
+static void hdmi_display_get_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	const struct hdmi_config *cfg;
+	struct hdmi_cm cm = hdmi.cfg.cm;
+
+	cfg = hdmi_get_timings(cm.mode, cm.code);
+	if (cfg == NULL)
+		cfg = hdmi_default_timing();
+
+	memcpy(timings, &cfg->timings, sizeof(cfg->timings));
+}
+
+static void hdmi_dump_regs(struct seq_file *s)
+{
+	mutex_lock(&hdmi.lock);
+
+	if (hdmi_runtime_get()) {
+		mutex_unlock(&hdmi.lock);
+		return;
+	}
+
+	hdmi_wp_dump(&hdmi.wp, s);
+	hdmi_pll_dump(&hdmi.pll, s);
+	hdmi_phy_dump(&hdmi.phy, s);
+	hdmi5_core_dump(&hdmi.core, s);
+
+	hdmi_runtime_put();
+	mutex_unlock(&hdmi.lock);
+}
+
+static int read_edid(u8 *buf, int len)
+{
+	int r;
+	int idlemode;
+
+	mutex_lock(&hdmi.lock);
+
+	r = hdmi_runtime_get();
+	BUG_ON(r);
+
+	idlemode = REG_GET(hdmi.wp.base, HDMI_WP_SYSCONFIG, 3, 2);
+	/* No-idle mode */
+	REG_FLD_MOD(hdmi.wp.base, HDMI_WP_SYSCONFIG, 1, 3, 2);
+
+	r = hdmi5_read_edid(&hdmi.core,  buf, len);
+
+	REG_FLD_MOD(hdmi.wp.base, HDMI_WP_SYSCONFIG, idlemode, 3, 2);
+
+	hdmi_runtime_put();
+	mutex_unlock(&hdmi.lock);
+
+	return r;
+}
+
+static int hdmi_display_enable(struct omap_dss_device *dssdev)
+{
+	struct omap_dss_device *out = &hdmi.output;
+	int r = 0;
+
+	DSSDBG("ENTER hdmi_display_enable\n");
+
+	mutex_lock(&hdmi.lock);
+
+	if (out == NULL || out->manager == NULL) {
+		DSSERR("failed to enable display: no output/manager\n");
+		r = -ENODEV;
+		goto err0;
+	}
+
+	r = hdmi_power_on_full(dssdev);
+	if (r) {
+		DSSERR("failed to power on device\n");
+		goto err0;
+	}
+
+	mutex_unlock(&hdmi.lock);
+	return 0;
+
+err0:
+	mutex_unlock(&hdmi.lock);
+	return r;
+}
+
+static void hdmi_display_disable(struct omap_dss_device *dssdev)
+{
+	DSSDBG("Enter hdmi_display_disable\n");
+
+	mutex_lock(&hdmi.lock);
+
+	hdmi_power_off_full(dssdev);
+
+	mutex_unlock(&hdmi.lock);
+}
+
+static int hdmi_core_enable(struct omap_dss_device *dssdev)
+{
+	int r = 0;
+
+	DSSDBG("ENTER omapdss_hdmi_core_enable\n");
+
+	mutex_lock(&hdmi.lock);
+
+	r = hdmi_power_on_core(dssdev);
+	if (r) {
+		DSSERR("failed to power on device\n");
+		goto err0;
+	}
+
+	mutex_unlock(&hdmi.lock);
+	return 0;
+
+err0:
+	mutex_unlock(&hdmi.lock);
+	return r;
+}
+
+static void hdmi_core_disable(struct omap_dss_device *dssdev)
+{
+	DSSDBG("Enter omapdss_hdmi_core_disable\n");
+
+	mutex_lock(&hdmi.lock);
+
+	hdmi_power_off_core(dssdev);
+
+	mutex_unlock(&hdmi.lock);
+}
+
+static int hdmi_get_clocks(struct platform_device *pdev)
+{
+	struct clk *clk;
+
+	clk = devm_clk_get(&pdev->dev, "sys_clk");
+	if (IS_ERR(clk)) {
+		DSSERR("can't get sys_clk\n");
+		return PTR_ERR(clk);
+	}
+
+	hdmi.sys_clk = clk;
+
+	return 0;
+}
+
+static int hdmi_connect(struct omap_dss_device *dssdev,
+		struct omap_dss_device *dst)
+{
+	struct omap_overlay_manager *mgr;
+	int r;
+
+	r = hdmi_init_regulator();
+	if (r)
+		return r;
+
+	mgr = omap_dss_get_overlay_manager(dssdev->dispc_channel);
+	if (!mgr)
+		return -ENODEV;
+
+	r = dss_mgr_connect(mgr, dssdev);
+	if (r)
+		return r;
+
+	r = omapdss_output_set_device(dssdev, dst);
+	if (r) {
+		DSSERR("failed to connect output to new device: %s\n",
+				dst->name);
+		dss_mgr_disconnect(mgr, dssdev);
+		return r;
+	}
+
+	return 0;
+}
+
+static void hdmi_disconnect(struct omap_dss_device *dssdev,
+		struct omap_dss_device *dst)
+{
+	WARN_ON(dst != dssdev->dst);
+
+	if (dst != dssdev->dst)
+		return;
+
+	omapdss_output_unset_device(dssdev);
+
+	if (dssdev->manager)
+		dss_mgr_disconnect(dssdev->manager, dssdev);
+}
+
+static int hdmi_read_edid(struct omap_dss_device *dssdev,
+		u8 *edid, int len)
+{
+	bool need_enable;
+	int r;
+
+	need_enable = hdmi.core_enabled == false;
+
+	if (need_enable) {
+		r = hdmi_core_enable(dssdev);
+		if (r)
+			return r;
+	}
+
+	r = read_edid(edid, len);
+
+	if (need_enable)
+		hdmi_core_disable(dssdev);
+
+	return r;
+}
+
+#if defined(CONFIG_OMAP5_DSS_HDMI_AUDIO)
+static int hdmi_audio_enable(struct omap_dss_device *dssdev)
+{
+	int r;
+
+	mutex_lock(&hdmi.lock);
+
+	if (!hdmi_mode_has_audio(hdmi.cfg.cm.mode)) {
+		r = -EPERM;
+		goto err;
+	}
+
+	r = hdmi_wp_audio_enable(&hdmi.wp, true);
+	if (r)
+		goto err;
+
+	mutex_unlock(&hdmi.lock);
+	return 0;
+
+err:
+	mutex_unlock(&hdmi.lock);
+	return r;
+}
+
+static void hdmi_audio_disable(struct omap_dss_device *dssdev)
+{
+	hdmi_wp_audio_enable(&hdmi.wp, false);
+}
+
+static int hdmi_audio_start(struct omap_dss_device *dssdev)
+{
+	return hdmi_wp_audio_core_req_enable(&hdmi.wp, true);
+}
+
+static void hdmi_audio_stop(struct omap_dss_device *dssdev)
+{
+	hdmi_wp_audio_core_req_enable(&hdmi.wp, false);
+}
+
+static bool hdmi_audio_supported(struct omap_dss_device *dssdev)
+{
+	bool r;
+
+	mutex_lock(&hdmi.lock);
+
+	r = hdmi_mode_has_audio(hdmi.cfg.cm.mode);
+
+	mutex_unlock(&hdmi.lock);
+	return r;
+}
+
+static int hdmi_audio_config(struct omap_dss_device *dssdev,
+		struct omap_dss_audio *audio)
+{
+	int r;
+	u32 pclk = hdmi.cfg.timings.pixelclock;
+
+	mutex_lock(&hdmi.lock);
+
+	if (!hdmi_mode_has_audio(hdmi.cfg.cm.mode)) {
+		r = -EPERM;
+		goto err;
+	}
+
+	r = hdmi5_audio_config(&hdmi.core, &hdmi.wp, audio, pclk);
+	if (r)
+		goto err;
+
+	mutex_unlock(&hdmi.lock);
+	return 0;
+
+err:
+	mutex_unlock(&hdmi.lock);
+	return r;
+}
+#else
+static int hdmi_audio_enable(struct omap_dss_device *dssdev)
+{
+	return -EPERM;
+}
+
+static void hdmi_audio_disable(struct omap_dss_device *dssdev)
+{
+}
+
+static int hdmi_audio_start(struct omap_dss_device *dssdev)
+{
+	return -EPERM;
+}
+
+static void hdmi_audio_stop(struct omap_dss_device *dssdev)
+{
+}
+
+static bool hdmi_audio_supported(struct omap_dss_device *dssdev)
+{
+	return false;
+}
+
+static int hdmi_audio_config(struct omap_dss_device *dssdev,
+		struct omap_dss_audio *audio)
+{
+	return -EPERM;
+}
+#endif
+
+static const struct omapdss_hdmi_ops hdmi_ops = {
+	.connect		= hdmi_connect,
+	.disconnect		= hdmi_disconnect,
+
+	.enable			= hdmi_display_enable,
+	.disable		= hdmi_display_disable,
+
+	.check_timings		= hdmi_display_check_timing,
+	.set_timings		= hdmi_display_set_timing,
+	.get_timings		= hdmi_display_get_timings,
+
+	.read_edid		= hdmi_read_edid,
+
+	.audio_enable		= hdmi_audio_enable,
+	.audio_disable		= hdmi_audio_disable,
+	.audio_start		= hdmi_audio_start,
+	.audio_stop		= hdmi_audio_stop,
+	.audio_supported	= hdmi_audio_supported,
+	.audio_config		= hdmi_audio_config,
+};
+
+static void hdmi_init_output(struct platform_device *pdev)
+{
+	struct omap_dss_device *out = &hdmi.output;
+
+	out->dev = &pdev->dev;
+	out->id = OMAP_DSS_OUTPUT_HDMI;
+	out->output_type = OMAP_DISPLAY_TYPE_HDMI;
+	out->name = "hdmi.0";
+	out->dispc_channel = OMAP_DSS_CHANNEL_DIGIT;
+	out->ops.hdmi = &hdmi_ops;
+	out->owner = THIS_MODULE;
+
+	omapdss_register_output(out);
+}
+
+static void __exit hdmi_uninit_output(struct platform_device *pdev)
+{
+	struct omap_dss_device *out = &hdmi.output;
+
+	omapdss_unregister_output(out);
+}
+
+static int hdmi_probe_of(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct device_node *ep;
+	int r;
+
+	ep = omapdss_of_get_first_endpoint(node);
+	if (!ep)
+		return 0;
+
+	r = hdmi_parse_lanes_of(pdev, ep, &hdmi.phy);
+	if (r)
+		goto err;
+
+	of_node_put(ep);
+	return 0;
+
+err:
+	of_node_put(ep);
+	return r;
+}
+
+/* HDMI HW IP initialisation */
+static int omapdss_hdmihw_probe(struct platform_device *pdev)
+{
+	int r;
+	int irq;
+
+	hdmi.pdev = pdev;
+
+	mutex_init(&hdmi.lock);
+
+	if (pdev->dev.of_node) {
+		r = hdmi_probe_of(pdev);
+		if (r)
+			return r;
+	}
+
+	r = hdmi_wp_init(pdev, &hdmi.wp);
+	if (r)
+		return r;
+
+	r = hdmi_pll_init(pdev, &hdmi.pll);
+	if (r)
+		return r;
+
+	r = hdmi_phy_init(pdev, &hdmi.phy);
+	if (r)
+		return r;
+
+	r = hdmi5_core_init(pdev, &hdmi.core);
+	if (r)
+		return r;
+
+	r = hdmi_get_clocks(pdev);
+	if (r) {
+		DSSERR("can't get clocks\n");
+		return r;
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		DSSERR("platform_get_irq failed\n");
+		return -ENODEV;
+	}
+
+	r = devm_request_threaded_irq(&pdev->dev, irq,
+			NULL, hdmi_irq_handler,
+			IRQF_ONESHOT, "OMAP HDMI", &hdmi.wp);
+	if (r) {
+		DSSERR("HDMI IRQ request failed\n");
+		return r;
+	}
+
+	pm_runtime_enable(&pdev->dev);
+
+	hdmi_init_output(pdev);
+
+	dss_debugfs_create_file("hdmi", hdmi_dump_regs);
+
+	return 0;
+}
+
+static int __exit omapdss_hdmihw_remove(struct platform_device *pdev)
+{
+	hdmi_uninit_output(pdev);
+
+	pm_runtime_disable(&pdev->dev);
+
+	return 0;
+}
+
+static int hdmi_runtime_suspend(struct device *dev)
+{
+	clk_disable_unprepare(hdmi.sys_clk);
+
+	dispc_runtime_put();
+
+	return 0;
+}
+
+static int hdmi_runtime_resume(struct device *dev)
+{
+	int r;
+
+	r = dispc_runtime_get();
+	if (r < 0)
+		return r;
+
+	clk_prepare_enable(hdmi.sys_clk);
+
+	return 0;
+}
+
+static const struct dev_pm_ops hdmi_pm_ops = {
+	.runtime_suspend = hdmi_runtime_suspend,
+	.runtime_resume = hdmi_runtime_resume,
+};
+
+static const struct of_device_id hdmi_of_match[] = {
+	{ .compatible = "ti,omap5-hdmi", },
+	{},
+};
+
+static struct platform_driver omapdss_hdmihw_driver = {
+	.probe		= omapdss_hdmihw_probe,
+	.remove         = __exit_p(omapdss_hdmihw_remove),
+	.driver         = {
+		.name   = "omapdss_hdmi5",
+		.owner  = THIS_MODULE,
+		.pm	= &hdmi_pm_ops,
+		.of_match_table = hdmi_of_match,
+	},
+};
+
+int __init hdmi5_init_platform_driver(void)
+{
+	return platform_driver_register(&omapdss_hdmihw_driver);
+}
+
+void __exit hdmi5_uninit_platform_driver(void)
+{
+	platform_driver_unregister(&omapdss_hdmihw_driver);
+}
diff --git a/drivers/video/fbdev/omap2/dss/hdmi5_core.c b/drivers/video/fbdev/omap2/dss/hdmi5_core.c
new file mode 100644
index 00000000000..7528c7a42aa
--- /dev/null
+++ b/drivers/video/fbdev/omap2/dss/hdmi5_core.c
@@ -0,0 +1,922 @@
+/*
+ * OMAP5 HDMI CORE IP driver library
+ *
+ * Copyright (C) 2014 Texas Instruments Incorporated
+ *
+ * Authors:
+ *	Yong Zhi
+ *	Mythri pk
+ *	Archit Taneja <archit@ti.com>
+ *	Tomi Valkeinen <tomi.valkeinen@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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/string.h>
+#include <linux/seq_file.h>
+#include <drm/drm_edid.h>
+#if defined(CONFIG_OMAP5_DSS_HDMI_AUDIO)
+#include <sound/asound.h>
+#include <sound/asoundef.h>
+#endif
+
+#include "hdmi5_core.h"
+
+/* only 24 bit color depth used for now */
+static const struct csc_table csc_table_deepcolor[] = {
+	/* HDMI_DEEP_COLOR_24BIT */
+	[0] = { 7036, 0, 0, 32, 0, 7036, 0, 32, 0, 0, 7036, 32, },
+	/* HDMI_DEEP_COLOR_30BIT */
+	[1] = { 7015, 0, 0, 128, 0, 7015, 0, 128, 0, 0, 7015, 128, },
+	/* HDMI_DEEP_COLOR_36BIT */
+	[2] = { 7010, 0, 0, 512, 0, 7010, 0, 512, 0, 0, 7010, 512, },
+	/* FULL RANGE */
+	[3] = { 8192, 0, 0, 0, 0, 8192, 0, 0, 0, 0, 8192, 0, },
+};
+
+static void hdmi_core_ddc_init(struct hdmi_core_data *core)
+{
+	void __iomem *base = core->base;
+	const unsigned long long iclk = 266000000;	/* DSS L3 ICLK */
+	const unsigned ss_scl_high = 4000;		/* ns */
+	const unsigned ss_scl_low = 4700;		/* ns */
+	const unsigned fs_scl_high = 600;		/* ns */
+	const unsigned fs_scl_low = 1300;		/* ns */
+	const unsigned sda_hold = 300;			/* ns */
+	const unsigned sfr_div = 10;
+	unsigned long long sfr;
+	unsigned v;
+
+	sfr = iclk / sfr_div;	/* SFR_DIV */
+	sfr /= 1000;		/* SFR clock in kHz */
+
+	/* Reset */
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_SOFTRSTZ, 0, 0, 0);
+	if (hdmi_wait_for_bit_change(base, HDMI_CORE_I2CM_SOFTRSTZ,
+				0, 0, 1) != 1)
+		DSSERR("HDMI I2CM reset failed\n");
+
+	/* Standard (0) or Fast (1) Mode */
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_DIV, 0, 3, 3);
+
+	/* Standard Mode SCL High counter */
+	v = DIV_ROUND_UP_ULL(ss_scl_high * sfr, 1000000);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_SS_SCL_HCNT_1_ADDR,
+			(v >> 8) & 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_SS_SCL_HCNT_0_ADDR,
+			v & 0xff, 7, 0);
+
+	/* Standard Mode SCL Low counter */
+	v = DIV_ROUND_UP_ULL(ss_scl_low * sfr, 1000000);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_SS_SCL_LCNT_1_ADDR,
+			(v >> 8) & 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_SS_SCL_LCNT_0_ADDR,
+			v & 0xff, 7, 0);
+
+	/* Fast Mode SCL High Counter */
+	v = DIV_ROUND_UP_ULL(fs_scl_high * sfr, 1000000);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_FS_SCL_HCNT_1_ADDR,
+			(v >> 8) & 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_FS_SCL_HCNT_0_ADDR,
+			v & 0xff, 7, 0);
+
+	/* Fast Mode SCL Low Counter */
+	v = DIV_ROUND_UP_ULL(fs_scl_low * sfr, 1000000);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_FS_SCL_LCNT_1_ADDR,
+			(v >> 8) & 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_FS_SCL_LCNT_0_ADDR,
+			v & 0xff, 7, 0);
+
+	/* SDA Hold Time */
+	v = DIV_ROUND_UP_ULL(sda_hold * sfr, 1000000);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_SDA_HOLD_ADDR, v & 0xff, 7, 0);
+
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_SLAVE, 0x50, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_SEGADDR, 0x30, 6, 0);
+
+	/* NACK_POL to high */
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x1, 7, 7);
+
+	/* NACK_MASK to unmasked */
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x0, 6, 6);
+
+	/* ARBITRATION_POL to high */
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x1, 3, 3);
+
+	/* ARBITRATION_MASK to unmasked */
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x0, 2, 2);
+
+	/* DONE_POL to high */
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_INT, 0x1, 3, 3);
+
+	/* DONE_MASK to unmasked */
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_INT, 0x0, 2, 2);
+}
+
+static void hdmi_core_ddc_uninit(struct hdmi_core_data *core)
+{
+	void __iomem *base = core->base;
+
+	/* Mask I2C interrupts */
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x1, 6, 6);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x1, 2, 2);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_INT, 0x1, 2, 2);
+}
+
+static int hdmi_core_ddc_edid(struct hdmi_core_data *core, u8 *pedid, u8 ext)
+{
+	void __iomem *base = core->base;
+	u8 cur_addr;
+	char checksum = 0;
+	const int retries = 1000;
+	u8 seg_ptr = ext / 2;
+	u8 edidbase = ((ext % 2) * 0x80);
+
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_SEGPTR, seg_ptr, 7, 0);
+
+	/*
+	 * TODO: We use polling here, although we probably should use proper
+	 * interrupts.
+	 */
+	for (cur_addr = 0; cur_addr < 128; ++cur_addr) {
+		int i;
+
+		/* clear ERROR and DONE */
+		REG_FLD_MOD(base, HDMI_CORE_IH_I2CM_STAT0, 0x3, 1, 0);
+
+		REG_FLD_MOD(base, HDMI_CORE_I2CM_ADDRESS,
+				edidbase + cur_addr, 7, 0);
+
+		if (seg_ptr)
+			REG_FLD_MOD(base, HDMI_CORE_I2CM_OPERATION, 1, 1, 1);
+		else
+			REG_FLD_MOD(base, HDMI_CORE_I2CM_OPERATION, 1, 0, 0);
+
+		for (i = 0; i < retries; ++i) {
+			u32 stat;
+
+			stat = REG_GET(base, HDMI_CORE_IH_I2CM_STAT0, 1, 0);
+
+			/* I2CM_ERROR */
+			if (stat & 1) {
+				DSSERR("HDMI I2C Master Error\n");
+				return -EIO;
+			}
+
+			/* I2CM_DONE */
+			if (stat & (1 << 1))
+				break;
+
+			usleep_range(250, 1000);
+		}
+
+		if (i == retries) {
+			DSSERR("HDMI I2C timeout reading EDID\n");
+			return -EIO;
+		}
+
+		pedid[cur_addr] = REG_GET(base, HDMI_CORE_I2CM_DATAI, 7, 0);
+		checksum += pedid[cur_addr];
+	}
+
+	return 0;
+
+}
+
+int hdmi5_read_edid(struct hdmi_core_data *core, u8 *edid, int len)
+{
+	int r, n, i;
+	int max_ext_blocks = (len / 128) - 1;
+
+	if (len < 128)
+		return -EINVAL;
+
+	hdmi_core_ddc_init(core);
+
+	r = hdmi_core_ddc_edid(core, edid, 0);
+	if (r)
+		goto out;
+
+	n = edid[0x7e];
+
+	if (n > max_ext_blocks)
+		n = max_ext_blocks;
+
+	for (i = 1; i <= n; i++) {
+		r = hdmi_core_ddc_edid(core, edid + i * EDID_LENGTH, i);
+		if (r)
+			goto out;
+	}
+
+out:
+	hdmi_core_ddc_uninit(core);
+
+	return r ? r : len;
+}
+
+void hdmi5_core_dump(struct hdmi_core_data *core, struct seq_file *s)
+{
+
+#define DUMPCORE(r) seq_printf(s, "%-35s %08x\n", #r,\
+		hdmi_read_reg(core->base, r))
+
+	DUMPCORE(HDMI_CORE_FC_INVIDCONF);
+	DUMPCORE(HDMI_CORE_FC_INHACTIV0);
+	DUMPCORE(HDMI_CORE_FC_INHACTIV1);
+	DUMPCORE(HDMI_CORE_FC_INHBLANK0);
+	DUMPCORE(HDMI_CORE_FC_INHBLANK1);
+	DUMPCORE(HDMI_CORE_FC_INVACTIV0);
+	DUMPCORE(HDMI_CORE_FC_INVACTIV1);
+	DUMPCORE(HDMI_CORE_FC_INVBLANK);
+	DUMPCORE(HDMI_CORE_FC_HSYNCINDELAY0);
+	DUMPCORE(HDMI_CORE_FC_HSYNCINDELAY1);
+	DUMPCORE(HDMI_CORE_FC_HSYNCINWIDTH0);
+	DUMPCORE(HDMI_CORE_FC_HSYNCINWIDTH1);
+	DUMPCORE(HDMI_CORE_FC_VSYNCINDELAY);
+	DUMPCORE(HDMI_CORE_FC_VSYNCINWIDTH);
+	DUMPCORE(HDMI_CORE_FC_CTRLDUR);
+	DUMPCORE(HDMI_CORE_FC_EXCTRLDUR);
+	DUMPCORE(HDMI_CORE_FC_EXCTRLSPAC);
+	DUMPCORE(HDMI_CORE_FC_CH0PREAM);
+	DUMPCORE(HDMI_CORE_FC_CH1PREAM);
+	DUMPCORE(HDMI_CORE_FC_CH2PREAM);
+	DUMPCORE(HDMI_CORE_FC_AVICONF0);
+	DUMPCORE(HDMI_CORE_FC_AVICONF1);
+	DUMPCORE(HDMI_CORE_FC_AVICONF2);
+	DUMPCORE(HDMI_CORE_FC_AVIVID);
+	DUMPCORE(HDMI_CORE_FC_PRCONF);
+
+	DUMPCORE(HDMI_CORE_MC_CLKDIS);
+	DUMPCORE(HDMI_CORE_MC_SWRSTZREQ);
+	DUMPCORE(HDMI_CORE_MC_FLOWCTRL);
+	DUMPCORE(HDMI_CORE_MC_PHYRSTZ);
+	DUMPCORE(HDMI_CORE_MC_LOCKONCLOCK);
+
+	DUMPCORE(HDMI_CORE_I2CM_SLAVE);
+	DUMPCORE(HDMI_CORE_I2CM_ADDRESS);
+	DUMPCORE(HDMI_CORE_I2CM_DATAO);
+	DUMPCORE(HDMI_CORE_I2CM_DATAI);
+	DUMPCORE(HDMI_CORE_I2CM_OPERATION);
+	DUMPCORE(HDMI_CORE_I2CM_INT);
+	DUMPCORE(HDMI_CORE_I2CM_CTLINT);
+	DUMPCORE(HDMI_CORE_I2CM_DIV);
+	DUMPCORE(HDMI_CORE_I2CM_SEGADDR);
+	DUMPCORE(HDMI_CORE_I2CM_SOFTRSTZ);
+	DUMPCORE(HDMI_CORE_I2CM_SEGPTR);
+	DUMPCORE(HDMI_CORE_I2CM_SS_SCL_HCNT_1_ADDR);
+	DUMPCORE(HDMI_CORE_I2CM_SS_SCL_HCNT_0_ADDR);
+	DUMPCORE(HDMI_CORE_I2CM_SS_SCL_LCNT_1_ADDR);
+	DUMPCORE(HDMI_CORE_I2CM_SS_SCL_LCNT_0_ADDR);
+	DUMPCORE(HDMI_CORE_I2CM_FS_SCL_HCNT_1_ADDR);
+	DUMPCORE(HDMI_CORE_I2CM_FS_SCL_HCNT_0_ADDR);
+	DUMPCORE(HDMI_CORE_I2CM_FS_SCL_LCNT_1_ADDR);
+	DUMPCORE(HDMI_CORE_I2CM_FS_SCL_LCNT_0_ADDR);
+	DUMPCORE(HDMI_CORE_I2CM_SDA_HOLD_ADDR);
+}
+
+static void hdmi_core_init(struct hdmi_core_vid_config *video_cfg,
+			struct hdmi_core_infoframe_avi *avi_cfg,
+			struct hdmi_config *cfg)
+{
+	DSSDBG("hdmi_core_init\n");
+
+	/* video core */
+	video_cfg->data_enable_pol = 1; /* It is always 1*/
+	video_cfg->v_fc_config.timings.hsync_level = cfg->timings.hsync_level;
+	video_cfg->v_fc_config.timings.x_res = cfg->timings.x_res;
+	video_cfg->v_fc_config.timings.hsw = cfg->timings.hsw - 1;
+	video_cfg->v_fc_config.timings.hbp = cfg->timings.hbp;
+	video_cfg->v_fc_config.timings.hfp = cfg->timings.hfp;
+	video_cfg->hblank = cfg->timings.hfp +
+				cfg->timings.hbp + cfg->timings.hsw - 1;
+	video_cfg->v_fc_config.timings.vsync_level = cfg->timings.vsync_level;
+	video_cfg->v_fc_config.timings.y_res = cfg->timings.y_res;
+	video_cfg->v_fc_config.timings.vsw = cfg->timings.vsw;
+	video_cfg->v_fc_config.timings.vfp = cfg->timings.vfp;
+	video_cfg->v_fc_config.timings.vbp = cfg->timings.vbp;
+	video_cfg->vblank_osc = 0; /* Always 0 - need to confirm */
+	video_cfg->vblank = cfg->timings.vsw +
+				cfg->timings.vfp + cfg->timings.vbp;
+	video_cfg->v_fc_config.cm.mode = cfg->cm.mode;
+	video_cfg->v_fc_config.timings.interlace = cfg->timings.interlace;
+
+	/* info frame */
+	avi_cfg->db1_format = 0;
+	avi_cfg->db1_active_info = 0;
+	avi_cfg->db1_bar_info_dv = 0;
+	avi_cfg->db1_scan_info = 0;
+	avi_cfg->db2_colorimetry = 0;
+	avi_cfg->db2_aspect_ratio = 0;
+	avi_cfg->db2_active_fmt_ar = 0;
+	avi_cfg->db3_itc = 0;
+	avi_cfg->db3_ec = 0;
+	avi_cfg->db3_q_range = 0;
+	avi_cfg->db3_nup_scaling = 0;
+	avi_cfg->db4_videocode = 0;
+	avi_cfg->db5_pixel_repeat = 0;
+	avi_cfg->db6_7_line_eoftop = 0;
+	avi_cfg->db8_9_line_sofbottom = 0;
+	avi_cfg->db10_11_pixel_eofleft = 0;
+	avi_cfg->db12_13_pixel_sofright = 0;
+}
+
+/* DSS_HDMI_CORE_VIDEO_CONFIG */
+static void hdmi_core_video_config(struct hdmi_core_data *core,
+			struct hdmi_core_vid_config *cfg)
+{
+	void __iomem *base = core->base;
+	unsigned char r = 0;
+	bool vsync_pol, hsync_pol;
+
+	vsync_pol =
+		cfg->v_fc_config.timings.vsync_level == OMAPDSS_SIG_ACTIVE_HIGH;
+	hsync_pol =
+		cfg->v_fc_config.timings.hsync_level == OMAPDSS_SIG_ACTIVE_HIGH;
+
+	/* Set hsync, vsync and data-enable polarity  */
+	r = hdmi_read_reg(base, HDMI_CORE_FC_INVIDCONF);
+	r = FLD_MOD(r, vsync_pol, 6, 6);
+	r = FLD_MOD(r, hsync_pol, 5, 5);
+	r = FLD_MOD(r, cfg->data_enable_pol, 4, 4);
+	r = FLD_MOD(r, cfg->vblank_osc, 1, 1);
+	r = FLD_MOD(r, cfg->v_fc_config.timings.interlace, 0, 0);
+	hdmi_write_reg(base, HDMI_CORE_FC_INVIDCONF, r);
+
+	/* set x resolution */
+	REG_FLD_MOD(base, HDMI_CORE_FC_INHACTIV1,
+			cfg->v_fc_config.timings.x_res >> 8, 4, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_INHACTIV0,
+			cfg->v_fc_config.timings.x_res & 0xFF, 7, 0);
+
+	/* set y resolution */
+	REG_FLD_MOD(base, HDMI_CORE_FC_INVACTIV1,
+			cfg->v_fc_config.timings.y_res >> 8, 4, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_INVACTIV0,
+			cfg->v_fc_config.timings.y_res & 0xFF, 7, 0);
+
+	/* set horizontal blanking pixels */
+	REG_FLD_MOD(base, HDMI_CORE_FC_INHBLANK1, cfg->hblank >> 8, 4, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_INHBLANK0, cfg->hblank & 0xFF, 7, 0);
+
+	/* set vertial blanking pixels */
+	REG_FLD_MOD(base, HDMI_CORE_FC_INVBLANK, cfg->vblank, 7, 0);
+
+	/* set horizontal sync offset */
+	REG_FLD_MOD(base, HDMI_CORE_FC_HSYNCINDELAY1,
+			cfg->v_fc_config.timings.hfp >> 8, 4, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_HSYNCINDELAY0,
+			cfg->v_fc_config.timings.hfp & 0xFF, 7, 0);
+
+	/* set vertical sync offset */
+	REG_FLD_MOD(base, HDMI_CORE_FC_VSYNCINDELAY,
+			cfg->v_fc_config.timings.vfp, 7, 0);
+
+	/* set horizontal sync pulse width */
+	REG_FLD_MOD(base, HDMI_CORE_FC_HSYNCINWIDTH1,
+			(cfg->v_fc_config.timings.hsw >> 8), 1, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_HSYNCINWIDTH0,
+			cfg->v_fc_config.timings.hsw & 0xFF, 7, 0);
+
+	/*  set vertical sync pulse width */
+	REG_FLD_MOD(base, HDMI_CORE_FC_VSYNCINWIDTH,
+			cfg->v_fc_config.timings.vsw, 5, 0);
+
+	/* select DVI mode */
+	REG_FLD_MOD(base, HDMI_CORE_FC_INVIDCONF,
+			cfg->v_fc_config.cm.mode, 3, 3);
+}
+
+static void hdmi_core_config_video_packetizer(struct hdmi_core_data *core)
+{
+	void __iomem *base = core->base;
+	int clr_depth = 0;	/* 24 bit color depth */
+
+	/* COLOR_DEPTH */
+	REG_FLD_MOD(base, HDMI_CORE_VP_PR_CD, clr_depth, 7, 4);
+	/* BYPASS_EN */
+	REG_FLD_MOD(base, HDMI_CORE_VP_CONF, clr_depth ? 0 : 1, 6, 6);
+	/* PP_EN */
+	REG_FLD_MOD(base, HDMI_CORE_VP_CONF, clr_depth ? 1 : 0, 5, 5);
+	/* YCC422_EN */
+	REG_FLD_MOD(base, HDMI_CORE_VP_CONF, 0, 3, 3);
+	/* PP_STUFFING */
+	REG_FLD_MOD(base, HDMI_CORE_VP_STUFF, clr_depth ? 1 : 0, 1, 1);
+	/* YCC422_STUFFING */
+	REG_FLD_MOD(base, HDMI_CORE_VP_STUFF, 1, 2, 2);
+	/* OUTPUT_SELECTOR */
+	REG_FLD_MOD(base, HDMI_CORE_VP_CONF, clr_depth ? 0 : 2, 1, 0);
+}
+
+static void hdmi_core_config_csc(struct hdmi_core_data *core)
+{
+	int clr_depth = 0;	/* 24 bit color depth */
+
+	/* CSC_COLORDEPTH */
+	REG_FLD_MOD(core->base, HDMI_CORE_CSC_SCALE, clr_depth, 7, 4);
+}
+
+static void hdmi_core_config_video_sampler(struct hdmi_core_data *core)
+{
+	int video_mapping = 1;	/* for 24 bit color depth */
+
+	/* VIDEO_MAPPING */
+	REG_FLD_MOD(core->base, HDMI_CORE_TX_INVID0, video_mapping, 4, 0);
+}
+
+static void hdmi_core_aux_infoframe_avi_config(struct hdmi_core_data *core)
+{
+	void __iomem *base = core->base;
+	struct hdmi_core_infoframe_avi avi = core->avi_cfg;
+
+	REG_FLD_MOD(base, HDMI_CORE_FC_AVICONF0, avi.db1_format, 1, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_AVICONF0, avi.db1_active_info, 6, 6);
+	REG_FLD_MOD(base, HDMI_CORE_FC_AVICONF0, avi.db1_bar_info_dv, 3, 2);
+	REG_FLD_MOD(base, HDMI_CORE_FC_AVICONF0, avi.db1_scan_info, 5, 4);
+	REG_FLD_MOD(base, HDMI_CORE_FC_AVICONF1, avi.db2_colorimetry, 7, 6);
+	REG_FLD_MOD(base, HDMI_CORE_FC_AVICONF1, avi.db2_aspect_ratio, 5, 4);
+	REG_FLD_MOD(base, HDMI_CORE_FC_AVICONF1, avi.db2_active_fmt_ar, 3, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_AVICONF2, avi.db3_itc, 7, 7);
+	REG_FLD_MOD(base, HDMI_CORE_FC_AVICONF2, avi.db3_ec, 6, 4);
+	REG_FLD_MOD(base, HDMI_CORE_FC_AVICONF2, avi.db3_q_range, 3, 2);
+	REG_FLD_MOD(base, HDMI_CORE_FC_AVICONF2, avi.db3_nup_scaling, 1, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_AVIVID, avi.db4_videocode, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_PRCONF, avi.db5_pixel_repeat, 3, 0);
+}
+
+static void hdmi_core_csc_config(struct hdmi_core_data *core,
+		struct csc_table csc_coeff)
+{
+	void __iomem *base = core->base;
+
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A1_MSB, csc_coeff.a1 >> 8 , 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A1_LSB, csc_coeff.a1, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A2_MSB, csc_coeff.a2 >> 8, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A2_LSB, csc_coeff.a2, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A3_MSB, csc_coeff.a3 >> 8, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A3_LSB, csc_coeff.a3, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A4_MSB, csc_coeff.a4 >> 8, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A4_LSB, csc_coeff.a4, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B1_MSB, csc_coeff.b1 >> 8, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B1_LSB, csc_coeff.b1, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B2_MSB, csc_coeff.b2 >> 8, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B2_LSB, csc_coeff.b2, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B3_MSB, csc_coeff.b3 >> 8, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B3_LSB, csc_coeff.b3, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B4_MSB, csc_coeff.b4 >> 8, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B4_LSB, csc_coeff.b4, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C1_MSB, csc_coeff.c1 >> 8, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C1_LSB, csc_coeff.c1, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C2_MSB, csc_coeff.c2 >> 8, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C2_LSB, csc_coeff.c2, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C3_MSB, csc_coeff.c3 >> 8, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C3_LSB, csc_coeff.c3, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C4_MSB, csc_coeff.c4 >> 8, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C4_LSB, csc_coeff.c4, 7, 0);
+
+	REG_FLD_MOD(base, HDMI_CORE_MC_FLOWCTRL, 0x1, 0, 0);
+}
+
+static void hdmi_core_configure_range(struct hdmi_core_data *core)
+{
+	struct csc_table csc_coeff = { 0 };
+
+	/* support limited range with 24 bit color depth for now */
+	csc_coeff = csc_table_deepcolor[0];
+	core->avi_cfg.db3_q_range = HDMI_INFOFRAME_AVI_DB3Q_LR;
+
+	hdmi_core_csc_config(core, csc_coeff);
+	hdmi_core_aux_infoframe_avi_config(core);
+}
+
+static void hdmi_core_enable_video_path(struct hdmi_core_data *core)
+{
+	void __iomem *base = core->base;
+
+	DSSDBG("hdmi_core_enable_video_path\n");
+
+	REG_FLD_MOD(base, HDMI_CORE_FC_CTRLDUR, 0x0C, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_EXCTRLDUR, 0x20, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_EXCTRLSPAC, 0x01, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_CH0PREAM, 0x0B, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_CH1PREAM, 0x16, 5, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_CH2PREAM, 0x21, 5, 0);
+	REG_FLD_MOD(base, HDMI_CORE_MC_CLKDIS, 0x00, 0, 0);
+	REG_FLD_MOD(base, HDMI_CORE_MC_CLKDIS, 0x00, 1, 1);
+}
+
+static void hdmi_core_mask_interrupts(struct hdmi_core_data *core)
+{
+	void __iomem *base = core->base;
+
+	/* Master IRQ mask */
+	REG_FLD_MOD(base, HDMI_CORE_IH_MUTE, 0x3, 1, 0);
+
+	/* Mask all the interrupts in HDMI core */
+
+	REG_FLD_MOD(base, HDMI_CORE_VP_MASK, 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_MASK0, 0xe7, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_MASK1, 0xfb, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_MASK2, 0x3, 1, 0);
+
+	REG_FLD_MOD(base, HDMI_CORE_AUD_INT, 0x3, 3, 2);
+	REG_FLD_MOD(base, HDMI_CORE_AUD_GP_MASK, 0x3, 1, 0);
+
+	REG_FLD_MOD(base, HDMI_CORE_CEC_MASK, 0x7f, 6, 0);
+
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x1, 6, 6);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x1, 2, 2);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_INT, 0x1, 2, 2);
+
+	REG_FLD_MOD(base, HDMI_CORE_PHY_MASK0, 0xf3, 7, 0);
+
+	REG_FLD_MOD(base, HDMI_CORE_IH_PHY_STAT0, 0xff, 7, 0);
+
+	/* Clear all the current interrupt bits */
+
+	REG_FLD_MOD(base, HDMI_CORE_IH_VP_STAT0, 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_IH_FC_STAT0, 0xe7, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_IH_FC_STAT1, 0xfb, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_IH_FC_STAT2, 0x3, 1, 0);
+
+	REG_FLD_MOD(base, HDMI_CORE_IH_AS_STAT0, 0x7, 2, 0);
+
+	REG_FLD_MOD(base, HDMI_CORE_IH_CEC_STAT0, 0x7f, 6, 0);
+
+	REG_FLD_MOD(base, HDMI_CORE_IH_I2CM_STAT0, 0x3, 1, 0);
+
+	REG_FLD_MOD(base, HDMI_CORE_IH_PHY_STAT0, 0xff, 7, 0);
+}
+
+static void hdmi_core_enable_interrupts(struct hdmi_core_data *core)
+{
+	/* Unmute interrupts */
+	REG_FLD_MOD(core->base, HDMI_CORE_IH_MUTE, 0x0, 1, 0);
+}
+
+int hdmi5_core_handle_irqs(struct hdmi_core_data *core)
+{
+	void __iomem *base = core->base;
+
+	REG_FLD_MOD(base, HDMI_CORE_IH_FC_STAT0, 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_IH_FC_STAT1, 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_IH_FC_STAT2, 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_IH_AS_STAT0, 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_IH_PHY_STAT0, 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_IH_I2CM_STAT0, 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_IH_CEC_STAT0, 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_IH_VP_STAT0, 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_IH_I2CMPHY_STAT0, 0xff, 7, 0);
+
+	return 0;
+}
+
+void hdmi5_configure(struct hdmi_core_data *core, struct hdmi_wp_data *wp,
+		struct hdmi_config *cfg)
+{
+	struct omap_video_timings video_timing;
+	struct hdmi_video_format video_format;
+	struct hdmi_core_vid_config v_core_cfg;
+	struct hdmi_core_infoframe_avi *avi_cfg = &core->avi_cfg;
+
+	hdmi_core_mask_interrupts(core);
+
+	hdmi_core_init(&v_core_cfg, avi_cfg, cfg);
+
+	hdmi_wp_init_vid_fmt_timings(&video_format, &video_timing, cfg);
+
+	hdmi_wp_video_config_timing(wp, &video_timing);
+
+	/* video config */
+	video_format.packing_mode = HDMI_PACK_24b_RGB_YUV444_YUV422;
+
+	hdmi_wp_video_config_format(wp, &video_format);
+
+	hdmi_wp_video_config_interface(wp, &video_timing);
+
+	hdmi_core_configure_range(core);
+
+	/*
+	 * configure core video part, set software reset in the core
+	 */
+	v_core_cfg.packet_mode = HDMI_PACKETMODE24BITPERPIXEL;
+
+	hdmi_core_video_config(core, &v_core_cfg);
+
+	hdmi_core_config_video_packetizer(core);
+	hdmi_core_config_csc(core);
+	hdmi_core_config_video_sampler(core);
+
+	/*
+	 * configure packet info frame video see doc CEA861-D page 65
+	 */
+	avi_cfg->db1_format = HDMI_INFOFRAME_AVI_DB1Y_RGB;
+	avi_cfg->db1_active_info =
+			HDMI_INFOFRAME_AVI_DB1A_ACTIVE_FORMAT_OFF;
+	avi_cfg->db1_bar_info_dv = HDMI_INFOFRAME_AVI_DB1B_NO;
+	avi_cfg->db1_scan_info = HDMI_INFOFRAME_AVI_DB1S_0;
+	avi_cfg->db2_colorimetry = HDMI_INFOFRAME_AVI_DB2C_NO;
+	avi_cfg->db2_aspect_ratio = HDMI_INFOFRAME_AVI_DB2M_NO;
+	avi_cfg->db2_active_fmt_ar = HDMI_INFOFRAME_AVI_DB2R_SAME;
+	avi_cfg->db3_itc = HDMI_INFOFRAME_AVI_DB3ITC_NO;
+	avi_cfg->db3_ec = HDMI_INFOFRAME_AVI_DB3EC_XVYUV601;
+	avi_cfg->db3_q_range = HDMI_INFOFRAME_AVI_DB3Q_DEFAULT;
+	avi_cfg->db3_nup_scaling = HDMI_INFOFRAME_AVI_DB3SC_NO;
+	avi_cfg->db4_videocode = cfg->cm.code;
+	avi_cfg->db5_pixel_repeat = HDMI_INFOFRAME_AVI_DB5PR_NO;
+	avi_cfg->db6_7_line_eoftop = 0;
+	avi_cfg->db8_9_line_sofbottom = 0;
+	avi_cfg->db10_11_pixel_eofleft = 0;
+	avi_cfg->db12_13_pixel_sofright = 0;
+
+	hdmi_core_aux_infoframe_avi_config(core);
+
+	hdmi_core_enable_video_path(core);
+
+	hdmi_core_enable_interrupts(core);
+}
+
+
+#if defined(CONFIG_OMAP5_DSS_HDMI_AUDIO)
+
+static void hdmi5_core_audio_config(struct hdmi_core_data *core,
+			struct hdmi_core_audio_config *cfg)
+{
+	void __iomem *base = core->base;
+	u8 val;
+
+	/* Mute audio before configuring */
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCONF, 0xf, 7, 4);
+
+	/* Set the N parameter */
+	REG_FLD_MOD(base, HDMI_CORE_AUD_N1, cfg->n, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_AUD_N2, cfg->n >> 8, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_AUD_N3, cfg->n >> 16, 3, 0);
+
+	/*
+	 * CTS manual mode. Automatic mode is not supported when using audio
+	 * parallel interface.
+	 */
+	REG_FLD_MOD(base, HDMI_CORE_AUD_CTS3, 1, 4, 4);
+	REG_FLD_MOD(base, HDMI_CORE_AUD_CTS1, cfg->cts, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_AUD_CTS2, cfg->cts >> 8, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_AUD_CTS3, cfg->cts >> 16, 3, 0);
+
+	/* Layout of Audio Sample Packets: 2-channel or multichannels */
+	if (cfg->layout == HDMI_AUDIO_LAYOUT_2CH)
+		REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCONF, 0, 0, 0);
+	else
+		REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCONF, 1, 0, 0);
+
+	/* Configure IEC-609580 Validity bits */
+	/* Channel 0 is valid */
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, 0, 0, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, 0, 4, 4);
+
+	if (cfg->layout == HDMI_AUDIO_LAYOUT_2CH)
+		val = 1;
+	else
+		val = 0;
+
+	/* Channels 1, 2 setting */
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, val, 1, 1);
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, val, 5, 5);
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, val, 2, 2);
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, val, 6, 6);
+	/* Channel 3 setting */
+	if (cfg->layout == HDMI_AUDIO_LAYOUT_6CH)
+		val = 1;
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, val, 3, 3);
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, val, 7, 7);
+
+	/* Configure IEC-60958 User bits */
+	/* TODO: should be set by user. */
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSU, 0, 7, 0);
+
+	/* Configure IEC-60958 Channel Status word */
+	/* CGMSA */
+	val = cfg->iec60958_cfg->status[5] & IEC958_AES5_CON_CGMSA;
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(0), val, 5, 4);
+
+	/* Copyright */
+	val = (cfg->iec60958_cfg->status[0] &
+			IEC958_AES0_CON_NOT_COPYRIGHT) >> 2;
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(0), val, 0, 0);
+
+	/* Category */
+	hdmi_write_reg(base, HDMI_CORE_FC_AUDSCHNLS(1),
+		cfg->iec60958_cfg->status[1]);
+
+	/* PCM audio mode */
+	val = (cfg->iec60958_cfg->status[0] & IEC958_AES0_CON_MODE) >> 6;
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(2), val, 6, 4);
+
+	/* Source number */
+	val = cfg->iec60958_cfg->status[2] & IEC958_AES2_CON_SOURCE;
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(2), val, 3, 4);
+
+	/* Channel number right 0  */
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(3), 2, 3, 0);
+	/* Channel number right 1*/
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(3), 4, 7, 4);
+	/* Channel number right 2  */
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(4), 6, 3, 0);
+	/* Channel number right 3*/
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(4), 8, 7, 4);
+	/* Channel number left 0  */
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(5), 1, 3, 0);
+	/* Channel number left 1*/
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(5), 3, 7, 4);
+	/* Channel number left 2  */
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(6), 5, 3, 0);
+	/* Channel number left 3*/
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(6), 7, 7, 4);
+
+	/* Clock accuracy and sample rate */
+	hdmi_write_reg(base, HDMI_CORE_FC_AUDSCHNLS(7),
+		cfg->iec60958_cfg->status[3]);
+
+	/* Original sample rate and word length */
+	hdmi_write_reg(base, HDMI_CORE_FC_AUDSCHNLS(8),
+		cfg->iec60958_cfg->status[4]);
+
+	/* Enable FIFO empty and full interrupts */
+	REG_FLD_MOD(base, HDMI_CORE_AUD_INT, 3, 3, 2);
+
+	/* Configure GPA */
+	/* select HBR/SPDIF interfaces */
+	if (cfg->layout == HDMI_AUDIO_LAYOUT_2CH) {
+		/* select HBR/SPDIF interfaces */
+		REG_FLD_MOD(base, HDMI_CORE_AUD_CONF0, 0, 5, 5);
+		/* enable two channels in GPA */
+		REG_FLD_MOD(base, HDMI_CORE_AUD_GP_CONF1, 3, 7, 0);
+	} else if (cfg->layout == HDMI_AUDIO_LAYOUT_6CH) {
+		/* select HBR/SPDIF interfaces */
+		REG_FLD_MOD(base, HDMI_CORE_AUD_CONF0, 0, 5, 5);
+		/* enable six channels in GPA */
+		REG_FLD_MOD(base, HDMI_CORE_AUD_GP_CONF1, 0x3F, 7, 0);
+	} else {
+		/* select HBR/SPDIF interfaces */
+		REG_FLD_MOD(base, HDMI_CORE_AUD_CONF0, 0, 5, 5);
+		/* enable eight channels in GPA */
+		REG_FLD_MOD(base, HDMI_CORE_AUD_GP_CONF1, 0xFF, 7, 0);
+	}
+
+	/* disable HBR */
+	REG_FLD_MOD(base, HDMI_CORE_AUD_GP_CONF2, 0, 0, 0);
+	/* enable PCUV */
+	REG_FLD_MOD(base, HDMI_CORE_AUD_GP_CONF2, 1, 1, 1);
+	/* enable GPA FIFO full and empty mask */
+	REG_FLD_MOD(base, HDMI_CORE_AUD_GP_MASK, 3, 1, 0);
+	/* set polarity of GPA FIFO empty interrupts */
+	REG_FLD_MOD(base, HDMI_CORE_AUD_GP_POL, 1, 0, 0);
+
+	/* unmute audio */
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCONF, 0, 7, 4);
+}
+
+static void hdmi5_core_audio_infoframe_cfg(struct hdmi_core_data *core,
+	 struct snd_cea_861_aud_if *info_aud)
+{
+	void __iomem *base = core->base;
+
+	/* channel count and coding type fields in AUDICONF0 are swapped */
+	hdmi_write_reg(base, HDMI_CORE_FC_AUDICONF0,
+		(info_aud->db1_ct_cc & CEA861_AUDIO_INFOFRAME_DB1CC) << 4 |
+		(info_aud->db1_ct_cc & CEA861_AUDIO_INFOFRAME_DB1CT) >> 4);
+
+	hdmi_write_reg(base, HDMI_CORE_FC_AUDICONF1, info_aud->db2_sf_ss);
+	hdmi_write_reg(base, HDMI_CORE_FC_AUDICONF2, info_aud->db4_ca);
+	hdmi_write_reg(base, HDMI_CORE_FC_AUDICONF3, info_aud->db5_dminh_lsv);
+}
+
+int hdmi5_audio_config(struct hdmi_core_data *core, struct hdmi_wp_data *wp,
+			struct omap_dss_audio *audio, u32 pclk)
+{
+	struct hdmi_audio_format audio_format;
+	struct hdmi_audio_dma audio_dma;
+	struct hdmi_core_audio_config core_cfg;
+	int err, n, cts, channel_count;
+	unsigned int fs_nr;
+	bool word_length_16b = false;
+
+	if (!audio || !audio->iec || !audio->cea || !core)
+		return -EINVAL;
+
+	core_cfg.iec60958_cfg = audio->iec;
+
+	if (!(audio->iec->status[4] & IEC958_AES4_CON_MAX_WORDLEN_24) &&
+		(audio->iec->status[4] & IEC958_AES4_CON_WORDLEN_20_16))
+			word_length_16b = true;
+
+	/* only 16-bit word length supported atm */
+	if (!word_length_16b)
+		return -EINVAL;
+
+	switch (audio->iec->status[3] & IEC958_AES3_CON_FS) {
+	case IEC958_AES3_CON_FS_32000:
+		fs_nr = 32000;
+		break;
+	case IEC958_AES3_CON_FS_44100:
+		fs_nr = 44100;
+		break;
+	case IEC958_AES3_CON_FS_48000:
+		fs_nr = 48000;
+		break;
+	case IEC958_AES3_CON_FS_88200:
+		fs_nr = 88200;
+		break;
+	case IEC958_AES3_CON_FS_96000:
+		fs_nr = 96000;
+		break;
+	case IEC958_AES3_CON_FS_176400:
+		fs_nr = 176400;
+		break;
+	case IEC958_AES3_CON_FS_192000:
+		fs_nr = 192000;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	err = hdmi_compute_acr(pclk, fs_nr, &n, &cts);
+	core_cfg.n = n;
+	core_cfg.cts = cts;
+
+	/* Audio channels settings */
+	channel_count = (audio->cea->db1_ct_cc & CEA861_AUDIO_INFOFRAME_DB1CC)
+				+ 1;
+
+	if (channel_count == 2)
+		core_cfg.layout = HDMI_AUDIO_LAYOUT_2CH;
+	else if (channel_count == 6)
+		core_cfg.layout = HDMI_AUDIO_LAYOUT_6CH;
+	else
+		core_cfg.layout = HDMI_AUDIO_LAYOUT_8CH;
+
+	/* DMA settings */
+	if (word_length_16b)
+		audio_dma.transfer_size = 0x10;
+	else
+		audio_dma.transfer_size = 0x20;
+	audio_dma.block_size = 0xC0;
+	audio_dma.mode = HDMI_AUDIO_TRANSF_DMA;
+	audio_dma.fifo_threshold = 0x20; /* in number of samples */
+
+	/* audio FIFO format settings for 16-bit samples*/
+	audio_format.samples_per_word = HDMI_AUDIO_ONEWORD_TWOSAMPLES;
+	audio_format.sample_size = HDMI_AUDIO_SAMPLE_16BITS;
+	audio_format.justification = HDMI_AUDIO_JUSTIFY_LEFT;
+
+	/* only LPCM atm */
+	audio_format.type = HDMI_AUDIO_TYPE_LPCM;
+
+	/* disable start/stop signals of IEC 60958 blocks */
+	audio_format.en_sig_blk_strt_end = HDMI_AUDIO_BLOCK_SIG_STARTEND_ON;
+
+	/* configure DMA and audio FIFO format*/
+	hdmi_wp_audio_config_dma(wp, &audio_dma);
+	hdmi_wp_audio_config_format(wp, &audio_format);
+
+	/* configure the core */
+	hdmi5_core_audio_config(core, &core_cfg);
+
+	/* configure CEA 861 audio infoframe */
+	hdmi5_core_audio_infoframe_cfg(core, audio->cea);
+
+	return 0;
+}
+#endif
+
+int hdmi5_core_init(struct platform_device *pdev, struct hdmi_core_data *core)
+{
+	struct resource *res;
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "core");
+	if (!res) {
+		DSSERR("can't get CORE IORESOURCE_MEM HDMI\n");
+		return -EINVAL;
+	}
+
+	core->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(core->base)) {
+		DSSERR("can't ioremap HDMI core\n");
+		return PTR_ERR(core->base);
+	}
+
+	return 0;
+}
diff --git a/drivers/video/fbdev/omap2/dss/hdmi5_core.h b/drivers/video/fbdev/omap2/dss/hdmi5_core.h
new file mode 100644
index 00000000000..ce7e9f376f0
--- /dev/null
+++ b/drivers/video/fbdev/omap2/dss/hdmi5_core.h
@@ -0,0 +1,306 @@
+/*
+ * HDMI driver definition for TI OMAP5 processors.
+ *
+ * Copyright (C) 2011-2012 Texas Instruments Incorporated - http://www.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.
+ *
+ * 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 _HDMI5_CORE_H_
+#define _HDMI5_CORE_H_
+
+#include "hdmi.h"
+
+/* HDMI IP Core System */
+
+/* HDMI Identification */
+#define HDMI_CORE_DESIGN_ID			0x00000
+#define HDMI_CORE_REVISION_ID			0x00004
+#define HDMI_CORE_PRODUCT_ID0			0x00008
+#define HDMI_CORE_PRODUCT_ID1			0x0000C
+#define HDMI_CORE_CONFIG0_ID			0x00010
+#define HDMI_CORE_CONFIG1_ID			0x00014
+#define HDMI_CORE_CONFIG2_ID			0x00018
+#define HDMI_CORE_CONFIG3_ID			0x0001C
+
+/* HDMI Interrupt */
+#define HDMI_CORE_IH_FC_STAT0			0x00400
+#define HDMI_CORE_IH_FC_STAT1			0x00404
+#define HDMI_CORE_IH_FC_STAT2			0x00408
+#define HDMI_CORE_IH_AS_STAT0			0x0040C
+#define HDMI_CORE_IH_PHY_STAT0			0x00410
+#define HDMI_CORE_IH_I2CM_STAT0			0x00414
+#define HDMI_CORE_IH_CEC_STAT0			0x00418
+#define HDMI_CORE_IH_VP_STAT0			0x0041C
+#define HDMI_CORE_IH_I2CMPHY_STAT0		0x00420
+#define HDMI_CORE_IH_MUTE			0x007FC
+
+/* HDMI Video Sampler */
+#define HDMI_CORE_TX_INVID0			0x00800
+#define HDMI_CORE_TX_INSTUFFING			0x00804
+#define HDMI_CORE_TX_RGYDATA0			0x00808
+#define HDMI_CORE_TX_RGYDATA1			0x0080C
+#define HDMI_CORE_TX_RCRDATA0			0x00810
+#define HDMI_CORE_TX_RCRDATA1			0x00814
+#define HDMI_CORE_TX_BCBDATA0			0x00818
+#define HDMI_CORE_TX_BCBDATA1			0x0081C
+
+/* HDMI Video Packetizer */
+#define HDMI_CORE_VP_STATUS			0x02000
+#define HDMI_CORE_VP_PR_CD			0x02004
+#define HDMI_CORE_VP_STUFF			0x02008
+#define HDMI_CORE_VP_REMAP			0x0200C
+#define HDMI_CORE_VP_CONF			0x02010
+#define HDMI_CORE_VP_STAT			0x02014
+#define HDMI_CORE_VP_INT			0x02018
+#define HDMI_CORE_VP_MASK			0x0201C
+#define HDMI_CORE_VP_POL			0x02020
+
+/* Frame Composer */
+#define HDMI_CORE_FC_INVIDCONF			0x04000
+#define HDMI_CORE_FC_INHACTIV0			0x04004
+#define HDMI_CORE_FC_INHACTIV1			0x04008
+#define HDMI_CORE_FC_INHBLANK0			0x0400C
+#define HDMI_CORE_FC_INHBLANK1			0x04010
+#define HDMI_CORE_FC_INVACTIV0			0x04014
+#define HDMI_CORE_FC_INVACTIV1			0x04018
+#define HDMI_CORE_FC_INVBLANK			0x0401C
+#define HDMI_CORE_FC_HSYNCINDELAY0		0x04020
+#define HDMI_CORE_FC_HSYNCINDELAY1		0x04024
+#define HDMI_CORE_FC_HSYNCINWIDTH0		0x04028
+#define HDMI_CORE_FC_HSYNCINWIDTH1		0x0402C
+#define HDMI_CORE_FC_VSYNCINDELAY		0x04030
+#define HDMI_CORE_FC_VSYNCINWIDTH		0x04034
+#define HDMI_CORE_FC_INFREQ0			0x04038
+#define HDMI_CORE_FC_INFREQ1			0x0403C
+#define HDMI_CORE_FC_INFREQ2			0x04040
+#define HDMI_CORE_FC_CTRLDUR			0x04044
+#define HDMI_CORE_FC_EXCTRLDUR			0x04048
+#define HDMI_CORE_FC_EXCTRLSPAC			0x0404C
+#define HDMI_CORE_FC_CH0PREAM			0x04050
+#define HDMI_CORE_FC_CH1PREAM			0x04054
+#define HDMI_CORE_FC_CH2PREAM			0x04058
+#define HDMI_CORE_FC_AVICONF3			0x0405C
+#define HDMI_CORE_FC_GCP			0x04060
+#define HDMI_CORE_FC_AVICONF0			0x04064
+#define HDMI_CORE_FC_AVICONF1			0x04068
+#define HDMI_CORE_FC_AVICONF2			0x0406C
+#define HDMI_CORE_FC_AVIVID			0x04070
+#define HDMI_CORE_FC_AVIETB0			0x04074
+#define HDMI_CORE_FC_AVIETB1			0x04078
+#define HDMI_CORE_FC_AVISBB0			0x0407C
+#define HDMI_CORE_FC_AVISBB1			0x04080
+#define HDMI_CORE_FC_AVIELB0			0x04084
+#define HDMI_CORE_FC_AVIELB1			0x04088
+#define HDMI_CORE_FC_AVISRB0			0x0408C
+#define HDMI_CORE_FC_AVISRB1			0x04090
+#define HDMI_CORE_FC_AUDICONF0			0x04094
+#define HDMI_CORE_FC_AUDICONF1			0x04098
+#define HDMI_CORE_FC_AUDICONF2			0x0409C
+#define HDMI_CORE_FC_AUDICONF3			0x040A0
+#define HDMI_CORE_FC_VSDIEEEID0			0x040A4
+#define HDMI_CORE_FC_VSDSIZE			0x040A8
+#define HDMI_CORE_FC_VSDIEEEID1			0x040C0
+#define HDMI_CORE_FC_VSDIEEEID2			0x040C4
+#define HDMI_CORE_FC_VSDPAYLOAD(n)		(n * 4 + 0x040C8)
+#define HDMI_CORE_FC_SPDVENDORNAME(n)		(n * 4 + 0x04128)
+#define HDMI_CORE_FC_SPDPRODUCTNAME(n)		(n * 4 + 0x04148)
+#define HDMI_CORE_FC_SPDDEVICEINF		0x04188
+#define HDMI_CORE_FC_AUDSCONF			0x0418C
+#define HDMI_CORE_FC_AUDSSTAT			0x04190
+#define HDMI_CORE_FC_AUDSV			0x04194
+#define HDMI_CORE_FC_AUDSU			0x04198
+#define HDMI_CORE_FC_AUDSCHNLS(n)		(n * 4 + 0x0419C)
+#define HDMI_CORE_FC_CTRLQHIGH			0x041CC
+#define HDMI_CORE_FC_CTRLQLOW			0x041D0
+#define HDMI_CORE_FC_ACP0			0x041D4
+#define HDMI_CORE_FC_ACP(n)			((16-n) * 4 + 0x04208)
+#define HDMI_CORE_FC_ISCR1_0			0x04248
+#define HDMI_CORE_FC_ISCR1(n)			((16-n) * 4 + 0x0424C)
+#define HDMI_CORE_FC_ISCR2(n)			((15-n) * 4 + 0x0428C)
+#define HDMI_CORE_FC_DATAUTO0			0x042CC
+#define HDMI_CORE_FC_DATAUTO1			0x042D0
+#define HDMI_CORE_FC_DATAUTO2			0x042D4
+#define HDMI_CORE_FC_DATMAN			0x042D8
+#define HDMI_CORE_FC_DATAUTO3			0x042DC
+#define HDMI_CORE_FC_RDRB(n)			(n * 4 + 0x042E0)
+#define HDMI_CORE_FC_STAT0			0x04340
+#define HDMI_CORE_FC_INT0			0x04344
+#define HDMI_CORE_FC_MASK0			0x04348
+#define HDMI_CORE_FC_POL0			0x0434C
+#define HDMI_CORE_FC_STAT1			0x04350
+#define HDMI_CORE_FC_INT1			0x04354
+#define HDMI_CORE_FC_MASK1			0x04358
+#define HDMI_CORE_FC_POL1			0x0435C
+#define HDMI_CORE_FC_STAT2			0x04360
+#define HDMI_CORE_FC_INT2			0x04364
+#define HDMI_CORE_FC_MASK2			0x04368
+#define HDMI_CORE_FC_POL2			0x0436C
+#define HDMI_CORE_FC_PRCONF			0x04380
+#define HDMI_CORE_FC_GMD_STAT			0x04400
+#define HDMI_CORE_FC_GMD_EN			0x04404
+#define HDMI_CORE_FC_GMD_UP			0x04408
+#define HDMI_CORE_FC_GMD_CONF			0x0440C
+#define HDMI_CORE_FC_GMD_HB			0x04410
+#define HDMI_CORE_FC_GMD_PB(n)			(n * 4 + 0x04414)
+#define HDMI_CORE_FC_DBGFORCE			0x04800
+#define HDMI_CORE_FC_DBGAUD0CH0			0x04804
+#define HDMI_CORE_FC_DBGAUD1CH0			0x04808
+#define HDMI_CORE_FC_DBGAUD2CH0			0x0480C
+#define HDMI_CORE_FC_DBGAUD0CH1			0x04810
+#define HDMI_CORE_FC_DBGAUD1CH1			0x04814
+#define HDMI_CORE_FC_DBGAUD2CH1			0x04818
+#define HDMI_CORE_FC_DBGAUD0CH2			0x0481C
+#define HDMI_CORE_FC_DBGAUD1CH2			0x04820
+#define HDMI_CORE_FC_DBGAUD2CH2			0x04824
+#define HDMI_CORE_FC_DBGAUD0CH3			0x04828
+#define HDMI_CORE_FC_DBGAUD1CH3			0x0482C
+#define HDMI_CORE_FC_DBGAUD2CH3			0x04830
+#define HDMI_CORE_FC_DBGAUD0CH4			0x04834
+#define HDMI_CORE_FC_DBGAUD1CH4			0x04838
+#define HDMI_CORE_FC_DBGAUD2CH4			0x0483C
+#define HDMI_CORE_FC_DBGAUD0CH5			0x04840
+#define HDMI_CORE_FC_DBGAUD1CH5			0x04844
+#define HDMI_CORE_FC_DBGAUD2CH5			0x04848
+#define HDMI_CORE_FC_DBGAUD0CH6			0x0484C
+#define HDMI_CORE_FC_DBGAUD1CH6			0x04850
+#define HDMI_CORE_FC_DBGAUD2CH6			0x04854
+#define HDMI_CORE_FC_DBGAUD0CH7			0x04858
+#define HDMI_CORE_FC_DBGAUD1CH7			0x0485C
+#define HDMI_CORE_FC_DBGAUD2CH7			0x04860
+#define HDMI_CORE_FC_DBGTMDS0			0x04864
+#define HDMI_CORE_FC_DBGTMDS1			0x04868
+#define HDMI_CORE_FC_DBGTMDS2			0x0486C
+#define HDMI_CORE_PHY_MASK0			0x0C018
+#define HDMI_CORE_PHY_I2CM_INT_ADDR		0x0C09C
+#define HDMI_CORE_PHY_I2CM_CTLINT_ADDR		0x0C0A0
+
+/* HDMI Audio */
+#define HDMI_CORE_AUD_CONF0			0x0C400
+#define HDMI_CORE_AUD_CONF1			0x0C404
+#define HDMI_CORE_AUD_INT			0x0C408
+#define HDMI_CORE_AUD_N1			0x0C800
+#define HDMI_CORE_AUD_N2			0x0C804
+#define HDMI_CORE_AUD_N3			0x0C808
+#define HDMI_CORE_AUD_CTS1			0x0C80C
+#define HDMI_CORE_AUD_CTS2			0x0C810
+#define HDMI_CORE_AUD_CTS3			0x0C814
+#define HDMI_CORE_AUD_INCLKFS			0x0C818
+#define HDMI_CORE_AUD_CC08			0x0CC08
+#define HDMI_CORE_AUD_GP_CONF0			0x0D400
+#define HDMI_CORE_AUD_GP_CONF1			0x0D404
+#define HDMI_CORE_AUD_GP_CONF2			0x0D408
+#define HDMI_CORE_AUD_D010			0x0D010
+#define HDMI_CORE_AUD_GP_STAT			0x0D40C
+#define HDMI_CORE_AUD_GP_INT			0x0D410
+#define HDMI_CORE_AUD_GP_POL			0x0D414
+#define HDMI_CORE_AUD_GP_MASK			0x0D418
+
+/* HDMI Main Controller */
+#define HDMI_CORE_MC_CLKDIS			0x10004
+#define HDMI_CORE_MC_SWRSTZREQ			0x10008
+#define HDMI_CORE_MC_FLOWCTRL			0x10010
+#define HDMI_CORE_MC_PHYRSTZ			0x10014
+#define HDMI_CORE_MC_LOCKONCLOCK		0x10018
+
+/* HDMI COLOR SPACE CONVERTER */
+#define HDMI_CORE_CSC_CFG			0x10400
+#define HDMI_CORE_CSC_SCALE			0x10404
+#define HDMI_CORE_CSC_COEF_A1_MSB		0x10408
+#define HDMI_CORE_CSC_COEF_A1_LSB		0x1040C
+#define HDMI_CORE_CSC_COEF_A2_MSB		0x10410
+#define HDMI_CORE_CSC_COEF_A2_LSB		0x10414
+#define HDMI_CORE_CSC_COEF_A3_MSB		0x10418
+#define HDMI_CORE_CSC_COEF_A3_LSB		0x1041C
+#define HDMI_CORE_CSC_COEF_A4_MSB		0x10420
+#define HDMI_CORE_CSC_COEF_A4_LSB		0x10424
+#define HDMI_CORE_CSC_COEF_B1_MSB		0x10428
+#define HDMI_CORE_CSC_COEF_B1_LSB		0x1042C
+#define HDMI_CORE_CSC_COEF_B2_MSB		0x10430
+#define HDMI_CORE_CSC_COEF_B2_LSB		0x10434
+#define HDMI_CORE_CSC_COEF_B3_MSB		0x10438
+#define HDMI_CORE_CSC_COEF_B3_LSB		0x1043C
+#define HDMI_CORE_CSC_COEF_B4_MSB		0x10440
+#define HDMI_CORE_CSC_COEF_B4_LSB		0x10444
+#define HDMI_CORE_CSC_COEF_C1_MSB		0x10448
+#define HDMI_CORE_CSC_COEF_C1_LSB		0x1044C
+#define HDMI_CORE_CSC_COEF_C2_MSB		0x10450
+#define HDMI_CORE_CSC_COEF_C2_LSB		0x10454
+#define HDMI_CORE_CSC_COEF_C3_MSB		0x10458
+#define HDMI_CORE_CSC_COEF_C3_LSB		0x1045C
+#define HDMI_CORE_CSC_COEF_C4_MSB		0x10460
+#define HDMI_CORE_CSC_COEF_C4_LSB		0x10464
+
+/* HDMI HDCP */
+#define HDMI_CORE_HDCP_MASK			0x14020
+
+/* HDMI CEC */
+#define HDMI_CORE_CEC_MASK			0x17408
+
+/* HDMI I2C Master */
+#define HDMI_CORE_I2CM_SLAVE			0x157C8
+#define HDMI_CORE_I2CM_ADDRESS			0x157CC
+#define HDMI_CORE_I2CM_DATAO			0x157D0
+#define HDMI_CORE_I2CM_DATAI			0X157D4
+#define HDMI_CORE_I2CM_OPERATION		0x157D8
+#define HDMI_CORE_I2CM_INT			0x157DC
+#define HDMI_CORE_I2CM_CTLINT			0x157E0
+#define HDMI_CORE_I2CM_DIV			0x157E4
+#define HDMI_CORE_I2CM_SEGADDR			0x157E8
+#define HDMI_CORE_I2CM_SOFTRSTZ			0x157EC
+#define HDMI_CORE_I2CM_SEGPTR			0x157F0
+#define HDMI_CORE_I2CM_SS_SCL_HCNT_1_ADDR	0x157F4
+#define HDMI_CORE_I2CM_SS_SCL_HCNT_0_ADDR	0x157F8
+#define HDMI_CORE_I2CM_SS_SCL_LCNT_1_ADDR	0x157FC
+#define HDMI_CORE_I2CM_SS_SCL_LCNT_0_ADDR	0x15800
+#define HDMI_CORE_I2CM_FS_SCL_HCNT_1_ADDR	0x15804
+#define HDMI_CORE_I2CM_FS_SCL_HCNT_0_ADDR	0x15808
+#define HDMI_CORE_I2CM_FS_SCL_LCNT_1_ADDR	0x1580C
+#define HDMI_CORE_I2CM_FS_SCL_LCNT_0_ADDR	0x15810
+#define HDMI_CORE_I2CM_SDA_HOLD_ADDR		0x15814
+
+enum hdmi_core_packet_mode {
+	HDMI_PACKETMODERESERVEDVALUE = 0,
+	HDMI_PACKETMODE24BITPERPIXEL = 4,
+	HDMI_PACKETMODE30BITPERPIXEL = 5,
+	HDMI_PACKETMODE36BITPERPIXEL = 6,
+	HDMI_PACKETMODE48BITPERPIXEL = 7,
+};
+
+struct hdmi_core_vid_config {
+	struct hdmi_config v_fc_config;
+	enum hdmi_core_packet_mode packet_mode;
+	int data_enable_pol;
+	int vblank_osc;
+	int hblank;
+	int vblank;
+};
+
+struct csc_table {
+	u16 a1, a2, a3, a4;
+	u16 b1, b2, b3, b4;
+	u16 c1, c2, c3, c4;
+};
+
+int hdmi5_read_edid(struct hdmi_core_data *core, u8 *edid, int len);
+void hdmi5_core_dump(struct hdmi_core_data *core, struct seq_file *s);
+int hdmi5_core_handle_irqs(struct hdmi_core_data *core);
+void hdmi5_configure(struct hdmi_core_data *core, struct hdmi_wp_data *wp,
+			struct hdmi_config *cfg);
+int hdmi5_core_init(struct platform_device *pdev, struct hdmi_core_data *core);
+
+#if defined(CONFIG_OMAP5_DSS_HDMI_AUDIO)
+int hdmi5_audio_config(struct hdmi_core_data *core, struct hdmi_wp_data *wp,
+			struct omap_dss_audio *audio, u32 pclk);
+#endif
+#endif
diff --git a/drivers/video/fbdev/omap2/dss/hdmi_common.c b/drivers/video/fbdev/omap2/dss/hdmi_common.c
index 0b12a3f62fe..9a2c39cf297 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi_common.c
+++ b/drivers/video/fbdev/omap2/dss/hdmi_common.c
@@ -17,6 +17,7 @@
 
 #include <linux/kernel.h>
 #include <linux/err.h>
+#include <linux/of.h>
 #include <video/omapdss.h>
 
 #include "hdmi.h"
@@ -323,6 +324,46 @@ end:
 	return cm;
 }
 
+int hdmi_parse_lanes_of(struct platform_device *pdev, struct device_node *ep,
+	struct hdmi_phy_data *phy)
+{
+	struct property *prop;
+	int r, len;
+
+	prop = of_find_property(ep, "lanes", &len);
+	if (prop) {
+		u32 lanes[8];
+
+		if (len / sizeof(u32) != ARRAY_SIZE(lanes)) {
+			dev_err(&pdev->dev, "bad number of lanes\n");
+			return -EINVAL;
+		}
+
+		r = of_property_read_u32_array(ep, "lanes", lanes,
+			ARRAY_SIZE(lanes));
+		if (r) {
+			dev_err(&pdev->dev, "failed to read lane data\n");
+			return r;
+		}
+
+		r = hdmi_phy_parse_lanes(phy, lanes);
+		if (r) {
+			dev_err(&pdev->dev, "failed to parse lane data\n");
+			return r;
+		}
+	} else {
+		static const u32 default_lanes[] = { 0, 1, 2, 3, 4, 5, 6, 7 };
+
+		r = hdmi_phy_parse_lanes(phy, default_lanes);
+		if (WARN_ON(r)) {
+			dev_err(&pdev->dev, "failed to parse lane data\n");
+			return r;
+		}
+	}
+
+	return 0;
+}
+
 #if defined(CONFIG_OMAP4_DSS_HDMI_AUDIO)
 int hdmi_compute_acr(u32 pclk, u32 sample_freq, u32 *n, u32 *cts)
 {
diff --git a/drivers/video/fbdev/omap2/dss/hdmi_phy.c b/drivers/video/fbdev/omap2/dss/hdmi_phy.c
index dd376ce8da0..e007ac892d7 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi_phy.c
+++ b/drivers/video/fbdev/omap2/dss/hdmi_phy.c
@@ -12,11 +12,22 @@
 #include <linux/err.h>
 #include <linux/io.h>
 #include <linux/platform_device.h>
+#include <linux/slab.h>
 #include <video/omapdss.h>
 
 #include "dss.h"
 #include "hdmi.h"
 
+struct hdmi_phy_features {
+	bool bist_ctrl;
+	bool calc_freqout;
+	bool ldo_voltage;
+	unsigned long dcofreq_min;
+	unsigned long max_phy;
+};
+
+static const struct hdmi_phy_features *phy_feat;
+
 void hdmi_phy_dump(struct hdmi_phy_data *phy, struct seq_file *s)
 {
 #define DUMPPHY(r) seq_printf(s, "%-35s %08x\n", #r,\
@@ -26,53 +37,104 @@ void hdmi_phy_dump(struct hdmi_phy_data *phy, struct seq_file *s)
 	DUMPPHY(HDMI_TXPHY_DIGITAL_CTRL);
 	DUMPPHY(HDMI_TXPHY_POWER_CTRL);
 	DUMPPHY(HDMI_TXPHY_PAD_CFG_CTRL);
+	if (phy_feat->bist_ctrl)
+		DUMPPHY(HDMI_TXPHY_BIST_CONTROL);
 }
 
-static irqreturn_t hdmi_irq_handler(int irq, void *data)
+int hdmi_phy_parse_lanes(struct hdmi_phy_data *phy, const u32 *lanes)
 {
-	struct hdmi_wp_data *wp = data;
-	u32 irqstatus;
-
-	irqstatus = hdmi_wp_get_irqstatus(wp);
-	hdmi_wp_set_irqstatus(wp, irqstatus);
-
-	if ((irqstatus & HDMI_IRQ_LINK_CONNECT) &&
-			irqstatus & HDMI_IRQ_LINK_DISCONNECT) {
-		/*
-		 * If we get both connect and disconnect interrupts at the same
-		 * time, turn off the PHY, clear interrupts, and restart, which
-		 * raises connect interrupt if a cable is connected, or nothing
-		 * if cable is not connected.
-		 */
-		hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_OFF);
-
-		hdmi_wp_set_irqstatus(wp, HDMI_IRQ_LINK_CONNECT |
-				HDMI_IRQ_LINK_DISCONNECT);
-
-		hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_LDOON);
-	} else if (irqstatus & HDMI_IRQ_LINK_CONNECT) {
-		hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_TXON);
-	} else if (irqstatus & HDMI_IRQ_LINK_DISCONNECT) {
-		hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_LDOON);
+	int i;
+
+	for (i = 0; i < 8; i += 2) {
+		u8 lane, pol;
+		int dx, dy;
+
+		dx = lanes[i];
+		dy = lanes[i + 1];
+
+		if (dx < 0 || dx >= 8)
+			return -EINVAL;
+
+		if (dy < 0 || dy >= 8)
+			return -EINVAL;
+
+		if (dx & 1) {
+			if (dy != dx - 1)
+				return -EINVAL;
+			pol = 1;
+		} else {
+			if (dy != dx + 1)
+				return -EINVAL;
+			pol = 0;
+		}
+
+		lane = dx / 2;
+
+		phy->lane_function[lane] = i / 2;
+		phy->lane_polarity[lane] = pol;
 	}
 
-	return IRQ_HANDLED;
+	return 0;
 }
 
-int hdmi_phy_enable(struct hdmi_phy_data *phy, struct hdmi_wp_data *wp,
-			struct hdmi_config *cfg)
+static void hdmi_phy_configure_lanes(struct hdmi_phy_data *phy)
 {
-	u16 r = 0;
-	u32 irqstatus;
-
-	hdmi_wp_clear_irqenable(wp, 0xffffffff);
-
-	irqstatus = hdmi_wp_get_irqstatus(wp);
-	hdmi_wp_set_irqstatus(wp, irqstatus);
+	static const u16 pad_cfg_list[] = {
+		0x0123,
+		0x0132,
+		0x0312,
+		0x0321,
+		0x0231,
+		0x0213,
+		0x1023,
+		0x1032,
+		0x3012,
+		0x3021,
+		0x2031,
+		0x2013,
+		0x1203,
+		0x1302,
+		0x3102,
+		0x3201,
+		0x2301,
+		0x2103,
+		0x1230,
+		0x1320,
+		0x3120,
+		0x3210,
+		0x2310,
+		0x2130,
+	};
+
+	u16 lane_cfg = 0;
+	int i;
+	unsigned lane_cfg_val;
+	u16 pol_val = 0;
+
+	for (i = 0; i < 4; ++i)
+		lane_cfg |= phy->lane_function[i] << ((3 - i) * 4);
+
+	pol_val |= phy->lane_polarity[0] << 0;
+	pol_val |= phy->lane_polarity[1] << 3;
+	pol_val |= phy->lane_polarity[2] << 2;
+	pol_val |= phy->lane_polarity[3] << 1;
+
+	for (i = 0; i < ARRAY_SIZE(pad_cfg_list); ++i)
+		if (pad_cfg_list[i] == lane_cfg)
+			break;
+
+	if (WARN_ON(i == ARRAY_SIZE(pad_cfg_list)))
+		i = 0;
+
+	lane_cfg_val = i;
+
+	REG_FLD_MOD(phy->base, HDMI_TXPHY_PAD_CFG_CTRL, lane_cfg_val, 26, 22);
+	REG_FLD_MOD(phy->base, HDMI_TXPHY_PAD_CFG_CTRL, pol_val, 30, 27);
+}
 
-	r = hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_LDOON);
-	if (r)
-		return r;
+int hdmi_phy_configure(struct hdmi_phy_data *phy, struct hdmi_config *cfg)
+{
+	u8 freqout;
 
 	/*
 	 * Read address 0 in order to get the SCP reset done completed
@@ -81,79 +143,112 @@ int hdmi_phy_enable(struct hdmi_phy_data *phy, struct hdmi_wp_data *wp,
 	hdmi_read_reg(phy->base, HDMI_TXPHY_TX_CTRL);
 
 	/*
+	 * In OMAP5+, the HFBITCLK must be divided by 2 before issuing the
+	 * HDMI_PHYPWRCMD_LDOON command.
+	*/
+	if (phy_feat->bist_ctrl)
+		REG_FLD_MOD(phy->base, HDMI_TXPHY_BIST_CONTROL, 1, 11, 11);
+
+	if (phy_feat->calc_freqout) {
+		/* DCOCLK/10 is pixel clock, compare pclk with DCOCLK_MIN/10 */
+		u32 dco_min = phy_feat->dcofreq_min / 10;
+		u32 pclk = cfg->timings.pixelclock;
+
+		if (pclk < dco_min)
+			freqout = 0;
+		else if ((pclk >= dco_min) && (pclk < phy_feat->max_phy))
+			freqout = 1;
+		else
+			freqout = 2;
+	} else {
+		freqout = 1;
+	}
+
+	/*
 	 * Write to phy address 0 to configure the clock
 	 * use HFBITCLK write HDMI_TXPHY_TX_CONTROL_FREQOUT field
 	 */
-	REG_FLD_MOD(phy->base, HDMI_TXPHY_TX_CTRL, 0x1, 31, 30);
+	REG_FLD_MOD(phy->base, HDMI_TXPHY_TX_CTRL, freqout, 31, 30);
 
 	/* Write to phy address 1 to start HDMI line (TXVALID and TMDSCLKEN) */
 	hdmi_write_reg(phy->base, HDMI_TXPHY_DIGITAL_CTRL, 0xF0000000);
 
 	/* Setup max LDO voltage */
-	REG_FLD_MOD(phy->base, HDMI_TXPHY_POWER_CTRL, 0xB, 3, 0);
+	if (phy_feat->ldo_voltage)
+		REG_FLD_MOD(phy->base, HDMI_TXPHY_POWER_CTRL, 0xB, 3, 0);
 
-	/* Write to phy address 3 to change the polarity control */
-	REG_FLD_MOD(phy->base, HDMI_TXPHY_PAD_CFG_CTRL, 0x1, 27, 27);
-
-	r = request_threaded_irq(phy->irq, NULL, hdmi_irq_handler,
-				IRQF_ONESHOT, "OMAP HDMI", wp);
-	if (r) {
-		DSSERR("HDMI IRQ request failed\n");
-		hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_OFF);
-		return r;
-	}
-
-	hdmi_wp_set_irqenable(wp,
-		HDMI_IRQ_LINK_CONNECT | HDMI_IRQ_LINK_DISCONNECT);
+	hdmi_phy_configure_lanes(phy);
 
 	return 0;
 }
 
-void hdmi_phy_disable(struct hdmi_phy_data *phy, struct hdmi_wp_data *wp)
+static const struct hdmi_phy_features omap44xx_phy_feats = {
+	.bist_ctrl	=	false,
+	.calc_freqout	=	false,
+	.ldo_voltage	=	true,
+	.dcofreq_min	=	500000000,
+	.max_phy	=	185675000,
+};
+
+static const struct hdmi_phy_features omap54xx_phy_feats = {
+	.bist_ctrl	=	true,
+	.calc_freqout	=	true,
+	.ldo_voltage	=	false,
+	.dcofreq_min	=	750000000,
+	.max_phy	=	186000000,
+};
+
+static int hdmi_phy_init_features(struct platform_device *pdev)
 {
-	free_irq(phy->irq, wp);
+	struct hdmi_phy_features *dst;
+	const struct hdmi_phy_features *src;
 
-	hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_OFF);
-}
+	dst = devm_kzalloc(&pdev->dev, sizeof(*dst), GFP_KERNEL);
+	if (!dst) {
+		dev_err(&pdev->dev, "Failed to allocate HDMI PHY Features\n");
+		return -ENOMEM;
+	}
+
+	switch (omapdss_get_version()) {
+	case OMAPDSS_VER_OMAP4430_ES1:
+	case OMAPDSS_VER_OMAP4430_ES2:
+	case OMAPDSS_VER_OMAP4:
+		src = &omap44xx_phy_feats;
+		break;
 
-#define PHY_OFFSET	0x300
-#define PHY_SIZE	0x100
+	case OMAPDSS_VER_OMAP5:
+		src = &omap54xx_phy_feats;
+		break;
+
+	default:
+		return -ENODEV;
+	}
+
+	memcpy(dst, src, sizeof(*dst));
+	phy_feat = dst;
+
+	return 0;
+}
 
 int hdmi_phy_init(struct platform_device *pdev, struct hdmi_phy_data *phy)
 {
+	int r;
 	struct resource *res;
-	struct resource temp_res;
+
+	r = hdmi_phy_init_features(pdev);
+	if (r)
+		return r;
 
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "phy");
 	if (!res) {
-		DSSDBG("can't get PHY mem resource by name\n");
-		/*
-		 * if hwmod/DT doesn't have the memory resource information
-		 * split into HDMI sub blocks by name, we try again by getting
-		 * the platform's first resource. this code will be removed when
-		 * the driver can get the mem resources by name
-		 */
-		res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-		if (!res) {
-			DSSERR("can't get PHY mem resource\n");
-			return -EINVAL;
-		}
-
-		temp_res.start = res->start + PHY_OFFSET;
-		temp_res.end = temp_res.start + PHY_SIZE - 1;
-		res = &temp_res;
+		DSSERR("can't get PHY mem resource\n");
+		return -EINVAL;
 	}
 
-	phy->base = devm_ioremap(&pdev->dev, res->start, resource_size(res));
-	if (!phy->base) {
+	phy->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(phy->base)) {
 		DSSERR("can't ioremap TX PHY\n");
-		return -ENOMEM;
-	}
-
-	phy->irq = platform_get_irq(pdev, 0);
-	if (phy->irq < 0) {
-		DSSERR("platform_get_irq failed\n");
-		return -ENODEV;
+		return PTR_ERR(phy->base);
 	}
 
 	return 0;
diff --git a/drivers/video/fbdev/omap2/dss/hdmi_pll.c b/drivers/video/fbdev/omap2/dss/hdmi_pll.c
index 5fc71215c30..54df12a8d74 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi_pll.c
+++ b/drivers/video/fbdev/omap2/dss/hdmi_pll.c
@@ -23,6 +23,18 @@
 #define HDMI_DEFAULT_REGN 16
 #define HDMI_DEFAULT_REGM2 1
 
+struct hdmi_pll_features {
+	bool sys_reset;
+	/* this is a hack, need to replace it with a better computation of M2 */
+	bool bound_dcofreq;
+	unsigned long fint_min, fint_max;
+	u16 regm_max;
+	unsigned long dcofreq_low_min, dcofreq_low_max;
+	unsigned long dcofreq_high_min, dcofreq_high_max;
+};
+
+static const struct hdmi_pll_features *pll_feat;
+
 void hdmi_pll_dump(struct hdmi_pll_data *pll, struct seq_file *s)
 {
 #define DUMPPLL(r) seq_printf(s, "%-35s %08x\n", #r,\
@@ -57,7 +69,11 @@ void hdmi_pll_compute(struct hdmi_pll_data *pll, unsigned long clkin, int phy)
 
 	refclk = clkin / pi->regn;
 
-	pi->regm2 = HDMI_DEFAULT_REGM2;
+	/* temorary hack to make sure DCO freq isn't calculated too low */
+	if (pll_feat->bound_dcofreq && phy <= 65000)
+		pi->regm2 = 3;
+	else
+		pi->regm2 = HDMI_DEFAULT_REGM2;
 
 	/*
 	 * multiplier is pixel_clk/ref_clk
@@ -154,7 +170,7 @@ static int hdmi_pll_config(struct hdmi_pll_data *pll)
 static int hdmi_pll_reset(struct hdmi_pll_data *pll)
 {
 	/* SYSRESET  controlled by power FSM */
-	REG_FLD_MOD(pll->base, PLLCTRL_PLL_CONTROL, 0x0, 3, 3);
+	REG_FLD_MOD(pll->base, PLLCTRL_PLL_CONTROL, pll_feat->sys_reset, 3, 3);
 
 	/* READ 0x0 reset is in progress */
 	if (hdmi_wait_for_bit_change(pll->base, PLLCTRL_PLL_STATUS, 0, 0, 1)
@@ -194,38 +210,81 @@ void hdmi_pll_disable(struct hdmi_pll_data *pll, struct hdmi_wp_data *wp)
 	hdmi_wp_set_pll_pwr(wp, HDMI_PLLPWRCMD_ALLOFF);
 }
 
-#define PLL_OFFSET	0x200
-#define PLL_SIZE	0x100
+static const struct hdmi_pll_features omap44xx_pll_feats = {
+	.sys_reset		=	false,
+	.bound_dcofreq		=	false,
+	.fint_min		=	500000,
+	.fint_max		=	2500000,
+	.regm_max		=	4095,
+	.dcofreq_low_min	=	500000000,
+	.dcofreq_low_max	=	1000000000,
+	.dcofreq_high_min	=	1000000000,
+	.dcofreq_high_max	=	2000000000,
+};
+
+static const struct hdmi_pll_features omap54xx_pll_feats = {
+	.sys_reset		=	true,
+	.bound_dcofreq		=	true,
+	.fint_min		=	620000,
+	.fint_max		=	2500000,
+	.regm_max		=	2046,
+	.dcofreq_low_min	=	750000000,
+	.dcofreq_low_max	=	1500000000,
+	.dcofreq_high_min	=	1250000000,
+	.dcofreq_high_max	=	2500000000UL,
+};
+
+static int hdmi_pll_init_features(struct platform_device *pdev)
+{
+	struct hdmi_pll_features *dst;
+	const struct hdmi_pll_features *src;
+
+	dst = devm_kzalloc(&pdev->dev, sizeof(*dst), GFP_KERNEL);
+	if (!dst) {
+		dev_err(&pdev->dev, "Failed to allocate HDMI PHY Features\n");
+		return -ENOMEM;
+	}
+
+	switch (omapdss_get_version()) {
+	case OMAPDSS_VER_OMAP4430_ES1:
+	case OMAPDSS_VER_OMAP4430_ES2:
+	case OMAPDSS_VER_OMAP4:
+		src = &omap44xx_pll_feats;
+		break;
+
+	case OMAPDSS_VER_OMAP5:
+		src = &omap54xx_pll_feats;
+		break;
+
+	default:
+		return -ENODEV;
+	}
+
+	memcpy(dst, src, sizeof(*dst));
+	pll_feat = dst;
+
+	return 0;
+}
 
 int hdmi_pll_init(struct platform_device *pdev, struct hdmi_pll_data *pll)
 {
+	int r;
 	struct resource *res;
-	struct resource temp_res;
+
+	r = hdmi_pll_init_features(pdev);
+	if (r)
+		return r;
 
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "pll");
 	if (!res) {
-		DSSDBG("can't get PLL mem resource by name\n");
-		/*
-		 * if hwmod/DT doesn't have the memory resource information
-		 * split into HDMI sub blocks by name, we try again by getting
-		 * the platform's first resource. this code will be removed when
-		 * the driver can get the mem resources by name
-		 */
-		res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-		if (!res) {
-			DSSERR("can't get PLL mem resource\n");
-			return -EINVAL;
-		}
-
-		temp_res.start = res->start + PLL_OFFSET;
-		temp_res.end = temp_res.start + PLL_SIZE - 1;
-		res = &temp_res;
+		DSSERR("can't get PLL mem resource\n");
+		return -EINVAL;
 	}
 
-	pll->base = devm_ioremap(&pdev->dev, res->start, resource_size(res));
-	if (!pll->base) {
+	pll->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(pll->base)) {
 		DSSERR("can't ioremap PLLCTRL\n");
-		return -ENOMEM;
+		return PTR_ERR(pll->base);
 	}
 
 	return 0;
diff --git a/drivers/video/fbdev/omap2/dss/hdmi_wp.c b/drivers/video/fbdev/omap2/dss/hdmi_wp.c
index f5f4ccf50d9..496327e2b21 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi_wp.c
+++ b/drivers/video/fbdev/omap2/dss/hdmi_wp.c
@@ -185,7 +185,7 @@ void hdmi_wp_init_vid_fmt_timings(struct hdmi_video_format *video_fmt,
 	timings->interlace = param->timings.interlace;
 }
 
-#if defined(CONFIG_OMAP4_DSS_HDMI_AUDIO)
+#if defined(CONFIG_OMAP4_DSS_HDMI_AUDIO) || defined(CONFIG_OMAP5_DSS_HDMI_AUDIO)
 void hdmi_wp_audio_config_format(struct hdmi_wp_data *wp,
 		struct hdmi_audio_format *aud_fmt)
 {
@@ -238,37 +238,20 @@ int hdmi_wp_audio_core_req_enable(struct hdmi_wp_data *wp, bool enable)
 }
 #endif
 
-#define WP_SIZE	0x200
-
 int hdmi_wp_init(struct platform_device *pdev, struct hdmi_wp_data *wp)
 {
 	struct resource *res;
-	struct resource temp_res;
 
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "wp");
 	if (!res) {
-		DSSDBG("can't get WP mem resource by name\n");
-		/*
-		 * if hwmod/DT doesn't have the memory resource information
-		 * split into HDMI sub blocks by name, we try again by getting
-		 * the platform's first resource. this code will be removed when
-		 * the driver can get the mem resources by name
-		 */
-		res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-		if (!res) {
-			DSSERR("can't get WP mem resource\n");
-			return -EINVAL;
-		}
-
-		temp_res.start = res->start;
-		temp_res.end = temp_res.start + WP_SIZE - 1;
-		res = &temp_res;
+		DSSERR("can't get WP mem resource\n");
+		return -EINVAL;
 	}
 
-	wp->base = devm_ioremap(&pdev->dev, res->start, resource_size(res));
-	if (!wp->base) {
+	wp->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(wp->base)) {
 		DSSERR("can't ioremap HDMI WP\n");
-		return -ENOMEM;
+		return PTR_ERR(wp->base);
 	}
 
 	return 0;
diff --git a/drivers/video/fbdev/omap2/dss/omapdss-boot-init.c b/drivers/video/fbdev/omap2/dss/omapdss-boot-init.c
new file mode 100644
index 00000000000..99af9e88b2d
--- /dev/null
+++ b/drivers/video/fbdev/omap2/dss/omapdss-boot-init.c
@@ -0,0 +1,229 @@
+/*
+ * Copyright (C) 2014 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ * As omapdss panel drivers are omapdss specific, but we want to define the
+ * DT-data in generic manner, we convert the compatible strings of the panel and
+ * encoder nodes from "panel-foo" to "omapdss,panel-foo". This way we can have
+ * both correct DT data and omapdss specific drivers.
+ *
+ * When we get generic panel drivers to the kernel, this file will be removed.
+ */
+
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/of_graph.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+
+static struct list_head dss_conv_list __initdata;
+
+static const char prefix[] __initconst = "omapdss,";
+
+struct dss_conv_node {
+	struct list_head list;
+	struct device_node *node;
+	bool root;
+};
+
+static int __init omapdss_count_strings(const struct property *prop)
+{
+	const char *p = prop->value;
+	int l = 0, total = 0;
+	int i;
+
+	for (i = 0; total < prop->length; total += l, p += l, i++)
+		l = strlen(p) + 1;
+
+	return i;
+}
+
+static void __init omapdss_update_prop(struct device_node *node, char *compat,
+	int len)
+{
+	struct property *prop;
+
+	prop = kzalloc(sizeof(*prop), GFP_KERNEL);
+	if (!prop)
+		return;
+
+	prop->name = "compatible";
+	prop->value = compat;
+	prop->length = len;
+
+	of_update_property(node, prop);
+}
+
+static void __init omapdss_prefix_strcpy(char *dst, int dst_len,
+	const char *src, int src_len)
+{
+	size_t total = 0;
+
+	while (total < src_len) {
+		size_t l = strlen(src) + 1;
+
+		strcpy(dst, prefix);
+		dst += strlen(prefix);
+
+		strcpy(dst, src);
+		dst += l;
+
+		src += l;
+		total += l;
+	}
+}
+
+/* prepend compatible property strings with "omapdss," */
+static void __init omapdss_omapify_node(struct device_node *node)
+{
+	struct property *prop;
+	char *new_compat;
+	int num_strs;
+	int new_len;
+
+	prop = of_find_property(node, "compatible", NULL);
+
+	if (!prop || !prop->value)
+		return;
+
+	if (strnlen(prop->value, prop->length) >= prop->length)
+		return;
+
+	/* is it already prefixed? */
+	if (strncmp(prefix, prop->value, strlen(prefix)) == 0)
+		return;
+
+	num_strs = omapdss_count_strings(prop);
+
+	new_len = prop->length + strlen(prefix) * num_strs;
+	new_compat = kmalloc(new_len, GFP_KERNEL);
+
+	omapdss_prefix_strcpy(new_compat, new_len, prop->value, prop->length);
+
+	omapdss_update_prop(node, new_compat, new_len);
+}
+
+static void __init omapdss_add_to_list(struct device_node *node, bool root)
+{
+	struct dss_conv_node *n = kmalloc(sizeof(struct dss_conv_node),
+		GFP_KERNEL);
+	n->node = node;
+	n->root = root;
+	list_add(&n->list, &dss_conv_list);
+}
+
+static bool __init omapdss_list_contains(const struct device_node *node)
+{
+	struct dss_conv_node *n;
+
+	list_for_each_entry(n, &dss_conv_list, list) {
+		if (n->node == node)
+			return true;
+	}
+
+	return false;
+}
+
+static void __init omapdss_walk_device(struct device_node *node, bool root)
+{
+	struct device_node *n;
+
+	omapdss_add_to_list(node, root);
+
+	/*
+	 * of_graph_get_remote_port_parent() prints an error if there is no
+	 * port/ports node. To avoid that, check first that there's the node.
+	 */
+	n = of_get_child_by_name(node, "ports");
+	if (!n)
+		n = of_get_child_by_name(node, "port");
+	if (!n)
+		return;
+
+	of_node_put(n);
+
+	n = NULL;
+	while ((n = of_graph_get_next_endpoint(node, n)) != NULL) {
+		struct device_node *pn;
+
+		pn = of_graph_get_remote_port_parent(n);
+
+		if (!pn) {
+			of_node_put(n);
+			continue;
+		}
+
+		if (!of_device_is_available(pn) || omapdss_list_contains(pn)) {
+			of_node_put(pn);
+			of_node_put(n);
+			continue;
+		}
+
+		omapdss_walk_device(pn, false);
+
+		of_node_put(n);
+	}
+}
+
+static const struct of_device_id omapdss_of_match[] __initconst = {
+	{ .compatible = "ti,omap2-dss", },
+	{ .compatible = "ti,omap3-dss", },
+	{ .compatible = "ti,omap4-dss", },
+	{ .compatible = "ti,omap5-dss", },
+	{},
+};
+
+static int __init omapdss_boot_init(void)
+{
+	struct device_node *dss, *child;
+
+	INIT_LIST_HEAD(&dss_conv_list);
+
+	dss = of_find_matching_node(NULL, omapdss_of_match);
+
+	if (dss == NULL || !of_device_is_available(dss))
+		return 0;
+
+	omapdss_walk_device(dss, true);
+
+	for_each_available_child_of_node(dss, child) {
+		if (!of_find_property(child, "compatible", NULL)) {
+			of_node_put(child);
+			continue;
+		}
+
+		omapdss_walk_device(child, true);
+	}
+
+	while (!list_empty(&dss_conv_list)) {
+		struct dss_conv_node *n;
+
+		n = list_first_entry(&dss_conv_list, struct dss_conv_node,
+			list);
+
+		if (!n->root)
+			omapdss_omapify_node(n->node);
+
+		list_del(&n->list);
+		of_node_put(n->node);
+		kfree(n);
+	}
+
+	return 0;
+}
+
+subsys_initcall(omapdss_boot_init);
diff --git a/drivers/video/fbdev/omap2/dss/venc_panel.c b/drivers/video/fbdev/omap2/dss/venc_panel.c
deleted file mode 100644
index af68cd444d7..00000000000
--- a/drivers/video/fbdev/omap2/dss/venc_panel.c
+++ /dev/null
@@ -1,232 +0,0 @@
-/*
- * Copyright (C) 2009 Nokia Corporation
- * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
- *
- * VENC panel 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.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <linux/kernel.h>
-#include <linux/err.h>
-#include <linux/io.h>
-#include <linux/mutex.h>
-#include <linux/module.h>
-
-#include <video/omapdss.h>
-
-#include "dss.h"
-
-static struct {
-	struct mutex lock;
-} venc_panel;
-
-static ssize_t display_output_type_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
-{
-	struct omap_dss_device *dssdev = to_dss_device(dev);
-	const char *ret;
-
-	switch (dssdev->phy.venc.type) {
-	case OMAP_DSS_VENC_TYPE_COMPOSITE:
-		ret = "composite";
-		break;
-	case OMAP_DSS_VENC_TYPE_SVIDEO:
-		ret = "svideo";
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	return snprintf(buf, PAGE_SIZE, "%s\n", ret);
-}
-
-static ssize_t display_output_type_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t size)
-{
-	struct omap_dss_device *dssdev = to_dss_device(dev);
-	enum omap_dss_venc_type new_type;
-
-	if (sysfs_streq("composite", buf))
-		new_type = OMAP_DSS_VENC_TYPE_COMPOSITE;
-	else if (sysfs_streq("svideo", buf))
-		new_type = OMAP_DSS_VENC_TYPE_SVIDEO;
-	else
-		return -EINVAL;
-
-	mutex_lock(&venc_panel.lock);
-
-	if (dssdev->phy.venc.type != new_type) {
-		dssdev->phy.venc.type = new_type;
-		omapdss_venc_set_type(dssdev, new_type);
-		if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE) {
-			omapdss_venc_display_disable(dssdev);
-			omapdss_venc_display_enable(dssdev);
-		}
-	}
-
-	mutex_unlock(&venc_panel.lock);
-
-	return size;
-}
-
-static DEVICE_ATTR(output_type, S_IRUGO | S_IWUSR,
-		display_output_type_show, display_output_type_store);
-
-static int venc_panel_probe(struct omap_dss_device *dssdev)
-{
-	/* set default timings to PAL */
-	const struct omap_video_timings default_timings = {
-		.x_res		= 720,
-		.y_res		= 574,
-		.pixelclock	= 13500000,
-		.hsw		= 64,
-		.hfp		= 12,
-		.hbp		= 68,
-		.vsw		= 5,
-		.vfp		= 5,
-		.vbp		= 41,
-
-		.vsync_level	= OMAPDSS_SIG_ACTIVE_HIGH,
-		.hsync_level	= OMAPDSS_SIG_ACTIVE_HIGH,
-
-		.interlace	= true,
-	};
-
-	mutex_init(&venc_panel.lock);
-
-	dssdev->panel.timings = default_timings;
-
-	return device_create_file(dssdev->dev, &dev_attr_output_type);
-}
-
-static void venc_panel_remove(struct omap_dss_device *dssdev)
-{
-	device_remove_file(dssdev->dev, &dev_attr_output_type);
-}
-
-static int venc_panel_enable(struct omap_dss_device *dssdev)
-{
-	int r;
-
-	dev_dbg(dssdev->dev, "venc_panel_enable\n");
-
-	mutex_lock(&venc_panel.lock);
-
-	if (dssdev->state != OMAP_DSS_DISPLAY_DISABLED) {
-		r = -EINVAL;
-		goto err;
-	}
-
-	omapdss_venc_set_timings(dssdev, &dssdev->panel.timings);
-	omapdss_venc_set_type(dssdev, dssdev->phy.venc.type);
-	omapdss_venc_invert_vid_out_polarity(dssdev,
-		dssdev->phy.venc.invert_polarity);
-
-	r = omapdss_venc_display_enable(dssdev);
-	if (r)
-		goto err;
-
-	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
-
-	mutex_unlock(&venc_panel.lock);
-
-	return 0;
-err:
-	mutex_unlock(&venc_panel.lock);
-
-	return r;
-}
-
-static void venc_panel_disable(struct omap_dss_device *dssdev)
-{
-	dev_dbg(dssdev->dev, "venc_panel_disable\n");
-
-	mutex_lock(&venc_panel.lock);
-
-	if (dssdev->state == OMAP_DSS_DISPLAY_DISABLED)
-		goto end;
-
-	omapdss_venc_display_disable(dssdev);
-
-	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
-end:
-	mutex_unlock(&venc_panel.lock);
-}
-
-static void venc_panel_set_timings(struct omap_dss_device *dssdev,
-		struct omap_video_timings *timings)
-{
-	dev_dbg(dssdev->dev, "venc_panel_set_timings\n");
-
-	mutex_lock(&venc_panel.lock);
-
-	omapdss_venc_set_timings(dssdev, timings);
-	dssdev->panel.timings = *timings;
-
-	mutex_unlock(&venc_panel.lock);
-}
-
-static int venc_panel_check_timings(struct omap_dss_device *dssdev,
-		struct omap_video_timings *timings)
-{
-	dev_dbg(dssdev->dev, "venc_panel_check_timings\n");
-
-	return omapdss_venc_check_timings(dssdev, timings);
-}
-
-static u32 venc_panel_get_wss(struct omap_dss_device *dssdev)
-{
-	dev_dbg(dssdev->dev, "venc_panel_get_wss\n");
-
-	return omapdss_venc_get_wss(dssdev);
-}
-
-static int venc_panel_set_wss(struct omap_dss_device *dssdev, u32 wss)
-{
-	dev_dbg(dssdev->dev, "venc_panel_set_wss\n");
-
-	return omapdss_venc_set_wss(dssdev, wss);
-}
-
-static struct omap_dss_driver venc_driver = {
-	.probe		= venc_panel_probe,
-	.remove		= venc_panel_remove,
-
-	.enable		= venc_panel_enable,
-	.disable	= venc_panel_disable,
-
-	.get_resolution	= omapdss_default_get_resolution,
-	.get_recommended_bpp = omapdss_default_get_recommended_bpp,
-
-	.set_timings	= venc_panel_set_timings,
-	.check_timings	= venc_panel_check_timings,
-
-	.get_wss	= venc_panel_get_wss,
-	.set_wss	= venc_panel_set_wss,
-
-	.driver         = {
-		.name   = "venc",
-		.owner  = THIS_MODULE,
-	},
-};
-
-int venc_panel_init(void)
-{
-	return omap_dss_register_driver(&venc_driver);
-}
-
-void venc_panel_exit(void)
-{
-	omap_dss_unregister_driver(&venc_driver);
-}
diff --git a/drivers/video/fbdev/pxa3xx-gcu.c b/drivers/video/fbdev/pxa3xx-gcu.c
index 417f9a27eb7..4df3657fe22 100644
--- a/drivers/video/fbdev/pxa3xx-gcu.c
+++ b/drivers/video/fbdev/pxa3xx-gcu.c
@@ -612,11 +612,9 @@ static int pxa3xx_gcu_probe(struct platform_device *pdev)
 
 	/* handle IO resources */
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	priv->mmio_base = devm_request_and_ioremap(dev, r);
-	if (IS_ERR(priv->mmio_base)) {
-		dev_err(dev, "failed to map I/O memory\n");
+	priv->mmio_base = devm_ioremap_resource(dev, r);
+	if (IS_ERR(priv->mmio_base))
 		return PTR_ERR(priv->mmio_base);
-	}
 
 	/* enable the clock */
 	priv->clk = devm_clk_get(dev, NULL);
diff --git a/drivers/video/fbdev/s3fb.c b/drivers/video/fbdev/s3fb.c
index 9a3f8f1c6aa..c43b969e1e2 100644
--- a/drivers/video/fbdev/s3fb.c
+++ b/drivers/video/fbdev/s3fb.c
@@ -1401,9 +1401,10 @@ err_enable_device:
 static void s3_pci_remove(struct pci_dev *dev)
 {
 	struct fb_info *info = pci_get_drvdata(dev);
-	struct s3fb_info __maybe_unused *par = info->par;
+	struct s3fb_info __maybe_unused *par;
 
 	if (info) {
+		par = info->par;
 
 #ifdef CONFIG_MTRR
 		if (par->mtrr_reg >= 0) {
diff --git a/drivers/video/fbdev/wm8505fb.c b/drivers/video/fbdev/wm8505fb.c
index 537d199612a..d2fafbbcd7f 100644
--- a/drivers/video/fbdev/wm8505fb.c
+++ b/drivers/video/fbdev/wm8505fb.c
@@ -162,7 +162,7 @@ static ssize_t contrast_show(struct device *dev,
 	struct fb_info *info = dev_get_drvdata(dev);
 	struct wm8505fb_info *fbi = to_wm8505fb_info(info);
 
-	return sprintf(buf, "%d\n", fbi->contrast);
+	return sprintf(buf, "%u\n", fbi->contrast);
 }
 
 static ssize_t contrast_store(struct device *dev,
diff --git a/drivers/video/of_display_timing.c b/drivers/video/of_display_timing.c
index ba5b40f581f..987edf11003 100644
--- a/drivers/video/of_display_timing.c
+++ b/drivers/video/of_display_timing.c
@@ -115,10 +115,8 @@ int of_get_display_timing(struct device_node *np, const char *name,
 {
 	struct device_node *timing_np;
 
-	if (!np) {
-		pr_err("%s: no devicenode given\n", of_node_full_name(np));
+	if (!np)
 		return -EINVAL;
-	}
 
 	timing_np = of_get_child_by_name(np, name);
 	if (!timing_np) {
@@ -142,10 +140,8 @@ struct display_timings *of_get_display_timings(struct device_node *np)
 	struct device_node *native_mode;
 	struct display_timings *disp;
 
-	if (!np) {
-		pr_err("%s: no device node given\n", of_node_full_name(np));
+	if (!np)
 		return NULL;
-	}
 
 	timings_np = of_get_child_by_name(np, "display-timings");
 	if (!timings_np) {
@@ -164,7 +160,7 @@ struct display_timings *of_get_display_timings(struct device_node *np)
 	entry = of_parse_phandle(timings_np, "native-mode", 0);
 	/* assume first child as native mode if none provided */
 	if (!entry)
-		entry = of_get_next_child(np, NULL);
+		entry = of_get_next_child(timings_np, NULL);
 	/* if there is no child, it is useless to go on */
 	if (!entry) {
 		pr_err("%s: no timing specifications given\n",
diff --git a/drivers/w1/w1.c b/drivers/w1/w1.c
index ff52618cafb..5d734152054 100644
--- a/drivers/w1/w1.c
+++ b/drivers/w1/w1.c
@@ -1078,6 +1078,8 @@ static void w1_search_process(struct w1_master *dev, u8 search_type)
  * w1_process_callbacks() - execute each dev->async_list callback entry
  * @dev: w1_master device
  *
+ * The w1 master list_mutex must be held.
+ *
  * Return: 1 if there were commands to executed 0 otherwise
  */
 int w1_process_callbacks(struct w1_master *dev)
diff --git a/drivers/w1/w1.h b/drivers/w1/w1.h
index 734dab7fc68..56a49ba41d8 100644
--- a/drivers/w1/w1.h
+++ b/drivers/w1/w1.h
@@ -203,7 +203,6 @@ enum w1_master_flags {
  * @search_id:		allows continuing a search
  * @refcnt:		reference count
  * @priv:		private data storage
- * @priv_size:		size allocated
  * @enable_pullup:	allows a strong pullup
  * @pullup_duration:	time for the next strong pullup
  * @flags:		one of w1_master_flags
@@ -214,7 +213,6 @@ enum w1_master_flags {
  * @dev:		sysfs device
  * @bus_master:		io operations available
  * @seq:		sequence number used for netlink broadcasts
- * @portid:		destination for the current netlink command
  */
 struct w1_master
 {
@@ -241,7 +239,6 @@ struct w1_master
 	atomic_t		refcnt;
 
 	void			*priv;
-	int			priv_size;
 
 	/** 5V strong pullup enabled flag, 1 enabled, zero disabled. */
 	int			enable_pullup;
@@ -260,11 +257,6 @@ struct w1_master
 	struct w1_bus_master	*bus_master;
 
 	u32			seq;
-	/* port id to send netlink responses to.  The value is temporarily
-	 * stored here while processing a message, set after locking the
-	 * mutex, zero before unlocking the mutex.
-	 */
-	u32			portid;
 };
 
 /**
diff --git a/drivers/w1/w1_family.c b/drivers/w1/w1_family.c
index 3bff6b37b47..3651ec801f4 100644
--- a/drivers/w1/w1_family.c
+++ b/drivers/w1/w1_family.c
@@ -139,9 +139,9 @@ void w1_family_get(struct w1_family *f)
 
 void __w1_family_get(struct w1_family *f)
 {
-	smp_mb__before_atomic_inc();
+	smp_mb__before_atomic();
 	atomic_inc(&f->refcnt);
-	smp_mb__after_atomic_inc();
+	smp_mb__after_atomic();
 }
 
 EXPORT_SYMBOL(w1_unregister_family);
diff --git a/drivers/w1/w1_int.c b/drivers/w1/w1_int.c
index 9b084db739c..728039d2efe 100644
--- a/drivers/w1/w1_int.c
+++ b/drivers/w1/w1_int.c
@@ -219,9 +219,13 @@ void __w1_remove_master_device(struct w1_master *dev)
 
 		if (msleep_interruptible(1000))
 			flush_signals(current);
+		mutex_lock(&dev->list_mutex);
 		w1_process_callbacks(dev);
+		mutex_unlock(&dev->list_mutex);
 	}
+	mutex_lock(&dev->list_mutex);
 	w1_process_callbacks(dev);
+	mutex_unlock(&dev->list_mutex);
 
 	memset(&msg, 0, sizeof(msg));
 	msg.id.mst.id = dev->id;
diff --git a/drivers/w1/w1_netlink.c b/drivers/w1/w1_netlink.c
index a02704a5932..351a2978ba7 100644
--- a/drivers/w1/w1_netlink.c
+++ b/drivers/w1/w1_netlink.c
@@ -29,51 +29,247 @@
 #include "w1_netlink.h"
 
 #if defined(CONFIG_W1_CON) && (defined(CONFIG_CONNECTOR) || (defined(CONFIG_CONNECTOR_MODULE) && defined(CONFIG_W1_MODULE)))
-void w1_netlink_send(struct w1_master *dev, struct w1_netlink_msg *msg)
+
+#define MIN(a, b)                   (((a) < (b)) ? (a) : (b))
+
+/* Bundle together everything required to process a request in one memory
+ * allocation.
+ */
+struct w1_cb_block {
+	atomic_t refcnt;
+	u32 portid; /* Sending process port ID */
+	/* maximum value for first_cn->len */
+	u16 maxlen;
+	/* pointers to building up the reply message */
+	struct cn_msg *first_cn; /* fixed once the structure is populated */
+	struct cn_msg *cn; /* advances as cn_msg is appeneded */
+	struct w1_netlink_msg *msg; /* advances as w1_netlink_msg is appened */
+	struct w1_netlink_cmd *cmd; /* advances as cmds are appened */
+	struct w1_netlink_msg *cur_msg; /* currently message being processed */
+	/* copy of the original request follows */
+	struct cn_msg request_cn;
+	/* followed by variable length:
+	 * cn_msg, data (w1_netlink_msg and w1_netlink_cmd)
+	 * one or more struct w1_cb_node
+	 * reply first_cn, data (w1_netlink_msg and w1_netlink_cmd)
+	 */
+};
+struct w1_cb_node {
+	struct w1_async_cmd async;
+	/* pointers within w1_cb_block and cn data */
+	struct w1_cb_block *block;
+	struct w1_netlink_msg *msg;
+	struct w1_slave *sl;
+	struct w1_master *dev;
+};
+
+/**
+ * w1_reply_len() - calculate current reply length, compare to maxlen
+ * @block: block to calculate
+ *
+ * Calculates the current message length including possible multiple
+ * cn_msg and data, excludes the first sizeof(struct cn_msg).  Direclty
+ * compariable to maxlen and usable to send the message.
+ */
+static u16 w1_reply_len(struct w1_cb_block *block)
+{
+	if (!block->cn)
+		return 0;
+	return (u8 *)block->cn - (u8 *)block->first_cn + block->cn->len;
+}
+
+static void w1_unref_block(struct w1_cb_block *block)
+{
+	if (atomic_sub_return(1, &block->refcnt) == 0) {
+		u16 len = w1_reply_len(block);
+		if (len) {
+			cn_netlink_send_mult(block->first_cn, len,
+				block->portid, 0, GFP_KERNEL);
+		}
+		kfree(block);
+	}
+}
+
+/**
+ * w1_reply_make_space() - send message if needed to make space
+ * @block: block to make space on
+ * @space: how many bytes requested
+ *
+ * Verify there is enough room left for the caller to add "space" bytes to the
+ * message, if there isn't send the message and reset.
+ */
+static void w1_reply_make_space(struct w1_cb_block *block, u16 space)
+{
+	u16 len = w1_reply_len(block);
+	if (len + space >= block->maxlen) {
+		cn_netlink_send_mult(block->first_cn, len, block->portid, 0, GFP_KERNEL);
+		block->first_cn->len = 0;
+		block->cn = NULL;
+		block->msg = NULL;
+		block->cmd = NULL;
+	}
+}
+
+/* Early send when replies aren't bundled. */
+static void w1_netlink_check_send(struct w1_cb_block *block)
+{
+	if (!(block->request_cn.flags & W1_CN_BUNDLE) && block->cn)
+		w1_reply_make_space(block, block->maxlen);
+}
+
+/**
+ * w1_netlink_setup_msg() - prepare to write block->msg
+ * @block: block to operate on
+ * @ack: determines if cn can be reused
+ *
+ * block->cn will be setup with the correct ack, advancing if needed
+ * block->cn->len does not include space for block->msg
+ * block->msg advances but remains uninitialized
+ */
+static void w1_netlink_setup_msg(struct w1_cb_block *block, u32 ack)
+{
+	if (block->cn && block->cn->ack == ack) {
+		block->msg = (struct w1_netlink_msg *)(block->cn->data + block->cn->len);
+	} else {
+		/* advance or set to data */
+		if (block->cn)
+			block->cn = (struct cn_msg *)(block->cn->data +
+				block->cn->len);
+		else
+			block->cn = block->first_cn;
+
+		memcpy(block->cn, &block->request_cn, sizeof(*block->cn));
+		block->cn->len = 0;
+		block->cn->ack = ack;
+		block->msg = (struct w1_netlink_msg *)block->cn->data;
+	}
+}
+
+/* Append cmd to msg, include cmd->data as well.  This is because
+ * any following data goes with the command and in the case of a read is
+ * the results.
+ */
+static void w1_netlink_queue_cmd(struct w1_cb_block *block,
+	struct w1_netlink_cmd *cmd)
+{
+	u32 space;
+	w1_reply_make_space(block, sizeof(struct cn_msg) +
+		sizeof(struct w1_netlink_msg) + sizeof(*cmd) + cmd->len);
+
+	/* There's a status message sent after each command, so no point
+	 * in trying to bundle this cmd after an existing one, because
+	 * there won't be one.  Allocate and copy over a new cn_msg.
+	 */
+	w1_netlink_setup_msg(block, block->request_cn.seq + 1);
+	memcpy(block->msg, block->cur_msg, sizeof(*block->msg));
+	block->cn->len += sizeof(*block->msg);
+	block->msg->len = 0;
+	block->cmd = (struct w1_netlink_cmd *)(block->msg->data);
+
+	space = sizeof(*cmd) + cmd->len;
+	if (block->cmd != cmd)
+		memcpy(block->cmd, cmd, space);
+	block->cn->len += space;
+	block->msg->len += space;
+}
+
+/* Append req_msg and req_cmd, no other commands and no data from req_cmd are
+ * copied.
+ */
+static void w1_netlink_queue_status(struct w1_cb_block *block,
+	struct w1_netlink_msg *req_msg, struct w1_netlink_cmd *req_cmd,
+	int error)
 {
-	char buf[sizeof(struct cn_msg) + sizeof(struct w1_netlink_msg)];
-	struct cn_msg *m = (struct cn_msg *)buf;
-	struct w1_netlink_msg *w = (struct w1_netlink_msg *)(m+1);
+	u16 space = sizeof(struct cn_msg) + sizeof(*req_msg) + sizeof(*req_cmd);
+	w1_reply_make_space(block, space);
+	w1_netlink_setup_msg(block, block->request_cn.ack);
+
+	memcpy(block->msg, req_msg, sizeof(*req_msg));
+	block->cn->len += sizeof(*req_msg);
+	block->msg->len = 0;
+	block->msg->status = (u8)-error;
+	if (req_cmd) {
+		struct w1_netlink_cmd *cmd = (struct w1_netlink_cmd *)block->msg->data;
+		memcpy(cmd, req_cmd, sizeof(*cmd));
+		block->cn->len += sizeof(*cmd);
+		block->msg->len += sizeof(*cmd);
+		cmd->len = 0;
+	}
+	w1_netlink_check_send(block);
+}
 
-	memset(buf, 0, sizeof(buf));
+/**
+ * w1_netlink_send_error() - sends the error message now
+ * @cn: original cn_msg
+ * @msg: original w1_netlink_msg
+ * @portid: where to send it
+ * @error: error status
+ *
+ * Use when a block isn't available to queue the message to and cn, msg
+ * might not be contiguous.
+ */
+static void w1_netlink_send_error(struct cn_msg *cn, struct w1_netlink_msg *msg,
+	int portid, int error)
+{
+	struct {
+		struct cn_msg cn;
+		struct w1_netlink_msg msg;
+	} packet;
+	memcpy(&packet.cn, cn, sizeof(packet.cn));
+	memcpy(&packet.msg, msg, sizeof(packet.msg));
+	packet.cn.len = sizeof(packet.msg);
+	packet.msg.len = 0;
+	packet.msg.status = (u8)-error;
+	cn_netlink_send(&packet.cn, portid, 0, GFP_KERNEL);
+}
+
+/**
+ * w1_netlink_send() - sends w1 netlink notifications
+ * @dev: w1_master the even is associated with or for
+ * @msg: w1_netlink_msg message to be sent
+ *
+ * This are notifications generated from the kernel.
+ */
+void w1_netlink_send(struct w1_master *dev, struct w1_netlink_msg *msg)
+{
+	struct {
+		struct cn_msg cn;
+		struct w1_netlink_msg msg;
+	} packet;
+	memset(&packet, 0, sizeof(packet));
 
-	m->id.idx = CN_W1_IDX;
-	m->id.val = CN_W1_VAL;
+	packet.cn.id.idx = CN_W1_IDX;
+	packet.cn.id.val = CN_W1_VAL;
 
-	m->seq = dev->seq++;
-	m->len = sizeof(struct w1_netlink_msg);
+	packet.cn.seq = dev->seq++;
+	packet.cn.len = sizeof(*msg);
 
-	memcpy(w, msg, sizeof(struct w1_netlink_msg));
+	memcpy(&packet.msg, msg, sizeof(*msg));
+	packet.msg.len = 0;
 
-	cn_netlink_send(m, dev->portid, 0, GFP_KERNEL);
+	cn_netlink_send(&packet.cn, 0, 0, GFP_KERNEL);
 }
 
 static void w1_send_slave(struct w1_master *dev, u64 rn)
 {
-	struct cn_msg *msg = dev->priv;
-	struct w1_netlink_msg *hdr = (struct w1_netlink_msg *)(msg + 1);
-	struct w1_netlink_cmd *cmd = (struct w1_netlink_cmd *)(hdr + 1);
-	int avail;
+	struct w1_cb_block *block = dev->priv;
+	struct w1_netlink_cmd *cache_cmd = block->cmd;
 	u64 *data;
 
-	avail = dev->priv_size - cmd->len;
+	w1_reply_make_space(block, sizeof(*data));
 
-	if (avail < 8) {
-		msg->ack++;
-		cn_netlink_send(msg, dev->portid, 0, GFP_KERNEL);
-
-		msg->len = sizeof(struct w1_netlink_msg) +
-			sizeof(struct w1_netlink_cmd);
-		hdr->len = sizeof(struct w1_netlink_cmd);
-		cmd->len = 0;
+	/* Add cmd back if the packet was sent */
+	if (!block->cmd) {
+		cache_cmd->len = 0;
+		w1_netlink_queue_cmd(block, cache_cmd);
 	}
 
-	data = (void *)(cmd + 1) + cmd->len;
+	data = (u64 *)(block->cmd->data + block->cmd->len);
 
 	*data = rn;
-	cmd->len += 8;
-	hdr->len += 8;
-	msg->len += 8;
+	block->cn->len += sizeof(*data);
+	block->msg->len += sizeof(*data);
+	block->cmd->len += sizeof(*data);
 }
 
 static void w1_found_send_slave(struct w1_master *dev, u64 rn)
@@ -85,40 +281,15 @@ static void w1_found_send_slave(struct w1_master *dev, u64 rn)
 }
 
 /* Get the current slave list, or search (with or without alarm) */
-static int w1_get_slaves(struct w1_master *dev,
-		struct cn_msg *req_msg, struct w1_netlink_msg *req_hdr,
-		struct w1_netlink_cmd *req_cmd)
+static int w1_get_slaves(struct w1_master *dev, struct w1_netlink_cmd *req_cmd)
 {
-	struct cn_msg *msg;
-	struct w1_netlink_msg *hdr;
-	struct w1_netlink_cmd *cmd;
 	struct w1_slave *sl;
 
-	msg = kzalloc(PAGE_SIZE, GFP_KERNEL);
-	if (!msg)
-		return -ENOMEM;
-
-	msg->id = req_msg->id;
-	msg->seq = req_msg->seq;
-	msg->ack = 0;
-	msg->len = sizeof(struct w1_netlink_msg) +
-		sizeof(struct w1_netlink_cmd);
-
-	hdr = (struct w1_netlink_msg *)(msg + 1);
-	cmd = (struct w1_netlink_cmd *)(hdr + 1);
-
-	hdr->type = W1_MASTER_CMD;
-	hdr->id = req_hdr->id;
-	hdr->len = sizeof(struct w1_netlink_cmd);
-
-	cmd->cmd = req_cmd->cmd;
-	cmd->len = 0;
-
-	dev->priv = msg;
-	dev->priv_size = PAGE_SIZE - msg->len - sizeof(struct cn_msg);
+	req_cmd->len = 0;
+	w1_netlink_queue_cmd(dev->priv, req_cmd);
 
 	if (req_cmd->cmd == W1_CMD_LIST_SLAVES) {
-		__u64 rn;
+		u64 rn;
 		mutex_lock(&dev->list_mutex);
 		list_for_each_entry(sl, &dev->slist, w1_slave_entry) {
 			memcpy(&rn, &sl->reg_num, sizeof(rn));
@@ -126,73 +297,26 @@ static int w1_get_slaves(struct w1_master *dev,
 		}
 		mutex_unlock(&dev->list_mutex);
 	} else {
-		w1_search_process_cb(dev, cmd->cmd == W1_CMD_ALARM_SEARCH ?
+		w1_search_process_cb(dev, req_cmd->cmd == W1_CMD_ALARM_SEARCH ?
 			W1_ALARM_SEARCH : W1_SEARCH, w1_found_send_slave);
 	}
 
-	msg->ack = 0;
-	cn_netlink_send(msg, dev->portid, 0, GFP_KERNEL);
-
-	dev->priv = NULL;
-	dev->priv_size = 0;
-
-	kfree(msg);
-
 	return 0;
 }
 
-static int w1_send_read_reply(struct cn_msg *msg, struct w1_netlink_msg *hdr,
-		struct w1_netlink_cmd *cmd, u32 portid)
-{
-	void *data;
-	struct w1_netlink_msg *h;
-	struct w1_netlink_cmd *c;
-	struct cn_msg *cm;
-	int err;
-
-	data = kzalloc(sizeof(struct cn_msg) +
-			sizeof(struct w1_netlink_msg) +
-			sizeof(struct w1_netlink_cmd) +
-			cmd->len, GFP_KERNEL);
-	if (!data)
-		return -ENOMEM;
-
-	cm = (struct cn_msg *)(data);
-	h = (struct w1_netlink_msg *)(cm + 1);
-	c = (struct w1_netlink_cmd *)(h + 1);
-
-	memcpy(cm, msg, sizeof(struct cn_msg));
-	memcpy(h, hdr, sizeof(struct w1_netlink_msg));
-	memcpy(c, cmd, sizeof(struct w1_netlink_cmd));
-
-	cm->ack = msg->seq+1;
-	cm->len = sizeof(struct w1_netlink_msg) +
-		sizeof(struct w1_netlink_cmd) + cmd->len;
-
-	h->len = sizeof(struct w1_netlink_cmd) + cmd->len;
-
-	memcpy(c->data, cmd->data, c->len);
-
-	err = cn_netlink_send(cm, portid, 0, GFP_KERNEL);
-
-	kfree(data);
-
-	return err;
-}
-
-static int w1_process_command_io(struct w1_master *dev, struct cn_msg *msg,
-		struct w1_netlink_msg *hdr, struct w1_netlink_cmd *cmd)
+static int w1_process_command_io(struct w1_master *dev,
+	struct w1_netlink_cmd *cmd)
 {
 	int err = 0;
 
 	switch (cmd->cmd) {
 	case W1_CMD_TOUCH:
 		w1_touch_block(dev, cmd->data, cmd->len);
-		w1_send_read_reply(msg, hdr, cmd, dev->portid);
+		w1_netlink_queue_cmd(dev->priv, cmd);
 		break;
 	case W1_CMD_READ:
 		w1_read_block(dev, cmd->data, cmd->len);
-		w1_send_read_reply(msg, hdr, cmd, dev->portid);
+		w1_netlink_queue_cmd(dev->priv, cmd);
 		break;
 	case W1_CMD_WRITE:
 		w1_write_block(dev, cmd->data, cmd->len);
@@ -206,14 +330,13 @@ static int w1_process_command_io(struct w1_master *dev, struct cn_msg *msg,
 }
 
 static int w1_process_command_addremove(struct w1_master *dev,
-	struct cn_msg *msg, struct w1_netlink_msg *hdr,
 	struct w1_netlink_cmd *cmd)
 {
 	struct w1_slave *sl;
 	int err = 0;
 	struct w1_reg_num *id;
 
-	if (cmd->len != 8)
+	if (cmd->len != sizeof(*id))
 		return -EINVAL;
 
 	id = (struct w1_reg_num *)cmd->data;
@@ -241,7 +364,6 @@ static int w1_process_command_addremove(struct w1_master *dev,
 }
 
 static int w1_process_command_master(struct w1_master *dev,
-	struct cn_msg *req_msg, struct w1_netlink_msg *req_hdr,
 	struct w1_netlink_cmd *req_cmd)
 {
 	int err = -EINVAL;
@@ -254,13 +376,13 @@ static int w1_process_command_master(struct w1_master *dev,
 	case W1_CMD_ALARM_SEARCH:
 	case W1_CMD_LIST_SLAVES:
 		mutex_unlock(&dev->bus_mutex);
-		err = w1_get_slaves(dev, req_msg, req_hdr, req_cmd);
+		err = w1_get_slaves(dev, req_cmd);
 		mutex_lock(&dev->bus_mutex);
 		break;
 	case W1_CMD_READ:
 	case W1_CMD_WRITE:
 	case W1_CMD_TOUCH:
-		err = w1_process_command_io(dev, req_msg, req_hdr, req_cmd);
+		err = w1_process_command_io(dev, req_cmd);
 		break;
 	case W1_CMD_RESET:
 		err = w1_reset_bus(dev);
@@ -269,8 +391,7 @@ static int w1_process_command_master(struct w1_master *dev,
 	case W1_CMD_SLAVE_REMOVE:
 		mutex_unlock(&dev->bus_mutex);
 		mutex_lock(&dev->mutex);
-		err = w1_process_command_addremove(dev, req_msg, req_hdr,
-			req_cmd);
+		err = w1_process_command_addremove(dev, req_cmd);
 		mutex_unlock(&dev->mutex);
 		mutex_lock(&dev->bus_mutex);
 		break;
@@ -282,22 +403,21 @@ static int w1_process_command_master(struct w1_master *dev,
 	return err;
 }
 
-static int w1_process_command_slave(struct w1_slave *sl, struct cn_msg *msg,
-		struct w1_netlink_msg *hdr, struct w1_netlink_cmd *cmd)
+static int w1_process_command_slave(struct w1_slave *sl,
+		struct w1_netlink_cmd *cmd)
 {
 	dev_dbg(&sl->master->dev, "%s: %02x.%012llx.%02x: cmd=%02x, len=%u.\n",
 		__func__, sl->reg_num.family, (unsigned long long)sl->reg_num.id,
 		sl->reg_num.crc, cmd->cmd, cmd->len);
 
-	return w1_process_command_io(sl->master, msg, hdr, cmd);
+	return w1_process_command_io(sl->master, cmd);
 }
 
-static int w1_process_command_root(struct cn_msg *msg,
-	struct w1_netlink_msg *mcmd, u32 portid)
+static int w1_process_command_root(struct cn_msg *req_cn, u32 portid)
 {
-	struct w1_master *m;
+	struct w1_master *dev;
 	struct cn_msg *cn;
-	struct w1_netlink_msg *w;
+	struct w1_netlink_msg *msg;
 	u32 *id;
 
 	cn = kmalloc(PAGE_SIZE, GFP_KERNEL);
@@ -307,32 +427,30 @@ static int w1_process_command_root(struct cn_msg *msg,
 	cn->id.idx = CN_W1_IDX;
 	cn->id.val = CN_W1_VAL;
 
-	cn->seq = msg->seq;
-	cn->ack = 1;
+	cn->seq = req_cn->seq;
+	cn->ack = req_cn->seq + 1;
 	cn->len = sizeof(struct w1_netlink_msg);
-	w = (struct w1_netlink_msg *)(cn + 1);
+	msg = (struct w1_netlink_msg *)cn->data;
 
-	w->type = W1_LIST_MASTERS;
-	w->status = 0;
-	w->len = 0;
-	id = (u32 *)(w + 1);
+	msg->type = W1_LIST_MASTERS;
+	msg->status = 0;
+	msg->len = 0;
+	id = (u32 *)msg->data;
 
 	mutex_lock(&w1_mlock);
-	list_for_each_entry(m, &w1_masters, w1_master_entry) {
+	list_for_each_entry(dev, &w1_masters, w1_master_entry) {
 		if (cn->len + sizeof(*id) > PAGE_SIZE - sizeof(struct cn_msg)) {
 			cn_netlink_send(cn, portid, 0, GFP_KERNEL);
-			cn->ack++;
 			cn->len = sizeof(struct w1_netlink_msg);
-			w->len = 0;
-			id = (u32 *)(w + 1);
+			msg->len = 0;
+			id = (u32 *)msg->data;
 		}
 
-		*id = m->id;
-		w->len += sizeof(*id);
+		*id = dev->id;
+		msg->len += sizeof(*id);
 		cn->len += sizeof(*id);
 		id++;
 	}
-	cn->ack = 0;
 	cn_netlink_send(cn, portid, 0, GFP_KERNEL);
 	mutex_unlock(&w1_mlock);
 
@@ -340,100 +458,44 @@ static int w1_process_command_root(struct cn_msg *msg,
 	return 0;
 }
 
-static int w1_netlink_send_error(struct cn_msg *rcmsg, struct w1_netlink_msg *rmsg,
-		struct w1_netlink_cmd *rcmd, int portid, int error)
-{
-	struct cn_msg *cmsg;
-	struct w1_netlink_msg *msg;
-	struct w1_netlink_cmd *cmd;
-
-	cmsg = kzalloc(sizeof(*msg) + sizeof(*cmd) + sizeof(*cmsg), GFP_KERNEL);
-	if (!cmsg)
-		return -ENOMEM;
-
-	msg = (struct w1_netlink_msg *)(cmsg + 1);
-	cmd = (struct w1_netlink_cmd *)(msg + 1);
-
-	memcpy(cmsg, rcmsg, sizeof(*cmsg));
-	cmsg->len = sizeof(*msg);
-
-	memcpy(msg, rmsg, sizeof(*msg));
-	msg->len = 0;
-	msg->status = (short)-error;
-
-	if (rcmd) {
-		memcpy(cmd, rcmd, sizeof(*cmd));
-		cmd->len = 0;
-		msg->len += sizeof(*cmd);
-		cmsg->len += sizeof(*cmd);
-	}
-
-	error = cn_netlink_send(cmsg, portid, 0, GFP_KERNEL);
-	kfree(cmsg);
-
-	return error;
-}
-
-/* Bundle together a reference count, the full message, and broken out
- * commands to be executed on each w1 master kthread in one memory allocation.
- */
-struct w1_cb_block {
-	atomic_t refcnt;
-	u32 portid; /* Sending process port ID */
-	struct cn_msg msg;
-	/* cn_msg data */
-	/* one or more variable length struct w1_cb_node */
-};
-struct w1_cb_node {
-	struct w1_async_cmd async;
-	/* pointers within w1_cb_block and msg data */
-	struct w1_cb_block *block;
-	struct w1_netlink_msg *m;
-	struct w1_slave *sl;
-	struct w1_master *dev;
-};
-
 static void w1_process_cb(struct w1_master *dev, struct w1_async_cmd *async_cmd)
 {
 	struct w1_cb_node *node = container_of(async_cmd, struct w1_cb_node,
 		async);
-	u16 mlen = node->m->len;
-	u8 *cmd_data = node->m->data;
+	u16 mlen = node->msg->len;
+	u16 len;
 	int err = 0;
 	struct w1_slave *sl = node->sl;
-	struct w1_netlink_cmd *cmd = NULL;
+	struct w1_netlink_cmd *cmd = (struct w1_netlink_cmd *)node->msg->data;
 
 	mutex_lock(&dev->bus_mutex);
-	dev->portid = node->block->portid;
+	dev->priv = node->block;
 	if (sl && w1_reset_select_slave(sl))
 		err = -ENODEV;
+	node->block->cur_msg = node->msg;
 
 	while (mlen && !err) {
-		cmd = (struct w1_netlink_cmd *)cmd_data;
-
 		if (cmd->len + sizeof(struct w1_netlink_cmd) > mlen) {
 			err = -E2BIG;
 			break;
 		}
 
 		if (sl)
-			err = w1_process_command_slave(sl, &node->block->msg,
-				node->m, cmd);
+			err = w1_process_command_slave(sl, cmd);
 		else
-			err = w1_process_command_master(dev, &node->block->msg,
-				node->m, cmd);
+			err = w1_process_command_master(dev, cmd);
+		w1_netlink_check_send(node->block);
 
-		w1_netlink_send_error(&node->block->msg, node->m, cmd,
-			node->block->portid, err);
+		w1_netlink_queue_status(node->block, node->msg, cmd, err);
 		err = 0;
 
-		cmd_data += cmd->len + sizeof(struct w1_netlink_cmd);
-		mlen -= cmd->len + sizeof(struct w1_netlink_cmd);
+		len = sizeof(*cmd) + cmd->len;
+		cmd = (struct w1_netlink_cmd *)((u8 *)cmd + len);
+		mlen -= len;
 	}
 
 	if (!cmd || err)
-		w1_netlink_send_error(&node->block->msg, node->m, cmd,
-			node->block->portid, err);
+		w1_netlink_queue_status(node->block, node->msg, cmd, err);
 
 	/* ref taken in w1_search_slave or w1_search_master_id when building
 	 * the block
@@ -442,99 +504,186 @@ static void w1_process_cb(struct w1_master *dev, struct w1_async_cmd *async_cmd)
 		w1_unref_slave(sl);
 	else
 		atomic_dec(&dev->refcnt);
-	dev->portid = 0;
+	dev->priv = NULL;
 	mutex_unlock(&dev->bus_mutex);
 
 	mutex_lock(&dev->list_mutex);
 	list_del(&async_cmd->async_entry);
 	mutex_unlock(&dev->list_mutex);
 
-	if (atomic_sub_return(1, &node->block->refcnt) == 0)
-		kfree(node->block);
+	w1_unref_block(node->block);
+}
+
+static void w1_list_count_cmds(struct w1_netlink_msg *msg, int *cmd_count,
+	u16 *slave_len)
+{
+	struct w1_netlink_cmd *cmd = (struct w1_netlink_cmd *)msg->data;
+	u16 mlen = msg->len;
+	u16 len;
+	int slave_list = 0;
+	while (mlen) {
+		if (cmd->len + sizeof(struct w1_netlink_cmd) > mlen)
+			break;
+
+		switch (cmd->cmd) {
+		case W1_CMD_SEARCH:
+		case W1_CMD_ALARM_SEARCH:
+		case W1_CMD_LIST_SLAVES:
+			++slave_list;
+		}
+		++*cmd_count;
+		len = sizeof(*cmd) + cmd->len;
+		cmd = (struct w1_netlink_cmd *)((u8 *)cmd + len);
+		mlen -= len;
+	}
+
+	if (slave_list) {
+		struct w1_master *dev = w1_search_master_id(msg->id.mst.id);
+		if (dev) {
+			/* Bytes, and likely an overstimate, and if it isn't
+			 * the results can still be split between packets.
+			 */
+			*slave_len += sizeof(struct w1_reg_num) * slave_list *
+				(dev->slave_count + dev->max_slave_count);
+			/* search incremented it */
+			atomic_dec(&dev->refcnt);
+		}
+	}
 }
 
-static void w1_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *nsp)
+static void w1_cn_callback(struct cn_msg *cn, struct netlink_skb_parms *nsp)
 {
-	struct w1_netlink_msg *m = (struct w1_netlink_msg *)(msg + 1);
+	struct w1_netlink_msg *msg = (struct w1_netlink_msg *)(cn + 1);
 	struct w1_slave *sl;
 	struct w1_master *dev;
 	u16 msg_len;
+	u16 slave_len = 0;
 	int err = 0;
 	struct w1_cb_block *block = NULL;
 	struct w1_cb_node *node = NULL;
 	int node_count = 0;
+	int cmd_count = 0;
+
+	/* If any unknown flag is set let the application know, that way
+	 * applications can detect the absence of features in kernels that
+	 * don't know about them.  http://lwn.net/Articles/587527/
+	 */
+	if (cn->flags & ~(W1_CN_BUNDLE)) {
+		w1_netlink_send_error(cn, msg, nsp->portid, -EINVAL);
+		return;
+	}
 
 	/* Count the number of master or slave commands there are to allocate
 	 * space for one cb_node each.
 	 */
-	msg_len = msg->len;
+	msg_len = cn->len;
 	while (msg_len && !err) {
-		if (m->len + sizeof(struct w1_netlink_msg) > msg_len) {
+		if (msg->len + sizeof(struct w1_netlink_msg) > msg_len) {
 			err = -E2BIG;
 			break;
 		}
 
-		if (m->type == W1_MASTER_CMD || m->type == W1_SLAVE_CMD)
+		/* count messages for nodes and allocate any additional space
+		 * required for slave lists
+		 */
+		if (msg->type == W1_MASTER_CMD || msg->type == W1_SLAVE_CMD) {
 			++node_count;
+			w1_list_count_cmds(msg, &cmd_count, &slave_len);
+		}
 
-		msg_len -= sizeof(struct w1_netlink_msg) + m->len;
-		m = (struct w1_netlink_msg *)(((u8 *)m) +
-			sizeof(struct w1_netlink_msg) + m->len);
+		msg_len -= sizeof(struct w1_netlink_msg) + msg->len;
+		msg = (struct w1_netlink_msg *)(((u8 *)msg) +
+			sizeof(struct w1_netlink_msg) + msg->len);
 	}
-	m = (struct w1_netlink_msg *)(msg + 1);
+	msg = (struct w1_netlink_msg *)(cn + 1);
 	if (node_count) {
-		/* msg->len doesn't include itself */
-		long size = sizeof(struct w1_cb_block) + msg->len +
-			node_count*sizeof(struct w1_cb_node);
-		block = kmalloc(size, GFP_KERNEL);
+		int size;
+		u16 reply_size = sizeof(*cn) + cn->len + slave_len;
+		if (cn->flags & W1_CN_BUNDLE) {
+			/* bundling duplicats some of the messages */
+			reply_size += 2 * cmd_count * (sizeof(struct cn_msg) +
+				sizeof(struct w1_netlink_msg) +
+				sizeof(struct w1_netlink_cmd));
+		}
+		reply_size = MIN(CONNECTOR_MAX_MSG_SIZE, reply_size);
+
+		/* allocate space for the block, a copy of the original message,
+		 * one node per cmd to point into the original message,
+		 * space for replies which is the original message size plus
+		 * space for any list slave data and status messages
+		 * cn->len doesn't include itself which is part of the block
+		 * */
+		size =  /* block + original message */
+			sizeof(struct w1_cb_block) + sizeof(*cn) + cn->len +
+			/* space for nodes */
+			node_count * sizeof(struct w1_cb_node) +
+			/* replies */
+			sizeof(struct cn_msg) + reply_size;
+		block = kzalloc(size, GFP_KERNEL);
 		if (!block) {
-			w1_netlink_send_error(msg, m, NULL, nsp->portid,
-				-ENOMEM);
+			/* if the system is already out of memory,
+			 * (A) will this work, and (B) would it be better
+			 * to not try?
+			 */
+			w1_netlink_send_error(cn, msg, nsp->portid, -ENOMEM);
 			return;
 		}
 		atomic_set(&block->refcnt, 1);
 		block->portid = nsp->portid;
-		memcpy(&block->msg, msg, sizeof(*msg) + msg->len);
-		node = (struct w1_cb_node *)((u8 *)block->msg.data + msg->len);
+		memcpy(&block->request_cn, cn, sizeof(*cn) + cn->len);
+		node = (struct w1_cb_node *)(block->request_cn.data + cn->len);
+
+		/* Sneeky, when not bundling, reply_size is the allocated space
+		 * required for the reply, cn_msg isn't part of maxlen so
+		 * it should be reply_size - sizeof(struct cn_msg), however
+		 * when checking if there is enough space, w1_reply_make_space
+		 * is called with the full message size including cn_msg,
+		 * because it isn't known at that time if an additional cn_msg
+		 * will need to be allocated.  So an extra cn_msg is added
+		 * above in "size".
+		 */
+		block->maxlen = reply_size;
+		block->first_cn = (struct cn_msg *)(node + node_count);
+		memset(block->first_cn, 0, sizeof(*block->first_cn));
 	}
 
-	msg_len = msg->len;
+	msg_len = cn->len;
 	while (msg_len && !err) {
 
 		dev = NULL;
 		sl = NULL;
 
-		if (m->len + sizeof(struct w1_netlink_msg) > msg_len) {
+		if (msg->len + sizeof(struct w1_netlink_msg) > msg_len) {
 			err = -E2BIG;
 			break;
 		}
 
 		/* execute on this thread, no need to process later */
-		if (m->type == W1_LIST_MASTERS) {
-			err = w1_process_command_root(msg, m, nsp->portid);
+		if (msg->type == W1_LIST_MASTERS) {
+			err = w1_process_command_root(cn, nsp->portid);
 			goto out_cont;
 		}
 
 		/* All following message types require additional data,
 		 * check here before references are taken.
 		 */
-		if (!m->len) {
+		if (!msg->len) {
 			err = -EPROTO;
 			goto out_cont;
 		}
 
-		/* both search calls take reference counts */
-		if (m->type == W1_MASTER_CMD) {
-			dev = w1_search_master_id(m->id.mst.id);
-		} else if (m->type == W1_SLAVE_CMD) {
-			sl = w1_search_slave((struct w1_reg_num *)m->id.id);
+		/* both search calls take references */
+		if (msg->type == W1_MASTER_CMD) {
+			dev = w1_search_master_id(msg->id.mst.id);
+		} else if (msg->type == W1_SLAVE_CMD) {
+			sl = w1_search_slave((struct w1_reg_num *)msg->id.id);
 			if (sl)
 				dev = sl->master;
 		} else {
 			printk(KERN_NOTICE
-				"%s: msg: %x.%x, wrong type: %u, len: %u.\n",
-				__func__, msg->id.idx, msg->id.val,
-				m->type, m->len);
+				"%s: cn: %x.%x, wrong type: %u, len: %u.\n",
+				__func__, cn->id.idx, cn->id.val,
+				msg->type, msg->len);
 			err = -EPROTO;
 			goto out_cont;
 		}
@@ -549,8 +698,8 @@ static void w1_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *nsp)
 		atomic_inc(&block->refcnt);
 		node->async.cb = w1_process_cb;
 		node->block = block;
-		node->m = (struct w1_netlink_msg *)((u8 *)&block->msg +
-			(size_t)((u8 *)m - (u8 *)msg));
+		node->msg = (struct w1_netlink_msg *)((u8 *)&block->request_cn +
+			(size_t)((u8 *)msg - (u8 *)cn));
 		node->sl = sl;
 		node->dev = dev;
 
@@ -561,11 +710,15 @@ static void w1_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *nsp)
 		++node;
 
 out_cont:
+		/* Can't queue because that modifies block and another
+		 * thread could be processing the messages by now and
+		 * there isn't a lock, send directly.
+		 */
 		if (err)
-			w1_netlink_send_error(msg, m, NULL, nsp->portid, err);
-		msg_len -= sizeof(struct w1_netlink_msg) + m->len;
-		m = (struct w1_netlink_msg *)(((u8 *)m) +
-			sizeof(struct w1_netlink_msg) + m->len);
+			w1_netlink_send_error(cn, msg, nsp->portid, err);
+		msg_len -= sizeof(struct w1_netlink_msg) + msg->len;
+		msg = (struct w1_netlink_msg *)(((u8 *)msg) +
+			sizeof(struct w1_netlink_msg) + msg->len);
 
 		/*
 		 * Let's allow requests for nonexisting devices.
@@ -573,8 +726,8 @@ out_cont:
 		if (err == -ENODEV)
 			err = 0;
 	}
-	if (block && atomic_sub_return(1, &block->refcnt) == 0)
-		kfree(block);
+	if (block)
+		w1_unref_block(block);
 }
 
 int w1_init_netlink(void)
@@ -591,7 +744,7 @@ void w1_fini_netlink(void)
 	cn_del_callback(&w1_id);
 }
 #else
-void w1_netlink_send(struct w1_master *dev, struct w1_netlink_msg *msg)
+void w1_netlink_send(struct w1_master *dev, struct w1_netlink_msg *cn)
 {
 }
 
diff --git a/drivers/w1/w1_netlink.h b/drivers/w1/w1_netlink.h
index 1e9504e6765..c99a9ce05e6 100644
--- a/drivers/w1/w1_netlink.h
+++ b/drivers/w1/w1_netlink.h
@@ -28,6 +28,17 @@
 #include "w1.h"
 
 /**
+ * enum w1_cn_msg_flags - bitfield flags for struct cn_msg.flags
+ *
+ * @W1_CN_BUNDLE: Request bundling replies into fewer messagse.  Be prepared
+ * to handle multiple struct cn_msg, struct w1_netlink_msg, and
+ * struct w1_netlink_cmd in one packet.
+ */
+enum w1_cn_msg_flags {
+	W1_CN_BUNDLE = 1,
+};
+
+/**
  * enum w1_netlink_message_types - message type
  *
  * @W1_SLAVE_ADD: notification that a slave device was added
@@ -49,6 +60,19 @@ enum w1_netlink_message_types {
 	W1_LIST_MASTERS,
 };
 
+/**
+ * struct w1_netlink_msg - holds w1 message type, id, and result
+ *
+ * @type: one of enum w1_netlink_message_types
+ * @status: kernel feedback for success 0 or errno failure value
+ * @len: length of data following w1_netlink_msg
+ * @id: union holding master bus id (msg.id) and slave device id (id[8]).
+ * @data: start address of any following data
+ *
+ * The base message structure for w1 messages over netlink.
+ * The netlink connector data sequence is, struct nlmsghdr, struct cn_msg,
+ * then one or more struct w1_netlink_msg (each with optional data).
+ */
 struct w1_netlink_msg
 {
 	__u8				type;
@@ -66,6 +90,7 @@ struct w1_netlink_msg
 
 /**
  * enum w1_commands - commands available for master or slave operations
+ *
  * @W1_CMD_READ: read len bytes
  * @W1_CMD_WRITE: write len bytes
  * @W1_CMD_SEARCH: initiate a standard search, returns only the slave
@@ -93,6 +118,17 @@ enum w1_commands {
 	W1_CMD_MAX
 };
 
+/**
+ * struct w1_netlink_cmd - holds the command and data
+ *
+ * @cmd: one of enum w1_commands
+ * @res: reserved
+ * @len: length of data following w1_netlink_cmd
+ * @data: start address of any following data
+ *
+ * One or more struct w1_netlink_cmd is placed starting at w1_netlink_msg.data
+ * each with optional data.
+ */
 struct w1_netlink_cmd
 {
 	__u8				cmd;
diff --git a/drivers/watchdog/Kconfig b/drivers/watchdog/Kconfig
index 74ec8fc5cc0..c845527b503 100644
--- a/drivers/watchdog/Kconfig
+++ b/drivers/watchdog/Kconfig
@@ -272,7 +272,7 @@ config PNX4008_WATCHDOG
 
 config IOP_WATCHDOG
 	tristate "IOP Watchdog"
-	depends on PLAT_IOP
+	depends on ARCH_IOP13XX
 	select WATCHDOG_NOWAYOUT if (ARCH_IOP32X || ARCH_IOP33X)
 	help
 	  Say Y here if to include support for the watchdog timer
@@ -378,6 +378,8 @@ config MAX63XX_WATCHDOG
 config IMX2_WDT
 	tristate "IMX2+ Watchdog"
 	depends on ARCH_MXC
+	select REGMAP_MMIO
+	select WATCHDOG_CORE
 	help
 	  This is the driver for the hardware watchdog
 	  on the Freescale IMX2 and later processors.
@@ -663,6 +665,19 @@ config INTEL_SCU_WATCHDOG
 
 	  To compile this driver as a module, choose M here.
 
+config INTEL_MID_WATCHDOG
+	tristate "Intel MID Watchdog Timer"
+	depends on X86_INTEL_MID
+	select WATCHDOG_CORE
+	---help---
+	  Watchdog timer driver built into the Intel SCU for Intel MID
+	  Platforms.
+
+	  This driver currently supports only the watchdog evolution
+	  implementation in SCU, available for Merrifield generation.
+
+	  To compile this driver as a module, choose M here.
+
 config ITCO_WDT
 	tristate "Intel TCO Timer/Watchdog"
 	depends on (X86 || IA64) && PCI
@@ -835,7 +850,7 @@ config 60XX_WDT
 
 config SBC8360_WDT
 	tristate "SBC8360 Watchdog Timer"
-	depends on X86
+	depends on X86_32
 	---help---
 
 	  This is the driver for the hardware watchdog on the SBC8360 Single
@@ -938,36 +953,6 @@ config W83627HF_WDT
 
 	  Most people will say N.
 
-config W83697HF_WDT
-	tristate "W83697HF/W83697HG Watchdog Timer"
-	depends on X86
-	---help---
-	  This is the driver for the hardware watchdog on the W83697HF/HG
-	  chipset as used in Dedibox/VIA motherboards (and likely others).
-	  This watchdog simply watches your kernel to make sure it doesn't
-	  freeze, and if it does, it reboots your computer after a certain
-	  amount of time.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called w83697hf_wdt.
-
-	  Most people will say N.
-
-config W83697UG_WDT
-	tristate "W83697UG/W83697UF Watchdog Timer"
-	depends on X86
-	---help---
-	  This is the driver for the hardware watchdog on the W83697UG/UF
-	  chipset as used in MSI Fuzzy CX700 VIA motherboards (and likely others).
-	  This watchdog simply watches your kernel to make sure it doesn't
-	  freeze, and if it does, it reboots your computer after a certain
-	  amount of time.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called w83697ug_wdt.
-
-	  Most people will say N.
-
 config W83877F_WDT
 	tristate "W83877F (EMACS) Watchdog Timer"
 	depends on X86
diff --git a/drivers/watchdog/Makefile b/drivers/watchdog/Makefile
index 1b5f3d5efad..7b8a91ed20e 100644
--- a/drivers/watchdog/Makefile
+++ b/drivers/watchdog/Makefile
@@ -107,13 +107,12 @@ obj-$(CONFIG_SMSC_SCH311X_WDT) += sch311x_wdt.o
 obj-$(CONFIG_SMSC37B787_WDT) += smsc37b787_wdt.o
 obj-$(CONFIG_VIA_WDT) += via_wdt.o
 obj-$(CONFIG_W83627HF_WDT) += w83627hf_wdt.o
-obj-$(CONFIG_W83697HF_WDT) += w83697hf_wdt.o
-obj-$(CONFIG_W83697UG_WDT) += w83697ug_wdt.o
 obj-$(CONFIG_W83877F_WDT) += w83877f_wdt.o
 obj-$(CONFIG_W83977F_WDT) += w83977f_wdt.o
 obj-$(CONFIG_MACHZ_WDT) += machzwd.o
 obj-$(CONFIG_SBC_EPX_C3_WATCHDOG) += sbc_epx_c3.o
 obj-$(CONFIG_INTEL_SCU_WATCHDOG) += intel_scu_watchdog.o
+obj-$(CONFIG_INTEL_MID_WATCHDOG) += intel-mid_wdt.o
 
 # M32R Architecture
 
diff --git a/drivers/watchdog/ath79_wdt.c b/drivers/watchdog/ath79_wdt.c
index 399c3fddecf..41ac4660fb8 100644
--- a/drivers/watchdog/ath79_wdt.c
+++ b/drivers/watchdog/ath79_wdt.c
@@ -20,6 +20,7 @@
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/bitops.h>
+#include <linux/delay.h>
 #include <linux/errno.h>
 #include <linux/fs.h>
 #include <linux/io.h>
@@ -90,6 +91,15 @@ static inline void ath79_wdt_keepalive(void)
 static inline void ath79_wdt_enable(void)
 {
 	ath79_wdt_keepalive();
+
+	/*
+	 * Updating the TIMER register requires a few microseconds
+	 * on the AR934x SoCs at least. Use a small delay to ensure
+	 * that the TIMER register is updated within the hardware
+	 * before enabling the watchdog.
+	 */
+	udelay(2);
+
 	ath79_wdt_wr(WDOG_REG_CTRL, WDOG_CTRL_ACTION_FCR);
 	/* flush write */
 	ath79_wdt_rr(WDOG_REG_CTRL);
@@ -255,7 +265,7 @@ static int ath79_wdt_probe(struct platform_device *pdev)
 	if (IS_ERR(wdt_clk))
 		return PTR_ERR(wdt_clk);
 
-	err = clk_enable(wdt_clk);
+	err = clk_prepare_enable(wdt_clk);
 	if (err)
 		return err;
 
@@ -286,14 +296,14 @@ static int ath79_wdt_probe(struct platform_device *pdev)
 	return 0;
 
 err_clk_disable:
-	clk_disable(wdt_clk);
+	clk_disable_unprepare(wdt_clk);
 	return err;
 }
 
 static int ath79_wdt_remove(struct platform_device *pdev)
 {
 	misc_deregister(&ath79_wdt_miscdev);
-	clk_disable(wdt_clk);
+	clk_disable_unprepare(wdt_clk);
 	return 0;
 }
 
diff --git a/drivers/watchdog/booke_wdt.c b/drivers/watchdog/booke_wdt.c
index a8dbceb3291..08a785398ea 100644
--- a/drivers/watchdog/booke_wdt.c
+++ b/drivers/watchdog/booke_wdt.c
@@ -41,6 +41,28 @@ 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);
+
 #ifdef CONFIG_PPC_FSL_BOOK3E
 
 /* For the specified period, determine the number of seconds
@@ -103,17 +125,18 @@ static unsigned int sec_to_period(unsigned int secs)
 static void __booke_wdt_set(void *data)
 {
 	u32 val;
+	struct watchdog_device *wdog = data;
 
 	val = mfspr(SPRN_TCR);
 	val &= ~WDTP_MASK;
-	val |= WDTP(booke_wdt_period);
+	val |= WDTP(sec_to_period(wdog->timeout));
 
 	mtspr(SPRN_TCR, val);
 }
 
-static void booke_wdt_set(void)
+static void booke_wdt_set(void *data)
 {
-	on_each_cpu(__booke_wdt_set, NULL, 0);
+	on_each_cpu(__booke_wdt_set, data, 0);
 }
 
 static void __booke_wdt_ping(void *data)
@@ -131,12 +154,13 @@ static int booke_wdt_ping(struct watchdog_device *wdog)
 static void __booke_wdt_enable(void *data)
 {
 	u32 val;
+	struct watchdog_device *wdog = data;
 
 	/* clear status before enabling watchdog */
 	__booke_wdt_ping(NULL);
 	val = mfspr(SPRN_TCR);
 	val &= ~WDTP_MASK;
-	val |= (TCR_WIE|TCR_WRC(WRC_CHIP)|WDTP(booke_wdt_period));
+	val |= (TCR_WIE|TCR_WRC(WRC_CHIP)|WDTP(sec_to_period(wdog->timeout)));
 
 	mtspr(SPRN_TCR, val);
 }
@@ -162,25 +186,17 @@ static void __booke_wdt_disable(void *data)
 
 }
 
-static void __booke_wdt_start(struct watchdog_device *wdog)
+static int booke_wdt_start(struct watchdog_device *wdog)
 {
-	on_each_cpu(__booke_wdt_enable, NULL, 0);
+	on_each_cpu(__booke_wdt_enable, wdog, 0);
 	pr_debug("watchdog enabled (timeout = %u sec)\n", wdog->timeout);
-}
 
-static int booke_wdt_start(struct watchdog_device *wdog)
-{
-	if (booke_wdt_enabled == 0) {
-		booke_wdt_enabled = 1;
-		__booke_wdt_start(wdog);
-	}
 	return 0;
 }
 
 static int booke_wdt_stop(struct watchdog_device *wdog)
 {
 	on_each_cpu(__booke_wdt_disable, NULL, 0);
-	booke_wdt_enabled = 0;
 	pr_debug("watchdog disabled\n");
 
 	return 0;
@@ -191,9 +207,8 @@ static int booke_wdt_set_timeout(struct watchdog_device *wdt_dev,
 {
 	if (timeout > MAX_WDT_TIMEOUT)
 		return -EINVAL;
-	booke_wdt_period = sec_to_period(timeout);
 	wdt_dev->timeout = timeout;
-	booke_wdt_set();
+	booke_wdt_set(wdt_dev);
 
 	return 0;
 }
@@ -231,10 +246,10 @@ static int __init booke_wdt_init(void)
 	pr_info("powerpc book-e watchdog driver loaded\n");
 	booke_wdt_info.firmware_version = cur_cpu_spec->pvr_value;
 	booke_wdt_set_timeout(&booke_wdt_dev,
-			      period_to_sec(CONFIG_BOOKE_WDT_DEFAULT_TIMEOUT));
+			      period_to_sec(booke_wdt_period));
 	watchdog_set_nowayout(&booke_wdt_dev, nowayout);
 	if (booke_wdt_enabled)
-		__booke_wdt_start(&booke_wdt_dev);
+		booke_wdt_start(&booke_wdt_dev);
 
 	ret = watchdog_register_device(&booke_wdt_dev);
 
diff --git a/drivers/watchdog/imx2_wdt.c b/drivers/watchdog/imx2_wdt.c
index dd51d9539b3..9d4874f0994 100644
--- a/drivers/watchdog/imx2_wdt.c
+++ b/drivers/watchdog/imx2_wdt.c
@@ -21,19 +21,17 @@
  * Halt on suspend:	Manual		Can be automatic
  */
 
+#include <linux/clk.h>
 #include <linux/init.h>
+#include <linux/io.h>
+#include <linux/jiffies.h>
 #include <linux/kernel.h>
-#include <linux/miscdevice.h>
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 #include <linux/platform_device.h>
-#include <linux/watchdog.h>
-#include <linux/clk.h>
-#include <linux/fs.h>
-#include <linux/io.h>
-#include <linux/uaccess.h>
+#include <linux/regmap.h>
 #include <linux/timer.h>
-#include <linux/jiffies.h>
+#include <linux/watchdog.h>
 
 #define DRIVER_NAME "imx2-wdt"
 
@@ -55,19 +53,12 @@
 
 #define WDOG_SEC_TO_COUNT(s)	((s * 2 - 1) << 8)
 
-#define IMX2_WDT_STATUS_OPEN	0
-#define IMX2_WDT_STATUS_STARTED	1
-#define IMX2_WDT_EXPECT_CLOSE	2
-
-static struct {
+struct imx2_wdt_device {
 	struct clk *clk;
-	void __iomem *base;
-	unsigned timeout;
-	unsigned long status;
+	struct regmap *regmap;
 	struct timer_list timer;	/* Pings the watchdog when closed */
-} imx2_wdt;
-
-static struct miscdevice imx2_wdt_miscdev;
+	struct watchdog_device wdog;
+};
 
 static bool nowayout = WATCHDOG_NOWAYOUT;
 module_param(nowayout, bool, 0);
@@ -85,9 +76,12 @@ static const struct watchdog_info imx2_wdt_info = {
 	.options = WDIOF_KEEPALIVEPING | WDIOF_SETTIMEOUT | WDIOF_MAGICCLOSE,
 };
 
-static inline void imx2_wdt_setup(void)
+static inline void imx2_wdt_setup(struct watchdog_device *wdog)
 {
-	u16 val = __raw_readw(imx2_wdt.base + IMX2_WDT_WCR);
+	struct imx2_wdt_device *wdev = watchdog_get_drvdata(wdog);
+	u32 val;
+
+	regmap_read(wdev->regmap, IMX2_WDT_WCR, &val);
 
 	/* Suspend timer in low power mode, write once-only */
 	val |= IMX2_WDT_WCR_WDZST;
@@ -98,227 +92,199 @@ static inline void imx2_wdt_setup(void)
 	/* Keep Watchdog Disabled */
 	val &= ~IMX2_WDT_WCR_WDE;
 	/* Set the watchdog's Time-Out value */
-	val |= WDOG_SEC_TO_COUNT(imx2_wdt.timeout);
+	val |= WDOG_SEC_TO_COUNT(wdog->timeout);
 
-	__raw_writew(val, imx2_wdt.base + IMX2_WDT_WCR);
+	regmap_write(wdev->regmap, IMX2_WDT_WCR, val);
 
 	/* enable the watchdog */
 	val |= IMX2_WDT_WCR_WDE;
-	__raw_writew(val, imx2_wdt.base + IMX2_WDT_WCR);
+	regmap_write(wdev->regmap, IMX2_WDT_WCR, val);
 }
 
-static inline void imx2_wdt_ping(void)
+static inline bool imx2_wdt_is_running(struct imx2_wdt_device *wdev)
 {
-	__raw_writew(IMX2_WDT_SEQ1, imx2_wdt.base + IMX2_WDT_WSR);
-	__raw_writew(IMX2_WDT_SEQ2, imx2_wdt.base + IMX2_WDT_WSR);
-}
+	u32 val;
 
-static void imx2_wdt_timer_ping(unsigned long arg)
-{
-	/* ping it every imx2_wdt.timeout / 2 seconds to prevent reboot */
-	imx2_wdt_ping();
-	mod_timer(&imx2_wdt.timer, jiffies + imx2_wdt.timeout * HZ / 2);
+	regmap_read(wdev->regmap, IMX2_WDT_WCR, &val);
+
+	return val & IMX2_WDT_WCR_WDE;
 }
 
-static void imx2_wdt_start(void)
+static int imx2_wdt_ping(struct watchdog_device *wdog)
 {
-	if (!test_and_set_bit(IMX2_WDT_STATUS_STARTED, &imx2_wdt.status)) {
-		/* at our first start we enable clock and do initialisations */
-		clk_prepare_enable(imx2_wdt.clk);
+	struct imx2_wdt_device *wdev = watchdog_get_drvdata(wdog);
 
-		imx2_wdt_setup();
-	} else	/* delete the timer that pings the watchdog after close */
-		del_timer_sync(&imx2_wdt.timer);
-
-	/* Watchdog is enabled - time to reload the timeout value */
-	imx2_wdt_ping();
+	regmap_write(wdev->regmap, IMX2_WDT_WSR, IMX2_WDT_SEQ1);
+	regmap_write(wdev->regmap, IMX2_WDT_WSR, IMX2_WDT_SEQ2);
+	return 0;
 }
 
-static void imx2_wdt_stop(void)
+static void imx2_wdt_timer_ping(unsigned long arg)
 {
-	/* we don't need a clk_disable, it cannot be disabled once started.
-	 * We use a timer to ping the watchdog while /dev/watchdog is closed */
-	imx2_wdt_timer_ping(0);
+	struct watchdog_device *wdog = (struct watchdog_device *)arg;
+	struct imx2_wdt_device *wdev = watchdog_get_drvdata(wdog);
+
+	/* ping it every wdog->timeout / 2 seconds to prevent reboot */
+	imx2_wdt_ping(wdog);
+	mod_timer(&wdev->timer, jiffies + wdog->timeout * HZ / 2);
 }
 
-static void imx2_wdt_set_timeout(int new_timeout)
+static int imx2_wdt_set_timeout(struct watchdog_device *wdog,
+				unsigned int new_timeout)
 {
-	u16 val = __raw_readw(imx2_wdt.base + IMX2_WDT_WCR);
+	struct imx2_wdt_device *wdev = watchdog_get_drvdata(wdog);
 
-	/* set the new timeout value in the WSR */
-	val &= ~IMX2_WDT_WCR_WT;
-	val |= WDOG_SEC_TO_COUNT(new_timeout);
-	__raw_writew(val, imx2_wdt.base + IMX2_WDT_WCR);
+	regmap_update_bits(wdev->regmap, IMX2_WDT_WCR, IMX2_WDT_WCR_WT,
+			   WDOG_SEC_TO_COUNT(new_timeout));
+	return 0;
 }
 
-static int imx2_wdt_open(struct inode *inode, struct file *file)
+static int imx2_wdt_start(struct watchdog_device *wdog)
 {
-	if (test_and_set_bit(IMX2_WDT_STATUS_OPEN, &imx2_wdt.status))
-		return -EBUSY;
+	struct imx2_wdt_device *wdev = watchdog_get_drvdata(wdog);
+
+	if (imx2_wdt_is_running(wdev)) {
+		/* delete the timer that pings the watchdog after close */
+		del_timer_sync(&wdev->timer);
+		imx2_wdt_set_timeout(wdog, wdog->timeout);
+	} else
+		imx2_wdt_setup(wdog);
 
-	imx2_wdt_start();
-	return nonseekable_open(inode, file);
+	return imx2_wdt_ping(wdog);
 }
 
-static int imx2_wdt_close(struct inode *inode, struct file *file)
+static int imx2_wdt_stop(struct watchdog_device *wdog)
 {
-	if (test_bit(IMX2_WDT_EXPECT_CLOSE, &imx2_wdt.status) && !nowayout)
-		imx2_wdt_stop();
-	else {
-		dev_crit(imx2_wdt_miscdev.parent,
-			"Unexpected close: Expect reboot!\n");
-		imx2_wdt_ping();
-	}
-
-	clear_bit(IMX2_WDT_EXPECT_CLOSE, &imx2_wdt.status);
-	clear_bit(IMX2_WDT_STATUS_OPEN, &imx2_wdt.status);
+	/*
+	 * We don't need a clk_disable, it cannot be disabled once started.
+	 * We use a timer to ping the watchdog while /dev/watchdog is closed
+	 */
+	imx2_wdt_timer_ping((unsigned long)wdog);
 	return 0;
 }
 
-static long imx2_wdt_ioctl(struct file *file, unsigned int cmd,
-							unsigned long arg)
+static inline void imx2_wdt_ping_if_active(struct watchdog_device *wdog)
 {
-	void __user *argp = (void __user *)arg;
-	int __user *p = argp;
-	int new_value;
-	u16 val;
-
-	switch (cmd) {
-	case WDIOC_GETSUPPORT:
-		return copy_to_user(argp, &imx2_wdt_info,
-			sizeof(struct watchdog_info)) ? -EFAULT : 0;
-
-	case WDIOC_GETSTATUS:
-		return put_user(0, p);
-
-	case WDIOC_GETBOOTSTATUS:
-		val = __raw_readw(imx2_wdt.base + IMX2_WDT_WRSR);
-		new_value = val & IMX2_WDT_WRSR_TOUT ? WDIOF_CARDRESET : 0;
-		return put_user(new_value, p);
-
-	case WDIOC_KEEPALIVE:
-		imx2_wdt_ping();
-		return 0;
-
-	case WDIOC_SETTIMEOUT:
-		if (get_user(new_value, p))
-			return -EFAULT;
-		if ((new_value < 1) || (new_value > IMX2_WDT_MAX_TIME))
-			return -EINVAL;
-		imx2_wdt_set_timeout(new_value);
-		imx2_wdt.timeout = new_value;
-		imx2_wdt_ping();
-
-		/* Fallthrough to return current value */
-	case WDIOC_GETTIMEOUT:
-		return put_user(imx2_wdt.timeout, p);
-
-	default:
-		return -ENOTTY;
-	}
-}
+	struct imx2_wdt_device *wdev = watchdog_get_drvdata(wdog);
 
-static ssize_t imx2_wdt_write(struct file *file, const char __user *data,
-						size_t len, loff_t *ppos)
-{
-	size_t i;
-	char c;
-
-	if (len == 0)	/* Can we see this even ? */
-		return 0;
-
-	clear_bit(IMX2_WDT_EXPECT_CLOSE, &imx2_wdt.status);
-	/* scan to see whether or not we got the magic character */
-	for (i = 0; i != len; i++) {
-		if (get_user(c, data + i))
-			return -EFAULT;
-		if (c == 'V')
-			set_bit(IMX2_WDT_EXPECT_CLOSE, &imx2_wdt.status);
+	if (imx2_wdt_is_running(wdev)) {
+		imx2_wdt_set_timeout(wdog, wdog->timeout);
+		imx2_wdt_timer_ping((unsigned long)wdog);
 	}
-
-	imx2_wdt_ping();
-	return len;
 }
 
-static const struct file_operations imx2_wdt_fops = {
+static struct watchdog_ops imx2_wdt_ops = {
 	.owner = THIS_MODULE,
-	.llseek = no_llseek,
-	.unlocked_ioctl = imx2_wdt_ioctl,
-	.open = imx2_wdt_open,
-	.release = imx2_wdt_close,
-	.write = imx2_wdt_write,
+	.start = imx2_wdt_start,
+	.stop = imx2_wdt_stop,
+	.ping = imx2_wdt_ping,
+	.set_timeout = imx2_wdt_set_timeout,
 };
 
-static struct miscdevice imx2_wdt_miscdev = {
-	.minor = WATCHDOG_MINOR,
-	.name = "watchdog",
-	.fops = &imx2_wdt_fops,
+static struct regmap_config imx2_wdt_regmap_config = {
+	.reg_bits = 16,
+	.reg_stride = 2,
+	.val_bits = 16,
+	.max_register = 0x8,
 };
 
 static int __init imx2_wdt_probe(struct platform_device *pdev)
 {
-	int ret;
+	struct imx2_wdt_device *wdev;
+	struct watchdog_device *wdog;
 	struct resource *res;
+	void __iomem *base;
+	int ret;
+	u32 val;
+
+	wdev = devm_kzalloc(&pdev->dev, sizeof(*wdev), GFP_KERNEL);
+	if (!wdev)
+		return -ENOMEM;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	imx2_wdt.base = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(imx2_wdt.base))
-		return PTR_ERR(imx2_wdt.base);
+	base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	wdev->regmap = devm_regmap_init_mmio_clk(&pdev->dev, NULL, base,
+						 &imx2_wdt_regmap_config);
+	if (IS_ERR(wdev->regmap)) {
+		dev_err(&pdev->dev, "regmap init failed\n");
+		return PTR_ERR(wdev->regmap);
+	}
 
-	imx2_wdt.clk = devm_clk_get(&pdev->dev, NULL);
-	if (IS_ERR(imx2_wdt.clk)) {
+	wdev->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(wdev->clk)) {
 		dev_err(&pdev->dev, "can't get Watchdog clock\n");
-		return PTR_ERR(imx2_wdt.clk);
+		return PTR_ERR(wdev->clk);
 	}
 
-	imx2_wdt.timeout = clamp_t(unsigned, timeout, 1, IMX2_WDT_MAX_TIME);
-	if (imx2_wdt.timeout != timeout)
-		dev_warn(&pdev->dev, "Initial timeout out of range! "
-			"Clamped from %u to %u\n", timeout, imx2_wdt.timeout);
+	wdog			= &wdev->wdog;
+	wdog->info		= &imx2_wdt_info;
+	wdog->ops		= &imx2_wdt_ops;
+	wdog->min_timeout	= 1;
+	wdog->max_timeout	= IMX2_WDT_MAX_TIME;
 
-	setup_timer(&imx2_wdt.timer, imx2_wdt_timer_ping, 0);
+	clk_prepare_enable(wdev->clk);
 
-	imx2_wdt_miscdev.parent = &pdev->dev;
-	ret = misc_register(&imx2_wdt_miscdev);
-	if (ret)
-		goto fail;
+	regmap_read(wdev->regmap, IMX2_WDT_WRSR, &val);
+	wdog->bootstatus = val & IMX2_WDT_WRSR_TOUT ? WDIOF_CARDRESET : 0;
 
-	dev_info(&pdev->dev,
-		"IMX2+ Watchdog Timer enabled. timeout=%ds (nowayout=%d)\n",
-						imx2_wdt.timeout, nowayout);
-	return 0;
+	wdog->timeout = clamp_t(unsigned, timeout, 1, IMX2_WDT_MAX_TIME);
+	if (wdog->timeout != timeout)
+		dev_warn(&pdev->dev, "Initial timeout out of range! Clamped from %u to %u\n",
+			 timeout, wdog->timeout);
+
+	platform_set_drvdata(pdev, wdog);
+	watchdog_set_drvdata(wdog, wdev);
+	watchdog_set_nowayout(wdog, nowayout);
+	watchdog_init_timeout(wdog, timeout, &pdev->dev);
+
+	setup_timer(&wdev->timer, imx2_wdt_timer_ping, (unsigned long)wdog);
+
+	imx2_wdt_ping_if_active(wdog);
 
-fail:
-	imx2_wdt_miscdev.parent = NULL;
-	return ret;
+	ret = watchdog_register_device(wdog);
+	if (ret) {
+		dev_err(&pdev->dev, "cannot register watchdog device\n");
+		return ret;
+	}
+
+	dev_info(&pdev->dev, "timeout %d sec (nowayout=%d)\n",
+		 wdog->timeout, nowayout);
+
+	return 0;
 }
 
 static int __exit imx2_wdt_remove(struct platform_device *pdev)
 {
-	misc_deregister(&imx2_wdt_miscdev);
+	struct watchdog_device *wdog = platform_get_drvdata(pdev);
+	struct imx2_wdt_device *wdev = watchdog_get_drvdata(wdog);
 
-	if (test_bit(IMX2_WDT_STATUS_STARTED, &imx2_wdt.status)) {
-		del_timer_sync(&imx2_wdt.timer);
+	watchdog_unregister_device(wdog);
 
-		dev_crit(imx2_wdt_miscdev.parent,
-			"Device removed: Expect reboot!\n");
+	if (imx2_wdt_is_running(wdev)) {
+		del_timer_sync(&wdev->timer);
+		imx2_wdt_ping(wdog);
+		dev_crit(&pdev->dev, "Device removed: Expect reboot!\n");
 	}
-
-	imx2_wdt_miscdev.parent = NULL;
 	return 0;
 }
 
 static void imx2_wdt_shutdown(struct platform_device *pdev)
 {
-	if (test_bit(IMX2_WDT_STATUS_STARTED, &imx2_wdt.status)) {
-		/* we are running, we need to delete the timer but will give
-		 * max timeout before reboot will take place */
-		del_timer_sync(&imx2_wdt.timer);
-		imx2_wdt_set_timeout(IMX2_WDT_MAX_TIME);
-		imx2_wdt_ping();
-
-		dev_crit(imx2_wdt_miscdev.parent,
-			"Device shutdown: Expect reboot!\n");
+	struct watchdog_device *wdog = platform_get_drvdata(pdev);
+	struct imx2_wdt_device *wdev = watchdog_get_drvdata(wdog);
+
+	if (imx2_wdt_is_running(wdev)) {
+		/*
+		 * We are running, we need to delete the timer but will
+		 * give max timeout before reboot will take place
+		 */
+		del_timer_sync(&wdev->timer);
+		imx2_wdt_set_timeout(wdog, IMX2_WDT_MAX_TIME);
+		imx2_wdt_ping(wdog);
+		dev_crit(&pdev->dev, "Device shutdown: Expect reboot!\n");
 	}
 }
 
diff --git a/drivers/watchdog/intel-mid_wdt.c b/drivers/watchdog/intel-mid_wdt.c
new file mode 100644
index 00000000000..ca66e8e7463
--- /dev/null
+++ b/drivers/watchdog/intel-mid_wdt.c
@@ -0,0 +1,184 @@
+/*
+ *      intel-mid_wdt: generic Intel MID SCU watchdog driver
+ *
+ *      Platforms supported so far:
+ *      - Merrifield only
+ *
+ *      Copyright (C) 2014 Intel Corporation. All rights reserved.
+ *      Contact: David Cohen <david.a.cohen@linux.intel.com>
+ *
+ *      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.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/nmi.h>
+#include <linux/platform_device.h>
+#include <linux/watchdog.h>
+#include <linux/platform_data/intel-mid_wdt.h>
+
+#include <asm/intel_scu_ipc.h>
+#include <asm/intel-mid.h>
+
+#define IPC_WATCHDOG 0xf8
+
+#define MID_WDT_PRETIMEOUT		15
+#define MID_WDT_TIMEOUT_MIN		(1 + MID_WDT_PRETIMEOUT)
+#define MID_WDT_TIMEOUT_MAX		170
+#define MID_WDT_DEFAULT_TIMEOUT		90
+
+/* SCU watchdog messages */
+enum {
+	SCU_WATCHDOG_START = 0,
+	SCU_WATCHDOG_STOP,
+	SCU_WATCHDOG_KEEPALIVE,
+};
+
+static inline int wdt_command(int sub, u32 *in, int inlen)
+{
+	return intel_scu_ipc_command(IPC_WATCHDOG, sub, in, inlen, NULL, 0);
+}
+
+static int wdt_start(struct watchdog_device *wd)
+{
+	int ret, in_size;
+	int timeout = wd->timeout;
+	struct ipc_wd_start {
+		u32 pretimeout;
+		u32 timeout;
+	} ipc_wd_start = { timeout - MID_WDT_PRETIMEOUT, timeout };
+
+	/*
+	 * SCU expects the input size for watchdog IPC to
+	 * be based on 4 bytes
+	 */
+	in_size = DIV_ROUND_UP(sizeof(ipc_wd_start), 4);
+
+	ret = wdt_command(SCU_WATCHDOG_START, (u32 *)&ipc_wd_start, in_size);
+	if (ret) {
+		struct device *dev = watchdog_get_drvdata(wd);
+		dev_crit(dev, "error starting watchdog: %d\n", ret);
+	}
+
+	return ret;
+}
+
+static int wdt_ping(struct watchdog_device *wd)
+{
+	int ret;
+
+	ret = wdt_command(SCU_WATCHDOG_KEEPALIVE, NULL, 0);
+	if (ret) {
+		struct device *dev = watchdog_get_drvdata(wd);
+		dev_crit(dev, "Error executing keepalive: 0x%x\n", ret);
+	}
+
+	return ret;
+}
+
+static int wdt_stop(struct watchdog_device *wd)
+{
+	int ret;
+
+	ret = wdt_command(SCU_WATCHDOG_STOP, NULL, 0);
+	if (ret) {
+		struct device *dev = watchdog_get_drvdata(wd);
+		dev_crit(dev, "Error stopping watchdog: 0x%x\n", ret);
+	}
+
+	return ret;
+}
+
+static irqreturn_t mid_wdt_irq(int irq, void *dev_id)
+{
+	panic("Kernel Watchdog");
+
+	/* This code should not be reached */
+	return IRQ_HANDLED;
+}
+
+static const struct watchdog_info mid_wdt_info = {
+	.identity = "Intel MID SCU watchdog",
+	.options = WDIOF_KEEPALIVEPING | WDIOF_SETTIMEOUT,
+};
+
+static const struct watchdog_ops mid_wdt_ops = {
+	.owner = THIS_MODULE,
+	.start = wdt_start,
+	.stop = wdt_stop,
+	.ping = wdt_ping,
+};
+
+static int mid_wdt_probe(struct platform_device *pdev)
+{
+	struct watchdog_device *wdt_dev;
+	struct intel_mid_wdt_pdata *pdata = pdev->dev.platform_data;
+	int ret;
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "missing platform data\n");
+		return -EINVAL;
+	}
+
+	if (pdata->probe) {
+		ret = pdata->probe(pdev);
+		if (ret)
+			return ret;
+	}
+
+	wdt_dev = devm_kzalloc(&pdev->dev, sizeof(*wdt_dev), GFP_KERNEL);
+	if (!wdt_dev)
+		return -ENOMEM;
+
+	wdt_dev->info = &mid_wdt_info;
+	wdt_dev->ops = &mid_wdt_ops;
+	wdt_dev->min_timeout = MID_WDT_TIMEOUT_MIN;
+	wdt_dev->max_timeout = MID_WDT_TIMEOUT_MAX;
+	wdt_dev->timeout = MID_WDT_DEFAULT_TIMEOUT;
+
+	watchdog_set_drvdata(wdt_dev, &pdev->dev);
+	platform_set_drvdata(pdev, wdt_dev);
+
+	ret = devm_request_irq(&pdev->dev, pdata->irq, mid_wdt_irq,
+			       IRQF_SHARED | IRQF_NO_SUSPEND, "watchdog",
+			       wdt_dev);
+	if (ret) {
+		dev_err(&pdev->dev, "error requesting warning irq %d\n",
+			pdata->irq);
+		return ret;
+	}
+
+	ret = watchdog_register_device(wdt_dev);
+	if (ret) {
+		dev_err(&pdev->dev, "error registering watchdog device\n");
+		return ret;
+	}
+
+	dev_info(&pdev->dev, "Intel MID watchdog device probed\n");
+
+	return 0;
+}
+
+static int mid_wdt_remove(struct platform_device *pdev)
+{
+	struct watchdog_device *wd = platform_get_drvdata(pdev);
+	watchdog_unregister_device(wd);
+	return 0;
+}
+
+static struct platform_driver mid_wdt_driver = {
+	.probe		= mid_wdt_probe,
+	.remove		= mid_wdt_remove,
+	.driver		= {
+		.owner	= THIS_MODULE,
+		.name	= "intel_mid_wdt",
+	},
+};
+
+module_platform_driver(mid_wdt_driver);
+
+MODULE_AUTHOR("David Cohen <david.a.cohen@linux.intel.com>");
+MODULE_DESCRIPTION("Watchdog Driver for Intel MID platform");
+MODULE_LICENSE("GPL");
diff --git a/drivers/watchdog/kempld_wdt.c b/drivers/watchdog/kempld_wdt.c
index 20dc7384473..d9c1a160192 100644
--- a/drivers/watchdog/kempld_wdt.c
+++ b/drivers/watchdog/kempld_wdt.c
@@ -162,7 +162,7 @@ static int kempld_wdt_set_stage_timeout(struct kempld_wdt_data *wdt_data,
 	kempld_get_mutex(pld);
 	stage_cfg = kempld_read8(pld, KEMPLD_WDT_STAGE_CFG(stage->id));
 	stage_cfg &= ~STAGE_CFG_PRESCALER_MASK;
-	stage_cfg |= STAGE_CFG_SET_PRESCALER(prescaler);
+	stage_cfg |= STAGE_CFG_SET_PRESCALER(PRESCALER_21);
 	kempld_write8(pld, KEMPLD_WDT_STAGE_CFG(stage->id), stage_cfg);
 	kempld_write32(pld, KEMPLD_WDT_STAGE_TIMEOUT(stage->id),
 			stage_timeout);
diff --git a/drivers/watchdog/of_xilinx_wdt.c b/drivers/watchdog/of_xilinx_wdt.c
index 57ccae8327f..1e6e28df5d7 100644
--- a/drivers/watchdog/of_xilinx_wdt.c
+++ b/drivers/watchdog/of_xilinx_wdt.c
@@ -225,7 +225,7 @@ static int xwdt_remove(struct platform_device *pdev)
 }
 
 /* Match table for of_platform binding */
-static struct of_device_id xwdt_of_match[] = {
+static const struct of_device_id xwdt_of_match[] = {
 	{ .compatible = "xlnx,xps-timebase-wdt-1.00.a", },
 	{ .compatible = "xlnx,xps-timebase-wdt-1.01.a", },
 	{},
diff --git a/drivers/watchdog/orion_wdt.c b/drivers/watchdog/orion_wdt.c
index 9b3c41d1870..00d0741228f 100644
--- a/drivers/watchdog/orion_wdt.c
+++ b/drivers/watchdog/orion_wdt.c
@@ -55,15 +55,19 @@ struct orion_watchdog_data {
 	int wdt_counter_offset;
 	int wdt_enable_bit;
 	int rstout_enable_bit;
+	int rstout_mask_bit;
 	int (*clock_init)(struct platform_device *,
 			  struct orion_watchdog *);
+	int (*enabled)(struct orion_watchdog *);
 	int (*start)(struct watchdog_device *);
+	int (*stop)(struct watchdog_device *);
 };
 
 struct orion_watchdog {
 	struct watchdog_device wdt;
 	void __iomem *reg;
 	void __iomem *rstout;
+	void __iomem *rstout_mask;
 	unsigned long clk_rate;
 	struct clk *clk;
 	const struct orion_watchdog_data *data;
@@ -142,9 +146,35 @@ static int orion_wdt_ping(struct watchdog_device *wdt_dev)
 	return 0;
 }
 
+static int armada375_start(struct watchdog_device *wdt_dev)
+{
+	struct orion_watchdog *dev = watchdog_get_drvdata(wdt_dev);
+	u32 reg;
+
+	/* Set watchdog duration */
+	writel(dev->clk_rate * wdt_dev->timeout,
+	       dev->reg + dev->data->wdt_counter_offset);
+
+	/* Clear the watchdog expiration bit */
+	atomic_io_modify(dev->reg + TIMER_A370_STATUS, WDT_A370_EXPIRED, 0);
+
+	/* Enable watchdog timer */
+	atomic_io_modify(dev->reg + TIMER_CTRL, dev->data->wdt_enable_bit,
+						dev->data->wdt_enable_bit);
+
+	/* Enable reset on watchdog */
+	reg = readl(dev->rstout);
+	reg |= dev->data->rstout_enable_bit;
+	writel(reg, dev->rstout);
+
+	atomic_io_modify(dev->rstout_mask, dev->data->rstout_mask_bit, 0);
+	return 0;
+}
+
 static int armada370_start(struct watchdog_device *wdt_dev)
 {
 	struct orion_watchdog *dev = watchdog_get_drvdata(wdt_dev);
+	u32 reg;
 
 	/* Set watchdog duration */
 	writel(dev->clk_rate * wdt_dev->timeout,
@@ -157,8 +187,10 @@ static int armada370_start(struct watchdog_device *wdt_dev)
 	atomic_io_modify(dev->reg + TIMER_CTRL, dev->data->wdt_enable_bit,
 						dev->data->wdt_enable_bit);
 
-	atomic_io_modify(dev->rstout, dev->data->rstout_enable_bit,
-				      dev->data->rstout_enable_bit);
+	/* Enable reset on watchdog */
+	reg = readl(dev->rstout);
+	reg |= dev->data->rstout_enable_bit;
+	writel(reg, dev->rstout);
 	return 0;
 }
 
@@ -189,7 +221,7 @@ static int orion_wdt_start(struct watchdog_device *wdt_dev)
 	return dev->data->start(wdt_dev);
 }
 
-static int orion_wdt_stop(struct watchdog_device *wdt_dev)
+static int orion_stop(struct watchdog_device *wdt_dev)
 {
 	struct orion_watchdog *dev = watchdog_get_drvdata(wdt_dev);
 
@@ -202,7 +234,48 @@ static int orion_wdt_stop(struct watchdog_device *wdt_dev)
 	return 0;
 }
 
-static int orion_wdt_enabled(struct orion_watchdog *dev)
+static int armada375_stop(struct watchdog_device *wdt_dev)
+{
+	struct orion_watchdog *dev = watchdog_get_drvdata(wdt_dev);
+	u32 reg;
+
+	/* Disable reset on watchdog */
+	atomic_io_modify(dev->rstout_mask, dev->data->rstout_mask_bit,
+					   dev->data->rstout_mask_bit);
+	reg = readl(dev->rstout);
+	reg &= ~dev->data->rstout_enable_bit;
+	writel(reg, dev->rstout);
+
+	/* Disable watchdog timer */
+	atomic_io_modify(dev->reg + TIMER_CTRL, dev->data->wdt_enable_bit, 0);
+
+	return 0;
+}
+
+static int armada370_stop(struct watchdog_device *wdt_dev)
+{
+	struct orion_watchdog *dev = watchdog_get_drvdata(wdt_dev);
+	u32 reg;
+
+	/* Disable reset on watchdog */
+	reg = readl(dev->rstout);
+	reg &= ~dev->data->rstout_enable_bit;
+	writel(reg, dev->rstout);
+
+	/* Disable watchdog timer */
+	atomic_io_modify(dev->reg + TIMER_CTRL, dev->data->wdt_enable_bit, 0);
+
+	return 0;
+}
+
+static int orion_wdt_stop(struct watchdog_device *wdt_dev)
+{
+	struct orion_watchdog *dev = watchdog_get_drvdata(wdt_dev);
+
+	return dev->data->stop(wdt_dev);
+}
+
+static int orion_enabled(struct orion_watchdog *dev)
 {
 	bool enabled, running;
 
@@ -212,6 +285,24 @@ static int orion_wdt_enabled(struct orion_watchdog *dev)
 	return enabled && running;
 }
 
+static int armada375_enabled(struct orion_watchdog *dev)
+{
+	bool masked, enabled, running;
+
+	masked = readl(dev->rstout_mask) & dev->data->rstout_mask_bit;
+	enabled = readl(dev->rstout) & dev->data->rstout_enable_bit;
+	running = readl(dev->reg + TIMER_CTRL) & dev->data->wdt_enable_bit;
+
+	return !masked && enabled && running;
+}
+
+static int orion_wdt_enabled(struct watchdog_device *wdt_dev)
+{
+	struct orion_watchdog *dev = watchdog_get_drvdata(wdt_dev);
+
+	return dev->data->enabled(dev);
+}
+
 static unsigned int orion_wdt_get_timeleft(struct watchdog_device *wdt_dev)
 {
 	struct orion_watchdog *dev = watchdog_get_drvdata(wdt_dev);
@@ -262,10 +353,6 @@ static void __iomem *orion_wdt_ioremap_rstout(struct platform_device *pdev,
 		return devm_ioremap(&pdev->dev, res->start,
 				    resource_size(res));
 
-	/* This workaround works only for "orion-wdt", DT-enabled */
-	if (!of_device_is_compatible(pdev->dev.of_node, "marvell,orion-wdt"))
-		return NULL;
-
 	rstout = internal_regs + ORION_RSTOUT_MASK_OFFSET;
 
 	WARN(1, FW_BUG "falling back to harcoded RSTOUT reg %pa\n", &rstout);
@@ -277,7 +364,9 @@ static const struct orion_watchdog_data orion_data = {
 	.wdt_enable_bit = BIT(4),
 	.wdt_counter_offset = 0x24,
 	.clock_init = orion_wdt_clock_init,
+	.enabled = orion_enabled,
 	.start = orion_start,
+	.stop = orion_stop,
 };
 
 static const struct orion_watchdog_data armada370_data = {
@@ -285,7 +374,9 @@ static const struct orion_watchdog_data armada370_data = {
 	.wdt_enable_bit = BIT(8),
 	.wdt_counter_offset = 0x34,
 	.clock_init = armada370_wdt_clock_init,
+	.enabled = orion_enabled,
 	.start = armada370_start,
+	.stop = armada370_stop,
 };
 
 static const struct orion_watchdog_data armadaxp_data = {
@@ -293,7 +384,31 @@ static const struct orion_watchdog_data armadaxp_data = {
 	.wdt_enable_bit = BIT(8),
 	.wdt_counter_offset = 0x34,
 	.clock_init = armadaxp_wdt_clock_init,
+	.enabled = orion_enabled,
 	.start = armada370_start,
+	.stop = armada370_stop,
+};
+
+static const struct orion_watchdog_data armada375_data = {
+	.rstout_enable_bit = BIT(8),
+	.rstout_mask_bit = BIT(10),
+	.wdt_enable_bit = BIT(8),
+	.wdt_counter_offset = 0x34,
+	.clock_init = armada370_wdt_clock_init,
+	.enabled = armada375_enabled,
+	.start = armada375_start,
+	.stop = armada375_stop,
+};
+
+static const struct orion_watchdog_data armada380_data = {
+	.rstout_enable_bit = BIT(8),
+	.rstout_mask_bit = BIT(10),
+	.wdt_enable_bit = BIT(8),
+	.wdt_counter_offset = 0x34,
+	.clock_init = armadaxp_wdt_clock_init,
+	.enabled = armada375_enabled,
+	.start = armada375_start,
+	.stop = armada375_stop,
 };
 
 static const struct of_device_id orion_wdt_of_match_table[] = {
@@ -309,16 +424,78 @@ static const struct of_device_id orion_wdt_of_match_table[] = {
 		.compatible = "marvell,armada-xp-wdt",
 		.data = &armadaxp_data,
 	},
+	{
+		.compatible = "marvell,armada-375-wdt",
+		.data = &armada375_data,
+	},
+	{
+		.compatible = "marvell,armada-380-wdt",
+		.data = &armada380_data,
+	},
 	{},
 };
 MODULE_DEVICE_TABLE(of, orion_wdt_of_match_table);
 
+static int orion_wdt_get_regs(struct platform_device *pdev,
+			      struct orion_watchdog *dev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct resource *res;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENODEV;
+	dev->reg = devm_ioremap(&pdev->dev, res->start,
+				resource_size(res));
+	if (!dev->reg)
+		return -ENOMEM;
+
+	/* Each supported compatible has some RSTOUT register quirk */
+	if (of_device_is_compatible(node, "marvell,orion-wdt")) {
+
+		dev->rstout = orion_wdt_ioremap_rstout(pdev, res->start &
+						       INTERNAL_REGS_MASK);
+		if (!dev->rstout)
+			return -ENODEV;
+
+	} else if (of_device_is_compatible(node, "marvell,armada-370-wdt") ||
+		   of_device_is_compatible(node, "marvell,armada-xp-wdt")) {
+
+		/* Dedicated RSTOUT register, can be requested. */
+		res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+		dev->rstout = devm_ioremap_resource(&pdev->dev, res);
+		if (IS_ERR(dev->rstout))
+			return PTR_ERR(dev->rstout);
+
+	} else if (of_device_is_compatible(node, "marvell,armada-375-wdt") ||
+		   of_device_is_compatible(node, "marvell,armada-380-wdt")) {
+
+		/* Dedicated RSTOUT register, can be requested. */
+		res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+		dev->rstout = devm_ioremap_resource(&pdev->dev, res);
+		if (IS_ERR(dev->rstout))
+			return PTR_ERR(dev->rstout);
+
+		res = platform_get_resource(pdev, IORESOURCE_MEM, 2);
+		if (!res)
+			return -ENODEV;
+		dev->rstout_mask = devm_ioremap(&pdev->dev, res->start,
+						resource_size(res));
+		if (!dev->rstout_mask)
+			return -ENOMEM;
+
+	} else {
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
 static int orion_wdt_probe(struct platform_device *pdev)
 {
 	struct orion_watchdog *dev;
 	const struct of_device_id *match;
 	unsigned int wdt_max_duration;	/* (seconds) */
-	struct resource *res;
 	int ret, irq;
 
 	dev = devm_kzalloc(&pdev->dev, sizeof(struct orion_watchdog),
@@ -336,19 +513,9 @@ static int orion_wdt_probe(struct platform_device *pdev)
 	dev->wdt.min_timeout = 1;
 	dev->data = match->data;
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res)
-		return -ENODEV;
-
-	dev->reg = devm_ioremap(&pdev->dev, res->start,
-			       resource_size(res));
-	if (!dev->reg)
-		return -ENOMEM;
-
-	dev->rstout = orion_wdt_ioremap_rstout(pdev, res->start &
-						     INTERNAL_REGS_MASK);
-	if (!dev->rstout)
-		return -ENODEV;
+	ret = orion_wdt_get_regs(pdev, dev);
+	if (ret)
+		return ret;
 
 	ret = dev->data->clock_init(pdev, dev);
 	if (ret) {
@@ -371,7 +538,7 @@ static int orion_wdt_probe(struct platform_device *pdev)
 	 * removed and re-insterted, or if the bootloader explicitly
 	 * set a running watchdog before booting the kernel.
 	 */
-	if (!orion_wdt_enabled(dev))
+	if (!orion_wdt_enabled(&dev->wdt))
 		orion_wdt_stop(&dev->wdt);
 
 	/* Request the IRQ only after the watchdog is disabled */
diff --git a/drivers/watchdog/shwdt.c b/drivers/watchdog/shwdt.c
index d04d02b41c3..061756e36cf 100644
--- a/drivers/watchdog/shwdt.c
+++ b/drivers/watchdog/shwdt.c
@@ -282,8 +282,6 @@ static int sh_wdt_probe(struct platform_device *pdev)
 	wdt->timer.data		= (unsigned long)wdt;
 	wdt->timer.expires	= next_ping_period(clock_division_ratio);
 
-	platform_set_drvdata(pdev, wdt);
-
 	dev_info(&pdev->dev, "initialized.\n");
 
 	pm_runtime_enable(&pdev->dev);
diff --git a/drivers/watchdog/sp805_wdt.c b/drivers/watchdog/sp805_wdt.c
index 47629d268e0..c1b03f4235b 100644
--- a/drivers/watchdog/sp805_wdt.c
+++ b/drivers/watchdog/sp805_wdt.c
@@ -59,7 +59,6 @@
  * @adev: amba device structure of wdt
  * @status: current status of wdt
  * @load_val: load value to be set for current timeout
- * @timeout: current programmed timeout
  */
 struct sp805_wdt {
 	struct watchdog_device		wdd;
@@ -68,7 +67,6 @@ struct sp805_wdt {
 	struct clk			*clk;
 	struct amba_device		*adev;
 	unsigned int			load_val;
-	unsigned int			timeout;
 };
 
 static bool nowayout = WATCHDOG_NOWAYOUT;
@@ -98,7 +96,7 @@ static int wdt_setload(struct watchdog_device *wdd, unsigned int timeout)
 	spin_lock(&wdt->lock);
 	wdt->load_val = load;
 	/* roundup timeout to closest positive integer value */
-	wdt->timeout = div_u64((load + 1) * 2 + (rate / 2), rate);
+	wdd->timeout = div_u64((load + 1) * 2 + (rate / 2), rate);
 	spin_unlock(&wdt->lock);
 
 	return 0;
diff --git a/drivers/watchdog/sunxi_wdt.c b/drivers/watchdog/sunxi_wdt.c
index cd00a7836cd..693b9d2c6e3 100644
--- a/drivers/watchdog/sunxi_wdt.c
+++ b/drivers/watchdog/sunxi_wdt.c
@@ -57,17 +57,17 @@ struct sunxi_wdt_dev {
  */
 
 static const int wdt_timeout_map[] = {
-	[1] = 0b0001,  /* 1s  */
-	[2] = 0b0010,  /* 2s  */
-	[3] = 0b0011,  /* 3s  */
-	[4] = 0b0100,  /* 4s  */
-	[5] = 0b0101,  /* 5s  */
-	[6] = 0b0110,  /* 6s  */
-	[8] = 0b0111,  /* 8s  */
-	[10] = 0b1000, /* 10s */
-	[12] = 0b1001, /* 12s */
-	[14] = 0b1010, /* 14s */
-	[16] = 0b1011, /* 16s */
+	[1] = 0x1,  /* 1s  */
+	[2] = 0x2,  /* 2s  */
+	[3] = 0x3,  /* 3s  */
+	[4] = 0x4,  /* 4s  */
+	[5] = 0x5,  /* 5s  */
+	[6] = 0x6,  /* 6s  */
+	[8] = 0x7,  /* 8s  */
+	[10] = 0x8, /* 10s */
+	[12] = 0x9, /* 12s */
+	[14] = 0xA, /* 14s */
+	[16] = 0xB, /* 16s */
 };
 
 static int sunxi_wdt_ping(struct watchdog_device *wdt_dev)
diff --git a/drivers/watchdog/via_wdt.c b/drivers/watchdog/via_wdt.c
index d2cd9f0bcb9..56369c4f196 100644
--- a/drivers/watchdog/via_wdt.c
+++ b/drivers/watchdog/via_wdt.c
@@ -232,7 +232,7 @@ err_out_disable_device:
 static void wdt_remove(struct pci_dev *pdev)
 {
 	watchdog_unregister_device(&wdt_dev);
-	del_timer(&timer);
+	del_timer_sync(&timer);
 	iounmap(wdt_mem);
 	release_mem_region(mmio, VIA_WDT_MMIO_LEN);
 	release_resource(&wdt_res);
diff --git a/drivers/watchdog/w83627hf_wdt.c b/drivers/watchdog/w83627hf_wdt.c
index b1da0c18fd1..7165704a3e3 100644
--- a/drivers/watchdog/w83627hf_wdt.c
+++ b/drivers/watchdog/w83627hf_wdt.c
@@ -64,6 +64,10 @@ MODULE_PARM_DESC(nowayout,
 		"Watchdog cannot be stopped once started (default="
 				__MODULE_STRING(WATCHDOG_NOWAYOUT) ")");
 
+static int early_disable;
+module_param(early_disable, int, 0);
+MODULE_PARM_DESC(early_disable, "Disable watchdog at boot time (default=0)");
+
 /*
  *	Kernel methods.
  */
@@ -208,9 +212,14 @@ static int w83627hf_init(struct watchdog_device *wdog, enum chips chip)
 
 	t = superio_inb(cr_wdt_timeout);
 	if (t != 0) {
-		pr_info("Watchdog already running. Resetting timeout to %d sec\n",
-			wdog->timeout);
-		superio_outb(cr_wdt_timeout, wdog->timeout);
+		if (early_disable) {
+			pr_warn("Stopping previously enabled watchdog until userland kicks in\n");
+			superio_outb(cr_wdt_timeout, 0);
+		} else {
+			pr_info("Watchdog already running. Resetting timeout to %d sec\n",
+				wdog->timeout);
+			superio_outb(cr_wdt_timeout, wdog->timeout);
+		}
 	}
 
 	/* set second mode & disable keyboard turning off watchdog */
diff --git a/drivers/watchdog/w83697hf_wdt.c b/drivers/watchdog/w83697hf_wdt.c
deleted file mode 100644
index e9ea856b8ff..00000000000
--- a/drivers/watchdog/w83697hf_wdt.c
+++ /dev/null
@@ -1,460 +0,0 @@
-/*
- *	w83697hf/hg WDT driver
- *
- *	(c) Copyright 2006 Samuel Tardieu <sam@rfc1149.net>
- *	(c) Copyright 2006 Marcus Junker <junker@anduras.de>
- *
- *	Based on w83627hf_wdt.c which is based on advantechwdt.c
- *	which is based on wdt.c.
- *	Original copyright messages:
- *
- *	(c) Copyright 2003 Pádraig Brady <P@draigBrady.com>
- *
- *	(c) Copyright 2000-2001 Marek Michalkiewicz <marekm@linux.org.pl>
- *
- *	(c) Copyright 1996 Alan Cox <alan@lxorguk.ukuu.org.uk>,
- *						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.
- *
- *	Neither Marcus Junker nor ANDURAS AG admit liability nor provide
- *	warranty for any of this software. This material is provided
- *	"AS-IS" and at no charge.
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/module.h>
-#include <linux/moduleparam.h>
-#include <linux/types.h>
-#include <linux/miscdevice.h>
-#include <linux/watchdog.h>
-#include <linux/fs.h>
-#include <linux/ioport.h>
-#include <linux/notifier.h>
-#include <linux/reboot.h>
-#include <linux/init.h>
-#include <linux/spinlock.h>
-#include <linux/io.h>
-#include <linux/uaccess.h>
-
-
-#define WATCHDOG_NAME "w83697hf/hg WDT"
-#define WATCHDOG_TIMEOUT 60		/* 60 sec default timeout */
-#define WATCHDOG_EARLY_DISABLE 1	/* Disable until userland kicks in */
-
-static unsigned long wdt_is_open;
-static char expect_close;
-static DEFINE_SPINLOCK(io_lock);
-
-/* You must set this - there is no sane way to probe for this board. */
-static int wdt_io = 0x2e;
-module_param(wdt_io, int, 0);
-MODULE_PARM_DESC(wdt_io,
-		"w83697hf/hg WDT io port (default 0x2e, 0 = autodetect)");
-
-static int timeout = WATCHDOG_TIMEOUT;	/* in seconds */
-module_param(timeout, int, 0);
-MODULE_PARM_DESC(timeout,
-	"Watchdog timeout in seconds. 1<= timeout <=255 (default="
-				__MODULE_STRING(WATCHDOG_TIMEOUT) ")");
-
-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) ")");
-
-static int early_disable = WATCHDOG_EARLY_DISABLE;
-module_param(early_disable, int, 0);
-MODULE_PARM_DESC(early_disable,
-	"Watchdog gets disabled at boot time (default="
-				__MODULE_STRING(WATCHDOG_EARLY_DISABLE) ")");
-
-/*
- *	Kernel methods.
- */
-
-#define W83697HF_EFER (wdt_io + 0)  /* Extended Function Enable Register */
-#define W83697HF_EFIR (wdt_io + 0)  /* Extended Function Index Register
-							(same as EFER) */
-#define W83697HF_EFDR (wdt_io + 1)  /* Extended Function Data Register */
-
-static inline void w83697hf_unlock(void)
-{
-	outb_p(0x87, W83697HF_EFER);	/* Enter extended function mode */
-	outb_p(0x87, W83697HF_EFER);	/* Again according to manual */
-}
-
-static inline void w83697hf_lock(void)
-{
-	outb_p(0xAA, W83697HF_EFER);	/* Leave extended function mode */
-}
-
-/*
- *	The three functions w83697hf_get_reg(), w83697hf_set_reg() and
- *	w83697hf_write_timeout() must be called with the device unlocked.
- */
-
-static unsigned char w83697hf_get_reg(unsigned char reg)
-{
-	outb_p(reg, W83697HF_EFIR);
-	return inb_p(W83697HF_EFDR);
-}
-
-static void w83697hf_set_reg(unsigned char reg, unsigned char data)
-{
-	outb_p(reg, W83697HF_EFIR);
-	outb_p(data, W83697HF_EFDR);
-}
-
-static void w83697hf_write_timeout(int timeout)
-{
-	/* Write Timeout counter to CRF4 */
-	w83697hf_set_reg(0xF4, timeout);
-}
-
-static void w83697hf_select_wdt(void)
-{
-	w83697hf_unlock();
-	w83697hf_set_reg(0x07, 0x08);	/* Switch to logic device 8 (GPIO2) */
-}
-
-static inline void w83697hf_deselect_wdt(void)
-{
-	w83697hf_lock();
-}
-
-static void w83697hf_init(void)
-{
-	unsigned char bbuf;
-
-	w83697hf_select_wdt();
-
-	bbuf = w83697hf_get_reg(0x29);
-	bbuf &= ~0x60;
-	bbuf |= 0x20;
-
-	/* Set pin 119 to WDTO# mode (= CR29, WDT0) */
-	w83697hf_set_reg(0x29, bbuf);
-
-	bbuf = w83697hf_get_reg(0xF3);
-	bbuf &= ~0x04;
-	w83697hf_set_reg(0xF3, bbuf);	/* Count mode is seconds */
-
-	w83697hf_deselect_wdt();
-}
-
-static void wdt_ping(void)
-{
-	spin_lock(&io_lock);
-	w83697hf_select_wdt();
-
-	w83697hf_write_timeout(timeout);
-
-	w83697hf_deselect_wdt();
-	spin_unlock(&io_lock);
-}
-
-static void wdt_enable(void)
-{
-	spin_lock(&io_lock);
-	w83697hf_select_wdt();
-
-	w83697hf_write_timeout(timeout);
-	w83697hf_set_reg(0x30, 1);	/* Enable timer */
-
-	w83697hf_deselect_wdt();
-	spin_unlock(&io_lock);
-}
-
-static void wdt_disable(void)
-{
-	spin_lock(&io_lock);
-	w83697hf_select_wdt();
-
-	w83697hf_set_reg(0x30, 0);	/* Disable timer */
-	w83697hf_write_timeout(0);
-
-	w83697hf_deselect_wdt();
-	spin_unlock(&io_lock);
-}
-
-static unsigned char wdt_running(void)
-{
-	unsigned char t;
-
-	spin_lock(&io_lock);
-	w83697hf_select_wdt();
-
-	t = w83697hf_get_reg(0xF4);	/* Read timer */
-
-	w83697hf_deselect_wdt();
-	spin_unlock(&io_lock);
-
-	return t;
-}
-
-static int wdt_set_heartbeat(int t)
-{
-	if (t < 1 || t > 255)
-		return -EINVAL;
-
-	timeout = t;
-	return 0;
-}
-
-static ssize_t wdt_write(struct file *file, const char __user *buf,
-						size_t count, loff_t *ppos)
-{
-	if (count) {
-		if (!nowayout) {
-			size_t i;
-
-			expect_close = 0;
-
-			for (i = 0; i != count; i++) {
-				char c;
-				if (get_user(c, buf + i))
-					return -EFAULT;
-				if (c == 'V')
-					expect_close = 42;
-			}
-		}
-		wdt_ping();
-	}
-	return count;
-}
-
-static long wdt_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
-{
-	void __user *argp = (void __user *)arg;
-	int __user *p = argp;
-	int new_timeout;
-	static const struct watchdog_info ident = {
-		.options = WDIOF_KEEPALIVEPING | WDIOF_SETTIMEOUT
-							| WDIOF_MAGICCLOSE,
-		.firmware_version = 1,
-		.identity = "W83697HF WDT",
-	};
-
-	switch (cmd) {
-	case WDIOC_GETSUPPORT:
-		if (copy_to_user(argp, &ident, sizeof(ident)))
-			return -EFAULT;
-		break;
-
-	case WDIOC_GETSTATUS:
-	case WDIOC_GETBOOTSTATUS:
-		return put_user(0, p);
-
-	case WDIOC_SETOPTIONS:
-	{
-		int options, retval = -EINVAL;
-
-		if (get_user(options, p))
-			return -EFAULT;
-
-		if (options & WDIOS_DISABLECARD) {
-			wdt_disable();
-			retval = 0;
-		}
-
-		if (options & WDIOS_ENABLECARD) {
-			wdt_enable();
-			retval = 0;
-		}
-
-		return retval;
-	}
-
-	case WDIOC_KEEPALIVE:
-		wdt_ping();
-		break;
-
-	case WDIOC_SETTIMEOUT:
-		if (get_user(new_timeout, p))
-			return -EFAULT;
-		if (wdt_set_heartbeat(new_timeout))
-			return -EINVAL;
-		wdt_ping();
-		/* Fall */
-
-	case WDIOC_GETTIMEOUT:
-		return put_user(timeout, p);
-
-	default:
-		return -ENOTTY;
-	}
-	return 0;
-}
-
-static int wdt_open(struct inode *inode, struct file *file)
-{
-	if (test_and_set_bit(0, &wdt_is_open))
-		return -EBUSY;
-	/*
-	 *	Activate
-	 */
-
-	wdt_enable();
-	return nonseekable_open(inode, file);
-}
-
-static int wdt_close(struct inode *inode, struct file *file)
-{
-	if (expect_close == 42)
-		wdt_disable();
-	else {
-		pr_crit("Unexpected close, not stopping watchdog!\n");
-		wdt_ping();
-	}
-	expect_close = 0;
-	clear_bit(0, &wdt_is_open);
-	return 0;
-}
-
-/*
- *	Notifier for system down
- */
-
-static int wdt_notify_sys(struct notifier_block *this, unsigned long code,
-	void *unused)
-{
-	if (code == SYS_DOWN || code == SYS_HALT)
-		wdt_disable();	/* Turn the WDT off */
-
-	return NOTIFY_DONE;
-}
-
-/*
- *	Kernel Interfaces
- */
-
-static const struct file_operations wdt_fops = {
-	.owner		= THIS_MODULE,
-	.llseek		= no_llseek,
-	.write		= wdt_write,
-	.unlocked_ioctl	= wdt_ioctl,
-	.open		= wdt_open,
-	.release	= wdt_close,
-};
-
-static struct miscdevice wdt_miscdev = {
-	.minor = WATCHDOG_MINOR,
-	.name = "watchdog",
-	.fops = &wdt_fops,
-};
-
-/*
- *	The WDT needs to learn about soft shutdowns in order to
- *	turn the timebomb registers off.
- */
-
-static struct notifier_block wdt_notifier = {
-	.notifier_call = wdt_notify_sys,
-};
-
-static int w83697hf_check_wdt(void)
-{
-	if (!request_region(wdt_io, 2, WATCHDOG_NAME)) {
-		pr_err("I/O address 0x%x already in use\n", wdt_io);
-		return -EIO;
-	}
-
-	pr_debug("Looking for watchdog at address 0x%x\n", wdt_io);
-	w83697hf_unlock();
-	if (w83697hf_get_reg(0x20) == 0x60) {
-		pr_info("watchdog found at address 0x%x\n", wdt_io);
-		w83697hf_lock();
-		return 0;
-	}
-	/* Reprotect in case it was a compatible device */
-	w83697hf_lock();
-
-	pr_info("watchdog not found at address 0x%x\n", wdt_io);
-	release_region(wdt_io, 2);
-	return -EIO;
-}
-
-static int w83697hf_ioports[] = { 0x2e, 0x4e, 0x00 };
-
-static int __init wdt_init(void)
-{
-	int ret, i, found = 0;
-
-	pr_info("WDT driver for W83697HF/HG initializing\n");
-
-	if (wdt_io == 0) {
-		/* we will autodetect the W83697HF/HG watchdog */
-		for (i = 0; ((!found) && (w83697hf_ioports[i] != 0)); i++) {
-			wdt_io = w83697hf_ioports[i];
-			if (!w83697hf_check_wdt())
-				found++;
-		}
-	} else {
-		if (!w83697hf_check_wdt())
-			found++;
-	}
-
-	if (!found) {
-		pr_err("No W83697HF/HG could be found\n");
-		ret = -ENODEV;
-		goto out;
-	}
-
-	w83697hf_init();
-	if (early_disable) {
-		if (wdt_running())
-			pr_warn("Stopping previously enabled watchdog until userland kicks in\n");
-		wdt_disable();
-	}
-
-	if (wdt_set_heartbeat(timeout)) {
-		wdt_set_heartbeat(WATCHDOG_TIMEOUT);
-		pr_info("timeout value must be 1 <= timeout <= 255, using %d\n",
-			WATCHDOG_TIMEOUT);
-	}
-
-	ret = register_reboot_notifier(&wdt_notifier);
-	if (ret != 0) {
-		pr_err("cannot register reboot notifier (err=%d)\n", ret);
-		goto unreg_regions;
-	}
-
-	ret = misc_register(&wdt_miscdev);
-	if (ret != 0) {
-		pr_err("cannot register miscdev on minor=%d (err=%d)\n",
-		       WATCHDOG_MINOR, ret);
-		goto unreg_reboot;
-	}
-
-	pr_info("initialized. timeout=%d sec (nowayout=%d)\n",
-		timeout, nowayout);
-
-out:
-	return ret;
-unreg_reboot:
-	unregister_reboot_notifier(&wdt_notifier);
-unreg_regions:
-	release_region(wdt_io, 2);
-	goto out;
-}
-
-static void __exit wdt_exit(void)
-{
-	misc_deregister(&wdt_miscdev);
-	unregister_reboot_notifier(&wdt_notifier);
-	release_region(wdt_io, 2);
-}
-
-module_init(wdt_init);
-module_exit(wdt_exit);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Marcus Junker <junker@anduras.de>");
-MODULE_AUTHOR("Samuel Tardieu <sam@rfc1149.net>");
-MODULE_DESCRIPTION("w83697hf/hg WDT driver");
diff --git a/drivers/watchdog/w83697ug_wdt.c b/drivers/watchdog/w83697ug_wdt.c
deleted file mode 100644
index ff58cb74671..00000000000
--- a/drivers/watchdog/w83697ug_wdt.c
+++ /dev/null
@@ -1,397 +0,0 @@
-/*
- *	w83697ug/uf WDT driver
- *
- *	(c) Copyright 2008 Flemming Fransen <ff@nrvissing.net>
- *		reused original code to support w83697ug/uf.
- *
- *	Based on w83627hf_wdt.c which is based on advantechwdt.c
- *	which is based on wdt.c.
- *	Original copyright messages:
- *
- *	(c) Copyright 2007 Vlad Drukker <vlad@storewiz.com>
- *		added support for W83627THF.
- *
- *	(c) Copyright 2003 Pádraig Brady <P@draigBrady.com>
- *
- *	(c) Copyright 2000-2001 Marek Michalkiewicz <marekm@linux.org.pl>
- *
- *	(c) Copyright 1996 Alan Cox <alan@redhat.com>, All Rights Reserved.
- *				http://www.redhat.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.
- *
- *	Neither Alan Cox nor CymruNet Ltd. admit liability nor provide
- *	warranty for any of this software. This material is provided
- *	"AS-IS" and at no charge.
- *
- *	(c) Copyright 1995    Alan Cox <alan@redhat.com>
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/module.h>
-#include <linux/moduleparam.h>
-#include <linux/types.h>
-#include <linux/miscdevice.h>
-#include <linux/watchdog.h>
-#include <linux/fs.h>
-#include <linux/ioport.h>
-#include <linux/notifier.h>
-#include <linux/reboot.h>
-#include <linux/init.h>
-#include <linux/spinlock.h>
-#include <linux/io.h>
-#include <linux/uaccess.h>
-
-
-#define WATCHDOG_NAME "w83697ug/uf WDT"
-#define WATCHDOG_TIMEOUT 60		/* 60 sec default timeout */
-
-static unsigned long wdt_is_open;
-static char expect_close;
-static DEFINE_SPINLOCK(io_lock);
-
-static int wdt_io = 0x2e;
-module_param(wdt_io, int, 0);
-MODULE_PARM_DESC(wdt_io, "w83697ug/uf WDT io port (default 0x2e)");
-
-static int timeout = WATCHDOG_TIMEOUT;	/* in seconds */
-module_param(timeout, int, 0);
-MODULE_PARM_DESC(timeout,
-	"Watchdog timeout in seconds. 1<= timeout <=255 (default="
-				__MODULE_STRING(WATCHDOG_TIMEOUT) ")");
-
-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) ")");
-
-/*
- *	Kernel methods.
- */
-
-#define WDT_EFER (wdt_io+0)   /* Extended Function Enable Registers */
-#define WDT_EFIR (wdt_io+0)   /* Extended Function Index Register
-							(same as EFER) */
-#define WDT_EFDR (WDT_EFIR+1) /* Extended Function Data Register */
-
-static int w83697ug_select_wd_register(void)
-{
-	unsigned char c;
-	unsigned char version;
-
-	outb_p(0x87, WDT_EFER); /* Enter extended function mode */
-	outb_p(0x87, WDT_EFER); /* Again according to manual */
-
-	outb(0x20, WDT_EFER);	/* check chip version	*/
-	version = inb(WDT_EFDR);
-
-	if (version == 0x68) {	/* W83697UG		*/
-		pr_info("Watchdog chip version 0x%02x = W83697UG/UF found at 0x%04x\n",
-			version, wdt_io);
-
-		outb_p(0x2b, WDT_EFER);
-		c = inb_p(WDT_EFDR);    /* select WDT0 */
-		c &= ~0x04;
-		outb_p(0x2b, WDT_EFER);
-		outb_p(c, WDT_EFDR);	/* set pin118 to WDT0 */
-
-	} else {
-		pr_err("No W83697UG/UF could be found\n");
-		return -ENODEV;
-	}
-
-	outb_p(0x07, WDT_EFER); /* point to logical device number reg */
-	outb_p(0x08, WDT_EFDR); /* select logical device 8 (GPIO2) */
-	outb_p(0x30, WDT_EFER); /* select CR30 */
-	c = inb_p(WDT_EFDR);
-	outb_p(c | 0x01, WDT_EFDR); /* set bit 0 to activate GPIO2 */
-
-	return 0;
-}
-
-static void w83697ug_unselect_wd_register(void)
-{
-	outb_p(0xAA, WDT_EFER); /* Leave extended function mode */
-}
-
-static int w83697ug_init(void)
-{
-	int ret;
-	unsigned char t;
-
-	ret = w83697ug_select_wd_register();
-	if (ret != 0)
-		return ret;
-
-	outb_p(0xF6, WDT_EFER); /* Select CRF6 */
-	t = inb_p(WDT_EFDR);    /* read CRF6 */
-	if (t != 0) {
-		pr_info("Watchdog already running. Resetting timeout to %d sec\n",
-			timeout);
-		outb_p(timeout, WDT_EFDR);    /* Write back to CRF6 */
-	}
-	outb_p(0xF5, WDT_EFER); /* Select CRF5 */
-	t = inb_p(WDT_EFDR);    /* read CRF5 */
-	t &= ~0x0C;             /* set second mode &
-					disable keyboard turning off watchdog */
-	outb_p(t, WDT_EFDR);    /* Write back to CRF5 */
-
-	w83697ug_unselect_wd_register();
-	return 0;
-}
-
-static void wdt_ctrl(int timeout)
-{
-	spin_lock(&io_lock);
-
-	if (w83697ug_select_wd_register() < 0) {
-		spin_unlock(&io_lock);
-		return;
-	}
-
-	outb_p(0xF4, WDT_EFER);    /* Select CRF4 */
-	outb_p(timeout, WDT_EFDR); /* Write Timeout counter to CRF4 */
-
-	w83697ug_unselect_wd_register();
-
-	spin_unlock(&io_lock);
-}
-
-static int wdt_ping(void)
-{
-	wdt_ctrl(timeout);
-	return 0;
-}
-
-static int wdt_disable(void)
-{
-	wdt_ctrl(0);
-	return 0;
-}
-
-static int wdt_set_heartbeat(int t)
-{
-	if (t < 1 || t > 255)
-		return -EINVAL;
-
-	timeout = t;
-	return 0;
-}
-
-static ssize_t wdt_write(struct file *file, const char __user *buf,
-						size_t count, loff_t *ppos)
-{
-	if (count) {
-		if (!nowayout) {
-			size_t i;
-
-			expect_close = 0;
-
-			for (i = 0; i != count; i++) {
-				char c;
-				if (get_user(c, buf + i))
-					return -EFAULT;
-				if (c == 'V')
-					expect_close = 42;
-			}
-		}
-		wdt_ping();
-	}
-	return count;
-}
-
-static long wdt_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
-{
-	void __user *argp = (void __user *)arg;
-	int __user *p = argp;
-	int new_timeout;
-	static const struct watchdog_info ident = {
-		.options =		WDIOF_KEEPALIVEPING |
-					WDIOF_SETTIMEOUT |
-					WDIOF_MAGICCLOSE,
-		.firmware_version =	1,
-		.identity =		"W83697UG WDT",
-	};
-
-	switch (cmd) {
-	case WDIOC_GETSUPPORT:
-		if (copy_to_user(argp, &ident, sizeof(ident)))
-			return -EFAULT;
-		break;
-
-	case WDIOC_GETSTATUS:
-	case WDIOC_GETBOOTSTATUS:
-		return put_user(0, p);
-
-	case WDIOC_SETOPTIONS:
-	{
-		int options, retval = -EINVAL;
-
-		if (get_user(options, p))
-			return -EFAULT;
-
-		if (options & WDIOS_DISABLECARD) {
-			wdt_disable();
-			retval = 0;
-		}
-
-		if (options & WDIOS_ENABLECARD) {
-			wdt_ping();
-			retval = 0;
-		}
-
-		return retval;
-	}
-
-	case WDIOC_KEEPALIVE:
-		wdt_ping();
-		break;
-
-	case WDIOC_SETTIMEOUT:
-		if (get_user(new_timeout, p))
-			return -EFAULT;
-		if (wdt_set_heartbeat(new_timeout))
-			return -EINVAL;
-		wdt_ping();
-		/* Fall */
-
-	case WDIOC_GETTIMEOUT:
-		return put_user(timeout, p);
-
-	default:
-		return -ENOTTY;
-	}
-	return 0;
-}
-
-static int wdt_open(struct inode *inode, struct file *file)
-{
-	if (test_and_set_bit(0, &wdt_is_open))
-		return -EBUSY;
-	/*
-	 *	Activate
-	 */
-
-	wdt_ping();
-	return nonseekable_open(inode, file);
-}
-
-static int wdt_close(struct inode *inode, struct file *file)
-{
-	if (expect_close == 42)
-		wdt_disable();
-	else {
-		pr_crit("Unexpected close, not stopping watchdog!\n");
-		wdt_ping();
-	}
-	expect_close = 0;
-	clear_bit(0, &wdt_is_open);
-	return 0;
-}
-
-/*
- *	Notifier for system down
- */
-
-static int wdt_notify_sys(struct notifier_block *this, unsigned long code,
-	void *unused)
-{
-	if (code == SYS_DOWN || code == SYS_HALT)
-		wdt_disable();	/* Turn the WDT off */
-
-	return NOTIFY_DONE;
-}
-
-/*
- *	Kernel Interfaces
- */
-
-static const struct file_operations wdt_fops = {
-	.owner		= THIS_MODULE,
-	.llseek		= no_llseek,
-	.write		= wdt_write,
-	.unlocked_ioctl	= wdt_ioctl,
-	.open		= wdt_open,
-	.release	= wdt_close,
-};
-
-static struct miscdevice wdt_miscdev = {
-	.minor = WATCHDOG_MINOR,
-	.name = "watchdog",
-	.fops = &wdt_fops,
-};
-
-/*
- *	The WDT needs to learn about soft shutdowns in order to
- *	turn the timebomb registers off.
- */
-
-static struct notifier_block wdt_notifier = {
-	.notifier_call = wdt_notify_sys,
-};
-
-static int __init wdt_init(void)
-{
-	int ret;
-
-	pr_info("WDT driver for the Winbond(TM) W83697UG/UF Super I/O chip initialising\n");
-
-	if (wdt_set_heartbeat(timeout)) {
-		wdt_set_heartbeat(WATCHDOG_TIMEOUT);
-		pr_info("timeout value must be 1<=timeout<=255, using %d\n",
-			WATCHDOG_TIMEOUT);
-	}
-
-	if (!request_region(wdt_io, 1, WATCHDOG_NAME)) {
-		pr_err("I/O address 0x%04x already in use\n", wdt_io);
-		ret = -EIO;
-		goto out;
-	}
-
-	ret = w83697ug_init();
-	if (ret != 0)
-		goto unreg_regions;
-
-	ret = register_reboot_notifier(&wdt_notifier);
-	if (ret != 0) {
-		pr_err("cannot register reboot notifier (err=%d)\n", ret);
-		goto unreg_regions;
-	}
-
-	ret = misc_register(&wdt_miscdev);
-	if (ret != 0) {
-		pr_err("cannot register miscdev on minor=%d (err=%d)\n",
-		       WATCHDOG_MINOR, ret);
-		goto unreg_reboot;
-	}
-
-	pr_info("initialized. timeout=%d sec (nowayout=%d)\n",
-		timeout, nowayout);
-
-out:
-	return ret;
-unreg_reboot:
-	unregister_reboot_notifier(&wdt_notifier);
-unreg_regions:
-	release_region(wdt_io, 1);
-	goto out;
-}
-
-static void __exit wdt_exit(void)
-{
-	misc_deregister(&wdt_miscdev);
-	unregister_reboot_notifier(&wdt_notifier);
-	release_region(wdt_io, 1);
-}
-
-module_init(wdt_init);
-module_exit(wdt_exit);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Flemming Frandsen <ff@nrvissing.net>");
-MODULE_DESCRIPTION("w83697ug/uf WDT driver");
diff --git a/drivers/xen/events/events_base.c b/drivers/xen/events/events_base.c
index dfa12a4a0a4..c919d3d5c84 100644
--- a/drivers/xen/events/events_base.c
+++ b/drivers/xen/events/events_base.c
@@ -390,22 +390,7 @@ static void xen_irq_init(unsigned irq)
 
 static int __must_check xen_allocate_irqs_dynamic(int nvec)
 {
-	int first = 0;
-	int i, irq;
-
-#ifdef CONFIG_X86_IO_APIC
-	/*
-	 * For an HVM guest or domain 0 which see "real" (emulated or
-	 * actual respectively) GSIs we allocate dynamic IRQs
-	 * e.g. those corresponding to event channels or MSIs
-	 * etc. from the range above those "real" GSIs to avoid
-	 * collisions.
-	 */
-	if (xen_initial_domain() || xen_hvm_domain())
-		first = get_nr_irqs_gsi();
-#endif
-
-	irq = irq_alloc_descs_from(first, nvec, -1);
+	int i, irq = irq_alloc_descs(-1, 0, nvec, -1);
 
 	if (irq >= 0) {
 		for (i = 0; i < nvec; i++)
diff --git a/drivers/xen/events/events_fifo.c b/drivers/xen/events/events_fifo.c
index 96109a9972b..84b4bfb8434 100644
--- a/drivers/xen/events/events_fifo.c
+++ b/drivers/xen/events/events_fifo.c
@@ -66,7 +66,22 @@ static DEFINE_PER_CPU(struct evtchn_fifo_queue, cpu_queue);
 static event_word_t *event_array[MAX_EVENT_ARRAY_PAGES] __read_mostly;
 static unsigned event_array_pages __read_mostly;
 
+/*
+ * sync_set_bit() and friends must be unsigned long aligned on non-x86
+ * platforms.
+ */
+#if !defined(CONFIG_X86) && BITS_PER_LONG > 32
+
+#define BM(w) (unsigned long *)((unsigned long)w & ~0x7UL)
+#define EVTCHN_FIFO_BIT(b, w) \
+    (((unsigned long)w & 0x4UL) ? (EVTCHN_FIFO_ ##b + 32) : EVTCHN_FIFO_ ##b)
+
+#else
+
 #define BM(w) ((unsigned long *)(w))
+#define EVTCHN_FIFO_BIT(b, w) EVTCHN_FIFO_ ##b
+
+#endif
 
 static inline event_word_t *event_word_from_port(unsigned port)
 {
@@ -161,33 +176,38 @@ static void evtchn_fifo_bind_to_cpu(struct irq_info *info, unsigned cpu)
 static void evtchn_fifo_clear_pending(unsigned port)
 {
 	event_word_t *word = event_word_from_port(port);
-	sync_clear_bit(EVTCHN_FIFO_PENDING, BM(word));
+	sync_clear_bit(EVTCHN_FIFO_BIT(PENDING, word), BM(word));
 }
 
 static void evtchn_fifo_set_pending(unsigned port)
 {
 	event_word_t *word = event_word_from_port(port);
-	sync_set_bit(EVTCHN_FIFO_PENDING, BM(word));
+	sync_set_bit(EVTCHN_FIFO_BIT(PENDING, word), BM(word));
 }
 
 static bool evtchn_fifo_is_pending(unsigned port)
 {
 	event_word_t *word = event_word_from_port(port);
-	return sync_test_bit(EVTCHN_FIFO_PENDING, BM(word));
+	return sync_test_bit(EVTCHN_FIFO_BIT(PENDING, word), BM(word));
 }
 
 static bool evtchn_fifo_test_and_set_mask(unsigned port)
 {
 	event_word_t *word = event_word_from_port(port);
-	return sync_test_and_set_bit(EVTCHN_FIFO_MASKED, BM(word));
+	return sync_test_and_set_bit(EVTCHN_FIFO_BIT(MASKED, word), BM(word));
 }
 
 static void evtchn_fifo_mask(unsigned port)
 {
 	event_word_t *word = event_word_from_port(port);
-	sync_set_bit(EVTCHN_FIFO_MASKED, BM(word));
+	sync_set_bit(EVTCHN_FIFO_BIT(MASKED, word), BM(word));
 }
 
+static bool evtchn_fifo_is_masked(unsigned port)
+{
+	event_word_t *word = event_word_from_port(port);
+	return sync_test_bit(EVTCHN_FIFO_BIT(MASKED, word), BM(word));
+}
 /*
  * Clear MASKED, spinning if BUSY is set.
  */
@@ -211,7 +231,7 @@ static void evtchn_fifo_unmask(unsigned port)
 	BUG_ON(!irqs_disabled());
 
 	clear_masked(word);
-	if (sync_test_bit(EVTCHN_FIFO_PENDING, BM(word))) {
+	if (evtchn_fifo_is_pending(port)) {
 		struct evtchn_unmask unmask = { .port = port };
 		(void)HYPERVISOR_event_channel_op(EVTCHNOP_unmask, &unmask);
 	}
@@ -243,7 +263,7 @@ static void handle_irq_for_port(unsigned port)
 
 static void consume_one_event(unsigned cpu,
 			      struct evtchn_fifo_control_block *control_block,
-			      unsigned priority, uint32_t *ready)
+			      unsigned priority, unsigned long *ready)
 {
 	struct evtchn_fifo_queue *q = &per_cpu(cpu_queue, cpu);
 	uint32_t head;
@@ -273,10 +293,9 @@ static void consume_one_event(unsigned cpu,
 	 * copy of the ready word.
 	 */
 	if (head == 0)
-		clear_bit(priority, BM(ready));
+		clear_bit(priority, ready);
 
-	if (sync_test_bit(EVTCHN_FIFO_PENDING, BM(word))
-	    && !sync_test_bit(EVTCHN_FIFO_MASKED, BM(word)))
+	if (evtchn_fifo_is_pending(port) && !evtchn_fifo_is_masked(port))
 		handle_irq_for_port(port);
 
 	q->head[priority] = head;
@@ -285,7 +304,7 @@ static void consume_one_event(unsigned cpu,
 static void evtchn_fifo_handle_events(unsigned cpu)
 {
 	struct evtchn_fifo_control_block *control_block;
-	uint32_t ready;
+	unsigned long ready;
 	unsigned q;
 
 	control_block = per_cpu(cpu_control_block, cpu);
diff --git a/drivers/xen/manage.c b/drivers/xen/manage.c
index 32f9236c959..c3667b202f2 100644
--- a/drivers/xen/manage.c
+++ b/drivers/xen/manage.c
@@ -41,9 +41,6 @@ static enum shutdown_state shutting_down = SHUTDOWN_INVALID;
 
 struct suspend_info {
 	int cancelled;
-	unsigned long arg; /* extra hypercall argument */
-	void (*pre)(void);
-	void (*post)(int cancelled);
 };
 
 static RAW_NOTIFIER_HEAD(xen_resume_notifier);
@@ -61,26 +58,6 @@ void xen_resume_notifier_unregister(struct notifier_block *nb)
 EXPORT_SYMBOL_GPL(xen_resume_notifier_unregister);
 
 #ifdef CONFIG_HIBERNATE_CALLBACKS
-static void xen_hvm_post_suspend(int cancelled)
-{
-	xen_arch_hvm_post_suspend(cancelled);
-	gnttab_resume();
-}
-
-static void xen_pre_suspend(void)
-{
-	xen_mm_pin_all();
-	gnttab_suspend();
-	xen_arch_pre_suspend();
-}
-
-static void xen_post_suspend(int cancelled)
-{
-	xen_arch_post_suspend(cancelled);
-	gnttab_resume();
-	xen_mm_unpin_all();
-}
-
 static int xen_suspend(void *data)
 {
 	struct suspend_info *si = data;
@@ -94,18 +71,20 @@ static int xen_suspend(void *data)
 		return err;
 	}
 
-	if (si->pre)
-		si->pre();
+	gnttab_suspend();
+	xen_arch_pre_suspend();
 
 	/*
 	 * This hypercall returns 1 if suspend was cancelled
 	 * or the domain was merely checkpointed, and 0 if it
 	 * is resuming in a new domain.
 	 */
-	si->cancelled = HYPERVISOR_suspend(si->arg);
+	si->cancelled = HYPERVISOR_suspend(xen_pv_domain()
+                                           ? virt_to_mfn(xen_start_info)
+                                           : 0);
 
-	if (si->post)
-		si->post(si->cancelled);
+	xen_arch_post_suspend(si->cancelled);
+	gnttab_resume();
 
 	if (!si->cancelled) {
 		xen_irq_resume();
@@ -154,16 +133,6 @@ static void do_suspend(void)
 
 	si.cancelled = 1;
 
-	if (xen_hvm_domain()) {
-		si.arg = 0UL;
-		si.pre = NULL;
-		si.post = &xen_hvm_post_suspend;
-	} else {
-		si.arg = virt_to_mfn(xen_start_info);
-		si.pre = &xen_pre_suspend;
-		si.post = &xen_post_suspend;
-	}
-
 	err = stop_machine(xen_suspend, &si, cpumask_of(0));
 
 	raw_notifier_call_chain(&xen_resume_notifier, 0, NULL);
diff --git a/drivers/xen/xen-acpi-processor.c b/drivers/xen/xen-acpi-processor.c
index 82358d14ecf..59fc190f1e9 100644
--- a/drivers/xen/xen-acpi-processor.c
+++ b/drivers/xen/xen-acpi-processor.c
@@ -127,7 +127,7 @@ static int push_cxx_to_hypervisor(struct acpi_processor *_pr)
 			pr_debug("     C%d: %s %d uS\n",
 				 cx->type, cx->desc, (u32)cx->latency);
 		}
-	} else if (ret != -EINVAL)
+	} else if ((ret != -EINVAL) && (ret != -ENOSYS))
 		/* EINVAL means the ACPI ID is incorrect - meaning the ACPI
 		 * table is referencing a non-existing CPU - which can happen
 		 * with broken ACPI tables. */
@@ -259,7 +259,7 @@ static int push_pxx_to_hypervisor(struct acpi_processor *_pr)
 			(u32) perf->states[i].power,
 			(u32) perf->states[i].transition_latency);
 		}
-	} else if (ret != -EINVAL)
+	} else if ((ret != -EINVAL) && (ret != -ENOSYS))
 		/* EINVAL means the ACPI ID is incorrect - meaning the ACPI
 		 * table is referencing a non-existing CPU - which can happen
 		 * with broken ACPI tables. */
diff --git a/drivers/xen/xen-pciback/pci_stub.c b/drivers/xen/xen-pciback/pci_stub.c
index 62fcd485f0a..d57a173685f 100644
--- a/drivers/xen/xen-pciback/pci_stub.c
+++ b/drivers/xen/xen-pciback/pci_stub.c
@@ -242,6 +242,15 @@ struct pci_dev *pcistub_get_pci_dev(struct xen_pcibk_device *pdev,
 	return found_dev;
 }
 
+/*
+ * Called when:
+ *  - XenBus state has been reconfigure (pci unplug). See xen_pcibk_remove_device
+ *  - XenBus state has been disconnected (guest shutdown). See xen_pcibk_xenbus_remove
+ *  - 'echo BDF > unbind' on pciback module with no guest attached. See pcistub_remove
+ *  - 'echo BDF > unbind' with a guest still using it. See pcistub_remove
+ *
+ *  As such we have to be careful.
+ */
 void pcistub_put_pci_dev(struct pci_dev *dev)
 {
 	struct pcistub_device *psdev, *found_psdev = NULL;
@@ -272,16 +281,16 @@ void pcistub_put_pci_dev(struct pci_dev *dev)
 	 * and want to inhibit the user from fiddling with 'reset'
 	 */
 	pci_reset_function(dev);
-	pci_restore_state(psdev->dev);
+	pci_restore_state(dev);
 
 	/* This disables the device. */
-	xen_pcibk_reset_device(found_psdev->dev);
+	xen_pcibk_reset_device(dev);
 
 	/* And cleanup up our emulated fields. */
-	xen_pcibk_config_free_dyn_fields(found_psdev->dev);
-	xen_pcibk_config_reset_dev(found_psdev->dev);
+	xen_pcibk_config_reset_dev(dev);
+	xen_pcibk_config_free_dyn_fields(dev);
 
-	xen_unregister_device_domain_owner(found_psdev->dev);
+	xen_unregister_device_domain_owner(dev);
 
 	spin_lock_irqsave(&found_psdev->lock, flags);
 	found_psdev->pdev = NULL;
@@ -493,6 +502,8 @@ static int pcistub_seize(struct pci_dev *dev)
 	return err;
 }
 
+/* Called when 'bind'. This means we must _NOT_ call pci_reset_function or
+ * other functions that take the sysfs lock. */
 static int pcistub_probe(struct pci_dev *dev, const struct pci_device_id *id)
 {
 	int err = 0;
@@ -520,6 +531,8 @@ out:
 	return err;
 }
 
+/* Called when 'unbind'. This means we must _NOT_ call pci_reset_function or
+ * other functions that take the sysfs lock. */
 static void pcistub_remove(struct pci_dev *dev)
 {
 	struct pcistub_device *psdev, *found_psdev = NULL;
@@ -551,6 +564,8 @@ static void pcistub_remove(struct pci_dev *dev)
 			pr_warn("****** shutdown driver domain before binding device\n");
 			pr_warn("****** to other drivers or domains\n");
 
+			/* N.B. This ends up calling pcistub_put_pci_dev which ends up
+			 * doing the FLR. */
 			xen_pcibk_release_pci_dev(found_psdev->pdev,
 						found_psdev->dev);
 		}
diff --git a/drivers/xen/xen-pciback/pciback_ops.c b/drivers/xen/xen-pciback/pciback_ops.c
index 607e41460c0..c4a0666de6f 100644
--- a/drivers/xen/xen-pciback/pciback_ops.c
+++ b/drivers/xen/xen-pciback/pciback_ops.c
@@ -348,9 +348,9 @@ void xen_pcibk_do_op(struct work_struct *data)
 	notify_remote_via_irq(pdev->evtchn_irq);
 
 	/* Mark that we're done. */
-	smp_mb__before_clear_bit(); /* /after/ clearing PCIF_active */
+	smp_mb__before_atomic(); /* /after/ clearing PCIF_active */
 	clear_bit(_PDEVF_op_active, &pdev->flags);
-	smp_mb__after_clear_bit(); /* /before/ final check for work */
+	smp_mb__after_atomic(); /* /before/ final check for work */
 
 	/* Check to see if the driver domain tried to start another request in
 	 * between clearing _XEN_PCIF_active and clearing _PDEVF_op_active.
diff --git a/drivers/xen/xen-pciback/xenbus.c b/drivers/xen/xen-pciback/xenbus.c
index a9ed867afab..4a7e6e0a5f4 100644
--- a/drivers/xen/xen-pciback/xenbus.c
+++ b/drivers/xen/xen-pciback/xenbus.c
@@ -93,6 +93,8 @@ static void free_pdev(struct xen_pcibk_device *pdev)
 
 	xen_pcibk_disconnect(pdev);
 
+	/* N.B. This calls pcistub_put_pci_dev which does the FLR on all
+	 * of the PCIe devices. */
 	xen_pcibk_release_devices(pdev);
 
 	dev_set_drvdata(&pdev->xdev->dev, NULL);
@@ -286,6 +288,8 @@ static int xen_pcibk_remove_device(struct xen_pcibk_device *pdev,
 	dev_dbg(&dev->dev, "unregistering for %d\n", pdev->xdev->otherend_id);
 	xen_unregister_device_domain_owner(dev);
 
+	/* N.B. This ends up calling pcistub_put_pci_dev which ends up
+	 * doing the FLR. */
 	xen_pcibk_release_pci_dev(pdev, dev);
 
 out: